[simbody] 01/10: Imported Upstream version 3.4.1
Jose Luis Rivero
jrivero-guest at moszumanska.debian.org
Fri Apr 4 22:31:44 UTC 2014
This is an automated email from the git hooks/post-receive script.
jrivero-guest pushed a commit to branch master
in repository simbody.
commit 60f0d31e55cf64ea372fc6b1a2db62b6d53d048c
Author: Jose Luis Rivero <jrivero at osrfoundation.org>
Date: Fri Apr 4 21:57:55 2014 +0200
Imported Upstream version 3.4.1
---
.gitattributes | 22 +
.gitignore | 12 +
.travis.yml | 24 +
ApiDoxygen.cmake | 20 +
CMakeLists.txt | 633 +
CTestConfig.cmake | 13 +
ChangeLog.txt | 107 +
Doxyfile.in | 2317 ++
LICENSE | 191 +
Platform/CMakeLists.txt | 98 +
Platform/Windows/include/dirent.h | 234 +
Platform/Windows/include/pthread.h | 1368 +
Platform/Windows/include/sched.h | 178 +
Platform/Windows/include/semaphore.h | 166 +
Platform/Windows/lib_x64/libblas.dll | Bin 0 -> 671682 bytes
Platform/Windows/lib_x64/libblas.lib | Bin 0 -> 25130 bytes
Platform/Windows/lib_x64/libgcc_s_sjlj-1.dll | Bin 0 -> 749404 bytes
Platform/Windows/lib_x64/libgfortran-3.dll | Bin 0 -> 7337949 bytes
Platform/Windows/lib_x64/liblapack.dll | Bin 0 -> 6187063 bytes
Platform/Windows/lib_x64/liblapack.lib | Bin 0 -> 252262 bytes
Platform/Windows/lib_x64/libquadmath-0.dll | Bin 0 -> 1310056 bytes
Platform/Windows/lib_x64/pthreadVC2_x64.dll | Bin 0 -> 42496 bytes
Platform/Windows/lib_x64/pthreadVC2_x64.lib | Bin 0 -> 29322 bytes
Platform/Windows/lib_x86/libblas.dll | Bin 0 -> 439942 bytes
Platform/Windows/lib_x86/libblas.lib | Bin 0 -> 25800 bytes
Platform/Windows/lib_x86/libgcc_s_dw2-1.dll | Bin 0 -> 119296 bytes
Platform/Windows/lib_x86/libgfortran-3.dll | Bin 0 -> 1069582 bytes
Platform/Windows/lib_x86/liblapack.dll | Bin 0 -> 5983228 bytes
Platform/Windows/lib_x86/liblapack.lib | Bin 0 -> 261136 bytes
Platform/Windows/lib_x86/libquadmath-0.dll | Bin 0 -> 704526 bytes
Platform/Windows/lib_x86/pthreadVC2.dll | Bin 0 -> 86070 bytes
Platform/Windows/lib_x86/pthreadVC2.lib | Bin 0 -> 29280 bytes
README.md | 207 +
.../include/SimTKcommon/internal/BigMatrix.h | 1564 +
.../include/SimTKcommon/internal/MatrixBase.h | 901 +
.../SimTKcommon/internal/MatrixCharacteristics.h | 1100 +
.../include/SimTKcommon/internal/MatrixHelper.h | 365 +
.../include/SimTKcommon/internal/MatrixView_.h | 98 +
.../include/SimTKcommon/internal/Matrix_.h | 152 +
.../include/SimTKcommon/internal/RowVectorBase.h | 315 +
.../include/SimTKcommon/internal/RowVectorView_.h | 95 +
.../include/SimTKcommon/internal/RowVector_.h | 122 +
.../include/SimTKcommon/internal/VectorBase.h | 475 +
.../include/SimTKcommon/internal/VectorIterator.h | 142 +
.../include/SimTKcommon/internal/VectorView_.h | 95 +
.../include/SimTKcommon/internal/Vector_.h | 208 +
SimTKcommon/BigMatrix/src/ElementFilter.cpp | 40 +
SimTKcommon/BigMatrix/src/ElementFilter.h | 316 +
.../BigMatrix/src/MatrixCharacteristics.cpp | 473 +
SimTKcommon/BigMatrix/src/MatrixHelper.cpp | 993 +
SimTKcommon/BigMatrix/src/MatrixHelperRep.h | 772 +
SimTKcommon/BigMatrix/src/MatrixHelperRep_Full.h | 624 +
SimTKcommon/BigMatrix/src/MatrixHelperRep_Tri.h | 467 +
SimTKcommon/BigMatrix/src/MatrixHelperRep_Vector.h | 708 +
SimTKcommon/CMakeLists.txt | 191 +
.../SimTKcommon/internal/DecorationGenerator.h | 62 +
.../SimTKcommon/internal/DecorativeGeometry.h | 666 +
.../include/SimTKcommon/internal/PolygonalMesh.h | 235 +
SimTKcommon/Geometry/src/DecorativeGeometry.cpp | 428 +
SimTKcommon/Geometry/src/DecorativeGeometryRep.h | 642 +
SimTKcommon/Geometry/src/PolygonalMesh.cpp | 710 +
SimTKcommon/Geometry/src/PolygonalMeshImpl.h | 52 +
.../Mechanics/include/SimTKcommon/Mechanics.h | 39 +
.../Mechanics/include/SimTKcommon/Orientation.h | 74 +
.../include/SimTKcommon/internal/CoordinateAxis.h | 417 +
.../include/SimTKcommon/internal/MassProperties.h | 1570 +
.../include/SimTKcommon/internal/Quaternion.h | 175 +
.../include/SimTKcommon/internal/Rotation.h | 1185 +
.../include/SimTKcommon/internal/SpatialAlgebra.h | 786 +
.../include/SimTKcommon/internal/Transform.h | 620 +
.../include/SimTKcommon/internal/UnitVec.h | 352 +
SimTKcommon/Mechanics/src/CoordinateAxis.cpp | 47 +
SimTKcommon/Mechanics/src/MassProperties.cpp | 145 +
SimTKcommon/Mechanics/src/Quaternion.cpp | 121 +
SimTKcommon/Mechanics/src/Rotation.cpp | 826 +
SimTKcommon/Mechanics/src/Transform.cpp | 65 +
SimTKcommon/Mechanics/src/UnitVec.cpp | 44 +
.../SimTKcommon/internal/PolynomialRootFinder.h | 126 +
.../Polynomial/src/PolynomialRootFinder.cpp | 225 +
SimTKcommon/Polynomial/src/cpoly.cpp | 681 +
SimTKcommon/Polynomial/src/cpoly.h | 99 +
SimTKcommon/Polynomial/src/rpoly.cpp | 782 +
SimTKcommon/Polynomial/src/rpoly.h | 59 +
SimTKcommon/Random/etc/LICENSE.txt | 29 +
.../Random/include/SimTKcommon/internal/Random.h | 179 +
SimTKcommon/Random/src/Random.cpp | 283 +
SimTKcommon/Random/src/SFMT-alti.h | 156 +
SimTKcommon/Random/src/SFMT-params.h | 94 +
SimTKcommon/Random/src/SFMT-params11213.h | 46 +
SimTKcommon/Random/src/SFMT-params1279.h | 46 +
SimTKcommon/Random/src/SFMT-params132049.h | 46 +
SimTKcommon/Random/src/SFMT-params19937.h | 46 +
SimTKcommon/Random/src/SFMT-params216091.h | 46 +
SimTKcommon/Random/src/SFMT-params2281.h | 46 +
SimTKcommon/Random/src/SFMT-params4253.h | 46 +
SimTKcommon/Random/src/SFMT-params44497.h | 46 +
SimTKcommon/Random/src/SFMT-params607.h | 46 +
SimTKcommon/Random/src/SFMT-params86243.h | 46 +
SimTKcommon/Random/src/SFMT-sse2.h | 121 +
SimTKcommon/Random/src/SFMT.cpp | 669 +
SimTKcommon/Random/src/SFMT.h | 172 +
SimTKcommon/Scalar/include/SimTKcommon/Scalar.h | 1388 +
.../SimTKcommon/internal/CompositeNumericalTypes.h | 275 +
.../Scalar/include/SimTKcommon/internal/NTraits.h | 1098 +
.../include/SimTKcommon/internal/conjugate.h | 1027 +
.../Scalar/include/SimTKcommon/internal/negator.h | 409 +
SimTKcommon/Scalar/src/Scalar.cpp | 153 +
.../include/SimTKcommon/internal/Event.h | 397 +
.../include/SimTKcommon/internal/EventHandler.h | 176 +
.../include/SimTKcommon/internal/EventReporter.h | 169 +
.../include/SimTKcommon/internal/Measure.h | 1079 +
.../SimTKcommon/internal/MeasureImplementation.h | 2140 ++
.../include/SimTKcommon/internal/Stage.h | 320 +
.../include/SimTKcommon/internal/State.h | 1119 +
.../include/SimTKcommon/internal/Study.h | 106 +
.../include/SimTKcommon/internal/StudyGuts.h | 112 +
.../include/SimTKcommon/internal/Subsystem.h | 256 +
.../include/SimTKcommon/internal/SubsystemGuts.h | 348 +
.../include/SimTKcommon/internal/System.h | 1178 +
.../include/SimTKcommon/internal/SystemGuts.h | 267 +
SimTKcommon/Simulation/src/Event.cpp | 187 +
SimTKcommon/Simulation/src/EventHandler.cpp | 106 +
SimTKcommon/Simulation/src/EventReporter.cpp | 106 +
SimTKcommon/Simulation/src/Measure.cpp | 72 +
SimTKcommon/Simulation/src/State.cpp | 2964 ++
SimTKcommon/Simulation/src/Study.cpp | 188 +
SimTKcommon/Simulation/src/Subsystem.cpp | 736 +
SimTKcommon/Simulation/src/SubsystemGutsRep.h | 153 +
SimTKcommon/Simulation/src/System.cpp | 1502 +
SimTKcommon/Simulation/src/SystemGutsRep.h | 235 +
.../SmallMatrix/include/SimTKcommon/SmallMatrix.h | 337 +
.../SmallMatrix/include/SimTKcommon/internal/Mat.h | 1593 +
.../include/SimTKcommon/internal/ResultType.h | 791 +
.../SmallMatrix/include/SimTKcommon/internal/Row.h | 1192 +
.../SimTKcommon/internal/SmallDefsThatNeedBig.h | 41 +
.../SimTKcommon/internal/SmallMatrixMixed.h | 1010 +
.../include/SimTKcommon/internal/SymMat.h | 1236 +
.../SmallMatrix/include/SimTKcommon/internal/Vec.h | 1430 +
SimTKcommon/SmallMatrix/src/SmallMatrix.cpp | 70 +
SimTKcommon/doc/Simmatrix.doc | Bin 0 -> 227328 bytes
SimTKcommon/doc/Simmatrix.pdf | Bin 0 -> 500404 bytes
SimTKcommon/doc/images/isaac.png | Bin 0 -> 70267 bytes
SimTKcommon/include/SimTKcommon.h | 66 +
SimTKcommon/include/SimTKcommon/Constants.h | 581 +
SimTKcommon/include/SimTKcommon/Simmatrix.h | 105 +
.../include/SimTKcommon/TemplatizedLapack.h | 361 +
SimTKcommon/include/SimTKcommon/Testing.h | 603 +
SimTKcommon/include/SimTKcommon/basics.h | 57 +
SimTKcommon/include/SimTKcommon/internal/Array.h | 3675 +++
.../include/SimTKcommon/internal/AtomicInteger.h | 75 +
.../include/SimTKcommon/internal/ClonePtr.h | 225 +
.../include/SimTKcommon/internal/Exception.h | 333 +
.../include/SimTKcommon/internal/ExceptionMacros.h | 386 +
SimTKcommon/include/SimTKcommon/internal/Fortran.h | 108 +
.../include/SimTKcommon/internal/Function.h | 412 +
.../SimTKcommon/internal/Parallel2DExecutor.h | 140 +
.../SimTKcommon/internal/ParallelExecutor.h | 122 +
.../SimTKcommon/internal/ParallelWorkQueue.h | 107 +
.../include/SimTKcommon/internal/Pathname.h | 239 +
SimTKcommon/include/SimTKcommon/internal/Plugin.h | 318 +
.../SimTKcommon/internal/PrivateImplementation.h | 381 +
.../internal/PrivateImplementation_Defs.h | 274 +
.../include/SimTKcommon/internal/ReferencePtr.h | 147 +
.../include/SimTKcommon/internal/Serialize.h | 346 +
.../include/SimTKcommon/internal/StableArray.h | 234 +
SimTKcommon/include/SimTKcommon/internal/String.h | 469 +
.../include/SimTKcommon/internal/ThreadLocal.h | 150 +
SimTKcommon/include/SimTKcommon/internal/Timing.h | 285 +
SimTKcommon/include/SimTKcommon/internal/Value.h | 107 +
.../include/SimTKcommon/internal/VectorMath.h | 466 +
SimTKcommon/include/SimTKcommon/internal/Xml.h | 1609 +
SimTKcommon/include/SimTKcommon/internal/common.h | 770 +
SimTKcommon/include/SimTKlapack.h | 3215 ++
SimTKcommon/sharedTarget/CMakeLists.txt | 54 +
SimTKcommon/src/About.cpp | 119 +
SimTKcommon/src/AtomicInteger.cpp | 169 +
SimTKcommon/src/Parallel2DExecutor.cpp | 223 +
SimTKcommon/src/Parallel2DExecutorImpl.h | 65 +
SimTKcommon/src/ParallelExecutor.cpp | 220 +
SimTKcommon/src/ParallelExecutorImpl.h | 92 +
SimTKcommon/src/ParallelWorkQueue.cpp | 152 +
SimTKcommon/src/ParallelWorkQueueImpl.h | 66 +
SimTKcommon/src/Pathname.cpp | 371 +
SimTKcommon/src/Plugin.cpp | 275 +
SimTKcommon/src/PrivateInstantiations.cpp | 48 +
SimTKcommon/src/String.cpp | 172 +
SimTKcommon/src/Timing.cpp | 338 +
SimTKcommon/src/Xml.cpp | 1067 +
SimTKcommon/src/gmx_atomic.h | 1596 +
SimTKcommon/src/tinyxml.cpp | 1906 ++
SimTKcommon/src/tinyxml.h | 1749 ++
SimTKcommon/src/tinyxmlparser.cpp | 1698 ++
SimTKcommon/staticTarget/CMakeLists.txt | 43 +
SimTKcommon/tests/BNTTest.cpp | 381 +
SimTKcommon/tests/CMakeLists.txt | 56 +
SimTKcommon/tests/MatVecTest.cpp | 553 +
SimTKcommon/tests/OrientationTest.cpp | 423 +
SimTKcommon/tests/PolynomialTest.cpp | 445 +
SimTKcommon/tests/RandomTest.cpp | 235 +
SimTKcommon/tests/RotationTest.cpp | 745 +
SimTKcommon/tests/SFMTTest.cpp | 62 +
SimTKcommon/tests/SpatialAlgebraTest.cpp | 188 +
SimTKcommon/tests/StateTest.cpp | 257 +
SimTKcommon/tests/TestArray.cpp | 868 +
SimTKcommon/tests/TestAtomicInteger.cpp | 152 +
SimTKcommon/tests/TestBigMatrix.cpp | 281 +
SimTKcommon/tests/TestFunction.cpp | 210 +
SimTKcommon/tests/TestMassProperties.cpp | 387 +
SimTKcommon/tests/TestParallel2DExecutor.cpp | 138 +
SimTKcommon/tests/TestParallelExecutor.cpp | 104 +
SimTKcommon/tests/TestParallelWorkQueue.cpp | 68 +
SimTKcommon/tests/TestPlugin.cpp | 240 +
SimTKcommon/tests/TestPolygonalMesh.cpp | 122 +
SimTKcommon/tests/TestPrivateImplementation.cpp | 398 +
SimTKcommon/tests/TestScalar.cpp | 766 +
SimTKcommon/tests/TestSimulation.cpp | 875 +
SimTKcommon/tests/TestSmallMatrix.cpp | 502 +
SimTKcommon/tests/TestVectorMath.cpp | 357 +
SimTKcommon/tests/TestXml.cpp | 340 +
SimTKcommon/tests/adhoc/BigMatrixTest.cpp | 859 +
SimTKcommon/tests/adhoc/CMakeLists.txt | 46 +
SimTKmath/CMakeLists.txt | 209 +
SimTKmath/Geometry/CMakeLists.txt | 10 +
.../include/simmath/internal/BicubicSurface.h | 605 +
.../simmath/internal/CollisionDetectionAlgorithm.h | 271 +
.../Geometry/include/simmath/internal/Contact.h | 563 +
.../include/simmath/internal/ContactGeometry.h | 1517 +
.../include/simmath/internal/ContactTracker.h | 602 +
.../Geometry/include/simmath/internal/GCVSPLUtil.h | 132 +
SimTKmath/Geometry/include/simmath/internal/Geo.h | 419 +
.../simmath/internal/Geo_BicubicBezierPatch.h | 523 +
.../simmath/internal/Geo_BicubicHermitePatch.h | 288 +
.../Geometry/include/simmath/internal/Geo_Box.h | 375 +
.../simmath/internal/Geo_CubicBezierCurve.h | 526 +
.../simmath/internal/Geo_CubicHermiteCurve.h | 334 +
.../include/simmath/internal/Geo_LineSeg.h | 149 +
.../Geometry/include/simmath/internal/Geo_Point.h | 474 +
.../Geometry/include/simmath/internal/Geo_Sphere.h | 125 +
.../include/simmath/internal/Geo_Triangle.h | 173 +
.../Geometry/include/simmath/internal/Geodesic.h | 318 +
.../include/simmath/internal/GeodesicIntegrator.h | 402 +
.../Geometry/include/simmath/internal/OBBTree.h | 112 +
.../include/simmath/internal/OrientedBoundingBox.h | 106 +
.../simmath/internal/ParticleConSurfaceSystem.h | 151 +
.../Geometry/include/simmath/internal/Spline.h | 205 +
.../include/simmath/internal/SplineFitter.h | 208 +
SimTKmath/Geometry/src/BicubicSurface.cpp | 1112 +
SimTKmath/Geometry/src/BicubicSurface_Guts.h | 359 +
.../Geometry/src/CollisionDetectionAlgorithm.cpp | 798 +
SimTKmath/Geometry/src/Contact.cpp | 276 +
SimTKmath/Geometry/src/ContactGeometry.cpp | 2189 ++
SimTKmath/Geometry/src/ContactGeometryImpl.h | 1146 +
.../Geometry/src/ContactGeometry_Cylinder.cpp | 315 +
.../Geometry/src/ContactGeometry_Ellipsoid.cpp | 397 +
.../Geometry/src/ContactGeometry_HalfSpace.cpp | 96 +
.../src/ContactGeometry_SmoothHeightMap.cpp | 244 +
SimTKmath/Geometry/src/ContactGeometry_Sphere.cpp | 324 +
SimTKmath/Geometry/src/ContactGeometry_Torus.cpp | 237 +
.../Geometry/src/ContactGeometry_TriangleMesh.cpp | 1028 +
SimTKmath/Geometry/src/ContactImpl.h | 259 +
SimTKmath/Geometry/src/ContactTracker.cpp | 1479 +
SimTKmath/Geometry/src/GCVSPL README.txt | 31 +
SimTKmath/Geometry/src/GCVSPLUtil.cpp | 36 +
SimTKmath/Geometry/src/Geo.cpp | 63 +
SimTKmath/Geometry/src/Geo_Box.cpp | 288 +
SimTKmath/Geometry/src/Geo_Point.cpp | 1379 +
SimTKmath/Geometry/src/Geo_Sphere.cpp | 49 +
SimTKmath/Geometry/src/Geo_Triangle.cpp | 632 +
SimTKmath/Geometry/src/Geodesic.cpp | 523 +
SimTKmath/Geometry/src/GeodesicEquations.h | 254 +
SimTKmath/Geometry/src/OBBTree.cpp | 33 +
SimTKmath/Geometry/src/OrientedBoundingBox.cpp | 387 +
SimTKmath/Geometry/src/gcvspl.cpp | 1097 +
SimTKmath/Integrators/CMakeLists.txt | 19 +
.../Integrators/include/simmath/CPodesIntegrator.h | 80 +
.../include/simmath/ExplicitEulerIntegrator.h | 58 +
SimTKmath/Integrators/include/simmath/Integrator.h | 419 +
.../include/simmath/RungeKutta2Integrator.h | 49 +
.../include/simmath/RungeKutta3Integrator.h | 49 +
.../include/simmath/RungeKuttaFeldbergIntegrator.h | 49 +
.../include/simmath/RungeKuttaMersonIntegrator.h | 49 +
.../include/simmath/SemiExplicitEuler2Integrator.h | 100 +
.../include/simmath/SemiExplicitEulerIntegrator.h | 108 +
.../Integrators/include/simmath/TimeStepper.h | 125 +
.../Integrators/include/simmath/VerletIntegrator.h | 81 +
.../include/simmath/internal/SimTKcpodes.h | 421 +
.../Integrators/src/AbstractIntegratorRep.cpp | 825 +
SimTKmath/Integrators/src/AbstractIntegratorRep.h | 171 +
SimTKmath/Integrators/src/CPodes/CMakeLists.txt | 25 +
SimTKmath/Integrators/src/CPodes/SimTKcpodes.cpp | 762 +
SimTKmath/Integrators/src/CPodes/nvector_SimTK.cpp | 538 +
SimTKmath/Integrators/src/CPodes/nvector_SimTK.h | 212 +
.../Integrators/src/CPodes/sundials/CMakeLists.txt | 30 +
.../src/CPodes/sundials/INSTALL_NOTES.txt | 494 +
.../Integrators/src/CPodes/sundials/LICENSE.txt | 63 +
.../Integrators/src/CPodes/sundials/README.txt | 53 +
.../src/CPodes/sundials/include/cpodes/cpodes.h | 1288 +
.../CPodes/sundials/include/cpodes/cpodes_band.h | 60 +
.../sundials/include/cpodes/cpodes_bandpre.h | 253 +
.../CPodes/sundials/include/cpodes/cpodes_bbdpre.h | 408 +
.../CPodes/sundials/include/cpodes/cpodes_dense.h | 83 +
.../CPodes/sundials/include/cpodes/cpodes_direct.h | 438 +
.../CPodes/sundials/include/cpodes/cpodes_lapack.h | 31 +
.../include/cpodes/cpodes_lapack_exports.h | 111 +
.../CPodes/sundials/include/cpodes/cpodes_spbcgs.h | 67 +
.../CPodes/sundials/include/cpodes/cpodes_spgmr.h | 68 +
.../CPodes/sundials/include/cpodes/cpodes_spils.h | 391 +
.../sundials/include/cpodes/cpodes_sptfqmr.h | 66 +
.../sundials/include/nvector/nvector_parallel.h | 314 +
.../sundials/include/nvector/nvector_serial.h | 265 +
.../sundials/include/sundials/sundials_band.h | 174 +
.../sundials/include/sundials/sundials_config.h | 96 +
.../sundials/include/sundials/sundials_config.in | 66 +
.../sundials/include/sundials/sundials_dense.h | 207 +
.../sundials/include/sundials/sundials_direct.h | 297 +
.../sundials/include/sundials/sundials_fnvector.h | 41 +
.../sundials/include/sundials/sundials_iterative.h | 242 +
.../sundials/include/sundials/sundials_lapack.h | 158 +
.../sundials/include/sundials/sundials_math.h | 139 +
.../sundials/include/sundials/sundials_nvector.h | 373 +
.../sundials/include/sundials/sundials_spbcgs.h | 199 +
.../sundials/include/sundials/sundials_spgmr.h | 296 +
.../sundials/include/sundials/sundials_sptfqmr.h | 255 +
.../sundials/include/sundials/sundials_types.h | 119 +
.../src/CPodes/sundials/src/cpodes/LICENSE.txt | 59 +
.../src/CPodes/sundials/src/cpodes/README.txt | 139 +
.../src/CPodes/sundials/src/cpodes/TODO.txt | 36 +
.../src/CPodes/sundials/src/cpodes/cpodes.c | 4361 +++
.../src/CPodes/sundials/src/cpodes/cpodes_band.c | 404 +
.../CPodes/sundials/src/cpodes/cpodes_bandpre.c | 764 +
.../sundials/src/cpodes/cpodes_bandpre_impl.h | 69 +
.../src/CPodes/sundials/src/cpodes/cpodes_bbdpre.c | 902 +
.../sundials/src/cpodes/cpodes_bbdpre_impl.h | 79 +
.../src/CPodes/sundials/src/cpodes/cpodes_dense.c | 1155 +
.../src/CPodes/sundials/src/cpodes/cpodes_direct.c | 850 +
.../sundials/src/cpodes/cpodes_direct_impl.h | 174 +
.../src/CPodes/sundials/src/cpodes/cpodes_ic.c | 664 +
.../src/CPodes/sundials/src/cpodes/cpodes_impl.h | 688 +
.../src/CPodes/sundials/src/cpodes/cpodes_io.c | 1485 +
.../src/CPodes/sundials/src/cpodes/cpodes_lapack.c | 1495 +
.../src/CPodes/sundials/src/cpodes/cpodes_nls.c | 801 +
.../CPodes/sundials/src/cpodes/cpodes_private.h | 371 +
.../src/CPodes/sundials/src/cpodes/cpodes_proj.c | 511 +
.../src/CPodes/sundials/src/cpodes/cpodes_root.c | 734 +
.../src/CPodes/sundials/src/cpodes/cpodes_spbcgs.c | 509 +
.../src/CPodes/sundials/src/cpodes/cpodes_spgmr.c | 516 +
.../src/CPodes/sundials/src/cpodes/cpodes_spils.c | 773 +
.../CPodes/sundials/src/cpodes/cpodes_spils_impl.h | 165 +
.../CPodes/sundials/src/cpodes/cpodes_sptfqmr.c | 510 +
.../src/CPodes/sundials/src/nvec_ser/README.txt | 121 +
.../CPodes/sundials/src/nvec_ser/fnvector_serial.c | 147 +
.../CPodes/sundials/src/nvec_ser/fnvector_serial.h | 84 +
.../CPodes/sundials/src/nvec_ser/nvector_serial.c | 1034 +
.../src/CPodes/sundials/src/sundials/README.txt | 198 +
.../CPodes/sundials/src/sundials/sundials_band.c | 259 +
.../CPodes/sundials/src/sundials/sundials_dense.c | 396 +
.../CPodes/sundials/src/sundials/sundials_direct.c | 275 +
.../sundials/src/sundials/sundials_iterative.c | 288 +
.../CPodes/sundials/src/sundials/sundials_lapack.c | 69 +
.../CPodes/sundials/src/sundials/sundials_math.c | 94 +
.../sundials/src/sundials/sundials_nvector.c | 233 +
.../CPodes/sundials/src/sundials/sundials_spbcgs.c | 379 +
.../CPodes/sundials/src/sundials/sundials_spgmr.c | 458 +
.../sundials/src/sundials/sundials_sptfqmr.c | 516 +
.../src/CPodes/sundials/tests/CMakeLists.txt | 62 +
.../src/CPodes/sundials/tests/README.txt | 22 +
.../src/CPodes/sundials/tests/cpsAdvDiff_bnd.c | 265 +
.../src/CPodes/sundials/tests/cpsAdvDiff_bndL.c | 265 +
.../src/CPodes/sundials/tests/cpsAdvDiff_non.c | 187 +
.../src/CPodes/sundials/tests/cpsNewtCrd_dns.c | 493 +
.../src/CPodes/sundials/tests/cpsPend_dns.c | 265 +
.../src/CPodes/sundials/tests/cpsPend_dnsL.c | 168 +
.../src/CPodes/sundials/tests/cpsRoberts_dns.c | 287 +
.../src/CPodes/sundials/tests/cpsRoberts_dnsL.c | 290 +
.../src/CPodes/sundials/tests/cpsVanDPol_non.c | 141 +
.../src/CPodes/sundials/tests/cpsadamsx.c | 283 +
.../src/CPodes/sundials/tests/cpsbanx.c | 265 +
.../src/CPodes/sundials/tests/cpsbanx_lap.c | 265 +
.../src/CPodes/sundials/tests/cpsdenx.c | 287 +
.../src/CPodes/sundials/tests/cpsdenx_lap.c | 290 +
.../src/CPodes/sundials/tests/initroot1.c | 151 +
.../Integrators/src/CPodes/sundials/tests/newton.c | 484 +
.../Integrators/src/CPodes/sundials/tests/pend.c | 258 +
.../Integrators/src/CPodes/sundials/tests/pendEs.c | 203 +
.../Integrators/src/CPodes/sundials/tests/pendLr.c | 168 +
.../src/CPodes/sundials/tests/pend_test.c | 265 +
.../Integrators/src/CPodes/tests/CMakeLists.txt | 54 +
.../Integrators/src/CPodes/tests/CpsAdamsXCpp.cpp | 281 +
.../Integrators/src/CPodes/tests/PendLrCpp.cpp | 250 +
SimTKmath/Integrators/src/CPodesIntegrator.cpp | 646 +
SimTKmath/Integrators/src/CPodesIntegratorRep.h | 84 +
.../Integrators/src/ExplicitEulerIntegrator.cpp | 182 +
.../Integrators/src/ExplicitEulerIntegratorRep.h | 43 +
SimTKmath/Integrators/src/Integrator.cpp | 451 +
SimTKmath/Integrators/src/IntegratorRep.h | 1133 +
.../Integrators/src/RungeKutta2Integrator.cpp | 120 +
.../Integrators/src/RungeKutta2IntegratorRep.h | 52 +
.../Integrators/src/RungeKutta3Integrator.cpp | 125 +
.../Integrators/src/RungeKutta3IntegratorRep.h | 52 +
.../src/RungeKuttaFeldbergIntegrator.cpp | 155 +
.../src/RungeKuttaFeldbergIntegratorRep.h | 52 +
.../Integrators/src/RungeKuttaMersonIntegrator.cpp | 141 +
.../src/RungeKuttaMersonIntegratorRep.h | 52 +
.../src/SemiExplicitEuler2Integrator.cpp | 263 +
.../src/SemiExplicitEuler2IntegratorRep.h | 45 +
.../src/SemiExplicitEulerIntegrator.cpp | 191 +
.../src/SemiExplicitEulerIntegratorRep.h | 43 +
SimTKmath/Integrators/src/TimeStepper.cpp | 212 +
SimTKmath/Integrators/src/TimeStepperRep.h | 102 +
SimTKmath/Integrators/src/VerletIntegrator.cpp | 234 +
SimTKmath/Integrators/src/VerletIntegratorRep.h | 41 +
SimTKmath/LinearAlgebra/CMakeLists.txt | 10 +
SimTKmath/LinearAlgebra/src/Eigen.cpp | 782 +
SimTKmath/LinearAlgebra/src/EigenRep.h | 318 +
SimTKmath/LinearAlgebra/src/Factor.cpp | 399 +
SimTKmath/LinearAlgebra/src/FactorQTZ.cpp | 530 +
SimTKmath/LinearAlgebra/src/FactorQTZRep.h | 152 +
SimTKmath/LinearAlgebra/src/FactorRep.h | 234 +
SimTKmath/LinearAlgebra/src/FactorSVD.cpp | 431 +
SimTKmath/LinearAlgebra/src/FactorSVDRep.h | 194 +
SimTKmath/LinearAlgebra/src/LATraits.h | 84 +
SimTKmath/LinearAlgebra/src/LapackConvert.cpp | 78 +
SimTKmath/LinearAlgebra/src/LapackConvert.h | 40 +
SimTKmath/LinearAlgebra/src/LapackInterface.cpp | 1213 +
SimTKmath/LinearAlgebra/src/LapackInterface.h | 143 +
SimTKmath/LinearAlgebra/src/WorkSpace.h | 90 +
SimTKmath/Optimizers/CMakeLists.txt | 18 +
SimTKmath/Optimizers/src/CFSQPOptimizer.cpp | 437 +
SimTKmath/Optimizers/src/CFSQPOptimizer.h | 136 +
.../Optimizers/src/InteriorPointOptimizer.cpp | 271 +
SimTKmath/Optimizers/src/InteriorPointOptimizer.h | 69 +
SimTKmath/Optimizers/src/IpOpt/CMakeLists.txt | 23 +
.../Optimizers/src/IpOpt/IpAdaptiveMuUpdate.cpp | 764 +
.../Optimizers/src/IpOpt/IpAdaptiveMuUpdate.hpp | 195 +
SimTKmath/Optimizers/src/IpOpt/IpAlgBuilder.cpp | 475 +
SimTKmath/Optimizers/src/IpOpt/IpAlgBuilder.hpp | 84 +
SimTKmath/Optimizers/src/IpOpt/IpAlgStrategy.hpp | 153 +
SimTKmath/Optimizers/src/IpOpt/IpAlgTypes.hpp | 58 +
.../Optimizers/src/IpOpt/IpAlgorithmRegOp.cpp | 90 +
.../Optimizers/src/IpOpt/IpAlgorithmRegOp.hpp | 22 +
.../src/IpOpt/IpAugRestoSystemSolver.cpp | 618 +
.../src/IpOpt/IpAugRestoSystemSolver.hpp | 197 +
.../Optimizers/src/IpOpt/IpAugSystemSolver.hpp | 199 +
.../src/IpOpt/IpBacktrackingLSAcceptor.hpp | 137 +
.../src/IpOpt/IpBacktrackingLineSearch.cpp | 1167 +
.../src/IpOpt/IpBacktrackingLineSearch.hpp | 361 +
SimTKmath/Optimizers/src/IpOpt/IpBlas.cpp | 334 +
SimTKmath/Optimizers/src/IpOpt/IpBlas.hpp | 78 +
SimTKmath/Optimizers/src/IpOpt/IpCachedResults.hpp | 747 +
.../Optimizers/src/IpOpt/IpCompoundMatrix.cpp | 582 +
.../Optimizers/src/IpOpt/IpCompoundMatrix.hpp | 337 +
.../Optimizers/src/IpOpt/IpCompoundSymMatrix.cpp | 314 +
.../Optimizers/src/IpOpt/IpCompoundSymMatrix.hpp | 281 +
.../Optimizers/src/IpOpt/IpCompoundVector.cpp | 467 +
.../Optimizers/src/IpOpt/IpCompoundVector.hpp | 339 +
SimTKmath/Optimizers/src/IpOpt/IpConvCheck.hpp | 86 +
SimTKmath/Optimizers/src/IpOpt/IpDebug.cpp | 107 +
SimTKmath/Optimizers/src/IpOpt/IpDebug.hpp | 139 +
.../src/IpOpt/IpDefaultIterateInitializer.cpp | 398 +
.../src/IpOpt/IpDefaultIterateInitializer.hpp | 131 +
.../Optimizers/src/IpOpt/IpDenseGenMatrix.cpp | 300 +
.../Optimizers/src/IpOpt/IpDenseGenMatrix.hpp | 215 +
.../Optimizers/src/IpOpt/IpDenseSymMatrix.cpp | 223 +
.../Optimizers/src/IpOpt/IpDenseSymMatrix.hpp | 184 +
SimTKmath/Optimizers/src/IpOpt/IpDenseVector.cpp | 1148 +
SimTKmath/Optimizers/src/IpOpt/IpDenseVector.hpp | 330 +
SimTKmath/Optimizers/src/IpOpt/IpDiagMatrix.cpp | 69 +
SimTKmath/Optimizers/src/IpOpt/IpDiagMatrix.hpp | 139 +
.../Optimizers/src/IpOpt/IpEqMultCalculator.hpp | 64 +
.../Optimizers/src/IpOpt/IpExactHessianUpdater.cpp | 35 +
.../Optimizers/src/IpOpt/IpExactHessianUpdater.hpp | 62 +
SimTKmath/Optimizers/src/IpOpt/IpException.hpp | 147 +
.../Optimizers/src/IpOpt/IpExpansionMatrix.cpp | 318 +
.../Optimizers/src/IpOpt/IpExpansionMatrix.hpp | 195 +
SimTKmath/Optimizers/src/IpOpt/IpFilter.cpp | 114 +
SimTKmath/Optimizers/src/IpOpt/IpFilter.hpp | 189 +
.../Optimizers/src/IpOpt/IpFilterLSAcceptor.cpp | 800 +
.../Optimizers/src/IpOpt/IpFilterLSAcceptor.hpp | 273 +
SimTKmath/Optimizers/src/IpOpt/IpGenTMatrix.cpp | 197 +
SimTKmath/Optimizers/src/IpOpt/IpGenTMatrix.hpp | 248 +
.../Optimizers/src/IpOpt/IpGradientScaling.cpp | 224 +
.../Optimizers/src/IpOpt/IpGradientScaling.hpp | 85 +
.../Optimizers/src/IpOpt/IpHessianUpdater.hpp | 65 +
.../Optimizers/src/IpOpt/IpIdentityMatrix.cpp | 69 +
.../Optimizers/src/IpOpt/IpIdentityMatrix.hpp | 147 +
.../Optimizers/src/IpOpt/IpInterfacesRegOp.cpp | 26 +
.../Optimizers/src/IpOpt/IpInterfacesRegOp.hpp | 22 +
SimTKmath/Optimizers/src/IpOpt/IpIpoptAlg.cpp | 838 +
SimTKmath/Optimizers/src/IpOpt/IpIpoptAlg.hpp | 205 +
.../Optimizers/src/IpOpt/IpIpoptApplication.cpp | 783 +
.../Optimizers/src/IpOpt/IpIpoptApplication.hpp | 184 +
.../src/IpOpt/IpIpoptCalculatedQuantities.cpp | 3312 ++
.../src/IpOpt/IpIpoptCalculatedQuantities.hpp | 659 +
SimTKmath/Optimizers/src/IpOpt/IpIpoptData.cpp | 236 +
SimTKmath/Optimizers/src/IpOpt/IpIpoptData.hpp | 645 +
SimTKmath/Optimizers/src/IpOpt/IpIpoptNLP.hpp | 256 +
.../Optimizers/src/IpOpt/IpIterateInitializer.hpp | 64 +
.../Optimizers/src/IpOpt/IpIteratesVector.cpp | 155 +
.../Optimizers/src/IpOpt/IpIteratesVector.hpp | 681 +
.../Optimizers/src/IpOpt/IpIterationOutput.hpp | 63 +
SimTKmath/Optimizers/src/IpOpt/IpJournalist.cpp | 434 +
SimTKmath/Optimizers/src/IpOpt/IpJournalist.hpp | 400 +
SimTKmath/Optimizers/src/IpOpt/IpLapack.cpp | 139 +
SimTKmath/Optimizers/src/IpOpt/IpLapack.hpp | 39 +
.../src/IpOpt/IpLapackSolverInterface.cpp | 248 +
.../src/IpOpt/IpLapackSolverInterface.hpp | 166 +
.../Optimizers/src/IpOpt/IpLeastSquareMults.cpp | 94 +
.../Optimizers/src/IpOpt/IpLeastSquareMults.hpp | 73 +
.../src/IpOpt/IpLimMemQuasiNewtonUpdater.cpp | 1289 +
.../src/IpOpt/IpLimMemQuasiNewtonUpdater.hpp | 354 +
SimTKmath/Optimizers/src/IpOpt/IpLineSearch.hpp | 96 +
.../Optimizers/src/IpOpt/IpLinearSolversRegOp.cpp | 26 +
.../Optimizers/src/IpOpt/IpLinearSolversRegOp.hpp | 22 +
SimTKmath/Optimizers/src/IpOpt/IpLoqoMuOracle.cpp | 85 +
SimTKmath/Optimizers/src/IpOpt/IpLoqoMuOracle.hpp | 61 +
.../src/IpOpt/IpLowRankAugSystemSolver.cpp | 599 +
.../src/IpOpt/IpLowRankAugSystemSolver.hpp | 255 +
.../src/IpOpt/IpLowRankUpdateSymMatrix.cpp | 187 +
.../src/IpOpt/IpLowRankUpdateSymMatrix.hpp | 251 +
SimTKmath/Optimizers/src/IpOpt/IpMatrix.cpp | 104 +
SimTKmath/Optimizers/src/IpOpt/IpMatrix.hpp | 303 +
.../Optimizers/src/IpOpt/IpMonotoneMuUpdate.cpp | 219 +
.../Optimizers/src/IpOpt/IpMonotoneMuUpdate.hpp | 99 +
SimTKmath/Optimizers/src/IpOpt/IpMuOracle.hpp | 71 +
SimTKmath/Optimizers/src/IpOpt/IpMuUpdate.hpp | 69 +
.../Optimizers/src/IpOpt/IpMultiVectorMatrix.cpp | 283 +
.../Optimizers/src/IpOpt/IpMultiVectorMatrix.hpp | 244 +
SimTKmath/Optimizers/src/IpOpt/IpNLP.hpp | 240 +
SimTKmath/Optimizers/src/IpOpt/IpNLPScaling.cpp | 539 +
SimTKmath/Optimizers/src/IpOpt/IpNLPScaling.hpp | 440 +
SimTKmath/Optimizers/src/IpOpt/IpObserver.cpp | 19 +
SimTKmath/Optimizers/src/IpOpt/IpObserver.hpp | 357 +
.../Optimizers/src/IpOpt/IpOptErrorConvCheck.cpp | 236 +
.../Optimizers/src/IpOpt/IpOptErrorConvCheck.hpp | 98 +
SimTKmath/Optimizers/src/IpOpt/IpOptionsList.cpp | 690 +
SimTKmath/Optimizers/src/IpOpt/IpOptionsList.hpp | 270 +
SimTKmath/Optimizers/src/IpOpt/IpOrigIpoptNLP.cpp | 829 +
SimTKmath/Optimizers/src/IpOpt/IpOrigIpoptNLP.hpp | 422 +
.../Optimizers/src/IpOpt/IpOrigIterationOutput.cpp | 247 +
.../Optimizers/src/IpOpt/IpOrigIterationOutput.hpp | 66 +
.../Optimizers/src/IpOpt/IpPDFullSpaceSolver.cpp | 720 +
.../Optimizers/src/IpOpt/IpPDFullSpaceSolver.hpp | 189 +
.../src/IpOpt/IpPDPerturbationHandler.cpp | 466 +
.../src/IpOpt/IpPDPerturbationHandler.hpp | 199 +
.../Optimizers/src/IpOpt/IpPDSystemSolver.hpp | 130 +
.../Optimizers/src/IpOpt/IpProbingMuOracle.cpp | 257 +
.../Optimizers/src/IpOpt/IpProbingMuOracle.hpp | 87 +
.../src/IpOpt/IpQualityFunctionMuOracle.cpp | 1300 +
.../src/IpOpt/IpQualityFunctionMuOracle.hpp | 277 +
SimTKmath/Optimizers/src/IpOpt/IpReferenced.hpp | 249 +
SimTKmath/Optimizers/src/IpOpt/IpRegOptions.cpp | 881 +
SimTKmath/Optimizers/src/IpOpt/IpRegOptions.hpp | 576 +
.../src/IpOpt/IpRestoFilterConvCheck.cpp | 228 +
.../src/IpOpt/IpRestoFilterConvCheck.hpp | 100 +
SimTKmath/Optimizers/src/IpOpt/IpRestoIpoptNLP.cpp | 734 +
SimTKmath/Optimizers/src/IpOpt/IpRestoIpoptNLP.hpp | 429 +
.../src/IpOpt/IpRestoIterateInitializer.cpp | 225 +
.../src/IpOpt/IpRestoIterateInitializer.hpp | 92 +
.../src/IpOpt/IpRestoIterationOutput.cpp | 272 +
.../src/IpOpt/IpRestoIterationOutput.hpp | 73 +
.../Optimizers/src/IpOpt/IpRestoMinC_1Nrm.cpp | 320 +
.../Optimizers/src/IpOpt/IpRestoMinC_1Nrm.hpp | 112 +
SimTKmath/Optimizers/src/IpOpt/IpRestoPhase.hpp | 71 +
.../Optimizers/src/IpOpt/IpRestoRestoPhase.cpp | 124 +
.../Optimizers/src/IpOpt/IpRestoRestoPhase.hpp | 69 +
SimTKmath/Optimizers/src/IpOpt/IpReturnCodes.h | 18 +
SimTKmath/Optimizers/src/IpOpt/IpReturnCodes.hpp | 21 +
SimTKmath/Optimizers/src/IpOpt/IpReturnCodes.inc | 64 +
SimTKmath/Optimizers/src/IpOpt/IpReturnCodes_inc.h | 44 +
SimTKmath/Optimizers/src/IpOpt/IpScaledMatrix.cpp | 181 +
SimTKmath/Optimizers/src/IpOpt/IpScaledMatrix.hpp | 251 +
SimTKmath/Optimizers/src/IpOpt/IpSmartPtr.hpp | 723 +
.../Optimizers/src/IpOpt/IpSolveStatistics.cpp | 96 +
.../Optimizers/src/IpOpt/IpSolveStatistics.hpp | 136 +
.../src/IpOpt/IpSparseSymLinearSolverInterface.hpp | 222 +
.../Optimizers/src/IpOpt/IpStdAugSystemSolver.cpp | 551 +
.../Optimizers/src/IpOpt/IpStdAugSystemSolver.hpp | 253 +
SimTKmath/Optimizers/src/IpOpt/IpStdCInterface.cpp | 221 +
SimTKmath/Optimizers/src/IpOpt/IpStdCInterface.h | 216 +
.../Optimizers/src/IpOpt/IpStdInterfaceTNLP.cpp | 318 +
.../Optimizers/src/IpOpt/IpStdInterfaceTNLP.hpp | 208 +
SimTKmath/Optimizers/src/IpOpt/IpSumMatrix.cpp | 165 +
SimTKmath/Optimizers/src/IpOpt/IpSumMatrix.hpp | 163 +
SimTKmath/Optimizers/src/IpOpt/IpSumSymMatrix.cpp | 145 +
SimTKmath/Optimizers/src/IpOpt/IpSumSymMatrix.hpp | 148 +
.../Optimizers/src/IpOpt/IpSymLinearSolver.hpp | 130 +
SimTKmath/Optimizers/src/IpOpt/IpSymMatrix.cpp | 50 +
SimTKmath/Optimizers/src/IpOpt/IpSymMatrix.hpp | 130 +
.../Optimizers/src/IpOpt/IpSymScaledMatrix.cpp | 83 +
.../Optimizers/src/IpOpt/IpSymScaledMatrix.hpp | 229 +
SimTKmath/Optimizers/src/IpOpt/IpSymTMatrix.cpp | 207 +
SimTKmath/Optimizers/src/IpOpt/IpSymTMatrix.hpp | 251 +
SimTKmath/Optimizers/src/IpOpt/IpTNLP.hpp | 234 +
SimTKmath/Optimizers/src/IpOpt/IpTNLPAdapter.cpp | 1908 ++
SimTKmath/Optimizers/src/IpOpt/IpTNLPAdapter.hpp | 353 +
.../Optimizers/src/IpOpt/IpTSymLinearSolver.cpp | 449 +
.../Optimizers/src/IpOpt/IpTSymLinearSolver.hpp | 195 +
.../Optimizers/src/IpOpt/IpTSymScalingMethod.hpp | 63 +
SimTKmath/Optimizers/src/IpOpt/IpTaggedObject.cpp | 16 +
SimTKmath/Optimizers/src/IpOpt/IpTaggedObject.hpp | 149 +
SimTKmath/Optimizers/src/IpOpt/IpTimedTask.hpp | 160 +
.../Optimizers/src/IpOpt/IpTimingStatistics.cpp | 134 +
.../Optimizers/src/IpOpt/IpTimingStatistics.hpp | 233 +
SimTKmath/Optimizers/src/IpOpt/IpTripletHelper.cpp | 734 +
SimTKmath/Optimizers/src/IpOpt/IpTripletHelper.hpp | 118 +
.../src/IpOpt/IpTripletToCSRConverter.cpp | 211 +
.../src/IpOpt/IpTripletToCSRConverter.hpp | 226 +
.../src/IpOpt/IpTripletToDenseConverter.cpp | 110 +
.../src/IpOpt/IpTripletToDenseConverter.hpp | 216 +
SimTKmath/Optimizers/src/IpOpt/IpTypes.hpp | 25 +
SimTKmath/Optimizers/src/IpOpt/IpUserScaling.cpp | 30 +
SimTKmath/Optimizers/src/IpOpt/IpUserScaling.hpp | 71 +
SimTKmath/Optimizers/src/IpOpt/IpUtils.cpp | 52 +
SimTKmath/Optimizers/src/IpOpt/IpUtils.hpp | 73 +
SimTKmath/Optimizers/src/IpOpt/IpVector.cpp | 197 +
SimTKmath/Optimizers/src/IpOpt/IpVector.hpp | 739 +
.../src/IpOpt/IpWarmStartIterateInitializer.cpp | 415 +
.../src/IpOpt/IpWarmStartIterateInitializer.hpp | 100 +
SimTKmath/Optimizers/src/IpOpt/IpZeroMatrix.cpp | 66 +
SimTKmath/Optimizers/src/IpOpt/IpZeroMatrix.hpp | 122 +
SimTKmath/Optimizers/src/IpOpt/IpoptConfig.h | 113 +
SimTKmath/Optimizers/src/IpOpt/LICENSE.txt | 234 +
SimTKmath/Optimizers/src/IpOpt/configall_system.h | 9 +
.../Optimizers/src/IpOpt/configall_system_msc.h | 130 +
SimTKmath/Optimizers/src/LBFGSBOptimizer.cpp | 131 +
SimTKmath/Optimizers/src/LBFGSBOptimizer.h | 60 +
SimTKmath/Optimizers/src/LBFGSOptimizer.cpp | 74 +
SimTKmath/Optimizers/src/LBFGSOptimizer.h | 63 +
SimTKmath/Optimizers/src/Optimizer.cpp | 208 +
SimTKmath/Optimizers/src/OptimizerRep.cpp | 267 +
SimTKmath/Optimizers/src/lbfgs.cpp | 1371 +
SimTKmath/Optimizers/src/lbfgsb.cpp | 5161 ++++
SimTKmath/doc/SimmathUserGuide.doc | Bin 0 -> 3793408 bytes
SimTKmath/doc/SimmathUserGuide.pdf | Bin 0 -> 676403 bytes
SimTKmath/doc/images/BicubicSurface1.png | Bin 0 -> 45509 bytes
SimTKmath/examples/CMakeLists.txt | 63 +
.../ConstrainedNumericalDiffOptimization.cpp | 123 +
SimTKmath/examples/ConstrainedOptimization.cpp | 166 +
SimTKmath/examples/Differentiator.cpp | 384 +
.../examples/ParameterConstrainedOptimization.cpp | 119 +
SimTKmath/examples/README.txt | 15 +
SimTKmath/examples/SimpleDifferentiator.cpp | 123 +
SimTKmath/examples/SimpleIntegrator.cpp | 188 +
.../UnconstrainedNumericalDiffOptimization.cpp | 78 +
SimTKmath/examples/UnconstrainedOptimization.cpp | 86 +
SimTKmath/examples/UserGuide.cpp | 147 +
SimTKmath/examples/UserGuideLimits.cpp | 155 +
SimTKmath/examples/makefile | 40 +
SimTKmath/include/SimTKmath.h | 66 +
SimTKmath/include/simmath/Differentiator.h | 243 +
SimTKmath/include/simmath/LinearAlgebra.h | 237 +
SimTKmath/include/simmath/MultibodyGraphMaker.h | 625 +
SimTKmath/include/simmath/Optimizer.h | 366 +
SimTKmath/include/simmath/internal/OptimizerRep.h | 216 +
SimTKmath/include/simmath/internal/common.h | 180 +
SimTKmath/sharedTarget/CMakeLists.txt | 78 +
SimTKmath/src/About.cpp | 116 +
SimTKmath/src/Differentiator.cpp | 794 +
SimTKmath/src/MultibodyGraphMaker.cpp | 752 +
SimTKmath/src/Simmath_f2c.h | 249 +
SimTKmath/staticTarget/CMakeLists.txt | 78 +
SimTKmath/tests/BestAvailableTest.cpp | 256 +
SimTKmath/tests/CMakeLists.txt | 53 +
SimTKmath/tests/CPodesIntegratorTest.cpp | 82 +
SimTKmath/tests/DifferentiatorTest.cpp | 354 +
SimTKmath/tests/EigenTest.cpp | 309 +
SimTKmath/tests/ExplicitEulerIntegratorTest.cpp | 53 +
SimTKmath/tests/FactorLUTest.cpp | 158 +
SimTKmath/tests/FactorQTZTest.cpp | 223 +
SimTKmath/tests/FactorSVDTest.cpp | 190 +
SimTKmath/tests/IntegratorTest.cpp | 803 +
SimTKmath/tests/IntegratorTestFramework.h | 269 +
SimTKmath/tests/IpoptDiffTest.cpp | 185 +
SimTKmath/tests/IpoptTest.cpp | 191 +
SimTKmath/tests/LBFGSBDiffTest.cpp | 155 +
SimTKmath/tests/LBFGSBTest.cpp | 154 +
SimTKmath/tests/LBFGSDiffTest.cpp | 116 +
SimTKmath/tests/LBFGSTest.cpp | 123 +
SimTKmath/tests/PendulumSystem.h | 549 +
SimTKmath/tests/RungeKutta2IntegratorTest.cpp | 62 +
SimTKmath/tests/RungeKutta3IntegratorTest.cpp | 62 +
.../tests/RungeKuttaFeldbergIntegratorTest.cpp | 59 +
SimTKmath/tests/RungeKuttaMersonIntegratorTest.cpp | 59 +
.../tests/SemiExplicitEuler2IntegratorTest.cpp | 68 +
.../tests/SemiExplicitEulerIntegratorTest.cpp | 69 +
SimTKmath/tests/SimpleDifferentiatorTest.cpp | 127 +
SimTKmath/tests/TestBicubicSurface.cpp | 1096 +
SimTKmath/tests/TestContactGeometry.cpp | 486 +
SimTKmath/tests/TestGeo.cpp | 428 +
SimTKmath/tests/TestSpline.cpp | 647 +
SimTKmath/tests/TestTriangleMesh.cpp | 320 +
SimTKmath/tests/TimeStepperTest.cpp | 177 +
SimTKmath/tests/VerletIntegratorTest.cpp | 59 +
SimTKmath/tests/adhoc/CMakeLists.txt | 46 +
SimTKmath/tests/adhoc/nlpqlp.cpp | 30286 +++++++++++++++++++
SimTKmath/tests/adhoc/nlpqlp.h | 1317 +
Simbody/CMakeLists.txt | 165 +
Simbody/SimbodyThingsToDoListAndTimeline.doc | Bin 0 -> 151040 bytes
Simbody/ThingsToDoList.doc | Bin 0 -> 151552 bytes
.../include/simbody/internal/Visualizer.h | 762 +
.../simbody/internal/Visualizer_InputListener.h | 335 +
.../include/simbody/internal/Visualizer_Reporter.h | 89 +
.../Visualizer/simbody-visualizer/CMakeLists.txt | 52 +
.../Visualizer/simbody-visualizer/glut32/glext.h | 11037 +++++++
.../Visualizer/simbody-visualizer/glut32/glut.h | 730 +
.../simbody-visualizer/glut32/lib/glut32.dll | Bin 0 -> 231936 bytes
.../simbody-visualizer/glut32/lib/glut32.lib | Bin 0 -> 28032 bytes
.../simbody-visualizer/glut32/lib64/glut32.dll | Bin 0 -> 272384 bytes
.../simbody-visualizer/glut32/lib64/glut32.lib | Bin 0 -> 26180 bytes
.../simbody-visualizer/glut32/readme.txt | 22 +
Simbody/Visualizer/simbody-visualizer/lodepng.cpp | 5114 ++++
Simbody/Visualizer/simbody-visualizer/lodepng.h | 1892 ++
.../simbody-visualizer/simbody-visualizer.cpp | 2881 ++
Simbody/Visualizer/src/Visualizer.cpp | 1153 +
Simbody/Visualizer/src/VisualizerGeometry.cpp | 169 +
Simbody/Visualizer/src/VisualizerGeometry.h | 67 +
Simbody/Visualizer/src/VisualizerProtocol.cpp | 731 +
Simbody/Visualizer/src/VisualizerProtocol.h | 165 +
.../Visualizer/src/Visualizer_InputListener.cpp | 246 +
Simbody/Visualizer/src/Visualizer_Reporter.cpp | 74 +
Simbody/doc/FrictionConstraints.docx | Bin 0 -> 676204 bytes
Simbody/doc/HowToComputeMuscleMomentArm.docx | Bin 0 -> 343135 bytes
Simbody/doc/SimbodyAdvancedProgrammingGuide.docx | Bin 0 -> 657674 bytes
Simbody/doc/SimbodyAdvancedProgrammingGuide.pdf | Bin 0 -> 759048 bytes
Simbody/doc/SimbodyAndMolmodelUserGuide.docx | Bin 0 -> 896783 bytes
Simbody/doc/SimbodyAndMolmodelUserGuide.pdf | Bin 0 -> 1246968 bytes
Simbody/doc/SimbodyTheoryManual.docx | Bin 0 -> 7164580 bytes
Simbody/doc/SimbodyTheoryManual.pdf | Bin 0 -> 2751495 bytes
Simbody/doc/images/MobilizerTerminology.png | Bin 0 -> 46267 bytes
Simbody/examples/Bricard_EVEN_PART.obj | 36 +
Simbody/examples/Bricard_ODD_PART.obj | 36 +
Simbody/examples/CMakeLists.txt | 107 +
Simbody/examples/ChainExample.cpp | 333 +
Simbody/examples/ExampleAmysIKProblem.cpp | 2719 ++
Simbody/examples/ExampleAssemblerPlayground.cpp | 273 +
Simbody/examples/ExampleBricardMechanism.cpp | 216 +
Simbody/examples/ExampleCablePath.cpp | 359 +
.../examples/ExampleClosedTopologyMechanism.cpp | 216 +
Simbody/examples/ExampleContactPlayground.cpp | 626 +
Simbody/examples/ExampleCustomConstraint.cpp | 242 +
Simbody/examples/ExampleEventHandler.cpp | 76 +
Simbody/examples/ExampleEventReporter.cpp | 78 +
Simbody/examples/ExampleGears.cpp | 83 +
Simbody/examples/ExampleGeodesic.cpp | 355 +
Simbody/examples/ExampleKneeJoint.cpp | 315 +
Simbody/examples/ExampleLongPendulum.cpp | 74 +
.../ExampleMotor-TorqueLimited-Constraint.cpp | 577 +
.../ExampleMotor-TorqueLimited-Controller.cpp | 528 +
.../examples/ExampleMotor-TorqueLimited-Motion.cpp | 630 +
Simbody/examples/ExampleMotorWithSpeedControl.cpp | 354 +
Simbody/examples/ExamplePendulum.cpp | 220 +
Simbody/examples/ExampleSampleAndHold.cpp | 166 +
Simbody/examples/ExampleScissorLift.cpp | 274 +
Simbody/examples/ExampleSimplePlanarMechanism.cpp | 95 +
Simbody/examples/ExampleTwoBoxCollide.cpp | 368 +
Simbody/examples/ExampleWrapping.cpp | 318 +
Simbody/examples/Gazebo2Simbody.cpp | 1123 +
Simbody/examples/Gazebo_double_pendulum.sdf | 115 +
Simbody/examples/Gazebo_ragdoll.sdf | 1866 ++
.../examples/Gazebo_revolute_joint_test_merged.sdf | 2675 ++
Simbody/examples/InstalledCMakeLists.txt | 114 +
Simbody/examples/JaredsDude.cpp | 580 +
Simbody/examples/Makefile | 63 +
Simbody/examples/PendulumNoViz.cpp | 94 +
Simbody/examples/README.txt | 63 +
Simbody/examples/README_Makefile.txt | 48 +
Simbody/examples/README_VisualStudio.txt | 137 +
Simbody/examples/Rattleback.cpp | 361 +
Simbody/examples/SimbodyInstallTest.cpp | 73 +
Simbody/examples/SimbodyInstallTestNoViz.cpp | 94 +
Simbody/examples/TheoJansenStrandbeest.cpp | 482 +
Simbody/include/SimTKsimbody.h | 76 +
Simbody/include/SimTKsimbody_aux.h | 12 +
Simbody/include/Simbody.h | 42 +
Simbody/include/simbody/internal/Assembler.h | 1445 +
Simbody/include/simbody/internal/Body.h | 291 +
Simbody/include/simbody/internal/CablePath.h | 371 +
Simbody/include/simbody/internal/CableSpring.h | 391 +
.../simbody/internal/CableTrackerSubsystem.h | 93 +
.../simbody/internal/CompliantContactSubsystem.h | 866 +
Simbody/include/simbody/internal/Constraint.h | 2530 ++
Simbody/include/simbody/internal/ContactSurface.h | 462 +
.../simbody/internal/ContactTrackerSubsystem.h | 375 +
.../include/simbody/internal/DecorationSubsystem.h | 83 +
.../simbody/internal/ElasticFoundationForce.h | 133 +
Simbody/include/simbody/internal/Force.h | 392 +
Simbody/include/simbody/internal/ForceSubsystem.h | 53 +
.../include/simbody/internal/ForceSubsystemGuts.h | 68 +
Simbody/include/simbody/internal/Force_BuiltIns.h | 48 +
Simbody/include/simbody/internal/Force_Custom.h | 274 +
.../simbody/internal/Force_DiscreteForces.h | 182 +
Simbody/include/simbody/internal/Force_Gravity.h | 554 +
.../include/simbody/internal/Force_LinearBushing.h | 601 +
.../simbody/internal/Force_MobilityConstantForce.h | 131 +
.../simbody/internal/Force_MobilityDiscreteForce.h | 130 +
.../simbody/internal/Force_MobilityLinearDamper.h | 137 +
.../simbody/internal/Force_MobilityLinearSpring.h | 186 +
.../simbody/internal/Force_MobilityLinearStop.h | 224 +
.../include/simbody/internal/Force_Thermostat.h | 292 +
.../simbody/internal/GeneralContactSubsystem.h | 133 +
.../simbody/internal/GeneralForceSubsystem.h | 108 +
.../include/simbody/internal/HuntCrossleyContact.h | 112 +
.../include/simbody/internal/HuntCrossleyForce.h | 166 +
.../simbody/internal/LocalEnergyMinimizer.h | 57 +
Simbody/include/simbody/internal/MobilizedBody.h | 1884 ++
.../include/simbody/internal/MobilizedBody_Ball.h | 120 +
.../simbody/internal/MobilizedBody_BendStretch.h | 91 +
.../simbody/internal/MobilizedBody_BuiltIns.h | 55 +
.../simbody/internal/MobilizedBody_Bushing.h | 258 +
.../simbody/internal/MobilizedBody_Custom.h | 463 +
.../simbody/internal/MobilizedBody_Cylinder.h | 90 +
.../simbody/internal/MobilizedBody_Ellipsoid.h | 149 +
.../include/simbody/internal/MobilizedBody_Free.h | 154 +
.../simbody/internal/MobilizedBody_FreeLine.h | 105 +
.../simbody/internal/MobilizedBody_FunctionBased.h | 197 +
.../simbody/internal/MobilizedBody_Gimbal.h | 233 +
.../simbody/internal/MobilizedBody_Ground.h | 66 +
.../internal/MobilizedBody_LineOrientation.h | 134 +
.../include/simbody/internal/MobilizedBody_Pin.h | 144 +
.../simbody/internal/MobilizedBody_Planar.h | 122 +
.../include/simbody/internal/MobilizedBody_Screw.h | 109 +
.../simbody/internal/MobilizedBody_Slider.h | 132 +
.../internal/MobilizedBody_SphericalCoords.h | 185 +
.../simbody/internal/MobilizedBody_Translation.h | 148 +
.../simbody/internal/MobilizedBody_Universal.h | 88 +
.../include/simbody/internal/MobilizedBody_Weld.h | 89 +
Simbody/include/simbody/internal/Motion.h | 535 +
Simbody/include/simbody/internal/MultibodySystem.h | 113 +
.../include/simbody/internal/ObservedPointFitter.h | 129 +
.../simbody/internal/SimbodyMatterSubsystem.h | 2772 ++
.../simbody/internal/SimbodyMatterSubtree.h | 290 +
.../simbody/internal/TextDataEventReporter.h | 83 +
Simbody/include/simbody/internal/common.h | 285 +
Simbody/sharedTarget/CMakeLists.txt | 64 +
Simbody/src/About.cpp | 115 +
Simbody/src/Assembler.cpp | 1087 +
Simbody/src/Body.cpp | 202 +
Simbody/src/BodyRep.h | 193 +
Simbody/src/CablePath.cpp | 1673 +
Simbody/src/CablePath_Impl.h | 764 +
Simbody/src/CableSpring.cpp | 321 +
Simbody/src/CableTrackerSubsystem.cpp | 87 +
Simbody/src/CableTrackerSubsystem_Impl.h | 268 +
Simbody/src/CompliantContactSubsystem.cpp | 1666 +
Simbody/src/Constraint.cpp | 4073 +++
Simbody/src/ConstraintImpl.h | 3244 ++
Simbody/src/ContactTrackerSubsystem.cpp | 671 +
Simbody/src/DecorationSubsystem.cpp | 143 +
Simbody/src/DecorationSubsystemRep.h | 171 +
Simbody/src/ElasticFoundationForce.cpp | 208 +
Simbody/src/ElasticFoundationForceImpl.h | 79 +
Simbody/src/Force.cpp | 1098 +
Simbody/src/ForceImpl.h | 652 +
Simbody/src/ForceSubsystem.cpp | 72 +
Simbody/src/Force_Gravity.cpp | 589 +
Simbody/src/Force_Instantiations.cpp | 39 +
Simbody/src/Force_LinearBushing.cpp | 615 +
Simbody/src/Force_Thermostat.cpp | 524 +
Simbody/src/GeneralContactSubsystem.cpp | 321 +
Simbody/src/GeneralForceSubsystem.cpp | 427 +
Simbody/src/HuntCrossleyContact.cpp | 478 +
Simbody/src/HuntCrossleyForce.cpp | 180 +
Simbody/src/HuntCrossleyForceImpl.h | 75 +
Simbody/src/LocalEnergyMinimizer.cpp | 102 +
Simbody/src/Matter_Instantiations.cpp | 67 +
Simbody/src/MobilizedBody.cpp | 3043 ++
Simbody/src/MobilizedBodyImpl.h | 1842 ++
Simbody/src/Motion.cpp | 404 +
Simbody/src/MotionImpl.h | 442 +
Simbody/src/MultibodySystem.cpp | 335 +
Simbody/src/MultibodySystemRep.h | 512 +
Simbody/src/OBSOLETE_LengthConstraints.cpp | 1387 +
Simbody/src/OBSOLETE_LengthConstraints.h | 464 +
Simbody/src/OBSOLETE_NewtonRaphson.h | 147 +
Simbody/src/ObservedPointFitter.cpp | 377 +
Simbody/src/RigidBodyNode.cpp | 253 +
Simbody/src/RigidBodyNode.h | 1093 +
Simbody/src/RigidBodyNodeSpec.cpp | 880 +
Simbody/src/RigidBodyNodeSpec.h | 759 +
Simbody/src/RigidBodyNodeSpec_Ball.h | 452 +
Simbody/src/RigidBodyNodeSpec_Bushing.h | 228 +
Simbody/src/RigidBodyNodeSpec_Custom.h | 269 +
Simbody/src/RigidBodyNodeSpec_Cylinder.h | 163 +
Simbody/src/RigidBodyNodeSpec_Derived.cpp | 302 +
Simbody/src/RigidBodyNodeSpec_Ellipsoid.h | 552 +
Simbody/src/RigidBodyNodeSpec_Free.h | 510 +
Simbody/src/RigidBodyNodeSpec_FreeLine.h | 510 +
Simbody/src/RigidBodyNodeSpec_Gimbal.h | 210 +
Simbody/src/RigidBodyNodeSpec_LineOrientation.h | 419 +
Simbody/src/RigidBodyNodeSpec_Pin.h | 163 +
Simbody/src/RigidBodyNodeSpec_Planar.h | 158 +
Simbody/src/RigidBodyNodeSpec_PolarCoords.h | 205 +
Simbody/src/RigidBodyNodeSpec_Screw.h | 171 +
Simbody/src/RigidBodyNodeSpec_Slider.h | 149 +
Simbody/src/RigidBodyNodeSpec_SphericalCoords.h | 313 +
Simbody/src/RigidBodyNodeSpec_Translation.h | 157 +
Simbody/src/RigidBodyNodeSpec_Universal.h | 197 +
Simbody/src/RigidBodyNode_LoneParticle.cpp | 489 +
Simbody/src/RigidBodyNode_Weld.cpp | 723 +
Simbody/src/SimbodyMatterSubsystem.cpp | 2208 ++
Simbody/src/SimbodyMatterSubsystemRep.cpp | 6216 ++++
Simbody/src/SimbodyMatterSubsystemRep.h | 1328 +
Simbody/src/SimbodyMatterSubtree.cpp | 1313 +
Simbody/src/SimbodyTreeState.h | 1696 ++
Simbody/src/TextDataEventReporter.cpp | 99 +
Simbody/staticTarget/CMakeLists.txt | 63 +
Simbody/tests/CMakeLists.txt | 56 +
Simbody/tests/GazeboInelasticCollision.cpp | 319 +
Simbody/tests/GazeboReactionForce.cpp | 290 +
.../tests/GazeboReactionForceWithAppliedForce.cpp | 362 +
Simbody/tests/TestAngleConversions.cpp | 102 +
Simbody/tests/TestCollisionDetectionAlgorithm.cpp | 551 +
Simbody/tests/TestConstraints.cpp | 601 +
Simbody/tests/TestCustomConstraints.cpp | 675 +
Simbody/tests/TestCustomMobilizedBodies.cpp | 478 +
Simbody/tests/TestElasticFoundationForce.cpp | 143 +
Simbody/tests/TestForces.cpp | 372 +
Simbody/tests/TestFunctionBasedMobilizedBodies.cpp | 1097 +
Simbody/tests/TestGravity.cpp | 380 +
Simbody/tests/TestHuntCrossleyForce.cpp | 148 +
Simbody/tests/TestLinearBushing.cpp | 754 +
Simbody/tests/TestLoneParticle.cpp | 169 +
Simbody/tests/TestMassMatrix.cpp | 1248 +
Simbody/tests/TestMobilizedBody.cpp | 280 +
Simbody/tests/TestMobilizerReactionForces.cpp | 547 +
Simbody/tests/TestNoseHooverThermostat.cpp | 288 +
Simbody/tests/TestObservedPointFitter.cpp | 208 +
Simbody/tests/TestOrientedBoundingBox.cpp | 257 +
Simbody/tests/TestPimpl1.cpp | 35 +
Simbody/tests/TestReverseMobilizers.cpp | 676 +
Simbody/tests/adhoc/CMakeLists.txt | 59 +
.../tests/adhoc/CableOverBicubicSurfaces-femur.vtp | 1842 ++
.../tests/adhoc/CableOverBicubicSurfaces-tibia.vtp | 464 +
Simbody/tests/adhoc/CableOverBicubicSurfaces.cpp | 245 +
Simbody/tests/adhoc/CoarseRNA.cpp | 412 +
Simbody/tests/adhoc/ContactBigMeshes.cpp | 294 +
Simbody/tests/adhoc/ContactBigMeshes_Femur.obj | 19612 ++++++++++++
Simbody/tests/adhoc/ContactBigMeshes_Patella.obj | 6504 ++++
Simbody/tests/adhoc/ContactTest.cpp | 429 +
Simbody/tests/adhoc/ElasticFoundation.cpp | 247 +
Simbody/tests/adhoc/FreeWater.cpp | 219 +
Simbody/tests/adhoc/GeometryPlayground.cpp | 744 +
Simbody/tests/adhoc/JunkMain1.cpp | 217 +
Simbody/tests/adhoc/JunkMain2.cpp | 191 +
Simbody/tests/adhoc/LockUnlockConstraint.cpp | 473 +
Simbody/tests/adhoc/MiscConstraints.cpp | 436 +
Simbody/tests/adhoc/MovingMusclePointMomentArm.cpp | 455 +
Simbody/tests/adhoc/OpenSimPartyDemoCable.cpp | 261 +
Simbody/tests/adhoc/PassThrough.cpp | 411 +
Simbody/tests/adhoc/PendulumExample.cpp | 179 +
Simbody/tests/adhoc/PrescribedMotionPlayground.cpp | 387 +
Simbody/tests/adhoc/RadusDrifter.cpp | 249 +
.../tests/adhoc/RattleBack_ReverseEllipsoid.cpp | 183 +
Simbody/tests/adhoc/SBPendulum1.cpp | 402 +
.../tests/adhoc/SphericalCoordsMobilizerTest.cpp | 196 +
Simbody/tests/adhoc/TestMultibodyPerformance.cpp | 592 +
Simbody/tests/adhoc/TestPLUS_SingleBrick.cpp | 2778 ++
Simbody/tests/adhoc/TestRiboseMobilizer.cpp | 528 +
Simbody/tests/adhoc/TestThermostat.cpp | 202 +
Simbody/tests/adhoc/TimsBox.cpp | 3340 ++
Simbody/tests/adhoc/TimsBoxBristle.cpp | 1189 +
Simbody/tests/adhoc/TimsBoxHybrid.cpp | 1731 ++
Simbody/tests/adhoc/TimsBoxPGS.cpp | 2172 ++
Simbody/tests/adhoc/TwoPendulums.cpp | 405 +
Simbody/tests/adhoc/UnilateralPointContact.cpp | 1391 +
.../adhoc/UnilateralPointContactWithFriction.cpp | 2921 ++
Simbody/tests/adhoc/WristMomentArm.cpp | 908 +
SimbodyAPI.html | 5 +
SimbodyMainpage.h | 190 +
cmake/FindSimbody.cmake | 287 +
cmake/SampleCMakeLists.txt | 22 +
cmake/SimbodyConfig.cmake.in | 216 +
cmake/SimbodyConfigVersion.cmake.in | 55 +
cmake/cmake_uninstall.cmake.in | 21 +
cmake/pkgconfig/simbody.pc.in | 10 +
debian/changelog | 12 +
debian/compat | 1 +
debian/control | 78 +
debian/copyright | 312 +
debian/libsimbody-dev.install | 4 +
debian/libsimbody-doc.doc-base | 9 +
debian/libsimbody-doc.docs | 1 +
debian/libsimbody-doc.install | 1 +
debian/libsimbody3.4.install | 1 +
debian/rules | 41 +
debian/source/format | 1 +
doc/HowToBuildSimbodyFromSource_MacLinux.doc | Bin 0 -> 564224 bytes
doc/HowToBuildSimbodyFromSource_MacLinux.pdf | Bin 0 -> 1200874 bytes
doc/HowToBuildSimbodyFromSource_Windows.doc | Bin 0 -> 594432 bytes
doc/HowToBuildSimbodyFromSource_Windows.pdf | Bin 0 -> 1187875 bytes
doc/INSTALL.txt | 52 +
doc/LICENSE.txt | 74 +
doc/images/isaac.png | Bin 0 -> 70267 bytes
doc/images/simbios_11000_body_RNA.gif | Bin 0 -> 648538 bytes
994 files changed, 452960 insertions(+)
diff --git a/.gitattributes b/.gitattributes
new file mode 100644
index 0000000..412eeda
--- /dev/null
+++ b/.gitattributes
@@ -0,0 +1,22 @@
+# Auto detect text files and perform LF normalization
+* text=auto
+
+# Custom for Visual Studio
+*.cs diff=csharp
+*.sln merge=union
+*.csproj merge=union
+*.vbproj merge=union
+*.fsproj merge=union
+*.dbproj merge=union
+
+# Standard to msysgit
+*.doc diff=astextplain
+*.DOC diff=astextplain
+*.docx diff=astextplain
+*.DOCX diff=astextplain
+*.dot diff=astextplain
+*.DOT diff=astextplain
+*.pdf diff=astextplain
+*.PDF diff=astextplain
+*.rtf diff=astextplain
+*.RTF diff=astextplain
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..ae3f670
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,12 @@
+# Debian packaging output.
+debian/libsimbody-dev
+debian/libsimbody-doc
+debian/libsimbody3.3
+debian/libsimbody3.3-dbg
+debian/tmp
+debian/files
+debian/*log
+debian/*debhelper
+debian/*substvars
+# The build for debian ends up in a directory like obj-x86_64-linux-gnu.
+obj-*
diff --git a/.travis.yml b/.travis.yml
new file mode 100644
index 0000000..90a80e9
--- /dev/null
+++ b/.travis.yml
@@ -0,0 +1,24 @@
+language: cpp
+compiler:
+ - gcc
+ - clang
+install:
+ - sudo apt-get update
+ - sudo apt-get install liblapack-dev liblas-dev
+ # from https://github.com/travis-ci/travis-ci/issues/979.
+ - sudo add-apt-repository ppa:ubuntu-toolchain-r/test -y
+ - sudo apt-get update -qq
+ - if [ "$CXX" = "g++" ]; then sudo apt-get install -qq g++-4.8; fi
+ - if [ "$CXX" = "g++" ]; then export CXX="g++-4.8" CC="gcc-4.8"; fi
+script:
+ # First, using c++03.
+ # Specify that we do not use 11, since
+ # the default for this variable may change soon.
+ - cmake . -DBUILD_EXAMPLES=ON -DBUILD_VISUALIZER=OFF -DSIMBODY_STANDARD_11=OFF
+ # Check which compiler and compiler version we're using.
+ - make -j8
+ - ctest -j8
+ # Then, test using c++11.
+ - if [ "$CXX" = "g++-4.8" ]; then cmake . -DSIMBODY_STANDARD_11=ON; fi
+ - if [ "$CXX" = "g++-4.8" ]; then make -j8; fi
+ - if [ "$CXX" = "g++-4.8" ]; then ctest -j8; fi
diff --git a/ApiDoxygen.cmake b/ApiDoxygen.cmake
new file mode 100644
index 0000000..d64d8be
--- /dev/null
+++ b/ApiDoxygen.cmake
@@ -0,0 +1,20 @@
+INCLUDE(FindDoxygen)
+
+IF(DOXYGEN_EXECUTABLE-NOTFOUND)
+ELSE(DOXYGEN_EXECUTABLE-NOTFOUND)
+ SET(DOXY_CONFIG "${CMAKE_CURRENT_BINARY_DIR}/Doxyfile")
+
+ CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile.in
+ ${DOXY_CONFIG}
+ @ONLY )
+
+ ADD_CUSTOM_TARGET(doxygen ${DOXYGEN_EXECUTABLE} ${DOXY_CONFIG})
+
+ FILE(MAKE_DIRECTORY "${PROJECT_BINARY_DIR}/html/")
+ INSTALL(DIRECTORY "${PROJECT_BINARY_DIR}/html/"
+ DESTINATION "${CMAKE_INSTALL_DOCDIR}/api/${PROJECT_NAME}/html"
+ )
+ # This is just a shortcut to the Doxygen index.html.
+ INSTALL(FILES "SimbodyAPI.html" DESTINATION "${CMAKE_INSTALL_DOCDIR}")
+ENDIF(DOXYGEN_EXECUTABLE-NOTFOUND)
+
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..b5d1526
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,633 @@
+#---------------------------------------------------
+# Simbody
+#
+# This is the master CMake file that coordinates
+# the build of Simbody. There are four steps:
+# (1) Choose appropriate platform
+# (2) Build SimTKcommon
+# (3) Build SimTKmath
+# (4) Build SimTKsimbody
+#
+#----------------------------------------------------
+
+cmake_minimum_required(VERSION 2.8)
+
+if(COMMAND cmake_policy)
+ cmake_policy(SET CMP0003 NEW)
+ cmake_policy(SET CMP0005 NEW)
+endif(COMMAND cmake_policy)
+
+PROJECT(Simbody)
+
+# Include GNUInstallDirs to get canonical paths
+include(GNUInstallDirs)
+
+IF(WIN32)
+ set (CMAKE_INSTALL_DOCDIR doc)
+ELSE()
+ # Redefine DOCDIR to use the project name in lowercase to avoid
+ # problems with some platforms: NTFS on Win, XFS or JFS variants
+ set (CMAKE_INSTALL_DOCDIR ${CMAKE_INSTALL_DATAROOTDIR}/doc/simbody)
+ENDIF()
+
+SET(SIMBODY_MAJOR_VERSION 3)
+SET(SIMBODY_MINOR_VERSION 4)
+SET(SIMBODY_PATCH_VERSION 1)
+
+SET(SIMBODY_COPYRIGHT_YEARS "2005-14")
+
+# underbar separated list of dotted authors, no spaces or commas
+SET(SIMBODY_AUTHORS "Michael.Sherman_Peter.Eastman")
+
+# Report the version number to the CMake UI. Don't include the
+# build version if it is zero.
+SET(PATCH_VERSION_STRING)
+IF(SIMBODY_PATCH_VERSION)
+ SET(PATCH_VERSION_STRING ".${SIMBODY_PATCH_VERSION}")
+ENDIF()
+
+SET(SIMBODY_VERSION
+ "${SIMBODY_MAJOR_VERSION}.${SIMBODY_MINOR_VERSION}${PATCH_VERSION_STRING}"
+ CACHE STRING
+ "This is the version that will be built (can't be changed here)."
+ FORCE)
+
+SET(SIMBODY_SONAME_VERSION
+ "${SIMBODY_MAJOR_VERSION}.${SIMBODY_MINOR_VERSION}"
+ CACHE STRING
+ "Soname version to use when generating shared libs")
+
+# This is the suffix if we're building and depending on versioned libraries.
+SET(VN "_${SIMBODY_VERSION}")
+
+SET(BUILD_BINARY_DIR ${CMAKE_BINARY_DIR}
+ CACHE PATH
+ "The Simbody build (not the install) puts all the libraries and executables together here (with /Release, etc. appended on some platforms).")
+
+# Make everything go in the same binary directory. (These are CMake-defined
+# variables.)
+SET(EXECUTABLE_OUTPUT_PATH ${BUILD_BINARY_DIR})
+SET(LIBRARY_OUTPUT_PATH ${BUILD_BINARY_DIR})
+
+# Static libraries, tests, and examples won't be built unless this
+# is set.
+SET(BUILD_STATIC_LIBRARIES FALSE CACHE BOOL
+"Build '_static' versions of libraries in addition to dynamic libraries?")
+
+# Use this to generate a private set of libraries whose names
+# won't conflict with installed versions.
+SET(BUILD_USING_NAMESPACE "" CACHE STRING
+ "All library names will be prefixed with 'xxx_' if this is set to xxx.")
+
+SET(BUILD_UNVERSIONED_LIBRARIES TRUE CACHE BOOL
+ "Build library names, and assume dependency names, with no version numbers?")
+
+SET(BUILD_VERSIONED_LIBRARIES FALSE CACHE BOOL
+ "Build library names, and assume dependency names, with version numbers?")
+
+SET(NS)
+IF(BUILD_USING_NAMESPACE)
+ SET(NS "${BUILD_USING_NAMESPACE}_")
+ENDIF(BUILD_USING_NAMESPACE)
+
+
+#
+# These are the names of all the libraries we may generate. These are
+# target names so can be used to specify dependencies of one library
+# on another. (In Debug mode the actual targets will have "_d" appended.)
+#
+
+SET(SimTKSIMBODY_LIBRARY_NAME ${NS}SimTKsimbody CACHE STRING
+"Base name of the library being built; can't be changed here; see BUILD_USING_NAMESPACE variable."
+FORCE)
+SET(SimTKMATH_LIBRARY_NAME ${NS}SimTKmath CACHE STRING
+"Base name of the library being built; can't be changed here; see BUILD_USING_NAMESPACE variable."
+FORCE)
+SET(SimTKCOMMON_LIBRARY_NAME ${NS}SimTKcommon CACHE STRING
+"Base name of the library being built; can't be changed here; see BUILD_USING_NAMESPACE variable."
+FORCE)
+
+
+SET(SimTKCOMMON_SHARED_LIBRARY ${SimTKCOMMON_LIBRARY_NAME})
+SET(SimTKCOMMON_STATIC_LIBRARY ${SimTKCOMMON_LIBRARY_NAME}_static)
+
+SET(SimTKCOMMON_LIBRARY_NAME_VN ${NS}SimTKcommon${VN})
+SET(SimTKCOMMON_SHARED_LIBRARY_VN ${SimTKCOMMON_LIBRARY_NAME_VN})
+SET(SimTKCOMMON_STATIC_LIBRARY_VN ${SimTKCOMMON_LIBRARY_NAME_VN}_static)
+
+SET(SimTKMATH_SHARED_LIBRARY ${SimTKMATH_LIBRARY_NAME})
+SET(SimTKMATH_STATIC_LIBRARY ${SimTKMATH_LIBRARY_NAME}_static)
+
+SET(SimTKMATH_LIBRARY_NAME_VN ${NS}SimTKmath${VN})
+SET(SimTKMATH_SHARED_LIBRARY_VN ${SimTKMATH_LIBRARY_NAME_VN})
+SET(SimTKMATH_STATIC_LIBRARY_VN ${SimTKMATH_LIBRARY_NAME_VN}_static)
+
+SET(SimTKSIMBODY_SHARED_LIBRARY ${SimTKSIMBODY_LIBRARY_NAME})
+SET(SimTKSIMBODY_STATIC_LIBRARY ${SimTKSIMBODY_LIBRARY_NAME}_static)
+
+SET(SimTKSIMBODY_LIBRARY_NAME_VN ${NS}SimTKsimbody${VN})
+SET(SimTKSIMBODY_SHARED_LIBRARY_VN ${SimTKSIMBODY_LIBRARY_NAME_VN})
+SET(SimTKSIMBODY_STATIC_LIBRARY_VN ${SimTKSIMBODY_LIBRARY_NAME_VN}_static)
+
+
+# Caution: this variable is automatically created by the CMake
+# ENABLE_TESTING() command, but we'll take it over here for
+# our own purposes too.
+SET(BUILD_TESTING ON CACHE BOOL
+ "Control building of Simbody test programs.
+ To actually build tests, one
+ or both of BUILD_TESTS_AND_EXAMPLES_STATIC and
+ BUILD_TESTS_AND_EXAMPLES_SHARED must be ON.")
+
+SET(BUILD_EXAMPLES ON CACHE BOOL
+ "Control building of Simbody example programs.
+ To actually build examples, one
+ or both of BUILD_TESTS_AND_EXAMPLES_STATIC and
+ BUILD_TESTS_AND_EXAMPLES_SHARED must be ON.")
+
+# Set whether to build the Visualizer code.
+SET(BUILD_VISUALIZER ON CACHE BOOL
+ "Control building of the visualizer component")
+
+# Turning this off reduces the build time (and space) substantially,
+# but you may miss the occasional odd bug. Also currently on Windows it
+# is easier to debug the static tests than the DLL-linked ones.
+SET(BUILD_TESTS_AND_EXAMPLES_STATIC ON CACHE BOOL
+ "If BUILDING_STATIC_LIBRARIES and BUILD_TESTING or BUILD_EXAMPLES, build
+ statically-linked test and example programs too? On Windows,
+ statically-linked tests may be easier to debug than DLL-linked tests.
+ Statically-linked examples are never installed.")
+MARK_AS_ADVANCED(BUILD_TESTS_AND_EXAMPLES_STATIC)
+
+SET(BUILD_TESTS_AND_EXAMPLES_SHARED ON CACHE BOOL
+ "If BUILD_TESTING or BUILD_EXAMPLES, build dynamically-linked ones?")
+MARK_AS_ADVANCED(BUILD_TESTS_AND_EXAMPLES_SHARED)
+
+IF(BUILD_TESTING AND NOT (BUILD_TESTS_AND_EXAMPLES_STATIC OR
+ BUILD_TESTS_AND_EXAMPLES_SHARED))
+ MESSAGE(SEND_ERROR "No tests would be built, despite BUILD_EXAMPLES"
+ "being on, because BUILD_TESTS_AND_EXAMPLES_STATIC and "
+ "BUILD_TESTS_AND_EXAMPLES_SHARED are both off.")
+ENDIF()
+
+IF(BUILD_EXAMPLES AND NOT (BUILD_TESTS_AND_EXAMPLES_STATIC OR
+ BUILD_TESTS_AND_EXAMPLES_SHARED))
+ MESSAGE(SEND_ERROR
+ "No examples would be built, despite BUILD_EXAMPLES being on, "
+ "because BUILD_TESTS_AND_EXAMPLES_STATIC and "
+ "BUILD_TESTS_AND_EXAMPLES_SHARED are both off.")
+ENDIF()
+
+#
+# Create a platform name useful for finding things in the Platform
+# directory.
+IF(WIN32)
+ SET(PLATFORM_NAME Windows)
+ SET(NATIVE_COPY_CMD copy)
+ELSEIF(APPLE)
+ SET(PLATFORM_NAME Mac)
+ SET(NATIVE_COPY_CMD cp)
+ELSE()
+ SET(PLATFORM_NAME Linux)
+ SET(NATIVE_COPY_CMD cp)
+ENDIF()
+
+# In addition to the platform name we need to know the Application Binary
+# Interface (ABI) we're building for. Currently that is either x86, meaning
+# 32 bit Intel instruction set, or x64 for 64 bit Intel instruction set.
+
+IF(${CMAKE_SIZEOF_VOID_P} EQUAL 8)
+ SET(PLATFORM_ABI x64)
+ELSE()
+ SET(PLATFORM_ABI x86)
+ENDIF()
+
+SET(BUILD_PLATFORM "${PLATFORM_NAME}:${PLATFORM_ABI}" CACHE STRING
+ "This is the platform and ABI we're building for. Not changeable here; use a different CMake generator instead."
+ FORCE)
+
+#
+# Make sure "sandbox" input & output directories are set. During
+# SimTK Core build, the build system will set them. Otherwise, we'll
+# set them to sensible local values.
+# If CMAKE_INSTALL_PREFIX is set then
+# it is a sandbox installation area, otherwise we want to install
+# in /usr/local/simbody or %ProgramFiles%\Simbody.
+#
+
+
+# For backward compatibility, use SimTK_INSTALL_PREFIX if
+# CMAKE_INSTALL_PREFIX hasn't been set.
+IF(SimTK_INSTALL_PREFIX AND CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT)
+ SET(CMAKE_INSTALL_PREFIX ${SimTK_INSTALL_PREFIX} CACHE PATH
+ "Install path prefix." FORCE)
+ENDIF()
+
+# C compiler is gcc on Linux, gcc or cc on Mac where command line tools
+# are installed from XCode. May be Clang in Mac 10.8 or later.
+
+IF(${CMAKE_C_COMPILER} MATCHES "cc" OR ${CMAKE_CXX_COMPILER_ID} MATCHES "Clang")
+ IF(NOT CMAKE_INSTALL_PREFIX)
+ SET(CMAKE_INSTALL_PREFIX "/usr/local/simbody"
+ CACHE PATH "Install directory")
+ ENDIF(NOT CMAKE_INSTALL_PREFIX)
+
+ELSE() # Windows
+ # On Win64, there are two environment variables, ProgramFiles(x86) for
+ # 32 bit binaries, and ProgramW6432 for 64 bit binaries. The variable
+ # ProgramFiles returns one or the other depending on whether the requesting
+ # executable is 32 or 64 bits. That's CMake in this case, but we want
+ # the right directory for the target. CMAKE_INSTALL_DIR after 2.8.3 gets
+ # this right but still isn't what we want.
+ IF( ${CMAKE_SIZEOF_VOID_P} EQUAL 8 )
+ # 64 bit target on Win64
+ set(PROGFILEDIR "$ENV{ProgramW6432}")
+ ELSE() # Target is 32 bit
+ set(PROGFILEDIR "$ENV{ProgramFiles(x86)}") # present if 64bit Windows
+ if (NOT PROGFILEDIR)
+ set(PROGFILEDIR "$ENV{ProgramFiles}") # on 32bit Windows
+ endif()
+ ENDIF()
+
+ IF(NOT CMAKE_INSTALL_PREFIX)
+ SET(CMAKE_INSTALL_PREFIX "${PROGFILEDIR}/Simbody"
+ CACHE PATH "Install directory")
+ ENDIF(NOT CMAKE_INSTALL_PREFIX)
+
+ENDIF()
+
+IF(UNIX AND NOT CMAKE_BUILD_TYPE)
+ SET(CMAKE_BUILD_TYPE Release CACHE STRING "Debug or Release build"
+ FORCE)
+ENDIF (UNIX AND NOT CMAKE_BUILD_TYPE)
+
+
+## Choose the maximum level of x86 instruction set that the compiler is
+## allowed to use. SSE2 is ubiquitous enough now that we don't mind
+## abandoning machines that can't handle those instructions. SSE3 might
+## also be reasonable by now (April 2009) so this default should be
+## revisited soon. On 64 bit MSVC, the default is sse2 and the argument
+## isn't recognized so we won't specify it.
+if (CMAKE_CL_64)
+ set(default_build_inst_set)
+else()
+ set(default_build_inst_set "sse2")
+endif()
+
+## This can be set to a different value by the person running CMake.
+SET(BUILD_INST_SET ""
+ CACHE STRING
+ "CPU instruction level compiler is permitted to use (default is sse2).")
+MARK_AS_ADVANCED( BUILD_INST_SET )
+
+if (BUILD_INST_SET)
+ set(inst_set_to_use ${BUILD_INST_SET})
+else()
+ set(inst_set_to_use ${default_build_inst_set})
+endif()
+
+## When building in any of the Release modes, tell gcc to use full
+## optimization and to generate SSE2 floating point instructions. Here we
+## are specifying *all* of the Release flags, overriding CMake's defaults.
+## Watch out for optimizer bugs in particular gcc versions!
+
+IF(${CMAKE_CXX_COMPILER_ID} MATCHES "GNU" OR
+ ${CMAKE_CXX_COMPILER_ID} MATCHES "Clang")
+
+ # If using either of these compilers, provide the option of
+ # compiling usin the c++11 standard.
+ OPTION(SIMBODY_STANDARD_11
+ "Compile using the c++11 standard instead of c++03,
+ if using GCC or Clang." OFF)
+
+ IF(${SIMBODY_STANDARD_11})
+ LIST(APPEND CMAKE_CXX_FLAGS " -std=c++11")
+ ENDIF()
+
+ if (inst_set_to_use)
+ string(TOLOWER ${inst_set_to_use} GCC_INST_SET)
+ set(GCC_INST_SET "-m${GCC_INST_SET}")
+ else()
+ set(GCC_INST_SET)
+ endif()
+
+ # Get the gcc or clang version number in major.minor.build format
+ execute_process(COMMAND ${CMAKE_C_COMPILER} -dumpversion
+ OUTPUT_VARIABLE GCC_VERSION)
+
+ # Unrolling fixed-count loops is a useful optimization for Simmatrix.
+ SET(GCC_OPT_ENABLE "-funroll-loops")
+
+ # If you know of optimization bugs that affect SimTK in particular
+ # gcc versions, this is the place to turn off those optimizations.
+ SET(GCC_OPT_DISABLE)
+ # We know Gcc 4.4.3 on Ubuntu 10 is buggy and that Snow Leopard's
+ # 4.2.1 is not. To be safe for now we'll assume anything over 4.3
+ # should have these disabled.
+ IF(${CMAKE_CXX_COMPILER_ID} MATCHES "GNU")
+ if (GCC_VERSION VERSION_GREATER 4.3 OR GCC_VERSION VERSION_EQUAL 4.3)
+ SET(GCC_OPT_DISABLE
+ "-fno-strict-aliasing -fno-tree-vrp -fno-guess-branch-probability")
+ endif()
+ ENDIF()
+
+ # C++
+ SET(BUILD_CXX_FLAGS_DEBUG "-g ${GCC_INST_SET}")
+ SET(BUILD_CXX_FLAGS_RELEASE
+ "-DNDEBUG -O3 ${GCC_OPT_ENABLE} ${GCC_OPT_DISABLE} ${GCC_INST_SET}")
+ SET(BUILD_CXX_FLAGS_RELWITHDEBINFO
+ "-DNDEBUG -O3 -g ${GCC_OPT_ENABLE} ${GCC_OPT_DISABLE} ${GCC_INST_SET}")
+ SET(BUILD_CXX_FLAGS_MINSIZEREL "-DNDEBUG -Os ${GCC_INST_SET}")
+
+ # C
+ SET(BUILD_C_FLAGS_DEBUG "-g ${GCC_INST_SET}")
+ SET(BUILD_C_FLAGS_RELEASE
+ "-DNDEBUG -O3 ${GCC_OPT_ENABLE} ${GCC_OPT_DISABLE} ${GCC_INST_SET}")
+ SET(BUILD_C_FLAGS_RELWITHDEBINFO
+ "-DNDEBUG -O3 -g ${GCC_OPT_ENABLE} ${GCC_OPT_DISABLE} ${GCC_INST_SET}")
+ SET(BUILD_C_FLAGS_MINSIZEREL "-DNDEBUG -Os ${GCC_INST_SET}")
+
+ # C++
+ SET(CMAKE_CXX_FLAGS_DEBUG ${BUILD_CXX_FLAGS_DEBUG}
+ CACHE STRING "Can't change here -- see BUILD_CXX..." FORCE)
+ SET(CMAKE_CXX_FLAGS_RELEASE ${BUILD_CXX_FLAGS_RELEASE}
+ CACHE STRING "Can't change here -- see BUILD_CXX..." FORCE)
+ SET(CMAKE_CXX_FLAGS_RELWITHDEBINFO ${BUILD_CXX_FLAGS_RELWITHDEBINFO}
+ CACHE STRING "Can't change here -- see BUILD_CXX..." FORCE)
+ SET(CMAKE_CXX_FLAGS_MINSIZEREL ${BUILD_CXX_FLAGS_MINSIZEREL}
+ CACHE STRING "Can't change here -- see BUILD_CXX..." FORCE)
+
+ # C
+ SET(CMAKE_C_FLAGS_DEBUG ${BUILD_C_FLAGS_DEBUG}
+ CACHE STRING "Can't change here -- see BUILD_C..." FORCE)
+ SET(CMAKE_C_FLAGS_RELEASE ${BUILD_C_FLAGS_RELEASE}
+ CACHE STRING "Can't change here -- see BUILD_C..." FORCE)
+ SET(CMAKE_C_FLAGS_RELWITHDEBINFO ${BUILD_C_FLAGS_RELWITHDEBINFO}
+ CACHE STRING "Can't change here -- see BUILD_C..." FORCE)
+ SET(CMAKE_C_FLAGS_MINSIZEREL ${BUILD_C_FLAGS_MINSIZEREL}
+ CACHE STRING "Can't change here -- see BUILD_C..." FORCE)
+
+ENDIF()
+
+## When building in any of the Release modes, tell VC++ cl compiler to use
+## intrinsics (i.e. sqrt instruction rather than sqrt subroutine) with
+## flag /Oi.
+## Caution: can't use CMAKE_CXX_COMPILER_ID MATCHES MSVC here because
+## "MSVC" is a predefined CMAKE variable and will get expanded to 1 or 0
+IF(MSVC)
+ if (inst_set_to_use)
+ string(TOUPPER ${inst_set_to_use} CL_INST_SET)
+ set(CL_INST_SET "/arch:${CL_INST_SET}")
+ else()
+ set(CL_INST_SET)
+ endif()
+
+ set(BUILD_LIMIT_PARALLEL_COMPILES "" CACHE STRING
+ "Set a maximum number of simultaneous compilations.")
+ mark_as_advanced(BUILD_LIMIT_PARALLEL_COMPILES)
+ set(mxcpu ${BUILD_LIMIT_PARALLEL_COMPILES}) # abbreviation
+
+ ## C++
+ SET(BUILD_CXX_FLAGS_DEBUG
+ "/D _DEBUG /MDd /Od /Ob0 /RTC1 /Zi /GS- ${CL_INST_SET}")
+ SET(BUILD_CXX_FLAGS_RELEASE
+ "/D NDEBUG /MD /O2 /Ob2 /Oi /GS- ${CL_INST_SET}")
+ SET(BUILD_CXX_FLAGS_RELWITHDEBINFO
+ "/D NDEBUG /MD /O2 /Ob2 /Oi /Zi /GS- ${CL_INST_SET}")
+ SET(BUILD_CXX_FLAGS_MINSIZEREL
+ "/D NDEBUG /MD /O1 /Ob1 /Oi /GS- ${CL_INST_SET}")
+
+ ## C
+ SET(BUILD_C_FLAGS_DEBUG
+ "/D _DEBUG /MDd /Od /Ob0 /RTC1 /Zi /GS- ${CL_INST_SET}")
+ SET(BUILD_C_FLAGS_RELEASE
+ "/D NDEBUG /MD /O2 /Ob2 /Oi /GS- ${CL_INST_SET}")
+ SET(BUILD_C_FLAGS_RELWITHDEBINFO
+ "/D NDEBUG /MD /O2 /Ob2 /Oi /Zi /GS- ${CL_INST_SET}")
+ SET(BUILD_C_FLAGS_MINSIZEREL
+ "/D NDEBUG /MD /O1 /Ob1 /Oi /GS- ${CL_INST_SET}")
+
+ ## C++
+ SET(CMAKE_CXX_FLAGS_DEBUG "/MP${mxcpu} ${BUILD_CXX_FLAGS_DEBUG}"
+ CACHE STRING "Can't change here -- see BUILD_CXX..." FORCE)
+ SET(CMAKE_CXX_FLAGS_RELEASE "/MP${mxcpu} ${BUILD_CXX_FLAGS_RELEASE}"
+ CACHE STRING "Can't change here -- see BUILD_CXX..." FORCE)
+ SET(CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MP${mxcpu} ${BUILD_CXX_FLAGS_RELWITHDEBINFO}"
+ CACHE STRING "Can't change here -- see BUILD_CXX..." FORCE)
+ SET(CMAKE_CXX_FLAGS_MINSIZEREL "/MP${mxcpu} ${BUILD_CXX_FLAGS_MINSIZEREL}"
+ CACHE STRING "Can't change here -- see BUILD_CXX..." FORCE)
+
+ ## C
+ SET(CMAKE_C_FLAGS_DEBUG "/MP${mxcpu} ${BUILD_C_FLAGS_DEBUG}"
+ CACHE STRING "Can't change here -- see BUILD_C_..." FORCE)
+ SET(CMAKE_C_FLAGS_RELEASE "/MP${mxcpu} ${BUILD_C_FLAGS_RELEASE}"
+ CACHE STRING "Can't change here -- see BUILD_C_..." FORCE)
+ SET(CMAKE_C_FLAGS_RELWITHDEBINFO "/MP${mxcpu} ${BUILD_C_FLAGS_RELWITHDEBINFO}"
+ CACHE STRING "Can't change here -- see BUILD_C_..." FORCE)
+ SET(CMAKE_C_FLAGS_MINSIZEREL "/MP${mxcpu} ${BUILD_C_FLAGS_MINSIZEREL}"
+ CACHE STRING "Can't change here -- see BUILD_C_..." FORCE)
+
+ENDIF()
+
+# Collect up information about the version of the simbody library we're building
+# and make it available to the code so it can be built into the binaries.
+# TODO removed SVN_REVSION; replace with GIT_SHA1
+# http://stackoverflow.com/questions/1435953/how-can-i-pass-git-sha1-to-compiler-as-definition-using-cmake
+
+# CMake quotes automatically when building Visual Studio projects but we need
+# to add them ourselves for Linux or Cygwin. Two cases to avoid duplicate quotes
+# in Visual Studio which end up in the binary.
+
+IF (${CMAKE_GENERATOR} MATCHES "Visual Studio")
+ SET(NEED_QUOTES FALSE)
+ELSE (${CMAKE_GENERATOR} MATCHES "Visual Studio")
+ SET(NEED_QUOTES TRUE)
+ENDIF (${CMAKE_GENERATOR} MATCHES "Visual Studio")
+
+##TODO: doesn't work without quotes in nightly build
+SET(NEED_QUOTES TRUE)
+
+IF(NEED_QUOTES)
+ ADD_DEFINITIONS(-DSimTK_SIMBODY_COPYRIGHT_YEARS="${SIMBODY_COPYRIGHT_YEARS}"
+ -DSimTK_SIMBODY_AUTHORS="${SIMBODY_AUTHORS}")
+ELSE(NEED_QUOTES)
+ ADD_DEFINITIONS(-DSimTK_SIMBODY_COPYRIGHT_YEARS=${SIMBODY_COPYRIGHT_YEARS}
+ -DSimTK_SIMBODY_AUTHORS=${SIMBODY_AUTHORS})
+ENDIF(NEED_QUOTES)
+
+# Ensure that debug libraries have "_d" appended to their names.
+# CMake gets this right on Windows automatically with this definition.
+# But on Unix or Cygwin we have to add the suffix manually
+IF (${CMAKE_GENERATOR} MATCHES "Visual Studio")
+ SET(CMAKE_DEBUG_POSTFIX "_d" CACHE INTERNAL "" FORCE)
+ENDIF (${CMAKE_GENERATOR} MATCHES "Visual Studio")
+
+# Determine which math libraries to use for this platform.
+# Intel MKL: mkl_intel_c_dll;mkl_sequential_dll;mkl_core_dll
+SET(BUILD_USING_OTHER_LAPACK "" CACHE STRING
+ "If you have your own Lapack and Blas, put library basenames here, separated by semicolons. See LAPACK_BEING_USED to see what's actually being used.")
+
+if(WIN32)
+ SET(LAPACK_PLATFORM_DEFAULT liblapack;libblas)
+else()
+ SET(LAPACK_PLATFORM_DEFAULT lapack;blas)
+endif()
+
+SET(LAPACK_BEING_USED ${LAPACK_PLATFORM_DEFAULT} CACHE STRING
+ "Basename of the actual Lapack library we're depending on; can't change here; see variable BUILD_USING_OTHER_LAPACK." FORCE)
+
+IF(BUILD_USING_OTHER_LAPACK)
+ SET(LAPACK_BEING_USED ${BUILD_USING_OTHER_LAPACK} CACHE STRING
+"Basename of the actual Lapack library we're depending on; can't change here; see variable BUILD_USING_OTHER_LAPACK." FORCE)
+ENDIF(BUILD_USING_OTHER_LAPACK)
+
+IF(${CMAKE_C_COMPILER} MATCHES "cc" OR ${CMAKE_CXX_COMPILER_ID} MATCHES "Clang")
+ IF(APPLE)
+ SET(REALTIME_LIB)
+ ELSE()
+ SET(REALTIME_LIB rt)
+ ENDIF()
+ SET(MATH_LIBS_TO_USE ${LAPACK_BEING_USED} pthread ${REALTIME_LIB} dl m)
+ SET(MATH_LIBS_TO_USE_VN ${LAPACK_BEING_USED} pthread ${REALTIME_LIB} dl m)
+ELSE()
+ ## Assume Microsoft Visual Studio
+ if (${PLATFORM_ABI} MATCHES "x64")
+ SET(MATH_LIBS_TO_USE ${LAPACK_BEING_USED} pthreadVC2_x64)
+ SET(MATH_LIBS_TO_USE_VN ${LAPACK_BEING_USED} pthreadVC2_x64)
+ else()
+ SET(MATH_LIBS_TO_USE ${LAPACK_BEING_USED} pthreadVC2)
+ SET(MATH_LIBS_TO_USE_VN ${LAPACK_BEING_USED} pthreadVC2)
+ endif()
+ENDIF()
+
+#
+# Allow automated build and dashboard.
+#
+INCLUDE (Dart)
+## When in Debug mode and running valgrind, some of the test
+## cases take longer than the default 1500 seconds.
+SET(DART_TESTING_TIMEOUT 7200)
+
+IF (BUILD_TESTING)
+
+ #IF (UNIX AND NOT CYGWIN AND NOT APPLE)
+ # IF (NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE MATCHES Debug)
+ # ADD_DEFINITIONS(-fprofile-arcs -ftest-coverage)
+ # LINK_LIBRARIES(gcov)
+ # ENDIF (NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE MATCHES Debug)
+ #ENDIF (UNIX AND NOT CYGWIN AND NOT APPLE)
+
+ #
+ # Testing
+ #
+ ENABLE_TESTING()
+
+ # Make a RUN_TESTS_PARALLEL target (thanks, Kevin!)
+ # Specify number of cores to run for testing
+ SET(TESTING_PROCESSOR_COUNT 4 CACHE STRING
+ "Number of CPUs to be used by the RUN_TESTS_PARALLEL target.")
+ MARK_AS_ADVANCED(TESTING_PROCESSOR_COUNT)
+ SET (cmd ${CMAKE_CTEST_COMMAND} -j${TESTING_PROCESSOR_COUNT})
+ IF (MSVC)
+ SET (cmd ${cmd} -C ${CMAKE_CFG_INTDIR})
+ ELSE (MSVC)
+ SET (cmd ${cmd} -C ${CMAKE_BUILD_TYPE})
+ ENDIF (MSVC)
+ ADD_CUSTOM_TARGET (RUN_TESTS_PARALLEL
+ COMMAND ${cmd}
+ )
+
+ENDIF (BUILD_TESTING)
+
+INCLUDE(ApiDoxygen.cmake)
+
+# Each build should look in our local binary directory to find the
+# earlier-built libraries that it depends on.
+LINK_DIRECTORIES(${BUILD_BINARY_DIR})
+
+# Specify where visualizer should be installed. This needs to be in the
+# root CMakeLists.txt so the cmake config file can see this value.
+#
+# Also specify where include files are installed.
+IF(WIN32)
+ # Install visualizer to bin, since it needs to be co-located with dll's
+ SET(SIMBODY_VISUALIZER_INSTALL_DIR ${CMAKE_INSTALL_FULL_BINDIR})
+ # Install include files into base include folder since it's a sandbox
+ SET(SIMBODY_INCLUDE_INSTALL_DIR ${CMAKE_INSTALL_INCLUDEDIR})
+ELSE()
+ # Visualizer is not intended to be a user executable. Proper place is
+ # inside the lib directory
+ SET(SIMBODY_VISUALIZER_INSTALL_DIR ${CMAKE_INSTALL_FULL_LIBEXECDIR}/simbody)
+ # Install include files in simbody subfolder to avoid polluting the
+ # global build folder
+ SET(SIMBODY_INCLUDE_INSTALL_DIR ${CMAKE_INSTALL_INCLUDEDIR}/simbody)
+ENDIF()
+
+# Each of these returns a list of API include directories for
+# use by the later builds.
+ADD_SUBDIRECTORY( Platform )
+# PLATFORM_INCLUDE_DIRECTORIES now set
+ADD_SUBDIRECTORY( SimTKcommon )
+# SimTKCOMMON_INCLUDE_DIRECTORIES now set
+ADD_SUBDIRECTORY( SimTKmath )
+# SimTKMATH_INCLUDE_DIRECTORIES now set
+ADD_SUBDIRECTORY( Simbody )
+# SimTKSIMBODY_INCLUDE_DIRECTORIES now set (but not used)
+
+FILE(GLOB TOPLEVEL_DOCS doc/*.pdf doc/*.txt)
+INSTALL(FILES ${TOPLEVEL_DOCS} DESTINATION ${CMAKE_INSTALL_DOCDIR})
+
+# Add uninstall target
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/cmake/cmake_uninstall.cmake.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/cmake/cmake_uninstall.cmake"
+ IMMEDIATE @ONLY)
+add_custom_target(uninstall
+ "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake/cmake_uninstall.cmake")
+
+# Make the cmake config files
+set(PKG_NAME ${PROJECT_NAME})
+set(PKG_LIBRARIES
+ SimTKsimbody
+ SimTKmath
+ SimTKcommon
+)
+
+if (WIN32)
+ set(SIMBODY_CMAKE_DIR cmake)
+elseif (UNIX)
+ set(SIMBODY_CMAKE_DIR ${CMAKE_INSTALL_FULL_LIBDIR}/cmake/simbody/)
+endif ()
+
+CONFIGURE_FILE(${CMAKE_SOURCE_DIR}/cmake/SimbodyConfig.cmake.in
+ ${CMAKE_CURRENT_BINARY_DIR}/cmake/SimbodyConfig.cmake @ONLY)
+INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/cmake/SimbodyConfig.cmake
+ DESTINATION ${SIMBODY_CMAKE_DIR})
+
+CONFIGURE_FILE(${CMAKE_SOURCE_DIR}/cmake/SimbodyConfigVersion.cmake.in
+ ${CMAKE_CURRENT_BINARY_DIR}/cmake/SimbodyConfigVersion.cmake @ONLY)
+INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/cmake/SimbodyConfigVersion.cmake
+ DESTINATION ${CMAKE_INSTALL_FULL_LIBDIR}/cmake/simbody/)
+
+# Install a sample CMakeLists.txt that uses SimbodyConfig.cmake.
+INSTALL(FILES ${CMAKE_SOURCE_DIR}/cmake/SampleCMakeLists.txt
+ DESTINATION ${SIMBODY_CMAKE_DIR})
+
+# Make the pkgconfig file: select the proper flags depending on the platform
+IF (WIN32)
+ IF( ${CMAKE_SIZEOF_VOID_P} EQUAL 8)
+ # win 64 bits
+ SET(PKGCONFIG_PLATFORM_LIBS "-lliblapack -llibblas -lpthreadVC2_x64")
+ ELSE()
+ SET(PKGCONFIG_PLATFORM_LIBS "-lliblapack -llibblas -lpthreadVC2")
+ ENDIF()
+ELSEIF (APPLE)
+ SET(PKGCONFIG_PLATFORM_LIBS "-llapack -lblas -lpthread -ldl")
+ELSE()
+ SET(PKGCONFIG_PLATFORM_LIBS "-llapack -lblas -lpthread -lrt -ldl -lm")
+ENDIF()
+
+CONFIGURE_FILE(${CMAKE_SOURCE_DIR}/cmake/pkgconfig/simbody.pc.in
+ ${CMAKE_CURRENT_BINARY_DIR}/cmake/pkgconfig/simbody.pc @ONLY)
+INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/cmake/pkgconfig/simbody.pc
+ DESTINATION ${CMAKE_INSTALL_FULL_LIBDIR}/pkgconfig/)
diff --git a/CTestConfig.cmake b/CTestConfig.cmake
new file mode 100644
index 0000000..a027aef
--- /dev/null
+++ b/CTestConfig.cmake
@@ -0,0 +1,13 @@
+## This file should be placed in the root directory of your project.
+## Then modify the CMakeLists.txt file in the root directory of your
+## project to incorporate the testing dashboard.
+## # The following are required to uses Dart and the Cdash dashboard
+## ENABLE_TESTING()
+## INCLUDE(CTest)
+set(CTEST_PROJECT_NAME "Simbody")
+set(CTEST_NIGHTLY_START_TIME "00:00:00 EST")
+
+set(CTEST_DROP_METHOD "http")
+set(CTEST_DROP_SITE "simdash.stanford.edu")
+set(CTEST_DROP_LOCATION "/submit.php?project=Simbody")
+set(CTEST_DROP_SITE_CDASH TRUE)
diff --git a/ChangeLog.txt b/ChangeLog.txt
new file mode 100644
index 0000000..7509422
--- /dev/null
+++ b/ChangeLog.txt
@@ -0,0 +1,107 @@
+Changes in Simbody 3.0 since Simbody 2.2 release (July, 2011)
+(includes changes in Simbody 2.3 branch)
+20 June 2012
+--------------------------------------------------------------------------------
+
+License changed from MIT to Apache 2.0 (still unrestricted open source but
+with less ambiguity).
+
+Converting from 2.X to 3.0
+--------------------------
+- project(): drop most arguments; always does q and u and prescribed now. If
+you want to do just q or just u, call projectQ() or projectU() instead.
+- ProjectOptions has changed; no longer accepted by project(); used only by
+lower-level projectQ and projectU methods
+- handleEvent() virtual: must convert to reduced signature, get rid of
+"lowestModified"
+- constraint virtuals: changed to operators, meaning needed inputs are
+supplied as arguments rather than extracted from State
+- adding an EventHandler or EventReporter invalidates System topology now so
+you must call realizeTopology() after the last handler or reporter has been
+added
+- Integrator::successfulStepStatusString() now getSuccessfulStepStatusString()
+- removed ill-conceived String::getAs() and updAs() methods
+- Concretize<T> renamed more conventional ClonePtr<T>
+- The UserFunction<T> class used by TextDataEventReporter has been made local to that class; rename it TextDataEventReporter::UserFunction.
+
+
+Changes
+-------
+- Performance improvements to basic multibody compuations & vectors
+- Added task space support methods in SimbodyMatterSubsystem: operators
+ for working with point, frame, and system Jacobians and their time
+ derivatives.
+- Numerous fixes to Visualizer; intermittent failures and mesh problems
+ fixed.
+- Added DecorativePoint, 3-axis scaling, search path for Visualizer
+- Pathname::fileExists() added
+- Bug 1609: Cpodes was ignoring setAllowInterpolation(false).
+- OSX Lion support
+- Make Spline_ copy & copy assign work properly for null Spline_
+- Fix mishandled work arrays in FactorQTZ
+- Implemented fast prescribed motion using mixed forward/inverse computation;
+ all integrators modified accordingly
+- u's and z's are now integrated to relative tolerance; q's still absolute
+- improved CPodes handling of triggered events
+- added macros to allows use of C++11 "override" and "final" keywords
+ (OVERRIDE_11, FINAL_11). Build system defines those for compilers that
+ support them (VS10)
+- Reimplemented predefined constants as global externals to avoid static
+initialization order dependency; now safe to use these in static initializers
+- Fix Verlet's initial estimates for u and z
+- Standardized I/O for true/false/NaN/Inf values; added readUnformatted() and writeUnformatted() methods
+- Added ReferencePtr<T> useful to allow pointer data members without requiring
+user-written copy/copy assign methods
+- Fix error-check bug in Xml::Element::insertNodeBefore()
+- Reorganized geometry source to localize in SimTKmath for complicated geometry (like contact) and SimTKcommon for basic primitives
+- Added BicubicSurface/BicubicFunction and related primitives
+
+
+
+8/16 Version changed to 3.0
+8/16 Checked in FrictionConstraints doc
+8/21 Rearchitected constraint API to support use of constraints
+ as operators (BREAKING CHANGE TO CUSTOM CONSTRAINTS)
+8/25 Fixed bug 1521 in SpeedCoupler constraint; backported to 2.3.
+8/29 Fixed bug 1522 in memory-sharing Vector constructor; backported to 2.3.
+8/29 Added calcBodyAccelerationFromUDot() method to matter subsystem.
+8/29 Renamed getCoriolisAcceleration() to getMobilizerCoriolisAcceleration()
+ and getCentrifugalForces() to getMobilizerCentrifugalForces()
+ to make it clear that these are just mobilizer-incremental contributions.
+ This is a BREAKING CHANGE if anyone ever called those, but probably if
+ they did it was a mistake and they should have been calling
+ getTotalCoriolisAccleration() or getTotalCentrifugalForces() which are
+ the meaningful physical quantities.
+8/30 Added multiplyByG(), calcG() and related methods to matter subsystem.
+8/31 Fixed bug 1523: 0-width slice of 1-d matrix was broken
+9/09 Bug fixes to mesh-mesh elastic foundation code; backported to 2.2 & 2.3
+ (bugs 1527, 1528)
+9/14 calcAcceleration operators are now really operators (no effect on state
+cache).
+9/15 Added calcResidualForces() method equivalent to sdfulltrq(); inverse
+dynamics with constraints.
+9/18 Reworked pos & vel projection to use new operators; fixed pos projection
+so that it produces a normal projection in q-space (was u-space before).
+9/19 Fixed bug 1535 -- mobilizer reaction torque sometimes miscalculated.
+9/21 Replaced freebody reaction force method with faster method due to Abhi
+Jain.
+10/27 Fix to copy of matrix subblock.
+
+11/17/2011
+Stage class has been simplified. Constants like Stage::Position are just enums
+now and can be used in case statements. Previously you had to use
+Stage::PositionIndex, etc. for that purpose. Those extra enums are gone now so
+if you were using Stage::VelocityIndex just switch to Stage::Velocity.
+
+The Enumeration<> and EnumerationSet<> types have been removed since they were
+not being used any more. If anyone was using them, let me know.
+
+11/30/2011
+- Added Function::Sinusoid
+- Added additional inertia sanity check looking at products of inertia which have to obey conditions like Ixx>=|2*Iyz| (thanks to Paul Mitiguy). The strictest test is for central inertias; if those are OK then shifting them away always results in a good inertia.
+
+12/3/2011
+- Added missing rowSum() methods to Matrix, Mat and SymMat; fixed a bug in SymMat's existing sum() method for complex matrices.
+- Added missing elementwiseMultiply()/Divide()/Assign() ops
+
+6/24/2012 Made UserFunction a local class of TextDataEventReporter since it isn't used anywhere else.
diff --git a/Doxyfile.in b/Doxyfile.in
new file mode 100644
index 0000000..d8b994a
--- /dev/null
+++ b/Doxyfile.in
@@ -0,0 +1,2317 @@
+# Doxyfile 1.8.5
+
+# This file describes the settings to be used by the documentation system
+# doxygen (www.doxygen.org) for a project.
+#
+# All text after a double hash (##) is considered a comment and is placed in
+# front of the TAG it is preceding.
+#
+# All text after a single hash (#) is considered a comment and will be ignored.
+# The format is:
+# TAG = value [value, ...]
+# For lists, items can also be appended using:
+# TAG += value [value, ...]
+# Values that contain spaces should be placed between quotes (\" \").
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+
+# This tag specifies the encoding used for all characters in the config file
+# that follow. The default is UTF-8 which is also the encoding used for all text
+# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv
+# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv
+# for the list of possible encodings.
+# The default value is: UTF-8.
+
+DOXYFILE_ENCODING = UTF-8
+
+# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by
+# double-quotes, unless you are using Doxywizard) that should identify the
+# project for which the documentation is generated. This name is used in the
+# title of most generated pages and in a few other places.
+# The default value is: My Project.
+
+PROJECT_NAME = @PROJECT_NAME@
+
+# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
+# could be handy for archiving the generated documentation or if some version
+# control system is used.
+
+PROJECT_NUMBER = "3.4"
+
+# Using the PROJECT_BRIEF tag one can provide an optional one line description
+# for a project that appears at the top of each page and should give viewer a
+# quick idea about the purpose of the project. Keep the description short.
+
+PROJECT_BRIEF =
+
+# With the PROJECT_LOGO tag one can specify an logo or icon that is included in
+# the documentation. The maximum height of the logo should not exceed 55 pixels
+# and the maximum width should not exceed 200 pixels. Doxygen will copy the logo
+# to the output directory.
+
+PROJECT_LOGO =
+
+# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
+# into which the generated documentation will be written. If a relative path is
+# entered, it will be relative to the location where doxygen was started. If
+# left blank the current directory will be used.
+
+OUTPUT_DIRECTORY =
+
+# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create 4096 sub-
+# directories (in 2 levels) under the output directory of each output format and
+# will distribute the generated files over these directories. Enabling this
+# option can be useful when feeding doxygen a huge amount of source files, where
+# putting all generated files in the same directory would otherwise causes
+# performance problems for the file system.
+# The default value is: NO.
+
+CREATE_SUBDIRS = NO
+
+# The OUTPUT_LANGUAGE tag is used to specify the language in which all
+# documentation generated by doxygen is written. Doxygen will use this
+# information to generate all constant output in the proper language.
+# Possible values are: Afrikaans, Arabic, Brazilian, Catalan, Chinese, Chinese-
+# Traditional, Croatian, Czech, Danish, Dutch, English, Esperanto, Farsi,
+# Finnish, French, German, Greek, Hungarian, Italian, Japanese, Japanese-en,
+# Korean, Korean-en, Latvian, Norwegian, Macedonian, Persian, Polish,
+# Portuguese, Romanian, Russian, Serbian, Slovak, Slovene, Spanish, Swedish,
+# Turkish, Ukrainian and Vietnamese.
+# The default value is: English.
+
+OUTPUT_LANGUAGE = English
+
+# If the BRIEF_MEMBER_DESC tag is set to YES doxygen will include brief member
+# descriptions after the members that are listed in the file and class
+# documentation (similar to Javadoc). Set to NO to disable this.
+# The default value is: YES.
+
+BRIEF_MEMBER_DESC = YES
+
+# If the REPEAT_BRIEF tag is set to YES doxygen will prepend the brief
+# description of a member or function before the detailed description
+#
+# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the
+# brief descriptions will be completely suppressed.
+# The default value is: YES.
+
+REPEAT_BRIEF = YES
+
+# This tag implements a quasi-intelligent brief description abbreviator that is
+# used to form the text in various listings. Each string in this list, if found
+# as the leading text of the brief description, will be stripped from the text
+# and the result, after processing the whole list, is used as the annotated
+# text. Otherwise, the brief description is used as-is. If left blank, the
+# following values are used ($name is automatically replaced with the name of
+# the entity):The $name class, The $name widget, The $name file, is, provides,
+# specifies, contains, represents, a, an and the.
+
+ABBREVIATE_BRIEF =
+
+# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then
+# doxygen will generate a detailed section even if there is only a brief
+# description.
+# The default value is: NO.
+
+ALWAYS_DETAILED_SEC = NO
+
+# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all
+# inherited members of a class in the documentation of that class as if those
+# members were ordinary class members. Constructors, destructors and assignment
+# operators of the base classes will not be shown.
+# The default value is: NO.
+
+INLINE_INHERITED_MEMB = NO
+
+# If the FULL_PATH_NAMES tag is set to YES doxygen will prepend the full path
+# before files name in the file list and in the header files. If set to NO the
+# shortest path that makes the file name unique will be used
+# The default value is: YES.
+
+FULL_PATH_NAMES = NO
+
+# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path.
+# Stripping is only done if one of the specified strings matches the left-hand
+# part of the path. The tag can be used to show relative paths in the file list.
+# If left blank the directory from which doxygen is run is used as the path to
+# strip.
+#
+# Note that you can specify absolute paths here, but also relative paths, which
+# will be relative from the directory where doxygen is started.
+# This tag requires that the tag FULL_PATH_NAMES is set to YES.
+
+STRIP_FROM_PATH =
+
+# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the
+# path mentioned in the documentation of a class, which tells the reader which
+# header file to include in order to use a class. If left blank only the name of
+# the header file containing the class definition is used. Otherwise one should
+# specify the list of include paths that are normally passed to the compiler
+# using the -I flag.
+
+STRIP_FROM_INC_PATH =
+
+# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but
+# less readable) file names. This can be useful is your file systems doesn't
+# support long names like on DOS, Mac, or CD-ROM.
+# The default value is: NO.
+
+SHORT_NAMES = NO
+
+# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the
+# first line (until the first dot) of a Javadoc-style comment as the brief
+# description. If set to NO, the Javadoc-style will behave just like regular Qt-
+# style comments (thus requiring an explicit @brief command for a brief
+# description.)
+# The default value is: NO.
+
+JAVADOC_AUTOBRIEF = YES
+
+# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first
+# line (until the first dot) of a Qt-style comment as the brief description. If
+# set to NO, the Qt-style will behave just like regular Qt-style comments (thus
+# requiring an explicit \brief command for a brief description.)
+# The default value is: NO.
+
+QT_AUTOBRIEF = NO
+
+# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a
+# multi-line C++ special comment block (i.e. a block of //! or /// comments) as
+# a brief description. This used to be the default behavior. The new default is
+# to treat a multi-line C++ comment block as a detailed description. Set this
+# tag to YES if you prefer the old behavior instead.
+#
+# Note that setting this tag to YES also means that rational rose comments are
+# not recognized any more.
+# The default value is: NO.
+
+MULTILINE_CPP_IS_BRIEF = NO
+
+# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the
+# documentation from any documented member that it re-implements.
+# The default value is: YES.
+
+INHERIT_DOCS = YES
+
+# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce a
+# new page for each member. If set to NO, the documentation of a member will be
+# part of the file/class/namespace that contains it.
+# The default value is: NO.
+
+SEPARATE_MEMBER_PAGES = NO
+
+# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen
+# uses this value to replace tabs by spaces in code fragments.
+# Minimum value: 1, maximum value: 16, default value: 4.
+
+TAB_SIZE = 4
+
+# This tag can be used to specify a number of aliases that act as commands in
+# the documentation. An alias has the form:
+# name=value
+# For example adding
+# "sideeffect=@par Side Effects:\n"
+# will allow you to put the command \sideeffect (or @sideeffect) in the
+# documentation, which will result in a user-defined paragraph with heading
+# "Side Effects:". You can put \n's in the value part of an alias to insert
+# newlines.
+
+ALIASES =
+
+# This tag can be used to specify a number of word-keyword mappings (TCL only).
+# A mapping has the form "name=value". For example adding "class=itcl::class"
+# will allow you to use the command class in the itcl::class meaning.
+
+TCL_SUBST =
+
+# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
+# only. Doxygen will then generate output that is more tailored for C. For
+# instance, some of the names that are used will be different. The list of all
+# members will be omitted, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_FOR_C = NO
+
+# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or
+# Python sources only. Doxygen will then generate output that is more tailored
+# for that language. For instance, namespaces will be presented as packages,
+# qualified scopes will look different, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_JAVA = NO
+
+# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
+# sources. Doxygen will then generate output that is tailored for Fortran.
+# The default value is: NO.
+
+OPTIMIZE_FOR_FORTRAN = NO
+
+# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL
+# sources. Doxygen will then generate output that is tailored for VHDL.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_VHDL = NO
+
+# Doxygen selects the parser to use depending on the extension of the files it
+# parses. With this tag you can assign which parser to use for a given
+# extension. Doxygen has a built-in mapping, but you can override or extend it
+# using this tag. The format is ext=language, where ext is a file extension, and
+# language is one of the parsers supported by doxygen: IDL, Java, Javascript,
+# C#, C, C++, D, PHP, Objective-C, Python, Fortran, VHDL. For instance to make
+# doxygen treat .inc files as Fortran files (default is PHP), and .f files as C
+# (default is Fortran), use: inc=Fortran f=C.
+#
+# Note For files without extension you can use no_extension as a placeholder.
+#
+# Note that for custom extensions you also need to set FILE_PATTERNS otherwise
+# the files are not read by doxygen.
+
+EXTENSION_MAPPING =
+
+# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments
+# according to the Markdown format, which allows for more readable
+# documentation. See http://daringfireball.net/projects/markdown/ for details.
+# The output of markdown processing is further processed by doxygen, so you can
+# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in
+# case of backward compatibilities issues.
+# The default value is: YES.
+
+MARKDOWN_SUPPORT = NO
+
+# When enabled doxygen tries to link words that correspond to documented
+# classes, or namespaces to their corresponding documentation. Such a link can
+# be prevented in individual cases by by putting a % sign in front of the word
+# or globally by setting AUTOLINK_SUPPORT to NO.
+# The default value is: YES.
+
+AUTOLINK_SUPPORT = YES
+
+# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
+# to include (a tag file for) the STL sources as input, then you should set this
+# tag to YES in order to let doxygen match functions declarations and
+# definitions whose arguments contain STL classes (e.g. func(std::string);
+# versus func(std::string) {}). This also make the inheritance and collaboration
+# diagrams that involve STL classes more complete and accurate.
+# The default value is: NO.
+
+BUILTIN_STL_SUPPORT = YES
+
+# If you use Microsoft's C++/CLI language, you should set this option to YES to
+# enable parsing support.
+# The default value is: NO.
+
+CPP_CLI_SUPPORT = NO
+
+# Set the SIP_SUPPORT tag to YES if your project consists of sip (see:
+# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen
+# will parse them like normal C++ but will assume all classes use public instead
+# of private inheritance when no explicit protection keyword is present.
+# The default value is: NO.
+
+SIP_SUPPORT = NO
+
+# For Microsoft's IDL there are propget and propput attributes to indicate
+# getter and setter methods for a property. Setting this option to YES will make
+# doxygen to replace the get and set methods by a property in the documentation.
+# This will only work if the methods are indeed getting or setting a simple
+# type. If this is not the case, or you want to show the methods anyway, you
+# should set this option to NO.
+# The default value is: YES.
+
+IDL_PROPERTY_SUPPORT = YES
+
+# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
+# tag is set to YES, then doxygen will reuse the documentation of the first
+# member in the group (if any) for the other members of the group. By default
+# all members of a group must be documented explicitly.
+# The default value is: NO.
+
+DISTRIBUTE_GROUP_DOC = YES
+
+# Set the SUBGROUPING tag to YES to allow class member groups of the same type
+# (for instance a group of public functions) to be put as a subgroup of that
+# type (e.g. under the Public Functions section). Set it to NO to prevent
+# subgrouping. Alternatively, this can be done per class using the
+# \nosubgrouping command.
+# The default value is: YES.
+
+SUBGROUPING = YES
+
+# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions
+# are shown inside the group in which they are included (e.g. using \ingroup)
+# instead of on a separate page (for HTML and Man pages) or section (for LaTeX
+# and RTF).
+#
+# Note that this feature does not work in combination with
+# SEPARATE_MEMBER_PAGES.
+# The default value is: NO.
+
+INLINE_GROUPED_CLASSES = NO
+
+# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions
+# with only public data fields or simple typedef fields will be shown inline in
+# the documentation of the scope in which they are defined (i.e. file,
+# namespace, or group documentation), provided this scope is documented. If set
+# to NO, structs, classes, and unions are shown on a separate page (for HTML and
+# Man pages) or section (for LaTeX and RTF).
+# The default value is: NO.
+
+INLINE_SIMPLE_STRUCTS = NO
+
+# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or
+# enum is documented as struct, union, or enum with the name of the typedef. So
+# typedef struct TypeS {} TypeT, will appear in the documentation as a struct
+# with name TypeT. When disabled the typedef will appear as a member of a file,
+# namespace, or class. And the struct will be named TypeS. This can typically be
+# useful for C code in case the coding convention dictates that all compound
+# types are typedef'ed and only the typedef is referenced, never the tag name.
+# The default value is: NO.
+
+TYPEDEF_HIDES_STRUCT = NO
+
+# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This
+# cache is used to resolve symbols given their name and scope. Since this can be
+# an expensive process and often the same symbol appears multiple times in the
+# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small
+# doxygen will become slower. If the cache is too large, memory is wasted. The
+# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range
+# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536
+# symbols. At the end of a run doxygen will report the cache usage and suggest
+# the optimal cache size from a speed point of view.
+# Minimum value: 0, maximum value: 9, default value: 0.
+
+LOOKUP_CACHE_SIZE = 0
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+
+# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in
+# documentation are documented, even if no documentation was available. Private
+# class members and static file members will be hidden unless the
+# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES.
+# Note: This will also disable the warnings about undocumented members that are
+# normally produced when WARNINGS is set to YES.
+# The default value is: NO.
+
+EXTRACT_ALL = YES
+
+# If the EXTRACT_PRIVATE tag is set to YES all private members of a class will
+# be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PRIVATE = NO
+
+# If the EXTRACT_PACKAGE tag is set to YES all members with package or internal
+# scope will be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PACKAGE = NO
+
+# If the EXTRACT_STATIC tag is set to YES all static members of a file will be
+# included in the documentation.
+# The default value is: NO.
+
+EXTRACT_STATIC = YES
+
+# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) defined
+# locally in source files will be included in the documentation. If set to NO
+# only classes defined in header files are included. Does not have any effect
+# for Java sources.
+# The default value is: YES.
+
+EXTRACT_LOCAL_CLASSES = NO
+
+# This flag is only useful for Objective-C code. When set to YES local methods,
+# which are defined in the implementation section but not in the interface are
+# included in the documentation. If set to NO only methods in the interface are
+# included.
+# The default value is: NO.
+
+EXTRACT_LOCAL_METHODS = NO
+
+# If this flag is set to YES, the members of anonymous namespaces will be
+# extracted and appear in the documentation as a namespace called
+# 'anonymous_namespace{file}', where file will be replaced with the base name of
+# the file that contains the anonymous namespace. By default anonymous namespace
+# are hidden.
+# The default value is: NO.
+
+EXTRACT_ANON_NSPACES = NO
+
+# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all
+# undocumented members inside documented classes or files. If set to NO these
+# members will be included in the various overviews, but no documentation
+# section is generated. This option has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_MEMBERS = NO
+
+# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all
+# undocumented classes that are normally visible in the class hierarchy. If set
+# to NO these classes will be included in the various overviews. This option has
+# no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_CLASSES = NO
+
+# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend
+# (class|struct|union) declarations. If set to NO these declarations will be
+# included in the documentation.
+# The default value is: NO.
+
+HIDE_FRIEND_COMPOUNDS = NO
+
+# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any
+# documentation blocks found inside the body of a function. If set to NO these
+# blocks will be appended to the function's detailed documentation block.
+# The default value is: NO.
+
+HIDE_IN_BODY_DOCS = YES
+
+# The INTERNAL_DOCS tag determines if documentation that is typed after a
+# \internal command is included. If the tag is set to NO then the documentation
+# will be excluded. Set it to YES to include the internal documentation.
+# The default value is: NO.
+
+INTERNAL_DOCS = NO
+
+# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file
+# names in lower-case letters. If set to YES upper-case letters are also
+# allowed. This is useful if you have classes or files whose names only differ
+# in case and if your file system supports case sensitive file names. Windows
+# and Mac users are advised to set this option to NO.
+# The default value is: system dependent.
+
+CASE_SENSE_NAMES = YES
+
+# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with
+# their full class and namespace scopes in the documentation. If set to YES the
+# scope will be hidden.
+# The default value is: NO.
+
+HIDE_SCOPE_NAMES = NO
+
+# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of
+# the files that are included by a file in the documentation of that file.
+# The default value is: YES.
+
+SHOW_INCLUDE_FILES = YES
+
+# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include
+# files with double quotes in the documentation rather than with sharp brackets.
+# The default value is: NO.
+
+FORCE_LOCAL_INCLUDES = NO
+
+# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the
+# documentation for inline members.
+# The default value is: YES.
+
+INLINE_INFO = YES
+
+# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the
+# (detailed) documentation of file and class members alphabetically by member
+# name. If set to NO the members will appear in declaration order.
+# The default value is: YES.
+
+SORT_MEMBER_DOCS = NO
+
+# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief
+# descriptions of file, namespace and class members alphabetically by member
+# name. If set to NO the members will appear in declaration order.
+# The default value is: NO.
+
+SORT_BRIEF_DOCS = NO
+
+# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the
+# (brief and detailed) documentation of class members so that constructors and
+# destructors are listed first. If set to NO the constructors will appear in the
+# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS.
+# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief
+# member documentation.
+# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting
+# detailed member documentation.
+# The default value is: NO.
+
+SORT_MEMBERS_CTORS_1ST = NO
+
+# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy
+# of group names into alphabetical order. If set to NO the group names will
+# appear in their defined order.
+# The default value is: NO.
+
+SORT_GROUP_NAMES = YES
+
+# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by
+# fully-qualified names, including namespaces. If set to NO, the class list will
+# be sorted only by class name, not including the namespace part.
+# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
+# Note: This option applies only to the class list, not to the alphabetical
+# list.
+# The default value is: NO.
+
+SORT_BY_SCOPE_NAME = YES
+
+# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper
+# type resolution of all parameters of a function it will reject a match between
+# the prototype and the implementation of a member function even if there is
+# only one candidate or it is obvious which candidate to choose by doing a
+# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still
+# accept a match between prototype and implementation in such cases.
+# The default value is: NO.
+
+STRICT_PROTO_MATCHING = NO
+
+# The GENERATE_TODOLIST tag can be used to enable ( YES) or disable ( NO) the
+# todo list. This list is created by putting \todo commands in the
+# documentation.
+# The default value is: YES.
+
+GENERATE_TODOLIST = NO
+
+# The GENERATE_TESTLIST tag can be used to enable ( YES) or disable ( NO) the
+# test list. This list is created by putting \test commands in the
+# documentation.
+# The default value is: YES.
+
+GENERATE_TESTLIST = YES
+
+# The GENERATE_BUGLIST tag can be used to enable ( YES) or disable ( NO) the bug
+# list. This list is created by putting \bug commands in the documentation.
+# The default value is: YES.
+
+GENERATE_BUGLIST = NO
+
+# The GENERATE_DEPRECATEDLIST tag can be used to enable ( YES) or disable ( NO)
+# the deprecated list. This list is created by putting \deprecated commands in
+# the documentation.
+# The default value is: YES.
+
+GENERATE_DEPRECATEDLIST= YES
+
+# The ENABLED_SECTIONS tag can be used to enable conditional documentation
+# sections, marked by \if <section_label> ... \endif and \cond <section_label>
+# ... \endcond blocks.
+
+ENABLED_SECTIONS =
+
+# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the
+# initial value of a variable or macro / define can have for it to appear in the
+# documentation. If the initializer consists of more lines than specified here
+# it will be hidden. Use a value of 0 to hide initializers completely. The
+# appearance of the value of individual variables and macros / defines can be
+# controlled using \showinitializer or \hideinitializer command in the
+# documentation regardless of this setting.
+# Minimum value: 0, maximum value: 10000, default value: 30.
+
+MAX_INITIALIZER_LINES = 30
+
+# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at
+# the bottom of the documentation of classes and structs. If set to YES the list
+# will mention the files that were used to generate the documentation.
+# The default value is: YES.
+
+SHOW_USED_FILES = YES
+
+# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
+# will remove the Files entry from the Quick Index and from the Folder Tree View
+# (if specified).
+# The default value is: YES.
+
+SHOW_FILES = YES
+
+# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces
+# page. This will remove the Namespaces entry from the Quick Index and from the
+# Folder Tree View (if specified).
+# The default value is: YES.
+
+SHOW_NAMESPACES = YES
+
+# The FILE_VERSION_FILTER tag can be used to specify a program or script that
+# doxygen should invoke to get the current version for each file (typically from
+# the version control system). Doxygen will invoke the program by executing (via
+# popen()) the command command input-file, where command is the value of the
+# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided
+# by doxygen. Whatever the program writes to standard output is used as the file
+# version. For an example see the documentation.
+
+FILE_VERSION_FILTER =
+
+# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
+# by doxygen. The layout file controls the global structure of the generated
+# output files in an output format independent way. To create the layout file
+# that represents doxygen's defaults, run doxygen with the -l option. You can
+# optionally specify a file name after the option, if omitted DoxygenLayout.xml
+# will be used as the name of the layout file.
+#
+# Note that if you run doxygen from a directory containing a file called
+# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE
+# tag is left empty.
+
+LAYOUT_FILE =
+
+# The CITE_BIB_FILES tag can be used to specify one or more bib files containing
+# the reference definitions. This must be a list of .bib files. The .bib
+# extension is automatically appended if omitted. This requires the bibtex tool
+# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info.
+# For LaTeX the style of the bibliography can be controlled using
+# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
+# search path. Do not use file names with spaces, bibtex cannot handle them. See
+# also \cite for info how to create references.
+
+CITE_BIB_FILES =
+
+#---------------------------------------------------------------------------
+# Configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+
+# The QUIET tag can be used to turn on/off the messages that are generated to
+# standard output by doxygen. If QUIET is set to YES this implies that the
+# messages are off.
+# The default value is: NO.
+
+QUIET = NO
+
+# The WARNINGS tag can be used to turn on/off the warning messages that are
+# generated to standard error ( stderr) by doxygen. If WARNINGS is set to YES
+# this implies that the warnings are on.
+#
+# Tip: Turn warnings on while writing the documentation.
+# The default value is: YES.
+
+WARNINGS = YES
+
+# If the WARN_IF_UNDOCUMENTED tag is set to YES, then doxygen will generate
+# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag
+# will automatically be disabled.
+# The default value is: YES.
+
+WARN_IF_UNDOCUMENTED = YES
+
+# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for
+# potential errors in the documentation, such as not documenting some parameters
+# in a documented function, or documenting parameters that don't exist or using
+# markup commands wrongly.
+# The default value is: YES.
+
+WARN_IF_DOC_ERROR = YES
+
+# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
+# are documented, but have no documentation for their parameters or return
+# value. If set to NO doxygen will only warn about wrong or incomplete parameter
+# documentation, but not about the absence of documentation.
+# The default value is: NO.
+
+WARN_NO_PARAMDOC = NO
+
+# The WARN_FORMAT tag determines the format of the warning messages that doxygen
+# can produce. The string should contain the $file, $line, and $text tags, which
+# will be replaced by the file and line number from which the warning originated
+# and the warning text. Optionally the format may contain $version, which will
+# be replaced by the version of the file (if it could be obtained via
+# FILE_VERSION_FILTER)
+# The default value is: $file:$line: $text.
+
+WARN_FORMAT = "$file:$line: $text"
+
+# The WARN_LOGFILE tag can be used to specify a file to which warning and error
+# messages should be written. If left blank the output is written to standard
+# error (stderr).
+
+WARN_LOGFILE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the input files
+#---------------------------------------------------------------------------
+
+# The INPUT tag is used to specify the files and/or directories that contain
+# documented source files. You may enter file names like myfile.cpp or
+# directories like /usr/src/myproject. Separate the files or directories with
+# spaces.
+# Note: If this tag is empty the current directory is searched.
+
+INPUT = "@PROJECT_SOURCE_DIR@"
+
+# This tag can be used to specify the character encoding of the source files
+# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
+# libiconv (or the iconv built into libc) for the transcoding. See the libiconv
+# documentation (see: http://www.gnu.org/software/libiconv) for the list of
+# possible encodings.
+# The default value is: UTF-8.
+
+INPUT_ENCODING = UTF-8
+
+# If the value of the INPUT tag contains directories, you can use the
+# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank the
+# following patterns are tested:*.c, *.cc, *.cxx, *.cpp, *.c++, *.java, *.ii,
+# *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, *.hh, *.hxx, *.hpp,
+# *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc, *.m, *.markdown,
+# *.md, *.mm, *.dox, *.py, *.f90, *.f, *.for, *.tcl, *.vhd, *.vhdl, *.ucf,
+# *.qsf, *.as and *.js.
+
+FILE_PATTERNS =
+
+# The RECURSIVE tag can be used to specify whether or not subdirectories should
+# be searched for input files as well.
+# The default value is: NO.
+
+RECURSIVE = YES
+
+# The EXCLUDE tag can be used to specify files and/or directories that should be
+# excluded from the INPUT source files. This way you can easily exclude a
+# subdirectory from a directory tree whose root is specified with the INPUT tag.
+#
+# Note that relative paths are relative to the directory from which doxygen is
+# run.
+
+EXCLUDE =
+
+# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
+# directories that are symbolic links (a Unix file system feature) are excluded
+# from the input.
+# The default value is: NO.
+
+EXCLUDE_SYMLINKS = NO
+
+# If the value of the INPUT tag contains directories, you can use the
+# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
+# certain files from those directories.
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories for example use the pattern */test/*
+
+EXCLUDE_PATTERNS = README.md \
+ */Platform/* \
+ */tests/* \
+ */examples/* \
+ */SimTKcommon/*src/* \
+ */SimTKmath/*src/* \
+ */Simbody/*src/* \
+ */SimTKlapack.h \
+ */internal/ResultType.h \
+ */simbody-visualizer/* \
+ */.svn/*
+
+# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
+# (namespaces, classes, functions, etc.) that should be excluded from the
+# output. The symbol name can be a fully qualified name, a word, or if the
+# wildcard * is used, a substring. Examples: ANamespace, AClass,
+# AClass::ANamespace, ANamespace::*Test
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories use the pattern */test/*
+
+EXCLUDE_SYMBOLS = std
+
+# The EXAMPLE_PATH tag can be used to specify one or more files or directories
+# that contain example code fragments that are included (see the \include
+# command).
+
+EXAMPLE_PATH =
+
+# If the value of the EXAMPLE_PATH tag contains directories, you can use the
+# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank all
+# files are included.
+
+EXAMPLE_PATTERNS =
+
+# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be
+# searched for input files to be used with the \include or \dontinclude commands
+# irrespective of the value of the RECURSIVE tag.
+# The default value is: NO.
+
+EXAMPLE_RECURSIVE = NO
+
+# The IMAGE_PATH tag can be used to specify one or more files or directories
+# that contain images that are to be included in the documentation (see the
+# \image command).
+
+IMAGE_PATH = "@PROJECT_SOURCE_DIR@/doc/images" \
+ "@PROJECT_SOURCE_DIR@/Simbody/doc/images" \
+ "@PROJECT_SOURCE_DIR@/SimTKmath/doc/images" \
+ "@PROJECT_SOURCE_DIR@/SimTKcommon/doc/images"
+
+# The INPUT_FILTER tag can be used to specify a program that doxygen should
+# invoke to filter for each input file. Doxygen will invoke the filter program
+# by executing (via popen()) the command:
+#
+# <filter> <input-file>
+#
+# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the
+# name of an input file. Doxygen will then use the output that the filter
+# program writes to standard output. If FILTER_PATTERNS is specified, this tag
+# will be ignored.
+#
+# Note that the filter must not add or remove lines; it is applied before the
+# code is scanned, but not when the output code is generated. If lines are added
+# or removed, the anchors will not be placed correctly.
+
+INPUT_FILTER =
+
+# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern
+# basis. Doxygen will compare the file name with each pattern and apply the
+# filter if there is a match. The filters are a list of the form: pattern=filter
+# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how
+# filters are used. If the FILTER_PATTERNS tag is empty or if none of the
+# patterns match the file name, INPUT_FILTER is applied.
+
+FILTER_PATTERNS =
+
+# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using
+# INPUT_FILTER ) will also be used to filter the input files that are used for
+# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES).
+# The default value is: NO.
+
+FILTER_SOURCE_FILES = NO
+
+# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file
+# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and
+# it is also possible to disable source filtering for a specific pattern using
+# *.ext= (so without naming a filter).
+# This tag requires that the tag FILTER_SOURCE_FILES is set to YES.
+
+FILTER_SOURCE_PATTERNS =
+
+# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that
+# is part of the input, its contents will be placed on the main page
+# (index.html). This can be useful if you have a project on for instance GitHub
+# and want to reuse the introduction page also for the doxygen output.
+
+USE_MDFILE_AS_MAINPAGE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to source browsing
+#---------------------------------------------------------------------------
+
+# If the SOURCE_BROWSER tag is set to YES then a list of source files will be
+# generated. Documented entities will be cross-referenced with these sources.
+#
+# Note: To get rid of all source code in the generated output, make sure that
+# also VERBATIM_HEADERS is set to NO.
+# The default value is: NO.
+
+SOURCE_BROWSER = NO
+
+# Setting the INLINE_SOURCES tag to YES will include the body of functions,
+# classes and enums directly into the documentation.
+# The default value is: NO.
+
+INLINE_SOURCES = NO
+
+# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any
+# special comment blocks from generated source code fragments. Normal C, C++ and
+# Fortran comments will always remain visible.
+# The default value is: YES.
+
+STRIP_CODE_COMMENTS = YES
+
+# If the REFERENCED_BY_RELATION tag is set to YES then for each documented
+# function all documented functions referencing it will be listed.
+# The default value is: NO.
+
+REFERENCED_BY_RELATION = NO
+
+# If the REFERENCES_RELATION tag is set to YES then for each documented function
+# all documented entities called/used by that function will be listed.
+# The default value is: NO.
+
+REFERENCES_RELATION = NO
+
+# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set
+# to YES, then the hyperlinks from functions in REFERENCES_RELATION and
+# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will
+# link to the documentation.
+# The default value is: YES.
+
+REFERENCES_LINK_SOURCE = YES
+
+# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the
+# source code will show a tooltip with additional information such as prototype,
+# brief description and links to the definition and documentation. Since this
+# will make the HTML file larger and loading of large files a bit slower, you
+# can opt to disable this feature.
+# The default value is: YES.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+SOURCE_TOOLTIPS = YES
+
+# If the USE_HTAGS tag is set to YES then the references to source code will
+# point to the HTML generated by the htags(1) tool instead of doxygen built-in
+# source browser. The htags tool is part of GNU's global source tagging system
+# (see http://www.gnu.org/software/global/global.html). You will need version
+# 4.8.6 or higher.
+#
+# To use it do the following:
+# - Install the latest version of global
+# - Enable SOURCE_BROWSER and USE_HTAGS in the config file
+# - Make sure the INPUT points to the root of the source tree
+# - Run doxygen as normal
+#
+# Doxygen will invoke htags (and that will in turn invoke gtags), so these
+# tools must be available from the command line (i.e. in the search path).
+#
+# The result: instead of the source browser generated by doxygen, the links to
+# source code will now point to the output of htags.
+# The default value is: NO.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+USE_HTAGS = NO
+
+# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a
+# verbatim copy of the header file for each class for which an include is
+# specified. Set to NO to disable this.
+# See also: Section \class.
+# The default value is: YES.
+
+VERBATIM_HEADERS = YES
+
+# If the CLANG_ASSISTED_PARSING tag is set to YES, then doxygen will use the
+# clang parser (see: http://clang.llvm.org/) for more acurate parsing at the
+# cost of reduced performance. This can be particularly helpful with template
+# rich C++ code for which doxygen's built-in parser lacks the necessary type
+# information.
+# Note: The availability of this option depends on whether or not doxygen was
+# compiled with the --with-libclang option.
+# The default value is: NO.
+
+CLANG_ASSISTED_PARSING = NO
+
+# If clang assisted parsing is enabled you can provide the compiler with command
+# line options that you would normally use when invoking the compiler. Note that
+# the include paths will already be set by doxygen for the files and directories
+# specified with INPUT and INCLUDE_PATH.
+# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES.
+
+CLANG_OPTIONS =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+
+# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all
+# compounds will be generated. Enable this if the project contains a lot of
+# classes, structs, unions or interfaces.
+# The default value is: YES.
+
+ALPHABETICAL_INDEX = YES
+
+# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in
+# which the alphabetical index list will be split.
+# Minimum value: 1, maximum value: 20, default value: 5.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+COLS_IN_ALPHA_INDEX = 5
+
+# In case all classes in a project start with a common prefix, all classes will
+# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
+# can be used to specify a prefix (or a list of prefixes) that should be ignored
+# while generating the index headers.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+IGNORE_PREFIX = SimTK
+
+#---------------------------------------------------------------------------
+# Configuration options related to the HTML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_HTML tag is set to YES doxygen will generate HTML output
+# The default value is: YES.
+
+GENERATE_HTML = YES
+
+# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_OUTPUT = html
+
+# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each
+# generated HTML page (for example: .htm, .php, .asp).
+# The default value is: .html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FILE_EXTENSION = .html
+
+# The HTML_HEADER tag can be used to specify a user-defined HTML header file for
+# each generated HTML page. If the tag is left blank doxygen will generate a
+# standard header.
+#
+# To get valid HTML the header file that includes any scripts and style sheets
+# that doxygen needs, which is dependent on the configuration options used (e.g.
+# the setting GENERATE_TREEVIEW). It is highly recommended to start with a
+# default header using
+# doxygen -w html new_header.html new_footer.html new_stylesheet.css
+# YourConfigFile
+# and then modify the file new_header.html. See also section "Doxygen usage"
+# for information on how to generate the default header that doxygen normally
+# uses.
+# Note: The header is subject to change so you typically have to regenerate the
+# default header when upgrading to a newer version of doxygen. For a description
+# of the possible markers and block names see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_HEADER =
+
+# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each
+# generated HTML page. If the tag is left blank doxygen will generate a standard
+# footer. See HTML_HEADER for more information on how to generate a default
+# footer and what special commands can be used inside the footer. See also
+# section "Doxygen usage" for information on how to generate the default footer
+# that doxygen normally uses.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FOOTER =
+
+# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style
+# sheet that is used by each HTML page. It can be used to fine-tune the look of
+# the HTML output. If left blank doxygen will generate a default style sheet.
+# See also section "Doxygen usage" for information on how to generate the style
+# sheet that doxygen normally uses.
+# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as
+# it is more robust and this tag (HTML_STYLESHEET) will in the future become
+# obsolete.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_STYLESHEET =
+
+# The HTML_EXTRA_STYLESHEET tag can be used to specify an additional user-
+# defined cascading style sheet that is included after the standard style sheets
+# created by doxygen. Using this option one can overrule certain style aspects.
+# This is preferred over using HTML_STYLESHEET since it does not replace the
+# standard style sheet and is therefor more robust against future updates.
+# Doxygen will copy the style sheet file to the output directory. For an example
+# see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_STYLESHEET =
+
+# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the HTML output directory. Note
+# that these files will be copied to the base HTML output directory. Use the
+# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these
+# files. In the HTML_STYLESHEET file, use the file name only. Also note that the
+# files will be copied as-is; there are no commands or markers available.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_FILES =
+
+# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
+# will adjust the colors in the stylesheet and background images according to
+# this color. Hue is specified as an angle on a colorwheel, see
+# http://en.wikipedia.org/wiki/Hue for more information. For instance the value
+# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300
+# purple, and 360 is red again.
+# Minimum value: 0, maximum value: 359, default value: 220.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_HUE = 220
+
+# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors
+# in the HTML output. For a value of 0 the output will use grayscales only. A
+# value of 255 will produce the most vivid colors.
+# Minimum value: 0, maximum value: 255, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_SAT = 100
+
+# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the
+# luminance component of the colors in the HTML output. Values below 100
+# gradually make the output lighter, whereas values above 100 make the output
+# darker. The value divided by 100 is the actual gamma applied, so 80 represents
+# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not
+# change the gamma.
+# Minimum value: 40, maximum value: 240, default value: 80.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_GAMMA = 80
+
+# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
+# page will contain the date and time when the page was generated. Setting this
+# to NO can help when comparing the output of multiple runs.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_TIMESTAMP = YES
+
+# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
+# documentation will contain sections that can be hidden and shown after the
+# page has loaded.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_DYNAMIC_SECTIONS = YES
+
+# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries
+# shown in the various tree structured indices initially; the user can expand
+# and collapse entries dynamically later on. Doxygen will expand the tree to
+# such a level that at most the specified number of entries are visible (unless
+# a fully collapsed tree already exceeds this amount). So setting the number of
+# entries 1 will produce a full collapsed tree by default. 0 is a special value
+# representing an infinite number of entries and will result in a full expanded
+# tree by default.
+# Minimum value: 0, maximum value: 9999, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_INDEX_NUM_ENTRIES = 100
+
+# If the GENERATE_DOCSET tag is set to YES, additional index files will be
+# generated that can be used as input for Apple's Xcode 3 integrated development
+# environment (see: http://developer.apple.com/tools/xcode/), introduced with
+# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a
+# Makefile in the HTML output directory. Running make will produce the docset in
+# that directory and running make install will install the docset in
+# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at
+# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html
+# for more information.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_DOCSET = NO
+
+# This tag determines the name of the docset feed. A documentation feed provides
+# an umbrella under which multiple documentation sets from a single provider
+# (such as a company or product suite) can be grouped.
+# The default value is: Doxygen generated docs.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_FEEDNAME = "Doxygen generated docs"
+
+# This tag specifies a string that should uniquely identify the documentation
+# set bundle. This should be a reverse domain-name style string, e.g.
+# com.mycompany.MyDocSet. Doxygen will append .docset to the name.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_BUNDLE_ID = org.doxygen.Project
+
+# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify
+# the documentation publisher. This should be a reverse domain-name style
+# string, e.g. com.mycompany.MyDocSet.documentation.
+# The default value is: org.doxygen.Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_ID = org.doxygen.Publisher
+
+# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher.
+# The default value is: Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_NAME = Publisher
+
+# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three
+# additional HTML index files: index.hhp, index.hhc, and index.hhk. The
+# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop
+# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on
+# Windows.
+#
+# The HTML Help Workshop contains a compiler that can convert all HTML output
+# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML
+# files are now used as the Windows 98 help format, and will replace the old
+# Windows help format (.hlp) on all Windows platforms in the future. Compressed
+# HTML files also contain an index, a table of contents, and you can search for
+# words in the documentation. The HTML workshop also contains a viewer for
+# compressed HTML files.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_HTMLHELP = NO
+
+# The CHM_FILE tag can be used to specify the file name of the resulting .chm
+# file. You can add a path in front of the file if the result should not be
+# written to the html output directory.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_FILE =
+
+# The HHC_LOCATION tag can be used to specify the location (absolute path
+# including file name) of the HTML help compiler ( hhc.exe). If non-empty
+# doxygen will try to run the HTML help compiler on the generated index.hhp.
+# The file has to be specified with full path.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+HHC_LOCATION =
+
+# The GENERATE_CHI flag controls if a separate .chi index file is generated (
+# YES) or that it should be included in the master .chm file ( NO).
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+GENERATE_CHI = NO
+
+# The CHM_INDEX_ENCODING is used to encode HtmlHelp index ( hhk), content ( hhc)
+# and project file content.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_INDEX_ENCODING =
+
+# The BINARY_TOC flag controls whether a binary table of contents is generated (
+# YES) or a normal table of contents ( NO) in the .chm file.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+BINARY_TOC = NO
+
+# The TOC_EXPAND flag can be set to YES to add extra items for group members to
+# the table of contents of the HTML help documentation and to the tree view.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+TOC_EXPAND = NO
+
+# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
+# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that
+# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help
+# (.qch) of the generated HTML documentation.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_QHP = NO
+
+# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify
+# the file name of the resulting .qch file. The path specified is relative to
+# the HTML output folder.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QCH_FILE =
+
+# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help
+# Project output. For more information please see Qt Help Project / Namespace
+# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace).
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_NAMESPACE = org.doxygen.Project
+
+# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt
+# Help Project output. For more information please see Qt Help Project / Virtual
+# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual-
+# folders).
+# The default value is: doc.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_VIRTUAL_FOLDER = doc
+
+# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom
+# filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_NAME =
+
+# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the
+# custom filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_ATTRS =
+
+# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
+# project's filter section matches. Qt Help Project / Filter Attributes (see:
+# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_SECT_FILTER_ATTRS =
+
+# The QHG_LOCATION tag can be used to specify the location of Qt's
+# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the
+# generated .qhp file.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHG_LOCATION =
+
+# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be
+# generated, together with the HTML files, they form an Eclipse help plugin. To
+# install this plugin and make it available under the help contents menu in
+# Eclipse, the contents of the directory containing the HTML and XML files needs
+# to be copied into the plugins directory of eclipse. The name of the directory
+# within the plugins directory should be the same as the ECLIPSE_DOC_ID value.
+# After copying Eclipse needs to be restarted before the help appears.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_ECLIPSEHELP = NO
+
+# A unique identifier for the Eclipse help plugin. When installing the plugin
+# the directory name containing the HTML and XML files should also have this
+# name. Each documentation set should have its own identifier.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES.
+
+ECLIPSE_DOC_ID = org.doxygen.Project
+
+# If you want full control over the layout of the generated HTML pages it might
+# be necessary to disable the index and replace it with your own. The
+# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top
+# of each HTML page. A value of NO enables the index and the value YES disables
+# it. Since the tabs in the index contain the same information as the navigation
+# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+DISABLE_INDEX = NO
+
+# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index
+# structure should be generated to display hierarchical information. If the tag
+# value is set to YES, a side panel will be generated containing a tree-like
+# index structure (just like the one that is generated for HTML Help). For this
+# to work a browser that supports JavaScript, DHTML, CSS and frames is required
+# (i.e. any modern browser). Windows users are probably better off using the
+# HTML help feature. Via custom stylesheets (see HTML_EXTRA_STYLESHEET) one can
+# further fine-tune the look of the index. As an example, the default style
+# sheet generated by doxygen has an example that shows how to put an image at
+# the root of the tree instead of the PROJECT_NAME. Since the tree basically has
+# the same information as the tab index, you could consider setting
+# DISABLE_INDEX to YES when enabling this option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_TREEVIEW = YES
+
+# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
+# doxygen will group on one line in the generated HTML documentation.
+#
+# Note that a value of 0 will completely suppress the enum values from appearing
+# in the overview section.
+# Minimum value: 0, maximum value: 20, default value: 4.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+ENUM_VALUES_PER_LINE = 1
+
+# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used
+# to set the initial width (in pixels) of the frame in which the tree is shown.
+# Minimum value: 0, maximum value: 1500, default value: 250.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+TREEVIEW_WIDTH = 250
+
+# When the EXT_LINKS_IN_WINDOW option is set to YES doxygen will open links to
+# external symbols imported via tag files in a separate window.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+EXT_LINKS_IN_WINDOW = NO
+
+# Use this tag to change the font size of LaTeX formulas included as images in
+# the HTML documentation. When you change the font size after a successful
+# doxygen run you need to manually remove any form_*.png images from the HTML
+# output directory to force them to be regenerated.
+# Minimum value: 8, maximum value: 50, default value: 10.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_FONTSIZE = 10
+
+# Use the FORMULA_TRANPARENT tag to determine whether or not the images
+# generated for formulas are transparent PNGs. Transparent PNGs are not
+# supported properly for IE 6.0, but are supported on all modern browsers.
+#
+# Note that when changing this option you need to delete any form_*.png files in
+# the HTML output directory before the changes have effect.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_TRANSPARENT = YES
+
+# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see
+# http://www.mathjax.org) which uses client side Javascript for the rendering
+# instead of using prerendered bitmaps. Use this if you do not have LaTeX
+# installed or if you want to formulas look prettier in the HTML output. When
+# enabled you may also need to install MathJax separately and configure the path
+# to it using the MATHJAX_RELPATH option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+USE_MATHJAX = NO
+
+# When MathJax is enabled you can set the default output format to be used for
+# the MathJax output. See the MathJax site (see:
+# http://docs.mathjax.org/en/latest/output.html) for more details.
+# Possible values are: HTML-CSS (which is slower, but has the best
+# compatibility), NativeMML (i.e. MathML) and SVG.
+# The default value is: HTML-CSS.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_FORMAT = HTML-CSS
+
+# When MathJax is enabled you need to specify the location relative to the HTML
+# output directory using the MATHJAX_RELPATH option. The destination directory
+# should contain the MathJax.js script. For instance, if the mathjax directory
+# is located at the same level as the HTML output directory, then
+# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax
+# Content Delivery Network so you can quickly see the result without installing
+# MathJax. However, it is strongly recommended to install a local copy of
+# MathJax from http://www.mathjax.org before deployment.
+# The default value is: http://cdn.mathjax.org/mathjax/latest.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_RELPATH = http://www.mathjax.org/mathjax
+
+# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax
+# extension names that should be enabled during MathJax rendering. For example
+# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_EXTENSIONS =
+
+# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
+# of code that will be used on startup of the MathJax code. See the MathJax site
+# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an
+# example see the documentation.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_CODEFILE =
+
+# When the SEARCHENGINE tag is enabled doxygen will generate a search box for
+# the HTML output. The underlying search engine uses javascript and DHTML and
+# should work on any modern browser. Note that when using HTML help
+# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET)
+# there is already a search function so this one should typically be disabled.
+# For large projects the javascript based search engine can be slow, then
+# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to
+# search using the keyboard; to jump to the search box use <access key> + S
+# (what the <access key> is depends on the OS and browser, but it is typically
+# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down
+# key> to jump into the search results window, the results can be navigated
+# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel
+# the search. The filter options can be selected when the cursor is inside the
+# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys>
+# to select a filter and <Enter> or <escape> to activate or cancel the filter
+# option.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+SEARCHENGINE = YES
+
+# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
+# implemented using a web server instead of a web client using Javascript. There
+# are two flavours of web server based searching depending on the
+# EXTERNAL_SEARCH setting. When disabled, doxygen will generate a PHP script for
+# searching and an index file used by the script. When EXTERNAL_SEARCH is
+# enabled the indexing and searching needs to be provided by external tools. See
+# the section "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SERVER_BASED_SEARCH = NO
+
+# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP
+# script for searching. Instead the search results are written to an XML file
+# which needs to be processed by an external indexer. Doxygen will invoke an
+# external search engine pointed to by the SEARCHENGINE_URL option to obtain the
+# search results.
+#
+# Doxygen ships with an example indexer ( doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/).
+#
+# See the section "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH = NO
+
+# The SEARCHENGINE_URL should point to a search engine hosted by a web server
+# which will return the search results when EXTERNAL_SEARCH is enabled.
+#
+# Doxygen ships with an example indexer ( doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/). See the section "External Indexing and
+# Searching" for details.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHENGINE_URL =
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed
+# search data is written to a file for indexing by an external tool. With the
+# SEARCHDATA_FILE tag the name of this file can be specified.
+# The default file is: searchdata.xml.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHDATA_FILE = searchdata.xml
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the
+# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is
+# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple
+# projects and redirect the results back to the right project.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH_ID =
+
+# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen
+# projects other than the one defined by this configuration file, but that are
+# all added to the same external search index. Each project needs to have a
+# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of
+# to a relative location where the documentation can be found. The format is:
+# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ...
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTRA_SEARCH_MAPPINGS =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_LATEX tag is set to YES doxygen will generate LaTeX output.
+# The default value is: YES.
+
+GENERATE_LATEX = NO
+
+# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_OUTPUT = latex
+
+# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be
+# invoked.
+#
+# Note that when enabling USE_PDFLATEX this option is only used for generating
+# bitmaps for formulas in the HTML output, but not in the Makefile that is
+# written to the output directory.
+# The default file is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_CMD_NAME = latex
+
+# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate
+# index for LaTeX.
+# The default file is: makeindex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+MAKEINDEX_CMD_NAME = makeindex
+
+# If the COMPACT_LATEX tag is set to YES doxygen generates more compact LaTeX
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+COMPACT_LATEX = NO
+
+# The PAPER_TYPE tag can be used to set the paper type that is used by the
+# printer.
+# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x
+# 14 inches) and executive (7.25 x 10.5 inches).
+# The default value is: a4.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PAPER_TYPE = letter
+
+# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names
+# that should be included in the LaTeX output. To get the times font for
+# instance you can specify
+# EXTRA_PACKAGES=times
+# If left blank no extra packages will be included.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+EXTRA_PACKAGES =
+
+# The LATEX_HEADER tag can be used to specify a personal LaTeX header for the
+# generated LaTeX document. The header should contain everything until the first
+# chapter. If it is left blank doxygen will generate a standard header. See
+# section "Doxygen usage" for information on how to let doxygen write the
+# default header to a separate file.
+#
+# Note: Only use a user-defined header if you know what you are doing! The
+# following commands have a special meaning inside the header: $title,
+# $datetime, $date, $doxygenversion, $projectname, $projectnumber. Doxygen will
+# replace them by respectively the title of the page, the current date and time,
+# only the current date, the version number of doxygen, the project name (see
+# PROJECT_NAME), or the project number (see PROJECT_NUMBER).
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HEADER =
+
+# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for the
+# generated LaTeX document. The footer should contain everything after the last
+# chapter. If it is left blank doxygen will generate a standard footer.
+#
+# Note: Only use a user-defined footer if you know what you are doing!
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_FOOTER =
+
+# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the LATEX_OUTPUT output
+# directory. Note that the files will be copied as-is; there are no commands or
+# markers available.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_FILES =
+
+# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is
+# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will
+# contain links (just like the HTML output) instead of page references. This
+# makes the output suitable for online browsing using a PDF viewer.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PDF_HYPERLINKS = YES
+
+# If the LATEX_PDFLATEX tag is set to YES, doxygen will use pdflatex to generate
+# the PDF file directly from the LaTeX files. Set this option to YES to get a
+# higher quality PDF documentation.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+USE_PDFLATEX = YES
+
+# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode
+# command to the generated LaTeX files. This will instruct LaTeX to keep running
+# if errors occur, instead of asking the user for help. This option is also used
+# when generating formulas in HTML.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BATCHMODE = NO
+
+# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the
+# index chapters (such as File Index, Compound Index, etc.) in the output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HIDE_INDICES = NO
+
+# If the LATEX_SOURCE_CODE tag is set to YES then doxygen will include source
+# code with syntax highlighting in the LaTeX output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_SOURCE_CODE = NO
+
+# The LATEX_BIB_STYLE tag can be used to specify the style to use for the
+# bibliography, e.g. plainnat, or ieeetr. See
+# http://en.wikipedia.org/wiki/BibTeX and \cite for more info.
+# The default value is: plain.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BIB_STYLE = plain
+
+#---------------------------------------------------------------------------
+# Configuration options related to the RTF output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_RTF tag is set to YES doxygen will generate RTF output. The
+# RTF output is optimized for Word 97 and may not look too pretty with other RTF
+# readers/editors.
+# The default value is: NO.
+
+GENERATE_RTF = NO
+
+# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: rtf.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_OUTPUT = rtf
+
+# If the COMPACT_RTF tag is set to YES doxygen generates more compact RTF
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+COMPACT_RTF = NO
+
+# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will
+# contain hyperlink fields. The RTF file will contain links (just like the HTML
+# output) instead of page references. This makes the output suitable for online
+# browsing using Word or some other Word compatible readers that support those
+# fields.
+#
+# Note: WordPad (write) and others do not support links.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_HYPERLINKS = NO
+
+# Load stylesheet definitions from file. Syntax is similar to doxygen's config
+# file, i.e. a series of assignments. You only have to provide replacements,
+# missing definitions are set to their default value.
+#
+# See also section "Doxygen usage" for information on how to generate the
+# default style sheet that doxygen normally uses.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_STYLESHEET_FILE =
+
+# Set optional variables used in the generation of an RTF document. Syntax is
+# similar to doxygen's config file. A template extensions file can be generated
+# using doxygen -e rtf extensionFile.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_EXTENSIONS_FILE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the man page output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_MAN tag is set to YES doxygen will generate man pages for
+# classes and files.
+# The default value is: NO.
+
+GENERATE_MAN = NO
+
+# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it. A directory man3 will be created inside the directory specified by
+# MAN_OUTPUT.
+# The default directory is: man.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_OUTPUT = man
+
+# The MAN_EXTENSION tag determines the extension that is added to the generated
+# man pages. In case the manual section does not start with a number, the number
+# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is
+# optional.
+# The default value is: .3.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_EXTENSION = .3
+
+# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it
+# will generate one additional man file for each entity documented in the real
+# man page(s). These additional files only source the real man page, but without
+# them the man command would be unable to find the correct page.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_LINKS = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the XML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_XML tag is set to YES doxygen will generate an XML file that
+# captures the structure of the code including all documentation.
+# The default value is: NO.
+
+GENERATE_XML = NO
+
+# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: xml.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_OUTPUT = xml
+
+# The XML_SCHEMA tag can be used to specify a XML schema, which can be used by a
+# validating XML parser to check the syntax of the XML files.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_SCHEMA =
+
+# The XML_DTD tag can be used to specify a XML DTD, which can be used by a
+# validating XML parser to check the syntax of the XML files.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_DTD =
+
+# If the XML_PROGRAMLISTING tag is set to YES doxygen will dump the program
+# listings (including syntax highlighting and cross-referencing information) to
+# the XML output. Note that enabling this will significantly increase the size
+# of the XML output.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_PROGRAMLISTING = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the DOCBOOK output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_DOCBOOK tag is set to YES doxygen will generate Docbook files
+# that can be used to generate PDF.
+# The default value is: NO.
+
+GENERATE_DOCBOOK = NO
+
+# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put.
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in
+# front of it.
+# The default directory is: docbook.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_OUTPUT = docbook
+
+#---------------------------------------------------------------------------
+# Configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_AUTOGEN_DEF tag is set to YES doxygen will generate an AutoGen
+# Definitions (see http://autogen.sf.net) file that captures the structure of
+# the code including all documentation. Note that this feature is still
+# experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_AUTOGEN_DEF = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_PERLMOD tag is set to YES doxygen will generate a Perl module
+# file that captures the structure of the code including all documentation.
+#
+# Note that this feature is still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_PERLMOD = NO
+
+# If the PERLMOD_LATEX tag is set to YES doxygen will generate the necessary
+# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI
+# output from the Perl module output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_LATEX = NO
+
+# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be nicely
+# formatted so it can be parsed by a human reader. This is useful if you want to
+# understand what is going on. On the other hand, if this tag is set to NO the
+# size of the Perl module output will be much smaller and Perl will parse it
+# just the same.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_PRETTY = YES
+
+# The names of the make variables in the generated doxyrules.make file are
+# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful
+# so different doxyrules.make files included by the same Makefile don't
+# overwrite each other's variables.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_MAKEVAR_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+
+# If the ENABLE_PREPROCESSING tag is set to YES doxygen will evaluate all
+# C-preprocessor directives found in the sources and include files.
+# The default value is: YES.
+
+ENABLE_PREPROCESSING = YES
+
+# If the MACRO_EXPANSION tag is set to YES doxygen will expand all macro names
+# in the source code. If set to NO only conditional compilation will be
+# performed. Macro expansion can be done in a controlled way by setting
+# EXPAND_ONLY_PREDEF to YES.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+MACRO_EXPANSION = YES
+
+# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then
+# the macro expansion is limited to the macros specified with the PREDEFINED and
+# EXPAND_AS_DEFINED tags.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_ONLY_PREDEF = YES
+
+# If the SEARCH_INCLUDES tag is set to YES the includes files in the
+# INCLUDE_PATH will be searched if a #include is found.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SEARCH_INCLUDES = YES
+
+# The INCLUDE_PATH tag can be used to specify one or more directories that
+# contain include files that are not input files but should be processed by the
+# preprocessor.
+# This tag requires that the tag SEARCH_INCLUDES is set to YES.
+
+INCLUDE_PATH =
+
+# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
+# patterns (like *.h and *.hpp) to filter out the header-files in the
+# directories. If left blank, the patterns specified with FILE_PATTERNS will be
+# used.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+INCLUDE_FILE_PATTERNS =
+
+# The PREDEFINED tag can be used to specify one or more macro names that are
+# defined before the preprocessor is started (similar to the -D option of e.g.
+# gcc). The argument of the tag is a list of macros of the form: name or
+# name=definition (no spaces). If the definition and the "=" are omitted, "=1"
+# is assumed. To prevent a macro definition from being undefined via #undef or
+# recursively expanded use the := operator instead of the = operator.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+PREDEFINED = SimTK_SIMBODY_EXPORT= \
+ SimTK_SimTKCOMMON_EXPORT= \
+ SimTK_SIMMATH_EXPORT= \
+ OVERRIDE_11=override \
+ FINAL_11=final \
+ __cplusplus
+
+# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
+# tag can be used to specify a list of macro names that should be expanded. The
+# macro definition that is found in the sources will be used. Use the PREDEFINED
+# tag if you want to use a different macro definition that overrules the
+# definition found in the source code.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_AS_DEFINED = SimTK_PIMPL_DOWNCAST
+
+# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will
+# remove all refrences to function-like macros that are alone on a line, have an
+# all uppercase name, and do not end with a semicolon. Such function macros are
+# typically used for boiler-plate code, and will confuse the parser if not
+# removed.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SKIP_FUNCTION_MACROS = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to external references
+#---------------------------------------------------------------------------
+
+# The TAGFILES tag can be used to specify one or more tag files. For each tag
+# file the location of the external documentation should be added. The format of
+# a tag file without this location is as follows:
+# TAGFILES = file1 file2 ...
+# Adding location for the tag files is done as follows:
+# TAGFILES = file1=loc1 "file2 = loc2" ...
+# where loc1 and loc2 can be relative or absolute paths or URLs. See the
+# section "Linking to external documentation" for more information about the use
+# of tag files.
+# Note: Each tag file must have an unique name (where the name does NOT include
+# the path). If a tag file is not located in the directory in which doxygen is
+# run, you must also specify the path to the tagfile here.
+
+TAGFILES =
+
+# When a file name is specified after GENERATE_TAGFILE, doxygen will create a
+# tag file that is based on the input files it reads. See section "Linking to
+# external documentation" for more information about the usage of tag files.
+
+GENERATE_TAGFILE = "html/@PROJECT_NAME at DoxygenTagfile"
+
+# If the ALLEXTERNALS tag is set to YES all external class will be listed in the
+# class index. If set to NO only the inherited external classes will be listed.
+# The default value is: NO.
+
+ALLEXTERNALS = YES
+
+# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed in
+# the modules index. If set to NO, only the current project's groups will be
+# listed.
+# The default value is: YES.
+
+EXTERNAL_GROUPS = YES
+
+# If the EXTERNAL_PAGES tag is set to YES all external pages will be listed in
+# the related pages index. If set to NO, only the current project's pages will
+# be listed.
+# The default value is: YES.
+
+EXTERNAL_PAGES = YES
+
+# The PERL_PATH should be the absolute path and name of the perl script
+# interpreter (i.e. the result of 'which perl').
+# The default file (with absolute path) is: /usr/bin/perl.
+
+PERL_PATH = /usr/bin/perl
+
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+
+# If the CLASS_DIAGRAMS tag is set to YES doxygen will generate a class diagram
+# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to
+# NO turns the diagrams off. Note that this option also works with HAVE_DOT
+# disabled, but it is recommended to install and use dot, since it yields more
+# powerful graphs.
+# The default value is: YES.
+
+CLASS_DIAGRAMS = YES
+
+# You can define message sequence charts within doxygen comments using the \msc
+# command. Doxygen will then run the mscgen tool (see:
+# http://www.mcternan.me.uk/mscgen/)) to produce the chart and insert it in the
+# documentation. The MSCGEN_PATH tag allows you to specify the directory where
+# the mscgen tool resides. If left empty the tool is assumed to be found in the
+# default search path.
+
+MSCGEN_PATH =
+
+# If set to YES, the inheritance and collaboration graphs will hide inheritance
+# and usage relations if the target is undocumented or is not a class.
+# The default value is: YES.
+
+HIDE_UNDOC_RELATIONS = YES
+
+# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
+# available from the path. This tool is part of Graphviz (see:
+# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
+# Bell Labs. The other options in this section have no effect if this option is
+# set to NO
+# The default value is: NO.
+
+HAVE_DOT = NO
+
+# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed
+# to run in parallel. When set to 0 doxygen will base this on the number of
+# processors available in the system. You can set it explicitly to a value
+# larger than 0 to get control over the balance between CPU load and processing
+# speed.
+# Minimum value: 0, maximum value: 32, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_NUM_THREADS = 0
+
+# When you want a differently looking font n the dot files that doxygen
+# generates you can specify the font name using DOT_FONTNAME. You need to make
+# sure dot is able to find the font, which can be done by putting it in a
+# standard location or by setting the DOTFONTPATH environment variable or by
+# setting DOT_FONTPATH to the directory containing the font.
+# The default value is: Helvetica.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTNAME =
+
+# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of
+# dot graphs.
+# Minimum value: 4, maximum value: 24, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTSIZE = 10
+
+# By default doxygen will tell dot to use the default font as specified with
+# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set
+# the path where dot can find it using this tag.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTPATH =
+
+# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for
+# each documented class showing the direct and indirect inheritance relations.
+# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CLASS_GRAPH = YES
+
+# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a
+# graph for each documented class showing the direct and indirect implementation
+# dependencies (inheritance, containment, and class references variables) of the
+# class with other documented classes.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+COLLABORATION_GRAPH = YES
+
+# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for
+# groups, showing the direct groups dependencies.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GROUP_GRAPHS = YES
+
+# If the UML_LOOK tag is set to YES doxygen will generate inheritance and
+# collaboration diagrams in a style similar to the OMG's Unified Modeling
+# Language.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LOOK = NO
+
+# If the UML_LOOK tag is enabled, the fields and methods are shown inside the
+# class node. If there are many fields or methods and many nodes the graph may
+# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the
+# number of items for each type to make the size more manageable. Set this to 0
+# for no limit. Note that the threshold may be exceeded by 50% before the limit
+# is enforced. So when you set the threshold to 10, up to 15 fields may appear,
+# but if the number exceeds 15, the total amount of fields shown is limited to
+# 10.
+# Minimum value: 0, maximum value: 100, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LIMIT_NUM_FIELDS = 10
+
+# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and
+# collaboration graphs will show the relations between templates and their
+# instances.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+TEMPLATE_RELATIONS = YES
+
+# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to
+# YES then doxygen will generate a graph for each documented file showing the
+# direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDE_GRAPH = YES
+
+# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
+# set to YES then doxygen will generate a graph for each documented file showing
+# the direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDED_BY_GRAPH = YES
+
+# If the CALL_GRAPH tag is set to YES then doxygen will generate a call
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable call graphs for selected
+# functions only using the \callgraph command.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALL_GRAPH = NO
+
+# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable caller graphs for selected
+# functions only using the \callergraph command.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALLER_GRAPH = NO
+
+# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical
+# hierarchy of all classes instead of a textual one.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GRAPHICAL_HIERARCHY = YES
+
+# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the
+# dependencies a directory has on other directories in a graphical way. The
+# dependency relations are determined by the #include relations between the
+# files in the directories.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DIRECTORY_GRAPH = YES
+
+# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
+# generated by dot.
+# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order
+# to make the SVG files visible in IE 9+ (other browsers do not have this
+# requirement).
+# Possible values are: png, jpg, gif and svg.
+# The default value is: png.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_IMAGE_FORMAT = png
+
+# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to
+# enable generation of interactive SVG images that allow zooming and panning.
+#
+# Note that this requires a modern browser other than Internet Explorer. Tested
+# and working are Firefox, Chrome, Safari, and Opera.
+# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make
+# the SVG files visible. Older versions of IE do not have SVG support.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INTERACTIVE_SVG = NO
+
+# The DOT_PATH tag can be used to specify the path where the dot tool can be
+# found. If left blank, it is assumed the dot tool can be found in the path.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_PATH =
+
+# The DOTFILE_DIRS tag can be used to specify one or more directories that
+# contain dot files that are included in the documentation (see the \dotfile
+# command).
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOTFILE_DIRS =
+
+# The MSCFILE_DIRS tag can be used to specify one or more directories that
+# contain msc files that are included in the documentation (see the \mscfile
+# command).
+
+MSCFILE_DIRS =
+
+# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes
+# that will be shown in the graph. If the number of nodes in a graph becomes
+# larger than this value, doxygen will truncate the graph, which is visualized
+# by representing a node as a red box. Note that doxygen if the number of direct
+# children of the root node in a graph is already larger than
+# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that
+# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH.
+# Minimum value: 0, maximum value: 10000, default value: 50.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_GRAPH_MAX_NODES = 50
+
+# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs
+# generated by dot. A depth value of 3 means that only nodes reachable from the
+# root by following a path via at most 3 edges will be shown. Nodes that lay
+# further from the root node will be omitted. Note that setting this option to 1
+# or 2 may greatly reduce the computation time needed for large code bases. Also
+# note that the size of a graph can be further restricted by
+# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction.
+# Minimum value: 0, maximum value: 1000, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+MAX_DOT_GRAPH_DEPTH = 0
+
+# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
+# background. This is disabled by default, because dot on Windows does not seem
+# to support this out of the box.
+#
+# Warning: Depending on the platform used, enabling this option may lead to
+# badly anti-aliased labels on the edges of a graph (i.e. they become hard to
+# read).
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_TRANSPARENT = NO
+
+# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output
+# files in one run (i.e. multiple -o and -T options on the command line). This
+# makes dot run faster, but since only newer versions of dot (>1.8.10) support
+# this, this feature is disabled by default.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_MULTI_TARGETS = NO
+
+# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page
+# explaining the meaning of the various boxes and arrows in the dot generated
+# graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GENERATE_LEGEND = YES
+
+# If the DOT_CLEANUP tag is set to YES doxygen will remove the intermediate dot
+# files that are used to generate the various graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_CLEANUP = YES
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000..37ec93a
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,191 @@
+Apache License
+Version 2.0, January 2004
+http://www.apache.org/licenses/
+
+TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+1. Definitions.
+
+"License" shall mean the terms and conditions for use, reproduction, and
+distribution as defined by Sections 1 through 9 of this document.
+
+"Licensor" shall mean the copyright owner or entity authorized by the copyright
+owner that is granting the License.
+
+"Legal Entity" shall mean the union of the acting entity and all other entities
+that control, are controlled by, or are under common control with that entity.
+For the purposes of this definition, "control" means (i) the power, direct or
+indirect, to cause the direction or management of such entity, whether by
+contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the
+outstanding shares, or (iii) beneficial ownership of such entity.
+
+"You" (or "Your") shall mean an individual or Legal Entity exercising
+permissions granted by this License.
+
+"Source" form shall mean the preferred form for making modifications, including
+but not limited to software source code, documentation source, and configuration
+files.
+
+"Object" form shall mean any form resulting from mechanical transformation or
+translation of a Source form, including but not limited to compiled object code,
+generated documentation, and conversions to other media types.
+
+"Work" shall mean the work of authorship, whether in Source or Object form, made
+available under the License, as indicated by a copyright notice that is included
+in or attached to the work (an example is provided in the Appendix below).
+
+"Derivative Works" shall mean any work, whether in Source or Object form, that
+is based on (or derived from) the Work and for which the editorial revisions,
+annotations, elaborations, or other modifications represent, as a whole, an
+original work of authorship. For the purposes of this License, Derivative Works
+shall not include works that remain separable from, or merely link (or bind by
+name) to the interfaces of, the Work and Derivative Works thereof.
+
+"Contribution" shall mean any work of authorship, including the original version
+of the Work and any modifications or additions to that Work or Derivative Works
+thereof, that is intentionally submitted to Licensor for inclusion in the Work
+by the copyright owner or by an individual or Legal Entity authorized to submit
+on behalf of the copyright owner. For the purposes of this definition,
+"submitted" means any form of electronic, verbal, or written communication sent
+to the Licensor or its representatives, including but not limited to
+communication on electronic mailing lists, source code control systems, and
+issue tracking systems that are managed by, or on behalf of, the Licensor for
+the purpose of discussing and improving the Work, but excluding communication
+that is conspicuously marked or otherwise designated in writing by the copyright
+owner as "Not a Contribution."
+
+"Contributor" shall mean Licensor and any individual or Legal Entity on behalf
+of whom a Contribution has been received by Licensor and subsequently
+incorporated within the Work.
+
+2. Grant of Copyright License.
+
+Subject to the terms and conditions of this License, each Contributor hereby
+grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free,
+irrevocable copyright license to reproduce, prepare Derivative Works of,
+publicly display, publicly perform, sublicense, and distribute the Work and such
+Derivative Works in Source or Object form.
+
+3. Grant of Patent License.
+
+Subject to the terms and conditions of this License, each Contributor hereby
+grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free,
+irrevocable (except as stated in this section) patent license to make, have
+made, use, offer to sell, sell, import, and otherwise transfer the Work, where
+such license applies only to those patent claims licensable by such Contributor
+that are necessarily infringed by their Contribution(s) alone or by combination
+of their Contribution(s) with the Work to which such Contribution(s) was
+submitted. If You institute patent litigation against any entity (including a
+cross-claim or counterclaim in a lawsuit) alleging that the Work or a
+Contribution incorporated within the Work constitutes direct or contributory
+patent infringement, then any patent licenses granted to You under this License
+for that Work shall terminate as of the date such litigation is filed.
+
+4. Redistribution.
+
+You may reproduce and distribute copies of the Work or Derivative Works thereof
+in any medium, with or without modifications, and in Source or Object form,
+provided that You meet the following conditions:
+
+You must give any other recipients of the Work or Derivative Works a copy of
+this License; and
+You must cause any modified files to carry prominent notices stating that You
+changed the files; and
+You must retain, in the Source form of any Derivative Works that You distribute,
+all copyright, patent, trademark, and attribution notices from the Source form
+of the Work, excluding those notices that do not pertain to any part of the
+Derivative Works; and
+If the Work includes a "NOTICE" text file as part of its distribution, then any
+Derivative Works that You distribute must include a readable copy of the
+attribution notices contained within such NOTICE file, excluding those notices
+that do not pertain to any part of the Derivative Works, in at least one of the
+following places: within a NOTICE text file distributed as part of the
+Derivative Works; within the Source form or documentation, if provided along
+with the Derivative Works; or, within a display generated by the Derivative
+Works, if and wherever such third-party notices normally appear. The contents of
+the NOTICE file are for informational purposes only and do not modify the
+License. You may add Your own attribution notices within Derivative Works that
+You distribute, alongside or as an addendum to the NOTICE text from the Work,
+provided that such additional attribution notices cannot be construed as
+modifying the License.
+You may add Your own copyright statement to Your modifications and may provide
+additional or different license terms and conditions for use, reproduction, or
+distribution of Your modifications, or for any such Derivative Works as a whole,
+provided Your use, reproduction, and distribution of the Work otherwise complies
+with the conditions stated in this License.
+
+5. Submission of Contributions.
+
+Unless You explicitly state otherwise, any Contribution intentionally submitted
+for inclusion in the Work by You to the Licensor shall be under the terms and
+conditions of this License, without any additional terms or conditions.
+Notwithstanding the above, nothing herein shall supersede or modify the terms of
+any separate license agreement you may have executed with Licensor regarding
+such Contributions.
+
+6. Trademarks.
+
+This License does not grant permission to use the trade names, trademarks,
+service marks, or product names of the Licensor, except as required for
+reasonable and customary use in describing the origin of the Work and
+reproducing the content of the NOTICE file.
+
+7. Disclaimer of Warranty.
+
+Unless required by applicable law or agreed to in writing, Licensor provides the
+Work (and each Contributor provides its Contributions) on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied,
+including, without limitation, any warranties or conditions of TITLE,
+NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are
+solely responsible for determining the appropriateness of using or
+redistributing the Work and assume any risks associated with Your exercise of
+permissions under this License.
+
+8. Limitation of Liability.
+
+In no event and under no legal theory, whether in tort (including negligence),
+contract, or otherwise, unless required by applicable law (such as deliberate
+and grossly negligent acts) or agreed to in writing, shall any Contributor be
+liable to You for damages, including any direct, indirect, special, incidental,
+or consequential damages of any character arising as a result of this License or
+out of the use or inability to use the Work (including but not limited to
+damages for loss of goodwill, work stoppage, computer failure or malfunction, or
+any and all other commercial damages or losses), even if such Contributor has
+been advised of the possibility of such damages.
+
+9. Accepting Warranty or Additional Liability.
+
+While redistributing the Work or Derivative Works thereof, You may choose to
+offer, and charge a fee for, acceptance of support, warranty, indemnity, or
+other liability obligations and/or rights consistent with this License. However,
+in accepting such obligations, You may act only on Your own behalf and on Your
+sole responsibility, not on behalf of any other Contributor, and only if You
+agree to indemnify, defend, and hold each Contributor harmless for any liability
+incurred by, or claims asserted against, such Contributor by reason of your
+accepting any such warranty or additional liability.
+
+END OF TERMS AND CONDITIONS
+
+APPENDIX: How to apply the Apache License to your work
+
+To apply the Apache License to your work, attach the following boilerplate
+notice, with the fields enclosed by brackets "[]" replaced with your own
+identifying information. (Don't include the brackets!) The text should be
+enclosed in the appropriate comment syntax for the file format. We also
+recommend that a file or class name and description of purpose be included on
+the same "printed page" as the copyright notice for easier identification within
+third-party archives.
+
+ Copyright [yyyy] [name of copyright owner]
+
+ 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.
diff --git a/Platform/CMakeLists.txt b/Platform/CMakeLists.txt
new file mode 100644
index 0000000..1cea738
--- /dev/null
+++ b/Platform/CMakeLists.txt
@@ -0,0 +1,98 @@
+#---------------------------------------------------
+# Simbody/Platform
+#
+# Simbody has dependencies on several low-level packages
+# for which the details differ on different platforms.
+# Dependencies are:
+# - pthreads
+# - Lapack
+# Pthreads is standard on Linux platforms, and close enough
+# on Macs. On Windows we have to supply our own.
+# We provide our own high-performance Lapack library
+# for Windows; Linux and Macs are expected to have one.
+#----------------------------------------------------
+
+SET(PLATFORM_ROOT ${CMAKE_SOURCE_DIR}/Platform/${PLATFORM_NAME})
+
+SET(LIB_ABI_DIR "${PLATFORM_ROOT}/lib_${PLATFORM_ABI}")
+FILE(GLOB LIB_FILES
+ "${LIB_ABI_DIR}/*.a"
+ "${LIB_ABI_DIR}/*.so"
+ "${LIB_ABI_DIR}/*.so.*"
+ "${LIB_ABI_DIR}/*.dylib"
+ "${LIB_ABI_DIR}/*.lib"
+ "${LIB_ABI_DIR}/*.dll")
+
+# This is where we're going to put these binaries.
+SET(COPIED_LIB_FILES)
+FOREACH(LIBF ${LIB_FILES})
+ GET_FILENAME_COMPONENT(LIBF_ROOT ${LIBF} NAME)
+ SET(COPIED_LIB_FILES ${COPIED_LIB_FILES}
+ "${BUILD_BINARY_DIR}/${CMAKE_CFG_INTDIR}/${LIBF_ROOT}")
+ENDFOREACH()
+
+
+ADD_CUSTOM_TARGET(PlatformFiles ALL DEPENDS ${COPIED_LIB_FILES})
+SET_TARGET_PROPERTIES(PlatformFiles
+ PROPERTIES PROJECT_LABEL "Code - Platform Files")
+
+#
+# During build, we want to copy all the platform binaries
+# into the same binary directory that we are using for all
+# the ones we build. That ensure that our build will always
+# use the current versions of these.
+#
+# At installation, we copy both includes and binaries into
+# the appropriate locations of the SimTK_INSTALL_DIR.
+#
+
+FOREACH(LIBF ${LIB_FILES})
+ GET_FILENAME_COMPONENT(LIBF_ROOT ${LIBF} NAME)
+ GET_FILENAME_COMPONENT(LIBF_SUFFIX ${LIBF} EXT)
+ SET(COPIED_LIBF "${BUILD_BINARY_DIR}/${CMAKE_CFG_INTDIR}/${LIBF_ROOT}")
+ FILE(TO_NATIVE_PATH "${LIBF}" LIBF_SRC)
+ FILE(TO_NATIVE_PATH "${COPIED_LIBF}" LIBF_DEST)
+
+ ADD_CUSTOM_COMMAND(OUTPUT "${COPIED_LIBF}"
+ COMMAND ${NATIVE_COPY_CMD} "${LIBF_SRC}" "${LIBF_DEST}"
+ DEPENDS "${LIBF}"
+ COMMENT "Copy ${LIBF_SRC} -> ${LIBF_DEST}"
+ VERBATIM)
+
+ IF (LIBF_SUFFIX STREQUAL ".dll")
+ INSTALL(FILES ${LIBF}
+ PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE
+ GROUP_READ GROUP_WRITE GROUP_EXECUTE
+ WORLD_READ WORLD_EXECUTE
+ DESTINATION ${CMAKE_INSTALL_BINDIR})
+ ELSE()
+ INSTALL(FILES ${LIBF}
+ PERMISSIONS OWNER_READ OWNER_WRITE
+ GROUP_READ GROUP_WRITE
+ WORLD_READ
+ DESTINATION ${CMAKE_INSTALL_LIBDIR})
+ ENDIF()
+ENDFOREACH()
+
+# These are directories that all SimTK libraries should link with
+# to find the appropriate Platform libraries they depend on.
+SET(PLATFORM_LINK_LIBRARIES "${PLATFORM_ROOT}/lib_${PLATFORM_ABI}"
+ PARENT_SCOPE)
+
+# There is just one item in PLATFORM_INCLUDE_DIRECTORIES
+SET(PLATFORM_INCLUDE_DIRECTORIES "${PLATFORM_ROOT}/include")
+# Copy to parent.
+SET(PLATFORM_INCLUDE_DIRECTORIES "${PLATFORM_INCLUDE_DIRECTORIES}"
+ PARENT_SCOPE)
+
+# This needs an outer loop if you add more include directories.
+FILE(GLOB INCL_FILES "${PLATFORM_INCLUDE_DIRECTORIES}/*.h")
+FOREACH(INCLF ${INCL_FILES})
+ GET_FILENAME_COMPONENT(INCLF_ROOT ${INCLF} NAME)
+ INSTALL(FILES ${INCLF}
+ PERMISSIONS OWNER_READ OWNER_WRITE
+ GROUP_READ GROUP_WRITE
+ WORLD_READ
+ DESTINATION ${CMAKE_INSTALL_PREFIX}/include )
+ENDFOREACH()
+
diff --git a/Platform/Windows/include/dirent.h b/Platform/Windows/include/dirent.h
new file mode 100644
index 0000000..9be1c6d
--- /dev/null
+++ b/Platform/Windows/include/dirent.h
@@ -0,0 +1,234 @@
+/*****************************************************************************
+ * dirent.h - dirent API for Microsoft Visual Studio
+ *
+ * Copyright (C) 2006 Toni Ronkko
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * ``Software''), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included
+ * in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED ``AS IS'', WITHOUT WARRANTY OF ANY KIND, EXPRESS
+ * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL TONI RONKKO BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Dec 15, 2009, John Cunningham
+ * Added rewinddir member function
+ *
+ * Jan 18, 2008, Toni Ronkko
+ * Using FindFirstFileA and WIN32_FIND_DATAA to avoid converting string
+ * between multi-byte and unicode representations. This makes the
+ * code simpler and also allows the code to be compiled under MingW. Thanks
+ * to Azriel Fasten for the suggestion.
+ *
+ * Mar 4, 2007, Toni Ronkko
+ * Bug fix: due to the strncpy_s() function this file only compiled in
+ * Visual Studio 2005. Using the new string functions only when the
+ * compiler version allows.
+ *
+ * Nov 2, 2006, Toni Ronkko
+ * Major update: removed support for Watcom C, MS-DOS and Turbo C to
+ * simplify the file, updated the code to compile cleanly on Visual
+ * Studio 2005 with both unicode and multi-byte character strings,
+ * removed rewinddir() as it had a bug.
+ *
+ * Aug 20, 2006, Toni Ronkko
+ * Removed all remarks about MSVC 1.0, which is antiqued now. Simplified
+ * comments by removing SGML tags.
+ *
+ * May 14 2002, Toni Ronkko
+ * Embedded the function definitions directly to the header so that no
+ * source modules need to be included in the Visual Studio project. Removed
+ * all the dependencies to other projects so that this very header can be
+ * used independently.
+ *
+ * May 28 1998, Toni Ronkko
+ * First version.
+ *****************************************************************************/
+#ifndef SimTK_DIRENT_H_
+#define SimTK_DIRENT_H_
+
+#define WIN32_LEAN_AND_MEAN
+#define NOMINMAX
+#include <windows.h>
+
+#include <cstring>
+#include <cassert>
+#include <cstdlib>
+
+
+typedef struct dirent
+{
+ char d_name[MAX_PATH + 1]; /* current dir entry (multi-byte char string) */
+ WIN32_FIND_DATAA data; /* file attributes */
+} dirent;
+
+
+typedef struct DIR
+{
+ dirent current; /* Current directory entry */
+ int cached; /* Indicates un-processed entry in memory */
+ HANDLE search_handle; /* File search handle */
+ char patt[MAX_PATH + 3]; /* search pattern (3 = pattern + "\\*\0") */
+} DIR;
+
+
+/* Forward declarations */
+static DIR *opendir (const char *dirname);
+static struct dirent *readdir (DIR *dirp);
+static int closedir (DIR *dirp);
+static void rewinddir(DIR* dirp);
+
+
+/* Use the new safe string functions introduced in Visual Studio 2005 */
+#if defined(_MSC_VER) && _MSC_VER >= 1400
+# define STRNCPY(dest,src,size) strncpy_s((dest),(size),(src),_TRUNCATE)
+#else
+# define STRNCPY(dest,src,size) strncpy((dest),(src),(size))
+#endif
+
+
+/*****************************************************************************
+ * Open directory stream DIRNAME for read and return a pointer to the
+ * internal working area that is used to retrieve individual directory
+ * entries.
+ */
+static DIR *opendir(const char *dirname)
+{
+ DIR *dirp;
+ assert (dirname != NULL);
+ assert (strlen (dirname) < MAX_PATH);
+
+ /* construct new DIR structure */
+ dirp = (DIR*) malloc (sizeof (struct DIR));
+ if (dirp != NULL) {
+ char *p;
+
+ /* take directory name... */
+ STRNCPY (dirp->patt, dirname, sizeof(dirp->patt));
+ dirp->patt[MAX_PATH] = '\0';
+
+ /* ... and append search pattern to it */
+ p = strchr (dirp->patt, '\0');
+ if (dirp->patt < p && *(p-1) != '\\' && *(p-1) != ':') {
+ *p++ = '\\';
+ }
+ *p++ = '*';
+ *p = '\0';
+
+ /* open stream and retrieve first file */
+ dirp->search_handle = FindFirstFileA (dirp->patt, &dirp->current.data);
+ if (dirp->search_handle == INVALID_HANDLE_VALUE) {
+ /* invalid search pattern? */
+ free (dirp);
+ return NULL;
+ }
+
+ /* there is an un-processed directory entry in memory now */
+ dirp->cached = 1;
+ }
+
+ return dirp;
+}
+
+
+/*****************************************************************************
+ * Read a directory entry, and return a pointer to a dirent structure
+ * containing the name of the entry in d_name field. Individual directory
+ * entries returned by this very function include regular files,
+ * sub-directories, pseudo-directories "." and "..", but also volume labels,
+ * hidden files and system files may be returned.
+ */
+static struct dirent *readdir(DIR *dirp)
+{
+ assert (dirp != NULL);
+
+ if (dirp->search_handle == INVALID_HANDLE_VALUE) {
+ /* directory stream was opened/rewound incorrectly or ended normally */
+ return NULL;
+ }
+
+ /* get next directory entry */
+ if (dirp->cached != 0) {
+ /* a valid directory entry already in memory */
+ dirp->cached = 0;
+ } else {
+ /* read next directory entry from disk */
+ if (FindNextFileA (dirp->search_handle, &dirp->current.data) == FALSE) {
+ /* the very last file has been processed or an error occured */
+ FindClose (dirp->search_handle);
+ dirp->search_handle = INVALID_HANDLE_VALUE;
+ return NULL;
+ }
+ }
+
+ /* copy as a multibyte character string */
+ STRNCPY ( dirp->current.d_name,
+ dirp->current.data.cFileName,
+ sizeof(dirp->current.d_name) );
+ dirp->current.d_name[MAX_PATH] = '\0';
+
+ return &dirp->current;
+}
+
+
+/*****************************************************************************
+ * Close directory stream opened by opendir() function. Close of the
+ * directory stream invalidates the DIR structure as well as any previously
+ * read directory entry.
+ */
+static int closedir(DIR *dirp)
+{
+ assert (dirp != NULL);
+
+ /* release search handle */
+ if (dirp->search_handle != INVALID_HANDLE_VALUE) {
+ FindClose (dirp->search_handle);
+ dirp->search_handle = INVALID_HANDLE_VALUE;
+ }
+
+ /* release directory handle */
+ free (dirp);
+ return 0;
+}
+
+
+/*****************************************************************************
+ * Resets the position of the directory stream to which dirp refers to the
+ * beginning of the directory. It also causes the directory stream to refer
+ * to the current state of the corresponding directory, as a call to opendir()
+ * would have done. If dirp does not refer to a directory stream, the effect
+ * is undefined.
+ */
+static void rewinddir(DIR* dirp)
+{
+ /* release search handle */
+ if (dirp->search_handle != INVALID_HANDLE_VALUE) {
+ FindClose (dirp->search_handle);
+ dirp->search_handle = INVALID_HANDLE_VALUE;
+ }
+
+ /* open new search handle and retrieve first file */
+ dirp->search_handle = FindFirstFileA (dirp->patt, &dirp->current.data);
+ if (dirp->search_handle == INVALID_HANDLE_VALUE) {
+ /* invalid search pattern? */
+ free (dirp);
+ return;
+ }
+
+ /* there is an un-processed directory entry in memory now */
+ dirp->cached = 1;
+}
+
+
+#endif /*SimTK_DIRENT_H_*/
diff --git a/Platform/Windows/include/pthread.h b/Platform/Windows/include/pthread.h
new file mode 100644
index 0000000..f3d2dac
--- /dev/null
+++ b/Platform/Windows/include/pthread.h
@@ -0,0 +1,1368 @@
+/* This is an implementation of the threads API of POSIX 1003.1-2001.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#if !defined( PTHREAD_H )
+#define PTHREAD_H
+
+/*
+ * See the README file for an explanation of the pthreads-win32 version
+ * numbering scheme and how the DLL is named etc.
+ */
+#define PTW32_VERSION 2,8,0,0
+#define PTW32_VERSION_STRING "2, 8, 0, 0\0"
+
+/* There are three implementations of cancel cleanup.
+ * Note that pthread.h is included in both application
+ * compilation units and also internally for the library.
+ * The code here and within the library aims to work
+ * for all reasonable combinations of environments.
+ *
+ * The three implementations are:
+ *
+ * WIN32 SEH
+ * C
+ * C++
+ *
+ * Please note that exiting a push/pop block via
+ * "return", "exit", "break", or "continue" will
+ * lead to different behaviour amongst applications
+ * depending upon whether the library was built
+ * using SEH, C++, or C. For example, a library built
+ * with SEH will call the cleanup routine, while both
+ * C++ and C built versions will not.
+ */
+
+/*
+ * Define defaults for cleanup code.
+ * Note: Unless the build explicitly defines one of the following, then
+ * we default to standard C style cleanup. This style uses setjmp/longjmp
+ * in the cancelation and thread exit implementations and therefore won't
+ * do stack unwinding if linked to applications that have it (e.g.
+ * C++ apps). This is currently consistent with most/all commercial Unix
+ * POSIX threads implementations.
+ */
+#if !defined( __CLEANUP_SEH ) && !defined( __CLEANUP_CXX ) && !defined( __CLEANUP_C )
+# define __CLEANUP_C
+#endif
+
+#if defined( __CLEANUP_SEH ) && ( !defined( _MSC_VER ) && !defined(PTW32_RC_MSC))
+#error ERROR [__FILE__, line __LINE__]: SEH is not supported for this compiler.
+#endif
+
+/*
+ * Stop here if we are being included by the resource compiler.
+ */
+#ifndef RC_INVOKED
+
+#undef PTW32_LEVEL
+
+#if defined(_POSIX_SOURCE)
+#define PTW32_LEVEL 0
+/* Early POSIX */
+#endif
+
+#if defined(_POSIX_C_SOURCE) && _POSIX_C_SOURCE >= 199309
+#undef PTW32_LEVEL
+#define PTW32_LEVEL 1
+/* Include 1b, 1c and 1d */
+#endif
+
+#if defined(INCLUDE_NP)
+#undef PTW32_LEVEL
+#define PTW32_LEVEL 2
+/* Include Non-Portable extensions */
+#endif
+
+#define PTW32_LEVEL_MAX 3
+
+#if !defined(PTW32_LEVEL)
+#define PTW32_LEVEL PTW32_LEVEL_MAX
+/* Include everything */
+#endif
+
+#ifdef _UWIN
+# define HAVE_STRUCT_TIMESPEC 1
+# define HAVE_SIGNAL_H 1
+# undef HAVE_CONFIG_H
+# pragma comment(lib, "pthread")
+#endif
+
+/*
+ * -------------------------------------------------------------
+ *
+ *
+ * Module: pthread.h
+ *
+ * Purpose:
+ * Provides an implementation of PThreads based upon the
+ * standard:
+ *
+ * POSIX 1003.1-2001
+ * and
+ * The Single Unix Specification version 3
+ *
+ * (these two are equivalent)
+ *
+ * in order to enhance code portability between Windows,
+ * various commercial Unix implementations, and Linux.
+ *
+ * See the ANNOUNCE file for a full list of conforming
+ * routines and defined constants, and a list of missing
+ * routines and constants not defined in this implementation.
+ *
+ * Authors:
+ * There have been many contributors to this library.
+ * The initial implementation was contributed by
+ * John Bossom, and several others have provided major
+ * sections or revisions of parts of the implementation.
+ * Often significant effort has been contributed to
+ * find and fix important bugs and other problems to
+ * improve the reliability of the library, which sometimes
+ * is not reflected in the amount of code which changed as
+ * result.
+ * As much as possible, the contributors are acknowledged
+ * in the ChangeLog file in the source code distribution
+ * where their changes are noted in detail.
+ *
+ * Contributors are listed in the CONTRIBUTORS file.
+ *
+ * As usual, all bouquets go to the contributors, and all
+ * brickbats go to the project maintainer.
+ *
+ * Maintainer:
+ * The code base for this project is coordinated and
+ * eventually pre-tested, packaged, and made available by
+ *
+ * Ross Johnson <rpj at callisto.canberra.edu.au>
+ *
+ * QA Testers:
+ * Ultimately, the library is tested in the real world by
+ * a host of competent and demanding scientists and
+ * engineers who report bugs and/or provide solutions
+ * which are then fixed or incorporated into subsequent
+ * versions of the library. Each time a bug is fixed, a
+ * test case is written to prove the fix and ensure
+ * that later changes to the code don't reintroduce the
+ * same error. The number of test cases is slowly growing
+ * and therefore so is the code reliability.
+ *
+ * Compliance:
+ * See the file ANNOUNCE for the list of implemented
+ * and not-implemented routines and defined options.
+ * Of course, these are all defined is this file as well.
+ *
+ * Web site:
+ * The source code and other information about this library
+ * are available from
+ *
+ * http://sources.redhat.com/pthreads-win32/
+ *
+ * -------------------------------------------------------------
+ */
+
+/* Try to avoid including windows.h */
+#if defined(__MINGW32__) && defined(__cplusplus)
+#define PTW32_INCLUDE_WINDOWS_H
+#endif
+
+#ifdef PTW32_INCLUDE_WINDOWS_H
+#include <windows.h>
+#endif
+
+#if defined(_MSC_VER) && _MSC_VER < 1300 || defined(__DMC__)
+/*
+ * VC++6.0 or early compiler's header has no DWORD_PTR type.
+ */
+typedef unsigned long DWORD_PTR;
+#endif
+/*
+ * -----------------
+ * autoconf switches
+ * -----------------
+ */
+
+#if HAVE_CONFIG_H
+#include "config.h"
+#endif /* HAVE_CONFIG_H */
+
+#ifndef NEED_FTIME
+#include <time.h>
+#else /* NEED_FTIME */
+/* use native WIN32 time API */
+#endif /* NEED_FTIME */
+
+#if HAVE_SIGNAL_H
+#include <signal.h>
+#endif /* HAVE_SIGNAL_H */
+
+#include <setjmp.h>
+#include <limits.h>
+
+/*
+ * Boolean values to make us independent of system includes.
+ */
+enum {
+ PTW32_FALSE = 0,
+ PTW32_TRUE = (! PTW32_FALSE)
+};
+
+/*
+ * This is a duplicate of what is in the autoconf config.h,
+ * which is only used when building the pthread-win32 libraries.
+ */
+
+#ifndef PTW32_CONFIG_H
+# if defined(WINCE)
+# define NEED_ERRNO
+# define NEED_SEM
+# endif
+# if defined(_UWIN) || defined(__MINGW32__)
+# define HAVE_MODE_T
+# endif
+#endif
+
+/*
+ *
+ */
+
+#if PTW32_LEVEL >= PTW32_LEVEL_MAX
+#ifdef NEED_ERRNO
+#include "need_errno.h"
+#else
+#include <errno.h>
+#endif
+#endif /* PTW32_LEVEL >= PTW32_LEVEL_MAX */
+
+/*
+ * Several systems don't define some error numbers.
+ */
+#ifndef ENOTSUP
+# define ENOTSUP 48 /* This is the value in Solaris. */
+#endif
+
+#ifndef ETIMEDOUT
+# define ETIMEDOUT 10060 /* This is the value in winsock.h. */
+#endif
+
+#ifndef ENOSYS
+# define ENOSYS 140 /* Semi-arbitrary value */
+#endif
+
+#ifndef EDEADLK
+# ifdef EDEADLOCK
+# define EDEADLK EDEADLOCK
+# else
+# define EDEADLK 36 /* This is the value in MSVC. */
+# endif
+#endif
+
+#include <sched.h>
+
+/*
+ * To avoid including windows.h we define only those things that we
+ * actually need from it.
+ */
+#ifndef PTW32_INCLUDE_WINDOWS_H
+#ifndef HANDLE
+# define PTW32__HANDLE_DEF
+# define HANDLE void *
+#endif
+#ifndef DWORD
+# define PTW32__DWORD_DEF
+# define DWORD unsigned long
+#endif
+#endif
+
+#ifndef HAVE_STRUCT_TIMESPEC
+#define HAVE_STRUCT_TIMESPEC 1
+struct timespec {
+ long tv_sec;
+ long tv_nsec;
+};
+#endif /* HAVE_STRUCT_TIMESPEC */
+
+#ifndef SIG_BLOCK
+#define SIG_BLOCK 0
+#endif /* SIG_BLOCK */
+
+#ifndef SIG_UNBLOCK
+#define SIG_UNBLOCK 1
+#endif /* SIG_UNBLOCK */
+
+#ifndef SIG_SETMASK
+#define SIG_SETMASK 2
+#endif /* SIG_SETMASK */
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif /* __cplusplus */
+
+/*
+ * -------------------------------------------------------------
+ *
+ * POSIX 1003.1-2001 Options
+ * =========================
+ *
+ * Options are normally set in <unistd.h>, which is not provided
+ * with pthreads-win32.
+ *
+ * For conformance with the Single Unix Specification (version 3), all of the
+ * options below are defined, and have a value of either -1 (not supported)
+ * or 200112L (supported).
+ *
+ * These options can neither be left undefined nor have a value of 0, because
+ * either indicates that sysconf(), which is not implemented, may be used at
+ * runtime to check the status of the option.
+ *
+ * _POSIX_THREADS (== 200112L)
+ * If == 200112L, you can use threads
+ *
+ * _POSIX_THREAD_ATTR_STACKSIZE (== 200112L)
+ * If == 200112L, you can control the size of a thread's
+ * stack
+ * pthread_attr_getstacksize
+ * pthread_attr_setstacksize
+ *
+ * _POSIX_THREAD_ATTR_STACKADDR (== -1)
+ * If == 200112L, you can allocate and control a thread's
+ * stack. If not supported, the following functions
+ * will return ENOSYS, indicating they are not
+ * supported:
+ * pthread_attr_getstackaddr
+ * pthread_attr_setstackaddr
+ *
+ * _POSIX_THREAD_PRIORITY_SCHEDULING (== -1)
+ * If == 200112L, you can use realtime scheduling.
+ * This option indicates that the behaviour of some
+ * implemented functions conforms to the additional TPS
+ * requirements in the standard. E.g. rwlocks favour
+ * writers over readers when threads have equal priority.
+ *
+ * _POSIX_THREAD_PRIO_INHERIT (== -1)
+ * If == 200112L, you can create priority inheritance
+ * mutexes.
+ * pthread_mutexattr_getprotocol +
+ * pthread_mutexattr_setprotocol +
+ *
+ * _POSIX_THREAD_PRIO_PROTECT (== -1)
+ * If == 200112L, you can create priority ceiling mutexes
+ * Indicates the availability of:
+ * pthread_mutex_getprioceiling
+ * pthread_mutex_setprioceiling
+ * pthread_mutexattr_getprioceiling
+ * pthread_mutexattr_getprotocol +
+ * pthread_mutexattr_setprioceiling
+ * pthread_mutexattr_setprotocol +
+ *
+ * _POSIX_THREAD_PROCESS_SHARED (== -1)
+ * If set, you can create mutexes and condition
+ * variables that can be shared with another
+ * process.If set, indicates the availability
+ * of:
+ * pthread_mutexattr_getpshared
+ * pthread_mutexattr_setpshared
+ * pthread_condattr_getpshared
+ * pthread_condattr_setpshared
+ *
+ * _POSIX_THREAD_SAFE_FUNCTIONS (== 200112L)
+ * If == 200112L you can use the special *_r library
+ * functions that provide thread-safe behaviour
+ *
+ * _POSIX_READER_WRITER_LOCKS (== 200112L)
+ * If == 200112L, you can use read/write locks
+ *
+ * _POSIX_SPIN_LOCKS (== 200112L)
+ * If == 200112L, you can use spin locks
+ *
+ * _POSIX_BARRIERS (== 200112L)
+ * If == 200112L, you can use barriers
+ *
+ * + These functions provide both 'inherit' and/or
+ * 'protect' protocol, based upon these macro
+ * settings.
+ *
+ * -------------------------------------------------------------
+ */
+
+/*
+ * POSIX Options
+ */
+#undef _POSIX_THREADS
+#define _POSIX_THREADS 200112L
+
+#undef _POSIX_READER_WRITER_LOCKS
+#define _POSIX_READER_WRITER_LOCKS 200112L
+
+#undef _POSIX_SPIN_LOCKS
+#define _POSIX_SPIN_LOCKS 200112L
+
+#undef _POSIX_BARRIERS
+#define _POSIX_BARRIERS 200112L
+
+#undef _POSIX_THREAD_SAFE_FUNCTIONS
+#define _POSIX_THREAD_SAFE_FUNCTIONS 200112L
+
+#undef _POSIX_THREAD_ATTR_STACKSIZE
+#define _POSIX_THREAD_ATTR_STACKSIZE 200112L
+
+/*
+ * The following options are not supported
+ */
+#undef _POSIX_THREAD_ATTR_STACKADDR
+#define _POSIX_THREAD_ATTR_STACKADDR -1
+
+#undef _POSIX_THREAD_PRIO_INHERIT
+#define _POSIX_THREAD_PRIO_INHERIT -1
+
+#undef _POSIX_THREAD_PRIO_PROTECT
+#define _POSIX_THREAD_PRIO_PROTECT -1
+
+/* TPS is not fully supported. */
+#undef _POSIX_THREAD_PRIORITY_SCHEDULING
+#define _POSIX_THREAD_PRIORITY_SCHEDULING -1
+
+#undef _POSIX_THREAD_PROCESS_SHARED
+#define _POSIX_THREAD_PROCESS_SHARED -1
+
+
+/*
+ * POSIX 1003.1-2001 Limits
+ * ===========================
+ *
+ * These limits are normally set in <limits.h>, which is not provided with
+ * pthreads-win32.
+ *
+ * PTHREAD_DESTRUCTOR_ITERATIONS
+ * Maximum number of attempts to destroy
+ * a thread's thread-specific data on
+ * termination (must be at least 4)
+ *
+ * PTHREAD_KEYS_MAX
+ * Maximum number of thread-specific data keys
+ * available per process (must be at least 128)
+ *
+ * PTHREAD_STACK_MIN
+ * Minimum supported stack size for a thread
+ *
+ * PTHREAD_THREADS_MAX
+ * Maximum number of threads supported per
+ * process (must be at least 64).
+ *
+ * SEM_NSEMS_MAX
+ * The maximum number of semaphores a process can have.
+ * (must be at least 256)
+ *
+ * SEM_VALUE_MAX
+ * The maximum value a semaphore can have.
+ * (must be at least 32767)
+ *
+ */
+#undef _POSIX_THREAD_DESTRUCTOR_ITERATIONS
+#define _POSIX_THREAD_DESTRUCTOR_ITERATIONS 4
+
+#undef PTHREAD_DESTRUCTOR_ITERATIONS
+#define PTHREAD_DESTRUCTOR_ITERATIONS _POSIX_THREAD_DESTRUCTOR_ITERATIONS
+
+#undef _POSIX_THREAD_KEYS_MAX
+#define _POSIX_THREAD_KEYS_MAX 128
+
+#undef PTHREAD_KEYS_MAX
+#define PTHREAD_KEYS_MAX _POSIX_THREAD_KEYS_MAX
+
+#undef PTHREAD_STACK_MIN
+#define PTHREAD_STACK_MIN 0
+
+#undef _POSIX_THREAD_THREADS_MAX
+#define _POSIX_THREAD_THREADS_MAX 64
+
+ /* Arbitrary value */
+#undef PTHREAD_THREADS_MAX
+#define PTHREAD_THREADS_MAX 2019
+
+#undef _POSIX_SEM_NSEMS_MAX
+#define _POSIX_SEM_NSEMS_MAX 256
+
+ /* Arbitrary value */
+#undef SEM_NSEMS_MAX
+#define SEM_NSEMS_MAX 1024
+
+#undef _POSIX_SEM_VALUE_MAX
+#define _POSIX_SEM_VALUE_MAX 32767
+
+#undef SEM_VALUE_MAX
+#define SEM_VALUE_MAX INT_MAX
+
+
+#if __GNUC__ && ! defined (__declspec)
+# error Please upgrade your GNU compiler to one that supports __declspec.
+#endif
+
+/*
+ * When building the DLL code, you should define PTW32_BUILD so that
+ * the variables/functions are exported correctly. When using the DLL,
+ * do NOT define PTW32_BUILD, and then the variables/functions will
+ * be imported correctly.
+ */
+#ifndef PTW32_STATIC_LIB
+# ifdef PTW32_BUILD
+# define PTW32_DLLPORT __declspec (dllexport)
+# else
+# define PTW32_DLLPORT __declspec (dllimport)
+# endif
+#else
+# define PTW32_DLLPORT
+#endif
+
+/*
+ * The Open Watcom C/C++ compiler uses a non-standard calling convention
+ * that passes function args in registers unless __cdecl is explicitly specified
+ * in exposed function prototypes.
+ *
+ * We force all calls to cdecl even though this could slow Watcom code down
+ * slightly. If you know that the Watcom compiler will be used to build both
+ * the DLL and application, then you can probably define this as a null string.
+ * Remember that pthread.h (this file) is used for both the DLL and application builds.
+ */
+#define PTW32_CDECL __cdecl
+
+#if defined(_UWIN) && PTW32_LEVEL >= PTW32_LEVEL_MAX
+# include <sys/types.h>
+#else
+/*
+ * Generic handle type - intended to extend uniqueness beyond
+ * that available with a simple pointer. It should scale for either
+ * IA-32 or IA-64.
+ */
+typedef struct {
+ void * p; /* Pointer to actual object */
+ unsigned int x; /* Extra information - reuse count etc */
+} ptw32_handle_t;
+
+typedef ptw32_handle_t pthread_t;
+typedef struct pthread_attr_t_ * pthread_attr_t;
+typedef struct pthread_once_t_ pthread_once_t;
+typedef struct pthread_key_t_ * pthread_key_t;
+typedef struct pthread_mutex_t_ * pthread_mutex_t;
+typedef struct pthread_mutexattr_t_ * pthread_mutexattr_t;
+typedef struct pthread_cond_t_ * pthread_cond_t;
+typedef struct pthread_condattr_t_ * pthread_condattr_t;
+#endif
+typedef struct pthread_rwlock_t_ * pthread_rwlock_t;
+typedef struct pthread_rwlockattr_t_ * pthread_rwlockattr_t;
+typedef struct pthread_spinlock_t_ * pthread_spinlock_t;
+typedef struct pthread_barrier_t_ * pthread_barrier_t;
+typedef struct pthread_barrierattr_t_ * pthread_barrierattr_t;
+
+/*
+ * ====================
+ * ====================
+ * POSIX Threads
+ * ====================
+ * ====================
+ */
+
+enum {
+/*
+ * pthread_attr_{get,set}detachstate
+ */
+ PTHREAD_CREATE_JOINABLE = 0, /* Default */
+ PTHREAD_CREATE_DETACHED = 1,
+
+/*
+ * pthread_attr_{get,set}inheritsched
+ */
+ PTHREAD_INHERIT_SCHED = 0,
+ PTHREAD_EXPLICIT_SCHED = 1, /* Default */
+
+/*
+ * pthread_{get,set}scope
+ */
+ PTHREAD_SCOPE_PROCESS = 0,
+ PTHREAD_SCOPE_SYSTEM = 1, /* Default */
+
+/*
+ * pthread_setcancelstate paramters
+ */
+ PTHREAD_CANCEL_ENABLE = 0, /* Default */
+ PTHREAD_CANCEL_DISABLE = 1,
+
+/*
+ * pthread_setcanceltype parameters
+ */
+ PTHREAD_CANCEL_ASYNCHRONOUS = 0,
+ PTHREAD_CANCEL_DEFERRED = 1, /* Default */
+
+/*
+ * pthread_mutexattr_{get,set}pshared
+ * pthread_condattr_{get,set}pshared
+ */
+ PTHREAD_PROCESS_PRIVATE = 0,
+ PTHREAD_PROCESS_SHARED = 1,
+
+/*
+ * pthread_barrier_wait
+ */
+ PTHREAD_BARRIER_SERIAL_THREAD = -1
+};
+
+/*
+ * ====================
+ * ====================
+ * Cancelation
+ * ====================
+ * ====================
+ */
+#define PTHREAD_CANCELED ((void *) -1)
+
+
+/*
+ * ====================
+ * ====================
+ * Once Key
+ * ====================
+ * ====================
+ */
+#define PTHREAD_ONCE_INIT { PTW32_FALSE, 0, 0, 0}
+
+struct pthread_once_t_
+{
+ int done; /* indicates if user function has been executed */
+ void * lock;
+ int reserved1;
+ int reserved2;
+};
+
+
+/*
+ * ====================
+ * ====================
+ * Object initialisers
+ * ====================
+ * ====================
+ */
+#define PTHREAD_MUTEX_INITIALIZER ((pthread_mutex_t) -1)
+#define PTHREAD_RECURSIVE_MUTEX_INITIALIZER ((pthread_mutex_t) -2)
+#define PTHREAD_ERRORCHECK_MUTEX_INITIALIZER ((pthread_mutex_t) -3)
+
+/*
+ * Compatibility with LinuxThreads
+ */
+#define PTHREAD_RECURSIVE_MUTEX_INITIALIZER_NP PTHREAD_RECURSIVE_MUTEX_INITIALIZER
+#define PTHREAD_ERRORCHECK_MUTEX_INITIALIZER_NP PTHREAD_ERRORCHECK_MUTEX_INITIALIZER
+
+#define PTHREAD_COND_INITIALIZER ((pthread_cond_t) -1)
+
+#define PTHREAD_RWLOCK_INITIALIZER ((pthread_rwlock_t) -1)
+
+#define PTHREAD_SPINLOCK_INITIALIZER ((pthread_spinlock_t) -1)
+
+
+/*
+ * Mutex types.
+ */
+enum
+{
+ /* Compatibility with LinuxThreads */
+ PTHREAD_MUTEX_FAST_NP,
+ PTHREAD_MUTEX_RECURSIVE_NP,
+ PTHREAD_MUTEX_ERRORCHECK_NP,
+ PTHREAD_MUTEX_TIMED_NP = PTHREAD_MUTEX_FAST_NP,
+ PTHREAD_MUTEX_ADAPTIVE_NP = PTHREAD_MUTEX_FAST_NP,
+ /* For compatibility with POSIX */
+ PTHREAD_MUTEX_NORMAL = PTHREAD_MUTEX_FAST_NP,
+ PTHREAD_MUTEX_RECURSIVE = PTHREAD_MUTEX_RECURSIVE_NP,
+ PTHREAD_MUTEX_ERRORCHECK = PTHREAD_MUTEX_ERRORCHECK_NP,
+ PTHREAD_MUTEX_DEFAULT = PTHREAD_MUTEX_NORMAL
+};
+
+
+typedef struct ptw32_cleanup_t ptw32_cleanup_t;
+
+#if defined(_MSC_VER)
+/* Disable MSVC 'anachronism used' warning */
+#pragma warning( disable : 4229 )
+#endif
+
+typedef void (* PTW32_CDECL ptw32_cleanup_callback_t)(void *);
+
+#if defined(_MSC_VER)
+#pragma warning( default : 4229 )
+#endif
+
+struct ptw32_cleanup_t
+{
+ ptw32_cleanup_callback_t routine;
+ void *arg;
+ struct ptw32_cleanup_t *prev;
+};
+
+#ifdef __CLEANUP_SEH
+ /*
+ * WIN32 SEH version of cancel cleanup.
+ */
+
+#define pthread_cleanup_push( _rout, _arg ) \
+ { \
+ ptw32_cleanup_t _cleanup; \
+ \
+ _cleanup.routine = (ptw32_cleanup_callback_t)(_rout); \
+ _cleanup.arg = (_arg); \
+ __try \
+ { \
+
+#define pthread_cleanup_pop( _execute ) \
+ } \
+ __finally \
+ { \
+ if( _execute || AbnormalTermination()) \
+ { \
+ (*(_cleanup.routine))( _cleanup.arg ); \
+ } \
+ } \
+ }
+
+#else /* __CLEANUP_SEH */
+
+#ifdef __CLEANUP_C
+
+ /*
+ * C implementation of PThreads cancel cleanup
+ */
+
+#define pthread_cleanup_push( _rout, _arg ) \
+ { \
+ ptw32_cleanup_t _cleanup; \
+ \
+ ptw32_push_cleanup( &_cleanup, (ptw32_cleanup_callback_t) (_rout), (_arg) ); \
+
+#define pthread_cleanup_pop( _execute ) \
+ (void) ptw32_pop_cleanup( _execute ); \
+ }
+
+#else /* __CLEANUP_C */
+
+#ifdef __CLEANUP_CXX
+
+ /*
+ * C++ version of cancel cleanup.
+ * - John E. Bossom.
+ */
+
+ class PThreadCleanup {
+ /*
+ * PThreadCleanup
+ *
+ * Purpose
+ * This class is a C++ helper class that is
+ * used to implement pthread_cleanup_push/
+ * pthread_cleanup_pop.
+ * The destructor of this class automatically
+ * pops the pushed cleanup routine regardless
+ * of how the code exits the scope
+ * (i.e. such as by an exception)
+ */
+ ptw32_cleanup_callback_t cleanUpRout;
+ void * obj;
+ int executeIt;
+
+ public:
+ PThreadCleanup() :
+ cleanUpRout( 0 ),
+ obj( 0 ),
+ executeIt( 0 )
+ /*
+ * No cleanup performed
+ */
+ {
+ }
+
+ PThreadCleanup(
+ ptw32_cleanup_callback_t routine,
+ void * arg ) :
+ cleanUpRout( routine ),
+ obj( arg ),
+ executeIt( 1 )
+ /*
+ * Registers a cleanup routine for 'arg'
+ */
+ {
+ }
+
+ ~PThreadCleanup()
+ {
+ if ( executeIt && ((void *) cleanUpRout != (void *) 0) )
+ {
+ (void) (*cleanUpRout)( obj );
+ }
+ }
+
+ void execute( int exec )
+ {
+ executeIt = exec;
+ }
+ };
+
+ /*
+ * C++ implementation of PThreads cancel cleanup;
+ * This implementation takes advantage of a helper
+ * class who's destructor automatically calls the
+ * cleanup routine if we exit our scope weirdly
+ */
+#define pthread_cleanup_push( _rout, _arg ) \
+ { \
+ PThreadCleanup cleanup((ptw32_cleanup_callback_t)(_rout), \
+ (void *) (_arg) );
+
+#define pthread_cleanup_pop( _execute ) \
+ cleanup.execute( _execute ); \
+ }
+
+#else
+
+#error ERROR [__FILE__, line __LINE__]: Cleanup type undefined.
+
+#endif /* __CLEANUP_CXX */
+
+#endif /* __CLEANUP_C */
+
+#endif /* __CLEANUP_SEH */
+
+/*
+ * ===============
+ * ===============
+ * Methods
+ * ===============
+ * ===============
+ */
+
+/*
+ * PThread Attribute Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_init (pthread_attr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_destroy (pthread_attr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_getdetachstate (const pthread_attr_t * attr,
+ int *detachstate);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_getstackaddr (const pthread_attr_t * attr,
+ void **stackaddr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_getstacksize (const pthread_attr_t * attr,
+ size_t * stacksize);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_setdetachstate (pthread_attr_t * attr,
+ int detachstate);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_setstackaddr (pthread_attr_t * attr,
+ void *stackaddr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_setstacksize (pthread_attr_t * attr,
+ size_t stacksize);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_getschedparam (const pthread_attr_t *attr,
+ struct sched_param *param);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_setschedparam (pthread_attr_t *attr,
+ const struct sched_param *param);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_setschedpolicy (pthread_attr_t *,
+ int);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_getschedpolicy (pthread_attr_t *,
+ int *);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_setinheritsched(pthread_attr_t * attr,
+ int inheritsched);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_getinheritsched(pthread_attr_t * attr,
+ int * inheritsched);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_setscope (pthread_attr_t *,
+ int);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_getscope (const pthread_attr_t *,
+ int *);
+
+/*
+ * PThread Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_create (pthread_t * tid,
+ const pthread_attr_t * attr,
+ void *(*start) (void *),
+ void *arg);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_detach (pthread_t tid);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_equal (pthread_t t1,
+ pthread_t t2);
+
+PTW32_DLLPORT void PTW32_CDECL pthread_exit (void *value_ptr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_join (pthread_t thread,
+ void **value_ptr);
+
+PTW32_DLLPORT pthread_t PTW32_CDECL pthread_self (void);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_cancel (pthread_t thread);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_setcancelstate (int state,
+ int *oldstate);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_setcanceltype (int type,
+ int *oldtype);
+
+PTW32_DLLPORT void PTW32_CDECL pthread_testcancel (void);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_once (pthread_once_t * once_control,
+ void (*init_routine) (void));
+
+#if PTW32_LEVEL >= PTW32_LEVEL_MAX
+PTW32_DLLPORT ptw32_cleanup_t * PTW32_CDECL ptw32_pop_cleanup (int execute);
+
+PTW32_DLLPORT void PTW32_CDECL ptw32_push_cleanup (ptw32_cleanup_t * cleanup,
+ void (*routine) (void *),
+ void *arg);
+#endif /* PTW32_LEVEL >= PTW32_LEVEL_MAX */
+
+/*
+ * Thread Specific Data Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_key_create (pthread_key_t * key,
+ void (*destructor) (void *));
+
+PTW32_DLLPORT int PTW32_CDECL pthread_key_delete (pthread_key_t key);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_setspecific (pthread_key_t key,
+ const void *value);
+
+PTW32_DLLPORT void * PTW32_CDECL pthread_getspecific (pthread_key_t key);
+
+
+/*
+ * Mutex Attribute Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_init (pthread_mutexattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_destroy (pthread_mutexattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_getpshared (const pthread_mutexattr_t
+ * attr,
+ int *pshared);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_setpshared (pthread_mutexattr_t * attr,
+ int pshared);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_settype (pthread_mutexattr_t * attr, int kind);
+PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_gettype (pthread_mutexattr_t * attr, int *kind);
+
+/*
+ * Barrier Attribute Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_barrierattr_init (pthread_barrierattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_barrierattr_destroy (pthread_barrierattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_barrierattr_getpshared (const pthread_barrierattr_t
+ * attr,
+ int *pshared);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_barrierattr_setpshared (pthread_barrierattr_t * attr,
+ int pshared);
+
+/*
+ * Mutex Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_mutex_init (pthread_mutex_t * mutex,
+ const pthread_mutexattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutex_destroy (pthread_mutex_t * mutex);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutex_lock (pthread_mutex_t * mutex);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutex_timedlock(pthread_mutex_t *mutex,
+ const struct timespec *abstime);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutex_trylock (pthread_mutex_t * mutex);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutex_unlock (pthread_mutex_t * mutex);
+
+/*
+ * Spinlock Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_spin_init (pthread_spinlock_t * lock, int pshared);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_spin_destroy (pthread_spinlock_t * lock);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_spin_lock (pthread_spinlock_t * lock);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_spin_trylock (pthread_spinlock_t * lock);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_spin_unlock (pthread_spinlock_t * lock);
+
+/*
+ * Barrier Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_barrier_init (pthread_barrier_t * barrier,
+ const pthread_barrierattr_t * attr,
+ unsigned int count);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_barrier_destroy (pthread_barrier_t * barrier);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_barrier_wait (pthread_barrier_t * barrier);
+
+/*
+ * Condition Variable Attribute Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_condattr_init (pthread_condattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_condattr_destroy (pthread_condattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_condattr_getpshared (const pthread_condattr_t * attr,
+ int *pshared);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_condattr_setpshared (pthread_condattr_t * attr,
+ int pshared);
+
+/*
+ * Condition Variable Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_cond_init (pthread_cond_t * cond,
+ const pthread_condattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_cond_destroy (pthread_cond_t * cond);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_cond_wait (pthread_cond_t * cond,
+ pthread_mutex_t * mutex);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_cond_timedwait (pthread_cond_t * cond,
+ pthread_mutex_t * mutex,
+ const struct timespec *abstime);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_cond_signal (pthread_cond_t * cond);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_cond_broadcast (pthread_cond_t * cond);
+
+/*
+ * Scheduling
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_setschedparam (pthread_t thread,
+ int policy,
+ const struct sched_param *param);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_getschedparam (pthread_t thread,
+ int *policy,
+ struct sched_param *param);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_setconcurrency (int);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_getconcurrency (void);
+
+/*
+ * Read-Write Lock Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_init(pthread_rwlock_t *lock,
+ const pthread_rwlockattr_t *attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_destroy(pthread_rwlock_t *lock);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_tryrdlock(pthread_rwlock_t *);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_trywrlock(pthread_rwlock_t *);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_rdlock(pthread_rwlock_t *lock);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_timedrdlock(pthread_rwlock_t *lock,
+ const struct timespec *abstime);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_wrlock(pthread_rwlock_t *lock);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_timedwrlock(pthread_rwlock_t *lock,
+ const struct timespec *abstime);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_unlock(pthread_rwlock_t *lock);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlockattr_init (pthread_rwlockattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlockattr_destroy (pthread_rwlockattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlockattr_getpshared (const pthread_rwlockattr_t * attr,
+ int *pshared);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlockattr_setpshared (pthread_rwlockattr_t * attr,
+ int pshared);
+
+#if PTW32_LEVEL >= PTW32_LEVEL_MAX - 1
+
+/*
+ * Signal Functions. Should be defined in <signal.h> but MSVC and MinGW32
+ * already have signal.h that don't define these.
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_kill(pthread_t thread, int sig);
+
+/*
+ * Non-portable functions
+ */
+
+/*
+ * Compatibility with Linux.
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_setkind_np(pthread_mutexattr_t * attr,
+ int kind);
+PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_getkind_np(pthread_mutexattr_t * attr,
+ int *kind);
+
+/*
+ * Possibly supported by other POSIX threads implementations
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_delay_np (struct timespec * interval);
+PTW32_DLLPORT int PTW32_CDECL pthread_num_processors_np(void);
+
+/*
+ * Useful if an application wants to statically link
+ * the lib rather than load the DLL at run-time.
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_win32_process_attach_np(void);
+PTW32_DLLPORT int PTW32_CDECL pthread_win32_process_detach_np(void);
+PTW32_DLLPORT int PTW32_CDECL pthread_win32_thread_attach_np(void);
+PTW32_DLLPORT int PTW32_CDECL pthread_win32_thread_detach_np(void);
+
+/*
+ * Features that are auto-detected at load/run time.
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_win32_test_features_np(int);
+enum ptw32_features {
+ PTW32_SYSTEM_INTERLOCKED_COMPARE_EXCHANGE = 0x0001, /* System provides it. */
+ PTW32_ALERTABLE_ASYNC_CANCEL = 0x0002 /* Can cancel blocked threads. */
+};
+
+/*
+ * Register a system time change with the library.
+ * Causes the library to perform various functions
+ * in response to the change. Should be called whenever
+ * the application's top level window receives a
+ * WM_TIMECHANGE message. It can be passed directly to
+ * pthread_create() as a new thread if desired.
+ */
+PTW32_DLLPORT void * PTW32_CDECL pthread_timechange_handler_np(void *);
+
+#endif /*PTW32_LEVEL >= PTW32_LEVEL_MAX - 1 */
+
+#if PTW32_LEVEL >= PTW32_LEVEL_MAX
+
+/*
+ * Returns the Win32 HANDLE for the POSIX thread.
+ */
+PTW32_DLLPORT HANDLE PTW32_CDECL pthread_getw32threadhandle_np(pthread_t thread);
+
+
+/*
+ * Protected Methods
+ *
+ * This function blocks until the given WIN32 handle
+ * is signaled or pthread_cancel had been called.
+ * This function allows the caller to hook into the
+ * PThreads cancel mechanism. It is implemented using
+ *
+ * WaitForMultipleObjects
+ *
+ * on 'waitHandle' and a manually reset WIN32 Event
+ * used to implement pthread_cancel. The 'timeout'
+ * argument to TimedWait is simply passed to
+ * WaitForMultipleObjects.
+ */
+PTW32_DLLPORT int PTW32_CDECL pthreadCancelableWait (HANDLE waitHandle);
+PTW32_DLLPORT int PTW32_CDECL pthreadCancelableTimedWait (HANDLE waitHandle,
+ DWORD timeout);
+
+#endif /* PTW32_LEVEL >= PTW32_LEVEL_MAX */
+
+/*
+ * Thread-Safe C Runtime Library Mappings.
+ */
+#ifndef _UWIN
+# if defined(NEED_ERRNO)
+ PTW32_DLLPORT int * PTW32_CDECL _errno( void );
+# else
+# ifndef errno
+# if (defined(_MT) || defined(_DLL))
+ __declspec(dllimport) extern int * __cdecl _errno(void);
+# define errno (*_errno())
+# endif
+# endif
+# endif
+#endif
+
+/*
+ * WIN32 C runtime library had been made thread-safe
+ * without affecting the user interface. Provide
+ * mappings from the UNIX thread-safe versions to
+ * the standard C runtime library calls.
+ * Only provide function mappings for functions that
+ * actually exist on WIN32.
+ */
+
+#if !defined(__MINGW32__)
+#define strtok_r( _s, _sep, _lasts ) \
+ ( *(_lasts) = strtok( (_s), (_sep) ) )
+#endif /* !__MINGW32__ */
+
+#define asctime_r( _tm, _buf ) \
+ ( strcpy( (_buf), asctime( (_tm) ) ), \
+ (_buf) )
+
+#define ctime_r( _clock, _buf ) \
+ ( strcpy( (_buf), ctime( (_clock) ) ), \
+ (_buf) )
+
+#define gmtime_r( _clock, _result ) \
+ ( *(_result) = *gmtime( (_clock) ), \
+ (_result) )
+
+#define localtime_r( _clock, _result ) \
+ ( *(_result) = *localtime( (_clock) ), \
+ (_result) )
+
+#define rand_r( _seed ) \
+ ( _seed == _seed? rand() : rand() )
+
+
+/*
+ * Some compiler environments don't define some things.
+ */
+#if defined(__BORLANDC__)
+# define _ftime ftime
+# define _timeb timeb
+#endif
+
+#ifdef __cplusplus
+
+/*
+ * Internal exceptions
+ */
+class ptw32_exception {};
+class ptw32_exception_cancel : public ptw32_exception {};
+class ptw32_exception_exit : public ptw32_exception {};
+
+#endif
+
+#if PTW32_LEVEL >= PTW32_LEVEL_MAX
+
+/* FIXME: This is only required if the library was built using SEH */
+/*
+ * Get internal SEH tag
+ */
+PTW32_DLLPORT DWORD PTW32_CDECL ptw32_get_exception_services_code(void);
+
+#endif /* PTW32_LEVEL >= PTW32_LEVEL_MAX */
+
+#ifndef PTW32_BUILD
+
+#ifdef __CLEANUP_SEH
+
+/*
+ * Redefine the SEH __except keyword to ensure that applications
+ * propagate our internal exceptions up to the library's internal handlers.
+ */
+#define __except( E ) \
+ __except( ( GetExceptionCode() == ptw32_get_exception_services_code() ) \
+ ? EXCEPTION_CONTINUE_SEARCH : ( E ) )
+
+#endif /* __CLEANUP_SEH */
+
+#ifdef __CLEANUP_CXX
+
+/*
+ * Redefine the C++ catch keyword to ensure that applications
+ * propagate our internal exceptions up to the library's internal handlers.
+ */
+#ifdef _MSC_VER
+ /*
+ * WARNING: Replace any 'catch( ... )' with 'PtW32CatchAll'
+ * if you want Pthread-Win32 cancelation and pthread_exit to work.
+ */
+
+#ifndef PtW32NoCatchWarn
+
+#pragma message("Specify \"/DPtW32NoCatchWarn\" compiler flag to skip this message.")
+#pragma message("------------------------------------------------------------------")
+#pragma message("When compiling applications with MSVC++ and C++ exception handling:")
+#pragma message(" Replace any 'catch( ... )' in routines called from POSIX threads")
+#pragma message(" with 'PtW32CatchAll' or 'CATCHALL' if you want POSIX thread")
+#pragma message(" cancelation and pthread_exit to work. For example:")
+#pragma message("")
+#pragma message(" #ifdef PtW32CatchAll")
+#pragma message(" PtW32CatchAll")
+#pragma message(" #else")
+#pragma message(" catch(...)")
+#pragma message(" #endif")
+#pragma message(" {")
+#pragma message(" /* Catchall block processing */")
+#pragma message(" }")
+#pragma message("------------------------------------------------------------------")
+
+#endif
+
+#define PtW32CatchAll \
+ catch( ptw32_exception & ) { throw; } \
+ catch( ... )
+
+#else /* _MSC_VER */
+
+#define catch( E ) \
+ catch( ptw32_exception & ) { throw; } \
+ catch( E )
+
+#endif /* _MSC_VER */
+
+#endif /* __CLEANUP_CXX */
+
+#endif /* ! PTW32_BUILD */
+
+#ifdef __cplusplus
+} /* End of extern "C" */
+#endif /* __cplusplus */
+
+#ifdef PTW32__HANDLE_DEF
+# undef HANDLE
+#endif
+#ifdef PTW32__DWORD_DEF
+# undef DWORD
+#endif
+
+#undef PTW32_LEVEL
+#undef PTW32_LEVEL_MAX
+
+#endif /* ! RC_INVOKED */
+
+#endif /* PTHREAD_H */
diff --git a/Platform/Windows/include/sched.h b/Platform/Windows/include/sched.h
new file mode 100644
index 0000000..dfb8e93
--- /dev/null
+++ b/Platform/Windows/include/sched.h
@@ -0,0 +1,178 @@
+/*
+ * Module: sched.h
+ *
+ * Purpose:
+ * Provides an implementation of POSIX realtime extensions
+ * as defined in
+ *
+ * POSIX 1003.1b-1993 (POSIX.1b)
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+#ifndef _SCHED_H
+#define _SCHED_H
+
+#undef PTW32_LEVEL
+
+#if defined(_POSIX_SOURCE)
+#define PTW32_LEVEL 0
+/* Early POSIX */
+#endif
+
+#if defined(_POSIX_C_SOURCE) && _POSIX_C_SOURCE >= 199309
+#undef PTW32_LEVEL
+#define PTW32_LEVEL 1
+/* Include 1b, 1c and 1d */
+#endif
+
+#if defined(INCLUDE_NP)
+#undef PTW32_LEVEL
+#define PTW32_LEVEL 2
+/* Include Non-Portable extensions */
+#endif
+
+#define PTW32_LEVEL_MAX 3
+
+#if !defined(PTW32_LEVEL)
+#define PTW32_LEVEL PTW32_LEVEL_MAX
+/* Include everything */
+#endif
+
+
+#if __GNUC__ && ! defined (__declspec)
+# error Please upgrade your GNU compiler to one that supports __declspec.
+#endif
+
+/*
+ * When building the DLL code, you should define PTW32_BUILD so that
+ * the variables/functions are exported correctly. When using the DLL,
+ * do NOT define PTW32_BUILD, and then the variables/functions will
+ * be imported correctly.
+ */
+#ifndef PTW32_STATIC_LIB
+# ifdef PTW32_BUILD
+# define PTW32_DLLPORT __declspec (dllexport)
+# else
+# define PTW32_DLLPORT __declspec (dllimport)
+# endif
+#else
+# define PTW32_DLLPORT
+#endif
+
+/*
+ * This is a duplicate of what is in the autoconf config.h,
+ * which is only used when building the pthread-win32 libraries.
+ */
+
+#ifndef PTW32_CONFIG_H
+# if defined(WINCE)
+# define NEED_ERRNO
+# define NEED_SEM
+# endif
+# if defined(_UWIN) || defined(__MINGW32__)
+# define HAVE_MODE_T
+# endif
+#endif
+
+/*
+ *
+ */
+
+#if PTW32_LEVEL >= PTW32_LEVEL_MAX
+#ifdef NEED_ERRNO
+#include "need_errno.h"
+#else
+#include <errno.h>
+#endif
+#endif /* PTW32_LEVEL >= PTW32_LEVEL_MAX */
+
+#if defined(__MINGW32__) || defined(_UWIN)
+#if PTW32_LEVEL >= PTW32_LEVEL_MAX
+/* For pid_t */
+# include <sys/types.h>
+/* Required by Unix 98 */
+# include <time.h>
+#endif /* PTW32_LEVEL >= PTW32_LEVEL_MAX */
+#else
+typedef int pid_t;
+#endif
+
+/* Thread scheduling policies */
+
+enum {
+ SCHED_OTHER = 0,
+ SCHED_FIFO,
+ SCHED_RR,
+ SCHED_MIN = SCHED_OTHER,
+ SCHED_MAX = SCHED_RR
+};
+
+struct sched_param {
+ int sched_priority;
+};
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif /* __cplusplus */
+
+PTW32_DLLPORT int __cdecl sched_yield (void);
+
+PTW32_DLLPORT int __cdecl sched_get_priority_min (int policy);
+
+PTW32_DLLPORT int __cdecl sched_get_priority_max (int policy);
+
+PTW32_DLLPORT int __cdecl sched_setscheduler (pid_t pid, int policy);
+
+PTW32_DLLPORT int __cdecl sched_getscheduler (pid_t pid);
+
+/*
+ * Note that this macro returns ENOTSUP rather than
+ * ENOSYS as might be expected. However, returning ENOSYS
+ * should mean that sched_get_priority_{min,max} are
+ * not implemented as well as sched_rr_get_interval.
+ * This is not the case, since we just don't support
+ * round-robin scheduling. Therefore I have chosen to
+ * return the same value as sched_setscheduler when
+ * SCHED_RR is passed to it.
+ */
+#define sched_rr_get_interval(_pid, _interval) \
+ ( errno = ENOTSUP, (int) -1 )
+
+
+#ifdef __cplusplus
+} /* End of extern "C" */
+#endif /* __cplusplus */
+
+#undef PTW32_LEVEL
+#undef PTW32_LEVEL_MAX
+
+#endif /* !_SCHED_H */
+
diff --git a/Platform/Windows/include/semaphore.h b/Platform/Windows/include/semaphore.h
new file mode 100644
index 0000000..a3330a6
--- /dev/null
+++ b/Platform/Windows/include/semaphore.h
@@ -0,0 +1,166 @@
+/*
+ * Module: semaphore.h
+ *
+ * Purpose:
+ * Semaphores aren't actually part of the PThreads standard.
+ * They are defined by the POSIX Standard:
+ *
+ * POSIX 1003.1b-1993 (POSIX.1b)
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+#if !defined( SEMAPHORE_H )
+#define SEMAPHORE_H
+
+#undef PTW32_LEVEL
+
+#if defined(_POSIX_SOURCE)
+#define PTW32_LEVEL 0
+/* Early POSIX */
+#endif
+
+#if defined(_POSIX_C_SOURCE) && _POSIX_C_SOURCE >= 199309
+#undef PTW32_LEVEL
+#define PTW32_LEVEL 1
+/* Include 1b, 1c and 1d */
+#endif
+
+#if defined(INCLUDE_NP)
+#undef PTW32_LEVEL
+#define PTW32_LEVEL 2
+/* Include Non-Portable extensions */
+#endif
+
+#define PTW32_LEVEL_MAX 3
+
+#if !defined(PTW32_LEVEL)
+#define PTW32_LEVEL PTW32_LEVEL_MAX
+/* Include everything */
+#endif
+
+#if __GNUC__ && ! defined (__declspec)
+# error Please upgrade your GNU compiler to one that supports __declspec.
+#endif
+
+/*
+ * When building the DLL code, you should define PTW32_BUILD so that
+ * the variables/functions are exported correctly. When using the DLL,
+ * do NOT define PTW32_BUILD, and then the variables/functions will
+ * be imported correctly.
+ */
+#ifndef PTW32_STATIC_LIB
+# ifdef PTW32_BUILD
+# define PTW32_DLLPORT __declspec (dllexport)
+# else
+# define PTW32_DLLPORT __declspec (dllimport)
+# endif
+#else
+# define PTW32_DLLPORT
+#endif
+
+/*
+ * This is a duplicate of what is in the autoconf config.h,
+ * which is only used when building the pthread-win32 libraries.
+ */
+
+#ifndef PTW32_CONFIG_H
+# if defined(WINCE)
+# define NEED_ERRNO
+# define NEED_SEM
+# endif
+# if defined(_UWIN) || defined(__MINGW32__)
+# define HAVE_MODE_T
+# endif
+#endif
+
+/*
+ *
+ */
+
+#if PTW32_LEVEL >= PTW32_LEVEL_MAX
+#ifdef NEED_ERRNO
+#include "need_errno.h"
+#else
+#include <errno.h>
+#endif
+#endif /* PTW32_LEVEL >= PTW32_LEVEL_MAX */
+
+#define _POSIX_SEMAPHORES
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif /* __cplusplus */
+
+#ifndef HAVE_MODE_T
+typedef unsigned int mode_t;
+#endif
+
+
+typedef struct sem_t_ * sem_t;
+
+PTW32_DLLPORT int __cdecl sem_init (sem_t * sem,
+ int pshared,
+ unsigned int value);
+
+PTW32_DLLPORT int __cdecl sem_destroy (sem_t * sem);
+
+PTW32_DLLPORT int __cdecl sem_trywait (sem_t * sem);
+
+PTW32_DLLPORT int __cdecl sem_wait (sem_t * sem);
+
+PTW32_DLLPORT int __cdecl sem_timedwait (sem_t * sem,
+ const struct timespec * abstime);
+
+PTW32_DLLPORT int __cdecl sem_post (sem_t * sem);
+
+PTW32_DLLPORT int __cdecl sem_post_multiple (sem_t * sem,
+ int count);
+
+PTW32_DLLPORT int __cdecl sem_open (const char * name,
+ int oflag,
+ mode_t mode,
+ unsigned int value);
+
+PTW32_DLLPORT int __cdecl sem_close (sem_t * sem);
+
+PTW32_DLLPORT int __cdecl sem_unlink (const char * name);
+
+PTW32_DLLPORT int __cdecl sem_getvalue (sem_t * sem,
+ int * sval);
+
+#ifdef __cplusplus
+} /* End of extern "C" */
+#endif /* __cplusplus */
+
+#undef PTW32_LEVEL
+#undef PTW32_LEVEL_MAX
+
+#endif /* !SEMAPHORE_H */
diff --git a/Platform/Windows/lib_x64/libblas.dll b/Platform/Windows/lib_x64/libblas.dll
new file mode 100644
index 0000000..5320b92
Binary files /dev/null and b/Platform/Windows/lib_x64/libblas.dll differ
diff --git a/Platform/Windows/lib_x64/libblas.lib b/Platform/Windows/lib_x64/libblas.lib
new file mode 100644
index 0000000..17d1560
Binary files /dev/null and b/Platform/Windows/lib_x64/libblas.lib differ
diff --git a/Platform/Windows/lib_x64/libgcc_s_sjlj-1.dll b/Platform/Windows/lib_x64/libgcc_s_sjlj-1.dll
new file mode 100644
index 0000000..da9f60f
Binary files /dev/null and b/Platform/Windows/lib_x64/libgcc_s_sjlj-1.dll differ
diff --git a/Platform/Windows/lib_x64/libgfortran-3.dll b/Platform/Windows/lib_x64/libgfortran-3.dll
new file mode 100644
index 0000000..7149746
Binary files /dev/null and b/Platform/Windows/lib_x64/libgfortran-3.dll differ
diff --git a/Platform/Windows/lib_x64/liblapack.dll b/Platform/Windows/lib_x64/liblapack.dll
new file mode 100644
index 0000000..16d46b7
Binary files /dev/null and b/Platform/Windows/lib_x64/liblapack.dll differ
diff --git a/Platform/Windows/lib_x64/liblapack.lib b/Platform/Windows/lib_x64/liblapack.lib
new file mode 100644
index 0000000..85662df
Binary files /dev/null and b/Platform/Windows/lib_x64/liblapack.lib differ
diff --git a/Platform/Windows/lib_x64/libquadmath-0.dll b/Platform/Windows/lib_x64/libquadmath-0.dll
new file mode 100644
index 0000000..1f63c9b
Binary files /dev/null and b/Platform/Windows/lib_x64/libquadmath-0.dll differ
diff --git a/Platform/Windows/lib_x64/pthreadVC2_x64.dll b/Platform/Windows/lib_x64/pthreadVC2_x64.dll
new file mode 100644
index 0000000..8c29046
Binary files /dev/null and b/Platform/Windows/lib_x64/pthreadVC2_x64.dll differ
diff --git a/Platform/Windows/lib_x64/pthreadVC2_x64.lib b/Platform/Windows/lib_x64/pthreadVC2_x64.lib
new file mode 100644
index 0000000..7c71acc
Binary files /dev/null and b/Platform/Windows/lib_x64/pthreadVC2_x64.lib differ
diff --git a/Platform/Windows/lib_x86/libblas.dll b/Platform/Windows/lib_x86/libblas.dll
new file mode 100644
index 0000000..0ef85a0
Binary files /dev/null and b/Platform/Windows/lib_x86/libblas.dll differ
diff --git a/Platform/Windows/lib_x86/libblas.lib b/Platform/Windows/lib_x86/libblas.lib
new file mode 100644
index 0000000..6e3072a
Binary files /dev/null and b/Platform/Windows/lib_x86/libblas.lib differ
diff --git a/Platform/Windows/lib_x86/libgcc_s_dw2-1.dll b/Platform/Windows/lib_x86/libgcc_s_dw2-1.dll
new file mode 100644
index 0000000..90bd246
Binary files /dev/null and b/Platform/Windows/lib_x86/libgcc_s_dw2-1.dll differ
diff --git a/Platform/Windows/lib_x86/libgfortran-3.dll b/Platform/Windows/lib_x86/libgfortran-3.dll
new file mode 100644
index 0000000..c3ed2b6
Binary files /dev/null and b/Platform/Windows/lib_x86/libgfortran-3.dll differ
diff --git a/Platform/Windows/lib_x86/liblapack.dll b/Platform/Windows/lib_x86/liblapack.dll
new file mode 100644
index 0000000..d16ca2b
Binary files /dev/null and b/Platform/Windows/lib_x86/liblapack.dll differ
diff --git a/Platform/Windows/lib_x86/liblapack.lib b/Platform/Windows/lib_x86/liblapack.lib
new file mode 100644
index 0000000..5436f63
Binary files /dev/null and b/Platform/Windows/lib_x86/liblapack.lib differ
diff --git a/Platform/Windows/lib_x86/libquadmath-0.dll b/Platform/Windows/lib_x86/libquadmath-0.dll
new file mode 100644
index 0000000..7c47d53
Binary files /dev/null and b/Platform/Windows/lib_x86/libquadmath-0.dll differ
diff --git a/Platform/Windows/lib_x86/pthreadVC2.dll b/Platform/Windows/lib_x86/pthreadVC2.dll
new file mode 100644
index 0000000..93f562b
Binary files /dev/null and b/Platform/Windows/lib_x86/pthreadVC2.dll differ
diff --git a/Platform/Windows/lib_x86/pthreadVC2.lib b/Platform/Windows/lib_x86/pthreadVC2.lib
new file mode 100644
index 0000000..5873325
Binary files /dev/null and b/Platform/Windows/lib_x86/pthreadVC2.lib differ
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..6da1372
--- /dev/null
+++ b/README.md
@@ -0,0 +1,207 @@
+Simbody [![Build Status][buildstatus_image]][travisci]
+=======
+
+**[Caution: this README.md intro is under development and is buggy]**
+--
+
+Simbody is a high-performance, open-source toolkit for science- and
+engineering-quality simulation of articulated mechanisms, including
+biomechanical structures such as human and animal skeletons,
+mechanical systems like robots, vehicles, and machines, and anything
+else that can be described as a set of rigid bodies interconnected
+by joints, influenced by forces and motions, and restricted by
+constraints. Simbody includes a multibody dynamics library for
+modeling motion in [generalized/internal coordinates in O(n) time][thy].
+This is sometimes called a Featherstone-style physics engine.
+
+Simbody provides a C++ API that is used to build domain-specific applications;
+it is not a standalone application itself. For example, it is used by
+biomechanists in [OpenSim](http://opensim.stanford.edu), by roboticists in
+[Gazebo](http://gazebosim.org), and for biomolecular research in
+[MacroMoleculeBuilder (MMB)](https://simtk.org/home/rnatoolbox). Here's an
+artful simulation of several RNA molecules containing thousands of bodies,
+performed with MMB by [Samuel Flores][flores]:
+
+[![Sam Flores' Simbody RNA simulation][rna]][simbios]
+
+Read more about Simbody [here](https://simtk.org/home/simbody).
+
+
+Simple example: a double pendulum
+---------------------------------
+Here's some code to simulate and visualize a 2-link chain:
+
+```cpp
+#include "Simbody.h"
+using namespace SimTK;
+int main() {
+ // Define the system.
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::Gravity gravity(forces, matter, -YAxis, 9.8);
+
+ // Describe mass and visualization properties for a generic body.
+ Body::Rigid bodyInfo(MassProperties(1.0, Vec3(0), UnitInertia(1)));
+ bodyInfo.addDecoration(Transform(), DecorativeSphere(0.1));
+
+ // Create the moving (mobilized) bodies of the pendulum.
+ MobilizedBody::Pin pendulum1(matter.Ground(), Transform(Vec3(0)),
+ bodyInfo, Transform(Vec3(0, 1, 0)));
+ MobilizedBody::Pin pendulum2(pendulum1, Transform(Vec3(0)),
+ bodyInfo, Transform(Vec3(0, 1, 0)));
+
+ // Set up visualization.
+ Visualizer viz(system);
+ system.addEventReporter(new Visualizer::Reporter(viz, 0.01));
+
+ // Initialize the system and state.
+ State state = system.realizeTopology();
+ pendulum2.setRate(state, 5.0);
+
+ // Simulate for 50 seconds.
+ RungeKuttaMersonIntegrator integ(system);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(50.0);
+}
+```
+
+See [Simbody's User Guide][user] for a step-by-step explanation of this
+example.
+
+
+Features
+--------
+- Wide variety of joint, constraint, and force types; easily user-extended.
+- Forward, inverse, and mixed dynamics. Motion driven by forces or
+ prescribed motion.
+- Contact (Hertz, Hunt and Crossley models).
+- Gradient descent and interior point optimizers.
+- A variety of numerical integrators with error control.
+- Visualizer, using [OpenGL](http://www.opengl.org/).
+
+
+You want to...
+--------------
+* build Simbody from the source code: [Windows][buildwin], [Mac or
+ Linux][buildunix] (somewhat outdated).
+* [install Simbody from a binary distribution][user] (outdated).
+* [use Simbody in your own program][user].
+* [learn the theory behind Simbody][thy].
+* [extend Simbody][extend].
+* [ask questions or share information in the Simbody forum][forum]
+* [report a bug or suggest a feature][issue]
+* [contribute or get the source code](https://github.com/simbody/simbody/)
+
+Dependencies
+------------
+Simbody depends on LAPACK and BLAS for math, and on FreeGLUT, Xi and Xmu for
+visualization.
+
+### Windows
+We give the dependencies to you!
+
+### Mac
+The XCode developer package gives LAPACK and BLAS to you via the Accelerate
+framework. Mac's come with the visualization dependencies.
+
+### Linux/Ubuntu
+Your system's package manager surely has the dependencies. On Ubuntu you can
+enter the following in a terminal:
+```sh
+$ apt-get install liblapack-dev
+```
+
+Optionally, for visualization:
+
+```sh
+$ apt-get install freeglut-dev libxi-dev libxmu-dev
+```
+You may need to run these lines as a superuser (`sudo apt-get ...`).
+
+
+Building and Installing
+-----------------------
+We currently do not provide a pre-built binary distribution; you must build the
+source code yourself. We're working on getting Simbody into the Debian and
+Ubuntu repositories, though, so you'll be able to `apt-get` them.
+
+#### Download the source code
+Download the source code from https://github.com/simbody/simbody/releases.
+Alternatively, if you have [Git](http://git-scm.com) installed, you can clone
+the Simbody repository. From a command line, this would look like:
+
+```sh
+$ git clone git at github.com/simbody/simbody.git
+```
+
+From this point, if you're on Windows or you don't want to do anything on the
+command line, head straight for either the [Windows][buildwin] or the [Unix
+(Mac/Linux)][buildunix] instructions; we can't do much for you without showing
+you screenshots.
+
+#### Install CMake
+
+[Download](http://www.cmake.org/cmake/resources/software.html) and install
+CMake, which is a program we use to manage the build process. On Ubuntu, you
+can obtain CMake via `$ apt-get install cmake` in a terminal.
+
+#### Build and install Simbody
+
+Say you've placed the source code into `~/simbody-source/` and that you want to
+install Simbody to `<install-dir>` (perhaps, `~/simbody-install`).
+
+```sh
+$ cd ~/
+$ mkdir simbody-build
+$ cd simbody-build
+$ cmake -DCMAKE_INSTALL_PREFIX=<install-dir> ../simbody-source
+$ make install
+```
+
+You may need to run the last line as a superuser (`sudo apt-get ...`), if you
+chose an `<install-dir>` that you can't otherwise write to.
+
+If you have the dependencies, you should have no issues with the installation.
+
+#### Configure your system to find Simbody
+
+Set the environment variable SimTK_INSTALL_DIR `<install-dir>`. This is not
+strictly required but helps with examples and locating the Visualizer, and we
+use it in the next step.
+
+* Mac: `$ echo 'export SimTK_INSTALL_DIR=<install-dir>' > ~/.bash_profile`
+* Ubuntu: `$ echo 'export SimTK_INSTALL_DIR=<install-dir>' > ~/.bashrc`
+
+
+Set the appropriate environment variable so the libraries can be found:
+
+* Mac: `echo 'export DYLD_LIBRARY_PATH:$SimTK_INSTALL_DIR/lib' > `/.bash_profile`
+* Ubuntu: `echo 'export
+ LD_LIBRARY_PATH:$SimTK_INSTALL_DIR/lib/x86_64-linux-gnu' > ~/.bashrc`.
+ Actually, you may need to replace `x86_64-linux-gnu` with the approriate
+ directory on your platform.
+
+Close and open a new terminal window.
+
+#### Test your installation
+
+In your new terminal window, run:
+
+* Mac: `$ $SimTK_INSTALL_DIR/examples/bin/SimbodyInstallTest`
+* Linux: `$ $SimTK_INSTALL_DIR/lib/x86_64-linux-gnu/simbody/examples/SimbodyInstallTest`
+
+
+[buildstatus_image]: https://travis-ci.org/simbody/simbody.png?branch=master
+[travisci]: https://travis-ci.org/simbody/simbody
+[user]: https://github.com/simbody/simbody/raw/master/Simbody/doc/SimbodyAndMolmodelUserGuide.pdf
+[rna]:https://raw2.github.com/simbody/simbody/master/doc/images/simbios_11000_body_RNA.gif
+[simbios]: http://simbios.stanford.edu/
+[thy]: https://github.com/simbody/simbody/raw/master/Simbody/doc/SimbodyTheoryManual.pdf
+[extend]: https://github.com/simbody/simbody/raw/master/Simbody/doc/SimbodyAdvancedProgrammingGuide.pdf
+[flores]: https://simtk.org/forums/memberlist.php?mode=viewprofile&u=482
+[forum]: https://simtk.org/forums/viewforum.php?f=47
+[issue]: https://github.com/simbody/simbody/issues/new
+[buildwin]: https://github.com/simbody/simbody/raw/master/doc/HowToBuildSimbodyFromSource_Windows.pdf
+[buildunix]: https://github.com/simbody/simbody/raw/master/doc/HowToBuildSimbodyFromSource_MacLinux.pdf
diff --git a/SimTKcommon/BigMatrix/include/SimTKcommon/internal/BigMatrix.h b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/BigMatrix.h
new file mode 100644
index 0000000..97e9936
--- /dev/null
+++ b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/BigMatrix.h
@@ -0,0 +1,1564 @@
+#ifndef SimTK_SIMMATRIX_BIGMATRIX_H_
+#define SimTK_SIMMATRIX_BIGMATRIX_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This file defines the client side of the SimTK::Matrix classes, which
+ * hold medium to large, variable-sized matrices whose elements are packed
+ * SimTK "Composite Numerical Types" (CNTs). Unlike CNTs, the implemention here
+ * is opaque, and almost all properties are captured in the implementation at
+ * run time rather than in the type at compile time.
+ *
+ * Every Matrix consists logically of three pieces:
+ * - the matrix handle
+ * - the matrix helper
+ * - and the matrix data.
+ *
+ * They are organized like this:
+ * <pre>
+ * ------------ ------------
+ * | Handle<E> | -------> | |
+ * ------------ <------- | Helper<S> |
+ * | |
+ * | | --------~ ~--
+ * | | ------> | Data<S> ... |
+ * ------------ --------~ ~--
+ * </pre>
+ * The handle is the object actually appearing in SimTK API user programs.
+ * It always consists of just a single pointer, pointing to a library-side
+ * "helper" object whose implementation is opaque. The handle is templatized
+ * by the user's element type, which may be any packed composite numerical
+ * type, including scalar types like \c float and \c complex<double>, but also
+ * composite types such as \c Vec3 or \c Mat<2,2,Mat<3,3>>. A Matrix handle
+ * owns the helper to which it points and must destruct the helper when
+ * the handle's destructor is called.
+ *
+ * The helper, on the other hand, is parameterized only by the underlying scalar
+ * type. There are exactly 12 SimTK scalar types, so all can be instantiated on
+ * the library side leaving the implementation opaque and thus flexible from
+ * release to release without compromising binary compatibility. (The scalar
+ * types are: the four C++ standard types float and double,
+ * complex<float>, and complex<double>; the SimTK numbers conjugate<float> and
+ * conjugate<double>; and negator<> types templatized by any of the six
+ * numeric types.) The helper contains several kinds of information:
+ * - the underlying scalar type S (as its template parameter)
+ * - the number of scalars in the handle's logical element type E
+ * - whether this is an owner matrix, or just a view
+ * - the handle "commitment"; defining the range of matrix characteristics
+ * to which that handle may refer
+ * - the actual characteristics of the matrix currently represented by
+ * the helper
+ * - a virtual function table full of methods which are aware of the
+ * logical structure of the Matrix and the physical structure of
+ * the data to support operations such as element indexing
+ * - a pointer to the underlying data, which may be shared with other
+ * helpers
+ *
+ * The data itself consists only of scalars
+ * S of the same type as the helper's template argument, but different
+ * helpers can look at the same data differently. For examples, when the
+ * elements are composite consisting of k scalars, the helper will provide a
+ * view of the data in which its scalars are interpreted in groups of k.
+ * Many other reinterpretations of the data are possible and useful, such
+ * as a real-valued helper viewing only the real or imaginary part of
+ * complex data, or a helper which views the data order as though it were
+ * transposed.
+ *
+ * At most \e one matrix helper owns the matrix data and is responsible
+ * for deleting that data when no longer needed. That is called an "owner"
+ * helper and its associated handle is an owner handle. Normally the owner
+ * is the handle (and helper) that allocated the data, and
+ * in most cases an owner can resize the data at will. Many other handles
+ * may reference the same data; those non-owner handles are called "views".
+ * Every view may present a different picture of the underlying data. The
+ * default view is "whole" meaning that all the elements of the data are
+ * visible, and appear in their normal order. A "transpose" view also shows
+ * all the elements but the matrix dimensions and indices are reversed.
+ * Other common views are "block" to select a sub-block of a matrix, and
+ * "diagonal" which shows only the diagonal of a matrix (as a vector).
+ *
+ * NOTE: Destruction of an owner destructs the data it owns
+ * *regardless* of the presence of other views into that data! I.e., these
+ * are not reference counted. TODO: should we change that?
+ *
+ * In some cases there may be no owner helper for a particular piece of
+ * matrix data. That occurs when pre-existing memory, such as a Fortran
+ * array, is used to construct a Matrix. In that case all the helpers are
+ * views and the data will persist after the destruction of the last
+ * referencing helper.
+ *
+ * A Matrix that is the owner of its data will be resized whenever
+ * necessary, unless you take active steps to prevent that. For example, if
+ * you declare a Vector, the number of rows can resize but the number of
+ * columns will be locked at 1. A RowVector does the reverse. You can also
+ * explicitly lock the number of rows and/or columns of a matrix to prevent
+ * unwanted resizes.
+ *
+ * Here are the classes and short descriptions:
+ * <pre>
+ * MatrixHelper<S> interface to the opaque implementation, templatized
+ * by scalar type only
+ * MatrixBase<CNT> fully templatized client, contains a MatrixHelper
+ * </pre>
+ *
+ * The rest are dataless classes all of which can be interconverted just
+ * by recasting. Every one of these classes has a default conversion to
+ * type Matrix_<same element type>, so users can write functions that expect
+ * a Matrix argument and pass it a Vector, RowVectorView, or whatever.
+ *
+ * <pre>
+ * VectorBase<CNT> these are derived from MatrixBase and add no new data,
+ * RowVectorBase<CNT> but change some of the operators and other methods to
+ * be appropriate for 1d data.
+ *
+ * Matrix_<CNT> 2d owner class (a MatrixBase<CNT>)
+ * Vector_<CNT> column owner class (a VectorBase<CNT>)
+ * RowVector_<CNT> row owner class (a RowVectorBase<CNT>)
+ * </pre>
+ *
+ * Views are exactly the same as the corresponding owner class, but with
+ * shallow construction and assignment semantics.
+ *
+ * <pre>
+ * MatrixView_<CNT>, VectorView_<CNT>, RowVectorView_<CNT>
+ * </pre>
+ *
+ */
+
+#include "SimTKcommon/Scalar.h"
+#include "SimTKcommon/SmallMatrix.h"
+
+#include "SimTKcommon/internal/MatrixHelper.h"
+#include "SimTKcommon/internal/MatrixCharacteristics.h"
+
+#include <iostream>
+#include <cassert>
+#include <complex>
+#include <cstddef>
+#include <limits>
+
+namespace SimTK {
+ template <class ELT> class MatrixBase;
+ template <class ELT> class VectorBase;
+ template <class ELT> class RowVectorBase;
+
+ template <class ELT = Real> class MatrixView_;
+ template <class ELT = Real> class Matrix_;
+
+ template <class ELT = Real> class VectorView_;
+ template <class ELT = Real> class Vector_;
+
+ template <class ELT = Real> class RowVectorView_;
+ template <class ELT = Real> class RowVector_;
+
+ template <class ELT, class VECTOR_CLASS> class VectorIterator;
+}
+
+#include "SimTKcommon/internal/MatrixBase.h"
+#include "SimTKcommon/internal/VectorBase.h"
+#include "SimTKcommon/internal/RowVectorBase.h"
+
+#include "SimTKcommon/internal/MatrixView_.h"
+#include "SimTKcommon/internal/Matrix_.h"
+
+#include "SimTKcommon/internal/VectorView_.h"
+#include "SimTKcommon/internal/Vector_.h"
+
+#include "SimTKcommon/internal/RowVectorView_.h"
+#include "SimTKcommon/internal/RowVector_.h"
+
+#include "SimTKcommon/internal/VectorIterator.h"
+
+
+namespace SimTK {
+
+// ------------------------ MatrixBase definitions ----------------------------
+
+template <class ELT> inline MatrixView_<ELT>
+MatrixBase<ELT>::block(int i, int j, int m, int n) const {
+ SimTK_INDEXCHECK(i,nrow()+1,"MatrixBase::block()");
+ SimTK_INDEXCHECK(j,ncol()+1,"MatrixBase::block()");
+ SimTK_SIZECHECK(i+m,nrow(),"MatrixBase::block()");
+ SimTK_SIZECHECK(j+n,ncol(),"MatrixBase::block()");
+
+ MatrixHelper<Scalar> h(MatrixCommitment(),helper,i,j,m,n);
+ return MatrixView_<ELT>(h.stealRep());
+}
+
+template <class ELT> inline MatrixView_<ELT>
+MatrixBase<ELT>::updBlock(int i, int j, int m, int n) {
+ SimTK_INDEXCHECK(i,nrow()+1,"MatrixBase::updBlock()");
+ SimTK_INDEXCHECK(j,ncol()+1,"MatrixBase::updBlock()");
+ SimTK_SIZECHECK(i+m,nrow(),"MatrixBase::updBlock()");
+ SimTK_SIZECHECK(j+n,ncol(),"MatrixBase::updBlock()");
+
+ MatrixHelper<Scalar> h(MatrixCommitment(),helper,i,j,m,n);
+ return MatrixView_<ELT>(h.stealRep());
+}
+
+template <class E> inline MatrixView_<typename CNT<E>::THerm>
+MatrixBase<E>::transpose() const {
+ MatrixHelper<typename CNT<Scalar>::THerm>
+ h(MatrixCommitment(),
+ helper, typename MatrixHelper<typename CNT<Scalar>::THerm>::TransposeView());
+ return MatrixView_<typename CNT<E>::THerm>(h.stealRep());
+}
+
+template <class E> inline MatrixView_<typename CNT<E>::THerm>
+MatrixBase<E>::updTranspose() {
+ MatrixHelper<typename CNT<Scalar>::THerm>
+ h(MatrixCommitment(),
+ helper, typename MatrixHelper<typename CNT<Scalar>::THerm>::TransposeView());
+ return MatrixView_<typename CNT<E>::THerm>(h.stealRep());
+}
+
+template <class E> inline VectorView_<E>
+MatrixBase<E>::diag() const {
+ MatrixHelper<Scalar> h(MatrixCommitment::Vector(),
+ helper, typename MatrixHelper<Scalar>::DiagonalView());
+ return VectorView_<E>(h.stealRep());
+}
+
+template <class E> inline VectorView_<E>
+MatrixBase<E>::updDiag() {
+ MatrixHelper<Scalar> h(MatrixCommitment::Vector(),
+ helper, typename MatrixHelper<Scalar>::DiagonalView());
+ return VectorView_<E>(h.stealRep());
+}
+
+template <class ELT> inline VectorView_<ELT>
+MatrixBase<ELT>::col(int j) const {
+ SimTK_INDEXCHECK(j,ncol(),"MatrixBase::col()");
+
+ MatrixHelper<Scalar> h(MatrixCommitment::Vector(),
+ helper,0,j,nrow(),1);
+ return VectorView_<ELT>(h.stealRep());
+}
+
+template <class ELT> inline VectorView_<ELT>
+MatrixBase<ELT>::updCol(int j) {
+ SimTK_INDEXCHECK(j,ncol(),"MatrixBase::updCol()");
+
+ MatrixHelper<Scalar> h(MatrixCommitment::Vector(),
+ helper,0,j,nrow(),1);
+ return VectorView_<ELT>(h.stealRep());
+}
+
+template <class ELT> inline RowVectorView_<ELT>
+MatrixBase<ELT>::row(int i) const {
+ SimTK_INDEXCHECK(i,nrow(),"MatrixBase::row()");
+
+ MatrixHelper<Scalar> h(MatrixCommitment::RowVector(),
+ helper,i,0,1,ncol());
+ return RowVectorView_<ELT>(h.stealRep());
+}
+
+template <class ELT> inline RowVectorView_<ELT>
+MatrixBase<ELT>::updRow(int i) {
+ SimTK_INDEXCHECK(i,nrow(),"MatrixBase::updRow()");
+
+ MatrixHelper<Scalar> h(MatrixCommitment::RowVector(),
+ helper,i,0,1,ncol());
+ return RowVectorView_<ELT>(h.stealRep());
+}
+
+// M = diag(v) * M; v must have nrow() elements.
+// That is, M[i] *= v[i].
+template <class ELT> template <class EE> inline MatrixBase<ELT>&
+MatrixBase<ELT>::rowScaleInPlace(const VectorBase<EE>& v) {
+ assert(v.nrow() == nrow());
+ for (int i=0; i < nrow(); ++i)
+ (*this)[i] *= v[i];
+ return *this;
+}
+
+template <class ELT> template <class EE> inline void
+MatrixBase<ELT>::rowScale(const VectorBase<EE>& v, typename MatrixBase<ELT>::template EltResult<EE>::Mul& out) const {
+ assert(v.nrow() == nrow());
+ out.resize(nrow(), ncol());
+ for (int j=0; j<ncol(); ++j)
+ for (int i=0; i<nrow(); ++i)
+ out(i,j) = (*this)(i,j) * v[i];
+}
+
+// M = M * diag(v); v must have ncol() elements
+// That is, M(i) *= v[i]
+template <class ELT> template <class EE> inline MatrixBase<ELT>&
+MatrixBase<ELT>::colScaleInPlace(const VectorBase<EE>& v) {
+ assert(v.nrow() == ncol());
+ for (int j=0; j < ncol(); ++j)
+ (*this)(j) *= v[j];
+ return *this;
+}
+
+template <class ELT> template <class EE> inline void
+MatrixBase<ELT>::colScale(const VectorBase<EE>& v, typename MatrixBase<ELT>::template EltResult<EE>::Mul& out) const {
+ assert(v.nrow() == ncol());
+ out.resize(nrow(), ncol());
+ for (int j=0; j<ncol(); ++j)
+ for (int i=0; i<nrow(); ++i)
+ out(i,j) = (*this)(i,j) * v[j];
+}
+
+
+// M(i,j) *= r[i]*c[j]; r must have nrow() elements; c must have ncol() elements
+template <class ELT> template <class ER, class EC> inline MatrixBase<ELT>&
+MatrixBase<ELT>::rowAndColScaleInPlace(const VectorBase<ER>& r, const VectorBase<EC>& c) {
+ assert(r.nrow()==nrow() && c.nrow()==ncol());
+ for (int j=0; j<ncol(); ++j)
+ for (int i=0; i<nrow(); ++i)
+ (*this)(i,j) *= (r[i]*c[j]);
+ return *this;
+}
+
+template <class ELT> template <class ER, class EC> inline void
+MatrixBase<ELT>::rowAndColScale(
+ const VectorBase<ER>& r,
+ const VectorBase<EC>& c,
+ typename EltResult<typename VectorBase<ER>::template EltResult<EC>::Mul>::Mul&
+ out) const
+{
+ assert(r.nrow()==nrow() && c.nrow()==ncol());
+ out.resize(nrow(), ncol());
+ for (int j=0; j<ncol(); ++j)
+ for (int i=0; i<nrow(); ++i)
+ out(i,j) = (*this)(i,j) * (r[i]*c[j]);
+}
+
+// M(i,j) = s
+template <class ELT> template <class S> inline MatrixBase<ELT>&
+MatrixBase<ELT>::elementwiseAssign(const S& s) {
+ for (int j=0; j<ncol(); ++j)
+ for (int i=0; i<nrow(); ++i)
+ (*this)(i,j) = s;
+ return *this;
+}
+
+// Set M(i,j) = M(i,j)^-1.
+template <class ELT> inline MatrixBase<ELT>&
+MatrixBase<ELT>::elementwiseInvertInPlace() {
+ const int nr=nrow(), nc=ncol();
+ for (int j=0; j<nc; ++j)
+ for (int i=0; i<nr; ++i) {
+ ELT& e = updElt(i,j);
+ e = CNT<ELT>::invert(e);
+ }
+ return *this;
+}
+
+template <class ELT> inline void
+MatrixBase<ELT>::elementwiseInvert(MatrixBase<typename CNT<E>::TInvert>& out) const {
+ const int nr=nrow(), nc=ncol();
+ out.resize(nr,nc);
+ for (int j=0; j<nc; ++j)
+ for (int i=0; i<nr; ++i)
+ out(i,j) = CNT<ELT>::invert((*this)(i,j));
+}
+
+// M(i,j) += s
+template <class ELT> template <class S> inline MatrixBase<ELT>&
+MatrixBase<ELT>::elementwiseAddScalarInPlace(const S& s) {
+ for (int j=0; j<ncol(); ++j)
+ for (int i=0; i<nrow(); ++i)
+ (*this)(i,j) += s;
+ return *this;
+}
+
+template <class ELT> template <class S> inline void
+MatrixBase<ELT>::elementwiseAddScalar(
+ const S& s,
+ typename MatrixBase<ELT>::template EltResult<S>::Add& out) const
+{
+ const int nr=nrow(), nc=ncol();
+ out.resize(nr,nc);
+ for (int j=0; j<nc; ++j)
+ for (int i=0; i<nr; ++i)
+ out(i,j) = (*this)(i,j) + s;
+}
+
+// M(i,j) -= s
+template <class ELT> template <class S> inline MatrixBase<ELT>&
+MatrixBase<ELT>::elementwiseSubtractScalarInPlace(const S& s) {
+ for (int j=0; j<ncol(); ++j)
+ for (int i=0; i<nrow(); ++i)
+ (*this)(i,j) -= s;
+ return *this;
+}
+
+template <class ELT> template <class S> inline void
+MatrixBase<ELT>::elementwiseSubtractScalar(
+ const S& s,
+ typename MatrixBase<ELT>::template EltResult<S>::Sub& out) const
+{
+ const int nr=nrow(), nc=ncol();
+ out.resize(nr,nc);
+ for (int j=0; j<nc; ++j)
+ for (int i=0; i<nr; ++i)
+ out(i,j) = (*this)(i,j) - s;
+}
+
+// M(i,j) = s - M(i,j)
+template <class ELT> template <class S> inline MatrixBase<ELT>&
+MatrixBase<ELT>::elementwiseSubtractFromScalarInPlace(const S& s) {
+ const int nr=nrow(), nc=ncol();
+ for (int j=0; j<nc; ++j)
+ for (int i=0; i<nr; ++i) {
+ ELT& e = updElt(i,j);
+ e = s - e;
+ }
+ return *this;
+}
+
+template <class ELT> template <class S> inline void
+MatrixBase<ELT>::elementwiseSubtractFromScalar(
+ const S& s,
+ typename MatrixBase<S>::template EltResult<ELT>::Sub& out) const
+{
+ const int nr=nrow(), nc=ncol();
+ out.resize(nr,nc);
+ for (int j=0; j<nc; ++j)
+ for (int i=0; i<nr; ++i)
+ out(i,j) = s - (*this)(i,j);
+}
+
+// M(i,j) *= R(i,j); R must have same dimensions as this
+template <class ELT> template <class EE> inline MatrixBase<ELT>&
+MatrixBase<ELT>::elementwiseMultiplyInPlace(const MatrixBase<EE>& r) {
+ const int nr=nrow(), nc=ncol();
+ assert(r.nrow()==nr && r.ncol()==nc);
+ for (int j=0; j<nc; ++j)
+ for (int i=0; i<nr; ++i)
+ (*this)(i,j) *= r(i,j);
+ return *this;
+}
+
+template <class ELT> template <class EE> inline void
+MatrixBase<ELT>::elementwiseMultiply(
+ const MatrixBase<EE>& r,
+ typename MatrixBase<ELT>::template EltResult<EE>::Mul& out) const
+{
+ const int nr=nrow(), nc=ncol();
+ assert(r.nrow()==nr && r.ncol()==nc);
+ out.resize(nr,nc);
+ for (int j=0; j<nc; ++j)
+ for (int i=0; i<nr; ++i)
+ out(i,j) = (*this)(i,j) * r(i,j);
+}
+
+// M(i,j) = R(i,j) * M(i,j); R must have same dimensions as this
+template <class ELT> template <class EE> inline MatrixBase<ELT>&
+MatrixBase<ELT>::elementwiseMultiplyFromLeftInPlace(const MatrixBase<EE>& r) {
+ const int nr=nrow(), nc=ncol();
+ assert(r.nrow()==nr && r.ncol()==nc);
+ for (int j=0; j<nc; ++j)
+ for (int i=0; i<nr; ++i) {
+ ELT& e = updElt(i,j);
+ e = r(i,j) * e;
+ }
+ return *this;
+}
+
+template <class ELT> template <class EE> inline void
+MatrixBase<ELT>::elementwiseMultiplyFromLeft(
+ const MatrixBase<EE>& r,
+ typename MatrixBase<EE>::template EltResult<ELT>::Mul& out) const
+{
+ const int nr=nrow(), nc=ncol();
+ assert(r.nrow()==nr && r.ncol()==nc);
+ out.resize(nr,nc);
+ for (int j=0; j<nc; ++j)
+ for (int i=0; i<nr; ++i)
+ out(i,j) = r(i,j) * (*this)(i,j);
+}
+
+// M(i,j) /= R(i,j); R must have same dimensions as this
+template <class ELT> template <class EE> inline MatrixBase<ELT>&
+MatrixBase<ELT>::elementwiseDivideInPlace(const MatrixBase<EE>& r) {
+ const int nr=nrow(), nc=ncol();
+ assert(r.nrow()==nr && r.ncol()==nc);
+ for (int j=0; j<nc; ++j)
+ for (int i=0; i<nr; ++i)
+ (*this)(i,j) /= r(i,j);
+ return *this;
+}
+
+template <class ELT> template <class EE> inline void
+MatrixBase<ELT>::elementwiseDivide(
+ const MatrixBase<EE>& r,
+ typename MatrixBase<ELT>::template EltResult<EE>::Dvd& out) const
+{
+ const int nr=nrow(), nc=ncol();
+ assert(r.nrow()==nr && r.ncol()==nc);
+ out.resize(nr,nc);
+ for (int j=0; j<nc; ++j)
+ for (int i=0; i<nr; ++i)
+ out(i,j) = (*this)(i,j) / r(i,j);
+}
+// M(i,j) = R(i,j) / M(i,j); R must have same dimensions as this
+template <class ELT> template <class EE> inline MatrixBase<ELT>&
+MatrixBase<ELT>::elementwiseDivideFromLeftInPlace(const MatrixBase<EE>& r) {
+ const int nr=nrow(), nc=ncol();
+ assert(r.nrow()==nr && r.ncol()==nc);
+ for (int j=0; j<nc; ++j)
+ for (int i=0; i<nr; ++i) {
+ ELT& e = updElt(i,j);
+ e = r(i,j) / e;
+ }
+ return *this;
+}
+
+template <class ELT> template <class EE> inline void
+MatrixBase<ELT>::elementwiseDivideFromLeft(
+ const MatrixBase<EE>& r,
+ typename MatrixBase<EE>::template EltResult<ELT>::Dvd& out) const
+{
+ const int nr=nrow(), nc=ncol();
+ assert(r.nrow()==nr && r.ncol()==nc);
+ out.resize(nr,nc);
+ for (int j=0; j<nc; ++j)
+ for (int i=0; i<nr; ++i)
+ out(i,j) = r(i,j) / (*this)(i,j);
+}
+
+/*
+template <class ELT> inline MatrixView_< typename CNT<ELT>::TReal >
+MatrixBase<ELT>::real() const {
+ if (!CNT<ELT>::IsComplex) { // known at compile time
+ return MatrixView_< typename CNT<ELT>::TReal >( // this is just ELT
+ MatrixHelper(helper,0,0,nrow(),ncol())); // a view of the whole matrix
+ }
+ // Elements are complex -- helper uses underlying precision (real) type.
+ MatrixHelper<Precision> h(helper,typename MatrixHelper<Precision>::RealView);
+ return MatrixView_< typename CNT<ELT>::TReal >(h);
+}
+*/
+
+
+// ----------------------------------------------------------------------------
+/// @name Global operators involving Matrix objects
+/// These operators take MatrixBase arguments and produce Matrix_ results.
+/// @{
+
+// + and - allow mixed element types, but will fail to compile if the elements aren't
+// compatible. At run time these will fail if the dimensions are incompatible.
+template <class E1, class E2>
+Matrix_<typename CNT<E1>::template Result<E2>::Add>
+operator+(const MatrixBase<E1>& l, const MatrixBase<E2>& r) {
+ return Matrix_<typename CNT<E1>::template Result<E2>::Add>(l) += r;
+}
+
+template <class E>
+Matrix_<E> operator+(const MatrixBase<E>& l, const typename CNT<E>::T& r) {
+ return Matrix_<E>(l) += r;
+}
+
+template <class E>
+Matrix_<E> operator+(const typename CNT<E>::T& l, const MatrixBase<E>& r) {
+ return Matrix_<E>(r) += l;
+}
+
+template <class E1, class E2>
+Matrix_<typename CNT<E1>::template Result<E2>::Sub>
+operator-(const MatrixBase<E1>& l, const MatrixBase<E2>& r) {
+ return Matrix_<typename CNT<E1>::template Result<E2>::Sub>(l) -= r;
+}
+
+template <class E>
+Matrix_<E> operator-(const MatrixBase<E>& l, const typename CNT<E>::T& r) {
+ return Matrix_<E>(l) -= r;
+}
+
+template <class E>
+Matrix_<E> operator-(const typename CNT<E>::T& l, const MatrixBase<E>& r) {
+ Matrix_<E> temp(r.nrow(), r.ncol());
+ temp = l;
+ return (temp -= r);
+}
+
+// Scalar multiply and divide. You might wish the scalar could be
+// a templatized type "E2", but that would create horrible ambiguities since
+// E2 would match not only scalar types but everything else including
+// matrices.
+template <class E> Matrix_<E>
+operator*(const MatrixBase<E>& l, const typename CNT<E>::StdNumber& r)
+ { return Matrix_<E>(l)*=r; }
+
+template <class E> Matrix_<E>
+operator*(const typename CNT<E>::StdNumber& l, const MatrixBase<E>& r)
+ { return Matrix_<E>(r)*=l; }
+
+template <class E> Matrix_<E>
+operator/(const MatrixBase<E>& l, const typename CNT<E>::StdNumber& r)
+ { return Matrix_<E>(l)/=r; }
+
+// Handle ints explicitly.
+template <class E> Matrix_<E>
+operator*(const MatrixBase<E>& l, int r)
+ { return Matrix_<E>(l)*= typename CNT<E>::StdNumber(r); }
+
+template <class E> Matrix_<E>
+operator*(int l, const MatrixBase<E>& r)
+ { return Matrix_<E>(r)*= typename CNT<E>::StdNumber(l); }
+
+template <class E> Matrix_<E>
+operator/(const MatrixBase<E>& l, int r)
+ { return Matrix_<E>(l)/= typename CNT<E>::StdNumber(r); }
+
+/// @}
+
+/// @name Global operators involving Vector objects
+/// These operators take VectorBase arguments and produce Vector_ results.
+/// @{
+
+template <class E1, class E2>
+Vector_<typename CNT<E1>::template Result<E2>::Add>
+operator+(const VectorBase<E1>& l, const VectorBase<E2>& r) {
+ return Vector_<typename CNT<E1>::template Result<E2>::Add>(l) += r;
+}
+template <class E>
+Vector_<E> operator+(const VectorBase<E>& l, const typename CNT<E>::T& r) {
+ return Vector_<E>(l) += r;
+}
+template <class E>
+Vector_<E> operator+(const typename CNT<E>::T& l, const VectorBase<E>& r) {
+ return Vector_<E>(r) += l;
+}
+template <class E1, class E2>
+Vector_<typename CNT<E1>::template Result<E2>::Sub>
+operator-(const VectorBase<E1>& l, const VectorBase<E2>& r) {
+ return Vector_<typename CNT<E1>::template Result<E2>::Sub>(l) -= r;
+}
+template <class E>
+Vector_<E> operator-(const VectorBase<E>& l, const typename CNT<E>::T& r) {
+ return Vector_<E>(l) -= r;
+}
+template <class E>
+Vector_<E> operator-(const typename CNT<E>::T& l, const VectorBase<E>& r) {
+ Vector_<E> temp(r.size());
+ temp = l;
+ return (temp -= r);
+}
+
+// Scalar multiply and divide.
+
+template <class E> Vector_<E>
+operator*(const VectorBase<E>& l, const typename CNT<E>::StdNumber& r)
+ { return Vector_<E>(l)*=r; }
+
+template <class E> Vector_<E>
+operator*(const typename CNT<E>::StdNumber& l, const VectorBase<E>& r)
+ { return Vector_<E>(r)*=l; }
+
+template <class E> Vector_<E>
+operator/(const VectorBase<E>& l, const typename CNT<E>::StdNumber& r)
+ { return Vector_<E>(l)/=r; }
+
+// Handle ints explicitly
+template <class E> Vector_<E>
+operator*(const VectorBase<E>& l, int r)
+ { return Vector_<E>(l)*= typename CNT<E>::StdNumber(r); }
+
+template <class E> Vector_<E>
+operator*(int l, const VectorBase<E>& r)
+ { return Vector_<E>(r)*= typename CNT<E>::StdNumber(l); }
+
+template <class E> Vector_<E>
+operator/(const VectorBase<E>& l, int r)
+ { return Vector_<E>(l)/= typename CNT<E>::StdNumber(r); }
+
+// These are fancier "scalars"; whether they are allowed depends on
+// whether the element type and the CNT are compatible.
+
+// Vector * Vec
+template <class E1, int M, class E2, int S>
+Vector_<typename CNT<E1>::template Result< Vec<M,E2,S> >::Mul>
+operator*(const VectorBase<E1>& v, const Vec<M,E2,S>& s) {
+ Vector_<typename CNT<E1>::template Result< Vec<M,E2,S> >::Mul> res(v.nrow());
+ for (int i=0; i < v.nrow(); ++i)
+ res[i] = v[i]*s;
+ return res;
+}
+
+// Vec * Vector
+template <class E1, int M, class E2, int S>
+Vector_<typename Vec<M,E2,S>::template Result<E1>::Mul>
+operator*(const Vec<M,E2,S>& s, const VectorBase<E1>& v) {
+ Vector_<typename Vec<M,E2,S>::template Result<E1>::Mul> res(v.nrow());
+ for (int i=0; i < v.nrow(); ++i)
+ res[i] = s*v[i];
+ return res;
+}
+
+// Vector * Row
+template <class E1, int N, class E2, int S>
+Vector_<typename CNT<E1>::template Result< Row<N,E2,S> >::Mul>
+operator*(const VectorBase<E1>& v, const Row<N,E2,S>& s) {
+ Vector_<typename CNT<E1>::template Result< Row<N,E2,S> >::Mul> res(v.nrow());
+ for (int i=0; i < v.nrow(); ++i)
+ res[i] = v[i]*s;
+ return res;
+}
+
+// Row * Vector
+template <class E1, int N, class E2, int S>
+Vector_<typename Row<N,E2,S>::template Result<E1>::Mul>
+operator*(const Row<N,E2,S>& s, const VectorBase<E1>& v) {
+ Vector_<typename Row<N,E2,S>::template Result<E1>::Mul> res(v.nrow());
+ for (int i=0; i < v.nrow(); ++i)
+ res[i] = s*v[i];
+ return res;
+}
+
+// Vector * Mat
+template <class E1, int M, int N, class E2, int S1, int S2>
+Vector_<typename CNT<E1>::template Result< Mat<M,N,E2,S1,S2> >::Mul>
+operator*(const VectorBase<E1>& v, const Mat<M,N,E2,S1,S2>& s) {
+ Vector_<typename CNT<E1>::template Result< Mat<M,N,E2,S1,S2> >::Mul> res(v.nrow());
+ for (int i=0; i < v.nrow(); ++i)
+ res[i] = v[i]*s;
+ return res;
+}
+
+// Mat * Vector
+template <class E1, int M, int N, class E2, int S1, int S2>
+Vector_<typename Mat<M,N,E2,S1,S2>::template Result<E1>::Mul>
+operator*(const Mat<M,N,E2,S1,S2>& s, const VectorBase<E1>& v) {
+ Vector_<typename Mat<M,N,E2,S1,S2>::template Result<E1>::Mul> res(v.nrow());
+ for (int i=0; i < v.nrow(); ++i)
+ res[i] = s*v[i];
+ return res;
+}
+
+// Vector * SymMat
+template <class E1, int M, class E2, int S>
+Vector_<typename CNT<E1>::template Result< SymMat<M,E2,S> >::Mul>
+operator*(const VectorBase<E1>& v, const SymMat<M,E2,S>& s) {
+ Vector_<typename CNT<E1>::template Result< SymMat<M,E2,S> >::Mul> res(v.nrow());
+ for (int i=0; i < v.nrow(); ++i)
+ res[i] = v[i]*s;
+ return res;
+}
+
+// SymMat * Vector
+template <class E1, int M, class E2, int S>
+Vector_<typename SymMat<M,E2,S>::template Result<E1>::Mul>
+operator*(const SymMat<M,E2,S>& s, const VectorBase<E1>& v) {
+ Vector_<typename SymMat<M,E2,S>::template Result<E1>::Mul> res(v.nrow());
+ for (int i=0; i < v.nrow(); ++i)
+ res[i] = s*v[i];
+ return res;
+}
+
+/// @}
+
+/// @name Global operators involving RowVector objects
+/// These operators take RowVectorBase arguments and produce RowVector_ results.
+/// @{
+
+template <class E1, class E2>
+RowVector_<typename CNT<E1>::template Result<E2>::Add>
+operator+(const RowVectorBase<E1>& l, const RowVectorBase<E2>& r) {
+ return RowVector_<typename CNT<E1>::template Result<E2>::Add>(l) += r;
+}
+template <class E>
+RowVector_<E> operator+(const RowVectorBase<E>& l, const typename CNT<E>::T& r) {
+ return RowVector_<E>(l) += r;
+}
+template <class E>
+RowVector_<E> operator+(const typename CNT<E>::T& l, const RowVectorBase<E>& r) {
+ return RowVector_<E>(r) += l;
+}
+template <class E1, class E2>
+RowVector_<typename CNT<E1>::template Result<E2>::Sub>
+operator-(const RowVectorBase<E1>& l, const RowVectorBase<E2>& r) {
+ return RowVector_<typename CNT<E1>::template Result<E2>::Sub>(l) -= r;
+}
+template <class E>
+RowVector_<E> operator-(const RowVectorBase<E>& l, const typename CNT<E>::T& r) {
+ return RowVector_<E>(l) -= r;
+}
+template <class E>
+RowVector_<E> operator-(const typename CNT<E>::T& l, const RowVectorBase<E>& r) {
+ RowVector_<E> temp(r.size());
+ temp = l;
+ return (temp -= r);
+}
+
+// Scalar multiply and divide
+
+template <class E> RowVector_<E>
+operator*(const RowVectorBase<E>& l, const typename CNT<E>::StdNumber& r)
+ { return RowVector_<E>(l)*=r; }
+
+template <class E> RowVector_<E>
+operator*(const typename CNT<E>::StdNumber& l, const RowVectorBase<E>& r)
+ { return RowVector_<E>(r)*=l; }
+
+template <class E> RowVector_<E>
+operator/(const RowVectorBase<E>& l, const typename CNT<E>::StdNumber& r)
+ { return RowVector_<E>(l)/=r; }
+
+// Handle ints explicitly.
+template <class E> RowVector_<E>
+operator*(const RowVectorBase<E>& l, int r)
+ { return RowVector_<E>(l)*= typename CNT<E>::StdNumber(r); }
+
+template <class E> RowVector_<E>
+operator*(int l, const RowVectorBase<E>& r)
+ { return RowVector_<E>(r)*= typename CNT<E>::StdNumber(l); }
+
+template <class E> RowVector_<E>
+operator/(const RowVectorBase<E>& l, int r)
+ { return RowVector_<E>(l)/= typename CNT<E>::StdNumber(r); }
+
+
+// These are fancier "scalars"; whether they are allowed depends on
+// whether the element type and the CNT are compatible.
+
+// RowVector * Vec
+template <class E1, int M, class E2, int S>
+RowVector_<typename CNT<E1>::template Result< Vec<M,E2,S> >::Mul>
+operator*(const RowVectorBase<E1>& v, const Vec<M,E2,S>& s) {
+ RowVector_<typename CNT<E1>::template Result< Vec<M,E2,S> >::Mul> res(v.ncol());
+ for (int i=0; i < v.ncol(); ++i)
+ res[i] = v[i]*s;
+ return res;
+}
+
+// Vec * RowVector
+template <class E1, int M, class E2, int S>
+RowVector_<typename Vec<M,E2,S>::template Result<E1>::Mul>
+operator*(const Vec<M,E2,S>& s, const RowVectorBase<E1>& v) {
+ RowVector_<typename Vec<M,E2,S>::template Result<E1>::Mul> res(v.ncol());
+ for (int i=0; i < v.ncol(); ++i)
+ res[i] = s*v[i];
+ return res;
+}
+
+// RowVector * Row
+template <class E1, int N, class E2, int S>
+RowVector_<typename CNT<E1>::template Result< Row<N,E2,S> >::Mul>
+operator*(const RowVectorBase<E1>& v, const Row<N,E2,S>& s) {
+ RowVector_<typename CNT<E1>::template Result< Row<N,E2,S> >::Mul> res(v.ncol());
+ for (int i=0; i < v.ncol(); ++i)
+ res[i] = v[i]*s;
+ return res;
+}
+
+// Row * RowVector
+template <class E1, int N, class E2, int S>
+RowVector_<typename Row<N,E2,S>::template Result<E1>::Mul>
+operator*(const Row<N,E2,S>& s, const RowVectorBase<E1>& v) {
+ RowVector_<typename Row<N,E2,S>::template Result<E1>::Mul> res(v.ncol());
+ for (int i=0; i < v.ncol(); ++i)
+ res[i] = s*v[i];
+ return res;
+}
+
+// RowVector * Mat
+template <class E1, int M, int N, class E2, int S1, int S2>
+RowVector_<typename CNT<E1>::template Result< Mat<M,N,E2,S1,S2> >::Mul>
+operator*(const RowVectorBase<E1>& v, const Mat<M,N,E2,S1,S2>& s) {
+ RowVector_<typename CNT<E1>::template Result< Mat<M,N,E2,S1,S2> >::Mul> res(v.ncol());
+ for (int i=0; i < v.ncol(); ++i)
+ res[i] = v[i]*s;
+ return res;
+}
+
+// Mat * RowVector
+template <class E1, int M, int N, class E2, int S1, int S2>
+RowVector_<typename Mat<M,N,E2,S1,S2>::template Result<E1>::Mul>
+operator*(const Mat<M,N,E2,S1,S2>& s, const RowVectorBase<E1>& v) {
+ RowVector_<typename Mat<M,N,E2,S1,S2>::template Result<E1>::Mul> res(v.ncol());
+ for (int i=0; i < v.ncol(); ++i)
+ res[i] = s*v[i];
+ return res;
+}
+
+// RowVector * SymMat
+template <class E1, int M, class E2, int S>
+RowVector_<typename CNT<E1>::template Result< SymMat<M,E2,S> >::Mul>
+operator*(const RowVectorBase<E1>& v, const SymMat<M,E2,S>& s) {
+ RowVector_<typename CNT<E1>::template Result< SymMat<M,E2,S> >::Mul> res(v.ncol());
+ for (int i=0; i < v.ncol(); ++i)
+ res[i] = v[i]*s;
+ return res;
+}
+
+// SymMat * RowVector
+template <class E1, int M, class E2, int S>
+RowVector_<typename SymMat<M,E2,S>::template Result<E1>::Mul>
+operator*(const SymMat<M,E2,S>& s, const RowVectorBase<E1>& v) {
+ RowVector_<typename SymMat<M,E2,S>::template Result<E1>::Mul> res(v.ncol());
+ for (int i=0; i < v.ncol(); ++i)
+ res[i] = s*v[i];
+ return res;
+}
+
+/// @}
+
+
+/// @name Global operators involving mixed matrix, vector, and row vector objects
+/// These operators take MatrixBase, VectorBase, and RowVectorBase arguments
+/// and produce Matrix_, Vector_, and RowVector_ results.
+/// @{
+
+ // TODO: these should use LAPACK!
+
+// Dot product
+template <class E1, class E2>
+typename CNT<E1>::template Result<E2>::Mul
+operator*(const RowVectorBase<E1>& r, const VectorBase<E2>& v) {
+ assert(r.ncol() == v.nrow());
+ typename CNT<E1>::template Result<E2>::Mul sum(0);
+ for (int j=0; j < r.ncol(); ++j)
+ sum += r(j) * v[j];
+ return sum;
+}
+
+template <class E1, class E2>
+Vector_<typename CNT<E1>::template Result<E2>::Mul>
+operator*(const MatrixBase<E1>& m, const VectorBase<E2>& v) {
+ assert(m.ncol() == v.nrow());
+ Vector_<typename CNT<E1>::template Result<E2>::Mul> res(m.nrow());
+ for (int i=0; i< m.nrow(); ++i)
+ res[i] = m[i]*v;
+ return res;
+}
+
+template <class E1, class E2>
+Matrix_<typename CNT<E1>::template Result<E2>::Mul>
+operator*(const MatrixBase<E1>& m1, const MatrixBase<E2>& m2) {
+ assert(m1.ncol() == m2.nrow());
+ Matrix_<typename CNT<E1>::template Result<E2>::Mul>
+ res(m1.nrow(),m2.ncol());
+
+ for (int j=0; j < res.ncol(); ++j)
+ for (int i=0; i < res.nrow(); ++i)
+ res(i,j) = m1[i] * m2(j);
+
+ return res;
+}
+
+/// @}
+
+// This "private" static method is used to implement VectorView's
+// fillVectorViewFromStream() and Vector's readVectorFromStream()
+// namespace-scope static methods, which are in turn used to implement
+// VectorView's and
+// Vector's stream extraction operators ">>". This method has to be in the
+// header file so that we don't need to pass streams through the API, but it
+// is not intended for use by users and has no Doxygen presence, unlike
+// fillArrayFromStream() and readArrayFromStream() and (more commonly)
+// the extraction operators.
+template <class T> static inline
+std::istream& readVectorFromStreamHelper
+ (std::istream& in, bool isFixedSize, Vector_<T>& out)
+{
+ // If already failed, bad, or eof, set failed bit and return without
+ // touching the Vector.
+ if (!in.good()) {in.setstate(std::ios::failbit); return in;}
+
+ // If the passed-in Vector isn't resizeable, then we have to treat it as
+ // a fixed size VectorView regardless of the setting of the isFixedSize
+ // argument.
+ if (!out.isResizeable())
+ isFixedSize = true; // might be overriding the argument here
+
+ // numRequired will be ignored unless isFixedSize==true.
+ const int numRequired = isFixedSize ? out.size() : 0;
+
+ if (!isFixedSize)
+ out.clear(); // We're going to replace the entire contents of the Array.
+
+ // Skip initial whitespace. If that results in eof this may be a successful
+ // read of a 0-length, unbracketed Vector. That is OK for either a
+ // variable-length Vector or a fixed-length VectorView of length zero.
+ std::ws(in); if (in.fail()) return in;
+ if (in.eof()) {
+ if (isFixedSize && numRequired != 0)
+ in.setstate(std::ios_base::failbit); // zero elements not OK
+ return in;
+ }
+
+ // Here the stream is good and the next character is non-white.
+ assert(in.good());
+
+ // Use this for raw i/o (peeks and gets).
+ typename std::iostream::int_type ch;
+ const typename std::iostream::int_type EOFch =
+ std::iostream::traits_type::eof();
+
+ // First we'll look for the optional "~". If found, the brackets become
+ // required.
+ bool tildeFound = false;
+ ch = in.peek(); if (in.fail()) return in;
+ assert(ch != EOFch); // we already checked above
+ if ((char)ch == '~') {
+ tildeFound = true;
+ in.get(); // absorb the tilde
+ // Eat whitespace after the tilde to see what's next.
+ if (in.good()) std::ws(in);
+ // If we hit eof after the tilde we don't like the formatting.
+ if (!in.good()) {in.setstate(std::ios_base::failbit); return in;}
+ }
+
+ // Here the stream is good, the next character is non-white, and we
+ // might have seen a tilde.
+ assert(in.good());
+
+ // Now see if the sequence is bare or surrounded by (), or [].
+ bool lookForCloser = true;
+ char openBracket, closeBracket;
+ ch = in.peek(); if (in.fail()) return in;
+ assert(ch != EOFch); // we already checked above
+
+ openBracket = (char)ch;
+ if (openBracket=='(') {in.get(); closeBracket = ')';}
+ else if (openBracket=='[') {in.get(); closeBracket = ']';}
+ else lookForCloser = false;
+
+ // If we found a tilde, the opening bracket was mandatory. If we didn't
+ // find one then we reject the formatting.
+ if (tildeFound && !lookForCloser)
+ { in.setstate(std::ios_base::failbit); return in;}
+
+ // If lookForCloser is true, then closeBracket contains the terminating
+ // delimiter, otherwise we're not going to quit until eof.
+
+ // Eat whitespace after the opening bracket to see what's next.
+ if (in.good()) std::ws(in);
+
+ // If we're at eof now it must be because the open bracket was the
+ // last non-white character in the stream, which is an error.
+ if (!in.good()) {
+ if (in.eof()) {
+ assert(lookForCloser); // or we haven't read anything that could eof
+ in.setstate(std::ios::failbit);
+ }
+ return in;
+ }
+
+ // istream is good and next character is non-white; ready to read first
+ // value or terminator.
+
+ // We need to figure out whether the elements are space- or comma-
+ // separated and then insist on consistency.
+ bool commaOK = true, commaRequired = false;
+ bool terminatorSeen = false;
+ int nextIndex = 0;
+ while (true) {
+ char c;
+
+ // Here at the top of this loop, we have already successfully read
+ // n=nextIndex values of type T. For fixed-size reads, it might be
+ // the case that n==numRequired already, but we still may need to
+ // look for a closing bracket before we can declare victory.
+ // The stream is good() (not at eof) but it might be the case that
+ // there is nothing but white space left; we don't know yet because
+ // if we have satisfied the fixed-size count and are not expecting
+ // a terminator then we should quit without absorbing the trailing
+ // white space.
+ assert(in.good());
+
+ // Look for closing bracket before trying to read value.
+ if (lookForCloser) {
+ // Eat white space to find the closing bracket.
+ std::ws(in); if (!in.good()) break; // eof?
+ ch = in.peek(); assert(ch != EOFch);
+ if (!in.good()) break;
+ c = (char)ch;
+ if (c == closeBracket) {
+ in.get(); // absorb the closing bracket
+ terminatorSeen = true;
+ break;
+ }
+ // next char not a closing bracket; fall through
+ }
+
+ // We didn't look or didn't find a closing bracket. The istream is good
+ // but we might be looking at white space.
+
+ // If we already got all the elements we want, break for final checks.
+ if (isFixedSize && (nextIndex == numRequired))
+ break; // that's a full count.
+
+ // Look for comma before value, except the first time.
+ if (commaOK && nextIndex != 0) {
+ // Eat white space to find the comma.
+ std::ws(in); if (!in.good()) break; // eof?
+ ch = in.peek(); assert(ch != EOFch);
+ if (!in.good()) break;
+ c = (char)ch;
+ if (c == ',') {
+ in.get(); // absorb comma
+ commaRequired = true; // all commas from now on
+ } else { // next char not a comma
+ if (commaRequired) // bad, e.g.: v1, v2, v3 v4
+ { in.setstate(std::ios::failbit); break; }
+ else commaOK = false; // saw: v1 v2 (no commas now)
+ }
+ if (!in.good()) break; // might be eof
+ }
+
+ // No closing bracket yet; don't have enough elements; skipped comma
+ // if any; istream is good; might be looking at white space.
+ assert(in.good());
+
+ // Now read in an element of type T.
+ // The extractor T::operator>>() will ignore leading white space.
+ if (!isFixedSize)
+ out.resizeKeep(out.size()+1); // grow by one (default consructed)
+ in >> out[nextIndex]; if (in.fail()) break;
+ ++nextIndex;
+
+ if (!in.good()) break; // might be eof
+ }
+
+ // We will get here under a number of circumstances:
+ // - the fail bit is set in the istream, or
+ // - we reached eof
+ // - we saw a closing brace
+ // - we got all the elements we wanted (for a fixed-size read)
+ // Note that it is possible that we consumed everything except some
+ // trailing white space (meaning we're not technically at eof), but
+ // for consistency with built-in operator>>()'s we won't try to absorb
+ // that trailing white space.
+
+ if (!in.fail()) {
+ if (lookForCloser && !terminatorSeen)
+ in.setstate(std::ios::failbit); // missing terminator
+
+ if (isFixedSize && nextIndex != numRequired)
+ in.setstate(std::ios::failbit); // wrong number of values
+ }
+
+ return in;
+}
+
+
+
+//------------------------------------------------------------------------------
+// RELATED GLOBAL OPERATORS
+//------------------------------------------------------------------------------
+// These are logically part of the Matrix_<T> class but are not actually
+// class members; that is, they are in the SimTK namespace.
+
+/**@name Matrix_<T> serialization and I/O
+These methods are at namespace scope but are logically part of the Vector
+classes. These deal with reading and writing Vectors from and to streams,
+which places an additional requirement on the element type T: the element
+must support the same operation you are trying to do on the Vector as a
+whole. **/
+/**@{**/
+
+/** Specialize for VectorBase<E> to delegate to element type E, with spaces
+separating the elements.
+ at relates SimTK::VectorBase **/
+template <class E> inline void
+writeUnformatted(std::ostream& o, const VectorBase<E>& v) {
+ const int sz = v.size();
+ for (int i=0; i < sz; ++i) {
+ if (i != 0) o << " ";
+ writeUnformatted(o, v[i]);
+ }
+}
+/** Raw serialization of VectorView_<E>; same as VectorBase<E>.
+ at relates SimTK::VectorView_ **/
+template <class E> inline void
+writeUnformatted(std::ostream& o, const VectorView_<E>& v)
+{ writeUnformatted(o, static_cast< const VectorBase<E> >(v)); }
+
+/** Raw serialization of Vector_<E>; same as VectorBase<E>.
+ at relates SimTK::Vector_ **/
+template <class E> inline void
+writeUnformatted(std::ostream& o, const Vector_<E>& v)
+{ writeUnformatted(o, static_cast< const VectorBase<E> >(v)); }
+
+/** Specialize for RowVectorBase<E> to delegate to element type E, with spaces
+separating the elements; raw output is same as VectorBase.
+ at relates SimTK::RowVectorBase **/
+template <class E> inline void
+writeUnformatted(std::ostream& o, const RowVectorBase<E>& v)
+{ writeUnformatted(o, ~v); }
+
+/** Raw serialization of RowVectorView_<E>; same as VectorView_<E>.
+ at relates SimTK::RowVectorView_ **/
+template <class E> inline void
+writeUnformatted(std::ostream& o, const RowVectorView_<E>& v)
+{ writeUnformatted(o, static_cast< const RowVectorBase<E> >(v)); }
+
+/** Raw serialization of RowVector_<E>; same as Vector_<E>.
+ at relates SimTK::RowVector_ **/
+template <class E> inline void
+writeUnformatted(std::ostream& o, const RowVector_<E>& v)
+{ writeUnformatted(o, static_cast< const RowVectorBase<E> >(v)); }
+
+/** Specialize for MatrixBase<E> delegating to RowVectorBase<E> with newlines
+separating the rows, but no final newline.
+ at relates SimTK::MatrixBase **/
+template <class E> inline void
+writeUnformatted(std::ostream& o, const MatrixBase<E>& v) {
+ const int nr = v.nrow();
+ for (int i=0; i < nr; ++i) {
+ if (i != 0) o << std::endl;
+ writeUnformatted(o, v[i]);
+ }
+}
+/** Raw serialization of MatrixView_<E>; same as MatrixBase<E>.
+ at relates SimTK::MatrixView_ **/
+template <class E> inline void
+writeUnformatted(std::ostream& o, const MatrixView_<E>& v)
+{ writeUnformatted(o, static_cast< const MatrixBase<E> >(v)); }
+
+/** Raw serialization of Vector_<E>; same as VectorBase<E>.
+ at relates SimTK::Matrix_ **/
+template <class E> inline void
+writeUnformatted(std::ostream& o, const Matrix_<E>& v)
+{ writeUnformatted(o, static_cast< const MatrixBase<E> >(v)); }
+
+/** Read fixed-size VectorView from input stream. It is an error if there
+aren't enough elements.
+ at relates SimTK::VectorView_ **/
+template <class E> inline bool
+readUnformatted(std::istream& in, VectorView_<E>& v) {
+ for (int i=0; i < v.size(); ++i)
+ if (!readUnformatted(in, v[i])) return false;
+ return true;
+}
+
+/** Read variable-size Vector from input stream. Reads until error or eof.
+ at relates SimTK::Vector_ **/
+template <class E> inline bool
+readUnformatted(std::istream& in, Vector_<E>& v) {
+ if (!v.isResizeable())
+ return readUnformatted(in, v.updAsVectorView());
+
+ Array_<E,int> a;
+ if (!readUnformatted(in,a)) return false;
+ v.resize(a.size());
+ for (int i=0; i<a.size(); ++i)
+ v[i] = a[i];
+ return true;
+}
+
+/** Read fixed-size RowVectorView from input stream. It is an error if there
+aren't enough elements.
+ at relates SimTK::RowVectorView_ **/
+template <class E> inline bool
+readUnformatted(std::istream& in, RowVectorView_<E>& v)
+{ VectorView_<E> vt(~v);
+ return readUnformatted<E>(in, vt); }
+
+/** Read variable-size RowVector from unformatted (whitespace-separated)
+input stream. Reads until error or eof.
+ at relates SimTK::RowVector_ **/
+template <class E> inline bool
+readUnformatted(std::istream& in, RowVector_<E>& v)
+{ Vector_<E> vt(~v);
+ return readUnformatted<E>(in, vt); }
+
+/** Read fixed-size MatrixView in row order from unformatted (whitespace-
+separated) input stream. Newlines in the input have no special meaning --
+we'll read them as whitespace. It is an error if there aren't enough
+elements.
+ at relates SimTK::MatrixView_ **/
+template <class E> inline bool
+readUnformatted(std::istream& in, MatrixView_<E>& v) {
+ for (int row=0; row < v.nrow(); ++row) {
+ RowVectorView_<E> oneRow(v[row]);
+ if (!readUnformatted<E>(in, oneRow)) return false;
+ }
+ return true;
+}
+
+/** Read in new values for a Matrix without changing its size, from a stream
+of whitespace-separated tokens with no other formatting recognized. Newlines in
+the input have no special meaning -- we'll read them as whitespace. It is an
+error if there aren't enough elements.
+ at relates SimTK::Matrix_ **/
+template <class E> inline bool
+fillUnformatted(std::istream& in, Matrix_<E>& v) {
+ return readUnformatted<E>(in, v.updAsMatrixView());
+}
+
+/** NOT IMPLEMENTED: read variable-size Matrix recognizing newlines as end
+of row; use fillUnformatted() instead.
+ at relates SimTK::Matrix_ **/
+template <class E> inline bool
+readUnformatted(std::istream& in, Matrix_<E>& v) {
+ SimTK_ASSERT_ALWAYS(!"implemented",
+ "SimTK::readUnformatted(istream, Matrix) is not implemented; try"
+ " SimTK::fillUnformatted(istream, Matrix) instead.");
+ return false;
+}
+
+/** Output a human readable representation of a Vector to an std::ostream
+(like std::cout). The format is ~[ \e elements ] where \e elements is a
+space-separated list of the Vector's contents output by invoking the "<<"
+operator on the elements. This function will not compile if the element type
+does not support the "<<" operator. No newline is issued before
+or after the output. @relates Vector_ **/
+template <class T> inline std::ostream&
+operator<<(std::ostream& o, const VectorBase<T>& v)
+{ o << "~[";
+ if (v.size()) {
+ o << v[0];
+ for (int i=1; i < v.size(); ++i) o << " " << v[i];
+ }
+ return o << "]";
+}
+
+/** Output a human readable representation of a RowVector to an std::ostream
+(like std::cout). The format is [ \e elements ] where \e elements is a
+space-separated list of the RowVector's contents output by invoking the "<<"
+operator on the elements. This function will not compile if the element type
+does not support the "<<" operator. No newline is issued before
+or after the output. @relates RowVector_ **/
+template <class T> inline std::ostream&
+operator<<(std::ostream& o, const RowVectorBase<T>& v)
+{ o << "[";
+ if (v.size()) {
+ o << v[0];
+ for (int i=1; i < v.size(); ++i) o << " " << v[i];
+ }
+ return o << "]";
+}
+
+/** Output a human readable representation of a Matrix to an std::ostream
+(like std::cout). The format is one row per line, with each row output as
+[ \e elements ] where \e elements is a
+space-separated list of the row's contents output by invoking the "<<"
+operator on the elements. This function will not compile if the element type
+does not support the "<<" operator. A newline is issued before each row and
+at the end. @relates Matrix_ **/
+template <class T> inline std::ostream&
+operator<<(std::ostream& o, const MatrixBase<T>& m) {
+ for (int i=0;i<m.nrow();++i)
+ o << std::endl << m[i];
+ if (m.nrow()) o << std::endl;
+ return o;
+}
+
+
+/** Read in a Vector_<T> from a stream, as a sequence of space-separated or
+comma-separated values optionally surrounded by parentheses (), or square
+brackets [], or the "transposed" ~() or ~[]. In the case that the transpose
+operator is present, the parentheses or brackets are required, otherwise they
+are optional. We will continue to read elements of
+type T from the stream until we find a reason to stop, using type T's stream
+extraction operator>>() to read in each element and resizing the Vector as
+necessary. If the data is bracketed, we'll read until we hit the closing
+bracket. If it is not bracketed, we'll read until we hit eof() or get an error
+such as the element extractor setting the stream's fail bit due to bad
+formatting. On successful return, the stream will be positioned right after
+the final read-in element or terminating bracket, and the stream's status will
+be good() or eof(). We will not consume trailing whitespace after bracketed
+elements; that means the stream might actually be empty even if we don't
+return eof(). If you want to know whether there is anything else in the
+stream, follow this call with the STL whitespace skipper std::ws() like this:
+ at code
+ if (readVectorFromStream(in,vec) && !in.eof())
+ std::ws(in); // might take us to eof
+ if (in.fail()) {...} // probably a formatting error
+ else {
+ // Here if the stream is good() then there is more to read; if the
+ // stream got used up the status is guaranteed to be eof().
+ }
+ at endcode
+A compilation error will occur if you try to use this method on an Vector_<T>
+for a type T for which there is no stream extraction operator>>().
+ at note If you want to fill a resizeable Vector_<T> with a fixed amount of data
+from the stream, resize() the Vector to the appropriate length and then use
+fillVectorFromStream() instead. @see fillVectorFromStream()
+ at relates Vector_ **/
+template <class T> static inline
+std::istream& readVectorFromStream(std::istream& in, Vector_<T>& out)
+{ return readVectorFromStreamHelper<T>(in, false /*variable sizez*/, out); }
+
+
+
+/** Read in a fixed number of elements from a stream into a Vector. We expect
+to read in exactly size() elements of type T, using type T's stream extraction
+operator>>(). This will stop reading when we've read size() elements, or set
+the fail bit in the stream if we run out of elements or if any element's
+extract operator sets the fail bit. On successful return, all size() elements
+will have been set, the stream will be positioned right after the final
+read-in element or terminating bracket, and the stream's status will be good()
+or eof(). We will not consume trailing whitespace after reading all the
+elements; that means the stream might actually be empty even if we don't
+return eof(). If you want to know whether there is anything else in the
+stream, follow this call with std::ws() like this:
+ at code
+ if (fillVectorFromStream(in,vec))
+ if (!in.eof()) std::ws(in); // might take us to eof
+ if (in.fail()) {...} // deal with I/O or formatting error
+ // Here if the stream is good() then there is more to read; if the
+ // stream got used up the status is guaranteed to be eof().
+ at endcode
+A compilation error will occur if you try to use this method on a Vector_<T>
+for a type T for which there is no stream extraction operator>>().
+ at note If you want to read in a variable number of elements and have the
+Vector_<T> resized as needed, use readVectorFromStream() instead.
+ at see readVectorFromStream()
+ at relates Vector_ **/
+template <class T> static inline
+std::istream& fillVectorFromStream(std::istream& in, Vector_<T>& out)
+{ return readVectorFromStreamHelper<T>(in, true /*fixed size*/, out); }
+
+/** Read in a fixed number of elements from a stream into an VectorView. See
+fillVectorFromStream() for more information; this works the same way.
+ at see fillVectorFromStream()
+ at relates VectorView_ **/
+template <class T> static inline
+std::istream& fillVectorViewFromStream(std::istream& in, VectorView_<T>& out)
+{ return readVectorFromStreamHelper<T>(in, true /*fixed size*/, out); }
+
+
+/** Read Vector_<T> from a stream as a sequence of space- or comma-separated
+values of type T, optionally delimited by parentheses, or brackets, and
+preceded by "~". The Vector_<T> may be an owner (variable size) or a view
+(fixed size n). In the case of an owner, we'll read all the elements in
+brackets or until eof if there are no brackets. In the case of a view, there
+must be exactly n elements in brackets, or if there are no brackets we'll
+consume exactly n elements and then stop. Each element is read in with its
+own operator ">>" so this won't work if no such operator is defined for
+type T. @relates Vector_ **/
+template <class T> inline
+std::istream& operator>>(std::istream& in, Vector_<T>& out)
+{ return readVectorFromStream<T>(in, out); }
+
+/** Read a (fixed size n) VectorView_<T> from a stream as a sequence of space-
+or comma-separated values of type T, optionally delimited by parentheses or
+square brackets, and preceded by "~". If there are no delimiters then we will
+read size() values and then stop. Otherwise, there must be exactly size()
+values within the brackets. Each element is read in with its own
+operator ">>" so this won't work if no such operator is defined for type T.
+ at relates VectorView_ **/
+template <class T> inline
+std::istream& operator>>(std::istream& in, VectorView_<T>& out)
+{ return fillVectorViewFromStream<T>(in, out); }
+/**@}**/ // End of Matrix serialization.
+
+// Friendly abbreviations for vectors and matrices with scalar elements.
+/** @ingroup MatVecTypedefs **/
+/**@{**/
+/** Variable-size column vector of Real elements; abbreviation for
+Vector_<Real>. This is the most common large-vector type in the Simbody API
+and in Simbody user programs. **/
+typedef Vector_<Real> Vector;
+
+/** Variable-size 2D matrix of Real elements; abbreviation for Matrix_<Real>.
+This is the most common large-matrix type in the Simbody API and in Simbody
+user programs. **/
+typedef Matrix_<Real> Matrix;
+
+/** Variable-size row vector of Real elements; abbreviation for
+RowVector_<Real>. This is the type of a transposed Vector and does not
+usually appear explicitly in the Simbody API or user programs. **/
+typedef RowVector_<Real> RowVector;
+/**@}**/
+
+/** @ingroup UncommonMatVecTypedefs **/
+/**@{**/
+/** Variable-size column vector of Complex (std::complex<Real>) elements. **/
+typedef Vector_<Complex> ComplexVector;
+/** Variable-size 2D matrix of Complex (std::complex<Real>) elements. **/
+typedef Matrix_<Complex> ComplexMatrix;
+/** Variable-size row vector of Complex (std::complex<Real>) elements. **/
+typedef RowVector_<Complex> ComplexRowVector;
+
+/** Non-owner column vector sharing Real elements. **/
+typedef VectorView_<Real> VectorView;
+/** Non-owner matrix sharing Real elements. **/
+typedef MatrixView_<Real> MatrixView;
+/** Non-owner row vector sharing Real elements. **/
+typedef RowVectorView_<Real> RowVectorView;
+
+/** Non-owner column vector sharing Complex (std::complex<Real>) elements. **/
+typedef VectorView_<Complex> ComplexVectorView;
+/** Non-owner matrix sharing Complex (std::complex<Real>) elements. **/
+typedef MatrixView_<Complex> ComplexMatrixView;
+/** Non-owner row vector sharing Complex (std::complex<Real>) elements. **/
+typedef RowVectorView_<Complex> ComplexRowVectorView;
+
+/** Abbreviation for Vector_<float>. **/
+typedef Vector_<float> fVector;
+/** Abbreviation for Vector_<double>. **/
+typedef Vector_<double> dVector;
+/** Abbreviation for Vector_<std::complex<float>>. **/
+typedef Vector_<fComplex> fComplexVector;
+/** Abbreviation for Vector_<std::complex<double>>. **/
+typedef Vector_<dComplex> dComplexVector;
+
+/** Abbreviation for VectorView_<float>. **/
+typedef VectorView_<float> fVectorView;
+/** Abbreviation for VectorView_<double>. **/
+typedef VectorView_<double> dVectorView;
+/** Abbreviation for VectorView_<std::complex<float>>. **/
+typedef VectorView_<fComplex> fComplexVectorView;
+/** Abbreviation for VectorView_<std::complex<double>>. **/
+typedef VectorView_<dComplex> dComplexVectorView;
+
+/** Abbreviation for RowVector_<float>. **/
+typedef RowVector_<float> fRowVector;
+/** Abbreviation for RowVector_<double>. **/
+typedef RowVector_<double> dRowVector;
+/** Abbreviation for RowVector_<std::complex<float>>. **/
+typedef RowVector_<fComplex> fComplexRowVector;
+/** Abbreviation for RowVector_<std::complex<double>>. **/
+typedef RowVector_<dComplex> dComplexRowVector;
+
+/** Abbreviation for RowVectorView_<float>. **/
+typedef RowVectorView_<float> fRowVectorView;
+/** Abbreviation for RowVectorView_<double>. **/
+typedef RowVectorView_<double> dRowVectorView;
+/** Abbreviation for RowVectorView_<std::complex<float>>. **/
+typedef RowVectorView_<fComplex> fComplexRowVectorView;
+/** Abbreviation for RowVectorView_<std::complex<double>>. **/
+typedef RowVectorView_<dComplex> dComplexRowVectorView;
+
+/** Abbreviation for Matrix_<float>. **/
+typedef Matrix_<float> fMatrix;
+/** Abbreviation for Matrix_<double>. **/
+typedef Matrix_<double> dMatrix;
+/** Abbreviation for Matrix_<std::complex<float>>. **/
+typedef Matrix_<fComplex> fComplexMatrix;
+/** Abbreviation for Matrix_<std::complex<double>>. **/
+typedef Matrix_<dComplex> dComplexMatrix;
+
+/** Abbreviation for MatrixView_<float>. **/
+typedef MatrixView_<float> fMatrixView;
+/** Abbreviation for MatrixView_<double>. **/
+typedef MatrixView_<double> dMatrixView;
+/** Abbreviation for MatrixView_<std::complex<float>>. **/
+typedef MatrixView_<fComplex> fComplexMatrixView;
+/** Abbreviation for MatrixView_<std::complex<double>>. **/
+typedef MatrixView_<dComplex> dComplexMatrixView;
+/**@}**/
+
+} //namespace SimTK
+
+#endif //SimTK_SIMMATRIX_BIGMATRIX_H_
diff --git a/SimTKcommon/BigMatrix/include/SimTKcommon/internal/MatrixBase.h b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/MatrixBase.h
new file mode 100644
index 0000000..6db3629
--- /dev/null
+++ b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/MatrixBase.h
@@ -0,0 +1,901 @@
+#ifndef SimTK_SIMMATRIX_MATRIXBASE_H_
+#define SimTK_SIMMATRIX_MATRIXBASE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Define the SimTK::MatrixBase class that is part of Simbody's BigMatrix
+toolset. **/
+
+namespace SimTK {
+
+//==============================================================================
+// MATRIX BASE
+//==============================================================================
+/** @brief This is the common base class for Simbody's Vector_ and Matrix_
+classes for handling large, variable-sized vectors and matrices.
+
+%MatrixBase does not normally appear in user programs. Instead classes
+Vector_ and Matrix_ are used, or more commonly the typedefs Vector and Matrix
+which are abbreviations for Vector_<Real> and Matrix_<Real>.
+
+%Matrix base is a variable-size 2d matrix of Composite Numerical Type (CNT)
+elements. This is a container of such elements, it is NOT a Composite Numerical
+Type itself.
+
+<h2>Implementation</h2>
+MatrixBase<ELT> uses MatrixHelper<S> for implementation, where S
+is ELT::Scalar, that is, the underlying float, double, long double,
+complex<float>, negator<conjugate<double>>,
+etc. from which ELT is constructed. This is a finite set of which all
+members are explicitly instantiated in the implementation code, so
+clients don't have to know how anything is implemented.
+
+MatrixBase is the only class in the Matrix/Vector family which has any
+data members (it has exactly one MatrixHelper, which itself consists only
+of a single pointer to an opaque class). Thus all other objects
+in this family (that is, derived from MatrixBase) are exactly the same
+size in memory and may be "reinterpreted" as appropriate. For example,
+a Vector may be reinterpreted as a Matrix or vice versa, provided runtime
+requirements are met (e.g., exactly 1 column).
+
+Unlike the small matrix classes, very little is encoded in the type.
+Only the element type, and matrix vs. vector vs. row are in the type;
+everything else like shape, storage layout, and writability are handled
+at run time. **/
+// ----------------------------------------------------------------------------
+template <class ELT> class MatrixBase {
+public:
+ // These typedefs are handy, but despite appearances you cannot
+ // treat a MatrixBase as a composite numerical type. That is,
+ // CNT<MatrixBase> will not compile, or if it does it won't be
+ // meaningful.
+
+ typedef ELT E;
+ typedef typename CNT<E>::TNeg ENeg;
+ typedef typename CNT<E>::TWithoutNegator EWithoutNegator;
+ typedef typename CNT<E>::TReal EReal;
+ typedef typename CNT<E>::TImag EImag;
+ typedef typename CNT<E>::TComplex EComplex;
+ typedef typename CNT<E>::THerm EHerm;
+ typedef typename CNT<E>::TPosTrans EPosTrans;
+
+ typedef typename CNT<E>::TAbs EAbs;
+ typedef typename CNT<E>::TStandard EStandard;
+ typedef typename CNT<E>::TInvert EInvert;
+ typedef typename CNT<E>::TNormalize ENormalize;
+ typedef typename CNT<E>::TSqHermT ESqHermT;
+ typedef typename CNT<E>::TSqTHerm ESqTHerm;
+
+ typedef typename CNT<E>::Scalar EScalar;
+ typedef typename CNT<E>::Number ENumber;
+ typedef typename CNT<E>::StdNumber EStdNumber;
+ typedef typename CNT<E>::Precision EPrecision;
+ typedef typename CNT<E>::ScalarNormSq EScalarNormSq;
+
+ typedef EScalar Scalar; // the underlying Scalar type
+ typedef ENumber Number; // negator removed from Scalar
+ typedef EStdNumber StdNumber; // conjugate goes to complex
+ typedef EPrecision Precision; // complex removed from StdNumber
+ typedef EScalarNormSq ScalarNormSq; // type of scalar^2
+
+ typedef MatrixBase<E> T;
+ typedef MatrixBase<ENeg> TNeg;
+ typedef MatrixBase<EWithoutNegator> TWithoutNegator;
+ typedef MatrixBase<EReal> TReal;
+ typedef MatrixBase<EImag> TImag;
+ typedef MatrixBase<EComplex> TComplex;
+ typedef MatrixBase<EHerm> THerm;
+ typedef MatrixBase<E> TPosTrans;
+
+ typedef MatrixBase<EAbs> TAbs;
+ typedef MatrixBase<EStandard> TStandard;
+ typedef MatrixBase<EInvert> TInvert;
+ typedef MatrixBase<ENormalize> TNormalize;
+ typedef MatrixBase<ESqHermT> TSqHermT; // ~Mat*Mat
+ typedef MatrixBase<ESqTHerm> TSqTHerm; // Mat*~Mat
+
+ const MatrixCommitment& getCharacterCommitment() const {return helper.getCharacterCommitment();}
+ const MatrixCharacter& getMatrixCharacter() const {return helper.getMatrixCharacter();}
+
+ /// Change the handle commitment for this matrix handle; only allowed if the
+ /// handle is currently clear.
+ void commitTo(const MatrixCommitment& mc)
+ { helper.commitTo(mc); }
+
+ // This gives the resulting matrix type when (m(i,j) op P) is applied to each element.
+ // It will have element types which are the regular composite result of E op P.
+ template <class P> struct EltResult {
+ typedef MatrixBase<typename CNT<E>::template Result<P>::Mul> Mul;
+ typedef MatrixBase<typename CNT<E>::template Result<P>::Dvd> Dvd;
+ typedef MatrixBase<typename CNT<E>::template Result<P>::Add> Add;
+ typedef MatrixBase<typename CNT<E>::template Result<P>::Sub> Sub;
+ };
+
+ /// Return the number of rows m in the logical shape of this matrix.
+ int nrow() const {return helper.nrow();}
+ /// Return the number of columns n in the logical shape of this matrix.
+ int ncol() const {return helper.ncol();}
+
+ /// Return the number of elements in the \e logical shape of this matrix.
+ /// This has nothing to do with how many elements are actually stored;
+ /// it is simply the product of the logical number of rows and columns,
+ /// that is, nrow()*ncol(). Note that although each dimension is limited
+ /// to a 32 bit size, the product of those dimensions may be > 32 bits
+ /// on a 64 bit machine so the return type may be larger than that of
+ /// nrow() and ncol().
+ ptrdiff_t nelt() const {return helper.nelt();}
+
+ /// Return true if either dimension of this Matrix is resizable.
+ bool isResizeable() const {return getCharacterCommitment().isResizeable();}
+
+ enum {
+ NScalarsPerElement = CNT<E>::NActualScalars,
+ CppNScalarsPerElement = sizeof(E) / sizeof(Scalar)
+ };
+
+ /// The default constructor builds a 0x0 matrix managed by a helper that
+ /// understands how many scalars there are in one of our elements but is
+ /// otherwise uncommitted.
+ MatrixBase() : helper(NScalarsPerElement,CppNScalarsPerElement) {}
+
+ /// This constructor allocates the default matrix a completely uncommitted
+ /// matrix commitment, given particular initial dimensions.
+ MatrixBase(int m, int n)
+ : helper(NScalarsPerElement,CppNScalarsPerElement,MatrixCommitment(),m,n) {}
+
+ /// This constructor takes a handle commitment and allocates the default
+ /// matrix for that kind of commitment. If a dimension is set to a
+ /// particular (unchangeable) value in the commitment then the initial
+ /// allocation will use that value. Unlocked dimensions are given the
+ /// smallest value consistent with other committed attributes, typically 0.
+ explicit MatrixBase(const MatrixCommitment& commitment)
+ : helper(NScalarsPerElement,CppNScalarsPerElement,commitment) {}
+
+
+ /// This constructor takes a handle commitment and allocates the default
+ /// matrix for that kind of commitment given particular initial minimum
+ /// dimensions, which cannot be larger than those permitted by the
+ /// commitment.
+ MatrixBase(const MatrixCommitment& commitment, int m, int n)
+ : helper(NScalarsPerElement,CppNScalarsPerElement,commitment,m,n) {}
+
+ /// Copy constructor is a deep copy (not appropriate for views!).
+ MatrixBase(const MatrixBase& b)
+ : helper(b.helper.getCharacterCommitment(),
+ b.helper, typename MatrixHelper<Scalar>::DeepCopy()) { }
+
+ /// Implicit conversion from matrix with negated elements (otherwise this
+ /// is just like the copy constructor.
+ MatrixBase(const TNeg& b)
+ : helper(b.helper.getCharacterCommitment(),
+ b.helper, typename MatrixHelper<Scalar>::DeepCopy()) { }
+
+ /// Copy assignment is a deep copy but behavior depends on type of lhs: if
+ /// view, rhs must match. If owner, we reallocate and copy rhs.
+ MatrixBase& copyAssign(const MatrixBase& b) {
+ helper.copyAssign(b.helper);
+ return *this;
+ }
+ MatrixBase& operator=(const MatrixBase& b) { return copyAssign(b); }
+
+
+ /// View assignment is a shallow copy, meaning that we disconnect the MatrixBase
+ /// from whatever it used to refer to (destructing as necessary), then make it a new view
+ /// for the data descriptor referenced by the source.
+ /// CAUTION: we always take the source as const, but that is ignored in
+ /// determining whether the resulting view is writable. Instead, that is
+ /// inherited from the writability status of the source. We have to do this
+ /// in order to allow temporary view objects to be writable -- the compiler
+ /// creates temporaries like m(i,j,m,n) as const.
+ MatrixBase& viewAssign(const MatrixBase& src) {
+ helper.writableViewAssign(const_cast<MatrixHelper<Scalar>&>(src.helper));
+ return *this;
+ }
+
+ // default destructor
+
+ /// Initializing constructor with all of the initially-allocated elements
+ /// initialized to the same value. The given dimensions are treated as
+ /// minimum dimensions in case the commitment requires more. So it is
+ /// always permissible to set them both to 0 in which case you'll get
+ /// the smallest matrix that satisfies the commitment, with each of its
+ /// elements (if any) set to the given initial value.
+ MatrixBase(const MatrixCommitment& commitment, int m, int n, const ELT& initialValue)
+ : helper(NScalarsPerElement, CppNScalarsPerElement, commitment, m, n)
+ { helper.fillWith(reinterpret_cast<const Scalar*>(&initialValue)); }
+
+ /// Initializing constructor with the initially-allocated elements
+ /// initialized from a C++ array of elements, which is provided in
+ /// <i>row major</i> order. The given dimensions are treated as
+ /// minimum dimensions in case the commitment requires more. The
+ /// array is presumed to be long enough to supply a value for each
+ /// element. Note that C++ packing for elements may be different than
+ /// Simmatrix packing of the same elements (Simmatrix packs them
+ /// more tightly in some cases). So you should not use this constructor
+ /// to copy elements from one Simmatrix matrix to another; this is
+ /// exclusively for initializing a Simmatrix from a C++ array.
+ MatrixBase(const MatrixCommitment& commitment, int m, int n,
+ const ELT* cppInitialValuesByRow)
+ : helper(NScalarsPerElement, CppNScalarsPerElement, commitment, m, n)
+ { helper.copyInByRowsFromCpp(reinterpret_cast<const Scalar*>(cppInitialValuesByRow)); }
+
+ /// @name Matrix view of pre-exising data
+ ///
+ /// Non-resizeable view of someone else's already-allocated
+ /// memory of a size and storage type indicated by the supplied
+ /// MatrixCharacter. The \a spacing argument has different interpretations
+ /// depending on the storage format. Typically it is the leading
+ /// dimension for Lapack-style full storage or stride for a vector.
+ /// Spacing is in units like "number of scalars between elements" or
+ /// "number of scalars between columns" so it can be used to deal
+ /// with C++ packing vs. Simmatrix packing if necessary.
+ /// @{
+
+ /// Construct a read-only view of pre-existing data.
+ MatrixBase(const MatrixCommitment& commitment,
+ const MatrixCharacter& character,
+ int spacing, const Scalar* data) // read only data
+ : helper(NScalarsPerElement, CppNScalarsPerElement,
+ commitment, character, spacing, data) {}
+
+ /// Construct a writable view of pre-existing data.
+ MatrixBase(const MatrixCommitment& commitment,
+ const MatrixCharacter& character,
+ int spacing, Scalar* data) // writable data
+ : helper(NScalarsPerElement, CppNScalarsPerElement,
+ commitment, character, spacing, data) {}
+ /// @}
+
+ // Create a new MatrixBase from an existing helper. Both shallow and deep copies are possible.
+ MatrixBase(const MatrixCommitment& commitment,
+ MatrixHelper<Scalar>& source,
+ const typename MatrixHelper<Scalar>::ShallowCopy& shallow)
+ : helper(commitment, source, shallow) {}
+ MatrixBase(const MatrixCommitment& commitment,
+ const MatrixHelper<Scalar>& source,
+ const typename MatrixHelper<Scalar>::ShallowCopy& shallow)
+ : helper(commitment, source, shallow) {}
+ MatrixBase(const MatrixCommitment& commitment,
+ const MatrixHelper<Scalar>& source,
+ const typename MatrixHelper<Scalar>::DeepCopy& deep)
+ : helper(commitment, source, deep) {}
+
+ /// This restores the MatrixBase to the state it would be in had it
+ /// been constructed specifying only its handle commitment. The size will
+ /// have been reduced to the smallest size consistent with the commitment.
+ void clear() {helper.clear();}
+
+ MatrixBase& operator*=(const StdNumber& t) { helper.scaleBy(t); return *this; }
+ MatrixBase& operator/=(const StdNumber& t) { helper.scaleBy(StdNumber(1)/t); return *this; }
+ MatrixBase& operator+=(const MatrixBase& r) { helper.addIn(r.helper); return *this; }
+ MatrixBase& operator-=(const MatrixBase& r) { helper.subIn(r.helper); return *this; }
+
+ template <class EE> MatrixBase(const MatrixBase<EE>& b)
+ : helper(MatrixCommitment(),b.helper, typename MatrixHelper<Scalar>::DeepCopy()) { }
+
+ template <class EE> MatrixBase& operator=(const MatrixBase<EE>& b)
+ { helper = b.helper; return *this; }
+ template <class EE> MatrixBase& operator+=(const MatrixBase<EE>& b)
+ { helper.addIn(b.helper); return *this; }
+ template <class EE> MatrixBase& operator-=(const MatrixBase<EE>& b)
+ { helper.subIn(b.helper); return *this; }
+
+ /// Matrix assignment to an element sets only the *diagonal* elements to
+ /// the indicated value; everything else is set to zero. This is particularly
+ /// useful for setting a Matrix to zero or to the identity; for other values
+ /// it creates a Matrix which acts like the scalar. That is, if the scalar
+ /// is s and we do M=s, then multiplying another Matrix B by the resulting
+ /// diagonal matrix M gives the same result as multiplying B by s. That is
+ /// (M=s)*B == s*B.
+ ///
+ /// NOTE: this must be overridden for Vector and RowVector since then scalar
+ /// assignment is defined to copy the scalar to every element.
+ MatrixBase& operator=(const ELT& t) {
+ setToZero(); updDiag().setTo(t);
+ return *this;
+ }
+
+ /// Set M's diagonal elements to a "scalar" value S, and all off-diagonal
+ /// elements to zero. S can be any type which is assignable to an element
+ /// of type E. This is the same as the Matrix assignment operator M=S for
+ /// a scalar type S. It is overriden for Vector and Row types to behave
+ /// as elementwiseScalarAssign.
+ template <class S> inline MatrixBase&
+ scalarAssign(const S& s) {
+ setToZero(); updDiag().setTo(s);
+ return *this;
+ }
+
+ /// Add a scalar to M's diagonal. This is the same as the Matrix +=
+ /// operator. This is overridden for Vector and Row types to behave
+ /// as elementwiseAddScalarInPlace.
+ template <class S> inline MatrixBase&
+ scalarAddInPlace(const S& s) {
+ updDiag().elementwiseAddScalarInPlace(s);
+ }
+
+
+ /// Subtract a scalar from M's diagonal. This is the same as the Matrix -=
+ /// operator. This is overridden for Vector and Row types to behave
+ /// as elementwiseSubtractScalarInPlace.
+ template <class S> inline MatrixBase&
+ scalarSubtractInPlace(const S& s) {
+ updDiag().elementwiseSubtractScalarInPlace(s);
+ }
+
+ /// Set M(i,i) = S - M(i,i), M(i,j) = -M(i,j) for i!=j. This is overridden
+ /// for Vector and Row types to behave as elementwiseSubtractFromScalarInPlace.
+ template <class S> inline MatrixBase&
+ scalarSubtractFromLeftInPlace(const S& s) {
+ negateInPlace();
+ updDiag().elementwiseAddScalarInPlace(s); // yes, add
+ }
+
+ /// Set M(i,j) = M(i,j)*S for some "scalar" S. Actually S can be any
+ /// type for which E = E*S makes sense. That is, S must be conformant
+ /// with E and it must be possible to store the result back in an E.
+ /// This is the *= operator for M *= S and behaves the same way for
+ /// Matrix, Vector, and RowVector: every element gets multiplied in
+ /// place on the right by S.
+ template <class S> inline MatrixBase&
+ scalarMultiplyInPlace(const S&);
+
+ /// Set M(i,j) = S * M(i,j) for some "scalar" S. This is the same
+ /// as the above routine if S really is a scalar, but for S a more
+ /// complicated CNT it will be different.
+ template <class S> inline MatrixBase&
+ scalarMultiplyFromLeftInPlace(const S&);
+
+ /// Set M(i,j) = M(i,j)/S for some "scalar" S. Actually S can be any
+ /// type for which E = E/S makes sense. That is, S^-1 must be conformant
+ /// with E and it must be possible to store the result back in an E.
+ /// This is the /= operator for M /= S and behaves the same way for
+ /// Matrix, Vector, and RowVector: every element gets divided in
+ /// place on the right by S.
+ template <class S> inline MatrixBase&
+ scalarDivideInPlace(const S&);
+
+ /// Set M(i,j) = S/M(i,j) for some "scalar" S. Actually S can be any
+ /// type for which E = S/E makes sense. That is, S must be conformant
+ /// with E^-1 and it must be possible to store the result back in an E.
+ template <class S> inline MatrixBase&
+ scalarDivideFromLeftInPlace(const S&);
+
+
+ /// M = diag(r) * M; r must have nrow() elements.
+ /// That is, M[i] *= r[i].
+ template <class EE> inline MatrixBase&
+ rowScaleInPlace(const VectorBase<EE>&);
+
+ /// Return type is a new matrix which will have the same dimensions as 'this' but
+ /// will have element types appropriate for the elementwise multiply being performed.
+ template <class EE> inline void
+ rowScale(const VectorBase<EE>& r, typename EltResult<EE>::Mul& out) const;
+
+ template <class EE> inline typename EltResult<EE>::Mul
+ rowScale(const VectorBase<EE>& r) const {
+ typename EltResult<EE>::Mul out(nrow(), ncol()); rowScale(r,out); return out;
+ }
+
+ /// M = M * diag(c); c must have ncol() elements.
+ /// That is, M(j) *= c[j].
+ template <class EE> inline MatrixBase&
+ colScaleInPlace(const VectorBase<EE>&);
+
+ template <class EE> inline void
+ colScale(const VectorBase<EE>& c, typename EltResult<EE>::Mul& out) const;
+
+ template <class EE> inline typename EltResult<EE>::Mul
+ colScale(const VectorBase<EE>& c) const {
+ typename EltResult<EE>::Mul out(nrow(), ncol()); colScale(c,out); return out;
+ }
+
+
+ /// M = diag(r) * M * diag(c); r must have nrow() elements; must have ncol() elements.
+ /// That is, M(i,j) *= r[i]*c[j].
+ /// Having a combined row & column scaling operator means we can go through the matrix
+ /// memory once instead of twice.
+ template <class ER, class EC> inline MatrixBase&
+ rowAndColScaleInPlace(const VectorBase<ER>& r, const VectorBase<EC>& c);
+
+ template <class ER, class EC> inline void
+ rowAndColScale(const VectorBase<ER>& r, const VectorBase<EC>& c,
+ typename EltResult<typename VectorBase<ER>::template EltResult<EC>::Mul>::Mul& out) const;
+
+ template <class ER, class EC> inline typename EltResult<typename VectorBase<ER>::template EltResult<EC>::Mul>::Mul
+ rowAndColScale(const VectorBase<ER>& r, const VectorBase<EC>& c) const {
+ typename EltResult<typename VectorBase<ER>::template EltResult<EC>::Mul>::Mul
+ out(nrow(), ncol());
+ rowAndColScale(r,c,out); return out;
+ }
+
+ /// Set M(i,j)=s for every element of M and some value s. This requires only
+ /// that s be assignment compatible with M's elements; s doesn't
+ /// actually have to be a scalar. Note that for Matrix types this behavior
+ /// is different than scalar assignment, which puts the scalar only on M's
+ /// diagonal and sets the rest of M to zero. For Vector and RowVector types,
+ /// this operator is identical to the normal assignment operator and
+ /// scalarAssignInPlace() method which also assign the scalar to every element.
+ template <class S> inline MatrixBase&
+ elementwiseAssign(const S& s);
+
+ /// Overloaded to allow an integer argument, which is converted to Real.
+ MatrixBase& elementwiseAssign(int s)
+ { return elementwiseAssign<Real>(Real(s)); }
+
+ /// Set M(i,j) = M(i,j)^-1.
+ MatrixBase& elementwiseInvertInPlace();
+
+ void elementwiseInvert(MatrixBase<typename CNT<E>::TInvert>& out) const;
+
+ MatrixBase<typename CNT<E>::TInvert> elementwiseInvert() const {
+ MatrixBase<typename CNT<E>::TInvert> out(nrow(), ncol());
+ elementwiseInvert(out);
+ return out;
+ }
+
+ /// Set M(i,j)+=s for every element of M and some value s. This requires that s be
+ /// conformant with M's elements (of type E) and that the result can
+ /// be stored in an E. For Matrix types this behavior is different than
+ /// the normal += or scalarAddInPlace() operators, which add the scalar
+ /// only to the Matrix diagonal. For Vector and RowVector, this operator
+ /// is identical to += and scalarAddInPlace() which also add the scalar
+ /// to every element.
+ template <class S> inline MatrixBase&
+ elementwiseAddScalarInPlace(const S& s);
+
+ template <class S> inline void
+ elementwiseAddScalar(const S& s, typename EltResult<S>::Add&) const;
+
+ template <class S> inline typename EltResult<S>::Add
+ elementwiseAddScalar(const S& s) const {
+ typename EltResult<S>::Add out(nrow(), ncol());
+ elementwiseAddScalar(s,out);
+ return out;
+ }
+
+ /// Set M(i,j)-=s for every element of M and some value s. This requires that s be
+ /// conformant with M's elements (of type E) and that the result can
+ /// be stored in an E. For Matrix types this behavior is different than
+ /// the normal -= or scalarSubtractInPlace() operators, which subtract the scalar
+ /// only from the Matrix diagonal. For Vector and RowVector, this operator
+ /// is identical to -= and scalarSubtractInPlace() which also subtract the scalar
+ /// from every element.
+ template <class S> inline MatrixBase&
+ elementwiseSubtractScalarInPlace(const S& s);
+
+ template <class S> inline void
+ elementwiseSubtractScalar(const S& s, typename EltResult<S>::Sub&) const;
+
+ template <class S> inline typename EltResult<S>::Sub
+ elementwiseSubtractScalar(const S& s) const {
+ typename EltResult<S>::Sub out(nrow(), ncol());
+ elementwiseSubtractScalar(s,out);
+ return out;
+ }
+
+ /// Set M(i,j) = s - M(i,j) for every element of M and some value s. This requires that s be
+ /// conformant with M's elements (of type E) and that the result can
+ /// be stored in an E. For Matrix types this behavior is different than
+ /// the scalarSubtractFromLeftInPlace() operator, which subtracts only the diagonal
+ /// elements of M from s, while simply negating the off diagonal elements.
+ /// For Vector and RowVector, this operator
+ /// is identical to scalarSubtractFromLeftInPlace() which also subtracts every
+ /// element of M from the scalar.
+ template <class S> inline MatrixBase&
+ elementwiseSubtractFromScalarInPlace(const S& s);
+
+ template <class S> inline void
+ elementwiseSubtractFromScalar(
+ const S&,
+ typename MatrixBase<S>::template EltResult<E>::Sub&) const;
+
+ template <class S> inline typename MatrixBase<S>::template EltResult<E>::Sub
+ elementwiseSubtractFromScalar(const S& s) const {
+ typename MatrixBase<S>::template EltResult<E>::Sub out(nrow(), ncol());
+ elementwiseSubtractFromScalar<S>(s,out);
+ return out;
+ }
+
+ /// M(i,j) *= R(i,j); R must have same dimensions as this.
+ template <class EE> inline MatrixBase&
+ elementwiseMultiplyInPlace(const MatrixBase<EE>&);
+
+ template <class EE> inline void
+ elementwiseMultiply(const MatrixBase<EE>&, typename EltResult<EE>::Mul&) const;
+
+ template <class EE> inline typename EltResult<EE>::Mul
+ elementwiseMultiply(const MatrixBase<EE>& m) const {
+ typename EltResult<EE>::Mul out(nrow(), ncol());
+ elementwiseMultiply<EE>(m,out);
+ return out;
+ }
+
+ /// M(i,j) = R(i,j) * M(i,j); R must have same dimensions as this.
+ template <class EE> inline MatrixBase&
+ elementwiseMultiplyFromLeftInPlace(const MatrixBase<EE>&);
+
+ template <class EE> inline void
+ elementwiseMultiplyFromLeft(
+ const MatrixBase<EE>&,
+ typename MatrixBase<EE>::template EltResult<E>::Mul&) const;
+
+ template <class EE> inline typename MatrixBase<EE>::template EltResult<E>::Mul
+ elementwiseMultiplyFromLeft(const MatrixBase<EE>& m) const {
+ typename EltResult<EE>::Mul out(nrow(), ncol());
+ elementwiseMultiplyFromLeft<EE>(m,out);
+ return out;
+ }
+
+ /// M(i,j) /= R(i,j); R must have same dimensions as this.
+ template <class EE> inline MatrixBase&
+ elementwiseDivideInPlace(const MatrixBase<EE>&);
+
+ template <class EE> inline void
+ elementwiseDivide(const MatrixBase<EE>&, typename EltResult<EE>::Dvd&) const;
+
+ template <class EE> inline typename EltResult<EE>::Dvd
+ elementwiseDivide(const MatrixBase<EE>& m) const {
+ typename EltResult<EE>::Dvd out(nrow(), ncol());
+ elementwiseDivide<EE>(m,out);
+ return out;
+ }
+
+ /// M(i,j) = R(i,j) / M(i,j); R must have same dimensions as this.
+ template <class EE> inline MatrixBase&
+ elementwiseDivideFromLeftInPlace(const MatrixBase<EE>&);
+
+ template <class EE> inline void
+ elementwiseDivideFromLeft(
+ const MatrixBase<EE>&,
+ typename MatrixBase<EE>::template EltResult<E>::Dvd&) const;
+
+ template <class EE> inline typename MatrixBase<EE>::template EltResult<EE>::Dvd
+ elementwiseDivideFromLeft(const MatrixBase<EE>& m) const {
+ typename MatrixBase<EE>::template EltResult<E>::Dvd out(nrow(), ncol());
+ elementwiseDivideFromLeft<EE>(m,out);
+ return out;
+ }
+
+ /// Fill every element in current allocation with given element (or NaN or 0).
+ MatrixBase& setTo(const ELT& t) {helper.fillWith(reinterpret_cast<const Scalar*>(&t)); return *this;}
+ MatrixBase& setToNaN() {helper.fillWithScalar(CNT<StdNumber>::getNaN()); return *this;}
+ MatrixBase& setToZero() {helper.fillWithScalar(StdNumber(0)); return *this;}
+
+ // View creating operators.
+ inline RowVectorView_<ELT> row(int i) const; // select a row
+ inline RowVectorView_<ELT> updRow(int i);
+ inline VectorView_<ELT> col(int j) const; // select a column
+ inline VectorView_<ELT> updCol(int j);
+
+ RowVectorView_<ELT> operator[](int i) const {return row(i);}
+ RowVectorView_<ELT> operator[](int i) {return updRow(i);}
+ VectorView_<ELT> operator()(int j) const {return col(j);}
+ VectorView_<ELT> operator()(int j) {return updCol(j);}
+
+ // Select a block.
+ inline MatrixView_<ELT> block(int i, int j, int m, int n) const;
+ inline MatrixView_<ELT> updBlock(int i, int j, int m, int n);
+
+ MatrixView_<ELT> operator()(int i, int j, int m, int n) const
+ { return block(i,j,m,n); }
+ MatrixView_<ELT> operator()(int i, int j, int m, int n)
+ { return updBlock(i,j,m,n); }
+
+ // Hermitian transpose.
+ inline MatrixView_<EHerm> transpose() const;
+ inline MatrixView_<EHerm> updTranspose();
+
+ MatrixView_<EHerm> operator~() const {return transpose();}
+ MatrixView_<EHerm> operator~() {return updTranspose();}
+
+ /// Select main diagonal (of largest leading square if rectangular) and
+ /// return it as a read-only view of the diagonal elements of this Matrix.
+ inline VectorView_<ELT> diag() const;
+ /// Select main diagonal (of largest leading square if rectangular) and
+ /// return it as a writable view of the diagonal elements of this Matrix.
+ inline VectorView_<ELT> updDiag();
+ /// This non-const version of diag() is an alternate name for updDiag()
+ /// available for historical reasons.
+ VectorView_<ELT> diag() {return updDiag();}
+
+ // Create a view of the real or imaginary elements. TODO
+ //inline MatrixView_<EReal> real() const;
+ //inline MatrixView_<EReal> updReal();
+ //inline MatrixView_<EImag> imag() const;
+ //inline MatrixView_<EImag> updImag();
+
+ // Overload "real" and "imag" for both read and write as a nod to convention. TODO
+ //MatrixView_<EReal> real() {return updReal();}
+ //MatrixView_<EReal> imag() {return updImag();}
+
+ // TODO: this routine seems ill-advised but I need it for the IVM port at the moment
+ TInvert invert() const { // return a newly-allocated inverse; dump negator
+ TInvert m(*this);
+ m.helper.invertInPlace();
+ return m; // TODO - bad: makes an extra copy
+ }
+
+ void invertInPlace() {helper.invertInPlace();}
+
+ /// Matlab-compatible debug output.
+ void dump(const char* msg=0) const {
+ helper.dump(msg);
+ }
+
+
+ // This routine is useful for implementing friendlier Matrix expressions and operators.
+ // It maps closely to the Level-3 BLAS family of pxxmm() routines like sgemm(). The
+ // operation performed assumes that "this" is the result, and that "this" has
+ // already been sized correctly to receive the result. We'll compute
+ // this = beta*this + alpha*A*B
+ // If beta is 0 then "this" can be uninitialized. If alpha is 0 we promise not
+ // to look at A or B. The routine can work efficiently even if A and/or B are transposed
+ // by their views, so an expression like
+ // C += s * ~A * ~B
+ // can be performed with the single equivalent call
+ // C.matmul(1., s, Tr(A), Tr(B))
+ // where Tr(A) indicates a transposed view of the original A.
+ // The ultimate efficiency of this call depends on the data layout and views which
+ // are used for the three matrices.
+ // NOTE: neither A nor B can be the same matrix as 'this', nor views of the same data
+ // which would expose elements of 'this' that will be modified by this operation.
+ template <class ELT_A, class ELT_B>
+ MatrixBase& matmul(const StdNumber& beta, // applied to 'this'
+ const StdNumber& alpha, const MatrixBase<ELT_A>& A, const MatrixBase<ELT_B>& B)
+ {
+ helper.matmul(beta,alpha,A.helper,B.helper);
+ return *this;
+ }
+
+ /// Element selection for stored elements. These are the fastest element access
+ /// methods but may not be able to access all elements of the logical matrix when
+ /// some of its elements are not stored in memory. For example, a Hermitian matrix
+ /// stores only half its elements and other ones have to be calculated by conjugation
+ /// if they are to be returned as type ELT. (You can get them for free by recasting
+ /// the matrix so that the elements are reinterpreted as conjugates.) If you want
+ /// to guarantee that you can access the value of every element of a matrix, stored or not,
+ /// use getAnyElt() instead.
+ const ELT& getElt(int i, int j) const { return *reinterpret_cast<const ELT*>(helper.getElt(i,j)); }
+ ELT& updElt(int i, int j) { return *reinterpret_cast< ELT*>(helper.updElt(i,j)); }
+
+ const ELT& operator()(int i, int j) const {return getElt(i,j);}
+ ELT& operator()(int i, int j) {return updElt(i,j);}
+
+ /// This returns a copy of the element value for any position in the logical matrix,
+ /// regardless of whether it is stored in memory. If necessary the element's value
+ /// is calculated. This is much slower than getElt() but less restrictive.
+ /// @see getElt()
+ void getAnyElt(int i, int j, ELT& value) const
+ { helper.getAnyElt(i,j,reinterpret_cast<Scalar*>(&value)); }
+ ELT getAnyElt(int i, int j) const {ELT e; getAnyElt(i,j,e); return e;}
+
+ /// Scalar norm square is sum( squares of all scalars ). Note that this
+ /// is not very useful unless the elements are themselves scalars.
+ // TODO: very slow! Should be optimized at least for the case
+ // where ELT is a Scalar.
+ ScalarNormSq scalarNormSqr() const {
+ const int nr=nrow(), nc=ncol();
+ ScalarNormSq sum(0);
+ for(int j=0;j<nc;++j)
+ for (int i=0; i<nr; ++i)
+ sum += CNT<E>::scalarNormSqr((*this)(i,j));
+ return sum;
+ }
+
+ /// abs() is elementwise absolute value; that is, the return value has the same
+ /// dimension as this Matrix but with each element replaced by whatever it thinks
+ /// its absolute value is.
+ // TODO: very slow! Should be optimized at least for the case
+ // where ELT is a Scalar.
+ void abs(TAbs& mabs) const {
+ const int nr=nrow(), nc=ncol();
+ mabs.resize(nr,nc);
+ for(int j=0;j<nc;++j)
+ for (int i=0; i<nr; ++i)
+ mabs(i,j) = CNT<E>::abs((*this)(i,j));
+ }
+
+ /// abs() with the result as a function return. More convenient than the
+ /// other abs() member function, but may involve an additional copy of the
+ /// matrix.
+ TAbs abs() const { TAbs mabs; abs(mabs); return mabs; }
+
+ /// Return a Matrix of the same shape and contents as this one but
+ /// with the element type converted to one based on the standard
+ /// C++ scalar types: float, double, complex<float>,
+ /// complex<double>. That is, negator<>
+ /// and conjugate<> are eliminated from the element type by
+ /// performing any needed negations computationally.
+ /// Note that this is actually producing a new matrix with new data;
+ /// you can also do this for free by reinterpreting the current
+ /// matrix as a different type, if you don't mind looking at
+ /// shared data.
+ TStandard standardize() const {
+ const int nr=nrow(), nc=ncol();
+ TStandard mstd(nr, nc);
+ for(int j=0;j<nc;++j)
+ for (int i=0; i<nr; ++i)
+ mstd(i,j) = CNT<E>::standardize((*this)(i,j));
+ return mstd;
+ }
+
+ /// This is the scalar Frobenius norm, and its square. Note: if this is a
+ /// Matrix then the Frobenius norm is NOT the same as the 2-norm, although
+ /// they are equivalent for Vectors.
+ ScalarNormSq normSqr() const { return scalarNormSqr(); }
+ // TODO -- not good; unnecessary overflow
+ typename CNT<ScalarNormSq>::TSqrt
+ norm() const { return CNT<ScalarNormSq>::sqrt(scalarNormSqr()); }
+
+ /// We only allow RMS norm if the elements are scalars. If there are no
+ /// elements in this Matrix, we'll define its RMS norm to be 0, although
+ /// NaN might be a better choice.
+ typename CNT<ScalarNormSq>::TSqrt
+ normRMS() const {
+ if (!CNT<ELT>::IsScalar)
+ SimTK_THROW1(Exception::Cant, "normRMS() only defined for scalar elements");
+ if (nelt() == 0)
+ return typename CNT<ScalarNormSq>::TSqrt(0);
+ return CNT<ScalarNormSq>::sqrt(scalarNormSqr()/nelt());
+ }
+
+ /// Form the column sums of this matrix, returned as a RowVector.
+ RowVector_<ELT> colSum() const {
+ const int cols = ncol();
+ RowVector_<ELT> row(cols);
+ for (int j = 0; j < cols; ++j)
+ helper.colSum(j, reinterpret_cast<Scalar*>(&row[j]));
+ return row;
+ }
+ /// Alternate name for colSum(); behaves like the Matlab function sum().
+ RowVector_<ELT> sum() const {return colSum();}
+
+ /// Form the row sums of this matrix, returned as a Vector.
+ Vector_<ELT> rowSum() const {
+ const int rows = nrow();
+ Vector_<ELT> col(rows);
+ for (int i = 0; i < rows; ++i)
+ helper.rowSum(i, reinterpret_cast<Scalar*>(&col[i]));
+ return col;
+ }
+
+ //TODO: make unary +/- return a self-view so they won't reallocate?
+ const MatrixBase& operator+() const {return *this; }
+ const TNeg& negate() const {return *reinterpret_cast<const TNeg*>(this); }
+ TNeg& updNegate() {return *reinterpret_cast<TNeg*>(this); }
+
+ const TNeg& operator-() const {return negate();}
+ TNeg& operator-() {return updNegate();}
+
+ MatrixBase& negateInPlace() {(*this) *= EPrecision(-1); return *this;}
+
+ /// Change the size of this matrix. This is only allowed for owner matrices. The
+ /// current storage format is retained, but all the data is lost. If you want
+ /// to keep the old data, use resizeKeep().
+ /// @see resizeKeep()
+ MatrixBase& resize(int m, int n) { helper.resize(m,n); return *this; }
+ /// Change the size of this matrix, retaining as much of the old data as will
+ /// fit. This is only allowed for owner matrices. The
+ /// current storage format is retained, and the existing data is copied
+ /// into the new memory to the extent that it will fit.
+ /// @see resize()
+ MatrixBase& resizeKeep(int m, int n) { helper.resizeKeep(m,n); return *this; }
+
+ // This prevents shape changes in a Matrix that would otherwise allow it. No harm if is
+ // are called on a Matrix that is locked already; it always succeeds.
+ void lockShape() {helper.lockShape();}
+
+ // This allows shape changes again for a Matrix which was constructed to allow them
+ // but had them locked with the above routine. No harm if this is called on a Matrix
+ // that is already unlocked, but it is not allowed to call this on a Matrix which
+ // *never* allowed resizing. An exception will be thrown in that case.
+ void unlockShape() {helper.unlockShape();}
+
+ // An assortment of handy conversions
+ const MatrixView_<ELT>& getAsMatrixView() const { return *reinterpret_cast<const MatrixView_<ELT>*>(this); }
+ MatrixView_<ELT>& updAsMatrixView() { return *reinterpret_cast< MatrixView_<ELT>*>(this); }
+ const Matrix_<ELT>& getAsMatrix() const { return *reinterpret_cast<const Matrix_<ELT>*>(this); }
+ Matrix_<ELT>& updAsMatrix() { return *reinterpret_cast< Matrix_<ELT>*>(this); }
+
+ const VectorView_<ELT>& getAsVectorView() const
+ { assert(ncol()==1); return *reinterpret_cast<const VectorView_<ELT>*>(this); }
+ VectorView_<ELT>& updAsVectorView()
+ { assert(ncol()==1); return *reinterpret_cast< VectorView_<ELT>*>(this); }
+ const Vector_<ELT>& getAsVector() const
+ { assert(ncol()==1); return *reinterpret_cast<const Vector_<ELT>*>(this); }
+ Vector_<ELT>& updAsVector()
+ { assert(ncol()==1); return *reinterpret_cast< Vector_<ELT>*>(this); }
+ const VectorBase<ELT>& getAsVectorBase() const
+ { assert(ncol()==1); return *reinterpret_cast<const VectorBase<ELT>*>(this); }
+ VectorBase<ELT>& updAsVectorBase()
+ { assert(ncol()==1); return *reinterpret_cast< VectorBase<ELT>*>(this); }
+
+ const RowVectorView_<ELT>& getAsRowVectorView() const
+ { assert(nrow()==1); return *reinterpret_cast<const RowVectorView_<ELT>*>(this); }
+ RowVectorView_<ELT>& updAsRowVectorView()
+ { assert(nrow()==1); return *reinterpret_cast< RowVectorView_<ELT>*>(this); }
+ const RowVector_<ELT>& getAsRowVector() const
+ { assert(nrow()==1); return *reinterpret_cast<const RowVector_<ELT>*>(this); }
+ RowVector_<ELT>& updAsRowVector()
+ { assert(nrow()==1); return *reinterpret_cast< RowVector_<ELT>*>(this); }
+ const RowVectorBase<ELT>& getAsRowVectorBase() const
+ { assert(nrow()==1); return *reinterpret_cast<const RowVectorBase<ELT>*>(this); }
+ RowVectorBase<ELT>& updAsRowVectorBase()
+ { assert(nrow()==1); return *reinterpret_cast< RowVectorBase<ELT>*>(this); }
+
+ // Access to raw data. We have to return the raw data
+ // pointer as pointer-to-scalar because we may pack the elements tighter
+ // than a C++ array would.
+
+ /// This is the number of consecutive scalars used to represent one
+ /// element of type ELT. This may be fewer than C++ would use for the
+ /// element, since it may introduce some padding.
+ int getNScalarsPerElement() const {return NScalarsPerElement;}
+
+ /// This is like sizeof(ELT), but returning the number of bytes \e we use
+ /// to store the element which may be fewer than what C++ would use. We store
+ /// these packed elements adjacent to one another in memory.
+ int getPackedSizeofElement() const {return NScalarsPerElement*sizeof(Scalar);}
+
+ bool hasContiguousData() const {return helper.hasContiguousData();}
+ ptrdiff_t getContiguousScalarDataLength() const {
+ return helper.getContiguousDataLength();
+ }
+ const Scalar* getContiguousScalarData() const {
+ return helper.getContiguousData();
+ }
+ Scalar* updContiguousScalarData() {
+ return helper.updContiguousData();
+ }
+ void replaceContiguousScalarData(Scalar* newData, ptrdiff_t length, bool takeOwnership) {
+ helper.replaceContiguousData(newData,length,takeOwnership);
+ }
+ void replaceContiguousScalarData(const Scalar* newData, ptrdiff_t length) {
+ helper.replaceContiguousData(newData,length);
+ }
+ void swapOwnedContiguousScalarData(Scalar* newData, ptrdiff_t length, Scalar*& oldData) {
+ helper.swapOwnedContiguousData(newData,length,oldData);
+ }
+
+ /// Helper rep-stealing constructor. We take over ownership of this rep here. Note
+ /// that this \e defines the handle commitment for this handle. This is intended
+ /// for internal use only -- don't call this constructor unless you really
+ /// know what you're doing.
+ explicit MatrixBase(MatrixHelperRep<Scalar>* hrep) : helper(hrep) {}
+
+protected:
+ const MatrixHelper<Scalar>& getHelper() const {return helper;}
+ MatrixHelper<Scalar>& updHelper() {return helper;}
+
+private:
+ MatrixHelper<Scalar> helper; // this is just one pointer
+
+ template <class EE> friend class MatrixBase;
+};
+
+} //namespace SimTK
+
+#endif // SimTK_SIMMATRIX_MATRIXBASE_H_
diff --git a/SimTKcommon/BigMatrix/include/SimTKcommon/internal/MatrixCharacteristics.h b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/MatrixCharacteristics.h
new file mode 100644
index 0000000..00a7bf7
--- /dev/null
+++ b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/MatrixCharacteristics.h
@@ -0,0 +1,1100 @@
+#ifndef SimTK_SIMMATRIX_MATRIX_CHARACTERISTICS_H_
+#define SimTK_SIMMATRIX_MATRIX_CHARACTERISTICS_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ *
+ * Here we declare the classes needed for managing the properties of matrices,
+ * which we call Matrix Characteristics.
+ */
+
+#include "SimTKcommon/Scalar.h"
+
+#include <iostream>
+#include <cassert>
+#include <complex>
+#include <cstddef>
+#include <utility> // for std::pair
+
+namespace SimTK {
+
+
+class MatrixStructure;
+class MatrixStorage;
+class MatrixOutline;
+class MatrixCondition;
+class MatrixCharacter;
+class MatrixCommitment;
+
+
+// ------------------------------ MatrixStructure -----------------------------
+/// Matrix "structure" refers to an inherent mathematical (or at least
+/// algorithmic) characteristic of the matrix rather than a storage strategy.
+/// Symmetry is the clearest example of this; it is far more significant
+/// mathematically than just a way to save storage and reduce operation count.
+// ----------------------------------------------------------------------------
+class SimTK_SimTKCOMMON_EXPORT MatrixStructure {
+public:
+ enum Structure {
+ NoStructure = 0x00000000, ///< unspecified structure
+ Matrix1d = 0x00000001, ///< a 1-d matrix (could be row or column)
+ Zero = 0x00000002, ///< a matrix of all zeroes
+ Identity = 0x00000004, ///< diagonal matrix with repeated 1's
+ Permutation = 0x00000008, ///< permutation of an identity matrix
+ RepeatedDiagonal = 0x00000010, ///< diagonal matrix with repeated element
+ Diagonal = 0x00000020, ///< diagonal matrix with arbitrary elements
+ BiDiagonal = 0x00000040, ///< diagonal plus one upper or lower band
+ TriDiagonal = 0x00000080, ///< diagonal plus one upper and one lower band
+ BandedSymmetric = 0x00000100, ///< diagonal plus adjacent symmetric bands
+ BandedHermitian = 0x00000200, ///< diagonal plus adjacent conjugate bands
+ Banded = 0x00000400, ///< diagonal plus upper and lower bands
+ Triangular = 0x00000800, ///< diagonal plus all upper or all lower bands
+ QuasiTriangular = 0x00001000, ///< triangular but with 2x2 blocks on the diagonal
+ Hessenberg = 0x00002000, ///< triangular plus one band above/below diagonal
+ Symmetric = 0x00004000, ///< symmetric: elt(i,j)==elt(j,i)
+ Hermitian = 0x00008000, ///< hermitian: elt(i,j)==conjugate(elt(j,i))
+ SkewSymmetric = 0x00010000, ///< skew symmetric: elt(i,j) == -elt(j,i)
+ SkewHermitian = 0x00020000, ///< skew hermitian: elt(i,j) == -conjugate(elt(j,i))
+ Full = 0x00040000 ///< full mxn matrix, all elements distinct
+ };
+ static const char* name(Structure);
+
+ typedef unsigned int StructureMask; // 32 bits
+ static const StructureMask AnyStructure = 0x0007ffffU; // see above
+ static const StructureMask UncommittedStructure = 0xffffffffU;
+ static StructureMask calcStructureMask(Structure);
+
+ /// For triangular matrices, we have to know which triangle
+ /// we're talking about. Don't confuse this with MatrixStorage::Placement
+ /// which has to do with where we put it in memory, *not* what
+ /// matrix is being represented.
+ enum Position {
+ NoPosition = 0x0000,
+ Lower = 0x0001, // matrix is lower triangular (default)
+ Upper = 0x0002 // matrix is upper triangular
+ };
+ static const char* name(Position);
+
+ typedef unsigned short PositionMask; // 16 bits
+ static const PositionMask AnyPosition = 0x0003U; // see above
+ static const PositionMask UncommittedPosition = 0xffffU;
+ static PositionMask calcPositionMask(Structure);
+
+ /// For triangular, symmetric, and hermitian matrices the diagonal elements
+ /// may have a single, assumed value rather than being stored in memory.
+ /// This specifies the value. Don't confuse this with the similar
+ /// MatrixStorage type which simply says whether there is a known value
+ /// rather than stored values, *not* what that value is.
+ enum DiagValue {
+ NoDiagValue = 0x0000,
+ StoredDiag = 0x0001, // could be anything (default)
+ ZeroDiag = 0x0002, // zero (e.g. for skew matrices)
+ UnitDiag = 0x0004 // unit (one) diagonal is used frequently by Lapack
+ };
+ static const char* name(DiagValue);
+
+ typedef unsigned short DiagValueMask; // 16 bits
+ static const DiagValueMask AnyDiagValue = 0x0003U;
+ static const DiagValueMask UncommittedDiagValue = 0xffffU;
+ static DiagValueMask calcDiagValueMask(Structure);
+
+ MatrixStructure& setMissingAttributes() {
+ if (structure == NoStructure)
+ structure = Full;
+ if (position == NoPosition)
+ position = Lower;
+ if (diagValue == NoDiagValue)
+ diagValue = StoredDiag;
+ return *this;
+ }
+
+ std::string name() const {
+ return std::string(name(getStructure()))
+ + "|" + std::string(name(getPosition()))
+ + "|" + std::string(name(getDiagValue()));
+ }
+
+ struct Mask {
+ Mask() {setToUncommitted();}
+ Mask(StructureMask sm, PositionMask pm, DiagValueMask dm)
+ : structure(sm), position(pm), diagValue(dm) {}
+ Mask& setToUncommitted()
+ { structure=UncommittedStructure; position=UncommittedPosition;
+ diagValue=UncommittedDiagValue; return *this; }
+ bool isUncommitted() const
+ { return structure==UncommittedStructure && position==UncommittedPosition
+ && diagValue==UncommittedDiagValue; }
+ bool isSatisfiedBy(Structure str, Position pos, DiagValue diag) const
+ { return ((StructureMask)str&structure)==(StructureMask)str
+ && ((PositionMask)pos&position)==(PositionMask)pos
+ && ((DiagValueMask)diag&diagValue)==(DiagValueMask)diag; }
+ bool isSatisfiedBy(const MatrixStructure& actual) const
+ { return isSatisfiedBy(actual.getStructure(), actual.getPosition(),
+ actual.getDiagValue()); }
+
+ StructureMask structure;
+ PositionMask position;
+ DiagValueMask diagValue;
+ };
+
+ MatrixStructure() {setToNone();}
+
+ /// This constructor is also an implicit conversion from the Structure enum
+ /// to a MatrixStructure object which does not specify Position or DiagValue.
+ MatrixStructure(Structure s, Position p=NoPosition, DiagValue d=NoDiagValue)
+ : structure(s), position(p), diagValue(d) {}
+
+ /// Given a Structure commitment, which more-restrictive Structures will
+ /// still satisfy this commitment? Returned value is a mask with a bit
+ /// set for every Structure that is satisfactory. For example, if the
+ /// commitment is "Banded", "Diagonal" is also acceptable.
+ Mask mask() const;
+
+ Structure getStructure() const {return structure;}
+ Position getPosition() const {return position;}
+ DiagValue getDiagValue() const {return diagValue;}
+
+ MatrixStructure& setStructure(Structure s) {structure=s; return *this;}
+ MatrixStructure& setPosition (Position p) {position=p; return *this;}
+ MatrixStructure& setDiagValue(DiagValue d) {diagValue=d; return *this;}
+
+ MatrixStructure& set(Structure s, Position p, DiagValue d)
+ { structure=s; position=p; diagValue=d; return *this; }
+
+ MatrixStructure& setToNone()
+ { structure=NoStructure; position=NoPosition;
+ diagValue=NoDiagValue; return *this; }
+
+private:
+ Structure structure:32;
+ Position position:16;
+ DiagValue diagValue:16;
+};
+
+
+// ------------------------------ MatrixStorage -------------------------------
+/// Matrix "storage" refers to the physical layout of data in the computer�s
+/// memory. Whenever possible we attempt to store data in a format that enables
+/// use of special high performance methods, such as those available in the
+/// SimTK LAPACK/BLAS implementation.
+// ----------------------------------------------------------------------------
+class SimTK_SimTKCOMMON_EXPORT MatrixStorage {
+public:
+ enum Packing {
+ NoPacking = 0x0000,
+ Full = 0x0001, // full storage layout
+ TriInFull = 0x0002, // a triangular piece of a full storage layout
+ TriPacked = 0x0004, // triangle packed into minimal storage, at performance cost
+ Banded = 0x0008, // a packed, banded storage format
+ Vector = 0x0010, // a possibly-strided or scattered vector
+ Scalar = 0x0020, // a single scalar is stored
+ Permutation = 0x0040 // a permuted identity matrix
+ };
+ static const char* name(Packing);
+ typedef unsigned short PackingMask;
+ static const PackingMask AllPacking = 0x007fU; // see above
+ static const PackingMask UncommittedPacking = 0xffffU;
+
+ enum Placement {
+ NoPlacement = 0x0000,
+ Lower = 0x0001, // stored in lower triangle of full matrix
+ Upper = 0x0002, // stored in upper triangle of full matrix
+ };
+ static const char* name(Placement);
+ typedef unsigned short PlacementMask;
+ static const PlacementMask AllPlacement = 0x0003U; // see above
+ static const PlacementMask UncommittedPlacement = 0xffffU;
+
+ enum Order {
+ NoOrder = 0x0000,
+ ColumnOrder = 0x0001, // matrix is stored by columns
+ RowOrder = 0x0002, // matrix is stored by rows
+ };
+ static const char* name(Order);
+ typedef unsigned short OrderMask;
+ static const OrderMask AllOrder = 0x03U; // see above
+ static const OrderMask UncommittedOrder = 0xffU;
+
+ enum Diagonal {
+ NoDiag = 0x0000,
+ StoredDiag = 0x0001, // matrix diagonal is stored
+ AssumedDiag = 0x0002 // matrix diagonal is not stored but has known value
+ };
+ static const char* name(Diagonal);
+ typedef unsigned short DiagonalMask;
+ static const DiagonalMask AllDiagonal = 0x0003U; // see above
+ static const DiagonalMask UncommittedDiagonal = 0xffffU;
+
+ /// Use this class to represent sets of acceptable values for each of
+ /// the storage attributes (packing, position, order, diagonal).
+ struct Mask {
+ Mask()
+ : packing(UncommittedPacking), placement(UncommittedPlacement),
+ order(UncommittedOrder), diagonal(UncommittedDiagonal) {}
+ Mask(PackingMask pkm, PlacementMask plm, OrderMask om, DiagonalMask dm)
+ : packing(pkm), placement(plm), order(om), diagonal(dm) {}
+ Mask& setToUncommitted()
+ { packing=UncommittedPacking; placement=UncommittedPlacement;
+ order=UncommittedOrder; diagonal=UncommittedDiagonal; return *this; }
+ bool isUncommitted() const
+ { return packing==UncommittedPacking && placement==UncommittedPlacement
+ && order==UncommittedOrder && diagonal==UncommittedDiagonal; }
+ bool isSatisfiedBy(Packing pack, Placement place, Order ord, Diagonal diag) const
+ { return ((PackingMask)pack & packing) == (PackingMask) pack
+ && ((PlacementMask)place & placement) == (PlacementMask)place
+ && ((OrderMask)ord & order) == (OrderMask) ord
+ && ((DiagonalMask)diag & diagonal) == (DiagonalMask) diag; }
+ bool isSatisfiedBy(const MatrixStorage& actual) const
+ { return isSatisfiedBy(actual.getPacking(), actual.getPlacement(),
+ actual.getOrder(), actual.getDiagonal());}
+
+ PackingMask packing;
+ PlacementMask placement;
+ OrderMask order;
+ DiagonalMask diagonal;
+ };
+
+ static MatrixStorage calcDefaultStorage(const MatrixStructure&,
+ const MatrixOutline&);
+
+ std::string name() const {
+ return std::string(name(getPacking()))
+ + "|" + std::string(name(getPlacement()))
+ + "|" + std::string(name(getOrder()))
+ + "|" + std::string(name(getDiagonal()));
+ }
+
+ /// Calculate the commitment mask associated with specifying "this" set
+ /// of storage attributes as a commitment. Here the mask will either be
+ /// fully uncommitted or set to a specific value for each attribute; they
+ /// are all mutually exclusive.
+ Mask mask() const {
+ Mask ms; // initially uncommitted
+ if (packing) ms.packing = (PackingMask)packing;
+ if (placement) ms.placement = (PlacementMask)placement;
+ if (order) ms.order = (OrderMask)order;
+ if (diagonal) ms.diagonal = (DiagonalMask)diagonal;
+ return ms;
+ }
+
+ /// Default constructor leaves all fields unspecified.
+ MatrixStorage()
+ : packing(NoPacking), placement(NoPlacement), order(NoOrder), diagonal(NoDiag) {}
+
+ /// This constructor is also an implicit conversion from the Packing enum to a
+ /// MatrixStorage object which does not contain any specification for placement,
+ /// order, or storage of diagonal elements.
+ MatrixStorage(Packing pk, Placement pl=NoPlacement, Order o=NoOrder, Diagonal d=NoDiag)
+ : packing(pk), placement(pl), order(o), diagonal(d) {}
+
+ /// This constructor is for the common case of just packing and order, with
+ /// no particular placement and a stored diagonal.
+ MatrixStorage(Packing pk, Order o)
+ : packing(pk), placement(NoPlacement), order(o), diagonal(StoredDiag) {}
+
+ /// Assuming this is an actual matrix description, set any unspecified attributes
+ /// to appropriate defaults to match the specified packing.
+ MatrixStorage& setMissingAttributes() {
+ if (packing==NoPacking)
+ packing = Full;
+ if (placement==NoPlacement)
+ placement = Lower;
+ if (order==NoOrder)
+ order = ColumnOrder;
+ if (diagonal==NoDiag)
+ diagonal = StoredDiag;
+ return *this;
+ }
+
+ /// Restore this object to its default-constructed state of "none".
+ MatrixStorage& setToNone()
+ { packing=NoPacking; placement=NoPlacement;
+ order=NoOrder; diagonal=NoDiag; return *this; }
+
+ MatrixStorage& setPacking(Packing p) {packing = p; return *this;}
+ MatrixStorage& setPlacement(Placement p) {placement = p; return *this;}
+ MatrixStorage& setOrder(Order o) {order = o; return *this;}
+ MatrixStorage& setDiagonal(Diagonal d) {diagonal = d; return *this;}
+
+ Packing getPacking() const {return packing;}
+ Placement getPlacement() const {return placement;}
+ Order getOrder() const {return order;}
+ Diagonal getDiagonal() const {return diagonal;}
+
+private:
+ Packing packing:16;
+ Placement placement:16;
+ Order order:16;
+ Diagonal diagonal:16;
+};
+
+
+// ------------------------------- MatrixOutline ------------------------------
+/// Matrix "outline" refers to the characteristic relationship between the number
+/// of rows and columns of a matrix, without necessarily specifying the absolute
+/// dimensions.
+///
+/// There are seven possible outline attributes:
+/// - Rectangular (any m x n)
+/// - Tall (m >= n)
+/// - Wide (m <= n)
+/// - Square (m == n)
+/// - Column (m x 1)
+/// - Row (1 x n)
+/// - Scalar (1 x 1)
+///
+/// A matrix handle with no outline commitment can hold a general (rectangular)
+/// matrix, which of course includes all the other outlines as well. Vector
+/// handles are always committed to column outline, RowVector handles to row
+/// outline. Scalar matrices are also rows, columns, and square and some
+/// rows and columns are square (if they are 1x1).
+///
+/// Note that certain outlines imply fixed sizes of one or both dimensions.
+// ----------------------------------------------------------------------------
+class SimTK_SimTKCOMMON_EXPORT MatrixOutline {
+public:
+ enum Outline {
+ NoOutline = 0x0000,
+ Scalar = 0x0001, // 1x1
+ Column = 0x0002, // mx1, m != 1
+ Row = 0x0004, // 1xn, n != 1
+ Square = 0x0008, // mxn, m == n
+ Wide = 0x0010, // mxn, m < n
+ Tall = 0x0020, // mxn, m > n
+ Rectangular = 0x0040 // mxn
+ };
+ static const char* name(Outline);
+
+ typedef unsigned short OutlineMask;
+ static const OutlineMask AnyOutline = 0x007fU; // see above
+ static const OutlineMask UncommittedOutline = 0xffffU;
+
+ struct Mask {
+ Mask() : outline(UncommittedOutline) {}
+ explicit Mask(OutlineMask mask) : outline(mask) {}
+ Mask& setToUncommitted() {outline=UncommittedOutline; return *this;}
+ bool isUncommitted() const {return outline==UncommittedOutline;}
+ bool isSatisfiedBy(const MatrixOutline& actual) const
+ { return ((OutlineMask)actual.outline & outline) == (OutlineMask)actual.outline; }
+
+ OutlineMask outline;
+ };
+
+ std::string name() const {return std::string(name(getOutline()));}
+
+ /// Default constructor produces an object containing no outline specification.
+ /// If used as a commitment it won't accept \e any outline!
+ MatrixOutline() : outline(NoOutline) {}
+
+ /// This is an implicit conversion from the Outline enum to a MatrixOutline object.
+ MatrixOutline(Outline outline) : outline(outline) {}
+
+ /// Set the outline back to its default-constructed value of "none".
+ MatrixOutline& setToNone() {outline=NoOutline; return *this;}
+
+ /// Compute a mask of acceptable Outline values given a particular
+ /// value specified as a commitment. For example, if the commitment
+ /// is "Wide" then Square, Row, and Scalar outlines are also acceptable.
+ static OutlineMask calcMask(Outline);
+
+ /// When "this" outline is used as a commitment, it represents a mask of
+ /// acceptable outlines. Calculate and return that mask.
+ Mask mask() const {return Mask(calcMask(getOutline()));}
+
+ /// Determine if the proposed shape satisfies this outline.
+ bool isSizeOK(int m, int n) const;
+
+ /// Return the minimum shape that will satisfy this outline.
+ void getMinimumSize(int& m, int& n) const;
+
+ /// Determine the outline from given actual dimensions.
+ static MatrixOutline calcFromSize(int m, int n);
+
+ /// Return the outline value stored in this MatrixOutline object.
+ Outline getOutline() const {return outline;}
+
+private:
+ Outline outline:16;
+};
+
+
+
+// ---------------------------- MatrixCondition -------------------------------
+/// Matrix "condition" is a statement about the numerical characteristics of a
+/// Matrix. It can be set as a result of an operation, or by a knowledgeable user.
+/// Simmatrix is entitled to rely on the correctness of these assertions,
+/// although it will try to check them if requested or when it is possible to do
+/// so without sacrificing performance.
+/// In most cases the condition will not be set at all, which we interpret to
+/// mean "unknown condition" in an actual character, and "uncommitted" in
+/// a character commitment.
+// ----------------------------------------------------------------------------
+class SimTK_SimTKCOMMON_EXPORT MatrixCondition {
+public:
+ enum Condition {
+ UnknownCondition = 0x0000,
+ Orthogonal = 0x0001, // implies well conditioned
+ PositiveDefinite = 0x0002, // implies well conditioned
+ WellConditioned = 0x0004, // implies full rank
+ FullRank = 0x0008, // but might have bad conditioning
+ Singular = 0x0010 // implies possible bad conditioning
+ };
+ static const char* name(Condition);
+
+ typedef unsigned short ConditionMask; // 16 bits in mask
+ static const ConditionMask AnyCondition = 0x001fU; // see above
+ static const ConditionMask UncommittedCondition = 0xffffU;
+
+ enum Diagonal {
+ UnknownDiagonal = 0x0000, ///< we don't know the condition of the diagonal elements
+ ZeroDiagonal = 0x0001, ///< all diagonal elements are zero (also pure real and pure imaginary)
+ OneDiagonal = 0x0002, ///< all diagonal elements are one (and real)
+ RealDiagonal = 0x0004, ///< all diagonal elements are pure real
+ ImaginaryDiagonal = 0x0008 ///< all diagonal elements are pure imaginary
+ };
+ static const char* name(Diagonal);
+
+ typedef unsigned short DiagonalMask; // 16 bits in mask
+ static const DiagonalMask AnyDiagonal = 0x000fU; // see above
+ static const DiagonalMask UncommittedDiagonal = 0xffffU;
+
+ /// Use this class to represent a set of acceptable Condition values.
+ struct Mask {
+ Mask() : condition(UncommittedCondition), diagonal(UncommittedDiagonal) {}
+ Mask(ConditionMask cmask, DiagonalMask dmask) : condition(cmask), diagonal(dmask) {}
+ Mask& setToUncommitted()
+ { condition=UncommittedCondition; diagonal=UncommittedDiagonal; return *this;}
+ bool isUncommitted() const
+ { return condition==UncommittedCondition && diagonal==UncommittedDiagonal;}
+ bool isSatisfiedBy(const MatrixCondition& actual) const
+ { return ((ConditionMask)actual.condition & condition) == (ConditionMask)actual.condition
+ && ((DiagonalMask) actual.diagonal & diagonal) == (DiagonalMask)actual.diagonal; }
+
+ ConditionMask condition;
+ DiagonalMask diagonal;
+ };
+
+ std::string name() const
+ { return std::string(name(getCondition())) + "|" + std::string(name(getDiagonal()));}
+
+ /// The default constructor sets the condition to Unknown, which is typically
+ /// where it remains.
+ MatrixCondition() : condition(UnknownCondition), diagonal(UnknownDiagonal) {}
+
+ /// This is an implicit conversion from the Condition enum to a
+ /// MatrixCondition object.
+ MatrixCondition(Condition cond, Diagonal diag=UnknownDiagonal) : condition(cond) {}
+
+ /// Restore to default-constructed state of "none".
+ MatrixCondition& setToNone() {condition=UnknownCondition; diagonal=UnknownDiagonal; return *this;}
+
+ /// Given a particular Condition provided as a commitment, calculate
+ /// the mask of all Condition values that would satisfy that commitment.
+ /// For example, if the commitment is "WellConditioned" then "Orthogonal"
+ /// and "PositiveDefinite" also qualify since they are automatically
+ /// well conditioned. If the Condition is "Unknown" we'll return an Uncommitted mask.
+ static ConditionMask calcMask(Condition);
+
+ /// Given a particular Diagonal condition provided as a commitment, calculate
+ /// the mask of all Diagonal conditions taht would satisfy that commitment.
+ /// For example, if the commitment is "RealDiagonal" then "ZeroDiagonal"
+ /// and "OneDiagonal" would also be acceptable. If the Diagonal condition
+ /// is specified as "UnknownDiagonal" then we'll return an Uncommitted mask.
+ static DiagonalMask calcMask(Diagonal);
+
+ /// Return the commitment mask corresponding to use of "this" condition
+ /// as a commitment.
+ Mask mask() const
+ { return Mask(calcMask(getCondition()), calcMask(getDiagonal())); }
+
+ Condition getCondition() const {return condition;}
+ Diagonal getDiagonal() const {return diagonal;}
+
+ MatrixCondition& setCondition(Condition c) {condition=c; return *this;}
+ MatrixCondition& setDiagonal (Diagonal d) {diagonal=d; return *this;}
+
+private:
+ Condition condition:16;
+ Diagonal diagonal:16;
+};
+
+
+
+// ------------------------------ MatrixCharacter -----------------------------
+/** A MatrixCharacter is a set containing a value for each of the matrix
+characteristics except element type, which is part of the templatized
+declaration of a Matrix_, Vector_, or RowVector_ handle. MatrixCharacters are
+used both as the handle "commitment", setting restrictions on what kinds
+of matrices a handle can reference, and as the "facts on the ground" current
+character of the matrix being referenced. The current character must always
+satisfy the character commitment.
+
+%Matrix characteristics are specifications of particular aspects of matrices:
+ - Element type
+ - Size
+ - Structure
+ - Storage format
+ - Outline
+ - Conditioning
+
+Collectively, the set of values for the above properties is called a <i>matrix
+character</i>. A matrix character can be used to describe an existing matrix,
+or a character mask can be used to describe the range of characteristics that
+a matrix handle may support. The character mask describing the acceptable
+matrices for a matrix handle is called the handle's <i>character commitment</i>
+or just the <i>handle commitment</i>. The character describing an existing
+matrix is called the <i>actual character</i> of that matrix. Thus there are
+always two sets of characteristics associated with a matrix: the handle's
+commitment, and the actual character of the matrix to which the handle currently
+refers. The actual character must always \e satisfy the character commitment.
+
+When a handle presents a view into another handle's data, it is the
+characteristics of the matrix as seen through the view that must satisfy the
+handle's character commitment. So for example, a view showing one column of a
+full matrix satisfies a "column" outline commitment.
+
+Element type for a matrix handle is always determined at compile time via the
+template argument used in the declaration. For example, a matrix handle declared
+\c Matrix_<Vec3> can only hold matrices whose elements are \c Vec3s. Also,
+recall that \c Matrix is an abbreviation for \c Matrix_<Real> so that
+declaration commits the matrix handle to Real-element matrices. Element type
+is the only matrix characteristic for which no matrix handle can remain
+uncommitted. However, different handles can provide views of the same data
+through which that data is seen to contain different element types.
+
+Each matrix characteristic other than sizes is represented by a class
+defining one or more enumerated types, where individual characteristics are
+assigned a single bit. Then an appropriate mask type (an unsigned integral
+type) is defined which can represent a set of allowable characteristics.
+The actual character of a matrix is represented via enumeration values; the
+character commitment is represented by the compatible masks. The operation
+of determining whether a particular actual character satisfies a handle
+commitment can then be performed very quickly via bitwise logical operations.
+**/
+class SimTK_SimTKCOMMON_EXPORT MatrixCharacter {
+public:
+ /// Default constructor sets lengths to zero and the other characteristics
+ /// to "none specified".
+ MatrixCharacter() : nr(0), nc(0), lband(0), uband(0) {}
+
+ // Some handy predefined MatrixCharacters.
+ class LapackFull;
+ class Vector;
+ class RowVector;
+
+ /// Restore this MatrixCharacter to its default-constructed state of "none".
+ MatrixCharacter& setToNone() {
+ nr=nc=lband=uband=0;
+ structure.setToNone(); outline.setToNone();
+ storage.setToNone(); condition.setToNone();
+ return *this;
+ }
+
+ /// These are dimensions of the logical matrix and have nothing to do with
+ /// how much storage may be used to hold the elements.
+ int nrow() const {return nr;}
+ int ncol() const {return nc;}
+ std::pair<int,int> getSize() const {return std::pair<int,int>(nrow(),ncol());}
+ ptrdiff_t nelt() const {return (ptrdiff_t)nrow() * (ptrdiff_t)ncol();}
+
+ int getLowerBandwidth() const {return lband;}
+ int getUpperBandwidth() const {return uband;}
+ std::pair<int,int> getBandwidth() const
+ { return std::pair<int,int>(getLowerBandwidth(), getUpperBandwidth()); }
+
+ const MatrixStructure& getStructure() const {return structure;}
+ const MatrixStorage& getStorage() const {return storage;}
+ const MatrixOutline& getOutline() const {return outline;}
+ const MatrixCondition& getCondition() const {return condition;}
+
+ MatrixStructure& updStructure() {return structure;}
+ MatrixStorage& updStorage() {return storage;}
+ MatrixOutline& updOutline() {return outline;}
+ MatrixCondition& updCondition() {return condition;}
+
+ MatrixCharacter& setStructure(const MatrixStructure& sa) {structure = sa; return *this;}
+ MatrixCharacter& setStorage (const MatrixStorage& sa) {storage = sa; return *this;}
+ MatrixCharacter& setOutline (const MatrixOutline& oa) {outline = oa; return *this;}
+ MatrixCharacter& setCondition(const MatrixCondition& ca) {condition = ca; return *this;}
+
+
+ /// Set the actual size and update the outline to match.
+ MatrixCharacter& setActualSize(int m, int n)
+ { setSize(m,n); outline = MatrixOutline::calcFromSize(m,n); return *this; }
+ MatrixCharacter& setActualNumRows(int m)
+ { setNumRows(m); outline = MatrixOutline::calcFromSize(m,ncol()); return *this; }
+ MatrixCharacter& setActualNumCols(int n)
+ { setNumCols(n); outline = MatrixOutline::calcFromSize(nrow(),n); return *this; }
+
+ MatrixCharacter& setBandwidth(int lb, int ub) {
+ assert(lb>=0 && lb>=0);
+ lband = lb; uband = ub;
+ return *this;
+ }
+ MatrixCharacter& setLowerBandwidth(int lb) {
+ assert(lb>=0);
+ lband = lb;
+ return *this;
+ }
+ MatrixCharacter& setUpperBandwidth(int ub) {
+ assert(ub>=0);
+ uband = ub;
+ return *this;
+ }
+
+ class Mask; // defined below
+
+protected:
+ MatrixCharacter(int m, int n,
+ int lb, int ub,
+ MatrixStructure structure,
+ MatrixStorage storage,
+ MatrixCondition condition)
+ : nr(m), nc(n), lband(lb), uband(ub),
+ structure(structure), storage(storage),
+ outline(MatrixOutline::calcFromSize(m,n)),
+ condition(condition) {}
+
+
+ int nr, ///< actual number of rows
+ nc; ///< actual number of columns
+ int lband, ///< actual lower bandwidth, if banded
+ uband; ///< actual upper bandwidth, if banded
+ MatrixStructure structure;
+ MatrixStorage storage;
+ MatrixOutline outline;
+ MatrixCondition condition;
+
+private:
+ // These are private because they don't set the outline as well.
+ MatrixCharacter& setSize(int m, int n)
+ { assert(m>=0 && n>=0); nr = m; nc = n; return *this; }
+ MatrixCharacter& setNumRows(int m)
+ { assert(m>=0); nr = m; return *this; }
+ MatrixCharacter& setNumCols(int n)
+ { assert(n>=0); nc = n; return *this; }
+};
+
+/// Output a textual description of a MatrixCharacter; handy for debugging.
+/// @relates SimTK::MatrixCharacter
+SimTK_SimTKCOMMON_EXPORT std::ostream&
+operator<<(std::ostream& o, const MatrixCharacter&);
+
+/// Predefined MatrixCharacter for an ordinary Lapack-style full matrix
+/// of a particular dimension m x n (nrows X ncols). Note that the storage
+/// format allows for a "leading dimension" larger than m, but that the
+/// leading dimension is not considered part of the matrix character. It is
+/// dealt with separately.
+class MatrixCharacter::LapackFull : public MatrixCharacter {
+public:
+ LapackFull(int m, int n)
+ : MatrixCharacter(m,n,0,0,
+ MatrixStructure(MatrixStructure::Full),
+ MatrixStorage(MatrixStorage::Full,MatrixStorage::ColumnOrder),
+ MatrixCondition()) {}
+};
+
+/// Predefined MatrixCharacter for an ordinary column vector of a particular
+/// size. Note that standard vector storage allows for a "stride" greater than
+/// 1, but that is not considered part of the matrix character. Stride is
+/// dealt with separately.
+class MatrixCharacter::Vector : public MatrixCharacter {
+public:
+ Vector(int m)
+ : MatrixCharacter(m,1,0,0,
+ MatrixStructure(MatrixStructure::Matrix1d),
+ MatrixStorage(MatrixStorage::Vector,MatrixStorage::ColumnOrder),
+ MatrixCondition()) {}
+};
+
+/// Predefined MatrixCharacter for an ordinary row vector of a particular
+/// size. Note that standard vector storage allows for a "stride" greater than
+/// 1, but that is not considered part of the matrix character. Stride is
+/// dealt with separately.
+class MatrixCharacter::RowVector : public MatrixCharacter {
+public:
+ RowVector(int n)
+ : MatrixCharacter(1,n,0,0,
+ MatrixStructure(MatrixStructure::Matrix1d),
+ MatrixStorage(MatrixStorage::Vector,MatrixStorage::RowOrder),
+ MatrixCondition()) {}
+};
+
+// -------------------------- MatrixCharacter::Mask ---------------------------
+/// This class collects masks of each characteristic type for representing sets
+/// of accceptable characteristics.
+// ----------------------------------------------------------------------------
+class MatrixCharacter::Mask {
+public:
+ Mask() {setToUncommitted();}
+
+ typedef unsigned int SizeMask;
+ static const SizeMask SizeUncommitted = 0xffffffffU;
+
+ bool isResizeable() const {return nr==SizeUncommitted || nc==SizeUncommitted;}
+ bool isFullyResizeable() const {return nr==SizeUncommitted && nc==SizeUncommitted;}
+ bool isNumRowsLocked() const {return nr!=SizeUncommitted;}
+ bool isNumColsLocked() const {return nc!=SizeUncommitted;}
+
+ unsigned int getNumRowsMask() const {return nr;}
+ unsigned int getNumColsMask() const {return nc;}
+ unsigned int getLowerBandwidthMask() const {return lband;}
+ unsigned int getUpperBandwidthMask() const {return uband;}
+
+ int getDefaultNumRows() const {return isNumRowsLocked() ? nr : 0;}
+ int getDefaultNumCols() const {return isNumColsLocked() ? nc : 0;}
+
+ bool isLowerBandwidthLocked() const {return lband!=SizeUncommitted;}
+ bool isUpperBandwidthLocked() const {return uband!=SizeUncommitted;}
+ int getDefaultLowerBandwidth() const {return isLowerBandwidthLocked() ? lband : 0;}
+ int getDefaultUpperBandwidth() const {return isUpperBandwidthLocked() ? uband : 0;}
+
+ /// Set all bits to one ("Uncommitted").
+ Mask& setToUncommitted() {
+ nr=nc=lband=uband=SizeUncommitted;
+ structure.setToUncommitted(); storage.setToUncommitted();
+ outline.setToUncommitted(); condition.setToUncommitted();
+ return *this;
+ }
+
+ /// Return if all fields are set to "Uncommitted" (all bits are one).
+ bool isUncommitted() const {
+ return nr==SizeUncommitted && nc==SizeUncommitted
+ && lband==SizeUncommitted && uband==SizeUncommitted
+ && structure.isUncommitted() && storage.isUncommitted()
+ && outline.isUncommitted() && condition.isUncommitted();
+ }
+
+ /// Check whether an actual matrix character satisfies this matrix commitment.
+ bool isSatisfiedBy(const MatrixCharacter& actual) const {
+ return isSizeOK(actual.nr, actual.nc)
+ && isBandwidthOK(actual.lband, actual.uband)
+ && structure.isSatisfiedBy(actual.getStructure())
+ && storage.isSatisfiedBy(actual.getStorage())
+ && outline.isSatisfiedBy(actual.getOutline())
+ && condition.isSatisfiedBy(actual.getCondition());
+ }
+
+ /// Check whether an actual size satisfies the size commitment.
+ bool isSizeOK(int m, int n) const
+ { return ((SizeMask)m & nr) == (SizeMask)m
+ && ((SizeMask)n & nc) == (SizeMask)n; }
+
+ /// Check whether an actual bandwidth satisfies the bandwidth commitment.
+ /// (If the matrix isn't banded any bandwidth will be OK.)
+ bool isBandwidthOK(int lower, int upper) const
+ { return ((SizeMask)lower & lband) == (SizeMask)lower
+ && ((SizeMask)upper & uband) == (SizeMask)upper; }
+
+ SizeMask nr, ///< number of rows
+ nc; ///< number of columns
+ SizeMask lband, ///< lower bandwidth, if banded
+ uband; ///< upper bandwidth, if banded
+ MatrixStructure::Mask structure;
+ MatrixStorage::Mask storage;
+ MatrixOutline::Mask outline;
+ MatrixCondition::Mask condition;
+
+friend class MatrixCommitment;
+};
+
+// ----------------------------- MatrixCommitment -----------------------------
+
+/// A MatrixCommitment provides a set of acceptable matrix characteristics.
+/// Since we define the characteristics each with its own bit, a commitment
+/// is implemented as a set of masks with bits set corresponding to the
+/// acceptable characteristics.
+
+// ----------------------------------------------------------------------------
+class SimTK_SimTKCOMMON_EXPORT MatrixCommitment {
+public:
+ MatrixCommitment() {} // set commitments to "none" and masks to "uncommitted"
+
+ /// This is an implicit conversion from a MatrixStructure specification to
+ /// a MatrixCommitment with storage, outline, and condition uncommitted.
+ MatrixCommitment(const MatrixStructure& str)
+ { new (this) MatrixCommitment(str, MatrixStorage(), MatrixOutline(), MatrixCondition());}
+
+ class Vector;
+ class RowVector;
+ class Triangular;
+ class Symmetric;
+ class Hermitian;
+ class SkewSymmetric;
+ class SkewHermitian;
+
+ MatrixCommitment& commitSize(int m, int n)
+ { commitNumRows(m); commitNumCols(n); return *this; }
+ MatrixCommitment& commitNumRows(int m)
+ { SimTK_SIZECHECK_NONNEG(m, "MatrixCommitment::commitNumRows()");
+ masks.nr = m; return *this; }
+ MatrixCommitment& commitNumCols(int n)
+ { SimTK_SIZECHECK_NONNEG(n, "MatrixCommitment::commitNumCols()");
+ masks.nc = n; return *this; }
+
+ MatrixCommitment& commitBandwidth(int lb, int ub)
+ { commitLowerBandwidth(lb); commitUpperBandwidth(ub); return *this;}
+ MatrixCommitment& commitLowerBandwidth(int lb)
+ { SimTK_SIZECHECK_NONNEG(lb, "MatrixCommitment::commitLowerBandwidth()");
+ masks.lband = lb; return *this; }
+ MatrixCommitment& commitUpperBandwidth(int ub)
+ { SimTK_SIZECHECK_NONNEG(ub, "MatrixCommitment::commitUpperBandwidth()");
+ masks.uband = ub; return *this; }
+
+ MatrixCommitment& commitStructure(const MatrixStructure& s)
+ { structure=s; masks.structure=s.mask(); return *this; }
+ MatrixCommitment& commitStorage (const MatrixStorage& s)
+ { storage=s; masks.storage =s.mask(); return *this; }
+ MatrixCommitment& commitOutline (const MatrixOutline& o)
+ { outline=o; masks.outline =o.mask(); return *this; }
+ MatrixCommitment& commitCondition(const MatrixCondition& c)
+ { condition=c; masks.condition=c.mask(); return *this; }
+
+ /// For any handle commitment, we can calculate a "best character" for
+ /// an allocation that satisfies the commitment, optionally with an
+ /// initial allocation size. Typically it is the least-restrictive
+ /// actual character that satisfies the commitment. For
+ /// example, if the commitment is Triangular we'll allocate a full triangular
+ /// matrix, not a banded one, or a symmetric one, or for that matter an
+ /// identity matrix, all of which would satisfy the commitment.
+ /// The supplied sizes are used as minima -- if the commitment requires
+ /// a larger minimum size you'll get that. For example, if you specify
+ /// 0x0 but you're committed to a Column outline, you'll get 0x1.
+ MatrixCharacter calcDefaultCharacter(int minNumRows, int minNumCols) const;
+
+ /// These report the commitment as it was specified.
+ const MatrixStructure& getStructureCommitment() const {return structure;}
+ const MatrixStorage& getStorageCommitment() const {return storage;}
+ const MatrixOutline& getOutlineCommitment() const {return outline;}
+ const MatrixCondition& getConditionCommitment() const {return condition;}
+
+ /// These report the masks of acceptable values generated from the commitment.
+ const MatrixStructure::Mask& getStructureMask() const {return masks.structure;}
+ const MatrixStorage::Mask& getStorageMask() const {return masks.storage;}
+ const MatrixOutline::Mask& getOutlineMask() const {return masks.outline;}
+ const MatrixCondition::Mask& getConditionMask() const {return masks.condition;}
+
+ MatrixCharacter::Mask::SizeMask getNumRowsMask() const {return masks.nr;}
+ MatrixCharacter::Mask::SizeMask getNumColsMask() const {return masks.nc;}
+ MatrixCharacter::Mask::SizeMask getLowerBandwidthMask() const {return masks.lband;}
+ MatrixCharacter::Mask::SizeMask getUpperBandwidthMask() const {return masks.uband;}
+
+ int getDefaultNumRows() const {return masks.getDefaultNumRows();}
+ int getDefaultNumCols() const {return masks.getDefaultNumRows();}
+
+ bool isSizeOK(int m, int n) const {return masks.isSizeOK(m,n);}
+ bool isSizeOK(const std::pair<int,int>& mn) const
+ { return isSizeOK(mn.first, mn.second); }
+
+ bool isBandwidthOK(int lower, int upper) const {return masks.isBandwidthOK(lower,upper);}
+
+ bool isSatisfiedBy(const MatrixCharacter& actual) const
+ { return masks.isSatisfiedBy(actual); }
+ bool isStructureOK(const MatrixStructure& s) const
+ { return getStructureMask().isSatisfiedBy(s); }
+ bool isStorageOK(const MatrixStorage& s) const
+ { return getStorageMask().isSatisfiedBy(s); }
+ bool isOutlineOK(const MatrixOutline& o) const
+ { return getOutlineMask().isSatisfiedBy(o); }
+ bool isConditionOK(const MatrixCondition& c) const
+ { return getConditionMask().isSatisfiedBy(c); }
+
+ bool isResizeable() const {return masks.isResizeable();}
+ bool isFullyResizeable() const {return masks.isFullyResizeable();;}
+ bool isNumRowsLocked() const {return masks.isNumRowsLocked();}
+ bool isNumColsLocked() const {return masks.isNumColsLocked();}
+
+ bool isStructureCommitted() const
+ { return !getStructureMask().isUncommitted(); }
+ bool isStorageCommitted() const
+ { return !getStorageMask().isUncommitted();}
+ bool isOutlineCommitted() const
+ { return !getOutlineMask().isUncommitted(); }
+ bool isConditionCommitted() const
+ { return !getConditionMask().isUncommitted();}
+
+ /// Set commitment s to "none" and masks to "uncommitted" for all characteristics.
+ void clear() {
+ structure.setToNone();
+ storage.setToNone();
+ outline.setToNone();
+ condition.setToNone();
+ masks.setToUncommitted();
+ }
+
+protected:
+ MatrixCommitment(const MatrixStructure& structure,
+ const MatrixStorage& storage,
+ const MatrixOutline& outline,
+ const MatrixCondition& condition)
+ : structure(structure), storage(storage),
+ outline(outline), condition(condition),
+ masks() // set to all 1's
+ {
+ if (outline.getOutline()==MatrixOutline::Scalar) commitSize(1,1);
+ else if (outline.getOutline()==MatrixOutline::Column) commitNumCols(1);
+ else if (outline.getOutline()==MatrixOutline::Row) commitNumRows(1);
+
+ masks.structure = structure.mask();
+ masks.storage = storage.mask();
+ masks.outline = outline.mask();
+ masks.condition = condition.mask();
+ }
+
+ /// These are the commitments as specified. They are used to fill in
+ /// the corresponding bitmasks of acceptable characteristics below.
+ MatrixStructure structure;
+ MatrixStorage storage;
+ MatrixOutline outline;
+ MatrixCondition condition;
+
+ /// These are the bitmasks of acceptable characteristics which
+ /// would satisfy the above-specified commitments.
+ MatrixCharacter::Mask masks;
+};
+
+
+/// This is the default commitment for a column vector.
+class MatrixCommitment::Vector : public MatrixCommitment {
+public:
+ /// Commit to a resizeable column vector.
+ Vector()
+ : MatrixCommitment
+ ( MatrixStructure(MatrixStructure::Matrix1d),
+ MatrixStorage(),
+ MatrixOutline(MatrixOutline::Column),
+ MatrixCondition())
+ {
+ }
+ /// Commit to a column vector of a particular length.
+ explicit Vector(int m)
+ : MatrixCommitment
+ ( MatrixStructure(MatrixStructure::Matrix1d),
+ MatrixStorage(),
+ MatrixOutline(MatrixOutline::Column),
+ MatrixCondition())
+ {
+ commitNumRows(m);
+ }
+};
+
+/// This is the default commitment for a row vector.
+class MatrixCommitment::RowVector : public MatrixCommitment {
+public:
+ /// Commit to a resizeable row vector.
+ RowVector()
+ : MatrixCommitment
+ ( MatrixStructure(MatrixStructure::Matrix1d),
+ MatrixStorage(),
+ MatrixOutline(MatrixOutline::Row),
+ MatrixCondition())
+ {
+ }
+ /// Commit to a row vector of a particular length.
+ explicit RowVector(int n)
+ : MatrixCommitment
+ ( MatrixStructure(MatrixStructure::Matrix1d),
+ MatrixStorage(),
+ MatrixOutline(MatrixOutline::Row),
+ MatrixCondition())
+ {
+ commitNumCols(n);
+ }
+};
+
+/// This is the default commitment for a triangular matrix.
+class MatrixCommitment::Triangular : public MatrixCommitment {
+public:
+ Triangular()
+ : MatrixCommitment(MatrixStructure::Triangular, MatrixStorage(),
+ MatrixOutline(), MatrixCondition())
+ {
+ }
+};
+
+/// This is the default commitment for a symmetric (*not* Hermitian) matrix.
+/// There are no restrictions on the underlying elements or diagonals.
+class MatrixCommitment::Symmetric : public MatrixCommitment {
+public:
+ Symmetric()
+ : MatrixCommitment(MatrixStructure::Symmetric, MatrixStorage(),
+ MatrixOutline(), MatrixCondition())
+ {
+ }
+};
+
+/// This is the default commitment for a Hermitian (*not* symmetric) matrix.
+/// Diagonal elements must be real since they have to serve as their own
+/// conjugates.
+class MatrixCommitment::Hermitian : public MatrixCommitment {
+public:
+ Hermitian()
+ : MatrixCommitment
+ ( MatrixStructure::Hermitian,
+ MatrixStorage(),
+ MatrixOutline(),
+ MatrixCondition().setDiagonal(MatrixCondition::RealDiagonal))
+ {
+ }
+};
+
+/// This is the default commitment for skew symmetric (*not* skew Hermitian)
+/// matrix. Diagonal elements must be all-zero since they have to be their
+/// own negation. Otherwise any elements are acceptable.
+class MatrixCommitment::SkewSymmetric : public MatrixCommitment {
+public:
+ SkewSymmetric()
+ : MatrixCommitment
+ ( MatrixStructure::SkewSymmetric,
+ MatrixStorage(),
+ MatrixOutline(),
+ MatrixCondition().setDiagonal(MatrixCondition::ZeroDiagonal))
+ {
+ }
+};
+
+/// This is the default commitment for a skew Hermitian (*not* skew symmetric)
+/// matrix. Diagonal elements must be pure imaginary since when conjugated they
+/// must be their own negation for a skew matrix.
+class MatrixCommitment::SkewHermitian : public MatrixCommitment {
+public:
+ SkewHermitian()
+ : MatrixCommitment
+ ( MatrixStructure::SkewHermitian,
+ MatrixStorage(),
+ MatrixOutline(),
+ MatrixCondition().setDiagonal(MatrixCondition::ImaginaryDiagonal))
+ {
+ }
+};
+
+/// Output a textual description of a MatrixCommitment; handy for debugging.
+/// @relates SimTK::MatrixCommitment
+SimTK_SimTKCOMMON_EXPORT std::ostream&
+operator<<(std::ostream& o, const MatrixCommitment&);
+
+} //namespace SimTK
+
+#endif // SimTK_SIMMATRIX_MATRIX_CHARACTERISTICS_H_
diff --git a/SimTKcommon/BigMatrix/include/SimTKcommon/internal/MatrixHelper.h b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/MatrixHelper.h
new file mode 100644
index 0000000..fce10c0
--- /dev/null
+++ b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/MatrixHelper.h
@@ -0,0 +1,365 @@
+#ifndef SimTK_SIMMATRIX_MATRIX_HELPER_H_
+#define SimTK_SIMMATRIX_MATRIX_HELPER_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ *
+ * Here we declare the detailed interface to the Simmatrix classes. This is
+ * still client-side code although nothing here should need to appear as part
+ * of the user documentation. Code here primarily deals with the interface
+ * between the arbitrarily-templatized user visible Matrix, Vector, and
+ * RowVector classes and the finitely templatized implementation classes,
+ * which are available only for SimTK scalar types.
+ */
+
+#include "SimTKcommon/Scalar.h"
+
+#include <iostream>
+#include <cassert>
+#include <complex>
+#include <cstddef>
+#include <utility> // for std::pair
+
+namespace SimTK {
+
+
+template <class S> class MatrixHelperRep;
+class MatrixCharacter;
+class MatrixCommitment;
+
+// ------------------------------ MatrixHelper --------------------------------
+
+/// Here we define class MatrixHelper<S>, the scalar-type templatized helper
+/// class for the more general, composite numerical type-templatized
+/// class MatrixBase<ELT>. The helper class is not intended to appear
+/// directly in user programs; it is client-side code used by the client-side
+/// Matrix<CNT>, Vector<CNT>, etc. templates to reduce the infinite set
+/// of possible CNT templates to a finite set of scalar templates which
+/// can then have hidden implementations in the Simmatrix library.
+/// The hidden implementation class will be instantiated once each for
+/// float, double, long double and the associated complex and conjugate types,
+/// and their negators (a total of 18 types). Element size is
+/// dealt with at run time; otherwise the helper knows nothing about the
+/// structure of the elements.
+///
+/// The constructors for numerical types should not initialize the numerical
+/// values. We take advantage of that here -- this class assumes it can simply
+/// allocate the appropriate amount of data as an array of the underlying
+/// scalar type, with no implicit initialization. We'll fill uninitialized data
+/// with NaNs when debugging or if specifically requested; otherwise it will
+/// contain garbage initially.
+///
+/// Note that this is just a templatized handle class. The implementation
+/// is private, in the undefined class MatrixHelperRep<S>.
+
+// ----------------------------------------------------------------------------
+template <class S>
+class SimTK_SimTKCOMMON_EXPORT MatrixHelper {
+ typedef MatrixHelper<S> This;
+ typedef MatrixHelper<typename CNT<S>::TNeg> ThisNeg;
+ typedef MatrixHelper<typename CNT<S>::THerm> ThisHerm;
+public:
+ typedef typename CNT<S>::Number Number; // strips off negator from S
+ typedef typename CNT<S>::StdNumber StdNumber; // turns conjugate to complex
+ typedef typename CNT<S>::Precision Precision; // strips off complex from StdNumber
+
+ // no default constructor
+ // copy constructor suppressed
+
+ // Destructor eliminates MatrixHelperRep object if "this" owns it.
+ ~MatrixHelper() {deleteRepIfOwner();}
+
+ // Local types for directing us to the right constructor at compile time.
+ class ShallowCopy { };
+ class DeepCopy { };
+ class TransposeView { };
+ class DiagonalView { };
+
+ //////////////////////////
+ // "Owner" constructors //
+ //////////////////////////
+
+ // 0x0, fully resizable, fully uncommitted.
+ MatrixHelper(int esz, int cppEsz);
+
+ // Default allocation for the given commitment.
+ MatrixHelper(int esz, int cppEsz, const MatrixCommitment&);
+
+ // Allocate a matrix that satisfies a given commitment and has a
+ // particular initial size.
+ MatrixHelper(int esz, int cppEsz, const MatrixCommitment&, int m, int n);
+
+ // Copy constructor that produces a new owner whose logical shape and contents are
+ // the same as the source, but with a possibly better storage layout. Data will
+ // be contiguous in the copy regardless of how spread out it was in the source.
+ // The second argument is just to disambiguate this constructor from similar ones.
+ MatrixHelper(const MatrixCommitment&, const MatrixHelper& source, const DeepCopy&);
+
+ // This has the same semantics as the previous DeepCopy constructor, except that
+ // the source has negated elements with respect to S. The resulting logical shape
+ // and logical values are identical to the source, meaning that the negation must
+ // actually be performed here, using one flop for every meaningful source scalar.
+ MatrixHelper(const MatrixCommitment&, const MatrixHelper<typename CNT<S>::TNeg>& source, const DeepCopy&);
+
+
+ //////////////////////////////////
+ // External "View" constructors //
+ //////////////////////////////////
+
+ // These constructors produce a matrix which provides a view of externally-allocated
+ // data, which is known to have the given MatrixCharacter. There is also provision
+ // for a "spacing" parameter which defines gaps in the supplied data, although
+ // the interpretation of that parameter depends on the MatrixCharacter (typically
+ // it is the leading dimension for a matrix or the stride for a vector). Note
+ // that spacing is interpreted as "number of scalars between adjacent elements"
+ // which has the usual Lapack interpretation if the elements are scalars but
+ // can be used for C++ vs. Simmatrix packing differences for composite elements.
+ // The resulting handle is *not* the owner of the data, so destruction of the
+ // handle does not delete the data.
+
+ // Create a read-only view into existing data.
+ MatrixHelper(int esz, int cppEsz, const MatrixCommitment&,
+ const MatrixCharacter&, int spacing, const S* data);
+ // Create a writable view into existing data.
+ MatrixHelper(int esz, int cppEsz, const MatrixCommitment&,
+ const MatrixCharacter&, int spacing, S* data);
+
+
+ ////////////////////////////////
+ // Matrix "View" constructors //
+ ////////////////////////////////
+
+ // Matrix view constructors, read only and writable. Use these for submatrices,
+ // rows, and columns. Indices are by *element* and these may consist of multiple
+ // scalars of type template parameter S.
+
+ // "Block" views
+ MatrixHelper(const MatrixCommitment&, const MatrixHelper&, int i, int j, int nrow, int ncol);
+ MatrixHelper(const MatrixCommitment&, MatrixHelper&, int i, int j, int nrow, int ncol);
+
+ // "Transpose" views (note that this is Hermitian transpose; i.e., element
+ // type is conjugated).
+ MatrixHelper(const MatrixCommitment&, const MatrixHelper<typename CNT<S>::THerm>&,
+ const TransposeView&); // a read only transposed view
+ MatrixHelper(const MatrixCommitment&, MatrixHelper<typename CNT<S>::THerm>&,
+ const TransposeView&); // a writable transposed view
+
+ // "Diagonal" views.
+ MatrixHelper(const MatrixCommitment&, const MatrixHelper&, const DiagonalView&); // a read only diagonal view
+ MatrixHelper(const MatrixCommitment&, MatrixHelper&, const DiagonalView&); // a writable diagonal view
+
+ // "Indexed" view of a 1d matrix.
+ MatrixHelper(const MatrixCommitment&, const MatrixHelper&,
+ int n, const int* indices);
+ MatrixHelper(const MatrixCommitment&, MatrixHelper&,
+ int n, const int* indices);
+
+ // These invoke the previous constructors but with friendlier index source.
+ MatrixHelper(const MatrixCommitment& mc, const MatrixHelper& h,
+ const Array_<int>& indices)
+ { new (this) MatrixHelper(mc, h, (int)indices.size(), indices.cbegin()); }
+ MatrixHelper(const MatrixCommitment& mc, MatrixHelper& h,
+ const Array_<int>& indices)
+ { new (this) MatrixHelper(mc, h, (int)indices.size(), indices.cbegin()); }
+
+ // "Copy" an existing MatrixHelper by making a new view into the same data.
+ // The const form loses writability, non-const retains same writability status
+ // as source. If the source is already a view then the destination will have
+ // an identical element filter so that the logical shape and values are the
+ // same for both source and copy. (The second argument is used just to
+ // disambiguate this constructor from similar ones.)
+ MatrixHelper(const MatrixCommitment&, const MatrixHelper& source, const ShallowCopy&);
+ MatrixHelper(const MatrixCommitment&, MatrixHelper& source, const ShallowCopy&);
+
+ /////////////////////
+ // Copy assignment //
+ /////////////////////
+
+ // Behavior of copy assignment depends on whether "this" is an owner or view. If
+ // it's an owner it is resized and ends up a new, dense copy of "source" just as
+ // for the DeepCopy constructor above. If "this" is a writable view, sizes must match
+ // and we copy elements from "source" to corresponding elements of "this". If
+ // "this" is not writable then the operation will fail.
+ MatrixHelper& copyAssign(const MatrixHelper& source);
+
+ // Same as copyAssign() except the source has element types which are logically
+ // negated from the element types of "this". Since the result must have the
+ // same value as the source, this requires that all the copied elements are
+ // actually negated at a cost of one flop per scalar.
+ MatrixHelper& negatedCopyAssign(const MatrixHelper<typename CNT<S>::TNeg>&);
+
+ /////////////////////
+ // View assignment //
+ /////////////////////
+
+ // View assign always disconnects this helper from whatever view & data
+ // it used to have (destructing as appropriate) and then makes it a new view
+ // of the source. Writability is lost if the source is const, otherwise
+ // writability is inherited from the source.
+ MatrixHelper& readOnlyViewAssign(const MatrixHelper& source);
+ MatrixHelper& writableViewAssign(MatrixHelper& source);
+
+ // Restore helper to its just-constructed state. We forget everything except
+ // the element size and handle commitment, which can never change. The
+ // allocated helper will be the same as if we had just default-constructed
+ // using the current commitment.
+ void clear();
+
+ // Return true if there is currently no data associated with this handle.
+ bool isClear() const;
+
+ // Using *element* indices, obtain a pointer to the beginning of a particular
+ // element. This is always a slow operation compared to raw array access;
+ // use sparingly. These will fail if the indices are outside the stored
+ // portion of the matrix. Use getAnyElt() if you want something that always
+ // works.
+ const S* getElt(int i, int j) const;
+ S* updElt(int i, int j);
+
+ // Faster for 1-d matrices (vectors) if you know you have one.
+ const S* getElt(int i) const;
+ S* updElt(int i);
+
+ // This returns the correct value for any element within the logical dimensions
+ // of the matrix. In some cases it has to compute the value; in all cases
+ // it has to copy it.
+ void getAnyElt(int i, int j, S* value) const;
+
+ // Faster for 1-d matrices (vectors) if you know you have one.
+ void getAnyElt(int i, S* value) const;
+
+ // Add up all the *elements*, returning the answer in the element given
+ // by pointer to its first scalar.
+ void sum(S* eltp) const;
+ void colSum(int j, S* eltp) const;
+ void rowSum(int i, S* eltp) const;
+
+ // addition and subtraction (+= and -=)
+ void addIn(const MatrixHelper&);
+ void addIn(const MatrixHelper<typename CNT<S>::TNeg>&);
+ void subIn(const MatrixHelper&);
+ void subIn(const MatrixHelper<typename CNT<S>::TNeg>&);
+
+ // Fill all our stored data with copies of the same supplied element.
+ void fillWith(const S* eltp);
+
+ // We're copying data from a C++ row-oriented matrix into our general
+ // Matrix. In addition to the row ordering, C++ may use different spacing
+ // for elements than Simmatrix does.
+ void copyInByRowsFromCpp(const S* elts);
+
+ // Scalar operations //
+
+ // Fill every element with repeated copies of a single scalar value.
+ void fillWithScalar(const StdNumber&);
+
+ // Scalar multiply (and divide). This is useful no matter what the
+ // element structure and will produce the correct result.
+ void scaleBy(const StdNumber&);
+
+ // This is only allowed for a matrix of real or complex or neg of those,
+ // which is square, well-conditioned, and for which we have no view,
+ // and element size 1.
+ void invertInPlace();
+
+ void dump(const char* msg=0) const; // For debugging -- comes out in a way you can feed to Matlab
+
+ // See comment in MatrixBase::matmul for an explanation.
+ template <class SA, class SB>
+ void matmul(const StdNumber& beta, // applied to 'this'
+ const StdNumber& alpha, const MatrixHelper<SA>& A, const MatrixHelper<SB>& B);
+
+ // Bookkeeping //
+
+ // This is the number of logical *elements* in each column of this matrix; i.e., m.
+ int nrow() const;
+ // This is the number of *elements* in each row of this matrix; i.e., n.
+ int ncol() const;
+ // This is the total number of *elements* in the logical shape of this matrix, i.e. m*n.
+ ptrdiff_t nelt() const; // nrow*ncol
+ // This is the number of elements if this is a 1d matrix (vector or rowvector). That is,
+ // it is the same as one of nrow() or ncol(); the other must be 1. It is also the
+ // same as nelt() but limited to fit in 32 bits.
+ int length() const;
+
+ // Change the logical size of the underlying memory for this Matrix to m X n, forgetting
+ // everything that used to be there. This will fail if it would have to resize any
+ // non-resizable dimension. However, it will succeed even on non-resizable matrices and
+ // views provided the existing dimensions are already correct. If no size change is made,
+ // no action is taken and the original data is still accessible.
+ void resize(int m, int n);
+
+ // Same as resize() except as much of the original data as will fit remains in place at
+ // the same (i,j) location it had before. This may require copying the elements after
+ // allocating new space. As for resize(), this is allowed for any Matrix whose dimensions
+ // are already right, even if that Matrix is not resizable.
+ void resizeKeep(int m, int n);
+
+ void lockShape();
+ void unlockShape();
+
+ const MatrixCommitment& getCharacterCommitment() const;
+ const MatrixCharacter& getMatrixCharacter() const;
+ void commitTo(const MatrixCommitment&);
+
+ // Access to raw data. For now this is only allowed if there is no view
+ // and the raw data is contiguous.
+ bool hasContiguousData() const;
+ ptrdiff_t getContiguousDataLength() const;
+ const S* getContiguousData() const;
+ S* updContiguousData();
+
+ void replaceContiguousData(S* newData, ptrdiff_t length, bool takeOwnership);
+ void replaceContiguousData(const S* newData, ptrdiff_t length);
+ void swapOwnedContiguousData(S* newData, ptrdiff_t length, S*& oldData);
+
+ const MatrixHelperRep<S>& getRep() const {assert(rep); return *rep;}
+ MatrixHelperRep<S>& updRep() {assert(rep); return *rep;}
+ void setRep(MatrixHelperRep<S>* hrep) {assert(!rep); rep = hrep;}
+ MatrixHelperRep<S>* stealRep()
+ { assert(rep); MatrixHelperRep<S>* stolen=rep; rep=0; return stolen; }
+
+ void deleteRepIfOwner();
+ void replaceRep(MatrixHelperRep<S>*);
+
+ // Rep-stealing constructor. We're taking ownership of the supplied rep.
+ // Internal use only!
+ explicit MatrixHelper(MatrixHelperRep<S>*);
+
+private:
+ // Pointer to the private implementation object. This is the only
+ // allowable data member in this class.
+ class MatrixHelperRep<S>* rep;
+
+ // Suppress copy constructor.
+ MatrixHelper(const MatrixHelper&);
+
+friend class MatrixHelper<typename CNT<S>::TNeg>;
+friend class MatrixHelper<typename CNT<S>::THerm>;
+};
+
+} //namespace SimTK
+
+#endif // SimTK_SIMMATRIX_MATRIX_HELPER_H_
diff --git a/SimTKcommon/BigMatrix/include/SimTKcommon/internal/MatrixView_.h b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/MatrixView_.h
new file mode 100644
index 0000000..40725c8
--- /dev/null
+++ b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/MatrixView_.h
@@ -0,0 +1,98 @@
+#ifndef SimTK_SIMMATRIX_MATRIXVIEW_H_
+#define SimTK_SIMMATRIX_MATRIXVIEW_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Define the SimTK::MatrixView_ class that is part of Simbody's BigMatrix
+toolset. **/
+
+namespace SimTK {
+
+//==============================================================================
+// MATRIX VIEW
+//==============================================================================
+/** @brief (Advanced) This class is identical to Matrix_ except that it has
+shallow (reference) copy and assignment semantics.
+
+Despite the name, this may be an owner if a Matrix_ is recast to a MatrixView_.
+However, there are no owner constructors for MatrixView_.
+
+ at see Matrix_, MatrixBase, VectorView_ **/
+template <class ELT> class MatrixView_ : public MatrixBase<ELT> {
+ typedef MatrixBase<ELT> Base;
+ typedef typename CNT<ELT>::Scalar S;
+ typedef typename CNT<ELT>::StdNumber StdNumber;
+public:
+ // Default construction is suppressed.
+ // Uses default destructor.
+
+ // Create a MatrixView_ handle using a given helper rep.
+ explicit MatrixView_(MatrixHelperRep<S>* hrep) : Base(hrep) {}
+
+ // Copy constructor is shallow. CAUTION: despite const argument, this preserves writability
+ // if it was present in the source. This is necessary to allow temporary views to be
+ // created and used as lvalues.
+ MatrixView_(const MatrixView_& m)
+ : Base(MatrixCommitment(),
+ const_cast<MatrixHelper<S>&>(m.getHelper()),
+ typename MatrixHelper<S>::ShallowCopy()) {}
+
+ // Copy assignment is deep but not reallocating.
+ MatrixView_& operator=(const MatrixView_& m) {
+ Base::operator=(m); return *this;
+ }
+
+ // Ask for shallow copy
+ MatrixView_(const MatrixHelper<S>& h) : Base(MatrixCommitment(), h, typename MatrixHelper<S>::ShallowCopy()) { }
+ MatrixView_(MatrixHelper<S>& h) : Base(MatrixCommitment(), h, typename MatrixHelper<S>::ShallowCopy()) { }
+
+ MatrixView_& operator=(const Matrix_<ELT>& v) { Base::operator=(v); return *this; }
+ MatrixView_& operator=(const ELT& e) { Base::operator=(e); return *this; }
+
+ template <class EE> MatrixView_& operator=(const MatrixBase<EE>& m)
+ { Base::operator=(m); return *this; }
+ template <class EE> MatrixView_& operator+=(const MatrixBase<EE>& m)
+ { Base::operator+=(m); return *this; }
+ template <class EE> MatrixView_& operator-=(const MatrixBase<EE>& m)
+ { Base::operator-=(m); return *this; }
+
+ MatrixView_& operator*=(const StdNumber& t) { Base::operator*=(t); return *this; }
+ MatrixView_& operator/=(const StdNumber& t) { Base::operator/=(t); return *this; }
+ MatrixView_& operator+=(const ELT& r) { this->updDiag() += r; return *this; }
+ MatrixView_& operator-=(const ELT& r) { this->updDiag() -= r; return *this; }
+
+ operator const Matrix_<ELT>&() const { return *reinterpret_cast<const Matrix_<ELT>*>(this); }
+ operator Matrix_<ELT>&() { return *reinterpret_cast<Matrix_<ELT>*>(this); }
+
+private:
+ // NO DATA MEMBERS ALLOWED
+ MatrixView_(); // default constructor suppressed (what's it a view of?)
+};
+
+
+
+} //namespace SimTK
+
+#endif // SimTK_SIMMATRIX_MATRIXVIEW_H_
diff --git a/SimTKcommon/BigMatrix/include/SimTKcommon/internal/Matrix_.h b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/Matrix_.h
new file mode 100644
index 0000000..e5a8da8
--- /dev/null
+++ b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/Matrix_.h
@@ -0,0 +1,152 @@
+#ifndef SimTK_SIMMATRIX_MATRIX_H_
+#define SimTK_SIMMATRIX_MATRIX_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Define the SimTK::Matrix_ class that is part of Simbody's BigMatrix toolset. **/
+
+namespace SimTK {
+
+//==============================================================================
+// MATRIX
+//==============================================================================
+/** @brief This is the matrix class intended to appear in user code for large,
+variable size matrices.
+
+ at ingroup MatVecUtilities
+
+More commonly, the typedef @ref SimTK::Matrix "Matrix" is used instead; that is
+just an abbreviation for \c Matrix_<Real>.
+
+A %Matrix_ can be a fixed-size view of someone else's data, or can be a
+resizable data owner itself.
+
+ at see Mat for handling of small, fixed-size matrices with no runtime overhead.
+ at see Vector_ for variable size, one-dimensional column vector.
+ at see MatrixView_, MatrixBase
+**/
+//------------------------------------------------------------------------------
+template <class ELT> class Matrix_ : public MatrixBase<ELT> {
+ typedef typename CNT<ELT>::Scalar S;
+ typedef typename CNT<ELT>::Number Number;
+ typedef typename CNT<ELT>::StdNumber StdNumber;
+
+ typedef typename CNT<ELT>::TNeg ENeg;
+ typedef typename CNT<ELT>::THerm EHerm;
+
+ typedef MatrixBase<ELT> Base;
+ typedef MatrixBase<ENeg> BaseNeg;
+ typedef MatrixBase<EHerm> BaseHerm;
+
+ typedef Matrix_<ELT> T;
+ typedef MatrixView_<ELT> TView;
+ typedef Matrix_<ENeg> TNeg;
+
+public:
+ Matrix_() : Base() { }
+ explicit Matrix_(const MatrixCommitment& mc) : Base(mc) {}
+
+ // Copy constructor is deep.
+ Matrix_(const Matrix_& src) : Base(src) { }
+
+ // Assignment is a deep copy and will also allow reallocation if this Matrix
+ // doesn't have a view.
+ Matrix_& operator=(const Matrix_& src) {
+ Base::operator=(src); return *this;
+ }
+
+ // Force a deep copy of the view or whatever this is.
+ // Note that this is an implicit conversion.
+ Matrix_(const Base& v) : Base(v) {} // e.g., MatrixView
+
+ // Allow implicit conversion from a source matrix that
+ // has a negated version of ELT.
+ Matrix_(const BaseNeg& v) : Base(v) {}
+
+ // TODO: implicit conversion from conjugate. This is trickier
+ // since real elements are their own conjugate so you'll get
+ // duplicate methods defined from Matrix_(BaseHerm) and Matrix_(Base).
+
+ Matrix_(int m, int n) : Base(MatrixCommitment(), m, n) {}
+
+ Matrix_(int m, int n, const ELT* cppInitialValuesByRow)
+ : Base(MatrixCommitment(), m, n, cppInitialValuesByRow) {}
+ Matrix_(int m, int n, const ELT& initialValue)
+ : Base(MatrixCommitment(), m, n, initialValue) {}
+
+ Matrix_(int m, int n, int leadingDim, const S* data) // read only
+ : Base(MatrixCommitment(), MatrixCharacter::LapackFull(m,n),
+ leadingDim, data) {}
+ Matrix_(int m, int n, int leadingDim, S* data) // writable
+ : Base(MatrixCommitment(), MatrixCharacter::LapackFull(m,n),
+ leadingDim, data) {}
+
+ /// Convert a Mat to a Matrix_.
+ template <int M, int N, int CS, int RS>
+ explicit Matrix_(const Mat<M,N,ELT,CS,RS>& mat)
+ : Base(MatrixCommitment(), M, N)
+ { for (int i = 0; i < M; ++i)
+ for (int j = 0; j < N; ++j)
+ this->updElt(i, j) = mat(i, j); }
+
+ Matrix_& operator=(const ELT& v) { Base::operator=(v); return *this; }
+
+ template <class EE> Matrix_& operator=(const MatrixBase<EE>& m)
+ { Base::operator=(m); return*this; }
+ template <class EE> Matrix_& operator+=(const MatrixBase<EE>& m)
+ { Base::operator+=(m); return*this; }
+ template <class EE> Matrix_& operator-=(const MatrixBase<EE>& m)
+ { Base::operator-=(m); return*this; }
+
+ Matrix_& operator*=(const StdNumber& t) { Base::operator*=(t); return *this; }
+ Matrix_& operator/=(const StdNumber& t) { Base::operator/=(t); return *this; }
+ Matrix_& operator+=(const ELT& r) { this->updDiag() += r; return *this; }
+ Matrix_& operator-=(const ELT& r) { this->updDiag() -= r; return *this; }
+
+ const TNeg& negate() const {return *reinterpret_cast<const TNeg*>(this); }
+ TNeg& updNegate() {return *reinterpret_cast<TNeg*>(this); }
+
+ const TNeg& operator-() const {return negate();}
+ TNeg& operator-() {return updNegate();}
+
+ // Functions to be used for Scripting in MATLAB and languages that do not support operator overloading
+ /** toString() returns a string representation of the Matrix_. Please refer to operator<< for details. **/
+ std::string toString() const {
+ std::stringstream stream;
+ stream << (*this) ;
+ return stream.str();
+ }
+ /** Variant of indexing operator that's scripting friendly to get entry (i, j) **/
+ const ELT& get(int i,int j) const { return this->getElt(i,j); }
+ /** Variant of indexing operator that's scripting friendly to set entry (i, j) **/
+ void set(int i,int j, const ELT& value) { this->updElt(i,j)=value; }
+
+private:
+ // NO DATA MEMBERS ALLOWED
+};
+
+} //namespace SimTK
+
+#endif // SimTK_SIMMATRIX_MATRIX_H_
diff --git a/SimTKcommon/BigMatrix/include/SimTKcommon/internal/RowVectorBase.h b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/RowVectorBase.h
new file mode 100644
index 0000000..ca32d0b
--- /dev/null
+++ b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/RowVectorBase.h
@@ -0,0 +1,315 @@
+#ifndef SimTK_SIMMATRIX_ROWVECTORBASE_H_
+#define SimTK_SIMMATRIX_ROWVECTORBASE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Define the SimTK::RowVectorBase class that is part of Simbody's BigMatrix
+toolset. **/
+
+namespace SimTK {
+
+//==============================================================================
+// ROW VECTOR BASE
+//==============================================================================
+/** @brief This is a dataless rehash of the MatrixBase class to specialize it
+for RowVectors.
+
+This mostly entails overriding a few of the methods. Note that all the
+MatrixBase operations remain available if you \c static_cast this up to a
+MatrixBase. **/
+template <class ELT> class RowVectorBase : public MatrixBase<ELT> {
+ typedef MatrixBase<ELT> Base;
+ typedef typename CNT<ELT>::Scalar Scalar;
+ typedef typename CNT<ELT>::Number Number;
+ typedef typename CNT<ELT>::StdNumber StdNumber;
+ typedef RowVectorBase<ELT> T;
+ typedef RowVectorBase<typename CNT<ELT>::TAbs> TAbs;
+ typedef RowVectorBase<typename CNT<ELT>::TNeg> TNeg;
+ typedef VectorView_<typename CNT<ELT>::THerm> THerm;
+public:
+ // ------------------------------------------------------------------------
+ /// @name RowVectorBase "owner" construction
+ ///
+ /// These constructors create new RowVectorBase objects which own their
+ /// own data and are (at least by default) resizable. The resulting matrices
+ /// are 1 x n with the number of rows locked at 1. If there is any data
+ /// allocated but not explicitly initialized, that data will be uninitialized
+ /// garbage in Release builds but will be initialized to NaN (at a performance
+ /// cost) in Debug builds.
+ /// @{
+
+ /// Default constructor makes a 1x0 matrix locked at 1 row; you can
+ /// provide an initial allocation if you want.
+ explicit RowVectorBase(int n=0) : Base(MatrixCommitment::RowVector(), 1, n) {}
+
+ /// Copy constructor is a deep copy (not appropriate for views!). That
+ /// means it creates a new, densely packed vector whose elements are
+ /// initialized from the source object.
+ RowVectorBase(const RowVectorBase& source) : Base(source) {}
+
+ /// Implicit conversion from compatible row vector with negated elements.
+ RowVectorBase(const TNeg& source) : Base(source) {}
+
+ /// Construct an owner row vector of length n, with each element initialized to
+ /// the given value.
+ RowVectorBase(int n, const ELT& initialValue)
+ : Base(MatrixCommitment::RowVector(),1,n,initialValue) {}
+
+ /// Construct an owner vector of length n, with the elements initialized sequentially
+ /// from a C++ array of elements which is assumed to be of length n. Note that we
+ /// are expecting C++ packing; don't use this to initialize one Simmatrix vector
+ /// from another because Simmatrix may pack its elements more densely than C++.
+ RowVectorBase(int n, const ELT* cppInitialValues)
+ : Base(MatrixCommitment::RowVector(),1,n,cppInitialValues) {}
+ /// @}
+
+ // ------------------------------------------------------------------------
+ /// @name RowVectorBase construction from pre-existing data
+ ///
+ /// Construct a non-resizeable, RowVectorBase view of externally supplied data. Note that
+ /// stride should be interpreted as "the number of scalars between elements" and
+ /// for composite elements may have a different value if the source is a C++ array
+ /// of elements vs. a Simmatrix packed data array. We provide constructors for
+ /// both read-only and writable external data.
+ /// @{
+
+ /// Construct a read-only view of existing data.
+ RowVectorBase(int n, int stride, const Scalar* s)
+ : Base(MatrixCommitment::RowVector(n), MatrixCharacter::RowVector(n),stride,s) { }
+ /// Construct a writable view into existing data.
+ RowVectorBase(int n, int stride, Scalar* s)
+ : Base(MatrixCommitment::RowVector(n), MatrixCharacter::RowVector(n),stride,s) { }
+ /// @}
+
+ // ------------------------------------------------------------------------
+ /// @name RowVectorBase construction from an existing Helper.
+ ///
+ /// Create a new RowVectorBase from an existing helper. Both shallow (view) and deep
+ /// copies are possible. For shallow copies, there is a constructor providing a read-only
+ /// view of the original data and one providing a writable view into the original data.
+ /// @{
+
+ /// Construct a writable view into the source data.
+ RowVectorBase(MatrixHelper<Scalar>& h, const typename MatrixHelper<Scalar>::ShallowCopy& s)
+ : Base(MatrixCommitment::RowVector(), h,s) { }
+ /// Construct a read-only view of the source data.
+ RowVectorBase(const MatrixHelper<Scalar>& h, const typename MatrixHelper<Scalar>::ShallowCopy& s)
+ : Base(MatrixCommitment::RowVector(), h,s) { }
+ /// Construct a new owner vector initialized with the data from the source.
+ RowVectorBase(const MatrixHelper<Scalar>& h, const typename MatrixHelper<Scalar>::DeepCopy& d)
+ : Base(MatrixCommitment::RowVector(), h,d) { }
+ /// @}
+
+ // This gives the resulting rowvector type when (r(i) op P) is applied to each element.
+ // It will have element types which are the regular composite result of ELT op P.
+ template <class P> struct EltResult {
+ typedef RowVectorBase<typename CNT<ELT>::template Result<P>::Mul> Mul;
+ typedef RowVectorBase<typename CNT<ELT>::template Result<P>::Dvd> Dvd;
+ typedef RowVectorBase<typename CNT<ELT>::template Result<P>::Add> Add;
+ typedef RowVectorBase<typename CNT<ELT>::template Result<P>::Sub> Sub;
+ };
+
+ /// Copy assignment is deep copy but behavior depends on type of lhs: if view, rhs
+ /// must match. If owner, we reallocate and copy rhs.
+ RowVectorBase& operator=(const RowVectorBase& b) {
+ Base::operator=(b); return *this;
+ }
+
+ // default destructor
+
+ RowVectorBase& operator*=(const StdNumber& t) {Base::operator*=(t); return *this;}
+ RowVectorBase& operator/=(const StdNumber& t) {Base::operator/=(t); return *this;}
+ RowVectorBase& operator+=(const RowVectorBase& r) {Base::operator+=(r); return *this;}
+ RowVectorBase& operator-=(const RowVectorBase& r) {Base::operator-=(r); return *this;}
+
+ template <class EE> RowVectorBase& operator=(const RowVectorBase<EE>& b)
+ { Base::operator=(b); return *this; }
+ template <class EE> RowVectorBase& operator+=(const RowVectorBase<EE>& b)
+ { Base::operator+=(b); return *this; }
+ template <class EE> RowVectorBase& operator-=(const RowVectorBase<EE>& b)
+ { Base::operator-=(b); return *this; }
+
+ // default destructor
+
+ /// Fill current allocation with copies of element. Note that this is not the
+ /// same behavior as assignment for Matrices, where only the diagonal is set (and
+ /// everything else is set to zero.)
+ RowVectorBase& operator=(const ELT& t) { Base::setTo(t); return *this; }
+
+ /// There's only one row here so it's a bit wierd to use colScale rather than
+ /// elementwiseMultiply, but there's nothing really wrong with it. Using rowScale
+ /// would be really wacky since it is the same as a scalar multiply. We won't support
+ /// rowScale here except through inheritance where it will not be much use.
+ template <class EE> RowVectorBase& colScaleInPlace(const VectorBase<EE>& v)
+ { Base::template colScaleInPlace<EE>(v); return *this; }
+ template <class EE> inline void colScale(const VectorBase<EE>& v, typename EltResult<EE>::Mul& out) const
+ { return Base::template colScale<EE>(v,out); }
+ template <class EE> inline typename EltResult<EE>::Mul colScale(const VectorBase<EE>& v) const
+ { typename EltResult<EE>::Mul out(ncol()); Base::template colScale<EE>(v,out); return out; }
+
+
+ // elementwise multiply
+ template <class EE> RowVectorBase& elementwiseMultiplyInPlace(const RowVectorBase<EE>& r)
+ { Base::template elementwiseMultiplyInPlace<EE>(r); return *this; }
+ template <class EE> inline void elementwiseMultiply(const RowVectorBase<EE>& v, typename EltResult<EE>::Mul& out) const
+ { Base::template elementwiseMultiply<EE>(v,out); }
+ template <class EE> inline typename EltResult<EE>::Mul elementwiseMultiply(const RowVectorBase<EE>& v) const
+ { typename EltResult<EE>::Mul out(nrow()); Base::template elementwiseMultiply<EE>(v,out); return out; }
+
+ // elementwise multiply from left
+ template <class EE> RowVectorBase& elementwiseMultiplyFromLeftInPlace(const RowVectorBase<EE>& r)
+ { Base::template elementwiseMultiplyFromLeftInPlace<EE>(r); return *this; }
+ template <class EE> inline void
+ elementwiseMultiplyFromLeft(
+ const RowVectorBase<EE>& v,
+ typename RowVectorBase<EE>::template EltResult<ELT>::Mul& out) const
+ {
+ Base::template elementwiseMultiplyFromLeft<EE>(v,out);
+ }
+ template <class EE> inline
+ typename RowVectorBase<EE>::template EltResult<ELT>::Mul
+ elementwiseMultiplyFromLeft(const RowVectorBase<EE>& v) const {
+ typename RowVectorBase<EE>::template EltResult<ELT>::Mul out(nrow());
+ Base::template elementwiseMultiplyFromLeft<EE>(v,out);
+ return out;
+ }
+
+ // elementwise divide
+ template <class EE> RowVectorBase& elementwiseDivideInPlace(const RowVectorBase<EE>& r)
+ { Base::template elementwiseDivideInPlace<EE>(r); return *this; }
+ template <class EE> inline void elementwiseDivide(const RowVectorBase<EE>& v, typename EltResult<EE>::Dvd& out) const
+ { Base::template elementwiseDivide<EE>(v,out); }
+ template <class EE> inline typename EltResult<EE>::Dvd elementwiseDivide(const RowVectorBase<EE>& v) const
+ { typename EltResult<EE>::Dvd out(nrow()); Base::template elementwiseDivide<EE>(v,out); return out; }
+
+ // elementwise divide from left
+ template <class EE> RowVectorBase& elementwiseDivideFromLeftInPlace(const RowVectorBase<EE>& r)
+ { Base::template elementwiseDivideFromLeftInPlace<EE>(r); return *this; }
+ template <class EE> inline void
+ elementwiseDivideFromLeft
+ (const RowVectorBase<EE>& v,
+ typename RowVectorBase<EE>::template EltResult<ELT>::Dvd& out) const {
+ Base::template elementwiseDivideFromLeft<EE>(v,out);
+ }
+ template <class EE> inline
+ typename RowVectorBase<EE>::template EltResult<ELT>::Dvd
+ elementwiseDivideFromLeft(const RowVectorBase<EE>& v) const {
+ typename RowVectorBase<EE>::template EltResult<ELT>::Dvd out(nrow());
+ Base::template elementwiseDivideFromLeft<EE>(v,out);
+ return out;
+ }
+
+ // Implicit conversions are allowed to RowVector or Matrix, but not to Vector.
+ operator const RowVector_<ELT>&() const {return *reinterpret_cast<const RowVector_<ELT>*>(this);}
+ operator RowVector_<ELT>&() {return *reinterpret_cast< RowVector_<ELT>*>(this);}
+ operator const RowVectorView_<ELT>&() const {return *reinterpret_cast<const RowVectorView_<ELT>*>(this);}
+ operator RowVectorView_<ELT>&() {return *reinterpret_cast< RowVectorView_<ELT>*>(this);}
+
+ operator const Matrix_<ELT>&() const {return *reinterpret_cast<const Matrix_<ELT>*>(this);}
+ operator Matrix_<ELT>&() {return *reinterpret_cast< Matrix_<ELT>*>(this);}
+ operator const MatrixView_<ELT>&() const {return *reinterpret_cast<const MatrixView_<ELT>*>(this);}
+ operator MatrixView_<ELT>&() {return *reinterpret_cast< MatrixView_<ELT>*>(this);}
+
+
+ // size() for RowVectors is Base::nelt() but returns int instead of ptrdiff_t.
+ int size() const {
+ assert(Base::nelt() <= (ptrdiff_t)std::numeric_limits<int>::max());
+ assert(Base::nrow()==1);
+ return (int)Base::nelt();
+ }
+ int nrow() const {assert(Base::nrow()==1); return Base::nrow();}
+ int ncol() const {assert(Base::nrow()==1); return Base::ncol();}
+ ptrdiff_t nelt() const {assert(Base::nrow()==1); return Base::nelt();}
+
+ // Override MatrixBase operators to return the right shape
+ TAbs abs() const {
+ TAbs result; Base::abs(result); return result;
+ }
+
+ // Override MatrixBase indexing operators
+ const ELT& operator[](int j) const {return *reinterpret_cast<const ELT*>(Base::getHelper().getElt(j));}
+ ELT& operator[](int j) {return *reinterpret_cast<ELT*> (Base::updHelper().updElt(j));}
+ const ELT& operator()(int j) const {return *reinterpret_cast<const ELT*>(Base::getHelper().getElt(j));}
+ ELT& operator()(int j) {return *reinterpret_cast<ELT*> (Base::updHelper().updElt(j));}
+
+ // Block (contiguous subvector) creation
+ RowVectorView_<ELT> operator()(int j, int n) const {return Base::operator()(0,j,1,n).getAsRowVectorView();}
+ RowVectorView_<ELT> operator()(int j, int n) {return Base::operator()(0,j,1,n).updAsRowVectorView();}
+
+ // Indexed view creation (arbitrary subvector). Indices must be monotonically increasing.
+ RowVectorView_<ELT> index(const Array_<int>& indices) const {
+ MatrixHelper<Scalar> h(Base::getHelper().getCharacterCommitment(), Base::getHelper(), indices);
+ return RowVectorView_<ELT>(h);
+ }
+ RowVectorView_<ELT> updIndex(const Array_<int>& indices) {
+ MatrixHelper<Scalar> h(Base::getHelper().getCharacterCommitment(), Base::updHelper(), indices);
+ return RowVectorView_<ELT>(h);
+ }
+
+ RowVectorView_<ELT> operator()(const Array_<int>& indices) const {return index(indices);}
+ RowVectorView_<ELT> operator()(const Array_<int>& indices) {return updIndex(indices);}
+
+ // Hermitian transpose.
+ THerm transpose() const {return Base::transpose().getAsVectorView();}
+ THerm updTranspose() {return Base::updTranspose().updAsVectorView();}
+
+ THerm operator~() const {return transpose();}
+ THerm operator~() {return updTranspose();}
+
+ const RowVectorBase& operator+() const {return *this; }
+
+ // Negation
+
+ const TNeg& negate() const {return *reinterpret_cast<const TNeg*>(this); }
+ TNeg& updNegate() {return *reinterpret_cast<TNeg*>(this); }
+
+ const TNeg& operator-() const {return negate();}
+ TNeg& operator-() {return updNegate();}
+
+ RowVectorBase& resize(int n) {Base::resize(1,n); return *this;}
+ RowVectorBase& resizeKeep(int n) {Base::resizeKeep(1,n); return *this;}
+
+ //TODO: this is not re-locking the number of rows at 1.
+ void clear() {Base::clear(); Base::resize(1,0);}
+
+ ELT sum() const {ELT s; Base::getHelper().sum(reinterpret_cast<Scalar*>(&s)); return s; } // add all the elements
+ VectorIterator<ELT, RowVectorBase<ELT> > begin() {
+ return VectorIterator<ELT, RowVectorBase<ELT> >(*this, 0);
+ }
+ VectorIterator<ELT, RowVectorBase<ELT> > end() {
+ return VectorIterator<ELT, RowVectorBase<ELT> >(*this, size());
+ }
+
+protected:
+ // Create a RowVectorBase handle using a given helper rep.
+ explicit RowVectorBase(MatrixHelperRep<Scalar>* hrep) : Base(hrep) {}
+
+private:
+ // NO DATA MEMBERS ALLOWED
+};
+
+} //namespace SimTK
+
+#endif // SimTK_SIMMATRIX_ROWVECTORBASE_H_
diff --git a/SimTKcommon/BigMatrix/include/SimTKcommon/internal/RowVectorView_.h b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/RowVectorView_.h
new file mode 100644
index 0000000..b504e32
--- /dev/null
+++ b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/RowVectorView_.h
@@ -0,0 +1,95 @@
+#ifndef SimTK_SIMMATRIX_ROWVECTORVIEW_H_
+#define SimTK_SIMMATRIX_ROWVECTORVIEW_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Define the SimTK::RowVectorView_ class that is part of Simbody's BigMatrix
+toolset. **/
+
+namespace SimTK {
+
+//==============================================================================
+// ROW VECTOR VIEW
+//==============================================================================
+/** @brief (Advanced) This class is identical to RowVector_ except that it has
+shallow (reference) copy and assignment semantics.
+
+Despite the name, this may be an owner if a RowVector_ is recast to a
+%RowVectorView_. However, there are no owner constructors for %RowVectorView_.
+ at see RowVector_, RowVectorBase, MatrixView_ **/
+template <class ELT> class RowVectorView_ : public RowVectorBase<ELT> {
+ typedef RowVectorBase<ELT> Base;
+ typedef typename CNT<ELT>::Scalar S;
+ typedef typename CNT<ELT>::Number Number;
+ typedef typename CNT<ELT>::StdNumber StdNumber;
+ typedef RowVectorView_<ELT> T;
+ typedef RowVectorView_< typename CNT<ELT>::TNeg > TNeg;
+ typedef VectorView_< typename CNT<ELT>::THerm > THerm;
+public:
+ // Default construction is suppressed.
+ // Uses default destructor.
+
+ // Create a RowVectorView_ handle using a given helper rep.
+ explicit RowVectorView_(MatrixHelperRep<S>* hrep) : Base(hrep) {}
+
+ // Copy constructor is shallow. CAUTION: despite const argument, this preserves writability
+ // if it was present in the source. This is necessary to allow temporary views to be
+ // created and used as lvalues.
+ RowVectorView_(const RowVectorView_& r)
+ : Base(const_cast<MatrixHelper<S>&>(r.getHelper()), typename MatrixHelper<S>::ShallowCopy()) { }
+
+ // Copy assignment is deep but not reallocating.
+ RowVectorView_& operator=(const RowVectorView_& r) {
+ Base::operator=(r); return *this;
+ }
+
+ // Ask for shallow copy
+ explicit RowVectorView_(const MatrixHelper<S>& h) : Base(h, typename MatrixHelper<S>::ShallowCopy()) { }
+ explicit RowVectorView_(MatrixHelper<S>& h) : Base(h, typename MatrixHelper<S>::ShallowCopy()) { }
+
+ RowVectorView_& operator=(const Base& b) { Base::operator=(b); return *this; }
+
+ RowVectorView_& operator=(const ELT& v) { Base::operator=(v); return *this; }
+
+ template <class EE> RowVectorView_& operator=(const RowVectorBase<EE>& m)
+ { Base::operator=(m); return*this; }
+ template <class EE> RowVectorView_& operator+=(const RowVectorBase<EE>& m)
+ { Base::operator+=(m); return*this; }
+ template <class EE> RowVectorView_& operator-=(const RowVectorBase<EE>& m)
+ { Base::operator-=(m); return*this; }
+
+ RowVectorView_& operator*=(const StdNumber& t) { Base::operator*=(t); return *this; }
+ RowVectorView_& operator/=(const StdNumber& t) { Base::operator/=(t); return *this; }
+ RowVectorView_& operator+=(const ELT& b) { this->elementwiseAddScalarInPlace(b); return *this; }
+ RowVectorView_& operator-=(const ELT& b) { this->elementwiseSubtractScalarInPlace(b); return *this; }
+
+private:
+ // NO DATA MEMBERS ALLOWED
+ RowVectorView_(); // default construction suppressed (what is it a view of?)
+};
+
+} //namespace SimTK
+
+#endif // SimTK_SIMMATRIX_ROWVECTORVIEW_H_
diff --git a/SimTKcommon/BigMatrix/include/SimTKcommon/internal/RowVector_.h b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/RowVector_.h
new file mode 100644
index 0000000..a3757cd
--- /dev/null
+++ b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/RowVector_.h
@@ -0,0 +1,122 @@
+#ifndef SimTK_SIMMATRIX_ROWVECTOR_H_
+#define SimTK_SIMMATRIX_ROWVECTOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Define the SimTK::RowVector_ class that is part of Simbody's BigMatrix
+toolset. **/
+
+namespace SimTK {
+
+//==============================================================================
+// ROW VECTOR
+//==============================================================================
+/** @brief Represents a variable size row vector; much less common than the
+column vector type Vector_.
+
+ at ingroup MatVecUtilities
+
+Row vectors are much less commonly used than column vectors; they mostly arise
+implicitly as the type of a transposed column vector (represented by Simbody's
+Vector_ class). However, you want to use rows this is the class intended to
+appear in user code. It can be a fixed-size view of someone else's data, or can
+be a resizable data owner itself, although of course it will always have just
+one row.
+
+ at see Row for handling of small, fixed-size row vectors with no runtime overhead
+ at see Matrix_ for variable %size, two-dimensional matrix.
+ at see RowVectorView_, RowVectorBase
+**/
+template <class ELT> class RowVector_ : public RowVectorBase<ELT> {
+ typedef typename CNT<ELT>::Scalar S;
+ typedef typename CNT<ELT>::Number Number;
+ typedef typename CNT<ELT>::StdNumber StdNumber;
+ typedef typename CNT<ELT>::TNeg ENeg;
+
+ typedef RowVectorBase<ELT> Base;
+ typedef RowVectorBase<ENeg> BaseNeg;
+public:
+ RowVector_() : Base() {} // 1x0 reallocatable
+ // Uses default destructor.
+
+ // Copy constructor is deep.
+ RowVector_(const RowVector_& src) : Base(src) {}
+
+ // Implicit conversions.
+ RowVector_(const Base& src) : Base(src) {} // e.g., RowVectorView
+ RowVector_(const BaseNeg& src) : Base(src) {}
+
+ // Copy assignment is deep and can be reallocating if this RowVector
+ // has no View.
+ RowVector_& operator=(const RowVector_& src) {
+ Base::operator=(src); return*this;
+ }
+
+
+ explicit RowVector_(int n) : Base(n) { }
+ RowVector_(int n, const ELT* cppInitialValues) : Base(n, cppInitialValues) {}
+ RowVector_(int n, const ELT& initialValue) : Base(n, initialValue) {}
+
+ /// Construct a Vector which uses borrowed space with assumed
+ /// element-to-element stride equal to the C++ element spacing.
+ /// Last parameter is a dummy to avoid overload conflicts when ELT=S;
+ /// pass it as "true".
+ RowVector_(int n, const S* cppData, bool): Base(n, Base::CppNScalarsPerElement, cppData) {}
+ RowVector_(int n, S* cppData, bool): Base(n, Base::CppNScalarsPerElement, cppData) {}
+
+ /// Borrowed-space construction with explicit stride supplied as
+ /// "number of scalars between elements". Last parameter is a
+ /// dummy to avoid overload conflicts; pass it as "true".
+ RowVector_(int n, int stride, const S* data, bool) : Base(n, stride, data) {}
+ RowVector_(int n, int stride, S* data, bool) : Base(n, stride, data) {}
+
+ /// Convert a Row to a RowVector_.
+ template <int M>
+ explicit RowVector_(const Row<M,ELT>& v) : Base(M) {
+ for (int i = 0; i < M; ++i)
+ this->updElt(0, i) = v(i);
+ }
+
+ RowVector_& operator=(const ELT& v) { Base::operator=(v); return *this; }
+
+ template <class EE> RowVector_& operator=(const RowVectorBase<EE>& b)
+ { Base::operator=(b); return*this; }
+ template <class EE> RowVector_& operator+=(const RowVectorBase<EE>& b)
+ { Base::operator+=(b); return*this; }
+ template <class EE> RowVector_& operator-=(const RowVectorBase<EE>& b)
+ { Base::operator-=(b); return*this; }
+
+ RowVector_& operator*=(const StdNumber& t) { Base::operator*=(t); return *this; }
+ RowVector_& operator/=(const StdNumber& t) { Base::operator/=(t); return *this; }
+ RowVector_& operator+=(const ELT& b) { this->elementwiseAddScalarInPlace(b); return *this; }
+ RowVector_& operator-=(const ELT& b) { this->elementwiseSubtractScalarInPlace(b); return *this; }
+
+private:
+ // NO DATA MEMBERS ALLOWED
+};
+
+} //namespace SimTK
+
+#endif // SimTK_SIMMATRIX_ROWVECTOR_H_
diff --git a/SimTKcommon/BigMatrix/include/SimTKcommon/internal/VectorBase.h b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/VectorBase.h
new file mode 100644
index 0000000..43fb33d
--- /dev/null
+++ b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/VectorBase.h
@@ -0,0 +1,475 @@
+#ifndef SimTK_SIMMATRIX_VECTORBASE_H_
+#define SimTK_SIMMATRIX_VECTORBASE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Define the SimTK::VectorBase class that is part of Simbody's BigMatrix
+toolset. **/
+
+namespace SimTK {
+
+//==============================================================================
+// VECTOR BASE
+//==============================================================================
+/** @brief This is a dataless rehash of the MatrixBase class to specialize it
+for Vectors.
+
+This mostly entails overriding a few of the methods. Note that all the
+MatrixBase operations remain available if you \c static_cast this up to a
+MatrixBase. **/
+template <class ELT> class VectorBase : public MatrixBase<ELT> {
+ typedef MatrixBase<ELT> Base;
+ typedef typename Base::ScalarNormSq ScalarNormSq;
+ typedef typename Base::EAbs EAbs;
+ typedef typename CNT<ELT>::Scalar Scalar;
+ typedef typename CNT<ELT>::Number Number;
+ typedef typename CNT<ELT>::StdNumber StdNumber;
+ typedef VectorBase<ELT> T;
+ typedef VectorBase<typename CNT<ELT>::TAbs> TAbs;
+ typedef VectorBase<typename CNT<ELT>::TNeg> TNeg;
+ typedef RowVectorView_<typename CNT<ELT>::THerm> THerm;
+public:
+ // ------------------------------------------------------------------------
+ /// @name VectorBase "owner" construction
+ ///
+ /// These constructors create new VectorBase objects which own their
+ /// own data and are (at least by default) resizable. The resulting matrices
+ /// are m X 1 with the number of columns locked at 1. If there is any data
+ /// allocated but not explicitly initialized, that data will be uninitialized
+ /// garbage in Release builds but will be initialized to NaN (at a performance
+ /// cost) in Debug builds.
+ /// @{
+
+ /// Default constructor makes a 0x1 matrix locked at 1 column; you can
+ /// provide an initial allocation if you want.
+ explicit VectorBase(int m=0) : Base(MatrixCommitment::Vector(), m, 1) {}
+
+ /// Copy constructor is a deep copy (not appropriate for views!). That
+ /// means it creates a new, densely packed vector whose elements are
+ /// initialized from the source object.
+ VectorBase(const VectorBase& source) : Base(source) {}
+
+ /// Implicit conversion from compatible vector with negated elements.
+ VectorBase(const TNeg& source) : Base(source) {}
+
+ /// Construct an owner vector of length m, with each element initialized to
+ /// the given value.
+ VectorBase(int m, const ELT& initialValue)
+ : Base(MatrixCommitment::Vector(),m,1,initialValue) {}
+
+ /// Construct an owner vector of length m, with the elements initialized sequentially
+ /// from a C++ array of elements which is assumed to be of length m. Note that we
+ /// are expecting C++ packing; don't use this to initialize one Simmatrix vector
+ /// from another because Simmatrix may pack its elements more densely than C++.
+ VectorBase(int m, const ELT* cppInitialValues)
+ : Base(MatrixCommitment::Vector(),m,1,cppInitialValues) {}
+ /// @}
+
+ // ------------------------------------------------------------------------
+ /// @name VectorBase construction from pre-existing data
+ ///
+ /// Construct a non-resizeable, VectorBase view of externally supplied data. Note that
+ /// stride should be interpreted as "the number of scalars between elements" and
+ /// for composite elements may have a different value if the source is a C++ array
+ /// of elements vs. a Simmatrix packed data array. We provide constructors for
+ /// both read-only and writable external data.
+ /// @{
+
+ /// Construct a read-only view of existing data.
+ VectorBase(int m, int stride, const Scalar* s)
+ : Base(MatrixCommitment::Vector(m), MatrixCharacter::Vector(m),stride,s) { }
+ /// Construct a writable view into existing data.
+ VectorBase(int m, int stride, Scalar* s)
+ : Base(MatrixCommitment::Vector(m), MatrixCharacter::Vector(m),stride,s) { }
+ /// @}
+
+ // ------------------------------------------------------------------------
+ /// @name VectorBase construction from an existing Helper.
+ ///
+ /// Create a new VectorBase from an existing helper. Both shallow (view) and deep
+ /// copies are possible. For shallow copies, there is a constructor providing a read-only
+ /// view of the original data and one providing a writable view into the original data.
+ /// @{
+
+ /// Construct a writable view into the source data.
+ VectorBase(MatrixHelper<Scalar>& h, const typename MatrixHelper<Scalar>::ShallowCopy& s)
+ : Base(MatrixCommitment::Vector(), h,s) { }
+ /// Construct a read-only view of the source data.
+ VectorBase(const MatrixHelper<Scalar>& h, const typename MatrixHelper<Scalar>::ShallowCopy& s)
+ : Base(MatrixCommitment::Vector(), h,s) { }
+ /// Construct a new owner vector initialized with the data from the source.
+ VectorBase(const MatrixHelper<Scalar>& h, const typename MatrixHelper<Scalar>::DeepCopy& d)
+ : Base(MatrixCommitment::Vector(), h,d) { }
+ /// @}
+
+ // This gives the resulting vector type when (v[i] op P) is applied to each element.
+ // It will have element types which are the regular composite result of ELT op P.
+ template <class P> struct EltResult {
+ typedef VectorBase<typename CNT<ELT>::template Result<P>::Mul> Mul;
+ typedef VectorBase<typename CNT<ELT>::template Result<P>::Dvd> Dvd;
+ typedef VectorBase<typename CNT<ELT>::template Result<P>::Add> Add;
+ typedef VectorBase<typename CNT<ELT>::template Result<P>::Sub> Sub;
+ };
+
+ /// Copy assignment is deep copy but behavior depends on type of lhs: if view, rhs
+ /// must match. If owner, we reallocate and copy rhs.
+ VectorBase& operator=(const VectorBase& b) {
+ Base::operator=(b); return *this;
+ }
+
+ // default destructor
+
+
+ VectorBase& operator*=(const StdNumber& t) { Base::operator*=(t); return *this; }
+ VectorBase& operator/=(const StdNumber& t) { Base::operator/=(t); return *this; }
+ VectorBase& operator+=(const VectorBase& r) { Base::operator+=(r); return *this; }
+ VectorBase& operator-=(const VectorBase& r) { Base::operator-=(r); return *this; }
+
+
+ template <class EE> VectorBase& operator=(const VectorBase<EE>& b)
+ { Base::operator=(b); return *this; }
+ template <class EE> VectorBase& operator+=(const VectorBase<EE>& b)
+ { Base::operator+=(b); return *this; }
+ template <class EE> VectorBase& operator-=(const VectorBase<EE>& b)
+ { Base::operator-=(b); return *this; }
+
+
+ /// Fill current allocation with copies of element. Note that this is not the
+ /// same behavior as assignment for Matrices, where only the diagonal is set (and
+ /// everything else is set to zero.)
+ VectorBase& operator=(const ELT& t) { Base::setTo(t); return *this; }
+
+ /// There's only one column here so it's a bit weird to use rowScale rather than
+ /// elementwiseMultiply, but there's nothing really wrong with it. Using colScale
+ /// would be really wacky since it is the same as a scalar multiply. We won't support
+ /// colScale here except through inheritance where it will not be much use.
+ template <class EE> VectorBase& rowScaleInPlace(const VectorBase<EE>& v)
+ { Base::template rowScaleInPlace<EE>(v); return *this; }
+ template <class EE> inline void rowScale(const VectorBase<EE>& v, typename EltResult<EE>::Mul& out) const
+ { Base::rowScale(v,out); }
+ template <class EE> inline typename EltResult<EE>::Mul rowScale(const VectorBase<EE>& v) const
+ { typename EltResult<EE>::Mul out(nrow()); Base::rowScale(v,out); return out; }
+
+ /** Return the root-mean-square (RMS) norm of a Vector of scalars, with
+ optional return of the index of the element of largest absolute value.
+ The RMS norm of a Vector v of length n is rms=sqrt(~v*v/n). If n==0 we
+ define the RMS norm to be zero but return the element index as -1. **/
+ typename CNT<ScalarNormSq>::TSqrt
+ normRMS(int* worstOne=0) const {
+ if (!CNT<ELT>::IsScalar)
+ SimTK_THROW1(Exception::Cant,
+ "Vector::normRMS() only defined for scalar elements.");
+ const int n = nrow();
+ if (n == 0) {
+ if (worstOne) *worstOne = -1;
+ return typename CNT<ScalarNormSq>::TSqrt(0);
+ }
+
+ ScalarNormSq sumsq = 0;
+ if (worstOne) {
+ *worstOne = 0;
+ ScalarNormSq maxsq = 0;
+ for (int i=0; i<n; ++i) {
+ const ScalarNormSq v2 = square((*this)[i]);
+ if (v2 > maxsq) maxsq=v2, *worstOne=i;
+ sumsq += v2;
+ }
+ } else { // don't track the worst element
+ for (int i=0; i<n; ++i) {
+ const ScalarNormSq v2 = square((*this)[i]);
+ sumsq += v2;
+ }
+ }
+
+ return CNT<ScalarNormSq>::sqrt(sumsq/n);
+ }
+
+ /** Return the weighted root-mean-square (WRMS) norm of a Vector of
+ scalars, with optional return of the index of the weighted element of
+ largest absolute value. The WRMS norm of a Vector v of length n with
+ weights w is wrms=sqrt(sum_i((w_i*v_i)^2))/n). If n==0 we
+ define the WRMS norm to be zero but return the element index as -1. **/
+ template <class EE>
+ typename CNT<ScalarNormSq>::TSqrt
+ weightedNormRMS(const VectorBase<EE>& w, int* worstOne=0) const {
+ if (!CNT<ELT>::IsScalar || !CNT<EE>::IsScalar)
+ SimTK_THROW1(Exception::Cant,
+ "Vector::weightedNormRMS() only defined for scalar elements"
+ " and weights.");
+ const int n = nrow();
+ assert(w.nrow()==n);
+ if (n == 0) {
+ if (worstOne) *worstOne = -1;
+ return typename CNT<ScalarNormSq>::TSqrt(0);
+ }
+
+ ScalarNormSq sumsq = 0;
+ if (worstOne) {
+ *worstOne = 0;
+ ScalarNormSq maxsq = 0;
+ for (int i=0; i<n; ++i) {
+ const ScalarNormSq wv2 = square(w[i]*(*this)[i]);
+ if (wv2 > maxsq) maxsq=wv2, *worstOne=i;
+ sumsq += wv2;
+ }
+ } else { // don't track the worst element
+ for (int i=0; i<n; ++i) {
+ const ScalarNormSq wv2 = square(w[i]*(*this)[i]);
+ sumsq += wv2;
+ }
+ }
+
+ return CNT<ScalarNormSq>::sqrt(sumsq/n);
+ }
+
+ /** Return the infinity norm (max absolute value) of a Vector of scalars,
+ with optional return of the index of the element of largest absolute value.
+ The Inf norm of a Vector v is inf=max_i(|v_i|). If n==0 we
+ define the Inf norm to be zero but return the element index as -1. **/
+ EAbs normInf(int* worstOne=0) const {
+ if (!CNT<ELT>::IsScalar)
+ SimTK_THROW1(Exception::Cant,
+ "Vector::normInf() only defined for scalar elements.");
+ const int n = nrow();
+ if (n == 0) {
+ if (worstOne) *worstOne = -1;
+ return EAbs(0);
+ }
+
+ EAbs maxabs = 0;
+ if (worstOne) {
+ *worstOne = 0;
+ for (int i=0; i<n; ++i) {
+ const EAbs a = std::abs((*this)[i]);
+ if (a > maxabs) maxabs=a, *worstOne=i;
+ }
+ } else { // don't track the worst element
+ for (int i=0; i<n; ++i) {
+ const EAbs a = std::abs((*this)[i]);
+ if (a > maxabs) maxabs=a;
+ }
+ }
+
+ return maxabs;
+ }
+
+ /** Return the weighted infinity norm (max absolute value) WInf of a Vector
+ of scalars, with optional return of the index of the weighted element of
+ largest absolute value. The WInf norm of a Vector v of length n with
+ weights w is winf=max_i(|w_i*v_i|). If n==0 we
+ define the WInf norm to be zero but return the element index as -1. **/
+ template <class EE>
+ EAbs weightedNormInf(const VectorBase<EE>& w, int* worstOne=0) const {
+ if (!CNT<ELT>::IsScalar || !CNT<EE>::IsScalar)
+ SimTK_THROW1(Exception::Cant,
+ "Vector::weightedNormInf() only defined for scalar elements"
+ " and weights.");
+ const int n = nrow();
+ assert(w.nrow()==n);
+ if (n == 0) {
+ if (worstOne) *worstOne = -1;
+ return EAbs(0);
+ }
+
+ EAbs maxabs = 0;
+ if (worstOne) {
+ *worstOne = 0;
+ for (int i=0; i<n; ++i) {
+ const EAbs wv = std::abs(w[i]*(*this)[i]);
+ if (wv > maxabs) maxabs=wv, *worstOne=i;
+ }
+ } else { // don't track the worst element
+ for (int i=0; i<n; ++i) {
+ const EAbs wv = std::abs(w[i]*(*this)[i]);
+ if (wv > maxabs) maxabs=wv;
+ }
+ }
+
+ return maxabs;
+ }
+
+ /// Set this[i] = this[i]^-1.
+ VectorBase& elementwiseInvertInPlace() {
+ Base::elementwiseInvertInPlace();
+ return *this;
+ }
+
+ /// Set supplied out[i] = this[i]^-1
+ void elementwiseInvert(VectorBase<typename CNT<ELT>::TInvert>& out) const {
+ Base::elementwiseInvert(out);
+ }
+
+ /// Return out[i]=this[i]^-1 as function return.
+ VectorBase<typename CNT<ELT>::TInvert> elementwiseInvert() const {
+ VectorBase<typename CNT<ELT>::TInvert> out(nrow());
+ Base::elementwiseInvert(out);
+ return out;
+ }
+
+ // elementwise multiply
+ template <class EE> VectorBase& elementwiseMultiplyInPlace(const VectorBase<EE>& r)
+ { Base::template elementwiseMultiplyInPlace<EE>(r); return *this; }
+ template <class EE> inline void elementwiseMultiply(const VectorBase<EE>& v, typename EltResult<EE>::Mul& out) const
+ { Base::template elementwiseMultiply<EE>(v,out); }
+ template <class EE> inline typename EltResult<EE>::Mul elementwiseMultiply(const VectorBase<EE>& v) const
+ { typename EltResult<EE>::Mul out(nrow()); Base::template elementwiseMultiply<EE>(v,out); return out; }
+
+ // elementwise multiply from left
+ template <class EE> VectorBase& elementwiseMultiplyFromLeftInPlace(const VectorBase<EE>& r)
+ { Base::template elementwiseMultiplyFromLeftInPlace<EE>(r); return *this; }
+ template <class EE> inline void
+ elementwiseMultiplyFromLeft(
+ const VectorBase<EE>& v,
+ typename VectorBase<EE>::template EltResult<ELT>::Mul& out) const
+ {
+ Base::template elementwiseMultiplyFromLeft<EE>(v,out);
+ }
+ template <class EE> inline typename VectorBase<EE>::template EltResult<ELT>::Mul
+ elementwiseMultiplyFromLeft(const VectorBase<EE>& v) const
+ {
+ typename VectorBase<EE>::template EltResult<ELT>::Mul out(nrow());
+ Base::template elementwiseMultiplyFromLeft<EE>(v,out);
+ return out;
+ }
+
+ // elementwise divide
+ template <class EE> VectorBase& elementwiseDivideInPlace(const VectorBase<EE>& r)
+ { Base::template elementwiseDivideInPlace<EE>(r); return *this; }
+ template <class EE> inline void elementwiseDivide(const VectorBase<EE>& v, typename EltResult<EE>::Dvd& out) const
+ { Base::template elementwiseDivide<EE>(v,out); }
+ template <class EE> inline typename EltResult<EE>::Dvd elementwiseDivide(const VectorBase<EE>& v) const
+ { typename EltResult<EE>::Dvd out(nrow()); Base::template elementwiseDivide<EE>(v,out); return out; }
+
+ // elementwise divide from left
+ template <class EE> VectorBase& elementwiseDivideFromLeftInPlace(const VectorBase<EE>& r)
+ { Base::template elementwiseDivideFromLeftInPlace<EE>(r); return *this; }
+ template <class EE> inline void
+ elementwiseDivideFromLeft(
+ const VectorBase<EE>& v,
+ typename VectorBase<EE>::template EltResult<ELT>::Dvd& out) const
+ {
+ Base::template elementwiseDivideFromLeft<EE>(v,out);
+ }
+ template <class EE> inline typename VectorBase<EE>::template EltResult<ELT>::Dvd
+ elementwiseDivideFromLeft(const VectorBase<EE>& v) const
+ {
+ typename VectorBase<EE>::template EltResult<ELT>::Dvd out(nrow());
+ Base::template elementwiseDivideFromLeft<EE>(v,out);
+ return out;
+ }
+
+ // Implicit conversions are allowed to Vector or Matrix, but not to RowVector.
+ operator const Vector_<ELT>&() const { return *reinterpret_cast<const Vector_<ELT>*>(this); }
+ operator Vector_<ELT>&() { return *reinterpret_cast< Vector_<ELT>*>(this); }
+ operator const VectorView_<ELT>&() const { return *reinterpret_cast<const VectorView_<ELT>*>(this); }
+ operator VectorView_<ELT>&() { return *reinterpret_cast< VectorView_<ELT>*>(this); }
+
+ operator const Matrix_<ELT>&() const { return *reinterpret_cast<const Matrix_<ELT>*>(this); }
+ operator Matrix_<ELT>&() { return *reinterpret_cast< Matrix_<ELT>*>(this); }
+ operator const MatrixView_<ELT>&() const { return *reinterpret_cast<const MatrixView_<ELT>*>(this); }
+ operator MatrixView_<ELT>&() { return *reinterpret_cast< MatrixView_<ELT>*>(this); }
+
+
+ // size() for Vectors is Base::nelt() but returns int instead of ptrdiff_t.
+ int size() const {
+ assert(Base::nelt() <= (ptrdiff_t)std::numeric_limits<int>::max());
+ assert(Base::ncol()==1);
+ return (int)Base::nelt();
+ }
+ int nrow() const {assert(Base::ncol()==1); return Base::nrow();}
+ int ncol() const {assert(Base::ncol()==1); return Base::ncol();}
+ ptrdiff_t nelt() const {assert(Base::ncol()==1); return Base::nelt();}
+
+ // Override MatrixBase operators to return the right shape
+ TAbs abs() const {TAbs result; Base::abs(result); return result;}
+
+ // Override MatrixBase indexing operators
+ const ELT& operator[](int i) const {return *reinterpret_cast<const ELT*>(Base::getHelper().getElt(i));}
+ ELT& operator[](int i) {return *reinterpret_cast<ELT*> (Base::updHelper().updElt(i));}
+ const ELT& operator()(int i) const {return *reinterpret_cast<const ELT*>(Base::getHelper().getElt(i));}
+ ELT& operator()(int i) {return *reinterpret_cast<ELT*> (Base::updHelper().updElt(i));}
+
+ // Block (contiguous subvector) view creation
+ VectorView_<ELT> operator()(int i, int m) const {return Base::operator()(i,0,m,1).getAsVectorView();}
+ VectorView_<ELT> operator()(int i, int m) {return Base::operator()(i,0,m,1).updAsVectorView();}
+
+ // Indexed view creation (arbitrary subvector). Indices must be
+ // monotonically increasing.
+ VectorView_<ELT> index(const Array_<int>& indices) const {
+ MatrixHelper<Scalar> h(Base::getHelper().getCharacterCommitment(),
+ Base::getHelper(), indices);
+ return VectorView_<ELT>(h);
+ }
+ VectorView_<ELT> updIndex(const Array_<int>& indices) {
+ MatrixHelper<Scalar> h(Base::getHelper().getCharacterCommitment(),
+ Base::updHelper(), indices);
+ return VectorView_<ELT>(h);
+ }
+
+ VectorView_<ELT> operator()(const Array_<int>& indices) const {return index(indices);}
+ VectorView_<ELT> operator()(const Array_<int>& indices) {return updIndex(indices);}
+
+ // Hermitian transpose.
+ THerm transpose() const {return Base::transpose().getAsRowVectorView();}
+ THerm updTranspose() {return Base::updTranspose().updAsRowVectorView();}
+
+ THerm operator~() const {return transpose();}
+ THerm operator~() {return updTranspose();}
+
+ const VectorBase& operator+() const {return *this; }
+
+ // Negation
+
+ const TNeg& negate() const {return *reinterpret_cast<const TNeg*>(this); }
+ TNeg& updNegate() {return *reinterpret_cast<TNeg*>(this); }
+
+ const TNeg& operator-() const {return negate();}
+ TNeg& operator-() {return updNegate();}
+
+ VectorBase& resize(int m) {Base::resize(m,1); return *this;}
+ VectorBase& resizeKeep(int m) {Base::resizeKeep(m,1); return *this;}
+
+ //TODO: this is not re-locking the number of columns at 1.
+ void clear() {Base::clear(); Base::resize(0,1);}
+
+ ELT sum() const {ELT s; Base::getHelper().sum(reinterpret_cast<Scalar*>(&s)); return s; } // add all the elements
+ VectorIterator<ELT, VectorBase<ELT> > begin() {
+ return VectorIterator<ELT, VectorBase<ELT> >(*this, 0);
+ }
+ VectorIterator<ELT, VectorBase<ELT> > end() {
+ return VectorIterator<ELT, VectorBase<ELT> >(*this, size());
+ }
+
+protected:
+ // Create a VectorBase handle using a given helper rep.
+ explicit VectorBase(MatrixHelperRep<Scalar>* hrep) : Base(hrep) {}
+
+private:
+ // NO DATA MEMBERS ALLOWED
+};
+
+} //namespace SimTK
+
+#endif // SimTK_SIMMATRIX_VECTORBASE_H_
diff --git a/SimTKcommon/BigMatrix/include/SimTKcommon/internal/VectorIterator.h b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/VectorIterator.h
new file mode 100644
index 0000000..61a1d69
--- /dev/null
+++ b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/VectorIterator.h
@@ -0,0 +1,142 @@
+#ifndef SimTK_SIMMATRIX_VECTORITERATOR_H_
+#define SimTK_SIMMATRIX_VECTORITERATOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-13 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Defines an iterator that can be used to iterate over the elements of any
+kind of Simbody vector. **/
+
+#include <cstddef>
+
+namespace SimTK {
+
+//==============================================================================
+// VECTOR ITERATOR
+//==============================================================================
+/** @brief This is an iterator for iterating over the elements of a Vector_
+or Vec object.
+
+ at tparam ELT The type of an element stored in the vector whose type
+ is given in \p VECTOR_CLASS.
+ at tparam VECTOR_CLASS The type of vector container to iterate through. Its
+ element type must be \p ELT.
+
+This random access iterator can be used with any container that supports
+random-access indexing and a <code>size()</code> method. However, the intent is
+for use internally to allow writing a variety of vector math functions without
+having to specialize them for the various flavors of vector we support.
+**/
+template <class ELT, class VECTOR_CLASS>
+class VectorIterator {
+public:
+ typedef ELT value_type;
+ typedef ptrdiff_t difference_type;
+ typedef ELT& reference;
+ typedef ELT* pointer;
+ typedef std::random_access_iterator_tag iterator_category;
+ VectorIterator(VECTOR_CLASS& vector, ptrdiff_t index)
+ : vector(vector), index(index) {}
+ VectorIterator(const VectorIterator& iter)
+ : vector(iter.vector), index(iter.index) {}
+ VectorIterator& operator=(const VectorIterator& iter) {
+ vector = iter.vector;
+ index = iter.index;
+ return *this;
+ }
+ ELT& operator*() {
+ assert (index >= 0 && index < vector.size());
+ return vector[(int)index];
+ }
+ ELT& operator[](ptrdiff_t i) {
+ assert (i >= 0 && i < vector.size());
+ return vector[(int)i];
+ }
+ VectorIterator operator++() {
+ assert (index < vector.size());
+ ++index;
+ return *this;
+ }
+ VectorIterator operator++(int) {
+ assert (index < vector.size());
+ VectorIterator current = *this;
+ ++index;
+ return current;
+ }
+ VectorIterator operator--() {
+ assert (index > 0);
+ --index;
+ return *this;
+ }
+ VectorIterator operator--(int) {
+ assert (index > 0);
+ VectorIterator current = *this;
+ --index;
+ return current;
+ }
+ VectorIterator operator+=(ptrdiff_t n) {
+ assert (0 <= index+n && index+n <= vector.size());
+ index += n;
+ return *this;
+ }
+ VectorIterator operator-=(ptrdiff_t n) {
+ assert (0 <= index-n && index-n <= vector.size());
+ index -= n;
+ return *this;
+ }
+ bool operator<(VectorIterator iter) const {
+ return (index < iter.index);
+ }
+ bool operator>(VectorIterator iter) const {
+ return (index > iter.index);
+ }
+ bool operator<=(VectorIterator iter) const {
+ return (index <= iter.index);
+ }
+ bool operator>=(VectorIterator iter) const {
+ return (index >= iter.index);
+ }
+ ptrdiff_t operator-(VectorIterator iter) const {
+ return (index - iter.index);
+ }
+ VectorIterator operator-(ptrdiff_t n) const {
+ return VectorIterator(vector, index-n);
+ }
+ VectorIterator operator+(ptrdiff_t n) const {
+ return VectorIterator(vector, index+n);
+ }
+ bool operator==(VectorIterator iter) const {
+ return (index == iter.index);
+ }
+ bool operator!=(VectorIterator iter) const {
+ return (index != iter.index);
+ }
+private:
+ VECTOR_CLASS& vector;
+ ptrdiff_t index;
+};
+
+} //namespace SimTK
+
+#endif // SimTK_SIMMATRIX_VECTORITERATOR_H_
diff --git a/SimTKcommon/BigMatrix/include/SimTKcommon/internal/VectorView_.h b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/VectorView_.h
new file mode 100644
index 0000000..fbc3f96
--- /dev/null
+++ b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/VectorView_.h
@@ -0,0 +1,95 @@
+#ifndef SimTK_SIMMATRIX_VECTORVIEW_H_
+#define SimTK_SIMMATRIX_VECTORVIEW_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Define the SimTK::VectorView_ class that is part of Simbody's BigMatrix
+toolset. **/
+
+namespace SimTK {
+
+//==============================================================================
+// VECTOR VIEW
+//==============================================================================
+/** @brief (Advanced) This class is identical to Vector_ except that it has
+shallow (reference) copy and assignment semantics.
+
+Despite the name, this may be an owner if a Vector_ is recast to a %VectorView_.
+However, there are no owner constructors for %VectorView_.
+ at see Vector_, VectorBase, MatrixView_ **/
+template <class ELT> class VectorView_ : public VectorBase<ELT> {
+ typedef VectorBase<ELT> Base;
+ typedef typename CNT<ELT>::Scalar S;
+ typedef typename CNT<ELT>::Number Number;
+ typedef typename CNT<ELT>::StdNumber StdNumber;
+ typedef VectorView_<ELT> T;
+ typedef VectorView_< typename CNT<ELT>::TNeg > TNeg;
+ typedef RowVectorView_< typename CNT<ELT>::THerm > THerm;
+public:
+ // Default construction is suppressed.
+ // Uses default destructor.
+
+ // Create a VectorView_ handle using a given helper rep.
+ explicit VectorView_(MatrixHelperRep<S>* hrep) : Base(hrep) {}
+
+ // Copy constructor is shallow. CAUTION: despite const argument, this preserves writability
+ // if it was present in the source. This is necessary to allow temporary views to be
+ // created and used as lvalues.
+ VectorView_(const VectorView_& v)
+ : Base(const_cast<MatrixHelper<S>&>(v.getHelper()), typename MatrixHelper<S>::ShallowCopy()) { }
+
+ // Copy assignment is deep but not reallocating.
+ VectorView_& operator=(const VectorView_& v) {
+ Base::operator=(v); return *this;
+ }
+
+ // Ask for shallow copy
+ explicit VectorView_(const MatrixHelper<S>& h) : Base(h, typename MatrixHelper<S>::ShallowCopy()) { }
+ explicit VectorView_(MatrixHelper<S>& h) : Base(h, typename MatrixHelper<S>::ShallowCopy()) { }
+
+ VectorView_& operator=(const Base& b) { Base::operator=(b); return *this; }
+
+ VectorView_& operator=(const ELT& v) { Base::operator=(v); return *this; }
+
+ template <class EE> VectorView_& operator=(const VectorBase<EE>& m)
+ { Base::operator=(m); return*this; }
+ template <class EE> VectorView_& operator+=(const VectorBase<EE>& m)
+ { Base::operator+=(m); return*this; }
+ template <class EE> VectorView_& operator-=(const VectorBase<EE>& m)
+ { Base::operator-=(m); return*this; }
+
+ VectorView_& operator*=(const StdNumber& t) { Base::operator*=(t); return *this; }
+ VectorView_& operator/=(const StdNumber& t) { Base::operator/=(t); return *this; }
+ VectorView_& operator+=(const ELT& b) { this->elementwiseAddScalarInPlace(b); return *this; }
+ VectorView_& operator-=(const ELT& b) { this->elementwiseSubtractScalarInPlace(b); return *this; }
+
+private:
+ // NO DATA MEMBERS ALLOWED
+ VectorView_(); // default construction suppressed (what's it a View of?)
+};
+
+} //namespace SimTK
+
+#endif // SimTK_SIMMATRIX_VECTORVIEW_H_
diff --git a/SimTKcommon/BigMatrix/include/SimTKcommon/internal/Vector_.h b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/Vector_.h
new file mode 100644
index 0000000..6153d4d
--- /dev/null
+++ b/SimTKcommon/BigMatrix/include/SimTKcommon/internal/Vector_.h
@@ -0,0 +1,208 @@
+#ifndef SimTK_SIMMATRIX_VECTOR_H_
+#define SimTK_SIMMATRIX_VECTOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Define the SimTK::Vector_ class that is part of Simbody's BigMatrix toolset. **/
+
+namespace SimTK {
+
+//==============================================================================
+// VECTOR
+//==============================================================================
+/** @brief This is the vector class intended to appear in user code for large,
+variable size column vectors.
+
+ at ingroup MatVecUtilities
+
+More commonly, the typedef @ref SimTK::Vector "Vector" is used instead; that is
+just an abbreviation for \c Vector_<Real>.
+
+A %Vector_ can be a fixed-size view of someone else's data, or can be a
+resizable data owner itself, although of course it will always have just one
+column.
+
+ at see Vec for handling of small, fixed-size vectors with no runtime overhead.
+ at see Matrix_ for variable %size, two-dimensional matrix.
+**/
+template <class ELT> class Vector_ : public VectorBase<ELT> {
+ typedef typename CNT<ELT>::Scalar S;
+ typedef typename CNT<ELT>::Number Number;
+ typedef typename CNT<ELT>::StdNumber StdNumber;
+ typedef typename CNT<ELT>::TNeg ENeg;
+ typedef VectorBase<ELT> Base;
+ typedef VectorBase<ENeg> BaseNeg;
+public:
+ // Uses default destructor.
+
+ /** @name Owner constructors
+ These constructors create a Vector_ object that allocates new data on the
+ heap and serves as the owner of that data. Any existing data that is
+ supplied in these constructors is \e copied, not \e shared. **/
+ /**@{**/
+
+ /** Default constructor creates a 0x1, reallocatable vector. **/
+ Vector_() : Base() {}
+
+ /** Copy constructor is deep, that is the source %Vector_ is copied, not
+ referenced. **/
+ Vector_(const Vector_& src) : Base(src) {}
+
+ /** This copy constructor serves as an implicit conversion from objects of
+ the base class type (for example, a VectorView_<ELT>), to objects of this
+ Vector_<ELT> type. Note that the source object is copied, not
+ referenced. **/
+ Vector_(const Base& src) : Base(src) {} // e.g., VectorView
+ /** This copy constructor serves as an implicit conversion from objects of
+ the base class but with negated elements, to objects of this Vector_<ELT>
+ type. Note that the source object is copied, not referenced. **/
+ Vector_(const BaseNeg& src) : Base(src) {}
+
+ /** Construct a %Vector_ with a given preallocated size, but with
+ uninitialized values. In Debug builds the elements will be initialized to
+ NaN as a debugging aid, but in Release (optimized) builds they will be
+ uninitialized garbage so that we don't waste time setting them. **/
+ explicit Vector_(int m) : Base(m) { }
+ /** Construct an owner %Vector_ of a given size \a m and initialize it from
+ a C++ array of \a m ELT values pointed to by \a cppInitialValues. Note that
+ there is no way to check that the correct number of elements has been
+ provided; make sure you have supplied enough of them. **/
+ Vector_(int m, const ELT* cppInitialValues) : Base(m, cppInitialValues) {}
+ /** Construct an owner %Vector_ of a given size \a m and initialize all the
+ elements to the given ELT value \a initialValue. **/
+ Vector_(int m, const ELT& initialValue) : Base(m, initialValue) {}
+
+ /** This constructor creates a new owner %Vector_ that is the same length
+ and has a copy of the contents of a given fixed-size Vec. **/
+ template <int M>
+ explicit Vector_(const Vec<M,ELT>& v) : Base(M) {
+ for (int i = 0; i < M; ++i)
+ this->updElt(i, 0) = v(i);
+ }
+ /**@}**/
+
+ /** @name View constructors
+ These constructors create a %Vector_ object that is a view of existing
+ data that is owned by some other object. Any existing data that is
+ supplied in these constructors is \e shared, not \e copied, so writing
+ to the created %Vector_ view modifies the original data. **/
+ /**@{**/
+
+ /** Construct a %Vector_ which shares read-only, borrowed space with assumed
+ element-to-element stride equal to the C++ element spacing. Last parameter
+ is a required dummy to avoid overload conflicts when ELT=S; pass it as
+ "true". **/
+ Vector_(int m, const S* cppData, bool)
+ : Base(m, Base::CppNScalarsPerElement, cppData) {}
+ /** Construct a %Vector_ which shares writable, borrowed space with assumed
+ element-to-element stride equal to the C++ element spacing. Last parameter
+ is a required dummy to avoid overload conflicts when ELT=S; pass it as
+ "true". **/
+ Vector_(int m, S* cppData, bool)
+ : Base(m, Base::CppNScalarsPerElement, cppData) {}
+
+ /** Borrowed-space read-only construction with explicit stride supplied as
+ "number of scalars between elements". Last parameter is a dummy to avoid
+ overload conflicts; pass it as "true". **/
+ Vector_(int m, int stride, const S* data, bool)
+ : Base(m, stride, data) {}
+ /** Borrowed-space writable construction with explicit stride supplied as
+ "number of scalars between elements". Last parameter is a dummy to avoid
+ overload conflicts; pass it as "true". **/
+ Vector_(int m, int stride, S* data, bool)
+ : Base(m, stride, data) {}
+ /**@}**/
+
+ /** @name Assignment operators **/
+ /**@{**/
+ /** Copy assignment is deep and can be reallocating if this %Vector_ is an
+ owner. Otherwise the source is copied into the destination view. **/
+ Vector_& operator=(const Vector_& src)
+ { Base::operator=(src); return*this; }
+
+ /** Like copy assignment but the source can be any object of the base
+ type, even with a different element type \a EE as long as that element
+ type is assignment-compatible to an element of our type \a ELT. **/
+ template <class EE> Vector_& operator=(const VectorBase<EE>& src)
+ { Base::operator=(src); return*this; }
+
+ /** Set all elements of this %Vector_ to the same value \a v. **/
+ Vector_& operator=(const ELT& v) { Base::operator=(v); return *this; }
+
+ /** Add in a conforming vector of the base type even if it has a different
+ element type \a EE, provided that element type is add-compatible with
+ our element type \a ELT, meaning that ELT+=EE is allowed. **/
+ template <class EE> Vector_& operator+=(const VectorBase<EE>& m)
+ { Base::operator+=(m); return*this; }
+ /** In-place subtract of a conforming vector of the base type even if it has
+ a different element type \a EE, provided that element type is
+ subtract-compatible with our element type \a ELT, meaning that ELT-=EE is
+ allowed. **/
+ template <class EE> Vector_& operator-=(const VectorBase<EE>& m)
+ { Base::operator-=(m); return*this; }
+
+ /** In-place multiply of each element of this %Vector_ by a scalar \a t.
+ Returns a reference to the now-modified %Vector_. **/
+ Vector_& operator*=(const StdNumber& t) { Base::operator*=(t); return *this; }
+ /** In-place divide of each element of this %Vector_ by a scalar \a t.
+ Returns a reference to the now-modified %Vector_. **/
+ Vector_& operator/=(const StdNumber& t) { Base::operator/=(t); return *this; }
+
+ /** In-place add to each element of this %Vector_ the same given value \a b.
+ Returns a reference to the now-modified %Vector_. This is the same
+ operation as elementwiseAddScalarInPlace(). **/
+ Vector_& operator+=(const ELT& b)
+ { this->elementwiseAddScalarInPlace(b); return *this; }
+ /** In-place subtract from each element of this %Vector_ the same given
+ value \a b. Returns a reference to the now-modified %Vector_. This is the
+ same operation as elementwiseSubtractScalarInPlace(). **/
+ Vector_& operator-=(const ELT& b)
+ { this->elementwiseSubtractScalarInPlace(b); return *this; }
+ /**@}**/
+
+ /** @name Operator equivalents for scripting
+ Functions to be used for Scripting in MATLAB and languages that do not
+ support operator overloading. **/
+ /**@{**/
+ /** toString() returns a string representation of the %Vector_. Please refer
+ to operator<< for details. **/
+ std::string toString() const {
+ std::stringstream stream;
+ stream << (*this) ;
+ return stream.str();
+ }
+ /** Variant of operator[] that's scripting friendly to get ith element. **/
+ const ELT& get(int i) const { return (*this)[i]; }
+ /** Variant of operator[] that's scripting friendly to set ith element. **/
+ void set(int i, const ELT& value) { (*this)[i]=value; }
+ /**@}**/
+
+private:
+ // NO DATA MEMBERS ALLOWED
+};
+
+} //namespace SimTK
+
+#endif // SimTK_SIMMATRIX_VECTOR_H_
diff --git a/SimTKcommon/BigMatrix/src/ElementFilter.cpp b/SimTKcommon/BigMatrix/src/ElementFilter.cpp
new file mode 100644
index 0000000..5fb3d1d
--- /dev/null
+++ b/SimTKcommon/BigMatrix/src/ElementFilter.cpp
@@ -0,0 +1,40 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/internal/common.h"
+
+#include "ElementFilter.h"
+
+namespace SimTK {
+
+
+std::ostream& operator<<(std::ostream& o, const RowCol& rc) {
+ return o << "(" << rc.row << "," << rc.col << ")";
+}
+
+std::ostream& operator<<(std::ostream& o, const NRowCol& s) {
+ return o << "[" << s.nrow << "x" << s.ncol << "]";
+}
+
+} // namespace SimTK
+
diff --git a/SimTKcommon/BigMatrix/src/ElementFilter.h b/SimTKcommon/BigMatrix/src/ElementFilter.h
new file mode 100644
index 0000000..5d70c73
--- /dev/null
+++ b/SimTKcommon/BigMatrix/src/ElementFilter.h
@@ -0,0 +1,316 @@
+#ifndef SimTK_SIMMATRIX_ELEMENT_FILTER_H_
+#define SimTK_SIMMATRIX_ELEMENT_FILTER_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This declares the ElementFilter class (TODO: this should be an abstract
+ * class with concrete derived classes implementing different filtering
+ * strategies.)
+ *
+ * These classes are part of the hidden implementation of MatrixHandle
+ * and are not visible to user programs.
+ */
+
+#include "SimTKcommon/Scalar.h"
+#include "SimTKcommon/SmallMatrix.h"
+
+#include "SimTKcommon/internal/BigMatrix.h"
+
+
+namespace SimTK {
+
+
+
+struct RowCol {
+ RowCol(int r, int c) : row(r), col(c) {}
+
+ RowCol transpose() const {return RowCol(col,row);}
+
+ int row, col;
+
+ RowCol& operator+=(const RowCol& rc) {row+=rc.row; col+=rc.col; return *this;}
+ RowCol& operator-=(const RowCol& rc) {row-=rc.row; col-=rc.col; return *this;}
+};
+
+std::ostream& operator<<(std::ostream&, const RowCol&);
+
+struct NRowCol {
+ NRowCol() : nrow(0), ncol(0) {}
+ NRowCol(int nr, int nc) : nrow(nr), ncol(nc) {}
+
+ RowCol firstElt() const {return RowCol(0,0);}
+ RowCol lastElt() const {return RowCol(nrow-1,ncol-1);}
+
+ NRowCol transpose() const {return NRowCol(ncol,nrow);}
+
+ int nrow, ncol;
+};
+
+inline bool operator==(const NRowCol& left, const NRowCol& right)
+{ return left.nrow == right.nrow && left.ncol == right.ncol; }
+inline bool operator!=(const NRowCol& left, const NRowCol& right)
+{ return left.nrow != right.nrow || left.ncol != right.ncol; }
+inline bool operator<(const NRowCol& left, const NRowCol& right)
+{ return left.nrow < right.nrow && left.ncol < right.ncol; }
+inline bool operator<=(const NRowCol& left, const NRowCol& right)
+{ return left.nrow <= right.nrow && left.ncol <= right.ncol; }
+inline bool operator>(const NRowCol& left, const NRowCol& right)
+{ return left.nrow > right.nrow && left.ncol > right.ncol; }
+inline bool operator>=(const NRowCol& left, const NRowCol& right)
+{ return left.nrow >= right.nrow && left.ncol >= right.ncol; }
+
+std::ostream& operator<<(std::ostream&, const NRowCol&);
+
+inline RowCol operator+(const RowCol& left, const RowCol& right) {
+ return RowCol(left) += right;
+}
+
+inline RowCol operator-(const RowCol& left, const RowCol& right) {
+ return RowCol(left) -= right;
+}
+
+inline bool operator==(const RowCol& left, const RowCol& right)
+{ return left.row == right.row && left.col == right.col; }
+inline bool operator!=(const RowCol& left, const RowCol& right)
+{ return left.row != right.row || left.col != right.col; }
+inline bool operator<(const RowCol& left, const RowCol& right)
+{ return left.row < right.row && left.col < right.col; }
+inline bool operator<=(const RowCol& left, const RowCol& right)
+{ return left.row <= right.row && left.col <= right.col; }
+inline bool operator>(const RowCol& left, const RowCol& right)
+{ return left.row > right.row && left.col > right.col; }
+inline bool operator>=(const RowCol& left, const RowCol& right)
+{ return left.row >= right.row && left.col >= right.col; }
+
+//------------------------------- EltIndexer -----------------------------------
+//
+// For a matrix whose elements are regularly spaced with respect to row and
+// column indices (i,j), this class captures the spacing *in elements* between
+// elements accessed by element index. We represent these like partial
+// derivatives dr/di, dr/dj, dc/di, dc/dj where the row and column spacings
+// (r,c) are in elements. Note that to be regular each of these values must
+// be independent of the current values of i and j.
+//------------------------------------------------------------------------------
+class EltIndexer {
+public:
+ EltIndexer() : drdi(1), drdj(0), dcdi(0), dcdj(1) {} // no-op
+
+ EltIndexer(int drdi, int drdj, int dcdi, int dcdj)
+ : drdi(drdi), drdj(drdj), dcdi(dcdi), dcdj(dcdj) {}
+
+ // Return an indexer in which the dependencies on i and j are reversed.
+ EltIndexer transpose() const
+ { return EltIndexer(drdj,drdi,dcdj,dcdi); }
+
+ // Apply a new indexer to this one to produce a single indexer with
+ // the composite effect (r,c) = post( this(i,j) ).
+ EltIndexer postIndexBy(const EltIndexer& post) const {
+ return EltIndexer(drdi*post.drdi + drdj*post.dcdi,
+ drdj*post.dcdj + drdi*post.drdj,
+ dcdi*post.drdi + dcdj*post.dcdi,
+ dcdj*post.dcdj + dcdi*post.drdj);
+ }
+
+ // Returns true if this indexer maps (i,j)->(i,j) and is thus a no-op.
+ bool doesNothing() const
+ { return drdi==1 && drdj==0 && dcdi==0 && dcdj==1; }
+
+ // Returns true if this indexer maps (i,j)->(j,i) and is thus a pure transpose.
+ bool isTransposeOnly() const
+ { return drdi==0 && drdj==1 && dcdi==1 && dcdj==0; }
+
+ // Given element index (i,j) relative to this view, return element
+ // indices (row,col) from which the data object can retrieve the
+ // desired element.
+ int row(int i, int j) const {return drdi*i + drdj*j;}
+ int col(int i, int j) const {return dcdi*i + dcdj*j;}
+
+private:
+ int drdi, drdj; // row selection
+ int dcdi, dcdj; // column selection
+};
+
+//---------------------------------- EltBlock ----------------------------------
+//
+// This selects a sub-block of a matrix by giving the element index of its
+// upper-left-hand element, and the size of the block.
+//------------------------------------------------------------------------------
+class EltBlock {
+public:
+ EltBlock(int nr, int nc)
+ : r0(0), c0(0), nr(nr), nc(nc)
+ { assert(nr>=0 && nc>=0); }
+ EltBlock(int r0, int c0, int nr, int nc)
+ : r0(r0), c0(c0), nr(nr), nc(nc)
+ { assert(r0>=0 && c0>=0 && nr>=0 && nc>=0); }
+
+ // Returns true if, for a matrix of the supplied dimensions, this block
+ // would show the whole thing.
+ bool isWholeMatrix(int nrow, int ncol) const
+ { return r0==0 && c0==0 && nr==nrow && nc==ncol; }
+
+ int row0() const {return r0;}
+ int col0() const {return c0;}
+ int nrow() const {return nr;}
+ int ncol() const {return nc;}
+ ptrdiff_t nelt() const {return ptrdiff_t(nr)*nc;}
+
+private:
+ int r0, c0;
+ int nr, nc;
+};
+
+/**
+ * Describe a subset of the elements of a DataDescriptor object referenced by
+ * the same MatrixHelper that references this ElementFilter object. Note that
+ * this is done entirely in terms of logical "elements" composed of one or more
+ * scalars -- this class is NOT templatized by element type.
+ *
+ * The DataDescriptor object contains only scalars. It is guaranteed
+ * that the storage for an element is composed of consecutive scalars, but
+ * otherwise we don't know anything about them here. Note that different views of
+ * the same data can claim elements of different sizes as long as the underlying
+ * scalar types match. (That would be used, for example, to select the real or
+ * imaginary submatrix of a complex matrix.)
+ *
+ * A MatrixBase object does not have to contain an ElementFilter object if it
+ * permits unfettered access to all the data elements. In that case all MatrixBase
+ * operations are forwarded directly to the DataDescriptor along with the
+ * element size we think we're looking at; otherwise the ElementFilter
+ * object must serve as an intermediary, slowing things down.
+ *
+ * This object expects to be intimately coupled with a DataDescriptor object
+ * via the MatrixHelper. Don't move it around separately!
+ */
+class ElementFilter {
+public:
+ /**
+ * This class handles the mundane details of mapping from logical, element-oriented
+ * view indices to physical but still element-oriented indices of the
+ * DataDescriptor object, which handles the final mapping of indices to
+ * memory address.
+ */
+ class Indexer {
+ public:
+ // All arguments refer to *elements*, not *scalars*.
+ Indexer(int r, int c, int drdx=1, int drdy=0, int dcdx=0, int dcdy=1)
+ : r0(r), c0(c), drdx(drdx), drdy(drdy), dcdx(dcdx), dcdy(dcdy)
+ {
+ }
+
+ // Compose a physical indexer (old) with a relative one (that is, an
+ // indexer relative to this view) to produce a new physical one
+ // suitable for use on the original data.
+ Indexer(const Indexer& old, const Indexer& ix)
+ : r0(old.row(ix.r0,ix.c0)), c0(old.col(ix.r0,ix.c0)),
+ drdx(old.drdx*ix.drdx + old.drdy*ix.dcdx),
+ drdy(old.drdy*ix.dcdy + old.drdx*ix.drdy),
+ dcdx(old.dcdx*ix.drdx + old.dcdy*ix.dcdx),
+ dcdy(old.dcdy*ix.dcdy + old.dcdx*ix.drdy)
+ {
+ }
+
+ // Given element index (x,y) relative to this view, return element
+ // indices (row,col) from which the data object can retrieve the
+ // desired element.
+ int row(int x, int y) const { return r0 + drdx*x + drdy*y; }
+ int col(int x, int y) const { return c0 + dcdx*x + dcdy*y; }
+
+ // Make an indexer just like this one but with the roles of x and y reversed.
+ // Still has same (0,0) element.
+ Indexer transpose() {
+ return Indexer(r0,c0,drdy,drdx,dcdy,dcdx);
+ }
+
+ private:
+ int r0,c0; // indices of (0,0) element
+ int drdx, drdy; // row selection
+ int dcdx, dcdy; // column selection
+
+ // no default construction
+ Indexer();
+ };
+
+ // Like copy constructor but can *reduce* writability. Can't create writability
+ // where none was permitted before, however.
+ ElementFilter(ElementFilter& v, bool wrt)
+ : writable(v.writable && wrt), nr(v.nr), nc(v.nc), indexer(v.indexer)
+ {
+ }
+
+ // Copy constructor takes a const ElementFilter and loses writability.
+ ElementFilter(const ElementFilter& v)
+ : writable(false), nr(v.nr), nc(v.nc), indexer(v.indexer)
+ {
+ }
+
+ // This is the basic constructor, giving the logical shape, writability
+ // and an indexer to use to extract the elements from the original data.
+ ElementFilter(bool wrt, int m, int n, const Indexer& ix)
+ : writable(wrt), nr(m), nc(n), indexer(ix)
+ {
+ }
+
+ // Offset constructor -- combine an old one and new instructions to get
+ // another ElementFilter suitable for use with the original data.
+ ElementFilter(const ElementFilter& old, bool wrt, int m, int n,
+ const Indexer& ix)
+ : writable(wrt), nr(m), nc(n), indexer(old.indexer, ix)
+ {
+ }
+
+ ElementFilter* clone() const {return new ElementFilter(*this);}
+
+ int nrow() const { return nr; }
+ int ncol() const { return nc; }
+ size_t nelt() const { return (size_t)nr*nc; }
+
+ const Indexer& getIndexer() const {return indexer;}
+
+ void setIndexer(const Indexer& newIndexer) {indexer=newIndexer;}
+
+ bool isViewWritable() const { return writable; }
+
+ int r(int i, int j) const { assert(i<nr&&j<nc); return indexer.row(i,j); }
+ int c(int i, int j) const { assert(i<nr&&j<nc); return indexer.col(i,j); }
+ void rc(int i, int j, int& r, int& c) const
+ { assert(i<nr&&j<nc); r=indexer.row(i,j); c=indexer.col(i,j); }
+
+ RowCol rowcol(int i, int j) const {
+ assert(i<nr&&j<nc);
+ return RowCol(indexer.row(i,j), indexer.col(i,j));
+ }
+
+private:
+ bool writable; // does this view allow writing?
+ int nr,nc; // logical shape
+ Indexer indexer; // view->data mapping
+};
+
+
+} // namespace SimTK
+
+
+#endif // SimTK_SIMMATRIX_ELEMENT_FILTER_H_
diff --git a/SimTKcommon/BigMatrix/src/MatrixCharacteristics.cpp b/SimTKcommon/BigMatrix/src/MatrixCharacteristics.cpp
new file mode 100644
index 0000000..8768b54
--- /dev/null
+++ b/SimTKcommon/BigMatrix/src/MatrixCharacteristics.cpp
@@ -0,0 +1,473 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Scalar.h"
+
+#include "SimTKcommon/internal/MatrixCharacteristics.h"
+
+#include <iostream>
+
+namespace SimTK {
+
+
+// ------------------------------ MatrixStructure -----------------------------
+const char*
+MatrixStructure::name(Structure structure) {
+ switch (structure) {
+ case NoStructure: return "No Structure";
+ case Matrix1d: return "Matrix1d";
+ case Zero: return "Zero";
+ case Identity: return "Identity";
+ case Permutation: return "Permutation";
+ case RepeatedDiagonal: return "RepeatedDiagonal";
+ case Diagonal: return "Diagonal";
+ case BiDiagonal: return "BiDiagonal";
+ case TriDiagonal: return "TriDiagonal";
+ case BandedSymmetric: return "BandedSymmetric";
+ case BandedHermitian: return "BandedHermitian";
+ case Banded: return "Banded";
+ case Triangular: return "Triangular";
+ case QuasiTriangular: return "QuasiTriangular";
+ case Hessenberg: return "Hessenberg";
+ case Symmetric: return "Symmetric";
+ case Hermitian: return "Hermitian";
+ case SkewSymmetric: return "SkewSymmetric";
+ case SkewHermitian: return "SkewHermitian";
+ case Full: return "Full";
+ default: return "INVALID MatrixStructure::Structure";
+ };
+}
+
+const char*
+MatrixStructure::name(Position position) {
+ switch (position) {
+ case NoPosition: return "No Position";
+ case Upper: return "Upper";
+ case Lower: return "Lower";
+ default: return "INVALID MatrixStructure::Position";
+ };
+}
+
+const char*
+MatrixStructure::name(DiagValue diagValue) {
+ switch (diagValue) {
+ case NoDiagValue: return "No DiagValue";
+ case StoredDiag: return "StoredDiag";
+ case ZeroDiag: return "ZeroDiag";
+ case UnitDiag: return "UnitDiag";
+ default: return "INVALID MatrixStructure::DiagValue";
+ };
+}
+
+MatrixStructure::Mask
+MatrixStructure::mask() const {
+ return Mask(calcStructureMask(getStructure()),
+ calcPositionMask(getStructure()),
+ calcDiagValueMask(getStructure()));
+}
+
+MatrixStructure::StructureMask
+MatrixStructure::calcStructureMask(Structure structure) {
+ switch (structure) {
+ case NoStructure: return UncommittedStructure;
+ case Matrix1d: return Matrix1d;
+ case Zero: return Zero;
+ case Identity: return Identity;
+ case Permutation: return Permutation | calcStructureMask(Identity);
+ case RepeatedDiagonal: return RepeatedDiagonal | calcStructureMask(Identity)
+ | calcStructureMask(Zero);
+ case Diagonal: return Diagonal | calcStructureMask(RepeatedDiagonal);
+ case BiDiagonal: return BiDiagonal | calcStructureMask(Diagonal);
+ case TriDiagonal: return TriDiagonal | calcStructureMask(BiDiagonal);
+ case BandedSymmetric: return BandedSymmetric | calcStructureMask(Diagonal);
+ case BandedHermitian: return BandedHermitian | calcStructureMask(Diagonal);
+ case Banded: return Banded | calcStructureMask(BandedHermitian)
+ | calcStructureMask(BandedSymmetric)
+ | calcStructureMask(TriDiagonal);
+ case Triangular: return Triangular | calcStructureMask(BiDiagonal);
+ case QuasiTriangular: return QuasiTriangular | calcStructureMask(Triangular);
+ case Hessenberg: return Hessenberg | calcStructureMask(QuasiTriangular)
+ | calcStructureMask(TriDiagonal);
+ case Symmetric: return Symmetric | calcStructureMask(BandedSymmetric);
+ case Hermitian: return Hermitian | calcStructureMask(BandedHermitian);
+ case SkewSymmetric: return SkewSymmetric;
+ case SkewHermitian: return SkewHermitian;
+ case Full: return Full | calcStructureMask(Hermitian) | calcStructureMask(SkewHermitian)
+ | calcStructureMask(Symmetric) | calcStructureMask(SkewSymmetric)
+ | calcStructureMask(Hessenberg)
+ | calcStructureMask(Banded)
+ | calcStructureMask(Permutation)
+ | calcStructureMask(Matrix1d);
+
+ default: SimTK_ASSERT1_ALWAYS(!"invalid MatrixStructure::Structure",
+ "MatrixStructure::mask(): value 0x%x is invalid.", (unsigned)structure);
+ return NoStructure;
+ };
+}
+
+MatrixStructure::PositionMask
+MatrixStructure::calcPositionMask(Structure structure) {
+ switch (structure) {
+ case BiDiagonal:
+ case Triangular:
+ case QuasiTriangular:
+ case Hessenberg:
+ return AnyPosition; // i.e., upper or lower
+ default:
+ return NoPosition;
+ };
+}
+
+MatrixStructure::DiagValueMask
+MatrixStructure::calcDiagValueMask(Structure structure) {
+ switch (structure) {
+ case Triangular:
+ case Symmetric:
+ case Hermitian:
+ return AnyDiagValue;
+
+ default:
+ return StoredDiag;
+ };
+}
+
+
+// ------------------------------ MatrixStorage --------------------------------
+const char*
+MatrixStorage::name(Packing p) {
+ switch (p) {
+ case NoPacking: return "No Packing";
+ case Full: return "Full";
+ case TriInFull: return "TriInFull";
+ case TriPacked: return "TriPacked";
+ case Banded: return "Banded";
+ case Vector: return "Vector";
+ case Scalar: return "Scalar";
+ case Permutation: return "Permutation";
+ default: return "INVALID Packing";
+ };
+}
+const char*
+MatrixStorage::name(Placement p) {
+ switch (p) {
+ case NoPlacement: return "No Placement";
+ case Lower: return "Lower";
+ case Upper: return "Upper";
+ default: return "INVALID Placement";
+ };
+}
+const char*
+MatrixStorage::name(Order o) {
+ switch (o) {
+ case NoOrder: return "No Order";
+ case ColumnOrder: return "ColumnOrder";
+ case RowOrder: return "RowOrder";
+ default: return "INVALID Order";
+ };
+}
+const char*
+MatrixStorage::name(Diagonal d) {
+ switch (d) {
+ case NoDiag: return "No Diag";
+ case StoredDiag: return "StoredDiag";
+ case AssumedDiag: return "AssumedDiag";
+ default: return "INVALID Diagonal";
+ };
+}
+
+MatrixStorage
+MatrixStorage::calcDefaultStorage(const MatrixStructure& structure,
+ const MatrixOutline& outline)
+{
+ // Just to be nice we'll default to Lower storage for lower-triangular
+ // matrices and Upper storage for upper-triangular ones. (There is no
+ // real necessity to do that, it's just more pleasant.)
+ Placement placement = Lower;
+ if (structure.getPosition()==MatrixStructure::Upper) placement = Upper;
+
+ // Default order is Lapack-standard column order except for rows.
+ // TODO: probably should use row order for "Wide" outlines too, but need
+ // to check if that will limit us too much with Lapack.
+ MatrixStorage::Order order = ColumnOrder;
+ if (outline.getOutline()==MatrixOutline::Row)
+ order = RowOrder;
+
+ Diagonal diagonal = StoredDiag;
+ if (structure.getDiagValue() && structure.getDiagValue()!=MatrixStructure::StoredDiag)
+ diagonal = AssumedDiag;
+
+ switch (structure.getStructure()) {
+ case MatrixStructure::NoStructure: return MatrixStorage();
+ case MatrixStructure::Matrix1d: return MatrixStorage(Vector, NoPlacement, order, NoDiag);
+ case MatrixStructure::Zero: return MatrixStorage(Scalar); // don't specify anything else
+ case MatrixStructure::Identity: return MatrixStorage(Scalar);
+ case MatrixStructure::Permutation: return MatrixStorage(Permutation);
+ case MatrixStructure::RepeatedDiagonal: return MatrixStorage(Scalar);
+ case MatrixStructure::Diagonal: return MatrixStorage(Vector);
+ case MatrixStructure::BiDiagonal: return MatrixStorage(Banded);
+ case MatrixStructure::TriDiagonal: return MatrixStorage(Banded);
+ case MatrixStructure::BandedSymmetric: return MatrixStorage(Banded, placement);
+ case MatrixStructure::BandedHermitian: return MatrixStorage(Banded, placement);
+ case MatrixStructure::Banded: return MatrixStorage(Banded);
+ case MatrixStructure::Triangular: return MatrixStorage(TriInFull, placement, order, diagonal);
+ case MatrixStructure::QuasiTriangular: return MatrixStorage(Full, NoPlacement, order, NoDiag);
+ case MatrixStructure::Hessenberg: return MatrixStorage(Full, NoPlacement, order, NoDiag);
+ case MatrixStructure::Symmetric: return MatrixStorage(TriInFull, placement, order, diagonal);
+ case MatrixStructure::Hermitian: return MatrixStorage(TriInFull, placement, order, diagonal);
+ case MatrixStructure::SkewSymmetric: return MatrixStorage(TriInFull, placement, order, diagonal);
+ case MatrixStructure::SkewHermitian: return MatrixStorage(TriInFull, placement, order, diagonal);
+ case MatrixStructure::Full: return MatrixStorage(Full, NoPlacement, order, NoDiag);
+ default: SimTK_ASSERT1_ALWAYS(!"invalid MatrixStructure::Structure",
+ "MatrixStorage::getDefaultStorage(): value 0x%x is invalid.", structure.getStructure());
+ return MatrixStorage();
+ };
+}
+
+
+// ------------------------------- MatrixOutline -------------------------------
+const char*
+MatrixOutline::name(Outline a) {
+ switch(a) {
+ case NoOutline: return "No Outline";
+ case Scalar: return "Scalar";
+ case Column: return "Column";
+ case Row: return "Row";
+ case Square: return "Square";
+ case Wide: return "Wide";
+ case Tall: return "Tall";
+ case Rectangular: return "Rectangular";
+ default: return "INVALID MatrixOutline::Outline";
+ };
+}
+
+
+MatrixOutline::OutlineMask
+MatrixOutline::calcMask(Outline outline) {
+ switch(outline) {
+ case NoOutline: return UncommittedOutline;
+ case Scalar: return Scalar;
+ case Column: return Column | calcMask(Scalar);
+ case Row: return Row | calcMask(Scalar);
+ case Square: return Square | calcMask(Scalar);
+ case Wide: return Wide | calcMask(Square) | calcMask(Row);
+ case Tall: return Tall | calcMask(Square) | calcMask(Column);
+ case Rectangular: return Rectangular | calcMask(Wide) | calcMask(Tall);
+ default: SimTK_ASSERT1_ALWAYS(!"invalid MatrixOutline::Outline",
+ "MatrixOutline::calcMask(): value 0x%x is invalid.", outline);
+ return NoOutline;
+ };
+}
+
+bool
+MatrixOutline::isSizeOK(int m, int n) const {
+ SimTK_ASSERT2(m>=0 && n>=0,
+ "MatrixOutline::isSizeOK(): sizes must be non-negative; got %dx%d", m, n);
+ switch(getOutline()) {
+ case Scalar: return m==1 && n==1;
+ case Column: return n==1;
+ case Row: return m==1;
+ case Square: return m==n;
+ case Wide: return m<=n;
+ case Tall: return m>=n;
+ case Rectangular: return true;
+ default: SimTK_ASSERT1_ALWAYS(!"invalid MatrixOutline::Outline",
+ "MatrixOutline::isSizeOK(): value 0x%x is invalid.", getOutline());
+ return false;
+ };
+}
+
+void
+MatrixOutline::getMinimumSize(int& m, int& n) const {
+ if (outline==Scalar) m=n=1;
+ else if (outline==Column) m=0, n=1;
+ else if (outline==Row) m=1, n=0;
+ else m=n=0;
+}
+
+MatrixOutline
+MatrixOutline::calcFromSize(int m, int n) {
+ Outline outline;
+
+ if (m==1) outline = n==1 ? Scalar : Row;
+ else if (n==1) outline = Column; // m!=1
+ else if (m==n) outline = Square;
+ else if (m<n) outline = Wide;
+ else outline = Tall;
+
+ return MatrixOutline(outline);
+}
+
+
+// ----------------------------- MatrixCondition -------------------------------
+const char*
+MatrixCondition::name(Condition c) {
+ switch(c) {
+ case UnknownCondition: return "UnknownCondition";
+ case Orthogonal: return "Orthogonal";
+ case PositiveDefinite: return "PositiveDefinite";
+ case WellConditioned: return "WellConditioned";
+ case FullRank: return "FullRank";
+ case Singular: return "Singular";
+ default: return "INVALID MatrixCondition::Condition";
+ };
+}
+
+MatrixCondition::ConditionMask
+MatrixCondition::calcMask(Condition c) {
+ switch(c) {
+ case UnknownCondition: return UncommittedCondition;
+ case Orthogonal: return Orthogonal;
+ case PositiveDefinite: return PositiveDefinite;
+ case WellConditioned: return WellConditioned
+ | calcMask(PositiveDefinite)
+ | calcMask(Orthogonal);
+ case FullRank: return FullRank
+ | calcMask(WellConditioned);
+ case Singular: return UncommittedCondition; // can't get worse than this
+ default: SimTK_ASSERT1_ALWAYS(!"invalid MatrixCondition::Condition",
+ "MatrixCondition::calcMask(Condition): value 0x%x is invalid.", c);
+ return UnknownCondition;
+ };
+}
+
+const char*
+MatrixCondition::name(Diagonal d) {
+ switch(d) {
+ case UnknownDiagonal: return "UnknownDiagonal";
+ case ZeroDiagonal: return "ZeroDiagonal";
+ case OneDiagonal: return "OneDiagonal";
+ case RealDiagonal: return "RealDiagonal";
+ case ImaginaryDiagonal: return "ImaginaryDiagonal";
+ default: return "INVALID MatrixCondition::Diagonal";
+ };
+}
+
+MatrixCondition::DiagonalMask
+MatrixCondition::calcMask(Diagonal d) {
+ switch(d) {
+ case UnknownDiagonal: return UncommittedDiagonal;
+ case ZeroDiagonal: return ZeroDiagonal;
+ case OneDiagonal: return OneDiagonal;
+ case RealDiagonal: return RealDiagonal
+ | calcMask(ZeroDiagonal)
+ | calcMask(OneDiagonal);
+ case ImaginaryDiagonal: return ImaginaryDiagonal
+ | calcMask(ZeroDiagonal);
+ default: SimTK_ASSERT1_ALWAYS(!"invalid MatrixCondition::Diagonal",
+ "MatrixCondition::calcMask(Diagonal): value 0x%x is invalid.", d);
+ return UnknownDiagonal;
+ };
+}
+
+
+// ----------------------------- MatrixCharacter -------------------------------
+std::ostream& operator<<(std::ostream& o, const MatrixCharacter& actual) {
+ o << "MatrixCharacter:";
+ o << "\n size=" << actual.nrow() << "x" << actual.ncol();
+ o << "\n structure=" << actual.getStructure().name();
+ o << "\n outline=" << actual.getOutline().name();
+ o << "\n storage=" << actual.getStorage().name();
+ o << "\n condition=" << actual.getCondition().name();
+ return o << std::endl;
+}
+
+
+// ----------------------------- MatrixCommitment ------------------------------
+MatrixCharacter
+MatrixCommitment::calcDefaultCharacter(int m, int n) const {
+ MatrixCharacter actual; // nothing specified yet
+
+ // Work on the size first.
+ int nr, nc;
+ outline.getMinimumSize(nr, nc);
+
+ nr = std::max(nr, getDefaultNumRows());
+ nc = std::max(nc, getDefaultNumCols());
+
+ nr = std::max(nr, m);
+ nc = std::max(nc, n);
+
+ actual.setActualSize(nr,nc); // calculate outline also
+
+ SimTK_ERRCHK3(isOutlineOK(actual.getOutline()) && isSizeOK(actual.getSize()),
+ "MatrixCommitment::calcDefaultCharacter()",
+ "Outline commitment %s and required initial size %d x %d were inconsistent.",
+ outline.name().c_str(), nr, nc);
+
+ // Size and outline are now set in actual.
+
+ if (isStructureCommitted())
+ actual.setStructure(structure);
+ else {
+ switch (getOutlineCommitment().getOutline()) {
+ case MatrixOutline::Scalar:
+ actual.setStructure(MatrixStructure::RepeatedDiagonal);
+ break;
+ case MatrixOutline::Column:
+ case MatrixOutline::Row:
+ actual.setStructure(MatrixStructure::Matrix1d);
+ break;
+ default:
+ actual.setStructure(MatrixStructure::Full);
+ };
+ }
+ actual.updStructure().setMissingAttributes();
+ const MatrixStructure& astruct = actual.getStructure();
+ const MatrixOutline& aoutline = actual.getOutline();
+
+ // Size, outline, and structure are now set in actual.
+
+ if (isStorageCommitted())
+ actual.setStorage(storage);
+ else {
+ // Default storage format depends on structure type and outline.
+ actual.setStorage(MatrixStorage::calcDefaultStorage(astruct, aoutline));
+ }
+ actual.updStorage().setMissingAttributes();
+
+ // We don't expect condition to be set as a commitment usually.
+ if (isConditionCommitted()) actual.setCondition(condition);
+ else actual.setCondition(MatrixCondition());
+
+ return actual;
+}
+
+
+std::ostream& operator<<(std::ostream& o, const MatrixCommitment& commit) {
+ const MatrixCharacter::Mask::SizeMask Uncommitted = MatrixCharacter::Mask::SizeUncommitted;
+ o << "MatrixCommitment:";
+ o << "\n size=";
+ if (commit.getNumRowsMask()==Uncommitted) o << "Any";
+ else o << commit.getNumRowsMask();
+ o << " x ";
+ if (commit.getNumColsMask()==Uncommitted) o << "Any";
+ else o << commit.getNumColsMask();
+ o << "\n structure=" << commit.getStructureCommitment().name();
+ o << "\n outline=" << commit.getOutlineCommitment().name();
+ o << "\n storage=" << commit.getStorageCommitment().name();
+ o << "\n condition=" << commit.getConditionCommitment().name();
+ return o << std::endl;
+}
+
+
+} // namespace SimTK
diff --git a/SimTKcommon/BigMatrix/src/MatrixHelper.cpp b/SimTKcommon/BigMatrix/src/MatrixHelper.cpp
new file mode 100644
index 0000000..f1e6ae9
--- /dev/null
+++ b/SimTKcommon/BigMatrix/src/MatrixHelper.cpp
@@ -0,0 +1,993 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Scalar.h"
+#include "SimTKcommon/SmallMatrix.h"
+#include "SimTKcommon/TemplatizedLapack.h"
+
+#include "SimTKcommon/internal/MatrixHelper.h"
+#include "SimTKcommon/internal/MatrixCharacteristics.h"
+
+#include "MatrixHelperRep.h"
+#include "MatrixHelperRep_Full.h"
+#include "MatrixHelperRep_Tri.h"
+#include "MatrixHelperRep_Vector.h"
+
+#include <iostream>
+#include <cstdio>
+
+namespace SimTK {
+
+
+//----------------------------- MATRIX HELPER ----------------------------------
+//
+// Implementations of MatrixHelper methods. Most are just pass throughs to
+// MatrixHelperRep, but some may involve replacing the current MatrixHelperRep
+// with a different one.
+//------------------------------------------------------------------------------
+
+template <class S> void
+MatrixHelper<S>::deleteRepIfOwner() {
+ if (rep && &rep->getMyHandle() == this)
+ delete rep;
+ rep=0;
+}
+
+// Rep replacement. Delete the existing rep if we're the owner of it.
+// We are going to be the owner of the new rep regardless.
+template <class S> void
+MatrixHelper<S>::replaceRep(MatrixHelperRep<S>* hrep) {
+ deleteRepIfOwner();
+ rep=hrep;
+ rep->setMyHandle(*this);
+}
+
+// Space-stealing constructor. We're going to take over ownership
+// of this rep.
+template <class S>
+MatrixHelper<S>::MatrixHelper(MatrixHelperRep<S>* hrep) : rep(hrep)
+{ if (rep) rep->setMyHandle(*this); }
+
+template <class S> const MatrixCommitment&
+MatrixHelper<S>::getCharacterCommitment() const
+{ return getRep().getCharacterCommitment(); }
+
+template <class S> const MatrixCharacter&
+MatrixHelper<S>::getMatrixCharacter() const
+{ return getRep().getMatrixCharacter(); }
+
+
+
+// This is the constructor for a Matrix in which only the handle commitment has been
+// supplied. The allocated matrix will have the smallest size that satisfies the
+// commitment, typically 0x0 or 0x1.
+template <class S>
+MatrixHelper<S>::MatrixHelper(int esz, int cppEsz, const MatrixCommitment& mc) : rep(0) {
+ // Determine the best actual matrix to allocate to satisfy this commitment.
+ const MatrixCharacter actual = mc.calcDefaultCharacter(0,0);
+
+ rep = MatrixHelperRep<S>::createOwnerMatrixHelperRep(esz, cppEsz, actual);
+ assert(rep);
+ rep->setMyHandle(*this);
+
+ rep->m_commitment = mc;
+}
+
+// This is effectively the default constructor. It creates a writable, fully resizable 0x0
+// matrix, with the handle committed only to the given element size.
+// Just calls the above constructor with a default commitment.
+template <class S>
+MatrixHelper<S>::MatrixHelper(int esz, int cppEsz) : rep(0) {
+ new (this) MatrixHelper(esz, cppEsz, MatrixCommitment());
+}
+
+// This is the constructor for a Matrix in which the handle commitment has been
+// supplied, along with an initial allocation size. Provided the size satisfies the
+// commitment, the resulting matrix will have that size.
+template <class S>
+MatrixHelper<S>::MatrixHelper
+ (int esz, int cppEsz, const MatrixCommitment& commitment,
+ int m, int n) : rep(0)
+{
+ SimTK_ERRCHK2(commitment.isSizeOK(m,n), "MatrixHelper::ctor()",
+ "The initial size allocation %s x %s didn't satisfy the supplied commitment.",
+ m, n);
+
+ // Determine the best actual matrix to allocate to satisfy this commitment.
+ const MatrixCharacter actual = commitment.calcDefaultCharacter(m,n);
+
+ rep = MatrixHelperRep<S>::createOwnerMatrixHelperRep(esz, cppEsz, actual);
+ assert(rep);
+ rep->setMyHandle(*this);
+
+ rep->m_commitment = commitment;
+}
+
+// Create a read-only view into existing data.
+template <class S>
+MatrixHelper<S>::MatrixHelper
+ (int esz, int cppEsz, const MatrixCommitment& commitment,
+ const MatrixCharacter& actual, int spacing, const S* data) : rep(0)
+{
+ SimTK_ERRCHK(commitment.isSatisfiedBy(actual), "MatrixHelper::ctor(external data)",
+ "The supplied actual matrix character for the external data did not "
+ "satisfy the specified handle character commitment.");
+
+ rep = MatrixHelperRep<S>::createExternalMatrixHelperRep
+ (esz, cppEsz, actual, spacing, const_cast<S*>(data), false);
+ assert(rep);
+ rep->setMyHandle(*this);
+
+ rep->m_commitment = commitment;
+}
+
+// Create a writable view into existing data.
+template <class S>
+MatrixHelper<S>::MatrixHelper
+ (int esz, int cppEsz, const MatrixCommitment& commitment,
+ const MatrixCharacter& actual, int spacing, S* data) : rep(0)
+{
+ SimTK_ERRCHK(commitment.isSatisfiedBy(actual), "MatrixHelper::ctor(external data)",
+ "The supplied actual matrix character for the external data did not "
+ "satisfy the specified handle character commitment.");
+
+ rep = MatrixHelperRep<S>::createExternalMatrixHelperRep
+ (esz, cppEsz, actual, spacing, data, true);
+ assert(rep);
+ rep->setMyHandle(*this);
+
+ rep->m_commitment = commitment;
+}
+
+// clear() restores this matrix to the state it would be in if it were constructed
+// using its current character commitment. We'll replace the current HelperRep with
+// a fresh one. Note that this is more expensive than resize(0,0) which doesn't
+// require replacement of the HelperRep.
+template <class S> void
+MatrixHelper<S>::clear() {
+ // Determine the best actual matrix to allocate to satisfy this commitment.
+ const MatrixCharacter actual = getCharacterCommitment().calcDefaultCharacter(0,0);
+ const int esz = getRep().getEltSize();
+ const int cppEsz = getRep().getCppEltSize();
+
+ MatrixHelperRep<S>* newRep =
+ MatrixHelperRep<S>::createOwnerMatrixHelperRep(esz, cppEsz, actual);
+ assert(newRep);
+
+ newRep->m_commitment = getRep().m_commitment;
+ if (!getRep().m_canBeOwner) {
+ newRep->m_canBeOwner = false;
+ newRep->m_owner = false;
+ }
+
+ replaceRep(newRep);
+}
+
+template <class S> bool
+MatrixHelper<S>::isClear() const {return getRep().isClear();}
+
+
+template <class S> void
+MatrixHelper<S>::commitTo(const MatrixCommitment& mc) {
+ SimTK_ERRCHK_ALWAYS(isClear(), "MatrixHelper::commitTo()",
+ "You can only replace the character commitment in an empty Matrix handle."
+ " Call clear() first to empty the handle.");
+ updRep().m_commitment = mc;
+ clear(); // reallocate to satisfy the new commitment
+}
+
+// Copy constructor is suppressed; these are closest things but require
+// specification of deep or shallow copy.
+
+// Create a read-only view of existing data.
+template <class S>
+MatrixHelper<S>::MatrixHelper(const MatrixCommitment& mc, const MatrixHelper& h, const ShallowCopy&) : rep(0) {
+ SimTK_ERRCHK(mc.isSatisfiedBy(h.getMatrixCharacter()), "MatrixHelper::ctor(const,shallow)",
+ "The actual matrix character of the source did not "
+ "satisfy the specified handle character commitment.");
+ rep = h.rep->createWholeView(false);
+ rep->setMyHandle(*this);
+ rep->m_commitment = mc;
+}
+
+// Create a (possibly) writable view of existing data.
+template <class S>
+MatrixHelper<S>::MatrixHelper(const MatrixCommitment& mc, MatrixHelper& h, const ShallowCopy&) : rep(0) {
+ SimTK_ERRCHK(mc.isSatisfiedBy(h.getMatrixCharacter()), "MatrixHelper::ctor(writable,shallow)",
+ "The actual matrix character of the source did not "
+ "satisfy the specified handle character commitment.");
+ rep = h.rep->createWholeView(true);
+ rep->setMyHandle(*this);
+ rep->m_commitment = mc;
+}
+
+// Deep copy. "This" be always writable and in densely packed storage.
+template <class S>
+MatrixHelper<S>::MatrixHelper(const MatrixCommitment& mc, const MatrixHelper& h, const DeepCopy&) : rep(0) {
+ SimTK_ERRCHK(mc.isSatisfiedBy(h.getMatrixCharacter()), "MatrixHelper::ctor(deep)",
+ "The actual matrix character of the source did not "
+ "satisfy the specified handle character commitment.");
+ rep = h.rep->createDeepCopy();
+ rep->setMyHandle(*this);
+ rep->m_commitment = mc;
+}
+
+// Negated deep copy. We'll get a brand new, filterless, writable, packed copy with
+// the same values as the original (duh, that's what "copy" means) -- BUT, the
+// physical floating point representations will all have been negated since we're
+// copying a Matrix whose elements are of type negator<S> while ours are type S (or
+// vice versa).
+template <class S>
+MatrixHelper<S>::MatrixHelper(const MatrixCommitment& mc, const MatrixHelper<typename CNT<S>::TNeg>& h, const DeepCopy&) : rep(0) {
+ SimTK_ERRCHK(mc.isSatisfiedBy(h.getMatrixCharacter()), "MatrixHelper::ctor(negated,deep)",
+ "The actual matrix character of the source did not "
+ "satisfy the specified handle character commitment.");
+ rep = h.rep->createNegatedDeepCopy();
+ rep->setMyHandle(*this);
+ rep->m_commitment = mc;
+}
+
+// construct read-only block view
+template <class S>
+MatrixHelper<S>::MatrixHelper(const MatrixCommitment& mc, const MatrixHelper& h, int i, int j, int m, int n) : rep(0) {
+ if (n==1)
+ rep = h.rep->createColumnView(j,i,m,false); // column j, from i to i+m-1
+ else if (m==1)
+ rep = h.rep->createRowView(i,j,n,false); // row i, from j to j+n-1
+ else
+ rep = h.rep->createBlockView(EltBlock(i,j,m,n), false);
+
+ SimTK_ERRCHK(mc.isSatisfiedBy(rep->getMatrixCharacter()), "MatrixHelper::ctor(const,block)",
+ "The actual matrix character of the source block did not "
+ "satisfy the specified handle character commitment.");
+
+ rep->setMyHandle(*this);
+ rep->m_commitment = mc;
+}
+
+// construct (possibly) writable block view
+template <class S>
+MatrixHelper<S>::MatrixHelper(const MatrixCommitment& mc, MatrixHelper& h, int i, int j, int m, int n) : rep(0) {
+ if (n==1)
+ rep = h.rep->createColumnView(j,i,m,true); // column j, from i to i+m-1
+ else if (m==1)
+ rep = h.rep->createRowView(i,j,n,true); // row i, from j to j+n-1
+ else
+ rep = h.rep->createBlockView(EltBlock(i,j,m,n),true);
+
+ SimTK_ERRCHK(mc.isSatisfiedBy(rep->getMatrixCharacter()), "MatrixHelper::ctor(writable,block)",
+ "The actual matrix character of the source block did not "
+ "satisfy the specified handle character commitment.");
+
+ rep->setMyHandle(*this);
+ rep->m_commitment = mc;
+}
+
+// Construct read only transposed view of passed-in helper.
+template <class S>
+MatrixHelper<S>::MatrixHelper(const MatrixCommitment& mc, const MatrixHelper<typename CNT<S>::THerm>& h,
+ const TransposeView&) : rep(0) {
+ rep = h.rep->createHermitianView(false);
+
+ SimTK_ERRCHK(mc.isSatisfiedBy(rep->getMatrixCharacter()), "MatrixHelper::ctor(const,transpose)",
+ "The actual matrix character of the transposed source did not "
+ "satisfy the specified handle character commitment.");
+
+ rep->setMyHandle(*this);
+ rep->m_commitment = mc;
+}
+
+// Construct (possibly) writable transposed view of passed-in helper.
+template <class S>
+MatrixHelper<S>::MatrixHelper(const MatrixCommitment& mc, MatrixHelper<typename CNT<S>::THerm>& h,
+ const TransposeView&) : rep(0) {
+ rep = h.rep->createHermitianView(true);
+
+ SimTK_ERRCHK(mc.isSatisfiedBy(rep->getMatrixCharacter()), "MatrixHelper::ctor(writable,transpose)",
+ "The actual matrix character of the transposed source did not "
+ "satisfy the specified handle character commitment.");
+
+ rep->setMyHandle(*this);
+ rep->m_commitment = mc;
+}
+
+// Construct read only diagonal view of passed-in helper.
+template <class S>
+MatrixHelper<S>::MatrixHelper(const MatrixCommitment& mc, const MatrixHelper& h,
+ const DiagonalView&) : rep(0) {
+ rep = h.rep->createDiagonalView(false);
+
+ SimTK_ERRCHK(mc.isSatisfiedBy(rep->getMatrixCharacter()), "MatrixHelper::ctor(const,diagonal)",
+ "The actual matrix character of the diagonal of the source did not "
+ "satisfy the specified handle character commitment.");
+
+ rep->setMyHandle(*this);
+ rep->m_commitment = mc;
+}
+
+// Construct (possibly) writable diagonal view of passed-in helper.
+template <class S>
+MatrixHelper<S>::MatrixHelper(const MatrixCommitment& mc, MatrixHelper& h,
+ const DiagonalView&) : rep(0) {
+ rep = h.rep->createDiagonalView(true);
+
+ SimTK_ERRCHK(mc.isSatisfiedBy(rep->getMatrixCharacter()),
+ "MatrixHelper::ctor(writable,diagonal)",
+ "The actual matrix character of the diagonal of the source did not "
+ "satisfy the specified handle character commitment.");
+
+ rep->setMyHandle(*this);
+ rep->m_commitment = mc;
+}
+
+// Construct a read-only indexed view of a Vector.
+template <class S>
+MatrixHelper<S>::MatrixHelper(const MatrixCommitment& mc, const MatrixHelper& h,
+ int n, const int* ix) : rep(0) {
+ const VectorHelper<S>& vh =
+ SimTK_DYNAMIC_CAST_DEBUG<const VectorHelper<S>&>(*h.rep);
+ rep = new IndexedVectorHelper<S>(vh.getEltSize(), vh.getCppEltSize(), n,
+ vh.preferRowOrder_(), ix, n ? vh.getElt_(0) : 0, false);
+
+ SimTK_ERRCHK(mc.isSatisfiedBy(rep->getMatrixCharacter()),
+ "MatrixHelper::ctor(const,indexed)",
+ "The actual matrix character of the indexed source did not "
+ "satisfy the specified handle character commitment.");
+
+ rep->setMyHandle(*this);
+ rep->m_commitment = mc;
+}
+
+// Construct a writable indexed view of a Vector.
+template <class S>
+MatrixHelper<S>::MatrixHelper(const MatrixCommitment& mc, MatrixHelper& h,
+ int n, const int* ix) : rep(0) {
+ VectorHelper<S>& vh = SimTK_DYNAMIC_CAST_DEBUG<VectorHelper<S>&>(*h.rep);
+ rep = new IndexedVectorHelper<S>(vh.getEltSize(), vh.getCppEltSize(), n,
+ vh.preferRowOrder_(), ix, n ? vh.updElt_(0) : 0, true);
+
+ SimTK_ERRCHK(mc.isSatisfiedBy(rep->getMatrixCharacter()),
+ "MatrixHelper::ctor(writable,indexed)",
+ "The actual matrix character of the indexed source did not "
+ "satisfy the specified handle character commitment.");
+
+ rep->setMyHandle(*this);
+ rep->m_commitment = mc;
+}
+
+// Perform deep assignment. "This" gets reallocated if necessary if it's an
+// owner, but if this is a view the source and destination sizes must match.
+template <class S> MatrixHelper<S>&
+MatrixHelper<S>::copyAssign(const MatrixHelper& h) {
+ if (rep->isOwner())
+ rep->resize(h.nrow(), h.ncol(), false);
+ else {
+ // OK if the sizes match.
+ assert(nrow()==h.nrow() && ncol()==h.ncol());
+ }
+
+ rep->copyInFromCompatibleSource(*h.rep);
+ return *this;
+}
+// Like copy assign but the source has elements with the opposite interpretation
+// of sign. Note that the result still has the original value, but the in-memory
+// representation will be different, meaning flops will be burned here.
+template <class S> MatrixHelper<S>&
+MatrixHelper<S>::negatedCopyAssign(const MatrixHelper<typename CNT<S>::TNeg>& h) {
+ if (rep->isOwner())
+ rep->resize(h.nrow(), h.ncol(), false);
+ else {
+ // OK if the sizes match.
+ assert(nrow()==h.nrow() && ncol()==h.ncol());
+ }
+
+ rep->negatedCopyInFromCompatibleSource(*h.rep);
+ return *this;
+}
+
+// Create a read-only view of existing data. Element size of source and destination
+// must match. The result will have no element filter unless the source already does.
+template <class S> MatrixHelper<S>&
+MatrixHelper<S>::readOnlyViewAssign(const MatrixHelper& h) {
+ SimTK_ERRCHK(getCharacterCommitment().isSatisfiedBy(h.getMatrixCharacter()),
+ "MatrixHelper<S>::readOnlyViewAssign()",
+ "The source matrix does not satisfy this handle's character commitment.");
+
+ MatrixHelperRep<S>* newRep = h.getRep().createWholeView(false);
+ newRep->m_commitment = getRep().m_commitment; // preserve commitment
+ newRep->m_canBeOwner = getRep().m_canBeOwner;
+ replaceRep(newRep);
+ return *this;
+}
+// Create a (possibly) writable view of existing data.
+template <class S> MatrixHelper<S>&
+MatrixHelper<S>::writableViewAssign(MatrixHelper& h) {
+ SimTK_ERRCHK(getCharacterCommitment().isSatisfiedBy(h.getMatrixCharacter()),
+ "MatrixHelper<S>::writableViewAssign()",
+ "The source matrix does not satisfy this handle's character commitment.");
+
+ MatrixHelperRep<S>* newRep = h.getRep().createWholeView(true);
+ newRep->m_commitment = getRep().m_commitment; // preserve commitment
+ newRep->m_canBeOwner = getRep().m_canBeOwner;
+ replaceRep(newRep);
+ return *this;
+}
+
+template <class S> void
+MatrixHelper<S>::scaleBy(const typename CNT<S>::StdNumber& s) {
+ rep->scaleBy(s);
+}
+template <class S> void
+MatrixHelper<S>::addIn(const MatrixHelper& h) {
+ rep->addIn(h);
+}
+template <class S> void
+MatrixHelper<S>::addIn(const MatrixHelper<typename CNT<S>::TNeg>& h) {
+ subIn(reinterpret_cast<const MatrixHelper<S>&>(h));
+}
+template <class S> void
+MatrixHelper<S>::subIn(const MatrixHelper& h) {
+ rep->subIn(h);
+}
+template <class S> void
+MatrixHelper<S>::subIn(const MatrixHelper<typename CNT<S>::TNeg>& h) {
+ addIn(reinterpret_cast<const MatrixHelper<S>&>(h));
+}
+template <class S> void
+MatrixHelper<S>::fillWith(const S* eltp) {
+ rep->fillWith(eltp);
+}
+template <class S> void
+MatrixHelper<S>::copyInByRowsFromCpp(const S* elts) {
+ rep->copyInByRowsFromCpp(elts);
+}
+
+template <class S> void
+MatrixHelper<S>::invertInPlace() {
+ rep->invertInPlace();
+}
+
+template <class S> void
+MatrixHelper<S>::dump(const char* msg) const {
+ rep->dump(msg);
+}
+
+template <class S> const S*
+MatrixHelper<S>::getElt(int i, int j) const {return rep->getElt(i,j);}
+template <class S> S*
+MatrixHelper<S>::updElt(int i, int j) {return rep->updElt(i,j);}
+template <class S> void
+MatrixHelper<S>::getAnyElt(int i, int j, S* value) const
+{ return rep->getAnyElt(i,j,value); }
+
+template <class S> const S*
+MatrixHelper<S>::getElt(int i) const {return rep->getElt(i);}
+template <class S> S*
+MatrixHelper<S>::updElt(int i) {return rep->updElt(i);}
+template <class S> void
+MatrixHelper<S>::getAnyElt(int i, S* value) const
+{ return rep->getAnyElt(i,value); }
+
+template <class S> int
+MatrixHelper<S>::nrow() const {
+ return rep->nrow();
+}
+template <class S> int
+MatrixHelper<S>::ncol() const {
+ return rep->ncol();
+}
+template <class S> ptrdiff_t
+MatrixHelper<S>::nelt() const {
+ return rep->nelt();
+}
+template <class S> int
+MatrixHelper<S>::length() const {
+ return rep->length();
+}
+
+template <class S> void
+MatrixHelper<S>::resize (int m, int n) {rep->resize(m,n,false);}
+template <class S> void
+MatrixHelper<S>::resizeKeep(int m, int n) {rep->resize(m,n,true);}
+
+template <class S> void MatrixHelper<S>::lockShape() {rep->lockHandle();}
+template <class S> void MatrixHelper<S>::unlockShape() {rep->unlockHandle();}
+
+template <class S> void
+MatrixHelper<S>::sum(S* const answer) const {
+ rep->sum(answer);
+}
+template <class S> void
+MatrixHelper<S>::colSum(int j, S* const answer) const {
+ rep->colSum(j,answer);
+}
+template <class S> void
+MatrixHelper<S>::rowSum(int i, S* const answer) const {
+ rep->rowSum(i,answer);
+}
+template <class S> void
+MatrixHelper<S>::fillWithScalar(const typename CNT<S>::StdNumber& s) {
+ rep->fillWithScalar(s);
+}
+
+template <class S> bool
+MatrixHelper<S>::hasContiguousData() const {
+ return rep->hasContiguousData();
+}
+template <class S> ptrdiff_t
+MatrixHelper<S>::getContiguousDataLength() const {
+ assert(hasContiguousData());
+ return rep->nScalars();
+}
+template <class S> const S*
+MatrixHelper<S>::getContiguousData() const {
+ assert(hasContiguousData());
+ return rep->m_data;
+}
+template <class S> S*
+MatrixHelper<S>::updContiguousData() {
+ assert(hasContiguousData());
+ return rep->m_data;
+}
+template <class S> void
+MatrixHelper<S>::replaceContiguousData(S* newData, ptrdiff_t length, bool takeOwnership) {
+ assert(length == getContiguousDataLength());
+ if (rep->m_owner) {
+ MatrixHelperRep<S>::deleteAllocatedMemory(rep->m_data);
+ rep->m_data=0;
+ }
+ rep->m_data = newData;
+ rep->m_owner = takeOwnership;
+}
+template <class S> void
+MatrixHelper<S>::replaceContiguousData(const S* newData, ptrdiff_t length) {
+ replaceContiguousData(const_cast<S*>(newData), length, false);
+ rep->m_writable = false;
+}
+template <class S> void
+MatrixHelper<S>::swapOwnedContiguousData(S* newData, ptrdiff_t length, S*& oldData) {
+ assert(length == getContiguousDataLength());
+ assert(rep->m_owner);
+ oldData = rep->m_data;
+ rep->m_data = newData;
+}
+
+
+
+
+//----------------------------- MATRIX HELPER REP ------------------------------
+//
+//------------------------------------------------------------------------------
+
+template <class S> MatrixHelperRep<S>*
+MatrixHelperRep<S>::createOwnerMatrixHelperRep(int esz, int cppEsz, const MatrixCharacter& want) {
+ SimTK_ASSERT2((esz==1&&cppEsz==1) || (esz>1&&cppEsz>=esz),
+ "MatrixHelperRep::createOwnerMatrixHelperRep(): bad element size esz=%d, cppEsz=%d", esz, cppEsz);
+
+ MatrixHelperRep<S>* rep = 0;
+
+ const int nr = want.nrow();
+ const int nc = want.ncol();
+ const MatrixStructure& structure = want.getStructure();
+ const MatrixStorage& storage = want.getStorage();
+ const bool rowOrder = (storage.getOrder() == MatrixStorage::RowOrder);
+
+ switch (structure.getStructure()) {
+
+ case MatrixStructure::Full:
+ if (rowOrder)
+ rep = (esz == 1 ? (RegularFullHelper<S>*)new FullRowOrderScalarHelper<S>(nr,nc)
+ : (RegularFullHelper<S>*)new FullRowOrderEltHelper<S>(esz, cppEsz,nr,nc));
+ else
+ rep = (esz == 1 ? (RegularFullHelper<S>*)new FullColOrderScalarHelper<S>(nr,nc)
+ : (RegularFullHelper<S>*)new FullColOrderEltHelper<S>(esz, cppEsz,nr,nc));
+ break;
+
+ case MatrixStructure::Triangular:
+ case MatrixStructure::Symmetric:
+ case MatrixStructure::Hermitian:
+ case MatrixStructure::SkewSymmetric:
+ case MatrixStructure::SkewHermitian: {
+ const bool triangular = structure.getStructure() == MatrixStructure::Triangular;
+ const bool hermitian = structure.getStructure() == MatrixStructure::Hermitian
+ || structure.getStructure() == MatrixStructure::SkewHermitian;
+ const bool skew = structure.getStructure() == MatrixStructure::SkewSymmetric
+ || structure.getStructure() == MatrixStructure::SkewHermitian;
+
+ if (/*storage.getPlacement() == MatrixStorage::Upper*/true)
+ rep = (esz == 1 ? (TriInFullHelper<S>*)new TriInFullUpperHelper<S>(1,1,nr,nc,
+ triangular,hermitian,skew,rowOrder)
+ : (TriInFullHelper<S>*)new TriInFullUpperHelper<S>(esz,cppEsz,nr,nc,
+ triangular,hermitian,skew,rowOrder));
+ //else
+ // rep = (esz == 1 ? (TriInFullHelper<S>*)new TriInFullLowerHelper<S>(1,1,nr,nc,
+ // false,true,false,rowOrder)
+ // : (TriInFullHelper<S>*)new TriInFullLowerHelper<S>(esz,cppEsz,nr,nc,
+ // false,true,false,rowOrder));
+ break;
+ }
+
+ case MatrixStructure::Matrix1d: {
+ assert(nr==1 || nc==1);
+ const int length = nr*nc;
+ rep = (esz==1 ? (FullVectorHelper<S>*)new ContiguousVectorScalarHelper<S>(length,rowOrder)
+ : (FullVectorHelper<S>*)new ContiguousVectorHelper<S>(esz,cppEsz,length,rowOrder));
+ break;
+ }
+
+ default:
+ SimTK_ERRCHK1(!"not implemented", "MatrixHelperRep::createOwnerMatrixHelperRep()",
+ "Matrix structure commitment %s not implemented yet.",
+ MatrixStructure::name(structure.getStructure()));
+ }
+
+ return rep;
+}
+
+
+template <class S> MatrixHelperRep<S>*
+MatrixHelperRep<S>::createExternalMatrixHelperRep
+ (int esz, int cppEsz, const MatrixCharacter& actual,
+ int spacing, S* data, bool canWrite)
+{
+ SimTK_ASSERT2((esz==1&&cppEsz==1) || (esz>1&&cppEsz>=esz),
+ "MatrixHelperRep::createExternalMatrixHelperRep(): bad element size esz=%d, cppEsz=%d", esz, cppEsz);
+
+ MatrixHelperRep<S>* rep = 0;
+
+ const int nr = actual.nrow();
+ const int nc = actual.ncol();
+ const MatrixStructure& structure = actual.getStructure();
+ const MatrixStorage& storage = actual.getStorage();
+ const bool rowOrder = (storage.getOrder() == MatrixStorage::RowOrder);
+
+ switch (structure.getStructure()) {
+
+ case MatrixStructure::Full:
+ if (rowOrder)
+ rep = (esz == 1 ? (RegularFullHelper<S>*)new FullRowOrderScalarHelper<S>(nr,nc,
+ spacing,data,canWrite)
+ : (RegularFullHelper<S>*)new FullRowOrderEltHelper<S>(esz, cppEsz,nr,nc,
+ spacing,data,canWrite));
+ else
+ rep = (esz == 1 ? (RegularFullHelper<S>*)new FullColOrderScalarHelper<S>(nr,nc,
+ spacing,data,canWrite)
+ : (RegularFullHelper<S>*)new FullColOrderEltHelper<S>(esz, cppEsz,nr,nc,
+ spacing,data,canWrite));
+ break;
+
+ case MatrixStructure::Triangular:
+ case MatrixStructure::Symmetric:
+ case MatrixStructure::Hermitian:
+ case MatrixStructure::SkewSymmetric:
+ case MatrixStructure::SkewHermitian: {
+ const bool triangular = structure.getStructure() == MatrixStructure::Triangular;
+ const bool hermitian = structure.getStructure() == MatrixStructure::Hermitian
+ || structure.getStructure() == MatrixStructure::SkewHermitian;
+ const bool skew = structure.getStructure() == MatrixStructure::SkewSymmetric
+ || structure.getStructure() == MatrixStructure::SkewHermitian;
+
+ if (/*storage.getPlacement() == MatrixStorage::Upper*/true)
+ rep = (esz == 1 ? (TriInFullHelper<S>*)new TriInFullUpperHelper<S>(1,1,nr,nc,
+ triangular,hermitian,skew,rowOrder,
+ spacing,data,canWrite)
+ : (TriInFullHelper<S>*)new TriInFullUpperHelper<S>(esz,cppEsz,nr,nc,
+ triangular,hermitian,skew,rowOrder,
+ spacing,data,canWrite));
+ //else
+ // rep = (esz == 1 ? (TriInFullHelper<S>*)new TriInFullLowerHelper<S>(1,1,nr,nc,
+ // false,true,false,rowOrder)
+ // : (TriInFullHelper<S>*)new TriInFullLowerHelper<S>(esz,cppEsz,nr,nc,
+ // false,true,false,rowOrder));
+ break;
+ }
+
+ case MatrixStructure::Matrix1d: {
+ assert(nr==1 || nc==1);
+ const int length = nr*nc;
+ const int strideInElements = spacing/esz;
+ if (strideInElements > 1)
+ rep = (esz==1 ? (FullVectorHelper<S>*)new StridedVectorScalarHelper<S>(length,rowOrder,
+ strideInElements,data,canWrite)
+ : (FullVectorHelper<S>*)new StridedVectorHelper<S>(esz,cppEsz,length,rowOrder,
+ strideInElements,data,canWrite));
+ else
+ rep = (esz==1 ? (FullVectorHelper<S>*)new ContiguousVectorScalarHelper<S>(length,rowOrder,
+ data,canWrite)
+ : (FullVectorHelper<S>*)new ContiguousVectorHelper<S>(esz,cppEsz,length,rowOrder,
+ data,canWrite));
+ break;
+ }
+
+ default:
+ SimTK_ERRCHK1(!"not implemented", "MatrixHelperRep::createOwnerMatrixHelperRep()",
+ "Matrix structure commitment %s not implemented yet.",
+ MatrixStructure::name(structure.getStructure()));
+ }
+
+ return rep;
+}
+
+template <class S> void
+MatrixHelperRep<S>::scaleBy(const typename CNT<S>::StdNumber& s) {
+ // XXX -- really, really bad! Optimize for contiguous data!
+ for (int j=0; j<ncol(); ++j)
+ for (int i=0; i<nrow(); ++i)
+ scaleElt(updElt(i,j),s);
+}
+
+template <class S> void
+MatrixHelperRep<S>::addIn(const MatrixHelper<S>& h) {
+ const MatrixHelperRep& hrep = h.getRep();
+
+ assert(nrow()==hrep.nrow() && ncol()==hrep.ncol());
+ assert(getEltSize()==hrep.getEltSize());
+ // XXX -- really, really bad! Optimize for contiguous data, missing views, etc.!
+ for (int j=0; j<ncol(); ++j)
+ for (int i=0; i<nrow(); ++i)
+ addToElt(updElt(i,j),hrep.getElt(i,j));
+}
+template <class S> void
+MatrixHelperRep<S>::addIn(const MatrixHelper<typename CNT<S>::TNeg>& nh) {
+ subIn(reinterpret_cast<const MatrixHelper<S>&>(nh));
+}
+
+template <class S> void
+MatrixHelperRep<S>::subIn(const MatrixHelper<S>& h) {
+ const MatrixHelperRep& hrep = h.getRep();
+
+ assert(nrow()==hrep.nrow() && ncol()==hrep.ncol());
+ assert(getEltSize()==hrep.getEltSize());
+ // XXX -- really, really bad! Optimize for contiguous data, missing views, etc.!
+ for (int j=0; j<ncol(); ++j)
+ for (int i=0; i<nrow(); ++i)
+ subFromElt(updElt(i,j),hrep.getElt(i,j));
+}
+template <class S> void
+MatrixHelperRep<S>::subIn(const MatrixHelper<typename CNT<S>::TNeg>& nh) {
+ addIn(reinterpret_cast<const MatrixHelper<S>&>(nh));
+}
+
+template <class S> void
+MatrixHelperRep<S>::fillWith(const S* eltp) {
+ if (hasContiguousData()) {
+ int len = length();
+ if (getEltSize() == 1)
+ for (int i = 0; i < len; i++)
+ m_data[i] = *eltp;
+ else
+ for (int i = 0; i < len; i++)
+ for (int j = 0; j < getEltSize(); j++)
+ m_data[i*getEltSize()+j] = eltp[j];
+ }
+ else {
+ for (int j=0; j<ncol(); ++j)
+ for (int i=0; i<nrow(); ++i)
+ copyElt(updElt(i,j),eltp);
+ }
+}
+
+// We're copying data from a C++ row-oriented matrix into our general
+// Matrix. In addition to the row ordering, C++ may use different spacing
+// for elements than Simmatrix does. Lucky we know that value!
+template <class S> void
+MatrixHelperRep<S>::copyInByRowsFromCpp(const S* elts) {
+ const int cppRowSz = m_cppEltSize * ncol();
+ // XXX -- really, really bad! Optimize for contiguous data, missing views, etc.!
+ for (int j=0; j<ncol(); ++j)
+ for (int i=0; i<nrow(); ++i)
+ copyElt(updElt(i,j), elts + i*cppRowSz + j*m_cppEltSize);
+}
+
+template <class S>
+MatrixHelperRep<S>::~MatrixHelperRep()
+{
+ if (isOwner())
+ deleteAllocatedMemory(m_data);
+}
+
+template <class S> static void
+dumpElt(const S* p, int sz) {
+ if (sz > 1) std::cout << "{";
+ for (int k=0; k<sz; ++k) {
+ if (k>0) std::cout << " ";
+ std::cout << *p++;
+ }
+ if (sz > 1) std::cout << "}";
+}
+
+template <class S> void
+MatrixHelperRep<S>::dump(const char* msg) const {
+ if (msg)
+ std::cout << std::string(msg) << std::endl;
+ std::cout << "Matrix " << nrow() << " X " << ncol() << " "
+ << getEltSize() << "-scalar entries:" << std::endl;
+ if (nrow()*ncol() == 0) {
+ std::cout << "<EMPTY>" << std::endl;
+ return;
+ }
+
+ S* elt = new S[getEltSize()];
+ const std::streamsize oldsz = std::cout.precision(20);
+ for (int i=0; i<nrow(); ++i) {
+ for (int j=0; j<ncol(); ++j) {
+ if (j>0) std::cout << "\t";
+ getAnyElt(i,j,elt);
+ dumpElt(elt, getEltSize());
+ }
+ std::cout << std::endl;
+ }
+ std::cout.precision(oldsz);
+ delete[] elt;
+}
+
+//----------------------------- RegularFullHelper ------------------------------
+//------------------------------------------------------------------------------
+
+template <class S> VectorHelper<S>*
+RegularFullHelper<S>::createDiagonalView_() {
+ VectorHelper<S>* p = 0;
+ const int length = std::min(this->nrow(), this->ncol());
+ S* data = length ? this->updElt_(0,0) : 0;
+
+ const int strideInScalars = length > 1 ? int(this->getElt_(1,1) - this->getElt_(0,0))
+ : this->m_eltSize;
+ const int strideInElements = strideInScalars / this->m_eltSize;
+
+ // No need for a stride if there's 0 or 1 element, or if the stride is 1. TODO: scalar helper
+ if (strideInElements == 1) {
+ p = (this->m_eltSize==1)
+ ? (VectorHelper<S>*)new ContiguousVectorScalarHelper<S>(length, false, data, false)
+ : (VectorHelper<S>*)new ContiguousVectorHelper<S>(this->m_eltSize, this->m_cppEltSize,
+ length, false, data, false);
+ return p;
+ }
+
+ p = (this->m_eltSize==1)
+ ? (VectorHelper<S>*)new StridedVectorScalarHelper<S>(length, false, strideInElements, data, false)
+ : (VectorHelper<S>*)new StridedVectorHelper<S>(this->m_eltSize, this->m_cppEltSize,
+ length, false, strideInElements, data, false);
+ return p;
+}
+
+template <class S> VectorHelper<S>*
+RegularFullHelper<S>::createColumnView_(int j, int i, int m) {
+ VectorHelper<S>* p = 0;
+ S* data = m ? this->updElt_(i,j) : 0;
+
+ const int strideInScalars = m > 1 ? int(this->getElt_(i+1,j) - this->getElt_(i,j)) : this->m_eltSize;
+ const int strideInElements = strideInScalars / this->m_eltSize;
+
+ // No need for a stride if there's 0 or 1 element, or if the stride is 1. TODO: scalar helper
+ if (strideInElements == 1) {
+ p = (this->m_eltSize==1)
+ ? (VectorHelper<S>*)new ContiguousVectorScalarHelper<S>(m, false, data, false)
+ : (VectorHelper<S>*)new ContiguousVectorHelper<S>(this->m_eltSize, this->m_cppEltSize,
+ m, false, data, false);
+ return p;
+ }
+
+ p = (this->m_eltSize==1)
+ ? (VectorHelper<S>*)new StridedVectorScalarHelper<S>(m, false, strideInElements, data, false)
+ : (VectorHelper<S>*)new StridedVectorHelper<S>(this->m_eltSize, this->m_cppEltSize,
+ m, false, strideInElements, data, false);
+ return p;
+}
+
+template <class S> VectorHelper<S>*
+RegularFullHelper<S>::createRowView_(int i, int j, int n) {
+ VectorHelper<S>* p = 0;
+ S* data = n ? this->updElt_(i,j) : 0;
+
+ const int strideInScalars = n > 1 ? int(this->getElt_(i,j+1) - this->getElt_(i,j))
+ : this->m_eltSize;
+ const int strideInElements = strideInScalars / this->m_eltSize;
+
+ // No need for a stride if there's 0 or 1 element, or if the stride is 1. TODO: scalar helper
+ if (strideInElements == 1) {
+ p = (this->m_eltSize==1)
+ ? (VectorHelper<S>*)new ContiguousVectorScalarHelper<S>(n, true, data, false)
+ : (VectorHelper<S>*)new ContiguousVectorHelper<S>(this->m_eltSize, this->m_cppEltSize,
+ n, true, data, false);
+ return p;
+ }
+
+ p = (this->m_eltSize==1)
+ ? (VectorHelper<S>*)new StridedVectorScalarHelper<S>(n, true, strideInElements, data, false)
+ : (VectorHelper<S>*)new StridedVectorHelper<S>(this->m_eltSize, this->m_cppEltSize,
+ n, true, strideInElements, data, false);
+ return p;
+}
+
+
+//------------------------------ TriInFullHelper -------------------------------
+//------------------------------------------------------------------------------
+
+template <class S> VectorHelper<S>*
+TriInFullHelper<S>::createDiagonalView_() {
+ SimTK_ERRCHK_ALWAYS(!hasKnownDiagonal(), "TriInFullHelper::createDiagonalView_()",
+ "Diagonal view of a known-diagonal matrix is not yet implemented. Sorry.");
+
+ VectorHelper<S>* p = 0;
+ const int length = std::min(this->nrow(), this->ncol());
+ S* data = length ? this->updElt_(0,0) : 0;
+
+ const int strideInScalars = length > 1 ? int(this->getElt_(1,1) - this->getElt_(0,0))
+ : this->m_eltSize;
+ const int strideInElements = strideInScalars / this->m_eltSize;
+
+ // No need for a stride if there's 0 or 1 element, or if the stride is 1. TODO: scalar helper
+ if (strideInElements == 1) {
+ p = (this->m_eltSize==1)
+ ? (VectorHelper<S>*)new ContiguousVectorScalarHelper<S>(length, false, data, false)
+ : (VectorHelper<S>*)new ContiguousVectorHelper<S>(this->m_eltSize, this->m_cppEltSize,
+ length, false, data, false);
+ return p;
+ }
+
+ p = (this->m_eltSize==1)
+ ? (VectorHelper<S>*)new StridedVectorScalarHelper<S>(length, false, strideInElements, data, false)
+ : (VectorHelper<S>*)new StridedVectorHelper<S>(this->m_eltSize, this->m_cppEltSize,
+ length, false, strideInElements, data, false);
+ return p;
+}
+
+//------------------------------ INSTANTIATIONS --------------------------------
+// Instantiations for each of the 12 Scalar types.
+// It isn't actually necessary to instantiate these classes since they
+// are only used internally. However, this is a good way to make sure that
+// everything supplied at least compiles. Otherwise you won't find errors
+// until all the code is actually used.
+
+#define INSTANTIATE(Helper) \
+template class Helper< float >; \
+template class Helper< double >; \
+template class Helper< std::complex<float> >; \
+template class Helper< std::complex<double> >; \
+template class Helper< conjugate<float> >; \
+template class Helper< conjugate<double> >; \
+template class Helper< negator< float > >; \
+template class Helper< negator< double > >; \
+template class Helper< negator< std::complex<float> > >; \
+template class Helper< negator< std::complex<double> > >; \
+template class Helper< negator< conjugate<float> > >; \
+template class Helper< negator< conjugate<double> > >
+
+
+INSTANTIATE(MatrixHelper);
+INSTANTIATE(MatrixHelperRep);
+
+INSTANTIATE(FullHelper);
+INSTANTIATE(RegularFullHelper);
+
+INSTANTIATE(TriHelper);
+
+INSTANTIATE(VectorHelper);
+INSTANTIATE(FullVectorHelper);
+INSTANTIATE(ContiguousVectorHelper);
+INSTANTIATE(ContiguousVectorScalarHelper);
+INSTANTIATE(StridedVectorHelper);
+INSTANTIATE(StridedVectorScalarHelper);
+INSTANTIATE(IndexedVectorHelper);
+
+} // namespace SimTK
diff --git a/SimTKcommon/BigMatrix/src/MatrixHelperRep.h b/SimTKcommon/BigMatrix/src/MatrixHelperRep.h
new file mode 100644
index 0000000..d65a775
--- /dev/null
+++ b/SimTKcommon/BigMatrix/src/MatrixHelperRep.h
@@ -0,0 +1,772 @@
+#ifndef SimTK_SimTKCOMMON_MATRIX_HELPER_REP_H_
+#define SimTK_SimTKCOMMON_MATRIX_HELPER_REP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Scalar.h"
+#include "SimTKcommon/SmallMatrix.h"
+
+#include "ElementFilter.h"
+
+#include <cstddef>
+
+namespace SimTK {
+template <class S> class MatrixHelper; // the helper handle class
+template <class S> class MatrixHelperRep; // implementation base class
+
+template <class S> class FullHelper; // all elements stored
+template <class S> class TriHelper; // half the elements are stored
+template <class S> class VectorHelper; // 1-d array of elements
+
+/* --------------------------- MatrixHelperRep ---------------------------------
+ *
+ * This is the private implementation of the MatrixHelper<S> class. Note that
+ * this must be explicitly instantiated in library-side source code for all
+ * possible values of the template parameter S, which is just the set of all
+ * SimTK scalar types.
+ *
+ * Every MatrixHelperRep has the following members:
+ * - handle properties
+ * - matrix character commitment
+ * - actual matrix character
+ * - pointer to memory
+ * - pointer to memory's reference counter
+ *
+ * Concrete derivatives of MatrixHelperRep contain additional information
+ * used to map logical element indices to physical memory.
+ *
+ * There are some extremely performance-sensitive aspects to this object,
+ * especially with regard to selecting individual elements of a matrix. In order
+ * to get here we have already had to follow the pointer in the Matrix handle.
+ * We would like to be able to return an element in the fewest additional
+ * instructions possible. This is made difficult by the variety of run time
+ * features we want to support here:
+ * - writable vs. read-only access to data
+ * - row- and column-ordered data
+ * - scalar elements vs. composite ones
+ * - 1d Vector and RowVector objects vs. 2d Matrix objects
+ * - various ways of selecting meaningful elements from memory based
+ * on regular and irregular spacing
+ *
+ * If we had to check flags for each of these possibilities before finally
+ * indexing the element of interest, access would be substantially slowed. We
+ * are particularly concerned about performance for the most common case
+ * of regularly-spaced scalar elements. Instead of checking flags at run time,
+ * we want to make use of the virtual function table so that the all of the
+ * above tests can be avoided with a single indirect call through the virtual
+ * function branch table. At run time the compiler will then translate a call
+ * like "rep->getElt(i,j)" to "call rep->vft[getElt](i,j)" where the particular
+ * function called is the right one given this rep's particular combination of
+ * all the possibilities mentioned above, which is encapsulated in the concrete
+ * type of the MatrixHelperRep object. Attempts to call update methods of
+ * non-writable reps will call directly to methods which throw an appropriate
+ * error condition.
+ *
+ * Sorting out all these issues at compile time requires a large number of
+ * small classes. All classes are templatized by Scalar type <S> and explicitly
+ * instantiated for all the SimTK scalar types.
+ *
+ * At handle construction, a suitable default helper is assigned with a minimal
+ * handle commitment derived from the handle type -- element size, column or
+ * row outline, and whether the handle must be a view. Everything else will be
+ * uncommitted. If a later assignment requires a different concrete helper, the
+ * original one will be replaced but the commitments will remain unchanged.
+ * Note that MatrixHelpers cannot be shared; each is paired with just one
+ * handle.
+ * ---------------------------------------------------------------------------*/
+template <class S>
+class MatrixHelperRep {
+ typedef MatrixHelperRep<S> This;
+ typedef MatrixHelperRep<typename CNT<S>::TNeg> ThisNeg;
+ typedef MatrixHelperRep<typename CNT<S>::THerm> ThisHerm;
+public:
+ typedef typename CNT<S>::Number Number; // strips off negator from S
+ typedef typename CNT<S>::StdNumber StdNumber; // turns conjugate to complex
+ typedef typename CNT<S>::Precision Precision; // strips off complex from StdNumber
+ typedef typename CNT<S>::TNeg SNeg; // the negated version of S
+ typedef typename CNT<S>::THerm SHerm; // the conjugate version of S
+
+ // Constructors are protected; for use by derived classes only.
+
+ // This is the MatrixHelper factory method for owner matrices. Given a
+ // particular matrix character, this method will deliver the best
+ // MatrixHelperRep subclass with those characteristics.
+ static This* createOwnerMatrixHelperRep(int esz, int cppEsz,
+ const MatrixCharacter&);
+
+ // This is the MatrixHelper factory method for matrices providing a view
+ // into externally-allocated data. The supplied MatrixCharacter describes
+ // the actual properties of the external data as we want it to appear
+ // through this view. Const and non-const methods are provided. There is
+ // provision for a "spacing" argument for the external data; this may
+ // mean different things. Typically it is the leading dimensions for
+ // full-storage matrices, and the stride for vectors. We'll return the
+ // best MatrixHelperRep we can come up with for these characteristics; it
+ // will not be an owner rep.
+ static This* createExternalMatrixHelperRep
+ (int esz, int cppEsz, const MatrixCharacter& actual,
+ int spacing, S* data, bool canWrite=true);
+
+ // This is the factory for const access to externally-allocated storage.
+ static This* createExternalMatrixHelperRep
+ (int esz, int cppEsz, const MatrixCharacter& actual, int spacing,
+ const S* data)
+ { return createExternalMatrixHelperRep(esz, cppEsz, actual, spacing,
+ const_cast<S*>(data), false); }
+
+ // Here we have determined that this source is acceptable for the data
+ // already allocated in this handle. In particular both dimensions match.
+ void copyInFromCompatibleSource(const MatrixHelperRep<S>& source) {
+ if (!m_writable)
+ SimTK_THROW1(Exception::OperationNotAllowedOnNonconstReadOnlyView,
+ "assignment");
+ copyInFromCompatibleSource_(source);
+ }
+
+ // Same as above except the source is negated as it is copied in.
+ void negatedCopyInFromCompatibleSource(const ThisNeg& source) {
+ copyInFromCompatibleSource
+ (reinterpret_cast<const MatrixHelperRep<S>&>(source));
+ scaleBy(StdNumber(-1));
+ }
+
+ // Fill every element with repeated copies of a single scalar value.
+ // The default implementation is very slow.
+ void fillWithScalar(const StdNumber& scalar) {
+ if (!m_writable)
+ SimTK_THROW1(Exception::OperationNotAllowedOnNonconstReadOnlyView,
+ "fillWithScalar()");
+ fillWithScalar_(scalar);
+ }
+
+ // These are used in copy constructors. Hence the resulting value must
+ // be the same as the source value, even if we're removing a negator<> from
+ // the elements. So the createNegatedDeepCopy actually has to negate every
+ // element with (yuck) floating point operations. Note that the returned
+ // Matrix is always an owner, writable, and has whatever is the most
+ // efficient storage type for the source data.
+ This* createDeepCopy() const {return createDeepCopy_();}
+ ThisNeg* createNegatedDeepCopy() const {
+ ThisNeg* p = reinterpret_cast<const ThisNeg*>(this)->createDeepCopy_();
+ p->scaleBy(StdNumber(-1));
+ return p;
+ }
+
+ This* createWholeView(bool wantToWrite) const {
+ This* p = cloneHelper_();
+ p->m_owner = false;
+ p->m_writable = m_writable && wantToWrite;
+ p->m_data = m_data;
+ p->m_actual = m_actual;
+ return p;
+ }
+
+
+ ThisHerm* createHermitianView(bool wantToWrite) const {
+ const ThisHerm* thisHerm = reinterpret_cast<const ThisHerm*>(this);
+ ThisHerm* p = const_cast<ThisHerm*>(thisHerm)->createTransposeView_();
+ p->m_writable = m_writable && wantToWrite;
+ p->m_actual.setActualSize(ncol(),nrow());
+ return p;
+ }
+
+
+ This* createBlockView(const EltBlock& block, bool wantToWrite) const {
+ SimTK_SIZECHECK(block.row0(), nrow()-block.nrow(),
+ "MatrixHelperRep::createBlockView()");
+ SimTK_SIZECHECK(block.col0(), ncol()-block.ncol(),
+ "MatrixHelperRep::createBlockView()");
+ This* p = const_cast<This*>(this)->createBlockView_(block);
+ p->m_writable = m_writable && wantToWrite;
+ p->m_actual.setActualSize(block.nrow(), block.ncol());
+ return p;
+ }
+ This* createRegularView(const EltBlock& block, const EltIndexer& ix,
+ bool wantToWrite) const {
+ SimTK_SIZECHECK(block.row0(), nrow()-ix.row(block.nrow(), block.ncol()),
+ "MatrixHelperRep::createRegularView()");
+ SimTK_SIZECHECK(block.col0(), ncol()-ix.col(block.nrow(), block.ncol()),
+ "MatrixHelperRep::createRegularView()");
+ This* p = const_cast<This*>(this)->createRegularView_(block, ix);
+ p->m_writable = m_writable && wantToWrite;
+ p->m_actual.setActualSize(block.nrow(), block.ncol());
+ return p;
+ }
+
+ // 1-d views //
+
+ VectorHelper<S>* createDiagonalView(bool wantToWrite) const {
+ VectorHelper<S>* p = const_cast<This*>(this)->createDiagonalView_();
+ p->m_writable = m_writable && wantToWrite;
+ p->m_actual.setActualSize(std::min(nrow(),ncol()), 1); // a column
+ return p;
+ }
+ // Select column j or part of column j.
+ VectorHelper<S>* createColumnView(int j, int i, int m, bool wantToWrite) const {
+ SimTK_INDEXCHECK(j, ncol(), "MatrixHelperRep::createColumnView()");
+ SimTK_SIZECHECK(i, nrow()-m, "MatrixHelperRep::createColumnView()");
+ VectorHelper<S>* p = const_cast<This*>(this)->createColumnView_(j,i,m);
+ p->m_writable = m_writable && wantToWrite;
+ p->m_actual.setActualSize(m, 1); // a column
+ return p;
+ }
+ // Select row i or part of row i.
+ VectorHelper<S>* createRowView(int i, int j, int n, bool wantToWrite) const {
+ SimTK_INDEXCHECK(i, nrow(), "MatrixHelperRep::createRowView()");
+ SimTK_SIZECHECK(j, ncol()-n, "MatrixHelperRep::createRowView()");
+ VectorHelper<S>* p = const_cast<This*>(this)->createRowView_(i,j,n);
+ p->m_writable = m_writable && wantToWrite;
+ p->m_actual.setActualSize(1, n); // a row
+ return p;
+ }
+
+ // Is it more efficient to access this Matrix in row order rather than
+ // the default column order? If so, you should try to operate on
+ // all columns of each row before moving on to the next row. Otherwise,
+ // try to operate on all rows of each column before moving on
+ // to the next column.
+ bool preferRowOrder() const {return preferRowOrder_();}
+
+ // Is the memory that we ultimately reference organized contiguously?
+ bool hasContiguousData() const {return hasContiguousData_();}
+
+ // Using *element* indices, obtain a pointer to the beginning of a
+ // particular element. This is always a slow operation compared to raw
+ // array access; use sparingly.
+ //
+ // These inline base class interface routines provide Debug-mode range
+ // checking but evaporate completely in Release mode so that the underlying
+ // virtual method is called directly.
+ const S* getElt(int i, int j) const {
+ SimTK_INDEXCHECK(i, nrow(), "MatrixHelperRep::getElt(i,j)");
+ SimTK_INDEXCHECK(j, ncol(), "MatrixHelperRep::getElt(i,j)");
+ return getElt_(i,j);
+ }
+ S* updElt(int i, int j) {
+ SimTK_INDEXCHECK(i, nrow(), "MatrixHelperRep::updElt(i,j)");
+ SimTK_INDEXCHECK(j, ncol(), "MatrixHelperRep::updElt(i,j)");
+ SimTK_ERRCHK(m_writable, "MatrixHelperRep::updElt()",
+ "Matrix not writable.");
+ return updElt_(i,j);
+ }
+
+ void getAnyElt(int i, int j, S* value) const {
+ SimTK_INDEXCHECK(i, nrow(), "MatrixHelperRep::getAnyElt(i,j)");
+ SimTK_INDEXCHECK(j, ncol(), "MatrixHelperRep::getAnyElt(i,j)");
+ SimTK_ERRCHK(value, "MatrixHelperRep::getAnyElt()",
+ "The value return pointer must not be null.");
+ return getAnyElt_(i,j,value);
+ }
+
+ const S* getElt(int i) const {
+ SimTK_INDEXCHECK(i, length(), "MatrixHelperRep::getElt(i)");
+ return getElt_(i);
+ }
+ S* updElt(int i) {
+ SimTK_INDEXCHECK(i, length(), "MatrixHelperRep::updElt(i)");
+ SimTK_ERRCHK(m_writable, "MatrixHelperRep::updElt(i)",
+ "Matrix not writable.");
+ return updElt_(i);
+ }
+
+ void getAnyElt(int i, S* value) const {
+ SimTK_INDEXCHECK(i, length(), "MatrixHelperRep::getAnyElt(i)");
+ SimTK_ERRCHK(value, "MatrixHelperRep::getAnyElt()",
+ "The value return pointer must not be null.");
+ return getAnyElt_(i,value);
+ }
+
+ // Many matrix storage formats assume values for some of their elements
+ // and thus those elements are not stored. Generic algorithms must avoid
+ // writing on such elements, so we expect to be able to query the concrete
+ // class to find out if particular elements actually have a memory
+ // representation.
+ //
+ // Note: for symmetric matrices only one of (i,j) and (j,i) should return
+ // true here, and for unit diagonals (i,i) will return false.
+ bool eltIsStored(int i, int j) const {
+ SimTK_INDEXCHECK(i, nrow(), "MatrixHelperRep::eltIsStored(i,j)");
+ SimTK_INDEXCHECK(j, ncol(), "MatrixHelperRep::eltIsStored(i,j)");
+ return eltIsStored_(i,j);
+ }
+ bool eltIsStored(int i) const {
+ SimTK_INDEXCHECK(i, length(), "MatrixHelperRep::eltIsStored(i)");
+ return eltIsStored_(i);
+ }
+
+ // Add up all the *elements*, returning the answer in the element given
+ // by pointer to its first scalar.
+ void sum(S* eltp) const {sum_(eltp);}
+ void colSum(int j, S* eltp) const {colSum_(j,eltp);}
+ void rowSum(int i, S* eltp) const {rowSum_(i,eltp);}
+
+
+
+ // Resizing is permitted only when one of these two conditions is met:
+ // 1. The matrix already has the indicated dimensions,
+ // OR 2. (a) this handle is the owner of the data,
+ // AND (b) handle commitment permits resizing of at least one dimension,
+ // AND (c) data has not been locked.
+ void resize(int m, int n, bool keep) {
+ SimTK_SIZECHECK_NONNEG(m, "MatrixHelperRep::resize()");
+ SimTK_SIZECHECK_NONNEG(n, "MatrixHelperRep::resize()");
+ if (m==nrow() && n==ncol()) return;
+ if (!m_owner)
+ SimTK_THROW1(Exception::OperationNotAllowedOnView, "resize()");
+ // owner
+ if (m_handleIsLocked)
+ SimTK_THROW1(Exception::Cant,
+ "resize(), because owner handle is locked.");
+ if (!m_commitment.isSizeOK(m,n))
+ SimTK_THROW1(Exception::Cant,
+ "resize(), because handle commitment doesn't allow this size.");
+ if (keep) resizeKeep_(m,n);
+ else resize_(m,n);
+ m_actual.setActualSize(m,n);
+ }
+
+
+ // addition and subtraction (+= and -=)
+ void addIn(const MatrixHelper<S>&);
+ void addIn(const MatrixHelper<typename CNT<S>::TNeg>&);
+ void subIn(const MatrixHelper<S>&);
+ void subIn(const MatrixHelper<typename CNT<S>::TNeg>&);
+
+ // Fill all our stored data with copies of the same supplied element.
+ void fillWith(const S* eltp);
+
+ // We're copying data from a C++ row-oriented matrix into our general
+ // Matrix. In addition to the row ordering, C++ may use different spacing
+ // for elements than Simmatrix does. Lucky we know that value!
+ void copyInByRowsFromCpp(const S* elts);
+
+ // Scalar multiply (and divide). This is useful no matter what the
+ // element structure and will produce the correct result.
+ void scaleBy(const StdNumber&);
+
+ // If this is a full or symmetric square matrix with scalar elements,
+ // it can be inverted in place (although that's not the best way to
+ // solve linear equations!).
+ void invertInPlace() {
+ if (!m_writable)
+ SimTK_THROW1(Exception::OperationNotAllowedOnNonconstReadOnlyView,
+ "invertInPlace()");
+ if (nrow() != ncol())
+ SimTK_THROW1(Exception::Cant,
+ "invertInPlace(): matrix must be square");
+ invertInPlace_();
+ }
+
+ void dump(const char* msg) const;
+
+ // See comment in MatrixBase::matmul for an explanation.
+ template <class SA, class SB>
+ void matmul(const StdNumber& beta, // applied to 'this'
+ const StdNumber& alpha, const MatrixHelper<SA>& A, const MatrixHelper<SB>& B);
+
+ // Bookkeeping //
+
+
+ // Dimensions come from the "actual" Matrix character. These are the *logical*
+ // dimensions, in elements (not scalars), and have nothing to do with how much
+ // memory is being used to store these elements.
+ int nrow() const {return m_actual.nrow();}
+ int ncol() const {return m_actual.ncol();}
+
+ // This is only for 1D vectors; their lengths must fit in an "int". For
+ // a 2D matrix use nelt() to get the total number of elements.
+ int length() const {assert(nrow()==1||ncol()==1); return nrow()*ncol();}
+
+ ptrdiff_t nelt() const {return ptrdiff_t(nrow()) * ptrdiff_t(ncol());}
+ ptrdiff_t nScalars() const {return nelt()*m_eltSize;}
+
+ // Does this handle own the data descriptor it points to?
+ int getEltSize() const {return m_eltSize;}
+ int getCppEltSize() const {return m_cppEltSize;}
+ bool isWritable() const {return m_writable;}
+ bool isOwner() const {return m_owner;}
+ bool isClear() const {return m_data==0;}
+
+ void lockHandle() {m_handleIsLocked=true;}
+ void unlockHandle() {m_handleIsLocked=false;}
+ bool isHandleLocked() const {return m_handleIsLocked;}
+
+
+ const MatrixCommitment& getCharacterCommitment() const {return m_commitment;}
+ const MatrixCharacter& getMatrixCharacter() const {return m_actual;}
+
+ // Get a pointer to the raw data viewed various ways.
+ const S* getData() const {assert(m_data); return m_data;}
+ S* updData() {assert(m_data); return m_data;}
+
+ const SNeg* getDataNeg() const
+ { return reinterpret_cast<const SNeg*>(getData()); }
+ SNeg* updDataNeg()
+ { return reinterpret_cast<SNeg*>(updData()); }
+ const SHerm* getDataHerm() const
+ { return reinterpret_cast<const SHerm*>(getData()); }
+ SHerm* updDataHerm()
+ { return reinterpret_cast<SHerm*>(updData()); }
+
+ // Delete the data if necessary, and leave the data pointer null. If this
+ // is not an owner handle, we just clear the pointer. If it is an owner
+ // handle, we check that it isn't locked and if it isn't we'll return the
+ // allocated data to the heap. It is an error to attempt to clear the
+ // data while the handle is locked.
+ void clearData() {
+ if (m_handleIsLocked) {
+ SimTK_THROW1(Exception::Cant,
+ "MatrixHelperRep::clearData(): handle is locked.");
+ return;
+ }
+ if (m_owner)
+ deleteAllocatedMemory(m_data);
+ m_data = 0;
+ }
+
+ // Given a number of elements (not scalars), allocate
+ // just enough memory to hold that many elements in packed storage.
+ void allocateData(ptrdiff_t nelt) {
+ assert(nelt >= 0);
+ assert(m_owner && m_data==0);
+ if (m_handleIsLocked) {
+ SimTK_THROW1(Exception::Cant,
+ "MatrixHelperRep::allocateData(): handle is locked.");
+ return;
+ }
+ m_data = allocateMemory(nelt);
+ }
+
+ // Given dimensions in number of elements (not scalars), allocate
+ // just enough memory to hold m*n elements in packed storage.
+ void allocateData(int m, int n)
+ { assert(m>=0 && n>=0);
+ allocateData(ptrdiff_t(m) * ptrdiff_t(n)); }
+
+ // Allocate new heap space to hold nelt densely-packed elements each
+ // composed of m_eltSize scalars of type S. We actually allocate an array of
+ // the underlying Precision type (float or double) to avoid any default
+ // construction of more complicated elements like complex. If we're in Debug
+ // mode, we'll initialize the resulting data to NaN, otherwise we won't
+ // touch it. If nelt is zero we return a null pointer.
+ S* allocateMemory(ptrdiff_t nElt) const {
+ assert(nElt >= 0);
+ if (nElt==0)
+ return 0;
+ assert(sizeof(S) % sizeof(Precision) == 0);
+ const ptrdiff_t nPrecPerElt = (sizeof(S)/sizeof(Precision))*m_eltSize;
+ const ptrdiff_t nPrec = nElt * nPrecPerElt;
+ Precision* p = new Precision[nPrec];
+ #ifndef NDEBUG
+ const Precision nan = CNT<Precision>::getNaN();
+ for (ptrdiff_t i=0; i < nPrec; ++i)
+ p[i] = nan;
+ #endif
+ return reinterpret_cast<S*>(p);
+ }
+ // Allocate enough memory to hold m*n elements. m and n are ints, but their
+ // product may not fit in an int, so we use ptrdiff_t which will be a 64-bit
+ // signed integer on a 64 bit machine.
+ S* allocateMemory(int m, int n) const
+ { assert(m>=0 && n>=0);
+ return allocateMemory(ptrdiff_t(m) * ptrdiff_t(n)); }
+
+ // Use this method to delete help space that you allocated using
+ // allocateNewData() above. We recast it back to the form in which it was
+ // allocated before deleting which will keep the heap system happy and also
+ // prevent the calling of any element destructor that might be present for
+ // the fancier scalar types.
+ static void deleteAllocatedMemory(S* mem) {
+ Precision* p = reinterpret_cast<Precision*>(mem);
+ delete[] p;
+ }
+
+ // Use setData only when there isn't already data in this handle. If this
+ // is an owner handle we're taking over responsibility for the heap space.
+ void setData(S* datap) {assert(!m_data); m_data = datap;}
+ void setOwner(bool isOwner) {m_owner=isOwner;}
+
+ // Single element manipulation: VERY SLOW, use sparingly.
+ void copyElt(S* dest, const S* src) const
+ { for (int k=0; k<m_eltSize; ++k) dest[k] = src[k]; }
+ void copyAndScaleElt(S* dest, const StdNumber& s, const S* src) const
+ { for (int k=0; k<m_eltSize; ++k) dest[k] = s*src[k]; }
+ void copyAndNegateElt(S* dest, const S* src) const
+ { for (int k=0; k<m_eltSize; ++k) dest[k] = CNT<S>::negate(src[k]); }
+ void copyAndConjugateElt(S* dest, const S* src) const
+ { for (int k=0; k<m_eltSize; ++k) dest[k] = CNT<S>::transpose(src[k]); }
+ void copyAndNegConjugateElt(S* dest, const S* src) const
+ { for (int k=0; k<m_eltSize; ++k) dest[k] = -CNT<S>::transpose(src[k]); }
+
+ void fillElt(S* dest, const StdNumber& src) const
+ { for (int k=0; k<m_eltSize; ++k) dest[k] = src; }
+ void addToElt(S* dest, const S* src) const
+ { for (int k=0; k<m_eltSize; ++k) dest[k] += src[k]; }
+ void subFromElt(S* dest, const S* src) const
+ { for (int k=0; k<m_eltSize; ++k) dest[k] -= src[k]; }
+ void scaleElt(S* dest, const StdNumber& s) const
+ { for (int k=0; k<m_eltSize; ++k) dest[k] *= s; }
+
+protected:
+ //--------------------------------------------------------------------------
+ // ABSTRACT INTERFACE
+ // This is the complete set of virtual methods which can be overridden
+ // by classes derived from MatrixHelperRep. All the virtual methods have
+ // names ending in an underscore "_". The base class provides an inline
+ // interface method of the same name but without the underscore. That
+ // method performs operations common to all the implementations, such
+ // as checking arguments and verifying that the handle permits the
+ // operation. It then transfers control to the virtual method, the
+ // various implementations of which do not have to repeat the common
+ // work.
+ //--------------------------------------------------------------------------
+
+ // DESTRUCTOR
+ // Any concrete class that uses additional heap space must provide
+ // a destructor.
+
+ virtual ~MatrixHelperRep();
+
+ // PURE VIRTUALS
+ // All concrete classes must provide implementations.
+
+ virtual MatrixHelperRep* createDeepCopy_() const = 0;
+
+ // Make a clone of the current helper, except that the data and
+ // myHandle pointer are left null.
+ virtual This* cloneHelper_() const = 0;
+
+
+ virtual This* createBlockView_(const EltBlock&) = 0;
+ virtual This* createTransposeView_() = 0;
+ virtual This* createRegularView_(const EltBlock&, const EltIndexer&) = 0;
+
+ virtual VectorHelper<S>* createDiagonalView_() = 0;
+ virtual VectorHelper<S>* createColumnView_(int j, int i, int m) = 0;
+ virtual VectorHelper<S>* createRowView_(int i, int j, int n) = 0;
+
+ virtual bool preferRowOrder_() const = 0;
+ virtual bool hasContiguousData_() const = 0;
+ virtual bool hasRegularData_() const = 0;
+ virtual bool eltIsStored_(int i, int j) const = 0;
+ virtual const S* getElt_(int i, int j) const = 0;
+ virtual S* updElt_(int i, int j) = 0;
+ virtual void getAnyElt_(int i, int j, S* value) const = 0;
+
+ // OPTIONAL FUNCTIONALITY
+ // Optional methods. No default implementation. A derived class can
+ // supply these, but if it doesn't then this functionality will not
+ // be available for matrices which use that class as a helper.
+
+ virtual void resize_(int m, int n) {
+ SimTK_THROW1(Exception::Cant,
+ "resize_() not implemented for this kind of matrix");
+ }
+ virtual void resizeKeep_(int m, int n) {
+ SimTK_THROW1(Exception::Cant,
+ "resizeKeep_() not implemented for this kind of matrix");
+ }
+
+ // If this gets called we know the matrix is writable and square.
+ virtual void invertInPlace_() {
+ SimTK_THROW1(Exception::Cant,
+ "invertInPlace_() not implemented for this kind of matrix");
+ }
+
+ // One-index versions of above two-index methods for use in Vector
+ // helpers.
+ virtual bool eltIsStored_(int i) const {
+ SimTK_THROW1(Exception::Cant,
+ "One-index eltIsStored_() not available for 2D matrices");
+ return false;
+ }
+ virtual const S* getElt_(int i) const {
+ SimTK_THROW1(Exception::Cant,
+ "One-index getElt_() not available for 2D matrices");
+ return 0;
+ }
+
+ virtual S* updElt_(int i) {
+ SimTK_THROW1(Exception::Cant,
+ "One-index updElt_() not available for 2D matrices");
+ return 0;
+ }
+
+ virtual void getAnyElt_(int i, S* value) const {
+ SimTK_THROW1(Exception::Cant,
+ "One-index getAnyElt_() not available for 2D matrices");
+ }
+
+ virtual void resize_(int n) {
+ SimTK_THROW1(Exception::Cant,
+ "One-index resize_() not available for 2D matrices");
+ }
+
+ virtual void resizeKeep_(int n) {
+ SimTK_THROW1(Exception::Cant,
+ "One-index resizeKeep_() not available for 2D matrices");
+ }
+
+
+
+ // VIRTUALS WITH DEFAULT IMPLEMENTATIONS
+ // This functionality is required of all MatrixHelpers, but there is
+ // a base class default implementation here, slow but functional.
+ // In many cases a concrete class can do much better because of its
+ // intimate knowledge of the data layout; override if you can.
+
+
+ // Overridable method to implement copyInFromCompatibleSource().
+ // The default implementation works but is very slow.
+ virtual void copyInFromCompatibleSource_(const MatrixHelperRep<S>& source) {
+ if (preferRowOrder_())
+ for (int i=0; i<nrow(); ++i)
+ for (int j=0; j<ncol(); ++j) {
+ if (eltIsStored_(i,j))
+ copyElt(updElt_(i,j), source.getElt_(i,j));
+ }
+ else // column order (rows vary fastest)
+ for (int j=0; j<ncol(); ++j)
+ for (int i=0; i<nrow(); ++i) {
+ if (eltIsStored_(i,j))
+ copyElt(updElt_(i,j), source.getElt_(i,j));
+ }
+ }
+
+ // Overridable method to implement fillWithScalar().
+ // The default implementation works but is very slow.
+ virtual void fillWithScalar_(const StdNumber& scalar) {
+ if (preferRowOrder_())
+ for (int i=0; i<nrow(); ++i)
+ for (int j=0; j<ncol(); ++j) {
+ if (eltIsStored_(i,j))
+ fillElt(updElt_(i,j), scalar);
+ }
+ else // column order (rows vary fastest)
+ for (int j=0; j<ncol(); ++j)
+ for (int i=0; i<nrow(); ++i) {
+ if (eltIsStored_(i,j))
+ fillElt(updElt_(i,j), scalar);
+ }
+ }
+
+ virtual void colSum_(int j, S* csum) const {
+ fillElt(csum, 0);
+ for (int i=0; i < nrow(); ++i)
+ addToElt(csum, getElt_(i,j));
+ }
+
+ virtual void rowSum_(int i, S* rsum) const {
+ fillElt(rsum, 0);
+ for (int j=0; j < ncol(); ++j)
+ addToElt(rsum, getElt_(i,j));
+ }
+ virtual void sum_(S* esum) const {
+ fillElt(esum, 0);
+ S* tsum = new S[m_eltSize]; // temporary variable for row or col sums
+ if (preferRowOrder_()) // i.e., row sums are cheaper
+ for (int i=0; i < nrow(); ++i)
+ { rowSum_(i, tsum); addToElt(esum, tsum); }
+ else // col sums are cheaper
+ for (int j=0; j < ncol(); ++j)
+ { colSum_(j, tsum); addToElt(esum, tsum); }
+ delete[] tsum;
+ }
+
+
+
+protected:
+ MatrixHelperRep(int esz, int cppesz)
+ : m_data(0), m_actual(), m_writable(false),
+ m_eltSize(esz), m_cppEltSize(cppesz),
+ m_canBeOwner(true), m_owner(false),
+ m_handleIsLocked(false), m_commitment(), m_handle(0) {}
+
+ MatrixHelperRep(int esz, int cppesz, const MatrixCommitment& commitment)
+ : m_data(0), m_actual(), m_writable(false),
+ m_eltSize(esz), m_cppEltSize(cppesz),
+ m_canBeOwner(true), m_owner(false),
+ m_handleIsLocked(false), m_commitment(commitment), m_handle(0) {}
+
+ // Copy constructor copies just the base class members, and *not* the data.
+ // We assume we don't have write access and that we aren't going to be
+ // the data owner. The handle character commitment and actual character are
+ // copied from the source, but may need to be changed by the caller.
+ MatrixHelperRep(const MatrixHelperRep& src)
+ : m_data(0), m_actual(src.m_actual), m_writable(false),
+ m_eltSize(src.m_eltSize), m_cppEltSize(src.m_cppEltSize),
+ m_canBeOwner(true), m_owner(false),
+ m_handleIsLocked(false), m_commitment(src.m_commitment), m_handle(0) {}
+
+ // Properties of the actual matrix //
+
+ // Raw memory holding the elements of this matrix, as an array of Scalars.
+ S* m_data;
+ // The actual characteristics of the matrix as seen through this handle.
+ MatrixCharacter m_actual;
+ // Whether we have write access to the data through the pointer above.
+ // If not, the data is treated as though it were "const".
+ bool m_writable;
+
+ // Properties of the handle //
+
+ const int m_eltSize;
+ const int m_cppEltSize;
+
+ bool m_canBeOwner;
+ bool m_owner;
+ bool m_handleIsLocked; // temporarily prevent resize of owner
+
+ /// All commitments are by default "Uncommitted", meaning we're happy to
+ /// take on matrices in any format, provided that the element types match.
+ /// Otherwise the settings here limit what we'll find acceptable as actual data.
+ /// Note: don't look at these to find out anything about the current
+ /// matrix. These are used only when the matrix is being created or
+ /// changed structurally.
+ MatrixCommitment m_commitment;
+
+
+private:
+ // Point back to the owner handle of this rep.
+ MatrixHelper<S>* m_handle;
+
+ // suppress assignment
+ MatrixHelperRep& operator=(const MatrixHelperRep&);
+
+ // For use by our friend, MatrixHelper<S>.
+ void setMyHandle(MatrixHelper<S>& h) {m_handle = &h;}
+ const MatrixHelper<S>& getMyHandle() const {assert(m_handle); return *m_handle;}
+ void clearMyHandle() {m_handle=0;}
+
+
+friend class MatrixHelperRep<typename CNT<S>::TNeg>;
+friend class MatrixHelperRep<typename CNT<S>::THerm>;
+friend class MatrixHelper<S>;
+};
+
+
+} // namespace SimTK
+
+
+#endif // SimTK_SimTKCOMMON_MATRIX_HELPER_REP_H_
diff --git a/SimTKcommon/BigMatrix/src/MatrixHelperRep_Full.h b/SimTKcommon/BigMatrix/src/MatrixHelperRep_Full.h
new file mode 100644
index 0000000..79033c2
--- /dev/null
+++ b/SimTKcommon/BigMatrix/src/MatrixHelperRep_Full.h
@@ -0,0 +1,624 @@
+#ifndef SimTK_SimTKCOMMON_MATRIX_HELPER_REP_FULL_H_
+#define SimTK_SimTKCOMMON_MATRIX_HELPER_REP_FULL_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Scalar.h"
+#include "SimTKcommon/SmallMatrix.h"
+
+#include "MatrixHelperRep.h"
+
+#include <cstddef>
+#include <cstring>
+
+namespace SimTK {
+
+
+//------------------------------- FullHelper -----------------------------------
+//
+// This abstract class represents a matrix for which every one
+// of the mXn elements is stored explicitly in data.
+// Derived classes provide fast scalar elements, column- or row-order storage,
+// or fancier indexing schemes.
+//------------------------------------------------------------------------------
+template <class S>
+class FullHelper : public MatrixHelperRep<S> {
+ typedef FullHelper<S> This;
+ typedef MatrixHelperRep<S> Base;
+ typedef typename CNT<S>::TNeg SNeg;
+ typedef typename CNT<S>::THerm SHerm;
+ typedef FullHelper<SNeg> ThisNeg;
+ typedef FullHelper<SHerm> ThisHerm;
+public:
+ // Allocate new memory for a full, contiguous storage, owner matrix. The
+ // leading dimension will be either nr or nc depending on whether
+ // this is column- or row-order storage. Note that nr and nc are
+ // in elements while leadingDim is in scalars.
+ FullHelper(int esz, int cppesz, int nr, int nc, int ldim)
+ : Base(esz,cppesz), m_leadingDim(ldim)
+ {
+ assert( m_leadingDim==nr*this->m_eltSize
+ || m_leadingDim==nc*this->m_eltSize);
+ // The "this->" (or Base::) is required here (by gcc and the standard,
+ // though not VC++ 9) to delay lookup of these non-tempatized members
+ // until instantiation. (Google "two-stage name lookup".)
+ this->m_owner = true;
+ this->m_writable = true;
+ this->allocateData(nr,nc);
+ this->m_actual.setStructure(MatrixStructure::Full);
+ this->m_actual.setActualSize(nr,nc);
+ }
+
+ // Use someone else's memory, which we assume to be the right size.
+ FullHelper(int esz, int cppesz, int nr, int nc, int ldim,
+ const S* shared, bool canWrite)
+ : Base(esz,cppesz), m_leadingDim(ldim)
+ {
+ this->m_owner = false;
+ this->m_writable = canWrite;
+ this->setData(const_cast<S*>(shared));
+ this->m_actual.setStructure(MatrixStructure::Full);
+ this->m_actual.setActualSize(nr,nc);
+ }
+
+ int getLeadingDim() const {return m_leadingDim;}
+
+ void getAnyElt_(int i, int j, S* value) const
+ { this->copyElt(value, this->getElt_(i,j)); }
+
+ // The meaning of "Full" is that every element is stored in memory somewhere.
+ bool eltIsStored_(int, int) const {return true;}
+
+ // Just changing the return type here.
+ virtual FullHelper* cloneHelper_() const = 0;
+
+ // A deep copy of a full matrix always results in contiguous storage, in
+ // row or column order depending on what is cheaper.
+ virtual FullHelper* createDeepCopy_() const = 0;
+
+ // This serves for all block views of Full matrices because selecting
+ // a block doesn't change the type of handle we need.
+ FullHelper* createBlockView_(const EltBlock& block) {
+ FullHelper* p = cloneHelper_();
+ p->m_data = this->updElt_(block.row0(), block.col0());
+ return p;
+ }
+
+protected:
+ int m_leadingDim; // in scalars
+
+ // Note that these indexers can take signed indices and produce signed
+ // data offsets. Here we don't know whether the data is stored in row
+ // or column order, so the indices are in terms of "fast" and "slow"
+ // where "fast" is the one that moves consecutively through memory.
+
+ // First is for use with composite elements, second is for scalar elements.
+ ptrdiff_t eltIx (int fast, int slow) const
+ { return (ptrdiff_t)slow*m_leadingDim + fast*this->m_eltSize; }
+ ptrdiff_t scalarIx(int fast, int slow) const
+ { return (ptrdiff_t)slow*m_leadingDim + fast; }
+
+ bool isContiguousElt (int nFast) const
+ { return m_leadingDim == nFast*this->m_eltSize; }
+ bool isContiguousScalar(int nFast) const
+ { return m_leadingDim == nFast; }
+
+ S scalarColSum(int j) const {
+ S csum = S(0);
+ for (int i=0; i<this->nrow(); ++i) csum += *this->getElt_(i,j);
+ return csum;
+ }
+
+ S scalarRowSum(int i) const {
+ S rsum = S(0);
+ for (int j=0; j<this->ncol(); ++j) rsum += *this->getElt_(i,j);
+ return rsum;
+ }
+};
+
+//----------------------------- RegularFullHelper ------------------------------
+//
+// This is a Full matrix whose elements are regularly spaced in memory, meaning
+// that a change in row index i creates a predictable memory offset independent
+// of j, and similarly a change in column index j creates a predictable offset
+// independent of i.
+//------------------------------------------------------------------------------
+template <class S>
+class RegularFullHelper : public FullHelper<S> {
+ typedef RegularFullHelper<S> This;
+ typedef FullHelper<S> Base;
+public:
+ RegularFullHelper(int esz, int cppesz, int nr, int nc, int ldim)
+ : Base(esz,cppesz,nr,nc,ldim) {}
+ RegularFullHelper(int esz, int cppesz, int nr, int nc, int ldim,
+ const S* shared, bool canWrite)
+ : Base(esz,cppesz,nr,nc,ldim,shared,canWrite) {}
+
+ bool hasRegularData_() const {return true;}
+
+ // These implementations work for any RegularFullHelper.
+ This* createRegularView_(const EltBlock&, const EltIndexer&);
+ VectorHelper<S>* createDiagonalView_();
+ VectorHelper<S>* createColumnView_(int j, int i, int m);
+ VectorHelper<S>* createRowView_(int i, int j, int n);
+
+ // This is a new pure virtual that all RegularFullHelpers must provide.
+ // Some will return their actual indexers, others will calculate one,
+ // which is why we don't return a reference here.
+ virtual EltIndexer getEltIndexer() const = 0;
+
+protected:
+ typedef typename CNT<S>::StdNumber StdNumber;
+ // This is for use by scalar col- and row- ordered matrices only. The same
+ // code works because transpose(inv(m))==inv(transpose(m)).
+ void lapackInvertInPlace() {
+ // should have been checked already
+ assert(this->m_eltSize==1 && this->nrow()==this->ncol());
+ const int m = this->nrow();
+ StdNumber* rawMem = reinterpret_cast<StdNumber*>(this->m_data);
+ Array_<int> ipiv(m);
+ int info;
+ Lapack::getrf<StdNumber>(m,m,rawMem,this->m_leadingDim,&ipiv[0],info);
+ assert(info==0);
+
+ // Calculate optimal size for work
+ StdNumber workSz;
+ Lapack::getri<StdNumber>(m,rawMem,this->m_leadingDim,&ipiv[0],
+ &workSz,-1,info);
+ const int wsz = (int)CNT<StdNumber>::real(workSz);
+
+ Array_<StdNumber> work(wsz);
+ Lapack::getri<StdNumber>(m,rawMem,this->m_leadingDim,&ipiv[0],
+ &work[0],wsz,info);
+ assert(info==0);
+ }
+};
+
+// Full, column order, composite. Row i is the fast dimension here.
+template <class S>
+class FullColOrderEltHelper : public RegularFullHelper<S> {
+ typedef FullColOrderEltHelper<S> This;
+ typedef RegularFullHelper<S> Base;
+public:
+ // Leading dimension is # rows for contiguous column-ordered memory.
+ FullColOrderEltHelper(int esz, int cppesz, int nr, int nc)
+ : Base(esz, cppesz, nr, nc, nr*esz) {
+ this->m_actual.setStorage
+ (MatrixStorage(MatrixStorage::Full, MatrixStorage::NoPlacement,
+ MatrixStorage::ColumnOrder, MatrixStorage::NoDiag));
+ }
+
+ FullColOrderEltHelper(int esz, int cppesz, int nr, int nc, int ldim,
+ S* shared, bool canWrite)
+ : Base(esz, cppesz, nr, nc, ldim, shared, canWrite) {
+ assert(ldim>=nr*esz);
+ this->m_actual.setStorage
+ (MatrixStorage(MatrixStorage::Full, MatrixStorage::NoPlacement,
+ MatrixStorage::ColumnOrder, MatrixStorage::NoDiag));
+ }
+
+ // This should be processed a column at a time if possible.
+ bool preferRowOrder_() const {return false;}
+
+ // These virtual methods should be overridden in the derived scalar class.
+ virtual const S* getElt_(int i, int j) const
+ { return this->m_data + this->eltIx(i,j); }
+ virtual S* updElt_(int i, int j)
+ { return this->m_data + this->eltIx(i,j); }
+ virtual bool hasContiguousData_() const
+ { return this->isContiguousElt(this->nrow()); }
+ virtual This* cloneHelper_() const
+ { return new This(*this); }
+
+ // This implementation will return a FullRowOrderEltHelper. Override for
+ // scalars.
+ virtual RegularFullHelper<S>* createTransposeView_();
+
+ // These implementations are fine for both composite and scalar class.
+
+ // Regular spacing as (dfast/di, dfast/dj) and (dslow/di, dslow/dj),
+ // with i,j in elements.
+ EltIndexer getEltIndexer() const {return EltIndexer(1,0,0,1);}
+
+ This* createDeepCopy_() const {
+ This* p = cloneHelper_();
+ p->m_leadingDim = this->nrow()*this->m_eltSize; // packed now
+ p->m_writable = true;
+ p->m_owner = true;
+ p->allocateData(this->nelt());
+ if (hasContiguousData_())
+ std::memcpy(p->m_data, this->m_data,
+ Base::nelt()*this->m_eltSize*sizeof(S));
+ else for (int j=0; j < this->ncol(); ++j)
+ std::memcpy(p->updElt_(0,j), getElt_(0,j),
+ this->nrow()*this->m_eltSize*sizeof(S));
+ return p;
+ }
+
+
+ // OK for any size elements.
+ void resize_(int m, int n) {
+ this->clearData();
+ this->allocateData(m,n);
+ this->m_leadingDim = m * this->m_eltSize; // # scalars in a column
+ }
+
+ // OK for any size elements.
+ void resizeKeep_(int m, int n) {
+ const int newLeadingDim = m * this->m_eltSize; // # scalars in a column
+ S* const newData = this->allocateMemory(m,n);
+ const int colsToCopy = std::min(n, this->ncol());
+ const int rowsToCopy = std::min(m, this->nrow()); // in elements
+ for (int j=0; j < colsToCopy; ++j) {
+ S* const dest = newData + (ptrdiff_t)j*newLeadingDim;
+ const S* const src = this->m_data + (ptrdiff_t)j*this->m_leadingDim;
+ std::memcpy(dest, src, rowsToCopy*this->m_eltSize*sizeof(S));
+ }
+ this->clearData();
+ this->setData(newData);
+ this->m_leadingDim = newLeadingDim;
+ }
+};
+
+// Full, column order, scalar, final.
+template <class S>
+class FullColOrderScalarHelper : public FullColOrderEltHelper<S> {
+ typedef FullColOrderScalarHelper<S> This;
+ typedef FullColOrderEltHelper<S> Base;
+public:
+ // Leading dimension is # rows for contiguous column-ordered memory.
+ FullColOrderScalarHelper(int nr, int nc)
+ : Base(1, 1, nr, nc) {}
+ FullColOrderScalarHelper(int nr, int nc, int ldim, S* shared, bool canWrite)
+ : Base(1, 1, nr, nc, ldim, shared, canWrite) {}
+
+ // For speed, these override the Base implementations for composite elements.
+ const S* getElt_(int i, int j) const
+ { return this->m_data + this->scalarIx(i,j); }
+ S* updElt_(int i, int j)
+ { return this->m_data + this->scalarIx(i,j); }
+ bool hasContiguousData_() const
+ { return this->isContiguousScalar(this->nrow()); }
+ This* cloneHelper_() const
+ { return new This(*this); }
+
+ // This implementation will return a FullRowOrderScalarHelper.
+ RegularFullHelper<S>* createTransposeView_();
+
+ void colSum_(int j, S* csum) const {*csum = this->scalarColSum(j);}
+ void rowSum_(int i, S* rsum) const {*rsum = this->scalarRowSum(i);}
+ // Sum element column by column to avoid cache faults.
+ void sum_(S* esum) const {
+ *esum = S(0);
+ for (int j=0; j<this->ncol(); ++j) *esum += this->scalarColSum(j);
+ }
+
+ void invertInPlace_() {this->lapackInvertInPlace();}
+};
+
+// Full, row order, composite. Column j is the fast dimension here.
+template <class S>
+class FullRowOrderEltHelper : public RegularFullHelper<S> {
+ typedef FullRowOrderEltHelper<S> This;
+ typedef RegularFullHelper<S> Base;
+public:
+ // Leading dimension is # cols for contiguous row-ordered memory.
+ FullRowOrderEltHelper(int esz, int cppesz, int nr, int nc)
+ : Base(esz, cppesz, nr, nc, nc*esz) {
+ this->m_actual.setStorage
+ (MatrixStorage(MatrixStorage::Full, MatrixStorage::NoPlacement,
+ MatrixStorage::RowOrder, MatrixStorage::NoDiag));
+ }
+
+ FullRowOrderEltHelper(int esz, int cppesz, int nr, int nc, int ldim,
+ S* shared, bool canWrite)
+ : Base(esz, cppesz, nr, nc, ldim, shared, canWrite) {
+ assert(ldim>=nc*esz);
+ this->m_actual.setStorage
+ (MatrixStorage(MatrixStorage::Full, MatrixStorage::NoPlacement,
+ MatrixStorage::RowOrder, MatrixStorage::NoDiag));
+ }
+
+ // This should be processed a row at a time if possible.
+ bool preferRowOrder_() const {return true;}
+
+ // These virtual methods should be overridden in the derived scalar class.
+ virtual const S* getElt_(int i, int j) const
+ { return this->m_data + this->eltIx(j,i); }
+ virtual S* updElt_(int i, int j)
+ { return this->m_data + this->eltIx(j,i); }
+ virtual bool hasContiguousData_() const
+ { return this->isContiguousElt(this->ncol()); }
+ virtual This* cloneHelper_() const
+ { return new This(*this); }
+
+ // This implementation will return a FullColOrderEltHelper. Override for
+ // scalars.
+ virtual RegularFullHelper<S>* createTransposeView_();
+
+ // These implementations are fine for both composite and scalar class.
+ EltIndexer getEltIndexer() const {return EltIndexer(0,1,1,0);}
+
+ This* createDeepCopy_() const {
+ This* p = cloneHelper_();
+ p->m_leadingDim = this->ncol()*this->m_eltSize; // packed now
+ p->m_writable = true;
+ p->m_owner = true;
+ p->allocateData(this->nelt());
+ if (hasContiguousData_())
+ std::memcpy(p->m_data, this->m_data,
+ this->nelt()*this->m_eltSize*sizeof(S));
+ else for (int i=0; i < this->nrow(); ++i)
+ std::memcpy(p->updElt_(i,0), this->getElt_(i,0),
+ this->ncol()*this->m_eltSize*sizeof(S));
+ return p;
+ }
+
+ void resize_(int m, int n) {
+ this->clearData();
+ this->allocateData(m,n);
+ this->m_leadingDim = n * this->m_eltSize; // # scalars in a row
+ }
+
+ // OK for any size elements.
+ void resizeKeep_(int m, int n) {
+ const int newLeadingDim = n * this->m_eltSize; // # scalars in a row
+ S* const newData = this->allocateMemory(m,n);
+ const int colsToCopy = std::min(n, this->ncol()); // in elements
+ const int rowsToCopy = std::min(m, this->nrow());
+ for (int i=0; i < rowsToCopy; ++i) {
+ S* const dest = newData + (ptrdiff_t)i*newLeadingDim;
+ const S* const src = this->m_data + (ptrdiff_t)i*this->m_leadingDim;
+ std::memcpy(dest, src, colsToCopy*this->m_eltSize*sizeof(S));
+ }
+ this->clearData();
+ this->setData(newData);
+ this->m_leadingDim = newLeadingDim;
+ }
+};
+
+// Full, row order, scalar, final.
+template <class S>
+class FullRowOrderScalarHelper : public FullRowOrderEltHelper<S> {
+ typedef FullRowOrderScalarHelper<S> This;
+ typedef FullRowOrderEltHelper<S> Base;
+public:
+ // Leading dimension is # cols for contiguous row-ordered memory.
+ FullRowOrderScalarHelper(int nr, int nc)
+ : Base(1, 1, nr, nc) {}
+ FullRowOrderScalarHelper(int nr, int nc, int ldim, S* shared, bool canWrite)
+ : Base(1, 1, nr, nc, ldim, shared, canWrite) {}
+
+ // For speed, these override the Base implementations for composite elements.
+ const S* getElt_(int i, int j) const
+ { return this->m_data + this->scalarIx(j,i); }
+ S* updElt_(int i, int j)
+ { return this->m_data + this->scalarIx(j,i); }
+ bool hasContiguousData_() const
+ { return this->isContiguousScalar(this->ncol()); }
+ This* cloneHelper_() const
+ { return new This(*this); }
+
+ // This implementation will return a FullColOrderScalarHelper.
+ RegularFullHelper<S>* createTransposeView_();
+
+ void colSum_(int j, S* csum) const {*csum = this->scalarColSum(j);}
+ void rowSum_(int i, S* rsum) const {*rsum = this->scalarRowSum(i);}
+ // Sum element row by row to avoid cache faults.
+ void sum_(S* esum) const {
+ *esum = S(0);
+ for (int i=0; i<this->nrow(); ++i) *esum += this->scalarRowSum(i);
+ }
+
+ void invertInPlace_() {this->lapackInvertInPlace();}
+};
+
+
+// These definitions for inline methods had to wait until other classes
+// were defined.
+
+template <class S> inline RegularFullHelper<S>*
+FullColOrderEltHelper<S>::createTransposeView_() {
+ FullRowOrderEltHelper<S>* p =
+ new FullRowOrderEltHelper<S>(this->m_eltSize, this->m_cppEltSize,
+ this->ncol(), this->nrow(),
+ this->m_leadingDim, this->m_data, this->m_writable);
+ return p;
+}
+
+template <class S> inline RegularFullHelper<S>*
+FullColOrderScalarHelper<S>::createTransposeView_() {
+ FullRowOrderScalarHelper<S>* p =
+ new FullRowOrderScalarHelper<S>(this->ncol(), this->nrow(),
+ this->m_leadingDim, this->m_data, this->m_writable);
+ return p;
+}
+
+template <class S> inline RegularFullHelper<S>*
+FullRowOrderEltHelper<S>::createTransposeView_() {
+ FullColOrderEltHelper<S>* p =
+ new FullColOrderEltHelper<S>(this->m_eltSize, this->m_cppEltSize,
+ this->ncol(), this->nrow(),
+ this->m_leadingDim, this->m_data, this->m_writable);
+ return p;
+}
+
+template <class S> inline RegularFullHelper<S>*
+FullRowOrderScalarHelper<S>::createTransposeView_() {
+ FullColOrderScalarHelper<S>* p =
+ new FullColOrderScalarHelper<S>(this->ncol(), this->nrow(),
+ this->m_leadingDim, this->m_data, this->m_writable);
+ return p;
+}
+
+// Full, indexed, composite.
+template <class S>
+class FullIndexedEltHelper : public RegularFullHelper<S> {
+ typedef FullIndexedEltHelper<S> This;
+ typedef RegularFullHelper<S> Base;
+public:
+ // Construct a new view of any regularly-spaced full matrix, allowing
+ // composite elements. Source is taken non-const but writability is only
+ // granted if the source had it.
+ // Note that the (r0,c0) element may be one element off the end of the
+ // matrix as long as the corresponding dimension is 0.
+ FullIndexedEltHelper(RegularFullHelper<S>& src, const EltBlock& block, const EltIndexer& ix)
+ : Base(src.getEltSize(), src.getCppEltSize(),
+ block.nrow(), block.ncol(), src.getLeadingDim(),
+ src.getElt(block.row0(), block.col0()), src.isWritable()),
+ m_indexer(src.getEltIndexer().postIndexBy(ix))
+ {
+ SimTK_SIZECHECK(block.row0(), src.nrow()-block.nrow(), "FullIndexedEltHelper ctor");
+ SimTK_SIZECHECK(block.col0(), src.ncol()-block.ncol(), "FullIndexedEltHelper ctor");
+ }
+
+ // Implementations good for both composite and scalar elements. //
+
+ bool hasContiguousData_() const {return false;}
+ EltIndexer getEltIndexer() const {return m_indexer;}
+
+ // Prefer to go through the data a row at a time if adjacent column
+ // elements in the same row are closer together than adjacent row
+ // elements in the same column.
+ bool preferRowOrder_() const {return ixEltIx(0,1) < ixEltIx(1,0);}
+
+ // Implementations that should be overridden for scalar elements. //
+
+ virtual const S* getElt_(int i, int j) const {return this->m_data + ixEltIx(i,j);}
+ virtual S* updElt_(int i, int j) {return this->m_data + ixEltIx(i,j);}
+ virtual This* cloneHelper_() const {return new This(*this);}
+
+ // This implementation works for this class and the scalar derived class.
+ This* createTransposeView_() {
+ This* p = cloneHelper_();
+ p->m_indexer = m_indexer.transpose();
+ p->m_actual.setActualSize(this->ncol(), this->nrow());
+ return p;
+ }
+
+ // A deep copy of an indexed matrix eliminates the index, producing a
+ // full, column- or row-ordered matrix with contiguous storage.
+ // We choose the destination storage order to match whichever way is
+ // fastest to travel through the indexed matrix, since then we can
+ // traverse both matrices in their fastest order.
+ virtual RegularFullHelper<S>* createDeepCopy_() const {
+ if (preferRowOrder_()) {
+ FullRowOrderEltHelper<S>* p =
+ new FullRowOrderEltHelper<S>(this->m_eltSize,this->m_cppEltSize,
+ this->nrow(),this->ncol());
+ for (int i=0; i < this->nrow(); ++i) {
+ S* dest = p->updElt_(i,0); // start of a dense row
+ for (int j=0; j < this->ncol(); ++j, dest += this->m_eltSize)
+ this->copyElt(dest, this->getElt_(i,j));
+ }
+ return p;
+ } else {
+ FullColOrderEltHelper<S>* p =
+ new FullColOrderEltHelper<S>(this->m_eltSize,this->m_cppEltSize,
+ this->nrow(), this->ncol());
+ for (int j=0; j < this->ncol(); ++j) {
+ S* dest = p->updElt_(0,j); // start of a dense column
+ for (int i=0; i < this->nrow(); ++i, dest += this->m_eltSize)
+ this->copyElt(dest, this->getElt_(i,j));
+ }
+ return p;
+ }
+ }
+
+protected:
+ EltIndexer m_indexer;
+
+ int row(int i, int j) const {return m_indexer.row(i,j);}
+ int col(int i, int j) const {return m_indexer.col(i,j);}
+
+ ptrdiff_t ixEltIx(int i, int j) const {return this->eltIx(row(i,j),col(i,j));}
+};
+
+// Full, indexed, scalar, final.
+template <class S>
+class FullIndexedScalarHelper : public FullIndexedEltHelper<S> {
+ typedef FullIndexedScalarHelper<S> This;
+ typedef FullIndexedEltHelper<S> Base;
+public:
+ // Construct a new view of any regularly-spaced full matrix, as long as that
+ // matrix has scalar elements.
+ FullIndexedScalarHelper(RegularFullHelper<S>& src, const EltBlock& block,
+ const EltIndexer& ix)
+ : Base(src, block, ix)
+ {
+ SimTK_ASSERT_ALWAYS(src.getEltSize()==1,
+ "FullIndexedScalarHelper ctor: source must have scalar elements");
+ }
+
+ // For speed, these override the composite-element implementatiosn.
+ const S* getElt_(int i, int j) const
+ { return this->m_data + ixScalarIx(i,j); }
+ S* updElt_(int i, int j)
+ { return this->m_data + ixScalarIx(i,j); }
+
+ This* cloneHelper_() const {return new This(*this);}
+
+ RegularFullHelper<S>* createDeepCopy_() const {
+ if (this->preferRowOrder_()) {
+ FullRowOrderScalarHelper<S>* p =
+ new FullRowOrderScalarHelper<S>(this->nrow(),this->ncol());
+ for (int i=0; i < this->nrow(); ++i) {
+ S* dest = p->updElt_(i,0); // start of a dense row
+ for (int j=0; j < this->ncol(); ++j, ++dest)
+ *dest = *this->getElt_(i,j);
+ }
+ return p;
+ } else {
+ FullColOrderScalarHelper<S>* p =
+ new FullColOrderScalarHelper<S>(this->nrow(),this->ncol());
+ for (int j=0; j < this->ncol(); ++j) {
+ S* dest = p->updElt_(0,j); // start of a dense column
+ for (int i=0; i < this->nrow(); ++i, ++dest)
+ *dest = *this->getElt_(i,j);
+ }
+ return p;
+ }
+ }
+
+private:
+ ptrdiff_t ixScalarIx(int i, int j) const
+ { return this->scalarIx(this->row(i,j),this->col(i,j)); }
+};
+
+template <class S> inline RegularFullHelper<S>*
+RegularFullHelper<S>::createRegularView_(const EltBlock& block,
+ const EltIndexer& ix) {
+ RegularFullHelper<S>* p;
+ if (this->m_eltSize==1) p = new FullIndexedScalarHelper<S>(*this, block, ix);
+ else p = new FullIndexedEltHelper<S>(*this, block, ix);
+ return p;
+}
+
+
+
+
+} // namespace SimTK
+
+
+#endif // SimTK_SimTKCOMMON_MATRIX_HELPER_REP_FULL_H_
diff --git a/SimTKcommon/BigMatrix/src/MatrixHelperRep_Tri.h b/SimTKcommon/BigMatrix/src/MatrixHelperRep_Tri.h
new file mode 100644
index 0000000..d1cf068
--- /dev/null
+++ b/SimTKcommon/BigMatrix/src/MatrixHelperRep_Tri.h
@@ -0,0 +1,467 @@
+#ifndef SimTK_SimTKCOMMON_MATRIX_HELPER_REP_TRI_H_
+#define SimTK_SimTKCOMMON_MATRIX_HELPER_REP_TRI_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Scalar.h"
+#include "SimTKcommon/SmallMatrix.h"
+
+#include "MatrixHelperRep.h"
+
+#include <cstddef>
+
+namespace SimTK {
+
+
+//--------------------------------- TriHelper ----------------------------------
+
+/// This abstract class represents a square matrix for which only half the elements
+/// need to be stored in memory, and that half is either the lower or upper triangle.
+/// The remaining half must have elements whose values can be determined from
+/// the stored ones. There are several cases:
+///
+/// - the missing elements are all zero (a triangular matrix)
+/// - the missing (i,j)th element is the same as the stored (j,i)th element
+/// (a positionally symmetric matrix)
+/// - the missing (i,j)th element is the generalized hermitian transpose
+/// of the stored (j,i)th element (a generalized hermitian matrix)
+/// - the missing elements are the negatives of the above (skew-symmetric
+/// or skew-hermitian matrix)
+///
+/// In addition, we allow for the possibility that the diagonal
+/// elements are all the same, known value and don't need to be stored; we'll
+/// call that a "known diagonal" matrix. The known diagonal capability permits
+/// two full triangle-shaped matrices to be packed in the space of a single
+/// full matrix provided that at least one of them is a known diagonal matrix.
+/// Typically the known diagonal value is one (unit diagonal), but it can have
+/// other values (e.g. a skew symmetric matrix has a zero diagonal).
+///
+/// Read-only references to any of the stored elements, and to the known diagonal
+/// elements if any, can be obtained through the usual getElt_(i,j) virtual.
+/// Writable references to the stored elements (but not a diagonal element of
+/// a known diagonal matrix) can be obtained with the updElt_(i,j) virtual.
+///
+/// In any matrix where the (i,j)th element must be derived
+/// from the (j,i)th via some non-identity transformation, the diagonal
+/// elements are required to be invariant under that transformation. For a
+/// skew-symmetric matrix the diagonals must be zero to make m(i,j)==-m(i,j).
+/// For a hermitian matrix, the diagonals must be real so that m(i,j)==conj(m(i,j)).
+/// For a skew-hermitian matrix the diagonals must be imaginary so that
+/// m(i,j)==-conj(m(i,j)). In general the diagonals must be recursively
+/// self-hermitian-transposes for composite elements in hermitian matrices.
+///
+/// Although the interesting part of the matrix is square, we allow that to be
+/// the upper left square of a rectangular matrix, where all the additional
+/// elements are zero (that's called "trapezoidal" although it might really
+/// be a trapezoid because the triangle part may be flipped. In any case if
+/// the matrix has dimension m X n, the leading square has dimension min(m,n).
+
+//------------------------------------------------------------------------------
+template <class S>
+class TriHelper : public MatrixHelperRep<S> {
+ typedef TriHelper<S> This;
+ typedef MatrixHelperRep<S> Base;
+ typedef typename CNT<S>::TNeg SNeg;
+ typedef typename CNT<S>::THerm SHerm;
+ typedef TriHelper<SNeg> ThisNeg;
+ typedef TriHelper<SHerm> ThisHerm;
+public:
+ TriHelper(int esz, int cppesz) : Base(esz,cppesz) {}
+
+ // Override in derived classes with assumed-unit diagonals.
+ virtual bool hasUnitDiagonal() const {return false;}
+
+ // Just changing the return type here.
+ virtual This* cloneHelper_() const = 0;
+
+ // A deep copy of a Tri matrix will always return another
+ // Tri matrix.
+ virtual This* createDeepCopy_() const = 0;
+
+
+ // TODO: this should allow at least views which are Triangular, meaning that
+ // the upper left corner should be on the diagonal.
+ MatrixHelperRep<S>* createBlockView_(const EltBlock& block) {
+ SimTK_ERRCHK_ALWAYS(!"implemented", "TriHelper::createBlockView_()", "not implemented");
+ return 0;
+ }
+ // TODO: the transpose has conjugate elements, which we would normally handle
+ // by typecasting to CNT<S>::THerm. However, here we're being asked to make a
+ // view without typecasting, so we have to do it with the elements we've got.
+ // No problem: turn a "lower" into an "upper"; the missing elements (the
+ // conjugated ones) will have moved to their proper locations.
+ MatrixHelperRep<S>* createTransposeView_() {
+ SimTK_ERRCHK_ALWAYS(!"not implemented", "TriHelper::createTransposeView_()", "not implemented");
+ return 0;
+ }
+
+ // Not sure if this should ever be supported.
+ MatrixHelperRep<S>* createRegularView_(const EltBlock&, const EltIndexer&) {
+ SimTK_ERRCHK_ALWAYS(!"not implemented", "TriHelper::createRegularView_()", "not implemented");
+ return 0;
+ }
+
+ VectorHelper<S>* createColumnView_(int,int,int){
+ SimTK_ERRCHK_ALWAYS(!"not implemented", "TriHelper::createColumnView_()", "not implemented");
+ return 0;
+ }
+ VectorHelper<S>* createRowView_(int,int,int){
+ SimTK_ERRCHK_ALWAYS(!"not implemented", "TriHelper::createRowView_()", "not implemented");
+ return 0;
+ }
+protected:
+};
+
+
+//----------------------------- TriInFullHelper --------------------------------
+/// We are assuming non-packed storage here: the triangle matrix is stored
+/// in a full matrix so the stored elements are indexed exactly as they would
+/// be if the matrix were full. Two triangle matrices can be stored
+/// in the available memory, one in the lower triangle and one in the upper,
+/// provided that at least one has known diagonal elements that don't need to be
+/// stored (that's still not considered "packed").
+//------------------------------------------------------------------------------
+template <class S>
+class TriInFullHelper : public TriHelper<S> {
+ typedef typename CNT<S>::StdNumber StdNumber;
+ typedef TriInFullHelper<S> This;
+ typedef TriHelper<S> Base;
+public:
+ // If we're allocating the space, we'll just allocate a square even if the
+ // matrix is trapezoidal.
+ TriInFullHelper(int esz, int cppesz, int m, int n,
+ bool triangular, bool hermitian, bool skew, bool rowOrder)
+ : Base(esz,cppesz), m_minmn(std::min(m,n)), m_leadingDim(m_minmn*esz),
+ m_triangular(triangular), m_hermitian(hermitian),
+ m_skew(skew), m_rowOrder(rowOrder), m_unstored(new S[NumUnstored*esz])
+ {
+ this->m_owner = true;
+ this->m_writable = true;
+ this->allocateData(m_minmn, m_minmn);
+ this->m_actual.setActualSize(m, n); // the apparent size
+ }
+
+ // Use someone else's memory, which we assume to be the right size.
+ TriInFullHelper(int esz, int cppesz, int m, int n,
+ bool triangular, bool hermitian, bool skew, bool rowOrder,
+ int ldim, const S* shared, bool canWrite)
+ : Base(esz,cppesz), m_minmn(std::min(m,n)), m_leadingDim(ldim),
+ m_triangular(triangular), m_hermitian(hermitian),
+ m_skew(skew), m_rowOrder(rowOrder), m_unstored(new S[NumUnstored*esz])
+ {
+ assert(m_leadingDim >= m_minmn*this->m_eltSize);
+ this->m_owner = false;
+ this->m_writable = canWrite;
+ this->setData(const_cast<S*>(shared));
+ this->m_actual.setActualSize(m, n);
+ }
+
+ ~TriInFullHelper() {delete[] m_unstored;}
+
+ bool preferRowOrder_() const {return m_rowOrder;}
+
+ // In full storage, Tri matrix elements are regularly spaced.
+ bool hasRegularData_() const {return true;}
+
+ // Just changing the return type here: a cloned Tri-in-full-storage
+ // handle will always be another Tri-in-full-storage handle.
+ virtual This* cloneHelper_() const = 0;
+
+ // In full storage, a Tri matrix is not stored contiguously.
+ bool hasContiguousData_() const {return false;}
+
+
+ // Compute the missing elements.
+ void getAnyElt_(int i, int j, S* value) const {
+ if (i==j || this->eltIsStored_(i,j)) {
+ this->copyElt(value, this->getElt_(i,j));
+ return;
+ }
+ // Missing elements are all zero for triangular matrices, or if
+ // either dimension is outside the square part of any triangular-storage
+ // matrix (i.e., the matrix is trapezoidal).
+ if (m_triangular || i >= m_minmn || j >= m_minmn) {
+ this->fillElt(value, StdNumber(0));
+ return;
+ }
+
+ // Symmetric or hermitian. The returned (i,j)th element is some
+ // function of the stored (j,i)th element.
+ const S* e = this->getElt_(j,i);
+
+ if (m_hermitian) {
+ if (m_skew) this->copyAndNegConjugateElt(value, e);
+ else this->copyAndConjugateElt(value, e);
+ } else {
+ // symmetric but not hermitian
+ if (m_skew) this->copyAndNegateElt(value, e);
+ else this->copyElt(value, e); // no change to stored element
+ }
+ }
+
+ VectorHelper<S>* createDiagonalView_();
+
+ virtual bool isUpper() const {return false;}
+ virtual bool hasKnownDiagonal() const {return false;}
+
+ This* createDeepCopy_() const {
+ This* p = cloneHelper_();
+ p->m_writable = true;
+ p->m_owner = true;
+ p->allocateData(m_minmn, m_minmn);
+ p->m_minmn = m_minmn;
+ p->m_leadingDim = m_minmn; // might be different than source
+
+ const bool shortFirst = (m_rowOrder&&!isUpper()) || (!m_rowOrder&&isUpper());
+ const int known = hasKnownDiagonal() ? 1 : 0;
+ const int eltBytes = this->m_eltSize*sizeof(S);
+
+ // Copy 1/2 of a square of this size, possibly excluding diagonals.
+ const int nToCopy = m_minmn;
+ S* const dest = p->m_data + known*this->m_eltSize; // skip diag if known
+ const S* const src = this->m_data + known*this->m_eltSize;
+ if (shortFirst) {
+ // column or row begins at (k,j) and has length (j+1)-k
+ int lengthInBytes = eltBytes; // 1st row/col has 1 element to copy
+ // skip 0-length row or col if diag is known
+ for (ptrdiff_t j=known; j < nToCopy; ++j, lengthInBytes += eltBytes) {
+ std::memcpy(dest+j*p->m_leadingDim, src+j*this->m_leadingDim, lengthInBytes);
+ }
+ } else { // longFirst
+ // column or row begins at (j+k,j), length m-j-k
+ int startInScalars = 0; // data was already shifted by 1 if needed
+ int lengthInBytes = (nToCopy-known)*eltBytes;
+ for (ptrdiff_t j=0; j < nToCopy-known; ++j) {
+ std::memcpy(dest + j*p->m_leadingDim + startInScalars,
+ src + j*m_leadingDim + startInScalars, lengthInBytes);
+ startInScalars += this->m_eltSize;
+ lengthInBytes -= eltBytes;
+ }
+ }
+ return p;
+ }
+
+ // OK for any size elements. Note that we only allocate a square to hold
+ // the data even if the matrix is trapezoidal. The rest is known to be zero.
+ // There is no need to reallocate if min(m,n) doesn't change -- that just
+ // means more zeroes so the size will be changed.
+ void resize_(int m, int n) {
+ const int newMinmn = std::min(m,n);
+ if (newMinmn == m_minmn) return;
+ this->clearData();
+ m_minmn = newMinmn;
+ this->allocateData(m_minmn, m_minmn);
+ m_leadingDim = m_minmn*this->m_eltSize; // number of scalars in a column
+ }
+
+ // OK for any size elements. We'll move along the fast direction; columns
+ // for column-ordered and rows for row-ordered storage. There are two cases:
+ // (upper, col order) and (lower, row order)
+ // -- data starts at 0 for each column [row] and get longer (shortFirst)
+ // (upper, row order) and (lower, col order)
+ // -- data starts at the diagonal and gets shorter (longFirst)
+ void resizeKeep_(int m, int n) {
+ const int newMinmn = std::min(m,n);
+ if (newMinmn == m_minmn) return;
+
+ const bool shortFirst = (m_rowOrder&&!isUpper()) || (!m_rowOrder&&isUpper());
+ const int newLeadingDim = newMinmn;
+ const int known = hasKnownDiagonal() ? 1 : 0;
+ const int eltBytes = this->m_eltSize*sizeof(S);
+
+ // Copy 1/2 of a square of this size, possibly excluding diagonals.
+ S* const newData = this->allocateMemory(newMinmn, newMinmn);
+ const int nToCopy = std::min(m_minmn, newMinmn);
+ S* const dest = newData + known*this->m_eltSize; // skip diag if known
+ const S* const src = this->m_data + known*this->m_eltSize;
+ if (shortFirst) {
+ // column or row begins at (k,j) and has length (j+1)-k
+ int lengthInBytes = eltBytes; // 1st row/col has 1 element to copy
+ // skip 0-length row or col if diag is known
+ for (ptrdiff_t j=known; j < nToCopy; ++j, lengthInBytes += eltBytes) {
+ std::memcpy(dest+j*newLeadingDim, src+j*this->m_leadingDim, lengthInBytes);
+ }
+ } else { // longFirst
+ // column or row begins at (j+k,j), length m-j-k
+ int startInScalars = 0; // data was already shifted by 1 if needed
+ int lengthInBytes = (nToCopy-known)*eltBytes;
+ for (ptrdiff_t j=0; j < nToCopy-known; ++j) {
+ std::memcpy(dest + j*newLeadingDim + startInScalars,
+ src + j*this->m_leadingDim + startInScalars, lengthInBytes);
+ startInScalars += this->m_eltSize;
+ lengthInBytes -= eltBytes;
+ }
+ }
+ this->clearData();
+ this->setData(newData);
+ m_minmn = newMinmn;
+ m_leadingDim = newLeadingDim;
+ }
+
+protected:
+ int m_minmn; // dimension of the square part (min(m,n))
+ int m_leadingDim; // in scalars; can be row- or column-ordered.
+
+ // These should be implicit in the subclasses but for now they're here
+ bool m_triangular; // true if triangular, false if symmetric or hermitian
+ bool m_hermitian; // m(i,j) = hermitianTranspose(m(j,i))
+ bool m_skew; // m(i,j) = -op(m(j,i))
+ bool m_rowOrder;
+
+ // Allocate heap space here for unstored elements. Don't forget to multiply
+ // by element size when accessing.
+ static const int NumUnstored = 3; // length is 3*element size in scalars
+ static const int UnstoredZero = 0; // first element holds a zero
+ static const int UnstoredDummy = 1; // second element is "write only" garbage
+ static const int UnstoredDiag = 2; // this is the known diagonal if any
+ S* m_unstored;
+
+private:
+
+};
+
+// Concrete class: Tri matrix in the upper triangle of full storage,
+// diagonals are stored, composite elements.
+// An upper triangle has i <= j.
+template <class S>
+class TriInFullUpperHelper : public TriInFullHelper<S> {
+ typedef TriInFullUpperHelper<S> This;
+ typedef TriInFullHelper<S> Base;
+public:
+ TriInFullUpperHelper(int esz, int cppesz, int m, int n,
+ bool triangular, bool hermitian, bool skew, bool rowOrder)
+ : Base(esz,cppesz,m,n,triangular,hermitian,skew,rowOrder)
+ { this->m_actual.setStructure(calcUpperTriStructure()); }
+
+ // Use someone else's memory, which we assume to be the right size.
+ TriInFullUpperHelper(int esz, int cppesz, int m, int n,
+ bool triangular, bool hermitian, bool skew, bool rowOrder,
+ int ldim, const S* shared, bool canWrite)
+ : Base(esz,cppesz,m,n,triangular,hermitian,skew,rowOrder,ldim,shared,canWrite)
+ { this->m_actual.setStructure(calcUpperTriStructure()); }
+
+ bool isUpper() const {return true;}
+
+ virtual This* cloneHelper_() const {return new This(*this);}
+
+ // override for unit diagonal
+ virtual bool eltIsStored_(int i, int j) const {return i <= j && j < this->m_minmn;}
+
+ // override for unit diagonals and scalar elements
+ virtual const S* getElt_(int i, int j) const {
+ SimTK_ERRCHK2(i <= j && j < this->m_minmn, "TriInFullUpperHelper::getElt_()",
+ "Element index was (i,j)=(%d,%d) which refers to an element which is\n"
+ " not available. Only the upper triangle and diagonal of this matrix are stored.",
+ i, j);
+ if (this->m_rowOrder) std::swap(i,j);
+ return this->m_data + j*this->m_leadingDim + i*this->m_eltSize;
+ }
+
+ // override for unit diagonals and scalar elements
+ virtual S* updElt_(int i, int j) {
+ SimTK_ERRCHK2(i <= j && j < this->m_minmn, "TriInFullUpperHelper::updElt_()",
+ "Element index was (i,j)=(%d,%d) which refers to an element which is\n"
+ " not stored. Only the upper triangle and diagonal of this matrix are stored.",
+ i, j);
+ if (this->m_rowOrder) std::swap(i,j);
+ return this->m_data + j*this->m_leadingDim + i*this->m_eltSize;
+ }
+
+private:
+ MatrixStructure calcUpperTriStructure() const {
+ MatrixStructure ms;
+ ms.setPosition(MatrixStructure::Upper);
+ ms.setDiagValue(this->hasKnownDiagonal() ? MatrixStructure::UnitDiag : MatrixStructure::StoredDiag);
+ if (this->m_triangular) ms.setStructure(MatrixStructure::Triangular);
+ else {
+ if (this->m_hermitian)
+ ms.setStructure(this->m_skew ? MatrixStructure::SkewHermitian
+ : MatrixStructure::Hermitian);
+ else // symmetric
+ ms.setStructure(this->m_skew ? MatrixStructure::SkewSymmetric
+ : MatrixStructure::Symmetric);
+ }
+ return ms;
+ }
+};
+
+
+// Concrete class: Tri matrix in the upper triangle of full storage,
+// diagonals are known, composite elements.
+template <class S>
+class TriInFullUpperKnownDiagHelper : public TriInFullUpperHelper<S> {
+ typedef TriInFullUpperKnownDiagHelper<S> This;
+ typedef TriInFullUpperHelper<S> Base;
+public:
+ TriInFullUpperKnownDiagHelper(int esz, int cppesz, int m, int n,
+ bool triangular, bool hermitian, bool skew, bool rowOrder, const S* knownDiag)
+ : Base(esz,cppesz,m,n,triangular,hermitian,skew,rowOrder)
+ {
+ copyElt(&this->m_unknown[this->UnstoredDiag*this->m_eltSize], knownDiag);
+ }
+
+ TriInFullUpperKnownDiagHelper(int esz, int cppesz, int m, int n,
+ bool triangular, bool hermitian, bool skew, bool rowOrder, const S* knownDiag,
+ int ldim, const S* shared, bool canWrite)
+ : Base(esz,cppesz,m,n,triangular,hermitian,skew,rowOrder,ldim,shared,canWrite)
+ {
+ copyElt(&this->m_unknown[this->UnstoredDiag*this->m_eltSize], knownDiag);
+ }
+
+ bool hasKnownDiagonal() const {return true;}
+
+ This* cloneHelper_() const {return new This(*this);}
+
+ // We're overriding since only i<j is stored.
+ bool eltIsStored_(int i, int j) const {return i<j && j < this->m_minmn;}
+
+ // These should be overwritten for scalars, although they will work as is.
+ virtual const S* getElt_(int i, int j) const {
+ SimTK_ERRCHK2(i<=j && j < this->m_minmn, "TriInFullUpperKnownDiagHelper::getElt_()",
+ "Element index (i,j)=(%d,%d) which refers to an element which is\n"
+ " not available. Only the upper triangle of this matrix is stored.",
+ i, j);
+ if (i==j) return &this->m_unknown[this->UnstoredDiag*this->m_eltSize];
+ if (this->m_rowOrder) std::swap(i,j);
+ return this->m_data + j*this->m_leadingDim + i*this->m_eltSize;
+ }
+
+ // override for unit diagonals and scalar elements
+ virtual S* updElt_(int i, int j) {
+ SimTK_ERRCHK2(i<j && j < this->m_minmn, "TriInFullUpperKnownDiagHelper::updElt_()",
+ "Element index (i,j)=(%d,%d) which refers to the lower triangle, but\n"
+ " only the upper triangle (i<j) of this matrix are stored.", i, j);
+ if (this->m_rowOrder) std::swap(i,j);
+ return this->m_data + j*this->m_leadingDim + i*this->m_eltSize;
+ }
+};
+
+
+
+
+
+} // namespace SimTK
+
+
+#endif // SimTK_SimTKCOMMON_MATRIX_HELPER_REP_TRI_H_
diff --git a/SimTKcommon/BigMatrix/src/MatrixHelperRep_Vector.h b/SimTKcommon/BigMatrix/src/MatrixHelperRep_Vector.h
new file mode 100644
index 0000000..07bfd5a
--- /dev/null
+++ b/SimTKcommon/BigMatrix/src/MatrixHelperRep_Vector.h
@@ -0,0 +1,708 @@
+#ifndef SimTK_SimTKCOMMON_MATRIX_HELPER_REP_VECTOR_H_
+#define SimTK_SimTKCOMMON_MATRIX_HELPER_REP_VECTOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Scalar.h"
+#include "SimTKcommon/SmallMatrix.h"
+
+#include "MatrixHelperRep.h"
+
+#include <cstddef>
+
+namespace SimTK {
+
+
+/*-------------------------------- VectorHelper --------------------------------
+
+This abstract class represents a 1d matrix, a.k.a. a Vector or a RowVector
+(covector). However, its use is not limited to the SimTK::Vector and and
+SimTK::RowVector classes; any skinny Matrix or skinny slice of a fatter
+Matrix might use a VectorHelper since it provides faster access to memory
+than a generic 2d helper would.
+
+Most operations don't care whether this is a column or a row,
+however we have to know so that we can support matrix operations on the
+vector when necessary. For example, getElt(i,j) still has to work (as long
+as the appropriate one of i or j is 0), even though getElt(i) is more
+efficient and direction-agnostic.
+
+The most common layout is that all elements are stored, either consecutively
+in memory or with a regular stride. Vectors constructed from a larger pool
+of stored data via indexing are also important and need to be implemented
+efficiently.
+
+TODO: However, there are several other important
+layouts that arise most commonly from row and column selections performed
+on non-full matrices, like triangular or symmetric matrices. Supporting such
+selections allows simple (if inefficient) implementations of operations on
+mixed types of matrices by breaking them into row and column operations.
+Vectors selected in this way can be repetitions of the same element (often
+zero), negations or conjugations of stored elements, and may sometimes have
+a single "distinguished" element whose value is known (this occurs for example
+when crossing a non-stored unit diagonal).
+
+TODO: Finally, we allow a Vector to be formed of a composition of smaller
+Vectors. Then the whole vector can be accessed by element or more efficiently
+by segments.
+------------------------------------------------------------------------------*/
+template <class S>
+class VectorHelper : public MatrixHelperRep<S> {
+ typedef VectorHelper<S> This;
+ typedef MatrixHelperRep<S> Base;
+ typedef typename CNT<S>::TNeg SNeg;
+ typedef typename CNT<S>::THerm SHerm;
+ typedef VectorHelper<SNeg> ThisNeg;
+ typedef VectorHelper<SHerm> ThisHerm;
+public:
+ VectorHelper(int esz, int cppesz, bool isRow)
+ : Base(esz,cppesz),m_row(isRow)
+ {
+ }
+
+
+ // A deep copy of a Vector will always return another Vector, so we'll
+ // change the return type here.
+ virtual This* createDeepCopy_() const = 0;
+
+ // Just changing the return type here.
+ virtual This* cloneHelper_() const = 0;
+
+ bool preferRowOrder_() const {return m_row;}
+
+
+ // Forward the two-index operations to the appropriate one-index operation.
+
+ // One of the indices must be zero.
+ bool eltIsStored_(int i, int j) const {return eltIsStored_(i+j);}
+ const S* getElt_ (int i, int j) const {return getElt_(i+j);}
+ S* updElt_ (int i, int j) {return updElt_(i+j);}
+ void getAnyElt_ (int i, int j, S* value) const {getAnyElt_(i+j, value);}
+
+
+ // These are mandatory for vectors.
+ virtual bool eltIsStored_(int i) const = 0;
+ virtual const S* getElt_ (int i) const = 0;
+ virtual S* updElt_ (int i) = 0;
+ virtual void getAnyElt_ (int i, S* value) const = 0;
+
+ // (Positional) transpose view is identical to this one except that we'll
+ // call it a row rather than a column or vice versa.
+ This* createTransposeView_() {
+ This* p = cloneHelper_();
+ p->m_data = this->m_data;
+ p->m_row = !m_row;
+ p->m_actual.updStorage().setOrder
+ (p->m_row ? MatrixStorage::RowOrder : MatrixStorage::ColumnOrder);
+ return p;
+ }
+
+ // Not sure if this should ever be supported.
+ MatrixHelperRep<S>* createRegularView_(const EltBlock&, const EltIndexer&) {
+ SimTK_ERRCHK_ALWAYS(!"not implemented", "VectorHelper::createRegularView_()", "not implemented");
+ return 0;
+ }
+
+
+protected:
+ bool m_row; // true if this should be a row when treated as a matrix
+};
+
+//----------------------------- FullVectorHelper -------------------------------
+/// All elements of the Vector are stored. The simplest form of this has the
+/// data pointing to the Vector's 0th element with all the rest following
+/// consecutively in memory. Derived classes add stride or indexing and
+/// optimize for scalar elements.
+//------------------------------------------------------------------------------
+template <class S>
+class FullVectorHelper : public VectorHelper<S> {
+ typedef FullVectorHelper<S> This;
+ typedef VectorHelper<S> Base;
+public:
+ FullVectorHelper(int esz, int cppesz, bool isRow)
+ : Base(esz,cppesz,isRow) {}
+
+ // This will always produce a 1-element "contiguous" column vector.
+ VectorHelper<S>* createDiagonalView_();
+
+ // Source matches size and shape of this row or column.
+ void copyInFromCompatibleSource_(const MatrixHelperRep<S>& source) {
+ if (this->getEltSize() == 1) {
+ // The elements are scalars, so we can copy them directly.
+ if (this->nrow() == 1) // a row vector
+ for (int j=0; j<this->ncol(); ++j)
+ *this->updElt_(j) = *source.getElt(0,j);
+ else // a column vector
+ for (int i=0; i<this->nrow(); ++i)
+ *this->updElt_(i) = *source.getElt(i,0);
+ }
+ else { // Here the elements are not scalars.
+ if (this->nrow() == 1) // a row vector
+ for (int j=0; j<this->ncol(); ++j)
+ this->copyElt(this->updElt_(j), source.getElt(0,j));
+ else // a column vector
+ for (int i=0; i<this->nrow(); ++i)
+ this->copyElt(this->updElt_(i), source.getElt(i,0));
+ }
+ }
+};
+
+//-------------------------- ContiguousVectorHelper ----------------------------
+/// All elements of the Vector are stored, and they are contiguous in memory.
+/// This class handles general elements; a derived class handles the special
+/// case of scalar elements by overriding some of the methods for speed.
+//------------------------------------------------------------------------------
+template <class S>
+class ContiguousVectorHelper : public FullVectorHelper<S> {
+ typedef ContiguousVectorHelper<S> This;
+ typedef FullVectorHelper<S> Base;
+public:
+ // Allocate our own space.
+ ContiguousVectorHelper(int esz, int cppesz, int n, bool isRow)
+ : Base(esz,cppesz,isRow)
+ {
+ this->m_owner = true;
+ this->m_writable = true;
+ this->allocateData(n);
+ this->m_actual.setStructure(MatrixStructure::Matrix1d);
+ this->m_actual.setStorage(
+ MatrixStorage(MatrixStorage::Vector, MatrixStorage::NoPlacement,
+ isRow ? MatrixStorage::RowOrder : MatrixStorage::ColumnOrder,
+ MatrixStorage::NoDiag));
+ this->m_actual.setActualSize(isRow?1:n, isRow?n:1); // apparent size; sets Outline
+ }
+
+ // Use someone else's memory, which we assume to be the right size.
+ // We take care of stride elsewhere.
+ ContiguousVectorHelper(int esz, int cppesz, int n, bool isRow,
+ const S* shared, bool canWrite)
+ : Base(esz,cppesz,isRow)
+ {
+ this->m_owner = false;
+ this->m_writable = canWrite;
+ this->setData(const_cast<S*>(shared));
+ this->m_actual.setStructure(MatrixStructure::Matrix1d);
+ this->m_actual.setStorage(
+ MatrixStorage(MatrixStorage::Vector, MatrixStorage::NoPlacement,
+ isRow ? MatrixStorage::RowOrder
+ : MatrixStorage::ColumnOrder,
+ MatrixStorage::NoDiag));
+ // apparent size; sets Outline
+ this->m_actual.setActualSize(isRow?1:n, isRow?n:1);
+ }
+
+ virtual This* cloneHelper_() const {return new This(*this);}
+
+ // A block view of a full, contiguous row/column is either
+ // (1) a smaller full, contiguous row/column, or (2) a zero-width
+ // slice. In the latter case it may no longer be a row or column so we
+ // have to switch helper types to a Full mX0 or 0Xn matrix.
+ MatrixHelperRep<S>* createBlockView_(const EltBlock& block) {
+ const int m=block.nrow(), n=block.ncol();
+ if (m && n) { // normal case; same orientation but smaller
+ This* p = cloneHelper_();
+ p->m_data = updElt_(block.row0() + block.col0());
+ return p;
+ }
+ // Here we know at least one of m or n is zero.
+ if (m==1 || n==1) { // i.e., (1,0) or (0,1)
+ This* p = cloneHelper_(); // still 1d "contiguous"
+ p->m_data = 0;
+ p->m_row = (m==1); // regardless of what it was
+ return p;
+ }
+
+ // Here one of m,n is zero and the other is > 1; not 1d any more.
+ RegularFullHelper<S>* p = 0;
+ if (this->getEltSize()==1)
+ p = new FullColOrderScalarHelper<S>(m,n,m,(S*)0,this->m_writable);
+ else p = new FullColOrderEltHelper<S>(this->getEltSize(),
+ this->getCppEltSize(),
+ m,n,m,(S*)0,this->m_writable);
+ // Called shared-data constructor so p is non-owner.
+ return p;
+ }
+
+ // This creates an mx1 column vector.
+ This* createColumnView_(int j, int i, int m) {
+ This* p = cloneHelper_();
+ p->m_data = m > 0 ? updElt_(i+j) : 0;
+ p->m_row = false; // even if this was a row
+ p->m_actual.updStorage().setOrder(MatrixStorage::ColumnOrder);
+ return p;
+ }
+
+ // This creates a 1xn row vector.
+ This* createRowView_(int i, int j, int n) {
+ This* p = cloneHelper_();
+ p->m_data = n > 0 ? updElt_(i+j) : 0;
+ p->m_row = true; // even if this was a column
+ p->m_actual.updStorage().setOrder(MatrixStorage::RowOrder);
+ return p;
+ }
+
+ // Override for strided or indexed data.
+ virtual bool hasContiguousData_() const {return true;}
+ // Override for indexed data.
+ virtual bool hasRegularData_() const {return true;}
+
+ const S* getElt_ (int i) const {return this->m_data + i*this->m_eltSize;}
+ S* updElt_ (int i) {return this->m_data + i*this->m_eltSize;}
+ bool eltIsStored_(int i) const {return true;}
+
+ // Every element is stored so this just forwards to getElt(i).
+ void getAnyElt_(int i, S* value) const
+ { this->copyElt(value, getElt_(i)); }
+
+ // OK for any size elements that are packed contiguously.
+ This* createDeepCopy_() const {
+ This* p = cloneHelper_();
+ p->m_writable = true;
+ p->m_owner = true;
+ p->allocateData(this->nelt());
+ std::memcpy(p->m_data, this->m_data, this->length()*this->m_eltSize*sizeof(S));
+ return p;
+ }
+
+ // One of the lengths must be 1.
+ void resize_ (int m, int n) {resize_(m*n);}
+ void resizeKeep_ (int m, int n) {resizeKeep_(m*n);}
+
+ // OK for any size elements.
+ void resize_(int n) {
+ this->clearData();
+ this->allocateData(n);
+ }
+
+ // OK for any size elements that are packed contiguously.
+ void resizeKeep_(int n) {
+ S* const newData = this->allocateMemory(n);
+ const int nToCopy = std::min(n, this->length());
+ std::memcpy(newData, this->m_data, nToCopy*this->m_eltSize*sizeof(S));
+ this->clearData();
+ this->setData(newData);
+ }
+
+ void copyInFromCompatibleSource_(const MatrixHelperRep<S>& source) {
+ if (source.hasContiguousData() && this->nScalars())
+ std::memcpy(this->m_data, source.getElt(0, 0), this->nScalars()*sizeof(S));
+ else
+ FullVectorHelper<S>::copyInFromCompatibleSource_(source);
+ }
+};
+
+
+//----------------------- ContiguousVectorScalarHelper -------------------------
+/// All elements of the Vector are stored, and they are contiguous in memory,
+/// and the elements are scalars. This inherits most functionality from the
+/// contiguous general-element case.
+//------------------------------------------------------------------------------
+template <class S>
+class ContiguousVectorScalarHelper : public ContiguousVectorHelper<S> {
+ typedef ContiguousVectorScalarHelper<S> This;
+ typedef ContiguousVectorHelper<S> Base;
+public:
+ // Allocate our own space.
+ ContiguousVectorScalarHelper(int n, bool isRow) : Base(1,1,n,isRow) {}
+ // Use someone else's memory.
+ ContiguousVectorScalarHelper(int n, bool isRow, const S* shared, bool canWrite)
+ : Base(1,1,n,isRow,shared,canWrite) {}
+
+ This* cloneHelper_() const {return new This(*this);}
+
+ const S* getElt_ (int i) const {return this->m_data + i;}
+ S* updElt_ (int i) {return this->m_data + i;}
+
+ // Every element is stored so this just forwards to getElt(i).
+ void getAnyElt_(int i, S* value) const {*value = *getElt_(i);}
+};
+
+
+template <class S> inline VectorHelper<S>*
+FullVectorHelper<S>::createDiagonalView_() {
+ VectorHelper<S>* p = 0;
+ const int nDiags = std::min(this->length(), 1); // 0 or 1
+ S* data = nDiags ? this->updElt_(0) : 0;
+
+ p = (this->m_eltSize==1)
+ ? new ContiguousVectorScalarHelper<S>(nDiags, false, data, false)
+ : new ContiguousVectorHelper<S>(this->m_eltSize, this->m_cppEltSize, nDiags,
+ false, data, false);
+ return p;
+}
+
+
+//---------------------------- StridedVectorHelper -----------------------------
+/// This is a vector with regularly-spaced but non-contiguous elements. This
+/// is only for views so does not include an implementation for resizing.
+//------------------------------------------------------------------------------
+template <class S>
+class StridedVectorHelper : public FullVectorHelper<S> {
+ typedef StridedVectorHelper<S> This;
+ typedef FullVectorHelper<S> Base;
+public:
+ /// Use someone else's memory, which we assume to be the right size. Note
+ /// that stride is given in elements, with stride==1 meaning the elements
+ /// are packed contiguously, in which case this is the wrong helper class to use.
+ StridedVectorHelper(int esz, int cppesz, int n, bool isRow,
+ int strideInElements, const S* shared, bool canWrite)
+ : Base(esz,cppesz,isRow), m_spacing((ptrdiff_t)strideInElements * esz)
+ {
+ SimTK_ASSERT1(strideInElements >= 2,
+ "StridedVectorHelper::ctor(): illegal stride %d", strideInElements);
+ this->m_owner = false;
+ this->m_writable = canWrite;
+ this->setData(const_cast<S*>(shared));
+ this->m_actual.setStructure(MatrixStructure::Matrix1d);
+ this->m_actual.setStorage(
+ MatrixStorage(MatrixStorage::Vector, MatrixStorage::NoPlacement,
+ isRow ? MatrixStorage::RowOrder : MatrixStorage::ColumnOrder,
+ MatrixStorage::NoDiag));
+ this->m_actual.setActualSize(isRow?1:n, isRow?n:1); // apparent size; sets Outline
+ }
+
+ virtual This* cloneHelper_() const {return new This(*this);}
+
+ // A block view of a strided vector is usually a smaller vector with
+ // identical stride. No stride needed if there are fewer than two
+ // elements, though. Also, this could be an mX0 or 0Xn slice which is
+ // no longer a Vector unless m==1 or n==1.
+ MatrixHelperRep<S>* createBlockView_(const EltBlock& block) {
+ const int m=block.nrow(), n=block.ncol();
+ if ((m==0 && n!=1) || (n==0 && m!=1)) {
+ // One or both dimensions is 0 and the other is not 1, so this
+ // is no longer a 1d object.
+ RegularFullHelper<S>* p = 0;
+ if (this->getEltSize()==1)
+ p = new FullColOrderScalarHelper<S>(m,n,m,(S*)0,this->m_writable);
+ else p = new FullColOrderEltHelper<S>(this->getEltSize(),
+ this->getCppEltSize(),
+ m,n,m,(S*)0,this->m_writable);
+ // Called shared-data constructor so p is non-owner.
+ return p;
+ }
+
+ // At least one of m,n is a 1. Could still be 1x0 or 0x1.
+
+ VectorHelper<S>* p = 0;
+ const int start = block.row0() + block.col0(); // one of those is zero
+ const int length = m*n; // one of those is one
+ S* data = length ? updElt_(start) : 0;
+
+ if (length <= 1) {
+ p = (this->m_eltSize==1)
+ ? new ContiguousVectorScalarHelper<S>(length, false, data, false)
+ : new ContiguousVectorHelper<S>(this->m_eltSize, this->m_cppEltSize, length,
+ false, data, false);
+ // called a shared-data constructor, so p is non-owner
+ return p;
+ }
+
+ p = cloneHelper_();
+ p->setData(data);
+ return p;
+ }
+
+ // Row and column view are like block view but without the possibility
+ // of a non-Vector result.
+ VectorHelper<S>* createColumnView_(int j, int i, int m) {
+ VectorHelper<S>* p = 0;
+ const int start = i+j; // one of those is zero
+ S* data = m ? updElt_(start) : 0;
+
+ if (m <= 1) {
+ p = (this->m_eltSize==1)
+ ? new ContiguousVectorScalarHelper<S>(m, false, data, false)
+ : new ContiguousVectorHelper<S>(this->m_eltSize, this->m_cppEltSize, m,
+ false, data, false);
+ return p;
+ }
+
+ // length is > 1 so this must already be a column
+ assert(j==0);
+ p = cloneHelper_();
+ p->setData(data);
+ return p;
+ }
+
+ VectorHelper<S>* createRowView_(int i, int j, int n) {
+ VectorHelper<S>* p = 0;
+ const int start = i+j; // one of those is zero
+ S* data = n ? updElt_(start) : 0;
+
+ if (n <= 1) {
+ p = (this->m_eltSize==1)
+ ? new ContiguousVectorScalarHelper<S>(n, true, data, false)
+ : new ContiguousVectorHelper<S>(this->m_eltSize, this->m_cppEltSize, n,
+ true, data, false);
+ return p;
+ }
+
+ // length is > 1 so this must already be a row
+ assert(i==0);
+ p = cloneHelper_();
+ p->setData(data);
+ return p;
+ }
+
+
+ bool hasContiguousData_() const {return false;}
+ bool hasRegularData_() const {return true;}
+
+ bool eltIsStored_(int i) const {return true;}
+ const S* getElt_ (int i) const {return this->m_data + i*m_spacing;}
+ S* updElt_ (int i) {return this->m_data + i*m_spacing;}
+
+ // Every element is stored so this just forwards to getElt(i).
+ void getAnyElt_(int i, S* value) const
+ { this->copyElt(value, getElt_(i)); }
+
+ /// A deep copy of a strided vector produces a contiguous (stride==1)
+ /// vector containing the same number of elements.
+ FullVectorHelper<S>* createDeepCopy_() const {
+ ContiguousVectorHelper<S>* p =
+ new ContiguousVectorHelper<S>(this->m_eltSize, this->m_cppEltSize,
+ this->length(), this->m_row);
+ for (int i=0; i < this->length(); ++i)
+ this->copyElt(p->updData() + i*this->m_eltSize, this->getData() + i*m_spacing);
+ return p;
+ }
+
+protected:
+ ptrdiff_t m_spacing; // m_eltsize*stride (spacing in scalars)
+};
+
+
+//------------------------- StridedVectorScalarHelper --------------------------
+/// This is a vector with regularly-spaced but non-contiguous elements, where
+/// the elements are scalars. Most functionality is inherited from the general-
+/// element case but we specialize a few methods here for speed. This
+/// is only for views so does not include an implementation for resizing.
+//------------------------------------------------------------------------------
+template <class S>
+class StridedVectorScalarHelper : public StridedVectorHelper<S> {
+ typedef StridedVectorScalarHelper<S> This;
+ typedef StridedVectorHelper<S> Base;
+public:
+ /// Use someone else's memory, which we assume to be the right size. Note
+ /// that stride is given in elements, with stride==1 meaning the elements
+ /// are packed contiguously, in which case this is the wrong helper class to use.
+ StridedVectorScalarHelper(int n, bool isRow, int stride, const S* shared, bool canWrite)
+ : Base(1,1,n,isRow,stride,shared,canWrite) {}
+
+ This* cloneHelper_() const {return new This(*this);}
+
+ // Every element is stored so this just forwards to getElt(i).
+ void getAnyElt_(int i, S* value) const {*value = *this->getElt_(i);}
+
+ /// A deep copy of a strided vector produces a contiguous (stride==1)
+ /// vector containing the same number of elements.
+ FullVectorHelper<S>* createDeepCopy_() const {
+ ContiguousVectorScalarHelper<S>* p =
+ new ContiguousVectorScalarHelper<S>(this->length(), this->m_row);
+ const int nToCopy = this->length();
+ for (int i=0; i < nToCopy; ++i)
+ p->updData()[i] = this->getData()[i*this->m_spacing];
+ return p;
+ }
+};
+
+//--------------------------- IndexedVectorHelper ------------------------------
+/// All elements of the Vector are stored, but they are selected irregularly
+/// from the available data. This is only for views so does not include an
+/// implementation for resizing, and there is no constructor for data allocation.
+/// This class handles general elements; a derived class handles the special
+/// case of scalar elements by overriding some of the methods for speed.
+/// TODO: we only allow 32 bit indices, although 64 bit indices
+/// are possible (just need another helper class for "long long" indices).
+//------------------------------------------------------------------------------
+template <class S>
+class IndexedVectorHelper : public FullVectorHelper<S> {
+ typedef IndexedVectorHelper<S> This;
+ typedef FullVectorHelper<S> Base;
+public:
+ // Use someone else's memory, which we assume to be the right size. Here the
+ // indices must be in terms of elements. We'll store them internally in terms
+ // of scalars instead. We insist here that the indices are all nonnegative and
+ // in monotonically increasing order.
+ IndexedVectorHelper(int esz, int cppesz, int n, bool isRow,
+ const int* eltIndices, const S* shared, bool canWrite)
+ : Base(esz,cppesz,isRow), m_scalarIndices(0)
+ {
+ this->m_owner = false;
+ this->m_writable = canWrite;
+ this->setData(const_cast<S*>(shared));
+ this->m_actual.setStructure(MatrixStructure::Matrix1d);
+ this->m_actual.setStorage(
+ MatrixStorage(MatrixStorage::Vector, MatrixStorage::NoPlacement,
+ isRow ? MatrixStorage::RowOrder : MatrixStorage::ColumnOrder,
+ MatrixStorage::NoDiag));
+ this->m_actual.setActualSize(isRow?1:n, isRow?n:1); // apparent size; sets Outline
+
+ if (n) {
+ m_scalarIndices = new int[n];
+ for (int i=0; i<n; ++i) {
+ SimTK_ERRCHK(i==0 || eltIndices[i]>eltIndices[i-1], "IndexedVectorHelper::ctor()",
+ "Indices must be in monotonically ascending order.");
+ m_scalarIndices[i] = this->m_eltSize*eltIndices[i];
+ }
+ }
+ }
+
+ // Copy constructor must copy indices.
+ IndexedVectorHelper(const This& src) : Base(src), m_scalarIndices(0) {
+ if (src.length()) {
+ m_scalarIndices = new int[src.length()];
+ std::memcpy(m_scalarIndices, src.m_scalarIndices, src.length()*sizeof(int));
+ }
+ }
+
+
+ // (Virtual) destructor must delete indices.
+ ~IndexedVectorHelper() {delete[] m_scalarIndices;}
+
+ // Note that cloning the helper also copies the indices.
+ virtual This* cloneHelper_() const {return new This(*this);}
+
+ // A block view of an indexed vector is a shorter indexed vector.
+ // Also, this could be an mX0 or 0Xn slice which is
+ // no longer a Vector unless m==1 or n==1.
+ MatrixHelperRep<S>* createBlockView_(const EltBlock& block) {
+ const int m=block.nrow(), n=block.ncol();
+ if ((m==0 && n!=1) || (n==0 && m!=1)) {
+ // One or both dimensions is 0 and the other is not 1, so this
+ // is no longer a 1d object.
+ RegularFullHelper<S>* p = 0;
+ if (this->getEltSize()==1)
+ p = new FullColOrderScalarHelper<S>(m,n,m,(S*)0,this->m_writable);
+ else p = new FullColOrderEltHelper<S>(this->getEltSize(),
+ this->getCppEltSize(),
+ m,n,m,(S*)0,this->m_writable);
+ // Called a shared-data constructor so p is non-owner.
+ return p;
+ }
+
+ // At least one of the dimensions is a 1. Could still be 1x0 or 0x1.
+
+ const int start = block.row0() + block.col0(); // one of these is zero
+ const int length = block.nrow()*block.ncol(); // one of these is 1
+
+ if (length <= 1) {
+ // No need for indices; this is contiguous now.
+ S* data = length ? updElt_(start) : 0;
+ ContiguousVectorHelper<S>* p = (this->m_eltSize==1)
+ ? new ContiguousVectorScalarHelper<S>
+ (length, false, data, false)
+ : new ContiguousVectorHelper<S>
+ (this->m_eltSize, this->m_cppEltSize, length,
+ false, data, false);
+ // Called a shared-data constructor so p is non-owner.
+ return p;
+ }
+
+ // Still an indexed vector.
+
+ This* p = new This(*this, true); // don't copy the indices
+ p->m_data = this->m_data;
+ p->m_scalarIndices = new int[length];
+ std::memcpy(p->m_scalarIndices, m_scalarIndices+start,
+ length*sizeof(int));
+ return p;
+ }
+
+ VectorHelper<S>* createColumnView_(int j, int i, int m) {
+ if (m <= 1) {
+ S* data = m ? updElt_(i+j) : 0; // one of i or j is 0
+ VectorHelper<S>* p = (this->m_eltSize==1)
+ ? (VectorHelper<S>*)new ContiguousVectorScalarHelper<S>(m, false, data, false)
+ : (VectorHelper<S>*)new ContiguousVectorHelper<S>(this->m_eltSize, this->m_cppEltSize,
+ m, false, data, false);
+ return p;
+ }
+
+ // This must already be a column or we couldn't make a >1 element column here.
+ assert(j==0);
+ This* p = new This(*this, true); // don't copy the indices
+ p->setData(this->m_data); // leaving the indices the same, so data starts at 0
+ p->m_scalarIndices = new int[m];
+ std::memcpy(p->m_scalarIndices, m_scalarIndices+i, m*sizeof(int));
+ return p;
+ }
+
+
+ VectorHelper<S>* createRowView_(int i, int j, int n) {
+ if (n<= 1) {
+ S* data = n ? updElt_(i+j) : 0; // one of i or j is 0
+ VectorHelper<S>* p = (this->m_eltSize==1)
+ ? (VectorHelper<S>*)new ContiguousVectorScalarHelper<S>(n, true, data, false)
+ : (VectorHelper<S>*)new ContiguousVectorHelper<S>(this->m_eltSize, this->m_cppEltSize,
+ n, true, data, false);
+ return p;
+ }
+
+ // This must already be a row or we couldn't make a >1 element row here.
+ assert(i==0);
+ This* p = new This(*this, true); // don't copy the indices
+ p->setData(this->m_data); // leaving the indices the same, so data starts at 0
+ p->m_scalarIndices = new int[n];
+ std::memcpy(p->m_scalarIndices, m_scalarIndices+j, n*sizeof(int));
+ return p;
+ }
+
+ bool hasContiguousData_() const {return false;}
+ bool hasRegularData_() const {return false;}
+
+ const S* getElt_ (int i) const {return this->m_data + m_scalarIndices[i];}
+ S* updElt_ (int i) {return this->m_data + m_scalarIndices[i];}
+ bool eltIsStored_(int i) const {return true;}
+
+ // Every element is stored so this just forwards to getElt(i).
+ void getAnyElt_(int i, S* value) const
+ { this->copyElt(value, getElt_(i)); }
+
+ // A deep copy of an indexed vector produces a contiguous, non-indexed vector.
+ ContiguousVectorHelper<S>* createDeepCopy_() const {
+ ContiguousVectorHelper<S>* p =
+ new ContiguousVectorHelper<S>(this->m_eltSize, this->m_cppEltSize,
+ this->length(), this->m_row);
+ for (int i=0; i<this->length(); ++i)
+ this->copyElt(p->updElt_(i), getElt_(i));
+ return p;
+ }
+
+protected:
+ int* m_scalarIndices;
+
+private:
+ // This is like a copy constructor, but it does not copy the indices. The second
+ // parameter is a dummy.
+ IndexedVectorHelper(const This& src, bool) : Base(src), m_scalarIndices(0) {}
+};
+
+
+
+} // namespace SimTK
+
+
+#endif // SimTK_SimTKCOMMON_MATRIX_HELPER_REP_VECTOR_H_
diff --git a/SimTKcommon/CMakeLists.txt b/SimTKcommon/CMakeLists.txt
new file mode 100644
index 0000000..8d5ada0
--- /dev/null
+++ b/SimTKcommon/CMakeLists.txt
@@ -0,0 +1,191 @@
+#---------------------------------------------------
+# SimTKcommon
+#
+# Creates SimTK Core library, base name=SimTKcommon.
+# Default libraries are shared & optimized. Variants
+# are created for static (_static) and debug (_d) and
+# provision is made for an optional "namespace" (ns)
+# and version number (vn).
+#
+# Windows:
+# [ns_]SimTKcommon[_vn][_d].dll
+# [ns_]SimTKcommon[_vn][_d].lib
+# [ns_]SimTKcommon[_vn]_static[_d].lib
+# Linux/Cygwin:
+# lib[ns_]SimTKcommon[_vn][_d].so
+# lib[ns_]SimTKcommon[_vn]_static[_d].a
+# Mac:
+# lib[ns_]SimTKcommon[_vn][_d].dylib
+# lib[ns_]SimTKcommon[_vn]_static[_d].a
+#
+# Targets are installed in
+# %ProgramFiles%\SimTK\lib,bin (Win32)
+# %ProgramFiles(x86)%\SimTK\lib,bin (32 bit target on Win64)
+# %ProgramW6432%\SimTK\lib,bin (64 bit target on Win64)
+# /usr/local/SimTK/lib[64] (Linux, Mac, Cygwin)
+#
+#----------------------------------------------------
+
+PROJECT (SimTKcommon)
+
+# SimTKcommon depends on the Platform files but nothing else.
+INCLUDE_DIRECTORIES(${PLATFORM_INCLUDE_DIRECTORIES})
+
+# The source is organized into subdirectories, but we handle them all from
+# this CMakeLists file rather than letting CMake visit them as SUBDIRS.
+SET(SimTKCOMMON_SOURCE_SUBDIRS
+ . Scalar SmallMatrix Mechanics BigMatrix
+ Geometry Simulation Random Polynomial)
+
+# Collect up information about the version of the SimTKcommon library
+# we're building and make it available to the code so it can be built
+# into the binaries. This also determines the versioned library names
+# in which case all dependencies must use the same version.
+
+SET(SimTKCOMMON_MAJOR_VERSION ${SIMBODY_MAJOR_VERSION})
+SET(SimTKCOMMON_MINOR_VERSION ${SIMBODY_MINOR_VERSION})
+SET(SimTKCOMMON_PATCH_VERSION ${SIMBODY_PATCH_VERSION})
+
+# Report the version number to the CMake UI. Don't include the
+# build version if it is zero.
+SET(PATCH_VERSION_STRING)
+IF(SimTKCOMMON_PATCH_VERSION)
+ SET(PATCH_VERSION_STRING ".${SimTKCOMMON_PATCH_VERSION}")
+ENDIF()
+
+SET(SimTKCOMMON_VERSION
+ "${SimTKCOMMON_MAJOR_VERSION}.${SimTKCOMMON_MINOR_VERSION}${PATCH_VERSION_STRING}")
+
+# This is the suffix if we're building and depending on versioned libraries.
+SET(VN "_${SimTKCOMMON_VERSION}")
+
+
+SET(SimTKCOMMON_COPYRIGHT_YEARS "2005-10")
+
+# underbar separated list of dotted authors, no spaces or commas
+SET(SimTKCOMMON_AUTHORS "Michael.Sherman_Peter.Eastman")
+
+SET (SimTKCOMMON_SVN_REVISION ${SIMBODY_SVN_REVISION})
+
+ADD_DEFINITIONS(-DSimTK_SimTKCOMMON_LIBRARY_NAME=${SimTKCOMMON_LIBRARY_NAME}
+ -DSimTK_SimTKCOMMON_MAJOR_VERSION=${SimTKCOMMON_MAJOR_VERSION}
+ -DSimTK_SimTKCOMMON_MINOR_VERSION=${SimTKCOMMON_MINOR_VERSION}
+ -DSimTK_SimTKCOMMON_PATCH_VERSION=${SimTKCOMMON_PATCH_VERSION})
+
+IF(NEED_QUOTES)
+ ADD_DEFINITIONS(-DSimTK_SimTKCOMMON_SVN_REVISION="${SimTKCOMMON_SVN_REVISION}"
+ -DSimTK_SimTKCOMMON_COPYRIGHT_YEARS="${SimTKCOMMON_COPYRIGHT_YEARS}"
+ -DSimTK_SimTKCOMMON_AUTHORS="${SimTKCOMMON_AUTHORS}")
+ELSE(NEED_QUOTES)
+ ADD_DEFINITIONS(-DSimTK_SimTKCOMMON_SVN_REVISION=${SimTKCOMMON_SVN_REVISION}
+ -DSimTK_SimTKCOMMON_COPYRIGHT_YEARS=${SimTKCOMMON_COPYRIGHT_YEARS}
+ -DSimTK_SimTKCOMMON_AUTHORS=${SimTKCOMMON_AUTHORS})
+ENDIF(NEED_QUOTES)
+
+# -DSimTK_SimTKCOMMON_LIBRARY_TYPE has to be defined in the target subdirectories.
+# -DSimTKcommon_EXPORTS defined automatically when Windows DLL build is being done.
+
+SET(SHARED_TARGET ${SimTKCOMMON_LIBRARY_NAME})
+SET(STATIC_TARGET ${SimTKCOMMON_LIBRARY_NAME}_static)
+SET(SHARED_TARGET_VN ${SimTKCOMMON_LIBRARY_NAME}${VN})
+SET(STATIC_TARGET_VN ${SimTKCOMMON_LIBRARY_NAME}${VN}_static)
+
+# But on Unix or Cygwin we have to add the suffix manually
+IF (UNIX AND CMAKE_BUILD_TYPE MATCHES Debug)
+ SET(SHARED_TARGET ${SHARED_TARGET}_d)
+ SET(STATIC_TARGET ${STATIC_TARGET}_d)
+ SET(SHARED_TARGET_VN ${SHARED_TARGET_VN}_d)
+ SET(STATIC_TARGET_VN ${STATIC_TARGET_VN}_d)
+ENDIF (UNIX AND CMAKE_BUILD_TYPE MATCHES Debug)
+
+
+## Test against the unversioned libraries if they are being build;
+## otherwise against the versioned libraries.
+IF(BUILD_UNVERSIONED_LIBRARIES)
+ SET(TEST_SHARED_TARGET ${SHARED_TARGET})
+ SET(TEST_STATIC_TARGET ${STATIC_TARGET})
+ELSE(BUILD_UNVERSIONED_LIBRARIES)
+ SET(TEST_SHARED_TARGET ${SHARED_TARGET_VN})
+ SET(TEST_STATIC_TARGET ${STATIC_TARGET_VN})
+ENDIF(BUILD_UNVERSIONED_LIBRARIES)
+
+
+# These are all the places to search for header files which are
+# to be part of the API.
+SET(API_INCLUDE_DIRS) # start empty
+SET(SimTKCOMMON_INCLUDE_DIRS) # start empty
+FOREACH(subdir ${SimTKCOMMON_SOURCE_SUBDIRS})
+ # append
+ SET(API_INCLUDE_DIRS ${API_INCLUDE_DIRS}
+ ${PROJECT_SOURCE_DIR}/${subdir}/include
+ ${PROJECT_SOURCE_DIR}/${subdir}/include/SimTKcommon
+ ${PROJECT_SOURCE_DIR}/${subdir}/include/SimTKcommon/internal)
+ # Referencing headers must always be done relative to this level.
+ SET(SimTKCOMMON_INCLUDE_DIRS ${SimTKCOMMON_INCLUDE_DIRS}
+ ${PROJECT_SOURCE_DIR}/${subdir}/include)
+ENDFOREACH(subdir)
+
+
+# Include the SimTKcommon API include directories now so that SimTKcommon code
+# can use them.
+INCLUDE_DIRECTORIES(${SimTKCOMMON_INCLUDE_DIRS})
+
+# Pass up the include directories list to the parent so
+# subsequent libraries can use them.
+SET(SimTKCOMMON_INCLUDE_DIRECTORIES ${SimTKCOMMON_INCLUDE_DIRS}
+ PARENT_SCOPE)
+
+# We'll need both *relative* path names, starting with their API_INCLUDE_DIRS,
+# and absolute pathnames.
+SET(API_REL_INCLUDE_FILES) # start these out empty
+SET(API_ABS_INCLUDE_FILES)
+
+FOREACH(dir ${API_INCLUDE_DIRS})
+ FILE(GLOB fullpaths ${dir}/*.h) # returns full pathnames
+ SET(API_ABS_INCLUDE_FILES ${API_ABS_INCLUDE_FILES} ${fullpaths})
+
+ FOREACH(pathname ${fullpaths})
+ GET_FILENAME_COMPONENT(filename ${pathname} NAME)
+ SET(API_REL_INCLUDE_FILES ${API_REL_INCLUDE_FILES} ${dir}/${filename})
+ ENDFOREACH(pathname)
+ENDFOREACH(dir)
+
+# collect up source files
+SET(SOURCE_FILES) # empty
+SET(SOURCE_INCLUDE_FILES)
+
+FOREACH(subdir ${SimTKCOMMON_SOURCE_SUBDIRS})
+ FILE(GLOB src_files ${subdir}/src/*.cpp)
+ FILE(GLOB incl_files ${subdir}/src/*.h)
+ SET(SOURCE_FILES ${SOURCE_FILES} ${src_files}) #append
+ SET(SOURCE_INCLUDE_FILES ${SOURCE_INCLUDE_FILES} ${incl_files})
+ENDFOREACH(subdir)
+
+#
+# Installation
+#
+# libraries are installed from their subdirectories; headers here
+
+# install headers
+FILE(GLOB CORE_HEADERS include/*.h */include/*.h)
+FILE(GLOB TOP_HEADERS include/SimTKcommon/*.h */include/SimTKcommon/*.h)
+FILE(GLOB INTERNAL_HEADERS include/SimTKcommon/internal/*.h */include/SimTKcommon/internal/*.h)
+INSTALL_FILES(/${SIMBODY_INCLUDE_INSTALL_DIR}/ FILES ${CORE_HEADERS})
+INSTALL_FILES(/${SIMBODY_INCLUDE_INSTALL_DIR}/SimTKcommon/ FILES ${TOP_HEADERS})
+INSTALL_FILES(/${SIMBODY_INCLUDE_INSTALL_DIR}/SimTKcommon/internal FILES ${INTERNAL_HEADERS})
+
+FILE(GLOB SIMTKCOMMON_DOCS doc/*.pdf doc/*.txt)
+INSTALL(FILES ${SIMTKCOMMON_DOCS} DESTINATION ${CMAKE_INSTALL_DOCDIR})
+
+# These are at the end because we want them processed after
+# all the various variables have been set above.
+
+IF(BUILD_STATIC_LIBRARIES)
+ ADD_SUBDIRECTORY( staticTarget )
+ENDIF()
+ADD_SUBDIRECTORY( sharedTarget )
+
+IF( BUILD_TESTING )
+ ADD_SUBDIRECTORY( tests )
+ENDIF( BUILD_TESTING )
+
diff --git a/SimTKcommon/Geometry/include/SimTKcommon/internal/DecorationGenerator.h b/SimTKcommon/Geometry/include/SimTKcommon/internal/DecorationGenerator.h
new file mode 100644
index 0000000..1fb2e60
--- /dev/null
+++ b/SimTKcommon/Geometry/include/SimTKcommon/internal/DecorationGenerator.h
@@ -0,0 +1,62 @@
+#ifndef SimTK_SimTKCOMMON_DECORATION_GENERATOR_H_
+#define SimTK_SimTKCOMMON_DECORATION_GENERATOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/basics.h"
+
+namespace SimTK {
+
+class State;
+
+/**
+ * A DecorationGenerator is used to define geometry that may change over the
+ * course of a simulation. Example include
+ * - Geometry whose position is not fixed relative to any single body.
+ * - Geometry which may appear or disappear during the simulation.
+ * - Geometry whose properties (color, size, etc.) may change during the
+ * simulation.
+ *
+ * To use it, define a concrete subclass that implements generateDecorations()
+ * to generate whatever geometry is appropriate for a given State. It can then
+ * be added to a DecorationSubsystem, or directly to a Visualizer.
+ */
+class DecorationGenerator {
+public:
+ /**
+ * This will be called every time a new State is about to be visualized.
+ * It should generate whatever decorations are appropriate for the State
+ * and append them to the array.
+ */
+ virtual void generateDecorations(const State& state,
+ Array_<DecorativeGeometry>& geometry) = 0;
+
+ /** Destructor is virtual; be sure to override it if you have something
+ to clean up at the end. **/
+ virtual ~DecorationGenerator() {}
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_DECORATION_GENERATOR_H_
diff --git a/SimTKcommon/Geometry/include/SimTKcommon/internal/DecorativeGeometry.h b/SimTKcommon/Geometry/include/SimTKcommon/internal/DecorativeGeometry.h
new file mode 100644
index 0000000..c112be4
--- /dev/null
+++ b/SimTKcommon/Geometry/include/SimTKcommon/internal/DecorativeGeometry.h
@@ -0,0 +1,666 @@
+#ifndef SimTK_SimTKCOMMON_DECORATIVE_GEOMETRY_H_
+#define SimTK_SimTKCOMMON_DECORATIVE_GEOMETRY_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Jack Middleton, Peter Eastman, Ayman Habib *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declarations of DecorativeGeometry and related derived classes. **/
+
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/PolygonalMesh.h"
+
+#include <cassert>
+
+
+namespace SimTK {
+
+// Some common RGB values;
+extern SimTK_SimTKCOMMON_EXPORT const Vec3 Black; ///< RGB=( 0, 0, 0)
+extern SimTK_SimTKCOMMON_EXPORT const Vec3 Gray; ///< RGB=(.5,.5,.5)
+extern SimTK_SimTKCOMMON_EXPORT const Vec3 Red; ///< RGB=( 1, 0, 0)
+extern SimTK_SimTKCOMMON_EXPORT const Vec3 Green; ///< RGB=( 0, 1, 0)
+extern SimTK_SimTKCOMMON_EXPORT const Vec3 Blue; ///< RGB=( 0, 0, 1)
+extern SimTK_SimTKCOMMON_EXPORT const Vec3 Yellow; ///< RGB=( 1, 1, 0)
+extern SimTK_SimTKCOMMON_EXPORT const Vec3 Orange; ///< RGB=( 1,.5, 0)
+extern SimTK_SimTKCOMMON_EXPORT const Vec3 Magenta; ///< RGB=( 1, 0, 1)
+extern SimTK_SimTKCOMMON_EXPORT const Vec3 Purple; ///< RGB=(.5, 0,.5)
+extern SimTK_SimTKCOMMON_EXPORT const Vec3 Cyan; ///< RGB=( 0, 1, 1)
+extern SimTK_SimTKCOMMON_EXPORT const Vec3 White; ///< RGB=( 1, 1, 1)
+
+// Drawing representations
+
+class DecorativeGeometryImplementation;
+
+/** This is the client-side interface to an implementation-independent
+representation of "Decorations" suitable for visualization, annotation,
+logging, or debugging but which cannot have any effect on the behavior of
+a System or the evolution of a Study. DO NOT confuse this with real geometry
+(like contact geometry) which can represent physically meaningful objects that
+may interact and change the behavior of a System. However, all geometry objects
+can generate %DecorativeGeometry for their visualization.
+
+Why is there a %DecorativeGeometry facility at the System level at all, so far
+away from any application program? That's because for crude visualization and
+debugging purposes, the Subsystems themselves are best able to produce some
+illustrative geometry. Otherwise, you need a special purpose visualization
+tool which understands what's going on inside each subsystem. If you don't mind
+taking what you get, just ask each subsystem to generate what it thinks would
+be helpful visualization. To do that, the subysystems need a way to talk about
+geometry without knowing anything about how that geometry will eventually get
+onto someone's screen. And that's why we're here!
+
+Each %DecorativeGeometry object has its own local coordinate system and is
+defined self-consistently but independent of anything else. Clients can
+associate these with a reference frame (e.g. a body), and place the local frame
+of the geometry objects on the reference frame, or at a fixed transform from
+the reference frame. That places the %DecorativeGeometry objects in a scene. We
+support both 3D objects which are attached to actors in the scene, and 2D
+"screen" objects like titles which are attached to the display rather than the
+actors. The classes here deal only with the local-frame definitions of the
+geometric objects, not their placement in the scene.
+
+This is an abstract handle class using the PIMPL design pattern to hide the
+private implementation. This is effectively an abstract class although the
+virtual function table is hidden in the private part. **/
+class SimTK_SimTKCOMMON_EXPORT DecorativeGeometry {
+public:
+/** Default constructor creates an empty handle. **/
+DecorativeGeometry() : rep(0) { }
+~DecorativeGeometry();
+/** Copy construction is deep; the source object will be cloned to create an
+independent copy. **/
+DecorativeGeometry(const DecorativeGeometry& source);
+/** Copy assignment is deep; the handle will be cleared if necessary and then
+the source object will be cloned to create an independent copy. **/
+DecorativeGeometry& operator=(const DecorativeGeometry& source);
+
+/** Drawing modes. **/
+enum Representation {
+ DrawPoints = 1, ///< Use a cloud of points.
+ DrawWireframe = 2, ///< Use a line drawing.
+ DrawSurface = 3, ///< Use a shaded surface.
+
+ DrawDefault = -1 ///< Let someone else decide.
+};
+
+/** By default the geometry should be placed relative to the Ground frame. If
+you want it attached to another reference frame (body), say so here. The
+geometry should be rendered with respect to the indicated body frame; however,
+the interpretation of this integer Id is left to the implementation. If you
+don't set the \a bodyId yourself it will be zero. For use in Simbody, the
+\a bodyId is interpreted as a MobilizedBodyIndex that can be mapped to a
+MobilizedBody that carries a reference frame.
+
+The \a bodyId is copied if you copy the %DecorativeGeometry object. If you are
+copying to a different body you'll need to change the bodyId afterwards. **/
+DecorativeGeometry& setBodyId(int bodyId);
+
+/** For selection or other purposes, you may want to use this method to store
+an index that can identify this particular piece of geometry. As an alternative,
+or addition, see setUserRef(). In any case the \a index is simply stored with
+the object and returned if you ask for it. If you don't set it the value
+is -1. The \a index is copied if you copy the %DecorativeGeometry object. Be
+sure to change it afterwards if that is not the correct index for the copy. **/
+DecorativeGeometry& setIndexOnBody(int index);
+
+/** Use this method to store an arbitrary reference pointer with this
+%DecorativeGeometry object. This is particularly useful in selection operations
+where the rendering of this object has been picked by a user and you want to
+map it back to some meaningful object in your model. You can also use the
+setIndexOnBody() method to store some additional identifying information.
+If you don't set this pointer it will be set to zero (nullptr). This value
+is stored and returned only; no interpretation is done and the pointed-to
+object will not be deleted when the %DecorativeGeometry object is deleted.
+
+ at warning The value of the \a userRef pointer is copied if you make a copy of the
+%DecorativeGeometry object. That is likely to be incorrect in many
+circumstances, depending on how you are using this value. Be sure to clear or
+change the pointer if necessary after you make a copy. **/
+DecorativeGeometry& setUserRef(void* userRef);
+
+/** This transform shifts the generated polygons with respect to this object's
+local frame. Subsequent calls with other transforms simply replace the earlier
+one; they do not accumulate. The default transform is identity and you can call
+setTransform(Transform()) to put the transform back into its original state.
+This value affects the generated polygonal data. **/
+DecorativeGeometry& setTransform(const Transform& X_BG);
+
+/** Each concrete %DecorativeGeometry object is expected to have a default
+resolution that gets the point across but is cheap to draw and hence probably
+somewhat "chunky". The resolution parameter here scales that default up or
+down. The face density in the displayed representation is roughly
+proportional to this value. 1.0 means to use the default resolution. Values
+less than 1.0 are lower resolution, and values greater than 1.0 are higher
+resolution. A value less than or equal to zero here is interpreted as an
+instruction to "use the default". **/
+DecorativeGeometry& setResolution(Real);
+
+/** Each concrete DecorativeGeometry object is expected to have a default size
+around "1", whatever that means for a particular object, and most objects also
+allow a user-specified size on construction. The x,y,z scale factors here are
+given in the object's coordinate frame, and apply to the object as the user
+built it, or to the default if the user didn't specify a size. The default
+scaling is 1,1,1 and any value less than or equal to zero here is interpreted
+as a request to "use the default" in that direction. **/
+DecorativeGeometry& setScaleFactors(const Vec3& scale);
+
+/** Convenience method to set all three scale factors to the same value. **/
+DecorativeGeometry& setScale(Real scale) {return setScaleFactors(Vec3(scale));}
+
+/** Return the \a bodyId that was supplied to the most recent setBodyId()
+call for this %DecorativeGeometry object, or zero if that method has not been
+called. Copy construction and copy assignment copy the \a bodyId. This is
+intended to identify the body frame to which this geometry's placement should be
+relative; with the default zero value meaning the Ground or World frame. **/
+int getBodyId() const;
+
+/** Return the \a index that was supplied to the most recent setIndexOnBody()
+call for this %DecorativeGeometry object, or -1 if that method has not been
+called. Copy construction and copy assignment copy the \a index. Interpretation
+of this integer is up to the caller. **/
+int getIndexOnBody() const;
+
+/** Return the pointer value that was supplied to the most recent setUserRef()
+call for this %DecorativeGeometry object, or zero (nullptr) if that method has
+not been called. Copy construction and copy assignment copy the pointer.
+Interpretation of this value is up to the caller. **/
+void* getUserRef() const;
+
+/** Return the current setting of the "resolution" factor. A return value of
+-1 means "use the default". **/
+Real getResolution() const;
+
+/** Return the current value of the object's transform. If none has been set
+this will be the identity transform. Note that this transform specifies how the
+polygons are placed with respect to the object's local frame. **/
+const Transform& getTransform() const;
+
+/** Return the current setting of the "scale" factors. A return value of -1
+in one of the factors means to "use the default" (which is typically 1) in
+that direction. **/
+const Vec3& getScaleFactors() const;
+
+/** Request a specific color for this DecorativeGeometry object. The default
+is that the color is determined elsewhere. To explicitly request the default,
+set the color to Vec3(-1). The implementation will check the 0'th element
+(that is, the "R" element) and if it is less than zero will ignore the other
+two elements and use the default for all three. **/
+DecorativeGeometry& setColor(const Vec3& rgb); // 0-1 for each color
+
+/** Request a level of transparency for this DecorativeGeometry. This does NOT
+affect the generated geometry here. The default is that opacity is
+determined elsewhere. **/
+DecorativeGeometry& setOpacity(Real); // 0-1; default is 1 (opaque)
+
+/** Request an adjustment to the default rendering of lines and curves. This
+does NOT affect geometry generated here; it is a request passed on to the
+renderer which will probably pass it on to the hardware. A value less
+than or equal to zero here is interpreted as "use the default". **/
+DecorativeGeometry& setLineThickness(Real);
+
+/** Return the color specified for this object, if any, otherwise Vec3(-1)
+indicating that the default color will be used. **/
+const Vec3& getColor() const;
+/** Return the opacity specified for this object. **/
+Real getOpacity() const;
+/** Return the line thickness specified for this object, if any, otherwise
+return -1 to indicate that the default line thickness should be used. **/
+Real getLineThickness() const;
+
+/** Set whether the geometry acts as a billboard, always rotating to face the
+camera. The default is typically no except for text. If you want 3D text that
+moves with your model, set this to true. Here 0 means false, 1 means true,
+and -1 means "use default". **/
+DecorativeGeometry& setFaceCamera(int shouldFace);
+/** Get whether the geometry acts as a billboard, always rotating to face the
+camera. Returns 0 for no, 1 for yes, -1 for "is using default". **/
+int getFaceCamera() const;
+
+/** Request a particular rendering representation of this DecorativeGeometry
+object. The default is that the rendering representation choice is made
+elsewhere. **/
+DecorativeGeometry& setRepresentation(const Representation&);
+
+/** Returns drawing mode: -1 means "use default"; see above for others. **/
+Representation getRepresentation() const;
+
+void implementGeometry(DecorativeGeometryImplementation&) const;
+
+// Bookkeeping below here -- internal use only. Don't look below or you will
+// turn into a pillar of salt.
+bool isOwnerHandle() const;
+bool isEmptyHandle() const;
+explicit DecorativeGeometry(class DecorativeGeometryRep* r) : rep(r) { }
+bool hasRep() const {return rep!=0;}
+const DecorativeGeometryRep& getRep() const {assert(rep); return *rep;}
+DecorativeGeometryRep& updRep() {assert(rep); return *rep;}
+protected:
+DecorativeGeometryRep* rep;
+};
+
+
+/** A point of interest. Note that the point's location is given relative
+to the DecorativeGeometry frame so it will move if the geometry is transformed
+when attached somewhere or displayed. The default constructor will put the
+point at (0,0,0). **/
+class SimTK_SimTKCOMMON_EXPORT DecorativePoint : public DecorativeGeometry {
+public:
+ explicit DecorativePoint(const Vec3& p=Vec3(0));
+
+ // These are specific to DecorativePoint.
+
+ DecorativePoint& setPoint(const Vec3& p);
+ const Vec3& getPoint() const;
+
+ // Retain the derived type when setting generic geometry options.
+ DecorativePoint& setBodyId(int b) {DecorativeGeometry::setBodyId(b); return *this;}
+ DecorativePoint& setIndexOnBody(int x) {DecorativeGeometry::setIndexOnBody(x); return *this;}
+ DecorativePoint& setUserRef(void* p) {DecorativeGeometry::setUserRef(p); return *this;}
+ DecorativePoint& setTransform(const Transform& X_BD) {DecorativeGeometry::setTransform(X_BD); return *this;}
+ DecorativePoint& setResolution(Real r) {DecorativeGeometry::setResolution(r); return *this;}
+ DecorativePoint& setScaleFactors(const Vec3& s) {DecorativeGeometry::setScaleFactors(s); return *this;}
+ DecorativePoint& setColor(const Vec3& rgb) {DecorativeGeometry::setColor(rgb); return *this;}
+ DecorativePoint& setOpacity(Real o) {DecorativeGeometry::setOpacity(o); return *this;}
+ DecorativePoint& setLineThickness(Real t) {DecorativeGeometry::setLineThickness(t); return *this;}
+ DecorativePoint& setRepresentation(const Representation& r)
+ { DecorativeGeometry::setRepresentation(r); return *this; }
+
+ SimTK_PIMPL_DOWNCAST(DecorativePoint, DecorativeGeometry);
+private:
+ class DecorativePointRep& updRep();
+ const DecorativePointRep& getRep() const;
+};
+
+/** A line between two points. Note that the actual placement can be changed
+by the parent class transform & scale; here we are just generating the
+initial line in the geometry object's local frame.
+
+There is a default constructor for this object but it is not much
+use unless followed by endpoint specifications. By default we produce
+a line going from (0,0,0) to (1,1,1) just so it will show up if you
+forget to set it to something meaningful. Having a default constructor
+allows us to have arrays of these objects. **/
+class SimTK_SimTKCOMMON_EXPORT DecorativeLine : public DecorativeGeometry {
+public:
+ explicit DecorativeLine(const Vec3& p1=Vec3(0), const Vec3& p2=Vec3(1)); // line between p1 and p2
+
+ // These are specific to lines.
+ DecorativeLine& setPoint1(const Vec3& p1);
+ DecorativeLine& setPoint2(const Vec3& p2);
+ DecorativeLine& setEndpoints(const Vec3& p1, const Vec3& p2);
+
+ // Retain the derived type when setting generic geometry options.
+ DecorativeLine& setBodyId(int b) {DecorativeGeometry::setBodyId(b); return *this;}
+ DecorativeLine& setIndexOnBody(int x) {DecorativeGeometry::setIndexOnBody(x); return *this;}
+ DecorativeLine& setUserRef(void* p) {DecorativeGeometry::setUserRef(p); return *this;}
+ DecorativeLine& setTransform(const Transform& X_BD) {DecorativeGeometry::setTransform(X_BD); return *this;}
+ DecorativeLine& setResolution(Real r) {DecorativeGeometry::setResolution(r); return *this;}
+ DecorativeLine& setScaleFactors(const Vec3& s) {DecorativeGeometry::setScaleFactors(s); return *this;}
+ DecorativeLine& setColor(const Vec3& rgb) {DecorativeGeometry::setColor(rgb); return *this;}
+ DecorativeLine& setOpacity(Real o) {DecorativeGeometry::setOpacity(o); return *this;}
+ DecorativeLine& setLineThickness(Real t) {DecorativeGeometry::setLineThickness(t); return *this;}
+ DecorativeLine& setRepresentation(const Representation& r)
+ { DecorativeGeometry::setRepresentation(r); return *this; }
+
+ const Vec3& getPoint1() const;
+ const Vec3& getPoint2() const;
+
+ SimTK_PIMPL_DOWNCAST(DecorativeLine, DecorativeGeometry);
+private:
+ class DecorativeLineRep& updRep();
+ const DecorativeLineRep& getRep() const;
+};
+
+/** This defines a circle in the x-y plane, centered at the origin. The
+default constructor creates a circle of diameter 1. **/
+class SimTK_SimTKCOMMON_EXPORT DecorativeCircle : public DecorativeGeometry {
+public:
+ explicit DecorativeCircle(Real radius=0.5);
+
+ DecorativeCircle& setRadius(Real);
+ Real getRadius() const;
+
+ // Retain the derived type when setting generic geometry options.
+ DecorativeCircle& setBodyId(int b) {DecorativeGeometry::setBodyId(b); return *this;}
+ DecorativeCircle& setIndexOnBody(int x) {DecorativeGeometry::setIndexOnBody(x); return *this;}
+ DecorativeCircle& setUserRef(void* p) {DecorativeGeometry::setUserRef(p); return *this;}
+ DecorativeCircle& setTransform(const Transform& X_BD) {DecorativeGeometry::setTransform(X_BD); return *this;}
+ DecorativeCircle& setResolution(Real r) {DecorativeGeometry::setResolution(r); return *this;}
+ DecorativeCircle& setScaleFactors(const Vec3& s) {DecorativeGeometry::setScaleFactors(s); return *this;}
+ DecorativeCircle& setColor(const Vec3& rgb) {DecorativeGeometry::setColor(rgb); return *this;}
+ DecorativeCircle& setOpacity(Real o) {DecorativeGeometry::setOpacity(o); return *this;}
+ DecorativeCircle& setLineThickness(Real t) {DecorativeGeometry::setLineThickness(t); return *this;}
+ DecorativeCircle& setRepresentation(const Representation& r)
+ { DecorativeGeometry::setRepresentation(r); return *this; }
+
+ SimTK_PIMPL_DOWNCAST(DecorativeCircle, DecorativeGeometry);
+private:
+ class DecorativeCircleRep& updRep();
+ const DecorativeCircleRep& getRep() const;
+};
+
+/** This defines a sphere centered at the origin. The default constructor
+creates a sphere of diameter 1. **/
+class SimTK_SimTKCOMMON_EXPORT DecorativeSphere : public DecorativeGeometry {
+public:
+ explicit DecorativeSphere(Real radius=0.5);
+
+ DecorativeSphere& setRadius(Real);
+ Real getRadius() const;
+
+ // Retain the derived type when setting generic geometry options.
+ DecorativeSphere& setBodyId(int b) {DecorativeGeometry::setBodyId(b); return *this;}
+ DecorativeSphere& setIndexOnBody(int x) {DecorativeGeometry::setIndexOnBody(x); return *this;}
+ DecorativeSphere& setUserRef(void* p) {DecorativeGeometry::setUserRef(p); return *this;}
+ DecorativeSphere& setTransform(const Transform& X_BD) {DecorativeGeometry::setTransform(X_BD); return *this;}
+ DecorativeSphere& setResolution(Real r) {DecorativeGeometry::setResolution(r); return *this;}
+ DecorativeSphere& setScaleFactors(const Vec3& s) {DecorativeGeometry::setScaleFactors(s); return *this;}
+ DecorativeSphere& setColor(const Vec3& rgb) {DecorativeGeometry::setColor(rgb); return *this;}
+ DecorativeSphere& setOpacity(Real o) {DecorativeGeometry::setOpacity(o); return *this;}
+ DecorativeSphere& setLineThickness(Real t) {DecorativeGeometry::setLineThickness(t); return *this;}
+ DecorativeSphere& setRepresentation(const Representation& r)
+ { DecorativeGeometry::setRepresentation(r); return *this; }
+
+ SimTK_PIMPL_DOWNCAST(DecorativeSphere, DecorativeGeometry);
+private:
+ class DecorativeSphereRep& updRep();
+ const DecorativeSphereRep& getRep() const;
+};
+
+/** This defines an ellipsoidal solid centered at the origin and aligned with
+the local frame axes. The default constructor creates an ellipsoid with radii
+(1/2, 1/3, 1/4) in x,y,z resp. **/
+class SimTK_SimTKCOMMON_EXPORT DecorativeEllipsoid : public DecorativeGeometry {
+public:
+ explicit DecorativeEllipsoid(const Vec3& radii =
+ Vec3(Real(0.5),Real(1/3.),Real(0.25)));
+
+ DecorativeEllipsoid& setRadii(const Vec3&);
+ const Vec3& getRadii() const;
+
+ // Retain the derived type when setting generic geometry options.
+ DecorativeEllipsoid& setBodyId(int b) {DecorativeGeometry::setBodyId(b); return *this;}
+ DecorativeEllipsoid& setIndexOnBody(int x) {DecorativeGeometry::setIndexOnBody(x); return *this;}
+ DecorativeEllipsoid& setUserRef(void* p) {DecorativeGeometry::setUserRef(p); return *this;}
+ DecorativeEllipsoid& setTransform(const Transform& X_BD) {DecorativeGeometry::setTransform(X_BD); return *this;}
+ DecorativeEllipsoid& setResolution(Real r) {DecorativeGeometry::setResolution(r); return *this;}
+ DecorativeEllipsoid& setScaleFactors(const Vec3& s) {DecorativeGeometry::setScaleFactors(s); return *this;}
+ DecorativeEllipsoid& setColor(const Vec3& rgb) {DecorativeGeometry::setColor(rgb); return *this;}
+ DecorativeEllipsoid& setOpacity(Real o) {DecorativeGeometry::setOpacity(o); return *this;}
+ DecorativeEllipsoid& setLineThickness(Real t) {DecorativeGeometry::setLineThickness(t); return *this;}
+ DecorativeEllipsoid& setRepresentation(const Representation& r)
+ { DecorativeGeometry::setRepresentation(r); return *this; }
+
+ SimTK_PIMPL_DOWNCAST(DecorativeEllipsoid, DecorativeGeometry);
+private:
+ class DecorativeEllipsoidRep& updRep();
+ const DecorativeEllipsoidRep& getRep() const;
+};
+
+/** This defines a rectangular solid centered at the origin and aligned with
+the local frame axes. The default constructor creates a cube of length 1 on
+each side. **/
+class SimTK_SimTKCOMMON_EXPORT DecorativeBrick : public DecorativeGeometry {
+public:
+ explicit DecorativeBrick(const Vec3& halfLengths = Vec3(Real(0.5)));
+
+ DecorativeBrick& setHalfLengths(const Vec3&);
+ const Vec3& getHalfLengths() const;
+
+ // Retain the derived type when setting generic geometry options.
+ DecorativeBrick& setBodyId(int b) {DecorativeGeometry::setBodyId(b); return *this;}
+ DecorativeBrick& setIndexOnBody(int x) {DecorativeGeometry::setIndexOnBody(x); return *this;}
+ DecorativeBrick& setUserRef(void* p) {DecorativeGeometry::setUserRef(p); return *this;}
+ DecorativeBrick& setTransform(const Transform& X_BD) {DecorativeGeometry::setTransform(X_BD); return *this;}
+ DecorativeBrick& setResolution(Real r) {DecorativeGeometry::setResolution(r); return *this;}
+ DecorativeBrick& setScaleFactors(const Vec3& s) {DecorativeGeometry::setScaleFactors(s); return *this;}
+ DecorativeBrick& setColor(const Vec3& rgb) {DecorativeGeometry::setColor(rgb); return *this;}
+ DecorativeBrick& setOpacity(Real o) {DecorativeGeometry::setOpacity(o); return *this;}
+ DecorativeBrick& setLineThickness(Real t) {DecorativeGeometry::setLineThickness(t); return *this;}
+ DecorativeBrick& setRepresentation(const Representation& r)
+ { DecorativeGeometry::setRepresentation(r); return *this; }
+
+ SimTK_PIMPL_DOWNCAST(DecorativeBrick, DecorativeGeometry);
+private:
+ class DecorativeBrickRep& updRep();
+ const DecorativeBrickRep& getRep() const;
+};
+
+/** This defines a cylinder centered on the origin and aligned in the y
+direction. The default constructor gives it a height of 1 and the base circle
+a diameter of 1. **/
+class SimTK_SimTKCOMMON_EXPORT DecorativeCylinder : public DecorativeGeometry {
+public:
+ explicit DecorativeCylinder(Real radius=0.5, Real halfHeight=0.5);
+
+ DecorativeCylinder& setRadius(Real);
+ DecorativeCylinder& setHalfHeight(Real);
+ Real getRadius() const;
+ Real getHalfHeight() const;
+
+ // Retain the derived type when setting generic geometry options.
+ DecorativeCylinder& setBodyId(int b) {DecorativeGeometry::setBodyId(b); return *this;}
+ DecorativeCylinder& setIndexOnBody(int x) {DecorativeGeometry::setIndexOnBody(x); return *this;}
+ DecorativeCylinder& setUserRef(void* p) {DecorativeGeometry::setUserRef(p); return *this;}
+ DecorativeCylinder& setTransform(const Transform& X_BD) {DecorativeGeometry::setTransform(X_BD); return *this;}
+ DecorativeCylinder& setResolution(Real r) {DecorativeGeometry::setResolution(r); return *this;}
+ DecorativeCylinder& setScaleFactors(const Vec3& s) {DecorativeGeometry::setScaleFactors(s); return *this;}
+ DecorativeCylinder& setColor(const Vec3& rgb) {DecorativeGeometry::setColor(rgb); return *this;}
+ DecorativeCylinder& setOpacity(Real o) {DecorativeGeometry::setOpacity(o); return *this;}
+ DecorativeCylinder& setLineThickness(Real t) {DecorativeGeometry::setLineThickness(t); return *this;}
+ DecorativeCylinder& setRepresentation(const Representation& r)
+ { DecorativeGeometry::setRepresentation(r); return *this; }
+
+ SimTK_PIMPL_DOWNCAST(DecorativeCylinder, DecorativeGeometry);
+private:
+ class DecorativeCylinderRep& updRep();
+ const DecorativeCylinderRep& getRep() const;
+};
+
+/** This defines geometry to represent a coordinate frame. The default
+constructor makes three perpendicular lines beginning at the origin and
+extending in the +x, +y, and +z directions by 1 unit. **/
+class SimTK_SimTKCOMMON_EXPORT DecorativeFrame : public DecorativeGeometry {
+public:
+ explicit DecorativeFrame(Real axisLength=1);
+
+ DecorativeFrame& setAxisLength(Real);
+ Real getAxisLength() const;
+
+ // Retain the derived type when setting generic geometry options.
+ DecorativeFrame& setBodyId(int b) {DecorativeGeometry::setBodyId(b); return *this;}
+ DecorativeFrame& setIndexOnBody(int x) {DecorativeGeometry::setIndexOnBody(x); return *this;}
+ DecorativeFrame& setUserRef(void* p) {DecorativeGeometry::setUserRef(p); return *this;}
+ DecorativeFrame& setTransform(const Transform& X_BD) {DecorativeGeometry::setTransform(X_BD); return *this;}
+ DecorativeFrame& setResolution(Real r) {DecorativeGeometry::setResolution(r); return *this;}
+ DecorativeFrame& setScaleFactors(const Vec3& s) {DecorativeGeometry::setScaleFactors(s); return *this;}
+ DecorativeFrame& setColor(const Vec3& rgb) {DecorativeGeometry::setColor(rgb); return *this;}
+ DecorativeFrame& setOpacity(Real o) {DecorativeGeometry::setOpacity(o); return *this;}
+ DecorativeFrame& setLineThickness(Real t) {DecorativeGeometry::setLineThickness(t); return *this;}
+ DecorativeFrame& setRepresentation(const Representation& r)
+ { DecorativeGeometry::setRepresentation(r); return *this; }
+
+ SimTK_PIMPL_DOWNCAST(DecorativeFrame, DecorativeGeometry);
+private:
+ class DecorativeFrameRep& updRep();
+ const DecorativeFrameRep& getRep() const;
+};
+
+/** This defines a text label with its base at the origin. The default
+constructor creates a blank label. **/
+class SimTK_SimTKCOMMON_EXPORT DecorativeText : public DecorativeGeometry {
+public:
+ explicit DecorativeText(const std::string& label="");
+
+ DecorativeText& setText(const std::string& label);
+ const std::string& getText() const;
+
+ /** By default the text is part of the scene; set this flag if you want
+ it to just show up in a fixed spot on the screen instead. **/
+ DecorativeText& setIsScreenText(bool isScreen);
+ bool getIsScreenText() const;
+
+ // Retain the derived type when setting generic geometry options.
+ DecorativeText& setBodyId(int b) {DecorativeGeometry::setBodyId(b); return *this;}
+ DecorativeText& setIndexOnBody(int x) {DecorativeGeometry::setIndexOnBody(x); return *this;}
+ DecorativeText& setUserRef(void* p) {DecorativeGeometry::setUserRef(p); return *this;}
+ DecorativeText& setTransform(const Transform& X_BD) {DecorativeGeometry::setTransform(X_BD); return *this;}
+ DecorativeText& setResolution(Real r) {DecorativeGeometry::setResolution(r); return *this;}
+ DecorativeText& setScaleFactors(const Vec3& s) {DecorativeGeometry::setScaleFactors(s); return *this;}
+ DecorativeText& setColor(const Vec3& rgb) {DecorativeGeometry::setColor(rgb); return *this;}
+ DecorativeText& setOpacity(Real o) {DecorativeGeometry::setOpacity(o); return *this;}
+ DecorativeText& setLineThickness(Real t) {DecorativeGeometry::setLineThickness(t); return *this;}
+ DecorativeText& setRepresentation(const Representation& r)
+ { DecorativeGeometry::setRepresentation(r); return *this; }
+
+ SimTK_PIMPL_DOWNCAST(DecorativeText, DecorativeGeometry);
+private:
+ class DecorativeTextRep& updRep();
+ const DecorativeTextRep& getRep() const;
+};
+
+/** This defines a displayable mesh by referencing an already-existing
+PolygonalMesh object. **/
+class SimTK_SimTKCOMMON_EXPORT DecorativeMesh : public DecorativeGeometry {
+public:
+ explicit DecorativeMesh(const PolygonalMesh& mesh);
+ const PolygonalMesh& getMesh() const;
+
+ // Retain the derived type when setting generic geometry options.
+ DecorativeMesh& setBodyId(int b) {DecorativeGeometry::setBodyId(b); return *this;}
+ DecorativeMesh& setIndexOnBody(int x) {DecorativeGeometry::setIndexOnBody(x); return *this;}
+ DecorativeMesh& setUserRef(void* p) {DecorativeGeometry::setUserRef(p); return *this;}
+ DecorativeMesh& setTransform(const Transform& X_BD) {DecorativeGeometry::setTransform(X_BD); return *this;}
+ DecorativeMesh& setResolution(Real r) {DecorativeGeometry::setResolution(r); return *this;}
+ DecorativeMesh& setScaleFactors(const Vec3& s) {DecorativeGeometry::setScaleFactors(s); return *this;}
+ DecorativeMesh& setColor(const Vec3& rgb) {DecorativeGeometry::setColor(rgb); return *this;}
+ DecorativeMesh& setOpacity(Real o) {DecorativeGeometry::setOpacity(o); return *this;}
+ DecorativeMesh& setLineThickness(Real t) {DecorativeGeometry::setLineThickness(t); return *this;}
+ DecorativeMesh& setRepresentation(const Representation& r)
+ { DecorativeGeometry::setRepresentation(r); return *this; }
+
+ SimTK_PIMPL_DOWNCAST(DecorativeMesh, DecorativeGeometry);
+private:
+ class DecorativeMeshRep& updRep();
+ const DecorativeMeshRep& getRep() const;
+};
+
+
+/** This defines a displayable mesh by referencing a file name containing the mesh. If format is not supported
+ by visualizer it will be ignored. . **/
+class SimTK_SimTKCOMMON_EXPORT DecorativeMeshFile : public DecorativeGeometry {
+public:
+ explicit DecorativeMeshFile(const std::string& meshFileName);
+ const std::string& getMeshFile() const;
+
+ // Retain the derived type when setting generic geometry options.
+ DecorativeMeshFile& setBodyId(int b) {DecorativeGeometry::setBodyId(b); return *this;}
+ DecorativeMeshFile& setIndexOnBody(int x) {DecorativeGeometry::setIndexOnBody(x); return *this;}
+ DecorativeMeshFile& setUserRef(void* p) {DecorativeGeometry::setUserRef(p); return *this;}
+ DecorativeMeshFile& setTransform(const Transform& X_BD) {DecorativeGeometry::setTransform(X_BD); return *this;}
+ DecorativeMeshFile& setResolution(Real r) {DecorativeGeometry::setResolution(r); return *this;}
+ DecorativeMeshFile& setScaleFactors(const Vec3& s) {DecorativeGeometry::setScaleFactors(s); return *this;}
+ DecorativeMeshFile& setColor(const Vec3& rgb) {DecorativeGeometry::setColor(rgb); return *this;}
+ DecorativeMeshFile& setOpacity(Real o) {DecorativeGeometry::setOpacity(o); return *this;}
+ DecorativeMeshFile& setLineThickness(Real t) {DecorativeGeometry::setLineThickness(t); return *this;}
+ DecorativeMeshFile& setRepresentation(const Representation& r)
+ { DecorativeGeometry::setRepresentation(r); return *this; }
+
+ SimTK_PIMPL_DOWNCAST(DecorativeMeshFile, DecorativeGeometry);
+private:
+ class DecorativeMeshFileRep& updRep();
+ const DecorativeMeshFileRep& getRep() const;
+};
+
+
+/** This defines a single DecorativeGeometry object that is composed of a
+collection of other DecorativeGeometry objects. Parameters set for the
+parent object serve as defaults for the contained objects, but those objects
+can override or be composed with the default values. Body id, index, and
+userRef pointer (if any) for the contained objects are overridden
+unconditionally from the %Decorations object so that implementations will see
+these as identical for each piece of the composite object. Reference frames are
+composed; scale factors, resolution, opacity, and line thickness are composed,
+with unspecified values (-1) being treated as 1. **/
+class SimTK_SimTKCOMMON_EXPORT Decorations : public DecorativeGeometry {
+public:
+ /** Construct an empty container for DecorativeGeometry objects. **/
+ Decorations();
+ /** Construct a Decorations container initially consting of just a single
+ DecorativeGeometry object. **/
+ explicit Decorations(const DecorativeGeometry& decoration);
+ /** Add a DecorativeGeometry object to this collection. **/
+ Decorations& addDecoration(const DecorativeGeometry& decoration);
+ /** Add a DecorativeGeometry object to this collection and place it
+ relative to the Decorations frame. **/
+ Decorations& addDecoration(const Transform& placement,
+ const DecorativeGeometry& decoration);
+ /** Determine how many DecorativeGeometry objects are in this
+ collection. **/
+ int getNumDecorations() const;
+ /** Get access to one of the DecorativeGeometry objects in this
+ collection. **/
+ const DecorativeGeometry& getDecoration(int i) const;
+
+ // Retain the derived type when setting generic geometry options.
+ Decorations& setBodyId(int b) {DecorativeGeometry::setBodyId(b); return *this;}
+ Decorations& setIndexOnBody(int x) {DecorativeGeometry::setIndexOnBody(x); return *this;}
+ Decorations& setUserRef(void* p) {DecorativeGeometry::setUserRef(p); return *this;}
+ Decorations& setTransform(const Transform& X_BD) {DecorativeGeometry::setTransform(X_BD); return *this;}
+ Decorations& setResolution(Real r) {DecorativeGeometry::setResolution(r); return *this;}
+ Decorations& setScaleFactors(const Vec3& s) {DecorativeGeometry::setScaleFactors(s); return *this;}
+ Decorations& setColor(const Vec3& rgb) {DecorativeGeometry::setColor(rgb); return *this;}
+ Decorations& setOpacity(Real o) {DecorativeGeometry::setOpacity(o); return *this;}
+ Decorations& setLineThickness(Real t) {DecorativeGeometry::setLineThickness(t); return *this;}
+ Decorations& setRepresentation(const Representation& r)
+ { DecorativeGeometry::setRepresentation(r); return *this; }
+
+
+ SimTK_PIMPL_DOWNCAST(Decorations, DecorativeGeometry);
+private:
+ class DecorationsRep& updRep();
+ const DecorationsRep& getRep() const;
+};
+
+/** Use this abstract class to connect your implementation of decorative
+geometry to the implementation-independent classes above. **/
+class SimTK_SimTKCOMMON_EXPORT DecorativeGeometryImplementation {
+public:
+ virtual ~DecorativeGeometryImplementation() { }
+ virtual void implementPointGeometry( const DecorativePoint&) = 0;
+ virtual void implementLineGeometry( const DecorativeLine&) = 0;
+ virtual void implementBrickGeometry( const DecorativeBrick&) = 0;
+ virtual void implementCylinderGeometry( const DecorativeCylinder&) = 0;
+ virtual void implementCircleGeometry( const DecorativeCircle&) = 0;
+ virtual void implementSphereGeometry( const DecorativeSphere&) = 0;
+ virtual void implementEllipsoidGeometry(const DecorativeEllipsoid&)= 0;
+ virtual void implementFrameGeometry( const DecorativeFrame&) = 0;
+ virtual void implementTextGeometry( const DecorativeText&) = 0;
+ virtual void implementMeshGeometry( const DecorativeMesh&) = 0;
+ virtual void implementMeshFileGeometry( const DecorativeMeshFile&) =0;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_DECORATIVE_GEOMETRY_H_
diff --git a/SimTKcommon/Geometry/include/SimTKcommon/internal/PolygonalMesh.h b/SimTKcommon/Geometry/include/SimTKcommon/internal/PolygonalMesh.h
new file mode 100644
index 0000000..ecdd570
--- /dev/null
+++ b/SimTKcommon/Geometry/include/SimTKcommon/internal/PolygonalMesh.h
@@ -0,0 +1,235 @@
+#ifndef SimTK_SimTKCOMMON_POLYGONAL_MESH_H_
+#define SimTK_SimTKCOMMON_POLYGONAL_MESH_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-13 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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/Simmatrix.h"
+#include "SimTKcommon/internal/PrivateImplementation.h"
+
+namespace SimTK {
+
+class PolygonalMesh;
+class PolygonalMeshImpl;
+
+// We only want the template instantiation to occur once. This symbol is defined
+// in the SimTKcommon compilation unit that defines the PolygonalMesh class but
+// should not be defined any other time.
+#ifndef SimTK_SIMTKCOMMON_DEFINING_POLYGONALMESH
+ extern template class PIMPLHandle<PolygonalMesh, PolygonalMeshImpl, true>;
+#endif
+
+/** This class provides a description of a mesh made of polygonal faces (not
+limited to triangles). Its primary purpose is for loading geometry from files,
+which can then be used for visualization or collision detection. For example,
+the following lines load a mesh from a Wavefront OBJ file, then create a
+DecorativeMesh from it.
+ at code
+ PolygonalMesh mesh;
+ std::ifstream file;
+ file.open("teapot.obj");
+ mesh.loadObjFile(file);
+ file.close();
+ DecorativeMesh decoration(mesh);
+ at endcode
+You can also read a polygon mesh from a VTK PolyData (.vtp) file.
+
+You can also build meshes programmatically, and some static methods are provided
+here for generating some common shapes.
+
+The mesh has its own local frame and vertex locations are given in that
+frame. You can scale and transform the vertices relative to that frame
+(changing the values stored in the mesh) but more commonly the mesh will be
+placed on a body relative to that body's frame, meaning you can re-use the
+same mesh in various places.
+
+We expect this to be a large object so give it shared (reference) semantics;
+that is, the copy constructor and copy assignment default to shallow copies
+(both handles will refer to the same data). If you want to make a deep
+(non-shared) copy of a PolygonalMesh, use the copyAssign() method provided by
+the PIMPLHandle base class. **/
+class SimTK_SimTKCOMMON_EXPORT PolygonalMesh
+: public PIMPLHandle<PolygonalMesh, PolygonalMeshImpl, true> {
+public:
+ /** Create an empty %PolygonalMesh, with no vertices or faces. **/
+ PolygonalMesh() {}
+
+ /** Create a sphere-shaped mesh, with roughly uniform mesh elements.
+
+ @param[in] radius
+ The radius of the underlying sphere. Vertices of the mesh will be on
+ the sphere, with mesh elements somewhat inside.
+ @param[in] resolution
+ Control for how dense a mesh to produce. Resolution 0 will produce an
+ octahedron (8 triangular faces). Resolution 1 (the default) gives
+ 32 faces, resolution 2 gives 128. In general for resolution n there
+ will be 2*4^(n+1) faces.
+ @return A %PolygonalMesh representing a sphere of the specified radius. **/
+ static PolygonalMesh createSphereMesh(Real radius,
+ int resolution = 1);
+
+ /** Create a brick-shaped mesh. A brick is a rectangular solid (a box)
+ centered at and aligned with the mesh local frame. Note that its size is
+ given with \e half dimensions. By default you will just get two mesh faces
+ along the longest edge of the brick, with all other edges roughly the same
+ size. You can control the mesh density with the \a resolution parameter.
+
+ @param[in] halfDims
+ The half-dimensions of the brick. The extreme vertices are at
+ -halfDims and +halfDims, so the brick is centered around the mesh
+ local frame.
+ @param[in] resolution
+ Control for how dense a mesh to produce. For this shape, \a resolution
+ is interpreted as the number of extra vertices to insert in the
+ \e longest edge of the brick. Extra vertices are inserted into the
+ shorter edges if needed to keep the edge lengths approximately
+ uniform for every mesh face. \a resolution=0 gives only
+ vertices at the corners; the default is 1 meaning that the longest
+ edge is split once.
+ @return A %PolygonalMesh representing a brick of the requested size.
+
+ <h3>Controlling the mesh density:</h3>
+ If you want a brick mesh where all the edges in the mesh are roughly the
+ same length, say \c wantEdgeLength, set \a resolution like this:
+ @code
+ Real wantEdgeLength = ...;
+ Vec3 halfDims = ...;
+ int resolution = (int)(max(halfDims)/wantEdgeLength + 0.5);
+ @endcode
+
+ If you want a brick mesh where all the edges are roughly the same length
+ as the shortest edge of the brick, just set
+ <code>wantEdgeLength=min(halfDims)</code> in the above calculation. **/
+ static PolygonalMesh createBrickMesh(const Vec3& halfDims,
+ int resolution = 1);
+
+ /** Create a cylinder-shaped mesh, with the long axis in a given
+ direction. By default you'll get a 12 sided polygon as the base and
+ elements of roughly similar dimension along the edges. You can control the
+ mesh density with the \a resolution parameter.
+
+ @param[in] axis
+ The central axis direction of the cylinder, in the mesh local frame.
+ This can be provided using the constants XAxis, YAxis, or ZAxis, or
+ you can provide a unit vector in any direction.
+ @param[in] radius
+ The cylinder radius.
+ @param[in] halfLength
+ Half the length of the cylinder along its axis. The bases are at
+ -halfLength and +halfLength along the \a axis, so the cylinder is
+ centered around the mesh local frame origin.
+ @param[in] resolution
+ Control for how dense a mesh to produce (see below for details).
+ @return A %PolygonalMesh representing a cylinder of the requested dimensions
+ and orientation.
+
+ <h3>Controlling the mesh density:</h3>
+ At resolution 0 the base is a hexagon with six triangular faces, and the
+ tube is meshed with quad faces that are about as long
+ as the diameter of the base. Resolution 1 (the default) makes the base
+ a 12-sided polygon and introduces an intermediate 12-sided polygon of
+ have the diameter. There will be triangles in the center still, but
+ quad faces between the polygons. The length of the tube faces will be
+ reduced to match. Higher resolutions refine the mesh similarly. **/
+ static PolygonalMesh createCylinderMesh(const UnitVec3& axis,
+ Real radius,
+ Real halfLength,
+ int resolution=1);
+
+ /** Restore this %PolygonalMesh to its default-constructed state, meaning
+ that it will contain no vertices or faces after this call. **/
+ void clear();
+
+ /** Get the number of faces in the mesh. **/
+ int getNumFaces() const;
+ /** Get the number of vertices in the mesh. **/
+ int getNumVertices() const;
+
+ /** Get the position of a vertex in the mesh.
+ @param[in] vertex The index of the vertex (as returned by addVertex()).
+ @return The position of the specified vertex, measured and expressed in
+ the mesh local frame. **/
+ const Vec3& getVertexPosition(int vertex) const;
+ /** Get the number of vertices that make up a particular face.
+ @param[in] face The index of the face (as returned by addFace()). **/
+ int getNumVerticesForFace(int face) const;
+ /** Get the index of one of the vertices of a face.
+ @param[in] face The index of the face (as returned by addFace()).
+ @param[in] vertex The index of the vertex within the face (from 0, 1, or 2
+ for a triangular face, etc.) These are ordered the same
+ way as when the face was defined.
+ @return The index of the specified vertex. **/
+ int getFaceVertex(int face, int vertex) const;
+
+ /** Add a vertex to the mesh.
+ @param[in] position The position of the vertex to add, measured and
+ expressed in the mesh local frame.
+ @return The index of the newly added vertex. **/
+ int addVertex(const Vec3& position);
+
+ /** Add a face to the mesh. Note that the ordering of the vertices defines
+ the outward normal for the face; they must be counterclockwise around the
+ desired normal.
+
+ @param[in] vertices Indices of the vertices which make up the new face,
+ in counterclockwise order with respect to the face
+ normal.
+ @return The index of the newly added face. **/
+ int addFace(const Array_<int>& vertices);
+
+ /** Scale a mesh by multiplying every vertex by a fixed value. Note that
+ this permanently modifies the vertex locations within the mesh. Since the
+ vertices are measured in the mesh local frame, scaling will appear to
+ occur around the mesh origin (that is, the origin will remain where it
+ was while everything else changes.
+ @param[in] scale The scale factor. Can be any value except zero.
+ @return A reference to this now-scaled mesh object. **/
+ PolygonalMesh& scaleMesh(Real scale);
+
+ /** %Transform a mesh by applying the given Transform to every vertex,
+ leaving the mesh permanently changed. This has the effect of replacing the
+ mesh local frame M with a new frame A.
+ @param[in] X_AM The transform giving the pose of the mesh local frame in
+ the new frame A. Every vertex v_M becomes v_A=X_AM*v_M.
+ @return A reference to this now-transformed mesh object. **/
+ PolygonalMesh& transformMesh(const Transform& X_AM);
+
+ /** Load a Wavefront OBJ file, adding the vertices and faces it contains
+ to this mesh.
+ @param[in,out] file An input stream from which to load the file
+ contents. **/
+ void loadObjFile(std::istream& file);
+
+ /** Load a VTK PolyData (.vtp) file, adding the vertices and faces it
+ contains to this mesh.
+ @param[in] pathname The name of a .vtp file. **/
+ void loadVtpFile(const String& pathname);
+
+private:
+ explicit PolygonalMesh(PolygonalMeshImpl* impl) : HandleBase(impl) {}
+ void initializeHandleIfEmpty();
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_POLYGONAL_MESH_H_
diff --git a/SimTKcommon/Geometry/src/DecorativeGeometry.cpp b/SimTKcommon/Geometry/src/DecorativeGeometry.cpp
new file mode 100644
index 0000000..eda0ee2
--- /dev/null
+++ b/SimTKcommon/Geometry/src/DecorativeGeometry.cpp
@@ -0,0 +1,428 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Jack Middleton, 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/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/DecorativeGeometry.h"
+
+#include "DecorativeGeometryRep.h"
+
+#include <cmath>
+
+namespace SimTK {
+
+// Some common RGB values; these constants are global external symbols exported
+// by the library, defined as externs in DecorativeGeometry.h.
+
+SimTK_SimTKCOMMON_EXPORT const Vec3 Black = Vec3( 0, 0, 0);
+SimTK_SimTKCOMMON_EXPORT const Vec3 Gray = Vec3(.5,.5,.5);
+SimTK_SimTKCOMMON_EXPORT const Vec3 Red = Vec3( 1, 0, 0);
+SimTK_SimTKCOMMON_EXPORT const Vec3 Green = Vec3( 0, 1, 0);
+SimTK_SimTKCOMMON_EXPORT const Vec3 Blue = Vec3( 0, 0, 1);
+SimTK_SimTKCOMMON_EXPORT const Vec3 Yellow = Vec3( 1, 1, 0);
+SimTK_SimTKCOMMON_EXPORT const Vec3 Orange = Vec3( 1,.5, 0);
+SimTK_SimTKCOMMON_EXPORT const Vec3 Magenta = Vec3( 1, 0, 1);
+SimTK_SimTKCOMMON_EXPORT const Vec3 Purple = Vec3(.5, 0,.5);
+SimTK_SimTKCOMMON_EXPORT const Vec3 Cyan = Vec3( 0, 1, 1);
+SimTK_SimTKCOMMON_EXPORT const Vec3 White = Vec3( 1, 1, 1);
+
+ /////////////////////////
+ // DECORATIVE GEOMETRY //
+ /////////////////////////
+
+// This is an owner handle if there is no rep or if the rep points back
+// to this handle.
+bool DecorativeGeometry::isOwnerHandle() const {
+ return rep==0 || rep->myHandle == this;
+}
+bool DecorativeGeometry::isEmptyHandle() const {return rep==0;}
+
+DecorativeGeometry::~DecorativeGeometry() {
+ if (isOwnerHandle())
+ delete rep;
+ rep = 0;
+}
+
+DecorativeGeometry::DecorativeGeometry(const DecorativeGeometry& src) : rep(0) {
+ if (src.rep) {
+ rep = src.rep->clone();
+ rep->setMyHandle(*this);
+ }
+}
+
+DecorativeGeometry& DecorativeGeometry::operator=(const DecorativeGeometry& src) {
+ if (&src != this) {
+ if (isOwnerHandle()) delete rep;
+ rep = 0;
+ if (src.rep) {
+ rep = src.rep->clone();
+ rep->setMyHandle(*this);
+ }
+ }
+ return *this;
+}
+
+DecorativeGeometry& DecorativeGeometry::setBodyId(int b) {updRep().setBodyId(b);return *this;}
+int DecorativeGeometry::getBodyId() const {return getRep().getBodyId();}
+DecorativeGeometry& DecorativeGeometry::setIndexOnBody(int x) {updRep().setIndexOnBody(x);return *this;}
+int DecorativeGeometry::getIndexOnBody() const {return getRep().getIndexOnBody();}
+DecorativeGeometry& DecorativeGeometry::setUserRef(void* p) {updRep().setUserRef(p);return *this;}
+void* DecorativeGeometry::getUserRef() const {return getRep().getUserRef();}
+
+DecorativeGeometry& DecorativeGeometry::setTransform(const Transform& X_BD) {updRep().setTransform(X_BD);return *this;}
+const Transform& DecorativeGeometry::getTransform() const {return getRep().getTransform();}
+
+DecorativeGeometry& DecorativeGeometry::setResolution(Real r) {updRep().setResolution(r);return *this;}
+Real DecorativeGeometry::getResolution() const {return getRep().getResolution();}
+
+DecorativeGeometry& DecorativeGeometry::setScaleFactors(const Vec3& s)
+{ updRep().setScaleFactors(s); return *this; }
+const Vec3& DecorativeGeometry::getScaleFactors() const
+{ return getRep().getScaleFactors(); }
+
+DecorativeGeometry& DecorativeGeometry::setColor(const Vec3& rgb) {updRep().setColor(rgb);return *this;}
+const Vec3& DecorativeGeometry::getColor() const {return getRep().getColor();}
+
+DecorativeGeometry& DecorativeGeometry::setOpacity(Real o) {updRep().setOpacity(o);return *this;}
+Real DecorativeGeometry::getOpacity() const {return getRep().getOpacity();}
+
+DecorativeGeometry& DecorativeGeometry::setLineThickness(Real t) {updRep().setLineThickness(t);return *this;}
+Real DecorativeGeometry::getLineThickness() const {return getRep().getLineThickness();}
+
+DecorativeGeometry& DecorativeGeometry::setRepresentation(const DecorativeGeometry::Representation& r) {
+ updRep().setRepresentation(r);return *this;
+}
+
+DecorativeGeometry& DecorativeGeometry::setFaceCamera(int shouldFace)
+{ updRep().setFaceCamera(shouldFace);return *this; }
+int DecorativeGeometry::getFaceCamera() const
+{ return getRep().getFaceCamera(); }
+
+DecorativeGeometry::Representation
+DecorativeGeometry::getRepresentation() const {return getRep().getRepresentation();}
+
+void DecorativeGeometry::implementGeometry(DecorativeGeometryImplementation& geometry) const
+{
+ getRep().implementGeometry(geometry);
+}
+
+
+ //////////////////////
+ // DECORATIVE POINT //
+ //////////////////////
+
+/*static*/ bool
+DecorativePoint::isInstanceOf(const DecorativeGeometry& s) {
+ return DecorativePointRep::isA(s.getRep());
+}
+/*static*/ const DecorativePoint&
+DecorativePoint::downcast(const DecorativeGeometry& s) {
+ assert(isInstanceOf(s));
+ return static_cast<const DecorativePoint&>(s);
+}
+/*static*/ DecorativePoint&
+DecorativePoint::updDowncast(DecorativeGeometry& s) {
+ assert(isInstanceOf(s));
+ return static_cast<DecorativePoint&>(s);
+}
+
+const DecorativePointRep&
+DecorativePoint::getRep() const {
+ return SimTK_DYNAMIC_CAST_DEBUG<const DecorativePointRep&>(*rep);
+}
+DecorativePointRep&
+DecorativePoint::updRep() {
+ return SimTK_DYNAMIC_CAST_DEBUG<DecorativePointRep&>(*rep);
+}
+
+DecorativePoint::DecorativePoint(const Vec3& p) {
+ rep = new DecorativePointRep(p);
+ rep->setMyHandle(*this);
+}
+DecorativePoint& DecorativePoint::setPoint(const Vec3& p) {
+ DecorativePointRep::downcast(*rep).setPoint(p); return *this;
+}
+const Vec3& DecorativePoint::getPoint() const {
+ return DecorativePointRep::downcast(*rep).getPoint();
+}
+
+
+ /////////////////////
+ // DECORATIVE LINE //
+ /////////////////////
+
+/*static*/ bool
+DecorativeLine::isInstanceOf(const DecorativeGeometry& s) {
+ return DecorativeLineRep::isA(s.getRep());
+}
+/*static*/ const DecorativeLine&
+DecorativeLine::downcast(const DecorativeGeometry& s) {
+ assert(isInstanceOf(s));
+ return static_cast<const DecorativeLine&>(s);
+}
+/*static*/ DecorativeLine&
+DecorativeLine::updDowncast(DecorativeGeometry& s) {
+ assert(isInstanceOf(s));
+ return static_cast<DecorativeLine&>(s);
+}
+
+const DecorativeLineRep&
+DecorativeLine::getRep() const {
+ return SimTK_DYNAMIC_CAST_DEBUG<const DecorativeLineRep&>(*rep);
+}
+DecorativeLineRep&
+DecorativeLine::updRep() {
+ return SimTK_DYNAMIC_CAST_DEBUG<DecorativeLineRep&>(*rep);
+}
+
+DecorativeLine::DecorativeLine(const Vec3& p1, const Vec3& p2) {
+ rep = new DecorativeLineRep(p1,p2);
+ rep->setMyHandle(*this);
+}
+DecorativeLine& DecorativeLine::setPoint1(const Vec3& p1) {
+ DecorativeLineRep::downcast(*rep).setPoint1(p1); return *this;
+}
+DecorativeLine& DecorativeLine::setPoint2(const Vec3& p2) {
+ DecorativeLineRep::downcast(*rep).setPoint2(p2); return *this;
+}
+DecorativeLine& DecorativeLine::setEndpoints(const Vec3& p1, const Vec3& p2) {
+ DecorativeLineRep::downcast(*rep).setEndpoints(p1,p2); return *this;
+}
+const Vec3& DecorativeLine::getPoint1() const {
+ return DecorativeLineRep::downcast(*rep).getPoint1();
+}
+const Vec3& DecorativeLine::getPoint2() const {
+ return DecorativeLineRep::downcast(*rep).getPoint2();
+}
+
+
+ ///////////////////////
+ // DECORATIVE CIRCLE //
+ ///////////////////////
+
+DecorativeCircle::DecorativeCircle(Real radius) {
+ rep = new DecorativeCircleRep(radius);
+ rep->setMyHandle(*this);
+}
+DecorativeCircle& DecorativeCircle::setRadius(Real r) {
+ DecorativeCircleRep::downcast(*rep).setRadius(r); return *this;
+}
+Real DecorativeCircle::getRadius() const {
+ return DecorativeCircleRep::downcast(*rep).getRadius();
+}
+
+
+ ///////////////////////
+ // DECORATIVE SPHERE //
+ ///////////////////////
+
+DecorativeSphere::DecorativeSphere(Real radius) {
+ rep = new DecorativeSphereRep(radius);
+ rep->setMyHandle(*this);
+}
+
+DecorativeSphere& DecorativeSphere::setRadius(Real r) {
+ DecorativeSphereRep::downcast(*rep).setRadius(r); return *this;
+}
+Real DecorativeSphere::getRadius() const {
+ return DecorativeSphereRep::downcast(*rep).getRadius();
+}
+
+
+ //////////////////////////
+ // DECORATIVE ELLIPSOID //
+ //////////////////////////
+
+DecorativeEllipsoid::DecorativeEllipsoid(const Vec3& radii) {
+ rep = new DecorativeEllipsoidRep(radii);
+ rep->setMyHandle(*this);
+}
+
+DecorativeEllipsoid& DecorativeEllipsoid::setRadii(const Vec3& r) {
+ DecorativeEllipsoidRep::downcast(*rep).setRadii(r); return *this;
+}
+const Vec3& DecorativeEllipsoid::getRadii() const {
+ return DecorativeEllipsoidRep::downcast(*rep).getRadii();
+}
+ //////////////////////
+ // DECORATIVE BRICK //
+ //////////////////////
+
+DecorativeBrick::DecorativeBrick(const Vec3& xyzHalfLengths) {
+ rep = new DecorativeBrickRep(xyzHalfLengths);
+ rep->setMyHandle(*this);
+}
+
+DecorativeBrick& DecorativeBrick::setHalfLengths(const Vec3& xyzHalfLengths) {
+ DecorativeBrickRep::downcast(*rep).setHalfLengths(xyzHalfLengths);
+ return *this;
+}
+const Vec3& DecorativeBrick::getHalfLengths() const {
+ return DecorativeBrickRep::downcast(*rep).getHalfLengths();
+}
+
+ /////////////////////////
+ // DECORATIVE CYLINDER //
+ /////////////////////////
+
+DecorativeCylinder::DecorativeCylinder(Real radius, Real halfHeight) {
+ rep = new DecorativeCylinderRep(radius,halfHeight);
+ rep->setMyHandle(*this);
+}
+
+DecorativeCylinder& DecorativeCylinder::setRadius(Real r) {
+ DecorativeCylinderRep::downcast(*rep).setRadius(r); return *this;
+}
+DecorativeCylinder& DecorativeCylinder::setHalfHeight(Real r) {
+ DecorativeCylinderRep::downcast(*rep).setHalfHeight(r); return *this;
+}
+
+Real DecorativeCylinder::getRadius() const {
+ return DecorativeCylinderRep::downcast(*rep).getRadius();
+}
+Real DecorativeCylinder::getHalfHeight() const {
+ return DecorativeCylinderRep::downcast(*rep).getHalfHeight();
+}
+
+ //////////////////////
+ // DECORATIVE FRAME //
+ //////////////////////
+
+DecorativeFrame::DecorativeFrame(Real axisLength) {
+ rep = new DecorativeFrameRep(axisLength);
+ rep->setMyHandle(*this);
+}
+
+DecorativeFrame& DecorativeFrame::setAxisLength(Real l) {
+ DecorativeFrameRep::downcast(*rep).setAxisLength(l); return *this;
+}
+Real DecorativeFrame::getAxisLength() const {
+ return DecorativeFrameRep::downcast(*rep).getAxisLength();
+}
+
+
+ /////////////////////
+ // DECORATIVE TEXT //
+ /////////////////////
+
+DecorativeText::DecorativeText(const std::string& label) {
+ rep = new DecorativeTextRep(label);
+ rep->setMyHandle(*this);
+}
+
+DecorativeText& DecorativeText::setText(const std::string& label) {
+ DecorativeTextRep::downcast(*rep).setText(label); return *this;
+}
+const std::string& DecorativeText::getText() const {
+return DecorativeTextRep::downcast(*rep).getText();
+}
+
+DecorativeText& DecorativeText::setIsScreenText(bool isScreen) {
+ DecorativeTextRep::downcast(*rep).setIsScreenText(isScreen);
+ return *this;
+}
+bool DecorativeText::getIsScreenText() const {
+return DecorativeTextRep::downcast(*rep).getIsScreenText();
+}
+
+ /////////////////////
+ // DECORATIVE MESH //
+ /////////////////////
+
+DecorativeMesh::DecorativeMesh(const PolygonalMesh& mesh) {
+ rep = new DecorativeMeshRep(mesh);
+ rep->setMyHandle(*this);
+}
+const PolygonalMesh& DecorativeMesh::getMesh() const {
+ return DecorativeMeshRep::downcast(*rep).getMesh();
+}
+
+
+ /////////////////////
+ // DECORATIVE MESHFILE //
+ /////////////////////
+
+DecorativeMeshFile::DecorativeMeshFile(const std::string& meshFile) {
+ rep = new DecorativeMeshFileRep(meshFile);
+ rep->setMyHandle(*this);
+}
+const std::string& DecorativeMeshFile::getMeshFile() const {
+ return DecorativeMeshFileRep::downcast(*rep).getMeshFile();
+}
+
+
+ /////////////////
+ // DECORATIONS //
+ /////////////////
+
+/*static*/ bool
+Decorations::isInstanceOf(const DecorativeGeometry& s) {
+ return DecorationsRep::isA(s.getRep());
+}
+/*static*/ const Decorations&
+Decorations::downcast(const DecorativeGeometry& s) {
+ assert(isInstanceOf(s));
+ return static_cast<const Decorations&>(s);
+}
+/*static*/ Decorations&
+Decorations::updDowncast(DecorativeGeometry& s) {
+ assert(isInstanceOf(s));
+ return static_cast<Decorations&>(s);
+}
+
+const DecorationsRep&
+Decorations::getRep() const {
+ return SimTK_DYNAMIC_CAST_DEBUG<const DecorationsRep&>(*rep);
+}
+DecorationsRep&
+Decorations::updRep() {
+ return SimTK_DYNAMIC_CAST_DEBUG<DecorationsRep&>(*rep);
+}
+
+Decorations::Decorations() {
+ rep = new DecorationsRep();
+ rep->setMyHandle(*this);
+}
+Decorations::Decorations(const DecorativeGeometry& decoration) {
+ rep = new DecorationsRep();
+ rep->setMyHandle(*this);
+ updRep().addDecoration(decoration);
+}
+Decorations& Decorations::
+addDecoration(const DecorativeGeometry& decoration) {
+ updRep().addDecoration(decoration);
+ return *this;
+}
+Decorations& Decorations::
+addDecoration(const Transform& placement,
+ const DecorativeGeometry& decoration) {
+ updRep().addDecoration(placement, decoration);
+ return *this;
+}
+int Decorations::getNumDecorations() const
+{ return getRep().getNumDecorations(); }
+const DecorativeGeometry& Decorations::getDecoration(int i) const
+{ return getRep().getDecoration(i); }
+
+} // namespace SimTK
+
diff --git a/SimTKcommon/Geometry/src/DecorativeGeometryRep.h b/SimTKcommon/Geometry/src/DecorativeGeometryRep.h
new file mode 100644
index 0000000..b769828
--- /dev/null
+++ b/SimTKcommon/Geometry/src/DecorativeGeometryRep.h
@@ -0,0 +1,642 @@
+#ifndef SimTK_SimTKCOMMON_DECORATIVE_GEOMETRY_REP_H_
+#define SimTK_SimTKCOMMON_DECORATIVE_GEOMETRY_REP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Jack Middleton, Peter Eastman, Ayman Habib *
+ * *
+ * 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/Simmatrix.h"
+#include "SimTKcommon/internal/DecorativeGeometry.h"
+
+#include <cmath>
+
+namespace SimTK {
+
+class DecorativeGeometryRep {
+public:
+ DecorativeGeometryRep()
+ : myHandle(0), body(0), indexOnBody(-1), userRef(0),
+ placement(), resolution(-1),
+ scaleFactors(-1,-1,-1), colorRGB(-1,-1,-1), opacity(-1),
+ lineThickness(-1), faceCamera(-1),
+ representation(DecorativeGeometry::DrawDefault)
+ {}
+
+ virtual ~DecorativeGeometryRep() {
+ clearMyHandle();
+ }
+
+ virtual DecorativeGeometryRep* cloneDecorativeGeometryRep() const = 0;
+ virtual void implementGeometry(DecorativeGeometryImplementation&) const = 0;
+
+ void setBodyId(int b) {body = b;}
+ int getBodyId() const {return body;}
+ void setIndexOnBody(int x) {indexOnBody = x;}
+ int getIndexOnBody() const {return indexOnBody;}
+ void setUserRef(void* p) {userRef = p;}
+ void* getUserRef() const {return userRef;}
+
+ void setTransform(const Transform& X_BD) {placement = X_BD;}
+ const Transform& getTransform() const {return placement;}
+
+ // This sets resolution to some factor times the object-specific default.
+ // Anything 0 or less becomes -1 and means "use default".
+ void setResolution(Real r) {
+ resolution = r > 0 ? r : Real(-1);
+ }
+
+ Real getResolution() const {return resolution;}
+
+ // This sets the scale to some factor times the default size of the object,
+ // which will be somewhere around 1 length unit. Set to 0 or less to mean
+ // "use default".
+ void setScaleFactors(const Vec3& s) {
+ for (int i=0; i<3; ++i)
+ scaleFactors[i] = s[i] > 0 ? s[i] : Real(-1);
+ }
+ const Vec3& getScaleFactors() const {return scaleFactors;}
+
+
+ void setColor(const Vec3& rgb) {
+ assert(0<=rgb[0]&&rgb[0]<=1); // TODO
+ assert(0<=rgb[1]&&rgb[1]<=1);
+ assert(0<=rgb[2]&&rgb[2]<=1);
+ colorRGB=rgb;
+ }
+ const Vec3& getColor() const {return colorRGB;}
+
+ // Opacity should be greater than zero (invisible) and less than
+ // or equal to 1. The default will generally be 1, which is opaque,
+ // but we use -1 to mean "use default" and let the client decide.
+ void setOpacity(Real o) {
+ opacity = o > 0 ? o : Real(-1);
+ }
+ Real getOpacity() const {return opacity;}
+
+ void setLineThickness(Real t) {
+ lineThickness = t > 0 ? t : Real(-1);
+ }
+ Real getLineThickness() const {return lineThickness;}
+
+ void setFaceCamera(int shouldFace) {
+ faceCamera = shouldFace==0 ? 0 : (shouldFace>0 ? 1 : -1);
+ }
+
+ int getFaceCamera() const {return faceCamera;}
+
+ void setRepresentation(const DecorativeGeometry::Representation& r)
+ { representation=r; }
+ DecorativeGeometry::Representation getRepresentation() const
+ { return representation; }
+
+ DecorativeGeometryRep* clone() const {
+ DecorativeGeometryRep* dup = cloneDecorativeGeometryRep();
+ dup->clearMyHandle();
+ return dup;
+ }
+
+ void setMyHandle(DecorativeGeometry& h) {myHandle = &h;}
+ void clearMyHandle() {myHandle=0;}
+
+ // Used by the composite Decorations object:
+ // Set any properties that still have their default values to the ones
+ // from src. Source body, index, and userRef are transferred
+ // unconditionally; source placement is composed with this one
+ // unconditionally. Scale factors, resolution, opacity, and line thickness
+ // are composed, with a default (-1) value treated as 1.
+ void inheritPropertiesFrom(const DecorativeGeometryRep& srep) {
+ body = srep.body;
+ indexOnBody = srep.indexOnBody;
+ userRef = srep.userRef;
+
+ placement = srep.placement * placement;
+
+ // These are multiplied together if both are valid, otherwise we
+ // take the valid one, or set to -1 if neither is valid.
+ for (int i=0; i<3; ++i)
+ scaleFactors[i] = compose(scaleFactors[i], srep.scaleFactors[i]);
+ resolution = compose(resolution, srep.resolution);
+ opacity = compose(opacity, srep.opacity);
+ lineThickness = compose(lineThickness, srep.lineThickness);
+
+ // These are left alone if already specified in the destination,
+ // otherwise they are given the source value.
+ if (colorRGB[0] == -1) colorRGB = srep.colorRGB;
+ if (faceCamera == -1) faceCamera = srep.faceCamera;
+ if (representation == DecorativeGeometry::DrawDefault)
+ representation = srep.representation;
+ }
+private:
+ // Given arguments where ai < 0 means it hasn't been specified,
+ // compose them if they are both valid, otherwise return the valid
+ // one if there is one, otherwise return -1. Note that we're treating
+ // 0 as specified; composing it with anything will return 0.
+ static Real compose(Real a1, Real a2) {
+ if (a1 >= 0) return a2 >= 0 ? a1*a2 : a1;
+ if (a2 >= 0) return a2;
+ return -1;
+ }
+
+protected:
+ friend class DecorativeGeometry;
+
+ int body;
+ int indexOnBody;
+ void* userRef;
+
+ Transform placement; // default is identity
+ Vec3 scaleFactors; // -1 means use default in that direction
+
+ Real resolution; // -1 means use default
+ Vec3 colorRGB; // set R to -1 for "use default"
+ Real opacity; // -1 means "use default"
+ Real lineThickness; // -1 means "use default"
+ int faceCamera;
+ DecorativeGeometry::Representation
+ representation; // e.g. points, wireframe, surface
+
+ DecorativeGeometry* myHandle; // the owner of this rep
+};
+
+ ////////////////////////
+ // DecorativePointRep //
+ ////////////////////////
+
+class DecorativePointRep : public DecorativeGeometryRep {
+public:
+ // no default constructor
+ DecorativePointRep(const Vec3& p) : point(p) {}
+
+ void setPoint(const Vec3& p) {point=p; }
+
+ const Vec3& getPoint() const {return point;}
+
+ // virtuals
+ DecorativePointRep* cloneDecorativeGeometryRep() const {
+ DecorativePointRep* DGRep = new DecorativePointRep(*this);
+ return DGRep;
+ }
+
+ void implementGeometry(DecorativeGeometryImplementation& geometry) const {
+ geometry.implementPointGeometry(getMyPointHandle());
+ }
+
+ SimTK_DOWNCAST(DecorativePointRep, DecorativeGeometryRep);
+private:
+ Vec3 point;
+
+ // This is just a static downcast since the DecorativeGeometry handle class
+ // is not virtual.
+ const DecorativePoint& getMyPointHandle() const {
+ return *static_cast<const DecorativePoint*>(myHandle);
+ }
+};
+
+ ///////////////////////
+ // DecorativeLineRep //
+ ///////////////////////
+
+class DecorativeLineRep : public DecorativeGeometryRep {
+public:
+ // no default constructor
+ DecorativeLineRep( const Vec3& p1, const Vec3& p2) : point1(p1), point2(p2) {
+ }
+
+ void setPoint1(const Vec3& p) {point1=p; }
+ void setPoint2(const Vec3& p) {point2=p; }
+ void setEndpoints(const Vec3& p1, const Vec3& p2) { point1=p1; point2=p2; }
+
+ const Vec3& getPoint1() const {return point1;}
+ const Vec3& getPoint2() const {return point2;}
+
+ // virtuals
+ DecorativeLineRep* cloneDecorativeGeometryRep() const {
+ DecorativeLineRep* DGRep = new DecorativeLineRep(*this);
+ return( DGRep );
+ }
+
+ void implementGeometry(DecorativeGeometryImplementation& geometry) const {
+ geometry.implementLineGeometry(getMyLineHandle());
+ }
+
+ SimTK_DOWNCAST(DecorativeLineRep, DecorativeGeometryRep);
+private:
+ Vec3 point1, point2;
+
+ // This is just a static downcast since the DecorativeGeometry handle class is not virtual.
+ const DecorativeLine& getMyLineHandle() const {
+ return *static_cast<const DecorativeLine*>(myHandle);
+ }
+};
+
+ /////////////////////////
+ // DecorativeCircleRep //
+ /////////////////////////
+
+class DecorativeCircleRep : public DecorativeGeometryRep {
+public:
+ // no default constructor
+ explicit DecorativeCircleRep( const Real& rad) : r(rad) {
+ assert(r > 0); // TODO
+ }
+
+ void setRadius(const Real& rad) {
+ assert(rad > 0); // TODO;
+ r = rad;
+ }
+ const Real& getRadius() const {return r;}
+
+ // virtuals
+ DecorativeCircleRep* cloneDecorativeGeometryRep() const {
+ DecorativeCircleRep* DGRep = new DecorativeCircleRep(*this);
+ return( DGRep );
+ }
+
+ void implementGeometry(DecorativeGeometryImplementation& geometry) const {
+ geometry.implementCircleGeometry(getMyCircleHandle());
+ }
+
+ SimTK_DOWNCAST(DecorativeCircleRep, DecorativeGeometryRep);
+private:
+ Real r;
+
+ // This is just a static downcast since the DecorativeGeometry handle class is not virtual.
+ const DecorativeCircle& getMyCircleHandle() const {
+ return *static_cast<const DecorativeCircle*>(myHandle);
+ }
+};
+
+ /////////////////////////
+ // DecorativeSphereRep //
+ /////////////////////////
+
+class DecorativeSphereRep : public DecorativeGeometryRep {
+ static const int DefaultResolution = 15;
+public:
+ // no default constructor
+ explicit DecorativeSphereRep( const Real& rad) : r(rad) {
+ assert(r > 0); // TODO
+ }
+
+ void setRadius(const Real& rad) {
+ assert(rad > 0); // TODO;
+ r = rad;
+ }
+ const Real& getRadius() const {return r;}
+
+ // virtuals
+ DecorativeGeometryRep* cloneDecorativeGeometryRep() const {
+ DecorativeSphereRep* DGRep = new DecorativeSphereRep(*this);
+ return( DGRep );
+ }
+
+ void implementGeometry(DecorativeGeometryImplementation& geometry) const {
+ geometry.implementSphereGeometry(getMySphereHandle());
+ }
+
+ SimTK_DOWNCAST(DecorativeSphereRep, DecorativeGeometryRep);
+private:
+ Real r;
+
+ // This is just a static downcast since the DecorativeGeometry handle class is not virtual.
+ const DecorativeSphere& getMySphereHandle() const {
+ return *static_cast<const DecorativeSphere*>(myHandle);
+ }
+};
+
+
+ ////////////////////////////
+ // DecorativeEllipsoidRep //
+ ////////////////////////////
+
+class DecorativeEllipsoidRep : public DecorativeGeometryRep {
+public:
+ // no default constructor
+ explicit DecorativeEllipsoidRep( const Vec3& xyzRadii) : radii(xyzRadii) {
+ assert(radii[0]>0&&radii[1]>0&&radii[2]>0); // TODO
+ }
+
+ void setRadii(const Vec3& r) {
+ assert(r[0]>0&&r[1]>0&&r[2]>0); // TODO;
+ radii = r;
+ }
+ const Vec3& getRadii() const {return radii;}
+
+ // virtuals
+ DecorativeEllipsoidRep* cloneDecorativeGeometryRep() const {
+ DecorativeEllipsoidRep* DGRep = new DecorativeEllipsoidRep(*this);
+ return( DGRep );
+ }
+
+ void implementGeometry(DecorativeGeometryImplementation& geometry) const {
+ geometry.implementEllipsoidGeometry(getMyEllipsoidHandle());
+ }
+
+ SimTK_DOWNCAST(DecorativeEllipsoidRep, DecorativeGeometryRep);
+private:
+ Vec3 radii;
+
+ // This is just a static downcast since the DecorativeGeometry handle class is not virtual.
+ const DecorativeEllipsoid& getMyEllipsoidHandle() const {
+ return *static_cast<const DecorativeEllipsoid*>(myHandle);
+ }
+};
+
+
+ ////////////////////////
+ // DecorativeBrickRep //
+ ////////////////////////
+
+class DecorativeBrickRep : public DecorativeGeometryRep {
+public:
+ // no default constructor
+ explicit DecorativeBrickRep( const Vec3& xyzHalfLengths) : halfLengths(xyzHalfLengths) {
+ assert(halfLengths[0]>0&&halfLengths[1]>0&&halfLengths[2]>0); // TODO
+ }
+
+ void setHalfLengths(const Vec3& hl) {
+ assert(hl[0]>0&&hl[1]>0&&hl[2]>0); // TODO;
+ halfLengths = hl;
+ }
+ const Vec3& getHalfLengths() const {return halfLengths;}
+
+ // virtuals
+ DecorativeBrickRep* cloneDecorativeGeometryRep() const {
+ DecorativeBrickRep* DGRep = new DecorativeBrickRep(*this);
+ return( DGRep );
+ }
+
+ void implementGeometry(DecorativeGeometryImplementation& geometry) const {
+ geometry.implementBrickGeometry(getMyBrickHandle());
+ }
+
+ SimTK_DOWNCAST(DecorativeBrickRep, DecorativeGeometryRep);
+private:
+ Vec3 halfLengths;
+
+ // This is just a static downcast since the DecorativeGeometry handle class is not virtual.
+ const DecorativeBrick& getMyBrickHandle() const {
+ return *static_cast<const DecorativeBrick*>(myHandle);
+ }
+};
+
+
+class DecorativeCylinderRep : public DecorativeGeometryRep {
+ static const int DefaultResolution = 10;
+public:
+ // no default constructor
+ DecorativeCylinderRep( Real r, Real hh)
+ : radius(r), halfHeight(hh) {
+ assert(radius>0&&halfHeight>0); // TODO
+ }
+
+ void setRadius(const Real& rad) {
+ assert(rad > 0); // TODO;
+ radius = rad;
+ }
+ void setHalfHeight(const Real& hh) {
+ assert(hh > 0); // TODO;
+ halfHeight = hh;
+ }
+ Real getRadius() const {return radius;}
+ Real getHalfHeight() const {return halfHeight;}
+
+ // virtuals
+ DecorativeCylinderRep* cloneDecorativeGeometryRep() const {
+ DecorativeCylinderRep* DGRep = new DecorativeCylinderRep(*this);
+ return( DGRep );
+ }
+
+ void implementGeometry(DecorativeGeometryImplementation& geometry) const {
+ geometry.implementCylinderGeometry(getMyCylinderHandle());
+ }
+
+ SimTK_DOWNCAST(DecorativeCylinderRep, DecorativeGeometryRep);
+private:
+ Real radius, halfHeight;
+
+ // This is just a static downcast since the DecorativeGeometry handle class is not virtual.
+ const DecorativeCylinder& getMyCylinderHandle() const {
+ return *static_cast<const DecorativeCylinder*>(myHandle);
+ }
+};
+
+class DecorativeFrameRep : public DecorativeGeometryRep {
+public:
+ // no default constructor
+ DecorativeFrameRep(const Real& len) : axisLength(len) {
+ assert(len > 0); // TODO
+ }
+
+ void setAxisLength(const Real& len) {
+ assert(len > 0); // TODO;
+ axisLength = len;
+ }
+ const Real& getAxisLength() const {return axisLength;}
+
+ // virtuals
+ DecorativeFrameRep* cloneDecorativeGeometryRep() const {
+ DecorativeFrameRep* DGRep = new DecorativeFrameRep(*this);
+ return( DGRep );
+ }
+
+ void implementGeometry(DecorativeGeometryImplementation& geometry) const {
+ geometry.implementFrameGeometry(getMyFrameHandle());
+ }
+
+ SimTK_DOWNCAST(DecorativeFrameRep, DecorativeGeometryRep);
+private:
+ Real axisLength;
+
+ // This is just a static downcast since the DecorativeGeometry handle class is not virtual.
+ const DecorativeFrame& getMyFrameHandle() const {
+ return *static_cast<const DecorativeFrame*>(myHandle);
+ }
+};
+
+///////////////////////
+// DecorativeTextRep //
+///////////////////////
+
+class DecorativeTextRep : public DecorativeGeometryRep {
+static const int DefaultResolution = 15;
+public:
+// no default constructor
+explicit DecorativeTextRep(const std::string& label)
+: text(label), isScreenText(false) {}
+
+void setText(const std::string& label) {
+ text = label;
+}
+const std::string& getText() const {
+ return text;
+}
+
+void setIsScreenText(bool isScreen) {isScreenText=isScreen;}
+bool getIsScreenText() const {return isScreenText;}
+
+// virtuals
+DecorativeGeometryRep* cloneDecorativeGeometryRep() const {
+ DecorativeTextRep* DGRep = new DecorativeTextRep(*this);
+ return( DGRep );
+}
+
+void implementGeometry(DecorativeGeometryImplementation& geometry) const {
+ geometry.implementTextGeometry(getMyTextHandle());
+}
+
+SimTK_DOWNCAST(DecorativeTextRep, DecorativeGeometryRep);
+private:
+std::string text;
+bool isScreenText; // in screen coordinates
+
+// This is just a static downcast since the DecorativeGeometry handle class is not virtual.
+const DecorativeText& getMyTextHandle() const {
+ return *static_cast<const DecorativeText*>(myHandle);
+}
+};
+
+///////////////////////
+// DecorativeMeshRep //
+///////////////////////
+
+class DecorativeMeshRep : public DecorativeGeometryRep {
+public:
+// no default constructor
+explicit DecorativeMeshRep(const PolygonalMesh& mesh) : mesh(mesh) {
+}
+
+const PolygonalMesh& getMesh() const {
+ return mesh;
+}
+
+// virtuals
+DecorativeGeometryRep* cloneDecorativeGeometryRep() const {
+ DecorativeMeshRep* DGRep = new DecorativeMeshRep(*this);
+ return( DGRep );
+}
+
+void implementGeometry(DecorativeGeometryImplementation& geometry) const {
+ geometry.implementMeshGeometry(getMyMeshHandle());
+}
+
+SimTK_DOWNCAST(DecorativeMeshRep, DecorativeGeometryRep);
+private:
+PolygonalMesh mesh;
+
+// This is just a static downcast since the DecorativeGeometry handle class is not virtual.
+const DecorativeMesh& getMyMeshHandle() const {
+ return *static_cast<const DecorativeMesh*>(myHandle);
+}
+};
+
+///////////////////////
+// DecorativeMeshFileRep //
+///////////////////////
+
+class DecorativeMeshFileRep : public DecorativeGeometryRep {
+public:
+// no default constructor
+explicit DecorativeMeshFileRep(const std::string& meshFileName) : meshFile(meshFileName) {
+}
+
+const std::string& getMeshFile() const {
+ return meshFile;
+}
+
+// virtuals
+DecorativeGeometryRep* cloneDecorativeGeometryRep() const {
+ DecorativeMeshFileRep* DGRep = new DecorativeMeshFileRep(*this);
+ return DGRep ;
+}
+
+void implementGeometry(DecorativeGeometryImplementation& geometry) const {
+ geometry.implementMeshFileGeometry(getMyMeshFileHandle());
+}
+
+SimTK_DOWNCAST(DecorativeMeshFileRep, DecorativeGeometryRep);
+private:
+std::string meshFile;
+
+// This is just a static downcast since the DecorativeGeometry handle class is not virtual.
+
+const DecorativeMeshFile& getMyMeshFileHandle() const {
+ return *static_cast<const DecorativeMeshFile*>(myHandle);
+}
+};
+
+ ////////////////////
+ // DecorationsRep //
+ ////////////////////
+
+class DecorationsRep : public DecorativeGeometryRep {
+public:
+ DecorationsRep() {}
+
+ int addDecoration(const DecorativeGeometry& decoration) {
+ geom.push_back(decoration);
+ return (int)(geom.size()-1);
+ }
+ int addDecoration(const Transform& X_DE,
+ const DecorativeGeometry& element) {
+ geom.push_back(element);
+ // The current transform goes from the element's frame E to the
+ // frame G of the actual geometry. We would normally put E at the
+ // Decorations frame D, but now we want to relocate it.
+ const Transform& X_EG = geom.back().getTransform();
+ geom.back().setTransform(X_DE*X_EG);
+ return (int)(geom.size()-1);
+ }
+
+ int getNumDecorations() const {return (int)geom.size();}
+ const DecorativeGeometry& getDecoration(int i) const {return geom[i];}
+
+ // virtuals
+
+ DecorationsRep* cloneDecorativeGeometryRep() const {
+ DecorationsRep* DGRep = new DecorationsRep(*this);
+ return DGRep;
+ }
+
+ void implementGeometry(DecorativeGeometryImplementation& geometry) const {
+ for (unsigned i=0; i < geom.size(); ++i) {
+ DecorativeGeometry copy = geom[i];
+ copy.updRep().inheritPropertiesFrom(*this);
+ copy.implementGeometry(geometry);
+ }
+ }
+
+ SimTK_DOWNCAST(DecorationsRep, DecorativeGeometryRep);
+private:
+ Array_<DecorativeGeometry> geom;
+
+ // This is just a static downcast since the DecorativeGeometry handle class
+ // is not virtual.
+ const Decorations& getMyHandle() const {
+ return *static_cast<const Decorations*>(myHandle);
+ }
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_DECORATIVE_GEOMETRY_REP_H_
diff --git a/SimTKcommon/Geometry/src/PolygonalMesh.cpp b/SimTKcommon/Geometry/src/PolygonalMesh.cpp
new file mode 100644
index 0000000..5da7360
--- /dev/null
+++ b/SimTKcommon/Geometry/src/PolygonalMesh.cpp
@@ -0,0 +1,710 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-13 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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 "PolygonalMeshImpl.h"
+#include "SimTKcommon/internal/Xml.h"
+
+#include <cassert>
+#include <sstream>
+#include <string>
+#include <set>
+#include <map>
+
+using namespace SimTK;
+
+//==============================================================================
+// POLYGONAL MESH
+//==============================================================================
+
+// If the handle is empty, reconstruct it to be an owner handle whose
+// implementation is present but contains no vertices.
+void PolygonalMesh::initializeHandleIfEmpty() {
+ if (isEmptyHandle())
+ new(this) PolygonalMesh(new PolygonalMeshImpl());
+}
+
+// default (shallow) copy constructor, copy assignment, destructor
+
+void PolygonalMesh::clear() {
+ if (!isEmptyHandle()) updImpl().clear();
+}
+
+int PolygonalMesh::getNumFaces() const {
+ return isEmptyHandle() ? 0 : getImpl().faceVertexStart.size()-1;
+}
+
+int PolygonalMesh::getNumVertices() const {
+ return isEmptyHandle() ? 0 : getImpl().vertices.size();
+}
+
+const Vec3& PolygonalMesh::getVertexPosition(int vertex) const {
+ assert(0 <= vertex && vertex < getNumVertices());
+ return getImpl().vertices[vertex];
+}
+
+int PolygonalMesh::getNumVerticesForFace(int face) const {
+ assert(0 <= face && face < getNumFaces());
+ const Array_<int>& faceVertexStart = getImpl().faceVertexStart;
+ return faceVertexStart[face+1]-faceVertexStart[face];
+}
+
+int PolygonalMesh::getFaceVertex(int face, int vertex) const {
+ assert(face >= 0 && face < getNumFaces());
+ assert(vertex >= 0 && vertex < getNumVerticesForFace(face));
+ return getImpl().faceVertexIndex[getImpl().faceVertexStart[face]+vertex];
+}
+
+int PolygonalMesh::addVertex(const Vec3& position) {
+ initializeHandleIfEmpty();
+ updImpl().vertices.push_back(position);
+ return getImpl().vertices.size()-1;
+}
+
+int PolygonalMesh::addFace(const Array_<int>& vertices) {
+ initializeHandleIfEmpty();
+ for (int i = 0; i < (int) vertices.size(); i++)
+ updImpl().faceVertexIndex.push_back(vertices[i]);
+
+ // faceVertexStart is preloaded to have its first element 0 before any
+ // faces have been added. So the back() element of faceVertexStart is
+ // already the starting entry for the face we're adding.
+ // This is where the *next* face will begin.
+ updImpl().faceVertexStart.push_back(getImpl().faceVertexIndex.size());
+ // The current face start is now at end()-2 (back()-1).
+ return getImpl().faceVertexStart.size()-2;
+}
+
+PolygonalMesh& PolygonalMesh::scaleMesh(Real scale) {
+ if (!isEmptyHandle()) {
+ Array_<Vec3>& vertices = updImpl().vertices;
+ for (int i = 0; i < (int) vertices.size(); i++)
+ vertices[i] *= scale;
+ }
+ return *this;
+}
+
+PolygonalMesh& PolygonalMesh::transformMesh(const Transform& X_AM) {
+ if (!isEmptyHandle()) {
+ Array_<Vec3>& vertices = updImpl().vertices;
+ for (int i = 0; i < (int) vertices.size(); i++)
+ vertices[i] = X_AM*vertices[i];
+ }
+ return *this;
+}
+
+void PolygonalMesh::loadObjFile(std::istream& file) {
+ const char* methodName = "PolygonalMesh::loadObjFile()";
+ SimTK_ERRCHK_ALWAYS(file.good(), methodName,
+ "The supplied std::istream object was not in good condition"
+ " on entrance -- did you check whether it opened successfully?");
+
+ std::string line;
+ Array_<int> indices;
+ int initialVertices = getNumVertices();
+ while (!file.eof()) {
+ SimTK_ERRCHK_ALWAYS(file.good(), methodName,
+ "An error occurred while reading the input file.");
+
+ std::getline(file, line);
+ while (line.size() > 0 && line[line.size()-1] == '\\') {
+ line[line.size()-1] = ' ';
+ std::string continuation;
+ std::getline(file, continuation);
+ line += continuation;
+ }
+ std::stringstream s(line);
+ std::string command;
+ s >> command;
+ if (command == "v") {
+ // A vertex
+
+ Real x, y, z;
+ s >> x;
+ s >> y;
+ s >> z;
+ SimTK_ERRCHK1_ALWAYS(!s.fail(), methodName,
+ "Found invalid vertex description: %s", line.c_str());
+ addVertex(Vec3(x, y, z));
+ }
+ else if (command == "f") {
+ // A face
+
+ indices.clear();
+ int index;
+ while (s >> index) {
+ s.ignore(line.size(), ' ');
+ if (index < 0)
+ index += getNumVertices()-initialVertices;
+ else
+ index--;
+ indices.push_back(index);
+ }
+ addFace(indices);
+ }
+ }
+}
+
+/* Use our XML reader to parse VTK's PolyData file format and add the polygons
+found there to whatever is currently in this PolygonalMesh object. OpenSim uses
+this format for its geometric objects.
+
+Here is a somewhat stripped down and annotated version of Kitware's description
+from vtk.org:
+
+All the metadata is case sensitive.
+
+PolyData -- Each PolyData piece specifies a set of points and cells
+independently from the other pieces. [SimTK Note: we will read in only the
+first Piece element.] The points are described explicitly by the
+Points element. The cells are described explicitly by the Verts, Lines, Strips,
+and Polys elements.
+ <VTKFile type="PolyData" ...>
+ <PolyData>
+ <Piece NumberOfPoints="#" NumberOfVerts="#" NumberOfLines="#"
+ NumberOfStrips="#" NumberOfPolys="#">
+ <PointData>...</PointData>
+ <CellData>...</CellData>
+ <Points>...</Points>
+ <Verts>...</Verts>
+ <Lines>...</Lines>
+ <Strips>...</Strips>
+ <Polys>...</Polys>
+ </Piece>
+ </PolyData>
+ </VTKFile>
+
+PointData and CellData -- Every dataset describes the data associated with
+its points and cells with PointData and CellData XML elements as follows:
+ <PointData Scalars="Temperature" Vectors="Velocity">
+ <DataArray Name="Velocity" .../>
+ <DataArray Name="Temperature" .../>
+ <DataArray Name="Pressure" .../>
+ </PointData>
+
+VTK allows an arbitrary number of data arrays to be associated with the points
+and cells of a dataset. Each data array is described by a DataArray element
+which, among other things, gives each array a name. The following attributes
+of PointData and CellData are used to specify the active arrays by name:
+ Scalars � The name of the active scalars array, if any.
+ Vectors � The name of the active vectors array, if any.
+ Normals � The name of the active normals array, if any.
+ Tensors � The name of the active tensors array, if any.
+ TCoords � The name of the active texture coordinates array, if any.
+That is, for each attribute of the form Sometype="Somename" there must be a
+DataArray element with attribute Name="Somename" whose text contains
+NumberOfPoints values each of type Sometype.
+
+Points -- The Points element explicitly defines coordinates for each point
+individually. It contains one DataArray element describing an array with
+three components per value, each specifying the coordinates of one point.
+ <Points>
+ <DataArray NumberOfComponents="3" .../>
+ </Points>
+
+Verts, Lines, Strips, and Polys -- The Verts, Lines, Strips, and Polys elements
+define cells explicitly by specifying point connectivity. Cell types are
+implicitly known by the type of element in which they are specified. Each
+element contains two DataArray elements. The first array specifies the point
+connectivity. All the cells� point lists are concatenated together. The second
+array specifies the offset into the connectivity array for the end of each
+cell.
+ <Polys>
+ <DataArray type="Int32" Name="connectivity" .../>
+ <DataArray type="Int32" Name="offsets" .../>
+ </Polys>
+(same format for Verts, Lines, and Strips)
+
+DataArray -- All of the data and geometry specifications use DataArray elements
+to describe their actual content as follows:
+
+The DataArray element stores a sequence of values of one type. There may be
+one or more components per value. [SimTK Note: there are also "binary" and
+"appended" formats which we do not support -- be sure to check that the
+format attribute for every DataArray is "ascii".]
+ <DataArray type="Int32" Name="offsets" format="ascii">
+ 10 20 30 ... </DataArray>
+
+The attributes of the DataArray elements are described as follows:
+ type -- The data type of a single component of the array. This is one of
+ Int8, UInt8, Int16, UInt16, Int32, UInt32, Int64, UInt64, Float32,
+ Float64.
+ Name -- The name of the array. This is usually a brief description of the
+ data stored in the array. [Note that the PolyData element uses the
+ DataArray Name attribute to figure out what's being provided.]
+ NumberOfComponents -- The number of components per value in the array.
+ format -- The means by which the data values themselves are stored in the
+ file. This is "ascii", "binary", or "appended". [SimTK only supports
+ "ascii".]
+ format="ascii" -- The data are listed in ASCII directly inside the
+ DataArray element. Whitespace is used for separation.
+*/
+void PolygonalMesh::loadVtpFile(const String& pathname) {
+ try
+ { const char* method = "PolygonalMesh::loadVtpFile()";
+ Xml vtp(pathname);
+ // The file has been read in and parsed into memory by the Xml system.
+
+ SimTK_ERRCHK1_ALWAYS(vtp.getRootTag() == "VTKFile", method,
+ "Expected to see document tag <VTKFile> but saw <%s> instead.",
+ vtp.getRootTag().c_str());
+ // This is a VTKFile document.
+
+ Xml::Element root = vtp.getRootElement();
+ SimTK_ERRCHK1_ALWAYS(root.getRequiredAttributeValue("type") == "PolyData",
+ method, "Expected VTK file type='PolyData' but got type='%s'.",
+ root.getRequiredAttributeValue("type").c_str());
+ // This is a VTK PolyData document.
+
+ Xml::Element polydata = root.getRequiredElement("PolyData");
+ Xml::Element piece = polydata.getRequiredElement("Piece");
+ Xml::Element points = piece.getRequiredElement("Points");
+ const int numPoints =
+ piece.getRequiredAttributeValueAs<int>("NumberOfPoints");
+ const int numPolys =
+ piece.getRequiredAttributeValueAs<int>("NumberOfPolys");
+
+ // Remember this because we'll have to use it to adjust the indices we use
+ // when referencing the vertices we're about to read in. This number is
+ // the index that our first vertex will be assigned.
+ const int firstVertex = getNumVertices();
+
+ // The lone DataArray element in the Points element contains the points'
+ // coordinates. Read it in as a Vector of Vec3s.
+ Xml::Element pointData = points.getRequiredElement("DataArray");
+ SimTK_ERRCHK1_ALWAYS(pointData.getRequiredAttributeValue("format")
+ == "ascii", method,
+ "Only format=\"ascii\" is supported for .vtp file DataArray elements,"
+ " got format=\"%s\" for Points DataArray.",
+ pointData.getRequiredAttributeValue("format").c_str());
+
+ Vector_<Vec3> coords =
+ points.getRequiredElementValueAs< Vector_<Vec3> >("DataArray");
+
+ SimTK_ERRCHK2_ALWAYS(coords.size() == numPoints, method,
+ "Expected coordinates for %d points but got %d.",
+ numPoints, coords.size());
+
+ // Now that we have the point coordinates, use them to create the vertices
+ // in our mesh.
+ for (int i=0; i < numPoints; ++i)
+ addVertex(coords[i]);
+
+ // Polys are given by a connectivity array which lists the points forming
+ // each polygon in a long unstructured list, then an offsets array, one per
+ // polygon, which gives the index+1 of the *last* connectivity entry for
+ // each polygon.
+ Xml::Element polys = piece.getRequiredElement("Polys");
+
+ // Find the connectivity and offset DataArrays.
+ Xml::Element econnectivity, eoffsets;
+ for (Xml::element_iterator p = polys.element_begin("DataArray");
+ p != polys.element_end(); ++p)
+ {
+ const String& name = p->getRequiredAttributeValue("Name");
+ SimTK_ERRCHK2_ALWAYS(p->getRequiredAttributeValue("format")
+ == "ascii", method,
+ "Only format=\"ascii\" is supported for .vtp file DataArray"
+ " elements, but format=\"%s\" for DataArray '%s'.",
+ p->getRequiredAttributeValue("format").c_str(), name.c_str());
+
+ if (name == "connectivity") econnectivity = *p;
+ else if (name == "offsets") eoffsets = *p;
+ }
+
+ SimTK_ERRCHK_ALWAYS(econnectivity.isValid() && eoffsets.isValid(), method,
+ "Expected to find a DataArray with name='connectivity' and one with"
+ " name='offsets' in the VTK PolyData file's <Polys> element but at"
+ " least one of them was missing.");
+
+ // Read in the arrays.
+ Array_<int> offsets = eoffsets.getValueAs< Array_<int> >();
+ // Size may have changed if file is bad.
+ SimTK_ERRCHK2_ALWAYS(offsets.size() == numPolys, method,
+ "The number of offsets (%d) should have matched the stated "
+ " NumberOfPolys value (%d).", offsets.size(), numPolys);
+
+ // We expect that the last entry in the offsets array is one past the
+ // end of the last polygon described in the connectivity array and hence
+ // is the size of the connectivity array.
+ const int expectedSize = numPolys ? offsets.back() : 0;
+ Array_<int> connectivity = econnectivity.getValueAs< Array_<int> >();
+
+ SimTK_ERRCHK2_ALWAYS(connectivity.size()==expectedSize, method,
+ "The connectivity array was the wrong size (%d). It should"
+ " match the last entry in the offsets array which was %d.",
+ connectivity.size(), expectedSize);
+
+ int startPoly = 0;
+ for (int i=0; i < numPolys; ++i) {
+ // Now read in the face in [startOffs,endOffs]
+ addFace(connectivity(startPoly, offsets[i]-startPoly));
+ startPoly = offsets[i]; // move to the next poly
+ }
+
+ } catch (const std::exception& e) {
+ // This will throw a new exception with an enhanced message that
+ // includes the original one.
+ SimTK_ERRCHK2_ALWAYS(!"failed", "PolygonalMesh::loadVtpFile()",
+ "Attempt to load a VTK PolyData (.vtp) file from file name"
+ " '%s' failed with message:\n %s", pathname.c_str(), e.what());
+ }
+}
+
+//------------------------------------------------------------------------------
+// CREATE SPHERE MESH
+//------------------------------------------------------------------------------
+
+// Use unnamed namespace to keep VertKey class and VertMap type private to
+// this file, as well as a few helper functions.
+namespace {
+
+ struct VertKey {
+ VertKey(const Vec3& v) : v(v) {}
+ Vec3 v;
+ bool operator<(const VertKey& other) const {
+ const Real tol = SignificantReal;
+ const Vec3 diff = v - other.v;
+ if (diff[0] < -tol) return true;
+ if (diff[0] > tol) return false;
+ if (diff[1] < -tol) return true;
+ if (diff[1] > tol) return false;
+ if (diff[2] < -tol) return true;
+ if (diff[2] > tol) return false;
+ return false; // they are numerically equal
+ }
+ };
+ typedef std::map<VertKey,int> VertMap;
+
+ /* Search a list of vertices for one close enough to this one and
+ return its index if found, otherwise add to the end. */
+ int getVertex(const Vec3& v, VertMap& vmap, Array_<Vec3>& verts) {
+ VertMap::const_iterator p = vmap.find(VertKey(v));
+ if (p != vmap.end()) return p->second;
+ const int ix = (int)verts.size();
+ verts.push_back(v);
+ vmap.insert(std::make_pair(VertKey(v),ix));
+ return ix;
+ }
+
+ /* Each face comes in as below, with vertices 0,1,2 on the surface
+ of a sphere or radius r centered at the origin. We bisect the edges to get
+ points a',b',c', then move out from the center to make points a,b,c
+ on the sphere.
+ 1
+ /\
+ / \
+ c /____\ b Then construct new triangles
+ /\ /\ [0,b,a]
+ / \ / \ [a,b,c]
+ /____\/____\ [c,2,a]
+ 2 a 0 [b,1,c]
+ */
+ void refineSphere(Real r, VertMap& vmap,
+ Array_<Vec3>& verts, Array_<int>& faces) {
+ assert(faces.size() % 3 == 0);
+ const int nVerts = faces.size(); // # face vertices on entry
+ for (int i=0; i < nVerts; i+=3) {
+ const int v0=faces[i], v1=faces[i+1], v2=faces[i+2];
+ const Vec3 a = r*UnitVec3(verts[v0]+verts[v2]);
+ const Vec3 b = r*UnitVec3(verts[v0]+verts[v1]);
+ const Vec3 c = r*UnitVec3(verts[v1]+verts[v2]);
+ const int va=getVertex(a,vmap,verts),
+ vb=getVertex(b,vmap,verts),
+ vc=getVertex(c,vmap,verts);
+ // Replace the existing face with the 0ba triangle, then add the
+ // rest. Refer to the above picture.
+ faces[i+1] = vb; faces[i+2] = va;
+ faces.push_back(va); faces.push_back(vb); faces.push_back(vc);//abc
+ faces.push_back(vc); faces.push_back(v2); faces.push_back(va);//c2a
+ faces.push_back(vb); faces.push_back(v1); faces.push_back(vc);//b1c
+ }
+ }
+
+
+ void makeOctahedralMesh(const Vec3& r, Array_<Vec3>& vertices,
+ Array_<int>& faceIndices) {
+ vertices.push_back(Vec3( r[0], 0, 0)); //0
+ vertices.push_back(Vec3(-r[0], 0, 0)); //1
+ vertices.push_back(Vec3( 0, r[1], 0)); //2
+ vertices.push_back(Vec3( 0, -r[1], 0)); //3
+ vertices.push_back(Vec3( 0, 0, r[2])); //4
+ vertices.push_back(Vec3( 0, 0, -r[2])); //5
+ int faces[8][3] = {{0, 2, 4}, {4, 2, 1}, {1, 2, 5}, {5, 2, 0},
+ {4, 3, 0}, {1, 3, 4}, {5, 3, 1}, {0, 3, 5}};
+ for (int i = 0; i < 8; i++)
+ for (int j = 0; j < 3; j++)
+ faceIndices.push_back(faces[i][j]);
+ }
+
+} // end unnamed namespace
+
+/*static*/ PolygonalMesh PolygonalMesh::
+createSphereMesh(Real radius, int resolution) {
+ SimTK_ERRCHK1_ALWAYS(radius > 0, "PolygonalMesh::createSphereMesh()",
+ "Radius %g illegal.", radius);
+ SimTK_ERRCHK1_ALWAYS(resolution >= 0, "PolygonalMesh::createSphereMesh()",
+ "Resolution %d illegal.", resolution);
+
+ Array_<Vec3> vertices;
+ Array_<int> faceIndices;
+ makeOctahedralMesh(Vec3(radius), vertices, faceIndices);
+
+ VertMap vmap;
+ for (unsigned i=0; i < vertices.size(); ++i)
+ vmap[vertices[i]] = i;
+
+ int level = resolution;
+ while (level > 0) {
+ refineSphere(radius, vmap, vertices, faceIndices);
+ --level;
+ }
+
+ PolygonalMesh sphere;
+ for (unsigned i=0; i < vertices.size(); ++i)
+ sphere.addVertex(vertices[i]);
+ for (unsigned i=0; i < faceIndices.size(); i += 3) {
+ const Array_<int> verts(&faceIndices[i], &faceIndices[i]+3);
+ sphere.addFace(verts);
+ }
+
+ return sphere; // just a shallow copy
+}
+
+
+//------------------------------------------------------------------------------
+// CREATE BRICK MESH
+//------------------------------------------------------------------------------
+// Resolution 0 is just the four corners and six quads.
+// Resolution n means divide longest edge with n extra vertices; then make the
+// other faces about the same size.
+
+// Extend the unnamed namespace with another local function.
+namespace {
+
+ // Given the number of vertices along the x,y,z edges and the three (i,j,k)
+ // coordinates of a particular vertex, return a packed representation of the
+ // (i,j,k) location that we can use as an index. Using a long long here
+ // just to avoid trouble, an int probably would have been fine.
+ long long locCode(int nv[3], int i, int j, int k)
+ { return (long long)i*nv[1]*nv[2] + (long long)j*nv[2] + (long long)k; }
+
+} // end of unnamed namespace
+
+/*static*/ PolygonalMesh PolygonalMesh::
+createBrickMesh(const Vec3& halfDims, int resolution) {
+ SimTK_ERRCHK3_ALWAYS(halfDims > 0, "PolygonalMesh::createBrickMesh()",
+ "Bad brick dimensions %g %g %g.", halfDims[0], halfDims[1], halfDims[2]);
+ SimTK_ERRCHK1_ALWAYS(resolution >= 0, "PolygonalMesh::createBrickMesh()",
+ "Resolution %d illegal.", resolution);
+
+ const Vec3 dims(2*halfDims);
+
+ PolygonalMesh brick;
+ const Real longest = max(dims);
+ const Real edgeLengthTarget = longest/(resolution+1);
+ int nv[3]; // number of vertices along each edge
+ for (int i=0; i<3; ++i)
+ nv[i] = 1 + std::max((int)(dims[i]/edgeLengthTarget + 0.49), 1);
+ const Vec3 edgeLengths(dims[0]/(nv[0]-1), dims[1]/(nv[1]-1),
+ dims[2]/(nv[2]-1));
+
+ // Add regularly-spaced vertices on the surfaces.
+ std::map<long long,int> vertLoc2Vert; // map i,j,k -> vertex number
+ for (int i=0; i < nv[0]; ++i) {
+ bool xface = (i==0 || i==nv[0]-1);
+ const Real iloc = i*edgeLengths[0] - halfDims[0];
+ for (int j=0; j < nv[1]; ++j) {
+ bool yface = (j==0 || j==nv[1]-1);
+ const Real jloc = j*edgeLengths[1] - halfDims[1];
+ for (int k=0; k < nv[2]; ++k) {
+ bool zface = (k==0 || k==nv[2]-1);
+ if (!(xface||yface||zface))
+ continue; // skip interior vertices
+ const Real kloc = k*edgeLengths[2] - halfDims[2];
+ const int vnum = brick.addVertex(Vec3(iloc,jloc,kloc));
+ vertLoc2Vert[locCode(nv,i,j,k)] = vnum;
+ }
+ }
+ }
+
+ // Add quad faces oriented with normal outwards.
+ Array_<int> face(4);
+
+ // This is the -x surface (y,z rectangle).
+ for (int j=0; j < nv[1]-1; ++j)
+ for (int k=0; k < nv[2]-1; ++k) {
+ face[0] = vertLoc2Vert[locCode(nv,0,j, k+1)];
+ face[1] = vertLoc2Vert[locCode(nv,0,j+1,k+1)];
+ face[2] = vertLoc2Vert[locCode(nv,0,j+1,k)];
+ face[3] = vertLoc2Vert[locCode(nv,0,j, k)];
+ brick.addFace(face);
+ }
+ // This is the +x surface (y,z rectangle).
+ for (int j=0; j < nv[1]-1; ++j)
+ for (int k=0; k < nv[2]-1; ++k) {
+ face[3] = vertLoc2Vert[locCode(nv,nv[0]-1,j, k+1)];
+ face[2] = vertLoc2Vert[locCode(nv,nv[0]-1,j+1,k+1)];
+ face[1] = vertLoc2Vert[locCode(nv,nv[0]-1,j+1,k)];
+ face[0] = vertLoc2Vert[locCode(nv,nv[0]-1,j, k)];
+ brick.addFace(face);
+ }
+ // This is the -y surface (x,z rectangle).
+ for (int i=0; i < nv[0]-1; ++i)
+ for (int k=0; k < nv[2]-1; ++k) {
+ face[0] = vertLoc2Vert[locCode(nv,i, 0,k)];
+ face[1] = vertLoc2Vert[locCode(nv,i+1,0,k)];
+ face[2] = vertLoc2Vert[locCode(nv,i+1,0,k+1)];
+ face[3] = vertLoc2Vert[locCode(nv,i, 0,k+1)];
+ brick.addFace(face);
+ }
+ // This is the +y surface (x,z rectangle).
+ for (int i=0; i < nv[0]-1; ++i)
+ for (int k=0; k < nv[2]-1; ++k) {
+ face[3] = vertLoc2Vert[locCode(nv,i, nv[1]-1,k)];
+ face[2] = vertLoc2Vert[locCode(nv,i+1,nv[1]-1,k)];
+ face[1] = vertLoc2Vert[locCode(nv,i+1,nv[1]-1,k+1)];
+ face[0] = vertLoc2Vert[locCode(nv,i, nv[1]-1,k+1)];
+ brick.addFace(face);
+ }
+ // This is the -z surface (x,y rectangle).
+ for (int i=0; i < nv[0]-1; ++i)
+ for (int j=0; j < nv[1]-1; ++j) {
+ face[0] = vertLoc2Vert[locCode(nv,i, j+1,0)];
+ face[1] = vertLoc2Vert[locCode(nv,i+1,j+1,0)];
+ face[2] = vertLoc2Vert[locCode(nv,i+1,j, 0)];
+ face[3] = vertLoc2Vert[locCode(nv,i, j, 0)];
+ brick.addFace(face);
+ }
+ // This is the +z surface (x,y rectangle).
+ for (int i=0; i < nv[0]-1; ++i)
+ for (int j=0; j < nv[1]-1; ++j) {
+ face[3] = vertLoc2Vert[locCode(nv,i, j+1,nv[2]-1)];
+ face[2] = vertLoc2Vert[locCode(nv,i+1,j+1,nv[2]-1)];
+ face[1] = vertLoc2Vert[locCode(nv,i+1,j, nv[2]-1)];
+ face[0] = vertLoc2Vert[locCode(nv,i, j, nv[2]-1)];
+ brick.addFace(face);
+ }
+
+ return brick; // just a shallow copy
+}
+
+
+//------------------------------------------------------------------------------
+// CREATE CYLINDER MESH
+//------------------------------------------------------------------------------
+// Minimum end surface is a hexagon (resolution 0).
+// Think of the axis as the +z axis, with the end caps in x-y planes at
+// height -z and +z.
+/*static*/ PolygonalMesh PolygonalMesh::
+createCylinderMesh(const UnitVec3& axis, Real radius, Real halfLength,
+ int resolution)
+{
+ SimTK_ERRCHK1_ALWAYS(radius > 0, "PolygonalMesh::createCylinderMesh()",
+ "Bad radius %g.", radius);
+ SimTK_ERRCHK1_ALWAYS(halfLength > 0, "PolygonalMesh::createCylinderMesh()",
+ "Bad half length %g.", halfLength);
+ SimTK_ERRCHK1_ALWAYS(resolution >= 0, "PolygonalMesh::createCylinderMesh()",
+ "Resolution %d illegal.", resolution);
+
+ int rezAround = 6*(resolution+1);
+
+ Real angle = 2*Pi/rezAround;
+ Real chordLen = 2*radius*std::sin(angle/2);
+ int rezRadial = (int)(radius/chordLen+0.5);
+ Real edgeLenRad = radius/rezRadial;
+
+ int rezAlong = 1 + std::max((int)(halfLength/edgeLenRad + 0.5), 1);
+ Real edgeLenAlong = 2*halfLength/(rezAlong-1);
+
+ PolygonalMesh cyl;
+ Rotation R_ZG = Rotation(axis, ZAxis);
+
+ int nv[3] = {rezRadial,rezAround,rezAlong};
+ // Do the tube.
+ std::map<long long, int> rak2Vert;
+ for (int k=0; k < rezAlong; ++k) {
+ bool isEndCap = (k==0 || k==rezAlong-1);
+ Real z = -halfLength + k*edgeLenAlong;
+ for (int a=0; a < rezAround; ++a) {
+ Real x = edgeLenRad*std::sin(a*angle);
+ Real y = edgeLenRad*std::cos(a*angle);
+ for (int r=1; r <= rezRadial; ++r) {
+ if (r < rezRadial && !isEndCap)
+ continue; // skip interior vertices
+ int vnum = cyl.addVertex(R_ZG*Vec3(r*x,r*y,z));
+ rak2Vert[locCode(nv,r,a,k)] = vnum;
+ }
+ }
+ }
+
+ Array_<int> qface(4), tface(3);
+ for (int a=0; a < rezAround; ++a) {
+ int ap = (a+1)%rezAround;
+ for (int k=0; k < rezAlong-1; ++k) {
+ int r = rezRadial;
+ qface[0] = rak2Vert[locCode(nv,r, a,k)];
+ qface[1] = rak2Vert[locCode(nv,r, a,k+1)];
+ qface[2] = rak2Vert[locCode(nv,r,ap,k+1)];
+ qface[3] = rak2Vert[locCode(nv,r,ap,k)];
+ cyl.addFace(qface);
+ }
+ }
+
+ // Add central face vertices.
+ rak2Vert[locCode(nv,0,0,0)] = cyl.addVertex(R_ZG*Vec3(0,0,-halfLength));
+ rak2Vert[locCode(nv,0,0,rezAlong-1)] = cyl.addVertex(R_ZG*Vec3(0,0,halfLength));
+
+ // Tri faces from center to first ring.
+ for (int a=0; a < rezAround; ++a) {
+ int ap = (a+1)%rezAround;
+ tface[0] = rak2Vert[locCode(nv,0,0, 0)];
+ tface[1] = rak2Vert[locCode(nv,1,a, 0)];
+ tface[2] = rak2Vert[locCode(nv,1,ap,0)];
+ cyl.addFace(tface);
+ tface[0] = rak2Vert[locCode(nv,0,0, rezAlong-1)];
+ tface[1] = rak2Vert[locCode(nv,1,ap, rezAlong-1)];
+ tface[2] = rak2Vert[locCode(nv,1,a,rezAlong-1)];
+ cyl.addFace(tface);
+ }
+
+
+ // Quad faces from first ring out.
+ for (int a=0; a < rezAround; ++a) {
+ int ap = (a+1)%rezAround;
+ for (int r=1; r <= rezRadial-1; ++r) {
+ qface[0] = rak2Vert[locCode(nv,r, a, 0)];
+ qface[1] = rak2Vert[locCode(nv,r+1,a, 0)];
+ qface[2] = rak2Vert[locCode(nv,r+1,ap,0)];
+ qface[3] = rak2Vert[locCode(nv,r, ap,0)];
+ cyl.addFace(qface);
+ qface[3] = rak2Vert[locCode(nv,r, a, rezAlong-1)];
+ qface[2] = rak2Vert[locCode(nv,r+1,a, rezAlong-1)];
+ qface[1] = rak2Vert[locCode(nv,r+1,ap,rezAlong-1)];
+ qface[0] = rak2Vert[locCode(nv,r, ap,rezAlong-1)];
+ cyl.addFace(qface);
+ }
+ }
+
+ return cyl; // just a shallow copy
+}
+
diff --git a/SimTKcommon/Geometry/src/PolygonalMeshImpl.h b/SimTKcommon/Geometry/src/PolygonalMeshImpl.h
new file mode 100644
index 0000000..2b5e6ac
--- /dev/null
+++ b/SimTKcommon/Geometry/src/PolygonalMeshImpl.h
@@ -0,0 +1,52 @@
+#ifndef SimTK_SimTKCOMMON_POLYGONAL_MESH_IMPL_H_
+#define SimTK_SimTKCOMMON_POLYGONAL_MESH_IMPL_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-13 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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/internal/PolygonalMesh.h"
+#include "SimTKcommon/internal/Array.h"
+
+namespace SimTK {
+
+/**
+ * This is the internal implementation of PolygonalMesh.
+ */
+class SimTK_SimTKCOMMON_EXPORT PolygonalMeshImpl
+: public PIMPLImplementation<PolygonalMesh, PolygonalMeshImpl> {
+public:
+ PolygonalMeshImpl() {faceVertexStart.push_back(0);}
+ ~PolygonalMeshImpl() {}
+ PolygonalMeshImpl* clone() const{return new PolygonalMeshImpl(*this);}
+ void clear() {
+ vertices.clear(); faceVertexIndex.clear(); faceVertexStart.clear();
+ faceVertexStart.push_back(0);
+ }
+ Array_<Vec3> vertices;
+ Array_<int> faceVertexIndex;
+ Array_<int> faceVertexStart;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_POLYGONAL_MESH_IMPL_H_
diff --git a/SimTKcommon/Mechanics/include/SimTKcommon/Mechanics.h b/SimTKcommon/Mechanics/include/SimTKcommon/Mechanics.h
new file mode 100644
index 0000000..580e25d
--- /dev/null
+++ b/SimTKcommon/Mechanics/include/SimTKcommon/Mechanics.h
@@ -0,0 +1,39 @@
+#ifndef SimTK_SIMMATRIX_MECHANICS_H_
+#define SimTK_SIMMATRIX_MECHANICS_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ *
+ * This header collects all the Mechanics header files in one place.
+ */
+#include "SimTKcommon/Scalar.h"
+#include "SimTKcommon/SmallMatrix.h"
+#include "SimTKcommon/Orientation.h"
+
+#include "SimTKcommon/internal/SpatialAlgebra.h"
+#include "SimTKcommon/internal/MassProperties.h"
+
+
+#endif // SimTK_SIMMATRIX_MECHANICS_H_
diff --git a/SimTKcommon/Mechanics/include/SimTKcommon/Orientation.h b/SimTKcommon/Mechanics/include/SimTKcommon/Orientation.h
new file mode 100644
index 0000000..9df3b63
--- /dev/null
+++ b/SimTKcommon/Mechanics/include/SimTKcommon/Orientation.h
@@ -0,0 +1,74 @@
+//-----------------------------------------------------------------------------
+// File: Orientation.h
+// Class: None
+// Parent: None
+// Purpose: Includes UnitVec3, Quaternion, Rotation, Transform, and related classes
+//-----------------------------------------------------------------------------
+#ifndef SimTK_SIMMATRIX_ORIENTATION_H_
+#define SimTK_SIMMATRIX_ORIENTATION_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ *
+ * These are numerical utility classes for dealing with the relative orientations
+ * of geometric objects. These build on the basic arithmetic classes for small
+ * vectors and matrices.
+ */
+
+//-----------------------------------------------------------------------------
+#include "SimTKcommon/internal/common.h"
+#include "SimTKcommon/SmallMatrix.h"
+#include "SimTKcommon/internal/UnitVec.h"
+#include "SimTKcommon/internal/Quaternion.h"
+#include "SimTKcommon/internal/Rotation.h"
+#include "SimTKcommon/internal/Transform.h"
+//-----------------------------------------------------------------------------
+#include <iosfwd> // Forward declaration of iostream
+//-----------------------------------------------------------------------------
+
+//-----------------------------------------------------------------------------
+// Some handy conversion constants.
+// Use templatized inline conversion routines instead whenever possible.
+// These are defined so that you can multiply by them. For example, if you have
+// an angle qRad in radians and want to convert to degrees, write qDeg = qRad*SimTK_RTD.
+// Note that the expressions here will always be evaluated at compile time, yielding
+// long double results which you can cast to smaller sizes if you want.
+//-----------------------------------------------------------------------------
+#define SimTK_RTD (180/SimTK_PI)
+#define SimTK_DTR (SimTK_PI/180)
+
+//-----------------------------------------------------------------------------
+namespace SimTK {
+
+inline static Real convertRadiansToDegrees(const Real rad) { return rad*Real(SimTK_RTD); }
+inline static Real convertDegreesToRadians(const Real deg) { return deg*Real(SimTK_DTR); }
+
+
+//------------------------------------------------------------------------------
+} // End of namespace SimTK
+
+//--------------------------------------------------------------------------
+#endif // SimTK_SIMMATRIX_ORIENTATION_H_
+//--------------------------------------------------------------------------
diff --git a/SimTKcommon/Mechanics/include/SimTKcommon/internal/CoordinateAxis.h b/SimTKcommon/Mechanics/include/SimTKcommon/internal/CoordinateAxis.h
new file mode 100644
index 0000000..6f0ec94
--- /dev/null
+++ b/SimTKcommon/Mechanics/include/SimTKcommon/internal/CoordinateAxis.h
@@ -0,0 +1,417 @@
+#ifndef SimTK_SimTKCOMMON_COORDINATE_AXIS_H_
+#define SimTK_SimTKCOMMON_COORDINATE_AXIS_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Defines the CoordinateAxis and CoordinateDirection classes. **/
+
+#include "SimTKcommon/internal/common.h"
+#include <cassert>
+
+namespace SimTK {
+
+/** This class, along with its sister class CoordinateDirection, provides
+convenient manipulation of the three coordinate axes via the definition of
+three constants XAxis, YAxis, and ZAxis each with a unique subtype and implicit
+conversion to the integers 0, 1, and 2 whenever necessary.\ Methods are
+provided to allow code to be written once that can be used to work with the
+axes in any order.
+
+There are also three CoordinateDirection constants NegXAxis, NegYAxis, and
+NegZAxis, also with unique types permitting efficient compile time
+manipulation. These do not correspond to integers, however. Instead, they are
+objects containing one of the CoordinateAxis objects combined with an integer
+that is 1 or -1 to indicate the direction along that axis. The unary negation
+operator is overloaded so that -XAxis is NegXAxis and -NegZAxis is ZAxis.
+There are implicit conversions to UnitVec3 for any CoordinateAxis or
+CoordinateDirection object, yielding the equivalent (normalized) unit vector
+corresponding to any of the six directions, without doing any computation
+(and in particular, without normalizing).
+ at see CoordinateDirection **/
+class CoordinateAxis {
+public:
+ /** Explicit construction of a CoordinateAxis from a calculated integer
+ that must be 0, 1, or 2 representing XAxis, YAxis, or ZAxis. **/
+ explicit CoordinateAxis( int i ) : m_myAxisId(i) {assertIndexIsInRange(i);}
+
+ /** Implicit conversion of a CoordinateAxis to int 0, 1, or 2. **/
+ operator int() const {return m_myAxisId;}
+
+ /** Return the "next" coordinate axis after this one:
+ - XAxis.getNextAxis() returns YAxis
+ - YAxis.getNextAxis() returns ZAxis
+ - ZAxis.getNextAxis() returns XAxis **/
+ CoordinateAxis getNextAxis() const
+ { return CoordinateAxis((m_myAxisId+1) % 3); }
+
+ /** Return the "previous" coordinate axis before this one:
+ - XAxis.getPreviousAxis() returns ZAxis
+ - YAxis.getPreviousAxis() returns XAxis
+ - ZAxis.getPreviousAxis() returns YAxis **/
+ CoordinateAxis getPreviousAxis() const
+ { return CoordinateAxis((m_myAxisId+2) % 3); }
+
+ /** Given this coordinate axis and one other, return the missing one:
+ - XAxis.getThirdAxis(YAxis) returns ZAxis (and vice versa)
+ - XAxis.getThirdAxis(ZAxis) returns YAxis (and vice versa)
+ - YAxis.getThirdAxis(ZAxis) returns XAxis (and vice versa)
+ @param[in] axis2 A coordinate axis that must be distinct from the
+ current one; it is a fatal error to provide the same axis.
+ @return The unmentioned third axis. **/
+ CoordinateAxis getThirdAxis( const CoordinateAxis& axis2 ) const {
+ assert( isDifferentAxis(axis2) );
+ CoordinateAxis nextAxis = getNextAxis();
+ return nextAxis.isDifferentAxis(axis2) ? nextAxis : axis2.getNextAxis();
+ }
+
+ /** Return true if this is the X axis. **/
+ bool isXAxis() const {return m_myAxisId == 0;}
+ /** Return true if this is the Y axis. **/
+ bool isYAxis() const {return m_myAxisId == 1;}
+ /** Return true if this is the Z axis. **/
+ bool isZAxis() const {return m_myAxisId == 2;}
+ /** Return true if the given \a axis2 is the one following this one as
+ would be reported by getNextAxis(). **/
+ bool isNextAxis( const CoordinateAxis& axis2 ) const
+ { return int(getNextAxis()) == int(axis2); }
+ /** Return true if the given \a axis2 is the one preceding this one as
+ would be reported by getPreviousAxis(). **/
+ bool isPreviousAxis( const CoordinateAxis& axis2 ) const
+ { return int(getPreviousAxis()) == int(axis2); }
+ /** Return true if the given \a axis2 is the same as this one.\ You
+ can use operator==() to perform the same comparison. **/
+ bool isSameAxis( const CoordinateAxis& axis2 ) const
+ { return m_myAxisId == int(axis2); }
+ /** Return true if both \a axis2 and \a axis3 are the same as this one. **/
+ bool areAllSameAxes( const CoordinateAxis& axis2,
+ const CoordinateAxis &axis3 ) const
+ { return isSameAxis(axis2) && isSameAxis(axis3); }
+ /** Return true if the given \a axis2 is not the same one as this
+ one.\ You can use operator!=() to perform the same comparison. **/
+ bool isDifferentAxis( const CoordinateAxis& axis2 ) const
+ { return m_myAxisId != int(axis2); }
+ /** Return true if neither \a axis2 nor \a axis3 is the same as this
+ axis nor each other; that is, (this,axis2,axis3) together cover all three
+ axes. **/
+ bool areAllDifferentAxes( const CoordinateAxis& axis2,
+ const CoordinateAxis& axis3 ) const
+ { return isDifferentAxis(axis2) && isDifferentAxis(axis3)
+ && axis2.isDifferentAxis(axis3); }
+ /** Return true if the given \a axis2 is the one following this one in a
+ forward cyclical direction, that is, if \a axis2 is the one that would be
+ reported by getNextAxis(). **/
+ bool isForwardCyclical( const CoordinateAxis& axis2 ) const
+ { return isNextAxis(axis2); }
+ /** Return true if the given \a axis2 is the one following this one in a
+ reverse cyclical direction, that is, if \a axis2 is the one that would be
+ reported by getPreviousAxis(). **/
+ bool isReverseCyclical( const CoordinateAxis& axis2 ) const
+ { return isPreviousAxis(axis2); }
+
+ /** Perform a specialized dot product between this axis and \a axis2;
+ returning one if they are the same axis and zero otherwise, without
+ performing any floating point operations. **/
+ int dotProduct( const CoordinateAxis& axis2 ) const
+ { return isSameAxis(axis2) ? 1 : 0; }
+ /** Return the sign that would result from a cross product between this
+ axis and \a axis2: zero if \a axis2 is the same as this axis; one if the
+ result would be in the positive direction along the third axis; -1 if it
+ would be in the negative direction. No floating point computations are
+ performed. @see crossProductAxis() **/
+ int crossProductSign( const CoordinateAxis& axis2 ) const
+ { return isSameAxis(axis2) ? 0 : (isNextAxis(axis2) ? 1 : -1); }
+ /** Return the coordinate axis along which the cross product of this axis
+ and \a axis2 would lie: same as this if \a axis2 is the same as this axis
+ (doesn't matter because the sign would be zero); otherwise, the third
+ axis that is neither this one nor \a axis2. But note that the actual
+ result may be along that axis or in the negative direction along that
+ axis. No floating point computations are performed.
+ @see crossProductSign(). **/
+ CoordinateAxis crossProductAxis( const CoordinateAxis& axis2 ) const
+ { return isSameAxis(axis2) ? CoordinateAxis(m_myAxisId)
+ : getThirdAxis(axis2); }
+ /** Return the axis and sign along that axis that would result from a
+ cross product between this axis and \a axis2; this combines the functions
+ of both crossProductAxis() and crossProductSign(). Note that if \a axis2 is
+ the same as this axis we'll just return this as the axis but the sign is
+ zero since the magnitude of the result would be zero. No floating point
+ calculations are performed.
+ @see crossProductSign(), crossProductAxis() **/
+ CoordinateAxis crossProduct( const CoordinateAxis& axis2, int& sign ) const
+ { sign = crossProductSign(axis2); return crossProductAxis(axis2); }
+
+ /** Return a reference to the CoordinateAxis constant XAxis, YAxis, or
+ ZAxis corresponding to the given integer index which must be 0, 1, or 2. **/
+ static const CoordinateAxis& getCoordinateAxis( int i );
+
+ /** Return true if the given integer is suitable as a coordinate axis,
+ meaning it is one of 0, 1, or 2 designating XAxis, YAxis, or ZAxis,
+ respectively. **/
+ static bool isIndexInRange( int i ) { return 0<=i && i<=2; }
+ /** When in Debug mode, throw an assertion if the given integer is not
+ suited as a coordinate axis, as defined by isIndexInRange(). **/
+ static void assertIndexIsInRange( int i ) { assert( isIndexInRange(i) ); }
+
+ // Forward declarations for subsequent helper classes
+ class XCoordinateAxis; class YCoordinateAxis; class ZCoordinateAxis;
+protected:
+ /** @cond **/ // turn off doxygen here; these aren't for users
+ class XTypeAxis{};
+ class YTypeAxis{};
+ class ZTypeAxis{};
+
+ CoordinateAxis( const XTypeAxis& ) : m_myAxisId(0) {}
+ CoordinateAxis( const YTypeAxis& ) : m_myAxisId(1) {}
+ CoordinateAxis( const ZTypeAxis& ) : m_myAxisId(2) {}
+ /** @endcond **/
+private:
+
+ int m_myAxisId;
+};
+
+
+// Helper classes that allow compile time recognition of axis directions.
+class CoordinateAxis::XCoordinateAxis : public CoordinateAxis {
+ public: XCoordinateAxis() : CoordinateAxis(XTypeAxis()) {}
+};
+class CoordinateAxis::YCoordinateAxis : public CoordinateAxis {
+ public: YCoordinateAxis() : CoordinateAxis(YTypeAxis()) {}
+};
+class CoordinateAxis::ZCoordinateAxis : public CoordinateAxis {
+ public: ZCoordinateAxis() : CoordinateAxis(ZTypeAxis()) {}
+};
+
+/** Constant representing the X coordinate axis; will implicitly convert to
+the integer 0 when used in a context requiring an integer. **/
+extern SimTK_SimTKCOMMON_EXPORT const CoordinateAxis::XCoordinateAxis XAxis;
+/** Constant representing the Y coordinate axis; will implicitly convert to
+the integer 1 when used in a context requiring an integer. **/
+extern SimTK_SimTKCOMMON_EXPORT const CoordinateAxis::YCoordinateAxis YAxis;
+/** Constant representing the Z coordinate axis; will implicitly convert to
+the integer 2 when used in a context requiring an integer. **/
+extern SimTK_SimTKCOMMON_EXPORT const CoordinateAxis::ZCoordinateAxis ZAxis;
+
+inline const CoordinateAxis& CoordinateAxis::getCoordinateAxis(int i) {
+ assertIndexIsInRange(i);
+ return (i==0 ? static_cast<const CoordinateAxis&>(XAxis)
+ : (i==1 ? static_cast<const CoordinateAxis&>(YAxis)
+ : static_cast<const CoordinateAxis&>(ZAxis)));
+}
+
+/// Compare two CoordinateAxis objects. @relates CoordinateAxis
+inline bool operator==(const CoordinateAxis& a1, const CoordinateAxis& a2)
+{ return a1.isSameAxis(a2); }
+
+/// Compare two CoordinateAxis objects. @relates CoordinateAxis
+inline bool operator!=(const CoordinateAxis& a1, const CoordinateAxis& a2)
+{ return a1.isDifferentAxis(a2); }
+
+
+/** A CoordinateDirection is a CoordinateAxis plus a direction indicating the
+positive or negative direction along that axis. There are only six possible
+values for a CoordinateDirection, and there are predefined constants available
+covering all of them:
+ - XAxis, YAxis, ZAxis are the CoordinateAxis types; they will implicitly
+ convert to positive axis directions.
+ - NegXAxis, NegYAxis, NegZAxis are the negative directions.
+ - The unary negation operator is overloaded so that -XAxis produces
+ NegXAxis and -NegYAxis produces YAxis.
+ - The unary plus operator is overloaded for the CoordinateAxis objects
+ so that +XAxis and so on are the positive CoordinateDirection objects.
+You can also produce CoordinateDirections at compile time or run time from
+calculated axes and directions.
+ at see CoordinateAxis **/
+class CoordinateDirection {
+public:
+ /** Use for compile-time construction of a negative CoordinateDirection
+ along one of the coordinate axes. **/
+ class Negative {};
+
+ /** Implicit conversion of a CoordinateAxis to a positive
+ CoordinateDirection along that axis. **/
+ CoordinateDirection(const CoordinateAxis& axis)
+ : m_axis(axis), m_direction(1) {}
+
+ /** Explicit creation of a negative CoordinateDirection from a
+ CoordinateAxis. **/
+ CoordinateDirection(const CoordinateAxis& axis, Negative)
+ : m_axis(axis), m_direction(-1) {}
+
+ /** Explicit creation of a CoordinateDirection from a CoordinateAxis
+ and a direction calculated at run time.
+ @param[in] axis XAxis, YAxis, or ZAxis
+ @param[in] direction Must be -1 or 1.
+ @note Zero is not allowed for \a direction, meaning that
+ you must not try to produce one of these from the "sign" result of one of
+ the cross product methods, because there the sign can be -1, 0, or 1. **/
+ CoordinateDirection(const CoordinateAxis& axis, int direction)
+ : m_axis(axis), m_direction(direction)
+ { assert(direction==1 || direction==-1); }
+
+ /** This is the coordinate axis XAxis, YAxis, or ZAxis contained in this
+ CoordinateDirection.\ Use getDirection() to determine whether this is the
+ positive or negative direction. **/
+ CoordinateAxis getAxis() const {return m_axis;}
+ /** Returns 1 or -1 to indicate the direction along the coordinate
+ axis returned by getAxis(). **/
+ int getDirection() const {return m_direction;}
+
+ /** Return true if this direction and \a dir2 are along the same axis,
+ even if the direction along that axis is not the same. **/
+ bool hasSameAxis(const CoordinateDirection& dir2) const
+ { return m_axis.isSameAxis(dir2.getAxis()); }
+
+ /** Return true if this direction and \a dir2 are along the same axis,
+ and in the same direction along that axis.\ You can also
+ use operator==() for this comparison. **/
+ bool isSameAxisAndDirection(const CoordinateDirection& dir2) const
+ { return m_axis==dir2.getAxis() && m_direction==dir2.getDirection(); }
+
+ /** Perform a specialized dot product between this coordinate direction
+ and \a dir2; returning 1 or -1 if they contain the same axis and 0
+ otherwise, without performing any floating point operations. **/
+ int dotProduct( const CoordinateDirection& dir2 ) const
+ { if (m_axis != dir2.getAxis()) return 0;
+ return m_direction == dir2.getDirection() ? 1 : -1; }
+
+ /** Return the sign that would result from a cross product between this
+ coordinate direction and \a dir2: 0 if they are along the same axis;
+ 1 if the result would be in the positive direction along the third axis;
+ -1 if it would be in the negative direction. No floating point
+ computations are performed. @see crossProductAxis() **/
+ int crossProductSign( const CoordinateDirection& dir2 ) const
+ { if (m_axis == dir2.getAxis()) return 0;
+ return m_axis.crossProductSign(dir2.getAxis())
+ * m_direction * dir2.getDirection(); }
+
+ /** Return the coordinate axis along which the cross product of this
+ coordinate direction and \a dir2 would lie: same as this if both contain
+ the same axis (doesn't matter because the sign would be zero); otherwise,
+ the third axis that neither this one nor \a dir2 contains. But note that
+ the actual result may be along that axis or in the negative direction
+ along that axis. No floating point computations are performed.
+ @see crossProductSign(). **/
+ CoordinateAxis crossProductAxis( const CoordinateDirection& dir2 ) const
+ { return m_axis.crossProductAxis(dir2.getAxis()); }
+
+ /** Return the axis and sign along that axis that would result from a
+ cross product between this coordinate direction and \a dir2; this
+ combines the functions of both crossProductAxis() and crossProductSign().
+ Note that if \a dir2 is along the same axis as this one, we'll just
+ return this as the axis but the sign is zero since the magnitude of the
+ result would be zero. No floating point calculations are
+ performed. @see crossProductSign(), crossProductAxis() **/
+ CoordinateAxis crossProduct( const CoordinateDirection& dir2,
+ int& sign ) const
+ { sign = crossProductSign(dir2); return crossProductAxis(dir2); }
+
+ // Local class declarations for helper classes.
+ class NegXDirection; class NegYDirection; class NegZDirection;
+private:
+ CoordinateAxis m_axis; // XAxis, YAxis, or ZAxis
+ int m_direction; // 1 or -1
+};
+
+
+// Helper classes that allow compile time recognition of negative axis
+// directions.
+class CoordinateDirection::NegXDirection : public CoordinateDirection {
+ public: NegXDirection() : CoordinateDirection(XAxis,Negative()) {}
+};
+class CoordinateDirection::NegYDirection : public CoordinateDirection {
+ public: NegYDirection() : CoordinateDirection(YAxis,Negative()) {}
+};
+class CoordinateDirection::NegZDirection : public CoordinateDirection {
+ public: NegZDirection() : CoordinateDirection(ZAxis,Negative()) {}
+};
+
+// Predefine constants for the negative X,Y,Z directions.
+extern SimTK_SimTKCOMMON_EXPORT const CoordinateDirection::NegXDirection
+ NegXAxis; ///< Global constant indicating -X coordinate direction.
+extern SimTK_SimTKCOMMON_EXPORT const CoordinateDirection::NegYDirection
+ NegYAxis; ///< Global constant indicating -Y coordinate direction.
+extern SimTK_SimTKCOMMON_EXPORT const CoordinateDirection::NegZDirection
+ NegZAxis; ///< Global constant indicating -Z coordinate direction.
+
+/// Compare two CoordinateDirection objects. @relates CoordinateDirection
+inline bool operator==(const CoordinateDirection& d1,
+ const CoordinateDirection& d2)
+{ return d1.isSameAxisAndDirection(d2); }
+
+/// Compare two CoordinateDirection objects. @relates CoordinateDirection
+inline bool operator!=(const CoordinateDirection& d1,
+ const CoordinateDirection& d2)
+{ return !d1.isSameAxisAndDirection(d2); }
+
+/// Create the NegXAxis direction by negating XAxis. No computation
+/// is necessary. @relates CoordinateAxis
+inline const CoordinateDirection::NegXDirection&
+operator-(const CoordinateAxis::XCoordinateAxis&){return NegXAxis;}
+/// Create the NegYAxis direction by negating YAxis. No computation
+/// is necessary. @relates CoordinateAxis
+inline const CoordinateDirection::NegYDirection&
+operator-(const CoordinateAxis::YCoordinateAxis&){return NegYAxis;}
+/// Create the NegZAxis direction by negating ZAxis. No computation
+/// is necessary. @relates CoordinateAxis
+inline const CoordinateDirection::NegZDirection&
+operator-(const CoordinateAxis::ZCoordinateAxis&){return NegZAxis;}
+
+/// Create the negative direction along the given axis. No computation
+/// is necessary. @relates CoordinateAxis
+inline CoordinateDirection
+operator-(const CoordinateAxis& axis)
+{ return CoordinateDirection(axis,CoordinateDirection::Negative()); }
+
+/// Create the positive direction along the given axis. No computation
+/// is necessary. @relates CoordinateAxis
+inline CoordinateDirection
+operator+(const CoordinateAxis& axis)
+{ return CoordinateDirection(axis); }
+
+/// Create the XAxis direction by negating NegXAxis. No computation
+/// is necessary. @relates CoordinateDirection
+inline const CoordinateAxis::XCoordinateAxis&
+operator-(const CoordinateDirection::NegXDirection&){return XAxis;}
+/// Create the YAxis direction by negating NegYAxis. No computation
+/// is necessary. @relates CoordinateDirection
+inline const CoordinateAxis::YCoordinateAxis&
+operator-(const CoordinateDirection::NegYDirection&){return YAxis;}
+/// Create the ZAxis direction by negating NegZAxis. No computation
+/// is necessary. @relates CoordinateDirection
+inline const CoordinateAxis::ZCoordinateAxis&
+operator-(const CoordinateDirection::NegZDirection&){return ZAxis;}
+
+/// Create the opposite direction from the given direction. No computation
+/// is necessary. @relates CoordinateDirection
+inline CoordinateDirection
+operator-(const CoordinateDirection& dir)
+{ return CoordinateDirection(dir.getAxis(), -dir.getDirection()); }
+
+} // End of namespace
+
+#endif // SimTK_SimTKCOMMON_COORDINATE_AXIS_H_
+
+
+
diff --git a/SimTKcommon/Mechanics/include/SimTKcommon/internal/MassProperties.h b/SimTKcommon/Mechanics/include/SimTKcommon/internal/MassProperties.h
new file mode 100644
index 0000000..03e224c
--- /dev/null
+++ b/SimTKcommon/Mechanics/include/SimTKcommon/internal/MassProperties.h
@@ -0,0 +1,1570 @@
+#ifndef SimTK_SIMMATRIX_MASS_PROPERTIES_H_
+#define SimTK_SIMMATRIX_MASS_PROPERTIES_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ *
+ * These are utility classes for dealing with mass properties, particularly
+ * those messy inertias.
+ */
+
+#include "SimTKcommon/Scalar.h"
+#include "SimTKcommon/SmallMatrix.h"
+#include "SimTKcommon/Orientation.h"
+
+#include <iostream>
+
+namespace SimTK {
+/** Spatial vectors are used for (rotation,translation) quantities and
+consist of a pair of Vec3 objects, arranged as a 2-vector of 3-vectors.
+Quantities represented this way include
+ - spatial velocity = (angularVelocity,linearVelocity)
+ - spatial acceleration = (angularAcceleration,linearAcceleration)
+ - generalized forces = (torque,force)
+
+Spatial configuration has to be handled differently though since
+orientation is not a vector quantity. (We use Transform for this concept
+which includes a Rotation matrix and a translation Vec3.) **/
+typedef Vec<2, Vec3> SpatialVec;
+/** A SpatialVec that is always single (float) precision regardless of
+the compiled-in precision of Real. **/
+typedef Vec<2, Vec<3,float> > fSpatialVec;
+/** A SpatialVec that is always double precision regardless of
+the compiled-in precision of Real. **/
+typedef Vec<2, Vec<3,double> > dSpatialVec;
+
+/** This is the type of a transposed SpatialVec; it does not usually appear
+explicitly in user programs. **/
+typedef Row<2, Row3> SpatialRow;
+/** A SpatialRow that is always single (float) precision regardless of
+the compiled-in precision of Real. **/
+typedef Row<2, Row<3,float> > fSpatialRow;
+/** A SpatialRow that is always double precision regardless of
+the compiled-in precision of Real. **/
+typedef Row<2, Row<3,double> > dSpatialRow;
+
+/** Spatial matrices are used to hold 6x6 matrices that are best viewed
+as 2x2 matrices of 3x3 matrices; most commonly for spatial and articulated
+body inertias and spatial shift matrices. They also arise commonly as
+intermediates in computations involving SpatialVec objects. **/
+typedef Mat<2,2, Mat33> SpatialMat;
+/** A SpatialMat that is always single (float) precision regardless of
+the compiled-in precision of Real. **/
+typedef Mat<2,2, Mat<3,3,float> > fSpatialMat;
+/** A SpatialMat that is always double precision regardless of
+the compiled-in precision of Real. **/
+typedef Mat<2,2, Mat<3,3,double> > dSpatialMat;
+
+// These are templatized by precision (float or double).
+template <class P> class UnitInertia_;
+template <class P> class Inertia_;
+template <class P> class SpatialInertia_;
+template <class P> class ArticulatedInertia_;
+template <class P> class MassProperties_;
+
+// The "no trailing underscore" typedefs use whatever the
+// compile-time precision is set to.
+
+/** A unit inertia (gyration) tensor at default precision. **/
+typedef UnitInertia_<Real> UnitInertia;
+/** A unit inertia (gyration) tensor at float precision. **/
+typedef UnitInertia_<float> fUnitInertia;
+/** A unit inertia (gyration) tensor at double precision. **/
+typedef UnitInertia_<double> dUnitInertia;
+
+/** An inertia tensor at default precision. **/
+typedef Inertia_<Real> Inertia;
+/** An inertia tensor at float precision. **/
+typedef Inertia_<float> fInertia;
+/** An inertia tensor at double precision. **/
+typedef Inertia_<double> dInertia;
+
+/** Rigid body mass properties at default precision. **/
+typedef MassProperties_<Real> MassProperties;
+/** Rigid body mass properties at float precision. **/
+typedef MassProperties_<float> fMassProperties;
+/** Rigid body mass properties at double precision. **/
+typedef MassProperties_<double> dMassProperties;
+
+/** A spatial (rigid body) inertia matrix at default precision. **/
+typedef SpatialInertia_<Real> SpatialInertia;
+/** A spatial (rigid body) inertia matrix at float precision. **/
+typedef SpatialInertia_<float> fSpatialInertia;
+/** A spatial (rigid body) inertia matrix at double precision. **/
+typedef SpatialInertia_<double> dSpatialInertia;
+
+/** An articulated body inertia matrix at default precision. **/
+typedef ArticulatedInertia_<Real> ArticulatedInertia;
+/** An articulated body inertia matrix at float precision. **/
+typedef ArticulatedInertia_<float> fArticulatedInertia;
+/** An articulated body inertia matrix at double precision. **/
+typedef ArticulatedInertia_<double> dArticulatedInertia;
+
+/** For backwards compatibility only; use UnitInertia instead. **/
+typedef UnitInertia Gyration;
+
+// -----------------------------------------------------------------------------
+// INERTIA MATRIX
+// -----------------------------------------------------------------------------
+/** The physical meaning of an inertia is the distribution of a rigid body's
+mass about a \e particular point. If that point is the center of mass of the
+body, then the measured inertia is called the "central inertia" of that body.
+To write down the inertia, we need to calculate the six scalars of the inertia
+tensor, which is a symmetric 3x3 matrix. These scalars must be expressed in
+an arbitrary but specified coordinate system. So an Inertia is meaningful only
+in conjunction with a particular set of axes, fixed to the body, whose origin
+is the point about which the inertia is being measured, and in whose
+coordinate system this measurement is being expressed. Note that changing the
+reference point results in a new physical quantity, but changing the reference
+axes only affects the measure numbers of that quantity. For any reference
+point, there is a unique set of reference axes in which the inertia tensor is
+diagonal; those are called the "principal axes" of the body at that point, and
+the resulting diagonal elements are the "principal moments of inertia". When
+we speak of an inertia being "in" a frame, we mean the physical quantity
+measured about the frame's origin and then expressed in the frame's axes.
+
+This low-level Inertia class does not attempt to keep track of \e which frame
+it is in. It provides construction and operations involving inertia that can
+proceed using only an implicit frame F. Clients of this class are responsible
+for keeping track of that frame. In particular, in order to shift the
+inertia's "measured-about" point one must know whether either the starting or
+final inertia is central, because we must always shift inertias by passing
+through the central inertia. So this class provides operations for doing the
+shifting, but expects to be told by the client where to find the center of mass.
+
+Re-expressing an Inertia in a different coordinate system does not entail a
+change of physical meaning in the way that shifting it to a different point
+does. Note that because inertia is a tensor, there is a "left frame" and
+"right frame". For our purposes, these will always be the same so we'll only
+indicate the frame once, as in 'I_pt_frame'. This should be understood to mean
+'frame_I_pt_frame' and re-expressing an Inertia requires both a left and right
+multiply by the rotation matrix. So I_OB_B is the inertia about body B's
+origin point OB, expressed in B, while I_OB_G is the same physical quantity
+but expressed in Ground (the latter is a component of the Spatial Inertia
+which we usually want in the Ground frame). Frame conversion is done logically
+like this:
+<pre>
+ I_OB_G = R_GB * I_OB_B * R_BG (R_BG=~R_GB)
+</pre>
+but we can save computation time by performing this as a single operation.
+
+The central inertia would be I_CB_B for body B.
+
+A Inertia is a symmetric matrix and is positive definite for nonsingular bodies
+(that is, a body composed of at least three noncollinear point masses).
+
+Some attempt is made to check the validity of an Inertia matrix, at least
+when running in Debug mode. Some conditions it must satisfy are:
+ - must be symmetric
+ - all diagonal elements must be nonnegative
+ - diagonal elements must satisfy the triangle inequality (sum of any two
+ is greater than or equal the other one)
+
+<h3>Abbreviations</h3>
+Typedefs exist for the most common invocations of Inertia_\<P\>:
+ - \ref SimTK::Inertia "Inertia" for default Real precision (this is
+ almost always used)
+ - \ref SimTK::fInertia "fInertia" for single (float) precision
+ - \ref SimTK::dInertia "dInertia" for double precision
+**/
+template <class P>
+class SimTK_SimTKCOMMON_EXPORT Inertia_ {
+ typedef P RealP;
+ typedef Vec<3,P> Vec3P;
+ typedef SymMat<3,P> SymMat33P;
+ typedef Mat<3,3,P> Mat33P;
+ typedef Rotation_<P> RotationP;
+public:
+/// Default is a NaN-ed out mess to avoid accidents, even in Release mode.
+/// Other than this value, an Inertia matrix should always be valid.
+Inertia_() : I_OF_F(NTraits<P>::getNaN()) {}
+
+// Default copy constructor, copy assignment, destructor.
+
+/// Create a principal inertia matrix with identical diagonal elements,
+/// like a sphere where moment=2/5 m r^2, or a cube where
+/// moment=1/6 m s^2, with m the total mass, r the sphere's radius
+/// and s the length of a side of the cube. Note that many rigid
+/// bodies of different shapes and masses can have the same inertia
+/// matrix.
+explicit Inertia_(const RealP& moment) : I_OF_F(moment)
+{ errChk("Inertia::Inertia(moment)"); }
+
+/// Create an Inertia matrix for a point mass at a given location,
+/// measured from the origin OF of the implicit frame F, and expressed
+/// in F. Cost is 14 flops.
+Inertia_(const Vec3P& p, const RealP& mass) : I_OF_F(pointMassAt(p,mass)) {}
+
+/// Create an inertia matrix from a vector of the \e moments of
+/// inertia (the inertia matrix diagonal) and optionally a vector of
+/// the \e products of inertia (the off-diagonals). Moments are
+/// in the order xx,yy,zz; products are xy,xz,yz.
+explicit Inertia_(const Vec3P& moments, const Vec3P& products=Vec3P(0))
+{ I_OF_F.updDiag() = moments;
+ I_OF_F.updLower() = products;
+ errChk("Inertia::Inertia(moments,products)"); }
+
+/// Create a principal inertia matrix (only non-zero on diagonal).
+Inertia_(const RealP& xx, const RealP& yy, const RealP& zz)
+{ I_OF_F = SymMat33P(xx,
+ 0, yy,
+ 0, 0, zz);
+ errChk("Inertia::setInertia(xx,yy,zz)"); }
+
+/// This is a general inertia matrix. Note the order of these
+/// arguments: moments of inertia first, then products of inertia.
+Inertia_(const RealP& xx, const RealP& yy, const RealP& zz,
+ const RealP& xy, const RealP& xz, const RealP& yz)
+{ I_OF_F = SymMat33P(xx,
+ xy, yy,
+ xz, yz, zz);
+ errChk("Inertia::setInertia(xx,yy,zz,xy,xz,yz)"); }
+
+/// Construct an Inertia from a symmetric 3x3 matrix. The diagonals must
+/// be nonnegative and satisfy the triangle inequality.
+explicit Inertia_(const SymMat33P& I) : I_OF_F(I)
+{ errChk("Inertia::Inertia(SymMat33)"); }
+
+/// Construct an Inertia matrix from a 3x3 symmetric matrix. In Debug mode
+/// we'll test that the supplied matrix is numerically close to symmetric, and
+/// that it satisfies other requirements of an Inertia matrix.
+explicit Inertia_(const Mat33P& m)
+{ SimTK_ERRCHK(m.isNumericallySymmetric(),
+ "Inertia(Mat33)", "The supplied matrix was not symmetric.");
+ I_OF_F = SymMat33P(m);
+ errChk("Inertia(Mat33)"); }
+
+
+/// Set an inertia matrix to have only principal moments (that is, it
+/// will be diagonal). Returns a reference to "this" like an assignment operator.
+Inertia_& setInertia(const RealP& xx, const RealP& yy, const RealP& zz) {
+ I_OF_F = RealP(0); I_OF_F(0,0) = xx; I_OF_F(1,1) = yy; I_OF_F(2,2) = zz;
+ errChk("Inertia::setInertia(xx,yy,zz)");
+ return *this;
+}
+
+/// Set principal moments and optionally off-diagonal terms.
+/// Returns a reference to "this" like an assignment operator.
+Inertia_& setInertia(const Vec3P& moments, const Vec3P& products=Vec3P(0)) {
+ I_OF_F.updDiag() = moments;
+ I_OF_F.updLower() = products;
+ errChk("Inertia::setInertia(moments,products)");
+ return *this;
+}
+
+/// Set this Inertia to a general matrix. Note the order of these
+/// arguments: moments of inertia first, then products of inertia.
+/// Behaves like an assignment statement. Will throw an error message
+/// in Debug mode if the supplied elements do not constitute a valid
+/// Inertia matrix.
+Inertia_& setInertia(const RealP& xx, const RealP& yy, const RealP& zz,
+ const RealP& xy, const RealP& xz, const RealP& yz) {
+ setInertia(Vec3P(xx,yy,zz), Vec3P(xy,xz,yz));
+ errChk("Inertia::setInertia(xx,yy,zz,xy,xz,yz)");
+ return *this;
+}
+
+
+/// Add in another inertia matrix. Frames and reference point must be the same but
+/// we can't check. (6 flops)
+Inertia_& operator+=(const Inertia_& I)
+{ I_OF_F += I.I_OF_F;
+ errChk("Inertia::operator+=()");
+ return *this; }
+
+/// Subtract off another inertia matrix. Frames and reference point must
+/// be the same but we can't check. (6 flops)
+Inertia_& operator-=(const Inertia_& I)
+{ I_OF_F -= I.I_OF_F;
+ errChk("Inertia::operator-=()");
+ return *this; }
+
+/// Multiply this inertia matrix by a scalar. Cost is 6 flops.
+Inertia_& operator*=(const P& s) {I_OF_F *= s; return *this;}
+
+/// Divide this inertia matrix by a scalar. Cost is about 20 flops (a divide
+/// and 6 multiplies).
+Inertia_& operator/=(const P& s) {I_OF_F /= s; return *this;}
+
+/// Assume that the current inertia is about the F frame's origin OF, and
+/// expressed in F. Given the vector from OF to the body center of mass CF,
+/// and the mass m of the body, we can shift the inertia to the center
+/// of mass. This produces a new Inertia I' whose (implicit) frame F' is
+/// aligned with F but has origin CF (an inertia like that is called a "central
+/// inertia". I' = I - Icom where Icom is the inertia of a fictitious
+/// point mass of mass m (that is, the same as the body mass) located at CF
+/// (measured in F) about OF. Cost is 20 flops.
+/// @see shiftToMassCenterInPlace(), shiftFromMassCenter()
+Inertia_ shiftToMassCenter(const Vec3P& CF, const RealP& mass) const
+{ Inertia_ I(*this); I -= pointMassAt(CF, mass);
+ I.errChk("Inertia::shiftToMassCenter()");
+ return I; }
+
+/// Assume that the current inertia is about the F frame's origin OF, and
+/// expressed in F. Given the vector from OF to the body center of mass CF,
+/// and the mass m of the body, we can shift the inertia to the center
+/// of mass. This produces a new Inertia I' whose (implicit) frame F' is
+/// aligned with F but has origin CF (an inertia like that is called a "central
+/// inertia". I' = I - Icom where Icom is the inertia of a fictitious
+/// point mass of mass m (that is, the same as the body mass) located at CF
+/// (measured in F) about OF. Cost is 20 flops.
+/// @see shiftToMassCenter() if you want to leave this object unmolested.
+/// @see shiftFromMassCenterInPlace()
+Inertia_& shiftToMassCenterInPlace(const Vec3P& CF, const RealP& mass)
+{ (*this) -= pointMassAt(CF, mass);
+ errChk("Inertia::shiftToMassCenterInPlace()");
+ return *this; }
+
+/// Assuming that the current inertia I is a central inertia (that is, it is
+/// inertia about the body center of mass CF), shift it to some other point p
+/// measured from the center of mass. This produces a new inertia I' about
+/// the point p given by I' = I + Ip where Ip is the inertia of a fictitious
+/// point mass of mass mtot (the total body mass) located at p, about CF.
+/// Cost is 20 flops.
+/// @see shiftFromMassCenterInPlace(), shiftToMassCenter()
+Inertia_ shiftFromMassCenter(const Vec3P& p, const RealP& mass) const
+{ Inertia_ I(*this); I += pointMassAt(p, mass);
+ I.errChk("Inertia::shiftFromMassCenter()");
+ return I; }
+
+/// Assuming that the current inertia I is a central inertia (that is, it is
+/// inertia about the body center of mass CF), shift it to some other point p
+/// measured from the center of mass. This produces a new inertia I' about
+/// the point p given by I' = I + Ip where Ip is the inertia of a fictitious
+/// point mass of mass mtot (the total body mass) located at p, about CF.
+/// Cost is 20 flops.
+/// @see shiftFromMassCenter() if you want to leave this object unmolested.
+/// @see shiftToMassCenterInPlace()
+Inertia_& shiftFromMassCenterInPlace(const Vec3P& p, const RealP& mass)
+{ (*this) += pointMassAt(p, mass);
+ errChk("Inertia::shiftFromMassCenterInPlace()");
+ return *this; }
+
+/// Return a new inertia matrix like this one but re-expressed in another
+/// frame (leaving the origin point unchanged). Call this inertia matrix
+/// I_OF_F, that is, it is taken about the origin of some frame F, and
+/// expressed in F. We want to return I_OF_B, the same inertia matrix,
+/// still taken about the origin of F, but expressed in the B frame, given
+/// by I_OF_B=R_BF*I_OF_F*R_FB where R_FB is the rotation matrix giving
+/// the orientation of frame B in F. This is handled here by a special
+/// method of the Rotation class which rotates a symmetric tensor
+/// at a cost of 57 flops.
+/// @see reexpressInPlace()
+Inertia_ reexpress(const Rotation_<P>& R_FB) const
+{ return Inertia_((~R_FB).reexpressSymMat33(I_OF_F)); }
+
+/// Rexpress using an inverse rotation to avoid having to convert it.
+/// @see rexpress(Rotation) for information
+Inertia_ reexpress(const InverseRotation_<P>& R_FB) const
+{ return Inertia_((~R_FB).reexpressSymMat33(I_OF_F)); }
+
+/// Re-express this inertia matrix in another frame, changing the object
+/// in place; see reexpress() if you want to leave this object unmolested
+/// and get a new one instead. Cost is 57 flops.
+/// @see reexpress() if you want to leave this object unmolested.
+Inertia_& reexpressInPlace(const Rotation_<P>& R_FB)
+{ I_OF_F = (~R_FB).reexpressSymMat33(I_OF_F); return *this; }
+
+/// Rexpress in place using an inverse rotation to avoid having to convert it.
+/// @see rexpressInPlace(Rotation) for information
+Inertia_& reexpressInPlace(const InverseRotation_<P>& R_FB)
+{ I_OF_F = (~R_FB).reexpressSymMat33(I_OF_F); return *this; }
+
+RealP trace() const {return I_OF_F.trace();}
+
+/// This is an implicit conversion to a const SymMat33.
+operator const SymMat33P&() const {return I_OF_F;}
+
+/// Obtain a reference to the underlying symmetric matrix type.
+const SymMat33P& asSymMat33() const {return I_OF_F;}
+
+/// Expand the internal packed representation into a full 3x3 symmetric
+/// matrix with all elements set.
+Mat33P toMat33() const {return Mat33P(I_OF_F);}
+
+/// Obtain the inertia moments (diagonal of the Inertia matrix) as a Vec3
+/// ordered xx, yy, zz.
+const Vec3P& getMoments() const {return I_OF_F.getDiag();}
+/// Obtain the inertia products (off-diagonals of the Inertia matrix)
+/// as a Vec3 with elements ordered xy, xz, yz.
+const Vec3P& getProducts() const {return I_OF_F.getLower();}
+
+bool isNaN() const {return I_OF_F.isNaN();}
+bool isInf() const {return I_OF_F.isInf();}
+bool isFinite() const {return I_OF_F.isFinite();}
+
+/// Compare this inertia matrix with another one and return true if they
+/// are close to within a default numerical tolerance. Cost is about
+/// 30 flops.
+bool isNumericallyEqual(const Inertia_<P>& other) const
+{ return I_OF_F.isNumericallyEqual(other.I_OF_F); }
+
+/// Compare this inertia matrix with another one and return true if they
+/// are close to within a specified numerical tolerance. Cost is about
+/// 30 flops.
+bool isNumericallyEqual(const Inertia_<P>& other, double tol) const
+{ return I_OF_F.isNumericallyEqual(other.I_OF_F, tol); }
+
+/// %Test some conditions that must hold for a valid Inertia matrix.
+/// Cost is about 25 flops.
+static bool isValidInertiaMatrix(const SymMat33P& m) {
+ if (m.isNaN()) return false;
+
+ const Vec3P& d = m.diag();
+ if (!(d >= 0)) return false; // diagonals must be nonnegative
+
+ const RealP Slop = std::max(d.sum(),RealP(1))
+ * NTraits<P>::getSignificant();
+
+ if (!(d[0]+d[1]+Slop>=d[2] && d[0]+d[2]+Slop>=d[1] && d[1]+d[2]+Slop>=d[0]))
+ return false; // must satisfy triangle inequality
+
+ // Thanks to Paul Mitiguy for this condition on products of inertia.
+ const Vec3P& p = m.getLower();
+ if (!( d[0]+Slop>=std::abs(2*p[2])
+ && d[1]+Slop>=std::abs(2*p[1])
+ && d[2]+Slop>=std::abs(2*p[0])))
+ return false; // max products are limited by moments
+
+ return true;
+}
+
+/// Create an Inertia matrix for a point located at the origin -- that is,
+/// an all-zero matrix.
+static Inertia_ pointMassAtOrigin() {return Inertia_(0);}
+
+/// Create an Inertia matrix for a point of a given mass, located at
+/// a given location measured from the origin of the implicit F frame.
+/// This is equivalent to m*crossMatSq(p) but is implemented elementwise
+/// here for speed, giving a cost of 14 flops.
+static Inertia_ pointMassAt(const Vec3P& p, const RealP& m) {
+ const Vec3P mp = m*p; // 3 flops
+ const RealP mxx = mp[0]*p[0];
+ const RealP myy = mp[1]*p[1];
+ const RealP mzz = mp[2]*p[2];
+ const RealP nmx = -mp[0];
+ const RealP nmy = -mp[1];
+ return Inertia_( myy+mzz, mxx+mzz, mxx+myy,
+ nmx*p[1], nmx*p[2], nmy*p[2] );
+}
+
+/// @name Unit inertia matrix factories
+/// These return UnitInertia matrices (inertias of unit-mass objects)
+/// converted to Inertias. Multiply the result by the actual mass
+/// to get the Inertia of an actual object of this shape. See the
+/// UnitInertia class for more information.
+//@{
+
+/// Create a UnitInertia matrix for a unit mass sphere of radius \a r centered
+/// at the origin.
+inline static Inertia_ sphere(const RealP& r);
+
+/// Unit-mass cylinder aligned along z axis; use radius and half-length.
+/// If r==0 this is a thin rod; hz=0 it is a thin disk.
+inline static Inertia_ cylinderAlongZ(const RealP& r, const RealP& hz);
+
+/// Unit-mass cylinder aligned along y axis; use radius and half-length.
+/// If r==0 this is a thin rod; hy=0 it is a thin disk.
+inline static Inertia_ cylinderAlongY(const RealP& r, const RealP& hy);
+
+/// Unit-mass cylinder aligned along x axis; use radius and half-length.
+/// If r==0 this is a thin rod; hx=0 it is a thin disk.
+inline static Inertia_ cylinderAlongX(const RealP& r, const RealP& hx);
+
+/// Unit-mass brick given by half-lengths in each direction. One dimension zero
+/// gives inertia of a thin rectangular sheet; two zero gives inertia
+/// of a thin rod in the remaining direction.
+inline static Inertia_ brick(const RealP& hx, const RealP& hy, const RealP& hz);
+
+/// Alternate interface to brick() that takes a Vec3 for the half lengths.
+inline static Inertia_ brick(const Vec3P& halfLengths);
+
+/// Unit-mass ellipsoid given by half-lengths in each direction.
+inline static Inertia_ ellipsoid(const RealP& hx, const RealP& hy, const RealP& hz);
+
+/// Alternate interface to ellipsoid() that takes a Vec3 for the half lengths.
+inline static Inertia_ ellipsoid(const Vec3P& halfLengths);
+
+//@}
+
+protected:
+// Reinterpret this Inertia matrix as a UnitInertia matrix, that is, as the
+// inertia of something with unit mass. This is useful in implementing
+// methods of the UnitInertia class in terms of Inertia methods. Be sure you
+// know that this is a unit-mass inertia!
+const UnitInertia_<P>& getAsUnitInertia() const
+{ return *static_cast<const UnitInertia_<P>*>(this); }
+UnitInertia_<P>& updAsUnitInertia()
+{ return *static_cast<UnitInertia_<P>*>(this); }
+
+// If error checking is enabled (only in Debug mode), this
+// method will run some tests on the current contents of this Inertia
+// matrix and throw an error message if it is not valid. This should be
+// the same set of tests as run by the isValidInertiaMatrix() method above.
+void errChk(const char* methodName) const {
+#ifndef NDEBUG
+ SimTK_ERRCHK(!isNaN(), methodName,
+ "Inertia matrix contains a NaN.");
+
+ const Vec3P& d = I_OF_F.getDiag(); // moments
+ const Vec3P& p = I_OF_F.getLower(); // products
+ const RealP Ixx = d[0], Iyy = d[1], Izz = d[2];
+ const RealP Ixy = p[0], Ixz = p[1], Iyz = p[2];
+
+ SimTK_ERRCHK3(d >= -SignificantReal, methodName,
+ "Diagonals of an Inertia matrix must be nonnegative; got %g,%g,%g.",
+ (double)Ixx,(double)Iyy,(double)Izz);
+
+ // TODO: This is looser than it should be as a workaround for distorted
+ // rotation matrices that were produced by an 11,000 body chain that
+ // Sam Flores encountered.
+ const RealP Slop = std::max(d.sum(),RealP(1))
+ * std::sqrt(NTraits<P>::getEps());
+
+ SimTK_ERRCHK3( Ixx+Iyy+Slop>=Izz
+ && Ixx+Izz+Slop>=Iyy
+ && Iyy+Izz+Slop>=Ixx,
+ methodName,
+ "Diagonals of an Inertia matrix must satisfy the triangle "
+ "inequality; got %g,%g,%g.",
+ (double)Ixx,(double)Iyy,(double)Izz);
+
+ // Thanks to Paul Mitiguy for this condition on products of inertia.
+ SimTK_ERRCHK( Ixx+Slop>=std::abs(2*Iyz)
+ && Iyy+Slop>=std::abs(2*Ixz)
+ && Izz+Slop>=std::abs(2*Ixy),
+ methodName,
+ "The magnitude of a product of inertia was too large to be physical.");
+#endif
+}
+
+// Inertia expressed in frame F and about F's origin OF. Note that frame F
+// is implicit here; all we actually have are the inertia scalars.
+SymMat33P I_OF_F;
+};
+
+/// Add two compatible inertia matrices, meaning they must be taken about the
+/// same point and expressed in the same frame. There is no way to verify
+/// compatibility; make sure you know what you're doing. Cost is 6 flops.
+/// @relates Inertia_
+template <class P> inline Inertia_<P>
+operator+(const Inertia_<P>& l, const Inertia_<P>& r)
+{ return Inertia_<P>(l) += r; }
+
+/// Subtract from one inertia matrix another one which is compatible, meaning
+/// that both must be taken about the same point and expressed in the same frame.
+/// There is no way to verify compatibility; make sure you know what you're doing.
+/// Cost is 6 flops.
+/// @relates Inertia_
+template <class P> inline Inertia_<P>
+operator-(const Inertia_<P>& l, const Inertia_<P>& r)
+{ return Inertia_<P>(l) -= r; }
+
+/// Multiply an inertia matrix by a scalar. Cost is 6 flops.
+/// @relates Inertia_
+template <class P> inline Inertia_<P>
+operator*(const Inertia_<P>& i, const P& r)
+{ return Inertia_<P>(i) *= r; }
+
+/// Multiply an inertia matrix by a scalar. Cost is 6 flops.
+/// @relates Inertia_
+template <class P> inline Inertia_<P>
+operator*(const P& r, const Inertia_<P>& i)
+{ return Inertia_<P>(i) *= r; }
+
+
+/// Multiply an inertia matrix by a scalar given as an int.
+/// Cost is 6 flops.
+/// @relates Inertia_
+template <class P> inline Inertia_<P>
+operator*(const Inertia_<P>& i, int r)
+{ return Inertia_<P>(i) *= P(r); }
+
+/// Multiply an inertia matrix by a scalar given as an int.
+/// Cost is 6 flops.
+/// @relates Inertia_
+template <class P> inline Inertia_<P>
+operator*(int r, const Inertia_<P>& i)
+{ return Inertia_<P>(i) *= P(r); }
+
+/// Divide an inertia matrix by a scalar. Cost is about 20
+/// flops (one divide and six multiplies).
+/// @relates Inertia_
+template <class P> inline Inertia_<P>
+operator/(const Inertia_<P>& i, const P& r)
+{ return Inertia_<P>(i) /= r; }
+
+/// Divide an inertia matrix by a scalar provided as an int.
+/// Cost is about 20 flops (one divide and six multiplies).
+/// @relates Inertia_
+template <class P> inline Inertia_<P>
+operator/(const Inertia_<P>& i, int r)
+{ return Inertia_<P>(i) /= P(r); }
+
+/// Multiply an inertia matrix I on the right by a vector w giving the
+/// vector result I*w.
+/// @relates Inertia_
+template <class P> inline Vec<3,P>
+operator*(const Inertia_<P>& I, const Vec<3,P>& w)
+{ return I.asSymMat33() * w; }
+
+/// Compare two inertia matrices for exact (bitwise) equality. This is
+/// too strict for most purposes; use Inertia::isNumericallyEqual() instead
+/// to test for approximate equality. Cost here is 6 flops.
+/// @relates Inertia_
+template <class P> inline bool
+operator==(const Inertia_<P>& i1, const Inertia_<P>& i2)
+{ return i1.asSymMat33() == i2.asSymMat33(); }
+
+/// Output a human-readable representation of an inertia matrix to the
+/// indicated stream.
+/// @relates Inertia_
+template <class P> inline std::ostream&
+operator<<(std::ostream& o, const Inertia_<P>& inertia)
+{ return o << inertia.toMat33(); }
+
+
+// -----------------------------------------------------------------------------
+// UNIT INERTIA MATRIX
+// -----------------------------------------------------------------------------
+/** A UnitInertia matrix is a unit-mass inertia matrix; you can convert it to an
+Inertia by multiplying it by the actual body mass. Functionality is limited
+here to those few operations which ensure unit mass; most operations on a
+UnitInertia matrix result in a general Inertia instead. You can use a
+UnitInertia object wherever an Inertia is expected but not vice versa.
+
+When constructing a UnitInertia matrix, note that we cannot verify that it
+actually has unit mass because every legal Inertia matrix can be viewed as
+the UnitInertia matrix for some differently-scaled object.
+
+Unit inertia matrices are sometimes called "gyration" matrices; we will often
+represent them with the symbol "G" to avoid confusion with general inertia
+matrices for which the symbol "I" (or sometimes "J") is used.
+
+<h3>Abbreviations</h3>
+Typedefs exist for the most common invocations of UnitInertia_\<P\>:
+ - \ref SimTK::UnitInertia "UnitInertia" for default Real precision (this is
+ almost always used)
+ - \ref SimTK::fUnitInertia "fUnitInertia" for single (float) precision
+ - \ref SimTK::dUnitInertia "dUnitInertia" for double precision **/
+template <class P>
+class SimTK_SimTKCOMMON_EXPORT UnitInertia_ : public Inertia_<P> {
+ typedef P RealP;
+ typedef Vec<3,P> Vec3P;
+ typedef SymMat<3,P> SymMat33P;
+ typedef Mat<3,3,P> Mat33P;
+ typedef Rotation_<P> RotationP;
+ typedef Inertia_<P> InertiaP;
+public:
+/// Default is a NaN-ed out mess to avoid accidents, even in Release mode.
+/// Other than this value, a UnitInertia_ should always be valid.
+UnitInertia_() {}
+
+// Default copy constructor, copy assignment, destructor.
+
+/// Create a principal unit inertia matrix with identical diagonal elements.
+/// This is the unit inertia matrix of a unit mass sphere of radius
+/// r = sqrt(5/2 * moment) centered on the origin.
+explicit UnitInertia_(const RealP& moment) : InertiaP(moment) {}
+
+/// Create a unit inertia matrix from a vector of the \e moments of
+/// inertia (the inertia matrix diagonal) and optionally a vector of
+/// the \e products of inertia (the off-diagonals). Moments are
+/// in the order xx,yy,zz; products are xy,xz,yz.
+explicit UnitInertia_(const Vec3P& moments, const Vec3P& products=Vec3P(0))
+: InertiaP(moments,products) {}
+
+/// Create a principal unit inertia matrix (only non-zero on diagonal).
+UnitInertia_(const RealP& xx, const RealP& yy, const RealP& zz)
+: InertiaP(xx,yy,zz) {}
+
+/// This is a general unit inertia matrix. Note the order of these
+/// arguments: moments of inertia first, then products of inertia.
+UnitInertia_(const RealP& xx, const RealP& yy, const RealP& zz,
+ const RealP& xy, const RealP& xz, const RealP& yz)
+: InertiaP(xx,yy,zz,xy,xz,yz) {}
+
+/// Construct a UnitInertia from a symmetric 3x3 matrix. The diagonals must
+/// be nonnegative and satisfy the triangle inequality.
+explicit UnitInertia_(const SymMat33P& m) : InertiaP(m) {}
+
+/// Construct a UnitInertia from a 3x3 symmetric matrix. In Debug mode
+/// we'll test that the supplied matrix is numerically close to symmetric,
+/// and that it satisfies other requirements of an inertia matrix.
+explicit UnitInertia_(const Mat33P& m) : InertiaP(m) {}
+
+/// Construct a UnitInertia matrix from an Inertia matrix. Note that there
+/// is no way to check whether this is really a unit inertia -- \e any
+/// inertia matrix may be interpreted as a unit inertia for some shape. So
+/// be sure you know what you're doing before you use this constructor!
+explicit UnitInertia_(const Inertia_<P>& I) : InertiaP(I) {}
+
+/// Set a UnitInertia matrix to have only principal moments (that is, it
+/// will be diagonal). Returns a reference to "this" like an assignment
+/// operator.
+UnitInertia_& setUnitInertia(const RealP& xx, const RealP& yy, const RealP& zz)
+{ InertiaP::setInertia(xx,yy,zz); return *this; }
+
+/// Set principal moments and optionally off-diagonal terms.
+/// Returns a reference to "this" like an assignment operator.
+UnitInertia_& setUnitInertia(const Vec3P& moments, const Vec3P& products=Vec3P(0))
+{ InertiaP::setInertia(moments,products); return *this; }
+
+/// Set this UnitInertia to a general matrix. Note the order of these
+/// arguments: moments of inertia first, then products of inertia.
+/// Behaves like an assignment statement. Will throw an error message
+/// in Debug mode if the supplied elements do not constitute a valid
+/// inertia matrix.
+UnitInertia_& setUnitInertia(const RealP& xx, const RealP& yy, const RealP& zz,
+ const RealP& xy, const RealP& xz, const RealP& yz)
+{ InertiaP::setInertia(xx,yy,zz,xy,xz,yz); return *this; }
+
+
+// No +=, -=, etc. operators because those don't result in a UnitInertia
+// matrix. The parent class ones are suppressed below.
+
+/// Assuming that this unit inertia matrix is currently taken about some (implicit)
+/// frame F's origin OF, produce a new unit inertia matrix which is the same as this one
+/// except measured about the body's centroid CF. We are given the vector from OF to
+/// the centroid CF, expressed in F. This produces a new UnitInertia matrix G' whose
+/// (implicit) frame F' is aligned with F but has origin CF (an inertia matrix like
+/// that is called "central" or "centroidal"). From the parallel axis theorem for
+/// inertias, G' = G - Gcom where Gcom is the inertia matrix of a fictitious,
+/// unit-mass point located at CF (measured in F) taken about OF. (17 flops)
+/// @see shiftToCentroidInPlace(), shiftFromCentroid()
+UnitInertia_ shiftToCentroid(const Vec3P& CF) const
+{ UnitInertia_ G(*this);
+ G.Inertia_<P>::operator-=(pointMassAt(CF));
+ return G; }
+
+/// Assuming that this unit inertia matrix is currently taken about some (implicit)
+/// frame F's origin OF, modify it so that it is instead taken about the body's
+/// centroid CF. We are given the vector from OF to
+/// the centroid CF, expressed in F. This produces a new UnitInertia G' whose
+/// (implicit) frame F' is aligned with F but has origin CF (an inertia matrix like
+/// that is called "central" or "centroidal"). From the parallel axis theorem for
+/// inertias, G' = G - Gcom where Gcom is the inertia matrix of a fictitious,
+/// unit-mass point located at CF (measured in F) taken about OF. A reference
+/// to the modified object is returned so that you can chain this method in
+/// the manner of assignment operators. Cost is 17 flops.
+/// @see shiftToCentroid() if you want to leave this object unmolested.
+/// @see shiftFromCentroidInPlace()
+UnitInertia_& shiftToCentroidInPlace(const Vec3P& CF)
+{ InertiaP::operator-=(pointMassAt(CF));
+ return *this; }
+
+/// Assuming that the current UnitInertia G is a central inertia (that is, it is
+/// inertia about the body centroid CF), create a new object that is the same
+/// as this one except shifted to some other point p measured from the centroid.
+/// This produces a new inertia G' about the point p given by G' = G + Gp where
+/// Gp is the inertia of a fictitious point located at p, taken about CF. Cost
+/// is 17 flops.
+/// @see shiftFromCentroidInPlace(), shiftToCentroid()
+UnitInertia_ shiftFromCentroid(const Vec3P& p) const
+{ UnitInertia_ G(*this);
+ G.Inertia_<P>::operator+=(pointMassAt(p));
+ return G; }
+
+/// Assuming that the current UnitInertia G is a central inertia (that is, it is
+/// inertia about the body centroid CF), shift it in place to some other point p
+/// measured from the centroid. This changes G to a modified inertia G' taken
+/// about the point p, with the parallel axis theorem for inertia giving
+/// G' = G + Gp where Gp is the inertia of a fictitious, unit-mass point located
+/// at p, taken about CF. Cost is 17 flops.
+/// @see shiftFromCentroid() if you want to leave this object unmolested.
+/// @see shitToCentroidInPlace()
+UnitInertia_& shiftFromCentroidInPlace(const Vec3P& p)
+{ InertiaP::operator+=(pointMassAt(p));
+ return *this; }
+
+/// Return a new unit inertia matrix like this one but re-expressed in another
+/// frame (leaving the origin point unchanged). Call this inertia matrix
+/// G_OF_F, that is, it is taken about the origin of some frame F, and
+/// expressed in F. We want to return G_OF_B, the same unit inertia matrix,
+/// still taken about the origin of F, but expressed in the B frame, given
+/// by G_OF_B=R_BF*G_OF_F*R_FB where R_FB is the rotation matrix giving
+/// the orientation of frame B in F. This is handled here by a special
+/// method of the Rotation class which rotates a symmetric tensor
+/// at a cost of 57 flops.
+/// @see reexpressInPlace()
+UnitInertia_ reexpress(const Rotation_<P>& R_FB) const
+{ return UnitInertia_((~R_FB).reexpressSymMat33(this->I_OF_F)); }
+
+/// Rexpress using an inverse rotation to avoid having to convert it.
+/// @see rexpress(Rotation) for information
+UnitInertia_ reexpress(const InverseRotation_<P>& R_FB) const
+{ return UnitInertia_((~R_FB).reexpressSymMat33(this->I_OF_F)); }
+
+/// Re-express this unit inertia matrix in another frame, changing the object
+/// in place; see reexpress() if you want to leave this object unmolested
+/// and get a new one instead. Cost is 57 flops.
+/// @see reexpress() if you want to leave this object unmolested.
+UnitInertia_& reexpressInPlace(const Rotation_<P>& R_FB)
+{ InertiaP::reexpressInPlace(R_FB); return *this; }
+
+/// Rexpress using an inverse rotation to avoid having to convert it.
+/// @see rexpressInPlace(Rotation) for information
+UnitInertia_& reexpressInPlace(const InverseRotation_<P>& R_FB)
+{ InertiaP::reexpressInPlace(R_FB); return *this; }
+
+
+/// This is an implicit conversion to const SymMat33.
+operator const SymMat33P&() const {return this->I_OF_F;}
+
+/// Recast this UnitInertia matrix as a unit inertia matrix. This is just for
+/// emphasis; a UnitInertia matrix is already a kind of Inertia matrix by
+/// inheritance.
+const Inertia_<P>& asUnitInertia() const
+{ return *static_cast<const Inertia_<P>*>(this); }
+
+/// Set from a unit inertia matrix. Note that we can't check; every Inertia
+/// matrix can be interpreted as a unit inertia for some shape.
+UnitInertia_& setFromUnitInertia(const Inertia_<P>& I)
+{ Inertia_<P>::operator=(I);
+ return *this; }
+
+/// %Test some conditions that must hold for a valid UnitInertia matrix.
+/// Cost is about 9 flops.
+/// TODO: this may not be comprehensive.
+static bool isValidUnitInertiaMatrix(const SymMat33P& m)
+{ return Inertia_<P>::isValidInertiaMatrix(m); }
+
+/// @name UnitInertia matrix factories
+/// These are UnitInertia matrix factories for some common 3D solids. Each
+/// defines its own frame aligned (when possible) with principal moments.
+/// Each has unit mass and its center of mass located at the origin (usually).
+/// Use this with shiftFromCentroid() to move it somewhere else, and with
+/// reexpress() to express the UnitInertia matrix in another frame.
+//@{
+
+/// Create a UnitInertia matrix for a point located at the origin -- that is,
+/// an all-zero matrix.
+static UnitInertia_ pointMassAtOrigin() {return UnitInertia_(0);}
+
+/// Create a UnitInertia matrix for a point of unit mass located at a given
+/// location measured from origin OF and expressed in F (where F is the
+/// implicit frame of this UnitInertia matrix).
+/// Cost is 11 flops.
+static UnitInertia_ pointMassAt(const Vec3P& p)
+{ return UnitInertia_(crossMatSq(p)); }
+
+/// Create a UnitInertia matrix for a unit mass sphere of radius \a r centered
+/// at the origin.
+static UnitInertia_ sphere(const RealP& r) {return UnitInertia_(RealP(0.4)*r*r);}
+
+/// Unit-mass cylinder aligned along z axis; use radius and half-length.
+/// If r==0 this is a thin rod; hz=0 it is a thin disk.
+static UnitInertia_ cylinderAlongZ(const RealP& r, const RealP& hz) {
+ const RealP Ixx = (r*r)/4 + (hz*hz)/3;
+ return UnitInertia_(Ixx,Ixx,(r*r)/2);
+}
+
+/// Unit-mass cylinder aligned along y axis; use radius and half-length.
+/// If r==0 this is a thin rod; hy=0 it is a thin disk.
+static UnitInertia_ cylinderAlongY(const RealP& r, const RealP& hy) {
+ const RealP Ixx = (r*r)/4 + (hy*hy)/3;
+ return UnitInertia_(Ixx,(r*r)/2,Ixx);
+}
+
+/// Unit-mass cylinder aligned along x axis; use radius and half-length.
+/// If r==0 this is a thin rod; hx=0 it is a thin disk.
+static UnitInertia_ cylinderAlongX(const RealP& r, const RealP& hx) {
+ const RealP Iyy = (r*r)/4 + (hx*hx)/3;
+ return UnitInertia_((r*r)/2,Iyy,Iyy);
+}
+
+/// Unit-mass brick given by half-lengths in each direction. One dimension zero
+/// gives inertia of a thin rectangular sheet; two zero gives inertia
+/// of a thin rod in the remaining direction.
+static UnitInertia_ brick(const RealP& hx, const RealP& hy, const RealP& hz) {
+ const RealP oo3 = RealP(1)/RealP(3);
+ const RealP hx2=hx*hx, hy2=hy*hy, hz2=hz*hz;
+ return UnitInertia_(oo3*(hy2+hz2), oo3*(hx2+hz2), oo3*(hx2+hy2));
+}
+
+/// Alternate interface to brick() that takes a Vec3 for the half lengths.
+static UnitInertia_ brick(const Vec3P& halfLengths)
+{ return brick(halfLengths[0],halfLengths[1],halfLengths[2]); }
+
+/// Unit-mass ellipsoid given by half-lengths in each direction.
+static UnitInertia_ ellipsoid(const RealP& hx, const RealP& hy, const RealP& hz) {
+ const RealP hx2=hx*hx, hy2=hy*hy, hz2=hz*hz;
+ return UnitInertia_((hy2+hz2)/5, (hx2+hz2)/5, (hx2+hy2)/5);
+}
+
+/// Alternate interface to ellipsoid() that takes a Vec3 for the half lengths.
+static UnitInertia_ ellipsoid(const Vec3P& halfLengths)
+{ return ellipsoid(halfLengths[0],halfLengths[1],halfLengths[2]); }
+
+//@}
+private:
+// Suppress Inertia_ methods which are not allowed for UnitInertia_.
+
+// These kill all flavors of Inertia_::setInertia() and the
+// Inertia_ assignment ops.
+void setInertia() {}
+void operator+=(int) {}
+void operator-=(int) {}
+void operator*=(int) {}
+void operator/=(int) {}
+};
+
+// Implement Inertia methods which are pass-throughs to UnitInertia methods.
+
+template <class P> inline Inertia_<P> Inertia_<P>::
+sphere(const RealP& r)
+{ return UnitInertia_<P>::sphere(r); }
+template <class P> inline Inertia_<P> Inertia_<P>::
+cylinderAlongZ(const RealP& r, const RealP& hz)
+{ return UnitInertia_<P>::cylinderAlongZ(r,hz); }
+template <class P> inline Inertia_<P> Inertia_<P>::
+cylinderAlongY(const RealP& r, const RealP& hy)
+{ return UnitInertia_<P>::cylinderAlongY(r,hy); }
+template <class P> inline Inertia_<P> Inertia_<P>::
+cylinderAlongX(const RealP& r, const RealP& hx)
+{ return UnitInertia_<P>::cylinderAlongX(r,hx); }
+template <class P> inline Inertia_<P> Inertia_<P>::
+brick(const RealP& hx, const RealP& hy, const RealP& hz)
+{ return UnitInertia_<P>::brick(hx,hy,hz); }
+template <class P> inline Inertia_<P> Inertia_<P>::
+brick(const Vec3P& halfLengths)
+{ return UnitInertia_<P>::brick(halfLengths); }
+template <class P> inline Inertia_<P> Inertia_<P>::
+ellipsoid(const RealP& hx, const RealP& hy, const RealP& hz)
+{ return UnitInertia_<P>::ellipsoid(hx,hy,hz); }
+template <class P> inline Inertia_<P> Inertia_<P>::
+ellipsoid(const Vec3P& halfLengths)
+{ return UnitInertia_<P>::ellipsoid(halfLengths); }
+
+
+// -----------------------------------------------------------------------------
+// SPATIAL INERTIA MATRIX
+// -----------------------------------------------------------------------------
+/** A spatial inertia contains the mass, center of mass point, and inertia
+matrix for a rigid body. This is 10 independent quantities altogether; however,
+inertia is mass-scaled making it linearly dependent on the mass. Here instead
+we represent inertia using a unit inertia matrix, which is equivalent to the
+inertia this body would have if it had unit mass. Then the actual inertia is
+given by mass*unitInertia. In this manner the mass, center of mass location, and
+inertia are completely independent so can be changed separately. That means
+if you double the mass, you'll also double the inertia as you would expect.
+
+Spatial inertia may be usefully viewed as a symmetric spatial matrix, that is,
+a 6x6 symmetric matrix arranged as 2x2 blocks of 3x3 matrices. Although this
+class represents the spatial inertia in compact form, it supports methods and
+operators that allow it to behave as though it were a spatial matrix (except
+much faster to work with). In spatial matrix form, the matrix has the following
+interpretation:
+<pre>
+ [ m*G m*px ]
+ M = [ ]
+ [ -m*px m*I ]
+</pre>
+Here m is mass, p is the vector from the body origin to the center of mass,
+G is the 3x3 symmetric unit inertia (gyration) matrix, and I is a 3x3 identity
+matrix. "px" indicates the skew symmetric cross product matrix formed from the
+vector p, so -px=~px.
+
+<h3>Abbreviations</h3>
+Typedefs exist for the most common invocations of SpatialInertia_\<P\>:
+ - \ref SimTK::SpatialInertia "SpatialInertia" for default Real precision (this is
+ almost always used)
+ - \ref SimTK::fSpatialInertia "fSpatialInertia" for single (float) precision
+ - \ref SimTK::dSpatialInertia "dSpatialInertia" for double precision **/
+template <class P>
+class SimTK_SimTKCOMMON_EXPORT SpatialInertia_ {
+ typedef P RealP;
+ typedef Vec<3,P> Vec3P;
+ typedef UnitInertia_<P> UnitInertiaP;
+ typedef Mat<3,3,P> Mat33P;
+ typedef Vec<2, Vec3P> SpatialVecP;
+ typedef Mat<2,2,Mat33P> SpatialMatP;
+ typedef Rotation_<P> RotationP;
+ typedef Transform_<P> TransformP;
+ typedef Inertia_<P> InertiaP;
+public:
+/// The default constructor fills everything with NaN, even in Release mode.
+SpatialInertia_()
+: m(nanP()), p(nanP()) {} // inertia is already NaN
+SpatialInertia_(RealP mass, const Vec3P& com, const UnitInertiaP& gyration)
+: m(mass), p(com), G(gyration) {}
+
+// default copy constructor, copy assignment, destructor
+
+SpatialInertia_& setMass(RealP mass)
+{ SimTK_ERRCHK1(mass >= 0, "SpatialInertia::setMass()",
+ "Negative mass %g is illegal.", (double)mass);
+ m=mass; return *this; }
+SpatialInertia_& setMassCenter(const Vec3P& com)
+{ p=com; return *this;}
+SpatialInertia_& setUnitInertia(const UnitInertiaP& gyration)
+{ G=gyration; return *this; }
+
+RealP getMass() const {return m;}
+const Vec3P& getMassCenter() const {return p;}
+const UnitInertiaP& getUnitInertia() const {return G;}
+
+/// Calculate the first mass moment (mass-weighted COM location)
+/// from the mass and COM vector. Cost is 3 inline flops.
+Vec3P calcMassMoment() const {return m*p;}
+
+/// Calculate the inertia matrix (second mass moment, mass-weighted gyration
+/// matrix) from the mass and unit inertia matrix. Cost is 6 inline flops.
+InertiaP calcInertia() const {return m*G;}
+
+/// Add in a compatible SpatialInertia. This is only valid if both
+/// SpatialInertias are expressed in the same frame and measured about
+/// the same point but there is no way for this method to check.
+/// Cost is about 40 flops.
+SpatialInertia_& operator+=(const SpatialInertia_& src) {
+ SimTK_ERRCHK(m+src.m != 0, "SpatialInertia::operator+=()",
+ "The combined mass cannot be zero.");
+ const RealP mtot = m+src.m, oomtot = 1/mtot; // ~11 flops
+ p = oomtot*(calcMassMoment() + src.calcMassMoment()); // 10 flops
+ G.setFromUnitInertia(oomtot*(calcInertia()+src.calcInertia())); // 19 flops
+ m = mtot; // must do this last
+ return *this;
+}
+
+/// Subtract off a compatible SpatialInertia. This is only valid if both
+/// SpatialInertias are expressed in the same frame and measured about
+/// the same point but there is no way for this method to check.
+/// Cost is about 40 flops.
+SpatialInertia_& operator-=(const SpatialInertia_& src) {
+ SimTK_ERRCHK(m != src.m, "SpatialInertia::operator-=()",
+ "The combined mass cannot be zero.");
+ const RealP mtot = m-src.m, oomtot = 1/mtot; // ~11 flops
+ p = oomtot*(calcMassMoment() - src.calcMassMoment()); // 10 flops
+ G.setFromUnitInertia(oomtot*(calcInertia()-src.calcInertia())); // 19 flops
+ m = mtot; // must do this last
+ return *this;
+}
+
+/// Multiply a SpatialInertia by a scalar. Because we keep the mass
+/// factored out, this requires only a single multiply.
+SpatialInertia_& operator*=(const RealP& s) {m *= s; return *this;}
+
+/// Divide a SpatialInertia by a scalar. Because we keep the mass
+/// factored out, this requires only a single divide.
+SpatialInertia_& operator/=(const RealP& s) {m /= s; return *this;}
+
+/// Multiply a SpatialInertia by a SpatialVec to produce a SpatialVec
+/// result; 45 flops.
+SpatialVecP operator*(const SpatialVecP& v) const
+{ return m*SpatialVecP(G*v[0]+p%v[1], v[1]-p%v[0]); }
+
+/// Return a new SpatialInertia object which is the same as this one except
+/// re-expressed in another coordinate frame. We consider this object to
+/// be expressed in some frame F and we're given a rotation matrix R_FB we
+/// can use to re-express in a new frame B. Cost is 72 flops.
+/// @see reexpressInPlace()
+SpatialInertia_ reexpress(const Rotation_<P>& R_FB) const
+{ return SpatialInertia_(*this).reexpressInPlace(R_FB); }
+
+/// Rexpress using an inverse rotation to avoid having to convert it.
+/// @see rexpress(Rotation) for information
+SpatialInertia_ reexpress(const InverseRotation_<P>& R_FB) const
+{ return SpatialInertia_(*this).reexpressInPlace(R_FB); }
+
+/// Re-express this SpatialInertia in another frame, modifying the original
+/// object. We return a reference to the object so that you can chain this
+/// operation in the manner of assignment operators. Cost is 72 flops.
+/// @see reexpress() if you want to leave this object unmolested.
+SpatialInertia_& reexpressInPlace(const Rotation_<P>& R_FB)
+{ p = (~R_FB)*p; G.reexpressInPlace(R_FB); return *this; }
+
+/// Rexpress using an inverse rotation to avoid having to convert it.
+/// @see rexpressInPlace(Rotation) for information
+SpatialInertia_& reexpressInPlace(const InverseRotation_<P>& R_FB)
+{ p = (~R_FB)*p; G.reexpressInPlace(R_FB); return *this; }
+
+/// Return a new SpatialInertia object which is the same as this one except
+/// the origin ("taken about" point) has changed from OF to OF+S.
+/// Cost is 37 flops.
+/// @see shiftInPlace()
+SpatialInertia_ shift(const Vec3P& S) const
+{ return SpatialInertia_(*this).shiftInPlace(S); }
+
+/// Change origin from OF to OF+S, modifying the original object in place.
+/// Returns a reference to the modified object so that you can chain this
+/// operation in the manner of assignment operators. Cost is 37 flops.
+/// @see shift() if you want to leave this object unmolested.
+SpatialInertia_& shiftInPlace(const Vec3P& S) {
+ G.shiftToCentroidInPlace(p); // change to central inertia
+ G.shiftFromCentroidInPlace(S); // now inertia is about S
+ p -= S; // was p=com-OF, now want p'=com-(OF+S)=p-S
+ return *this;
+}
+
+/// Return a new SpatialInertia object which is the same as this
+/// one but measured about and expressed in a new frame. We consider
+/// the current spatial inertia M to be measured (implicitly) in some
+/// frame F, that is, we have M=M_OF_F. We want M_OB_B for some new
+/// frame B, given the transform X_FB giving the location and orientation
+/// of B in F. This combines the reexpress() and shift() operations
+/// available separately. Cost is 109 flops.
+/// @see transformInPlace()
+SpatialInertia_ transform(const Transform_<P>& X_FB) const
+{ return SpatialInertia_(*this).transformInPlace(X_FB); }
+
+/// Transform using an inverse transform to avoid having to convert it.
+/// @see transform(Transform) for information
+SpatialInertia_ transform(const InverseTransform_<P>& X_FB) const
+{ return SpatialInertia_(*this).transformInPlace(X_FB); }
+
+/// Transform this SpatialInertia object so that it is measured about and
+/// expressed in a new frame, modifying the object in place. We consider the
+/// current spatial inertia M to be measured (implicitly) in some frame F, that
+/// is, we have M=M_OF_F. We want to change it to M_OB_B for some new frame B,
+/// given the transform X_FB giving the location and orientation of B in F. This
+/// combines the reexpressInPlace() and shiftInPlace() operations available
+/// separately. Returns a reference to the modified object so that you can
+/// chain this operation in the manner of assignment operators. Cost is 109 flops.
+/// @see transform() if you want to leave this object unmolested.
+SpatialInertia_& transformInPlace(const Transform_<P>& X_FB) {
+ shiftInPlace(X_FB.p()); // shift to the new origin OB.
+ reexpressInPlace(X_FB.R()); // get everything in B
+ return *this;
+}
+
+/// Transform using an inverse transform to avoid having to convert it.
+/// @see transformInPlace(Transform) for information
+SpatialInertia_& transformInPlace(const InverseTransform_<P>& X_FB) {
+ shiftInPlace(X_FB.p()); // shift to the new origin OB.
+ reexpressInPlace(X_FB.R()); // get everything in B
+ return *this;
+}
+
+const SpatialMatP toSpatialMat() const {
+ Mat33P offDiag = crossMat(m*p);
+ return SpatialMatP(m*G.toMat33(), offDiag,
+ -offDiag, Mat33P(m));
+}
+
+private:
+RealP m; // mass of this rigid body F
+Vec3P p; // location of body's COM from OF, expressed in F
+UnitInertiaP G; // mass distribution; inertia is mass*gyration
+
+static P nanP() {return NTraits<P>::getNaN();}
+};
+
+/// Add two compatible spatial inertias. Cost is about 40 flops.
+/// @relates SpatialInertia_
+template <class P> inline SpatialInertia_<P>
+operator+(const SpatialInertia_<P>& l, const SpatialInertia_<P>& r)
+{ return SpatialInertia_<P>(l) += r; }
+
+/// Subtract one compatible spatial inertia from another. Cost is
+/// about 40 flops.
+/// @relates SpatialInertia_
+template <class P> inline SpatialInertia_<P>
+operator-(const SpatialInertia_<P>& l, const SpatialInertia_<P>& r)
+{ return SpatialInertia_<P>(l) -= r; }
+
+
+// -----------------------------------------------------------------------------
+// ARTICULATED BODY INERTIA MATRIX
+// -----------------------------------------------------------------------------
+/** An articulated body inertia (ABI) matrix P(q) contains the spatial inertia
+properties that a body appears to have when it is the free base body of
+an articulated multibody tree in a given configuration q. Despite the
+complex relative motion that occurs within a multibody tree, at any given
+configuration q there is still a linear relationship between a spatial
+force F applied to a point of the base body and the resulting acceleration
+A of that body and that point: F = P(q)*A + c, where c is a velocity-
+dependent inertial bias force. P is thus analogous to a rigid body
+spatial inertia (RBI), but for a body which has other bodies connected to it
+by joints which are free to move.
+
+An ABI P is a symmetric 6x6 spatial matrix, consisting of 2x2 blocks of 3x3
+matrices, similar to the RBI. However, unlike the RBI which has only 10 independent
+elements, all 21 elements of P's lower triangle are significant. For example,
+the apparent mass of an articulated body depends on which way you push it,
+and in general there is no well-defined center of mass. This
+is a much more expensive matrix to manipulate than an RBI. In Simbody's formulation,
+we only work with ABIs in the Ground frame, so there
+is never a need to rotate or re-express them. (That is done by rotating RBIs
+prior to using them to construct the ABIs.) Thus only shifting operations need
+be performed when transforming ABIs from body to body.
+Cheap rigid body shifting is done when moving an ABI
+within a body or across a prescribed mobilizer; otherwise we have to perform
+an articulated shift operation which is quite expensive.
+
+For a full discussion of the properties of articulated body inertias, see
+Section 7.1 (pp. 119-123) of Roy Featherstone's excellent 2008 book, Rigid
+Body Dynamics Algorithms.
+
+In spatial matrix form, an ABI P may be considered to consist of the
+following 3x3 subblocks:
+<pre>
+ P = [ J F ]
+ [~F M ]
+</pre>
+Here M is a (symmetric) mass distribution, F is a full matrix giving the
+first mass moment distribution, and J is a (symmetric) inertia matrix.
+
+<h3>Abbreviations</h3>
+Typedefs exist for the most common invocations of ArticulatedInertia_\<P\>:
+ - \ref SimTK::ArticulatedInertia "ArticulatedInertia" for default Real
+ precision (this is almost always used)
+ - \ref SimTK::fArticulatedInertia "fArticulatedInertia" for single (float)
+ precision
+ - \ref SimTK::dArticulatedInertia "dArticulatedInertia" for double
+ precision **/
+template <class P>
+class ArticulatedInertia_ {
+ typedef P RealP;
+ typedef Vec<3,P> Vec3P;
+ typedef UnitInertia_<P> UnitInertiaP;
+ typedef Mat<3,3,P> Mat33P;
+ typedef SymMat<3,P> SymMat33P;
+ typedef Vec<2, Vec3P> SpatialVecP;
+ typedef Mat<2,2,Mat33P> SpatialMatP;
+ typedef Rotation_<P> RotationP;
+ typedef Transform_<P> TransformP;
+ typedef Inertia_<P> InertiaP;
+public:
+/// Default contruction produces uninitialized junk at zero cost; be sure to
+/// fill this in before referencing it.
+ArticulatedInertia_() {}
+/// Construct an ArticulatedInertia from the mass, first moment, and inertia matrices it contains.
+ArticulatedInertia_(const SymMat33P& mass, const Mat33P& massMoment, const SymMat33P& inertia)
+: M(mass), J(inertia), F(massMoment) {}
+
+/// Construct an articulated body inertia (ABI) from a rigid body spatial inertia (RBI).
+/// Every RBI is also the ABI for that (unarticulated) rigid body. 12 flops.
+explicit ArticulatedInertia_(const SpatialInertia_<P>& rbi)
+: M(rbi.getMass()), J(rbi.calcInertia()), F(crossMat(rbi.calcMassMoment())) {}
+
+/// Set the mass distribution matrix M in this ArticulatedInertia (symmetric).
+ArticulatedInertia_& setMass (const SymMat33P& mass) {M=mass; return *this;}
+/// Set the mass first moment distribution matrix F in this ArticulatedInertia (full).
+ArticulatedInertia_& setMassMoment(const Mat33P& massMoment) {F=massMoment; return *this;}
+/// Set the mass second moment (inertia) matrix J in this ArticulatedInertia (symmetric).
+ArticulatedInertia_& setInertia (const SymMat33P& inertia) {J=inertia; return *this;}
+
+/// Get the mass distribution matrix M from this ArticulatedInertia (symmetric).
+const SymMat33P& getMass() const {return M;}
+/// Get the mass first moment distribution matrix F from this ArticulatedInertia (full).
+const Mat33P& getMassMoment() const {return F;}
+/// Get the mass second moment (inertia) matrix J from this ArticulatedInertia (symmetric).
+const SymMat33P& getInertia() const {return J;}
+
+// default destructor, copy constructor, copy assignment
+
+/// Add in a compatible ArticulatedInertia to this one. Both inertias must be expressed
+/// in the same frame and measured about the same point. 21 flops.
+ArticulatedInertia_& operator+=(const ArticulatedInertia_& src)
+{ M+=src.M; J+=src.J; F+=src.F; return *this; }
+/// Subtract a compatible ArticulatedInertia from this one. Both inertias must be expressed
+/// in the same frame and measured about the same point. 21 flops.
+ArticulatedInertia_& operator-=(const ArticulatedInertia_& src)
+{ M-=src.M; J-=src.J; F-=src.F; return *this; }
+
+/// Multiply an ArticulatedIneria by a SpatialVec (66 flops).
+SpatialVecP operator*(const SpatialVecP& v) const
+{ return SpatialVecP(J*v[0]+F*v[1], ~F*v[0]+M*v[1]); }
+
+/// Multiply an ArticulatedInertia by a row of SpatialVecs (66*N flops).
+template <int N>
+Mat<2,N,Vec3P> operator*(const Mat<2,N,Vec3P>& m) const {
+ Mat<2,N,Vec3P> res;
+ for (int j=0; j < N; ++j)
+ res.col(j) = (*this) * m.col(j); // above this*SpatialVec operator
+ return res;
+}
+
+/// Rigid-shift the origin of this Articulated Body Inertia P by a
+/// shift vector s to produce a new ABI P'. The calculation is
+/// <pre>
+/// P' = [ J' F' ] = [ 1 sx ] [ J F ] [ 1 0 ]
+/// [~F' M ] [ 0 1 ] [~F M ] [-sx 1 ]
+/// </pre>
+/// where sx is the cross product matrix of s. Cost is 72 flops.
+SimTK_SimTKCOMMON_EXPORT ArticulatedInertia_ shift(const Vec3P& s) const;
+
+/// Rigid-shift this ABI in place. 72 flops.
+/// @see shift() for details
+SimTK_SimTKCOMMON_EXPORT ArticulatedInertia_& shiftInPlace(const Vec3P& s);
+
+/// Convert the compactly-stored ArticulatedInertia (21 elements) into a
+/// full SpatialMat with 36 elements.
+const SpatialMatP toSpatialMat() const {
+ return SpatialMatP( Mat33P(J), F,
+ ~F, Mat33P(M) );
+}
+private:
+SymMat33P M;
+SymMat33P J;
+Mat33P F;
+};
+
+/// Add two compatible articulated inertias. Cost is 21 flops.
+/// @relates ArticulatedInertia_
+template <class P> inline ArticulatedInertia_<P>
+operator+(const ArticulatedInertia_<P>& l, const ArticulatedInertia_<P>& r)
+{ return ArticulatedInertia_<P>(l) += r; }
+
+/// Subtract one compatible articulated inertia from another. Cost is
+/// 21 flops.
+/// @relates ArticulatedInertia_
+template <class P> inline ArticulatedInertia_<P>
+operator-(const ArticulatedInertia_<P>& l, const ArticulatedInertia_<P>& r)
+{ return ArticulatedInertia_<P>(l) -= r; }
+
+
+// -----------------------------------------------------------------------------
+// MASS PROPERTIES
+// -----------------------------------------------------------------------------
+/** This class contains the mass, center of mass, and unit inertia matrix of a
+rigid body B. The center of mass is a vector from B's origin, expressed in the
+B frame. The inertia is taken about the B origin, and expressed in B. The frame
+B is implicit; only the measurements are stored here. The unit inertia can be
+multiplied by the mass to get the inertia matrix for the body.
+
+<h3>Abbreviations</h3>
+Typedefs exist for the most common invocations of MassProperties_\<P\>:
+ - \ref SimTK::MassProperties "MassProperties" for default Real precision (this is
+ almost always used)
+ - \ref SimTK::fMassProperties "fMassProperties" for single (float) precision
+ - \ref SimTK::dMassProperties "dMassProperties" for double precision **/
+template <class P>
+class SimTK_SimTKCOMMON_EXPORT MassProperties_ {
+ typedef P RealP;
+ typedef Vec<3,P> Vec3P;
+ typedef UnitInertia_<P> UnitInertiaP;
+ typedef Mat<3,3,P> Mat33P;
+ typedef Mat<6,6,P> Mat66P;
+ typedef SymMat<3,P> SymMat33P;
+ typedef Mat<2,2,Mat33P> SpatialMatP;
+ typedef Rotation_<P> RotationP;
+ typedef Transform_<P> TransformP;
+ typedef Inertia_<P> InertiaP;
+public:
+/** Create a mass properties object in which the mass, mass center, and
+inertia are meaningless; you must assign values before using this. **/
+MassProperties_() { setMassProperties(0,Vec3P(0),UnitInertiaP()); }
+/** Create a mass properties object from individually supplied mass,
+mass center, and inertia matrix. The inertia matrix is divided by the
+mass to produce the unit inertia. **/
+MassProperties_(const RealP& m, const Vec3P& com, const InertiaP& inertia)
+ { setMassProperties(m,com,inertia); }
+/** Create a mass properties object from individually supplied mass,
+mass center, and unit inertia (gyration) matrix. **/
+MassProperties_(const RealP& m, const Vec3P& com, const UnitInertiaP& gyration)
+ { setMassProperties(m,com,gyration); }
+
+/** Set mass, center of mass, and inertia. The inertia is divided by the mass to
+produce the unit inertia. Behaves like an assignment in that
+a reference to the modified MassProperties object is returned. **/
+MassProperties_& setMassProperties(const RealP& m, const Vec3P& com, const InertiaP& inertia) {
+ mass = m;
+ comInB = com;
+ if (m == 0) {
+ SimTK_ASSERT(inertia.asSymMat33().getAsVec() == 0, "Mass is zero but inertia is nonzero");
+ unitInertia_OB_B = UnitInertiaP(0);
+ }
+ else {
+ unitInertia_OB_B = UnitInertiaP(inertia*(1/m));
+ }
+ return *this;
+}
+
+/** Set mass, center of mass, and unit inertia. Behaves like an assignment in
+that a reference to the modified MassProperties object is returned. **/
+MassProperties_& setMassProperties
+ (const RealP& m, const Vec3P& com, const UnitInertiaP& gyration)
+{ mass=m; comInB=com; unitInertia_OB_B=gyration; return *this; }
+
+/** Return the mass currently stored in this MassProperties object. **/
+const RealP& getMass() const {return mass;}
+/** Return the mass center currently stored in this MassProperties object;
+this is expressed in an implicit frame we call "B", and measured from B's
+origin, but you have to know what that frame is in order to interpret the
+returned vector. **/
+const Vec3P& getMassCenter() const {return comInB;}
+/** Return the unit inertia currently stored in this MassProperties object;
+this is expressed in an implicit frame we call "B", and measured about B's
+origin, but you have to know what that frame is in order to interpret the
+returned value correctly. **/
+const UnitInertiaP& getUnitInertia() const {return unitInertia_OB_B;}
+/** Return the inertia matrix for this MassProperties object; this is equal
+to the unit inertia times the mass and costs 6 flops. It is expressed in
+an implicit frame we call "B", and measured about B's origin, but you have
+to know what that frame is in order to interpret the returned value
+correctly. **/
+const InertiaP calcInertia() const {return mass*unitInertia_OB_B;}
+/** OBSOLETE -- this is just here for compatibility with 2.2; since the
+UnitInertia is stored now the full inertia must be calculated at a cost of
+6 flops, hence the method name should be calcInertia() and that's what
+you should use unless you want getUnitInertia(). **/
+const InertiaP getInertia() const {return calcInertia();}
+
+/** Return the inertia of this MassProperties object, but measured about the
+mass center rather than about the (implicit) B frame origin. The result is
+still expressed in B. **/
+InertiaP calcCentralInertia() const {
+ return mass*unitInertia_OB_B - InertiaP(comInB, mass);
+}
+/** Return the inertia of this MassProperties object, but with the "measured
+about" point shifted from the (implicit) B frame origin to a new point that
+is supplied in \a newOriginB which must be a vector measured from the B frame
+origin and expressed in B. The result is still expressed in B. **/
+InertiaP calcShiftedInertia(const Vec3P& newOriginB) const {
+ return calcCentralInertia() + InertiaP(newOriginB-comInB, mass);
+}
+/** Return the inertia of this MassProperties object, but transformed to
+from the implicit B frame to a new frame C whose pose relative to B is
+supplied. Note that this affects both the "measured about" point and the
+"expressed in" frame. **/
+InertiaP calcTransformedInertia(const TransformP& X_BC) const {
+ return calcShiftedInertia(X_BC.p()).reexpress(X_BC.R());
+}
+/** Return a new MassProperties object that is the same as this one but with
+the origin point shifted from the (implicit) B frame origin to a new point that
+is supplied in \a newOriginB which must be a vector measured from the B frame
+origin and expressed in B. This affects both the mass center vector and the
+inertia. The result is still expressed in B. **/
+MassProperties_ calcShiftedMassProps(const Vec3P& newOriginB) const {
+ return MassProperties_(mass, comInB-newOriginB,
+ calcShiftedInertia(newOriginB));
+}
+
+/** %Transform these mass properties from the current frame "B" to a new
+frame "C", given the pose of C in B.\ Caution: this \e shifts
+the point from which the mass properties are measured from the origin of B to
+the origin of C. See reexpress() to change only the measure numbers without
+moving the "measured from" point. Note that the frame in which a MassProperties
+object is expressed, and the point about which the mass properties are
+measured, are implicit; we don't actually have any way to verify that
+it is in B. Make sure you are certain about the current frame before using this
+method. **/
+MassProperties_ calcTransformedMassProps(const TransformP& X_BC) const {
+ return MassProperties_(mass, ~X_BC*comInB, calcTransformedInertia(X_BC));
+}
+
+/** Re-express these mass properties from the current frame "B" to a new
+frame "C", given the orientation of C in B.\ Caution: this does not \e shift
+the point from which the mass properties are measured, it just uses a different
+frame to express that measurement. See calcTransformedMassProps() to perform
+a shift as well. Note that the frame in which a MassProperties object is
+expressed is implicit; we don't actually have any way to verify that it is in
+B. Make sure you are certain about the current frame before using this
+method. **/
+MassProperties_ reexpress(const RotationP& R_BC) const {
+ return MassProperties_(mass, ~R_BC*comInB, unitInertia_OB_B.reexpress(R_BC));
+}
+
+/** Return true only if the mass stored here is \e exactly zero.\ If the mass
+resulted from a computation, you should use isNearlyMassless() instead.
+ at see isNearlyMassless(), isExactlyCentral() **/
+bool isExactlyMassless() const { return mass==0; }
+/** Return true if the mass stored here is zero to within a small tolerance.
+By default we use SignificantReal (about 1e-14 in double precision) as the
+tolerance but you can override that. If you are just checking to see whether
+the mass was explicitly set to zero (rather than calculated) you can use
+isExactlyMassless() instead. @see isExactlyMassless(), isNearlyCentral() **/
+bool isNearlyMassless(const RealP& tol=SignificantReal) const {
+ return mass <= tol;
+}
+
+/** Return true only if the mass center stored here is \e exactly zero.\ If
+the mass center resulted from a computation, you should use isNearlyCentral()
+instead. @see isNearlyCentral(), isExactlyMassless() **/
+bool isExactlyCentral() const { return comInB==Vec3P(0); }
+/** Return true if the mass center stored here is zero to within a small tolerance.
+By default we use SignificantReal (about 1e-14 in double precision) as the
+tolerance but you can override that. If you are just checking to see whether
+the mass center was explicitly set to zero (rather than calculated) you can use
+isExactlyCentral() instead. @see isExactlyCentral(), isNearlyMassless() **/
+bool isNearlyCentral(const RealP& tol=SignificantReal) const {
+ return comInB.normSqr() <= tol*tol;
+}
+
+/** Return true if any element of this MassProperties object is NaN.
+ at see isInf(), isFinite() **/
+bool isNaN() const {return SimTK::isNaN(mass) || comInB.isNaN() || unitInertia_OB_B.isNaN();}
+/** Return true only if there are no NaN's in this MassProperties object, and
+at least one of the elements is Infinity.\ Ground's mass properties satisfy
+these conditions.
+ at see isNan(), isFinite() **/
+bool isInf() const {
+ if (isNaN()) return false;
+ return SimTK::isInf(mass) || comInB.isInf() || unitInertia_OB_B.isInf();
+}
+/** Return true if none of the elements of this MassProperties object are
+NaN or Infinity.\ Note that Ground's mass properties are not finite.
+ at see isNaN(), isInf() **/
+bool isFinite() const {
+ return SimTK::isFinite(mass) && comInB.isFinite() && unitInertia_OB_B.isFinite();
+}
+
+/** Convert this MassProperties object to a spatial inertia matrix and return
+it as a SpatialMat, which is a 2x2 matrix of 3x3 submatrices.
+ at see toMat66() **/
+SpatialMatP toSpatialMat() const {
+ SpatialMatP M;
+ M(0,0) = mass*unitInertia_OB_B.toMat33();
+ M(0,1) = mass*crossMat(comInB);
+ M(1,0) = ~M(0,1);
+ M(1,1) = mass; // a diagonal matrix
+ return M;
+}
+
+/** Convert this MassProperties object to a spatial inertia matrix in the
+form of an ordinary 6x6 matrix, \e not a SpatialMat. Logically these are
+the same but the ordering of the elements in memory is different between
+a Mat66 and SpatialMat.
+ at see toSpatialMat() **/
+Mat66P toMat66() const {
+ Mat66P M;
+ M.template updSubMat<3,3>(0,0) = mass*unitInertia_OB_B.toMat33();
+ M.template updSubMat<3,3>(0,3) = mass*crossMat(comInB);
+ M.template updSubMat<3,3>(3,0) = ~M.template getSubMat<3,3>(0,3);
+ M.template updSubMat<3,3>(3,3) = mass; // a diagonal matrix
+ return M;
+}
+
+private:
+RealP mass;
+Vec3P comInB; // meas. from B origin, expr. in B
+UnitInertiaP unitInertia_OB_B; // about B origin, expr. in B
+};
+
+/** Output a human-readable representation of a MassProperties object to
+the given output stream. **/
+template <class P> static inline std::ostream&
+operator<<(std::ostream& o, const MassProperties_<P>& mp) {
+ return o << "{ mass=" << mp.getMass()
+ << "\n com=" << mp.getMassCenter()
+ << "\n Ixx,yy,zz=" << mp.getUnitInertia().getMoments()
+ << "\n Ixy,xz,yz=" << mp.getUnitInertia().getProducts()
+ << "\n}\n";
+}
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATRIX_MASS_PROPERTIES_H_
diff --git a/SimTKcommon/Mechanics/include/SimTKcommon/internal/Quaternion.h b/SimTKcommon/Mechanics/include/SimTKcommon/internal/Quaternion.h
new file mode 100644
index 0000000..c0f8651
--- /dev/null
+++ b/SimTKcommon/Mechanics/include/SimTKcommon/internal/Quaternion.h
@@ -0,0 +1,175 @@
+#ifndef SimTK_SimTKCOMMON_QUATERNION_H
+#define SimTK_SimTKCOMMON_QUATERNION_H
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Paul Mitiguy *
+ * Contributors: *
+ * *
+ * 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/SmallMatrix.h"
+#include "SimTKcommon/internal/UnitVec.h"
+//-----------------------------------------------------------------------------
+#include <iosfwd> // Forward declaration of iostream
+//-----------------------------------------------------------------------------
+
+
+//-----------------------------------------------------------------------------
+namespace SimTK {
+
+//-----------------------------------------------------------------------------
+// Forward declarations
+template <class P> class Rotation_;
+template <class P> class Quaternion_;
+
+typedef Quaternion_<Real> Quaternion;
+typedef Quaternion_<float> fQuaternion;
+typedef Quaternion_<double> dQuaternion;
+
+//-----------------------------------------------------------------------------
+/**
+ * A Quaternion is a Vec4 with the following behavior:
+ * - its length is always 1 (or else it is all NaN)
+ * - it is equivalent to an angle/axis rotation for
+ * angle a, axis unit vector v, as: q = [ cos(a/2) sin(a/2)*v ]
+ * A quaternion is in "canonical form" when its first element is nonnegative.
+ * This corresponds to rotation angles in the range -180 < a <= 180 degrees.
+ * Quaternions are not required to be in canonical form (e.g., during numerical
+ * integration). When appropriate, they are put in canonical form.
+ *
+ * Conversion from quaternion to (angle,axis) form is handled here also.
+ * (angle,axis) is in canonical form when -180 < angle <= 180 and |axis|=1.
+ * However, (angle,axis) is meaningful for any angle and for any axis where
+ * |axis| > 0.
+ */
+//-----------------------------------------------------------------------------
+template <class P>
+class Quaternion_ : public Vec<4,P> {
+ typedef P RealP;
+ typedef Vec<3,P> Vec3P;
+ typedef Vec<4,P> Vec4P;
+public:
+ /// Default constructor produces the ZeroRotation quaternion [1 0 0 0]
+ /// (not NaN - even in debug mode).
+ Quaternion_() : Vec4P(1,0,0,0) { }
+
+ /// Zero-cost copy constructor just copies the source without conversion to
+ /// canonical form or normalization.
+ Quaternion_(const Quaternion_& q) : Vec4P(q) {}
+
+ /// Zero-cost copy assignment just copies the source without conversion to
+ /// canonical form or normalization.
+ Quaternion_& operator=( const Quaternion_& q )
+ { Vec4P::operator=(q.asVec4()); return *this; }
+
+ /// Construct a quaternion from four scalars and normalize the result,
+ /// which costs about 40 flops.
+ Quaternion_( RealP e0, RealP e1, RealP e2, RealP e3 ) : Vec4P(e0,e1,e2,e3)
+ { normalizeThis(); }
+ /// Construct a quaternion from a 4-vector and normalize the result,
+ /// which costs about 40 flops.
+ explicit Quaternion_( const Vec4P& q ) : Vec4P(q)
+ { normalizeThis(); }
+
+ /// Constructs a canonical quaternion from a rotation matrix (cost is
+ /// about 60 flops).
+ SimTK_SimTKCOMMON_EXPORT explicit Quaternion_(const Rotation_<P>&);
+
+ /// The ZeroRotation quaternion is [1 0 0 0]. Note: Default constructor
+ /// is ZeroRotation (unlike Vec4P which start as NaN in Debug mode).
+ void setQuaternionToZeroRotation() {Vec4P::operator=( Vec4P(1,0,0,0) );}
+ /// Set quaternion to all-NaN. Note that this is not the same as produced
+ /// by default construction, even in Debug mode -- default construction
+ /// always produces an identity rotation of [1 0 0 0].
+ void setQuaternionToNaN() {Vec4P::setToNaN();}
+
+ /// Set this quaternion from an angle-axis rotation packed into a 4-vector
+ /// as [a vx vy vz]. The result will be put in canonical form, i.e.,
+ /// it will have a non-negative first element. If the "axis" portion of av
+ /// is a zero vector on input, the quaternion is set to all-NaN.
+ SimTK_SimTKCOMMON_EXPORT void setQuaternionFromAngleAxis(const Vec4P& av);
+ /// Set this quaternion from an angle-axis rotation provided as an angle a
+ /// and a separate unit vector [vx vy vz]. The result will be put in
+ /// canonical form, i.e., it will have a non-negative first element.
+ SimTK_SimTKCOMMON_EXPORT void setQuaternionFromAngleAxis(const RealP& a, const UnitVec<P,1>& v);
+
+ /// Returns [ a vx vy vz ] with (a,v) in canonical form, i.e.,
+ /// -180 < a <= 180 and |v|=1.
+ SimTK_SimTKCOMMON_EXPORT Vec4P convertQuaternionToAngleAxis() const;
+
+ /// Zero-cost cast of a Quaternion_ to its underlying Vec4; this is \e not
+ /// converted to axis-angle form.
+ const Vec4P& asVec4() const {return *static_cast<const Vec4P*>(this);}
+
+ /// Normalize an already constructed quaternion in place; but do you really
+ /// need to do this? Quaternions should be kept normalized at all times.
+ /// One of the advantages of using them is that you don't have to check if
+ /// they are normalized or renormalize them. However, under some situations
+ /// they do need renormalization, but it is costly if you don't actually
+ /// need it. If the quaternion is \e exactly zero, set it to [1 0 0 0]. If
+ /// its magnitude is 0 < magnitude < epsilon (epsilon is machine
+ /// tolerance), set it to NaN (treated as an error). Otherwise, normalize
+ /// the quaternion which costs about 40 flops. The quaternion is NOT put
+ /// in canonical form.
+ Quaternion_& normalizeThis() {
+ const RealP epsilon = std::numeric_limits<RealP>::epsilon();
+ const RealP magnitude = Vec4P::norm();
+ if( magnitude == 0 ) setQuaternionToZeroRotation();
+ else if( magnitude < epsilon ) setQuaternionToNaN();
+ else (*this) *= (1/magnitude);
+ return *this;
+ }
+
+ /// Return a normalized copy of this quaternion; but do you really need to
+ /// do this? Quaternions should be kept normalized at all times. One of
+ /// the advantages of using them is that you don't have to check if they
+ /// are normalized or renormalize them. However, under some situations they
+ /// do need renormalization, but it is costly if you don't actually need it.
+ /// @see normalizeThis() for details.
+ Quaternion_ normalize() const {return Quaternion_(*this).normalizeThis();}
+
+ /// Use this constructor only if you are *sure* v is normalized to 1.0.
+ /// This zero cost method is faster than the Quaternion(Vec4) constructor
+ /// which normalizes the Vec4. The second argument forces the compiler to
+ /// call the fast constructor; it is otherwise ignored. By convention, set
+ /// the second argument to "true".
+ Quaternion_(const Vec4P& v, bool) : Vec4P(v) {}
+
+//----------------------------------------------------------------------------------------------------
+// The following code is obsolete - it is here temporarily for backward compatibility (Mitiguy 10/13/2007)
+//----------------------------------------------------------------------------------------------------
+private:
+ Vec4P convertToAngleAxis() const { return convertQuaternionToAngleAxis();}
+ void setToAngleAxis(const Vec4P& av) { setQuaternionFromAngleAxis(av);}
+ void setToAngleAxis(const RealP& a, const UnitVec<P,1>& v){ setQuaternionFromAngleAxis(a,v);}
+ void setToNaN() { setQuaternionToNaN(); }
+ void setToZero() { setQuaternionToZeroRotation();}
+
+};
+
+
+//------------------------------------------------------------------------------
+} // End of namespace SimTK
+
+//--------------------------------------------------------------------------
+#endif // SimTK_SimTKCOMMON_QUATERNION_H_
+//--------------------------------------------------------------------------
+
diff --git a/SimTKcommon/Mechanics/include/SimTKcommon/internal/Rotation.h b/SimTKcommon/Mechanics/include/SimTKcommon/internal/Rotation.h
new file mode 100644
index 0000000..a682867
--- /dev/null
+++ b/SimTKcommon/Mechanics/include/SimTKcommon/internal/Rotation.h
@@ -0,0 +1,1185 @@
+#ifndef SimTK_SimTKCOMMON_ROTATION_H_
+#define SimTK_SimTKCOMMON_ROTATION_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Paul Mitiguy, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/SmallMatrix.h"
+#include "SimTKcommon/internal/CoordinateAxis.h"
+#include "SimTKcommon/internal/UnitVec.h"
+#include "SimTKcommon/internal/Quaternion.h"
+
+//------------------------------------------------------------------------------
+#include <iosfwd> // Forward declaration of iostream
+//------------------------------------------------------------------------------
+
+//------------------------------------------------------------------------------
+namespace SimTK {
+
+
+enum BodyOrSpaceType { BodyRotationSequence=0, SpaceRotationSequence=1 };
+
+//------------------------------------------------------------------------------
+// Forward declarations
+template <class P> class Rotation_;
+template <class P> class InverseRotation_;
+
+typedef Rotation_<Real> Rotation;
+typedef Rotation_<float> fRotation;
+typedef Rotation_<double> dRotation;
+
+typedef InverseRotation_<Real> InverseRotation;
+typedef InverseRotation_<float> fInverseRotation;
+typedef InverseRotation_<double> dInverseRotation;
+
+//------------------------------------------------------------------------------
+/** The Rotation class is a Mat33 that guarantees that the matrix can be
+interpreted as a legitimate 3x3 rotation matrix giving the relative orientation
+of two right-handed, orthogonal, unit vector bases.
+
+A rotation matrix, also known as a direction cosine matrix, is an orthogonal
+matrix whose columns and rows are directions (that is, unit vectors) that are
+mutually orthogonal. Furthermore, if the columns (or rows) are labeled x,y,z it
+always holds that z = x X y (rather than -(x X y)) ensuring that this is a
+right-handed rotation matrix and not a reflection. This is equivalent to saying
+that the determinant of a rotation matrix is 1, not -1.
+
+The Rotation class takes advantage of known properties of orthogonal matrices.
+For example, multiplication by a rotation matrix preserves a vector's length so
+unit vectors are still unit vectors afterwards and don't need to be
+re-normalized.
+
+Suppose there is a vector v_F expressed in terms of the right-handed,
+orthogonal unit vectors Fx, Fy, Fz and one would like to express v instead
+as v_G, in terms of a right-handed, orthogonal unit vectors Gx, Gy, Gz. To
+calculate it, we form a rotation matrix R_GF whose columns are the F unit
+vectors re-expressed in G:
+<pre>
+ G F ( | | )
+ R_GF = R = ( Fx_G | Fy_G | Fz_G )
+ ( | | )
+where
+ Fx_G = ~( ~Fx*Gx, ~Fx*Gy, ~Fx*Gz ), etc.
+</pre>
+(~Fx*Gx means dot(Fx,Gx)). Note that we use "monogram" notation R_GF in
+code to represent the more typographically demanding superscripted notation
+for rotation matrices. Now we can re-express the vector v from frame F to
+frame G via
+<pre>
+ v_G = R_GF * v_F.
+</pre>
+Because a rotation is orthogonal, its transpose is its inverse. Hence
+R_FG = ~R_GF (where ~ is the SimTK "transpose" operator). This transpose
+matrix can be used to expressed v_G in terms of Fx, Fy, Fz as
+<pre>
+ v_F = R_FG * v_G or v_F = ~R_GF * v_G
+</pre>
+In either direction, correct behavior can be obtained by using the
+recommended notation and then matching up the frame labels (after
+interpreting the "~" operator as reversing the labels).
+
+The Rotation_ class is templatized by the precision P, which should be float
+or double. A typedef defining type Rotation as Rotation_<Real> is always
+defined and is normally used in user programs rather than the templatized class.
+**/
+//------------------------------------------------------------------------------
+template <class P> // templatized by precision
+class Rotation_ : public Mat<3,3,P> {
+public:
+ typedef P RealP; ///< These are just local abbreviations.
+ typedef Mat<2,2,P> Mat22P;
+ typedef Mat<3,2,P> Mat32P;
+ typedef Mat<3,3,P> Mat33P;
+ typedef Mat<4,3,P> Mat43P;
+ typedef Mat<3,4,P> Mat34P;
+ typedef Vec<2,P> Vec2P;
+ typedef Vec<3,P> Vec3P;
+ typedef Vec<4,P> Vec4P;
+ typedef UnitVec<P,1> UnitVec3P; // stride is 1 here, length is always 3
+ typedef SymMat<3,P> SymMat33P;
+ typedef Quaternion_<P> QuaternionP;
+
+ // Default constructor and constructor-like methods
+ Rotation_() : Mat33P(1) {}
+ Rotation_& setRotationToIdentityMatrix() { Mat33P::operator=(RealP(1)); return *this; }
+ Rotation_& setRotationToNaN() { Mat33P::setToNaN(); return *this; }
+
+ // Default copy constructor and assignment operator
+ Rotation_( const Rotation_& R ) : Mat33P(R) {}
+ Rotation_& operator=( const Rotation_& R ) { Mat33P::operator=( R.asMat33() ); return *this; }
+
+ /// Constructor for right-handed rotation by an angle (in radians) about a coordinate axis.
+ //@{
+ Rotation_( RealP angle, const CoordinateAxis& axis ) { setRotationFromAngleAboutAxis( angle, axis ); }
+ Rotation_( RealP angle, const CoordinateAxis::XCoordinateAxis ) { setRotationFromAngleAboutX( std::cos(angle), std::sin(angle) ); }
+ Rotation_( RealP angle, const CoordinateAxis::YCoordinateAxis ) { setRotationFromAngleAboutY( std::cos(angle), std::sin(angle) ); }
+ Rotation_( RealP angle, const CoordinateAxis::ZCoordinateAxis ) { setRotationFromAngleAboutZ( std::cos(angle), std::sin(angle) ); }
+ //@}
+ /// Set this Rotation_ object to a right-handed rotation by an angle (in radians) about a coordinate axis.
+ //@{
+ Rotation_& setRotationFromAngleAboutAxis( RealP angle, const CoordinateAxis& axis ) { return axis.isXAxis() ? setRotationFromAngleAboutX(angle) : (axis.isYAxis() ? setRotationFromAngleAboutY(angle) : setRotationFromAngleAboutZ(angle) ); }
+ Rotation_& setRotationFromAngleAboutX( RealP angle ) { return setRotationFromAngleAboutX( std::cos(angle), std::sin(angle) ); }
+ Rotation_& setRotationFromAngleAboutY( RealP angle ) { return setRotationFromAngleAboutY( std::cos(angle), std::sin(angle) ); }
+ Rotation_& setRotationFromAngleAboutZ( RealP angle ) { return setRotationFromAngleAboutZ( std::cos(angle), std::sin(angle) ); }
+ Rotation_& setRotationFromAngleAboutX( RealP cosAngle, RealP sinAngle ) { Mat33P& R = *this; R[0][0] = 1; R[0][1] = R[0][2] = R[1][0] = R[2][0] = 0; R[1][1] = R[2][2] = cosAngle; R[1][2] = -(R[2][1] = sinAngle); return *this; }
+ Rotation_& setRotationFromAngleAboutY( RealP cosAngle, RealP sinAngle ) { Mat33P& R = *this; R[1][1] = 1; R[0][1] = R[1][0] = R[1][2] = R[2][1] = 0; R[0][0] = R[2][2] = cosAngle; R[2][0] = -(R[0][2] = sinAngle); return *this; }
+ Rotation_& setRotationFromAngleAboutZ( RealP cosAngle, RealP sinAngle ) { Mat33P& R = *this; R[2][2] = 1; R[0][2] = R[1][2] = R[2][0] = R[2][1] = 0; R[0][0] = R[1][1] = cosAngle; R[0][1] = -(R[1][0] = sinAngle); return *this; }
+ //@}
+
+ /// Constructor for right-handed rotation by an angle (in radians) about an arbitrary vector.
+ //@{
+ Rotation_( RealP angle, const UnitVec3P& unitVector ) { setRotationFromAngleAboutUnitVector(angle,unitVector); }
+ Rotation_( RealP angle, const Vec3P& nonUnitVector ) { setRotationFromAngleAboutNonUnitVector(angle,nonUnitVector); }
+ //@}
+ /// Set this Rotation_ object to a right-handed rotation of an angle (in radians) about an arbitrary vector.
+ //@{
+ Rotation_& setRotationFromAngleAboutNonUnitVector( RealP angle, const Vec3P& nonUnitVector ) { return setRotationFromAngleAboutUnitVector( angle, UnitVec3P(nonUnitVector) ); }
+ SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromAngleAboutUnitVector( RealP angle, const UnitVec3P& unitVector );
+ //@}
+
+ /// Constructor for two-angle, two-axes, Body-fixed or Space-fixed rotation sequences (angles are in radians)
+ Rotation_( BodyOrSpaceType bodyOrSpace, RealP angle1, const CoordinateAxis& axis1, RealP angle2, const CoordinateAxis& axis2 ) { setRotationFromTwoAnglesTwoAxes( bodyOrSpace,angle1,axis1,angle2,axis2); }
+ /// Constructor for three-angle Body-fixed or Space-fixed rotation sequences (angles are in radians)
+ Rotation_( BodyOrSpaceType bodyOrSpace, RealP angle1, const CoordinateAxis& axis1, RealP angle2, const CoordinateAxis& axis2, RealP angle3, const CoordinateAxis& axis3 ) { setRotationFromThreeAnglesThreeAxes(bodyOrSpace,angle1,axis1,angle2,axis2,angle3,axis3); }
+ /// Set this Rotation_ object to a two-angle, two-axes, Body-fixed or Space-fixed rotation sequences (angles are in radians)
+ SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromTwoAnglesTwoAxes( BodyOrSpaceType bodyOrSpace, RealP angle1, const CoordinateAxis& axis1, RealP angle2, const CoordinateAxis& axis2 );
+ /// Set this Rotation_ object to a three-angle Body-fixed or Space-fixed rotation sequences (angles are in radians)
+ SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromThreeAnglesThreeAxes( BodyOrSpaceType bodyOrSpace, RealP angle1, const CoordinateAxis& axis1, RealP angle2, const CoordinateAxis& axis2, RealP angle3, const CoordinateAxis& axis3 );
+
+ /// Set this Rotation_ to represent a rotation characterized by subsequent rotations of:
+ /// +v[0] about the body frame's X axis, followed by a rotation of
+ /// +v[1] about the body frame's NEW Y axis. See Kane, Spacecraft Dynamics, pg. 423, body-three: 1-2-3.
+ void setRotationToBodyFixedXY( const Vec2P& v) { setRotationFromTwoAnglesTwoAxes( BodyRotationSequence, v[0], XAxis, v[1], YAxis ); }
+ /// Set this Rotation_ to represent a rotation characterized by subsequent rotations of:
+ /// +v[0] about the body frame's X axis, followed by a rotation of
+ /// +v[1] about the body frame's NEW Y axis, followed by a rotation of
+ /// +v[2] about the body frame's NEW Z axis. See Kane, Spacecraft Dynamics, pg. 423, body-three: 1-2-3.
+ void setRotationToBodyFixedXYZ( const Vec3P& v) { setRotationFromThreeAnglesThreeAxes( BodyRotationSequence, v[0], XAxis, v[1], YAxis, v[2], ZAxis ); }
+
+ /// Constructor for creating a rotation matrix from a quaternion.
+ explicit Rotation_( const QuaternionP& q ) { setRotationFromQuaternion(q); }
+ /// Method for creating a rotation matrix from a quaternion.
+ SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromQuaternion( const QuaternionP& q );
+
+ /// Construct a Rotation_ directly from a Mat33P (we trust that m is a valid Rotation_!)
+ Rotation_( const Mat33P& m, bool ) : Mat33P(m) {}
+
+ /// Constructs an (hopefully nearby) orthogonal rotation matrix from a generic Mat33P.
+ explicit Rotation_( const Mat33P& m ) { setRotationFromApproximateMat33(m); }
+ /// Set this Rotation_ object to an (hopefully nearby) orthogonal rotation matrix from a generic Mat33P.
+ SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromApproximateMat33( const Mat33P& m );
+
+ /// Calculate R_AB by knowing one of B's unit vector expressed in A.
+ /// Note: The other vectors are perpendicular (but somewhat arbitrarily so).
+ //@{
+ Rotation_( const UnitVec3P& uvec, const CoordinateAxis axis ) { setRotationFromOneAxis(uvec,axis); }
+ SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromOneAxis( const UnitVec3P& uvec, const CoordinateAxis axis );
+ //@}
+
+ /// Calculate R_AB by knowing one of B's unit vectors u1 (could be Bx, By, or Bz)
+ /// expressed in A and a vector v (also expressed in A) that is approximately in
+ /// the desired direction for a second one of B's unit vectors, u2 (!= u1).
+ /// If v is not perpendicular to u1, no worries - we'll find a direction for u2
+ /// that is perpendicular to u1 and comes closest to v. The third vector u3
+ /// is +/- u1 X u2, as appropriate for a right-handed rotation matrix.
+ //@{
+ Rotation_( const UnitVec3P& uveci, const CoordinateAxis& axisi, const Vec3P& vecjApprox, const CoordinateAxis& axisjApprox ) { setRotationFromTwoAxes(uveci,axisi,vecjApprox,axisjApprox); }
+ SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromTwoAxes( const UnitVec3P& uveci, const CoordinateAxis& axisi, const Vec3P& vecjApprox, const CoordinateAxis& axisjApprox );
+ //@}
+
+ // Converts rotation matrix to one or two or three orientation angles.
+ // Note: The result is most meaningful if the Rotation_ matrix is one that can be produced by such a sequence.
+ // Use1: someRotation.convertOneAxisRotationToOneAngle( XAxis );
+ // Use2: someRotation.convertTwoAxesRotationToTwoAngles( SpaceRotationSequence, YAxis, ZAxis );
+ // Use3: someRotation.convertThreeAxesRotationToThreeAngles( SpaceRotationSequence, ZAxis, YAxis, XAxis );
+ // Use4: someRotation.convertRotationToAngleAxis(); Return: [angleInRadians, unitVectorX, unitVectorY, unitVectorZ].
+
+ /// Converts rotation matrix to a single orientation angle.
+ /// Note: The result is most meaningful if the Rotation_ matrix is one that can be produced by such a sequence.
+ SimTK_SimTKCOMMON_EXPORT RealP convertOneAxisRotationToOneAngle( const CoordinateAxis& axis1 ) const;
+ /// Converts rotation matrix to two orientation angles.
+ /// Note: The result is most meaningful if the Rotation_ matrix is one that can be produced by such a sequence.
+ SimTK_SimTKCOMMON_EXPORT Vec2P convertTwoAxesRotationToTwoAngles( BodyOrSpaceType bodyOrSpace, const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const;
+ /// Converts rotation matrix to three orientation angles.
+ /// Note: The result is most meaningful if the Rotation_ matrix is one that can be produced by such a sequence.
+ SimTK_SimTKCOMMON_EXPORT Vec3P convertThreeAxesRotationToThreeAngles( BodyOrSpaceType bodyOrSpace, const CoordinateAxis& axis1, const CoordinateAxis& axis2, const CoordinateAxis& axis3 ) const;
+ /// Converts rotation matrix to a quaternion.
+ SimTK_SimTKCOMMON_EXPORT QuaternionP convertRotationToQuaternion() const;
+ /// Converts rotation matrix to angle-axis form.
+ Vec4P convertRotationToAngleAxis() const { return convertRotationToQuaternion().convertQuaternionToAngleAxis(); }
+
+ /// A convenient special case of convertTwoAxesRotationToTwoAngles().
+ Vec2P convertRotationToBodyFixedXY() const { return convertTwoAxesRotationToTwoAngles( BodyRotationSequence, XAxis, YAxis ); }
+ /// A convenient special case of convertThreeAxesRotationToThreeAngles().
+ Vec3P convertRotationToBodyFixedXYZ() const { return convertThreeAxesRotationToThreeAngles( BodyRotationSequence, XAxis, YAxis, ZAxis ); }
+
+ /// Perform an efficient transform of a symmetric matrix that must be re-expressed with
+ /// a multiply from both left and right, such as an inertia matrix. Details: assuming
+ /// this Rotation is R_AB, and given a symmetric dyadic matrix S_BB expressed in B,
+ /// we can reexpress it in A using S_AA=R_AB*S_BB*R_BA. The matrix should be one that
+ /// is formed as products of vectors expressed in A, such as inertia, gyration or
+ /// covariance matrices. This can be done efficiently exploiting properties of R
+ /// (orthogonal) and S (symmetric). Total cost is 57 flops.
+ SimTK_SimTKCOMMON_EXPORT SymMat33P reexpressSymMat33(const SymMat33P& S_BB) const;
+
+ /// Return true if "this" Rotation is nearly identical to "R" within a specified pointing angle error
+ //@{
+ SimTK_SimTKCOMMON_EXPORT bool isSameRotationToWithinAngle( const Rotation_& R, RealP okPointingAngleErrorRads ) const;
+ bool isSameRotationToWithinAngleOfMachinePrecision( const Rotation_& R) const
+ { return isSameRotationToWithinAngle( R, NTraits<P>::getSignificant() ); }
+ //@}
+ RealP getMaxAbsDifferenceInRotationElements( const Rotation_& R ) const {
+ const Mat33P& A = asMat33(); const Mat33P& B = R.asMat33(); RealP maxDiff = 0;
+ for( int i=0; i<=2; i++ ) for( int j=0; j<=2; j++ )
+ { RealP absDiff = std::fabs(A[i][j] - B[i][j]);
+ if( absDiff > maxDiff ) maxDiff = absDiff; }
+ return maxDiff;
+ }
+
+ bool areAllRotationElementsSameToEpsilon( const Rotation_& R, RealP epsilon ) const
+ { return getMaxAbsDifferenceInRotationElements(R) <= epsilon ; }
+ bool areAllRotationElementsSameToMachinePrecision( const Rotation_& R ) const
+ { return areAllRotationElementsSameToEpsilon( R, NTraits<P>::getSignificant() ); }
+
+ /// Like copy constructor but for inverse rotation. This allows implicit conversion from InverseRotation_ to Rotation_.
+ inline Rotation_( const InverseRotation_<P>& );
+ /// Like copy assignment but for inverse rotation.
+ inline Rotation_& operator=( const InverseRotation_<P>& );
+
+ /// Convert from Rotation_ to InverseRotation_ (no cost). Overrides base class invert().
+ const InverseRotation_<P>& invert() const { return *reinterpret_cast<const InverseRotation_<P>*>(this); }
+ /// Convert from Rotation_ to writable InverseRotation_ (no cost).
+ InverseRotation_<P>& updInvert() { return *reinterpret_cast<InverseRotation_<P>*>(this); }
+
+ /// Transpose, and transpose operators. For an orthogonal matrix like this one, transpose
+ /// is the same thing as inversion. These override the base class transpose methods.
+ //@{
+ const InverseRotation_<P>& transpose() const { return invert(); }
+ const InverseRotation_<P>& operator~() const { return invert(); }
+ InverseRotation_<P>& updTranspose() { return updInvert(); }
+ InverseRotation_<P>& operator~() { return updInvert(); }
+ //@}
+
+ /// In-place composition of Rotation matrices.
+ //@{
+ inline Rotation_& operator*=( const Rotation_<P>& R );
+ inline Rotation_& operator/=( const Rotation_<P>& R );
+ inline Rotation_& operator*=( const InverseRotation_<P>& );
+ inline Rotation_& operator/=( const InverseRotation_<P>& );
+ //@}
+
+ /// Conversion from Rotation to its base class Mat33.
+ /// Note: asMat33 is more efficient than toMat33() (no copy), but you have to know the internal layout.
+ //@{
+ const Mat33P& asMat33() const { return *static_cast<const Mat33P*>(this); }
+ Mat33P toMat33() const { return asMat33(); }
+ //@}
+
+ /// The column and row unit vector types do not necessarily have unit spacing.
+ typedef UnitVec<P,Mat33P::RowSpacing> ColType;
+ typedef UnitRow<P,Mat33P::ColSpacing> RowType;
+ const RowType& row( int i ) const { return reinterpret_cast<const RowType&>(asMat33()[i]); }
+ const ColType& col( int j ) const { return reinterpret_cast<const ColType&>(asMat33()(j)); }
+ const ColType& x() const { return col(0); }
+ const ColType& y() const { return col(1); }
+ const ColType& z() const { return col(2); }
+ const RowType& operator[]( int i ) const { return row(i); }
+ const ColType& operator()( int j ) const { return col(j); }
+
+ /// Set the Rotation_ matrix directly - but you had better know what you are doing!
+ //@{
+ Rotation_& setRotationFromMat33TrustMe( const Mat33P& m )
+ { Mat33P& R = *this; R=m; return *this; }
+ Rotation_& setRotationColFromUnitVecTrustMe( int colj, const UnitVec3P& uvecj )
+ { Mat33P& R = *this; R(colj)=uvecj.asVec3(); return *this; }
+ Rotation_& setRotationFromUnitVecsTrustMe( const UnitVec3P& colA, const UnitVec3P& colB, const UnitVec3P& colC )
+ { Mat33P& R = *this; R(0)=colA.asVec3(); R(1)=colB.asVec3(); R(2)=colC.asVec3(); return *this; }
+ //@}
+
+//--------------------------- PAUL CONTINUE FROM HERE --------------------------
+public:
+//------------------------------------------------------------------------------
+
+ // These are ad hoc routines that don't match the nice API Paul Mitiguy
+ // implemented above.
+
+
+ /// Given cosines and sines (in that order) of three angles, set this
+ /// Rotation matrix to the body-fixed 1-2-3 sequence of those angles.
+ /// Cost is 18 flops.
+ void setRotationToBodyFixedXYZ(const Vec3P& c, const Vec3P& s) {
+ Mat33P& R = *this;
+ const RealP s0s1=s[0]*s[1], s2c0=s[2]*c[0], c0c2=c[0]*c[2], nc1= -c[1];
+
+ R = Mat33P( c[1]*c[2] , s[2]*nc1 , s[1] ,
+ s2c0 + s0s1*c[2] , c0c2 - s0s1*s[2] , s[0]*nc1 ,
+ s[0]*s[2] - s[1]*c0c2 , s[0]*c[2] + s[1]*s2c0 , c[0]*c[1] );
+ }
+
+
+ /// Given Euler angles forming a body-fixed 3-2-1 sequence, and the relative
+ /// angular velocity vector of B in the parent frame, *BUT EXPRESSED IN
+ /// THE BODY FRAME*, return the Euler angle
+ /// derivatives. You are dead if q[1] gets near 90 degrees!
+ /// See Kane's Spacecraft Dynamics, page 428, body-three: 3-2-1.
+ static Vec3P convertAngVelToBodyFixed321Dot(const Vec3P& q, const Vec3P& w_PB_B) {
+ const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]);
+ const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]);
+ const RealP ooc1 = RealP(1)/c1;
+ const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;
+
+ const Mat33P E( 0, s2oc1 , c2oc1 ,
+ 0, c2 , -s2 ,
+ 1, s1*s2oc1 , s1*c2oc1 );
+ return E * w_PB_B;
+ }
+
+ /// Inverse of the above routine. Returned angular velocity is B in P,
+ /// expressed in *B*: w_PB_B.
+ static Vec3P convertBodyFixed321DotToAngVel(const Vec3P& q, const Vec3P& qd) {
+ const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]);
+ const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]);
+
+ const Mat33P Einv( -s1 , 0 , 1 ,
+ c1*s2 , c2 , 0 ,
+ c1*c2 , -s2 , 0 );
+ return Einv*qd;
+ }
+
+ // TODO: sherm: is this right? Warning: everything is measured in the
+ // *PARENT* frame, but angular velocities and accelerations are
+ // expressed in the *BODY* frame.
+ // TODO: this is not an efficient way to do this computation.
+ static Vec3P convertAngVelDotToBodyFixed321DotDot
+ (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
+ {
+ const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]);
+ const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]);
+ const RealP ooc1 = 1/c1;
+ const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1, s1oc1 = s1*ooc1;
+
+ const Mat33P E( 0 , s2oc1 , c2oc1 ,
+ 0 , c2 , -s2 ,
+ 1 , s1*s2oc1 , s1*c2oc1 );
+ const Vec3P qdot = E * w_PB_B;
+
+ const RealP t = qdot[1]*s1oc1;
+ const RealP a = t*s2oc1 + qdot[2]*c2oc1; // d/dt s2oc1
+ const RealP b = t*c2oc1 - qdot[2]*s2oc1; // d/dt c2oc1
+
+ const Mat33P Edot( 0 , a , b ,
+ 0 , -qdot[2]*s2 , -qdot[2]*c2 ,
+ 0 , s1*a + qdot[1]*s2 , s1*b + qdot[1]*c2 );
+
+ return E*wdot_PB_B + Edot*w_PB_B;
+ }
+
+ /// Given Euler angles q forming a body-fixed X-Y-Z sequence return the
+ /// block N_B of the system N matrix such that qdot=N_B(q)*w_PB_B where
+ /// w_PB_B is the angular velocity of B in P EXPRESSED IN *B*!!! Note that
+ /// N_B=N_P*R_PB. This matrix will be singular if Y (q[1]) gets near 90
+ /// degrees!
+ /// @note This version is very expensive because it has to calculate sines
+ /// and cosines. If you already have those, use the alternate form
+ /// of this method.
+ ///
+ /// Cost: about 100 flops for sin/cos plus 12 to calculate N_B.
+ /// @see Kane's Spacecraft Dynamics, page 427, body-three: 1-2-3.
+ static Mat33P calcNForBodyXYZInBodyFrame(const Vec3P& q) {
+ // Note: q[0] is not referenced so we won't waste time calculating
+ // its cosine and sine here.
+ return calcNForBodyXYZInBodyFrame
+ (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
+ Vec3P(0, std::sin(q[1]), std::sin(q[2])));
+ }
+
+ /// This faster version of calcNForBodyXYZInBodyFrame() assumes you have
+ /// already calculated the cosine and sine of the three q's. Note that we
+ /// only look at the cosines and sines of q[1] and q[2]; q[0] does not
+ /// matter so you don't have to fill in the 0'th element of cq and sq.
+ /// Cost is one divide plus 6 flops, say 12 flops.
+ static Mat33P calcNForBodyXYZInBodyFrame(const Vec3P& cq, const Vec3P& sq) {
+ const RealP s1 = sq[1], c1 = cq[1];
+ const RealP s2 = sq[2], c2 = cq[2];
+ const RealP ooc1 = 1/c1;
+ const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;
+
+ return Mat33P( c2oc1 , -s2oc1 , 0,
+ s2 , c2 , 0,
+ -s1*c2oc1 , s1*s2oc1, 1 );
+ }
+
+ /// Given Euler angles q forming a body-fixed X-Y-Z (123) sequence return
+ /// the block N_P of the system N matrix such that qdot=N_P(q)*w_PB where
+ /// w_PB is the angular velocity of B in P expressed in P (not the
+ /// convention that Kane uses, where angular velocities are expressed in
+ /// the outboard body B). Note that N_P = N_B*~R_PB. This matrix will be
+ /// singular if Y (q[1]) gets near 90 degrees!
+ ///
+ /// @note This version is very expensive because it has to calculate sines
+ /// and cosines. If you already have those, use the alternate form
+ /// of this method.
+ ///
+ /// Cost: about 100 flops for sin/cos plus 12 to calculate N_P.
+ static Mat33P calcNForBodyXYZInParentFrame(const Vec3P& q) {
+ // Note: q[2] is not referenced so we won't waste time calculating
+ // its cosine and sine here.
+ return calcNForBodyXYZInParentFrame
+ (Vec3P(std::cos(q[0]), std::cos(q[1]), 0),
+ Vec3P(std::sin(q[0]), std::sin(q[1]), 0));
+ }
+
+ /// This faster version of calcNForBodyXYZInParentFrame() assumes you have
+ /// already calculated the cosine and sine of the three q's. Note that we
+ /// only look at the cosines and sines of q[0] and q[1]; q[2] does not
+ /// matter so you don't have to fill in the 3rd element of cq and sq.
+ /// Cost is one divide plus 6 flops, say 12 flops.
+ /// @see Paul Mitiguy
+ static Mat33P calcNForBodyXYZInParentFrame(const Vec3P& cq, const Vec3P& sq) {
+ const RealP s0 = sq[0], c0 = cq[0];
+ const RealP s1 = sq[1], c1 = cq[1];
+ const RealP ooc1 = 1/c1;
+ const RealP s0oc1 = s0*ooc1, c0oc1 = c0*ooc1;
+
+ return Mat33P( 1 , s1*s0oc1 , -s1*c0oc1,
+ 0 , c0 , s0,
+ 0 , -s0oc1 , c0oc1 );
+ }
+
+ /// This is the fastest way to form the product qdot=N_P*w_PB for a
+ /// body-fixed XYZ sequence where angular velocity of child in parent is
+ /// expected to be expressed in the parent. Here we assume you have
+ /// previously calculated sincos(qx), sincos(qy), and 1/cos(qy).
+ /// Cost is 10 flops, faster even than the 15 it would take if you had saved
+ /// N_P and then formed the N_P*w_PB product explicitly.
+ static Vec3P multiplyByBodyXYZ_N_P(const Vec2P& cosxy,
+ const Vec2P& sinxy,
+ RealP oocosy,
+ const Vec3P& w_PB)
+ {
+ const RealP s0 = sinxy[0], c0 = cosxy[0];
+ const RealP s1 = sinxy[1];
+ const RealP w0 = w_PB[0], w1 = w_PB[1], w2 = w_PB[2];
+
+ const RealP t = (s0*w1-c0*w2)*oocosy;
+ return Vec3P( w0 + t*s1, c0*w1 + s0*w2, -t ); // qdot
+ }
+
+ /// This is the fastest way to form the product v_P=~N_P*q=~(~q*N_P);
+ /// see the untransposed method multiplyByBodyXYZ_N_P() for information.
+ /// Cost is 9 flops.
+ static Vec3P multiplyByBodyXYZ_NT_P(const Vec2P& cosxy,
+ const Vec2P& sinxy,
+ RealP oocosy,
+ const Vec3P& q)
+ {
+ const RealP s0 = sinxy[0], c0 = cosxy[0];
+ const RealP s1 = sinxy[1];
+ const RealP q0 = q[0], q1 = q[1], q2 = q[2];
+
+ const RealP t = (q0*s1-q2) * oocosy;
+ return Vec3P( q0, c0*q1 + t*s0, s0*q1 - t*c0 ); // v_P
+ }
+
+ /// Calculate first time derivative qdot of body-fixed XYZ Euler angles q
+ /// given sines and cosines of the Euler angles and the angular velocity
+ /// w_PB of child B in parent P, expressed in P. Cost is 10 flops.
+ ///
+ /// Theory: calculate qdot=N_P(q)*w_PB using multiplyByBodyXYZ_N_P().
+ /// @see multiplyByBodyXYZ_N_P()
+ static Vec3P convertAngVelInParentToBodyXYZDot
+ (const Vec2P& cosxy, ///< cos(qx), cos(qy)
+ const Vec2P& sinxy, ///< sin(qx), sin(qy)
+ RealP oocosy, ///< 1/cos(qy)
+ const Vec3P& w_PB) ///< angular velocity of B in P, exp. in P
+ {
+ return multiplyByBodyXYZ_N_P(cosxy,sinxy,oocosy,w_PB);
+ }
+
+ /// Calculate second time derivative qdotdot of body-fixed XYZ Euler
+ /// angles q given sines and cosines of the Euler angles, the first
+ /// derivative qdot and the angular acceleration b_PB of child B in
+ /// parent P, expressed in P. Cost is 22 flops.
+ ///
+ /// Theory: we have qdot=N_P*w_PB, which we differentiate in P to
+ /// get qdotdot=N_P*b_PB + NDot_P*w_PB. Note that NDot_P=NDot_P(q,qdot)
+ /// and w_PB=NInv_P*qdot (because N_P is invertible). We can then rewrite
+ /// qdotdot=N_P*b_PB + NDot_P*(NInv_P*qdot) which can be calculated very
+ /// efficiently. The second term is just an acceleration remainder term
+ /// quadratic in qdot.
+ static Vec3P convertAngAccInParentToBodyXYZDotDot
+ (const Vec2P& cosxy, ///< cos(qx), cos(qy)
+ const Vec2P& sinxy, ///< sin(qx), sin(qy)
+ RealP oocosy, ///< 1/cos(qy)
+ const Vec3P& qdot, ///< previously calculated BodyXYZDot
+ const Vec3P& b_PB) ///< angular acceleration, a.k.a. wdot_PB
+ {
+ const RealP s1 = sinxy[1], c1 = cosxy[1];
+ const RealP q0 = qdot[0], q1 = qdot[1], q2 = qdot[2];
+
+ // 10 flops
+ const Vec3P Nb = multiplyByBodyXYZ_N_P(cosxy,sinxy,oocosy,b_PB);
+
+ const RealP q1oc1 = q1*oocosy;
+ const Vec3P NDotw((q0*s1-q2)*q1oc1, // NDot_P*w_PB
+ q0*q2*c1, // = NDot_P*(NInv_P*qdot)
+ (q2*s1-q0)*q1oc1 ); // (9 flops)
+
+ return Nb + NDotw; // 3 flops
+ }
+
+
+ /// Fastest way to form the product w_PB=NInv_P*qdot. This is never
+ /// singular. Cost is 9 flops.
+ static Vec3P multiplyByBodyXYZ_NInv_P(const Vec2P& cosxy,
+ const Vec2P& sinxy,
+ const Vec3P& qdot)
+ {
+ const RealP s0 = sinxy[0], c0 = cosxy[0];
+ const RealP s1 = sinxy[1], c1 = cosxy[1];
+ const RealP q0 = qdot[0], q1 = qdot[1], q2 = qdot[2];
+ const RealP c1q2 = c1*q2;
+
+ return Vec3P( q0 + s1*q2, // w_PB
+ c0*q1 - s0*c1q2,
+ s0*q1 + c0*c1q2 );
+ }
+
+ /// Fastest way to form the product q=~NInv_P*v_P=~(~v_P*NInv_P).
+ /// This is never singular. Cost is 10 flops.
+ static Vec3P multiplyByBodyXYZ_NInvT_P(const Vec2P& cosxy,
+ const Vec2P& sinxy,
+ const Vec3P& v_P)
+ {
+ const RealP s0 = sinxy[0], c0 = cosxy[0];
+ const RealP s1 = sinxy[1], c1 = cosxy[1];
+ const RealP w0 = v_P[0], w1 = v_P[1], w2 = v_P[2];
+
+ return Vec3P( w0, // qdot-like
+ c0*w1 + s0*w2,
+ s1*w0 - s0*c1*w1 + c0*c1*w2);
+ }
+
+ /// Given Euler angles forming a body-fixed X-Y-Z (123) sequence q, and
+ /// their time derivatives qdot, return the block of the NDot matrix such
+ /// that qdotdot=N(q)*wdot + NDot(q,u)*w where w is the angular velocity
+ /// of B in P EXPRESSED IN *B*!!! This matrix will be singular if Y (q[1])
+ /// gets near 90 degrees! See calcNForBodyXYZInBodyFrame() for the matrix
+ /// we're differentiating here.
+ /// @note This version is very expensive because it has to calculate sines
+ /// and cosines. If you already have those, use the alternate form
+ /// of this method.
+ static Mat33P calcNDotForBodyXYZInBodyFrame
+ (const Vec3P& q, const Vec3P& qdot) {
+ // Note: q[0] is not referenced so we won't waste time calculating
+ // its cosine and sine here.
+ return calcNDotForBodyXYZInBodyFrame
+ (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
+ Vec3P(0, std::sin(q[1]), std::sin(q[2])),
+ qdot);
+ }
+
+ /// This faster version of calcNDotForBodyXYZInBodyFrame() assumes you
+ /// have already calculated the cosine and sine of the three q's. Note
+ /// that we only look at the cosines and sines of q[1] and q[2]; q[0] does
+ /// not matter so you don't have to fill in the 0'th element of cq and sq.
+ /// Cost is one divide plus 21 flops.
+ static Mat33P calcNDotForBodyXYZInBodyFrame
+ (const Vec3P& cq, const Vec3P& sq, const Vec3P& qdot)
+ {
+ const RealP s1 = sq[1], c1 = cq[1];
+ const RealP s2 = sq[2], c2 = cq[2];
+ const RealP ooc1 = 1/c1;
+ const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;
+
+ const RealP t = qdot[1]*s1*ooc1;
+ const RealP a = t*s2oc1 + qdot[2]*c2oc1; // d/dt s2oc1
+ const RealP b = t*c2oc1 - qdot[2]*s2oc1; // d/dt c2oc1
+
+ return Mat33P( b , -a , 0,
+ qdot[2]*c2 , -qdot[2]*s2 , 0,
+ -(s1*b + qdot[1]*c2) , s1*a + qdot[1]*s2 , 0 );
+ }
+
+ /// Given Euler angles forming a body-fixed X-Y-Z (123) sequence q, and
+ /// their time derivatives qdot, return the block of the NDot matrix such
+ /// that qdotdot=N(q)*wdot + NDot(q,u)*w where w is the angular velocity of
+ /// B in P expressed in P. This matrix will be singular if Y (q[1]) gets
+ /// near 90 degrees! See calcNForBodyXYZInParentFrame() for the matrix
+ /// we're differentiating here.
+ /// @note This version is very expensive because it has to calculate sines
+ /// and cosines. If you already have those, use the alternate form
+ /// of this method.
+ static Mat33P calcNDotForBodyXYZInParentFrame
+ (const Vec3P& q, const Vec3P& qdot) {
+ // Note: q[2] is not referenced so we won't waste time calculating
+ // its cosine and sine here.
+ const RealP cy = std::cos(q[1]); // cos(y)
+ return calcNDotForBodyXYZInParentFrame
+ (Vec2P(std::cos(q[0]), cy),
+ Vec2P(std::sin(q[0]), std::sin(q[1])),
+ 1/cy, qdot);
+ }
+
+ /// This faster version of calcNDotForBodyXYZInParentFrame() assumes you
+ /// have already calculated the cosine and sine of the three q's. Note that
+ /// we only look at the cosines and sines of q[0] and q[1].
+ /// Cost is 21 flops.
+ static Mat33P calcNDotForBodyXYZInParentFrame
+ (const Vec2P& cq, const Vec2P& sq, RealP ooc1, const Vec3P& qdot) {
+ const RealP s0 = sq[0], c0 = cq[0];
+ const RealP s1 = sq[1], c1 = cq[1];
+ const RealP s0oc1 = s0*ooc1, c0oc1 = c0*ooc1;
+
+ const RealP t = qdot[1]*s1*ooc1;
+ const RealP a = t*s0oc1 + qdot[0]*c0oc1; // d/dt s0oc1
+ const RealP b = t*c0oc1 - qdot[0]*s0oc1; // d/dt c0oc1
+
+ return Mat33P( 0, s1*a + qdot[1]*s0, -(s1*b + qdot[1]*c0),
+ 0, -qdot[0]*s0 , qdot[0]*c0 ,
+ 0, -a , b );
+ }
+
+ /// Inverse of routine calcNForBodyXYZInBodyFrame(). Return the inverse
+ /// NInv_B of the N_B block computed above, such that w_PB_B=NInv_B(q)*qdot
+ /// where w_PB_B is the angular velocity of B in P EXPRESSED IN *B*!!!
+ /// (Kane's convention.) Note that NInv_B=~R_PB*NInv_P. This matrix is
+ /// never singular.
+ /// @note This version is very expensive because it has to calculate sines
+ /// and cosines. If you already have those, use the alternate form
+ /// of this method.
+ static Mat33P calcNInvForBodyXYZInBodyFrame(const Vec3P& q) {
+ // Note: q[0] is not referenced so we won't waste time calculating
+ // its cosine and sine here.
+ return calcNInvForBodyXYZInBodyFrame
+ (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
+ Vec3P(0, std::sin(q[1]), std::sin(q[2])));
+ }
+
+ /// This faster version of calcNInvForBodyXYZInBodyFrame() assumes you have
+ /// already calculated the cosine and sine of the three q's. Note that we
+ /// only look at the cosines and sines of q[1] and q[2]; q[0] does not
+ /// matter so you don't have to fill in the 0'th element of cq and sq.
+ /// Cost is 3 flops.
+ static Mat33P calcNInvForBodyXYZInBodyFrame
+ (const Vec3P& cq, const Vec3P& sq) {
+ const RealP s1 = sq[1], c1 = cq[1];
+ const RealP s2 = sq[2], c2 = cq[2];
+
+ return Mat33P( c1*c2 , s2 , 0 ,
+ -c1*s2 , c2 , 0 ,
+ s1 , 0 , 1 );
+ }
+
+ /// Inverse of the above routine. Return the inverse NInv_P of the N_P
+ /// block computed above, such that w_PB=NInv_P(q)*qdot where w_PB is the
+ /// angular velocity of B in P (expressed in P). Note that
+ /// NInv_P=R_PB*NInv_B. This matrix is never singular.
+ /// @note This version is very expensive because it has to calculate sines
+ /// and cosines. If you already have those, use the alternate form
+ /// of this method.
+ static Mat33P calcNInvForBodyXYZInParentFrame(const Vec3P& q) {
+ // Note: q[0] is not referenced so we won't waste time calculating
+ // its cosine and sine here.
+ return calcNInvForBodyXYZInParentFrame
+ (Vec3P(std::cos(q[0]), std::cos(q[1]), 0),
+ Vec3P(std::sin(q[0]), std::sin(q[1]), 0));
+ }
+
+ /// This faster version of calcNInvForBodyXYZInParentFrame() assumes you
+ /// have already calculated the cosine and sine of the three q's. Note that
+ /// we only look at the cosines and sines of q[0] and q[1]; q[2] does not
+ /// matter so you don't have to fill in the 3rd element of cq and sq.
+ /// Cost is 3 flops.
+ static Mat33P calcNInvForBodyXYZInParentFrame
+ (const Vec3P& cq, const Vec3P& sq) {
+ const RealP s0 = sq[0], c0 = cq[0];
+ const RealP s1 = sq[1], c1 = cq[1];
+
+ return Mat33P( 1 , 0 , s1 ,
+ 0 , c0 , -s0*c1 ,
+ 0 , s0 , c0*c1 );
+ }
+
+ /// Given Euler angles forming a body-fixed X-Y-Z (123) sequence, and the
+ /// relative angular velocity vector w_PB_B of B in the parent frame,
+ /// <em>BUT EXPRESSED IN THE BODY FRAME</em>, return the Euler angle
+ /// derivatives. You are dead if q[1] gets near 90 degrees!
+ /// @note This version is very expensive because it has to calculate sines
+ /// and cosines. If you already have those, use the alternate form
+ /// of this method.
+ /// @see Kane's Spacecraft Dynamics, page 427, body-three: 1-2-3.
+ static Vec3P convertAngVelInBodyFrameToBodyXYZDot
+ (const Vec3P& q, const Vec3P& w_PB_B) {
+ return convertAngVelInBodyFrameToBodyXYZDot
+ (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
+ Vec3P(0, std::sin(q[1]), std::sin(q[2])),
+ w_PB_B);
+ }
+
+ /// This faster version of convertAngVelInBodyFrameToBodyXYZDot() assumes
+ /// you have already calculated the cosine and sine of the three q's. Note
+ /// that we only look at the cosines and sines of q[1] and q[2]; q[0] does
+ /// not matter so you don't have to fill in the 0'th element of cq and sq.
+ /// Cost is XXX.
+ //TODO: reimplement
+ static Vec3P convertAngVelInBodyFrameToBodyXYZDot
+ (const Vec3P& cq, const Vec3P& sq, const Vec3P& w_PB_B)
+ { return calcNForBodyXYZInBodyFrame(cq,sq)*w_PB_B; }
+
+ /// Inverse of the above routine. Returned angular velocity is B in P,
+ /// expressed in *B*: w_PB_B.
+ /// @note This version is very expensive because it has to calculate sines
+ /// and cosines. If you already have those, use the alternate form
+ /// of this method.
+ static Vec3P convertBodyXYZDotToAngVelInBodyFrame
+ (const Vec3P& q, const Vec3P& qdot) {
+ return convertBodyXYZDotToAngVelInBodyFrame
+ (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
+ Vec3P(0, std::sin(q[1]), std::sin(q[2])),
+ qdot);
+ }
+
+ /// This faster version of convertBodyXYZDotToAngVelInBodyFrame() assumes
+ /// you have already calculated the cosine and sine of the three q's. Note
+ /// that we only look at the cosines and sines of q[1] and q[2]; q[0] does
+ /// not matter so you don't have to fill in the 0'th element of cq and sq.
+ /// Cost is XXX flops.
+ // TODO: reimplement
+ static Vec3P convertBodyXYZDotToAngVelInBodyFrame
+ (const Vec3P& cq, const Vec3P& sq, const Vec3P& qdot)
+ { return calcNInvForBodyXYZInBodyFrame(cq,sq)*qdot; }
+
+ /// TODO: sherm: is this right? Warning: everything is measured in the
+ /// *PARENT* frame, but has to be expressed in the *BODY* frame.
+ /// @note This version is very expensive because it has to calculate sines
+ /// and cosines. If you already have those, use the alternate form
+ /// of this method.
+ static Vec3P convertAngVelDotInBodyFrameToBodyXYZDotDot
+ (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
+ {
+ // Note: q[0] is not referenced so we won't waste time calculating
+ // its cosine and sine here.
+ return convertAngVelDotInBodyFrameToBodyXYZDotDot
+ (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
+ Vec3P(0, std::sin(q[1]), std::sin(q[2])),
+ w_PB_B, wdot_PB_B);
+ }
+
+ /// This faster version of convertAngVelDotInBodyFrameToBodyXYZDotDot()
+ /// assumes you have already calculated the cosine and sine of the three
+ /// q's. Note that we only look at the cosines and sines of q[1] and q[2];
+ /// q[0] does not matter so you don't have to fill in the 0'th element of
+ /// cq and sq.
+ /// Cost is XXX flops.
+ // TODO: reimplement
+ static Vec3P convertAngVelDotInBodyFrameToBodyXYZDotDot
+ (const Vec3P& cq, const Vec3P& sq, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
+ {
+ const Mat33P N = calcNForBodyXYZInBodyFrame(cq,sq);
+ const Vec3P qdot = N * w_PB_B; // 15 flops
+ const Mat33P NDot = calcNDotForBodyXYZInBodyFrame(cq,sq,qdot);
+
+ return N*wdot_PB_B + NDot*w_PB_B; // 33 flops
+ }
+
+ /// Given a possibly unnormalized quaternion q, calculate the 4x3 matrix
+ /// N which maps angular velocity w to quaternion derivatives qdot. We
+ /// expect the angular velocity in the parent frame, i.e. w==w_PB_P.
+ /// We don't normalize, so N=|q|N' where N' is the normalized version.
+ /// Cost is 7 flops.
+ static Mat43P calcUnnormalizedNForQuaternion(const Vec4P& q) {
+ const Vec4P e = q/2;
+ const RealP ne1 = -e[1], ne2 = -e[2], ne3 = -e[3];
+ return Mat43P( ne1, ne2, ne3,
+ e[0], e[3], ne2,
+ ne3, e[0], e[1],
+ e[2], ne1, e[0]);
+ }
+
+ /// Given the time derivative qdot of a possibly unnormalized quaternion
+ /// q, calculate the 4x3 matrix NDot which is the time derivative of the
+ /// matrix N as described in calcUnnormalizedNForQuaternion(). Note that
+ /// NDot = d/dt N = d/dt (|q|N') = |q|(d/dt N'), where N' is the normalized
+ /// matrix, since the length of the quaternion should be a constant.
+ /// Cost is 7 flops.
+ static Mat43P calcUnnormalizedNDotForQuaternion(const Vec4P& qdot) {
+ const Vec4P ed = qdot/2;
+ const RealP ned1 = -ed[1], ned2 = -ed[2], ned3 = -ed[3];
+ return Mat43P( ned1, ned2, ned3,
+ ed[0], ed[3], ned2,
+ ned3, ed[0], ed[1],
+ ed[2], ned1, ed[0]);
+ }
+
+ /// Given a (possibly unnormalized) quaternion q, calculate the 3x4 matrix
+ /// NInv (= N^-1) which maps quaternion derivatives qdot to angular velocity
+ /// w, where the angular velocity is in the parent frame, i.e. w==w_PB_P.
+ /// Note: when the quaternion is not normalized, this is not precisely the
+ /// (pseudo)inverse of N. inv(N)=inv(N')/|q| but we're returning
+ /// |q|*inv(N')=|q|^2*inv(N). That is, NInv*N =|q|^2*I, which is I
+ /// if the original q was normalized. (Note: N*NInv != I, not even close.)
+ /// Cost is 7 flops.
+ static Mat34P calcUnnormalizedNInvForQuaternion(const Vec4P& q) {
+ const Vec4P e = 2*q;
+ const RealP ne1 = -e[1], ne2 = -e[2], ne3 = -e[3];
+ return Mat34P(ne1, e[0], ne3, e[2],
+ ne2, e[3], e[0], ne1,
+ ne3, ne2, e[1], e[0]);
+ }
+
+
+ /// Given a possibly unnormalized quaternion (0th element is the scalar) and the
+ /// relative angular velocity vector of B in its parent, expressed
+ /// in the *PARENT*, return the quaternion derivatives. This is never singular.
+ /// Cost is 27 flops.
+ static Vec4P convertAngVelToQuaternionDot(const Vec4P& q, const Vec3P& w_PB_P) {
+ return calcUnnormalizedNForQuaternion(q)*w_PB_P;
+ }
+
+ /// Inverse of the above routine. Returned AngVel is expressed in
+ /// the *PARENT* frame: w_PB_P.
+ /// Cost is 28 flops.
+ static Vec3P convertQuaternionDotToAngVel(const Vec4P& q, const Vec4P& qdot) {
+ return calcUnnormalizedNInvForQuaternion(q)*qdot;
+ }
+
+ /// Given a quaternion q representing R_PB, angular velocity of B in P, and
+ /// the time derivative of the angular velocity, return the second time
+ /// derivative qdotdot of the quaternion. Everything is measured and
+ /// expressed in the parent.
+ /// Cost is 78 flops.
+ // TODO: reimplement
+ static Vec4P OLDconvertAngVelDotToQuaternionDotDot
+ (const Vec4P& q, const Vec3P& w_PB_P, const Vec3P& wdot_PB_P)
+ {
+ const Mat43P N = calcUnnormalizedNForQuaternion(q);
+ const Vec4P qdot = N*w_PB_P; // 20 flops
+ const Mat43P NDot = calcUnnormalizedNDotForQuaternion(qdot);
+
+ return N*wdot_PB_P + NDot*w_PB_P; // 44 flops
+ }
+
+ /// We want to differentiate qdot=N(q)*w to get qdotdot=N*b+NDot*w where
+ /// b is angular acceleration wdot. Note that NDot=NDot(qdot), but it is
+ /// far better to calculate the matrix-vector product NDot(N*w)*w directly
+ /// rather than calculate NDot separately. That gives
+ /// <pre>NDot*w = -(w^2)/4 * q</pre>
+ /// Cost is 41 flops.
+ static Vec4P convertAngVelDotToQuaternionDotDot
+ (const Vec4P& q, const Vec3P& w_PB, const Vec3P& b_PB)
+ {
+ const Mat43P N = calcUnnormalizedNForQuaternion(q); // 7 flops
+ const Vec4P Nb = N*b_PB; // 20 flops
+ const Vec4P NDotw = RealP(-.25)*w_PB.normSqr()*q; // 10 flops
+ return Nb + NDotw; // 4 flops
+ }
+
+
+private:
+ // This is only for the most trustworthy of callers, that is, methods of
+ // the Rotation_ class. There are a lot of ways for this NOT to be a
+ // legitimate rotation matrix -- be careful!!
+ // Note that these are supplied in rows.
+ Rotation_( const RealP& xx, const RealP& xy, const RealP& xz,
+ const RealP& yx, const RealP& yy, const RealP& yz,
+ const RealP& zx, const RealP& zy, const RealP& zz )
+ : Mat33P( xx,xy,xz, yx,yy,yz, zx,zy,zz ) {}
+
+ // These next methods are highly-efficient power-user methods. Read the
+ // code to understand them.
+ SimTK_SimTKCOMMON_EXPORT Rotation_& setTwoAngleTwoAxesBodyFixedForwardCyclicalRotation( RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1, RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2 );
+ SimTK_SimTKCOMMON_EXPORT Rotation_& setThreeAngleTwoAxesBodyFixedForwardCyclicalRotation( RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1, RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2, RealP cosAngle3, RealP sinAngle3 );
+ SimTK_SimTKCOMMON_EXPORT Rotation_& setThreeAngleThreeAxesBodyFixedForwardCyclicalRotation( RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1, RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2, RealP cosAngle3, RealP sinAngle3, const CoordinateAxis& axis3 );
+
+ // These next methods are highly-efficient power-user methods to convert
+ // Rotation matrices to orientation angles. Read the code to understand them.
+ SimTK_SimTKCOMMON_EXPORT Vec2P convertTwoAxesBodyFixedRotationToTwoAngles( const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const;
+ SimTK_SimTKCOMMON_EXPORT Vec3P convertTwoAxesBodyFixedRotationToThreeAngles( const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const;
+ SimTK_SimTKCOMMON_EXPORT Vec3P convertThreeAxesBodyFixedRotationToThreeAngles( const CoordinateAxis& axis1, const CoordinateAxis& axis2, const CoordinateAxis& axis3 ) const;
+
+//------------------------------------------------------------------------------
+// These are obsolete names from a previous release, listed here so that
+// users will get a decipherable compilation error. (sherm 091101)
+//------------------------------------------------------------------------------
+private:
+ // REPLACED BY: calcNForBodyXYZInBodyFrame()
+ static Mat33P calcQBlockForBodyXYZInBodyFrame(const Vec3P& a)
+ { return calcNForBodyXYZInBodyFrame(a); }
+ // REPLACED BY: calcNInvForBodyXYZInBodyFrame()
+ static Mat33P calcQInvBlockForBodyXYZInBodyFrame(const Vec3P& a)
+ { return calcNInvForBodyXYZInBodyFrame(a); }
+ // REPLACED BY: calcUnnormalizedNForQuaternion()
+ static Mat<4,3,P> calcUnnormalizedQBlockForQuaternion(const Vec4P& q)
+ { return calcUnnormalizedNForQuaternion(q); }
+ // REPLACED BY: calcUnnormalizedNInvForQuaternion()
+ static Mat<3,4,P> calcUnnormalizedQInvBlockForQuaternion(const Vec4P& q)
+ { return calcUnnormalizedNInvForQuaternion(q); }
+ // REPLACED BY: convertAngVelInBodyFrameToBodyXYZDot
+ static Vec3P convertAngVelToBodyFixed123Dot(const Vec3P& q, const Vec3P& w_PB_B)
+ { return convertAngVelInBodyFrameToBodyXYZDot(q,w_PB_B); }
+ // REPLACED BY: convertBodyXYZDotToAngVelInBodyFrame
+ static Vec3P convertBodyFixed123DotToAngVel(const Vec3P& q, const Vec3P& qdot)
+ { return convertBodyXYZDotToAngVelInBodyFrame(q,qdot); }
+ // REPLACED BY: convertAngVelDotInBodyFrameToBodyXYZDotDot
+ static Vec3P convertAngVelDotToBodyFixed123DotDot
+ (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
+ { return convertAngVelDotInBodyFrameToBodyXYZDotDot(q,w_PB_B,wdot_PB_B); }
+
+//------------------------------------------------------------------------------
+// The following code is obsolete - it is here temporarily for backward
+// compatibility (Mitiguy 9/5/2007)
+//------------------------------------------------------------------------------
+private:
+ // These static methods are like constructors with friendlier names.
+ static Rotation_ zero() { return Rotation_(); }
+ static Rotation_ NaN() { Rotation_ r; r.setRotationToNaN(); return r; }
+
+ /// By zero we mean "zero rotation", i.e., an identity matrix.
+ Rotation_& setToZero() { return setRotationToIdentityMatrix(); }
+ Rotation_& setToIdentityMatrix() { return setRotationToIdentityMatrix(); }
+ Rotation_& setToNaN() { return setRotationToNaN(); }
+ static Rotation_ trustMe( const Mat33P& m ) { return Rotation_(m,true); }
+
+ // One-angle rotations.
+ static Rotation_ aboutX( const RealP& angleInRad ) { return Rotation_( angleInRad, XAxis ); }
+ static Rotation_ aboutY( const RealP& angleInRad ) { return Rotation_( angleInRad, YAxis ); }
+ static Rotation_ aboutZ( const RealP& angleInRad ) { return Rotation_( angleInRad, ZAxis ); }
+ static Rotation_ aboutAxis( const RealP& angleInRad, const UnitVec3P& axis ) { return Rotation_(angleInRad,axis); }
+ static Rotation_ aboutAxis( const RealP& angleInRad, const Vec3P& axis ) { return Rotation_(angleInRad,axis); }
+ void setToRotationAboutZ( const RealP& q ) { setRotationFromAngleAboutZ( q ); }
+
+ // Two-angle space-fixed rotations.
+ static Rotation_ aboutXThenOldY(const RealP& xInRad, const RealP& yInRad) { return Rotation_( SpaceRotationSequence, xInRad, XAxis, yInRad, YAxis ); }
+ static Rotation_ aboutYThenOldX(const RealP& yInRad, const RealP& xInRad) { return Rotation_( SpaceRotationSequence, yInRad, YAxis, xInRad, XAxis ); }
+ static Rotation_ aboutXThenOldZ(const RealP& xInRad, const RealP& zInRad) { return Rotation_( SpaceRotationSequence, xInRad, XAxis, zInRad, ZAxis ); }
+ static Rotation_ aboutZThenOldX(const RealP& zInRad, const RealP& xInRad) { return Rotation_( SpaceRotationSequence, zInRad, ZAxis, xInRad, XAxis ); }
+ static Rotation_ aboutYThenOldZ(const RealP& yInRad, const RealP& zInRad) { return Rotation_( SpaceRotationSequence, yInRad, YAxis, zInRad, ZAxis ); }
+ static Rotation_ aboutZThenOldY(const RealP& zInRad, const RealP& yInRad) { return Rotation_( SpaceRotationSequence, zInRad, ZAxis, yInRad, YAxis ); }
+
+ // Two-angle body fixed rotations (reversed space-fixed ones).
+ static Rotation_ aboutXThenNewY(const RealP& xInRad, const RealP& yInRad) { return Rotation_( BodyRotationSequence, xInRad, XAxis, yInRad, YAxis ); }
+ static Rotation_ aboutYThenNewX(const RealP& yInRad, const RealP& xInRad) { return aboutXThenOldY(xInRad, yInRad); }
+ static Rotation_ aboutXThenNewZ(const RealP& xInRad, const RealP& zInRad) { return aboutZThenOldX(zInRad, xInRad); }
+ static Rotation_ aboutZThenNewX(const RealP& zInRad, const RealP& xInRad) { return aboutXThenOldZ(xInRad, zInRad); }
+ static Rotation_ aboutYThenNewZ(const RealP& yInRad, const RealP& zInRad) { return aboutZThenOldY(zInRad, yInRad); }
+ static Rotation_ aboutZThenNewY(const RealP& zInRad, const RealP& yInRad) { return aboutYThenOldZ(yInRad, zInRad); }
+
+ /// Create a Rotation_ matrix by specifying only its z axis.
+ /// This will work for any stride UnitVec because there is always an implicit conversion available to the packed form used as the argument.
+ explicit Rotation_( const UnitVec3P& uvecZ ) { setRotationFromOneAxis(uvecZ,ZAxis); }
+
+ /// Create a Rotation_ matrix by specifying its x axis, and a "y like" axis.
+ // We will take x seriously after normalizing, but use the y only to create z = normalize(x X y),
+ // then y = z X x. Bad things happen if x and y are aligned but we may not catch it.
+ Rotation_( const Vec3P& x, const Vec3P& yish ) { setRotationFromTwoAxes( UnitVec3P(x), XAxis, yish, YAxis ); }
+
+ /// Set this Rotation_ to represent the same rotation as the passed-in quaternion.
+ void setToQuaternion( const QuaternionP& q ) { setRotationFromQuaternion(q); }
+
+ /// Set this Rotation_ to represent a rotation of +q0 about the body frame's Z axis,
+ /// followed by a rotation of +q1 about the body frame's NEW Y axis,
+ /// followed by a rotation of +q3 about the body frame's NEW X axis.
+ /// See Kane, Spacecraft Dynamics, pg. 423, body-three: 3-2-1.
+ // Similarly for BodyFixed123.
+ void setToBodyFixed321( const Vec3P& v) { setRotationFromThreeAnglesThreeAxes( BodyRotationSequence, v[0], ZAxis, v[1], YAxis, v[2], XAxis ); }
+ void setToBodyFixed123( const Vec3P& v) { setRotationToBodyFixedXYZ(v); }
+
+ /// Convert this Rotation_ matrix to an equivalent (angle,axis) representation:
+ /// The returned Vec4P is [angleInRadians, unitVectorX, unitVectorY, unitVectorZ].
+ Vec4P convertToAngleAxis() const { return convertRotationToAngleAxis(); }
+
+ /// Convert this Rotation_ matrix to equivalent quaternion representation.
+ QuaternionP convertToQuaternion() const { return convertRotationToQuaternion(); }
+
+ /// Set this Rotation_ to represent a rotation of +q0 about the base frame's X axis,
+ /// followed by a rotation of +q1 about the base frame's (unchanged) Y axis.
+ void setToSpaceFixed12( const Vec2P& q ) { setRotationFromTwoAnglesTwoAxes( SpaceRotationSequence, q[0], XAxis, q[1], YAxis ); }
+
+ /// Convert this Rotation_ matrix to the equivalent 1-2-3 body fixed Euler angle sequence.
+ /// Similarly, convert Rotation_ matrix to the equivalent 1-2 body fixed Euler angle sequence.
+ /// Similarly, convert Rotation_ matrix to the equivalent 1-2 space fixed Euler angle sequence.
+ Vec3P convertToBodyFixed123() const { return convertRotationToBodyFixedXYZ(); }
+ Vec2P convertToBodyFixed12() const { return convertRotationToBodyFixedXY(); }
+ Vec2P convertToSpaceFixed12() const { return convertTwoAxesRotationToTwoAngles( SpaceRotationSequence, XAxis, YAxis ); }
+};
+
+
+///-----------------------------------------------------------------------------
+/// This InverseRotation class is the inverse of a Rotation
+/// See the Rotation class for information.
+///-----------------------------------------------------------------------------
+template <class P>
+class InverseRotation_ : public Mat<3,3,P>::TransposeType {
+ typedef P RealP;
+ typedef Rotation_<P> RotationP;
+ typedef Mat<3,3,P> Mat33P; // not the base type!
+ typedef SymMat<3,P> SymMat33P;
+ typedef Mat<2,2,P> Mat22P;
+ typedef Mat<3,2,P> Mat32P;
+ typedef Vec<2,P> Vec2P;
+ typedef Vec<3,P> Vec3P;
+ typedef Vec<4,P> Vec4P;
+ typedef Quaternion_<P> QuaternionP;
+public:
+ /// This is the type of the underlying 3x3 matrix; note that it will have
+ /// unusual row and column spacing since we're viewing it as transposed.
+ typedef typename Mat<3,3,P>::TransposeType BaseMat;
+
+ /// Note that the unit vectors representing the rows and columns of this
+ /// matrix do not necessarily have unit stride.
+ //@{
+ /// This is the type of a column of this InverseRotation.
+ typedef UnitVec<P,BaseMat::RowSpacing> ColType;
+ /// This is the type of a row of this InverseRotation.
+ typedef UnitRow<P,BaseMat::ColSpacing> RowType;
+ //@}
+
+ /// You should not ever construct one of these as they should only occur as expression
+ /// intermediates resulting from use of the "~" operator on a Rotation.
+ /// But if you must, the default will produce an identity rotation.
+ InverseRotation_() : BaseMat(1) {}
+
+ /// This is an explicit implementation of the default copy constructor.
+ InverseRotation_( const InverseRotation_& R ) : BaseMat(R) {}
+ /// This is an explicit implementation of the default copy assignment operator.
+ InverseRotation_& operator=( const InverseRotation_& R )
+ { BaseMat::operator=(R.asMat33()); return *this; }
+
+ /// Assuming this InverseRotation_ is R_AB, and given a symmetric dyadic matrix S_BB expressed
+ /// in B, we can reexpress it in A using S_AA=R_AB*S_BB*R_BA. The matrix should be one
+ /// that is formed as products of vectors expressed in A, such as inertia, gyration or
+ /// covariance matrices. This can be done efficiently exploiting properties of R and S.
+ /// Cost is 57 flops.
+ /// @see Rotation::reexpressSymMat33()
+ SimTK_SimTKCOMMON_EXPORT SymMat33P reexpressSymMat33(const SymMat33P& S_BB) const;
+
+ /// We can invert an InverseRotation just by recasting it to a Rotation at zero cost.
+ //@{
+ const RotationP& invert() const {return *reinterpret_cast<const RotationP*>(this);}
+ RotationP& updInvert() {return *reinterpret_cast<RotationP*>(this);}
+ //@}
+
+ /// Transpose, and transpose operators (override BaseMat versions of transpose).
+ /// For an orthogonal matrix like this one transpose is the same as inverse.
+ //@{
+ const RotationP& transpose() const { return invert(); }
+ const RotationP& operator~() const { return invert(); }
+ RotationP& updTranspose() { return updInvert(); }
+ RotationP& operator~() { return updInvert(); }
+ //@}
+
+ /// Access individual rows and columns of this InverseRotation; no cost or
+ /// copying since suitably-cast references to the actual data are returned.
+ /// There are no writable versions of these methods since changing a single
+ /// row or column would violate the contract that these are always legitimate
+ /// rotation matrices.
+ //@{
+ const RowType& row( int i ) const { return reinterpret_cast<const RowType&>(asMat33()[i]); }
+ const ColType& col( int j ) const { return reinterpret_cast<const ColType&>(asMat33()(j)); }
+ const ColType& x() const { return col(0); }
+ const ColType& y() const { return col(1); }
+ const ColType& z() const { return col(2); }
+ const RowType& operator[]( int i ) const { return row(i); }
+ const ColType& operator()( int j ) const { return col(j); }
+ //@}
+
+ /// Conversion from InverseRotation_ to BaseMat.
+ /// Note: asMat33 is more efficient than toMat33() (no copy), but you have to know the internal layout.
+ //@{
+ const BaseMat& asMat33() const { return *static_cast<const BaseMat*>(this); }
+ BaseMat toMat33() const { return asMat33(); }
+ //@}
+};
+
+/// Write a Rotation matrix to an output stream by writing out its underlying Mat33.
+template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream&
+operator<<(std::ostream&, const Rotation_<P>&);
+/// Write an InverseRotation matrix to an output stream by writing out its underlying Mat33.
+template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream&
+operator<<(std::ostream&, const InverseRotation_<P>&);
+
+/// Rotating a unit vector leaves it unit length, saving us from having to perform
+/// an expensive normalization. So we override the multiply operators here changing
+/// the return type to UnitVec or UnitRow.
+//@{
+template <class P, int S> inline UnitVec<P,1>
+operator*(const Rotation_<P>& R, const UnitVec<P,S>& v) {return UnitVec<P,1>(R.asMat33()* v.asVec3(), true);}
+template <class P, int S> inline UnitRow<P,1>
+operator*(const UnitRow<P,S>& r, const Rotation_<P>& R) {return UnitRow<P,1>(r.asRow3() * R.asMat33(), true);}
+template <class P, int S> inline UnitVec<P,1>
+operator*(const InverseRotation_<P>& R, const UnitVec<P,S>& v) {return UnitVec<P,1>(R.asMat33()* v.asVec3(), true);}
+template <class P, int S> inline UnitRow<P,1>
+operator*(const UnitRow<P,S>& r, const InverseRotation_<P>& R) {return UnitRow<P,1>(r.asRow3() * R.asMat33(), true);}
+//@}
+
+// Couldn't implement these Rotation_ methods until InverseRotation_ was defined.
+template <class P> inline
+Rotation_<P>::Rotation_(const InverseRotation_<P>& R) : Mat<3,3,P>( R.asMat33() ) {}
+template <class P> inline Rotation_<P>&
+Rotation_<P>::operator=(const InverseRotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) = R.asMat33(); return *this;}
+template <class P> inline Rotation_<P>&
+Rotation_<P>::operator*=(const Rotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) *= R.asMat33(); return *this;}
+template <class P> inline Rotation_<P>&
+Rotation_<P>::operator/=(const Rotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) *= (~R).asMat33(); return *this;}
+template <class P> inline Rotation_<P>&
+Rotation_<P>::operator*=(const InverseRotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) *= R.asMat33(); return *this;}
+template <class P> inline Rotation_<P>&
+Rotation_<P>::operator/=(const InverseRotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) *= (~R).asMat33(); return *this;}
+
+/// Composition of Rotation matrices via operator*.
+//@{
+template <class P> inline Rotation_<P>
+operator*(const Rotation_<P>& R1, const Rotation_<P>& R2) {return Rotation_<P>(R1) *= R2;}
+template <class P> inline Rotation_<P>
+operator*(const Rotation_<P>& R1, const InverseRotation_<P>& R2) {return Rotation_<P>(R1) *= R2;}
+template <class P> inline Rotation_<P>
+operator*(const InverseRotation_<P>& R1, const Rotation_<P>& R2) {return Rotation_<P>(R1) *= R2;}
+template <class P> inline Rotation_<P>
+operator*(const InverseRotation_<P>& R1, const InverseRotation_<P>& R2) {return Rotation_<P>(R1) *= R2;}
+//@}
+
+/// Composition of a Rotation matrix and the inverse of another Rotation via operator/, that is
+/// R1/R2 == R1*(~R2).
+//@{
+template <class P> inline Rotation_<P>
+operator/( const Rotation_<P>& R1, const Rotation_<P>& R2 ) {return Rotation_<P>(R1) /= R2;}
+template <class P> inline Rotation_<P>
+operator/( const Rotation_<P>& R1, const InverseRotation& R2 ) {return Rotation_<P>(R1) /= R2;}
+template <class P> inline Rotation_<P>
+operator/( const InverseRotation_<P>& R1, const Rotation_<P>& R2 ) {return Rotation_<P>(R1) /= R2;}
+template <class P> inline Rotation_<P>
+operator/( const InverseRotation_<P>& R1, const InverseRotation_<P>& R2 ) {return Rotation_<P>(R1) /= R2;}
+//@}
+
+
+//------------------------------------------------------------------------------
+} // End of namespace SimTK
+
+//--------------------------------------------------------------------------
+#endif // SimTK_SimTKCOMMON_ROTATION_H_
+//--------------------------------------------------------------------------
+
+
diff --git a/SimTKcommon/Mechanics/include/SimTKcommon/internal/SpatialAlgebra.h b/SimTKcommon/Mechanics/include/SimTKcommon/internal/SpatialAlgebra.h
new file mode 100644
index 0000000..6b6e190
--- /dev/null
+++ b/SimTKcommon/Mechanics/include/SimTKcommon/internal/SpatialAlgebra.h
@@ -0,0 +1,786 @@
+#ifndef SimTK_SIMMATRIX_SPATIAL_ALGEBRA_H_
+#define SimTK_SIMMATRIX_SPATIAL_ALGEBRA_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+These are declarations for special matrices and vectors of use in implementing
+Rodriguez and Jain's Spatial Operator Algebra. **/
+
+#include "SimTKcommon/SmallMatrix.h"
+
+#include <iostream>
+
+namespace SimTK {
+
+
+/**@defgroup SpatialAlgebraUtilities Spatial Algebra Utilities
+ @ingroup GlobalFunctions
+
+These utility functions are used for manipulation of spatial quantities that
+are contained in SpatialVec or SpatialMat objects. These are intended for
+expert use and are mostly used in the implemention of friendlier methods such
+as those in MobilizedBody that are used to obtain various spatial quantities.
+
+ at note Although we use SpatialVec for both, there are two different spatial
+vector bases: one for motion quantities like velocities, accelerations, and
+momentum and another for forces and impulses; be sure to use the appropriate
+functions. Also, we use a pair of ordinary vectors, following Abhi Jain,
+rather than the similar but subtly different Plucker basis vectors used by
+Roy Featherstone.
+
+Spatial vectors are used for combined (rotational,translational) quantities.
+These include
+ at verbatim
+ spatial velocity = (angularVelocity,linearVelocity)
+ spatial acceleration = (angularAcceleration,linearAcceleration)
+ spatial force = (moment,force)
+ at endverbatim
+
+Spatial configuration (pose) has to be handled differently though since
+orientation is not a vector quantity. We use the Transform class for this
+concept, which includes an orientation matrix and a translation vector.
+ at see Transform **/
+/**@{**/
+
+/** SpatialVec[0] is the rotational component; [1] is translational. **/
+typedef Vec<2, Vec3> SpatialVec;
+/** This is the type of a transposed SpatialVec. **/
+typedef Row<2, Row3> SpatialRow;
+/** This is used for primarily for spatial mass properties. **/
+typedef Mat<2,2, Mat33> SpatialMat;
+
+
+// Pre-declare methods here so that we can list them in whatever order we'd
+// like them to appear in Doxygen.
+inline SpatialVec findRelativeVelocity( const Transform& X_FA,
+ const SpatialVec& V_FA,
+ const Transform& X_FB,
+ const SpatialVec& V_FB);
+inline SpatialVec findRelativeVelocityInF( const Vec3& p_AB_F,
+ const SpatialVec& V_FA,
+ const SpatialVec& V_FB);
+
+inline SpatialVec findRelativeAcceleration( const Transform& X_FA,
+ const SpatialVec& V_FA,
+ const SpatialVec& A_FA,
+ const Transform& X_FB,
+ const SpatialVec& V_FB,
+ const SpatialVec& A_FB);
+inline SpatialVec findRelativeAccelerationInF( const Vec3& p_AB_F,
+ const SpatialVec& V_FA,
+ const SpatialVec& A_FA,
+ const SpatialVec& V_FB,
+ const SpatialVec& A_FB);
+
+inline SpatialVec reverseRelativeVelocity(const Transform& X_AB,
+ const SpatialVec& V_AB);
+inline SpatialVec reverseRelativeVelocityInA(const Transform& X_AB,
+ const SpatialVec& V_AB);
+
+inline SpatialVec shiftVelocityBy(const SpatialVec& V_AB, const Vec3& r_A);
+inline SpatialVec shiftVelocityFromTo(const SpatialVec& V_A_BP,
+ const Vec3& fromP_A,
+ const Vec3& toQ_A);
+
+inline SpatialVec shiftForceBy(const SpatialVec& F_AP, const Vec3& r_A);
+inline SpatialVec shiftForceFromTo(const SpatialVec& F_AP,
+ const Vec3& fromP_A,
+ const Vec3& toQ_A);
+
+
+
+//==============================================================================
+// FIND RELATIVE VELOCITY
+//==============================================================================
+/** @brief Find the relative spatial velocity between two frames A and B whose
+individual spatial velocities are known with respect to a third frame F, with
+the result returned in A.
+
+ at param[in] X_FA
+ The pose of frame A measured and expressed in frame F.
+ at param[in] V_FA
+ The spatial velocity of frame A measured and expressed in frame F.
+ at param[in] X_FB
+ The pose of frame B measured and expressed in frame F.
+ at param[in] V_FB
+ The spatial velocity of frame B measured and expressed in frame F.
+ at return V_AB, the relative spatial velocity of frame B in frame A, expressed
+ in A.
+
+Given the spatial velocity V_FA of frame A in a reference frame F, and
+the spatial velocity V_FB of frame B in F, and transforms giving the poses of
+frames A and B in F, calculate the relative velocity V_AB of frame B in
+frame A, measured and expressed in A. Typical usage:
+ at code
+ Transform X_GA, X_GB; // assume these are known from somewhere
+ SpatialVec V_GA, V_GB;
+
+ SpatialVec V_AB = findRelativeVelocity(X_GA, V_GA,
+ X_GB, V_GB);
+ at endcode
+ at note This returns the result expressed in A which is almost always what you
+want; however, if you don't want it in that frame you can save 30 flops by
+calling findRelativeVelocityInF() instead.
+
+Cost is 51 flops. @see findRelativeVelocityInF() **/
+inline SpatialVec findRelativeVelocity(const Transform& X_FA,
+ const SpatialVec& V_FA,
+ const Transform& X_FB,
+ const SpatialVec& V_FB)
+{
+ const Vec3 p_AB_F = X_FB.p() - X_FA.p(); // 3 flops
+ return ~X_FA.R()*findRelativeVelocityInF(p_AB_F,V_FA,V_FB); // 48 flops
+}
+
+
+
+//==============================================================================
+// FIND RELATIVE VELOCITY IN F
+//==============================================================================
+/** @brief Find the relative spatial velocity between two frames A and B whose
+individual spatial velocities are known in a third frame F, but leave the
+result in F.
+
+ at param[in] p_AB_F
+ The vector from the A frame origin OA to the B frame origin OB, but
+ expressed in frame F.
+ at param[in] V_FA
+ The spatial velocity of frame A measured and expressed in frame F.
+ at param[in] V_FB
+ The spatial velocity of frame B measured and expressed in frame F.
+ at return V_AB_F, the relative spatial velocity of frame B in frame A, but still
+ expressed in F.
+
+Typically the relative velocity of B in A would be returned in A; most
+users will want to use findRelativeVelocity() instead which returns the result
+in A. Use of this method saves the substantial cost of reexpressing the result,
+so is useful in the rare case that you don't want the final result in A.
+Example:
+ at code
+ Transform X_GA, X_GB; // assume these are known from somewhere
+ SpatialVec V_GA, V_GB;
+
+ const Vec3 p_AB_G = X_GB.p() - X_GA.p();
+ SpatialVec V_AB_G = findRelativeVelocityInF(p_AB_G, V_GA, V_GB);
+ at endcode
+Cost is 18 flops. @see findRelativeVelocity() **/
+inline SpatialVec findRelativeVelocityInF(const Vec3& p_AB_F,
+ const SpatialVec& V_FA,
+ const SpatialVec& V_FB)
+{
+ // Relative angular velocity of B in A, expressed in F.
+ const Vec3 w_AB_F = V_FB[0] - V_FA[0]; // 3 flops
+ // Relative linear velocity of B in A, taken and expressed in F.
+ const Vec3 p_AB_F_dot = V_FB[1] - V_FA[1]; // 3 flops
+ // Get linear velocity taken in A by removing the component due
+ // to A's rotation in F (still expressed in F).
+ const Vec3 v_AB_F = p_AB_F_dot - V_FA[0] % p_AB_F; // 12 flops
+
+ return SpatialVec(w_AB_F, v_AB_F);
+}
+
+
+
+//==============================================================================
+// FIND RELATIVE ACCELERATION
+//==============================================================================
+/** @brief Find the relative spatial acceleration between two frames A and B
+whose individual spatial accelerations are known with respect to a third
+frame F, with the result returned in A.
+
+ at param[in] X_FA
+ The pose of frame A measured and expressed in frame F.
+ at param[in] V_FA
+ The spatial velocity of frame A measured and expressed in frame F.
+ at param[in] A_FA
+ The spatial acceleration of frame A measured and expressed in frame F.
+ at param[in] X_FB
+ The pose of frame B measured and expressed in frame F.
+ at param[in] V_FB
+ The spatial velocity of frame B measured and expressed in frame F.
+ at param[in] A_FB
+ The spatial acceleration of frame B measured and expressed in frame F.
+ at return A_AB, the relative spatial acceleration of frame B in frame A,
+ expressed in A.
+
+Given the spatial acceleration A_FA of frame A in a reference frame F, and
+the spatial acceleration A_FB of frame B in F, and corresonding pose and
+velcoity information, calculate the relative acceleration A_AB of frame B in
+frame A, measured and expressed in A. Typical usage:
+ at code
+ Transform X_GA, X_GB; // assume these are known from somewhere
+ SpatialVec V_GA, V_GB;
+ SpatialVec A_GA, A_GB;
+
+ SpatialVec A_AB = findRelativeAcceleration(X_GA, V_GA, A_GA,
+ X_GB, V_GB, A_GB);
+ at endcode
+ at note This returns the result expressed in A which is almost always what you
+want; however, if you don't want it in that frame you can save 30 flops by
+calling findRelativeAccelerationInF() instead.
+
+Cost is 105 flops. @see findRelativeAccelerationInF() **/
+inline SpatialVec findRelativeAcceleration( const Transform& X_FA,
+ const SpatialVec& V_FA,
+ const SpatialVec& A_FA,
+ const Transform& X_FB,
+ const SpatialVec& V_FB,
+ const SpatialVec& A_FB)
+{
+ const Vec3 p_AB_F = X_FB.p() - X_FA.p(); // 3 flops
+ return ~X_FA.R() * // 30 flops
+ findRelativeAccelerationInF(p_AB_F,V_FA,A_FA,V_FB,A_FB); // 72 flops
+}
+
+
+
+//==============================================================================
+// FIND RELATIVE ACCELERATION IN F
+//==============================================================================
+/** @brief Find the relative spatial acceleration between two frames A and B
+whose individual spatial acceleration are known in a third frame F, but leave
+the result in F.
+
+ at param[in] p_AB_F
+ The vector from the A frame origin OA to the B frame origin OB, but
+ expressed in frame F.
+ at param[in] V_FA
+ The spatial velocity of frame A measured and expressed in frame F.
+ at param[in] A_FA
+ The spatial acceleration of frame A measured and expressed in frame F.
+ at param[in] V_FB
+ The spatial velocity of frame B measured and expressed in frame F.
+ at param[in] A_FB
+ The spatial acceleration of frame B measured and expressed in frame F.
+ at return A_AB_F, the relative spatial acceleration of frame B in frame A, but
+ still expressed in F.
+
+Typically the relative acceleration of B in A would be returned in A; most
+users will want to use findRelativeAcceleration() instead which returns the
+result in A. Use of this method saves the substantial cost of reexpressing the
+result, so is useful in the rare case that you don't want the final result in A.
+Example:
+ at code
+ Transform X_GA, X_GB; // assume these are known from somewhere
+ SpatialVec V_GA, V_GB;
+ SpatialVec A_GA, A_GB;
+
+ const Vec3 p_AB_G = X_GB.p() - X_GA.p();
+ SpatialVec V_AB_G = findRelativeAccelerationInF(p_AB_G, V_GA, A_GA,
+ V_GB, A_GB);
+ at endcode
+Cost is 72 flops. @see findRelativeAcceleration() **/
+inline SpatialVec findRelativeAccelerationInF( const Vec3& p_AB_F,
+ const SpatialVec& V_FA,
+ const SpatialVec& A_FA,
+ const SpatialVec& V_FB,
+ const SpatialVec& A_FB)
+{
+ const Vec3& w_FA = V_FA[0]; // aliases for convenience
+ const Vec3& w_FB = V_FB[0];
+ const Vec3& b_FA = A_FA[0];
+ const Vec3& b_FB = A_FB[0];
+
+ const Vec3 p_AB_F_dot = V_FB[1] - V_FA[1]; // d/dt p taken in F (3 flops)
+ const Vec3 p_AB_F_dotdot = A_FB[1] - A_FA[1]; // d^2/dt^2 taken in F (3 flops)
+
+ const Vec3 w_AB_F = // relative angvel of B in A, exp. in F
+ w_FB - w_FA; // (3 flops)
+ const Vec3 v_AB_F = // d/dt p taken in A, exp in F
+ p_AB_F_dot - w_FA % p_AB_F; // (12 flops)
+
+ const Vec3 w_AB_F_dot = b_FB - b_FA; // d/dt of w_AB_F taken in F (3 flops)
+ const Vec3 v_AB_F_dot = // d/dt v_AB_F taken in F
+ p_AB_F_dotdot - (b_FA % p_AB_F + w_FA % p_AB_F_dot); // (24 flops)
+
+ // We have the derivative in F; change it to derivative in A by adding in
+ // contribution caused by motion of F in A, that is w_AF X w_AB_F. (Note
+ // that w_AF=-w_FA.)
+ const Vec3 b_AB_F = // ang. accel. of B in A, exp. in F
+ w_AB_F_dot - w_FA % w_AB_F; // (12 flops)
+ const Vec3 a_AB_F = // taken in A, exp. in F
+ v_AB_F_dot - w_FA % v_AB_F; // (12 flops)
+
+ return SpatialVec(b_AB_F, a_AB_F); // taken in A, expressed in F
+}
+
+
+
+//==============================================================================
+// REVERSE RELATIVE VELOCITY
+//==============================================================================
+/** @brief Given the relative velocity of frame B in frame A, reverse that to
+give the relative velocity of frame A in B.
+
+ at param[in] X_AB
+ The pose of frame B in frame A, measured and expressed in A.
+ at param[in] V_AB
+ The relative spatial velocity of frame B in frame A, measured and
+ expressed in frame A.
+ at return V_BA, the relative spatial velocity of frame A in frame B, measured
+ and expressed in B.
+
+The input is expressed in the A frame; the result will be expressed in the B
+frame instead. If you prefer that the result remain in the A frame you should
+call reverseRelativeVelocityInA() instead to avoid the extra cost of changing
+frames. Example:
+ at code
+ Transform X_AB; // assume these are known from somewhere
+ SpatialVec V_AB;
+
+ SpatialVec V_BA = reverseRelativeVelocity(X_AB, V_AB);
+ at endcode
+ at note If the frame origins were in the same spatial location, then the result
+would just be the negative of the supplied velocity. However, since the linear
+component of spatial velocity has to be measured at a point, and we're
+switching from measuring at a point coincident with B's origin OB to one
+coincident with A's origin OA, there is going to be a change in the linear
+part of the result. The angular velocity will just be negated, though, and
+then reexpressed in B.
+
+Cost is 51 flops. @see reverseRelativeVelocityInA() **/
+inline SpatialVec reverseRelativeVelocity(const Transform& X_AB,
+ const SpatialVec& V_AB)
+{
+ // Reverse the velocity but with the result still expressed in A.
+ const SpatialVec V_BA_A = reverseRelativeVelocityInA(X_AB,V_AB);
+ // 21 flops
+ // Then reexpress in B.
+ return ~X_AB.R()*V_BA_A; // 30 flops
+}
+
+
+
+//==============================================================================
+// REVERSE RELATIVE VELOCITY IN A
+//==============================================================================
+/** @brief Given the relative velocity of frame B in frame A, reverse that to
+give the relative velocity of frame A in B, but leave the result expressed
+in frame A.
+
+ at param[in] X_AB
+ The pose of frame B in frame A, measured and expressed in A.
+ at param[in] V_AB
+ The relative spatial velocity of frame B in frame A, measured and
+ expressed in frame A.
+ at return V_BA_A, the relative velocity of frame A in frame B, but still
+ expressed in A.
+
+The input V_AB is expressed in the A frame; you will almost always want the
+output V_BA expressed in the B frame which is what the function
+reverseRelativeVelocity() does. However, if you're going to want it in
+some other frame ultimately you may prefer to avoid the substantial cost of
+reexpressing it in B now, in which case this routine is useful.
+
+See reverseRelativeVelocity() for more information about what this does.
+Example:
+ at code
+ Transform X_AB; // assume these are known from somewhere
+ SpatialVec V_AB; // (expressed in A)
+
+ // result is still expressed in A
+ SpatialVec V_BA_A = reverseRelativeVelocityInA(X_AB, V_AB);
+ at endcode
+
+Cost is 21 flops. @see reverseRelativeVelocity() **/
+inline SpatialVec reverseRelativeVelocityInA(const Transform& X_AB,
+ const SpatialVec& V_AB)
+{
+ // Change the measurement point from a point coincident with OB
+ // to a point coincident with OA, and negate since we want A's velocity
+ // in B rather than the other way around.
+ const SpatialVec V_BA_A = -shiftVelocityBy(V_AB, -X_AB.p()); // 21 flops
+ return V_BA_A;
+}
+
+
+
+//==============================================================================
+// SHIFT VELOCITY BY
+//==============================================================================
+/** @brief Shift a relative spatial velocity measured at some point to that
+same relative spatial quantity but measured at a new point given by an offset
+from the old one.
+
+ at param[in] V_AB
+ The relative spatial velocity of frame B in frame A, measured and
+ expressed in frame A.
+ at param[in] r_A
+ The vector offset, expressed in frame A, by which to change the point at
+ which the translational component of the relative spatial velocity is
+ measured.
+ at return V_A_BQ, the relative velocity of frame B in frame A, but measured at
+ the point Q=Bo+r rather than at B's origin Bo.
+
+Given the spatial velocity V_AB of frame B in A, measured at a point
+coincident with B's origin Bo, change it to the spatial velocity V_A_BQ
+representing the same relationship but with the velocity measured at a new
+point Q=Bo+r for some position vector r. All vectors are measured and expressed
+in frame A, including the vector r. Example:
+ at code
+ SpatialVec V_AB; // assume these are known from somewhere
+ Vec3 offset_A; // Q = Bo + offset
+
+ SpatialVec V_A_BQ = shiftVelocityBy(V_AB, offset_A);
+ at endcode
+
+ at note The shift in location leaves the relative angular velocity w the same but
+results in the linear velocity changing by w X r.
+
+Cost is 12 flops. @see shiftVelocityFromTo() **/
+inline SpatialVec shiftVelocityBy(const SpatialVec& V_AB, const Vec3& r_A)
+{ return SpatialVec( V_AB[0], V_AB[1] + V_AB[0] % r_A ); } // vp=v + wXr
+
+
+//==============================================================================
+// SHIFT VELOCITY FROM TO
+//==============================================================================
+/** @brief Shift a relative spatial velocity measured at some point P to that
+same relative spatial quantity but measured at a new point Q given the points
+P and Q.
+
+ at param[in] V_A_BP
+ The relative spatial velocity of frame B in frame A, measured and
+ expressed in frame A, with the linear component measured at a point P.
+ at param[in] fromP_A
+ The "from" point P at which the input linear velocity was measured, given
+ as a vector from A's origin OA to the point P, expressed in A.
+ at param[in] toQ_A
+ The "to" point Q at which we want to re-measure the linear velocity, given
+ as a vector from A's origin OA to the point Q, expressed in A.
+ at return V_A_BQ, the relative velocity of frame B in frame A, but measured at
+ the point Q rather than at point P.
+
+Given the spatial velocity V_A_BP of frame B in A, measured at a point P,
+change it to the spatial velocity V_A_BQ representing the same relationship but
+with the velocity measured at a new point Q. Example:
+ at code
+ // assume these are known from somewhere
+ Transform X_AB; // contains the vector from OA to OB
+ SpatialVec V_AB; // linear velocity is measured at origin OB of B
+ Vec3 p_AQ; // vector from OA to some other point Q, in A
+
+ SpatialVec V_A_BQ = shiftVelocityFromTo(V_AB, X_AB.p(), p_AQ);
+ at endcode
+
+ at note There is no way to know whether the supplied velocity was actually
+measured at P; this method really just shifts the relative velocity by
+the vector r=(to-from). Use it carefully.
+
+Cost is 15 flops. @see shiftVelocityBy() **/
+inline SpatialVec shiftVelocityFromTo(const SpatialVec& V_A_BP,
+ const Vec3& fromP_A,
+ const Vec3& toQ_A)
+{ return shiftVelocityBy(V_A_BP, toQ_A - fromP_A); }
+
+
+
+//==============================================================================
+// SHIFT ACCELERATION BY
+//==============================================================================
+/** @brief Shift a relative spatial acceleration measured at some point to that
+same relative spatial quantity but measured at a new point given by an offset
+from the old one.
+
+ at param[in] A_AB
+ The relative spatial acceleration of frame B in frame A, measured and
+ expressed in frame A.
+ at param[in] w_AB
+ The relative angular velocity of frame B in frame A, expressed in frame A.
+ at param[in] r_A
+ The vector offset, expressed in frame A, by which to change the point at
+ which the translational component of the relative spatial acceleration is
+ measured.
+ at return A_A_BQ, the relative acceleration of frame B in frame A, but measured at
+ the point Q=Bo+r rather than at B's origin Bo.
+
+Given the spatial acceleration A_AB and angular velocity w_AB of frame B in A,
+measured at a point coincident with B's origin Bo, change it to the spatial
+acceleration A_A_BQ representing the same relationship but with the acceleration
+measured at a new point Q=Bo+r for some position vector r. All vectors are
+measured and expressed in frame A, including the vector r. Example:
+ at code
+ SpatialVec A_AB; // assume these are known from somewhere
+ Vec3 w_AB;
+ Vec3 offset_A; // Q = Bo + offset
+
+ SpatialVec A_A_BQ = shiftAccelerationBy(A_AB, w_AB, offset_A);
+ at endcode
+
+ at note The shift in location leaves the relative angular acceleration b the same
+but results in the linear acceleration changing by b X r + w X (w X r).
+
+Cost is 33 flops. @see shiftAccelerationFromTo() **/
+inline SpatialVec shiftAccelerationBy(const SpatialVec& A_AB,
+ const Vec3& w_AB,
+ const Vec3& r_A)
+{ return SpatialVec( A_AB[0],
+ A_AB[1] + A_AB[0] % r_A + w_AB % (w_AB % r_A) ); }
+
+
+
+//==============================================================================
+// SHIFT ACCELERATION FROM TO
+//==============================================================================
+/** @brief Shift a relative spatial acceleration measured at some point P to
+that same relative spatial quantity but measured at a new point Q given the
+points P and Q.
+
+ at param[in] A_A_BP
+ The relative spatial acceleration of frame B in frame A, measured and
+ expressed in frame A, with the linear component measured at a point P.
+ at param[in] w_AB
+ The relative angular velocity of frame B in frame A, expressed in frame A.
+ at param[in] fromP_A
+ The "from" point P at which the input linear acceleration was
+ measured, given as a vector from A's origin Ao to the point P,
+ expressed in A.
+ at param[in] toQ_A
+ The "to" point Q at which we want to re-measure the linear acceleration,
+ given as a vector from A's origin Ao to the point Q, expressed
+ in A.
+ at return A_A_BQ, the relative acceleration of frame B in frame A, but measured at
+ the point Q rather than at point P.
+
+Given the spatial acceleration A_A_BP of frame B in A, measured at a point P,
+change it to the spatial acceleration A_A_BQ representing the same relationship
+but with the acceleration measured at a new point Q. Example:
+ at code
+ // assume these are known from somewhere
+ Transform X_AB; // contains the vector from Ao to Bo
+ SpatialVec A_AB; // linear acceleration is measured at origin Bo of B
+ Vec3 w_AB;
+ Vec3 p_AQ; // vector from Ao to some other point Q, in A
+
+ SpatialVec A_A_BQ = shiftAccelerationFromTo(A_AB, w_AB, X_AB.p(), p_AQ);
+ at endcode
+
+ at note There is no way to know whether the supplied acceleration was
+actually measured at P; this method really just shifts the relative
+acceleration by the vector r=(to-from). Use it carefully.
+
+Cost is 36 flops. @see shiftAccelerationBy() **/
+inline SpatialVec shiftAccelerationFromTo(const SpatialVec& A_A_BP,
+ const Vec3& w_AB,
+ const Vec3& fromP_A,
+ const Vec3& toQ_A)
+{ return shiftAccelerationBy(A_A_BP, w_AB, toQ_A - fromP_A); }
+
+
+
+//==============================================================================
+// SHIFT FORCE BY
+//==============================================================================
+/** @brief Shift a spatial force applied at some point of a body to that
+same spatial force applied at a new point given by an offset
+from the old one.
+
+ at param[in] F_AP
+ A spatial force (moment and linear force), expressed in the A frame,
+ whose translational component is applied at a point P.
+ at param[in] r_A
+ The vector offset, expressed in frame A, by which to change the point at
+ which the translational component of the input force is to be applied.
+ at return F_AQ, the same physical effect as the input but with the moment
+ adjusted to reflect force application at point Q=P+r rather than at the
+ original point P.
+
+Given the spatial force F_AP including a pure moment m and a force vector f
+applied at a point P, return the equivalent force F_AQ representing the same
+physical quantity but as though the force were applied at a point Q=P+r
+for some position vector r. All vectors are expressed in frame A. Example:
+ at code
+ SpatialVec F_AP; // assume these are known from somewhere
+ Vec3 offset_A; // Q = P + offset
+
+ SpatialVec F_AQ = shiftForceBy(F_AP, offset_A);
+ at endcode
+
+ at note The shift in location leaves the force f the same but
+results in an adjustment to the moment of -(r X f).
+
+Cost is 12 flops. @see shiftForceFromTo() **/
+inline SpatialVec shiftForceBy(const SpatialVec& F_AP, const Vec3& r_A)
+{ return SpatialVec(F_AP[0] - r_A % F_AP[1], F_AP[1]); } // mq = mp - r X f
+
+
+
+//==============================================================================
+// SHIFT FORCE FROM TO
+//==============================================================================
+/** @brief Shift a spatial force applied at some point P of a body to that
+same spatial force applied at a new point Q, given P and Q.
+
+ at param[in] F_AP
+ A spatial force (moment and linear force), expressed in the A frame, whose
+ translational component is applied at a point P.
+ at param[in] fromP_A
+ The "from" point P at which the input force is applied, given as a
+ vector from A's origin OA to the point P, expressed in A.
+ at param[in] toQ_A
+ The "to" point Q to which we want to move the force application point,
+ given as a vector from A's origin OA to the point Q, expressed in A.
+ at return F_AQ, the same physical effect as the input but with the moment
+ adjusted to reflect force application at point Q rather than at the
+ original point P.
+
+Given the spatial force F_AP including a pure moment m and a force vector f
+applied at a point P, return the equivalent force F_AQ representing the same
+physical quantity but as though the force were applied at a new point Q. All
+vectors are expressed in frame A and points are measured from A's origin OA.
+Example:
+ at code
+ // assume these are known from somewhere
+ SpatialVec F_AP; // linear force is applied at point P
+ Vec3 p_AP; // vector from OA to P, in A
+ Vec3 p_AQ; // vector from OA to some other point Q, in A
+
+ SpatialVec F_AQ = shiftForceFromTo(F_AP, p_AP, p_AQ);
+ at endcode
+
+ at note There is no way to know whether the supplied force was actually
+applied at P; this method really just shifts the application point by
+the vector r=(to-from). Use it carefully.
+
+Cost is 15 flops. @see shiftForceBy() **/
+inline SpatialVec shiftForceFromTo(const SpatialVec& F_AP,
+ const Vec3& fromP_A,
+ const Vec3& toQ_A)
+{ return shiftForceBy(F_AP, toQ_A - fromP_A); }
+
+/**@}**/
+
+
+
+//==============================================================================
+// PHI MATRIX
+//==============================================================================
+// support for efficient matrix multiplication involving the special phi
+// matrix
+
+class PhiMatrixTranspose;
+
+class PhiMatrix {
+public:
+ typedef PhiMatrixTranspose TransposeType;
+
+ PhiMatrix() { setToNaN(); }
+ PhiMatrix(const Vec3& l) : l_(l) {}
+
+ void setToZero() { l_ = 0; }
+ void setToNaN() { l_.setToNaN(); }
+
+ SpatialMat toSpatialMat() const {
+ return SpatialMat(Mat33(1), crossMat(l_),
+ Mat33(0), Mat33(1));
+ }
+
+ const Vec3& l() const { return l_; }
+private:
+ Vec3 l_;
+};
+
+class PhiMatrixTranspose {
+public:
+ PhiMatrixTranspose(const PhiMatrix& phi) : phi(phi) {}
+
+ SpatialMat toSpatialMat() const {
+ return SpatialMat( Mat33(1) , Mat33(0),
+ crossMat(-l()) , Mat33(1));
+ }
+
+ const Vec3& l() const {return phi.l();}
+private:
+ const PhiMatrix& phi;
+};
+
+inline PhiMatrixTranspose
+transpose(const PhiMatrix& phi)
+{
+ PhiMatrixTranspose ret(phi);
+ return ret;
+}
+
+inline PhiMatrixTranspose
+operator~(const PhiMatrix& phi) {return transpose(phi);}
+
+inline SpatialVec
+operator*(const PhiMatrix& phi,
+ const SpatialVec& v)
+{
+ return SpatialVec(v[0] + phi.l() % v[1], // 12 flops
+ v[1]);
+}
+
+inline SpatialMat
+operator*(const PhiMatrix& phi,
+ const SpatialMat& m)
+{
+ const Mat33 x = crossMat(phi.l()); // 3 flops
+ return SpatialMat( m(0,0) + x*m(1,0), m(0,1) + x*m(1,1), // 108 flops
+ m(1,0) , m(1,1));
+}
+
+inline SpatialVec
+operator*(const PhiMatrixTranspose& phiT,
+ const SpatialVec& v)
+{
+ return SpatialVec(v[0],
+ v[1] + v[0] % phiT.l()); // 12 flops
+}
+
+
+inline SpatialMat
+operator*(const SpatialMat::THerm& m,
+ const PhiMatrixTranspose& phiT)
+{
+ const Mat33 x = crossMat(phiT.l()); // 3 flops
+ return SpatialMat( m(0,0) - m(0,1) * x, m(0,1), // 54 flops
+ m(1,0) - m(1,1) * x, m(1,1) ); // 54 flops
+}
+
+inline SpatialMat
+operator*(const SpatialMat& m,
+ const PhiMatrixTranspose& phiT)
+{
+ const Mat33 x = crossMat(phiT.l()); // 3 flops
+ return SpatialMat( m(0,0) - m(0,1) * x, m(0,1), // 54 flops
+ m(1,0) - m(1,1) * x, m(1,1) ); // 54 flops
+}
+
+inline bool
+operator==(const PhiMatrix& p1, const PhiMatrix& p2)
+{
+ return p1.l() == p2.l();
+}
+
+inline bool
+operator==(const PhiMatrixTranspose& p1, const PhiMatrixTranspose& p2)
+{
+ return p1.l() == p2.l();
+}
+} // namespace SimTK
+
+#endif // SimTK_SIMMATRIX_SPATIAL_ALGEBRA_H_
diff --git a/SimTKcommon/Mechanics/include/SimTKcommon/internal/Transform.h b/SimTKcommon/Mechanics/include/SimTKcommon/internal/Transform.h
new file mode 100644
index 0000000..e2315a4
--- /dev/null
+++ b/SimTKcommon/Mechanics/include/SimTKcommon/internal/Transform.h
@@ -0,0 +1,620 @@
+#ifndef SimTK_SimTKCOMMON_TRANSFORM_H
+#define SimTK_SimTKCOMMON_TRANSFORM_H
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy *
+ * *
+ * 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/SmallMatrix.h"
+#include "SimTKcommon/internal/BigMatrix.h"
+#include "SimTKcommon/internal/UnitVec.h"
+#include "SimTKcommon/internal/Quaternion.h"
+#include "SimTKcommon/internal/Rotation.h"
+//-----------------------------------------------------------------------------
+#include <iosfwd> // Forward declaration of iostream
+//-----------------------------------------------------------------------------
+
+
+//-----------------------------------------------------------------------------
+namespace SimTK {
+
+//-----------------------------------------------------------------------------
+// Forward declarations (everything is templatized by precision).
+template <class P> class Transform_;
+template <class P> class InverseTransform_;
+
+typedef Transform_<Real> Transform;
+typedef Transform_<float> fTransform;
+typedef Transform_<double> dTransform;
+
+
+//-----------------------------------------------------------------------------
+/**
+ * This class represents the rotate-and-shift transform which gives the
+ * location and orientation of a new frame F in a base (reference) frame
+ * B. A frame is an orthogonal, right-handed set of three axes, and an
+ * origin point. A transform X from frame B to F consists of 3 perpendicular
+ * unit vectors defining F's axes as viewed from B (that is, as expressed in
+ * the basis formed by B's axes), and a vector from B's origin point OB to F's
+ * origin point OF. Note that the meaning of "B" comes from the context in
+ * which the transform is used. We use the phrase "frame F is in frame B" to
+ * describe the above relationship, that is, "in" means both measured from
+ * and expressed in.
+ *
+ * The axis vectors constitute a Rotation. They are ordered 1-2-3 or x-y-z
+ * as you prefer, with z = x X y, making a right-handed set. These axes are
+ * arranged as columns of a 3x3 rotation matrix R_BF = [ x y z ] which is a
+ * direction cosine (rotation) matrix useful for conversions between frame
+ * B and F. (The columns of R_BF are F's coordinate axes, expressed in B.) For
+ * example, given a vector vF expressed in the F frame, that same vector
+ * re-expressed in B is given by vB = R_BF*vF. F's origin point OF is
+ * stored as the translation vector p_BF=(OF-OB) and expressed in B.
+ *
+ * Transform is designed to behave as much as possible like the computer
+ * graphics 4x4 transform X which would be arranged like this:
+ * <pre>
+ *
+ * [ | ]
+ * X = [ R | p ] R is a 3x3 orthogonal rotation matrix
+ * [.......|...] p os a 3x1 translation vector
+ * [ 0 0 0 1 ]
+ * </pre>
+ *
+ * These can be composed directly by matrix multiplication, but more
+ * importantly they have a particularly simple inverse:
+ * <pre>
+ *
+ * -1 [ | ]
+ * X = [ ~R | p* ] ~R is R transpose, p* = ~R(-p).
+ * [.......|....]
+ * [ 0 0 0 1 ]
+ * </pre>
+ *
+ * This inverse is so simple that we compute it simply by defining another
+ * type, InverseTransform, which is identical to Transform in memory but
+ * behaves as though it contains the inverse. That way we invert just by
+ * changing point of view (recasting) rather than computing.
+ *
+ * This is a "POD" (plain old data) class with a well-defined memory
+ * layout on which a client of this class may depend: There are
+ * exactly 4 consecutive, packed 3-vectors in the order x,y,z,p.
+ * That is, this class is equivalent to an array of 12 Reals with
+ * the order x1,x2,x3,y1,y2,y3,z1,z2,z3,p1,p2,p3. It is expressly allowed
+ * to reinterpret Transform objects in any appropriate manner that depends
+ * on this memory layout.
+ */
+//-----------------------------------------------------------------------------
+template <class P>
+class Transform_ {
+public:
+ /// Default constructor gives an identity transform.
+ Transform_() : R_BF(), p_BF(0) { }
+
+ /// Combine a rotation and a translation into a transform.
+ Transform_( const Rotation_<P>& R, const Vec<3,P>& p ) : R_BF(R), p_BF(p) { }
+
+ /// Construct or default-convert a rotation into a transform
+ /// containing that rotation and zero translation.
+ Transform_( const Rotation_<P>& R ) : R_BF(R), p_BF(0) { }
+
+ /// Construct or default-convert a translation (expressed as a Vec3)
+ /// into a transform with that translation and a zero rotation.
+ Transform_( const Vec<3,P>& p ) : R_BF(), p_BF(p) { }
+
+ // default copy, assignment, destructor
+
+ /// Assignment from InverseTransform. This means that the
+ /// transform we're assigning to must end up with the same @em meaning
+ /// as the inverse transform X has, so we'll need to end up with:
+ /// @li p == X.p()
+ /// @li R == X.R()
+ ///
+ /// Cost: one frame conversion and a negation, 18 flops.
+ // (Definition is below after InverseTransform is declared.)
+ inline Transform_& operator=( const InverseTransform_<P>& X );
+
+ /// Add an offset to the position vector in this %Transform. Cost
+ /// is 3 flops.
+ template <int S>
+ Transform_& operator+=(const Vec<3,P,S>& offset_B)
+ { p_BF += offset_B; return *this; }
+
+ /// Subtract an offset from the position vector in this %Transform. Cost
+ /// is 3 flops.
+ template <int S>
+ Transform_& operator-=(const Vec<3,P,S>& offset_B)
+ { p_BF -= offset_B; return *this; }
+
+ /// Assign a new value to this transform, explicitly providing
+ /// the rotation and translation separately. We return a reference to
+ /// the now-modified transform as though this were an assignment operator.
+ Transform_& set( const Rotation_<P>& R, const Vec<3,P>& p ) { p_BF=p; R_BF=R; return *this; }
+
+ /// By zero we mean "zero transform", i.e., an identity rotation
+ /// and zero translation. We return a reference to the now-modified
+ /// transform as though this were an assignment operator.
+ Transform_& setToZero() { R_BF.setRotationToIdentityMatrix(); p_BF = P(0); return *this; }
+
+ /// This fills both the rotation and translation with NaNs. Note: this is
+ /// @em not the same as a default-constructed transform, which is a
+ /// legitimate identity transform instead. We return a reference to the now-modified
+ /// transform as though this were an assignment operator.
+ Transform_& setToNaN() { R_BF.setRotationToNaN(); p_BF.setToNaN(); return *this; }
+
+ /// Return a read-only inverse of the current Transform_<P>, simply by casting it to
+ /// the InverseTransform_<P> type. Zero cost.
+ const InverseTransform_<P>& invert() const { return *reinterpret_cast<const InverseTransform_<P>*>(this); }
+
+ /// Return a writable (lvalue) inverse of the current transform, simply by casting it to
+ /// the InverseTransform_<P> type. That is, this is an lvalue. Zero cost.
+ InverseTransform_<P>& updInvert() { return *reinterpret_cast<InverseTransform_<P>*>(this); }
+
+ /// Overload transpose operator to mean inversion. @see invert
+ const InverseTransform_<P>& operator~() const {return invert();}
+
+ /// Overload transpose operator to mean inversion. @see updInvert
+ InverseTransform_<P>& operator~() {return updInvert();}
+
+ /// Compose the current transform (X_BF) with the given one. That is,
+ /// return X_BY=X_BF*X_FY. Cost is 63 flops.
+ Transform_ compose(const Transform_& X_FY) const {
+ return Transform_( R_BF * X_FY.R(), p_BF + R_BF * X_FY.p() );
+ }
+
+ /// Compose the current transform (X_BF) with one that is supplied
+ /// as an InverseTransform_ (typically as a result of applying
+ /// the "~" operator to a transform). That is, return
+ /// X_BY=X_BF*X_FY, but now X_FY is represented as ~X_YF. Cost
+ /// is an extra 18 flops to calculate X_FY.p(), total 81 flops.
+ // (Definition is below after InverseTransform_ is declared.)
+ inline Transform_ compose( const InverseTransform_<P>& X_FY ) const;
+
+ /// %Transform a vector expressed in our "F" frame to our "B" frame.
+ /// Note that this involves rotation only; it is independent of
+ /// the translation stored in this transform. Cost is 15 flops.
+ Vec<3,P> xformFrameVecToBase( const Vec<3,P>& vF ) const {return R_BF*vF;}
+
+ /// %Transform a vector expressed in our "B" frame to our "F" frame.
+ /// Note that this involves rotation only; it is independent of
+ /// the translation stored in this transform. Cost is 15 flops.
+ Vec<3,P> xformBaseVecToFrame( const Vec<3,P>& vB ) const { return ~R_BF*vB; }
+
+ /// %Transform a point (station) measured from and expressed in
+ /// our "F" frame to that same point but measured from and
+ /// expressed in our "B" frame. Cost is 18 flops.
+ Vec<3,P> shiftFrameStationToBase( const Vec<3,P>& sF ) const
+ { return p_BF + xformFrameVecToBase(sF); }
+
+ /// %Transform a point (station) measured from and expressed in
+ /// our "B" frame to that same point but measured from and
+ /// expressed in our "F" frame. Cost is 18 flops.
+ Vec<3,P> shiftBaseStationToFrame( const Vec<3,P>& sB ) const
+ { return xformBaseVecToFrame(sB - p_BF); }
+
+ /// Return a read-only reference to the contained rotation R_BF.
+ const Rotation_<P>& R() const { return R_BF; }
+
+ /// Return a writable (lvalue) reference to the contained rotation R_BF.
+ Rotation_<P>& updR() { return R_BF; }
+
+ /// Return a read-only reference to the x direction (unit vector)
+ /// of the F frame, expressed in the B frame.
+ const typename Rotation_<P>::ColType& x() const { return R().x(); }
+ /// Return a read-only reference to the y direction (unit vector)
+ /// of the F frame, expressed in the B frame.
+ const typename Rotation_<P>::ColType& y() const { return R().y(); }
+ /// Return a read-only reference to the z direction (unit vector)
+ /// of the F frame, expressed in the B frame.
+ const typename Rotation_<P>::ColType& z() const { return R().z(); }
+
+ /// Return a read-only reference to the inverse (transpose) of
+ /// our contained rotation, that is R_FB.
+ const InverseRotation_<P>& RInv() const { return ~R_BF; }
+
+ /// Return a writable (lvalue) reference to the inverse (transpose) of
+ /// our contained rotation, that is R_FB.
+ InverseRotation_<P>& updRInv() { return ~R_BF; }
+
+ /// Return a read-only reference to our translation vector p_BF.
+ const Vec<3,P>& p() const { return p_BF; }
+
+ /// Return a writable (lvalue) reference to our translation vector p_BF.
+ /// Caution: if you write through this reference you update the transform.
+ Vec<3,P>& updP() { return p_BF; }
+
+ /// Assign a new value to our translation vector. We expect the
+ /// supplied vector @p p to be expressed in our B frame. A reference
+ /// to the now-modified transform is returned as though this were
+ /// an assignment operator.
+ Transform_<P>& setP( const Vec<3,P>& p ) { p_BF=p; return *this; }
+
+ /// Calculate the inverse of the translation vector in this transform.
+ /// The returned vector will be the negative of the original and will
+ /// be expressed in the F frame rather than our B frame. Cost is 18 flops.
+ Vec<3,P> pInv() const { return -(~R_BF*p_BF); }
+
+ /// Assign a value to the @em inverse of our translation vector.
+ /// That is, we're given a vector in F which we invert and reexpress
+ /// in B to store it in p, so that we get the original argument back if
+ /// we ask for the inverse of p. Sorry, can't update pInv as an lvalue, but here we
+ /// want -(~R_BF*p_BF)=p_FB => p_BF=-(R_BF*p_FB) so we can calculate
+ /// it in 18 flops. A reference to the now-modified transform is returned
+ /// as though this were an assignment operator.
+ Transform_<P>& setPInv( const Vec<3,P>& p_FB ) { p_BF = -(R_BF*p_FB); return *this; }
+
+ /// Recast this transform as a read-only 3x4 matrix. This is a zero-cost
+ /// reinterpretation of the data; the first three columns are the
+ /// columns of the rotation and the last column is the translation.
+ const Mat<3,4,P>& asMat34() const { return Mat<3,4,P>::getAs(reinterpret_cast<const P*>(this)); }
+
+ /// Less efficient version of asMat34(); copies into return variable.
+ Mat<3,4,P> toMat34() const { return asMat34(); }
+
+ /// Return the equivalent 4x4 transformation matrix.
+ Mat<4,4,P> toMat44() const {
+ Mat<4,4,P> tmp;
+ tmp.template updSubMat<3,4>(0,0) = asMat34();
+ tmp[3] = Row<4,P>(0,0,0,1);
+ return tmp;
+ }
+
+ // OBSOLETE -- alternate name for p
+ const Vec<3,P>& T() const {return p();}
+ Vec<3,P>& updT() {return updP();}
+
+private:
+ //TODO: these might not pack correctly; should use an array of 12 Reals.
+ Rotation_<P> R_BF; // rotation matrix that expresses F's axes in R
+ Vec<3,P> p_BF; // location of F's origin measured from B's origin, expressed in B
+};
+
+
+//-----------------------------------------------------------------------------
+/**
+ * %Transform from frame B to frame F, but with the internal representation
+ * inverted. That is, we store R*,p* here but the transform this represents is
+ * <pre>
+ *
+ * B F [ | ]
+ * X_BF = X = [ R | p ] where R=~(R*), p = - ~(R*)(p*).
+ * [.......|...]
+ * [ 0 0 0 1 ]
+ * </pre>
+ */
+//-----------------------------------------------------------------------------
+template <class P>
+class InverseTransform_ {
+public:
+ /// Default constructor produces an identity transform.
+ InverseTransform_() : R_FB(), p_FB(0) { }
+
+ // default copy, assignment, destructor
+
+ /// Implicit conversion from InverseTransform to Transform.
+ operator Transform_<P>() const { return Transform_<P>( R(), p() ); }
+
+ // Assignment from Transform. This means that the inverse
+ // transform we're assigning to must end up with the same meaning
+ // as the inverse transform X has, so we'll need:
+ // p* == X.pInv()
+ // R* == X.RInv()
+ // Cost: one frame conversion and a negation for pInv, 18 flops.
+ InverseTransform_& operator=( const Transform_<P>& X ) {
+ // Be careful to do this in the right order in case X and this
+ // are the same object, i.e. ~X = X which is weird but has
+ // the same meaning as X = ~X, i.e. invert X in place.
+ p_FB = X.pInv(); // This might change X.p ...
+ R_FB = X.RInv(); // ... but this doesn't depend on X.p.
+ return *this;
+ }
+
+ // Inverting one of these just recasts it back to a Transform_<P>.
+ const Transform_<P>& invert() const { return *reinterpret_cast<const Transform_<P>*>(this); }
+ Transform_<P>& updInvert() { return *reinterpret_cast<Transform_<P>*>(this); }
+
+ // Overload transpose to mean inversion.
+ const Transform_<P>& operator~() const { return invert(); }
+ Transform_<P>& operator~() { return updInvert(); }
+
+ // Return X_BY=X_BF*X_FY, where X_BF (this) is represented here as ~X_FB. This
+ // costs exactly the same as a composition of two Transforms (63 flops).
+ Transform_<P> compose(const Transform_<P>& X_FY) const {
+ return Transform_<P>( ~R_FB * X_FY.R(), ~R_FB *(X_FY.p() - p_FB) );
+ }
+ // Return X_BY=X_BF*X_FY, but now both xforms are represented by their inverses.
+ // This costs one extra vector transformation and a negation (18 flops) more
+ // than a composition of two Transforms, for a total of 81 flops.
+ Transform_<P> compose(const InverseTransform_<P>& X_FY) const {
+ return Transform_<P>( ~R_FB * X_FY.R(), ~R_FB *(X_FY.p() - p_FB) );
+ }
+
+ // Forward and inverse vector transformations cost the same here as
+ // for a Transform_<P> (or for that matter, a Rotation_<P>): 15 flops.
+ Vec<3,P> xformFrameVecToBase(const Vec<3,P>& vF) const {return ~R_FB*vF;}
+ Vec<3,P> xformBaseVecToFrame(const Vec<3,P>& vB) const {return R_FB*vB;}
+
+ // Forward and inverse station shift & transform cost the same here as for a Transform_<P>: 18 flops.
+ Vec<3,P> shiftFrameStationToBase(const Vec<3,P>& sF) const { return ~R_FB*(sF-p_FB); }
+ Vec<3,P> shiftBaseStationToFrame(const Vec<3,P>& sB) const { return R_FB*sB + p_FB; }
+
+ const InverseRotation_<P>& R() const {return ~R_FB;}
+ InverseRotation_<P>& updR() {return ~R_FB;}
+
+ const typename InverseRotation_<P>::ColType& x() const {return R().x();}
+ const typename InverseRotation_<P>::ColType& y() const {return R().y();}
+ const typename InverseRotation_<P>::ColType& z() const {return R().z();}
+
+ const Rotation_<P>& RInv() const {return R_FB;}
+ Rotation_<P>& updRInv() {return R_FB;}
+
+ /// Calculate the actual translation vector at a cost of 18 flops.
+ /// It is better if you can just work with the InverseTransform
+ /// directly since then you'll never have to pay this cost.
+ Vec<3,P> p() const { return -(~R_FB*p_FB); }
+
+
+ // no updP lvalue
+
+ // Sorry, can't update translation as an lvalue, but here we
+ // want -(R_BF*p_FB)=p_BF => p_FB=-(R_FB*p_BF). Cost: 18 flops.
+ void setP( const Vec<3,P>& p_BF ) { p_FB = -(R_FB*p_BF); }
+
+ // Inverse translation is free.
+ const Vec<3,P>& pInv() const { return p_FB; }
+ void setPInv( const Vec<3,P>& p ) { p_FB = p; }
+
+ /// For compatibility with Transform_<P>, but we don't provide an "as"
+ /// method here since the internal storage layout is somewhat odd.
+ Mat<3,4,P> toMat34() const { return Transform_<P>(*this).asMat34(); }
+
+ /// Return the equivalent 4x4 transformation matrix.
+ Mat<4,4,P> toMat44() const { return Transform_<P>(*this).toMat44(); }
+
+ // OBSOLETE -- alternate name for p.
+ Vec<3,P> T() const {return p();}
+
+private:
+ // DATA LAYOUT MUST BE IDENTICAL TO Transform_<P> !!
+ // TODO: redo packing here when it is done for Transform_<P>.
+ Rotation_<P> R_FB; // transpose of our rotation matrix, R_BF
+ Vec<3,P> p_FB; // our translation is -(R_BF*p_FB)=-(~R_FB*p_FB)
+};
+
+/// If we multiply a transform or inverse transform by a 3-vector, we treat
+/// the vector as though it had a 4th element "1" appended, that is, it is
+/// treated as a \e station rather than a \e vector. This way we use both
+/// the rotational and translational components of the transform.
+/// @relates Transform_
+template <class P, int S> inline Vec<3,P>
+operator*(const Transform_<P>& X_BF, const Vec<3,P,S>& s_F)
+{ return X_BF.shiftFrameStationToBase(s_F); }
+template <class P, int S> inline Vec<3,P>
+operator*(const InverseTransform_<P>& X_BF, const Vec<3,P,S>& s_F)
+{ return X_BF.shiftFrameStationToBase(s_F); }
+template <class P, int S> inline Vec<3,P>
+operator*(const Transform_<P>& X_BF, const Vec<3,negator<P>,S>& s_F)
+{ return X_BF*Vec<3,P>(s_F); }
+template <class P, int S> inline Vec<3,P>
+operator*(const InverseTransform_<P>& X_BF, const Vec<3,negator<P>,S>& s_F)
+{ return X_BF*Vec<3,P>(s_F); }
+
+/// Adding a 3-vector to a Transform produces a new shifted transform.
+/// @relates Transform_
+template <class P, int S> inline Transform_<P>
+operator+(const Transform_<P>& X_BF, const Vec<3,P,S>& offset_B)
+{ return Transform_<P>(X_BF) += offset_B; }
+/// Adding a 3-vector to a Transform produces a new shifted transform.
+/// @relates Transform_
+template <class P, int S> inline Transform_<P>
+operator+(const Vec<3,P,S>& offset_B, const Transform_<P>& X_BF)
+{ return Transform_<P>(X_BF) += offset_B; }
+
+/// Subtracting a 3-vector from a Transform produces a new shifted transform.
+/// @relates Transform_
+template <class P, int S> inline Transform_<P>
+operator-(const Transform_<P>& X_BF, const Vec<3,P,S>& offset_B)
+{ return Transform_<P>(X_BF) -= offset_B; }
+
+//-----------------------------------------------------------------------------
+/// If we multiply a transform or inverse transform by an augmented 4-vector,
+/// we use the 4th element to decide how to treat it. The 4th element must be
+/// 0 or 1. If 0 it is treated as a vector only and the translation is ignored.
+/// If 1 it is treated as a station and rotated & shifted.
+/// @relates Transform_
+template <class P, int S> inline Vec<4,P>
+operator*(const Transform_<P>& X_BF, const Vec<4,P,S>& a_F) {
+ assert(a_F[3]==0 || a_F[3]==1);
+ const Vec<3,P,S>& v_F = Vec<3,P,S>::getAs(&a_F[0]); // recast the 1st 3 elements as Vec3
+
+ Vec<4,P> out;
+ if( a_F[3] == 0 ) { Vec<3,P>::updAs(&out[0]) = X_BF.xformFrameVecToBase(v_F); out[3] = 0; }
+ else { Vec<3,P>::updAs(&out[0]) = X_BF.shiftFrameStationToBase(v_F); out[3] = 1; }
+ return out;
+}
+
+template <class P, int S> inline Vec<4,P>
+operator*(const InverseTransform_<P>& X_BF, const Vec<4,P,S>& a_F ) {
+ assert(a_F[3]==0 || a_F[3]==1);
+ const Vec<3,P,S>& v_F = Vec<3,P,S>::getAs(&a_F[0]); // recast the 1st 3 elements as Vec3
+
+ Vec<4,P> out;
+ if( a_F[3] == 0 ) { Vec<3,P>::updAs(&out[0]) = X_BF.xformFrameVecToBase(v_F); out[3] = 0; }
+ else { Vec<3,P>::updAs(&out[0]) = X_BF.shiftFrameStationToBase(v_F); out[3] = 1; }
+ return out;
+}
+template <class P, int S> inline Vec<4,P>
+operator*(const Transform_<P>& X_BF, const Vec<4,negator<P>,S>& s_F) {return X_BF*Vec<4,P>(s_F);}
+template <class P, int S> inline Vec<4,P>
+operator*(const InverseTransform_<P>& X_BF, const Vec<4,negator<P>,S>& s_F) {return X_BF*Vec<4,P>(s_F);}
+//-----------------------------------------------------------------------------
+
+/// Multiplying a matrix or vector by a Transform_<P> applies it to each element
+/// individually.
+/// @relates Transform_
+template <class P, class E> inline Vector_<E>
+operator*(const Transform_<P>& X, const VectorBase<E>& v) {
+ Vector_<E> result(v.size());
+ for (int i = 0; i < v.size(); ++i)
+ result[i] = X*v[i];
+ return result;
+}
+template <class P, class E> inline Vector_<E>
+operator*(const VectorBase<E>& v, const Transform_<P>& X) {
+ Vector_<E> result(v.size());
+ for (int i = 0; i < v.size(); ++i)
+ result[i] = X*v[i];
+ return result;
+}
+template <class P, class E> inline RowVector_<E>
+operator*(const Transform_<P>& X, const RowVectorBase<E>& v) {
+ RowVector_<E> result(v.size());
+ for (int i = 0; i < v.size(); ++i)
+ result[i] = X*v[i];
+ return result;
+}
+template <class P, class E> inline RowVector_<E>
+operator*(const RowVectorBase<E>& v, const Transform_<P>& X) {
+ RowVector_<E> result(v.size());
+ for (int i = 0; i < v.size(); ++i)
+ result[i] = X*v[i];
+ return result;
+}
+template <class P, class E> inline Matrix_<E>
+operator*(const Transform_<P>& X, const MatrixBase<E>& v) {
+ Matrix_<E> result(v.nrow(), v.ncol());
+ for (int i = 0; i < v.nrow(); ++i)
+ for (int j = 0; j < v.ncol(); ++j)
+ result(i, j) = X*v(i, j);
+ return result;
+}
+template <class P, class E> inline Matrix_<E>
+operator*(const MatrixBase<E>& v, const Transform_<P>& X) {
+ Matrix_<E> result(v.nrow(), v.ncol());
+ for (int i = 0; i < v.nrow(); ++i)
+ for (int j = 0; j < v.ncol(); ++j)
+ result(i, j) = X*v(i, j);
+ return result;
+}
+template <class P, int N, class E, int S> inline Vec<N,E>
+operator*(const Transform_<P>& X, const Vec<N,E,S>& v) {
+ Vec<N,E> result;
+ for (int i = 0; i < N; ++i)
+ result[i] = X*v[i];
+ return result;
+}
+template <class P, int N, class E, int S> inline Vec<N,E>
+operator*(const Vec<N,E,S>& v, const Transform_<P>& X) {
+ Vec<N,E> result;
+ for (int i = 0; i < N; ++i)
+ result[i] = X*v[i];
+ return result;
+}
+template <class P, int N, class E, int S> inline Row<N,E>
+operator*(const Transform_<P>& X, const Row<N,E,S>& v) {
+ Row<N,E> result;
+ for (int i = 0; i < N; ++i)
+ result[i] = X*v[i];
+ return result;
+}
+template <class P, int N, class E, int S> inline Row<N,E>
+operator*(const Row<N,E,S>& v, const Transform_<P>& X) {
+ Row<N,E> result;
+ for (int i = 0; i < N; ++i)
+ result[i] = X*v[i];
+ return result;
+}
+template <class P, int M, int N, class E, int CS, int RS> inline Mat<M,N,E>
+operator*(const Transform_<P>& X, const Mat<M,N,E,CS,RS>& v) {
+ Mat<M,N,E> result;
+ for (int i = 0; i < M; ++i)
+ for (int j = 0; j < N; ++j)
+ result(i, j) = X*v(i, j);
+ return result;
+}
+template <class P, int M, int N, class E, int CS, int RS> inline Mat<M,N,E>
+operator*(const Mat<M,N,E,CS,RS>& v, const Transform_<P>& X) {
+ Mat<M,N,E> result;
+ for (int i = 0; i < M; ++i)
+ for (int j = 0; j < N; ++j)
+ result(i, j) = X*v(i, j);
+ return result;
+}
+
+// These Transform definitions had to wait for InverseTransform to be declared.
+
+template <class P> inline Transform_<P>&
+Transform_<P>::operator=( const InverseTransform_<P>& X ) {
+ // Be careful to do this in the right order in case X and this
+ // are the same object, i.e. we're doing X = ~X, inverting X in place.
+ p_BF = X.p(); // This might change X.p ...
+ R_BF = X.R(); // ... but this doesn't depend on X.p.
+ return *this;
+}
+
+template <class P> inline Transform_<P>
+Transform_<P>::compose( const InverseTransform_<P>& X_FY ) const {
+ return Transform_<P>( R_BF * X_FY.R(), p_BF + R_BF * X_FY.p() );
+}
+
+/// Composition of transforms. Operators are provided for all the combinations
+/// of transform and inverse transform.
+/// @relates Transform_
+template <class P> inline Transform_<P>
+operator*(const Transform_<P>& X1, const Transform_<P>& X2) {return X1.compose(X2);}
+template <class P> inline Transform_<P>
+operator*(const Transform_<P>& X1, const InverseTransform_<P>& X2) {return X1.compose(X2);}
+template <class P> inline Transform_<P>
+operator*(const InverseTransform_<P>& X1, const Transform_<P>& X2) {return X1.compose(X2);}
+template <class P> inline Transform_<P>
+operator*(const InverseTransform_<P>& X1, const InverseTransform_<P>& X2) {return X1.compose(X2);}
+
+/// Comparison operators return true only if the two transforms are bit
+/// identical; that's not too useful.
+/// @relates Transform_
+template <class P> inline bool
+operator==(const Transform_<P>& X1, const Transform_<P>& X2) {return X1.R()==X2.R() && X1.p()==X2.p();}
+template <class P> inline bool
+operator==(const InverseTransform_<P>& X1, const InverseTransform_<P>& X2) {return X1.R()==X2.R() && X1.p()==X2.p();}
+template <class P> inline bool
+operator==(const Transform_<P>& X1, const InverseTransform_<P>& X2) {return X1.R()==X2.R() && X1.p()==X2.p();}
+template <class P> inline bool
+operator==(const InverseTransform_<P>& X1, const Transform_<P>& X2) {return X1.R()==X2.R() && X1.p()==X2.p();}
+
+/// Generate formatted output of a Transform to an output stream.
+/// @relates Transform_
+template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream&
+operator<<(std::ostream&, const Transform_<P>&);
+/// Generate formatted output of an InverseTransform to an output stream.
+/// @relates InverseTransform_
+template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream&
+operator<<(std::ostream&, const InverseTransform_<P>&);
+
+
+
+//------------------------------------------------------------------------------
+} // End of namespace SimTK
+
+//--------------------------------------------------------------------------
+#endif // SimTK_SimTKCOMMON_TRANSFORM_H
+//--------------------------------------------------------------------------
+
diff --git a/SimTKcommon/Mechanics/include/SimTKcommon/internal/UnitVec.h b/SimTKcommon/Mechanics/include/SimTKcommon/internal/UnitVec.h
new file mode 100644
index 0000000..19523ba
--- /dev/null
+++ b/SimTKcommon/Mechanics/include/SimTKcommon/internal/UnitVec.h
@@ -0,0 +1,352 @@
+#ifndef SimTK_UNITVEC_H
+#define SimTK_UNITVEC_H
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares and defines the UnitVec and UnitRow classes. **/
+
+#include "SimTKcommon/SmallMatrix.h"
+#include "SimTKcommon/internal/CoordinateAxis.h"
+
+#include <iosfwd> // Forward declaration of iostream
+
+namespace SimTK {
+
+//-----------------------------------------------------------------------------
+// Forward declarations. These are templatized by precision P and and stride S
+// but always have length 3. TODO: this should be generalized to other lengths.
+template <class P, int S> class UnitVec;
+template <class P, int S> class UnitRow;
+
+// UnitVec3 is more intelligible name for UnitVec<Real,1>.
+typedef UnitVec<Real,1> UnitVec3;
+typedef UnitVec<float,1> fUnitVec3;
+typedef UnitVec<double,1> dUnitVec3;
+
+//-----------------------------------------------------------------------------
+/**
+ * This class is a Vec3 plus an ironclad guarantee either that:
+ * - the length is one (to within a very small tolerance), or
+ * - all components are NaN.
+ */
+//-----------------------------------------------------------------------------
+template <class P, int S>
+class UnitVec : public Vec<3,P,S> {
+ typedef P RealP;
+public:
+ typedef Vec<3,P,S> BaseVec;
+ typedef UnitRow<P,S> TransposeType;
+
+ /// Default constructor initializes to all-NaN even in Release mode so that
+ /// we maintain the above-promised contract.
+ UnitVec() : BaseVec(NTraits<P>::getNaN()) {}
+
+ /// Copy constructor does not require normalization since we know the
+ /// source is a unit vector.
+ UnitVec(const UnitVec& u)
+ : BaseVec( static_cast<const BaseVec&>(u) ) {}
+
+ /// Automatic conversion from UnitVec with different stride; no computation
+ /// required.
+ template <int S2> UnitVec(const UnitVec<P,S2>& u)
+ : BaseVec( static_cast<const typename UnitVec<P,S2>::BaseVec&>(u) ) {}
+
+ /// Explicit conversion from Vec to UnitVec, requiring expensive normalization.
+ explicit UnitVec(const BaseVec& v) : BaseVec(v/v.norm()) {}
+ /// Explicit conversion from Vec of any stride to this UnitVec, requiring
+ /// expensive normalization.
+ template <int S2>
+ explicit UnitVec(const Vec<3,P,S2>& v) : BaseVec(v/v.norm()) {}
+
+ /// Create a unit vector in the direction of the vector (x,y,z) whose measure
+ /// numbers are supplied -- this requires an expensive normalization since
+ /// we don't know that the supplied vector is normalized.
+ UnitVec(const RealP& x, const RealP& y, const RealP& z) : BaseVec(x,y,z)
+ { static_cast<BaseVec&>(*this) /= BaseVec::norm(); }
+
+ /// Implicit conversion from a coordinate axis XAxis, YAxis, or ZAxis to
+ /// a UnitVec3.\ Does not require any computation.
+ UnitVec(const CoordinateAxis& axis) : BaseVec(0)
+ { BaseVec::operator[](axis) = 1; }
+
+ /// Implicit conversion from a coordinate axis direction to a
+ /// UnitVec3.\ The axis direction is given by one of XAxis, YAxis, ZAxis
+ /// or NegXAxis, NegYAxis, NegZAxis.\ Does not require any computation.
+ UnitVec(const CoordinateDirection& dir) : BaseVec(0)
+ { BaseVec::operator[](dir.getAxis()) = RealP(dir.getDirection()); }
+
+ /// Construct a unit axis vector 100 010 001 given 0,1, or 2; this is not
+ /// an implicit conversion.
+ explicit UnitVec(int axis) : BaseVec(0)
+ { assert(0 <= axis && axis <= 2);
+ BaseVec::operator[](axis) = 1; }
+
+ /// Copy assignment does not require normalization.
+ UnitVec& operator=(const UnitVec& u)
+ { BaseVec::operator=(static_cast<const BaseVec&>(u));
+ return *this; }
+
+ /// Copy assignment from a UnitVec whose stride differs from this one; no
+ /// normalization required.
+ template <int S2> UnitVec& operator=(const UnitVec<P,S2>& u)
+ { BaseVec::operator=(static_cast<const typename UnitVec<P,S2>::BaseVec&>(u));
+ return *this; }
+
+ /// Return a reference to the underlying Vec3 (no copying here).
+ const BaseVec& asVec3() const {return static_cast<const BaseVec&>(*this);}
+
+ // Override Vec3 methods which preserve length. These return a
+ // packed UnitVec regardless of our stride.
+
+ /// Returns a new unit vector pointing in the opposite direction from this one;
+ /// does \e not modify this UnitVec object. Cost is 3 flops.
+ UnitVec<P,1> negate() const {return UnitVec<P,1>(-asVec3(),true);}
+ /// Returns a new unit vector pointing in the opposite direction from this one.
+ /// Cost is 3 flops.
+ UnitVec<P,1> operator-() const {return negate();}
+
+ /// Return a const reference to this unit vector re-expressed as a unit row; no
+ /// computational cost.
+ const TransposeType& operator~() const {return *reinterpret_cast<const TransposeType*>(this);}
+ /// Return a writable reference to this unit vector re-expressed as a unit row; no
+ /// computational cost.
+ TransposeType& operator~() {return *reinterpret_cast<TransposeType*>(this);}
+
+ // We have to define these here so that the non-const ones won't be
+ // inherited. We don't trust anyone to write on one element of a UnitVec!
+
+ /// Return one element of this unit vector as a const reference; there is no
+ /// corresponding writable index function since changing a single element of
+ /// a unit vector would violate the contract that it has unit length at all times.
+ const RealP& operator[](int i) const { return BaseVec::operator[](i); }
+ /// Return one element of this unit vector as a const reference; there is no
+ /// corresponding writable index function since changing a single element of
+ /// a unit vector would violate the contract that it has unit length at all times.
+ const RealP& operator()(int i) const { return BaseVec::operator()(i); }
+
+ /// Return a new unit vector whose measure numbers are the absolute values
+ /// of the ones here. This will still have unit length but will be
+ /// a reflection of this unit vector into the first octant (+x,+y,+z).
+ /// Note that we are returning the packed form of UnitVec regardless
+ /// of our stride here.
+ UnitVec<P,1> abs() const {return UnitVec<P,1>( asVec3().abs(), true );}
+
+ /// Return a new unit vector perpendicular to this one but otherwise
+ /// arbitrary. Some care is taken to ensure good numerical conditioning
+ /// for the result regardless of what goes in. Cost is about 50 flops.
+ inline UnitVec<P,1> perp() const;
+
+ /// (Advanced) This constructor is only for our friends whom we trust to
+ /// give us an already-normalized vector which we simply accept as
+ /// normalized without checking.
+ UnitVec(const BaseVec& v, bool) : BaseVec(v) {}
+ /// (Advanced) This constructor is only for our friends whom we trust to
+ /// give us an already-normalized vector which we simply accept as
+ /// normalized without checking (this version accepts an input
+ /// vector of any stride).
+ template <int S2> UnitVec(const Vec<3,RealP,S2>& v, bool) : BaseVec(v) { }
+
+ /// (Advanced) Reinterpret a given memory location as a %UnitVec like
+ /// this one, without checking -- don't use this if you aren't absolutely
+ /// certain that the memory location actually \e does contain a unit vector,
+ /// with the correct stride! This overrides the base Vec class method of the
+ /// same name.
+ static const UnitVec& getAs(const RealP* p)
+ { return *reinterpret_cast<const UnitVec*>(p); }
+};
+
+
+template <class P, int S> inline UnitVec<P,1>
+UnitVec<P,S>::perp() const {
+ // Choose the coordinate axis which makes the largest angle
+ // with this vector, that is, has the "least u" along it.
+ const UnitVec<P,1> u(abs()); // reflect to first octant
+ const int minAxis = u[0] <= u[1] ? (u[0] <= u[2] ? 0 : 2)
+ : (u[1] <= u[2] ? 1 : 2);
+ // Cross returns a Vec3 result which is then normalized.
+ return UnitVec<P,1>( *this % UnitVec<P,1>(minAxis) );
+}
+
+/// Compare two UnitVec3 objects for exact, bitwise equality (not very useful).
+/// @relates UnitVec
+template <class P, int S1, int S2> inline bool
+operator==(const UnitVec<P,S1>& u1, const UnitVec<P,S2>& u2)
+{ return u1.asVec3() == u2.asVec3(); }
+
+/// Compare two UnitVec3 objects and return true unless they are exactly
+/// bitwise equal (not very useful).
+/// @relates UnitVec
+template <class P, int S1, int S2> inline bool
+operator!=(const UnitVec<P,S1>& u1, const UnitVec<P,S2>& u2)
+{ return !(u1==u2); }
+
+//-----------------------------------------------------------------------------
+/**
+ * This type is used for the transpose of UnitVec, and as the returned row
+ * type of a Rotation. Don't construct these directly.
+ */
+//-----------------------------------------------------------------------------
+template <class P, int S>
+class UnitRow : public Row<3,P,S> {
+ typedef P RealP;
+public:
+ typedef Row<3,P,S> BaseRow;
+ typedef UnitVec<P,S> TransposeType;
+
+ UnitRow() : BaseRow(NTraits<P>::getNaN()) { }
+
+ /// Copy constructor does not require normalization.
+ UnitRow(const UnitRow& u)
+ : BaseRow(static_cast<const BaseRow&>(u)) {}
+
+ /// Implicit conversion from UnitRow with different stride; no
+ /// normalization required.
+ template <int S2> UnitRow(const UnitRow<P,S2>& u)
+ : BaseRow(static_cast<const typename UnitRow<P,S2>::BaseRow&>(u)) { }
+
+ /// Copy assignment does not require normalization.
+ UnitRow& operator=(const UnitRow& u)
+ { BaseRow::operator=(static_cast<const BaseRow&>(u));
+ return *this; }
+
+ /// Copy assignment from UnitRow with different stride; no computation needed.
+ template <int S2> UnitRow& operator=(const UnitRow<P,S2>& u)
+ { BaseRow::operator=(static_cast<const typename UnitRow<P,S2>::BaseRow&>(u));
+ return *this; }
+
+ /// Explicit conversion from Row to UnitRow, requiring expensive normalization.
+ explicit UnitRow(const BaseRow& v) : BaseRow(v/v.norm()) {}
+ /// Explicit conversion from Row of any stride to UnitRow, requiring expensive
+ /// normalization.
+ template <int S2>
+ explicit UnitRow(const Row<3,P,S2>& v) : BaseRow(v/v.norm()) {}
+
+ /// Create a unit row from explicitly specified measure numbers (x,y,z);
+ /// requires expensive normalization.
+ UnitRow(const RealP& x, const RealP& y, const RealP& z)
+ : BaseRow(x,y,z)
+ { static_cast<BaseRow&>(*this) /= BaseRow::norm(); }
+
+ /// Create a unit axis vector 100 010 001 given 0, 1, or 2.
+ explicit UnitRow(int axis) : BaseRow(0)
+ { assert(0 <= axis && axis <= 2);
+ BaseRow::operator[](axis) = 1; }
+
+ /// Return a const reference to the Row3 underlying this UnitRow.
+ const BaseRow& asRow3() const {return static_cast<const BaseRow&>(*this);}
+
+ // Override Row3 methods which preserve length. These return the
+ // packed UnitRow regardless of our stride.
+
+ /// Returns a new unit vector pointing in the opposite direction from this one;
+ /// does \e not modify this UnitVec object. Cost is 3 flops.
+ UnitRow<P,1> negate() const { return UnitRow<P,1>(-asRow3(),true); }
+ /// Returns a new unit vector pointing in the opposite direction from this one.
+ /// Cost is 3 flops.
+ UnitRow<P,1> operator-() const { return negate();}
+
+ /// Return a const reference to this UnitRow reinterpreted as a UnitVec; no
+ /// computation requires since this is just a type cast.
+ const TransposeType& operator~() const {return *reinterpret_cast<const TransposeType*>(this);}
+ /// Return a writable reference to this UnitRow reinterpreted as a UnitVec; no
+ /// computation requires since this is just a type cast.
+ TransposeType& operator~() {return *reinterpret_cast<TransposeType*>(this);}
+
+ // We have to define these here so that the non-const ones won't be
+ // inherited. We don't trust anyone to write on one element of a UnitRow!
+
+ /// Return one element of this unit row as a const reference; there is no
+ /// corresponding writable index function since changing a single element of
+ /// a unit vector would violate the contract that it has unit length at all times.
+ const RealP& operator[](int i) const { return BaseRow::operator[](i); }
+ /// Return one element of this unit row as a const reference; there is no
+ /// corresponding writable index function since changing a single element of
+ /// a unit vector would violate the contract that it has unit length at all times.
+ const RealP& operator()(int i) const { return BaseRow::operator()(i); }
+
+ /// Return a new UnitRow whose measure numbers are the absolute values
+ /// of the ones here. This will still have unit length but will be
+ /// a reflection of this unit vector into the first octant (+x,+y,+z).
+ /// Note that we are returning the packed form of UnitRow regardless
+ /// of our stride here.
+ UnitRow<P,1> abs() const {return UnitRow<P,1>(asRow3().abs(),true);}
+
+ /// Return a new UnitRow perpendicular to this one but otherwise
+ /// arbitrary. Some care is taken to ensure good numerical conditioning
+ /// for the result regardless of what goes in. Cost is about 50 flops.
+ inline UnitRow<P,1> perp() const;
+
+ /// (Advanced) This constructor is only for our friends whom we trust to
+ /// give us an already-normalized vector which we simply accept as
+ /// normalized without checking.
+ UnitRow( const BaseRow& v, bool ) : BaseRow(v) { }
+ /// (Advanced) This constructor is only for our friends whom we trust to
+ /// give us an already-normalized vector which we simply accept as
+ /// normalized without checking (this version accepts an input
+ /// vector of any stride).
+ template <int S2> UnitRow( const Row<3,P,S2>& v, bool ) : BaseRow(v) { }
+
+ /// (Advanced) Reinterpret a given memory location as a %UnitRow like
+ /// this one, without checking -- don't use this if you aren't absolutely
+ /// certain that the memory location actually \e does contain a unit vector,
+ /// with the correct stride! This overrides the base Row class method of the
+ /// same name.
+ static const UnitRow& getAs(const RealP* p)
+ { return *reinterpret_cast<const UnitRow*>(p); }
+};
+
+template <class P, int S>
+inline UnitRow<P,1> UnitRow<P,S>::perp() const {
+ // Choose the coordinate axis which makes the largest angle
+ // with this vector, that is, has the "least u" along it.
+ const UnitRow<P,1> u(abs()); // reflect to first octant
+ const int minAxis = u[0] <= u[1] ? (u[0] <= u[2] ? 0 : 2)
+ : (u[1] <= u[2] ? 1 : 2);
+ // Cross returns a Row3 result which is then normalized.
+ return UnitRow<P,1>(*this % UnitRow<P,1>(minAxis));
+}
+
+
+/// Compare two UnitRow3 objects for exact, bitwise equality (not very useful).
+/// @relates UnitRow
+template <class P, int S1, int S2> inline bool
+operator==(const UnitRow<P,S1>& u1, const UnitRow<P,S2>& u2)
+{ return u1.asRow3() == u2.asRow3(); }
+
+/// Compare two UnitRow3 objects and return true unless they are exactly
+/// bitwise equal (not very useful).
+/// @relates UnitRow
+template <class P, int S1, int S2> inline bool
+operator!=(const UnitRow<P,S1>& u1, const UnitRow<P,S2>& u2)
+{ return !(u1==u2); }
+
+//------------------------------------------------------------------------------
+} // End of namespace SimTK
+
+//--------------------------------------------------------------------------
+#endif // SimTK_UNITVEC_H_
+//--------------------------------------------------------------------------
+
+
diff --git a/SimTKcommon/Mechanics/src/CoordinateAxis.cpp b/SimTKcommon/Mechanics/src/CoordinateAxis.cpp
new file mode 100644
index 0000000..3e7b0a2
--- /dev/null
+++ b/SimTKcommon/Mechanics/src/CoordinateAxis.cpp
@@ -0,0 +1,47 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Definitions of external global constants of types CoordinateAxis and
+ * CoordinateDirection.
+ */
+
+#include "SimTKcommon/internal/common.h"
+#include "SimTKcommon/internal/CoordinateAxis.h"
+
+namespace SimTK {
+
+// These constants are global external symbols exported by the library. See
+// the CoordinateAxis.h header file for information.
+SimTK_SimTKCOMMON_EXPORT const CoordinateAxis::XCoordinateAxis XAxis;
+SimTK_SimTKCOMMON_EXPORT const CoordinateAxis::YCoordinateAxis YAxis;
+SimTK_SimTKCOMMON_EXPORT const CoordinateAxis::ZCoordinateAxis ZAxis;
+
+SimTK_SimTKCOMMON_EXPORT const CoordinateDirection::NegXDirection NegXAxis;
+SimTK_SimTKCOMMON_EXPORT const CoordinateDirection::NegYDirection NegYAxis;
+SimTK_SimTKCOMMON_EXPORT const CoordinateDirection::NegZDirection NegZAxis;
+
+
+} // End of namespace SimTK
+
+
diff --git a/SimTKcommon/Mechanics/src/MassProperties.cpp b/SimTKcommon/Mechanics/src/MassProperties.cpp
new file mode 100644
index 0000000..8245313
--- /dev/null
+++ b/SimTKcommon/Mechanics/src/MassProperties.cpp
@@ -0,0 +1,145 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Implementations of non-inline methods of MassProperties classes.
+ */
+
+#include "SimTKcommon/internal/common.h"
+#include "SimTKcommon/internal/MassProperties.h"
+
+#include <iostream>
+
+namespace SimTK {
+ /////////////////////////
+ // INERTIA //
+ /////////////////////////
+
+// Instantiate so we catch bugs now.
+template class Inertia_<float>;
+template class Inertia_<double>;
+
+ /////////////////////////
+ // UNIT INERTIA //
+ /////////////////////////
+
+// Instantiate so we catch bugs now.
+template class UnitInertia_<float>;
+template class UnitInertia_<double>;
+
+ /////////////////////////
+ // MASS PROPERTIES //
+ /////////////////////////
+
+// Instantiate so we catch bugs now.
+template class MassProperties_<float>;
+template class MassProperties_<double>;
+
+ /////////////////////////
+ // SPATIAL INERTIA //
+ /////////////////////////
+
+// Instantiate so we catch bugs now.
+template class SpatialInertia_<float>;
+template class SpatialInertia_<double>;
+
+ /////////////////////////
+ // ARTICULATED INERTIA //
+ /////////////////////////
+
+// Calculate the lower half of vx*F where vx is the cross product matrix
+// of v and F is a full 3x3 matrix. This result would normally be a full
+// 3x3 but for the uses below we know we're only going to need the diagonal
+// and lower triangle so we can save some flops by working this out by hand.
+// The method is templatized so that it will work on a transposed matrix
+// as efficiently as an untransposed one. (18 flops)
+template <class P, int CS, int RS>
+static inline SymMat<3,P>
+halfCross(const Vec<3,P>& v, const Mat<3,3,P,CS,RS>& F) {
+ return SymMat<3,P>
+ ( v[1]*F(2,0)-v[2]*F(1,0),
+ v[2]*F(0,0)-v[0]*F(2,0), v[2]*F(0,1)-v[0]*F(2,1),
+ v[0]*F(1,0)-v[1]*F(0,0), v[0]*F(1,1)-v[1]*F(0,1), v[0]*F(1,2)-v[1]*F(0,2) );
+}
+
+// Calculate the lower half of G*vx where G is a full 3x3 matrix and vx
+// is the cross product matrix of v. See comment above for details.
+// (18 flops)
+template <class P, int CS, int RS>
+static inline SymMat<3,P>
+halfCross(const Mat<3,3,P,CS,RS>& G, const Vec<3,P>& v) {
+ return SymMat<3,P>
+ ( v[2]*G(0,1)-v[1]*G(0,2),
+ v[2]*G(1,1)-v[1]*G(1,2), v[0]*G(1,2)-v[2]*G(1,0),
+ v[2]*G(2,1)-v[1]*G(2,2), v[0]*G(2,2)-v[2]*G(2,0), v[1]*G(2,0)-v[0]*G(2,1) );
+}
+
+// This method computes the lower half of the difference vx*F-G*vx using
+// the same methods as above, but done together in order to pull out the
+// common v terms. This is 33 flops, down from 42 if you call the two
+// methods above and add them.
+template <class P, int CS1, int RS1, int CS2, int RS2>
+static inline SymMat<3,P>
+halfCrossDiff(const Vec<3,P>& v, const Mat<3,3,P,CS1,RS1>& F, const Mat<3,3,P,CS2,RS2>& G) {
+ return SymMat<3,P>
+ ( v[1]*(F(2,0)+G(0,2)) - v[2]*(F(1,0)+G(0,1)),
+ v[2]*(F(0,0)-G(1,1)) - v[0]*F(2,0) + v[1]*G(1,2),
+ v[2]*(F(0,1)+G(1,0)) - v[0]*(F(2,1)+G(1,2)),
+ v[0]*F(1,0) - v[2]*G(2,1) - v[1]*(F(0,0)-G(2,2)),
+ v[0]*(F(1,1)-G(2,2)) - v[1]*F(0,1) + v[2]*G(2,0),
+ v[0]*(F(1,2)+G(2,1)) - v[1]*(F(0,2)+G(2,0)) );
+}
+
+// We're computing
+// P' = [ J' F' ] = [ 1 sx ] [ J F ] [ 1 0 ]
+// [~F' M ] [ 0 1 ] [~F M ] [-sx 1 ]
+// like this:
+// F' = F + sx*M
+// J' = J + (sx*~F - F'*sx)
+// where the parenthesized quantity is symmetric although its
+// individual terms are not. Cost is 72 flops.
+template <class P> ArticulatedInertia_<P>
+ArticulatedInertia_<P>::shift(const Vec3P& s) const {
+ const Mat33P Fp = F + s % M; // same meaning as sx*M but faster (33 flops)
+ const SymMat33P Jp = J + halfCrossDiff(s, ~F, Fp); // sx*~F - F'*sx (39 flops)
+ return ArticulatedInertia_(M, Fp, Jp);
+}
+
+// Same as above but perform the shift in place. Same flop count but less copying.
+template <class P> ArticulatedInertia_<P>&
+ArticulatedInertia_<P>::shiftInPlace(const Vec3P& s) {
+ const Mat33P Fp = F + s % M; // same meaning as sx*M but faster (33 flops)
+ J += halfCrossDiff(s, ~F, Fp); // J + (sx*~F - F'*sx) (39 flops)
+ F = Fp;
+ // M doesn't change
+ return *this;
+}
+
+// Instantiate so we catch bugs now.
+template class ArticulatedInertia_<float>;
+template class ArticulatedInertia_<double>;
+
+
+
+} // namespace SimTK
+
diff --git a/SimTKcommon/Mechanics/src/Quaternion.cpp b/SimTKcommon/Mechanics/src/Quaternion.cpp
new file mode 100644
index 0000000..770628f
--- /dev/null
+++ b/SimTKcommon/Mechanics/src/Quaternion.cpp
@@ -0,0 +1,121 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Paul Mitiguy *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Implementations of non-inline methods of classes dealing with quaternions.
+ */
+
+#include "SimTKcommon/basics.h"
+#include "SimTKcommon/internal/Quaternion.h"
+#include "SimTKcommon/internal/Rotation.h"
+
+//------------------------------------------------------------------------------
+namespace SimTK {
+
+
+//------------------------------------------------------------------------------
+// Constructs a canonical quaternion from a rotation matrix (cost is ~60 flops).
+//------------------------------------------------------------------------------
+template <class P>
+Quaternion_<P>::Quaternion_(const Rotation_<P>& r)
+: Vec<4,P>(r.convertRotationToQuaternion()) {}
+
+
+//------------------------------------------------------------------------------
+// Returns [ a vx vy vz ] with (a,v) in canonical form, i.e., -180 < a <= 180 and |v|=1.
+// The cost of this operation is roughly one atan2, one sqrt, and one divide (about 100 flops).
+//------------------------------------------------------------------------------
+template <class P> Vec<4,P>
+Quaternion_<P>::convertQuaternionToAngleAxis() const {
+ const RealP& ca2 = (*this)[0]; // cos(a/2)
+ const Vec3P& sa2v = this->template getSubVec<3>(1); // sin(a/2) * v
+ RealP sa2 = sa2v.norm(); // sa2 is always >= 0
+
+ const RealP Eps = NTraits<P>::getEps();
+ const RealP Pi = NTraits<P>::getPi();
+
+ // TODO: what is the right value to use here?? Norms can be
+ // much less than eps and still OK -- this is 1e-32 in double.
+ if( sa2 < square(Eps) ) return Vec4P(0,1,0,0); // no rotation, x axis
+
+ // Use atan2. Do NOT just use acos(q[0]) to calculate the rotation angle!!!
+ // Otherwise results are numerical garbage anywhere near zero (or less near).
+ RealP angle = 2 * std::atan2(sa2,ca2);
+
+ // Since sa2>=0, atan2 returns a value between 0 and pi, which is then
+ // multiplied by 2 which means the angle is between 0 and 2pi.
+ // We want an angle in the range: -pi < angle <= pi range.
+ // E.g., instead of rotating 359 degrees clockwise, rotate -1 degree counterclockwise.
+ if( angle > Pi ) angle -= 2*Pi;
+
+ // Normalize the axis part of the return value
+ const Vec3P axis = sa2v / sa2;
+
+ // Return (angle/axis)
+ return Vec4P( angle, axis[0], axis[1], axis[2] );
+}
+
+
+//-------------------------------------------------------------------
+template <class P> void
+Quaternion_<P>::setQuaternionFromAngleAxis( const Vec4P& av ) {
+ // av = [ a vx vy vz ]
+ // If |a| < machine precision, we treat as a zero rotation which produces quaternion q=[1 0 0 0].
+ const RealP eps = std::numeric_limits<RealP>::epsilon();
+ const RealP& a = av[0]; // the angle
+ if( std::fabs(a) < eps ) { Vec4P::operator=( Vec4P(1,0,0,0) ); return; }
+
+ // The vector v must have length at least machine precision (or return NaN).
+ const Vec3P& vIn = av.template getSubVec<3>(1);
+ const RealP vnorm = vIn.norm();
+ if( vnorm < eps ) setQuaternionToNaN();
+
+ // Otherwise, the vector v is normalized and used as the rotation axis.
+ // Note: The cost of this method is 120 flops, including normalization (about 40 flops)
+ // and the sine and cosine calculations (80 flops) used in the next method.
+ else setToAngleAxis( a, UnitVec<P,1>(vIn/vnorm, true) );
+}
+
+
+//-------------------------------------------------------------------
+template <class P> void
+Quaternion_<P>::setQuaternionFromAngleAxis( const RealP& a, const UnitVec<P,1>& v ) {
+ /// The cost of this method is approximately 80 flops (one sin and one cos).
+ RealP ca2 = std::cos(a/2), sa2 = std::sin(a/2);
+
+ // Multiplying an entire quaternion by -1 produces the same Rotation matrix
+ // (each element of the Rotation element involves the product of two quaternion elements).
+ // The canonical form is to make the first element of the quaternion positive.
+ if( ca2 < 0 ) { ca2 = -ca2; sa2 = -sa2; }
+ (*this)[0] = ca2;
+ (*this).template updSubVec<3>(1) = sa2*v;
+}
+
+// Instantiate now to catch bugs.
+template class Quaternion_<float>;
+template class Quaternion_<double>;
+
+//------------------------------------------------------------------------------
+} // End of namespace SimTK
+
+
diff --git a/SimTKcommon/Mechanics/src/Rotation.cpp b/SimTKcommon/Mechanics/src/Rotation.cpp
new file mode 100644
index 0000000..619b56f
--- /dev/null
+++ b/SimTKcommon/Mechanics/src/Rotation.cpp
@@ -0,0 +1,826 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Paul Mitiguy, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Implementations of non-inline methods associated with Rotation class.
+ */
+
+#include "SimTKcommon/basics.h"
+#include "SimTKcommon/internal/Rotation.h"
+
+#include <cmath>
+#include <utility>
+
+//------------------------------------------------------------------------------
+namespace SimTK {
+
+
+//------------------------------------------------------------------------------
+// Set Rotation for ANY two-angle ij rotation sequence (i,j = X,Y,Z)
+//------------------------------------------------------------------------------
+template <class P> Rotation_<P>&
+Rotation_<P>::setRotationFromTwoAnglesTwoAxes
+ ( BodyOrSpaceType bodyOrSpace,
+ RealP angle1, const CoordinateAxis& axis1In,
+ RealP angle2, const CoordinateAxis& axis2In )
+{
+ // Non-const CoordinateAxes in case we have to switch axes
+ CoordinateAxis axis1 = axis1In;
+ CoordinateAxis axis2 = axis2In;
+
+ // If axis2 is same as axis1, efficiently calculate with a one-angle, one-axis rotation
+ if( axis1.isSameAxis(axis2) ) { return setRotationFromAngleAboutAxis( angle1+angle2, axis1 ); }
+
+ // If using a SpaceRotationSequence, switch the order of the axes and the angles
+ if( bodyOrSpace == SpaceRotationSequence ) { std::swap(angle1,angle2); std::swap(axis1,axis2); }
+
+ // If using a reverse cyclical, negate the signs of the angles
+ if( axis1.isReverseCyclical(axis2) ) { angle1 = -angle1; angle2 = -angle2; }
+
+ // Calculate the sines and cosines (some hardware can do this more efficiently as one Taylor series)
+ const RealP c1 = std::cos( angle1 ), s1 = std::sin( angle1 );
+ const RealP c2 = std::cos( angle2 ), s2 = std::sin( angle2 );
+
+ // All calculations are based on a body-fixed forward-cyclical rotation sequence
+ return setTwoAngleTwoAxesBodyFixedForwardCyclicalRotation( c1,s1,axis1, c2,s2,axis2 );
+}
+
+
+//------------------------------------------------------------------------------
+// Set Rotation for ANY three-angle ijk rotation sequence where (i,j,k = X,Y,Z)
+//------------------------------------------------------------------------------
+template <class P> Rotation_<P>&
+Rotation_<P>::setRotationFromThreeAnglesThreeAxes
+ ( BodyOrSpaceType bodyOrSpace,
+ RealP angle1, const CoordinateAxis& axis1In,
+ RealP angle2, const CoordinateAxis& axis2,
+ RealP angle3, const CoordinateAxis& axis3In ) {
+
+ // Non-const CoordinateAxes in case we have to switch axes.
+ CoordinateAxis axis1 = axis1In;
+ CoordinateAxis axis3 = axis3In;
+
+ // If axis2 is same as axis1 or axis3, efficiently calculate with a two-angle,
+ // two-axis rotation.
+ if( axis2.isSameAxis(axis1) )
+ return setRotationFromTwoAnglesTwoAxes(bodyOrSpace, angle1+angle2, axis1, angle3,axis3);
+ if( axis2.isSameAxis(axis3) )
+ return setRotationFromTwoAnglesTwoAxes(bodyOrSpace, angle1,axis1, angle2+angle3, axis3);
+
+ // If using a SpaceRotationSequence, switch the order of the axes and the angles.
+ if( bodyOrSpace == SpaceRotationSequence ) { std::swap(angle1,angle3); std::swap(axis1,axis3); }
+
+ // If using a reverse cyclical, negate the signs of the angles.
+ if( axis1.isReverseCyclical(axis2) ) { angle1 = -angle1; angle2 = -angle2; angle3 = -angle3; }
+
+ // Calculate the sines and cosines (some hardware can do this more
+ // efficiently as one Taylor series).
+ const RealP c1 = std::cos( angle1 ), s1 = std::sin( angle1 );
+ const RealP c2 = std::cos( angle2 ), s2 = std::sin( angle2 );
+ const RealP c3 = std::cos( angle3 ), s3 = std::sin( angle3 );
+
+ // All calculations are based on a body-fixed rotation sequence.
+ // Determine whether this is a BodyXYX or BodyXYZ type of rotation sequence.
+ if( axis1.isSameAxis(axis3) )
+ setThreeAngleTwoAxesBodyFixedForwardCyclicalRotation( c1,s1,axis1, c2,s2,axis2, c3,s3);
+ else
+ setThreeAngleThreeAxesBodyFixedForwardCyclicalRotation(c1,s1,axis1, c2,s2,axis2, c3,s3,axis3);
+
+ return *this;
+}
+
+
+//------------------------------------------------------------------------------
+// Calculate angle for ANY X or Y or Z rotation sequence
+//------------------------------------------------------------------------------
+template <class P> P
+Rotation_<P>::convertOneAxisRotationToOneAngle( const CoordinateAxis& axis1 ) const
+{
+ // Get proper indices into Rotation matrix
+ const CoordinateAxis axis2 = axis1.getNextAxis();
+ const CoordinateAxis axis3 = axis2.getNextAxis();
+ const int j = int( axis2 );
+ const int k = int( axis3 );
+
+ const Mat33P& R = asMat33();
+ const RealP sinTheta = ( R[k][j] - R[j][k] ) / 2;
+ const RealP cosTheta = ( R[j][j] + R[k][k] ) / 2;
+
+ return std::atan2( sinTheta, cosTheta );
+}
+
+
+//------------------------------------------------------------------------------
+// Calculate angles for ANY two-angle ij rotation sequence (i,j = X,Y,Z)
+//------------------------------------------------------------------------------
+template <class P> Vec<2,P>
+Rotation_<P>::convertTwoAxesRotationToTwoAngles
+ ( BodyOrSpaceType bodyOrSpace,
+ const CoordinateAxis& axis1In,
+ const CoordinateAxis& axis2In ) const
+{
+ // Non-const CoordinateAxes in case we have to switch axes
+ CoordinateAxis axis1 = axis1In;
+ CoordinateAxis axis2 = axis2In;
+
+ // If axis2 is same as axis1, efficiently calculate with a one-axis, one-angle method
+ if( axis1.isSameAxis(axis2) )
+ { const RealP theta = convertOneAxisRotationToOneAngle(axis1) / 2;
+ return Vec<2,P>(theta,theta); }
+
+ // If using a SpaceRotationSequence, switch the order of the axes (later switch the angles)
+ if( bodyOrSpace == SpaceRotationSequence ) std::swap(axis1,axis2);
+
+ // All calculations are based on a body-fixed rotation sequence
+ Vec<2,P> ans = convertTwoAxesBodyFixedRotationToTwoAngles( axis1, axis2 );
+
+ // If using a SpaceRotationSequence, switch the angles now.
+ if( bodyOrSpace == SpaceRotationSequence ) std::swap( ans[0], ans[1] );
+
+ return ans;
+}
+
+
+//------------------------------------------------------------------------------
+// Calculate angles for ANY three-angle ijk rotation sequence where (i,j,k = X,Y,Z)
+//------------------------------------------------------------------------------
+template <class P> Vec<3,P>
+Rotation_<P>::convertThreeAxesRotationToThreeAngles
+ ( BodyOrSpaceType bodyOrSpace,
+ const CoordinateAxis& axis1In,
+ const CoordinateAxis& axis2,
+ const CoordinateAxis& axis3In ) const
+{
+ // Non-const CoordinateAxes in case we have to switch axes
+ CoordinateAxis axis1 = axis1In;
+ CoordinateAxis axis3 = axis3In;
+
+ // If all axes are same, efficiently calculate with a one-axis, one-angle method
+ if( axis1.areAllSameAxes(axis2,axis3) ) {
+ RealP theta = convertOneAxisRotationToOneAngle(axis1) / 3;
+ return Vec3P(theta,theta,theta);
+ }
+
+ // If axis2 is same as axis1, efficiently calculate with a two-angle, two-axis rotation
+ if( axis2.isSameAxis(axis1) ) {
+ const Vec2P xz = convertTwoAxesRotationToTwoAngles(bodyOrSpace,axis1,axis3);
+ const RealP theta = xz[0] / 2;
+ return Vec3P( theta, theta, xz[1] );
+ }
+
+ // If axis2 is same as axis3, efficiently calculate with a two-angle, two-axis rotation
+ if( axis2.isSameAxis(axis3) ) {
+ const Vec2P xz = convertTwoAxesRotationToTwoAngles(bodyOrSpace,axis1,axis3);
+ const RealP theta = xz[1] / 2;
+ return Vec3P( xz[0], theta, theta);
+ }
+
+ // If using a SpaceRotationSequence, switch the order of the axes (later switch the angles)
+ if( bodyOrSpace == SpaceRotationSequence ) std::swap(axis1,axis3);
+
+ // All calculations are based on a body-fixed rotation sequence.
+ // Determine whether this is a BodyXYX or BodyXYZ type of rotation sequence.
+ Vec3P ans = axis1.isSameAxis(axis3)
+ ? convertTwoAxesBodyFixedRotationToThreeAngles(axis1, axis2)
+ : convertThreeAxesBodyFixedRotationToThreeAngles(axis1, axis2, axis3);
+
+ // If using a SpaceRotationSequence, switch the angles now.
+ if( bodyOrSpace == SpaceRotationSequence ) std::swap(ans[0], ans[2]);
+
+ return ans;
+}
+
+
+
+//------------------------------------------------------------------------------
+// Set Rotation ONLY for two-angle, two-axes, body-fixed, ij rotation sequence
+// where i != j.
+//------------------------------------------------------------------------------
+template <class P> Rotation_<P>&
+Rotation_<P>::setTwoAngleTwoAxesBodyFixedForwardCyclicalRotation
+ ( RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1,
+ RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2 )
+{
+ // Ensure this method has proper arguments
+ assert( axis1.isDifferentAxis( axis2 ) );
+
+ CoordinateAxis axis3 = axis1.getThirdAxis( axis2 );
+ const int i = int( axis1 );
+ const int j = int( axis2 );
+ const int k = int( axis3 );
+
+ Mat33P& R = *this;
+ R[i][i] = cosAngle2;
+ R[i][j] = 0;
+ R[i][k] = sinAngle2;
+ R[j][i] = sinAngle2 * sinAngle1;
+ R[j][j] = cosAngle1;
+ R[j][k] = -sinAngle1 * cosAngle2;
+ R[k][i] = -sinAngle2 * cosAngle1;
+ R[k][j] = sinAngle1;
+ R[k][k] = cosAngle1 * cosAngle2;
+ return *this;
+}
+
+
+//------------------------------------------------------------------------------
+// Set Rotation ONLY for three-angle, two-axes, body-fixed, iji rotation
+// sequence where i != j.
+//------------------------------------------------------------------------------
+template <class P> Rotation_<P>&
+Rotation_<P>::setThreeAngleTwoAxesBodyFixedForwardCyclicalRotation
+ ( RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1,
+ RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2,
+ RealP cosAngle3, RealP sinAngle3 )
+{
+ // Ensure this method has proper arguments
+ assert( axis1.isDifferentAxis( axis2 ) );
+
+ // Repeated calculations (for efficiency)
+ RealP s1c3 = sinAngle1 * cosAngle3;
+ RealP s3c1 = sinAngle3 * cosAngle1;
+ RealP s1s3 = sinAngle1 * sinAngle3;
+ RealP c1c3 = cosAngle1 * cosAngle3;
+
+ CoordinateAxis axis3 = axis1.getThirdAxis( axis2 );
+ const int i = int( axis1 );
+ const int j = int( axis2 );
+ const int k = int( axis3 );
+
+ Mat33P& R = *this;
+ R[i][i] = cosAngle2;
+ R[i][j] = sinAngle2 * sinAngle3;
+ R[i][k] = sinAngle2 * cosAngle3;
+ R[j][i] = sinAngle1 * sinAngle2;
+ R[j][j] = c1c3 - cosAngle2 * s1s3;
+ R[j][k] = -s3c1 - cosAngle2 * s1c3;
+ R[k][i] = -sinAngle2 * cosAngle1;
+ R[k][j] = s1c3 + cosAngle2 * s3c1;
+ R[k][k] = -s1s3 + cosAngle2 * c1c3;
+ return *this;
+}
+
+
+//------------------------------------------------------------------------------
+// Set Rotation ONLY for three-angle, three-axes, body-fixed, ijk rotation
+// sequence where i != j != k.
+//------------------------------------------------------------------------------
+template <class P> Rotation_<P>&
+Rotation_<P>::setThreeAngleThreeAxesBodyFixedForwardCyclicalRotation
+ ( RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1,
+ RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2,
+ RealP cosAngle3, RealP sinAngle3, const CoordinateAxis& axis3 )
+{
+ // Ensure this method has proper arguments
+ assert( axis1.areAllDifferentAxes(axis2,axis3) );
+
+ // Repeated calculations (for efficiency)
+ RealP s1c3 = sinAngle1 * cosAngle3;
+ RealP s3c1 = sinAngle3 * cosAngle1;
+ RealP s1s3 = sinAngle1 * sinAngle3;
+ RealP c1c3 = cosAngle1 * cosAngle3;
+
+ const int i = int(axis1);
+ const int j = int(axis2);
+ const int k = int(axis3);
+
+ Mat33P& R = *this;
+ R[i][i] = cosAngle2 * cosAngle3;
+ R[i][j] = -sinAngle3 * cosAngle2;
+ R[i][k] = sinAngle2;
+ R[j][i] = s3c1 + sinAngle2 * s1c3;
+ R[j][j] = c1c3 - sinAngle2 * s1s3;
+ R[j][k] = -sinAngle1 * cosAngle2;
+ R[k][i] = s1s3 - sinAngle2 * c1c3;
+ R[k][j] = s1c3 + sinAngle2 * s3c1;
+ R[k][k] = cosAngle1 * cosAngle2;
+ return *this;
+}
+
+
+//------------------------------------------------------------------------------
+// Calculate angles ONLY for a two-angle, two-axes, body-fixed, ij rotation
+// sequence where i != j.
+//------------------------------------------------------------------------------
+template <class P> Vec<2,P>
+Rotation_<P>::convertTwoAxesBodyFixedRotationToTwoAngles
+ ( const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const
+{
+ // Ensure this method has proper arguments
+ assert( axis1.isDifferentAxis( axis2 ) );
+
+ CoordinateAxis axis3 = axis1.getThirdAxis(axis2);
+ const int i = int(axis1);
+ const int j = int(axis2);
+ const int k = int(axis3);
+ const Mat33P& R = asMat33();
+
+ // Can use either direct method (fast) or all matrix elements with the
+ // overhead of two additional square roots (possibly more accurate).
+ const RealP sinTheta1Direct = R[k][j];
+ const RealP signSinTheta1 = sinTheta1Direct > 0 ? RealP(1) : RealP(-1);
+ const RealP sinTheta1Alternate = signSinTheta1 * std::sqrt( square(R[j][i]) + square(R[j][k]) );
+ const RealP sinTheta1 = ( sinTheta1Direct + sinTheta1Alternate ) / 2;
+
+ const RealP cosTheta1Direct = R[j][j];
+ const RealP signCosTheta1 = cosTheta1Direct > 0 ? RealP(1) : RealP(-1);
+ const RealP cosTheta1Alternate = signCosTheta1 * std::sqrt( square(R[k][i]) + square(R[k][k]) );
+ const RealP cosTheta1 = ( cosTheta1Direct + cosTheta1Alternate ) / 2;
+
+ RealP theta1 = std::atan2( sinTheta1, cosTheta1 );
+
+ // Repeat for theta2
+ const RealP sinTheta2Direct = R[i][k];
+ const RealP signSinTheta2 = sinTheta2Direct > 0 ? RealP(1) : RealP(-1);
+ const RealP sinTheta2Alternate = signSinTheta2 * std::sqrt( square(R[j][i]) + square(R[k][i]) );
+ const RealP sinTheta2 = ( sinTheta2Direct + sinTheta2Alternate ) / 2;
+
+ const RealP cosTheta2Direct = R[i][i];
+ const RealP signCosTheta2 = cosTheta2Direct > 0 ? RealP(1) : RealP(-1);
+ const RealP cosTheta2Alternate = signCosTheta2 * std::sqrt( square(R[j][k]) + square(R[k][k]) );
+ const RealP cosTheta2 = ( cosTheta2Direct + cosTheta2Alternate ) / 2;
+
+ RealP theta2 = std::atan2( sinTheta2, cosTheta2 );
+
+ // If using a reverse cyclical, negate the signs of the angles
+ if( axis1.isReverseCyclical(axis2) ) { theta1 = -theta1; theta2 = -theta2; }
+
+ // Return values have the following ranges:
+ // -pi <= theta1 <= +pi
+ // -pi <= theta2 <= +pi
+ return Vec2P( theta1, theta2 );
+}
+
+
+//------------------------------------------------------------------------------
+// Calculate angles ONLY for a three-angle, two-axes, body-fixed, iji rotation sequence where i != j.
+//------------------------------------------------------------------------------
+template <class P> Vec<3,P>
+Rotation_<P>::convertTwoAxesBodyFixedRotationToThreeAngles
+ ( const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const
+{
+ // Ensure this method has proper arguments
+ assert( axis1.isDifferentAxis( axis2 ) );
+
+ CoordinateAxis axis3 = axis1.getThirdAxis( axis2 );
+ const int i = int( axis1 );
+ const int j = int( axis2 );
+ const int k = int( axis3 );
+
+ // Need to know if using a forward or reverse cyclical
+ RealP plusMinus = 1.0, minusPlus = -1.0;
+ if( axis1.isReverseCyclical(axis2) ) { plusMinus = -1.0, minusPlus = 1.0; }
+
+ // Shortcut to the elements of the rotation matrix
+ const Mat33P& R = asMat33();
+
+ // Calculate theta2 using lots of information in the rotation matrix
+ const RealP Rsum = std::sqrt( (square(R[i][j]) + square(R[i][k]) + square(R[j][i]) + square(R[k][i])) / 2 );
+ const RealP theta2 = std::atan2( Rsum, R[i][i] ); // Rsum = abs(sin(theta2)) is inherently positive
+ RealP theta1, theta3;
+
+ // There is a "singularity" when sin(theta2) == 0
+ if( Rsum > 4*Eps ) {
+ theta1 = std::atan2( R[j][i], minusPlus*R[k][i] );
+ theta3 = std::atan2( R[i][j], plusMinus*R[i][k] );
+ }
+ else if( R[i][i] > 0 ) {
+ const RealP spos = plusMinus*R[k][j] + minusPlus*R[j][k]; // 2*sin(theta1 + theta3)
+ const RealP cpos = R[j][j] + R[k][k]; // 2*cos(theta1 + theta3)
+ const RealP theta1PlusTheta3 = std::atan2( spos, cpos );
+ theta1 = theta1PlusTheta3; // Arbitrary split
+ theta3 = 0; // Arbitrary split
+ }
+ else {
+ const RealP sneg = plusMinus*R[k][j] + plusMinus*R[j][k]; // 2*sin(theta1 - theta3)
+ const RealP cneg = R[j][j] - R[k][k]; // 2*cos(theta1 - theta3)
+ const RealP theta1MinusTheta3 = std::atan2( sneg, cneg );
+ theta1 = theta1MinusTheta3; // Arbitrary split
+ theta3 = 0; // Arbitrary split
+ }
+
+ // Return values have the following ranges:
+ // -pi <= theta1 <= +pi
+ // 0 <= theta2 <= +pi (Rsum is inherently positive)
+ // -pi <= theta3 <= +pi
+ return Vec3P( theta1, theta2, theta3 );
+}
+
+
+//------------------------------------------------------------------------------
+// Calculate angles ONLY for a three-angle, three-axes, body-fixed, ijk rotation sequence where i != j and j != k.
+//------------------------------------------------------------------------------
+template <class P> Vec<3,P>
+Rotation_<P>::convertThreeAxesBodyFixedRotationToThreeAngles
+ ( const CoordinateAxis& axis1, const CoordinateAxis& axis2, const CoordinateAxis& axis3 ) const
+{
+ // Ensure this method has proper arguments
+ assert( axis1.areAllDifferentAxes(axis2,axis3) );
+
+ const int i = int(axis1);
+ const int j = int(axis2);
+ const int k = int(axis3);
+
+ // Need to know if using a forward or reverse cyclical
+ RealP plusMinus = 1.0, minusPlus = -1.0;
+ if( axis1.isReverseCyclical(axis2) ) { plusMinus = -1.0, minusPlus = 1.0; }
+
+ // Shortcut to the elements of the rotation matrix
+ const Mat33P& R = asMat33();
+
+ // Calculate theta2 using lots of information in the rotation matrix
+ RealP Rsum = std::sqrt((square(R[i][i]) + square(R[i][j]) + square(R[j][k]) + square(R[k][k])) / 2);
+ RealP theta2 = std::atan2( plusMinus*R[i][k], Rsum ); // Rsum = abs(cos(theta2)) is inherently positive
+ RealP theta1, theta3;
+
+ // There is a "singularity" when cos(theta2) == 0
+ if( Rsum > 4*Eps ) {
+ theta1 = std::atan2( minusPlus*R[j][k], R[k][k] );
+ theta3 = std::atan2( minusPlus*R[i][j], R[i][i] );
+ }
+ else if( plusMinus*R[i][k] > 0 ) {
+ const RealP spos = R[j][i] + plusMinus*R[k][j]; // 2*sin(theta1 + plusMinus*theta3)
+ const RealP cpos = R[j][j] + minusPlus*R[k][i]; // 2*cos(theta1 + plusMinus*theta3)
+ const RealP theta1PlusMinusTheta3 = std::atan2( spos, cpos );
+ theta1 = theta1PlusMinusTheta3; // Arbitrary split
+ theta3 = 0; // Arbitrary split
+ }
+ else {
+ const RealP sneg = plusMinus*(R[k][j] + minusPlus*R[j][i]); // 2*sin(theta1 + minusPlus*theta3)
+ const RealP cneg = R[j][j] + plusMinus*R[k][i]; // 2*cos(theta1 + minusPlus*theta3)
+ const RealP theta1MinusPlusTheta3 = std::atan2( sneg, cneg );
+ theta1 = theta1MinusPlusTheta3; // Arbitrary split
+ theta3 = 0; // Arbitrary split
+ }
+
+ // Return values have the following ranges:
+ // -pi <= theta1 <= +pi
+ // -pi/2 <= theta2 <= +pi/2 (Rsum is inherently positive)
+ // -pi <= theta3 <= +pi
+ return Vec3P( theta1, theta2, theta3 );
+}
+
+
+//------------------------------------------------------------------------------
+// Calculate A.RotationMatrix.B by knowing one of B's unit vector expressed in A.
+// Note: The other vectors are perpendicular (but somewhat arbitrarily so).
+//------------------------------------------------------------------------------
+template <class P> Rotation_<P>&
+Rotation_<P>::setRotationFromOneAxis(const UnitVec3P& uveci, const CoordinateAxis axisi)
+{
+ // Find a unit vector that is perpendicular to uveci
+ const UnitVec3P uvecj = uveci.perp();
+
+ // Find another unit vector that is perpendicular to both uveci and uvecj
+ const UnitVec3P uveck = UnitVec3P( cross( uveci, uvecj ) );
+
+ // Determine which of the axes this corresponds to
+ CoordinateAxis axisj = axisi.getNextAxis();
+ CoordinateAxis axisk = axisj.getNextAxis();
+
+ // Fill in the correct elements of the Rotation matrix
+ setRotationColFromUnitVecTrustMe( int(axisi), uveci );
+ setRotationColFromUnitVecTrustMe( int(axisj), uvecj );
+ setRotationColFromUnitVecTrustMe( int(axisk), uveck );
+
+ return *this;
+}
+
+
+//------------------------------------------------------------------------------
+// Calculate A.RotationMatrix.B by knowing one of B's unit vectors expressed in
+// A and another vector that may be perpendicular. If the 2nd vector is not
+// perpendicular, no worries - we'll make it so it is perpendicular.
+//------------------------------------------------------------------------------
+template <class P> Rotation_<P>&
+Rotation_<P>::setRotationFromTwoAxes
+ ( const UnitVec3P& uveci, const CoordinateAxis& axisi,
+ const Vec3P& vecjApprox, const CoordinateAxis& axisjApprox )
+{
+ // Ensure vecjApprox is not a zero vector and the axes are not the same.
+ RealP magnitudeOfVecjApprox = vecjApprox.normSqr();
+ if( magnitudeOfVecjApprox==0 || axisi.isSameAxis(axisjApprox) )
+ return setRotationFromOneAxis( uveci, axisi );
+
+ // Create a unit vector that is perpendicular to both uveci and vecjApprox.
+ Vec3P veck = cross( uveci, vecjApprox );
+
+ // Make sure (a x b) is not too close to the zero vector (vectors are nearly parallel)
+ // Since |a x b| = |a|*|b|*sin(theta), it is easy to determine when sin(theta) = |a x b| / (|a| * |b|) is small.
+ // In other words, sin(theta) = |a x b| / (|a| * |b|) where |a| = 1 (since uveci is a unit vector).
+ // I use SqrtEps (which is approx 1.0E-8) which means that the angle between the vectors is less than 1.E-08 rads = 6.0E-7 deg.
+ RealP magnitudeOfVeck = veck.normSqr(); // Magnitude of the cross product
+ if( magnitudeOfVeck < SimTK::SqrtEps * magnitudeOfVecjApprox )
+ return setRotationFromOneAxis( uveci, axisi );
+
+ // Find a unit vector that is perpendicular to both uveci and vecjApprox
+ UnitVec3P uveck = UnitVec3P( veck );
+
+ // Compute the unit vector perpendicular to both uveci and uveck
+ UnitVec3P uvecj = UnitVec3P( cross( uveck, uveci ) );
+
+ // Determine which of the axes this corresponds to
+ CoordinateAxis axisj = axisi.getNextAxis();
+ CoordinateAxis axisk = axisj.getNextAxis();
+
+ // If axisj is axisjApprox, all is good, otherwise switch axisj and axisk and negate the k'th axis.
+ if( axisj.isDifferentAxis(axisjApprox) ) {
+ std::swap( axisj, axisk );
+ uveck = -uveck;
+ }
+
+ // Fill in the correct elements of the Rotation matrix
+ setRotationColFromUnitVecTrustMe( int(axisi), uveci );
+ setRotationColFromUnitVecTrustMe( int(axisj), uvecj );
+ setRotationColFromUnitVecTrustMe( int(axisk), uveck );
+
+ return *this;
+}
+
+
+//------------------------------------------------------------------------------
+// Set this rotation matrix from the associated quaternion. (29 flops)
+//------------------------------------------------------------------------------
+template <class P> Rotation_<P>&
+Rotation_<P>::setRotationFromQuaternion( const Quaternion_<P>& q ) {
+ const RealP q00=q[0]*q[0], q11=q[1]*q[1], q22=q[2]*q[2], q33=q[3]*q[3];
+ const RealP q01=q[0]*q[1], q02=q[0]*q[2], q03=q[0]*q[3];
+ const RealP q12=q[1]*q[2], q13=q[1]*q[3], q23=q[2]*q[3];
+ const RealP q00mq11 = q00-q11, q22mq33 = q22-q33;
+
+ Mat33P::operator=( Mat33P( q00+q11-q22-q33, 2*(q12-q03) , 2*(q13+q02),
+ 2*(q12+q03) , q00mq11+q22mq33, 2*(q23-q01),
+ 2*(q13-q02) , 2*(q23+q01) , q00mq11-q22mq33 ) );
+ return *this;
+}
+
+
+//------------------------------------------------------------------------------
+// Convert this Rotation matrix to the equivalent quaternion.
+//
+// We use a modification of Richard Spurrier's method: Spurrier, R.A., "Comment
+// on 'Singularity-Free Extraction of a Quaternion from a Direction-Cosine
+// Matrix'", J. Spacecraft and Rockets, 15(4):255, 1977. Our modification
+// avoids all but one square root and divide. In each of the four cases we
+// compute 4q[m]*q where m is the "max" element, with
+// m=0 if the trace is larger than any diagonal or
+// m=i if the i,i element is the largest diagonal and larger than the trace.
+// Then when we normalize at the end the scalar 4q[m] evaporates leaving us
+// with a perfectly normalized quaternion.
+//
+// The returned quaternion can be interpreted as a rotation angle a about a unit
+// vector v=[vx vy vz] like this: q = [ cos(a/2) sin(a/2)*v ]
+// We canonicalize the returned quaternion by insisting that
+// cos(a/2) >= 0, meaning that -180 < a <= 180.
+//
+// This takes about 40 flops.
+//------------------------------------------------------------------------------
+template <class P> Quaternion_<P>
+Rotation_<P>::convertRotationToQuaternion() const {
+ const Mat33P& R = asMat33();
+
+ // Stores the return values [cos(theta/2), lambda1*sin(theta/2), lambda2*sin(theta/2), lambda3*sin(theta/2)]
+ Vec4P q;
+
+ // Check if the trace is larger than any diagonal
+ const RealP tr = R.trace();
+ if( tr >= R(0,0) && tr >= R(1,1) && tr >= R(2,2) ) {
+ q[0] = 1 + tr;
+ q[1] = R(2,1) - R(1,2);
+ q[2] = R(0,2) - R(2,0);
+ q[3] = R(1,0) - R(0,1);
+
+ // Check if R(0,0) is largest along the diagonal
+ } else if( R(0,0) >= R(1,1) && R(0,0) >= R(2,2) ) {
+ q[0] = R(2,1) - R(1,2);
+ q[1] = 1 - (tr - 2*R(0,0));
+ q[2] = R(0,1)+R(1,0);
+ q[3] = R(0,2)+R(2,0);
+
+ // Check if R(1,1) is largest along the diagonal
+ } else if( R(1,1) >= R(2,2) ) {
+ q[0] = R(0,2) - R(2,0);
+ q[1] = R(0,1) + R(1,0);
+ q[2] = 1 - (tr - 2*R(1,1));
+ q[3] = R(1,2) + R(2,1);
+
+ // R(2,2) is largest along the diagonal
+ } else {
+ q[0] = R(1,0) - R(0,1);
+ q[1] = R(0,2) + R(2,0);
+ q[2] = R(1,2) + R(2,1);
+ q[3] = 1 - (tr - 2*R(2,2));
+ }
+ RealP scale = q.norm();
+ if( q[0] < 0 ) scale = -scale; // canonicalize
+ return Quaternion_<P>(q/scale, true); // prevent re-normalization
+}
+
+
+
+//------------------------------------------------------------------------------
+// Determine whether "this" Rotation matrix is nearly identical to the one
+// passed in (R) within a specified "pointing angle error". If "this" and "R"
+// are nearly identical, transpose(this) * R = closeToIdentity matrix. After
+// finding the equivalent angle-axis for closeToIdentityMatrix, check the angle
+// to see if it is less than okPointingAngleErrorRads.
+//------------------------------------------------------------------------------
+template <class P> bool
+Rotation_<P>::isSameRotationToWithinAngle
+ (const Rotation_<P>& R, RealP okPointingAngleErrorRads ) const {
+ // The absolute minimum pointing error is 0 if "this" and R are identical.
+ // The absolute maximum pointing error should be Pi (to machine precision).
+ assert( 0 <= okPointingAngleErrorRads && okPointingAngleErrorRads <= Pi);
+ const Rotation_<P> closeToIdentityMatrix = ~(*this) * R;
+ const Vec4P angleAxisEquivalent =
+ closeToIdentityMatrix.convertRotationToAngleAxis();
+ const RealP pointingError = std::abs( angleAxisEquivalent[0] );
+ return pointingError <= okPointingAngleErrorRads;
+}
+
+
+//------------------------------------------------------------------------------
+// This method is a poor man's orthogonalization from the supplied matrix to
+// make a legitimate rotation. This is done by conversion to quaternions and
+// back to Rotation and may not produce the closest rotation.
+//------------------------------------------------------------------------------
+template <class P> Rotation_<P>&
+Rotation_<P>::setRotationFromApproximateMat33( const Mat33P& m ) {
+ // First, create a Rotation from the given Mat33P
+ const Rotation_<P> approximateRotation(m, true);
+ const Quaternion_<P> q = approximateRotation.convertRotationToQuaternion();
+ this->setRotationFromQuaternion( q );
+ return *this;
+}
+
+
+//------------------------------------------------------------------------------
+// Note: It may be slightly more efficient to set this matrix directly rather
+// than via the quaternion.
+//------------------------------------------------------------------------------
+template <class P> Rotation_<P>&
+Rotation_<P>::setRotationFromAngleAboutUnitVector
+ ( RealP angleInRad, const UnitVec3P& unitVector )
+{
+ QuaternionP q;
+ q.setQuaternionFromAngleAxis( angleInRad, unitVector );
+ return setRotationFromQuaternion( q );
+}
+
+//------------------------------------------------------------------------------
+// Use sneaky tricks from Featherstone to rotate a symmetric dyadic
+// matrix. Consider the current Rotation matrix to be R_AB. We want
+// to return S_AA=R_AB*S_BB*R_BA. Would be 90 flops for 3x3s, 75
+// since we only need six elements in final result. Here we'll get
+// it done in 57 flops.
+// Consider S=[ a d e ]
+// [ d b f ]
+// [ e f c ]
+//
+// First, factor S into S=L+D+vx with v=~[-f e 0] (x means cross
+// product matrix):
+// [a-c d 0] [c 0 0] [ 0 0 e]
+// L = [ d b-c 0] D = [0 c 0] vx = [ 0 0 f]
+// [2e 2f 0] [0 0 c] [-e -f 0]
+// (4 flops to calculate L)
+//
+// A cross product matrix identity says R*vx*R'=(R*v)x, so:
+// S'=R*S*~R = R*L*~R + D + (R*v)x.
+// Let Y'=R*L, Z=Y'*~R. We only need the lower triangle of Z and a
+// 2x2 square of Y'.
+//
+// Don't-care's below are marked "-". Reminder: square bracket [i]
+// index of a matrix means "row i", round bracket (j) means "col j".
+//
+// [ - - 0 ]
+// Y' = [ Y00 Y01 0 ] Y = [ R[1]*L(0) R[1]*L(1) ] 20 flops
+// [ Y10 Y11 0 ] [ R[2]*L(0) R[2]*L(1) ]
+//
+// Z = [ Z00 - - ]
+// [ Y[0]*~R[0] Y[0]*~R[1] - ] 15 flops (use only 2
+// [ Y[1]*~R[0] Y[1]*~R[1] Y[1]*~R[2] ] elements of R's rows)
+//
+// Z00 = (L00+L11)-(Z11+Z22) 3 flops ( because rotation preserves trace)
+//
+// [R01*c-R00*e] [ 0 - - ]
+// R*v =[R11*c-R10*e] (R*v)x = [ Rv[2] 0 - ]
+// [R21*c-R20*e] [-Rv[1] Rv[0] 0 ]
+// (R*v is 9 flops)
+//
+// [ Z00 + c - - ]
+// S' = [ Z10 + Rv[2] Z11 + c - ]
+// [ Z20 - Rv[1] Z21 + Rv[0] Z22 + c ]
+//
+// which takes 6 more flops. Total 6+9Rv+18Z+20Y+4L=57.
+//
+// (I actually looked at the generated code in VC++ 2005 and Intel C++
+// version 11.1 and counted exactly 57 inline flops.)
+//
+// NOTE: there are two implementations of this routine that have
+// to be kept in sync -- this one and the identical one for
+// InverseRotation right below.
+//------------------------------------------------------------------------------
+template <class P> SymMat<3,P>
+Rotation_<P>::reexpressSymMat33(const SymMat33P& S_BB) const {
+ const RealP a=S_BB(0,0), b=S_BB(1,1), c=S_BB(2,2);
+ const RealP d=S_BB(1,0), e=S_BB(2,0), f=S_BB(2,1);
+ const Mat33P& R = this->asMat33();
+ const Mat32P& RR = R.template getSubMat<3,2>(0,0); // first two columns of R
+
+ const Mat32P L( a-c , d,
+ d , b-c,
+ 2*e , 2*f );
+
+ const Mat22P Y( R[1]*L(0), R[1]*L(1),
+ R[2]*L(0), R[2]*L(1) );
+
+ const RealP Z10 = Y[0]*~RR[0], Z11 = Y[0]*~RR[1],
+ Z20 = Y[1]*~RR[0], Z21 = Y[1]*~RR[1], Z22= Y[1]*~RR[2];
+ const RealP Z00 = (L(0,0)+L(1,1)) - (Z11+Z22);
+
+ const Vec3P Rv( R(0,1)*e-R(0,0)*f,
+ R(1,1)*e-R(1,0)*f,
+ R(2,1)*e-R(2,0)*f );
+
+ return SymMat33P( Z00 + c,
+ Z10 + Rv[2], Z11 + c,
+ Z20 - Rv[1], Z21 + Rv[0], Z22 + c );
+}
+
+// See above method for details. This method is identical except that
+// the layout of the matrix used to store the rotation matrix has
+// changed. Note that all the indexing here is identical to the normal
+// case above; but the rotation matrix elements are drawn from different
+// memory locations so that the net effect is to use the transpose of
+// the original rotation from which this was created.
+template <class P> SymMat<3,P>
+InverseRotation_<P>::reexpressSymMat33(const SymMat33P& S_BB) const {
+ const RealP a=S_BB(0,0), b=S_BB(1,1), c=S_BB(2,2);
+ const RealP d=S_BB(1,0), e=S_BB(2,0), f=S_BB(2,1);
+ // Note reversal of row and column spacing here (normal is 3,1).
+ const Mat<3,3,RealP,1,3>& R = this->asMat33();
+ const Mat<3,2,RealP,1,3>& RR = R.template getSubMat<3,2>(0,0); // first two columns of R
+
+ const Mat32P L( a-c , d,
+ d , b-c,
+ 2*e , 2*f );
+
+ const Mat22P Y( R[1]*L(0), R[1]*L(1),
+ R[2]*L(0), R[2]*L(1) );
+
+ const RealP Z10 = Y[0]*~RR[0], Z11 = Y[0]*~RR[1],
+ Z20 = Y[1]*~RR[0], Z21 = Y[1]*~RR[1], Z22= Y[1]*~RR[2];
+ const RealP Z00 = (L(0,0)+L(1,1)) - (Z11+Z22);
+
+ const Vec3P Rv( R(0,1)*e-R(0,0)*f,
+ R(1,1)*e-R(1,0)*f,
+ R(2,1)*e-R(2,0)*f );
+
+ return SymMat33P( Z00 + c,
+ Z10 + Rv[2], Z11 + c,
+ Z20 - Rv[1], Z21 + Rv[0], Z22 + c );
+}
+
+// Make sure there are instantiations for all the non-inline methods.
+template class Rotation_<float>;
+template class Rotation_<double>;
+template class InverseRotation_<float>;
+template class InverseRotation_<double>;
+
+//------------------------------------------------------------------------------
+template <class P> std::ostream&
+operator<<( std::ostream& o, const Rotation_<P>& m ) { return o << m.asMat33(); }
+template <class P> std::ostream&
+operator<<( std::ostream& o, const InverseRotation_<P>& m ) { return o << Rotation_<P>(m); }
+
+template SimTK_SimTKCOMMON_EXPORT std::ostream& operator<<(std::ostream&, const Rotation_<float>&);
+template SimTK_SimTKCOMMON_EXPORT std::ostream& operator<<(std::ostream&, const Rotation_<double>&);
+
+template SimTK_SimTKCOMMON_EXPORT std::ostream& operator<<(std::ostream&, const InverseRotation_<float>&);
+template SimTK_SimTKCOMMON_EXPORT std::ostream& operator<<(std::ostream&, const InverseRotation_<double>&);
+
+
+
+//------------------------------------------------------------------------------
+} // End of namespace SimTK
+
diff --git a/SimTKcommon/Mechanics/src/Transform.cpp b/SimTKcommon/Mechanics/src/Transform.cpp
new file mode 100644
index 0000000..a41d4b9
--- /dev/null
+++ b/SimTKcommon/Mechanics/src/Transform.cpp
@@ -0,0 +1,65 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Implementations of non-inline methods of classes dealing with Transforms.
+ */
+
+#include "SimTKcommon/basics.h"
+#include "SimTKcommon/internal/Transform.h"
+
+//-------------------------------------------------------------------
+namespace SimTK {
+
+// Instantiate Transforms and InverseTransforms for float and double. This
+// will catch bugs now rather than when some poor user is the first to
+// instantiate some broken method.
+template class Transform_<float>;
+template class Transform_<double>;
+template class InverseTransform_<float>;
+template class InverseTransform_<double>;
+
+
+// Define the stream output operators and instantiate them for float and
+// double Transforms.
+template <class P> std::ostream&
+operator<<(std::ostream& o, const Transform_<P>& x )
+{ return o << x.asMat34() << Row<4,P>(0,0,0,1) << std::endl;}
+template <class P> std::ostream&
+operator<<(std::ostream& o, const InverseTransform_<P>& x )
+{ return o << Transform_<P>(x);}
+
+template SimTK_SimTKCOMMON_EXPORT std::ostream&
+operator<<(std::ostream& o, const Transform_<float>& x );
+template SimTK_SimTKCOMMON_EXPORT std::ostream&
+operator<<(std::ostream& o, const Transform_<double>& x );
+
+template SimTK_SimTKCOMMON_EXPORT std::ostream&
+operator<<(std::ostream& o, const InverseTransform_<float>& x );
+template SimTK_SimTKCOMMON_EXPORT std::ostream&
+operator<<(std::ostream& o, const InverseTransform_<double>& x );
+
+//------------------------------------------------------------------------------
+} // End of namespace SimTK
+
+
diff --git a/SimTKcommon/Mechanics/src/UnitVec.cpp b/SimTKcommon/Mechanics/src/UnitVec.cpp
new file mode 100644
index 0000000..70603b2
--- /dev/null
+++ b/SimTKcommon/Mechanics/src/UnitVec.cpp
@@ -0,0 +1,44 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Implementations of non-inline methods of classes dealing with UnitVec and
+ * UnitRow.
+ */
+
+#include "SimTKcommon/internal/UnitVec.h"
+
+namespace SimTK {
+
+// Currently this is EMPTY as all methods are inline
+
+// Instantiate here so we catch all compile-time bugs when we build.
+template class UnitVec<float,1>;
+template class UnitVec<double,1>;
+
+template class UnitRow<float,1>;
+template class UnitRow<double,1>;
+
+} // End of namespace SimTK
+
+
diff --git a/SimTKcommon/Polynomial/include/SimTKcommon/internal/PolynomialRootFinder.h b/SimTKcommon/Polynomial/include/SimTKcommon/internal/PolynomialRootFinder.h
new file mode 100644
index 0000000..9fcebfc
--- /dev/null
+++ b/SimTKcommon/Polynomial/include/SimTKcommon/internal/PolynomialRootFinder.h
@@ -0,0 +1,126 @@
+#ifndef SimTK_SimTKCOMMON_POLYNOMIALROOTFINDER_H_
+#define SimTK_SimTKCOMMON_POLYNOMIALROOTFINDER_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+
+namespace SimTK {
+
+/**
+ * This class provides static methods for finding the roots of polynomials. There are
+ * specialized methods for quadratic and cubic polynomials, as well as general methods
+ * for polynomials of arbitrary degree. In each case, there are methods for polynomials
+ * with both real and complex coefficients.
+ *
+ * There are two different algorithms used by this class. The specialized methods for
+ * quadratic polynomials calculate the roots by explicit evaluation of the quadratic
+ * formula. They use the evaluation method described in section 5.6 of "Numerical
+ * Recipes in C++, Second Edition", by Press, Teukolsky, Vetterling, and Flannery.
+ * In addition, the method for quadratic polynomials with real coefficients performs
+ * an extra check to detect when the discriminant is zero to within machine precision.
+ * This helps to prevent round-off error from producing a tiny imaginary part in a
+ * multiple root.
+ *
+ * The methods for cubic and arbitrary degree polynomials use the Jenkins-Traub method,
+ * as implemented in the classic RPOLY and CPOLY functions:
+ *
+ * Jenkins, M. A. and Traub, J. F. (1972), Algorithm 419: Zeros of a Complex Polynomial, Comm. ACM, 15, 97-99.
+ *
+ * Jenkins, M. A. (1975), Algorithm 493: Zeros of a Real Polynomial, ACM TOMS, 1, 178-189.
+ *
+ * This is an iterative method that provides rapid convergence and high accuracy in most cases.
+ */
+
+class SimTK_SimTKCOMMON_EXPORT PolynomialRootFinder {
+public:
+ class ZeroLeadingCoefficient;
+ /**
+ * Find the roots of a quadratic polynomial with real coefficients.
+ *
+ * @param coefficients The polynomial coefficients in order of decreasing powers
+ * @param roots On exit, the roots of the polynomial are stored in this
+ */
+ template <class T>
+ static void findRoots(const Vec<3,T>& coefficients, Vec<2,complex<T> >& roots);
+ /**
+ * Find the roots of a quadratic polynomial with complex coefficients.
+ *
+ * @param coefficients The polynomial coefficients in order of decreasing powers
+ * @param roots On exit, the roots of the polynomial are stored in this
+ */
+ template <class T>
+ static void findRoots(const Vec<3,complex<T> >& coefficients, Vec<2,complex<T> >& roots);
+ /**
+ * Find the roots of a cubic polynomial with real coefficients.
+ *
+ * @param coefficients The polynomial coefficients in order of decreasing powers
+ * @param roots On exit, the roots of the polynomial are stored in this
+ */
+ template <class T>
+ static void findRoots(const Vec<4,T>& coefficients, Vec<3,complex<T> >& roots);
+ /**
+ * Find the roots of a cubic polynomial with complex coefficients.
+ *
+ * @param coefficients The polynomial coefficients in order of decreasing powers
+ * @param roots On exit, the roots of the polynomial are stored in this
+ */
+ template <class T>
+ static void findRoots(const Vec<4,complex<T> >& coefficients, Vec<3,complex<T> >& roots);
+ /**
+ * Find the roots of a polynomial of arbitrary degree with real coefficients.
+ *
+ * @param coefficients The polynomial coefficients in order of decreasing powers
+ * @param roots On exit, the roots of the polynomial are stored in this
+ */
+ template <class T>
+ static void findRoots(const Vector_<T>& coefficients, Vector_<complex<T> >& roots);
+ /**
+ * Find the roots of a polynomial of arbitrary degree with complex coefficients.
+ *
+ * @param coefficients The polynomial coefficients in order of decreasing powers
+ * @param roots On exit, the roots of the polynomial are stored in this
+ */
+ template <class T>
+ static void findRoots(const Vector_<complex<T> >& coefficients, Vector_<complex<T> >& roots);
+};
+
+/**
+ * This is an exception which is thrown by all of the PolynomialRootFinder::findRoots() methods.
+ * It indicates that the leading polynomial coefficient is zero. This means that the polynomial
+ * is not really of the stated degree, and does not have the expected number of roots.
+ */
+
+class PolynomialRootFinder::ZeroLeadingCoefficient : public Exception::Base {
+public:
+ ZeroLeadingCoefficient(const char* fn, int ln) : Base(fn,ln) {
+ setMessage("Attempting to find roots of a polynomial whose leading coefficient is 0.");
+ }
+ virtual ~ZeroLeadingCoefficient() throw() { }
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_POLYNOMIALROOTFINDER_H_
diff --git a/SimTKcommon/Polynomial/src/PolynomialRootFinder.cpp b/SimTKcommon/Polynomial/src/PolynomialRootFinder.cpp
new file mode 100644
index 0000000..07914b3
--- /dev/null
+++ b/SimTKcommon/Polynomial/src/PolynomialRootFinder.cpp
@@ -0,0 +1,225 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/internal/PolynomialRootFinder.h"
+#include "rpoly.h"
+#include "cpoly.h"
+
+namespace SimTK {
+
+template <class T>
+void PolynomialRootFinder::findRoots(const Vec<3,T>& coefficients, Vec<2,complex<T> >& roots) {
+ T a = coefficients[0], b = coefficients[1], c = coefficients[2];
+ if (a == 0.0)
+ SimTK_THROW(ZeroLeadingCoefficient);
+ T b2 = b*b;
+ T discriminant = b2 - (T) 4.0*a*c;
+ T tol = (T) 2.0*NTraits<T>::getEps()*b2;
+ if (discriminant < tol && discriminant > -tol) {
+
+ // b^2 == 4ac to within machine precision, so make the roots identical.
+
+ T root = -b/((T) 2.0*a);
+ roots[0] = complex<T>(root, 0.0);
+ roots[1] = complex<T>(root, 0.0);
+ return;
+ }
+ if (b == 0.0) {
+
+ // The coefficient of the linear term is zero, which makes the formula simpler.
+
+ if (discriminant >= 0.0) {
+ T root = std::sqrt(discriminant)/(T) 2.0*a;
+ roots[0] = root;
+ roots[1] = -root;
+ }
+ else {
+ T root = std::sqrt(-discriminant)/(T) 2.0*a;
+ roots[0] = complex<T>(0, root);
+ roots[1] = complex<T>(0, -root);
+ }
+ return;
+ }
+ complex<T> q = ((T) -0.5)*(b+(b > 0.0 ? std::sqrt(complex<T>(discriminant)) : -std::sqrt(complex<T>(discriminant))));
+ roots[0] = q/a;
+ roots[1] = c/q;
+}
+
+template <class T>
+void PolynomialRootFinder::findRoots(const Vec<3,complex<T> >& coefficients, Vec<2,complex<T> >& roots) {
+ complex<T> a = coefficients[0], b = coefficients[1], c = coefficients[2];
+ if (a == (T) 0.0)
+ SimTK_THROW(ZeroLeadingCoefficient);
+ complex<T> b2 = b*b;
+ complex<T> discriminant = b2 - ((T) 4.0)*a*c;
+ if (b == (T) 0.0) {
+
+ // The coefficient of the linear term is zero, which makes the formula simpler.
+
+ complex<T> root = std::sqrt(discriminant)/((T) 2.0)*a;
+ roots[0] = root;
+ roots[1] = -root;
+ return;
+ }
+ T temp = (conj(b)*sqrt(discriminant)).real();
+ complex<T> q = ((T) -0.5)*(b+(temp > 0.0 ? std::sqrt(discriminant) : -std::sqrt(discriminant)));
+ roots[0] = q/a;
+ roots[1] = c/q;
+}
+
+template <class T>
+void PolynomialRootFinder::findRoots(const Vec<4,T>& coefficients, Vec<3,complex<T> >& roots) {
+ if (coefficients[0] == 0.0)
+ SimTK_THROW(ZeroLeadingCoefficient);
+ T coeff[4] = {coefficients[0], coefficients[1], coefficients[2], coefficients[3]};
+ T rootr[3];
+ T rooti[3];
+ for (int i = 0; i < 3; ++i) // in case these don't get filled in
+ rootr[i] = rooti[i] = NTraits<T>::getNaN();
+ const int nrootsFound = RPoly<T>().findRoots(coeff, 3, rootr, rooti);
+ roots[0] = complex<T>(rootr[0], rooti[0]);
+ roots[1] = complex<T>(rootr[1], rooti[1]);
+ roots[2] = complex<T>(rootr[2], rooti[2]);
+
+ SimTK_ERRCHK_ALWAYS(nrootsFound != -1,
+ "PolynomialRootFinder::findRoots()",
+ "Leading coefficient is zero; can't solve.");
+ SimTK_ERRCHK_ALWAYS(nrootsFound > 0,
+ "PolynomialRootFinder::findRoots()",
+ "Failure to find any roots for polynomial of order 3.");
+}
+
+template <class T>
+void PolynomialRootFinder::findRoots(const Vec<4,complex<T> >& coefficients, Vec<3,complex<T> >& roots) {
+ if (coefficients[0] == (T) 0.0)
+ SimTK_THROW(ZeroLeadingCoefficient);
+ T coeffr[4] = {coefficients[0].real(), coefficients[1].real(), coefficients[2].real(), coefficients[3].real()};
+ T coeffi[4] = {coefficients[0].imag(), coefficients[1].imag(), coefficients[2].imag(), coefficients[3].imag()};
+ T rootr[3];
+ T rooti[3];
+ for (int i = 0; i < 3; ++i) // in case these don't get filled in
+ rootr[i] = rooti[i] = NTraits<T>::getNaN();
+ const int nrootsFound = CPoly<T>().findRoots(coeffr, coeffi, 3, rootr, rooti);
+ roots[0] = complex<T>(rootr[0], rooti[0]);
+ roots[1] = complex<T>(rootr[1], rooti[1]);
+ roots[2] = complex<T>(rootr[2], rooti[2]);
+
+ SimTK_ERRCHK_ALWAYS(nrootsFound != -1,
+ "PolynomialRootFinder::findRoots()",
+ "Leading coefficient is zero; can't solve.");
+ SimTK_ERRCHK_ALWAYS(nrootsFound > 0,
+ "PolynomialRootFinder::findRoots()",
+ "Failure to find any roots for polynomial of order 3.");
+}
+
+template <class T>
+void PolynomialRootFinder::findRoots(const Vector_<T>& coefficients, Vector_<complex<T> >& roots) {
+ if (coefficients[0] == 0.0)
+ SimTK_THROW(ZeroLeadingCoefficient);
+ int n = roots.size();
+ assert(coefficients.size() == n+1);
+ T *coeff = new T[n+1];
+ T *rootr = new T[n];
+ T *rooti = new T[n];
+ try {
+ for (int i = 0; i < n+1; ++i)
+ coeff[i] = coefficients[i];
+ for (int i = 0; i < n; ++i) // in case these don't get filled in
+ rootr[i] = rooti[i] = NTraits<T>::getNaN();
+ const int nrootsFound = RPoly<T>().findRoots(coeff, n, rootr, rooti);
+ for (int i = 0; i < n; ++i)
+ roots[i] = complex<T>(rootr[i], rooti[i]);
+
+ SimTK_ERRCHK_ALWAYS(nrootsFound != -1,
+ "PolynomialRootFinder::findRoots()",
+ "Leading coefficient is zero; can't solve.");
+ SimTK_ERRCHK1_ALWAYS(nrootsFound > 0,
+ "PolynomialRootFinder::findRoots()",
+ "Failure to find any roots for polynomial of order %d.", n);
+ }
+ catch (...) {
+ delete[] coeff;
+ delete[] rootr;
+ delete[] rooti;
+ throw;
+ }
+ delete[] coeff;
+ delete[] rootr;
+ delete[] rooti;
+}
+
+template <class T>
+void PolynomialRootFinder::findRoots(const Vector_<complex<T> >& coefficients, Vector_<complex<T> >& roots) {
+ if (coefficients[0] == (T) 0.0)
+ SimTK_THROW(ZeroLeadingCoefficient);
+ int n = roots.size();
+ assert(coefficients.size() == n+1);
+ T *coeffr = new T[n+1];
+ T *coeffi = new T[n+1];
+ T *rootr = new T[n];
+ T *rooti = new T[n];
+ try {
+ for (int i = 0; i < n+1; ++i) {
+ coeffr[i] = coefficients[i].real();
+ coeffi[i] = coefficients[i].imag();
+ }
+ for (int i = 0; i < n; ++i) // in case these don't get filled in
+ rootr[i] = rooti[i] = NTraits<T>::getNaN();
+ const int nrootsFound = CPoly<T>().findRoots(coeffr, coeffi, n, rootr, rooti);
+ for (int i = 0; i < n; ++i)
+ roots[i] = complex<T>(rootr[i], rooti[i]);
+
+ SimTK_ERRCHK_ALWAYS(nrootsFound != -1,
+ "PolynomialRootFinder::findRoots()",
+ "Leading coefficient is zero; can't solve.");
+ SimTK_ERRCHK1_ALWAYS(nrootsFound > 0,
+ "PolynomialRootFinder::findRoots()",
+ "Failure to find any roots for polynomial of order %d.", n); }
+ catch (...) {
+ delete[] coeffr;
+ delete[] coeffi;
+ delete[] rootr;
+ delete[] rooti;
+ throw;
+ }
+ delete[] coeffr;
+ delete[] coeffi;
+ delete[] rootr;
+ delete[] rooti;
+}
+
+template SimTK_SimTKCOMMON_EXPORT void PolynomialRootFinder::findRoots<float>(const Vec<3,float>& coefficients, Vec<2,complex<float> >& roots);
+template SimTK_SimTKCOMMON_EXPORT void PolynomialRootFinder::findRoots<float>(const Vec<3,complex<float> >& coefficients, Vec<2,complex<float> >& roots);
+template SimTK_SimTKCOMMON_EXPORT void PolynomialRootFinder::findRoots<float>(const Vec<4,float>& coefficients, Vec<3,complex<float> >& roots);
+template SimTK_SimTKCOMMON_EXPORT void PolynomialRootFinder::findRoots<float>(const Vec<4,complex<float> >& coefficients, Vec<3,complex<float> >& roots);
+template SimTK_SimTKCOMMON_EXPORT void PolynomialRootFinder::findRoots<float>(const Vector_<float>& coefficients, Vector_<complex<float> >& roots);
+template SimTK_SimTKCOMMON_EXPORT void PolynomialRootFinder::findRoots<float>(const Vector_<complex<float> >& coefficients, Vector_<complex<float> >& roots);
+
+template SimTK_SimTKCOMMON_EXPORT void PolynomialRootFinder::findRoots<double>(const Vec<3,double>& coefficients, Vec<2,complex<double> >& roots);
+template SimTK_SimTKCOMMON_EXPORT void PolynomialRootFinder::findRoots<double>(const Vec<3,complex<double> >& coefficients, Vec<2,complex<double> >& roots);
+template SimTK_SimTKCOMMON_EXPORT void PolynomialRootFinder::findRoots<double>(const Vec<4,double>& coefficients, Vec<3,complex<double> >& roots);
+template SimTK_SimTKCOMMON_EXPORT void PolynomialRootFinder::findRoots<double>(const Vec<4,complex<double> >& coefficients, Vec<3,complex<double> >& roots);
+template SimTK_SimTKCOMMON_EXPORT void PolynomialRootFinder::findRoots<double>(const Vector_<double>& coefficients, Vector_<complex<double> >& roots);
+template SimTK_SimTKCOMMON_EXPORT void PolynomialRootFinder::findRoots<double>(const Vector_<complex<double> >& coefficients, Vector_<complex<double> >& roots);
+
+} // namespace SimTK
diff --git a/SimTKcommon/Polynomial/src/cpoly.cpp b/SimTKcommon/Polynomial/src/cpoly.cpp
new file mode 100644
index 0000000..bf61a0f
--- /dev/null
+++ b/SimTKcommon/Polynomial/src/cpoly.cpp
@@ -0,0 +1,681 @@
+/*
+ *******************************************************************************
+ *
+ *
+ * Copyright (c) 2002
+ * Henrik Vestermark
+ * Denmark
+ *
+ * All Rights Reserved
+ *
+ * Permission to use, copy, distribute, and sell this software and its
+ * documentation for any purpose is hereby granted without fee, provided:
+ * THE SOFTWARE IS PROVIDED "AS-IS" AND WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS, IMPLIED OR OTHERWISE, INCLUDING WITHOUT LIMITATION, ANY WARRANTY
+ * OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE. IN NO EVENT SHALL
+ * Henrik Vestermark or Future Team Aps, BE LIABLE FOR ANY SPECIAL,
+ * INCIDENTAL, INDIRECT OR CONSEQUENTIAL DAMAGES OF ANY KIND, OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER OR NOT
+ * ADVISED OF THE POSSIBILITY OF DAMAGE, AND ON ANY THEORY OF LIABILITY,
+ * ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS
+ * SOFTWARE.
+ *
+ *******************************************************************************
+ *
+ *
+ * Module name : cpoly.cpp
+ * Module ID Nbr :
+ * Description : cpoly.cpp -- Jenkins-Traub real polynomial root finder.
+ * Translation of TOMS493 from FORTRAN to C. This
+ * implementation of Jenkins-Traub partially adapts
+ * the original code to a C environment by restruction
+ * many of the 'goto' controls to better fit a block
+ * structured form. It also eliminates the global memory
+ * allocation in favor of local, dynamic memory management.
+ *
+ * The calling conventions are slightly modified to return
+ * the number of roots found as the function value.
+ *
+ * INPUT:
+ * opr - vector of real coefficients in order of
+ * decreasing powers.
+ * opi - vector of imaginary coefficients in order of
+ * decreasing powers.
+ * degree - integer degree of polynomial
+ *
+ * OUTPUT:
+ * zeror,zeroi - output vectors of the
+ * real and imaginary parts of the zeros.
+ * to be consistent with rpoly.cpp the zeros is inthe index
+ * [0..max_degree-1]
+ *
+ * RETURN:
+ * returnval: -1 if leading coefficient is zero, otherwise
+ * number of roots found.
+ * --------------------------------------------------------------------------
+ * Change Record :
+ *
+ * Version Author/Date Description of changes
+ * ------- ----------- ----------------------
+ * 01.01 HVE/20021101 Initial release
+ * 01.02 PE/20070808 Converted to a class, templatized
+ * 01.03 MAS/20130410 Minor changes to match RPoly changes
+ *
+ * End of Change Record
+ * --------------------------------------------------------------------------
+*/
+
+#include <cmath>
+#include <limits>
+#include "cpoly.h"
+
+namespace SimTK {
+
+/* define version string */
+template<class T>
+char CPoly<T>::_V_[] = "@(#)cpoly.h 01.03 -- Copyright (C) Henrik Vestermark";
+
+template<class T>
+int CPoly<T>::findRoots( const T *opr, const T *opi, int degree, T *zeror, T *zeroi )
+ {
+ int cnt1, cnt2, idnn2, i, conv;
+ T xx, yy, cosr, sinr, smalno, base, xxx, zr, zi, bnd;
+
+ mcon( &eta, &infin, &smalno, &base );
+ are = eta;
+ mre = (T) (2.0 * sqrt( 2.0 ) * eta);
+ xx = (T) 0.70710678;
+ yy = -xx;
+ cosr = (T) -0.060756474;
+ sinr = (T) -0.99756405;
+ nn = degree;
+
+ // Algorithm fails if the leading coefficient is zero, or degree is zero.
+ if( nn < 1 || (opr[ 0 ] == 0 && opi[ 0 ] == 0) )
+ return -1;
+
+
+ // Remove the zeros at the origin if any
+ while( opr[ nn ] == 0 && opi[ nn ] == 0 )
+ {
+ idnn2 = degree - nn;
+ zeror[ idnn2 ] = 0;
+ zeroi[ idnn2 ] = 0;
+ nn--;
+ }
+
+ // sherm 20130410: If all coefficients but the leading one were zero, then
+ // all solutions are zero; should be a successful (if boring) return.
+ if (nn == 0)
+ return degree;
+
+ // Allocate arrays
+ pr = new T [ degree+1 ];
+ pi = new T [ degree+1 ];
+ hr = new T [ degree+1 ];
+ hi = new T [ degree+1 ];
+ qpr= new T [ degree+1 ];
+ qpi= new T [ degree+1 ];
+ qhr= new T [ degree+1 ];
+ qhi= new T [ degree+1 ];
+ shr= new T [ degree+1 ];
+ shi= new T [ degree+1 ];
+
+ // Make a copy of the coefficients
+ for( i = 0; i <= nn; i++ )
+ {
+ pr[ i ] = opr[ i ];
+ pi[ i ] = opi[ i ];
+ shr[ i ] = cmod( pr[ i ], pi[ i ] );
+ }
+
+ // Scale the polynomial
+ bnd = scale( nn, shr, eta, infin, smalno, base );
+ if( bnd != 1 )
+ for( i = 0; i <= nn; i++ )
+ {
+ pr[ i ] *= bnd;
+ pi[ i ] *= bnd;
+ }
+
+search:
+ if( nn <= 1 )
+ {
+ cdivid( -pr[ 1 ], -pi[ 1 ], pr[ 0 ], pi[ 0 ], &zeror[ degree-1 ], &zeroi[ degree-1 ] );
+ goto finish;
+ }
+
+ // Calculate bnd, alower bound on the modulus of the zeros
+ for( i = 0; i<= nn; i++ )
+ shr[ i ] = cmod( pr[ i ], pi[ i ] );
+
+ cauchy( nn, shr, shi, &bnd );
+
+ // Outer loop to control 2 Major passes with different sequences of shifts
+ for( cnt1 = 1; cnt1 <= 2; cnt1++ )
+ {
+ // First stage calculation , no shift
+ noshft( 5 );
+
+ // Inner loop to select a shift
+ for( cnt2 = 1; cnt2 <= 9; cnt2++ )
+ {
+ // Shift is chosen with modulus bnd and amplitude rotated by 94 degree from the previous shif
+ xxx = cosr * xx - sinr * yy;
+ yy = sinr * xx + cosr * yy;
+ xx = xxx;
+ sr = bnd * xx;
+ si = bnd * yy;
+
+ // Second stage calculation, fixed shift
+ fxshft( 10 * cnt2, &zr, &zi, &conv );
+ if( conv )
+ {
+ // The second stage jumps directly to the third stage ieration
+ // If successful the zero is stored and the polynomial deflated
+ idnn2 = degree - nn;
+ zeror[ idnn2 ] = zr;
+ zeroi[ idnn2 ] = zi;
+ nn--;
+ for( i = 0; i <= nn; i++ )
+ {
+ pr[ i ] = qpr[ i ];
+ pi[ i ] = qpi[ i ];
+ }
+ goto search;
+ }
+ // If the iteration is unsuccessful another shift is chosen
+ }
+ // if 9 shifts fail, the outer loop is repeated with another sequence of shifts
+ }
+
+ // The zerofinder has failed on two major passes
+ // return empty handed with the number of roots found (less than the original degree)
+ degree -= nn;
+
+finish:
+ // Deallocate arrays
+ delete [] pr;
+ delete [] pi;
+ delete [] hr;
+ delete [] hi;
+ delete [] qpr;
+ delete [] qpi;
+ delete [] qhr;
+ delete [] qhi;
+ delete [] shr;
+ delete [] shi;
+
+ return degree;
+ }
+
+
+// COMPUTES THE DERIVATIVE POLYNOMIAL AS THE INITIAL H
+// POLYNOMIAL AND COMPUTES L1 NO-SHIFT H POLYNOMIALS.
+//
+template<class T>
+void CPoly<T>::noshft( const int l1 )
+ {
+ int i, j, jj, n, nm1;
+ T xni, t1, t2;
+
+ n = nn;
+ nm1 = n - 1;
+ for( i = 0; i < n; i++ )
+ {
+ xni = (T) (nn - i);
+ hr[ i ] = xni * pr[ i ] / n;
+ hi[ i ] = xni * pi[ i ] / n;
+ }
+ for( jj = 1; jj <= l1; jj++ )
+ {
+ if( cmod( hr[ n - 1 ], hi[ n - 1 ] ) > eta * 10 * cmod( pr[ n - 1 ], pi[ n - 1 ] ) )
+ {
+ cdivid( -pr[ nn ], -pi[ nn ], hr[ n - 1 ], hi[ n - 1 ], &tr, &ti );
+ for( i = 0; i < nm1; i++ )
+ {
+ j = nn - i - 1;
+ t1 = hr[ j - 1 ];
+ t2 = hi[ j - 1 ];
+ hr[ j ] = tr * t1 - ti * t2 + pr[ j ];
+ hi[ j ] = tr * t2 + ti * t1 + pi[ j ];
+ }
+ hr[ 0 ] = pr[ 0 ];
+ hi[ 0 ] = pi[ 0 ];
+ }
+ else
+ {
+ // If the constant term is essentially zero, shift H coefficients
+ for( i = 0; i < nm1; i++ )
+ {
+ j = nn - i - 1;
+ hr[ j ] = hr[ j - 1 ];
+ hi[ j ] = hi[ j - 1 ];
+ }
+ hr[ 0 ] = 0;
+ hi[ 0 ] = 0;
+ }
+ }
+ }
+
+// COMPUTES L2 FIXED-SHIFT H POLYNOMIALS AND TESTS FOR CONVERGENCE.
+// INITIATES A VARIABLE-SHIFT ITERATION AND RETURNS WITH THE
+// APPROXIMATE ZERO IF SUCCESSFUL.
+// L2 - LIMIT OF FIXED SHIFT STEPS
+// ZR,ZI - APPROXIMATE ZERO IF CONV IS .TRUE.
+// CONV - LOGICAL INDICATING CONVERGENCE OF STAGE 3 ITERATION
+//
+template<class T>
+void CPoly<T>::fxshft( const int l2, T *zr, T *zi, int *conv )
+ {
+ int i, j, n;
+ int test, pasd, bol;
+ T otr, oti, svsr, svsi;
+
+ n = nn;
+ polyev( nn, sr, si, pr, pi, qpr, qpi, &pvr, &pvi );
+ test = 1;
+ pasd = 0;
+
+ // Calculate first T = -P(S)/H(S)
+ calct( &bol );
+
+ // Main loop for second stage
+ for( j = 1; j <= l2; j++ )
+ {
+ otr = tr;
+ oti = ti;
+
+ // Compute the next H Polynomial and new t
+ nexth( bol );
+ calct( &bol );
+ *zr = sr + tr;
+ *zi = si + ti;
+
+ // Test for convergence unless stage 3 has failed once or this
+ // is the last H Polynomial
+ if( !( bol || !test || j == 12 ) )
+ {
+ if( cmod( tr - otr, ti - oti ) < 0.5 * cmod( *zr, *zi ) )
+ {
+ if( pasd )
+ {
+ // The weak convergence test has been passed twice, start the third stage
+ // Iteration, after saving the current H polynomial and shift
+ for( i = 0; i < n; i++ )
+ {
+ shr[ i ] = hr[ i ];
+ shi[ i ] = hi[ i ];
+ }
+ svsr = sr;
+ svsi = si;
+ vrshft( 10, zr, zi, conv );
+ if( *conv ) return;
+
+ //The iteration failed to converge. Turn off testing and restore h,s,pv and T
+ test = 0;
+ for( i = 0; i < n; i++ )
+ {
+ hr[ i ] = shr[ i ];
+ hi[ i ] = shi[ i ];
+ }
+ sr = svsr;
+ si = svsi;
+ polyev( nn, sr, si, pr, pi, qpr, qpi, &pvr, &pvi );
+ calct( &bol );
+ continue;
+ }
+ pasd = 1;
+ }
+ else
+ pasd = 0;
+ }
+ }
+
+ // Attempt an iteration with final H polynomial from second stage
+ vrshft( 10, zr, zi, conv );
+ }
+
+// CARRIES OUT THE THIRD STAGE ITERATION.
+// L3 - LIMIT OF STEPS IN STAGE 3.
+// ZR,ZI - ON ENTRY CONTAINS THE INITIAL ITERATE, IF THE
+// ITERATION CONVERGES IT CONTAINS THE FINAL ITERATE ON EXIT.
+// CONV - .TRUE. IF ITERATION CONVERGES
+//
+template<class T>
+void CPoly<T>::vrshft( const int l3, T *zr, T *zi, int *conv )
+ {
+ int b, bol;
+ int i, j;
+ T mp, ms, omp, relstp, r1, r2, tp;
+
+ *conv = 0;
+ b = 0;
+ sr = *zr;
+ si = *zi;
+
+ // Main loop for stage three
+ for( i = 1; i <= l3; i++ )
+ {
+ // Evaluate P at S and test for convergence
+ polyev( nn, sr, si, pr, pi, qpr, qpi, &pvr, &pvi );
+ mp = cmod( pvr, pvi );
+ ms = cmod( sr, si );
+ if( mp <= 20 * errev( nn, qpr, qpi, ms, mp, are, mre ) )
+ {
+ // Polynomial value is smaller in value than a bound onthe error
+ // in evaluationg P, terminate the ietartion
+ *conv = 1;
+ *zr = sr;
+ *zi = si;
+ return;
+ }
+ if( i != 1 )
+ {
+ if( !( b || mp < omp || relstp >= 0.05 ) )
+ {
+ // Iteration has stalled. Probably a cluster of zeros. Do 5 fixed
+ // shift steps into the cluster to force one zero to dominate
+ tp = relstp;
+ b = 1;
+ if( relstp < eta ) tp = eta;
+ r1 = sqrt( tp );
+ r2 = sr * ( 1 + r1 ) - si * r1;
+ si = sr * r1 + si * ( 1 + r1 );
+ sr = r2;
+ polyev( nn, sr, si, pr, pi, qpr, qpi, &pvr, &pvi );
+ for( j = 1; j <= 5; j++ )
+ {
+ calct( &bol );
+ nexth( bol );
+ }
+ omp = infin;
+ goto _20;
+ }
+
+ // Exit if polynomial value increase significantly
+ if( mp *0.1 > omp ) return;
+ }
+
+ omp = mp;
+
+ // Calculate next iterate
+_20: calct( &bol );
+ nexth( bol );
+ calct( &bol );
+ if( !bol )
+ {
+ relstp = cmod( tr, ti ) / cmod( sr, si );
+ sr += tr;
+ si += ti;
+ }
+ }
+ }
+
+// COMPUTES T = -P(S)/H(S).
+// BOOL - LOGICAL, SET TRUE IF H(S) IS ESSENTIALLY ZERO.
+template<class T>
+void CPoly<T>::calct( int *bol )
+ {
+ int n;
+ T hvr, hvi;
+
+ n = nn;
+
+ // evaluate h(s)
+ polyev( n - 1, sr, si, hr, hi, qhr, qhi, &hvr, &hvi );
+ *bol = cmod( hvr, hvi ) <= are * 10 * cmod( hr[ n - 1 ], hi[ n - 1 ] ) ? 1 : 0;
+ if( !*bol )
+ {
+ cdivid( -pvr, -pvi, hvr, hvi, &tr, &ti );
+ return;
+ }
+
+ tr = 0;
+ ti = 0;
+ }
+
+// CALCULATES THE NEXT SHIFTED H POLYNOMIAL.
+// BOOL - LOGICAL, IF .TRUE. H(S) IS ESSENTIALLY ZERO
+//
+template<class T>
+void CPoly<T>::nexth( const int bol )
+ {
+ int j, n;
+ T t1, t2;
+
+ n = nn;
+ if( !bol )
+ {
+ for( j = 1; j < n; j++ )
+ {
+ t1 = qhr[ j - 1 ];
+ t2 = qhi[ j - 1 ];
+ hr[ j ] = tr * t1 - ti * t2 + qpr[ j ];
+ hi[ j ] = tr * t2 + ti * t1 + qpi[ j ];
+ }
+ hr[ 0 ] = qpr[ 0 ];
+ hi[ 0 ] = qpi[ 0 ];
+ return;
+ }
+
+ // If h[s] is zero replace H with qh
+ for( j = 1; j < n; j++ )
+ {
+ hr[ j ] = qhr[ j - 1 ];
+ hi[ j ] = qhi[ j - 1 ];
+ }
+ hr[ 0 ] = 0;
+ hi[ 0 ] = 0;
+ }
+
+// EVALUATES A POLYNOMIAL P AT S BY THE HORNER RECURRENCE
+// PLACING THE PARTIAL SUMS IN Q AND THE COMPUTED VALUE IN PV.
+//
+template<class T>
+void CPoly<T>::polyev( const int nn, const T sr, const T si, const T pr[], const T pi[], T qr[], T qi[], T *pvr, T *pvi )
+ {
+ int i;
+ T t;
+
+ qr[ 0 ] = pr[ 0 ];
+ qi[ 0 ] = pi[ 0 ];
+ *pvr = qr[ 0 ];
+ *pvi = qi[ 0 ];
+
+ for( i = 1; i <= nn; i++ )
+ {
+ t = ( *pvr ) * sr - ( *pvi ) * si + pr[ i ];
+ *pvi = ( *pvr ) * si + ( *pvi ) * sr + pi[ i ];
+ *pvr = t;
+ qr[ i ] = *pvr;
+ qi[ i ] = *pvi;
+ }
+ }
+
+// BOUNDS THE ERROR IN EVALUATING THE POLYNOMIAL BY THE HORNER RECURRENCE.
+// QR,QI - THE PARTIAL SUMS
+// MS -MODULUS OF THE POINT
+// MP -MODULUS OF POLYNOMIAL VALUE
+// ARE, MRE -ERROR BOUNDS ON COMPLEX ADDITION AND MULTIPLICATION
+//
+template<class T>
+T CPoly<T>::errev( const int nn, const T qr[], const T qi[], const T ms, const T mp, const T are, const T mre )
+ {
+ int i;
+ T e;
+
+ e = cmod( qr[ 0 ], qi[ 0 ] ) * mre / ( are + mre );
+ for( i = 0; i <= nn; i++ )
+ e = e * ms + cmod( qr[ i ], qi[ i ] );
+
+ return e * ( are + mre ) - mp * mre;
+ }
+
+// CAUCHY COMPUTES A LOWER BOUND ON THE MODULI OF THE ZEROS OF A
+// POLYNOMIAL - PT IS THE MODULUS OF THE COEFFICIENTS.
+//
+template<class T>
+void CPoly<T>::cauchy( const int nn, T pt[], T q[], T *fn_val )
+ {
+ int i, n;
+ T x, xm, f, dx, df;
+
+ pt[ nn ] = -pt[ nn ];
+
+ // Compute upper estimate bound
+ n = nn;
+ x = exp( log( -pt[ nn ] ) - log( pt[ 0 ] ) ) / n;
+ if( pt[ n - 1 ] != 0 )
+ {
+ // Newton step at the origin is better, use it
+ xm = -pt[ nn ] / pt[ n - 1 ];
+ if( xm < x ) x = xm;
+ }
+
+ // Chop the interval (0,x) until f < 0
+ while(1)
+ {
+ xm = x * (T) 0.1;
+ f = pt[ 0 ];
+ for( i = 1; i <= nn; i++ )
+ f = f * xm + pt[ i ];
+ if( f <= 0 )
+ break;
+ x = xm;
+ }
+ dx = x;
+
+ // Do Newton iteration until x converges to two decimal places
+ while( fabs( dx / x ) > 0.005 )
+ {
+ q[ 0 ] = pt[ 0 ];
+ for( i = 1; i <= nn; i++ )
+ q[ i ] = q[ i - 1 ] * x + pt[ i ];
+ f = q[ nn ];
+ df = q[ 0 ];
+ for( i = 1; i < n; i++ )
+ df = df * x + q[ i ];
+ dx = f / df;
+ x -= dx;
+ }
+
+ *fn_val = x;
+ }
+
+// RETURNS A SCALE FACTOR TO MULTIPLY THE COEFFICIENTS OF THE POLYNOMIAL.
+// THE SCALING IS DONE TO AVOID OVERFLOW AND TO AVOID UNDETECTED UNDERFLOW
+// INTERFERING WITH THE CONVERGENCE CRITERION. THE FACTOR IS A POWER OF THE
+// BASE.
+// PT - MODULUS OF COEFFICIENTS OF P
+// ETA, INFIN, SMALNO, BASE - CONSTANTS DESCRIBING THE FLOATING POINT ARITHMETIC.
+//
+template<class T>
+T CPoly<T>::scale( const int nn, const T pt[], const T eta, const T infin, const T smalno, const T base )
+ {
+ int i, l;
+ T hi, lo, max, min, x, sc;
+ T fn_val;
+
+ // Find largest and smallest moduli of coefficients
+ hi = sqrt( infin );
+ lo = smalno / eta;
+ max = 0;
+ min = infin;
+
+ for( i = 0; i <= nn; i++ )
+ {
+ x = pt[ i ];
+ if( x > max ) max = x;
+ if( x != 0 && x < min ) min = x;
+ }
+
+ // Scale only if there are very large or very small components
+ fn_val = 1;
+ if( min >= lo && max <= hi ) return fn_val;
+ x = lo / min;
+ if( x <= 1 )
+ sc = 1 / ( sqrt( max )* sqrt( min ) );
+ else
+ {
+ sc = x;
+ if( infin / sc > max ) sc = 1;
+ }
+ l = (int)( log( sc ) / log(base ) + 0.5 );
+ fn_val = pow( base , l );
+ return fn_val;
+ }
+
+// COMPLEX DIVISION C = A/B, AVOIDING OVERFLOW.
+//
+template<class T>
+void CPoly<T>::cdivid( const T ar, const T ai, const T br, const T bi, T *cr, T *ci )
+ {
+ T r, d, t, infin;
+
+ if( br == 0 && bi == 0 )
+ {
+ // Division by zero, c = infinity
+ mcon( &t, &infin, &t, &t );
+ *cr = infin;
+ *ci = infin;
+ return;
+ }
+
+ if( fabs( br ) < fabs( bi ) )
+ {
+ r = br/ bi;
+ d = bi + r * br;
+ *cr = ( ar * r + ai ) / d;
+ *ci = ( ai * r - ar ) / d;
+ return;
+ }
+
+ r = bi / br;
+ d = br + r * bi;
+ *cr = ( ar + ai * r ) / d;
+ *ci = ( ai - ar * r ) / d;
+ }
+
+// MODULUS OF A COMPLEX NUMBER AVOIDING OVERFLOW.
+//
+template<class T>
+T CPoly<T>::cmod( const T r, const T i )
+ {
+ T ar, ai;
+
+ ar = fabs( r );
+ ai = fabs( i );
+ if( ar < ai )
+ return ai * sqrt( (T) 1.0 + pow( ( ar / ai ), (T) 2.0 ) );
+
+ if( ar > ai )
+ return ar * sqrt( (T) 1.0 + pow( ( ai / ar ),(T) 2.0 ) );
+
+ return ar * sqrt( (T) 2.0 );
+ }
+
+// MCON PROVIDES MACHINE CONSTANTS USED IN VARIOUS PARTS OF THE PROGRAM.
+// THE USER MAY EITHER SET THEM DIRECTLY OR USE THE STATEMENTS BELOW TO
+// COMPUTE THEM. THE MEANING OF THE FOUR CONSTANTS ARE -
+// ETA THE MAXIMUM RELATIVE REPRESENTATION ERROR WHICH CAN BE DESCRIBED
+// AS THE SMALLEST POSITIVE FLOATING-POINT NUMBER SUCH THAT
+// 1.0_dp + ETA > 1.0.
+// INFINY THE LARGEST FLOATING-POINT NUMBER
+// SMALNO THE SMALLEST POSITIVE FLOATING-POINT NUMBER
+// BASE THE BASE OF THE FLOATING-POINT NUMBER SYSTEM USED
+//
+template<class T>
+void CPoly<T>::mcon( T *eta, T *infiny, T *smalno, T *base )
+ {
+ *base = FLT_RADIX;
+ *eta = std::numeric_limits<T>::epsilon();
+ *infiny = std::numeric_limits<T>::max();
+ *smalno = std::numeric_limits<T>::min();
+ }
+
+template class CPoly<float>;
+template class CPoly<double>;
+template class CPoly<long double>;
+
+} // namespace SimTK
diff --git a/SimTKcommon/Polynomial/src/cpoly.h b/SimTKcommon/Polynomial/src/cpoly.h
new file mode 100644
index 0000000..554e187
--- /dev/null
+++ b/SimTKcommon/Polynomial/src/cpoly.h
@@ -0,0 +1,99 @@
+#ifndef SimTK_SimTKCOMMON_CPOLY_H_
+#define SimTK_SimTKCOMMON_CPOLY_H_
+
+/*
+ *******************************************************************************
+ *
+ *
+ * Copyright (c) 2002
+ * Henrik Vestermark
+ * Denmark
+ *
+ * All Rights Reserved
+ *
+ * Permission to use, copy, distribute, and sell this software and its
+ * documentation for any purpose is hereby granted without fee, provided:
+ * THE SOFTWARE IS PROVIDED "AS-IS" AND WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS, IMPLIED OR OTHERWISE, INCLUDING WITHOUT LIMITATION, ANY WARRANTY
+ * OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE. IN NO EVENT SHALL
+ * Henrik Vestermark or Future Team Aps, BE LIABLE FOR ANY SPECIAL,
+ * INCIDENTAL, INDIRECT OR CONSEQUENTIAL DAMAGES OF ANY KIND, OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER OR NOT
+ * ADVISED OF THE POSSIBILITY OF DAMAGE, AND ON ANY THEORY OF LIABILITY,
+ * ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS
+ * SOFTWARE.
+ *
+ *******************************************************************************
+ *
+ *
+ * Module name : cpoly.cpp
+ * Module ID Nbr :
+ * Description : cpoly.cpp -- Jenkins-Traub real polynomial root finder.
+ * Translation of TOMS493 from FORTRAN to C. This
+ * implementation of Jenkins-Traub partially adapts
+ * the original code to a C environment by restruction
+ * many of the 'goto' controls to better fit a block
+ * structured form. It also eliminates the global memory
+ * allocation in favor of local, dynamic memory management.
+ *
+ * The calling conventions are slightly modified to return
+ * the number of roots found as the function value.
+ *
+ * INPUT:
+ * opr - vector of real coefficients in order of
+ * decreasing powers.
+ * opi - vector of imaginary coefficients in order of
+ * decreasing powers.
+ * degree - integer degree of polynomial
+ *
+ * OUTPUT:
+ * zeror,zeroi - output vectors of the
+ * real and imaginary parts of the zeros.
+ * to be consistent with rpoly.cpp the zeros is inthe index
+ * [0..max_degree-1]
+ *
+ * RETURN:
+ * returnval: -1 if leading coefficient is zero, otherwise
+ * number of roots found.
+ * --------------------------------------------------------------------------
+ * Change Record :
+ *
+ * Version Author/Date Description of changes
+ * ------- ----------- ----------------------
+ * 01.01 HVE/021101 Initial release
+ * 01.02 PE/8Aug07 Converted to a class, templatized
+ *
+ * End of Change Record
+ * --------------------------------------------------------------------------
+*/
+
+#include <float.h>
+
+namespace SimTK {
+
+template<class T>
+class CPoly {
+public:
+ static char _V_[];
+ int findRoots( const T *opr, const T *opi, int degree, T *zeror, T *zeroi );
+private:
+ T sr, si, tr, ti, pvr, pvi, are, mre, eta, infin;
+ int nn;
+ T *pr, *pi, *hr, *hi, *qpr, *qpi, *qhr, *qhi, *shr, *shi;
+ void noshft( const int l1 );
+ void fxshft( const int l2, T *zr, T *zi, int *conv );
+ void vrshft( const int l3, T *zr, T *zi, int *conv );
+ void calct( int *bol );
+ void nexth( const int bol );
+ void polyev( const int nn, const T sr, const T si, const T pr[], const T pi[], T qr[], T qi[], T *pvr, T *pvi );
+ T errev( const int nn, const T qr[], const T qi[], const T ms, const T mp, const T are, const T mre );
+ void cauchy( const int nn, T pt[], T q[], T *fn_val );
+ T scale( const int nn, const T pt[], const T eta, const T infin, const T smalno, const T base );
+ void cdivid( const T ar, const T ai, const T br, const T bi, T *cr, T *ci );
+ T cmod( const T r, const T i );
+ void mcon( T *eta, T *infiny, T *smalno, T *base );
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_CPOLY_H_
diff --git a/SimTKcommon/Polynomial/src/rpoly.cpp b/SimTKcommon/Polynomial/src/rpoly.cpp
new file mode 100644
index 0000000..daf18fc
--- /dev/null
+++ b/SimTKcommon/Polynomial/src/rpoly.cpp
@@ -0,0 +1,782 @@
+/* rpoly.cpp -- Jenkins-Traub real polynomial root finder.
+ *
+ * Written by C. Bond, with minor changes by Peter Eastman and Michael
+ * Sherman. This file is in the public domain.
+ *
+ * Translation of TOMS493 from FORTRAN to C. This
+ * implementation of Jenkins-Traub partially adapts
+ * the original code to a C environment by restruction
+ * many of the 'goto' controls to better fit a block
+ * structured form. It also eliminates the global memory
+ * allocation in favor of local, dynamic memory management.
+ *
+ * The calling conventions are slightly modified to return
+ * the number of roots found as the function value.
+ *
+ * INPUT:
+ * op - vector of coefficients in order of
+ * decreasing powers.
+ * degree - integer degree of polynomial
+ *
+ * OUTPUT:
+ * zeror,zeroi - output vectors of the
+ * real and imaginary parts of the zeros.
+ *
+ * RETURN:
+ * returnval: -1 if leading coefficient is zero, otherwise
+ * number of roots found.
+ */
+
+#include <cmath>
+#include <limits>
+#include <algorithm>
+#include "rpoly.h"
+
+namespace SimTK {
+
+template <class T>
+int RPoly<T>::findRoots(T *op, int degree, T *zeror, T *zeroi)
+{
+ T t,aa,bb,cc,*temp,factor,rot;
+ T *pt;
+ T lo,max,min,xx,yy,cosr,sinr,xxx,x,sc,bnd;
+ T xm,ff,df,dx,infin,smalno,base;
+ int cnt,nz,i,j,jj,l,nm1,zerok;
+/* The following statements set machine constants. */
+ base = 2.0;
+ eta = std::numeric_limits<T>::epsilon();
+ infin = std::numeric_limits<T>::max();
+ smalno = std::numeric_limits<T>::min();
+
+ are = eta;
+ mre = eta;
+ lo = smalno/eta;
+/* Initialization of constants for shift rotation. */
+ xx = sqrt((T) 0.5);
+ yy = -xx;
+ rot = (T) 94.0;
+ rot *= (T) 0.017453293;
+ cosr = cos(rot);
+ sinr = sin(rot);
+ n = degree;
+
+/* Algorithm fails if the leading coefficient is zero (or if degree==0). */
+ if (n < 1 || op[0] == 0.0)
+ return -1;
+
+/* Remove the zeros at the origin, if any. */
+ while (op[n] == 0.0) {
+ j = degree - n;
+ zeror[j] = 0.0;
+ zeroi[j] = 0.0;
+ n--;
+ }
+
+ // sherm 20130410: If all coefficients but the leading one were zero, then
+ // all solutions are zero; should be a successful (if boring) return.
+ if (n == 0)
+ return degree;
+
+/*
+ * Allocate memory here
+ */
+ temp = new T [degree+1];
+ pt = new T [degree+1];
+ p = new T [degree+1];
+ qp = new T [degree+1];
+ k = new T [degree+1];
+ qk = new T [degree+1];
+ svk = new T [degree+1];
+/* Make a copy of the coefficients. */
+ for (i=0;i<=n;i++)
+ p[i] = op[i];
+/* Start the algorithm for one zero. */
+_40:
+ if (n == 1) {
+ zeror[degree-1] = -p[1]/p[0];
+ zeroi[degree-1] = 0.0;
+ n -= 1;
+ goto _99;
+ }
+/* Calculate the final zero or pair of zeros. */
+ if (n == 2) {
+ quad(p[0],p[1],p[2],&zeror[degree-2],&zeroi[degree-2],
+ &zeror[degree-1],&zeroi[degree-1]);
+ n -= 2;
+ goto _99;
+ }
+/* Find largest and smallest moduli of coefficients. */
+ max = 0.0;
+ min = infin;
+ for (i=0;i<=n;i++) {
+ x = fabs(p[i]);
+ if (x > max) max = x;
+ if (x != 0.0 && x < min) min = x;
+ }
+/* Scale if there are large or very small coefficients.
+ * Computes a scale factor to multiply the coefficients of the
+ * polynomial. The scaling si done to avoid overflow and to
+ * avoid undetected underflow interfering with the convergence
+ * criterion. The factor is a power of the base.
+ */
+ sc = lo/min;
+ if (sc > 1.0 && infin/sc < max) goto _110;
+ if (sc <= 1.0) {
+ if (max < 10.0) goto _110;
+ if (sc == 0.0)
+ sc = smalno;
+ }
+ l = (int)(log(sc)/log(base) + 0.5);
+ factor = pow(base*(T) 1.0,l);
+ if (factor != 1.0) {
+ for (i=0;i<=n;i++)
+ p[i] = factor*p[i]; /* Scale polynomial. */
+ }
+_110:
+/* Compute lower bound on moduli of roots. */
+ for (i=0;i<=n;i++) {
+ pt[i] = (fabs(p[i]));
+ }
+ pt[n] = - pt[n];
+/* Compute upper estimate of bound. */
+ x = exp((log(-pt[n])-log(pt[0])) / (T)n);
+/* If Newton step at the origin is better, use it. */
+ if (pt[n-1] != 0.0) {
+ xm = -pt[n]/pt[n-1];
+ if (xm < x) x = xm;
+ }
+/* Chop the interval (0,x) until ff <= 0 */
+ while (1) {
+ xm = x*(T) 0.1;
+ ff = pt[0];
+ for (i=1;i<=n;i++)
+ ff = ff*xm + pt[i];
+ if (ff <= 0.0) break;
+ x = xm;
+ }
+ dx = x;
+/* Do Newton interation until x converges to two
+ * decimal places.
+ */
+ while (fabs(dx/x) > 0.005) {
+ ff = pt[0];
+ df = ff;
+ for (i=1;i<n;i++) {
+ ff = ff*x + pt[i];
+ df = df*x + ff;
+ }
+ ff = ff*x + pt[n];
+ dx = ff/df;
+ x -= dx;
+ }
+ bnd = x;
+/* Compute the derivative as the initial k polynomial
+ * and do 5 steps with no shift.
+ */
+ nm1 = n - 1;
+ for (i=1;i<n;i++)
+ k[i] = (T)(n-i)*p[i]/(T)n;
+ k[0] = p[0];
+ aa = p[n];
+ bb = p[n-1];
+ zerok = (k[n-1] == 0);
+ for(jj=0;jj<5;jj++) {
+ cc = k[n-1];
+ if (!zerok) {
+/* Use a scaled form of recurrence if value of k at 0 is nonzero. */
+ t = -aa/cc;
+ for (i=0;i<nm1;i++) {
+ j = n-i-1;
+ k[j] = t*k[j-1]+p[j];
+ }
+ k[0] = p[0];
+ zerok = (fabs(k[n-1]) <= fabs(bb)*eta*10.0);
+ }
+ else {
+/* Use unscaled form of recurrence. */
+ for (i=0;i<nm1;i++) {
+ j = n-i-1;
+ k[j] = k[j-1];
+ }
+ k[0] = 0.0;
+ zerok = (k[n-1] == 0.0);
+ }
+ }
+/* Save k for restarts with new shifts. */
+ for (i=0;i<n;i++)
+ temp[i] = k[i];
+/* Loop to select the quadratic corresponding to each new shift. */
+ for (cnt = 0;cnt < 20;cnt++) {
+/* Quadratic corresponds to a double shift to a
+ * non-real point and its complex conjugate. The point
+ * has modulus bnd and amplitude rotated by 94 degrees
+ * from the previous shift.
+ */
+ xxx = cosr*xx - sinr*yy;
+ yy = sinr*xx + cosr*yy;
+ xx = xxx;
+ sr = bnd*xx;
+ si = bnd*yy;
+ u = (T) -2.0 * sr;
+ v = bnd;
+ fxshfr(20*(cnt+1),&nz);
+ if (nz != 0) {
+/* The second stage jumps directly to one of the third
+ * stage iterations and returns here if successful.
+ * Deflate the polynomial, store the zero or zeros and
+ * return to the main algorithm.
+ */
+ j = degree - n;
+ zeror[j] = szr;
+ zeroi[j] = szi;
+ n -= nz;
+ for (i=0;i<=n;i++)
+ p[i] = qp[i];
+ if (nz != 1) {
+ zeror[j+1] = lzr;
+ zeroi[j+1] = lzi;
+ }
+ goto _40;
+ }
+/* If the iteration is unsuccessful another quadratic
+ * is chosen after restoring k.
+ */
+ for (i=0;i<n;i++) {
+ k[i] = temp[i];
+ }
+ }
+/* Return with failure if no convergence with 20 shifts. */
+_99:
+ delete [] svk;
+ delete [] qk;
+ delete [] k;
+ delete [] qp;
+ delete [] p;
+ delete [] pt;
+ delete [] temp;
+
+ return degree - n;
+}
+/* Computes up to L2 fixed shift k-polynomials,
+ * testing for convergence in the linear or quadratic
+ * case. Initiates one of the variable shift
+ * iterations and returns with the number of zeros
+ * found.
+ */
+template <class T>
+void RPoly<T>::fxshfr(int l2,int *nz)
+{
+ T svu,svv,ui,vi,s;
+ T betas,betav,oss,ovv,ss,vv,ts,tv;
+ T ots,otv,tvv,tss;
+ int type, i,j,iflag,vpass,spass,vtry,stry;
+
+ *nz = 0;
+ betav = 0.25;
+ betas = 0.25;
+ oss = sr;
+ ovv = v;
+/* Evaluate polynomial by synthetic division. */
+ quadsd(n,&u,&v,p,qp,&a,&b);
+ calcsc(&type);
+ for (j=0;j<l2;j++) {
+/* Calculate next k polynomial and estimate v. */
+ nextk(&type);
+ calcsc(&type);
+ newest(type,&ui,&vi);
+ vv = vi;
+/* Estimate s. */
+ ss = 0.0;
+ if (k[n-1] != 0.0) ss = -p[n]/k[n-1];
+ tv = 1.0;
+ ts = 1.0;
+ if (j == 0 || type == 3) goto _70;
+/* Compute relative measures of convergence of s and v sequences. */
+ if (vv != 0.0) tv = fabs((vv-ovv)/vv);
+ if (ss != 0.0) ts = fabs((ss-oss)/ss);
+/* If decreasing, multiply two most recent convergence measures. */
+ tvv = 1.0;
+ if (tv < otv) tvv = tv*otv;
+ tss = 1.0;
+ if (ts < ots) tss = ts*ots;
+/* Compare with convergence criteria. */
+ vpass = (tvv < betav);
+ spass = (tss < betas);
+ if (!(spass || vpass)) goto _70;
+/* At least one sequence has passed the convergence test.
+ * Store variables before iterating.
+ */
+ svu = u;
+ svv = v;
+ for (i=0;i<n;i++) {
+ svk[i] = k[i];
+ }
+ s = ss;
+/* Choose iteration according to the fastest converging
+ * sequence.
+ */
+ vtry = 0;
+ stry = 0;
+ if ( ( spass && (!vpass) ) || tss < tvv) goto _40;
+_20:
+ quadit(&ui,&vi,nz);
+ if (*nz > 0) return;
+/* Quadratic iteration has failed. Flag that it has
+ * been tried and decrease the convergence criterion.
+ */
+ vtry = 1;
+ betav *= 0.25;
+/* Try linear iteration if it has not been tried and
+ * the S sequence is converging.
+ */
+ if (stry || !spass) goto _50;
+ for (i=0;i<n;i++) {
+ k[i] = svk[i];
+ }
+_40:
+ realit(&s,nz,&iflag);
+ if (*nz > 0) return;
+/* Linear iteration has failed. Flag that it has been
+ * tried and decrease the convergence criterion.
+ */
+ stry = 1;
+ betas *=0.25;
+ if (iflag == 0) goto _50;
+/* If linear iteration signals an almost double real
+ * zero attempt quadratic iteration.
+ */
+ ui = -(s+s);
+ vi = s*s;
+ goto _20;
+/* Restore variables. */
+_50:
+ u = svu;
+ v = svv;
+ for (i=0;i<n;i++) {
+ k[i] = svk[i];
+ }
+/* Try quadratic iteration if it has not been tried
+ * and the V sequence is convergin.
+ */
+ if (vpass && !vtry) goto _20;
+/* Recompute QP and scalar values to continue the
+ * second stage.
+ */
+ quadsd(n,&u,&v,p,qp,&a,&b);
+ calcsc(&type);
+_70:
+ ovv = vv;
+ oss = ss;
+ otv = tv;
+ ots = ts;
+ }
+}
+/* Variable-shift k-polynomial iteration for a
+ * quadratic factor converges only if the zeros are
+ * equimodular or nearly so.
+ * uu, vv - coefficients of starting quadratic.
+ * nz - number of zeros found.
+ */
+template <class T>
+void RPoly<T>::quadit(T *uu,T *vv,int *nz)
+{
+ T ui,vi;
+ T mp,omp,ee,relstp,t,zm;
+ int type,i,j,tried;
+
+ *nz = 0;
+ tried = 0;
+ u = *uu;
+ v = *vv;
+ j = 0;
+/* Main loop. */
+_10:
+ quad(1.0,u,v,&szr,&szi,&lzr,&lzi);
+/* Return if roots of the quadratic are real and not
+ * close to multiple or nearly equal and of opposite
+ * sign.
+ */
+
+// sherm 20130410: this early return caused premature termination and
+// then failure to find any roots in rare circumstances with 6th order
+// ellipsoid nearest point equations. Previously (and in all implementations of
+// Jenkins-Traub that I could find) it was just a test on the
+// relative size of the difference with respect to the larger root lzr.
+// I added the std::max(...,0.1) so that if the roots are small then an
+// absolute difference below 0.001 will be considered "close enough" to
+// continue iterating. In the case that failed, the roots were around .0001
+// and .0002, so their difference was considered large compared with 1% of
+// the larger root, 2e-6. But in fact continuing the loop instead resulted in
+// six very high-quality roots instead of none.
+//
+// I'm sorry to say I don't know if I have correctly diagnosed the problem or
+// whether this is the right fix! It does pass the regression tests and
+// apparently cured the ellipsoid problem. I added the particular bad
+// polynomial to the PolynomialTest regression if you want to see it fail.
+// These are the problematic coefficients:
+// 1.0000000000000000
+// 0.021700000000000004
+// 2.9889970904696875e-005
+// 1.0901272298136685e-008
+// -4.4822782160985054e-012
+// -2.6193432740351220e-015
+// -3.0900602527225053e-019
+//
+// Original code:
+ //if (fabs(fabs(szr)-fabs(lzr)) > 0.01 * fabs(lzr)) return;
+// Fixed version:
+ if ((T)fabs(fabs(szr)-fabs(lzr)) > (T)0.01 * std::max((T)fabs(lzr),(T)0.1))
+ return;
+
+/* Evaluate polynomial by quadratic synthetic division. */
+ quadsd(n,&u,&v,p,qp,&a,&b);
+ mp = fabs(a-szr*b) + fabs(szi*b);
+/* Compute a rigorous bound on the rounding error in
+ * evaluating p.
+ */
+ zm = sqrt(fabs(v));
+ ee = (T) 2.0*fabs(qp[0]);
+ t = -szr*b;
+ for (i=1;i<n;i++) {
+ ee = ee*zm + fabs(qp[i]);
+ }
+ ee = ee*zm + fabs(a+t);
+ ee *= ((T)5.0 *mre + (T)4.0*are);
+ ee = ee - ((T)5.0*mre+(T)2.0*are)*(fabs(a+t)+fabs(b)*zm)+(T)2.0*are*fabs(t);
+/* Iteration has converged sufficiently if the
+ * polynomial value is less than 20 times this bound.
+ */
+ if (mp <= 20.0*ee) {
+ *nz = 2;
+ return;
+ }
+ j++;
+/* Stop iteration after 20 steps. */
+ if (j > 20) return;
+ if (j < 2) goto _50;
+ if (relstp > 0.01 || mp < omp || tried) goto _50;
+/* A cluster appears to be stalling the convergence.
+ * Five fixed shift steps are taken with a u,v close
+ * to the cluster.
+ */
+ if (relstp < eta) relstp = eta;
+ relstp = sqrt(relstp);
+ u = u - u*relstp;
+ v = v + v*relstp;
+ quadsd(n,&u,&v,p,qp,&a,&b);
+ for (i=0;i<5;i++) {
+ calcsc(&type);
+ nextk(&type);
+ }
+ tried = 1;
+ j = 0;
+_50:
+ omp = mp;
+/* Calculate next k polynomial and new u and v. */
+ calcsc(&type);
+ nextk(&type);
+ calcsc(&type);
+ newest(type,&ui,&vi);
+/* If vi is zero the iteration is not converging. */
+ if (vi == 0.0) return;
+ relstp = fabs((vi-v)/vi);
+ u = ui;
+ v = vi;
+ goto _10;
+}
+/* Variable-shift H polynomial iteration for a real zero.
+ * sss - starting iterate
+ * nz - number of zeros found
+ * iflag - flag to indicate a pair of zeros near real axis.
+ */
+template <class T>
+void RPoly<T>::realit(T *sss, int *nz, int *iflag)
+{
+ T pv,kv,t,s;
+ T ms,mp,omp,ee;
+ int i,j;
+
+ *nz = 0;
+ s = *sss;
+ *iflag = 0;
+ j = 0;
+/* Main loop */
+ while (1) {
+ pv = p[0];
+/* Evaluate p at s. */
+ qp[0] = pv;
+ for (i=1;i<=n;i++) {
+ pv = pv*s + p[i];
+ qp[i] = pv;
+ }
+ mp = fabs(pv);
+/* Compute a rigorous bound on the error in evaluating p. */
+ ms = fabs(s);
+ ee = (mre/(are+mre))*fabs(qp[0]);
+ for (i=1;i<=n;i++) {
+ ee = ee*ms + fabs(qp[i]);
+ }
+/* Iteration has converged sufficiently if the polynomial
+ * value is less than 20 times this bound.
+ */
+ if (mp <= 20.0*((are+mre)*ee-mre*mp)) {
+ *nz = 1;
+ szr = s;
+ szi = 0.0;
+ return;
+ }
+ j++;
+/* Stop iteration after 10 steps. */
+ if (j > 10) return;
+ if (j < 2) goto _50;
+ if (fabs(t) > 0.001*fabs(s-t) || mp < omp) goto _50;
+/* A cluster of zeros near the real axis has been
+ * encountered. Return with iflag set to initiate a
+ * quadratic iteration.
+ */
+ *iflag = 1;
+ *sss = s;
+ return;
+/* Return if the polynomial value has increased significantly. */
+_50:
+ omp = mp;
+/* Compute t, the next polynomial, and the new iterate. */
+ kv = k[0];
+ qk[0] = kv;
+ for (i=1;i<n;i++) {
+ kv = kv*s + k[i];
+ qk[i] = kv;
+ }
+ if (fabs(kv) <= fabs(k[n-1])*10.0*eta) {
+/* Use unscaled form. */
+ k[0] = 0.0;
+ for (i=1;i<n;i++) {
+ k[i] = qk[i-1];
+ }
+ }
+ else {
+/* Use the scaled form of the recurrence if the value
+ * of k at s is nonzero.
+ */
+ t = -pv/kv;
+ k[0] = qp[0];
+ for (i=1;i<n;i++) {
+ k[i] = t*qk[i-1] + qp[i];
+ }
+ }
+ kv = k[0];
+ for (i=1;i<n;i++) {
+ kv = kv*s + k[i];
+ }
+ t = 0.0;
+ if (fabs(kv) > fabs(k[n-1]*10.0*eta)) t = -pv/kv;
+ s += t;
+ }
+}
+
+/* This routine calculates scalar quantities used to
+ * compute the next k polynomial and new estimates of
+ * the quadratic coefficients.
+ * type - integer variable set here indicating how the
+ * calculations are normalized to avoid overflow.
+ */
+template <class T>
+void RPoly<T>::calcsc(int *type)
+{
+/* Synthetic division of k by the quadratic 1,u,v */
+ quadsd(n-1,&u,&v,k,qk,&c,&d);
+ if (fabs(c) > fabs(k[n-1]*100.0*eta)) goto _10;
+ if (fabs(d) > fabs(k[n-2]*100.0*eta)) goto _10;
+ *type = 3;
+/* Type=3 indicates the quadratic is almost a factor of k. */
+ return;
+_10:
+ if (fabs(d) < fabs(c)) {
+ *type = 1;
+/* Type=1 indicates that all formulas are divided by c. */
+ e = a/c;
+ f = d/c;
+ g = u*e;
+ h = v*b;
+ a3 = a*e + (h/c+g)*b;
+ a1 = b - a*(d/c);
+ a7 = a + g*d + h*f;
+ return;
+ }
+ *type = 2;
+/* Type=2 indicates that all formulas are divided by d. */
+ e = a/d;
+ f = c/d;
+ g = u*b;
+ h = v*b;
+ a3 = (a+g)*e + h*(b/d);
+ a1 = b*f-a;
+ a7 = (f+u)*a + h;
+}
+/* Computes the next k polynomials using scalars
+ * computed in calcsc.
+ */
+template <class T>
+void RPoly<T>::nextk(int *type)
+{
+ T temp;
+ int i;
+
+ if (*type == 3) {
+/* Use unscaled form of the recurrence if type is 3. */
+ k[0] = 0.0;
+ k[1] = 0.0;
+ for (i=2;i<n;i++) {
+ k[i] = qk[i-2];
+ }
+ return;
+ }
+ temp = a;
+ if (*type == 1) temp = b;
+ if (fabs(a1) <= fabs(temp)*eta*10.0) {
+/* If a1 is nearly zero then use a special form of the
+ * recurrence.
+ */
+ k[0] = 0.0;
+ k[1] = -a7*qp[0];
+ for(i=2;i<n;i++) {
+ k[i] = a3*qk[i-2] - a7*qp[i-1];
+ }
+ return;
+ }
+/* Use scaled form of the recurrence. */
+ a7 /= a1;
+ a3 /= a1;
+ k[0] = qp[0];
+ k[1] = qp[1] - a7*qp[0];
+ for (i=2;i<n;i++) {
+ k[i] = a3*qk[i-2] - a7*qp[i-1] + qp[i];
+ }
+}
+/* Compute new estimates of the quadratic coefficients
+ * using the scalars computed in calcsc.
+ */
+template <class T>
+void RPoly<T>::newest(int type,T *uu,T *vv)
+{
+ T a4,a5,b1,b2,c1,c2,c3,c4,temp;
+
+/* Use formulas appropriate to setting of type. */
+ if (type == 3) {
+/* If type=3 the quadratic is zeroed. */
+ *uu = 0.0;
+ *vv = 0.0;
+ return;
+ }
+ if (type == 2) {
+ a4 = (a+g)*f + h;
+ a5 = (f+u)*c + v*d;
+ }
+ else {
+ a4 = a + u*b +h*f;
+ a5 = c + (u+v*f)*d;
+ }
+/* Evaluate new quadratic coefficients. */
+ b1 = -k[n-1]/p[n];
+ b2 = -(k[n-2]+b1*p[n-1])/p[n];
+ c1 = v*b2*a1;
+ c2 = b1*a7;
+ c3 = b1*b1*a3;
+ c4 = c1 - c2 - c3;
+ temp = a5 + b1*a4 - c4;
+ if (temp == 0.0) {
+ *uu = 0.0;
+ *vv = 0.0;
+ return;
+ }
+ *uu = u - (u*(c3+c2)+v*(b1*a1+b2*a7))/temp;
+ *vv = v*((T)1.0+c4/temp);
+ return;
+}
+
+/* Divides p by the quadratic 1,u,v placing the quotient
+ * in q and the remainder in a,b.
+ */
+template <class T>
+void RPoly<T>::quadsd(int nn,T *u,T *v,T *p,T *q,
+ T *a,T *b)
+{
+ T c;
+ int i;
+ *b = p[0];
+ q[0] = *b;
+ *a = p[1] - (*b)*(*u);
+ q[1] = *a;
+ for (i=2;i<=nn;i++) {
+ c = p[i] - (*a)*(*u) - (*b)*(*v);
+ q[i] = c;
+ *b = *a;
+ *a = c;
+ }
+}
+/* Calculate the zeros of the quadratic a*z^2 + b1*z + c.
+ * The quadratic formula, modified to avoid overflow, is used
+ * to find the larger zero if the zeros are real and both
+ * are complex. The smaller real zero is found directly from
+ * the product of the zeros c/a.
+ */
+template <class T>
+void RPoly<T>::quad(T a,T b1,T c,T *sr,T *si,
+ T *lr,T *li)
+{
+ T b,d,e;
+
+ if (a == 0.0) { /* less than two roots */
+ if (b1 != 0.0)
+ *sr = -c/b1;
+ else
+ *sr = 0.0;
+ *lr = 0.0;
+ *si = 0.0;
+ *li = 0.0;
+ return;
+ }
+ if (c == 0.0) { /* one real root, one zero root */
+ *sr = 0.0;
+ *lr = -b1/a;
+ *si = 0.0;
+ *li = 0.0;
+ return;
+ }
+/* Compute discriminant avoiding overflow. */
+ b = b1/(T) 2.0;
+ if (fabs(b) < fabs(c)) {
+ if (c < 0.0)
+ e = -a;
+ else
+ e = a;
+ e = b*(b/fabs(c)) - e;
+ d = sqrt(fabs(e))*sqrt(fabs(c));
+ }
+ else {
+ e = (T) 1.0 - (a/b)*(c/b);
+ d = sqrt(fabs(e))*fabs(b);
+ }
+ if (e < 0.0) { /* complex conjugate zeros */
+ *sr = -b/a;
+ *lr = *sr;
+ *si = fabs(d/a);
+ *li = -(*si);
+ }
+ else {
+ if (b >= 0.0) /* real zeros. */
+ d = -d;
+ *lr = (-b+d)/a;
+ *sr = 0.0;
+ if (*lr != 0.0)
+ *sr = (c/ *lr)/a;
+ *si = 0.0;
+ *li = 0.0;
+ }
+}
+
+template class RPoly<float>;
+template class RPoly<double>;
+template class RPoly<long double>;
+
+} // namespace SimTK
diff --git a/SimTKcommon/Polynomial/src/rpoly.h b/SimTKcommon/Polynomial/src/rpoly.h
new file mode 100644
index 0000000..6fbfee5
--- /dev/null
+++ b/SimTKcommon/Polynomial/src/rpoly.h
@@ -0,0 +1,59 @@
+#ifndef SimTK_SimTKCOMMON_RPOLY_H_
+#define SimTK_SimTKCOMMON_RPOLY_H_
+
+/* rpoly.cpp -- Jenkins-Traub real polynomial root finder.
+ *
+ * Written by C. Bond, with minor changes by Peter Eastman.
+ * This file is in the public domain.
+ *
+ * Translation of TOMS493 from FORTRAN to C. This
+ * implementation of Jenkins-Traub partially adapts
+ * the original code to a C environment by restruction
+ * many of the 'goto' controls to better fit a block
+ * structured form. It also eliminates the global memory
+ * allocation in favor of local, dynamic memory management.
+ *
+ * The calling conventions are slightly modified to return
+ * the number of roots found as the function value.
+ *
+ * INPUT:
+ * op - vector of coefficients in order of
+ * decreasing powers.
+ * degree - integer degree of polynomial
+ *
+ * OUTPUT:
+ * zeror,zeroi - output vectors of the
+ * real and imaginary parts of the zeros.
+ *
+ * RETURN:
+ * returnval: -1 if leading coefficient is zero, otherwise
+ * number of roots found.
+ */
+
+namespace SimTK {
+
+template <class T>
+class RPoly {
+public:
+ int findRoots(T *op, int degree, T *zeror, T *zeroi);
+private:
+ void quad(T a,T b1,T c,T *sr,T *si,
+ T *lr,T *li);
+ void fxshfr(int l2, int *nz);
+ void quadit(T *uu,T *vv,int *nz);
+ void realit(T *sss, int *nz, int *iflag);
+ void calcsc(int *type);
+ void nextk(int *type);
+ void newest(int type,T *uu,T *vv);
+ void quadsd(int n,T *u,T *v,T *p,T *q,
+ T *a,T *b);
+ T *p,*qp,*k,*qk,*svk;
+ T sr,si,u,v,a,b,c,d,a1,a2;
+ T a3,a6,a7,e,f,g,h,szr,szi,lzr,lzi;
+ T eta,are,mre;
+ int n,nn,nmi,zerok;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_RPOLY_H_
diff --git a/SimTKcommon/Random/etc/LICENSE.txt b/SimTKcommon/Random/etc/LICENSE.txt
new file mode 100644
index 0000000..decb299
--- /dev/null
+++ b/SimTKcommon/Random/etc/LICENSE.txt
@@ -0,0 +1,29 @@
+Copyright (c) 2006,2007 Mutsuo Saito, Makoto Matsumoto and Hiroshima
+University. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are
+met:
+
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above
+ copyright notice, this list of conditions and the following
+ disclaimer in the documentation and/or other materials provided
+ with the distribution.
+ * Neither the name of the Hiroshima University nor the names of
+ its contributors may be used to endorse or promote products
+ derived from this software without specific prior written
+ permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/SimTKcommon/Random/include/SimTKcommon/internal/Random.h b/SimTKcommon/Random/include/SimTKcommon/internal/Random.h
new file mode 100644
index 0000000..1fb6694
--- /dev/null
+++ b/SimTKcommon/Random/include/SimTKcommon/internal/Random.h
@@ -0,0 +1,179 @@
+#ifndef SimTK_SimTKCOMMON_RANDOM_H_
+#define SimTK_SimTKCOMMON_RANDOM_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/basics.h"
+
+namespace SimTK {
+
+/**
+ * This class defines the interface for pseudo-random number generators. Subclasses generate numbers according to specific
+ * distributions. Currently, there are two such subclasses: Random::Uniform and Random::Gaussian. For example, to generate
+ * a series of pseudo-random numbers uniformly distributed between 0 and 100, you would call:
+ * <pre>
+ * Random::Uniform random(0.0, 100.0);
+ * Real nextValue = random.getValue(); // Each time you call this, it will return a different value.
+ * </pre>
+ * Although the numbers are distributed in a seemingly random way, they are nonetheless deterministic, so you can create
+ * several random number generators that each returns exactly the same sequence of numbers. The sequence is determined
+ * by the seed value with which the Random object is initialized. By default, a different seed is used for every object.
+ * You can invoke setSeed(int seed) on a Random object to explicitly specify the seed to use. Each seed
+ * value corresponds to a different sequence of numbers that is uncorrelated with all others.
+ *
+ * This class is implemented using the SIMD-oriented Fast Mersenne Twister (SFMT) library. It provides
+ * good performance, excellent statistical properties, and a very long period.
+ *
+ * The methods of this class do not provide any synchronization or other mechanism to ensure thread safety.
+ * It is therefore important that a single Random object not be accessed from multiple threads. One minor
+ * concession to threads: even if you don't set the seed explicitly, each thread's Random object will
+ * use a different seed so you'll get a unique series of numbers in each thread.
+ */
+
+class SimTK_SimTKCOMMON_EXPORT Random {
+public:
+ class Uniform;
+ class Gaussian;
+ class RandomImpl;
+ ~Random();
+ /**
+ * Reinitialize this random number generator with a new seed value.
+ */
+ void setSeed(int seed);
+ /**
+ * Get the next value in the pseudo-random sequence.
+ */
+ Real getValue() const;
+ /**
+ * Fill an array with values from the pseudo-random sequence.
+ */
+ void fillArray(Real array[], int length) const;
+protected:
+ RandomImpl* impl;
+ /**
+ * This constructor should never be invoked directly. Instead, create an instance of one of the subclasses.
+ */
+ Random();
+ /**
+ * Get the internal object which implements the random number generator.
+ */
+ RandomImpl& getImpl();
+ /**
+ * Get a constant reference to the internal object which implements the random number generator.
+ */
+ const RandomImpl& getConstImpl() const;
+private:
+ // Suppress copy constructor and copy assignment.
+ Random(const Random& r);
+ Random operator=(const Random& r);
+};
+
+/**
+ * This is a subclass of Random that generates numbers uniformly distributed within a specified range.
+ */
+
+class SimTK_SimTKCOMMON_EXPORT Random::Uniform : public Random {
+public:
+ class UniformImpl;
+ /**
+ * Create a new random number generator that produces values uniformly distributed between 0 (inclusive) and 1 (exclusive).
+ */
+ Uniform();
+ /**
+ * Create a new random number generator that produces values uniformly distributed between min (inclusive) and max (exclusive).
+ */
+ Uniform(Real min, Real max);
+ /**
+ * Get a random integer, uniformly distributed between 0 (inclusive) and max (exclusive).
+ */
+ int getIntValue();
+ /**
+ * Get the lower end of the range in which values are uniformly distributed.
+ */
+ Real getMin() const;
+ /**
+ * Set the lower end of the range in which values are uniformly distributed.
+ */
+ void setMin(Real min);
+ /**
+ * Get the upper end of the range in which values are uniformly distributed.
+ */
+ Real getMax() const;
+ /**
+ * Set the upper end of the range in which values are uniformly distributed.
+ */
+ void setMax(Real max);
+protected:
+ UniformImpl& getImpl();
+ const UniformImpl& getConstImpl() const;
+private:
+ // Must suppress here if base class members are suppressed.
+ Uniform(const Uniform& r);
+ Uniform operator=(const Uniform& r);
+};
+
+/**
+ * This is a subclass of Random that generates numbers according to a Gaussian distribution with a
+ * specified mean and standard deviation.
+ */
+
+class SimTK_SimTKCOMMON_EXPORT Random::Gaussian : public Random {
+public:
+ class GaussianImpl;
+ /**
+ * Create a new random number generator that produces values according to a Gaussian distribution with mean 0 and standard deviation 1.
+ */
+ Gaussian();
+ /**
+ * Create a new random number generator that produces values according to a Gaussian distribution with the specified mean and standard deviation.
+ */
+ Gaussian(Real mean, Real stddev);
+ /**
+ * Get the mean of the Gaussian distribution.
+ */
+ Real getMean() const;
+ /**
+ * Set the mean of the Gaussian distribution.
+ */
+ void setMean(Real mean);
+ /**
+ * Get the standard deviation of the Gaussian distribution.
+ */
+ Real getStdDev() const;
+ /**
+ * Set the standard deviation of the Gaussian distribution.
+ */
+ void setStdDev(Real stddev);
+protected:
+ GaussianImpl& getImpl();
+ const GaussianImpl& getConstImpl() const;
+private:
+ // Must suppress here if base class members are suppressed.
+ Gaussian(const Gaussian& r);
+ Gaussian operator=(const Gaussian& r);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_RANDOM_H_
diff --git a/SimTKcommon/Random/src/Random.cpp b/SimTKcommon/Random/src/Random.cpp
new file mode 100644
index 0000000..582cec9
--- /dev/null
+++ b/SimTKcommon/Random/src/Random.cpp
@@ -0,0 +1,283 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/basics.h"
+#include "SimTKcommon/internal/Random.h"
+#include "SimTKcommon/internal/AtomicInteger.h"
+#include "SFMT.h"
+
+#include <cassert>
+#include <cmath>
+
+using namespace SimTK_SFMT;
+
+namespace SimTK {
+
+/**
+ * This is the private implementation class for Random. It has a subclass corresponding to each subclass of Random.
+ */
+
+class Random::RandomImpl {
+private:
+ mutable SimTK_SFMT::SFMTData* sfmt;
+ static const int bufferSize = 1024;
+ mutable uint64_t buffer[bufferSize];
+ mutable int nextIndex;
+ static AtomicInteger nextSeed;
+public:
+ class UniformImpl;
+ class GaussianImpl;
+
+ RandomImpl() {
+ sfmt = createSFMTData();
+ nextIndex = bufferSize;
+ init_gen_rand(++nextSeed, *sfmt);
+ }
+
+ virtual ~RandomImpl() {
+ deleteSFMTData(sfmt);
+ }
+
+ virtual void setSeed(int seed) {
+ nextIndex = bufferSize;
+ init_gen_rand(seed, *sfmt);
+ }
+
+ virtual Real getValue() const = 0;
+
+ Real getNextRandom() const {
+ if (nextIndex >= bufferSize) {
+ // There are no remaining values in the buffer, so we need to refill it.
+
+ fill_array64(buffer, bufferSize, *sfmt);
+ nextIndex = 0;
+ }
+ return Real(to_res53(buffer[nextIndex++]));
+ }
+
+ int getInt(int max) {
+ return (int) floor(getValue()*max);
+ }
+
+ void fillArray(Real array[], int length) const {
+ for (int i = 0; i < length; ++i)
+ array[i] = getValue();
+ }
+};
+
+AtomicInteger Random::RandomImpl::nextSeed = 0;
+
+/**
+ * This is the private implementation class for uniform random numbers.
+ */
+
+class Random::Uniform::UniformImpl : public Random::RandomImpl {
+private:
+ Real min, max, range;
+public:
+ UniformImpl(Real min, Real max) : min(min), max(max), range(max-min) {
+ }
+
+ Real getValue() const {
+ return min+getNextRandom()*range;
+ }
+
+ Real getMin() const {
+ return min;
+ }
+
+ void setMin(Real value) {
+ min = value;
+ range = max-min;
+ }
+
+ Real getMax() const {
+ return max;
+ }
+
+ void setMax(Real value) {
+ max = value;
+ range = max-min;
+ }
+};
+
+/**
+ * This is the private implementation class for Gaussian random numbers.
+ */
+
+class Random::Gaussian::GaussianImpl : public Random::RandomImpl {
+private:
+ Real mean, stddev;
+ mutable Real nextGaussian;
+ mutable bool nextGaussianIsValid;
+public:
+ GaussianImpl(Real mean, Real stddev) : mean(mean), stddev(stddev) {
+ nextGaussianIsValid = false;
+ }
+
+ Real getValue() const {
+ if (nextGaussianIsValid) {
+ nextGaussianIsValid = false;
+ return mean+stddev*nextGaussian;
+ }
+
+ // Use the polar form of the Box-Muller transformation to generate two Gaussian random numbers.
+
+ Real x, y, r2;
+ do {
+ x = 2*getNextRandom()-1;
+ y = 2*getNextRandom()-1;
+ r2 = x*x + y*y;
+ } while (r2 >= 1.0 || r2 == 0.0);
+ Real multiplier = std::sqrt((-2*std::log(r2))/r2);
+ nextGaussian = y*multiplier;
+ nextGaussianIsValid = true;
+ return mean+stddev*x*multiplier;
+ }
+
+ void setSeed(int seed) {
+ RandomImpl::setSeed(seed);
+ nextGaussianIsValid = false;
+ }
+
+ Real getMean() const {
+ return mean;
+ }
+
+ void setMean(Real value) {
+ mean = value;
+ }
+
+ Real getStdDev() const {
+ return stddev;
+ }
+
+ void setStdDev(Real value) {
+ stddev = value;
+ }
+};
+
+Random::Random() : impl(0) {
+}
+
+Random::~Random() {
+ delete impl;
+}
+
+Random::RandomImpl& Random::getImpl() {
+ assert(impl);
+ return *impl;
+}
+
+const Random::RandomImpl& Random::getConstImpl() const {
+ assert(impl);
+ return *impl;
+}
+
+void Random::setSeed(int seed) {
+ getImpl().setSeed(seed);
+}
+
+Real Random::getValue() const {
+ return getConstImpl().getValue();
+}
+
+
+void Random::fillArray(Real array[], int length) const {
+ getConstImpl().fillArray(array, length);
+}
+
+Random::Uniform::Uniform() {
+ impl = new Random::Uniform::UniformImpl(0.0, 1.0);
+}
+
+Random::Uniform::Uniform(Real min, Real max) {
+ impl = new Random::Uniform::UniformImpl(min, max);
+}
+
+Random::Uniform::UniformImpl& Random::Uniform::getImpl() {
+ assert(impl);
+ return SimTK_DYNAMIC_CAST_DEBUG<Random::Uniform::UniformImpl&>(*impl);
+}
+
+const Random::Uniform::UniformImpl& Random::Uniform::getConstImpl() const {
+ assert(impl);
+ return SimTK_DYNAMIC_CAST_DEBUG<Random::Uniform::UniformImpl&>(*impl);
+}
+
+int Random::Uniform::getIntValue() {
+ return (int) std::floor(getImpl().getValue());
+}
+
+Real Random::Uniform::getMin() const {
+ return getConstImpl().getMin();
+}
+
+void Random::Uniform::setMin(Real min) {
+ getImpl().setMin(min);
+}
+
+Real Random::Uniform::getMax() const {
+ return getConstImpl().getMax();
+}
+
+void Random::Uniform::setMax(Real max) {
+ getImpl().setMax(max);
+}
+
+Random::Gaussian::Gaussian() {
+ impl = new Random::Gaussian::GaussianImpl(0.0, 1.0);
+}
+
+Random::Gaussian::Gaussian(Real mean, Real stddev) {
+ impl = new Random::Gaussian::GaussianImpl(mean, stddev);
+}
+
+Random::Gaussian::GaussianImpl& Random::Gaussian::getImpl() {
+ assert(impl);
+ return SimTK_DYNAMIC_CAST_DEBUG<Random::Gaussian::GaussianImpl&>(*impl);
+}
+
+const Random::Gaussian::GaussianImpl& Random::Gaussian::getConstImpl() const {
+ assert(impl);
+ return SimTK_DYNAMIC_CAST_DEBUG<Random::Gaussian::GaussianImpl&>(*impl);
+}
+
+Real Random::Gaussian::getMean() const {
+ return getConstImpl().getMean();
+}
+
+void Random::Gaussian::setMean(Real mean) {
+ getImpl().setMean(mean);
+}
+
+Real Random::Gaussian::getStdDev() const {
+ return getConstImpl().getStdDev();
+}
+
+void Random::Gaussian::setStdDev(Real stddev) {
+ getImpl().setStdDev(stddev);
+}
+
+} // namespace SimTK
diff --git a/SimTKcommon/Random/src/SFMT-alti.h b/SimTKcommon/Random/src/SFMT-alti.h
new file mode 100644
index 0000000..df3186c
--- /dev/null
+++ b/SimTKcommon/Random/src/SFMT-alti.h
@@ -0,0 +1,156 @@
+/**
+ * @file SFMT-alti.h
+ *
+ * @brief SIMD oriented Fast Mersenne Twister(SFMT)
+ * pseudorandom number generator
+ *
+ * @author Mutsuo Saito (Hiroshima University)
+ * @author Makoto Matsumoto (Hiroshima University)
+ *
+ * Copyright (C) 2007 Mutsuo Saito, Makoto Matsumoto and Hiroshima
+ * University. All rights reserved.
+ *
+ * The new BSD License is applied to this software.
+ * see LICENSE.txt
+ */
+
+#ifndef SFMT_ALTI_H
+#define SFMT_ALTI_H
+
+inline static vector unsigned int vec_recursion(vector unsigned int a,
+ vector unsigned int b,
+ vector unsigned int c,
+ vector unsigned int d)
+ ALWAYSINLINE;
+
+/**
+ * This function represents the recursion formula in AltiVec and BIG ENDIAN.
+ * @param a a 128-bit part of the interal state array
+ * @param b a 128-bit part of the interal state array
+ * @param c a 128-bit part of the interal state array
+ * @param d a 128-bit part of the interal state array
+ * @return output
+ */
+inline static vector unsigned int vec_recursion(vector unsigned int a,
+ vector unsigned int b,
+ vector unsigned int c,
+ vector unsigned int d) {
+
+ const vector unsigned int sl1 = ALTI_SL1;
+ const vector unsigned int sr1 = ALTI_SR1;
+#ifdef ONLY64
+ const vector unsigned int mask = ALTI_MSK64;
+ const vector unsigned char perm_sl = ALTI_SL2_PERM64;
+ const vector unsigned char perm_sr = ALTI_SR2_PERM64;
+#else
+ const vector unsigned int mask = ALTI_MSK;
+ const vector unsigned char perm_sl = ALTI_SL2_PERM;
+ const vector unsigned char perm_sr = ALTI_SR2_PERM;
+#endif
+ vector unsigned int v, w, x, y, z;
+ x = vec_perm(a, (vector unsigned int)perm_sl, perm_sl);
+ v = a;
+ y = vec_sr(b, sr1);
+ z = vec_perm(c, (vector unsigned int)perm_sr, perm_sr);
+ w = vec_sl(d, sl1);
+ z = vec_xor(z, w);
+ y = vec_and(y, mask);
+ v = vec_xor(v, x);
+ z = vec_xor(z, y);
+ z = vec_xor(z, v);
+ return z;
+}
+
+/**
+ * This function fills the internal state array with pseudorandom
+ * integers.
+ */
+inline static void gen_rand_all(void) {
+ int i;
+ vector unsigned int r, r1, r2;
+
+ r1 = sfmt[N - 2].s;
+ r2 = sfmt[N - 1].s;
+ for (i = 0; i < N - POS1; i++) {
+ r = vec_recursion(sfmt[i].s, sfmt[i + POS1].s, r1, r2);
+ sfmt[i].s = r;
+ r1 = r2;
+ r2 = r;
+ }
+ for (; i < N; i++) {
+ r = vec_recursion(sfmt[i].s, sfmt[i + POS1 - N].s, r1, r2);
+ sfmt[i].s = r;
+ r1 = r2;
+ r2 = r;
+ }
+}
+
+/**
+ * This function fills the user-specified array with pseudorandom
+ * integers.
+ *
+ * @param array an 128-bit array to be filled by pseudorandom numbers.
+ * @param size number of 128-bit pesudorandom numbers to be generated.
+ */
+inline static void gen_rand_array(w128_t *array, int size) {
+ int i, j;
+ vector unsigned int r, r1, r2;
+
+ r1 = sfmt[N - 2].s;
+ r2 = sfmt[N - 1].s;
+ for (i = 0; i < N - POS1; i++) {
+ r = vec_recursion(sfmt[i].s, sfmt[i + POS1].s, r1, r2);
+ array[i].s = r;
+ r1 = r2;
+ r2 = r;
+ }
+ for (; i < N; i++) {
+ r = vec_recursion(sfmt[i].s, array[i + POS1 - N].s, r1, r2);
+ array[i].s = r;
+ r1 = r2;
+ r2 = r;
+ }
+ /* main loop */
+ for (; i < size - N; i++) {
+ r = vec_recursion(array[i - N].s, array[i + POS1 - N].s, r1, r2);
+ array[i].s = r;
+ r1 = r2;
+ r2 = r;
+ }
+ for (j = 0; j < 2 * N - size; j++) {
+ sfmt[j].s = array[j + size - N].s;
+ }
+ for (; i < size; i++) {
+ r = vec_recursion(array[i - N].s, array[i + POS1 - N].s, r1, r2);
+ array[i].s = r;
+ sfmt[j++].s = r;
+ r1 = r2;
+ r2 = r;
+ }
+}
+
+#ifndef ONLY64
+#if defined(__APPLE__)
+#define ALTI_SWAP (vector unsigned char) \
+ (4, 5, 6, 7, 0, 1, 2, 3, 12, 13, 14, 15, 8, 9, 10, 11)
+#else
+#define ALTI_SWAP {4, 5, 6, 7, 0, 1, 2, 3, 12, 13, 14, 15, 8, 9, 10, 11}
+#endif
+/**
+ * This function swaps high and low 32-bit of 64-bit integers in user
+ * specified array.
+ *
+ * @param array an 128-bit array to be swaped.
+ * @param size size of 128-bit array.
+ */
+inline static void swap(w128_t *array, int size) {
+ int i;
+ const vector unsigned char perm = ALTI_SWAP;
+
+ for (i = 0; i < size; i++) {
+ array[i].s = vec_perm(array[i].s, (vector unsigned int)perm, perm);
+ }
+}
+#endif
+
+#endif
diff --git a/SimTKcommon/Random/src/SFMT-params.h b/SimTKcommon/Random/src/SFMT-params.h
new file mode 100644
index 0000000..ff9f238
--- /dev/null
+++ b/SimTKcommon/Random/src/SFMT-params.h
@@ -0,0 +1,94 @@
+#ifndef SFMT_PARAMS_H
+#define SFMT_PARAMS_H
+
+#if !defined(MEXP)
+ #define MEXP 19937
+#endif
+/*-----------------
+ BASIC DEFINITIONS
+ -----------------*/
+/** Mersenne Exponent. The period of the sequence
+ * is a multiple of 2^MEXP-1.
+ * #define MEXP 19937 */
+/** SFMT generator has an internal state array of 128-bit integers,
+ * and N is its size. */
+#define N (MEXP / 128 + 1)
+/** N32 is the size of internal state array when regarded as an array
+ * of 32-bit integers.*/
+#define N32 (N * 4)
+/** N64 is the size of internal state array when regarded as an array
+ * of 64-bit integers.*/
+#define N64 (N * 2)
+
+/*----------------------
+ the parameters of SFMT
+ following definitions are in paramsXXXX.h file.
+ ----------------------*/
+/** the pick up position of the array.
+#define POS1 122
+*/
+
+/** the parameter of shift left as four 32-bit registers.
+#define SL1 18
+ */
+
+/** the parameter of shift left as one 128-bit register.
+ * The 128-bit integer is shifted by (SL2 * 8) bits.
+#define SL2 1
+*/
+
+/** the parameter of shift right as four 32-bit registers.
+#define SR1 11
+*/
+
+/** the parameter of shift right as one 128-bit register.
+ * The 128-bit integer is shifted by (SL2 * 8) bits.
+#define SR2 1
+*/
+
+/** A bitmask, used in the recursion. These parameters are introduced
+ * to break symmetry of SIMD.
+#define MSK1 0xdfffffefU
+#define MSK2 0xddfecb7fU
+#define MSK3 0xbffaffffU
+#define MSK4 0xbffffff6U
+*/
+
+/** These definitions are part of a 128-bit period certification vector.
+#define PARITY1 0x00000001U
+#define PARITY2 0x00000000U
+#define PARITY3 0x00000000U
+#define PARITY4 0xc98e126aU
+*/
+
+#if MEXP == 607
+ #include "SFMT-params607.h"
+#elif MEXP == 1279
+ #include "SFMT-params1279.h"
+#elif MEXP == 2281
+ #include "SFMT-params2281.h"
+#elif MEXP == 4253
+ #include "SFMT-params4253.h"
+#elif MEXP == 11213
+ #include "SFMT-params11213.h"
+#elif MEXP == 19937
+ #include "SFMT-params19937.h"
+#elif MEXP == 44497
+ #include "SFMT-params44497.h"
+#elif MEXP == 86243
+ #include "SFMT-params86243.h"
+#elif MEXP == 132049
+ #include "SFMT-params132049.h"
+#elif MEXP == 216091
+ #include "SFMT-params216091.h"
+#else
+#ifdef __GNUC__
+ #error "MEXP is not valid."
+ #undef MEXP
+#else
+ #undef MEXP
+#endif
+
+#endif
+
+#endif /* SFMT_PARAMS_H */
diff --git a/SimTKcommon/Random/src/SFMT-params11213.h b/SimTKcommon/Random/src/SFMT-params11213.h
new file mode 100644
index 0000000..244d313
--- /dev/null
+++ b/SimTKcommon/Random/src/SFMT-params11213.h
@@ -0,0 +1,46 @@
+#ifndef SFMT_PARAMS11213_H
+#define SFMT_PARAMS11213_H
+
+#define POS1 68
+#define SL1 14
+#define SL2 3
+#define SR1 7
+#define SR2 3
+#define MSK1 0xeffff7fbU
+#define MSK2 0xffffffefU
+#define MSK3 0xdfdfbfffU
+#define MSK4 0x7fffdbfdU
+#define PARITY1 0x00000001U
+#define PARITY2 0x00000000U
+#define PARITY3 0xe8148000U
+#define PARITY4 0xd0c7afa3U
+
+
+/* PARAMETERS FOR ALTIVEC */
+#if defined(__APPLE__) /* For OSX */
+ #define ALTI_SL1 (vector unsigned int)(SL1, SL1, SL1, SL1)
+ #define ALTI_SR1 (vector unsigned int)(SR1, SR1, SR1, SR1)
+ #define ALTI_MSK (vector unsigned int)(MSK1, MSK2, MSK3, MSK4)
+ #define ALTI_MSK64 \
+ (vector unsigned int)(MSK2, MSK1, MSK4, MSK3)
+ #define ALTI_SL2_PERM \
+ (vector unsigned char)(3,21,21,21,7,0,1,2,11,4,5,6,15,8,9,10)
+ #define ALTI_SL2_PERM64 \
+ (vector unsigned char)(3,4,5,6,7,29,29,29,11,12,13,14,15,0,1,2)
+ #define ALTI_SR2_PERM \
+ (vector unsigned char)(5,6,7,0,9,10,11,4,13,14,15,8,19,19,19,12)
+ #define ALTI_SR2_PERM64 \
+ (vector unsigned char)(13,14,15,0,1,2,3,4,19,19,19,8,9,10,11,12)
+#else /* For OTHER OSs(Linux?) */
+ #define ALTI_SL1 {SL1, SL1, SL1, SL1}
+ #define ALTI_SR1 {SR1, SR1, SR1, SR1}
+ #define ALTI_MSK {MSK1, MSK2, MSK3, MSK4}
+ #define ALTI_MSK64 {MSK2, MSK1, MSK4, MSK3}
+ #define ALTI_SL2_PERM {3,21,21,21,7,0,1,2,11,4,5,6,15,8,9,10}
+ #define ALTI_SL2_PERM64 {3,4,5,6,7,29,29,29,11,12,13,14,15,0,1,2}
+ #define ALTI_SR2_PERM {5,6,7,0,9,10,11,4,13,14,15,8,19,19,19,12}
+ #define ALTI_SR2_PERM64 {13,14,15,0,1,2,3,4,19,19,19,8,9,10,11,12}
+#endif /* For OSX */
+#define IDSTR "SFMT-11213:68-14-3-7-3:effff7fb-ffffffef-dfdfbfff-7fffdbfd"
+
+#endif /* SFMT_PARAMS11213_H */
diff --git a/SimTKcommon/Random/src/SFMT-params1279.h b/SimTKcommon/Random/src/SFMT-params1279.h
new file mode 100644
index 0000000..df538df
--- /dev/null
+++ b/SimTKcommon/Random/src/SFMT-params1279.h
@@ -0,0 +1,46 @@
+#ifndef SFMT_PARAMS1279_H
+#define SFMT_PARAMS1279_H
+
+#define POS1 7
+#define SL1 14
+#define SL2 3
+#define SR1 5
+#define SR2 1
+#define MSK1 0xf7fefffdU
+#define MSK2 0x7fefcfffU
+#define MSK3 0xaff3ef3fU
+#define MSK4 0xb5ffff7fU
+#define PARITY1 0x00000001U
+#define PARITY2 0x00000000U
+#define PARITY3 0x00000000U
+#define PARITY4 0x20000000U
+
+
+/* PARAMETERS FOR ALTIVEC */
+#if defined(__APPLE__) /* For OSX */
+ #define ALTI_SL1 (vector unsigned int)(SL1, SL1, SL1, SL1)
+ #define ALTI_SR1 (vector unsigned int)(SR1, SR1, SR1, SR1)
+ #define ALTI_MSK (vector unsigned int)(MSK1, MSK2, MSK3, MSK4)
+ #define ALTI_MSK64 \
+ (vector unsigned int)(MSK2, MSK1, MSK4, MSK3)
+ #define ALTI_SL2_PERM \
+ (vector unsigned char)(3,21,21,21,7,0,1,2,11,4,5,6,15,8,9,10)
+ #define ALTI_SL2_PERM64 \
+ (vector unsigned char)(3,4,5,6,7,29,29,29,11,12,13,14,15,0,1,2)
+ #define ALTI_SR2_PERM \
+ (vector unsigned char)(7,0,1,2,11,4,5,6,15,8,9,10,17,12,13,14)
+ #define ALTI_SR2_PERM64 \
+ (vector unsigned char)(15,0,1,2,3,4,5,6,17,8,9,10,11,12,13,14)
+#else /* For OTHER OSs(Linux?) */
+ #define ALTI_SL1 {SL1, SL1, SL1, SL1}
+ #define ALTI_SR1 {SR1, SR1, SR1, SR1}
+ #define ALTI_MSK {MSK1, MSK2, MSK3, MSK4}
+ #define ALTI_MSK64 {MSK2, MSK1, MSK4, MSK3}
+ #define ALTI_SL2_PERM {3,21,21,21,7,0,1,2,11,4,5,6,15,8,9,10}
+ #define ALTI_SL2_PERM64 {3,4,5,6,7,29,29,29,11,12,13,14,15,0,1,2}
+ #define ALTI_SR2_PERM {7,0,1,2,11,4,5,6,15,8,9,10,17,12,13,14}
+ #define ALTI_SR2_PERM64 {15,0,1,2,3,4,5,6,17,8,9,10,11,12,13,14}
+#endif /* For OSX */
+#define IDSTR "SFMT-1279:7-14-3-5-1:f7fefffd-7fefcfff-aff3ef3f-b5ffff7f"
+
+#endif /* SFMT_PARAMS1279_H */
diff --git a/SimTKcommon/Random/src/SFMT-params132049.h b/SimTKcommon/Random/src/SFMT-params132049.h
new file mode 100644
index 0000000..94e297e
--- /dev/null
+++ b/SimTKcommon/Random/src/SFMT-params132049.h
@@ -0,0 +1,46 @@
+#ifndef SFMT_PARAMS132049_H
+#define SFMT_PARAMS132049_H
+
+#define POS1 110
+#define SL1 19
+#define SL2 1
+#define SR1 21
+#define SR2 1
+#define MSK1 0xffffbb5fU
+#define MSK2 0xfb6ebf95U
+#define MSK3 0xfffefffaU
+#define MSK4 0xcff77fffU
+#define PARITY1 0x00000001U
+#define PARITY2 0x00000000U
+#define PARITY3 0xcb520000U
+#define PARITY4 0xc7e91c7dU
+
+
+/* PARAMETERS FOR ALTIVEC */
+#if defined(__APPLE__) /* For OSX */
+ #define ALTI_SL1 (vector unsigned int)(SL1, SL1, SL1, SL1)
+ #define ALTI_SR1 (vector unsigned int)(SR1, SR1, SR1, SR1)
+ #define ALTI_MSK (vector unsigned int)(MSK1, MSK2, MSK3, MSK4)
+ #define ALTI_MSK64 \
+ (vector unsigned int)(MSK2, MSK1, MSK4, MSK3)
+ #define ALTI_SL2_PERM \
+ (vector unsigned char)(1,2,3,23,5,6,7,0,9,10,11,4,13,14,15,8)
+ #define ALTI_SL2_PERM64 \
+ (vector unsigned char)(1,2,3,4,5,6,7,31,9,10,11,12,13,14,15,0)
+ #define ALTI_SR2_PERM \
+ (vector unsigned char)(7,0,1,2,11,4,5,6,15,8,9,10,17,12,13,14)
+ #define ALTI_SR2_PERM64 \
+ (vector unsigned char)(15,0,1,2,3,4,5,6,17,8,9,10,11,12,13,14)
+#else /* For OTHER OSs(Linux?) */
+ #define ALTI_SL1 {SL1, SL1, SL1, SL1}
+ #define ALTI_SR1 {SR1, SR1, SR1, SR1}
+ #define ALTI_MSK {MSK1, MSK2, MSK3, MSK4}
+ #define ALTI_MSK64 {MSK2, MSK1, MSK4, MSK3}
+ #define ALTI_SL2_PERM {1,2,3,23,5,6,7,0,9,10,11,4,13,14,15,8}
+ #define ALTI_SL2_PERM64 {1,2,3,4,5,6,7,31,9,10,11,12,13,14,15,0}
+ #define ALTI_SR2_PERM {7,0,1,2,11,4,5,6,15,8,9,10,17,12,13,14}
+ #define ALTI_SR2_PERM64 {15,0,1,2,3,4,5,6,17,8,9,10,11,12,13,14}
+#endif /* For OSX */
+#define IDSTR "SFMT-132049:110-19-1-21-1:ffffbb5f-fb6ebf95-fffefffa-cff77fff"
+
+#endif /* SFMT_PARAMS132049_H */
diff --git a/SimTKcommon/Random/src/SFMT-params19937.h b/SimTKcommon/Random/src/SFMT-params19937.h
new file mode 100644
index 0000000..04708cd
--- /dev/null
+++ b/SimTKcommon/Random/src/SFMT-params19937.h
@@ -0,0 +1,46 @@
+#ifndef SFMT_PARAMS19937_H
+#define SFMT_PARAMS19937_H
+
+#define POS1 122
+#define SL1 18
+#define SL2 1
+#define SR1 11
+#define SR2 1
+#define MSK1 0xdfffffefU
+#define MSK2 0xddfecb7fU
+#define MSK3 0xbffaffffU
+#define MSK4 0xbffffff6U
+#define PARITY1 0x00000001U
+#define PARITY2 0x00000000U
+#define PARITY3 0x00000000U
+#define PARITY4 0x13c9e684U
+
+
+/* PARAMETERS FOR ALTIVEC */
+#if defined(__APPLE__) /* For OSX */
+ #define ALTI_SL1 (vector unsigned int)(SL1, SL1, SL1, SL1)
+ #define ALTI_SR1 (vector unsigned int)(SR1, SR1, SR1, SR1)
+ #define ALTI_MSK (vector unsigned int)(MSK1, MSK2, MSK3, MSK4)
+ #define ALTI_MSK64 \
+ (vector unsigned int)(MSK2, MSK1, MSK4, MSK3)
+ #define ALTI_SL2_PERM \
+ (vector unsigned char)(1,2,3,23,5,6,7,0,9,10,11,4,13,14,15,8)
+ #define ALTI_SL2_PERM64 \
+ (vector unsigned char)(1,2,3,4,5,6,7,31,9,10,11,12,13,14,15,0)
+ #define ALTI_SR2_PERM \
+ (vector unsigned char)(7,0,1,2,11,4,5,6,15,8,9,10,17,12,13,14)
+ #define ALTI_SR2_PERM64 \
+ (vector unsigned char)(15,0,1,2,3,4,5,6,17,8,9,10,11,12,13,14)
+#else /* For OTHER OSs(Linux?) */
+ #define ALTI_SL1 {SL1, SL1, SL1, SL1}
+ #define ALTI_SR1 {SR1, SR1, SR1, SR1}
+ #define ALTI_MSK {MSK1, MSK2, MSK3, MSK4}
+ #define ALTI_MSK64 {MSK2, MSK1, MSK4, MSK3}
+ #define ALTI_SL2_PERM {1,2,3,23,5,6,7,0,9,10,11,4,13,14,15,8}
+ #define ALTI_SL2_PERM64 {1,2,3,4,5,6,7,31,9,10,11,12,13,14,15,0}
+ #define ALTI_SR2_PERM {7,0,1,2,11,4,5,6,15,8,9,10,17,12,13,14}
+ #define ALTI_SR2_PERM64 {15,0,1,2,3,4,5,6,17,8,9,10,11,12,13,14}
+#endif /* For OSX */
+#define IDSTR "SFMT-19937:122-18-1-11-1:dfffffef-ddfecb7f-bffaffff-bffffff6"
+
+#endif /* SFMT_PARAMS19937_H */
diff --git a/SimTKcommon/Random/src/SFMT-params216091.h b/SimTKcommon/Random/src/SFMT-params216091.h
new file mode 100644
index 0000000..46c7303
--- /dev/null
+++ b/SimTKcommon/Random/src/SFMT-params216091.h
@@ -0,0 +1,46 @@
+#ifndef SFMT_PARAMS216091_H
+#define SFMT_PARAMS216091_H
+
+#define POS1 627
+#define SL1 11
+#define SL2 3
+#define SR1 10
+#define SR2 1
+#define MSK1 0xbff7bff7U
+#define MSK2 0xbfffffffU
+#define MSK3 0xbffffa7fU
+#define MSK4 0xffddfbfbU
+#define PARITY1 0xf8000001U
+#define PARITY2 0x89e80709U
+#define PARITY3 0x3bd2b64bU
+#define PARITY4 0x0c64b1e4U
+
+
+/* PARAMETERS FOR ALTIVEC */
+#if defined(__APPLE__) /* For OSX */
+ #define ALTI_SL1 (vector unsigned int)(SL1, SL1, SL1, SL1)
+ #define ALTI_SR1 (vector unsigned int)(SR1, SR1, SR1, SR1)
+ #define ALTI_MSK (vector unsigned int)(MSK1, MSK2, MSK3, MSK4)
+ #define ALTI_MSK64 \
+ (vector unsigned int)(MSK2, MSK1, MSK4, MSK3)
+ #define ALTI_SL2_PERM \
+ (vector unsigned char)(3,21,21,21,7,0,1,2,11,4,5,6,15,8,9,10)
+ #define ALTI_SL2_PERM64 \
+ (vector unsigned char)(3,4,5,6,7,29,29,29,11,12,13,14,15,0,1,2)
+ #define ALTI_SR2_PERM \
+ (vector unsigned char)(7,0,1,2,11,4,5,6,15,8,9,10,17,12,13,14)
+ #define ALTI_SR2_PERM64 \
+ (vector unsigned char)(15,0,1,2,3,4,5,6,17,8,9,10,11,12,13,14)
+#else /* For OTHER OSs(Linux?) */
+ #define ALTI_SL1 {SL1, SL1, SL1, SL1}
+ #define ALTI_SR1 {SR1, SR1, SR1, SR1}
+ #define ALTI_MSK {MSK1, MSK2, MSK3, MSK4}
+ #define ALTI_MSK64 {MSK2, MSK1, MSK4, MSK3}
+ #define ALTI_SL2_PERM {3,21,21,21,7,0,1,2,11,4,5,6,15,8,9,10}
+ #define ALTI_SL2_PERM64 {3,4,5,6,7,29,29,29,11,12,13,14,15,0,1,2}
+ #define ALTI_SR2_PERM {7,0,1,2,11,4,5,6,15,8,9,10,17,12,13,14}
+ #define ALTI_SR2_PERM64 {15,0,1,2,3,4,5,6,17,8,9,10,11,12,13,14}
+#endif /* For OSX */
+#define IDSTR "SFMT-216091:627-11-3-10-1:bff7bff7-bfffffff-bffffa7f-ffddfbfb"
+
+#endif /* SFMT_PARAMS216091_H */
diff --git a/SimTKcommon/Random/src/SFMT-params2281.h b/SimTKcommon/Random/src/SFMT-params2281.h
new file mode 100644
index 0000000..ee2ebdf
--- /dev/null
+++ b/SimTKcommon/Random/src/SFMT-params2281.h
@@ -0,0 +1,46 @@
+#ifndef SFMT_PARAMS2281_H
+#define SFMT_PARAMS2281_H
+
+#define POS1 12
+#define SL1 19
+#define SL2 1
+#define SR1 5
+#define SR2 1
+#define MSK1 0xbff7ffbfU
+#define MSK2 0xfdfffffeU
+#define MSK3 0xf7ffef7fU
+#define MSK4 0xf2f7cbbfU
+#define PARITY1 0x00000001U
+#define PARITY2 0x00000000U
+#define PARITY3 0x00000000U
+#define PARITY4 0x41dfa600U
+
+
+/* PARAMETERS FOR ALTIVEC */
+#if defined(__APPLE__) /* For OSX */
+ #define ALTI_SL1 (vector unsigned int)(SL1, SL1, SL1, SL1)
+ #define ALTI_SR1 (vector unsigned int)(SR1, SR1, SR1, SR1)
+ #define ALTI_MSK (vector unsigned int)(MSK1, MSK2, MSK3, MSK4)
+ #define ALTI_MSK64 \
+ (vector unsigned int)(MSK2, MSK1, MSK4, MSK3)
+ #define ALTI_SL2_PERM \
+ (vector unsigned char)(1,2,3,23,5,6,7,0,9,10,11,4,13,14,15,8)
+ #define ALTI_SL2_PERM64 \
+ (vector unsigned char)(1,2,3,4,5,6,7,31,9,10,11,12,13,14,15,0)
+ #define ALTI_SR2_PERM \
+ (vector unsigned char)(7,0,1,2,11,4,5,6,15,8,9,10,17,12,13,14)
+ #define ALTI_SR2_PERM64 \
+ (vector unsigned char)(15,0,1,2,3,4,5,6,17,8,9,10,11,12,13,14)
+#else /* For OTHER OSs(Linux?) */
+ #define ALTI_SL1 {SL1, SL1, SL1, SL1}
+ #define ALTI_SR1 {SR1, SR1, SR1, SR1}
+ #define ALTI_MSK {MSK1, MSK2, MSK3, MSK4}
+ #define ALTI_MSK64 {MSK2, MSK1, MSK4, MSK3}
+ #define ALTI_SL2_PERM {1,2,3,23,5,6,7,0,9,10,11,4,13,14,15,8}
+ #define ALTI_SL2_PERM64 {1,2,3,4,5,6,7,31,9,10,11,12,13,14,15,0}
+ #define ALTI_SR2_PERM {7,0,1,2,11,4,5,6,15,8,9,10,17,12,13,14}
+ #define ALTI_SR2_PERM64 {15,0,1,2,3,4,5,6,17,8,9,10,11,12,13,14}
+#endif /* For OSX */
+#define IDSTR "SFMT-2281:12-19-1-5-1:bff7ffbf-fdfffffe-f7ffef7f-f2f7cbbf"
+
+#endif /* SFMT_PARAMS2281_H */
diff --git a/SimTKcommon/Random/src/SFMT-params4253.h b/SimTKcommon/Random/src/SFMT-params4253.h
new file mode 100644
index 0000000..f391a70
--- /dev/null
+++ b/SimTKcommon/Random/src/SFMT-params4253.h
@@ -0,0 +1,46 @@
+#ifndef SFMT_PARAMS4253_H
+#define SFMT_PARAMS4253_H
+
+#define POS1 17
+#define SL1 20
+#define SL2 1
+#define SR1 7
+#define SR2 1
+#define MSK1 0x9f7bffffU
+#define MSK2 0x9fffff5fU
+#define MSK3 0x3efffffbU
+#define MSK4 0xfffff7bbU
+#define PARITY1 0xa8000001U
+#define PARITY2 0xaf5390a3U
+#define PARITY3 0xb740b3f8U
+#define PARITY4 0x6c11486dU
+
+
+/* PARAMETERS FOR ALTIVEC */
+#if defined(__APPLE__) /* For OSX */
+ #define ALTI_SL1 (vector unsigned int)(SL1, SL1, SL1, SL1)
+ #define ALTI_SR1 (vector unsigned int)(SR1, SR1, SR1, SR1)
+ #define ALTI_MSK (vector unsigned int)(MSK1, MSK2, MSK3, MSK4)
+ #define ALTI_MSK64 \
+ (vector unsigned int)(MSK2, MSK1, MSK4, MSK3)
+ #define ALTI_SL2_PERM \
+ (vector unsigned char)(1,2,3,23,5,6,7,0,9,10,11,4,13,14,15,8)
+ #define ALTI_SL2_PERM64 \
+ (vector unsigned char)(1,2,3,4,5,6,7,31,9,10,11,12,13,14,15,0)
+ #define ALTI_SR2_PERM \
+ (vector unsigned char)(7,0,1,2,11,4,5,6,15,8,9,10,17,12,13,14)
+ #define ALTI_SR2_PERM64 \
+ (vector unsigned char)(15,0,1,2,3,4,5,6,17,8,9,10,11,12,13,14)
+#else /* For OTHER OSs(Linux?) */
+ #define ALTI_SL1 {SL1, SL1, SL1, SL1}
+ #define ALTI_SR1 {SR1, SR1, SR1, SR1}
+ #define ALTI_MSK {MSK1, MSK2, MSK3, MSK4}
+ #define ALTI_MSK64 {MSK2, MSK1, MSK4, MSK3}
+ #define ALTI_SL2_PERM {1,2,3,23,5,6,7,0,9,10,11,4,13,14,15,8}
+ #define ALTI_SL2_PERM64 {1,2,3,4,5,6,7,31,9,10,11,12,13,14,15,0}
+ #define ALTI_SR2_PERM {7,0,1,2,11,4,5,6,15,8,9,10,17,12,13,14}
+ #define ALTI_SR2_PERM64 {15,0,1,2,3,4,5,6,17,8,9,10,11,12,13,14}
+#endif /* For OSX */
+#define IDSTR "SFMT-4253:17-20-1-7-1:9f7bffff-9fffff5f-3efffffb-fffff7bb"
+
+#endif /* SFMT_PARAMS4253_H */
diff --git a/SimTKcommon/Random/src/SFMT-params44497.h b/SimTKcommon/Random/src/SFMT-params44497.h
new file mode 100644
index 0000000..ddef00d
--- /dev/null
+++ b/SimTKcommon/Random/src/SFMT-params44497.h
@@ -0,0 +1,46 @@
+#ifndef SFMT_PARAMS44497_H
+#define SFMT_PARAMS44497_H
+
+#define POS1 330
+#define SL1 5
+#define SL2 3
+#define SR1 9
+#define SR2 3
+#define MSK1 0xeffffffbU
+#define MSK2 0xdfbebfffU
+#define MSK3 0xbfbf7befU
+#define MSK4 0x9ffd7bffU
+#define PARITY1 0x00000001U
+#define PARITY2 0x00000000U
+#define PARITY3 0xa3ac4000U
+#define PARITY4 0xecc1327aU
+
+
+/* PARAMETERS FOR ALTIVEC */
+#if defined(__APPLE__) /* For OSX */
+ #define ALTI_SL1 (vector unsigned int)(SL1, SL1, SL1, SL1)
+ #define ALTI_SR1 (vector unsigned int)(SR1, SR1, SR1, SR1)
+ #define ALTI_MSK (vector unsigned int)(MSK1, MSK2, MSK3, MSK4)
+ #define ALTI_MSK64 \
+ (vector unsigned int)(MSK2, MSK1, MSK4, MSK3)
+ #define ALTI_SL2_PERM \
+ (vector unsigned char)(3,21,21,21,7,0,1,2,11,4,5,6,15,8,9,10)
+ #define ALTI_SL2_PERM64 \
+ (vector unsigned char)(3,4,5,6,7,29,29,29,11,12,13,14,15,0,1,2)
+ #define ALTI_SR2_PERM \
+ (vector unsigned char)(5,6,7,0,9,10,11,4,13,14,15,8,19,19,19,12)
+ #define ALTI_SR2_PERM64 \
+ (vector unsigned char)(13,14,15,0,1,2,3,4,19,19,19,8,9,10,11,12)
+#else /* For OTHER OSs(Linux?) */
+ #define ALTI_SL1 {SL1, SL1, SL1, SL1}
+ #define ALTI_SR1 {SR1, SR1, SR1, SR1}
+ #define ALTI_MSK {MSK1, MSK2, MSK3, MSK4}
+ #define ALTI_MSK64 {MSK2, MSK1, MSK4, MSK3}
+ #define ALTI_SL2_PERM {3,21,21,21,7,0,1,2,11,4,5,6,15,8,9,10}
+ #define ALTI_SL2_PERM64 {3,4,5,6,7,29,29,29,11,12,13,14,15,0,1,2}
+ #define ALTI_SR2_PERM {5,6,7,0,9,10,11,4,13,14,15,8,19,19,19,12}
+ #define ALTI_SR2_PERM64 {13,14,15,0,1,2,3,4,19,19,19,8,9,10,11,12}
+#endif /* For OSX */
+#define IDSTR "SFMT-44497:330-5-3-9-3:effffffb-dfbebfff-bfbf7bef-9ffd7bff"
+
+#endif /* SFMT_PARAMS44497_H */
diff --git a/SimTKcommon/Random/src/SFMT-params607.h b/SimTKcommon/Random/src/SFMT-params607.h
new file mode 100644
index 0000000..fc2be6d
--- /dev/null
+++ b/SimTKcommon/Random/src/SFMT-params607.h
@@ -0,0 +1,46 @@
+#ifndef SFMT_PARAMS607_H
+#define SFMT_PARAMS607_H
+
+#define POS1 2
+#define SL1 15
+#define SL2 3
+#define SR1 13
+#define SR2 3
+#define MSK1 0xfdff37ffU
+#define MSK2 0xef7f3f7dU
+#define MSK3 0xff777b7dU
+#define MSK4 0x7ff7fb2fU
+#define PARITY1 0x00000001U
+#define PARITY2 0x00000000U
+#define PARITY3 0x00000000U
+#define PARITY4 0x5986f054U
+
+
+/* PARAMETERS FOR ALTIVEC */
+#if defined(__APPLE__) /* For OSX */
+ #define ALTI_SL1 (vector unsigned int)(SL1, SL1, SL1, SL1)
+ #define ALTI_SR1 (vector unsigned int)(SR1, SR1, SR1, SR1)
+ #define ALTI_MSK (vector unsigned int)(MSK1, MSK2, MSK3, MSK4)
+ #define ALTI_MSK64 \
+ (vector unsigned int)(MSK2, MSK1, MSK4, MSK3)
+ #define ALTI_SL2_PERM \
+ (vector unsigned char)(3,21,21,21,7,0,1,2,11,4,5,6,15,8,9,10)
+ #define ALTI_SL2_PERM64 \
+ (vector unsigned char)(3,4,5,6,7,29,29,29,11,12,13,14,15,0,1,2)
+ #define ALTI_SR2_PERM \
+ (vector unsigned char)(5,6,7,0,9,10,11,4,13,14,15,8,19,19,19,12)
+ #define ALTI_SR2_PERM64 \
+ (vector unsigned char)(13,14,15,0,1,2,3,4,19,19,19,8,9,10,11,12)
+#else /* For OTHER OSs(Linux?) */
+ #define ALTI_SL1 {SL1, SL1, SL1, SL1}
+ #define ALTI_SR1 {SR1, SR1, SR1, SR1}
+ #define ALTI_MSK {MSK1, MSK2, MSK3, MSK4}
+ #define ALTI_MSK64 {MSK2, MSK1, MSK4, MSK3}
+ #define ALTI_SL2_PERM {3,21,21,21,7,0,1,2,11,4,5,6,15,8,9,10}
+ #define ALTI_SL2_PERM64 {3,4,5,6,7,29,29,29,11,12,13,14,15,0,1,2}
+ #define ALTI_SR2_PERM {5,6,7,0,9,10,11,4,13,14,15,8,19,19,19,12}
+ #define ALTI_SR2_PERM64 {13,14,15,0,1,2,3,4,19,19,19,8,9,10,11,12}
+#endif /* For OSX */
+#define IDSTR "SFMT-607:2-15-3-13-3:fdff37ff-ef7f3f7d-ff777b7d-7ff7fb2f"
+
+#endif /* SFMT_PARAMS607_H */
diff --git a/SimTKcommon/Random/src/SFMT-params86243.h b/SimTKcommon/Random/src/SFMT-params86243.h
new file mode 100644
index 0000000..3b52b76
--- /dev/null
+++ b/SimTKcommon/Random/src/SFMT-params86243.h
@@ -0,0 +1,46 @@
+#ifndef SFMT_PARAMS86243_H
+#define SFMT_PARAMS86243_H
+
+#define POS1 366
+#define SL1 6
+#define SL2 7
+#define SR1 19
+#define SR2 1
+#define MSK1 0xfdbffbffU
+#define MSK2 0xbff7ff3fU
+#define MSK3 0xfd77efffU
+#define MSK4 0xbf9ff3ffU
+#define PARITY1 0x00000001U
+#define PARITY2 0x00000000U
+#define PARITY3 0x00000000U
+#define PARITY4 0xe9528d85U
+
+
+/* PARAMETERS FOR ALTIVEC */
+#if defined(__APPLE__) /* For OSX */
+ #define ALTI_SL1 (vector unsigned int)(SL1, SL1, SL1, SL1)
+ #define ALTI_SR1 (vector unsigned int)(SR1, SR1, SR1, SR1)
+ #define ALTI_MSK (vector unsigned int)(MSK1, MSK2, MSK3, MSK4)
+ #define ALTI_MSK64 \
+ (vector unsigned int)(MSK2, MSK1, MSK4, MSK3)
+ #define ALTI_SL2_PERM \
+ (vector unsigned char)(25,25,25,25,3,25,25,25,7,0,1,2,11,4,5,6)
+ #define ALTI_SL2_PERM64 \
+ (vector unsigned char)(7,25,25,25,25,25,25,25,15,0,1,2,3,4,5,6)
+ #define ALTI_SR2_PERM \
+ (vector unsigned char)(7,0,1,2,11,4,5,6,15,8,9,10,17,12,13,14)
+ #define ALTI_SR2_PERM64 \
+ (vector unsigned char)(15,0,1,2,3,4,5,6,17,8,9,10,11,12,13,14)
+#else /* For OTHER OSs(Linux?) */
+ #define ALTI_SL1 {SL1, SL1, SL1, SL1}
+ #define ALTI_SR1 {SR1, SR1, SR1, SR1}
+ #define ALTI_MSK {MSK1, MSK2, MSK3, MSK4}
+ #define ALTI_MSK64 {MSK2, MSK1, MSK4, MSK3}
+ #define ALTI_SL2_PERM {25,25,25,25,3,25,25,25,7,0,1,2,11,4,5,6}
+ #define ALTI_SL2_PERM64 {7,25,25,25,25,25,25,25,15,0,1,2,3,4,5,6}
+ #define ALTI_SR2_PERM {7,0,1,2,11,4,5,6,15,8,9,10,17,12,13,14}
+ #define ALTI_SR2_PERM64 {15,0,1,2,3,4,5,6,17,8,9,10,11,12,13,14}
+#endif /* For OSX */
+#define IDSTR "SFMT-86243:366-6-7-19-1:fdbffbff-bff7ff3f-fd77efff-bf9ff3ff"
+
+#endif /* SFMT_PARAMS86243_H */
diff --git a/SimTKcommon/Random/src/SFMT-sse2.h b/SimTKcommon/Random/src/SFMT-sse2.h
new file mode 100644
index 0000000..4e91d9c
--- /dev/null
+++ b/SimTKcommon/Random/src/SFMT-sse2.h
@@ -0,0 +1,121 @@
+/**
+ * @file SFMT-sse2.h
+ * @brief SIMD oriented Fast Mersenne Twister(SFMT) for Intel SSE2
+ *
+ * @author Mutsuo Saito (Hiroshima University)
+ * @author Makoto Matsumoto (Hiroshima University)
+ *
+ * @note We assume LITTLE ENDIAN in this file
+ *
+ * Copyright (C) 2006, 2007 Mutsuo Saito, Makoto Matsumoto and Hiroshima
+ * University. All rights reserved.
+ *
+ * The new BSD License is applied to this software, see LICENSE.txt
+ */
+
+#ifndef SFMT_SSE2_H
+#define SFMT_SSE2_H
+
+PRE_ALWAYS static __m128i mm_recursion(__m128i *a, __m128i *b, __m128i c,
+ __m128i d, __m128i mask) ALWAYSINLINE;
+
+/**
+ * This function represents the recursion formula.
+ * @param a a 128-bit part of the interal state array
+ * @param b a 128-bit part of the interal state array
+ * @param c a 128-bit part of the interal state array
+ * @param d a 128-bit part of the interal state array
+ * @param mask 128-bit mask
+ * @return output
+ */
+PRE_ALWAYS static __m128i mm_recursion(__m128i *a, __m128i *b,
+ __m128i c, __m128i d, __m128i mask) {
+ __m128i v, x, y, z;
+
+ x = _mm_load_si128(a);
+ y = _mm_srli_epi32(*b, SR1);
+ z = _mm_srli_si128(c, SR2);
+ v = _mm_slli_epi32(d, SL1);
+ z = _mm_xor_si128(z, x);
+ z = _mm_xor_si128(z, v);
+ x = _mm_slli_si128(x, SL2);
+ y = _mm_and_si128(y, mask);
+ z = _mm_xor_si128(z, x);
+ z = _mm_xor_si128(z, y);
+ return z;
+}
+
+/**
+ * This function fills the internal state array with pseudorandom
+ * integers.
+ */
+inline static void gen_rand_all(void) {
+ int i;
+ __m128i r, r1, r2, mask;
+ mask = _mm_set_epi32(MSK4, MSK3, MSK2, MSK1);
+
+ r1 = _mm_load_si128(&sfmt[N - 2].si);
+ r2 = _mm_load_si128(&sfmt[N - 1].si);
+ for (i = 0; i < N - POS1; i++) {
+ r = mm_recursion(&sfmt[i].si, &sfmt[i + POS1].si, r1, r2, mask);
+ _mm_store_si128(&sfmt[i].si, r);
+ r1 = r2;
+ r2 = r;
+ }
+ for (; i < N; i++) {
+ r = mm_recursion(&sfmt[i].si, &sfmt[i + POS1 - N].si, r1, r2, mask);
+ _mm_store_si128(&sfmt[i].si, r);
+ r1 = r2;
+ r2 = r;
+ }
+}
+
+/**
+ * This function fills the user-specified array with pseudorandom
+ * integers.
+ *
+ * @param array an 128-bit array to be filled by pseudorandom numbers.
+ * @param size number of 128-bit pesudorandom numbers to be generated.
+ */
+inline static void gen_rand_array(w128_t *array, int size) {
+ int i, j;
+ __m128i r, r1, r2, mask;
+ mask = _mm_set_epi32(MSK4, MSK3, MSK2, MSK1);
+
+ r1 = _mm_load_si128(&sfmt[N - 2].si);
+ r2 = _mm_load_si128(&sfmt[N - 1].si);
+ for (i = 0; i < N - POS1; i++) {
+ r = mm_recursion(&sfmt[i].si, &sfmt[i + POS1].si, r1, r2, mask);
+ _mm_store_si128(&array[i].si, r);
+ r1 = r2;
+ r2 = r;
+ }
+ for (; i < N; i++) {
+ r = mm_recursion(&sfmt[i].si, &array[i + POS1 - N].si, r1, r2, mask);
+ _mm_store_si128(&array[i].si, r);
+ r1 = r2;
+ r2 = r;
+ }
+ /* main loop */
+ for (; i < size - N; i++) {
+ r = mm_recursion(&array[i - N].si, &array[i + POS1 - N].si, r1, r2,
+ mask);
+ _mm_store_si128(&array[i].si, r);
+ r1 = r2;
+ r2 = r;
+ }
+ for (j = 0; j < 2 * N - size; j++) {
+ r = _mm_load_si128(&array[j + size - N].si);
+ _mm_store_si128(&sfmt[j].si, r);
+ }
+ for (; i < size; i++) {
+ r = mm_recursion(&array[i - N].si, &array[i + POS1 - N].si, r1, r2,
+ mask);
+ _mm_store_si128(&array[i].si, r);
+ _mm_store_si128(&sfmt[j++].si, r);
+ r1 = r2;
+ r2 = r;
+ }
+}
+
+#endif
diff --git a/SimTKcommon/Random/src/SFMT.cpp b/SimTKcommon/Random/src/SFMT.cpp
new file mode 100644
index 0000000..9fc314c
--- /dev/null
+++ b/SimTKcommon/Random/src/SFMT.cpp
@@ -0,0 +1,669 @@
+/**
+ * @file SFMT.c
+ * @brief SIMD oriented Fast Mersenne Twister(SFMT)
+ *
+ * @author Mutsuo Saito (Hiroshima University)
+ * @author Makoto Matsumoto (Hiroshima University)
+ *
+ * Copyright (C) 2006,2007 Mutsuo Saito, Makoto Matsumoto and Hiroshima
+ * University. All rights reserved.
+ *
+ * The new BSD License is applied to this software, see LICENSE.txt
+ */
+#include "SFMT.h"
+#include "SFMT-params.h"
+
+#include <cstring>
+#include <cassert>
+
+#if defined(__BIG_ENDIAN__) && !defined(__amd64) && !defined(BIG_ENDIAN64)
+#define BIG_ENDIAN64 1
+#endif
+#if defined(HAVE_ALTIVEC) && !defined(BIG_ENDIAN64)
+#define BIG_ENDIAN64 1
+#endif
+#if defined(ONLY64) && !defined(BIG_ENDIAN64)
+ #if defined(__GNUC__)
+ #error "-DONLY64 must be specified with -DBIG_ENDIAN64"
+ #endif
+#undef ONLY64
+#endif
+/*------------------------------------------------------
+ 128-bit SIMD data type for Altivec, SSE2 or standard C
+ ------------------------------------------------------*/
+#if defined(HAVE_ALTIVEC)
+ #if !defined(__APPLE__)
+ #include <altivec.h>
+ #endif
+/** 128-bit data structure */
+union W128_T {
+ vector unsigned int s;
+ uint32_t u[4];
+};
+/** 128-bit data type */
+typedef union W128_T w128_t;
+
+#elif defined(HAVE_SSE2)
+ #include <emmintrin.h>
+
+/** 128-bit data structure */
+union W128_T {
+ __m128i si;
+ uint32_t u[4];
+};
+/** 128-bit data type */
+typedef union W128_T w128_t;
+
+#else
+
+/** 128-bit data structure */
+struct W128_T {
+ uint32_t u[4];
+};
+/** 128-bit data type */
+typedef struct W128_T w128_t;
+
+#endif
+
+/*--------------------------------------
+ FILE GLOBAL VARIABLES
+ internal state, index counter and flag
+ --------------------------------------*/
+///** the 128-bit internal state array */
+//static w128_t sfmt[N];
+///** the 32bit integer pointer to the 128-bit internal state array */
+//static uint32_t *psfmt32 = &sfmt[0].u[0];
+//#if !defined(BIG_ENDIAN64) || defined(ONLY64)
+///** the 64bit integer pointer to the 128-bit internal state array */
+//static uint64_t *psfmt64 = (uint64_t *)&sfmt[0].u[0];
+//#endif
+///** index counter to the 32-bit internal state array */
+//static int idx;
+///** a flag: it is 0 if and only if the internal state is not yet
+// * initialized. */
+//static int initialized = 0;
+///** a parity check vector which certificate the period of 2^{MEXP} */
+//static uint32_t parity[4] = {PARITY1, PARITY2, PARITY3, PARITY4};
+
+namespace SimTK_SFMT {
+
+class SFMTData {
+public:
+ /** the 128-bit internal state array */
+ w128_t sfmt[N];
+ /** the 32bit integer pointer to the 128-bit internal state array */
+ uint32_t *psfmt32;
+ #if !defined(BIG_ENDIAN64) || defined(ONLY64)
+ /** the 64bit integer pointer to the 128-bit internal state array */
+ uint64_t *psfmt64;
+ #endif
+ /** index counter to the 32-bit internal state array */
+ int idx;
+ /** a flag: it is 0 if and only if the internal state is not yet
+ * initialized. */
+ int initialized;
+ /** a parity check vector which certificate the period of 2^{MEXP} */
+ uint32_t parity[4];
+ SFMTData() {
+ psfmt32 = &sfmt[0].u[0];
+#if !defined(BIG_ENDIAN64) || defined(ONLY64)
+ psfmt64 = (uint64_t *)&sfmt[0].u[0];
+#endif
+ initialized = 0;
+ parity[0] = PARITY1;
+ parity[1] = PARITY2;
+ parity[2] = PARITY3;
+ parity[3] = PARITY4;
+ }
+};
+
+/*----------------
+ STATIC FUNCTIONS
+ ----------------*/
+inline static int idxof(int i);
+inline static void rshift128(w128_t *out, w128_t const *in, int shift);
+inline static void lshift128(w128_t *out, w128_t const *in, int shift);
+inline static void gen_rand_all(SFMTData& data);
+inline static void gen_rand_array(w128_t *array, int size, SFMTData& data);
+inline static uint32_t func1(uint32_t x);
+inline static uint32_t func2(uint32_t x);
+static void period_certification(SFMTData& data);
+#if defined(BIG_ENDIAN64) && !defined(ONLY64)
+inline static void swap(w128_t *array, int size);
+#endif
+
+#if defined(HAVE_ALTIVEC)
+ #include "SFMT-alti.h"
+#elif defined(HAVE_SSE2)
+ #include "SFMT-sse2.h"
+#endif
+
+/**
+ * This function simulate a 64-bit index of LITTLE ENDIAN
+ * in BIG ENDIAN machine.
+ */
+#ifdef ONLY64
+inline static int idxof(int i) {
+ return i ^ 1;
+}
+#else
+inline static int idxof(int i) {
+ return i;
+}
+#endif
+/**
+ * This function simulates SIMD 128-bit right shift by the standard C.
+ * The 128-bit integer given in in is shifted by (shift * 8) bits.
+ * This function simulates the LITTLE ENDIAN SIMD.
+ * @param out the output of this function
+ * @param in the 128-bit data to be shifted
+ * @param shift the shift value
+ */
+#ifdef ONLY64
+inline static void rshift128(w128_t *out, w128_t const *in, int shift) {
+ uint64_t th, tl, oh, ol;
+
+ th = ((uint64_t)in->u[2] << 32) | ((uint64_t)in->u[3]);
+ tl = ((uint64_t)in->u[0] << 32) | ((uint64_t)in->u[1]);
+
+ oh = th >> (shift * 8);
+ ol = tl >> (shift * 8);
+ ol |= th << (64 - shift * 8);
+ out->u[0] = (uint32_t)(ol >> 32);
+ out->u[1] = (uint32_t)ol;
+ out->u[2] = (uint32_t)(oh >> 32);
+ out->u[3] = (uint32_t)oh;
+}
+#else
+inline static void rshift128(w128_t *out, w128_t const *in, int shift) {
+ uint64_t th, tl, oh, ol;
+
+ th = ((uint64_t)in->u[3] << 32) | ((uint64_t)in->u[2]);
+ tl = ((uint64_t)in->u[1] << 32) | ((uint64_t)in->u[0]);
+
+ oh = th >> (shift * 8);
+ ol = tl >> (shift * 8);
+ ol |= th << (64 - shift * 8);
+ out->u[1] = (uint32_t)(ol >> 32);
+ out->u[0] = (uint32_t)ol;
+ out->u[3] = (uint32_t)(oh >> 32);
+ out->u[2] = (uint32_t)oh;
+}
+#endif
+/**
+ * This function simulates SIMD 128-bit left shift by the standard C.
+ * The 128-bit integer given in in is shifted by (shift * 8) bits.
+ * This function simulates the LITTLE ENDIAN SIMD.
+ * @param out the output of this function
+ * @param in the 128-bit data to be shifted
+ * @param shift the shift value
+ */
+#ifdef ONLY64
+inline static void lshift128(w128_t *out, w128_t const *in, int shift) {
+ uint64_t th, tl, oh, ol;
+
+ th = ((uint64_t)in->u[2] << 32) | ((uint64_t)in->u[3]);
+ tl = ((uint64_t)in->u[0] << 32) | ((uint64_t)in->u[1]);
+
+ oh = th << (shift * 8);
+ ol = tl << (shift * 8);
+ oh |= tl >> (64 - shift * 8);
+ out->u[0] = (uint32_t)(ol >> 32);
+ out->u[1] = (uint32_t)ol;
+ out->u[2] = (uint32_t)(oh >> 32);
+ out->u[3] = (uint32_t)oh;
+}
+#else
+inline static void lshift128(w128_t *out, w128_t const *in, int shift) {
+ uint64_t th, tl, oh, ol;
+
+ th = ((uint64_t)in->u[3] << 32) | ((uint64_t)in->u[2]);
+ tl = ((uint64_t)in->u[1] << 32) | ((uint64_t)in->u[0]);
+
+ oh = th << (shift * 8);
+ ol = tl << (shift * 8);
+ oh |= tl >> (64 - shift * 8);
+ out->u[1] = (uint32_t)(ol >> 32);
+ out->u[0] = (uint32_t)ol;
+ out->u[3] = (uint32_t)(oh >> 32);
+ out->u[2] = (uint32_t)oh;
+}
+#endif
+
+/**
+ * This function represents the recursion formula.
+ * @param r output
+ * @param a a 128-bit part of the internal state array
+ * @param b a 128-bit part of the internal state array
+ * @param c a 128-bit part of the internal state array
+ * @param d a 128-bit part of the internal state array
+ */
+#ifdef ONLY64
+inline static void do_recursion(w128_t *r, w128_t *a, w128_t *b, w128_t *c,
+ w128_t *d) {
+ w128_t x;
+ w128_t y;
+
+ lshift128(&x, a, SL2);
+ rshift128(&y, c, SR2);
+ r->u[0] = a->u[0] ^ x.u[0] ^ ((b->u[0] >> SR1) & MSK2) ^ y.u[0]
+ ^ (d->u[0] << SL1);
+ r->u[1] = a->u[1] ^ x.u[1] ^ ((b->u[1] >> SR1) & MSK1) ^ y.u[1]
+ ^ (d->u[1] << SL1);
+ r->u[2] = a->u[2] ^ x.u[2] ^ ((b->u[2] >> SR1) & MSK4) ^ y.u[2]
+ ^ (d->u[2] << SL1);
+ r->u[3] = a->u[3] ^ x.u[3] ^ ((b->u[3] >> SR1) & MSK3) ^ y.u[3]
+ ^ (d->u[3] << SL1);
+}
+#else
+inline static void do_recursion(w128_t *r, w128_t *a, w128_t *b, w128_t *c,
+ w128_t *d) {
+ w128_t x;
+ w128_t y;
+
+ lshift128(&x, a, SL2);
+ rshift128(&y, c, SR2);
+ r->u[0] = a->u[0] ^ x.u[0] ^ ((b->u[0] >> SR1) & MSK1) ^ y.u[0]
+ ^ (d->u[0] << SL1);
+ r->u[1] = a->u[1] ^ x.u[1] ^ ((b->u[1] >> SR1) & MSK2) ^ y.u[1]
+ ^ (d->u[1] << SL1);
+ r->u[2] = a->u[2] ^ x.u[2] ^ ((b->u[2] >> SR1) & MSK3) ^ y.u[2]
+ ^ (d->u[2] << SL1);
+ r->u[3] = a->u[3] ^ x.u[3] ^ ((b->u[3] >> SR1) & MSK4) ^ y.u[3]
+ ^ (d->u[3] << SL1);
+}
+#endif
+
+#if (!defined(HAVE_ALTIVEC)) && (!defined(HAVE_SSE2))
+/**
+ * This function fills the internal state array with pseudorandom
+ * integers.
+ */
+inline static void gen_rand_all(SFMTData& data) {
+ int i;
+ w128_t *r1, *r2;
+
+ r1 = &(data.sfmt[N - 2]);
+ r2 = &(data.sfmt[N - 1]);
+ for (i = 0; i < N - POS1; i++) {
+ do_recursion(&(data.sfmt[i]), &(data.sfmt[i]), &(data.sfmt[i + POS1]), r1, r2);
+ r1 = r2;
+ r2 = &(data.sfmt[i]);
+ }
+ for (; i < N; i++) {
+ do_recursion(&(data.sfmt[i]), &(data.sfmt[i]), &(data.sfmt[i + POS1 - N]), r1, r2);
+ r1 = r2;
+ r2 = &(data.sfmt[i]);
+ }
+}
+
+/**
+ * This function fills the user-specified array with pseudorandom
+ * integers.
+ *
+ * @param array an 128-bit array to be filled by pseudorandom numbers.
+ * @param size number of 128-bit pseudorandom numbers to be generated.
+ */
+inline static void gen_rand_array(w128_t *array, int size, SFMTData& data) {
+ int i, j;
+ w128_t *r1, *r2;
+
+ r1 = &(data.sfmt[N - 2]);
+ r2 = &(data.sfmt[N - 1]);
+ for (i = 0; i < N - POS1; i++) {
+ do_recursion(&array[i], &(data.sfmt[i]), &(data.sfmt[i + POS1]), r1, r2);
+ r1 = r2;
+ r2 = &array[i];
+ }
+ for (; i < N; i++) {
+ do_recursion(&array[i], &(data.sfmt[i]), &array[i + POS1 - N], r1, r2);
+ r1 = r2;
+ r2 = &array[i];
+ }
+ for (; i < size - N; i++) {
+ do_recursion(&array[i], &array[i - N], &array[i + POS1 - N], r1, r2);
+ r1 = r2;
+ r2 = &array[i];
+ }
+ for (j = 0; j < 2 * N - size; j++) {
+ data.sfmt[j] = array[j + size - N];
+ }
+ for (; i < size; i++, j++) {
+ do_recursion(&array[i], &array[i - N], &array[i + POS1 - N], r1, r2);
+ r1 = r2;
+ r2 = &array[i];
+ data.sfmt[j] = array[i];
+ }
+}
+#endif
+
+#if defined(BIG_ENDIAN64) && !defined(ONLY64) && !defined(HAVE_ALTIVEC)
+inline static void swap(w128_t *array, int size) {
+ int i;
+ uint32_t x, y;
+
+ for (i = 0; i < size; i++) {
+ x = array[i].u[0];
+ y = array[i].u[2];
+ array[i].u[0] = array[i].u[1];
+ array[i].u[2] = array[i].u[3];
+ array[i].u[1] = x;
+ array[i].u[3] = y;
+ }
+}
+#endif
+/**
+ * This function represents a function used in the initialization
+ * by init_by_array
+ * @param x 32-bit integer
+ * @return 32-bit integer
+ */
+static uint32_t func1(uint32_t x) {
+ return (x ^ (x >> 27)) * (uint32_t)1664525UL;
+}
+
+/**
+ * This function represents a function used in the initialization
+ * by init_by_array
+ * @param x 32-bit integer
+ * @return 32-bit integer
+ */
+static uint32_t func2(uint32_t x) {
+ return (x ^ (x >> 27)) * (uint32_t)1566083941UL;
+}
+
+/**
+ * This function certificate the period of 2^{MEXP}
+ */
+static void period_certification(SFMTData& data) {
+ int inner = 0;
+ int i, j;
+ uint32_t work;
+
+ for (i = 0; i < 4; i++)
+ inner ^= data.psfmt32[idxof(i)] & data.parity[i];
+ for (i = 16; i > 0; i >>= 1)
+ inner ^= inner >> i;
+ inner &= 1;
+ /* check OK */
+ if (inner == 1) {
+ return;
+ }
+ /* check NG, and modification */
+ for (i = 0; i < 4; i++) {
+ work = 1;
+ for (j = 0; j < 32; j++) {
+ if ((work & data.parity[i]) != 0) {
+ data.psfmt32[idxof(i)] ^= work;
+ return;
+ }
+ work = work << 1;
+ }
+ }
+}
+
+/*----------------
+ PUBLIC FUNCTIONS
+ ----------------*/
+/**
+ * This function returns the identification string.
+ * The string shows the word size, the Mersenne exponent,
+ * and all parameters of this generator.
+ */
+const char *get_idstring(void) {
+ return IDSTR;
+}
+
+/**
+ * This function returns the minimum size of array used for \b
+ * fill_array32() function.
+ * @return minimum size of array used for fill_array32() function.
+ */
+int get_min_array_size32(void) {
+ return N32;
+}
+
+/**
+ * This function returns the minimum size of array used for \b
+ * fill_array64() function.
+ * @return minimum size of array used for fill_array64() function.
+ */
+int get_min_array_size64(void) {
+ return N64;
+}
+
+#ifndef ONLY64
+/**
+ * This function generates and returns 32-bit pseudorandom number.
+ * init_gen_rand or init_by_array must be called before this function.
+ * @return 32-bit pseudorandom number
+ */
+uint32_t gen_rand32(SFMTData& data) {
+ uint32_t r;
+
+ assert(data.initialized);
+ if (data.idx >= N32) {
+ gen_rand_all(data);
+ data.idx = 0;
+ }
+ r = data.psfmt32[data.idx++];
+ return r;
+}
+#endif
+/**
+ * This function generates and returns 64-bit pseudorandom number.
+ * init_gen_rand or init_by_array must be called before this function.
+ * The function gen_rand64 should not be called after gen_rand32,
+ * unless an initialization is again executed.
+ * @return 64-bit pseudorandom number
+ */
+uint64_t gen_rand64(SFMTData& data) {
+#if defined(BIG_ENDIAN64) && !defined(ONLY64)
+ uint32_t r1, r2;
+#else
+ uint64_t r;
+#endif
+
+ assert(data.initialized);
+ assert(data.idx % 2 == 0);
+
+ if (data.idx >= N32) {
+ gen_rand_all(data);
+ data.idx = 0;
+ }
+#if defined(BIG_ENDIAN64) && !defined(ONLY64)
+ r1 = data.psfmt32[data.idx];
+ r2 = data.psfmt32[data.idx + 1];
+ data.idx += 2;
+ return ((uint64_t)r2 << 32) | r1;
+#else
+ r = data.psfmt64[data.idx / 2];
+ data.idx += 2;
+ return r;
+#endif
+}
+
+#ifndef ONLY64
+/**
+ * This function generates pseudorandom 32-bit integers in the
+ * specified array[] by one call. The number of pseudorandom integers
+ * is specified by the argument size, which must be at least 624 and a
+ * multiple of four. The generation by this function is much faster
+ * than the following gen_rand function.
+ *
+ * For initialization, init_gen_rand or init_by_array must be called
+ * before the first call of this function. This function can not be
+ * used after calling gen_rand function, without initialization.
+ *
+ * @param array an array where pseudorandom 32-bit integers are filled
+ * by this function. The pointer to the array must be \b "aligned"
+ * (namely, must be a multiple of 16) in the SIMD version, since it
+ * refers to the address of a 128-bit integer. In the standard C
+ * version, the pointer is arbitrary.
+ *
+ * @param size the number of 32-bit pseudorandom integers to be
+ * generated. size must be a multiple of 4, and greater than or equal
+ * to (MEXP / 128 + 1) * 4.
+ *
+ * @note \b memalign or \b posix_memalign is available to get aligned
+ * memory. Mac OSX doesn't have these functions, but \b malloc of OSX
+ * returns the pointer to the aligned memory block.
+ */
+void fill_array32(uint32_t *array, int size, SFMTData& data) {
+ assert(data.initialized);
+ assert(data.idx == N32);
+ assert(size % 4 == 0);
+ assert(size >= N32);
+
+ gen_rand_array((w128_t *)array, size / 4, data);
+ data.idx = N32;
+}
+#endif
+
+/**
+ * This function generates pseudorandom 64-bit integers in the
+ * specified array[] by one call. The number of pseudorandom integers
+ * is specified by the argument size, which must be at least 312 and a
+ * multiple of two. The generation by this function is much faster
+ * than the following gen_rand function.
+ *
+ * For initialization, init_gen_rand or init_by_array must be called
+ * before the first call of this function. This function can not be
+ * used after calling gen_rand function, without initialization.
+ *
+ * @param array an array where pseudorandom 64-bit integers are filled
+ * by this function. The pointer to the array must be "aligned"
+ * (namely, must be a multiple of 16) in the SIMD version, since it
+ * refers to the address of a 128-bit integer. In the standard C
+ * version, the pointer is arbitrary.
+ *
+ * @param size the number of 64-bit pseudorandom integers to be
+ * generated. size must be a multiple of 2, and greater than or equal
+ * to (MEXP / 128 + 1) * 2
+ *
+ * @note \b memalign or \b posix_memalign is available to get aligned
+ * memory. Mac OSX doesn't have these functions, but \b malloc of OSX
+ * returns the pointer to the aligned memory block.
+ */
+void fill_array64(uint64_t *array, int size, SFMTData& data) {
+ assert(data.initialized);
+ assert(data.idx == N32);
+ assert(size % 2 == 0);
+ assert(size >= N64);
+
+ gen_rand_array((w128_t *)array, size / 2, data);
+ data.idx = N32;
+
+#if defined(BIG_ENDIAN64) && !defined(ONLY64)
+ swap((w128_t *)array, size /2);
+#endif
+}
+
+/**
+ * This function initializes the internal state array with a 32-bit
+ * integer seed.
+ *
+ * @param seed a 32-bit integer used as the seed.
+ */
+void init_gen_rand(uint32_t seed, SFMTData& data) {
+ int i;
+
+ data.psfmt32[idxof(0)] = seed;
+ for (i = 1; i < N32; i++) {
+ data.psfmt32[idxof(i)] = 1812433253UL * (data.psfmt32[idxof(i - 1)]
+ ^ (data.psfmt32[idxof(i - 1)] >> 30))
+ + i;
+ }
+ data.idx = N32;
+ period_certification(data);
+ data.initialized = 1;
+}
+
+/**
+ * This function initializes the internal state array,
+ * with an array of 32-bit integers used as the seeds
+ * @param init_key the array of 32-bit integers, used as a seed.
+ * @param key_length the length of init_key.
+ */
+void init_by_array(uint32_t *init_key, int key_length, SFMTData& data) {
+ int i, j, count;
+ uint32_t r;
+ int lag;
+ int mid;
+ int size = N * 4;
+
+ if (size >= 623) {
+ lag = 11;
+ } else if (size >= 68) {
+ lag = 7;
+ } else if (size >= 39) {
+ lag = 5;
+ } else {
+ lag = 3;
+ }
+ mid = (size - lag) / 2;
+
+ memset(data.sfmt, 0x8b, sizeof(data.sfmt));
+ if (key_length + 1 > N32) {
+ count = key_length + 1;
+ } else {
+ count = N32;
+ }
+ r = func1(data.psfmt32[idxof(0)] ^ data.psfmt32[idxof(mid)]
+ ^ data.psfmt32[idxof(N32 - 1)]);
+ data.psfmt32[idxof(mid)] += r;
+ r += key_length;
+ data.psfmt32[idxof(mid + lag)] += r;
+ data.psfmt32[idxof(0)] = r;
+
+ count--;
+ for (i = 1, j = 0; (j < count) && (j < key_length); j++) {
+ r = func1(data.psfmt32[idxof(i)] ^ data.psfmt32[idxof((i + mid) % N32)]
+ ^ data.psfmt32[idxof((i + N32 - 1) % N32)]);
+ data.psfmt32[idxof((i + mid) % N32)] += r;
+ r += init_key[j] + i;
+ data.psfmt32[idxof((i + mid + lag) % N32)] += r;
+ data.psfmt32[idxof(i)] = r;
+ i = (i + 1) % N32;
+ }
+ for (; j < count; j++) {
+ r = func1(data.psfmt32[idxof(i)] ^ data.psfmt32[idxof((i + mid) % N32)]
+ ^ data.psfmt32[idxof((i + N32 - 1) % N32)]);
+ data.psfmt32[idxof((i + mid) % N32)] += r;
+ r += i;
+ data.psfmt32[idxof((i + mid + lag) % N32)] += r;
+ data.psfmt32[idxof(i)] = r;
+ i = (i + 1) % N32;
+ }
+ for (j = 0; j < N32; j++) {
+ r = func2(data.psfmt32[idxof(i)] + data.psfmt32[idxof((i + mid) % N32)]
+ + data.psfmt32[idxof((i + N32 - 1) % N32)]);
+ data.psfmt32[idxof((i + mid) % N32)] ^= r;
+ r -= i;
+ data.psfmt32[idxof((i + mid + lag) % N32)] ^= r;
+ data.psfmt32[idxof(i)] = r;
+ i = (i + 1) % N32;
+ }
+
+ data.idx = N32;
+ period_certification(data);
+ data.initialized = 1;
+}
+
+/**
+ * Create an SFMTData object. This allows outside code to create these objects without needing to know their definition.
+ */
+SFMTData* createSFMTData(void) {
+ return new SFMTData();
+}
+
+/**
+ * Delete an SFMTData object that was created with createSFMTData().
+ */
+void deleteSFMTData(SFMTData* data) {
+ delete data;
+}
+
+} // SimTK_SFMT
+
+
diff --git a/SimTKcommon/Random/src/SFMT.h b/SimTKcommon/Random/src/SFMT.h
new file mode 100644
index 0000000..18e46fb
--- /dev/null
+++ b/SimTKcommon/Random/src/SFMT.h
@@ -0,0 +1,172 @@
+/**
+ * @file SFMT.h
+ *
+ * @brief SIMD oriented Fast Mersenne Twister(SFMT) pseudorandom
+ * number generator
+ *
+ * @author Mutsuo Saito (Hiroshima University)
+ * @author Makoto Matsumoto (Hiroshima University)
+ *
+ * Copyright (C) 2006, 2007 Mutsuo Saito, Makoto Matsumoto and Hiroshima
+ * University. All rights reserved.
+ *
+ * The new BSD License is applied to this software.
+ * see LICENSE.txt
+ *
+ * @note We assume that your system has inttypes.h. If your system
+ * doesn't have inttypes.h, you have to typedef uint32_t and uint64_t,
+ * and you have to define PRIu64 and PRIx64 in this file as follows:
+ * @verbatim
+ typedef unsigned int uint32_t
+ typedef unsigned long long uint64_t
+ #define PRIu64 "llu"
+ #define PRIx64 "llx"
+ at endverbatim
+ * uint32_t must be exactly 32-bit unsigned integer type (no more, no
+ * less), and uint64_t must be exactly 64-bit unsigned integer type.
+ * PRIu64 and PRIx64 are used for printf function to print 64-bit
+ * unsigned int and 64-bit unsigned int in hexadecimal format.
+ */
+
+#ifndef SFMT_H
+#define SFMT_H
+
+
+#include "SimTKcommon/internal/common.h"
+#include <cstdio>
+
+#if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L)
+ #include <inttypes.h>
+#elif defined(_MSC_VER) || defined(__BORLANDC__)
+ typedef unsigned int uint32_t;
+ typedef unsigned __int64 uint64_t;
+ #define inline __inline
+#else
+ #include <inttypes.h>
+ #if defined(__GNUC__)
+ #define inline __inline__
+ #endif
+#endif
+
+#ifndef PRIu64
+ #if defined(_MSC_VER) || defined(__BORLANDC__)
+ #define PRIu64 "I64u"
+ #define PRIx64 "I64x"
+ #else
+ #define PRIu64 "llu"
+ #define PRIx64 "llx"
+ #endif
+#endif
+
+#if defined(__GNUC__)
+#define ALWAYSINLINE __attribute__((always_inline))
+#else
+#define ALWAYSINLINE
+#endif
+
+#if defined(_MSC_VER)
+ #if _MSC_VER >= 1200
+ #define PRE_ALWAYS __forceinline
+ #else
+ #define PRE_ALWAYS inline
+ #endif
+#else
+ #define PRE_ALWAYS inline
+#endif
+
+/**
+ * This namespace contains the functions defined by the SFMT library.
+ */
+
+namespace SimTK_SFMT {
+
+class SFMTData;
+
+SimTK_SimTKCOMMON_EXPORT uint32_t gen_rand32(SFMTData& data);
+SimTK_SimTKCOMMON_EXPORT uint64_t gen_rand64(SFMTData& data);
+SimTK_SimTKCOMMON_EXPORT void fill_array32(uint32_t *array, int size, SFMTData& data);
+SimTK_SimTKCOMMON_EXPORT void fill_array64(uint64_t *array, int size, SFMTData& data);
+SimTK_SimTKCOMMON_EXPORT void init_gen_rand(uint32_t seed, SFMTData& data);
+SimTK_SimTKCOMMON_EXPORT void init_by_array(uint32_t *init_key, int key_length, SFMTData& data);
+SimTK_SimTKCOMMON_EXPORT const char* get_idstring(void);
+SimTK_SimTKCOMMON_EXPORT int get_min_array_size32(void);
+SimTK_SimTKCOMMON_EXPORT int get_min_array_size64(void);
+SimTK_SimTKCOMMON_EXPORT SFMTData* createSFMTData(void);
+SimTK_SimTKCOMMON_EXPORT void deleteSFMTData(SFMTData* data);
+
+/* These real versions are due to Isaku Wada */
+/** generates a random number on [0,1]-real-interval */
+inline static double to_real1(uint32_t v)
+{
+ return v * (1.0/4294967295.0);
+ /* divided by 2^32-1 */
+}
+
+/** generates a random number on [0,1]-real-interval */
+inline static double genrand_real1(SFMTData& data)
+{
+ return to_real1(gen_rand32(data));
+}
+
+/** generates a random number on [0,1)-real-interval */
+inline static double to_real2(uint32_t v)
+{
+ return v * (1.0/4294967296.0);
+ /* divided by 2^32 */
+}
+
+/** generates a random number on [0,1)-real-interval */
+inline static double genrand_real2(SFMTData& data)
+{
+ return to_real2(gen_rand32(data));
+}
+
+/** generates a random number on (0,1)-real-interval */
+inline static double to_real3(uint32_t v)
+{
+ return (((double)v) + 0.5)*(1.0/4294967296.0);
+ /* divided by 2^32 */
+}
+
+/** generates a random number on (0,1)-real-interval */
+inline static double genrand_real3(SFMTData& data)
+{
+ return to_real3(gen_rand32(data));
+}
+/** These real versions are due to Isaku Wada */
+
+/** generates a random number on [0,1) with 53-bit resolution*/
+inline static double to_res53(uint64_t v)
+{
+ return v * (1.0/18446744073709551616.0L);
+}
+
+/** generates a random number on [0,1) with 53-bit resolution from two
+ * 32 bit integers */
+inline static double to_res53_mix(uint32_t x, uint32_t y)
+{
+ return to_res53(x | ((uint64_t)y << 32));
+}
+
+/** generates a random number on [0,1) with 53-bit resolution
+ */
+inline static double genrand_res53(SFMTData& data)
+{
+ return to_res53(gen_rand64(data));
+}
+
+/** generates a random number on [0,1) with 53-bit resolution
+ using 32bit integer.
+ */
+inline static double genrand_res53_mix(SFMTData& data)
+{
+ uint32_t x, y;
+
+ x = gen_rand32(data);
+ y = gen_rand32(data);
+ return to_res53_mix(x, y);
+}
+
+} // SimTK_SFMT
+
+#endif
diff --git a/SimTKcommon/Scalar/include/SimTKcommon/Scalar.h b/SimTKcommon/Scalar/include/SimTKcommon/Scalar.h
new file mode 100644
index 0000000..afa0732
--- /dev/null
+++ b/SimTKcommon/Scalar/include/SimTKcommon/Scalar.h
@@ -0,0 +1,1388 @@
+#ifndef SimTK_SIMMATRIX_SCALAR_H_
+#define SimTK_SIMMATRIX_SCALAR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This is a user-includable header which includes everything needed
+ * to make use of SimMatrix Scalar code. More commonly, this will be
+ * included from within Matrix code.
+ */
+
+#include "SimTKcommon/internal/common.h"
+#include "SimTKcommon/Constants.h"
+#include "SimTKcommon/internal/Exception.h"
+#include "SimTKcommon/internal/ExceptionMacros.h"
+#include "SimTKcommon/internal/String.h"
+#include "SimTKcommon/internal/Array.h"
+
+#include "SimTKcommon/internal/conjugate.h"
+#include "SimTKcommon/internal/CompositeNumericalTypes.h"
+#include "SimTKcommon/internal/NTraits.h"
+#include "SimTKcommon/internal/negator.h"
+
+#include <complex>
+#include <cmath>
+#include <climits>
+#include <cassert>
+#include <utility>
+
+namespace SimTK {
+
+ /////////////////////////////////////////
+ // Handy default-precision definitions //
+ /////////////////////////////////////////
+
+typedef conjugate<Real> Conjugate; // like Complex
+
+/** @defgroup TypedNumConstants Numerical Constants with Types
+ @ingroup PredefinedConstants
+
+This is a set of predefined constants in the form of Real (%SimTK default
+precision) symbols that are important for writing precision-independent
+numerical algorithms. These constants have memory addresses (that is, they
+are not macros), so you can return references to them. These are global
+external symbols rather than static members to avoid problems with static
+initialization order.
+
+Constants defined here include common mathematical values like pi, e, and
+sqrt(2) and also numerical constants related to the floating point
+implementation of the Real type, such as NaN, Infinity, and the machine
+precision Epsilon. For convenience we also provide several common numerical
+values for which it is useful to have a precision-independent representation
+(mostly to avoid warnings or casts to avoid them), and also where it is
+useful to have a referenceable memory location that contains those values.
+These include small integers and common small fractions like 1/2 and 1/3.
+
+Note that the Simbody convention for typed constants is to name them like
+ordinary variables except with an initial capital letter (like a class name).
+Typed constants are processed by the compiler rather than the preprocessor and
+do not require any special treatment when used; they behave just like variables
+of the same type and value would behave. So we don't feel the need to draw
+attention to them with ALL_CAPS like we do with preprocessor symbols. **/
+/**@{**/
+
+/** This is the IEEE "not a number" constant for this implementation of
+the default-precision Real type; be very careful using this because it has
+many strange properties such as not comparing equal to itself. You must use
+the SimTK::isNaN() function instead to determine whether something contains
+a NaN value. **/
+extern SimTK_SimTKCOMMON_EXPORT const Real NaN;
+/** This is the IEEE positive infinity constant for this implementation of
+the default-precision Real type; -Infinity will produce the negative infinity
+constant. Infinity tests larger than any other Real value. **/
+extern SimTK_SimTKCOMMON_EXPORT const Real Infinity;
+
+/** Epsilon is the size of roundoff noise; it is the smallest positive number
+of default-precision type Real such that 1+Eps != 1. If Real is double (the
+normal case) then Eps ~= 1e-16; if Real is float then Eps ~= 1e-7. **/
+extern SimTK_SimTKCOMMON_EXPORT const Real Eps;
+/** This is the square root of Eps, ~1e-8 if Real is double, ~3e-4 if Real
+is float. Many numerical algorithms are limited to accuracy of sqrt(Eps)
+so this constant is useful in checking for termination of them. **/
+extern SimTK_SimTKCOMMON_EXPORT const Real SqrtEps;
+/** TinyReal is a floating point value smaller than the floating point
+precision; it is defined as Eps^(5/4) which is ~1e-20 for Real==double and
+~1e-9 for float. This is commonly used as a number to add to a computation in
+a denominator (such as a vector length) that might come out zero, just for
+the purpose of avoiding a divide by zero. **/
+extern SimTK_SimTKCOMMON_EXPORT const Real TinyReal;
+/** SignificantReal is the smallest value that we consider to be clearly
+distinct from roundoff error when it is the result of a computation;
+it is defined as Eps^(7/8) which is ~1e-14 when Real==double, ~1e-6 when
+Real==float. **/
+extern SimTK_SimTKCOMMON_EXPORT const Real SignificantReal;
+
+/** This is the smallest positive real number that can be expressed in the
+type Real; it is ~1e-308 when Real==double, ~1e-38 when Real==float. **/
+extern SimTK_SimTKCOMMON_EXPORT const Real LeastPositiveReal;
+/** This is the largest finite positive real number that can be expressed in
+the Real type; ~1e+308 when Real==double, ~1e+38 when Real==float.\ Note
+that there is also a value Infinity that will test larger than this one. **/
+extern SimTK_SimTKCOMMON_EXPORT const Real MostPositiveReal;
+/** This is the largest negative real number (that is, closest to zero) that
+can be expressed in values of type Real. **/
+extern SimTK_SimTKCOMMON_EXPORT const Real LeastNegativeReal;
+/** This is the smallest finite negative real number that
+can be expressed in values of type Real.\ Note that -Infinity is a value
+that will still test smaller than this one. **/
+extern SimTK_SimTKCOMMON_EXPORT const Real MostNegativeReal;
+
+/** This is the number of decimal digits that can be reliably stored and
+retrieved in the default Real precision (typically log10(1/eps)-1), that is,
+about 15 digits when Real==double and 6 digits when Real==float. **/
+extern SimTK_SimTKCOMMON_EXPORT const int NumDigitsReal;
+
+/** This is the smallest number of decimal digits you should store in a text
+file if you want to be able to get exactly the same bit pattern back when you
+read it back in and convert the text to a Real value. Typically, this is about
+log10(1/tiny), which is about 20 digits when Real==double and 9 digits when
+Real==float. **/
+extern SimTK_SimTKCOMMON_EXPORT const int LosslessNumDigitsReal; // double ~20,
+ // float ~9
+
+ // Carefully calculated constants, with convenient memory addresses.
+
+extern SimTK_SimTKCOMMON_EXPORT const Real Zero; ///< Real(0)
+extern SimTK_SimTKCOMMON_EXPORT const Real One; ///< Real(1)
+extern SimTK_SimTKCOMMON_EXPORT const Real MinusOne; ///< Real(-1)
+extern SimTK_SimTKCOMMON_EXPORT const Real Two; ///< Real(2)
+extern SimTK_SimTKCOMMON_EXPORT const Real Three; ///< Real(3)
+
+extern SimTK_SimTKCOMMON_EXPORT const Real OneHalf; ///< Real(1)/2
+extern SimTK_SimTKCOMMON_EXPORT const Real OneThird; ///< Real(1)/3
+extern SimTK_SimTKCOMMON_EXPORT const Real OneFourth; ///< Real(1)/4
+extern SimTK_SimTKCOMMON_EXPORT const Real OneFifth; ///< Real(1)/5
+extern SimTK_SimTKCOMMON_EXPORT const Real OneSixth; ///< Real(1)/6
+extern SimTK_SimTKCOMMON_EXPORT const Real OneSeventh; ///< Real(1)/7
+extern SimTK_SimTKCOMMON_EXPORT const Real OneEighth; ///< Real(1)/8
+extern SimTK_SimTKCOMMON_EXPORT const Real OneNinth; ///< Real(1)/9
+extern SimTK_SimTKCOMMON_EXPORT const Real Pi; ///< Real(pi)
+extern SimTK_SimTKCOMMON_EXPORT const Real OneOverPi; ///< 1/Real(pi)
+extern SimTK_SimTKCOMMON_EXPORT const Real E; ///< \e e = Real(exp(1))
+extern SimTK_SimTKCOMMON_EXPORT const Real Log2E; ///< Real(log2(\e e)) (log base 2)
+extern SimTK_SimTKCOMMON_EXPORT const Real Log10E; ///< Real(log10(\e e)) (log base 10)
+extern SimTK_SimTKCOMMON_EXPORT const Real Sqrt2; ///< Real(sqrt(2))
+extern SimTK_SimTKCOMMON_EXPORT const Real OneOverSqrt2;///< 1/sqrt(2)==sqrt(2)/2 as Real
+extern SimTK_SimTKCOMMON_EXPORT const Real Sqrt3; ///< Real(sqrt(3))
+extern SimTK_SimTKCOMMON_EXPORT const Real OneOverSqrt3;///< Real(1/sqrt(3))
+extern SimTK_SimTKCOMMON_EXPORT const Real CubeRoot2; ///< Real(2^(1/3)) (cube root of 2)
+extern SimTK_SimTKCOMMON_EXPORT const Real CubeRoot3; ///< Real(3^(1/3)) (cube root of 3)
+extern SimTK_SimTKCOMMON_EXPORT const Real Ln2; ///< Real(ln(2)) (natural log of 2)
+extern SimTK_SimTKCOMMON_EXPORT const Real Ln10; ///< Real(ln(10)) (natural log of 10)
+
+/** We only need one complex constant, \e i = sqrt(-1).\ For the rest
+just multiply the real constant by \e i, or convert with
+Complex(the Real constant), or if you need an address you can use
+NTraits<Complex>::getPi(), etc.
+ at see NTraits **/
+extern SimTK_SimTKCOMMON_EXPORT const Complex I;
+/**@}**/
+
+ ///////////////////////////
+ // SOME SCALAR UTILITIES //
+ ///////////////////////////
+
+
+/**
+ * @defgroup atMostOneBitIsSet atMostOneBitIsSet()
+ * @ingroup BitFunctions
+ *
+ * atMostOneBitIsSet(i) provides an extremely fast way to determine whether
+ * an integral type is either zero or consists of a single set bit. This
+ * question arises when using bits to represent set membership where one
+ * may wish to verify that an integer represents a single element rather
+ * than a set of elements.
+ *
+ * @see exactlyOneBitIsSet()
+ */
+//@{
+// We use the trick that v & v-1 returns the value that is v with its
+// rightmost bit cleared (if it has a rightmost bit set).
+inline bool atMostOneBitIsSet(unsigned char v) {return (v&(v-1))==0;}
+inline bool atMostOneBitIsSet(unsigned short v) {return (v&(v-1))==0;}
+inline bool atMostOneBitIsSet(unsigned int v) {return (v&(v-1))==0;}
+inline bool atMostOneBitIsSet(unsigned long v) {return (v&(v-1))==0;}
+inline bool atMostOneBitIsSet(unsigned long long v) {return (v&(v-1))==0;}
+inline bool atMostOneBitIsSet(signed char v) {return (v&(v-1))==0;}
+inline bool atMostOneBitIsSet(char v) {return (v&(v-1))==0;}
+inline bool atMostOneBitIsSet(short v) {return (v&(v-1))==0;}
+inline bool atMostOneBitIsSet(int v) {return (v&(v-1))==0;}
+inline bool atMostOneBitIsSet(long v) {return (v&(v-1))==0;}
+inline bool atMostOneBitIsSet(long long v) {return (v&(v-1))==0;}
+//@}
+
+/**
+ * @defgroup exactlyOneBitIsSet exactlyOneBitIsSet()
+ * @ingroup BitFunctions
+ *
+ * exactlyOneBitIsSet(i) provides a very fast way to determine whether
+ * an integral type has exactly one bit set. For unsigned and positive
+ * signed values, this is equivalent to the value being a power of two.
+ * Note that negative powers of two are <em>not</em> represented with
+ * a single bit set -- negate it first if you want to use this routine
+ * to determine if a signed value is a negative power of two.
+ *
+ * @see atMostOneBitIsSet()
+ */
+//@{
+inline bool exactlyOneBitIsSet(unsigned char v) {return v && atMostOneBitIsSet(v);}
+inline bool exactlyOneBitIsSet(unsigned short v) {return v && atMostOneBitIsSet(v);}
+inline bool exactlyOneBitIsSet(unsigned int v) {return v && atMostOneBitIsSet(v);}
+inline bool exactlyOneBitIsSet(unsigned long v) {return v && atMostOneBitIsSet(v);}
+inline bool exactlyOneBitIsSet(unsigned long long v) {return v && atMostOneBitIsSet(v);}
+inline bool exactlyOneBitIsSet(signed char v) {return v && atMostOneBitIsSet(v);}
+inline bool exactlyOneBitIsSet(char v) {return v && atMostOneBitIsSet(v);}
+inline bool exactlyOneBitIsSet(short v) {return v && atMostOneBitIsSet(v);}
+inline bool exactlyOneBitIsSet(int v) {return v && atMostOneBitIsSet(v);}
+inline bool exactlyOneBitIsSet(long v) {return v && atMostOneBitIsSet(v);}
+inline bool exactlyOneBitIsSet(long long v) {return v && atMostOneBitIsSet(v);}
+//@}
+
+/**
+ * @defgroup SignBitGroup signBit()
+ * @ingroup BitFunctions
+ *
+ * signBit(i) provides a fast way to determine the value of the sign
+ * bit (as a bool) for integral and floating types. Note that this is
+ * significantly different than sign(x); be sure you know what you're doing
+ * if you use this method. signBit() refers to the underlying representation
+ * rather than the numerical value. For example, for floating types there
+ * are two zeroes, +0 and -0 which have opposite sign bits but the same
+ * sign (0). Also, unsigned types have sign() of 0 or 1, but they are
+ * considered here to have a sign bit of 0 always, since the stored high
+ * bit does not indicate a negative value.
+ *
+ * \b Notes
+ * - signBit() is overloaded for 'signed char' and 'unsigned char', but not
+ * for plain 'char' because we don't know whether to interpret the high
+ * bit as a sign in that case (because the C++ standard leaves it unspecified).
+ * - complex and conjugate numbers do not have sign bits.
+ * - negator<float> and negator<double> have the \e same sign bit as the
+ * underlying representation -- it's up to you to realize that it is
+ * interpreted differently!
+ *
+ * @see sign()
+ */
+/*@{*/
+
+inline bool signBit(unsigned char u) {return false;}
+inline bool signBit(unsigned short u) {return false;}
+inline bool signBit(unsigned int u) {return false;}
+inline bool signBit(unsigned long u) {return false;}
+inline bool signBit(unsigned long long u) {return false;}
+
+// Note that plain 'char' type is not overloaded -- see above.
+
+// We're assuming sizeof(char)==1, short==2, int==4, long long==8 which is safe
+// for all our anticipated platforms. But some 64 bit implementations have
+// sizeof(long)==4 while others have sizeof(long)==8 so we'll use the ANSI
+// standard value LONG_MIN which is a long value with just the high bit set.
+// (We're assuming two's complement integers here; I haven't seen anything
+// else in decades.)
+inline bool signBit(signed char i) {return ((unsigned char)i & 0x80U) != 0;}
+inline bool signBit(short i) {return ((unsigned short)i & 0x8000U) != 0;}
+inline bool signBit(int i) {return ((unsigned int)i & 0x80000000U) != 0;}
+inline bool signBit(long long i) {return ((unsigned long long)i & 0x8000000000000000ULL) != 0;}
+
+inline bool signBit(long i) {return ((unsigned long)i
+ & (unsigned long)LONG_MIN) != 0;}
+
+inline bool signBit(const float& f) {return std::signbit(f);}
+inline bool signBit(const double& d) {return std::signbit(d);}
+inline bool signBit(const negator<float>& nf) {return std::signbit(-nf);} // !!
+inline bool signBit(const negator<double>& nd) {return std::signbit(-nd);} // !!
+
+/*@}*/
+
+/**
+ * @defgroup SignGroup sign()
+ * @ingroup ScalarFunctions
+ *
+ * s=sign(n) returns int -1,0,1 according to n<0, n==0, n>0 for any integer
+ * or real numeric type, unsigned 0 or 1 for any unsigned argument. This
+ * routine is specialized for each of the int, unsigned, and real types
+ * of all sizes. Sign is defined for "signed char" and "unsigned char" but
+ * not plain "char" since the language leaves unspecified whether that is
+ * a signed or unsigned type. Sign is not defined for complex or conjugate.
+ */
+//@{
+inline unsigned int sign(unsigned char u) {return u==0 ? 0 : 1;}
+inline unsigned int sign(unsigned short u) {return u==0 ? 0 : 1;}
+inline unsigned int sign(unsigned int u) {return u==0 ? 0 : 1;}
+inline unsigned int sign(unsigned long u) {return u==0 ? 0 : 1;}
+inline unsigned int sign(unsigned long long u) {return u==0 ? 0 : 1;}
+
+// Don't overload for plain "char" because it may be signed or unsigned
+// depending on the compiler.
+
+inline int sign(signed char i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
+inline int sign(short i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
+inline int sign(int i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
+inline int sign(long i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
+inline int sign(long long i) {return i>0 ? 1 : (i<0 ? -1 : 0);}
+
+inline int sign(const float& x) {return x>0 ? 1 : (x<0 ? -1 : 0);}
+inline int sign(const double& x) {return x>0 ? 1 : (x<0 ? -1 : 0);}
+inline int sign(const long double& x) {return x>0 ? 1 : (x<0 ? -1 : 0);}
+
+inline int sign(const negator<float>& x) {return -sign(-x);} // -x is free
+inline int sign(const negator<double>& x) {return -sign(-x);}
+inline int sign(const negator<long double>& x) {return -sign(-x);}
+//@}
+
+/**
+ * @defgroup square square()
+ * @ingroup ScalarFunctions
+ *
+ * y=square(x) returns the square of the argument for any numeric type.
+ * We promise to evaluate x only once. We assume is is acceptable for
+ * the result type to be the same as the argument type; if it won't
+ * fit caller must cast argument to a wider type first. This is an
+ * inline routine which will run as fast as an explicit multiply
+ * (x*x) in optimized code, and somewhat faster for complex and
+ * conjugate types (5 flops instead of the usual 6).
+ *
+ * Squaring a negated number loses the negator at no cost; squaring
+ * a conjugate number returns a complex result at no additional cost.
+ */
+//@{
+inline unsigned char square(unsigned char u) {return u*u;}
+inline unsigned short square(unsigned short u) {return u*u;}
+inline unsigned int square(unsigned int u) {return u*u;}
+inline unsigned long square(unsigned long u) {return u*u;}
+inline unsigned long long square(unsigned long long u) {return u*u;}
+
+inline char square(char c) {return c*c;}
+
+inline signed char square(signed char i) {return i*i;}
+inline short square(short i) {return i*i;}
+inline int square(int i) {return i*i;}
+inline long square(long i) {return i*i;}
+inline long long square(long long i) {return i*i;}
+
+inline float square(const float& x) {return x*x;}
+inline double square(const double& x) {return x*x;}
+inline long double square(const long double& x) {return x*x;}
+
+// Negation is free for negators, so we can square them and clean
+// them up at the same time at no extra cost.
+inline float square(const negator<float>& x) {return square(-x);}
+inline double square(const negator<double>& x) {return square(-x);}
+inline long double square(const negator<long double>& x) {return square(-x);}
+
+// It is safer to templatize using complex classes, and doesn't make
+// debugging any worse since complex is already templatized.
+// 5 flops vs. 6 for general complex multiply.
+template <class P> inline
+std::complex<P> square(const std::complex<P>& x) {
+ const P re=x.real(), im=x.imag();
+ return std::complex<P>(re*re-im*im, 2*re*im);
+}
+
+// We can square a conjugate and clean it up back to complex at
+// the same time at zero cost (or maybe 1 negation depending
+// on how the compiler handles the "-2" below).
+template <class P> inline
+std::complex<P> square(const conjugate<P>& x) {
+ const P re=x.real(), nim=x.negImag();
+ return std::complex<P>(re*re-nim*nim, -2*re*nim);
+}
+
+template <class P> inline
+std::complex<P> square(const negator< std::complex<P> >& x) {
+ return square(-x); // negation is free for negators
+}
+
+// Note return type here after squaring negator<conjugate>
+// is complex, not conjugate.
+template <class P> inline
+std::complex<P> square(const negator< conjugate<P> >& x) {
+ return square(-x); // negation is free for negators
+}
+//@}
+
+/**
+ * @defgroup cube cube()
+ * @ingroup ScalarFunctions
+ *
+ * y=cube(x) returns the cube of the argument for any numeric type,
+ * integral or floating point. We promise to evaluate x only once.
+ * We assume is is acceptable for the result type to be the same
+ * as the argument type; if it won't fit caller must cast argument
+ * to a wider type first. This is an inline routine which will run
+ * as fast as explicit multiplies (x*x*x) in optimized code, and
+ * significantly faster for complex or conjugate types (8 flops
+ * vs. 11).
+ *
+ * Cubing a negated real type returns a negated result. Cubing
+ * a negated complex or conjugate returns a non-negated complex
+ * result since that can be done with no additional cost.
+ */
+//@{
+inline unsigned char cube(unsigned char u) {return u*u*u;}
+inline unsigned short cube(unsigned short u) {return u*u*u;}
+inline unsigned int cube(unsigned int u) {return u*u*u;}
+inline unsigned long cube(unsigned long u) {return u*u*u;}
+inline unsigned long long cube(unsigned long long u) {return u*u*u;}
+
+inline char cube(char c) {return c*c*c;}
+
+inline signed char cube(signed char i) {return i*i*i;}
+inline short cube(short i) {return i*i*i;}
+inline int cube(int i) {return i*i*i;}
+inline long cube(long i) {return i*i*i;}
+inline long long cube(long long i) {return i*i*i;}
+
+inline float cube(const float& x) {return x*x*x;}
+inline double cube(const double& x) {return x*x*x;}
+inline long double cube(const long double& x) {return x*x*x;}
+
+// To keep this cheap we'll defer getting rid of the negator<> until
+// some other operation. We cube -x and then recast that to negator<>
+// on return, for a total cost of 2 flops.
+inline negator<float> cube(const negator<float>& x) {
+ return negator<float>::recast(cube(-x));
+}
+inline negator<double> cube(const negator<double>& x) {
+ return negator<double>::recast(cube(-x));
+}
+inline negator<long double> cube(const negator<long double>& x) {
+ return negator<long double>::recast(cube(-x));
+}
+
+// Cubing a complex this way is cheaper than doing it by
+// multiplication. Cost here is 8 flops vs. 11 for a square
+// followed by a multiply.
+template <class P> inline
+std::complex<P> cube(const std::complex<P>& x) {
+ const P re=x.real(), im=x.imag(), rr=re*re, ii=im*im;
+ return std::complex<P>(re*(rr-3*ii), im*(3*rr-ii));
+}
+
+// Cubing a negated complex allows us to cube and eliminate the
+// negator at the same time for zero extra cost. Compare the
+// expressions here to the normal cube above to see the free
+// sign changes in both parts. 8 flops here.
+template <class P> inline
+std::complex<P> cube(const negator< std::complex<P> >& x) {
+ // -x is free for a negator
+ const P nre=(-x).real(), nim=(-x).imag(), rr=nre*nre, ii=nim*nim;
+ return std::complex<P>(nre*(3*ii-rr), nim*(ii-3*rr));
+}
+
+// Cubing a conjugate this way saves a lot over multiplying, and
+// also lets us convert the result to complex for free.
+template <class P> inline
+std::complex<P> cube(const conjugate<P>& x) {
+ const P re=x.real(), nim=x.negImag(), rr=re*re, ii=nim*nim;
+ return std::complex<P>(re*(rr-3*ii), nim*(ii-3*rr));
+}
+
+
+// Cubing a negated conjugate this way saves a lot over multiplying, and
+// also lets us convert the result to non-negated complex for free.
+template <class P> inline
+std::complex<P> cube(const negator< conjugate<P> >& x) {
+ // -x is free for a negator
+ const P nre=(-x).real(), im=(-x).negImag(), rr=nre*nre, ii=im*im;
+ return std::complex<P>(nre*(3*ii-rr), im*(3*rr-ii));
+}
+//@}
+
+/** @defgroup ClampingGroup clamp(), clampInPlace()
+ @ingroup ScalarFunctions
+
+Limit a numerical value so that it does not go outside a given range.
+There are two functions (plus overloads) defined here: clamp() and
+clampInPlace(). The first one is the most commonly used and simply
+calculates an in-range value from an input value and a given range
+[low,high]. clampInPlace() is given a reference to a variable and if
+necessary modifies that variable so that its value is in the given range
+[low,high]. Both functions are overloaded for all the integral and real
+types but are not defined for complex or conjugate types.
+
+The following examples shows how clamp() and clampInPlace() can be defined
+in terms of std::min() and std::max().
+
+clamp():
+ at code
+ const double low, high; // from somewhere
+ const double v;
+ double clampedValue;
+ clampedValue = clamp(low,v,high); // clampedValue is in [low,high]
+ clampedValue = std::min(std::max(low,v), high); // equivalent
+ at endcode
+clampInPlace():
+ at code
+ const double low, high; // from somewhere
+ double v;
+ clampInPlace(low,v,high); // now v is in [low,high]
+ v = std::min(std::max(low,v), high); // equivalent
+ at endcode **/
+/*@{*/
+
+/** Check that low <= v <= high and modify v in place if necessary to
+bring it into that range.
+ at param[in] low The lower bound; must be <= high.
+ at param[in,out] v The variable whose value is changed if necessary.
+ at param[in] high The upper bound; must be >= low.
+ at return A writable reference to the now-possibly-modified input variable v.
+
+This method is overloaded for all standard integral and floating point
+types. All the arguments and the return type will be the same type; there
+are no explicit overloads for mixed types so you may find you need to
+cast the bounds in some cases to ensure you are calling the correct
+overload.
+
+Cost: These are very fast inline methods; the floating point ones use
+just two flops. **/
+inline double& clampInPlace(double low, double& v, double high)
+{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
+/** @copydoc SimTK::clampInPlace(double,double&,double) **/
+inline float& clampInPlace(float low, float& v, float high)
+{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
+/** @copydoc SimTK::clampInPlace(double,double&,double) **/
+inline long double& clampInPlace(long double low, long double& v, long double high)
+{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
+
+ // Floating point clamps with integer bounds; without these
+ // explicit casts are required.
+
+/** @copydoc SimTK::clampInPlace(double,double&,double)
+Takes integer bounds to avoid need for explicit casts. **/
+inline double& clampInPlace(int low, double& v, int high)
+{ return clampInPlace((double)low,v,(double)high); }
+/** @copydoc SimTK::clampInPlace(double,double&,double)
+Takes integer bounds to avoid need for explicit casts. **/
+inline float& clampInPlace(int low, float& v, int high)
+{ return clampInPlace((float)low,v,(float)high); }
+/** @copydoc SimTK::clampInPlace(double,double&,double)
+Takes integer bounds to avoid need for explicit casts. **/
+inline long double& clampInPlace(int low, long double& v, int high)
+{ return clampInPlace((long double)low,v,(long double)high); }
+
+/** @copydoc SimTK::clampInPlace(double,double&,double)
+Takes an integer bound to avoid need for explicit casts. **/
+inline double& clampInPlace(int low, double& v, double high)
+{ return clampInPlace((double)low,v,high); }
+/** @copydoc SimTK::clampInPlace(double,double&,double)
+Takes an integer bound to avoid need for explicit casts. **/
+inline float& clampInPlace(int low, float& v, float high)
+{ return clampInPlace((float)low,v,high); }
+/** @copydoc SimTK::clampInPlace(double,double&,double)
+Takes an integer bound to avoid need for explicit casts. **/
+inline long double& clampInPlace(int low, long double& v, long double high)
+{ return clampInPlace((long double)low,v,high); }
+
+/** @copydoc SimTK::clampInPlace(double,double&,double)
+Takes an integer bound to avoid need for explicit casts. **/
+inline double& clampInPlace(double low, double& v, int high)
+{ return clampInPlace(low,v,(double)high); }
+/** @copydoc SimTK::clampInPlace(double,double&,double)
+Takes an integer bound to avoid need for explicit casts. **/
+inline float& clampInPlace(float low, float& v, int high)
+{ return clampInPlace(low,v,(float)high); }
+/** @copydoc SimTK::clampInPlace(double,double&,double)
+Takes an integer bound to avoid need for explicit casts. **/
+inline long double& clampInPlace(long double low, long double& v, int high)
+{ return clampInPlace(low,v,(long double)high); }
+
+/** @copydoc SimTK::clampInPlace(double,double&,double) **/
+inline unsigned char& clampInPlace(unsigned char low, unsigned char& v, unsigned char high)
+{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
+/** @copydoc SimTK::clampInPlace(double,double&,double) **/
+inline unsigned short& clampInPlace(unsigned short low, unsigned short& v, unsigned short high)
+{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
+/** @copydoc SimTK::clampInPlace(double,double&,double) **/
+inline unsigned int& clampInPlace(unsigned int low, unsigned int& v, unsigned int high)
+{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
+/** @copydoc SimTK::clampInPlace(double,double&,double) **/
+inline unsigned long& clampInPlace(unsigned long low, unsigned long& v, unsigned long high)
+{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
+/** @copydoc SimTK::clampInPlace(double,double&,double) **/
+inline unsigned long long& clampInPlace(unsigned long long low, unsigned long long& v, unsigned long long high)
+{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
+
+/** @copydoc SimTK::clampInPlace(double,double&,double) **/
+inline char& clampInPlace(char low, char& v, char high)
+{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
+/** @copydoc SimTK::clampInPlace(double,double&,double) **/
+inline signed char& clampInPlace(signed char low, signed char& v, signed char high)
+{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
+
+/** @copydoc SimTK::clampInPlace(double,double&,double) **/
+inline short& clampInPlace(short low, short& v, short high)
+{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
+/** @copydoc SimTK::clampInPlace(double,double&,double) **/
+inline int& clampInPlace(int low, int& v, int high)
+{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
+/** @copydoc SimTK::clampInPlace(double,double&,double) **/
+inline long& clampInPlace(long low, long& v, long high)
+{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
+/** @copydoc SimTK::clampInPlace(double,double&,double) **/
+inline long long& clampInPlace(long long low, long long& v, long long high)
+{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
+
+/** @copydoc SimTK::clampInPlace(double,double&,double) **/
+inline negator<float>& clampInPlace(float low, negator<float>& v, float high)
+{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
+/** @copydoc SimTK::clampInPlace(double,double&,double) **/
+inline negator<double>& clampInPlace(double low, negator<double>& v, double high)
+{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
+/** @copydoc SimTK::clampInPlace(double,double&,double) **/
+inline negator<long double>& clampInPlace(long double low, negator<long double>& v, long double high)
+{ assert(low<=high); if (v<low) v=low; else if (v>high) v=high; return v; }
+
+
+
+/** If v is in range low <= v <= high then return v, otherwise return
+the nearest bound; this function does not modify the input variable v.
+ at param[in] low The lower bound; must be <= high.
+ at param[in] v The value to be put into range [low,high].
+ at param[in] high The upper bound; must be >= low.
+ at return Either the value v or one of the bounds.
+
+This method is overloaded for all standard integral and floating point
+types. All the arguments and the return type will be the same type; there
+are no explicit overloads for mixed types so you may find you need to
+cast the bounds in some cases to ensure you are calling the correct
+overload.
+
+ at warning Viewed as a function of the input value v, clamp(v) is not
+very smooth -- it is continuous (C0) but even its first derivative is
+infinite at the bounds.
+
+Cost: These are very fast, inline methods; the floating point ones use
+just two flops.
+ at see clampInPlace() **/
+inline double clamp(double low, double v, double high)
+{ return clampInPlace(low,v,high); }
+/** @copydoc SimTK::clamp(double,double,double) **/
+inline float clamp(float low, float v, float high)
+{ return clampInPlace(low,v,high); }
+/** @copydoc SimTK::clamp(double,double,double) **/
+inline long double clamp(long double low, long double v, long double high)
+{ return clampInPlace(low,v,high); }
+
+/** @copydoc SimTK::clamp(double,double,double)
+Takes integer bounds to avoid need for explicit casts. **/
+inline double clamp(int low, double v, int high)
+{ return clampInPlace(low,v,high); }
+/** @copydoc SimTK::clamp(double,double,double)
+Takes integer bounds to avoid need for explicit casts. **/
+inline float clamp(int low, float v, int high)
+{ return clampInPlace(low,v,high); }
+/** @copydoc SimTK::clamp(double,double,double)
+Takes integer bounds to avoid need for explicit casts. **/
+inline long double clamp(int low, long double v, int high)
+{ return clampInPlace(low,v,high); }
+
+/** @copydoc SimTK::clamp(double,double,double)
+Takes an integer bound to avoid need for explicit casts. **/
+inline double clamp(int low, double v, double high)
+{ return clampInPlace(low,v,high); }
+/** @copydoc SimTK::clamp(double,double,double)
+Takes an integer bound to avoid need for explicit casts. **/
+inline float clamp(int low, float v, float high)
+{ return clampInPlace(low,v,high); }
+/** @copydoc SimTK::clamp(double,double,double)
+Takes an integer bound to avoid need for explicit casts. **/
+inline long double clamp(int low, long double v, long double high)
+{ return clampInPlace(low,v,high); }
+
+/** @copydoc SimTK::clamp(double,double,double)
+Takes an integer bound to avoid need for explicit casts. **/
+inline double clamp(double low, double v, int high)
+{ return clampInPlace(low,v,high); }
+/** @copydoc SimTK::clamp(double,double,double)
+Takes an integer bound to avoid need for explicit casts. **/
+inline float clamp(float low, float v, int high)
+{ return clampInPlace(low,v,high); }
+/** @copydoc SimTK::clamp(double,double,double)
+Takes an integer bound to avoid need for explicit casts. **/
+inline long double clamp(long double low, long double v, int high)
+{ return clampInPlace(low,v,high); }
+
+/** @copydoc SimTK::clamp(double,double,double) **/
+inline unsigned char clamp(unsigned char low, unsigned char v, unsigned char high)
+{ return clampInPlace(low,v,high); }
+/** @copydoc SimTK::clamp(double,double,double) **/
+inline unsigned short clamp(unsigned short low, unsigned short v, unsigned short high)
+{ return clampInPlace(low,v,high); }
+/** @copydoc SimTK::clamp(double,double,double) **/
+inline unsigned int clamp(unsigned int low, unsigned int v, unsigned int high)
+{ return clampInPlace(low,v,high); }
+/** @copydoc SimTK::clamp(double,double,double) **/
+inline unsigned long clamp(unsigned long low, unsigned long v, unsigned long high)
+{ return clampInPlace(low,v,high); }
+/** @copydoc SimTK::clamp(double,double,double) **/
+inline unsigned long long clamp(unsigned long long low, unsigned long long v, unsigned long long high)
+{ return clampInPlace(low,v,high); }
+
+/** @copydoc SimTK::clamp(double,double,double) **/
+inline char clamp(char low, char v, char high)
+{ return clampInPlace(low,v,high); }
+/** @copydoc SimTK::clamp(double,double,double) **/
+inline signed char clamp(signed char low, signed char v, signed char high)
+{ return clampInPlace(low,v,high); }
+
+/** @copydoc SimTK::clamp(double,double,double) **/
+inline short clamp(short low, short v, short high)
+{ return clampInPlace(low,v,high); }
+/** @copydoc SimTK::clamp(double,double,double) **/
+inline int clamp(int low, int v, int high)
+{ return clampInPlace(low,v,high); }
+/** @copydoc SimTK::clamp(double,double,double) **/
+inline long clamp(long low, long v, long high)
+{ return clampInPlace(low,v,high); }
+/** @copydoc SimTK::clamp(double,double,double) **/
+inline long long clamp(long long low, long long v, long long high)
+{ return clampInPlace(low,v,high); }
+
+
+// These aren't strictly necessary but are here to help the
+// compiler find the right overload to call. These cost an
+// extra flop because the negator<T> has to be cast to T which
+// requires that the pending negation be performed. Note that
+// the result types are not negated.
+
+/** @copydoc SimTK::clamp(double,double,double)
+Explicitly takes a negator<float> argument to help the compiler find the
+right overload, but the negation is performed (1 extra flop) and the result
+type is an ordinary float. **/
+inline float clamp(float low, negator<float> v, float high)
+{ return clamp(low,(float)v,high); }
+/** @copydoc SimTK::clamp(double,double,double)
+Explicitly takes a negator<double> argument to help the compiler find the
+right overload, but the negation is performed (1 extra flop) and the result
+type is an ordinary double. **/
+inline double clamp(double low, negator<double> v, double high)
+{ return clamp(low,(double)v,high); }
+/** @copydoc SimTK::clamp(double,double,double)
+Explicitly takes a negator<long double> argument to help the compiler find the
+right overload, but the negation is performed (1 extra flop) and the result
+type is an ordinary long double. **/
+inline long double clamp(long double low, negator<long double> v, long double high)
+{ return clamp(low,(long double)v,high); }
+/*@}*/
+
+
+
+/** @defgroup SmoothedStepFunctions Smoothed step functions
+ @ingroup ScalarFunctions
+
+Functions stepUp(), stepDown() and stepAny() provide smooth, S-shaped
+step functions that are useful for "softening" abrupt transitions between
+two values. Functions that return the first three derivatives of these
+step functions are also provided.
+
+y=stepUp(x) for x=0:1 returns a smooth, S-shaped step function y(x),
+symmetric about the midpoint x=0.5, such that y(0)=0, y(1)=1,
+y'(0)=y''(0)=y'(1)=y''(1)=0, where the primes indicate differentiation
+with respect to x. No guarantees are made about the behavior of higher
+derivatives except to say that y''' does exist.
+
+y=stepDown(x) for x=0:1 is a mirror image S curve that takes y
+down from 1 to 0 smoothly as x goes from 0 to 1. As with stepUp()
+y' and y'' are 0 at both ends, but here y(0)=1 and y(1)=0.
+
+The stepAny() function is also available to make a step function
+that takes y smoothly from y0 to y1 as x goes from x0 to x1.
+
+We also provide functions that calculate the first three derivatives
+of stepUp(), stepDown(), and stepAny(), namely: dstepUp(), d2stepUp(),
+d3stepUp(), and similarly dstepDown(), dstepAny() and so on. Note again
+that the third derivative is not guaranteed to be well behaved.
+
+Costs of the current implementations of these inline functions are:
+ - stepUp() 7 flops
+ - dstepUp() 4 flops
+ - d2stepUp() 6 flops
+ - d3stepUp() 4 flops
+
+The corresponding stepDown() methods cost one extra flop; the
+stepAny() methods cost 6 extra flops (provided you do some precalculation;
+see the stepAny() documentation). **/
+/*@{*/
+
+/** Interpolate smoothly from 0 up to 1 as the input argument goes from 0 to 1,
+with first and second derivatives zero at either end of the interval.
+
+ at param[in] x The control parameter, in range [0,1].
+ at return The smoothed output value, in range [0,1] but with first and second
+derivatives smoothly approaching zero as x approaches either end of its
+interval.
+
+See the documentation for stepAny() for a discussion about how to shift and
+scale this function to produce arbitrary steps.
+
+This function is overloaded for all the floating point precisions.
+Cost is 7 flops.
+ at see stepDown(), stepAny() **/
+inline double stepUp(double x)
+{ assert(0 <= x && x <= 1);
+ return x*x*x*(10+x*(6*x-15)); } //10x^3-15x^4+6x^5
+
+
+/** Interpolate smoothly from 1 down to 0 as the input argument goes from
+0 to 1, with first and second derivatives zero at either end of the interval.
+
+ at param[in] x The control parameter, in range [0,1].
+ at return The smoothed output value, in range [1,0] but with first and second
+derivatives smoothly approaching zero as x approaches either end of its
+interval.
+
+See the documentation for stepAny() for a discussion about how to shift and
+scale this function to produce arbitrary steps.
+
+This function is overloaded for all the floating point precisions.
+Cost is 8 flops.
+ at see stepUp(), stepAny() **/
+inline double stepDown(double x) {return 1.0 -stepUp(x);}
+
+
+/** Interpolate smoothly from y0 to y1 as the input argument goes from x0 to x1,
+with first and second derivatives zero at either end of the interval.
+
+ at param[in] y0
+ The output value when x=x0.
+ at param[in] yRange
+ The amount by which the output can change over the full interval, that is,
+ yRange=(y1-y0) where y1 is the value of the output when x=x1.
+ at param[in] x0
+ The minimum allowable value for x.
+ at param[in] oneOverXRange
+ 1/xRange, where xRange is the amount by which the input variable x can
+ change over its full interval, that is, xRange=(x1-x0) where x1 is the
+ maximum allowable value for x.
+ at param[in] x
+ The control parameter, in range [x0,x1]. This is often a time over
+ which the output transition from y0 to y1 is to occur.
+ at return
+ The smoothed output value, in range [y0,y1] (where y1=y0+yRange) but with
+ first and second derivatives smoothly approaching zero as x approaches
+ either end of its interval.
+
+Note that the desired curve is defined in terms of y0 and (y1-y0), and
+x0 and 1/(x1-x0), rather than y0,y1,x0,x1 which would make for a nicer
+calling signature. This is a concession to efficiency since the two ranges
+are likely to be unchanged during many calls to this function and can thus
+be precalculated. It wouldn't matter except that division is so expensive
+(equivalent to 15-20 floating point operations). If you aren't concerned
+about that (and in most cases it won't matter), you can call this
+function using y0,y1,x0,x1 like this: @code
+ y = stepAny(y0,y1-y0,x0,1/(x1-x0), x);
+ at endcode
+
+Not counting the cost of calculating the ranges, each call to this function
+requires 13 flops. Calculating the ranges in the argument list as shown
+above raises the per-call cost to about 30 flops. However, there are many
+common special cases that are much simpler. For example, if y is to go from
+-1 to 1 while x goes from 0 to 1, then you can write: @code
+ y = stepAny(-1,2,0,1, x);
+ at endcode
+which is still only 13 flops despite the lack of pre-calculation.
+
+ at par Theory
+
+stepUp() and stepDown() and their derivatives are defined only for arguments
+in the range 0 to 1. To create a general step function that smoothly
+interpolates from y=y0 to y1 while x goes from x0 to x1, and three derivatives,
+we use the stepUp() functions like this:
+ at code
+ ooxr = 1/(x1-x0); // one over x range (signed)
+ yr = y1-y0; // y range (signed)
+ xadj = (x-x0)*ooxr // x adjusted into 0:1 range (watch for roundoff)
+ y = y0 + yr * stepUp(xadj);
+ dy = yr*ooxr * dstepUp(xadj);
+ d2y = yr*ooxr^2 * d2stepUp(xadj);
+ d3y = yr*ooxr^3 * d3stepUp(xadj);
+ at endcode
+
+As a common special case, note that when y has a general range but x is
+still in [0,1], the above simplifies considerably and you can save a few
+flops if you want by working with stepUp() or stepDown() directly. For
+example, in the common case where you want y to go from -1 to 1 as x goes
+from 0 to 1:
+ at code
+ y = stepAny(-1,2,0,1, x); // y in [-1,1]; 13 flops
+ y = -1 + 2*stepUp(x); // equivalent, saves 4 flops
+ dy = 2*dstepUp(x); // these save 5 flops over dstepAny(), etc.
+ d2y = 2*d2stepUp(x);
+ d3y = 2*d3stepUp(x);
+ at endcode
+
+It would be extremely rare for these few flops to matter at all; you should
+almost always choose based on what looks better and/or is less error prone
+instead. **/
+inline double stepAny(double y0, double yRange,
+ double x0, double oneOverXRange,
+ double x)
+{ double xadj = (x-x0)*oneOverXRange;
+ assert(-NTraits<double>::getSignificant() <= xadj
+ && xadj <= 1 + NTraits<double>::getSignificant());
+ clampInPlace(0.0,xadj,1.0);
+ return y0 + yRange*stepUp(xadj); }
+
+/** First derivative of stepUp(): d/dx stepUp(x).
+ at param[in] x Control parameter in range [0,1].
+ at return First derivative of stepUp() at x. **/
+inline double dstepUp(double x) {
+ assert(0 <= x && x <= 1);
+ const double xxm1=x*(x-1);
+ return 30*xxm1*xxm1; //30x^2-60x^3+30x^4
+}
+
+/** First derivative of stepDown(): d/dx stepDown(x).
+ at param[in] x Control parameter in range [0,1].
+ at return First derivative of stepDown() at x. **/
+inline double dstepDown(double x) {return -dstepUp(x);}
+
+/** First derivative of stepAny(): d/dx stepAny(x).
+See stepAny() for parameter documentation.
+ at return First derivative of stepAny() at x. **/
+inline double dstepAny(double yRange,
+ double x0, double oneOverXRange,
+ double x)
+{ double xadj = (x-x0)*oneOverXRange;
+ assert(-NTraits<double>::getSignificant() <= xadj
+ && xadj <= 1 + NTraits<double>::getSignificant());
+ clampInPlace(0.0,xadj,1.0);
+ return yRange*oneOverXRange*dstepUp(xadj); }
+
+/** Second derivative of stepUp(): d^2/dx^2 stepUp(x).
+ at param[in] x Control parameter in range [0,1].
+ at return Second derivative of stepUp() at x. **/
+inline double d2stepUp(double x) {
+ assert(0 <= x && x <= 1);
+ return 60*x*(1+x*(2*x-3)); //60x-180x^2+120x^3
+}
+
+/** Second derivative of stepDown(): d^2/dx^2 stepDown(x).
+ at param[in] x Control parameter in range [0,1].
+ at return Second derivative of stepDown() at x. **/
+inline double d2stepDown(double x) {return -d2stepUp(x);}
+
+/** Second derivative of stepAny(): d^2/dx^2 stepAny(x).
+See stepAny() for parameter documentation.
+ at return Second derivative of stepAny() at x. **/
+inline double d2stepAny(double yRange,
+ double x0, double oneOverXRange,
+ double x)
+{ double xadj = (x-x0)*oneOverXRange;
+ assert(-NTraits<double>::getSignificant() <= xadj
+ && xadj <= 1 + NTraits<double>::getSignificant());
+ clampInPlace(0.0,xadj,1.0);
+ return yRange*square(oneOverXRange)*d2stepUp(xadj); }
+
+/** Third derivative of stepUp(): d^3/dx^3 stepUp(x).
+ at param[in] x Control parameter in range [0,1].
+ at return Third derivative of stepUp() at x. **/
+inline double d3stepUp(double x) {
+ assert(0 <= x && x <= 1);
+ return 60+360*x*(x-1); //60-360*x+360*x^2
+}
+
+/** Third derivative of stepDown(): d^3/dx^3 stepDown(x).
+ at param[in] x Control parameter in range [0,1].
+ at return Third derivative of stepDown() at x. **/
+inline double d3stepDown(double x) {return -d3stepUp(x);}
+
+/** Third derivative of stepAny(): d^3/dx^3 stepAny(x).
+See stepAny() for parameter documentation.
+ at return Third derivative of stepAny() at x. **/
+inline double d3stepAny(double yRange,
+ double x0, double oneOverXRange,
+ double x)
+{ double xadj = (x-x0)*oneOverXRange;
+ assert(-NTraits<double>::getSignificant() <= xadj
+ && xadj <= 1 + NTraits<double>::getSignificant());
+ clampInPlace(0.0,xadj,1.0);
+ return yRange*cube(oneOverXRange)*d3stepUp(xadj); }
+
+ // float
+
+/** @copydoc SimTK::stepUp(double) **/
+inline float stepUp(float x)
+{ assert(0 <= x && x <= 1);
+ return x*x*x*(10+x*(6*x-15)); } //10x^3-15x^4+6x^5
+/** @copydoc SimTK::stepDown(double) **/
+inline float stepDown(float x) {return 1.0f-stepUp(x);}
+/** @copydoc SimTK::stepAny(double,double,double,double,double) **/
+inline float stepAny(float y0, float yRange,
+ float x0, float oneOverXRange,
+ float x)
+{ float xadj = (x-x0)*oneOverXRange;
+ assert(-NTraits<float>::getSignificant() <= xadj
+ && xadj <= 1 + NTraits<float>::getSignificant());
+ clampInPlace(0.0f,xadj,1.0f);
+ return y0 + yRange*stepUp(xadj); }
+
+/** @copydoc SimTK::dstepUp(double) **/
+inline float dstepUp(float x)
+{ assert(0 <= x && x <= 1);
+ const float xxm1=x*(x-1);
+ return 30*xxm1*xxm1; } //30x^2-60x^3+30x^4
+/** @copydoc SimTK::dstepDown(double) **/
+inline float dstepDown(float x) {return -dstepUp(x);}
+/** @copydoc SimTK::dstepAny(double,double,double,double) **/
+inline float dstepAny(float yRange,
+ float x0, float oneOverXRange,
+ float x)
+{ float xadj = (x-x0)*oneOverXRange;
+ assert(-NTraits<float>::getSignificant() <= xadj
+ && xadj <= 1 + NTraits<float>::getSignificant());
+ clampInPlace(0.0f,xadj,1.0f);
+ return yRange*oneOverXRange*dstepUp(xadj); }
+
+/** @copydoc SimTK::d2stepUp(double) **/
+inline float d2stepUp(float x)
+{ assert(0 <= x && x <= 1);
+ return 60*x*(1+x*(2*x-3)); } //60x-180x^2+120x^3
+/** @copydoc SimTK::d2stepDown(double) **/
+inline float d2stepDown(float x) {return -d2stepUp(x);}
+/** @copydoc SimTK::d2stepAny(double,double,double,double) **/
+inline float d2stepAny(float yRange,
+ float x0, float oneOverXRange,
+ float x)
+{ float xadj = (x-x0)*oneOverXRange;
+ assert(-NTraits<float>::getSignificant() <= xadj
+ && xadj <= 1 + NTraits<float>::getSignificant());
+ clampInPlace(0.0f,xadj,1.0f);
+ return yRange*square(oneOverXRange)*d2stepUp(xadj); }
+
+/** @copydoc SimTK::d3stepUp(double) **/
+inline float d3stepUp(float x)
+{ assert(0 <= x && x <= 1);
+ return 60+360*x*(x-1); } //60-360*x+360*x^2
+/** @copydoc SimTK::d3stepDown(double) **/
+inline float d3stepDown(float x) {return -d3stepUp(x);}
+/** @copydoc SimTK::d3stepAny(double,double,double,double) **/
+inline float d3stepAny(float yRange,
+ float x0, float oneOverXRange,
+ float x)
+{ float xadj = (x-x0)*oneOverXRange;
+ assert(-NTraits<float>::getSignificant() <= xadj
+ && xadj <= 1 + NTraits<float>::getSignificant());
+ clampInPlace(0.0f,xadj,1.0f);
+ return yRange*cube(oneOverXRange)*d3stepUp(xadj); }
+
+ // long double
+
+/** @copydoc SimTK::stepUp(double) **/
+inline long double stepUp(long double x)
+{ assert(0 <= x && x <= 1);
+ return x*x*x*(10+x*(6*x-15)); } //10x^3-15x^4+6x^5
+/** @copydoc SimTK::stepDown(double) **/
+inline long double stepDown(long double x) {return 1.0L-stepUp(x);}
+/** @copydoc SimTK::stepAny(double,double,double,double,double) **/
+inline long double stepAny(long double y0, long double yRange,
+ long double x0, long double oneOverXRange,
+ long double x)
+{ long double xadj = (x-x0)*oneOverXRange;
+ assert(-NTraits<long double>::getSignificant() <= xadj
+ && xadj <= 1 + NTraits<long double>::getSignificant());
+ clampInPlace(0.0L,xadj,1.0L);
+ return y0 + yRange*stepUp(xadj); }
+
+
+/** @copydoc SimTK::dstepUp(double) **/
+inline long double dstepUp(long double x)
+{ assert(0 <= x && x <= 1);
+ const long double xxm1=x*(x-1);
+ return 30*xxm1*xxm1; } //30x^2-60x^3+30x^4
+/** @copydoc SimTK::dstepDown(double) **/
+inline long double dstepDown(long double x) {return -dstepUp(x);}
+/** @copydoc SimTK::dstepAny(double,double,double,double) **/
+inline long double dstepAny(long double yRange,
+ long double x0, long double oneOverXRange,
+ long double x)
+{ long double xadj = (x-x0)*oneOverXRange;
+ assert(-NTraits<long double>::getSignificant() <= xadj
+ && xadj <= 1 + NTraits<long double>::getSignificant());
+ clampInPlace(0.0L,xadj,1.0L);
+ return yRange*oneOverXRange*dstepUp(xadj); }
+
+/** @copydoc SimTK::d2stepUp(double) **/
+inline long double d2stepUp(long double x)
+{ assert(0 <= x && x <= 1);
+ return 60*x*(1+x*(2*x-3)); } //60x-180x^2+120x^3
+/** @copydoc SimTK::d2stepDown(double) **/
+inline long double d2stepDown(long double x) {return -d2stepUp(x);}
+/** @copydoc SimTK::d2stepAny(double,double,double,double) **/
+inline long double d2stepAny(long double yRange,
+ long double x0, long double oneOverXRange,
+ long double x)
+{ long double xadj = (x-x0)*oneOverXRange;
+ assert(-NTraits<long double>::getSignificant() <= xadj
+ && xadj <= 1 + NTraits<long double>::getSignificant());
+ clampInPlace(0.0L,xadj,1.0L);
+ return yRange*square(oneOverXRange)*d2stepUp(xadj); }
+
+
+/** @copydoc SimTK::d3stepUp(double) **/
+inline long double d3stepUp(long double x)
+{ assert(0 <= x && x <= 1);
+ return 60+360*x*(x-1); } //60-360*x+360*x^2
+/** @copydoc SimTK::d3stepDown(double) **/
+inline long double d3stepDown(long double x) {return -d3stepUp(x);}
+/** @copydoc SimTK::d3stepAny(double,double,double,double) **/
+inline long double d3stepAny(long double yRange,
+ long double x0, long double oneOverXRange,
+ long double x)
+{ long double xadj = (x-x0)*oneOverXRange;
+ assert(-NTraits<long double>::getSignificant() <= xadj
+ && xadj <= 1 + NTraits<long double>::getSignificant());
+ clampInPlace(0.0L,xadj,1.0L);
+ return yRange*cube(oneOverXRange)*d3stepUp(xadj); }
+
+ // int converts to double; only supplied for stepUp(), stepDown()
+/** @copydoc SimTK::stepUp(double)
+Treats int argument as a double (avoids ambiguity). **/
+inline double stepUp(int x) {return stepUp((double)x);}
+/** @copydoc SimTK::stepDown(double)
+Treats int argument as a double (avoids ambiguity). **/
+inline double stepDown(int x) {return stepDown((double)x);}
+
+
+/*@}*/
+
+/** @defgroup EllipticIntegralsGroup Elliptic integrals
+ @ingroup ScalarFunctions
+
+Elliptic integrals arise occasionally in contexts relevant to us, particularly
+in geometric calculations involving ellipses or ellipsoids. Here we provide
+functions for calculating the complete elliptic integrals of the first and
+second kinds, which arise in Hertz contact theory for point contacts where
+the contact area is elliptical. We use the following definitions for these
+two integrals:
+<pre>
+ K(m) = integ[0,Pi/2] {1 / sqrt(1 - m sin^2(t)) dt} 1st kind
+ E(m) = integ[0,Pi/2] { sqrt(1 - m sin^2(t)) dt} 2nd kind
+ 0 <= m <= 1
+</pre>
+Elliptic integrals are defined only for arguments in range [0,1] inclusive.
+
+We provide a function that calculates K(m) and E(m) to machine precision
+(float or double) with a fast-converging iterative method adapted from
+ref. 1, which was in turn adapted from ref. 2. A much faster approximate
+version is also available, using the higher-precision approximation of the
+two provided in ref. 2. The approximate version provides a smooth function
+that gives at least 7 digits of accuracy (in either float or double
+precision) across the full range at about 1/4 the cost of the machine
+precision version. For many applications, including engineering- or
+scientific-quality contact, 7 digits is more than adequate and in float
+precision that's all you can expect anyway.
+
+ at warning In the literature there are two different definitions for
+elliptic integrals. The other definition (call them K'(k) and E'(k)) uses
+the argument k=sqrt(m) so that K'(k)=K(m^2), E'(k)=E(m^2). Our definition
+is used by Matlab's ellipke() function and much engineering literature while
+the K',E' definitions seem to be preferred by mathematicians.
+
+For an ellipse with semimajor axis \a a and semiminor axis \a b (so a >= b),
+the eccentricity e=sqrt(1-(b/a)^2). Our argument to the elliptic integrals in
+that case is m = e^2 = 1-(b/a)^2. In constrast, K.L. Johnson uses the
+mathematicians' definition in Chapter 4, pg. 95 of his book (ref. 4)
+where he writes K(e) and E(e), where e is eccentricity as defined above,
+that is e=sqrt(m), so we would call his K'(e)=K(e^2) and E'=E(e^2).
+
+ at author Michael Sherman
+
+<h3>References</h3>
+(1) Dyson, Evans, Snidle. "A simple accurate method for calculation of stresses
+and deformations in elliptical Hertzian contacts", J. Mech. Eng. Sci. Proc.
+IMechE part C 206:139-141, 1992.
+
+(2) Abramovitz, Stegun, eds. Handbook of Mathematical Functions with
+Formulas, Graphs, and Mathematical Tables, Dover, NY, 1972.
+
+(3) Antoine, Visa, Sauvey, Abba. "Approximate analytical model for
+Hertzian Elliptical Contact Problems", ASME J. Tribology 128:660, 2006.
+
+(4) Johnson, K.L. %Contact Mechanics. Cambridge University Press 1987
+(corrected edition). **/
+/*@{*/
+
+/** @cond **/ // Doxygen should skip this helper template function
+template <class T> // float or double
+static inline std::pair<T,T> approxCompleteEllipticIntegralsKE_T(T m) {
+ static const T a[] =
+ { (T)1.38629436112, (T)0.09666344259, (T)0.03590092383,
+ (T)0.03742563713, (T)0.01451196212 };
+ static const T b[] =
+ { (T)0.5, (T)0.12498593597, (T)0.06880248576,
+ (T)0.03328355346, (T)0.00441787012 };
+ static const T c[] =
+ { (T)1, (T)0.44325141463, (T)0.06260601220,
+ (T)0.04757383546, (T)0.01736506451 };
+ static const T d[] =
+ { (T)0, (T)0.24998368310, (T)0.09200180037,
+ (T)0.04069697526, (T)0.00526449639 };
+
+ const T SignificantReal = NTraits<T>::getSignificant();
+ const T PiOver2 = NTraits<T>::getPi()/2;
+ const T Infinity = NTraits<T>::getInfinity();
+
+ SimTK_ERRCHK1_ALWAYS(-SignificantReal < m && m < 1+SignificantReal,
+ "approxCompleteEllipticIntegralsKE()",
+ "Argument m (%g) is outside the legal range [0,1].", (double)m);
+ if (m >= 1) return std::make_pair(Infinity, (T)1);
+ if (m <= 0) return std::make_pair(PiOver2, PiOver2);
+
+ const T m1=1-m, m2=m1*m1, m3=m1*m2, m4=m2*m2; // 4 flops
+ const T lnm2 = std::log(m1); // ~50 flops
+
+ // The rest is 35 flops.
+ const T K = (a[0] + a[1]*m1 + a[2]*m2 + a[3]*m3 + a[4]*m4)
+ - lnm2*(b[0] + b[1]*m1 + b[2]*m2 + b[3]*m3 + b[4]*m4);
+ const T E = (c[0] + c[1]*m1 + c[2]*m2 + c[3]*m3 + c[4]*m4)
+ - lnm2*( d[1]*m1 + d[2]*m2 + d[3]*m3 + d[4]*m4);
+
+ return std::make_pair(K,E);
+}
+/** @endcond **/
+
+/** Given 0<=m<=1, return complete elliptic integrals of the first and
+second kinds, K(m) and E(m), approximated but with a maximum error of
+2e-8 so at least 7 digits are correct (same in float or double
+precision).\ See @ref EllipticIntegralsGroup "Elliptic integrals"
+for a discussion.
+
+Note that a full-precision computation of these integrals is also
+available; see completeEllipticIntegralsKE(). However, if you are using
+float precision there is no point in using the exact computation since
+this approximation is just as good and much faster.
+
+ at param[in] m The argument to the elliptic integrals. Must be in range [0,1]
+ although we allow for a very small amount of numerical error
+ (see @ref SimTK::SignificantReal "SignificantReal") that might
+ put m outside that range.
+ at return A std::pair p from which K(m)=p.first and E(m)=p.second.
+
+You can find the approximating formula we're using in ref. 2, sections
+17.3.34 and 17.3.36, pp. 591-2, or you can look at the code for this function
+(in the header file). The formulas are accurate to 2e-8 over the full [0-1]
+argument range, according to the authors. I checked our implementation against
+Matlab's ellipke() function and the results are very good, providing at
+least 7 correct digits for a range of sample values.
+
+The cost is about 90 flops.
+
+ at see completeEllipticIntegralsKE()
+ at see @ref EllipticIntegralsGroup "Elliptic integrals"
+
+<h3>References</h3>
+(1) Antoine, Visa, Sauvey, Abba. "Approximate analytical model for
+Hertzian Elliptical Contact Problems", ASME J. Tribology 128:660, 2006.
+
+(2) Abramovitz, Stegun, eds. Handbook of Mathematical Functions with
+Formulas, Graphs, and Mathematical Tables, Dover, NY, 1972.
+**/
+inline std::pair<double,double>
+approxCompleteEllipticIntegralsKE(double m)
+{ return approxCompleteEllipticIntegralsKE_T<double>(m); }
+/** This is the single precision (float) version of the approximate calculation
+of elliptic integrals, still yielding about 7 digits of accuracy even
+though all calculations are done in float precision.
+ at see approxCompleteEllipticIntegralsKE(double)
+ at see @ref EllipticIntegralsGroup "Elliptic integrals" **/
+inline std::pair<float,float>
+approxCompleteEllipticIntegralsKE(float m)
+{ return approxCompleteEllipticIntegralsKE_T<float>(m); }
+/** This integer overload is present to prevent ambiguity; it converts its
+argument to double precision and then calls
+approxCompleteEllipticIntegralsKE(double). Note that the only legal values
+here are 0 and 1.
+ at see approxCompleteEllipticIntegralsKE(double)
+ at see @ref EllipticIntegralsGroup "Elliptic integrals" **/
+inline std::pair<double,double>
+approxCompleteEllipticIntegralsKE(int m)
+{ return approxCompleteEllipticIntegralsKE_T<double>((double)m); }
+
+
+/** @cond **/ // Doxygen should skip this template helper function
+template <class T> // float or double
+static inline std::pair<T,T> completeEllipticIntegralsKE_T(T m) {
+ const T SignificantReal = NTraits<T>::getSignificant();
+ const T TenEps = 10*NTraits<T>::getEps();
+ const T Infinity = NTraits<T>::getInfinity();
+ const T PiOver2 = NTraits<T>::getPi() / 2;
+
+ // Allow a little slop in the argument since it may have resulted
+ // from a numerical operation that gave 0 or 1 plus or minus
+ // roundoff noise.
+ SimTK_ERRCHK1_ALWAYS(-SignificantReal < m && m < 1+SignificantReal,
+ "completeEllipticIntegralsKE()",
+ "Argument m (%g) is outside the legal range [0,1].", (double)m);
+ if (m >= 1) return std::make_pair(Infinity, (T)1);
+ if (m <= 0) return std::make_pair(PiOver2, PiOver2);
+
+ const T k = std::sqrt(1-m); // ~25 flops
+ T v1=1, w1=k, c1=1, d1=k*k; // initialize iteration
+ do { // ~50 flops per iteration
+ T v2 = (v1+w1)/2;
+ T w2 = std::sqrt(v1*w1);
+ T c2 = (c1+d1)/2;
+ T d2 = (w1*c1+v1*d1)/(v1+w1);
+ v1=v2; w1=w2; c1=c2; d1=d2;
+ } while(std::abs(v1-w1) >= TenEps);
+
+ const T K = PiOver2/v1; // ~20 flops
+ const T E = K*c1;
+
+ return std::make_pair(K,E);
+}
+/** @endcond **/
+
+/** Given 0<=m<=1, return complete elliptic integrals of the first and
+second kinds, K(m) and E(m), calculated to (roughly) machine precision
+(float or double).\ See @ref EllipticIntegralsGroup "Elliptic integrals"
+for a discussion.
+
+Note that we also provide a faster approximate method for calculating these
+functions (see approxCompleteEllipticIntegralsKE()). The approximate method
+is good enough for many scientific and engineering applications and is always
+preferred if you are using float precision.
+
+ at param[in] m The argument to the elliptic integrals. Must be in range [0,1]
+ although we allow for a very small amount of numerical error
+ (see @ref SimTK::SignificantReal "SignificantReal") that
+ might put m outside that range.
+ at return A std::pair p from which K(m)=p.first and E(m)=p.second.
+
+Cost here is about 50 + 50*n flops, where n is the number of iterations
+required. The number of iterations n you can expect to get a
+double-precision result is 4 for a:b < 2, 5 for a:b < 20, 6 for a:b < 1000,
+7 after that; for single-precision it will take one fewer iterations.
+In flops that's 250, 300, 350, 400 -- 300 will be typical for double,
+250 for float.
+ at see approxCompleteEllipticIntegralsKE()
+ at see @ref EllipticIntegralsGroup "Elliptic integrals" **/
+inline std::pair<double,double> completeEllipticIntegralsKE(double m)
+{ return completeEllipticIntegralsKE_T<double>(m); }
+/** This is the single precision (float) version of the machine-precision
+calculation of elliptic integrals, providing accuracy to float precision
+(about 7 digits) which is no better than you'll get with the much faster
+approximate version, so use that instead!
+ at see approxCompleteEllipticIntegralsKE()
+ at see completeEllipticIntegralsKE(double)
+ at see @ref EllipticIntegralsGroup "Elliptic integrals" **/
+inline std::pair<float,float> completeEllipticIntegralsKE(float m)
+{ return completeEllipticIntegralsKE_T<float>(m); }
+/** This integer overload is present to prevent ambiguity; it converts its
+argument to double precision and then calls
+completeEllipticIntegralsKE(double). Note that the only legal values
+here are 0 and 1.
+ at see completeEllipticIntegralsKE(double)
+ at see @ref EllipticIntegralsGroup "Elliptic integrals" **/
+inline std::pair<double,double> completeEllipticIntegralsKE(int m)
+{ return completeEllipticIntegralsKE_T<double>((double)m); }
+
+/*@}*/
+
+} // namespace SimTK
+
+#endif //SimTK_SIMMATRIX_SCALAR_H_
diff --git a/SimTKcommon/Scalar/include/SimTKcommon/internal/CompositeNumericalTypes.h b/SimTKcommon/Scalar/include/SimTKcommon/internal/CompositeNumericalTypes.h
new file mode 100644
index 0000000..4a0d743
--- /dev/null
+++ b/SimTKcommon/Scalar/include/SimTKcommon/internal/CompositeNumericalTypes.h
@@ -0,0 +1,275 @@
+#ifndef SimTK_SIMMATRIX_COMPOSITE_NUMERICAL_TYPES_H_
+#define SimTK_SIMMATRIX_COMPOSITE_NUMERICAL_TYPES_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * The purpose of the CNT<T> class is to hide the differences between
+ * built-in numerical types and composite ones like Vec<3>. We can
+ * decorate all the composite types with whatever information we
+ * need, but we cannot add to the built-in types in the same way.
+ * So we define a templatized class CNT<T>, where the template
+ * parameter can be any composite numerical type whether built in or
+ * composite. Then CNT members are used to access information about
+ * class T. When T is a built-in, that information comes from
+ * specializations of CNT<T> for CNT<float>, CNT<std::complex<double>>,
+ * etc. When T is composite, CNT<T> acts as a pass-through to allow
+ * the composite type to provide its own information.
+ *
+ * Here is the information that must be provided by a SimTK Composite
+ * Numerical Type T (or faked up for built-ins). Some of these are
+ * given friendly names and documented since they are useful in
+ * user code.
+ * <pre>
+ *
+ * Type Meaning
+ * ---------------- --------------------------------------------------------
+ *
+ * TNeg same shape as T, but elements are negated
+ * TReal same shape as T, but with real elements
+ * TImag same shape as T, with real elements from imaginary part
+ * TComplex same shape as T, but with Complex or conjugate elements
+ * THerm transpose of T, with Hermitian transposed elements
+ * TPosTrans positional transpose of T; i.e., elements not transposed
+ * TSqHermT type of ~T*T (default vector & matrix square; symmetric)
+ * TSqTHerm type of T*~T (row square; symmetric)
+ *
+ * Scalar the underlying scalar type (see below)
+ * ScalarNormSq type of the "conjugate square" ~s*s of underlying scalar
+ * (always real)
+ *
+ * Substitute<E>::Type
+ * A CNT of the same shape and container type as this one,
+ * but with elements of type E instead of ElementType.
+ * Special case: if this CNT is a scalar then
+ * Substitute<E>::Type just returns E.
+ * Result<RHS>::Mul (Dvd,Add,Sub)
+ * The type of the result of T op RHS, where RHS is *any* CNT
+ *
+ * ENUMS (all sizes are in units of T's elements)
+ *
+ * NRows logical num rows in type T (i.e., num elements in a column)
+ * NCols logical number of columns in type T
+ * RowSpacing num elements from one row to the next (default 1)
+ * ColSpacing num elements from one col to the next (default NRows for Mat)
+ * NPackedElements minimum num elements it would take to store this data
+ * NActualElements num elements covered by T due to element spacing
+ * NActualScalars NActualElements * CNT<ElementType>::NActualScalars. This
+ * should be the physical spacing between array elements
+ * in an array containing this kind of CNT. Our big
+ * Matrix/Vector types guarantee this packing.
+ * </pre>
+ *
+ * @verbatim
+ *
+ * The Scalar Types
+ * ----------------
+ * Here is a complete taxonomy of the scalar types we support.
+ *
+ * <scalar> ::= quantity< unitType, <unitlessScalar> >
+ * <unitlessScalar> ::= <number> | negator< <number> >
+ * <number> ::= <standard> | <conjugate>
+ * <standard> ::= <real> | <complex>
+ *
+ * <real> ::= float | double | long double
+ * <complex> ::= std::complex< <real> >
+ * <conjugate> ::= SimTK::conjugate< <real> >
+ *
+ * @endverbatim
+ *
+ * With this in hand, we can build a clean facility in which scalars,
+ * vectors, matrices, vectors of vectors, matrices of vectors of
+ * matrices, etc. can all be treated uniformly.
+ */
+
+#include "SimTKcommon/internal/common.h"
+
+namespace SimTK {
+
+// These are CNT "depths". 0 means the corresponding CNT is a scalar,
+// 1 means it is a composite with scalar elements, 2 means a composite
+// with composite elements, and 3 means a composite with depth-2
+// composite elements. Beyond that the user will have to diambiguate
+// operations by using named routines rather than operators.
+enum {
+ SCALAR_DEPTH = 0,
+ SCALAR_COMPOSITE_DEPTH = 1,
+ COMPOSITE_COMPOSITE_DEPTH = 2,
+ COMPOSITE_3_DEPTH = 3,
+ MAX_RESOLVED_DEPTH = COMPOSITE_3_DEPTH
+};
+
+/**
+ * Specialized information about Composite Numerical Types which
+ * allows us to define appropriate templatized classes using them.
+ * Transpose is particularly tricky -- we insist on Hermitian
+ * transpose meaning the elements must also be transposed and
+ * complex subelements must be conjugated.
+ *
+ * This class exists because the built-in scalar types don't
+ * have the members we need. CNT<> is specialized for those types
+ * only; it is just a pass-through for the rest. The idea
+ * is to capture everything that has to be specialized here rather
+ * than in the template classes which use these types.
+ */
+template <class K> class CNT : private K {
+public:
+ typedef K T;
+ typedef typename K::TNeg TNeg;
+ typedef typename K::TWithoutNegator TWithoutNegator;
+ typedef typename K::TReal TReal;
+ typedef typename K::TImag TImag;
+ typedef typename K::TComplex TComplex;
+ typedef typename K::THerm THerm;
+ typedef typename K::TPosTrans TPosTrans;
+ typedef typename K::TSqHermT TSqHermT;
+ typedef typename K::TSqTHerm TSqTHerm;
+ typedef typename K::TElement TElement;
+ typedef typename K::TRow TRow; // type of a row or column
+ typedef typename K::TCol TCol;
+
+ // These are the results of calculations and should be packed regardless
+ // of the spacing of this CNT.
+ typedef typename K::TSqrt TSqrt; // also turns unit^2 to unit
+ typedef typename K::TAbs TAbs;
+ typedef typename K::TStandard TStandard; // packed, StdNumbers
+ typedef typename K::TInvert TInvert; // also turns units into 1/units
+ typedef typename K::TNormalize TNormalize; // TODO: what effect on units?
+
+ typedef typename K::Scalar Scalar; // quantity< units, <unitlessScalar> >
+ typedef typename K::ULessScalar ULessScalar; // <number> or negator<number>
+ typedef typename K::Number Number; // <real>, <complex> or <conjugate>
+ typedef typename K::StdNumber StdNumber; // <real>, <complex>
+ typedef typename K::Precision Precision; // float, double, long double
+
+ typedef typename K::ScalarNormSq ScalarNormSq; // type of conjugate square of underlying scalar or
+ // numeric value (squares the units too)
+
+ template <class P> struct Result {
+ typedef typename K::template Result<P>::Mul Mul;
+ typedef typename K::template Result<P>::Dvd Dvd;
+ typedef typename K::template Result<P>::Add Add;
+ typedef typename K::template Result<P>::Sub Sub;
+ };
+
+ // Shape-preserving element substitution
+ template <class P> struct Substitute {
+ typedef typename K::template Substitute<P>::Type Type;
+ };
+
+ enum {
+ NRows = K::NRows,
+ NCols = K::NCols,
+ RowSpacing = K::RowSpacing,
+ ColSpacing = K::ColSpacing,
+ NPackedElements = K::NPackedElements,
+ NActualElements = K::NActualElements,
+ NActualScalars = K::NActualScalars,
+ ImagOffset = K::ImagOffset,
+ RealStrideFactor = K::RealStrideFactor,
+ ArgDepth = K::ArgDepth,
+ IsScalar = K::IsScalar, // scalar with units, real, complex, conjugate, negator
+ IsULessScalar = K::IsULessScalar, // real, complex, conjugate, negator
+ IsNumber = K::IsNumber, // real, complex, conjugate
+ IsStdNumber = K::IsStdNumber, // real, complex
+ IsPrecision = K::IsPrecision, // real (float, double, long double)
+ SignInterpretation = K::SignInterpretation // 1 normally, -1 if elements are negated
+ };
+
+ static const Scalar* getData(const T& t) { return t.getData(); }
+ static Scalar* updData(T& t) { return t.updData(); }
+
+ static const TReal& real(const T& t) { return t.real(); }
+ static TReal& real(T& t) { return t.real(); }
+ static const TImag& imag(const T& t) { return t.imag(); }
+ static TImag& imag(T& t) { return t.imag(); }
+
+ // We expect to be able to negate and transpose (hermitian or
+ // positional) with just type casting; no need for help from class
+ // K except to tell us the appropriate types.
+ static const TNeg& negate(const T& t)
+ { return reinterpret_cast<const TNeg&>(t); }
+ static TNeg& negate(T& t)
+ { return reinterpret_cast<TNeg&>(t); }
+
+ static const THerm& transpose(const K& t)
+ { return reinterpret_cast<const THerm&>(t); }
+ static THerm& transpose(K& t)
+ { return reinterpret_cast<THerm&>(t); }
+
+ static const TPosTrans& positionalTranspose(const K& t)
+ { return reinterpret_cast<const TPosTrans&>(t); }
+ static TPosTrans& positionalTranspose(K& t)
+ { return reinterpret_cast<TPosTrans&>(t); }
+
+ // If the underlying scalars of this CNT are negator<N> for some numeric type N,
+ // this method removes the negator<>, effectively negating the entire CNT. You
+ // can still deal with the sign correctly by using the above enum SignInterpretation
+ // which will be -1 in that case, 1 if there was no negator<> to remove. Note:
+ // I'm not talking about TWithoutNegator::SignInterpretation -- that one is guaranteed
+ // to be 1! T::SignInterpretation is the one you want.
+ static const TWithoutNegator& castAwayNegatorIfAny(const T& t)
+ {return reinterpret_cast<const TWithoutNegator&>(t);}
+ static TWithoutNegator& updCastAwayNegatorIfAny(T& t)
+ {return reinterpret_cast<TWithoutNegator&>(t);}
+
+ static ScalarNormSq scalarNormSqr(const K& t) {return t.scalarNormSqr();}
+
+ static TSqrt sqrt(const K& t) {return t.sqrt();}
+ static TAbs abs(const K& t) {return t.abs();}
+ static TStandard standardize(const K& t) {return t.standardize();}
+ static TNormalize normalize(const K& t) {return t.normalize();}
+ static TInvert invert(const K& t) {return t.invert();}
+
+ static K getInfinity() {return K::getInfinity();}
+ static K getNaN() {return K::getNaN();}
+
+ /// This is true if any element contains a NaN anywhere.
+ static bool isNaN(const K& t) {return t.isNaN();}
+ /// This is true if at least one element contains a +Infinity or -Infinity
+ /// and no element contains a NaN.
+ static bool isInf(const K& t) {return t.isInf();}
+ /// This is true only if no element has any entry that it NaN or Infinity.
+ static bool isFinite(const K& t) {return t.isFinite();}
+
+ /// CNTs are expected to support an "==" operator for exact, bitwise equality.
+ /// This method implements approximate, numerical equality. For scalar types,
+ /// this should boil down to the isNumericallyEqual() scalar method. For 2D composite
+ /// types, the default tolerance should be loosened from the element's default
+ /// tolerance by the shorter of the two dimensions. For example, if element E's
+ /// default numerical tolerance is tol, then a Mat<3,5,E>'s default tolerance
+ /// should be 3*tol.
+ template <class K2> static bool
+ isNumericallyEqual(const K& t1, const K2& t2)
+ { return t1.isNumericallyEqual(t2);}
+ template <class K2> static bool
+ isNumericallyEqual(const K& t1, const K2& t2, double tol)
+ { return t1.isNumericallyEqual(t2,tol);}
+ static double getDefaultTolerance() {return K::getDefaultTolerance();}
+
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATRIX_COMPOSITE_NUMERICAL_TYPES_H_
diff --git a/SimTKcommon/Scalar/include/SimTKcommon/internal/NTraits.h b/SimTKcommon/Scalar/include/SimTKcommon/internal/NTraits.h
new file mode 100644
index 0000000..19bab89
--- /dev/null
+++ b/SimTKcommon/Scalar/include/SimTKcommon/internal/NTraits.h
@@ -0,0 +1,1098 @@
+#ifndef SimTK_SIMMATRIX_NTRAITS_H_
+#define SimTK_SIMMATRIX_NTRAITS_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This file contains classes and typedefs needed to provide uniform handling of
+ * floating point numeric values. There are three numeric types: real, complex,
+ * conjugate and each comes in float, double, and long double precision. Each of
+ * these may be modified by a negator, which does not change the in-memory
+ * \e representation but negates the \e interpretation. Thus there are 18
+ * distinct scalar types: 3 precisions each of real, complex, and conjugate and
+ * their negators.
+ * @verbatim
+ * The Scalar Types
+ * ----------------
+ * Here is a complete taxonomy of the scalar types we support.
+ *
+ * <scalar> ::= <number> | negator< <number> >
+ * <number> ::= <standard> | <conjugate>
+ * <standard> ::= <real> | <complex>
+ *
+ * <real> ::= float | double | long double
+ * <complex> ::= complex< <real> >
+ * <conjugate> ::= conjugate< <real> >
+ * @endverbatim
+ */
+
+#include "SimTKcommon/Constants.h"
+#include "SimTKcommon/internal/CompositeNumericalTypes.h"
+
+#include <cstddef>
+#include <cassert>
+#include <complex>
+#include <iostream>
+#include <limits>
+
+using std::complex;
+
+namespace SimTK {
+
+// This is the 3rd type of number, conjugate. It is like complex except that
+// the represented value is the conjugate of the value represented by a complex
+// number containing the same bit pattern. That is, complex and conjugate both
+// contain two real numbers, re and im, with complex(re,im) meaning
+// (re + im*i) while conjugate(re,im) means (re - im*i). It is guaranteed that
+// our conjugate type and complex type have identical sizes and representations.
+// Together, these defininitions and guarantees permit conjugation
+// to be done by reinterpretation rather than be computation.
+template <class R> class conjugate; // Only defined for float, double, long double
+
+// Specializations of this class provide information about Composite Numerical
+// Types in the style of std::numeric_limits<T>. It is specialized for the
+// numeric types but can be invoked on any composite numerical type as well.
+template <class T> class CNT;
+
+// NTraits provides information like CNT, but for numeric types only.
+template <class N> class NTraits; // allowed only for N=<number>
+template <class R> class NTraits< complex<R> >;
+template <class R> class NTraits< conjugate<R> >;
+template <> class NTraits<float>;
+template <> class NTraits<double>;
+template <> class NTraits<long double>;
+
+// This is an adaptor for numeric types which negates the apparent values. A
+// negator<N> has exactly the same internal representation as a numeric value N,
+// but it is to be interpreted has having the negative of the value it would
+// have if interpreted as an N. This permits negation to be done by
+// reinterpretation rather than compuation. A full set of arithmetic operators
+// are provided involving negator<N>'s and N's. Sometimes we can save an op or
+// two this way. For example negator<N>*negator<N> can be performed as an N*N
+// since the negations cancel, and we saved two floating point negations.
+template <class N> class negator; // Only defined for numbers
+
+// This is here so we can provide references to 0's when needed, e.g.
+// when returning the imaginary part of a real number. These are local to
+// the compilation unit, so the returned addresses will differ in different
+// files. There are enough zeroes here for the widest number,
+// complex<long double> (or conjugate<long double>).
+static const complex<long double> zeroes(0);
+
+/// This class is specialized for all 36 combinations of standard types
+/// (that is, real and complex types in each of three precisions)
+/// and has typedefs "Type" which is the appropriate "widened"
+/// type for use when R1 & R2 appear in an operation together, and
+/// "Precision" which is the wider precision (float,double,long double).
+/// For example, if R1=complex< float > and R2=long double, Widest<R1,R2>::Type is
+/// complex<long double> and Widest<R1,R2>::Precision is long double.
+template <class R1, class R2> struct Widest {/* Only defined for built-ins. */};
+template <> struct Widest<float,float> {typedef float Type; typedef float Precision;};
+template <> struct Widest<float,double> {typedef double Type; typedef double Precision;};
+template <> struct Widest<float,long double> {typedef long double Type; typedef long double Precision;};
+template <> struct Widest<double,float> {typedef double Type; typedef double Precision;};
+template <> struct Widest<double,double> {typedef double Type; typedef double Precision;};
+template <> struct Widest<double,long double> {typedef long double Type; typedef long double Precision;};
+template <> struct Widest<long double,float> {typedef long double Type; typedef long double Precision;};
+template <> struct Widest<long double,double> {typedef long double Type; typedef long double Precision;};
+template <> struct Widest<long double,long double> {typedef long double Type; typedef long double Precision;};
+template <class R1, class R2> struct Widest< complex<R1>,complex<R2> > {
+ typedef complex< typename Widest<R1,R2>::Type > Type;
+ typedef typename Widest<R1,R2>::Precision Precision;
+};
+template <class R1, class R2> struct Widest< complex<R1>,R2 > {
+ typedef complex< typename Widest<R1,R2>::Type > Type;
+ typedef typename Widest<R1,R2>::Precision Precision;
+};
+template <class R1, class R2> struct Widest< R1,complex<R2> > {
+ typedef complex< typename Widest<R1,R2>::Type > Type;
+ typedef typename Widest<R1,R2>::Precision Precision;
+};
+
+/// This class is specialized for all 36 combinations of standard types
+/// (that is, real and complex types in each of three precisions)
+/// and has typedefs "Type" which is the appropriate "narrowed"
+/// type for use when R1 & R2 appear in an operation together where the
+/// result must be of the narrower precision, and "Precision" which is
+/// the expected precision of the result (float,
+/// double, long double). For example, if R1=complex< double > and R2=float,
+/// Narrowest<R1,R2>::Type is complex< float > and Narrowest<R1,R2>::Precision
+/// is float.
+template <class R1, class R2> struct Narrowest {/* Only defined for built-ins. */};
+template <> struct Narrowest<float,float> {typedef float Type; typedef float Precision;};
+template <> struct Narrowest<float,double> {typedef float Type; typedef float Precision;};
+template <> struct Narrowest<float,long double> {typedef float Type; typedef float Precision;};
+template <> struct Narrowest<double,float> {typedef float Type; typedef float Precision;};
+template <> struct Narrowest<double,double> {typedef double Type; typedef double Precision;};
+template <> struct Narrowest<double,long double> {typedef double Type; typedef double Precision;};
+template <> struct Narrowest<long double,float> {typedef float Type; typedef float Precision;};
+template <> struct Narrowest<long double,double> {typedef double Type; typedef double Precision;};
+template <> struct Narrowest<long double,long double> {typedef long double Type; typedef long double Precision;};
+template <class R1, class R2> struct Narrowest< complex<R1>,complex<R2> > {
+ typedef complex< typename Narrowest<R1,R2>::Type > Type;
+ typedef typename Narrowest<R1,R2>::Precision Precision;
+};
+template <class R1, class R2> struct Narrowest< complex<R1>,R2 > {
+ typedef complex< typename Narrowest<R1,R2>::Type > Type;
+ typedef typename Narrowest<R1,R2>::Precision Precision;
+};
+template <class R1, class R2> struct Narrowest< R1,complex<R2> > {
+ typedef complex< typename Narrowest<R1,R2>::Type > Type;
+ typedef typename Narrowest<R1,R2>::Precision Precision;
+};
+
+/// RTraits is a helper class for NTraits.
+template <class R> class RTraits {/* Only defined for real types */};
+template <> class RTraits<float> {
+public:
+ /// Attainable accuracy at this precision.
+ static const float& getEps() {static const float c=std::numeric_limits<float>::epsilon(); return c;}
+ /// What multiple of attainable accuracy do we consider significant?
+ static const float& getSignificant() {static const float c=std::pow(getEps(), 0.875f); return c;}
+ /// The default numerical error tolerance is always given in double precision.
+ static double getDefaultTolerance() {return (double)getSignificant();}
+};
+template <> class RTraits<double> {
+public:
+ static const double& getEps() {static const double c=std::numeric_limits<double>::epsilon(); return c;}
+ static const double& getSignificant() {static const double c=std::pow(getEps(), 0.875); return c;}
+ static double getDefaultTolerance() {return getSignificant();}
+};
+template <> class RTraits<long double> {
+public:
+ static const long double& getEps() {static const long double c=std::numeric_limits<long double>::epsilon(); return c;}
+ static const long double& getSignificant() {static const long double c=std::pow(getEps(), 0.875L); return c;}
+ static double getDefaultTolerance() {return (double)getSignificant();}
+};
+
+/**
+ * @defgroup isNaN isNaN()
+ * @ingroup ScalarFunctions
+ *
+ * isNaN(x) provides a reliable way to determine if x is one of the "not a
+ * number" floating point forms. Comparing x==NaN does not work because any
+ * relational operation involving NaN always return false, even (NaN==NaN)!
+ * This routine is specialized for all SimTK scalar types:
+ * - float, double, long double
+ * - std::complex<P> (P is one of the above precisions)
+ * - SimTK::conjugate<P>
+ * - SimTK::negator<T> (T is any of the above)
+ *
+ * For complex and conjugate types, isNaN() returns true if either the real or
+ * imaginary part or both are NaN.
+ */
+// See negator.h for isNaN() applied to negated scalars.
+//@{
+inline bool isNaN(const float& x) {return std::isnan(x);}
+inline bool isNaN(const double& x) {return std::isnan(x);}
+inline bool isNaN(const long double& x) {return std::isnan(x);}
+
+template <class P> inline bool
+isNaN(const std::complex<P>& x)
+{ return isNaN(x.real()) || isNaN(x.imag());}
+
+template <class P> inline bool
+isNaN(const conjugate<P>& x)
+{ return isNaN(x.real()) || isNaN(x.negImag());}
+//@}
+
+/**
+ * @defgroup isFinite isFinite()
+ * @ingroup ScalarFunctions
+ *
+ * isFinite(x) provides a reliable way to determine if x is a "normal"
+ * floating point number, meaning not a NaN or +/- Infinity.
+ * This routine is specialized for all SimTK scalar types:
+ * float, double, std::complex<P>, SimTK::conjugate<P>,
+ * and SimTK::negator<T>, where T is any of the above. For
+ * complex and conjugate types, isFinite() returns true if
+ * the real and imaginary parts are both finite.
+ */
+// See negator.h for isFinite() applied to negated scalars.
+//@{
+inline bool isFinite(const float& x) {return std::isfinite(x);}
+inline bool isFinite(const double& x) {return std::isfinite(x);}
+inline bool isFinite(const long double& x) {return std::isfinite(x);}
+
+template <class P> inline bool
+isFinite(const std::complex<P>& x)
+{ return isFinite(x.real()) && isFinite(x.imag());}
+
+template <class P> inline bool
+isFinite(const conjugate<P>& x)
+{ return isFinite(x.real()) && isFinite(x.negImag());}
+//@}
+
+/**
+ * @defgroup isInf isInf()
+ * @ingroup ScalarFunctions
+ *
+ * isInf(x) provides a reliable way to determine if x is one of
+ * the two infinities (either negative or positive).
+ * This routine is specialized for all SimTK scalar types:
+ * float, double std::complex<P>, SimTK::conjugate<P>,
+ * and SimTK::negator<T>, where T is any of the above. For
+ * complex and conjugate types, isInf() returns true if both
+ * components are infinite, or one is infinite and the other
+ * finite. That is, isInf() will never return true if one
+ * component is NaN.
+ */
+// See negator.h for isInf() applied to negated scalars.
+//@{
+inline bool isInf(const float& x) {return std::isinf(x);}
+inline bool isInf(const double& x) {return std::isinf(x);}
+inline bool isInf(const long double& x) {return std::isinf(x);}
+
+template <class P> inline bool
+isInf(const std::complex<P>& x) {
+ return (isInf(x.real()) && !isNaN(x.imag()))
+ || (isInf(x.imag()) && !isNaN(x.real()));
+}
+
+template <class P> inline bool
+isInf(const conjugate<P>& x) {
+ return (isInf(x.real()) && !isNaN(x.negImag()))
+ || (isInf(x.negImag()) && !isNaN(x.real()));
+}
+//@}
+
+/**
+ * @defgroup isNumericallyEqual isNumericallyEqual()
+ * @ingroup ScalarFunctions
+ *
+ * isNumericallyEqual(x,y) compares two scalar types using a tolerance (default
+ * or explicitly specified) and returns true if they are close enough.
+ *
+ * The default tolerance used is the NTraits<P>::getSignificant() value (about
+ * 1e-14 in double precision, 1e-6 in float) for the narrower of the types being
+ * compared but you can override that. The tolerance is both a relative and
+ * absolute tolerance; for two numbers a and b and tolerance tol we compute the
+ * following condition:
+ * <pre>
+ * scale = max(|a|,|b|,1)
+ * isNumericallyEqual = |a-b| <= scale*tol
+ * </pre>
+ * For complex or conjugate numbers we insist that both the real and
+ * imaginary parts independently satisfy the above condition.
+ *
+ * \par Mixed precision
+ * We support mixed argument types here in which case the default
+ * tolerance used is the one appropriate to the \e lower-precision
+ * argument. When one argument is an integer, the default tolerance
+ * used is that of the floating point argument. Comparisons may be performed
+ * at a higher precision than the tolerance to avoid incorrect truncation; for
+ * example an int cannot generally be contained in a float so an int/float
+ * comparison is done as double/double, but to float tolerance.
+ *
+ * \par Treatment of NaN
+ * When both arguments are NaN they are considered equal here, which is different
+ * than the behavior of the IEEE-sanctioned "==" comparison for which
+ * NaN==NaN returns false. If only one argument is NaN we return false. When
+ * comparing complex or conjugate numbers the real and imaginary parts are
+ * tested separately, so (NaN,0) and (0,NaN) don't test equal despite the
+ * fact that isNaN() would return true for both of them. We don't distinguish
+ * among *types* of NaNs, though, so NaN and -NaN (if there is such a thing)
+ * will test numerically equal.
+ */
+//@{
+/// Compare two floats for approximate equality.
+inline bool isNumericallyEqual(const float& a, const float& b,
+ double tol = RTraits<float>::getDefaultTolerance())
+{ if (isNaN(a)) return isNaN(b); else if (isNaN(b)) return false;
+ const float scale = std::max(std::max(std::abs(a),std::abs(b)), 1.f);
+ return std::abs(a-b) <= scale*(float)tol; }
+/// Compare two doubles for approximate equality.
+inline bool isNumericallyEqual(const double& a, const double& b,
+ double tol = RTraits<double>::getDefaultTolerance())
+{ if (isNaN(a)) return isNaN(b); else if (isNaN(b)) return false;
+ const double scale = std::max(std::max(std::abs(a),std::abs(b)), 1.);
+ return std::abs(a-b) <= scale*tol; }
+/// Compare two long doubles for approximate equality.
+inline bool isNumericallyEqual(const long double& a, const long double& b,
+ double tol = RTraits<long double>::getDefaultTolerance())
+{ if (isNaN(a)) return isNaN(b); else if (isNaN(b)) return false;
+ const long double scale = std::max(std::max(std::abs(a),std::abs(b)), 1.L);
+ return std::abs(a-b) <= scale*(long double)tol; }
+
+/// Compare a float and a double for approximate equality at float precision.
+inline bool isNumericallyEqual(const float& a, const double& b,
+ double tol = RTraits<float>::getDefaultTolerance())
+{ return isNumericallyEqual((double)a, b, tol); }
+/// Compare a float and a double for approximate equality at float precision.
+inline bool isNumericallyEqual(const double& a, const float& b,
+ double tol = RTraits<float>::getDefaultTolerance())
+{ return isNumericallyEqual(a, (double)b, tol); }
+/// Compare a float and a long double for approximate equality at float precision.
+inline bool isNumericallyEqual(const float& a, const long double& b,
+ double tol = RTraits<float>::getDefaultTolerance())
+{ return isNumericallyEqual((long double)a, b, tol); }
+/// Compare a float and a long double for approximate equality at float precision.
+inline bool isNumericallyEqual(const long double& a, const float& b,
+ double tol = RTraits<float>::getDefaultTolerance())
+{ return isNumericallyEqual(a, (long double)b, tol); }
+/// Compare a double and a long double for approximate equality at double precision.
+inline bool isNumericallyEqual(const double& a, const long double& b,
+ double tol = RTraits<double>::getDefaultTolerance())
+{ return isNumericallyEqual((long double)a, b, tol); }
+/// Compare a double and a long double for approximate equality at double precision.
+inline bool isNumericallyEqual(const long double& a, const double& b,
+ double tol = RTraits<double>::getDefaultTolerance())
+{ return isNumericallyEqual(a, (long double)b, tol); }
+
+/// %Test a float for approximate equality to an integer.
+inline bool isNumericallyEqual(const float& a, int b,
+ double tol = RTraits<float>::getDefaultTolerance())
+{ return isNumericallyEqual(a, (double)b, tol); }
+/// %Test a float for approximate equality to an integer.
+inline bool isNumericallyEqual(int a, const float& b,
+ double tol = RTraits<float>::getDefaultTolerance())
+{ return isNumericallyEqual((double)a, b, tol); }
+/// %Test a double for approximate equality to an integer.
+inline bool isNumericallyEqual(const double& a, int b,
+ double tol = RTraits<double>::getDefaultTolerance())
+{ return isNumericallyEqual(a, (double)b, tol); }
+/// %Test a double for approximate equality to an integer.
+inline bool isNumericallyEqual(int a, const double& b,
+ double tol = RTraits<double>::getDefaultTolerance())
+{ return isNumericallyEqual((double)a, b, tol); }
+/// %Test a long double for approximate equality to an integer.
+inline bool isNumericallyEqual(const long double& a, int b,
+ double tol = RTraits<long double>::getDefaultTolerance())
+{ return isNumericallyEqual(a, (long double)b, tol); }
+/// %Test a long double for approximate equality to an integer.
+inline bool isNumericallyEqual(int a, const long double& b,
+ double tol = RTraits<long double>::getDefaultTolerance())
+{ return isNumericallyEqual((long double)a, b, tol); }
+
+/// Compare two complex numbers for approximate equality, using the numerical
+/// accuracy expectation of the narrower of the two precisions in the case of mixed
+/// precision.
+template <class P, class Q>
+inline bool isNumericallyEqual
+ ( const std::complex<P>& a, const std::complex<Q>& b,
+ double tol = RTraits<typename Narrowest<P,Q>::Precision>::getDefaultTolerance())
+{ return isNumericallyEqual(a.real(),b.real(),tol)
+ && isNumericallyEqual(a.imag(),b.imag(),tol); }
+
+/// Compare two conjugate numbers for approximate equality, using the numerical
+/// accuracy expectation of the narrower of the two precisions in the case of mixed
+/// precision.
+template <class P, class Q>
+inline bool isNumericallyEqual
+ ( const conjugate<P>& a, const conjugate<Q>& b,
+ double tol = RTraits<typename Narrowest<P,Q>::Precision>::getDefaultTolerance())
+{ return isNumericallyEqual(a.real(),b.real(),tol)
+ && isNumericallyEqual(a.imag(),b.imag(),tol); }
+
+/// Compare a complex and a conjugate number for approximate equality, using the
+/// numerical accuracy expectation of the narrower of the two precisions in the
+/// case of mixed precision.
+template <class P, class Q>
+inline bool isNumericallyEqual
+ ( const std::complex<P>& a, const conjugate<Q>& b,
+ double tol = RTraits<typename Narrowest<P,Q>::Precision>::getDefaultTolerance())
+{ return isNumericallyEqual(a.real(),b.real(),tol)
+ && isNumericallyEqual(a.imag(),b.imag(),tol); }
+
+/// Compare a complex and a conjugate number for approximate equality, using the
+/// numerical accuracy expectation of the narrower of the two precisions in the
+/// case of mixed precision.
+template <class P, class Q>
+inline bool isNumericallyEqual
+ ( const conjugate<P>& a, const std::complex<Q>& b,
+ double tol = RTraits<typename Narrowest<P,Q>::Precision>::getDefaultTolerance())
+{ return isNumericallyEqual(a.real(),b.real(),tol)
+ && isNumericallyEqual(a.imag(),b.imag(),tol); }
+
+/// %Test whether a complex number is approximately equal to a particular real float.
+template <class P> inline bool
+isNumericallyEqual(const std::complex<P>& a, const float& b,
+ double tol = RTraits<float>::getDefaultTolerance())
+{ return isNumericallyEqual(a.real(),b,tol) && isNumericallyEqual(a.imag(),0.f,tol); }
+/// %Test whether a complex number is approximately equal to a particular real float.
+template <class P> inline bool
+isNumericallyEqual(const float& a, const std::complex<P>& b,
+ double tol = RTraits<float>::getDefaultTolerance())
+{ return isNumericallyEqual(b,a,tol); }
+/// %Test whether a complex number is approximately equal to a particular real double.
+template <class P> inline bool
+isNumericallyEqual(const std::complex<P>& a, const double& b,
+ double tol = RTraits<typename Narrowest<P,double>::Precision>::getDefaultTolerance())
+{ return isNumericallyEqual(a.real(),b,tol) && isNumericallyEqual(a.imag(),0.,tol); }
+/// %Test whether a complex number is approximately equal to a particular real double.
+template <class P> inline bool
+isNumericallyEqual(const double& a, const std::complex<P>& b,
+ double tol = RTraits<typename Narrowest<P,double>::Precision>::getDefaultTolerance())
+{ return isNumericallyEqual(b,a,tol); }
+/// %Test whether a complex number is approximately equal to a particular real long double.
+template <class P> inline bool
+isNumericallyEqual(const std::complex<P>& a, const long double& b,
+ double tol = RTraits<P>::getDefaultTolerance())
+{ return isNumericallyEqual(a.real(),b,tol) && isNumericallyEqual(a.imag(),0.L,tol); }
+/// %Test whether a complex number is approximately equal to a particular real long double.
+template <class P> inline bool
+isNumericallyEqual(const long double& a, const std::complex<P>& b,
+ double tol = RTraits<P>::getDefaultTolerance())
+{ return isNumericallyEqual(b,a,tol); }
+/// %Test whether a complex number is approximately equal to a particular integer.
+template <class P> inline bool
+isNumericallyEqual(const std::complex<P>& a, int b,
+ double tol = RTraits<P>::getDefaultTolerance())
+{ typedef typename Widest<P,double>::Precision W; return isNumericallyEqual(a,(W)b,tol); }
+/// %Test whether a complex number is approximately equal to a particular integer.
+template <class P> inline bool
+isNumericallyEqual(int a, const std::complex<P>& b,
+ double tol = RTraits<P>::getDefaultTolerance())
+{ return isNumericallyEqual(b,a,tol); }
+
+/// %Test whether a conjugate number is approximately equal to a particular real float.
+template <class P> inline bool
+isNumericallyEqual(const conjugate<P>& a, const float& b,
+ double tol = RTraits<float>::getDefaultTolerance())
+{ return isNumericallyEqual(a.real(),b,tol) && isNumericallyEqual(a.imag(),0.f,tol); }
+/// %Test whether a conjugate number is approximately equal to a particular real float.
+template <class P> inline bool
+isNumericallyEqual(const float& a, const conjugate<P>& b,
+ double tol = RTraits<float>::getDefaultTolerance())
+{ return isNumericallyEqual(b,a,tol); }
+/// %Test whether a conjugate number is approximately equal to a particular real double.
+template <class P> inline bool
+isNumericallyEqual(const conjugate<P>& a, const double& b,
+ double tol = RTraits<typename Narrowest<P,double>::Precision>::getDefaultTolerance())
+{ return isNumericallyEqual(a.real(),b,tol) && isNumericallyEqual(a.imag(),0.,tol); }
+/// %Test whether a conjugate number is approximately equal to a particular real double.
+template <class P> inline bool
+isNumericallyEqual(const double& a, const conjugate<P>& b,
+ double tol = RTraits<typename Narrowest<P,double>::Precision>::getDefaultTolerance())
+{ return isNumericallyEqual(b,a,tol); }
+/// %Test whether a conjugate number is approximately equal to a particular real long double.
+template <class P> inline bool
+isNumericallyEqual(const conjugate<P>& a, const long double& b,
+ double tol = RTraits<P>::getDefaultTolerance())
+{ return isNumericallyEqual(a.real(),b,tol) && isNumericallyEqual(a.imag(),0.L,tol); }
+/// %Test whether a conjugate number is approximately equal to a particular real long double.
+template <class P> inline bool
+isNumericallyEqual(const long double& a, const conjugate<P>& b,
+ double tol = RTraits<P>::getDefaultTolerance())
+{ return isNumericallyEqual(b,a,tol); }
+/// %Test whether a conjugate number is approximately equal to a particular integer.
+template <class P> inline bool
+isNumericallyEqual(const conjugate<P>& a, int b,
+ double tol = RTraits<P>::getDefaultTolerance())
+{ typedef typename Widest<P,double>::Precision W; return isNumericallyEqual(a,(W)b,tol); }
+/// %Test whether a conjugate number is approximately equal to a particular integer.
+template <class P> inline bool
+isNumericallyEqual(int a, const conjugate<P>& b,
+ double tol = RTraits<P>::getDefaultTolerance())
+{ return isNumericallyEqual(b,a,tol); }
+
+//@}
+
+
+template <class N> class NTraits {
+ // only the specializations below are allowed
+};
+
+/// Partial specialization for complex numbers -- underlying real R is
+/// still a template parameter.
+template <class R> class NTraits< complex<R> > {
+ typedef complex<R> C;
+public:
+ typedef C T;
+ typedef negator<C> TNeg; // type of this after *cast* to its negative
+ typedef C TWithoutNegator; // type of this ignoring negator (there isn't one!)
+
+ typedef R TReal;
+ typedef R TImag;
+ typedef C TComplex;
+ typedef conjugate<R> THerm; // this is a recast
+ typedef C TPosTrans;
+ typedef R TSqHermT; // ~C*C
+ typedef R TSqTHerm; // C*~C (same)
+ typedef C TElement;
+ typedef C TRow;
+ typedef C TCol;
+
+ typedef C TSqrt;
+ typedef R TAbs;
+ typedef C TStandard; // complex is a standard type
+ typedef C TInvert; // this is a calculation, so use standard number
+ typedef C TNormalize;
+
+ typedef C Scalar;
+ typedef C ULessScalar;
+ typedef C Number;
+ typedef C StdNumber;
+ typedef R Precision;
+ typedef R ScalarNormSq;
+
+ // For complex scalar C, op result types are:
+ // Typeof(C*P) = Typeof(P*C)
+ // Typeof(C/P) = Typeof(inv(P)*C)
+ // Typeof(C+P) = Typeof(P+C)
+ // typeof(C-P) = Typeof(P::TNeg + C)
+ // These must be specialized for P=real, complex, conjugate.
+ template <class P> struct Result {
+ typedef typename CNT<P>::template Result<C>::Mul Mul;
+ typedef typename CNT< typename CNT<P>::THerm >::template Result<C>::Mul Dvd;
+ typedef typename CNT<P>::template Result<C>::Add Add;
+ typedef typename CNT< typename CNT<P>::TNeg >::template Result<C>::Add Sub;
+ };
+
+ // Shape-preserving element substitution (easy for scalars!)
+ template <class P> struct Substitute {
+ typedef P Type;
+ };
+
+ enum {
+ NRows = 1,
+ NCols = 1,
+ RowSpacing = 1,
+ ColSpacing = 1,
+ NPackedElements = 1, // not two!
+ NActualElements = 1,
+ NActualScalars = 1,
+ ImagOffset = 1,
+ RealStrideFactor = 2, // double stride when casting to real or imaginary
+ ArgDepth = SCALAR_DEPTH,
+ IsScalar = 1,
+ IsULessScalar = 1,
+ IsNumber = 1,
+ IsStdNumber = 1,
+ IsPrecision = 0,
+ SignInterpretation = 1 // after cast to Number, what is the sign?
+ };
+ static const T* getData(const T& t) { return &t; }
+ static T* updData(T& t) { return &t; }
+ static const R& real(const T& t) { return (reinterpret_cast<const R*>(&t))[0]; }
+ static R& real(T& t) { return (reinterpret_cast<R*>(&t))[0]; }
+ static const R& imag(const T& t) { return (reinterpret_cast<const R*>(&t))[1]; }
+ static R& imag(T& t) { return (reinterpret_cast<R*>(&t))[1]; }
+
+ static const TNeg& negate(const T& t) {return reinterpret_cast<const TNeg&>(t);}
+ static TNeg& negate(T& t) {return reinterpret_cast<TNeg&>(t);}
+
+ static const THerm& transpose(const T& t) {return reinterpret_cast<const THerm&>(t);}
+ static THerm& transpose(T& t) {return reinterpret_cast<THerm&>(t);}
+
+ static const TPosTrans& positionalTranspose(const T& t)
+ {return reinterpret_cast<const TPosTrans&>(t);}
+ static TPosTrans& positionalTranspose(T& t)
+ {return reinterpret_cast<TPosTrans&>(t);}
+
+ static const TWithoutNegator& castAwayNegatorIfAny(const T& t)
+ {return reinterpret_cast<const TWithoutNegator&>(t);}
+ static TWithoutNegator& updCastAwayNegatorIfAny(T& t)
+ {return reinterpret_cast<TWithoutNegator&>(t);}
+
+ static ScalarNormSq scalarNormSqr(const T& t)
+ { return t.real()*t.real() + t.imag()*t.imag(); }
+ static TSqrt sqrt(const T& t)
+ { return std::sqrt(t); }
+ static TAbs abs(const T& t)
+ { return std::abs(t); } // no, not just sqrt of scalarNormSqr()!
+ static const TStandard& standardize(const T& t) {return t;} // already standard
+ static TNormalize normalize(const T& t) {return t/abs(t);}
+ static TInvert invert(const T& t) {return TReal(1)/t;}
+
+ static const T& getNaN() {
+ static const T c=T(NTraits<R>::getNaN(), NTraits<R>::getNaN());
+ return c;
+ }
+ static const T& getInfinity() {
+ static const T c=T(NTraits<R>::getInfinity(),NTraits<R>::getInfinity());
+ return c;
+ }
+
+ static const T& getI() {
+ static const T c = T(0,1);
+ return c;
+ }
+
+ static bool isFinite(const T& t) {return SimTK::isFinite(t);}
+ static bool isNaN(const T& t) {return SimTK::isNaN(t);}
+ static bool isInf(const T& t) {return SimTK::isInf(t);}
+
+ static double getDefaultTolerance() {return RTraits<R>::getDefaultTolerance();}
+
+ template <class R2> static bool isNumericallyEqual(const T& a, const complex<R2>& b)
+ { return SimTK::isNumericallyEqual(a,b); }
+ template <class R2> static bool isNumericallyEqual(const T& a, const complex<R2>& b, double tol)
+ { return SimTK::isNumericallyEqual(a,b,tol); }
+ template <class R2> static bool isNumericallyEqual(const T& a, const conjugate<R2>& b)
+ { return SimTK::isNumericallyEqual(a,b); }
+ template <class R2> static bool isNumericallyEqual(const T& a, const conjugate<R2>& b, double tol)
+ { return SimTK::isNumericallyEqual(a,b,tol); }
+
+ static bool isNumericallyEqual(const T& a, const float& b) {return SimTK::isNumericallyEqual(a,b);}
+ static bool isNumericallyEqual(const T& a, const float& b, double tol) {return SimTK::isNumericallyEqual(a,b,tol);}
+ static bool isNumericallyEqual(const T& a, const double& b) {return SimTK::isNumericallyEqual(a,b);}
+ static bool isNumericallyEqual(const T& a, const double& b, double tol) {return SimTK::isNumericallyEqual(a,b,tol);}
+ static bool isNumericallyEqual(const T& a, const long double& b) {return SimTK::isNumericallyEqual(a,b);}
+ static bool isNumericallyEqual(const T& a, const long double& b, double tol) {return SimTK::isNumericallyEqual(a,b,tol);}
+ static bool isNumericallyEqual(const T& a, int b) {return SimTK::isNumericallyEqual(a,b);}
+ static bool isNumericallyEqual(const T& a, int b, double tol) {return SimTK::isNumericallyEqual(a,b,tol);}
+
+ // The rest are the same as the real equivalents, with zero imaginary part.
+ static const T& getZero() {static const T c(NTraits<R>::getZero()); return c;}
+ static const T& getOne() {static const T c(NTraits<R>::getOne()); return c;}
+ static const T& getMinusOne() {static const T c(NTraits<R>::getMinusOne()); return c;}
+ static const T& getTwo() {static const T c(NTraits<R>::getTwo()); return c;}
+ static const T& getThree() {static const T c(NTraits<R>::getThree()); return c;}
+ static const T& getOneHalf() {static const T c(NTraits<R>::getOneHalf()); return c;}
+ static const T& getOneThird() {static const T c(NTraits<R>::getOneThird()); return c;}
+ static const T& getOneFourth() {static const T c(NTraits<R>::getOneFourth()); return c;}
+ static const T& getOneFifth() {static const T c(NTraits<R>::getOneFifth()); return c;}
+ static const T& getOneSixth() {static const T c(NTraits<R>::getOneSixth()); return c;}
+ static const T& getOneSeventh() {static const T c(NTraits<R>::getOneSeventh()); return c;}
+ static const T& getOneEighth() {static const T c(NTraits<R>::getOneEighth()); return c;}
+ static const T& getOneNinth() {static const T c(NTraits<R>::getOneNinth()); return c;}
+ static const T& getPi() {static const T c(NTraits<R>::getPi()); return c;}
+ static const T& getOneOverPi() {static const T c(NTraits<R>::getOneOverPi()); return c;}
+ static const T& getE() {static const T c(NTraits<R>::getE()); return c;}
+ static const T& getLog2E() {static const T c(NTraits<R>::getLog2E()); return c;}
+ static const T& getLog10E() {static const T c(NTraits<R>::getLog10E()); return c;}
+ static const T& getSqrt2() {static const T c(NTraits<R>::getSqrt2()); return c;}
+ static const T& getOneOverSqrt2() {static const T c(NTraits<R>::getOneOverSqrt2()); return c;}
+ static const T& getSqrt3() {static const T c(NTraits<R>::getSqrt3()); return c;}
+ static const T& getOneOverSqrt3() {static const T c(NTraits<R>::getOneOverSqrt3()); return c;}
+ static const T& getCubeRoot2() {static const T c(NTraits<R>::getCubeRoot2()); return c;}
+ static const T& getCubeRoot3() {static const T c(NTraits<R>::getCubeRoot3()); return c;}
+ static const T& getLn2() {static const T c(NTraits<R>::getLn2()); return c;}
+ static const T& getLn10() {static const T c(NTraits<R>::getLn10()); return c;}
+};
+
+
+// Specialize NTraits<complex>::Results for <complex> OP <scalar>. Result type is
+// always just the complex type of sufficient precision.
+#define SimTK_BNTCMPLX_SPEC(T1,T2) \
+template<> template<> struct NTraits< complex<T1> >::Result<T2> { \
+ typedef Widest< complex<T1>,T2 >::Type W; \
+ typedef W Mul; typedef W Dvd; typedef W Add; typedef W Sub; \
+}; \
+template<> template<> struct NTraits< complex<T1> >::Result< complex<T2> > { \
+ typedef Widest< complex<T1>,complex<T2> >::Type W; \
+ typedef W Mul; typedef W Dvd; typedef W Add; typedef W Sub; \
+}; \
+template<> template<> struct NTraits< complex<T1> >::Result< conjugate<T2> > { \
+ typedef Widest< complex<T1>,complex<T2> >::Type W; \
+ typedef W Mul; typedef W Dvd; typedef W Add; typedef W Sub; \
+}
+SimTK_BNTCMPLX_SPEC(float,float);SimTK_BNTCMPLX_SPEC(float,double);SimTK_BNTCMPLX_SPEC(float,long double);
+SimTK_BNTCMPLX_SPEC(double,float);SimTK_BNTCMPLX_SPEC(double,double);SimTK_BNTCMPLX_SPEC(double,long double);
+SimTK_BNTCMPLX_SPEC(long double,float);SimTK_BNTCMPLX_SPEC(long double,double);SimTK_BNTCMPLX_SPEC(long double,long double);
+#undef SimTK_BNTCMPLX_SPEC
+
+
+// conjugate -- should be instantiated only for float, double, long double.
+template <class R> class NTraits< conjugate<R> > {
+ typedef complex<R> C;
+public:
+ typedef conjugate<R> T;
+ typedef negator<T> TNeg; // type of this after *cast* to its negative
+ typedef conjugate<R> TWithoutNegator; // type of this ignoring negator (there isn't one!)
+ typedef R TReal;
+ typedef negator<R> TImag;
+ typedef conjugate<R> TComplex;
+ typedef complex<R> THerm; // conjugate evaporates
+ typedef conjugate<R> TPosTrans; // Positional transpose of scalar does nothing
+ typedef R TSqHermT; // C*C~
+ typedef R TSqTHerm; // ~C*C (same)
+ typedef conjugate<R> TElement;
+ typedef conjugate<R> TRow;
+ typedef conjugate<R> TCol;
+
+ typedef complex<R> TSqrt;
+ typedef R TAbs;
+ typedef complex<R> TStandard;
+ typedef conjugate<R> TInvert;
+ typedef conjugate<R> TNormalize;
+
+ typedef conjugate<R> Scalar;
+ typedef conjugate<R> ULessScalar;
+ typedef conjugate<R> Number;
+ typedef complex<R> StdNumber;
+ typedef R Precision;
+ typedef R ScalarNormSq;
+
+ // Typeof( Conj<S>*P ) is Typeof(P*Conj<S>)
+ // Typeof( Conj<S>/P ) is Typeof(inv(P)*Conj<S>)
+ // Typeof( Conj<S>+P ) is Typeof(P+Conj<S>)
+ // Typeof( Conj<S>-P ) is Typeof(P::TNeg+Conj<S>)
+ // Must specialize for P=real or P=complex or P=conjugate
+ template <class P> struct Result {
+ typedef typename CNT<P>::template Result<T>::Mul Mul;
+ typedef typename CNT<typename CNT<P>::THerm>::template Result<T>::Mul Dvd;
+ typedef typename CNT<P>::template Result<T>::Add Add;
+ typedef typename CNT<typename CNT<P>::TNeg>::template Result<T>::Add Sub;
+ };
+
+ // Shape-preserving element substitution (easy for scalars!)
+ template <class P> struct Substitute {
+ typedef P Type;
+ };
+
+ enum {
+ NRows = 1,
+ NCols = 1,
+ RowSpacing = 1,
+ ColSpacing = 1,
+ NPackedElements = 1, // not two!
+ NActualElements = 1,
+ NActualScalars = 1,
+ ImagOffset = 1,
+ RealStrideFactor = 2, // double stride when casting to real or imaginary
+ ArgDepth = SCALAR_DEPTH,
+ IsScalar = 1,
+ IsULessScalar = 1,
+ IsNumber = 1,
+ IsStdNumber = 0,
+ IsPrecision = 0,
+ SignInterpretation = 1 // after cast to Number, what is the sign?
+ };
+
+ static const T* getData(const T& t) { return &t; }
+ static T* updData(T& t) { return &t; }
+ static const TReal& real(const T& t) { return t.real(); }
+ static TReal& real(T& t) { return t.real(); }
+ static const TImag& imag(const T& t) { return t.imag(); }
+ static TImag& imag(T& t) { return t.imag(); }
+
+ static const TNeg& negate(const T& t) {return reinterpret_cast<const TNeg&>(t);}
+ static TNeg& negate(T& t) {return reinterpret_cast<TNeg&>(t);}
+
+ static const THerm& transpose(const T& t) {return t.conj();}
+ static THerm& transpose(T& t) {return t.conj();}
+
+ static const TPosTrans& positionalTranspose(const T& t)
+ {return reinterpret_cast<const TPosTrans&>(t);}
+ static TPosTrans& positionalTranspose(T& t)
+ {return reinterpret_cast<TPosTrans&>(t);}
+
+ static const TWithoutNegator& castAwayNegatorIfAny(const T& t)
+ {return reinterpret_cast<const TWithoutNegator&>(t);}
+ static TWithoutNegator& updCastAwayNegatorIfAny(T& t)
+ {return reinterpret_cast<TWithoutNegator&>(t);}
+
+ static ScalarNormSq scalarNormSqr(const T& t)
+ { return t.real()*t.real() + t.negImag()*t.negImag(); }
+ static TSqrt sqrt(const T& t)
+ { return std::sqrt(C(t)); } // cast to complex (one negation)
+ static TAbs abs(const T& t)
+ { return std::abs(t.conj()); } // no, not just sqrt of scalarNormSqr()!
+ static TStandard standardize(const T& t)
+ { return TStandard(t); } // i.e., convert to complex
+ static TNormalize normalize(const T& t) {return TNormalize(t/abs(t));}
+
+ // 1/conj(z) = conj(1/z), for complex z.
+ static TInvert invert(const T& t)
+ { const typename NTraits<THerm>::TInvert cmplx(NTraits<THerm>::invert(t.conj()));
+ return reinterpret_cast<const TInvert&>(cmplx); } // recast complex to conjugate it
+
+ // We want a "conjugate NaN", NaN - NaN*i, meaning both reals should
+ // be positive NaN.
+ static const T& getNaN() {
+ static const T c=T(NTraits<R>::getNaN(),NTraits<R>::getNaN());
+ return c;
+ }
+ // We want a "conjugate infinity", Inf - Inf*i, meaning both stored reals
+ // are positive Inf.
+ static const T& getInfinity() {
+ static const T c=T(NTraits<R>::getInfinity(),NTraits<R>::getInfinity());
+ return c;
+ }
+ // But we want the constant i (=sqrt(-1)) to be the same however we represent it,
+ // so for conjugate i = 0 - (-1)i.
+ static const T& getI() {
+ static const T c = T(0,-1);
+ return c;
+ }
+
+ static bool isFinite(const T& t) {return SimTK::isFinite(t);}
+ static bool isNaN(const T& t) {return SimTK::isNaN(t);}
+ static bool isInf(const T& t) {return SimTK::isInf(t);}
+
+ static double getDefaultTolerance() {return RTraits<R>::getDefaultTolerance();}
+
+ template <class R2> static bool isNumericallyEqual(const T& a, const conjugate<R2>& b)
+ { return SimTK::isNumericallyEqual(a,b); }
+ template <class R2> static bool isNumericallyEqual(const T& a, const conjugate<R2>& b, double tol)
+ { return SimTK::isNumericallyEqual(a,b,tol); }
+ template <class R2> static bool isNumericallyEqual(const T& a, const complex<R2>& b)
+ { return SimTK::isNumericallyEqual(a,b); }
+ template <class R2> static bool isNumericallyEqual(const T& a, const complex<R2>& b, double tol)
+ { return SimTK::isNumericallyEqual(a,b,tol); }
+
+ static bool isNumericallyEqual(const T& a, const float& b) {return SimTK::isNumericallyEqual(a,b);}
+ static bool isNumericallyEqual(const T& a, const float& b, double tol) {return SimTK::isNumericallyEqual(a,b,tol);}
+ static bool isNumericallyEqual(const T& a, const double& b) {return SimTK::isNumericallyEqual(a,b);}
+ static bool isNumericallyEqual(const T& a, const double& b, double tol) {return SimTK::isNumericallyEqual(a,b,tol);}
+ static bool isNumericallyEqual(const T& a, const long double& b) {return SimTK::isNumericallyEqual(a,b);}
+ static bool isNumericallyEqual(const T& a, const long double& b, double tol) {return SimTK::isNumericallyEqual(a,b,tol);}
+ static bool isNumericallyEqual(const T& a, int b) {return SimTK::isNumericallyEqual(a,b);}
+ static bool isNumericallyEqual(const T& a, int b, double tol) {return SimTK::isNumericallyEqual(a,b,tol);}
+
+ // The rest are the same as the real equivalents, with zero imaginary part.
+ static const T& getZero() {static const T c(NTraits<R>::getZero()); return c;}
+ static const T& getOne() {static const T c(NTraits<R>::getOne()); return c;}
+ static const T& getMinusOne() {static const T c(NTraits<R>::getMinusOne()); return c;}
+ static const T& getTwo() {static const T c(NTraits<R>::getTwo()); return c;}
+ static const T& getThree() {static const T c(NTraits<R>::getThree()); return c;}
+ static const T& getOneHalf() {static const T c(NTraits<R>::getOneHalf()); return c;}
+ static const T& getOneThird() {static const T c(NTraits<R>::getOneThird()); return c;}
+ static const T& getOneFourth() {static const T c(NTraits<R>::getOneFourth()); return c;}
+ static const T& getOneFifth() {static const T c(NTraits<R>::getOneFifth()); return c;}
+ static const T& getOneSixth() {static const T c(NTraits<R>::getOneSixth()); return c;}
+ static const T& getOneSeventh() {static const T c(NTraits<R>::getOneSeventh()); return c;}
+ static const T& getOneEighth() {static const T c(NTraits<R>::getOneEighth()); return c;}
+ static const T& getOneNinth() {static const T c(NTraits<R>::getOneNinth()); return c;}
+ static const T& getPi() {static const T c(NTraits<R>::getPi()); return c;}
+ static const T& getOneOverPi() {static const T c(NTraits<R>::getOneOverPi()); return c;}
+ static const T& getE() {static const T c(NTraits<R>::getE()); return c;}
+ static const T& getLog2E() {static const T c(NTraits<R>::getLog2E()); return c;}
+ static const T& getLog10E() {static const T c(NTraits<R>::getLog10E()); return c;}
+ static const T& getSqrt2() {static const T c(NTraits<R>::getSqrt2()); return c;}
+ static const T& getOneOverSqrt2() {static const T c(NTraits<R>::getOneOverSqrt2()); return c;}
+ static const T& getSqrt3() {static const T c(NTraits<R>::getSqrt3()); return c;}
+ static const T& getOneOverSqrt3() {static const T c(NTraits<R>::getOneOverSqrt3()); return c;}
+ static const T& getCubeRoot2() {static const T c(NTraits<R>::getCubeRoot2()); return c;}
+ static const T& getCubeRoot3() {static const T c(NTraits<R>::getCubeRoot3()); return c;}
+ static const T& getLn2() {static const T c(NTraits<R>::getLn2()); return c;}
+ static const T& getLn10() {static const T c(NTraits<R>::getLn10()); return c;}
+};
+
+// Any op involving conjugate & a real is best left as a conjugate. However,
+// an op involving conjugate & a complex or conjugate can lose the conjugate at zero cost
+// and return just a complex in some cases. Also, we prefer negator<complex> to conjugate.
+//
+// Conj op complex
+// a-bi * r+si = (ar+bs) + (as-br)i (complex)
+// a-bi / r+si = hairy and slow anyway; we'll convert to complex
+// a-bi + r+si = (a+r) + (s-b)i (complex)
+// a-bi - r+si = (a-r) - (b+s)i = -[(r-a)+(b+s)i] (negator<complex>)
+//
+// Conj op Conj
+// a-bi * r-si = (ar-bs) - (as+br)i = -[(bs-ar)+(as+br)i] (negator<complex>)
+// a-bi / r-si = hairy and slow anyway; we'll convert to complex
+// a-bi + r-si = (a+r) - (b+s)i (conjugate)
+// a-bi - r-si = (a-r) + (s-b)i (complex)
+
+#define SimTK_NTRAITS_CONJ_SPEC(T1,T2) \
+template<> template<> struct NTraits< conjugate<T1> >::Result<T2> { \
+ typedef conjugate<Widest<T1,T2>::Type> W; \
+ typedef W Mul; typedef W Dvd; typedef W Add; typedef W Sub; \
+}; \
+template<> template<> struct NTraits< conjugate<T1> >::Result<complex<T2> >{\
+ typedef Widest<complex<T1>,complex<T2> >::Type W; \
+ typedef W Mul; typedef W Dvd; typedef W Add; typedef negator<W> Sub; \
+}; \
+template<> template<> struct NTraits< conjugate<T1> >::Result<conjugate<T2> >{\
+ typedef Widest<T1,T2>::Type W; typedef complex<W> WC; \
+ typedef negator<WC> Mul; typedef WC Dvd; typedef conjugate<W> Add; typedef WC Sub;\
+}
+SimTK_NTRAITS_CONJ_SPEC(float,float);SimTK_NTRAITS_CONJ_SPEC(float,double);
+SimTK_NTRAITS_CONJ_SPEC(float,long double);
+SimTK_NTRAITS_CONJ_SPEC(double,float);SimTK_NTRAITS_CONJ_SPEC(double,double);
+SimTK_NTRAITS_CONJ_SPEC(double,long double);
+SimTK_NTRAITS_CONJ_SPEC(long double,float);SimTK_NTRAITS_CONJ_SPEC(long double,double);
+SimTK_NTRAITS_CONJ_SPEC(long double,long double);
+#undef SimTK_NTRAITS_CONJ_SPEC
+
+
+// Specializations for real numbers.
+// For real scalar R, op result types are:
+// Typeof(R*P) = Typeof(P*R)
+// Typeof(R/P) = Typeof(inv(P)*R)
+// Typeof(R+P) = Typeof(P+R)
+// typeof(R-P) = Typeof(P::TNeg + R)
+// These must be specialized for P=Real and P=Complex.
+#define SimTK_DEFINE_REAL_NTRAITS(R) \
+template <> class NTraits<R> { \
+public: \
+ typedef R T; \
+ typedef negator<T> TNeg; \
+ typedef T TWithoutNegator; \
+ typedef T TReal; \
+ typedef T TImag; \
+ typedef complex<T> TComplex; \
+ typedef T THerm; \
+ typedef T TPosTrans; \
+ typedef T TSqHermT; \
+ typedef T TSqTHerm; \
+ typedef T TElement; \
+ typedef T TRow; \
+ typedef T TCol; \
+ typedef T TSqrt; \
+ typedef T TAbs; \
+ typedef T TStandard; \
+ typedef T TInvert; \
+ typedef T TNormalize; \
+ typedef T Scalar; \
+ typedef T ULessScalar; \
+ typedef T Number; \
+ typedef T StdNumber; \
+ typedef T Precision; \
+ typedef T ScalarNormSq; \
+ template <class P> struct Result { \
+ typedef typename CNT<P>::template Result<R>::Mul Mul; \
+ typedef typename CNT< typename CNT<P>::THerm >::template Result<R>::Mul Dvd; \
+ typedef typename CNT<P>::template Result<R>::Add Add; \
+ typedef typename CNT< typename CNT<P>::TNeg >::template Result<R>::Add Sub; \
+ }; \
+ template <class P> struct Substitute { \
+ typedef P Type; \
+ }; \
+ enum { \
+ NRows = 1, \
+ NCols = 1, \
+ RowSpacing = 1, \
+ ColSpacing = 1, \
+ NPackedElements = 1, \
+ NActualElements = 1, \
+ NActualScalars = 1, \
+ ImagOffset = 0, \
+ RealStrideFactor = 1, \
+ ArgDepth = SCALAR_DEPTH, \
+ IsScalar = 1, \
+ IsULessScalar = 1, \
+ IsNumber = 1, \
+ IsStdNumber = 1, \
+ IsPrecision = 1, \
+ SignInterpretation = 1 \
+ }; \
+ static const T* getData(const T& t) { return &t; } \
+ static T* updData(T& t) { return &t; } \
+ static const T& real(const T& t) { return t; } \
+ static T& real(T& t) { return t; } \
+ static const T& imag(const T&) { return reinterpret_cast<const T&>(zeroes); } \
+ static T& imag(T&) { assert(false); return *reinterpret_cast<T*>(0); } \
+ static const TNeg& negate(const T& t) {return reinterpret_cast<const TNeg&>(t);} \
+ static TNeg& negate(T& t) {return reinterpret_cast<TNeg&>(t);} \
+ static const THerm& transpose(const T& t) {return reinterpret_cast<const THerm&>(t);} \
+ static THerm& transpose(T& t) {return reinterpret_cast<THerm&>(t);} \
+ static const TPosTrans& positionalTranspose(const T& t) \
+ {return reinterpret_cast<const TPosTrans&>(t);} \
+ static TPosTrans& positionalTranspose(T& t) \
+ {return reinterpret_cast<TPosTrans&>(t);} \
+ static const TWithoutNegator& castAwayNegatorIfAny(const T& t) \
+ {return reinterpret_cast<const TWithoutNegator&>(t);} \
+ static TWithoutNegator& updCastAwayNegatorIfAny(T& t) \
+ {return reinterpret_cast<TWithoutNegator&>(t);} \
+ static ScalarNormSq scalarNormSqr(const T& t) {return t*t;} \
+ static TSqrt sqrt(const T& t) {return std::sqrt(t);} \
+ static TAbs abs(const T& t) {return std::abs(t);} \
+ static const TStandard& standardize(const T& t) {return t;} \
+ static TNormalize normalize(const T& t) {return (t>0?T(1):(t<0?T(-1):getNaN()));} \
+ static TInvert invert(const T& t) {return T(1)/t;} \
+ /* properties of this floating point representation, with memory addresses */ \
+ static const T& getEps() {return RTraits<T>::getEps();} \
+ static const T& getSignificant() {return RTraits<T>::getSignificant();} \
+ static const T& getNaN() {static const T c=std::numeric_limits<T>::quiet_NaN(); return c;} \
+ static const T& getInfinity() {static const T c=std::numeric_limits<T>::infinity(); return c;} \
+ static const T& getLeastPositive(){static const T c=std::numeric_limits<T>::min(); return c;} \
+ static const T& getMostPositive() {static const T c=std::numeric_limits<T>::max(); return c;} \
+ static const T& getLeastNegative(){static const T c=-std::numeric_limits<T>::min(); return c;} \
+ static const T& getMostNegative() {static const T c=-std::numeric_limits<T>::max(); return c;} \
+ static const T& getSqrtEps() {static const T c=std::sqrt(getEps()); return c;} \
+ static const T& getTiny() {static const T c=std::pow(getEps(), (T)1.25L); return c;} \
+ static bool isFinite(const T& t) {return SimTK::isFinite(t);} \
+ static bool isNaN (const T& t) {return SimTK::isNaN(t);} \
+ static bool isInf (const T& t) {return SimTK::isInf(t);} \
+ /* Methods to use for approximate comparisons. Perform comparison in the wider of the two */ \
+ /* precisions, using the default tolerance from the narrower of the two precisions. */ \
+ static double getDefaultTolerance() {return RTraits<T>::getDefaultTolerance();} \
+ static bool isNumericallyEqual(const T& t, const float& f) {return SimTK::isNumericallyEqual(t,f);} \
+ static bool isNumericallyEqual(const T& t, const double& d) {return SimTK::isNumericallyEqual(t,d);} \
+ static bool isNumericallyEqual(const T& t, const long double& l) {return SimTK::isNumericallyEqual(t,l);} \
+ static bool isNumericallyEqual(const T& t, int i) {return SimTK::isNumericallyEqual(t,i);} \
+ /* Here the tolerance is given so we don't have to figure it out. */ \
+ static bool isNumericallyEqual(const T& t, const float& f, double tol){return SimTK::isNumericallyEqual(t,f,tol);} \
+ static bool isNumericallyEqual(const T& t, const double& d, double tol){return SimTK::isNumericallyEqual(t,d,tol);} \
+ static bool isNumericallyEqual(const T& t, const long double& l, double tol){return SimTK::isNumericallyEqual(t,l,tol);} \
+ static bool isNumericallyEqual(const T& t, int i, double tol){return SimTK::isNumericallyEqual(t,i,tol);} \
+ /* Carefully calculated constants with convenient memory addresses. */ \
+ static const T& getZero() {static const T c=(T)(0); return c;} \
+ static const T& getOne() {static const T c=(T)(1); return c;} \
+ static const T& getMinusOne() {static const T c=(T)(-1); return c;} \
+ static const T& getTwo() {static const T c=(T)(2); return c;} \
+ static const T& getThree() {static const T c=(T)(3); return c;} \
+ static const T& getOneHalf() {static const T c=(T)(0.5L); return c;} \
+ static const T& getOneThird() {static const T c=(T)(1.L/3.L); return c;} \
+ static const T& getOneFourth() {static const T c=(T)(0.25L); return c;} \
+ static const T& getOneFifth() {static const T c=(T)(0.2L); return c;} \
+ static const T& getOneSixth() {static const T c=(T)(1.L/6.L); return c;} \
+ static const T& getOneSeventh() {static const T c=(T)(1.L/7.L); return c;} \
+ static const T& getOneEighth() {static const T c=(T)(0.125L); return c;} \
+ static const T& getOneNinth() {static const T c=(T)(1.L/9.L); return c;} \
+ static const T& getPi() {static const T c=(T)(SimTK_PI); return c;} \
+ static const T& getOneOverPi() {static const T c=(T)(1.L/SimTK_PI); return c;} \
+ static const T& getE() {static const T c=(T)(SimTK_E); return c;} \
+ static const T& getLog2E() {static const T c=(T)(SimTK_LOG2E); return c;} \
+ static const T& getLog10E() {static const T c=(T)(SimTK_LOG10E); return c;} \
+ static const T& getSqrt2() {static const T c=(T)(SimTK_SQRT2); return c;} \
+ static const T& getOneOverSqrt2() {static const T c=(T)(1.L/SimTK_SQRT2); return c;} \
+ static const T& getSqrt3() {static const T c=(T)(SimTK_SQRT3); return c;} \
+ static const T& getOneOverSqrt3() {static const T c=(T)(1.L/SimTK_SQRT3); return c;} \
+ static const T& getCubeRoot2() {static const T c=(T)(SimTK_CBRT2); return c;} \
+ static const T& getCubeRoot3() {static const T c=(T)(SimTK_CBRT3); return c;} \
+ static const T& getLn2() {static const T c=(T)(SimTK_LN2); return c;} \
+ static const T& getLn10() {static const T c=(T)(SimTK_LN10); return c;} \
+ /* integer digit counts useful for formatted input and output */ \
+ static const int getNumDigits() {static const int c=(int)(std::log10(1/getEps()) -0.5); return c;} \
+ static const int getLosslessNumDigits() {static const int c=(int)(std::log10(1/getTiny())+0.5); return c;} \
+}; \
+template<> struct NTraits<R>::Result<float> \
+ {typedef Widest<R,float>::Type Mul;typedef Mul Dvd;typedef Mul Add;typedef Mul Sub;}; \
+template<> struct NTraits<R>::Result<double> \
+ {typedef Widest<R,double>::Type Mul;typedef Mul Dvd;typedef Mul Add;typedef Mul Sub;}; \
+template<> struct NTraits<R>::Result<long double> \
+ {typedef Widest<R,long double>::Type Mul;typedef Mul Dvd;typedef Mul Add;typedef Mul Sub;}; \
+template<> struct NTraits<R>::Result<complex<float> > \
+ {typedef Widest<R,complex<float> >::Type Mul;typedef Mul Dvd;typedef Mul Add;typedef Mul Sub;}; \
+template<> struct NTraits<R>::Result<complex<double> > \
+ {typedef Widest<R,complex<double> >::Type Mul;typedef Mul Dvd;typedef Mul Add;typedef Mul Sub;}; \
+template<> struct NTraits<R>::Result<complex<long double> > \
+ {typedef Widest<R,complex<long double> >::Type Mul;typedef Mul Dvd;typedef Mul Add;typedef Mul Sub;}; \
+template<> struct NTraits<R>::Result<conjugate<float> > \
+ {typedef conjugate<Widest<R,float>::Type> Mul;typedef Mul Dvd;typedef Mul Add;typedef Mul Sub;}; \
+template<> struct NTraits<R>::Result<conjugate<double> > \
+ {typedef conjugate<Widest<R,double>::Type> Mul;typedef Mul Dvd;typedef Mul Add;typedef Mul Sub;}; \
+template<> struct NTraits<R>::Result<conjugate<long double> > \
+ {typedef conjugate<Widest<R,long double>::Type> Mul;typedef Mul Dvd;typedef Mul Add;typedef Mul Sub;}
+SimTK_DEFINE_REAL_NTRAITS(float);
+SimTK_DEFINE_REAL_NTRAITS(double);
+SimTK_DEFINE_REAL_NTRAITS(long double);
+#undef SimTK_DEFINE_REAL_NTRAITS
+
+/// Specializations of CNT for numeric types.
+template <class R> class CNT< complex<R> > : public NTraits< complex<R> > { };
+template <class R> class CNT< conjugate<R> > : public NTraits< conjugate<R> > { };
+template <> class CNT<float> : public NTraits<float> { };
+template <> class CNT<double> : public NTraits<double> { };
+template <> class CNT<long double> : public NTraits<long double> { };
+
+
+} // namespace SimTK
+
+#endif //SimTK_SIMMATRIX_NTRAITS_H_
diff --git a/SimTKcommon/Scalar/include/SimTKcommon/internal/conjugate.h b/SimTKcommon/Scalar/include/SimTKcommon/internal/conjugate.h
new file mode 100644
index 0000000..d88d6bd
--- /dev/null
+++ b/SimTKcommon/Scalar/include/SimTKcommon/internal/conjugate.h
@@ -0,0 +1,1027 @@
+#ifndef SimTK_SIMMATRIX_CONJUGATE_H_
+#define SimTK_SIMMATRIX_CONJUGATE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This file defines the conjugate<R> template class, where R is one of
+ * the three built-in real types. It is exactly like the C++ complex<R>
+ * template except that the represented value is the conjugate of the value
+ * represented by a complex number containing the same bit pattern. That is,
+ * complex and conjugate both contain two real numbers, re and im, with
+ * complex(re,im) meaning (re + im*i) while conjugate(re,im) means (re - im*i).
+ * It is guaranteed that our conjugate type has the identical size and
+ * representation as complex. Together, these definitions and
+ * guarantee permit conjugation to be done by reinterpretation (i.e. type
+ * casting) rather than by computation.
+ *
+ * A caution on implementation of complex numbers: it's not as simple
+ * as you might think. A mildly nasty issue is handling mixed precision
+ * arguments. More substantial is getting the right complex answer using
+ * real arithmetic. For example, if c=a+bi then even a seemingly simple
+ * calculation like |c|=sqrt(a*a+b*b) may fail if calculated that way,
+ * due to unnecessary overflow. Take a look at a good complex<>
+ * implementation or see Press, et al., Numerical Recipes in C++, 2nd ed.,
+ * 2003, section 5.4, pp 183-4 for a discussion. Consequently, wherever
+ * it doesn't matter for performance, conjugate<R> objects are simply
+ * converted to complex<R> (which costs one negation) and then the
+ * system class is used to do the hard stuff.
+ *
+ * References for the complex<> class, which I used as a model
+ * for SimTK::conjugate<>:
+ * (1) Stroustrup, B., The C++ Programming Language, 3rd ed., 1997, section
+ * 22.5, pg 679ff.
+ * (2) The C++ Standard Incorporating Technical Corrigendum 1, British
+ * Standards Institute ISO/IEC 14882:2003 2nd ed., 2003, section 26.2,
+ * pg 592ff.
+ */
+
+#include <complex>
+#include <iostream>
+#include <limits>
+
+using std::complex;
+
+
+// These mixed-precision real/complex operators should not be needed
+// and may have to be removed on some systems. However neither
+// gcc 3.4.4 nor VC++ 7 provide them.
+// Define the symbol below if these conflict with standard library operators.
+
+#ifndef SimTK_MIXED_PRECISION_REAL_COMPLEX_ALREADY_DEFINED
+namespace SimTK {
+// complex<float> with int, double, long double
+inline complex<float> operator*(const complex<float>& c,int r) {return c*(float)r;}
+inline complex<float> operator*(int r,const complex<float>& c) {return (float)r*c;}
+inline complex<double> operator*(const complex<float>& c,const double& r) {return complex<double>(c)*r;}
+inline complex<double> operator*(const double& r,const complex<float>& c) {return r*complex<double>(c);}
+inline complex<long double> operator*(const complex<float>& c,const long double& r) {return complex<long double>(c)*r;}
+inline complex<long double> operator*(const long double& r,const complex<float>& c) {return r*complex<long double>(c);}
+
+inline complex<float> operator/(const complex<float>& c,int r) {return c/(float)r;}
+inline complex<float> operator/(int r,const complex<float>& c) {return (float)r/c;}
+inline complex<double> operator/(const complex<float>& c,const double& r) {return complex<double>(c)/r;}
+inline complex<double> operator/(const double& r,const complex<float>& c) {return r/complex<double>(c);}
+inline complex<long double> operator/(const complex<float>& c,const long double& r) {return complex<long double>(c)/r;}
+inline complex<long double> operator/(const long double& r,const complex<float>& c) {return r/complex<long double>(c);}
+
+inline complex<float> operator+(const complex<float>& c,int r) {return c+(float)r;}
+inline complex<float> operator+(int r,const complex<float>& c) {return (float)r+c;}
+inline complex<double> operator+(const complex<float>& c,const double& r) {return complex<double>(c)+r;}
+inline complex<double> operator+(const double& r,const complex<float>& c) {return r+complex<double>(c);}
+inline complex<long double> operator+(const complex<float>& c,const long double& r) {return complex<long double>(c)+r;}
+inline complex<long double> operator+(const long double& r,const complex<float>& c) {return r+complex<long double>(c);}
+
+inline complex<float> operator-(const complex<float>& c,int r) {return c-(float)r;}
+inline complex<float> operator-(int r,const complex<float>& c) {return (float)r-c;}
+inline complex<double> operator-(const complex<float>& c,const double& r) {return complex<double>(c)-r;}
+inline complex<double> operator-(const double& r,const complex<float>& c) {return r-complex<double>(c);}
+inline complex<long double> operator-(const complex<float>& c,const long double& r) {return complex<long double>(c)-r;}
+inline complex<long double> operator-(const long double& r,const complex<float>& c) {return r-complex<long double>(c);}
+
+// complex<double> with int, float, long double
+inline complex<double> operator*(const complex<double>& c,int r) {return c*(double)r;}
+inline complex<double> operator*(int r,const complex<double>& c) {return (double)r*c;}
+inline complex<double> operator*(const complex<double>& c,const float& r) {return c*(double)r;}
+inline complex<double> operator*(const float& r,const complex<double>& c) {return (double)r*c;}
+inline complex<long double> operator*(const complex<double>& c,const long double& r){return complex<long double>(c)*r;}
+inline complex<long double> operator*(const long double& r,const complex<double>& c){return r*complex<long double>(c);}
+
+inline complex<double> operator/(const complex<double>& c,int r) {return c/(double)r;}
+inline complex<double> operator/(int r,const complex<double>& c) {return (double)r/c;}
+inline complex<double> operator/(const complex<double>& c,const float& r) {return c/(double)r;}
+inline complex<double> operator/(const float& r,const complex<double>& c) {return (double)r/c;}
+inline complex<long double> operator/(const complex<double>& c,const long double& r){return complex<long double>(c)/r;}
+inline complex<long double> operator/(const long double& r,const complex<double>& c){return r/complex<long double>(c);}
+
+inline complex<double> operator+(const complex<double>& c,int r) {return c+(double)r;}
+inline complex<double> operator+(int r,const complex<double>& c) {return (double)r+c;}
+inline complex<double> operator+(const complex<double>& c,const float& r) {return c+(double)r;}
+inline complex<double> operator+(const float& r,const complex<double>& c) {return (double)r+c;}
+inline complex<long double> operator+(const complex<double>& c,const long double& r){return complex<long double>(c)+r;}
+inline complex<long double> operator+(const long double& r,const complex<double>& c){return r+complex<long double>(c);}
+
+inline complex<double> operator-(const complex<double>& c,int r) {return c-(double)r;}
+inline complex<double> operator-(int r,const complex<double>& c) {return (double)r-c;}
+inline complex<double> operator-(const complex<double>& c,const float& r) {return c-(double)r;}
+inline complex<double> operator-(const float& r,const complex<double>& c) {return (double)r-c;}
+inline complex<long double> operator-(const complex<double>& c,const long double& r){return complex<long double>(c)-r;}
+inline complex<long double> operator-(const long double& r,const complex<double>& c){return r-complex<long double>(c);}
+
+// complex<long double> with int, float, double
+inline complex<long double> operator*(const complex<long double>& c,int r) {return c*(long double)r;}
+inline complex<long double> operator*(int r,const complex<long double>& c) {return (long double)r*c;}
+inline complex<long double> operator*(const complex<long double>& c,const float& r) {return c*(long double)r;}
+inline complex<long double> operator*(const float& r,const complex<long double>& c) {return (long double)r*c;}
+inline complex<long double> operator*(const complex<long double>& c,const double& r){return c*(long double)r;}
+inline complex<long double> operator*(const double& r,const complex<long double>& c){return (long double)r*c;}
+
+inline complex<long double> operator/(const complex<long double>& c,int r) {return c/(long double)r;}
+inline complex<long double> operator/(int r,const complex<long double>& c) {return (long double)r/c;}
+inline complex<long double> operator/(const complex<long double>& c,const float& r) {return c/(long double)r;}
+inline complex<long double> operator/(const float& r,const complex<long double>& c) {return (long double)r/c;}
+inline complex<long double> operator/(const complex<long double>& c,const double& r){return c/(long double)r;}
+inline complex<long double> operator/(const double& r,const complex<long double>& c){return (long double)r/c;}
+
+inline complex<long double> operator+(const complex<long double>& c,int r) {return c+(long double)r;}
+inline complex<long double> operator+(int r,const complex<long double>& c) {return (long double)r+c;}
+inline complex<long double> operator+(const complex<long double>& c,const float& r) {return c+(long double)r;}
+inline complex<long double> operator+(const float& r,const complex<long double>& c) {return (long double)r+c;}
+inline complex<long double> operator+(const complex<long double>& c,const double& r){return c+(long double)r;}
+inline complex<long double> operator+(const double& r,const complex<long double>& c){return (long double)r+c;}
+
+inline complex<long double> operator-(const complex<long double>& c,int r) {return c-(long double)r;}
+inline complex<long double> operator-(int r,const complex<long double>& c) {return (long double)r-c;}
+inline complex<long double> operator-(const complex<long double>& c,const float& r) {return c-(long double)r;}
+inline complex<long double> operator-(const float& r,const complex<long double>& c) {return (long double)r-c;}
+inline complex<long double> operator-(const complex<long double>& c,const double& r){return c-(long double)r;}
+inline complex<long double> operator-(const double& r,const complex<long double>& c){return (long double)r-c;}
+} // namespace SimTK
+#endif
+
+namespace SimTK {
+
+template <class R> class conjugate; // Only defined for float, double, long double
+template <> class conjugate<float>;
+template <> class conjugate<double>;
+template <> class conjugate<long double>;
+
+// This is an adaptor for number types which negates the apparent values. A
+// negator<N> has exactly the same internal representation as a number
+// type N, but it is to be interpreted has having the negative of the value
+// it would have if interpreted as an N. This permits negation to be done
+// by reinterpretation rather than computation. A full set of arithmetic operators
+// are provided involving negator<N>'s and N's. Sometimes we can save an op or
+// two this way. For example negator<N>*negator<N> can be performed as an N*N
+// since the negations cancel, and we saved two floating point negations.
+template <class N> class negator; // Only defined for numbers
+
+/*
+ * This class is specialized for all 9 combinations of built-in real types
+ * and contains a typedef for the appropriate "widened" real, complex, or
+ * conjugate type for use when R1 & R2 appear in an operation together.
+ */
+template <class R1, class R2> struct Wider {/* Only defined for built-ins. */};
+template <> struct Wider<float,float> {
+ typedef float WReal;
+ typedef complex<float> WCplx;
+ typedef conjugate<float> WConj;
+};
+template <> struct Wider<float,double> {
+ typedef double WReal;
+ typedef complex<double> WCplx;
+ typedef conjugate<double> WConj;
+};
+template <> struct Wider<double,float> {
+ typedef double WReal;
+ typedef complex<double> WCplx;
+ typedef conjugate<double> WConj;
+};
+template <> struct Wider<double,double> {
+ typedef double WReal;
+ typedef complex<double> WCplx;
+ typedef conjugate<double> WConj;
+};
+template <> struct Wider<float,long double> {
+ typedef long double WReal;
+ typedef complex<long double> WCplx;
+ typedef conjugate<long double> WConj;
+};
+template <> struct Wider<double,long double> {
+ typedef long double WReal;
+ typedef complex<long double> WCplx;
+ typedef conjugate<long double> WConj;
+};
+template <> struct Wider<long double,float> {
+ typedef long double WReal;
+ typedef complex<long double> WCplx;
+ typedef conjugate<long double> WConj;
+};
+template <> struct Wider<long double,double> {
+ typedef long double WReal;
+ typedef complex<long double> WCplx;
+ typedef conjugate<long double> WConj;
+};
+template <> struct Wider<long double,long double> {
+ typedef long double WReal;
+ typedef complex<long double> WCplx;
+ typedef conjugate<long double> WConj;
+};
+
+
+/**
+ * SimTK::conjugate<R> should be instantiated only for float, double, long double.
+ * This should behave just like std::complex<R> and in most cases we'll just
+ * convert and punt to the std class.
+ *
+ * The three specializations are almost identical, but differ in the rules
+ * for implicit conversion. Implicit conversions are allowed from a narrow
+ * type to a wider one, but the other direction must be explicit.
+ *
+ * Some of this could be done more compactly with member templates, helper
+ * classes and so on, but this can make debugging of client programs very
+ * awkward, both by producing indecipherable error messages when compiling
+ * and by referencing unexpanded template code when debugging. Since this
+ * class need only be done once, and has a grand total of three specializations,
+ * I felt it made more sense to do each of them explicitly here (sherm 051006).
+ */
+template <class R> class conjugate {/*Only defined for float, double, long double*/};
+
+/////////////////////////////////////////
+// Specialization for conjugate<float> //
+/////////////////////////////////////////
+
+template <> class conjugate<float> {
+public:
+ conjugate() {
+ #ifndef NDEBUG
+ re = negIm = std::numeric_limits<float>::quiet_NaN();
+ #endif
+ }
+ // default copy constructor, copy assignment, destructor
+
+ /// Construction from reals. Note that the numeric result is (real-imag*i).
+ conjugate(const float& real, const float& imag) { re = real; negIm = imag; }
+ conjugate(const float& real, int i) { re = real; negIm = float(i); }
+ conjugate(int r, const float& imag) { re = float(r); negIm = imag; }
+ conjugate(int r, int i) { re = float(r); negIm = float(i); }
+
+ /// Implicit conversion from float to conjugate<float>.
+ conjugate(const float& real) { re = real; negIm = 0.f; }
+ conjugate(int r) { re = float(r); negIm = 0.f; }
+
+ // No implicit conversions from double or long double because precision
+ // will be lost. Some definitions must be deferred until conjugate<double>
+ // and conjugate<long double> are defined below.
+ inline explicit conjugate(const conjugate<double>& cd);
+ inline explicit conjugate(const conjugate<long double>& cl);
+
+ explicit conjugate(const double& rd)
+ { re = float(rd); negIm = 0.f; }
+ explicit conjugate(const long double& rl)
+ { re = float(rl); negIm = 0.f; }
+
+ // Conversions from complex are always explicit. Note that the value
+ // represented by the conjugate must be identical to that represented by
+ // the complex, which means we must negate the imaginary part.
+ explicit conjugate(const complex<float>& x)
+ { re = x.real(); negIm = -x.imag(); }
+ explicit conjugate(const complex<double>& x)
+ { re = float(x.real()); negIm = float(-x.imag()); }
+ explicit conjugate(const complex<long double>& x)
+ { re = float(x.real()); negIm = float(-x.imag()); }
+
+ /// Implicit conversion to complex<float> when necessary
+ /// (costs an actual negation -- yuck!).
+ operator complex<float>() const
+ { return complex<float>(re,-negIm); }
+
+ // Can't defer here by casting to negator<conjugate> -- this must act
+ // like a built-in. But ... we can use this as a chance to convert
+ // to complex and save one negation.
+ complex<float> operator-() const { return complex<float>(-re,negIm); }
+
+ // Useless.
+ const conjugate& operator+() const { return *this; }
+
+ // Computed assignment operators. We don't depend on implicit conversions
+ // from reals to conjugates here because we can save a few flops by handling
+ // the reals explicitly. Note that we only provide operators for implicitly
+ // convertible precisions, though, which in this case means only floats.
+ conjugate& operator=(const float& r)
+ { re = r; negIm = 0.f; return *this; }
+ conjugate& operator+=(const float& r)
+ { re += r; return *this; }
+ conjugate& operator-=(const float& r)
+ { re -= r; return *this; }
+ conjugate& operator*=(const float& r)
+ { re *= r; negIm *= r; return *this; }
+ conjugate& operator/=(const float& r)
+ { re /= r; negIm /= r; return *this; }
+
+ conjugate& operator+=(const conjugate<float>& c)
+ { re += c.re; negIm += c.negIm; return *this; }
+ conjugate& operator-=(const conjugate<float>& c)
+ { re -= c.re; negIm -= c.negIm; return *this; }
+
+ conjugate& operator=(const complex<float>& c)
+ { re = c.real(); negIm = -c.imag(); return *this; }
+ conjugate& operator+=(const complex<float>& c)
+ { re += c.real(); negIm -= c.imag(); return *this; }
+ conjugate& operator-=(const complex<float>& c)
+ { re -= c.real(); negIm += c.imag(); return *this; }
+
+ // It is pleasant to note that we can self-multiply by either a complex or
+ // a conjugate (leaving a conjugate result) in six flops which is the same
+ // cost as an ordinary complex multiply:
+ // cplx=cplx*cplx: (a+bi)(r+si) = (ar-bs)+(as+br)i
+ // conj=conj*conj: (a-bi)(r-si) = (ar-bs)-(as+br)i
+ // conj=conj*cplx: (a-bi)(r+si) = (ar+bs)-(br-as)i
+ conjugate& operator*=(const conjugate<float>& c) {
+ const float r=(re*c.re - negIm*c.negIm);
+ negIm=(re*c.negIm + negIm*c.re); re=r; return *this;
+ }
+ conjugate& operator*=(const complex<float>& t) {
+ const float r=(re*t.real() + negIm*t.imag());
+ negIm=(negIm*t.real() - re*t.imag()); re=r; return *this;
+ }
+
+ // Complex divide is messy and slow anyway so we'll convert to complex and back here,
+ // making use of the fact that for complex c and d, c/d=conj(conj(c)/conj(d)).
+ conjugate& operator/=(const conjugate<float>& d) {
+ const complex<float> t = conj()/d.conj();
+ re = t.real(); negIm = t.imag(); // conjugating!
+ return *this;
+ }
+ conjugate& operator/=(const complex<float>& d) {
+ const complex<float> t = conj()/std::conj(d);
+ re = t.real(); negIm = t.imag(); // conjugating!
+ return *this;
+ }
+
+ const float& real() const { return re; }
+ float& real() { return re; }
+
+ const negator<float>& imag() const { return reinterpret_cast<const negator<float>&>(negIm); }
+ negator<float>& imag() { return reinterpret_cast<negator<float>&>(negIm); }
+
+ const complex<float>& conj() const { return reinterpret_cast<const complex<float>&>(*this); }
+ complex<float>& conj() { return reinterpret_cast<complex<float>&>(*this); }
+
+ // Special conjugate methods of use primarily in operator implementations.
+ const float& negImag() const { return negIm; }
+ float& negImag() { return negIm; }
+ bool isReal() const { return negIm==0.f; }
+
+private:
+ float re; // The value represented here is re - negIm*i.
+ float negIm;
+};
+
+
+
+
+//////////////////////////////////////////
+// Specialization for conjugate<double> //
+//////////////////////////////////////////
+
+template <> class conjugate<double> {
+public:
+ conjugate() {
+ #ifndef NDEBUG
+ re = negIm = std::numeric_limits<double>::quiet_NaN();
+ #endif
+ }
+ // default copy constructor, copy assignment, destructor
+
+ /// Construction from reals. Note that the numeric result is (real-imag*i).
+ conjugate(const double& real, const double& imag) { re = real; negIm = imag; }
+ conjugate(const double& real, int i) { re = real; negIm = double(i); }
+ conjugate(int r, const double& imag) { re = double(r); negIm = imag; }
+ conjugate(int r, int i) { re = double(r); negIm = double(i); }
+
+ /// Implicit conversion from double to conjugate<double>.
+ conjugate(const double& real) { re = real; negIm = 0.; }
+ conjugate(int r) { re = double(r); negIm = 0.; }
+
+ // Implicit conversions from float are allowed since
+ // there is no loss in going to double, but long double
+ // requires explicit conversions.
+ conjugate(const conjugate<float>& cf)
+ { re = double(cf.real()); negIm = double(cf.negImag()); }
+ conjugate(const float& rf)
+ { re = double(rf); negIm = 0.; }
+
+ // Definition must be deferred until conjugate<long double> is defined below.
+ inline explicit conjugate(const conjugate<long double>& cl);
+ explicit conjugate(const long double& rl)
+ { re = double(rl); negIm = 0.; }
+
+ // Conversions from complex are always explicit. Note that the value
+ // represented by the conjugate must be identical to that represented by
+ // the complex, which means we must negate the imaginary part.
+ explicit conjugate(const complex<float>& x)
+ { re = double(x.real()); negIm = double(-x.imag()); }
+ explicit conjugate(const complex<double>& x)
+ { re = x.real(); negIm = -x.imag(); }
+ explicit conjugate(const complex<long double>& x)
+ { re = double(x.real()); negIm = double(-x.imag()); }
+
+ /// Implicit conversion to complex<double> when necessary
+ /// (costs an actual negation -- yuck!).
+ operator complex<double>() const
+ { return complex<double>(re,-negIm); }
+
+ // Can't defer here by casting to negator<conjugate> -- this must act
+ // like a built-in. But ... we can use this as a chance to convert
+ // to complex and save one negation.
+ complex<double> operator-() const { return complex<double>(-re,negIm); }
+
+ // Useless.
+ const conjugate& operator+() const { return *this; }
+
+ // Computed assignment operators. We don't depend on implicit conversions
+ // from reals to conjugates here because we can save a few flops by handling
+ // the reals explicitly. Note that we only provide operators for implicitly
+ // convertible precisions, though, which in this case means floats and doubles.
+ conjugate& operator=(const double& r)
+ { re = r; negIm = 0.; return *this; }
+ conjugate& operator+=(const double& r)
+ { re += r; return *this; }
+ conjugate& operator-=(const double& r)
+ { re -= r; return *this; }
+ conjugate& operator*=(const double& r)
+ { re *= r; negIm *= r; return *this; }
+ conjugate& operator/=(const double& r)
+ { re /= r; negIm /= r; return *this; }
+
+ conjugate& operator=(const float& r)
+ { re = r; negIm = 0.; return *this; }
+ conjugate& operator+=(const float& r)
+ { re += r; return *this; }
+ conjugate& operator-=(const float& r)
+ { re -= r; return *this; }
+ conjugate& operator*=(const float& r)
+ { re *= r; negIm *= r; return *this; }
+ conjugate& operator/=(const float& r)
+ { re /= r; negIm /= r; return *this; }
+
+ // Disambiguate int to be a double.
+ conjugate& operator =(int i) {*this =(double)i; return *this;}
+ conjugate& operator+=(int i) {*this+=(double)i; return *this;}
+ conjugate& operator-=(int i) {*this-=(double)i; return *this;}
+ conjugate& operator*=(int i) {*this*=(double)i; return *this;}
+ conjugate& operator/=(int i) {*this/=(double)i; return *this;}
+
+ conjugate& operator+=(const conjugate<double>& c)
+ { re += c.re; negIm += c.negIm; return *this; }
+ conjugate& operator-=(const conjugate<double>& c)
+ { re -= c.re; negIm -= c.negIm; return *this; }
+
+ conjugate& operator+=(const conjugate<float>& c)
+ { re += c.real(); negIm += c.negImag(); return *this; }
+ conjugate& operator-=(const conjugate<float>& c)
+ { re -= c.real(); negIm -= c.negImag(); return *this; }
+
+ conjugate& operator=(const complex<double>& c)
+ { re = c.real(); negIm = -c.imag(); return *this; }
+ conjugate& operator+=(const complex<double>& c)
+ { re += c.real(); negIm -= c.imag(); return *this; }
+ conjugate& operator-=(const complex<double>& c)
+ { re -= c.real(); negIm += c.imag(); return *this; }
+
+ conjugate& operator=(const complex<float>& c)
+ { re = c.real(); negIm = -c.imag(); return *this; }
+ conjugate& operator+=(const complex<float>& c)
+ { re += c.real(); negIm -= c.imag(); return *this; }
+ conjugate& operator-=(const complex<float>& c)
+ { re -= c.real(); negIm += c.imag(); return *this; }
+
+ // It is pleasant to note that we can self-multiply by either a complex or
+ // a conjugate (leaving a conjugate result) in six flops which is the same
+ // cost as an ordinary complex multiply:
+ // cplx=cplx*cplx: (a+bi)(r+si) = (ar-bs)+(as+br)i
+ // conj=conj*conj: (a-bi)(r-si) = (ar-bs)-(as+br)i
+ // conj=conj*cplx: (a-bi)(r+si) = (ar+bs)-(br-as)i
+ conjugate& operator*=(const conjugate<double>& c) {
+ const double r=(re*c.re - negIm*c.negIm);
+ negIm=(re*c.negIm + negIm*c.re); re=r; return *this;
+ }
+ conjugate& operator*=(const complex<double>& t) {
+ const double r=(re*t.real() + negIm*t.imag());
+ negIm=(negIm*t.real() - re*t.imag()); re=r; return *this;
+ }
+
+ conjugate& operator*=(const conjugate<float>& c) { return operator*=(conjugate<double>(c)); }
+ conjugate& operator*=(const complex<float>& c) { return operator*=(complex<double>(c)); }
+
+ // Complex divide is messy and slow anyway so we'll convert to complex and back here,
+ // making use of the fact that for complex c and d, c/d=conj(conj(c)/conj(d)).
+ conjugate& operator/=(const conjugate<double>& d) {
+ const complex<double> t = conj()/d.conj();
+ re = t.real(); negIm = t.imag(); // conjugating!
+ return *this;
+ }
+ conjugate& operator/=(const complex<double>& d) {
+ const complex<double> t = conj()/std::conj(d);
+ re = t.real(); negIm = t.imag(); // conjugating!
+ return *this;
+ }
+
+ conjugate& operator/=(const conjugate<float>& c) { return operator/=(conjugate<double>(c)); }
+ conjugate& operator/=(const complex<float>& c) { return operator/=(complex<double>(c)); }
+
+ const double& real() const { return re; }
+ double& real() { return re; }
+
+ const negator<double>& imag() const { return reinterpret_cast<const negator<double>&>(negIm); }
+ negator<double>& imag() { return reinterpret_cast<negator<double>&>(negIm); }
+
+ const complex<double>& conj() const { return reinterpret_cast<const complex<double>&>(*this); }
+ complex<double>& conj() { return reinterpret_cast<complex<double>&>(*this); }
+
+ // Special conjugate methods of use primarily in operator implementations.
+ const double& negImag() const { return negIm; }
+ double& negImag() { return negIm; }
+ bool isReal() const { return negIm==0.; }
+
+private:
+ double re; // The value represented here is re - negIm*i.
+ double negIm;
+};
+
+
+
+///////////////////////////////////////////////
+// Specialization for conjugate<long double> //
+///////////////////////////////////////////////
+
+template <> class conjugate<long double> {
+public:
+ conjugate() {
+ #ifndef NDEBUG
+ re = negIm = std::numeric_limits<long double>::quiet_NaN();
+ #endif
+ }
+ // default copy constructor, copy assignment, destructor
+
+ /// Construction from reals. Note that the numeric result is (real-imag*i).
+ conjugate(const long double& real, const long double& imag) { re = real; negIm = imag; }
+ conjugate(const long double& real, int i) { re = real; negIm = (long double)i; }
+ conjugate(int r, const long double& imag) { re = (long double)r; negIm = imag; }
+ conjugate(int r, int i) { re = (long double)r; negIm = (long double)i; }
+
+ /// Implicit conversion from long double to conjugate<long double>.
+ conjugate(const long double& real) { re = real; negIm = 0.L; }
+ conjugate(int r) { re = (long double)r; negIm = 0.L; }
+
+ // Implicit conversions from float and double are allowed since
+ // there is no loss in going to long double.
+ conjugate(const conjugate<float>& cf)
+ { re = (long double)cf.real(); negIm = (long double)cf.negImag(); }
+ conjugate(const conjugate<double>& cd)
+ { re = (long double)cd.real(); negIm = (long double)cd.negImag(); }
+
+ conjugate(const float& rf)
+ { re = (long double)rf; negIm = 0.L; }
+ conjugate(const double& rd)
+ { re = (long double)rd; negIm = 0.L; }
+
+
+ // Conversions from complex are always explicit. Note that the value
+ // represented by the conjugate must be identical to that represented by
+ // the complex, which means we must negate the imaginary part.
+ explicit conjugate(const complex<float>& x)
+ { re = (long double)x.real(); negIm = (long double)(-x.imag()); }
+ explicit conjugate(const complex<double>& x)
+ { re = (long double)x.real(); negIm = (long double)(-x.imag()); }
+ explicit conjugate(const complex<long double>& x)
+ { re = x.real(); negIm = -x.imag(); }
+
+ /// Implicit conversion to complex<long double> when necessary
+ /// (costs an actual negation -- yuck!).
+ operator complex<long double>() const
+ { return complex<long double>(re,-negIm); }
+
+ // Can't defer here by casting to negator<conjugate> -- this must act
+ // like a built-in. But ... we can use this as a chance to convert
+ // to complex and save one negation.
+ complex<long double> operator-() const
+ { return complex<long double>(-re,negIm); }
+
+ // Useless.
+ const conjugate& operator+() const { return *this; }
+
+ // Computed assignment operators. We don't depend on implicit conversions
+ // from reals to conjugates here because we can save a few flops by handling
+ // the reals explicitly. Note that we only provide operators for implicitly
+ // convertible precisions, though, which in this case means any floating
+ // point precision.
+ conjugate& operator=(const long double& r)
+ { re = r; negIm = 0.L; return *this; }
+ conjugate& operator+=(const long double& r)
+ { re += r; return *this; }
+ conjugate& operator-=(const long double& r)
+ { re -= r; return *this; }
+ conjugate& operator*=(const long double& r)
+ { re *= r; negIm *= r; return *this; }
+ conjugate& operator/=(const long double& r)
+ { re /= r; negIm /= r; return *this; }
+
+ conjugate& operator=(const double& r)
+ { re = r; negIm = 0.L; return *this; }
+ conjugate& operator+=(const double& r)
+ { re += r; return *this; }
+ conjugate& operator-=(const double& r)
+ { re -= r; return *this; }
+ conjugate& operator*=(const double& r)
+ { re *= r; negIm *= r; return *this; }
+ conjugate& operator/=(const double& r)
+ { re /= r; negIm /= r; return *this; }
+
+ conjugate& operator=(const float& r)
+ { re = r; negIm = 0.L; return *this; }
+ conjugate& operator+=(const float& r)
+ { re += r; return *this; }
+ conjugate& operator-=(const float& r)
+ { re -= r; return *this; }
+ conjugate& operator*=(const float& r)
+ { re *= r; negIm *= r; return *this; }
+ conjugate& operator/=(const float& r)
+ { re /= r; negIm /= r; return *this; }
+
+ // Disambiguate int to be a long double.
+ conjugate& operator =(int i) {*this =(long double)i; return *this;}
+ conjugate& operator+=(int i) {*this+=(long double)i; return *this;}
+ conjugate& operator-=(int i) {*this-=(long double)i; return *this;}
+ conjugate& operator*=(int i) {*this*=(long double)i; return *this;}
+ conjugate& operator/=(int i) {*this/=(long double)i; return *this;}
+
+ conjugate& operator+=(const conjugate<long double>& c)
+ { re += c.re; negIm += c.negIm; return *this; }
+ conjugate& operator-=(const conjugate<long double>& c)
+ { re -= c.re; negIm -= c.negIm; return *this; }
+
+ conjugate& operator+=(const conjugate<double>& c)
+ { re += c.real(); negIm += c.negImag(); return *this; }
+ conjugate& operator-=(const conjugate<double>& c)
+ { re -= c.real(); negIm -= c.negImag(); return *this; }
+
+ conjugate& operator+=(const conjugate<float>& c)
+ { re += c.real(); negIm += c.negImag(); return *this; }
+ conjugate& operator-=(const conjugate<float>& c)
+ { re -= c.real(); negIm -= c.negImag(); return *this; }
+
+ conjugate& operator=(const complex<long double>& c)
+ { re = c.real(); negIm = -c.imag(); return *this; }
+ conjugate& operator+=(const complex<long double>& c)
+ { re += c.real(); negIm -= c.imag(); return *this; }
+ conjugate& operator-=(const complex<long double>& c)
+ { re -= c.real(); negIm += c.imag(); return *this; }
+
+ conjugate& operator=(const complex<double>& c)
+ { re = c.real(); negIm = -c.imag(); return *this; }
+ conjugate& operator+=(const complex<double>& c)
+ { re += c.real(); negIm -= c.imag(); return *this; }
+ conjugate& operator-=(const complex<double>& c)
+ { re -= c.real(); negIm += c.imag(); return *this; }
+
+ conjugate& operator=(const complex<float>& c)
+ { re = c.real(); negIm = -c.imag(); return *this; }
+ conjugate& operator+=(const complex<float>& c)
+ { re += c.real(); negIm -= c.imag(); return *this; }
+ conjugate& operator-=(const complex<float>& c)
+ { re -= c.real(); negIm += c.imag(); return *this; }
+
+ // It is pleasant to note that we can self-multiply by either a complex or
+ // a conjugate (leaving a conjugate result) in six flops which is the same
+ // cost as an ordinary complex multiply:
+ // cplx=cplx*cplx: (a+bi)(r+si) = (ar-bs)+(as+br)i
+ // conj=conj*conj: (a-bi)(r-si) = (ar-bs)-(as+br)i
+ // conj=conj*cplx: (a-bi)(r+si) = (ar+bs)-(br-as)i
+ conjugate& operator*=(const conjugate<long double>& c) {
+ const long double r=(re*c.re - negIm*c.negIm);
+ negIm=(re*c.negIm + negIm*c.re); re=r; return *this;
+ }
+ conjugate& operator*=(const complex<long double>& t) {
+ const long double r=(re*t.real() + negIm*t.imag());
+ negIm=(negIm*t.real() - re*t.imag()); re=r; return *this;
+ }
+
+ conjugate& operator*=(const conjugate<double>& c) { return operator*=(conjugate<long double>(c)); }
+ conjugate& operator*=(const complex<double>& c) { return operator*=(complex<long double>(c)); }
+ conjugate& operator*=(const conjugate<float>& c) { return operator*=(conjugate<long double>(c)); }
+ conjugate& operator*=(const complex<float>& c) { return operator*=(complex<long double>(c)); }
+
+ // Complex divide is messy and slow anyway so we'll convert to complex and back here,
+ // making use of the fact that for complex c and d, c/d=conj(conj(c)/conj(d)).
+ conjugate& operator/=(const conjugate<long double>& d) {
+ const complex<long double> t = conj()/d.conj();
+ re = t.real(); negIm = t.imag(); // conjugating!
+ return *this;
+ }
+ conjugate& operator/=(const complex<long double>& d) {
+ const complex<long double> t = conj()/std::conj(d);
+ re = t.real(); negIm = t.imag(); // conjugating!
+ return *this;
+ }
+
+ conjugate& operator/=(const conjugate<double>& c) { return operator/=(conjugate<long double>(c)); }
+ conjugate& operator/=(const complex<double>& c) { return operator/=(complex<long double>(c)); }
+ conjugate& operator/=(const conjugate<float>& c) { return operator/=(conjugate<long double>(c)); }
+ conjugate& operator/=(const complex<float>& c) { return operator/=(complex<long double>(c)); }
+
+ const long double& real() const { return re; }
+ long double& real() { return re; }
+
+ const negator<long double>& imag() const { return reinterpret_cast<const negator<long double>&>(negIm); }
+ negator<long double>& imag() { return reinterpret_cast<negator<long double>&>(negIm); }
+
+ const complex<long double>& conj() const { return reinterpret_cast<const complex<long double>&>(*this); }
+ complex<long double>& conj() { return reinterpret_cast<complex<long double>&>(*this); }
+
+ // Special conjugate methods of use primarily in operator implementations.
+ const long double& negImag() const { return negIm; }
+ long double& negImag() { return negIm; }
+ bool isReal() const { return negIm==0.L; }
+
+private:
+ long double re; // The value represented here is re - negIm*i.
+ long double negIm;
+};
+
+// These definitions had to be deferred until all the specializations have been declared.
+conjugate<float>::conjugate(const conjugate<double>& cd) {
+ re = float(cd.real()); negIm = float(cd.negImag());
+}
+conjugate<float>::conjugate(const conjugate<long double>& cl) {
+ re = float(cl.real()); negIm = float(cl.negImag());
+}
+conjugate<double>::conjugate(const conjugate<long double>& cl) {
+ re = double(cl.real()); negIm = double(cl.negImag());
+}
+
+// Global functions real(),imag(), conj(), abs(), and norm() are overloaded here
+// for efficiency (e.g., abs(c)==abs(conj(c))). The others still work through
+// the implicit conversion from conjugate<T> to complex<T>, which costs
+// one negation. The non-overloaded functions defined for complex are
+// arg(), which returns a real, and a set of functions which return complex:
+// polar,cos,cosh,exp,log,log10,pow,sin,sinh,sqrt,tan,tanh.
+inline const float& real(const conjugate<float>& c) { return c.real(); }
+inline const negator<float>& imag(const conjugate<float>& c) { return c.imag(); }
+inline const complex<float>& conj(const conjugate<float>& c) { return c.conj(); }
+inline float abs (const conjugate<float>& c) { return std::abs(c.conj()); }
+inline float norm(const conjugate<float>& c) { return std::norm(c.conj()); }
+
+inline const double& real(const conjugate<double>& c) { return c.real(); }
+inline const negator<double>& imag(const conjugate<double>& c) { return c.imag(); }
+inline const complex<double>& conj(const conjugate<double>& c) { return c.conj(); }
+inline double abs (const conjugate<double>& c) { return std::abs(c.conj()); }
+inline double norm(const conjugate<double>& c) { return std::norm(c.conj()); }
+
+inline const long double& real(const conjugate<long double>& c) { return c.real(); }
+inline const negator<long double>& imag(const conjugate<long double>& c) { return c.imag(); }
+inline const complex<long double>& conj(const conjugate<long double>& c) { return c.conj(); }
+inline long double abs (const conjugate<long double>& c) { return std::abs(c.conj()); }
+inline long double norm(const conjugate<long double>& c) { return std::norm(c.conj()); }
+
+
+
+
+
+
+// Binary operators with conjugate as one of the operands, and the other any
+// numerical type (real, complex, conjugate) but NOT a negator type. Each operator
+// will silently work with operands of mixed precision, widening the result as
+// necessary. We try to return complex rather than conjugate whenever possible.
+
+template <class R, class CHAR, class TRAITS> inline std::basic_istream<CHAR,TRAITS>&
+operator>>(std::basic_istream<CHAR,TRAITS>& is, conjugate<R>& c) {
+ complex<R> z; is >> z; c=z;
+ return is;
+}
+template <class R, class CHAR, class TRAITS> inline std::basic_ostream<CHAR,TRAITS>&
+operator<<(std::basic_ostream<CHAR,TRAITS>& os, const conjugate<R>& c) {
+ return os << complex<R>(c);
+}
+
+// Operators involving only conjugate and complex can be templatized reliably, as can
+// operators which do not mix precision. But we have to deal explicitly with mixes
+// of conjugate<R> and some other real type S, because the 'class S' template
+// argument can match anything and create ambiguities.
+
+// conjugate<R> with float, double, long double. With 'float' we can be sure that R
+// is the right width for the return value. With 'long double' we are sure that
+// 'long double' is the return width. 'double' is trickier and we have to use the
+// Wider<R,...> helper class to give us the right return type.
+
+// Commutative ops need be done only once: +, *, ==, and != is defined in terms of ==.
+
+// conjugate = conjugate + real
+template <class R> inline conjugate<R> operator+(const conjugate<R>& a, const float& b)
+ { return conjugate<R>(a) += b; }
+template <class R> inline conjugate<long double> operator+(const conjugate<R>& a, const long double& b)
+ { return conjugate<long double>(a) += b; }
+template <class R> inline typename Wider<R,double>::WConj operator+(const conjugate<R>& a, const double& b)
+ { return typename Wider<R,double>::WConj(a) += b; }
+
+// conjugate = real + conjugate
+template <class R> inline conjugate<R> operator+(const float& a, const conjugate<R>& b) {return b+a;}
+template <class R> inline conjugate<long double> operator+(const long double& a, const conjugate<R>& b) {return b+a;}
+template <class R> inline typename Wider<R,double>::WConj operator+(const double& a, const conjugate<R>& b) {return b+a;}
+
+// conjugate = conjugate * real
+template <class R> inline conjugate<R> operator*(const conjugate<R>& a, const float& b)
+ { return conjugate<R>(a) *= b; }
+template <class R> inline conjugate<long double> operator*(const conjugate<R>& a, const long double& b)
+ { return conjugate<long double>(a) *= b; }
+template <class R> inline typename Wider<R,double>::WConj operator*(const conjugate<R>& a, const double& b)
+ { return typename Wider<R,double>::WConj(a) *= b; }
+
+// conjugate = real * conjugate
+template <class R> inline conjugate<R> operator*(const float& a, const conjugate<R>& b) {return b*a;}
+template <class R> inline conjugate<long double> operator*(const long double& a, const conjugate<R>& b) {return b*a;}
+template <class R> inline typename Wider<R,double>::WConj operator*(const double& a, const conjugate<R>& b) {return b*a;}
+
+// bool = conjugate==real
+template <class R> inline bool operator==(const conjugate<R>& a, const float& b)
+ { return a.isReal() && a.real()==b; }
+template <class R> inline bool operator==(const conjugate<R>& a, const long double& b)
+ { return a.isReal() && a.real()==b; }
+template <class R> inline bool operator==(const conjugate<R>& a, const double& b)
+ { return a.isReal() && a.real()==b; }
+
+// bool = real==conjugate, bool = conjugate!=real, bool = real!=conjugate
+template <class R> inline bool operator==(const float& a, const conjugate<R>& b) {return b==a;}
+template <class R> inline bool operator==(const long double& a, const conjugate<R>& b) {return b==a;}
+template <class R> inline bool operator==(const double& a, const conjugate<R>& b) {return b==a;}
+template <class R> inline bool operator!=(const conjugate<R>& a, const float& b) {return !(a==b);}
+template <class R> inline bool operator!=(const conjugate<R>& a, const long double& b) {return !(a==b);}
+template <class R> inline bool operator!=(const conjugate<R>& a, const double& b) {return !(a==b);}
+template <class R> inline bool operator!=(const float& a, const conjugate<R>& b) {return !(a==b);}
+template <class R> inline bool operator!=(const long double& a, const conjugate<R>& b) {return !(a==b);}
+template <class R> inline bool operator!=(const double& a, const conjugate<R>& b) {return !(a==b);}
+
+// Non-commutative ops are a little messier.
+
+// conjugate = conjugate - real
+template <class R> inline conjugate<R> operator-(const conjugate<R>& a, const float& b)
+ { return conjugate<R>(a) -= b; }
+template <class R> inline conjugate<long double> operator-(const conjugate<R>& a, const long double& b)
+ { return conjugate<long double>(a) -= b; }
+template <class R> inline typename Wider<R,double>::WConj operator-(const conjugate<R>& a, const double& b)
+ { return typename Wider<R,double>::WConj(a) -= b; }
+
+// complex = real - conjugate
+// This is nice because -conjugate.imag() is free.
+template <class R> inline complex<R> operator-(const float& a, const conjugate<R>& b)
+ { return complex<R>(a-b.real(), -b.imag()); }
+template <class R> inline complex<long double> operator-(const long double& a, const conjugate<R>& b)
+ { return complex<long double>(a-b.real(), -b.imag()); }
+template <class R> inline typename Wider<R,double>::WCplx operator-(const double& a, const conjugate<R>& b)
+ { return typename Wider<R,double>::WCplx(a-b.real(), -b.imag()); }
+
+// conjugate = conjugate / real
+template <class R> inline conjugate<R> operator/(const conjugate<R>& a, const float& b)
+ { return conjugate<R>(a) /= b; }
+template <class R> inline conjugate<long double> operator/(const conjugate<R>& a, const long double& b)
+ { return conjugate<long double>(a) /= b; }
+template <class R> inline typename Wider<R,double>::WConj operator/(const conjugate<R>& a, const double& b)
+ { return typename Wider<R,double>::WConj(a) /= b; }
+
+// complex = real / conjugate
+// Division by complex is tricky and slow anyway so we'll just convert to complex
+// at the cost of one negation.
+template <class R> inline complex<R> operator/(const float& a, const conjugate<R>& b)
+ { return (R)a/complex<R>(b); }
+template <class R> inline complex<long double> operator/(const long double& a, const conjugate<R>& b)
+ { return a/complex<long double>(b); }
+template <class R> inline typename Wider<R,double>::WCplx operator/(const double& a, const conjugate<R>& b)
+ { return (typename Wider<R,double>::WReal)a/(typename Wider<R,double>::WCplx(b)); }
+
+
+// That's it for (conjugate, real) combinations. Now we need to do all the (conjugate, conjugate) and
+// (conjugate, complex) combinations which are safer to templatize fully. There are many more opportunities
+// to return complex rather than conjugate here. Keep in mind here that for a conjugate number c=a-bi,
+// a=c.real() and b=-c.imag() are available for free. conjugate provides negImag() to return b directly.
+// Below we'll capitalize components that should be accessed with negImag().
+
+// conjugate = conjugate + conjugate: (a-Bi)+(r-Si) = (a+r)-(B+S)i
+template <class R, class S> inline typename Wider<R,S>::WConj
+operator+(const conjugate<R>& a, const conjugate<S>& r) {
+ return typename Wider<R,S>::WConj(a.real()+r.real(),
+ a.negImag()+r.negImag());
+}
+
+// complex = conjugate + complex = complex + conjugate: (a-Bi)+(r+si) = (a+r)+(s-B)i
+template <class R, class S> inline typename Wider<R,S>::WCplx
+operator+(const conjugate<R>& a, const complex<S>& r) {
+ return typename Wider<R,S>::WCplx(a.real()+r.real(),
+ r.imag()-a.negImag());
+}
+template <class R, class S> inline typename Wider<R,S>::WCplx
+operator+(const complex<R>& a, const conjugate<S>& r) { return r+a; }
+
+// complex = conjugate - conjugate: (a-Bi)-(r-Si) = (a-r)+(S-B)i
+template <class R, class S> inline typename Wider<R,S>::WCplx
+operator-(const conjugate<R>& a, const conjugate<S>& r) {
+ return typename Wider<R,S>::WCplx(a.real()-r.real(),
+ r.negImag()-a.negImag());
+}
+
+// Neg<complex> = conjugate - complex: (a-Bi)-(r+si) = (a-r)+(-B-s)i = -[(r-a)+(B+s)i]
+template <class R, class S> inline negator<typename Wider<R,S>::WCplx>
+operator-(const conjugate<R>& a, const complex<S>& r) {
+ return negator<typename Wider<R,S>::WCplx>::recast
+ (typename Wider<R,S>::WCplx(r.real()-a.real(),
+ a.negImag()+r.imag()));
+}
+
+// complex = complex - conjugate: (a+bi)-(r-Si) = (a-r)+(b+S)i
+template <class R, class S> inline typename Wider<R,S>::WCplx
+operator-(const complex<R>& a, const conjugate<S>& r) {
+ return typename Wider<R,S>::WCplx(a.real()-r.real(),
+ a.imag()+r.negImag());
+}
+
+// We can multiply by either a complex or a conjugate (leaving a complex
+// or negated complex result) in six flops which is the same cost as an
+// ordinary complex multiply.
+// (cplx=cplx*cplx: (a+bi)(r+si) = (ar-bs)+(as+br)i)
+
+// Neg<cplx>=conj*conj: (a-Bi)(r-Si) = -[(BS-ar)+(aS+Br)i]
+template <class R, class S> inline negator<typename Wider<R,S>::WCplx>
+operator*(const conjugate<R>& a, const conjugate<S>& r) {
+ return negator<typename Wider<R,S>::WCplx>::recast
+ (typename Wider<R,S>::WCplx(a.negImag()*r.negImag() - a.real()*r.real(),
+ a.real()*r.negImag() + a.negImag()*r.real()));
+}
+
+// cplx=conj*cplx: (a-Bi)(r+si) = (ar+Bs)+(as-Br)i
+template <class R, class S> inline typename Wider<R,S>::WCplx
+operator*(const conjugate<R>& a, const complex<S>& r) {
+ return typename Wider<R,S>::WCplx(a.real()*r.real() + a.negImag()*r.imag(),
+ a.real()*r.imag() - a.negImag()*r.real());
+}
+
+template <class R, class S> inline typename Wider<R,S>::WCplx
+operator*(const complex<R>& a, const conjugate<S>& r)
+ { return r*a; }
+
+// If there's a negator on the complex number, move it to the conjugate
+// one which will change to complex with just one flop; then this
+// is an ordinary complex mutiply.
+template <class R, class S> inline typename Wider<R,S>::WCplx
+operator*(const negator< complex<R> >& a, const conjugate<S>& r)
+ { return (-a)*(-r); } // -a is free here
+template <class R, class S> inline typename Wider<R,S>::WCplx
+operator*(const conjugate<R>& a, const negator< complex<S> >& r)
+ { return (-a)*(-r); } // -r is free here
+
+// Division is tricky and there is little to gain by trying to exploit
+// the conjugate class, so we'll just convert to complex. (Remember that
+// conj() is free for Conjugates.)
+
+template <class R, class S> inline typename Wider<R,S>::WCplx
+operator/(const conjugate<R>& a, const conjugate<S>& r) {
+ return std::conj(a.conj()/r.conj());
+}
+
+template <class R, class S> inline typename Wider<R,S>::WCplx
+operator/(const conjugate<R>& a, const complex<S>& r) {
+ return std::conj(a.conj()/std::conj(r));
+}
+
+template <class R, class S> inline typename Wider<R,S>::WCplx
+operator/(const complex<R>& a, const conjugate<S>& r) {
+ return std::conj(std::conj(a)/r.conj());
+}
+
+
+template <class R, class S> inline bool
+operator==(const conjugate<R>& a, const conjugate<S>& r) {
+ return a.real() == r.real() && a.negImag() == r.negImag();
+}
+
+template <class R, class S> inline bool
+operator==(const conjugate<R>& a, const complex<S>& r) {
+ return a.real() == r.real() && -a.negImag() == r.imag();
+}
+
+template <class R, class S> inline bool
+operator==(const complex<R>& a, const conjugate<S>& r) {return r==a;}
+
+template <class R, class S> inline bool
+operator!=(const conjugate<R>& a, const conjugate<S>& r) {return !(a==r);}
+
+template <class R, class S> inline bool
+operator!=(const conjugate<R>& a, const complex<S>& r) {return !(a==r);}
+
+template <class R, class S> inline bool
+operator!=(const complex<R>& a, const conjugate<S>& r) {return !(a==r);}
+
+
+} // namespace SimTK
+
+#endif //SimTK_SIMMATRIX_CONJUGATE_H_
diff --git a/SimTKcommon/Scalar/include/SimTKcommon/internal/negator.h b/SimTKcommon/Scalar/include/SimTKcommon/internal/negator.h
new file mode 100644
index 0000000..1d92ec5
--- /dev/null
+++ b/SimTKcommon/Scalar/include/SimTKcommon/internal/negator.h
@@ -0,0 +1,409 @@
+#ifndef SimTK_SIMMATRIX_NEGATOR_H_
+#define SimTK_SIMMATRIX_NEGATOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This file defines the negator<N> template which is an adaptor for
+ * the numeric types N (Real, Complex, conjugate). negator must NOT
+ * be instantiated for anything other than these three types (each
+ * of which comes in three precisions). negator<N> is guaranteed to
+ * have the same memory layout as N, except that the stored values
+ * represent the *negative* value of that number.
+ *
+ * This is part of the SimTK Scalar package, which forms the basis
+ * for composite numerical types like vectors and matrices. The negator
+ * class allows negation to be performed by reinterpretation rather
+ * than by computation.
+ *
+ * @verbatim
+ *
+ * The Scalar Types
+ * ----------------
+ * Here is a complete taxonomy of the scalar types we support.
+ *
+ * <scalar> ::= <number> | negator< <number> >
+ * <number> ::= <standard> | <conjugate>
+ * <standard> ::= <real> | <complex>
+ *
+ * <real> ::= float | double | long double
+ * <complex> ::= std::complex< <real> >
+ * <conjugate> ::= SimTK::conjugate< <real> >
+ *
+ * @endverbatim
+ */
+
+#include <iostream>
+
+namespace SimTK {
+
+// Specializations of this class provide information about Composite Numerical Types
+// (i.e. composite types) in the style of std::numeric_limits<T>.
+template <class T> class CNT;
+template <class N> class NTraits; // Same, but only defined for numeric types.
+template <class N> class negator; // negator is only defined for numbers.
+
+
+/**
+ * negator<N>, where N is a number type (real, complex, conjugate), is represented in
+ * memory identically to N, but behaves as though multiplied by -1, though at zero
+ * cost. Only negators instantiated with the nine number types (real, complex,
+ * conjugate) are allowed.
+ */
+template <class NUMBER>
+class SimTK_SimTKCOMMON_EXPORT negator {
+ typedef NUMBER N;
+ typedef typename NTraits<N>::TReal NReal;
+ typedef typename NTraits<N>::TImag NImag;
+ typedef typename NTraits<N>::TComplex NComplex;
+ typedef typename NTraits<N>::THerm NHerm;
+ typedef typename NTraits<N>::TInvert NInvert;
+public:
+ typedef negator<N> T;
+ typedef NUMBER TNeg; // negator evaporates
+ typedef NUMBER TWithoutNegator; // "
+ typedef typename CNT<NReal>::TNeg TReal;
+ typedef typename CNT<NImag>::TNeg TImag;
+ typedef typename CNT<NComplex>::TNeg TComplex;
+ typedef typename CNT<NHerm>::TNeg THerm;
+ typedef negator<N> TPosTrans;
+ typedef typename NTraits<N>::TSqHermT TSqHermT;
+ typedef typename NTraits<N>::TSqTHerm TSqTHerm;
+ typedef negator<N> TElement;
+ typedef negator<N> TRow;
+ typedef negator<N> TCol;
+
+ typedef typename NTraits<N>::TSqrt TSqrt;
+ typedef typename NTraits<N>::TAbs TAbs;
+ typedef typename NTraits<N>::TStandard TStandard;
+ typedef typename CNT<NInvert>::TNeg TInvert;
+ typedef typename NTraits<N>::TStandard TNormalize; // neg<conj> -> complex
+
+
+ typedef negator<N> Scalar;
+ typedef negator<N> ULessScalar;
+ typedef NUMBER Number;
+ typedef typename NTraits<N>::StdNumber StdNumber;
+ typedef typename NTraits<N>::Precision Precision;
+ typedef typename NTraits<N>::ScalarNormSq ScalarNormSq;
+
+ // negator may be used in combination with any composite numerical type, not just
+ // numbers. Hence we must use CNT<P> here rather than NTraits<P> (they are
+ // the same when P turns out to be a numeric type).
+ // Typeof( Neg<N>*P ) is Typeof(P*N)::TNeg
+ // Typeof( Neg<N>/P ) is Typeof(N/P)::TNeg
+ // Typeof( Neg<N>+P ) is Typeof(P-N)
+ // Typeof( Neg<N>-P ) is Typeof(P+N)::TNeg
+ // No need to specialize because we strip off the "negator" here.
+ template <class P> struct Result {
+ private:
+ // These are the type of the calculations we actually perform.
+ typedef typename CNT<P>::template Result<N>::Mul PMul;
+ typedef typename NTraits<N>::template Result<P>::Dvd PDvd;
+ typedef typename CNT<P>::template Result<N>::Sub PAdd;
+ typedef typename CNT<P>::template Result<N>::Add PSub;
+ public:
+ // These are the types to which we must recast the results.
+ typedef typename CNT<PMul>::TNeg Mul;
+ typedef typename CNT<PDvd>::TNeg Dvd;
+ typedef PAdd Add;
+ typedef typename CNT<PSub>::TNeg Sub;
+ };
+
+ // Shape-preserving element substitution (easy for scalars!)
+ template <class P> struct Substitute {
+ typedef P Type;
+ };
+
+ enum {
+ NRows = 1, // Negators are always scalars
+ NCols = 1,
+ RowSpacing = 1,
+ ColSpacing = 1,
+ NPackedElements = 1,
+ NActualElements = 1,
+ NActualScalars = 1,
+ ImagOffset = NTraits<N>::ImagOffset,
+ RealStrideFactor = NTraits<N>::RealStrideFactor,
+ ArgDepth = SCALAR_DEPTH,
+ IsScalar = 1,
+ IsULessScalar = 1,
+ IsNumber = 0,
+ IsStdNumber = 0,
+ IsPrecision = 0,
+ SignInterpretation = -1 // if you cast away the negator, don't forget this!
+ };
+ const negator<N>* getData() const {return this;}
+ negator<N>* updData() {return this;}
+
+ const TReal& real() const {return reinterpret_cast<const TReal&>(NTraits<N>::real(v));}
+ TReal& real() {return reinterpret_cast< TReal&>(NTraits<N>::real(v));}
+ const TImag& imag() const {return reinterpret_cast<const TImag&>(NTraits<N>::imag(v));}
+ TImag& imag() {return reinterpret_cast< TImag&>(NTraits<N>::imag(v));}
+
+ ScalarNormSq scalarNormSqr() const {return NTraits<N>::scalarNormSqr(v);}
+ TSqrt sqrt() const {return NTraits<N>::sqrt(N(v));}
+ TAbs abs() const {return NTraits<N>::abs(v);}
+ TStandard standardize() const {return -NTraits<N>::standardize(v);}
+ TNormalize normalize() const {return -NTraits<N>::normalize(v);}
+
+ // Inverse (1/x) of a non-negated type N will return a non-negated type, so we
+ // can cast it to a negated type here to save a flop. The return type might
+ // not be N (a negated conjugate comes back as a complex), so there may be
+ // a flop done in the final conversion to TInvert.
+ TInvert invert() const {return recast(NTraits<N>::invert(v));}
+
+ static negator<N> getNaN() {return recast(NTraits<N>::getNaN());}
+ static negator<N> getInfinity() {return recast(NTraits<N>::getInfinity());}
+
+ /// Returns true if the negated value is finite (i.e., not NaN or Inf).
+ inline bool isFinite() const;
+ /// Returns true if the negated value contains a NaN.
+ inline bool isNaN() const;
+ /// Returns true if the negated value contains an Inf or -Inf and does not
+ /// contain a NaN.
+ inline bool isInf() const;
+
+ static double getDefaultTolerance() {return NTraits<N>::getDefaultTolerance();}
+
+ /// In the generic case we'll perform the negation here to get a number,
+ /// and then delegate to the other type which can be any CNT.
+ template <class T2> bool isNumericallyEqual(const T2& t2) const
+ { return CNT<T2>::isNumericallyEqual(t2, -v); } // perform negation
+
+ /// In this partial specialization we know that both types have negators so we
+ /// can just compare the underlying numbers, each of which has the reversed sign,
+ /// using the global SimTK method available for comparing numbers.
+ template <class N2> bool isNumericallyEqual(const negator<N2>& t2) const
+ { return SimTK::isNumericallyEqual(v, t2.v); }
+
+ /// This is the generic case (see above) but with an explicitly-provided tolerance.
+ template <class T2> bool isNumericallyEqual(const T2& t2, double tol) const
+ { return CNT<T2>::isNumericallyEqual(t2, -v, tol); } // perform negation
+
+ /// This is the partially specialized case again (see above) but with an explicitly-provided
+ /// tolerance.
+ template <class N2> bool isNumericallyEqual(const negator<N2>& t2, double tol) const
+ { return SimTK::isNumericallyEqual(v, t2.v, tol); }
+
+
+ negator() {
+ #ifndef NDEBUG
+ v = NTraits<N>::getNaN();
+ #endif
+ }
+ negator(const negator& n) : v(n.v) { }
+ negator& operator=(const negator& n) { v=n.v; return *this; }
+
+ // These are implicit conversions from numeric type NN to negator<N>. The
+ // value must be unchanged, so we must negate. Note that NN==N is a
+ // certainty for one of these cases.
+ negator(int t) {v = -N((typename NTraits<N>::Precision)t);}
+ negator(const float& t) {v = -N((typename NTraits<N>::Precision)t);}
+ negator(const double& t) {v = -N((typename NTraits<N>::Precision)t);}
+ negator(const long double& t) {v = -N((typename NTraits<N>::Precision)t);}
+
+ // Some of these may not compile if instantiated -- you can't cast a complex
+ // to a float, for example.
+ template <class P> negator(const std::complex<P>& t) {v = -N(t);}
+ template <class P> negator(const conjugate<P>& t) {v = -N(t);}
+
+ // This can be used to negate a value of type N at zero cost. It is typically
+ // used for recasting temporary expressions to apply a final negation. Note that
+ // this is *not* the same as constructing a negator<N> from an N, which actually
+ // peforms a floating point negation.
+ static const negator<N>& recast(const N& val)
+ { return reinterpret_cast<const negator<N>&>(val); }
+
+ const N& operator-() const { return v; }
+ N& operator-() { return v; } // an lvalue!
+ N operator+() const { return N(-v); } // performs the actual negation (expensive)
+
+ operator N() const { return N(-v); } // implicit conversion to N (expensive)
+
+ template <class P> negator& operator =(const P& t) { v = -t; return *this; }
+ template <class P> negator& operator+=(const P& t) { v -= t; return *this; } //swap sign
+ template <class P> negator& operator-=(const P& t) { v += t; return *this; }
+ template <class P> negator& operator*=(const P& t) { v *= t; return *this; } //don't swap!
+ template <class P> negator& operator/=(const P& t) { v /= t; return *this; }
+
+ // If we know we've got a negator as an argument, get rid of its negation
+ // and change signs as necessary. We're guaranteed to get rid of at least
+ // one negator<> this way. Nothing to gain for multiplication or division,
+ // though.
+ template <class NN> negator& operator =(const negator<NN>& t)
+ { v = -t; return *this; }
+ template <class NN> negator& operator+=(const negator<NN>& t)
+ { v += -t; return *this; } //swap sign
+ template <class NN> negator& operator-=(const negator<NN>& t)
+ { v -= -t; return *this; }
+
+private:
+ N v;
+
+template <class N2> friend class negator;
+};
+
+// isNaN() for real, complex, and conjugate numbers is provided in
+// NTraits. Here we add isNaN() for negated scalar types.
+
+/// @addtogroup isNaN
+//@{
+inline bool isNaN(const negator<float>& x) {return isNaN(-x);}
+inline bool isNaN(const negator<double>& x) {return isNaN(-x);}
+inline bool isNaN(const negator<long double>& x) {return isNaN(-x);}
+template <class P> inline bool
+isNaN(const negator< std::complex<P> >& x) {return isNaN(-x);}
+template <class P> inline bool
+isNaN(const negator< conjugate<P> >& x) {return isNaN(-x);}
+//@}
+
+// isFinite() for real, complex, and conjugate numbers is provided in
+// NTraits. Here we add isFinite() for negated scalar types.
+
+/// @addtogroup isFinite
+//@{
+inline bool isFinite(const negator<float>& x) {return isFinite(-x);}
+inline bool isFinite(const negator<double>& x) {return isFinite(-x);}
+inline bool isFinite(const negator<long double>& x) {return isFinite(-x);}
+template <class P> inline bool
+isFinite(const negator< std::complex<P> >& x) {return isFinite(-x);}
+template <class P> inline bool
+isFinite(const negator< conjugate<P> >& x) {return isFinite(-x);}
+//@}
+
+// isInf(x) for real, complex, and conjugate numbers is provided in
+// NTraits. Here we add isInf() for negated scalar types.
+
+/// @addtogroup isInf
+//@{
+inline bool isInf(const negator<float>& x) {return isInf(-x);}
+inline bool isInf(const negator<double>& x) {return isInf(-x);}
+inline bool isInf(const negator<long double>& x) {return isInf(-x);}
+template <class P> inline bool
+isInf(const negator< std::complex<P> >& x) {return isInf(-x);}
+template <class P> inline bool
+isInf(const negator< conjugate<P> >& x) {return isInf(-x);}
+//@}
+
+// The member functions call the global ones just defined.
+template <class N> inline bool
+negator<N>::isFinite() const {return SimTK::isFinite(*this);}
+template <class N> inline bool
+negator<N>::isNaN() const {return SimTK::isNaN(*this);}
+template <class N> inline bool
+negator<N>::isInf() const {return SimTK::isInf(*this);}
+
+// Handle all binary numerical operators involving a negator<A> and a B, or negator<A>
+// and negator<B>, obtaining results by stripping away the negator<>s and fiddling
+// with signs appropriately.
+// Careful: don't remove both negators in one step because Result isn't specialized
+// for negators so it might not predict the same result type as you actually get.
+// Be patient and let it strip one negator at a time -- in Release mode that won't
+// add any code since all this stuff drops away.
+//
+// To appreciate the beauty of these operators, remember that -x is free when x
+// is a negator.
+
+template <class DEST, class SRC> static inline const DEST&
+negRecast(const SRC& s) { return reinterpret_cast<const DEST&>(s); }
+
+ // Binary '+' with a negator as one or both arguments //
+template <class A, class B> inline typename negator<A>::template Result<B>::Add
+operator+(const negator<A>& l, const B& r)
+ {return negRecast<typename negator<A>::template Result<B>::Add>(r-(-l));}
+template <class A, class B> inline typename CNT<A>::template Result<negator<B> >::Add
+operator+(const A& l, const negator<B>& r)
+ {return negRecast<typename CNT<A>::template Result<negator<B> >::Add>(l-(-r));}
+// One step at a time here (see above).
+template <class A, class B> inline typename negator<A>::template Result<negator<B> >::Add
+operator+(const negator<A>& l, const negator<B>& r)
+ {return negRecast<typename negator<A>::template Result<negator<B> >::Add>(r-(-l)); }
+
+ // Binary '-' with a negator as one or both arguments //
+template <class A, class B> inline typename negator<A>::template Result<B>::Sub
+operator-(const negator<A>& l, const B& r)
+ {return negRecast<typename negator<A>::template Result<B>::Sub>(r+(-l));}
+template <class A, class B> inline typename CNT<A>::template Result<negator<B> >::Sub
+operator-(const A& l, const negator<B>& r)
+ {return negRecast<typename CNT<A>::template Result<negator<B> >::Sub>(l+(-r));}
+// One step at a time here (see above).
+template <class A, class B> inline typename negator<A>::template Result<negator<B> >::Sub
+operator-(const negator<A>& l, const negator<B>& r)
+ {return negRecast<typename negator<A>::template Result<negator<B> >::Sub>(r+(-l));}
+
+// Binary '*' with a negator as one or both arguments
+template <class A, class B> inline typename negator<A>::template Result<B>::Mul
+operator*(const negator<A>& l, const B& r)
+ {return negRecast<typename negator<A>::template Result<B>::Mul>((-l)*r);}
+template <class A, class B> inline typename CNT<A>::template Result<negator<B> >::Mul
+operator*(const A& l, const negator<B>& r)
+ {return negRecast<typename CNT<A>::template Result<negator<B> >::Mul>(l*(-r));}
+// One step at a time here (see above).
+template <class A, class B> inline typename negator<A>::template Result<negator<B> >::Mul
+operator*(const negator<A>& l, const negator<B>& r)
+ {return negRecast<typename negator<A>::template Result<negator<B> >::Mul>((-l)*r);}
+
+// Binary '/' with a negator as one or both arguments
+template <class A, class B> inline typename negator<A>::template Result<B>::Dvd
+operator/(const negator<A>& l, const B& r)
+ {return negRecast<typename negator<A>::template Result<B>::Dvd>((-l)/r);}
+template <class A, class B> inline typename CNT<A>::template Result<negator<B> >::Dvd
+operator/(const A& l, const negator<B>& r)
+ {return negRecast<typename CNT<A>::template Result<negator<B> >::Dvd>(l/(-r));}
+// One step at a time here (see above).
+template <class A, class B> inline typename negator<A>::template Result<negator<B> >::Dvd
+operator/(const negator<A>& l, const negator<B>& r)
+ {return negRecast<typename negator<A>::template Result<negator<B> >::Dvd>((-l)/r);}
+
+// Binary '==' with a negator as one or both arguments
+template <class A, class B> inline bool
+operator==(const negator<A>& l, const B& r) { return (A)l == r; }
+template <class A, class B> inline bool
+operator==(const A& l, const negator<B>& r) { return l == (B)r; }
+template <class A, class B> inline bool
+operator==(const negator<A>& l, const negator<B>& r) { return (-l) == (-r); } // cheap!
+
+// The lazy man's '!=' operator.
+template <class A, class B> inline bool
+operator!=(const negator<A>& l, const B& r) { return !(l==r); }
+template <class A, class B> inline bool
+operator!=(const A& l, const negator<B>& r) { return !(l==r); }
+template <class A, class B> inline bool
+operator!=(const negator<A>& l, const negator<B>& r) { return !(l==r); }
+
+// And a final touch of elegance allowing smooth interoperability with iostream.
+template <class NUM, class CHAR, class TRAITS> inline std::basic_istream<CHAR,TRAITS>&
+operator>>(std::basic_istream<CHAR,TRAITS>& is, negator<NUM>& nn) {
+ NUM z; is >> z; nn=z;
+ return is;
+}
+template <class NUM, class CHAR, class TRAITS> inline std::basic_ostream<CHAR,TRAITS>&
+operator<<(std::basic_ostream<CHAR,TRAITS>& os, const negator<NUM>& nn) {
+ return os << NUM(nn);
+}
+
+} // namespace SimTK
+
+#endif //SimTK_SIMMATRIX_NEGATOR_H_
diff --git a/SimTKcommon/Scalar/src/Scalar.cpp b/SimTKcommon/Scalar/src/Scalar.cpp
new file mode 100644
index 0000000..4e60793
--- /dev/null
+++ b/SimTKcommon/Scalar/src/Scalar.cpp
@@ -0,0 +1,153 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Scalar.h"
+
+#include <limits>
+#include <cmath>
+
+#include <complex>
+using std::complex;
+
+namespace SimTK {
+
+
+// These constants are global external symbols exported by the library. See
+// the Scalar.h header file for information.
+
+SimTK_SimTKCOMMON_EXPORT const Real NaN = NTraits<Real>::getNaN();
+SimTK_SimTKCOMMON_EXPORT const Real Infinity = NTraits<Real>::getInfinity();
+SimTK_SimTKCOMMON_EXPORT const Real Eps = NTraits<Real>::getEps();
+SimTK_SimTKCOMMON_EXPORT const Real SqrtEps = NTraits<Real>::getSqrtEps();
+SimTK_SimTKCOMMON_EXPORT const Real TinyReal = NTraits<Real>::getTiny();
+SimTK_SimTKCOMMON_EXPORT const Real SignificantReal = NTraits<Real>::getSignificant();
+SimTK_SimTKCOMMON_EXPORT const Real LeastPositiveReal = NTraits<Real>::getLeastPositive();
+SimTK_SimTKCOMMON_EXPORT const Real MostPositiveReal = NTraits<Real>::getMostPositive();
+SimTK_SimTKCOMMON_EXPORT const Real LeastNegativeReal = NTraits<Real>::getLeastNegative();
+SimTK_SimTKCOMMON_EXPORT const Real MostNegativeReal = NTraits<Real>::getMostNegative();
+
+SimTK_SimTKCOMMON_EXPORT const int NumDigitsReal = NTraits<Real>::getNumDigits();
+SimTK_SimTKCOMMON_EXPORT const int LosslessNumDigitsReal = NTraits<Real>::getLosslessNumDigits();
+
+SimTK_SimTKCOMMON_EXPORT const Real Zero = NTraits<Real>::getZero();
+SimTK_SimTKCOMMON_EXPORT const Real One = NTraits<Real>::getOne();
+SimTK_SimTKCOMMON_EXPORT const Real MinusOne = NTraits<Real>::getMinusOne();
+SimTK_SimTKCOMMON_EXPORT const Real Two = NTraits<Real>::getTwo();
+SimTK_SimTKCOMMON_EXPORT const Real Three = NTraits<Real>::getThree();
+
+SimTK_SimTKCOMMON_EXPORT const Real OneHalf = NTraits<Real>::getOneHalf();
+SimTK_SimTKCOMMON_EXPORT const Real OneThird = NTraits<Real>::getOneThird();
+SimTK_SimTKCOMMON_EXPORT const Real OneFourth = NTraits<Real>::getOneFourth();
+SimTK_SimTKCOMMON_EXPORT const Real OneFifth = NTraits<Real>::getOneFifth();
+SimTK_SimTKCOMMON_EXPORT const Real OneSixth = NTraits<Real>::getOneSixth();
+SimTK_SimTKCOMMON_EXPORT const Real OneSeventh = NTraits<Real>::getOneSeventh();
+SimTK_SimTKCOMMON_EXPORT const Real OneEighth = NTraits<Real>::getOneEighth();
+SimTK_SimTKCOMMON_EXPORT const Real OneNinth = NTraits<Real>::getOneNinth();
+SimTK_SimTKCOMMON_EXPORT const Real Pi = NTraits<Real>::getPi();
+SimTK_SimTKCOMMON_EXPORT const Real OneOverPi = NTraits<Real>::getOneOverPi();
+SimTK_SimTKCOMMON_EXPORT const Real E = NTraits<Real>::getE();
+SimTK_SimTKCOMMON_EXPORT const Real Log2E = NTraits<Real>::getLog2E();
+SimTK_SimTKCOMMON_EXPORT const Real Log10E = NTraits<Real>::getLog10E();
+SimTK_SimTKCOMMON_EXPORT const Real Sqrt2 = NTraits<Real>::getSqrt2();
+SimTK_SimTKCOMMON_EXPORT const Real OneOverSqrt2 = NTraits<Real>::getOneOverSqrt2();
+SimTK_SimTKCOMMON_EXPORT const Real Sqrt3 = NTraits<Real>::getSqrt3();
+SimTK_SimTKCOMMON_EXPORT const Real OneOverSqrt3 = NTraits<Real>::getOneOverSqrt3();
+SimTK_SimTKCOMMON_EXPORT const Real CubeRoot2 = NTraits<Real>::getCubeRoot2();
+SimTK_SimTKCOMMON_EXPORT const Real CubeRoot3 = NTraits<Real>::getCubeRoot3();
+SimTK_SimTKCOMMON_EXPORT const Real Ln2 = NTraits<Real>::getLn2();
+SimTK_SimTKCOMMON_EXPORT const Real Ln10 = NTraits<Real>::getLn10();
+
+SimTK_SimTKCOMMON_EXPORT const Complex I = NTraits<Complex>::getI();
+
+// These instantiations are just here to make sure everything is working. We would
+// rather have these fail to compile here than in some poor user's program.
+// (sherm 090827: also, the Intel compiler 11.1.038 seems to need some of these to be
+// present in the library)
+
+template class negator<float>;
+template class negator<double>;
+template class negator<long double>;
+
+template class negator< complex<float> >;
+template class negator< complex<double> >;
+template class negator< complex<long double> >;
+
+template class negator< conjugate<float> >;
+template class negator< conjugate<double> >;
+template class negator< conjugate<long double> >;
+
+template class CNT< negator<float> >;
+template class CNT< negator<double> >;
+template class CNT< negator<long double> >;
+
+template class CNT< complex<float> >;
+template class CNT< complex<double> >;
+template class CNT< complex<long double> >;
+
+template class CNT< negator< complex<float> > >;
+template class CNT< negator< complex<double> > >;
+template class CNT< negator< complex<long double> > >;
+
+template class CNT< conjugate<float> >;
+template class CNT< conjugate<double> >;
+template class CNT< conjugate<long double> >;
+
+template class CNT< negator< conjugate<float> > >;
+template class CNT< negator< conjugate<double> > >;
+template class CNT< negator< conjugate<long double> > >;
+
+
+#define INSTANTIATE_ALL_LEFT(T) \
+template bool isNumericallyEqual(const T&, const complex<float>&, double tol); \
+template bool isNumericallyEqual(const T&, const complex<double>&, double tol); \
+template bool isNumericallyEqual(const T&, const complex<long double>&, double tol); \
+template bool isNumericallyEqual(const T&, const conjugate<float>&, double tol); \
+template bool isNumericallyEqual(const T&, const conjugate<double>&, double tol); \
+template bool isNumericallyEqual(const T&, const conjugate<long double>&, double tol); \
+template bool isNumericallyEqual(const T&, const float&, double tol); \
+template bool isNumericallyEqual(const T&, const double&, double tol); \
+template bool isNumericallyEqual(const T&, const long double&, double tol); \
+template bool isNumericallyEqual(const T&, int, double tol)
+
+INSTANTIATE_ALL_LEFT(complex<float>);
+INSTANTIATE_ALL_LEFT(complex<double>);
+INSTANTIATE_ALL_LEFT(complex<long double>);
+INSTANTIATE_ALL_LEFT(conjugate<float>);
+INSTANTIATE_ALL_LEFT(conjugate<double>);
+INSTANTIATE_ALL_LEFT(conjugate<long double>);
+
+// Don't duplicate anything instantiated with the previous macro.
+#define INSTANTIATE_ALL_RIGHT(T) \
+template bool isNumericallyEqual(const float&, const T&, double tol); \
+template bool isNumericallyEqual(const double&, const T&, double tol); \
+template bool isNumericallyEqual(const long double&, const T&, double tol); \
+template bool isNumericallyEqual(int, const T&, double tol)
+
+INSTANTIATE_ALL_RIGHT(complex<float>);
+INSTANTIATE_ALL_RIGHT(complex<double>);
+INSTANTIATE_ALL_RIGHT(complex<long double>);
+INSTANTIATE_ALL_RIGHT(conjugate<float>);
+INSTANTIATE_ALL_RIGHT(conjugate<double>);
+INSTANTIATE_ALL_RIGHT(conjugate<long double>);
+
+}
diff --git a/SimTKcommon/Simulation/include/SimTKcommon/internal/Event.h b/SimTKcommon/Simulation/include/SimTKcommon/internal/Event.h
new file mode 100644
index 0000000..f4b07e1
--- /dev/null
+++ b/SimTKcommon/Simulation/include/SimTKcommon/internal/Event.h
@@ -0,0 +1,397 @@
+#ifndef SimTK_SimTKCOMMON_EVENT_H_
+#define SimTK_SimTKCOMMON_EVENT_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ *
+ * This file declares the types needed for Simbody's support for Events.
+ */
+
+#include "SimTKcommon/basics.h"
+
+namespace SimTK {
+
+/** @class SimTK::EventId
+This is a class to represent unique IDs for events in a type-safe way. These
+are created and managed by the System, not the State. **/
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(EventId);
+
+/** @class SimTK::SystemEventTriggerIndex
+This unique integer type is for identifying a triggered event in the full
+System-level view of the State. More precisely, this is the index of the slot
+in the global array in the cache allocated to hold the value of that event's
+trigger function.
+ at see EventTriggerIndex **/
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemEventTriggerIndex);
+
+/** @class SimTK::SystemEventTriggerByStageIndex
+This unique integer type is for identifying a triggered event within a
+particular Stage of the full System-level view of the State. (Event triggers
+for a particular Stage are stored consecutively within the full collection of
+event triggers.) That is, the EventTriggerByStageIndex will be 0 for the first
+event trigger at that stage.
+ at see EventTriggerByStageIndex
+**/
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemEventTriggerByStageIndex);
+
+/** @class SimTK::EventTriggerByStageIndex
+Unique integer type for Subsystem-local, per-stage event indexing.
+ at see SystemEventTriggerByStageIndex **/
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(EventTriggerByStageIndex);
+
+/** An Event is "something that happens" during a Study that is advancing
+through time. Its occurence interrupts the normal flow of computation, allowing
+an event Handler to adjust the State prior to resuming the Study.
+
+Events are allocated by Subsystems, but require some System global resources.
+All Events are given a unique SystemEventId. Some Events require other State
+resources, such as slots for the values of trigger functions in the case of
+Triggered events.
+
+Events can be allocated at Topology, Model, and Instance Stages. All Event
+resources are assigned when the Instance stage is realized. However, if an
+Event requires state variables, then it must be allocated by Model stage. **/
+class Event {
+public:
+
+ /** These are all the possible causes for events.
+
+ @par Initialization
+ A Study has performed its own initialization and is about to start.
+ @par Triggered
+ An event trigger function underwent a monitored sign transition.
+ @par Scheduled
+ An integrator reached a previously-scheduled time for the Event to occur.
+ @par TimeAdvanced
+ An integrator completed an internal step, meaning that it has reached
+ a point where time has advanced irreversibly.
+ @par Signaled
+ A flag in the State has been explicitly set, meaning that a particular
+ Event has occurred. Anyone with write access to a State can set these,
+ but typically they are set in event handlers associated with one of
+ the other kinds of events.
+ @par Termination
+ The Study has finished. If a Termination event handler signals more
+ Events, those signaled events are not processed by the Study; that is,
+ the signals remain set in the final State.
+
+ In case several of these causes are detected in
+ a single step, they are sequentialized in the order shown,
+ like this:
+ 1. The occurrence of triggered events is reported and
+ the triggering state and a list of triggered events
+ are passed to the event handler for processing (meaning
+ the state, but not the time, is modified). [Note that
+ simultaneity *within* the set of triggered events may
+ also require special handling; we're not talking about
+ that here, just simultaneity of *causes*.]
+ 2. Next, using the state resulting from step 1, the time is checked
+ to see if scheduled events have occurred. If so, a list of
+ those events is passed to the event handler for processing.
+ 3. Next, if this system has requested time-advanced events,
+ the event handler is called with the state that resulted
+ from step 2 and the "time advanced" cause noted. No event
+ list is passed in that case. The state may be modified.
+ 4. Last, if the final time has been reached or if any of
+ the event handlers asked for termination, we pass the
+ state to the event handler again noting that we have
+ reached termination. The state may be modified and the
+ result will be the final state of the simulation.
+ **/
+ class Cause {
+ public:
+ enum Num {
+ Initialization = 1,
+ Triggered = 2,
+ Scheduled = 3,
+ TimeAdvanced = 4,
+ Signaled = 5,
+ Termination = 6,
+ Invalid = -1
+ };
+
+ Cause() : value(Invalid) {}
+ Cause(Num n) : value(n) {} // implicit conversion
+ operator Num() const {return value;} // implicit conversion
+ Cause& operator=(Num n) {value=n; return *this;}
+
+ bool isValid() const {return Initialization<=value && value<=Termination;}
+
+ private:
+ Num value;
+ };
+
+ /** This is useful for debugging; it translates an Event::Cause into a
+ readable string. **/
+ SimTK_SimTKCOMMON_EXPORT static const char* getCauseName(Cause);
+
+
+ /** Triggered Events respond to zero crossings of their associated trigger
+ function. This enum defines constants for use in specifying which kind
+ of zero crossing has been seen, or which kinds are considered interesting.
+ For the latter purpose, these can be or'ed together to make a mask. **/
+ enum Trigger {
+ NoEventTrigger =0x0000, // must be 0
+
+ PositiveToNegative =0x0001, // 1
+ NegativeToPositive =0x0002, // 2
+
+ Falling =(PositiveToNegative), // 1
+ Rising =(NegativeToPositive), // 2
+ AnySignChange =(PositiveToNegative|NegativeToPositive) // 3
+ };
+
+ /** This is useful for debugging; it translates an Event::Trigger or a mask
+ formed by a union of Event::Triggers, into a readable string. **/
+ SimTK_SimTKCOMMON_EXPORT static std::string eventTriggerString(Trigger);
+
+
+ /** Classify a before/after sign transition. Before and after must both
+ be -1,0, or 1 as returned by the SimTK::sign() function applied to
+ the trigger function value at the beginning and end of a step. **/
+ static Trigger classifyTransition(int before, int after) {
+ if (before==after)
+ return NoEventTrigger;
+ if (before==0)
+ return NoEventTrigger; // Do not report transitions away from zero.
+ if (before==1)
+ return PositiveToNegative;
+ // before==-1
+ return NegativeToPositive;
+ }
+
+ /** Given an observed transition, weed out ignorable ones using the supplied
+ mask. That is, the return will indicate NoEventTrigger unless the original
+ Trigger was present in the mask. **/
+ static Trigger maskTransition(Trigger transition, Trigger mask) {
+ // we're depending on NoEventTrigger==0
+ return Trigger(transition & mask);
+ }
+
+private:
+};
+
+
+/** This class is used to communicate between the System and an Integrator
+regarding the properties of a particular event trigger function. Currently
+these are:
+ - Whether to watch for rising sign transitions, falling, or both. [BOTH]
+ - Whether to watch for transitions to and from zero. [NO]
+ - The localization window in units of the System's timescale. [10%]
+ (That is then the "unit" window which is reduced by the accuracy setting.)
+The default values are shown in brackets above. **/
+class SimTK_SimTKCOMMON_EXPORT EventTriggerInfo {
+public:
+ EventTriggerInfo();
+ explicit EventTriggerInfo(EventId eventId);
+ ~EventTriggerInfo();
+ EventTriggerInfo(const EventTriggerInfo&);
+ EventTriggerInfo& operator=(const EventTriggerInfo&);
+
+ EventId getEventId() const; // returns -1 if not set
+ bool shouldTriggerOnRisingSignTransition() const; // default=true
+ bool shouldTriggerOnFallingSignTransition() const; // default=true
+ Real getRequiredLocalizationTimeWindow() const; // default=0.1
+
+ // These return the modified 'this', like assignment operators.
+ EventTriggerInfo& setEventId(EventId);
+ EventTriggerInfo& setTriggerOnRisingSignTransition(bool);
+ EventTriggerInfo& setTriggerOnFallingSignTransition(bool);
+ EventTriggerInfo& setRequiredLocalizationTimeWindow(Real);
+
+ Event::Trigger calcTransitionMask() const {
+ unsigned mask = 0;
+ if (shouldTriggerOnRisingSignTransition()) {
+ mask |= Event::NegativeToPositive;
+ }
+ if (shouldTriggerOnFallingSignTransition()) {
+ mask |= Event::PositiveToNegative;
+ }
+ return Event::Trigger(mask);
+ }
+
+ Event::Trigger calcTransitionToReport
+ (Event::Trigger transitionSeen) const
+ {
+ // report -1 to 1 or 1 to -1 as appropriate
+ if (transitionSeen & Event::Rising)
+ return Event::NegativeToPositive;
+ if (transitionSeen & Event::Falling)
+ return Event::PositiveToNegative;
+ assert(!"impossible event transition situation");
+ return Event::NoEventTrigger;
+ }
+
+private:
+ class EventTriggerInfoRep;
+
+ // opaque implementation for binary compatibility
+ EventTriggerInfoRep* rep;
+
+ const EventTriggerInfoRep& getRep() const {assert(rep); return *rep;}
+ EventTriggerInfoRep& updRep() {assert(rep); return *rep;}
+};
+
+
+
+
+//==============================================================================
+// HANDLE EVENTS OPTIONS and HANDLE EVENTS RESULTS
+//==============================================================================
+/** Options for the handleEvent() method. Accuracy should be be set by the
+caller, but if not the default is 1e-4. **/
+class HandleEventsOptions {
+public:
+ enum Option {
+ /** Take all defaults. **/
+ None = 0x0000,
+ /** Normally failure to meet the accuracy requirements throws an
+ exception. This will force the handleEvent() method to quietly return bad
+ status instead. **/
+ DontThrow = 0x0001,
+ /** Use the stricter infinity (max absolute value) norm rather than
+ the default RMS norm to determine when accuracy has been achieved. **/
+ UseInfinityNorm = 0x0002
+ };
+
+
+ HandleEventsOptions() {clear();}
+ explicit HandleEventsOptions(Real accuracy)
+ { clear(); setAccuracy(accuracy); }
+ explicit HandleEventsOptions(Option opt)
+ { clear(); setOption(opt); }
+
+ /** Restore this object to its default-constructed state (no options
+ selected, default accuracy). A reference to the
+ newly-cleared object is returned. **/
+ HandleEventsOptions& clear()
+ { optionSet=0; setAccuracyDefaults(); return *this; }
+
+ /** The norm of the constraint errors must be driven to below this value
+ for a project() to be considered successful. Normally an RMS norm is used
+ but you can override that to use an infinity norm instead. **/
+ HandleEventsOptions& setAccuracy(Real accuracy) {
+ assert(accuracy > 0);
+ requiredAccuracy = accuracy;
+ return *this;
+ }
+
+ /** Remove a given option from the set. Nothing happens if the option wasn't
+ already set. **/
+ HandleEventsOptions& clearOption(Option opt)
+ { optionSet &= ~(unsigned)opt; return *this; }
+ /** Select a given option from the set. Nothing happens if the option wasn't
+ already set. **/
+ HandleEventsOptions& setOption (Option opt)
+ { optionSet |= (unsigned)opt; return *this; }
+
+ /** Return the current value for the accuracy option. **/
+ Real getAccuracy() const {return requiredAccuracy;}
+
+ bool isOptionSet(Option opt) const {return (optionSet&(unsigned)opt) != 0;}
+
+ static Real getDefaultAccuracy() {return Real(1e-4);}
+
+ // Set operators: not, or, and, set difference
+ HandleEventsOptions& operator|=(const HandleEventsOptions& opts)
+ { optionSet |= opts.optionSet; return *this; }
+ HandleEventsOptions& operator&=(const HandleEventsOptions& opts)
+ { optionSet &= opts.optionSet; return *this; }
+ HandleEventsOptions& operator-=(const HandleEventsOptions& opts)
+ { optionSet &= ~opts.optionSet; return *this; }
+
+ HandleEventsOptions& operator|=(Option opt) {setOption(opt); return *this;}
+ HandleEventsOptions& operator-=(Option opt) {clearOption(opt); return *this;}
+
+private:
+ Real requiredAccuracy;
+ unsigned optionSet;
+
+ void setAccuracyDefaults() {
+ requiredAccuracy = getDefaultAccuracy();
+ }
+};
+
+/** Results returned by the handleEvent() method. In addition to return
+status, this records the lowest stage in the state that was modified by
+the handler. The caller can use this to determine how much reinitialization
+is required before time stepping can proceed. **/
+class HandleEventsResults {
+public:
+ HandleEventsResults() : m_lowestModifiedStage(Stage::Infinity) {clear();}
+
+ enum Status {
+ /** This object has not been filled in yet and holds no results. **/
+ Invalid = -1,
+ /** The handleEvent() operation was successful and time stepping
+ may continue. **/
+ Succeeded = 0,
+ /** The handleEvent() call was successful but the event requires
+ time stepping to terminate. An explanation may have been placed in
+ the message argument. **/
+ ShouldTerminate = 1,
+ /** The handleEvent() call was unable to successfully handle the
+ event. This is likely to be a fatal error. A human-readable
+ explanation is in the message argument. **/
+ Failed = 2
+ };
+
+ /** Restore this object to its default-constructed state, with the return
+ status set to Invalid. **/
+ HandleEventsResults& clear() {
+ m_exitStatus = Invalid;
+ m_anyChangeMade = false;
+ m_lowestModifiedStage = Stage::Infinity; // i.e., nothing modified
+ m_message.clear();
+ return *this;
+ }
+ bool isValid() const {return m_exitStatus != Invalid;}
+ Status getExitStatus() const {return m_exitStatus;}
+
+ bool getAnyChangeMade() const
+ { assert(isValid()); return m_anyChangeMade; }
+ Stage getLowestModifiedStage() const
+ { assert(isValid()); return m_lowestModifiedStage; }
+ const String& getMessage() const
+ { assert(isValid()); return m_message; }
+
+ HandleEventsResults& setExitStatus(Status status)
+ { m_exitStatus=status; return *this; }
+ HandleEventsResults& setAnyChangeMade(bool changeMade)
+ { m_anyChangeMade=changeMade; return *this; }
+ HandleEventsResults& setLowestModifiedStage(Stage stage)
+ { m_lowestModifiedStage=stage; return *this; }
+ HandleEventsResults& setMessage(const String& message)
+ { m_message=message; return *this; }
+private:
+ Status m_exitStatus;
+ bool m_anyChangeMade;
+ Stage m_lowestModifiedStage;
+ String m_message;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_EVENT_H_
diff --git a/SimTKcommon/Simulation/include/SimTKcommon/internal/EventHandler.h b/SimTKcommon/Simulation/include/SimTKcommon/internal/EventHandler.h
new file mode 100644
index 0000000..a5a580f
--- /dev/null
+++ b/SimTKcommon/Simulation/include/SimTKcommon/internal/EventHandler.h
@@ -0,0 +1,176 @@
+#ifndef SimTK_SimTKCOMMON_EVENT_HANDLER_H_
+#define SimTK_SimTKCOMMON_EVENT_HANDLER_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/State.h"
+#include "SimTKcommon/internal/System.h"
+
+namespace SimTK {
+
+/** An EventHandler is an object that defines an event that can occur within a
+system. It is an abstract class. Subclasses define how to determine when the
+event occurs, and what happens when it does. You will not generally subclass
+EventHandler directly. Instead, subclass ScheduledEventHandler (for events
+that occur at a particular time that is know in advance) or
+TriggeredEventHandler (for events that occur when some condition is satisfied
+within the system). ScheduledEventHandler also has another subclass,
+PeriodicEventHandler, for the common situation of events that occur at regular
+intervals.
+
+An EventHandler should be thought of as an integral part of the system it
+belongs to, and may alter the physical properties or behavior of the system.
+If you merely want to observe the system but not to alter it, you should
+generally use a EventReporter instead.
+
+Once you have created an EventHandler, you can add it to a System by calling
+addEventHandler() on the System. **/
+class SimTK_SimTKCOMMON_EXPORT EventHandler {
+public:
+ virtual ~EventHandler();
+
+ /** This method is invoked to handle the event. It is given a State which
+ describes the system at the time when the event occurs, and it is permitted
+ to modify any aspect of the state except the time. In doing so, it should
+ respect the specified accuracy requirements for the continuous variables
+ and constraints.
+
+ @param state
+ The state of the system when the event occurred. This method should
+ modify \a state to reflect the changes caused by the event.
+ @param accuracy
+ The accuracy to which this simulation is being computed. If your
+ handler performs any approximate operation it should do so consistent
+ with the simulation accuracy.
+ @param shouldTerminate
+ If the event handler sets this to true, it will cause the simulation to
+ terminate immediately. This does not necessarily indicate an error
+ condition.
+ **/
+ virtual void handleEvent(State& state, Real accuracy,
+ bool& shouldTerminate) const = 0;
+};
+
+/**
+ * ScheduledEventHandler is a subclass of EventHandler for events that occur at a particular
+ * time that is known in advance. This includes events that occur multiple times. The only
+ * requirement is that, at any time, it must be able to report the next time at which an
+ * event will occur.
+ */
+
+class SimTK_SimTKCOMMON_EXPORT ScheduledEventHandler : public EventHandler {
+public:
+ virtual ~ScheduledEventHandler();
+
+ /**
+ * Get the next time at which an event will occur.
+ *
+ * @param state the current state of the system
+ * @param includeCurrentTime if true, return the next event whose time is >= the current time.
+ * If false, only return events after (not at) the current time.
+ */
+
+ virtual Real getNextEventTime(const State& state,
+ bool includeCurrentTime) const = 0;
+};
+
+/**
+ * TriggeredEventHandler is a subclass of EventHandler for events that occur when some condition
+ * is satisfied within the system. This is implemented by means of an "event trigger function".
+ * For any State, the handler must be able to calculate the value of the function. When it
+ * passes through 0, that indicates the event has occurred. You can also customize when the
+ * event is triggered, for example to specify that it is only triggered when the function
+ * goes from negative to positive, not when it goes from positive to negative.
+ */
+
+class SimTK_SimTKCOMMON_EXPORT TriggeredEventHandler : public EventHandler {
+public:
+ class TriggeredEventHandlerImpl;
+ TriggeredEventHandler(const TriggeredEventHandler& clone);
+ TriggeredEventHandler& operator=(const TriggeredEventHandler& clone);
+ virtual ~TriggeredEventHandler();
+
+ /**
+ * Construct a new TriggeredEventHandler.
+ *
+ * @param requiredStage
+ * The stage at which the trigger function will be evaluated.
+ */
+ TriggeredEventHandler(Stage requiredStage);
+
+ /**
+ * Get the value of the event trigger function for a State.
+ */
+ virtual Real getValue(const State&) const = 0;
+
+ /**
+ * Get an EventTriggerInfo object which can be used to customize when the
+ * event occurs.
+ */
+ EventTriggerInfo& getTriggerInfo();
+
+ /**
+ * Get the stage at which the trigger function will be evaluated.
+ */
+ Stage getRequiredStage() const;
+private:
+ TriggeredEventHandlerImpl* impl;
+};
+
+/** PeriodicEventHandler is a subclass of ScheduledEventHandler which generates
+a series of uniformly spaced events at regular intervals. This allows you to
+very easily create event handlers with this behavior. **/
+class SimTK_SimTKCOMMON_EXPORT PeriodicEventHandler
+: public ScheduledEventHandler {
+public:
+ class PeriodicEventHandlerImpl;
+ ~PeriodicEventHandler();
+ Real getNextEventTime(const State& state, bool includeCurrentTime) const;
+
+ /**
+ * Create a PeriodicEventHandler.
+ *
+ * @param eventInterval
+ * The time interval at which events should occur.
+ */
+ PeriodicEventHandler(Real eventInterval);
+
+ /**
+ * Get the time interval at which events occur.
+ */
+ Real getEventInterval() const;
+
+ /**
+ * Set the time interval at which events occur.
+ */
+ void setEventInterval(Real eventInterval);
+private:
+ PeriodicEventHandlerImpl* impl;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_EVENT_HANDLER_H_
diff --git a/SimTKcommon/Simulation/include/SimTKcommon/internal/EventReporter.h b/SimTKcommon/Simulation/include/SimTKcommon/internal/EventReporter.h
new file mode 100644
index 0000000..981edb1
--- /dev/null
+++ b/SimTKcommon/Simulation/include/SimTKcommon/internal/EventReporter.h
@@ -0,0 +1,169 @@
+#ifndef SimTK_SimTKCOMMON_EVENT_REPORTER_H_
+#define SimTK_SimTKCOMMON_EVENT_REPORTER_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/State.h"
+#include "SimTKcommon/internal/System.h"
+
+namespace SimTK {
+
+/**
+ * An EventReporter is an object that defines an event that can occur within a system.
+ * It is an abstract class. Subclasses define how to determine when the event occurs,
+ * and what happens when it does. You will not generally subclass EventReporter
+ * directly. Instead, subclass ScheduledEventReporter (for events that occur at a
+ * particular time that is know in advance) or TriggeredEventReporter (for events that
+ * occur when some condition is satisfied within the system). ScheduledEventReporter
+ * also has another subclass, PeriodicEventReporter, for the common situation of
+ * events that occur at regular intervals.
+ *
+ * EventReporter is very similar to EventHandler, but differs in that it is not permitted
+ * to modify the state of the system. It can observe the system's behavior, but not
+ * alter it. This means that adding an EventReporter to a System is not considered a
+ * change to the physical system it represents.
+ *
+ * Once you have created an EventReporter, you can add it to a System by calling
+ * addEventReporter() on the System.
+ */
+
+class SimTK_SimTKCOMMON_EXPORT EventReporter {
+public:
+ virtual ~EventReporter();
+
+ /**
+ * This method is invoked to handle the event. It is given a State which describes the
+ * system at the time when the event occurs.
+ */
+
+ virtual void handleEvent(const State& state) const = 0;
+};
+
+/**
+ * ScheduledEventReporter is a subclass of EventReporter for events that occur at a particular
+ * time that is known in advance. This includes events that occur multiple times. The only
+ * requirement is that, at any time, it must be able to report the next time at which an
+ * event will occur.
+ */
+
+class SimTK_SimTKCOMMON_EXPORT ScheduledEventReporter : public EventReporter {
+public:
+ virtual ~ScheduledEventReporter();
+
+ /**
+ * Get the next time at which an event will occur.
+ *
+ * @param state the current state of the system
+ * @param includeCurrentTime if true, return the next event whose time is >= the current time.
+ * If false, only return events after (not at) the current time.
+ */
+
+ virtual Real getNextEventTime(const State& state, bool includeCurrentTime) const = 0;
+};
+
+/**
+ * TriggeredEventReporter is a subclass of EventReporter for events that occur when some condition
+ * is satisfied within the system. This is implemented by means of an "event trigger function".
+ * For any State, the handler must be able to calculate the value of the function. When it
+ * passes through 0, that indicates the event has occurred. You can also customize when the
+ * event is triggered, for example to specify that it is only triggered when the function
+ * goes from negative to positive, not when it goes from positive to negative.
+ */
+
+class SimTK_SimTKCOMMON_EXPORT TriggeredEventReporter : public EventReporter {
+public:
+ class TriggeredEventReporterImpl;
+ TriggeredEventReporter(const TriggeredEventReporter& clone);
+ TriggeredEventReporter& operator=(const TriggeredEventReporter& clone);
+ virtual ~TriggeredEventReporter();
+
+ /**
+ * Construct a new TriggeredEventReporter.
+ *
+ * @param requiredStage the stage at which the trigger function will be evaluated
+ */
+
+ TriggeredEventReporter(Stage requiredStage);
+
+ /**
+ * Get the value of the event trigger function for a State.
+ */
+
+ virtual Real getValue(const State&) const = 0;
+
+ /**
+ * Get an EventTriggerInfo object which can be used to customize when the event occurs.
+ */
+
+ EventTriggerInfo& getTriggerInfo();
+
+ /**
+ * Get the stage at which the trigger function will be evaluated.
+ */
+
+ Stage getRequiredStage() const;
+private:
+ TriggeredEventReporterImpl* impl;
+};
+
+/**
+ * PeriodicEventReporter is a subclass of ScheduledEventReporter which generates a series
+ * of uniformly spaced events at regular intervals. This allows you to very easily create
+ * event reporters with this behavior.
+ */
+
+class SimTK_SimTKCOMMON_EXPORT PeriodicEventReporter : public ScheduledEventReporter {
+public:
+ class PeriodicEventReporterImpl;
+ ~PeriodicEventReporter();
+ Real getNextEventTime(const State& state, bool includeCurrentTime) const;
+
+ /**
+ * Create a PeriodicEventReporter.
+ *
+ * @param eventInterval the time interval at which events should occur.
+ */
+
+ PeriodicEventReporter(Real eventInterval);
+
+ /**
+ * Get the time interval at which events occur.
+ */
+
+ Real getEventInterval() const;
+
+ /**
+ * Set the time interval at which events occur.
+ */
+
+ void setEventInterval(Real eventInterval);
+private:
+ PeriodicEventReporterImpl* impl;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_EVENT_REPORTER_H_
diff --git a/SimTKcommon/Simulation/include/SimTKcommon/internal/Measure.h b/SimTKcommon/Simulation/include/SimTKcommon/internal/Measure.h
new file mode 100644
index 0000000..bc90eaa
--- /dev/null
+++ b/SimTKcommon/Simulation/include/SimTKcommon/internal/Measure.h
@@ -0,0 +1,1079 @@
+#ifndef SimTK_SimTKCOMMON_MEASURE_H_
+#define SimTK_SimTKCOMMON_MEASURE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ *
+ * This file declares the base class AbstractMeasure for all derived Measure
+ * handle classes, and the handle classes for built-in Measures. Measure
+ * handles provide the end user API, while the implementations of Measures
+ * derive from the abstract Measure::Implementation class defined in
+ * MeasureImplementation.h. Measure Implementation classes provide the Measure
+ * developer's API.
+ */
+
+#include "SimTKcommon/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+
+#include <cassert>
+
+/**
+ * Every concrete measure handle class "MH" derived from a parent handle "PH"
+ * that derives directly or indirectly from the AbstractMeasure handle class
+ * should include the macro SimTK_MEASURE_HANDLE_PREAMBLE(MH,PH) at the
+ * beginning of the public section of its declaration. It performs the following
+ * declarations:
+ * <pre>
+ * MH::Implementation the handle's local implementation class
+ * MH::MH() default constructor creates an empty handle
+ * MH::MH(Implementation*) create a handle referencing an existing object
+ * MH::MH(Subsystem&, Implementation*)
+ * create a handle referencing an existing but
+ * unowned object, then installs that in the
+ * given Subsystem which becomes the owner
+ * </pre>
+ *
+ * MH::Implementation must be defined elsewhere as a class derived
+ * from Measure::Implementation.
+ */
+
+// Helper macro shared by SimTK_MEASURE_HANDLE_PREAMBLE and
+// SimTK_MEASURE_HANDLE_PREAMBLE_ABSTRACT.
+#define SimTK_MEASURE_HANDLE_PREAMBLE_BASE(MH,PH) \
+ class Implementation; \
+ explicit MH(Implementation* imp) : PH(imp) {} \
+ MH(SimTK::Subsystem& sub, Implementation* imp, \
+ const SimTK::AbstractMeasure::SetHandle& sh) \
+ : PH(sub,imp,sh) {} \
+ MH& operator=(const MH& src) {PH::operator=(src); return *this;}\
+ MH& shallowAssign(const MH& src) {PH::shallowAssign(src); return *this;}\
+ MH& deepAssign(const MH& src) {PH::deepAssign(src); return *this;}
+
+
+// The default constructor for concrete classes should instantiate
+// a default-constructed Implementation object if no Implementation object
+// is provided.
+#define SimTK_MEASURE_HANDLE_PREAMBLE(MH,PH) \
+ SimTK_MEASURE_HANDLE_PREAMBLE_BASE(MH,PH) \
+ MH() : PH(new Implementation()) {} \
+ explicit MH(SimTK::Subsystem& sub) \
+ : PH(sub,new Implementation(), typename PH::SetHandle()) {}
+
+
+
+// The default constructor for a still-abstract derived class can't
+// instantiate an Implementation.
+#define SimTK_MEASURE_HANDLE_PREAMBLE_ABSTRACT(MH,PH) \
+ SimTK_MEASURE_HANDLE_PREAMBLE_BASE(MH,PH) \
+ MH() : PH() {}
+
+/**
+ * Every measure handle class "MH" derived directly or indirectly from the
+ * abstract measure handle class "Measure" should include this macro at
+ * the end of the "public" section of its declaration. The macro expects
+ * there to be a local class, MH::Implementation,
+ * already declared. (MH::Implementation is the type of MH's
+ * implementation object to be derived from Measure::Implementation
+ * and defined elsewhere.) Then the following
+ * type-safe downcast methods will be added to MH's definition:
+ * <pre>
+ * MH::getAs(const AbstractMeasure&) generic handle to const MH (static)
+ * MH::updAs(AbstractMeasure&) generic handle to writable MH (static)
+ * MH::isA(AbstractMeasure&) test if generic handle is of type MH
+ * getImpl() generic implementation to const MH::Implementation
+ * updImpl() generic implementation to writable MH::Implementation
+ * </pre>
+ * Type checking for the handle class conversions is done only in Debug
+ * builds.
+ */
+#define SimTK_MEASURE_HANDLE_POSTSCRIPT(MH,PH) \
+ static bool isA(const SimTK::AbstractMeasure& m) \
+ { return dynamic_cast<const Implementation*>(&m.getImpl()) != 0; } \
+ static const MH& getAs(const SimTK::AbstractMeasure& m) \
+ { assert(isA(m)); return static_cast<const MH&>(m); } \
+ static MH& updAs(SimTK::AbstractMeasure& m) \
+ { assert(isA(m)); return static_cast<MH&>(m); } \
+ const Implementation& getImpl() const \
+ { return SimTK_DYNAMIC_CAST_DEBUG<const Implementation&> \
+ (SimTK::AbstractMeasure::getImpl());} \
+ Implementation& updImpl() \
+ { return SimTK_DYNAMIC_CAST_DEBUG<Implementation&> \
+ (SimTK::AbstractMeasure::updImpl());}
+
+namespace SimTK {
+
+class State;
+class Subsystem;
+class System;
+class EventId;
+
+/// Define a unique integral type for safe indexing of Measures.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(MeasureIndex);
+
+//==============================================================================
+// ABSTRACT MEASURE
+//==============================================================================
+/** This is the base class for all Measure handle classes. This class is not
+templatized, and represents a Measure generically without knowledge of its
+value type. This is useful for most of the basic operations that are
+performed on measures, such as realization, adoption by a Subsystem, and
+other bookkeeping tasks. For most user purposes, the still-generic derived
+class Measure_<T> is a more useful handle since its value, of known type T,
+can be obtained. All the built-in concrete Measure types derive from
+Measure_<T> rather than AbstractMeasure. The various concrete Measures,
+built in or otherwise, may set restrictions on the kinds of types that
+are allowable as the template argument.
+
+Note that handles always consist of exactly one pointer, and handle classes
+are always concrete (meaning they have no virtual methods). **/
+class SimTK_SimTKCOMMON_EXPORT AbstractMeasure {
+protected:
+ /// An object of this type is used as a dummy argument to make sure the
+ /// automatically-generated handle constructor's signature doesn't conflict
+ /// with an explicitly-defined one.
+ class SetHandle {};
+
+public:
+ class Implementation; // local; name is AbstractMeasure::Implementation
+
+ /// Provide an Implementation for this AbstractMeasure and bump its
+ /// reference count. This is also the default constructor for the
+ /// base class producing an empty handle.
+ explicit AbstractMeasure(Implementation* g=0);
+
+ /// Construct this handle with a given Implementation object (whose
+ /// reference count will be bumped) and then let the given Subsystem
+ /// adopt this Measure (which will again bump the Implementation's
+ /// reference count, leaving us with two new handles).
+ AbstractMeasure(Subsystem&, Implementation* g, const SetHandle&);
+
+ /// Shallow copy constructor copies the pointer from the source
+ /// Implementation object and bumps its reference count.
+ AbstractMeasure(const AbstractMeasure&);
+
+ /// Shallow assignment operator results in this handle referencing
+ /// the same Implementation object as does the source.
+ /// @see shallowAssign for more details
+ AbstractMeasure& operator=(const AbstractMeasure& source)
+ { return shallowAssign(source); }
+
+ /// Destructor decrements the Implementation's reference count and
+ /// deletes the object if the count goes to zero.
+ ~AbstractMeasure();
+
+ /// Shallow assignment operator destructs the current Implementation
+ /// object (meaning its reference count is decremented and the object
+ /// actually deleted only if the count goes to zero), then copies the
+ /// Implementation pointer from the source and bumps its reference count.
+ /// This is what the copy assignment operator= does for this class.
+ AbstractMeasure& shallowAssign(const AbstractMeasure&);
+
+ /// Deep assignment clones the Implementation object pointed to by
+ /// the source handle, so that this handle ends up pointing to a
+ /// new Measure object similar to the original but not yet contained
+ /// in any Subsystem.
+ AbstractMeasure& deepAssign(const AbstractMeasure& source);
+
+ /// Every Measure can produce a value, and some can provide one or
+ /// more total derivatives with respect to time of that value. This
+ /// method reports how many are available: 1 -> first derivative
+ /// d/dt is available, 2 -> first and second derivative d^2/dt^2
+ /// are available, etc. We interpret the "0th derivative" to be the
+ /// Measure's value.
+ /// @return The maximum available derivative order.
+ int getNumTimeDerivatives() const;
+
+ /// At what Stage can we expect the value of this AbstractMeasure or
+ /// one of its time derivatives to be available? Users of Measures will
+ /// typically impose restrictions on the levels they will accept.
+ /// @param[in] derivOrder
+ /// Which derivative level is to be checked: 0 -> the value,
+ /// 1 -> the 1st time derivative, etc. Must not be higher than the
+ /// value returned by getNumTimeDerivatives().
+ /// @return The Stage after which this value is available.
+ Stage getDependsOnStage(int derivOrder=0) const;
+
+
+ /// There can be multiple handles on the same Measure.
+ bool isSameMeasure(const AbstractMeasure& other) const
+ { return impl && impl==other.impl;}
+
+ bool isEmptyHandle() const {return !hasImpl();}
+
+ /// Test whether this Measure object has been adopted by a Subsystem.
+ bool isInSubsystem() const;
+ /// Return a reference to the Subsystem that owns this Measure. Will
+ /// throw an exception if the Measure is not currently owned by any
+ /// Subsystem.
+ const Subsystem& getSubsystem() 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.
+ MeasureIndex getSubsystemMeasureIndex() const;
+
+ // Internal use only
+
+ // dynamic_cast the returned reference to a reference to your concrete
+ // Implementation class.
+ const Implementation& getImpl() const {assert(impl); return *impl;}
+ Implementation& updImpl() {assert(impl); return *impl;}
+ bool hasImpl() const {return impl!=0;}
+
+ int getRefCount() const;
+private:
+ // This is the only data member in this class. Also, any class derived
+ // from AbstractMeasure must have *NO* data members at all (data goes
+ // in the Implementation class).
+ Implementation* impl;
+
+friend class Implementation;
+};
+
+
+//==============================================================================
+// MEASURE <T>
+//==============================================================================
+/** This is the base handle class for all Measures whose value type is known,
+including all the Simbody built-in %Measure types. **/
+template <class T>
+class Measure_ : public AbstractMeasure {
+public:
+ /** This class is still abstract so we don't want it to allocate an
+ Implementation object in its default constructor. **/
+ SimTK_MEASURE_HANDLE_PREAMBLE_ABSTRACT(Measure_, AbstractMeasure);
+
+ /** Retrieve the Value of this Measure or one of its time derivatives,
+ assuming the supplied State has been realized to at least the required
+ stage for the selected value or derivative, as reported by
+ getDependsOnStage(). If the stage is sufficient but the corresponding
+ value has not yet been computed, it will be computed first with its
+ value going into this State's cache so that subsequent calls do not
+ require further computation. **/
+ const T& getValue(const State& s, int derivOrder=0) const
+ { return getImpl().getValue(s,derivOrder); }
+
+ /** Change the default value associated with this %Measure. How this is
+ used varies with the %Measure type but generally it is the value that
+ the %Measure will have before any calculations are performed. Note
+ that this does not require a State since it is a Topology-stage
+ change; you have to call realizeTopology() again if you call this
+ method. **/
+ Measure_& setDefaultValue(const T& defaultValue)
+ { updImpl().setDefaultValue(defaultValue); return *this; }
+
+ /** Obtain a reference to the default value associated with
+ this %Measure. **/
+ const T& getDefaultValue() const
+ { return getImpl().getDefaultValue(); }
+
+ // These are built-in Measures with local class names.
+
+ // Templatized measures may have restrictions on the allowable template
+ // type and may be specialized for particular types.
+ class Zero; // T is any numerical type
+ class One; // T is any numerical type
+ class Constant; // T is any assignable type
+ class Time; // T is any type for which T(t) makes sense.
+ class Variable; // T is any assignable type (state)
+ class Result; // T is any assignable type (cache)
+ class SampleAndHold;// T is any assignable type
+ class Delay; // T is any assignable type
+
+ // This requires any numerical type.
+ class Plus;
+ class Minus;
+ class Scale;
+ class Differentiate;
+
+ // These find extreme values *in time*, not among inputs at the same
+ // time. They perform elementwise on aggregate types.
+ class Extreme; // base class for min/max/minabs/maxabs
+ class Minimum; // most positive value
+ class Maximum; // most negative value
+ class MinAbs; // the signed quantity whose absolute value was min
+ class MaxAbs; // the signed quantity whose absolute value was max
+
+ // These accept floating point numerical template arguments only.
+ class Integrate;
+ class Sinusoid;
+
+ SimTK_MEASURE_HANDLE_POSTSCRIPT(Measure_, AbstractMeasure);
+};
+
+/** This typedef is a convenient abbreviation for the most common kind of
+%Measure -- one that returns a single Real result; the underlying class is
+Measure_; look there for documentation. **/
+typedef Measure_<Real> Measure;
+
+
+//==============================================================================
+// CONSTANT
+//==============================================================================
+/** This creates a Measure whose value is a Topology-stage constant of any
+type T. This does not have to be part of a Subsystem, but if it is then changing
+the constant's value invalidates the containing Subsystem's Topology.
+ at tparam T This can be any type that supports copy construction. **/
+template <class T>
+class Measure_<T>::Constant : public Measure_<T> {
+public:
+ SimTK_MEASURE_HANDLE_PREAMBLE(Constant, Measure_<T>);
+
+ /** Create a constant measure that is not part of any Subsystem, and
+ provide the constant value. **/
+ explicit Constant(const T& value)
+ : Measure_<T>(new Implementation(value)) {}
+
+ /** Create a constant measure with the given \a value and install it into
+ the given Subsystem. **/
+ Constant(Subsystem& sub, const T& value)
+ : Measure_<T>(sub, new Implementation(value),
+ AbstractMeasure::SetHandle()) {}
+
+ /** Change the value returned by this %Measure. Note that this does not
+ require a State since it is a Topology-stage change. **/
+ Constant& setValue(const T& value)
+ { updImpl().setValue(value); return *this; }
+
+ SimTK_MEASURE_HANDLE_POSTSCRIPT(Constant, Measure_<T>);
+};
+
+//==============================================================================
+// ZERO
+//==============================================================================
+/** This creates a Measure::Constant whose value is always T(0) and can't be
+changed. This class is specialized for Vector so that you must provide a
+size at construction. **/
+template <class T>
+class Measure_<T>::Zero : public Measure_<T>::Constant {
+public:
+ Zero();
+ explicit Zero(Subsystem& sub);
+};
+
+template <>
+class Measure_< Vector >::Zero : public Measure_< Vector >::Constant {
+public:
+ explicit Zero(int size);
+ Zero(Subsystem& sub, int size);
+};
+
+//==============================================================================
+// ONE
+//==============================================================================
+/** This creates a Measure::Constant whose value is always T(1) and can't be
+changed. This class is specialized for Vector so that you must provide a
+size at construction. **/
+template <class T>
+class Measure_<T>::One : public Measure_<T>::Constant {
+public:
+ One();
+ explicit One(Subsystem& sub);
+};
+
+template <>
+class Measure_< Vector >::One : public Measure_< Vector >::Constant {
+public:
+ explicit One(int size);
+ One(Subsystem& sub, int size);
+};
+
+//==============================================================================
+// TIME
+//==============================================================================
+/** This creates a Measure::Time whose value is always T(time). **/
+template <class T>
+class Measure_<T>::Time : public Measure_<T> {
+public:
+ SimTK_MEASURE_HANDLE_PREAMBLE(Time, Measure_<T>);
+
+ SimTK_MEASURE_HANDLE_POSTSCRIPT(Time, Measure_<T>);
+};
+
+//==============================================================================
+// VARIABLE
+//==============================================================================
+/** This creates a Measure whose value is a discrete State variable of any
+type T. **/
+template <class T>
+class Measure_<T>::Variable : public Measure_<T> {
+public:
+ SimTK_MEASURE_HANDLE_PREAMBLE(Variable, Measure_<T>);
+
+ // TODO: should not require invalidated Stage here. Instead,
+ // should have a unique "generation" counter for this variable
+ // and allow subsequent users to check it.
+ Variable(Subsystem& sub, Stage invalidates, const T& defaultValue)
+ : Measure_<T>(sub, new Implementation(invalidates, defaultValue),
+ AbstractMeasure::SetHandle()) {}
+
+
+ void setValue(State& state, const T& value) const
+ { getImpl().setValue(state, value); }
+
+ SimTK_MEASURE_HANDLE_POSTSCRIPT(Variable, Measure_<T>);
+};
+
+//==============================================================================
+// RESULT
+//==============================================================================
+/** This Measure holds the result of some externally-determined computation,
+and helps to coordinate the validity of that computation with respect
+to the state variables. The value must be set manually and explicitly
+marked valid when it is complete. The value will be automatically
+invalidated after a state change at or below a specified Stage, and changing
+the value here will automatically invalidate a specified Stage and above.
+
+In constrast to Measure::Variable, Measure::Result is not a state variable;
+it is a cache variable meaning that it works with a const State. It is
+expected that the result can be recalculated from the state variables when
+needed, or contains ephemeral information that can be discarded.
+
+No provision for derivatives is made; there is only the one result. **/
+template <class T>
+class Measure_<T>::Result : public Measure_<T> {
+public:
+ SimTK_MEASURE_HANDLE_PREAMBLE(Result, Measure_<T>);
+
+ // TODO: should not require invalidated Stage here. Instead,
+ // should have a unique "generation" counter for this cache entry
+ // and allow subsequent users of the value to check it.
+
+ /// Create a new Result measure and add it to the indicated
+ /// subsystem. The Result measure's value depends on earlier
+ /// values calculated at \a dependsOn stage, so whenever that
+ /// stage or earlier is invalidated, so is the value here.
+ /// In addition, any change made to the value here will invalidate
+ /// stage \a invalidated and all subsequent stages. The
+ /// \a invalidated stage must be later than the \a dependsOn stage,
+ /// and you cannot update the value here in a state that hasn't
+ /// already been realized to the dependsOn stage.
+ /// Set \a dependsOn = Stage::Empty if your value computation doesn't
+ /// depend on anything else (that will be interpreted as Stage::Topology,
+ /// however, since space for the value gets allocated then).
+ /// Set \a invalidated = Stage::Infinity if you don't want anything
+ /// invalidated when this value is changed.
+ Result(Subsystem& sub, Stage dependsOn, Stage invalidated)
+ : Measure_<T>(sub, new Implementation(dependsOn, invalidated),
+ AbstractMeasure::SetHandle()) {}
+
+ /// Get the \a dependsOn stage for this measure's value.
+ Stage getDependsOnStage() const {return getImpl().getDependsOnStage();}
+ /// Get the \a invalidated stage for this measure's value.
+ Stage getInvalidatedStage() const {return getImpl().getInvalidatedStage();}
+ /// Change the \a dependsOn stage for this measure's value, which must
+ /// be strictly less than the current setting for the \a invalidated
+ /// stage. If you set the dependsOn stage to Stage::Empty it will be
+ /// interpreted as Stage::Topology since the value must always depend
+ /// on at least topology. Setting the dependsOn stage is itself a
+ /// topological change requiring reallocation of the value if it has
+ /// already been allocated; you must call realizeTopology() again.
+ Result& setDependsOnStage(Stage dependsOn)
+ { updImpl().setDependsOnStage(dependsOn); return *this; }
+ /// Change the \a invalidated stage for this measure's value, which must
+ /// be strictly greater than the current setting for the \a dependsOn
+ /// stage. This is a topological change requiring reallocation of the
+ /// value if it has already been allocated; you must call
+ /// realizeTopology() again.
+ Result& setInvalidatedStage(Stage invalidated)
+ { updImpl().setInvalidatedStage(invalidated); return *this; }
+
+ /// Normally a Result measure's value is not considered valid unless
+ /// we are notified explicitly that it is, via a call to markAsValid()
+ /// or setValue(); this method allows the value to be assumed valid
+ /// after the \a dependsOn stage has been realized. The reason this
+ /// exists is that in some cases it is difficult to find a place from
+ /// which to call markAsValid(). That means you must set the value during
+ /// realization of the dependsOn stage, but there is no way for this to
+ /// be checked automatically. Thus use of this feature can lead
+ /// to very difficult-to-find bugs; you should try hard to find a place
+ /// to call markAsValid() before resorting to this method. This is a
+ /// topological change requiring reallocation of the value if it has
+ /// already been allocated; you must call realizeTopology() again.
+ Result& setIsPresumedValidAtDependsOnStage(bool presume)
+ { updImpl().setIsPresumedValidAtDependsOnStage(presume); return *this; }
+
+ /// Return the value of the "presumed valid at dependsOn stage" flag.
+ /// @see setIsPresumedValidAtDependsOnStage() for a discussion.
+ bool getIsPresumedValidAtDependsOnStage() const
+ { return getImpl().getIsPresumedValidAtDependsOnStage(); }
+
+
+ /// Obtain write access to the Measure's value in order to modify it.
+ /// Calling this method marks the result invalid; you must explicitly
+ /// validate it when you're done. Also, if this Measure was created with
+ /// an \a invalidates stage, then that stage and all later stages in
+ /// the given state are marked invalid also.
+ T& updValue(const State& state) const
+ { return getImpl().updValue(state); }
+
+ /// Mark the current value as valid. This is done automatically if you
+ /// call setValue() but must be done manually if you use updValue() to
+ /// access the value. Note that you cannot mark this valid if
+ /// \a state hasn't been realized already to at least the stage prior
+ /// to this measure's \a dependsOn stage; that is, you must at least
+ /// be working on the dependsOn stage at the time this is called.
+ void markAsValid(const State& state) const {getImpl().markAsValid(state);}
+
+ /// Check whether the value contained in this Measure is currently
+ /// valid. If true, you can call getValue() to obtain it. If false,
+ /// calling getValue() will throw an exception.
+ bool isValid(const State& state) const {return getImpl().isValid(state);}
+
+ /// Manually mark the contained value as invalid. This will also
+ /// invalidate any stages in \a state that depend on this value. This
+ /// is called automatically whenever the updValue() method is invoked,
+ /// and the value starts out invalid. The value becomes valid either
+ /// by calling markAsValid() explicitly or calling setValue().
+ /// @warning If you have set this Result measure to be presumed valid
+ /// at dependsOn stage, this method will have no effect.
+ void markAsNotValid(const State& state) const
+ { getImpl().markAsNotValid(state); }
+
+ /// Set a new value and mark it as valid. For more flexibility, use the
+ /// updValue() method and markAsValid() manually when you are done with
+ /// the new value.
+ void setValue(const State& state, const T& value) const
+ { updValue(state) = value; markAsValid(state); }
+
+ SimTK_MEASURE_HANDLE_POSTSCRIPT(Result, Measure_<T>);
+};
+
+//==============================================================================
+// SINUSOID
+//==============================================================================
+/** This measure produces a sinusoidal function of time:
+<pre>
+ m(t) = a*sin(w*t+p)
+</pre>
+where a=amplitude in arbitrary units, w=frequency in rad/unit time, p=phase
+in radians. **/
+template <class T>
+class Measure_<T>::Sinusoid : public Measure_<T> {
+public:
+ SimTK_MEASURE_HANDLE_PREAMBLE(Sinusoid, Measure_<T>);
+
+ Sinusoid(Subsystem& sub,
+ const T& amplitude,
+ const T& frequency,
+ const T& phase=T(0))
+ : Measure_<T>(sub, new Implementation(amplitude,frequency,phase),
+ AbstractMeasure::SetHandle()) {}
+
+ SimTK_MEASURE_HANDLE_POSTSCRIPT(Sinusoid, Measure_<T>);
+};
+
+//==============================================================================
+// PLUS
+//==============================================================================
+/** This Measure is the sum of two Measures of the same type T.
+ at tparam T Any type that supports a plus operator that returns a sum
+ as another object of type T. In particular, Real, Vec<N>,
+ and Vector will work. **/
+template <class T>
+class Measure_<T>::Plus : public Measure_<T> {
+public:
+ SimTK_MEASURE_HANDLE_PREAMBLE(Plus, Measure_<T>);
+
+ Plus(Subsystem& sub, const Measure_<T>& left, const Measure_<T>& right)
+ : Measure_<T>(sub, new Implementation(left, right),
+ AbstractMeasure::SetHandle())
+ { SimTK_ERRCHK_ALWAYS
+ ( this->getSubsystem().isSameSubsystem(left.getSubsystem())
+ && this->getSubsystem().isSameSubsystem(right.getSubsystem()),
+ "Measure_<T>::Plus::ctor()",
+ "Arguments must be in the same Subsystem as this Measure.");
+ }
+
+ SimTK_MEASURE_HANDLE_POSTSCRIPT(Plus, Measure_<T>);
+};
+
+//==============================================================================
+// MINUS
+//==============================================================================
+/** This Measure is the difference of two Measures of the same type T.
+ at tparam T Any type that supports a subtract operator that returns the
+ difference as another object of type T. In particular, Real,
+ Vec<N>, and Vector will work. **/
+template <class T>
+class Measure_<T>::Minus : public Measure_<T> {
+public:
+ SimTK_MEASURE_HANDLE_PREAMBLE(Minus, Measure_<T>);
+
+ Minus(Subsystem& sub, const Measure_<T>& left, const Measure_<T>& right)
+ : Measure_<T>(sub, new Implementation(left, right),
+ AbstractMeasure::SetHandle())
+ { SimTK_ERRCHK_ALWAYS
+ ( this->getSubsystem().isSameSubsystem(left.getSubsystem())
+ && this->getSubsystem().isSameSubsystem(right.getSubsystem()),
+ "Measure_<T>::Minus::ctor()",
+ "Arguments must be in the same Subsystem as this Measure.");
+ }
+
+ SimTK_MEASURE_HANDLE_POSTSCRIPT(Minus, Measure_<T>);
+};
+
+//==============================================================================
+// SCALE
+//==============================================================================
+/** This Measure multiplies some other Measure by a Real scale factor.
+ at tparam T Any type that supports a scalar multiply, with the scalar on
+ the left, that returns the product as another object of type T.
+ In particular, Real, Vec<N>, and Vector will work. **/
+template <class T>
+class Measure_<T>::Scale : public Measure_<T> {
+public:
+ SimTK_MEASURE_HANDLE_PREAMBLE(Scale, Measure_<T>);
+
+ Scale(Subsystem& sub, Real factor, const Measure_<T>& operand)
+ : Measure_<T>(sub, new Implementation(factor, operand),
+ AbstractMeasure::SetHandle())
+ { SimTK_ERRCHK_ALWAYS
+ (this->getSubsystem().isSameSubsystem(operand.getSubsystem()),
+ "Measure_<T>::Scale::ctor()",
+ "Argument must be in the same Subsystem as this Measure.");
+ }
+
+ /** Get the operand (thing being scaled) measure for this measure. **/
+ const Measure_<T>& getOperandMeasure() const
+ { return getImpl().getOperandMeasure(); }
+
+ SimTK_MEASURE_HANDLE_POSTSCRIPT(Scale, Measure_<T>);
+};
+
+//==============================================================================
+// INTEGRATE
+//==============================================================================
+/** This measure yields the time integral of a given derivative measure,
+initializing with an initial condition measure of the same type T. The type
+T can be Real or a fixed size Vec type like Vec<3>, or a variable-size
+Vector. But in the case of a Vector you must say at construction what size
+the Vector will be during the simulation, so that an appropriate number
+of state variables can be allocated. **/
+template <class T>
+class Measure_<T>::Integrate : public Measure_<T> {
+public:
+ SimTK_MEASURE_HANDLE_PREAMBLE(Integrate, Measure_<T>);
+
+ /** Create a new measure that will use %Measure \a ic's value for initial
+ conditions, and then integrate the \a deriv %Measure. If type T is a
+ variable-length Vector you must also provide a const Vector to be used
+ to size and initialized the number of state variables at allocation. In
+ that case both the \a ic and \a deriv measures must return Vector values
+ of the same size as the allocation constant. **/
+ Integrate(Subsystem& subsystem,
+ const Measure_<T>& deriv,
+ const Measure_<T>& ic,
+ const T& initAlloc=T(0))
+ : Measure_<T>(subsystem, new Implementation(deriv,ic,initAlloc),
+ AbstractMeasure::SetHandle()) {}
+
+ /** Set the current value of this measure by modifying the state variables
+ that hold the integral. **/
+ void setValue(State& s, const T& value) const
+ { return getImpl().setValue(s, value); }
+
+ /** Get the integrand (derivative) measure for this integral. **/
+ const Measure_<T>& getDerivativeMeasure() const
+
+ { return getImpl().getDerivativeMeasure(); }
+ /** Get the measure whose value is used as an initial condition for the
+ integral at the start of an integration. **/
+ const Measure_<T>& getInitialConditionMeasure() const
+ { return getImpl().getInitialConditionMeasure(); }
+
+ Integrate& setDerivativeMeasure(const Measure_<T>& d)
+ { updImpl().setDerivativeMeasure(d); return *this; }
+ Integrate& setInitialConditionMeasure(const Measure_<T>& ic)
+ { updImpl().setInitialConditionMeasure(ic); return *this; }
+
+ SimTK_MEASURE_HANDLE_POSTSCRIPT(Integrate, Measure_<T>);
+};
+
+//==============================================================================
+// DIFFERENTIATE
+//==============================================================================
+/** This Measure operator returns the time derivative of its operand measure,
+or a numerical approximation of the time derivative if an analytic one is not
+available.
+
+If the operand measure provides its own derivative measure, then the value of
+the Differentiate operator is just the value of the operand's derivative
+measure, and this measure will have one fewer available derivatives than does
+the operand. If the operand does not have a derivative, then we will estimate
+it by the following method:
+ - retrieve the previous value f0 and previous derivative fdot0 of the
+ operand measure, and their sample time t0
+ - obtain the current value f(t) of the operand
+ - estimate fdot(t)=2(f-f0)/(t-t0) - fdot0 (fit a quadratic)
+ - record new samples f(t), fdot(t) with timestamp t
+
+Special cases:
+ - if t==t0 then fdot(t)=fdot0 (if available) else fdot(t)=0
+ - if fdot0 not available, fdot(t)=(f-f0)/(t-t0) (first order estimate)
+
+At initialization of a timestepping study beginning at t=t0, we sample the
+operand and record its initial value f0 at t0, and set fdot0=NaN. This
+ensures that we'll return zero as the initial derivative (for lack of anything
+better) and then use the first order method for the first step's derivative.
+**/
+template <class T>
+class Measure_<T>::Differentiate : public Measure_<T> {
+public:
+ SimTK_MEASURE_HANDLE_PREAMBLE(Differentiate, Measure_<T>);
+
+ /** Create a measure whose value is the time derivative of the given
+ \a operand measure.
+ @param subsystem The Subsystem into which this measure will be placed.
+ @param operand The Measure to be differentiated. **/
+ Differentiate(Subsystem& subsystem, const Measure_<T>& operand)
+ : Measure_<T>(subsystem, new Implementation(operand),
+ AbstractMeasure::SetHandle()) {}
+
+ /** Test whether the derivative returned as the value of this measure is
+ being estimated numerically, either because the operand measure is unable
+ to supply its derivative or because setForceUseApproximation(true) has
+ been called. **/
+ bool isUsingApproximation() const
+ { return getImpl().isUsingApproximation(); }
+
+ /** Get a reference to the measure that is being differentiated by this
+ measure. **/
+ const Measure_<T>& getOperandMeasure() const
+ { return getImpl().getOperandMeasure(); }
+
+ /** Set the measure that is to be differentiated by this measure. This is
+ a topology-stage change so you'll have to call realizeTopology() again on
+ the enclosing System before using it. **/
+ Differentiate& setOperandMeasure(const Measure_<T>& operand)
+ { updImpl().setOperandMeasure(operand); return *this; }
+
+ /** Force use of numerical approximation for the derivative, even if the
+ operand measure can supply its own derivative. This is not recommended!
+ This is a Topology-stage change. **/
+ void setForceUseApproximation(bool mustApproximate)
+ { updImpl().setForceUseApproximation(mustApproximate); }
+
+ /** Check the current value of the flag which forces this measure to use
+ numerical approximation regardless of whether the operand can supply its
+ own derivative. Note that even if the flag is currently false (the default)
+ we may still have to use approximation; see isUsingApproximation(). **/
+ bool getForceUseApproximation() const
+ { return getImpl().getForceUseApproximation(); }
+
+ SimTK_MEASURE_HANDLE_POSTSCRIPT(Differentiate, Measure_<T>);
+};
+
+//==============================================================================
+// EXTREME, MINIMUM, MAXIMUM, MINABS, MAXABS
+//==============================================================================
+/** This Measure tracks extreme values attained by the elements of its source
+operand since the last initialize() call or explicit call to setValue(). The
+extreme is either minimum or maximum and may be determined by the actual or
+absolute value of the operand. In any case the value of the %Extreme measure is
+the actual extreme value of the operand, not its absolute value.
+
+The template type T must be the same as the template type of the operand
+measure. If T is a Vec or Vector type, each element is treated separately.
+
+Normally %Extreme is not used directly; it is the common implementation
+underlying the Minimum, Maximum, MinAbs, and MaxAbs measures.
+
+Information available from this Measure:
+ - the current extreme value (at the source's stage)
+ - the value at the start of the current step (Time stage)
+ - time of last change
+ - the DiscreteVariableIndex holding the previous value
+ - the CacheEntryIndex designated for the current/next value
+ - a reference to the source Measure
+
+The time derivative fdot of f(t)=min_t0_t(s(t')) where s is the
+source measure and t0 <= t' <= t is
+<pre>
+ fdot(t) = s(t) < f(ti) && sdot(t) < 0 ? sdot(t) : 0
+</pre>
+where ti is the time at the start of the current step.
+
+At the start of a continuous interval, the updated value (if any)
+replaces the stored value.
+
+<h3>Extreme isolation (not implemented yet)</h3>
+If the time derivative of the
+source operand is available, the measure will arrange to ensure
+precise isolation of the minimum values by defining a triggered
+event that watches for negative-to-positive sign changes of the derivative.
+Then if you output reporting data upon the occurrence of triggered
+events (as well as your regularly scheduled output) your data will
+include the precise minimum (to within a specifiable isolation time window).
+
+Additional information available in this case:
+ - the EventId of the extreme-trapping event if there is one
+ - the time derivative of the Extreme measure
+**/
+template <class T>
+class Measure_<T>::Extreme : public Measure_<T> {
+public:
+ SimTK_MEASURE_HANDLE_PREAMBLE(Extreme, Measure_<T>);
+
+ enum Operation {
+ MaxAbs, // default
+ Maximum,
+ MinAbs,
+ Minimum
+ };
+
+ /** Default behavior for the %Extreme measure is to find the operand's
+ value that is of maximum absolute value. You can change that to minimum
+ and/or actual value. **/
+ Extreme(Subsystem& sub, const Measure_<T>& operand, Operation op=MaxAbs)
+ : Measure_<T>(sub, new Implementation(operand, op),
+ AbstractMeasure::SetHandle()) {}
+
+ /** Set the operation to be performed. The default operation is MaxAbs. **/
+ Extreme& setOperation(Operation op)
+ { updImpl().setOperation(op); return *this; }
+
+ /** Return the operation currently being performed by this measure. **/
+ Operation getOperation() const {return getImpl().getOperation();}
+
+ /** Return the time at which the reported extreme value first occurred.
+ This is the current time if the operand is at its extreme value now,
+ otherwise it is the time that the extreme value first occurred during a
+ time stepping study. The \a state must be realized to the level required
+ to evaluate the operand measure. **/
+ Real getTimeOfExtremeValue(const State& state) const
+ { return getImpl().getTimeOfExtremeValue(state); }
+
+ void setValue(State& s, const T& value) const
+ { return getImpl().setValue(s, value); }
+
+ const Measure_<T>& getOperandMeasure() const
+ { return getImpl().getOperandMeasure(); }
+
+ Extreme& setOperandMeasure(const Measure_<T>& s)
+ { updImpl().setOperandMeasure(s); return *this; }
+
+ SimTK_MEASURE_HANDLE_POSTSCRIPT(Extreme, Measure_<T>);
+};
+
+/** Track the minimum value of the operand (signed).
+ at see Measure_::Extreme **/
+template <class T>
+class Measure_<T>::Minimum : public Measure_<T>::Extreme {
+ typedef typename Measure_<T>::Extreme Super;
+public:
+ Minimum(Subsystem& sub, const Measure_<T>& operand)
+ : Super(sub, operand, Super::Minimum) {}
+};
+
+/** Track the maximum value of the operand (signed).
+ at see Measure_::Extreme **/
+template <class T>
+class Measure_<T>::Maximum : public Measure_<T>::Extreme {
+ typedef typename Measure_<T>::Extreme Super;
+public:
+ Maximum(Subsystem& sub, const Measure_<T>& operand)
+ : Super(sub, operand, Super::Maximum) {}
+};
+
+/** Track the value of the operand that is of maximum absolute value.
+ at see Measure_::Extreme **/
+template <class T>
+class Measure_<T>::MaxAbs : public Measure_<T>::Extreme {
+ typedef typename Measure_<T>::Extreme Super;
+public:
+ MaxAbs(Subsystem& sub, const Measure_<T>& operand)
+ : Super(sub, operand, Super::MaxAbs) {}
+};
+
+/** Track the value of the operand that is of minimum absolute value (not very
+useful).
+ at see Measure_::Extreme **/
+template <class T>
+class Measure_<T>::MinAbs : public Measure_<T>::Extreme {
+ typedef typename Measure_<T>::Extreme Super;
+public:
+ MinAbs(Subsystem& sub, const Measure_<T>& operand)
+ : Super(sub, operand, Super::MinAbs) {}
+};
+
+//==============================================================================
+// DELAY
+//==============================================================================
+/** (CAUTION: still under development)
+This is a %Measure whose value at time t is the value that its
+\a source operand had at time t-delay for a specified \a delay. For times prior
+to the start of a simulation this %Measure behaves as though the source value
+had been constant at its initial value.
+
+When the \a source %Measure can provide a time derivative dvalue we use saved
+(t,value,dvalue) triples surrounding the required time to construct a cubic
+Hermite interpolant giving a third-order accurate estimate of the delayed
+value. Otherwise we use more data points to construct the cubic interpolant but
+the accuracy cannot be guaranteed. If there aren't enough data points, then
+linear interpolation is used instead. There is an option to force use of linear
+interpolation if you prefer.
+
+In the case where the delayed time is within the current step, we would need
+the current \a source value in order to interpolate. We assume that is not
+available (commonly the current value depends on the delayed value) so have to
+extrapolate beyond the last buffered value in that case. Extrapolation is
+considerably less accurate than interpolation, so when step sizes are large
+compared to delay times the accuracy of the delayed value is reduced. In cases
+where the \a source does not depend on its delayed value, you can request that
+the current value be used if necessary, ensuring consistent accuracy.
+Alternatively, you can set the maximum integrator step size to be just less
+than the minimum delay time, guaranteeing that there will always be an entry
+already in the buffer that is later than any requested delayed time. That
+could have a substantial performance penalty if steps much larger than the
+delay would otherwise have been taken.
+
+<h3>%Implementation</h3>
+This %Measure maintains a variable-sized buffer holding values that the
+\a source measure and its time derivative (if available) had at each time step
+starting just prior to t-delay until just before the current time t. New values
+are added to the end of the buffer as integrator steps are completed, and old
+values that are no longer needed are removed from the beginning. When a value is
+requested at current time t, the %Measure interpolates using values from just
+prior to t-delay and just afterwards to approximate the value at t-delay.
+
+ at bug Only linear interpolation implemented so far.
+ at bug There should be an option for the measure to specify a sampling interval
+that would force the integrator to provide interpolated states at least that
+often.
+ at bug The current implementation involves a lot of unnecessary copying because
+it uses an auto-update state variable and cache entry. It should be using
+an end-of-step event handler to update the buffer directly in the state.
+
+ at see Measure_::Integrate, Measure_::Differentiate **/
+template <class T>
+class Measure_<T>::Delay : public Measure_<T> {
+public:
+ /** @cond **/
+ SimTK_MEASURE_HANDLE_PREAMBLE(Delay, Measure_<T>);
+ /** @endcond **/
+
+ /** Create a %Measure whose output is the same as the given \a source
+ measure but delayed by a time \a delay. **/
+ Delay(Subsystem& sub, const Measure_<T>& source, Real delay)
+ : Measure_<T>(sub, new Implementation(source, delay),
+ AbstractMeasure::SetHandle()) {}
+
+ /** (Advanced) Restrict the %Delay measure to use only linear interpolation
+ to estimate delayed values. By default it uses cubic interpolation whenever
+ possible. Cubic interpolation will almost always be better but can be
+ unstable in some circumstances. Despite its name this flag also applies
+ to extrapolation if we have to do any. This is a topological change. **/
+ Delay& setUseLinearInterpolationOnly(bool linearOnly)
+ { updImpl().setUseLinearInterpolationOnly(linearOnly); return *this; }
+
+ /** (Advanced) Allow the %Delay measure to refer to the current
+ value when estimating the delayed value. Normally we expect that the current
+ value might depend on the delayed value so is not available at the time
+ we ask for the delayed value. That means that if the delayed time is
+ between the current time and the last saved time (that is, it is a time
+ during the current integration step), the measure will have to
+ extrapolate from the last-saved values to avoid requiring the current
+ value to be available. With this approach the "depends on" time for a %Delay
+ measure is just Time stage since it does not depend on any current
+ calculations. However, \e extrapolation is much less accurate than
+ \e interpolation so if you don't mind the "depends on" stage for a %Delay
+ measure being the same stage as for its \a source measure, then you can get
+ nicer interpolated values. This is a topological change. **/
+ Delay& setCanUseCurrentValue(bool canUseCurrentValue)
+ { updImpl().setCanUseCurrentValue(canUseCurrentValue); return *this; }
+
+ /** Replace the \a source measure. This is a topological change. **/
+ Delay& setSourceMeasure(const Measure_<T>& source)
+ { updImpl().setSourceMeasure(source); return *this; }
+
+ /** Change the \a delay time. This is a topological change. **/
+ Delay& setDelay(Real delay)
+ { updImpl().setDelay(delay); return *this; }
+
+ /** Return the value of the "use linear interpolation only" flag. **/
+ bool getUseLinearInterpolationOnly() const
+ { return getImpl().getUseLinearInterpolationOnly(); }
+
+ /** Return the value of the "can use current value" flag. **/
+ bool getCanUseCurrentValue() const
+ { return getImpl().getCanUseCurrentValue(); }
+
+ /** Obtain a reference to the \a source %Measure. **/
+ const Measure_<T>& getSourceMeasure() const
+ { return getImpl().getSourceMeasure(); }
+
+ /** Get the amount of time by which this %Measure is delaying its
+ \a source %Measure. **/
+ Real getDelay() const
+ { return getImpl().getDelay(); }
+
+ /** @cond **/
+ SimTK_MEASURE_HANDLE_POSTSCRIPT(Delay, Measure_<T>);
+ /** @endcond **/
+};
+
+//==============================================================================
+// SAMPLE AND HOLD
+//==============================================================================
+/** NOT IMPLEMENTED YET --
+This is a Measure operator which, upon occurrence of a designated event,
+samples its source Measure and then holds its value in a discrete state
+variable until the next occurrence of the event. Any type of data can be
+sampled this way.
+
+Information available from this Measure:
+ - the held value (Time stage)
+ - time of last sample
+ - the DiscreteVariableIndex holding the sampled value
+ - a reference to the operand Measure
+
+Study initialization is always considered a sampling event.
+This measure has no time derivative. **/
+template <class T>
+class Measure_<T>::SampleAndHold : public Measure_<T> {
+public:
+ SimTK_MEASURE_HANDLE_PREAMBLE(SampleAndHold, Measure_<T>);
+
+ SampleAndHold(Subsystem& sub, const Measure_<T>& source, EventId e);
+
+ /// Set the held value to a particular value, unrelated to the source.
+ /// The time stamp will be taken from the supplied State.
+ void setValue(State& s, const T& value) const;
+
+ /// Force this Measure to sample its input at the current time.
+ void sample(State& s) const;
+
+ const Measure_<T>& getSource() const;
+ EventId getEventId() const;
+
+ SampleAndHold& setSource(const Measure_<T>& s);
+ SampleAndHold& setEventId(EventId);
+
+ SimTK_MEASURE_HANDLE_POSTSCRIPT(SampleAndHold, Measure_<T>);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_MEASURE_H_
diff --git a/SimTKcommon/Simulation/include/SimTKcommon/internal/MeasureImplementation.h b/SimTKcommon/Simulation/include/SimTKcommon/internal/MeasureImplementation.h
new file mode 100644
index 0000000..3acfd0f
--- /dev/null
+++ b/SimTKcommon/Simulation/include/SimTKcommon/internal/MeasureImplementation.h
@@ -0,0 +1,2140 @@
+#ifndef SimTK_SimTKCOMMON_MEASURE_IMPLEMENTATION_H_
+#define SimTK_SimTKCOMMON_MEASURE_IMPLEMENTATION_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/State.h"
+#include "SimTKcommon/internal/Measure.h"
+#include "SimTKcommon/internal/Subsystem.h"
+#include "SimTKcommon/internal/System.h"
+#include "SimTKcommon/internal/SubsystemGuts.h"
+
+#include <cmath>
+
+
+namespace SimTK {
+
+
+//==============================================================================
+// ABSTRACT MEASURE :: IMPLEMENTATION
+//==============================================================================
+
+/**
+ * The abstract parent of all Measure Implementation classes.
+ */
+class SimTK_SimTKCOMMON_EXPORT AbstractMeasure::Implementation {
+protected:
+ /** This default constructor is for use by concrete measure implementation
+ classes. **/
+ Implementation() : copyNumber(0), mySubsystem(0), refCount(0) {}
+
+ /** Base class copy constructor removes the Subsystem
+ and sets the reference count to zero. This gets used by the clone()
+ methods in the concrete classes. **/
+ Implementation(const Implementation& src)
+ : copyNumber(src.copyNumber+1), mySubsystem(0), refCount(0) {}
+
+ /** Base class copy assignment operator removes the
+ Subsystem, and sets the reference count to zero. This is probably
+ not used. **/
+ Implementation& operator=(const Implementation& src) {
+ if (&src != this)
+ { copyNumber=src.copyNumber+1;
+ refCount=0; mySubsystem=0; }
+ return *this;
+ }
+
+ // destructor is virtual; below
+
+ // Increment the reference count and return its new value.
+ int incrRefCount() const {return ++refCount;}
+
+ // Decrement the reference count and return its new value.
+ int decrRefCount() const {return --refCount;}
+
+ // Get the current value of the reference counter.
+ int getRefCount() const {return refCount;}
+
+ int getCopyNumber() const {return copyNumber;}
+
+ /** This is a deep copy of the concrete Implementation object, except the
+ Subsystem will have been removed. The reference count on the new object
+ will be zero; be sure to increment it if you put it in a handle. **/
+ Implementation* clone() const {return cloneVirtual();}
+
+ // realizeTopology() is pure virtual below for Measure_<T> to supply.
+ void realizeModel (State& s) const {realizeMeasureModelVirtual(s);}
+ void realizeInstance (const State& s) const {realizeMeasureInstanceVirtual(s);}
+ void realizeTime (const State& s) const {realizeMeasureTimeVirtual(s);}
+ void realizePosition (const State& s) const {realizeMeasurePositionVirtual(s);}
+ void realizeVelocity (const State& s) const {realizeMeasureVelocityVirtual(s);}
+ void realizeDynamics (const State& s) const {realizeMeasureDynamicsVirtual(s);}
+ void realizeAcceleration(const State& s) const {realizeMeasureAccelerationVirtual(s);}
+ void realizeReport (const State& s) const {realizeMeasureReportVirtual(s);}
+
+ /** This should be called at the start of a time stepping study to
+ cause this %Measure to set its state variables (if any) in the supplied
+ state to their initial conditions. **/
+ void initialize(State& s) const {initializeVirtual(s);}
+
+ int getNumTimeDerivatives() const {return getNumTimeDerivativesVirtual();}
+
+ Stage getDependsOnStage(int derivOrder) const {
+ SimTK_ERRCHK2(0 <= derivOrder && derivOrder <= getNumTimeDerivatives(),
+ "Measure::getDependsOnStage()",
+ "derivOrder %d was out of range; this Measure allows 0-%d.",
+ derivOrder, getNumTimeDerivatives());
+ return getDependsOnStageVirtual(derivOrder);
+ }
+
+
+ void setSubsystem(Subsystem& sub, MeasureIndex mx)
+ { assert(!mySubsystem && mx.isValid());
+ mySubsystem = ⊂ myIndex = mx; }
+
+ bool isInSubsystem() const {return mySubsystem != 0;}
+ const Subsystem& getSubsystem() const {assert(mySubsystem); return *mySubsystem;}
+ Subsystem& updSubsystem() {assert(mySubsystem); return *mySubsystem;}
+ MeasureIndex getSubsystemMeasureIndex() const {assert(mySubsystem); return myIndex;}
+ SubsystemIndex getSubsystemIndex() const
+ { return getSubsystem().getMySubsystemIndex(); }
+
+ void invalidateTopologyCache() const
+ { if (isInSubsystem()) getSubsystem().invalidateSubsystemTopologyCache(); }
+
+ Stage getStage(const State& s) const {return getSubsystem().getStage(s);}
+
+ // VIRTUALS //
+ // Ordinals must retain the same meaning from release to release
+ // to preserve binary compatibility.
+
+ /* 0*/virtual ~Implementation() {}
+ /* 1*/virtual Implementation* cloneVirtual() const = 0;
+
+ /* 2*/virtual void realizeTopology(State&)const = 0;
+
+ /* 3*/virtual void realizeMeasureModelVirtual(State&) const {}
+ /* 4*/virtual void realizeMeasureInstanceVirtual(const State&) const {}
+ /* 5*/virtual void realizeMeasureTimeVirtual(const State&) const {}
+ /* 6*/virtual void realizeMeasurePositionVirtual(const State&) const {}
+ /* 7*/virtual void realizeMeasureVelocityVirtual(const State&) const {}
+ /* 8*/virtual void realizeMeasureDynamicsVirtual(const State&) const {}
+ /* 9*/virtual void realizeMeasureAccelerationVirtual(const State&) const {}
+ /*10*/virtual void realizeMeasureReportVirtual(const State&) const {}
+
+ /*11*/virtual void initializeVirtual(State&) const {}
+ /*12*/virtual int
+ getNumTimeDerivativesVirtual() const {return 0;}
+ /*13*/virtual Stage
+ getDependsOnStageVirtual(int order) const = 0;
+
+private:
+ int copyNumber; // bumped each time we do a deep copy
+
+ // These are set when this Measure is adopted by a Subsystem.
+ Subsystem* mySubsystem;
+ MeasureIndex myIndex;
+
+ // Measures have shallow copy semantics so they share the Implementation
+ // objects, which are only deleted when the refCount goes to zero.
+ mutable int refCount;
+
+friend class AbstractMeasure;
+friend class Subsystem::Guts;
+friend class Subsystem::Guts::GutsRep;
+};
+
+//==============================================================================
+// ABSTRACT MEASURE DEFINITIONS
+//==============================================================================
+// These had to wait for AbstractMeasure::Implementation to be defined.
+
+inline AbstractMeasure::
+AbstractMeasure(Implementation* g)
+: impl(g)
+{ if (impl) impl->incrRefCount(); }
+
+inline AbstractMeasure::
+AbstractMeasure(Subsystem& sub, Implementation* g, const SetHandle&)
+: impl(g) {
+ SimTK_ERRCHK(hasImpl(), "AbstractMeasure::AbstractMeasure()",
+ "An empty Measure handle can't be put in a Subsystem.");
+ impl->incrRefCount();
+ sub.adoptMeasure(*this);
+}
+
+// Shallow copy constructor.
+inline AbstractMeasure::AbstractMeasure(const AbstractMeasure& src)
+: impl(0) {
+ if (src.impl) {
+ impl = src.impl;
+ impl->incrRefCount();
+ }
+}
+
+// Shallow assignment.
+inline AbstractMeasure& AbstractMeasure::
+shallowAssign(const AbstractMeasure& src) {
+ if (impl != src.impl) {
+ if (impl && impl->decrRefCount()==0) delete impl;
+ impl = src.impl;
+ impl->incrRefCount();
+ }
+ return *this;
+}
+
+// Note that even if the source and destination are currently pointing
+// to the same Implementation, we still have to make a new copy so that
+// afterwards the destination has its own, refcount==1 copy.
+inline AbstractMeasure& AbstractMeasure::
+deepAssign(const AbstractMeasure& src) {
+ if (&src != this) {
+ if (impl && impl->decrRefCount()==0) delete impl;
+ if (src.impl) {
+ impl = src.impl->clone();
+ impl->incrRefCount();
+ } else
+ impl = 0;
+ }
+ return *this;
+}
+
+inline AbstractMeasure::
+~AbstractMeasure()
+{ if (impl && impl->decrRefCount()==0) delete impl;}
+
+inline bool AbstractMeasure::
+isInSubsystem() const
+{ return hasImpl() && getImpl().isInSubsystem(); }
+
+inline const Subsystem& AbstractMeasure::
+getSubsystem() const
+{ return getImpl().getSubsystem(); }
+
+inline MeasureIndex AbstractMeasure::
+getSubsystemMeasureIndex() const
+{ return getImpl().getSubsystemMeasureIndex();}
+
+inline int AbstractMeasure::
+getNumTimeDerivatives() const
+{ return getImpl().getNumTimeDerivatives(); }
+
+inline Stage AbstractMeasure::
+getDependsOnStage(int derivOrder) const
+{ return getImpl().getDependsOnStage(derivOrder); }
+
+inline int AbstractMeasure::
+getRefCount() const
+{ return getImpl().getRefCount(); }
+
+
+/** @cond **/ // Hide from Doxygen.
+// This is a helper class that makes it possible to treat Real, Vec, and
+// Vector objects uniformly.
+template <class T> class Measure_Num {
+};
+
+template <> class Measure_Num<float> {
+public:
+ typedef float Element;
+ static int size(const float&) {return 1;}
+ static const float& get(const float& v, int i) {assert(i==0); return v;}
+ static float& upd(float& v, int i) {assert(i==0); return v;}
+ static void makeNaNLike(const float&, float& nanValue)
+ { nanValue = CNT<float>::getNaN();}
+ static void makeZeroLike(const float&, float& zeroValue) {zeroValue=0.f;}
+};
+
+template <> class Measure_Num<double> {
+public:
+ typedef double Element;
+ static int size(const double&) {return 1;}
+ static const double& get(const double& v, int i) {assert(i==0); return v;}
+ static double& upd(double& v, int i) {assert(i==0); return v;}
+ static void makeNaNLike(const double&, double& nanValue)
+ { nanValue = CNT<double>::getNaN(); }
+ static void makeZeroLike(const double&, double& zeroValue) {zeroValue=0.;}
+};
+
+// We only support stride 1 (densely packed) Vec types.
+template <int M, class E>
+class Measure_Num< Vec<M,E,1> > {
+ typedef Vec<M,E,1> T;
+public:
+ typedef E Element;
+ static int size(const T&) {return M;}
+ static const E& get(const T& v, int i) {return v[i];}
+ static E& upd(T& v, int i) {return v[i];}
+ static void makeNaNLike (const T&, T& nanValue) {nanValue.setToNaN();}
+ static void makeZeroLike(const T&, T& zeroValue) {zeroValue.setToZero();}
+};
+
+// We only support column major (densely packed) Mat types.
+template <int M, int N, class E>
+class Measure_Num< Mat<M,N,E> > {
+ typedef Mat<M,N,E> T;
+public:
+ typedef E Element;
+ static int size(const T&) {return N;} // number of columns
+ static const typename T::TCol& get(const T& m, int j) {return m.col(j);}
+ static typename T::TCol& upd(T& m, int j) {return m.col(j);}
+ static void makeNaNLike (const T&, T& nanValue) {nanValue.setToNaN();}
+ static void makeZeroLike(const T&, T& zeroValue) {zeroValue.setToZero();}
+};
+
+
+template <class E>
+class Measure_Num< Vector_<E> > {
+ typedef Vector_<E> T;
+public:
+ typedef E Element;
+ static int size(const T& v) {return v.size();}
+ static const E& get(const T& v, int i) {return v[i];}
+ static E& upd(T& v, int i) {return v[i];}
+ static void makeNaNLike(const T& v, T& nanValue)
+ { nanValue.resize(v.size()); nanValue.setToNaN(); }
+ static void makeZeroLike(const T& v, T& zeroValue)
+ { zeroValue.resize(v.size()); zeroValue.setToZero(); }
+
+};
+
+/** @endcond **/
+
+//==============================================================================
+// MEASURE_<T> :: IMPLEMENTATION
+//==============================================================================
+/** This is the base Implementation class for all Measures whose value type is
+known. This class is still abstract but provides many services related to the
+values of the derived Measure and its derivatives, all of which require cache
+entries of type T.
+
+The constructor needs to be told how many type-T cache entries to allocate. **/
+template <class T>
+class Measure_<T>::Implementation : public AbstractMeasure::Implementation {
+public:
+ const T& getValue(const State& s, int derivOrder) const {
+ SimTK_ERRCHK2(0 <= derivOrder && derivOrder <= getNumTimeDerivatives(),
+ "Measure_<T>::getValue()",
+ "derivOrder %d was out of range; this Measure allows 0-%d.",
+ derivOrder, getNumTimeDerivatives());
+
+ // We require the stage to have been advanced to at least the one
+ // before this measure's depends-on stage since this will get called
+ // towards the end of the depends-on stage realization.
+ if (getDependsOnStage(derivOrder) != Stage::Empty) {
+ Stage prevStage = getDependsOnStage(derivOrder).prev();
+
+ SimTK_ERRCHK2
+ ( ( isInSubsystem() && getStage(s)>=prevStage)
+ || (!isInSubsystem() && s.getSystemStage()>=prevStage),
+ "Measure_<T>::getValue()",
+ "Expected State to have been realized to at least stage "
+ "%s but stage was %s.",
+ prevStage.getName().c_str(),
+ (isInSubsystem() ? getStage(s) : s.getSystemStage())
+ .getName().c_str());
+ }
+
+ if (derivOrder < getNumCacheEntries()) {
+ if (!isCacheValueRealized(s,derivOrder)) {
+ T& value = updCacheEntry(s,derivOrder);
+ calcCachedValueVirtual(s, derivOrder, value);
+ markCacheValueRealized(s,derivOrder);
+ return value;
+ }
+ return getCacheEntry(s,derivOrder);
+ }
+
+ // We can't handle it here -- punt to the concrete Measure
+ // for higher order derivatives.
+ return getUncachedValueVirtual(s,derivOrder);
+ }
+
+ /** Set a new default value for this %Measure. This is a topological
+ change. **/
+ void setDefaultValue(const T& defaultValue) {
+ this->defaultValue = defaultValue;
+ Measure_Num<T>::makeZeroLike(defaultValue, zeroValue);
+ this->invalidateTopologyCache();
+ }
+
+ /** Return a reference to the value that this %Measure will use to
+ initialize its value-level state resource (state variable or cache entry)
+ during the next call to realizeTopology(). **/
+ const T& getDefaultValue() const {return defaultValue;}
+
+ void setIsPresumedValidAtDependsOnStage(bool presume)
+ { presumeValidAtDependsOnStage = presume;
+ this->invalidateTopologyCache(); }
+
+ bool getIsPresumedValidAtDependsOnStage() const
+ { return presumeValidAtDependsOnStage; }
+
+protected:
+ explicit Implementation(const T& defaultValue, int numCacheEntries=1)
+ : presumeValidAtDependsOnStage(false),
+ defaultValue(defaultValue),
+ derivIx(numCacheEntries)
+ {
+ Measure_Num<T>::makeZeroLike(defaultValue, zeroValue);
+ }
+
+ /** Argument \a numCacheEntries should be one greater than the number of
+ derivatives; that is, there is room for the value ("0th" derivative) also.
+ The default is to allocate just room for the value. **/
+ explicit Implementation(int numCacheEntries=1)
+ : presumeValidAtDependsOnStage(false),
+ defaultValue(),
+ derivIx(numCacheEntries)
+ {
+ Measure_Num<T>::makeZeroLike(defaultValue, zeroValue);
+ }
+
+ /** Copy constructor copies the \e number of cache entries from the source,
+ but not the cache indices themselves as those must be allocated uniquely
+ for the copy. **/
+ Implementation(const Implementation& source)
+ : presumeValidAtDependsOnStage(source.presumeValidAtDependsOnStage),
+ defaultValue(source.defaultValue),
+ derivIx(source.derivIx.size())
+ {
+ Measure_Num<T>::makeZeroLike(defaultValue, zeroValue);
+ }
+
+
+ /** Return the number of elements in the data type of this %Measure; for
+ Vector measures this is determined by the size of the default value. **/
+ int size() const {return Measure_Num<T>::size(defaultValue);}
+
+ /** Return the number of cache entries allocated for the value and
+ derivatives of this %Measure. **/
+ int getNumCacheEntries() const {return (int)derivIx.size();}
+
+ /** Get a const reference to the value stored in one of this %Measure's
+ cache entries, indexed by the derivative order (with the value treated as
+ the 0th derivative). **/
+ const T& getCacheEntry(const State& s, int derivOrder) const {
+ SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(),
+ "Measure_<T>::Implementation::getCacheEntry()",
+ "Derivative order %d is out of range; only %d cache entries"
+ " were allocated.", derivOrder, getNumCacheEntries());
+
+ return Value<T>::downcast(
+ this->getSubsystem().getCacheEntry(s, derivIx[derivOrder]));
+ }
+
+ /** Get a writable reference to the value stored in one of this %Measure's
+ cache entries, indexed by the derivative order (with the value treated as
+ the 0th derivative). **/
+ T& updCacheEntry(const State& s, int derivOrder) const {
+ SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(),
+ "Measure_<T>::Implementation::updCacheEntry()",
+ "Derivative order %d is out of range; only %d cache entries"
+ " were allocated.", derivOrder, getNumCacheEntries());
+
+ return Value<T>::updDowncast(
+ this->getSubsystem().updCacheEntry(s, derivIx[derivOrder]));
+ }
+
+ /** Determine whether a particular one of this %Measure's cache entries has
+ already been realized since the given state was modified. **/
+ bool isCacheValueRealized(const State& s, int derivOrder) const {
+ SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(),
+ "Measure_<T>::Implementation::isCacheValueRealized()",
+ "Derivative order %d is out of range; only %d cache entries"
+ " were allocated.", derivOrder, getNumCacheEntries());
+
+ return this->getSubsystem().isCacheValueRealized(s, derivIx[derivOrder]);
+ }
+
+ /** Mark one of this %Measure's cache entries up to date; call this after
+ you have calculated a value or derivative and stored it in the
+ corresponding cache entry. **/
+ void markCacheValueRealized(const State& s, int derivOrder) const {
+ SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(),
+ "Measure_<T>::Implementation::markCacheValueRealized()",
+ "Derivative order %d is out of range; only %d cache entries"
+ " were allocated.", derivOrder, getNumCacheEntries());
+
+ this->getSubsystem().markCacheValueRealized(s, derivIx[derivOrder]);
+ }
+
+ /** Invalidate one of this %Measure's cache entries. This is not normally
+ necessary since the cache entries will be invalidated automatically when
+ state variables they depend on change. However, this can be useful in
+ some cases, particularly during debugging and testing. **/
+ void markCacheValueNotRealized(const State& s, int derivOrder) const {
+ SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(),
+ "Measure_<T>::Implementation::markCacheValueNotRealized()",
+ "Derivative order %d is out of range; only %d cache entries"
+ " were allocated.", derivOrder, getNumCacheEntries());
+
+ this->getSubsystem().markCacheValueNotRealized(s, derivIx[derivOrder]);
+ }
+
+ // VIRTUALS //
+ // Ordinals must retain the same meaning from release to release
+ // to preserve binary compatibility.
+
+ /** Concrete measures can override this to allocate Topology-stage
+ resources. **/
+ /* 0*/virtual void realizeMeasureTopologyVirtual(State&) const
+ {}
+
+ /** Concrete measures must override this if the state cache is used for
+ precalculated values or derivatives. **/
+ /* 1*/virtual void
+ calcCachedValueVirtual(const State&, int derivOrder, T& value) const
+ { SimTK_ERRCHK1_ALWAYS(!"implemented",
+ "Measure_<T>::Implementation::calcCachedValueVirtual()",
+ "This method should have been overridden by the derived"
+ " Measure but was not. It is needed to calculate the"
+ " cached value for derivOrder=%d.", derivOrder); }
+
+ /** This is only called when derivOrder >= the number of cache
+ entries we have, but still <= the number of derivatives the
+ %Measure says it can deliver. You don't need to override this if that
+ condition can't occur. This is commonly used for functions whose derivatives
+ above a certain order are zero. **/
+ /* 2*/virtual const T&
+ getUncachedValueVirtual(const State&, int derivOrder) const
+ { SimTK_ERRCHK1_ALWAYS(!"implemented",
+ "Measure_<T>::Implementation::getUncachedValueVirtual()",
+ "This method should have been overridden by the derived"
+ " Measure but was not. It is needed to return the uncached"
+ " value at derivOrder=%d.", derivOrder);
+ return *reinterpret_cast<T*>(0);
+ }
+
+ /** Return a reference to a zero of the same type and size as this
+ %Measure's value. **/
+ const T& getValueZero() const {return zeroValue;}
+
+private:
+ // Satisfy the realizeTopology() pure virtual here now that we know the
+ // data type T. Allocate lazy- or auto-validated- cache entries depending
+ // on the setting of presumeValidAtDependsOnStage.
+ void realizeTopology(State& s) const FINAL_11 {
+ // Allocate cache entries. Initialize the value cache entry to
+ // the given defaultValue; all the derivative cache entries should be
+ // initialized to a NaN of the same size.
+ if (getNumCacheEntries()) {
+ derivIx[0] = presumeValidAtDependsOnStage
+ ? this->getSubsystem().allocateCacheEntry
+ (s, getDependsOnStage(0), new Value<T>(defaultValue))
+ : this->getSubsystem().allocateLazyCacheEntry
+ (s, getDependsOnStage(0), new Value<T>(defaultValue));
+
+ if (getNumCacheEntries() > 1) {
+ T nanValue; Measure_Num<T>::makeNaNLike(defaultValue, nanValue);
+ for (int i=1; i < getNumCacheEntries(); ++i) {
+ derivIx[i] = presumeValidAtDependsOnStage
+ ? this->getSubsystem().allocateCacheEntry
+ (s, getDependsOnStage(i), new Value<T>(nanValue))
+ : this->getSubsystem().allocateLazyCacheEntry
+ (s, getDependsOnStage(i), new Value<T>(nanValue));
+ }
+ }
+ }
+
+ // Call the concrete class virtual if any.
+ realizeMeasureTopologyVirtual(s);
+ }
+
+//------------------------------------------------------------------------------
+private:
+ // TOPOLOGY STATE
+ bool presumeValidAtDependsOnStage;
+ T defaultValue;
+ T zeroValue;
+
+ // TOPOLOGY CACHE
+ mutable Array_<CacheEntryIndex> derivIx;
+};
+
+
+
+//==============================================================================
+// CONSTANT :: IMPLEMENTATION
+//==============================================================================
+template <class T>
+class Measure_<T>::Constant::Implementation
+: public Measure_<T>::Implementation
+{
+public:
+ // We don't want the base class to allocate *any* cache entries.
+ Implementation() : Measure_<T>::Implementation(0) {}
+ explicit Implementation(const T& value)
+ : Measure_<T>::Implementation(value,0) {}
+
+ /** Changing the value of a %Constant measure is a topological change;
+ if this is a Vector measure you can change the size here too. **/
+ void setValue(const T& v) {this->setDefaultValue(v);}
+
+ // Implementations of virtual methods.
+ // Measure_<T> virtuals:
+ // No cached values.
+
+ const T& getUncachedValueVirtual(const State&, int derivOrder) const
+ OVERRIDE_11
+ { return derivOrder>0 ? this->getValueZero() : this->getDefaultValue(); }
+
+ // AbstractMeasure virtuals:
+ Implementation* cloneVirtual() const OVERRIDE_11
+ { return new Implementation(*this); }
+ Stage getDependsOnStageVirtual(int derivOrder) const OVERRIDE_11
+ { return derivOrder>0 ? Stage::Empty : Stage::Topology; }
+ int getNumTimeDerivativesVirtual() const OVERRIDE_11
+ { return std::numeric_limits<int>::max(); }
+};
+
+
+
+//==============================================================================
+// MEASURE ZERO and ONE
+//==============================================================================
+// These had to wait for Constant::Implementation to be declared.
+
+template <class T> inline
+Measure_<T>::Zero::Zero() : Constant(T(0)) {}
+template <class T> inline
+Measure_<T>::Zero::Zero(Subsystem& sub) : Constant(sub, T(0)) {}
+
+inline Measure_< Vector >::Zero::Zero(int size)
+: Constant(Vector(size, Real(0))) {}
+inline Measure_< Vector >::Zero::Zero(Subsystem& sub, int size)
+: Constant(sub, Vector(size, Real(0))) {}
+
+template <class T> inline
+Measure_<T>::One::One() : Constant(T(1)) {}
+template <class T> inline
+Measure_<T>::One::One(Subsystem& sub) : Constant(sub, T(1)) {}
+
+inline Measure_< Vector >::One::One(int size)
+: Constant(Vector(size, Real(1))) {}
+inline Measure_< Vector >::One::One(Subsystem& sub, int size)
+: Constant(sub, Vector(size, Real(1))) {}
+
+
+
+//==============================================================================
+// TIME :: IMPLEMENTATION
+//==============================================================================
+template <class T>
+class Measure_<T>::Time::Implementation {};
+
+template <>
+class Measure_<Real>::Time::Implementation
+: public Measure_<Real>::Implementation
+{
+public:
+ // We don't want the base class to allocate *any* cache entries.
+ Implementation() : Measure_<Real>::Implementation(0) {}
+
+ // Implementations of virtual methods.
+ // Measure_<Real> virtuals:
+ // No cached values.
+
+ const Real& getUncachedValueVirtual(const State& s, int derivOrder) const
+ OVERRIDE_11
+ { return derivOrder==0 ? s.getTime()
+ : (derivOrder==1 ? SimTK::One
+ : SimTK::Zero); }
+
+ // AbstractMeasure virtuals:
+ Implementation* cloneVirtual() const OVERRIDE_11
+ { return new Implementation(*this); }
+ Stage getDependsOnStageVirtual(int derivOrder) const OVERRIDE_11
+ { return derivOrder>0 ? Stage::Empty : Stage::Time; }
+
+ // Value is t, 1st derivative is 1, the rest are 0.
+ int getNumTimeDerivativesVirtual() const OVERRIDE_11
+ { return std::numeric_limits<int>::max(); }
+};
+
+
+
+//==============================================================================
+// VARIABLE :: IMPLEMENTATION
+//==============================================================================
+template <class T>
+class Measure_<T>::Variable::Implementation
+: public Measure_<T>::Implementation
+{
+public:
+ // We don't want the base class to allocate *any* cache entries;
+ // we'll use the variable as its own value and zeroes for all
+ // the derivatives.
+ Implementation()
+ : Measure_<T>::Implementation(0),
+ invalidatedStage(Stage::Empty) {}
+
+ Implementation(Stage invalidated, const T& defaultValue)
+ : Measure_<T>::Implementation(defaultValue, 0),
+ invalidatedStage(invalidated) {}
+
+ // Copy constructor should not copy the variable.
+ Implementation(const Implementation& source)
+ : Measure_<T>::Implementation(source.getDefaultValue(), 0),
+ invalidatedStage(source.invalidatedStage) {}
+
+ void setInvalidatedStage(Stage invalidates) {
+ invalidatedStage = invalidates;
+ this->invalidateTopologyCache();
+ }
+
+ Stage getInvalidatedStage() const {return invalidatedStage;}
+
+ /** Change the value of this %Measure in the given \a state. Invalidates
+ cache entries in that \a state for any stage at or above the "invalidates"
+ stage that was set when this %Measure was constructed. **/
+ void setValue(State& state, const T& value) const
+ { updVarValue(state) = value; }
+
+ // Implementations of virtual methods.
+ Implementation* cloneVirtual() const OVERRIDE_11
+ { return new Implementation(*this); }
+
+ int getNumTimeDerivativesVirtual() const OVERRIDE_11
+ { return std::numeric_limits<int>::max(); }
+
+ // Discrete variable is available after Model stage; but all its
+ // derivatives are zero so are always available.
+ Stage getDependsOnStageVirtual(int derivOrder) const OVERRIDE_11
+ { return derivOrder>0 ? Stage::Empty : Stage::Model;}
+
+ const T& getUncachedValueVirtual(const State& s, int derivOrder) const
+ OVERRIDE_11
+ { return derivOrder>0 ? this->getValueZero() : getVarValue(s); }
+
+ // No cached values.
+
+ void realizeMeasureTopologyVirtual(State& s) const OVERRIDE_11 {
+ discreteVarIndex = this->getSubsystem().allocateDiscreteVariable
+ (s, invalidatedStage, new Value<T>(this->getDefaultValue()));
+ }
+private:
+ const T& getVarValue(const State& s) const {
+ assert(discreteVarIndex.isValid());
+ return Value<T>::downcast(
+ this->getSubsystem().getDiscreteVariable(s, discreteVarIndex));
+ }
+ T& updVarValue(State& s) const {
+ assert(discreteVarIndex.isValid());
+ return Value<T>::downcast(
+ this->getSubsystem().updDiscreteVariable(s, discreteVarIndex));
+ }
+
+ // TOPOLOGY STATE
+ Stage invalidatedStage; // TODO this shouldn't be needed
+
+ // TOPOLOGY CACHE
+ mutable DiscreteVariableIndex discreteVarIndex;
+};
+
+
+
+//==============================================================================
+// RESULT :: IMPLEMENTATION
+//==============================================================================
+template <class T>
+class Measure_<T>::Result::Implementation
+: public Measure_<T>::Implementation
+{
+public:
+ // We want the base class to allocate a single cache entry of type T.
+ Implementation()
+ : Measure_<T>::Implementation(1),
+ dependsOnStage(Stage::Topology), invalidatedStage(Stage::Infinity) {}
+
+ Implementation(Stage dependsOn, Stage invalidated)
+ : Measure_<T>::Implementation(1),
+ dependsOnStage(dependsOn==Stage::Empty ? Stage::Topology : dependsOn),
+ invalidatedStage(invalidated)
+ { SimTK_ERRCHK2_ALWAYS(invalidated > dependsOn,"Measure::Result::ctor()",
+ "Got invalidated stage %s and dependsOn stage %s which is illegal "
+ "because the invalidated stage must be later than dependsOn.",
+ invalidated.getName().c_str(), dependsOn.getName().c_str());
+ }
+
+ // Copy constructor will not copy the cache entry index.
+ Implementation(const Implementation& source)
+ : Measure_<T>::Implementation(source),
+ dependsOnStage(source.dependsOnStage),
+ invalidatedStage(source.invalidatedStage) {}
+
+ void setDependsOnStage(Stage dependsOn) {
+ if (dependsOn == Stage::Empty) dependsOn = Stage::Topology;
+ SimTK_ERRCHK2_ALWAYS(dependsOn < getInvalidatedStage(),
+ "Measure::Result::setDependsOnStage()",
+ "The provided dependsOn stage %s is illegal because it is not "
+ "less than the current invalidated stage %s. Change the "
+ "invalidated stage first with setInvalidatedStage().",
+ dependsOn.getName().c_str(),
+ getInvalidatedStage().getName().c_str());
+
+ dependsOnStage = dependsOn;
+ this->invalidateTopologyCache();
+ }
+
+ void setInvalidatedStage(Stage invalidated) {
+ SimTK_ERRCHK2_ALWAYS(invalidated > getDependsOnStage(),
+ "Measure::Result::setInvalidatedStage()",
+ "The provided invalidated stage %s is illegal because it is not "
+ "greater than the current dependsOn stage %s. Change the "
+ "dependsOn stage first with setDependsOnStage().",
+ invalidated.getName().c_str(),
+ getDependsOnStage().getName().c_str());
+
+ invalidatedStage = invalidated;
+ this->invalidateTopologyCache();
+ }
+
+
+ Stage getDependsOnStage() const {return dependsOnStage;}
+ Stage getInvalidatedStage() const {return invalidatedStage;}
+
+
+ void markAsValid(const State& state) const
+ { const Stage subsystemStage = this->getSubsystem().getStage(state);
+ SimTK_ERRCHK3_ALWAYS(subsystemStage >= getDependsOnStage().prev(),
+ "Measure::Result::markAsValid()",
+ "This Result Measure cannot be marked valid in a State where this "
+ "measure's Subsystem has been realized only to stage %s, because "
+ "its value was declared to depend on stage %s. To mark it valid, "
+ "we require that the State have been realized at least to the "
+ "previous stage (%s in this case); that is, you must at least be "
+ "*working on* the dependsOn stage in order to claim this result is "
+ "available.",
+ subsystemStage.getName().c_str(),
+ getDependsOnStage().getName().c_str(),
+ getDependsOnStage().prev().getName().c_str());
+ this->markCacheValueRealized(state, 0); }
+
+ bool isValid(const State& state) const
+ { return this->isCacheValueRealized(state, 0); }
+
+ void markAsNotValid(const State& state) const
+ { this->markCacheValueNotRealized(state, 0);
+ state.invalidateAllCacheAtOrAbove(invalidatedStage); }
+
+ T& updValue(const State& state) const
+ { markAsNotValid(state); return this->updCacheEntry(state, 0); }
+
+
+ // Implementations of virtual methods.
+ Implementation* cloneVirtual() const OVERRIDE_11
+ { return new Implementation(*this); }
+
+ int getNumTimeDerivativesVirtual() const OVERRIDE_11 {return 0;}
+
+ /** Cache value is available after its "depends on" stage has been
+ realized; but all its derivatives are zero so are always available. **/
+ Stage getDependsOnStageVirtual(int derivOrder) const OVERRIDE_11
+ { return derivOrder>0 ? Stage::Empty : dependsOnStage;}
+
+ void calcCachedValueVirtual(const State&, int derivOrder, T& value) const
+ OVERRIDE_11
+ { SimTK_ERRCHK_ALWAYS(!"calcCachedValueVirtual() implemented",
+ "Measure_<T>::Result::getValue()",
+ "Measure_<T>::Result::getValue() was called when the value was not "
+ "yet valid. For most Measure types, this would have initiated "
+ "computation of the value, but Result measures must have their values "
+ "calculated and set externally, and then marked valid."); }
+
+private:
+ // TOPOLOGY STATE
+ Stage dependsOnStage;
+ Stage invalidatedStage;
+};
+
+
+
+//==============================================================================
+// SINUSOID :: IMPLEMENTATION
+//==============================================================================
+template <class T>
+class Measure_<T>::Sinusoid::Implementation
+: public Measure_<T>::Implementation
+{
+ static const int NumDerivs = 3;
+public:
+ Implementation()
+ : Measure_<T>::Implementation(NumDerivs+1),
+ a(CNT<T>::getNaN()), w(CNT<T>::getNaN()), p(CNT<T>::getNaN()) {}
+
+ Implementation(const T& amplitude,
+ const T& frequency,
+ const T& phase=T(0))
+ : Measure_<T>::Implementation(NumDerivs+1),
+ a(amplitude), w(frequency), p(phase) {}
+
+ // Default copy constructor is fine.
+
+ // Implementations of virtual methods.
+ Implementation* cloneVirtual() const OVERRIDE_11
+ { return new Implementation(*this); }
+
+ int getNumTimeDerivativesVirtual() const OVERRIDE_11 {return NumDerivs;}
+
+ Stage getDependsOnStageVirtual(int order) const OVERRIDE_11
+ { return Stage::Time; }
+
+ void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
+ OVERRIDE_11
+ {
+ // We need to allow the compiler to select std::sin or SimTK::sin
+ // based on the argument type.
+ using std::sin; using std::cos;
+
+ assert(NumDerivs == 3);
+ const Real t = s.getTime();
+ const T arg = w*t + p;
+
+ switch (derivOrder) {
+ case 0: value = a*sin(arg); break;
+ case 1: value = w*a*cos(arg); break;
+ case 2: value = -w*w*a*sin(arg); break;
+ case 3: value = -w*w*w*a*cos(arg); break;
+ default: SimTK_ASSERT1_ALWAYS(!"out of range",
+ "Measure::Sinusoid::Implementation::calcCachedValueVirtual():"
+ " derivOrder %d is out of range 0-3.", derivOrder);
+ }
+ }
+
+ // There are no uncached values.
+
+private:
+ // TOPOLOGY STATE
+ T a, w, p;
+
+ // TOPOLOGY CACHE
+ // nothing
+};
+
+
+
+//==============================================================================
+// PLUS :: IMPLEMENTATION
+//==============================================================================
+template <class T>
+class Measure_<T>::Plus::Implementation : public Measure_<T>::Implementation {
+public:
+ // TODO: Currently allocates just one cache entry.
+ // left and right will be empty handles.
+ Implementation() {}
+
+ Implementation(const Measure_<T>& left,
+ const Measure_<T>& right)
+ : left(left), right(right) {}
+
+ // Default copy constructor gives us a new Implementation object,
+ // but with references to the *same* operand measures.
+
+ // Implementations of virtual methods.
+
+ // This uses the default copy constructor.
+ Implementation* cloneVirtual() const OVERRIDE_11
+ { return new Implementation(*this); }
+
+ // TODO: Let this be settable up to the min number of derivatives
+ // provided by the arguments.
+ int getNumTimeDerivativesVirtual() const OVERRIDE_11 {return 0;}
+ //{ return std::min(left.getNumTimeDerivatives(),
+ // right.getNumTimeDerivatives()); }
+
+ Stage getDependsOnStageVirtual(int order) const OVERRIDE_11
+ { return Stage(std::max(left.getDependsOnStage(order),
+ right.getDependsOnStage(order))); }
+
+
+ void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
+ OVERRIDE_11
+ {
+ value = left.getValue(s,derivOrder) + right.getValue(s,derivOrder);
+ }
+
+ // There are no uncached values.
+
+private:
+ // TOPOLOGY STATE
+ Measure_<T> left;
+ Measure_<T> right;
+
+ // TOPOLOGY CACHE
+ // nothing
+};
+
+
+
+//==============================================================================
+// MINUS :: IMPLEMENTATION
+//==============================================================================
+template <class T>
+class Measure_<T>::Minus::Implementation : public Measure_<T>::Implementation {
+public:
+ // TODO: Currently allocates just one cache entry.
+ // left and right will be empty handles.
+ Implementation() {}
+
+ Implementation(const Measure_<T>& left,
+ const Measure_<T>& right)
+ : left(left), right(right) {}
+
+ // Default copy constructor gives us a new Implementation object,
+ // but with references to the *same* operand measures.
+
+ // Implementations of virtual methods.
+
+ // This uses the default copy constructor.
+ Implementation* cloneVirtual() const OVERRIDE_11
+ { return new Implementation(*this); }
+
+ // TODO: Let this be settable up to the min number of derivatives
+ // provided by the arguments.
+ int getNumTimeDerivativesVirtual() const OVERRIDE_11 {return 0;}
+ //{ return std::min(left.getNumTimeDerivatives(),
+ // right.getNumTimeDerivatives()); }
+
+ Stage getDependsOnStageVirtual(int order) const OVERRIDE_11
+ { return Stage(std::max(left.getDependsOnStage(order),
+ right.getDependsOnStage(order))); }
+
+
+ void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
+ OVERRIDE_11
+ {
+ value = left.getValue(s,derivOrder) - right.getValue(s,derivOrder);
+ }
+
+ // There are no uncached values.
+
+private:
+ // TOPOLOGY STATE
+ Measure_<T> left;
+ Measure_<T> right;
+
+ // TOPOLOGY CACHE
+ // nothing
+};
+
+
+
+//==============================================================================
+// SCALE :: IMPLEMENTATION
+//==============================================================================
+template <class T>
+class Measure_<T>::Scale::Implementation
+: public Measure_<T>::Implementation
+{
+public:
+ // TODO: Currently allocates just one cache entry.
+ // scale will be uninitialized, operand will be empty handle.
+ Implementation() : factor(NaN) {}
+
+ Implementation(Real factor, const Measure_<T>& operand)
+ : factor(factor), operand(operand) {}
+
+ // Default copy constructor gives us a new Implementation object,
+ // but with references to the *same* operand measure.
+
+ void setScaleFactor(Real sf) {
+ factor = sf;
+ this->invalidateTopologyCache();
+ }
+
+ const Measure_<T>& getOperandMeasure() const
+ {
+ return operand;
+ }
+
+ // Implementations of virtual methods.
+
+ // This uses the default copy constructor.
+ Implementation* cloneVirtual() const OVERRIDE_11
+ { return new Implementation(*this); }
+
+ // TODO: Let this be settable up to the min number of derivatives
+ // provided by the arguments.
+ int getNumTimeDerivativesVirtual() const OVERRIDE_11 {return 0;}
+ //{ return std::min(left.getNumTimeDerivatives(),
+ // right.getNumTimeDerivatives()); }
+
+ Stage getDependsOnStageVirtual(int order) const OVERRIDE_11
+ { return operand.getDependsOnStage(order); }
+
+
+ void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
+ OVERRIDE_11
+ {
+ value = factor * operand.getValue(s,derivOrder);
+ }
+
+ // There are no uncached values.
+
+private:
+ // TOPOLOGY STATE
+ Real factor;
+ Measure_<T> operand;
+
+ // TOPOLOGY CACHE
+ // nothing
+};
+
+
+
+//==============================================================================
+// INTEGRATE :: IMPLEMENTATION
+//==============================================================================
+/** The implementation for %Integrate measures allocates a continuous state
+variable or variables from the State's z pool and generates zdot values to be
+integrated into those z variables. The z's are then copied into a type T,
+Time-stage cache entry so that we can return the value as a type T reference.
+Derivative requests are passed through to the integrand so only one cache
+entry is required here. **/
+template <class T>
+class Measure_<T>::Integrate::Implementation
+: public Measure_<T>::Implementation {
+public:
+ /** The derivative and initialConditions Measures will be empty handles if
+ this is default constructed. **/
+ Implementation() : Measure_<T>::Implementation(1) {}
+
+ /** Here we're shallow-copying the Measure handles so we'll be referring to
+ the original Measures. **/
+ Implementation(const Measure_<T>& deriv, const Measure_<T>& ic,
+ const T& defaultValue)
+ : Measure_<T>::Implementation(defaultValue, 1),
+ derivMeasure(deriv), icMeasure(ic) {}
+
+ /** Copy constructor shallow-copies the referenced measures, but we don't
+ want to share our state variables. **/
+ Implementation(const Implementation& source)
+ : Measure_<T>::Implementation(source.getDefaultValue(), 1),
+ derivMeasure(source.derivMeasure), icMeasure(source.icMeasure) {}
+
+ /** Set the value of the state variables(s) that hold the integral. This
+ cannot be used to change the size if the type T is a Vector; the supplied
+ \a value must be the same length as the default value of this %Measure. **/
+ void setValue(State& s, const T& value) const
+ { assert(zIndex >= 0);
+ for (int i=0; i < this->size(); ++i)
+ this->getSubsystem().updZ(s)[zIndex+i] =
+ Measure_Num<T>::get(value, i); }
+
+ const Measure_<T>& getDerivativeMeasure() const
+ { SimTK_ERRCHK(!derivMeasure.isEmptyHandle(),
+ "Measure_<T>::Integrate::getDerivativeMeasure()",
+ "No derivative measure is available for this integrated measure.");
+ return derivMeasure; }
+
+ const Measure_<T>& getInitialConditionMeasure() const
+ { SimTK_ERRCHK(!icMeasure.isEmptyHandle(),
+ "Measure_<T>::Integrate::getInitialConditionMeasure()",
+ "No initial condition measure is available for this "
+ "integrated measure.");
+ return icMeasure; }
+
+ void setDerivativeMeasure(const Measure_<T>& d)
+ { derivMeasure = d; this->invalidateTopologyCache(); }
+ void setInitialConditionMeasure(const Measure_<T>& ic)
+ { icMeasure = ic; this->invalidateTopologyCache(); }
+
+ // Implementations of virtuals.
+
+ // This uses the copy constructor defined above.
+ Implementation* cloneVirtual() const OVERRIDE_11
+ { return new Implementation(*this); }
+
+ /** This measure has one more time derivative than the integrand. **/
+ int getNumTimeDerivativesVirtual() const OVERRIDE_11
+ { int integralDerivs = getDerivativeMeasure().getNumTimeDerivatives();
+ // Careful - can't add 1 to max int and stay an int.
+ if (integralDerivs < std::numeric_limits<int>::max())
+ ++integralDerivs;
+ return integralDerivs; }
+
+ void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
+ OVERRIDE_11
+ { assert(derivOrder == 0); // only one cache entry
+ assert(Measure_Num<T>::size(value) == this->size());
+ assert(zIndex.isValid());
+ const Vector& allZ = this->getSubsystem().getZ(s);
+ for (int i=0; i < this->size(); ++i)
+ Measure_Num<T>::upd(value,i) = allZ[zIndex+i];
+ }
+
+ const T& getUncachedValueVirtual(const State& s, int derivOrder) const
+ OVERRIDE_11
+ { assert(derivOrder > 0); // 0th entry is cached
+ return getDerivativeMeasure().getValue(s, derivOrder-1);
+ }
+
+ Stage getDependsOnStageVirtual(int derivOrder) const OVERRIDE_11
+ { return derivOrder>0
+ ? getDerivativeMeasure().getDependsOnStage(derivOrder-1)
+ : Stage::Time; }
+
+ /** Initialize the state to the current value of the initial condition
+ measure, if there is one, otherwise to the default value. **/
+ void initializeVirtual(State& s) const OVERRIDE_11 {
+ assert(zIndex.isValid());
+ Vector& allZ = this->getSubsystem().updZ(s);
+ if (!icMeasure.isEmptyHandle()) {
+ this->getSubsystem().getSystem()
+ .realize(s, icMeasure.getDependsOnStage());
+ const T& ic = icMeasure.getValue(s);
+ for (int i=0; i < this->size(); ++i)
+ allZ[zIndex+i] = Measure_Num<T>::get(ic,i);
+ } else {
+ for (int i=0; i < this->size(); ++i)
+ allZ[zIndex+i] = Measure_Num<T>::get(this->getDefaultValue(),i);
+ }
+ }
+
+ /** Allocate one Real continuous state variable z per element of this
+ %Measure's data type T, using the default value to determine
+ how many are needed (if that's not part of the type T), and initialize them
+ to the corresponding element from the default value. **/
+ void realizeMeasureTopologyVirtual(State& s) const OVERRIDE_11 {
+ Vector init(this->size());
+ for (int i=0; i < this->size(); ++i)
+ init[i] = Measure_Num<T>::get(this->getDefaultValue(),i);
+ zIndex = this->getSubsystem().allocateZ(s, init);
+ }
+
+ /** Set the zdots to the integrand (derivative measure) value. If no
+ integrand was provided it is treated as though it were zero. **/
+ void realizeMeasureAccelerationVirtual(const State& s) const OVERRIDE_11 {
+ assert(zIndex.isValid());
+ Vector& allZDot = this->getSubsystem().updZDot(s);
+ if (!derivMeasure.isEmptyHandle()) {
+ const T& deriv = derivMeasure.getValue(s);
+ for (int i=0; i < this->size(); ++i)
+ allZDot[zIndex+i] = Measure_Num<T>::get(deriv,i);
+ } else {
+ allZDot(zIndex,this->size()) = 0; // derivative is zero
+ }
+ }
+
+private:
+ // TOPOLOGY STATE
+ Measure_<T> derivMeasure; // just handles
+ Measure_<T> icMeasure;
+
+ // TOPOLOGY CACHE
+ mutable ZIndex zIndex; // This is the first index if more than one z.
+};
+
+
+
+//==============================================================================
+// DIFFERENTIATE :: IMPLEMENTATION
+//==============================================================================
+
+/** @cond **/ // Hide from Doxygen.
+// This helper class is the contents of the discrete state variable and
+// corresponding cache entry maintained by this measure. The variable is
+// auto-update, meaning the value of the cache entry replaces the state
+// variable at the start of each step.
+// TODO: This was a local class in Measure_<T>::Differentiate::Implementation
+// but VC++ 8 (2005) failed to properly instantiate the templatized operator<<()
+// in that case; doing it this way is a workaround.
+template <class T>
+class Measure_Differentiate_Result {
+public:
+ Measure_Differentiate_Result() : derivIsGood(false) {}
+ T operand; // previous value of operand
+ T operandDot; // previous value of derivative
+ bool derivIsGood; // do we think the deriv is a good one?
+};
+/** @endcond **/
+
+template <class T>
+class Measure_<T>::Differentiate::Implementation
+: public Measure_<T>::Implementation
+{
+ typedef Measure_Differentiate_Result<T> Result;
+public:
+ // Don't allocate any cache entries in the base class.
+ Implementation() : Measure_<T>::Implementation(0) {}
+
+ Implementation(const Measure_<T>& operand)
+ : Measure_<T>::Implementation(0),
+ operand(operand), forceUseApprox(false), isApproxInUse(false) {}
+
+ // Default copy constructor gives us a new Implementation object,
+ // but with reference to the *same* operand measure.
+
+ void setForceUseApproximation(bool mustApproximate) {
+ forceUseApprox = mustApproximate;
+ this->invalidateTopologyCache();
+ }
+
+ void setOperandMeasure(const Measure_<T>& operand) {
+ this->operand = operand;
+ this->invalidateTopologyCache();
+ }
+
+ bool getForceUseApproximation() const {return forceUseApprox;}
+ bool isUsingApproximation() const {return isApproxInUse;}
+ const Measure_<T>& getOperandMeasure() const {return operand;}
+
+ // Implementations of virtual methods.
+
+ // This uses the default copy constructor.
+ Implementation* cloneVirtual() const OVERRIDE_11
+ { return new Implementation(*this); }
+
+ // This has one fewer than the operand.
+ int getNumTimeDerivativesVirtual() const OVERRIDE_11
+ { if (!isApproxInUse) return operand.getNumTimeDerivatives()-1;
+ else return 0; }
+
+ Stage getDependsOnStageVirtual(int order) const OVERRIDE_11
+ { if (!isApproxInUse) return operand.getDependsOnStage(order+1);
+ else return operand.getDependsOnStage(order); }
+
+
+ // We're not using the Measure_<T> base class cache services, but
+ // we do have one of our own. It looks uncached from the base class
+ // point of view which is why we're implementing it here.
+ const T& getUncachedValueVirtual(const State& s, int derivOrder) const
+ OVERRIDE_11
+ { if (!isApproxInUse)
+ return operand.getValue(s, derivOrder+1);
+
+ ensureDerivativeIsRealized(s);
+ const Subsystem& subsys = this->getSubsystem();
+ const Result& result = Value<Result>::downcast
+ (subsys.getDiscreteVarUpdateValue(s,resultIx));
+ return result.operandDot; // has a value but might not be a good one
+ }
+
+ void initializeVirtual(State& s) const OVERRIDE_11 {
+ if (!isApproxInUse) return;
+
+ assert(resultIx.isValid());
+ const Subsystem& subsys = this->getSubsystem();
+ Result& result = Value<Result>::updDowncast
+ (subsys.updDiscreteVariable(s,resultIx));
+ this->getSubsystem().getSystem().realize(s,operand.getDependsOnStage());
+ result.operand = operand.getValue(s);
+ result.operandDot = this->getValueZero();
+ result.derivIsGood = false;
+ }
+
+ void realizeMeasureTopologyVirtual(State& s) const OVERRIDE_11 {
+ isApproxInUse = (forceUseApprox || operand.getNumTimeDerivatives()==0);
+ if (!isApproxInUse)
+ return;
+
+ resultIx = this->getSubsystem()
+ .allocateAutoUpdateDiscreteVariable(s, operand.getDependsOnStage(0),
+ new Value<Result>(), operand.getDependsOnStage(0));
+ }
+
+ /** In case no one has updated the value of this measure yet, we have
+ to make sure it gets updated before the integration moves ahead. **/
+ void realizeMeasureAccelerationVirtual(const State& s) const OVERRIDE_11 {
+ ensureDerivativeIsRealized(s);
+ }
+
+ void ensureDerivativeIsRealized(const State& s) const {
+ assert(resultIx.isValid());
+ const Subsystem& subsys = this->getSubsystem();
+ if (subsys.isDiscreteVarUpdateValueRealized(s,resultIx))
+ return;
+
+ const Real t0 = subsys.getDiscreteVarLastUpdateTime(s,resultIx);
+ const Result& prevResult = Value<Result>::downcast
+ (subsys.getDiscreteVariable(s,resultIx));
+ const T& f0 = prevResult.operand;
+ const T& fdot0 = prevResult.operandDot; // may be invalid
+ const bool good0 = prevResult.derivIsGood;
+
+ const Real t = s.getTime();
+ Result& result = Value<Result>::updDowncast
+ (subsys.updDiscreteVarUpdateValue(s,resultIx));
+ T& f = result.operand; // renaming
+ T& fdot = result.operandDot;
+ bool& good = result.derivIsGood;
+
+ f = operand.getValue(s);
+ good = false;
+ if (!isFinite(t0))
+ fdot = this->getValueZero();
+ else if (t == t0) {
+ fdot = fdot0;
+ good = good0;
+ } else {
+ fdot = (f-f0)/(t-t0); // 1st order
+ if (good0)
+ fdot = Real(2)*fdot - fdot0; // now 2nd order
+ good = true; // either 1st or 2nd order estimate
+ }
+ subsys.markDiscreteVarUpdateValueRealized(s,resultIx);
+ }
+private:
+ // TOPOLOGY STATE
+ Measure_<T> operand;
+ bool forceUseApprox;
+
+ // TOPOLOGY CACHE
+ mutable bool isApproxInUse;
+ mutable DiscreteVariableIndex resultIx; // auto-update
+};
+
+
+
+//==============================================================================
+// EXTREME :: IMPLEMENTATION
+//==============================================================================
+template <class T>
+class Measure_<T>::Extreme::Implementation : public Measure_<T>::Implementation
+{
+ typedef typename Measure_<T>::Extreme Extreme;
+ typedef typename Extreme::Operation Operation;
+public:
+ /** Default constructor leaves the operand measure unspecified; no base
+ class cache entries are allocated. **/
+ Implementation()
+ : Measure_<T>::Implementation(0), operation(Extreme::MaxAbs) {}
+
+ /** Construct a measure that returns the extreme value taken on by the
+ operand measure during a time stepping study. **/
+ Implementation(const Measure_<T>& operand, Operation op)
+ : Measure_<T>::Implementation(0), operand(operand), operation(op) {}
+
+ // Default copy constructor gives us a new Implementation object,
+ // but with reference to the *same* operand measure.
+
+ /** Set the operand measure for this %Extreme measure; this is a Topology
+ stage change so you'll have to call realizeTopology() again if you call
+ this. **/
+ void setOperandMeasure(const Measure_<T>& operand) {
+ this->operand = operand;
+ this->invalidateTopologyCache();
+ }
+
+ /** Set the particular operation to be performed by this %Extreme measure;
+ this is a Topology stage change so you'll have to call realizeTopology()
+ again if you call this. **/
+ void setOperation(Operation op) {
+ this->operation = op;
+ this->invalidateTopologyCache();
+ }
+
+ /** Return a reference to the operand measure for this %Extreme measure. **/
+ const Measure_<T>& getOperandMeasure() const {return operand;}
+
+ /** Return the particular operation being performed by this %Extreme
+ measure. **/
+ Operation getOperation() const {return operation;}
+
+ /** Set the current extreme value stored in this %Extreme measure's state
+ variable. **/
+ void setValue(State& s, const T& value) const {
+ assert(extremeIx.isValid());
+ const Subsystem& subsys = this->getSubsystem();
+ T& prevMin = Value<T>::updDowncast
+ (subsys.updDiscreteVariable(s,extremeIx));
+ prevMin = value;
+ }
+
+ /** Return the time at which the extreme was last updated. This will be
+ the current time if the operand is currently at its most extreme value,
+ otherwise it will be sometime in the past. **/
+ Real getTimeOfExtremeValue(const State& s) const {
+ const Subsystem& subsys = this->getSubsystem();
+ const bool hasNewExtreme = ensureExtremeHasBeenUpdated(s);
+ Real tUpdate;
+ if (hasNewExtreme)
+ tUpdate = s.getTime(); // i.e., now
+ else
+ tUpdate = subsys.getDiscreteVarLastUpdateTime(s,extremeIx);
+ return tUpdate;
+ }
+
+ // Implementations of virtual methods.
+
+ // This uses the default copy constructor.
+ Implementation* cloneVirtual() const OVERRIDE_11
+ { return new Implementation(*this); }
+
+ /** Extreme(f(t)) has the same number of derivatives as f except that
+ they are all zero unless f(t) is a new extreme. **/
+ int getNumTimeDerivativesVirtual() const OVERRIDE_11
+ { return operand.getNumTimeDerivatives(); }
+
+ /** The depends-on stage for this measure is the same as for its
+ operand. **/
+ Stage getDependsOnStageVirtual(int order) const OVERRIDE_11
+ { return operand.getDependsOnStage(order); }
+
+
+ /** We're not using the Measure_<T> base class cache services, but
+ we do have one of our own. It looks uncached from the base class
+ point of view which is why we're implementing it here. **/
+ const T& getUncachedValueVirtual(const State& s, int derivOrder) const
+ OVERRIDE_11
+ {
+ const Subsystem& subsys = this->getSubsystem();
+ const bool hasNewExtreme = ensureExtremeHasBeenUpdated(s);
+ if (derivOrder > 0) {
+ // TODO: should be handled elementwise and zero unless the
+ // derivative is acting in the direction that changes the
+ // extreme.
+ return hasNewExtreme ? operand.getValue(s, derivOrder)
+ : this->getValueZero();
+ }
+ if (hasNewExtreme) {
+ const T& newExt = Value<T>::downcast
+ (subsys.getDiscreteVarUpdateValue(s,extremeIx));
+ return newExt;
+ } else {
+ const T& currentExt = Value<T>::downcast
+ (subsys.getDiscreteVariable(s,extremeIx));
+ return currentExt;
+ }
+ }
+
+ /** At start of a time stepping study, this should be called to set the
+ current extreme value to the current value of the operand. **/
+ void initializeVirtual(State& s) const OVERRIDE_11 {
+ this->getSubsystem().getSystem().realize(s,operand.getDependsOnStage());
+ setValue(s, operand.getValue(s));
+ }
+
+ /** Allocate the auto-updated state variable that holds the extreme seen
+ so far. We'll assume that changes to this variable invalidate Dynamics
+ (force) stage so that any forces that depend on it will be recomputed if
+ it changes. Also allocate an auxiliary boolean variable that is used to hold
+ whether the current value is a new extreme; that is private to the
+ implementation and not user-accessible since you can instead check the
+ time of last update. **/
+ void realizeMeasureTopologyVirtual(State& s) const OVERRIDE_11 {
+ // TODO: this should be NaN once initialization is working properly.
+ T initVal = this->getDefaultValue();
+ switch(operation) {
+ case Minimum: initVal = Infinity; break;
+ case Maximum: initVal = -Infinity; break;
+ case MinAbs: initVal = Infinity; break;
+ case MaxAbs: initVal = 0; break;
+ };
+
+ extremeIx = this->getSubsystem()
+ .allocateAutoUpdateDiscreteVariable(s, Stage::Dynamics,
+ new Value<T>(initVal), operand.getDependsOnStage(0));
+
+ isNewExtremeIx = this->getSubsystem()
+ .allocateAutoUpdateDiscreteVariable(s, Stage::Dynamics,
+ new Value<bool>(false), operand.getDependsOnStage(0));
+ }
+
+ /** In case no one has updated the value of this measure yet, we have
+ to make sure it gets updated before the integration moves ahead. **/
+ void realizeMeasureAccelerationVirtual(const State& s) const OVERRIDE_11 {
+ ensureExtremeHasBeenUpdated(s);
+ }
+
+ /** Here we make sure that the cache entry is updated if the current value
+ of the operand is more extreme than the previous one, and return a bool
+ indicating whether we have a new extreme. We don't want to create an update
+ entry unless the extreme value has changed, because we would like the state
+ variable's last update value to reflect the last actual change. **/
+ bool ensureExtremeHasBeenUpdated(const State& s) const {
+ assert(extremeIx.isValid() && isNewExtremeIx.isValid());
+ const Subsystem& subsys = this->getSubsystem();
+
+ // We may have already determined whether we're at a new extreme in
+ // which case we don't need to do it again.
+ if (subsys.isDiscreteVarUpdateValueRealized(s, isNewExtremeIx))
+ return Value<bool>::downcast
+ (subsys.getDiscreteVarUpdateValue(s,isNewExtremeIx));
+
+ // We're going to have to decide if we're at a new extreme, and if
+ // so record the new extreme value in the auto-update cache entry of
+ // the extreme value state variable.
+
+ // Get the previous extreme value and the current operand value.
+ const T& prevExtreme = Value<T>::downcast
+ (subsys.getDiscreteVariable(s,extremeIx));
+ const T& currentVal = operand.getValue(s);
+
+ // Search to see if any element has reached a new extreme.
+ bool foundNewExt = false;
+ for (int i=0; i < this->size() && !foundNewExt; ++i)
+ foundNewExt = isNewExtreme(Measure_Num<T>::get(currentVal,i),
+ Measure_Num<T>::get(prevExtreme,i));
+
+ // Record the result and mark the auto-update cache entry valid
+ // so we won't have to recalculate. When the integrator advances to the
+ // next step this cache entry will be swapped with the corresponding
+ // state and marked invalid so we'll be sure to recalculate each step.
+ Value<bool>::updDowncast
+ (subsys.updDiscreteVarUpdateValue(s,isNewExtremeIx)) = foundNewExt;
+ subsys.markDiscreteVarUpdateValueRealized(s,isNewExtremeIx);
+
+ // Don't update the auto-update cache entry if we didn't see a new
+ // extreme. That way no auto-update will occur and the state variable
+ // will remain unchanged with the existing update time preserved.
+ if (!foundNewExt)
+ return false;
+
+ // We have encountered a new extreme. We'll record the new extreme
+ // in the auto-update cache entry which will be used as the current
+ // result until the integrator advances to the next step at which time
+ // this will be swapped with the state variable to serve as the previous
+ // extreme value until a further extreme is encountered.
+ T& newExtreme = Value<T>::updDowncast
+ (subsys.updDiscreteVarUpdateValue(s,extremeIx));
+
+ for (int i=0; i < this->size(); ++i)
+ Measure_Num<T>::upd(newExtreme,i) =
+ extremeOf(Measure_Num<T>::get(currentVal,i),
+ Measure_Num<T>::get(prevExtreme,i));
+
+ // Marking this valid is what ensures that an auto-update occurs later.
+ subsys.markDiscreteVarUpdateValueRealized(s,extremeIx);
+ return true;
+ }
+private:
+ // Return true if newVal is "more extreme" than oldExtreme, according
+ // to the operation we're performing.
+ bool isNewExtreme(const typename Measure_Num<T>::Element& newVal,
+ const typename Measure_Num<T>::Element& oldExtreme) const
+ {
+ switch (operation) {
+ case Extreme::Maximum: return newVal > oldExtreme;
+ case Extreme::Minimum: return newVal < oldExtreme;
+ case Extreme::MaxAbs: return std::abs(newVal) > std::abs(oldExtreme);
+ case Extreme::MinAbs: return std::abs(newVal) < std::abs(oldExtreme);
+ };
+ SimTK_ASSERT1_ALWAYS(!"recognized",
+ "Measure::Extreme::Implementation::isNewExtreme(): "
+ "unrecognized operation %d", (int)operation);
+ return false; /*NOTREACHED*/
+ }
+
+ // Given the value of one element of the operand, and that value's time
+ // derivative, determine whether the derivative is moving the element
+ // into the "more extreme" direction, according to the operation.
+ bool isExtremeDir(const typename Measure_Num<T>::Element& value,
+ const typename Measure_Num<T>::Element& deriv) const
+ {
+ const int sv = sign(value), sd = sign(deriv);
+ if (sd == 0) return false; // derivative is zero; not changing
+ switch (operation) {
+ case Extreme::Maximum: return sd == 1; // getting larger
+ case Extreme::Minimum: return sd == -1; // getting smaller
+ case Extreme::MaxAbs: return sv==0 || sd==sv; // abs is growing
+ case Extreme::MinAbs: return sd == -sv;
+ };
+ SimTK_ASSERT1_ALWAYS(!"recognized",
+ "Measure::Extreme::Implementation::isExtremeDir(): "
+ "unrecognized operation %d", (int)operation);
+ return false; /*NOTREACHED*/
+ }
+
+ typename Measure_Num<T>::Element
+ extremeOf(const typename Measure_Num<T>::Element& newVal,
+ const typename Measure_Num<T>::Element& oldExtreme) const
+ {
+ return isNewExtreme(newVal,oldExtreme) ? newVal : oldExtreme;
+ }
+
+ // TOPOLOGY STATE
+ Measure_<T> operand;
+ Operation operation;
+
+ // TOPOLOGY CACHE
+ mutable DiscreteVariableIndex extremeIx; // extreme so far; auto-update
+
+ // This auto-update flag records whether the current value is a new
+ // extreme. We don't really need to save it as a state variable since you
+ // can figure this out from the timestamp, but we need to to get invalidated
+ // by the auto-update swap so that we'll figure it out anew each step.
+ mutable DiscreteVariableIndex isNewExtremeIx;
+};
+
+
+
+//==============================================================================
+// DELAY :: IMPLEMENTATION
+//==============================================================================
+/** @cond **/ // Hide from Doxygen.
+// This helper class is the contents of the discrete state variable and
+// corresponding cache entry maintained by this measure. The variable is
+// auto-update, meaning the value of the cache entry replaces the state
+// variable at the start of each step.
+//
+// Circular buffers look like this:
+//
+// oldest=0, n=0
+// v
+// Empty buffer: | available |
+//
+// By convention, oldest=0 whenever the buffer is empty.
+//
+//
+// oldest next=(oldest+n)%capacity
+// v v
+// | available | | | | | | | available |
+// ^ n=6 ^
+// 0 capacity
+// v v
+// or | | | | | | available | | | | | | | | n=12
+// ^ ^
+// next oldest
+// = (oldest+n)%capacity
+//
+// Number of entries = n (called size() below)
+// Empty = n==0
+// Full = n==capacity()
+// Next available = (oldest+n)%capacity()
+template <class T>
+class Measure_Delay_Buffer {
+public:
+ explicit Measure_Delay_Buffer() {initDataMembers();}
+ void clear() {initDataMembers();}
+ int size() const {return m_size;} // # saved entries, *not* size of arrays
+ int capacity() const {return m_times.size();}
+ bool empty() const {return size()==0;}
+ bool full() const {return size()==capacity();}
+
+ double getEntryTime(int i) const
+ { assert(i < size()); return m_times[getArrayIndex(i)];}
+ const T& getEntryValue(int i) const
+ { assert(i < size()); return m_values[getArrayIndex(i)];}
+
+ enum {
+ InitialAllocation = 8, // smallest allocation
+ GrowthFactor = 2, // how fast to grow (double)
+ MaxShrinkProofSize = 16, // won't shrink unless bigger
+ TooBigFactor = 5 // 5X too much->maybe shrink
+ };
+
+ // Add a new entry to the end of the list, throwing out old entries that
+ // aren't needed to answer requests at tEarliest or later.
+ void append(double tEarliest, double tNow, const T& valueNow) {
+ forgetEntriesMuchOlderThan(tEarliest);
+ removeEntriesLaterOrEq(tNow);
+ if (full())
+ makeMoreRoom();
+ else if (capacity() > std::max((int)MaxShrinkProofSize,
+ (int)TooBigFactor * (size()+1)))
+ makeLessRoom(); // less than 1/TooBigFactor full
+ const int nextFree = getArrayIndex(m_size++);
+ m_times[nextFree] = tNow;
+ m_values[nextFree] = valueNow;
+ m_maxSize = std::max(m_maxSize, size());
+ }
+
+ // Prepend an older entry to the beginning of the list. No cleanup is done.
+ void prepend(double tNewOldest, const T& value) {
+ assert(empty() || tNewOldest < m_times[m_oldest]);
+ if (full()) makeMoreRoom();
+ m_oldest = empty() ? 0 : getArrayIndex(-1);
+ m_times[m_oldest] = tNewOldest;
+ m_values[m_oldest] = value;
+ ++m_size;
+ m_maxSize = std::max(m_maxSize, size());
+ }
+
+ // This is a specialized copy assignment for copying an old buffer
+ // to a new one with updated contents. We are told the earliest time we'll
+ // be asked about from now on, and won't copy any entries older than those
+ // needed to answer that earliest request. We won't copy anything at or
+ // newer than tNow, and finally we'll push (tNow,valueNow) as the newest
+ // entry.
+ void copyInAndUpdate(const Measure_Delay_Buffer& oldBuf, double tEarliest,
+ double tNow, const T& valueNow) {
+ // clear all current entries (no heap activity)
+ m_oldest = m_size = 0;
+
+ // determine how may old entries we have to keep
+ int firstNeeded = oldBuf.countNumUnneededOldEntries(tEarliest);
+ int lastNeeded = oldBuf.findLastEarlier(tNow); // might be -1
+ int numOldEntriesToKeep = lastNeeded-firstNeeded+1;
+ int newSize = numOldEntriesToKeep+1; // includes the new one
+
+ int newSizeRequest = -1;
+ if (capacity() < newSize) {
+ newSizeRequest = std::max((int)InitialAllocation,
+ (int)GrowthFactor * newSize);
+ ++m_nGrows;
+ } else if (capacity() > std::max((int)MaxShrinkProofSize,
+ (int)TooBigFactor * newSize)) {
+ newSizeRequest = std::max((int)MaxShrinkProofSize,
+ (int)GrowthFactor * newSize);
+ ++m_nShrinks;
+ }
+
+ // Reallocate space if advisable.
+ if (newSizeRequest != -1) {
+ const double dNaN = NTraits<double>::getNaN();
+ m_values.resize(newSizeRequest);
+ if (m_values.capacity() > m_values.size())
+ m_values.resize(m_values.capacity()); // don't waste any
+ m_times.resize(m_values.size(), dNaN);
+ }
+
+ m_maxCapacity = std::max(m_maxCapacity, capacity());
+
+ // Copy the entries we need to keep.
+ int nxt = 0;
+ for (int i=firstNeeded; i<=lastNeeded; ++i, ++nxt) {
+ m_times[nxt] = oldBuf.getEntryTime(i);
+ m_values[nxt] = oldBuf.getEntryValue(i);
+ }
+ // Now add the newest entry and set the size.
+ m_times[nxt] = tNow;
+ m_values[nxt] = valueNow;
+ assert(nxt+1==newSize);
+ m_size = nxt+1;
+ m_maxSize = std::max(m_maxSize, size());
+ }
+
+ // Given the current time and value and the earlier time at which the
+ // value is needed, use the buffer and (if necessary) the current value
+ // to estimate the delayed value.
+ T calcValueAtTime(double tDelay, double tNow, const T& valueNow) const;
+
+ // Given the current time but *not* the current value of the source measure,
+ // provide an estimate for the value at tDelay=tNow-delay using only the
+ // buffer contents and linear interpolation or extrapolation.
+ void calcValueAtTimeLinearOnly(double tDelay, T& delayedValue) const {
+ if (empty()) {
+ // Nothing in the buffer?? Shouldn't happen. Return empty Vector
+ // or NaN for fixed-size types.
+ Measure_Num<T>::makeNaNLike(T(), delayedValue);
+ return;
+ }
+
+ int firstLater = findFirstLaterOrEq(tDelay);
+
+ if (firstLater > 0) {
+ // Normal case: tDelay is between two buffer entries.
+ int firstEarlier = firstLater-1;
+ double t0=getEntryTime(firstEarlier), t1=getEntryTime(firstLater);
+ const T& v0=getEntryValue(firstEarlier);
+ const T& v1=getEntryValue(firstLater);
+ Real fraction = Real((tDelay-t0)/(t1-t0));
+ delayedValue = T(v0 + fraction*(v1-v0));
+ return;
+ }
+
+ if (firstLater==0) {
+ // Startup case: tDelay is at or before the oldest buffer entry.
+ // Assume the value was flat before that.
+ delayedValue = getEntryValue(firstLater);
+ return;
+ }
+
+ // tDelay is later than the latest entry in the buffer. We are going
+ // to have to extrapolate (yuck).
+
+ if (size() == 1) {
+ // Just one entry; we'll have to assume the value is flat.
+ delayedValue = getEntryValue(0);
+ return;
+ }
+
+ // Extrapolate using the last two entries.
+ double t0=getEntryTime(size()-2), t1=getEntryTime(size()-1);
+ const T& v0=getEntryValue(size()-2);
+ const T& v1=getEntryValue(size()-1);
+ Real fraction = Real((tDelay-t0)/(t1-t0)); // > 1
+ assert(fraction > 1.0);
+ delayedValue = T(v0 + fraction*(v1-v0)); // Extrapolate.
+ }
+
+ // Return the number of times we had to grow the buffer.
+ int getNumGrows() const {return m_nGrows;}
+ // Return the number of times we decided the buffer was so overallocated
+ // that we had to shrink it.
+ int getNumShrinks() const {return m_nShrinks;}
+ // Return the largest number of values we ever had in the buffer.
+ int getMaxSize() const {return m_maxSize;}
+ // Return the largest capacity the buffer ever had.
+ int getMaxCapacity() const {return m_maxCapacity;}
+
+private:
+ // Return the i'th oldest entry
+ // (0 -> oldest, size-1 -> newest, size -> first free, -1 -> last free)
+ int getArrayIndex(int i) const
+ { assert(-1<=i && i<=size());
+ const int rawIndex = m_oldest + i;
+ if (rawIndex < 0) return rawIndex + capacity();
+ else return rawIndex % capacity(); }
+
+ // Remove all but two entries older than the given time.
+ void forgetEntriesMuchOlderThan(double tEarliest) {
+ const int numToRemove = countNumUnneededOldEntries(tEarliest);
+ if (numToRemove) {
+ m_oldest = getArrayIndex(numToRemove);
+ m_size -= numToRemove;
+ }
+ }
+
+ // Count up how many old entries at the beginning of the buffer are so old
+ // that they wouldn't be needed to respond to a request at time tEarliest or
+ // later. We'll keep no more than two entries earlier than tEarliest.
+ int countNumUnneededOldEntries(double tEarliest) const {
+ const int firstLater = findFirstLaterOrEq(tEarliest);
+ return std::max(0, firstLater-2);
+ }
+
+ // Given the time now, delete anything at the end of the queue that is
+ // at that same time or later.
+ void removeEntriesLaterOrEq(double t) {
+ int lastEarlier = findLastEarlier(t);
+ m_size = lastEarlier+1;
+ if (m_size==0) m_oldest=0; // restart at beginning of array
+ }
+
+ // Return the entry number (0..size-1) of the first entry whose time
+ // is >= the given time, or -1 if there is none such.
+ int findFirstLaterOrEq(double tDelay) const {
+ for (int i=0; i < size(); ++i)
+ if (getEntryTime(i) >= tDelay)
+ return i;
+ return -1;
+ }
+
+ // Return the entry number(size-1..0) of the last entry whose time
+ // is < the given time, or -1 if there is none such.
+ int findLastEarlier(double t) const {
+ for (int i=size()-1; i>=0; --i)
+ if (getEntryTime(i) < t)
+ return i;
+ return -1;
+ }
+
+ // We don't have enough space. This is either the initial allocation or
+ // we need to double the current space.
+ void makeMoreRoom() {
+ const int newSizeRequest = std::max((int)InitialAllocation,
+ (int)GrowthFactor * size());
+ resize(newSizeRequest);
+ ++m_nGrows;
+ m_maxCapacity = std::max(m_maxCapacity, capacity());
+ }
+
+ // We are wasting a lot of space, reduce the heap allocation to just
+ // double what we're using now.
+ void makeLessRoom() {
+ const int targetMaxSize = std::max((int)MaxShrinkProofSize,
+ (int)GrowthFactor * size());
+ if (capacity() > targetMaxSize) {
+ resize(targetMaxSize);
+ ++m_nShrinks;
+ }
+ }
+
+ // Reallocate memory to get more space or stop wasting space. The new
+ // size request must be big enough to hold all the current contents. The
+ // amount we actually get may be somewhat larger than the request. On
+ // return, the times and values arrays will have been resized and the
+ // oldest entry will now be entry 0.
+ void resize(int newSizeRequest) {
+ assert(newSizeRequest >= size());
+ const double dNaN = NTraits<double>::getNaN();
+ Array_<T,int> newValues(newSizeRequest);
+ if (newValues.capacity() > newValues.size())
+ newValues.resize(newValues.capacity()); // don't waste any
+ Array_<double,int> newTimes(newValues.size(), dNaN);
+
+ // Pack existing values into start of new arrays.
+ for (int i=0; i < size(); ++i) {
+ const int ix = getArrayIndex(i);
+ newTimes[i] = m_times[ix];
+ newValues[i] = m_values[ix];
+ }
+ m_times.swap(newTimes); // switch heap space
+ m_values.swap(newValues);
+ m_oldest = 0; // starts at the beginning now; size unchanged
+ }
+
+ // Initialize everything to its default-constructed state.
+ void initDataMembers() {
+ m_times.clear(); m_values.clear();
+ m_oldest=m_size=0;
+ m_nGrows=m_nShrinks=m_maxSize=m_maxCapacity=0;
+ }
+
+ // These are circular buffers of the same size.
+ Array_<double,int> m_times;
+ Array_<T,int> m_values;
+ int m_oldest; // Array index of oldest (time,value)
+ int m_size; // number of entries in use
+
+ // Statistics.
+ int m_nGrows, m_nShrinks, m_maxSize, m_maxCapacity;
+};
+/** @endcond **/
+
+template <class T>
+class Measure_<T>::Delay::Implementation: public Measure_<T>::Implementation {
+ typedef Measure_Delay_Buffer<T> Buffer;
+public:
+ // Allocate one cache entry in the base class for the value; we allocate
+ // a specialized one for the buffer.
+ Implementation()
+ : Measure_<T>::Implementation(1), m_delay(NaN),
+ m_canUseCurrentValue(false), m_useLinearInterpolationOnly(false) {}
+
+ Implementation(const Measure_<T>& source, Real delay)
+ : Measure_<T>::Implementation(1), m_source(source), m_delay(delay),
+ m_canUseCurrentValue(false), m_useLinearInterpolationOnly(false) {}
+
+ // Default copy constructor gives us a new Implementation object,
+ // but with reference to the *same* source measure.
+
+ void setSourceMeasure(const Measure_<T>& source) {
+ if (!source.isSameMeasure(this->m_source)) {
+ this->m_source = source;
+ this->invalidateTopologyCache();
+ }
+ }
+
+ void setDelay(Real delay) {
+ if (delay != this->m_delay) {
+ this->m_delay = delay;
+ this->invalidateTopologyCache();
+ }
+ }
+
+ void setUseLinearInterpolationOnly(bool linearOnly) {
+ if (linearOnly != this->m_useLinearInterpolationOnly) {
+ this->m_useLinearInterpolationOnly = linearOnly;
+ this->invalidateTopologyCache();
+ }
+ }
+
+ void setCanUseCurrentValue(bool canUseCurrentValue) {
+ if (canUseCurrentValue != this->m_canUseCurrentValue) {
+ this->m_canUseCurrentValue = canUseCurrentValue;
+ this->invalidateTopologyCache();
+ }
+ }
+
+ const Measure_<T>& getSourceMeasure() const {return this->m_source;}
+ Real getDelay() const {return this->m_delay;}
+ bool getUseLinearInterpolationOnly() const
+ { return this->m_useLinearInterpolationOnly; }
+ bool getCanUseCurrentValue() const
+ { return this->m_canUseCurrentValue; }
+
+
+ // Implementations of virtual methods.
+
+ // This uses the default copy constructor.
+ Implementation* cloneVirtual() const OVERRIDE_11
+ { return new Implementation(*this); }
+
+ // Currently no derivative supported.
+ int getNumTimeDerivativesVirtual() const OVERRIDE_11
+ { return 0; }
+
+ // If we are allowed to use the current value of the source measure to
+ // determine the delayed value, the depends-on stage here is the same as
+ // for the source; otherwise it is Stage::Time.
+ Stage getDependsOnStageVirtual(int order) const OVERRIDE_11
+ { return this->m_canUseCurrentValue ? m_source.getDependsOnStage(order)
+ : Stage::Time; }
+
+ // Calculate the delayed value and return it to the Measure base class to
+ // be put in a cache entry.
+ void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
+ OVERRIDE_11
+ { const Subsystem& subsys = this->getSubsystem();
+ const Buffer& buffer = Value<Buffer>::downcast
+ (subsys.getDiscreteVariable(s,m_bufferIx));
+ //TODO: use cubic interpolation if allowed
+ buffer.calcValueAtTimeLinearOnly(s.getTime()-m_delay, value);
+ }
+
+ void initializeVirtual(State& s) const OVERRIDE_11 {
+ assert(m_bufferIx.isValid());
+ const Subsystem& subsys = this->getSubsystem();
+ Buffer& buffer = Value<Buffer>::updDowncast
+ (subsys.updDiscreteVariable(s,m_bufferIx));
+ buffer.clear();
+ this->getSubsystem().getSystem().realize(s,m_source.getDependsOnStage());
+ buffer.append(s.getTime()-m_delay, s.getTime(), m_source.getValue(s));
+ }
+
+ void realizeMeasureTopologyVirtual(State& s) const OVERRIDE_11 {
+ m_bufferIx = this->getSubsystem()
+ .allocateAutoUpdateDiscreteVariable(s, Stage::Report,
+ new Value<Buffer>(), getDependsOnStageVirtual(0));
+ }
+
+ /** In case no one has updated the value of this measure yet, we have
+ to make sure it gets updated before the integration moves ahead. **/
+ void realizeMeasureAccelerationVirtual(const State& s) const OVERRIDE_11 {
+ updateBuffer(s);
+ }
+
+ // This uses the buffer from the state to update the one in the
+ // corresponding cache entry. The update adds the current value of the
+ // source to the end of the buffer and tosses out unneeded old entries.
+ void updateBuffer(const State& s) const {
+ assert(m_bufferIx.isValid());
+ const Subsystem& subsys = this->getSubsystem();
+
+ const Buffer& prevBuffer = Value<Buffer>::downcast
+ (subsys.getDiscreteVariable(s,m_bufferIx));
+
+ Buffer& nextBuffer = Value<Buffer>::updDowncast
+ (subsys.updDiscreteVarUpdateValue(s,m_bufferIx));
+
+ const Real t = s.getTime();
+ nextBuffer.copyInAndUpdate(prevBuffer, t-m_delay,
+ t, m_source.getValue(s));
+
+ subsys.markDiscreteVarUpdateValueRealized(s,m_bufferIx);
+ }
+private:
+ // TOPOLOGY STATE
+ Measure_<T> m_source;
+ Real m_delay;
+ bool m_canUseCurrentValue;
+ bool m_useLinearInterpolationOnly;
+
+ // TOPOLOGY CACHE
+ mutable DiscreteVariableIndex m_bufferIx; // auto-update
+};
+
+
+} // namespace SimTK
+
+
+
+
+#endif // SimTK_SimTKCOMMON_MEASURE_IMPLEMENTATION_H_
diff --git a/SimTKcommon/Simulation/include/SimTKcommon/internal/Stage.h b/SimTKcommon/Simulation/include/SimTKcommon/internal/Stage.h
new file mode 100644
index 0000000..25dd86d
--- /dev/null
+++ b/SimTKcommon/Simulation/include/SimTKcommon/internal/Stage.h
@@ -0,0 +1,320 @@
+#ifndef SimTK_SimTKCOMMON_STAGE_H_
+#define SimTK_SimTKCOMMON_STAGE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/internal/common.h"
+#include "SimTKcommon/internal/String.h"
+#include "SimTKcommon/internal/Exception.h"
+
+#include <cassert>
+#include <iostream>
+#include <iomanip>
+#include <cstdarg>
+
+namespace SimTK {
+
+/** This class is basically a glorified enumerated type, type-safe and range
+checked but permitting convenient (if limited) arithmetic. Constants look like
+Stage::Position, and loops can be written like
+ at code
+ for(Stage s = Stage::LowestValid; s <= Stage::HighestValid; ++s) {
+ // ...
+ }
+ at endcode
+Stage constants (of type Stage::Level) are implicitly converted to type
+Stage when necessary.
+
+Default construction gives Stage::Empty which really means "invalid". **/
+class Stage {
+public:
+ enum Level {
+ Empty = 0, ///< Lower than any legitimate Stage.
+ Topology = 1, ///< System topology realized.
+ Model = 2, ///< Modeling choices made.
+ Instance = 3, ///< Physical parameters set.
+ Time = 4, ///< A new time has been realized.
+ Position = 5, ///< Spatial configuration available.
+ Velocity = 6, ///< Spatial velocities available.
+ Dynamics = 7, ///< Forces calculated.
+ Acceleration = 8, ///< Accelerations and multipliers calculated.
+ Report = 9, ///< Report-only quantities evaluated.
+ Infinity = 10, ///< Higher than any legitimate Stage.
+
+ LowestValid = Empty, ///< For iterating over all stage values.
+ HighestValid = Infinity,
+ LowestRuntime = Model, ///< For iterating over meaningful stage values.
+ HighestRuntime = Report
+ };
+
+ enum {
+ NValid = HighestValid-LowestValid+1,
+ NRuntime = HighestRuntime-LowestRuntime+1
+ };
+
+ /** Default construction gives Stage::Empty. **/
+ Stage() : level(Stage::Empty) {}
+ /** This is an implicit conversion from Stage::Level to Stage. **/
+ Stage(Level l) {
+ assert(LowestValid <= l && l <= HighestValid);
+ level = l;
+ }
+ /** You can explicitly create a Stage from an int if it is in range. **/
+ explicit Stage(int l) {
+ assert(LowestValid <= l && l <= HighestValid);
+ level = Level(l);
+ }
+ /** Stage will implicitly convert to int so you can use it as an index. **/
+ operator int() const {return level;}
+
+ bool operator==(Level other) const {return level==other;}
+ bool operator!=(Level other) const {return level!=other;}
+ bool operator<(Level other) const {return level<other;}
+ bool operator<=(Level other) const {return level<=other;}
+ bool operator>(Level other) const {return level>other;}
+ bool operator>=(Level other) const {return level>=other;}
+ bool operator==(Stage other) const {return level==other.level;}
+ bool operator!=(Stage other) const {return level!=other.level;}
+ bool operator<(Stage other) const {return level<other.level;}
+ bool operator<=(Stage other) const {return level<=other.level;}
+ bool operator>(Stage other) const {return level>other.level;}
+ bool operator>=(Stage other) const {return level>=other.level;}
+
+ // Prefix operators
+ const Stage& operator++()
+ { assert(level<HighestValid); level=Level(level+1); return *this; }
+ const Stage& operator--()
+ { assert(level>LowestValid); level=Level(level-1); return *this;}
+ // Postfix operators
+ Stage operator++(int)
+ { assert(level<HighestValid); level=Level(level+1); return prev(); }
+ Stage operator--(int)
+ { assert(level>LowestValid); level=Level(level-1); return next(); }
+
+ /** Return the Stage following this one, with Stage::Infinity returned
+ if this Stage is already at its highest value, Stage::Report. An exception
+ is thrown if this Stage is already Stage::Infinity. **/
+ Stage next() const
+ { assert(level<HighestValid); return Stage(Level(level+1)); }
+ /** Return the Stage before this one, with Stage::Empty returned
+ if this Stage is already at its lowest value, Stage::Topology. An exception
+ is thrown if this Stage is already Stage::Empty. **/
+ Stage prev() const
+ { assert(level>LowestValid); return Stage(Level(level-1)); }
+
+ /** Return a printable name corresponding to the stage level currently
+ stored in this Stage. **/
+ String getName() const {
+ switch (level) {
+ case Empty: return "Empty"; break;
+ case Topology: return "Topology"; break;
+ case Model: return "Model"; break;
+ case Instance: return "Instance"; break;
+ case Time: return "Time"; break;
+ case Position: return "Position"; break;
+ case Velocity: return "Velocity"; break;
+ case Dynamics: return "Dynamics"; break;
+ case Acceleration: return "Acceleration"; break;
+ case Report: return "Report"; break;
+ case Infinity: return "Infinity"; break;
+ default: assert(!"Stage::getName(): illegal level");
+ }
+ return String("INVALID STAGE LEVEL ") + String(level);
+ }
+
+ /** Set this Stage=min(stageNow, tooHigh-1). **/
+ void invalidate(Stage tooHigh) {
+ if (level >= tooHigh.level)
+ *this = tooHigh.prev();
+ }
+
+ /** Return true if this Stage has one of the meaningful values between
+ Stage::Topology and Stage::Report, rather than one of the end markers
+ Stage::Empty or Stage::Infinity. **/
+ bool isInRuntimeRange() const
+ { return Stage::LowestRuntime <= level
+ && level <= Stage::HighestRuntime; }
+
+private:
+ Level level;
+};
+
+
+
+
+namespace Exception {
+
+class RealizeTopologyMustBeCalledFirst : public Base {
+public:
+ RealizeTopologyMustBeCalledFirst(const char* fn, int ln,
+ const char* objectType, // e.g., "System", "Subsystem"
+ const char* objectName, const char* methodName) : Base(fn,ln)
+ {
+ setMessage(String(methodName) + ": " + String(objectType) + " " + String(objectName)
+ + " topology has not been realized since the last topological change"
+ " -- you must call realizeTopology() first.");
+ }
+ virtual ~RealizeTopologyMustBeCalledFirst() throw() { }
+};
+
+class StateAndSystemTopologyVersionsMustMatch : public Base {
+public:
+ StateAndSystemTopologyVersionsMustMatch(const char* fn, int ln,
+ const char* objectType, // e.g., "System", "Subsystem"
+ const char* objectName, const char* methodName,
+ int sysTopoVersion,
+ int stateTopoVersion) : Base(fn,ln)
+ {
+ setMessage(String(methodName)
+ + ": The given State's Topology stage version number ("
+ + String(stateTopoVersion)
+ + ") doesn't match the current topology cache version number ("
+ + String(sysTopoVersion)
+ + ") of " + String(objectType) + " " + String(objectName) + "."
+ + " That means there has been a topology change to this System since this"
+ " State was created so they are no longer compatible. You should create"
+ " a new State from the System's default State."
+ " (Loopholes exist for advanced users.)");
+ }
+ virtual ~StateAndSystemTopologyVersionsMustMatch() throw() { }
+};
+
+
+
+class StageTooLow : public Base {
+public:
+ StageTooLow(const char* fn, int ln,
+ Stage currentStage, Stage targetStage, const char* where) : Base(fn,ln)
+ {
+ setMessage("Expected stage to be at least " + targetStage.getName() + " in " + String(where)
+ + " but current stage was " + currentStage.getName());
+ }
+ virtual ~StageTooLow() throw() { }
+};
+
+class StageIsWrong : public Base {
+public:
+ StageIsWrong(const char* fn, int ln,
+ Stage currentStage, Stage targetStage, const char* where) : Base(fn,ln)
+ {
+ setMessage("Expected stage to be " + targetStage.getName() + " in " + String(where)
+ + " but current stage was " + currentStage.getName());
+ }
+ virtual ~StageIsWrong() throw() { }
+};
+
+class StageTooHigh : public Base {
+public:
+ StageTooHigh(const char* fn, int ln,
+ Stage currentStage, Stage targetStage, const char* where) : Base(fn,ln)
+ {
+ setMessage("Expected stage to be less than " + targetStage.getName() + " in " + String(where)
+ + " but current stage was " + currentStage.getName());
+ }
+ virtual ~StageTooHigh() throw() { }
+};
+
+class StageOutOfRange : public Base {
+public:
+ StageOutOfRange(const char* fn, int ln,
+ Stage lower, Stage currentStage, Stage upper, const char* where) : Base(fn,ln)
+ {
+ setMessage("Expected (" + lower.getName() + " <= stage <= " + upper.getName() + ") in " + String(where)
+ + " but stage was " + currentStage.getName());
+ }
+ virtual ~StageOutOfRange() throw() { }
+};
+
+class CacheEntryOutOfDate : public Base {
+public:
+ CacheEntryOutOfDate(const char* fn, int ln,
+ Stage currentStage, Stage dependsOn, int dependsOnVersion, int lastCalculatedVersion)
+ : Base(fn,ln)
+ {
+ setMessage("State Cache entry was out of date at Stage " + currentStage.getName()
+ + ". This entry depends on version " + String(dependsOnVersion)
+ + " of Stage " + dependsOn.getName()
+ + " but was last updated at version " + String(lastCalculatedVersion) + ".");
+ }
+ virtual ~CacheEntryOutOfDate() throw() { }
+};
+
+// An attempt to realize a particular subsystem to a particular stage failed.
+class RealizeCheckFailed : public Base {
+public:
+ RealizeCheckFailed(const char* fn, int ln, Stage g,
+ int subsystemId, const char* subsystemName,
+ const char* fmt ...) : Base(fn,ln)
+ {
+ char buf[1024];
+ va_list args;
+ va_start(args, fmt);
+ vsprintf(buf, fmt, args);
+ setMessage("Couldn't realize subsystem " + String(subsystemId)
+ + "(" + String(subsystemName) + ") to Stage "
+ + g.getName() + ": " + String(buf) + ".");
+ va_end(args);
+ }
+ virtual ~RealizeCheckFailed() throw() { }
+};
+
+
+} // namespace Exception
+
+inline std::ostream& operator<<(std::ostream& o, Stage g)
+{ o << g.getName(); return o; }
+
+
+} // namespace SimTK
+
+ // REALIZECHECKs: these should be used to catch and report problems that
+ // occur when realizing a subsystem.
+
+#define SimTK_REALIZECHECK_ALWAYS(cond,stage,subsysIx,subsysName,msg) \
+ do{if(!(cond))SimTK_THROW4(SimTK::Exception::RealizeCheckFailed, \
+ (stage),(subsysIx),(subsysName),(msg)); \
+ }while(false)
+#define SimTK_REALIZECHECK1_ALWAYS(cond,stage,subsysIx,subsysName,msg,a1) \
+ do{if(!(cond))SimTK_THROW5(SimTK::Exception::RealizeCheckFailed, \
+ (stage),(subsysIx),(subsysName),(msg),(a1)); \
+ }while(false)
+#define SimTK_REALIZECHECK2_ALWAYS(cond,stage,subsysIx,subsysName,msg,a1,a2)\
+ do{if(!(cond))SimTK_THROW6(SimTK::Exception::RealizeCheckFailed, \
+ (stage),(subsysIx),(subsysName),(msg),(a1),(a2)); \
+ }while(false)
+#define SimTK_REALIZECHECK3_ALWAYS(cond,stage,subsysIx,subsysName,msg,a1,a2,a3) \
+ do{if(!(cond))SimTK_THROW7(SimTK::Exception::RealizeCheckFailed, \
+ (stage),(subsysIx),(subsysName),(msg),(a1),(a2),(a3)); \
+ }while(false)
+#define SimTK_REALIZECHECK4_ALWAYS(cond,stage,subsysIx,subsysName,msg,a1,a2,a3,a4) \
+ do{if(!(cond))SimTK_THROW8(SimTK::Exception::RealizeCheckFailed, \
+ (stage),(subsysIx),(subsysName),(msg),(a1),(a2),(a3),(a4)); \
+ }while(false)
+#define SimTK_REALIZECHECK5_ALWAYS(cond,stage,subsysIx,subsysName,msg,a1,a2,a3,a4,a5) \
+ do{if(!(cond))SimTK_THROW9(SimTK::Exception::RealizeCheckFailed, \
+ (stage),(subsysIx),(subsysName),(msg),(a1),(a2),(a3),(a4),(a5)); \
+ }while(false)
+
+
+#endif // SimTK_SimTKCOMMON_STAGE_H_
diff --git a/SimTKcommon/Simulation/include/SimTKcommon/internal/State.h b/SimTKcommon/Simulation/include/SimTKcommon/internal/State.h
new file mode 100644
index 0000000..99af869
--- /dev/null
+++ b/SimTKcommon/Simulation/include/SimTKcommon/internal/State.h
@@ -0,0 +1,1119 @@
+#ifndef SimTK_SimTKCOMMON_STATE_H_
+#define SimTK_SimTKCOMMON_STATE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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. *
+ * *
+ * Portions copyright (c) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * 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/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/Event.h"
+
+#include <ostream>
+#include <cassert>
+#include <set>
+
+namespace SimTK {
+
+
+/// @class SimTK::SubsystemIndex
+/// Provide a unique integer type for identifying Subsystems.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(SubsystemIndex);
+
+/// @class SimTK::SystemYIndex
+/// This unique integer type is for indexing the global, System-level "y-like"
+/// arrays, that is, the arrays in which all of the various Subsystems' continuous
+/// state variables q, u, and z have been collected into contiguous memory.
+/// This type should be used for the global y and its global time derivative yDot.
+/// Note that there is no Subsystem-local equivalent of the y array.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemYIndex);
+
+/// @class SimTK::SystemQIndex
+/// This unique integer type is for indexing global "q-like" arrays, that is, arrays
+/// that inherently have the same dimension as the total number of second
+/// order state variables (generalized coordinates) in the full System-level
+/// view of the State. This type should be used for the global q and its global
+/// time derivatives qDot and qDotDot.
+/// @see QIndex for Subsystem-local q indexing
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemQIndex);
+/// @class SimTK::QIndex
+/// Unique integer type for Subsystem-local q indexing
+/// @see SystemQIndex
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(QIndex);
+
+/// @class SimTK::SystemUIndex
+/// This unique integer type is for indexing global "u-like" arrays, that is, arrays
+/// that inherently have the same dimension as the total number of mobilities
+/// (generalized speeds) in the full System-level view of the State. This type
+/// should be used for the global u and its global time derivative uDot.
+/// @see UIndex for Subsystem-local u indexing
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemUIndex);
+/// @class SimTK::UIndex
+/// Unique integer type for Subsystem-local u indexing
+/// @see SystemUIndex
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(UIndex);
+
+/// @class SimTK::SystemZIndex
+/// This unique integer type is for indexing global "z-like" arrays, that is, arrays
+/// that inherently have the same dimension as the total number of auxiliary
+/// state variables in the full System-level view of the State. This type
+/// should be used for the global z and its global time derivative zDot.
+/// @see ZIndex for Subsystem-local z indexing
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemZIndex);
+/// @class SimTK::ZIndex
+/// Unique integer type for Subsystem-local z indexing
+/// @see SystemZIndex
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(ZIndex);
+
+/// @class SimTK::DiscreteVariableIndex
+/// This unique integer type is for selecting discrete variables. These indices
+/// are always Subsystem-local, that is, the first discrete variable belonging
+/// to each Subsystem has index 0.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(DiscreteVariableIndex);
+
+/// @class SimTK::CacheEntryIndex
+/// This unique integer type is for selecting non-shared cache entries. These indices
+/// are always Subsystem-local, that is, the first explicitly-allocated cache
+/// entry belonging to each Subsystem has index 0.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(CacheEntryIndex);
+
+/// @class SimTK::SystemYErrIndex
+/// This unique integer type is for indexing the global, System-level "yErr-like"
+/// arrays, that is, the arrays in which all of the various Subsystems' qErr and
+/// uErr constraint equation slots have been collected together.
+/// Note that there is no Subsystem-local equivalent of the yErr array.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemYErrIndex);
+
+/// @class SimTK::SystemQErrIndex
+/// This unique integer type is for indexing global "qErr-like" arrays, that is, arrays
+/// that inherently have the same dimension as the total number of position-level
+/// constraint equations in the full System-level view of the State.
+/// @see QErrIndex for Subsystem-local qErr indexing
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemQErrIndex);
+/// @class SimTK::QErrIndex
+/// Unique integer type for Subsystem-local qErr indexing
+/// @see SystemQErrIndex
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(QErrIndex);
+
+/// @class SimTK::SystemUErrIndex
+/// This unique integer type is for indexing global "uErr-like" arrays, that is, arrays
+/// that inherently have the same dimension as the total number of velocity-level
+/// constraint equations in the full System-level view of the State.
+/// @see UErrIndex for Subsystem-local uErr indexing
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemUErrIndex);
+/// @class SimTK::UErrIndex
+/// Unique integer type for Subsystem-local uErr indexing
+/// @see SystemUErrIndex
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(UErrIndex);
+
+/// @class SimTK::SystemUDotErrIndex
+/// This unique integer type is for indexing global "uDotErr-like" arrays, that is, arrays
+/// that inherently have the same dimension as the total number of acceleration-level
+/// constraint equations in the full System-level view of the State.
+/// @see UDotErrIndex for Subsystem-local uDotErr indexing
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemUDotErrIndex);
+/// @class SimTK::UDotErrIndex
+/// Unique integer type for Subsystem-local uDotErr indexing
+/// @see SystemUDotErrIndex
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(UDotErrIndex);
+
+/// @class SimTK::SystemMultiplierIndex
+/// This unique integer type is for indexing global "multiplier-like" arrays, that is, arrays
+/// that inherently have the same dimension as the total number of Lagrange multipliers
+/// in the full System-level view of the State.
+/// @see MultiplierIndex for Subsystem-local multiplier indexing
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(SystemMultiplierIndex);
+/// @class SimTK::MultiplierIndex
+/// Unique integer type for Subsystem-local multiplier indexing
+/// @see SystemMultiplierIndex
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(MultiplierIndex);
+
+/// This is the type to use for Stage version numbers. Whenever any state
+/// variable is modified, we increment the stage version for the stage(s) that
+/// depend on it. -1 means "unintialized". 0 is never used as a stage version,
+/// but is allowed as a cache value which is guaranteed never to look valid.
+typedef int StageVersion;
+
+
+/** This is the handle class for the hidden State implementation.
+
+This object is intended to contain all state information for a SimTK::System,
+except topological information which is stored in the system itself. A system
+is "const" after its topology has been constructed and realized.
+
+Systems contain a set of Subsystem objects, and %State supports that concept by
+allowing per-subsystem partitioning of the total system state. This allows
+subsystems to have their own private state variables, while permitting the
+system to allow shared access to state among the subsystems when necessary.
+
+The %State provides services reflecting the structure of the equations it
+expects to find in the System. Three different views of the same state
+information are supported to accommodate three different users:
+ - the system as a whole
+ - Subsystems contained in the system
+ - numerical methods operating on the state
+
+Typically numerical methods have a much less nuanced view of the state
+than do the system or subsystems.
+
+The system is expected to be a "hybrid DAE", that is, a mixture of continuous
+and discrete dynamic equations, and algebraic constraints. There is an
+independent variable t, continuous state variables y, and discrete state
+variables d.
+
+The continuous part is an ODE-on-a-manifold system suitable for solution via
+coordinate projection, structured like this for the view taken by numerical
+methods:
+<pre>
+ (1) y' = f(d;t,y) differential equations
+ (2) c = c(d;t,y) algebraic equations (manifold is c=0)
+ (3) e = e(d;t,y) event triggers (watch for zero crossings)
+</pre>
+with initial conditions t0,y0,d0 such that c=0. The discrete variables d are
+updated upon occurence of specific events. When those events are functions of
+time or state, they are detected using the set of scalar-valued event trigger
+functions e (3).
+
+In the more detailed view as seen from the System, we consider y={q,u,z} to
+be partitioned into position variables q, velocity variables u, and auxiliary
+variables z. There will be algebraic constraints involving q, u, and u's time
+derivatives udot. The system is now assumed to look like this:
+<pre>
+ (4) qdot = N(q) u
+ (5) zdot = zdot(d;t,q,u,z)
+
+ (6) M(q) udot + ~G(q) mult = f(d;t,q,u,z)
+ G(q) udot = b(d;t,q,u)
+
+ [ pdotdot(d;t,q,u,udot) ]
+ (7) udotErr = [ vdot(d;t,q,u,udot) ] = 0
+ [ a(d;t,q,u,udot) ]
+
+ (8) uErr = [ pdot(d;t,q,u) ] = 0
+ [ v(d;t,q,u) ]
+
+ (9) qErr = [ p(d;t,q) ] = 0
+ [ n(q) ]
+</pre>
+The q's can also be dealt with directly as second order variables via
+<pre>
+ (10) qdotdot = Ndot(q,qdot) u + N(q) udot
+</pre>
+
+Here G = [P;V;A] with A(q) being the coefficient matrix for constraints
+appearing only at the acceleration level, and V(q)=partial(v)/partial(u)
+the coefficient matrix for the velocity (nonholonomic) constraints, and
+P(q)=partial(pdot)/partial(u) is the coefficient matrix of the first
+time derivatives of the position (holonomic) constraints.
+Note that uErr in Eq 8 is assumed to include equations resulting from
+differentiation of p() in Eq 9, as well as ones first introduced at the
+velocity level (nonholonomic constraints), and udotErr is similarly
+built from acceleration-only constraints a() and derivatives of higher-level
+constraints.
+
+If a system allocates nq q's, nu u's, and nz z's the State will also allocate
+matching cache variables qdot, qdotdot, udot, and zdot. If mp position
+(holonomic) constraints (9), mpv velocity constraints (8) and mpva acceleration
+constraints (7) are allocated, the state creates cache entries of like sizes
+qErr, uErr, udotErr. In addition room for the mpva Lagrange multipliers 'mult'
+is allocated in the cache.
+
+In the final view, the Subsystem view, the same variables and cache entries
+exist, but only the ones allocated by that Subsystem are visible. All of a
+Subsystem's q's are consecutive in memory, as are its u's, uErr's, etc., but
+the q's are not adjacent to the u's as they are for the System's view.
+
+The default constructor creates a %State containing no state variables and with
+its realization cache stage set to Stage::Empty. During subsystem construction,
+variables and cache entries for any stage can be allocated, however \e all
+Model stage variables must be allocated during this time. At the end of
+construction, call advanceSubsystemToStage(Topology) which will put the
+subsystem at Stage::Topology. Then the subsystems realize their Model stages,
+during which variables at any stage > Model, and cache entries at any stage
+>= Model can be allocated. After that call advanceSubsystemToStage(Model)
+which sets the stage to Stage::Model and disallows further state allocation.
+
+Note that there is a global Stage for the state as a whole, and individual
+Stages for each subsystem. The global stage can never be higher than
+the lowest subsystem stage. Global state resources are allocated when the
+global Stage advances to "Model" and tossed out if that stage is
+invalidated. Similarly, cache resources are allocated at stage Instance
+and forgotten when Instance is invalidated. Note that subsystems will
+"register" their use of the global variable pools during their own modeling
+stages, but that the actual global resources won't exist until the \e system
+has been advanced to Model or Instance stage. **/
+class SimTK_SimTKCOMMON_EXPORT State {
+public:
+/// Create an empty State.
+State();
+~State();
+
+/// Restore State to default-constructed condition.
+void clear();
+
+/// Set the number of subsystems in this state. This is done during
+/// initialization of the State by a System; it completely wipes out
+/// anything that used to be in the State so use cautiously!
+void setNumSubsystems(int i);
+
+/// Set the name and version for a given subsystem, which must already
+/// have a slot allocated.
+void initializeSubsystem(SubsystemIndex, const String& name,
+ const String& version);
+
+/// Make the current State a copy of the source state, copying only
+/// state variables and not the cache. If the source state hasn't
+/// been realized to Model stage, then we don't copy its state
+/// variables either, except those associated with the Topology stage.
+State(const State&);
+
+/// Make the current State a copy of the source state, copying only
+/// state variables and not the cache. If the source state hasn't
+/// been realized to Model stage, then we don't copy its state
+/// variables either, except those associated with the Topology stage.
+State& operator=(const State&);
+
+/// Register a new subsystem as a client of this State. The
+/// supplied strings are stored with the State but are not
+/// interpreted by it. The intent is that they can be used to
+/// perform "sanity checks" on deserialized States to make
+/// sure they match the currently instantiated System.
+/// The subsystem index (a small integer) is returned.
+SubsystemIndex addSubsystem(const String& name, const String& version);
+
+int getNumSubsystems() const;
+const String& getSubsystemName (SubsystemIndex) const;
+const String& getSubsystemVersion(SubsystemIndex) const;
+const Stage& getSubsystemStage (SubsystemIndex) const;
+
+/// This returns the *global* stage for this State.
+const Stage& getSystemStage() const;
+
+/// If any subsystem or the system stage is currently at or
+/// higher than the passed-in one, back up to the stage just prior;
+/// otherwise do nothing. This is for use if you have write
+/// access to the state and can invalidate even Topology and Model
+/// stages which may destroy state variables. "All" here refers to
+/// all Subysystems.
+void invalidateAll(Stage);
+
+/// If any subsystem or the system stage is currently at or
+/// higher than the passed-in one, back up to the stage just prior;
+/// otherwise do nothing. This const method can only be used to
+/// invalidate Stage::Instance or higher. To invalidate Model or
+/// Topology stage you must have write access to the state because
+/// invalidating those stages can destroy state variables in addition
+/// to cache entries. "All" here refers to all Subsystems.
+void invalidateAllCacheAtOrAbove(Stage) const;
+
+/// Advance a particular Subsystem's current stage by one to
+/// the indicated stage. The stage is passed in just to give us a
+/// chance to verify that all is as expected. You can only advance one stage at
+/// a time. Advancing to Topology, Model, or Instance stage affects
+/// what you can do later.
+/// @see advanceSystemToStage()
+void advanceSubsystemToStage(SubsystemIndex, Stage) const;
+/// Advance the System-level current stage by one to the indicated stage.
+/// This can only be done if <em>all</em> Subsystem have already been
+/// advanced to this Stage.
+/// @see advanceSubsystemToStage()
+void advanceSystemToStage(Stage) const;
+
+/** The Topology stage version number (an integer) stored in this %State must
+match the topology cache version number stored in the System for which it is
+allegedly a state.
+ at see System::getSystemTopologyCacheVersion() **/
+StageVersion getSystemTopologyStageVersion() const;
+
+/** @name Continuous state variables
+These continuous state variables are shared among all the subsystems
+and are not allocated until the \e system is advanced to Stage::Model.
+The returned index is local to each subsystem. After the system is modeled,
+we guarantee that all the q's for a subsystem will be contiguous, and
+similarly for u's and z's. However, q,u,z will \e not be contiguous with
+each other. The \e global y={q,u,z} is contiguous, and global q,u,z are
+contiguous within y, in that order. Corresponding cache entries for
+the derivatives of these variables are allocated at Model stage also. **/
+/**@{**/
+/** Allocate generalized coordinates \e q, which are second order continuous
+state variables. Matching cache entries \e qdot and \e qdotdot are allocated to
+hold the first and second time derivatives of \e q. The supplied vector \a qInit
+is used to specify the number of \e q's to be allocated and their initial
+values. The Subsystem-local QIndex of the first allocated \e q is returned; the
+others follow consecutively. **/
+QIndex allocateQ(SubsystemIndex, const Vector& qInit);
+/** Allocate generalized speeds \e u, which are first order continuous
+state variables related to the derivatives of the second order \e q's by
+qdot=N(q)*u, for a %System-defined coupling matrix N. A matching cache entry
+\e udot is allocated to hold the time derivative of \e u. The supplied vector
+\a uInit is used to specify the number of \e u's to be allocated and their
+initial values. The Subsystem-local UIndex of the first allocated \e u is
+returned; the others follow consecutively. **/
+UIndex allocateU(SubsystemIndex, const Vector& uInit);
+/** Allocate auxiliary first order continuous state variables \e z. A matching
+cache entry \e zdot is allocated to hold the time derivative of \e z. The
+supplied vector \a zInit is used to specify the number of \e z's to be allocated
+and their initial values. The Subsystem-local ZIndex of the first allocated
+\e z is returned; the others follow consecutively. **/
+ZIndex allocateZ(SubsystemIndex, const Vector& zInit);
+/**@}**/
+
+/** @name Constraint errors and multipliers
+These constraint error cache entries are shared among all the subsystems
+and are not allocated until the \e system is advanced to Stage::Instance.
+The returned index is local to each subsystem. Q errors and U errors will
+each be contiguous for a given subsystem, but \e not with each other. However,
+the System-level yerr={qerr,uerr} \e is a single contiguous vector.
+UDotErr is a separate quantity, not part of yerr. Again the UDotErr's for
+each subsystem will be contiguous within the larger UDotErr Vector.
+Allocating a UDotErr has the side effect of allocating another Vector
+of the same size in the cache for the corresponding Lagrange multipliers,
+and these are partitioned identically to UDotErrs. **/
+/**@{**/
+/** Allocate \a nqerr cache slots to hold the current error for position-level
+(holonomic) constraint equations. **/
+QErrIndex allocateQErr (SubsystemIndex, int nqerr) const;
+/** Allocate \a nuerr cache slots to hold the current error for velocity-level
+(nonholonomic and holonomic first derivative) constraint equations. **/
+UErrIndex allocateUErr (SubsystemIndex, int nuerr) const;
+/** Allocate \a nudoterr cache slots to hold the current error for
+acceleration-level (acceleration-only, nonholonomic first derivative, and
+holonomic second derivative) constraint equations. This also allocates the
+same number of slot in the constraint multipliers vector. **/
+UDotErrIndex allocateUDotErr(SubsystemIndex, int nudoterr) const;
+/**@}**/
+
+/** @name Event witness functions
+Some Events require a slot in the %State cache to hold the current value
+of the event trigger function (a.k.a. event "witness" function).
+The Stage here is the stage at which the trigger function's value
+should be examined by a TimeStepper. The returned index is local to
+the Subsystem and also to the stage. These can be allocated in a %State
+that has not yet been realized to Instance stage, and will be forgotten
+appropriately if the %State is later backed up to an earlier stage.
+When this %State is realized to Instance stage, global event trigger slots
+will be allocated, collecting all same-stage event triggers together
+consecutively for the convenience of the TimeStepper. Within a stage,
+a given subsystem's event trigger slots for that stage are consecutive. **/
+/**@{**/
+/** Allocate room for \a nevent witness function values that will be available
+at the indicated \a stage. The Subsystem- and Stage-local index of the first
+allocated witness is returned; the rest follow consecutively. **/
+EventTriggerByStageIndex
+allocateEventTrigger(SubsystemIndex, Stage stage, int nevent) const;
+/**@}**/
+
+
+/** @name Discrete Variables
+
+You can allocate a new DiscreteVariable in any State whose stage has not yet
+been advanced to Model stage. The stage at allocation (Empty or Topology) is
+remembered so that the appropriate discrete variables can be forgotten if the
+%State's stage is reduced back to that stage later after advancing past it.
+DiscreteVariables are private to each Subsystem and allocated immediately. The
+returned index is unique within the Subsystem and there is no corresponding
+global index. **/
+/**@{**/
+/** The Stage supplied here in the call is the earliest subsystem stage which is invalidated
+by a change made to this discrete variable. You may access the value of the discrete
+variable for reading (via getDiscreteVariable()) or writing (via updDiscreteVariable())
+any time after it has been allocated. Access for writing has the side effect of
+reducing the subsystem and system stages for this State to one stage below the one
+supplied here, that is, the stage supplied here is invalidated. Note that you must
+have write access to the State in order to change the value of any state variable.
+
+Ownership of the AbstractValue object supplied here is taken over by the State --
+don't delete the object after this call!
+ at see getDiscreteVariable()
+ at see updDiscreteVariable() **/
+DiscreteVariableIndex
+allocateDiscreteVariable(SubsystemIndex, Stage invalidates, AbstractValue*);
+
+/** This method allocates a DiscreteVariable whose value should be updated
+automatically after each time step. A CacheEntry of the same value type as
+the variable is allocated to hold the update value. The discrete variable is
+allocated as described for allocateDiscreteVariable(), except that the
+\a invalidates stage must be higher than Stage::Time. The cache entry is
+allocated as described for allocateCacheEntry() without an automatic calculation
+(\a latest) stage. The cache entry is then considered to be the "update" value
+for the discrete variable. Update values play a similar role for
+discrete variables as derivatives play for continuous variables. That is, they
+define how the variable is to be updated when a TimeStepper accepts a step.
+
+Update occurs as follows: at the start of every continuous interval, after all
+other pending events have been handled, a time stepper should call the State
+method autoUpdateDiscreteVariables(). That method looks at all the
+auto-update discrete variables to see which ones have valid update values. For
+each valid value, the discrete variable and its update value are swapped, and
+the new cache value is marked invalid.
+
+ at note No stage is invalidated by the swap even though this is clearly modifying
+the state variable. It is up to the user of this variable to make sure that is
+reasonable, by using the <em>update value</em>, not the <em>variable value</em>
+for computations during realize(). In that way the results are always calculated
+using the value as it will be \e after an update. That
+means that no results will change when the swap occurs, so no stage needs
+to be invalidated upon updating. If you do use both values, make sure that all
+computed results remain unchanged from the end of one step to the beginning of
+the next.
+
+The above behavior is entirely analogous to the treatment of continuous
+variables like q: the integrator ensures that only updated values of q are
+seen when evaluations are made at intermediate or trial steps; you should
+do the same. In contrast to this auto-update behavior, any \e explicit change
+to the discrete variable will invalidate the variable's \a invalidates stage
+just as for a non-auto-updating discrete variable. The auto-update cache entry
+is always invalidated by an explicit change to the variable, as well as by
+the \a updateDependsOn stage being invalidated.
+
+Ownership of the AbstractValue object supplied here is taken over by the
+State -- don't delete the object after this call! A clone() of this value will
+be used in the auto-update cache entry so there will be two objects of this type
+around at run time that get swapped back and forth between the state variable
+and the cache entry.
+
+You can allocate discrete variables in a State at Topology stage or Model
+stage but not later. That is, you allocate the variable while the State
+is in Stage::Empty, and then it appears when you do realizeTopology(); or,
+you allocate the variable when the State is in Stage::Topology and it
+appears when you do realizeModel().
+
+ at see allocateDiscreteVariable()
+ at see allocateCacheEntry() **/
+DiscreteVariableIndex
+allocateAutoUpdateDiscreteVariable(SubsystemIndex, Stage invalidates,
+ AbstractValue*, Stage updateDependsOn);
+/** For an auto-updating discrete variable, return the CacheEntryIndex for
+its associated update cache entry, otherwise return an invalid index. **/
+CacheEntryIndex
+getDiscreteVarUpdateIndex(SubsystemIndex, DiscreteVariableIndex) const;
+/** At what stage was this State when this discrete variable was allocated? The answer must be Stage::Empty or Stage::Topology. **/
+Stage getDiscreteVarAllocationStage(SubsystemIndex, DiscreteVariableIndex) const;
+/** What is the earliest stage that is invalidated when this discrete variable
+is modified? All later stages are also invalidated. This stage was set
+when the discrete variable was allocated and can't be changed with
+unallocating it first. **/
+Stage getDiscreteVarInvalidatesStage(SubsystemIndex, DiscreteVariableIndex) const;
+
+
+/** Get the current value of the indicated discrete variable. This requires
+only that the variable has already been allocated and will fail otherwise. **/
+const AbstractValue&
+getDiscreteVariable(SubsystemIndex, DiscreteVariableIndex) const;
+/** Return the time of last update for this discrete variable. **/
+Real getDiscreteVarLastUpdateTime(SubsystemIndex, DiscreteVariableIndex) const;
+/** For an auto-updating discrete variable, return the current value of its
+associated update cache entry; this is the value the discrete variable will have
+the next time it is updated. This will fail if the value is not valid or if this
+is not an auto-update discrete variable. **/
+const AbstractValue&
+getDiscreteVarUpdateValue(SubsystemIndex, DiscreteVariableIndex) const;
+/** For an auto-updating discrete variable, return a writable reference to
+the value of its associated update cache entry. This will be the value that this
+discrete variable will have when it is next updated. Don't forget to mark
+the cache entry valid after you have updated it. This will fail if this is
+not an auto-update discrete variable. **/
+AbstractValue&
+updDiscreteVarUpdateValue(SubsystemIndex, DiscreteVariableIndex) const;
+/** Check whether the update value for this auto-update discrete variable has
+already been computed since the last change to state variables it depends on.
+**/
+bool isDiscreteVarUpdateValueRealized(SubsystemIndex, DiscreteVariableIndex) const;
+/** Mark the update value for this auto-update discrete variable as up-to-date
+with respect to the state variables it depends on. **/
+void markDiscreteVarUpdateValueRealized(SubsystemIndex, DiscreteVariableIndex) const;
+
+/** Get a writable reference to the value stored in the indicated discrete
+state variable dv, and invalidate stage dv.invalidates and all higher stages.
+The current time is recorded as the variable's "last update time". **/
+AbstractValue& updDiscreteVariable(SubsystemIndex, DiscreteVariableIndex);
+/** Alternate interface to updDiscreteVariable. **/
+void setDiscreteVariable(SubsystemIndex, DiscreteVariableIndex,
+ const AbstractValue&);
+/**@}**/
+
+/** @name Cache Entries
+
+You can allocate a new CacheEntry in any State whose stage has not yet been
+advanced to Instance stage. The stage at allocation (Empty, Topology, or
+Model) is remembered so that the appropriate cache entries can be forgotten
+if the State's stage is reduced back to that stage later after advancing
+past it. CacheEntries are private to each Subsystem and allocated
+immediately. The returned index is unique within the Subsystem and there
+is no corresponding global index. **/
+/**@{**/
+
+/** There are two Stages supplied explicitly as arguments to this method:
+\a earliest and \a latest. The \a earliest Stage is the stage at which the
+cache entry \e could be calculated. Hence if the Subsystem stage is reduced
+below \a earliest the cache entry is known to be invalid. The \a latest
+Stage, if any, is the stage at which the cache entry is \e guaranteed to
+have been calculated (typically as the result of a System-wide realize()
+call to that stage). For stages \a earliest through \a latest-1, the
+cache entry \e may be valid, if it has already been calculated. In that
+case an explicit validity indicator will have been set at the time it was
+computed, via markCacheValueRealized(). That indicator is cleared
+automatically whenever the Subsystem stage is reduced below \a earliest.
+The validity indicator need not have been set in order for the cache entry
+to be deemed valid at \a latest stage.
+
+If \a latest is given as Stage::Infinity then there is no guarantee that
+this Subsystem will automatically calculate a value for this cache entry,
+which makes it a "lazy" evaluation that is done only if requested. In that
+case the only way the cache entry can become valid is if the calculation
+is performed and the validity indicator is set explicitly with
+markCacheValueRealized(). Here is how we suggest you structure lazy
+evaluation of a cache entry CE of type CEType and CacheEntryIndex CEIndex
+(this is pseudocode):
+
+(1) Allocate your lazy cache entry something like this:
+\code
+ CEIndex = s.allocateLazyCacheEntry(subsys,stage,new Value<CEType>());
+\endcode
+(2) Write a realizeCE() method structured like this:
+\code
+ void realizeCE(const State& s) const {
+ if (s.isCacheValueRealized(subsys,CEIndex))
+ return;
+ // calculate the cache entry, update with updCacheEntry()
+ s.markCacheValueRealized(subsys,CEIndex);
+ }
+\endcode
+(3) Write a getCE() method structured like this:
+\code
+ const CEType& getCE(const State& s) const {
+ realizeCE(s); // make sure CE has been calculated
+ return Value<CEType>::downcast(s.getCacheEntry(subsys,CEIndex));
+ }
+\endcode
+(4) Write an updCE() method like this:
+\code
+ CEType& updCE(const State& s) const {
+ return Value<CEType>::updDowncast(s.updCacheEntry(subsys,CEIndex));
+ }
+\endcode
+
+Then access CE \e only through your getCE() method. There
+should be only one place in your code where isCacheValueRealized() and
+markCacheValueRealized() are called for a particular cache entry. If
+you do this from multiple locations there is a high probability of a bug
+being introduced, especially due to later modification of the code.
+
+Prior to the Subsystem advancing to \a earliest stage, and prior to \a latest
+stage unless the validity indicator is set, attempts to look at the value via
+getCacheEntry() will throw an exception. However, you may access the cache entry
+for writing via updCacheEntry() any time after stage \a earliest-1. If you
+evaluate it prior to \a latest, be sure to explicitly mark it valid.
+Note that cache entries are mutable so you do not need write
+access to the State in order to access a cache entry for writing.
+
+Ownership of the AbstractValue object supplied here is taken over by the State --
+don't delete the object after this call!
+ at see getCacheEntry(), updCacheEntry()
+ at see allocateLazyCacheEntry(), isCacheValueRealized(), markCacheValueRealized() **/
+CacheEntryIndex allocateCacheEntry(SubsystemIndex, Stage earliest, Stage latest,
+ AbstractValue*) const;
+
+/** This is an abbreviation for allocation of a cache entry whose earliest
+and latest Stages are the same. That is, this cache entry is guaranteed to
+be valid if its Subsystem has advanced to the supplied Stage or later, and
+is guaranteed to be invalid below that Stage. **/
+CacheEntryIndex allocateCacheEntry(SubsystemIndex sx, Stage g, AbstractValue* v) const
+{ return allocateCacheEntry(sx, g, g, v); }
+
+/** This is an abbreviation for allocation of a lazy cache entry. The \a earliest
+stage at which this \e can be evaluated is provided; but there is no stage
+at which the cache entry will automatically be evaluated. Instead you have
+to evaluate it explicitly when someone asks for it, and then call
+markCacheValueRealized() to indicate that the value is available. The value
+is automatically invalidated when the indicated stage \a earliest is
+invalidated in the State.
+ at see allocateCacheEntry(), isCacheValueRealized(), markCacheValueRealized() **/
+CacheEntryIndex allocateLazyCacheEntry(SubsystemIndex sx, Stage earliest, AbstractValue* v) const
+{ return allocateCacheEntry(sx, earliest, Stage::Infinity, v); }
+
+/** At what stage was this State when this cache entry was allocated?
+The answer must be Stage::Empty, Stage::Topology, or Stage::Model. **/
+Stage getCacheEntryAllocationStage(SubsystemIndex, CacheEntryIndex) const;
+
+/** Retrieve a const reference to the value contained in a particular cache
+entry. The value must be up to date with respect to the state variables it
+depends on or this will throw an exception. No calculation will be
+performed here.
+ at see updCacheEntry()
+ at see allocateCacheEntry(), isCacheValueRealized(), markCacheValueRealized() **/
+const AbstractValue& getCacheEntry(SubsystemIndex, CacheEntryIndex) const;
+
+/** Retrieve a writable reference to the value contained in a particular cache
+entry. You can access a cache entry for writing any time after it has been
+allocated. This does not affect the current stage. The cache entry will
+neither be invalidated nor marked valid by accessing it here.
+ at see getCacheEntry()
+ at see allocateCacheEntry(), isCacheValueRealized(), markCacheValueRealized() **/
+AbstractValue& updCacheEntry(SubsystemIndex, CacheEntryIndex) const; // mutable
+
+/** Check whether the value in a particular cache entry has been recalculated
+since the last change to the state variables it depends on. Validity can
+result either from an explicit call to markCacheValueRealized() or by
+this %State's stage reaching the \a latest stage specified when the cache
+entry was allocated, after which the value is \e presumed valid. If this
+method returns true, then you can access the value with getCacheEntry()
+without getting an exception thrown.
+ at see allocateCacheEntry(), markCacheValueRealized(), getCacheEntry() **/
+bool isCacheValueRealized(SubsystemIndex, CacheEntryIndex) const;
+
+/** Mark the value of a particular cache entry as up to date after it has
+been recalculated. This %State's current stage must be at least the
+\a earliest stage as supplied when this cache entry was allocated, and
+it is unnecessary to call this method if the stage has reached the
+specified \a latest stage since after that we'll \e presume that the
+cache entry's value has been realized. Note that if the \a latest stage
+was given as Stage::Infinity then it is always necessary to call this
+method prior to accessing the cache entry's value. After a cache entry
+has been marked valid here, isCacheValueRealized() will return true. The
+cache entry is marked invalid automatically whenever a change occurs to
+a state variable on which it depends.
+ at see allocateCacheEntry(), isCacheValueRealized(), getCacheEntry() **/
+void markCacheValueRealized(SubsystemIndex, CacheEntryIndex) const;
+
+/** Normally cache entries are invalidated automatically, however this
+method allows manual invalidation of the value of a particular cache
+entry. After a cache entry has been marked invalid here,
+isCacheValueRealized() will return false.
+ at see isCacheValueRealized(), markCacheValueRealized() **/
+void markCacheValueNotRealized(SubsystemIndex, CacheEntryIndex) const;
+/**@}**/
+
+/// @name Global Resource Dimensions
+///
+/// These are the dimensions of the global shared state and cache resources,
+/// as well as the dimensions of the per-Subsystem partioning of those
+/// resources. State resource dimensions (including cache resources directly
+/// related to state variables) are known after the System has been
+/// realized to Model stage. Other cache resource dimensions are known after
+/// the System has been realized to Instance stage. Access to the actual data arrays
+/// may have stricter requirements (for example, you can't ask to look at UErr
+/// arrays until Velocity stage). Hence it is better to use these explicit
+/// dimension-providing methods than to get a reference to a Vector and ask
+/// for its size().
+///
+/// @see Per-Subsystem Dimensions group
+/// @see Global-to-Subsystem Maps group
+/// @{
+
+/// Get the total number ny=nq+nu+nz of shared continuous state variables.
+/// This is also the number of state derivatives in the cache entry ydot.
+/// Callable at Model stage.
+int getNY() const;
+/// Get total number of shared q's (generalized coordinates; second order
+/// state variables). This is also the number of first and second q time
+/// derivatives in the cache entries qdot and qdotdot.
+/// Callable at Model stage.
+int getNQ() const;
+/// Returns the y index at which the q's begin. Callable at Model stage.
+SystemYIndex getQStart() const;
+/// Get total number of shared u's (generalized speeds; mobilities).
+/// This is also the number of u time derivatives in the cache entry udot.
+/// Callable at Model stage.
+int getNU() const;
+/// Returns the y index at which the u's begin. Callable at Model stage.
+SystemYIndex getUStart() const;
+/// Get total number of shared z's (auxiliary state variables).
+/// This is also the number of z time derivatives in the cache entry zdot.
+/// Callable at Model stage.
+int getNZ() const;
+/// Returns the y index at which the z's begin. Callable at Model stage.
+SystemYIndex getZStart() const;
+/// Get the total number nyerr=nqerr+nuerr of shared cache entries for
+/// position-level and velocity-level constraint errors.
+/// Callable at Instance stage.
+int getNYErr() const;
+/// Return the total number nqerr=mp+nQuaternions of cache entries for
+/// position-level constraint errors. Callable at Instance stage.
+int getNQErr() const;
+/// Returns the yErr index at which the qErr's begin. Callable at Instance stage.
+SystemYErrIndex getQErrStart() const;
+/// Return the total number nuerr=mp+mv of cache entries for
+/// velocity-level constraint errors (including also errors in the
+/// time derivatives of position-level constraints). Callable at Instance stage.
+int getNUErr() const;
+/// Returns the yErr index at which the uErr's begin. Callable at Instance stage.
+SystemYErrIndex getUErrStart() const;
+/// Return the total number nudotErr=mp+mv+ma of cache entries for
+/// acceleration-level constraint errors (including also errors in the
+/// second time derivatives of position-level constraints and the first
+/// time derivatives of velocity-level constraints). Callable at Instance stage.
+int getNUDotErr() const;
+/// Return the total number of constraint multipliers; necessarily the same
+/// as the number of acceleration-level constraint errors nUDotErr. Callable
+/// at Instance stage.
+/// @see getNUDotErr()
+int getNMultipliers() const; // =mp+mv+ma, necessarily the same as NUDotErr
+/// Return the total number of event trigger function slots in the cache.
+/// Callable at Instance stage.
+int getNEventTriggers() const;
+/// Return the size of the partition of event trigger functions which are
+/// evaluated at a given Stage. Callable at Instance stage.
+int getNEventTriggersByStage(Stage) const;
+/// Return the index within the global event trigger array at which the
+/// first of the event triggers associated with a particular Stage are stored;
+/// the rest follow contiguously. Callable at Instance stage.
+SystemEventTriggerIndex getEventTriggerStartByStage(Stage) const; // per-stage
+
+/// @}
+
+/// @name Per-Subsystem Dimensions
+///
+/// These are the dimensions and locations within the global resource arrays
+/// of state and cache resources allocated to a particular Subsystem. Note
+/// that a Subsystem has contiguous q's, contiguous u's, and contiguous z's
+/// but that the q-, u-, and z-partitions are not contiguous. Hence there is
+/// no Subsystem equivalent of the global y vector.
+///
+/// These serve as a mapping from Subsystem-local indices for the various
+/// shared resources to their global resource indices.
+///
+/// @see Global Resource Dimensions
+/// @{
+
+SystemQIndex getQStart(SubsystemIndex) const;
+int getNQ(SubsystemIndex) const;
+SystemUIndex getUStart(SubsystemIndex) const;
+int getNU(SubsystemIndex) const;
+SystemZIndex getZStart(SubsystemIndex) const;
+int getNZ(SubsystemIndex) const;
+
+
+SystemQErrIndex getQErrStart(SubsystemIndex) const;
+int getNQErr(SubsystemIndex) const;
+SystemUErrIndex getUErrStart(SubsystemIndex) const;
+int getNUErr(SubsystemIndex) const;
+SystemUDotErrIndex getUDotErrStart(SubsystemIndex) const;
+int getNUDotErr(SubsystemIndex) const;
+SystemMultiplierIndex getMultipliersStart(SubsystemIndex) const;
+int getNMultipliers(SubsystemIndex) const;
+
+SystemEventTriggerByStageIndex
+ getEventTriggerStartByStage(SubsystemIndex, Stage) const;
+int getNEventTriggersByStage(SubsystemIndex, Stage) const;
+
+/// @}
+
+/// @name Global-to-Subsystem Maps
+///
+/// TODO -- not implemented yet.
+///
+/// Once the dimensions and allocations of the global shared resources
+/// are known, you can call these methods to map a global resource index
+/// to Subsystem to which it belongs and the index by which that resource
+/// is known locally to the Subsystem.
+///
+/// @see Global Resource Dimensions
+/// @see Per-Subsystem Dimensions group
+/// @{
+
+/// For a given global q, return the Subsystem that allocated it and the Subsystem-local
+/// index by which it is known; callable at Model stage.
+void mapQToSubsystem(SystemQIndex, SubsystemIndex&, QIndex&) const;
+/// For a given global u, return the Subsystem that allocated it and the Subsystem-local
+/// index by which it is known; callable at Model stage.
+void mapUToSubsystem(SystemUIndex, SubsystemIndex&, UIndex&) const;
+/// For a given global z, return the Subsystem that allocated it and the Subsystem-local
+/// index by which it is known; callable at Model stage.
+void mapZToSubsystem(SystemZIndex, SubsystemIndex&, ZIndex&) const;
+/// For a given global qErr index, return the Subsystem that allocated it and the Subsystem-local
+/// index by which it is known; callable at Instance stage.
+void mapQErrToSubsystem(SystemQErrIndex, SubsystemIndex&, QErrIndex&) const;
+/// For a given global uErr index, return the Subsystem that allocated it and the Subsystem-local
+/// index by which it is known; callable at Instance stage.
+void mapUErrToSubsystem(SystemUErrIndex, SubsystemIndex&, UErrIndex&) const;
+/// For a given global uDotErr index, return the Subsystem that allocated it and the Subsystem-local
+/// index by which it is known; callable at Instance stage.
+void mapUDotErrToSubsystem(SystemUDotErrIndex, SubsystemIndex&, UDotErrIndex&) const;
+/// For a given global multiplier index, return the Subsystem that allocated it and the Subsystem-local
+/// index by which it is known; callable at Instance stage. This is necessarily the same Subsystme
+/// and index as for the corresponding global uDotErr.
+void mapMultiplierToSubsystem(SystemMultiplierIndex, SubsystemIndex&, MultiplierIndex&) const;
+/// For a given global event trigger function index, return the Subsystem that allocated it and
+/// the Subsystem-local index by which it is known; callable at Instance stage.
+//void mapEventTriggerToSubsystem(SystemEventTriggerIndex, SubsystemIndex&, EventTriggerIndex&) const;
+/// For a given global event trigger function index, return the Stage at which that
+/// trigger function should be evaluated; callable at Instance stage.
+void mapEventTriggerToStage(SystemEventTriggerIndex, Stage&, SystemEventTriggerByStageIndex&) const;
+/// Given a Subsystem-wide event index, map that to a particular Stage and an index
+/// within that Stage.
+//void mapSubsystemEventTriggerToStage(EventTriggerIndex, Stage&, EventTriggerByStageIndex&) const;
+
+/// @}
+
+const Vector& getEventTriggers() const;
+const Vector& getEventTriggersByStage(Stage) const;
+const Vector& getEventTriggersByStage(SubsystemIndex, Stage) const;
+
+Vector& updEventTriggers() const; // mutable
+Vector& updEventTriggersByStage(Stage) const;
+Vector& updEventTriggersByStage(SubsystemIndex, Stage) const;
+
+/// Per-subsystem access to the global shared variables.
+const Vector& getQ(SubsystemIndex) const;
+const Vector& getU(SubsystemIndex) const;
+const Vector& getZ(SubsystemIndex) const;
+
+const Vector& getUWeights(SubsystemIndex) const;
+const Vector& getZWeights(SubsystemIndex) const;
+
+Vector& updQ(SubsystemIndex);
+Vector& updU(SubsystemIndex);
+Vector& updZ(SubsystemIndex);
+
+Vector& updUWeights(SubsystemIndex);
+Vector& updZWeights(SubsystemIndex);
+
+/// Per-subsystem access to the shared cache entries.
+const Vector& getQDot(SubsystemIndex) const;
+const Vector& getUDot(SubsystemIndex) const;
+const Vector& getZDot(SubsystemIndex) const;
+const Vector& getQDotDot(SubsystemIndex) const;
+
+Vector& updQDot(SubsystemIndex) const; // these are mutable
+Vector& updUDot(SubsystemIndex) const;
+Vector& updZDot(SubsystemIndex) const;
+Vector& updQDotDot(SubsystemIndex) const;
+
+const Vector& getQErr(SubsystemIndex) const;
+const Vector& getUErr(SubsystemIndex) const;
+const Vector& getUDotErr(SubsystemIndex) const;
+const Vector& getMultipliers(SubsystemIndex) const;
+
+const Vector& getQErrWeights(SubsystemIndex) const;
+const Vector& getUErrWeights(SubsystemIndex) const;
+
+Vector& updQErr(SubsystemIndex) const; // these are mutable
+Vector& updUErr(SubsystemIndex) const;
+Vector& updUDotErr(SubsystemIndex) const;
+Vector& updMultipliers(SubsystemIndex) const;
+
+Vector& updQErrWeights(SubsystemIndex);
+Vector& updUErrWeights(SubsystemIndex);
+
+/// You can call these as long as *system* stage >= Model.
+const Real& getTime() const;
+const Vector& getY() const; // {Q,U,Z} packed and in that order
+
+/// These are just views into Y.
+const Vector& getQ() const;
+const Vector& getU() const;
+const Vector& getZ() const;
+
+
+/** Get a unit weighting (1/unit change) for each u that can be used to
+weight a vector du so that the disparate elements are comparable in physical
+effect. This permits mixing of generalized speeds
+that have different units, and scaling of generalized speeds that have
+differing amounts of leverage due to their positions in the multibody tree.
+This can be used to create a scaled norm that represents the overall
+significance of a change du to u.
+
+Define a unit change di for each ui such that a change
+ui+eps*di to each generalized speed in turn produces a physical velocity change
+of roughly equal significance. Then a diagonal matrix Wu=diag(1/di) is
+a weighting matrix such that wdu=Wu*du is a vector in which each element wdu_i
+has units of "unit change" for its corresponding ui. This method returns a
+vector which is the diagonal of Wu.
+
+These same weights on u also determine the scaling of the generalized
+coordinates q, because q and u are related via qdot=N*u. For cases where
+qdot_i=u_i, the numerical value of the unit change to q_i is just di because
+dP/dq_i == dV/du_i. Otherwise, they are related by Wq = N*Wu*pinv(N) where
+Wq is the weighting matrix for dq (block diagonal), and pinv() is the
+pseudoinverse.
+
+For example, say you define unit scaling for an angle coordinate to be 1 radian
+(about 57 degrees), meaning that a 1 radian change of coordinate produces
+(roughly) one length unit of meaningful position change. Then if a generalized
+coordinate is measured in radians, its unit scale would be 1. If instead you
+created a generalized coordinate with units of degrees, its unit scale would
+be 57 degrees. That would allow mixing of such coordinates in the same system
+by bringing the coordinates into a physically-meaningful basis.
+Scaling is defined in the u basis where each variable is independent;
+the N matrix couples variables in the q basis. So here the units would actually
+be 1 radian/time unit and 57 degrees/time unit (numerically identical).
+
+This is allocated and set to 1 at the end of realize(Model). **/
+const Vector& getUWeights() const; // diag(Wu)
+
+/** Get a unit weighting (1/unit change) for each z that can be used to
+weight a vector dz so that the disparate elements are comparable in physical
+effect. This defines a weighting matrix Wz=diag(1/unitchange_zi) such
+that wdz=Wz*dz is a vector in which each element wdz_i has units of
+"unit change" for its corresponding zi. This method returns a
+vector which is the diagonal of Wz. **/
+const Vector& getZWeights() const;
+
+/** Set u weights (and q weights indirectly). You can call this after Model
+stage has been realized. This will invalidate just Report stage because it is
+not used in calculating udots. **/
+Vector& updUWeights();
+
+/** Set z weights. You can call this after Model stage has been realized. This
+will invalidate just Report stage because it is not used in calculating
+zdots. **/
+Vector& updZWeights();
+
+/// You can call these as long as System stage >= Model, but the
+/// stage will be backed up if necessary to the indicated stage.
+Real& updTime(); // Back up to Stage::Time-1
+Vector& updY(); // Back up to Stage::Dynamics-1
+
+/// An alternate syntax equivalent to updTime() and updY().
+void setTime(Real t);
+void setY(const Vector& y);
+
+/// These are just views into Y.
+Vector& updQ(); // Back up to Stage::Position-1
+Vector& updU(); // Back up to Stage::Velocity-1
+Vector& updZ(); // Back up to Stage::Dynamics-1
+
+/// Alternate interface.
+void setQ(const Vector& q);
+void setU(const Vector& u);
+void setZ(const Vector& z);
+
+const Vector& getYDot() const; // Stage::Acceleration
+
+/// These are just views into YDot.
+const Vector& getQDot() const; // Stage::Velocity
+const Vector& getZDot() const; // Stage::Dynamics
+const Vector& getUDot() const; // Stage::Acceleration
+
+/// This has its own space, not a view.
+const Vector& getQDotDot() const; // Stage::Acceleration
+
+/// These are mutable
+Vector& updYDot() const; // Stage::Acceleration-1
+Vector& updQDot() const; // Stage::Velocity-1 (view into YDot)
+Vector& updZDot() const; // Stage::Dynamics-1 "
+Vector& updUDot() const; // Stage::Acceleration-1 "
+
+/// This is a separate shared cache entry, not part of YDot. If you
+/// have a direct 2nd order integrator you can integrate QDotDot
+/// (twice) to get Q.
+Vector& updQDotDot() const; // Stage::Acceleration-1
+
+/// Return the current constraint errors for all constraints. This
+/// is {QErr,UErr} packed and in that order.
+const Vector& getYErr() const; // Stage::Velocity
+
+/// These are just views into YErr.
+const Vector& getQErr() const; // Stage::Position (index 3 constraints)
+const Vector& getUErr() const; // Stage::Velocity (index 2 constraints)
+
+/// These have their own space, they are not views.
+const Vector& getUDotErr() const; // Stage::Acceleration (index 1 constraints)
+const Vector& getMultipliers() const; // Stage::Acceleration
+
+/** Get the unit weighting (1/unit error) for each of the mp+mquat position
+constraints equations. Allocated and initialized to 1 on realize(Instance). **/
+const Vector& getQErrWeights() const;
+
+/** Get the unit weighting (1/unit error) for each of the mp+mv velocity-level
+constraint equations, meaning mp time derivatives of position (holonomic)
+constraint equations followed by mv velocity (nonholonomic) constraints.
+Typically the weight of position constraint derivatives is just the
+position constraint weight times the System's characteristic time scale.
+
+There is no entry corresponding to quaternions here since they do not
+produce velocity-level constraints in Simbody's forumulation.
+
+This is allocated and initialized to 1 on realize(Instance). **/
+const Vector& getUErrWeights() const;
+
+/** Set the unit weighting (1/unit error) for each of the mp+mquat position
+constraint equations. You can call this after the weight variable is allocated
+at the end of Instance stage. Position stage is invalidated to force
+recalculation of weighted position constraint errors. **/
+Vector& updQErrWeights();
+
+/** Set the unit weighting (1/unit error) for each of the mp+mv velocity-level
+constraints. You can call this after the weight variable is allocated at the
+end of Instance stage. Velocity stage is invalidated to force recalculation of
+weighted velocity-level constraint errors. **/
+Vector& updUErrWeights();
+
+/// These are mutable
+Vector& updYErr() const; // Stage::Velocity-1
+Vector& updQErr() const; // Stage::Position-1 (view into YErr)
+Vector& updUErr() const; // Stage::Velocity-1 "
+
+Vector& updUDotErr() const; // Stage::Acceleration-1 (not a view)
+Vector& updMultipliers() const; // Stage::Acceleration-1 (not a view)
+
+/** (Advanced) Record the current version numbers of each valid System-level
+stage. This can be used to unambiguously determine what stages have been
+changed by some opaque operation, even if that operation realized the stages
+after modifying them. This is particularly useful for event handlers as a way
+for a time stepper to know how much damage may have been done by a handler, and
+thus how much reinitialization is required before continuing on.
+ at see getLowestSystemStageDifference() **/
+void getSystemStageVersions(Array_<StageVersion>& versions) const;
+
+/** (Advanced) Given a list of per-stage version numbers extracted by an
+earlier call to getSystemStageVersions(), note the lowest system stage in the
+current State whose version number differs from the corresponding previous
+version number. Returns Stage::Infinity if all the stages present in
+\a prevVersions are valid and have identical versions now, even if there are
+additional valid stages now, since nothing the caller cared about before has
+been changed. If the current State is not realized as far as the previous one,
+then the first unrealized stage is returned if all the lower versions match.
+ at see getSystemStageVersions() **/
+Stage getLowestSystemStageDifference
+ (const Array_<StageVersion>& prevVersions) const;
+
+/** (Advanced) This explicitly modifies the Topology stage version; don't
+use this method unless you know what you're doing! This can be used to force
+compatibility with a System that has had Topology changes since this %State
+was created. This has no effect on the realization level.
+ at see getSystemTopologyStageVersion(), System::getSystemTopologyCacheVersion()
+**/
+void setSystemTopologyStageVersion(StageVersion topoVersion);
+
+/** (Advanced) This is called at the beginning of every integration step to set
+the values of auto-update discrete variables from the values stored in their
+associated cache entries. **/
+void autoUpdateDiscreteVariables();
+
+String toString() const;
+String cacheToString() const;
+
+//------------------------------------------------------------------------------
+ private:
+class StateImpl* impl;
+const StateImpl& getImpl() const {assert(impl); return *impl;}
+StateImpl& updImpl() {assert(impl); return *impl;}
+};
+
+SimTK_SimTKCOMMON_EXPORT std::ostream&
+operator<<(std::ostream& o, const State& s);
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_STATE_H_
diff --git a/SimTKcommon/Simulation/include/SimTKcommon/internal/Study.h b/SimTKcommon/Simulation/include/SimTKcommon/internal/Study.h
new file mode 100644
index 0000000..e3030e9
--- /dev/null
+++ b/SimTKcommon/Simulation/include/SimTKcommon/internal/Study.h
@@ -0,0 +1,106 @@
+#ifndef SimTK_SimTKCOMMON_STUDY_H_
+#define SimTK_SimTKCOMMON_STUDY_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/State.h"
+#include "SimTKcommon/internal/System.h"
+
+namespace SimTK {
+
+/* TODO: THIS CLASS IS NOT CURRENTLY USED AND IS JUST A SKELETON.
+ * PLEASE IGNORE FOR NOW.
+ *
+ * The handle class which serves as the abstract parent of all Studies.
+ *
+ * There are two distinct users of this class:
+ * - Study Users: people who are making use of a concrete Study (which will
+ * inherit methods from this class)
+ * - Study Developers: people who are writing concrete Study classes
+ *
+ * Only methods intended for Study Users and a few bookkeeping methods
+ * are in the main Study class, which is a SimTK Handle class, meaning
+ * that it consists only of a single pointer, which points to a
+ * Study::Guts class. The Guts class is abstract, and virtual methods
+ * to be implemented by Study Developers in the concrete
+ * Study are defined there, along
+ * with other utilities of use to the concrete Study Developer but
+ * not to the end user. The Guts class is declared in a separate
+ * header file, and only people who are writing their own Study
+ * classes need look there.
+ */
+
+class SimTK_SimTKCOMMON_EXPORT Study {
+public:
+ class Guts; // local; name is Study::Guts
+ friend class Guts;
+private:
+ // This is the only data member in this class. Also, any class derived from
+ // Study must have *NO* data members at all (data goes in the Guts class).
+ Guts* guts;
+public:
+ Study() : guts(0) { }
+ Study(const Study&);
+ Study& operator=(const Study&);
+ ~Study();
+
+ explicit Study(const System& sys);
+
+ const String& getName() const;
+ const String& getVersion() const;
+
+ const System& getSystem() const;
+ const State& getState() const;
+ State& updState();
+
+ /// Is this handle the owner of this rep? This is true if the
+ /// handle is empty or if its rep points back here.
+ bool isOwnerHandle() const;
+ bool isEmptyHandle() const;
+
+
+ // There can be multiple handles on the same Study.
+ bool isSameStudy(const Study& otherStudy) const;
+
+ // Internal use only
+
+ // dynamic_cast the returned reference to a reference to your concrete Guts
+ // class.
+ const Study::Guts& getStudyGuts() const {assert(guts); return *guts;}
+ Study::Guts& updStudyGuts() {assert(guts); return *guts;}
+
+ // Put new *unowned* Guts into this *empty* handle and take over ownership.
+ // If this handle is already in use, or if Guts is already owned this
+ // routine will throw an exception.
+ void adoptStudyGuts(Study::Guts* g);
+
+ explicit Study(Study::Guts* g) : guts(g) { }
+ bool hasGuts() const {return guts!=0;}
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_STUDY_H_
diff --git a/SimTKcommon/Simulation/include/SimTKcommon/internal/StudyGuts.h b/SimTKcommon/Simulation/include/SimTKcommon/internal/StudyGuts.h
new file mode 100644
index 0000000..24016d6
--- /dev/null
+++ b/SimTKcommon/Simulation/include/SimTKcommon/internal/StudyGuts.h
@@ -0,0 +1,112 @@
+#ifndef SimTK_SimTKCOMMON_STUDY_GUTS_H_
+#define SimTK_SimTKCOMMON_STUDY_GUTS_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/State.h"
+
+namespace SimTK {
+
+// TODO: more to come.
+
+/**
+ * This is the declaration for the Study::Guts class, the abstract object to
+ * which a Study handle points. This is in a separate header file from Study
+ * because only people who are extending the Study class to make their own
+ * Studies need to be aware of the details. End users access only methods from
+ * the Study class and classes derived from Study, never anything from
+ * Study::Guts or its derived classes.
+ *
+ * Below is the physical layout of memory for a Study, and which
+ * portions are allocated by the client program and which by the
+ * binary library code. For binary compatiblity, only the side
+ * which allocated a piece of memory can access it. Exception: both
+ * the client and library side must agree on the virtual function
+ * table (VFT) ordering of the client's virtual functions.
+ * @verbatim
+ * CLIENT SIDE . LIBRARY SIDE
+ * .
+ * Study Study::Guts . Study::Guts::GutsRep
+ * --------------- ------------------ . -------------
+ * | Study::Guts* | --> | Study::GutsRep* | --> | GutsRep |
+ * --------------- ------------------ . | |
+ * ^ | Concrete Guts | . | Other opaque|
+ * | | class data and | . | stuff |
+ * =============== | client-side VFT | . | |
+ * Concrete Study ------------------ . | |
+ * adds no data . -------------
+ * members
+ * @endverbatim
+ *
+ * If the concrete Study::Guts class also has an opaque implementation,
+ * as it will for concrete Studies provided by the SimTK Core, then
+ * the Study author should expose only the data-free handle class
+ * derived from Study.
+ */
+class SimTK_SimTKCOMMON_EXPORT Study::Guts {
+ class GutsRep;
+ friend class GutsRep;
+
+ // This is the only data member in this class.
+ GutsRep* rep; // opaque implementation of Study::Guts base class.
+public:
+ // Note that this serves as a default constructor since both arguments have defaults.
+ explicit Guts(const String& name="<UNNAMED STUDY>",
+ const String& version="0.0.0");
+ virtual ~Guts();
+
+ const String& getName() const;
+ const String& getVersion() const;
+
+ // Obtain the owner handle for this Study::Guts object.
+ const Study& getStudy() const;
+ Study& updStudy();
+
+ void setOwnerHandle(Study&);
+ bool hasOwnerHandle() const;
+
+ explicit Guts(class GutsRep* r) : rep(r) { }
+ bool hasRep() const {return rep!=0;}
+ const GutsRep& getRep() const {assert(rep); return *rep;}
+ GutsRep& updRep() const {assert(rep); return *rep;}
+
+ // Wrap the cloneImpl virtual method.
+ Study::Guts* clone() const;
+
+protected:
+ Guts(const Guts&); // copies the base class; for use from derived class copy constructors
+
+ // The destructor is already virtual; see above.
+
+ virtual Study::Guts* cloneImpl() const = 0;
+
+private:
+ Guts& operator=(const Guts&); // suppress default copy assignment operator
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_STUDY_GUTS_H_
diff --git a/SimTKcommon/Simulation/include/SimTKcommon/internal/Subsystem.h b/SimTKcommon/Simulation/include/SimTKcommon/internal/Subsystem.h
new file mode 100644
index 0000000..ee2707d
--- /dev/null
+++ b/SimTKcommon/Simulation/include/SimTKcommon/internal/Subsystem.h
@@ -0,0 +1,256 @@
+#ifndef SimTK_SimTKCOMMON_SUBSYSTEM_H_
+#define SimTK_SimTKCOMMON_SUBSYSTEM_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/State.h"
+#include "SimTKcommon/internal/Measure.h"
+
+#include <cassert>
+
+namespace SimTK {
+
+class System;
+
+
+/**
+ * The abstract parent of all Subsystems.
+ *
+ * A Subsystem is expected to be part of a larger System and to have
+ * interdependencies with other subsystems of that same system. It
+ * must NOT have dependencies on objects which are outside the System.
+ * Consequently construction of any concrete subsystem requires
+ * specification of a system at that time.
+ * Subsystems go through an extended construction phase in which
+ * their contents and interdependencies are created. Thus all
+ * of a System's Subsystems generally need to be available simultaneously
+ * during construction, so that they can reference each other.
+ *
+ * There are three distinct users of this class:
+ * - the System class
+ * - the concrete Subsystems derived from this class
+ * - the end user of a concrete Subsystem
+ * Only end user methods are public here. Methods intended for
+ * use by the concrete Subsystem class can be found in the Subsystem::Guts
+ * class which is defined in a separate header file. End users need not
+ * look over there -- trust me, you'll find it disturbing if you do!
+ */
+class SimTK_SimTKCOMMON_EXPORT Subsystem {
+public:
+ class Guts; // local; name is Subsystem::Guts
+ friend class Guts;
+private:
+ // This is the only data member in this class. Also, any class derived from
+ // Subsystem must have *NO* data members at all (data goes in the Guts class).
+ Guts* guts;
+public:
+ Subsystem() : guts(0) { } // an empty handle
+ Subsystem(const Subsystem&);
+ Subsystem& operator=(const Subsystem&);
+ ~Subsystem();
+
+ const String& getName() const;
+ const String& getVersion() const;
+
+ // These call the corresponding State method, supplying this Subsystem's
+ // SubsystemIndex. The returned indices are local to this Subsystem.
+ QIndex allocateQ(State&, const Vector& qInit) const;
+ UIndex allocateU(State&, const Vector& uInit) const;
+ ZIndex allocateZ(State&, const Vector& zInit) const;
+
+ DiscreteVariableIndex allocateDiscreteVariable
+ (State&, Stage invalidates, AbstractValue* v) const;
+ DiscreteVariableIndex allocateAutoUpdateDiscreteVariable
+ (State&, Stage invalidates, AbstractValue* v, Stage updateDependsOn) const;
+
+ CacheEntryIndex allocateCacheEntry
+ (const State&, Stage dependsOn, Stage computedBy, AbstractValue* v) const;
+ CacheEntryIndex allocateCacheEntry
+ (const State& state, Stage g, AbstractValue* v) const
+ { return allocateCacheEntry(state, g, g, v); }
+ CacheEntryIndex allocateLazyCacheEntry
+ (const State& state, Stage earliest, AbstractValue* v) const
+ { return allocateCacheEntry(state, earliest, Stage::Infinity, v); }
+
+ QErrIndex allocateQErr (const State&, int nqerr) const;
+ UErrIndex allocateUErr (const State&, int nuerr) const;
+ UDotErrIndex allocateUDotErr (const State&, int nudoterr) const;
+ EventTriggerByStageIndex allocateEventTriggersByStage
+ (const State&, Stage, int ntriggers) const;
+
+ // These return views on State shared global resources. The views
+ // are private to this subsystem, but the global resources themselves
+ // are not allocated until the *System* advances to stage Model.
+ // Note that there is no subsystem equivalent of the State's "y"
+ // vector because in general a subsystem's state variables will
+ // not be contiguous. However, a subsystem's q's, u's, and z's
+ // will all be contiguous within those arrays.
+ const Vector& getQ(const State&) const;
+ const Vector& getU(const State&) const;
+ const Vector& getZ(const State&) const;
+ const Vector& getQDot(const State&) const;
+ const Vector& getUDot(const State&) const;
+ const Vector& getZDot(const State&) const;
+ const Vector& getQDotDot(const State&) const;
+ const Vector& getQErr(const State&) const;
+ const Vector& getUErr(const State&) const;
+ const Vector& getUDotErr(const State&) const;
+ const Vector& getMultipliers(const State&) const;
+ const Vector& getEventTriggersByStage(const State&, Stage) const;
+
+ // These return writable access to this subsystem's partition in the
+ // State pool of continuous variables. These can be called at Stage::Model
+ // or higher, and if necesary they invalidate the Position (q), Velocity (u),
+ // or Dynamics (z) stage respectively.
+ Vector& updQ(State&) const; // invalidates Stage::Position
+ Vector& updU(State&) const; // invalidates Stage::Velocity
+ Vector& updZ(State&) const; // invalidates Stage::Dynamics
+
+ // For convenience.
+ void setQ(State& s, const Vector& q) const {
+ assert(q.size() == getNQ(s));
+ updQ(s) = q;
+ }
+ void setU(State& s, const Vector& u) const {
+ assert(u.size() == getNU(s));
+ updU(s) = u;
+ }
+ void setZ(State& s, const Vector& z) const {
+ assert(z.size() == getNZ(s));
+ updZ(s) = z;
+ }
+
+ // These update the State cache which is mutable; hence, const State. They
+ // can be called only if the previous stage has already been realized, e.g.,
+ // updQDot() is allowed only while realizing the Velocity stage, requiring
+ // that Position stage has already been realized.
+ Vector& updQDot(const State&) const;
+ Vector& updUDot(const State&) const;
+ Vector& updZDot(const State&) const;
+ Vector& updQDotDot(const State&) const;
+ Vector& updQErr(const State&) const;
+ Vector& updUErr(const State&) const;
+ Vector& updUDotErr(const State&) const;
+ Vector& updMultipliers(const State&) const;
+ Vector& updEventTriggersByStage(const State&, Stage) const;
+
+ // These pull out the State entries which belong exclusively to
+ // this Subsystem. These variables and cache entries are available
+ // as soon as this subsystem is at stage Model.
+ Stage getStage(const State&) const;
+ const AbstractValue& getDiscreteVariable(const State& s, DiscreteVariableIndex dx) const;
+
+ Real getDiscreteVarLastUpdateTime(const State& s, DiscreteVariableIndex dx) const
+ { return s.getDiscreteVarLastUpdateTime(getMySubsystemIndex(),dx); }
+ CacheEntryIndex getDiscreteVarUpdateIndex(const State& s, DiscreteVariableIndex dx) const
+ { return s.getDiscreteVarUpdateIndex(getMySubsystemIndex(),dx); }
+ const AbstractValue& getDiscreteVarUpdateValue(const State& s, DiscreteVariableIndex dx) const
+ { return s.getDiscreteVarUpdateValue(getMySubsystemIndex(),dx); }
+ AbstractValue& updDiscreteVarUpdateValue(const State& s, DiscreteVariableIndex dx) const
+ { return s.updDiscreteVarUpdateValue(getMySubsystemIndex(),dx); }
+ bool isDiscreteVarUpdateValueRealized(const State& s, DiscreteVariableIndex dx) const
+ { return s.isDiscreteVarUpdateValueRealized(getMySubsystemIndex(),dx); }
+ void markDiscreteVarUpdateValueRealized(const State& s, DiscreteVariableIndex dx) const
+ { return s.markDiscreteVarUpdateValueRealized(getMySubsystemIndex(),dx); }
+
+ // State is *not* mutable here -- must have write access to change state variables.
+ AbstractValue& updDiscreteVariable(State&, DiscreteVariableIndex) const;
+
+ const AbstractValue& getCacheEntry(const State&, CacheEntryIndex) const;
+ // State is mutable here.
+ AbstractValue& updCacheEntry(const State&, CacheEntryIndex) const;
+
+ bool isCacheValueRealized(const State&, CacheEntryIndex) const;
+ void markCacheValueRealized(const State&, CacheEntryIndex) const;
+ void markCacheValueNotRealized(const State&, CacheEntryIndex) const;
+
+ // Dimensions. These are valid at System Stage::Model while access to the
+ // various arrays may have stricter requirements. Hence it is better to use
+ // these routines than to get a reference to a Vector above and ask for
+ // its size().
+
+ SystemQIndex getQStart (const State&) const;
+ int getNQ (const State&) const;
+ SystemUIndex getUStart (const State&) const;
+ int getNU (const State&) const;
+ SystemZIndex getZStart (const State&) const;
+ int getNZ (const State&) const;
+ SystemQErrIndex getQErrStart (const State&) const;
+ int getNQErr (const State&) const;
+ SystemUErrIndex getUErrStart (const State&) const;
+ int getNUErr (const State&) const;
+ SystemUDotErrIndex getUDotErrStart(const State&) const;
+ int getNUDotErr (const State&) const;
+ SystemMultiplierIndex getMultipliersStart (const State&) const;
+ int getNMultipliers (const State&) const;
+ SystemEventTriggerByStageIndex getEventTriggerStartByStage(const State&, Stage) const;
+ int getNEventTriggersByStage (const State&, Stage) const;
+
+ bool isInSystem() const;
+ bool isInSameSystem(const Subsystem& otherSubsystem) const;
+
+ const System& getSystem() const;
+ System& updSystem();
+
+ SubsystemIndex getMySubsystemIndex() const;
+
+ // Is this handle the owner of this rep? This is true if the
+ // handle is empty or if its rep points back here.
+ bool isOwnerHandle() const;
+ bool isEmptyHandle() const;
+
+ // There can be multiple handles on the same Subsystem.
+ bool isSameSubsystem(const Subsystem& otherSubsystem) const;
+
+ bool subsystemTopologyHasBeenRealized() const;
+ void invalidateSubsystemTopologyCache() const;
+
+ // Add a new Measure to this Subsystem. This method is generally used by Measure
+ // constructors to install a newly-constructed Measure into its Subsystem.
+ MeasureIndex adoptMeasure(AbstractMeasure&);
+
+ AbstractMeasure getMeasure(MeasureIndex) const;
+ template <class T> Measure_<T> getMeasure_(MeasureIndex mx) const
+ { return Measure_<T>::getAs(getMeasure(mx));}
+
+ // dynamic_cast the returned reference to a reference to your concrete Guts
+ // class.
+ const Subsystem::Guts& getSubsystemGuts() const {assert(guts); return *guts;}
+ Subsystem::Guts& updSubsystemGuts() {assert(guts); return *guts;}
+
+ // Put new Guts into this *empty* handle and take over ownership.
+ // If this handle is already in use, this routine will throw
+ // an exception.
+ void adoptSubsystemGuts(Subsystem::Guts* g);
+ void setSystem(System&, SubsystemIndex);
+
+ explicit Subsystem(Subsystem::Guts* g) : guts(g) { }
+ bool hasGuts() const {return guts!=0;}
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_SUBSYSTEM_H_
diff --git a/SimTKcommon/Simulation/include/SimTKcommon/internal/SubsystemGuts.h b/SimTKcommon/Simulation/include/SimTKcommon/internal/SubsystemGuts.h
new file mode 100644
index 0000000..00288b8
--- /dev/null
+++ b/SimTKcommon/Simulation/include/SimTKcommon/internal/SubsystemGuts.h
@@ -0,0 +1,348 @@
+#ifndef SimTK_SimTKCOMMON_SUBSYSTEM_GUTS_H_
+#define SimTK_SimTKCOMMON_SUBSYSTEM_GUTS_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/State.h"
+
+#include <cassert>
+
+namespace SimTK {
+
+class System;
+class DecorativeGeometry;
+
+
+/**
+ * The abstract parent of all Subsystem "Guts" implementation classes.
+ */
+class SimTK_SimTKCOMMON_EXPORT Subsystem::Guts {
+public:
+ Guts(const Guts&);
+ Guts& operator=(const Guts&);
+
+ // This constructor is for use by concrete Subsystems. Note that this
+ // serves as a default constructor since both arguments have defaults.
+ explicit Guts(const String& name="<NONAME>",
+ const String& version="0.0.0");
+ virtual ~Guts();
+
+ const String& getName() const;
+ const String& getVersion() const;
+
+ // Use these to allocate state variables and cache entries that are owned
+ // by this Subsystem.
+
+ // qdot, qdotdot also allocated in cache
+ QIndex allocateQ(State& s, const Vector& qInit) const;
+ // udot is also allocated in the cache
+ UIndex allocateU(State& s, const Vector& uInit) const;
+ // zdot is also allocated in the cache
+ ZIndex allocateZ(State& s, const Vector& zInit) const;
+
+ DiscreteVariableIndex allocateDiscreteVariable
+ (State& s, Stage g, AbstractValue* v) const;
+ DiscreteVariableIndex allocateAutoUpdateDiscreteVariable
+ (State&, Stage invalidates, AbstractValue* v, Stage updateDependsOn) const;
+
+ // Cache entries
+ CacheEntryIndex allocateCacheEntry
+ (const State&, Stage dependsOn, Stage computedBy, AbstractValue* v) const;
+ CacheEntryIndex allocateCacheEntry
+ (const State& state, Stage g, AbstractValue* v) const
+ { return allocateCacheEntry(state, g, g, v); }
+ CacheEntryIndex allocateLazyCacheEntry
+ (const State& state, Stage earliest, AbstractValue* v) const
+ { return allocateCacheEntry(state, earliest, Stage::Infinity, v); }
+
+ // qerr, uerr, udoterr are all cache entries, not variables
+ // allocating udoterr also allocates matching multipliers
+ QErrIndex allocateQErr(const State& s, int nqerr) const;
+ UErrIndex allocateUErr(const State& s, int nuerr) const;
+ UDotErrIndex allocateUDotErr(const State& s, int nudoterr) const;
+ EventTriggerByStageIndex allocateEventTriggersByStage
+ (const State&, Stage, int ntriggers) const;
+
+ // These return views on State shared global resources. The views
+ // are private to this subsystem, but the global resources themselves
+ // are not allocated until the *System* advances to stage Model.
+ // Note that there is no subsystem equivalent of the State's "y"
+ // vector because in general a subsystem's state variables will
+ // not be contiguous. However, a subsystem's q's, u's, and z's
+ // will all be contiguous within those arrays.
+ const Vector& getQ(const State&) const;
+ const Vector& getU(const State&) const;
+ const Vector& getZ(const State&) const;
+ const Vector& getUWeights(const State&) const;
+ const Vector& getZWeights(const State&) const;
+
+ const Vector& getQDot(const State&) const;
+ const Vector& getUDot(const State&) const;
+ const Vector& getZDot(const State&) const;
+ const Vector& getQDotDot(const State&) const;
+
+ const Vector& getQErr(const State&) const;
+ const Vector& getUErr(const State&) const;
+ const Vector& getQErrWeights(const State&) const;
+ const Vector& getUErrWeights(const State&) const;
+
+ const Vector& getUDotErr(const State&) const;
+ const Vector& getMultipliers(const State&) const;
+ const Vector& getEventTriggersByStage(const State&, Stage) const;
+
+ // These return writable access to this subsystem's partition in the
+ // State pool of continuous variables. These can be called at Stage::Model
+ // or higher, and if necesary they invalidate the Position (q), Velocity (u),
+ // or Dynamics (z) stage respectively.
+ Vector& updQ(State&) const; // invalidates Stage::Position
+ Vector& updU(State&) const; // invalidates Stage::Velocity
+ Vector& updZ(State&) const; // invalidates Stage::Dynamics
+
+ // For convenience.
+ void setQ(State& s, const Vector& q) const {
+ assert(q.size() == getNQ(s));
+ updQ(s) = q;
+ }
+ void setU(State& s, const Vector& u) const {
+ assert(u.size() == getNU(s));
+ updU(s) = u;
+ }
+ void setZ(State& s, const Vector& z) const {
+ assert(z.size() == getNZ(s));
+ updZ(s) = z;
+ }
+
+ // These update the State cache which is mutable; hence, const State. They
+ // can be called only if the previous stage has already been realized, e.g.,
+ // updQDot() is allowed only while realizing the Velocity stage, requiring
+ // that Position stage has already been realized.
+ Vector& updQDot(const State&) const;
+ Vector& updUDot(const State&) const;
+ Vector& updZDot(const State&) const;
+ Vector& updQDotDot(const State&) const;
+ Vector& updQErr(const State&) const;
+ Vector& updUErr(const State&) const;
+ Vector& updUDotErr(const State&) const;
+ Vector& updMultipliers(const State&) const;
+ Vector& updEventTriggersByStage(const State&, Stage) const;
+
+ // These pull out the State entries which belong exclusively to
+ // this Subsystem. These variables and cache entries are available
+ // as soon as this subsystem is at stage Model.
+ Stage getStage(const State&) const;
+ const AbstractValue& getDiscreteVariable(const State&, DiscreteVariableIndex) const;
+
+ Real getDiscreteVarLastUpdateTime(const State& s, DiscreteVariableIndex dx) const
+ { return s.getDiscreteVarLastUpdateTime(getMySubsystemIndex(),dx); }
+ CacheEntryIndex getDiscreteVarUpdateIndex(const State& s, DiscreteVariableIndex dx) const
+ { return s.getDiscreteVarUpdateIndex(getMySubsystemIndex(),dx); }
+ const AbstractValue& getDiscreteVarUpdateValue(const State& s, DiscreteVariableIndex dx) const
+ { return s.getDiscreteVarUpdateValue(getMySubsystemIndex(),dx); }
+ AbstractValue& updDiscreteVarUpdateValue(const State& s, DiscreteVariableIndex dx) const
+ { return s.updDiscreteVarUpdateValue(getMySubsystemIndex(),dx); }
+ bool isDiscreteVarUpdateValueRealized(const State& s, DiscreteVariableIndex dx) const
+ { return s.isDiscreteVarUpdateValueRealized(getMySubsystemIndex(),dx); }
+ void markDiscreteVarUpdateValueRealized(const State& s, DiscreteVariableIndex dx) const
+ { return s.markDiscreteVarUpdateValueRealized(getMySubsystemIndex(),dx); }
+
+ // State is *not* mutable here -- must have write access to change state variables.
+ AbstractValue& updDiscreteVariable(State&, DiscreteVariableIndex) const;
+ const AbstractValue& getCacheEntry(const State&, CacheEntryIndex) const;
+ // State is mutable here.
+ AbstractValue& updCacheEntry(const State&, CacheEntryIndex) const;
+
+ bool isCacheValueRealized(const State&, CacheEntryIndex) const;
+ void markCacheValueRealized(const State&, CacheEntryIndex) const;
+ void markCacheValueNotRealized(const State&, CacheEntryIndex) const;
+
+ // Dimensions. These are valid at System Stage::Model while access to the various
+ // arrays may have stricter requirements. Hence it is better to use these
+ // routines than to get a reference to a Vector above and ask for its size().
+
+ SystemQIndex getQStart (const State&) const;
+ int getNQ (const State&) const;
+ SystemUIndex getUStart (const State&) const;
+ int getNU (const State&) const;
+ SystemZIndex getZStart (const State&) const;
+ int getNZ (const State&) const;
+ SystemQErrIndex getQErrStart (const State&) const;
+ int getNQErr (const State&) const;
+ SystemUErrIndex getUErrStart (const State&) const;
+ int getNUErr (const State&) const;
+ SystemUDotErrIndex getUDotErrStart(const State&) const;
+ int getNUDotErr (const State&) const;
+ SystemMultiplierIndex getMultipliersStart(const State&) const;
+ int getNMultipliers(const State&) const;
+ SystemEventTriggerByStageIndex
+ getEventTriggerStartByStage(const State&, Stage) const;
+ int getNEventTriggersByStage(const State&, Stage) const;
+
+ MeasureIndex adoptMeasure(AbstractMeasure& m);
+ AbstractMeasure getMeasure(MeasureIndex) const;
+ template <class T> Measure_<T> getMeasure_(MeasureIndex mx) const
+ { return Measure_<T>::getAs(getMeasure(mx));}
+
+ bool isInSystem() const;
+ bool isInSameSystem(const Subsystem& otherSubsystem) const;
+
+ const System& getSystem() const;
+ System& updSystem();
+
+ SubsystemIndex getMySubsystemIndex() const;
+
+ // Internal use only
+ const Subsystem& getOwnerSubsystemHandle() const;
+ Subsystem& updOwnerSubsystemHandle();
+ void setOwnerSubsystemHandle(Subsystem&);
+ bool hasOwnerSubsystemHandle() const;
+
+ void setSystem(System&, SubsystemIndex);
+
+ class GutsRep;
+ explicit Guts(GutsRep* r) : rep(r) { }
+ bool hasRep() const {return rep!=0;}
+ const GutsRep& getRep() const {assert(rep); return *rep;}
+ GutsRep& updRep() const {assert(rep); return *rep;}
+ void setRep(GutsRep& r) {assert(!rep); rep = &r;}
+
+ bool subsystemTopologyHasBeenRealized() const;
+ void invalidateSubsystemTopologyCache() const;
+
+ // These are wrappers for the virtual methods defined below. They
+ // are used to ensure good behavior. Most of them deal automatically with
+ // the Subsystem's Measures, as well as invoking the corresponding virtual
+ // for the Subsystem's own processing.
+
+ Subsystem::Guts* clone() const;
+
+ // Realize this subsystem's part of the State from Stage-1 to Stage
+ // for the indicated stage. After doing some checking, these routines
+ // call the concrete subsystem's corresponding virtual method, and
+ // on return they make sure the stage has been properly updated.
+ // Note that these will do nothing if the Subsystem stage is already
+ // at or greater than the indicated stage.
+ void realizeSubsystemTopology (State&) const;
+ void realizeSubsystemModel (State&) const;
+ void realizeSubsystemInstance (const State&) const;
+ void realizeSubsystemTime (const State&) const;
+ void realizeSubsystemPosition (const State&) const;
+ void realizeSubsystemVelocity (const State&) const;
+ void realizeSubsystemDynamics (const State&) const;
+ void realizeSubsystemAcceleration(const State&) const;
+ void realizeSubsystemReport (const State&) const;
+
+ // Generate decorative geometry computable at a specific stage. This will
+ // throw an exception if this subsystem's state hasn't already been realized
+ // to that stage. Note that the list is not inclusive -- you have to
+ // request geometry from each stage to get all of it.
+ // The generated geometry will be *appended* to the supplied output vector.
+ void calcDecorativeGeometryAndAppend
+ (const State&, Stage, Array_<DecorativeGeometry>&) const;
+
+ void createScheduledEvent(const State& state, EventId& eventId) const;
+ void createTriggeredEvent(const State& state, EventId& eventId,
+ EventTriggerByStageIndex& triggerFunctionIndex,
+ Stage stage) const;
+
+ // These methods are called by the corresponding methods of System.
+ // Each subsystem is responsible for defining its own events, and
+ // System then combines the information from them, and dispatches events
+ // to the appropriate subsystems for handling when they occur.
+ void calcEventTriggerInfo
+ (const State&, Array_<EventTriggerInfo>&) const;
+ void calcTimeOfNextScheduledEvent
+ (const State&, Real& tNextEvent, Array_<EventId>& eventIds,
+ bool includeCurrentTime) const;
+ void calcTimeOfNextScheduledReport
+ (const State&, Real& tNextEvent, Array_<EventId>& eventIds,
+ bool includeCurrentTime) const;
+ void handleEvents
+ (State&, Event::Cause, const Array_<EventId>& eventIds,
+ const HandleEventsOptions& options, HandleEventsResults& results) const;
+ void reportEvents
+ (const State&, Event::Cause, const Array_<EventId>& eventIds) const;
+
+protected:
+ // These virtual methods should be overridden in concrete Subsystems as
+ // necessary. They should never be called directly; instead call the
+ // wrapper routines above, which have the same name but without the "Impl"
+ // (implementation) at the end.
+
+ // The "realize..." wrappers will call the "realize...Impl" methods below
+ // only when the current stage for the Subsystem is the one just prior
+ // to the stage being realized. For example, realizeSubsystemVelocityImpl()
+ // is called by realizeSubsystemVelocity() only when the passed-in State
+ // shows this subsystem's stage to be exactly Stage::Position.
+ //
+ // The default implementations provided here do nothing. That means the
+ // wrappers will simply check that the current stage is correct and
+ // advance it if necessary.
+
+ // The destructor is already virtual; see above.
+
+ virtual Subsystem::Guts* cloneImpl() const = 0;
+
+ virtual int realizeSubsystemTopologyImpl(State& s) const {return 0;}
+ virtual int realizeSubsystemModelImpl (State& s) const {return 0;}
+ virtual int realizeSubsystemInstanceImpl(const State& s) const {return 0;}
+ virtual int realizeSubsystemTimeImpl (const State& s) const {return 0;}
+ virtual int realizeSubsystemPositionImpl(const State& s) const {return 0;}
+ virtual int realizeSubsystemVelocityImpl(const State& s) const {return 0;}
+ virtual int realizeSubsystemDynamicsImpl(const State& s) const {return 0;}
+ virtual int realizeSubsystemAccelerationImpl(const State& s)const{return 0;}
+ virtual int realizeSubsystemReportImpl (const State& s) const {return 0;}
+
+ virtual int calcDecorativeGeometryAndAppendImpl
+ (const State&, Stage, Array_<DecorativeGeometry>&) const {return 0;}
+
+ virtual void calcEventTriggerInfoImpl
+ (const State&, Array_<EventTriggerInfo>&) const {}
+ virtual void calcTimeOfNextScheduledEventImpl
+ (const State&, Real& tNextEvent, Array_<EventId>& eventIds,
+ bool includeCurrentTime) const {}
+ virtual void calcTimeOfNextScheduledReportImpl
+ (const State&, Real& tNextEvent, Array_<EventId>& eventIds,
+ bool includeCurrentTime) const {}
+ virtual void handleEventsImpl
+ (State&, Event::Cause, const Array_<EventId>& eventIds,
+ const HandleEventsOptions& options,
+ HandleEventsResults& results) const {}
+ virtual void reportEventsImpl
+ (const State&, Event::Cause, const Array_<EventId>& eventIds) const {}
+
+protected:
+ void advanceToStage(const State& s, Stage g) const;
+
+private:
+ // this is the only data member in the base class
+ GutsRep* rep; // opaque implementation of Subsystem::Guts base class.
+
+friend class GutsRep;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_SUBSYSTEM_GUTS_H_
diff --git a/SimTKcommon/Simulation/include/SimTKcommon/internal/System.h b/SimTKcommon/Simulation/include/SimTKcommon/internal/System.h
new file mode 100644
index 0000000..761a715
--- /dev/null
+++ b/SimTKcommon/Simulation/include/SimTKcommon/internal/System.h
@@ -0,0 +1,1178 @@
+#ifndef SimTK_SimTKCOMMON_SYSTEM_H_
+#define SimTK_SimTKCOMMON_SYSTEM_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * 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/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/State.h"
+#include "SimTKcommon/internal/Subsystem.h"
+
+#include <cassert>
+
+namespace SimTK {
+
+class DecorativeGeometry;
+class DefaultSystemSubsystem;
+class ScheduledEventHandler;
+class ScheduledEventReporter;
+class TriggeredEventHandler;
+class TriggeredEventReporter;
+class RealizeOptions;
+class RealizeResults;
+class ProjectOptions;
+class ProjectResults;
+
+//==============================================================================
+// SYSTEM
+//==============================================================================
+/** This is the base class that serves as the parent of all SimTK %System
+objects; most commonly Simbody's MultibodySystem class. %System is also the
+object type on which the Simbody numerical integrators operate, and many of
+the methods provided here are primarily for use by integrators.
+
+A %System serves as a mediator for a group of interacting Subsystem objects.
+All will share a single system State, and typically subsystems will need access
+to content in the state which is managed by other subsystems. A %System provides
+a unique SubsystemIndex (a small positive integer) for each
+of its subsystems, and the subsystems are constructed knowing their indices.
+The indices are used subsequently by the subsystems to find their own entries
+in the system state, and by each subsystem to refer to others within the same
+system. Index 0 is reserved for use by the %System itself, e.g. for
+system-global state variables, which are maintained in subsystem 0.
+
+Concrete %Systems understand the kinds of subsystems they contain. For example,
+a MultibodySystem might contain a mechanical subsystem, some force subsystems,
+and a geometry subsystem. At each computation stage, a subsystem is realized
+in a single operation. That operation can refer to computations from
+already-realized subsystems, but cannot initiate computation in other
+subsystems. The %System must know the proper order in which to realize the
+subsystems at each stage, and that ordering is likely to vary with stage. For
+example, at Position stage the mechanical positions must be realized before
+the configuration-dependent force elements. However, at Acceleration stage,
+the force elements must be realized before the mechanical accelerations can
+be calculated.
+
+There are three distinct users of this class:
+ - %System Users: people who are making use of a concrete %System (which will
+ inherit methods from this class).
+ - Numerical integrators: TimeStepper and Integrator objects requires a
+ %System object whose State can be advanced through time.
+ - %System Developers: people who are writing concrete System classes.
+
+Note that %System Users may include people who are writing studies, reporters,
+modelers and so on as well as end users who are accessing the %System directly.
+
+Methods intended for %System Users, numerical integrators, and a few bookkeeping
+methods are in the main %System class, which is a SimTK Handle class. A Handle
+class consists only of a single pointer, which in this case points to a
+System::Guts object. The System::Guts class is abstract, and virtual methods to
+be implemented by %System Developers in the concrete %System are defined there,
+along with other utilities of use to the concrete %System Developer but not to
+the end user or numerical integrator. The System::Guts class is declared
+in a separate header file, and only people who are writing their own %System
+classes need look there. **/
+class SimTK_SimTKCOMMON_EXPORT System {
+public:
+class Guts; // local; name is System::Guts
+
+
+//------------------------------------------------------------------------------
+/**@name System-wide properties
+
+These methods set parameters that provide %System-level defaults that can be
+used by various algorithms. This includes nominal time and length scales, and
+hints that can be useful when visualizing this %System (for example, does
+it make sense to show a ground plane?). There are defaults for all of these
+parameters so you don't need to set them unless the defaults are not
+satisfactory. **/
+/**@{**/
+
+/** This is a hint to other software as to which way this System's designer
+considers to be "up". For visualization, this is the best direction to use as
+the default up direction for the camera, and the opposite direction is likely to
+be a good direction in which to apply gravitational forces. The default up
+direction is +YAxis, which is the same as the OpenGL convention for the camera
+up direction. You can set this to any of the coordinate axes in the positive or
+negative direction. For example, use setUpDirection(ZAxis) for the "virtual
+world" convention where ground is the x-y plane and setUpDirection(-ZAxis)
+might be appropriate for an aviation convention in which Z is directed downward.
+A visualizer that is showing a ground plane should make the ground plane normal
+be this up direction.
+ at see setUseUniformBackground() **/
+System& setUpDirection(const CoordinateDirection& up);
+
+/** This is a hint to visualization software that this System is best viewed
+against a uniform background (e.g.\ all white) rather than against a ground
+plane. A molecular system will typically set this flag so that the visualizer
+will not attempt to place the molecule on the ground. The default is to
+consider this system best viewed with a ground plane displayed, perpendicular
+to the "up" direction and located at a height of zero.
+ at see setUpDirection() **/
+System& setUseUniformBackground(bool useUniformBackground);
+
+/** (Advanced) This is a hint used for some default behaviors, such as
+determining an initial step size for an integrator, or the default unit error
+for a constraint error derivative from the original constraint. Most users can
+ignore this and just take the default.
+
+This should be set to roughly the time scale at which you expect to see
+interesting things happen, that is the scale at which you might choose to view
+reporting output. An orbital simulation using seconds as time units might set
+this to 10 or 100s, for example, while a biomechanical simulation could use
+0.1s. This will affect the time scale on which velocity constraints are
+stabilized, with longer time scales being more demanding since there is more
+time to drift. By default this is 0.1 time units, so 100ms for systems measuring
+time in seconds and 100fs for systems measuring time in ps. **/
+System& setDefaultTimeScale(Real tc);
+
+/** (Advanced) This is a hint that can be used to get a sense of what a "unit
+length" looks like for this System in the units it uses. Most users can ignore
+this and just take the default.
+
+For example, a model of a small toy car expressed in MKS units might set this to
+0.01 (1 cm). The default for this is 1 length unit, meaning 1 meter in MKS and 1
+nm in MD units. **/
+System& setDefaultLengthScale(Real lc);
+
+/** This determines whether this System wants to be notified whenever time
+advances irreversibly. If set true, time advancement is treated as an
+event, and the handleEvents() method is invoked with its \a cause argument
+set to indicate a time-advanced event occurred. By default, time advancement
+proceeds silently. **/
+void setHasTimeAdvancedEvents(bool); // default=false
+
+/** Get the current setting of the "up" direction hint. **/
+CoordinateDirection getUpDirection() const;
+/** Get the current setting of the "use uniform background" visualization
+hint. **/
+bool getUseUniformBackground() const;
+/** Get the currently-set value for the default time scale, 0.1 time units
+if it has never been set. **/
+Real getDefaultTimeScale() const;
+/** Get the currently-set value for the default length scale, 1 length unit
+if it has never been set. **/
+Real getDefaultLengthScale() const;
+/** Return the current value of the flag indicating whether this %System wants
+an event generated whenever time advances irreversibly. **/
+bool hasTimeAdvancedEvents() const;
+/**@}**/
+
+
+//------------------------------------------------------------------------------
+/**@name Event handlers and reporters
+
+These methods are used to attach event handlers and reporters to this %System.
+These are actually added to the DefaultSystemSubsystem that is contained in
+every System. **/
+/**@{**/
+/** Add a ScheduledEventHandler to this %System, which takes over ownership
+of the event handler object. **/
+inline void addEventHandler(ScheduledEventHandler* handler);
+/** Add a TriggeredEventHandler to this %System, which takes over ownership
+of the event handler object. **/
+inline void addEventHandler(TriggeredEventHandler* handler);
+/** Add a ScheduledEventReporter to this %System, which takes over ownership
+of the event reporter object. **/
+inline void addEventReporter(ScheduledEventReporter* handler) const;
+/** Add a TriggeredEventReporter to this %System, which takes over ownership
+of the event reporter object. **/
+inline void addEventReporter(TriggeredEventReporter* handler) const;
+/**@}**/
+
+
+//------------------------------------------------------------------------------
+/**@name Realization
+
+These methods provide the ability to compute the consequences that follow
+from the values of state variables, leaving the results in the
+appropriate cache. Usually this refers to values in a given State object, but
+we also use the same concept for values assigned to a %System during its
+(extended) construction, which we call \e Topological variables. The
+realizeTopology() method is called at the end of construction, allocating
+resources, performing computations, and creating a default State, with all
+results stored in a cache that is maintained by the %System itself. The other
+realize() methods are given a State object to work with and store their results
+in that State object without making any changes to the %System. **/
+/**@{**/
+
+/** The following call must be made after any topological change has been
+made to this %System, before the %System can be used to perform any
+computations. Perhaps surprisingly, the method is const. That's because
+the topology cannot be changed by this method. Various mutable "cache"
+entries will get calculated, including the default State, a reference
+to which is returned. The returned State has a Topology stage version number
+that matches the %System, and will have already been realized
+through the Model Stage, using the defaults for the Model-stage
+variables, meaning that all later stage variables have been allocated
+and set to their default values as well. You can access this same
+default State again using getDefaultState(). If the current topology
+has already been realized, this call does nothing but return a reference
+to the already-built default State.
+ at see getDefaultState(), realizeModel(), realize() **/
+const State& realizeTopology() const;
+
+/** This is available after realizeTopology(), and will throw an
+exception if realizeTopology() has not been called since the
+most recent topological change to this %System. This method returns the
+same reference returned by realizeTopology(). The State to which
+a reference is returned was created by the most recent
+realizeTopology() call. It has a Topology version number that matches the one
+currently in this %System, and has already been realized through the
+Model Stage, using default values for all the Model-stage variables.
+All later-stage variables have been allocated and set to their
+default values. You can use this state directly to obtain information
+about the %System in its default state or you can use this state
+to initialize other States to which you have write access. Those
+States are then suitable for further computation with this System.
+ at see realizeTopology() **/
+const State& getDefaultState() const;
+/** Don't use this; make a copy of the default state instead and modify
+your copy. **/
+State& updDefaultState();
+
+/** Realize the model to be used for subsequent computations with the given
+\a state. This call is required if Model-stage variables are changed from their
+default values. The %System topology must already have been realized (that is,
+realizeTopology() must have been called since the last topological change made
+to the %System). Also, the supplied \a state must already have been initialized
+to work with this %System either by copying the default state or some other
+State of this %System. If it has already been realized to Stage::Model or
+higher, nothing happens here. Otherwise, all the state variables at
+Stage::Instance or higher are allocated or reallocated (if necessary), and
+reinitialized to their default values.
+
+ at note Any state information at Stage::Instance or higher in the passed-in
+\a state is \e destroyed here. The number, types and memory locations of those
+state variables will change, so any existing references or pointers to them are
+invalid after this call. Note that this routine modifies its \a state argument,
+but makes no changes at all to the %System itself (not even to the %System's
+mutable cache) and is hence const.
+ at see realizeTopology(), realize() **/
+void realizeModel(State& state) const;
+
+/** Realize the given \a state to the indicated \a stage. The passed-in State
+must have been initialized to work with this %System, and it must already have
+been realized through Stage::Model, since this realize() method doesn't have
+write access to the State. If the state has already been realized to the
+requested stage or higher, nothing happens here. Otherwise, the state is
+realized one stage at a time until it reaches the requested stage.
+ at see realizeTopology(), realizeModel() **/
+void realize(const State& state, Stage stage = Stage::HighestRuntime) const;
+/**@}**/
+
+
+//------------------------------------------------------------------------------
+/**@name The Constrained System
+
+These methods deal with prescribed motion and constraints on the allowable
+values of q and u. They are primarly for use by numerical integrators.
+Prescribed motion is always resolved exactly because it involves explicit
+computation of q=q(t) and u=u(t,q). Constraints are given by implicit
+equations like g(q)=0 and are solved to a specified tolerance. Also, prescribed
+motion has precedence over constraints in the sense that no changes will be
+made to prescribed variables in order to satisfy constraints; only the free
+variables are modified by constraint solvers. **/
+/**@{**/
+
+
+/** Apply prescribed motion and attempt to satisfy all position and velocity
+constraints by projecting the generalized coordinates q and generalized speeds
+u onto the position and velocity constraint manifolds. This method expects to
+be given a \a state that is close to satisfying the constraints already. It
+will apply prescribed motion and then attempt a least-squares projection to
+satisfy constraints, working first on q's and then on u's. If you are trying to
+assemble a system from an arbitrary configuration, would like control over
+which states get modified, or are attempting to track markers, use an
+Assembler solver instead.
+
+ at param[in,out] state
+ A State whose q's and u's are to be modified. Must already have been
+ realized to at least Stage::Model. On return will be realized to
+ Stage::Velocity.
+ at param[in] accuracy
+ If provided, sets the required accuracy to which the constraints must
+ be satisfied; an exception will be thrown if that accuracy cannot be
+ achieved. If accuracy is left off or is nonpositive, the accuracy used is
+ the default as provided by the ProjectOptions class.
+
+This method is provided for convenience and involves several calls to realize(),
+prescribeQ(), prescribeU(), projectQ(), and projectU(); call those directly if
+you want more control.
+
+On return the given \a state's q and u variables will have been modified and
+the \a state will be realized through Velocity stage.
+
+
+<h3>Theory</h3>
+
+This solver projects the given state back on to the position or velocity
+constraint manifold, by the shortest path possible in the scaled norm defined in
+that state, and modifying only free (non-prescribed) variables. Constraint
+errors are scaled by their unit error weightings, then satisfied to a given
+accuracy using an RMS norm, or optionally using the stricter infinity norm.
+You can specify accuracy here explicitly; otherwise it uses the default as
+provided by the ProjectOptions class.
+
+Position constraints are satisfied as follows:
+<pre>
+ solve |Tp*perr(t;q+dq)|_n <= accuracy for dq (n==rms or inf)
+ such that |Wq*dq|_2 is minimized
+</pre>
+Here Tp=diag(1./unit_p) scales each position constraint error to a fraction of
+its unit error. Wq=N*Wu*pinv(N) weights dq to include both the "unit change"
+weightings on u and the artifactual configuration-dependent weightings on q
+generated by choice of orientation coordinates such as quaternions or rotation
+angles. (N as in qdot=N*u; pinv() is pseudoinverse.) We do not allow relative
+weighting on dq based on the current values of q; Simbody always solves q to
+absolute accuracy since arbitrary translations and rotations by 2pi should not
+affect physically-significant results.
+
+Velocity constraints are satisfied as follows:
+<pre>
+ solve |Tpv*pverr(t,q;u+du)|_n <= accuracy for du (n==rms or inf)
+ such that |Eu*du|_2 is minimized
+</pre>
+Here Tpv=diag(ts./unit_p 1./unit_v) where ts is the system's time scale used to
+scale the holonomic constraint's unit errors, and unit_v is the unit error for
+the holonomic constraints. The error weighting matrix Eu combines relative
+and absolute accuracy requirements as follows:
+<pre>
+ Eu_i={ min(Wu_i, 1/u_i), relative accuracy OK for u_i
+ { Wu_i, otherwise
+</pre>
+
+ at see Assembler, projectQ(), projectU(), prescribeQ(), prescribeU() **/
+void project(State& state, Real accuracy=-1) const;
+
+/** Set prescribed positions q=q(t) and attempt to satisfy position constraints
+by modifying the remaining free q's. The given \a state will be realized as
+necessary and on return will have been realized through Position stage. See
+project() for more information.
+
+ at param[in,out] state
+ A State whose q's are to be modified. Must already have been realized to at
+ least Stage::Model. On return will be realized to Stage::Position.
+ at param[in] accuracy
+ If provided, sets the required accuracy to which the constraints must
+ be satisfied; an exception will be thrown if that accuracy cannot be
+ achieved. If accuracy is left off or is nonpositive, the accuracy used is
+ the default as provided by the ProjectOptions class.
+
+ at see project(), projectU(), ProjectOptions **/
+void projectQ(State& state, Real accuracy=-1) const;
+
+/** Set prescribed velocities u=u(t,q) and attempt to satisfy velocity
+constraints by modifying the remaining free u's. The given \a state will be
+realized as necessary and on return will have been realized through Velocity
+stage. Note that no q's are modified by this method; you should already have
+ensured that they satisfy position constraints. See project() for more
+information.
+
+ at param[in,out] state
+ A State whose u's are to be modified. Must already have been realized to at
+ least Stage::Model. On return will be realized to Stage::Velocity.
+ at param[in] accuracy
+ If provided, sets the required accuracy to which the constraints must
+ be satisfied; an exception will be thrown if that accuracy cannot be
+ achieved. If accuracy is left off or is nonpositive, the accuracy used is
+ the default as provided by the ProjectOptions class.
+
+ at see project(), projectQ(), ProjectOptions **/
+void projectU(State& state, Real accuracy=-1) const;
+
+/** (Advanced) Project free q's so that position constraints are satisfied and
+remove the corresponding error from the supplied error estimate. This is
+primarily intended for use by numerical integration algorithms. You must
+already have set prescribed q's; this method will not modify them but may
+depend on their current values. State must be realized to Position stage on
+entry and will still be realized through Position stage on return.
+
+If the norm of the position constraint errors is already less than or equal to
+the accuracy requested in \a options on entry, nothing will happen unless you
+have selected the "force projection" option. You can find out what actually
+happened by looking in the returned \a results.
+
+Optionally, this method can also project out the constraint-normal component of
+the passed-in error estimate vector \a qErrEst. This is part of the integration
+of the continuous DAE system and thus should never require an integrator
+restart. Just pass a 0-length (default constructed) Vector in place of the
+error estimate if you don't want error projection.
+
+Some options are available, see ProjectOptions for complete list. For example,
+- use infinity norm
+- local projection only
+- force projection
+- don't throw an exception
+- force full Newton
+
+This method and projectU() are not for satisfying acceleration constraints,
+which does not involve modifications to state variables. Acceleration
+constraints are satisfied automatically when you realize a state to
+Acceleration stage using realize(state); the resulting udots will satisfy the
+acceleration constraints (if possible), even if the position and velocity
+constraints are not satisfied.
+
+See the Theory section in the documentation for project() for more information.
+
+ at see ProjectOptions, ProjectResults, projectU(), project() **/
+void projectQ(State& state, Vector& qErrEst,
+ const ProjectOptions& options, ProjectResults& results) const;
+
+/** (Advanced) Project free u's so that velocity constraints are satisfied and
+remove the corresponding error from the supplied error estimate. This is
+primarily intended for use by numerical integration algorithms. You must
+already have dealt with q's and already set prescribed u's; this method will
+not modify them but may depend on their current values. State must be realized
+to Velocity stage on entry and will still be realized through Velocity stage on
+return.
+
+If the norm of the velocity constraint errors is already less than or equal to
+the accuracy requested in \a options on entry, nothing will happen unless you
+have selected the "force projection" option. You can find out what actually
+happened by looking in the returned \a results.
+
+Optionally, this method can also project out the constraint-normal component of
+the passed-in error estimate vector \a uErrEst. This is part of the integration
+of the continuous DAE system and thus should never require an integrator
+restart. Just pass a 0-length (default constructed) Vector in place of the
+error estimate if you don't want error projection.
+
+See the Theory section in the documentation for project(), and the comments
+for projectQ(), for more information.
+
+ at see ProjectOptions, ProjectResults, projectQ(), project() **/
+void projectU(State& state, Vector& uErrEst,
+ const ProjectOptions& options, ProjectResults& results) const;
+
+
+/** Set values for prescribed positions q and velocities u.
+Prescribed positions are functions of time q(t) and prescribed velocities are
+functions of time and position u(t,q). Both can also depend on earlier-stage
+discrete variables such as modeling and instance parameters.
+
+ at param[in,out] state
+ The State to be modified. Time and the values of non-prescribed q's are
+ obtained from \a state and prescribed q's and u's are modified on return.
+ The \a state will be realized as needed and on return will have been
+ realized through Position stage. The prescribed velocities will have been
+ set but not yet realized.
+
+This is a convenience method that performs several realize() calls and uses
+prescribeQ() and prescribeU(). If you want more control, use those methods
+directly instead of this one.
+
+ at see prescribeQ(), prescribeU(), project(), realize() **/
+void prescribe(State& state) const {
+ realize(state, Stage::Time);
+ prescribeQ(state);
+ realize(state, Stage::Position);
+ prescribeU(state);
+}
+
+/** This solver sets prescribed q's to their known values q=q(t) as a function
+of time (and possibly earlier-stage discrete state variables).
+
+We expect the supplied State already to have been realized to Stage::Time.
+Note that the \e derivatives qdot of prescribed q's (which are
+of necessity also prescribed but are not themselves state variables) are set in
+the subsequent realize() call. For example, prescribeU sets the
+prescribed u's, then the next realize(Velocity) call will use them to calculate
+the prescribed qdots=N*u. realize(Dynamics) calculates known forces and the
+prescribed udoti=udoti(t,q,u). realize(Acceleration) calculates the
+remaining udots, lambdas, taus, and all the zdots.
+
+Note that this method is \e not used to set prescribed udots, because those are
+not state variables. Instead, prescribed udots (which depend on time, positions,
+and velocities) are set as part of realize(Dynamics).
+
+ at return \c true if any change was made to \a state.
+ at see prescribe(), prescribeU() **/
+bool prescribeQ(State& state) const;
+
+/** This solver sets prescribed u's to their known values u=u(t,q) as a
+function of time and position variables q (and possibly earlier-stage discrete
+state variables).
+
+We expect the supplied State already to have been realized to Stage::Position.
+Note that the \e derivatives of prescribed variables (which are
+of necessity also prescribed but are not themselves state variables) are set in
+the subsequent realize() calls. For example, prescribeU() sets the
+prescribed u's, then the next realize(Velocity) call will use them to calculate
+the prescribed qdots=N*u. realize(Dynamics) calculates known forces and the
+prescribed udoti=udoti(t,q,u). realize(Acceleration) calculates the
+remaining udots, lambdas, taus, and all the zdots.
+
+ at return \c true if any change was made to \a state.
+ at see prescribe(), prescribeQ() **/
+bool prescribeU(State& state) const;
+
+/** Return the indices of the q's in the given \a state that are free; that is,
+they are not affected by prescribeQ(). The indices are copied into the return
+argument \a freeQs in ascending order; missing ones are prescribed. \a freeQs is
+resized if necessary to the number of free q's, nfq. \a state must be realized
+through Stage::Instance. **/
+void getFreeQIndex(const State& state, Array_<SystemQIndex>& freeQs) const;
+
+/** Return the indices of the u's in the given \a state that are free; that is,
+they are not affected by prescribeU(). The indices are copied into the return
+argument \a freeUs in ascending order; missing ones are prescribed. \a freeUs is
+resized if necessary to the number of free u's, nfu. \a state must be realized
+through Stage::Instance. **/
+void getFreeUIndex(const State& state, Array_<SystemUIndex>& freeUs) const;
+/**@}**/
+
+
+//------------------------------------------------------------------------------
+/**@name The Discrete System
+
+These methods deal with the discrete (event-driven) aspects of this %System.
+These methods are primarily for the use of Integrator objects that detect
+events and TimeStepper objects that deal with them by calling appropriate
+handlers. In general, events interrupt the continuous evolution of a system,
+dividing a simulation into continuous segments interrupted by discontinuous
+changes of state. There are also discrete variables that can be updated without
+disrupting the continuous flow. These are called "auto update variables" and are
+updated at the beginning of every integration step, right \e after the initial
+state derivatives have been recorded. Thus the step proceeds with those
+derivatives even if the update could cause them to change. These updates are
+initiated by Integrator objects at the start of every continuous step, while
+trajectory-affecting discrete variable updates are initiated only by TimeStepper
+objects via calls to event handlers.
+
+ at see State::allocateAutoUpdateDiscreteVariable()
+**/
+/**@{**/
+
+/** This solver handles a set of events which a TimeStepper has denoted as
+having occurred at the given time and state. The event handler may make
+discontinuous changes in the State, in general both to discrete and continuous
+variables, but \e not to time or topological information. If changes are made
+to continuous variables, the handler is required to make sure the returned
+state satisfies the constraints to the accuracy level specified in \a options.
+
+On return, the handleEvents() method will set the output variable \a results
+to indicate what happened. If any invoked handler determines that the
+occurrence of some event requires that the simulation be terminated, that
+information is returned in \a results and a well-behaved TimeStepper will stop
+when it sees that.
+
+Simbody will automatically set a field in \a results that says how much of
+the \a state was changed by the handler so that the calling TimeStepper will
+be able to determine how much reinitialization is required.
+
+ at see HandleEventsOptions, HandleEventsResults **/
+void handleEvents(State& state,
+ Event::Cause cause,
+ const Array_<EventId>& eventIds,
+ const HandleEventsOptions& options,
+ HandleEventsResults& results) const;
+
+/** This method is similar to handleEvents(), but does not allow the State
+to be modified. It is used for scheduled events that were marked as
+being reports. **/
+void reportEvents(const State& state,
+ Event::Cause cause,
+ const Array_<EventId>& eventIds) const;
+
+/** This routine provides the Integrator with information it needs about the
+individual event trigger functions, such as which sign transitions are relevant
+and how tightly we need to localize. This is considered Instance stage
+information so cannot change during a continuous integration interval (so an
+Integrator can process it upon restart(Instance)), however it can be updated
+whenever a discrete update is made to the State. A default implementation is
+provided which returns default EventTriggerInfo for each event trigger in
+\a state. The \a state must already be realized to Stage::Instance. **/
+void calcEventTriggerInfo(const State& state,
+ Array_<EventTriggerInfo>& triggerInfo) const;
+
+/** This routine should be called to determine if and when there is an event
+scheduled to occur at a particular time. This is a \e lot cheaper than
+making the Integrator hunt these down like ordinary state-dependent events.
+The returned time can be passed to the Integrator's stepping function as
+the advance time limit. **/
+void calcTimeOfNextScheduledEvent(const State& state,
+ Real& tNextEvent,
+ Array_<EventId>& eventIds,
+ bool includeCurrentTime) const;
+
+/** This routine is similar to calcTimeOfNextScheduledEvent(), but is used for
+"reporting events" which do not modify the state. Events returned by this method
+should be handled by invoking reportEvents() instead of handleEvents(). **/
+void calcTimeOfNextScheduledReport(const State& state,
+ Real& tNextEvent,
+ Array_<EventId>& eventIds,
+ bool includeCurrentTime) const;
+/**@}**/
+
+
+//------------------------------------------------------------------------------
+/**@name The Fast System
+
+(NOT IMPLEMENTED YET) These methods are for state variables whose dynamics
+are considered very fast compared to the rest of the system, meaning that
+their values can be determined by solving for algebraic equilibrium
+conditions rather than integrating differential equations. **/
+/**@{**/
+
+/** This optional method should modify fast variables at the given stage
+until they satisfy some relaxation criteria. The criteria may involve
+anything in the State *except* fast variables at higher stages. Anything
+that can be calculated from the state is also fair game provided that
+those calculations do not depend on higher-stage fast variables.
+"Relaxation" criteria may require that fast variables satisfy some
+implicit or explicit algebraic conditions (constraints), or reach some
+minimization or maximization condition. A common criterion is that fast
+q's are moved to minimize potential energy; that can be achieved by
+driving the fast mobilities' lambdas (calculated joint torques) to zero
+since they are the potential energy gradient. That may require repeated
+realization to Acceleration stage.
+
+Note that when q's are fast, the corresponding u's and udots are
+\e prescribed (to zero), they are not \e fast. And when u's are fast, their
+udots are also zero (their q's are regular integrated variables). Fast
+z's have zero zdots. Any other variables (that is, x-partition variables)
+can also be fast but don't have derivatives.
+
+TODO: should take options and return results. **/
+void relax(State& state, Stage stage, Real accuracy=-1) const;
+/**@}**/
+
+
+//------------------------------------------------------------------------------
+/**@name Kinematic differential equations
+
+These methods are primarily for use by numerical integration methods.
+
+Generalized coordinates q are not independent of generalized speeds u;
+they are related by the kinematic differential equation qdot=N(q)*u, where
+N is an nqXnu block diagonal, invertible matrix in the sense that
+u=pinv(N)*qdot, where pinv(N) is the pseudoinverse of N. N has full column rank,
+so pinv(N)*N = I, but N*pinv(N) != I.
+
+Just as N provides the relation between velocities expressed in u-space and
+the equivalent in q-space, its transpose ~N relates forces in q-space to
+their equivalent in u-space: fu=~N*fq, and fq=~pinv(N)*fu. (Note that
+~pinv(X)==pinv(~X).) This satisfies power=~fq*qdot==~fu*u as it must.
+
+We provide fast O(n) operators for multiplication by N, pinv(N), and their
+transposes. N is often mostly an identity matrix so very little computation
+is required. **/
+/**@{**/
+/** Calculate dq=N*u in O(n) time (very fast). **/
+void multiplyByN(const State& state, const Vector& u,
+ Vector& dq) const;
+/** Calculate fu=~N*fq in O(n) time (very fast). **/
+void multiplyByNTranspose(const State& state, const Vector& fq,
+ Vector& fu) const;
+/** Calculate u=pinv(N)*dq in O(n) time (very fast). **/
+void multiplyByNPInv(const State& state, const Vector& dq,
+ Vector& u) const;
+/** Calculate fq=~pinv(N)*fu in O(n) time (very fast). **/
+void multiplyByNPInvTranspose(const State& state, const Vector& fu,
+ Vector& fq) const;
+/**@}**/
+
+
+//------------------------------------------------------------------------------
+/**@name Statistics
+
+The %System keeps mutable statistics internally, initialized to zero at
+construction. These <em>must not</em> affect results in any way. **/
+/**@{**/
+
+/** Zero out the statistics for this System. Although the statistics are
+mutable, we only allow them to be reset by a caller who has write access since
+setting the stats to zero is associated with construction. **/
+void resetAllCountersToZero();
+
+ // Realization
+
+/** Whenever the system was realized from Stage-1 to the indicated Stage,
+this counter is bumped. Note that a single call to realize() can cause
+several counters to get bumped. **/
+int getNumRealizationsOfThisStage(Stage) const;
+
+/** Return the total number of calls to realizeTopology(), realizeModel(),
+or realize(), regardless of whether these routines actually did
+anything when called. **/
+int getNumRealizeCalls() const;
+
+ // Prescribed motion
+
+/** Return the total number of calls to the System's prescribeQ() method. **/
+int getNumPrescribeQCalls() const;
+/** Return the total number of calls to the System's prescribeU() method. **/
+int getNumPrescribeUCalls() const;
+
+ // Projection
+
+/** Return the total number of calls to projectQ(), regardless of
+whether the call did anything. **/
+int getNumProjectQCalls() const;
+/** Return the total number of calls to projectQ() that failed. **/
+int getNumFailedProjectQCalls() const;
+/** How many of the successful projectQ() calls actually did a constraint
+projection, rather than returning quickly? **/
+int getNumQProjections() const;
+/** How many of the projectQ() calls that did a constraint projection also
+projected an error estimate? **/
+int getNumQErrorEstimateProjections() const;
+
+/** Return the total number of calls to projectU(), regardless of
+whether the call did anything. **/
+int getNumProjectUCalls() const;
+/** Return the total number of calls to projectU() that failed. **/
+int getNumFailedProjectUCalls() const;
+/** How many of the successful projectU() calls actually did a constraint
+projection, rather than returning quickly? **/
+int getNumUProjections() const;
+/** How many of the projectU() calls that did a constraint projection also
+projected an error estimate? **/
+int getNumUErrorEstimateProjections() const;
+
+ // Event handling and reporting
+
+/** handleEvents() reports the lowest Stage it modified and we bump
+the counter for that Stage. We also count reportEvents() calls here
+as having "changed" Stage::Report. **/
+int getNumHandlerCallsThatChangedStage(Stage) const;
+
+/** This is the total number of calls to handleEvents() regardless
+of the outcome. **/
+int getNumHandleEventCalls() const;
+
+/** This is the total number of calls to reportEvents() regardless
+of the outcome. **/
+int getNumReportEventCalls() const;
+/**@}**/
+
+
+//------------------------------------------------------------------------------
+/**@name Construction and bookkeeping
+
+These methods are mostly for use by concrete Systems and will not typically
+be of interest to users. **/
+/**@{**/
+/** Default constructor creates an empty handle. **/
+System() : guts(0) { }
+/** Copy constructor (untested). **/
+System(const System&);
+/** Copy assignment (untested). **/
+System& operator=(const System&);
+/** Destructor here will invoke the virtual destructor in the System::Guts
+class and thus clean up all unneeded detritus owned by this %System. **/
+~System();
+
+/** Return the name assigned to this %System (arbitrary). **/
+const String& getName() const;
+/** Return the version string assigned to this %System (arbitrary). **/
+const String& getVersion() const;
+
+/** Take over ownership of the supplied subsystem and install it into
+the next free subsystem slot. The new slot index is returned. **/
+SubsystemIndex adoptSubsystem(Subsystem& child);
+
+/** How may Subsystems are in here? **/
+int getNumSubsystems() const;
+/** Obtain read-only access to a particular subsystem by its index. **/
+const Subsystem& getSubsystem(SubsystemIndex) const;
+/** Obtain writable access to a particular subsystem by its index. **/
+Subsystem& updSubsystem(SubsystemIndex);
+/** Get read-only access to the default subsystem which is present in every
+system. **/
+const DefaultSystemSubsystem& getDefaultSubsystem() const;
+/** Get writable access to the default subsystem which is present in every
+system. **/
+DefaultSystemSubsystem& updDefaultSubsystem();
+
+/** Implicitly convert this System into a const Subsystem reference; this
+actually returns a reference to the DefaultSystemSubsystem contained in this
+System. **/
+inline operator const Subsystem&() const; // implemented below
+/** Implicitly convert this System into a writable Subsystem reference; this
+actually returns a reference to the DefaultSystemSubsystem contained in this
+System. **/
+inline operator Subsystem&();
+
+/** (Advanced) You can check whether realizeTopology() has been called since the
+last topological change to this Syatem. If you don't check and just plunge
+ahead you are likely to encounter an exception since very few things
+will work without topology having been realized. **/
+bool systemTopologyHasBeenRealized() const;
+
+/** (Advanced) Return the current version number of this system's Topology
+cache information. This is a counter that is incremented each time the Topology
+is invalidated. Any State to be used with this System must have the same
+Topology version number as the System does. The version number is returned
+regardless of whether topology has been realized; you can check that with
+systemTopologyHasBeenRealized().
+ at see State::getSystemTopologyStageVersion() **/
+StageVersion getSystemTopologyCacheVersion() const;
+
+/** (Really advanced) Set the current version number of this system's
+Topology cache information. Don't use this method unless you really know what
+you're doing! This has no effect on realization status; if topology has not
+yet been realized this is the version number it will have after the next
+realizeTopology() call. **/
+void setSystemTopologyCacheVersion(StageVersion topoVersion) const;
+
+/** (Advanced) Mark the Topology stage of this system and all its subsystems
+"not realized." This is normally handled automatically by whenever you make a
+Topology-stage change to any subsystem. Occasionally you may want to force
+recomputation of the Topology-stage cache, for example during testing. After
+this call the method systemTopologyHasBeenRealized() will return false and you
+will not be able to call getDefaultState(). A subsequent call to
+realizeTopology() will invoke all the subsystems' realizeTopology() methods.
+The Topology stage version number will have changed, so all previously-created
+State objects will be invalid. **/
+void invalidateSystemTopologyCache() const;
+
+/** (Advanced) Generate all decorative geometry computable at a specific stage;
+this is useful for visualizers. This will throw an exception if the state hasn't
+already been realized to the given stage. Note that the list is not inclusive --
+you have to request geometry from each stage to get all of it. This routine
+asks each subsystem in succession to generate its decorative geometry and
+append it to the end of the array. If the stage is Stage::Topology,
+realizeTopology() must already have been called but the State is ignored. **/
+void calcDecorativeGeometryAndAppend(const State&, Stage,
+ Array_<DecorativeGeometry>&) const;
+
+
+/** There can be multiple handles referring to the same System::Guts object;
+they are considered to be the same System. **/
+bool isSameSystem(const System& otherSystem) const;
+
+
+/** Obtain a const reference to the System::Guts object to which this handle
+refers. You should then dynamic_cast the returned reference to a reference to
+your concrete Guts class. **/
+const Guts& getSystemGuts() const {assert(guts); return *guts;}
+/** Obtain a writable reference to the System::Guts object to which this handle
+refers. You should then dynamic_cast the returned reference to a reference to
+your concrete Guts class. **/
+Guts& updSystemGuts() {assert(guts); return *guts;}
+
+/** Put new \e unowned Guts into this *empty* handle and take over ownership.
+If this handle is already in use, or if Guts is already owned this
+routine will throw an exception. **/
+void adoptSystemGuts(System::Guts* g);
+
+/** Constructor for internal use only. **/
+explicit System(System::Guts* g) : guts(g) { }
+/** Return true if this %System handle is not empty. **/
+bool hasGuts() const {return guts!=0;}
+
+/** Internal use only. **/
+bool isOwnerHandle() const;
+/** Internal use only. **/
+bool isEmptyHandle() const;
+/**@}**/
+
+private:
+friend class Guts;
+// This is the only data member in this class. Also, any class derived from
+// System must have *NO* data members at all (data goes in the Guts class).
+Guts* guts;
+};
+
+
+/** This is a concrete Subsystem that is part of every System.\ It provides a
+variety of services for the System, such as maintaining lists of event handlers
+and reporters, and acting as a source of globally unique event IDs.
+
+To obtain the default subsystem for a System, call getDefaultSubsystem() or
+updDefaultSubsystem() on it. Also, a System can be implicitly converted
+to a Subsystem, in which case it actually returns a reference to
+this Subsystem. **/
+class SimTK_SimTKCOMMON_EXPORT DefaultSystemSubsystem : public Subsystem {
+public:
+ explicit DefaultSystemSubsystem(System& sys);
+ void addEventHandler(ScheduledEventHandler* handler);
+ void addEventHandler(TriggeredEventHandler* handler);
+ void addEventReporter(ScheduledEventReporter* handler) const;
+ void addEventReporter(TriggeredEventReporter* handler) const;
+ EventId createEventId(SubsystemIndex subsys, const State& state) const;
+ void findSubsystemEventIds
+ (SubsystemIndex subsys, const State& state,
+ const Array_<EventId>& allEvents,
+ Array_<EventId>& eventsForSubsystem) const;
+
+ /** @cond **/ // don't let doxygen see this private class
+ class Guts;
+ /** @endcond **/
+private:
+ const Guts& getGuts() const;
+ Guts& updGuts();
+};
+
+inline void System::addEventHandler(ScheduledEventHandler* handler)
+{ updDefaultSubsystem().addEventHandler(handler); }
+inline void System::addEventHandler(TriggeredEventHandler* handler)
+{ updDefaultSubsystem().addEventHandler(handler); }
+inline void System::addEventReporter(ScheduledEventReporter* handler) const
+{ getDefaultSubsystem().addEventReporter(handler); }
+inline void System::addEventReporter(TriggeredEventReporter* handler) const
+{ getDefaultSubsystem().addEventReporter(handler); }
+
+inline System::operator const Subsystem&() const {return getDefaultSubsystem();}
+inline System::operator Subsystem&() {return updDefaultSubsystem();}
+
+
+//==============================================================================
+// PROJECT OPTIONS and PROJECT RESULTS
+//==============================================================================
+/** Options for the advanced project() methods. The default is to require
+project() methods to reduce constraint errors to an RMS norm of 1e-4, while
+asking them to attempt 10X tighter accuracy if possible. **/
+class ProjectOptions {
+public:
+ enum Option {
+ /** Take all defaults. **/
+ None = 0x0000,
+ /** This option says we expect the state to be close to a solution
+ already and restricts projection to move downhill in the local
+ vicinity. This should be used during any continuous integration to
+ prevent erroneous jumps in the state. **/
+ LocalOnly = 0x0001,
+ /** Normally failure to meet the accuracy requirements throws an
+ exception. This will force the project() method to quietly return bad
+ status instead. **/
+ DontThrow = 0x0002,
+ /** Use the stricter infinity (max absolute value) norm rather than
+ the default RMS norm to determine when accuracy has been achieved. **/
+ UseInfinityNorm = 0x0004,
+ /** Normally a project() method will return immediately after
+ evaluating the norm if it is already at or below the required accuracy.
+ This option forces it to make at least one iteration. **/
+ ForceProjection = 0x0008,
+ /** A project() method is free to use an out-of-date Jacobian when
+ solving the nonlinear system. This option forces recalculation of
+ the Jacobian at the start of each iteration. **/
+ ForceFullNewton = 0x0010
+ };
+
+ /** Default constructor sets options to their default values. **/
+ ProjectOptions() {clear();}
+ /** This constructor allows the default accuracy to be overridden while
+ leaving all other options at their default values. If \a accuracy is
+ nonpositive, the default accuracy will be used. **/
+ explicit ProjectOptions(Real accuracy)
+ { clear(); setRequiredAccuracy(accuracy); }
+ /** This constructor creates default options except one setting one
+ non-default Option. You can add more with the setOption() method. **/
+ explicit ProjectOptions(Option opt)
+ { clear(); setOption(opt); }
+
+ /** Restore this object to its default-constructed state (no options
+ selected, default accuracy and overshoot). A reference to the
+ newly-cleared object is returned. **/
+ ProjectOptions& clear()
+ { optionSet=0; setAccuracyDefaults(); return *this; }
+
+ /** The norm of the constraint errors must be driven to below this value
+ for a project() to be considered successful. Normally an RMS norm is used
+ but you can override that to use an infinity norm instead. If accuracy
+ is nonpositive we'll use the default; see getDefaultRequiredAccuracy(). **/
+ ProjectOptions& setRequiredAccuracy(Real accuracy) {
+ requiredAccuracy = accuracy > 0 ? accuracy
+ : getDefaultRequiredAccuracy();
+ return *this;
+ }
+
+ /** Project will attempt to reach accuracy*overshoot but settle for
+ just accuracy. **/
+ ProjectOptions& setOvershootFactor(Real overshoot) {
+ assert(0 < overshoot && overshoot <= 1);
+ desiredOvershoot = overshoot;
+ return *this;
+ }
+
+ /** Project will fail immediately if the initial norm is greater than
+ the projection limit, with status FailureToConverge. **/
+ ProjectOptions& setProjectionLimit(Real limit) {
+ assert(limit > 0);
+ projectionLimit = limit;
+ return *this;
+ }
+
+ /** Remove a given option from the set. Nothing happens if the option wasn't
+ already set. **/
+ ProjectOptions& clearOption(Option opt)
+ { optionSet &= ~(unsigned)opt; return *this; }
+ /** Set a particular option. Multiple calls are or'ed together. **/
+ ProjectOptions& setOption (Option opt)
+ { optionSet |= (unsigned)opt; return *this; }
+
+ /** Return the current value for the required accuracy option. **/
+ Real getRequiredAccuracy() const {return requiredAccuracy;}
+ /** Return the factor by which a project() method should try to do better
+ than the required accuracy. **/
+ Real getOvershootFactor() const {return desiredOvershoot;}
+ /** Return the maximum norm we're allowed to attempt to correct. **/
+ Real getProjectionLimit() const {return projectionLimit;}
+
+ bool isOptionSet(Option opt) const {return (optionSet&(unsigned)opt) != 0;}
+
+ static Real getDefaultRequiredAccuracy() {return Real(1e-4);}
+ static Real getDefaultOvershootFactor() {return Real(0.1);} //i.e., 1e-5
+
+ // Set operators: not, or, and, set difference
+ ProjectOptions& operator|=(const ProjectOptions& opts)
+ { optionSet |= opts.optionSet; return *this; }
+ ProjectOptions& operator&=(const ProjectOptions& opts)
+ { optionSet &= opts.optionSet; return *this; }
+ ProjectOptions& operator-=(const ProjectOptions& opts)
+ { optionSet &= ~opts.optionSet; return *this; }
+
+ ProjectOptions& operator|=(Option opt) {setOption(opt); return *this;}
+ ProjectOptions& operator-=(Option opt) {clearOption(opt); return *this;}
+
+private:
+ Real requiredAccuracy;
+ Real desiredOvershoot; // try for accuracy*overshoot
+ Real projectionLimit; // abort if initial norm is worse than this
+ unsigned optionSet;
+
+ void setAccuracyDefaults() {
+ requiredAccuracy = getDefaultRequiredAccuracy();
+ desiredOvershoot = getDefaultOvershootFactor();
+ projectionLimit = Infinity; // we'll try from however far away
+ }
+};
+
+/** Results for advanced users of project() methods. **/
+class ProjectResults {
+public:
+ ProjectResults() {clear();}
+
+ enum Status {
+ /** This object has not been filled in yet and holds no results. **/
+ Invalid = -1,
+ /** The project() was successful either because no projection was
+ necessary or projection was able to achieve the required accuracy. **/
+ Succeeded = 0,
+ /** Projection converged but was unable to achieve the required
+ accuracy. **/
+ FailedToAchieveAccuracy = 1,
+ /** The Newton iterations were diverging. This is especially common
+ when the LocalOnly option is set since project() will quit at the
+ first sign of divergence in that case. This is also the return if
+ a projection limit has been set and the initial norm is larger. **/
+ FailedToConverge = 2
+ };
+
+ /** Restore this object to its default-constructed state, with the return
+ status set to Invalid. **/
+ ProjectResults& clear() {
+ m_exitStatus = Invalid;
+ m_anyChangeMade = m_projectionLimitExceeded = false;
+ m_numIterations = 0;
+ m_worstError = -1;
+ m_normOnEntrance = m_normOnExit = NaN;
+ return *this;
+ }
+ bool isValid() const {return m_exitStatus != Invalid;}
+ Status getExitStatus() const {return m_exitStatus;}
+
+ bool getAnyChangeMade() const {assert(isValid());return m_anyChangeMade;}
+ int getNumIterations() const {assert(isValid());return m_numIterations;}
+ Real getNormOnEntrance() const {assert(isValid());return m_normOnEntrance;}
+ Real getNormOnExit() const {assert(isValid());return m_normOnExit;}
+ int getWorstErrorOnEntrance() const
+ { assert(isValid());return m_worstError; }
+ bool getProjectionLimitExceeded() const
+ { assert(isValid());return m_projectionLimitExceeded; }
+
+ ProjectResults& setExitStatus(Status status)
+ { m_exitStatus=status; return *this; }
+ ProjectResults& setAnyChangeMade(bool changeMade)
+ { m_anyChangeMade=changeMade; return *this; }
+ ProjectResults& setProjectionLimitExceeded(bool limitExceeded)
+ { m_projectionLimitExceeded=limitExceeded; return *this; }
+ ProjectResults& setNumIterations(int numIterations)
+ { m_numIterations=numIterations; return *this; }
+ ProjectResults& setNormOnEntrance(Real norm, int worstError)
+ { m_normOnEntrance=norm; m_worstError=worstError; return *this; }
+ ProjectResults& setNormOnExit(Real norm)
+ { m_normOnExit=norm; return *this; }
+private:
+ Status m_exitStatus;
+ bool m_anyChangeMade;
+ bool m_projectionLimitExceeded;
+ int m_numIterations;
+ int m_worstError; // index of worst error on entrance
+ Real m_normOnEntrance; // in selected rms or infinity norm
+ Real m_normOnExit;
+};
+
+
+
+//==============================================================================
+// REALIZE OPTIONS and REALIZE RESULTS
+//==============================================================================
+/** (NOT USED YET) Options for the advanced realize() methods. **/
+class RealizeOptions {
+ unsigned int optionSet;
+ explicit RealizeOptions(unsigned o) : optionSet(o) { }
+public:
+
+ enum Option {
+ None = 0x00,
+ DontThrow = 0x01
+ };
+
+
+ RealizeOptions() : optionSet(0) { }
+
+ // This is an implicit conversion
+ RealizeOptions(Option opt) : optionSet((unsigned)opt) { }
+
+ // Implicit conversion to bool when needed
+ operator bool() const {return optionSet != 0;}
+ bool isEmpty() const {return optionSet==0;}
+
+ bool isOptionSet(Option opt) const {return (optionSet&(unsigned)opt) != 0;}
+ void clear() {optionSet=0;}
+ void clearOption(Option opt) {optionSet &= ~(unsigned)opt;}
+ void setOption (Option opt) {optionSet |= (unsigned)opt;}
+
+ // Set operators: or, and
+ RealizeOptions& operator|=(RealizeOptions opts) {optionSet |= opts.optionSet; return *this;}
+ RealizeOptions& operator&=(RealizeOptions opts) {optionSet &= opts.optionSet; return *this;}
+
+ RealizeOptions& operator|=(Option opt) {setOption(opt); return *this;}
+ RealizeOptions& operator-=(Option opt) {clearOption(opt); return *this;}
+};
+
+/** (NOT USED YET) Results for advanced users of realize() methods. **/
+class RealizeResults {
+};
+
+
+
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_SYSTEM_H_
diff --git a/SimTKcommon/Simulation/include/SimTKcommon/internal/SystemGuts.h b/SimTKcommon/Simulation/include/SimTKcommon/internal/SystemGuts.h
new file mode 100644
index 0000000..df2f4c3
--- /dev/null
+++ b/SimTKcommon/Simulation/include/SimTKcommon/internal/SystemGuts.h
@@ -0,0 +1,267 @@
+#ifndef SimTK_SimTKCOMMON_SYSTEM_GUTS_H_
+#define SimTK_SimTKCOMMON_SYSTEM_GUTS_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/State.h"
+#include "SimTKcommon/internal/System.h"
+
+namespace SimTK {
+
+class Subsystem;
+class DecorativeGeometry;
+
+/**
+ * This is the declaration for the System::Guts class, the abstract object to
+ * which a System handle points. This is in a separate header file from System
+ * because only people who are extending the System class to make their own
+ * Systems need to be aware of the details. End users access only methods from
+ * the System class and classes derived from System, never anything from
+ * System::Guts or its derived classes.
+ *
+ * Below is the physical layout of memory for a System, and which
+ * portions are allocated by the client program and which by the
+ * binary library code. For binary compatiblity, only the side
+ * which allocated a piece of memory can access it. Exception: both
+ * the client and library side must agree on the virtual function
+ * table (VFT) ordering of the client's virtual functions.
+ * <pre>
+ * CLIENT SIDE | LIBRARY SIDE
+ * |
+ * System System::Guts | System::Guts::GutsRep
+ * --------------- ------------------ | -------------
+ * | System::Guts* | --> | System::GutsRep* | --> | GutsRep |
+ * --------------- ------------------ | | |
+ * ^ | Concrete Guts | | | Opaque |
+ * | | class data and | | | stuff |
+ * =============== | virt func table | | | |
+ * Concrete System ------------------ | | |
+ * adds no data | -------------
+ * members
+ * </pre>
+ *
+ * If the concrete System::Guts class also has an opaque implementation,
+ * as it will for concrete Systems provided by Simbody, then
+ * the System author should expose only the data-free handle class
+ * derived from System.
+ */
+class SimTK_SimTKCOMMON_EXPORT System::Guts {
+ class GutsRep;
+ friend class GutsRep;
+
+ // This is the only data member in this class.
+ GutsRep* rep; // opaque implementation of System::Guts base class.
+public:
+ // Note that this serves as a default constructor since both arguments have defaults.
+ explicit Guts(const String& name="<NONAME>",
+ const String& version="0.0.0");
+ virtual ~Guts();
+
+ const String& getName() const;
+ const String& getVersion() const;
+
+ void setHasTimeAdvancedEvents(bool hasEm);
+ bool hasTimeAdvancedEvents() const;
+
+ //////////////////////////////
+ // EVALUATION (REALIZATION) //
+ //////////////////////////////
+
+ // These are the routines to which the System class forwards requests.
+
+ const State& getDefaultState() const;
+ State& updDefaultState();
+
+ void realize(const State& s, Stage g = Stage::HighestRuntime) const;
+
+ SubsystemIndex adoptSubsystem(Subsystem& child);
+
+ int getNumSubsystems() const;
+ const Subsystem& getSubsystem(SubsystemIndex) const;
+ Subsystem& updSubsystem(SubsystemIndex);
+
+ // Obtain the owner handle for this System::Guts object.
+ const System& getSystem() const;
+ System& updSystem();
+
+ void setOwnerHandle(System&);
+ bool hasOwnerHandle() const;
+
+ explicit Guts(class GutsRep* r) : rep(r) { }
+ bool hasRep() const {return rep!=0;}
+ const GutsRep& getRep() const {assert(rep); return *rep;}
+ GutsRep& updRep() const {assert(rep); return *rep;}
+
+ bool systemTopologyHasBeenRealized() const;
+ StageVersion getSystemTopologyCacheVersion() const;
+ void setSystemTopologyCacheVersion(StageVersion topoVersion) const;
+ void invalidateSystemTopologyCache() const;
+
+ // Wrap the cloneImpl virtual method.
+ System::Guts* clone() const;
+
+ // These routines wrap the virtual realize...Impl() methods to ensure
+ // good behavior such as checking that stage requirements are met and
+ // updating the stage at the end. Note that these will do nothing if
+ // the System stage is already at or greater than the indicated stage.
+
+ const State& realizeTopology() const;
+ void realizeModel(State&) const;
+ void realizeInstance (const State& s) const;
+ void realizeTime (const State& s) const;
+ void realizePosition (const State& s) const;
+ void realizeVelocity (const State& s) const;
+ void realizeDynamics (const State& s) const;
+ void realizeAcceleration(const State& s) const;
+ void realizeReport (const State& s) const;
+
+ // These wrap the other virtual methods.
+ void multiplyByN(const State& state, const Vector& u,
+ Vector& dq) const;
+ void multiplyByNTranspose(const State& state, const Vector& fq,
+ Vector& fu) const;
+ void multiplyByNPInv(const State& state, const Vector& dq,
+ Vector& u) const;
+ void multiplyByNPInvTranspose(const State& state, const Vector& fu,
+ Vector& fq) const;
+
+ bool prescribeQ(State&) const;
+ bool prescribeU(State&) const;
+ void getFreeQIndex(const State&, Array_<SystemQIndex>& freeQs) const;
+ void getFreeUIndex(const State&, Array_<SystemUIndex>& freeUs) const;
+
+ void projectQ(State&, Vector& qErrEst,
+ const ProjectOptions& options, ProjectResults& results) const;
+ void projectU(State&, Vector& uErrEst,
+ const ProjectOptions& options, ProjectResults& results) const;
+
+ void handleEvents
+ (State&, Event::Cause, const Array_<EventId>& eventIds,
+ const HandleEventsOptions& options,
+ HandleEventsResults& results) const;
+ void reportEvents(const State&, Event::Cause, const Array_<EventId>& eventIds) const;
+ void calcEventTriggerInfo(const State&, Array_<EventTriggerInfo>&) const;
+ void calcTimeOfNextScheduledEvent(const State&, Real& tNextEvent, Array_<EventId>& eventIds, bool includeCurrentTime) const;
+ void calcTimeOfNextScheduledReport(const State&, Real& tNextEvent, Array_<EventId>& eventIds, bool includeCurrentTime) const;
+
+ void calcDecorativeGeometryAndAppend(const State&, Stage,
+ Array_<DecorativeGeometry>&) const;
+
+
+protected:
+ Guts(const Guts&); // copies the base class; for use from derived class copy constructors
+
+ // The destructor is already virtual; see above.
+
+ virtual System::Guts* cloneImpl() const = 0;
+
+ // Override these to change the evaluation order of the Subsystems.
+ // The default is to evaluate them in increasing order of SubsystemIndex.
+ // These methods should not be called directly; they are invoked by the
+ // above wrapper methods. Note: the wrappers *will not* call these
+ // routines if the system stage has already met the indicated stage level.
+ // If fact these routines will be called only when the system stage
+ // is at the level just prior to the one indicated here. For example,
+ // realizeVelocityImpl() will be called only if the passed-in State
+ // has been determined to have its system stage exactly Stage::Position.
+ virtual int realizeTopologyImpl(State& state) const {return 0;}
+ virtual int realizeModelImpl (State& state) const {return 0;}
+ virtual int realizeInstanceImpl(const State& state) const {return 0;}
+ virtual int realizeTimeImpl (const State& state) const {return 0;}
+ virtual int realizePositionImpl(const State& state) const {return 0;}
+ virtual int realizeVelocityImpl(const State& state) const {return 0;}
+ virtual int realizeDynamicsImpl(const State& state) const {return 0;}
+ virtual int realizeAccelerationImpl(const State& state) const {return 0;}
+ virtual int realizeReportImpl (const State& state) const {return 0;}
+
+ virtual void multiplyByNImpl(const State& state, const Vector& u,
+ Vector& dq) const;
+ virtual void multiplyByNTransposeImpl(const State& state, const Vector& fq,
+ Vector& fu) const;
+ virtual void multiplyByNPInvImpl(const State& state, const Vector& dq,
+ Vector& u) const;
+ virtual void multiplyByNPInvTransposeImpl(const State& state, const Vector& fu,
+ Vector& fq) const;
+
+ // Defaults assume no prescribed motion; hence, no change made.
+ virtual bool prescribeQImpl(State&) const {return false;}
+ virtual bool prescribeUImpl(State&) const {return false;}
+
+
+ // Defaults assume no constraints and return success meaning "all
+ // constraints satisfied".
+ virtual void projectQImpl(State& state, Vector& qErrEst,
+ const ProjectOptions& options, ProjectResults& results) const
+ { results.clear(); results.setExitStatus(ProjectResults::Succeeded); }
+ virtual void projectUImpl(State& state, Vector& uErrEst,
+ const ProjectOptions& options, ProjectResults& results) const
+ { results.clear(); results.setExitStatus(ProjectResults::Succeeded); }
+
+ virtual void handleEventsImpl
+ (State& state, Event::Cause cause, const Array_<EventId>& eventIds,
+ const HandleEventsOptions& options, HandleEventsResults& results) const;
+
+ virtual int reportEventsImpl(const State& state, Event::Cause cause,
+ const Array_<EventId>& eventIds) const;
+
+ virtual int calcEventTriggerInfoImpl(const State& state,
+ Array_<EventTriggerInfo>& info) const;
+
+ virtual int calcTimeOfNextScheduledEventImpl
+ (const State& state, Real& tNextEvent, Array_<EventId>& eventIds,
+ bool includeCurrentTime) const;
+ virtual int calcTimeOfNextScheduledReportImpl
+ (const State& state, Real& tNextEvent, Array_<EventId>& eventIds,
+ bool includeCurrentTime) const;
+
+
+ // Default is that all the state variables are free.
+ virtual void getFreeQIndexImpl
+ (const State& s, Array_<SystemQIndex>& freeQs) const {
+ const unsigned nq = (unsigned)s.getNQ();
+ freeQs.resize(nq);
+ for (unsigned i=0; i<nq; ++i)
+ freeQs[i] = SystemQIndex(i);
+ }
+ virtual void getFreeUIndexImpl
+ (const State& s, Array_<SystemUIndex>& freeUs) const {
+ const unsigned nu = (unsigned)s.getNU();
+ freeUs.resize(nu);
+ for (unsigned i=0; i<nu; ++i)
+ freeUs[i] = SystemUIndex(i);
+ }
+
+private:
+ Guts& operator=(const Guts&); // suppress default copy assignment operator
+
+ class EventTriggerInfoRep;
+
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_SYSTEM_GUTS_H_
diff --git a/SimTKcommon/Simulation/src/Event.cpp b/SimTKcommon/Simulation/src/Event.cpp
new file mode 100644
index 0000000..183bfb9
--- /dev/null
+++ b/SimTKcommon/Simulation/src/Event.cpp
@@ -0,0 +1,187 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+
+/**@file
+ *
+ * Implementation of non-inline methods from the Event classes.
+ */
+
+#include "SimTKcommon/basics.h"
+#include "SimTKcommon/internal/Event.h"
+
+#include <cassert>
+#include <string>
+
+namespace SimTK {
+
+
+const char* Event::getCauseName(Cause cause) {
+ switch(cause) {
+ case Cause::Initialization: return "Initialization";
+ case Cause::Triggered: return "Triggered";
+ case Cause::Scheduled: return "Scheduled";
+ case Cause::TimeAdvanced: return "TimeAdvanced";
+ case Cause::Signaled: return "Signaled";
+ case Cause::Termination: return "Termination";
+ case Cause::Invalid: return "Invalid";
+ }
+ return "UNRECOGNIZED EVENT CAUSE";
+}
+
+
+std::string Event::eventTriggerString(Trigger e) {
+ // Catch special combos first
+ if (e==NoEventTrigger) return "NoEventTrigger";
+ if (e==Falling) return "Falling";
+ if (e==Rising) return "Rising";
+ if (e==AnySignChange) return "AnySignChange";
+
+ // Not a special combo; unmask one at a time.
+ const Trigger triggers[] =
+ { PositiveToNegative,NegativeToPositive,NoEventTrigger };
+ const char *triggerNames[] =
+ { "PositiveToNegative","NegativeToPositive" };
+
+ String s;
+ for (int i=0; triggers[i] != NoEventTrigger; ++i)
+ if (e & triggers[i]) {
+ if (s.size()) s += "|";
+ s += triggerNames[i];
+ e = Trigger((unsigned)e & ~((unsigned)triggers[i]));
+ }
+
+ // should have accounted for everything by now
+ if (e != NoEventTrigger) {
+ char buf[128];
+ std::sprintf(buf, "0x%x", (unsigned)e);
+ if (s.size()) s += " + ";
+ s += "UNRECOGNIZED EVENT TRIGGER GARBAGE ";
+ s += buf;
+ }
+ return s;
+}
+
+
+
+////////////////////////////
+// EVENT TRIGGER INFO REP //
+////////////////////////////
+
+class EventTriggerInfo::EventTriggerInfoRep {
+public:
+ explicit EventTriggerInfoRep(EventTriggerInfo* h)
+ : myHandle(h), eventId(EventId(InvalidIndex)), triggerOnRising(true),
+ triggerOnFalling(true), localizationWindow(Real(0.1))
+ {
+ assert(h);
+ }
+
+private:
+ EventTriggerInfo* myHandle;
+ friend class EventTriggerInfo;
+
+ EventId eventId;
+ bool triggerOnRising;
+ bool triggerOnFalling;
+ Real localizationWindow;
+};
+
+
+
+ ////////////////////////
+ // EVENT TRIGGER INFO //
+ ////////////////////////
+
+EventTriggerInfo::EventTriggerInfo() : rep(0) {
+ rep = new EventTriggerInfoRep(this);
+}
+EventTriggerInfo::~EventTriggerInfo() {
+ if (getRep().myHandle == this)
+ delete rep;
+ rep = 0;
+}
+
+EventTriggerInfo::EventTriggerInfo(EventId eventId) : rep(0) {
+ rep = new EventTriggerInfoRep(this);
+ rep->eventId = eventId;
+}
+
+EventTriggerInfo::EventTriggerInfo(const EventTriggerInfo& src) : rep(0) {
+ rep = new EventTriggerInfoRep(src.getRep());
+ rep->myHandle = this;
+}
+
+EventTriggerInfo&
+EventTriggerInfo::operator=(const EventTriggerInfo& src) {
+ if (&src != this) {
+ if (getRep().myHandle == this)
+ delete rep;
+ rep = new EventTriggerInfoRep(src.getRep());
+ rep->myHandle = this;
+ }
+ return *this;
+}
+
+EventId EventTriggerInfo::getEventId() const {
+ return getRep().eventId;
+}
+bool EventTriggerInfo::shouldTriggerOnRisingSignTransition() const {
+ return getRep().triggerOnRising;
+}
+bool EventTriggerInfo::shouldTriggerOnFallingSignTransition() const {
+ return getRep().triggerOnFalling;
+}
+Real EventTriggerInfo::getRequiredLocalizationTimeWindow() const {
+ return getRep().localizationWindow;
+}
+
+EventTriggerInfo&
+EventTriggerInfo::setEventId(EventId id) {
+ updRep().eventId = id;
+ return *this;
+}
+EventTriggerInfo&
+EventTriggerInfo::setTriggerOnRisingSignTransition(bool shouldTrigger) {
+ updRep().triggerOnRising = shouldTrigger;
+ return *this;
+}
+EventTriggerInfo&
+EventTriggerInfo::setTriggerOnFallingSignTransition(bool shouldTrigger) {
+ updRep().triggerOnFalling = shouldTrigger;
+ return *this;
+}
+EventTriggerInfo&
+EventTriggerInfo::setRequiredLocalizationTimeWindow(Real w) {
+ assert(w > 0);
+ updRep().localizationWindow = w;
+ return *this;
+}
+ ////////////////////////////
+ // EVENT TRIGGER INFO REP //
+ ////////////////////////////
+
+// All inline currently.
+
+} // namespace SimTK
+
diff --git a/SimTKcommon/Simulation/src/EventHandler.cpp b/SimTKcommon/Simulation/src/EventHandler.cpp
new file mode 100644
index 0000000..6c17ca3
--- /dev/null
+++ b/SimTKcommon/Simulation/src/EventHandler.cpp
@@ -0,0 +1,106 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/internal/EventHandler.h"
+
+// Workaround for a Microsoft compiler bug
+#pragma optimize("g", off)
+
+namespace SimTK {
+
+EventHandler::~EventHandler() {
+}
+
+class TriggeredEventHandler::TriggeredEventHandlerImpl {
+public:
+ TriggeredEventHandlerImpl(Stage requiredStage) : requiredStage(requiredStage) {
+ }
+ EventTriggerInfo triggerInfo;
+ Stage requiredStage;
+};
+
+TriggeredEventHandler::TriggeredEventHandler(Stage requiredStage) {
+ impl = new TriggeredEventHandlerImpl(requiredStage);
+}
+
+TriggeredEventHandler::TriggeredEventHandler(const TriggeredEventHandler& clone) {
+ impl = new TriggeredEventHandlerImpl(*clone.impl);
+}
+
+TriggeredEventHandler& TriggeredEventHandler::operator=(const TriggeredEventHandler& clone) {
+ impl = new TriggeredEventHandlerImpl(*clone.impl);
+ return *this;
+}
+
+TriggeredEventHandler::~TriggeredEventHandler() {
+ delete impl;
+}
+
+EventTriggerInfo& TriggeredEventHandler::getTriggerInfo() {
+ return impl->triggerInfo;
+}
+
+Stage TriggeredEventHandler::getRequiredStage() const {
+ return impl->requiredStage;
+}
+
+ScheduledEventHandler::~ScheduledEventHandler() {
+}
+
+class PeriodicEventHandler::PeriodicEventHandlerImpl {
+public:
+ PeriodicEventHandlerImpl(Real eventInterval) : eventInterval(eventInterval) {
+ SimTK_APIARGCHECK1_ALWAYS(eventInterval > 0.0, "PeriodicEventHandlerImpl", "PeriodicEventHandlerImpl", "The interval was %d. It must be > 0", eventInterval);
+ }
+ Real eventInterval;
+};
+
+PeriodicEventHandler::PeriodicEventHandler(Real eventInterval) {
+ impl = new PeriodicEventHandlerImpl(eventInterval);
+}
+
+PeriodicEventHandler::~PeriodicEventHandler() {
+ delete impl;
+}
+
+Real PeriodicEventHandler::getNextEventTime(const State& state, bool includeCurrentTime) const {
+ Real currentTime = state.getTime();
+ long count = (long)std::floor(currentTime/impl->eventInterval);
+ volatile Real eventTime = count*impl->eventInterval;
+ while (eventTime < currentTime || (eventTime == currentTime && !includeCurrentTime)) {
+ count++;
+ eventTime = count*impl->eventInterval;
+ }
+ return eventTime;
+}
+
+Real PeriodicEventHandler::getEventInterval() const {
+ return impl->eventInterval;
+}
+
+void PeriodicEventHandler::setEventInterval(Real eventInterval) {
+ SimTK_APIARGCHECK1_ALWAYS(eventInterval > 0.0, "PeriodicEventHandler", "setEventInterval", "The interval was %d. It must be > 0", eventInterval);
+ impl->eventInterval = eventInterval;
+}
+
+} // namespace SimTK
diff --git a/SimTKcommon/Simulation/src/EventReporter.cpp b/SimTKcommon/Simulation/src/EventReporter.cpp
new file mode 100644
index 0000000..a6754c2
--- /dev/null
+++ b/SimTKcommon/Simulation/src/EventReporter.cpp
@@ -0,0 +1,106 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/internal/EventReporter.h"
+
+// Workaround for a Microsoft compiler bug
+#pragma optimize("g", off)
+
+namespace SimTK {
+
+EventReporter::~EventReporter() {
+}
+
+class TriggeredEventReporter::TriggeredEventReporterImpl {
+public:
+ TriggeredEventReporterImpl(Stage requiredStage) : requiredStage(requiredStage) {
+ }
+ EventTriggerInfo triggerInfo;
+ Stage requiredStage;
+};
+
+TriggeredEventReporter::TriggeredEventReporter(Stage requiredStage) {
+ impl = new TriggeredEventReporterImpl(requiredStage);
+}
+
+TriggeredEventReporter::TriggeredEventReporter(const TriggeredEventReporter& clone) {
+ impl = new TriggeredEventReporterImpl(*clone.impl);
+}
+
+TriggeredEventReporter& TriggeredEventReporter::operator=(const TriggeredEventReporter& clone) {
+ impl = new TriggeredEventReporterImpl(*clone.impl);
+ return *this;
+}
+
+TriggeredEventReporter::~TriggeredEventReporter() {
+ delete impl;
+}
+
+EventTriggerInfo& TriggeredEventReporter::getTriggerInfo() {
+ return impl->triggerInfo;
+}
+
+Stage TriggeredEventReporter::getRequiredStage() const {
+ return impl->requiredStage;
+}
+
+ScheduledEventReporter::~ScheduledEventReporter() {
+}
+
+class PeriodicEventReporter::PeriodicEventReporterImpl {
+public:
+ PeriodicEventReporterImpl(Real eventInterval) : eventInterval(eventInterval) {
+ SimTK_APIARGCHECK1_ALWAYS(eventInterval > 0.0, "PeriodicEventReporterImpl", "PeriodicEventReporterImpl", "The interval was %d. It must be > 0", eventInterval);
+ }
+ Real eventInterval;
+};
+
+PeriodicEventReporter::PeriodicEventReporter(Real eventInterval) {
+ impl = new PeriodicEventReporterImpl(eventInterval);
+}
+
+PeriodicEventReporter::~PeriodicEventReporter() {
+ delete impl;
+}
+
+Real PeriodicEventReporter::getNextEventTime(const State& state, bool includeCurrentTime) const {
+ Real currentTime = state.getTime();
+ long count = (long)std::floor(currentTime/impl->eventInterval);
+ volatile Real eventTime = count*impl->eventInterval;
+ while (eventTime < currentTime || (eventTime == currentTime && !includeCurrentTime)) {
+ count++;
+ eventTime = count*impl->eventInterval;
+ }
+ return eventTime;
+}
+
+Real PeriodicEventReporter::getEventInterval() const {
+ return impl->eventInterval;
+}
+
+void PeriodicEventReporter::setEventInterval(Real eventInterval) {
+ SimTK_APIARGCHECK1_ALWAYS(eventInterval > 0.0, "PeriodicEventReporter", "setEventInterval", "The interval was %d. It must be > 0", eventInterval);
+ impl->eventInterval = eventInterval;
+}
+
+} // namespace SimTK
diff --git a/SimTKcommon/Simulation/src/Measure.cpp b/SimTKcommon/Simulation/src/Measure.cpp
new file mode 100644
index 0000000..26cdc70
--- /dev/null
+++ b/SimTKcommon/Simulation/src/Measure.cpp
@@ -0,0 +1,72 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2009-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+
+/**@file
+ *
+ * Implementation of non-inline methods from the Measure family of classes.
+ */
+
+#include "SimTKcommon/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/Measure.h"
+#include "SimTKcommon/internal/MeasureImplementation.h"
+
+#include <cassert>
+#include <algorithm>
+
+namespace SimTK {
+
+// These are here just to make sure they compile.
+template class Measure_<Real>::Constant;
+template class Measure_<Real>::Zero;
+template class Measure_<Real>::One;
+template class Measure_<Real>::Differentiate;
+template class Measure_<Real>::Integrate;
+template class Measure_<Real>::Result;
+template class Measure_<Real>::Variable;
+template class Measure_<Real>::Extreme;
+template class Measure_<Real>::Delay;
+
+template class Measure_<Vec3>::Constant;
+template class Measure_<Vec3>::Zero;
+template class Measure_<Vec3>::One;
+template class Measure_<Vec3>::Differentiate;
+template class Measure_<Vec3>::Integrate;
+template class Measure_<Vec3>::Result;
+template class Measure_<Vec3>::Variable;
+template class Measure_<Vec3>::Extreme;
+template class Measure_<Vec3>::Delay;
+
+template class Measure_<Vector>::Constant;
+template class Measure_<Vector>::Zero;
+template class Measure_<Vector>::One;
+template class Measure_<Vector>::Differentiate;
+template class Measure_<Vector>::Integrate;
+template class Measure_<Vector>::Result;
+template class Measure_<Vector>::Variable;
+template class Measure_<Vector>::Extreme;
+template class Measure_<Vector>::Delay;
+
+} // namespace SimTK
+
diff --git a/SimTKcommon/Simulation/src/State.cpp b/SimTKcommon/Simulation/src/State.cpp
new file mode 100644
index 0000000..999b029
--- /dev/null
+++ b/SimTKcommon/Simulation/src/State.cpp
@@ -0,0 +1,2964 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * 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/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/Event.h"
+#include "SimTKcommon/internal/State.h"
+
+#include <cassert>
+#include <algorithm>
+#include <ostream>
+#include <set>
+
+namespace SimTK {
+
+// These static methods implement a specialized stacking mechanism for State
+// resources that can be allocated at different stages, which we'll call an
+// "allocation stack". A resource that was
+// allocated at a later stage must be forgotten again when that stage is
+// subsequently invalidated, and keeping their allocations stacked by
+// stage allows that to be done efficiently.
+//
+// The method are templatized and expect the stacks to be in Arrays
+// of the same template. The template value must be a type that supports
+// three methods (the template analog to virtual functions):
+// deepAssign() a non-shallow assignment, i.e. clone the value
+// deepDestruct() destroy any owned heap space
+// getAllocationStage() return the stage being worked on when this was
+// allocated
+// The template type must otherwise support shallow copy semantics so that
+// the Array_ can move them around without causing any heap activity.
+
+// Clear the contents of an allocation stack, freeing up all associated heap space.
+template <class T>
+static void clearAllocationStack(Array_<T>& stack) {
+ for (int i=stack.size()-1; i >= 0; --i)
+ stack[i].deepDestruct();
+ stack.clear();
+}
+
+// Resize the given allocation stack, taking care to free the heap space if the size is reduced.
+template <class T>
+static void resizeAllocationStack(Array_<T>& stack, int newSize) {
+ assert(newSize >= 0);
+ for (int i = stack.size()-1; i >= newSize; --i)
+ stack[i].deepDestruct();
+ stack.resize(newSize);
+}
+
+// Keep only those stack entries whose allocation stage is <= the supplied one.
+template <class T>
+static void popAllocationStackBackToStage(Array_<T>& stack, const Stage& g) {
+ unsigned newSize = stack.size();
+ while (newSize > 0 && stack[newSize-1].getAllocationStage() > g)
+ stack[--newSize].deepDestruct();
+ stack.resize(newSize);
+}
+
+// Make this allocation stack the same as the source, copying only through the given stage.
+template <class T>
+static void copyAllocationStackThroughStage
+ (Array_<T>& stack, const Array_<T>& src, const Stage& g)
+{
+ unsigned nVarsToCopy = src.size(); // assume we'll copy all
+ while (nVarsToCopy && src[nVarsToCopy-1].getAllocationStage() > g)
+ --nVarsToCopy;
+ resizeAllocationStack(stack, nVarsToCopy);
+ for (unsigned i=0; i < nVarsToCopy; ++i)
+ stack[i].deepAssign(src[i]);
+}
+
+// These local classes
+// DiscreteVarInfo
+// CacheVarInfo
+// EventInfo
+// contain the information needed for each discrete variable and cache entry,
+// including a reference to its current value and everything needed to
+// understand its dependencies, as well as information for each Event allocated
+// by a Subsystem.
+//
+// These are intended as elements in an allocation stack as described above,
+// so it is expected that they will get reaallocated, copied, and destructed
+// during allocation as the Array_ gets resized from time to time. However, the
+// discrete variable and cache entry *values* must remain in a fixed location in
+// memory once allocated, because callers are permitted to retain references to
+// these values once they have been allocated. So pointers to the AbstractValues
+// are kept in these objects, and only the pointers are copied around. That
+// means the actual value object will not be deleted by the destructor; be sure
+// to do that explicitly in the higher-level destructor or you'll have a nasty
+// leak.
+
+
+
+//==============================================================================
+// DISCRETE VAR INFO
+//==============================================================================
+class DiscreteVarInfo {
+public:
+ DiscreteVarInfo()
+ : allocationStage(Stage::Empty), invalidatedStage(Stage::Empty),
+ value(0), timeLastUpdated(NaN) {}
+
+ DiscreteVarInfo(Stage allocation, Stage invalidated, AbstractValue* v)
+ : allocationStage(allocation), invalidatedStage(invalidated), value(v),
+ autoUpdateEntry(), timeLastUpdated(NaN)
+ { assert(isReasonable()); }
+
+ // Default copy constructor, copy assignment, destructor are shallow.
+
+ // Use this to make this entry contain a *copy* of the source value.
+ // If the destination already has a value, the new value must be
+ // assignment compatible.
+ DiscreteVarInfo& deepAssign(const DiscreteVarInfo& src) {
+ assert(src.isReasonable());
+
+ allocationStage = src.allocationStage;
+ invalidatedStage = src.invalidatedStage;
+ autoUpdateEntry = src.autoUpdateEntry;
+ if (value) *value = *src.value;
+ else value = src.value->clone();
+ timeLastUpdated = src.timeLastUpdated;
+ return *this;
+ }
+
+ // For use in the containing class's destructor.
+ void deepDestruct() {delete value; value=0;}
+ const Stage& getAllocationStage() const {return allocationStage;}
+
+ // Exchange value pointers (should be from this dv's update cache entry).
+ void swapValue(Real updTime, AbstractValue*& other)
+ { std::swap(value, other); timeLastUpdated=updTime; }
+
+ const AbstractValue& getValue() const {assert(value); return *value;}
+ Real getTimeLastUpdated() const {assert(value); return timeLastUpdated;}
+ AbstractValue& updValue(Real updTime)
+ { assert(value); timeLastUpdated=updTime; return *value; }
+
+ const Stage& getInvalidatedStage() const {return invalidatedStage;}
+ CacheEntryIndex getAutoUpdateEntry() const {return autoUpdateEntry;}
+ void setAutoUpdateEntry(CacheEntryIndex cx) {autoUpdateEntry = cx;}
+
+private:
+ // These are fixed at construction.
+ Stage allocationStage;
+ Stage invalidatedStage;
+ CacheEntryIndex autoUpdateEntry;
+
+ // These change at run time.
+ AbstractValue* value;
+ Real timeLastUpdated;
+
+ bool isReasonable() const
+ { return (allocationStage==Stage::Topology
+ || allocationStage==Stage::Model)
+ && (invalidatedStage > allocationStage)
+ && (value != 0); }
+};
+
+
+
+//==============================================================================
+// CACHE ENTRY INFO
+//==============================================================================
+class CacheEntryInfo {
+public:
+ CacheEntryInfo()
+ : allocationStage(Stage::Empty), dependsOnStage(Stage::Empty), computedByStage(Stage::Empty),
+ value(0), versionWhenLastComputed(-1) {}
+
+ CacheEntryInfo(Stage allocation, Stage dependsOn, Stage computedBy, AbstractValue* v)
+ : allocationStage(allocation), dependsOnStage(dependsOn), computedByStage(computedBy),
+ value(v), versionWhenLastComputed(0)
+ { assert(isReasonable()); }
+
+ bool isCurrent(const Stage& current, const StageVersion versions[]) const
+ { if (current >= computedByStage) return true;
+ if (current < dependsOnStage) return false;
+ return versions[dependsOnStage] == versionWhenLastComputed;}
+
+ StageVersion getVersionWhenLastComputed() const {return versionWhenLastComputed;}
+
+ // These affect only the explicit "last computed" flag which does not fully
+ // determine whether the value is current; see isCurrent() above.
+ void invalidate() {versionWhenLastComputed = 0;}
+ void markAsComputed(const StageVersion versions[])
+ { versionWhenLastComputed = versions[dependsOnStage];}
+
+ // Default copy constructor, copy assignment, destructor are shallow.
+
+ // Use this to make this entry contain a *copy* of the source value.
+ CacheEntryInfo& deepAssign(const CacheEntryInfo& src) {
+ assert(src.isReasonable());
+
+ allocationStage = src.allocationStage;
+ dependsOnStage = src.dependsOnStage;
+ computedByStage = src.computedByStage;
+ associatedVar = src.associatedVar;
+ if (value) *value = *src.value;
+ else value = src.value->clone();
+ versionWhenLastComputed = src.versionWhenLastComputed;
+ return *this;
+ }
+
+ // For use in the containing class's destructor.
+ void deepDestruct() {delete value; value=0;}
+ const Stage& getAllocationStage() const {return allocationStage;}
+
+ // Exchange values with a discrete variable (presumably this
+ // cache entry has been determined to be that variable's update
+ // entry but we're not checking here).
+ void swapValue(Real updTime, DiscreteVarInfo& dv)
+ { dv.swapValue(updTime, value); }
+ const AbstractValue& getValue() const {assert(value); return *value;}
+ AbstractValue& updValue() {assert(value); return *value;}
+
+ const Stage& getDependsOnStage() const {return dependsOnStage;}
+ const Stage& getComputedByStage() const {return computedByStage;}
+ DiscreteVariableIndex getAssociatedVar() const {return associatedVar;}
+ void setAssociatedVar(DiscreteVariableIndex dx) {associatedVar=dx;}
+private:
+ // These are fixed at construction.
+ Stage allocationStage;
+ Stage dependsOnStage;
+ Stage computedByStage;
+ DiscreteVariableIndex associatedVar; // if this is an auto-update entry
+
+ // These change at run time.
+ AbstractValue* value;
+ StageVersion versionWhenLastComputed; // version of Stage dependsOn
+
+ bool isReasonable() const
+ { return ( allocationStage==Stage::Topology
+ || allocationStage==Stage::Model
+ || allocationStage==Stage::Instance)
+ && (computedByStage >= dependsOnStage)
+ && (value != 0)
+ && (versionWhenLastComputed >= 0); }
+};
+
+
+
+//==============================================================================
+// TRIGGER INFO
+//==============================================================================
+class TriggerInfo {
+public:
+ TriggerInfo()
+ : allocationStage(Stage::Empty), firstIndex(-1), nslots(0) {}
+
+ TriggerInfo(Stage allocation, int index, int n)
+ : allocationStage(allocation), firstIndex(index), nslots(n)
+ { assert(isReasonable()); assert(n>0);}
+
+ // Default copy constructor, copy assignment, destructor are fine since
+ // there is no heap object owned here.
+
+ int getFirstIndex() const {return firstIndex;}
+ int getNumSlots() const {return nslots;}
+
+ // These the the "virtual" methods required by template methods elsewhere.
+ TriggerInfo& deepAssign(const TriggerInfo& src)
+ { return operator=(src); }
+ void deepDestruct() {}
+ const Stage& getAllocationStage() const {return allocationStage;}
+private:
+ // These are fixed at construction.
+ Stage allocationStage;
+ int firstIndex;
+ int nslots;
+
+ bool isReasonable() const {
+ return ( allocationStage==Stage::Topology
+ || allocationStage==Stage::Model
+ || allocationStage==Stage::Instance);
+ }
+};
+
+
+
+//==============================================================================
+// CONTINUOUS VAR INFO
+//==============================================================================
+// Used for q, u, and z (in separate stacks).
+// These accumulate default values for this subsystem's use of shared
+// global state variables. After the System is advanced to Stage::Model,
+// the state will allocate those globals and copy the initial
+// values stored here into them. Some of these are allocated at Topology
+// stage, and some at Model stage. If Model stage is invalidated, variables
+// allocated then are forgotten while the ones allocated at Topology stage
+// remain.
+class ContinuousVarInfo {
+public:
+ ContinuousVarInfo() : allocationStage(Stage::Empty), firstIndex(-1) {}
+
+ ContinuousVarInfo(Stage allocation,
+ int index, // QIndex, UIndex, or ZIndex
+ const Vector& initVals,
+ const Vector& varWeights=Vector())
+ : allocationStage(allocation), firstIndex(index), initialValues(initVals)
+ { assert(isReasonable());
+ assert(varWeights.size()==0 || varWeights.size()==initVals.size());
+ assert(weightsAreOK(varWeights));
+ if (varWeights.size()) weights=varWeights;
+ else weights=Vector(initVals.size(), Real(1));
+ }
+
+ int getFirstIndex() const {return firstIndex;}
+ int getNumVars() const {return initialValues.size();}
+ const Vector& getInitialValues() const {return initialValues;}
+ const Vector& getWeights() const {return weights;}
+
+ // Default copy constructor, copy assignment, destructor are fine since
+ // there is no heap object owned here.
+
+ // These the the "virtual" methods required by template methods elsewhere.
+ ContinuousVarInfo& deepAssign(const ContinuousVarInfo& src)
+ { return operator=(src); }
+ void deepDestruct() {}
+ const Stage& getAllocationStage() const {return allocationStage;}
+private:
+ // These are fixed at construction.
+ Stage allocationStage;
+ int firstIndex; // a QIndex, UIndex, or ZIndex
+ Vector initialValues;
+ Vector weights; // only used for u and z
+
+ static bool weightsAreOK(const Vector& wts) {
+ for (int i=0; i<wts.size(); ++i)
+ if (wts[i] <= 0) return false;
+ return true;
+ }
+
+ bool isReasonable() const {
+ return ( allocationStage==Stage::Topology
+ || allocationStage==Stage::Model);
+ }
+};
+
+//==============================================================================
+// CONSTRAINT ERR INFO
+//==============================================================================
+// Used for qerr, uerr, and udoterr.
+class ConstraintErrInfo {
+public:
+ ConstraintErrInfo() : allocationStage(Stage::Empty), firstIndex(-1) {}
+
+ ConstraintErrInfo(Stage allocation,
+ int index, // QErr, UErr, or UDotErrIndex
+ int nerr,
+ const Vector& varWeights=Vector())
+ : allocationStage(allocation), firstIndex(index)
+ { assert(isReasonable());
+ assert(varWeights.size()==0 || varWeights.size()==nerr);
+ assert(weightsAreOK(varWeights));
+ if (varWeights.size()) weights=varWeights;
+ else weights=Vector(nerr, Real(1));
+ }
+
+ int getFirstIndex() const {return firstIndex;}
+ int getNumErrs() const {return weights.size();}
+ const Vector& getWeights() const {return weights;}
+
+ // Default copy constructor, copy assignment, destructor are fine since
+ // there is no heap object owned here.
+
+ // These the the "virtual" methods required by template methods elsewhere.
+ ConstraintErrInfo& deepAssign(const ConstraintErrInfo& src)
+ { return operator=(src); }
+ void deepDestruct() {}
+ const Stage& getAllocationStage() const {return allocationStage;}
+private:
+ // These are fixed at construction.
+ Stage allocationStage;
+ int firstIndex; // a QErrIndex, UErrIndex, or UDotErrIndex
+ Vector weights; // only used for u and z
+
+ static bool weightsAreOK(const Vector& wts) {
+ for (int i=0; i<wts.size(); ++i)
+ if (wts[i] <= 0) return false;
+ return true;
+ }
+
+ bool isReasonable() const {
+ return ( allocationStage==Stage::Topology
+ || allocationStage==Stage::Model
+ || allocationStage==Stage::Instance);
+ }
+};
+
+
+
+//==============================================================================
+// PER SUBSYSTEM INFO
+//==============================================================================
+// This internal utility class is used to capture all the information needed for
+// a single subsystem within the StateImpl.
+class PerSubsystemInfo {
+public:
+ PerSubsystemInfo() : currentStage(Stage::Empty) {initialize();}
+ PerSubsystemInfo(const String& n, const String& v)
+ : name(n), version(v), currentStage(Stage::Empty) {initialize();}
+
+ // Everything will properly clean itself up except for the AbstractValues
+ // stored in the discrete variable and cache entry arrays. Be sure to
+ // delete those prior to allowing the arrays themselves to destruct.
+ ~PerSubsystemInfo() {
+ clearAllStacks();
+ }
+
+ // Copy constructor copies all variables but cache only through
+ // Instance stage. Note that this must be done in conjunction with
+ // copying the whole state or our global resource indices will
+ // be nonsense.
+ PerSubsystemInfo(const PerSubsystemInfo& src) : currentStage(Stage::Empty) {
+ initialize();
+ copyFrom(src, Stage::Instance);
+ }
+
+ PerSubsystemInfo& operator=(const PerSubsystemInfo& src) {
+ // destination is already initialized; copyFrom() will try
+ // to reuse space and will properly clean up unused stuff
+ if (&src != this)
+ copyFrom(src, Stage::Instance);
+ return *this;
+ }
+
+ // Back up to the stage just before g if this subsystem thinks
+ // it is already at g or beyond. Note that we may be backing up
+ // over many stages here. Careful: invalidating the stage
+ // for a subsystem must also invalidate the same stage for all
+ // the other subsystems and the system as a whole but we don't
+ // take care of that here. Also, you can't invalidate Stage::Empty.
+ void invalidateStageJustThisSubsystem(Stage g) {
+ assert(g > Stage::Empty);
+ restoreToStage(g.prev());
+ }
+
+ // Advance from stage g-1 to stage g. This is called at the end
+ // of realize(g). You can't use this to "advance" to Stage::Empty.
+ // It is a fatal error if the current stage isn't g-1.
+ void advanceToStage(Stage g) const {
+ assert(g > Stage::Empty);
+ assert(currentStage == g.prev());
+
+ // This validates whatever the current version number is of Stage g.
+ currentStage = g;
+ }
+
+
+ void clearReferencesToModelStageGlobals() {
+ qstart.invalidate(); ustart.invalidate();
+ zstart.invalidate();
+ q.clear(); u.clear(); z.clear();
+ uWeights.clear(); zWeights.clear();
+ qdot.clear(); udot.clear(); zdot.clear(); qdotdot.clear();
+ }
+
+ void clearReferencesToInstanceStageGlobals() {
+ // These are late-allocated state variables.
+ qerrWeights.clear(); uerrWeights.clear();
+
+ // These are all mutable cache entries.
+ qerrstart.invalidate(); uerrstart.invalidate(); udoterrstart.invalidate();
+ qerr.clear(); uerr.clear(); udoterr.clear(); multipliers.clear();
+
+ for (int j=0; j<Stage::NValid; ++j) {
+ triggerstart[j].invalidate();
+ triggers[j].clear();
+ }
+ }
+
+ QIndex getNextQIndex() const {
+ if (qInfo.empty()) return QIndex(0);
+ const ContinuousVarInfo& last = qInfo.back();
+ return QIndex(last.getFirstIndex()+last.getNumVars());
+ }
+ UIndex getNextUIndex() const {
+ if (uInfo.empty()) return UIndex(0);
+ const ContinuousVarInfo& last = uInfo.back();
+ return UIndex(last.getFirstIndex()+last.getNumVars());
+ }
+ ZIndex getNextZIndex() const {
+ if (zInfo.empty()) return ZIndex(0);
+ const ContinuousVarInfo& last = zInfo.back();
+ return ZIndex(last.getFirstIndex()+last.getNumVars());
+ }
+
+ QErrIndex getNextQErrIndex() const {
+ if (qerrInfo.empty()) return QErrIndex(0);
+ const ConstraintErrInfo& last = qerrInfo.back();
+ return QErrIndex(last.getFirstIndex()+last.getNumErrs());
+ }
+ UErrIndex getNextUErrIndex() const {
+ if (uerrInfo.empty()) return UErrIndex(0);
+ const ConstraintErrInfo& last = uerrInfo.back();
+ return UErrIndex(last.getFirstIndex()+last.getNumErrs());
+ }
+ UDotErrIndex getNextUDotErrIndex() const {
+ if (udoterrInfo.empty()) return UDotErrIndex(0);
+ const ConstraintErrInfo& last = udoterrInfo.back();
+ return UDotErrIndex(last.getFirstIndex()+last.getNumErrs());
+ }
+ DiscreteVariableIndex getNextDiscreteVariableIndex() const {
+ return DiscreteVariableIndex(discreteInfo.size());
+ }
+ CacheEntryIndex getNextCacheEntryIndex() const {
+ return CacheEntryIndex(cacheInfo.size());
+ }
+ EventTriggerByStageIndex getNextEventTriggerByStageIndex(Stage g) const {
+ if (triggerInfo[g].empty()) return EventTriggerByStageIndex(0);
+ const TriggerInfo& last = triggerInfo[g].back();
+ return EventTriggerByStageIndex
+ (last.getFirstIndex()+last.getNumSlots());
+ }
+
+
+ String name;
+ String version;
+
+ // DEFINITIONS //
+
+ // State variables (continuous or discrete) can be defined (allocated)
+ // during realization of Topology or Model stages. Cache entries,
+ // constraint error slots, and event trigger slots can be defined during
+ // realization of Topology, Model, or Instance stages. No further
+ // allocations are allowed. Then, when one of these stages is invalidated,
+ // all the definitions that occurred during realization of that stage must
+ // be forgotten.
+ //
+ // To do that the allocation entries are stored in arrays which are really
+ // stacks, with definitions pushed onto the ends as the stage is advanced
+ // and popped off the ends as the stage is reduced.
+
+ // Topology and Model stage definitions.
+ Array_<ContinuousVarInfo> qInfo, uInfo, zInfo;
+ Array_<DiscreteVarInfo> discreteInfo;
+
+ // Topology, Model, and Instance stage definitions.
+ mutable Array_<ConstraintErrInfo> qerrInfo, uerrInfo, udoterrInfo;
+ mutable Array_<TriggerInfo> triggerInfo[Stage::NValid];
+ mutable Array_<CacheEntryInfo> cacheInfo;
+
+ // GLOBAL RESOURCE ALLOCATIONS //
+
+ // These are our own private views into partitions of the global
+ // state and cache entries of the same names. The State will assign
+ // contiguous blocks to this subsystem when the *System* stage is raised
+ // to Model or Instance stage, and they are invalidated whenever that
+ // stage is invalidated. The starting indices are filled in here at
+ // the time the views are built.
+
+ // Model stage global resources and views into them.
+ SystemQIndex qstart;
+ SystemUIndex ustart;
+ SystemZIndex zstart;
+ Vector q, u, z;
+ Vector uWeights, zWeights;
+
+ mutable Vector qdot, udot, zdot, qdotdot;
+
+ // Instance stage global resources and views into them.
+ Vector qerrWeights, uerrWeights;
+
+ // Note that multipliers just use the same indices as udoterr.
+ mutable SystemQErrIndex qerrstart;
+ mutable SystemUErrIndex uerrstart;
+ mutable SystemUDotErrIndex udoterrstart;
+ mutable SystemEventTriggerByStageIndex triggerstart[Stage::NValid];
+
+ mutable Vector qerr, uerr;
+
+ mutable Vector udoterr, multipliers; // same size and partioning
+ mutable Vector triggers[Stage::NValid];
+
+ // The currentStage is the highest stage of this subsystem that is valid,
+ // meaning that it has been realized since the last change to any variable
+ // that might affect it. All stages less than currentStage are also valid,
+ // and all higher stages are invalid.
+ // Each stage has a "stage version" which is like a serial number that is
+ // bumped every time the stage is invalidated by a variable change. Cache
+ // entries that are calculated from a particular stage version can record
+ // the version number to allow a quick check later -- if the current version
+ // of a cache entry's dependsOn stage is different than the one stored with
+ // the cache entry, then that cache entry cannot be valid.
+ mutable Stage currentStage;
+ mutable StageVersion stageVersions[Stage::NValid];
+
+private:
+ // This is for use in constructors and for resetting an existing State into
+ // its just-constructed condition.
+ void initialize() {
+ clearAllStacks();
+ qstart.invalidate(); ustart.invalidate(); zstart.invalidate();
+ qerrstart.invalidate(); uerrstart.invalidate(); udoterrstart.invalidate();
+ for (int j=0; j<Stage::NValid; ++j) {
+ triggerstart[j].invalidate();
+ stageVersions[j] = 1; // never 0
+ }
+ currentStage = Stage::Empty;
+ }
+
+ // Manage allocation stacks.
+
+ void clearContinuousVars() {clearAllocationStack(qInfo);
+ clearAllocationStack(uInfo);
+ clearAllocationStack(zInfo); }
+ void clearConstraintErrs() {clearAllocationStack(qerrInfo);
+ clearAllocationStack(uerrInfo);
+ clearAllocationStack(udoterrInfo); }
+ void clearDiscreteVars() {clearAllocationStack(discreteInfo);}
+ void clearEventTriggers(int g) {clearAllocationStack(triggerInfo[g]);}
+ void clearCache() {clearAllocationStack(cacheInfo);}
+
+ void clearAllStacks() {
+ clearContinuousVars(); clearDiscreteVars();
+ clearConstraintErrs(); clearCache();
+ for (int i=0; i < Stage::NValid; ++i)
+ clearEventTriggers(i);
+ }
+
+ void popContinuousVarsBackToStage(const Stage& g)
+ { popAllocationStackBackToStage(qInfo,g);
+ popAllocationStackBackToStage(uInfo,g);
+ popAllocationStackBackToStage(zInfo,g); }
+ void popDiscreteVarsBackToStage(const Stage& g)
+ { popAllocationStackBackToStage(discreteInfo,g); }
+ void popConstraintErrsBackToStage(const Stage& g)
+ { popAllocationStackBackToStage(qerrInfo,g);
+ popAllocationStackBackToStage(uerrInfo,g);
+ popAllocationStackBackToStage(udoterrInfo,g); }
+ void popCacheBackToStage(const Stage& g)
+ { popAllocationStackBackToStage(cacheInfo,g); }
+ void popEventTriggersBackToStage(const Stage& g) {
+ for (int i=0; i < Stage::NValid; ++i)
+ popAllocationStackBackToStage(triggerInfo[i],g);
+ }
+
+ void popAllStacksBackToStage(const Stage& g)
+ { popContinuousVarsBackToStage(g);
+ popDiscreteVarsBackToStage(g);
+ popConstraintErrsBackToStage(g);
+ popCacheBackToStage(g);
+ popEventTriggersBackToStage(g); }
+
+ // Call once each for qInfo, uInfo, zInfo.
+ void copyContinuousVarInfoThroughStage
+ (const Array_<ContinuousVarInfo>& src, const Stage& g,
+ Array_<ContinuousVarInfo>& dest)
+ { copyAllocationStackThroughStage(dest, src, g); }
+
+ void copyDiscreteVarsThroughStage
+ (const Array_<DiscreteVarInfo>& src, const Stage& g)
+ { copyAllocationStackThroughStage(discreteInfo, src, g); }
+
+ // Call once each for qerrInfo, uerrInfo, udoterrInfo.
+ void copyConstraintErrInfoThroughStage
+ (const Array_<ConstraintErrInfo>& src, const Stage& g,
+ Array_<ConstraintErrInfo>& dest)
+ { copyAllocationStackThroughStage(dest, src, g); }
+
+ void copyCacheThroughStage
+ (const Array_<CacheEntryInfo>& src, const Stage& g)
+ { copyAllocationStackThroughStage(cacheInfo, src, g); }
+
+ void copyEventsThroughStage
+ (const Array_<TriggerInfo>& src, const Stage& g,
+ Array_<TriggerInfo>& dest)
+ { copyAllocationStackThroughStage(dest, src, g); }
+
+ void copyAllStacksThroughStage(const PerSubsystemInfo& src, const Stage& g)
+ {
+ copyContinuousVarInfoThroughStage(src.qInfo, g, qInfo);
+ copyContinuousVarInfoThroughStage(src.uInfo, g, uInfo);
+ copyContinuousVarInfoThroughStage(src.zInfo, g, zInfo);
+
+ copyDiscreteVarsThroughStage(src.discreteInfo, g);
+
+ copyConstraintErrInfoThroughStage(src.qerrInfo, g, qerrInfo);
+ copyConstraintErrInfoThroughStage(src.uerrInfo, g, uerrInfo);
+ copyConstraintErrInfoThroughStage(src.udoterrInfo, g, udoterrInfo);
+
+ copyCacheThroughStage(src.cacheInfo, g);
+ for (int i=0; i < Stage::NValid; ++i)
+ copyEventsThroughStage(src.triggerInfo[i], g, triggerInfo[i]);
+ }
+
+
+
+ // Restore this subsystem to the way it last was at realize(g) for a given
+ // Stage g; that is, invalidate all stages > g. Allocations will be
+ // forgotten as Instance, Model, and Topology stages are invalidated.
+ void restoreToStage(Stage g) {
+ if (currentStage <= g)
+ return;
+
+ if (g < Stage::Instance) {
+ clearReferencesToInstanceStageGlobals();
+ }
+
+ if (g < Stage::Model) {
+ clearReferencesToModelStageGlobals();
+ }
+
+ if (g == Stage::Empty) {
+ // Throw out everything, reset stage versions to 1. Leave
+ // name and version alone.
+ initialize();
+ return;
+ }
+
+ // Backup all the allocation stacks.
+ popAllStacksBackToStage(g);
+
+ // Raise the version number for every stage that we're invalidating.
+ for (int i=currentStage; i > g; --i)
+ stageVersions[i]++;
+ currentStage = g;
+ }
+
+ // Utility which makes "this" a copy of the source subsystem exactly as it
+ // was after being realized to stage maxStage. If maxStage >= Model then
+ // all the subsystem-private state variables will be copied, but only
+ // cached computations up through maxStage come through. We clear
+ // our references to global variables regardless -- those will have to
+ // be repaired at the System (State global) level.
+ void copyFrom(const PerSubsystemInfo& src, Stage maxStage) {
+ const Stage targetStage = std::min<Stage>(src.currentStage, maxStage);
+
+ // Forget any references to global resources.
+ clearReferencesToInstanceStageGlobals();
+ clearReferencesToModelStageGlobals();
+
+ // Make sure destination state doesn't have anything past targetStage.
+ restoreToStage(targetStage);
+
+ name = src.name;
+ version = src.version;
+ copyAllStacksThroughStage(src, targetStage);
+
+ // Set stage versions so that any cache entries we copied can still
+ // be valid if they were valid in the source and depended only on
+ // things we copied.
+ for (int i=0; i<=targetStage; ++i)
+ stageVersions[i] = src.stageVersions[i];
+ // The rest of the stages need to be invalidated in the destination
+ // since we didn't copy any state information from those stages.
+ for (int i=targetStage+1; i<=src.currentStage; ++i)
+ stageVersions[i] = src.stageVersions[i] + 1;
+
+ // Subsystem stage should now match what we copied.
+ currentStage = targetStage;
+ }
+};
+
+
+//==============================================================================
+// STATE IMPL
+//==============================================================================
+
+class StateImpl {
+ void initializeStageVersions() {
+ for (int i=0; i < Stage::NValid; ++i)
+ systemStageVersions[i] = 1; // never 0
+ }
+public:
+ StateImpl()
+ : t(NaN), currentSystemStage(Stage::Empty) {initializeStageVersions();}
+
+ // We'll do the copy constructor and assignment explicitly here
+ // to get tight control over what's allowed.
+ StateImpl(const StateImpl& src)
+ : currentSystemStage(Stage::Empty)
+ {
+ initializeStageVersions();
+
+ // Make sure that no copied cache entry could accidentally think
+ // it was up to date. We'll change some of these below if appropriate.
+ // (We're skipping the Empty stage 0.)
+ for (int i=1; i <= src.currentSystemStage; ++i)
+ systemStageVersions[i] = src.systemStageVersions[i]+1;
+
+ subsystems = src.subsystems;
+ if (src.currentSystemStage >= Stage::Topology) {
+ advanceSystemToStage(Stage::Topology);
+ systemStageVersions[Stage::Topology] =
+ src.systemStageVersions[Stage::Topology];
+ t = src.t;
+ if (src.currentSystemStage >= Stage::Model) {
+ advanceSystemToStage(Stage::Model);
+ systemStageVersions[Stage::Model] =
+ src.systemStageVersions[Stage::Model];
+ // careful -- don't allow reallocation
+ y = src.y;
+ uWeights = src.uWeights;
+ zWeights = src.zWeights;
+ }
+ if (src.currentSystemStage >= Stage::Instance) {
+ advanceSystemToStage(Stage::Instance);
+ systemStageVersions[Stage::Instance] =
+ src.systemStageVersions[Stage::Instance];
+ // careful -- don't allow reallocation
+ qerrWeights = src.qerrWeights;
+ uerrWeights = src.uerrWeights;
+ }
+ }
+ }
+
+ StateImpl& operator=(const StateImpl& src) {
+ if (&src == this) return *this;
+ invalidateJustSystemStage(Stage::Topology);
+ for (SubsystemIndex i(0); i<(int)subsystems.size(); ++i)
+ subsystems[i].invalidateStageJustThisSubsystem(Stage::Topology);
+
+ // Make sure that no copied cache entry could accidentally think
+ // it was up to date. We'll change some of these below if appropriate.
+ // (We're skipping the Empty stage 0.)
+ for (int i=1; i <= src.currentSystemStage; ++i)
+ systemStageVersions[i] = src.systemStageVersions[i]+1;
+
+ subsystems = src.subsystems;
+ if (src.currentSystemStage >= Stage::Topology) {
+ advanceSystemToStage(Stage::Topology);
+ systemStageVersions[Stage::Topology] =
+ src.systemStageVersions[Stage::Topology];
+ t = src.t;
+ if (src.currentSystemStage >= Stage::Model) {
+ advanceSystemToStage(Stage::Model);
+ systemStageVersions[Stage::Model] =
+ src.systemStageVersions[Stage::Model];
+ // careful -- don't allow reallocation
+ y = src.y;
+ uWeights = src.uWeights;
+ zWeights = src.zWeights;
+ }
+ if (src.currentSystemStage >= Stage::Instance) {
+ advanceSystemToStage(Stage::Instance);
+ systemStageVersions[Stage::Instance] =
+ src.systemStageVersions[Stage::Instance];
+ // careful -- don't allow reallocation
+ qerrWeights = src.qerrWeights;
+ uerrWeights = src.uerrWeights;
+ }
+ }
+ return *this;
+ }
+
+ ~StateImpl() { // default destructor
+ }
+
+ // Copies all the variables but not the cache.
+ StateImpl* clone() const {return new StateImpl(*this);}
+
+ const Stage& getSystemStage() const {return currentSystemStage;}
+ Stage& updSystemStage() const {return currentSystemStage;} // mutable
+
+
+ const PerSubsystemInfo& getSubsystem(int subsystem) const {
+ SimTK_INDEXCHECK(subsystem, (int)subsystems.size(), "StateImpl::getSubsystem()");
+ return subsystems[subsystem];
+ }
+
+ PerSubsystemInfo& updSubsystem(int subsystem) {
+ SimTK_INDEXCHECK(subsystem, (int)subsystems.size(), "StateImpl::updSubsystem()");
+ return subsystems[subsystem];
+ }
+
+ const Stage& getSubsystemStage(int subsystem) const {
+ return subsystems[subsystem].currentStage;
+ }
+ Stage& updSubsystemStage(int subsystem) const {
+ return subsystems[subsystem].currentStage; // mutable
+ }
+
+ const StageVersion* getSubsystemStageVersions(int subsystem) const {
+ return subsystems[subsystem].stageVersions;
+ }
+
+
+ // Back up the System stage just before stg if it thinks
+ // it is already at stg or beyond. Note that we may be backing up
+ // over many stages here. Careful: invalidating the stage
+ // for the system must also invalidate the same stage for all
+ // the subsystems (because we trash the shared resource pool
+ // here if we back up earlier than Stage::Model) but we don't
+ // take care of that here. Also, you can't invalidate Stage::Empty.
+ void invalidateJustSystemStage(Stage stg) {
+ assert(stg > Stage::Empty);
+ if (currentSystemStage < stg)
+ return;
+
+ if (currentSystemStage >= Stage::Instance && Stage::Instance >= stg) {
+ // We are "uninstancing" this State. Trash all the shared
+ // cache entries that are allocated at Instance stage.
+
+ // First make sure no subsystem is looking at the
+ // shared cache entries any more.
+ for (SubsystemIndex i(0); i < (int)subsystems.size(); ++i)
+ subsystems[i].clearReferencesToInstanceStageGlobals();
+
+ // Next get rid of the global views of these cache entries.
+ qerr.clear(); uerr.clear(); // yerr views
+ for (int j=0; j<Stage::NValid; ++j)
+ triggers[j].clear(); // event trigger views
+
+ // Finally nuke the actual cache data.
+ yerr.unlockShape(); yerr.clear();
+ qerrWeights.unlockShape(); qerrWeights.clear();
+ uerrWeights.unlockShape(); uerrWeights.clear();
+ udoterr.unlockShape(); udoterr.clear();
+ multipliers.unlockShape(); multipliers.clear();
+ allTriggers.unlockShape(); allTriggers.clear();
+ }
+ if (currentSystemStage >= Stage::Model && Stage::Model >= stg) {
+ // We are "unmodeling" this State. Trash all the global
+ // shared states & corresponding cache entries.
+
+ // First make sure no subsystem is looking at the
+ // global shared state any more.
+ for (SubsystemIndex i(0); i < (int)subsystems.size(); ++i)
+ subsystems[i].clearReferencesToModelStageGlobals();
+
+ // Next get rid of the global views of these state variables
+ // and corresponding cache entries.
+ q.clear(); u.clear(); z.clear(); // y views
+ // Finally nuke the actual y data.
+ y.unlockShape(); y.clear();
+ uWeights.unlockShape(); uWeights.clear();
+ zWeights.unlockShape(); zWeights.clear();
+
+ qdot.clear(); udot.clear(); zdot.clear(); // ydot views
+ ydot.unlockShape(); ydot.clear(); // ydot data
+ qdotdot.unlockShape(); qdotdot.clear(); // qdotdot data (no views)
+ }
+ if (currentSystemStage >= Stage::Topology && Stage::Topology >= stg) {
+ // We're invalidating the topology stage. Time is considered
+ // a topology stage variable so needs to be invalidated here.
+ t = NaN;
+ }
+
+ // Raise the version number for every stage that we're invalidating and
+ // set the current System Stage one lower than the one being invalidated.
+ for (int i=currentSystemStage; i >= stg; --i)
+ systemStageVersions[i]++;
+ currentSystemStage = stg.prev();
+ }
+
+ // Advance the System stage from stg-1 to stg. It is a fatal error if
+ // we're not already at stg-1, and you can't advance to Stage::Empty.
+ // Also, you can't advance the system to stg unless ALL subsystems have
+ // already gotten there.
+ void advanceSystemToStage(Stage stg) const {
+ assert(stg > Stage::Empty);
+ assert(currentSystemStage == stg.prev());
+ assert(allSubsystemsAtLeastAtStage(stg));
+
+ if (stg == Stage::Topology) {
+ // As the final "Topology" step, initialize time to 0 (it's NaN
+ // before this).
+ const_cast<StateImpl*>(this)->t = 0;
+ }
+ else if (stg == Stage::Model) {
+ // We know the shared state pool sizes now. Allocate the
+ // states and matching shared cache pools.
+ int nq=0, nu=0, nz=0; // total sizes
+ Array_<int> ssnq(subsystems.size(), 0); // per subsystem sizes
+ Array_<int> ssnu(subsystems.size(), 0);
+ Array_<int> ssnz(subsystems.size(), 0);
+
+ // Count up all
+ for (SubsystemIndex i(0); i<subsystems.size(); ++i) {
+ const PerSubsystemInfo& ss = subsystems[i];
+ for (unsigned j=0; j<ss.qInfo.size(); ++j)
+ ssnq[i] += ss.qInfo[j].getNumVars();
+ nq += ssnq[i];
+ for (unsigned j=0; j<ss.uInfo.size(); ++j)
+ ssnu[i] += ss.uInfo[j].getNumVars();
+ nu += ssnu[i];
+ for (unsigned j=0; j<ss.zInfo.size(); ++j)
+ ssnz[i] += ss.zInfo[j].getNumVars();
+ nz += ssnz[i];
+ }
+
+ // Allocate the actual shared state variables & cache
+ // entries and make sure no one can accidentally change the size.
+ // We need write access temporarily to set up the state.
+ StateImpl* wThis = const_cast<StateImpl*>(this);
+ wThis->y.resize(nq+nu+nz); wThis->y.lockShape();
+ wThis->uWeights.resize(nu); wThis->uWeights.lockShape();
+ wThis->zWeights.resize(nz); wThis->zWeights.lockShape();
+
+ ydot.resize(nq+nu+nz); ydot.lockShape();
+ qdotdot.resize(nq); qdotdot.lockShape();
+
+ // Allocate subviews of the shared state & cache entries.
+ wThis->q.viewAssign(wThis->y(0,nq));
+ wThis->u.viewAssign(wThis->y(nq,nu));
+ wThis->z.viewAssign(wThis->y(nq+nu,nz));
+
+ qdot.viewAssign(ydot(0, nq));
+ udot.viewAssign(ydot(nq, nu));
+ zdot.viewAssign(ydot(nq+nu, nz));
+
+ // Now partition the global resources among the subsystems and copy
+ // in the initial values for the state variables.
+ SystemQIndex nxtq(0);
+ SystemUIndex nxtu(0);
+ SystemZIndex nxtz(0);
+
+ for (SubsystemIndex i(0); i<(int)subsystems.size(); ++i) {
+ PerSubsystemInfo& ss =
+ const_cast<PerSubsystemInfo&>(subsystems[i]);
+ const int nq=ssnq[i], nu=ssnu[i], nz=ssnz[i];
+
+ // Assign the starting indices.
+ ss.qstart=nxtq; ss.ustart=nxtu; ss.zstart=nxtz;
+
+ // Build the views.
+ ss.q.viewAssign(wThis->q(nxtq, nq));
+ int nxt=0;
+ for (unsigned j=0; j<ss.qInfo.size(); ++j) {
+ const int nv = ss.qInfo[j].getNumVars();
+ ss.q(nxt, nv) = ss.qInfo[j].getInitialValues();
+ nxt += nv;
+ }
+
+ ss.u.viewAssign(wThis->u(nxtu, nu));
+ ss.uWeights.viewAssign(wThis->uWeights(nxtu, nu));
+ nxt=0;
+ for (unsigned j=0; j<ss.uInfo.size(); ++j) {
+ const int nv = ss.uInfo[j].getNumVars();
+ ss.u(nxt, nv) = ss.uInfo[j].getInitialValues();
+ ss.uWeights(nxt, nv) = ss.uInfo[j].getWeights();
+ nxt += nv;
+ }
+
+ ss.z.viewAssign(wThis->z(nxtz, nz));
+ ss.zWeights.viewAssign(wThis->zWeights(nxtz, nz));
+ nxt=0;
+ for (unsigned j=0; j<ss.zInfo.size(); ++j) {
+ const int nv = ss.zInfo[j].getNumVars();
+ ss.z(nxt, nv) = ss.zInfo[j].getInitialValues();
+ ss.zWeights(nxt, nv) = ss.zInfo[j].getWeights();
+ nxt += nv;
+ }
+
+ ss.qdot.viewAssign(qdot(nxtq, nq));
+ ss.qdotdot.viewAssign(qdotdot(nxtq, nq));
+ ss.udot.viewAssign(udot(nxtu, nu));
+ ss.zdot.viewAssign(zdot(nxtz, nz));
+
+ // Consume the slots.
+ nxtq += nq; nxtu += nu; nxtz += nz;
+ }
+ }
+ else if (stg == Stage::Instance) {
+ // We know the shared cache pool sizes now. Allocate them.
+
+ // Global sizes.
+ int nqerr=0, nuerr=0, nudoterr=0, nAllTriggers=0;
+ Array_<int> ntriggers(Stage::NValid, 0);
+
+ // Per-subsystem sizes.
+ const unsigned nss = subsystems.size();
+ Array_<int> ssnqerr(nss, 0), ssnuerr(nss, 0), ssnudoterr(nss, 0);
+ Array_< Array_<int> > ssntriggers(nss);
+ for (unsigned i=0; i<nss; ++i)
+ ssntriggers[i].resize(Stage::NValid, 0);
+
+ // Count up all
+ for (SubsystemIndex i(0); i<nss; ++i) {
+ const PerSubsystemInfo& ss = subsystems[i];
+ for (unsigned j=0; j<ss.qerrInfo.size(); ++j)
+ ssnqerr[i] += ss.qerrInfo[j].getNumErrs();
+ nqerr += ssnqerr[i];
+ for (unsigned j=0; j<ss.uerrInfo.size(); ++j)
+ ssnuerr[i] += ss.uerrInfo[j].getNumErrs();
+ nuerr += ssnuerr[i];
+ for (unsigned j=0; j<ss.udoterrInfo.size(); ++j)
+ ssnudoterr[i] += ss.udoterrInfo[j].getNumErrs();
+ nudoterr += ssnudoterr[i];
+
+ Array_<int>& ssntrigs = ssntriggers[i];
+ for (int g=0; g<Stage::NValid; ++g)
+ for (unsigned j=0; j<ss.triggerInfo[g].size(); ++j)
+ ssntrigs[g] += ss.triggerInfo[g][j].getNumSlots();
+
+ for (int g=0; g<Stage::NValid; ++g)
+ ntriggers[g] += ssntrigs[g];
+ }
+ for (int g=0; g<Stage::NValid; ++g)
+ nAllTriggers += ntriggers[g];
+
+ // We need write access temporarily to set up the state.
+ StateImpl* wThis = const_cast<StateImpl*>(this);
+ wThis->qerrWeights.resize(nqerr); wThis->qerrWeights.lockShape();
+ wThis->uerrWeights.resize(nuerr); wThis->uerrWeights.lockShape();
+
+ // Allocate the actual shared state variables & cache
+ // entries and make sure no one can accidentally change the size.
+ yerr.resize(nqerr+nuerr); yerr.lockShape();
+
+ udoterr.resize(nudoterr); udoterr.lockShape();
+ multipliers.resize(nudoterr); multipliers.lockShape(); // same size as udoterr
+ allTriggers.resize(nAllTriggers); allTriggers.lockShape();
+
+ // Allocate subviews of the shared state & cache entries.
+
+ qerr.viewAssign(yerr(0, nqerr));
+ uerr.viewAssign(yerr(nqerr, nuerr));
+
+ int stageStart=0;
+ for (int j=0; j<Stage::NValid; ++j) {
+ triggers[j].viewAssign(allTriggers(stageStart, ntriggers[j]));
+ stageStart += ntriggers[j];
+ }
+
+ // Now partition the global resources among the subsystems and copy
+ // in the initial values for the state variables.
+ SystemQErrIndex nxtqerr(0);
+ SystemUErrIndex nxtuerr(0);
+ SystemUDotErrIndex nxtudoterr(0);
+ SystemEventTriggerByStageIndex nxttrigger[Stage::NValid];
+ for (int g=0; g<Stage::NValid; ++g)
+ nxttrigger[g] = SystemEventTriggerByStageIndex(0);
+
+ for (SubsystemIndex i(0); i<(int)subsystems.size(); ++i) {
+ PerSubsystemInfo& ss =
+ const_cast<PerSubsystemInfo&>(subsystems[i]);
+ const int nqerr=ssnqerr[i], nuerr=ssnuerr[i],
+ nudoterr=ssnudoterr[i];
+ const Array_<int>& ssntrigs = ssntriggers[i];
+
+ // Build the views. Only weights need initialization.
+ ss.qerr.viewAssign(qerr(nxtqerr, nqerr));
+ ss.qerrWeights.viewAssign(wThis->qerrWeights(nxtqerr, nqerr));
+ int nxt=0;
+ for (unsigned j=0; j<ss.qerrInfo.size(); ++j) {
+ const int nerr = ss.qerrInfo[j].getNumErrs();
+ ss.qerrWeights(nxt, nerr) = ss.qerrInfo[j].getWeights();
+ nxt += nerr;
+ }
+ ss.uerr.viewAssign(uerr(nxtuerr, nuerr));
+ ss.uerrWeights.viewAssign(wThis->uerrWeights(nxtuerr, nuerr));
+ nxt=0;
+ for (unsigned j=0; j<ss.uerrInfo.size(); ++j) {
+ const int nerr = ss.uerrInfo[j].getNumErrs();
+ ss.uerrWeights(nxt, nerr) = ss.uerrInfo[j].getWeights();
+ nxt += nerr;
+ }
+
+ ss.udoterr.viewAssign(udoterr(nxtudoterr, nudoterr));
+ // multipliers have same partitioning as udoterr
+ ss.multipliers.viewAssign(multipliers(nxtudoterr, nudoterr));
+
+ // Assign the starting indices.
+ ss.qerrstart=nxtqerr; ss.uerrstart=nxtuerr;
+ ss.udoterrstart=nxtudoterr;
+
+ // Consume the slots.
+ nxtqerr += nqerr; nxtuerr += nuerr; nxtudoterr += nudoterr;
+
+ // Same thing for event trigger slots, but by stage.
+ for (int g=0; g<Stage::NValid; ++g) {
+ ss.triggerstart[g] = nxttrigger[g];
+ ss.triggers[g].viewAssign
+ (triggers[g](nxttrigger[g], ssntrigs[g]));
+ nxttrigger[g] += ssntrigs[g];
+ }
+
+ }
+ }
+
+ // All cases fall through to here.
+
+ currentSystemStage = stg;
+ }
+
+
+ void setNumSubsystems(int i) {
+ assert(i >= 0);
+ subsystems.clear();
+ subsystems.resize(i);
+ }
+
+ void initializeSubsystem(SubsystemIndex i, const String& name, const String& version) {
+ updSubsystem(i).name = name;
+ updSubsystem(i).version = version;
+ }
+
+
+ SubsystemIndex addSubsystem(const String& name, const String& version) {
+ const SubsystemIndex sx(subsystems.size());
+ subsystems.push_back(PerSubsystemInfo(name,version));
+ return sx;
+ }
+
+ int getNumSubsystems() const {return (int)subsystems.size();}
+
+ const String& getSubsystemName(SubsystemIndex subsys) const {
+ return subsystems[subsys].name;
+ }
+ const String& getSubsystemVersion(SubsystemIndex subsys) const {
+ return subsystems[subsys].version;
+ }
+
+ // Make sure the stage is no higher than g-1 for *any* subsystem and
+ // hence for the system stage also. TODO: this should be more selective.
+ void invalidateAll(Stage g) {
+ invalidateJustSystemStage(g);
+ for (SubsystemIndex i(0); i<(int)subsystems.size(); ++i)
+ subsystems[i].invalidateStageJustThisSubsystem(g);
+ }
+
+ // Make sure the stage is no higher than g-1 for *any* subsystem and
+ // hence for the system stage also. Same as invalidateAll() except this
+ // requires only const access and can't be used for g below Instance.
+ void invalidateAllCacheAtOrAbove(Stage g) const {
+ SimTK_STAGECHECK_GE_ALWAYS(g, Stage::Instance,
+ "StateImpl::invalidateAllCacheAtOrAbove()");
+
+ // We promise not to hurt this State; get non-const access just so
+ // we can call these methods.
+ StateImpl* mthis = const_cast<StateImpl*>(this);
+ mthis->invalidateJustSystemStage(g);
+ for (SubsystemIndex i(0); i<(int)subsystems.size(); ++i)
+ mthis->subsystems[i].invalidateStageJustThisSubsystem(g);
+ }
+
+ // Move the stage for a particular subsystem from g-1 to g. No other subsystems
+ // are affected, nor the global system stage.
+ void advanceSubsystemToStage(SubsystemIndex subsys, Stage g) const {
+ subsystems[subsys].advanceToStage(g);
+ // We don't automatically advance the System stage even if this brings
+ // ALL the subsystems up to stage g.
+ }
+
+
+
+ // We don't expect State entry allocations to be performance critical so
+ // we'll keep error checking on even in Release mode.
+
+ QIndex allocateQ(SubsystemIndex subsys, const Vector& qInit) {
+ SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), Stage::Model,
+ "StateImpl::allocateQ()");
+ // We are currently realizing the next stage.
+ const Stage allocStage = getSubsystemStage(subsys).next();
+ PerSubsystemInfo& ss = subsystems[subsys];
+ const QIndex nxt(ss.getNextQIndex());
+ ss.qInfo.push_back(ContinuousVarInfo(allocStage,nxt,qInit,Vector()));
+ return nxt;
+ }
+
+ UIndex allocateU(SubsystemIndex subsys, const Vector& uInit) {
+ SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), Stage::Model,
+ "StateImpl::allocateU()");
+ const Stage allocStage = getSubsystemStage(subsys).next();
+ PerSubsystemInfo& ss = subsystems[subsys];
+ const UIndex nxt(ss.getNextUIndex());
+ ss.uInfo.push_back(ContinuousVarInfo(allocStage,nxt,uInit,Vector()));
+ return nxt;
+ }
+ ZIndex allocateZ(SubsystemIndex subsys, const Vector& zInit) {
+ SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), Stage::Model, "StateImpl::allocateZ()");
+ const Stage allocStage = getSubsystemStage(subsys).next();
+ PerSubsystemInfo& ss = subsystems[subsys];
+ const ZIndex nxt(ss.getNextZIndex());
+ ss.zInfo.push_back(ContinuousVarInfo(allocStage,nxt,zInit,Vector()));
+ return nxt;
+ }
+
+ QErrIndex allocateQErr(SubsystemIndex subsys, int nqerr) const {
+ SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), Stage::Instance,
+ "StateImpl::allocateQErr()");
+ const Stage allocStage = getSubsystemStage(subsys).next();
+ const PerSubsystemInfo& ss = subsystems[subsys];
+ const QErrIndex nxt(ss.getNextQErrIndex());
+ ss.qerrInfo.push_back(ConstraintErrInfo(allocStage,nxt,nqerr,Vector()));
+ return nxt;
+ }
+ UErrIndex allocateUErr(SubsystemIndex subsys, int nuerr) const {
+ SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys),
+ Stage::Instance, "StateImpl::allocateUErr()");
+ const Stage allocStage = getSubsystemStage(subsys).next();
+ const PerSubsystemInfo& ss = subsystems[subsys];
+ const UErrIndex nxt(ss.getNextUErrIndex());
+ ss.uerrInfo.push_back(ConstraintErrInfo(allocStage,nxt,nuerr,Vector()));
+ return nxt;
+ }
+ UDotErrIndex allocateUDotErr(SubsystemIndex subsys, int nudoterr) const {
+ SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), Stage::Instance,
+ "StateImpl::allocateUDotErr()");
+ const Stage allocStage = getSubsystemStage(subsys).next();
+ const PerSubsystemInfo& ss = subsystems[subsys];
+ const UDotErrIndex nxt(ss.getNextUDotErrIndex());
+ ss.udoterrInfo.push_back
+ (ConstraintErrInfo(allocStage,nxt,nudoterr,Vector()));
+ return nxt;
+ }
+ EventTriggerByStageIndex allocateEventTrigger
+ (SubsystemIndex subsys, Stage g, int nt) const {
+ SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), Stage::Instance,
+ "StateImpl::allocateEventTrigger()");
+ const Stage allocStage = getSubsystemStage(subsys).next();
+ const PerSubsystemInfo& ss = subsystems[subsys];
+ const EventTriggerByStageIndex nxt(ss.getNextEventTriggerByStageIndex(g));
+ ss.triggerInfo[g].push_back(TriggerInfo(allocStage,nxt,nt));
+ return nxt;
+ }
+
+ // Topology- and Model-stage State variables can only be added during
+ // construction; that is, while stage <= Topology. Other entries can be
+ // added while stage < Model.
+ DiscreteVariableIndex allocateDiscreteVariable
+ (SubsystemIndex subsys, Stage invalidates, AbstractValue* vp)
+ {
+ SimTK_STAGECHECK_RANGE_ALWAYS(Stage(Stage::LowestRuntime).prev(),
+ invalidates, Stage::HighestRuntime,
+ "StateImpl::allocateDiscreteVariable()");
+
+ const Stage maxAcceptable = (invalidates <= Stage::Model
+ ? Stage::Empty : Stage::Topology);
+ SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys),
+ maxAcceptable.next(), "StateImpl::allocateDiscreteVariable()");
+
+ const Stage allocStage = getSubsystemStage(subsys).next();
+ PerSubsystemInfo& ss = subsystems[subsys];
+ const DiscreteVariableIndex nxt(ss.getNextDiscreteVariableIndex());
+ ss.discreteInfo.push_back
+ (DiscreteVarInfo(allocStage,invalidates,vp));
+ return nxt;
+ }
+
+ // Cache entries can be allocated while stage < Instance.
+ CacheEntryIndex allocateCacheEntry
+ (SubsystemIndex subsys, Stage dependsOn, Stage computedBy,
+ AbstractValue* vp) const
+ {
+ SimTK_STAGECHECK_RANGE_ALWAYS(Stage(Stage::LowestRuntime).prev(),
+ dependsOn, Stage::HighestRuntime,
+ "StateImpl::allocateCacheEntry()");
+ SimTK_STAGECHECK_RANGE_ALWAYS(dependsOn, computedBy, Stage::Infinity,
+ "StateImpl::allocateCacheEntry()");
+ SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys),
+ Stage::Instance, "StateImpl::allocateCacheEntry()");
+
+ const Stage allocStage = getSubsystemStage(subsys).next();
+ const PerSubsystemInfo& ss = subsystems[subsys];
+ const CacheEntryIndex nxt(ss.getNextCacheEntryIndex());
+ ss.cacheInfo.push_back(CacheEntryInfo(allocStage,
+ dependsOn,computedBy,vp));//mutable
+ return nxt;
+ }
+
+ // Allocate a discrete variable and a corresponding cache entry for
+ // updating it, and connect them together.
+ DiscreteVariableIndex allocateAutoUpdateDiscreteVariable
+ (SubsystemIndex subsys, Stage invalidates, AbstractValue* vp,
+ Stage updateDependsOn)
+ {
+ const DiscreteVariableIndex dx =
+ allocateDiscreteVariable(subsys,invalidates,vp->clone());
+ const CacheEntryIndex cx =
+ allocateCacheEntry(subsys,updateDependsOn,Stage::Infinity,vp);
+
+ PerSubsystemInfo& ss = subsystems[subsys];
+ DiscreteVarInfo& dvinfo = ss.discreteInfo[dx];
+ CacheEntryInfo& ceinfo = ss.cacheInfo[cx];
+ dvinfo.setAutoUpdateEntry(cx);
+ ceinfo.setAssociatedVar(dx);
+ return dx;
+ }
+
+ // State dimensions for shared continuous variables.
+
+ int getNY() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getNY()");
+ return y.size();
+ }
+
+ SystemYIndex getQStart() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getQStart()");
+ return SystemYIndex(0); // q's come first
+ }
+ int getNQ() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getNQ()");
+ return q.size();
+ }
+
+ SystemYIndex getUStart() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getUStart()");
+ return SystemYIndex(q.size()); // u's come right after q's
+ }
+ int getNU() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getNU()");
+ return u.size();
+ }
+
+ SystemYIndex getZStart() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getZStart()");
+ return SystemYIndex(q.size() + u.size()); // q,u, then z
+ }
+ int getNZ() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getNZ()");
+ return z.size();
+ }
+
+ int getNYErr() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getNYErr()");
+ return yerr.size();
+ }
+
+ SystemYErrIndex getQErrStart() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getQErrStart()");
+ return SystemYErrIndex(0); // qerr's come first
+ }
+ int getNQErr() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getNQErr()");
+ return qerr.size();
+ }
+
+ SystemYErrIndex getUErrStart() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getUErrStart()");
+ return SystemYErrIndex(qerr.size()); // uerr's follow qerrs
+ }
+ int getNUErr() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getNUErr()");
+ return uerr.size();
+ }
+
+ // UDot errors are independent of qerr & uerr.
+ // This is used for multipliers also.
+ int getNUDotErr() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getNUDotErr()");
+ return udoterr.size();
+ }
+
+ int getNEventTriggers() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getNEventTriggers()");
+ return allTriggers.size();
+ }
+
+ SystemEventTriggerIndex getEventTriggerStartByStage(Stage g) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getEventTriggerStartByStage()");
+ int nxt = 0;
+ for (int j=0; j<g; ++j)
+ nxt += triggers[j].size();
+ return SystemEventTriggerIndex(nxt); // g starts where g-1 leaves off
+ }
+
+ int getNEventTriggersByStage(Stage g) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getNEventTriggersByStage()");
+ return triggers[g].size();
+ }
+
+ // Subsystem dimensions.
+
+ SystemQIndex getQStart(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getQStart(subsys)");
+ return getSubsystem(subsys).qstart;
+ }
+ int getNQ(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getNQ(subsys)");
+ return getSubsystem(subsys).q.size();
+ }
+
+ SystemUIndex getUStart(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getUStart(subsys)");
+ return getSubsystem(subsys).ustart;
+ }
+ int getNU(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getNU(subsys)");
+ return getSubsystem(subsys).u.size();
+ }
+
+ SystemZIndex getZStart(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getZStart(subsys)");
+ return getSubsystem(subsys).zstart;
+ }
+ int getNZ(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getNZ(subsys)");
+ return getSubsystem(subsys).z.size();
+ }
+
+ SystemQErrIndex getQErrStart(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getQErrStart(subsys)");
+ return getSubsystem(subsys).qerrstart;
+ }
+ int getNQErr(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getNQErr(subsys)");
+ return getSubsystem(subsys).qerr.size();
+ }
+
+ SystemUErrIndex getUErrStart(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getUErrStart(subsys)");
+ return getSubsystem(subsys).uerrstart;
+ }
+ int getNUErr(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getNUErr(subsys)");
+ return getSubsystem(subsys).uerr.size();
+ }
+
+ // These are used for multipliers also.
+ SystemUDotErrIndex getUDotErrStart(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getUDotErrStart(subsys)");
+ return getSubsystem(subsys).udoterrstart;
+ }
+ int getNUDotErr(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getNUDotErr(subsys)");
+ return getSubsystem(subsys).udoterr.size();
+ }
+
+ SystemEventTriggerByStageIndex getEventTriggerStartByStage(SubsystemIndex subsys, Stage g) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getEventTriggerStartByStage(subsys)");
+ return getSubsystem(subsys).triggerstart[g];
+ }
+
+ int getNEventTriggersByStage(SubsystemIndex subsys, Stage g) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getNEventTriggersByStage(subsys)");
+ return getSubsystem(subsys).triggers[g].size();
+ }
+
+ // Per-subsystem access to the global shared variables.
+
+ const Vector& getQ(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getQ(subsys)");
+ return getSubsystem(subsys).q;
+ }
+ const Vector& getU(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getU(subsys)");
+ return getSubsystem(subsys).u;
+ }
+ const Vector& getZ(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getZ(subsys)");
+ return getSubsystem(subsys).z;
+ }
+
+ const Vector& getUWeights(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
+ "StateImpl::getUWeights(subsys)");
+ return getSubsystem(subsys).uWeights;
+ }
+ const Vector& getZWeights(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
+ "StateImpl::getZWeights(subsys)");
+ return getSubsystem(subsys).zWeights;
+ }
+
+ const Vector& getQDot(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getQDot(subsys)");
+ SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Velocity, "StateImpl::getQDot(subsys)");
+ return getSubsystem(subsys).qdot;
+ }
+ const Vector& getUDot(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getUDot(subsys)");
+ SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Acceleration, "StateImpl::getUDot(subsys)");
+ return getSubsystem(subsys).udot;
+ }
+ const Vector& getZDot(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getZDot(subsys)");
+ SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Dynamics, "StateImpl::getZDot(subsys)");
+ return getSubsystem(subsys).zdot;
+ }
+ const Vector& getQDotDot(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getQDotDot(subsys)");
+ SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Acceleration, "StateImpl::getQDotDot(subsys)");
+ return getSubsystem(subsys).qdotdot;
+ }
+
+ Vector& updQ(SubsystemIndex subsys) {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::updQ(subsys)");
+ invalidateAll(Stage::Position);
+ return updSubsystem(subsys).q;
+ }
+ Vector& updU(SubsystemIndex subsys) {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::updU(subsys)");
+ invalidateAll(Stage::Velocity);
+ return updSubsystem(subsys).u;
+ }
+ Vector& updZ(SubsystemIndex subsys) {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::updZ(subsys)");
+ invalidateAll(Stage::Dynamics);
+ return updSubsystem(subsys).z;
+ }
+
+ Vector& updUWeights(SubsystemIndex subsys) {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
+ "StateImpl::updUWeights(subsys)");
+ invalidateAll(Stage::Report);
+ return updSubsystem(subsys).uWeights;
+ }
+ Vector& updZWeights(SubsystemIndex subsys) {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
+ "StateImpl::updZWeights(subsys)");
+ invalidateAll(Stage::Report);
+ return updSubsystem(subsys).zWeights;
+ }
+
+ // These are mutable so the routines are const.
+
+ Vector& updQDot(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::updQDot(subsys)");
+ return getSubsystem(subsys).qdot;
+ }
+ Vector& updUDot(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::updUDot(subsys)");
+ return getSubsystem(subsys).udot;
+ }
+ Vector& updZDot(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::updZDot(subsys)");
+ return getSubsystem(subsys).zdot;
+ }
+ Vector& updQDotDot(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::updQDotDot(subsys)");
+ return getSubsystem(subsys).qdotdot;
+ }
+
+
+ const Vector& getQErr(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getQErr(subsys)");
+ SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Position, "StateImpl::getQErr(subsys)");
+ return getSubsystem(subsys).qerr;
+ }
+ const Vector& getUErr(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getUErr(subsys)");
+ SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Velocity, "StateImpl::getUErr(subsys)");
+ return getSubsystem(subsys).uerr;
+ }
+
+ const Vector& getQErrWeights(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
+ "StateImpl::getQErrWeights(subsys)");
+ return getSubsystem(subsys).qerrWeights;
+ }
+ const Vector& getUErrWeights(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
+ "StateImpl::getUErrWeights(subsys)");
+ return getSubsystem(subsys).uerrWeights;
+ }
+
+ const Vector& getUDotErr(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getUDotErr(subsys)");
+ SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Acceleration, "StateImpl::getUDotErr(subsys)");
+ return getSubsystem(subsys).udoterr;
+ }
+ const Vector& getMultipliers(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getMultipliers(subsys)");
+ SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Acceleration, "StateImpl::getMultipliers(subsys)");
+ return getSubsystem(subsys).multipliers;
+ }
+
+ const Vector& getEventTriggersByStage(SubsystemIndex subsys, Stage g) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::getEventTriggersByStage(subsys)");
+ SimTK_STAGECHECK_GE(getSubsystemStage(subsys), g, "StateImpl::getEventTriggersByStage(subsys)");
+ return getSubsystem(subsys).triggers[g];
+ }
+
+ Vector& updQErr(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::updQErr(subsys)");
+ return getSubsystem(subsys).qerr;
+ }
+ Vector& updUErr(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::updUErr(subsys)");
+ return getSubsystem(subsys).uerr;
+ }
+
+ Vector& updQErrWeights(SubsystemIndex subsys) {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
+ "StateImpl::updQErrWeights(subsys)");
+ invalidateAll(Stage::Position);
+ return updSubsystem(subsys).qerrWeights;
+ }
+ Vector& updUErrWeights(SubsystemIndex subsys) {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
+ "StateImpl::updUErrWeights(subsys)");
+ invalidateAll(Stage::Velocity);
+ return updSubsystem(subsys).uerrWeights;
+ }
+
+ Vector& updUDotErr(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::updUDotErr(subsys)");
+ return getSubsystem(subsys).udoterr;
+ }
+ Vector& updMultipliers(SubsystemIndex subsys) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::updMultipliers(subsys)");
+ return getSubsystem(subsys).multipliers;
+ }
+ Vector& updEventTriggersByStage(SubsystemIndex subsys, Stage g) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::updEventTriggersByStage(subsys)");
+ return getSubsystem(subsys).triggers[g];
+ }
+
+ // Direct access to the global shared state and cache entries.
+ // Time is allocated in Stage::Topology, State in Stage::Model, and
+ // Cache in Stage::Instance.
+
+ const Real& getTime() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Topology, "StateImpl::getTime()");
+ return t;
+ }
+
+ const Vector& getY() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getY()");
+ return y;
+ }
+
+ const Vector& getQ() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getQ()");
+ return q;
+ }
+
+ const Vector& getU() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getU()");
+ return u;
+ }
+
+ const Vector& getZ() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::getZ()");
+ return z;
+ }
+
+ const Vector& getUWeights() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
+ "StateImpl::getUWeights()");
+ return uWeights;
+ }
+
+ const Vector& getZWeights() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
+ "StateImpl::getZWeights()");
+ return zWeights;
+ }
+
+ // You can call these as long as stage >= allocation stage, but the
+ // stage will be backed up if necessary to one stage prior to the invalidated stage.
+ Real& updTime() { // Back to Stage::Time-1
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Topology, "StateImpl::updTime()");
+ invalidateAll(Stage::Time);
+ return t;
+ }
+
+ Vector& updY() { // Back to Stage::Position-1
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::updY()");
+ invalidateAll(Stage::Position);
+ return y;
+ }
+
+ Vector& updQ() { // Stage::Position-1
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::updQ()");
+ invalidateAll(Stage::Position);
+ return q;
+ }
+
+ Vector& updU() { // Stage::Velocity-1
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::updU()");
+ invalidateAll(Stage::Velocity);
+ return u;
+ }
+
+ Vector& updZ() { // Stage::Dynamics-1
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::updZ()");
+ invalidateAll(Stage::Dynamics);
+ return z;
+ }
+
+ Vector& updUWeights() { // Invalidates Report stage only
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
+ "StateImpl::updUWeights()");
+ invalidateAll(Stage::Report);
+ return uWeights;
+ }
+
+ Vector& updZWeights() { // Invalidates Report stage only
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
+ "StateImpl::updZWeights()");
+ invalidateAll(Stage::Dynamics);
+ return zWeights;
+ }
+
+ // These cache entries you can get at their "computedBy" stages.
+ const Vector& getYDot() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Acceleration, "StateImpl::getYDot()");
+ return ydot;
+ }
+
+ const Vector& getQDot() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Velocity, "StateImpl::getQDot()");
+ return qdot;
+ }
+
+ const Vector& getZDot() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Dynamics, "StateImpl::getZDot()");
+ return zdot;
+ }
+
+ const Vector& getUDot() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Acceleration, "StateImpl::getUDot()");
+ return udot;
+ }
+
+ const Vector& getQDotDot() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Acceleration, "StateImpl::getQDotDot()");
+ return qdotdot;
+ }
+
+ // Cache updates are allowed any time after they have been allocated.
+ Vector& updYDot() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::updYDot()");
+ return ydot;
+ }
+
+ Vector& updQDot() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::updQDot()");
+ return qdot;
+ }
+
+ Vector& updUDot() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::updUDot()");
+ return udot;
+ }
+
+ Vector& updZDot() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::updZDot()");
+ return zdot;
+ }
+
+ Vector& updQDotDot() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, "StateImpl::updQDotDot()");
+ return qdotdot;
+ }
+
+
+ const Vector& getYErr() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Velocity, "StateImpl::getYErr()");
+ return yerr;
+ }
+
+ const Vector& getQErr() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Position, "StateImpl::getQErr()");
+ return qerr;
+ }
+ const Vector& getUErr() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Velocity, "StateImpl::getUErr()");
+ return uerr;
+ }
+
+ const Vector& getQErrWeights() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
+ "StateImpl::getQErrWeights()");
+ return qerrWeights;
+ }
+ const Vector& getUErrWeights() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
+ "StateImpl::getUErrWeights()");
+ return uerrWeights;
+ }
+
+ const Vector& getUDotErr() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Acceleration, "StateImpl::getUDotErr()");
+ return udoterr;
+ }
+ const Vector& getMultipliers() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Acceleration, "StateImpl::getMultipliers()");
+ return multipliers;
+ }
+
+ Vector& updYErr() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::updYErr()");
+ return yerr;
+ }
+ Vector& updQErr() const{
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::updQErr()");
+ return qerr;
+ }
+ Vector& updUErr() const{
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::updUErr()");
+ return uerr;
+ }
+
+ Vector& updQErrWeights() {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
+ "StateImpl::updQErrWeights()");
+ invalidateAll(Stage::Position);
+ return qerrWeights;
+ }
+ Vector& updUErrWeights() {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
+ "StateImpl::updUErrWeights()");
+ invalidateAll(Stage::Velocity);
+ return uerrWeights;
+ }
+
+ Vector& updUDotErr() const{
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
+ "StateImpl::updUDotErr()");
+ return udoterr;
+ }
+ Vector& updMultipliers() const{
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
+ "StateImpl::updMultipliers()");
+ return multipliers;
+ }
+
+ const Vector& getEventTriggers() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Acceleration, "StateImpl::getEventTriggers()");
+ return allTriggers;
+ }
+ const Vector& getEventTriggersByStage(Stage g) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), g, "StateImpl::getEventTriggersByStage()");
+ return triggers[g];
+ }
+
+ // These are mutable; hence 'const'.
+ Vector& updEventTriggers() const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::updEventTriggers()");
+ return allTriggers;
+ }
+ Vector& updEventTriggersByStage(Stage g) const {
+ SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, "StateImpl::updEventTriggersByStage()");
+ return triggers[g];
+ }
+
+ CacheEntryIndex getDiscreteVarUpdateIndex(SubsystemIndex subsys, DiscreteVariableIndex index) const {
+ const PerSubsystemInfo& ss = subsystems[subsys];
+ SimTK_INDEXCHECK(index,(int)ss.discreteInfo.size(),
+ "StateImpl::getDiscreteVarUpdateIndex()");
+ const DiscreteVarInfo& dv = ss.discreteInfo[index];
+ return dv.getAutoUpdateEntry();
+ }
+
+ Stage getDiscreteVarAllocationStage(SubsystemIndex subsys, DiscreteVariableIndex index) const {
+ const PerSubsystemInfo& ss = subsystems[subsys];
+ SimTK_INDEXCHECK(index,(int)ss.discreteInfo.size(),
+ "StateImpl::getDiscreteVarAllocationStage()");
+ const DiscreteVarInfo& dv = ss.discreteInfo[index];
+ return dv.getAllocationStage();
+ }
+
+ Stage getDiscreteVarInvalidatesStage(SubsystemIndex subsys, DiscreteVariableIndex index) const {
+ const PerSubsystemInfo& ss = subsystems[subsys];
+ SimTK_INDEXCHECK(index,(int)ss.discreteInfo.size(),
+ "StateImpl::getDiscreteVarInvalidatesStage()");
+ const DiscreteVarInfo& dv = ss.discreteInfo[index];
+ return dv.getInvalidatedStage();
+ }
+
+ // You can access at any time a variable that was allocated during realizeTopology(),
+ // but don't access others until you have done realizeModel().
+ const AbstractValue&
+ getDiscreteVariable(SubsystemIndex subsys, DiscreteVariableIndex index) const {
+ const PerSubsystemInfo& ss = subsystems[subsys];
+
+ SimTK_INDEXCHECK(index,(int)ss.discreteInfo.size(),"StateImpl::getDiscreteVariable()");
+ const DiscreteVarInfo& dv = ss.discreteInfo[index];
+
+ if (dv.getAllocationStage() > Stage::Topology) {
+ SimTK_STAGECHECK_GE(getSubsystemStage(subsys),
+ Stage::Model, "StateImpl::getDiscreteVariable()");
+ }
+
+ return dv.getValue();
+ }
+ Real getDiscreteVarLastUpdateTime(SubsystemIndex subsys, DiscreteVariableIndex index) const {
+ const PerSubsystemInfo& ss = subsystems[subsys];
+ SimTK_INDEXCHECK(index,(int)ss.discreteInfo.size(),"StateImpl::getDiscreteVarLastUpdateTime()");
+ const DiscreteVarInfo& dv = ss.discreteInfo[index];
+ return dv.getTimeLastUpdated();
+ }
+
+ const AbstractValue& getDiscreteVarUpdateValue(SubsystemIndex subsys, DiscreteVariableIndex index) const {
+ const CacheEntryIndex cx = getDiscreteVarUpdateIndex(subsys,index);
+ SimTK_ERRCHK2(cx.isValid(), "StateImpl::getDiscreteVarUpdateValue()",
+ "Subsystem %d has a discrete variable %d but it does not have an"
+ " associated update cache variable.", (int)subsys, (int)index);
+ return getCacheEntry(subsys, cx);
+ }
+ AbstractValue& updDiscreteVarUpdateValue(SubsystemIndex subsys, DiscreteVariableIndex index) const {
+ const CacheEntryIndex cx = getDiscreteVarUpdateIndex(subsys,index);
+ SimTK_ERRCHK2(cx.isValid(), "StateImpl::updDiscreteVarUpdateValue()",
+ "Subsystem %d has a discrete variable %d but it does not have an"
+ " associated update cache variable.", (int)subsys, (int)index);
+ return updCacheEntry(subsys, cx);
+ }
+ bool isDiscreteVarUpdateValueRealized(SubsystemIndex subsys, DiscreteVariableIndex index) const {
+ const CacheEntryIndex cx = getDiscreteVarUpdateIndex(subsys,index);
+ SimTK_ERRCHK2(cx.isValid(), "StateImpl::isDiscreteVarUpdateValueRealized()",
+ "Subsystem %d has a discrete variable %d but it does not have an"
+ " associated update cache variable.", (int)subsys, (int)index);
+ return isCacheValueRealized(subsys, cx);
+ }
+ void markDiscreteVarUpdateValueRealized(SubsystemIndex subsys, DiscreteVariableIndex index) const {
+ const CacheEntryIndex cx = getDiscreteVarUpdateIndex(subsys,index);
+ SimTK_ERRCHK2(cx.isValid(), "StateImpl::markDiscreteVarUpdateValueRealized()",
+ "Subsystem %d has a discrete variable %d but it does not have an"
+ " associated update cache variable.", (int)subsys, (int)index);
+ markCacheValueRealized(subsys, cx);
+ }
+
+ // You can update at any time a variable that was allocated during
+ // realizeTopology(), but later variables must wait until you have done
+ // realizeModel(). This always backs the stage up to one earlier than the
+ // variable's "invalidates" stage, and if there is an auto-update cache
+ // entry it is also invalidated, regardless of its "depends on" stage.
+ AbstractValue&
+ updDiscreteVariable(SubsystemIndex subsys, DiscreteVariableIndex index) {
+ PerSubsystemInfo& ss = subsystems[subsys];
+
+ SimTK_INDEXCHECK(index,(int)ss.discreteInfo.size(),"StateImpl::updDiscreteVariable()");
+ DiscreteVarInfo& dv = ss.discreteInfo[index];
+
+ if (dv.getAllocationStage() > Stage::Topology) {
+ SimTK_STAGECHECK_GE(getSubsystemStage(subsys),
+ Stage::Model, "StateImpl::updDiscreteVariable()");
+ }
+
+ invalidateAll(dv.getInvalidatedStage());
+
+ // Invalidate the auto-update entry, if any.
+ const CacheEntryIndex cx = dv.getAutoUpdateEntry();
+ if (cx.isValid()) {
+ CacheEntryInfo& ce = ss.cacheInfo[cx];
+ ce.invalidate();
+ }
+
+ // We're now marking this variable as having been updated at the current time.
+ return dv.updValue(t);
+ }
+
+ Stage getCacheEntryAllocationStage(SubsystemIndex subsys, CacheEntryIndex index) const {
+ const PerSubsystemInfo& ss = subsystems[subsys];
+ SimTK_INDEXCHECK(index,(int)ss.cacheInfo.size(),
+ "StateImpl::getCacheEntryAllocationStage()");
+ const CacheEntryInfo& ce = ss.cacheInfo[index];
+ return ce.getAllocationStage();
+ }
+
+ // Stage >= ce.stage
+ // This method gets called a lot, so make it fast in Release mode.
+ const AbstractValue&
+ getCacheEntry(SubsystemIndex subsys, CacheEntryIndex index) const {
+ const PerSubsystemInfo& ss = subsystems[subsys];
+
+ SimTK_INDEXCHECK(index,(int)ss.cacheInfo.size(),"StateImpl::getCacheEntry()");
+ const CacheEntryInfo& ce = ss.cacheInfo[index];
+
+ // These two unconditional checks are somewhat expensive; I'm leaving
+ // them in though because they catch so many user errors.
+ // (sherm 20130222).
+ SimTK_STAGECHECK_GE_ALWAYS(ss.currentStage,
+ ce.getDependsOnStage(), "StateImpl::getCacheEntry()");
+
+ if (ss.currentStage < ce.getComputedByStage()) {
+ const StageVersion currentDependsOnVersion =
+ ss.stageVersions[ce.getDependsOnStage()];
+ const StageVersion lastCacheVersion =
+ ce.getVersionWhenLastComputed();
+
+ if (lastCacheVersion != currentDependsOnVersion) {
+ SimTK_THROW4(Exception::CacheEntryOutOfDate,
+ ss.currentStage, ce.getDependsOnStage(),
+ currentDependsOnVersion, lastCacheVersion);
+ }
+ }
+
+ // If we get here then we're either past the "computed by" stage, or we're
+ // past "depends on" with an explicit validation having been made at the
+ // "depends on" stage's current version.
+
+ return ce.getValue();
+ }
+
+ // You can access a cache entry for update any time after it has been allocated.
+ // This does not affect the stage.
+ AbstractValue&
+ updCacheEntry(SubsystemIndex subsys, CacheEntryIndex index) const {
+ const PerSubsystemInfo& ss = subsystems[subsys];
+
+ SimTK_INDEXCHECK(index,(int)ss.cacheInfo.size(),"StateImpl::updCacheEntry()");
+ CacheEntryInfo& ce = ss.cacheInfo[index];
+
+ return ce.updValue();
+ }
+
+ bool isCacheValueRealized(SubsystemIndex subx, CacheEntryIndex cx) const {
+ const PerSubsystemInfo& ss = subsystems[subx];
+ SimTK_INDEXCHECK(cx,(int)ss.cacheInfo.size(),"StateImpl::isCacheValueRealized()");
+ const CacheEntryInfo& ce = ss.cacheInfo[cx];
+ return ce.isCurrent(getSubsystemStage(subx), getSubsystemStageVersions(subx));
+ }
+ void markCacheValueRealized(SubsystemIndex subx, CacheEntryIndex cx) const {
+ const PerSubsystemInfo& ss = subsystems[subx];
+ SimTK_INDEXCHECK(cx,(int)ss.cacheInfo.size(),"StateImpl::markCacheValueRealized()");
+ CacheEntryInfo& ce = ss.cacheInfo[cx];
+
+ // If this cache entry depends on anything, it can't be valid unless we're
+ // at least *working* on its depends-on stage, meaning the current stage would
+ // have to be the one before that. The depends-on stage is required to be at
+ // least Stage::Topology, so its prev() stage exists.
+ SimTK_STAGECHECK_GE(getSubsystemStage(subx),
+ ce.getDependsOnStage().prev(), "StateImpl::markCacheValueRealized()");
+
+ ce.markAsComputed(getSubsystemStageVersions(subx));
+ }
+
+ void markCacheValueNotRealized(SubsystemIndex subx, CacheEntryIndex cx) const {
+ const PerSubsystemInfo& ss = subsystems[subx];
+ SimTK_INDEXCHECK(cx,(int)ss.cacheInfo.size(),
+ "StateImpl::markCacheValueNotRealized()");
+ CacheEntryInfo& ce = ss.cacheInfo[cx];
+
+ ce.invalidate();
+ }
+
+ StageVersion getSystemTopologyStageVersion() const
+ { return systemStageVersions[Stage::Topology]; }
+
+ void setSystemTopologyStageVersion(StageVersion topoVersion)
+ { assert(topoVersion>0);
+ systemStageVersions[Stage::Topology]=topoVersion; }
+
+ // Capture the stage versions only for currently-realized stages.
+ void getSystemStageVersions(Array_<StageVersion>& versions) const {
+ versions.resize(currentSystemStage+1);
+ for (int i=0; i <= currentSystemStage; ++i)
+ versions[i] = systemStageVersions[i];
+ }
+
+ // If the current state is realized at least as high as the previous one,
+ // then report Stage::Infinity if all of those stage versions match.
+ // Otherwise report either the first mismatch or the first now-invalid
+ // stage if lower stages match.
+ Stage getLowestSystemStageDifference
+ (const Array_<StageVersion>& prevVersions) const {
+ const int nRealizedBefore = (int)prevVersions.size();
+ const int nRealizedNow = (int)currentSystemStage+1; // count from 0
+ const int nRealizedBoth = std::min(nRealizedBefore,nRealizedNow);
+
+ // First check the stages both had in common.
+ Stage g=Stage::Topology;
+ for (; g < nRealizedBoth; ++g)
+ if (systemStageVersions[g] != prevVersions[g])
+ return g;
+
+ // All stages that were valid before and now have identical versions.
+ // If that's all there was before then nothing has changed.
+ return nRealizedNow >= nRealizedBefore ? Stage::Infinity
+ : g; // 1st unrealized stage
+ }
+
+ void autoUpdateDiscreteVariables() {
+ // TODO: make this more efficient
+ for (SubsystemIndex subx(0); subx < subsystems.size(); ++subx) {
+ PerSubsystemInfo& ss = subsystems[subx];
+ Array_<DiscreteVarInfo>& dvars = ss.discreteInfo;
+ for (DiscreteVariableIndex dx(0); dx < dvars.size(); ++dx) {
+ DiscreteVarInfo& dinfo = dvars[dx];
+ const CacheEntryIndex cx = dinfo.getAutoUpdateEntry();
+ if (!cx.isValid()) continue; // not an auto-update variable
+ CacheEntryInfo& cinfo = ss.cacheInfo[cx];
+ if (cinfo.isCurrent(getSubsystemStage(subx), getSubsystemStageVersions(subx)))
+ cinfo.swapValue(getTime(), dinfo);
+ cinfo.invalidate();
+ }
+ }
+ }
+
+ String toString() const {
+ String out;
+ out += "<State>\n";
+
+ out += "<Real name=time>" + String(t) + "</Real>\n";
+
+ out += "<Vector name=q size=" + String(q.size()) + ">";
+ if (q.size()) out += "\n";
+ for (int i=0; i<q.size(); ++i)
+ out += String(q[i]) + "\n";
+ out += "</Vector>\n";
+
+ out += "<Vector name=u size=" + String(u.size()) + ">";
+ if (u.size()) out += "\n";
+ for (int i=0; i<u.size(); ++i)
+ out += String(u[i]) + "\n";
+ out += "</Vector>\n";
+
+ out += "<Vector name=z size=" + String(z.size()) + ">";
+ if (z.size()) out += "\n";
+ for (int i=0; i<z.size(); ++i)
+ out += String(z[i]) + "\n";
+ out += "</Vector>\n";
+
+ out += "<Vector name=uWeights size=" + String(uWeights.size()) + ">";
+ if (uWeights.size()) out += "\n";
+ for (int i=0; i<uWeights.size(); ++i)
+ out += String(uWeights[i]) + "\n";
+ out += "</Vector>\n";
+
+ out += "<Vector name=zWeights size=" + String(zWeights.size()) + ">";
+ if (zWeights.size()) out += "\n";
+ for (int i=0; i<zWeights.size(); ++i)
+ out += String(zWeights[i]) + "\n";
+ out += "</Vector>\n";
+
+ for (SubsystemIndex ss(0); ss < (int)subsystems.size(); ++ss) {
+ const PerSubsystemInfo& info = subsystems[ss];
+ out += "<Subsystem index=" + String(ss) + " name=" + info.name
+ + " version=" + info.version + ">\n";
+
+ out += " <DISCRETE VARS TODO>\n";
+
+ out += " <Vector name=q size=" + String(info.q.size()) + ">\n";
+ out += " <Vector name=u size=" + String(info.u.size()) + ">\n";
+ out += " <Vector name=z size=" + String(info.z.size()) + ">\n";
+
+ out += " <Vector name=uWeights size=" + String(info.uWeights.size()) + ">\n";
+ out += " <Vector name=zWeights size=" + String(info.zWeights.size()) + ">\n";
+ out += "</Subsystem>\n";
+ }
+
+ out += "</State>\n";
+ return out;
+ }
+
+ String cacheToString() const {
+ String out;
+ out += "<Cache>\n";
+ out += "<Stage>" + getSystemStage().getName() + "</Stage>\n";
+
+ for (SubsystemIndex ss(0); ss < (int)subsystems.size(); ++ss) {
+ const PerSubsystemInfo& info = subsystems[ss];
+ out += "<Subsystem index=" + String(ss) + " name=" + info.name
+ + " version=" + info.version + ">\n";
+ out += " <Stage>" + info.currentStage.getName() + "</Stage>\n";
+
+ out += " <DISCRETE CACHE TODO>\n";
+
+ out += " <Vector name=qdot size=" + String(info.qdot.size()) + ">\n";
+ out += " <Vector name=udot size=" + String(info.udot.size()) + ">\n";
+ out += " <Vector name=zdot size=" + String(info.zdot.size()) + ">\n";
+ out += " <Vector name=qdotdot size=" + String(info.qdotdot.size()) + ">\n";
+
+ out += " <Vector name=qerr size=" + String(info.qerr.size()) + ">\n";
+ out += " <Vector name=qerrWeights size=" + String(info.qerrWeights.size()) + ">\n";
+ out += " <Vector name=uerr size=" + String(info.uerr.size()) + ">\n";
+ out += " <Vector name=uerrWeights size=" + String(info.uerrWeights.size()) + ">\n";
+
+ out += " <Vector name=udoterr size=" + String(info.udoterr.size()) + ">\n";
+ out += " <Vector name=multipliers size=" + String(info.multipliers.size()) + ">\n";
+
+
+ for (int j=0; j<Stage::NValid; ++j) {
+ out += " <Vector name=triggers[";
+ out += Stage(j).getName();
+ out += "] size=" + String(info.triggers[j].size()) + ">\n";
+ }
+
+ out += "</Subsystem>\n";
+ }
+
+ out += "<Vector name=qdot size=" + String(qdot.size()) + ">";
+ if (qdot.size()) out += "\n";
+ for (int i=0; i<qdot.size(); ++i)
+ out += String(qdot[i]) + "\n";
+ out += "</Vector>\n";
+
+ out += "<Vector name=udot size=" + String(udot.size()) + ">";
+ if (udot.size()) out += "\n";
+ for (int i=0; i<udot.size(); ++i)
+ out += String(udot[i]) + "\n";
+ out += "</Vector>\n";
+
+ out += "<Vector name=zdot size=" + String(zdot.size()) + ">";
+ if (zdot.size()) out += "\n";
+ for (int i=0; i<zdot.size(); ++i)
+ out += String(zdot[i]) + "\n";
+ out += "</Vector>\n";
+
+ out += "<Vector name=qdotdot size=" + String(qdotdot.size()) + ">";
+ if (qdotdot.size()) out += "\n";
+ for (int i=0; i<qdotdot.size(); ++i)
+ out += String(qdotdot[i]) + "\n";
+ out += "</Vector>\n";
+
+ out += "<Vector name=qerr size=" + String(qerr.size()) + ">";
+ if (qerr.size()) out += "\n";
+ for (int i=0; i<qerr.size(); ++i)
+ out += String(qerr[i]) + "\n";
+ out += "</Vector>\n";
+
+ out += "<Vector name=qerrWeights size=" + String(qerrWeights.size()) + ">";
+ if (qerrWeights.size()) out += "\n";
+ for (int i=0; i<qerrWeights.size(); ++i)
+ out += String(qerrWeights[i]) + "\n";
+ out += "</Vector>\n";
+
+ out += "<Vector name=uerr size=" + String(uerr.size()) + ">";
+ if (uerr.size()) out += "\n";
+ for (int i=0; i<uerr.size(); ++i)
+ out += String(uerr[i]) + "\n";
+ out += "</Vector>\n";
+
+ out += "<Vector name=uerrWeights size=" + String(uerrWeights.size()) + ">";
+ if (uerrWeights.size()) out += "\n";
+ for (int i=0; i<uerrWeights.size(); ++i)
+ out += String(uerrWeights[i]) + "\n";
+ out += "</Vector>\n";
+
+ out += "<Vector name=udoterr size=" + String(udoterr.size()) + ">";
+ if (udoterr.size()) out += "\n";
+ for (int i=0; i<udoterr.size(); ++i)
+ out += String(udoterr[i]) + "\n";
+ out += "</Vector>\n";
+
+ out += "<Vector name=multipliers size=" + String(multipliers.size()) + ">";
+ if (multipliers.size()) out += "\n";
+ for (int i=0; i<multipliers.size(); ++i)
+ out += String(multipliers[i]) + "\n";
+ out += "</Vector>\n";
+
+ out += "</Cache>\n";
+ return out;
+ }
+
+private:
+ // Shared global resource State variables //
+
+ // We consider time t to be a state variable allocated at Topology stage,
+ // with its "invalidated" stage Stage::Time. The value of t is NaN in an Empty
+ // State, and is initialized to zero when the System stage advances
+ // to Stage::Topology (i.e., when the System is realized to stage Topology).
+ Real t;
+
+ // The continuous state variables are allocated at Model stage, and given
+ // their specified initial values when the System stage advances to
+ // Stage::Model (i.e., when the System is realized to Model stage).
+ Vector y; // All the continuous state variables taken together {q,u,z}
+
+ // These are views into y.
+ Vector q; // Stage::Position continuous variables
+ Vector u; // Stage::Velocity continuous variables
+ Vector z; // Stage::Dynamics continuous variables
+
+ // These are not views; there are no qWeights.
+ Vector uWeights; // scaling for u
+ Vector zWeights; // scaling for z
+
+ // These are Instance stage state variables.
+ Vector qerrWeights; // Scaling for perrs
+ Vector uerrWeights; // Scaling for pdoterrs and verrs
+
+ // Shared global resource Cache entries //
+
+ // This is the System's currently highest-valid Stage.
+ mutable Stage currentSystemStage;
+ // This contains a counter for each system stage which is bumped each
+ // time that stage is invalidated. This allows detection of a state
+ // that has been changed even after a subsequent realization. The
+ // Topology stage entry should match the System's Topology version.
+ mutable StageVersion systemStageVersions[Stage::NValid];
+
+ // DIFFERENTIAL EQUATIONS
+
+ // All the state derivatives taken together (qdot,udot,zdot)
+ mutable Vector ydot;
+
+ // These are views into ydot.
+ mutable Vector qdot; // Stage::Velocity
+ mutable Vector udot; // Stage::Acceleration
+ mutable Vector zdot; // Stage::Acceleration
+
+ // This is an independent cache entry.
+ mutable Vector qdotdot; // Stage::Acceleration
+
+ // ALGEBRAIC EQUATIONS
+
+ mutable Vector yerr; // All constraint errors taken together (qerr,uerr)
+ mutable Vector udoterr; // Stage::Acceleration (Index 1 constraints)
+ mutable Vector multipliers; // Stage::Acceleration (Index 1 algebraic variables)
+
+ // These are views into yerr.
+ mutable Vector qerr; // Stage::Position (Index 3 constraints)
+ mutable Vector uerr; // Stage::Velocity (Index 2 constraints)
+
+ // DISCRETE EQUATIONS
+
+ // All the event triggers together, ordered by stage.
+ mutable Vector allTriggers;
+
+ // These are views into allTriggers.
+ mutable Vector triggers[Stage::NValid];
+
+
+ // Subsystem support //
+
+ // Subsystem 0 (always present) is for the System as a whole. Its name
+ // and version are the System name and version.
+ Array_<PerSubsystemInfo> subsystems;
+
+ // Return true only if all subsystems have been realized to at least Stage g.
+ bool allSubsystemsAtLeastAtStage(Stage g) const {
+ for (SubsystemIndex i(0); i < (int)subsystems.size(); ++i)
+ if (subsystems[i].currentStage < g)
+ return false;
+ return true;
+ }
+
+};
+
+
+
+
+//==============================================================================
+// STATE
+//==============================================================================
+
+State::State() {
+ impl = new StateImpl();
+}
+// Restore state to default-constructed condition
+void State::clear() {
+ delete impl;
+ impl = new StateImpl();
+}
+State::~State() {
+ delete impl; impl=0;
+}
+// copy constructor
+State::State(const State& state) {
+ impl = new StateImpl(*state.impl);
+}
+
+// copy assignment
+State& State::operator=(const State& src) {
+ if (&src == this) return *this;
+ if (!impl) {
+ // we're defining this state here (if src is not empty)
+ if (src.impl)
+ impl = src.impl->clone();
+ return *this;
+ }
+
+ // Assignment or redefinition
+ if (src.impl) *impl = *src.impl;
+ else {delete impl; impl=0;}
+ return *this;
+}
+
+
+void State::setNumSubsystems(int i) {
+ updImpl().setNumSubsystems(i);
+}
+void State::initializeSubsystem(SubsystemIndex subsys, const String& name, const String& version) {
+ updImpl().initializeSubsystem(subsys, name, version);
+}
+
+SubsystemIndex State::addSubsystem(const String& name, const String& version) {
+ return updImpl().addSubsystem(name, version);
+}
+int State::getNumSubsystems() const {
+ return getImpl().getNumSubsystems();
+}
+const String& State::getSubsystemName(SubsystemIndex subsys) const {
+ return getImpl().getSubsystemName(subsys);
+}
+const String& State::getSubsystemVersion(SubsystemIndex subsys) const {
+ return getImpl().getSubsystemVersion(subsys);
+}
+const Stage& State::getSubsystemStage(SubsystemIndex subsys) const {
+ return getImpl().getSubsystemStage(subsys);
+}
+const Stage& State::getSystemStage() const {
+ return getImpl().getSystemStage();
+}
+void State::invalidateAll(Stage stage) {
+ updImpl().invalidateAll(stage);
+}
+void State::invalidateAllCacheAtOrAbove(Stage stage) const {
+ getImpl().invalidateAllCacheAtOrAbove(stage);
+}
+void State::advanceSubsystemToStage(SubsystemIndex subsys, Stage stage) const {
+ getImpl().advanceSubsystemToStage(subsys, stage);
+}
+void State::advanceSystemToStage(Stage stage) const {
+ getImpl().advanceSystemToStage(stage);
+}
+QIndex State::allocateQ(SubsystemIndex subsys, const Vector& qInit) {
+ return updImpl().allocateQ(subsys, qInit);
+}
+UIndex State::allocateU(SubsystemIndex subsys, const Vector& uInit) {
+ return updImpl().allocateU(subsys, uInit);
+}
+ZIndex State::allocateZ(SubsystemIndex subsys, const Vector& zInit) {
+ return updImpl().allocateZ(subsys, zInit);
+}
+QErrIndex State::allocateQErr(SubsystemIndex subsys, int nqerr) const {
+ return getImpl().allocateQErr(subsys, nqerr);
+}
+UErrIndex State::allocateUErr(SubsystemIndex subsys, int nuerr) const {
+ return getImpl().allocateUErr(subsys, nuerr);
+}
+UDotErrIndex State::allocateUDotErr(SubsystemIndex subsys, int nudoterr) const {
+ return getImpl().allocateUDotErr(subsys, nudoterr);
+}
+EventTriggerByStageIndex State::allocateEventTrigger(SubsystemIndex subsys, Stage stage, int nevent) const {
+ return getImpl().allocateEventTrigger(subsys, stage, nevent);
+}
+DiscreteVariableIndex State::allocateDiscreteVariable(SubsystemIndex subsys, Stage stage, AbstractValue* v) {
+ return updImpl().allocateDiscreteVariable(subsys, stage, v);
+}
+DiscreteVariableIndex
+State::allocateAutoUpdateDiscreteVariable(SubsystemIndex subsys, Stage invalidates, AbstractValue* v,
+ Stage updateDependsOn) {
+ return updImpl().allocateAutoUpdateDiscreteVariable(subsys, invalidates, v, updateDependsOn);
+}
+CacheEntryIndex State::allocateCacheEntry(SubsystemIndex subsys, Stage dependsOn, Stage computedBy, AbstractValue* v) const {
+ return getImpl().allocateCacheEntry(subsys, dependsOn, computedBy, v);
+}
+int State::getNY() const {
+ return getImpl().getNY();
+}
+SystemYIndex State::getQStart() const {
+ return getImpl().getQStart();
+}
+int State::getNQ() const {
+ return getImpl().getNQ();
+}
+SystemYIndex State::getUStart() const {
+ return getImpl().getUStart();
+}
+int State::getNU() const {
+ return getImpl().getNU();
+}
+SystemYIndex State::getZStart() const {
+ return getImpl().getZStart();
+}
+int State::getNZ() const {
+ return getImpl().getNZ();
+}
+int State::getNYErr() const {
+ return getImpl().getNYErr();
+}
+SystemYErrIndex State::getQErrStart() const {
+ return getImpl().getQErrStart();
+}
+int State::getNQErr() const {
+ return getImpl().getNQErr();
+}
+SystemYErrIndex State::getUErrStart() const {
+ return getImpl().getUErrStart();
+}
+int State::getNUErr() const {
+ return getImpl().getNUErr();
+}
+int State::getNUDotErr() const {
+ return getImpl().getNUDotErr();
+}
+int State::getNMultipliers() const {
+ return getNUDotErr();
+}
+SystemQIndex State::getQStart(SubsystemIndex subsys) const {
+ return getImpl().getQStart(subsys);
+}
+int State::getNQ(SubsystemIndex subsys) const {
+ return getImpl().getNQ(subsys);
+}
+SystemUIndex State::getUStart(SubsystemIndex subsys) const {
+ return getImpl().getUStart(subsys);
+}
+int State::getNU(SubsystemIndex subsys) const {
+ return getImpl().getNU(subsys);
+}
+SystemZIndex State::getZStart(SubsystemIndex subsys) const {
+ return getImpl().getZStart(subsys);
+}
+int State::getNZ(SubsystemIndex subsys) const {
+ return getImpl().getNZ(subsys);
+}
+SystemQErrIndex State::getQErrStart(SubsystemIndex subsys) const {
+ return getImpl().getQErrStart(subsys);
+}
+int State::getNQErr(SubsystemIndex subsys) const {
+ return getImpl().getNQErr(subsys);
+}
+SystemUErrIndex State::getUErrStart(SubsystemIndex subsys) const {
+ return getImpl().getUErrStart(subsys);
+}
+int State::getNUErr(SubsystemIndex subsys) const {
+ return getImpl().getNUErr(subsys);
+}
+SystemUDotErrIndex State::getUDotErrStart(SubsystemIndex subsys) const {
+ return getImpl().getUDotErrStart(subsys);
+}
+int State::getNUDotErr(SubsystemIndex subsys) const {
+ return getImpl().getNUDotErr(subsys);
+}
+SystemMultiplierIndex State::getMultipliersStart(SubsystemIndex i) const {
+ return SystemMultiplierIndex(getUDotErrStart(i));
+}
+int State::getNMultipliers(SubsystemIndex i) const {
+ return getNUDotErr(i);
+}
+int State::getNEventTriggers() const {
+ return getImpl().getNEventTriggers();
+}
+SystemEventTriggerIndex State::getEventTriggerStartByStage(Stage stage) const {
+ return getImpl().getEventTriggerStartByStage(stage);
+}
+int State::getNEventTriggersByStage(Stage stage) const {
+ return getImpl().getNEventTriggersByStage(stage);
+}
+SystemEventTriggerByStageIndex State::getEventTriggerStartByStage(SubsystemIndex subsys, Stage stage) const {
+ return getImpl().getEventTriggerStartByStage(subsys, stage);
+}
+int State::getNEventTriggersByStage(SubsystemIndex subsys, Stage stage) const {
+ return getImpl().getNEventTriggersByStage(subsys, stage);
+}
+const Vector& State::getEventTriggers() const {
+ return getImpl().getEventTriggers();
+}
+const Vector& State::getEventTriggersByStage(Stage stage) const {
+ return getImpl().getEventTriggersByStage(stage);
+}
+const Vector& State::getEventTriggersByStage(SubsystemIndex subsys, Stage stage) const {
+ return getImpl().getEventTriggersByStage(subsys, stage);
+}
+Vector& State::updEventTriggers() const {
+ return getImpl().updEventTriggers();
+}
+Vector& State::updEventTriggersByStage(Stage stage) const {
+ return getImpl().updEventTriggersByStage(stage);
+}
+Vector& State::updEventTriggersByStage(SubsystemIndex subsys, Stage stage) const {
+ return getImpl().updEventTriggersByStage(subsys, stage);
+}
+const Vector& State::getQ(SubsystemIndex subsys) const {
+ return getImpl().getQ(subsys);
+}
+const Vector& State::getU(SubsystemIndex subsys) const {
+ return getImpl().getU(subsys);
+}
+const Vector& State::getZ(SubsystemIndex subsys) const {
+ return getImpl().getZ(subsys);
+}
+const Vector& State::getUWeights(SubsystemIndex subsys) const {
+ return getImpl().getUWeights(subsys);
+}
+const Vector& State::getZWeights(SubsystemIndex subsys) const {
+ return getImpl().getZWeights(subsys);
+}
+Vector& State::updQ(SubsystemIndex subsys) {
+ return updImpl().updQ(subsys);
+}
+Vector& State::updU(SubsystemIndex subsys) {
+ return updImpl().updU(subsys);
+}
+Vector& State::updZ(SubsystemIndex subsys) {
+ return updImpl().updZ(subsys);
+}
+Vector& State::updUWeights(SubsystemIndex subsys) {
+ return updImpl().updUWeights(subsys);
+}
+Vector& State::updZWeights(SubsystemIndex subsys) {
+ return updImpl().updZWeights(subsys);
+}
+const Vector& State::getQDot(SubsystemIndex subsys) const {
+ return getImpl().getQDot(subsys);
+}
+const Vector& State::getUDot(SubsystemIndex subsys) const {
+ return getImpl().getUDot(subsys);
+}
+const Vector& State::getZDot(SubsystemIndex subsys) const {
+ return getImpl().getZDot(subsys);
+}
+const Vector& State::getQDotDot(SubsystemIndex subsys) const {
+ return getImpl().getQDotDot(subsys);
+}
+Vector& State::updQDot(SubsystemIndex subsys) const {
+ return getImpl().updQDot(subsys);
+}
+Vector& State::updUDot(SubsystemIndex subsys) const {
+ return getImpl().updUDot(subsys);
+}
+Vector& State::updZDot(SubsystemIndex subsys) const {
+ return getImpl().updZDot(subsys);
+}
+Vector& State::updQDotDot(SubsystemIndex subsys) const {
+ return getImpl().updQDotDot(subsys);
+}
+const Vector& State::getQErr(SubsystemIndex subsys) const {
+ return getImpl().getQErr(subsys);
+}
+const Vector& State::getUErr(SubsystemIndex subsys) const {
+ return getImpl().getUErr(subsys);
+}
+const Vector& State::getQErrWeights(SubsystemIndex subsys) const {
+ return getImpl().getQErrWeights(subsys);
+}
+const Vector& State::getUErrWeights(SubsystemIndex subsys) const {
+ return getImpl().getUErrWeights(subsys);
+}
+const Vector& State::getUDotErr(SubsystemIndex subsys) const {
+ return getImpl().getUDotErr(subsys);
+}
+const Vector& State::getMultipliers(SubsystemIndex subsys) const {
+ return getImpl().getMultipliers(subsys);
+}
+Vector& State::updQErr(SubsystemIndex subsys) const {
+ return getImpl().updQErr(subsys);
+}
+Vector& State::updUErr(SubsystemIndex subsys) const {
+ return getImpl().updUErr(subsys);
+}
+Vector& State::updQErrWeights(SubsystemIndex subsys) {
+ return updImpl().updQErrWeights(subsys);
+}
+Vector& State::updUErrWeights(SubsystemIndex subsys) {
+ return updImpl().updUErrWeights(subsys);
+}
+Vector& State::updUDotErr(SubsystemIndex subsys) const {
+ return getImpl().updUDotErr(subsys);
+}
+Vector& State::updMultipliers(SubsystemIndex subsys) const {
+ return getImpl().updMultipliers(subsys);
+}
+const Real& State::getTime() const {
+ return getImpl().getTime();
+}
+const Vector& State::getY() const {
+ return getImpl().getY();
+}
+const Vector& State::getQ() const {
+ return getImpl().getQ();
+}
+const Vector& State::getU() const {
+ return getImpl().getU();
+}
+const Vector& State::getZ() const {
+ return getImpl().getZ();
+}
+const Vector& State::getUWeights() const {
+ return getImpl().getUWeights();
+}
+const Vector& State::getZWeights() const {
+ return getImpl().getZWeights();
+}
+Real& State::updTime() {
+ return updImpl().updTime();
+}
+Vector& State::updY() {
+ return updImpl().updY();
+}
+void State::setTime(Real t) {
+ updTime() = t;
+}
+void State::setY(const Vector& y) {
+ updY() = y;
+}
+Vector& State::updQ() {
+ return updImpl().updQ();
+}
+Vector& State::updU() {
+ return updImpl().updU();
+}
+Vector& State::updZ() {
+ return updImpl().updZ();
+}
+Vector& State::updUWeights() {
+ return updImpl().updUWeights();
+}
+Vector& State::updZWeights() {
+ return updImpl().updZWeights();
+}
+void State::setQ(const Vector& q) {
+ updQ() = q;
+}
+void State::setU(const Vector& u) {
+ updU() = u;
+}
+void State::setZ(const Vector& z) {
+ updZ() = z;
+}
+const Vector& State::getYDot() const {
+ return getImpl().getYDot();
+}
+const Vector& State::getQDot() const {
+ return getImpl().getQDot();
+}
+const Vector& State::getZDot() const {
+ return getImpl().getZDot();
+}
+const Vector& State::getUDot() const {
+ return getImpl().getUDot();
+}
+const Vector& State::getQDotDot() const {
+ return getImpl().getQDotDot();
+}
+Vector& State::updYDot() const {
+ return getImpl().updYDot();
+}
+Vector& State::updQDot() const {
+ return getImpl().updQDot();
+}
+Vector& State::updZDot() const {
+ return getImpl().updZDot();
+}
+Vector& State::updUDot() const {
+ return getImpl().updUDot();
+}
+Vector& State::updQDotDot() const {
+ return getImpl().updQDotDot();
+}
+const Vector& State::getYErr() const {
+ return getImpl().getYErr();
+}
+const Vector& State::getQErr() const {
+ return getImpl().getQErr();
+}
+const Vector& State::getUErr() const {
+ return getImpl().getUErr();
+}
+const Vector& State::getQErrWeights() const {
+ return getImpl().getQErrWeights();
+}
+const Vector& State::getUErrWeights() const {
+ return getImpl().getUErrWeights();
+}
+const Vector& State::getUDotErr() const {
+ return getImpl().getUDotErr();
+}
+const Vector& State::getMultipliers() const {
+ return getImpl().getMultipliers();
+}
+Vector& State::updYErr() const {
+ return getImpl().updYErr();
+}
+Vector& State::updQErr() const {
+ return getImpl().updQErr();
+}
+Vector& State::updUErr() const {
+ return getImpl().updUErr();
+}
+Vector& State::updQErrWeights() {
+ return updImpl().updQErrWeights();
+}
+Vector& State::updUErrWeights() {
+ return updImpl().updUErrWeights();
+}
+Vector& State::updUDotErr() const {
+ return getImpl().updUDotErr();
+}
+Vector& State::updMultipliers() const {
+ return getImpl().updMultipliers();
+}
+
+
+
+CacheEntryIndex State::getDiscreteVarUpdateIndex(SubsystemIndex subsys, DiscreteVariableIndex index) const {
+ return getImpl().getDiscreteVarUpdateIndex(subsys, index);
+}
+Stage State::getDiscreteVarAllocationStage(SubsystemIndex subsys, DiscreteVariableIndex index) const {
+ return getImpl().getDiscreteVarAllocationStage(subsys, index);
+}
+Stage State::getDiscreteVarInvalidatesStage(SubsystemIndex subsys, DiscreteVariableIndex index) const {
+ return getImpl().getDiscreteVarInvalidatesStage(subsys, index);
+}
+const AbstractValue& State::getDiscreteVariable(SubsystemIndex subsys, DiscreteVariableIndex index) const {
+ return getImpl().getDiscreteVariable(subsys, index);
+}
+Real State::getDiscreteVarLastUpdateTime(SubsystemIndex subsys, DiscreteVariableIndex index) const {
+ return getImpl().getDiscreteVarLastUpdateTime(subsys, index);
+}
+const AbstractValue& State::getDiscreteVarUpdateValue(SubsystemIndex subsys, DiscreteVariableIndex index) const {
+ return getImpl().getDiscreteVarUpdateValue(subsys, index);
+}
+AbstractValue& State::updDiscreteVarUpdateValue(SubsystemIndex subsys, DiscreteVariableIndex index) const {
+ return getImpl().updDiscreteVarUpdateValue(subsys, index);
+}
+bool State::isDiscreteVarUpdateValueRealized(SubsystemIndex subsys, DiscreteVariableIndex index) const {
+ return getImpl().isDiscreteVarUpdateValueRealized(subsys, index);
+}
+void State::markDiscreteVarUpdateValueRealized(SubsystemIndex subsys, DiscreteVariableIndex index) const {
+ getImpl().markDiscreteVarUpdateValueRealized(subsys, index);
+}
+
+
+AbstractValue& State::updDiscreteVariable(SubsystemIndex subsys, DiscreteVariableIndex index) {
+ return updImpl().updDiscreteVariable(subsys, index);
+}
+void State::setDiscreteVariable(SubsystemIndex i, DiscreteVariableIndex index, const AbstractValue& v) {
+ updDiscreteVariable(i,index) = v;
+}
+
+Stage State::getCacheEntryAllocationStage(SubsystemIndex subsys, CacheEntryIndex index) const {
+ return getImpl().getCacheEntryAllocationStage(subsys, index);
+}
+const AbstractValue& State::getCacheEntry(SubsystemIndex subsys, CacheEntryIndex index) const {
+ return getImpl().getCacheEntry(subsys, index);
+}
+AbstractValue& State::updCacheEntry(SubsystemIndex subsys, CacheEntryIndex index) const {
+ return getImpl().updCacheEntry(subsys, index);
+}
+
+
+bool State::isCacheValueRealized(SubsystemIndex subx, CacheEntryIndex cx) const {
+ return getImpl().isCacheValueRealized(subx, cx);
+}
+void State::markCacheValueRealized(SubsystemIndex subx, CacheEntryIndex cx) const {
+ getImpl().markCacheValueRealized(subx, cx);
+}
+void State::markCacheValueNotRealized(SubsystemIndex subx, CacheEntryIndex cx) const {
+ getImpl().markCacheValueNotRealized(subx, cx);
+}
+
+StageVersion State::getSystemTopologyStageVersion() const
+{ return getImpl().getSystemTopologyStageVersion(); }
+void State::setSystemTopologyStageVersion(StageVersion topoVersion)
+{ return updImpl().setSystemTopologyStageVersion(topoVersion); }
+
+
+void State::getSystemStageVersions(Array_<StageVersion>& versions) const {
+ return getImpl().getSystemStageVersions(versions);
+}
+Stage State::getLowestSystemStageDifference(const Array_<StageVersion>& prev) const {
+ return getImpl().getLowestSystemStageDifference(prev);
+}
+void State::autoUpdateDiscreteVariables() {
+ updImpl().autoUpdateDiscreteVariables();
+}
+
+String State::toString() const {
+ return getImpl().toString();
+}
+String State::cacheToString() const {
+ return getImpl().cacheToString();
+}
+
+std::ostream&
+operator<<(std::ostream& o, const State& s) {
+ o << "STATE:" << std::endl;
+ o << s.toString() << std::endl;
+ o << "CACHE:" << std::endl;
+ return o << s.cacheToString() << std::endl;
+}
+
+} // namespace SimTK
+
diff --git a/SimTKcommon/Simulation/src/Study.cpp b/SimTKcommon/Simulation/src/Study.cpp
new file mode 100644
index 0000000..8e3f625
--- /dev/null
+++ b/SimTKcommon/Simulation/src/Study.cpp
@@ -0,0 +1,188 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+
+/**@file
+ *
+ * Implementation of Study and Study::Guts, and declaration and implementation
+ * of Study::GutsRep.
+ */
+
+#include "SimTKcommon/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/Study.h"
+#include "SimTKcommon/internal/StudyGuts.h"
+
+#include <cassert>
+
+namespace SimTK {
+
+
+// This is the hidden implementation of the Study::Guts abstract base class.
+class Study::Guts::GutsRep {
+public:
+ GutsRep()
+ : studyName("<NONAME>"), studyVersion("0.0.0"), myHandle(0)
+ {
+ }
+ GutsRep(const String& name, const String& version)
+ : studyName(name), studyVersion(version), myHandle(0)
+ {
+ }
+
+ GutsRep(const GutsRep& src)
+ : studyName(src.studyName),
+ studyVersion(src.studyVersion),
+ myHandle(0)
+ {
+ }
+
+ ~GutsRep() {
+ clearMyHandle();
+ }
+
+ const String& getName() const {return studyName;}
+ const String& getVersion() const {return studyVersion;}
+
+ void setMyHandle(Study& h) {myHandle = &h;}
+ void clearMyHandle() {myHandle=0;}
+
+protected:
+ String studyName;
+ String studyVersion;
+
+private:
+ friend class Study;
+ friend class Study::Guts;
+ Study* myHandle; // the owner handle of these guts
+};
+
+ ///////////
+ // STUDY //
+ ///////////
+
+bool Study::isEmptyHandle() const {return guts==0;}
+bool Study::isOwnerHandle() const {return guts==0 || &guts->getStudy()==this;}
+bool Study::isSameStudy(const Study& otherStudy) const {
+ return guts && (guts==otherStudy.guts);
+}
+
+Study::Study(const Study& src) : guts(0) {
+ if (src.guts) {
+ guts = src.guts->clone();
+ guts->setOwnerHandle(*this);
+ }
+}
+
+Study& Study::operator=(const Study& src) {
+ if (!isSameStudy(src)) {
+ if (isOwnerHandle())
+ delete guts;
+ guts=0;
+ if (src.guts) {
+ guts = src.guts->clone();
+ guts->setOwnerHandle(*this);
+ }
+ }
+ return *this;
+}
+
+Study::~Study() {
+ if (guts && isOwnerHandle())
+ delete guts;
+ guts=0;
+}
+
+void Study::adoptStudyGuts(Study::Guts* g) {
+ SimTK_ASSERT_ALWAYS(g, "Study::adoptStudyGuts(): can't adopt null Guts");
+ SimTK_ASSERT_ALWAYS(!guts,
+ "Study::adoptStudyGuts(): this Study handle is already in use");
+ guts = g;
+ guts->setOwnerHandle(*this);
+}
+
+const String& Study::getName() const {return getStudyGuts().getName();}
+const String& Study::getVersion() const {return getStudyGuts().getVersion();}
+
+ /////////////////
+ // STUDY::GUTS //
+ /////////////////
+
+// This is also the default constructor.
+Study::Guts::Guts(const String& name, const String& version) {
+ rep = new GutsRep(name,version);
+ // note that the GutsRep object currently has no owner handle
+}
+
+Study::Guts::~Guts() {
+ delete rep;
+ rep=0;
+}
+
+// Copy constructor
+Study::Guts::Guts(const Guts& src) : rep(0) {
+ if (src.rep) {
+ rep = new GutsRep(*src.rep);
+ // note that the GutsRep object currently has no owner handle
+ }
+}
+
+// Copy assignment is suppressed
+
+
+const Study& Study::Guts::getStudy() const {
+ assert(rep->myHandle);
+ return *rep->myHandle;
+}
+
+Study& Study::Guts::updStudy() {
+ assert(rep->myHandle);
+ return *rep->myHandle;
+}
+
+void Study::Guts::setOwnerHandle(Study& sys) {
+ assert(!rep->myHandle);
+ rep->myHandle = &sys;
+}
+
+bool Study::Guts::hasOwnerHandle() const {
+ return rep->myHandle != 0;
+}
+
+const String& Study::Guts::getName() const {return getRep().getName();}
+const String& Study::Guts::getVersion() const {return getRep().getVersion();}
+
+Study::Guts* Study::Guts::clone() const {
+ return cloneImpl();
+}
+
+
+ //////////////////////////
+ // STUDY::GUTS::GUTSREP //
+ //////////////////////////
+
+// All inline currently.
+
+
+} // namespace SimTK
+
diff --git a/SimTKcommon/Simulation/src/Subsystem.cpp b/SimTKcommon/Simulation/src/Subsystem.cpp
new file mode 100644
index 0000000..e6e2a5f
--- /dev/null
+++ b/SimTKcommon/Simulation/src/Subsystem.cpp
@@ -0,0 +1,736 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+
+/**@file
+ *
+ * Implementation of Subsystem, Subsystem::Guts and DefaultSystemSubsystem.
+ */
+
+#include "SimTKcommon/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/Measure.h"
+#include "SimTKcommon/internal/State.h"
+#include "SimTKcommon/internal/EventHandler.h"
+#include "SimTKcommon/internal/EventReporter.h"
+#include "SimTKcommon/internal/System.h"
+#include "SimTKcommon/internal/Subsystem.h"
+
+#include "SimTKcommon/internal/MeasureImplementation.h"
+
+#include "SystemGutsRep.h"
+#include "SubsystemGutsRep.h"
+
+#include <cassert>
+
+namespace SimTK {
+
+//==============================================================================
+// SUBSYSTEM
+//==============================================================================
+
+bool Subsystem::isEmptyHandle() const {return guts==0;}
+bool Subsystem::isOwnerHandle() const
+{ return guts==0 || &guts->getOwnerSubsystemHandle()==this; }
+bool Subsystem::isSameSubsystem(const Subsystem& otherSubsystem) const
+{ return guts && (guts==otherSubsystem.guts); }
+
+Subsystem::Subsystem(const Subsystem& src) : guts(0) {
+ if (src.guts) {
+ guts = src.guts->clone();
+ guts->setOwnerSubsystemHandle(*this);
+ }
+}
+
+Subsystem& Subsystem::operator=(const Subsystem& src) {
+ if (!isSameSubsystem(src)) {
+ if (isOwnerHandle())
+ delete guts;
+ guts=0;
+ if (src.guts) {
+ guts = src.guts->clone();
+ guts->setOwnerSubsystemHandle(*this);
+ }
+ }
+ return *this;
+}
+
+Subsystem::~Subsystem() {
+ if (guts && isOwnerHandle())
+ delete guts;
+ guts=0;
+}
+
+void Subsystem::adoptSubsystemGuts(Subsystem::Guts* g) {
+ SimTK_ASSERT_ALWAYS(g, "Subsystem::adoptSubsystemGuts(): can't adopt null Guts");
+ SimTK_ASSERT_ALWAYS(!guts,
+ "Subsystem::adoptSubsystemGuts(): this Subsystem handle is already in use");
+ guts = g;
+ guts->setOwnerSubsystemHandle(*this);
+}
+
+void Subsystem::setSystem(System& sys, SubsystemIndex id) {
+ updSubsystemGuts().setSystem(sys,id);
+}
+
+const String& Subsystem::getName() const {return getSubsystemGuts().getName();}
+const String& Subsystem::getVersion() const {return getSubsystemGuts().getVersion();}
+
+bool Subsystem::subsystemTopologyHasBeenRealized() const {
+ return getSubsystemGuts().subsystemTopologyHasBeenRealized();
+}
+
+void Subsystem::invalidateSubsystemTopologyCache() const {
+ getSubsystemGuts().invalidateSubsystemTopologyCache(); // mutable
+}
+
+MeasureIndex Subsystem::adoptMeasure(AbstractMeasure& m)
+{ return updSubsystemGuts().adoptMeasure(m); }
+AbstractMeasure Subsystem::getMeasure(MeasureIndex mx) const
+{ return getSubsystemGuts().getMeasure(mx); }
+
+
+bool Subsystem::isInSystem() const {return getSubsystemGuts().isInSystem();}
+bool Subsystem::isInSameSystem(const Subsystem& otherSubsystem) const {
+ return getSubsystemGuts().isInSameSystem(otherSubsystem);
+}
+
+const System& Subsystem::getSystem() const {return getSubsystemGuts().getSystem();}
+System& Subsystem::updSystem() {return updSubsystemGuts().updSystem();}
+
+SubsystemIndex Subsystem::getMySubsystemIndex() const {
+ return getSubsystemGuts().getMySubsystemIndex();
+}
+
+QIndex Subsystem::allocateQ(State& s, const Vector& qInit)const {return getSubsystemGuts().allocateQ(s,qInit);}
+UIndex Subsystem::allocateU(State& s, const Vector& uInit)const {return getSubsystemGuts().allocateU(s,uInit);}
+ZIndex Subsystem::allocateZ(State& s, const Vector& zInit)const {return getSubsystemGuts().allocateZ(s,zInit);}
+
+DiscreteVariableIndex Subsystem::allocateDiscreteVariable
+ (State& s, Stage g, AbstractValue* v) const
+{ return getSubsystemGuts().allocateDiscreteVariable(s,g,v); }
+DiscreteVariableIndex Subsystem::allocateAutoUpdateDiscreteVariable
+ (State& s, Stage invalidates, AbstractValue* v, Stage updateDependsOn) const
+{ return getSubsystemGuts().allocateAutoUpdateDiscreteVariable(s,invalidates,v,updateDependsOn); }
+
+CacheEntryIndex Subsystem::allocateCacheEntry
+ (const State& s, Stage dependsOn, Stage computedBy, AbstractValue* v) const
+{ return getSubsystemGuts().allocateCacheEntry(s,dependsOn,computedBy,v); }
+
+QErrIndex Subsystem::allocateQErr (const State& s, int nqerr) const {return getSubsystemGuts().allocateQErr(s,nqerr);}
+UErrIndex Subsystem::allocateUErr (const State& s, int nuerr) const {return getSubsystemGuts().allocateUErr(s,nuerr);}
+UDotErrIndex Subsystem::allocateUDotErr (const State& s, int nudoterr) const {return getSubsystemGuts().allocateUDotErr(s,nudoterr);}
+EventTriggerByStageIndex Subsystem::allocateEventTriggersByStage(const State& s, Stage g, int ntriggers) const {return getSubsystemGuts().allocateEventTriggersByStage(s,g,ntriggers);}
+
+const Vector& Subsystem::getQ(const State& s) const {return getSubsystemGuts().getQ(s);}
+const Vector& Subsystem::getU(const State& s) const {return getSubsystemGuts().getU(s);}
+const Vector& Subsystem::getZ(const State& s) const {return getSubsystemGuts().getZ(s);}
+const Vector& Subsystem::getQDot(const State& s) const {return getSubsystemGuts().getQDot(s);}
+const Vector& Subsystem::getUDot(const State& s) const {return getSubsystemGuts().getUDot(s);}
+const Vector& Subsystem::getZDot(const State& s) const {return getSubsystemGuts().getZDot(s);}
+const Vector& Subsystem::getQDotDot(const State& s) const {return getSubsystemGuts().getQDotDot(s);}
+const Vector& Subsystem::getQErr(const State& s) const {return getSubsystemGuts().getQErr(s);}
+const Vector& Subsystem::getUErr(const State& s) const {return getSubsystemGuts().getUErr(s);}
+const Vector& Subsystem::getUDotErr(const State& s) const {return getSubsystemGuts().getUDotErr(s);}
+const Vector& Subsystem::getMultipliers(const State& s) const {return getSubsystemGuts().getMultipliers(s);}
+const Vector& Subsystem::getEventTriggersByStage(const State& s, Stage g) const {return getSubsystemGuts().getEventTriggersByStage(s,g);}
+
+
+Vector& Subsystem::updQ(State& s) const {return getSubsystemGuts().updQ(s);}
+Vector& Subsystem::updU(State& s) const {return getSubsystemGuts().updU(s);}
+Vector& Subsystem::updZ(State& s) const {return getSubsystemGuts().updZ(s);}
+
+Vector& Subsystem::updQDot(const State& s) const {return getSubsystemGuts().updQDot(s);}
+Vector& Subsystem::updUDot(const State& s) const {return getSubsystemGuts().updUDot(s);}
+Vector& Subsystem::updZDot(const State& s) const {return getSubsystemGuts().updZDot(s);}
+Vector& Subsystem::updQDotDot(const State& s) const {return getSubsystemGuts().updQDotDot(s);}
+Vector& Subsystem::updQErr(const State& s) const {return getSubsystemGuts().updQErr(s);}
+Vector& Subsystem::updUErr(const State& s) const {return getSubsystemGuts().updUErr(s);}
+Vector& Subsystem::updUDotErr(const State& s) const {return getSubsystemGuts().updUDotErr(s);}
+Vector& Subsystem::updMultipliers(const State& s) const {return getSubsystemGuts().updMultipliers(s);}
+Vector& Subsystem::updEventTriggersByStage(const State& s, Stage g) const {return getSubsystemGuts().updEventTriggersByStage(s,g);}
+
+Stage Subsystem::getStage(const State& s) const {return getSubsystemGuts().getStage(s);}
+const AbstractValue& Subsystem::getDiscreteVariable(const State& s, DiscreteVariableIndex index) const {
+ return getSubsystemGuts().getDiscreteVariable(s, index);
+}
+AbstractValue& Subsystem::updDiscreteVariable(State& s, DiscreteVariableIndex index) const {
+ return getSubsystemGuts().updDiscreteVariable(s, index);
+}
+const AbstractValue& Subsystem::getCacheEntry(const State& s, CacheEntryIndex index) const {
+ return getSubsystemGuts().getCacheEntry(s, index);
+}
+AbstractValue& Subsystem::updCacheEntry(const State& s, CacheEntryIndex index) const {
+ return getSubsystemGuts().updCacheEntry(s, index);
+}
+bool Subsystem::isCacheValueRealized(const State& s, CacheEntryIndex cx) const {
+ return getSubsystemGuts().isCacheValueRealized(s, cx);
+}
+void Subsystem::markCacheValueRealized(const State& s, CacheEntryIndex cx) const {
+ getSubsystemGuts().markCacheValueRealized(s, cx);
+}
+void Subsystem::markCacheValueNotRealized(const State& s, CacheEntryIndex cx) const {
+ getSubsystemGuts().markCacheValueNotRealized(s, cx);
+}
+
+SystemQIndex Subsystem::getQStart (const State& s) const {return getSubsystemGuts().getQStart(s);}
+int Subsystem::getNQ (const State& s) const {return getSubsystemGuts().getNQ(s);}
+SystemUIndex Subsystem::getUStart (const State& s) const {return getSubsystemGuts().getUStart(s);}
+int Subsystem::getNU (const State& s) const {return getSubsystemGuts().getNU(s);}
+SystemZIndex Subsystem::getZStart (const State& s) const {return getSubsystemGuts().getZStart(s);}
+int Subsystem::getNZ (const State& s) const {return getSubsystemGuts().getNZ(s);}
+SystemQErrIndex Subsystem::getQErrStart (const State& s) const {return getSubsystemGuts().getQErrStart(s);}
+int Subsystem::getNQErr (const State& s) const {return getSubsystemGuts().getNQErr(s);}
+SystemUErrIndex Subsystem::getUErrStart (const State& s) const {return getSubsystemGuts().getUErrStart(s);}
+int Subsystem::getNUErr (const State& s) const {return getSubsystemGuts().getNUErr(s);}
+SystemUDotErrIndex Subsystem::getUDotErrStart(const State& s) const {return getSubsystemGuts().getUDotErrStart(s);}
+int Subsystem::getNUDotErr (const State& s) const {return getSubsystemGuts().getNUDotErr(s);}
+SystemMultiplierIndex Subsystem::getMultipliersStart(const State& s) const {return getSubsystemGuts().getMultipliersStart(s);}
+int Subsystem::getNMultipliers (const State& s) const {return getSubsystemGuts().getNMultipliers(s);}
+SystemEventTriggerByStageIndex Subsystem::getEventTriggerStartByStage(const State& s, Stage g) const {return getSubsystemGuts().getEventTriggerStartByStage(s,g);}
+int Subsystem::getNEventTriggersByStage (const State& s, Stage g) const {return getSubsystemGuts().getNEventTriggersByStage(s,g);}
+
+
+
+//==============================================================================
+// SUBSYSTEM :: GUTS
+//==============================================================================
+// This is also the default constructor.
+Subsystem::Guts::Guts(const String& name, const String& version) {
+ rep = new GutsRep(name,version);
+ // note that the GutsRep object currently has no owner handle
+}
+
+Subsystem::Guts::~Guts() {
+ delete rep;
+ rep=0;
+}
+
+
+// Copy constructor
+Subsystem::Guts::Guts(const Guts& src) : rep(0) {
+ if (src.rep) {
+ rep = new GutsRep(*src.rep);
+ // note that the GutsRep object currently has no owner handle
+ }
+}
+
+// Copy assignment is suppressed
+
+
+const Subsystem& Subsystem::Guts::getOwnerSubsystemHandle() const {
+ assert(rep->myHandle);
+ return *rep->myHandle;
+}
+
+Subsystem& Subsystem::Guts::updOwnerSubsystemHandle() {
+ assert(rep->myHandle);
+ return *rep->myHandle;
+}
+
+void Subsystem::Guts::setOwnerSubsystemHandle(Subsystem& sys) {
+ // might be the first owner or a replacement
+ rep->myHandle = &sys;
+}
+
+bool Subsystem::Guts::hasOwnerSubsystemHandle() const {
+ return rep->myHandle != 0;
+}
+
+void Subsystem::Guts::setSystem(System& sys, SubsystemIndex id) {
+ updRep().setSystem(sys,id);
+}
+
+const String& Subsystem::Guts::getName() const {return getRep().getName();}
+const String& Subsystem::Guts::getVersion() const {return getRep().getVersion();}
+
+MeasureIndex Subsystem::Guts::adoptMeasure(AbstractMeasure& m)
+{ return updRep().adoptMeasure(m); }
+AbstractMeasure Subsystem::Guts::getMeasure(MeasureIndex mx) const
+{ return getRep().getMeasure(mx); }
+
+bool Subsystem::Guts::isInSystem() const {return getRep().isInSystem();}
+bool Subsystem::Guts::isInSameSystem(const Subsystem& otherSubsystem) const {
+ return getRep().isInSameSystem(otherSubsystem);
+}
+const System& Subsystem::Guts::getSystem() const {return getRep().getSystem();}
+System& Subsystem::Guts::updSystem() {return updRep().updSystem();}
+SubsystemIndex Subsystem::Guts::getMySubsystemIndex() const
+{ return getRep().getMySubsystemIndex(); }
+
+QIndex Subsystem::Guts::allocateQ(State& s, const Vector& qInit) const {
+ return s.allocateQ(getRep().getMySubsystemIndex(), qInit);
+}
+
+UIndex Subsystem::Guts::allocateU(State& s, const Vector& uInit) const {
+ return s.allocateU(getRep().getMySubsystemIndex(), uInit);
+}
+
+ZIndex Subsystem::Guts::allocateZ(State& s, const Vector& zInit) const {
+ return s.allocateZ(getRep().getMySubsystemIndex(), zInit);
+}
+
+DiscreteVariableIndex Subsystem::Guts::allocateDiscreteVariable
+ (State& s, Stage g, AbstractValue* v) const
+{ return s.allocateDiscreteVariable(getRep().getMySubsystemIndex(), g, v); }
+DiscreteVariableIndex Subsystem::Guts::allocateAutoUpdateDiscreteVariable
+ (State& s, Stage invalidates, AbstractValue* v, Stage updateDependsOn) const
+{ return s.allocateAutoUpdateDiscreteVariable
+ (getRep().getMySubsystemIndex(),invalidates,v,updateDependsOn); }
+
+CacheEntryIndex Subsystem::Guts::allocateCacheEntry
+ (const State& s, Stage dependsOn, Stage computedBy, AbstractValue* v) const
+{ return s.allocateCacheEntry
+ (getRep().getMySubsystemIndex(), dependsOn, computedBy, v); }
+
+QErrIndex Subsystem::Guts::allocateQErr(const State& s, int nqerr) const {
+ return s.allocateQErr(getRep().getMySubsystemIndex(), nqerr);
+}
+
+UErrIndex Subsystem::Guts::allocateUErr(const State& s, int nuerr) const {
+ return s.allocateUErr(getRep().getMySubsystemIndex(), nuerr);
+}
+
+UDotErrIndex Subsystem::Guts::
+allocateUDotErr(const State& s, int nudoterr) const {
+ return s.allocateUDotErr(getRep().getMySubsystemIndex(), nudoterr);
+}
+
+EventTriggerByStageIndex Subsystem::Guts::
+allocateEventTriggersByStage(const State& s, Stage g, int ntriggers) const {
+ return s.allocateEventTrigger(getRep().getMySubsystemIndex(),g,ntriggers);
+}
+
+void Subsystem::Guts::advanceToStage(const State& s, Stage g) const {
+ s.advanceSubsystemToStage(getRep().getMySubsystemIndex(), g);
+}
+
+Stage Subsystem::Guts::getStage(const State& s) const {
+ return s.getSubsystemStage(getRep().getMySubsystemIndex());
+}
+const AbstractValue& Subsystem::Guts::
+getDiscreteVariable(const State& s, DiscreteVariableIndex index) const {
+ return s.getDiscreteVariable(getRep().getMySubsystemIndex(), index);
+}
+
+AbstractValue& Subsystem::Guts::
+updDiscreteVariable(State& s, DiscreteVariableIndex index) const {
+ return s.updDiscreteVariable(getRep().getMySubsystemIndex(), index);
+}
+
+const AbstractValue& Subsystem::Guts::
+getCacheEntry(const State& s, CacheEntryIndex index) const {
+ return s.getCacheEntry(getRep().getMySubsystemIndex(), index);
+}
+
+AbstractValue& Subsystem::Guts::
+updCacheEntry(const State& s, CacheEntryIndex index) const {
+ return s.updCacheEntry(getRep().getMySubsystemIndex(), index);
+}
+
+bool Subsystem::Guts::
+isCacheValueRealized(const State& s, CacheEntryIndex cx) const {
+ return s.isCacheValueRealized(getRep().getMySubsystemIndex(), cx);
+}
+void Subsystem::Guts::
+markCacheValueRealized(const State& s, CacheEntryIndex cx) const {
+ s.markCacheValueRealized(getRep().getMySubsystemIndex(), cx);
+}
+void Subsystem::Guts::
+markCacheValueNotRealized(const State& s, CacheEntryIndex cx) const {
+ s.markCacheValueNotRealized(getRep().getMySubsystemIndex(), cx);
+}
+
+const Vector& Subsystem::Guts::getQ(const State& s) const {return s.getQ(getRep().getMySubsystemIndex());}
+const Vector& Subsystem::Guts::getU(const State& s) const {return s.getU(getRep().getMySubsystemIndex());}
+const Vector& Subsystem::Guts::getZ(const State& s) const {return s.getZ(getRep().getMySubsystemIndex());}
+const Vector& Subsystem::Guts::getUWeights(const State& s) const {return s.getUWeights(getRep().getMySubsystemIndex());}
+const Vector& Subsystem::Guts::getZWeights(const State& s) const {return s.getZWeights(getRep().getMySubsystemIndex());}
+
+Vector& Subsystem::Guts::updQ(State& s) const {return s.updQ(getRep().getMySubsystemIndex());}
+Vector& Subsystem::Guts::updU(State& s) const {return s.updU(getRep().getMySubsystemIndex());}
+Vector& Subsystem::Guts::updZ(State& s) const {return s.updZ(getRep().getMySubsystemIndex());}
+
+const Vector& Subsystem::Guts::getQDot (const State& s) const {return s.getQDot(getRep().getMySubsystemIndex());}
+const Vector& Subsystem::Guts::getUDot (const State& s) const {return s.getUDot(getRep().getMySubsystemIndex());}
+const Vector& Subsystem::Guts::getZDot (const State& s) const {return s.getZDot(getRep().getMySubsystemIndex());}
+const Vector& Subsystem::Guts::getQDotDot(const State& s) const {return s.getQDotDot(getRep().getMySubsystemIndex());}
+
+Vector& Subsystem::Guts::updQDot (const State& s) const {return s.updQDot(getRep().getMySubsystemIndex());}
+Vector& Subsystem::Guts::updUDot (const State& s) const {return s.updUDot(getRep().getMySubsystemIndex());}
+Vector& Subsystem::Guts::updZDot (const State& s) const {return s.updZDot(getRep().getMySubsystemIndex());}
+Vector& Subsystem::Guts::updQDotDot(const State& s) const {return s.updQDotDot(getRep().getMySubsystemIndex());}
+
+const Vector& Subsystem::Guts::getQErr(const State& s) const {return s.getQErr(getRep().getMySubsystemIndex());}
+const Vector& Subsystem::Guts::getUErr(const State& s) const {return s.getUErr(getRep().getMySubsystemIndex());}
+const Vector& Subsystem::Guts::getQErrWeights(const State& s) const {return s.getQErrWeights(getRep().getMySubsystemIndex());}
+const Vector& Subsystem::Guts::getUErrWeights(const State& s) const {return s.getUErrWeights(getRep().getMySubsystemIndex());}
+
+const Vector& Subsystem::Guts::getUDotErr(const State& s) const {return s.getUDotErr(getRep().getMySubsystemIndex());}
+const Vector& Subsystem::Guts::getMultipliers(const State& s) const {return s.getMultipliers(getRep().getMySubsystemIndex());}
+const Vector& Subsystem::Guts::getEventTriggersByStage(const State& s, Stage g) const
+{ return s.getEventTriggersByStage(getRep().getMySubsystemIndex(),g); }
+
+Vector& Subsystem::Guts::updQErr(const State& s) const {return s.updQErr(getRep().getMySubsystemIndex());}
+Vector& Subsystem::Guts::updUErr(const State& s) const {return s.updUErr(getRep().getMySubsystemIndex());}
+Vector& Subsystem::Guts::updUDotErr(const State& s) const {return s.updUDotErr(getRep().getMySubsystemIndex());}
+Vector& Subsystem::Guts::updMultipliers(const State& s) const {return s.updMultipliers(getRep().getMySubsystemIndex());}
+Vector& Subsystem::Guts::updEventTriggersByStage(const State& s, Stage g) const
+{ return s.updEventTriggersByStage(getRep().getMySubsystemIndex(),g); }
+
+SystemQIndex Subsystem::Guts::getQStart(const State& s) const {return s.getQStart(getRep().getMySubsystemIndex());}
+int Subsystem::Guts::getNQ(const State& s) const {return s.getNQ(getRep().getMySubsystemIndex());}
+
+SystemUIndex Subsystem::Guts::getUStart(const State& s) const {return s.getUStart(getRep().getMySubsystemIndex());}
+int Subsystem::Guts::getNU(const State& s) const {return s.getNU(getRep().getMySubsystemIndex());}
+
+SystemZIndex Subsystem::Guts::getZStart(const State& s) const {return s.getZStart(getRep().getMySubsystemIndex());}
+int Subsystem::Guts::getNZ(const State& s) const {return s.getNZ(getRep().getMySubsystemIndex());}
+
+SystemQErrIndex Subsystem::Guts::getQErrStart(const State& s) const {return s.getQErrStart(getRep().getMySubsystemIndex());}
+int Subsystem::Guts::getNQErr(const State& s) const {return s.getNQErr(getRep().getMySubsystemIndex());}
+
+SystemUErrIndex Subsystem::Guts::getUErrStart(const State& s) const {return s.getUErrStart(getRep().getMySubsystemIndex());}
+int Subsystem::Guts::getNUErr(const State& s) const {return s.getNUErr(getRep().getMySubsystemIndex());}
+
+SystemUDotErrIndex Subsystem::Guts::getUDotErrStart(const State& s) const {return s.getUDotErrStart(getRep().getMySubsystemIndex());}
+int Subsystem::Guts::getNUDotErr(const State& s) const {return s.getNUDotErr(getRep().getMySubsystemIndex());}
+
+SystemMultiplierIndex Subsystem::Guts::getMultipliersStart(const State& s) const {return s.getMultipliersStart(getRep().getMySubsystemIndex());}
+int Subsystem::Guts::getNMultipliers(const State& s) const {return s.getNMultipliers(getRep().getMySubsystemIndex());}
+
+SystemEventTriggerByStageIndex Subsystem::Guts::getEventTriggerStartByStage(const State& s, Stage g) const {return s.getEventTriggerStartByStage(getRep().getMySubsystemIndex(),g);}
+int Subsystem::Guts::getNEventTriggersByStage (const State& s, Stage g) const {return s.getNEventTriggersByStage(getRep().getMySubsystemIndex(),g);}
+
+void Subsystem::Guts::invalidateSubsystemTopologyCache() const {
+ getRep().invalidateSubsystemTopologyCache();
+}
+
+bool Subsystem::Guts::subsystemTopologyHasBeenRealized() const {
+ return getRep().subsystemTopologyHasBeenRealized();
+}
+
+
+
+//------------------------------------------------------------------------------
+// CREATE SCHEDULED EVENT
+//------------------------------------------------------------------------------
+/*
+ * A Subsystem should invoke this method during Instance stage for each
+ * scheduled event it defines.
+ * It allocates a global event ID for the event, and registers that ID as
+ * belonging to this Subsystem.
+ *
+ * @param state the State which is being realized
+ * @param eventId on exit, the newly allocated event ID is stored here
+ */
+void Subsystem::Guts::
+createScheduledEvent(const State& state, EventId& eventId) const {
+ eventId = getSystem().getDefaultSubsystem()
+ .createEventId(getMySubsystemIndex(), state);
+}
+
+//------------------------------------------------------------------------------
+// CREATE TRIGGERED EVENT
+//------------------------------------------------------------------------------
+/*
+ * A Subsystem should invoke this method during Instance stage for each
+ * triggered event it defines. It allocates a global event ID for the event,
+ * registers that ID as belonging to this Subsystem, and allocates space in the
+ * State for the event trigger function.
+ *
+ * @param state the State which is being realized
+ * @param eventId on exit, the newly allocated event ID is stored here
+ * @param triggerFunctionIndex
+ * on exit, the index corresponding to the event's trigger function
+ * is stored here (this is a local, per-Subsystem, per-Stage index)
+ * @param stage the Stage at which the event will be evaluated
+ */
+void Subsystem::Guts::
+createTriggeredEvent(const State& state, EventId& eventId,
+ EventTriggerByStageIndex& triggerFunctionIndex,
+ Stage stage) const
+{
+ eventId = getSystem().getDefaultSubsystem()
+ .createEventId(getMySubsystemIndex(), state);
+ triggerFunctionIndex =
+ state.allocateEventTrigger(getMySubsystemIndex(), stage, 1);
+}
+
+
+ // wrappers for Subsystem::Guts virtuals
+
+//------------------------------------------------------------------------------
+// CLONE
+//------------------------------------------------------------------------------
+Subsystem::Guts* Subsystem::Guts::clone() const {
+ return cloneImpl();
+}
+
+//------------------------------------------------------------------------------
+// REALIZE SUBSYSTEM TOPOLOGY
+//------------------------------------------------------------------------------
+void Subsystem::Guts::realizeSubsystemTopology(State& s) const {
+ SimTK_STAGECHECK_EQ_ALWAYS(getStage(s), Stage::Empty,
+ "Subsystem::Guts::realizeSubsystemTopology()");
+ realizeSubsystemTopologyImpl(s);
+
+ // Realize this Subsystem's Measures.
+ for (MeasureIndex mx(0); mx < getRep().measures.size(); ++mx)
+ getRep().measures[mx]->realizeTopology(s);
+
+ getRep().subsystemTopologyRealized = true; // mark subsys itself (mutable)
+ advanceToStage(s, Stage::Topology); // mark the State as well
+}
+
+//------------------------------------------------------------------------------
+// REALIZE SUBSYSTEM MODEL
+//------------------------------------------------------------------------------
+void Subsystem::Guts::realizeSubsystemModel(State& s) const {
+ SimTK_STAGECHECK_TOPOLOGY_REALIZED_ALWAYS(subsystemTopologyHasBeenRealized(),
+ "Subsystem", getName(), "Subsystem::Guts::realizeSubsystemModel()");
+
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(s), Stage::Topology,
+ "Subsystem::Guts::realizeSubsystemModel()");
+ if (getStage(s) < Stage::Model) {
+ realizeSubsystemModelImpl(s);
+
+ // Realize this Subsystem's Measures.
+ for (MeasureIndex mx(0); mx < getRep().measures.size(); ++mx)
+ getRep().measures[mx]->realizeModel(s);
+
+ advanceToStage(s, Stage::Model);
+ }
+}
+
+//------------------------------------------------------------------------------
+// REALIZE SUBSYSTEM INSTANCE
+//------------------------------------------------------------------------------
+void Subsystem::Guts::realizeSubsystemInstance(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(s), Stage(Stage::Instance).prev(),
+ "Subsystem::Guts::realizeSubsystemInstance()");
+ if (getStage(s) < Stage::Instance) {
+ realizeSubsystemInstanceImpl(s);
+
+ // Realize this Subsystem's Measures.
+ for (MeasureIndex mx(0); mx < getRep().measures.size(); ++mx)
+ getRep().measures[mx]->realizeInstance(s);
+
+ advanceToStage(s, Stage::Instance);
+ }
+}
+
+//------------------------------------------------------------------------------
+// REALIZE SUBSYSTEM TIME
+//------------------------------------------------------------------------------
+void Subsystem::Guts::realizeSubsystemTime(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(s), Stage(Stage::Time).prev(),
+ "Subsystem::Guts::realizeTime()");
+ if (getStage(s) < Stage::Time) {
+ realizeSubsystemTimeImpl(s);
+
+ // Realize this Subsystem's Measures.
+ for (MeasureIndex mx(0); mx < getRep().measures.size(); ++mx)
+ getRep().measures[mx]->realizeTime(s);
+
+ advanceToStage(s, Stage::Time);
+ }
+}
+
+//------------------------------------------------------------------------------
+// REALIZE SUBSYSTEM POSITION
+//------------------------------------------------------------------------------
+void Subsystem::Guts::realizeSubsystemPosition(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(s), Stage(Stage::Position).prev(),
+ "Subsystem::Guts::realizeSubsystemPosition()");
+ if (getStage(s) < Stage::Position) {
+ realizeSubsystemPositionImpl(s);
+
+ // Realize this Subsystem's Measures.
+ for (MeasureIndex mx(0); mx < getRep().measures.size(); ++mx)
+ getRep().measures[mx]->realizePosition(s);
+
+ advanceToStage(s, Stage::Position);
+ }
+}
+
+//------------------------------------------------------------------------------
+// REALIZE SUBSYSTEM VELOCITY
+//------------------------------------------------------------------------------
+void Subsystem::Guts::realizeSubsystemVelocity(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(s), Stage(Stage::Velocity).prev(),
+ "Subsystem::Guts::realizeSubsystemVelocity()");
+ if (getStage(s) < Stage::Velocity) {
+ realizeSubsystemVelocityImpl(s);
+
+ // Realize this Subsystem's Measures.
+ for (MeasureIndex mx(0); mx < getRep().measures.size(); ++mx)
+ getRep().measures[mx]->realizeVelocity(s);
+
+ advanceToStage(s, Stage::Velocity);
+ }
+}
+
+//------------------------------------------------------------------------------
+// REALIZE SUBSYSTEM DYNAMICS
+//------------------------------------------------------------------------------
+void Subsystem::Guts::realizeSubsystemDynamics(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(s), Stage(Stage::Dynamics).prev(),
+ "Subsystem::Guts::realizeSubsystemDynamics()");
+ if (getStage(s) < Stage::Dynamics) {
+ realizeSubsystemDynamicsImpl(s);
+
+ // Realize this Subsystem's Measures.
+ for (MeasureIndex mx(0); mx < getRep().measures.size(); ++mx)
+ getRep().measures[mx]->realizeDynamics(s);
+
+ advanceToStage(s, Stage::Dynamics);
+ }
+}
+
+//------------------------------------------------------------------------------
+// REALIZE SUBSYSTEM ACCELERATION
+//------------------------------------------------------------------------------
+void Subsystem::Guts::realizeSubsystemAcceleration(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(s), Stage(Stage::Acceleration).prev(),
+ "Subsystem::Guts::realizeSubsystemAcceleration()");
+ if (getStage(s) < Stage::Acceleration) {
+ realizeSubsystemAccelerationImpl(s);
+
+ // Realize this Subsystem's Measures.
+ for (MeasureIndex mx(0); mx < getRep().measures.size(); ++mx)
+ getRep().measures[mx]->realizeAcceleration(s);
+
+ advanceToStage(s, Stage::Acceleration);
+ }
+}
+
+//------------------------------------------------------------------------------
+// REALIZE SUBSYSTEM REPORT
+//------------------------------------------------------------------------------
+void Subsystem::Guts::realizeSubsystemReport(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(s), Stage(Stage::Report).prev(),
+ "Subsystem::Guts::realizeSubsystemReport()");
+ if (getStage(s) < Stage::Report) {
+ realizeSubsystemReportImpl(s);
+
+ // Realize this Subsystem's Measures.
+ for (MeasureIndex mx(0); mx < getRep().measures.size(); ++mx)
+ getRep().measures[mx]->realizeReport(s);
+
+ advanceToStage(s, Stage::Report);
+ }
+}
+
+//------------------------------------------------------------------------------
+// CALC DECORATIVE GEOMETRY AND APPEND
+//------------------------------------------------------------------------------
+void Subsystem::Guts::calcDecorativeGeometryAndAppend
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+{
+ calcDecorativeGeometryAndAppendImpl(s,stage,geom);
+}
+
+//------------------------------------------------------------------------------
+// HANDLE EVENTS
+//------------------------------------------------------------------------------
+void Subsystem::Guts::handleEvents(State& state, Event::Cause cause,
+ const Array_<EventId>& eventIds,
+ const HandleEventsOptions& options, HandleEventsResults& results) const
+{
+ // Invoke Measure handlers where appropriate (TODO: just initialization
+ // so far). Initialize measures first in case the Subsystem initialization
+ // handler references measures.
+ if (cause == Event::Cause::Initialization) {
+ for (MeasureIndex mx(0); mx < getRep().measures.size(); ++mx)
+ getRep().measures[mx]->initialize(state);
+ }
+
+ // assume success
+ results.clear(); results.setExitStatus(HandleEventsResults::Succeeded);
+ handleEventsImpl(state, cause, eventIds, options, results);
+}
+
+//------------------------------------------------------------------------------
+// REPORT EVENTS
+//------------------------------------------------------------------------------
+void Subsystem::Guts::reportEvents(const State& state, Event::Cause cause,
+ const Array_<EventId>& eventIds) const
+{
+ reportEventsImpl(state, cause, eventIds);
+}
+
+//------------------------------------------------------------------------------
+// CALC EVENT TRIGGER INFO
+//------------------------------------------------------------------------------
+void Subsystem::Guts::
+calcEventTriggerInfo(const State& state, Array_<EventTriggerInfo>& info) const {
+ calcEventTriggerInfoImpl(state, info);
+}
+
+//------------------------------------------------------------------------------
+// CALC TIME OF NEXT SCHEDULED EVENT
+//------------------------------------------------------------------------------
+void Subsystem::Guts::
+calcTimeOfNextScheduledEvent(const State& state, Real& tNextEvent,
+ Array_<EventId>& eventIds,
+ bool includeCurrentTime) const
+{
+ tNextEvent = Infinity;
+ eventIds.clear();
+ calcTimeOfNextScheduledEventImpl(state, tNextEvent, eventIds,
+ includeCurrentTime);
+}
+
+//------------------------------------------------------------------------------
+// CALC TIME OF NEXT SCHEDULED REPORT
+//------------------------------------------------------------------------------
+void Subsystem::Guts::
+calcTimeOfNextScheduledReport(const State& state, Real& tNextReport,
+ Array_<EventId>& eventIds,
+ bool includeCurrentTime) const
+{
+ tNextReport = Infinity;
+ eventIds.clear();
+ calcTimeOfNextScheduledReportImpl(state, tNextReport, eventIds,
+ includeCurrentTime);
+}
+
+//==============================================================================
+// SUBSYSTEM :: GUTS :: GUTS REP
+//==============================================================================
+
+// Invalidating a Subsystem's topology cache forces invalidation of the
+// whole System's topology cache, which will in turn invalidate all the other
+// Subsystem's topology caches.
+void Subsystem::Guts::GutsRep::invalidateSubsystemTopologyCache() const {
+ if (subsystemTopologyRealized) {
+ subsystemTopologyRealized = false;
+ if (isInSystem())
+ getSystem().getSystemGuts().invalidateSystemTopologyCache();
+ }
+}
+
+
+} // namespace SimTK
+
diff --git a/SimTKcommon/Simulation/src/SubsystemGutsRep.h b/SimTKcommon/Simulation/src/SubsystemGutsRep.h
new file mode 100644
index 0000000..fd6f2e6
--- /dev/null
+++ b/SimTKcommon/Simulation/src/SubsystemGutsRep.h
@@ -0,0 +1,153 @@
+#ifndef SimTK_SimTKCOMMON_SUBSYSTEM_GUTSREP_H_
+#define SimTK_SimTKCOMMON_SUBSYSTEM_GUTSREP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+// This header is internal source code and is not part of the SimTKcommon
+// API or distribution. This is the private, opaque implementation of
+// the Subsystem::Guts class, which contains just a pointer to the
+// object declared here.
+
+#include "SimTKcommon/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/State.h"
+#include "SimTKcommon/internal/System.h"
+#include "SimTKcommon/internal/Subsystem.h"
+#include "SimTKcommon/internal/SubsystemGuts.h"
+#include "SimTKcommon/internal/Measure.h"
+#include "SimTKcommon/internal/MeasureImplementation.h"
+
+#include <algorithm>
+
+namespace SimTK {
+
+class Subsystem::Guts::GutsRep {
+ friend class Subsystem::Guts;
+public:
+ GutsRep(const String& name, const String& version)
+ : subsystemName(name), subsystemVersion(version),
+ mySystem(0), mySubsystemIndex(InvalidSubsystemIndex), myHandle(0),
+ subsystemTopologyRealized(false)
+ {
+ }
+
+ GutsRep(const GutsRep& src)
+ : subsystemName(src.subsystemName),
+ subsystemVersion(src.subsystemVersion),
+ mySystem(0),
+ mySubsystemIndex(InvalidSubsystemIndex),
+ myHandle(0),
+ subsystemTopologyRealized(false)
+ {
+ }
+
+ ~GutsRep() {
+ for (MeasureIndex mx(0); mx < measures.size(); ++mx)
+ if (measures[mx]->decrRefCount()==0) delete measures[mx];
+ clearMyHandle();
+ invalidateSubsystemTopologyCache();
+ }
+
+
+ const String& getName() const {return subsystemName;}
+ const String& getVersion() const {return subsystemVersion;}
+
+ void invalidateSubsystemTopologyCache() const;
+
+ bool subsystemTopologyHasBeenRealized() const {
+ return subsystemTopologyRealized;
+ }
+
+ bool isInSystem() const {return mySystem != 0;}
+ bool isInSameSystem(const Subsystem& otherSubsystem) const {
+ return isInSystem() && otherSubsystem.isInSystem()
+ && getSystem().isSameSystem(otherSubsystem.getSystem());
+ }
+
+ const System& getSystem() const {
+ SimTK_ASSERT(isInSystem(), "Subsystem::getSystem()");
+ return *mySystem;
+ }
+ System& updSystem() {
+ SimTK_ASSERT(isInSystem(), "Subsystem::updSystem()");
+ return *mySystem;
+ }
+ void setSystem(System& sys, SubsystemIndex id) {
+ SimTK_ASSERT(!isInSystem(), "Subsystem::setSystem()");
+ SimTK_ASSERT(id.isValid(), "Subsystem::setSystem()");
+ mySystem = &sys;
+ mySubsystemIndex = id;
+ }
+ SubsystemIndex getMySubsystemIndex() const {
+ SimTK_ASSERT(isInSystem(), "Subsystem::getMySubsystemIndex()");
+ return mySubsystemIndex;
+ }
+
+ void setMyHandle(Subsystem& h) {myHandle = &h;}
+ const Subsystem& getMyHandle() const {assert(myHandle); return *myHandle;}
+ Subsystem& updMyHandle() {assert(myHandle); return *myHandle;}
+ void clearMyHandle() {myHandle=0;}
+
+ AbstractMeasure getMeasure(MeasureIndex mx) const {
+ assert(0 <= mx && mx < measures.size());
+ return AbstractMeasure(measures[mx]);
+ }
+
+ MeasureIndex adoptMeasure(AbstractMeasure& m) {
+ assert(m.hasImpl());
+ // This is an expensive check if there are lots of measures.
+ assert(std::find(measures.begin(), measures.end(), &m.getImpl())
+ == measures.end());
+
+ invalidateSubsystemTopologyCache();
+ const MeasureIndex mx(measures.size());
+ measures.push_back(&m.updImpl());
+ measures.back()->incrRefCount();
+ measures.back()->setSubsystem(updMyHandle(), mx);
+ return mx;
+ }
+
+private:
+ String subsystemName;
+ String subsystemVersion;
+ System* mySystem; // the System to which this Subsystem belongs
+ SubsystemIndex mySubsystemIndex; // Subsystem # within System
+
+ friend class Subsystem;
+ Subsystem* myHandle; // the owner handle of this rep
+
+ Array_<AbstractMeasure::Implementation*> measures;
+
+ // TOPOLOGY CACHE
+
+ mutable bool subsystemTopologyRealized;
+
+private:
+ // suppress automatic copy assignment operator
+ GutsRep& operator=(const GutsRep&);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_SUBSYSTEM_GUTSREP_H_
diff --git a/SimTKcommon/Simulation/src/System.cpp b/SimTKcommon/Simulation/src/System.cpp
new file mode 100644
index 0000000..5cf2590
--- /dev/null
+++ b/SimTKcommon/Simulation/src/System.cpp
@@ -0,0 +1,1502 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+
+/**@file
+ *
+ * Implementation of System, System::Guts, and System::GutsRep, and also
+ * EventTriggerInfo and EventTriggerInfoRep.
+ */
+
+#include "SimTKcommon/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/Subsystem.h"
+#include "SimTKcommon/internal/SubsystemGuts.h"
+#include "SimTKcommon/internal/System.h"
+#include "SimTKcommon/internal/SystemGuts.h"
+#include "SimTKcommon/internal/EventHandler.h"
+#include "SimTKcommon/internal/EventReporter.h"
+
+#include "SystemGutsRep.h"
+
+#include <cassert>
+#include <map>
+#include <set>
+
+namespace SimTK {
+
+//==============================================================================
+// SYSTEM
+//==============================================================================
+// Most of the System:: methods just forward to System::Guts.
+
+bool System::isEmptyHandle() const {return guts==0;}
+bool System::isOwnerHandle() const {return guts==0 || &guts->getSystem()==this;}
+bool System::isSameSystem(const System& otherSystem) const {
+ return guts && (guts==otherSystem.guts);
+}
+
+System::System(const System& src) : guts(0) {
+ if (src.guts) {
+ guts = src.guts->clone();
+ guts->setOwnerHandle(*this);
+ }
+}
+
+System& System::operator=(const System& src) {
+ if (!isSameSystem(src)) {
+ if (isOwnerHandle())
+ delete guts;
+ guts=0;
+ if (src.guts) {
+ guts = src.guts->clone();
+ guts->setOwnerHandle(*this);
+ }
+ }
+ return *this;
+}
+
+System::~System() {
+ if (guts && isOwnerHandle())
+ delete guts;
+ guts=0;
+}
+
+void System::adoptSystemGuts(System::Guts* g) {
+ SimTK_ASSERT_ALWAYS(g, "System::adoptSystemGuts(): can't adopt null Guts");
+ SimTK_ASSERT_ALWAYS(!guts,
+ "System::adoptSystemGuts(): this System handle is already in use");
+ guts = g;
+ guts->setOwnerHandle(*this);
+}
+
+const String& System::getName() const {return getSystemGuts().getName();}
+const String& System::getVersion() const {return getSystemGuts().getVersion();}
+
+System& System::setDefaultTimeScale(Real tc)
+{ updSystemGuts().updRep().setDefaultTimeScale(tc); return *this; }
+Real System::getDefaultTimeScale() const
+{ return getSystemGuts().getRep().getDefaultTimeScale(); }
+System& System::setDefaultLengthScale(Real lc)
+{ updSystemGuts().updRep().setDefaultLengthScale(lc); return *this; }
+Real System::getDefaultLengthScale() const
+{ return getSystemGuts().getRep().getDefaultLengthScale(); }
+
+System& System::setUpDirection(const CoordinateDirection& up)
+{ updSystemGuts().updRep().setUpDirection(up); return *this; }
+CoordinateDirection System::getUpDirection() const
+{ return getSystemGuts().getRep().getUpDirection(); }
+System& System::setUseUniformBackground(bool useUniformBackground)
+{ updSystemGuts().updRep().setUseUniformBackground(useUniformBackground);
+ return *this; }
+bool System::getUseUniformBackground() const
+{ return getSystemGuts().getRep().getUseUniformBackground(); }
+
+void System::resetAllCountersToZero() {updSystemGuts().updRep().resetAllCounters();}
+int System::getNumRealizationsOfThisStage(Stage g) const {return getSystemGuts().getRep().nRealizationsOfStage[g];}
+int System::getNumRealizeCalls() const {return getSystemGuts().getRep().nRealizeCalls;}
+
+int System::getNumPrescribeQCalls() const {return getSystemGuts().getRep().nPrescribeQCalls;}
+int System::getNumPrescribeUCalls() const {return getSystemGuts().getRep().nPrescribeUCalls;}
+
+int System::getNumProjectQCalls() const {return getSystemGuts().getRep().nProjectQCalls;}
+int System::getNumProjectUCalls() const {return getSystemGuts().getRep().nProjectUCalls;}
+int System::getNumFailedProjectQCalls() const {return getSystemGuts().getRep().nFailedProjectQCalls;}
+int System::getNumFailedProjectUCalls() const {return getSystemGuts().getRep().nFailedProjectUCalls;}
+int System::getNumQProjections() const {return getSystemGuts().getRep().nQProjections;}
+int System::getNumUProjections() const {return getSystemGuts().getRep().nUProjections;}
+int System::getNumQErrorEstimateProjections() const {return getSystemGuts().getRep().nQErrEstProjections;}
+int System::getNumUErrorEstimateProjections() const {return getSystemGuts().getRep().nUErrEstProjections;}
+
+int System::getNumHandlerCallsThatChangedStage(Stage g) const {return getSystemGuts().getRep().nHandlerCallsThatChangedStage[g];}
+int System::getNumHandleEventCalls() const {return getSystemGuts().getRep().nHandleEventsCalls;}
+int System::getNumReportEventCalls() const {return getSystemGuts().getRep().nReportEventsCalls;}
+
+const State& System::getDefaultState() const {return getSystemGuts().getDefaultState();}
+State& System::updDefaultState() {return updSystemGuts().updDefaultState();}
+
+SubsystemIndex System::adoptSubsystem(Subsystem& child) {return updSystemGuts().adoptSubsystem(child);}
+int System::getNumSubsystems() const {return getSystemGuts().getNumSubsystems();}
+const Subsystem& System::getSubsystem(SubsystemIndex i) const {return getSystemGuts().getSubsystem(i);}
+Subsystem& System::updSubsystem(SubsystemIndex i) {return updSystemGuts().updSubsystem(i);}
+const DefaultSystemSubsystem& System::getDefaultSubsystem() const {
+ return static_cast<const DefaultSystemSubsystem&>(getSystemGuts().getSubsystem(SubsystemIndex(0)));
+}
+DefaultSystemSubsystem& System::updDefaultSubsystem() {
+ return static_cast<DefaultSystemSubsystem&>(updSystemGuts().updSubsystem(SubsystemIndex(0)));
+}
+
+// TODO: this should be a Model stage variable allocated by the base class.
+// Currently it is just a Topology stage variable stored in the base class.
+void System::setHasTimeAdvancedEvents(bool hasEm) {
+ updSystemGuts().setHasTimeAdvancedEvents(hasEm);
+ getSystemGuts().invalidateSystemTopologyCache();
+}
+bool System::hasTimeAdvancedEvents() const {
+ return getSystemGuts().hasTimeAdvancedEvents();
+}
+bool System::systemTopologyHasBeenRealized() const
+{ return getSystemGuts().systemTopologyHasBeenRealized(); }
+StageVersion System::getSystemTopologyCacheVersion() const
+{ return getSystemGuts().getSystemTopologyCacheVersion(); }
+void System::setSystemTopologyCacheVersion(StageVersion topoVersion) const
+{ getSystemGuts().setSystemTopologyCacheVersion(topoVersion); }
+void System::invalidateSystemTopologyCache() const
+{ getSystemGuts().invalidateSystemTopologyCache(); }
+
+
+
+const State& System::realizeTopology() const {return getSystemGuts().realizeTopology();}
+void System::realizeModel(State& s) const {getSystemGuts().realizeModel(s);}
+void System::realize(const State& s, Stage g) const {getSystemGuts().realize(s,g);}
+void System::calcDecorativeGeometryAndAppend
+ (const State& s, Stage g, Array_<DecorativeGeometry>& geom) const
+{ getSystemGuts().calcDecorativeGeometryAndAppend(s,g,geom); }
+
+void System::multiplyByN(const State& s, const Vector& u, Vector& dq) const
+{ getSystemGuts().multiplyByN(s,u,dq); }
+void System::multiplyByNTranspose(const State& s, const Vector& fq, Vector& fu) const
+{ getSystemGuts().multiplyByNTranspose(s,fq,fu); }
+void System::multiplyByNPInv(const State& s, const Vector& dq, Vector& u) const
+{ getSystemGuts().multiplyByNPInv(s,dq,u); }
+void System::multiplyByNPInvTranspose(const State& s, const Vector& fu, Vector& fq) const
+{ getSystemGuts().multiplyByNPInvTranspose(s,fu,fq); }
+
+bool System::prescribeQ(State& s) const
+{ return getSystemGuts().prescribeQ(s); }
+bool System::prescribeU(State& s) const
+{ return getSystemGuts().prescribeU(s); }
+void System::getFreeQIndex(const State& s, Array_<SystemQIndex>& freeQs) const
+{ return getSystemGuts().getFreeQIndex(s,freeQs); }
+void System::getFreeUIndex(const State& s, Array_<SystemUIndex>& freeUs) const
+{ return getSystemGuts().getFreeUIndex(s,freeUs); }
+
+void System::project(State& state, Real accuracy) const {
+ const ProjectOptions projOptions(accuracy);
+ ProjectResults projResults;
+ Vector noErrEst; // empty
+
+ realize(state, Stage::Time);
+ prescribeQ(state);
+ realize(state, Stage::Position);
+ projectQ(state, noErrEst, projOptions, projResults);
+ prescribeU(state);
+ realize(state, Stage::Velocity);
+ projectU(state, noErrEst, projOptions, projResults);
+}
+
+void System::projectQ(State& state, Real accuracy) const {
+ const ProjectOptions projOptions(accuracy);
+ ProjectResults projResults;
+ Vector noErrEst; // empty
+
+ realize(state, Stage::Time);
+ prescribeQ(state);
+ realize(state, Stage::Position);
+ projectQ(state, noErrEst, projOptions, projResults);
+}
+
+void System::projectU(State& state, Real accuracy) const {
+ const ProjectOptions projOptions(accuracy);
+ ProjectResults projResults;
+ Vector noErrEst; // empty
+
+ realize(state, Stage::Position);
+ prescribeU(state);
+ realize(state, Stage::Velocity);
+ projectU(state, noErrEst, projOptions, projResults);
+}
+
+void System::projectQ(State& s, Vector& qErrEst,
+ const ProjectOptions& options, ProjectResults& results) const
+{ getSystemGuts().projectQ(s,qErrEst,options,results); }
+void System::projectU(State& s, Vector& uErrEst,
+ const ProjectOptions& options, ProjectResults& results) const
+{ getSystemGuts().projectU(s,uErrEst,options,results); }
+
+void System::handleEvents
+ (State& s, Event::Cause cause, const Array_<EventId>& eventIds,
+ const HandleEventsOptions& options, HandleEventsResults& results) const
+{ getSystemGuts().handleEvents(s,cause,eventIds,options,results); }
+void System::reportEvents(const State& s, Event::Cause cause, const Array_<EventId>& eventIds) const
+{ getSystemGuts().reportEvents(s,cause,eventIds); }
+void System::calcEventTriggerInfo(const State& s, Array_<EventTriggerInfo>& info) const
+{ getSystemGuts().calcEventTriggerInfo(s,info); }
+void System::calcTimeOfNextScheduledEvent(const State& s, Real& tNextEvent,
+ Array_<EventId>& eventIds, bool includeCurrentTime) const
+{ getSystemGuts().calcTimeOfNextScheduledEvent(s,tNextEvent,eventIds,includeCurrentTime); }
+void System::calcTimeOfNextScheduledReport(const State& s, Real& tNextEvent,
+ Array_<EventId>& eventIds, bool includeCurrentTime) const
+{ getSystemGuts().calcTimeOfNextScheduledReport(s,tNextEvent,eventIds,includeCurrentTime); }
+
+
+
+
+//==============================================================================
+// SYSTEM :: GUTS
+//==============================================================================
+
+// This is also the default constructor.
+System::Guts::Guts(const String& name, const String& version) {
+ rep = new GutsRep(name,version);
+ // note that the GutsRep object currently has no owner handle
+}
+
+System::Guts::~Guts() {
+ delete rep;
+ rep=0;
+}
+
+// Copy constructor
+System::Guts::Guts(const Guts& src) : rep(0) {
+ if (src.rep) {
+ rep = new GutsRep(*src.rep);
+ // note that the GutsRep object currently has no owner handle
+ }
+}
+
+// Copy assignment is suppressed
+
+
+const System& System::Guts::getSystem() const {
+ assert(rep->myHandle);
+ return *rep->myHandle;
+}
+
+System& System::Guts::updSystem() {
+ assert(rep->myHandle);
+ return *rep->myHandle;
+}
+
+void System::Guts::setOwnerHandle(System& sys) {
+ assert(!rep->myHandle);
+ rep->myHandle = &sys;
+}
+
+bool System::Guts::hasOwnerHandle() const {
+ return rep->myHandle != 0;
+}
+
+void System::Guts::setHasTimeAdvancedEvents(bool hasEm) {
+ updRep().hasTimeAdvancedEventsFlag = hasEm;
+}
+bool System::Guts::hasTimeAdvancedEvents() const {
+ return getRep().hasTimeAdvancedEventsFlag;
+}
+
+const String& System::Guts::getName() const {return getRep().getName();}
+const String& System::Guts::getVersion() const {return getRep().getVersion();}
+
+bool System::Guts::systemTopologyHasBeenRealized() const
+{ return getRep().systemTopologyHasBeenRealized(); }
+StageVersion System::Guts::getSystemTopologyCacheVersion() const
+{ return getRep().getSystemTopologyCacheVersion(); }
+void System::Guts::setSystemTopologyCacheVersion(StageVersion topoVersion) const
+{ getRep().setSystemTopologyCacheVersion(topoVersion); }
+void System::Guts::invalidateSystemTopologyCache() const
+{ return getRep().invalidateSystemTopologyCache(); } // mutable
+
+const State& System::Guts::getDefaultState() const {
+ SimTK_STAGECHECK_TOPOLOGY_REALIZED_ALWAYS(systemTopologyHasBeenRealized(),
+ "System", getName(), "System::Guts::getDefaultState()");
+
+ return getRep().getDefaultState();
+}
+
+State& System::Guts::updDefaultState() {
+ SimTK_STAGECHECK_TOPOLOGY_REALIZED_ALWAYS(systemTopologyHasBeenRealized(),
+ "System", getName(), "System::Guts::updDefaultState()");
+
+ return updRep().updDefaultState();
+}
+
+int System::Guts::getNumSubsystems() const {return getRep().getNumSubsystems();}
+const Subsystem& System::Guts::getSubsystem(SubsystemIndex i) const {return getRep().getSubsystem(i);}
+Subsystem& System::Guts::updSubsystem(SubsystemIndex i) {return updRep().updSubsystem(i);}
+
+System::Guts* System::Guts::clone() const {
+ return cloneImpl();
+}
+
+
+
+//------------------------------------------------------------------------------
+// REALIZE TOPOLOGY
+//------------------------------------------------------------------------------
+const State& System::Guts::realizeTopology() const {
+ State& defaultState = getRep().defaultState; // mutable
+ if (getRep().systemTopologyHasBeenRealized())
+ return defaultState;
+
+ defaultState.clear();
+ defaultState.setNumSubsystems(getNumSubsystems());
+ for (SubsystemIndex i(0); i<getNumSubsystems(); ++i)
+ defaultState.initializeSubsystem(i, getRep().subsystems[i].getName(),
+ getRep().subsystems[i].getVersion());
+
+ // Allow the concrete System subclass to do its processing.
+ realizeTopologyImpl(defaultState); // defaultState is mutable
+
+ // Realize any subsystems that the subclass didn't already take care of.
+ for (SubsystemIndex i(0); i<getNumSubsystems(); ++i)
+ if (getRep().subsystems[i].getStage(defaultState) < Stage::Topology)
+ getRep().subsystems[i].getSubsystemGuts()
+ .realizeSubsystemTopology(defaultState);
+
+ // Force the defaultState's Topology stage version number to match the
+ // Topology cache version in this System.
+ defaultState.setSystemTopologyStageVersion
+ (getRep().getSystemTopologyCacheVersion());
+ defaultState.advanceSystemToStage(Stage::Topology);
+
+ getRep().systemTopologyRealized = true; // mutable
+ getRep().nRealizationsOfStage[Stage::Topology]++; // mutable counter
+
+ // Realize the model using the default settings of the Model variables.
+ // This allocates all the later-stage State variables.
+ realizeModel(defaultState);
+
+ return defaultState;
+}
+
+
+
+//------------------------------------------------------------------------------
+// REALIZE MODEL
+//------------------------------------------------------------------------------
+void System::Guts::realizeModel(State& s) const {
+ SimTK_STAGECHECK_TOPOLOGY_REALIZED_ALWAYS(systemTopologyHasBeenRealized(),
+ "System", getName(), "System::Guts::realizeModel()");
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage::Topology,
+ "System::Guts::realizeModel()");
+ SimTK_STAGECHECK_TOPOLOGY_VERSION_ALWAYS(
+ getSystemTopologyCacheVersion(), s.getSystemTopologyStageVersion(),
+ "System", getName(), "System::Guts::realizeModel()");
+ if (s.getSystemStage() < Stage::Model) {
+ // Allow the subclass to do its processing.
+ realizeModelImpl(s);
+ // Realize any subsystems that the subclass didn't already take care of.
+ for (SubsystemIndex i(0); i<getNumSubsystems(); ++i)
+ if (getRep().subsystems[i].getStage(s) < Stage::Model)
+ getRep().subsystems[i].getSubsystemGuts()
+ .realizeSubsystemModel(s);
+ s.advanceSystemToStage(Stage::Model);
+
+ getRep().nRealizationsOfStage[Stage::Model]++; // mutable counter
+ }
+}
+
+
+
+//------------------------------------------------------------------------------
+// REALIZE INSTANCE
+//------------------------------------------------------------------------------
+void System::Guts::realizeInstance(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage(Stage::Instance).prev(),
+ "System::Guts::realizeInstance()");
+ if (s.getSystemStage() < Stage::Instance) {
+ realizeInstanceImpl(s); // take care of the Subsystems
+ // Realize any subsystems that the subclass didn't already take care of.
+ for (SubsystemIndex i(0); i<getNumSubsystems(); ++i)
+ if (getRep().subsystems[i].getStage(s) < Stage::Instance)
+ getRep().subsystems[i].getSubsystemGuts()
+ .realizeSubsystemInstance(s);
+ s.advanceSystemToStage(Stage::Instance);
+
+ getRep().nRealizationsOfStage[Stage::Instance]++; // mutable counter
+ }
+}
+
+
+
+//------------------------------------------------------------------------------
+// REALIZE TIME
+//------------------------------------------------------------------------------
+void System::Guts::realizeTime(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage(Stage::Time).prev(),
+ "System::Guts::realizeTime()");
+ if (s.getSystemStage() < Stage::Time) {
+ // Allow the subclass to do processing.
+ realizeTimeImpl(s);
+ // Realize any subsystems that the subclass didn't already take care of.
+ for (SubsystemIndex i(0); i<getNumSubsystems(); ++i)
+ if (getRep().subsystems[i].getStage(s) < Stage::Time)
+ getRep().subsystems[i].getSubsystemGuts()
+ .realizeSubsystemTime(s);
+ s.advanceSystemToStage(Stage::Time);
+
+ getRep().nRealizationsOfStage[Stage::Time]++; // mutable counter
+ }
+}
+
+
+
+//------------------------------------------------------------------------------
+// REALIZE POSITION
+//------------------------------------------------------------------------------
+void System::Guts::realizePosition(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage(Stage::Position).prev(),
+ "System::Guts::realizePosition()");
+ if (s.getSystemStage() < Stage::Position) {
+ // Allow the subclass to do processing.
+ realizePositionImpl(s);
+ // Realize any subsystems that the subclass didn't already take care of.
+ for (SubsystemIndex i(0); i<getNumSubsystems(); ++i)
+ if (getRep().subsystems[i].getStage(s) < Stage::Position)
+ getRep().subsystems[i].getSubsystemGuts()
+ .realizeSubsystemPosition(s);
+ s.advanceSystemToStage(Stage::Position);
+
+ getRep().nRealizationsOfStage[Stage::Position]++; // mutable counter
+ }
+}
+
+
+
+//------------------------------------------------------------------------------
+// REALIZE VELOCITY
+//------------------------------------------------------------------------------
+void System::Guts::realizeVelocity(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage(Stage::Velocity).prev(),
+ "System::Guts::realizeVelocity()");
+ if (s.getSystemStage() < Stage::Velocity) {
+ // Allow the subclass to do processing.
+ realizeVelocityImpl(s);
+ // Realize any subsystems that the subclass didn't already take care of.
+ for (SubsystemIndex i(0); i<getNumSubsystems(); ++i)
+ if (getRep().subsystems[i].getStage(s) < Stage::Velocity)
+ getRep().subsystems[i].getSubsystemGuts()
+ .realizeSubsystemVelocity(s);
+ s.advanceSystemToStage(Stage::Velocity);
+
+ getRep().nRealizationsOfStage[Stage::Velocity]++; // mutable counter
+ }
+}
+
+
+
+//------------------------------------------------------------------------------
+// REALIZE DYNAMICS
+//------------------------------------------------------------------------------
+void System::Guts::realizeDynamics(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage(Stage::Dynamics).prev(),
+ "System::Guts::realizeDynamics()");
+ if (s.getSystemStage() < Stage::Dynamics) {
+ // Allow the subclass to do processing.
+ realizeDynamicsImpl(s);
+ // Realize any subsystems that the subclass didn't already take care of.
+ for (SubsystemIndex i(0); i<getNumSubsystems(); ++i)
+ if (getRep().subsystems[i].getStage(s) < Stage::Dynamics)
+ getRep().subsystems[i].getSubsystemGuts()
+ .realizeSubsystemDynamics(s);
+ s.advanceSystemToStage(Stage::Dynamics);
+
+ getRep().nRealizationsOfStage[Stage::Dynamics]++; // mutable counter
+ }
+}
+
+
+
+//------------------------------------------------------------------------------
+// REALIZE ACCELERATION
+//------------------------------------------------------------------------------
+void System::Guts::realizeAcceleration(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage(Stage::Acceleration).prev(),
+ "System::Guts::realizeAcceleration()");
+ if (s.getSystemStage() < Stage::Acceleration) {
+ // Allow the subclass to do processing.
+ realizeAccelerationImpl(s);
+ // Realize any subsystems that the subclass didn't already take care of.
+ for (SubsystemIndex i(0); i<getNumSubsystems(); ++i)
+ if (getRep().subsystems[i].getStage(s) < Stage::Acceleration)
+ getRep().subsystems[i].getSubsystemGuts()
+ .realizeSubsystemAcceleration(s);
+ s.advanceSystemToStage(Stage::Acceleration);
+
+ getRep().nRealizationsOfStage[Stage::Acceleration]++; // mutable counter
+ }
+}
+
+
+
+//------------------------------------------------------------------------------
+// REALIZE REPORT
+//------------------------------------------------------------------------------
+void System::Guts::realizeReport(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage(Stage::Report).prev(),
+ "System::Guts::realizeReport()");
+ if (s.getSystemStage() < Stage::Report) {
+ // Allow the subclass to do processing.
+ realizeReportImpl(s);
+ // Realize any subsystems that the subclass didn't already take care of.
+ for (SubsystemIndex i(0); i<getNumSubsystems(); ++i)
+ if (getRep().subsystems[i].getStage(s) < Stage::Report)
+ getRep().subsystems[i].getSubsystemGuts().realizeSubsystemReport(s);
+ s.advanceSystemToStage(Stage::Report);
+
+ getRep().nRealizationsOfStage[Stage::Report]++; // mutable counter
+ }
+}
+
+
+
+//------------------------------------------------------------------------------
+// MULTIPLY BY N, ~N, N+, ~(N+)
+//------------------------------------------------------------------------------
+
+void System::Guts::multiplyByN(const State& s, const Vector& u,
+ Vector& dq) const {
+ SimTK_STAGECHECK_GE(s.getSystemStage(), Stage::Position,
+ "System::Guts::multiplyByN()");
+ return multiplyByNImpl(s,u,dq);
+}
+void System::Guts::multiplyByNTranspose(const State& s, const Vector& fq,
+ Vector& fu) const {
+ SimTK_STAGECHECK_GE(s.getSystemStage(), Stage::Position,
+ "System::Guts::multiplyByNTranspose()");
+ return multiplyByNTransposeImpl(s,fq,fu);
+}
+void System::Guts::multiplyByNPInv(const State& s, const Vector& dq,
+ Vector& u) const {
+ SimTK_STAGECHECK_GE(s.getSystemStage(), Stage::Position,
+ "System::Guts::multiplyByNPInv()");
+ return multiplyByNPInvImpl(s,dq,u);
+}
+void System::Guts::multiplyByNPInvTranspose(const State& s, const Vector& fu,
+ Vector& fq) const {
+ SimTK_STAGECHECK_GE(s.getSystemStage(), Stage::Position,
+ "System::Guts::multiplyByNPInvTranspose()");
+ return multiplyByNPInvTransposeImpl(s,fu,fq);
+}
+
+
+
+//------------------------------------------------------------------------------
+// PRESCRIBE Q
+//------------------------------------------------------------------------------
+bool System::Guts::prescribeQ(State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage::Time,
+ "System::Guts::prescribeQ()");
+ getRep().nPrescribeQCalls++; // mutable counter
+ return prescribeQImpl(s);
+}
+
+
+//------------------------------------------------------------------------------
+// PRESCRIBE U
+//------------------------------------------------------------------------------
+bool System::Guts::prescribeU(State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage::Position,
+ "System::Guts::prescribeU()");
+ getRep().nPrescribeUCalls++; // mutable counter
+ return prescribeUImpl(s);
+}
+
+
+//------------------------------------------------------------------------------
+// GET FREE Q(U) INDEX
+//------------------------------------------------------------------------------
+void System::Guts::
+getFreeQIndex(const State& s, Array_<SystemQIndex>& freeQs) const {
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage::Instance,
+ "System::Guts::getFreeQIndex()");
+ return getFreeQIndexImpl(s, freeQs);
+}
+void System::Guts::
+getFreeUIndex(const State& s, Array_<SystemUIndex>& freeUs) const {
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage::Instance,
+ "System::Guts::getFreeUIndex()");
+ return getFreeUIndexImpl(s, freeUs);
+}
+
+
+
+//------------------------------------------------------------------------------
+// PROJECT Q
+//------------------------------------------------------------------------------
+void System::Guts::projectQ(State& s, Vector& qErrEst,
+ const ProjectOptions& options,
+ ProjectResults& results) const {
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage::Position,
+ "System::Guts::projectQ()");
+ const System::Guts::GutsRep& rep = getRep();
+
+ rep.nProjectQCalls++; // counters are mutable
+ rep.nFailedProjectQCalls++; // assume this will throw an exception
+ //---------------------------------------------------------
+ projectQImpl(s,qErrEst,options,results);
+ //---------------------------------------------------------
+ if (results.getExitStatus()==ProjectResults::Succeeded) {
+ rep.nFailedProjectQCalls--; // never mind!
+ if (results.getAnyChangeMade()) rep.nQProjections++;
+ if (qErrEst.size()) rep.nQErrEstProjections++;
+ }
+}
+
+
+
+//------------------------------------------------------------------------------
+// PROJECT U
+//------------------------------------------------------------------------------
+void System::Guts::projectU(State& s, Vector& uErrEst,
+ const ProjectOptions& options,
+ ProjectResults& results) const {
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage::Velocity,
+ "System::Guts::projectU()");
+ const System::Guts::GutsRep& rep = getRep();
+
+ rep.nProjectUCalls++; // counters are mutable
+ rep.nFailedProjectUCalls++; // assume this will throw an exception
+ //---------------------------------------------------------
+ projectUImpl(s,uErrEst,options,results);
+ //---------------------------------------------------------
+ if (results.getExitStatus()==ProjectResults::Succeeded) {
+ rep.nFailedProjectUCalls--; // never mind!
+ if (results.getAnyChangeMade()) rep.nUProjections++;
+ if (uErrEst.size()) rep.nUErrEstProjections++;
+ }
+}
+
+
+
+//------------------------------------------------------------------------------
+// HANDLE EVENTS
+//------------------------------------------------------------------------------
+void System::Guts::handleEvents
+ (State& s, Event::Cause cause, const Array_<EventId>& eventIds,
+ const HandleEventsOptions& options, HandleEventsResults& results) const
+{
+ // TODO: is Model the right stage?
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage::Model,
+ "System::Guts::handleEvents()");
+ const Real savedTime = s.getTime();
+
+ // Save the stage version numbers so we can look for changes.
+ Array_<StageVersion> stageVersions;
+ s.getSystemStageVersions(stageVersions);
+
+ handleEventsImpl(s, cause, eventIds, options, results);
+
+ // Note the lowest stage whose version was changed by the handler.
+ const Stage lowestModified = s.getLowestSystemStageDifference(stageVersions);
+ results.setLowestModifiedStage(lowestModified);
+
+ getRep().nHandleEventsCalls++; // mutable counters
+ getRep().nHandlerCallsThatChangedStage[lowestModified]++;
+
+ SimTK_ASSERT_ALWAYS(s.getTime() == savedTime,
+ "System::Guts::handleEvents(): handleEventsImpl() tried to change the time");
+}
+
+
+
+//------------------------------------------------------------------------------
+// REPORT EVENTS
+//------------------------------------------------------------------------------
+void System::Guts::reportEvents
+ (const State& s, Event::Cause cause, const Array_<EventId>& eventIds) const
+{
+ // TODO: is Model this the right stage?
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage::Model,
+ "System::Guts::reportEvents()");
+ reportEventsImpl(s,cause,eventIds);
+
+ getRep().nReportEventsCalls++; // mutable counter
+ getRep().nHandlerCallsThatChangedStage[Stage::Report]++;
+}
+
+
+
+//------------------------------------------------------------------------------
+// CALC TIME OF NEXT SCHEDULED EVENT
+//------------------------------------------------------------------------------
+void System::Guts::calcTimeOfNextScheduledEvent
+ (const State& s, Real& tNextEvent, Array_<EventId>& eventIds,
+ bool includeCurrentTime) const
+{
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage::Time,
+ "System::Guts::calcTimeOfNextScheduledEvent()");
+ tNextEvent = Infinity;
+ calcTimeOfNextScheduledEventImpl(s,tNextEvent,eventIds,includeCurrentTime);
+}
+
+
+
+//------------------------------------------------------------------------------
+// CALC TIME OF NEXT SCHEDULED REPORT
+//------------------------------------------------------------------------------
+void System::Guts::calcTimeOfNextScheduledReport
+ (const State& s, Real& tNextEvent, Array_<EventId>& eventIds,
+ bool includeCurrentTime) const
+{
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage::Time,
+ "System::Guts::calcTimeOfNextScheduledReport()");
+ tNextEvent = Infinity;
+ calcTimeOfNextScheduledReportImpl(s,tNextEvent,eventIds,includeCurrentTime);
+}
+
+
+
+//------------------------------------------------------------------------------
+// CALC EVENT TRIGGER INFO
+//------------------------------------------------------------------------------
+void System::Guts::calcEventTriggerInfo
+ (const State& s, Array_<EventTriggerInfo>& info) const {
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage::Instance,
+ "System::Guts::calcEventTriggerInfo()");
+ calcEventTriggerInfoImpl(s,info);
+}
+
+
+
+//------------------------------------------------------------------------------
+// REALIZE
+//------------------------------------------------------------------------------
+void System::Guts::realize(const State& s, Stage g) const {
+ SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage::Model,
+ "System::Guts::realize()");
+
+ Stage stageNow = Stage::Empty;
+ while ((stageNow=s.getSystemStage()) < g) {
+ switch (stageNow) {
+ case Stage::Model: realizeInstance(s); break;
+ case Stage::Instance: realizeTime(s); break;
+ case Stage::Time: realizePosition(s); break;
+ case Stage::Position: realizeVelocity(s); break;
+ case Stage::Velocity: realizeDynamics(s); break;
+ case Stage::Dynamics: realizeAcceleration(s); break;
+ case Stage::Acceleration: realizeReport(s); break;
+ default: assert(!"System::Guts::realize(): bad stage");
+ }
+ }
+}
+
+//------------------------------------------------------------------------------
+// CALC DECORATIVE GEOMETRY AND APPEND
+//------------------------------------------------------------------------------
+void System::Guts::calcDecorativeGeometryAndAppend
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+{
+ assert(stage==Stage::Topology || s.getSystemStage() >= stage);
+ for (SubsystemIndex i(0); i<getNumSubsystems(); ++i)
+ getRep().subsystems[i].getSubsystemGuts()
+ .calcDecorativeGeometryAndAppend(s, stage, geom);
+}
+
+
+SubsystemIndex System::Guts::adoptSubsystem(Subsystem& src) {
+ return updRep().adoptSubsystem(src);
+}
+
+//------------------------------------------------------------------------------
+// Default implementations of virtual methods. Most quietly do nothing. A
+// concrete System will typically override these.
+//------------------------------------------------------------------------------
+
+
+// These multiplyByN methods must be implemented if used.
+void System::Guts::multiplyByNImpl
+ (const State& state, const Vector& u, Vector& dq) const
+{ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "System::Guts",
+ "multiplyByNImpl"); }
+void System::Guts::multiplyByNTransposeImpl
+ (const State& state, const Vector& fq, Vector& fu) const
+{ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "System::Guts",
+ "multiplyByNTransposeImpl"); }
+void System::Guts::multiplyByNPInvImpl
+ (const State& state, const Vector& dq, Vector& u) const
+{ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "System::Guts",
+ "multiplyByNPInvImpl"); }
+void System::Guts::multiplyByNPInvTransposeImpl
+ (const State& state, const Vector& fu, Vector& fq) const
+{ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "System::Guts",
+ "multiplyByNPInvTransposeImpl"); }
+
+
+//------------------------------------------------------------------------------
+// HANDLE EVENTS IMPL
+//------------------------------------------------------------------------------
+// This is the default implementation but shouldn't need to be overridden. It
+// distributes handleEvents() calls to each relevant Subsystem, in order of
+// SubsystemIndex.
+void System::Guts::handleEventsImpl
+ (State& state, Event::Cause cause, const Array_<EventId>& eventIds,
+ const HandleEventsOptions& options, HandleEventsResults& results) const
+{
+ // Event handlers should not be able to modify the time. We'll save time
+ // here and restore it at the end if it might have been changed.
+ const Real savedT = state.getTime();
+
+ // Note: caller will take care of lowest modified stage.
+ results.setExitStatus(HandleEventsResults::Succeeded); // assume success
+
+ // If we have been supplied a list of eventIds, group them by Subsystem
+ // ownership and only invoke handleEvents() on Subsystems that own one or
+ // more of the eventIds. If there are no eventIds then this is a generic
+ // event like Initialization and all the Subsystems get a call.
+
+ Array_<EventId> eventsForSubsystem;
+ for (SubsystemIndex sx(0); sx < getNumSubsystems(); ++sx) {
+ const Subsystem::Guts& sub = getRep().subsystems[sx].getSubsystemGuts();
+
+ if (!eventIds.empty()) {
+ getSystem().getDefaultSubsystem().findSubsystemEventIds
+ (sub.getMySubsystemIndex(), state, eventIds, eventsForSubsystem);
+ if (eventsForSubsystem.empty())
+ continue; // no events for this Subsystem
+ }
+
+ //---------------------------------------------------------------------
+ HandleEventsResults subsysResults;
+ sub.handleEvents(state,cause,eventsForSubsystem,options,subsysResults);
+ //---------------------------------------------------------------------
+
+ if (subsysResults.getAnyChangeMade())
+ results.setAnyChangeMade(true);
+
+ if (subsysResults.getExitStatus() == HandleEventsResults::Failed) {
+ results.setExitStatus(HandleEventsResults::Failed);
+ results.setMessage(subsysResults.getMessage());
+ break; // must quit at first failure
+ }
+
+ if (subsysResults.getExitStatus()==HandleEventsResults::ShouldTerminate)
+ results.setExitStatus(HandleEventsResults::ShouldTerminate);
+ // termination required, but keep going
+ }
+
+ if (state.getTime() != savedT)
+ state.setTime(savedT);
+}
+
+
+
+//------------------------------------------------------------------------------
+// REPORT EVENTS IMPL
+//------------------------------------------------------------------------------
+// This is the default implementation but shouldn't need to be overridden. It
+// distributes reportEvents() calls to each relevant Subsystem, in order of
+// SubsystemIndex.
+int System::Guts::reportEventsImpl
+ (const State& s, Event::Cause cause, const Array_<EventId>& eventIds) const
+{
+ // If we have been supplied a list of eventIds, group them by Subsystem
+ // ownership and only invoke reportEvents() on Subsystems that own one or
+ // more of the eventIds. If there are no eventIds then this is a generic
+ // reporting event and all the Subsystems get a call.
+
+ Array_<EventId> eventsForSubsystem;
+ for (SubsystemIndex sx(0); sx < getNumSubsystems(); ++sx) {
+ const Subsystem::Guts& sub = getRep().subsystems[sx].getSubsystemGuts();
+
+ if (!eventIds.empty()) {
+ getSystem().getDefaultSubsystem().findSubsystemEventIds
+ (sub.getMySubsystemIndex(), s, eventIds, eventsForSubsystem);
+ if (eventsForSubsystem.empty())
+ continue; // no reporting events for this Subsystem
+ }
+
+ //---------------------------------------------------------------------
+ sub.reportEvents(s, cause, eventsForSubsystem);
+ //---------------------------------------------------------------------
+ }
+ return 0;
+}
+
+
+
+//------------------------------------------------------------------------------
+// CALC EVENT TRIGGER INFO IMPL
+//------------------------------------------------------------------------------
+// This is the default implementation but shouldn't need to be overridden.
+int System::Guts::calcEventTriggerInfoImpl
+ (const State& s, Array_<EventTriggerInfo>& info) const {
+
+ // Loop over each subsystem, get its EventTriggerInfos, and combine all of
+ // them into a single list.
+
+ info.clear();
+ for (SubsystemIndex sx(0); sx < getNumSubsystems(); ++sx) {
+ const Subsystem::Guts& sub = getRep().subsystems[sx].getSubsystemGuts();
+ Array_<EventTriggerInfo> subinfo;
+ sub.calcEventTriggerInfo(s, subinfo);
+ for (Array_<EventTriggerInfo>::const_iterator e = subinfo.begin();
+ e != subinfo.end(); e++)
+ {
+ info.push_back(*e);
+ }
+ }
+ return 0;
+}
+
+
+
+//------------------------------------------------------------------------------
+// CALC TIME OF NEXT SCHEDULED EVENT IMPL
+//------------------------------------------------------------------------------
+// This is the default implementation but shouldn't need to be overridden.
+int System::Guts::calcTimeOfNextScheduledEventImpl
+ (const State& s, Real& tNextEvent, Array_<EventId>& eventIds,
+ bool includeCurrentTime) const
+{
+ tNextEvent = Infinity;
+ eventIds.clear();
+ Array_<EventId> ids;
+ for (SubsystemIndex sx(0); sx < getNumSubsystems(); ++sx) {
+ const Subsystem::Guts& sub = getRep().subsystems[sx].getSubsystemGuts();
+ Real time;
+ ids.clear();
+ sub.calcTimeOfNextScheduledEvent(s, time, ids, includeCurrentTime);
+ if (time <= tNextEvent) {
+ tNextEvent = time;
+ if (time < tNextEvent)
+ eventIds.clear(); // otherwise just accumulate
+ for (int i = 0; i < (int)ids.size(); ++i)
+ eventIds.push_back(ids[i]);
+ }
+ }
+ return 0;
+}
+
+
+
+//------------------------------------------------------------------------------
+// CALC TIME OF NEXT SCHEDULED REPORT IMPL
+//------------------------------------------------------------------------------
+// This is the default implementation but shouldn't need to be overridden.
+int System::Guts::calcTimeOfNextScheduledReportImpl
+ (const State& s, Real& tNextEvent, Array_<EventId>& eventIds,
+ bool includeCurrentTime) const
+{
+ tNextEvent = Infinity;
+ eventIds.clear();
+ Array_<EventId> ids;
+ for (SubsystemIndex sx(0); sx < getNumSubsystems(); ++sx) {
+ const Subsystem::Guts& sub = getRep().subsystems[sx].getSubsystemGuts();
+ Real time;
+ ids.clear();
+ sub.calcTimeOfNextScheduledReport(s, time, ids, includeCurrentTime);
+ if (time <= tNextEvent) {
+ tNextEvent = time;
+ if (time < tNextEvent)
+ eventIds.clear(); // otherwise just accumulate
+ for (int i = 0; i < (int)ids.size(); ++i)
+ eventIds.push_back(ids[i]);
+ }
+ }
+ return 0;
+}
+
+
+
+//==============================================================================
+// SYSTEM :: GUTS :: GUTS REP
+//==============================================================================
+// All inline currently.
+
+
+
+//==============================================================================
+// DEFAULT SYSTEM SUBSYSTEM
+//==============================================================================
+
+// Instance-stage cache entry that records the motion method for every q, u,
+// and udot.
+// TODO: not used yet
+class MotionInfo {
+public:
+ // Allocated at end of realize(Model) when we know nq and nu and initialized
+ // to "free". Subsystems fill these in during realize(Instance).
+ Array_<unsigned char> qMotion; // nq
+ Array_<unsigned char> uMotion; // nu
+ Array_<unsigned char> udotMotion; // nu
+
+ // We'll calculate these index arrays once all Subsystems have had a
+ // chance to set the motion info for their variables.
+ Array_<int>& freeQIndex; // nfq indices of each q still marked "free"
+ Array_<int>& freeUIndex; // nfu indices of each u still marked "free"
+ Array_<int>& freeUDotIndex; // nfudot indices of udot still marked "free"
+};
+
+// Whether to use relative accuracy for u and z variables.
+// TODO: not used yet
+class StateVarUseRelAccuracyUZ {
+public:
+ // Whether to scale by 1/u and 1/z when u and z are large. We never
+ // use relative accuracy for q.
+ Array_<bool> useRelativeAccuracyU; // nu
+ Array_<bool> useRelativeAccuracyZ; // nz
+};
+
+// This class stores various information used by the default subsystem
+// that needs to be stored in the state.
+class CachedEventInfo {
+public:
+ CachedEventInfo() : eventIdCounter(0) {}
+ int eventIdCounter;
+ std::map<int, SubsystemIndex> eventOwnerMap;
+ Array_<EventId> scheduledEventIds;
+ Array_<EventTriggerByStageIndex> triggeredEventIndices;
+ Array_<EventId> triggeredEventIds;
+ Array_<EventId> scheduledReportIds;
+ Array_<EventTriggerByStageIndex> triggeredReportIndices;
+ Array_<EventId> triggeredReportIds;
+};
+
+class DefaultSystemSubsystem::Guts : public Subsystem::Guts {
+public:
+
+ Guts() : Subsystem::Guts("DefaultSystemSubsystem::Guts", "0.0.1") { }
+
+ ~Guts() {
+ for (int i = 0; i < (int)scheduledEventHandlers.size(); ++i)
+ delete scheduledEventHandlers[i];
+ for (int i = 0; i < (int)triggeredEventHandlers.size(); ++i)
+ delete triggeredEventHandlers[i];
+ for (int i = 0; i < (int)scheduledEventReporters.size(); ++i)
+ delete scheduledEventReporters[i];
+ for (int i = 0; i < (int)triggeredEventReporters.size(); ++i)
+ delete triggeredEventReporters[i];
+ }
+
+ Guts* cloneImpl() const OVERRIDE_11 {
+ return new Guts(*this);
+ }
+
+ const Array_<ScheduledEventHandler*>& getScheduledEventHandlers() const {
+ return scheduledEventHandlers;
+ }
+
+ Array_<ScheduledEventHandler*>& updScheduledEventHandlers() {
+ invalidateSubsystemTopologyCache();
+ return scheduledEventHandlers;
+ }
+
+ const Array_<TriggeredEventHandler*>& getTriggeredEventHandlers() const {
+ return triggeredEventHandlers;
+ }
+
+ Array_<TriggeredEventHandler*>& updTriggeredEventHandlers() {
+ invalidateSubsystemTopologyCache();
+ return triggeredEventHandlers;
+ }
+
+ const Array_<ScheduledEventReporter*>& getScheduledEventReporters() const {
+ return scheduledEventReporters;
+ }
+
+ Array_<ScheduledEventReporter*>& updScheduledEventReporters() const {
+ invalidateSubsystemTopologyCache();
+ return scheduledEventReporters;
+ }
+
+ const Array_<TriggeredEventReporter*>& getTriggeredEventReporters() const {
+ return triggeredEventReporters;
+ }
+
+ Array_<TriggeredEventReporter*>& updTriggeredEventReporters() const {
+ invalidateSubsystemTopologyCache();
+ return triggeredEventReporters;
+ }
+
+ const CachedEventInfo& getCachedEventInfo(const State& s) const {
+ return Value<CachedEventInfo>::downcast
+ (getCacheEntry(s, cachedEventInfoIndex)).get();
+ }
+
+ CachedEventInfo& updCachedEventInfo(const State& s) const {
+ return Value<CachedEventInfo>::downcast
+ (updCacheEntry(s, cachedEventInfoIndex)).upd();
+ }
+
+ int realizeSubsystemTopologyImpl(State& s) const OVERRIDE_11 {
+ cachedEventInfoIndex = s.allocateCacheEntry(getMySubsystemIndex(),
+ Stage::Topology,
+ new Value<CachedEventInfo>());
+ CachedEventInfo& info = updCachedEventInfo(s);
+ info.scheduledEventIds.clear();
+ info.triggeredEventIndices.clear();
+ info.triggeredEventIds.clear();
+ info.scheduledReportIds.clear();
+ info.triggeredReportIndices.clear();
+ info.triggeredReportIds.clear();
+ info.eventIdCounter = 0;
+ for (Array_<ScheduledEventHandler*>::const_iterator
+ e = scheduledEventHandlers.begin();
+ e != scheduledEventHandlers.end(); e++) {
+ EventId id;
+ createScheduledEvent(s, id);
+ info.scheduledEventIds.push_back(id);
+ }
+ for (Array_<TriggeredEventHandler*>::const_iterator
+ e = triggeredEventHandlers.begin();
+ e != triggeredEventHandlers.end(); e++) {
+ EventId id;
+ EventTriggerByStageIndex index;
+ createTriggeredEvent(s, id, index, (*e)->getRequiredStage());
+ info.triggeredEventIds.push_back(id);
+ info.triggeredEventIndices.push_back(index);
+ }
+ for (Array_<ScheduledEventReporter*>::const_iterator
+ e = scheduledEventReporters.begin();
+ e != scheduledEventReporters.end(); e++) {
+ EventId id;
+ createScheduledEvent(s, id);
+ info.scheduledReportIds.push_back(id);
+ }
+ for (Array_<TriggeredEventReporter*>::const_iterator
+ e = triggeredEventReporters.begin();
+ e != triggeredEventReporters.end(); e++) {
+ EventId id;
+ EventTriggerByStageIndex index;
+ createTriggeredEvent(s, id, index, (*e)->getRequiredStage());
+ info.triggeredReportIds.push_back(id);
+ info.triggeredReportIndices.push_back(index);
+ }
+ return 0;
+ }
+
+ int realizeSubsystemModelImpl(State& s) const OVERRIDE_11 {
+ return 0;
+ }
+
+ int realizeEventTriggers(const State& s, Stage g) const {
+ const CachedEventInfo& info = getCachedEventInfo(s);
+ Vector& triggers = s.updEventTriggersByStage(getMySubsystemIndex(), g);
+ for (int i = 0; i < (int)triggeredEventHandlers.size(); ++i) {
+ if (g == triggeredEventHandlers[i]->getRequiredStage())
+ triggers[info.triggeredEventIndices[i]] =
+ triggeredEventHandlers[i]->getValue(s);
+ }
+ for (int i = 0; i < (int)triggeredEventReporters.size(); ++i) {
+ if (g == triggeredEventReporters[i]->getRequiredStage())
+ triggers[info.triggeredReportIndices[i]] =
+ triggeredEventReporters[i]->getValue(s);
+ }
+ return 0;
+ }
+
+ int realizeSubsystemInstanceImpl(const State& s) const OVERRIDE_11 {
+ return 0;
+ }
+ int realizeSubsystemTimeImpl(const State& s) const OVERRIDE_11 {
+ return realizeEventTriggers(s, Stage::Time);
+ }
+ int realizeSubsystemPositionImpl(const State& s) const OVERRIDE_11 {
+ return realizeEventTriggers(s, Stage::Position);
+ }
+ int realizeSubsystemVelocityImpl(const State& s) const OVERRIDE_11 {
+ return realizeEventTriggers(s, Stage::Velocity);
+ }
+ int realizeSubsystemDynamicsImpl(const State& s) const OVERRIDE_11 {
+ return realizeEventTriggers(s, Stage::Dynamics);
+ }
+ int realizeSubsystemAccelerationImpl(const State& s) const OVERRIDE_11 {
+ return realizeEventTriggers(s, Stage::Acceleration);
+ }
+ int realizeSubsystemReportImpl(const State& s) const OVERRIDE_11 {
+ return realizeEventTriggers(s, Stage::Report);
+ }
+
+ void calcEventTriggerInfoImpl
+ (const State& s, Array_<EventTriggerInfo>& trigInfo) const OVERRIDE_11
+ {
+
+ // Loop over all registered TriggeredEventHandlers and
+ // TriggeredEventReporters, and ask each one for its EventTriggerInfo.
+
+ const CachedEventInfo& info = getCachedEventInfo(s);
+ trigInfo.resize( triggeredEventHandlers.size()
+ + triggeredEventReporters.size());
+
+ for (int i = 0; i < (int)triggeredEventHandlers.size(); ++i) {
+ const Stage stage = triggeredEventHandlers[i]->getRequiredStage();
+ const SystemEventTriggerIndex index
+ ( info.triggeredEventIndices[i]
+ + s.getEventTriggerStartByStage(stage)
+ + s.getEventTriggerStartByStage(getMySubsystemIndex(), stage));
+ trigInfo[index] = triggeredEventHandlers[i]->getTriggerInfo();
+ trigInfo[index].setEventId(info.triggeredEventIds[i]);
+ }
+
+ for (int i = 0; i < (int)triggeredEventReporters.size(); ++i) {
+ const Stage stage = triggeredEventReporters[i]->getRequiredStage();
+ const SystemEventTriggerIndex index
+ ( info.triggeredReportIndices[i]
+ + s.getEventTriggerStartByStage(stage)
+ + s.getEventTriggerStartByStage(getMySubsystemIndex(), stage));
+ trigInfo[index] = triggeredEventReporters[i]->getTriggerInfo();
+ trigInfo[index].setEventId(info.triggeredReportIds[i]);
+ }
+ }
+ void calcTimeOfNextScheduledEventImpl(const State& s, Real& tNextEvent,
+ Array_<EventId>& eventIds, bool includeCurrentTime) const OVERRIDE_11 {
+ // Loop over all registered ScheduledEventHandlers, and ask each one
+ // when its next event occurs.
+
+ const CachedEventInfo& info = getCachedEventInfo(s);
+ tNextEvent = Infinity;
+ for (int i = 0; i < (int)scheduledEventHandlers.size(); ++i) {
+ Real time = scheduledEventHandlers[i]->getNextEventTime
+ (s, includeCurrentTime);
+ if (time <= tNextEvent
+ && (time > s.getTime()
+ || (includeCurrentTime && time == s.getTime())))
+ {
+ if (time < tNextEvent)
+ eventIds.clear();
+ tNextEvent = time;
+ eventIds.push_back(info.scheduledEventIds[i]);
+ }
+ }
+ }
+ void calcTimeOfNextScheduledReportImpl(const State& s, Real& tNextEvent,
+ Array_<EventId>& eventIds, bool includeCurrentTime) const OVERRIDE_11 {
+ // Loop over all registered ScheduledEventReporters, and ask each one
+ // when its next event occurs.
+
+ const CachedEventInfo& info = getCachedEventInfo(s);
+ tNextEvent = Infinity;
+ for (int i = 0; i < (int)scheduledEventReporters.size(); ++i) {
+ Real time = scheduledEventReporters[i]->getNextEventTime
+ (s, includeCurrentTime);
+ if (time <= tNextEvent
+ && (time > s.getTime()
+ || (includeCurrentTime && time == s.getTime())))
+ {
+ if (time < tNextEvent)
+ eventIds.clear();
+ tNextEvent = time;
+ eventIds.push_back(info.scheduledReportIds[i]);
+ }
+ }
+ }
+ void handleEventsImpl(State& s, Event::Cause cause,
+ const Array_<EventId>& eventIds,
+ const HandleEventsOptions& options,
+ HandleEventsResults& results) const OVERRIDE_11
+ {
+ const CachedEventInfo& info = getCachedEventInfo(s);
+ const Real accuracy = options.getAccuracy();
+ bool shouldTerminate = false;
+
+ // Build a set of the ids for quick lookup.
+ std::set<EventId> idSet;
+ for (int i = 0; i < (int)eventIds.size(); ++i)
+ idSet.insert(eventIds[i]);
+
+ // Process triggered events.
+
+ if (cause == Event::Cause::Triggered) {
+ for (int i = 0; i < (int)triggeredEventHandlers.size(); ++i) {
+ if (idSet.find(info.triggeredEventIds[i]) != idSet.end()) {
+ bool eventShouldTerminate = false;
+ triggeredEventHandlers[i]->handleEvent
+ (s, accuracy, eventShouldTerminate);
+ if (eventShouldTerminate)
+ shouldTerminate = true;
+ }
+ }
+ for (int i = 0; i < (int)triggeredEventReporters.size(); ++i) {
+ if (idSet.find(info.triggeredReportIds[i]) != idSet.end())
+ triggeredEventReporters[i]->handleEvent(s);
+ }
+ }
+
+ // Process scheduled events.
+
+ if (cause == Event::Cause::Scheduled) {
+ for (int i = 0; i < (int)scheduledEventHandlers.size(); ++i) {
+ if (idSet.find(info.scheduledEventIds[i]) != idSet.end()) {
+ bool eventShouldTerminate = false;
+ scheduledEventHandlers[i]->handleEvent
+ (s, accuracy, eventShouldTerminate);
+ if (eventShouldTerminate)
+ shouldTerminate = true;
+ }
+ }
+ for (int i = 0; i < (int)scheduledEventReporters.size(); ++i) {
+ if (idSet.find(info.scheduledReportIds[i]) != idSet.end())
+ scheduledEventReporters[i]->handleEvent(s);
+ }
+ }
+
+ // Assume some change was made.
+ results.setAnyChangeMade(true);
+
+ results.setExitStatus(shouldTerminate
+ ? HandleEventsResults::ShouldTerminate
+ : HandleEventsResults::Succeeded);
+ }
+
+ void reportEventsImpl(const State& s, Event::Cause cause,
+ const Array_<EventId>& eventIds) const OVERRIDE_11 {
+ const CachedEventInfo& info = getCachedEventInfo(s);
+
+ // Build a set of the ids for quick lookup.
+
+ std::set<EventId> idSet;
+ for (int i = 0; i < (int)eventIds.size(); ++i)
+ idSet.insert(eventIds[i]);
+
+ // Process triggered events.
+
+ if (cause == Event::Cause::Triggered) {
+ for (int i = 0; i < (int)triggeredEventReporters.size(); ++i) {
+ if (idSet.find(info.triggeredReportIds[i]) != idSet.end())
+ triggeredEventReporters[i]->handleEvent(s);
+ }
+ }
+
+ // Process scheduled events.
+
+ if (cause == Event::Cause::Scheduled) {
+ for (int i = 0; i < (int)scheduledEventReporters.size(); ++i) {
+ if (idSet.find(info.scheduledReportIds[i]) != idSet.end())
+ scheduledEventReporters[i]->handleEvent(s);
+ }
+ }
+ }
+
+private:
+ mutable CacheEntryIndex cachedEventInfoIndex;
+ mutable Array_<ScheduledEventHandler*> scheduledEventHandlers;
+ mutable Array_<TriggeredEventHandler*> triggeredEventHandlers;
+ mutable Array_<ScheduledEventReporter*> scheduledEventReporters;
+ mutable Array_<TriggeredEventReporter*> triggeredEventReporters;
+};
+
+DefaultSystemSubsystem::DefaultSystemSubsystem(System& sys) {
+ adoptSubsystemGuts(new DefaultSystemSubsystem::Guts());
+ sys.adoptSubsystem(*this);
+}
+
+const DefaultSystemSubsystem::Guts& DefaultSystemSubsystem::getGuts() const {
+ return SimTK_DYNAMIC_CAST_DEBUG<const DefaultSystemSubsystem::Guts&>
+ (getSubsystemGuts());
+}
+
+DefaultSystemSubsystem::Guts& DefaultSystemSubsystem::updGuts() {
+ return SimTK_DYNAMIC_CAST_DEBUG<DefaultSystemSubsystem::Guts&>
+ (updSubsystemGuts());
+}
+
+/**
+ * Add a ScheduledEventHandler to the System. This must be called before the
+ * Model stage is realized.
+ *
+ * The System assumes ownership of the object passed to this method, and will
+ * delete it when the System is deleted.
+ */
+void DefaultSystemSubsystem::addEventHandler(ScheduledEventHandler* handler) {
+ updGuts().updScheduledEventHandlers().push_back(handler);
+}
+
+/*
+ * Add a TriggeredEventHandler to the System. This must be called before the
+ * Model stage is realized.
+ *
+ * The System assumes ownership of the object passed to this method, and will
+ * delete it when the System is deleted.
+ */
+void DefaultSystemSubsystem::
+addEventHandler(TriggeredEventHandler* handler) {
+ updGuts().updTriggeredEventHandlers().push_back(handler);
+}
+
+/*
+ * Add a ScheduledEventReporter to the System. This must be called before the
+ * Model stage is realized.
+ *
+ * The System assumes ownership of the object passed to this method, and will
+ * delete it when the System is deleted.
+ *
+ * Note that this method is const. Because an EventReporter cannot affect the
+ * behavior of the system being simulated, it is permitted to add one to a
+ * const System.
+ */
+void DefaultSystemSubsystem::
+addEventReporter(ScheduledEventReporter* handler) const {
+ getGuts().updScheduledEventReporters().push_back(handler);
+}
+
+/*
+ * Add a TriggeredEventReporter to the System. This must be called before the
+ * Model stage is realized.
+ *
+ * The System assumes ownership of the object passed to this method, and will
+ * delete it when the System is deleted.
+ *
+ * Note that this method is const. Because an EventReporter cannot affect the
+ * behavior of the system being simulated, it is permitted to add one to a
+ * const System.
+ */
+void DefaultSystemSubsystem::
+addEventReporter(TriggeredEventReporter* handler) const {
+ getGuts().updTriggeredEventReporters().push_back(handler);
+}
+
+/**
+ * Generate a new, globally unique event ID. Typically you will not call this
+ * directly. When a Subsystem needs to obtain an event ID for an event it
+ * defines, it should do so by calling Subsystem::Guts::createScheduledEvent()
+ * or Subsystem::Guts::createTriggeredEvent().
+ */
+EventId DefaultSystemSubsystem::
+createEventId(SubsystemIndex subsys, const State& state) const {
+ // Must use "upd" here because this is called from realize()
+ // while we're still filling in the CachedEventInfo.
+ CachedEventInfo& info = getGuts().updCachedEventInfo(state);
+ int id = info.eventIdCounter++;
+ info.eventOwnerMap[id] = subsys;
+ return EventId(id);
+}
+
+/**
+ * Given a list of event IDs, filter it to produce a list of those events
+ * belonging to a particular Subsystem.
+ *
+ * @param subsys the Subsystem for which to find events
+ * @param state the State which produced the events
+ * @param allEvents a list of event IDs to filter
+ * @param eventsForSubsystem on exit, this Array_ contains the filtered list
+ * of event IDs belonging to the specified Subsystem.
+ */
+void DefaultSystemSubsystem::findSubsystemEventIds
+ (SubsystemIndex subsys, const State& state, const Array_<EventId>& allEvents,
+ Array_<EventId>& eventsForSubsystem) const
+{
+ const CachedEventInfo& info = getGuts().getCachedEventInfo(state);
+ eventsForSubsystem.clear();
+ for (int i = 0; i < (int)allEvents.size(); ++i) {
+ std::map<int, SubsystemIndex>::const_iterator p =
+ info.eventOwnerMap.find(allEvents[i]);
+ assert(p != info.eventOwnerMap.end());
+ if (p->second == subsys)
+ eventsForSubsystem.push_back(allEvents[i]);
+ }
+}
+
+
+} // namespace SimTK
+
diff --git a/SimTKcommon/Simulation/src/SystemGutsRep.h b/SimTKcommon/Simulation/src/SystemGutsRep.h
new file mode 100644
index 0000000..04af54d
--- /dev/null
+++ b/SimTKcommon/Simulation/src/SystemGutsRep.h
@@ -0,0 +1,235 @@
+#ifndef SimTK_SimTKCOMMON_SYSTEM_GUTSREP_H_
+#define SimTK_SimTKCOMMON_SYSTEM_GUTSREP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+
+// This header is internal source code and is not part of the SimTKcommon
+// API or distribution. This is the private, opaque implementation of
+// the System::Guts class, which contains just a pointer to the
+// object declared here.
+
+#include "SimTKcommon/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/State.h"
+#include "SimTKcommon/internal/Subsystem.h"
+
+#include "SimTKcommon/internal/System.h"
+#include "SimTKcommon/internal/SystemGuts.h"
+
+#include "SubsystemGutsRep.h"
+
+namespace SimTK {
+
+class System::Guts::GutsRep {
+public:
+
+ GutsRep(const String& name, const String& version)
+ : systemName(name),
+ systemVersion(version),
+ myHandle(0),
+ defaultTimeScale(Real(0.1)),
+ defaultLengthScale(Real(1)),
+ defaultUpDirection(YAxis),
+ useUniformBackground(false),
+ hasTimeAdvancedEventsFlag(false),
+ systemTopologyRealized(false),
+ topologyCacheVersion(1) // not zero
+
+ {
+ resetAllCounters();
+ }
+
+ // Default constructor invokes the one above.
+ GutsRep() : defaultUpDirection(YAxis)
+ { new (this) GutsRep("<NONAME>", "2.2.0"); }
+
+ GutsRep(const GutsRep& src)
+ : systemName(src.systemName),
+ systemVersion(src.systemVersion),
+ subsystems(src.subsystems),
+ myHandle(0),
+ defaultTimeScale(src.defaultTimeScale),
+ defaultLengthScale(src.defaultLengthScale),
+ defaultUpDirection(src.defaultUpDirection),
+ useUniformBackground(src.useUniformBackground),
+ hasTimeAdvancedEventsFlag(src.hasTimeAdvancedEventsFlag),
+ systemTopologyRealized(false),
+ topologyCacheVersion(src.topologyCacheVersion)
+ {
+ resetAllCounters();
+ }
+
+ ~GutsRep() {
+ clearMyHandle();
+ subsystems.clear();
+ invalidateSystemTopologyCache();
+ }
+
+ const String& getName() const {return systemName;}
+ const String& getVersion() const {return systemVersion;}
+
+ void setDefaultTimeScale(Real tc)
+ { defaultTimeScale = tc; }
+ Real getDefaultTimeScale() const {return defaultTimeScale;}
+ void setDefaultLengthScale(Real lc)
+ { defaultLengthScale = lc; }
+ Real getDefaultLengthScale() const {return defaultLengthScale;}
+
+ void setUpDirection(const CoordinateDirection& up)
+ { defaultUpDirection = up; }
+ CoordinateDirection getUpDirection() const {return defaultUpDirection;}
+ void setUseUniformBackground(bool useUniform)
+ { useUniformBackground = useUniform; }
+ bool getUseUniformBackground() const {return useUniformBackground;}
+
+ const State& getDefaultState() const {return defaultState;}
+ State& updDefaultState() {return defaultState;}
+
+ int getNumSubsystems() const {return (int)subsystems.size();}
+ const Subsystem& getSubsystem(SubsystemIndex i) const {return subsystems[i];}
+ Subsystem& updSubsystem(SubsystemIndex i) {return subsystems[i];}
+
+
+ // Take over ownership from the Subsystem handle, allocate a new
+ // subsystem slot for it, and return the slot number. This is only
+ // allowed if the supplied Subsytem already has a rep, but is
+ // NOT part of some other System.
+ SubsystemIndex adoptSubsystem(Subsystem& child) {
+ assert(child.hasGuts() && !child.isInSystem()); // TODO
+ assert(child.isOwnerHandle());
+
+ // This is a topology change.
+ invalidateSystemTopologyCache();
+
+ const SubsystemIndex id = SubsystemIndex((int)subsystems.size());
+ subsystems.resize(id+1); // grow
+ Subsystem& s = subsystems.back(); // refer to the empty handle
+
+ // Take over ownership of the child's guts, leaving the child
+ // as a non-owner reference to the same guts.
+ s.adoptSubsystemGuts(&child.updSubsystemGuts());
+ s.setSystem(*myHandle, id);
+
+ return id;
+ }
+
+ void setMyHandle(System& h) {myHandle = &h;}
+ void clearMyHandle() {myHandle=0;}
+
+
+ bool systemTopologyHasBeenRealized() const
+ { return systemTopologyRealized; }
+
+ StageVersion getSystemTopologyCacheVersion() const
+ { return topologyCacheVersion; }
+
+ // Use this cautiously if at all!
+ void setSystemTopologyCacheVersion(StageVersion version) const
+ { assert(version>0); topologyCacheVersion = version; }
+
+ // Invalidating the System topology cache requires invalidating all
+ // Subsystem topology caches also so that the next realizeTopology()
+ // will cause them to request their State resources again so we can
+ // build up the defaultState.
+ void invalidateSystemTopologyCache() const {
+ if (systemTopologyRealized) {
+ // Mark system topology invalid *first* so that the invalidate
+ // subsystem calls below don't recurse back here!
+ systemTopologyRealized = false;
+ topologyCacheVersion++;
+ defaultState.clear();
+
+ for (SubsystemIndex i(0); i < (int)subsystems.size(); ++i)
+ subsystems[i].invalidateSubsystemTopologyCache();
+ }
+ }
+
+protected:
+ String systemName;
+ String systemVersion;
+ StableArray<Subsystem> subsystems;
+
+private:
+ friend class System;
+ friend class System::Guts;
+ System* myHandle; // the owner handle of these guts
+
+ // TOPOLOGY STAGE STATE //
+
+ Real defaultTimeScale; // scaling hints
+ Real defaultLengthScale;
+
+ CoordinateDirection defaultUpDirection; // visualization hint
+ bool useUniformBackground; // visualization hint
+
+ bool hasTimeAdvancedEventsFlag; //TODO: should be in State as a Model variable
+
+
+ // TOPOLOGY STAGE CACHE //
+
+ // This should only be true when *all* subsystems have successfully
+ // completed realizeTopology(). Anything which invalidates topology for
+ // one of the contained subsystem must invalidate topology for the system
+ // as a whole also.
+ mutable bool systemTopologyRealized;
+ mutable StageVersion topologyCacheVersion;
+
+ // This is only meaningful if systemTopologyRealized==true. In that case
+ // its Topology stage version will match the above. A State with a different
+ // Topology version cannot be used with this Subsystem.
+ mutable State defaultState;
+
+ // STATISTICS //
+ mutable int nRealizationsOfStage[Stage::NValid];
+ mutable int nRealizeCalls; // counts realizeTopology(), realizeModel(), realize()
+
+ mutable int nPrescribeQCalls, nPrescribeUCalls;
+
+ mutable int nProjectQCalls, nProjectUCalls;
+ mutable int nFailedProjectQCalls, nFailedProjectUCalls;
+ mutable int nQProjections, nUProjections; // the ones that did something
+ mutable int nQErrEstProjections, nUErrEstProjections;
+
+ mutable int nHandlerCallsThatChangedStage[Stage::NValid];
+ mutable int nHandleEventsCalls;
+ mutable int nReportEventsCalls;
+
+ void resetAllCounters() {
+ for (int i=0; i<Stage::NValid; ++i)
+ nRealizationsOfStage[i] = nHandlerCallsThatChangedStage[i] = 0;
+ nRealizeCalls = nPrescribeQCalls = nPrescribeUCalls = 0;
+ nProjectQCalls = nProjectUCalls = 0;
+ nFailedProjectQCalls = nFailedProjectUCalls = 0;
+ nQProjections = nUProjections = 0;
+ nQErrEstProjections = nUErrEstProjections = 0;
+ nHandleEventsCalls = nReportEventsCalls = 0;
+ }
+
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_SYSTEM_GUTSREP_H_
diff --git a/SimTKcommon/SmallMatrix/include/SimTKcommon/SmallMatrix.h b/SimTKcommon/SmallMatrix/include/SimTKcommon/SmallMatrix.h
new file mode 100644
index 0000000..9b3a70a
--- /dev/null
+++ b/SimTKcommon/SmallMatrix/include/SimTKcommon/SmallMatrix.h
@@ -0,0 +1,337 @@
+#ifndef SimTK_SIMMATRIX_SMALLMATRIX_H_
+#define SimTK_SIMMATRIX_SMALLMATRIX_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This file is the user-includeable header to be included in
+ * user programs to provide fixed-length Vec & Mat classes. The
+ * included internal headers declare templates needed for zero-overhead handling of small
+ * vectors and matrices. The idea is to convey all needed information,
+ * including the size, at compile time through the templatized types.
+ * These classes have well-defined minimal storage layouts and can
+ * be easily interconverted to/from arrays of scalar types, with zero
+ * conversion overhead.
+ *
+ * There are three generic types required: a column vector Vec, a row
+ * vector (a.k.a. "covector") Row, and a matrix Mat. Real and Complex
+ * elements at single, double, and quad precision are supported.
+ * Almost all operations are inline -- this package is intended to
+ * provide performance as good as one could achieve by special-purpose C code.
+ *
+ * When the element type is a "basic numerical type" then the resulting
+ * Vec, Row or Mat is also a basic numerical type, so these can
+ * be built up with structured elements so that Vec< 2,Vec<3> > for
+ * example is a 2-element vector whose elements are Vec<3>'s.
+ *
+ * These classes are designed so that many operations that look like
+ * computations are free. These include: negation, conjugation, extraction
+ * of real and imaginary parts from complex numerical types, and
+ * hermitian or positional transpose. These are performed
+ * by type casting rather than computation. That is, these operations
+ * can be viewed as a change in perspective rather than an actual
+ * computation.
+ *
+ * To achieve zero overhead, separate types are needed to convey
+ * special matrix structure, although appropriate interconversions
+ * are defined. So symmetric matrices are SymMat's and diagonal
+ * matrices are DiagMat's. Note that symmetric matrices are more
+ * properly called Hermitian in that the reflected elements are
+ * conjugates of one another.
+ *
+ * Supported operations
+ *
+ * Class member functions for Vec<M>:
+ * Unary: -, +, ~ (Hermitian transpose)
+ * Assignment: copy assignment of identical object
+ * elementwise assignment from any M-row Vec or Mat
+ * scalar assignment to each element
+ * TODO
+ *
+ * Global operations involving only Vecs or Vec and scalar,
+ * yielding a vector result. (Row has the same set.)
+ * v+s, s+v, v+v
+ * v-s, s-v, v-v
+ * v*s, s*v (can't multiply a vector by another vector)
+ * v/s
+ * v==s, s==v, v==v (same for !=)
+ *
+ * Global operations mixing Row, Vec, Mat
+ * s=r*v (dot product)
+ * m=v*r (outer product)
+ * v=m*v, r=r*m
+ * m=m*m (with compatible dimensions)
+ *
+ */
+
+#include "SimTKcommon/Scalar.h"
+#include "SimTKcommon/TemplatizedLapack.h"
+
+
+#include "SimTKcommon/internal/ResultType.h"
+#include "SimTKcommon/internal/Vec.h"
+#include "SimTKcommon/internal/Row.h"
+#include "SimTKcommon/internal/Mat.h"
+#include "SimTKcommon/internal/SymMat.h"
+#include "SimTKcommon/internal/SmallMatrixMixed.h"
+
+// Friendly abbreviations.
+namespace SimTK {
+/** @defgroup MatVecTypedefs Predefined typedefs
+ at ingroup MatVecUtilities
+
+These typedefs provide convenient synonyms for common matrix and vector types.
+Note that the typedef name may be used interchangeably with the fully
+templatized names; they represent exactly the same type.
+
+The simplest names are for types whose elements are of the compile-time default
+precision type @ref SimTK::Real "Real" which is typically \c double but
+can be changed to \c float at compile time. There are also names prefixed with
+a lowercase "f" that always use float regardless of the default precision.
+
+Note that there are more template parameters than are specified here;
+these typedefs are using default values for them. The missing parameters
+specify the spacing between elements; these typedefs always refer to types for
+which the elements are packed in memory. See Vec, Row, Mat, SymMat for more
+information.
+
+For fixed size vectors and matrices, only the 2-, 3-, and 4-element sizes
+are commonly used. However we provide typedefs for sizes up to 9 in case they
+are needed. For larger sizes, just use the explicit templatized forms.
+**/
+/**@{**/
+/** This is the most common 2D vector type: a column of 2 Real values stored
+consecutively in memory (packed). **/
+typedef Vec<2> Vec2;
+/** This is the most common 3D vector type: a column of 3 Real values stored
+consecutively in memory (packed). **/
+typedef Vec<3> Vec3;
+/** This is the most common 4D vector type: a column of 4 Real values stored
+consecutively in memory (packed). **/
+typedef Vec<4> Vec4;
+
+
+/** This is the most common 2x2 matrix type: two packed columns of 2 Real
+values each. The columns have type \c Vec2 but rows have a stride of 2 so the
+row type is \c Row<2,Real,2>, \e not \c Row2. **/
+typedef Mat<2, 2> Mat22;
+/** This is the most common 3x3 matrix type: three packed columns of 3 Real
+values each. The columns have type \c Vec3 but rows have a stride of 3 so the
+row type is \c Row<3,Real,3>, \e not \c Row3. **/
+typedef Mat<3, 3> Mat33;
+/** This is the most common 4x4 matrix type: four packed columns of 4 Real
+values each. The columns have type \c Vec4 but rows have a stride of 4 so the
+row type is \c Row<4,Real,4>, \e not \c Row4. **/
+typedef Mat<4, 4> Mat44;
+
+/** A compact, 2x2 Real symmetric matrix; only 3 elements are stored. **/
+typedef SymMat<2> SymMat22;
+/** A compact, 3x3 Real symmetric matrix; only 6 elements are stored. **/
+typedef SymMat<3> SymMat33;
+/** A compact, 2x2 Real symmetric matrix; only 10 elements are stored. **/
+typedef SymMat<4> SymMat44;
+
+/** Packed, 2-element row of Real values. This is the type of a transposed Vec2
+and usually does not appear explicitly in user programs. **/
+typedef Row<2> Row2;
+/** Packed, 3-element row of Real values. This is the type of a transposed Vec3
+and usually does not appear explicitly in user programs. **/
+typedef Row<3> Row3;
+/** Packed, 4-element row of Real values. This is the type of a transposed Vec4
+and usually does not appear explicitly in user programs. **/
+typedef Row<4> Row4;
+/**@}**/
+
+/** @defgroup UncommonMatVecTypedefs Less commonly-used typedefs
+ at ingroup MatVecTypedefs **/
+/**@{**/
+
+// Less-popular Vec typedefs.
+typedef Vec<1> Vec1; ///< A vector of just one Real element (not too useful).
+typedef Vec<5> Vec5; ///< Packed, 5-element vector of Real values.
+typedef Vec<6> Vec6; ///< Packed, 6-element vector of Real values.
+typedef Vec<7> Vec7; ///< Packed, 7-element vector of Real values.
+typedef Vec<8> Vec8; ///< Packed, 8-element vector of Real values.
+typedef Vec<9> Vec9; ///< Packed, 9-element vector of Real values.
+
+// Less-popular Mat typedefs.
+typedef Mat<1,1> Mat11; ///< 1x1 Real matrix, that is, a scalar.
+typedef Mat<1,2> Mat12; ///< 1x2 Real row matrix.
+typedef Mat<1,3> Mat13; ///< 1x3 Real row matrix.
+typedef Mat<1,4> Mat14; ///< 1x4 Real row matrix.
+typedef Mat<1,5> Mat15; ///< 1x5 Real row matrix.
+typedef Mat<1,6> Mat16; ///< 1x6 Real row matrix.
+typedef Mat<1,7> Mat17; ///< 1x7 Real row matrix.
+typedef Mat<1,8> Mat18; ///< 1x8 Real row matrix.
+typedef Mat<1,9> Mat19; ///< 1x9 Real row matrix.
+
+typedef Mat<2,1> Mat21; ///< 2x1 Real column matrix.
+typedef Mat<2,3> Mat23; ///< 2x3 Real matrix, packed by columns.
+typedef Mat<2,4> Mat24; ///< 2x4 Real matrix, packed by columns.
+typedef Mat<2,5> Mat25; ///< 2x5 Real matrix, packed by columns.
+typedef Mat<2,6> Mat26; ///< 2x6 Real matrix, packed by columns.
+typedef Mat<2,7> Mat27; ///< 2x7 Real matrix, packed by columns.
+typedef Mat<2,8> Mat28; ///< 2x8 Real matrix, packed by columns.
+typedef Mat<2,9> Mat29; ///< 2x9 Real matrix, packed by columns.
+
+typedef Mat<3,1> Mat31; ///< 3x1 Real column matrix.
+typedef Mat<3,2> Mat32; ///< 3x2 Real matrix, packed by columns.
+typedef Mat<3,4> Mat34; ///< 3x4 Real matrix, packed by columns.
+typedef Mat<3,5> Mat35; ///< 3x5 Real matrix, packed by columns.
+typedef Mat<3,6> Mat36; ///< 3x6 Real matrix, packed by columns.
+typedef Mat<3,7> Mat37; ///< 3x7 Real matrix, packed by columns.
+typedef Mat<3,8> Mat38; ///< 3x8 Real matrix, packed by columns.
+typedef Mat<3,9> Mat39; ///< 3x9 Real matrix, packed by columns.
+
+typedef Mat<4,1> Mat41; ///< 4x1 Real column matrix.
+typedef Mat<4,2> Mat42; ///< 4x2 Real matrix, packed by columns.
+typedef Mat<4,3> Mat43; ///< 4x3 Real matrix, packed by columns.
+typedef Mat<4,5> Mat45; ///< 4x5 Real matrix, packed by columns.
+typedef Mat<4,6> Mat46; ///< 4x6 Real matrix, packed by columns.
+typedef Mat<4,7> Mat47; ///< 4x7 Real matrix, packed by columns.
+typedef Mat<4,8> Mat48; ///< 4x8 Real matrix, packed by columns.
+typedef Mat<4,9> Mat49; ///< 4x9 Real matrix, packed by columns.
+
+typedef Mat<5,1> Mat51; ///< 5x1 Real column matrix.
+typedef Mat<5,2> Mat52; ///< 5x2 Real matrix, packed by columns.
+typedef Mat<5,3> Mat53; ///< 5x3 Real matrix, packed by columns.
+typedef Mat<5,4> Mat54; ///< 5x4 Real matrix, packed by columns.
+typedef Mat<5,5> Mat55; ///< 5x5 Real matrix, packed by columns.
+typedef Mat<5,6> Mat56; ///< 5x6 Real matrix, packed by columns.
+typedef Mat<5,7> Mat57; ///< 5x7 Real matrix, packed by columns.
+typedef Mat<5,8> Mat58; ///< 5x8 Real matrix, packed by columns.
+typedef Mat<5,9> Mat59; ///< 5x9 Real matrix, packed by columns.
+
+typedef Mat<6,1> Mat61; ///< 6x1 Real column matrix.
+typedef Mat<6,2> Mat62; ///< 6x2 Real matrix, packed by columns.
+typedef Mat<6,3> Mat63; ///< 6x3 Real matrix, packed by columns.
+typedef Mat<6,4> Mat64; ///< 6x4 Real matrix, packed by columns.
+typedef Mat<6,5> Mat65; ///< 6x5 Real matrix, packed by columns.
+typedef Mat<6,6> Mat66; ///< 6x6 Real matrix, packed by columns.
+typedef Mat<6,7> Mat67; ///< 6x7 Real matrix, packed by columns.
+typedef Mat<6,8> Mat68; ///< 6x8 Real matrix, packed by columns.
+typedef Mat<6,9> Mat69; ///< 6x9 Real matrix, packed by columns.
+
+typedef Mat<7,1> Mat71; ///< 7x1 Real column matrix.
+typedef Mat<7,2> Mat72; ///< 7x2 Real matrix, packed by columns.
+typedef Mat<7,3> Mat73; ///< 7x3 Real matrix, packed by columns.
+typedef Mat<7,4> Mat74; ///< 7x4 Real matrix, packed by columns.
+typedef Mat<7,5> Mat75; ///< 7x5 Real matrix, packed by columns.
+typedef Mat<7,6> Mat76; ///< 7x6 Real matrix, packed by columns.
+typedef Mat<7,7> Mat77; ///< 7x7 Real matrix, packed by columns.
+typedef Mat<7,8> Mat78; ///< 7x8 Real matrix, packed by columns.
+typedef Mat<7,9> Mat79; ///< 7x9 Real matrix, packed by columns.
+
+typedef Mat<8,1> Mat81; ///< 8x1 Real column matrix.
+typedef Mat<8,2> Mat82; ///< 8x2 Real matrix, packed by columns.
+typedef Mat<8,3> Mat83; ///< 8x3 Real matrix, packed by columns.
+typedef Mat<8,4> Mat84; ///< 8x4 Real matrix, packed by columns.
+typedef Mat<8,5> Mat85; ///< 8x5 Real matrix, packed by columns.
+typedef Mat<8,6> Mat86; ///< 8x6 Real matrix, packed by columns.
+typedef Mat<8,7> Mat87; ///< 8x7 Real matrix, packed by columns.
+typedef Mat<8,8> Mat88; ///< 8x8 Real matrix, packed by columns.
+typedef Mat<8,9> Mat89; ///< 8x9 Real matrix, packed by columns.
+
+typedef Mat<9,1> Mat91; ///< 9x1 Real column matrix.
+typedef Mat<9,2> Mat92; ///< 9x2 Real matrix, packed by columns.
+typedef Mat<9,3> Mat93; ///< 9x3 Real matrix, packed by columns.
+typedef Mat<9,4> Mat94; ///< 9x4 Real matrix, packed by columns.
+typedef Mat<9,5> Mat95; ///< 9x5 Real matrix, packed by columns.
+typedef Mat<9,6> Mat96; ///< 9x6 Real matrix, packed by columns.
+typedef Mat<9,7> Mat97; ///< 9x7 Real matrix, packed by columns.
+typedef Mat<9,8> Mat98; ///< 9x8 Real matrix, packed by columns.
+typedef Mat<9,9> Mat99; ///< 9x9 Real matrix, packed by columns.
+
+// Less-popular SymMat typedefs.
+typedef SymMat<1> SymMat11; ///< 1x1 Real symmetric matrix, that is, a scalar.
+typedef SymMat<5> SymMat55; ///< 5x5 compact Real symmetric matrix.
+typedef SymMat<6> SymMat66; ///< 6x6 compact Real symmetric matrix.
+typedef SymMat<7> SymMat77; ///< 7x7 compact Real symmetric matrix.
+typedef SymMat<8> SymMat88; ///< 8x8 compact Real symmetric matrix.
+typedef SymMat<9> SymMat99; ///< 9x9 compact Real symmetric matrix.
+
+// Less-popular Row typedefs.
+typedef Row<1> Row1; ///< A row vector of one Real element (not too useful).
+/** Packed, 5-element row of Real values. This is the type of a transposed Vec5. **/
+typedef Row<5> Row5;
+/** Packed, 6-element row of Real values. This is the type of a transposed Vec6. **/
+typedef Row<6> Row6;
+/** Packed, 7-element row of Real values. This is the type of a transposed Vec7. **/
+typedef Row<7> Row7;
+/** Packed, 8-element row of Real values. This is the type of a transposed Vec8. **/
+typedef Row<8> Row8;
+/** Packed, 9-element row of Real values. This is the type of a transposed Vec9. **/
+typedef Row<9> Row9;
+
+// float-precision Vecs.
+typedef Vec<1, float> fVec1; ///< A vector of one float element (not too useful).
+typedef Vec<2, float> fVec2; ///< Packed, 2-element vector of \c float values.
+typedef Vec<3, float> fVec3; ///< Packed, 3-element vector of \c float values.
+typedef Vec<4, float> fVec4; ///< Packed, 4-element vector of \c float values.
+typedef Vec<5, float> fVec5; ///< Packed, 5-element vector of \c float values.
+typedef Vec<6, float> fVec6; ///< Packed, 6-element vector of \c float values.
+typedef Vec<7, float> fVec7; ///< Packed, 7-element vector of \c float values.
+typedef Vec<8, float> fVec8; ///< Packed, 8-element vector of \c float values.
+typedef Vec<9, float> fVec9; ///< Packed, 9-element vector of \c float values.
+
+// Just doing some of the popular ones for now.
+typedef Mat<1,1,float> fMat11; ///< 1x1 \c float matrix, that is, a scalar.
+typedef Mat<2,2,float> fMat22; ///< 2x2 \c float matrix, packed by columns.
+typedef Mat<3,3,float> fMat33; ///< 3x3 \c float matrix, packed by columns.
+typedef Mat<3,4,float> fMat34; ///< 3x4 \c float matrix, packed by columns.
+typedef Mat<4,3,float> fMat43; ///< 4x3 \c float matrix, packed by columns.
+typedef Mat<4,4,float> fMat44; ///< 4x4 \c float matrix, packed by columns.
+typedef Mat<5,5,float> fMat55; ///< 5x5 \c float matrix, packed by columns.
+typedef Mat<6,6,float> fMat66; ///< 6x6 \c float matrix, packed by columns.
+typedef Mat<7,7,float> fMat77; ///< 7x7 \c float matrix, packed by columns.
+typedef Mat<8,8,float> fMat88; ///< 8x8 \c float matrix, packed by columns.
+typedef Mat<9,9,float> fMat99; ///< 9x9 \c float matrix, packed by columns.
+
+/** A 1x1 \c float symmetric matrix, that is, a scalar. **/
+typedef SymMat<1, float> fSymMat11;
+typedef SymMat<2, float> fSymMat22; ///< 2x2 compact \c float symmetric matrix.
+typedef SymMat<3, float> fSymMat33; ///< 3x3 compact \c float symmetric matrix.
+typedef SymMat<4, float> fSymMat44; ///< 4x4 compact \c float symmetric matrix.
+typedef SymMat<5, float> fSymMat55; ///< 5x5 compact \c float symmetric matrix.
+typedef SymMat<6, float> fSymMat66; ///< 6x6 compact \c float symmetric matrix.
+typedef SymMat<7, float> fSymMat77; ///< 7x7 compact \c float symmetric matrix.
+typedef SymMat<8, float> fSymMat88; ///< 8x8 compact \c float symmetric matrix.
+typedef SymMat<9, float> fSymMat99; ///< 9x9 compact \c float symmetric matrix.
+
+// float-precision Rows.
+typedef Row<1, float> fRow1; ///< A row vector of one float element (not too useful).
+typedef Row<2, float> fRow2; ///< Packed, 2-element row vector of \c float values.
+typedef Row<3, float> fRow3; ///< Packed, 3-element row vector of \c float values.
+typedef Row<4, float> fRow4; ///< Packed, 4-element row vector of \c float values.
+typedef Row<5, float> fRow5; ///< Packed, 5-element row vector of \c float values.
+typedef Row<6, float> fRow6; ///< Packed, 6-element row vector of \c float values.
+typedef Row<7, float> fRow7; ///< Packed, 7-element row vector of \c float values.
+typedef Row<8, float> fRow8; ///< Packed, 8-element row vector of \c float values.
+typedef Row<9, float> fRow9; ///< Packed, 9-element row vector of \c float values.
+/**@}**/
+
+} //namespace SimTK
+
+
+#endif //SimTK_SIMMATRIX_SMALLMATRIX_H_
diff --git a/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/Mat.h b/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/Mat.h
new file mode 100644
index 0000000..6262a84
--- /dev/null
+++ b/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/Mat.h
@@ -0,0 +1,1593 @@
+#ifndef SimTK_SIMMATRIX_SMALLMATRIX_MAT_H_
+#define SimTK_SIMMATRIX_SMALLMATRIX_MAT_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This file declares class Mat<NROWS, NCOLS, ELEMENT_TYPE, COL_SPACING, ROW_SPACING>.
+ */
+
+#include "SimTKcommon/internal/common.h"
+
+namespace SimTK {
+
+/** @brief This class represents a small matrix whose size is known at compile
+time, containing elements of any Composite Numerical Type (CNT) and engineered
+to have no runtime overhead whatsoever.
+
+ at ingroup MatVecUtilities
+
+Memory layout defaults to packed, column-ordered storage but can be specified to
+have any regular row and column spacing. A %Mat object is itself a Composite
+Numerical Type and can thus be the element type for other matrix and vector
+types. Some common use cases are provided below.
+
+ at tparam M The number of rows in this matrix (no default).
+ at tparam N The number of columns in this matrix (no default).
+ at tparam ELT The element type; default is Real.
+ at tparam CS Column spacing in memory as a multiple of element size (default M).
+ at tparam RS %Row spacing in memory as a multiple of element size (default 1).
+
+<b>Construction</b>
+
+A 3x3 identity matrix can be constructed in the following ways:
+\code
+Mat<3,3,Real>(1,0,0, 0,1,0, 0,0,1); //row-major
+Mat33(1,0,0,0,1,0,0,0,1);
+Mat33(Vec3(1,0,0),Vec3(0,1,0),Vec3(0,0,1)); //column-by-column
+Mat33(1);
+\endcode
+Note that the default element type is Real, and that Mat33 is a typedef for
+%Mat<3,3>; analogous typedefs exist for matrices of up to 9x9 elements.
+
+<b>Manipulation</b>
+
+Standard arithmetic operators can be used, as well as methods like %trace() and
+%transpose(). Here are some usage examples, each of which prints a 2x2 identity
+matrix:
+\code
+Mat23 myMat(2,1,0, 3,0,1);
+std::cout << myMat.getSubMat<2,2>(0,1) << std::endl;
+std::cout << myMat.dropCol(0) << std::endl;
+std::cout << Mat12(1,0).appendRow(~Vec2(0,1)) << std::endl;
+std::cout << Mat21(0,1).insertCol(0,Vec2(1,0)) << std::endl;
+\endcode
+
+<b>Conversion</b>
+
+It may be necessary to convert between a %Mat and a Matrix (to interface with
+%FactorQTZ, for instance). In the example below, we print a Mat33 created from
+a 3x3 Matrix:
+\code
+Matrix myMatrix(3,3);
+for (int i=0; i<9; ++i) myMatrix[i/3][i%3] = i+1;
+Mat33 myMat33 = Mat33(&myMatrix[0][0]).transpose();
+std::cout << myMat33 << std::endl;
+\endcode
+Converting from a Mat33 to a Matrix is straightforward:
+\code
+Mat33 myMat33(1,2,3, 4,5,6, 7,8,9);
+std::cout << Matrix(myMat33) << std::endl;
+\endcode
+
+ at see Matrix_ for handling of large or variable-sized matrices.
+ at see SymMat, Vec, Row
+**/
+template <int M, int N, class ELT, int CS, int RS> class Mat {
+ typedef ELT E;
+ typedef typename CNT<E>::TNeg ENeg;
+ typedef typename CNT<E>::TWithoutNegator EWithoutNegator;
+ typedef typename CNT<E>::TReal EReal;
+ typedef typename CNT<E>::TImag EImag;
+ typedef typename CNT<E>::TComplex EComplex;
+ typedef typename CNT<E>::THerm EHerm;
+ typedef typename CNT<E>::TPosTrans EPosTrans;
+ typedef typename CNT<E>::TSqHermT ESqHermT;
+ typedef typename CNT<E>::TSqTHerm ESqTHerm;
+
+ typedef typename CNT<E>::TSqrt ESqrt;
+ typedef typename CNT<E>::TAbs EAbs;
+ typedef typename CNT<E>::TStandard EStandard;
+ typedef typename CNT<E>::TInvert EInvert;
+ typedef typename CNT<E>::TNormalize ENormalize;
+
+ typedef typename CNT<E>::Scalar EScalar;
+ typedef typename CNT<E>::ULessScalar EULessScalar;
+ typedef typename CNT<E>::Number ENumber;
+ typedef typename CNT<E>::StdNumber EStdNumber;
+ typedef typename CNT<E>::Precision EPrecision;
+ typedef typename CNT<E>::ScalarNormSq EScalarNormSq;
+
+public:
+ /** Every Composite Numerical Type (CNT) must define these values. **/
+ enum {
+ NRows = M,
+ NCols = N,
+ MinDim = N < M ? N : M,
+ MaxDim = N > M ? N : M,
+ RowSpacing = RS,
+ ColSpacing = CS,
+ NPackedElements = M * N,
+ NActualElements = (N-1)*CS + (M-1)*RS + 1,
+ NActualScalars = CNT<E>::NActualScalars * NActualElements,
+ ImagOffset = NTraits<ENumber>::ImagOffset,
+ RealStrideFactor = 1, // composite types don't change size when
+ // cast from complex to real or imaginary
+ ArgDepth = ((int)CNT<E>::ArgDepth < (int)MAX_RESOLVED_DEPTH
+ ? CNT<E>::ArgDepth + 1
+ : MAX_RESOLVED_DEPTH),
+ IsScalar = 0,
+ IsULessScalar = 0,
+ IsNumber = 0,
+ IsStdNumber = 0,
+ IsPrecision = 0,
+ SignInterpretation = CNT<E>::SignInterpretation
+ };
+
+ typedef Mat<M,N,E,CS,RS> T;
+ typedef Mat<M,N,ENeg,CS,RS> TNeg;
+ typedef Mat<M,N,EWithoutNegator,CS,RS> TWithoutNegator;
+
+ typedef Mat<M,N,EReal,CS*CNT<E>::RealStrideFactor,RS*CNT<E>::RealStrideFactor>
+ TReal;
+ typedef Mat<M,N,EImag,CS*CNT<E>::RealStrideFactor,RS*CNT<E>::RealStrideFactor>
+ TImag;
+ typedef Mat<M,N,EComplex,CS,RS> TComplex;
+ typedef Mat<N,M,EHerm,RS,CS> THerm;
+ typedef Mat<N,M,E,RS,CS> TPosTrans;
+ typedef E TElement;
+ typedef Row<N,E,CS> TRow;
+ typedef Vec<M,E,RS> TCol;
+ typedef Vec<MinDim,E,RS+CS> TDiag;
+
+ // These are the results of calculations, so are returned in new, packed
+ // memory. Be sure to refer to element types here which are also packed.
+ typedef Mat<M,N,ESqrt,M,1> TSqrt; // Note strides are packed
+ typedef Mat<M,N,EAbs,M,1> TAbs; // Note strides are packed
+ typedef Mat<M,N,EStandard,M,1> TStandard;
+ typedef Mat<N,M,EInvert,N,1> TInvert; // like THerm but packed
+ typedef Mat<M,N,ENormalize,M,1> TNormalize;
+
+ typedef SymMat<N,ESqHermT> TSqHermT; // ~Mat*Mat
+ typedef SymMat<M,ESqTHerm> TSqTHerm; // Mat*~Mat
+
+ // Here the elements are copied unchanged but the result matrix
+ // is an ordinary packed, column order matrix.
+ typedef Mat<M,N,E,M,1> TPacked;
+ typedef Mat<M-1,N,E,M-1,1> TDropRow;
+ typedef Mat<M,N-1,E,M,1> TDropCol;
+ typedef Mat<M-1,N-1,E,M-1,1> TDropRowCol;
+ typedef Mat<M+1,N,E,M+1,1> TAppendRow;
+ typedef Mat<M,N+1,E,M,1> TAppendCol;
+ typedef Mat<M+1,N+1,E,M+1,1> TAppendRowCol;
+
+ typedef EScalar Scalar;
+ typedef EULessScalar ULessScalar;
+ typedef ENumber Number;
+ typedef EStdNumber StdNumber;
+ typedef EPrecision Precision;
+ typedef EScalarNormSq ScalarNormSq;
+
+ typedef THerm TransposeType; // TODO
+
+ /** Return the total number of elements M*N contained in this Mat. **/
+ static int size() { return M*N; }
+ /** Return the number of rows in this Mat, echoing the value supplied
+ for the template paramter \a M. **/
+ static int nrow() { return M; }
+ /** Return the number of columns in this Mat, echoing the value supplied
+ for the template paramter \a N. **/
+ static int ncol() { return N; }
+
+ /** Scalar norm square is the sum of squares of all the scalars that
+ comprise the value of this Mat. For Mat objects with composite element
+ types, this is defined recursively as the sum of the scalar norm squares
+ of all the elements, where the scalar norm square of a scalar is just that
+ scalar squared. **/
+ ScalarNormSq scalarNormSqr() const {
+ ScalarNormSq sum(0);
+ for(int j=0;j<N;++j) sum += CNT<TCol>::scalarNormSqr((*this)(j));
+ return sum;
+ }
+
+ /** Elementwise square root; that is, the return value has the same
+ dimensions as this Mat but with each element replaced by whatever it thinks
+ its square root is. **/
+ TSqrt sqrt() const {
+ TSqrt msqrt;
+ for(int j=0;j<N;++j) msqrt(j) = (*this)(j).sqrt();
+ return msqrt;
+ }
+
+ /** Elementwise absolute value; that is, the return value has the same
+ dimensions as this Mat but with each element replaced by whatever it thinks
+ its absolute value is. **/
+ TAbs abs() const {
+ TAbs mabs;
+ for(int j=0;j<N;++j) mabs(j) = (*this)(j).abs();
+ return mabs;
+ }
+
+ TStandard standardize() const {
+ TStandard mstd;
+ for(int j=0;j<N;++j) mstd(j) = (*this)(j).standardize();
+ return mstd;
+ }
+
+ // This gives the resulting matrix type when (m(i,j) op P) is applied to each element.
+ // It is an MxN vector with default column and row spacing, and element types which
+ // are the regular composite result of E op P. Typically P is a scalar type but
+ // it doesn't have to be.
+ template <class P> struct EltResult {
+ typedef Mat<M,N, typename CNT<E>::template Result<P>::Mul, M, 1> Mul;
+ typedef Mat<M,N, typename CNT<E>::template Result<P>::Dvd, M, 1> Dvd;
+ typedef Mat<M,N, typename CNT<E>::template Result<P>::Add, M, 1> Add;
+ typedef Mat<M,N, typename CNT<E>::template Result<P>::Sub, M, 1> Sub;
+ };
+
+ // This is the composite result for m op P where P is some kind of appropriately shaped
+ // non-scalar type.
+ template <class P> struct Result {
+ typedef MulCNTs<M,N,ArgDepth,Mat,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> MulOp;
+ typedef typename MulOp::Type Mul;
+
+ typedef MulCNTsNonConforming<M,N,ArgDepth,Mat,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> MulOpNonConforming;
+ typedef typename MulOpNonConforming::Type MulNon;
+
+ typedef DvdCNTs<M,N,ArgDepth,Mat,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> DvdOp;
+ typedef typename DvdOp::Type Dvd;
+
+ typedef AddCNTs<M,N,ArgDepth,Mat,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> AddOp;
+ typedef typename AddOp::Type Add;
+
+ typedef SubCNTs<M,N,ArgDepth,Mat,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> SubOp;
+ typedef typename SubOp::Type Sub;
+ };
+
+ // Shape-preserving element substitution (always packed)
+ template <class P> struct Substitute {
+ typedef Mat<M,N,P> Type;
+ };
+
+ /** Default construction initializes to NaN when debugging but is left
+ uninitialized otherwise to ensure that there is no overhead. **/
+ Mat(){
+ #ifndef NDEBUG
+ setToNaN();
+ #endif
+ }
+
+ // It's important not to use the default copy constructor or copy
+ // assignment because the compiler doesn't understand that we may
+ // have noncontiguous storage and will try to copy the whole array.
+
+ /** Copy constructor copies only the elements that are present and does
+ not touch any unused memory space between them if they are not packed. **/
+ Mat(const Mat& src) {
+ for (int j=0; j<N; ++j)
+ (*this)(j) = src(j);
+ }
+ /** Copy assignment copies only the elements that are present and does
+ not touch any unused memory space between them if they are not packed.
+ Works correctly even if source and destination are the same object. **/
+ Mat& operator=(const Mat& src) {
+ for (int j=0; j<N; ++j)
+ (*this)(j) = src(j); // no harm if src and 'this' are the same
+ return *this;
+ }
+
+ /** Explicit construction of a Mat from a SymMat (symmetric/Hermitian
+ matrix). Note that a SymMat is a Hermitian matrix when the elements are
+ complex, so in that case the resulting Mat's upper triangle values are
+ complex conjugates of the lower triangle ones. **/
+ explicit Mat(const SymMat<M, ELT>& src) {
+ updDiag() = src.diag();
+ for (int j = 0; j < M; ++j)
+ for (int i = j+1; i < M; ++i) {
+ (*this)(i, j) = src.getEltLower(i, j);
+ (*this)(j, i) = src.getEltUpper(j, i);
+ }
+ }
+
+ /** This provides an \e implicit conversion from a Mat of the same
+ dimensions and element type but with different element spacing.
+ @tparam CSS Column spacing of the source Mat.
+ @tparam RSS %Row spacing of the source Mat. **/
+ template <int CSS, int RSS>
+ Mat(const Mat<M,N,E,CSS,RSS>& src) {
+ for (int j=0; j<N; ++j)
+ (*this)(j) = src(j);
+ }
+
+ /** This provides an \e implicit conversion from a Mat of the same
+ dimensions and \e negated element type, possibly with different element
+ spacing.
+ @tparam CSS Column spacing of the source Mat.
+ @tparam RSS %Row spacing of the source Mat. **/
+ template <int CSS, int RSS>
+ Mat(const Mat<M,N,ENeg,CSS,RSS>& src) {
+ for (int j=0; j<N; ++j)
+ (*this)(j) = src(j);
+ }
+
+ /** Explicit construction of a Mat from a source Mat of the same
+ dimensions and an assignment-compatible element type, with any element
+ spacing allowed.
+ @tparam EE The element type of the source Mat; must be assignment
+ compatible with element type E of this Mat.
+ @tparam CSS Column spacing of the source Mat.
+ @tparam RSS %Row spacing of the source Mat. **/
+ template <class EE, int CSS, int RSS>
+ explicit Mat(const Mat<M,N,EE,CSS,RSS>& mm)
+ { for (int j=0;j<N;++j) (*this)(j) = mm(j);}
+
+ /** Explicit construction from a single element \a e of this Mat's element
+ type E sets all the main diagonal elements to \a e but sets the rest of
+ the elements to zero. **/
+ explicit Mat(const E& e)
+ { for (int j=0;j<N;++j) (*this)(j) = E(0); diag()=e; }
+
+ /** Explicit construction from a single element \a e whose type is
+ negator<E> (abbreviated ENeg here) where E is this Mat's element
+ type sets all the main diagonal elements to \a e but sets the rest of
+ the elements to zero. **/
+ explicit Mat(const ENeg& e)
+ { for (int j=0;j<N;++j) (*this)(j) = E(0); diag()=e; }
+
+ /** Explicit construction from an int value means we convert the int into
+ an object of this Mat's element type E, and then apply the single-element
+ constructor above which sets the Mat to zero except for its main diagonal
+ elements which will all be set to the given value. To convert an int to
+ an element, we first turn it into the appropriate-precision floating point
+ number, and then call E's constructor that takes a single scalar. **/
+ explicit Mat(int i)
+ { new (this) Mat(E(Precision(i))); }
+
+ // A bevy of constructors from individual exact-match elements IN ROW ORDER.
+ Mat(const E& e0,const E& e1)
+ {assert(M*N==2);d[rIx(0)]=e0;d[rIx(1)]=e1;}
+ Mat(const E& e0,const E& e1,const E& e2)
+ {assert(M*N==3);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;}
+ Mat(const E& e0,const E& e1,const E& e2,const E& e3)
+ {assert(M*N==4);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;}
+ Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4)
+ {assert(M*N==5);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;}
+ Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
+ const E& e5)
+ {assert(M*N==6);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
+ d[rIx(5)]=e5;}
+ Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
+ const E& e5,const E& e6)
+ {assert(M*N==7);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
+ d[rIx(5)]=e5;d[rIx(6)]=e6;}
+ Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
+ const E& e5,const E& e6,const E& e7)
+ {assert(M*N==8);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
+ d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;}
+ Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
+ const E& e5,const E& e6,const E& e7,const E& e8)
+ {assert(M*N==9);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
+ d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;}
+ Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
+ const E& e5,const E& e6,const E& e7,const E& e8,const E& e9)
+ {assert(M*N==10);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
+ d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;}
+ Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
+ const E& e5,const E& e6,const E& e7,const E& e8,const E& e9,
+ const E& e10)
+ {assert(M*N==11);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
+ d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;d[rIx(10)]=e10;}
+ Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
+ const E& e5,const E& e6,const E& e7,const E& e8,const E& e9,
+ const E& e10, const E& e11)
+ {assert(M*N==12);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
+ d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;d[rIx(10)]=e10;
+ d[rIx(11)]=e11;}
+ Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
+ const E& e5,const E& e6,const E& e7,const E& e8,const E& e9,
+ const E& e10, const E& e11, const E& e12)
+ {assert(M*N==13);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
+ d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;d[rIx(10)]=e10;
+ d[rIx(11)]=e11;d[rIx(12)]=e12;}
+ Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
+ const E& e5,const E& e6,const E& e7,const E& e8,const E& e9,
+ const E& e10, const E& e11, const E& e12, const E& e13)
+ {assert(M*N==14);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
+ d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;d[rIx(10)]=e10;
+ d[rIx(11)]=e11;d[rIx(12)]=e12;d[rIx(13)]=e13;}
+ Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
+ const E& e5,const E& e6,const E& e7,const E& e8,const E& e9,
+ const E& e10, const E& e11, const E& e12, const E& e13, const E& e14)
+ {assert(M*N==15);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
+ d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;d[rIx(10)]=e10;
+ d[rIx(11)]=e11;d[rIx(12)]=e12;d[rIx(13)]=e13;d[rIx(14)]=e14;}
+ Mat(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,
+ const E& e5,const E& e6,const E& e7,const E& e8,const E& e9,
+ const E& e10, const E& e11, const E& e12, const E& e13, const E& e14,
+ const E& e15)
+ {assert(M*N==16);d[rIx(0)]=e0;d[rIx(1)]=e1;d[rIx(2)]=e2;d[rIx(3)]=e3;d[rIx(4)]=e4;
+ d[rIx(5)]=e5;d[rIx(6)]=e6;d[rIx(7)]=e7;d[rIx(8)]=e8;d[rIx(9)]=e9;d[rIx(10)]=e10;
+ d[rIx(11)]=e11;d[rIx(12)]=e12;d[rIx(13)]=e13;d[rIx(14)]=e14;d[rIx(15)]=e15;}
+
+ // Construction from 1-6 *exact match* Rows
+ explicit Mat(const TRow& r0)
+ { assert(M==1); (*this)[0]=r0; }
+ Mat(const TRow& r0,const TRow& r1)
+ { assert(M==2);(*this)[0]=r0;(*this)[1]=r1; }
+ Mat(const TRow& r0,const TRow& r1,const TRow& r2)
+ { assert(M==3);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2; }
+ Mat(const TRow& r0,const TRow& r1,const TRow& r2,
+ const TRow& r3)
+ { assert(M==4);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2;(*this)[3]=r3; }
+ Mat(const TRow& r0,const TRow& r1,const TRow& r2,
+ const TRow& r3,const TRow& r4)
+ { assert(M==5);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2;
+ (*this)[3]=r3;(*this)[4]=r4; }
+ Mat(const TRow& r0,const TRow& r1,const TRow& r2,
+ const TRow& r3,const TRow& r4,const TRow& r5)
+ { assert(M==6);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2;
+ (*this)[3]=r3;(*this)[4]=r4;(*this)[5]=r5; }
+
+ // Construction from 1-6 *compatible* Rows
+ template <class EE, int SS> explicit Mat(const Row<N,EE,SS>& r0)
+ { assert(M==1); (*this)[0]=r0; }
+ template <class EE, int SS> Mat(const Row<N,EE,SS>& r0,const Row<N,EE,SS>& r1)
+ { assert(M==2);(*this)[0]=r0;(*this)[1]=r1; }
+ template <class EE, int SS>
+ Mat(const Row<N,EE,SS>& r0,const Row<N,EE,SS>& r1,const Row<N,EE,SS>& r2)
+ { assert(M==3);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2; }
+ template <class EE, int SS>
+ Mat(const Row<N,EE,SS>& r0,const Row<N,EE,SS>& r1,const Row<N,EE,SS>& r2,
+ const Row<N,EE,SS>& r3)
+ { assert(M==4);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2;(*this)[3]=r3; }
+ template <class EE, int SS>
+ Mat(const Row<N,EE,SS>& r0,const Row<N,EE,SS>& r1,const Row<N,EE,SS>& r2,
+ const Row<N,EE,SS>& r3,const Row<N,EE,SS>& r4)
+ { assert(M==5);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2;
+ (*this)[3]=r3;(*this)[4]=r4; }
+ template <class EE, int SS>
+ Mat(const Row<N,EE,SS>& r0,const Row<N,EE,SS>& r1,const Row<N,EE,SS>& r2,
+ const Row<N,EE,SS>& r3,const Row<N,EE,SS>& r4,const Row<N,EE,SS>& r5)
+ { assert(M==6);(*this)[0]=r0;(*this)[1]=r1;(*this)[2]=r2;
+ (*this)[3]=r3;(*this)[4]=r4;(*this)[5]=r5; }
+
+
+ // Construction from 1-6 *exact match* Vecs
+ explicit Mat(const TCol& r0)
+ { assert(N==1); (*this)(0)=r0; }
+ Mat(const TCol& r0,const TCol& r1)
+ { assert(N==2);(*this)(0)=r0;(*this)(1)=r1; }
+ Mat(const TCol& r0,const TCol& r1,const TCol& r2)
+ { assert(N==3);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2; }
+ Mat(const TCol& r0,const TCol& r1,const TCol& r2,
+ const TCol& r3)
+ { assert(N==4);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2;(*this)(3)=r3; }
+ Mat(const TCol& r0,const TCol& r1,const TCol& r2,
+ const TCol& r3,const TCol& r4)
+ { assert(N==5);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2;
+ (*this)(3)=r3;(*this)(4)=r4; }
+ Mat(const TCol& r0,const TCol& r1,const TCol& r2,
+ const TCol& r3,const TCol& r4,const TCol& r5)
+ { assert(N==6);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2;
+ (*this)(3)=r3;(*this)(4)=r4;(*this)(5)=r5; }
+
+ // Construction from 1-6 *compatible* Vecs
+ template <class EE, int SS> explicit Mat(const Vec<M,EE,SS>& r0)
+ { assert(N==1); (*this)(0)=r0; }
+ template <class EE, int SS> Mat(const Vec<M,EE,SS>& r0,const Vec<M,EE,SS>& r1)
+ { assert(N==2);(*this)(0)=r0;(*this)(1)=r1; }
+ template <class EE, int SS>
+ Mat(const Vec<M,EE,SS>& r0,const Vec<M,EE,SS>& r1,const Vec<M,EE,SS>& r2)
+ { assert(N==3);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2; }
+ template <class EE, int SS>
+ Mat(const Vec<M,EE,SS>& r0,const Vec<M,EE,SS>& r1,const Vec<M,EE,SS>& r2,
+ const Vec<M,EE,SS>& r3)
+ { assert(N==4);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2;(*this)(3)=r3; }
+ template <class EE, int SS>
+ Mat(const Vec<M,EE,SS>& r0,const Vec<M,EE,SS>& r1,const Vec<M,EE,SS>& r2,
+ const Vec<M,EE,SS>& r3,const Vec<M,EE,SS>& r4)
+ { assert(N==5);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2;
+ (*this)(3)=r3;(*this)(4)=r4; }
+ template <class EE, int SS>
+ Mat(const Vec<M,EE,SS>& r0,const Vec<M,EE,SS>& r1,const Vec<M,EE,SS>& r2,
+ const Vec<M,EE,SS>& r3,const Vec<M,EE,SS>& r4,const Vec<M,EE,SS>& r5)
+ { assert(N==6);(*this)(0)=r0;(*this)(1)=r1;(*this)(2)=r2;
+ (*this)(3)=r3;(*this)(4)=r4;(*this)(5)=r5; }
+
+ // Construction from a pointer to anything assumes we're pointing
+ // at a packed element list of the right length, in row order.
+ template <class EE> explicit Mat(const EE* p)
+ { assert(p); for(int i=0;i<M;++i) (*this)[i]=&p[i*N]; }
+
+ // Assignment works similarly to copy -- if the lengths match,
+ // go element-by-element. Otherwise, zero and then copy to each
+ // diagonal element.
+ template <class EE, int CSS, int RSS> Mat& operator=(const Mat<M,N,EE,CSS,RSS>& mm) {
+ for (int j=0; j<N; ++j) (*this)(j) = mm(j);
+ return *this;
+ }
+
+ template <class EE> Mat& operator=(const EE* p) {
+ assert(p); for(int i=0;i<M;++i) (*this)[i]=&p[i*N];
+ return *this;
+ }
+
+ // Assignment ops
+ template <class EE, int CSS, int RSS> Mat&
+ operator+=(const Mat<M,N,EE,CSS,RSS>& mm) {
+ for (int j=0; j<N; ++j) (*this)(j) += mm(j);
+ return *this;
+ }
+ template <class EE, int CSS, int RSS> Mat&
+ operator+=(const Mat<M,N,negator<EE>,CSS,RSS>& mm) {
+ for (int j=0; j<N; ++j) (*this)(j) -= -(mm(j));
+ return *this;
+ }
+
+ template <class EE, int CSS, int RSS> Mat&
+ operator-=(const Mat<M,N,EE,CSS,RSS>& mm) {
+ for (int j=0; j<N; ++j) (*this)(j) -= mm(j);
+ return *this;
+ }
+ template <class EE, int CSS, int RSS> Mat&
+ operator-=(const Mat<M,N,negator<EE>,CSS,RSS>& mm) {
+ for (int j=0; j<N; ++j) (*this)(j) += -(mm(j));
+ return *this;
+ }
+
+ // In place matrix multiply can only be done when the RHS matrix is square of dimension
+ // N (i.e., number of columns), and the elements are also *= compatible.
+ template <class EE, int CSS, int RSS> Mat&
+ operator*=(const Mat<N,N,EE,CSS,RSS>& mm) {
+ const Mat t(*this);
+ for (int j=0; j<N; ++j)
+ for (int i=0; i<M; ++i)
+ (*this)(i,j) = t[i] * mm(j);
+ return *this;
+ }
+
+ // Conforming binary ops with 'this' on left, producing new packed result.
+ // Cases: m=m+-m, m=m+-sy, m=m*m, m=m*sy, v=m*v
+
+ // m= this + m
+ template <class E2, int CS2, int RS2>
+ typename Result<Mat<M,N,E2,CS2,RS2> >::Add
+ conformingAdd(const Mat<M,N,E2,CS2,RS2>& r) const {
+ typename Result<Mat<M,N,E2,CS2,RS2> >::Add result;
+ for (int j=0;j<N;++j) result(j) = (*this)(j) + r(j);
+ return result;
+ }
+ // m= this - m
+ template <class E2, int CS2, int RS2>
+ typename Result<Mat<M,N,E2,CS2,RS2> >::Sub
+ conformingSubtract(const Mat<M,N,E2,CS2,RS2>& r) const {
+ typename Result<Mat<M,N,E2,CS2,RS2> >::Sub result;
+ for (int j=0;j<N;++j) result(j) = (*this)(j) - r(j);
+ return result;
+ }
+ // m= m - this
+ template <class E2, int CS2, int RS2>
+ typename Mat<M,N,E2,CS2,RS2>::template Result<Mat>::Sub
+ conformingSubtractFromLeft(const Mat<M,N,E2,CS2,RS2>& l) const {
+ return l.conformingSubtract(*this);
+ }
+
+ // m= this .* m
+ template <class E2, int CS2, int RS2>
+ typename EltResult<E2>::Mul
+ elementwiseMultiply(const Mat<M,N,E2,CS2,RS2>& r) const {
+ typename EltResult<E2>::Mul result;
+ for (int j=0;j<N;++j)
+ result(j) = (*this)(j).elementwiseMultiply(r(j));
+ return result;
+ }
+
+ // m= this ./ m
+ template <class E2, int CS2, int RS2>
+ typename EltResult<E2>::Dvd
+ elementwiseDivide(const Mat<M,N,E2,CS2,RS2>& r) const {
+ typename EltResult<E2>::Dvd result;
+ for (int j=0;j<N;++j)
+ result(j) = (*this)(j).elementwiseDivide(r(j));
+ return result;
+ }
+
+ // We always punt to the SymMat since it knows better what to do.
+ // m = this + sym
+ template <class E2, int RS2>
+ typename Result<SymMat<M,E2,RS2> >::Add
+ conformingAdd(const SymMat<M,E2,RS2>& sy) const {
+ assert(M==N);
+ return sy.conformingAdd(*this);
+ }
+ // m= this - sym
+ template <class E2, int RS2>
+ typename Result<SymMat<M,E2,RS2> >::Sub
+ conformingSubtract(const SymMat<M,E2,RS2>& sy) const {
+ assert(M==N);
+ return sy.conformingSubtractFromLeft(*this);
+ }
+ // m= sym - this
+ template <class E2, int RS2>
+ typename SymMat<M,E2,RS2>::template Result<Mat>::Sub
+ conformingSubtractFromLeft(const SymMat<M,E2,RS2>& sy) const {
+ assert(M==N);
+ return sy.conformingSubtract(*this);
+ }
+
+ // m= this * m
+ template <int N2, class E2, int CS2, int RS2>
+ typename Result<Mat<N,N2,E2,CS2,RS2> >::Mul
+ conformingMultiply(const Mat<N,N2,E2,CS2,RS2>& m) const {
+ typename Result<Mat<N,N2,E2,CS2,RS2> >::Mul result;
+ for (int j=0;j<N2;++j)
+ for (int i=0;i<M;++i)
+ result(i,j) = (*this)[i].conformingMultiply(m(j)); // i.e., dot()
+ return result;
+ }
+ // m= m * this
+ template <int M2, class E2, int CS2, int RS2>
+ typename Mat<M2,M,E2,CS2,RS2>::template Result<Mat>::Mul
+ conformingMultiplyFromLeft(const Mat<M2,M,E2,CS2,RS2>& m) const {
+ return m.conformingMultiply(*this);
+ }
+
+ // m= this / m = this * m.invert()
+ template <int M2, class E2, int CS2, int RS2>
+ typename Result<Mat<M2,N,E2,CS2,RS2> >::Dvd
+ conformingDivide(const Mat<M2,N,E2,CS2,RS2>& m) const {
+ return conformingMultiply(m.invert());
+ }
+ // m= m / this = m * this.invert()
+ template <int M2, class E2, int CS2, int RS2>
+ typename Mat<M2,N,E2,CS2,RS2>::template Result<Mat>::Dvd
+ conformingDivideFromLeft(const Mat<M2,N,E2,CS2,RS2>& m) const {
+ return m.conformingMultiply((*this).invert());
+ }
+
+ const TRow& operator[](int i) const { return row(i); }
+ TRow& operator[](int i) { return row(i); }
+ const TCol& operator()(int j) const { return col(j); }
+ TCol& operator()(int j) { return col(j); }
+
+ const E& operator()(int i,int j) const { return elt(i,j); }
+ E& operator()(int i,int j) { return elt(i,j); }
+
+ // This is the scalar Frobenius norm.
+ ScalarNormSq normSqr() const { return scalarNormSqr(); }
+ typename CNT<ScalarNormSq>::TSqrt
+ norm() const { return CNT<ScalarNormSq>::sqrt(scalarNormSqr()); }
+
+ // There is no conventional meaning for normalize() applied to a matrix. We
+ // choose to define it as follows:
+ // If the elements of this Mat are scalars, the result is what you get by
+ // dividing each element by the Frobenius norm() calculated above. If the elements are
+ // *not* scalars, then the elements are *separately* normalized. That means
+ // you will get a different answer from Mat<2,2,Mat33>::normalize() than you
+ // would from a Mat<6,6>::normalize() containing the same scalars.
+ //
+ // Normalize returns a matrix of the same dimension but in new, packed storage
+ // and with a return type that does not include negator<> even if the original
+ // Mat<> does, because we can eliminate the negation here almost for free.
+ // But we can't standardize (change conjugate to complex) for free, so we'll retain
+ // conjugates if there are any.
+ TNormalize normalize() const {
+ if (CNT<E>::IsScalar) {
+ return castAwayNegatorIfAny() / (SignInterpretation*norm());
+ } else {
+ TNormalize elementwiseNormalized;
+ // punt to the column Vec to deal with the elements
+ for (int j=0; j<N; ++j)
+ elementwiseNormalized(j) = (*this)(j).normalize();
+ return elementwiseNormalized;
+ }
+ }
+
+ // Default inversion. Assume full rank if square, otherwise return
+ // pseudoinverse. (Mostly TODO)
+ TInvert invert() const;
+
+ const Mat& operator+() const { return *this; }
+ const TNeg& operator-() const { return negate(); }
+ TNeg& operator-() { return updNegate(); }
+ const THerm& operator~() const { return transpose(); }
+ THerm& operator~() { return updTranspose(); }
+
+ const TNeg& negate() const { return *reinterpret_cast<const TNeg*>(this); }
+ TNeg& updNegate() { return *reinterpret_cast<TNeg*>(this); }
+
+ const THerm& transpose() const { return *reinterpret_cast<const THerm*>(this); }
+ THerm& updTranspose() { return *reinterpret_cast<THerm*>(this); }
+
+ const TPosTrans& positionalTranspose() const
+ { return *reinterpret_cast<const TPosTrans*>(this); }
+ TPosTrans& updPositionalTranspose()
+ { return *reinterpret_cast<TPosTrans*>(this); }
+
+ // If the underlying scalars are complex or conjugate, we can return a
+ // reference to the real part just by recasting the data to a matrix of
+ // the same dimensions but containing real elements, with the scalar
+ // spacing doubled.
+ const TReal& real() const { return *reinterpret_cast<const TReal*>(this); }
+ TReal& real() { return *reinterpret_cast< TReal*>(this); }
+
+ // Getting the imaginary part is almost the same as real, but we have
+ // to shift the starting address of the returned object by 1 real-size
+ // ("precision") scalar so that the first element is the imaginary part
+ // of the original first element.
+ // TODO: should blow up or return a reference to a zero matrix if called
+ // on a real object.
+ // Had to contort these routines to get them through VC++ 7.net
+ const TImag& imag() const {
+ const int offs = ImagOffset;
+ const Precision* p = reinterpret_cast<const Precision*>(this);
+ return *reinterpret_cast<const TImag*>(p+offs);
+ }
+ TImag& imag() {
+ const int offs = ImagOffset;
+ Precision* p = reinterpret_cast<Precision*>(this);
+ return *reinterpret_cast<TImag*>(p+offs);
+ }
+
+ const TWithoutNegator& castAwayNegatorIfAny() const {return *reinterpret_cast<const TWithoutNegator*>(this);}
+ TWithoutNegator& updCastAwayNegatorIfAny() {return *reinterpret_cast<TWithoutNegator*>(this);}
+
+ const TRow& row(int i) const {
+ SimTK_INDEXCHECK(i,M, "Mat::row[i]");
+ return *reinterpret_cast<const TRow*>(&d[i*RS]);
+ }
+ TRow& row(int i) {
+ SimTK_INDEXCHECK(i,M, "Mat::row[i]");
+ return *reinterpret_cast<TRow*>(&d[i*RS]);
+ }
+
+ const TCol& col(int j) const {
+ SimTK_INDEXCHECK(j,N, "Mat::col(j)");
+ return *reinterpret_cast<const TCol*>(&d[j*CS]);
+ }
+ TCol& col(int j) {
+ SimTK_INDEXCHECK(j,N, "Mat::col(j)");
+ return *reinterpret_cast<TCol*>(&d[j*CS]);
+ }
+
+ const E& elt(int i, int j) const {
+ SimTK_INDEXCHECK(i,M, "Mat::elt(i,j)");
+ SimTK_INDEXCHECK(j,N, "Mat::elt(i,j)");
+ return d[i*RS+j*CS];
+ }
+ E& elt(int i, int j) {
+ SimTK_INDEXCHECK(i,M, "Mat::elt(i,j)");
+ SimTK_INDEXCHECK(j,N, "Mat::elt(i,j)");
+ return d[i*RS+j*CS];
+ }
+
+ /// Select main diagonal (of largest leading square if rectangular) and
+ /// return it as a read-only view (as a Vec) of the diagonal elements
+ /// of this Mat.
+ const TDiag& diag() const { return *reinterpret_cast<const TDiag*>(d); }
+ /// Select main diagonal (of largest leading square if rectangular) and
+ /// return it as a writable view (as a Vec) of the diagonal elements
+ /// of this Mat.
+ TDiag& updDiag() { return *reinterpret_cast<TDiag*>(d); }
+ /// This non-const version of diag() is an alternate name for updDiag()
+ /// available for historical reasons.
+ TDiag& diag() { return *reinterpret_cast<TDiag*>(d); }
+
+ EStandard trace() const {return diag().sum();}
+
+ // These are elementwise binary operators, (this op ee) by default but (ee op this) if
+ // 'FromLeft' appears in the name. The result is a packed Mat<M,N> but the element type
+ // may change. These are mostly used to implement global operators.
+ // We call these "scalar" operators but actually the "scalar" can be a composite type.
+
+ //TODO: consider converting 'e' to Standard Numbers as precalculation and changing
+ // return type appropriately.
+ template <class EE> Mat<M,N, typename CNT<E>::template Result<EE>::Mul>
+ scalarMultiply(const EE& e) const {
+ Mat<M,N, typename CNT<E>::template Result<EE>::Mul> result;
+ for (int j=0; j<N; ++j) result(j) = (*this)(j).scalarMultiply(e);
+ return result;
+ }
+ template <class EE> Mat<M,N, typename CNT<EE>::template Result<E>::Mul>
+ scalarMultiplyFromLeft(const EE& e) const {
+ Mat<M,N, typename CNT<EE>::template Result<E>::Mul> result;
+ for (int j=0; j<N; ++j) result(j) = (*this)(j).scalarMultiplyFromLeft(e);
+ return result;
+ }
+
+ // TODO: should precalculate and store 1/e, while converting to Standard Numbers. Note
+ // that return type should change appropriately.
+ template <class EE> Mat<M,N, typename CNT<E>::template Result<EE>::Dvd>
+ scalarDivide(const EE& e) const {
+ Mat<M,N, typename CNT<E>::template Result<EE>::Dvd> result;
+ for (int j=0; j<N; ++j) result(j) = (*this)(j).scalarDivide(e);
+ return result;
+ }
+ template <class EE> Mat<M,N, typename CNT<EE>::template Result<E>::Dvd>
+ scalarDivideFromLeft(const EE& e) const {
+ Mat<M,N, typename CNT<EE>::template Result<E>::Dvd> result;
+ for (int j=0; j<N; ++j) result(j) = (*this)(j).scalarDivideFromLeft(e);
+ return result;
+ }
+
+ // Additive operators for scalars operate only on the diagonal.
+ template <class EE> Mat<M,N, typename CNT<E>::template Result<EE>::Add>
+ scalarAdd(const EE& e) const {
+ Mat<M,N, typename CNT<E>::template Result<EE>::Add> result(*this);
+ result.diag() += e;
+ return result;
+ }
+ // Add is commutative, so no 'FromLeft'.
+
+ template <class EE> Mat<M,N, typename CNT<E>::template Result<EE>::Sub>
+ scalarSubtract(const EE& e) const {
+ Mat<M,N, typename CNT<E>::template Result<EE>::Sub> result(*this);
+ result.diag() -= e;
+ return result;
+ }
+ // Should probably do something clever with negation here (s - m)
+ template <class EE> Mat<M,N, typename CNT<EE>::template Result<E>::Sub>
+ scalarSubtractFromLeft(const EE& e) const {
+ Mat<M,N, typename CNT<EE>::template Result<E>::Sub> result(-(*this));
+ result.diag() += e; // yes, add
+ return result;
+ }
+
+ // Generic assignments for any element type not listed explicitly, including scalars.
+ // These are done repeatedly for each element and only work if the operation can
+ // be performed leaving the original element type.
+ template <class EE> Mat& operator =(const EE& e) {return scalarEq(e);}
+ template <class EE> Mat& operator+=(const EE& e) {return scalarPlusEq(e);}
+ template <class EE> Mat& operator-=(const EE& e) {return scalarMinusEq(e);}
+ template <class EE> Mat& operator*=(const EE& e) {return scalarTimesEq(e);}
+ template <class EE> Mat& operator/=(const EE& e) {return scalarDivideEq(e);}
+
+ // Generalized scalar assignment & computed assignment methods. These will work
+ // for any assignment-compatible element, not just scalars.
+ template <class EE> Mat& scalarEq(const EE& ee)
+ { for(int j=0; j<N; ++j) (*this)(j).scalarEq(EE(0));
+ diag().scalarEq(ee);
+ return *this; }
+
+ template <class EE> Mat& scalarPlusEq(const EE& ee)
+ { diag().scalarPlusEq(ee); return *this; }
+
+ template <class EE> Mat& scalarMinusEq(const EE& ee)
+ { diag().scalarMinusEq(ee); return *this; }
+ // m = s - m; negate m, then add s
+ template <class EE> Mat& scalarMinusEqFromLeft(const EE& ee)
+ { scalarTimesEq(E(-1)); diag().scalarAdd(ee); return *this; }
+
+ template <class EE> Mat& scalarTimesEq(const EE& ee)
+ { for(int j=0; j<N; ++j) (*this)(j).scalarTimesEq(ee); return *this; }
+ template <class EE> Mat& scalarTimesEqFromLeft(const EE& ee)
+ { for(int j=0; j<N; ++j) (*this)(j).scalarTimesEqFromLeft(ee); return *this; }
+
+ template <class EE> Mat& scalarDivideEq(const EE& ee)
+ { for(int j=0; j<N; ++j) (*this)(j).scalarDivideEq(ee); return *this; }
+ template <class EE> Mat& scalarDivideEqFromLeft(const EE& ee)
+ { for(int j=0; j<N; ++j) (*this)(j).scalarDivideEqFromLeft(ee); return *this; }
+
+ void setToNaN() {
+ for (int j=0; j<N; ++j)
+ (*this)(j).setToNaN();
+ }
+
+ void setToZero() {
+ for (int j=0; j<N; ++j)
+ (*this)(j).setToZero();
+ }
+
+ // Extract a sub-Mat with size known at compile time. These have to be
+ // called with explicit template arguments, e.g. getSubMat<3,4>(i,j).
+
+ template <int MM, int NN> struct SubMat {
+ typedef Mat<MM,NN,ELT,CS,RS> Type;
+ };
+
+ template <int MM, int NN>
+ const typename SubMat<MM,NN>::Type& getSubMat(int i, int j) const {
+ assert(0 <= i && i + MM <= M);
+ assert(0 <= j && j + NN <= N);
+ return SubMat<MM,NN>::Type::getAs(&(*this)(i,j));
+ }
+ template <int MM, int NN>
+ typename SubMat<MM,NN>::Type& updSubMat(int i, int j) {
+ assert(0 <= i && i + MM <= M);
+ assert(0 <= j && j + NN <= N);
+ return SubMat<MM,NN>::Type::updAs(&(*this)(i,j));
+ }
+ template <int MM, int NN>
+ void setSubMat(int i, int j, const typename SubMat<MM,NN>::Type& value) {
+ assert(0 <= i && i + MM <= M);
+ assert(0 <= j && j + NN <= N);
+ SubMat<MM,NN>::Type::updAs(&(*this)(i,j)) = value;
+ }
+
+ /// Return a matrix one row smaller than this one by dropping row
+ /// i. The result is packed but has same element type as this one.
+ TDropRow dropRow(int i) const {
+ assert(0 <= i && i < M);
+ TDropRow out;
+ for (int r=0, nxt=0; r<M-1; ++r, ++nxt) {
+ if (nxt==i) ++nxt; // skip the loser
+ out[r] = (*this)[nxt];
+ }
+ return out;
+ }
+
+ /// Return a matrix one column smaller than this one by dropping column
+ /// j. The result is packed but has same element type as this one.
+ TDropCol dropCol(int j) const {
+ assert(0 <= j && j < N);
+ TDropCol out;
+ for (int c=0, nxt=0; c<N-1; ++c, ++nxt) {
+ if (nxt==j) ++nxt; // skip the loser
+ out(c) = (*this)(nxt);
+ }
+ return out;
+ }
+
+ /// Return a matrix one row and one column smaller than this one by
+ /// dropping row i and column j. The result is packed but has same
+ /// element type as this one.
+ TDropRowCol dropRowCol(int i, int j) const {
+ assert(0 <= i && i < M);
+ assert(0 <= j && j < N);
+ TDropRowCol out;
+ for (int c=0, nxtc=0; c<N-1; ++c, ++nxtc) {
+ if (nxtc==j) ++nxtc;
+ for (int r=0, nxtr=0; r<M-1; ++r, ++nxtr) {
+ if (nxtr==i) ++nxtr;
+ out(r,c) = (*this)(nxtr,nxtc);
+ }
+ }
+ return out;
+ }
+
+ /// Return a matrix one row larger than this one by adding a row
+ /// to the end. The result is packed but has same element type as
+ /// this one. Works for any assignment compatible row.
+ template <class EE, int SS>
+ TAppendRow appendRow(const Row<N,EE,SS>& row) const {
+ TAppendRow out;
+ out.template updSubMat<M,N>(0,0) = (*this);
+ out[M] = row;
+ return out;
+ }
+
+ /// Return a matrix one column larger than this one by adding a column
+ /// to the end. The result is packed but has same element type as
+ /// this one. Works for any assignment compatible column.
+ template <class EE, int SS>
+ TAppendCol appendCol(const Vec<M,EE,SS>& col) const {
+ TAppendCol out;
+ out.template updSubMat<M,N>(0,0) = (*this);
+ out(N) = col;
+ return out;
+ }
+
+ /// Return a matrix one row and one column larger than this one by
+ /// adding a row to the bottom and a column to the right. The final
+ /// element of the row is ignored; that value is taken from the
+ /// final element of the column instead. The result is packed
+ /// but has same element type as this one. Works for any assignment
+ /// compatible row and column.
+ template <class ER, int SR, class EC, int SC>
+ TAppendRowCol appendRowCol(const Row<N+1,ER,SR>& row,
+ const Vec<M+1,EC,SC>& col) const
+ {
+ TAppendRowCol out;
+ out.template updSubMat<M,N>(0,0) = (*this);
+ out[M].template updSubRow<N>(0) =
+ row.template getSubRow<N>(0); // ignore last element
+ out(N) = col;
+ return out;
+ }
+
+ /// Return a matrix one row larger than this one by inserting a row
+ /// *before* row i. The result is packed but has same element type as
+ /// this one. Works for any assignment compatible row. The index
+ /// can be one greater than normally allowed in which case the row
+ /// is appended.
+ template <class EE, int SS>
+ TAppendRow insertRow(int i, const Row<N,EE,SS>& row) const {
+ assert(0 <= i && i <= M);
+ if (i==M) return appendRow(row);
+ TAppendRow out;
+ for (int r=0, nxt=0; r<M; ++r, ++nxt) {
+ if (nxt==i) out[nxt++] = row;
+ out[nxt] = (*this)[r];
+ }
+ return out;
+ }
+
+ /// Return a matrix one column larger than this one by inserting a column
+ /// *before* column j. The result is packed but has same element type as
+ /// this one. Works for any assignment compatible column. The index
+ /// can be one greater than normally allowed in which case the column
+ /// is appended.
+ template <class EE, int SS>
+ TAppendCol insertCol(int j, const Vec<M,EE,SS>& col) const {
+ assert(0 <= j && j <= N);
+ if (j==N) return appendCol(col);
+ TAppendCol out;
+ for (int c=0, nxt=0; c<N; ++c, ++nxt) {
+ if (nxt==j) out(nxt++) = col;
+ out(nxt) = (*this)(c);
+ }
+ return out;
+ }
+
+ /// Return a matrix one row and one column larger than this one by
+ /// inserting a row *before* row i and a column *before* column j.
+ /// The intersecting element of the row is ignored; that element is
+ /// taken from the column. The result is packed but has same element
+ /// type as this one. Works for any assignment compatible row and
+ /// column. The indices can be one greater than normally allowed
+ /// in which case the row or column is appended.
+ template <class ER, int SR, class EC, int SC>
+ TAppendRowCol insertRowCol(int i, int j, const Row<N+1,ER,SR>& row,
+ const Vec<M+1,EC,SC>& col) const {
+ assert(0 <= i && i <= M);
+ assert(0 <= j && j <= N);
+ TAppendRowCol out;
+ for (int c=0, nxtc=0; c<N; ++c, ++nxtc) {
+ if (nxtc==j) ++nxtc; // leave room
+ for (int r=0, nxtr=0; r<M; ++r, ++nxtr) {
+ if (nxtr==i) ++nxtr;
+ out(nxtr,nxtc) = (*this)(r,c);
+ }
+ }
+ out[i] = row;
+ out(j) = col; // overwrites row's j'th element
+ return out;
+ }
+
+ // These assume we are given a pointer to d[0] of a Mat<M,N,E,CS,RS> like this one.
+ static const Mat& getAs(const ELT* p) {return *reinterpret_cast<const Mat*>(p);}
+ static Mat& updAs(ELT* p) {return *reinterpret_cast<Mat*>(p);}
+
+ // Note packed spacing
+ static Mat<M,N,ELT,M,1> getNaN() {
+ Mat<M,N,ELT,M,1> m;
+ m.setToNaN();
+ return m;
+ }
+
+ /// Return true if any element of this Mat contains a NaN anywhere.
+ bool isNaN() const {
+ for (int j=0; j<N; ++j)
+ if (this->col(j).isNaN())
+ return true;
+ return false;
+ }
+
+ /// Return true if any element of this Mat contains a +Inf
+ /// or -Inf somewhere but no element contains a NaN anywhere.
+ bool isInf() const {
+ bool seenInf = false;
+ for (int j=0; j<N; ++j) {
+ if (!this->col(j).isFinite()) {
+ if (!this->col(j).isInf())
+ return false; // something bad was found
+ seenInf = true;
+ }
+ }
+ return seenInf;
+ }
+
+ /// Return true if no element contains an Infinity or a NaN.
+ bool isFinite() const {
+ for (int j=0; j<N; ++j)
+ if (!this->col(j).isFinite())
+ return false;
+ return true;
+ }
+
+ /// For approximate comparisions, the default tolerance to use for a matrix is
+ /// its shortest dimension times its elements' default tolerance.
+ static double getDefaultTolerance() {return MinDim*CNT<ELT>::getDefaultTolerance();}
+
+ /// %Test whether this matrix is numerically equal to some other matrix with
+ /// the same shape, using a specified tolerance.
+ template <class E2, int CS2, int RS2>
+ bool isNumericallyEqual(const Mat<M,N,E2,CS2,RS2>& m, double tol) const {
+ for (int j=0; j < N; ++j)
+ if (!(*this)(j).isNumericallyEqual(m(j), tol))
+ return false;
+ return true;
+ }
+
+ /// %Test whether this matrix is numerically equal to some other matrix with
+ /// the same shape, using a default tolerance which is the looser of the
+ /// default tolerances of the two objects being compared.
+ template <class E2, int CS2, int RS2>
+ bool isNumericallyEqual(const Mat<M,N,E2,CS2,RS2>& m) const {
+ const double tol = std::max(getDefaultTolerance(),m.getDefaultTolerance());
+ return isNumericallyEqual(m, tol);
+ }
+
+ /// %Test whether this is numerically a "scalar" matrix, meaning that it is
+ /// a diagonal matrix in which each diagonal element is numerically equal to
+ /// the same scalar, using either a specified tolerance or the matrix's
+ /// default tolerance (which is always the same or looser than the default
+ /// tolerance for one of its elements).
+ bool isNumericallyEqual
+ (const ELT& e,
+ double tol = getDefaultTolerance()) const
+ {
+ for (int i=0; i<M; ++i)
+ for (int j=0; j<N; ++j) {
+ if (i==j) {
+ if (!CNT<ELT>::isNumericallyEqual((*this)(i,i), e, tol))
+ return false;
+ } else {
+ // off-diagonals must be zero
+ if (!CNT<ELT>::isNumericallyEqual((*this)(i,j), ELT(0), tol))
+ return false;
+ }
+ }
+ return true;
+ }
+
+ /// A Matrix is symmetric (actually Hermitian) if it is square and each
+ /// element (i,j) is the Hermitian transpose of element (j,i). Here we
+ /// are testing for numerical symmetry, meaning that the symmetry condition
+ /// is satisified to within a tolerance (supplied or default). This is
+ /// a relatively expensive test since all elements must be examined but
+ /// can be very useful in Debug mode to check assumptions.
+ /// @see isExactlySymmetric() for a rarely-used exact equality test
+ bool isNumericallySymmetric(double tol = getDefaultTolerance()) const {
+ if (M != N) return false; // handled at compile time
+ for (int j=0; j<M; ++j)
+ for (int i=j; i<M; ++i)
+ if (!CNT<ELT>::isNumericallyEqual(elt(j,i), CNT<ELT>::transpose(elt(i,j)), tol))
+ return false;
+ return true;
+ }
+
+ /// A Matrix is symmetric (actually Hermitian) if it is square and each
+ /// element (i,j) is the Hermitian (conjugate) transpose of element (j,i). This
+ /// method tests for exact (bitwise) equality and is too stringent for most
+ /// purposes; don't use it unless you know that the corresponding elements
+ /// should be bitwise conjugates, typically because you put them there directly.
+ /// @see isNumericallySymmetric() for a more useful method
+ bool isExactlySymmetric() const {
+ if (M != N) return false; // handled at compile time
+ for (int j=0; j<M; ++j)
+ for (int i=j; i<M; ++i)
+ if (elt(j,i) != CNT<ELT>::transpose(elt(i,j)))
+ return false;
+ return true;
+ }
+
+ /// Returns a row vector (Row) containing the column sums of this matrix.
+ TRow colSum() const {
+ TRow temp;
+ for (int j = 0; j < N; ++j)
+ temp[j] = col(j).sum();
+ return temp;
+ }
+ /// This is an alternate name for colSum(); behaves like the Matlab
+ /// function of the same name.
+ TRow sum() const {return colSum();}
+
+ /// Returns a column vector (Vec) containing the row sums of this matrix.
+ TCol rowSum() const {
+ TCol temp;
+ for (int i = 0; i < M; ++i)
+ temp[i] = row(i).sum();
+ return temp;
+ }
+
+ // Functions to be used for Scripting in MATLAB and languages that do not support operator overloading
+ /** toString() returns a string representation of the Mat. Please refer to operator<< for details. **/
+ std::string toString() const {
+ std::stringstream stream;
+ stream << (*this) ;
+ return stream.str();
+ }
+ /** Variant of indexing operator that's scripting friendly to get entry (i, j) **/
+ const ELT& get(int i,int j) const { return elt(i,j); }
+ /** Variant of indexing operator that's scripting friendly to set entry (i, j) **/
+ void set(int i,int j, const ELT& value) { elt(i,j)=value; }
+
+private:
+ E d[NActualElements];
+
+ // This permits running through d as though it were stored
+ // in row order with packed elements. Pass in the k'th value
+ // of the row ordering and get back the index into d where
+ // that element is stored.
+ int rIx(int k) const {
+ const int row = k / N;
+ const int col = k % N; // that's modulus, not cross product!
+ return row*RS + col*CS;
+ }
+};
+
+//////////////////////////////////////////////
+// Global operators involving two matrices. //
+// m+m, m-m, m*m, m==m, m!=m //
+//////////////////////////////////////////////
+
+template <int M, int N, class EL, int CSL, int RSL, class ER, int CSR, int RSR> inline
+typename Mat<M,N,EL,CSL,RSL>::template Result<Mat<M,N,ER,CSR,RSR> >::Add
+operator+(const Mat<M,N,EL,CSL,RSL>& l, const Mat<M,N,ER,CSR,RSR>& r) {
+ return Mat<M,N,EL,CSL,RSL>::template Result<Mat<M,N,ER,CSR,RSR> >
+ ::AddOp::perform(l,r);
+}
+
+template <int M, int N, class EL, int CSL, int RSL, class ER, int CSR, int RSR> inline
+typename Mat<M,N,EL,CSL,RSL>::template Result<Mat<M,N,ER,CSR,RSR> >::Sub
+operator-(const Mat<M,N,EL,CSL,RSL>& l, const Mat<M,N,ER,CSR,RSR>& r) {
+ return Mat<M,N,EL,CSL,RSL>::template Result<Mat<M,N,ER,CSR,RSR> >
+ ::SubOp::perform(l,r);
+}
+
+// Matrix multiply of an MxN by NxP to produce a packed MxP.
+template <int M, int N, class EL, int CSL, int RSL, int P, class ER, int CSR, int RSR> inline
+typename Mat<M,N,EL,CSL,RSL>::template Result<Mat<N,P,ER,CSR,RSR> >::Mul
+operator*(const Mat<M,N,EL,CSL,RSL>& l, const Mat<N,P,ER,CSR,RSR>& r) {
+ return Mat<M,N,EL,CSL,RSL>::template Result<Mat<N,P,ER,CSR,RSR> >
+ ::MulOp::perform(l,r);
+}
+
+// Non-conforming matrix multiply of an MxN by MMxNN; will be a scalar multiply if one
+// has scalar elements and the other has composite elements.
+template <int M, int N, class EL, int CSL, int RSL, int MM, int NN, class ER, int CSR, int RSR> inline
+typename Mat<M,N,EL,CSL,RSL>::template Result<Mat<MM,NN,ER,CSR,RSR> >::MulNon
+operator*(const Mat<M,N,EL,CSL,RSL>& l, const Mat<MM,NN,ER,CSR,RSR>& r) {
+ return Mat<M,N,EL,CSL,RSL>::template Result<Mat<MM,NN,ER,CSR,RSR> >
+ ::MulOpNonConforming::perform(l,r);
+}
+
+template <int M, int N, class EL, int CSL, int RSL, class ER, int CSR, int RSR> inline
+bool operator==(const Mat<M,N,EL,CSL,RSL>& l, const Mat<M,N,ER,CSR,RSR>& r) {
+ for (int j=0; j<N; ++j)
+ if (l(j) != r(j)) return false;
+ return true;
+}
+template <int M, int N, class EL, int CSL, int RSL, class ER, int CSR, int RSR> inline
+bool operator!=(const Mat<M,N,EL,CSL,RSL>& l, const Mat<M,N,ER,CSR,RSR>& r) {
+ return !(l==r);
+}
+
+
+///////////////////////////////////////////////////////
+// Global operators involving a matrix and a scalar. //
+///////////////////////////////////////////////////////
+
+// SCALAR MULTIPLY
+
+// m = m*real, real*m
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<float>::Mul
+operator*(const Mat<M,N,E,CS,RS>& l, const float& r)
+ { return Mat<M,N,E,CS,RS>::template Result<float>::MulOp::perform(l,r); }
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<float>::Mul
+operator*(const float& l, const Mat<M,N,E,CS,RS>& r) {return r*l;}
+
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<double>::Mul
+operator*(const Mat<M,N,E,CS,RS>& l, const double& r)
+ { return Mat<M,N,E,CS,RS>::template Result<double>::MulOp::perform(l,r); }
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<double>::Mul
+operator*(const double& l, const Mat<M,N,E,CS,RS>& r) {return r*l;}
+
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<long double>::Mul
+operator*(const Mat<M,N,E,CS,RS>& l, const long double& r)
+ { return Mat<M,N,E,CS,RS>::template Result<long double>::MulOp::perform(l,r); }
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<long double>::Mul
+operator*(const long double& l, const Mat<M,N,E,CS,RS>& r) {return r*l;}
+
+// m = m*int, int*m -- just convert int to m's precision float
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<typename CNT<E>::Precision>::Mul
+operator*(const Mat<M,N,E,CS,RS>& l, int r) {return l * (typename CNT<E>::Precision)r;}
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<typename CNT<E>::Precision>::Mul
+operator*(int l, const Mat<M,N,E,CS,RS>& r) {return r * (typename CNT<E>::Precision)l;}
+
+// Complex, conjugate, and negator are all easy to templatize.
+
+// m = m*complex, complex*m
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Mul
+operator*(const Mat<M,N,E,CS,RS>& l, const std::complex<R>& r)
+ { return Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::MulOp::perform(l,r); }
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Mul
+operator*(const std::complex<R>& l, const Mat<M,N,E,CS,RS>& r) {return r*l;}
+
+// m = m*conjugate, conjugate*m (convert conjugate->complex)
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Mul
+operator*(const Mat<M,N,E,CS,RS>& l, const conjugate<R>& r) {return l*(std::complex<R>)r;}
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Mul
+operator*(const conjugate<R>& l, const Mat<M,N,E,CS,RS>& r) {return r*(std::complex<R>)l;}
+
+// m = m*negator, negator*m: convert negator to standard number
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename Mat<M,N,E,CS,RS>::template Result<typename negator<R>::StdNumber>::Mul
+operator*(const Mat<M,N,E,CS,RS>& l, const negator<R>& r) {return l * (typename negator<R>::StdNumber)(R)r;}
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename Mat<M,N,E,CS,RS>::template Result<typename negator<R>::StdNumber>::Mul
+operator*(const negator<R>& l, const Mat<M,N,E,CS,RS>& r) {return r * (typename negator<R>::StdNumber)(R)l;}
+
+
+// SCALAR DIVIDE. This is a scalar operation when the scalar is on the right,
+// but when it is on the left it means scalar * pseudoInverse(mat),
+// which is a matrix whose type is like the matrix's Hermitian transpose.
+// TODO: for now it is just going to call mat.invert() which will fail on
+// singular matrices.
+
+// m = m/real, real/m
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<float>::Dvd
+operator/(const Mat<M,N,E,CS,RS>& l, const float& r)
+{ return Mat<M,N,E,CS,RS>::template Result<float>::DvdOp::perform(l,r); }
+
+template <int M, int N, class E, int CS, int RS> inline
+typename CNT<float>::template Result<Mat<M,N,E,CS,RS> >::Dvd
+operator/(const float& l, const Mat<M,N,E,CS,RS>& r)
+{ return l * r.invert(); }
+
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<double>::Dvd
+operator/(const Mat<M,N,E,CS,RS>& l, const double& r)
+{ return Mat<M,N,E,CS,RS>::template Result<double>::DvdOp::perform(l,r); }
+
+template <int M, int N, class E, int CS, int RS> inline
+typename CNT<double>::template Result<Mat<M,N,E,CS,RS> >::Dvd
+operator/(const double& l, const Mat<M,N,E,CS,RS>& r)
+{ return l * r.invert(); }
+
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<long double>::Dvd
+operator/(const Mat<M,N,E,CS,RS>& l, const long double& r)
+{ return Mat<M,N,E,CS,RS>::template Result<long double>::DvdOp::perform(l,r); }
+
+template <int M, int N, class E, int CS, int RS> inline
+typename CNT<long double>::template Result<Mat<M,N,E,CS,RS> >::Dvd
+operator/(const long double& l, const Mat<M,N,E,CS,RS>& r)
+{ return l * r.invert(); }
+
+// m = m/int, int/m -- just convert int to m's precision float
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<typename CNT<E>::Precision>::Dvd
+operator/(const Mat<M,N,E,CS,RS>& l, int r)
+{ return l / (typename CNT<E>::Precision)r; }
+
+template <int M, int N, class E, int CS, int RS> inline
+typename CNT<typename CNT<E>::Precision>::template Result<Mat<M,N,E,CS,RS> >::Dvd
+operator/(int l, const Mat<M,N,E,CS,RS>& r)
+{ return (typename CNT<E>::Precision)l / r; }
+
+
+// Complex, conjugate, and negator are all easy to templatize.
+
+// m = m/complex, complex/m
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Dvd
+operator/(const Mat<M,N,E,CS,RS>& l, const std::complex<R>& r)
+ { return Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::DvdOp::perform(l,r); }
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename CNT<std::complex<R> >::template Result<Mat<M,N,E,CS,RS> >::Dvd
+operator/(const std::complex<R>& l, const Mat<M,N,E,CS,RS>& r)
+ { return CNT<std::complex<R> >::template Result<Mat<M,N,E,CS,RS> >::DvdOp::perform(l,r); }
+
+// m = m/conjugate, conjugate/m (convert conjugate->complex)
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Dvd
+operator/(const Mat<M,N,E,CS,RS>& l, const conjugate<R>& r) {return l/(std::complex<R>)r;}
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename CNT<std::complex<R> >::template Result<Mat<M,N,E,CS,RS> >::Dvd
+operator/(const conjugate<R>& l, const Mat<M,N,E,CS,RS>& r) {return (std::complex<R>)l/r;}
+
+// m = m/negator, negator/m: convert negator to a standard number
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename Mat<M,N,E,CS,RS>::template Result<typename negator<R>::StdNumber>::Dvd
+operator/(const Mat<M,N,E,CS,RS>& l, const negator<R>& r) {return l/(typename negator<R>::StdNumber)(R)r;}
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename CNT<R>::template Result<Mat<M,N,E,CS,RS> >::Dvd
+operator/(const negator<R>& l, const Mat<M,N,E,CS,RS>& r) {return (typename negator<R>::StdNumber)(R)l/r;}
+
+
+// Add and subtract are odd as scalar ops. They behave as though the
+// scalar stands for a conforming matrix whose diagonal elements are that,
+// scalar and then a normal matrix add or subtract is done.
+
+// SCALAR ADD
+
+// m = m+real, real+m
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<float>::Add
+operator+(const Mat<M,N,E,CS,RS>& l, const float& r)
+ { return Mat<M,N,E,CS,RS>::template Result<float>::AddOp::perform(l,r); }
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<float>::Add
+operator+(const float& l, const Mat<M,N,E,CS,RS>& r) {return r+l;}
+
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<double>::Add
+operator+(const Mat<M,N,E,CS,RS>& l, const double& r)
+ { return Mat<M,N,E,CS,RS>::template Result<double>::AddOp::perform(l,r); }
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<double>::Add
+operator+(const double& l, const Mat<M,N,E,CS,RS>& r) {return r+l;}
+
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<long double>::Add
+operator+(const Mat<M,N,E,CS,RS>& l, const long double& r)
+ { return Mat<M,N,E,CS,RS>::template Result<long double>::AddOp::perform(l,r); }
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<long double>::Add
+operator+(const long double& l, const Mat<M,N,E,CS,RS>& r) {return r+l;}
+
+// m = m+int, int+m -- just convert int to m's precision float
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<typename CNT<E>::Precision>::Add
+operator+(const Mat<M,N,E,CS,RS>& l, int r) {return l + (typename CNT<E>::Precision)r;}
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<typename CNT<E>::Precision>::Add
+operator+(int l, const Mat<M,N,E,CS,RS>& r) {return r + (typename CNT<E>::Precision)l;}
+
+// Complex, conjugate, and negator are all easy to templatize.
+
+// m = m+complex, complex+m
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Add
+operator+(const Mat<M,N,E,CS,RS>& l, const std::complex<R>& r)
+ { return Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::AddOp::perform(l,r); }
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Add
+operator+(const std::complex<R>& l, const Mat<M,N,E,CS,RS>& r) {return r+l;}
+
+// m = m+conjugate, conjugate+m (convert conjugate->complex)
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Add
+operator+(const Mat<M,N,E,CS,RS>& l, const conjugate<R>& r) {return l+(std::complex<R>)r;}
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Add
+operator+(const conjugate<R>& l, const Mat<M,N,E,CS,RS>& r) {return r+(std::complex<R>)l;}
+
+// m = m+negator, negator+m: convert negator to standard number
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename Mat<M,N,E,CS,RS>::template Result<typename negator<R>::StdNumber>::Add
+operator+(const Mat<M,N,E,CS,RS>& l, const negator<R>& r) {return l + (typename negator<R>::StdNumber)(R)r;}
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename Mat<M,N,E,CS,RS>::template Result<typename negator<R>::StdNumber>::Add
+operator+(const negator<R>& l, const Mat<M,N,E,CS,RS>& r) {return r + (typename negator<R>::StdNumber)(R)l;}
+
+// SCALAR SUBTRACT -- careful, not commutative.
+
+// m = m-real, real-m
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<float>::Sub
+operator-(const Mat<M,N,E,CS,RS>& l, const float& r)
+ { return Mat<M,N,E,CS,RS>::template Result<float>::SubOp::perform(l,r); }
+template <int M, int N, class E, int CS, int RS> inline
+typename CNT<float>::template Result<Mat<M,N,E,CS,RS> >::Sub
+operator-(const float& l, const Mat<M,N,E,CS,RS>& r)
+ { return CNT<float>::template Result<Mat<M,N,E,CS,RS> >::SubOp::perform(l,r); }
+
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<double>::Sub
+operator-(const Mat<M,N,E,CS,RS>& l, const double& r)
+ { return Mat<M,N,E,CS,RS>::template Result<double>::SubOp::perform(l,r); }
+template <int M, int N, class E, int CS, int RS> inline
+typename CNT<double>::template Result<Mat<M,N,E,CS,RS> >::Sub
+operator-(const double& l, const Mat<M,N,E,CS,RS>& r)
+ { return CNT<double>::template Result<Mat<M,N,E,CS,RS> >::SubOp::perform(l,r); }
+
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<long double>::Sub
+operator-(const Mat<M,N,E,CS,RS>& l, const long double& r)
+ { return Mat<M,N,E,CS,RS>::template Result<long double>::SubOp::perform(l,r); }
+template <int M, int N, class E, int CS, int RS> inline
+typename CNT<long double>::template Result<Mat<M,N,E,CS,RS> >::Sub
+operator-(const long double& l, const Mat<M,N,E,CS,RS>& r)
+ { return CNT<long double>::template Result<Mat<M,N,E,CS,RS> >::SubOp::perform(l,r); }
+
+// m = m-int, int-m // just convert int to m's precision float
+template <int M, int N, class E, int CS, int RS> inline
+typename Mat<M,N,E,CS,RS>::template Result<typename CNT<E>::Precision>::Sub
+operator-(const Mat<M,N,E,CS,RS>& l, int r) {return l - (typename CNT<E>::Precision)r;}
+template <int M, int N, class E, int CS, int RS> inline
+typename CNT<typename CNT<E>::Precision>::template Result<Mat<M,N,E,CS,RS> >::Sub
+operator-(int l, const Mat<M,N,E,CS,RS>& r) {return (typename CNT<E>::Precision)l - r;}
+
+
+// Complex, conjugate, and negator are all easy to templatize.
+
+// m = m-complex, complex-m
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Sub
+operator-(const Mat<M,N,E,CS,RS>& l, const std::complex<R>& r)
+ { return Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::SubOp::perform(l,r); }
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename CNT<std::complex<R> >::template Result<Mat<M,N,E,CS,RS> >::Sub
+operator-(const std::complex<R>& l, const Mat<M,N,E,CS,RS>& r)
+ { return CNT<std::complex<R> >::template Result<Mat<M,N,E,CS,RS> >::SubOp::perform(l,r); }
+
+// m = m-conjugate, conjugate-m (convert conjugate->complex)
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename Mat<M,N,E,CS,RS>::template Result<std::complex<R> >::Sub
+operator-(const Mat<M,N,E,CS,RS>& l, const conjugate<R>& r) {return l-(std::complex<R>)r;}
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename CNT<std::complex<R> >::template Result<Mat<M,N,E,CS,RS> >::Sub
+operator-(const conjugate<R>& l, const Mat<M,N,E,CS,RS>& r) {return (std::complex<R>)l-r;}
+
+// m = m-negator, negator-m: convert negator to standard number
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename Mat<M,N,E,CS,RS>::template Result<typename negator<R>::StdNumber>::Sub
+operator-(const Mat<M,N,E,CS,RS>& l, const negator<R>& r) {return l-(typename negator<R>::StdNumber)(R)r;}
+template <int M, int N, class E, int CS, int RS, class R> inline
+typename CNT<R>::template Result<Mat<M,N,E,CS,RS> >::Sub
+operator-(const negator<R>& l, const Mat<M,N,E,CS,RS>& r) {return (typename negator<R>::StdNumber)(R)l-r;}
+
+
+// Mat I/O
+template <int M, int N, class E, int CS, int RS, class CHAR, class TRAITS> inline
+std::basic_ostream<CHAR,TRAITS>&
+operator<<(std::basic_ostream<CHAR,TRAITS>& o, const Mat<M,N,E,CS,RS>& m) {
+ for (int i=0;i<M;++i) {
+ o << std::endl << "[";
+ for (int j=0;j<N;++j)
+ o << (j>0?",":"") << m(i,j);
+ o << "]";
+ }
+ if (M) o << std::endl;
+ return o;
+}
+
+template <int M, int N, class E, int CS, int RS, class CHAR, class TRAITS> inline
+std::basic_istream<CHAR,TRAITS>&
+operator>>(std::basic_istream<CHAR,TRAITS>& is, Mat<M,N,E,CS,RS>& m) {
+ // TODO: not sure how to do Vec input yet
+ assert(false);
+ return is;
+}
+
+} //namespace SimTK
+
+
+#endif //SimTK_SIMMATRIX_SMALLMATRIX_MAT_H_
diff --git a/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/ResultType.h b/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/ResultType.h
new file mode 100644
index 0000000..12cd77d
--- /dev/null
+++ b/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/ResultType.h
@@ -0,0 +1,791 @@
+#ifndef SimTK_SIMMATRIX_RESULT_TYPE_H_
+#define SimTK_SIMMATRIX_RESULT_TYPE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This file defines a template class with specializations that helps
+ * determine the result type when arithmetic operators are applied
+ * to Composite Numerical Types.
+ */
+
+#include "SimTKcommon/internal/common.h"
+
+namespace SimTK {
+
+
+/**
+ * The classes here can be thought of as compile-time type calculators
+ * which can compute the appropriate return type when the binary
+ * arithmetic operators +,-,*,/ are applied to Composite Numerical
+ * Type (CNT) arguments. We define these template classes:
+ * AddCNTs MulCNTs
+ * SubCNTs DvdCNTs
+ * and specialize them for each distinct combination of arguments.
+ * Each has a typedef member "Type" which gives the result type,
+ * and a static method "perform()" which delegates the operation to
+ * the appropriate method of one of the arguments.
+
+ * The general template class must be sufficiently parameterized that ALL
+ * combinations of CNT arguments can be viewed as special cases.
+ * That permits us to specialize properly here, and then individual
+ * CNTs base their Result classes on this one, without the need
+ * for specialization (which doesn't work well for internal
+ * classes on some compilers).
+ *
+ * The argument attributes that we must be able to specialize on
+ * are: shape, type (Row,Vec,Mat,SymMat) and "depth", meaning whether
+ * an argument is a scalar (depth 0), composite with scalar elements
+ * (depth 1), or composite with composite elements (depth 2). We do
+ * not implement the most general form at all; all meaningful
+ * combinations of CNT shapes must be specialized. We distinguish
+ * conforming arguments from non-conforming. The idea is to determine
+ * whether we are doing a matrix/matrix operation, or a matrix/element
+ * operation, and then in the latter case which of the two arguments is
+ * to be treated as the matrix.
+ *
+ * Here are the rules:
+ * (0) if both arguments are scalars results typing will be
+ * handled elsewhere, so we don't worry about that case here
+ * (1) if one argument is a scalar, we are doing a matrix/element
+ * operation (and of course the other one is the matrix) unless
+ * the scalar is the left operand of a divide
+ * (2) if neither argument is scalar, and the dimensions are
+ * conforming for the particular operation being performed,
+ * then we are doing a matrix/matrix operation
+ * (3) (no scalar, not conforming) If one argument has scalar
+ * elements (depth 1) and the other has composites (depth 2),
+ * treat the depth 1 argument as though it were just a scalar
+ * and perform a matrix/element operation.
+ * (4) (no scalar, not conforming, both or neither have scalar
+ * elements) This is an error and won't compile.
+ *
+ * Conforming matrices
+ * -------------------
+ *
+ * Whether two CNTs are conforming depends on the operation being
+ * performed. For add and subtract, they are conforming if and only
+ * if both CNTs have the same outer shape (number of rows and columns)
+ * and both are rows, both are vectors, or both are matrices. (Note that
+ * the Mat and SymMat CNTs are both matrices so can be mixed.) For
+ * conforming multiplication the number of columns of the LHS must
+ * match the number of rows on the RHS. For conforming division (that is,
+ * right multiplication by the inverse) the number of columns of the LHS
+ * must match the number of columns on the RHS. Here are the cases, with
+ * m=Mat, sy=SymMat, v=Vec, r=Row, and s=scalar:
+ *
+ * Add, Sub: conforming means LHS_ncol==RHS_ncol && LHS_nrow==RHS_nrow
+ * m=m+m m=sy+m m=m+sy sy=sy+sy
+ * v=v+v r=r+r
+ * Mul: conforming means LHS_ncol==RHS_nrow
+ * m=m*m m=sy*m m=m*sy m=sy*sy m=v*r (outer product)
+ * v=m*v v=sy*v
+ * r=r*m r=r*sy
+ * s=r*v (dot product)
+ * Dvd: conforming means LHS_ncol==RHS_ncol
+ * m=m/m m=sy/m m=m/sy m=sy/sy
+ * v=m/r v=sy/r
+ * r=r/m r=r/sy
+ * r=s/v v=s/r m=s/m (inversion)
+ * Note that we don't care about the element types when determining
+ * conformation; we'll delegate to the elements later and let them
+ * deal with their own problems recursively.
+ *
+ * We only allow non-conforming arguments when (a) one argument is
+ * a scalar and the other composite, or (b) one composite argument
+ * has scalar elements and the other has composite elements. In case
+ * (b), we treat the "less composite" argument as though it were a
+ * scalar, and apply it to each of the elements of the "more composite"
+ * argument. The cases are (e is the less composite 'element'):
+ * m= m op e m=e op m
+ * sy=sy op e sy=e op sy
+ * v= v op e v=e op v
+ * r= r op e r=e op r
+ *
+ * We assume that results types for scalars are dealt with
+ * elsewhere, and that we can access ResultType for elements
+ * using CNT<LHSType>::Result<RHSType>::op where 'op' is Add,
+ * Sub, Mul, Dvd. Note that elements of a CNT can be arbitrarily
+ * complicated CNTs themselves.
+ */
+
+// The template arguments are repeated for LHS and RHS with these meanings:
+// NRow, NCol: Dimensions of the argument. NRow=1 for rows, NCol=1 for vecs,
+// and NRow=NCol=1 for scalars.
+// ArgDepth: Complexity of the argument as described above. 0=scalar,
+// 1=composite with scalar elements, 2=composite of composites
+// L or R: The complete type of the LHS or RHS argument,
+// e.g. Row<5,Vec3,7>
+// CS, RS: Column spacing and row spacing (1 each for scalars). These
+// parameters are here because they are a "don't care" item;
+// the return types are always packed.
+
+template <int LNRow, int LNCol, int LArgDepth, class L, int LCS, int LRS,
+ int RNRow, int RNCol, int RArgDepth, class R, int RCS, int RRS>
+class AddCNTs {
+ // MUST BE SPECIALIZED
+public:
+ typedef void Type;
+ static void perform(const L&,const R&) {assert(false);}
+};
+template <int LNRow, int LNCol, int LArgDepth, class L, int LCS, int LRS,
+ int RNRow, int RNCol, int RArgDepth, class R, int RCS, int RRS>
+class SubCNTs {
+ // MUST BE SPECIALIZED
+public:
+ typedef void Type;
+ static void perform(const L&,const R&) {assert(false);}
+};
+template <int LNRow, int LNCol, int LArgDepth, class L, int LCS, int LRS,
+ int RNRow, int RNCol, int RArgDepth, class R, int RCS, int RRS>
+class MulCNTs {
+ // MUST BE SPECIALIZED
+public:
+ typedef void Type;
+ static void perform(const L&,const R&) {assert(false);}
+};
+template <int LNRow, int LNCol, int LArgDepth, class L, int LCS, int LRS,
+ int RNRow, int RNCol, int RArgDepth, class R, int RCS, int RRS>
+class DvdCNTs {
+ // MUST BE SPECIALIZED
+public:
+ typedef void Type;
+ static void perform(const L&,const R&) {assert(false);}
+};
+
+///////////////////////////////////////////////////////////////////////////
+// RULE 1: if an argument is a scalar, perform matrix/element operation, //
+// except for scalar on left of divide which is really just an //
+// inversion of the RHS, followed by a scalar multiply. //
+///////////////////////////////////////////////////////////////////////////
+
+// CNT+scalar, scalar+CNT
+template <int LNRow, int LNCol, class L, int LCS, int LRS, class R>
+class AddCNTs<LNRow,LNCol,SCALAR_COMPOSITE_DEPTH,L,LCS,LRS,1,1,SCALAR_DEPTH,R,1,1> {
+ typedef typename CNT<L>::TElement LE;
+ typedef typename CNT<LE>::template Result<R>::Add EAdd;
+public:
+ typedef typename CNT<L>::template Substitute<EAdd>::Type Type;
+ static Type perform(const L& l, const R& r) {return l.scalarAdd(r);}
+};
+template <int LNRow, int LNCol, class L, int LCS, int LRS, class R>
+class AddCNTs<LNRow,LNCol,COMPOSITE_COMPOSITE_DEPTH,L,LCS,LRS,1,1,SCALAR_DEPTH,R,1,1> {
+ typedef typename CNT<L>::TElement LE;
+ typedef typename CNT<LE>::template Result<R>::Add EAdd;
+public:
+ typedef typename CNT<L>::template Substitute<EAdd>::Type Type;
+ static Type perform(const L& l, const R& r) {return l.scalarAdd(r);}
+};
+template <int LNRow, int LNCol, class L, int LCS, int LRS, class R>
+class AddCNTs<LNRow,LNCol,COMPOSITE_3_DEPTH,L,LCS,LRS,1,1,SCALAR_DEPTH,R,1,1> {
+ typedef typename CNT<L>::TElement LE;
+ typedef typename CNT<LE>::template Result<R>::Add EAdd;
+public:
+ typedef typename CNT<L>::template Substitute<EAdd>::Type Type;
+ static Type perform(const L& l, const R& r) {return l.scalarAdd(r);}
+};
+
+// scalar+CNT
+template <class L, int RNRow, int RNCol, class R, int RCS, int RRS>
+class AddCNTs<1,1,SCALAR_DEPTH,L,1,1, RNRow,RNCol,SCALAR_COMPOSITE_DEPTH,R,RCS,RRS> {
+ typedef typename CNT<R>::TElement RE;
+ typedef typename CNT<RE>::template Result<L>::Add EAdd;
+public:
+ typedef typename CNT<R>::template Substitute<EAdd>::Type Type;
+ static Type perform(const L& l, const R& r) {return r.scalarAdd(l);}
+};
+template <class L, int RNRow, int RNCol, class R, int RCS, int RRS>
+class AddCNTs<1,1,SCALAR_DEPTH,L,1,1, RNRow,RNCol,COMPOSITE_COMPOSITE_DEPTH,R,RCS,RRS> {
+ typedef typename CNT<R>::TElement RE;
+ typedef typename CNT<RE>::template Result<L>::Add EAdd;
+public:
+ typedef typename CNT<R>::template Substitute<EAdd>::Type Type;
+ static Type perform(const L& l, const R& r) {return r.scalarAdd(l);}
+};
+template <class L, int RNRow, int RNCol, class R, int RCS, int RRS>
+class AddCNTs<1,1,SCALAR_DEPTH,L,1,1, RNRow,RNCol,COMPOSITE_3_DEPTH,R,RCS,RRS> {
+ typedef typename CNT<R>::TElement RE;
+ typedef typename CNT<RE>::template Result<L>::Add EAdd;
+public:
+ typedef typename CNT<R>::template Substitute<EAdd>::Type Type;
+ static Type perform(const L& l, const R& r) {return r.scalarAdd(l);}
+};
+
+// CNT-scalar
+template <int LNRow, int LNCol, class L, int LCS, int LRS, class R>
+class SubCNTs<LNRow,LNCol,SCALAR_COMPOSITE_DEPTH,L,LCS,LRS,1,1,SCALAR_DEPTH,R,1,1> {
+ typedef typename CNT<L>::TElement LE;
+ typedef typename CNT<LE>::template Result<R>::Sub ESub;
+public:
+ typedef typename CNT<L>::template Substitute<ESub>::Type Type;
+ static Type perform(const L& l, const R& r) {return l.scalarSubtract(r);}
+};
+template <int LNRow, int LNCol, class L, int LCS, int LRS, class R>
+class SubCNTs<LNRow,LNCol,COMPOSITE_COMPOSITE_DEPTH,L,LCS,LRS,1,1,SCALAR_DEPTH,R,1,1> {
+ typedef typename CNT<L>::TElement LE;
+ typedef typename CNT<LE>::template Result<R>::Sub ESub;
+public:
+ typedef typename CNT<L>::template Substitute<ESub>::Type Type;
+ static Type perform(const L& l, const R& r) {return l.scalarSubtract(r);}
+};
+template <int LNRow, int LNCol, class L, int LCS, int LRS, class R>
+class SubCNTs<LNRow,LNCol,COMPOSITE_3_DEPTH,L,LCS,LRS,1,1,SCALAR_DEPTH,R,1,1> {
+ typedef typename CNT<L>::TElement LE;
+ typedef typename CNT<LE>::template Result<R>::Sub ESub;
+public:
+ typedef typename CNT<L>::template Substitute<ESub>::Type Type;
+ static Type perform(const L& l, const R& r) {return l.scalarSubtract(r);}
+};
+// scalar-CNT
+template <class L, int RNRow, int RNCol, class R, int RCS, int RRS>
+class SubCNTs<1,1,SCALAR_DEPTH,L,1,1, RNRow,RNCol,SCALAR_COMPOSITE_DEPTH,R,RCS,RRS> {
+ typedef typename CNT<R>::TElement RE;
+ typedef typename CNT<L>::template Result<RE>::Sub ESub;
+public:
+ typedef typename CNT<R>::template Substitute<ESub>::Type Type;
+ static Type perform(const L& l, const R& r) {return r.scalarSubFromLeft(l);}
+};
+template <class L, int RNRow, int RNCol, class R, int RCS, int RRS>
+class SubCNTs<1,1,SCALAR_DEPTH,L,1,1, RNRow,RNCol,COMPOSITE_COMPOSITE_DEPTH,R,RCS,RRS> {
+ typedef typename CNT<R>::TElement RE;
+ typedef typename CNT<L>::template Result<RE>::Sub ESub;
+public:
+ typedef typename CNT<R>::template Substitute<ESub>::Type Type;
+ static Type perform(const L& l, const R& r) {return r.scalarSubFromLeft(l);}
+};
+template <class L, int RNRow, int RNCol, class R, int RCS, int RRS>
+class SubCNTs<1,1,SCALAR_DEPTH,L,1,1, RNRow,RNCol,COMPOSITE_3_DEPTH,R,RCS,RRS> {
+ typedef typename CNT<R>::TElement RE;
+ typedef typename CNT<L>::template Result<RE>::Sub ESub;
+public:
+ typedef typename CNT<R>::template Substitute<ESub>::Type Type;
+ static Type perform(const L& l, const R& r) {return r.scalarSubFromLeft(l);}
+};
+
+// CNT*scalar
+template <int LNRow, int LNCol, class L, int LCS, int LRS, class R>
+class MulCNTs<LNRow,LNCol,SCALAR_COMPOSITE_DEPTH,L,LCS,LRS,1,1,SCALAR_DEPTH,R,1,1> {
+ typedef typename CNT<L>::TElement LE;
+ typedef typename CNT<LE>::template Result<R>::Mul EMul;
+public:
+ typedef typename CNT<L>::template Substitute<EMul>::Type Type;
+ static Type perform(const L& l, const R& r) {return l.scalarMultiply(r);}
+};
+template <int LNRow, int LNCol, class L, int LCS, int LRS, class R>
+class MulCNTs<LNRow,LNCol,COMPOSITE_COMPOSITE_DEPTH,L,LCS,LRS,1,1,SCALAR_DEPTH,R,1,1> {
+ typedef typename CNT<L>::TElement LE;
+ typedef typename CNT<LE>::template Result<R>::Mul EMul;
+public:
+ typedef typename CNT<L>::template Substitute<EMul>::Type Type;
+ static Type perform(const L& l, const R& r) {return l.scalarMultiply(r);}
+};
+template <int LNRow, int LNCol, class L, int LCS, int LRS, class R>
+class MulCNTs<LNRow,LNCol,COMPOSITE_3_DEPTH,L,LCS,LRS,1,1,SCALAR_DEPTH,R,1,1> {
+ typedef typename CNT<L>::TElement LE;
+ typedef typename CNT<LE>::template Result<R>::Mul EMul;
+public:
+ typedef typename CNT<L>::template Substitute<EMul>::Type Type;
+ static Type perform(const L& l, const R& r) {return l.scalarMultiply(r);}
+};
+// scalar*CNT
+template <class L, int RNRow, int RNCol, class R, int RCS, int RRS>
+class MulCNTs<1,1,SCALAR_DEPTH,L,1,1, RNRow,RNCol,SCALAR_COMPOSITE_DEPTH,R,RCS,RRS> {
+ typedef typename CNT<R>::TElement RE;
+ typedef typename CNT<L>::template Result<RE>::Mul EMul;
+public:
+ typedef typename CNT<R>::template Substitute<EMul>::Type Type;
+ static Type perform(const L& l, const R& r) {return r.scalarMultiplyFromLeft(l);}
+};
+template <class L, int RNRow, int RNCol, class R, int RCS, int RRS>
+class MulCNTs<1,1,SCALAR_DEPTH,L,1,1, RNRow,RNCol,COMPOSITE_COMPOSITE_DEPTH,R,RCS,RRS> {
+ typedef typename CNT<R>::TElement RE;
+ typedef typename CNT<L>::template Result<RE>::Mul EMul;
+public:
+ typedef typename CNT<R>::template Substitute<EMul>::Type Type;
+ static Type perform(const L& l, const R& r) {return r.scalarMultiplyFromLeft(l);}
+};
+template <class L, int RNRow, int RNCol, class R, int RCS, int RRS>
+class MulCNTs<1,1,SCALAR_DEPTH,L,1,1, RNRow,RNCol,COMPOSITE_3_DEPTH,R,RCS,RRS> {
+ typedef typename CNT<R>::TElement RE;
+ typedef typename CNT<L>::template Result<RE>::Mul EMul;
+public:
+ typedef typename CNT<R>::template Substitute<EMul>::Type Type;
+ static Type perform(const L& l, const R& r) {return r.scalarMultiplyFromLeft(l);}
+};
+
+// CNT/scalar
+template <int LNRow, int LNCol, class L, int LCS, int LRS, class R>
+class DvdCNTs<LNRow,LNCol,SCALAR_COMPOSITE_DEPTH,L,LCS,LRS,1,1,SCALAR_DEPTH,R,1,1> {
+ typedef typename CNT<L>::TElement LE;
+ typedef typename CNT<LE>::template Result<R>::Dvd EDvd;
+public:
+ typedef typename CNT<L>::template Substitute<EDvd>::Type Type;
+ static Type perform(const L& l, const R& r) {return l.scalarDivide(r);}
+};
+template <int LNRow, int LNCol, class L, int LCS, int LRS, class R>
+class DvdCNTs<LNRow,LNCol,COMPOSITE_COMPOSITE_DEPTH,L,LCS,LRS,1,1,SCALAR_DEPTH,R,1,1> {
+ typedef typename CNT<L>::TElement LE;
+ typedef typename CNT<LE>::template Result<R>::Dvd EDvd;
+public:
+ typedef typename CNT<L>::template Substitute<EDvd>::Type Type;
+ static Type perform(const L& l, const R& r) {return l.scalarDivide(r);}
+};
+template <int LNRow, int LNCol, class L, int LCS, int LRS, class R>
+class DvdCNTs<LNRow,LNCol,COMPOSITE_3_DEPTH,L,LCS,LRS,1,1,SCALAR_DEPTH,R,1,1> {
+ typedef typename CNT<L>::TElement LE;
+ typedef typename CNT<LE>::template Result<R>::Dvd EDvd;
+public:
+ typedef typename CNT<L>::template Substitute<EDvd>::Type Type;
+ static Type perform(const L& l, const R& r) {return l.scalarDivide(r);}
+};
+
+// scalar/CNT is really scalar * inverse(CNT) so we're combining the
+// inverse with a scalar multiply as handled in MulCNTs above.
+template <class L, int RNRow, int RNCol, class R, int RCS, int RRS>
+class DvdCNTs<1,1,SCALAR_DEPTH,L,1,1, RNRow,RNCol,SCALAR_COMPOSITE_DEPTH,R,RCS,RRS> {
+ typedef typename CNT<R>::TInverse RInv;
+public:
+ typedef typename CNT<RInv>::template Result<L>::Mul Type;
+ static Type perform(const L& l, const R& r) {return l*r.invert();}
+};
+template <class L, int RNRow, int RNCol, class R, int RCS, int RRS>
+class DvdCNTs<1,1,SCALAR_DEPTH,L,1,1, RNRow,RNCol,COMPOSITE_COMPOSITE_DEPTH,R,RCS,RRS> {
+ typedef typename CNT<R>::TInverse RInv;
+public:
+ typedef typename CNT<RInv>::template Result<L>::Mul Type;
+ static Type perform(const L& l, const R& r) {return l*r.invert();}
+};
+template <class L, int RNRow, int RNCol, class R, int RCS, int RRS>
+class DvdCNTs<1,1,SCALAR_DEPTH,L,1,1, RNRow,RNCol,COMPOSITE_3_DEPTH,R,RCS,RRS> {
+ typedef typename CNT<R>::TInverse RInv;
+public:
+ typedef typename CNT<RInv>::template Result<L>::Mul Type;
+ static Type perform(const L& l, const R& r) {return l*r.invert();}
+};
+
+ // CONFORMING Add and Subtract
+
+// conforming: vec=vec+vec
+template <int NRow, int LArgDepth, class LE, int LCS, int LRS,
+ int RArgDepth, class RE, int RCS, int RRS>
+class AddCNTs<NRow,1,LArgDepth,Vec<NRow,LE,LRS>,LCS,LRS,
+ NRow,1,RArgDepth,Vec<NRow,RE,RRS>,RCS,RRS>
+{
+ typedef Vec<NRow,LE,LRS> L;
+ typedef Vec<NRow,RE,RRS> R;
+ typedef typename CNT<LE>::template Result<RE>::Add EAdd;
+public:
+ typedef Vec<NRow,EAdd> Type;
+ static Type perform(const L& l, const R& r) {return l.conformingAdd(r);}
+};
+// conforming: vec=vec-vec
+template <int NRow, int LArgDepth, class LE, int LCS, int LRS,
+ int RArgDepth, class RE, int RCS, int RRS>
+class SubCNTs<NRow,1,LArgDepth,Vec<NRow,LE,LRS>,LCS,LRS,
+ NRow,1,RArgDepth,Vec<NRow,RE,RRS>,RCS,RRS>
+{
+ typedef Vec<NRow,LE,LRS> L;
+ typedef Vec<NRow,RE,RRS> R;
+ typedef typename CNT<LE>::template Result<RE>::Sub ESub;
+public:
+ typedef Vec<NRow,ESub> Type;
+ static Type perform(const L& l, const R& r) {return l.conformingSubtract(r);}
+};
+// conforming: row=row+row
+template <int NCol, int LArgDepth, class LE, int LCS, int LRS,
+ int RArgDepth, class RE, int RCS, int RRS>
+class AddCNTs<1,NCol,LArgDepth,Row<NCol,LE,LCS>,LCS,LRS,
+ 1,NCol,RArgDepth,Row<NCol,RE,RCS>,RCS,RRS>
+{
+ typedef Row<NCol,LE,LCS> L;
+ typedef Row<NCol,RE,RCS> R;
+ typedef typename CNT<LE>::template Result<RE>::Add EAdd;
+public:
+ typedef Row<NCol,EAdd> Type;
+ static Type perform(const L& l, const R& r) {return l.conformingAdd(r);}
+};
+// conforming: row=row-row
+template <int NCol, int LArgDepth, class LE, int LCS, int LRS,
+ int RArgDepth, class RE, int RCS, int RRS>
+class SubCNTs<1,NCol,LArgDepth,Row<NCol,LE,LCS>,LCS,LRS,
+ 1,NCol,RArgDepth,Row<NCol,RE,RCS>,RCS,RRS>
+{
+ typedef Row<NCol,LE,LCS> L;
+ typedef Row<NCol,RE,RCS> R;
+ typedef typename CNT<LE>::template Result<RE>::Sub ESub;
+public:
+ typedef Row<NCol,ESub> Type;
+ static Type perform(const L& l, const R& r) {return l.conformingSubtract(r);}
+};
+// conforming: sym=sym+sym
+template <int Dim, int LArgDepth, class LE, int LCS, int LRS,
+ int RArgDepth, class RE, int RCS, int RRS>
+class AddCNTs<Dim,Dim,LArgDepth,SymMat<Dim,LE,LRS>,LCS,LRS,
+ Dim,Dim,RArgDepth,SymMat<Dim,RE,RRS>,RCS,RRS>
+{
+ typedef SymMat<Dim,LE,LRS> L;
+ typedef SymMat<Dim,RE,RRS> R;
+ typedef typename CNT<LE>::template Result<RE>::Add EAdd;
+public:
+ typedef SymMat<Dim,EAdd> Type;
+ static Type perform(const L& l, const R& r) {return l.conformingAdd(r);}
+};
+// conforming: sym=sym-sym
+template <int Dim, int LArgDepth, class LE, int LCS, int LRS,
+ int RArgDepth, class RE, int RCS, int RRS>
+class SubCNTs<Dim,Dim,LArgDepth,SymMat<Dim,LE,LRS>,LCS,LRS,
+ Dim,Dim,RArgDepth,SymMat<Dim,RE,RRS>,RCS,RRS>
+{
+ typedef SymMat<Dim,LE,LRS> L;
+ typedef SymMat<Dim,RE,RRS> R;
+ typedef typename CNT<LE>::template Result<RE>::Sub ESub;
+public:
+ typedef SymMat<Dim,ESub> Type;
+ static Type perform(const L& l, const R& r) {return l.conformingSubtract(r);}
+};
+// conforming: mat=mat+mat, mat=mat+sym, mat=sym+mat
+template <int NRow, int NCol, int LArgDepth, class LE, int LCS, int LRS,
+ int RArgDepth, class RE, int RCS, int RRS>
+class AddCNTs<NRow,NCol,LArgDepth,Mat<NRow,NCol,LE,LCS,LRS>,LCS,LRS,
+ NRow,NCol,RArgDepth,Mat<NRow,NCol,RE,RCS,RRS>,RCS,RRS>
+{
+ typedef Mat<NRow,NCol,LE,LCS,LRS> L;
+ typedef Mat<NRow,NCol,RE,RCS,RRS> R;
+ typedef typename CNT<LE>::template Result<RE>::Add EAdd;
+public:
+ typedef Mat<NRow,NCol,EAdd> Type;
+ static Type perform(const L& l, const R& r) {return l.conformingAdd(r);}
+};
+template <int Dim, int LArgDepth, class LE, int LCS, int LRS,
+ int RArgDepth, class RE, int RCS, int RRS>
+class AddCNTs<Dim,Dim,LArgDepth,Mat<Dim,Dim,LE,LCS,LRS>,LCS,LRS,
+ Dim,Dim,RArgDepth,SymMat<Dim,RE,RRS>, RCS,RRS>
+{
+ typedef Mat<Dim,Dim,LE,LCS,LRS> L;
+ typedef SymMat<Dim,RE,RRS> R;
+ typedef typename CNT<LE>::template Result<RE>::Add EAdd;
+public:
+ typedef Mat<Dim,Dim,EAdd> Type;
+ // Let the SymMat deal with this
+ static Type perform(const L& l, const R& r) {return r.conformingAdd(l);}
+};
+template <int Dim, int LArgDepth, class LE, int LCS, int LRS,
+ int RArgDepth, class RE, int RCS, int RRS>
+class AddCNTs<Dim,Dim,LArgDepth,SymMat<Dim,LE,LRS>, LCS,LRS,
+ Dim,Dim,RArgDepth,Mat<Dim,Dim,RE,RCS,RRS>,RCS,RRS>
+{
+ typedef SymMat<Dim,LE,LRS> L;
+ typedef Mat<Dim,Dim,RE,RCS,RRS> R;
+ typedef typename CNT<LE>::template Result<RE>::Add EAdd;
+public:
+ typedef Mat<Dim,Dim,EAdd> Type;
+ // Let the SymMat deal with this
+ static Type perform(const L& l, const R& r) {return l.conformingAdd(r);}
+};
+
+// conforming mat=mat-mat, mat=mat-sym, mat=sym-mat
+template <int NRow, int NCol, int LArgDepth, class LE, int LCS, int LRS,
+ int RArgDepth, class RE, int RCS, int RRS>
+class SubCNTs<NRow,NCol,LArgDepth,Mat<NRow,NCol,LE,LCS,LRS>,LCS,LRS,
+ NRow,NCol,RArgDepth,Mat<NRow,NCol,RE,RCS,RRS>,RCS,RRS>
+{
+ typedef Mat<NRow,NCol,LE,LCS,LRS> L;
+ typedef Mat<NRow,NCol,RE,RCS,RRS> R;
+ typedef typename CNT<LE>::template Result<RE>::Sub ESub;
+public:
+ typedef Mat<NRow,NCol,ESub> Type;
+ static Type perform(const L& l, const R& r) {return l.conformingSubtract(r);}
+};
+template <int Dim, int LArgDepth, class LE, int LCS, int LRS,
+ int RArgDepth, class RE, int RCS, int RRS>
+class SubCNTs<Dim,Dim,LArgDepth,Mat<Dim,Dim,LE,LCS,LRS>,LCS,LRS,
+ Dim,Dim,RArgDepth,SymMat<Dim,RE,RRS>, RCS,RRS>
+{
+ typedef Mat<Dim,Dim,LE,LCS,LRS> L;
+ typedef SymMat<Dim,RE,RRS> R;
+ typedef typename CNT<LE>::template Result<RE>::Sub ESub;
+public:
+ typedef Mat<Dim,Dim,ESub> Type;
+ // Let the SymMat deal with this
+ static Type perform(const L& l, const R& r) {return r.conformingSubtractFromLeft(l);}
+};
+template <int Dim, int LArgDepth, class LE, int LCS, int LRS,
+ int RArgDepth, class RE, int RCS, int RRS>
+class SubCNTs<Dim,Dim,LArgDepth,SymMat<Dim,LE,LRS>, LCS,LRS,
+ Dim,Dim,RArgDepth,Mat<Dim,Dim,RE,RCS,RRS>,RCS,RRS>
+{
+ typedef SymMat<Dim,LE,LRS> L;
+ typedef Mat<Dim,Dim,RE,RCS,RRS> R;
+ typedef typename CNT<LE>::template Result<RE>::Sub ESub;
+public:
+ typedef Mat<Dim,Dim,ESub> Type;
+ // Let the SymMat deal with this
+ static Type perform(const L& l, const R& r) {return l.conformingSubtract(r);}
+};
+
+ // NON-CONFORMING ADD/SUBTRACT NOT ALLOWED (except for scalars)
+
+ // CONFORMING MULTIPLY
+
+// conforming: element=row*vec (dot product)
+template <int Dim, int LArgDepth, class LE, int LCS, int LRS,
+ int RArgDepth, class RE, int RCS, int RRS>
+class MulCNTs<1,Dim,LArgDepth,Row<Dim,LE,LCS>,LCS,LRS,
+ Dim,1,RArgDepth,Vec<Dim,RE,RRS>,RCS,RRS>
+{
+ typedef Row<Dim,LE,LCS> L;
+ typedef Vec<Dim,RE,RRS> R;
+ typedef typename CNT<LE>::template Result<RE>::Mul EMul;
+public:
+ typedef EMul Type;
+ static Type perform(const L& l, const R& r) {return l.conformingMultiply(r);}
+};
+
+// conforming: mat=vec*row (outer product)
+template <int Dim, int LArgDepth, class LE, int LCS, int LRS,
+ int RArgDepth, class RE, int RCS, int RRS>
+class MulCNTs<Dim,1,LArgDepth,Vec<Dim,LE,LRS>,LCS,LRS,
+ 1,Dim,RArgDepth,Row<Dim,RE,RCS>,RCS,RRS>
+{
+ typedef Vec<Dim,LE,LRS> L;
+ typedef Row<Dim,RE,RCS> R;
+ typedef typename CNT<LE>::template Result<RE>::Mul EMul;
+public:
+ typedef Mat<Dim,Dim,EMul> Type;
+ static Type perform(const L& l, const R& r) {return l.conformingMultiply(r);}
+};
+
+// conforming: row=row*mat
+template <int LNCol, int LArgDepth, class LE, int LCS, int LRS,
+ int RNCol, int RArgDepth, class RE, int RCS, int RRS>
+class MulCNTs<1, LNCol,LArgDepth,Row<LNCol,LE,LCS>, LCS,LRS,
+ LNCol,RNCol,RArgDepth,Mat<LNCol,RNCol,RE,RCS,RRS>,RCS,RRS>
+{
+ typedef Row<LNCol,LE,LCS> L;
+ typedef Mat<LNCol,RNCol,RE,RCS,RRS> R;
+ typedef typename CNT<LE>::template Result<RE>::Mul EMul;
+public:
+ typedef Row<RNCol,EMul> Type;
+ // Give the job to the matrix rather than the row.
+ static Type perform(const L& l, const R& r) {return r.conformingMultiplyFromLeft(l);}
+};
+// conforming: row=row*symmat
+template <int LNCol, int LArgDepth, class LE, int LCS, int LRS,
+ int RArgDepth, class RE, int RCS, int RRS>
+class MulCNTs<1, LNCol,LArgDepth,Row<LNCol,LE,LCS>, LCS,LRS,
+ LNCol,LNCol,RArgDepth,SymMat<LNCol,RE,RRS>,RCS,RRS>
+{
+ typedef Row<LNCol,LE,LCS> L;
+ typedef SymMat<LNCol,RE,RRS> R;
+ typedef typename CNT<LE>::template Result<RE>::Mul EMul;
+public:
+ typedef Row<LNCol,EMul> Type;
+ // Give the job to the matrix rather than the row.
+ static Type perform(const L& l, const R& r) {return r.conformingMultiplyFromLeft(l);}
+};
+
+// conforming: vec=mat*vec
+template <int LNRow, int LNCol, int LArgDepth, class LE, int LCS, int LRS,
+ int RArgDepth, class RE, int RCS, int RRS>
+class MulCNTs<LNRow,LNCol,LArgDepth,Mat<LNRow,LNCol,LE,LCS,LRS>,LCS,LRS,
+ LNCol,1, RArgDepth,Vec<LNCol,RE,RRS>, RCS,RRS>
+{
+ typedef Mat<LNRow,LNCol,LE,LCS,LRS> L;
+ typedef Vec<LNCol,RE,RRS> R;
+ typedef typename CNT<LE>::template Result<RE>::Mul EMul;
+public:
+ typedef Vec<LNRow,EMul> Type;
+ // We want the matrix to deal with this rather than the vec.
+ static Type perform(const L& l, const R& r) {return l.conformingMultiply(r);}
+};
+
+// conforming: vec=sym*vec
+template <int Dim, int LArgDepth, class LE, int LCS, int LRS,
+ int RArgDepth, class RE, int RCS, int RRS>
+class MulCNTs<Dim,Dim,LArgDepth,SymMat<Dim,LE,LRS>,LCS,LRS,
+ Dim,1, RArgDepth,Vec<Dim,RE,RRS>, RCS,RRS>
+{
+ typedef SymMat<Dim,LE,LRS> L;
+ typedef Vec<Dim,RE,RRS> R;
+ typedef typename CNT<LE>::template Result<RE>::Mul EMul;
+public:
+ typedef Vec<Dim,EMul> Type;
+ // We want the matrix to deal with this rather than the vec.
+ static Type perform(const L& l, const R& r) {return l.conformingMultiply(r);}
+};
+
+// conforming: mat=mat*mat
+template <int LNRow, int LNCol, int LArgDepth, class LE, int LCS, int LRS,
+ int RNCol, int RArgDepth, class RE, int RCS, int RRS>
+class MulCNTs<LNRow,LNCol,LArgDepth,Mat<LNRow,LNCol,LE,LCS,LRS>,LCS,LRS,
+ LNCol,RNCol,RArgDepth,Mat<LNCol,RNCol,RE,RCS,RRS>,RCS,RRS>
+{
+ typedef Mat<LNRow,LNCol,LE,LCS,LRS> L;
+ typedef Mat<LNCol,RNCol,RE,RCS,RRS> R;
+ typedef typename CNT<LE>::template Result<RE>::Mul EMul;
+public:
+ typedef Mat<LNRow,RNCol,EMul> Type;
+ static Type perform(const L& l, const R& r) {return l.conformingMultiply(r);}
+};
+// conforming: mat=mat*sym
+template <int LNRow, int LNCol, int LArgDepth, class LE, int LCS, int LRS,
+ int RArgDepth, class RE, int RCS, int RRS>
+class MulCNTs<LNRow,LNCol,LArgDepth,Mat<LNRow,LNCol,LE,LCS,LRS>,LCS,LRS,
+ LNCol,LNCol,RArgDepth,SymMat<LNCol,RE,RRS>, RCS,RRS>
+{
+ typedef Mat<LNRow,LNCol,LE,LCS,LRS> L;
+ typedef SymMat<LNCol,RE,RRS> R;
+ typedef typename CNT<LE>::template Result<RE>::Mul EMul;
+public:
+ typedef Mat<LNRow,LNCol,EMul> Type;
+ // Let SymMat deal with this.
+ static Type perform(const L& l, const R& r) {return r.conformingMultiplyFromLeft(l);}
+};
+// conforming: mat=sym*mat
+template <int LDim, int LArgDepth, class LE, int LCS, int LRS,
+ int RNCol, int RArgDepth, class RE, int RCS, int RRS>
+class MulCNTs<LDim,LDim, LArgDepth,SymMat<LDim,LE,LRS>, LCS,LRS,
+ LDim,RNCol,RArgDepth,Mat<LDim,RNCol,RE,RCS,RRS>,RCS,RRS>
+{
+ typedef SymMat<LDim,LE,LRS> L;
+ typedef Mat<LDim,RNCol,RE,RCS,RRS> R;
+ typedef typename CNT<LE>::template Result<RE>::Mul EMul;
+public:
+ typedef Mat<LDim,RNCol,EMul> Type;
+ // Let SymMat deal with this.
+ static Type perform(const L& l, const R& r) {return l.conformingMultiply(r);}
+};
+// conforming: mat=sym*sym
+template <int Dim, int LArgDepth, class LE, int LCS, int LRS,
+ int RArgDepth, class RE, int RCS, int RRS>
+class MulCNTs<Dim,Dim,LArgDepth,SymMat<Dim,LE,LRS>,LCS,LRS,
+ Dim,Dim,RArgDepth,SymMat<Dim,RE,RRS>,RCS,RRS>
+{
+ typedef SymMat<Dim,LE,LRS> L;
+ typedef SymMat<Dim,RE,RRS> R;
+ typedef typename CNT<LE>::template Result<RE>::Mul EMul;
+public:
+ typedef Mat<Dim,Dim,EMul> Type;
+ static Type perform(const L& l, const R& r) {return l.conformingMultiply(r);}
+};
+
+ // TODO: CONFORMING DIVIDE
+
+ // NON-CONFORMING MULTIPLY
+template <int LNRow, int LNCol, int LArgDepth, class L, int LCS, int LRS,
+ int RNRow, int RNCol, int RArgDepth, class R, int RCS, int RRS>
+class MulCNTsNonConforming {
+ // MUST BE SPECIALIZED
+public:
+ typedef void Type;
+ static void perform(const L&,const R&) {assert(false);}
+};
+
+// nonconforming: left = left(2)*right(1) (like scalar multiply)
+template <int LNRow, int LNCol, class L, int LCS, int LRS,
+ int RNRow, int RNCol, class R, int RCS, int RRS>
+class MulCNTsNonConforming<LNRow,LNCol,COMPOSITE_COMPOSITE_DEPTH,L,LCS,LRS,
+ RNRow,RNCol,SCALAR_COMPOSITE_DEPTH, R,RCS,RRS>
+{
+ typedef typename CNT<L>::TElement LE;
+ typedef typename CNT<R>::TElement RE;
+ typedef typename CNT<LE>::template Result<R>::Mul EMul;
+public:
+ typedef typename CNT<L>::template Substitute<EMul>::Type Type;
+ static Type perform(const L& l, const R& r) {return l.scalarMultiply(r);}
+};
+// nonconforming: left = left(3)*right(1) (like scalar multiply)
+template <int LNRow, int LNCol, class L, int LCS, int LRS,
+ int RNRow, int RNCol, class R, int RCS, int RRS>
+class MulCNTsNonConforming<LNRow,LNCol,COMPOSITE_3_DEPTH, L,LCS,LRS,
+ RNRow,RNCol,SCALAR_COMPOSITE_DEPTH, R,RCS,RRS>
+{
+ typedef typename CNT<L>::TElement LE;
+ typedef typename CNT<R>::TElement RE;
+ typedef typename CNT<LE>::template Result<R>::Mul EMul;
+public:
+ typedef typename CNT<L>::template Substitute<EMul>::Type Type;
+ static Type perform(const L& l, const R& r) {return l.scalarMultiply(r);}
+};
+// nonconforming: left = left(3)*right(2) (like scalar multiply)
+template <int LNRow, int LNCol, class L, int LCS, int LRS,
+ int RNRow, int RNCol, class R, int RCS, int RRS>
+class MulCNTsNonConforming<LNRow,LNCol,COMPOSITE_3_DEPTH, L,LCS,LRS,
+ RNRow,RNCol,COMPOSITE_COMPOSITE_DEPTH, R,RCS,RRS>
+{
+ typedef typename CNT<L>::TElement LE;
+ typedef typename CNT<R>::TElement RE;
+ typedef typename CNT<LE>::template Result<R>::Mul EMul;
+public:
+ typedef typename CNT<L>::template Substitute<EMul>::Type Type;
+ static Type perform(const L& l, const R& r) {return l.scalarMultiply(r);}
+};
+
+// nonconforming: right = left(1)*right(2) (like scalar multiply)
+template <int LNRow, int LNCol, class L, int LCS, int LRS,
+ int RNRow, int RNCol, class R, int RCS, int RRS>
+class MulCNTsNonConforming<LNRow,LNCol,SCALAR_COMPOSITE_DEPTH, L,LCS,LRS,
+ RNRow,RNCol,COMPOSITE_COMPOSITE_DEPTH,R,RCS,RRS>
+{
+ typedef typename CNT<L>::TElement LE;
+ typedef typename CNT<R>::TElement RE;
+ typedef typename CNT<L>::template Result<RE>::Mul EMul;
+public:
+ typedef typename CNT<R>::template Substitute<EMul>::Type Type;
+ static Type perform(const L& l, const R& r) {return r.scalarMultiplyFromLeft(l);}
+};
+// nonconforming: right = left(1)*right(3) (like scalar multiply)
+template <int LNRow, int LNCol, class L, int LCS, int LRS,
+ int RNRow, int RNCol, class R, int RCS, int RRS>
+class MulCNTsNonConforming<LNRow,LNCol,SCALAR_COMPOSITE_DEPTH, L,LCS,LRS,
+ RNRow,RNCol,COMPOSITE_3_DEPTH,R,RCS,RRS>
+{
+ typedef typename CNT<L>::TElement LE;
+ typedef typename CNT<R>::TElement RE;
+ typedef typename CNT<L>::template Result<RE>::Mul EMul;
+public:
+ typedef typename CNT<R>::template Substitute<EMul>::Type Type;
+ static Type perform(const L& l, const R& r) {return r.scalarMultiplyFromLeft(l);}
+};
+// nonconforming: right = left(2)*right(3) (like scalar multiply)
+template <int LNRow, int LNCol, class L, int LCS, int LRS,
+ int RNRow, int RNCol, class R, int RCS, int RRS>
+class MulCNTsNonConforming<LNRow,LNCol,COMPOSITE_COMPOSITE_DEPTH, L,LCS,LRS,
+ RNRow,RNCol,COMPOSITE_3_DEPTH,R,RCS,RRS>
+{
+ typedef typename CNT<L>::TElement LE;
+ typedef typename CNT<R>::TElement RE;
+ typedef typename CNT<L>::template Result<RE>::Mul EMul;
+public:
+ typedef typename CNT<R>::template Substitute<EMul>::Type Type;
+ static Type perform(const L& l, const R& r) {return r.scalarMultiplyFromLeft(l);}
+};
+
+ // TODO: NON-CONFORMING DIVIDE
+
+
+} //namespace SimTK
+
+
+#endif // SimTK_SIMMATRIX_RESULT_TYPE_H_
diff --git a/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/Row.h b/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/Row.h
new file mode 100644
index 0000000..ae31d2c
--- /dev/null
+++ b/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/Row.h
@@ -0,0 +1,1192 @@
+#ifndef SimTK_SIMMATRIX_SMALLMATRIX_ROW_H_
+#define SimTK_SIMMATRIX_SMALLMATRIX_ROW_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This file declares class Row<NCOLS, ELEMENT_TYPE, STRIDE>.
+ */
+
+#include "SimTKcommon/internal/common.h"
+
+
+namespace SimTK {
+
+// The following functions are used internally by Row.
+
+namespace Impl {
+
+// For those wimpy compilers that don't unroll short, constant-limit loops,
+// Peter Eastman added these recursive template implementations of
+// elementwise add, subtract, and copy. Sherm added multiply and divide.
+
+template <class E1, int S1, class E2, int S2> void
+conformingAdd(const Row<1,E1,S1>& r1, const Row<1,E2,S2>& r2,
+ Row<1,typename CNT<E1>::template Result<E2>::Add>& result) {
+ result[0] = r1[0] + r2[0];
+}
+template <int N, class E1, int S1, class E2, int S2> void
+conformingAdd(const Row<N,E1,S1>& r1, const Row<N,E2,S2>& r2,
+ Row<N,typename CNT<E1>::template Result<E2>::Add>& result) {
+ conformingAdd(reinterpret_cast<const Row<N-1,E1,S1>&>(r1),
+ reinterpret_cast<const Row<N-1,E2,S2>&>(r2),
+ reinterpret_cast<Row<N-1,typename CNT<E1>::
+ template Result<E2>::Add>&>(result));
+ result[N-1] = r1[N-1] + r2[N-1];
+}
+
+template <class E1, int S1, class E2, int S2> void
+conformingSubtract(const Row<1,E1,S1>& r1, const Row<1,E2,S2>& r2,
+ Row<1,typename CNT<E1>::template Result<E2>::Sub>& result) {
+ result[0] = r1[0] - r2[0];
+}
+template <int N, class E1, int S1, class E2, int S2> void
+conformingSubtract(const Row<N,E1,S1>& r1, const Row<N,E2,S2>& r2,
+ Row<N,typename CNT<E1>::template Result<E2>::Sub>& result) {
+ conformingSubtract(reinterpret_cast<const Row<N-1,E1,S1>&>(r1),
+ reinterpret_cast<const Row<N-1,E2,S2>&>(r2),
+ reinterpret_cast<Row<N-1,typename CNT<E1>::
+ template Result<E2>::Sub>&>(result));
+ result[N-1] = r1[N-1] - r2[N-1];
+}
+
+template <class E1, int S1, class E2, int S2> void
+elementwiseMultiply(const Row<1,E1,S1>& r1, const Row<1,E2,S2>& r2,
+ Row<1,typename CNT<E1>::template Result<E2>::Mul>& result) {
+ result[0] = r1[0] * r2[0];
+}
+template <int N, class E1, int S1, class E2, int S2> void
+elementwiseMultiply(const Row<N,E1,S1>& r1, const Row<N,E2,S2>& r2,
+ Row<N,typename CNT<E1>::template Result<E2>::Mul>& result) {
+ elementwiseMultiply(reinterpret_cast<const Row<N-1,E1,S1>&>(r1),
+ reinterpret_cast<const Row<N-1,E2,S2>&>(r2),
+ reinterpret_cast<Row<N-1,typename CNT<E1>::
+ template Result<E2>::Mul>&>(result));
+ result[N-1] = r1[N-1] * r2[N-1];
+}
+
+template <class E1, int S1, class E2, int S2> void
+elementwiseDivide(const Row<1,E1,S1>& r1, const Row<1,E2,S2>& r2,
+ Row<1,typename CNT<E1>::template Result<E2>::Dvd>& result) {
+ result[0] = r1[0] / r2[0];
+}
+template <int N, class E1, int S1, class E2, int S2> void
+elementwiseDivide(const Row<N,E1,S1>& r1, const Row<N,E2,S2>& r2,
+ Row<N,typename CNT<E1>::template Result<E2>::Dvd>& result) {
+ elementwiseDivide(reinterpret_cast<const Row<N-1,E1,S1>&>(r1),
+ reinterpret_cast<const Row<N-1,E2,S2>&>(r2),
+ reinterpret_cast<Row<N-1,typename CNT<E1>::
+ template Result<E2>::Dvd>&>(result));
+ result[N-1] = r1[N-1] / r2[N-1];
+}
+
+template <class E1, int S1, class E2, int S2> void
+copy(Row<1,E1,S1>& r1, const Row<1,E2,S2>& r2) {
+ r1[0] = r2[0];
+}
+template <int N, class E1, int S1, class E2, int S2> void
+copy(Row<N,E1,S1>& r1, const Row<N,E2,S2>& r2) {
+ copy(reinterpret_cast<Row<N-1,E1,S1>&>(r1),
+ reinterpret_cast<const Row<N-1,E2,S2>&>(r2));
+ r1[N-1] = r2[N-1];
+}
+
+}
+
+/** @brief This is a fixed-length row vector designed for no-overhead inline
+computation.
+
+ at ingroup MatVecUtilities
+
+The %Row type is not commonly used in Simbody user programs; the column vector
+class Vec is much more common. Typically %Row objects arise either from
+transposing a Vec or selecting rows from a Mat.
+
+ at tparam N The number of columns in the row vector.
+ at tparam ELT The element type. Must be a composite numerical type (CNT).
+The default is ELT=Real.
+ at tparam STRIDE The spacing from one element to the next in memory, as an
+integer number of elements of type ELT. The default is STRIDE=1.
+**/
+template <int N, class ELT, int STRIDE> class Row {
+ typedef ELT E;
+ typedef typename CNT<E>::TNeg ENeg;
+ typedef typename CNT<E>::TWithoutNegator EWithoutNegator;
+ typedef typename CNT<E>::TReal EReal;
+ typedef typename CNT<E>::TImag EImag;
+ typedef typename CNT<E>::TComplex EComplex;
+ typedef typename CNT<E>::THerm EHerm;
+ typedef typename CNT<E>::TPosTrans EPosTrans;
+ typedef typename CNT<E>::TSqHermT ESqHermT;
+ typedef typename CNT<E>::TSqTHerm ESqTHerm;
+
+ typedef typename CNT<E>::TSqrt ESqrt;
+ typedef typename CNT<E>::TAbs EAbs;
+ typedef typename CNT<E>::TStandard EStandard;
+ typedef typename CNT<E>::TInvert EInvert;
+ typedef typename CNT<E>::TNormalize ENormalize;
+
+ typedef typename CNT<E>::Scalar EScalar;
+ typedef typename CNT<E>::ULessScalar EULessScalar;
+ typedef typename CNT<E>::Number ENumber;
+ typedef typename CNT<E>::StdNumber EStdNumber;
+ typedef typename CNT<E>::Precision EPrecision;
+ typedef typename CNT<E>::ScalarNormSq EScalarNormSq;
+
+public:
+
+ enum {
+ NRows = 1,
+ NCols = N,
+ NPackedElements = N,
+ NActualElements = N * STRIDE, // includes trailing gap
+ NActualScalars = CNT<E>::NActualScalars * NActualElements,
+ RowSpacing = NActualElements,
+ ColSpacing = STRIDE,
+ ImagOffset = NTraits<ENumber>::ImagOffset,
+ RealStrideFactor = 1, // composite types don't change size when
+ // cast from complex to real or imaginary
+ ArgDepth = ((int)CNT<E>::ArgDepth < (int)MAX_RESOLVED_DEPTH
+ ? CNT<E>::ArgDepth + 1
+ : MAX_RESOLVED_DEPTH),
+ IsScalar = 0,
+ IsULessScalar = 0,
+ IsNumber = 0,
+ IsStdNumber = 0,
+ IsPrecision = 0,
+ SignInterpretation = CNT<E>::SignInterpretation
+ };
+
+ typedef Row<N,E,STRIDE> T;
+ typedef Row<N,ENeg,STRIDE> TNeg;
+ typedef Row<N,EWithoutNegator,STRIDE> TWithoutNegator;
+
+ typedef Row<N,EReal,STRIDE*CNT<E>::RealStrideFactor>
+ TReal;
+ typedef Row<N,EImag,STRIDE*CNT<E>::RealStrideFactor>
+ TImag;
+ typedef Row<N,EComplex,STRIDE> TComplex;
+ typedef Vec<N,EHerm,STRIDE> THerm;
+ typedef Vec<N,E,STRIDE> TPosTrans;
+ typedef E TElement;
+ typedef Row TRow;
+ typedef E TCol;
+
+ // These are the results of calculations, so are returned in new, packed
+ // memory. Be sure to refer to element types here which are also packed.
+ typedef Vec<N,ESqrt,1> TSqrt; // Note stride
+ typedef Row<N,EAbs,1> TAbs; // Note stride
+ typedef Row<N,EStandard,1> TStandard;
+ typedef Vec<N,EInvert,1> TInvert; // packed
+ typedef Row<N,ENormalize,1> TNormalize;
+
+ typedef SymMat<N,ESqHermT> TSqHermT; // result of self outer product
+ typedef EScalarNormSq TSqTHerm; // result of self dot product
+
+ // These recurse right down to the underlying scalar type no matter how
+ // deep the elements are.
+ typedef EScalar Scalar;
+ typedef EULessScalar ULessScalar;
+ typedef ENumber Number;
+ typedef EStdNumber StdNumber;
+ typedef EPrecision Precision;
+ typedef EScalarNormSq ScalarNormSq;
+
+ static int size() { return N; }
+ static int nrow() { return 1; }
+ static int ncol() { return N; }
+
+
+ // Scalar norm square is sum( conjugate squares of all scalars )
+ ScalarNormSq scalarNormSqr() const {
+ ScalarNormSq sum(0);
+ for(int i=0;i<N;++i) sum += CNT<E>::scalarNormSqr(d[i*STRIDE]);
+ return sum;
+ }
+
+ // sqrt() is elementwise square root; that is, the return value has the same
+ // dimension as this Vec but with each element replaced by whatever it thinks
+ // its square root is.
+ TSqrt sqrt() const {
+ TSqrt rsqrt;
+ for(int i=0;i<N;++i) rsqrt[i] = CNT<E>::sqrt(d[i*STRIDE]);
+ return rsqrt;
+ }
+
+ // abs() is elementwise absolute value; that is, the return value has the same
+ // dimension as this Row but with each element replaced by whatever it thinks
+ // its absolute value is.
+ TAbs abs() const {
+ TAbs rabs;
+ for(int i=0;i<N;++i) rabs[i] = CNT<E>::abs(d[i*STRIDE]);
+ return rabs;
+ }
+
+ TStandard standardize() const {
+ TStandard rstd;
+ for(int i=0;i<N;++i) rstd[i] = CNT<E>::standardize(d[i*STRIDE]);
+ return rstd;
+ }
+
+ // Sum just adds up all the elements, getting rid of negators and
+ // conjugates in the process.
+ EStandard sum() const {
+ E sum(0);
+ for (int i=0;i<N;++i) sum += d[i*STRIDE];
+ return CNT<E>::standardize(sum);
+ }
+
+ // This gives the resulting rowvector type when (v[i] op P) is applied to each element of v.
+ // It is a row of length N, stride 1, and element types which are the regular
+ // composite result of E op P. Typically P is a scalar type but it doesn't have to be.
+ template <class P> struct EltResult {
+ typedef Row<N, typename CNT<E>::template Result<P>::Mul, 1> Mul;
+ typedef Row<N, typename CNT<E>::template Result<P>::Dvd, 1> Dvd;
+ typedef Row<N, typename CNT<E>::template Result<P>::Add, 1> Add;
+ typedef Row<N, typename CNT<E>::template Result<P>::Sub, 1> Sub;
+ };
+
+ // This is the composite result for v op P where P is some kind of appropriately shaped
+ // non-scalar type.
+ template <class P> struct Result {
+ typedef MulCNTs<1,N,ArgDepth,Row,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> MulOp;
+ typedef typename MulOp::Type Mul;
+
+ typedef MulCNTsNonConforming<1,N,ArgDepth,Row,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> MulOpNonConforming;
+ typedef typename MulOpNonConforming::Type MulNon;
+
+
+ typedef DvdCNTs<1,N,ArgDepth,Row,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> DvdOp;
+ typedef typename DvdOp::Type Dvd;
+
+ typedef AddCNTs<1,N,ArgDepth,Row,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> AddOp;
+ typedef typename AddOp::Type Add;
+
+ typedef SubCNTs<1,N,ArgDepth,Row,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> SubOp;
+ typedef typename SubOp::Type Sub;
+ };
+
+ // Shape-preserving element substitution (always packed)
+ template <class P> struct Substitute {
+ typedef Row<N,P> Type;
+ };
+
+ // Default construction initializes to NaN when debugging but
+ // is left uninitialized otherwise.
+ Row(){
+ #ifndef NDEBUG
+ setToNaN();
+ #endif
+ }
+
+ // It's important not to use the default copy constructor or copy
+ // assignment because the compiler doesn't understand that we may
+ // have noncontiguous storage and will try to copy the whole array.
+ Row(const Row& src) {
+ Impl::copy(*this, src);
+ }
+ Row& operator=(const Row& src) { // no harm if src and 'this' are the same
+ Impl::copy(*this, src);
+ return *this;
+ }
+
+ // We want an implicit conversion from a Row of the same length
+ // and element type but with a different stride.
+ template <int SS> Row(const Row<N,E,SS>& src) {
+ Impl::copy(*this, src);
+ }
+
+ // We want an implicit conversion from a Row of the same length
+ // and *negated* element type, possibly with a different stride.
+ template <int SS> Row(const Row<N,ENeg,SS>& src) {
+ Impl::copy(*this, src);
+ }
+
+ // Construct a Row from a Row of the same length, with any
+ // stride. Works as long as the element types are compatible.
+ template <class EE, int SS> explicit Row(const Row<N,EE,SS>& vv) {
+ Impl::copy(*this, vv);
+ }
+
+ // Construction using an element assigns to each element.
+ explicit Row(const E& e)
+ { for (int i=0;i<N;++i) d[i*STRIDE]=e; }
+
+ // Construction using a negated element assigns to each element.
+ explicit Row(const ENeg& ne)
+ { for (int i=0;i<N;++i) d[i*STRIDE]=ne; }
+
+ // Given an int, turn it into a suitable floating point number
+ // and then feed that to the above single-element constructor.
+ explicit Row(int i)
+ { new (this) Row(E(Precision(i))); }
+
+ // A bevy of constructors for Rows up to length 6.
+ Row(const E& e0,const E& e1)
+ { assert(N==2);(*this)[0]=e0;(*this)[1]=e1; }
+ Row(const E& e0,const E& e1,const E& e2)
+ { assert(N==3);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2; }
+ Row(const E& e0,const E& e1,const E& e2,const E& e3)
+ { assert(N==4);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2;(*this)[3]=e3; }
+ Row(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4)
+ { assert(N==5);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2;
+ (*this)[3]=e3;(*this)[4]=e4; }
+ Row(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,const E& e5)
+ { assert(N==6);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2;
+ (*this)[3]=e3;(*this)[4]=e4;(*this)[5]=e5; }
+ Row(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,const E& e5,const E& e6)
+ { assert(N==7);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2;
+ (*this)[3]=e3;(*this)[4]=e4;(*this)[5]=e5;(*this)[6]=e6; }
+ Row(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,const E& e5,const E& e6,const E& e7)
+ { assert(N==8);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2;
+ (*this)[3]=e3;(*this)[4]=e4;(*this)[5]=e5;(*this)[6]=e6;(*this)[7]=e7; }
+ Row(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,const E& e5,const E& e6,const E& e7,const E& e8)
+ { assert(N==9);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2;
+ (*this)[3]=e3;(*this)[4]=e4;(*this)[5]=e5;(*this)[6]=e6;(*this)[7]=e7;(*this)[8]=e8; }
+
+ // Construction from a pointer to anything assumes we're pointing
+ // at an element list of the right length.
+ template <class EE> explicit Row(const EE* p)
+ { assert(p); for(int i=0;i<N;++i) d[i*STRIDE]=p[i]; }
+ template <class EE> Row& operator=(const EE* p)
+ { assert(p); for(int i=0;i<N;++i) d[i*STRIDE]=p[i]; return *this; }
+
+ // Conforming assignment ops.
+ template <class EE, int SS> Row& operator=(const Row<N,EE,SS>& vv) {
+ Impl::copy(*this, vv);
+ return *this;
+ }
+ template <class EE, int SS> Row& operator+=(const Row<N,EE,SS>& r)
+ { for(int i=0;i<N;++i) d[i*STRIDE] += r[i]; return *this; }
+ template <class EE, int SS> Row& operator+=(const Row<N,negator<EE>,SS>& r)
+ { for(int i=0;i<N;++i) d[i*STRIDE] -= -(r[i]); return *this; }
+ template <class EE, int SS> Row& operator-=(const Row<N,EE,SS>& r)
+ { for(int i=0;i<N;++i) d[i*STRIDE] -= r[i]; return *this; }
+ template <class EE, int SS> Row& operator-=(const Row<N,negator<EE>,SS>& r)
+ { for(int i=0;i<N;++i) d[i*STRIDE] += -(r[i]); return *this; }
+
+ // Conforming binary ops with 'this' on left, producing new packed result.
+ // Cases: r=r+r, r=r-r, s=r*v r=r*m
+
+ /** Vector addition -- use operator+ instead. **/
+ template <class EE, int SS> Row<N,typename CNT<E>::template Result<EE>::Add>
+ conformingAdd(const Row<N,EE,SS>& r) const {
+ Row<N,typename CNT<E>::template Result<EE>::Add> result;
+ Impl::conformingAdd(*this, r, result);
+ return result;
+ }
+
+ /** Vector subtraction -- use operator- instead. **/
+ template <class EE, int SS> Row<N,typename CNT<E>::template Result<EE>::Sub>
+ conformingSubtract(const Row<N,EE,SS>& r) const {
+ Row<N,typename CNT<E>::template Result<EE>::Sub> result;
+ Impl::conformingSubtract(*this, r, result);
+ return result;
+ }
+
+ /** Same as dot product (s = row*col) -- use operator* or dot() instead. **/
+ template <class EE, int SS> typename CNT<E>::template Result<EE>::Mul
+ conformingMultiply(const Vec<N,EE,SS>& r) const {
+ return (*this)*r;
+ }
+
+ /** Row times a conforming matrix, row=row*mat -- use operator* instead. **/
+ template <int MatNCol, class EE, int CS, int RS>
+ Row<MatNCol,typename CNT<E>::template Result<EE>::Mul>
+ conformingMultiply(const Mat<N,MatNCol,EE,CS,RS>& m) const {
+ Row<MatNCol,typename CNT<E>::template Result<EE>::Mul> result;
+ for (int j=0;j<N;++j) result[j] = conformingMultiply(m(j));
+ return result;
+ }
+
+ /** Elementwise multiply (Matlab .* operator). **/
+ template <class EE, int SS> Row<N,typename CNT<E>::template Result<EE>::Mul>
+ elementwiseMultiply(const Row<N,EE,SS>& r) const {
+ Row<N,typename CNT<E>::template Result<EE>::Mul> result;
+ Impl::elementwiseMultiply(*this, r, result);
+ return result;
+ }
+
+ /** Elementwise divide (Matlab ./ operator). **/
+ template <class EE, int SS> Row<N,typename CNT<E>::template Result<EE>::Dvd>
+ elementwiseDivide(const Row<N,EE,SS>& r) const {
+ Row<N,typename CNT<E>::template Result<EE>::Dvd> result;
+ Impl::elementwiseDivide(*this, r, result);
+ return result;
+ }
+
+ const E& operator[](int i) const { assert(0 <= i && i < N); return d[i*STRIDE]; }
+ E& operator[](int i) { assert(0 <= i && i < N); return d[i*STRIDE]; }
+ const E& operator()(int i) const { return (*this)[i]; }
+ E& operator()(int i) { return (*this)[i]; }
+
+ ScalarNormSq normSqr() const { return scalarNormSqr(); }
+ typename CNT<ScalarNormSq>::TSqrt
+ norm() const { return CNT<ScalarNormSq>::sqrt(scalarNormSqr()); }
+
+ // If the elements of this Row are scalars, the result is what you get by
+ // dividing each element by the norm() calculated above. If the elements are
+ // *not* scalars, then the elements are *separately* normalized. That means
+ // you will get a different answer from Row<2,Row3>::normalize() than you
+ // would from a Row<6>::normalize() containing the same scalars.
+ //
+ // Normalize returns a row of the same dimension but in new, packed storage
+ // and with a return type that does not include negator<> even if the original
+ // Row<> does, because we can eliminate the negation here almost for free.
+ // But we can't standardize (change conjugate to complex) for free, so we'll retain
+ // conjugates if there are any.
+ TNormalize normalize() const {
+ if (CNT<E>::IsScalar) {
+ return castAwayNegatorIfAny() / (SignInterpretation*norm());
+ } else {
+ TNormalize elementwiseNormalized;
+ for (int j=0; j<N; ++j)
+ elementwiseNormalized[j] = CNT<E>::normalize((*this)[j]);
+ return elementwiseNormalized;
+ }
+ }
+
+ TInvert invert() const {assert(false); return TInvert();} // TODO default inversion
+
+ const Row& operator+() const { return *this; }
+ const TNeg& operator-() const { return negate(); }
+ TNeg& operator-() { return updNegate(); }
+ const THerm& operator~() const { return transpose(); }
+ THerm& operator~() { return updTranspose(); }
+
+ const TNeg& negate() const { return *reinterpret_cast<const TNeg*>(this); }
+ TNeg& updNegate() { return *reinterpret_cast<TNeg*>(this); }
+
+ const THerm& transpose() const { return *reinterpret_cast<const THerm*>(this); }
+ THerm& updTranspose() { return *reinterpret_cast<THerm*>(this); }
+
+ const TPosTrans& positionalTranspose() const
+ { return *reinterpret_cast<const TPosTrans*>(this); }
+ TPosTrans& updPositionalTranspose()
+ { return *reinterpret_cast<TPosTrans*>(this); }
+
+ const TReal& real() const { return *reinterpret_cast<const TReal*>(this); }
+ TReal& real() { return *reinterpret_cast< TReal*>(this); }
+
+ // Had to contort these routines to get them through VC++ 7.net
+ const TImag& imag() const {
+ const int offs = ImagOffset;
+ const EImag* p = reinterpret_cast<const EImag*>(this);
+ return *reinterpret_cast<const TImag*>(p+offs);
+ }
+ TImag& imag() {
+ const int offs = ImagOffset;
+ EImag* p = reinterpret_cast<EImag*>(this);
+ return *reinterpret_cast<TImag*>(p+offs);
+ }
+
+ const TWithoutNegator& castAwayNegatorIfAny() const {return *reinterpret_cast<const TWithoutNegator*>(this);}
+ TWithoutNegator& updCastAwayNegatorIfAny() {return *reinterpret_cast<TWithoutNegator*>(this);}
+
+
+ // These are elementwise binary operators, (this op ee) by default but
+ // (ee op this) if 'FromLeft' appears in the name. The result is a packed
+ // Row<N> but the element type may change. These are mostly used to
+ // implement global operators. We call these "scalar" operators but
+ // actually the "scalar" can be a composite type.
+
+ //TODO: consider converting 'e' to Standard Numbers as precalculation and
+ // changing return type appropriately.
+ template <class EE> Row<N, typename CNT<E>::template Result<EE>::Mul>
+ scalarMultiply(const EE& e) const {
+ Row<N, typename CNT<E>::template Result<EE>::Mul> result;
+ for (int j=0; j<N; ++j) result[j] = (*this)[j] * e;
+ return result;
+ }
+ template <class EE> Row<N, typename CNT<EE>::template Result<E>::Mul>
+ scalarMultiplyFromLeft(const EE& e) const {
+ Row<N, typename CNT<EE>::template Result<E>::Mul> result;
+ for (int j=0; j<N; ++j) result[j] = e * (*this)[j];
+ return result;
+ }
+
+ // TODO: should precalculate and store 1/e, while converting to Standard
+ // Numbers. Note that return type should change appropriately.
+ template <class EE> Row<N, typename CNT<E>::template Result<EE>::Dvd>
+ scalarDivide(const EE& e) const {
+ Row<N, typename CNT<E>::template Result<EE>::Dvd> result;
+ for (int j=0; j<N; ++j) result[j] = (*this)[j] / e;
+ return result;
+ }
+ template <class EE> Row<N, typename CNT<EE>::template Result<E>::Dvd>
+ scalarDivideFromLeft(const EE& e) const {
+ Row<N, typename CNT<EE>::template Result<E>::Dvd> result;
+ for (int j=0; j<N; ++j) result[j] = e / (*this)[j];
+ return result;
+ }
+
+ template <class EE> Row<N, typename CNT<E>::template Result<EE>::Add>
+ scalarAdd(const EE& e) const {
+ Row<N, typename CNT<E>::template Result<EE>::Add> result;
+ for (int j=0; j<N; ++j) result[j] = (*this)[j] + e;
+ return result;
+ }
+ // Add is commutative, so no 'FromLeft'.
+
+ template <class EE> Row<N, typename CNT<E>::template Result<EE>::Sub>
+ scalarSubtract(const EE& e) const {
+ Row<N, typename CNT<E>::template Result<EE>::Sub> result;
+ for (int j=0; j<N; ++j) result[j] = (*this)[j] - e;
+ return result;
+ }
+ template <class EE> Row<N, typename CNT<EE>::template Result<E>::Sub>
+ scalarSubtractFromLeft(const EE& e) const {
+ Row<N, typename CNT<EE>::template Result<E>::Sub> result;
+ for (int j=0; j<N; ++j) result[j] = e - (*this)[j];
+ return result;
+ }
+
+ // Generic assignments for any element type not listed explicitly, including scalars.
+ // These are done repeatedly for each element and only work if the operation can
+ // be performed leaving the original element type.
+ template <class EE> Row& operator =(const EE& e) {return scalarEq(e);}
+ template <class EE> Row& operator+=(const EE& e) {return scalarPlusEq(e);}
+ template <class EE> Row& operator-=(const EE& e) {return scalarMinusEq(e);}
+ template <class EE> Row& operator*=(const EE& e) {return scalarTimesEq(e);}
+ template <class EE> Row& operator/=(const EE& e) {return scalarDivideEq(e);}
+
+ // Generalized scalar assignment & computed assignment methods. These will work
+ // for any assignment-compatible element, not just scalars.
+ template <class EE> Row& scalarEq(const EE& ee)
+ { for(int i=0;i<N;++i) d[i*STRIDE] = ee; return *this; }
+ template <class EE> Row& scalarPlusEq(const EE& ee)
+ { for(int i=0;i<N;++i) d[i*STRIDE] += ee; return *this; }
+ template <class EE> Row& scalarMinusEq(const EE& ee)
+ { for(int i=0;i<N;++i) d[i*STRIDE] -= ee; return *this; }
+ template <class EE> Row& scalarMinusEqFromLeft(const EE& ee)
+ { for(int i=0;i<N;++i) d[i*STRIDE] = ee - d[i*STRIDE]; return *this; }
+ template <class EE> Row& scalarTimesEq(const EE& ee)
+ { for(int i=0;i<N;++i) d[i*STRIDE] *= ee; return *this; }
+ template <class EE> Row& scalarTimesEqFromLeft(const EE& ee)
+ { for(int i=0;i<N;++i) d[i*STRIDE] = ee * d[i*STRIDE]; return *this; }
+ template <class EE> Row& scalarDivideEq(const EE& ee)
+ { for(int i=0;i<N;++i) d[i*STRIDE] /= ee; return *this; }
+ template <class EE> Row& scalarDivideEqFromLeft(const EE& ee)
+ { for(int i=0;i<N;++i) d[i*STRIDE] = ee / d[i*STRIDE]; return *this; }
+
+
+ // Specialize for int to avoid warnings and ambiguities.
+ Row& scalarEq(int ee) {return scalarEq(Precision(ee));}
+ Row& scalarPlusEq(int ee) {return scalarPlusEq(Precision(ee));}
+ Row& scalarMinusEq(int ee) {return scalarMinusEq(Precision(ee));}
+ Row& scalarTimesEq(int ee) {return scalarTimesEq(Precision(ee));}
+ Row& scalarDivideEq(int ee) {return scalarDivideEq(Precision(ee));}
+ Row& scalarMinusEqFromLeft(int ee) {return scalarMinusEqFromLeft(Precision(ee));}
+ Row& scalarTimesEqFromLeft(int ee) {return scalarTimesEqFromLeft(Precision(ee));}
+ Row& scalarDivideEqFromLeft(int ee) {return scalarDivideEqFromLeft(Precision(ee));}
+
+ /** Set every scalar in this %Row to NaN; this is the default initial
+ value in Debug builds, but not in Release. **/
+ void setToNaN() {
+ (*this) = CNT<ELT>::getNaN();
+ }
+
+ /** Set every scalar in this %Row to zero. **/
+ void setToZero() {
+ (*this) = ELT(0);
+ }
+
+ /** Extract a const reference to a sub-Row with size known at compile time.
+ This must be called with an explicit template argument for the size, for
+ example, getSubRow<3>(j). This is only a recast; no copying or computation
+ is performed. The size and index are range checked in Debug builds but
+ not in Release builds. **/
+ template <int NN>
+ const Row<NN,ELT,STRIDE>& getSubRow(int j) const {
+ assert(0 <= j && j + NN <= N);
+ return Row<NN,ELT,STRIDE>::getAs(&(*this)[j]);
+ }
+ /** Extract a writable reference to a sub-Row with size known at compile time.
+ This must be called with an explicit template argument for the size, for
+ example, updSubRow<3>(j). This is only a recast; no copying or computation
+ is performed. The size and index are range checked in Debug builds but
+ not in Release builds. **/
+ template <int NN>
+ Row<NN,ELT,STRIDE>& updSubRow(int j) {
+ assert(0 <= j && j + NN <= N);
+ return Row<NN,ELT,STRIDE>::updAs(&(*this)[j]);
+ }
+
+ /** Extract a subvector of type %Row from a longer one that has the same
+ element type and stride, and return a const reference to the selected
+ subsequence. **/
+ template <int NN>
+ static const Row& getSubRow(const Row<NN,ELT,STRIDE>& r, int j) {
+ assert(0 <= j && j + N <= NN);
+ return getAs(&r[j]);
+ }
+ /** Extract a subvector of type %Row from a longer one that has the same
+ element type and stride, and return a writable reference to the selected
+ subsequence. **/
+ template <int NN>
+ static Row& updSubRow(Row<NN,ELT,STRIDE>& r, int j) {
+ assert(0 <= j && j + N <= NN);
+ return updAs(&r[j]);
+ }
+
+ /** Return a row one smaller than this one by dropping the element
+ at the indicated position p. The result is a packed copy with the same
+ element type as this one. **/
+ Row<N-1,ELT,1> drop1(int p) const {
+ assert(0 <= p && p < N);
+ Row<N-1,ELT,1> out;
+ int nxt=0;
+ for (int i=0; i<N-1; ++i, ++nxt) {
+ if (nxt==p) ++nxt; // skip the loser
+ out[i] = (*this)[nxt];
+ }
+ return out;
+ }
+
+ /** Return a row one larger than this one by adding an element
+ to the end. The result is a packed copy with the same element type as
+ this one. Works for any assignment compatible element. **/
+ template <class EE> Row<N+1,ELT,1> append1(const EE& v) const {
+ Row<N+1,ELT,1> out;
+ Row<N,ELT,1>::updAs(&out[0]) = (*this);
+ out[N] = v;
+ return out;
+ }
+
+
+ /** Return a row one larger than this one by inserting an element
+ \e before the indicated one. The result is a packed copy with the same
+ element type as this one. Works for any assignment compatible element. The
+ index can be one greater than normally allowed in which case the element
+ is appended (but use append1() if you know you're appending). **/
+ template <class EE> Row<N+1,ELT,1> insert1(int p, const EE& v) const {
+ assert(0 <= p && p <= N);
+ if (p==N) return append1(v);
+ Row<N+1,ELT,1> out;
+ int nxt=0;
+ for (int i=0; i<N; ++i, ++nxt) {
+ if (i==p) out[nxt++] = v;
+ out[nxt] = (*this)[i];
+ }
+ return out;
+ }
+
+ /** Recast an ordinary C++ array E[] to a const %Row<N,E,S>; assumes
+ compatible length, stride, and packing. **/
+ static const Row& getAs(const ELT* p) {return *reinterpret_cast<const Row*>(p);}
+ /** Recast a writable ordinary C++ array E[] to a writable %Row<N,E,S>;
+ assumes compatible length, stride, and packing. **/
+ static Row& updAs(ELT* p) {return *reinterpret_cast<Row*>(p);}
+
+ /** Return a %Row of the same length and element type as this one but
+ with all elements set to NaN. The result is packed (stride==1) regardless
+ of the stride of this %Row. **/
+ static Row<N,ELT,1> getNaN() { return Row<N,ELT,1>(CNT<ELT>::getNaN()); }
+
+ /** Return true if any element of this Row contains a NaN anywhere. **/
+ bool isNaN() const {
+ for (int j=0; j<N; ++j)
+ if (CNT<ELT>::isNaN((*this)[j]))
+ return true;
+ return false;
+ }
+
+ /** Return true if any element of this Row contains a +Infinity
+ or -Infinity somewhere but no element contains a NaN anywhere. **/
+ bool isInf() const {
+ bool seenInf = false;
+ for (int j=0; j<N; ++j) {
+ const ELT& e = (*this)[j];
+ if (!CNT<ELT>::isFinite(e)) {
+ if (!CNT<ELT>::isInf(e))
+ return false; // something bad was found
+ seenInf = true;
+ }
+ }
+ return seenInf;
+ }
+
+ /** Return true if no element of this %Row contains an Infinity or a NaN
+ anywhere. **/
+ bool isFinite() const {
+ for (int j=0; j<N; ++j)
+ if (!CNT<ELT>::isFinite((*this)[j]))
+ return false;
+ return true;
+ }
+
+ /** For approximate comparisions, the default tolerance to use for a vector is
+ the same as its elements' default tolerance. **/
+ static double getDefaultTolerance() {return CNT<ELT>::getDefaultTolerance();}
+
+ /** %Test whether this row is numerically equal to some other row with
+ the same shape, using a specified tolerance. **/
+ template <class E2, int CS2>
+ bool isNumericallyEqual(const Row<N,E2,CS2>& r, double tol) const {
+ for (int j=0; j<N; ++j)
+ if (!CNT<ELT>::isNumericallyEqual((*this)(j), r(j), tol))
+ return false;
+ return true;
+ }
+
+ /** %Test whether this row vector is numerically equal to some other row with
+ the same shape, using a default tolerance which is the looser of the
+ default tolerances of the two objects being compared. **/
+ template <class E2, int CS2>
+ bool isNumericallyEqual(const Row<N,E2,CS2>& r) const {
+ const double tol = std::max(getDefaultTolerance(),r.getDefaultTolerance());
+ return isNumericallyEqual(r, tol);
+ }
+
+ /** %Test whether every element of this row vector is numerically equal to
+ the given element, using either a specified tolerance or the row's
+ default tolerance (which is always the same or looser than the default
+ tolerance for one of its elements). **/
+ bool isNumericallyEqual
+ (const ELT& e,
+ double tol = getDefaultTolerance()) const
+ {
+ for (int j=0; j<N; ++j)
+ if (!CNT<ELT>::isNumericallyEqual((*this)(j), e, tol))
+ return false;
+ return true;
+ }
+private:
+ ELT d[NActualElements]; // data
+};
+
+/////////////////////////////////////////////
+// Global operators involving two rows. //
+// v+v, v-v, v==v, v!=v //
+/////////////////////////////////////////////
+
+// v3 = v1 + v2 where all v's have the same length N.
+template <int N, class E1, int S1, class E2, int S2> inline
+typename Row<N,E1,S1>::template Result< Row<N,E2,S2> >::Add
+operator+(const Row<N,E1,S1>& l, const Row<N,E2,S2>& r) {
+ return Row<N,E1,S1>::template Result< Row<N,E2,S2> >
+ ::AddOp::perform(l,r);
+}
+
+// v3 = v1 - v2, similar to +
+template <int N, class E1, int S1, class E2, int S2> inline
+typename Row<N,E1,S1>::template Result< Row<N,E2,S2> >::Sub
+operator-(const Row<N,E1,S1>& l, const Row<N,E2,S2>& r) {
+ return Row<N,E1,S1>::template Result< Row<N,E2,S2> >
+ ::SubOp::perform(l,r);
+}
+
+/// bool = v1[i] == v2[i], for all elements i
+template <int N, class E1, int S1, class E2, int S2> inline bool
+operator==(const Row<N,E1,S1>& l, const Row<N,E2,S2>& r) {
+ for (int i=0; i < N; ++i) if (l[i] != r[i]) return false;
+ return true;
+}
+/// bool = v1[i] != v2[i], for any element i
+template <int N, class E1, int S1, class E2, int S2> inline bool
+operator!=(const Row<N,E1,S1>& l, const Row<N,E2,S2>& r) {return !(l==r);}
+
+/// bool = v1[i] < v2[i], for all elements i
+template <int N, class E1, int S1, class E2, int S2> inline bool
+operator<(const Row<N,E1,S1>& l, const Row<N,E2,S2>& r)
+{ for (int i=0; i < N; ++i) if (l[i] >= r[i]) return false;
+ return true; }
+/// bool = v[i] < e, for all elements v[i] and element e
+template <int N, class E1, int S1, class E2> inline bool
+operator<(const Row<N,E1,S1>& v, const E2& e)
+{ for (int i=0; i < N; ++i) if (v[i] >= e) return false;
+ return true; }
+
+/// bool = v1[i] > v2[i], for all elements i
+template <int N, class E1, int S1, class E2, int S2> inline bool
+operator>(const Row<N,E1,S1>& l, const Row<N,E2,S2>& r)
+{ for (int i=0; i < N; ++i) if (l[i] <= r[i]) return false;
+ return true; }
+/// bool = v[i] > e, for all elements v[i] and element e
+template <int N, class E1, int S1, class E2> inline bool
+operator>(const Row<N,E1,S1>& v, const E2& e)
+{ for (int i=0; i < N; ++i) if (v[i] <= e) return false;
+ return true; }
+
+/// bool = v1[i] <= v2[i], for all elements i.
+/// This is not the same as !(v1>v2).
+template <int N, class E1, int S1, class E2, int S2> inline bool
+operator<=(const Row<N,E1,S1>& l, const Row<N,E2,S2>& r)
+{ for (int i=0; i < N; ++i) if (l[i] > r[i]) return false;
+ return true; }
+/// bool = v[i] <= e, for all elements v[i] and element e.
+/// This is not the same as !(v1>e).
+template <int N, class E1, int S1, class E2> inline bool
+operator<=(const Row<N,E1,S1>& v, const E2& e)
+{ for (int i=0; i < N; ++i) if (v[i] > e) return false;
+ return true; }
+
+/// bool = v1[i] >= v2[i], for all elements i
+/// This is not the same as !(v1<v2).
+template <int N, class E1, int S1, class E2, int S2> inline bool
+operator>=(const Row<N,E1,S1>& l, const Row<N,E2,S2>& r)
+{ for (int i=0; i < N; ++i) if (l[i] < r[i]) return false;
+ return true; }
+/// bool = v[i] >= e, for all elements v[i] and element e.
+/// This is not the same as !(v1<e).
+template <int N, class E1, int S1, class E2> inline bool
+operator>=(const Row<N,E1,S1>& v, const E2& e)
+{ for (int i=0; i < N; ++i) if (v[i] < e) return false;
+ return true; }
+
+////////////////////////////////////////////////////
+// Global operators involving a row and a scalar. //
+////////////////////////////////////////////////////
+
+// I haven't been able to figure out a nice way to templatize for the
+// built-in reals without introducing a lot of unwanted type matches
+// as well. So we'll just grind them out explicitly here.
+
+// SCALAR MULTIPLY
+
+// v = v*real, real*v
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<float>::Mul
+operator*(const Row<N,E,S>& l, const float& r)
+ { return Row<N,E,S>::template Result<float>::MulOp::perform(l,r); }
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<float>::Mul
+operator*(const float& l, const Row<N,E,S>& r) {return r*l;}
+
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<double>::Mul
+operator*(const Row<N,E,S>& l, const double& r)
+ { return Row<N,E,S>::template Result<double>::MulOp::perform(l,r); }
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<double>::Mul
+operator*(const double& l, const Row<N,E,S>& r) {return r*l;}
+
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<long double>::Mul
+operator*(const Row<N,E,S>& l, const long double& r)
+ { return Row<N,E,S>::template Result<long double>::MulOp::perform(l,r); }
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<long double>::Mul
+operator*(const long double& l, const Row<N,E,S>& r) {return r*l;}
+
+// v = v*int, int*v -- just convert int to v's precision float
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<typename CNT<E>::Precision>::Mul
+operator*(const Row<N,E,S>& l, int r) {return l * (typename CNT<E>::Precision)r;}
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<typename CNT<E>::Precision>::Mul
+operator*(int l, const Row<N,E,S>& r) {return r * (typename CNT<E>::Precision)l;}
+
+// Complex, conjugate, and negator are all easy to templatize.
+
+// v = v*complex, complex*v
+template <int N, class E, int S, class R> inline
+typename Row<N,E,S>::template Result<std::complex<R> >::Mul
+operator*(const Row<N,E,S>& l, const std::complex<R>& r)
+ { return Row<N,E,S>::template Result<std::complex<R> >::MulOp::perform(l,r); }
+template <int N, class E, int S, class R> inline
+typename Row<N,E,S>::template Result<std::complex<R> >::Mul
+operator*(const std::complex<R>& l, const Row<N,E,S>& r) {return r*l;}
+
+// v = v*conjugate, conjugate*v (convert conjugate->complex)
+template <int N, class E, int S, class R> inline
+typename Row<N,E,S>::template Result<std::complex<R> >::Mul
+operator*(const Row<N,E,S>& l, const conjugate<R>& r) {return l*(std::complex<R>)r;}
+template <int N, class E, int S, class R> inline
+typename Row<N,E,S>::template Result<std::complex<R> >::Mul
+operator*(const conjugate<R>& l, const Row<N,E,S>& r) {return r*(std::complex<R>)l;}
+
+// v = v*negator, negator*v: convert negator to standard number
+template <int N, class E, int S, class R> inline
+typename Row<N,E,S>::template Result<typename negator<R>::StdNumber>::Mul
+operator*(const Row<N,E,S>& l, const negator<R>& r) {return l * (typename negator<R>::StdNumber)(R)r;}
+template <int N, class E, int S, class R> inline
+typename Row<N,E,S>::template Result<typename negator<R>::StdNumber>::Mul
+operator*(const negator<R>& l, const Row<N,E,S>& r) {return r * (typename negator<R>::StdNumber)(R)l;}
+
+
+// SCALAR DIVIDE. This is a scalar operation when the scalar is on the right,
+// but when it is on the left it means scalar * pseudoInverse(row), which is
+// a vec.
+
+// v = v/real, real/v
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<float>::Dvd
+operator/(const Row<N,E,S>& l, const float& r)
+ { return Row<N,E,S>::template Result<float>::DvdOp::perform(l,r); }
+template <int N, class E, int S> inline
+typename CNT<float>::template Result<Row<N,E,S> >::Dvd
+operator/(const float& l, const Row<N,E,S>& r)
+ { return CNT<float>::template Result<Row<N,E,S> >::DvdOp::perform(l,r); }
+
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<double>::Dvd
+operator/(const Row<N,E,S>& l, const double& r)
+ { return Row<N,E,S>::template Result<double>::DvdOp::perform(l,r); }
+template <int N, class E, int S> inline
+typename CNT<double>::template Result<Row<N,E,S> >::Dvd
+operator/(const double& l, const Row<N,E,S>& r)
+ { return CNT<double>::template Result<Row<N,E,S> >::DvdOp::perform(l,r); }
+
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<long double>::Dvd
+operator/(const Row<N,E,S>& l, const long double& r)
+ { return Row<N,E,S>::template Result<long double>::DvdOp::perform(l,r); }
+template <int N, class E, int S> inline
+typename CNT<long double>::template Result<Row<N,E,S> >::Dvd
+operator/(const long double& l, const Row<N,E,S>& r)
+ { return CNT<long double>::template Result<Row<N,E,S> >::DvdOp::perform(l,r); }
+
+// v = v/int, int/v -- just convert int to v's precision float
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<typename CNT<E>::Precision>::Dvd
+operator/(const Row<N,E,S>& l, int r) {return l / (typename CNT<E>::Precision)r;}
+template <int N, class E, int S> inline
+typename CNT<typename CNT<E>::Precision>::template Result<Row<N,E,S> >::Dvd
+operator/(int l, const Row<N,E,S>& r) {return (typename CNT<E>::Precision)l / r;}
+
+
+// Complex, conjugate, and negator are all easy to templatize.
+
+// v = v/complex, complex/v
+template <int N, class E, int S, class R> inline
+typename Row<N,E,S>::template Result<std::complex<R> >::Dvd
+operator/(const Row<N,E,S>& l, const std::complex<R>& r)
+ { return Row<N,E,S>::template Result<std::complex<R> >::DvdOp::perform(l,r); }
+template <int N, class E, int S, class R> inline
+typename CNT<std::complex<R> >::template Result<Row<N,E,S> >::Dvd
+operator/(const std::complex<R>& l, const Row<N,E,S>& r)
+ { return CNT<std::complex<R> >::template Result<Row<N,E,S> >::DvdOp::perform(l,r); }
+
+// v = v/conjugate, conjugate/v (convert conjugate->complex)
+template <int N, class E, int S, class R> inline
+typename Row<N,E,S>::template Result<std::complex<R> >::Dvd
+operator/(const Row<N,E,S>& l, const conjugate<R>& r) {return l/(std::complex<R>)r;}
+template <int N, class E, int S, class R> inline
+typename CNT<std::complex<R> >::template Result<Row<N,E,S> >::Dvd
+operator/(const conjugate<R>& l, const Row<N,E,S>& r) {return (std::complex<R>)l/r;}
+
+// v = v/negator, negator/v: convert negator to number
+template <int N, class E, int S, class R> inline
+typename Row<N,E,S>::template Result<typename negator<R>::StdNumber>::Dvd
+operator/(const Row<N,E,S>& l, const negator<R>& r) {return l/(typename negator<R>::StdNumber)(R)r;}
+template <int N, class E, int S, class R> inline
+typename CNT<R>::template Result<Row<N,E,S> >::Dvd
+operator/(const negator<R>& l, const Row<N,E,S>& r) {return (typename negator<R>::StdNumber)(R)l/r;}
+
+
+// Add and subtract are odd as scalar ops. They behave as though the
+// scalar stands for a vector each of whose elements is that scalar,
+// and then a normal vector add or subtract is done.
+
+// SCALAR ADD
+
+// v = v+real, real+v
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<float>::Add
+operator+(const Row<N,E,S>& l, const float& r)
+ { return Row<N,E,S>::template Result<float>::AddOp::perform(l,r); }
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<float>::Add
+operator+(const float& l, const Row<N,E,S>& r) {return r+l;}
+
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<double>::Add
+operator+(const Row<N,E,S>& l, const double& r)
+ { return Row<N,E,S>::template Result<double>::AddOp::perform(l,r); }
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<double>::Add
+operator+(const double& l, const Row<N,E,S>& r) {return r+l;}
+
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<long double>::Add
+operator+(const Row<N,E,S>& l, const long double& r)
+ { return Row<N,E,S>::template Result<long double>::AddOp::perform(l,r); }
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<long double>::Add
+operator+(const long double& l, const Row<N,E,S>& r) {return r+l;}
+
+// v = v+int, int+v -- just convert int to v's precision float
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<typename CNT<E>::Precision>::Add
+operator+(const Row<N,E,S>& l, int r) {return l + (typename CNT<E>::Precision)r;}
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<typename CNT<E>::Precision>::Add
+operator+(int l, const Row<N,E,S>& r) {return r + (typename CNT<E>::Precision)l;}
+
+// Complex, conjugate, and negator are all easy to templatize.
+
+// v = v+complex, complex+v
+template <int N, class E, int S, class R> inline
+typename Row<N,E,S>::template Result<std::complex<R> >::Add
+operator+(const Row<N,E,S>& l, const std::complex<R>& r)
+ { return Row<N,E,S>::template Result<std::complex<R> >::AddOp::perform(l,r); }
+template <int N, class E, int S, class R> inline
+typename Row<N,E,S>::template Result<std::complex<R> >::Add
+operator+(const std::complex<R>& l, const Row<N,E,S>& r) {return r+l;}
+
+// v = v+conjugate, conjugate+v (convert conjugate->complex)
+template <int N, class E, int S, class R> inline
+typename Row<N,E,S>::template Result<std::complex<R> >::Add
+operator+(const Row<N,E,S>& l, const conjugate<R>& r) {return l+(std::complex<R>)r;}
+template <int N, class E, int S, class R> inline
+typename Row<N,E,S>::template Result<std::complex<R> >::Add
+operator+(const conjugate<R>& l, const Row<N,E,S>& r) {return r+(std::complex<R>)l;}
+
+// v = v+negator, negator+v: convert negator to standard number
+template <int N, class E, int S, class R> inline
+typename Row<N,E,S>::template Result<typename negator<R>::StdNumber>::Add
+operator+(const Row<N,E,S>& l, const negator<R>& r) {return l + (typename negator<R>::StdNumber)(R)r;}
+template <int N, class E, int S, class R> inline
+typename Row<N,E,S>::template Result<typename negator<R>::StdNumber>::Add
+operator+(const negator<R>& l, const Row<N,E,S>& r) {return r + (typename negator<R>::StdNumber)(R)l;}
+
+// SCALAR SUBTRACT -- careful, not commutative.
+
+// v = v-real, real-v
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<float>::Sub
+operator-(const Row<N,E,S>& l, const float& r)
+ { return Row<N,E,S>::template Result<float>::SubOp::perform(l,r); }
+template <int N, class E, int S> inline
+typename CNT<float>::template Result<Row<N,E,S> >::Sub
+operator-(const float& l, const Row<N,E,S>& r)
+ { return CNT<float>::template Result<Row<N,E,S> >::SubOp::perform(l,r); }
+
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<double>::Sub
+operator-(const Row<N,E,S>& l, const double& r)
+ { return Row<N,E,S>::template Result<double>::SubOp::perform(l,r); }
+template <int N, class E, int S> inline
+typename CNT<double>::template Result<Row<N,E,S> >::Sub
+operator-(const double& l, const Row<N,E,S>& r)
+ { return CNT<double>::template Result<Row<N,E,S> >::SubOp::perform(l,r); }
+
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<long double>::Sub
+operator-(const Row<N,E,S>& l, const long double& r)
+ { return Row<N,E,S>::template Result<long double>::SubOp::perform(l,r); }
+template <int N, class E, int S> inline
+typename CNT<long double>::template Result<Row<N,E,S> >::Sub
+operator-(const long double& l, const Row<N,E,S>& r)
+ { return CNT<long double>::template Result<Row<N,E,S> >::SubOp::perform(l,r); }
+
+// v = v-int, int-v // just convert int to v's precision float
+template <int N, class E, int S> inline
+typename Row<N,E,S>::template Result<typename CNT<E>::Precision>::Sub
+operator-(const Row<N,E,S>& l, int r) {return l - (typename CNT<E>::Precision)r;}
+template <int N, class E, int S> inline
+typename CNT<typename CNT<E>::Precision>::template Result<Row<N,E,S> >::Sub
+operator-(int l, const Row<N,E,S>& r) {return (typename CNT<E>::Precision)l - r;}
+
+
+// Complex, conjugate, and negator are all easy to templatize.
+
+// v = v-complex, complex-v
+template <int N, class E, int S, class R> inline
+typename Row<N,E,S>::template Result<std::complex<R> >::Sub
+operator-(const Row<N,E,S>& l, const std::complex<R>& r)
+ { return Row<N,E,S>::template Result<std::complex<R> >::SubOp::perform(l,r); }
+template <int N, class E, int S, class R> inline
+typename CNT<std::complex<R> >::template Result<Row<N,E,S> >::Sub
+operator-(const std::complex<R>& l, const Row<N,E,S>& r)
+ { return CNT<std::complex<R> >::template Result<Row<N,E,S> >::SubOp::perform(l,r); }
+
+// v = v-conjugate, conjugate-v (convert conjugate->complex)
+template <int N, class E, int S, class R> inline
+typename Row<N,E,S>::template Result<std::complex<R> >::Sub
+operator-(const Row<N,E,S>& l, const conjugate<R>& r) {return l-(std::complex<R>)r;}
+template <int N, class E, int S, class R> inline
+typename CNT<std::complex<R> >::template Result<Row<N,E,S> >::Sub
+operator-(const conjugate<R>& l, const Row<N,E,S>& r) {return (std::complex<R>)l-r;}
+
+// v = v-negator, negator-v: convert negator to standard number
+template <int N, class E, int S, class R> inline
+typename Row<N,E,S>::template Result<typename negator<R>::StdNumber>::Sub
+operator-(const Row<N,E,S>& l, const negator<R>& r) {return l-(typename negator<R>::StdNumber)(R)r;}
+template <int N, class E, int S, class R> inline
+typename CNT<R>::template Result<Row<N,E,S> >::Sub
+operator-(const negator<R>& l, const Row<N,E,S>& r) {return (typename negator<R>::StdNumber)(R)l-r;}
+
+
+// Row I/O
+template <int N, class E, int S, class CHAR, class TRAITS> inline
+std::basic_ostream<CHAR,TRAITS>&
+operator<<(std::basic_ostream<CHAR,TRAITS>& o, const Row<N,E,S>& v) {
+ o << "[" << v[0]; for(int i=1;i<N;++i) o<<','<<v[i]; o<<']'; return o;
+}
+
+/** Read a Row from a stream as M elements separated by white space or
+by commas, optionally enclosed in () or [] (but no leading "~"). **/
+template <int N, class E, int S, class CHAR, class TRAITS> inline
+std::basic_istream<CHAR,TRAITS>&
+operator>>(std::basic_istream<CHAR,TRAITS>& is, Row<N,E,S>& v) {
+ CHAR openBracket, closeBracket;
+ is >> openBracket; if (is.fail()) return is;
+ if (openBracket==CHAR('('))
+ closeBracket = CHAR(')');
+ else if (openBracket==CHAR('['))
+ closeBracket = CHAR(']');
+ else {
+ closeBracket = CHAR(0);
+ is.unget(); if (is.fail()) return is;
+ }
+
+ for (int i=0; i < N; ++i) {
+ is >> v[i];
+ if (is.fail()) return is;
+ if (i != N-1) {
+ CHAR c; is >> c; if (is.fail()) return is;
+ if (c != ',') is.unget();
+ if (is.fail()) return is;
+ }
+ }
+
+ // Get the closing bracket if there was an opening one. If we don't
+ // see the expected character we'll set the fail bit in the istream.
+ if (closeBracket != CHAR(0)) {
+ CHAR closer; is >> closer; if (is.fail()) return is;
+ if (closer != closeBracket) {
+ is.unget(); if (is.fail()) return is;
+ is.setstate( std::ios::failbit );
+ }
+ }
+
+ return is;
+}
+
+} //namespace SimTK
+
+
+#endif //SimTK_SIMMATRIX_SMALLMATRIX_ROW_H_
diff --git a/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/SmallDefsThatNeedBig.h b/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/SmallDefsThatNeedBig.h
new file mode 100644
index 0000000..a497496
--- /dev/null
+++ b/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/SmallDefsThatNeedBig.h
@@ -0,0 +1,41 @@
+#ifndef SimTK_SIMMATRIX_SMALL_DEFS_THAT_NEED_BIG_H_
+#define SimTK_SIMMATRIX_SMALL_DEFS_THAT_NEED_BIG_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This file defines leftover SmallMatrix implementations which need to know
+ * about our BigMatrix classes.
+ */
+
+
+namespace SimTK {
+
+ // none currently
+
+
+} //namespace SimTK
+
+
+#endif // SimTK_SIMMATRIX_SMALL_DEFS_THAT_NEED_BIG_H_
diff --git a/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/SmallMatrixMixed.h b/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/SmallMatrixMixed.h
new file mode 100644
index 0000000..fe8afdb
--- /dev/null
+++ b/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/SmallMatrixMixed.h
@@ -0,0 +1,1010 @@
+#ifndef SimTK_SIMMATRIX_SMALLMATRIX_MIXED_H_
+#define SimTK_SIMMATRIX_SMALLMATRIX_MIXED_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This file defines global functions and class members which use a mix
+ * of Vec, Row, and Mat types and hence need to wait until everything is
+ * defined. Some of them may depend on Lapack also.
+ */
+
+namespace SimTK {
+
+ // COMPARISON
+
+// m==s
+template <int M, class EL, int CSL, int RSL, class ER, int RSR> inline
+bool operator==(const Mat<M,M,EL,CSL,RSL>& l, const SymMat<M,ER,RSR>& r) {
+ for (int i=0; i<M; ++i) {
+ if (l(i,i) != r.getDiag()[i]) return false;
+ for (int j=0; j<i; ++j)
+ if (l(i,j) != r.getEltLower(i,j)) return false;
+ for (int j=i+1; j<M; ++j)
+ if (l(i,j) != r.getEltUpper(i,j)) return false;
+ }
+
+ return true;
+}
+// m!=s
+template <int M, class EL, int CSL, int RSL, class ER, int RSR> inline
+bool operator!=(const Mat<M,M,EL,CSL,RSL>& l, const SymMat<M,ER,RSR>& r) {
+ return !(l==r);
+}
+
+// s==m
+template <int M, class EL, int RSL, class ER, int CSR, int RSR> inline
+bool operator==(const SymMat<M,EL,RSL>& l, const Mat<M,M,ER,CSR,RSR>& r) {
+ return r==l;
+}
+// s!=m
+template <int M, class EL, int RSL, class ER, int CSR, int RSR> inline
+bool operator!=(const SymMat<M,EL,RSL>& l, const Mat<M,M,ER,CSR,RSR>& r) {
+ return !(r==l);
+}
+
+ // DOT PRODUCT
+
+// Dot product and corresponding infix operator*(). Note that
+// the '*' operator is just a matrix multiply so is strictly
+// row*column to produce a scalar (1x1) result.
+//
+// In keeping with Matlab precedent, however, the explicit dot()
+// function is defined on two column vectors
+// v and w such that dot(v,w)= ~v * w. That means we use the
+// Hermitian transpose of the elements of v, and the elements of
+// w unchanged. If v and/or w are rows, we first convert them
+// to vectors via *positional* transpose. So no matter what shape
+// these have on the way in it is always the Hermitian transpose
+// (or complex conjugate for scalars) of the first item times
+// the unchanged elements of the second item.
+
+
+template <int M, class E1, int S1, class E2, int S2> inline
+typename CNT<typename CNT<E1>::THerm>::template Result<E2>::Mul
+dot(const Vec<M,E1,S1>& r, const Vec<M,E2,S2>& v) {
+ typename CNT<typename CNT<E1>::THerm>::template Result<E2>::Mul sum(dot(reinterpret_cast<const Vec<M-1,E1,S1>&>(r), reinterpret_cast<const Vec<M-1,E2,S2>&>(v)) + CNT<E1>::transpose(r[M-1])*v[M-1]);
+ return sum;
+}
+template <class E1, int S1, class E2, int S2> inline
+typename CNT<typename CNT<E1>::THerm>::template Result<E2>::Mul
+dot(const Vec<1,E1,S1>& r, const Vec<1,E2,S2>& v) {
+ typename CNT<typename CNT<E1>::THerm>::template Result<E2>::Mul sum(CNT<E1>::transpose(r[0])*v[0]);
+ return sum;
+}
+
+// dot product (row * conforming vec)
+template <int N, class E1, int S1, class E2, int S2> inline
+typename CNT<E1>::template Result<E2>::Mul
+operator*(const Row<N,E1,S1>& r, const Vec<N,E2,S2>& v) {
+ typename CNT<E1>::template Result<E2>::Mul sum(reinterpret_cast<const Row<N-1,E1,S1>&>(r)*reinterpret_cast<const Vec<N-1,E2,S2>&>(v) + r[N-1]*v[N-1]);
+ return sum;
+}
+template <class E1, int S1, class E2, int S2> inline
+typename CNT<E1>::template Result<E2>::Mul
+operator*(const Row<1,E1,S1>& r, const Vec<1,E2,S2>& v) {
+ typename CNT<E1>::template Result<E2>::Mul sum(r[0]*v[0]);
+ return sum;
+}
+
+// Alternate acceptable forms for dot().
+template <int N, class E1, int S1, class E2, int S2> inline
+typename CNT<E1>::template Result<E2>::Mul
+dot(const Row<N,E1,S1>& r, const Vec<N,E2,S2>& v) {
+ return dot(r.positionalTranspose(),v);
+}
+template <int M, class E1, int S1, class E2, int S2> inline
+typename CNT<E1>::template Result<E2>::Mul
+dot(const Vec<M,E1,S1>& v, const Row<M,E2,S2>& r) {
+ return dot(v,r.positionalTranspose());
+}
+template <int N, class E1, int S1, class E2, int S2> inline
+typename CNT<E1>::template Result<E2>::Mul
+dot(const Row<N,E1,S1>& r, const Row<N,E2,S2>& s) {
+ return dot(r.positionalTranspose(),s.positionalTranspose());
+}
+
+ // OUTER PRODUCT
+
+// Outer product and corresponding infix operator*(). Note that
+// the '*' operator is just a matrix multiply so is strictly
+// column_mX1 * row_1Xm to produce an mXm matrix result.
+//
+// Although Matlab doesn't provide outer(), to be consistent
+// we'll treat it as discussed for dot() above. That is, outer
+// is defined on two column vectors
+// v and w such that outer(v,w)= v * ~w. That means we use the
+// elements of v unchanged but use the Hermitian transpose of
+// the elements of w. If v and/or w are rows, we first convert them
+// to vectors via *positional* transpose. So no matter what shape
+// these have on the way in it is always the unchanged elements of
+// the first item times the Hermitian transpose (or complex
+// conjugate for scalars) of the elements of the second item.
+
+template <int M, class E1, int S1, class E2, int S2> inline
+Mat<M,M, typename CNT<E1>::template Result<typename CNT<E2>::THerm>::Mul>
+outer(const Vec<M,E1,S1>& v, const Vec<M,E2,S2>& w) {
+ Mat<M,M, typename CNT<E1>::template Result<typename CNT<E2>::THerm>::Mul> m;
+ for (int i=0; i<M; ++i)
+ m[i] = v[i] * ~w;
+ return m;
+}
+
+// This is the general conforming case of Vec*Row (outer product)
+template <int M, class E1, int S1, class E2, int S2> inline
+typename Vec<M,E1,S1>::template Result<Row<M,E2,S2> >::Mul
+operator*(const Vec<M,E1,S1>& v, const Row<M,E2,S2>& r) {
+ return Vec<M,E1,S1>::template Result<Row<M,E2,S2> >::MulOp::perform(v,r);
+}
+
+
+// Alternate acceptable forms for outer().
+template <int M, class E1, int S1, class E2, int S2> inline
+Mat<M,M, typename CNT<E1>::template Result<E2>::Mul>
+outer(const Vec<M,E1,S1>& v, const Row<M,E2,S2>& r) {
+ return outer(v,r.positionalTranspose());
+}
+template <int M, class E1, int S1, class E2, int S2> inline
+Mat<M,M, typename CNT<E1>::template Result<E2>::Mul>
+outer(const Row<M,E1,S1>& r, const Vec<M,E2,S2>& v) {
+ return outer(r.positionalTranspose(),v);
+}
+template <int M, class E1, int S1, class E2, int S2> inline
+Mat<M,M, typename CNT<E1>::template Result<E2>::Mul>
+outer(const Row<M,E1,S1>& r, const Row<M,E2,S2>& s) {
+ return outer(r.positionalTranspose(),s.positionalTranspose());
+}
+
+ // MAT*VEC, ROW*MAT (conforming)
+
+// vec = mat * vec (conforming)
+template <int M, int N, class ME, int CS, int RS, class E, int S> inline
+typename Mat<M,N,ME,CS,RS>::template Result<Vec<N,E,S> >::Mul
+operator*(const Mat<M,N,ME,CS,RS>& m,const Vec<N,E,S>& v) {
+ typename Mat<M,N,ME,CS,RS>::template Result<Vec<N,E,S> >::Mul result;
+ for (int i=0; i<M; ++i)
+ result[i] = m[i]*v;
+ return result;
+}
+
+// row = row * mat (conforming)
+template <int M, class E, int S, int N, class ME, int CS, int RS> inline
+typename Row<M,E,S>::template Result<Mat<M,N,ME,CS,RS> >::Mul
+operator*(const Row<M,E,S>& r, const Mat<M,N,ME,CS,RS>& m) {
+ typename Row<M,E,S>::template Result<Mat<M,N,ME,CS,RS> >::Mul result;
+ for (int i=0; i<N; ++i)
+ result[i] = r*m(i);
+ return result;
+}
+
+ // SYMMAT*VEC, ROW*SYMMAT (conforming)
+
+// vec = sym * vec (conforming)
+template <int N, class ME, int RS, class E, int S> inline
+typename SymMat<N,ME,RS>::template Result<Vec<N,E,S> >::Mul
+operator*(const SymMat<N,ME,RS>& m,const Vec<N,E,S>& v) {
+ typename SymMat<N,ME,RS>::template Result<Vec<N,E,S> >::Mul result;
+ for (int i=0; i<N; ++i) {
+ result[i] = m.getDiag()[i]*v[i];
+ for (int j=0; j<i; ++j)
+ result[i] += m.getEltLower(i,j)*v[j];
+ for (int j=i+1; j<N; ++j)
+ result[i] += m.getEltUpper(i,j)*v[j];
+ }
+ return result;
+}
+
+// Unroll loops for small cases.
+
+// 1 flop.
+template <class ME, int RS, class E, int S> inline
+typename SymMat<1,ME,RS>::template Result<Vec<1,E,S> >::Mul
+operator*(const SymMat<1,ME,RS>& m,const Vec<1,E,S>& v) {
+ typename SymMat<1,ME,RS>::template Result<Vec<1,E,S> >::Mul result;
+ result[0] = m.getDiag()[0]*v[0];
+ return result;
+}
+
+// 6 flops.
+template <class ME, int RS, class E, int S> inline
+typename SymMat<2,ME,RS>::template Result<Vec<2,E,S> >::Mul
+operator*(const SymMat<2,ME,RS>& m,const Vec<2,E,S>& v) {
+ typename SymMat<2,ME,RS>::template Result<Vec<2,E,S> >::Mul result;
+ result[0] = m.getDiag()[0] *v[0] + m.getEltUpper(0,1)*v[1];
+ result[1] = m.getEltLower(1,0)*v[0] + m.getDiag()[1] *v[1];
+ return result;
+}
+
+// 15 flops.
+template <class ME, int RS, class E, int S> inline
+typename SymMat<3,ME,RS>::template Result<Vec<3,E,S> >::Mul
+operator*(const SymMat<3,ME,RS>& m,const Vec<3,E,S>& v) {
+ typename SymMat<3,ME,RS>::template Result<Vec<3,E,S> >::Mul result;
+ result[0] = m.getDiag()[0] *v[0] + m.getEltUpper(0,1)*v[1] + m.getEltUpper(0,2)*v[2];
+ result[1] = m.getEltLower(1,0)*v[0] + m.getDiag()[1] *v[1] + m.getEltUpper(1,2)*v[2];
+ result[2] = m.getEltLower(2,0)*v[0] + m.getEltLower(2,1)*v[1] + m.getDiag()[2] *v[2];
+ return result;
+}
+
+// row = row * sym (conforming)
+template <int M, class E, int S, class ME, int RS> inline
+typename Row<M,E,S>::template Result<SymMat<M,ME,RS> >::Mul
+operator*(const Row<M,E,S>& r, const SymMat<M,ME,RS>& m) {
+ typename Row<M,E,S>::template Result<SymMat<M,ME,RS> >::Mul result;
+ for (int j=0; j<M; ++j) {
+ result[j] = r[j]*m.getDiag()[j];
+ for (int i=0; i<j; ++i)
+ result[j] += r[i]*m.getEltUpper(i,j);
+ for (int i=j+1; i<M; ++i)
+ result[j] += r[i]*m.getEltLower(i,j);
+ }
+ return result;
+}
+
+// Unroll loops for small cases.
+
+// 1 flop.
+template <class E, int S, class ME, int RS> inline
+typename Row<1,E,S>::template Result<SymMat<1,ME,RS> >::Mul
+operator*(const Row<1,E,S>& r, const SymMat<1,ME,RS>& m) {
+ typename Row<1,E,S>::template Result<SymMat<1,ME,RS> >::Mul result;
+ result[0] = r[0]*m.getDiag()[0];
+ return result;
+}
+
+// 6 flops.
+template <class E, int S, class ME, int RS> inline
+typename Row<2,E,S>::template Result<SymMat<2,ME,RS> >::Mul
+operator*(const Row<2,E,S>& r, const SymMat<2,ME,RS>& m) {
+ typename Row<2,E,S>::template Result<SymMat<2,ME,RS> >::Mul result;
+ result[0] = r[0]*m.getDiag()[0] + r[1]*m.getEltLower(1,0);
+ result[1] = r[0]*m.getEltUpper(0,1) + r[1]*m.getDiag()[1];
+ return result;
+}
+
+// 15 flops.
+template <class E, int S, class ME, int RS> inline
+typename Row<3,E,S>::template Result<SymMat<3,ME,RS> >::Mul
+operator*(const Row<3,E,S>& r, const SymMat<3,ME,RS>& m) {
+ typename Row<3,E,S>::template Result<SymMat<3,ME,RS> >::Mul result;
+ result[0] = r[0]*m.getDiag()[0] + r[1]*m.getEltLower(1,0) + r[2]*m.getEltLower(2,0);
+ result[1] = r[0]*m.getEltUpper(0,1) + r[1]*m.getDiag()[1] + r[2]*m.getEltLower(2,1);
+ result[2] = r[0]*m.getEltUpper(0,2) + r[1]*m.getEltUpper(1,2) + r[2]*m.getDiag()[2];
+ return result;
+}
+
+ // NONCONFORMING MULTIPLY
+
+ // Nonconforming: Vec on left: v*r v*m v*sym v*v
+ // Result has the shape of the "most composite" (deepest) argument.
+
+// elementwise multiply (vec * nonconforming row)
+template <int M, class E1, int S1, int N, class E2, int S2> inline
+typename Vec<M,E1,S1>::template Result<Row<N,E2,S2> >::MulNon
+operator*(const Vec<M,E1,S1>& v, const Row<N,E2,S2>& r) {
+ return Vec<M,E1,S1>::template Result<Row<N,E2,S2> >::MulOpNonConforming::perform(v,r);
+}
+// elementwise multiply (vec * nonconforming mat)
+template <int M, class E1, int S1, int MM, int NN, class E2, int CS2, int RS2> inline
+typename Vec<M,E1,S1>::template Result<Mat<MM,NN,E2,CS2,RS2> >::MulNon
+operator*(const Vec<M,E1,S1>& v, const Mat<MM,NN,E2,CS2,RS2>& m) {
+ return Vec<M,E1,S1>::template Result<Mat<MM,NN,E2,CS2,RS2> >
+ ::MulOpNonConforming::perform(v,m);
+}
+// elementwise multiply (vec * nonconforming symmat)
+template <int M, class E1, int S1, int MM, class E2, int RS2> inline
+typename Vec<M,E1,S1>::template Result<SymMat<MM,E2,RS2> >::MulNon
+operator*(const Vec<M,E1,S1>& v, const SymMat<MM,E2,RS2>& m) {
+ return Vec<M,E1,S1>::template Result<SymMat<MM,E2,RS2> >
+ ::MulOpNonConforming::perform(v,m);
+}
+// elementwise multiply (vec * nonconforming vec)
+template <int M, class E1, int S1, int MM, class E2, int S2> inline
+typename Vec<M,E1,S1>::template Result<Vec<MM,E2,S2> >::MulNon
+operator*(const Vec<M,E1,S1>& v1, const Vec<MM,E2,S2>& v2) {
+ return Vec<M,E1,S1>::template Result<Vec<MM,E2,S2> >
+ ::MulOpNonConforming::perform(v1,v2);
+}
+
+ // Nonconforming: Row on left -- r*v r*r r*m r*sym
+
+
+// (row or mat) = row * mat (nonconforming)
+template <int M, class E, int S, int MM, int NN, class ME, int CS, int RS> inline
+typename Row<M,E,S>::template Result<Mat<MM,NN,ME,CS,RS> >::MulNon
+operator*(const Row<M,E,S>& r, const Mat<MM,NN,ME,CS,RS>& m) {
+ return Row<M,E,S>::template Result<Mat<MM,NN,ME,CS,RS> >
+ ::MulOpNonConforming::perform(r,m);
+}
+// (row or vec) = row * vec (nonconforming)
+template <int N, class E1, int S1, int M, class E2, int S2> inline
+typename Row<N,E1,S1>::template Result<Vec<M,E2,S2> >::MulNon
+operator*(const Row<N,E1,S1>& r, const Vec<M,E2,S2>& v) {
+ return Row<N,E1,S1>::template Result<Vec<M,E2,S2> >
+ ::MulOpNonConforming::perform(r,v);
+}
+// (row1 or row2) = row1 * row2(nonconforming)
+template <int N1, class E1, int S1, int N2, class E2, int S2> inline
+typename Row<N1,E1,S1>::template Result<Row<N2,E2,S2> >::MulNon
+operator*(const Row<N1,E1,S1>& r1, const Row<N2,E2,S2>& r2) {
+ return Row<N1,E1,S1>::template Result<Row<N2,E2,S2> >
+ ::MulOpNonConforming::perform(r1,r2);
+}
+
+ // Nonconforming: Mat on left -- m*v m*r m*sym
+
+// (mat or vec) = mat * vec (nonconforming)
+template <int M, int N, class ME, int CS, int RS, int MM, class E, int S> inline
+typename Mat<M,N,ME,CS,RS>::template Result<Vec<MM,E,S> >::MulNon
+operator*(const Mat<M,N,ME,CS,RS>& m,const Vec<MM,E,S>& v) {
+ return Mat<M,N,ME,CS,RS>::template Result<Vec<MM,E,S> >
+ ::MulOpNonConforming::perform(m,v);
+}
+// (mat or row) = mat * row (nonconforming)
+template <int M, int N, class ME, int CS, int RS, int NN, class E, int S> inline
+typename Mat<M,N,ME,CS,RS>::template Result<Row<NN,E,S> >::MulNon
+operator*(const Mat<M,N,ME,CS,RS>& m,const Row<NN,E,S>& r) {
+ return Mat<M,N,ME,CS,RS>::template Result<Row<NN,E,S> >
+ ::MulOpNonConforming::perform(m,r);
+}
+
+// (mat or sym) = mat * sym (nonconforming)
+template <int M, int N, class ME, int CS, int RS, int Dim, class E, int S> inline
+typename Mat<M,N,ME,CS,RS>::template Result<SymMat<Dim,E,S> >::MulNon
+operator*(const Mat<M,N,ME,CS,RS>& m,const SymMat<Dim,E,S>& sy) {
+ return Mat<M,N,ME,CS,RS>::template Result<SymMat<Dim,E,S> >
+ ::MulOpNonConforming::perform(m,sy);
+}
+
+ // CROSS PRODUCT
+
+// Cross product and corresponding operator%, but only for 2- and 3-Vecs
+// and Rows. It is OK to mix same-length Vecs & Rows; cross product is
+// defined elementwise and never transposes the individual elements.
+//
+// We also define crossMat(v) below which produces a 2x2 or 3x3
+// matrix M such that M*w = v % w for w the same length vector (or row) as v.
+// TODO: the 3d crossMat is skew symmetric but currently there is no
+// SkewMat class defined so we have to return a full 3x3.
+
+// For 3d cross product, we'll follow Matlab and make the return type a
+// Row if either or both arguments are Rows, although I'm not sure that's
+// a good idea! Note that the definition of crossMat means crossMat(r)*v
+// will yield a column, while r%v is a Row.
+
+// We define v % m where v is a 3-vector and m is a 3xN matrix.
+// This returns a matrix c of the same dimensions as m where
+// column c(i) = v % m(i), that is, each column of c is the cross
+// product of v and the corresponding column of m. This definition means that
+// v % m == crossMat(v)*m
+// which seems like a good idea. (But note that v%m takes 9*N flops while
+// crossMat(v)*m takes 15*N flops.) If we have a row vector r instead,
+// we define r % m == v % m so again r%m==crossMat(r)*m. We also
+// define the cross product operator with an Mx3 matrix on the left,
+// defined so that c[i] = m[i]%v, that is, the rows of c are the
+// cross products of the corresonding rows of m and vector v. Again,
+// we allow v to be a row without any change to the results or return type.
+// This definition means m % v = m * crossMat(v), but again it is faster.
+
+// v = v % v
+template <class E1, int S1, class E2, int S2> inline
+Vec<3,typename CNT<E1>::template Result<E2>::Mul>
+cross(const Vec<3,E1,S1>& a, const Vec<3,E2,S2>& b) {
+ return Vec<3,typename CNT<E1>::template Result<E2>::Mul>
+ (a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]);
+}
+template <class E1, int S1, class E2, int S2> inline
+Vec<3,typename CNT<E1>::template Result<E2>::Mul>
+operator%(const Vec<3,E1,S1>& a, const Vec<3,E2,S2>& b) {return cross(a,b);}
+
+// r = v % r
+template <class E1, int S1, class E2, int S2> inline
+Row<3,typename CNT<E1>::template Result<E2>::Mul>
+cross(const Vec<3,E1,S1>& a, const Row<3,E2,S2>& b) {
+ return Row<3,typename CNT<E1>::template Result<E2>::Mul>
+ (a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]);
+}
+template <class E1, int S1, class E2, int S2> inline
+Row<3,typename CNT<E1>::template Result<E2>::Mul>
+operator%(const Vec<3,E1,S1>& a, const Row<3,E2,S2>& b) {return cross(a,b);}
+
+// r = r % v
+template <class E1, int S1, class E2, int S2> inline
+Row<3,typename CNT<E1>::template Result<E2>::Mul>
+cross(const Row<3,E1,S1>& a, const Vec<3,E2,S2>& b) {
+ return Row<3,typename CNT<E1>::template Result<E2>::Mul>
+ (a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]);
+}
+template <class E1, int S1, class E2, int S2> inline
+Row<3,typename CNT<E1>::template Result<E2>::Mul>
+operator%(const Row<3,E1,S1>& a, const Vec<3,E2,S2>& b) {return cross(a,b);}
+
+// r = r % r
+template <class E1, int S1, class E2, int S2> inline
+Row<3,typename CNT<E1>::template Result<E2>::Mul>
+cross(const Row<3,E1,S1>& a, const Row<3,E2,S2>& b) {
+ return Row<3,typename CNT<E1>::template Result<E2>::Mul>
+ (a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]);
+}
+template <class E1, int S1, class E2, int S2> inline
+Row<3,typename CNT<E1>::template Result<E2>::Mul>
+operator%(const Row<3,E1,S1>& a, const Row<3,E2,S2>& b) {return cross(a,b);}
+
+
+ // Cross a vector with a matrix. The meaning is given by substituting
+ // the vector's cross product matrix and performing a matrix multiply.
+ // We implement v % m(3,N) for a full matrix m, and v % s(3,3) for
+ // a 3x3 symmetric matrix (producing a 3x3 full result). Variants are
+ // provided with the vector on the right and for when the vector is
+ // supplied as a row (which doesn't change the result). See above
+ // for more details.
+
+// m = v % m
+// Cost is 9*N flops.
+template <class E1, int S1, int N, class E2, int CS, int RS> inline
+Mat<3,N,typename CNT<E1>::template Result<E2>::Mul> // packed
+cross(const Vec<3,E1,S1>& v, const Mat<3,N,E2,CS,RS>& m) {
+ Mat<3,N,typename CNT<E1>::template Result<E2>::Mul> result;
+ for (int j=0; j < N; ++j)
+ result(j) = v % m(j);
+ return result;
+}
+template <class E1, int S1, int N, class E2, int CS, int RS> inline
+Mat<3,N,typename CNT<E1>::template Result<E2>::Mul>
+operator%(const Vec<3,E1,S1>& v, const Mat<3,N,E2,CS,RS>& m) {return cross(v,m);}
+
+// Same as above except we have a Row of N Vec<3>s instead of the matrix.
+// Cost is 9*N flops.
+template <class E1, int S1, int N, class E2, int S2, int S3> inline
+Row< N,Vec<3,typename CNT<E1>::template Result<E2>::Mul> > // packed
+cross(const Vec<3,E1,S1>& v, const Row<N,Vec<3,E2,S2>,S3>& m) {
+ Row< N,Vec<3,typename CNT<E1>::template Result<E2>::Mul> > result;
+ for (int j=0; j < N; ++j)
+ result(j) = v % m(j);
+ return result;
+}
+// Specialize for N==3 to avoid ambiguity
+template <class E1, int S1, class E2, int S2, int S3> inline
+Row< 3,Vec<3,typename CNT<E1>::template Result<E2>::Mul> > // packed
+cross(const Vec<3,E1,S1>& v, const Row<3,Vec<3,E2,S2>,S3>& m) {
+ Row< 3,Vec<3,typename CNT<E1>::template Result<E2>::Mul> > result;
+ for (int j=0; j < 3; ++j)
+ result(j) = v % m(j);
+ return result;
+}
+template <class E1, int S1, int N, class E2, int S2, int S3> inline
+Row< N,Vec<3,typename CNT<E1>::template Result<E2>::Mul> > // packed
+operator%(const Vec<3,E1,S1>& v, const Row<N,Vec<3,E2,S2>,S3>& m)
+{ return cross(v,m); }
+template <class E1, int S1, class E2, int S2, int S3> inline
+Row< 3,Vec<3,typename CNT<E1>::template Result<E2>::Mul> > // packed
+operator%(const Vec<3,E1,S1>& v, const Row<3,Vec<3,E2,S2>,S3>& m)
+{ return cross(v,m); }
+
+// m = v % s
+// By writing this out elementwise for the symmetric case we can do this
+// in 24 flops, a small savings over doing three cross products of 9 flops each.
+template<class EV, int SV, class EM, int RS> inline
+Mat<3,3,typename CNT<EV>::template Result<EM>::Mul> // packed
+cross(const Vec<3,EV,SV>& v, const SymMat<3,EM,RS>& s) {
+ const EV& x=v[0]; const EV& y=v[1]; const EV& z=v[2];
+ const EM& a=s(0,0);
+ const EM& b=s(1,0); const EM& d=s(1,1);
+ const EM& c=s(2,0); const EM& e=s(2,1); const EM& f=s(2,2);
+
+ typedef typename CNT<EV>::template Result<EM>::Mul EResult;
+ const EResult xe=x*e, yc=y*c, zb=z*b;
+ return Mat<3,3,EResult>
+ ( yc-zb, y*e-z*d, y*f-z*e,
+ z*a-x*c, zb-xe, z*c-x*f,
+ x*b-y*a, x*d-y*b, xe-yc );
+}
+template <class EV, int SV, class EM, int RS> inline
+Mat<3,3,typename CNT<EV>::template Result<EM>::Mul>
+operator%(const Vec<3,EV,SV>& v, const SymMat<3,EM,RS>& s) {return cross(v,s);}
+
+// m = r % m
+template <class E1, int S1, int N, class E2, int CS, int RS> inline
+Mat<3,N,typename CNT<E1>::template Result<E2>::Mul> // packed
+cross(const Row<3,E1,S1>& r, const Mat<3,N,E2,CS,RS>& m) {
+ return cross(r.positionalTranspose(), m);
+}
+template <class E1, int S1, int N, class E2, int CS, int RS> inline
+Mat<3,N,typename CNT<E1>::template Result<E2>::Mul>
+operator%(const Row<3,E1,S1>& r, const Mat<3,N,E2,CS,RS>& m) {return cross(r,m);}
+
+// m = r % s
+template<class EV, int SV, class EM, int RS> inline
+Mat<3,3,typename CNT<EV>::template Result<EM>::Mul> // packed
+cross(const Row<3,EV,SV>& r, const SymMat<3,EM,RS>& s) {
+ return cross(r.positionalTranspose(), s);
+}
+template<class EV, int SV, class EM, int RS> inline
+Mat<3,3,typename CNT<EV>::template Result<EM>::Mul> // packed
+operator%(const Row<3,EV,SV>& r, const SymMat<3,EM,RS>& s) {return cross(r,s);}
+
+// m = m % v
+template <int M, class EM, int CS, int RS, class EV, int S> inline
+Mat<M,3,typename CNT<EM>::template Result<EV>::Mul> // packed
+cross(const Mat<M,3,EM,CS,RS>& m, const Vec<3,EV,S>& v) {
+ Mat<M,3,typename CNT<EM>::template Result<EV>::Mul> result;
+ for (int i=0; i < M; ++i)
+ result[i] = m[i] % v;
+ return result;
+}
+template <int M, class EM, int CS, int RS, class EV, int S> inline
+Mat<M,3,typename CNT<EM>::template Result<EV>::Mul> // packed
+operator%(const Mat<M,3,EM,CS,RS>& m, const Vec<3,EV,S>& v) {return cross(m,v);}
+
+// m = s % v
+// By writing this out elementwise for the symmetric case we can do this
+// in 24 flops, a small savings over doing three cross products of 9 flops each.
+template<class EM, int RS, class EV, int SV> inline
+Mat<3,3,typename CNT<EM>::template Result<EV>::Mul> // packed
+cross(const SymMat<3,EM,RS>& s, const Vec<3,EV,SV>& v) {
+ const EV& x=v[0]; const EV& y=v[1]; const EV& z=v[2];
+ const EM& a=s(0,0);
+ const EM& b=s(1,0); const EM& d=s(1,1);
+ const EM& c=s(2,0); const EM& e=s(2,1); const EM& f=s(2,2);
+
+ typedef typename CNT<EV>::template Result<EM>::Mul EResult;
+ const EResult xe=x*e, yc=y*c, zb=z*b;
+ return Mat<3,3,EResult>
+ ( zb-yc, x*c-z*a, y*a-x*b,
+ z*d-y*e, xe-zb, y*b-x*d,
+ z*e-y*f, x*f-z*c, yc-xe );
+}
+template<class EM, int RS, class EV, int SV> inline
+Mat<3,3,typename CNT<EM>::template Result<EV>::Mul> // packed
+operator%(const SymMat<3,EM,RS>& s, const Vec<3,EV,SV>& v) {return cross(s,v);}
+
+// m = m % r
+template <int M, class EM, int CS, int RS, class ER, int S> inline
+Mat<M,3,typename CNT<EM>::template Result<ER>::Mul> // packed
+cross(const Mat<M,3,EM,CS,RS>& m, const Row<3,ER,S>& r) {
+ return cross(m,r.positionalTranspose());
+}
+template <int M, class EM, int CS, int RS, class ER, int S> inline
+Mat<M,3,typename CNT<EM>::template Result<ER>::Mul> // packed
+operator%(const Mat<M,3,EM,CS,RS>& m, const Row<3,ER,S>& r) {return cross(m,r);}
+
+// m = s % r
+template<class EM, int RS, class EV, int SV> inline
+Mat<3,3,typename CNT<EM>::template Result<EV>::Mul> // packed
+cross(const SymMat<3,EM,RS>& s, const Row<3,EV,SV>& r) {
+ return cross(s,r.positionalTranspose());
+}
+template<class EM, int RS, class EV, int SV> inline
+Mat<3,3,typename CNT<EM>::template Result<EV>::Mul> // packed
+operator%(const SymMat<3,EM,RS>& s, const Row<3,EV,SV>& r) {return cross(s,r);}
+
+// 2d cross product just returns a scalar. This is the z-value you
+// would get if the arguments were treated as 3-vectors with 0
+// z components.
+
+template <class E1, int S1, class E2, int S2> inline
+typename CNT<E1>::template Result<E2>::Mul
+cross(const Vec<2,E1,S1>& a, const Vec<2,E2,S2>& b) {
+ return a[0]*b[1]-a[1]*b[0];
+}
+template <class E1, int S1, class E2, int S2> inline
+typename CNT<E1>::template Result<E2>::Mul
+operator%(const Vec<2,E1,S1>& a, const Vec<2,E2,S2>& b) {return cross(a,b);}
+
+template <class E1, int S1, class E2, int S2> inline
+typename CNT<E1>::template Result<E2>::Mul
+cross(const Row<2,E1,S1>& a, const Vec<2,E2,S2>& b) {
+ return a[0]*b[1]-a[1]*b[0];
+}
+template <class E1, int S1, class E2, int S2> inline
+typename CNT<E1>::template Result<E2>::Mul
+operator%(const Row<2,E1,S1>& a, const Vec<2,E2,S2>& b) {return cross(a,b);}
+
+template <class E1, int S1, class E2, int S2> inline
+typename CNT<E1>::template Result<E2>::Mul
+cross(const Vec<2,E1,S1>& a, const Row<2,E2,S2>& b) {
+ return a[0]*b[1]-a[1]*b[0];
+}
+template <class E1, int S1, class E2, int S2> inline
+typename CNT<E1>::template Result<E2>::Mul
+operator%(const Vec<2,E1,S1>& a, const Row<2,E2,S2>& b) {return cross(a,b);}
+
+template <class E1, int S1, class E2, int S2> inline
+typename CNT<E1>::template Result<E2>::Mul
+cross(const Row<2,E1,S1>& a, const Row<2,E2,S2>& b) {
+ return a[0]*b[1]-a[1]*b[0];
+}
+template <class E1, int S1, class E2, int S2> inline
+typename CNT<E1>::template Result<E2>::Mul
+operator%(const Row<2,E1,S1>& a, const Row<2,E2,S2>& b) {return cross(a,b);}
+
+ // CROSS MAT
+
+/// Calculate matrix M(v) such that M(v)*w = v % w. We produce the
+/// same M regardless of whether v is a column or row.
+/// Requires 3 flops to form.
+template <class E, int S> inline
+Mat<3,3,E>
+crossMat(const Vec<3,E,S>& v) {
+ return Mat<3,3,E>(Row<3,E>( E(0), -v[2], v[1]),
+ Row<3,E>( v[2], E(0), -v[0]),
+ Row<3,E>(-v[1], v[0], E(0)));
+}
+/// Specialize crossMat() for negated scalar types. Returned matrix loses negator.
+/// Requires 3 flops to form.
+template <class E, int S> inline
+Mat<3,3,E>
+crossMat(const Vec<3,negator<E>,S>& v) {
+ // Here the "-" operators are just recasts, but the casts
+ // to type E have to perform floating point negations.
+ return Mat<3,3,E>(Row<3,E>( E(0), -v[2], E(v[1])),
+ Row<3,E>( E(v[2]), E(0), -v[0]),
+ Row<3,E>(-v[1], E(v[0]), E(0)));
+}
+
+/// Form cross product matrix from a Row vector; 3 flops.
+template <class E, int S> inline
+Mat<3,3,E> crossMat(const Row<3,E,S>& r) {return crossMat(r.positionalTranspose());}
+/// Form cross product matrix from a Row vector whose elements are negated scalars; 3 flops.
+template <class E, int S> inline
+Mat<3,3,E> crossMat(const Row<3,negator<E>,S>& r) {return crossMat(r.positionalTranspose());}
+
+/// Calculate 2D cross product matrix M(v) such that M(v)*w = v0*w1-v1*w0 = v % w (a scalar).
+/// Whether v is a Vec<2> or Row<2> we create the same M, which will be a 2-element Row.
+/// Requires 1 flop to form.
+template <class E, int S> inline
+Row<2,E> crossMat(const Vec<2,E,S>& v) {
+ return Row<2,E>(-v[1], v[0]);
+}
+/// Specialize 2D cross product matrix for negated scalar types; 1 flop.
+template <class E, int S> inline
+Row<2,E> crossMat(const Vec<2,negator<E>,S>& v) {
+ return Row<2,E>(-v[1], E(v[0]));
+}
+
+/// Form 2D cross product matrix from a Row<2>; 1 flop.
+template <class E, int S> inline
+Row<2,E> crossMat(const Row<2,E,S>& r) {return crossMat(r.positionalTranspose());}
+/// Form 2D cross product matrix from a Row<2> with negated scalar elements; 1 flop.
+template <class E, int S> inline
+Row<2,E> crossMat(const Row<2,negator<E>,S>& r) {return crossMat(r.positionalTranspose());}
+
+ // CROSS MAT SQ
+
+/// Calculate matrix S(v) such that S(v)*w = -v % (v % w) = (v % w) % v. S is a symmetric,
+/// 3x3 matrix with nonnegative diagonals that obey the triangle inequality.
+/// If M(v) = crossMat(v), then S(v) = square(M(v)) = ~M(v)*M(v) = -M(v)*M(v)
+/// since M is skew symmetric.
+///
+/// Also, S(v) = dot(v,v)*I - outer(v,v) = ~v*v*I - v*~v, where I is the identity
+/// matrix. This is the form necessary for shifting inertia matrices using
+/// the parallel axis theorem, something we do a lot of in multibody dynamics.
+/// Consequently we want to calculate S very efficiently, which we can do because
+/// it has the following very simple form. Assume v=[x y z]. Then
+/// <pre>
+/// y^2+z^2 T T
+/// S(v) = -xy x^2+z^2 T
+/// -xz -yz x^2+y^2
+/// </pre>
+/// where "T" indicates that the element is identical to the symmetric one.
+/// This requires 11 flops to form.
+/// We produce the same S(v) regardless of whether v is a column or row.
+/// Note that there is no 2D equivalent of this operator.
+template <class E, int S> inline
+SymMat<3,E>
+crossMatSq(const Vec<3,E,S>& v) {
+ const E xx = square(v[0]);
+ const E yy = square(v[1]);
+ const E zz = square(v[2]);
+ const E nx = -v[0];
+ const E ny = -v[1];
+ return SymMat<3,E>( yy+zz,
+ nx*v[1], xx+zz,
+ nx*v[2], ny*v[2], xx+yy );
+}
+// Specialize above for negated types. Returned matrix loses negator.
+// The total number of flops here is the same as above: 11 flops.
+template <class E, int S> inline
+SymMat<3,E>
+crossMatSq(const Vec<3,negator<E>,S>& v) {
+ const E xx = square(v[0]);
+ const E yy = square(v[1]);
+ const E zz = square(v[2]);
+ const E y = v[1]; // requires a negation to convert to E
+ const E z = v[2];
+ // The negations in the arguments below are not floating point
+ // operations because the element type is already negated.
+ return SymMat<3,E>( yy+zz,
+ -v[0]*y, xx+zz,
+ -v[0]*z, -v[1]*z, xx+yy );
+}
+
+template <class E, int S> inline
+SymMat<3,E> crossMatSq(const Row<3,E,S>& r) {return crossMatSq(r.positionalTranspose());}
+template <class E, int S> inline
+SymMat<3,E> crossMatSq(const Row<3,negator<E>,S>& r) {return crossMatSq(r.positionalTranspose());}
+
+
+ // DETERMINANT
+
+/// Special case Mat 1x1 determinant. No computation.
+template <class E, int CS, int RS> inline
+E det(const Mat<1,1,E,CS,RS>& m) {
+ return m(0,0);
+}
+
+/// Special case SymMat 1x1 determinant. No computation.
+template <class E, int RS> inline
+E det(const SymMat<1,E,RS>& s) {
+ return s.diag()[0]; // s(0,0) is trouble for a 1x1 symmetric
+}
+
+/// Special case Mat 2x2 determinant. 3 flops (if elements are Real).
+template <class E, int CS, int RS> inline
+E det(const Mat<2,2,E,CS,RS>& m) {
+ // Constant element indices here allow the compiler to select
+ // exactly the right element at compile time.
+ return E(m(0,0)*m(1,1) - m(0,1)*m(1,0));
+}
+
+/// Special case 2x2 SymMat determinant. 3 flops (if elements are Real).
+template <class E, int RS> inline
+E det(const SymMat<2,E,RS>& s) {
+ // For Hermitian matrix (i.e., E is complex or conjugate), s(0,1)
+ // and s(1,0) are complex conjugates of one another. Because of the
+ // constant indices here, the SymMat goes right to the correct
+ // element, so everything gets done inline here with no conditionals.
+ return E(s.getEltDiag(0)*s.getEltDiag(1) - s.getEltUpper(0,1)*s.getEltLower(1,0));
+}
+
+/// Special case Mat 3x3 determinant. 14 flops (if elements are Real).
+template <class E, int CS, int RS> inline
+E det(const Mat<3,3,E,CS,RS>& m) {
+ return E( m(0,0)*(m(1,1)*m(2,2)-m(1,2)*m(2,1))
+ - m(0,1)*(m(1,0)*m(2,2)-m(1,2)*m(2,0))
+ + m(0,2)*(m(1,0)*m(2,1)-m(1,1)*m(2,0)));
+}
+
+/// Special case SymMat 3x3 determinant. 14 flops (if elements are Real).
+template <class E, int RS> inline
+E det(const SymMat<3,E,RS>& s) {
+ return E( s.getEltDiag(0)*
+ (s.getEltDiag(1)*s.getEltDiag(2)-s.getEltUpper(1,2)*s.getEltLower(2,1))
+ - s.getEltUpper(0,1)*
+ (s.getEltLower(1,0)*s.getEltDiag(2)-s.getEltUpper(1,2)*s.getEltLower(2,0))
+ + s.getEltUpper(0,2)*
+ (s.getEltLower(1,0)*s.getEltLower(2,1)-s.getEltDiag(1)*s.getEltLower(2,0)));
+}
+
+/// Calculate the determinant of a square matrix larger than 3x3
+/// by recursive template expansion. The matrix elements must be
+/// multiplication compatible for this to compile successfully.
+/// All scalar element types are acceptable; some composite types
+/// will also work but the result is probably meaningless.
+/// The determinant suffers from increasingly bad scaling as the
+/// matrices get bigger; you should consider an alternative if
+/// possible (see Golub and van Loan, "Matrix Computations").
+/// This uses M*det(M-1) + 4*M flops. For 4x4 that's 60 flops,
+/// for 5x5 it's 320, and it grows really fast from there!
+/// TODO: this is not the right way to calculate determinant --
+/// should calculate LU factorization at 2/3 n^3 flops, then
+/// determinant is product of LU's diagonals.
+template <int M, class E, int CS, int RS> inline
+E det(const Mat<M,M,E,CS,RS>& m) {
+ typename CNT<E>::StdNumber sign(1);
+ E result(0);
+ // We're always going to drop the first row.
+ const Mat<M-1,M,E,CS,RS>& m2 = m.template getSubMat<M-1,M>(1,0);
+ for (int j=0; j < M; ++j) {
+ // det() here is recursive but terminates at 3x3 above.
+ result += sign*m(0,j)*det(m2.dropCol(j));
+ sign = -sign;
+ }
+ return result;
+}
+
+/// Determinant of SymMat larger than 3x3.
+/// TODO: This should be done
+/// instead with a symmetric factorization; the determinant will
+/// be calculable as a product of some diagonal in the factorization.
+/// For now we'll punt to the really bad Mat determinant above.
+template <int M, class E, int RS> inline
+E det(const SymMat<M,E,RS>& s) {
+ return det(Mat<M,M,E>(s));
+}
+
+
+ // INVERSE
+
+/// Specialized 1x1 lapackInverse(): costs one divide.
+template <class E, int CS, int RS> inline
+typename Mat<1,1,E,CS,RS>::TInvert lapackInverse(const Mat<1,1,E,CS,RS>& m) {
+ typedef typename Mat<1,1,E,CS,RS>::TInvert MInv;
+ return MInv( E(typename CNT<E>::StdNumber(1)/m(0,0)) );
+}
+
+/// General inverse of small, fixed-size, square (mXm), non-singular matrix with
+/// scalar elements: use Lapack's LU routine with pivoting. This will only work
+/// if the element type E is a scalar type, although negator<> and conjugate<>
+/// are OK. This routine is <em>not</em> specialized for small matrix sizes other
+/// than 1x1, while the inverse() method is specialized for other small sizes
+/// for speed, possibly losing some numerical stability. The inverse() function
+/// ends up calling this one at sizes for which it has no specialization.
+/// Normally you should call inverse(), but you can call lapackInverse()
+/// explicitly if you want to ensure that the most stable algorithm is used.
+/// @see inverse()
+template <int M, class E, int CS, int RS> inline
+typename Mat<M,M,E,CS,RS>::TInvert lapackInverse(const Mat<M,M,E,CS,RS>& m) {
+ // Copy the source matrix, which has arbitrary row and column spacing,
+ // into the type for its inverse, which must be dense, columnwise
+ // storage. That means its column spacing will be M and row spacing
+ // will be 1, as Lapack expects for a Full matrix.
+ typename Mat<M,M,E,CS,RS>::TInvert inv = m; // dense, columnwise storage
+
+ // We'll perform the inversion ignoring negation and conjugation altogether,
+ // but the TInvert Mat type negates or conjugates the result. And because
+ // of the famous Sherman-Eastman theorem, we know that
+ // conj(inv(m))==inv(conj(m)), and of course -inv(m)==inv(-m).
+ typedef typename CNT<E>::StdNumber Raw; // Raw is E without negator or conjugate.
+ Raw* rawData = reinterpret_cast<Raw*>(&inv(0,0));
+ int ipiv[M], info;
+
+ // This replaces "inv" with its LU facorization and pivot matrix P, such that
+ // P*L*U = m (ignoring negation and conjugation).
+ Lapack::getrf<Raw>(M,M,rawData,M,&ipiv[0],info);
+ SimTK_ASSERT1(info>=0, "Argument %d to Lapack getrf routine was bad", -info);
+ SimTK_ERRCHK1_ALWAYS(info==0, "lapackInverse(Mat<>)",
+ "Matrix is singular so can't be inverted (Lapack getrf info=%d).", info);
+
+ // The workspace size must be at least M. For larger matrices, Lapack wants
+ // to do factorization in blocks of size NB in which case the workspace should
+ // be M*NB. However, we will assume that this matrix is small (in the sense
+ // that it fits entirely in cache) so the minimum workspace size is fine and
+ // we don't need an extra getri call to find the workspace size nor any heap
+ // allocation.
+
+ Raw work[M];
+ Lapack::getri<Raw>(M,rawData,M,&ipiv[0],&work[0],M,info);
+ SimTK_ASSERT1(info>=0, "Argument %d to Lapack getri routine was bad", -info);
+ SimTK_ERRCHK1_ALWAYS(info==0, "lapackInverse(Mat<>)",
+ "Matrix is singular so can't be inverted (Lapack getri info=%d).", info);
+ return inv;
+}
+
+
+/// Specialized 1x1 Mat inverse: costs one divide.
+template <class E, int CS, int RS> inline
+typename Mat<1,1,E,CS,RS>::TInvert inverse(const Mat<1,1,E,CS,RS>& m) {
+ typedef typename Mat<1,1,E,CS,RS>::TInvert MInv;
+ return MInv( E(typename CNT<E>::StdNumber(1)/m(0,0)) );
+}
+
+/// Specialized 1x1 SymMat inverse: costs one divide.
+template <class E, int RS> inline
+typename SymMat<1,E,RS>::TInvert inverse(const SymMat<1,E,RS>& s) {
+ typedef typename SymMat<1,E,RS>::TInvert SInv;
+ return SInv( E(typename CNT<E>::StdNumber(1)/s.diag()[0]) );
+}
+
+/// Specialized 2x2 Mat inverse: costs one divide plus 9 flops.
+template <class E, int CS, int RS> inline
+typename Mat<2,2,E,CS,RS>::TInvert inverse(const Mat<2,2,E,CS,RS>& m) {
+ const E d ( det(m) );
+ const typename CNT<E>::TInvert ood( typename CNT<E>::StdNumber(1)/d );
+ return typename Mat<2,2,E,CS,RS>::TInvert( E( ood*m(1,1)), E(-ood*m(0,1)),
+ E(-ood*m(1,0)), E( ood*m(0,0)));
+}
+
+/// Specialized 2x2 SymMat inverse: costs one divide plus 7 flops.
+template <class E, int RS> inline
+typename SymMat<2,E,RS>::TInvert inverse(const SymMat<2,E,RS>& s) {
+ const E d ( det(s) );
+ const typename CNT<E>::TInvert ood( typename CNT<E>::StdNumber(1)/d );
+ return typename SymMat<2,E,RS>::TInvert( E( ood*s(1,1)),
+ E(-ood*s(1,0)), E(ood*s(0,0)));
+}
+
+/// Specialized 3x3 inverse: costs one divide plus 41 flops (for real-valued
+/// matrices). No pivoting done here so this may be subject to numerical errors
+/// that Lapack would avoid. Call lapackInverse() instead if you're worried.
+/// @see lapackInverse()
+template <class E, int CS, int RS> inline
+typename Mat<3,3,E,CS,RS>::TInvert inverse(const Mat<3,3,E,CS,RS>& m) {
+ // Calculate determinants for each 2x2 submatrix with first row removed.
+ // (See the specialized 3x3 determinant routine above.) We're calculating
+ // this explicitly here because we can re-use the intermediate terms.
+ const E d00 (m(1,1)*m(2,2)-m(1,2)*m(2,1)),
+ nd01(m(1,2)*m(2,0)-m(1,0)*m(2,2)), // -d01
+ d02 (m(1,0)*m(2,1)-m(1,1)*m(2,0));
+
+ // This is the 3x3 determinant and its inverse.
+ const E d (m(0,0)*d00 + m(0,1)*nd01 + m(0,2)*d02);
+ const typename CNT<E>::TInvert
+ ood(typename CNT<E>::StdNumber(1)/d);
+
+ // The other six 2x2 determinants we can't re-use, but we can still
+ // avoid some copying by calculating them explicitly here.
+ const E nd10(m(0,2)*m(2,1)-m(0,1)*m(2,2)), // -d10
+ d11 (m(0,0)*m(2,2)-m(0,2)*m(2,0)),
+ nd12(m(0,1)*m(2,0)-m(0,0)*m(2,1)), // -d12
+ d20 (m(0,1)*m(1,2)-m(0,2)*m(1,1)),
+ nd21(m(0,2)*m(1,0)-m(0,0)*m(1,2)), // -d21
+ d22 (m(0,0)*m(1,1)-m(0,1)*m(1,0));
+
+ return typename Mat<3,3,E,CS,RS>::TInvert
+ ( E(ood* d00), E(ood*nd10), E(ood* d20),
+ E(ood*nd01), E(ood* d11), E(ood*nd21),
+ E(ood* d02), E(ood*nd12), E(ood* d22) );
+}
+
+/// Specialized 3x3 inverse for symmetric or Hermitian: costs one divide plus
+/// 29 flops (for real-valued matrices). No pivoting done here so this may be
+/// subject to numerical errors that Lapack would avoid. Call lapackSymInverse()
+/// instead if you're worried.
+/// @see lapackSymInverse()
+template <class E, int RS> inline
+typename SymMat<3,E,RS>::TInvert inverse(const SymMat<3,E,RS>& s) {
+ // Calculate determinants for each 2x2 submatrix with first row removed.
+ // (See the specialized 3x3 determinant routine above.) We're calculating
+ // this explicitly here because we can re-use the intermediate terms.
+ const E d00 (s(1,1)*s(2,2)-s(1,2)*s(2,1)),
+ nd01(s(1,2)*s(2,0)-s(1,0)*s(2,2)), // -d01
+ d02 (s(1,0)*s(2,1)-s(1,1)*s(2,0));
+
+ // This is the 3x3 determinant and its inverse.
+ const E d (s(0,0)*d00 + s(0,1)*nd01 + s(0,2)*d02);
+ const typename CNT<E>::TInvert
+ ood(typename CNT<E>::StdNumber(1)/d);
+
+ // The other six 2x2 determinants we can't re-use, but we can still
+ // avoid some copying by calculating them explicitly here.
+ const E d11 (s(0,0)*s(2,2)-s(0,2)*s(2,0)),
+ nd12(s(0,1)*s(2,0)-s(0,0)*s(2,1)), // -d12
+ d22 (s(0,0)*s(1,1)-s(0,1)*s(1,0));
+
+ return typename SymMat<3,E,RS>::TInvert
+ ( E(ood* d00),
+ E(ood*nd01), E(ood* d11),
+ E(ood* d02), E(ood*nd12), E(ood* d22) );
+}
+
+/// For any matrix larger than 3x3, we just punt to the Lapack implementation.
+/// @see lapackInverse()
+template <int M, class E, int CS, int RS> inline
+typename Mat<M,M,E,CS,RS>::TInvert inverse(const Mat<M,M,E,CS,RS>& m) {
+ return lapackInverse(m);
+}
+
+// Implement the Mat<>::invert() method using the above specialized
+// inverse functions. This will only compile if M==N.
+template <int M, int N, class ELT, int CS, int RS> inline
+typename Mat<M,N,ELT,CS,RS>::TInvert
+Mat<M,N,ELT,CS,RS>::invert() const {
+ return SimTK::inverse(*this);
+}
+
+} //namespace SimTK
+
+
+#endif //SimTK_SIMMATRIX_SMALLMATRIX_MIXED_H_
diff --git a/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/SymMat.h b/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/SymMat.h
new file mode 100644
index 0000000..a5585a3
--- /dev/null
+++ b/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/SymMat.h
@@ -0,0 +1,1236 @@
+#ifndef SimTK_SIMMATRIX_SMALLMATRIX_SYMMAT_H_
+#define SimTK_SIMMATRIX_SMALLMATRIX_SYMMAT_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ This file declares templatized class SymMat for small, fixed-size symmetric
+ matrices. **/
+
+
+#include "SimTKcommon/internal/common.h"
+
+namespace SimTK {
+
+/** @brief This is a small, fixed-size symmetric or Hermitian matrix designed
+for no-overhead inline computation.
+
+ at ingroup MatVecUtilities
+
+ at tparam M The dimension of this square matrix.
+ at tparam ELT The element type. Must be a composite numerical type (CNT).
+The default is ELT=Real.
+ at tparam RS The spacing from one element to the next in memory, as an
+integer number of elements of type ELT. The default is RS=1.
+
+This is logically a square MxM Hermitian matrix, but only the diagonal
+and \e lower triangle are stored. Elements above the diagonal are
+the Hermitan transpose of their mirrored elements below the diagonal.
+
+The storage is packed by column, with an optional stride RS when going element-
+to-element. We use an unconventional storage scheme here which
+provides substantial conveniences and some performance advantage over the
+conventional (LAPACK) scheme which stores the matrix in packed
+columns. Instead, we store the diagonal first, as a Vec<M> and then store
+the lower triangle separately in the conventional columnwise form. This
+allows easy handling of the diagonal elements separately, which is very
+common in a symmetric matrix. The diagonals are "special"; for example
+they must be "self-Hermitian" meaning their imaginary parts must be zero.
+We could just store real elements for the diagonals, but we don't for
+a number of good reasons which I'll leave as an exercise for the reader.
+However, we will ensure that their imaginary parts are zero, although
+a determined user could screw this up.
+
+Here is an example. Imagine a full 4x4 Hermitian matrix with complex
+elements. We'll use a' to mean conjugate(a). Then the elements are
+<pre>
+ w a' b' c'
+ a x d' e'
+ b d y f'
+ c e f z
+</pre>
+Note that it must be the case that w=w', x=x', y=y', and z=z' so the
+diagonal elements must be real. Here's how we store that as a SymMat:
+<pre>
+ w x y z a b c d e f
+</pre>
+These are stored in consecutive memory locations (possibly separated
+by meaningless elements as indicated by the row spacing RS). Mere
+recasting allows us to view this as a 4x4 matrix, or a 4-element
+diagonal and 6-element "lower" vector, or as these vectors
+apparently containing the Hermitian types (i.e. one may cast
+the "lower" portion into what looks like the "upper" portion).
+
+This is a Composite Numerical Type (CNT).
+**/
+template <int M, class ELT, int RS> class SymMat {
+ typedef ELT E;
+ typedef typename CNT<E>::TNeg ENeg;
+ typedef typename CNT<E>::TWithoutNegator EWithoutNegator;
+ typedef typename CNT<E>::TReal EReal;
+ typedef typename CNT<E>::TImag EImag;
+ typedef typename CNT<E>::TComplex EComplex;
+ typedef typename CNT<E>::THerm EHerm;
+ typedef typename CNT<E>::TPosTrans EPosTrans;
+ typedef typename CNT<E>::TSqHermT ESqHermT;
+ typedef typename CNT<E>::TSqTHerm ESqTHerm;
+
+ typedef typename CNT<E>::TSqrt ESqrt;
+ typedef typename CNT<E>::TAbs EAbs;
+ typedef typename CNT<E>::TStandard EStandard;
+ typedef typename CNT<E>::TInvert EInvert;
+ typedef typename CNT<E>::TNormalize ENormalize;
+
+ typedef typename CNT<E>::Scalar EScalar;
+ typedef typename CNT<E>::ULessScalar EULessScalar;
+ typedef typename CNT<E>::Number ENumber;
+ typedef typename CNT<E>::StdNumber EStdNumber;
+ typedef typename CNT<E>::Precision EPrecision;
+ typedef typename CNT<E>::ScalarNormSq EScalarNormSq;
+
+public:
+
+ enum {
+ NRows = M,
+ NCols = M,
+ NDiagElements = M,
+ NLowerElements = (M*(M-1))/2,
+ NPackedElements = NDiagElements+NLowerElements,
+ NActualElements = RS * NPackedElements,
+ NActualScalars = CNT<E>::NActualScalars * NActualElements,
+ RowSpacing = RS,
+ ColSpacing = NActualElements,
+ ImagOffset = NTraits<ENumber>::ImagOffset,
+ RealStrideFactor = 1, // composite types don't change size when
+ // cast from complex to real or imaginary
+ ArgDepth = ((int)CNT<E>::ArgDepth < (int)MAX_RESOLVED_DEPTH
+ ? CNT<E>::ArgDepth + 1
+ : MAX_RESOLVED_DEPTH),
+ IsScalar = 0,
+ IsULessScalar = 0,
+ IsNumber = 0,
+ IsStdNumber = 0,
+ IsPrecision = 0,
+ SignInterpretation = CNT<E>::SignInterpretation
+ };
+
+ typedef SymMat<M,E,RS> T;
+ typedef SymMat<M,ENeg,RS> TNeg;
+ typedef SymMat<M,EWithoutNegator,RS> TWithoutNegator;
+
+ typedef SymMat<M,EReal,RS*CNT<E>::RealStrideFactor>
+ TReal;
+ typedef SymMat<M,EImag,RS*CNT<E>::RealStrideFactor>
+ TImag;
+ typedef SymMat<M,EComplex,RS> TComplex;
+ typedef T THerm; // These two are opposite of what you might expect
+ typedef SymMat<M,EHerm,RS> TPosTrans;
+ typedef E TElement;
+ typedef Vec<M,E,RS> TDiag; // storage type for the diagonal elements
+ typedef Vec<(M*(M-1))/2,E,RS> TLower; // storage type for the below-diag elements
+ typedef Vec<(M*(M-1))/2,EHerm,RS> TUpper; // cast TLower to this for upper elements
+ typedef Vec<(M*(M+1))/2,E,RS> TAsVec; // the whole SymMat as a single Vec
+
+ // These are the results of calculations, so are returned in new, packed
+ // memory. Be sure to refer to element types here which are also packed.
+ typedef Row<M,E,1> TRow; // packed since we have to copy
+ typedef Vec<M,E,1> TCol;
+ typedef SymMat<M,ESqrt,1> TSqrt;
+ typedef SymMat<M,EAbs,1> TAbs;
+ typedef SymMat<M,EStandard,1> TStandard;
+ typedef SymMat<M,EInvert,1> TInvert;
+ typedef SymMat<M,ENormalize,1> TNormalize;
+ typedef SymMat<M,ESqHermT,1> TSqHermT; // ~Mat*Mat
+ typedef SymMat<M,ESqTHerm,1> TSqTHerm; // Mat*~Mat
+ typedef SymMat<M,E,1> TPacked; // no extra row spacing for new data
+
+ typedef EScalar Scalar;
+ typedef EULessScalar ULessScalar;
+ typedef ENumber Number;
+ typedef EStdNumber StdNumber;
+ typedef EPrecision Precision;
+ typedef EScalarNormSq ScalarNormSq;
+
+ static int size() { return (M*(M+1))/2; }
+ static int nrow() { return M; }
+ static int ncol() { return M; }
+
+ // Scalar norm square is sum( squares of all scalars ). The off-diagonals
+ // come up twice.
+ ScalarNormSq scalarNormSqr() const {
+ return getDiag().scalarNormSqr() + 2.*getLower().scalarNormSqr();
+ }
+
+ // sqrt() is elementwise square root; that is, the return value has the same
+ // dimension as this SymMat but with each element replaced by whatever it thinks
+ // its square root is.
+ TSqrt sqrt() const {
+ return TSqrt(getAsVec().sqrt());
+ }
+
+ // abs() is elementwise absolute value; that is, the return value has the same
+ // dimension as this SymMat but with each element replaced by whatever it thinks
+ // its absolute value is.
+ TAbs abs() const {
+ return TAbs(getAsVec().abs());
+ }
+
+ TStandard standardize() const {
+ return TStandard(getAsVec().standardize());
+ }
+
+ EStandard trace() const {return getDiag().sum();}
+
+ // This gives the resulting SymMat type when (m[i] op P) is applied to each element of m.
+ // It is a SymMat of dimension M, spacing 1, and element types which are the regular
+ // composite result of E op P. Typically P is a scalar type but it doesn't have to be.
+ template <class P> struct EltResult {
+ typedef SymMat<M, typename CNT<E>::template Result<P>::Mul, 1> Mul;
+ typedef SymMat<M, typename CNT<E>::template Result<P>::Dvd, 1> Dvd;
+ typedef SymMat<M, typename CNT<E>::template Result<P>::Add, 1> Add;
+ typedef SymMat<M, typename CNT<E>::template Result<P>::Sub, 1> Sub;
+ };
+
+ // This is the composite result for m op P where P is some kind of appropriately shaped
+ // non-scalar type.
+ template <class P> struct Result {
+ typedef MulCNTs<M,M,ArgDepth,SymMat,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> MulOp;
+ typedef typename MulOp::Type Mul;
+
+ typedef MulCNTsNonConforming<M,M,ArgDepth,SymMat,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> MulOpNonConforming;
+ typedef typename MulOpNonConforming::Type MulNon;
+
+
+ typedef DvdCNTs<M,M,ArgDepth,SymMat,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> DvdOp;
+ typedef typename DvdOp::Type Dvd;
+
+ typedef AddCNTs<M,M,ArgDepth,SymMat,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> AddOp;
+ typedef typename AddOp::Type Add;
+
+ typedef SubCNTs<M,M,ArgDepth,SymMat,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> SubOp;
+ typedef typename SubOp::Type Sub;
+ };
+
+ // Shape-preserving element substitution (always packed)
+ template <class P> struct Substitute {
+ typedef SymMat<M,P> Type;
+ };
+
+ /// Default construction initializes to NaN when debugging but
+ /// is left uninitialized otherwise.
+ SymMat(){
+ #ifndef NDEBUG
+ setToNaN();
+ #endif
+ }
+
+ /// Copy constructor.
+ SymMat(const SymMat& src) {
+ updAsVec() = src.getAsVec();
+ }
+
+ /// Copy assignment; no harm if source and this are the same matrix.
+ SymMat& operator=(const SymMat& src) {
+ updAsVec() = src.getAsVec();
+ return *this;
+ }
+
+ /// This is an \e explicit conversion from square Mat of right size, assuming
+ /// that the source matrix is symmetric to within a reasonable numerical
+ /// tolerance. In Debug mode we'll test that assumption and throw an exception
+ /// if it is wrong. In Release mode you're on your own. All the elements of
+ /// the source Mat are used; off-diagonal elements (i,j) are averaged with
+ /// their corresponding element (j,i); the imaginary part of the diagonal
+ /// is set exactly to zero. If you don't want to spend the flops to average
+ /// the off-diagonals, and you're sure the source is symmetric, use either
+ /// setFromLower() or setFromUpper() which will just copy the elements.
+ /// @see setFromLower(), setFromUpper()
+ template <class EE, int CSS, int RSS>
+ explicit SymMat(const Mat<M,M,EE,CSS,RSS>& m)
+ { setFromSymmetric(m); }
+
+ /// Create a new SymMat of this type from a square Mat of the right
+ /// size, looking only at lower elements and the real part of the
+ /// diagonal.
+ template <class EE, int CSS, int RSS>
+ SymMat& setFromLower(const Mat<M,M,EE,CSS,RSS>& m) {
+ this->updDiag() = m.diag().real();
+ for (int j=0; j<M; ++j)
+ for (int i=j+1; i<M; ++i)
+ this->updEltLower(i,j) = m(i,j);
+ return *this;
+ }
+
+ /// Create a new SymMat of this type from a square Mat of the right
+ /// size, looking only at upper elements and the real part of the
+ /// diagonal. Note that the SymMat's stored elements are still in
+ /// its \e lower triangle; they are just initialized from the Mat's
+ /// \e upper triangle. There is no transposing of elements here;
+ /// we simply copy the upper elements of the Mat to the corresponding
+ /// lower elements of the SymMat.
+ template <class EE, int CSS, int RSS>
+ SymMat& setFromUpper(const Mat<M,M,EE,CSS,RSS>& m) {
+ this->updDiag() = m.diag().real();
+ for (int j=0; j<M; ++j)
+ for (int i=j+1; i<M; ++i)
+ this->updEltLower(i,j) = m(j,i);
+ return *this;
+ }
+
+ /// Create a new SymMat of this type from a square Mat of the right
+ /// size, that is expected to be symmetric (hermitian) to within
+ /// a tolerance. All elements are used; we average the upper and
+ /// lower elements of the Mat to produce the corresponding element
+ /// of the SymMat.
+ template <class EE, int CSS, int RSS>
+ SymMat& setFromSymmetric(const Mat<M,M,EE,CSS,RSS>& m) {
+ SimTK_ERRCHK1(m.isNumericallySymmetric(), "SymMat::setFromSymmetric()",
+ "The allegedly symmetric source matrix was not symmetric to within "
+ "a tolerance of %g.", m.getDefaultTolerance());
+ this->updDiag() = m.diag().real();
+ for (int j=0; j<M; ++j)
+ for (int i=j+1; i<M; ++i)
+ this->updEltLower(i,j) =
+ (m(i,j) + CNT<EE>::transpose(m(j,i)))/2;
+ return *this;
+ }
+
+ /// This is an \e implicit conversion from a SymMat of the same length
+ /// and element type but with different spacing.
+ template <int RSS> SymMat(const SymMat<M,E,RSS>& src)
+ { updAsVec() = src.getAsVec(); }
+
+ /// This is an \e implicit conversion from a SymMat of the same length
+ /// and \e negated element type, possibly with different spacing.
+ template <int RSS> SymMat(const SymMat<M,ENeg,RSS>& src)
+ { updAsVec() = src.getAsVec(); }
+
+ /// Construct a SymMat from a SymMat of the same dimensions, with any
+ /// element type and spacing. Works as long as the element types are
+ /// assignment compatible.
+ template <class EE, int RSS> explicit SymMat(const SymMat<M,EE,RSS>& src)
+ { updAsVec() = src.getAsVec(); }
+
+ // Construction using an element repeats that element on the diagonal
+ // but sets the rest of the matrix to zero.
+ explicit SymMat(const E& e) {
+ updDiag() = CNT<E>::real(e);
+ for (int i=0; i < NLowerElements; ++i) updlowerE(i) = E(0);
+ }
+
+ // Construction using a negated element is just like construction from
+ // the element.
+ explicit SymMat(const ENeg& e) {
+ updDiag() = CNT<ENeg>::real(e);
+ for (int i=0; i < NLowerElements; ++i) updlowerE(i) = E(0);
+ }
+
+ // Given an int, turn it into a suitable floating point number
+ // and then feed that to the above single-element constructor.
+ explicit SymMat(int i)
+ { new (this) SymMat(E(Precision(i))); }
+
+ /// A bevy of constructors from individual exact-match elements IN ROW ORDER,
+ /// giving the LOWER TRIANGLE, like this:
+ /// <pre>
+ /// a
+ /// b c
+ /// d e f
+ /// g h i j
+ /// </pre>
+ /// Note that this will be mapped to our diagonal/lower layout, which in
+ /// the above example would be:
+ /// <pre>
+ /// [a c f j][b d g e h i]
+ /// </pre>
+
+ SymMat(const E& e0,
+ const E& e1,const E& e2)
+ { assert(M==2); TDiag& d=updDiag(); TLower& l=updLower();
+ d[0]=CNT<E>::real(e0); d[1]=CNT<E>::real(e2);
+ l[0]=e1; }
+
+ SymMat(const E& e0,
+ const E& e1,const E& e2,
+ const E& e3,const E& e4, const E& e5)
+ { assert(M==3); TDiag& d=updDiag(); TLower& l=updLower();
+ d[0]=CNT<E>::real(e0);d[1]=CNT<E>::real(e2);d[2]=CNT<E>::real(e5);
+ l[0]=e1;l[1]=e3;
+ l[2]=e4; }
+
+ SymMat(const E& e0,
+ const E& e1,const E& e2,
+ const E& e3,const E& e4, const E& e5,
+ const E& e6,const E& e7, const E& e8, const E& e9)
+ { assert(M==4); TDiag& d=updDiag(); TLower& l=updLower();
+ d[0]=CNT<E>::real(e0);d[1]=CNT<E>::real(e2);d[2]=CNT<E>::real(e5);d[3]=CNT<E>::real(e9);
+ l[0]=e1;l[1]=e3;l[2]=e6;
+ l[3]=e4;l[4]=e7;
+ l[5]=e8; }
+
+ SymMat(const E& e0,
+ const E& e1, const E& e2,
+ const E& e3, const E& e4, const E& e5,
+ const E& e6, const E& e7, const E& e8, const E& e9,
+ const E& e10,const E& e11, const E& e12, const E& e13, const E& e14)
+ { assert(M==5); TDiag& d=updDiag(); TLower& l=updLower();
+ d[0]=CNT<E>::real(e0);d[1]=CNT<E>::real(e2);d[2]=CNT<E>::real(e5);d[3]=CNT<E>::real(e9);d[4]=CNT<E>::real(e14);
+ l[0]=e1;l[1]=e3;l[2]=e6;l[3]=e10;
+ l[4]=e4;l[5]=e7;l[6]=e11;
+ l[7]=e8;l[8]=e12;
+ l[9]=e13; }
+
+ SymMat(const E& e0,
+ const E& e1, const E& e2,
+ const E& e3, const E& e4, const E& e5,
+ const E& e6, const E& e7, const E& e8, const E& e9,
+ const E& e10,const E& e11, const E& e12, const E& e13, const E& e14,
+ const E& e15,const E& e16, const E& e17, const E& e18, const E& e19, const E& e20)
+ { assert(M==6); TDiag& d=updDiag(); TLower& l=updLower();
+ d[0]=CNT<E>::real(e0);d[1]=CNT<E>::real(e2);d[2]=CNT<E>::real(e5);
+ d[3]=CNT<E>::real(e9);d[4]=CNT<E>::real(e14);d[5]=CNT<E>::real(e20);
+ l[0] =e1; l[1] =e3; l[2] =e6; l[3]=e10; l[4]=e15;
+ l[5] =e4; l[6] =e7; l[7] =e11; l[8]=e16;
+ l[9] =e8; l[10]=e12;l[11]=e17;
+ l[12]=e13;l[13]=e18;
+ l[14]=e19; }
+
+ // Construction from a pointer to anything assumes we're pointing
+ // at a packed element list of the right length, providing the
+ // lower triangle in row order, so a b c d e f means
+ // a
+ // b c
+ // d e f
+ // g h i j
+ // This has to be mapped to our diagonal/lower layout, which in
+ // the above example will be:
+ // [a c f j][b d g e h i]
+ //
+ // In the input layout, the i'th row begins at element i(i+1)/2,
+ // so diagonals are at i(i+1)/2 + i, while lower
+ // elements (i,j; i>j) are at i(i+1)/2 + j.
+ template <class EE> explicit SymMat(const EE* p) {
+ assert(p);
+ for (int i=0; i<M; ++i) {
+ const int rowStart = (i*(i+1))/2;
+ updDiag()[i] = CNT<EE>::real(p[rowStart + i]);
+ for (int j=0; j<i; ++j)
+ updEltLower(i,j) = p[rowStart + j];
+ }
+ }
+
+ // This is the same thing except as an assignment to pointer rather
+ // than a constructor from pointer.
+ template <class EE> SymMat& operator=(const EE* p) {
+ assert(p);
+ for (int i=0; i<M; ++i) {
+ const int rowStart = (i*(i+1))/2;
+ updDiag()[i] = CNT<EE>::real(p[rowStart + i]);
+ for (int j=0; j<i; ++j)
+ updEltLower(i,j) = p[rowStart + j];
+ }
+ return *this;
+ }
+
+ // Assignment works similarly to copy -- if the lengths match,
+ // go element-by-element. Otherwise, zero and then copy to each
+ // diagonal element.
+ template <class EE, int RSS> SymMat& operator=(const SymMat<M,EE,RSS>& mm) {
+ updAsVec() = mm.getAsVec();
+ return *this;
+ }
+
+
+ // Conforming assignment ops
+ template <class EE, int RSS> SymMat&
+ operator+=(const SymMat<M,EE,RSS>& mm) {
+ updAsVec() += mm.getAsVec();
+ return *this;
+ }
+ template <class EE, int RSS> SymMat&
+ operator+=(const SymMat<M,negator<EE>,RSS>& mm) {
+ updAsVec() -= -mm.getAsVec(); // negation of negator is free
+ return *this;
+ }
+
+ template <class EE, int RSS> SymMat&
+ operator-=(const SymMat<M,EE,RSS>& mm) {
+ updAsVec() -= mm.getAsVec();
+ return *this;
+ }
+ template <class EE, int RSS> SymMat&
+ operator-=(const SymMat<M,negator<EE>,RSS>& mm) {
+ updAsVec() += -mm.getAsVec(); // negation of negator is free
+ return *this;
+ }
+
+ // In place matrix multiply can only be done when the RHS matrix is the same
+ // size and the elements are also *= compatible.
+ template <class EE, int RSS> SymMat&
+ operator*=(const SymMat<M,EE,RSS>& mm) {
+ assert(false); // TODO
+ return *this;
+ }
+
+ // Conforming binary ops with 'this' on left, producing new packed result.
+ // Cases: sy=sy+-sy, m=sy+-m, m=sy*sy, m=sy*m, v=sy*v
+
+ // sy= this + sy
+ template <class E2, int RS2>
+ typename Result<SymMat<M,E2,RS2> >::Add
+ conformingAdd(const SymMat<M,E2,RS2>& r) const {
+ return typename Result<SymMat<M,E2,RS2> >::Add
+ (getAsVec().conformingAdd(r.getAsVec()));
+ }
+ // m= this - m
+ template <class E2, int RS2>
+ typename Result<SymMat<M,E2,RS2> >::Sub
+ conformingSubtract(const SymMat<M,E2,RS2>& r) const {
+ return typename Result<SymMat<M,E2,RS2> >::Sub
+ (getAsVec().conformingSubtract(r.getAsVec()));
+ }
+
+ // symmetric * symmetric produces a full result
+ // m= this * s
+ // TODO: this is not a good implementation
+ template <class E2, int RS2>
+ typename Result<SymMat<M,E2,RS2> >::Mul
+ conformingMultiply(const SymMat<M,E2,RS2>& s) const {
+ typename Result<SymMat<M,E2,RS2> >::Mul result;
+ for (int j=0;j<M;++j)
+ for (int i=0;i<M;++i)
+ result(i,j) = (*this)[i] * s(j);
+ return result;
+ }
+
+ // sy= this .* sy
+ template <class E2, int RS2>
+ typename EltResult<E2>::Mul
+ elementwiseMultiply(const SymMat<M,E2,RS2>& r) const {
+ return typename EltResult<E2>::Mul
+ (getAsVec().elementwiseMultiply(r.getAsVec()));
+ }
+
+ // sy= this ./ sy
+ template <class E2, int RS2>
+ typename EltResult<E2>::Dvd
+ elementwiseDivide(const SymMat<M,E2,RS2>& r) const {
+ return typename EltResult<E2>::Dvd
+ (getAsVec().elementwiseDivide(r.getAsVec()));
+ }
+
+ // TODO: need the rest of the SymMat operators
+
+ // Must be i >= j.
+ const E& operator()(int i,int j) const
+ { return i==j ? getDiag()[i] : getEltLower(i,j); }
+ E& operator()(int i,int j)
+ { return i==j ? updDiag()[i] : updEltLower(i,j); }
+
+ // These are slow for a symmetric matrix, requiring copying and
+ // possibly floating point operations for conjugation.
+ TRow operator[](int i) const {return row(i);}
+ TCol operator()(int j) const {return col(j);}
+
+
+ // This is the scalar Frobenius norm.
+ ScalarNormSq normSqr() const { return scalarNormSqr(); }
+ typename CNT<ScalarNormSq>::TSqrt
+ norm() const { return CNT<ScalarNormSq>::sqrt(scalarNormSqr()); }
+
+ /// There is no conventional meaning for normalize() applied to a matrix. We
+ /// choose to define it as follows:
+ /// If the elements of this SymMat are scalars, the result is what you get by
+ /// dividing each element by the Frobenius norm() calculated above. If the elements are
+ /// *not* scalars, then the elements are *separately* normalized.
+ ///
+ /// Normalize returns a matrix of the same dimension but in new, packed storage
+ /// and with a return type that does not include negator<> even if the original
+ /// SymMat<> does, because we can eliminate the negation here almost for free.
+ /// But we can't standardize (change conjugate to complex) for free, so we'll retain
+ /// conjugates if there are any.
+ TNormalize normalize() const {
+ if (CNT<E>::IsScalar) {
+ return castAwayNegatorIfAny() / (SignInterpretation*norm());
+ } else {
+ TNormalize elementwiseNormalized;
+ // punt to the equivalent Vec to get the elements normalized
+ elementwiseNormalized.updAsVec() = getAsVec().normalize();
+ return elementwiseNormalized;
+ }
+ }
+
+ TInvert invert() const {assert(false); return TInvert();} // TODO default inversion
+
+ const SymMat& operator+() const { return *this; }
+ const TNeg& operator-() const { return negate(); }
+ TNeg& operator-() { return updNegate(); }
+ const THerm& operator~() const { return transpose(); }
+ THerm& operator~() { return updTranspose(); }
+
+ const TNeg& negate() const { return *reinterpret_cast<const TNeg*>(this); }
+ TNeg& updNegate() { return *reinterpret_cast<TNeg*>(this); }
+
+ const THerm& transpose() const { return *reinterpret_cast<const THerm*>(this); }
+ THerm& updTranspose() { return *reinterpret_cast<THerm*>(this); }
+
+ const TPosTrans& positionalTranspose() const
+ { return *reinterpret_cast<const TPosTrans*>(this); }
+ TPosTrans& updPositionalTranspose()
+ { return *reinterpret_cast<TPosTrans*>(this); }
+
+ const TReal& real() const { return *reinterpret_cast<const TReal*>(this); }
+ TReal& real() { return *reinterpret_cast< TReal*>(this); }
+
+ // Had to contort these routines to get them through VC++ 7.net
+ const TImag& imag() const {
+ const int offs = ImagOffset;
+ const EImag* p = reinterpret_cast<const EImag*>(this);
+ return *reinterpret_cast<const TImag*>(p+offs);
+ }
+ TImag& imag() {
+ const int offs = ImagOffset;
+ EImag* p = reinterpret_cast<EImag*>(this);
+ return *reinterpret_cast<TImag*>(p+offs);
+ }
+
+ const TWithoutNegator& castAwayNegatorIfAny() const {return *reinterpret_cast<const TWithoutNegator*>(this);}
+ TWithoutNegator& updCastAwayNegatorIfAny() {return *reinterpret_cast<TWithoutNegator*>(this);}
+
+ // These are elementwise binary operators, (this op ee) by default but (ee op this) if
+ // 'FromLeft' appears in the name. The result is a packed SymMat<M> but the element type
+ // may change. These are mostly used to implement global operators.
+ // We call these "scalar" operators but actually the "scalar" can be a composite type.
+
+ //TODO: consider converting 'e' to Standard Numbers as precalculation and changing
+ // return type appropriately.
+ template <class EE> SymMat<M, typename CNT<E>::template Result<EE>::Mul>
+ scalarMultiply(const EE& e) const {
+ SymMat<M, typename CNT<E>::template Result<EE>::Mul> result;
+ result.updAsVec() = getAsVec().scalarMultiply(e);
+ return result;
+ }
+ template <class EE> SymMat<M, typename CNT<EE>::template Result<E>::Mul>
+ scalarMultiplyFromLeft(const EE& e) const {
+ SymMat<M, typename CNT<EE>::template Result<E>::Mul> result;
+ result.updAsVec() = getAsVec().scalarMultiplyFromLeft(e);
+ return result;
+ }
+
+ // TODO: should precalculate and store 1/e, while converting to Standard Numbers. Note
+ // that return type should change appropriately.
+ template <class EE> SymMat<M, typename CNT<E>::template Result<EE>::Dvd>
+ scalarDivide(const EE& e) const {
+ SymMat<M, typename CNT<E>::template Result<EE>::Dvd> result;
+ result.updAsVec() = getAsVec().scalarDivide(e);
+ return result;
+ }
+ template <class EE> SymMat<M, typename CNT<EE>::template Result<E>::Dvd>
+ scalarDivideFromLeft(const EE& e) const {
+ SymMat<M, typename CNT<EE>::template Result<E>::Dvd> result;
+ result.updAsVec() = getAsVec().scalarDivideFromLeft(e);
+ return result;
+ }
+
+ // Additive ops involving a scalar update only the diagonal
+ template <class EE> SymMat<M, typename CNT<E>::template Result<EE>::Add>
+ scalarAdd(const EE& e) const {
+ SymMat<M, typename CNT<E>::template Result<EE>::Add> result(*this);
+ result.updDiag() += e;
+ return result;
+ }
+ // Add is commutative, so no 'FromLeft'.
+
+ template <class EE> SymMat<M, typename CNT<E>::template Result<EE>::Sub>
+ scalarSubtract(const EE& e) const {
+ SymMat<M, typename CNT<E>::template Result<EE>::Sub> result(*this);
+ result.diag() -= e;
+ return result;
+ }
+ // This is s-m; negate m and add s to diagonal
+ // TODO: Should do something clever with negation here.
+ template <class EE> SymMat<M, typename CNT<EE>::template Result<E>::Sub>
+ scalarSubtractFromLeft(const EE& e) const {
+ SymMat<M, typename CNT<EE>::template Result<E>::Sub> result(-(*this));
+ result.diag() += e;
+ return result;
+ }
+
+ // Generic assignments for any element type not listed explicitly, including scalars.
+ // These are done repeatedly for each element and only work if the operation can
+ // be performed leaving the original element type.
+ template <class EE> SymMat& operator =(const EE& e) {return scalarEq(e);}
+ template <class EE> SymMat& operator+=(const EE& e) {return scalarPlusEq(e);}
+ template <class EE> SymMat& operator-=(const EE& e) {return scalarMinusEq(e);}
+ template <class EE> SymMat& operator*=(const EE& e) {return scalarTimesEq(e);}
+ template <class EE> SymMat& operator/=(const EE& e) {return scalarDivideEq(e);}
+
+ // Generalized scalar assignment & computed assignment methods. These will work
+ // for any assignment-compatible element, not just scalars.
+ template <class EE> SymMat& scalarEq(const EE& ee)
+ { updDiag() = ee; updLower() = E(0); return *this; }
+ template <class EE> SymMat& scalarPlusEq(const EE& ee)
+ { updDiag() += ee; return *this; }
+ template <class EE> SymMat& scalarMinusEq(const EE& ee)
+ { updDiag() -= ee; return *this; }
+
+ // this is m = s-m; negate off diagonal, do d=s-d for each diagonal element d
+ template <class EE> SymMat& scalarMinusEqFromLeft(const EE& ee)
+ { updLower() *= E(-1); updDiag().scalarMinusEqFromLeft(ee); return *this; }
+
+ template <class EE> SymMat& scalarTimesEq(const EE& ee)
+ { updAsVec() *= ee; return *this; }
+ template <class EE> SymMat& scalarTimesEqFromLeft(const EE& ee)
+ { updAsVec().scalarTimesEqFromLeft(ee); return *this; }
+ template <class EE> SymMat& scalarDivideEq(const EE& ee)
+ { updAsVec() /= ee; return *this; }
+ template <class EE> SymMat& scalarDivideEqFromLeft(const EE& ee)
+ { updAsVec().scalarDivideEqFromLeft(ee); return *this; }
+
+ void setToNaN() { updAsVec().setToNaN(); }
+ void setToZero() { updAsVec().setToZero(); }
+
+ // These assume we are given a pointer to d[0] of a SymMat<M,E,RS> like this one.
+ static const SymMat& getAs(const ELT* p) {return *reinterpret_cast<const SymMat*>(p);}
+ static SymMat& updAs(ELT* p) {return *reinterpret_cast<SymMat*>(p);}
+
+ // Note packed spacing
+ static TPacked getNaN() {
+ return TPacked(CNT<typename TPacked::TDiag>::getNaN(),
+ CNT<typename TPacked::TLower>::getNaN());
+ }
+
+ /// Return true if any element of this SymMat contains a NaN anywhere.
+ bool isNaN() const {return getAsVec().isNaN();}
+
+ /// Return true if any element of this SymMat contains a +Inf
+ /// or -Inf somewhere but no element contains a NaN anywhere.
+ bool isInf() const {return getAsVec().isInf();}
+
+ /// Return true if no element contains an Infinity or a NaN.
+ bool isFinite() const {return getAsVec().isFinite();}
+
+ /// For approximate comparisions, the default tolerance to use for a matrix is
+ /// its shortest dimension times its elements' default tolerance.
+ static double getDefaultTolerance() {return M*CNT<ELT>::getDefaultTolerance();}
+
+ /// %Test whether this matrix is numerically equal to some other matrix with
+ /// the same shape, using a specified tolerance.
+ template <class E2, int RS2>
+ bool isNumericallyEqual(const SymMat<M,E2,RS2>& m, double tol) const {
+ return getAsVec().isNumericallyEqual(m.getAsVec(), tol);
+ }
+
+ /// %Test whether this matrix is numerically equal to some other matrix with
+ /// the same shape, using a default tolerance which is the looser of the
+ /// default tolerances of the two objects being compared.
+ template <class E2, int RS2>
+ bool isNumericallyEqual(const SymMat<M,E2,RS2>& m) const {
+ const double tol = std::max(getDefaultTolerance(),m.getDefaultTolerance());
+ return isNumericallyEqual(m, tol);
+ }
+
+ /// %Test whether this is numerically a "scalar" matrix, meaning that it is
+ /// a diagonal matrix in which each diagonal element is numerically equal to
+ /// the same scalar, using either a specified tolerance or the matrix's
+ /// default tolerance (which is always the same or looser than the default
+ /// tolerance for one of its elements).
+ bool isNumericallyEqual
+ (const ELT& e,
+ double tol = getDefaultTolerance()) const
+ {
+ if (!diag().isNumericallyEqual(e, tol))
+ return false;
+ return getLower().isNumericallyEqual(ELT(0), tol);
+ }
+
+ // Rows and columns have to be copied and Hermitian elements have to
+ // be conjugated at a floating point cost. This isn't the best way
+ // to work with a symmetric matrix.
+ TRow row(int i) const {
+ SimTK_INDEXCHECK(i,M,"SymMat::row[i]");
+ TRow rowi;
+ // Columns left of diagonal are lower.
+ for (int j=0; j<i; ++j)
+ rowi[j] = getEltLower(i,j);
+ rowi[i] = getEltDiag(i);
+ for (int j=i+1; j<M; ++j)
+ rowi[j] = getEltUpper(i,j); // conversion from EHerm to E may cost flops
+ return rowi;
+ }
+
+ TCol col(int j) const {
+ SimTK_INDEXCHECK(j,M,"SymMat::col(j)");
+ TCol colj;
+ // Rows above diagonal are upper (with conjugated elements).
+ for (int i=0; i<j; ++i)
+ colj[i] = getEltUpper(i,j); // conversion from EHerm to E may cost flops
+ colj[j] = getEltDiag(j);
+ for (int i=j+1; i<M; ++i)
+ colj[i] = getEltLower(i,j);
+ return colj;
+ }
+
+ /// Return a value for \e any element of a symmetric matrix, even those
+ /// in the upper triangle which aren't actually stored anywhere. For elements
+ /// whose underlying numeric types are complex, this will require computation
+ /// in order to return the conjugates. So we always have to copy out the
+ /// element, and may also have to conjugate it (one flop per complex number).
+ E elt(int i, int j) const {
+ SimTK_INDEXCHECK(i,M,"SymMat::elt(i,j)");
+ SimTK_INDEXCHECK(j,M,"SymMat::elt(i,j)");
+ if (i>j) return getEltLower(i,j); // copy element
+ else if (i==j) return getEltDiag(i); // copy element
+ else return getEltUpper(i,j); // conversion from EHerm to E may cost flops
+ }
+
+ const TDiag& getDiag() const {return TDiag::getAs(d);}
+ TDiag& updDiag() {return TDiag::updAs(d);}
+
+ // Conventional synonym
+ const TDiag& diag() const {return getDiag();}
+ TDiag& diag() {return updDiag();}
+
+ const TLower& getLower() const {return TLower::getAs(&d[M*RS]);}
+ TLower& updLower() {return TLower::updAs(&d[M*RS]);}
+
+ const TUpper& getUpper() const {return reinterpret_cast<const TUpper&>(getLower());}
+ TUpper& updUpper() {return reinterpret_cast< TUpper&>(updLower());}
+
+ const TAsVec& getAsVec() const {return TAsVec::getAs(d);}
+ TAsVec& updAsVec() {return TAsVec::updAs(d);}
+
+ const E& getEltDiag(int i) const {return getDiag()[i];}
+ E& updEltDiag(int i) {return updDiag()[i];}
+
+ // must be i > j
+ const E& getEltLower(int i, int j) const {return getLower()[lowerIx(i,j)];}
+ E& updEltLower(int i, int j) {return updLower()[lowerIx(i,j)];}
+
+ // must be i < j
+ const EHerm& getEltUpper(int i, int j) const {return getUpper()[lowerIx(j,i)];}
+ EHerm& updEltUpper(int i, int j) {return updUpper()[lowerIx(j,i)];}
+
+ /// Returns a row vector (Row) containing the column sums of this matrix.
+ TRow colSum() const {
+ TRow temp(~getDiag());
+ for (int i = 1; i < M; ++i)
+ for (int j = 0; j < i; ++j) {
+ const E& value = getEltLower(i, j);;
+ temp[j] += value;
+ temp[i] += E(reinterpret_cast<const EHerm&>(value));
+ }
+ return temp;
+ }
+ /// This is an alternate name for colSum(); behaves like the Matlab
+ /// function of the same name.
+ TRow sum() const {return colSum();}
+
+ /// Returns a column vector (Vec) containing the row sums of this matrix.
+ /// This will be the conjugate transpose of the column sums.
+ TCol rowSum() const {
+ TCol temp(getDiag());
+ for (int i = 1; i < M; ++i)
+ for (int j = 0; j < i; ++j) {
+ const E& value = getEltLower(i, j);;
+ temp[i] += value;
+ temp[j] += E(reinterpret_cast<const EHerm&>(value));
+ }
+ return temp;
+ }
+private:
+ E d[NActualElements];
+
+ // This utility doesn't turn lower or upper into a Vec which could turn
+ // out to have zero length if this is a 1x1 matrix.
+ const E& getlowerE(int i) const {return d[(M+i)*RS];}
+ E& updlowerE(int i) {return d[(M+i)*RS];}
+
+ SymMat(const TDiag& di, const TLower& low) {
+ updDiag() = di; updLower() = low;
+ }
+
+ explicit SymMat(const TAsVec& v) {
+ TAsVec::updAs(d) = v;
+ }
+
+ // Convert i,j with i>j to an index in "lower". This also gives the
+ // right index for "upper" with i and j reversed.
+ static int lowerIx(int i, int j) {
+ assert(0 <= j && j < i && i < M);
+ return (i-j-1) + j*(M-1) - (j*(j-1))/2;
+ }
+
+ template <int MM, class EE, int RSS> friend class SymMat;
+};
+
+////////////////////////////////////////////////////////
+// Global operators involving two symmetric matrices. //
+// sy=sy+sy, sy=sy-sy, m=sy*sy, sy==sy, sy!=sy //
+////////////////////////////////////////////////////////
+
+// m3 = m1 + m2 where all m's have the same dimension M.
+template <int M, class E1, int S1, class E2, int S2> inline
+typename SymMat<M,E1,S1>::template Result< SymMat<M,E2,S2> >::Add
+operator+(const SymMat<M,E1,S1>& l, const SymMat<M,E2,S2>& r) {
+ return SymMat<M,E1,S1>::template Result< SymMat<M,E2,S2> >
+ ::AddOp::perform(l,r);
+}
+
+// m3 = m1 - m2 where all m's have the same dimension M.
+template <int M, class E1, int S1, class E2, int S2> inline
+typename SymMat<M,E1,S1>::template Result< SymMat<M,E2,S2> >::Sub
+operator-(const SymMat<M,E1,S1>& l, const SymMat<M,E2,S2>& r) {
+ return SymMat<M,E1,S1>::template Result< SymMat<M,E2,S2> >
+ ::SubOp::perform(l,r);
+}
+
+// m3 = m1 * m2 where all m's have the same dimension M.
+// The result will not be symmetric.
+template <int M, class E1, int S1, class E2, int S2> inline
+typename SymMat<M,E1,S1>::template Result< SymMat<M,E2,S2> >::Mul
+operator*(const SymMat<M,E1,S1>& l, const SymMat<M,E2,S2>& r) {
+ return SymMat<M,E1,S1>::template Result< SymMat<M,E2,S2> >
+ ::MulOp::perform(l,r);
+}
+
+// bool = m1 == m2, m1 and m2 have the same dimension M
+template <int M, class E1, int S1, class E2, int S2> inline bool
+operator==(const SymMat<M,E1,S1>& l, const SymMat<M,E2,S2>& r) {
+ return l.getAsVec() == r.getAsVec();
+}
+
+// bool = m1 == m2, m1 and m2 have the same dimension M
+template <int M, class E1, int S1, class E2, int S2> inline bool
+operator!=(const SymMat<M,E1,S1>& l, const SymMat<M,E2,S2>& r) {return !(l==r);}
+
+///////////////////////////////////////////////////////
+// Global operators involving a SymMat and a scalar. //
+///////////////////////////////////////////////////////
+
+// SCALAR MULTIPLY
+
+// m = m*real, real*m
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<float>::Mul
+operator*(const SymMat<M,E,S>& l, const float& r)
+ { return SymMat<M,E,S>::template Result<float>::MulOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<float>::Mul
+operator*(const float& l, const SymMat<M,E,S>& r) {return r*l;}
+
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<double>::Mul
+operator*(const SymMat<M,E,S>& l, const double& r)
+ { return SymMat<M,E,S>::template Result<double>::MulOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<double>::Mul
+operator*(const double& l, const SymMat<M,E,S>& r) {return r*l;}
+
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<long double>::Mul
+operator*(const SymMat<M,E,S>& l, const long double& r)
+ { return SymMat<M,E,S>::template Result<long double>::MulOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<long double>::Mul
+operator*(const long double& l, const SymMat<M,E,S>& r) {return r*l;}
+
+// m = m*int, int*m -- just convert int to m's precision float
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<typename CNT<E>::Precision>::Mul
+operator*(const SymMat<M,E,S>& l, int r) {return l * (typename CNT<E>::Precision)r;}
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<typename CNT<E>::Precision>::Mul
+operator*(int l, const SymMat<M,E,S>& r) {return r * (typename CNT<E>::Precision)l;}
+
+// Complex, conjugate, and negator are all easy to templatize.
+
+// m = m*complex, complex*m
+template <int M, class E, int S, class R> inline
+typename SymMat<M,E,S>::template Result<std::complex<R> >::Mul
+operator*(const SymMat<M,E,S>& l, const std::complex<R>& r)
+ { return SymMat<M,E,S>::template Result<std::complex<R> >::MulOp::perform(l,r); }
+template <int M, class E, int S, class R> inline
+typename SymMat<M,E,S>::template Result<std::complex<R> >::Mul
+operator*(const std::complex<R>& l, const SymMat<M,E,S>& r) {return r*l;}
+
+// m = m*conjugate, conjugate*m (convert conjugate->complex)
+template <int M, class E, int S, class R> inline
+typename SymMat<M,E,S>::template Result<std::complex<R> >::Mul
+operator*(const SymMat<M,E,S>& l, const conjugate<R>& r) {return l*(std::complex<R>)r;}
+template <int M, class E, int S, class R> inline
+typename SymMat<M,E,S>::template Result<std::complex<R> >::Mul
+operator*(const conjugate<R>& l, const SymMat<M,E,S>& r) {return r*(std::complex<R>)l;}
+
+// m = m*negator, negator*m: convert negator to standard number
+template <int M, class E, int S, class R> inline
+typename SymMat<M,E,S>::template Result<typename negator<R>::StdNumber>::Mul
+operator*(const SymMat<M,E,S>& l, const negator<R>& r) {return l * (typename negator<R>::StdNumber)(R)r;}
+template <int M, class E, int S, class R> inline
+typename SymMat<M,E,S>::template Result<typename negator<R>::StdNumber>::Mul
+operator*(const negator<R>& l, const SymMat<M,E,S>& r) {return r * (typename negator<R>::StdNumber)(R)l;}
+
+
+// SCALAR DIVIDE. This is a scalar operation when the scalar is on the right,
+// but when it is on the left it means scalar * pseudoInverse(mat), which is
+// a similar symmetric matrix.
+
+// m = m/real, real/m
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<float>::Dvd
+operator/(const SymMat<M,E,S>& l, const float& r)
+ { return SymMat<M,E,S>::template Result<float>::DvdOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename CNT<float>::template Result<SymMat<M,E,S> >::Dvd
+operator/(const float& l, const SymMat<M,E,S>& r)
+ { return CNT<float>::template Result<SymMat<M,E,S> >::DvdOp::perform(l,r); }
+
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<double>::Dvd
+operator/(const SymMat<M,E,S>& l, const double& r)
+ { return SymMat<M,E,S>::template Result<double>::DvdOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename CNT<double>::template Result<SymMat<M,E,S> >::Dvd
+operator/(const double& l, const SymMat<M,E,S>& r)
+ { return CNT<double>::template Result<SymMat<M,E,S> >::DvdOp::perform(l,r); }
+
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<long double>::Dvd
+operator/(const SymMat<M,E,S>& l, const long double& r)
+ { return SymMat<M,E,S>::template Result<long double>::DvdOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename CNT<long double>::template Result<SymMat<M,E,S> >::Dvd
+operator/(const long double& l, const SymMat<M,E,S>& r)
+ { return CNT<long double>::template Result<SymMat<M,E,S> >::DvdOp::perform(l,r); }
+
+// m = m/int, int/m -- just convert int to m's precision float
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<typename CNT<E>::Precision>::Dvd
+operator/(const SymMat<M,E,S>& l, int r) {return l / (typename CNT<E>::Precision)r;}
+template <int M, class E, int S> inline
+typename CNT<typename CNT<E>::Precision>::template Result<SymMat<M,E,S> >::Dvd
+operator/(int l, const SymMat<M,E,S>& r) {return (typename CNT<E>::Precision)l / r;}
+
+
+// Complex, conjugate, and negator are all easy to templatize.
+
+// m = m/complex, complex/m
+template <int M, class E, int S, class R> inline
+typename SymMat<M,E,S>::template Result<std::complex<R> >::Dvd
+operator/(const SymMat<M,E,S>& l, const std::complex<R>& r)
+ { return SymMat<M,E,S>::template Result<std::complex<R> >::DvdOp::perform(l,r); }
+template <int M, class E, int S, class R> inline
+typename CNT<std::complex<R> >::template Result<SymMat<M,E,S> >::Dvd
+operator/(const std::complex<R>& l, const SymMat<M,E,S>& r)
+ { return CNT<std::complex<R> >::template Result<SymMat<M,E,S> >::DvdOp::perform(l,r); }
+
+// m = m/conjugate, conjugate/m (convert conjugate->complex)
+template <int M, class E, int S, class R> inline
+typename SymMat<M,E,S>::template Result<std::complex<R> >::Dvd
+operator/(const SymMat<M,E,S>& l, const conjugate<R>& r) {return l/(std::complex<R>)r;}
+template <int M, class E, int S, class R> inline
+typename CNT<std::complex<R> >::template Result<SymMat<M,E,S> >::Dvd
+operator/(const conjugate<R>& l, const SymMat<M,E,S>& r) {return (std::complex<R>)l/r;}
+
+// m = m/negator, negator/m: convert negator to number
+template <int M, class E, int S, class R> inline
+typename SymMat<M,E,S>::template Result<typename negator<R>::StdNumber>::Dvd
+operator/(const SymMat<M,E,S>& l, const negator<R>& r) {return l/(typename negator<R>::StdNumber)(R)r;}
+template <int M, class E, int S, class R> inline
+typename CNT<R>::template Result<SymMat<M,E,S> >::Dvd
+operator/(const negator<R>& l, const SymMat<M,E,S>& r) {return (typename negator<R>::StdNumber)(R)l/r;}
+
+
+// Add and subtract are odd as scalar ops. They behave as though the
+// scalar stands for a conforming matrix whose diagonal elements are that,
+// scalar and then a normal matrix add or subtract is done.
+
+// SCALAR ADD
+
+// m = m+real, real+m
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<float>::Add
+operator+(const SymMat<M,E,S>& l, const float& r)
+ { return SymMat<M,E,S>::template Result<float>::AddOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<float>::Add
+operator+(const float& l, const SymMat<M,E,S>& r) {return r+l;}
+
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<double>::Add
+operator+(const SymMat<M,E,S>& l, const double& r)
+ { return SymMat<M,E,S>::template Result<double>::AddOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<double>::Add
+operator+(const double& l, const SymMat<M,E,S>& r) {return r+l;}
+
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<long double>::Add
+operator+(const SymMat<M,E,S>& l, const long double& r)
+ { return SymMat<M,E,S>::template Result<long double>::AddOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<long double>::Add
+operator+(const long double& l, const SymMat<M,E,S>& r) {return r+l;}
+
+// m = m+int, int+m -- just convert int to m's precision float
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<typename CNT<E>::Precision>::Add
+operator+(const SymMat<M,E,S>& l, int r) {return l + (typename CNT<E>::Precision)r;}
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<typename CNT<E>::Precision>::Add
+operator+(int l, const SymMat<M,E,S>& r) {return r + (typename CNT<E>::Precision)l;}
+
+// Complex, conjugate, and negator are all easy to templatize.
+
+// m = m+complex, complex+m
+template <int M, class E, int S, class R> inline
+typename SymMat<M,E,S>::template Result<std::complex<R> >::Add
+operator+(const SymMat<M,E,S>& l, const std::complex<R>& r)
+ { return SymMat<M,E,S>::template Result<std::complex<R> >::AddOp::perform(l,r); }
+template <int M, class E, int S, class R> inline
+typename SymMat<M,E,S>::template Result<std::complex<R> >::Add
+operator+(const std::complex<R>& l, const SymMat<M,E,S>& r) {return r+l;}
+
+// m = m+conjugate, conjugate+m (convert conjugate->complex)
+template <int M, class E, int S, class R> inline
+typename SymMat<M,E,S>::template Result<std::complex<R> >::Add
+operator+(const SymMat<M,E,S>& l, const conjugate<R>& r) {return l+(std::complex<R>)r;}
+template <int M, class E, int S, class R> inline
+typename SymMat<M,E,S>::template Result<std::complex<R> >::Add
+operator+(const conjugate<R>& l, const SymMat<M,E,S>& r) {return r+(std::complex<R>)l;}
+
+// m = m+negator, negator+m: convert negator to standard number
+template <int M, class E, int S, class R> inline
+typename SymMat<M,E,S>::template Result<typename negator<R>::StdNumber>::Add
+operator+(const SymMat<M,E,S>& l, const negator<R>& r) {return l + (typename negator<R>::StdNumber)(R)r;}
+template <int M, class E, int S, class R> inline
+typename SymMat<M,E,S>::template Result<typename negator<R>::StdNumber>::Add
+operator+(const negator<R>& l, const SymMat<M,E,S>& r) {return r + (typename negator<R>::StdNumber)(R)l;}
+
+// SCALAR SUBTRACT -- careful, not commutative.
+
+// m = m-real, real-m
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<float>::Sub
+operator-(const SymMat<M,E,S>& l, const float& r)
+ { return SymMat<M,E,S>::template Result<float>::SubOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename CNT<float>::template Result<SymMat<M,E,S> >::Sub
+operator-(const float& l, const SymMat<M,E,S>& r)
+ { return CNT<float>::template Result<SymMat<M,E,S> >::SubOp::perform(l,r); }
+
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<double>::Sub
+operator-(const SymMat<M,E,S>& l, const double& r)
+ { return SymMat<M,E,S>::template Result<double>::SubOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename CNT<double>::template Result<SymMat<M,E,S> >::Sub
+operator-(const double& l, const SymMat<M,E,S>& r)
+ { return CNT<double>::template Result<SymMat<M,E,S> >::SubOp::perform(l,r); }
+
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<long double>::Sub
+operator-(const SymMat<M,E,S>& l, const long double& r)
+ { return SymMat<M,E,S>::template Result<long double>::SubOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename CNT<long double>::template Result<SymMat<M,E,S> >::Sub
+operator-(const long double& l, const SymMat<M,E,S>& r)
+ { return CNT<long double>::template Result<SymMat<M,E,S> >::SubOp::perform(l,r); }
+
+// m = m-int, int-m // just convert int to m's precision float
+template <int M, class E, int S> inline
+typename SymMat<M,E,S>::template Result<typename CNT<E>::Precision>::Sub
+operator-(const SymMat<M,E,S>& l, int r) {return l - (typename CNT<E>::Precision)r;}
+template <int M, class E, int S> inline
+typename CNT<typename CNT<E>::Precision>::template Result<SymMat<M,E,S> >::Sub
+operator-(int l, const SymMat<M,E,S>& r) {return (typename CNT<E>::Precision)l - r;}
+
+
+// Complex, conjugate, and negator are all easy to templatize.
+
+// m = m-complex, complex-m
+template <int M, class E, int S, class R> inline
+typename SymMat<M,E,S>::template Result<std::complex<R> >::Sub
+operator-(const SymMat<M,E,S>& l, const std::complex<R>& r)
+ { return SymMat<M,E,S>::template Result<std::complex<R> >::SubOp::perform(l,r); }
+template <int M, class E, int S, class R> inline
+typename CNT<std::complex<R> >::template Result<SymMat<M,E,S> >::Sub
+operator-(const std::complex<R>& l, const SymMat<M,E,S>& r)
+ { return CNT<std::complex<R> >::template Result<SymMat<M,E,S> >::SubOp::perform(l,r); }
+
+// m = m-conjugate, conjugate-m (convert conjugate->complex)
+template <int M, class E, int S, class R> inline
+typename SymMat<M,E,S>::template Result<std::complex<R> >::Sub
+operator-(const SymMat<M,E,S>& l, const conjugate<R>& r) {return l-(std::complex<R>)r;}
+template <int M, class E, int S, class R> inline
+typename CNT<std::complex<R> >::template Result<SymMat<M,E,S> >::Sub
+operator-(const conjugate<R>& l, const SymMat<M,E,S>& r) {return (std::complex<R>)l-r;}
+
+// m = m-negator, negator-m: convert negator to standard number
+template <int M, class E, int S, class R> inline
+typename SymMat<M,E,S>::template Result<typename negator<R>::StdNumber>::Sub
+operator-(const SymMat<M,E,S>& l, const negator<R>& r) {return l-(typename negator<R>::StdNumber)(R)r;}
+template <int M, class E, int S, class R> inline
+typename CNT<R>::template Result<SymMat<M,E,S> >::Sub
+operator-(const negator<R>& l, const SymMat<M,E,S>& r) {return (typename negator<R>::StdNumber)(R)l-r;}
+
+
+// SymMat I/O
+template <int M, class E, int RS, class CHAR, class TRAITS> inline
+std::basic_ostream<CHAR,TRAITS>&
+operator<<(std::basic_ostream<CHAR,TRAITS>& o, const SymMat<M,E,RS>& m) {
+ for (int i=0;i<M;++i) {
+ o << std::endl << "[";
+ for (int j=0; j<=i; ++j)
+ o << (j>0?" ":"") << m(i,j);
+ for (int j=i+1; j<M; ++j)
+ o << " *";
+ o << "]";
+ }
+ if (M) o << std::endl;
+ return o;
+}
+
+template <int M, class E, int RS, class CHAR, class TRAITS> inline
+std::basic_istream<CHAR,TRAITS>&
+operator>>(std::basic_istream<CHAR,TRAITS>& is, SymMat<M,E,RS>& m) {
+ // TODO: not sure how to do input yet
+ assert(false);
+ return is;
+}
+
+} //namespace SimTK
+
+
+#endif //SimTK_SIMMATRIX_SMALLMATRIX_SYMMAT_H_
diff --git a/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/Vec.h b/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/Vec.h
new file mode 100644
index 0000000..fad047a
--- /dev/null
+++ b/SimTKcommon/SmallMatrix/include/SimTKcommon/internal/Vec.h
@@ -0,0 +1,1430 @@
+#ifndef SimTK_SIMMATRIX_SMALLMATRIX_VEC_H_
+#define SimTK_SIMMATRIX_SMALLMATRIX_VEC_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Declaration of class Vec<NROWS, ELEMENT_TYPE, STRIDE>.
+ */
+
+#include "SimTKcommon/internal/common.h"
+
+namespace SimTK {
+
+
+// The following functions are used internally by Vec.
+
+// Hide from Doxygen.
+/** @cond **/
+namespace Impl {
+
+// For those wimpy compilers that don't unroll short, constant-limit loops,
+// Peter Eastman added these recursive template implementations of
+// elementwise add, subtract, and copy. Sherm added multiply and divide.
+
+template <class E1, int S1, class E2, int S2> void
+conformingAdd(const Vec<1,E1,S1>& r1, const Vec<1,E2,S2>& r2,
+ Vec<1,typename CNT<E1>::template Result<E2>::Add>& result) {
+ result[0] = r1[0] + r2[0];
+}
+template <int N, class E1, int S1, class E2, int S2> void
+conformingAdd(const Vec<N,E1,S1>& r1, const Vec<N,E2,S2>& r2,
+ Vec<N,typename CNT<E1>::template Result<E2>::Add>& result) {
+ conformingAdd(reinterpret_cast<const Vec<N-1,E1,S1>&>(r1),
+ reinterpret_cast<const Vec<N-1,E2,S2>&>(r2),
+ reinterpret_cast<Vec<N-1,typename CNT<E1>::
+ template Result<E2>::Add>&>(result));
+ result[N-1] = r1[N-1] + r2[N-1];
+}
+
+template <class E1, int S1, class E2, int S2> void
+conformingSubtract(const Vec<1,E1,S1>& r1, const Vec<1,E2,S2>& r2,
+ Vec<1,typename CNT<E1>::template Result<E2>::Sub>& result) {
+ result[0] = r1[0] - r2[0];
+}
+template <int N, class E1, int S1, class E2, int S2> void
+conformingSubtract(const Vec<N,E1,S1>& r1, const Vec<N,E2,S2>& r2,
+ Vec<N,typename CNT<E1>::template Result<E2>::Sub>& result) {
+ conformingSubtract(reinterpret_cast<const Vec<N-1,E1,S1>&>(r1),
+ reinterpret_cast<const Vec<N-1,E2,S2>&>(r2),
+ reinterpret_cast<Vec<N-1,typename CNT<E1>::
+ template Result<E2>::Sub>&>(result));
+ result[N-1] = r1[N-1] - r2[N-1];
+}
+
+template <class E1, int S1, class E2, int S2> void
+elementwiseMultiply(const Vec<1,E1,S1>& r1, const Vec<1,E2,S2>& r2,
+ Vec<1,typename CNT<E1>::template Result<E2>::Mul>& result) {
+ result[0] = r1[0] * r2[0];
+}
+template <int N, class E1, int S1, class E2, int S2> void
+elementwiseMultiply(const Vec<N,E1,S1>& r1, const Vec<N,E2,S2>& r2,
+ Vec<N,typename CNT<E1>::template Result<E2>::Mul>& result) {
+ elementwiseMultiply(reinterpret_cast<const Vec<N-1,E1,S1>&>(r1),
+ reinterpret_cast<const Vec<N-1,E2,S2>&>(r2),
+ reinterpret_cast<Vec<N-1,typename CNT<E1>::
+ template Result<E2>::Mul>&>(result));
+ result[N-1] = r1[N-1] * r2[N-1];
+}
+
+template <class E1, int S1, class E2, int S2> void
+elementwiseDivide(const Vec<1,E1,S1>& r1, const Vec<1,E2,S2>& r2,
+ Vec<1,typename CNT<E1>::template Result<E2>::Dvd>& result) {
+ result[0] = r1[0] / r2[0];
+}
+template <int N, class E1, int S1, class E2, int S2> void
+elementwiseDivide(const Vec<N,E1,S1>& r1, const Vec<N,E2,S2>& r2,
+ Vec<N,typename CNT<E1>::template Result<E2>::Dvd>& result) {
+ elementwiseDivide(reinterpret_cast<const Vec<N-1,E1,S1>&>(r1),
+ reinterpret_cast<const Vec<N-1,E2,S2>&>(r2),
+ reinterpret_cast<Vec<N-1,typename CNT<E1>::
+ template Result<E2>::Dvd>&>(result));
+ result[N-1] = r1[N-1] / r2[N-1];
+}
+
+template <class E1, int S1, class E2, int S2> void
+copy(Vec<1,E1,S1>& r1, const Vec<1,E2,S2>& r2) {
+ r1[0] = r2[0];
+}
+template <int N, class E1, int S1, class E2, int S2> void
+copy(Vec<N,E1,S1>& r1, const Vec<N,E2,S2>& r2) {
+ copy(reinterpret_cast<Vec<N-1,E1,S1>&>(r1),
+ reinterpret_cast<const Vec<N-1,E2,S2>&>(r2));
+ r1[N-1] = r2[N-1];
+}
+
+}
+/** @endcond **/
+
+/** This is a fixed-length column vector designed for no-overhead inline
+computation.
+
+ at ingroup MatVecUtilities
+
+ at tparam M The number of rows in the vector.
+ at tparam ELT The element type. Must be a composite numerical type (CNT).
+ The default is ELT=Real.
+ at tparam STRIDE The spacing from one element to the next in memory, as an
+ integer number of elements of type ELT. The default is
+ STRIDE=1.
+
+<b>Usage</b>
+
+The %Vec and @ref SimTK::Vector_ "Vector" classes are commonly used to represent
+tuples of Real values, and have methods like %norm() to calculate the vector
+2-norm. Use %Vec for a small vector whose length is known at compile time;
+otherwise, use @ref SimTK::Vector_ "Vector". To collect elements of the same
+type that do not constitute a tuple, it is more appropriate to use the Array_
+container. Some common %Vec use cases are provided below.
+
+<b>Construction</b>
+
+The 3-tuple <tt>~[0,0,0]</tt> can be created in the following ways:
+\code
+Vec<3,Real>(0,0,0);
+Vec3(0,0,0);
+Vec3(0);
+\endcode
+Note that the default element type is Real, and that Vec3 is a typedef for
+%Vec<3>; analogous typedefs exist for vectors of up to 9 elements.
+
+<b>Manipulation</b>
+
+Standard arithmetic operators can be used, as well as methods like %sum() and
+%normalize(). Here are some usage examples, each of which returns
+<tt>~[1,2,3]</tt>:
+\code
+Vec9(0,0,0,0,0,1,2,3,0).getSubVec<3>(5);
+Vec4(1,2,4,3).drop1(2);
+Vec2(2,3).insert1(0,1);
+Vec2(1,2).append1(3);
+\endcode
+
+<b>Conversion</b>
+
+It may be necessary to convert between a %Vec and a Vector (to interface with
+%FactorQTZ, for instance). In the example below, we print a Vec3 created from a
+3-element Vector:
+\code
+Vector myVector(3);
+for (int i=0; i<myVector.size(); ++i) myVector[i]=i+1;
+std::cout << Vec3(&myVector[0]) << std::endl;
+\endcode
+Converting from a Vec3 to a Vector is also straightforward:
+\code
+Vec3 myVec3(1,2,3);
+std::cout << Vector(myVec3) << std::endl;
+\endcode
+
+ at see Vector_ for handling of large or variable-sized vectors.
+ at see Array_, Mat, Matrix_
+**/
+template <int M, class ELT, int STRIDE>
+class Vec {
+public:
+ /** @name Advanced
+ These are obscure members of %Vec that are used for template metaprogramming
+ and can be ignored by most users. **/
+ /**@{**/
+ /** Element type of this %Vec. **/
+ typedef ELT E;
+ /** Negated version of this %Vec's element type; ENeg==negator< E >. **/
+ typedef typename CNT<E>::TNeg ENeg;
+ /** Element type, stripped of negator<> if it has one. **/
+ typedef typename CNT<E>::TWithoutNegator EWithoutNegator;
+ /** Type showing just the real part of an element of this %Vec if elements
+ are complex; otherwise just the element type. **/
+ typedef typename CNT<E>::TReal EReal;
+ /** Type showing the imaginary part of an element of this %Vec as real,
+ if elements are complex; otherwise a type that can hold a zero of the
+ element type. **/
+ typedef typename CNT<E>::TImag EImag;
+ /** Type that elements would have if complex, if E is currently real;
+ otherwise just the element type E. **/
+ typedef typename CNT<E>::TComplex EComplex;
+ /** Type of the Hermitian transpose of an element of this %Vec. **/
+ typedef typename CNT<E>::THerm EHerm;
+ /** Type of a \e positional transpose of an element of this %Vec. **/
+ typedef typename CNT<E>::TPosTrans EPosTrans;
+ /** Type of the expression ~E*E (default vector and matrix square;
+ symmetric). **/
+ typedef typename CNT<E>::TSqHermT ESqHermT;
+ /** Type of the expression E*~E ("row square"; symmetric). **/
+ typedef typename CNT<E>::TSqTHerm ESqTHerm;
+ /** Type required to hold the result of sqrt(E). **/
+ typedef typename CNT<E>::TSqrt ESqrt;
+ /** Type required to hold the result of abs(E). **/
+ typedef typename CNT<E>::TAbs EAbs;
+ /** Return type of standardize(E) method; a packed type that can hold the
+ value of an element after eliminating negator and conjugate types. **/
+ typedef typename CNT<E>::TStandard EStandard;
+ /** Packed type that can hold the value returned from invert(E), the
+ inverse type of an element. **/
+ typedef typename CNT<E>::TInvert EInvert;
+ /** Packed type that can hold the value returned from normalize(E). **/
+ typedef typename CNT<E>::TNormalize ENormalize;
+
+ typedef typename CNT<E>::Scalar EScalar;
+ typedef typename CNT<E>::ULessScalar EULessScalar;
+ typedef typename CNT<E>::Number ENumber;
+ typedef typename CNT<E>::StdNumber EStdNumber;
+ typedef typename CNT<E>::Precision EPrecision;
+ typedef typename CNT<E>::ScalarNormSq EScalarNormSq;
+
+ /** These compile-time constants are required of every Composite
+ Numerical Type (CNT). **/
+ enum {
+ NRows = M,
+ NCols = 1,
+ NPackedElements = M,
+ NActualElements = M * STRIDE, // includes trailing gap
+ NActualScalars = CNT<E>::NActualScalars * NActualElements,
+ RowSpacing = STRIDE,
+ ColSpacing = NActualElements,
+ ImagOffset = NTraits<ENumber>::ImagOffset,
+ RealStrideFactor = 1, // composite types don't change size when
+ // cast from complex to real or imaginary
+ ArgDepth = ((int)CNT<E>::ArgDepth < (int)MAX_RESOLVED_DEPTH
+ ? CNT<E>::ArgDepth + 1
+ : MAX_RESOLVED_DEPTH),
+ IsScalar = 0,
+ IsULessScalar = 0,
+ IsNumber = 0,
+ IsStdNumber = 0,
+ IsPrecision = 0,
+ SignInterpretation = CNT<E>::SignInterpretation
+ };
+
+ // These are reinterpretations of the current data, so have the
+ // same packing (stride).
+
+ /** The type of this Vec. **/
+ typedef Vec<M,E,STRIDE> T;
+ /** Type this Vec would have if its elements were interpreted as
+ negated. **/
+ typedef Vec<M,ENeg,STRIDE> TNeg;
+ /** Type of this Vec with negator removed from its element type, if
+ the element is negated. **/
+ typedef Vec<M,EWithoutNegator,STRIDE> TWithoutNegator;
+ /** Type of this Vec cast to show only the real part of its element;
+ this might affect the stride. **/
+ typedef Vec<M,EReal,STRIDE*CNT<E>::RealStrideFactor>
+ TReal;
+ /** Type of this Vec cast to show only the imaginary part of its element;
+ this might affect the stride. **/
+ typedef Vec<M,EImag,STRIDE*CNT<E>::RealStrideFactor>
+ TImag;
+ typedef Vec<M,EComplex,STRIDE> TComplex;
+ /** Type of this Vec after casting to its Hermitian transpose; that is,
+ the Vec turns into a Row and each element turns into \e its Hermitian
+ transpose. **/
+ typedef Row<M,EHerm,STRIDE> THerm;
+ /** Type of this Vec after casting to its positional transpose; that is,
+ the Vec turns into a Row but the element type remains unchanged. **/
+ typedef Row<M,E,STRIDE> TPosTrans;
+ /** Element type of this Vec. **/
+ typedef E TElement;
+ /** Type of a row of this CNT object (for a Vec, just its element type). **/
+ typedef E TRow;
+ /** Type of a column of this CNT object (for a Vec, the whole thing). **/
+ typedef Vec TCol;
+
+ // These are the results of calculations, so are returned in new, packed
+ // memory. Be sure to refer to element types here which are also packed.
+ typedef Vec<M,ESqrt,1> TSqrt; // Note stride
+ typedef Vec<M,EAbs,1> TAbs; // Note stride
+ typedef Vec<M,EStandard,1> TStandard;
+ typedef Row<M,EInvert,1> TInvert;
+ typedef Vec<M,ENormalize,1> TNormalize;
+
+ typedef ESqHermT TSqHermT; // result of self dot product
+ typedef SymMat<M,ESqTHerm> TSqTHerm; // result of self outer product
+
+ // These recurse right down to the underlying scalar type no matter how
+ // deep the elements are.
+ typedef EScalar Scalar;
+ typedef EULessScalar ULessScalar;
+ typedef ENumber Number;
+ typedef EStdNumber StdNumber;
+ typedef EPrecision Precision;
+ typedef EScalarNormSq ScalarNormSq;
+ /**@}**/
+
+ /** The number of elements in this Vec (note that stride does not
+ affect this number.) **/
+ static int size() { return M; }
+ /** The number of rows in a Vec is the number of elements. **/
+ static int nrow() { return M; }
+ /** The number of columns in a Vec is always 1. **/
+ static int ncol() { return 1; }
+
+
+ /** Scalar norm square is sum( conjugate squares of all underlying scalars ),
+ where conjugate square of scalar s is conj(s)*s. **/
+ ScalarNormSq scalarNormSqr() const {
+ ScalarNormSq sum(0);
+ for(int i=0;i<M;++i) sum += CNT<E>::scalarNormSqr(d[i*STRIDE]);
+ return sum;
+ }
+
+ /** Elementwise square root; that is, the return value has the same
+ length as this Vec but with each element replaced by whatever it thinks
+ its square root is. The element type may have changed and the stride
+ of the return Vec is always 1. **/
+ TSqrt sqrt() const {
+ TSqrt vsqrt;
+ for(int i=0;i<M;++i) vsqrt[i] = CNT<E>::sqrt(d[i*STRIDE]);
+ return vsqrt;
+ }
+
+ /** Elementwise absolute value; that is, the return value has the same
+ dimension as this Vec but with each element replaced by whatever it thinks
+ its absolute value is. The element type may have changed and the stride
+ of the return Vec is always 1. **/
+ TAbs abs() const {
+ TAbs vabs;
+ for(int i=0;i<M;++i) vabs[i] = CNT<E>::abs(d[i*STRIDE]);
+ return vabs;
+ }
+
+ /** Return a copy of this Vec but with the underlying scalar type
+ converted (if necessary) to one of the C++ standard real or complex
+ floating point types. This may require floating point negations to
+ occur to get read of negator or conjugate types. **/
+ TStandard standardize() const {
+ TStandard vstd;
+ for(int i=0;i<M;++i) vstd[i] = CNT<E>::standardize(d[i*STRIDE]);
+ return vstd;
+ }
+
+ /** Sum just adds up all the elements into a single return element that
+ is the same type as this Vec's elements except standardized to use one
+ of the C++ built-in real or complex types as its underlying scalars. **/
+ EStandard sum() const {
+ E sum(0);
+ for (int i=0;i<M;++i) sum += d[i*STRIDE];
+ return CNT<E>::standardize(sum);
+ }
+
+
+ // This gives the resulting vector type when (v[i] op P) is applied to
+ // each element of v. It is a vector of length M, stride 1, and element
+ // types which are the regular composite result of E op P. Typically P is
+ // a scalar type but it doesn't have to be.
+ template <class P> struct EltResult {
+ typedef Vec<M, typename CNT<E>::template Result<P>::Mul, 1> Mul;
+ typedef Vec<M, typename CNT<E>::template Result<P>::Dvd, 1> Dvd;
+ typedef Vec<M, typename CNT<E>::template Result<P>::Add, 1> Add;
+ typedef Vec<M, typename CNT<E>::template Result<P>::Sub, 1> Sub;
+ };
+
+ // This is the composite result for v op P where P is some kind of
+ // appropriately shaped non-scalar type.
+ template <class P> struct Result {
+ typedef MulCNTs<M,1,ArgDepth,Vec,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> MulOp;
+ typedef typename MulOp::Type Mul;
+
+ typedef MulCNTsNonConforming<M,1,ArgDepth,Vec,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> MulOpNonConforming;
+ typedef typename MulOpNonConforming::Type MulNon;
+
+ typedef DvdCNTs<M,1,ArgDepth,Vec,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> DvdOp;
+ typedef typename DvdOp::Type Dvd;
+
+ typedef AddCNTs<M,1,ArgDepth,Vec,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> AddOp;
+ typedef typename AddOp::Type Add;
+
+ typedef SubCNTs<M,1,ArgDepth,Vec,ColSpacing,RowSpacing,
+ CNT<P>::NRows, CNT<P>::NCols, CNT<P>::ArgDepth,
+ P, CNT<P>::ColSpacing, CNT<P>::RowSpacing> SubOp;
+ typedef typename SubOp::Type Sub;
+ };
+
+ /** Shape-preserving element substitution (always packed). That is, if
+ T1==%Vec\<M,E,S> is the current %Vec type, then type
+ T2==T1::%Substitute\<P>::%Type is the type of a shape-compatible,
+ packed %Vec whose elements are of type P rather than E, that is,
+ T2==%Vec\<M,P,1>. **/
+ template <class P> struct Substitute {
+ typedef Vec<M,P> Type;
+ };
+
+ /** Default construction initializes %Vec's elements to NaN when debugging
+ but leaves them uninitialized garbage otherwise, so declarations have zero
+ cost in Release builds. **/
+ Vec(){
+ #ifndef NDEBUG
+ setToNaN();
+ #endif
+ }
+
+ // It's important not to use the default copy constructor or copy
+ // assignment because the compiler doesn't understand that we may
+ // have noncontiguous storage and will try to copy the whole array.
+
+ /** Copy constructor copies the logically-included elements from the
+ source %Vec; gaps due to stride are not accessed in either source or
+ destination. **/
+ Vec(const Vec& src) {
+ Impl::copy(*this, src);
+ }
+ /** Copy assignment operator copies the logically-included elements from
+ the source %Vec; gaps due to stride are not accessed in either source or
+ destination. OK if source and destination are the same vector; results
+ are unpredictable if they otherwise overlap with elements in common. **/
+ Vec& operator=(const Vec& src) {
+ Impl::copy(*this, src);
+ return *this;
+ }
+
+ /** This is an implicit conversion from a %Vec of the same length
+ and element type but with a different stride. **/
+ template <int SS> Vec(const Vec<M,E,SS>& src) {
+ Impl::copy(*this, src);
+ }
+
+ /** This is an implicit conversion from a %Vec of the same length
+ and \e negated element type (possibly with a different stride). **/
+ template <int SS> Vec(const Vec<M,ENeg,SS>& src) {
+ Impl::copy(*this, src);
+ }
+
+ /** Construct a Vec from a Vec of the same length, with any stride. Works
+ as long as the element types are assignment compatible. **/
+ template <class EE, int SS> explicit Vec(const Vec<M,EE,SS>& src) {
+ Impl::copy(*this, src);
+ }
+
+ /** Construction from a single value of this %Vec's element type assigns
+ that value to each element. **/
+ explicit Vec(const E& e) {for (int i=0;i<M;++i) d[i*STRIDE]=e;}
+
+ /** Construction from a single value of this %Vec's negated element type
+ assigns that value to each element, requiring floating point negation
+ to be performed once to compute the type-E representation of the
+ type negator<E> value provided. **/
+ explicit Vec(const ENeg& ne) {
+ const E e = ne; // requires floating point negation
+ for (int i=0;i<M;++i) d[i*STRIDE]=e;
+ }
+
+ /** Given an int value, turn it into a suitable floating point number,
+ convert that to element type E and then feed that to the above
+ single-element constructor.
+ @see Vec::Vec(const E&). **/
+ explicit Vec(int i) {new (this) Vec(E(Precision(i)));}
+
+ // A bevy of constructors for Vecs up to length 9.
+
+ /** Construct a Vec<2,E> from two elements of type E, etc. **/
+ Vec(const E& e0,const E& e1)
+ { assert(M==2);(*this)[0]=e0;(*this)[1]=e1; }
+ Vec(const E& e0,const E& e1,const E& e2)
+ { assert(M==3);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2; }
+ Vec(const E& e0,const E& e1,const E& e2,const E& e3)
+ { assert(M==4);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2;(*this)[3]=e3; }
+ Vec(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4)
+ { assert(M==5);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2;
+ (*this)[3]=e3;(*this)[4]=e4; }
+ Vec(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,const E& e5)
+ { assert(M==6);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2;
+ (*this)[3]=e3;(*this)[4]=e4;(*this)[5]=e5; }
+ Vec(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,const E& e5, const E& e6)
+ { assert(M==7);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2;
+ (*this)[3]=e3;(*this)[4]=e4;(*this)[5]=e5;(*this)[6]=e6; }
+ Vec(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,const E& e5, const E& e6, const E& e7)
+ { assert(M==8);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2;
+ (*this)[3]=e3;(*this)[4]=e4;(*this)[5]=e5;(*this)[6]=e6;(*this)[7]=e7; }
+ Vec(const E& e0,const E& e1,const E& e2,const E& e3,const E& e4,const E& e5, const E& e6, const E& e7, const E& e8)
+ { assert(M==9);(*this)[0]=e0;(*this)[1]=e1;(*this)[2]=e2;
+ (*this)[3]=e3;(*this)[4]=e4;(*this)[5]=e5;(*this)[6]=e6;(*this)[7]=e7;(*this)[8]=e8; }
+
+ /** Construction from a pointer to elements of any type EE assumes we're
+ pointing at a C++ array of EE's of the right length, and that EE is
+ assignment compatible with this %Vec's element type E. The supplied
+ pointer cannot be null. **/
+ template <class EE> explicit Vec(const EE* p)
+ { assert(p); for(int i=0;i<M;++i) d[i*STRIDE]=p[i]; }
+
+ /** Assignment to a pointer to elements of any type EE assumes we're
+ pointing at a C++ array of EE's of the right length, and that EE is
+ assignment compatible with this %Vec's element type E. The supplied
+ pointer cannot be null. **/
+ template <class EE> Vec& operator=(const EE* p)
+ { assert(p); for(int i=0;i<M;++i) d[i*STRIDE]=p[i]; return *this; }
+
+ /** Assignment to a conforming %Vec, of any element type and stride,
+ provided that the element types are assignment-compatible. **/
+ template <class EE, int SS> Vec& operator=(const Vec<M,EE,SS>& vv)
+ { Impl::copy(*this, vv); return *this; }
+
+ /** Add in a conforming %Vec, of any element type and stride,
+ provided that the element types are addition-compatible. **/
+ template <class EE, int SS> Vec& operator+=(const Vec<M,EE,SS>& r)
+ { for(int i=0;i<M;++i) d[i*STRIDE] += r[i]; return *this; }
+ /** Add in a conforming %Vec, of any negated element type and stride,
+ provided that the element types are addition-compatible. The negation
+ is removed at zero cost by subtracting rather than adding. **/
+ template <class EE, int SS> Vec& operator+=(const Vec<M,negator<EE>,SS>& r)
+ { for(int i=0;i<M;++i) d[i*STRIDE] -= -(r[i]); return *this; }
+
+ /** Subtract off a conforming %Vec, of any element type and stride,
+ provided that the element types are addition-compatible. **/
+ template <class EE, int SS> Vec& operator-=(const Vec<M,EE,SS>& r)
+ { for(int i=0;i<M;++i) d[i*STRIDE] -= r[i]; return *this; }
+ /** Subtract off a conforming %Vec, of any negated element type and stride,
+ provided that the element types are addition-compatible. The negation
+ is removed at zero cost by adding rather than subtracting. **/
+ template <class EE, int SS> Vec& operator-=(const Vec<M,negator<EE>,SS>& r)
+ { for(int i=0;i<M;++i) d[i*STRIDE] += -(r[i]); return *this; }
+
+ // Conforming binary ops with 'this' on left, producing new packed result.
+ // Cases: v=v+v, v=v-v, m=v*r
+
+ /** Vector addition -- use operator+ instead. **/
+ template <class EE, int SS> Vec<M,typename CNT<E>::template Result<EE>::Add>
+ conformingAdd(const Vec<M,EE,SS>& r) const {
+ Vec<M,typename CNT<E>::template Result<EE>::Add> result;
+ Impl::conformingAdd(*this, r, result);
+ return result;
+ }
+ /** Vector subtraction -- use operator- instead. **/
+ template <class EE, int SS> Vec<M,typename CNT<E>::template Result<EE>::Sub>
+ conformingSubtract(const Vec<M,EE,SS>& r) const {
+ Vec<M,typename CNT<E>::template Result<EE>::Sub> result;
+ Impl::conformingSubtract(*this, r, result);
+ return result;
+ }
+
+ /** Same as outer product (m = col*row) -- use operator* or outer()
+ instead. **/
+ template <class EE, int SS> Mat<M,M,typename CNT<E>::template Result<EE>::Mul>
+ conformingMultiply(const Row<M,EE,SS>& r) const {
+ Mat<M,M,typename CNT<E>::template Result<EE>::Mul> result;
+ for (int j=0;j<M;++j) result(j) = scalarMultiply(r(j));
+ return result;
+ }
+
+ /** Elementwise multiply (Matlab " .* " operator). **/
+ template <class EE, int SS> Vec<M,typename CNT<E>::template Result<EE>::Mul>
+ elementwiseMultiply(const Vec<M,EE,SS>& r) const {
+ Vec<M,typename CNT<E>::template Result<EE>::Mul> result;
+ Impl::elementwiseMultiply(*this, r, result);
+ return result;
+ }
+ /** Elementwise divide (Matlab " ./ " operator). **/
+ template <class EE, int SS> Vec<M,typename CNT<E>::template Result<EE>::Dvd>
+ elementwiseDivide(const Vec<M,EE,SS>& r) const {
+ Vec<M,typename CNT<E>::template Result<EE>::Dvd> result;
+ Impl::elementwiseDivide(*this, r, result);
+ return result;
+ }
+
+ /** Select an element of this %Vec and return a const reference to it.
+ This is range-checked in Debug builds but has zero overhead in Release
+ builds. **/
+ const E& operator[](int i) const
+ { assert(0 <= i && i < M); return d[i*STRIDE]; }
+ /** Same as const operator[] above. **/
+ const E& operator()(int i) const {return (*this)[i];}
+
+ /** Select an element of this %Vec and return a writable reference
+ to it. This is range-checked in Debug builds but has zero overhead in
+ Release builds. **/
+ E& operator[](int i) {assert(0 <= i && i < M); return d[i*STRIDE];}
+ /** Same as non-const operator[] above. **/
+ E& operator()(int i) {return (*this)[i];}
+
+ ScalarNormSq normSqr() const { return scalarNormSqr(); }
+ typename CNT<ScalarNormSq>::TSqrt
+ norm() const { return CNT<ScalarNormSq>::sqrt(scalarNormSqr()); }
+
+ /** If the elements of this Vec are scalars, the result is what you get by
+ dividing each element by the norm() calculated above. If the elements are
+ \e not scalars, then the elements are *separately* normalized. That means
+ you will get a different answer from Vec<2,Vec3>::normalize() than you
+ would from a Vec<6>::normalize() containing the same scalars.
+
+ Normalize returns a vector of the same dimension but in new, packed storage
+ and with a return type that does not include negator<> even if the original
+ Vec<> does, because we can eliminate the negation here almost for free.
+ But we can't standardize (change conjugate to complex) for free, so we'll retain
+ conjugates if there are any. **/
+ TNormalize normalize() const {
+ if (CNT<E>::IsScalar) {
+ return castAwayNegatorIfAny() / (SignInterpretation*norm());
+ } else {
+ TNormalize elementwiseNormalized;
+ for (int i=0; i<M; ++i)
+ elementwiseNormalized[i] = CNT<E>::normalize((*this)[i]);
+ return elementwiseNormalized;
+ }
+ }
+
+ /** This method is not supported for %Vec objects. **/
+ TInvert invert() const {assert(false); return TInvert();} // TODO default inversion
+
+ /** Unary plus does nothing. **/
+ const Vec& operator+() const { return *this; }
+ /** Unary minus recasts this %Vec to a type that has the opposite
+ interpretation of the sign but is otherwise identical, so no computation
+ or copying is performed here. **/
+ const TNeg& operator-() const { return negate(); }
+ /** Recast to negated type and return a writable reference; writing to
+ this will cause the negated result to be placed in the original %Vec. **/
+ TNeg& operator-() { return updNegate(); }
+ /** The Hermitian transpose operator recasts this %Vec to a type that
+ specifies the opposite storage order (row vs.\ column) then returns a
+ reference, so no computation or copying is performed here. **/
+ const THerm& operator~() const { return transpose(); }
+ /** Recast to Hermitian transposed type and return a writable reference;
+ the effect is that writing to elements of the result affects the transposed
+ element of the original %Vec. **/
+ THerm& operator~() { return updTranspose(); }
+
+ /** Non-operator version of unary negation; just a recast. **/
+ const TNeg& negate() const { return *reinterpret_cast<const TNeg*>(this); }
+ /** Non-operator version of unary negation; recasts and returns a
+ writable reference. **/
+ TNeg& updNegate() { return *reinterpret_cast< TNeg*>(this); }
+
+ /** Non-operator version of Hermitian transpose; just a recast. **/
+ const THerm& transpose() const { return *reinterpret_cast<const THerm*>(this); }
+ /** Non-operator version of Hermitian transpose; recasts and returns a
+ writable reference. **/
+ THerm& updTranspose() { return *reinterpret_cast< THerm*>(this); }
+
+ /** Positional transpose turns this %Vec into a Row but does not transpose
+ the individual elements. That is, a Vec<2,Vec3> becomes a Row<2,Vec3>,
+ rather than a Row<2,Row3> as would happen with ordinary transpose(). This
+ is just a recast; no copying or computation is performed here. **/
+ const TPosTrans& positionalTranspose() const
+ { return *reinterpret_cast<const TPosTrans*>(this); }
+ /** Positional transpose returning a writable reference. **/
+ TPosTrans& updPositionalTranspose()
+ { return *reinterpret_cast<TPosTrans*>(this); }
+
+ /** Return a reference to the real portion of this %Vec if it has complex
+ elements; otherwise the type doesn't change. This is just a recast; no
+ copying or computation is done here. The result may have a different
+ stride than the original since the imaginary parts must be skipped. **/
+ const TReal& real() const { return *reinterpret_cast<const TReal*>(this); }
+ /** Recast to show only the real portion of this %Vec and return a writable
+ reference. **/
+ TReal& real() { return *reinterpret_cast< TReal*>(this); }
+
+ // Had to contort these next two routines to get them through VC++ 7.net
+
+ /** Return a reference to the imaginary portion of this %Vec if it has
+ complex elements; otherwise the type doesn't change. This is just a recast;
+ no copying or computation is done here. The result may have a different
+ stride than the original since the real parts must be skipped. **/
+ const TImag& imag() const {
+ const int offs = ImagOffset;
+ const EImag* p = reinterpret_cast<const EImag*>(this);
+ return *reinterpret_cast<const TImag*>(p+offs);
+ }
+ /** Recast to show only the imaginary portion of this %Vec and return a
+ writable reference. **/
+ TImag& imag() {
+ const int offs = ImagOffset;
+ EImag* p = reinterpret_cast<EImag*>(this);
+ return *reinterpret_cast<TImag*>(p+offs);
+ }
+
+ /** Recast to remove negators from this %Vec's type if present; this is
+ handy for simplifying operations where we know the sign can be ignored
+ such as squaring. **/
+ const TWithoutNegator& castAwayNegatorIfAny() const
+ { return *reinterpret_cast<const TWithoutNegator*>(this); }
+ /** Recast to remove negators from this %Vec's type if present and return
+ a writable reference. **/
+ TWithoutNegator& updCastAwayNegatorIfAny()
+ { return *reinterpret_cast<TWithoutNegator*>(this); }
+
+ // These are elementwise binary operators, (this op ee) by default but
+ // (ee op this) if 'FromLeft' appears in the name. The result is a packed
+ // Vec<M> but the element type may change. These are mostly used to
+ // implement global operators. We call these "scalar" operators but
+ // actually the "scalar" can be a composite type.
+
+ //TODO: consider converting 'e' to Standard Numbers as precalculation and
+ // changing return type appropriately.
+ template <class EE> Vec<M, typename CNT<E>::template Result<EE>::Mul>
+ scalarMultiply(const EE& e) const {
+ Vec<M, typename CNT<E>::template Result<EE>::Mul> result;
+ for (int i=0; i<M; ++i) result[i] = (*this)[i] * e;
+ return result;
+ }
+ template <class EE> Vec<M, typename CNT<EE>::template Result<E>::Mul>
+ scalarMultiplyFromLeft(const EE& e) const {
+ Vec<M, typename CNT<EE>::template Result<E>::Mul> result;
+ for (int i=0; i<M; ++i) result[i] = e * (*this)[i];
+ return result;
+ }
+
+ // TODO: should precalculate and store 1/e, while converting to Standard
+ // Numbers. Note that return type should change appropriately.
+ template <class EE> Vec<M, typename CNT<E>::template Result<EE>::Dvd>
+ scalarDivide(const EE& e) const {
+ Vec<M, typename CNT<E>::template Result<EE>::Dvd> result;
+ for (int i=0; i<M; ++i) result[i] = (*this)[i] / e;
+ return result;
+ }
+ template <class EE> Vec<M, typename CNT<EE>::template Result<E>::Dvd>
+ scalarDivideFromLeft(const EE& e) const {
+ Vec<M, typename CNT<EE>::template Result<E>::Dvd> result;
+ for (int i=0; i<M; ++i) result[i] = e / (*this)[i];
+ return result;
+ }
+
+ template <class EE> Vec<M, typename CNT<E>::template Result<EE>::Add>
+ scalarAdd(const EE& e) const {
+ Vec<M, typename CNT<E>::template Result<EE>::Add> result;
+ for (int i=0; i<M; ++i) result[i] = (*this)[i] + e;
+ return result;
+ }
+ // Add is commutative, so no 'FromLeft'.
+
+ template <class EE> Vec<M, typename CNT<E>::template Result<EE>::Sub>
+ scalarSubtract(const EE& e) const {
+ Vec<M, typename CNT<E>::template Result<EE>::Sub> result;
+ for (int i=0; i<M; ++i) result[i] = (*this)[i] - e;
+ return result;
+ }
+ template <class EE> Vec<M, typename CNT<EE>::template Result<E>::Sub>
+ scalarSubtractFromLeft(const EE& e) const {
+ Vec<M, typename CNT<EE>::template Result<E>::Sub> result;
+ for (int i=0; i<M; ++i) result[i] = e - (*this)[i];
+ return result;
+ }
+
+ // Generic assignments for any element type not listed explicitly, including scalars.
+ // These are done repeatedly for each element and only work if the operation can
+ // be performed leaving the original element type.
+ template <class EE> Vec& operator =(const EE& e) {return scalarEq(e);}
+ template <class EE> Vec& operator+=(const EE& e) {return scalarPlusEq(e);}
+ template <class EE> Vec& operator-=(const EE& e) {return scalarMinusEq(e);}
+ template <class EE> Vec& operator*=(const EE& e) {return scalarTimesEq(e);}
+ template <class EE> Vec& operator/=(const EE& e) {return scalarDivideEq(e);}
+
+ // Generalized element assignment & computed assignment methods. These will work
+ // for any assignment-compatible element, not just scalars.
+ template <class EE> Vec& scalarEq(const EE& ee)
+ { for(int i=0;i<M;++i) d[i*STRIDE] = ee; return *this; }
+ template <class EE> Vec& scalarPlusEq(const EE& ee)
+ { for(int i=0;i<M;++i) d[i*STRIDE] += ee; return *this; }
+ template <class EE> Vec& scalarMinusEq(const EE& ee)
+ { for(int i=0;i<M;++i) d[i*STRIDE] -= ee; return *this; }
+ template <class EE> Vec& scalarMinusEqFromLeft(const EE& ee)
+ { for(int i=0;i<M;++i) d[i*STRIDE] = ee - d[i*STRIDE]; return *this; }
+ template <class EE> Vec& scalarTimesEq(const EE& ee)
+ { for(int i=0;i<M;++i) d[i*STRIDE] *= ee; return *this; }
+ template <class EE> Vec& scalarTimesEqFromLeft(const EE& ee)
+ { for(int i=0;i<M;++i) d[i*STRIDE] = ee * d[i*STRIDE]; return *this; }
+ template <class EE> Vec& scalarDivideEq(const EE& ee)
+ { for(int i=0;i<M;++i) d[i*STRIDE] /= ee; return *this; }
+ template <class EE> Vec& scalarDivideEqFromLeft(const EE& ee)
+ { for(int i=0;i<M;++i) d[i*STRIDE] = ee / d[i*STRIDE]; return *this; }
+
+ // Specialize for int to avoid warnings and ambiguities.
+ Vec& scalarEq(int ee) {return scalarEq(Precision(ee));}
+ Vec& scalarPlusEq(int ee) {return scalarPlusEq(Precision(ee));}
+ Vec& scalarMinusEq(int ee) {return scalarMinusEq(Precision(ee));}
+ Vec& scalarTimesEq(int ee) {return scalarTimesEq(Precision(ee));}
+ Vec& scalarDivideEq(int ee) {return scalarDivideEq(Precision(ee));}
+ Vec& scalarMinusEqFromLeft(int ee) {return scalarMinusEqFromLeft(Precision(ee));}
+ Vec& scalarTimesEqFromLeft(int ee) {return scalarTimesEqFromLeft(Precision(ee));}
+ Vec& scalarDivideEqFromLeft(int ee) {return scalarDivideEqFromLeft(Precision(ee));}
+
+ /** Set every scalar in this %Vec to NaN; this is the default initial
+ value in Debug builds, but not in Release. **/
+ void setToNaN() {
+ (*this) = CNT<ELT>::getNaN();
+ }
+
+ /** Set every scalar in this %Vec to zero. **/
+ void setToZero() {
+ (*this) = ELT(0);
+ }
+
+ /** Extract a const reference to a sub-Vec with size known at compile time.
+ This must be called with an explicit template argument for the size, for
+ example, getSubVec<3>(i). This is only a recast; no copying or computation
+ is performed. The size and index are range checked in Debug builds but
+ not in Release builds. **/
+ template <int MM>
+ const Vec<MM,ELT,STRIDE>& getSubVec(int i) const {
+ assert(0 <= i && i + MM <= M);
+ return Vec<MM,ELT,STRIDE>::getAs(&(*this)[i]);
+ }
+ /** Extract a writable reference to a sub-Vec with size known at compile time.
+ This must be called with an explicit template argument for the size, for
+ example, updSubVec<3>(i). This is only a recast; no copying or computation
+ is performed. The size and index are range checked in Debug builds but
+ not in Release builds. **/
+ template <int MM>
+ Vec<MM,ELT,STRIDE>& updSubVec(int i) {
+ assert(0 <= i && i + MM <= M);
+ return Vec<MM,ELT,STRIDE>::updAs(&(*this)[i]);
+ }
+
+
+ /** Extract a subvector of type %Vec from a longer one that has the same
+ element type and stride, and return a const reference to the selected
+ subsequence. **/
+ template <int MM>
+ static const Vec& getSubVec(const Vec<MM,ELT,STRIDE>& v, int i) {
+ assert(0 <= i && i + M <= MM);
+ return getAs(&v[i]);
+ }
+ /** Extract a subvector of type %Vec from a longer one that has the same
+ element type and stride, and return a writable reference to the selected
+ subsequence. **/
+ template <int MM>
+ static Vec& updSubVec(Vec<MM,ELT,STRIDE>& v, int i) {
+ assert(0 <= i && i + M <= MM);
+ return updAs(&v[i]);
+ }
+
+ /** Return a vector one smaller than this one by dropping the element
+ at the indicated position p. The result is a packed copy with the same
+ element type as this one. **/
+ Vec<M-1,ELT,1> drop1(int p) const {
+ assert(0 <= p && p < M);
+ Vec<M-1,ELT,1> out;
+ int nxt=0;
+ for (int i=0; i<M-1; ++i, ++nxt) {
+ if (nxt==p) ++nxt; // skip the loser
+ out[i] = (*this)[nxt];
+ }
+ return out;
+ }
+
+ /** Return a vector one larger than this one by adding an element
+ to the end. The result is a packed copy with the same element type as
+ this one. Works for any assignment compatible element. **/
+ template <class EE> Vec<M+1,ELT,1> append1(const EE& v) const {
+ Vec<M+1,ELT,1> out;
+ Vec<M,ELT,1>::updAs(&out[0]) = (*this);
+ out[M] = v;
+ return out;
+ }
+
+
+ /** Return a vector one larger than this one by inserting an element
+ \e before the indicated one. The result is a packed copy with the same
+ element type as this one. Works for any assignment compatible element. The
+ index can be one greater than normally allowed in which case the element
+ is appended (but use append1() if you know you're appending). **/
+ template <class EE> Vec<M+1,ELT,1> insert1(int p, const EE& v) const {
+ assert(0 <= p && p <= M);
+ if (p==M) return append1(v);
+ Vec<M+1,ELT,1> out;
+ int nxt=0;
+ for (int i=0; i<M; ++i, ++nxt) {
+ if (i==p) out[nxt++] = v;
+ out[nxt] = (*this)[i];
+ }
+ return out;
+ }
+
+ /** Recast an ordinary C++ array E[] to a const %Vec<M,E,S>; assumes
+ compatible length, stride, and packing. **/
+ static const Vec& getAs(const ELT* p)
+ { return *reinterpret_cast<const Vec*>(p); }
+ /** Recast a writable ordinary C++ array E[] to a writable %Vec<M,E,S>;
+ assumes compatible length, stride, and packing. **/
+ static Vec& updAs(ELT* p)
+ { return *reinterpret_cast<Vec*>(p); }
+
+
+ /** Return a %Vec of the same length and element type as this one but
+ with all elements set to NaN. The result is packed (stride==1) regardless
+ of the stride of this %Vec. **/
+ static Vec<M,ELT,1> getNaN() { return Vec<M,ELT,1>(CNT<ELT>::getNaN()); }
+
+ /** Return true if any element of this Vec contains a NaN anywhere. **/
+ bool isNaN() const {
+ for (int i=0; i<M; ++i)
+ if (CNT<ELT>::isNaN((*this)[i]))
+ return true;
+ return false;
+ }
+
+ /** Return true if any element of this Vec contains a +Infinity
+ or -Infinity somewhere but no element contains a NaN anywhere. **/
+ bool isInf() const {
+ bool seenInf = false;
+ for (int i=0; i<M; ++i) {
+ const ELT& e = (*this)[i];
+ if (!CNT<ELT>::isFinite(e)) {
+ if (!CNT<ELT>::isInf(e))
+ return false; // something bad was found
+ seenInf = true;
+ }
+ }
+ return seenInf;
+ }
+
+ /** Return true if no element of this %Vec contains an Infinity or a NaN
+ anywhere. **/
+ bool isFinite() const {
+ for (int i=0; i<M; ++i)
+ if (!CNT<ELT>::isFinite((*this)[i]))
+ return false;
+ return true;
+ }
+
+ /** For approximate comparisions, the default tolerance to use for a vector is
+ the same as its elements' default tolerance. **/
+ static double getDefaultTolerance() {return CNT<ELT>::getDefaultTolerance();}
+
+ /** %Test whether this vector is numerically equal to some other vector with
+ the same shape, using a specified tolerance. **/
+ template <class E2, int RS2>
+ bool isNumericallyEqual(const Vec<M,E2,RS2>& v, double tol) const {
+ for (int i=0; i<M; ++i)
+ if (!CNT<ELT>::isNumericallyEqual((*this)[i], v[i], tol))
+ return false;
+ return true;
+ }
+
+ /** %Test whether this vector is numerically equal to some other vector with
+ the same shape, using a default tolerance which is the looser of the
+ default tolerances of the two objects being compared. **/
+ template <class E2, int RS2>
+ bool isNumericallyEqual(const Vec<M,E2,RS2>& v) const {
+ const double tol = std::max(getDefaultTolerance(),v.getDefaultTolerance());
+ return isNumericallyEqual(v, tol);
+ }
+
+ /** %Test whether every element of this vector is numerically equal to the given
+ element, using either a specified tolerance or the vector's
+ default tolerance (which is always the same or looser than the default
+ tolerance for one of its elements). **/
+ bool isNumericallyEqual
+ (const ELT& e,
+ double tol = getDefaultTolerance()) const
+ {
+ for (int i=0; i<M; ++i)
+ if (!CNT<ELT>::isNumericallyEqual((*this)[i], e, tol))
+ return false;
+ return true;
+ }
+
+ // Functions to be used for Scripting in MATLAB and languages that do not support operator overloading
+ /** Print Vec into a string and return it. Please refer to operator<< for details. **/
+ std::string toString() const {
+ std::stringstream stream;
+ stream << (*this);
+ return stream.str();
+ }
+
+ /** Variant of operator[] that's scripting friendly to set ith entry **/
+ void set(int i, const E& value)
+ { (*this)[i] = value; }
+
+ /** Variant of operator[] that's scripting friendly to get const reference to ith entry **/
+ const E& get(int i) const
+ { return operator[](i); }
+
+private:
+ // TODO: should be an array of scalars rather than elements to control
+ // packing more carefully.
+ ELT d[NActualElements]; // data
+};
+
+/////////////////////////////////////////////
+// Global operators involving two vectors. //
+// v+v, v-v, v==v, v!=v //
+/////////////////////////////////////////////
+
+// v3 = v1 + v2 where all v's have the same length M.
+template <int M, class E1, int S1, class E2, int S2> inline
+typename Vec<M,E1,S1>::template Result< Vec<M,E2,S2> >::Add
+operator+(const Vec<M,E1,S1>& l, const Vec<M,E2,S2>& r) {
+ return Vec<M,E1,S1>::template Result< Vec<M,E2,S2> >
+ ::AddOp::perform(l,r);
+}
+
+// v3 = v1 - v2, similar to +
+template <int M, class E1, int S1, class E2, int S2> inline
+typename Vec<M,E1,S1>::template Result< Vec<M,E2,S2> >::Sub
+operator-(const Vec<M,E1,S1>& l, const Vec<M,E2,S2>& r) {
+ return Vec<M,E1,S1>::template Result< Vec<M,E2,S2> >
+ ::SubOp::perform(l,r);
+}
+
+/// bool = v1[i] == v2[i], for all elements i
+template <int M, class E1, int S1, class E2, int S2> inline bool
+operator==(const Vec<M,E1,S1>& l, const Vec<M,E2,S2>& r)
+{ for (int i=0; i < M; ++i) if (l[i] != r[i]) return false;
+ return true; }
+/// bool = v1[i] != v2[i], for any element i
+template <int M, class E1, int S1, class E2, int S2> inline bool
+operator!=(const Vec<M,E1,S1>& l, const Vec<M,E2,S2>& r) {return !(l==r);}
+
+/// bool = v[i] == e, for all elements v[i] and element e
+template <int M, class E1, int S1, class E2> inline bool
+operator==(const Vec<M,E1,S1>& v, const E2& e)
+{ for (int i=0; i < M; ++i) if (v[i] != e) return false;
+ return true; }
+/// bool = v[i] != e, for any element v[i] and element e
+template <int M, class E1, int S1, class E2> inline bool
+operator!=(const Vec<M,E1,S1>& v, const E2& e) {return !(v==e);}
+
+/// bool = v1[i] < v2[i], for all elements i
+template <int M, class E1, int S1, class E2, int S2> inline bool
+operator<(const Vec<M,E1,S1>& l, const Vec<M,E2,S2>& r)
+{ for (int i=0; i < M; ++i) if (l[i] >= r[i]) return false;
+ return true; }
+/// bool = v[i] < e, for all elements v[i] and element e
+template <int M, class E1, int S1, class E2> inline bool
+operator<(const Vec<M,E1,S1>& v, const E2& e)
+{ for (int i=0; i < M; ++i) if (v[i] >= e) return false;
+ return true; }
+
+/// bool = v1[i] > v2[i], for all elements i
+template <int M, class E1, int S1, class E2, int S2> inline bool
+operator>(const Vec<M,E1,S1>& l, const Vec<M,E2,S2>& r)
+{ for (int i=0; i < M; ++i) if (l[i] <= r[i]) return false;
+ return true; }
+/// bool = v[i] > e, for all elements v[i] and element e
+template <int M, class E1, int S1, class E2> inline bool
+operator>(const Vec<M,E1,S1>& v, const E2& e)
+{ for (int i=0; i < M; ++i) if (v[i] <= e) return false;
+ return true; }
+
+/// bool = v1[i] <= v2[i], for all elements i.
+/// This is not the same as !(v1>v2).
+template <int M, class E1, int S1, class E2, int S2> inline bool
+operator<=(const Vec<M,E1,S1>& l, const Vec<M,E2,S2>& r)
+{ for (int i=0; i < M; ++i) if (l[i] > r[i]) return false;
+ return true; }
+/// bool = v[i] <= e, for all elements v[i] and element e.
+/// This is not the same as !(v1>e).
+template <int M, class E1, int S1, class E2> inline bool
+operator<=(const Vec<M,E1,S1>& v, const E2& e)
+{ for (int i=0; i < M; ++i) if (v[i] > e) return false;
+ return true; }
+
+/// bool = v1[i] >= v2[i], for all elements i
+/// This is not the same as !(v1<v2).
+template <int M, class E1, int S1, class E2, int S2> inline bool
+operator>=(const Vec<M,E1,S1>& l, const Vec<M,E2,S2>& r)
+{ for (int i=0; i < M; ++i) if (l[i] < r[i]) return false;
+ return true; }
+/// bool = v[i] >= e, for all elements v[i] and element e.
+/// This is not the same as !(v1<e).
+template <int M, class E1, int S1, class E2> inline bool
+operator>=(const Vec<M,E1,S1>& v, const E2& e)
+{ for (int i=0; i < M; ++i) if (v[i] < e) return false;
+ return true; }
+
+///////////////////////////////////////////////////////
+// Global operators involving a vector and a scalar. //
+///////////////////////////////////////////////////////
+
+// I haven't been able to figure out a nice way to templatize for the
+// built-in reals without introducing a lot of unwanted type matches
+// as well. So we'll just grind them out explicitly here.
+
+// SCALAR MULTIPLY
+
+// v = v*real, real*v
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<float>::Mul
+operator*(const Vec<M,E,S>& l, const float& r)
+ { return Vec<M,E,S>::template Result<float>::MulOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<float>::Mul
+operator*(const float& l, const Vec<M,E,S>& r) {return r*l;}
+
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<double>::Mul
+operator*(const Vec<M,E,S>& l, const double& r)
+ { return Vec<M,E,S>::template Result<double>::MulOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<double>::Mul
+operator*(const double& l, const Vec<M,E,S>& r) {return r*l;}
+
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<long double>::Mul
+operator*(const Vec<M,E,S>& l, const long double& r)
+ { return Vec<M,E,S>::template Result<long double>::MulOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<long double>::Mul
+operator*(const long double& l, const Vec<M,E,S>& r) {return r*l;}
+
+// v = v*int, int*v -- just convert int to v's precision float
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<typename CNT<E>::Precision>::Mul
+operator*(const Vec<M,E,S>& l, int r) {return l * (typename CNT<E>::Precision)r;}
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<typename CNT<E>::Precision>::Mul
+operator*(int l, const Vec<M,E,S>& r) {return r * (typename CNT<E>::Precision)l;}
+
+// Complex, conjugate, and negator are all easy to templatize.
+
+// v = v*complex, complex*v
+template <int M, class E, int S, class R> inline
+typename Vec<M,E,S>::template Result<std::complex<R> >::Mul
+operator*(const Vec<M,E,S>& l, const std::complex<R>& r)
+ { return Vec<M,E,S>::template Result<std::complex<R> >::MulOp::perform(l,r); }
+template <int M, class E, int S, class R> inline
+typename Vec<M,E,S>::template Result<std::complex<R> >::Mul
+operator*(const std::complex<R>& l, const Vec<M,E,S>& r) {return r*l;}
+
+// v = v*conjugate, conjugate*v (convert conjugate->complex)
+template <int M, class E, int S, class R> inline
+typename Vec<M,E,S>::template Result<std::complex<R> >::Mul
+operator*(const Vec<M,E,S>& l, const conjugate<R>& r) {return l*(std::complex<R>)r;}
+template <int M, class E, int S, class R> inline
+typename Vec<M,E,S>::template Result<std::complex<R> >::Mul
+operator*(const conjugate<R>& l, const Vec<M,E,S>& r) {return r*(std::complex<R>)l;}
+
+// v = v*negator, negator*v: convert negator to standard number
+template <int M, class E, int S, class R> inline
+typename Vec<M,E,S>::template Result<typename negator<R>::StdNumber>::Mul
+operator*(const Vec<M,E,S>& l, const negator<R>& r) {return l * (typename negator<R>::StdNumber)(R)r;}
+template <int M, class E, int S, class R> inline
+typename Vec<M,E,S>::template Result<typename negator<R>::StdNumber>::Mul
+operator*(const negator<R>& l, const Vec<M,E,S>& r) {return r * (typename negator<R>::StdNumber)(R)l;}
+
+
+// SCALAR DIVIDE. This is a scalar operation when the scalar is on the right,
+// but when it is on the left it means scalar * pseudoInverse(vec), which is
+// a row.
+
+// v = v/real, real/v
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<float>::Dvd
+operator/(const Vec<M,E,S>& l, const float& r)
+ { return Vec<M,E,S>::template Result<float>::DvdOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename CNT<float>::template Result<Vec<M,E,S> >::Dvd
+operator/(const float& l, const Vec<M,E,S>& r)
+ { return CNT<float>::template Result<Vec<M,E,S> >::DvdOp::perform(l,r); }
+
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<double>::Dvd
+operator/(const Vec<M,E,S>& l, const double& r)
+ { return Vec<M,E,S>::template Result<double>::DvdOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename CNT<double>::template Result<Vec<M,E,S> >::Dvd
+operator/(const double& l, const Vec<M,E,S>& r)
+ { return CNT<double>::template Result<Vec<M,E,S> >::DvdOp::perform(l,r); }
+
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<long double>::Dvd
+operator/(const Vec<M,E,S>& l, const long double& r)
+ { return Vec<M,E,S>::template Result<long double>::DvdOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename CNT<long double>::template Result<Vec<M,E,S> >::Dvd
+operator/(const long double& l, const Vec<M,E,S>& r)
+ { return CNT<long double>::template Result<Vec<M,E,S> >::DvdOp::perform(l,r); }
+
+// v = v/int, int/v -- just convert int to v's precision float
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<typename CNT<E>::Precision>::Dvd
+operator/(const Vec<M,E,S>& l, int r) {return l / (typename CNT<E>::Precision)r;}
+template <int M, class E, int S> inline
+typename CNT<typename CNT<E>::Precision>::template Result<Vec<M,E,S> >::Dvd
+operator/(int l, const Vec<M,E,S>& r) {return (typename CNT<E>::Precision)l / r;}
+
+
+// Complex, conjugate, and negator are all easy to templatize.
+
+// v = v/complex, complex/v
+template <int M, class E, int S, class R> inline
+typename Vec<M,E,S>::template Result<std::complex<R> >::Dvd
+operator/(const Vec<M,E,S>& l, const std::complex<R>& r)
+ { return Vec<M,E,S>::template Result<std::complex<R> >::DvdOp::perform(l,r); }
+template <int M, class E, int S, class R> inline
+typename CNT<std::complex<R> >::template Result<Vec<M,E,S> >::Dvd
+operator/(const std::complex<R>& l, const Vec<M,E,S>& r)
+ { return CNT<std::complex<R> >::template Result<Vec<M,E,S> >::DvdOp::perform(l,r); }
+
+// v = v/conjugate, conjugate/v (convert conjugate->complex)
+template <int M, class E, int S, class R> inline
+typename Vec<M,E,S>::template Result<std::complex<R> >::Dvd
+operator/(const Vec<M,E,S>& l, const conjugate<R>& r) {return l/(std::complex<R>)r;}
+template <int M, class E, int S, class R> inline
+typename CNT<std::complex<R> >::template Result<Vec<M,E,S> >::Dvd
+operator/(const conjugate<R>& l, const Vec<M,E,S>& r) {return (std::complex<R>)l/r;}
+
+// v = v/negator, negator/v: convert negator to number
+template <int M, class E, int S, class R> inline
+typename Vec<M,E,S>::template Result<typename negator<R>::StdNumber>::Dvd
+operator/(const Vec<M,E,S>& l, const negator<R>& r) {return l/(typename negator<R>::StdNumber)(R)r;}
+template <int M, class E, int S, class R> inline
+typename CNT<R>::template Result<Vec<M,E,S> >::Dvd
+operator/(const negator<R>& l, const Vec<M,E,S>& r) {return (typename negator<R>::StdNumber)(R)l/r;}
+
+
+// Add and subtract are odd as scalar ops. They behave as though the
+// scalar stands for a vector each of whose elements is that scalar,
+// and then a normal vector add or subtract is done.
+
+// SCALAR ADD
+
+// v = v+real, real+v
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<float>::Add
+operator+(const Vec<M,E,S>& l, const float& r)
+ { return Vec<M,E,S>::template Result<float>::AddOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<float>::Add
+operator+(const float& l, const Vec<M,E,S>& r) {return r+l;}
+
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<double>::Add
+operator+(const Vec<M,E,S>& l, const double& r)
+ { return Vec<M,E,S>::template Result<double>::AddOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<double>::Add
+operator+(const double& l, const Vec<M,E,S>& r) {return r+l;}
+
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<long double>::Add
+operator+(const Vec<M,E,S>& l, const long double& r)
+ { return Vec<M,E,S>::template Result<long double>::AddOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<long double>::Add
+operator+(const long double& l, const Vec<M,E,S>& r) {return r+l;}
+
+// v = v+int, int+v -- just convert int to v's precision float
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<typename CNT<E>::Precision>::Add
+operator+(const Vec<M,E,S>& l, int r) {return l + (typename CNT<E>::Precision)r;}
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<typename CNT<E>::Precision>::Add
+operator+(int l, const Vec<M,E,S>& r) {return r + (typename CNT<E>::Precision)l;}
+
+// Complex, conjugate, and negator are all easy to templatize.
+
+// v = v+complex, complex+v
+template <int M, class E, int S, class R> inline
+typename Vec<M,E,S>::template Result<std::complex<R> >::Add
+operator+(const Vec<M,E,S>& l, const std::complex<R>& r)
+ { return Vec<M,E,S>::template Result<std::complex<R> >::AddOp::perform(l,r); }
+template <int M, class E, int S, class R> inline
+typename Vec<M,E,S>::template Result<std::complex<R> >::Add
+operator+(const std::complex<R>& l, const Vec<M,E,S>& r) {return r+l;}
+
+// v = v+conjugate, conjugate+v (convert conjugate->complex)
+template <int M, class E, int S, class R> inline
+typename Vec<M,E,S>::template Result<std::complex<R> >::Add
+operator+(const Vec<M,E,S>& l, const conjugate<R>& r) {return l+(std::complex<R>)r;}
+template <int M, class E, int S, class R> inline
+typename Vec<M,E,S>::template Result<std::complex<R> >::Add
+operator+(const conjugate<R>& l, const Vec<M,E,S>& r) {return r+(std::complex<R>)l;}
+
+// v = v+negator, negator+v: convert negator to standard number
+template <int M, class E, int S, class R> inline
+typename Vec<M,E,S>::template Result<typename negator<R>::StdNumber>::Add
+operator+(const Vec<M,E,S>& l, const negator<R>& r) {return l + (typename negator<R>::StdNumber)(R)r;}
+template <int M, class E, int S, class R> inline
+typename Vec<M,E,S>::template Result<typename negator<R>::StdNumber>::Add
+operator+(const negator<R>& l, const Vec<M,E,S>& r) {return r + (typename negator<R>::StdNumber)(R)l;}
+
+// SCALAR SUBTRACT -- careful, not commutative.
+
+// v = v-real, real-v
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<float>::Sub
+operator-(const Vec<M,E,S>& l, const float& r)
+ { return Vec<M,E,S>::template Result<float>::SubOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename CNT<float>::template Result<Vec<M,E,S> >::Sub
+operator-(const float& l, const Vec<M,E,S>& r)
+ { return CNT<float>::template Result<Vec<M,E,S> >::SubOp::perform(l,r); }
+
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<double>::Sub
+operator-(const Vec<M,E,S>& l, const double& r)
+ { return Vec<M,E,S>::template Result<double>::SubOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename CNT<double>::template Result<Vec<M,E,S> >::Sub
+operator-(const double& l, const Vec<M,E,S>& r)
+ { return CNT<double>::template Result<Vec<M,E,S> >::SubOp::perform(l,r); }
+
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<long double>::Sub
+operator-(const Vec<M,E,S>& l, const long double& r)
+ { return Vec<M,E,S>::template Result<long double>::SubOp::perform(l,r); }
+template <int M, class E, int S> inline
+typename CNT<long double>::template Result<Vec<M,E,S> >::Sub
+operator-(const long double& l, const Vec<M,E,S>& r)
+ { return CNT<long double>::template Result<Vec<M,E,S> >::SubOp::perform(l,r); }
+
+// v = v-int, int-v // just convert int to v's precision float
+template <int M, class E, int S> inline
+typename Vec<M,E,S>::template Result<typename CNT<E>::Precision>::Sub
+operator-(const Vec<M,E,S>& l, int r) {return l - (typename CNT<E>::Precision)r;}
+template <int M, class E, int S> inline
+typename CNT<typename CNT<E>::Precision>::template Result<Vec<M,E,S> >::Sub
+operator-(int l, const Vec<M,E,S>& r) {return (typename CNT<E>::Precision)l - r;}
+
+
+// Complex, conjugate, and negator are all easy to templatize.
+
+// v = v-complex, complex-v
+template <int M, class E, int S, class R> inline
+typename Vec<M,E,S>::template Result<std::complex<R> >::Sub
+operator-(const Vec<M,E,S>& l, const std::complex<R>& r)
+ { return Vec<M,E,S>::template Result<std::complex<R> >::SubOp::perform(l,r); }
+template <int M, class E, int S, class R> inline
+typename CNT<std::complex<R> >::template Result<Vec<M,E,S> >::Sub
+operator-(const std::complex<R>& l, const Vec<M,E,S>& r)
+ { return CNT<std::complex<R> >::template Result<Vec<M,E,S> >::SubOp::perform(l,r); }
+
+// v = v-conjugate, conjugate-v (convert conjugate->complex)
+template <int M, class E, int S, class R> inline
+typename Vec<M,E,S>::template Result<std::complex<R> >::Sub
+operator-(const Vec<M,E,S>& l, const conjugate<R>& r) {return l-(std::complex<R>)r;}
+template <int M, class E, int S, class R> inline
+typename CNT<std::complex<R> >::template Result<Vec<M,E,S> >::Sub
+operator-(const conjugate<R>& l, const Vec<M,E,S>& r) {return (std::complex<R>)l-r;}
+
+// v = v-negator, negator-v: convert negator to standard number
+template <int M, class E, int S, class R> inline
+typename Vec<M,E,S>::template Result<typename negator<R>::StdNumber>::Sub
+operator-(const Vec<M,E,S>& l, const negator<R>& r) {return l-(typename negator<R>::StdNumber)(R)r;}
+template <int M, class E, int S, class R> inline
+typename CNT<R>::template Result<Vec<M,E,S> >::Sub
+operator-(const negator<R>& l, const Vec<M,E,S>& r) {return (typename negator<R>::StdNumber)(R)l-r;}
+
+// Vec I/O
+template <int M, class E, int S, class CHAR, class TRAITS> inline
+std::basic_ostream<CHAR,TRAITS>&
+operator<<(std::basic_ostream<CHAR,TRAITS>& o, const Vec<M,E,S>& v) {
+ o << "~[" << v[0]; for(int i=1;i<M;++i) o<<','<<v[i]; o<<']'; return o;
+}
+
+/** Read a Vec from a stream as M elements separated by white space or
+by commas, optionally enclosed in () [] ~() or ~[]. **/
+template <int M, class E, int S, class CHAR, class TRAITS> inline
+std::basic_istream<CHAR,TRAITS>&
+operator>>(std::basic_istream<CHAR,TRAITS>& is, Vec<M,E,S>& v) {
+ CHAR tilde;
+ is >> tilde; if (is.fail()) return is;
+ if (tilde != CHAR('~')) {
+ tilde = CHAR(0);
+ is.unget(); if (is.fail()) return is;
+ }
+
+ CHAR openBracket, closeBracket;
+ is >> openBracket; if (is.fail()) return is;
+ if (openBracket==CHAR('('))
+ closeBracket = CHAR(')');
+ else if (openBracket==CHAR('['))
+ closeBracket = CHAR(']');
+ else {
+ closeBracket = CHAR(0);
+ is.unget(); if (is.fail()) return is;
+ }
+
+ // If we saw a "~" but then we didn't see any brackets, that's an
+ // error. Set the fail bit and return.
+ if (tilde != CHAR(0) && closeBracket == CHAR(0)) {
+ is.setstate( std::ios::failbit );
+ return is;
+ }
+
+ for (int i=0; i < M; ++i) {
+ is >> v[i];
+ if (is.fail()) return is;
+ if (i != M-1) {
+ CHAR c; is >> c; if (is.fail()) return is;
+ if (c != ',') is.unget();
+ if (is.fail()) return is;
+ }
+ }
+
+ // Get the closing bracket if there was an opening one. If we don't
+ // see the expected character we'll set the fail bit in the istream.
+ if (closeBracket != CHAR(0)) {
+ CHAR closer; is >> closer; if (is.fail()) return is;
+ if (closer != closeBracket) {
+ is.unget(); if (is.fail()) return is;
+ is.setstate( std::ios::failbit );
+ }
+ }
+
+ return is;
+}
+
+} //namespace SimTK
+
+
+#endif //SimTK_SIMMATRIX_SMALLMATRIX_VEC_H_
diff --git a/SimTKcommon/SmallMatrix/src/SmallMatrix.cpp b/SimTKcommon/SmallMatrix/src/SmallMatrix.cpp
new file mode 100644
index 0000000..e6f7e00
--- /dev/null
+++ b/SimTKcommon/SmallMatrix/src/SmallMatrix.cpp
@@ -0,0 +1,70 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Scalar.h"
+#include "SimTKcommon/SmallMatrix.h"
+
+namespace SimTK {
+
+template class Vec<3,Real>;
+template class Vec<2,Real>;
+template class Vec<3,Complex>;
+template class Vec<2,Complex>;
+template class Vec< 3,negator<Real> >;
+template class Vec< 3,conjugate<float>,2 >;
+
+template class Mat<3,3,Real>;
+template class Mat<2,2,Real>;
+template class Mat<3,3,Complex>;
+template class Mat<2,2,Complex>;
+template class Mat<5,5,negator< std::complex<double> > >;
+template class Mat< 3,3,negator<Real> >;
+template class Mat< 3,3,conjugate<float>,2 >;
+
+template class SymMat< 4, std::complex<double>, 7>;
+
+
+template Real det(const Mat<1,1,Real>&);
+template Real det(const SymMat<1,Real>&);
+template Complex det(const Mat<2,2,Complex>&);
+template Real det(const SymMat<2,Real>&);
+template Real det(const Mat<3,3,Real>&);
+template Real det(const SymMat<3,Real>&);
+template Real det(const Mat<5,5,Real>&);
+template Complex det(const SymMat<5,Complex>&);
+
+template Mat<1,1,Real>::TInvert lapackInverse(const Mat<1,1,Real>&);
+template Mat<2,2,Real>::TInvert lapackInverse(const Mat<2,2,Real>&);
+template Mat<3,3,Conjugate>::TInvert lapackInverse(const Mat<3,3,Conjugate>&);
+
+template Mat<1,1,Real>::TInvert inverse(const Mat<1,1,Real>&);
+template SymMat<1,Real>::TInvert inverse(const SymMat<1,Real>&);
+template Mat<2,2,Complex>::TInvert inverse(const Mat<2,2,Complex>&);
+template SymMat<2,Real>::TInvert inverse(const SymMat<2,Real>&);
+template Mat<3,3,Real>::TInvert inverse(const Mat<3,3,Real>&);
+template SymMat<3,Real>::TInvert inverse(const SymMat<3,Real>&);
+template Mat<5,5,Real>::TInvert inverse(const Mat<5,5,Real>&);
+//template SymMat<5,Complex>::TInvert SimTK::inverse(const SymMat<5,Complex>&);
+
+} //namespace SimTK
+
diff --git a/SimTKcommon/doc/Simmatrix.doc b/SimTKcommon/doc/Simmatrix.doc
new file mode 100644
index 0000000..64deb5b
Binary files /dev/null and b/SimTKcommon/doc/Simmatrix.doc differ
diff --git a/SimTKcommon/doc/Simmatrix.pdf b/SimTKcommon/doc/Simmatrix.pdf
new file mode 100644
index 0000000..a4ff1f4
Binary files /dev/null and b/SimTKcommon/doc/Simmatrix.pdf differ
diff --git a/SimTKcommon/doc/images/isaac.png b/SimTKcommon/doc/images/isaac.png
new file mode 100644
index 0000000..f43ecb6
Binary files /dev/null and b/SimTKcommon/doc/images/isaac.png differ
diff --git a/SimTKcommon/include/SimTKcommon.h b/SimTKcommon/include/SimTKcommon.h
new file mode 100644
index 0000000..bd835a3
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon.h
@@ -0,0 +1,66 @@
+#ifndef SimTK_SimTKCOMMON_H_
+#define SimTK_SimTKCOMMON_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Includes internal headers providing declarations for the basic SimTK
+ * Core classes, including Simmatrix.
+ */
+
+#include "SimTKcommon/basics.h"
+
+#if defined(__cplusplus)
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/State.h"
+#include "SimTKcommon/internal/Measure.h"
+#include "SimTKcommon/internal/MeasureImplementation.h"
+#include "SimTKcommon/internal/PolygonalMesh.h"
+#include "SimTKcommon/internal/DecorativeGeometry.h"
+#include "SimTKcommon/internal/DecorationGenerator.h"
+#include "SimTKcommon/internal/System.h"
+#include "SimTKcommon/internal/SystemGuts.h"
+#include "SimTKcommon/internal/Subsystem.h"
+#include "SimTKcommon/internal/SubsystemGuts.h"
+#include "SimTKcommon/internal/Study.h"
+#include "SimTKcommon/internal/Function.h"
+#include "SimTKcommon/internal/Random.h"
+#include "SimTKcommon/internal/PolynomialRootFinder.h"
+#include "SimTKcommon/internal/PrivateImplementation.h"
+#include "SimTKcommon/internal/EventHandler.h"
+#include "SimTKcommon/internal/EventReporter.h"
+#include "SimTKcommon/internal/ParallelExecutor.h"
+#include "SimTKcommon/internal/Parallel2DExecutor.h"
+#include "SimTKcommon/internal/ParallelWorkQueue.h"
+#include "SimTKcommon/internal/ThreadLocal.h"
+#include "SimTKcommon/internal/AtomicInteger.h"
+#include "SimTKcommon/internal/Pathname.h"
+#include "SimTKcommon/internal/Plugin.h"
+#include "SimTKcommon/internal/Timing.h"
+#include "SimTKcommon/internal/Xml.h"
+#include "SimTKcommon/Testing.h"
+#endif
+
+
+#endif /* SimTK_SimTKCOMMON_H_ */
diff --git a/SimTKcommon/include/SimTKcommon/Constants.h b/SimTKcommon/include/SimTKcommon/Constants.h
new file mode 100644
index 0000000..942dd7a
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/Constants.h
@@ -0,0 +1,581 @@
+#ifndef SimTK_SimTKCOMMON_CONSTANTS_H_
+#define SimTK_SimTKCOMMON_CONSTANTS_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+High precision mathematical and physical constants. This file is self-contained
+and can be included in ANSI C programs as well as C++.**/
+
+/** @defgroup PredefinedConstants Predefined Constants
+
+There are two kinds of numerical constants predefined by %SimTK: (1) a set
+of typed, const, in-memory values in the SimTK namespace, at the default Real
+precision or with other specific types, and (2) a set of preprocessor
+(\#define) macros containing extremely high-precision precalculated numerical
+values in long double precision.
+
+You should use the typed constants whenever possible in your code since they
+are very compact and well-behaved in C++. They have memory addresses so can
+be returned as references. Because they are filled in at startup, they can
+include machine- and precision-specific values like NaN, Infinity, machine
+roundoff error, number of digits in a float, etc. that are very useful for
+writing robust, precision-independent numerical algorithms.
+
+The macro values must be cast to the appropriate type before use, and are
+mostly useful as raw material for making \e typed constants. These macros
+contain <em>machine-independent</em> constants, including unitless mathematical
+constants like pi, as well as physical constants and unit conversion factors.
+These constants are provided at extremely high precision as compile-time
+macros in long double precision. By using very high precision we ensure
+sufficient accuracy for any IEEE long double precision implementation (they
+can be 64, 80, or 128 bits). These constants can be used as raw material for
+providing nicer templatized constants in appropriate precisions and unit
+systems.
+
+<h2>Naming conventions</h2>
+Note that the %SimTK convention for typed constants is to name them
+like ordinary variables except with an initial capital letter (like a
+class name). This is distinct from the widely-used convention for
+constants that are defined via the presprocessor as macros (that is,
+using \#define). Those are written entirely in
+\c UPPER_CASE_WITH_UNDERSCORES, after an initial \c SimTK_. Typed constants
+are processed instead by the compiler itself and do not require any
+special treatment when used; they behave just like variables of the
+same type and value would behave so there is no need to shout when
+using them. **/
+
+/** @defgroup MacroConstants Preprocessor Macro Constants
+ @ingroup PredefinedConstants
+
+These are preprocessor (\#define) macros providing constants values at very
+high precision.\ See the discussion under the main \ref PredefinedConstants
+module heading.
+
+<h2>Units</h2>
+Our most common unit systems are the "SI" (MKS) system, and the "MD" system
+used for molecular dynamics. SI units are meters, kg, seconds, coulombs
+(ampere-s), kelvins and moles. MD units are nanometers, atomic mass units
+(Daltons, g/mol), picoseconds, proton charge e, kelvins, and moles. Many
+molecular dynamicists and chemists prefer kcals for energy and angstroms for
+length. This does not constitute a consistent set of units, however, so
+we provide for it by conversion from the MD units, which are consistent.
+(By consistent, we mean that force units = mass-length/time^2, so f=ma!)
+
+<pre>
+Unit systems
+
+ SI (MKS) MD KCAL-ANGSTROM
+--------- -------------- ------------------------ ------------------
+length meter nanometer angstrom (A)
+mass kg amu, dalton amu, dalton
+time second picosecond picosecond
+charge coulomb e, proton charge e, proton charge
+temp. kelvin kelvin kelvin
+substance mole mole mole
+
+velocity m/s km/s (nm/ps) 100m/s (A/ps)
+
+energy J (kg-m^2/s^2) kJ/mol kcal/mol
+ (Da-nm^2/ps^2) (418.4 Da-A^2/ps^2)
+force N (kg-m/s^2) kJ/(mol-nm) = TN/mol kcal/(mol-A)
+ (Da-nm/ps^2) (T=10^12) (418.4 Da-A/ps^2)
+
+</pre>
+
+We always keep angles in radians internally, which are unitless. However,
+most humans prefer degrees where 1 degree = Pi/180 radians so we provide
+convenient conversions. **/
+
+
+ /**************************/
+ /* MATHEMATICAL CONSTANTS */
+ /**************************/
+
+/** @defgroup MathConstants Mathematical Constants
+ @ingroup MacroConstants
+
+These are some common unitless numerical constants evaluated to 64 digits and
+written here in maximal (long double) precision. (These values were generated
+using the symbolic calculator Maple which is part of Matlab's Symbolic
+Toolbox.) These can be cast to lower precisions when needed, and can be used
+in compile-time constant expressions like 2*SimTK_PI or 1/SimTK_SQRT2 for which
+the compiler will properly calculate a long double result with no runtime cost.
+
+These constants are also available as type-safe, already-rounded,
+precision-templatized values with static memory addresses as part of our scalar
+system (see NTraits<T>). You should use the templatized versions when possible.
+The templatized versions also contain more elaborate constants such as NaN,
+Infinity, and "epsilon" (machine precision) which can only be generated for
+specific types. **/
+
+/**@{**/
+
+/** The ratio pi of a circle's circumference to its diameter in Euclidean geometry.
+ * @par uncertainty
+ * approximation of an exact value
+ */
+#define SimTK_PI 3.141592653589793238462643383279502884197169399375105820974944592L
+
+/** e, or exp(1).
+ * @par uncertainty
+ * approximation of an exact value
+ */
+#define SimTK_E 2.718281828459045235360287471352662497757247093699959574966967628L
+
+/** The natural (base e) logarithm of 2.
+ * @par uncertainty
+ * approximation of an exact value
+ * @see SimTK_E
+ */
+#define SimTK_LN2 6.931471805599453094172321214581765680755001343602552541206800095e-1L
+
+/** The natural (base e) logarithm of 10.
+ * @par uncertainty
+ * approximation of an exact value
+ * @see SimTK_E
+ */
+#define SimTK_LN10 2.302585092994045684017991454684364207601101488628772976033327901L
+
+/** log2(e).
+ * @par uncertainty
+ * approximation of an exact value
+ */
+#define SimTK_LOG2E 1.442695040888963407359924681001892137426645954152985934135449407L
+
+/** log10(e).
+ * @par uncertainty
+ * approximation of an exact value
+ */
+#define SimTK_LOG10E 4.342944819032518276511289189166050822943970058036665661144537832e-1L
+
+/** The square root of 2.
+ * @par uncertainty
+ * approximation of an exact value
+ */
+#define SimTK_SQRT2 1.414213562373095048801688724209698078569671875376948073176679738L
+
+/** One over the square root of 2; also half the square root of 2 since
+ * 1/sqrt(2) == 2^(-1/2) == sqrt(2)/2.
+ * @par uncertainty
+ * approximation of an exact value
+ */
+#define SimTK_OOSQRT2 .7071067811865475244008443621048490392848359376884740365883398690L
+
+/** The cube root of 2, 2^(1/3).
+ * @par uncertainty
+ * approximation of an exact value
+ */
+#define SimTK_CBRT2 1.259921049894873164767210607278228350570251464701507980081975112L
+
+/** One over the cube root of 2, 2^(-1/3).
+ * @par uncertainty
+ * approximation of an exact value
+ */
+#define SimTK_OOCBRT2 .7937005259840997373758528196361541301957466639499265049041428810L
+
+/** The sixth root of 2, 2^(1/6).
+ * @par uncertainty
+ * approximation of an exact value
+ */
+#define SimTK_SIXRT2 1.122462048309372981433533049679179516232411110613986753440409546L
+
+/** One over the sixth root of 2, 2^(-1/6).
+ * @par uncertainty
+ * approximation of an exact value
+ */
+#define SimTK_OOSIXRT2 .8908987181403393047402262055905125079872126158781604033837569922L
+
+/** The square root of 3.
+ * @par uncertainty
+ * approximation of an exact value
+ */
+#define SimTK_SQRT3 1.732050807568877293527446341505872366942805253810380628055806979L
+
+/** The cube root of 3.
+ * @par uncertainty
+ * approximation of an exact value
+ */
+#define SimTK_CBRT3 1.442249570307408382321638310780109588391869253499350577546416195L
+
+
+/**@}**/ /*end of Mathematical Constants*/
+
+ /**********************/
+ /* PHYSICAL CONSTANTS */
+ /**********************/
+
+/** @defgroup PhysConstants Physical Constants
+ @ingroup MacroConstants
+
+These constants are from the CODATA 2002 set from the NIST Physics Laboratory
+web site http://physics.nist.gov/constants (NIST SP 961 Dec 2005).
+Ref: P.J. Mohr and B.N. Taylor, Rev. Mod. Phys. 77(1) (2005).
+
+ at par Uncertainty
+Uncertainties are given in the CODATA set as the one-std-deviation uncertainty
+in the last 2 digits of the given value. That means that there is about a 68%
+chance that the last two digits are as shown +/- the uncertainty.
+
+How to combine uncertainties (extracted from
+http://physics.nist.gov/cuu/Uncertainty/combination.html):
+Assume measured quantities are x1, y1 with u1=uncertainty(x1),
+u2=uncertainty(x2). We want to combine them into a new quantity y and calculate
+u=uncertainty(y).
+<pre>
+ Addition rule: y = a1*x1 + a2*x2 for factors a1,a2.
+ then u^2 = a1*u1^2 + a2*u2^2
+ Multiplication rule y = a*x1^e1*x2^e2 for factor a and exponents e1,e2.
+ Let ur1=u1/|x1|, ur2=u2/|x2| be the relative uncertainties, ur is u/|y|.
+ then ur^2 = e1^2*ur1^2 + e2^2*ur2^2, u = ur*|y|
+</pre>
+**/
+
+/**@{**/
+
+/**
+ * Avogadro's number (NA) is defined as the number of atoms in 12g of pure Carbon-12 in
+ * its unbound, rest state. The number is 1 mole (mol).
+ * @par uncertainty
+ * 10e16
+ */
+#define SimTK_AVOGADROS_NUMBER 6.0221415e23L
+
+/**
+ * Mass of a proton in MD units.
+ *
+ * The atomic mass unit u (or amu) is defined as 1/12 of the mass of a Carbon-12 atom,
+ * unbound and in its rest state. This definition matched to Avogadro's number's definition
+ * ensures that 1 mole of particles of mass 1u each has total mass exactly 1g. This is
+ * synonymous with the dalton (Da), with units of g/mole, so 1u = 1Dalton = 1g/mole.
+ * We will use Da for this mass unit, with kDa being a common mass measure for
+ * large biomolecules.
+ *
+ * @par uncertainty
+ * 13e-11
+ * @see SimTK_AVOGADROS_NUMBER
+ */
+#define SimTK_MASS_OF_PROTON_IN_MD 1.00727646688L
+
+/**
+ * Mass of a neutron in MD units.
+ * @par uncertainty
+ * 55e-11
+ * @see SimTK_MASS_OF_PROTON_IN_MD
+ */
+#define SimTK_MASS_OF_NEUTRON_IN_MD 1.00866491560L
+
+/**
+ * Mass of an electron in MD units.
+ * @par uncertainty
+ * 24e-14
+ * @see SimTK_MASS_OF_PROTON_IN_MD
+ */
+#define SimTK_MASS_OF_ELECTRON_IN_MD 5.4857990945e-4L
+
+/**
+ * Atomic charge unit e expressed in MKS unit of Coulombs.
+ * The charge on an electron is just the negative of this value.
+ * @par uncertainty
+ * 14e-27
+ */
+#define SimTK_CHARGE_OF_PROTON_IN_SI 1.60217653e-19L
+
+/**
+ * Atomic charge unit e expressed in MD units, which uses e as its charge unit!
+ * The charge on an electron is just the negative of this value.
+ * @par uncertainty
+ * exact (duh!)
+ */
+#define SimTK_CHARGE_OF_PROTON_IN_MD 1.L
+
+/**
+ * The charge of 1 mole of protons, expressed in Coulombs.
+ * <pre>
+ * 1.60217653(14)e-19 C/e * 6.0221415(10)e23 = 9.6485338(18)e+4
+ * </pre>
+ * @par uncertainty
+ * 18e-3
+ */
+#define SimTK_MOLAR_CHARGE_IN_SI 9.6485338e+4L
+
+/**
+ * The charge of 1 mole of protons, expressed in MD units where the unit
+ * of charge is just the charge on one proton. So in MD units this is just
+ * Avogadro's number.
+ * @see SimTK_AVOGADROS_NUMBER
+ */
+#define SimTK_MOLAR_CHARGE_IN_MD SimTK_AVOGADROS_NUMBER
+
+/**
+ * Speed of light c is exact in MKS units of m/s.
+ * @par uncertainty
+ * exact
+ * @see SimTK_LIGHTSPEED_IN_MD
+ */
+#define SimTK_LIGHTSPEED_IN_SI 2.99792458e+8L
+
+/**
+ * Speed of light c is exact in MD units of nm/ps.
+ * @par uncertainty
+ * exact
+ * @see SimTK_LIGHTSPEED_IN_SI
+ */
+#define SimTK_LIGHTSPEED_IN_MD 2.99792458e+5L
+
+/**
+ * Newton's gravitational constant G in N-m^2/kg^2 = m^3 kg^-1 s^-2.
+ * The force between two point masses m1,m2 separated by a distance d is
+ * <pre> F = -G m1*m2/d^2 </pre>
+ * (with the "-" indicating an attractive force).
+ * @par uncertainty
+ * 10e-15
+ * @see SimTK_GRAVITATIONAL_CONSTANT_IN_MD
+ */
+#define SimTK_GRAVITATIONAL_CONSTANT_IN_SI 6.6742e-11L
+
+/**
+ * Newton's gravitational constant G in (kJ/mol)-nm^2/u^2 = nm^3 u^-1 ps^-2.
+ * <pre>
+ * Conversion is (nm/m)^3 (u/kg)^-1 (ps/s)^-2
+ * = 1.66053886(28)e-24L (uncertainty: 28e-32)
+ * </pre>
+ * This is why gravity doesn't matter in molecular systems. Don't try
+ * this in single precision -- you'll run out of exponent!
+ * @par uncertainty
+ * 17e-39
+ * @see SimTK_GRAVITATIONAL_CONSTANT_IN_SI
+ */
+#define SimTK_GRAVITATIONAL_CONSTANT_IN_MD 1.10827e-34L
+
+/**
+ * Free space magnetic permeability constant mu0 in SI units (exact).
+ * <pre>
+ * = 4*pi * 1e-7 exactly in N/A^2 (Newtons/Ampere^2) = kg-m/C^2
+ * </pre>
+ * @par uncertainty
+ * approximation of an exact quantity
+ * @see SimTK_ELECTRIC_PERMITTIVITY_IN_SI
+ */
+#define SimTK_MAGNETIC_PERMEABILITY_IN_SI \
+ 1.256637061435917295385057353311801153678867759750042328389977837e-6L
+
+/**
+ * Free space magnetic permeability constant mu0 in MD units (not exact).
+ * <pre>
+ * Convert kg->g/mole, m->nm, C->e = (4*pi*1e5)*1.60217653e-19^2*6.0221415e23
+ * (exact in SI units, but not exact here)
+ * </pre>
+ * @par uncertainty
+ * 47e-16
+ * @see SimTK_ELECTRIC_PERMITTIVITY_IN_MD
+ */
+#define SimTK_MAGNETIC_PERMEABILITY_IN_MD 1.94259179e-8L
+
+/**
+ * Free space permittivity constant e0 = 1/(mu0*c^2) Farad/m = Coulomb^2/(N-m^2) (exact in SI units).
+ * @par uncertainty
+ * approximation of an exact quantity
+ * @see SimTK_MAGNETIC_PERMEABILITY_IN_SI
+ */
+#define SimTK_ELECTRIC_PERMITTIVITY_IN_SI \
+ 8.854187817620389850536563031710750260608370166599449808102417149e-12L /* approx of exact */
+
+/**
+ * Free space permittivity constant e0=1/(mu0*c^2) e^2/(kN-nm^2) using MD permeability and
+ * MD lightspeed.
+ * @par uncertainty
+ * 14e-11
+ * @see SimTK_MAGNETIC_PERMEABILITY_IN_MD
+ */
+#define SimTK_ELECTRIC_PERMITTIVITY_IN_MD 5.7276575e-4L
+
+/**
+ * Coulomb's constant kappa = 1/(4pi*e0)=1e-7*c^2 N-m^2/Coulomb^2 (exact in SI units).
+ * This is the constant that appears in Coulomb's law f(r)= kappa*q1*q2/r^2.
+ * @par uncertainty
+ * exact
+ */
+#define SimTK_COULOMB_CONSTANT_IN_SI 8.9875517873681764e+9L
+
+/**
+ * Coulomb's constant kappa = 1/(4*pi*e0) in MD units.
+ * This is the constant that appears in Coulomb's law f(r)= kappa*q1*q2/r^2.
+ * <pre>
+ * Coulomb's consant in MD units uses MD e0 & c:
+ * 1/(4*pi*e0)=1e5*1.60217653e-19^2*6.0221415e23*c^2 kN-nm^2/e^2 (=kJ-nm/e^2)
+ * (exact in SI units but not exact in MD)
+ * </pre>
+ * @par uncertainty
+ * 33e-6
+ */
+#define SimTK_COULOMB_CONSTANT_IN_MD 1.38935456e+2L
+
+/**
+ * Coulomb's constant kappa = 1/(4*pi*e0) in kcal-Angstroms/e^2.
+ * This is the constant that appears in Coulomb's law f(r)= kappa*q1*q2/r^2.
+ * This is an exact conversion from MD units (which are inexact).
+ * @par uncertainty
+ * 80e-6
+ */
+#define SimTK_COULOMB_CONSTANT_IN_KCAL_ANGSTROM 3.32063711e+2L
+
+/**
+ * This is the gas constant R in (J/mol)/K.
+ * @par uncertainty
+ * 15e-6
+ */
+#define SimTK_MOLAR_GAS_CONSTANT_SI 8.314472L
+
+/**
+ * This is the gas constant R in (kJ/mol)/K.
+ * This is an exact conversion from SI units, differing only in the use of kJ
+ * here vs. J in SI.
+ * @par uncertainty
+ * 15e-9
+ */
+#define SimTK_MOLAR_GAS_CONSTANT_MD 8.314472e-3L
+
+/**
+ * This is the gas constant R in (kcal/mol)/K.
+ * This is an exact conversion from MD units, differing only in the use of kcal
+ * here vs. kJ in MD.
+ * @par uncertainty
+ * 36e-10
+ */
+#define SimTK_MOLAR_GAS_CONSTANT_KCAL_ANGSTROM 1.9872065e-3L
+
+/**
+ * Boltzmann's constant in SI units of joules/kelvin; just divide R by NA.
+ * @par uncertainty
+ * 24e-30
+ */
+#define SimTK_BOLTZMANN_CONSTANT_SI 1.3806505e-23L
+
+/**
+ * Boltzmann's constant in MD units of (kJ/mol)/kelvin; same as R.
+ * @see SimTK_MOLAR_GAS_CONSTANT_MD
+ */
+#define SimTK_BOLTZMANN_CONSTANT_MD SimTK_MOLAR_GAS_CONSTANT_MD
+
+/**
+ * Boltzmann's constant in Kcal-Angstrom units of (kcal/mol)/kelvin; same as R.
+ * @see SimTK_MOLAR_GAS_CONSTANT_KCAL_ANGSTROM
+ */
+#define SimTK_BOLTZMANN_CONSTANT_KCAL_ANGSTROM SimTK_MOLAR_GAS_CONSTANT_KCAL_ANGSTROM
+
+/**@}**/ /*end of Physical Constants*/
+
+ /***************************/
+ /* UNIT CONVERSION FACTORS */
+ /***************************/
+
+/** @defgroup UnitConversionFactors Unit Conversion Factors
+ @ingroup MacroConstants
+
+In each case here, given a value in the units mentioned first in the name, you
+should multiply by the given constant to produce the equivalent quantity
+measured in the units that appear second in the name. You can perform the
+reverse conversion by dividing by the constant, or by using another conversion
+constant with the names reversed if one is supplied here.**/
+
+/**@{**/
+
+/**
+ * Convert radians to degrees.
+ * @par uncertainty
+ * approximation of an exact quantity
+ * @see SimTK_DEGREE_TO_RADIAN
+ */
+#define SimTK_RADIAN_TO_DEGREE 5.729577951308232087679815481410517033240547246656432154916024386e+1L
+
+/**
+ * Convert degrees to radians.
+ * @par uncertainty
+ * approximation of an exact quantity
+ * @see SimTK_RADIAN_TO_DEGREE
+ */
+#define SimTK_DEGREE_TO_RADIAN 1.745329251994329576923690768488612713442871888541725456097191440e-2L
+
+/**
+ * Convert nanoseconds to seconds.
+ * @par uncertainty
+ * exact
+ * @see SimTK_S_TO_NS
+ */
+#define SimTK_NS_TO_S 1e-9L
+
+/**
+ * Convert seconds to nanoseconds.
+ * @par uncertainty
+ * exact
+ * @see SimTK_NS_TO_S
+ */
+#define SimTK_S_TO_NS 1e9L
+
+/**
+ * Convert Kcal to Kjoule (also Kcal/mol to Kjoule/mol).
+ * @par uncertainty
+ * exact
+ * @see SimTK_KJOULE_TO_KCAL
+ */
+#define SimTK_KCAL_TO_KJOULE 4.184L /* exact */
+
+/**
+ * Convert Kjoule to Kcal (also Kjoule/mol to Kcal/mol).
+ * @par uncertainty
+ * approximation of an exact quantity
+ * @see SimTK_KCAL_TO_KJOULE
+ */
+#define SimTK_KJOULE_TO_KCAL 2.390057361376673040152963671128107074569789674952198852772466539e-1L
+
+/**
+ * Convert atomic mass unit (amu, Dalton) to g. This is 1/NA (NA=avogadro's number).
+ * @par uncertainty
+ * 28e-32
+ * @see SimTK_AVOGADROS_NUMBER
+ */
+#define SimTK_DALTON_TO_GRAM 1.66053886e-24L
+
+/**
+ * Convert proton charge units to Coulombs. This is the same as the
+ * conversion from electron volts to Joules, and both are just the
+ * charge of a proton in SI units.
+ * @see SimTK_CHARGE_OF_PROTON_IN_SI
+ * @see SimTK_EV_TO_JOULE
+ */
+#define SimTK_E_TO_COULOMB SimTK_CHARGE_OF_PROTON_IN_SI
+
+/**
+ * Convert electron volts to Joules. This is the same as the
+ * conversion from proton charge units to Coulombs, and both are just the
+ * charge of a proton in SI units.
+ * @see SimTK_CHARGE_OF_PROTON_IN_SI
+ * @see SimTK_E_TO_COULOMB
+ */
+#define SimTK_EV_TO_JOULE SimTK_CHARGE_OF_PROTON_IN_SI
+
+/**@}**/ /*end of Unit Conversion Factors*/
+
+#endif /* SimTK_SimTKCOMMON_CONSTANTS_H_ */
diff --git a/SimTKcommon/include/SimTKcommon/Simmatrix.h b/SimTKcommon/include/SimTKcommon/Simmatrix.h
new file mode 100644
index 0000000..99592db
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/Simmatrix.h
@@ -0,0 +1,105 @@
+#ifndef SimTK_SIMMATRIX_H_
+#define SimTK_SIMMATRIX_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This is the header which should be included in user programs that would
+ * like to make use of all the Simmatrix facilities, but none of the other
+ * parts of SimTKcommon.
+ */
+
+
+// Each of these is independently user-includable, with later ones including
+// former ones.
+#include "SimTKcommon/Scalar.h" // self-contained
+#include "SimTKcommon/SmallMatrix.h" // includes Scalar.h
+#include "SimTKcommon/Orientation.h" // includes SmallMatrix.h
+#include "SimTKcommon/Mechanics.h" // includes Orientation.h
+
+// Here we add the missing pieces that provide large matrix functionality,
+// and some additional small matrix functionality that depends on having
+// access to large matrix capabilities.
+#include "SimTKcommon/internal/BigMatrix.h"
+#include "SimTKcommon/internal/SmallDefsThatNeedBig.h"
+#include "SimTKcommon/internal/VectorMath.h"
+
+
+// This is so Doxygen can locate the symbols we mention.
+namespace SimTK {
+
+/** @defgroup MatVecUtilities Matrix and Vector Utilities
+
+ at brief Simbody contains an extensive library for manipulating Matrix and Vector
+objects, modeled after Matlab's similar features.
+
+Simbody's matrix library contains two separate but related sets of classes for
+vector and matrix objects, one set for small, fixed-size objects and the other
+for larger, run-time allocated objects.
+
+First, there are classes to represent small, fixed size vectors
+and matrices with zero runtime overhead: Vec for column vectors, and Mat for
+matrices. There is also a Row type that does not normally appear in user
+programs. These classes are templatized based on size and element type. Synonyms
+(typedefs) are defined for common combinations; for example,
+ at ref SimTK::Vec3 "Vec3" is a synonym for \c Vec<3,Real>, while
+ at ref SimTK::Mat22 "Mat22" is a synonym for \c Mat<2,2,Real>. (Typedef
+ at ref SimTK::Real "Real" is synonymous with C++ \c double unless Simbody was
+compiled with \c float as the default precision.) You can also
+create other combinations, such as \c Mat<2,10,Real> or
+\c Vec<4,std::complex<Real>>. However, the size must always be determinable at
+compile time. The in-memory representation of these small objects is minimal:
+only the data elements are stored.
+
+Second, there are classes to represent large vectors and matrices whose sizes
+are determined at runtime: Vector_ for column vectors and Matrix_ for matrices.
+There is also a RowVector_ type that rarely appears in user programs. These
+classes are templatized based on element type. In user code, it is most common
+to see typedefs @ref SimTK::Vector "Vector" and @ref SimTK::Matrix "Matrix"
+which are synonyms for \c Vector_<Real> and \c Matrix_<Real>. As for small matrices,
+you can use other element types. In fact, the element type can even be one of
+the fixed-size vector or matrix objects. For example, \c Vector_<Vec3> is a
+variable-length vector, where each element is itself a fixed-size, 3-component
+vector. The type \c Vec<2,Vec3>, called a spatial vector (@ref SimTK::SpatialVec
+"SpatialVec"), is useful for
+combining rotational and translational quantities into a single object
+representing a spatial velocity or spatial force, for example. However, it is not permissible to use the variable-size
+Vector_ or Matrix_ objects as element types. The in-memory representation of
+these objects includes, in addition to the data, an opaque descriptor containing
+the length and information on how the data is laid out; the declared objects
+actually consist only of a pointer (essentially a \c void*) to the descriptors.
+This has many advantages for implementation and binary compatibility, but makes
+it difficult to look through these objects in a debugger as you can with the
+small Vec and Mat classes.
+
+<h2>Implementation</h2>
+The intent of Simbody's matrix library, which we call \e Simmatrix, is similar
+to that of the %Eigen library (http://eigen.tuxfamily.org). If you want to know
+more about the design goals and implementation of Simmatrix, see the design
+document here: https://simtk.org/home/simbody, Documents tab.
+**/
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATRIX_H_
diff --git a/SimTKcommon/include/SimTKcommon/TemplatizedLapack.h b/SimTKcommon/include/SimTKcommon/TemplatizedLapack.h
new file mode 100644
index 0000000..60069fd
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/TemplatizedLapack.h
@@ -0,0 +1,361 @@
+#ifndef SimTK_SimTKCOMMON_TEMPLATIZED_LAPACK_H_
+#define SimTK_SimTKCOMMON_TEMPLATIZED_LAPACK_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * These is a templatized, C++ callable interface to LAPACK and BLAS.
+ * Each method must be explicitly specialized for the supported precisions.
+ */
+
+
+#include "SimTKcommon/internal/common.h"
+#include "SimTKlapack.h"
+
+#include <complex>
+using std::complex;
+
+namespace SimTK {
+
+class Lapack {
+public:
+ // MEANINGLESS IF NOT SPECIALIZED
+
+ template <class P> static void
+ gemm
+ (char transa, char transb,
+ int m, int n, int k,
+ const P& alpha, const P a[], int lda,
+ const P b[], int ldb,
+ const P& beta, P c[], int ldc) {assert(false);}
+
+ template <class P> static void
+ getri
+ (int n,
+ P a[],
+ int lda,
+ const int ipiv[],
+ P work[],
+ int lwork,
+ int &info ) {assert(false);}
+
+ template <class P> static void
+ getrf
+ (int m,
+ int n,
+ P a[],
+ int lda,
+ int ipiv[],
+ int &info ) {assert(false);}
+
+};
+
+ // xGEMM //
+
+template <> inline void Lapack::gemm<float>
+ (char transa, char transb,
+ int m, int n, int k,
+ const float& alpha, const float a[], int lda,
+ const float b[], int ldb,
+ const float& beta, float c[], int ldc)
+{
+ sgemm_(
+ transa, transb,
+ m,n,k,alpha,a,lda,b,ldb,beta,c,ldc
+ );
+}
+template <> inline void Lapack::gemm<double>
+ (char transa, char transb,
+ int m, int n, int k,
+ const double& alpha, const double a[], int lda,
+ const double b[], int ldb,
+ const double& beta, double c[], int ldc)
+{
+ dgemm_(
+ transa, transb,
+ m,n,k,alpha,a,lda,b,ldb,beta,c,ldc
+ );
+}
+template <> inline void Lapack::gemm< complex<float> >
+ (char transa, char transb,
+ int m, int n, int k,
+ const complex<float>& alpha, const complex<float> a[], int lda,
+ const complex<float> b[], int ldb,
+ const complex<float>& beta, complex<float> c[], int ldc)
+{
+ cgemm_(
+ transa, transb,
+ m,n,k,alpha,a,lda,b,ldb,beta,c,ldc
+ );
+}
+template <> inline void Lapack::gemm< complex<double> >
+ (char transa, char transb,
+ int m, int n, int k,
+ const complex<double>& alpha, const complex<double> a[], int lda,
+ const complex<double> b[], int ldb,
+ const complex<double>& beta, complex<double> c[], int ldc)
+{
+ zgemm_(
+ transa, transb,
+ m,n,k,alpha,a,lda,b,ldb,beta,c,ldc
+ );
+}
+
+ // xGETRI //
+
+template <> inline void Lapack::getri<float>
+ (int n,
+ float a[],
+ int lda,
+ const int ipiv[],
+ float work[],
+ int lwork,
+ int& info )
+{
+ sgetri_(n,a,lda,ipiv,work,lwork,info);
+}
+
+template <> inline void Lapack::getri<double>
+ (int n,
+ double a[],
+ int lda,
+ const int ipiv[],
+ double work[],
+ int lwork,
+ int& info )
+{
+ dgetri_(n,a,lda,ipiv,work,lwork,info);
+}
+
+template <> inline void Lapack::getri< complex<float> >
+ (int n,
+ complex<float> a[],
+ int lda,
+ const int ipiv[],
+ complex<float> work[],
+ int lwork,
+ int& info )
+{
+ cgetri_(n,a,lda,ipiv,work,lwork,info);
+}
+
+template <> inline void Lapack::getri< complex<double> >
+ (int n,
+ complex<double> a[],
+ int lda,
+ const int ipiv[],
+ complex<double> work[],
+ int lwork,
+ int& info )
+{
+ zgetri_(n,a,lda,ipiv,work,lwork,info);
+}
+ // xGETRF //
+
+template <> inline void Lapack::getrf<float>
+ (int m,
+ int n,
+ float a[],
+ int lda,
+ int ipiv[],
+ int& info )
+{
+ sgetrf_(m,n,a,lda,ipiv,info);
+}
+
+template <> inline void Lapack::getrf<double>
+ (int m,
+ int n,
+ double a[],
+ int lda,
+ int ipiv[],
+ int& info )
+{
+ dgetrf_(m,n,a,lda,ipiv,info);
+}
+
+template <> inline void Lapack::getrf< complex<float> >
+ (int m,
+ int n,
+ complex<float> a[],
+ int lda,
+ int ipiv[],
+ int& info )
+{
+ cgetrf_(m,n,a,lda,ipiv,info);
+}
+
+template <> inline void Lapack::getrf< complex<double> >
+ (int m,
+ int n,
+ complex<double> a[],
+ int lda,
+ int ipiv[],
+ int& info )
+{
+ zgetrf_(m,n,a,lda,ipiv,info);
+}
+
+
+/*
+void SimTK_STDCALL
+SimTK_LAPACK(dgeev,DGEEV)
+ (const char &jobvl SimTK_LAPACK_STRLEN_FOLLOWS_DECL,
+ const char &jobvr SimTK_LAPACK_STRLEN_FOLLOWS_DECL,
+ const int &n,
+ double a[],
+ const int &lda,
+ double wr[],
+ double wi[],
+ double vl[],
+ const int &ldvl,
+ double vr[],
+ const int &ldvr,
+ double work[],
+ const int &lwork,
+ int &info
+ SimTK_LAPACK_STRLEN_ATEND_DECL
+ SimTK_LAPACK_STRLEN_ATEND_DECL);
+
+void SimTK_STDCALL
+SimTK_LAPACK(dsyev,DSYEV)
+ (const char &jobz SimTK_LAPACK_STRLEN_FOLLOWS_DECL,
+ const char &uplo SimTK_LAPACK_STRLEN_FOLLOWS_DECL,
+ const int &n,
+ double a[],
+ const int &lda,
+ double w[],
+ double work[],
+ const int &lwork,
+ int &info
+ SimTK_LAPACK_STRLEN_ATEND_DECL
+ SimTK_LAPACK_STRLEN_ATEND_DECL);
+
+void SimTK_STDCALL
+SimTK_LAPACK(dspev,DSPEV)
+ (const char &jobz SimTK_LAPACK_STRLEN_FOLLOWS_DECL,
+ const char &uplo SimTK_LAPACK_STRLEN_FOLLOWS_DECL,
+ const int &n,
+ double a[],
+ double w[],
+ double z[],
+ const int &ldz,
+ double work[],
+ int &info
+ SimTK_LAPACK_STRLEN_ATEND_DECL
+ SimTK_LAPACK_STRLEN_ATEND_DECL);
+
+void SimTK_STDCALL
+SimTK_LAPACK(dsptri,DSPTRI)
+ (const char &uplo SimTK_LAPACK_STRLEN_FOLLOWS_DECL,
+ const int &size,
+ double a[],
+ int ipiv[],
+ double work[],
+ int &info
+ SimTK_LAPACK_STRLEN_ATEND_DECL);
+
+void SimTK_STDCALL
+SimTK_LAPACK(dsptrf,DSPTRF)
+ (const char &uplo SimTK_LAPACK_STRLEN_FOLLOWS_DECL,
+ const int &size,
+ double a[],
+ int ipiv[],
+ int &info
+ SimTK_LAPACK_STRLEN_ATEND_DECL);
+
+void SimTK_STDCALL
+SimTK_LAPACK(dsyevx,DSYEVX)
+ (const char &jobz,
+ const char &range,
+ const char &uplo,
+ const int &n,
+ double a[],
+ const int &lda,
+ const double &vl,
+ const double &vu,
+ const int &il,
+ const int &iu,
+ const double &abstol,
+ int &m,
+ double w[],
+ double z[],
+ const int &ldz,
+ double work[],
+ const int &lwork,
+ int iwork[],
+ int ifail[],
+ int &info);
+
+void SimTK_STDCALL
+SimTK_LAPACK(dgelss,DGELSS)
+ (int &m,
+ const int &n,
+ const int &nrhs,
+ double a[],
+ const int &lda,
+ double b[],
+ const int &ldb,
+ double s[],
+ const double &rcond,
+ int &rank,
+ double work[],
+ const int &lwork,
+ int &info );
+
+void SimTK_STDCALL
+SimTK_LAPACK(dgesv,DGESV)
+ (int &n,
+ int &nrhs,
+ double a[],
+ int &lda,
+ int ipiv[],
+ double b[],
+ int &ldb,
+ int &info);
+
+void SimTK_STDCALL
+SimTK_LAPACK(dgesvd,DGESVD)
+ (const char &jobu SimTK_LAPACK_STRLEN_FOLLOWS_DECL,
+ const char &jobvt SimTK_LAPACK_STRLEN_FOLLOWS_DECL,
+ const int &m,
+ const int &n,
+ double a[],
+ const int &lda,
+ double s[],
+ double u[],
+ const int &ldu,
+ double vt[],
+ const int &ldvt,
+ double work[],
+ const int &lwork,
+ int &info
+ SimTK_LAPACK_STRLEN_ATEND_DECL
+ SimTK_LAPACK_STRLEN_ATEND_DECL);
+
+*/
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_TEMPLATIZED_LAPACK_H_
diff --git a/SimTKcommon/include/SimTKcommon/Testing.h b/SimTKcommon/include/SimTKcommon/Testing.h
new file mode 100644
index 0000000..6e6be9f
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/Testing.h
@@ -0,0 +1,603 @@
+#ifndef SimTK_SimTKCOMMON_TESTING_H_
+#define SimTK_SimTKCOMMON_TESTING_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include "SimTKcommon/internal/Random.h"
+
+#include <cmath>
+#include <ctime>
+#include <algorithm>
+#include <iostream>
+
+/** @file
+ * This file defines a SimTK::Test class and some related macros which
+ * provide functionality useful in regression tests.
+ */
+
+namespace SimTK {
+
+/**@defgroup RegressionTesting SimTK Regression Testing
+ *
+ * SimTK defines some utilities to facilitate the creation of regression
+ * tests for SimTK facilities. These utilities consist of a SimTK::Test
+ * class and related support macros.
+ *
+ * Features include:
+ * - uniform, readable output including execution times
+ * - identical comparison tests for all numerical types, scalar and composite
+ * - careful treatment of numerical tolerances using relative and absolute
+ * comparisons, with provision for size-dependent, reduced accuracy
+ * expectations for matrix operations
+ * - default tolerance varies with precision (caller can override)
+ * - convenient generation of random test data
+ * - convenient testing of required argument checking (i.e., test
+ * fails unless an exception is thrown)
+ *
+ * The Testing.h header file is
+ * <em>not</em> automatically included with SimTKcommon.h; you have to
+ * ask for it explicilty. Here's how you use this facility:
+ * <pre>
+ * \#include "SimTKcommon/Testing.h"
+ * void myFirstSubtest() {...}
+ * void myNextSubtest() {...}
+ * int main() {
+ * SimTK_START_TEST("OverallTestName");
+ * SimTK_SUBTEST(myFirstSubtest);
+ * SimTK_SUBTEST(myNextSubtest);
+ * SimTK_END_TEST();
+ * }
+ * </pre>
+ * The arguments to SimTK_SUBTEST are function names and will be
+ * called with "()" appended. If your subtest functions have arguments,
+ * use SimTK_SUBTEST1(name,arg) or SimTK_SUBTEST2(name,arg1,arg2) which
+ * will call name(arg) or name(arg1,arg2) as appropriate.
+ *
+ * This will result in nice output including execution times for
+ * the overall test and the individual subtests, and arrange for
+ * any exceptions raised in the tests to be caught, properly reported,
+ * and cause a non-zero return from main(). If everything runs
+ * successfully, main() will return 0. Here is an example of the
+ * output produced:
+ * <pre>
+ * Starting test TestScalar ...
+ * testIsNaN ... done. testIsNaN time: 0ms.
+ * testIsInf ... done. testIsInf time: 0ms.
+ * testIsFinite ... done. testIsFinite time: 0ms.
+ * testSignBit ... done. testSignBit time: 0ms.
+ * testSign ... done. testSign time: 0ms.
+ * testSquareAndCube ... done. testSquareAndCube time: 0ms.
+ * Done. TestScalar time: 15ms.
+ * </pre>
+ * (Admittedly the timings aren't much use in that example!)
+ *
+ * Within your subtests, several useful macros and static functions
+ * are available. By using these macros, the resulting message will
+ * include the actual line number at which the test failure occurred.
+ * <pre>
+ * SimTK_TEST(cond) -- this is like assert(cond)
+ * SimTK_TEST_FAILED("message") -- like assert(!"message")
+ *
+ * SimTK_TEST_EQ(a,b) -- equal to within a default tolerance
+ * SimTK_TEST_NOTEQ(a,b) -- not equal to within a default tolerance
+ *
+ * SimTK_TEST_EQ_SIZE(a,b,n) -- equal to within n * default tolerance
+ * SimTK_TEST_NOTEQ_SIZE(a,b,n) -- not equal to within n * default tolerance
+ *
+ * SimTK_TEST_EQ_TOL(a,b,tol) -- same as above with specified tolerance
+ * SimTK_TEST_NOTEQ_TOL(a,b,tol)
+ *
+ * SimTK_TEST_MUST_THROW(statement) -- we expect the statement to throw some exception
+ * SimTK_TEST_MUST_THROW_EXC(statement, exception) -- we expect a particular exception type
+ * SimTK_TEST_MUST_THROW_DEBUG(statement) -- same as above but only checked in Debug builds
+ * SimTK_TEST_MUST_THROW_EXC_DEBUG(statement, exception) -- ditto
+ * </pre>
+ * The SimTK_TEST_EQ macros test scalar and composite numerical values for
+ * equality to within a numerical tolerance, using both relative
+ * and absolute tolerances. The default is the value of SignificantReal
+ * for the underlying numerical type. For composite types the equality test is done
+ * elementwise; that is, we apply it strictly to each pair of elements not
+ * to an overall norm.
+ *
+ * The SimTK_TEST_EQ_SIZE macros allows you to specify a multiple of default
+ * tolerance to be used. This is necessary for most Matrix operations since
+ * attainable accuracy falls off with the size of the matrix. Typically, if
+ * the smallest dimension of the Matrix is n, then the tolerance you should allow
+ * is n*scalarTol where scalarTol is the default tolerance for a scalar
+ * operation. Note that you still need to specify size when comparing
+ * Vector or scalar values if those values were produced using a matrix
+ * computation.
+ *
+ * The SimTK_TEST_EQ_TOL macros take a user-specified tolerance value for
+ * the elementwise tests, overriding the default.
+ *
+ * The SimTK::Test class has a number of static methods that are useful
+ * in tests. Currently these are all for generating numerical objects
+ * filled with random numbers (all uniform between -1 and 1). These are:
+ * <pre>
+ * randReal() randFloat() randDouble()
+ * randComplex() randConjugate()
+ * randVec<M>() randRow<N>() randMat<M,N>() randSymMat<N>()
+ * randVector(m) randMatrix(m,n)
+ * randVec3() randMat33()
+ * randSpatialVec() randSpatialMat()
+ * randRotation() randTransform()
+ * </pre>
+ * These are invoked Test::randReal() etc.
+ *
+ * @{
+ */
+
+
+/// This is the main class to support testing. Objects of this type are
+/// created by the SimTK_START_TEST macro; don't allocate them directly.
+/// The class name appears directly in tests only for access to its
+/// static members like Test::randMatrix().
+class Test {
+public:
+ class Subtest;
+ Test(const std::string& name) : testName(name)
+ {
+ std::clog << "Starting test " << testName << " ...\n";
+ startTime = std::clock();
+ }
+ ~Test() {
+ std::clog << "Done. " << testName << " time: "
+ << 1000*(std::clock()-startTime)/CLOCKS_PER_SEC << "ms.\n";
+ }
+
+ template <class T>
+ static double defTol() {return (double)NTraits<typename CNT<T>::Precision>::getSignificant();}
+
+ // For dissimilar types, the default tolerance is the narrowest of the two.
+ template <class T1, class T2>
+ static double defTol2() {return std::max(defTol<T1>(), defTol<T2>());}
+
+ // Scale by the magnitude of the quantities being compared, so that we don't
+ // ask for unreasonable precision. For magnitudes near zero, we'll be satisfied
+ // if both are very small without demanding that they must also be relatively
+ // close. That is, we use a relative tolerance for big numbers and an absolute
+ // tolerance for small ones.
+ static bool numericallyEqual(float v1, float v2, int n, double tol=defTol<float>()) {
+ const float scale = n*std::max(std::max(std::abs(v1), std::abs(v2)), 1.0f);
+ return std::abs(v1-v2) < scale*(float)tol;
+ }
+ static bool numericallyEqual(double v1, double v2, int n, double tol=defTol<double>()) {
+ const double scale = n*std::max(std::max(std::abs(v1), std::abs(v2)), 1.0);
+ return std::abs(v1-v2) < scale*(double)tol;
+ }
+ static bool numericallyEqual(long double v1, long double v2, int n, double tol=defTol<long double>()) {
+ const long double scale = n*std::max(std::max(std::abs(v1), std::abs(v2)), 1.0l);
+ return std::abs(v1-v2) < scale*(long double)tol;
+ }
+
+ // For integers we ignore tolerance.
+ static bool numericallyEqual(int i1, int i2, int n, double tol=0) {return i1==i2;}
+ static bool numericallyEqual(unsigned u1, unsigned u2, int n, double tol=0) {return u1==u2;}
+
+ // Mixed floating types use default tolerance for the narrower type.
+ static bool numericallyEqual(float v1, double v2, int n, double tol=defTol<float>())
+ { return numericallyEqual((double)v1, v2, n, tol); }
+ static bool numericallyEqual(double v1, float v2, int n, double tol=defTol<float>())
+ { return numericallyEqual(v1, (double)v2, n, tol); }
+ static bool numericallyEqual(float v1, long double v2, int n, double tol=defTol<float>())
+ { return numericallyEqual((long double)v1, v2, n, tol); }
+ static bool numericallyEqual(long double v1, float v2, int n, double tol=defTol<float>())
+ { return numericallyEqual(v1, (long double)v2, n, tol); }
+ static bool numericallyEqual(double v1, long double v2, int n, double tol=defTol<double>())
+ { return numericallyEqual((long double)v1, v2, n, tol); }
+ static bool numericallyEqual(long double v1, double v2, int n, double tol=defTol<double>())
+ { return numericallyEqual(v1, (long double)v2, n, tol); }
+
+ // Mixed int/floating just upgrades int to floating type.
+ static bool numericallyEqual(int i1, float f2, int n, double tol=defTol<float>())
+ { return numericallyEqual((float)i1,f2,n,tol); }
+ static bool numericallyEqual(float f1, int i2, int n, double tol=defTol<float>())
+ { return numericallyEqual(f1,(float)i2,n,tol); }
+ static bool numericallyEqual(unsigned i1, float f2, int n, double tol=defTol<float>())
+ { return numericallyEqual((float)i1,f2,n,tol); }
+ static bool numericallyEqual(float f1, unsigned i2, int n, double tol=defTol<float>())
+ { return numericallyEqual(f1,(float)i2,n,tol); }
+ static bool numericallyEqual(int i1, double f2, int n, double tol=defTol<double>())
+ { return numericallyEqual((double)i1,f2,n,tol); }
+ static bool numericallyEqual(double f1, int i2, int n, double tol=defTol<double>())
+ { return numericallyEqual(f1,(double)i2,n,tol); }
+ static bool numericallyEqual(unsigned i1, double f2, int n, double tol=defTol<double>())
+ { return numericallyEqual((double)i1,f2,n,tol); }
+ static bool numericallyEqual(double f1, unsigned i2, int n, double tol=defTol<double>())
+ { return numericallyEqual(f1,(double)i2,n,tol); }
+ static bool numericallyEqual(int i1, long double f2, int n, double tol=defTol<long double>())
+ { return numericallyEqual((long double)i1,f2,n,tol); }
+ static bool numericallyEqual(long double f1, int i2, int n, double tol=defTol<long double>())
+ { return numericallyEqual(f1,(long double)i2,n,tol); }
+ static bool numericallyEqual(unsigned i1, long double f2, int n, double tol=defTol<long double>())
+ { return numericallyEqual((long double)i1,f2,n,tol); }
+ static bool numericallyEqual(long double f1, unsigned i2, int n, double tol=defTol<long double>())
+ { return numericallyEqual(f1,(long double)i2,n,tol); }
+
+ template <class P>
+ static bool numericallyEqual(const std::complex<P>& v1, const std::complex<P>& v2, int n, double tol=defTol<P>()) {
+ return numericallyEqual(v1.real(), v2.real(), n, tol)
+ && numericallyEqual(v1.imag(), v2.imag(), n, tol);
+ }
+ template <class P>
+ static bool numericallyEqual(const conjugate<P>& v1, const conjugate<P>& v2, int n, double tol=defTol<P>()) {
+ return numericallyEqual(v1.real(), v2.real(), n, tol)
+ && numericallyEqual(v1.imag(), v2.imag(), n, tol);
+ }
+ template <class P>
+ static bool numericallyEqual(const std::complex<P>& v1, const conjugate<P>& v2, int n, double tol=defTol<P>()) {
+ return numericallyEqual(v1.real(), v2.real(), n, tol)
+ && numericallyEqual(v1.imag(), v2.imag(), n, tol);
+ }
+ template <class P>
+ static bool numericallyEqual(const conjugate<P>& v1, const std::complex<P>& v2, int n, double tol=defTol<P>()) {
+ return numericallyEqual(v1.real(), v2.real(), n, tol)
+ && numericallyEqual(v1.imag(), v2.imag(), n, tol);
+ }
+ template <class P>
+ static bool numericallyEqual(const negator<P>& v1, const negator<P>& v2, int n, double tol=defTol<P>()) {
+ return numericallyEqual(-v1, -v2, n, tol); // P, P
+ }
+ template <class P>
+ static bool numericallyEqual(const P& v1, const negator<P>& v2, int n, double tol=defTol<P>()) {
+ return numericallyEqual(-v1, -v2, n, tol); // P, P
+ }
+ template <class P>
+ static bool numericallyEqual(const negator<P>& v1, const P& v2, int n, double tol=defTol<P>()) {
+ return numericallyEqual(-v1, -v2, n, tol); // P, P
+ }
+ template <class P>
+ static bool numericallyEqual(const negator<std::complex<P> >& v1, const conjugate<P>& v2, int n, double tol=defTol<P>()) {
+ return numericallyEqual(-v1, -v2, n, tol); // complex, conjugate
+ }
+ template <class P>
+ static bool numericallyEqual(const negator<conjugate<P> >& v1, const std::complex<P>& v2, int n, double tol=defTol<P>()) {
+ return numericallyEqual(-v1, -v2, n, tol); // conjugate, complex
+ }
+ template <class P>
+ static bool numericallyEqual(const std::complex<P>& v1, const negator<conjugate<P> >& v2, int n, double tol=defTol<P>()) {
+ return numericallyEqual(-v1, -v2, n, tol); // complex, conjugate
+ }
+ template <class P>
+ static bool numericallyEqual(const conjugate<P>& v1, const negator<std::complex<P> >& v2, int n, double tol=defTol<P>()) {
+ return numericallyEqual(-v1, -v2, n, tol); // conjugate, complex
+ }
+ template <int M, class E1, int S1, class E2, int S2>
+ static bool numericallyEqual(const Vec<M,E1,S1>& v1, const Vec<M,E2,S2>& v2, int n, double tol=(defTol2<E1,E2>())) {
+ for (int i=0; i<M; ++i) if (!numericallyEqual(v1[i],v2[i], n, tol)) return false;
+ return true;
+ }
+ template <int N, class E1, int S1, class E2, int S2>
+ static bool numericallyEqual(const Row<N,E1,S1>& v1, const Row<N,E2,S2>& v2, int n, double tol=(defTol2<E1,E2>())) {
+ for (int j=0; j<N; ++j) if (!numericallyEqual(v1[j],v2[j], n, tol)) return false;
+ return true;
+ }
+ template <int M, int N, class E1, int CS1, int RS1, class E2, int CS2, int RS2>
+ static bool numericallyEqual(const Mat<M,N,E1,CS1,RS1>& v1, const Mat<M,N,E2,CS2,RS2>& v2, int n, double tol=(defTol2<E1,E2>())) {
+ for (int j=0; j<N; ++j) if (!numericallyEqual(v1(j),v2(j), n, tol)) return false;
+ return true;
+ }
+ template <int N, class E1, int S1, class E2, int S2>
+ static bool numericallyEqual(const SymMat<N,E1,S1>& v1, const SymMat<N,E2,S2>& v2, int n, double tol=(defTol2<E1,E2>())) {
+ return numericallyEqual(v1.getAsVec(), v2.getAsVec(), n, tol);
+ }
+ template <class E1, class E2>
+ static bool numericallyEqual(const VectorView_<E1>& v1, const VectorView_<E2>& v2, int n, double tol=(defTol2<E1,E2>())) {
+ if (v1.size() != v2.size()) return false;
+ for (int i=0; i < v1.size(); ++i)
+ if (!numericallyEqual(v1[i], v2[i], n, tol)) return false;
+ return true;
+ }
+ template <class E1, class E2>
+ static bool numericallyEqual(const Vector_<E1>& v1, const Vector_<E2>& v2, int n, double tol=(defTol2<E1,E2>()))
+ { return numericallyEqual((const VectorView_<E1>&)v1, (const VectorView_<E2>&)v2, n, tol); }
+ template <class E1, class E2>
+ static bool numericallyEqual(const Vector_<E1>& v1, const VectorView_<E2>& v2, int n, double tol=(defTol2<E1,E2>()))
+ { return numericallyEqual((const VectorView_<E1>&)v1, (const VectorView_<E2>&)v2, n, tol); }
+ template <class E1, class E2>
+ static bool numericallyEqual(const VectorView_<E1>& v1, const Vector_<E2>& v2, int n, double tol=(defTol2<E1,E2>()))
+ { return numericallyEqual((const VectorView_<E1>&)v1, (const VectorView_<E2>&)v2, n, tol); }
+
+ template <class E1, class E2>
+ static bool numericallyEqual(const RowVectorView_<E1>& v1, const RowVectorView_<E2>& v2, int n, double tol=(defTol2<E1,E2>())) {
+ if (v1.size() != v2.size()) return false;
+ for (int i=0; i < v1.size(); ++i)
+ if (!numericallyEqual(v1[i], v2[i], n, tol)) return false;
+ return true;
+ }
+ template <class E1, class E2>
+ static bool numericallyEqual(const RowVector_<E1>& v1, const RowVector_<E2>& v2, int n, double tol=(defTol2<E1,E2>()))
+ { return numericallyEqual((const RowVectorView_<E1>&)v1, (const RowVectorView_<E2>&)v2, n, tol); }
+ template <class E1, class E2>
+ static bool numericallyEqual(const RowVector_<E1>& v1, const RowVectorView_<E2>& v2, int n, double tol=(defTol2<E1,E2>()))
+ { return numericallyEqual((const RowVectorView_<E1>&)v1, (const RowVectorView_<E2>&)v2, n, tol); }
+ template <class E1, class E2>
+ static bool numericallyEqual(const RowVectorView_<E1>& v1, const RowVector_<E2>& v2, int n, double tol=(defTol2<E1,E2>()))
+ { return numericallyEqual((const RowVectorView_<E1>&)v1, (const RowVectorView_<E2>&)v2, n, tol); }
+
+ template <class E1, class E2>
+ static bool numericallyEqual(const MatrixView_<E1>& v1, const MatrixView_<E2>& v2, int n, double tol=(defTol2<E1,E2>())) {
+ if (v1.nrow() != v2.nrow() || v1.ncol() != v2.ncol()) return false;
+ for (int j=0; j < v1.ncol(); ++j)
+ if (!numericallyEqual(v1(j), v2(j), n, tol)) return false;
+ return true;
+ }
+ template <class E1, class E2>
+ static bool numericallyEqual(const Matrix_<E1>& m1, const Matrix_<E2>& m2, int n, double tol=(defTol2<E1,E2>()))
+ { return numericallyEqual((const MatrixView_<E1>&)m1, (const MatrixView_<E2>&)m2, n, tol); }
+ template <class E1, class E2>
+ static bool numericallyEqual(const Matrix_<E1>& m1, const MatrixView_<E2>& m2, int n, double tol=(defTol2<E1,E2>()))
+ { return numericallyEqual((const MatrixView_<E1>&)m1, (const MatrixView_<E2>&)m2, n, tol); }
+ template <class E1, class E2>
+ static bool numericallyEqual(const MatrixView_<E1>& m1, const Matrix_<E2>& m2, int n, double tol=(defTol2<E1,E2>()))
+ { return numericallyEqual((const MatrixView_<E1>&)m1, (const MatrixView_<E2>&)m2, n, tol); }
+
+ template <class P>
+ static bool numericallyEqual(const Rotation_<P>& R1, const Rotation_<P>& R2, int n, double tol=defTol<P>()) {
+ return R1.isSameRotationToWithinAngle(R2, (Real)(n*tol));
+ }
+
+ template <class P>
+ static bool numericallyEqual(const Transform_<P>& T1, const Transform_<P>& T2, int n, double tol=defTol<P>()) {
+ return numericallyEqual(T1.R(), T2.R(), n, tol)
+ && numericallyEqual(T1.p(), T2.p(), n, tol);
+ }
+
+ template <class P>
+ static bool numericallyEqual(const UnitInertia_<P>& G1, const UnitInertia_<P>& G2, int n, double tol=defTol<P>()) {
+ return numericallyEqual(G1.asSymMat33(),G2.asSymMat33(), n, tol);
+ }
+
+ template <class P>
+ static bool numericallyEqual(const Inertia_<P>& I1, const Inertia_<P>& I2, int n, double tol=defTol<P>()) {
+ return numericallyEqual(I1.asSymMat33(),I2.asSymMat33(), n, tol);
+ }
+
+ // Random numbers
+ static Real randReal() {
+ static Random::Uniform rand(-1,1);
+ return rand.getValue();
+ }
+ static Complex randComplex() {return Complex(randReal(),randReal());}
+ static Conjugate randConjugate() {return Conjugate(randReal(),randReal());}
+ static float randFloat() {return (float)randReal();}
+ static double randDouble() {return (double)randReal();}
+
+ template <int M> static Vec<M> randVec()
+ { Vec<M> v; for (int i=0; i<M; ++i) v[i]=randReal(); return v;}
+ template <int N> static Row<N> randRow() {return ~randVec<N>();}
+ template <int M, int N> static Mat<M,N> randMat()
+ { Mat<M,N> m; for (int j=0; j<N; ++j) m(j)=randVec<M>(); return m;}
+ template <int N> static SymMat<N> randSymMat()
+ { SymMat<N> s; s.updAsVec() = randVec<N*(N+1)/2>(); return s; }
+
+ static Vector randVector(int m)
+ { Vector v(m); for (int i=0; i<m; ++i) v[i]=randReal(); return v;}
+ static Matrix randMatrix(int m, int n)
+ { Matrix M(m,n); for (int j=0; j<n; ++j) M(j)=randVector(m); return M;}
+
+ static Vec3 randVec3() {return randVec<3>();}
+ static Mat33 randMat33() {return randMat<3,3>();}
+ static SymMat33 randSymMat33() {return randSymMat<3>();}
+ static SpatialVec randSpatialVec() {
+ return SpatialVec(randVec3(), randVec3());
+ }
+ static SpatialMat randSpatialMat() {
+ return SpatialMat(randMat33(), randMat33(),
+ randMat33(), randMat33());
+ }
+ static Rotation randRotation() {
+ // Generate random angle and random axis to rotate around.
+ return Rotation((Pi/2)*randReal(), randVec3());
+ }
+ static Transform randTransform() {
+ return Transform(randRotation(), randVec3());
+ }
+private:
+ std::clock_t startTime;
+ std::string testName;
+};
+
+/// Internal utility class for generating test messages for subtests.
+class Test::Subtest {
+public:
+ Subtest(const std::string& name) : subtestName(name)
+ {
+ char padded[128];
+ sprintf(padded, "%-20s", name.c_str());
+ paddedName = std::string(padded);
+ std::clog << " " << paddedName << " ... " << std::flush;
+ startTime = std::clock();
+ }
+ ~Subtest() {
+ std::clog << "done. " << paddedName << " time: "
+ << 1000*(std::clock()-startTime)/CLOCKS_PER_SEC << "ms.\n";
+ }
+private:
+ std::clock_t startTime;
+ std::string subtestName;
+ std::string paddedName; // name plus some blanks
+};
+
+} // namespace SimTK
+
+/// Invoke this macro before anything else in your test's main().
+#define SimTK_START_TEST(testName) \
+ SimTK::Test simtk_test_(testName); \
+ try {
+
+/// Invoke this macro as the last thing in your test's main().
+#define SimTK_END_TEST() \
+ } catch(const std::exception& e) { \
+ std::cerr << "Test failed due to exception: " \
+ << e.what() << std::endl; \
+ return 1; \
+ } catch(...) { \
+ std::cerr << "Test failed due to unrecognized exception.\n"; \
+ return 1; \
+ } \
+ return 0;
+
+/// Invoke a subtest in the form of a no-argument function, arranging for some
+/// friendly output and timing information.
+#define SimTK_SUBTEST(testFunction) \
+ do {SimTK::Test::Subtest sub(#testFunction); (testFunction)();} while(false)
+/// Invoke a subtest in the form of a 1-argument function, arranging for some
+/// friendly output and timing information.
+#define SimTK_SUBTEST1(testFunction,arg1) \
+ do {SimTK::Test::Subtest sub(#testFunction); (testFunction)(arg1);} while(false)
+/// Invoke a subtest in the form of a 2-argument function, arranging for some
+/// friendly output and timing information.
+#define SimTK_SUBTEST2(testFunction,arg1,arg2) \
+ do {SimTK::Test::Subtest sub(#testFunction); (testFunction)(arg1,arg2);} while(false)
+/// Invoke a subtest in the form of a 3-argument function, arranging for some
+/// friendly output and timing information.
+#define SimTK_SUBTEST3(testFunction,arg1,arg2,arg3) \
+ do {SimTK::Test::Subtest sub(#testFunction); (testFunction)(arg1,arg2,arg3);} while(false)
+/// Invoke a subtest in the form of a 4-argument function, arranging for some
+/// friendly output and timing information.
+#define SimTK_SUBTEST4(testFunction,arg1,arg2,arg3,arg4) \
+ do {SimTK::Test::Subtest sub(#testFunction); (testFunction)(arg1,arg2,arg3,arg4);} while(false)
+
+/// Test that some condition holds and complain if it doesn't.
+#define SimTK_TEST(cond) {SimTK_ASSERT_ALWAYS((cond), "Test condition failed.");}
+
+/// Call this if you have determined that a test case has failed and just need
+/// to report it and die. Pass the message as a string in quotes.
+#define SimTK_TEST_FAILED(msg) {SimTK_ASSERT_ALWAYS(!"Test case failed.", msg);}
+
+/// Call this if you have determined that a test case has failed and just need
+/// to report it and die. The message is a printf format string in quotes; here
+/// with one argument expected.
+#define SimTK_TEST_FAILED1(fmt,a1) {SimTK_ASSERT1_ALWAYS(!"Test case failed.",fmt,a1);}
+
+/// Call this if you have determined that a test case has failed and just need
+/// to report it and die. The message is a printf format string in quotes; here
+/// with two arguments expected.
+#define SimTK_TEST_FAILED2(fmt,a1,a2) {SimTK_ASSERT2_ALWAYS(!"Test case failed.",fmt,a1,a2);}
+
+/// Test that two numerical values are equal to within a reasonable numerical
+/// error tolerance, using a relative and absolute error tolerance. In the
+/// case of composite types, the test is performed elementwise.
+#define SimTK_TEST_EQ(v1,v2) \
+ {SimTK_ASSERT_ALWAYS(SimTK::Test::numericallyEqual((v1),(v2),1), \
+ "Test values should have been numerically equivalent at default tolerance.");}
+
+/// Test that two numerical values are equal to within a specified multiple of the
+/// default error tolerance.
+#define SimTK_TEST_EQ_SIZE(v1,v2,n) \
+ {SimTK_ASSERT1_ALWAYS(SimTK::Test::numericallyEqual((v1),(v2),(n)), \
+ "Test values should have been numerically equivalent at size=%d times default tolerance.",(n));}
+
+/// Test that two numerical values are equal to within a specified numerical
+/// error tolerance, using a relative and absolute error tolerance. In the
+/// case of composite types, the test is performed elementwise.
+#define SimTK_TEST_EQ_TOL(v1,v2,tol) \
+ {SimTK_ASSERT1_ALWAYS(SimTK::Test::numericallyEqual((v1),(v2),1,(tol)), \
+ "Test values should have been numerically equivalent at tolerance=%g.",(tol));}
+
+/// Test that two numerical values are NOT equal to within a reasonable numerical
+/// error tolerance, using a relative and absolute error tolerance. In the
+/// case of composite types, the equality test is performed elementwise.
+#define SimTK_TEST_NOTEQ(v1,v2) \
+ {SimTK_ASSERT_ALWAYS(!SimTK::Test::numericallyEqual((v1),(v2),1), \
+ "Test values should NOT have been numerically equivalent (at default tolerance).");}
+
+/// Test that two numerical values are NOT equal to within a specified multiple of
+/// the default error tolerance, using a relative and absolute error tolerance. In the
+/// case of composite types, the equality test is performed elementwise.
+#define SimTK_TEST_NOTEQ_SIZE(v1,v2,n) \
+ {SimTK_ASSERT1_ALWAYS(!SimTK::Test::numericallyEqual((v1),(v2),(n)), \
+ "Test values should NOT have been numerically equivalent at size=%d times default tolerance.",(n));}
+
+/// Test that two numerical values are NOT equal to within a specified numerical
+/// error tolerance, using a relative and absolute error tolerance. In the
+/// case of composite types, the equality test is performed elementwise.
+#define SimTK_TEST_NOTEQ_TOL(v1,v2,tol) \
+ {SimTK_ASSERT1_ALWAYS(!SimTK::Test::numericallyEqual((v1),(v2),1,(tol)), \
+ "Test values should NOT have been numerically equivalent at tolerance=%g.",(tol));}
+
+/// Test that the supplied statement throws an std::exception of some kind.
+#define SimTK_TEST_MUST_THROW(stmt) \
+ do {int threw=0; try {stmt;} \
+ catch(const std::exception&){threw=1;} \
+ catch(...){threw=2;} \
+ if (threw==0) SimTK_TEST_FAILED1("Expected statement\n----\n%s\n----\n to throw an exception but it did not.",#stmt); \
+ if (threw==2) SimTK_TEST_FAILED1("Expected statement\n%s\n to throw an std::exception but it threw something else.",#stmt); \
+ }while(false)
+
+/// Test that the supplied statement throws a particular exception.
+#define SimTK_TEST_MUST_THROW_EXC(stmt,exc) \
+ do {int threw=0; try {stmt;} \
+ catch(const exc&){threw=1;} \
+ catch(...){threw=2;} \
+ if (threw==0) SimTK_TEST_FAILED1("Expected statement\n----\n%s\n----\n to throw an exception but it did not.",#stmt); \
+ if (threw==2) SimTK_TEST_FAILED2("Expected statement\n----\n%s\n----\n to throw exception type %s but it threw something else.",#stmt,#exc); \
+ }while(false)
+
+/// Allow the supplied statement to throw any std::exception without failing.
+#define SimTK_TEST_MAY_THROW(stmt) \
+ do {int threw=0; try {stmt;} \
+ catch(const std::exception&){threw=1;} \
+ catch(...){threw=2;} \
+ if (threw==2) SimTK_TEST_FAILED1("Expected statement\n%s\n to throw an std::exception but it threw something else.",#stmt); \
+ }while(false)
+
+/// Allow the supplied statement to throw a particular exception without failing.
+#define SimTK_TEST_MAY_THROW_EXC(stmt,exc) \
+ do {int threw=0; try {stmt;} \
+ catch(const exc&){threw=1;} \
+ catch(...){threw=2;} \
+ if (threw==2) SimTK_TEST_FAILED2("Expected statement\n----\n%s\n----\n to throw exception type %s but it threw something else.",#stmt,#exc); \
+ }while(false)
+
+// When we're only required to throw in Debug, we have to suppress the
+// test case altogether in Release because it may cause damage.
+#if defined(NDEBUG)
+ /// Include a bad statement when in Debug and insist that it get caught,
+ /// but don't include the statement at all in Release.
+ #define SimTK_TEST_MUST_THROW_DEBUG(stmt)
+ /// Include a bad statement when in Debug and insist that it get caught,
+ /// but don't include the statement at all in Release.
+ #define SimTK_TEST_MUST_THROW_EXC_DEBUG(stmt,exc)
+#else
+ /// Include a bad statement when in Debug and insist that it get caught,
+ /// but don't include the statement at all in Release.
+ #define SimTK_TEST_MUST_THROW_DEBUG(stmt) SimTK_TEST_MUST_THROW(stmt)
+ /// Include a bad statement when in Debug and insist that it get caught,
+ /// but don't include the statement at all in Release.
+ #define SimTK_TEST_MUST_THROW_EXC_DEBUG(stmt,exc) \
+ SimTK_TEST_MUST_THROW_EXC(stmt,exc)
+#endif
+
+
+
+
+// End of Regression testing group.
+/// @}
+
+#endif // SimTK_SimTKCOMMON_TESTING_H_
diff --git a/SimTKcommon/include/SimTKcommon/basics.h b/SimTKcommon/include/SimTKcommon/basics.h
new file mode 100644
index 0000000..00e7488
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/basics.h
@@ -0,0 +1,57 @@
+#ifndef SimTK_SimTKCOMMON_BASICS_H_
+#define SimTK_SimTKCOMMON_BASICS_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Includes internal headers providing declarations for the basic SimTK
+ * Core classes. This file can be included from ANSI C code as well as
+ * C++, although only a small subset of the definitions will be done
+ * in a C program. These include default precision and simple macros
+ * such as those used for physical constants.
+ */
+
+/* NOTE: don't use "//" comments in this file! */
+
+/* These two are safe for C programs. */
+#include "SimTKcommon/internal/common.h"
+#include "SimTKcommon/Constants.h"
+
+#if defined(__cplusplus)
+#include "SimTKcommon/internal/Exception.h"
+#include "SimTKcommon/internal/ExceptionMacros.h"
+#include "SimTKcommon/internal/ClonePtr.h"
+#include "SimTKcommon/internal/ReferencePtr.h"
+#include "SimTKcommon/internal/String.h"
+#include "SimTKcommon/internal/Serialize.h"
+#include "SimTKcommon/internal/Fortran.h"
+#include "SimTKcommon/internal/Array.h"
+#include "SimTKcommon/internal/StableArray.h"
+#include "SimTKcommon/internal/Value.h"
+#include "SimTKcommon/internal/Stage.h"
+#include "SimTKcommon/internal/CoordinateAxis.h"
+#endif
+
+
+#endif /* SimTK_SimTKCOMMON_BASICS_H_ */
diff --git a/SimTKcommon/include/SimTKcommon/internal/Array.h b/SimTKcommon/include/SimTKcommon/internal/Array.h
new file mode 100644
index 0000000..9fd9561
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/Array.h
@@ -0,0 +1,3675 @@
+#ifndef SimTK_SimTKCOMMON_ARRAY_H_
+#define SimTK_SimTKCOMMON_ARRAY_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This file defines the Array_<T,X> class and related support classes
+ * including base classes ArrayViewConst_<T,X> and ArrayView_<T,X>, and
+ * helper class ArrayIndexTraits<X>.
+ */
+
+#include "SimTKcommon/internal/common.h"
+#include "SimTKcommon/internal/ExceptionMacros.h"
+#include "SimTKcommon/internal/Serialize.h"
+
+#include <algorithm>
+#include <iterator>
+#include <vector>
+#include <ostream>
+#include <climits>
+#include <typeinfo>
+
+namespace SimTK {
+
+// These are the classes defined in this header.
+template <class X> struct ArrayIndexTraits;
+template <class T, class X=unsigned> class ArrayViewConst_;
+template <class T, class X=unsigned> class ArrayView_;
+template <class T, class X=unsigned> class Array_;
+
+
+
+//==============================================================================
+// CLASS ArrayIndexTraits
+//==============================================================================
+
+/** This templatized type is used by the Array_<T,X> classes to obtain the
+information they need to use the class X as an index class for the array.
+There must be a specialization here providing ArrayIndexTraits for each of
+the built-in integral types that is suitable for use as an index. Any other
+type X will qualify as an index if it defines the following members:
+
+ - typedef size_type
+ - typedef difference_type
+ - static size_type max_size()
+ - operator size_type() const (conversion from X to size_type)
+
+max_size() determines the largest number of elements that a container may hold
+if its index type is X.
+
+size_type must be an integral type large enough to hold
+all the values from 0 to max_size(), including all the index values (which
+range from 0 to max_size()-1). size_type may be signed or unsigned; it is the
+type returned by size(), max_size(), capacity(), etc. and may be compared
+directly against an index of type X without producing a compiler warning.
+
+difference_type is a signed integral type that can hold all possible
+differences between two indices, that is, values between -(max_size()-1) and
++(max_size()-1). In most cases we use an integral type with the same number of
+bits for size_type and difference_type but when the index type is very small
+(bool, unsigned char, or unsigned short) we want to allow the full range (2,
+255, or 65535 elements, resp.) in which case we need a wider type to hold the
+differences.
+
+The conversion operator ensures that we can write size_type(i) for an index
+i of type X. An explicit conversion member does not need to be present as long
+as the conversion size_type(i) already works as it does for all the integral
+type specializations.
+
+The SimTK type-generating macro SimTK_DEFINE_UNIQUE_INDEX_TYPE() provides the
+necessary members so that these types can be used directly as index types for
+Array_ objects with no further preparation. For example, you can make an
+Array_<int,MobilizedBodyIndex> that stores ints that can be indexed only via
+MobilizedBodyIndex indices.
+
+ at tparam X A type suitable for use as an Array_ index.
+ at see Array_
+**/
+template <class X> struct ArrayIndexTraits {
+ /** The signed or unsigned integral type to which an object of index type
+ X can be converted without producing any compiler warnings. **/
+ typedef typename X::size_type size_type;
+ /** A signed integral type large enough to hold the full range of
+ possible signed differences i-j between two indices i and j of type X. **/
+ typedef typename X::difference_type difference_type;
+ /** The maximum allowable size for any Array_<T,X> that uses this type X
+ as its index type. **/
+ static size_type max_size() {return X::max_size();}
+};
+
+/** Specialization of ArrayIndexTraits for \c unsigned (that is, \c unsigned
+\c int) used as an index. **/
+template <> struct ArrayIndexTraits<unsigned> {
+ typedef unsigned size_type;
+ typedef int difference_type;
+ static size_type max_size() {return (unsigned)INT_MAX;}
+};
+
+/** Specialization of ArrayIndexTraits for (signed) \c int used as an index. **/
+template <> struct ArrayIndexTraits<int> {
+ typedef int size_type;
+ typedef int difference_type;
+ static size_type max_size() {return INT_MAX;}
+};
+
+/** Specialization of ArrayIndexTraits for \c unsigned \c long used as an index.
+ at warning
+Different 64 bit platforms have different lengths for long. In particular,
+64 bit MSVC++ has sizeof(long)==sizeof(int) while 64 bit gcc has
+sizeof(long)==sizeof(long long). We recommend that you avoid using long
+and unsigned long (ever, not just here) and instead use int or long long (or
+their unsigned versions) which are unambiguously 32 or 64 bits, resp. **/
+template <> struct ArrayIndexTraits<unsigned long> {
+ typedef unsigned long size_type;
+ typedef long difference_type;
+ static size_type max_size() {return (unsigned long)LONG_MAX;}
+};
+
+/** Specialization of ArrayIndexTraits for (signed) \c long used as an index.
+ at warning
+Different 64 bit platforms have different lengths for long. In particular,
+64 bit MSVC++ has sizeof(long)==sizeof(int) while 64 bit gcc has
+sizeof(long)==sizeof(long long). We recommend that you avoid using long
+and unsigned long (ever, not just here) and instead use int or long long (or
+their unsigned versions) which are unambiguously 32 or 64 bits, resp. **/
+template <> struct ArrayIndexTraits<long> {
+ typedef long size_type;
+ typedef long difference_type;
+ static size_type max_size() {return LONG_MAX;}
+};
+
+/** Specialization of ArrayIndexTraits for \c unsigned \c short used as an
+index. We don't have any bits to spare here so we want to allow the full
+65535 elements for an unsigned short indexed container. That means the index
+difference range is -65534..+65534 which doesn't fit in a short so we have to
+use an int for difference_type to accommodate the whole range. **/
+template <> struct ArrayIndexTraits<unsigned short> {
+ typedef unsigned short size_type;
+ typedef int difference_type;
+ static size_type max_size() {return USHRT_MAX;}
+};
+
+/** Specialization of ArrayIndexTraits for (signed) \c short used as an
+index. In contrast to unsigned short, here the max size is 32767 so the index
+difference range -32766..+32766 still fits in a short so we don't need a wider
+type for difference_type. **/
+template <> struct ArrayIndexTraits<short> {
+ typedef short size_type;
+ typedef short difference_type;
+ static size_type max_size() {return SHRT_MAX;}
+};
+
+
+/** Specialization of ArrayIndexTraits for \c unsigned \c char used as
+an index. Here we don't have any bits to spare and we want to use the full
+max size of 255. The max index must then be 254, so the difference_type must
+hold -254..254 which takes a short. **/
+template <> struct ArrayIndexTraits<unsigned char> {
+ typedef unsigned char size_type;
+ typedef short difference_type;
+ static size_type max_size() {return UCHAR_MAX;} // not CHAR_MAX
+};
+
+/** Specialization of ArrayIndexTraits for \c signed \c char used as
+an index. In contrast with the unsigned char case which allows 255 elements,
+the max size here is 127 meaning the max index is 126 and the difference range
+is -126..126 which still fits in a signed char so we don't need a wider type
+for difference_type. **/
+template <> struct ArrayIndexTraits<signed char> {
+ typedef signed char size_type;
+ typedef signed char difference_type;
+ static size_type max_size() {return SCHAR_MAX;}
+};
+
+/** Specialization of ArrayIndexTraits for \c char used as
+an index. The C++ standard does not specify whether \c char is a signed or
+unsigned type; here we'll limit its max size to 127 so that we don't have
+to use the non-standard high bit. That means it behaves just like the signed
+char case; if you want the full range to a size of 255, use unsigned char
+instead. **/
+template <> struct ArrayIndexTraits<char> {
+ typedef char size_type;
+ typedef signed char difference_type;
+ static size_type max_size() {return (char)SCHAR_MAX;}
+};
+
+/** Specialization of ArrayIndexTraits for \c bool used as an index. OK, this
+seems unlikely but it works fine -- you get a container that can hold only two
+elements indexed by either \c false (0) or \c true (1). You'll get warnings
+if you try to index with an ordinary int. If anyone finds a legitimate use for
+this, please post to the forum and let us know! **/
+template <> struct ArrayIndexTraits<bool> {
+ typedef unsigned char size_type;
+ typedef signed char difference_type;
+ static size_type max_size() {return 2;}
+};
+
+/** Specialization of ArrayIndexTraits for \c unsigned \c long \c long used as
+an index. This only makes sense in a 64-bit compilation. **/
+template <> struct ArrayIndexTraits<unsigned long long> {
+ typedef unsigned long long size_type;
+ typedef long long difference_type;
+ static size_type max_size()
+ {return (unsigned long long)LLONG_MAX;}
+};
+
+/** Specialization of ArrayIndexTraits for \c long \c long used as
+an index. This only makes sense in a 64-bit compilation. **/
+template <> struct ArrayIndexTraits<long long> {
+ typedef long long size_type;
+ typedef long long difference_type;
+ static size_type max_size() {return LLONG_MAX;}
+};
+
+// Don't show this in Doxygen.
+/** @cond **/
+// This helper class decides what integral type we should use to best pack
+// the index type's size_type representation. The idea is to pack the whole
+// Array_ structure into 8 bytes on a 32 bit machine, 16 bytes on a 64 bit
+// machine, using the largest integral type that will work, giving a layout
+// like this: | data pointer |
+// | nUsed | nAllocated |
+
+// The default implementation just uses the integral type itself.
+template <class Integral, class is64Bit> struct ArrayIndexPackTypeHelper
+{ typedef Integral packed_size_type;};
+
+// On 32 bit machine, pack anything smaller than a short into a short.
+template<> struct ArrayIndexPackTypeHelper<bool,FalseType>
+{ typedef unsigned short packed_size_type;};
+template<> struct ArrayIndexPackTypeHelper<char,FalseType>
+{ typedef unsigned short packed_size_type;};
+template<> struct ArrayIndexPackTypeHelper<unsigned char,FalseType>
+{ typedef unsigned short packed_size_type;};
+template<> struct ArrayIndexPackTypeHelper<signed char,FalseType>
+{ typedef short packed_size_type;};
+
+// On 64 bit machine, pack anything smaller than an int into an int.
+template<> struct ArrayIndexPackTypeHelper<bool,TrueType>
+{ typedef unsigned int packed_size_type;};
+template<> struct ArrayIndexPackTypeHelper<char,TrueType>
+{ typedef unsigned int packed_size_type;};
+template<> struct ArrayIndexPackTypeHelper<unsigned char,TrueType>
+{ typedef unsigned int packed_size_type;};
+template<> struct ArrayIndexPackTypeHelper<signed char,TrueType>
+{ typedef int packed_size_type;};
+template<> struct ArrayIndexPackTypeHelper<unsigned short,TrueType>
+{ typedef unsigned int packed_size_type;};
+template<> struct ArrayIndexPackTypeHelper<short,TrueType>
+{ typedef int packed_size_type;};
+
+template <class Integral> struct ArrayIndexPackType
+{ typedef typename ArrayIndexPackTypeHelper<Integral,Is64BitPlatformType>
+ ::packed_size_type packed_size_type;};
+/** @endcond **/
+
+
+
+
+
+
+//==============================================================================
+// CLASS ArrayViewConst_
+//==============================================================================
+/** This Array_ helper class is the base class for ArrayView_ which is the
+base class for Array_; here we provide only the minimal read-only "const"
+functionality required by any Array_ object, and shallow copy semantics. The
+ability to write is added by the ArrayView_ class, and the additional ability
+to reallocate, insert, erase, etc. is added by the Array_ class.
+
+This class is particularly useful for recasting existing const data into a
+const Array_ without copying. For example a const std::vector can be passed
+to a const Array& argument by an implicit, near-zero cost conversion to an
+ArrayViewConst_ which can then convert to a const Array&.
+
+An ArrayViewConst_ is given all the data it is going to have at the time it is
+constructed (except when it is being accessed from the derived Array_ class
+that has more capability). The contents and size of a ArrayViewConst_ cannot be
+changed after construction. In particular, the default copy assignment operator
+is suppressed. The destructor simply disconnects the ArrayViewConst_ handle
+from the data it was referencing; no element destruction or heap deallocation
+occurs.
+
+ at tparam T
+ The type of object to be stored in this container.
+ at tparam X
+ The type to be used for indexing this container, with default unsigned
+ (not size_t). Any integral type may be used, as well as user types that
+ satisfy the requirements discussed with class ArrayIndexTraits.
+ at see Array_, ArrayView_, ArrayIndexTraits **/
+template <class T, class X> class ArrayViewConst_ {
+public:
+
+
+//------------------------------------------------------------------------------
+/** @name Typedefs
+
+Types required of STL containers, plus index_type which is an extension, and
+packed_size_type which is an implementation detail. **/
+/*@{*/
+/** The type of object stored in this container. **/
+typedef T value_type;
+/** The index type (an extension). **/
+typedef X index_type;
+/** A writable pointer to a value_type. **/
+typedef T* pointer;
+/** A const pointer to a value_type. **/
+typedef const T* const_pointer;
+/** A writable value_type reference. **/
+typedef T& reference;
+/** A const value_type reference. **/
+typedef const T& const_reference;
+/** A writable iterator for this container (same as pointer here). **/
+typedef T* iterator;
+/** A const iterator for this container (same as const_pointer here). **/
+typedef const T* const_iterator;
+/** A writable reverse iterator for this container. **/
+typedef std::reverse_iterator<iterator> reverse_iterator;
+/** A const reverse iterator for this container. **/
+typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
+/** An integral type suitable for all indices and sizes for this array. **/
+typedef typename ArrayIndexTraits<X>::size_type size_type;
+/** A signed integral type that can represent the difference between any two
+legitimate index values for this array. **/
+typedef typename ArrayIndexTraits<X>::difference_type difference_type;
+/** The integral type we actually use internally to store size_type values. **/
+typedef typename ArrayIndexPackType<size_type>::packed_size_type
+ packed_size_type;
+/*@} End of typedefs **/
+
+
+//------------------------------------------------------------------------------
+/** @name Construction, conversion, and destruction
+
+Constructors here are limited to those that don't allocate new data, and can
+only accept const data to reference. Copy assignment is suppressed. **/
+/*@{*/
+
+/** Default constructor allocates no heap space and is very fast. **/
+ArrayViewConst_() : pData(0), nUsed(0), nAllocated(0) {}
+
+/** Copy constructor is shallow; the constructed const array object will be
+referencing the original source data. However, if the source is zero length,
+this will result in a default-constructed array view handle with a null data
+pointer, even if the source had some unused data allocated.
+
+ at param[in] src
+ The object whose data will be referenced.
+ at par Complexity:
+ Constant time; extremely fast. **/
+ArrayViewConst_(const ArrayViewConst_& src)
+: pData(0), nUsed(src.nUsed), nAllocated(0) {
+ if (nUsed) pData = const_cast<T*>(src.pData);
+}
+
+// Copy assignment is suppressed.
+
+/** Construct an ArrayViewConst_<T> by referencing (sharing) a given range of
+const data [first,last1), without copying that data. This will work as long as
+the size of the source data does not exceed the array's max_size. The resulting
+object is not resizeable but can be used to read elements of the original data.
+This will becomes invalid if the original data is destructed or resized, but
+there is no way for the ArrayViewConst_ class to detect that.
+
+ at param[in] first
+ A pointer to the first data element to be referenced.
+ at param[in] last1
+ A pointer to the position one element past the last one in the range to be
+ referenced.
+ at remarks
+ - If the source data is empty, the resulting ArrayViewConst_ will also
+ be empty and will look as though it had been default-constructed.
+ - You can break the connection between the array handle and the data it
+ was constructed from by calling disconnect().
+ at pre first <= last1, last1-first <= max_size()
+ at par Complexity:
+ Dirt cheap. There will be no construction, destruction, or heap allocation
+ performed.
+ at see disconnect() **/
+ArrayViewConst_(const T* first, const T* last1)
+: pData(0),nUsed(0),nAllocated(0) {
+ if (last1==first) return; // empty
+
+ SimTK_ERRCHK((first&&last1)||(first==last1),
+ "ArrayViewConst_<T>(first,last1)",
+ "One of the source pointers was null (0); either both must be"
+ " non-null or both must be null.");
+
+ SimTK_ERRCHK3(this->isSizeOK(last1-first),
+ "ArrayViewConst_<T>(first,last1)",
+ "The source data's size %llu is too big for this array which"
+ " is limited to %llu elements by its index type %s.",
+ this->ull(last1-first), ullMaxSize(), indexName());
+
+ pData = const_cast<T*>(first);
+ nUsed = packed_size_type(last1-first);
+ // nAllocated is already zero
+}
+
+/** Construct a ArrayViewConst_<T> by referencing (sharing) the data in a const
+std::vector<T>, without copying the data; this is also an implicit conversion.
+This will work as long as the size of the vector does not exceed the array's
+max_size. The resulting array object is not resizeable but can be used to read
+elements of the original std::vector. The array becomes invalid if the original
+std::vector is destructed or resized, but there is no way for the array class
+to detect that.
+
+ at param[in] src
+ The std::vector<T> whose data will be referenced by the constructed
+ ArrayViewConst_ handle.
+ at remarks
+ - If the source std::vector is empty, the resulting array will also be empty
+ and will look as though it had been default-constructed. It will therefore
+ not have any connection to the source vector.
+ - This is quite dangerous to use since the connection between the array
+ and the vector is tenuous and subject to the vector remaining untouched
+ during the lifetime of the array handle. There is no reference
+ counting; destructing the vector leaves the array referring to garbage. Be
+ careful!
+ - You can break the connection between the array view and the vector it was
+ constructed from by calling disconnect().
+ at pre src.size() <= max_size()
+ at par Complexity:
+ Dirt cheap. There will be no construction, destruction, or heap allocation
+ performed.
+ at see disconnect() **/
+template <class A>
+ArrayViewConst_(const std::vector<T,A>& src)
+: pData(0),nUsed(0),nAllocated(0) {
+ if (src.empty()) return;
+
+ SimTK_ERRCHK3(this->isSizeOK(src.size()),
+ "ArrayViewConst_<T>::ctor(std::vector<T>)",
+ "The source std::vector's size %llu is too big for this array which"
+ " is limited to %llu elements by its index type %s.",
+ this->ull(src.size()), ullMaxSize(), indexName());
+
+ pData = const_cast<T*>(&src.front());
+ nUsed = packed_size_type(src.size());
+ // nAllocated is already zero
+}
+/** This is an implicit conversion to const ArrayView_<T,X>&, which is
+harmless since the const result won't permit writing on the elements. **/
+operator const ArrayView_<T,X>&() const
+{ return *reinterpret_cast<const ArrayView_<T,X>*>(this); }
+/** This is an implicit conversion to const Array_<T,X>&, which is harmless
+since the const result can't be used to write on or resize the data. **/
+operator const Array_<T,X>&() const
+{ return *reinterpret_cast<const Array_<T,X>*>(this); }
+
+/** Disconnect this array handle from any data to which it refers,
+restoring it to the condition it would be in if it had just been
+default-constructed. The data pointer will simply be set to null; we'll assume
+the owner will clean things up later. In either case the size() and capacity()
+will be zero after this call and data() will return null (0). **/
+void disconnect() {
+ SimTK_ASSERT(nAllocated==0,
+ "ArrayViewConst_::deallocate(): called on an owner Array_");
+ nUsed = 0;
+ pData = 0;
+}
+
+/** The destructor just disconnects the array view handle from its data; see
+disconnect() for more information. @see disconnect() **/
+~ArrayViewConst_() {
+ disconnect();
+}
+/*@} End of constructors, etc. **/
+
+
+//------------------------------------------------------------------------------
+/** @name Size and capacity
+
+These methods examine the number of elements (size) or the amount of allocated
+heap space (capacity). See the derived Array_<T,X> class for methods that can
+\e change the size or capacity. **/
+/*@{*/
+
+/** Return the current number of elements stored in this array. **/
+size_type size() const {return size_type(nUsed);}
+/** Return the maximum allowable size for this array. **/
+size_type max_size() const {return ArrayIndexTraits<X>::max_size();}
+/** Return true if there are no elements currently stored in this array. This
+is equivalent to the tests begin()==end() or size()==0. **/
+bool empty() const {return nUsed==0;}
+/** Return the number of elements this array can currently hold without
+requiring reallocation. The value returned by capacity() is always greater
+than or equal to size(), even if the data is not owned by this array in
+which case we have capacity()==size() and the array is not reallocatable. **/
+size_type capacity() const {return size_type(nAllocated?nAllocated:nUsed);}
+/** Return the amount of heap space owned by this array; this is the same
+as capacity() for owner arrays but is zero for non-owners.
+ at note There is no equivalent of this method for std::vector. **/
+size_type allocated() const {return size_type(nAllocated);}
+/** Does this array own the data to which it refers? If not, it can't be
+resized, and the destructor will not free any heap space nor call any element
+destructors. If the array does not refer to any data it is considered to be
+an owner since it is resizeable.
+ at note There is no equivalent of this method for std::vector. **/
+bool isOwner() const {return nAllocated || pData==0;}
+/*}*/
+
+
+//------------------------------------------------------------------------------
+/** @name Read-only element access
+
+These methods provide read-only (const) access to individual elements that are
+currently present in the array. The derived ArrayView_<T,X> class adds the
+non-const versions of these methods. **/
+/*@{*/
+
+/** Select an element by its index, returning a const reference. Note that only
+a value of the array's templatized index type is allowed (default is unsigned).
+This will be range-checked in a Debug build but not in Release.
+ at pre 0 <= \a i < size()
+ at par Complexity:
+ Constant time. **/
+const T& operator[](index_type i) const {
+ SimTK_INDEXCHECK(size_type(i),size(),"ArrayViewConst_<T>::operator[]()");
+ return pData[i];
+}
+/** Same as operator[] but always range-checked, even in a Release build.
+ at pre 0 <= \a i < size()
+ at par Complexity:
+ Constant time. **/
+const T& at(index_type i) const {
+ SimTK_INDEXCHECK_ALWAYS(size_type(i),size(),"ArrayViewConst_<T>::at()");
+ return pData[i];
+}
+/** Same as the const form of operator[]; exists to provide a non-operator
+method for element access in case that's needed. **/
+const T& getElt(index_type i) const {
+ SimTK_INDEXCHECK(size_type(i),size(),"ArrayViewConst_<T>::getElt()");
+ return pData[i];
+}
+/** Return a const reference to the first element in this array, which must
+not be empty (we'll check in a Debug build but not Release).
+ at pre The array is not empty.
+ at par Complexity:
+ Constant time. **/
+const T& front() const
+{ SimTK_ERRCHK(!empty(), "ArrayViewConst_<T>::front()", "Array was empty.");
+ return pData[0]; }
+/** Return a const reference to the last element in this array, which must
+not be empty (we'll check in a Debug build but not Release).
+ at pre The array is not empty.
+ at par Complexity:
+ Constant time. **/
+const T& back() const
+{ SimTK_ERRCHK(!empty(), "ArrayViewConst_<T>::back()", "Array was empty.");
+ return pData[nUsed-1]; }
+
+/** Select a contiguous subarray of the elements of this array and create
+another ArrayViewConst_ that refers only to those element (without copying).
+ at param[in] index
+ The index of the first element to be included in the subarray; this can
+ be one past the end of the array if \a length is zero.
+ at param[in] length
+ The length of the subarray to be produced.
+ at return
+ A new ArrayViewConst_<T,X> object referencing the original data.
+ at note
+ If \a length==0 the returned array will be in a default-constructed,
+ all-zero and null state with no connection to the original data.
+ at pre \a index >= 0, \a length >= 0
+ at pre \a index + \a length <= size()
+ at pre We'll validate preconditions in Debug builds but not Release.
+ at par Complexity:
+ Dirt cheap; no element construction or destruction or heap allocation
+ is required. **/
+ArrayViewConst_ operator()(index_type index, size_type length) const {
+ const size_type ix(index);
+ SimTK_ERRCHK2(isSizeInRange(ix, size()), "ArrayViewConst_<T>(index,length)",
+ "For this operator, we must have 0 <= index <= size(), but"
+ " index==%llu and size==%llu.", this->ull(ix), ullSize());
+ SimTK_ERRCHK2(isSizeInRange(length, size_type(size()-ix)),
+ "ArrayViewConst_<T>(index,length)",
+ "This operator requires 0 <= length <= size()-index, but"
+ " length==%llu and size()-index==%llu.",this->ull(length),this->ull(size()-ix));
+
+ return ArrayViewConst_(pData+ix, pData+ix+length);
+}
+/** Same as const form of operator()(index,length); exists to provide
+non-operator access to that functionality in case it is needed. **/
+ArrayViewConst_ getSubArray(index_type index, size_type length) const
+{ return (*this)(index,length); }
+
+/*@} End of element access. **/
+
+
+//------------------------------------------------------------------------------
+/** @name Iterators (const only)
+
+These methods deal in iterators, which are STL generalized pointers. For this
+class, iterators are just ordinary const pointers to T, and you may depend on
+that. By necessity, reverse iterators can't be just pointers; however, they
+contain an ordinary iterator (i.e. a pointer) that can be obtained by calling
+the reverse iterator's base() method. **/
+/*@{*/
+
+/** Return a const pointer to the first element of this array if any, otherwise
+cend(), which may be null (0) in that case but does not have to be. This method
+is from the proposed C++0x standard; there is also an overloaded begin() from
+the original standard that returns a const pointer. **/
+const T* cbegin() const {return pData;}
+/** Return a const pointer to what would be the element just after the last one
+in the array; this may be null (0) if there are no elements but doesn't have to
+be. This method is from the proposed C++0x standard; there is also an
+overloaded end() from the original standard that returns a const pointer. **/
+const T* cend() const {return pData + nUsed;}
+/** The const version of begin() is the same as cbegin(). **/
+const T* begin() const {return pData;}
+/** The const version of end() is the same as cend(). **/
+const T* end() const {return pData + nUsed;}
+
+/** Return a const reverse iterator pointing to the last element in the array
+or crend() if the array is empty. **/
+const_reverse_iterator crbegin() const {return const_reverse_iterator(cend());}
+/** Return the past-the-end reverse iterator that tests equal to a reverse
+iterator that has been incremented past the front of the array. You cannot
+dereference this iterator. **/
+const_reverse_iterator crend() const {return const_reverse_iterator(cbegin());}
+/** The const version of rbegin() is the same as crbegin(). **/
+const_reverse_iterator rbegin() const {return crbegin();}
+/** The const version of rend() is the same as crend(). **/
+const_reverse_iterator rend() const {return crend();}
+
+/** Return a const pointer to the first element of the array, or possibly
+(but not necessarily) null (0) if the array is empty.
+ at note
+ cdata() does not appear to be in the C++0x standard although it would seem
+ obvious in view of the cbegin() and cend() methods that had to be added.
+ The C++0x overloaded const data() method is also available. **/
+const T* cdata() const {return pData;}
+/** The const version of the data() method is identical to cdata(). **/
+const T* data() const {return pData;}
+/*@} End of iterators. **/
+
+
+//------------------------------------------------------------------------------
+ protected:
+//------------------------------------------------------------------------------
+// The remainder of this class is for the use of the ArrayView_<T,X> and
+// Array_<T,X> derived classes only and should not be documented for users to
+// see.
+
+// Don't let doxygen see any of this.
+/** @cond **/
+packed_size_type psize() const {return nUsed;}
+packed_size_type pallocated() const {return nAllocated;}
+
+// These provide direct access to the data members for our trusted friends.
+void setData(const T* p) {pData = const_cast<T*>(p);}
+void setSize(size_type n) {nUsed = packed_size_type(n);}
+void incrSize() {++nUsed;}
+void decrSize() {--nUsed;}
+void setAllocated(size_type n) {nAllocated = packed_size_type(n);}
+
+// Check whether a given size is the same as the current size of this array,
+// avoiding any compiler warnings due to mismatched integral types.
+template <class S>
+bool isSameSize(S sz) const
+{ return ull(sz) == ullSize(); }
+
+// Check that a source object's size will fit in the array being careful to
+// avoid overflow and warnings in the comparison.
+template <class S>
+bool isSizeOK(S srcSz) const
+{ return ull(srcSz) <= ullMaxSize(); }
+
+// This is identical in function to std::distance() (reports how many
+// elements lie between two iterators) but avoids any slow
+// Release-build bugcatchers that Microsoft may have felt compelled to add.
+// The implementation is specialized for random access iterators because
+// they can measure distance very fast.
+template<class Iter> static
+typename std::iterator_traits<Iter>::difference_type
+iterDistance(const Iter& first, const Iter& last1) {
+ return iterDistanceImpl(first,last1,
+ typename std::iterator_traits<Iter>::iterator_category());
+}
+
+// Generic slow implementation for non-random access iterators. This is fine
+// for forward and bidirectional iterators, but it will *consume* input
+// iterators so is useless for them.
+template<class Iter> static
+typename std::iterator_traits<Iter>::difference_type
+iterDistanceImpl(const Iter& first, const Iter& last1, std::input_iterator_tag) {
+ typename std::iterator_traits<Iter>::difference_type d = 0;
+ for (Iter src=first; src != last1; ++src, ++d)
+ ;
+ return d;
+}
+
+// Fast specialization for random access iterators (including ordinary
+// pointers) -- just subtract.
+template<class Iter> static
+typename std::iterator_traits<Iter>::difference_type
+iterDistanceImpl(const Iter& first, const Iter& last1,
+ std::random_access_iterator_tag) {
+ return last1 - first;
+}
+
+// This method attempts to determine whether any elements in the iterator range
+// [first,last1) overlap with the elements stored in this array. This is used
+// for error checks for operations where source is not permitted to overlap the
+// destination. For random access iterators (including ordinary pointers), we
+// can answer this question definitively because we expect the data to be
+// consecutive in memory. For other kinds of iterators, we will just assume
+// there is no overlap. Note that null ranges do not overlap even if the
+// pair of equal iterators points within the other range -- what matters is
+// the number of overlapping elements.
+template<class Iter> bool
+overlapsWithData(const Iter& first, const Iter& last1) {
+ return overlapsWithDataImpl(first,last1,
+ typename std::iterator_traits<Iter>::iterator_category());
+}
+
+// This is a partial specialization of the above where the data is given
+// with ordinary pointers.
+template <class T2> bool
+overlapsWithData(const T2* first, const T2* last1) {
+ // Find the start and end+1 of the alleged overlap region. There is
+ // overlap iff end+1 > start. Note that this works if either range
+ // is [0,0) or [p,p), or if last1 is illegally less than first (we just
+ // want to report no overlap in that case -- it is someone else's business
+ // to complain).
+ const T* obegin = std::max(cbegin(), (const T*)first);
+ const T* oend1 = std::min(cend(), (const T*)last1);
+
+ return obegin < oend1;
+}
+
+// This is the generic implementation for any type of input iterator other than
+// random access (i.e., bidirectional, forward, or input) -- assume no overlap.
+template<class Iter> bool
+overlapsWithDataImpl(const Iter&, const Iter&, std::input_iterator_tag)
+{ return false; }
+
+// Here we can actually test for overlap since we have random access iterators.
+// We convert them to pointers and then look for memory overlap.
+template<class Iter> bool
+overlapsWithDataImpl(const Iter& first, const Iter& last1,
+ std::random_access_iterator_tag) {
+ // We must check that the input iterators span a non-zero range before
+ // assuming we can dereference them.
+ if (last1 <= first)
+ return false; // zero or malformed source range: no overlap
+
+ // We now know we can dereference first and last1-1 (can't safely
+ // dereference last1 but we can use pointer arithmetic to point past
+ // the (last-1)th element in memory). We then take the dereferenced
+ // object's address to get ordinary pointers that we can use to
+ // watch for illegal overlap.
+ return overlapsWithData(&*first, &*(last1-1)); // use pointer overload
+}
+
+// Cast an integral type to maximal-width unsigned long long to avoid accidental
+// overflows that might otherwise occur due to wraparound that can happen
+// with small index types.
+template <class S>
+static unsigned long long ull(S sz)
+{ return (unsigned long long)sz; }
+
+// Return size(), capacity(), and max_size() cast to unsigned long long.
+unsigned long long ullSize() const {return ull(size());}
+unsigned long long ullCapacity() const {return ull(capacity());}
+unsigned long long ullMaxSize() const {return ull(max_size());}
+
+// Useful in error messages for explaining why something was too big.
+const char* indexName() const {return NiceTypeName<X>::name();}
+
+/** @endcond **/
+
+private:
+//------------------------------------------------------------------------------
+// DATA MEMBERS
+// These are the only data members and this layout is guaranteed not to change
+// from release to release. If data is null, then nUsed==nAllocated==0.
+
+T* pData; // ptr to data referenced here, or 0 if none
+packed_size_type nUsed; // number of elements currently present (size)
+packed_size_type nAllocated; // heap allocation; 0 if pData is not owned
+
+ArrayViewConst_& operator=(const ArrayViewConst_& src); // suppressed
+};
+
+
+
+
+
+
+//==============================================================================
+// CLASS ArrayView_
+//==============================================================================
+/** This Array_ helper class is the base class for Array_, extending
+ArrayViewConst_ to add the ability to modify elements, but not the ability to
+change size or reallocate.
+
+ at tparam T
+ The type of object to be stored in this container.
+ at tparam X
+ The type to be used for indexing this container, with default unsigned
+ (not size_t). Any integral type may be used, as well as user types that
+ satisfy the requirements discussed with class ArrayIndexTraits.
+ at see Array_, ArrayViewConst_, ArrayIndexTraits **/
+template <class T, class X> class ArrayView_ : public ArrayViewConst_<T,X> {
+typedef ArrayViewConst_<T,X> CBase;
+public:
+//------------------------------------------------------------------------------
+/** @name Typedefs
+
+Types required of STL containers, plus index_type which is an extension, and
+packed_size_type which is an implementation detail. **/
+/*{*/
+typedef T value_type;
+typedef X index_type;
+typedef T* pointer;
+typedef const T* const_pointer;
+typedef T& reference;
+typedef const T& const_reference;
+typedef T* iterator;
+typedef const T* const_iterator;
+typedef std::reverse_iterator<iterator> reverse_iterator;
+typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
+typedef typename ArrayIndexTraits<X>::size_type size_type;
+typedef typename ArrayIndexTraits<X>::difference_type difference_type;
+typedef typename ArrayIndexPackType<size_type>::packed_size_type
+ packed_size_type;
+/*}*/
+
+
+//------------------------------------------------------------------------------
+/** @name Construction, conversion, and destruction
+
+Constructors here are limited to those that don't allocate new data, however
+they can reference writable data. **/
+/*@{*/
+
+/** Default constructor allocates no heap space and is very fast. **/
+ArrayView_() : CBase() {}
+
+/** Copy constructor is shallow. **/
+ArrayView_(const ArrayView_& src) : CBase(src) {}
+
+/** Construct from a range of writable memory. **/
+ArrayView_(T* first, const T* last1) : CBase(first,last1) {}
+
+/** Construct to reference memory owned by a writable std::vector. **/
+template <class A>
+ArrayView_(std::vector<T,A>& v) : CBase(v) {}
+
+/** Implicit conversion of const ArrayView_ to const Array_& (zero cost). **/
+operator const Array_<T,X>&() const
+{ return *reinterpret_cast<const Array_<T,X>*>(this); }
+
+/** Implicit conversion of non-const ArrayView_ to Array_& (zero cost). **/
+operator Array_<T,X>&()
+{ return *reinterpret_cast<Array_<T,X>*>(this); }
+
+/** Forward to base class disconnect() method -- clears the handle without
+doing anything to the data. */
+void disconnect() {this->CBase::disconnect();}
+
+/** The destructor just disconnects the array view handle from its data; see
+ArrayViewConst_<T,X>::disconnect() for more information. **/
+~ArrayView_() {this->CBase::disconnect();}
+/*@} End of construction, etc. **/
+
+
+//------------------------------------------------------------------------------
+/** @name Assignment
+
+Assignment is permitted only if the source and destination are the same
+size. The semantics here are different than for a resizeable Array_
+object: here the meaning is elementwise assignment rather than destruction
+followed by copy construction. That is, if our elements are of type T, and
+the source elements are of type T2, we will use the operator of T that
+best matches the signature T::operator=(const T2&) to perform the assignments.
+When the source also has type T, this is just T's copy assignment operator.
+We never perform any element destruction or construction here. **/
+/*@{*/
+
+/** Copy assignment; source must be the same size as this array. **/
+ArrayView_& operator=(const ArrayView_& src) {
+ if (&src != this)
+ avAssignIteratorDispatch(src.cbegin(), src.cend(),
+ std::random_access_iterator_tag(),
+ "ArrayView_<T>::operator=(ArrayView_<T>)");
+ return *this;
+}
+
+
+/** Assignment from any other array object is allowed as long as the number
+of elements matches and the types are assignment compatible. **/
+template <class T2, class X2>
+ArrayView_& operator=(const ArrayViewConst_<T2,X2>& src) {
+ if ((const void*)&src != (void*)this)
+ avAssignIteratorDispatch(src.cbegin(), src.cend(),
+ std::random_access_iterator_tag(),
+ "ArrayView_<T>::operator=(Array_<T2>)");
+ return *this;
+}
+
+// Help out dumb compilers struggling to match the template arguments and
+// promote the Array_ or ArrayView_ to ArrayConstView_ at the same time.
+
+/** Assignment from any other array object is allowed as long as the number
+of elements matches and the types are assignment compatible. **/
+template <class T2, class X2>
+ArrayView_& operator=(const ArrayView_<T2,X2>& src)
+{ return *this = static_cast<const ArrayViewConst_<T2,X2>&>(src); }
+/** Assignment from any other array object is allowed as long as the number
+of elements matches and the types are assignment compatible. **/
+template <class T2, class X2>
+ArrayView_& operator=(const Array_<T2,X2>& src)
+{ return *this = static_cast<const ArrayViewConst_<T2,X2>&>(src); }
+
+/** Assignment from any std::vector object is allowed as long as the number
+of elements matches and the types are assignment compatible. **/
+template <class T2, class A2>
+ArrayView_& operator=(const std::vector<T2,A2>& src) {
+ avAssignIteratorDispatch(src.begin(), src.end(),
+ std::random_access_iterator_tag(),
+ "ArrayView_<T>::operator=(std::vector<T2>)");
+ return *this;
+}
+
+/** Fill assignment -- all elements are set to fillValue. @see fill() **/
+ArrayView_& operator=(const T& fillValue)
+{ fill(fillValue); return *this; }
+
+/** Assign the supplied fill value to each element of this array, using T's
+copy assignment operator for each element. Note that this also serves to allow
+fill from an object whose type T2 is different from T, as long as there is a
+constructor T(T2) that works since that can be invoked (implicitly or
+explicitly) to convert the T2 object to type T prior to the call. **/
+ArrayView_& fill(const T& fillValue) {
+ for (T* d = begin(); d != end(); ++d)
+ *d = fillValue; // using T::operator=(T)
+ return *this;
+}
+
+/** This is the same as fill() but has the usual std::vector signature for
+compatibility; it will only work if the given number of elements is the same
+as this array's (fixed) size. **/
+void assign(size_type n, const T& fillValue) {
+ SimTK_ERRCHK2(n == size(), "ArrayView_<T>::assign(n,value)",
+ "Assignment to an ArrayView is permitted only if the source"
+ " is the same size. Here n==%llu element(s) but the"
+ " ArrayView has a fixed size of %llu.",
+ this->ull(n), this->ull(size()));
+
+ fill(fillValue);
+}
+
+/** Assign to this array to make it a copy of the elements in range
+[first,last1) given by ordinary pointers, provided that the range is the same
+size as the array. It is not allowed for the source range to include any of the
+elements currently in the array. The source elements can be
+of a type T2 that may be the same or different than this array's element type
+T as long as there is a T=T2 assignment operator that works. Note that although
+the source arguments are pointers, those may be iterators for some container
+depending on implementation details of the container. Specifically, any
+ArrayViewConst_, ArrayView_, or Array_ iterator is an ordinary pointer.
+
+ at param[in] first
+ A pointer to the first element to be copied.
+ at param[in] last1
+ A pointer to the element one past the last element to be copied.
+ at pre last1-first == size()
+ at par Complexity:
+ The T=T2 assignment operator will be called exactly size() times. **/
+template <class T2>
+void assign(const T2* first, const T2* last1) {
+ const char* methodName = "ArrayView_<T>::assign(T2* first, T2* last1)";
+ SimTK_ERRCHK((first&&last1)||(first==last1), methodName,
+ "One of the source pointers was null (0); either both must be"
+ " non-null or both must be null.");
+ // Valid pointers are random access iterators.
+ avAssignIteratorDispatch(first, last1, std::random_access_iterator_tag(),
+ methodName);
+}
+
+/** Assign to this array to make it a copy of the elements in range
+[first,last1) given by non-pointer iterators (the pointer case is handled
+with a specialized assign() variant). It is not allowed for this range to
+include any of the elements currently in the array. The source elements can be
+of a type T2 that may be the same or different than this array's element type
+T as long as there is a T=T2 operator that works.
+
+The source must have the same number of elements as the current (fixed) size
+of this ArrayView. For input_iterators we'll be happy if we get enough elements
+and won't insist that the input stream is empty after that. For forward_ and
+bidirectional_iterators we'll copy the elements and complain at the end if
+there are too few or too many. For random_access_iterators we'll check in
+advance since we can do that fast.
+
+ at param[in] first
+ An iterator pointing to the first element to be copied.
+ at param[in] last1
+ An iterator pointing to the element one past the last element to be copied.
+
+ at remarks
+This variant of assign() will not be called when the iterators are forward
+iterators from ArrayViewConst_, ArrayView_, or Array_ objects since those are
+ordinary pointers.
+
+ at pre last1 is reachable from first
+ at pre distance(first,last1)==size()
+ at par Complexity:
+ The T=T2 assignment operator will be called exactly size() times. **/
+
+// Watch out for integral types matching this signature -- they must be
+// forwarded to the assign(n, fillValue) signature instead.
+template <class Iter>
+void assign(const Iter& first, const Iter& last1)
+{ avAssignDispatch(first,last1,typename IsIntegralType<Iter>::Result(),
+ "ArrayView_<T>::assign(Iter first, Iter last1)"); }
+/*@} End of assignment. */
+
+
+//------------------------------------------------------------------------------
+/** @name Element access
+
+These methods provide read and write access to individual elements that are
+currently present in the array; the ArrayViewConst_<T,X> base class provides the
+read-only (const) methods. **/
+/*@{*/
+
+/** Select an element by its index, returning a const reference. Note that only
+a value of the array's templatized index type is allowed (default is unsigned).
+This will be range-checked in a Debug build but not in Release.
+ at pre 0 <= \a i < size()
+ at par Complexity:
+ Constant time. **/
+const T& operator[](index_type i) const {return this->CBase::operator[](i);}
+
+/** Select an element by its index, returning a writable (lvalue) reference.
+Note that only a value of the Array's templatized index type is allowed
+(default is unsigned). This will be range-checked in a Debug build but not
+in Release.
+ at pre 0 <= \a i < size()
+ at par Complexity:
+ Constant time. **/
+T& operator[](index_type i) {return const_cast<T&>(this->CBase::operator[](i));}
+
+/** Same as operator[] but always range-checked, even in a Release build.
+ at pre 0 <= \a i < size()
+ at par Complexity:
+ Constant time. **/
+const T& at(index_type i) const {return this->CBase::at(i);}
+
+/** Same as operator[] but always range-checked, even in a Release build.
+ at pre 0 <= \a i < size()
+ at par Complexity:
+ Constant time. **/
+T& at(index_type i) {return const_cast<T&>(this->CBase::at(i));}
+
+/** Same as the const form of operator[]; exists to provide a non-operator
+method for element access in case that's needed. **/
+const T& getElt(index_type i) const {return this->CBase::getElt(i);}
+/** Same as the non-const form of operator[]; exists to provide a non-operator
+method for element access in case that's needed. **/
+T& updElt(index_type i) {return const_cast<T&>(this->CBase::getElt(i));}
+
+/** Return a const reference to the first element in this array, which must
+not be empty.
+ at pre The array is not empty.
+ at par Complexity:
+ Constant time. **/
+const T& front() const {return this->CBase::front();}
+
+/** Return a writable reference to the first element in this array, which must
+not be empty.
+ at pre The array is not empty.
+ at par Complexity:
+ Constant time. **/
+T& front() {return const_cast<T&>(this->CBase::front());}
+
+/** Return a const reference to the last element in this array, which must
+not be empty.
+ at pre The array is not empty.
+ at par Complexity:
+ Constant time. **/
+const T& back() const {return this->CBase::back();}
+
+/** Return a writable reference to the last element in this array, which must
+not be empty.
+ at pre The array is not empty.
+ at par Complexity:
+ Constant time. **/
+T& back() {return const_cast<T&>(this->CBase::back());}
+
+/** Select a contiguous subarray of the elements of this array and create
+another ArrayView_ that refers only to those element (without copying).
+ at param[in] index
+ The index of the first element to be included in the subarray; this can
+ be one past the end of the array if \a length is zero.
+ at param[in] length
+ The length of the subarray to be produced.
+ at return
+ A new ArrayView_<T,X> object referencing the original data.
+ at note
+ If \a length==0 the returned array will be in a default-constructed,
+ all-zero and null state with no connection to the original data.
+ at pre \a index >= 0, \a length >= 0
+ at pre \a index + \a length <= size()
+ at pre We'll validate preconditions in Debug builds but not Release.
+ at par Complexity:
+ Dirt cheap; no element construction or destruction or heap allocation
+ is required. **/
+ArrayView_ operator()(index_type index, size_type length) {
+ const size_type ix(index);
+ SimTK_ERRCHK2(isSizeInRange(ix, size()), "ArrayView_<T>(index,length)",
+ "For this operator, we must have 0 <= index <= size(), but"
+ " index==%llu and size==%llu.", this->ull(ix), ullSize());
+ SimTK_ERRCHK2(isSizeInRange(length, size_type(size()-ix)),
+ "ArrayView_<T>(index,length)",
+ "This operator requires 0 <= length <= size()-index, but"
+ " length==%llu and size()-index==%llu.",this->ull(length),this->ull(size()-ix));
+
+ return ArrayView_(data()+ix, data()+ix+length);
+}
+/** Same as non-const operator()(index,length); exists to provide non-operator
+access to that functionality in case it is needed. **/
+ArrayView_ updSubArray(index_type index, size_type length)
+{ return (*this)(index,length); }
+/*@} End of element access. **/
+
+
+//------------------------------------------------------------------------------
+/** @name Iterators
+
+These methods deal in iterators, which are STL generalized pointers. For this
+class, iterators are just ordinary pointers to T, and you may depend on that.
+By necessity, reverse iterators can't be just pointers; however, they contain
+an ordinary iterator (i.e. a pointer) that can be obtained by calling the
+reverse iterator's base() method. **/
+/*@{*/
+
+/** Return a const pointer to the first element of this array if any, otherwise
+end(), which may be null (0) in that case but does not have to be. This method
+is from the proposed C++0x standard; there is also an overloaded begin() from
+the original standard that returns a const pointer. **/
+const T* cbegin() const {return this->CBase::cbegin();}
+/** The const version of begin() is the same as cbegin(). **/
+const T* begin() const {return this->CBase::cbegin();}
+/** Return a writable pointer to the first element of this array if any,
+otherwise end(). If the array is empty, this \e may return null (0) but does
+not have to -- the only thing you can be sure of is that begin() == end() for
+an empty array. **/
+T* begin() {return const_cast<T*>(this->CBase::cbegin());}
+
+/** Return a const pointer to what would be the element just after the last one
+in the array; this may be null (0) if there are no elements but doesn't have to
+be. This method is from the proposed C++0x standard; there is also an
+overloaded end() from the original standard that returns a const pointer. **/
+const T* cend() const {return this->CBase::cend();}
+/** The const version of end() is the same as cend(). **/
+const T* end() const {return this->CBase::cend();}
+/** Return a writable pointer to what would be the element just after the last
+one in this array. If the array is empty, this \e may return null (0) but does
+not have to -- the only thing you can be sure of is that begin()==end() for an
+empty array. **/
+T* end() {return const_cast<T*>(this->CBase::cend());}
+
+/** Return a const reverse iterator pointing to the last element in the array
+or crend() if the array is empty. **/
+const_reverse_iterator crbegin() const {return this->CBase::crbegin();}
+/** The const version of rbegin() is the same as crbegin(). **/
+const_reverse_iterator rbegin() const {return this->CBase::crbegin();}
+/** Return a writable reverse iterator pointing to the last element in the
+array or rend() if the array is empty. **/
+reverse_iterator rbegin() {return reverse_iterator(end());}
+
+/** Return the past-the-end reverse iterator that tests equal to a reverse
+iterator that has been incremented past the front of the array. You cannot
+dereference this iterator. **/
+const_reverse_iterator crend() const {return this->CBase::crend();}
+/** The const version of rend() is the same as crend(). **/
+const_reverse_iterator rend() const {return this->CBase::crend();}
+/** Return a writable past-the-end reverse iterator that tests equal to a
+reverse iterator that has been incremented past the front of the array. You
+cannot dereference this iterator. **/
+reverse_iterator rend() {return reverse_iterator(begin());}
+
+/** Return a const pointer to the first element of the array, or possibly
+(but not necessarily) null (0) if the array is empty.
+ at note
+ cdata() does not appear to be in the C++0x standard although it would seem
+ obvious in view of the cbegin() and cend() methods that had to be added.
+ The C++0x overloaded const data() method is also available. **/
+const T* cdata() const {return this->CBase::cdata();}
+/** The const version of the data() method is identical to cdata().
+ at note This method is from the proposed C++0x std::vector. **/
+const T* data() const {return this->CBase::cdata();}
+/** Return a writable pointer to the first allocated element of the array, or
+a null pointer if no space is associated with the array.
+ at note This method is from the proposed C++0x std::vector. **/
+T* data() {return const_cast<T*>(this->CBase::cdata());}
+/*@} End of iterators. */
+
+
+//------------------------------------------------------------------------------
+/** @name Size and capacity
+
+These methods report the number of elements (size) or the amount of allocated
+heap space (capacity) or both but cannot be used to change size. **/
+/*@{*/
+
+// Note: these have to be explicitly forwarded to the base class methods
+// in order to keep gcc from complaining. Note that the "this->" is
+// apparently necessary in order to permit delayed definition of templatized
+// methods. Doxygen picks up the comments from the base class.
+
+size_type size() const {return this->CBase::size();}
+size_type max_size() const {return this->CBase::max_size();}
+bool empty() const {return this->CBase::empty();}
+size_type capacity() const {return this->CBase::capacity();}
+size_type allocated() const {return this->CBase::allocated();}
+bool isOwner() const {return this->CBase::isOwner();}
+/*@} End of size and capacity. **/
+
+
+//------------------------------------------------------------------------------
+ private:
+//------------------------------------------------------------------------------
+// no data members are allowed
+
+//------------------------------------------------------------------------------
+// ARRAY VIEW ASSIGN DISPATCH
+// This is the assign() implementation for ArrayView_ when the class that
+// matched the alleged InputIterator template argument turned out to be one of
+// the integral types in which case this should match the assign(n, fillValue)
+// signature.
+template <class IntegralType>
+void avAssignDispatch(IntegralType n, IntegralType v, TrueType isIntegralType,
+ const char*)
+{ assign(size_type(n), value_type(v)); }
+
+// This is the assign() implementation for ArrayView_ when the class that
+// matched the alleged InputIterator template argument is NOT an integral type
+// and may very well be an iterator.
+template <class InputIterator>
+void avAssignDispatch(const InputIterator& first, const InputIterator& last1,
+ FalseType isIntegralType, const char* methodName)
+{ avAssignIteratorDispatch(first, last1,
+ typename std::iterator_traits<InputIterator>::iterator_category(),
+ methodName); }
+
+// This is the assign() implementation for a plain input_iterator
+// (i.e., not a forward, bidirectional, or random access iterator). These
+// have the unfortunate property that we can't count the elements in advance.
+// Here we're going to complain if there aren't enough; but will simply stop
+// when we get size() elements and not insist that the input stream reached
+// the supplied last1 iterator. Semantics is elementwise assignment.
+template <class InputIterator>
+void avAssignIteratorDispatch(const InputIterator& first,
+ const InputIterator& last1,
+ std::input_iterator_tag,
+ const char* methodName)
+{
+ T* p = begin();
+ InputIterator src = first;
+ while (src != last1 && p != end())
+ *p++ = *src++; // call T's assignment operator
+
+ // p now points just after the last element that was copied.
+ const size_type nCopied = size_type(p - begin());
+ SimTK_ERRCHK2_ALWAYS(nCopied == size(), methodName,
+ "The supplied input_iterator provided only %llu elements but this"
+ " ArrayView has a fixed size of %llu elements.",
+ this->ull(nCopied), ullSize());
+
+ // We don't care if there are still more input elements available.
+}
+
+// This is the assign() implementation that works for forward and bidirectional
+// iterators, but is not used for random_access_iterators. Here we'll count
+// the elements as we copy them and complain at the end if there were too
+// few or too many.
+template <class ForwardIterator>
+void avAssignIteratorDispatch(const ForwardIterator& first,
+ const ForwardIterator& last1,
+ std::forward_iterator_tag,
+ const char* methodName)
+{
+ T* p = begin();
+ ForwardIterator src = first;
+ while (src != last1 && p != end())
+ *p++ = *src++; // call T's assignment operator
+
+ // p now points just after the last element that was copied.
+ const size_type nCopied = size_type(p - begin());
+ SimTK_ERRCHK2_ALWAYS(nCopied == size(), methodName,
+ "The supplied forward_ or bidirectional_iterator source range provided"
+ " only %llu elements but this ArrayView has a fixed size of"
+ " %llu elements.", this->ull(nCopied), ullSize());
+
+ // Make sure we ran out of source elements.
+ SimTK_ERRCHK1_ALWAYS(src == last1, methodName,
+ "The supplied forward_ or bidirectional_iterator source range"
+ " contained too many elements; this ArrayView has a fixed size of"
+ " %llu elements.", ullSize());
+}
+
+// This is the assign() implementation that works for random_access_iterators
+// including ordinary pointers. Here we check the number of elements in advance
+// and complain if the source and destination aren't the same size. The
+// copying loop can be done faster in this case.
+template <class RandomAccessIterator>
+void avAssignIteratorDispatch(const RandomAccessIterator& first,
+ const RandomAccessIterator& last1,
+ std::random_access_iterator_tag,
+ const char* methodName)
+{
+ SimTK_ERRCHK2_ALWAYS(this->isSameSize(last1-first), methodName,
+ "Assignment to an ArrayView is permitted only if the source"
+ " is the same size. Here the source had %llu element(s) but the"
+ " ArrayView has a fixed size of %llu.",
+ this->ull(last1-first), this->ull(size()));
+
+ SimTK_ERRCHK_ALWAYS(!this->overlapsWithData(first,last1), methodName,
+ "Source range can't overlap with the destination data.");
+
+ T* p = begin();
+ RandomAccessIterator src = first;
+ while (p != end())
+ *p++ = *src++; // call T's assignment operator
+}
+
+
+//------------------------------------------------------------------------------
+// The following private methods are protected methods in the ArrayViewConst_
+// base class, so they should not need repeating here. However, we explicitly
+// forward to the base methods to avoid gcc errors. The gcc complaint
+// is due to their not depending on any template parameters; the "this->"
+// apparently fixes that problem.
+
+packed_size_type psize() const {return this->CBase::psize();}
+packed_size_type pallocated() const {return this->CBase::pallocated();}
+
+// This just cast sizes to unsigned long long so that we can do comparisons
+// without getting warnings.
+unsigned long long ullSize() const {return this->CBase::ullSize();}
+unsigned long long ullCapacity() const {return this->CBase::ullCapacity();}
+unsigned long long ullMaxSize() const {return this->CBase::ullMaxSize();}
+// This is the index type name and is handy for error messages to explain
+// why some size was too big.
+const char* indexName() const {return this->CBase::indexName();}
+};
+
+
+
+
+
+//==============================================================================
+// CLASS Array_
+//==============================================================================
+/** The SimTK::Array_<T> container class is a plug-compatible replacement for
+the C++ standard template library (STL) std::vector<T> class, but with some
+important advantages in performance, and functionality, and binary
+compatibility.
+
+ at tparam T
+ The type of object to be stored in this container.
+ at tparam X
+ The type to be used for indexing this container, with default unsigned
+ (not size_t). Any integral type may be used, as well as user types that
+ satisfy the requirements discussed with class ArrayIndexTraits.
+
+ at par Performance:
+There are several performance and memory footprint problems with the C++
+standard STL design in general, and with Microsoft's implementation in
+particular, that are addressed here. Microsoft in its wisdom decided that STL
+containers should still do runtime range checks in Release builds for safety,
+but that makes them too slow for use in some high-performance contexts (and
+also breaks the promise of generic programming but that's another rant). In
+practice, VC++9 std::vector runs about half speed for simple operations like
+indexing and push_back. Attempting to disable these runtime checks with
+_SECURE_SCL breaks binary compatibility. In contrast the performance of this
+Array_<T> class on any platform is indistinguishable from what you would get
+by managing your own heap-allocated arrays.
+
+ at par
+Regarding memory footprint, the typical implementation of std::vector uses
+three pointers: 12 bytes for 32 bit machines; 24 bytes for 64 bit machines.
+Microsoft somehow manages to trump this with 20 to 24 bytes on a 32 bit
+machine -- I don't know what they do on a 64 bit machine but I'm not
+optimistic! Array_ instead uses one pointer and two lengths for a total size
+as little as 8 bytes on 32 bits and 16 on 64 bits; see below for details.
+
+ at par
+Some nuts and bolts:
+
+- We promise that no heap allocation occurs when an empty Array_<T> object
+ is declared (that is, when an Array_<T> is default-constructed); in
+ that case both begin() and end() are null.
+- Array_<T> methods are extremely fast in Release builds with zero overhead,
+ inline, unchecked methods. The implementations of inline methods are kept
+ small to ensure that they are actually inlined in practice; and generated
+ assembly code was examined to make sure.
+- There are some dangerous extensions provided that permit the expert user
+ to construct objects directly into the array without having to copy them,
+ a big win for complicated objects and even bigger for those that don't
+ have copy constructors!
+- There is a constant-time eraseFast() method you can use if you don't mind the
+ array being reordered after the erase. This avoids the extremely expensive
+ "compress" activity required by the standard erase() method.
+- The optional index-type template parameter can be used to reduce the memory
+ footprint to as little as 8 bytes on a 32 bit machine (e.g., a 32 bit
+ pointer and two shorts).
+- The default size_type for an Array_<T> is a 32-bit unsigned integer rather
+ than a size_t. On a 64-bit machine that keeps the overhead down substantially
+ since the structure is then one 64-bit pointer and two 32-bit integers,
+ fitting tightly into a cleanly alignable 16 bytes.
+
+
+ at par Functionality:
+For the most part Array_<T> is a plug-compatible replacement for std::vector<T>,
+and everything that both classes can do is done with an identical API. However,
+there are a few additions and subtractions:
+
+- This class always uses the default new/delete allocator; there is no option
+ to specify your own as there is in std::vector.
+- Instead of an allocator, the second template argument X to Array_<T,X> is an
+ optional index type which can be used to provide type-safe indexing (i.e. the
+ array can only be indexed by indices of a particular type, like
+ MobilizedBodyIndex). This has zero performance cost if the index is an
+ integral type or class consisting of only an integral value such as those
+ produced by the SimTK_DEFINE_UNIQUE_INDEX_TYPE macro.
+- You can create uninitialized slots in the array and construct directly into
+ them rather than having to construct a temporary object which must then be
+ copied into the array.
+- You can create Array_<T> objects that reference existing data, including
+ the contents of std::vectors.
+- Where possible this class implements the new std::vector features proposed
+ for the C++0x standard (see below).
+
+ at par Compatibility:
+Included here are binary compatibility issues and compatibility with the C++
+standard STL objects.
+
+- Most important, it is safe to pass an Array_<T> through an API to a binary
+ library without worrying about compiler version or Release/Debug compatibility
+ issues. For a given compiler (e.g. gcc or Microsoft cl) and word size (64 bit
+ vs. 32 bit), Array_<T> has an extremely stable memory layout that is preserved
+ across compiler versions, and between Release and Debug builds. This allows us
+ to use Array_<T> in the SimTK API where use of std::vector<T> would be
+ desirable but problematic.
+- It supports all standard types, methods, iterators, and operators of the
+ C++98 standard std::vector and the C++0x proposed improvements other than
+ those requiring rvalue references, so it works smoothly with all STL
+ containers and algorithms.
+- It is convertible to and from std::vector, usually without copying the
+ elements. It is easy to provide APIs that accept either Array_<T> or
+ std::vector<T>; the std::vector's data is referenced by an Array_ handle
+ that is used to convey the data across the API without binary compatibility
+ problems.
+
+ at see Array_, ArrayViewConst_, ArrayIndexTraits **/
+template <class T, class X> class Array_ : public ArrayView_<T,X> {
+ typedef ArrayView_<T,X> Base;
+ typedef ArrayViewConst_<T,X> CBase;
+public:
+
+
+//------------------------------------------------------------------------------
+/** @name Typedefs
+
+Types required of STL containers, plus index_type which is an extension, and
+packed_size_type which is an implementation detail. **/
+
+// Doxygen picks up individual descriptions from the base class.
+/*{*/
+typedef T value_type;
+typedef X index_type;
+typedef T* pointer;
+typedef const T* const_pointer;
+typedef T& reference;
+typedef const T& const_reference;
+typedef T* iterator;
+typedef const T* const_iterator;
+typedef std::reverse_iterator<iterator> reverse_iterator;
+typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
+typedef typename ArrayIndexTraits<X>::size_type size_type;
+typedef typename ArrayIndexTraits<X>::difference_type difference_type;
+typedef typename ArrayIndexPackType<size_type>::packed_size_type
+ packed_size_type;
+/*}*/
+
+//------------------------------------------------------------------------------
+/** @name Construction, conversion and destruction
+
+A variety of constructors are provided for this class, including all those
+required by the C++ standard for std::vector implementations, plus additional
+ones providing smooth conversions between Array_<T> and std::vector<T> objects.
+**/
+/*{*/
+
+/** Default constructor allocates no heap space and is very fast. **/
+Array_() : Base() {}
+
+/** Construct an array containing \a n default-constructed elements. T's default
+constructor (if any) is called exactly \a n times. If \a n is zero no heap space
+will be allocated; although in that case it is preferable to use the default
+constructor if you can since that will be somewhat faster. **/
+explicit Array_(size_type n) : Base() {
+ SimTK_SIZECHECK(n, max_size(), "Array_<T>::ctor(n)");
+ allocateNoConstruct(n);
+ defaultConstruct(data(), data()+n);
+ setSize(n);
+}
+
+/** Construct an array containing \a n elements each set to a copy of the given
+initial value. T's copy constructor will be called exactly \a n times. If \a n
+is zero no space will be allocated. **/
+Array_(size_type n, const T& initVal) : Base() {
+ SimTK_SIZECHECK(n, max_size(), "Array_<T>::ctor(n,T)");
+ setSize(n);
+ allocateNoConstruct(size());
+ fillConstruct(begin(), cend(), initVal);
+}
+/** Construct an Array_<T> from a range [first,last1) of values identified by a
+pair of iterators.
+ at note
+The standard requires that if an integral type matches this signature, it must
+behave as the Array_(size_type,value_type) constructor.
+ at par Complexity:
+The performance of this constructor depends on the type
+of iterator:
+- random_access_iterator: n=(last1-first); a single space allocation;
+ n calls to T's copy constructor.
+- forward or bidirectional iterator: must increment from first to last1 to
+ determine n; otherwise same as random access.
+- input iterator: can't determine n in advance; expect log n reallocations
+ during construction as we "push back" one input element at a time.
+**/
+template <class InputIter>
+Array_(const InputIter& first, const InputIter& last1) : Base() {
+ ctorDispatch(first,last1,typename IsIntegralType<InputIter>::Result());
+}
+
+/** Construct an Array_<T> from a range [first,last1) of values identified by a
+pair of ordinary pointers to elements of type T2 (where T2 might be the same as
+T but doesn't have to be). This is templatized so can be used with any source
+type T2 for which there is a working conversion constructor T(T2), provided
+that the number of source elements does not exceed the array's max_size(). **/
+template <class T2>
+Array_(const T2* first, const T2* last1) : Base() {
+ SimTK_ERRCHK((first&&last1)||(first==last1), "Array_<T>(first,last1)",
+ "Pointers must be non-null unless they are both null.");
+ SimTK_ERRCHK3(this->isSizeOK(last1-first), "Array_<T>(first,last1)",
+ "Source has %llu elements but this array is limited to %llu"
+ " elements by its index type %s.",
+ this->ull(last1-first), ullMaxSize(), indexName());
+
+ setSize(size_type(last1-first));
+ allocateNoConstruct(size());
+ copyConstruct(begin(), cend(), first);
+}
+
+/** Construct an Array_<T> by copying from an std::vector<T2>, where T2 may
+be the same type as T but doesn't have to be. This will work as long as the
+size of the vector does not exceed the array's max_size(), and provided there
+is a working T(T2) conversion constructor. **/
+template <class T2>
+explicit Array_(const std::vector<T2>& v) : Base() {
+ if (v.empty()) return;
+
+ SimTK_ERRCHK3(this->isSizeOK(v.size()), "Array_<T>::ctor(std::vector<T2>)",
+ "The source std::vector's size %llu is too big for this array which"
+ " is limited to %llu elements by its index type %s.",
+ this->ull(v.size()), ullMaxSize(), indexName());
+
+ // Call the above constructor, making sure to use pointers into the
+ // vector's data rather than the iterators begin() and end() in case
+ // they are different types.
+ new (this) Array_(&v.front(), (&v.back())+1);
+}
+
+/** Copy constructor allocates exactly as much memory as is in use in the
+source (not its capacity) and copy constructs the elements so that T's copy
+constructor will be called exactly src.size() times. If the source is empty,
+no heap space will be allocated. **/
+Array_(const Array_& src) : Base() {
+ setSize(src.size());
+ allocateNoConstruct(size());
+ copyConstruct(begin(), cend(), src.data());
+}
+
+/** Construct this Array_<T,X> as a copy of another Array_<T2,X2> where T2!=T
+or X2!=X. This will work as long as the source is not larger than will fit
+here, and as long as the source element type T2 is assignment compatible with
+this array's element type T. One of T's constructors will be called exactly
+src.size() times; the particular constructor is whichever one best matches
+T(T2). **/
+template <class T2, class X2>
+Array_(const Array_<T2,X2>& src) : Base() {
+ new (this) Array_(src.begin(), src.cend()); // see above
+}
+
+/** Construct an Array_<T> by referencing (sharing) a given range of data
+[first,last1), without copying that data; better to use the corresponding
+ArrayView_<T> constructor if you can. This is very fast but can be
+dangerous -- it is most useful for argument passing where the array handle
+will be discarded immediately after use. Note that this is available only if
+you have write access to the data because there is no way to construct
+a non-writable array. This will work as long as the size of the data does
+not exceed the array's max_size. The resulting array object is not resizeable
+but can be used to read and write elements of the original data. The
+array is invalid if the original data is destructed or resized, but there is
+no way for the array class to detect that.
+
+ at remarks
+ - If the source data is empty, the resulting array will also
+ be empty and will look just like a default-constructed array. It will
+ therefore not have any connection to the source and will be an
+ ordinary resizable array.
+ - This is quite dangerous to use since the connection between the array and
+ the data is tenuous and subject to the data remaining untouched during
+ the lifetime of the array handle. There is no reference counting;
+ destructing the original data would leave the array referring to garbage.
+ Be careful!
+ - You can break the connection between the array and the data it was
+ constructed from by calling deallocate().
+
+ at par Complexity:
+ Dirt cheap. There will be no construction, destruction, or heap allocation
+ performed.
+ at see deallocate() **/
+Array_(T* first, const T* last1, const DontCopy&) : Base(first,last1) {}
+
+/** Construct an Array_<T> by referencing (sharing) the data in an
+std::vector<T>, without copying the data; better to use the ArrayView_<T>
+constructor instead if you can. This is very fast but can be
+dangerous -- it is most useful for argument passing where the array handle
+will be discarded immediately after use. Note that this is available only if
+you have write access to the std::vector because there is no way to construct
+a non-writable array. This will work as long as the size of the vector does
+not exceed the array's max_size. The resulting array object is not resizeable
+but can be used to read and write elements of the original std::vector. The
+array is invalid if the original std::vector is destructed or resized.
+
+ at remarks
+ - If the source std::vector is empty, the resulting array will also
+ be empty and will look just like a default-constructed array. It will
+ therefore not have any connection to the source vector and will be an
+ ordinary resizable array.
+ - This is quite dangerous to use since the connection between the array and
+ the vector is tenuous and subject to the vector remaining untouchged during
+ the lifetime of the array handle. There is no reference counting;
+ destructing the vector leaves the array referring to garbage. Be careful!
+ - You can break the connection between the array and the vector it was
+ constructed from by calling deallocate().
+
+ at par Complexity:
+ Dirt cheap. There will be no construction, destruction, or heap allocation
+ performed.
+ at see deallocate() **/
+template <class A>
+Array_(std::vector<T,A>& v, const DontCopy&) : Base(v) {}
+
+/** The destructor performs a deallocate() operation which may result in
+element destruction and freeing of heap space; see deallocate() for more
+information. @see deallocate() **/
+~Array_() {
+ deallocate();
+}
+
+/** Empty this array of its contents, returning the array to its
+default-constructed, all-zero state. If this array is the owner of its data,
+the destructor (if any) is called for each data element and the array's
+allocated heap space is freed. If it is a non-owner the array handle is
+cleaned out using disconnect() but the referenced data is untouched.
+ at note There is no equivalent to this method for std::vector.
+ at return A reference to the now-empty, default-constructed array, ready for
+reassignment. **/
+Array_& deallocate() {
+ if (allocated()) { // owner with non-zero allocation
+ clear(); // each element is destructed; size()=0; allocated() unchanged
+ deallocateNoDestruct(); // free data(); allocated()=0
+ }
+ this->Base::disconnect(); // clear the handle
+ return *this;
+}
+
+/*@} End of constructors, etc. **/
+
+
+//------------------------------------------------------------------------------
+/** @name Assignment methods and operators
+
+These methods put new data values in an existing array, but the meaning of
+assignment is subtly different for resizeable (owner) arrays and fixed
+(non-owner) arrays. The standard std::vector type is always an owner so the
+non-owner description here is an extension applying only to Array_.
+
+For the normal case of resizeable arrays, assignment does not have an
+elementwise definition because the source will typically have a different
+number of elements than the array's current size. So regardless of the actual
+numbers, assignment in the resizeable case is defined as it is for std::vector:
+first clear the array by erasing (destructing) all the current elements in the
+array, then reserve sufficient heap space to hold a copy of the source, then
+use appropriate constructors of type T (most commonly T's copy constructor
+T(T)) to initialize each element to be a copy of the corresponding source
+element. T's assignment operators are never used in this case.
+
+For fixed arrays, the source must have the same number of elments as are
+currently in the array and the meaning is conventional elementwise assignment;
+that is, an appropriate assignment operator of type T (most commonly T's copy
+assignment operator T=T) is used to change the value of each existing element.
+
+So there are different requirements on the value type T for owner and non-owner
+assignments to type T2: for owner assignment T must have a constructor T(T2)
+available; for non-owner assignment, T must have an assignment operator T=T2
+available; .
+
+ at remarks
+- When reallocating the destination array, we may reuse the existing heap
+allocation if it is sufficient and not \e too big; otherwise we'll reallocate
+before copying.
+- The fill() method here has elementwise assignment semantics regardless of
+whether the array is an owner or non-owner. **/
+/*@{*/
+
+/** Set this array to be \a n copies of the supplied \a fillValue. Note that
+this serves to allow fill from an object whose type T2 is different from T, as
+long as there is a constructor T(T2) that works since that can be invoked
+(implicitly or explicitly) to convert the T2 object to type T prior to the
+call. If this is a non-owner array then \a n must be the same as the current
+size(); consider using the fill() method instead.
+ at param[in] n The number of elements to be in the result.
+ at param[in] fillValue The value to which to initialize each element.
+
+ at pre \a n <= max_size()
+ at pre for non-owner, n==size()
+ at par Complexity:
+For a non-owner with \a n==size(), there will be exactly \a n calls to T's
+copy assignment operator. For an owner, there will be size() calls to T's
+destructor (if it has one), possibly a heap reallocation (but with no element
+copying), followed by \a n calls to T's copy constructor.
+ at see fill() **/
+void assign(size_type n, const T& fillValue) {
+ SimTK_ERRCHK3(this->isSizeOK(n), "Array_<T>::assign(n,value)",
+ "Requested size %llu is too big for this array which is limited"
+ " to %llu elements by its index type %s.",
+ this->ull(n), ullMaxSize(), indexName());
+
+ SimTK_ERRCHK2(isOwner() || n==size(), "Array_<T>::assign(n,value)",
+ "Requested size %llu is not allowed because this is a non-owner"
+ " array of fixed size %llu.", this->ull(n), this->ull(size()));
+
+ if (!isOwner())
+ this->Base::fill(fillValue);
+ else {
+ clear(); // all elements destructed; allocation unchanged
+ reallocateIfAdvisable(n); // change size if too small or too big
+ fillConstruct(data(), cdata()+n, fillValue);
+ setSize(n);
+ }
+}
+
+/** Assign all current elements of the array to the same \a fillValue. This is
+similar to assign(size(),fillValue) but the semantics are subtly different.
+Here we use repeated application of T's copy assignment operator T=fillValue,
+whereas the assign() semantics are to first destruct all the existing elements,
+then allocate if necessary, then use the copy constructor to initialize the
+new elements. Note that you can use this to fill from a source type T2 that
+is different from T as long as there exists a suitable constructor T(T2) that
+can be used to create the type T \a fillValue from the original T2 source.
+ at note Unlike other assignment methods, the behavior of fill() is identical for
+owner and non-owner arrays.
+
+ at param[in] fillValue The value to which all existing elements are set.
+ at par Complexity:
+Just size() calls to T's copy assignment operator. **/
+void fill(const T& fillValue) {this->Base::fill(fillValue);}
+
+
+/** Assign to this array to to make it a copy of the elements in range
+[first,last1) given by ordinary pointers. It is not allowed for this range to
+include any of the elements currently in the array. The source elements can be
+of a type T2 that may be the same or different than this array's element type
+T as long as there is a working constructor T(T2) (for owner arrays) or a
+working assignment operator T=T2 (for non-owner arrays). Note that although the
+source arguments are pointers, those may be iterators for some container
+depending on implementation details of the container. Specifically, any
+Array_<T2>::iterator or const_iterator is an ordinary pointer.
+
+ at param[in] first A pointer to the first source element to be copied.
+ at param[in] last1 A pointer to one element past the last source element.
+
+ at par Complexity:
+For non-owner arrays, n=last1-first must equal the current size() in which
+case there will be exactly size() calls to the T=T2 assignment operator.
+For owner arrays, say the array initially has capacity c, and the
+source provides n new elements. If type T has a destructor, it will be called
+exactly size() times. Reallocation will then occur if c < n and may occur if
+c >> n to avoid leaving a lot of unused space. Then the constructor T(T2) will
+be called exactly n times. **/
+template <class T2>
+void assign(const T2* first, const T2* last1) {
+ const char* methodName = "Array_<T>::assign(T2* first, T2* last1)";
+ SimTK_ERRCHK((first&&last1)||(first==last1), methodName,
+ "Pointers must be non-null unless they are both null.");
+ SimTK_ERRCHK(!this->overlapsWithData(first,last1), methodName,
+ "Source range can't overlap the current array contents.");
+ // Pointers are random access iterators.
+ assignIteratorDispatch(first,last1,std::random_access_iterator_tag(),
+ methodName);
+}
+
+
+/** Assign this array from a range [first,last1) given by non-pointer
+iterators. See the assign(first,last1) method with pointer arguments for a
+relevant discussion.
+
+ at remarks
+ - For a non-owner array this is only allowed if we can calculate the number of
+ source elements, and if that number is exactly the same as the current
+ size().
+ - See Complexity discussion below for behavior for the different kinds of
+ iterators that might be supplied.
+ - It is not permitted for any of the source elements to overlap in memory
+ with the initial contents of the array.
+
+ at param[in] first
+ An iterator pointing to the first source element to be copied.
+ at param[in] last1
+ A iterator pointing one element past the last source element.
+
+ at pre last1-first <= max_size()
+ at pre for non-owner array, last1-first == size()
+ at par Complexity:
+For a non-owner array, this is only allowed if n=last1-first equals the
+current size(), in which case we'll perform exactly n calls to the appropriate
+assignment operator of element type T. For owner arrays, if we can determine
+how many elements n=last1-first the source contains in advance, we'll do only
+a single allocation here and call one of T's constructors exactly n times after
+just size() destructor calls needed to erase the original data. If the
+iterators are random access iterators, calculating n is a fast constant-time
+operation. For forward or bidirectional iterators, we have to advance through
+the iterators once to count the source elements prior to allocating space,
+adding an O(n) cost. For input iterators, we can't count them in advance so
+we just have to add elements as we find them using push_back() meaning we may
+need to reallocate log(n) times, calling the destructor and copy constructor
+each time to move the elements around.
+ at see assign(T2* first, T2* last1) **/
+template <class Iter>
+void assign(const Iter& first, const Iter& last1) {
+ assignDispatch(first,last1,typename IsIntegralType<Iter>::Result(),
+ "Array_<T>::assign(Iter first, Iter last1)");
+}
+
+/** Copy assignment operator destructs the current contents of this array and
+then makes it a copy of the source array by repeated calls to the element
+type's copy constructor. At most one reallocation of heap space occurs that
+may result in this array having a larger or smaller capacity, although of
+course it will be at least as large as the source. **/
+Array_& operator=(const Array_& src) {
+ if (this != &src)
+ assignIteratorDispatch(src.begin(), src.end(),
+ std::random_access_iterator_tag(),
+ "Array_<T>::operator=(Array_<T>)");
+ return *this;
+}
+
+/** This is assignment from a source array whose element type T2 and/or index
+type X2 are different from this array's T and X. This will work as long as
+this array can accommodate all the elements in the source and T2 is assignment
+compatible with T. See discussion for the copy assignment operator for more
+information. */
+template <class T2, class X2>
+Array_& operator=(const Array_<T2,X2>& src) {
+ assignIteratorDispatch(src.begin(), src.end(),
+ std::random_access_iterator_tag(),
+ "Array_<T>::operator=(Array_<T2,X2>)");
+ return *this;
+}
+
+
+/** This is assignment from a source std::vector<T2>. This will work as long as
+this array can accommodate all the elements in the source and T2 is assignment
+compatible with T. See discussion for the copy assignment operator for more
+information. */
+template <class T2, class A>
+Array_& operator=(const std::vector<T2,A>& src) {
+ assignIteratorDispatch(src.begin(), src.end(),
+ std::random_access_iterator_tag(),
+ "Array_<T>::operator=(std::vector)");
+ return *this;
+}
+
+/** This is a specialized algorithm providing constant time exchange of data
+with another array that has identical element and index types. This is \e much
+faster than using the std::swap() algorithm on the arrays since that would
+involve O(n) copying operations. This method makes no calls to any constructors
+or destructors. This is allowable even for non-owner arrays; the non-owner
+attribute will follow the non-owned data. **/
+void swap(Array_& other) {
+ T* const pTmp=data(); setData(other.data()); other.setData(pTmp);
+ size_type nTmp=size(); setSize(other.size()); other.setSize(nTmp);
+ nTmp=allocated(); setAllocated(other.allocated()); other.setAllocated(nTmp);
+}
+
+/** This dangerous extension allows you to supply your own already-allocated
+heap space for use by this array, which then becomes the owner of the supplied
+heap space. Any memory currently associated with the array is deallocated;
+see deallocate() for more information.
+ at see deallocate(), shareData() **/
+Array_& adoptData(T* newData, size_type dataSize,
+ size_type dataCapacity)
+{
+ SimTK_SIZECHECK(dataCapacity, max_size(), "Array_<T>::adoptData()");
+ SimTK_ERRCHK2(dataSize <= dataCapacity, "Array_<T>::adoptData()",
+ "Specified data size %llu was greater than the specified data"
+ " capacity of %llu.", this->ull(dataSize), this->ull(dataCapacity));
+ SimTK_ERRCHK(newData || dataCapacity==0, "Array_<T>::adoptData()",
+ "A null data pointer is allowed only if the size and capacity are"
+ " specified as zero.");
+ SimTK_ERRCHK(!this->overlapsWithData(newData, newData+dataSize),
+ "Array_<T>::adoptData()",
+ "The new data can't overlap with the old data.");
+
+ deallocate();
+ setData(newData);
+ setSize(dataSize);
+ setAllocated(dataCapacity);
+ return *this;
+}
+/** A variant of adoptData() that assumes the capacity is the same as the
+current size. @see adoptData(data,size,capacity) **/
+Array_& adoptData(T* newData, size_type dataSize)
+{ return adoptData(newData, dataSize, dataSize); }
+
+
+/** This dangerous extension allows you to make this array handle refer to
+someone else's data without copying it. Any memory currently associated
+with the array is deallocated; see deallocate() for more information. This
+method makes the array a fixed-size, non-owner array that cannot be
+reallocated, and no element destruction nor heap deallocation will occur when
+the handle is subsequently destructed or deallocated.
+ at note
+ - A null (0) pointer is allowed for the pointer as long as \a dataSize==0,
+ however in that case the array handle ends up deallocated (that is,
+ indistinguishable from a default-constructed array) so is resizeable.
+ - This is implemented by setting the nAllocated data member to zero while
+ the nUsed data member is set to the given \a dataSize.
+ at see deallocate(), adoptData() **/
+Array_& shareData(T* newData, size_type dataSize) {
+ SimTK_SIZECHECK(dataSize, max_size(), "Array_<T>::shareData()");
+ SimTK_ERRCHK(newData || dataSize==0, "Array_<T>::shareData()",
+ "A null data pointer is allowed only if the size is zero.");
+ SimTK_ERRCHK(!this->overlapsWithData(newData, newData+dataSize),
+ "Array_<T>::shareData()",
+ "The new data can't overlap with the old data.");
+
+ deallocate();
+ setData(newData);
+ setSize(dataSize);
+ setAllocated(0); // indicates shared data
+ return *this;
+}
+
+/** Same as shareData(data,size) but uses a pointer range [first,last1) to
+identify the data to be referenced. **/
+Array_& shareData(T* first, const T* last1) {
+ SimTK_ERRCHK3(this->isSizeOK(last1-first), "Array_<T>::shareData(first,last1)",
+ "Requested size %llu is too big for this array which is limited"
+ " to %llu elements by its index type %s.",
+ this->ull(last1-first), ullMaxSize(), indexName());
+ return shareData(first, size_type(last1-first));
+}
+
+/*@} End of assignment. **/
+
+
+//------------------------------------------------------------------------------
+/** @name Size and capacity
+
+These methods examine and alter the number of elements (size) or the amount of
+allocated heap space (capacity) or both. **/
+/*@{*/
+
+// Note: these have to be explicitly forwarded to the base class methods
+// in order to keep gcc from complaining. Note that the "this->" is
+// apparently necessary in order to permit delayed definition of templatized
+// methods.
+
+/** Return the current number of elements stored in this array. **/
+size_type size() const {return this->CBase::size();}
+/** Return the maximum allowable size for this array. **/
+size_type max_size() const {return this->CBase::max_size();}
+/** Return true if there are no elements currently stored in this array. This
+is equivalent to the tests begin() == end() or size()==0. **/
+bool empty() const {return this->CBase::empty();}
+/** Return the number of elements this array can currently hold without
+requiring reallocation. The value returned by capacity() is always greater
+than or equal to size(), even if the data is not owned by this array in
+which case we have capacity() == size() and the array is not reallocatable. **/
+size_type capacity() const {return this->CBase::capacity();}
+
+/** Change the size of this Array, preserving all the elements that will still
+fit, and default constructing any new elements that are added. This is not
+allowed for non-owner arrays unless the requested size is the same as the
+current size. **/
+void resize(size_type n) {
+ if (n == size()) return;
+
+ SimTK_ERRCHK2(isOwner(), "Array_<T>::resize(n)",
+ "Requested size change to %llu is not allowed because this is a"
+ " non-owner array of fixed size %llu.", this->ull(n), this->ull(size()));
+
+ if (n < size()) {
+ erase(data()+n, cend());
+ return;
+ }
+ // n > size()
+ reserve(n);
+ defaultConstruct(data()+size(), cdata()+n); // data() has changed
+ setSize(n);
+}
+
+/** Change the size of this array, preserving all the elements that will still
+fit, and initializing any new elements that are added by repeatedly copy-
+constructing from the supplied value. This is not allowed for non-owner arrays
+unless the requested size is the same as the current size. **/
+void resize(size_type n, const T& initVal) {
+ if (n == size()) return;
+
+ SimTK_ERRCHK2(isOwner(), "Array_<T>::resize(n,value)",
+ "Requested size change to %llu is not allowed because this is a"
+ " non-owner array of fixed size %llu.", this->ull(n), this->ull(size()));
+
+ if (n < size()) {
+ erase(data()+n, cend());
+ return;
+ }
+ // n > size()
+ reserve(n);
+ fillConstruct(data()+size(), cdata()+n, initVal);
+ setSize(n);
+}
+
+/** Ensure that this array has enough allocated capacity to hold the indicated
+number of elements. No heap reallocation will occur after this until the array
+is grown beyond this capacity, meaning that adding elements will not invalidate
+any iterators or element addresses until that point. This method will never
+reduce the capacity of the array. It is OK to call this on a non-owner array
+as long as you are not asking for an increase in capacity. **/
+void reserve(size_type n) {
+ if (capacity() >= n)
+ return;
+
+ SimTK_ERRCHK2(isOwner(), "Array_<T>::reserve()",
+ "Requested capacity change to %llu is not allowed because this is a"
+ " non-owner array of fixed size %llu.", this->ull(n), this->ull(size()));
+
+ T* newData = allocN(n); // no construction yet
+ copyConstructThenDestructSource(newData, newData+size(), data());
+ freeN(data());
+ setData(newData);
+ setAllocated(n);
+}
+
+/** Request that the capacity of this array be reduced to the minimum necessary
+to hold the number of elements currently in use. In practice no shrinkage will
+occur if the current size is just slightly too big, unless the current size is
+exactly zero in which case we guarantee to deallocate all heap space associated
+with this array leaving a null data pointer and begin()==end()==0, exactly as
+though the array had just been default-constructed. Otherwise you can check
+capacity() afterwards to see what happened. If the capacity() is reduced by
+this method, then all the elements will have been moved to new locations so
+existing iterators and references into the array will become invalid.
+
+ at note
+ - This method is from the proposed C++0x standard for std::vector, except for
+ the guaranteed behavior for a zero-size container.
+ - It is OK to call this on a non-owner array but it has no effect since
+ capacity()==size() already in that case.
+
+ at par Complexity:
+ If the capacity is reduced, there will be one call to T's copy constructor
+ and destructor (if any) for each element currently in the array. Otherwise
+ this is very fast. **/
+void shrink_to_fit() {
+ // Allow 25% slop, but note that if size()==0 this will always reallocate
+ // unless capacity is already zero.
+ if (capacity() - size()/4 <= size()) // avoid overflow if size() near max
+ return;
+ T* newData = allocN(size());
+ copyConstructThenDestructSource(newData, newData+size(), data());
+ deallocateNoDestruct(); // data()=0, allocated()=0, size() unchanged
+ setData(newData);
+ setAllocated(size());
+}
+
+/** Return the amount of heap space owned by this array; this is the same
+as capacity() for owner arrays but is zero for non-owners.
+ at note There is no equivalent of this method for std::vector. **/
+size_type allocated() const {return this->CBase::allocated();}
+/** Does this array own the data to which it refers? If not, it can't be
+resized, and the destructor will not free any heap space nor call any element
+destructors. If the array does not refer to \e any data it is considered to be
+an owner and it is resizeable.
+ at note There is no equivalent of this method for std::vector. **/
+bool isOwner() const {return this->CBase::isOwner();}
+/*@} End of size and capacity. **/
+
+
+//------------------------------------------------------------------------------
+/** @name Iterators
+
+These methods deal in iterators, which are STL generalized pointers. For this
+class, iterators are just ordinary pointers to T, and you may depend on that.
+By necessity, reverse iterators can't be just pointers; however, they contain
+an ordinary iterator (i.e. a pointer) that can be obtained by calling the
+reverse iterator's base() method. **/
+/*@{*/
+
+/** Return a const pointer to the first element of this array if any, otherwise
+cend(), which may be null (0) in that case but does not have to be. This method
+is from the proposed C++0x standard; there is also an overloaded begin() from
+the original standard that returns a const pointer. **/
+const T* cbegin() const {return this->CBase::cbegin();}
+/** The const version of begin() is the same as cbegin(). **/
+const T* begin() const {return this->CBase::cbegin();}
+/** Return a writable pointer to the first element of this array if any,
+otherwise end(). If the array is empty, this \e may return null (0) but does
+not have to -- the only thing you can be sure of is that begin() == end() for
+an empty array. **/
+T* begin() {return this->Base::begin();}
+
+/** Return a const pointer to what would be the element just after the last one
+in the array; this may be null (0) if there are no elements but doesn't have to
+be. This method is from the proposed C++0x standard; there is also an
+overloaded end() from the original standard that returns a const pointer. **/
+const T* cend() const {return this->CBase::cend();}
+/** The const version of end() is the same as cend(). **/
+const T* end() const {return this->CBase::cend();}
+/** Return a writable pointer to what would be the element just after the last
+one in this array. If the array is empty, this \e may return null (0) but does
+not have to -- the only thing you can be sure of is that begin()==end() for an
+empty array. **/
+T* end() {return this->Base::end();}
+
+/** Return a const reverse iterator pointing to the last element in the array
+or crend() if the array is empty. **/
+const_reverse_iterator crbegin() const {return this->CBase::crbegin();}
+/** The const version of rbegin() is the same as crbegin(). **/
+const_reverse_iterator rbegin() const {return this->CBase::crbegin();}
+/** Return a writable reverse iterator pointing to the last element in the
+array or rend() if the array is empty. **/
+reverse_iterator rbegin() {return this->Base::rbegin();}
+
+/** Return the past-the-end reverse iterator that tests equal to a reverse
+iterator that has been incremented past the front of the array. You cannot
+dereference this iterator. **/
+const_reverse_iterator crend() const {return this->CBase::crend();}
+/** The const version of rend() is the same as crend(). **/
+const_reverse_iterator rend() const {return this->CBase::crend();}
+/** Return a writable past-the-end reverse iterator that tests equal to a
+reverse iterator that has been incremented past the front of the array. You
+cannot dereference this iterator. **/
+reverse_iterator rend() {return this->Base::rend();}
+
+/** Return a const pointer to the first element of the array, or possibly
+(but not necessarily) null (0) if the array is empty.
+ at note
+ cdata() does not appear to be in the C++0x standard although it would seem
+ obvious in view of the cbegin() and cend() methods that had to be added.
+ The C++0x overloaded const data() method is also available. **/
+const T* cdata() const {return this->CBase::cdata();}
+/** The const version of the data() method is identical to cdata().
+ at note This method is from the proposed C++0x std::vector. **/
+const T* data() const {return this->CBase::cdata();}
+/** Return a writable pointer to the first allocated element of the array, or
+a null pointer if no space is associated with the array.
+ at note This method is from the proposed C++0x std::vector. **/
+T* data() {return this->Base::data();}
+/*@}*/
+
+/** @name Element access
+
+These methods provide read and write access to individual elements, or groups
+of elements, that are currently present in the array. **/
+/*@{*/
+
+/** Select an element by its index, returning a const reference. Note that only
+a value of the Array's templatized index type is allowed (default is unsigned).
+This will be range-checked in a Debug build but not in Release.
+ at pre 0 <= \a i < size()
+ at par Complexity:
+ Constant time. **/
+const T& operator[](index_type i) const {return this->CBase::operator[](i);}
+
+/** Select an element by its index, returning a writable (lvalue) reference.
+Note that only a value of the Array's templatized index type is allowed
+(default is unsigned). This will be range-checked in a Debug build but not
+in Release.
+ at pre 0 <= \a i < size()
+ at par Complexity:
+ Constant time. **/
+T& operator[](index_type i) {return this->Base::operator[](i);}
+
+/** Same as operator[] but always range-checked, even in a Release build.
+ at pre 0 <= \a i < size()
+ at par Complexity:
+ Constant time. **/
+const T& at(index_type i) const {return this->CBase::at(i);}
+
+/** Same as operator[] but always range-checked, even in a Release build.
+ at pre 0 <= \a i < size()
+ at par Complexity:
+ Constant time. **/
+T& at(index_type i) {return const_cast<T&>(this->Base::at(i));}
+
+/** Same as the const form of operator[]; exists to provide a non-operator
+method for element access in case that's needed. **/
+const T& getElt(index_type i) const {return this->CBase::getElt(i);}
+/** Same as the non-const form of operator[]; exists to provide a non-operator
+method for element access in case that's needed. **/
+T& updElt(index_type i) {return this->Base::updElt(i);}
+
+/** Return a const reference to the first element in this array, which must
+not be empty.
+ at pre The array is not empty.
+ at par Complexity:
+ Constant time. **/
+const T& front() const {return this->CBase::front();}
+
+/** Return a writable reference to the first element in this array, which must
+not be empty.
+ at pre The array is not empty.
+ at par Complexity:
+ Constant time. **/
+T& front() {return const_cast<T&>(this->Base::front());}
+
+/** Return a const reference to the last element in this array, which must
+not be empty.
+ at pre The array is not empty.
+ at par Complexity:
+ Constant time. **/
+const T& back() const {return this->CBase::back();}
+
+/** Return a writable reference to the last element in this array, which must
+not be empty.
+ at pre The array is not empty.
+ at par Complexity:
+ Constant time. **/
+T& back() {return const_cast<T&>(this->Base::back());}
+
+/** Select a subrange of this const array by starting index and length, and
+return a ArrayViewConst_ referencing that data without copying it. **/
+ArrayViewConst_<T,X> operator()(index_type index, size_type length) const
+{ return CBase::operator()(index,length); }
+/** Same as const form of operator()(index,length); exists to provide
+non-operator access to that functionality in case it is needed. **/
+ArrayViewConst_<T,X> getSubArray(index_type index, size_type length) const
+{ return CBase::getSubArray(index,length); }
+
+/** Select a subrange of this array by starting index and length, and
+return an ArrayView_ referencing that data without copying it. **/
+ArrayView_<T,X> operator()(index_type index, size_type length)
+{ return Base::operator()(index,length); }
+/** Same as non-const operator()(index,length); exists to provide non-operator
+access to that functionality in case it is needed. **/
+ArrayView_<T,X> updSubArray(index_type index, size_type length)
+{ return Base::updSubArray(index,length); }
+/*@} End of element access. **/
+
+
+//------------------------------------------------------------------------------
+/**@name Element insertion and removal
+
+These are methods that change the number of elements in the array by insertion
+or erasure. **/
+/*@{*/
+
+/** This method increases the size of the Array by one element at the end and
+initializes that element by copy constructing it from the given value. If
+capacity() > size(), that's all that will happen. If capacity()==size(), there
+is no room for another element so we'll allocate more space and move all the
+elements there. A reference to the just-inserted element can be obtained using
+the back() method after the call to push_back().
+ at param[in] value
+ An object of type T from which the new element is copy-constructed.
+
+ at remarks
+ - If you are appending a default-constructed object of type T, consider using
+ the alternate non-standard but safe push_back() method rather than
+ push_back(T()). The non-standard method default-constructs the new element
+ internally. That avoids a call to the copy constructor which can be
+ expensive for some objects, and nonexistent for others.
+ - If you are constructing the source object with a non-default constructor,
+ and the object is expensive or impossible to default-construct and/or
+ copy-construct, consider using the non-standard and dangerous method
+ raw_push_back() which enables you to construct the new element in place.
+
+ at par Complexity:
+ Constant time if no reallocation is required; otherwise the current
+ contents of the array must be copied to new space, costing one call to T's
+ copy constructor and destructor (if any) for each element currently in the
+ array. Either way there is also one call to T's copy constructor to
+ construct the new element from the supplied value. **/
+void push_back(const T& value) {
+ if (pallocated() == psize())
+ growAtEnd(1,"Array_<T>::push_back(value)");
+ copyConstruct(end(), value);
+ incrSize();
+}
+
+/** This is a non-standard version of push_back() that increases the size of the
+array by one default-constructed element at the end. This avoids having to
+default-construct the argument to the standard push_back(value) method which
+then has to copy-construct it into the array. By carefully avoiding
+reallocation and using this form of push_back() you can use the Array_<T> class
+to hold objects of type T even if T has no copy constructor, which is
+prohibited by the standard std::vector<T> definition.
+
+ at par Complexity:
+ Same as the standard push_back(value) method except without the final
+ call to T's copy constructor.
+ at see push_back(value)
+**/
+void push_back() {
+ if (pallocated() == psize())
+ growAtEnd(1,"Array_<T>::push_back()");
+ defaultConstruct(end());
+ incrSize();
+}
+
+/** This dangerous method increases the Array's size by one element at the end
+but doesn't perform any construction so the memory is filled with garbage. You
+must immediately construct into this space, using code like:
+ at code
+ new(a.raw_push_back()) MyConstructor(...args...);
+ at endcode
+This is a substantial performance improvement when the element type is something
+complicated since the constructor is called once and not copied; it can also be
+used for objects that have neither default nor copy constructors.
+ at return
+ An iterator (pointer) pointing at the unconstructed element.
+ at par Complexity:
+ Same as ordinary push_back().
+ at see push_back(value), push_back()
+**/
+T* raw_push_back() {
+ if (pallocated() == psize())
+ growAtEnd(1,"Array_<T>::raw_push_back()");
+ T* const p = end();
+ incrSize();
+ return p;
+}
+
+/** Remove the last element from this array, which must not be empty. The
+element is destructed, not returned. The array's size() is reduced by one. **/
+void pop_back() {
+ SimTK_ERRCHK(!empty(), "Array_<T>::pop_back()", "Array was empty.");
+ destruct(&back());
+ decrSize();
+}
+
+/** Erase elements in range [first,last1), packing in any later elements into
+the newly-available space and reducing the array's size by the number of
+elements erased. Capacity is unchanged. If the range is empty nothing happens.
+
+ at pre begin() <= \a first <= \a last1 <= end()
+ at param first
+ Points to the first element that will be erased.
+ at param last1
+ Points one element past the last element to be erased.
+ at return
+ An iterator pointing to the new location of the element immediately
+ following the erased ones, or end() if there are none. Either way, this is
+ the same memory address as the passed-in \a first argument since there can
+ be no reallocation here.
+ at par Complexity:
+ Calls T's destructor once for each erased element and calls T's copy
+ constructor and destructor once for each element that has to be moved. **/
+T* erase(T* first, const T* last1) {
+ SimTK_ERRCHK(begin() <= first && first <= last1 && last1 <= end(),
+ "Array<T>::erase(first,last1)", "Pointers out of range or out of order.");
+
+ const size_type nErased = size_type(last1-first);
+ SimTK_ERRCHK(isOwner() || nErased==0, "Array<T>::erase(first,last1)",
+ "No elements can be erased from a non-owner array.");
+
+ if (nErased) {
+ destruct(first, last1); // Destruct the elements we're erasing.
+ moveElementsDown(first+nErased, nErased); // Compress followers into the gap.
+ setSize(size()-nErased);
+ }
+ return first;
+}
+
+/** Erase just one element, moving all subsequent elements down one slot and
+reducing the array's size by one. This is equivalent to erase(p,p+1) but faster;
+that means \a p cannot be end() because end()+1 is not defined. Capacity is
+unchanged.
+
+ at note If you don't mind the elements being reordered, you can erase an element
+in constant time using the non-standard extension eraseFast().
+
+ at param p
+ Points to the element that will be erased; \a p cannot be end().
+ at return
+ A pointer to the element that replaced the one at \a p, or end() if \a p
+ was the last element. Either way, this is the same memory address as the
+ erased element had since there can be no reallocation here.
+ at pre begin() <= \a p < end()
+ at par Complexity:
+ Calls T's destructor once for the erased element and calls T's copy
+ constructor and destructor once for each element that has to be moved.
+ at see eraseFast() **/
+T* erase(T* p) {
+ SimTK_ERRCHK(begin() <= p && p < end(),
+ "Array<T>::erase(p)", "Pointer must point to a valid element.");
+ SimTK_ERRCHK(isOwner(), "Array<T>::erase(p)",
+ "No elements can be erased from a non-owner array.");
+
+ destruct(p); // Destruct the element we're erasing.
+ moveElementsDown(p+1, 1); // Compress followers into the gap.
+ decrSize();
+ return p;
+}
+
+
+/** Be careful with this non-standard extension; it erases one element and
+then moves the last one in its place which changes the element order
+from what it was before (unlike the standard erase() method). This avoids
+having to compress the elements so this runs in constant time:
+the element is destructed; then if it wasn't the last element the
+copy constructor is used to copy the last element into the vacated
+space, and the destructor is called to clear the last element. The
+size is reduced by 1 but the capacity does not change.
+
+ at param p
+ Points to the element that will be erased; \a p cannot be end().
+ at return
+ A pointer to the element that replaced the one at \a p, or end() if \a p
+ was the last element. Either way, this is the same memory address as the
+ erased element had since there can be no reallocation here.
+ at pre begin() <= \a p < end()
+ at par Complexity:
+ Calls T's destructor once for the erased element and calls T's copy
+ constructor and destructor once for each element that has to be moved.
+ at see erase() **/
+T* eraseFast(T* p) {
+ SimTK_ERRCHK(begin() <= p && p < end(),
+ "Array<T>::eraseFast(p)", "Pointer must point to a valid element.");
+ SimTK_ERRCHK(isOwner(), "Array<T>::eraseFast(p)",
+ "No elements can be erased from a non-owner array.");
+
+ destruct(p);
+ if (p+1 != end())
+ moveOneElement(p, &back());
+ decrSize();
+ return p;
+}
+
+/** Erase all the elements currently in this array without changing the
+capacity; equivalent to erase(begin(),end()) but a little faster. Size is
+zero after this call. T's destructor is called exactly once for each element
+in the array.
+
+ at par Complexity:
+ O(n) if T has a destructor; constant time otherwise. **/
+void clear() {
+ SimTK_ERRCHK(isOwner() || empty(), "Array_<T>::clear()",
+ "clear() is not allowed for a non-owner array.");
+ destruct(begin(), end());
+ setSize(0);
+}
+
+
+/** Insert \a n copies of a given value at a particular location within this
+array, moving all following elements up by \a n positions.
+
+ at param[in] p
+ Where to insert the new elements. This must be an iterator (pointer) that
+ is valid for this array, that is, begin() <= \a p <= end().
+ at param[in] n
+ How many copies of the given \a value to insert. Nothing happens if
+ \a n is zero.
+ at param[in] value
+ A value of the element type that is copied into the newly-created elements
+ using T's copy constructor.
+ at return
+ A pointer to the first of the newly-created elements in the array. This
+ will be different from \a p if reallocation occurred, otherwise it is the
+ same as \a p was on entry.
+
+ at pre begin() <= \a p <= end()
+ at par Complexity:
+ If size() + \a n > capacity() then the array must be reallocated, resulting
+ in size() copy constructor/destructor call pairs to move the old data to
+ the new location. Otherwise, the m=(end()-\a p) elements above the insertion
+ point must be moved up \a n positions resulting in m copy/destruct pairs.
+ Then there are n additional copy constructor calls to construct the new
+ elements from the given value.
+**/
+T* insert(T* p, size_type n, const T& value) {
+ T* const gap = insertGapAt(p, n, "Array<T>::insert(p,n,value)");
+ // Copy construct into the inserted elements and note the size change.
+ fillConstruct(gap, gap+n, value);
+ setSize(size()+n);
+ return gap;
+}
+
+/** Insert a new element at a given location within this array, initializing
+it to a copy of a given value and moving all following elements up one
+position. This is identical to insert(\a p,1,\a value) but slightly faster; see
+that method for full documentation. **/
+T* insert(T* p, const T& value) {
+ T* const gap = insertGapAt(p, 1, "Array<T>::insert(p,value)");
+ // Copy construct into the inserted element and note the size change.
+ copyConstruct(gap, value);
+ incrSize();
+ return gap;
+}
+
+/** Insert elements in a range [first,last1) into this array at a given position
+\a p, moving all following elements up by n=(last1-first) positions. This
+variant of insert() takes iterators which are ordinary pointers, although the
+source elements do not have to be of type T as long as there is a constructor
+T(T2) that works.
+
+ at param[in] p
+ Where to insert the new elements. This must be an iterator (pointer) that
+ is valid for this array, that is, begin() <= \a p <= end().
+ at param[in] first
+ This is a pointer to the first element of the source to be copied.
+ at param[in] last1
+ This points one element past the last element of the source to be copied.
+ at return
+ A pointer to the first of the newly-created elements in the array. This
+ will be different from \a p if reallocation occurred, otherwise it is the
+ same as \a p was on entry.
+
+ at pre begin() <= \a p <= end()
+ at pre first <= last1
+ at pre The range [first,last1) does not include any of the current contents
+of this array.
+ at par Complexity:
+ If capacity() < size()+n then the array must be reallocated, resulting in
+ size() calls to T's copy constructor and destructor (if any)to move the old
+ data to the new location. Otherwise, the m=(end()-\a p) elements above the
+ insertion point must be moved up n positions resulting in m copy/destruct
+ pairs. Then there are n additional copy constructor calls to construct the
+ new elements from the given value. **/
+template <class T2>
+T* insert(T* p, const T2* first, const T2* last1) {
+ const char* methodName = "Array_<T>::insert(T* p, T2* first, T2* last1)";
+ SimTK_ERRCHK((first&&last1) || (first==last1), methodName,
+ "One of first or last1 was null; either both or neither must be null.");
+ SimTK_ERRCHK(!this->overlapsWithData(first,last1), methodName,
+ "Source range can't overlap with the current array contents.");
+ // Pointers are random access iterators.
+ return insertIteratorDispatch(p, first, last1,
+ std::random_access_iterator_tag(),
+ methodName);
+}
+
+/** Insert elements in a range [first,last1) where the range is given by
+non-pointer iterators. **/
+template <class Iter>
+T* insert(T* p, const Iter& first, const Iter& last1) {
+ return insertDispatch(p, first, last1,
+ typename IsIntegralType<Iter>::Result(),
+ "Array_<T>::insert(T* p, Iter first, Iter last1)");
+}
+/*@} End of insertion and erase. **/
+
+
+
+//------------------------------------------------------------------------------
+ private:
+//------------------------------------------------------------------------------
+
+
+// This method is used when we have already decided we need to make room for
+// some new elements by reallocation, by creating a gap somewhere within the
+// existing data. We'll issue an error message if this would violate the
+// max_size restriction (we can afford to do that even in a Release build since
+// we don't expect to grow very often). Otherwise we'll allocate some more space
+// and copy construct the existing elements into the new space, leaving a gap
+// where indicated. Note that this method does not change the current size but
+// does change the capacity.
+//
+// The gapPos must point within the existing data with null OK if the array
+// itself is null, and end() being OK any time although you should use the
+// more specialized growAtEnd() method if you know that's what's happening.
+//
+// Don't call this with a gapSz of zero.
+T* growWithGap(T* gapPos, size_type gapSz, const char* methodName) {
+ assert(gapSz > 0); // <= 0 is a bug, not a user error
+
+ // Note that gapPos may be null if begin() and end() are also.
+ SimTK_ERRCHK(begin() <= gapPos && gapPos <= end(), methodName,
+ "Given insertion point is not valid for this array.");
+
+ // Get some new space of a reasonable size.
+ setAllocated(calcNewCapacityForGrowthBy(gapSz, methodName));
+ T* newData = allocN(allocated());
+
+ // How many elements will be before the gap?
+ const size_type nBefore = (size_type)(gapPos-begin());
+
+ // Locate the gap in the new space allocation.
+ T* newGap = newData + nBefore;
+ T* newGapEnd = newGap + gapSz; // one past the last element in the gap
+
+ // Copy elements before insertion point; destruct source as we go.
+ copyConstructThenDestructSource(newData, newGap, data());
+ // Copy/destruct elements at and after insertion pt; leave gapSz gap.
+ copyConstructThenDestructSource(newGapEnd, newData+size(), gapPos);
+
+ // Throw away the old data and switch to the new.
+ freeN(data()); setData(newData);
+ return newGap;
+}
+
+// Same as growWithGap(end(), n, methodName); see above.
+void growAtEnd(size_type n, const char* methodName) {
+ assert(n > 0); // <= 0 is a bug, not a user error
+ // Get some new space of a reasonable size.
+ setAllocated(calcNewCapacityForGrowthBy(n, methodName));
+ T* newData = allocN(allocated());
+ // Copy all the elements; destruct source as we go.
+ copyConstructThenDestructSource(newData, newData+size(), data());
+ // Throw away the old data and switch to the new.
+ freeN(data()); setData(newData);
+}
+
+// This method determines how much we should increase the array's capacity
+// when asked to insert n elements, due to an insertion or push_back. We will
+// generally allocate more new space than requested, in anticipation of
+// further insertions. This has to be based on the current size so that only
+// log(n) reallocations are performed to insert n elements one at a time. Our
+// policy here is to at least double the capacity unless that would exceed
+// max_size(). There is also a minimum amount of allocation we'll do if the
+// current size is zero or very small.
+size_type calcNewCapacityForGrowthBy(size_type n, const char* methodName) const {
+ SimTK_ERRCHK3_ALWAYS(isGrowthOK(n), methodName,
+ "Can't grow this Array by %llu element(s) because it would"
+ " then exceed the max_size of %llu set by its index type %s.",
+ (unsigned long long)n, ullMaxSize(), indexName());
+
+ // At this point we know that capacity()+n <= max_size().
+ const size_type mustHave = capacity() + n;
+
+ // Be careful not to overflow size_type as you could if you
+ // double capacity() rather than halving max_size().
+ const size_type wantToHave = capacity() <= max_size()/2
+ ? 2*capacity()
+ : max_size();
+
+ const size_type newCapacity = std::max(std::max(mustHave,wantToHave),
+ minAlloc());
+ return newCapacity;
+}
+
+// Insert an unconstructed gap of size n beginning at position p. The return
+// value is a pointer to the first element in the gap. It will be p if no
+// reallocation occurred, otherwise it will be pointing into the new data.
+// On return size() will be unchanged although allocated() may be bigger.
+T* insertGapAt(T* p, size_type n, const char* methodName) {
+ // Note that p may be null if begin() and end() are also.
+ SimTK_ERRCHK(begin() <= p && p <= end(), methodName,
+ "Given insertion point is not valid for this Array.");
+
+ if (n==0) return p; // nothing to do
+
+ SimTK_ERRCHK_ALWAYS(isOwner(), methodName,
+ "No elements can be inserted into a non-owner array.");
+
+ // Determine the number of elements before the insertion point and
+ // the number at or afterwards (those must be moved up by one slot).
+ const size_type before = (size_type)(p-begin()), after = (size_type)(end()-p);
+
+ // Grow the container if necessary. Note that if we have to grow we
+ // can create the gap at the same time we copy the old elements over
+ // to the new space.
+ if (capacity() >= size()+n) {
+ moveElementsUp(p, n); // leave a gap at p
+ } else { // need to grow
+ setAllocated(calcNewCapacityForGrowthBy(n, methodName));
+ T* newdata = allocN(allocated());
+ // Copy the elements before the insertion point, and destroy source.
+ copyConstructThenDestructSource(newdata, newdata+before, data());
+ // Copy the elements at and after the insertion point, leaving a gap
+ // of n elements.
+ copyConstructThenDestructSource(newdata+before+n,
+ newdata+before+n+after,
+ p); // i.e., pData+before
+ p = newdata + before; // points into newdata now
+ freeN(data());
+ setData(newdata);
+ }
+
+ return p;
+}
+
+//------------------------------------------------------------------------------
+// CTOR DISPATCH
+// This is the constructor implementation for when the class that matches
+// the alleged InputIterator type turns out to be one of the integral types
+// in which case this should be the ctor(n, initValue) constructor.
+template <class IntegralType> void
+ctorDispatch(IntegralType n, IntegralType v, TrueType isIntegralType) {
+ new(this) Array_(size_type(n), value_type(v));
+}
+
+// This is the constructor implementation for when the class that matches
+// the alleged InputIterator type is NOT an integral type and may very well
+// be an iterator. In that case we split into iterators for which we can
+// determine the number of elements in advance (forward, bidirectional,
+// random access) and input iterators, for which we can't. Note: iterator
+// types are arranged hierarchically random->bi->forward->input with each
+// deriving from the one on its right, so the forward iterator tag also
+// matches bi and random.
+template <class InputIterator> void
+ctorDispatch(const InputIterator& first, const InputIterator& last1,
+ FalseType isIntegralType)
+{ ctorIteratorDispatch(first, last1,
+ typename std::iterator_traits<InputIterator>::iterator_category()); }
+
+// This is the slow generic ctor implementation for any iterator that can't
+// make it up to forward_iterator capability (that is, an input_iterator).
+// The issue here is that we can't advance the iterator to count the number
+// of elements before allocating because input_iterators are consumed when
+// reference so we can't go back to look. That means we may have to reallocate
+// memory log n times as we "push back" these elements onto the array.
+template <class InputIterator> void
+ctorIteratorDispatch(const InputIterator& first, const InputIterator& last1,
+ std::input_iterator_tag)
+{
+ InputIterator src = first;
+ while (src != last1) {
+ // We can afford to check this always since we are probably doing I/O.
+ // Throwing an exception in a constructor is tricky, though -- this
+ // won't go through the Array_ destructor although it will call the
+ // Base (ArrayView_) destructor. Since we have already allocated
+ // some space, we must call deallocate() manually.
+ if (size() == max_size()) {
+ deallocate();
+ SimTK_ERRCHK2_ALWAYS(!"too many elements",
+ "Array_::ctor(InputIterator first, InputIterator last1)",
+ "There were still source elements available when the array"
+ " reached its maximum size of %llu as determined by its index"
+ " type %s.", ullMaxSize(), indexName());
+ }
+ push_back(*src++);
+ }
+}
+
+// This is the faster constructor implementation for iterator types for which
+// we can calculate the number of elements in advance. This will be optimal
+// for a random access iterator since we can count in constant time, but for
+// forward or bidirectional we'll have to advance n times to count and then
+// go back again to do the copy constructions.
+template <class ForwardIterator> void
+ctorIteratorDispatch(const ForwardIterator& first, const ForwardIterator& last1,
+ std::forward_iterator_tag)
+{
+ typedef typename std::iterator_traits<ForwardIterator>::difference_type
+ difference_type;
+ // iterDistance() is constant time for random access iterators, but
+ // O(last1-first) for forward and bidirectional since it has to increment
+ // to count how far apart they are.
+ const difference_type nInput = this->iterDistance(first,last1);
+
+ SimTK_ERRCHK(nInput >= 0,
+ "Array_(ForwardIterator first, ForwardIterator last1)",
+ "Iterators were out of order.");
+
+ SimTK_ERRCHK3(this->isSizeOK(nInput),
+ "Array_(ForwardIterator first, ForwardIterator last1)",
+ "Source has %llu elements but this array is limited to %llu"
+ " elements by its index type %s.",
+ this->ull(nInput), ullMaxSize(), indexName());
+
+ const size_type n = size_type(nInput);
+ setSize(n);
+ allocateNoConstruct(n);
+ copyConstruct(data(), data()+n, first);
+}
+
+//------------------------------------------------------------------------------
+// INSERT DISPATCH
+// This is the insert() implementation for when the class that matches
+// the alleged InputIterator type turns out to be one of the integral types
+// in which case this should be the insert(p, n, initValue) constructor.
+template <class IntegralType>
+T* insertDispatch(T* p, IntegralType n, IntegralType v,
+ TrueType isIntegralType, const char*)
+{ return insert(p, size_type(n), value_type(v)); }
+
+// This is the insert() implementation for when the class that matches
+// the alleged InputIterator type is NOT an integral type and may very well
+// be an iterator. See ctorDispatch() above for more information.
+template <class InputIterator>
+T* insertDispatch(T* p, const InputIterator& first, const InputIterator& last1,
+ FalseType isIntegralType, const char* methodName)
+{ return insertIteratorDispatch(p, first, last1,
+ typename std::iterator_traits<InputIterator>::iterator_category(),
+ methodName); }
+
+// This is the slow generic insert implementation for any iterator that can't
+// make it up to forward_iterator capability (that is, an input_iterator).
+// See ctorIteratorDispatch() above for more information.
+template <class InputIterator>
+T* insertIteratorDispatch(T* p, InputIterator first, InputIterator last1,
+ std::input_iterator_tag, const char* methodName)
+{
+ size_type nInserted = 0;
+ while (first != last1) {
+ // We can afford to check this always since we are probably doing I/O.
+ SimTK_ERRCHK2_ALWAYS(size() < max_size(), methodName,
+ "There were still source elements available when the array"
+ " reached its maximum size of %llu as determined by its index"
+ " type %s.", ullMaxSize(), indexName());
+ p = insert(p, *first++); // p may now point to reallocated memory
+ ++p; ++nInserted;
+ }
+ // p now points just after the last inserted element; subtract the
+ // number inserted to get a pointer to the first inserted element.
+ return p-nInserted;
+}
+
+// This is the faster constructor implementation for iterator types for which
+// we can calculate the number of elements in advance. This will be optimal
+// for a random access iterator since we can count in constant time, but for
+// forward or bidirectional we'll have to advance n times to count and then
+// go back again to do the copy constructions.
+template <class ForwardIterator>
+T* insertIteratorDispatch(T* p, const ForwardIterator& first,
+ const ForwardIterator& last1,
+ std::forward_iterator_tag,
+ const char* methodName)
+{
+ typedef typename std::iterator_traits<ForwardIterator>::difference_type
+ difference_type;
+ // iterDistance() is constant time for random access iterators, but
+ // O(last1-first) for forward and bidirectional since it has to increment
+ // to count how far apart they are.
+ const difference_type nInput = this->iterDistance(first,last1);
+
+ SimTK_ERRCHK(nInput >= 0, methodName, "Iterators were out of order.");
+
+ SimTK_ERRCHK3(isGrowthOK(nInput), methodName,
+ "Source has %llu elements which would make this array exceed the %llu"
+ " elements allowed by its index type %s.",
+ this->ull(nInput), ullMaxSize(), indexName());
+
+ const size_type n = size_type(nInput);
+ p = insertGapAt(p, n, methodName);
+ copyConstruct(p, p+n, first);
+ setSize(size()+n);
+ return p;
+}
+
+//------------------------------------------------------------------------------
+// ASSIGN DISPATCH
+// This is the assign() implementation for when the class that matches
+// the alleged InputIterator type turns out to be one of the integral types
+// in which case this should be the assign(n, initValue) constructor.
+template <class IntegralType>
+void assignDispatch(IntegralType n, IntegralType v, TrueType isIntegralType,
+ const char* methodName)
+{ assign(size_type(n), value_type(v)); }
+
+// This is the assign() implementation for when the class that matches
+// the alleged InputIterator type is NOT an integral type and may very well
+// be an iterator. See ctorDispatch() above for more information.
+template <class InputIterator>
+void assignDispatch(const InputIterator& first, const InputIterator& last1,
+ FalseType isIntegralType, const char* methodName)
+{ assignIteratorDispatch(first, last1,
+ typename std::iterator_traits<InputIterator>::iterator_category(),
+ methodName); }
+
+// This is the slow generic implementation for a plain input_iterator
+// (i.e., not a forward, bidirectional, or random access iterator). These
+// have the unfortunate property that we can't count the elements in advance.
+template <class InputIterator>
+void assignIteratorDispatch(const InputIterator& first,
+ const InputIterator& last1,
+ std::input_iterator_tag,
+ const char* methodName)
+{
+ // TODO: should probably allow this and just blow up when the size()+1st
+ // element is seen.
+ SimTK_ERRCHK_ALWAYS(isOwner(), methodName,
+ "Assignment to a non-owner array can only be done from a source"
+ " designated with forward iterators or pointers because we"
+ " must be able to verify that the source and destination sizes"
+ " are the same.");
+
+ clear(); // TODO: change space allocation here?
+ InputIterator src = first;
+ while (src != last1) {
+ // We can afford to check this always since we are probably doing I/O.
+ SimTK_ERRCHK2_ALWAYS(size() < max_size(), methodName,
+ "There were still source elements available when the array"
+ " reached its maximum size of %llu as determined by its index"
+ " type %s.", ullMaxSize(), indexName());
+
+ push_back(*src++);
+ }
+}
+
+// This is the faster implementation that works for forward, bidirectional,
+// and random access iterators including ordinary pointers. We can check here that the
+// iterators are in the right order, and that the source is not too big to
+// fit in this array. Null pointer checks should be done prior to calling,
+// however, since iterators in general aren't pointers.
+template <class ForwardIterator>
+void assignIteratorDispatch(const ForwardIterator& first,
+ const ForwardIterator& last1,
+ std::forward_iterator_tag,
+ const char* methodName)
+{
+ typedef typename std::iterator_traits<ForwardIterator>::difference_type
+ IterDiffType;
+ // iterDistance() is constant time for random access iterators, but
+ // O(last1-first) for forward and bidirectional since it has to increment
+ // to count how far apart they are.
+ const IterDiffType nInput = this->iterDistance(first,last1);
+
+ SimTK_ERRCHK(nInput >= 0, methodName, "Iterators were out of order.");
+
+ SimTK_ERRCHK3(this->isSizeOK(nInput), methodName,
+ "Source has %llu elements but this Array is limited to %llu"
+ " elements by its index type %s.",
+ this->ull(nInput), ullMaxSize(), indexName());
+
+ const size_type n = size_type(nInput);
+ if (isOwner()) {
+ // This is an owner Array; assignment is considered deallocation
+ // followed by copy construction.
+
+ clear(); // all elements destructed; allocation unchanged
+ reallocateIfAdvisable(n); // change allocation if too small or too big
+ copyConstruct(data(), cdata()+n, first);
+ setSize(n);
+ } else {
+ // This is a non-owner Array. Assignment can occur only if the
+ // source is the same size as the array, and the semantics are of
+ // repeated assignment using T::operator=() not destruction followed
+ // by copy construction.
+ SimTK_ERRCHK2(n == size(), methodName,
+ "Source has %llu elements which does not match the size %llu"
+ " of the non-owner array it is being assigned into.",
+ this->ull(n), ullSize());
+
+ T* p = begin();
+ ForwardIterator src = first;
+ while (src != last1)
+ *p++ = *src++; // call T's assignment operator
+ }
+}
+
+// We are going to put a total of n elements into the Array (probably
+// because of an assignment or resize) and we want the space allocation
+// to be reasonable. That means of course that the allocation must be
+// *at least* n, but we also don't want it to be too big. Our policy
+// here is that if it is currently less than twice what we need we
+// won't reallocate, otherwise we'll shrink the space. When changing
+// the size to zero or something very small we'll treat the Array as
+// though its current size is minAlloc, meaning we won't reallocate
+// if the existing space is less than twice minAlloc.
+// nAllocated will be set appropriately; size() is not touched here.
+// No constructors or destructors are called.
+void reallocateIfAdvisable(size_type n) {
+ if (allocated() < n || allocated()/2 > std::max(minAlloc(), n))
+ reallocateNoDestructOrConstruct(n);
+}
+
+
+void allocateNoConstruct(size_type n)
+{ setData(allocN(n)); setAllocated(n); } // size() left unchanged
+void deallocateNoDestruct()
+{ freeN(data()); setData(0); setAllocated(0); } // size() left unchanged
+void reallocateNoDestructOrConstruct(size_type n)
+{ deallocateNoDestruct(); allocateNoConstruct(n); }
+
+// This sets the smallest allocation we'll do when growing.
+size_type minAlloc() const
+{ return std::min(max_size(), size_type(4)); }
+
+// Allocate without construction. Returns a null pointer if asked
+// to allocate 0 elements. In Debug mode we'll fill memory with
+// all 1's as a bug catcher.
+static T* allocN(size_type n) {
+ if (n==0) return 0;
+ unsigned char* newdata = new unsigned char[n * sizeof(T)];
+ #ifndef NDEBUG
+ unsigned char* b=newdata;
+ const unsigned char* e=newdata+(n*sizeof(T));
+ while (b != e) *b++ = 0xff;
+ #endif
+ return reinterpret_cast<T*>(newdata);
+}
+
+// Free memory without calling T's destructor. Nothing happens if passed
+// a null pointer.
+static void freeN(T* p) {
+ delete[] reinterpret_cast<char*>(p);
+}
+
+// default construct one element
+static void defaultConstruct(T* p) {new(p) T();}
+// default construct range [b,e)
+static void defaultConstruct(T* b, const T* e)
+{ while (b!=e) new(b++) T(); }
+
+// copy construct range [b,e) with repeats of a given value
+static void fillConstruct(T* b, const T* e, const T& v)
+{ while(b!=e) new(b++) T(v); }
+
+// copy construct one element from a given value
+static void copyConstruct(T* p, const T& v) {new(p) T(v);}
+// copy construct range [b,e) from sequence of source values
+static void copyConstruct(T* b, const T* e, T* src)
+{ while(b!=e) new(b++) T(*src++); }
+// Templatized copy construct will work if the source elements are
+// assignment compatible with the destination elements.
+template <class InputIterator>
+static void copyConstruct(T* b, const T* e, InputIterator src)
+{ while(b!=e) new(b++) T(*src++); }
+
+// Copy construct range [b,e] from sequence of source values and
+// destruct the source after it is copied. It's better to alternate
+// copying and destructing than to do this in two passes since we
+// will already have touched the memory.
+static void copyConstructThenDestructSource(T* b, const T* e, T* src)
+{ while(b!=e) {new(b++) T(*src); src++->~T();} }
+
+// We have an element at from that we would like to move into the currently-
+// unconstructed slot at to. Both from and to are expected to be pointing to
+// elements within the currently allocated space. From's slot will be left
+// unconstructed.
+void moveOneElement(T* to, T* from) {
+ assert(data() <= to && to < data()+allocated());
+ assert(data() <= from && from < data()+allocated());
+ copyConstruct(to, *from);
+ destruct(from);
+}
+
+
+// Move elements from p to end() down by n>0 places to fill an unconstructed
+// gap beginning at p-n. Any leftover space at the end will be unconstructed.
+void moveElementsDown(T* p, size_type n) {
+ assert(n > 0);
+ for (; p != end(); ++p)
+ moveOneElement(p-n,p);
+}
+
+// Move elements from p to end() up by n>0 places to make an unconstructed gap
+// at [p,p+n). Note that this has to be done backwards so that we don't
+// write on any elements until after they've been copied.
+void moveElementsUp(T* p, size_type n) {
+ assert(n > 0);
+ T* src = end(); // points one past source element (begin()-1 not allowed)
+ while (src != p) {
+ --src; // now points to source
+ moveOneElement(src+n, src);;
+ }
+}
+
+// destruct one element
+static void destruct(T* p) {p->~T();}
+// destruct range [b,e)
+static void destruct(T* b, const T* e)
+{ while(b!=e) b++->~T(); }
+
+// Check that growing this array by n elements wouldn't cause it to exceed
+// its allowable maximum size.
+template <class S>
+bool isGrowthOK(S n) const
+{ return this->isSizeOK(ullCapacity() + this->ull(n)); }
+
+// The following private methods are protected methods in the ArrayView base
+// class, so they should not need repeating here. Howevr, we explicitly
+// forward to the Base methods to avoid gcc errors. The gcc complaint
+// is due to their not depending on any template parameters; the "this->"
+// apparently fixes that problem.
+
+// These provide direct access to the data members.
+packed_size_type psize() const {return this->CBase::psize();}
+packed_size_type pallocated() const {return this->CBase::pallocated();}
+
+void setData(const T* p) {this->CBase::setData(p);}
+void setSize(size_type n) {this->CBase::setSize(n);}
+void incrSize() {this->CBase::incrSize();}
+void decrSize() {this->CBase::decrSize();}
+void setAllocated(size_type n) {this->CBase::setAllocated(n);}
+// This just cast sizes to unsigned long long so that we can do comparisons
+// without getting warnings.
+unsigned long long ullSize() const {return this->CBase::ullSize();}
+unsigned long long ullCapacity() const {return this->CBase::ullCapacity();}
+unsigned long long ullMaxSize() const {return this->CBase::ullMaxSize();}
+// This is the index type name and is handy for error messages to explain
+// why some size was too big.
+const char* indexName() const {return this->CBase::indexName();}
+};
+
+
+// This "private" static method is used to implement ArrayView's
+// fillArrayViewFromStream() and Array's readArrayFromStream() namespace-scope
+// static methods, which are in turn used to implement ArrayView's and
+// Array's stream extraction operators ">>". This method has to be in the
+// header file so that we don't need to pass streams through the API, but it
+// is not intended for use by users and has no Doxygen presence, unlike
+// fillArrayFromStream() and readArrayFromStream() and (more commonly)
+// the extraction operators.
+template <class T, class X> static inline
+std::istream& readArrayFromStreamHelper
+ (std::istream& in, bool isFixedSize, Array_<T,X>& out)
+{
+ // If already failed, bad, or eof, set failed bit and return without
+ // touching the Array.
+ if (!in.good()) {in.setstate(std::ios::failbit); return in;}
+
+ // If the passed-in Array isn't an owner, then we have to treat it as
+ // a fixed size ArrayView regardless of the setting of the isFixedSize
+ // argument.
+ if (!out.isOwner())
+ isFixedSize = true; // might be overriding the argument here
+
+ // numRequired will be ignored unless isFixedSize==true.
+ const typename Array_<T,X>::size_type
+ numRequired = isFixedSize ? out.size() : 0;
+
+ if (!isFixedSize)
+ out.clear(); // We're going to replace the entire contents of the Array.
+
+ // Skip initial whitespace. If that results in eof this may be a successful
+ // read of a 0-length, unbracketed Array. That is OK for either a
+ // variable-length Array or a fixed-length ArrayView of length zero.
+ std::ws(in); if (in.fail()) return in;
+ if (in.eof()) {
+ if (isFixedSize && numRequired != 0)
+ in.setstate(std::ios_base::failbit); // zero elements not OK
+ return in;
+ }
+
+ // Here the stream is good and the next character is non-white.
+ assert(in.good());
+
+ // Use this for raw i/o (peeks and gets).
+ typename std::iostream::int_type ch;
+ const typename std::iostream::int_type EOFch =
+ std::iostream::traits_type::eof();
+
+ // Now see if the sequence is bare or surrounded by (), [], or {}.
+ bool lookForCloser = true;
+ char openBracket, closeBracket;
+ ch = in.peek(); if (in.fail()) return in;
+ assert(ch != EOFch); // we already checked above
+
+ openBracket = (char)ch;
+ if (openBracket=='(') {in.get(); closeBracket = ')';}
+ else if (openBracket=='[') {in.get(); closeBracket = ']';}
+ else if (openBracket=='{') {in.get(); closeBracket = '}';}
+ else lookForCloser = false;
+
+ // If lookForCloser is true, then closeBracket contains the terminating
+ // delimiter, otherwise we're not going to quit until eof.
+
+ // Eat whitespace after the opening bracket to see what's next.
+ if (in.good()) std::ws(in);
+
+ // If we're at eof now it must be because the open bracket was the
+ // last non-white character in the stream, which is an error.
+ if (!in.good()) {
+ if (in.eof()) {
+ assert(lookForCloser); // or we haven't read anything that could eof
+ in.setstate(std::ios::failbit);
+ }
+ return in;
+ }
+
+ // istream is good and next character is non-white; ready to read first
+ // value or terminator.
+
+ // We need to figure out whether the elements are space- or comma-
+ // separated and then insist on consistency.
+ bool commaOK = true, commaRequired = false;
+ bool terminatorSeen = false;
+ X nextIndex(0);
+ while (true) {
+ char c;
+
+ // Here at the top of this loop, we have already successfully read
+ // n=nextIndex values of type T. For fixed-size reads, it might be
+ // the case that n==numRequired already, but we still may need to
+ // look for a closing bracket before we can declare victory.
+ // The stream is good() (not at eof) but it might be the case that
+ // there is nothing but white space left; we don't know yet because
+ // if we have satisfied the fixed-size count and are not expecting
+ // a terminator then we should quit without absorbing the trailing
+ // white space.
+ assert(in.good());
+
+ // Look for closing bracket before trying to read value.
+ if (lookForCloser) {
+ // Eat white space to find the closing bracket.
+ std::ws(in); if (!in.good()) break; // eof?
+ ch = in.peek(); assert(ch != EOFch);
+ if (!in.good()) break;
+ c = (char)ch;
+ if (c == closeBracket) {
+ in.get(); // absorb the closing bracket
+ terminatorSeen = true;
+ break;
+ }
+ // next char not a closing bracket; fall through
+ }
+
+ // We didn't look or didn't find a closing bracket. The istream is good
+ // but we might be looking at white space.
+
+ // If we already got all the elements we want, break for final checks.
+ if (isFixedSize && (nextIndex == numRequired))
+ break; // that's a full count.
+
+ // Look for comma before value, except the first time.
+ if (commaOK && nextIndex != 0) {
+ // Eat white space to find the comma.
+ std::ws(in); if (!in.good()) break; // eof?
+ ch = in.peek(); assert(ch != EOFch);
+ if (!in.good()) break;
+ c = (char)ch;
+ if (c == ',') {
+ in.get(); // absorb comma
+ commaRequired = true; // all commas from now on
+ } else { // next char not a comma
+ if (commaRequired) // bad, e.g.: v1, v2, v3 v4
+ { in.setstate(std::ios::failbit); break; }
+ else commaOK = false; // saw: v1 v2 (no commas now)
+ }
+ if (!in.good()) break; // might be eof
+ }
+
+ // No closing bracket yet; don't have enough elements; skipped comma
+ // if any; istream is good; might be looking at white space.
+ assert(in.good());
+
+ // Now read in an element of type T.
+ // The extractor T::operator>>() will ignore leading white space.
+ if (!isFixedSize)
+ out.push_back(); // grow by one (default consructed)
+ in >> out[nextIndex]; if (in.fail()) break;
+ ++nextIndex;
+
+ if (!in.good()) break; // might be eof
+ }
+
+ // We will get here under a number of circumstances:
+ // - the fail bit is set in the istream, or
+ // - we reached eof
+ // - we saw a closing brace
+ // - we got all the elements we wanted (for a fixed-size read)
+ // Note that it is possible that we consumed everything except some
+ // trailing white space (meaning we're not technically at eof), but
+ // for consistency with built-in operator>>()'s we won't try to absorb
+ // that trailing white space.
+
+ if (!in.fail()) {
+ if (lookForCloser && !terminatorSeen)
+ in.setstate(std::ios::failbit); // missing terminator
+
+ if (isFixedSize && nextIndex != numRequired)
+ in.setstate(std::ios::failbit); // wrong number of values
+ }
+
+ return in;
+}
+
+
+
+//------------------------------------------------------------------------------
+// RELATED GLOBAL OPERATORS
+//------------------------------------------------------------------------------
+// These are logically part of the Array_<T,X> class but are not actually
+// class members; that is, they are in the SimTK namespace.
+
+// Some of the serialization methods could have been member functions but
+// then an attempt to explicitly instantiate the whole Array_<T> class for
+// some type T would fail if T did not support the requisite I/O operations
+// even if those operations were never used. This came up when Chris Bruns was
+// trying to wrap Array objects for Python, which requires explicit
+// instantiation.
+
+/**@name Array_<T> serialization and I/O
+These methods are at namespace scope but are logically part of the Array
+classes. These deal with reading and writing Arrays from and to streams,
+which places an additional requirement on the element type T: the element
+must support the same operation you are trying to do on the Array as a
+whole. **/
+/*@{*/
+
+/** Specialize writeUnformatted() for Array_<E,X> to delegate to element type
+E, with spaces separating the elements.
+ at relates SimTK::Array_ **/
+template <class T, class X> inline void
+writeUnformatted(std::ostream& o, const Array_<T,X>& v) {
+ for (X i(0); i < v.size(); ++i) {
+ if (i != 0) o << " ";
+ writeUnformatted(o, v[i]);
+ }
+}
+
+
+/** Specialize writeFormatted() for Array_<E,X> to delegate to element type
+E, with surrounding parentheses and commas separating the elements.
+ at relates SimTK::Array_ **/
+template <class T, class X> inline void
+writeFormatted(std::ostream& o, const Array_<T,X>& v) {
+ o << '(';
+ for (X i(0); i < v.size(); ++i) {
+ if (i != 0) o << ',';
+ writeFormatted(o, v[i]);
+ }
+ o << ')';
+}
+
+/** Output a human readable representation of an array to an std::ostream
+(like std::cout). The format is ( \e elements ) where \e elements is a
+comma-separated list of the Array's contents output by invoking the "<<"
+operator on the elements. This function will not compile if the element type
+does not support the "<<" operator. No newline is issued before
+or after the output. @relates Array_ **/
+template <class T, class X> inline
+std::ostream&
+operator<<(std::ostream& o,
+ const ArrayViewConst_<T,X>& a)
+{
+ o << '(';
+ if (!a.empty()) {
+ o << a.front();
+ for (const T* p = a.begin()+1; p != a.end(); ++p)
+ o << ',' << *p;
+ }
+ return o << ')';
+}
+
+
+/** Specialization of readUnformatted() for variable-length Array_<T,X>;
+continues reading whitespace-separated tokens until error or eof.
+ at relates Array_ **/
+template <class T, class X> inline bool
+readUnformatted(std::istream& in, Array_<T,X>& v) {
+ v.clear();
+ T element;
+ std::ws(in); // Make sure we're at eof if stream is all whitespace.
+ while (!in.eof() && readUnformatted(in, element))
+ v.push_back(element);
+ return !in.fail(); // eof is expected and ok
+}
+
+/** Specialization of readUnformatted() for fixed-length ArrayView_<T,X>; reads
+whitespace-separated tokens until the expected number have been read. If fewer
+are available we fail.
+ at relates ArrayView_ **/
+template <class T, class X> inline bool
+readUnformatted(std::istream& in, ArrayView_<T,X>& v) {
+ for (X i(0); i < v.size(); ++i)
+ if (!readUnformatted(in, v[i])) return false;
+ return true;
+}
+
+
+/** Specialization of readFormatted() for variable-length Array_<T,X>;
+uses readArrayFromStream() to consume an appropriately-formatted array
+until error, closing parenthesis or bracket, or eof.
+ at see readArrayFromStream() for details
+ at relates Array_ **/
+template <class T, class X> inline bool
+readFormatted(std::istream& in, Array_<T,X>& v) {
+ return !readArrayFromStream(in,v).fail();
+}
+
+/** Specialization of readFormatted() for fixed-length ArrayView_<T,X>; uses
+fillArrayViewFromStream() to consume an appropriately-formatted fixed-size
+array.
+ at see fillArrayViewFromStream() for details
+ at relates ArrayView_ **/
+template <class T, class X> inline bool
+readFormatted(std::istream& in, ArrayView_<T,X>& v) {
+ return !fillArrayViewFromStream(in,v).fail();
+}
+
+/** Read in an Array_<T> from a stream, as a sequence of space-separated or
+comma-separated values optionally surrounded by parentheses (), square
+brackets [], or curly braces {}. We will continue to read elements of
+type T from the stream until we find a reason to stop, using type T's stream
+extraction operator>>() to read in each element and resizing the Array as
+necessary. If the data is bracketed, we'll read until we hit the closing
+bracket. If it is not bracketed, we'll read until we hit eof() or get an error
+such as the element extractor setting the stream's fail bit due to bad
+formatting. On successful return, the stream will be positioned right after
+the final read-in element or terminating bracket, and the stream's status will
+be good() or eof(). We will not consume trailing whitespace after bracketed
+elements; that means the stream might actually be empty even if we don't
+return eof(). If you want to know whether there is anything else in the
+stream, follow this call with the STL whitespace skipper std::ws() like this:
+ at code
+ if (readArrayFromStream(in,array) && !in.eof())
+ std::ws(in); // might take us to eof
+ if (in.fail()) {...} // probably a formatting error
+ else {
+ // Here if the stream is good() then there is more to read; if the
+ // stream got used up the status is guaranteed to be eof().
+ }
+ at endcode
+A compilation error will occur if you try to use this method on an Array_<T>
+for a type T for which there is no stream extraction operator>>().
+ at note If you want to fill an owner Array_<T> with a fixed amount of data from
+the stream, resize() the array to the appropriate length and then use
+fillArrayFromStream() instead. @see fillArrayFromStream()
+ at relates Array_ **/
+template <class T, class X> static inline
+std::istream& readArrayFromStream(std::istream& in, Array_<T,X>& out)
+{ return readArrayFromStreamHelper<T,X>(in, false /*variable sizez*/, out); }
+
+
+
+/** Read in a fixed number of elements from a stream into an Array. We expect
+to read in exactly size() elements of type T, using type T's stream extraction
+operator>>(). This will stop reading when we've read size() elements, or set
+the fail bit in the stream if we run out of elements or if any element's
+extract operator sets the fail bit. On successful return, all size() elements
+will have been set, the stream will be positioned right after the final
+read-in element or terminating bracket, and the stream's status will be good()
+or eof(). We will not consume trailing whitespace after reading all the
+elements; that means the stream might actually be empty even if we don't
+return eof(). If you want to know whether there is anything else in the
+stream, follow this call with std::ws() like this:
+ at code
+ if (fillArrayFromStream(in,array))
+ if (!in.eof()) std::ws(in); // might take us to eof
+ if (in.fail()) {...} // deal with I/O or formatting error
+ // Here if the stream is good() then there is more to read; if the
+ // stream got used up the status is guaranteed to be eof().
+ at endcode
+A compilation error will occur if you try to use this method on an Array_<T>
+for a type T for which there is no stream extraction operator>>().
+ at note If you want to read in a variable number of elements and have the
+Array_<T> resized as needed, use readArrayFromStream() instead.
+ at see readArrayFromStream()
+ at relates Array_ **/
+template <class T, class X> static inline
+std::istream& fillArrayFromStream(std::istream& in, Array_<T,X>& out)
+{ return readArrayFromStreamHelper<T,X>(in, true /*fixed size*/, out); }
+
+/** Read in a fixed number of elements from a stream into an ArrayView. See
+fillArrayFromStream() for more information; this works the same way.
+ at see fillArrayFromStream()
+ at relates ArrayView_ **/
+template <class T, class X> static inline
+std::istream& fillArrayViewFromStream(std::istream& in, ArrayView_<T,X>& out)
+{ return readArrayFromStreamHelper<T,X>(in, true /*fixed size*/, out); }
+
+
+
+
+/** Read an Array_<T> from a stream as a sequence of space- or comma-separated
+values of type T, optionally delimited by parentheses, brackets, or braces.
+The Array_<T> may be an owner (variable size) or a view (fixed size n). In
+the case of an owner, we'll read all the elements in brackets or until eof if
+there are no brackets. In the case of a view, there must be exactly n elements
+in brackets, or if there are no brackets we'll consume exactly n elements and
+then stop. Each element is read in with its own operator ">>" so this won't
+work if no such operator is defined for type T.
+ at relates Array_ **/
+template <class T, class X> inline
+std::istream& operator>>(std::istream& in, Array_<T,X>& out)
+{ return readArrayFromStream<T,X>(in, out); }
+
+/** Read a (fixed size n) ArrayView_<T> from a stream as a sequence of space-
+or comma-separated values of type T, optionally delimited by parentheses,
+square brackets, or curly braces. If there are no delimiters then we will read
+size() values and then stop. Otherwise, there must be exactly size() values
+within the brackets. Each element is read in with its own operator ">>" so
+this won't work if no such operator is defined for type T.
+ at relates ArrayView_ **/
+template <class T, class X> inline
+std::istream& operator>>(std::istream& in, ArrayView_<T,X>& out)
+{ return fillArrayViewFromStream<T,X>(in, out); }
+
+/*@} End of Array serialization. **/
+
+
+
+/**@name Comparison operators
+These operators permit lexicographical comparisons between two comparable
+Array_ objects, possibly with differing element and index types, and between
+an Array_ object and a comparable std::vector object. @relates Array_ **/
+/*@{*/
+
+/** Two Array_ objects are equal if and only if they are the same size() and
+each element compares equal using an operator T1==T2. @relates Array_ **/
+template <class T1, class X1, class T2, class X2> inline bool
+operator==(const ArrayViewConst_<T1,X1>& a1, const ArrayViewConst_<T2,X2>& a2) {
+ // Avoid warnings in size comparison by using common type.
+ const ptrdiff_t sz1 = a1.end()-a1.begin();
+ const ptrdiff_t sz2 = a2.end()-a2.begin();
+ if (sz1 != sz2) return false;
+ const T1* p1 = a1.begin();
+ const T2* p2 = a2.begin();
+ while (p1 != a1.end())
+ if (!(*p1++ == *p2++)) return false;
+ return true;
+}
+/** The not equal operator is implemented using the equal operator.
+ at relates Array_ **/
+template <class T1, class X1, class T2, class X2> inline bool
+operator!=(const ArrayViewConst_<T1,X1>& a1, const ArrayViewConst_<T2,X2>& a2)
+{ return !(a1 == a2); }
+
+/** Array_ objects are ordered lexicographically; that is, by first differing
+element or by length if there are no differing elements up to the length of the
+shorter array (in which case the shorter one is "less than" the longer).
+This depends on T1==T2 and T1<T2 operators working. @relates Array_ **/
+template <class T1, class X1, class T2, class X2> inline bool
+operator<(const ArrayViewConst_<T1,X1>& a1, const ArrayViewConst_<T2,X2>& a2) {
+ const T1* p1 = a1.begin();
+ const T2* p2 = a2.begin();
+ while (p1 != a1.end() && p2 != a2.end()) {
+ if (!(*p1 == *p2))
+ return *p1 < *p2; // otherwise p1 > p2
+ ++p1; ++p2;
+ }
+ // All elements were equal until one or both arrays ran out of elements.
+ // a1 is less than a2 only if a1 ran out and a2 didn't.
+ return p1 == a1.end() && p2 != a2.end();
+}
+/** The greater than or equal operator is implemented using the less than
+operator. @relates Array_ **/
+template <class T1, class X1, class T2, class X2> inline bool
+operator>=(const ArrayViewConst_<T1,X1>& a1, const ArrayViewConst_<T2,X2>& a2)
+{ return !(a1 < a2); }
+/** The greater than operator is implemented by using less than with the
+arguments reversed, meaning the elements must have working comparison
+operators of the form T2==T1 and T2<T1. @relates Array_ **/
+template <class T1, class X1, class T2, class X2> inline bool
+operator>(const ArrayViewConst_<T1,X1>& a1, const ArrayViewConst_<T2,X2>& a2)
+{ return a2 < a1; }
+/** The less than or equal operator is implemented using the greater than
+operator. @relates Array_ **/
+template <class T1, class X1, class T2, class X2> inline bool
+operator<=(const ArrayViewConst_<T1,X1>& a1, const ArrayViewConst_<T2,X2>& a2)
+{ return !(a1 > a2); }
+
+/** An Array_<T1> and an std::vector<T2> are equal if and only if they are the
+same size() and each element compares equal using an operator T1==T2.
+ at relates Array_ **/
+template <class T1, class X1, class T2, class A2> inline bool
+operator==(const ArrayViewConst_<T1,X1>& a1, const std::vector<T2,A2>& v2)
+{ return a1 == ArrayViewConst_<T2,size_t>(v2); }
+
+/** An std::vector<T1> and an Array_<T2> are equal if and only if they are the
+same size() and each element compares equal using an operator T2==T1.
+ at relates Array_ **/
+template <class T1, class A1, class T2, class X2> inline bool
+operator==(const std::vector<T1,A1>& v1, const ArrayViewConst_<T2,X2>& a2)
+{ return a2 == v1; }
+
+/** The not equal operator is implemented using the equal operator.
+ at relates Array_ **/
+template <class T1, class X1, class T2, class A2> inline bool
+operator!=(const ArrayViewConst_<T1,X1>& a1, const std::vector<T2,A2>& v2)
+{ return !(a1 == v2); }
+/** The not equal operator is implemented using the equal operator.
+ at relates Array_ **/
+template <class T1, class A1, class T2, class X2> inline bool
+operator!=(const std::vector<T1,A1>& v1, const ArrayViewConst_<T2,X2>& a2)
+{ return !(a2 == v1); }
+
+/** An Array_<T1> and std::vector<T2> are ordered lexicographically; that is,
+by first differing element or by length if there are no differing elements up
+to the length of the shorter container (in which case the shorter one is
+"less than" the longer). This depends on having working element operators
+T1==T2 and T1<T2. @relates Array_ **/
+template <class T1, class X1, class T2, class A2> inline bool
+operator<(const ArrayViewConst_<T1,X1>& a1, const std::vector<T2,A2>& v2)
+{ return a1 < ArrayViewConst_<T2,size_t>(v2); }
+
+/** An std::vector<T1> and Array_<T2> are ordered lexicographically; that is,
+by first differing element or by length if there are no differing elements up
+to the length of the shorter container (in which case the shorter one is
+"less than" the longer). This depends on having working element operators
+T1==T2 and T1<T2. @relates Array_ **/
+template <class T1, class A1, class T2, class X2> inline bool
+operator<(const std::vector<T1,A1>& v1, const ArrayViewConst_<T2,X2>& a2)
+{ return ArrayViewConst_<T1,size_t>(v1) < a2; }
+
+/** The greater than or equal operator is implemented using the less than
+operator. @relates Array_ **/
+template <class T1, class X1, class T2, class A2> inline bool
+operator>=(const ArrayViewConst_<T1,X1>& a1, const std::vector<T2,A2>& v2)
+{ return !(a1 < v2); }
+/** The greater than or equal operator is implemented using the less than
+operator. @relates Array_ **/
+template <class T1, class A1, class T2, class X2> inline bool
+operator>=(const std::vector<T1,A1>& v1, const ArrayViewConst_<T2,X2>& a2)
+{ return !(v1 < a2); }
+
+/** The greater than operator is implemented by using less than with the
+arguments reversed, meaning the elements must have working comparison
+operators of the form T2==T1 and T2<T1. @relates Array_ **/
+template <class T1, class X1, class T2, class A2> inline bool
+operator>(const ArrayViewConst_<T1,X1>& a1, const std::vector<T2,A2>& v2)
+{ return v2 < a1; }
+/** The greater than operator is implemented by using less than with the
+arguments reversed, meaning the elements must have working comparison
+operators of the form T2==T1 and T2<T1. @relates Array_ **/
+template <class T1, class A1, class T2, class X2> inline bool
+operator>(const std::vector<T1,A1>& v1, const ArrayViewConst_<T2,X2>& a2)
+{ return a2 < v1; }
+
+/** The less than or equal operator is implemented using the greater than
+operator. @relates Array_ **/
+template <class T1, class X1, class T2, class A2> inline bool
+operator<=(const ArrayViewConst_<T1,X1>& a1, const std::vector<T2,A2>& v2)
+{ return !(a1 > v2); }
+/** The less than or equal operator is implemented using the greater than
+operator. @relates Array_ **/
+template <class T1, class A1, class T2, class X2> inline bool
+operator<=(const std::vector<T1,A1>& v1, const ArrayViewConst_<T2,X2>& a2)
+{ return !(v1 > a2); }
+
+/*@}*/
+
+} // namespace SimTK
+
+namespace std {
+/** This is a specialization of the STL std::swap() algorithm which uses the
+constant time built-in swap() member of the Array_ class.
+ at relates SimTK::Array_ **/
+template <class T, class X> inline void
+swap(SimTK::Array_<T,X>& a1, SimTK::Array_<T,X>& a2) {
+ a1.swap(a2);
+}
+
+} // namespace std
+
+#endif // SimTK_SimTKCOMMON_ARRAY_H_
diff --git a/SimTKcommon/include/SimTKcommon/internal/AtomicInteger.h b/SimTKcommon/include/SimTKcommon/internal/AtomicInteger.h
new file mode 100644
index 0000000..cf2ac0e
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/AtomicInteger.h
@@ -0,0 +1,75 @@
+#ifndef SimTK_SimTKCOMMON_ATOMIC_INTEGER_H_
+#define SimTK_SimTKCOMMON_ATOMIC_INTEGER_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/internal/common.h"
+
+namespace SimTK {
+
+/**
+ * This class functions exactly like an int, except that the following operators are atomic:
+ * ++, --, +=, -=, *=, /=, %=, &=, |=, ^=, <<=, and >>=. For example, suppose myInt is an AtomicInteger
+ * that initially has the value 5. If two threads both evaluate the expression ++myInt, it is guaranteed
+ * that one thread will get the value 6 and the other will get the value 7, and myInt will
+ * equal 7 afterward. This would not be true for an ordinary int.
+ *
+ * On most processors, this form of thread-safety can be implemented in a lightweight way
+ * which is much faster than acquiring a lock. When possible, this class uses these mechanisms
+ * to achieve maximum efficiency. On platforms that do not support atomic operations directly
+ * it uses locking, which is slower but still guaranteed to produce a correct result.
+ */
+
+class SimTK_SimTKCOMMON_EXPORT AtomicInteger {
+public:
+ AtomicInteger();
+ AtomicInteger(int value);
+ ~AtomicInteger();
+ AtomicInteger& operator=(int value);
+ operator int() const;
+ int operator++();
+ int operator++(int);
+ int operator--();
+ int operator--(int);
+ AtomicInteger& operator+=(int value);
+ AtomicInteger& operator-=(int value);
+ AtomicInteger& operator*=(int value);
+ AtomicInteger& operator/=(int value);
+ AtomicInteger& operator%=(int value);
+ AtomicInteger& operator&=(int value);
+ AtomicInteger& operator|=(int value);
+ AtomicInteger& operator^=(int value);
+ AtomicInteger& operator<<=(int value);
+ AtomicInteger& operator>>=(int value);
+ bool operator==(int value) const;
+ bool operator!=(int value) const;
+private:
+ void* atomic;
+};
+
+std::ostream& operator<<(std::ostream& stream, const AtomicInteger& value);
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_ATOMIC_INTEGER_H_
diff --git a/SimTKcommon/include/SimTKcommon/internal/ClonePtr.h b/SimTKcommon/include/SimTKcommon/internal/ClonePtr.h
new file mode 100644
index 0000000..324e862
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/ClonePtr.h
@@ -0,0 +1,225 @@
+#ifndef SimTK_SimTKCOMMON_CLONE_PTR_H_
+#define SimTK_SimTKCOMMON_CLONE_PTR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+namespace SimTK {
+
+/** Wrap a pointer to an abstract base class in a way that makes it behave like
+a concrete class. This is similar to std::unique_ptr in that it does not permit
+shared ownership of the object. However, unlike std::unique_ptr, %ClonePtr
+supports copy and assigment operations, by insisting that the contained object
+have a clone() method that returns a pointer to a heap-allocated deep copy of
+the <em>concrete</em> object.
+
+We define operator==() and operator<() here that delegate to the contained
+object; if you want to use those the contained type must support that
+operator.
+
+This class is entirely inline and has no computational or space overhead; it
+contains just a single pointer and does no reference counting.
+
+ at see ReferencePtr **/
+template <class T> class ClonePtr {
+public:
+ typedef T element_type;
+ typedef T* pointer;
+ typedef T& reference;
+
+ /** Default constructor creates an empty object. **/
+ ClonePtr() : p(0) { }
+ /** Given a pointer to a writable heap-allocated object, take over
+ ownership of that object. **/
+ explicit ClonePtr(T* obj) : p(obj) { }
+ /** Given a pointer to a writable heap-allocated object, take over
+ ownership of that object and set the original pointer to null. **/
+ explicit ClonePtr(T** obj) : p(*obj) { *obj=0; }
+ /** Given a pointer to a read-only object, create a new heap-allocated
+ copy of that object via its clone() method and make this %ClonePtr
+ object the owner of the copy. Ownership of the original object is not
+ affected. If the supplied pointer is null, the resulting %ClonePtr
+ object is empty. **/
+ explicit ClonePtr(const T* obj) : p(obj?obj->clone():0) { }
+ /** Given a read-only reference to an object, create a new heap-allocated
+ copy of that object via its clone() method and make this %ClonePtr
+ object the owner of the copy. Ownership of the original object is not
+ affected. **/
+ explicit ClonePtr(const T& obj) : p(&obj?obj.clone():0) { }
+ /** Copy constructor is deep; the new %ClonePtr object contains a new
+ copy of the object in the source, created via the source object's clone()
+ method. If the source container is empty this one will be empty also. **/
+ ClonePtr(const ClonePtr& c) : p(c.p?c.p->clone():0) { }
+ /** Copy assignment replaces the currently-held object by a heap-allocated
+ copy of the object held in the source container. The copy is created using
+ the source object's clone() method. The currently-held object is deleted.
+ If the source container is empty this one will be empty after the
+ assignment. **/
+ ClonePtr& operator=(const ClonePtr& c)
+ { reset(c.p?c.p->clone():0); return *this; }
+ /** This form of assignment replaces the currently-held object by a
+ heap-allocated copy of the source object. The copy is created using the
+ source object's clone() method. The currently-held object is deleted. **/
+ ClonePtr& operator=(const T& t)
+ { reset(&t ? t.clone() :0); return *this; }
+ /** This form of assignment replaces the currently-held object by the given
+ source object and takes over ownership of the source object. The
+ currently-held object is deleted. **/
+ ClonePtr& operator=(T* tp)
+ { reset(tp); return *this; }
+
+ /** Destructor deletes the referenced object. **/
+ ~ClonePtr() { delete p; }
+
+ /** Compare the contained objects for equality using the contained
+ objects' operator==() operator. If both containers are empty or both
+ refer to the same object they will test equal; if only one is empty they
+ will test not equal. Otherwise the objects are compared. **/
+ bool operator==(const ClonePtr& other) const {
+ if (p == other.p) return true; // same object or both empty
+ if (empty() || other.empty()) return false;
+ return getRef()==other.getRef();
+ }
+ /** Compare the contained objects for inequality using operator==(). **/
+ bool operator!=(const ClonePtr& other) const {return !((*this)==other);}
+
+ /** Provide an ordering for use in sorted containers using the contained
+ objects' operator<(). If both containers are empty or both
+ refer to the same object they will test equal; if only one is empty that
+ one is considered less than the other one. Otherwise the objects are
+ compared using T::operator<(). **/
+ bool operator<(const ClonePtr& other) const {
+ if (p == other.p) return false; // same object or both empty
+ if (empty()) return true; // empty < !empty
+ if (other.empty()) return false; // !empty > empty
+ return getRef() < other.getRef();
+ }
+
+ /** Dereference a const pointer to the contained object. This will fail if
+ the container is empty. **/
+ const T* operator->() const { return &getRef(); }
+ /** Dereference a writable pointer to the contained object. This will fail
+ if the container is empty. **/
+ T* operator->() { return &updRef(); }
+
+ /** This "dereference" operator returns a const reference to the contained
+ object. This will fail if the container is empty. **/
+ const T& operator*() const { return getRef(); }
+ /** This "dereference" operator returns a writable reference to the
+ contained object. This will fail if the container is empty. **/
+ T& operator*() { return updRef(); }
+
+ /** This "address of" operator returns a const pointer to the contained
+ object (or null if none). **/
+ const T* operator&() const { return p; }
+ /** This "address of" operator returns a writable pointer to the contained
+ object (or null if none). **/
+ T* operator&() { return p; }
+
+ /** This is an implicit conversion from %ClonePtr\<T> to a const
+ reference to the contained object. This will fail if the container is
+ empty. **/
+ operator const T&() const { return getRef(); }
+ /** This is an implicit conversion from %ClonePtr\<T> to a writable
+ reference to the contained object. **/
+ operator T&() { return updRef(); }
+
+ /** This is an implicit conversion to type bool that returns true if
+ the container is non-null (that is, not empty). **/
+ operator bool() const { return !empty(); }
+
+ /** Return a writable pointer to the contained object if any, or null.
+ You can use the "address of" operator\&() instead if you prefer. **/
+ T* updPtr() { return p; }
+ /** Return a const pointer to the contained object if any, or null.
+ You can use the "address of" operator\&() instead if you prefer. **/
+ const T* getPtr() const { return p; }
+
+ /** Return a writable reference to the contained object. Don't call this
+ this container is empty. There is also an implicit conversion to reference
+ that allows %ClonePtr\<T> to be used as though it were a T\&. **/
+ T& updRef() {
+ SimTK_ERRCHK(p!=0, "ClonePtr::updRef()",
+ "An attempt was made to dereference a null pointer.");
+ return *p;
+ }
+ /** Return a const reference to the contained object. Don't call this if
+ this container is empty. There is also an implicit conversion to reference
+ that allows %ClonePtr\<T> to be used as though it were a T\&. **/
+ const T& getRef() const {
+ SimTK_ERRCHK(p!=0, "ClonePtr::getRef()",
+ "An attempt was made to dereference a null pointer.");
+ return *p;
+ }
+
+ /** Return true if this container is empty. **/
+ bool empty() const { return p==0; }
+ /** Make this container empty, deleting the currently contained object if
+ there is one. **/
+ void clear() { delete p; p=0; }
+ /** Extract the object from this container, leaving the container empty
+ and transferring ownership to the caller. A pointer to the object is
+ returned. No destruction occurs. **/
+ T* release() { T* x=p; p=0; return x; }
+ /** Replace the contents of this container with the supplied heap-allocated
+ object, taking over ownership of that object and deleting the current one
+ first if necessary. Nothing happens if the supplied pointer is the same
+ as the one already stored. **/
+ void reset(T* tp) { if (tp!=p) {delete p; p=tp;} }
+ /** Replace the contents of this container with the supplied heap-allocated
+ object, taking over ownership of that object, deleting the current one
+ first if necessary, and setting the caller's supplied pointer to null.
+ If the supplied pointer is the same as the one we're already storing, then
+ we just set the caller's pointer to null and return. **/
+ void reset(T** tpp) {
+ if (*tpp!=p) {delete p; p=*tpp;}
+ *tpp=0;
+ }
+ /** Swap the contents of this %ClonePtr with another one, with ownership
+ changing hands but no copying performed. **/
+ void swap(ClonePtr& other) {
+ T* otherp = other.release();
+ other.reset(p);
+ reset(otherp);
+ }
+
+private:
+ // Warning: ClonePtr must be exactly the same size as type T*. That way
+ // one can reinterpret_cast a T* to a ClonePtr<T> when needed.
+ T* p;
+};
+
+} // namespace SimTK
+
+namespace std {
+/** This is a specialization of the STL std::swap() algorithm which uses the
+cheap built-in swap() member of the ClonePtr class.
+ at relates SimTK::ClonePtr **/
+template <class T> inline void
+swap(SimTK::ClonePtr<T>& p1, SimTK::ClonePtr<T>& p2) {
+ p1.swap(p2);
+}
+
+} // namespace std
+
+#endif // SimTK_SimTKCOMMON_CLONE_PTR_H_
diff --git a/SimTKcommon/include/SimTKcommon/internal/Exception.h b/SimTKcommon/include/SimTKcommon/internal/Exception.h
new file mode 100644
index 0000000..ba6e804
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/Exception.h
@@ -0,0 +1,333 @@
+#ifndef SimTK_SimTKCOMMON_EXCEPTION_H_
+#define SimTK_SimTKCOMMON_EXCEPTION_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+#include "SimTKcommon/internal/common.h"
+
+#include <string>
+#include <iostream>
+#include <exception>
+#include <cstdarg>
+#include <cstdio>
+
+namespace SimTK {
+
+namespace Exception {
+
+// SimTK::Exception::Base
+class Base : public std::exception {
+public:
+ explicit Base(const char* fn="<UNKNOWN>", int ln=0)
+ : fileName(fn), lineNo(ln) { }
+ virtual ~Base() throw() { }
+ const std::string& getMessage() const { return msg; }
+ const std::string& getMessageText() const { return text; }
+
+ // override virtual function from std::exception
+ const char* what() const throw() {return getMessage().c_str();}
+protected:
+ void setMessage(const std::string& msgin) {
+ text = msgin;
+ msg = "SimTK Exception thrown at " + where() + ":\n " + msgin;
+ }
+private:
+ std::string fileName; // where the exception was thrown
+ int lineNo;
+ std::string msg; // a message formatted for display by catcher
+ std::string text; // the original passed-in text
+
+ static std::string shortenFileName(const std::string& fn)
+ { std::string::size_type pos = fn.find_last_of("/\\");
+ if (pos+1>=fn.size()) pos=0;
+ return std::string(fn,(int)(pos+1),(int)(fn.size()-(pos+1)));
+ }
+
+ std::string where() const {
+ char buf[32];
+ sprintf(buf,"%d",lineNo);
+ return shortenFileName(fileName) + ":" + std::string(buf);
+ }
+};
+
+/// This is for reporting internally-detected bugs only, not problems induced by
+/// confused users (that is, it is for confused developers instead). The exception
+/// message accepts printf-style arguments and should contain lots of useful
+/// information for developers. Don't throw
+/// this exception directly; use one of the family of SimTK_ASSERT and
+/// SimTK_ASSERT_ALWAYS macros.
+class Assert : public Base {
+public:
+ Assert(const char* fn, int ln, const char* assertion,
+ const char* fmt ...) : Base(fn,ln)
+ {
+ char buf[1024];
+ va_list args;
+ va_start(args, fmt);
+ vsprintf(buf, fmt, args);
+
+ setMessage("Internal bug detected: " + std::string(buf)
+ + "\n (Assertion '" + std::string(assertion) + "' failed).\n"
+ " Please file an Issue at https://github.com/simbody/simbody/issues.\n"
+ " Include the above information and anything else needed to reproduce the problem.");
+ va_end(args);
+ }
+ virtual ~Assert() throw() { }
+};
+
+/// This is for reporting errors occurring during execution of SimTK core methods,
+/// beyond those caused by mere improper API arguments, which should be reported with
+/// APIArgcheck instead. Nor is this intended for detection of internal
+/// bugs; use Assert instead for that. It is expected that this error resulted from
+/// something the API user did, so the messages should be suitable for reporting to
+/// that programmer. The exception message accepts printf-style arguments and should
+/// contain lots of useful information for the API user. Don't throw this exception
+/// directly; use one of the family SimTK_ERRCHK and SimTK_ERRCHK_ALWAYS macros.
+class ErrorCheck : public Base {
+public:
+ ErrorCheck(const char* fn, int ln, const char* assertion,
+ const char* whereChecked, // e.g., ClassName::methodName()
+ const char* fmt ...) : Base(fn,ln)
+ {
+ char buf[1024];
+ va_list args;
+ va_start(args, fmt);
+ vsprintf(buf, fmt, args);
+
+ setMessage("Error detected by Simbody method "
+ + std::string(whereChecked) + ": "
+ + std::string(buf)
+ + "\n (Required condition '" + std::string(assertion) + "' was not met.)\n");
+ va_end(args);
+ }
+ virtual ~ErrorCheck() throw() { }
+};
+
+/// This is for reporting problems detected by checking the caller's supplied arguments
+/// to a SimTK API method. Messages should be suitable for SimTK API users. This is not
+/// intended for detection of internal bugs where a SimTK developer passed bad arguments
+/// to some internal routine -- use Assert instead for that. The exception message
+/// accepts printf-style arguments and should contain useful information for the API user.
+/// Don't throw this exception directly; use one of the family SimTK_APIARGCHECK and
+/// SimTK_APIARGCHECK_ALWAYS macros.
+class APIArgcheckFailed : public Base {
+public:
+ APIArgcheckFailed(const char* fn, int ln, const char* assertion,
+ const char* className, const char* methodName,
+ const char* fmt ...) : Base(fn,ln)
+ {
+ char buf[1024];
+ va_list args;
+ va_start(args, fmt);
+ vsprintf(buf, fmt, args);
+ setMessage("Bad call to Simbody API method "
+ + std::string(className) + "::" + std::string(methodName) + "(): "
+ + std::string(buf)
+ + "\n (Required condition '" + std::string(assertion) + "' was not met.)");
+ va_end(args);
+ }
+ virtual ~APIArgcheckFailed() throw() { }
+};
+
+
+class IndexOutOfRange : public Base {
+public:
+ IndexOutOfRange(const char* fn, int ln, const char* indexName,
+ long long lb, long long index, long long ub, const char* where)
+ : Base(fn,ln)
+ {
+ char buf[1024];
+
+ sprintf(buf, "Index out of range in %s: expected %lld <= %s < %lld but %s=%lld.",
+ where,lb,indexName,ub,indexName,index);
+ setMessage(std::string(buf));
+ }
+ virtual ~IndexOutOfRange() throw() { }
+};
+
+class SizeOutOfRange : public Base {
+public:
+ SizeOutOfRange(const char* fn, int ln, const char* szName,
+ unsigned long long sz, unsigned long long maxsz, const char* where)
+ : Base(fn,ln)
+ {
+ char buf[1024];
+
+ sprintf(buf, "Size out of range in %s: expected 0 <= %s <= %llu but %s=%llu.",
+ where,szName,maxsz,szName,sz);
+ setMessage(std::string(buf));
+ }
+ virtual ~SizeOutOfRange() throw() { }
+};
+
+class SizeWasNegative : public Base {
+public:
+ SizeWasNegative(const char* fn, int ln, const char* szName,
+ unsigned long long sz, const char* where)
+ : Base(fn,ln)
+ {
+ char buf[1024];
+
+ sprintf(buf, "Size argument was negative in %s: expected 0 <= %s but %s=%llu.",
+ where,szName,szName,sz);
+ setMessage(std::string(buf));
+ }
+ virtual ~SizeWasNegative() throw() { }
+};
+
+class ValueOutOfRange : public Base {
+public:
+ ValueOutOfRange(const char* fn, int ln, const char* valueName,
+ double lowerBound, double value, double upperBound,
+ const char* where)
+ : Base(fn,ln)
+ {
+ char buf[1024];
+
+ sprintf(buf, "Value out of range in %s: expected %g <= %s <= %g but %s=%g.",
+ where,lowerBound,valueName,upperBound,valueName,value);
+ setMessage(std::string(buf));
+ }
+ virtual ~ValueOutOfRange() throw() { }
+};
+
+class ValueWasNegative : public Base {
+public:
+ ValueWasNegative(const char* fn, int ln, const char* valueName,
+ double value, const char* where)
+ : Base(fn,ln)
+ {
+ char buf[1024];
+
+ sprintf(buf, "Expected non-negative value for %s in %s but got %g.",
+ valueName,where,value);
+ setMessage(std::string(buf));
+ }
+ virtual ~ValueWasNegative() throw() { }
+};
+
+class UnimplementedMethod : public Base {
+public:
+ UnimplementedMethod(const char* fn, int ln, std::string methodName)
+ : Base(fn,ln)
+ {
+ setMessage("The method " + methodName
+ + "is not yet implemented. Please post to the Simbody forum"
+ " to find a workaround or request implementation.");
+ }
+ virtual ~UnimplementedMethod() throw() { }
+};
+
+class UnimplementedVirtualMethod : public Base {
+public:
+ UnimplementedVirtualMethod(const char* fn, int ln,
+ std::string baseClass, std::string methodName)
+ : Base(fn,ln)
+ {
+ setMessage("The base class " + baseClass +
+ " dummy implementation of method " + methodName
+ + "() was invoked because a derived class did not provide an implementation.");
+ }
+ virtual ~UnimplementedVirtualMethod() throw() { }
+};
+
+class IncompatibleValues : public Base {
+public:
+ IncompatibleValues(const char* fn, int ln, std::string src, std::string dest) : Base(fn,ln)
+ {
+ setMessage("Attempt to assign a Value<"+src+"> to a Value<"+dest+">");
+ }
+ virtual ~IncompatibleValues() throw() { }
+};
+
+class OperationNotAllowedOnView : public Base {
+public:
+ OperationNotAllowedOnView(const char* fn, int ln, const std::string& op) : Base(fn,ln)
+ {
+ setMessage("Operation '" + op + "' allowed only for owners, not views");
+ }
+ virtual ~OperationNotAllowedOnView() throw() { }
+};
+
+class OperationNotAllowedOnOwner : public Base {
+public:
+ OperationNotAllowedOnOwner(const char* fn, int ln, const std::string& op) : Base(fn,ln)
+ {
+ setMessage("Operation '" + op + "' allowed only for views, not owners");
+ }
+ virtual ~OperationNotAllowedOnOwner() throw() { }
+};
+
+class OperationNotAllowedOnNonconstReadOnlyView : public Base {
+public:
+ OperationNotAllowedOnNonconstReadOnlyView(const char* fn, int ln, const std::string& op) : Base(fn,ln)
+ {
+ setMessage("Operation '" + op + "' not allowed on non-const readonly view");
+ }
+ virtual ~OperationNotAllowedOnNonconstReadOnlyView() throw() { }
+};
+
+// SimTK::Exception::Cant
+class Cant : public Base {
+public:
+ Cant(const char* fn, int ln, const std::string& s) : Base(fn,ln)
+ {
+ setMessage("Can't perform operation: " + s);
+ }
+ virtual ~Cant() throw() { }
+};
+
+} // namespace Exception
+} // namespace SimTK
+
+#define SimTK_THROW(exc) \
+ throw exc(__FILE__, __LINE__)
+#define SimTK_THROW1(exc,a1) \
+ throw exc(__FILE__, __LINE__,a1)
+#define SimTK_THROW2(exc,a1,a2) \
+ throw exc(__FILE__, __LINE__,a1,a2)
+#define SimTK_THROW3(exc,a1,a2,a3) \
+ throw exc(__FILE__, __LINE__,a1,a2,a3)
+#define SimTK_THROW4(exc,a1,a2,a3,a4) \
+ throw exc(__FILE__, __LINE__,a1,a2,a3,a4)
+#define SimTK_THROW5(exc,a1,a2,a3,a4,a5) \
+ throw exc(__FILE__, __LINE__,a1,a2,a3,a4,a5)
+#define SimTK_THROW6(exc,a1,a2,a3,a4,a5,a6) \
+ throw exc(__FILE__, __LINE__,a1,a2,a3,a4,a5,a6)
+#define SimTK_THROW7(exc,a1,a2,a3,a4,a5,a6,a7) \
+ throw exc(__FILE__, __LINE__,a1,a2,a3,a4,a5,a6,a7)
+#define SimTK_THROW8(exc,a1,a2,a3,a4,a5,a6,a7,a8) \
+ throw exc(__FILE__, __LINE__,a1,a2,a3,a4,a5,a6,a7,a8)
+#define SimTK_THROW9(exc,a1,a2,a3,a4,a5,a6,a7,a8,a9) \
+ throw exc(__FILE__, __LINE__,a1,a2,a3,a4,a5,a6,a7,a8,a9)
+#define SimTK_THROW10(exc,a1,a2,a3,a4,a5,a6,a7,a8,a9,a10) \
+ throw exc(__FILE__, __LINE__,a1,a2,a3,a4,a5,a6,a7,a8,a9,a10)
+
+#endif // SimTK_SimTKCOMMON_EXCEPTION_H_
+
diff --git a/SimTKcommon/include/SimTKcommon/internal/ExceptionMacros.h b/SimTKcommon/include/SimTKcommon/internal/ExceptionMacros.h
new file mode 100644
index 0000000..2ea4d88
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/ExceptionMacros.h
@@ -0,0 +1,386 @@
+#ifndef SimTK_SimTKCOMMON_EXCEPTION_MACROS_H_
+#define SimTK_SimTKCOMMON_EXCEPTION_MACROS_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This file contains macros which are convenient to use for
+ * sprinkling error checking around liberally in SimTK programs, a
+ * practice which is highly encouraged. You can think of this as
+ * a generalization of the standard assert() macro. By default,
+ * these macros evaporate completely in a release build, but are
+ * present in any debug build. Macros are also provided which are
+ * always present in cases where the error checking is not a
+ * performance problem, and those should be used in preference
+ * to the disappearing ones when appropriate. Also, you can force
+ * the disappearing macros to remain present on a file-by-file basis,
+ * primarily for use in debugging those annoying problems which only
+ * occur in release builds and won't reproduce in a debug build.
+ *
+ * Most macros have a similar structure, something like this:
+ * <pre>
+ * SimTK_MACRONAME[nargs][_ALWAYS](cond, printfString [, args...])
+ * for example
+ * SimTK_ASSERT3(lower<count && count<upper,
+ * "expected %d < count < %d but count=%d",
+ * lower, upper, count);
+ * or
+ * SimTK_ASSERT3_ALWAYS(lower<count && count<upper,
+ * "expected %d < count < %d but count=%d",
+ * lower, upper, count);
+ * </pre>
+ * To override the disappearance of the non-ALWAYS macros, your
+ * compilation should define a preprocessor symbols like
+ * SimTK_KEEP_ASSERT, SimTK_KEEP_ERRCHK, etc.
+ *
+ * These macros will also capture the current file name and line
+ * number for reporting to developers when appropriate, and those
+ * with a condition that failed will include the condition in the
+ * message.
+ *
+ * Note that these are *global* symbols, so we use the reserved
+ * SimTK_ name prefix (since we can't use the SimTK:: namespace
+ * for macros) to attempt to avoid pollution of user programs.
+ *
+ * We distinguish between macros which are used as internal
+ * "bugcatchers" and those which are used to report errors to
+ * API users. The C++ exception mechanism is used in both circumstances
+ * but the meaning and intended audience is quite different. Any
+ * macro with 'ASSERT' in the name represents an internal error
+ * which cannot be attributed to user misbehavior. Other macros
+ * are for communicating with users. Those need to be carefully documented
+ * so that users can selectively catch the exceptions when appropriate.
+ */
+
+#include "SimTKcommon/internal/common.h"
+#include "SimTKcommon/internal/Exception.h"
+
+#include <string>
+#include <iostream>
+#include <exception>
+
+// --------------------------------- RANGECHECK --------------------------------
+// These exceptions are to be used for situations in which a user of a SimTK
+// API method screws up by providing bad indices or dimensions. These are special
+// cases of the more general APIARGCHECK macros, providing "canned" error messages
+// for several common situations. Although there are several different macro
+// names here, all are controlled by SimTK_KEEP_RANGECHECK to allow enabling of
+// these index- and size-validating tests together in Release mode.
+//
+// INDEXCHECK: Note that we allow the index to be equal to the lower
+// bound (zero) but it must be strictly less than the upper bound.
+// SIZECHECK: A size or size expression must be >= 0 and less than OR EQUAL
+// to the maximum size.
+// SIZECHECK_NONNEG: A size argument must be >= 0.
+// VALUECHECK: A floating point is required to be within a certain range.
+// VALUECHECK_NONNEG: A floating point argument must be non-negative.
+//
+// TODO: SHAPECHECK, DOMAINCHECK
+// -----------------------------------------------------------------------------
+
+// This is a rangecheck that is always present, even in Release mode. This may be
+// applied both to signed and unsigned types (the latter are always nonnegative) so
+// to avoid warnings we use the isIndexInRange() method which doesn't perform
+// a nonnegativity check on unsigned quantities.
+#define SimTK_INDEXCHECK_ALWAYS(ix,ub,where) \
+ do{if(!isIndexInRange((ix),(ub)))SimTK_THROW5(SimTK::Exception::IndexOutOfRange, \
+ #ix,0,(ix),(ub),(where));}while(false)
+
+// This is a rangecheck that is always present, even in Release mode. This may be
+// applied both to signed and unsigned types (the latter are always nonnegative) so
+// to avoid warnings we use the isSizeInRange() method which doesn't perform
+// a nonnegativity check on unsigned quantities.
+#define SimTK_SIZECHECK_ALWAYS(sz,maxsz,where) \
+ do{if(!isSizeInRange((sz),(maxsz)))SimTK_THROW4(SimTK::Exception::SizeOutOfRange, \
+ #sz,(sz),(maxsz),(where));}while(false)
+
+// This is a rangecheck that is always present, even in Release mode. Use
+// isNonnegative() here in case sz is an unsigned type to avoid compiler
+// warning.
+#define SimTK_SIZECHECK_NONNEG_ALWAYS(sz,where) \
+ do{if(!isNonnegative(sz))SimTK_THROW3(SimTK::Exception::SizeWasNegative, \
+ #sz,(sz),(where));}while(false)
+
+ // Similar checks for floating point values.
+
+#define SimTK_VALUECHECK_ALWAYS(lb,val,ub,valName,where) \
+ do{if(!(lb)<=(val)&&(val)<=(ub)))SimTK_THROW5(SimTK::Exception::ValueOutOfRange, \
+ (valName),(lb),(val),(ub),(where));}while(false)
+
+
+#define SimTK_VALUECHECK_NONNEG_ALWAYS(val,valName,where) \
+ do{if((val)<0)SimTK_THROW3(SimTK::Exception::ValueWasNegative, \
+ (valName),(val),(where));}while(false)
+
+
+
+#if defined(NDEBUG) && !defined(SimTK_KEEP_RANGECHECK)
+ #define SimTK_INDEXCHECK(ix,ub,where)
+ #define SimTK_SIZECHECK(sz,maxsz,where)
+ #define SimTK_SIZECHECK_NONNEG(sz,where)
+ #define SimTK_VALUECHECK(lb,val,ub,valName,where)
+ #define SimTK_VALUECHECK_NONNEG(val,valName,where)
+#else
+ #define SimTK_INDEXCHECK(ix,ub,where) SimTK_INDEXCHECK_ALWAYS(ix,ub,where)
+ #define SimTK_SIZECHECK(sz,maxsz,where) SimTK_SIZECHECK_ALWAYS(sz,maxsz,where)
+ #define SimTK_SIZECHECK_NONNEG(sz,where) SimTK_SIZECHECK_NONNEG_ALWAYS(sz,where)
+ #define SimTK_VALUECHECK(lb,val,ub,valName,where) SimTK_VALUECHECK_ALWAYS(lb,val,ub,valName,where)
+ #define SimTK_VALUECHECK_NONNEG(val,valName,where) SimTK_VALUECHECK_NONNEG_ALWAYS(val,valName,where)
+#endif
+
+
+// --------------------------------- STAGECHECK --------------------------------
+// These exceptions are to be used for situations in which a
+// user of an API screws up by attempting to access something in the
+// state before it has been realized to the appropriate stage.
+//
+// STAGECHECK_TOPOLOGY_REALIZED: Check that realizeTopology() has been done
+// since the last topological change.
+// STAGECHECK_EQ: Check that the current stage is == a particular stage.
+// STAGECHECK_GE: Check that the current stage is >= a particular stage.
+// STAGECHECK_LT: Check that the current stage is < a particular stage.
+// STAGECHECK_RANGE: Check that lower <= stage <= upper.
+// -----------------------------------------------------------------------------
+
+// These are stagechecks that is always present, even in Release mode.
+#define SimTK_STAGECHECK_TOPOLOGY_REALIZED_ALWAYS(cond,objType,objName,methodNm) \
+ do{if(!(cond)) SimTK_THROW3(SimTK::Exception::RealizeTopologyMustBeCalledFirst, \
+ (objType),(objName),(methodNm));}while(false)
+#define SimTK_STAGECHECK_TOPOLOGY_VERSION_ALWAYS(sysTopoVersion, \
+ stateTopoVersion,objType,objName,methodNm) \
+ do{if((stateTopoVersion)!=(sysTopoVersion)) \
+ SimTK_THROW5(SimTK::Exception::StateAndSystemTopologyVersionsMustMatch, \
+ (objType),(objName),(methodNm), \
+ (int)(sysTopoVersion),(int)(stateTopoVersion));} \
+ while(false)
+#define SimTK_STAGECHECK_EQ_ALWAYS(currentStage,targetStage,methodNm) \
+ do{if((currentStage)!=(targetStage)) SimTK_THROW3(SimTK::Exception::StageIsWrong, \
+ (currentStage),(targetStage),(methodNm));}while(false)
+#define SimTK_STAGECHECK_GE_ALWAYS(currentStage,targetStage,methodNm) \
+ do{if(!((currentStage)>=(targetStage))) SimTK_THROW3(SimTK::Exception::StageTooLow, \
+ (currentStage),(targetStage),(methodNm));}while(false)
+#define SimTK_STAGECHECK_LT_ALWAYS(currentStage,targetStage,methodNm) \
+ do{if((currentStage)>=(targetStage)) SimTK_THROW3(SimTK::Exception::StageTooHigh, \
+ (currentStage),(targetStage),(methodNm));}while(false)
+#define SimTK_STAGECHECK_RANGE_ALWAYS(lower,current,upper,methodNm) \
+ do{if(!((lower)<=(current)&&(current)<=(upper))) SimTK_THROW4(SimTK::Exception::StageOutOfRange, \
+ (lower),(current),(upper),(methodNm));}while(false)
+
+// This one is present only in Debug mode or if SimTK_KEEP_STAGECHECK is explicitly defined.
+#if defined(NDEBUG) && !defined(SimTK_KEEP_STAGECHECK)
+ #define SimTK_STAGECHECK_TOPOLOGY_REALIZED(cond,objType,objName,methodName)
+ #define SimTK_STAGECHECK_TOPOLOGY_VERSIONS(sysTopoVersion,stateTopoVersion,\
+ objType,objName,methodNm)
+ #define SimTK_STAGECHECK_EQ(currentStage,targetStage,methodNm)
+ #define SimTK_STAGECHECK_GE(currentStage,targetStage,methodNm)
+ #define SimTK_STAGECHECK_LT(currentStage,targetStage,methodNm)
+ #define SimTK_STAGECHECK_RANGE(lower,current,upper,methodNm)
+#else
+ #define SimTK_STAGECHECK_TOPOLOGY_REALIZED(cond,objType,objName,methodName) \
+ SimTK_STAGECHECK_TOPOLOGY_REALIZED_ALWAYS(cond,objType,objName,methodName)
+ #define SimTK_STAGECHECK_TOPOLOGY_VERSION(sysTopoVersion,stateTopoVersion, \
+ objType,objName,methodNm) \
+ SimTK_STAGECHECK_TOPOLOGY_VERSION_ALWAYS(sysTopoVersion,stateTopoVersion,\
+ objType,objName,methodNm)
+ #define SimTK_STAGECHECK_EQ(currentStage,targetStage,methodNm) \
+ SimTK_STAGECHECK_EQ_ALWAYS(currentStage,targetStage,methodNm)
+ #define SimTK_STAGECHECK_GE(currentStage,targetStage,methodNm) \
+ SimTK_STAGECHECK_GE_ALWAYS(currentStage,targetStage,methodNm)
+ #define SimTK_STAGECHECK_LT(currentStage,targetStage,methodNm) \
+ SimTK_STAGECHECK_LE_ALWAYS(currentStage,targetStage,methodNm)
+ #define SimTK_STAGECHECK_RANGE(lower,current,upper,methodNm) \
+ SimTK_STAGECHECK_RANGE_ALWAYS(lower,current,upper,methodNm)
+#endif
+
+// -------------------------------- APIARGCHECK --------------------------------
+// These should be used to catch all manner of problems with the arguments passed
+// in an API user's call to a method that is part of a SimTK API. Note that these
+// are intended for direct consumption by an application programmer using a SimTK
+// API, so should be wordy and helpful. These macros accept printf-style format
+// strings and arguments of whatever are the appropriate types for those formats.
+// -----------------------------------------------------------------------------
+
+#define SimTK_APIARGCHECK_ALWAYS(cond,className,methodName,msg) \
+ do{if(!(cond))SimTK_THROW4(SimTK::Exception::APIArgcheckFailed, \
+ #cond,(className),(methodName),(msg)); \
+ }while(false)
+#define SimTK_APIARGCHECK1_ALWAYS(cond,className,methodName,fmt,a1) \
+ do{if(!(cond))SimTK_THROW5(SimTK::Exception::APIArgcheckFailed, \
+ #cond,(className),(methodName),(fmt),(a1)); \
+ }while(false)
+#define SimTK_APIARGCHECK2_ALWAYS(cond,className,methodName,fmt,a1,a2) \
+ do{if(!(cond))SimTK_THROW6(SimTK::Exception::APIArgcheckFailed, \
+ #cond,(className),(methodName),(fmt),(a1),(a2)); \
+ }while(false)
+#define SimTK_APIARGCHECK3_ALWAYS(cond,className,methodName,fmt,a1,a2,a3) \
+ do{if(!(cond))SimTK_THROW7(SimTK::Exception::APIArgcheckFailed, \
+ #cond,(className),(methodName),(fmt),(a1),(a2),(a3)); \
+ }while(false)
+#define SimTK_APIARGCHECK4_ALWAYS(cond,className,methodName,fmt,a1,a2,a3,a4) \
+ do{if(!(cond))SimTK_THROW8(SimTK::Exception::APIArgcheckFailed, \
+ #cond,(className),(methodName),(fmt),(a1),(a2),(a3),(a4)); \
+ }while(false)
+#define SimTK_APIARGCHECK5_ALWAYS(cond,className,methodName,fmt,a1,a2,a3,a4,a5) \
+ do{if(!(cond))SimTK_THROW9(SimTK::Exception::APIArgcheckFailed, \
+ #cond,(className),(methodName),(fmt),(a1),(a2),(a3),(a4),(a5)); \
+ }while(false)
+
+#if defined(NDEBUG) && !defined(SimTK_KEEP_APIARGCHECK)
+ #define SimTK_APIARGCHECK(cond,className,methodName,msg)
+ #define SimTK_APIARGCHECK1(cond,className,methodName,fmt,a1)
+ #define SimTK_APIARGCHECK2(cond,className,methodName,fmt,a1,a2)
+ #define SimTK_APIARGCHECK3(cond,className,methodName,fmt,a1,a2,a3)
+ #define SimTK_APIARGCHECK4(cond,className,methodName,fmt,a1,a2,a3,a4)
+ #define SimTK_APIARGCHECK5(cond,className,methodName,fmt,a1,a2,a3,a4,a5)
+#else
+ #define SimTK_APIARGCHECK(cond,className,methodName,msg) \
+ SimTK_APIARGCHECK_ALWAYS(cond,className,methodName,msg)
+ #define SimTK_APIARGCHECK1(cond,className,methodName,fmt,a1) \
+ SimTK_APIARGCHECK1_ALWAYS(cond,className,methodName,fmt,a1)
+ #define SimTK_APIARGCHECK2(cond,className,methodName,fmt,a1,a2) \
+ SimTK_APIARGCHECK2_ALWAYS(cond,className,methodName,fmt,a1,a2)
+ #define SimTK_APIARGCHECK3(cond,className,methodName,fmt,a1,a2,a3) \
+ SimTK_APIARGCHECK3_ALWAYS(cond,className,methodName,fmt,a1,a2,a3)
+ #define SimTK_APIARGCHECK4(cond,className,methodName,fmt,a1,a2,a3,a4) \
+ SimTK_APIARGCHECK4_ALWAYS(cond,className,methodName,fmt,a1,a2,a3,a4)
+ #define SimTK_APIARGCHECK5(cond,className,methodName,fmt,a1,a2,a3,a4,a5) \
+ SimTK_APIARGCHECK5_ALWAYS(cond,className,methodName,fmt,a1,a2,a3,a4,a5)
+#endif
+
+
+// ----------------------------------- ERRCHK ----------------------------------
+// ERRCHK: these should be used to catch all manner of problems that occur
+// during execution of an API user's request by a method that is part of
+// a SimTK API. Note that these are intended for direct consumption by
+// an application programmer using a SimTK API, so should be wordy and
+// helpful. These macros accept printf-style format strings and arguments
+// of whatever are the appropriate types for those formats.
+// -----------------------------------------------------------------------------
+
+#define SimTK_ERRCHK_ALWAYS(cond,whereChecked,msg) \
+ do{if(!(cond))SimTK_THROW3(SimTK::Exception::ErrorCheck, \
+ #cond,(whereChecked),(msg)); \
+ }while(false)
+#define SimTK_ERRCHK1_ALWAYS(cond,whereChecked,fmt,a1) \
+ do{if(!(cond))SimTK_THROW4(SimTK::Exception::ErrorCheck, \
+ #cond,(whereChecked),(fmt),(a1)); \
+ }while(false)
+#define SimTK_ERRCHK2_ALWAYS(cond,whereChecked,fmt,a1,a2) \
+ do{if(!(cond))SimTK_THROW5(SimTK::Exception::ErrorCheck, \
+ #cond,(whereChecked),(fmt),(a1),(a2)); \
+ }while(false)
+#define SimTK_ERRCHK3_ALWAYS(cond,whereChecked,fmt,a1,a2,a3) \
+ do{if(!(cond))SimTK_THROW6(SimTK::Exception::ErrorCheck, \
+ #cond,(whereChecked),(fmt),(a1),(a2),(a3)); \
+ }while(false)
+#define SimTK_ERRCHK4_ALWAYS(cond,whereChecked,fmt,a1,a2,a3,a4) \
+ do{if(!(cond))SimTK_THROW7(SimTK::Exception::ErrorCheck, \
+ #cond,(whereChecked),(fmt),(a1),(a2),(a3),(a4)); \
+ }while(false)
+#define SimTK_ERRCHK5_ALWAYS(cond,whereChecked,fmt,a1,a2,a3,a4,a5) \
+ do{if(!(cond))SimTK_THROW8(SimTK::Exception::ErrorCheck, \
+ #cond,(whereChecked),(fmt),(a1),(a2),(a3),(a4),(a5)); \
+ }while(false)
+#define SimTK_ERRCHK6_ALWAYS(cond,whereChecked,fmt,a1,a2,a3,a4,a5,a6) \
+ do{if(!(cond))SimTK_THROW9(SimTK::Exception::ErrorCheck, \
+ #cond,(whereChecked),(fmt),(a1),(a2),(a3),(a4),(a5),(a6)); \
+ }while(false)
+#define SimTK_ERRCHK7_ALWAYS(cond,whereChecked,fmt,a1,a2,a3,a4,a5,a6,a7) \
+ do{if(!(cond))SimTK_THROW10(SimTK::Exception::ErrorCheck, \
+ #cond,(whereChecked),(fmt),(a1),(a2),(a3),(a4),(a5),(a6),(a7)); \
+ }while(false)
+
+#if defined(NDEBUG) && !defined(SimTK_KEEP_ERRCHK)
+ #define SimTK_ERRCHK(cond,whereChecked,msg)
+ #define SimTK_ERRCHK1(cond,whereChecked,fmt,a1)
+ #define SimTK_ERRCHK2(cond,whereChecked,fmt,a1,a2)
+ #define SimTK_ERRCHK3(cond,whereChecked,fmt,a1,a2,a3)
+ #define SimTK_ERRCHK4(cond,whereChecked,fmt,a1,a2,a3,a4)
+ #define SimTK_ERRCHK5(cond,whereChecked,fmt,a1,a2,a3,a4,a5)
+ #define SimTK_ERRCHK6(cond,whereChecked,fmt,a1,a2,a3,a4,a5,a6)
+ #define SimTK_ERRCHK7(cond,whereChecked,fmt,a1,a2,a3,a4,a5,a6)
+#else
+ #define SimTK_ERRCHK(cond,whereChecked,msg) \
+ SimTK_ERRCHK_ALWAYS(cond,whereChecked,msg)
+ #define SimTK_ERRCHK1(cond,whereChecked,fmt,a1) \
+ SimTK_ERRCHK1_ALWAYS(cond,whereChecked,fmt,a1)
+ #define SimTK_ERRCHK2(cond,whereChecked,fmt,a1,a2) \
+ SimTK_ERRCHK2_ALWAYS(cond,whereChecked,fmt,a1,a2)
+ #define SimTK_ERRCHK3(cond,whereChecked,fmt,a1,a2,a3) \
+ SimTK_ERRCHK3_ALWAYS(cond,whereChecked,fmt,a1,a2,a3)
+ #define SimTK_ERRCHK4(cond,whereChecked,fmt,a1,a2,a3,a4) \
+ SimTK_ERRCHK4_ALWAYS(cond,whereChecked,fmt,a1,a2,a3,a4)
+ #define SimTK_ERRCHK5(cond,whereChecked,fmt,a1,a2,a3,a4,a5) \
+ SimTK_ERRCHK5_ALWAYS(cond,whereChecked,fmt,a1,a2,a3,a4,a5)
+ #define SimTK_ERRCHK6(cond,whereChecked,fmt,a1,a2,a3,a4,a5,a6) \
+ SimTK_ERRCHK6_ALWAYS(cond,whereChecked,fmt,a1,a2,a3,a4,a5,a6)
+ #define SimTK_ERRCHK7(cond,whereChecked,fmt,a1,a2,a3,a4,a5,a6,a7) \
+ SimTK_ERRCHK7_ALWAYS(cond,whereChecked,fmt,a1,a2,a3,a4,a5,a6,a7)
+#endif
+
+// ----------------------------------- ASSERT ----------------------------------
+// ASSERT: use this *only* for internal errors, that is, bugs. This must
+// not be used to catch usage errors by clients; if you want to catch
+// user errors use different exceptions.
+// -----------------------------------------------------------------------------
+
+// This is an assertion that is always active, even in Release mode.
+#define SimTK_ASSERT_ALWAYS(cond,msg) \
+ do{if(!(cond))SimTK_THROW2(SimTK::Exception::Assert,#cond,(msg));}while(false)
+#define SimTK_ASSERT1_ALWAYS(cond,msg,a1) \
+ do{if(!(cond))SimTK_THROW3(SimTK::Exception::Assert,#cond,(msg),(a1));}while(false)
+#define SimTK_ASSERT2_ALWAYS(cond,msg,a1,a2) \
+ do{if(!(cond))SimTK_THROW4(SimTK::Exception::Assert,#cond,(msg),(a1),(a2));}while(false)
+#define SimTK_ASSERT3_ALWAYS(cond,msg,a1,a2,a3) \
+ do{if(!(cond))SimTK_THROW5(SimTK::Exception::Assert,#cond,(msg),(a1),(a2),(a3));}while(false)
+#define SimTK_ASSERT4_ALWAYS(cond,msg,a1,a2,a3,a4) \
+ do{if(!(cond))SimTK_THROW6(SimTK::Exception::Assert,#cond,(msg),(a1),(a2),(a3),(a4));}while(false)
+#define SimTK_ASSERT5_ALWAYS(cond,msg,a1,a2,a3,a4,a5) \
+ do{if(!(cond))SimTK_THROW7(SimTK::Exception::Assert,#cond,(msg),(a1),(a2),(a3),(a4),(a5));}while(false)
+
+// Note: unlike the system assert() we're putting ours within the header guards.
+// So if you want to override NDEBUG do it at the *beginning* (that is, before
+// the first #include or #ifdef) of whatever compilation unit you are fiddling with.
+#if defined(NDEBUG) && !defined(SimTK_KEEP_ASSERT)
+ #define SimTK_ASSERT(cond,msg)
+ #define SimTK_ASSERT(cond,msg)
+ #define SimTK_ASSERT1(cond,msg,a1)
+ #define SimTK_ASSERT2(cond,msg,a1,a2)
+ #define SimTK_ASSERT3(cond,msg,a1,a2,a3)
+ #define SimTK_ASSERT4(cond,msg,a1,a2,a3,a4)
+ #define SimTK_ASSERT5(cond,msg,a1,a2,a3,a4,a5)
+#else
+ #define SimTK_ASSERT(cond,msg) SimTK_ASSERT_ALWAYS(cond,msg)
+ #define SimTK_ASSERT1(cond,msg,a1) SimTK_ASSERT1_ALWAYS(cond,msg,a1)
+ #define SimTK_ASSERT2(cond,msg,a1,a2) SimTK_ASSERT2_ALWAYS(cond,msg,a1,a2)
+ #define SimTK_ASSERT3(cond,msg,a1,a2,a3) SimTK_ASSERT3_ALWAYS(cond,msg,a1,a2,a3)
+ #define SimTK_ASSERT4(cond,msg,a1,a2,a3,a4) SimTK_ASSERT4_ALWAYS(cond,msg,a1,a2,a3,a4)
+ #define SimTK_ASSERT5(cond,msg,a1,a2,a3,a4,a5) SimTK_ASSERT5_ALWAYS(cond,msg,a1,a2,a3,a4,a5)
+#endif
+
+
+#endif // SimTK_SimTKCOMMON_EXCEPTION_MACROS_H_
+
+
+
diff --git a/SimTKcommon/include/SimTKcommon/internal/Fortran.h b/SimTKcommon/include/SimTKcommon/internal/Fortran.h
new file mode 100644
index 0000000..940c839
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/Fortran.h
@@ -0,0 +1,108 @@
+#ifndef SimTK_SimTKCOMMON_FORTRAN_H_
+#define SimTK_SimTKCOMMON_FORTRAN_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This header defines a set of macros which are useful for
+ * making calls to Fortran routines from C++ programs. Please don't
+ * take this as encouragement to use Fortran, but sometimes it
+ * is necessary.
+ */
+
+// Although we are currently triggering this off the OS, these
+// really are compiler dependencies.
+// (1) The calling convention (__stdcall for Windows)
+// (2) Name capitalization (either all-lower or all-uppercase)
+// (3) Is a trailing underscore added to the name?
+// (4) And ugliest, Fortran passes string lengths as a hidden
+// value parameter. On some compilers, that length follows
+// the string immediately. On others, all the lengths
+// appear at the end of the argument list, in the same
+// order as the strings to which they correspond.
+// Point (4) requires four ugly macros to be used, two in declarations
+// and two in calls to Fortran routines. One macro appears immediately
+// after each string, and the other appears at the end of the argument
+// list, repeated as many times as necessary. One or the other will
+// evaluate to nothing.
+
+// These macros should be used for whatever the expected default
+// Fortran behavior is for whatever Fortran is typically used in
+// conjunction with the current C++ compiler.
+#ifdef _WIN32
+ #define SimTK_FORTRAN_STDCALL __stdcall
+ #define SimTK_FORTRAN(x,X) X
+ #define SimTK_FORTRAN_STRLEN_FOLLOWS_DECL ,int
+ #define SimTK_FORTRAN_STRLEN_FOLLOWS_CALL(n) ,n
+ #define SimTK_FORTRAN_STRLEN_ATEND_DECL // nothing
+ #define SimTK_FORTRAN_STRLEN_ATEND_CALL(n)
+#else
+ #define SimTK_FORTRAN_STDCALL
+ #define SimTK_FORTRAN(x,X) x ## _
+ #define SimTK_FORTRAN_STRLEN_FOLLOWS_DECL // nothing
+ #define SimTK_FORTRAN_STRLEN_FOLLOWS_CALL(n)
+ #define SimTK_FORTRAN_STRLEN_ATEND_DECL ,int
+ #define SimTK_FORTRAN_STRLEN_ATEND_CALL(n) ,n
+#endif
+
+// These macros should be used for whatever our chosen LAPACK and
+// BLAS libraries will look like from here.
+#ifdef SimTK_USE_ACML_LAPACK
+ #ifdef _WIN32
+ #define SimTK_LAPACK_STDCALL __stdcall
+ #define SimTK_LAPACK(x,X) X
+ #define SimTK_LAPACK_STRLEN_FOLLOWS_DECL ,int
+ #define SimTK_LAPACK_STRLEN_FOLLOWS_CALL(n) ,n
+ #define SimTK_LAPACK_STRLEN_ATEND_DECL // nothing
+ #define SimTK_LAPACK_STRLEN_ATEND_CALL(n)
+ #else
+ #define SimTK_LAPACK_STDCALL
+ #define SimTK_LAPACK(x,X) x ## _
+ #define SimTK_LAPACK_STRLEN_FOLLOWS_DECL // nothing
+ #define SimTK_LAPACK_STRLEN_FOLLOWS_CALL(n)
+ #define SimTK_LAPACK_STRLEN_ATEND_DECL ,int
+ #define SimTK_LAPACK_STRLEN_ATEND_CALL(n) ,n
+ #endif
+#else // default assumes we're using libSimTKlapack
+ #define SimTK_LAPACK_STDCALL
+ #define SimTK_LAPACK(x,X) x ## _
+ #define SimTK_LAPACK_STRLEN_FOLLOWS_DECL // nothing
+ #define SimTK_LAPACK_STRLEN_FOLLOWS_CALL(n)
+ #define SimTK_LAPACK_STRLEN_ATEND_DECL ,int
+ #define SimTK_LAPACK_STRLEN_ATEND_CALL(n) ,n
+#endif
+
+// TODO: Currently this is unused and may not be needed anymore.
+// Call these routines to intialize the GNU Fortran RTL.
+//
+
+#ifdef USING_G77
+ extern "C" {
+ void f_setsig();
+ void f_init();
+ }
+ #define SimTK_FORTRAN_INIT do {f_setsig(); f_init();} while(false)
+#endif
+
+#endif // SimTK_SimTKCOMMON_FORTRAN_H_
diff --git a/SimTKcommon/include/SimTKcommon/internal/Function.h b/SimTKcommon/include/SimTKcommon/internal/Function.h
new file mode 100644
index 0000000..faf75de
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/Function.h
@@ -0,0 +1,412 @@
+#ifndef SimTK_SimTKCOMMON_FUNCTION_H_
+#define SimTK_SimTKCOMMON_FUNCTION_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+// Note: this file was moved from Simmath to SimTKcommon 20100601; see the
+// Simmath repository for earlier history.
+
+#include "SimTKcommon/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+#include <cassert>
+
+namespace SimTK {
+
+/**
+ * This abstract class represents a mathematical function that calculates a
+ * value of arbitrary type based on M real arguments. The output type is set
+ * as a template argument, while the number of input components may be
+ * determined at runtime. The name "Function" (with no trailing _) may be
+ * used as a synonym for Function_<Real>.
+ *
+ * Subclasses define particular mathematical functions. Predefined subclasses
+ * are provided for several common function types: Function_<T>::Constant,
+ * Function_<T>::Linear, Function_<T>::Polynomial, and Function_<T>::Step.
+ * You can define your own subclasses for other function types. The
+ * Spline_ class also provides a convenient way to create various types of
+ * Functions.
+ */
+template <class T>
+class Function_ {
+public:
+ class Constant;
+ class Linear;
+ class Polynomial;
+ class Sinusoid;
+ class Step;
+ virtual ~Function_() {
+ }
+ /**
+ * Calculate the value of this function at a particular point.
+ *
+ * @param x the Vector of input arguments. Its size must equal the
+ * value returned by getArgumentSize().
+ */
+ virtual T calcValue(const Vector& x) const = 0;
+ /**
+ * Calculate a partial derivative of this function at a particular point.
+ * Which derivative to take is specified by listing the input components
+ * with which to take it. For example, if derivComponents=={0}, that
+ * indicates a first derivative with respective to component 0. If
+ * derivComponents=={0, 0, 0}, that indicates a third derivative with
+ * respective to component 0. If derivComponents=={4, 7}, that indicates a
+ * partial second derivative with respect to components 4 and 7.
+ *
+ * @param derivComponents
+ * The input components with respect to which the derivative should be
+ * taken. Its size must be less than or equal to the value returned
+ * by getMaxDerivativeOrder().
+ * @param x
+ * The Vector of input arguments. Its size must equal the value
+ * returned by getArgumentSize().
+ * @return
+ * The value of the selected derivative, which is of type T.
+ */
+ virtual T calcDerivative(const Array_<int>& derivComponents,
+ const Vector& x) const = 0;
+
+ /** This provides compatibility with std::vector without requiring any
+ copying. **/
+ T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const
+ { return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
+
+ /**
+ * Get the number of components expected in the input vector.
+ */
+ virtual int getArgumentSize() const = 0;
+ /**
+ * Get the maximum derivative order this Function_ object can calculate.
+ */
+ virtual int getMaxDerivativeOrder() const = 0;
+};
+
+/** This typedef is used for the very common case that the return type of
+the Function object is Real. **/
+typedef Function_<Real> Function;
+
+
+
+/**
+ * This is a Function_ subclass which simply returns a fixed value, independent
+ * of its arguments.
+ */
+template <class T>
+class Function_<T>::Constant : public Function_<T> {
+public:
+ /**
+ * Create a Function_::Constant object.
+ *
+ * @param value the value which should be returned by calcValue();
+ * @param argumentSize the value which should be returned by
+ * getArgumentSize(), with a default of 1.
+ */
+ explicit Constant(T value, int argumentSize=1)
+ : argumentSize(argumentSize), value(value) {
+ }
+ T calcValue(const Vector& x) const {
+ assert(x.size() == argumentSize);
+ return value;
+ }
+ T calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
+ return static_cast<T>(0);
+ }
+ virtual int getArgumentSize() const {
+ return argumentSize;
+ }
+ int getMaxDerivativeOrder() const {
+ return std::numeric_limits<int>::max();
+ }
+
+ /** This provides compatibility with std::vector without requiring any
+ copying. **/
+ T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const
+ { return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
+
+private:
+ const int argumentSize;
+ const T value;
+};
+
+/**
+ * This is a Function_ subclass whose output value is a linear function of its
+ * arguments: f(x, y, ...) = ax+by+...+c.
+ */
+template <class T>
+class Function_<T>::Linear : public Function_<T> {
+public:
+ /**
+ * Create a Function_::Linear object.
+ *
+ * @param coefficients
+ * The coefficients of the linear function. The number of arguments
+ * expected by the function is equal to coefficients.size()-1.
+ * coefficients[0] is the coefficient for the first argument,
+ * coefficients[1] is the coefficient for the second argument, etc.
+ * The final element of coefficients contains the constant term.
+ */
+ explicit Linear(const Vector_<T>& coefficients) : coefficients(coefficients) {
+ }
+ T calcValue(const Vector& x) const {
+ assert(x.size() == coefficients.size()-1);
+ T value = static_cast<T>(0);
+ for (int i = 0; i < x.size(); ++i)
+ value += x[i]*coefficients[i];
+ value += coefficients[x.size()];
+ return value;
+ }
+ T calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
+ assert(x.size() == coefficients.size()-1);
+ assert(derivComponents.size() > 0);
+ if (derivComponents.size() == 1)
+ return coefficients(derivComponents[0]);
+ return static_cast<T>(0);
+ }
+ virtual int getArgumentSize() const {
+ return coefficients.size()-1;
+ }
+ int getMaxDerivativeOrder() const {
+ return std::numeric_limits<int>::max();
+ }
+
+ /** This provides compatibility with std::vector without requiring any
+ copying. **/
+ T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const
+ { return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
+private:
+ const Vector_<T> coefficients;
+};
+
+
+/**
+ * This is a Function_ subclass whose output value is a polynomial of its
+ * argument: f(x) = ax^n+bx^(n-1)+...+c.
+ */
+template <class T>
+class Function_<T>::Polynomial : public Function_<T> {
+public:
+ /**
+ * Create a Function_::Polynomial object.
+ *
+ * @param coefficients the polynomial coefficients in order of decreasing
+ * powers
+ */
+ Polynomial(const Vector_<T>& coefficients) : coefficients(coefficients) {
+ }
+ T calcValue(const Vector& x) const {
+ assert(x.size() == 1);
+ Real arg = x[0];
+ T value = static_cast<T>(0);
+ for (int i = 0; i < coefficients.size(); ++i)
+ value = value*arg + coefficients[i];
+ return value;
+ }
+ T calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
+ assert(x.size() == 1);
+ assert(derivComponents.size() > 0);
+ Real arg = x[0];
+ T value = static_cast<T>(0);
+ const int derivOrder = (int)derivComponents.size();
+ const int polyOrder = coefficients.size()-1;
+ for (int i = 0; i <= polyOrder-derivOrder; ++i) {
+ T coeff = coefficients[i];
+ for (int j = 0; j < derivOrder; ++j)
+ coeff *= polyOrder-i-j;
+ value = value*arg + coeff;
+ }
+ return value;
+ }
+ virtual int getArgumentSize() const {
+ return 1;
+ }
+ int getMaxDerivativeOrder() const {
+ return std::numeric_limits<int>::max();
+ }
+
+ /** This provides compatibility with std::vector without requiring any
+ copying. **/
+ T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const
+ { return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
+private:
+ const Vector_<T> coefficients;
+};
+
+
+/**
+ * This is a Function_ subclass whose output value is a sinusoid of its
+ * argument: f(x) = a*sin(w*x + p) where a is amplitude, w is frequency
+ * in radians per unit of x, p is phase in radians.
+ *
+ * This is only defined for a scalar (Real) return value.
+ */
+template <>
+class Function_<Real>::Sinusoid : public Function_<Real> {
+public:
+ /**
+ * Create a Function::Sinusoid object, returning a*sin(w*x+p).
+ *
+ * @param[in] amplitude 'a' in the above formula
+ * @param[in] frequency 'w' in the above formula
+ * @param[in] phase 'p' in the above formula
+ */
+ Sinusoid(Real amplitude, Real frequency, Real phase=0)
+ : a(amplitude), w(frequency), p(phase) {}
+
+ void setAmplitude(Real amplitude) {a=amplitude;}
+ void setFrequency(Real frequency) {w=frequency;}
+ void setPhase (Real phase) {p=phase;}
+
+ Real getAmplitude() const {return a;}
+ Real getFrequency() const {return w;}
+ Real getPhase () const {return p;}
+
+ // Implementation of Function_<T> virtuals.
+
+ virtual Real calcValue(const Vector& x) const {
+ const Real t = x[0]; // we expect just one argument
+ return a*std::sin(w*t + p);
+ }
+
+ virtual Real calcDerivative(const Array_<int>& derivComponents,
+ const Vector& x) const {
+ const Real t = x[0]; // time is the only argument
+ const int order = derivComponents.size();
+ // The n'th derivative is
+ // sign * a * w^n * sc
+ // where sign is -1 if floor(order/2) is odd, else 1
+ // and sc is cos(w*t+p) if order is odd, else sin(w*t+p)
+ switch(order) {
+ case 0: return a* std::sin(w*t + p);
+ case 1: return a*w* std::cos(w*t + p);
+ case 2: return -a*w*w* std::sin(w*t + p);
+ case 3: return -a*w*w*w*std::cos(w*t + p);
+ default:
+ const Real sign = Real(((order/2) & 0x1) ? -1 : 1);
+ const Real sc = (order & 0x1) ? std::cos(w*t+p) : std::sin(w*t+p);
+ const Real wn = std::pow(w, order);
+ return sign*a*wn*sc;
+ }
+ }
+
+ virtual int getArgumentSize() const {return 1;} // just time
+ virtual int getMaxDerivativeOrder() const {
+ return std::numeric_limits<int>::max();
+ }
+
+ /** This provides compatibility with std::vector without requiring any
+ copying. **/
+ Real calcDerivative(const std::vector<int>& derivComponents,
+ const Vector& x) const
+ { return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
+private:
+ Real a, w, p;
+};
+
+/**
+ * This is a Function_ subclass whose output value y=f(x) is smoothly stepped
+ * from y=y0 to y1 as its input argument goes from x=x0 to x1. This is
+ * an S-shaped function with first and second derivatives y'(x0)=y'(x1)=0
+ * and y''(x0)=y''(x1)==0. The third derivative y''' exists and is continuous
+ * but we cannot guarantee anything about it at the end points.
+ */
+template <class T>
+class Function_<T>::Step : public Function_<T> {
+public:
+ /**
+ * Create a Function_::Step object that smoothly interpolates its output
+ * through a given range as its input moves through its range.
+ *
+ * @param y0 Output value when (x-x0)*sign(x1-x0) <= 0.
+ * @param y1 Output value when (x-x1)*sign(x1-x0) >= 0.
+ * @param x0 Start of switching interval.
+ * @param x1 End of switching interval.
+ *
+ * @tparam T The template type is the type of y0 and y1. This must
+ * be a type that supports subtraction and scalar
+ * multiplication by a Real so that we can compute
+ * an expression like y=y0 + f*(y1-y0) for some Real scalar f.
+ *
+ * Note that the numerical values of x0 and x1 can be in either order
+ * x0 < x1 or x0 > x1.
+ */
+ Step(const T& y0, const T& y1, Real x0, Real x1)
+ : m_y0(y0), m_y1(y1), m_yr(y1-y0), m_zero(Real(0)*y0),
+ m_x0(x0), m_x1(x1), m_ooxr(1/(x1-x0)), m_sign(sign(m_ooxr))
+ { SimTK_ERRCHK1_ALWAYS(x0 != x1, "Function_<T>::Step::ctor()",
+ "A zero-length switching interval is illegal; both ends were %g.", x0);
+ }
+
+ T calcValue(const Vector& xin) const {
+ SimTK_ERRCHK1_ALWAYS(xin.size() == 1,
+ "Function_<T>::Step::calcValue()",
+ "Expected just one input argument but got %d.", xin.size());
+
+ const Real x = xin[0];
+ if ((x-m_x0)*m_sign <= 0) return m_y0;
+ if ((x-m_x1)*m_sign >= 0) return m_y1;
+ // f goes from 0 to 1 as x goes from x0 to x1.
+ const Real f = stepAny(0,1,m_x0,m_ooxr, x);
+ return m_y0 + f*m_yr;
+ }
+
+ T calcDerivative(const Array_<int>& derivComponents, const Vector& xin) const {
+ SimTK_ERRCHK1_ALWAYS(xin.size() == 1,
+ "Function_<T>::Step::calcDerivative()",
+ "Expected just one input argument but got %d.", xin.size());
+
+ const int derivOrder = (int)derivComponents.size();
+ SimTK_ERRCHK1_ALWAYS(1 <= derivOrder && derivOrder <= 3,
+ "Function_<T>::Step::calcDerivative()",
+ "Only 1st, 2nd, and 3rd derivatives of the step are available,"
+ " but derivative %d was requested.", derivOrder);
+ const Real x = xin[0];
+ if ((x-m_x0)*m_sign <= 0) return m_zero;
+ if ((x-m_x1)*m_sign >= 0) return m_zero;
+ switch(derivOrder) {
+ case 1: return dstepAny (1,m_x0,m_ooxr, x) * m_yr;
+ case 2: return d2stepAny(1,m_x0,m_ooxr, x) * m_yr;
+ case 3: return d3stepAny(1,m_x0,m_ooxr, x) * m_yr;
+ default: assert(!"impossible derivOrder");
+ }
+ return NaN*m_yr; /*NOTREACHED*/
+ }
+
+ virtual int getArgumentSize() const {return 1;}
+ int getMaxDerivativeOrder() const {return 3;}
+
+ /** This provides compatibility with std::vector without requiring any
+ copying. **/
+ T calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const
+ { return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
+private:
+ const T m_y0, m_y1, m_yr; // precalculate yr=(y1-y0)
+ const T m_zero; // precalculate T(0)
+ const Real m_x0, m_x1, m_ooxr; // precalculate ooxr=1/(x1-x0)
+ const Real m_sign; // sign(x1-x0) is 1 or -1
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_FUNCTION_H_
+
+
diff --git a/SimTKcommon/include/SimTKcommon/internal/Parallel2DExecutor.h b/SimTKcommon/include/SimTKcommon/internal/Parallel2DExecutor.h
new file mode 100644
index 0000000..5a358e7
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/Parallel2DExecutor.h
@@ -0,0 +1,140 @@
+#ifndef SimTK_SimTKCOMMON_PARALLEL_2D_EXECUTOR_H_
+#define SimTK_SimTKCOMMON_PARALLEL_2D_EXECUTOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "ParallelExecutor.h"
+#include "PrivateImplementation.h"
+
+namespace SimTK {
+
+class Parallel2DExecutor;
+class Parallel2DExecutorImpl;
+
+// We only want the template instantiation to occur once. This symbol is defined in the SimTK core
+// compilation unit that defines the Parallel2DExecutor class but should not be defined any other time.
+#ifndef SimTK_SIMTKCOMMON_DEFINING_PARALLEL_2D_EXECUTOR
+ extern template class PIMPLHandle<Parallel2DExecutor, Parallel2DExecutorImpl>;
+#endif
+
+/**
+ * This class is used for performing multithreaded computations over two dimensional ranges. That is,
+ * it performs some calculation once for each pair (i, j) where i and j vary over some range. For
+ * example, it is useful for calculating pairwise forces between a set of bodies.
+ *
+ * To use it, define a subclass of Parallel2DExecutor::Task that performs a computation. Then create a
+ * Parallel2DExecutor object and ask it to execute the task:
+ *
+ * <pre>
+ * Parallel2DExecutor executor(gridSize);
+ * executor.execute(myTask, Parallel2DExecutor::FullMatrix);
+ * </pre>
+ *
+ * The Task's execute() method will be called once with each pair (i, j) where i and j vary between 0 and
+ * gridSize-1. You also can restrict it to only pairs with i > j or i >= j.
+ *
+ * The invocations are done in parallel on multiple threads, but they are divided up in a way that avoids
+ * index conflicts between simultaneous calculations. If the task is executed with indices (i1, j1)
+ * on one thread, it is guaranteed that no other thread is simultaneously executing the task with
+ * either the first or second index equal to either i1 or j1. (More precisely, if either index of one
+ * invocation is equal to either index of another invocation, the two invocations are guaranteed to be
+ * separated by a happens-before edge.) This allows the task to modify data that is indexed by i and j
+ * without needing to worry about concurrent modifications.
+ *
+ * The threads are created in the Parallel2DExecutor's constructor and remain active until it is deleted.
+ * This means that creating a Parallel2DExecutor is a somewhat expensive operation, but it may then be
+ * used repeatedly for executing various calculations. By default, the number of threads is chosen
+ * to be equal to the number of available processor cores. You can optionally specify a different number
+ * of threads to create. For example, using more threads than processors can sometimes lead to better
+ * processor utilitization.
+ */
+
+class SimTK_SimTKCOMMON_EXPORT Parallel2DExecutor : public PIMPLHandle<Parallel2DExecutor, Parallel2DExecutorImpl> {
+public:
+ class Task;
+ enum RangeType {FullMatrix, HalfMatrix, HalfPlusDiagonal};
+ /**
+ * Construct a Parallel2DExecutor.
+ *
+ * @param gridSize the size of the range over which i and j should vary
+ * @param numThreads the number of threads to create. By default, this is set equal to the number
+ * of processors.
+ */
+ explicit Parallel2DExecutor(int gridSize, int numThreads = ParallelExecutor::getNumProcessors());
+ /**
+ * Construct a Parallel2DExecutor. This constructor allows you to specify an existing ParallelExecutor
+ * to use for parallelizing the calculation. This can improve efficiency by reusing an existing thread
+ * pool. It is your responsibility to make sure that the ParallelExecutor does not get deleted as long
+ * as this object exists.
+ *
+ * @param gridSize the size of the range over which i and j should vary
+ * @param executor the ParallelExecutor to use for parallelizing calculations
+ */
+ Parallel2DExecutor(int gridSize, ParallelExecutor& executor);
+ /**
+ * Execute a parallel task.
+ *
+ * @param task the Task to execute
+ * @param rangeType specifies what part of the range i and j should vary over. Specify FullyMatrix to
+ * execute the task for all values of i and j between 0 and gridSize, HalfMatrix to
+ * restrict it to i > j, and HalfPlusDiagonal to restrict it to i >= j.
+ */
+ void execute(Task& task, RangeType rangeType);
+ /**
+ * Get the ParallelExecutor used by this object to parallelize calculations.
+ */
+ ParallelExecutor& getExecutor();
+};
+
+/**
+ * Concrete subclasses of this abstract class represent tasks that can be executed by a Parallel2DExecutor.
+ */
+
+class Parallel2DExecutor::Task {
+public:
+ virtual ~Task() {
+ }
+ /**
+ * This method defines the task to be performed. When the Task is passed to a Parallel2DExecutor's execute()
+ * method, this method will be called in parallel for each allowed value of i and j.
+ */
+ virtual void execute(int i, int j) = 0;
+ /**
+ * This method is invoked once by each worker thread before the task is executed. This can be used to
+ * initialize thread-local storage.
+ */
+ virtual void initialize() {
+ }
+ /**
+ * This method is invoked once by each worker thread after all invocations of the task on that thread are complete.
+ * This can be used to clean up thread-local storage, or to record per-thread results. All calls to this method
+ * are synchronized, so it can safely write to global variables without danger of interference between worker threads.
+ */
+ virtual void finish() {
+ }
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_PARALLEL_2D_EXECUTOR_H_
diff --git a/SimTKcommon/include/SimTKcommon/internal/ParallelExecutor.h b/SimTKcommon/include/SimTKcommon/internal/ParallelExecutor.h
new file mode 100644
index 0000000..c1096b2
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/ParallelExecutor.h
@@ -0,0 +1,122 @@
+#ifndef SimTK_SimTKCOMMON_PARALLEL_EXECUTOR_H_
+#define SimTK_SimTKCOMMON_PARALLEL_EXECUTOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "PrivateImplementation.h"
+
+namespace SimTK {
+
+class ParallelExecutor;
+class ParallelExecutorImpl;
+
+// We only want the template instantiation to occur once. This symbol is defined in the SimTK core
+// compilation unit that defines the ParallelExecutor class but should not be defined any other time.
+#ifndef SimTK_SIMTKCOMMON_DEFINING_PARALLEL_EXECUTOR
+ extern template class PIMPLHandle<ParallelExecutor, ParallelExecutorImpl>;
+#endif
+
+/**
+ * This class is used for performing multithreaded computations. To use it, define a subclass of
+ * ParallelExecutor::Task that performs some computation. Then create a ParallelExecutor object
+ * and ask it to execute the task:
+ *
+ * <pre>
+ * ParallelExecutor executor;
+ * executor.execute(myTask, times);
+ * </pre>
+ *
+ * The Task's execute() method will be called the specified number of times, with each invocation
+ * being given a different index value from 0 to times-1. The invocations are done in parallel
+ * on multiple threads, so you cannot make any assumptions about what order they will occur in
+ * or which ones will happen at the same time.
+ *
+ * The threads are created in the ParallelExecutor's constructor and remain active until it is deleted.
+ * This means that creating a ParallelExecutor is a somewhat expensive operation, but it may then be
+ * used repeatedly for executing various calculations. By default, the number of threads is chosen
+ * to be equal to the number of available processor cores. You can optionally specify a different number
+ * of threads to create. For example, using more threads than processors can sometimes lead to better
+ * processor utilitization. Alternatively, if the Task will only be executed four times, you might
+ * specify min(4, ParallelExecutor::getNumProcessors()) to avoid creating extra threads that will never
+ * have any work to do.
+ */
+
+class SimTK_SimTKCOMMON_EXPORT ParallelExecutor : public PIMPLHandle<ParallelExecutor, ParallelExecutorImpl> {
+public:
+ class Task;
+ /**
+ * Construct a ParallelExecutor.
+ *
+ * @param numThreads the number of threads to create. By default, this is set equal to the number
+ * of processors.
+ */
+ explicit ParallelExecutor(int numThreads = getNumProcessors());
+ /**
+ * Execute a parallel task.
+ *
+ * @param task the Task to execute
+ * @param times the number of times the Task should be executed
+ */
+ void execute(Task& task, int times);
+ /**
+ * Get the number of available processor cores.
+ */
+ static int getNumProcessors();
+ /**
+ * Determine whether the thread invoking this method is a worker thread created by ParallelExecutor.
+ */
+ static bool isWorkerThread();
+};
+
+/**
+ * Concrete subclasses of this abstract class represent tasks that can be executed by a ParallelExecutor.
+ */
+
+class ParallelExecutor::Task {
+public:
+ virtual ~Task() {
+ }
+ /**
+ * This method defines the task to be performed. When the Task is passed to a ParallelExecutor's execute()
+ * method, this method will be called in parallel the specified number of times.
+ */
+ virtual void execute(int index) = 0;
+ /**
+ * This method is invoked once by each worker thread before the task is executed. This can be used to
+ * initialize thread-local storage.
+ */
+ virtual void initialize() {
+ }
+ /**
+ * This method is invoked once by each worker thread after all invocations of the task on that thread are complete.
+ * This can be used to clean up thread-local storage, or to record per-thread results. All calls to this method
+ * are synchronized, so it can safely write to global variables without danger of interference between worker threads.
+ */
+ virtual void finish() {
+ }
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_PARALLEL_EXECUTOR_H_
diff --git a/SimTKcommon/include/SimTKcommon/internal/ParallelWorkQueue.h b/SimTKcommon/include/SimTKcommon/internal/ParallelWorkQueue.h
new file mode 100644
index 0000000..3afdce6
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/ParallelWorkQueue.h
@@ -0,0 +1,107 @@
+#ifndef SimTK_SimTKCOMMON_PARALLEL_WORK_QUEUE_H_
+#define SimTK_SimTKCOMMON_PARALLEL_WORK_QUEUE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "ParallelExecutor.h"
+#include "PrivateImplementation.h"
+
+namespace SimTK {
+
+class ParallelWorkQueue;
+class ParallelWorkQueueImpl;
+
+// We only want the template instantiation to occur once. This symbol is defined in the SimTK core
+// compilation unit that defines the ParallelWorkQueue class but should not be defined any other time.
+#ifndef SimTK_SIMTKCOMMON_DEFINING_PARALLEL_WORK_QUEUE
+ extern template class PIMPLHandle<ParallelWorkQueue, ParallelWorkQueueImpl>;
+#endif
+
+/**
+ * This class is used for performing multithreaded computations.\ It maintains a queue of tasks to be
+ * executed, and a pool of threads for executing them. To use it, define one or more subclasses of
+ * ParallelWorkQueue::Task that performs computations. Then create a ParallelWorkQueue and add Tasks
+ * to it:
+ *
+ * <pre>
+ * ParallelWorkQueue queue(20);
+ * queue.addTask(new MyTask());
+ * </pre>
+ *
+ * Each Task's execute() method will be called, and then the Task will be deleted. The invocations are
+ * done in parallel on multiple threads, so you cannot make any assumptions about what order they will occur in
+ * or which ones will happen at the same time.
+ *
+ * The threads are created in the ParallelWorkQueue's constructor and remain active until it is deleted.
+ * This means that creating a ParallelWorkQueue is a somewhat expensive operation, but it may then be
+ * used repeatedly for executing various calculations. By default, the number of threads is chosen
+ * to be equal to the number of available processor cores. You can optionally specify a different number
+ * of threads to create. For example, using more threads than processors can sometimes lead to better
+ * processor utilitization. Alternatively, if only four Tasks will be executed, you might specify
+ * min(4, ParallelExecutor::getNumProcessors()) to avoid creating extra threads that will never
+ * have any work to do.
+ */
+
+class SimTK_SimTKCOMMON_EXPORT ParallelWorkQueue : public PIMPLHandle<ParallelWorkQueue, ParallelWorkQueueImpl> {
+public:
+ class Task;
+ /**
+ * Construct a ParallelWorkQueue.
+ *
+ * @param queueSize the maximum number of Tasks that can be in the queue waiting to start executing at any time
+ * @param numThreads the number of threads to create. By default, this is set equal to the number of processors.
+ */
+ explicit ParallelWorkQueue(int queueSize, int numThreads = ParallelExecutor::getNumProcessors());
+ /**
+ * Add a Task to the queue. If the queue is currently full, this method will block until space is freed up in
+ * the queue by the worker threads. The queue assumes ownership of the Task object and deletes it once it has
+ * finished executing.
+ *
+ * @param task the Task to add to the queue
+ */
+ void addTask(Task* task);
+ /**
+ * Block until all Tasks that have been added to the queue finish executing.
+ */
+ void flush();
+};
+
+/**
+ * Concrete subclasses of this abstract class represent tasks that can be executed by a ParallelWorkQueue.
+ */
+
+class ParallelWorkQueue::Task {
+public:
+ virtual ~Task() {
+ }
+ /**
+ * This method defines the task to be performed. At some point after the Task is added to a
+ * ParallelWorkQueue, this method will be called by one of the worker threads.
+ */
+ virtual void execute() = 0;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_PARALLEL_WORK_QUEUE_H_
diff --git a/SimTKcommon/include/SimTKcommon/internal/Pathname.h b/SimTKcommon/include/SimTKcommon/internal/Pathname.h
new file mode 100644
index 0000000..fa9c573
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/Pathname.h
@@ -0,0 +1,239 @@
+#ifndef SimTK_SimTKCOMMON_PATHNAME_H_
+#define SimTK_SimTKCOMMON_PATHNAME_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declaration of the SimTK::Pathname class providing platform-independent
+manipulation of file pathnames. **/
+
+#include "SimTKcommon/internal/common.h"
+#include "SimTKcommon/internal/Array.h"
+#include <string>
+#include <stdexcept>
+
+namespace SimTK {
+
+/**
+ * This class encapsulates the handling of file and directory pathnames
+ * in a platform-independent manner. We consider a pathname to consist
+ * of three components:
+ * <pre> [directory] [filename [extension]] </pre>
+ * where the directory may be an absolute location or relative to a
+ * current working directory.
+ *
+ * Several special directory names are supported here:
+ * - root (/)
+ * - current working directory (.)
+ * - current executable directory (@)
+ * - platform default installation directory
+ * - parent directory (..)
+ *
+ * On Windows root and current working directory are drive-specific, referring
+ * to the current drive if none is specified. The current executable
+ * directory is the absolute directory name containing the executable
+ * program which is currently running.
+ *
+ * A pathname has "segments" which are separated by either forward
+ * slashes or backslashes. We are relaxed about the slashes and will
+ * accept either one and pathnames which use both. However, each operating
+ * system platform has a preferred separator character, backslash on
+ * Windows and forward slash everywhere else and we will clean up
+ * returned pathnames to use exclusively the preferred separator for
+ * the current platform.
+ *
+ * Pathnames that end in an empty segment, or a segment consisting of
+ * just "." or ".." are directory path names, meaning that the "filename"
+ * and "extension" components are empty. Other pathnames may be directories
+ * or filenames depending on context. Whenever we generate a pathname that
+ * we know to be a directory, it will end in a final slash.
+ *
+ * There is also the concept of a "drive" which on Windows is a drive
+ * letter followed by a colon (e.g. "c:") but is always the empty string
+ * on non-Windows platforms. The drive, if present is considered part of
+ * the directory and does not affect whether the directory is considered
+ * relative or absolute. Drive designators are recognized only on Windows;
+ * they are just considered ordinary pathname characters on other platforms.
+ *
+ * This class is useful for generating "canonicalized" pathnames from
+ * names that have been pieced together from environment variables and
+ * user entry. Canonicalized names are always absolute pathnames; they
+ * contain no empty, ".", or ".." segments. On Windows a canonicalized
+ * name is always prefixed by an explicit disk designator followed by
+ * a backslash; on other platforms the canonicalized name will always
+ * begin with a forward slash.
+ *
+ */
+class SimTK_SimTKCOMMON_EXPORT Pathname {
+public:
+ /// Dismantle a supplied pathname into its component
+ /// parts. This can take pathnames like <pre>
+ /// /usr/local/libMyDll_d.so
+ /// e:\\Program Files\\Something\\myLibrary_d.dll
+ /// </pre> and chop them into <pre>
+ /// directory fileName extension
+ /// ------------------------------- -------------- ---------
+ /// /usr/local/ libMyDll_d .so
+ /// e:\\Program Files\\Something\\ myLibrary_d .dll
+ /// </pre>
+ /// as well as tell you whether the given pathname is absolute or relative
+ /// (and thus subject to search rules). At the beginning of the pathname
+ /// (or right after the drive specification on Windows) we recognize
+ /// three special symbols:
+ /// - "/" means root; i.e., this is an absolute path name starting from
+ /// the root directory (this drive's root for Windows).
+ /// - "." starts an absolute path name which is relative to the
+ /// current working directory (or drive's cwd on Windows).
+ /// - "@" starts an absolute path name which is relative to the
+ /// directory in which the currently running executable is located.
+ ///
+ /// Anywhere in the pathname, the name ".." means "go up one level from
+ /// the prior directory". ".." at the start is interpreted as "./..".
+ /// A '.' appearing anywhere in the path name except the begining is
+ /// ignored. An '@' appearing anywhere in the pathname other than the
+ /// beginning is treated as an ordinary file character.
+ ///
+ /// The pathname components are returned
+ /// as separate strings with separators included such that concatenating
+ /// all the strings reproduces the pathname in a canonicalized form.
+ /// The "drive" letter prefix is recognized only when running on Windows;
+ /// otherwise a prefix like "C:" is treated as ordinary file name
+ /// characters. Note that we include the drive letter as part of the
+ /// absolute directory.
+ /// White space is removed, and path separator characters
+ /// in the directory are changed to the appropriate slash
+ /// for the currently running platform (i.e. backslash for
+ /// Windows and forward slash everywhere else).
+ static void deconstructPathname( const std::string& name,
+ bool& isAbsolutePath,
+ std::string& directory,
+ std::string& fileName,
+ std::string& extension);
+
+ /// Get canonicalized absolute pathname from a given pathname which
+ /// can be relative or absolute. Canonicalizing means
+ /// - drive designator is recognized if we're on Windows;
+ /// - leading "." and "@" are replaced with the current working
+ /// directory or the executable directory, resp.
+ /// - each ".." segment is processed, removing it and its
+ /// previous segment; initial ".." is treated as "./..".
+ /// - empty segments and interior "." segments are removed
+ /// - if the input pathname ends in a slash after above processing,
+ /// then the returned pathname will also end in a slash.
+ /// - separators are made all-forward slash or all-backslash
+ /// - on Windows, the returned pathname begins with an explicit
+ /// disk designator in lower case, e.g. "c:".
+ ///
+ /// The result here is what you get by reassembling the components
+ /// from deconstructPathname(), plus inserting the current working
+ /// directory in front if the path name was relative.
+ static std::string getAbsolutePathname(const std::string& pathname) {
+ bool isAbsolutePath;
+ std::string directory, fileName, extension;
+ deconstructPathname(pathname, isAbsolutePath, directory, fileName, extension);
+ if (!isAbsolutePath)
+ directory = getCurrentWorkingDirectory() + directory;
+ return directory + fileName + extension;
+ }
+
+ /// This is the same as getAbsolutePathname() except that the final
+ /// segment is interpreted as a directory name rather than a file name,
+ /// meaning that we append a slash if necessary.
+ static std::string getAbsoluteDirectoryPathname(const std::string& dirPathname) {
+ std::string absPath = getAbsolutePathname(dirPathname);
+ if (!absPath.empty() && absPath[absPath.size()-1] != getPathSeparatorChar())
+ absPath += getPathSeparatorChar();
+ return absPath;
+ }
+
+ /// Return true if the given pathname names a file that exists and is
+ /// readable.
+ static bool fileExists(const std::string& fileName);
+
+ /// Get the default installation directory for this platform. This will
+ /// be /usr/local/ for Linux and Apple, and the value of the \%ProgramFiles\%
+ /// registry entry on Windows (typically c:\\Program Files\\).
+ static std::string getDefaultInstallDir();
+
+ /// Append a subdirectory offset to an existing pathname (relative or absolute).
+ /// A leading "/" in the offset is ignored, and the result ends in "/".
+ static std::string addDirectoryOffset(const std::string& base, const std::string& offset);
+
+ /// Find the installation directory for something, using the named
+ /// installation directory environment variable if it exists, otherwise
+ /// by appending the supplied path offset to the default install directory.
+ static std::string getInstallDir(const std::string& envInstallDir,
+ const std::string& offsetFromDefaultInstallDir);
+
+ /// Get the absolute pathname of the currently executing program.
+ static std::string getThisExecutablePath();
+ /// Get the absolute pathname of the directory which contains the
+ /// currently executing program.
+ static std::string getThisExecutableDirectory();
+ /// Get the absolute pathname of the current working directory
+ /// including a trailing separator character. Windows keeps a current
+ /// working directory for each drive which can be optionally specified
+ /// (otherwise we use the current drive). If the specified drive
+ /// doesn't exist we'll behave as though root were its current
+ /// working directory. The drive argument is ignored
+ /// on non-Windows platforms.
+ static std::string getCurrentWorkingDirectory(const std::string& drive="");
+ /// Get the canonicalized name of the root directory. This is "x:\" on Windows
+ /// with "x" replaced by the current drive letter or the specified drive
+ /// (in lowercase), and just "/" on non-Windows systems.
+ static std::string getRootDirectory(const std::string& drive="");
+ /// On Windows, return the current drive letter in lowercase, with no
+ /// trailing ":"; on other platforms return an empty string.
+ static std::string getCurrentDriveLetter();
+ /// On Windows, return the current drive letter in lowercase,
+ /// followed by ":"; on other platforms just return an empty string.
+ static std::string getCurrentDrive();
+ /// Return true if the named environment variable is present
+ /// in the environment.
+ static bool environmentVariableExists(const std::string& name);
+ /// Return the value of the named environment variable or
+ /// the empty string if the variable is not found. Note that that
+ /// is indistinguishable from a variable that is present but with
+ /// a null value -- use environmentVariableExists() if you really
+ /// need to know the difference.
+ static std::string getEnvironmentVariable(const std::string& name);
+ /// Return this platform's pathname separator character as
+ /// a string. This is backslash on Windows and forward slash
+ /// everywhere else.
+ static std::string getPathSeparator();
+ /// Return this platform's pathname separator character as
+ /// a char. This is backslash on Windows and forward slash
+ /// everywhere else.
+ static char getPathSeparatorChar();
+ /// Returns true if the character is slash or backslash.
+ static bool isPathSeparator(char c) {
+ return c=='/' || c=='\\';
+ }
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_PATHNAME_H_
+
+
diff --git a/SimTKcommon/include/SimTKcommon/internal/Plugin.h b/SimTKcommon/include/SimTKcommon/internal/Plugin.h
new file mode 100644
index 0000000..033a173
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/Plugin.h
@@ -0,0 +1,318 @@
+#ifndef SimTK_SimTKCOMMON_PLUGIN_H_
+#define SimTK_SimTKCOMMON_PLUGIN_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declaration of the SimTK::Plugin class providing platform-independent
+handling of dynamically-loaded libraries. **/
+
+#include "SimTKcommon/internal/common.h"
+#include "SimTKcommon/internal/Array.h"
+#include "SimTKcommon/internal/Pathname.h"
+#include <string>
+#include <stdexcept>
+
+namespace SimTK {
+
+/**
+ * This is the base class for representing a runtime-linked dynamic library,
+ * also known as a "plugin", in a platform-independent manner. For any particular
+ * kind of plugin, derive a concrete class from this one and use the macros
+ * to describe the functions or data you expect to find there. Then each
+ * plugin library you load of that type is an object of the concrete class
+ * you defined. The derived class constructor sets the search policy for
+ * plugin libraries of that type. For example:
+ * <pre>
+ * class MyPlugin : public Plugin {
+ * public:
+ * explicit MyPlugin() : Plugin() {
+ * addSearchDirectory(Pathname::getInstallDir("SimTK_INSTALL_DIR", "SimTK")
+ * + "/lib/plugins/");
+ * }
+ *
+ * SimTK_PLUGIN_DEFINE_FUNCTION1(int,exportedFunctionWithOneArg,const std::string&);
+ * SimTK_PLUGIN_DEFINE_FUNCTION(SomeObjectType*,anExportedFunctionNoArgs);
+ * SimTK_PLUGIN_DEFINE_FUNCTION(void, someFunction);
+ * SimTK_PLUGIN_DEFINE_SYMBOL(SymbolType, nameOfExportedSymbol);
+ * };
+ * </pre>
+ *
+ * Then this class is used as follows:
+ * <pre>
+ * MyPlugin lib1;
+ * if (!lib1.load("libraryName")) {
+ * std::cerr << lib1.getLastErrorMessage() << std::endl;
+ * // exit, retry or whatever
+ * }
+ * // At this point you can call the functions and access
+ * // the symbols, e.g.
+ * int i = lib1.exportedFunctionWithOneArg("hi");
+ * </pre>
+ *
+ * The currently available macros are:
+ * - SimTK_PLUGIN_DEFINE_SYMBOL(type, name)
+ * - SimTK_PLUGIN_DEFINE_FUNCTION(returnType, name)
+ * - SimTK_PLUGIN_DEFINE_FUNCTION1(returnType, name, typeOfArg)
+ * - SimTK_PLUGIN_DEFINE_FUNCTION2(returnType, name, typeOfArg1, typeOfArg2)
+ *
+ * These define methods in the derived class which can obtain the symbol
+ * address from the plugin library as needed. The methods will have the
+ * same signature as the library's exported function. In the case of the
+ * symbol, a no-argument method named the same as the symbol will be
+ * created. For example,
+ * <pre> SimTK_PLUGIN_DEFINE_SYMBOL(int, myCounter); </pre>
+ * says the library contains an exported external symbol of type int
+ * named "myCounter". If the Plugin object is "plugin" then you can
+ * access this symbol (read only) as
+ * <pre> const int& counter = plugin.myCounter(); </pre>
+ * The macros also define another method of the same name as the symbol
+ * or function, but prepended with "has_". This returns true if the
+ * named object is exported by the library, otherwise false. If you try
+ * to access a method or symbol without checking first, an exception
+ * will be thrown if the library doesn't export the symbol.
+ */
+class SimTK_SimTKCOMMON_EXPORT Plugin {
+public:
+ explicit Plugin(const std::string& defaultPathname="");
+ ~Plugin(); // unloads the plugin if it was loaded
+
+ /// Attempt to load a plugin of the given name if any, otherwise the
+ /// default name (if any). If the name is relative it will be subject to
+ /// the search rule associated with this Plugin object. In any case
+ /// the filename will be deconstructed to find the library baseName
+ /// to which we will add the "lib" prefix if necessary, the "_d"
+ /// suffix if appropriate, and the platform-specific suffix.
+ ///
+ /// load() returns true if it succeeds, otherwise you
+ /// can call getLastErrorMessage() to find out what's wrong. If a library
+ /// is already loaded, this returns false without checking to see whether
+ /// you are trying to reload the same library. Call isLoaded() to check
+ /// whether a plugin has already been loaded, getLoadedPathname() to
+ /// get a canonicalized absolute pathname for the loaded library.
+ bool load(const std::string& name="");
+
+ /// If a plugin is loaded, unload it now otherwise do nothing. Note that
+ /// whether the corresponding DLL is actually unloaded from the address
+ /// space is up to the operating system, however, this Plugin object will
+ /// be disassociated from it regardless.
+ void unload();
+
+ /// Is there currently a DLL associated with this Plugin object? If so you
+ /// can call getLoadedPathname() to find out which one.
+ bool isLoaded() const {return m_handle != 0;}
+
+ const std::string& getLoadedPathname() const {
+ return m_loadedPathname; // empty if nothing loaded
+ }
+
+ /// If anything goes wrong the last error message is stored so you can
+ /// retrieve it with this method.
+ std::string getLastErrorMessage() const {return m_lastMessage;}
+
+ /// Provide a list of directories in the order you want them
+ /// searched to find a plugin that was provided as a relative path name.
+ /// This is ignored if an absolute path name is given.
+ /// Each of the directory names will be immediately expanded to an
+ /// absolute path name if it isn't already.
+ void setSearchPath(const Array_<std::string>& pathIn) {
+ m_searchPath.clear();
+ for (unsigned i=0; i < pathIn.size(); ++i)
+ addSearchDirectory(pathIn[i]);
+ }
+
+ const Array_<std::string>& getSearchPath() const {return m_searchPath;}
+
+ /// Add a directory to the end of the search path for this kind of
+ /// plugin. This will be expanded immediately to an absolute path
+ /// name if it isn't already. If the directory is blank it is ignored.
+ void addSearchDirectory(const std::string& directory) {
+ const std::string absDir = Pathname::getAbsoluteDirectoryPathname(directory);
+ if (!absDir.empty())
+ m_searchPath.push_back(absDir);
+ }
+
+ /// Put a directory on the front of the search path for this kind of
+ /// plugin. This will be expanded immediately to an absolute path
+ /// name if it isn't already. If the directory name is blank it is
+ /// ignored.
+ void prependSearchDirectory(const std::string& directory) {
+ const std::string absDir = Pathname::getAbsoluteDirectoryPathname(directory);
+ if (!absDir.empty())
+ m_searchPath.insert(m_searchPath.begin(), absDir);
+ }
+
+
+ /// If this fails the return value will be null and the system's human-readable
+ /// error message is in errMsg.
+ static void* loadPluginByFileName(const std::string& name, std::string& errMsg);
+ /// If we're in Debug mode then this method attempts first to load the Debug
+ /// version of the indicated library which it constructs as base+"_d"+extension.
+ /// If that fails (or if we're not in Debug mode) it will try to load the
+ /// Release version (base+extension) instead.
+ static void* loadDebugOrReleasePlugin(const std::string& base, const std::string& extension,
+ std::string& loadedFileName, std::string& errMsg);
+ /// If this fails the return value will be null and the system's human-readable
+ /// error message is in errMsg.
+ static void* getSymbolAddress(void* handle, const std::string& name, std::string& errMsg);
+ /// Given a handle returned by loadPluginByFileName(), unload the plugin. Nothing
+ /// happens until the last reference is unloaded.
+ static void unloadPlugin(void* handle);
+
+ /// Dismantle a supplied library's file or pathname into its component
+ /// parts. This can take pathnames like <pre>
+ /// /usr/local/libMyDll_d.so
+ /// e:\\Program Files\\Something\\myLibrary_d.dll
+ /// </pre> and chop them into <pre>
+ /// directory libPrefix baseName debug extension
+ /// ------------------------------- --------- ----------- ----- ---------
+ /// /usr/local/ lib MyDll _d .so
+ /// e:\\Program Files\\Something\\ (none) myLibrary _d .dll
+ /// </pre>
+ /// as well as tell you whether the given pathname is absolute or relative
+ /// (and thus subject to search rules). At the beginning of the pathname
+ /// (or right after the drive specification on Windows) we recognize
+ /// three special symbols:
+ /// - "/" means root; i.e., this is an absolute path name starting from
+ /// the root directory (this drive's root for Windows).
+ /// - "." starts an absolute path name which is relative to the
+ /// current working directory (or drive's cwd on Windows).
+ /// - "@" starts an absolute path name which is relative to the
+ /// directory in which the currently running executable is located.
+ ///
+ /// Anywhere in the pathname, the name ".." means "go up one level from
+ /// the prior directory". ".." at the start is interpreted as "./..".
+ /// A '.' appearing anywhere in the path name except the begining is
+ /// ignored. An '@' appearing anywhere in the pathname other than the
+ /// beginning is treated as an ordinary file character.
+ ///
+ /// The pathname components are returned
+ /// as separate strings with separators included such that concatenating
+ /// all the strings reproduces the pathname in a canonicalized form.
+ /// The "drive" letter prefix is recognized only when running on Windows;
+ /// otherwise a prefix like "C:" is treated as ordinary file name
+ /// characters. Note that we include the drive letter as part of the
+ /// absolute directory.
+ /// White space is removed, and path separator characters
+ /// in the directory are changed to the appropriate slash
+ /// for the currently running platform (i.e. backslash for
+ /// Windows and forward slash everywhere else).
+ /// The return value is false if the input path name is
+ /// ill-formed; in that case it still tries to parse as
+ /// much as it can.
+ static bool deconstructLibraryName( const std::string& name,
+ bool& isAbsolutePath,
+ std::string& directory,
+ std::string& libPrefix,
+ std::string& baseName,
+ std::string& debugSuffix,
+ std::string& extension);
+
+ /// This is the platform dependent string which gets prepended to
+ /// a dynamic library baseName to form the fileName -- "lib" on Unix-based
+ /// systems and "" (empty string) for Windows.
+ static std::string getDynamicLibPrefix();
+
+ /// This is the platform dependent extension (including the ".") used
+ /// by default to identify dynamically linked libraries -- ".so" on
+ /// Linux, ".dylib" on Apple, and ".dll" on Windows.
+ static std::string getDynamicLibExtension();
+
+ /// Obtain the appropriate debug suffix to use. This is not platform
+ /// dependent but rather depends on whether this compilation unit
+ /// was built in Debug or Release modes. If Debug, then the string
+ /// "_d" is returned, otherwise the empty string.
+ static std::string getDynamicLibDebugSuffix();
+
+protected:
+ std::string m_defaultName; // if any
+ Array_<std::string> m_searchPath;
+
+ std::string m_loadedPathname; // absolute
+ void* m_handle;
+ mutable std::string m_lastMessage;
+
+};
+
+
+#define SimTK_PLUGIN_XXX_MAKE_HOLDER(FuncName) \
+ struct FuncName##__Holder__ { \
+ FuncName##__Holder__() : fp(0) {} \
+ bool loadSym(void* h, std::string& msg) const { \
+ if(!fp) fp =(FuncName##__Type__) \
+ Plugin::getSymbolAddress(h, #FuncName, msg); \
+ return (fp!=0); \
+ } \
+ mutable FuncName##__Type__ fp; \
+ } FuncName##__Ref__
+#define SimTK_PLUGIN_XXX_MAKE_BODY(FuncName) \
+ if (!FuncName##__Ref__.loadSym(m_handle,m_lastMessage)) \
+ throw std::runtime_error \
+ ("Plugin function " #FuncName " not found: " + m_lastMessage); \
+ return FuncName##__Ref__.fp
+#define SimTK_PLUGIN_XXX_MAKE_SYMTEST(Symbol) \
+ bool has_##Symbol() const { \
+ return Symbol##__Ref__.loadSym(m_handle,m_lastMessage); \
+ }
+
+#define SimTK_PLUGIN_DEFINE_SYMBOL(Type, SymName) \
+ typedef Type SymName##__Type__; \
+ SimTK_PLUGIN_XXX_MAKE_HOLDER(SymName); \
+ const Type& SymName() const { \
+ if (!SymName##__Ref__.loadSym(m_handle,m_lastMessage)) \
+ throw std::runtime_error \
+ ("Plugin symbol " #SymName " not found: " + m_lastMessage); \
+ return *(SymName##__Ref__.fp); \
+ }
+
+#define SimTK_PLUGIN_DEFINE_FUNCTION(RetType, FuncName) \
+ typedef RetType (*FuncName##__Type__)(); \
+ SimTK_PLUGIN_XXX_MAKE_HOLDER(FuncName); \
+ RetType FuncName() const { \
+ SimTK_PLUGIN_XXX_MAKE_BODY(FuncName)(); \
+ } \
+ SimTK_PLUGIN_XXX_MAKE_SYMTEST(FuncName)
+
+#define SimTK_PLUGIN_DEFINE_FUNCTION1(RetType, FuncName, Arg1) \
+ typedef RetType (*FuncName##__Type__)(Arg1); \
+ SimTK_PLUGIN_XXX_MAKE_HOLDER(FuncName); \
+ RetType FuncName(Arg1 a1) const { \
+ SimTK_PLUGIN_XXX_MAKE_BODY(FuncName)(a1); \
+ } \
+ SimTK_PLUGIN_XXX_MAKE_SYMTEST(FuncName)
+
+#define SimTK_PLUGIN_DEFINE_FUNCTION2(RetType, FuncName, Arg1, Arg2) \
+ typedef RetType (*FuncName##__Type__)(Arg1,Arg2); \
+ SimTK_PLUGIN_XXX_MAKE_HOLDER(FuncName); \
+ RetType FuncName(Arg1 a1, Arg2 a2) const { \
+ SimTK_PLUGIN_XXX_MAKE_BODY(FuncName)(a1,a2); \
+ } \
+ SimTK_PLUGIN_XXX_MAKE_SYMTEST(FuncName)
+
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_PLUGIN_H_
+
+
diff --git a/SimTKcommon/include/SimTKcommon/internal/PrivateImplementation.h b/SimTKcommon/include/SimTKcommon/internal/PrivateImplementation.h
new file mode 100644
index 0000000..2a0e00b
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/PrivateImplementation.h
@@ -0,0 +1,381 @@
+#ifndef SimTK_PRIVATE_IMPLEMENTATION_H_
+#define SimTK_PRIVATE_IMPLEMENTATION_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Christopher Bruns, 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This header provides declarations of the user-visible portion of the
+ * PIMPLHandle template classes that are used in the SimTK Core to implement
+ * the PIMPL (private implementation) design pattern.
+ *
+ * The definitions associated with these template method declarations are
+ * separated into the companion header file PrivateImplementation_Defs.h
+ * with the intent that those definitions will be visible only in library-side
+ * code where they need to be instantiated. The definition header file is
+ * available for end users as part of the SimTK Core installation, but is
+ * not automatically included with SimTKcomon.h as this file is.
+ *
+ * SimTK Core client-side headers include this declarations file in order to
+ * define the various client side Handle classes, but SimTK Core client-side
+ * code <em>never</em> includes the definitions; that is done exclusively in
+ * private, library-side code.
+ */
+
+#include "SimTKcommon/internal/common.h"
+#include "SimTKcommon/internal/ExceptionMacros.h"
+
+#include <cassert>
+#include <iosfwd>
+
+namespace SimTK {
+
+
+/**
+ * This class provides some infrastructure useful in making SimTK Private
+ * Implementation (PIMPL) classes. These consist of a "handle" class and
+ * an "implementation" class. The handle contains only a single pointer,
+ * which points to the implementation class whose definition is unknown
+ * to the SimTK client. The implementation class has a pointer back to
+ * *one* of the handles that points to it -- that one is called the "owner
+ * handle" and is the only one which will delete the implementation object
+ * when the handle is deleted or goes out of scope. All other handles are
+ * merely references to the implementation object, and must not be used
+ * after the owner handle is deleted.
+ *
+ * The methods defined below require a definition for the implementation
+ * class, so can't be instantiated on the client side. Instead they are
+ * instantiated on the library side when needed in the implementation of
+ * the PIMPL handle class derived from the PIMPLHandle base.
+ *
+ * By the time of instantiation, we must have a definition for the IMPL
+ * class supplied to the templatized base class. We expect that the IMPL
+ * class will be derived from PIMPLImplementation declared below. We
+ * also expect to find certain methods defined, with these names and
+ * meanings:
+ *
+ * IMPL* IMPL::clone() const
+ * This creates an implementation object identical to the one
+ * we have, except that its owner handle is set to null. We
+ * expect the owner handle to be filled in by the derived
+ * Handle class, which should have initiated the PIMPLHandle
+ * operation which had the need to clone().
+ *
+ * const HANDLE& IMPL::getOwnerHandle() const
+ * If the IMPL object does not have an owner, this is expected
+ * to assert(); that would indicate that the derived Handle class
+ * did not properly register itself as the owner upon construction.
+ * Otherwise, this routine returns a reference to the *derived*
+ * Handle class, NOT to the PIMPLHandle parent class! We expect,
+ * however that the Handle class was derived from this PIMPLHandle
+ * so that we can static_cast up here and then compare with 'this'.
+ *
+ * Usage:
+ * class MySecretImpl;
+ * class MyHandle : public PIMPLHandle<MyHandle,MySecretImpl>
+ *
+ * There is an optional third template argument, a bool, which can be
+ * set true if you want the handle to have pointer semantics rather than
+ * the usual object ("value") semantics. Pointer semantics objects
+ * have shallow copy constuctor and copy assignment implementations so
+ * that they are normally references to objects rather than owners,
+ * and pointer semantics owner handles can't be the target of an
+ * assignment.
+ */
+template <class HANDLE, class IMPL, bool PTR=false>
+class PIMPLHandle {
+private:
+ /// This is the only data member allowed in a handle class. It is
+ /// guaranteed to stay this way for eternity. That is, a well-formed
+ /// SimTK Core handle class is just a pointer; and this fact may be
+ /// depended upon when necessary.
+ IMPL *impl;
+
+public:
+ typedef PIMPLHandle<HANDLE, IMPL, PTR> HandleBase;
+ typedef HandleBase ParentHandle;
+
+ /// Returns true if this handle is empty, that is, does not refer
+ /// to any implementation object.
+ bool isEmptyHandle() const {return impl==0;}
+
+ /// Returns true if this handle is the owner of the implementation
+ /// object to which it refers. An empty handle is <em>not</em>
+ /// considered by this method to be an owner. You can check for an
+ /// empty handle using isEmptyHandle().
+ /// @see isEmptyHandle()
+ bool isOwnerHandle() const;
+
+ /// Determine whether the supplied handle is the same object as
+ /// "this" PIMPLHandle.
+ bool isSameHandle(const HANDLE& other) const;
+
+ /// Give up ownership of the implementation to an empty handle. The
+ /// current handle retains a reference to the implementation but is
+ /// no longer its owner. This method requires the current handle to
+ /// be an owner, and the supplied handle to be empty.
+ void disown(HANDLE& newOwner);
+
+
+ /// "Copy" assignment but with shallow (pointer) semantics. As long as
+ /// this is not an owner handle already, make it reference the source
+ /// implementation. It is not allowed for an <em>owner</em> handle
+ /// to be the target of a reference assignment; clear the handle explicitly
+ /// first with clearHandle() if you want to do that.
+ /// This is the default copy and assignment behavior for pointer semantics handle
+ /// classes (that is, those which set the PTR template argument to true).
+ /// Caution: although the PIMPLHandle is taken const here, we obtain
+ /// a writable pointer to the implementation, meaning that it can
+ /// be modified through the reference handle if that handle is non-const.
+ /// @see copyAssign()
+ /// @see operator=()
+ /// @see clearHandle()
+ PIMPLHandle& referenceAssign(const HANDLE& source);
+
+ /// This is real copy assignment, with ordinary C++ object ("value") semantics.
+ /// Deletes the current implementation if owned; then replaces with a new
+ /// copy of the source implementation, of which this handle will be the owner.
+ /// This is the default copy and assignment behavior for normal handle objects,
+ /// that is, those that let the PTR template argument default or set it to
+ /// false explicitly. Use referenceAssign() to make a handle refer to an
+ /// existing implementation rather than creating a new copy.
+ /// @see referenceAssign()
+ PIMPLHandle& copyAssign(const HANDLE& source);
+
+ /// Make this an empty handle, deleting the implementation object if
+ /// this handle is the owner of it. A call to isEmptyHandle() will return
+ /// true after this.
+ /// @see isEmptyHandle()
+ void clearHandle();
+
+ /// Get a const reference to the implementation associated with this Handle.
+ /// This will throw an exception if there is no implementation.
+ const IMPL& getImpl() const {assert(!isEmptyHandle()); return *impl;}
+
+ /// Get a writable reference to the implementation associated with this Handle.
+ /// Note that this requires writable access to the handle also. This will
+ /// throw an exception if there is no implementation.
+ IMPL& updImpl() {assert(!isEmptyHandle()); return *impl;}
+
+ /// Return the number of handles the implementation believes are referencing it.
+ /// Throws an exception if there is no implementation.
+ /// This is for degugging and consistency checking and shouldn't normally be used.
+ int getImplHandleCount() const;
+
+protected:
+ /// The default constructor makes this an empty handle.
+ PIMPLHandle() : impl(0) {}
+
+ /// This provides consruction of a handle referencing an existing
+ /// implementation object. If the supplied pointer is null the result is
+ /// the same as the default constructor.
+ explicit PIMPLHandle(IMPL* p);
+
+ /// Note that the destructor is non-virtual. This is a concrete class and so
+ /// should be all the handle classes derived from it. If this handle is the
+ /// owner of its implementation, the destructor will destroy the implementation
+ /// object as well. Any other handles referencing the same implementation will
+ /// then be invalid, although there will be automated detection of that. Be very
+ /// careful to ensure that owner handles always outlive their reference handles.
+ ~PIMPLHandle();
+
+ /// The copy constructor makes either a deep (value) or shallow (reference) copy
+ /// of the supplied source PIMPL object, based on whether this is a "pointer
+ /// semantics" (PTR=true) or "object (value) semantics" (PTR=false, default)
+ /// class.
+ /// @see referenceAssign
+ /// @see copyAssign
+ PIMPLHandle(const PIMPLHandle& source);
+
+ /// Copy assignment makes the current handle either a deep (value) or shallow
+ /// (reference) copy of the supplied source PIMPL object, based on whether this
+ /// is a "pointer sematics" (PTR=true) or "object (value) semantics" (PTR=false,
+ /// default) class. In the case of a pointer semantics class, an owner handle
+ /// can <em>not</em> be the target of an assignment. You can call copyAssign()
+ /// directly if you want to make a fresh copy of the source, or you can clear
+ /// this handle first with clearHandle() and then use operator=() or referenceAssign()
+ /// to turn this handle into a mere reference to the source.
+ /// @see referenceAssign
+ /// @see copyAssign
+ PIMPLHandle& operator=(const PIMPLHandle& source);
+
+ /// Set the implementation for this empty handle. This may result in either
+ /// an owner or reference handle, depending on the owner handle reference
+ /// stored in the implementation object. This will throw an exception if the
+ /// handle is already occupied; it <em>cannot</em> be used to replace one
+ /// implementation with another.
+ void setImpl(IMPL* p);
+
+ /// Determine whether the supplied handle is a reference to the same implementation
+ /// object as is referenced by "this" PIMPLHandle.
+ bool hasSameImplementation(const HANDLE& other) const;
+
+private:
+ const HANDLE& downcastToHandle() const {return static_cast<const HANDLE&>(*this);}
+ HANDLE& updDowncastToHandle() {return static_cast<HANDLE&>(*this);}
+};
+
+/**
+ * This class provides some infrastructure useful in creating PIMPL
+ * Implementation classes (the ones referred to by Handles). Note that
+ * this class is used by SimTK Core code ONLY on the library side; it never
+ * appears in headers intended for use by clients. However it is
+ * generally useful enough that we include it here to assist people
+ * who would like to make their own PIMPL classes. Consequently, there
+ * are no binary compatibility issues raised by the exposure of data
+ * members here, and no guarantees that they won't change from release to
+ * relase of the SimTK Core; if the definition should change at some point
+ * the library code will change but it will be using the updated definition
+ * and does not have to coordinate in any way with client code.
+ *
+ * Other users of this class should be aware that if you include it in
+ * code you expose to your own users you may create binary compatibility
+ * problems for yourself. Better to restrict use of this class (and indeed
+ * inclusion of this header file) to your private ".cpp" source code and
+ * not in your API header files.
+ *
+ * The PIMPLImplementation base class keeps track of how many Handles
+ * are referencing it, so that that the last handle to be deleted can
+ * delete the implementation. One handle is designated as the "owner"
+ * handle of this implementation. We keep a pointer to that handle here,
+ * so special handling is required if the owner handle is deleted while
+ * other references still exist.
+ */
+template <class HANDLE, class IMPL>
+class PIMPLImplementation {
+ HANDLE* ownerHandle;
+ mutable int handleCount; // ref count determining when this is destructed
+public:
+ /// This serves as a default constructor and as a way to construct
+ /// an implementation class which already knows its owner handle.
+ /// If the handle is supplied then the handle count is set to one.
+ /// If not (default constructor) owner handle is null and the
+ /// handle count at 0.
+ explicit PIMPLImplementation(HANDLE* h=0);
+
+ /// Get the number of handles known to be referencing this implementation.
+ int getHandleCount() const;
+
+ /// Register that a new handle is referencing this implementation so we
+ /// won't delete the implementation prematurely.
+ void incrementHandleCount() const;
+
+ /// Register the fact that one of the previously-referencing handles no
+ /// longer references this implementation. The remaining number of references
+ /// is returned; if it is zero the caller should delete the implementation.
+ int decrementHandleCount() const;
+
+ /// Note that the base class destructor is non-virtual, although it is
+ /// expected that derived classes will be abstract. Be sure to provide
+ /// a virtual destructor in any abstract class which is derived from
+ /// this base, and be sure to delete a pointer to the abstract class
+ /// <em>not</em> a pointer to this base class!
+ ~PIMPLImplementation();
+
+ /// The copy constructor for the base class makes sure that the
+ /// new object has a null owner handle. A derived class must set
+ /// the appropriate owner handle after this is called, that is, in
+ /// the <em>body</em> (not the initializer list) of the derived
+ /// class's copy constructor. Also the caller must make sure to
+ /// increment the handle count.
+ PIMPLImplementation(const PIMPLImplementation&);
+
+ /// Copy assignment for the base class just makes sure that the
+ /// owner handle is not copied, and that the handle count is zero
+ /// for the copy. Caller is required to register a handle and increment
+ /// the handle counter.
+ PIMPLImplementation& operator=(const PIMPLImplementation& src);
+
+ /// Provide an owner handle for an implementation which currently does
+ /// not have one. This can't be used to <em>replace</em> the owner handle.
+ /// This will increment the handle count also.
+ void setOwnerHandle(HANDLE& p);
+
+ /// Remove the owner reference from an implementation that currently has
+ /// an owner. This decrements the handle count also. The number of remaining
+ /// handles is returned.
+ int removeOwnerHandle();
+
+ /// Replace the current owner handle with another one. This can't be used to
+ /// set the initial owner handle; just to replace an existing one with a
+ /// new one. The handle count is not changed here.
+ void replaceOwnerHandle(HANDLE& p);
+
+ /// Check whether this implementation currently has a reference to its
+ /// owner handle.
+ bool hasOwnerHandle() const;
+
+ /// Check whether a given Handle of the appropriate type is the owner of
+ /// this implementation.
+ bool isOwnerHandle(const HANDLE& p) const;
+
+ /// Return a reference to the owner handle of this implementation. This will
+ /// throw an exception if there is no owner handle currently known to this
+ /// implementation.
+ const HANDLE& getOwnerHandle() const;
+};
+
+template <class H, class IMPL, bool PTR>
+std::ostream& operator<<(std::ostream& o, const PIMPLHandle<H,IMPL,PTR>& h);
+
+// This macro declares methods to be included in classes derived from a PIMPLHandle subclass.
+
+#define SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(DERIVED, DERIVED_IMPL, PARENT) \
+const DERIVED_IMPL& getImpl() const;\
+DERIVED_IMPL& updImpl();\
+const PARENT& upcast() const;\
+PARENT& updUpcast();\
+static bool isInstanceOf(const PARENT& p);\
+static const DERIVED& downcast(const PARENT& p);\
+static DERIVED& updDowncast(PARENT& p);
+
+// This macro provides the definitions for the above declarations.
+
+#define SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(DERIVED, DERIVED_IMPL, PARENT) \
+const DERIVED_IMPL& DERIVED::getImpl() const {\
+ return SimTK_DYNAMIC_CAST_DEBUG<const DERIVED_IMPL&>(PARENT::getImpl());\
+}\
+DERIVED_IMPL& DERIVED::updImpl() {\
+ return SimTK_DYNAMIC_CAST_DEBUG<DERIVED_IMPL&>(PARENT::updImpl());\
+}\
+const PARENT& DERIVED::upcast() const {\
+ return static_cast<const PARENT&>(*this);\
+}\
+PARENT& DERIVED::updUpcast() {\
+ return static_cast<PARENT&>(*this);\
+}\
+bool DERIVED::isInstanceOf(const PARENT& p) {\
+ return dynamic_cast<const DERIVED_IMPL*>(&p.getImpl()) != 0;\
+}\
+const DERIVED& DERIVED::downcast(const PARENT& p) {\
+ assert(isInstanceOf(p));\
+ return static_cast<const DERIVED&>(p);\
+}\
+DERIVED& DERIVED::updDowncast(PARENT& p) {\
+ assert(isInstanceOf(p));\
+ return static_cast<DERIVED&>(p);\
+}\
+
+} // namespace SimTK
+
+#endif // SimTK_PRIVATE_IMPLEMENTATION_H_
diff --git a/SimTKcommon/include/SimTKcommon/internal/PrivateImplementation_Defs.h b/SimTKcommon/include/SimTKcommon/internal/PrivateImplementation_Defs.h
new file mode 100644
index 0000000..dec6b0e
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/PrivateImplementation_Defs.h
@@ -0,0 +1,274 @@
+#ifndef SimTK_PRIVATE_IMPLEMENTATION_DEFS_H_
+#define SimTK_PRIVATE_IMPLEMENTATION_DEFS_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Christopher Bruns, 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This header provides the definitions of the PIMPLHandle template methods
+ * as declared in PrivateImplementation.h, and also the
+ * declaration and definition of the template classes used for creating well-
+ * behaved implementation classes.
+ *
+ * Although this header is part of the SimTK Core installation, it is not automatically
+ * included with SimTKcommon.h. It is intended to be included explicitly in
+ * compilation units where the private implementations are being defined.
+ * When used in SimTK Core software, this header will be included only
+ * in library-side compilation units.
+ */
+
+#include "SimTKcommon/internal/PrivateImplementation.h"
+
+#include <cassert>
+#include <iostream>
+#include <typeinfo>
+
+namespace SimTK {
+
+ /////////////////////////////////////
+ // PIMPLImplementation definitions //
+ /////////////////////////////////////
+
+template <class HANDLE, class IMPL>
+PIMPLImplementation<HANDLE, IMPL>::PIMPLImplementation(HANDLE* h) : ownerHandle(h), handleCount(h ? 1 : 0) {
+}
+
+template <class HANDLE, class IMPL>
+int PIMPLImplementation<HANDLE, IMPL>::getHandleCount() const {
+ return handleCount;
+}
+
+template <class HANDLE, class IMPL>
+void PIMPLImplementation<HANDLE, IMPL>::incrementHandleCount() const {
+ handleCount++;
+}
+
+template <class HANDLE, class IMPL>
+int PIMPLImplementation<HANDLE, IMPL>::decrementHandleCount() const {
+ assert(handleCount>=1); return --handleCount;
+}
+
+template <class HANDLE, class IMPL>
+PIMPLImplementation<HANDLE, IMPL>::~PIMPLImplementation() {
+ assert(handleCount==0); ownerHandle=0;
+}
+
+template <class HANDLE, class IMPL>
+PIMPLImplementation<HANDLE, IMPL>::PIMPLImplementation(const PIMPLImplementation&) : ownerHandle(0), handleCount(0) {
+}
+
+template <class HANDLE, class IMPL>
+PIMPLImplementation<HANDLE, IMPL>& PIMPLImplementation<HANDLE, IMPL>::operator=(const PIMPLImplementation& src) {
+ if (&src != this)
+ ownerHandle=0, handleCount=0;
+ return *this;
+}
+
+template <class HANDLE, class IMPL>
+void PIMPLImplementation<HANDLE, IMPL>::setOwnerHandle(HANDLE& p) {
+ assert(!hasOwnerHandle());
+ ownerHandle=&p;
+ incrementHandleCount();
+}
+
+template <class HANDLE, class IMPL>
+int PIMPLImplementation<HANDLE, IMPL>::removeOwnerHandle() {
+ assert(hasOwnerHandle());
+ ownerHandle=0;
+ return decrementHandleCount();
+}
+
+template <class HANDLE, class IMPL>
+void PIMPLImplementation<HANDLE, IMPL>::replaceOwnerHandle(HANDLE& p) {
+ assert(hasOwnerHandle());
+ ownerHandle=&p;
+}
+
+template <class HANDLE, class IMPL>
+bool PIMPLImplementation<HANDLE, IMPL>::hasOwnerHandle() const {
+ return ownerHandle != 0;
+}
+
+template <class HANDLE, class IMPL>
+bool PIMPLImplementation<HANDLE, IMPL>::isOwnerHandle(const HANDLE& p) const {
+ return hasOwnerHandle() && ownerHandle==&p;
+}
+
+template <class HANDLE, class IMPL>
+const HANDLE& PIMPLImplementation<HANDLE, IMPL>::getOwnerHandle() const {
+ assert(hasOwnerHandle());
+ return *ownerHandle;
+}
+
+ /////////////////////////////
+ // PIMPLHandle definitions //
+ /////////////////////////////
+
+template <class HANDLE, class IMPL, bool PTR>
+/*explicit*/ PIMPLHandle<HANDLE,IMPL,PTR>::
+PIMPLHandle(IMPL* p) : impl(p) {
+ // this bumps the reference count in the implementation
+ if (impl) impl->setOwnerHandle(updDowncastToHandle());
+}
+
+// destructor
+template <class HANDLE, class IMPL, bool PTR>
+PIMPLHandle<HANDLE,IMPL,PTR>::~PIMPLHandle() {
+ // reduces the implementation reference count and deletes it if it hits 0
+ clearHandle();
+}
+
+// copy constructor
+template <class HANDLE, class IMPL, bool PTR>
+PIMPLHandle<HANDLE,IMPL,PTR>::PIMPLHandle(const PIMPLHandle& src) : impl(0) {
+ if (PTR) referenceAssign(src.downcastToHandle());
+ else copyAssign(src.downcastToHandle());
+}
+
+// copy assignment
+template <class HANDLE, class IMPL, bool PTR>
+PIMPLHandle<HANDLE,IMPL,PTR>& PIMPLHandle<HANDLE,IMPL,PTR>::
+operator=(const PIMPLHandle& src) {
+ if (PTR) referenceAssign(src.downcastToHandle());
+ else copyAssign(src.downcastToHandle());
+ return *this;
+}
+
+template <class HANDLE, class IMPL, bool PTR>
+bool PIMPLHandle<HANDLE,IMPL,PTR>::isOwnerHandle() const {
+ return impl && impl->hasOwnerHandle() &&
+ static_cast<const PIMPLHandle*>(&impl->getOwnerHandle()) == this;
+}
+
+template <class HANDLE, class IMPL, bool PTR>
+bool PIMPLHandle<HANDLE,IMPL,PTR>::isSameHandle(const HANDLE& other) const {
+ return static_cast<const PIMPLHandle*>(&other) == this;
+}
+
+
+template <class HANDLE, class IMPL, bool PTR>
+bool PIMPLHandle<HANDLE,IMPL,PTR>::hasSameImplementation(const HANDLE& other) const {
+ return impl && (impl==other.impl);
+}
+
+// The current (this) handle is an owner. Here it transfers ownership to the supplied
+// new empty handle, while retaining a reference to the implementation.
+template <class HANDLE, class IMPL, bool PTR>
+void PIMPLHandle<HANDLE,IMPL,PTR>::
+disown(HANDLE& newOwner) {
+ assert(!isSameHandle(newOwner));
+ assert(!this->isEmptyHandle() && newOwner.isEmptyHandle());
+ newOwner.impl = impl;
+ impl->replaceOwnerHandle(newOwner);
+ // since the old handle retains a reference, there is now one more handle
+ impl->incrementHandleCount();
+}
+
+// Reference assignment:
+// - if target (this) is an owner handle, throw an exception; we don't allow that
+// - if source and target have same implementation, there is nothing to do
+// - otherwise, clear the handle, then set implementation and bump handle count
+template <class HANDLE, class IMPL, bool PTR>
+PIMPLHandle<HANDLE,IMPL,PTR>& PIMPLHandle<HANDLE,IMPL,PTR>::
+referenceAssign(const HANDLE& src) {
+ assert(!isOwnerHandle()); // owner can't be target of a reference assign
+ if (!hasSameImplementation(src)) {
+ clearHandle();
+ impl = src.impl;
+ if (impl)
+ impl->incrementHandleCount();
+ }
+ return *this;
+}
+
+// Copy assignment:
+// - if same handle, nothing to do
+// - clear this handle, decrementing ref count and deleting implementation if necessary
+// - clone the source implementation, then reference the copy in this target handle
+template <class HANDLE, class IMPL, bool PTR>
+PIMPLHandle<HANDLE,IMPL,PTR>& PIMPLHandle<HANDLE,IMPL,PTR>::
+copyAssign(const HANDLE& src) {
+ if (isSameHandle(src)) return *this; // that was easy!
+ clearHandle();
+ if (src.impl) {
+ impl = src.impl->clone(); // NOTE: instantiation requires definition of IMPL class
+ impl->setOwnerHandle(updDowncastToHandle()); // bumps ref count (to 1)
+ assert(impl->getHandleCount() == 1);
+ }
+ return *this;
+}
+
+// Provide an implementation for this empty handle, bumping the handle count.
+// We do not assume this handle is the owner of the implementation; the caller
+// must handle that separately.
+template <class HANDLE, class IMPL, bool PTR>
+void PIMPLHandle<HANDLE,IMPL,PTR>::
+setImpl(IMPL* p){
+ assert(isEmptyHandle());
+ impl=p;
+ impl->incrementHandleCount();
+}
+
+// Remove this handle from its current implementation (if any). If this was the
+// owner handle, we clear the owner reference in the implementation. We decrement
+// the implementation's handle count and delete the implementation if this
+// was the last handle referencing it.
+template <class HANDLE, class IMPL, bool PTR>
+void PIMPLHandle<HANDLE,IMPL,PTR>::
+clearHandle() {
+ if (isEmptyHandle()) return; // handle is already clear
+ const int nHandlesLeft =
+ isOwnerHandle() ? impl->removeOwnerHandle()
+ : impl->decrementHandleCount();
+ if (nHandlesLeft == 0)
+ delete impl;
+ impl=0;
+}
+
+template <class HANDLE, class IMPL, bool PTR>
+int PIMPLHandle<HANDLE,IMPL,PTR>::
+getImplHandleCount() const {
+ assert(!isEmptyHandle());
+ return impl->getHandleCount();
+}
+
+ ////////////////////////////////
+ // TEMPLATIZED GLOBAL METHODS //
+ ////////////////////////////////
+
+template <class HANDLE, class IMPL, bool PTR>
+std::ostream& operator<<(std::ostream& o, const PIMPLHandle<HANDLE,IMPL,PTR>& h) {
+ o << "PIMPLHandle<" << typeid(HANDLE).name() << "," << typeid(IMPL).name() << "> @" << &h;
+ if (h.isEmptyHandle())
+ return o << " is EMPTY." << std::endl;
+
+ if (h.isOwnerHandle()) o << " is OWNER of";
+ else o << " is REFERENCE to";
+
+ return o << " Implementation @" << &h.getImpl() << " (handle count=" << h.getImpl().getHandleCount() << ")" << std::endl;
+}
+
+
+} // namespace SimTK
+
+#endif // SimTK_PRIVATE_IMPLEMENTATION_DEFS_H_
diff --git a/SimTKcommon/include/SimTKcommon/internal/ReferencePtr.h b/SimTKcommon/include/SimTKcommon/internal/ReferencePtr.h
new file mode 100644
index 0000000..04f4703
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/ReferencePtr.h
@@ -0,0 +1,147 @@
+#ifndef SimTK_SimTKCOMMON_REFERENCE_PTR_H_
+#define SimTK_SimTKCOMMON_REFERENCE_PTR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+namespace SimTK {
+
+/** This is a smart pointer that implements "cross reference" semantics where
+a pointer data member of some object is intended to refer to some target
+object in a larger data structure. Judicious use of this container will allow
+you to use compiler-generated copy constructors and copy assignment operators
+for classes which would otherwise have to implement their own in order to
+properly initialize these pointer data members, which must not be copied.
+
+The contained pointer is initialized to null (0) on construction, and it is
+reinitialized to null upon copy construction or copy assignment. That's
+because we are assuming this is part of copying the entire data structure and
+copying the old pointer would create a reference into the old data structure
+rather than the new copy. This pointer does not own the target to which it
+points, and there is no reference counting so it will become stale if the
+target is deleted.
+
+Whether you can write through the pointer is controlled by whether type T
+is a const type. For example %ReferencePtr\<int> is equivalent to an int*, while
+%ReferencePtr\<const int> is equivalent to a const int*.
+
+This class is entirely inline and has no computational or space overhead; it
+contains just a single pointer.
+
+ at see ClonePtr **/
+template <class T> class ReferencePtr {
+public:
+ typedef T element_type;
+ typedef T* pointer;
+ typedef T& reference;
+
+ /** Default constructor creates an empty object. **/
+ ReferencePtr() : p(0) { }
+ /** Construct from a given pointer stores the pointer. **/
+ explicit ReferencePtr(T* tp) : p(tp) { }
+ /** Construct from a reference stores the address of the supplied
+ object. **/
+ explicit ReferencePtr(T& t) : p(&t) { }
+ /** Copy constructor unconditionally sets the pointer to null; see class
+ comments for why. **/
+ ReferencePtr(const ReferencePtr&) : p(0) { }
+ /** Copy assignment sets the pointer to null (except for a self-assign);
+ see class comments for why. **/
+ ReferencePtr& operator=(const ReferencePtr& r)
+ { if (&r != this) clear(); return *this; }
+ /** This form of assignment replaces the currently-referenced object by a
+ reference to the source object; no destruction occurs. **/
+ ReferencePtr& operator=(T& t)
+ { reset(&t); return *this; }
+ /** This form of assignment replaces the current pointer with the given
+ one; no destruction occurs. **/
+ ReferencePtr& operator=(T* tp)
+ { reset(tp); return *this; }
+
+ /** Destructor does nothing. **/
+ ~ReferencePtr() {clear();} // just being tidy
+
+ /** Return the contained pointer. This will fail if the container is
+ empty. **/
+ T* operator->() const { return &getRef(); }
+
+ /** The "dereference" operator returns a reference to the target object.
+ This will fail if the container is empty. **/
+ T& operator*() const { return getRef(); }
+
+ /** This is an implicit conversion from %ReferencePtr\<T> to T*. **/
+ operator T*() const { return p; }
+
+ /** This is an implicit conversion to type bool that returns true if
+ the container is non-null (that is, not empty). **/
+ operator bool() const { return !empty(); }
+
+ /** Return the contained pointer, or null if the container is empty. **/
+ T* get() const { return p; }
+
+ /** Return a reference to the target object. Fails if the pointer is
+ null. **/
+ T& getRef() const {
+ SimTK_ERRCHK(p!=0, "ReferencePtr::getRef()",
+ "An attempt was made to dereference a null pointer.");
+ return *p;
+ }
+
+ /** Return true if this container is empty. **/
+ bool empty() const { return p==0; }
+ /** Make this container empty; no destruction occurs. **/
+ void clear() { p=0; }
+ /** Extract the pointer from this container, leaving the container empty.
+ The pointer is returned. **/
+ T* release() { T* x=p; p=0; return x; }
+ /** Replace the stored pointer with a different one; no destruction
+ occurs. **/
+ void reset(T* tp) { p=tp;}
+
+ /** Swap the contents of this %ReferencePtr with another one. **/
+ void swap(ReferencePtr& other) {
+ T* otherp = other.release();
+ other.reset(p);
+ reset(otherp);
+ }
+
+private:
+ // Warning: ReferencePtr must be exactly the same size as type T*. That way
+ // one can reinterpret_cast a T* to a ReferencePtr<T> when needed.
+ T* p;
+};
+
+} // namespace SimTK
+
+namespace std {
+/** This is a specialization of the STL std::swap() algorithm which uses the
+cheap built-in swap() member of the ReferencePtr class.
+ at relates SimTK::ReferencePtr **/
+template <class T> inline void
+swap(SimTK::ReferencePtr<T>& p1, SimTK::ReferencePtr<T>& p2) {
+ p1.swap(p2);
+}
+
+} // namespace std
+
+#endif // SimTK_SimTKCOMMON_REFERENCE_PTR_H_
diff --git a/SimTKcommon/include/SimTKcommon/internal/Serialize.h b/SimTKcommon/include/SimTKcommon/internal/Serialize.h
new file mode 100644
index 0000000..21142c3
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/Serialize.h
@@ -0,0 +1,346 @@
+#ifndef SimTK_SimTKCOMMON_SERIALIZE_H_
+#define SimTK_SimTKCOMMON_SERIALIZE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+This file contains definitions of templatized serialize-to-stream methods
+specialized for the built-in C++ and SimTK low-level classes. These are used
+to provide the concrete level of recursively-defined methods for serialization
+of container classes (Array_, Vector_, Vec, Mat, etc.) each of which should
+define templatized methods of the same names that serialize using the same
+serialization method applied to their elements. Methods are provided for
+different kinds of formatting; at the concrete level these are likely to be
+identical but they differ for containers. Unformatted output will write just
+space separated elements, while formatted output may write surrounding
+parentheses or brackets, comma separators, or whatever else might be
+appropriate.
+**/
+
+#include "SimTKcommon/internal/common.h"
+#include "SimTKcommon/internal/ExceptionMacros.h"
+#include "SimTKcommon/internal/String.h"
+
+#include <iostream>
+
+namespace SimTK {
+
+//------------------------------------------------------------------------------
+// WRITE UNFORMATTED
+//------------------------------------------------------------------------------
+/**
+ at defgroup writeUnformatted writeUnformatted()
+ at ingroup Serialization
+
+Namespace-scope utility method SimTK::writeUnformatted\<T>() writes a value of
+type T to an output stream as a space-separated series of tokens with no
+brackets, commas, semicolons or other formatting characters. If there is only
+one token it is serialized without any added leading or trailing whitespace.
+Output of bool values is "true" or "false"; output of non-finite floating point
+values is "NaN", "Inf", or "-Inf" as in Matlab. For matrix output, we will
+write a newline between each row, meaning there won't be one after the last
+row. This method is specialized for many SimTK types, and you can specialize
+it for your own types in which case containers like Array_\<YourType> will
+work correctly too. **/
+/**@{**/
+
+/** The default implementation of writeUnformatted\<T> converts the object to
+a String using the templatized String constructor, and then writes that
+string to the stream using String::operator<<(). This is suitable for use
+with any of the built-in types. Note that bool will be output "true" or
+"false" and non-finite floating point values are written as NaN, Inf,
+or -Inf as appropriate. **/
+template <class T> inline void
+writeUnformatted(std::ostream& o, const T& v) {
+ o << String(v);
+}
+
+/** Specialize for float to help some compilers with template matching. **/
+template <class T> inline void
+writeUnformatted(std::ostream& o, const float& v)
+{ writeUnformatted<float>(o,v); }
+/** Specialize for double to help some compilers with template matching. **/
+template <class T> inline void
+writeUnformatted(std::ostream& o, const double& v)
+{ writeUnformatted<double>(o,v); }
+/** Specialize for long double to help some compilers with template
+matching. **/
+template <class T> inline void
+writeUnformatted(std::ostream& o, const long double& v)
+{ writeUnformatted<long double>(o,v); }
+
+/** Specialize for SimTK::negator\<T>: convert to T and write. **/
+template <class T> inline void
+writeUnformatted(std::ostream& o, const negator<T>& v)
+{ writeUnformatted(o, T(v)); }
+
+/** Specialize for std::complex\<T>: just write two T's separated by a space;
+no parentheses or comma. **/
+template <class T> inline void
+writeUnformatted(std::ostream& o, const std::complex<T>& v)
+{ writeUnformatted(o, v.real()); o << " "; writeUnformatted(o, v.imag()); }
+
+/** Specialize for SimTK::conjugate\<T>: same as std::complex\<T>. **/
+template <class T> inline void
+writeUnformatted(std::ostream& o, const conjugate<T>& v)
+{ writeUnformatted(o, std::complex<T>(v)); }
+
+
+/** Specialize for Vec<M,E,S> to delegate to element type E, with spaces
+separating the elements.
+ at relates SimTK::Vec **/
+template <int M, class E, int S> inline void
+writeUnformatted(std::ostream& o, const Vec<M,E,S>& v) {
+ for (int i=0; i < M; ++i) {
+ if (i != 0) o << " ";
+ writeUnformatted(o, v[i]);
+ }
+}
+/** Specialize for Row<N,E,S> to delegate to element type E, with spaces
+separating the elements; raw output is same as Vec.
+ at relates SimTK::Row **/
+template <int N, class E, int S> inline void
+writeUnformatted(std::ostream& o, const Row<N,E,S>& v)
+{ writeUnformatted(o, ~v); }
+
+/** Specialize for Mat<M,N,E,CS,RS> delegating to Row<N,E,RS> with newlines
+separating the rows, but no final newline.
+ at relates SimTK::Mat **/
+template <int M, int N, class E, int CS, int RS> inline void
+writeUnformatted(std::ostream& o, const Mat<M,N,E,CS,RS>& v) {
+ for (int i=0; i < M; ++i) {
+ if (i != 0) o << std::endl;
+ writeUnformatted(o, v[i]);
+ }
+}
+
+/** Specialize for SymMat<M,E,RS> delegating to Row<M,E,RS> with newlines
+separating the rows, but no final newline.
+ at relates SimTK::SymMat **/
+template <int M, class E, int RS> inline void
+writeUnformatted(std::ostream& o, const SymMat<M,E,RS>& v) {
+ for (int i=0; i < M; ++i) {
+ if (i != 0) o << std::endl;
+ writeUnformatted(o, v[i]);
+ }
+}
+/**@}**/
+
+
+//------------------------------------------------------------------------------
+// READ UNFORMATTED
+//------------------------------------------------------------------------------
+/**
+ at defgroup readFromStream readUnformatted()
+ at ingroup Serialization
+
+Namespace-scope utility function SimTK::readUnformatted\<T>() reads the next
+whitespace-separated token(s) from a given stream and attempts to interpret
+them as a value of type T. **/
+/**@{**/
+
+
+/** Read in the next whitespace-delimited token as a String, ignoring
+leading whitespace. The token is terminated by whitespace or eof and the
+terminating character is left in the stream. Failure to find any non-whitespace
+characters is treated as a failure. If we're successful at getting a token the
+function returns true; the stream may be good() or eof(). Otherwise this
+function returns false and the fail bit in the stream is also set. **/
+inline bool
+readOneTokenUnformatted(std::istream& in, String& token) {
+ // If the stream is already bad or at eof, we fail.
+ if (!in.good()) {in.setstate(std::ios::failbit); return false;}
+ // Skip whitespace. Failure or eof here means no token.
+ std::ws(in);
+ if (!in.good()) {in.setstate(std::ios::failbit); return false;}
+ in >> token; if (in.fail()) return false;
+ if (token.empty()) {in.setstate(std::ios_base::failbit); return false;}
+ return true;
+}
+
+/** The default implementation of readUnformatted\<T> reads in the next
+whitespace-separated token and then attempts to convert the whole thing into
+one value of type T. If that fails, or if the whole token isn't consumed, we
+return false and the fail bit is set in the stream.
+ at returns true if we are successful in reading the value **/
+template <class T> inline bool
+readUnformatted(std::istream& in, T& v) {
+ String token;
+ if (!readOneTokenUnformatted(in, token)) return false;
+ if (!token.tryConvertTo<T>(v))
+ { in.setstate(std::ios::failbit); return false; }
+ return true;
+}
+
+/** Specialize for float to help some compilers with template matching. **/
+template <class T> inline bool
+readUnformatted(std::istream& in, float& v)
+{ return readUnformatted<float>(in,v); }
+/** Specialize for double to help some compilers with template matching. **/
+template <class T> inline bool
+readUnformatted(std::istream& in, double& v)
+{ return readUnformatted<double>(in,v); }
+/** Specialize for long double to help some compilers with template
+matching. **/
+template <class T> inline bool
+readUnformatted(std::istream& in, long double& v)
+{ return readUnformatted<long double>(in,v); }
+
+/** Specialization for negator<T>: read as type T and convert. **/
+template <class T> inline bool
+readUnformatted(std::istream& in, negator<T>& v) {
+ T nonneg; if (!readUnformatted(in, nonneg)) return false;
+ v = nonneg; // Changes representation, not value.
+ return true;
+}
+
+/** Specialization for std::complex<T> (two space-separated T's). **/
+template <class T> inline bool
+readUnformatted(std::istream& in, std::complex<T>& v) {
+ T real, imag;
+ if (!readUnformatted(in, real)) return false;
+ if (!readUnformatted(in, imag)) return false;
+ v = std::complex<T>(real,imag);
+ return true;
+}
+
+/** Specialization for SimTK::conjugate<T> (same as std::complex<T>). **/
+template <class T> inline bool
+readUnformatted(std::istream& in, conjugate<T>& v) {
+ std::complex<T> cmplx; if (!readUnformatted(in, cmplx)) return false;
+ v = cmplx; // Changes representation, not value.
+ return true;
+}
+
+/** Specialization for SimTK::String (just read token). **/
+template <> inline bool
+readUnformatted<String>(std::istream& in, String& v)
+{ return readOneTokenUnformatted(in,v); }
+
+
+/** Specialize for Vec<M,E,S> to delegate to element type E, with spaces
+separating the elements.
+ at relates SimTK::Vec **/
+template <int M, class E, int S> inline bool
+readUnformatted(std::istream& in, Vec<M,E,S>& v) {
+ for (int i=0; i < M; ++i)
+ if (!readUnformatted(in, v[i])) return false;
+ return true;
+}
+
+/** Specialize for Row<N,E,S> to delegate to element type E, with spaces
+separating the elements; format is same as Vec.
+ at relates SimTK::Row **/
+template <int N, class E, int S> inline bool
+readUnformatted(std::istream& in, Row<N,E,S>& v)
+{ return readUnformatted(in, ~v); }
+
+/** Specialize for Mat<M,N,E,CS,RS> delegating to Row<N,E,RS>.
+ at relates SimTK::Mat **/
+template <int M, int N, class E, int CS, int RS> inline bool
+readUnformatted(std::istream& in, Mat<M,N,E,CS,RS>& v) {
+ for (int i=0; i < M; ++i)
+ if (!readUnformatted(in, v[i])) return false;
+ return true;
+}
+
+/** Specialize for SymMat<M,E,RS>. We require the whole matrix, then
+verify symmetry and assign to the output argument. We'll return false with
+the fail bit set in the stream if we get a Mat\<M,M> but it isn't
+symmetric to a tight numerical tolerance.
+ at see SimTK::SymMat::setFromSymmetric(), SimTK::Mat::isNumericallySymmetric()
+ at relates SimTK::SymMat **/
+template <int M, class E, int RS> inline bool
+readUnformatted(std::istream& in, SymMat<M,E,RS>& v) {
+ Mat<M,M,E> m; if (!readUnformatted(in, m)) return false;
+ if (!m.isNumericallySymmetric()) {
+ in.setstate(std::ios::failbit);
+ return false;
+ }
+ v.setFromSymmetric(m);
+ return true;
+}
+/**@}**/
+
+//------------------------------------------------------------------------------
+// WRITE FORMATTED
+//------------------------------------------------------------------------------
+/**
+ at defgroup writeFormatted writeFormatted()
+ at ingroup Serialization
+
+Namespace-scope utility method SimTK::writeFormatted\<T>() writes a value of
+type T to an output stream in a mildly formatted way depending on the type
+of container represented by T. For the basic types this is the same as
+what you get from the templatized String<T>() constructor when given a value
+of type T. For containers like Array_ or Vector_, this method is specialized as
+part of the definition of that container. Usually the containers will surround
+their contents with some kind of brackets, and separate elements by commas.
+
+Anything written by writeFormatted() can be read back in with readFormatted().
+**/
+/**@{**/
+
+/** The default implementation of writeFormatted\<T> converts the object to
+a String using the templatized String constructor, and then writes that
+string to the stream using String::operator<<(). This is suitable for use
+with any of the built-in and scalar SimTK types. Note that bool will be output
+"true" or "false" and non-finite floating point values are written as NaN,
+Inf, or -Inf as appropriate. Complex numbers will serialize as (real,imag). **/
+template <class T> inline void
+writeFormatted(std::ostream& o, const T& v) {
+ o << String(v);
+}
+/**@}**/
+
+//------------------------------------------------------------------------------
+// READ FORMATTED
+//------------------------------------------------------------------------------
+/**
+ at defgroup readFormatted readFormatted()
+ at ingroup Serialization
+
+Namespace-scope utility method SimTK::readFormatted\<T>() reads a value of
+type T from an input stream, recognizing a variety of formats such as the
+format produced by writeFormatted() or simpler unformatted data such as is
+produced by writeUnformatted(). When T is a container, the recognized
+formats depend on the readFormatted() specialization provided with as part of
+the definition of that container. **/
+/**@{**/
+
+/** The default implementation of readFormatted\<T>() uses
+readUnformatted\<T>(). **/
+template <class T> inline bool
+readFormatted(std::istream& in, T& v) {
+ return readUnformatted(in, v);
+}
+
+// TODO: need specializations for complex to support (real,imag) where the
+// numbers can be NaN, Inf, -Inf.
+/**@}**/
+
+
+
+} // namespace SimTK
+#endif // SimTK_SimTKCOMMON_SERIALIZE_H_
diff --git a/SimTKcommon/include/SimTKcommon/internal/StableArray.h b/SimTKcommon/include/SimTKcommon/internal/StableArray.h
new file mode 100644
index 0000000..d4e6368
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/StableArray.h
@@ -0,0 +1,234 @@
+#ifndef SimTK_SimTKCOMMON_STABLEARRAY_H_
+#define SimTK_SimTKCOMMON_STABLEARRAY_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/internal/common.h"
+#include "SimTKcommon/internal/Array.h"
+
+#include <cstddef>
+#include <cassert>
+
+namespace SimTK {
+
+/**
+ * StableArray<T> is like std::vector<T> (or SimTK::Array_<T>) but more stable
+ * in two ways:
+ * - the addresses of the inserted items never change, even if the array
+ * has to be resized, and
+ * - the index of an inserted item never changes either.
+ *
+ * The above means that once you insert an item (meaning that a copy of
+ * it resides in the StableArray), you can save the address of that copy
+ * and/or its index and be certain that adding or deleting other items
+ * will leave those unaffected. Once an item has been erased, however, we
+ * will feel free to reuse both the heap it once consumed and its index.
+ *
+ * As your punishment for the crime of enjoying this stability guarantee,
+ * consecutive elements of a StableArray are not consecutive in memory.
+ *
+ * CAUTION: this is not suited for use across binary interfaces because
+ * the implementation is fully exposed.
+ */
+template <class T> class StableArray {
+public:
+ StableArray() : nOccupiedSlots(0) { }
+
+ // Allocate and fill every slot with the same value.
+ explicit StableArray(size_t z, const T& ival=T()) : stuff(z), nOccupiedSlots(z) {
+ for (size_t i=0; i<z; ++i) stuff[i] = new T(ival);
+ }
+
+ // Copy constructor must preserve slot numbers.
+ StableArray(const StableArray& s) : stuff(s.size(),0), nOccupiedSlots(0) {
+ for (size_t i=0; i<s.size(); ++i)
+ if (!s.empty(i)) initializeEmptyElement(i, s[i]);
+ assert(nItems() == s.nItems());
+ }
+
+ // Assignment must preserve slot numbers.
+ StableArray& operator=(const StableArray& s) {
+ clear();
+ stuff.resize(s.size(),0);
+ for (size_t i=0; i<s.size(); ++i)
+ if (!s.empty(i)) initializeEmptyElement(i, s[i]);
+ assert(nItems() == s.nItems());
+ return *this;
+ }
+
+ ~StableArray() { clear(); }
+
+ bool empty() const { return stuff.size()==0; }
+ bool empty(size_t i) const {
+ assert(i < stuff.size());
+ return stuff[i] == 0;
+ }
+ size_t size() const {return stuff.size();}
+ size_t nItems() const {return nOccupiedSlots;}
+
+ // This routine preserves as many items as possible and fills
+ // in all empty slots with default values. The array will
+ // thus have exactly newz slots with nItems==newz.
+ // I'm not sure this is useful for anything.
+ void resize(size_t newz, const T& ival=T()) {
+ const size_t oldz = stuff.size();
+ // If we're throwing anything away, destruct it.
+ for (size_t i=newz; i < oldz; ++i)
+ eraseElementIfNecessary(i);
+ stuff.resize(newz,0);
+ // Fill in all empty slots with default values.
+ for (size_t i=0; i < newz; ++i)
+ initializeElementIfNecessary(i,ival);
+ assert(nItems() == newz);
+ }
+
+ void clear() {
+ for (size_t i=0; i < stuff.size(); ++i)
+ eraseElementIfNecessary(i);
+ stuff.resize(0);
+ assert(nItems()==0);
+ }
+
+ // You can push a new item onto the end, or put one in an
+ // empty slot.
+ void push_back(const T& t) {
+ stuff.push_back(new T(t));
+ ++nOccupiedSlots;
+ }
+
+ // Remove the last element and shrink the list by 1. If the second-to-the-last
+ // was empty we'll reduce the length more, so that pop_back() guarantees either
+ // that the the last element (back()) is not empty, or the entire list is empty.
+ // Don't call this on an empty array.
+ void pop_back() {
+ assert(!empty() && stuff.back());
+ eraseOccupiedElement(stuff.size()-1); // last element *better* be occupied!
+
+ // Skip over any exposed empties. No need to adjust count.
+ do { stuff.pop_back(); } while (!stuff.empty() && !stuff.back());
+ }
+
+ void insertAt(size_t i, const T& t) {
+ assert(i <= stuff.size());
+ if (i == size()) push_back(t);
+ else initializeEmptyElement(i,t);
+ }
+
+ size_t findFreeSlot() const {
+ if (nItems() == size())
+ return size(); // no room at the inn!
+ for (size_t i=0; i<size(); ++i)
+ if (empty(i)) return i;
+ assert(false); // where's the empty slot???
+ return size_t(-1);
+ }
+
+ // Look for the first occupied slot at or after i. Returns
+ // size() (that is, one past the end) if none found. Use like this:
+ // for (size_t i=findNextItem(0); i < size(); i=findNextItem(i+1))
+ // do something with item[i] here
+ size_t findNextItem(size_t i) {
+ assert(i < stuff.size());
+ for (; i < stuff.size() && !stuff[i]; ++i);
+ return i;
+ }
+
+ // Insert the item in the first available slot, or grow the array
+ // by one and stick it on the end if there are no free slots. The
+ // slot in which it was placed is returned.
+ size_t insert(const T& t) {
+ const size_t free = findFreeSlot();
+ insertAt(free, t);
+ return free;
+ }
+
+
+ // Erasing an element slot if it isn't already empty. If we erase the
+ // last element we don't have to leave a hole, and in fact we might
+ // expose a hole in the second-to-the-last element too. In that
+ // case we erase by resizing away trailing detritus, otherwise we'll
+ // make a hole.
+ void erase(size_t i) {
+ if (i == stuff.size()-1) pop_back();
+ else eraseElementIfNecessary(i);
+ }
+
+ // This returns the first *occupied* element and blows up if there isn't one.
+ const T& front() const {
+ const size_t firstItem = findNextItem(0);
+ assert(firstItem < stuff.size());
+ return *stuff[firstItem];
+ }
+ T& front() {
+ const size_t firstItem = findNextItem(0);
+ assert(firstItem < stuff.size());
+ return *stuff[firstItem];
+ }
+
+ // We don't need to search for back() because the rules here ensure that the
+ // last element is not empty.
+ const T& back() const {assert(!empty() && stuff.back()); return *stuff.back();}
+ T& back() {assert(!empty() && stuff.back()); return *stuff.back();}
+
+ const T& operator[](size_t i) const {
+ assert(i < stuff.size() && stuff[i]);
+ return *stuff[i];
+ }
+ T& operator[](size_t i) {
+ assert(i < stuff.size() && stuff[i]);
+ return *stuff[i];
+ }
+
+private:
+ size_t nOccupiedSlots; // not counting empty slots
+ Array_<T*,size_t> stuff;
+
+ // Note that this can leave empty slots at the end of the list which
+ // is not a legitimate condition for the StableArray.
+
+ void eraseOccupiedElement(size_t i) {
+ assert(i < stuff.size() && stuff[i]);
+ delete stuff[i]; stuff[i]=0; --nOccupiedSlots;
+ }
+
+ void initializeEmptyElement(size_t i, const T& t) {
+ assert(i < stuff.size() && !stuff[i]);
+ stuff[i] = new T(t); ++nOccupiedSlots;
+ }
+
+ void eraseElementIfNecessary(size_t i) {
+ assert(i < stuff.size());
+ if (stuff[i]) eraseOccupiedElement(i);
+ }
+
+ void initializeElementIfNecessary(size_t i, const T& t) {
+ assert(i < stuff.size());
+ if (!stuff[i]) initializeEmptyElement(i,t);
+ }
+
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_STABLEARRAY_H_
diff --git a/SimTKcommon/include/SimTKcommon/internal/String.h b/SimTKcommon/include/SimTKcommon/internal/String.h
new file mode 100644
index 0000000..f378291
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/String.h
@@ -0,0 +1,469 @@
+#ifndef SimTK_SimTKCOMMON_STRING_H_
+#define SimTK_SimTKCOMMON_STRING_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+
+#include "SimTKcommon/internal/common.h"
+#include "SimTKcommon/internal/ExceptionMacros.h"
+
+#include <cstdio>
+#include <string>
+#include <limits>
+#include <complex>
+#include <sstream>
+
+namespace SimTK {
+
+template <class N> class negator;
+template <class R> class conjugate;
+
+/** SimTK::String is a plug-compatible std::string replacement (plus some
+additional functionality) intended to be suitable for passing through the
+SimTK API without introducing binary compatibility problems the way
+std::string does, especially on Windows. You can work in your own code with
+std::strings which will be quietly converted to and from SimTK::Strings when
+invoking SimTK API methods. Or, you can use SimTK::Strings and still pass them
+to standard library or other methods that are expecting std::strings, usually
+transparently. The SimTK::Array_<T> class is used similarly to avoid binary
+compatibility problems that arise with std::vector<T>.
+
+ at todo Currently this is just derived from std::string and inherits all the
+binary compatibility issues. Use it now anyway and you'll pick up the
+compatibility benefits later, when we get the time ...
+
+ at see SimTK::Array_ **/
+class String : public std::string {
+public:
+
+/** Default constructor produces an empty string. **/
+String() { }
+
+// uses default copy constructor, copy assignment, and destructor
+
+/** This is an implicit conversion from const char* to String. **/
+String(const char* s) : std::string(s) { }
+
+/** We allow creating a String from a char but you have to do it explicitly. **/
+explicit String(char c) {push_back(c);}
+
+/** This is an implicit conversion from std::string to String **/
+String(const std::string& s) : std::string(s) { }
+
+/** Construct a String as a copy of a substring begining at position \a start
+with length \a len. **/
+String(const String& s, int start, int len) : std::string(s,start,len) { }
+
+/** This is an implicit conversion from String to null-terminated C-style
+string (array of chars). **/
+operator const char*() const { return c_str(); }
+
+/** Add operator[] that takes int index instead of size_type. **/
+char& operator[](int i) {
+ assert(i >= 0);
+ return std::string::operator[]((std::string::size_type)i);
+}
+
+/** Add operator[] that takes int index instead of size_type. **/
+char operator[](int i) const {
+ assert(i >= 0);
+ return std::string::operator[]((std::string::size_type)i);
+}
+
+/** Pass through to string::operator[]. **/
+char& operator[](std::string::size_type i) {return std::string::operator[](i);}
+/** Pass through to string::operator[]. **/
+char operator[](std::string::size_type i) const {return std::string::operator[](i);}
+
+/** Override std::string size() method to return an int instead of the
+inconvenient unsigned type size_type. **/
+int size() const {return (int)std::string::size();}
+
+/** Override std::string length() method to return an int instead of the
+inconvenient unsigned type size_type. **/
+int length() const {return (int)std::string::length();}
+
+/** @name Formatted output constructors
+These contructors format the supplied argument into a human-readable %String,
+using a default or caller-supplied printf-like format. By default, maximum
+precision is used for floating point values, and user-friendly strings are
+used for bool (true or false) and non-finite floating point values (NaN,
+Inf, -Inf). **/
+/*@{*/
+/** Format an int as a printable %String. **/
+explicit String(int i, const char* fmt="%d")
+{ char buf[32]; sprintf(buf,fmt,i); (*this)=buf; }
+/** Format a long as a printable %String. **/
+explicit String(long i, const char* fmt="%ld")
+{ char buf[64]; sprintf(buf,fmt,i); (*this)=buf; }
+/** Format a long long as a printable %String. **/
+explicit String(long long i, const char* fmt="%lld")
+{ char buf[64]; sprintf(buf,fmt,i); (*this)=buf; }
+/** Format an unsigned int as a printable %String. **/
+explicit String(unsigned int s, const char* fmt="%u")
+{ char buf[32]; sprintf(buf,fmt,s); (*this)=buf; }
+/** Format an unsigned long as a printable %String. **/
+explicit String(unsigned long s, const char* fmt="%lu")
+{ char buf[64]; sprintf(buf,fmt,s); (*this)=buf; }
+/** Format an unsigned long long as a printable %String. **/
+explicit String(unsigned long long s, const char* fmt="%llu")
+{ char buf[64]; sprintf(buf,fmt,s); (*this)=buf; }
+
+/** Format a float as a printable %String. Nonfinite values are formatted as
+NaN, Inf, or -Inf as appropriate (Matlab compatible). **/
+SimTK_SimTKCOMMON_EXPORT explicit String(float r, const char* fmt="%.7g");
+
+/** Format a double as a printable %String. Nonfinite values are formatted as
+NaN, Inf, or -Inf as appropriate (Matlab compatible). **/
+SimTK_SimTKCOMMON_EXPORT explicit String(double r, const char* fmt="%.15g");
+
+/** Format a long double as a printable %String. Nonfinite values are
+formatted as NaN, Inf, or -Inf as appropriate (Matlab compatible). **/
+SimTK_SimTKCOMMON_EXPORT explicit String(long double r,
+ const char* fmt="%.20Lg");
+
+/** Format a complex\<float> as a printable %String (real,imag) with parentheses
+and a comma as shown. The format string should be for a single float and will
+be used twice; the default format is the same as for float. **/
+explicit String(std::complex<float> r, const char* fmt="%.7g")
+{ (*this)="(" + String(r.real(),fmt) + "," + String(r.imag(),fmt) + ")"; }
+/** Format a complex\<double> as a printable %String (real,imag) with
+parentheses and a comma as shown. The format string should be for a single
+double and will be used twice; the default format is the same as for double. **/
+explicit String(std::complex<double> r, const char* fmt="%.15g")
+{ (*this)="(" + String(r.real(),fmt) + "," + String(r.imag(),fmt) + ")"; }
+/** Format a complex\<long double> as a printable %String (real,imag) with
+parentheses and a comma as shown. The format string should be for a single long
+double and will be used twice; the default format is the same as for long
+double. **/
+explicit String(std::complex<long double> r, const char* fmt="%.20Lg")
+{ (*this)="(" + String(r.real(),fmt) + "," + String(r.imag(),fmt) + ")"; }
+
+/** Format a bool as a printable %String "true" or "false"; if you want "1"
+or "0" cast the bool to an int first. **/
+explicit String(bool b) : std::string(b?"true":"false") { }
+
+/** For any type T for which there is no matching constructor, this templatized
+constructor will format an object of type T into a %String provided that there
+is either an available specialization or (as a last resort) a stream insertion
+operator<<() available for type T. **/
+template <class T> inline explicit String(const T& t); // see below
+
+/** Constructing a %String from a negated value converts to the underlying
+native type and then uses one of the native-type constructors. **/
+template <class T> explicit
+String(const negator<T>& nt) {
+ new (this) String(T(nt));
+}
+/** Same, but allows for format specification. **/
+template <class T>
+String(const negator<T>& nt, const char* fmt) {
+ new (this) String(T(nt), fmt);
+}
+
+/** Constructing a %String from a conjugate value converts to the underlying
+complex type and then uses one of the native-type constructors. **/
+template <class T> explicit
+String(const conjugate<T>& ct) {
+ new (this) String(std::complex<T>(ct));
+}
+/** Same, but allows for format specification. **/
+template <class T>
+String(const conjugate<T>& ct, const char* fmt) {
+ new (this) String(std::complex<T>(ct), fmt);
+}
+
+
+/*@}*/
+
+/** @name Formatted input from String
+These templatized methods attempt to interpret the entire contents of
+this String as a single object of type T (although T may itself be a
+container like an Array or Vector). It is an error if the String has
+the wrong format for an object of this type, or if the entire String is
+not consumed. The acceptable formatting is defined by type T based on what
+it thinks is acceptable stream formatting. Leading and trailing white space
+are ignored except when type T is itself a String or std::string in which case
+the white space is included in the result. It is not acceptable for type T
+to be a pointer type. In particular if you want to convert a String to a null-
+terminated C-style char*, use the standard c_str() method rather than any of
+these.
+ at see Related namespace-level static methods convertStringTo<T>().
+**/
+/*@{*/
+/** Attempt to convert this String to an object of type T, returning a status
+value to indicate success or failure. We require that the whole string is
+consumed except possibly for some trailing white space.
+ at tparam T
+ A non-pointer type that supports extraction operator>>() from an istream.
+ You will get a compilation failure if you try to use this method for a
+ type T for which no extraction operator is available and a runtime error
+ if T is a pointer type.
+ at param[out] out
+ The converted value if we were able to parse the string successfully
+ (i.e., function return is true), otherwise the output value is
+ undefined.
+ at return true if we got what we're looking for, false if anything went
+wrong including failure to consume the entire string.
+ at see convertTo<T>() **/
+template <class T> inline bool tryConvertTo(T& out) const; // see below
+
+/** Convert this String to an object of type T using the tryConvertTo<T>()
+method but throwing an error on failure rather than returning status. Using
+this routine can save you a lot of error-checking code if you were going to
+have to throw an error anyway.
+ at param[out] out The converted value.
+ at see tryConvertTo<T>(out), SimTK::convertStringTo<T>() **/
+template <class T> inline void convertTo(T& out) const; // see below
+
+/** A more convenient form of convertTo<T>() that returns the result as its
+function argument, although this may involve an extra copy operation. For very
+large objects you may want to use the other form where the output is written
+to an already-constructed object you provide.
+ at return The converted value as an object of type T.
+ at see convertTo<T>(out), tryConvertTo<T>(out), SimTK::convertStringTo<T>() **/
+template <class T> T convertTo() const
+{ T temp; convertTo<T>(temp); return temp; }
+
+/** Special-purpose method for interpreting this %String as a bool. Recognizes
+"true" and "false" (in any case) as well as whatever operator>>() accepts.
+Returns false if the contents of this %String, ignoring leading and trailing
+whitespace, can't be interpreted as a bool. **/
+SimTK_SimTKCOMMON_EXPORT bool tryConvertToBool(bool& out) const;
+
+/** Special-purpose method for interpreting this %String as a float. Recognizes
+NaN, [-]Inf, [-]Infinity (in any case) as well as whatever operator>>() accepts.
+Returns false if the contents of this %String, ignoring leading and trailing
+whitespace, can't be interpreted as a float. **/
+SimTK_SimTKCOMMON_EXPORT bool tryConvertToFloat(float& out) const;
+
+/** Special-purpose method for interpreting this %String as a double. Recognizes
+NaN, [-]Inf, [-]Infinity (in any case) as well as whatever operator>>() accepts.
+Returns false if the contents of this %String, ignoring leading and trailing
+whitespace, can't be interpreted as a double. **/
+SimTK_SimTKCOMMON_EXPORT bool tryConvertToDouble(double& out) const;
+
+/** Special-purpose method for interpreting this %String as a long double.
+Recognizes NaN, [-]Inf, [-]Infinity (in any case) as well as whatever
+operator>>() accepts. Returns false if the contents of this %String, ignoring
+leading and trailing whitespace, can't be interpreted as a long double. **/
+SimTK_SimTKCOMMON_EXPORT bool tryConvertToLongDouble(long double& out) const;
+/*@}*/
+
+/** @name In-place modifications
+These are member functions which add to the existing std::string functionality.
+These methods return a reference to "this" String, so may be chained like
+assignment statements. If you would like to use these on an std::string, use
+the String::updAs() method to recast the std::string to a String. Note that
+there is also an equivalent set of static methods which return a new String
+rather than changing the original. **/
+/*@{*/
+/** Upshift the given String in place, so that lowercase letters are replaced
+with their uppercase equivalents as defined by std::toupper(). **/
+SimTK_SimTKCOMMON_EXPORT String& toUpper();
+/** Downshift the given String in place, so that uppercase letters are replaced
+with their lowercase equivalents as defined by std::tolower(). **/
+SimTK_SimTKCOMMON_EXPORT String& toLower();
+/** Trim this String in place, removing all the initial leading and trailing
+white space, as defined by std::isspace() which typically includes space,
+tab (\\t), newline (\\n), return (\\r), and form feed (\\f). **/
+SimTK_SimTKCOMMON_EXPORT String& trimWhiteSpace();
+/** Substitute in place \a newChar for \a oldChar wherever \a oldChar appears
+in this String. **/
+SimTK_SimTKCOMMON_EXPORT String& replaceAllChar(char oldChar, char newChar);
+/*@}*/
+
+
+/** @name Utility methods
+These static methods operate on SimTK::String or std::string objects and return
+SimTK::String objects. **/
+/*@{*/
+/** Upshift the given std::string returning a new SimTK::String in which all
+the letters have been made upper case with toupper(). **/
+static String toUpper(const std::string& in)
+{ return String(in).toUpper(); }
+/** Downshift the given std::string returning a new SimTK::String in which all
+the letters have be made lower case with tolower(). **/
+static String toLower(const std::string& in)
+{ return String(in).toLower(); }
+/** Copy the input std::string to a new SimTK::String leaving off all the
+initial leading and trailing white space, as defined by isspace() which
+typically includes space, tab (\\t), newline (\\n), return (\\r),
+and form feed (\\f). **/
+static SimTK_SimTKCOMMON_EXPORT String trimWhiteSpace(const std::string& in);
+/** Copy the input std::string to a new SimTK::String while substituting
+\a newChar for \a oldChar wherever \a oldChar appears in the input. **/
+String& replaceAllChar(const std::string& in, char oldChar, char newChar)
+{ return String(in).replaceAllChar(oldChar, newChar); }
+/*@}*/
+
+};
+
+// All std::stream activity should be dealt with inline so that we don't have
+// to worry about binary compatibility issues that can arise when passing
+// streams through the API.
+
+/** Generic templatized %String constructor uses T::operator<<() to generate
+the %String when no specialization is available. **/
+template <class T> inline
+String::String(const T& t) {
+ std::ostringstream stream;
+ stream << t;
+ *this = stream.str();
+}
+
+
+// This namespace-level static method should not be necessary but gcc 4.1
+// still has trouble with template specialization for template member
+// functions. So rather than specializing the tryConvertTo() member, I'm
+// specializing this helper function instead.
+template <class T> inline static
+bool tryConvertStringTo(const String& value, T& out) {
+ std::istringstream sstream(value);
+ sstream >> out; if (sstream.fail()) return false;
+ if (sstream.eof()) return true;
+ // Successful conversion but didn't use all the characters. Maybe the
+ // rest is just whitespace?
+ std::ws(sstream); // Skip trailing whitespace if any.
+ return sstream.eof(); // We must have used up the whole string now.
+}
+
+// This specialization ensures that "true" and "false" are recognized as
+// values for bools (with any case).
+template <> inline
+bool tryConvertStringTo(const String& value, bool& out)
+{ return value.tryConvertToBool(out); }
+
+// Specialization to ensure recognition of non-finite values NaN, Inf, etc.
+template <> inline
+bool tryConvertStringTo(const String& value, float& out)
+{ return value.tryConvertToFloat(out); }
+
+// Specialization to ensure recognition of non-finite values NaN, Inf, etc.
+template <> inline
+bool tryConvertStringTo(const String& value, double& out)
+{ return value.tryConvertToDouble(out); }
+
+// Specialization to ensure recognition of non-finite values NaN, Inf, etc.
+template <> inline
+bool tryConvertStringTo(const String& value, long double& out)
+{ return value.tryConvertToLongDouble(out); }
+
+// This specialization ensures that we get the whole String including
+// leading and trailing white space. Of course this is not useful for
+// anything but may occur as a result of some higher-level templatized
+// method that doesn't know what type it is converting here.
+template<> inline
+bool tryConvertStringTo(const String& value, String& out)
+{ out = value; return true; }
+
+// Same as above but for std::string output rather than String.
+template<> inline
+bool tryConvertStringTo(const String& value, std::string& out)
+{ out = value; return true; }
+
+/** Partial specialization to read negator<T> as a T. **/
+template <class T> inline
+bool tryConvertStringTo(const String& value, negator<T>& out) {
+ T nonnegated;
+ if (!tryConvertStringTo(value, nonnegated)) return false;
+ out = nonnegated;
+ return true;
+}
+
+/** Partial specialization to read conjugate<T> as a std::complex<T>. **/
+template <class T> inline
+bool tryConvertStringTo(const String& value, conjugate<T>& out) {
+ std::complex<T> cmplx;
+ if (!tryConvertStringTo(value, cmplx)) return false;
+ out = cmplx;
+ return true;
+}
+
+
+// This partial specialization ensures that you can't interpret
+// a String as a pointer.
+template<class T> inline static
+bool tryConvertStringTo(const String& value, T*& out) {
+ SimTK_ERRCHK1_ALWAYS(false, "SimTK::convertStringTo(value,T*)",
+ "Can't interpret a string as a pointer (%s*).",
+ NiceTypeName<T>::name());
+ return false;
+}
+
+template <class T> inline bool
+String::tryConvertTo(T& out) const
+{ return tryConvertStringTo(*this, out); }
+
+template <class T> inline void
+String::convertTo(T& out) const {
+ const int MaxStr = 50;
+ const bool convertOK = tryConvertTo<T>(out);
+ if (convertOK) return;
+
+ // Make sure we don't try to output more than MaxStr characters of
+ // the bad string in the error message.
+ String shorter = this->substr(0, MaxStr);
+ if (shorter.size() < this->size()) shorter += " ...";
+ SimTK_ERRCHK2_ALWAYS(convertOK, "String::convertTo()",
+ "Couldn't interpret string '%s' as type T=%s.",
+ shorter.c_str(), NiceTypeName<T>::name());
+}
+
+/** This method converts its String argument to type T and returns it into
+the variable supplied as its second argument; this is particularly convenient
+when you have a string literal or std::string since the conversion to String
+happens automatically. For example the two lines shown are equivalent:
+ at code
+ Array_<float> array;
+ convertStringTo("1.2 -4.1e-3 5", array);
+ String("1.2 -4.1e-3 5").convertTo(array);
+ at endcode
+ at see String::convertTo()
+ at relates String **/
+template <class T> inline static
+void convertStringTo(const String& in, T& out)
+{ in.convertTo<T>(out); }
+
+/** This method converts its String argument to type T and returns it as its
+function value; this is particularly convenient when you have a string literal
+or std::string since the conversion to String happens automatically. For
+example the two lines shown are equivalent:
+ at code
+ Array_<float> array = convertStringTo< Array_<float> >("1.2 -4.1e-3 5");
+ Array_<float> array = String("1.2 -4.1e-3 5").convertTo< Array_<float> >();
+ at endcode
+ at see String::convertTo()
+ at relates String **/
+template <class T> inline static
+T convertStringTo(const String& in)
+{ return in.convertTo<T>(); }
+
+}
+#endif // SimTK_SimTKCOMMON_STRING_H_
diff --git a/SimTKcommon/include/SimTKcommon/internal/ThreadLocal.h b/SimTKcommon/include/SimTKcommon/internal/ThreadLocal.h
new file mode 100644
index 0000000..4fadbd1
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/ThreadLocal.h
@@ -0,0 +1,150 @@
+#ifndef SimTK_SimTKCOMMON_THREAD_LOCAL_H_
+#define SimTK_SimTKCOMMON_THREAD_LOCAL_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 <pthread.h>
+#include <map>
+#include <set>
+
+namespace SimTK {
+
+static std::map<void*, pthread_key_t> instanceMap;
+static std::map<pthread_key_t, std::set<void*> > keyInstances;
+static pthread_mutex_t keyLock = PTHREAD_MUTEX_INITIALIZER;
+
+template <class T>
+static void cleanUpThreadLocalStorage(void* value) {
+
+ // Delete the value.
+
+ T* t = reinterpret_cast<T*>(value);
+ delete t;
+
+ // Remove it from the set of values needing to be deleted.
+
+ pthread_mutex_lock(&keyLock);
+ pthread_key_t key = instanceMap[value];
+ instanceMap.erase(value);
+ if (keyInstances.find(key) != keyInstances.end())
+ keyInstances.find(key)->second.erase(value);
+ pthread_mutex_unlock(&keyLock);
+
+}
+
+/**
+ * This class represents a "thread local" variable: one which has a different value on each thread.
+ * This is useful in many situations when writing multithreaded code. For example, it can be used
+ * as temporary workspace for calculations. If a single workspace object were created, all access
+ * to it would need to be synchronized to prevent threads from overwriting each other's values.
+ * Using a ThreadLocal instead means that a separate workspace object will automatically be created
+ * for each thread.
+ *
+ * To use it, simply create a ThreadLocal, then call get() or upd() to get a readable or writable
+ * reference to the value for the current thread:
+ *
+ * <pre>
+ * ThreadLocal<int> x;
+ * ...
+ * x.upd() = 5;
+ * assert(x.get() == 5);
+ * </pre>
+ */
+
+template <class T>
+class ThreadLocal {
+public:
+ /**
+ * Create a new ThreadLocal variable.
+ */
+ ThreadLocal() {
+ this->initialize();
+ }
+ /**
+ * Create a new ThreadLocal variable.
+ *
+ * @param defaultValue the initial value which the variable will have on each thread
+ */
+ ThreadLocal(const T& defaultValue) : defaultValue(defaultValue) {
+ this->initialize();
+ }
+ ~ThreadLocal() {
+
+ // Delete the key.
+
+ pthread_key_delete(key);
+
+ // Once the key is deleted, cleanUpThreadLocalStorage() will no longer be called, so delete
+ // all instances now.
+
+ pthread_mutex_lock(&keyLock);
+ std::set<void*>& instances = keyInstances[key];
+ for (std::set<void*>::const_iterator iter = instances.begin(); iter != instances.end(); ++iter) {
+ instanceMap.erase(*iter);
+ delete (T*) *iter;
+ }
+ keyInstances.erase(key);
+ pthread_mutex_unlock(&keyLock);
+ }
+ /**
+ * Get a reference to the value for the current thread.
+ */
+ T& upd() {
+ T* value = reinterpret_cast<T*>(pthread_getspecific(key));
+ if (value == NULL)
+ return createValue();
+ return *value;
+ }
+ /**
+ * Get a const reference to the value for the current thread.
+ */
+ const T& get() const {
+ T* value = reinterpret_cast<T*>(pthread_getspecific(key));
+ if (value == NULL)
+ return createValue();
+ return *value;
+ }
+private:
+ void initialize() {
+ pthread_key_create(&key, cleanUpThreadLocalStorage<T>);
+ pthread_mutex_lock(&keyLock);
+ keyInstances[key] = std::set<void*>();
+ pthread_mutex_unlock(&keyLock);
+ }
+ T& createValue() const {
+ T* value = new T(defaultValue);
+ pthread_setspecific(key, value);
+ pthread_mutex_lock(&keyLock);
+ instanceMap[value] = key;
+ keyInstances[key].insert(value);
+ pthread_mutex_unlock(&keyLock);
+ return *value;
+ }
+ pthread_key_t key;
+ T defaultValue;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_THREAD_LOCAL_H_
diff --git a/SimTKcommon/include/SimTKcommon/internal/Timing.h b/SimTKcommon/include/SimTKcommon/internal/Timing.h
new file mode 100644
index 0000000..d73acc2
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/Timing.h
@@ -0,0 +1,285 @@
+#ifndef SimTK_SimTKCOMMON_TIMING_H_
+#define SimTK_SimTKCOMMON_TIMING_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+This file ensures that we have access to the Posix time functions
+clock_getttime() and nanosleep(), and also provides some convenient methods
+for use in common timing situations. **/
+
+/**@defgroup TimingFunctions Timing Functions
+ @ingroup GlobalFunctions
+
+These functions provide a convenient way to do high-precision timing, either
+for real time use or performance measurement, and to sleep for precise time
+intervals. Both elapsed and CPU timing is supported, with the latter on a
+per-process or per-thread basis. Times are measured using a double precision
+floating point number of seconds, which is usually the most convenient form.
+Alternatives are available that measure time as a 64 bit integer count of
+nanosecond ticks, providing the highest resolution and consistency for very
+short measurements.
+
+Note that you can also use the Posix functions clock_gettime() and nanosleep()
+on any SimTK-supported platform, as well as usleep() (time in microseconds)
+although Posix considers that obsolete.
+
+On Linux systems, use of these timing functions requires linking with the
+librt realtime library (-lrt). **/
+
+#include "SimTKcommon/internal/common.h"
+#include "SimTKcommon/Constants.h"
+
+// This header is needed on Mac and Linux for some or all of the Posix time
+// functions and the timespec struct. We include it on Windows also for
+// uniform cross-platform behavior, since there are many other useful time-
+// date-handling symbols declared here on all platforms.
+#include <ctime>
+
+#if defined(_MSC_VER)
+ /* On Windows, the timespec struct is not defined. However, note that the
+ timespec struct is also defined in the pthread.h header on Windows, so the
+ guard symbols must match here to avoid a duplicate declaration. */
+ #ifndef HAVE_STRUCT_TIMESPEC
+ #define HAVE_STRUCT_TIMESPEC 1
+ struct timespec {
+ long tv_sec; // TODO: this should be time_t but must fix in pthreads too
+ long tv_nsec;
+ };
+ #endif /* HAVE_STRUCT_TIMESPEC */
+
+ /* Posix nanosleep() sleeps the indicated number of nanoseconds and returns
+ 0, or if it is interrupted early it returns how much time was left in
+ rem and returns EINTR. Ours is not interruptable so will always succeed and
+ return rem==0. It is OK if rem is NULL, but req==NULL or req<0 returns
+ EINVAL. A time of req==0 is allowed and our interpretation is that the
+ thread relinquishes its time slice to another ready-to-run thread if there
+ is one, otherwise returns immediately. This implementation rounds the
+ desired sleep time to the nearest millisecond. On a Linux system, this
+ requires including <time.h> (or <ctime>), which we already included
+ above. */
+ SimTK_SimTKCOMMON_EXPORT int nanosleep(const struct timespec* req, struct timespec* rem);
+
+ /* Posix declares this handy function obsolete, but I don't think it is in
+ any danger of going away. It sleeps for the given number of microseconds.
+ However, using SimTK::sleepInNs() or SimTK::sleepInSec() is safer. */
+ typedef unsigned int useconds_t;
+ inline int usleep(useconds_t us) {
+ struct timespec req;
+ req.tv_sec = (long) (us / 1000000U);
+ req.tv_nsec = (long)((us % 1000000U)*1000U);
+ int status = nanosleep(&req,0);
+ return status ? -1 : 0;
+ }
+#endif
+
+#if defined(_MSC_VER) || defined(__APPLE__)
+ // On Windows and OSX, the Posix clock_gettime function is missing.
+ typedef long clockid_t;
+
+ /* These constants are the clock ids we support. All the varieties of
+ CLOCK_MONOTONIC are high resolution with no NTP adjustments. I measured
+ the resolutions on a single Windows 7 machine; hopefully they are typical
+ (resolution here means how often they are updated):
+ - MONOTONIC (counter): 0.001ms 1us
+ - REALTIME (time of day): 1ms 1000us
+ - CPUTIME (either): 20ms 20000us
+ These are slightly conservative resolutions so you should be able to
+ achieve them in practice. */
+ #define CLOCK_REALTIME 1 // time of day clock, from 1/1/1970
+ #define CLOCK_MONOTONIC 2 // counter from last boot time
+ #define CLOCK_MONOTONIC_HR 3 // "high resolution" (same)
+ #define CLOCK_MONOTONIC_RAW 4 // "not subject to NTP adjustments" (same)
+ #define CLOCK_THREAD_CPUTIME_ID 5 // current thread's cpu time (kernel+user)
+ #define CLOCK_PROCESS_CPUTIME_ID 6 // cumulative cpu time of all threads of
+ // this process, live or dead
+
+ /* Returns zero if it succeeds (or if tp==NULL); otherwise EINVAL. On a
+ Linux system, this requires including <time.h> (or <ctime>) and linking
+ with -lrt to get the realtime library. */
+ SimTK_SimTKCOMMON_EXPORT int clock_gettime(clockid_t clock_id,
+ struct timespec *tp);
+#endif
+
+
+
+namespace SimTK {
+
+/** @defgroup TimeConversions Timespec/Nanosecond/Second Conversions
+ @ingroup TimingFunctions
+
+These inline functions provide a fast and convenient way for doing arithmetic
+with the ugly Posix timespec struct. Use them to convert the timespec to
+a long long integer number of nanoseconds, do arithmetic in that form, and
+then convert back. Negative times are handled correctly (they come up as
+the result of subtraction and comparisons).
+
+We usually prefer to deal with times as a double precision floating point
+number of seconds and functions are provided for converting between
+nanoseconds and seconds in this format.
+
+ at par Cautions:
+ - For long intervals the precision of time in seconds will necessarily be
+ less than the precision of the nanosecond count, since IEEE double
+ precision has a 53 bit mantissa, while 63 bits are available for the count.
+ - A signed long long integer containing a count of nanosecond ticks
+ is limited to time intervals of about +/- 292 years, which is
+ substantially less than can be contained in a timespec, but is plenty
+ for interval timing.
+**/
+
+/**@{**/
+/** Convert a time stored in a timespec struct to the equivalent number
+of nanoseconds (as a signed quantity). @see nsToTimespec() **/
+inline long long timespecToNs(const timespec& ts)
+{ return (long long)ts.tv_sec*1000000000LL + (long long)ts.tv_nsec; }
+
+/** Given a signed number of nanoseconds, convert that into seconds and
+leftover nanoseconds in a timespec struct. @see timespecToNs() **/
+inline void nsToTimespec(const long long& ns, timespec& ts)
+{ ts.tv_sec = (long)(ns / 1000000000LL); // signed
+ if (ns >= 0) ts.tv_nsec = (long)( ns % 1000000000LL);
+ else ts.tv_nsec = -(long)((-ns) % 1000000000LL); }
+
+/** Given a count of nanosecond ticks as a signed 64 bit integer, return
+the same time interval as a double precision floating point number of
+seconds. See @ref TimeConversions for cautions. @see secToNs() **/
+inline double nsToSec(const long long& ns)
+{ return (double)(ns*SimTK_NS_TO_S); }
+
+/** Given a signed time interval as a double precision floating point number of
+seconds, return the same time interval as a count of nanosecond ticks in a
+signed 64 bit integer. See @ref TimeConversions for cautions. @see nsToSec() **/
+inline long long secToNs(const double& s)
+{ return (long long)(s*SimTK_S_TO_NS); }
+/**@}**/
+
+/** @defgroup CPUTimers Measuring CPU Time
+ @ingroup TimingFunctions
+
+These functions provide measurement of CPU time consumed by a process or
+by individual threads. Time includes both kernel and user time together,
+and is reported as a double precision floating point value in seconds.
+CPU timers typically have a very coarse resolution, likely to be in the
+10-50ms range depending on the particulars of your system. That means you
+won't get repeatable results unless you measure substantial amounts of
+CPU time; don't expect to get meaningful information measuring CPU times
+of less than a second or so. **/
+/**@{**/
+
+/** Return the cumulative CPU time in seconds (both kernel and user time) that
+has been used so far by any of the threads in the currently executing process.
+
+ at return CPU time used since this process was created, in seconds, as
+a double precision floating point number.
+ at see threadCpuTime() **/
+inline double cpuTime()
+{ timespec ts;
+ clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &ts);
+ return (double)(timespecToNs(ts)*SimTK_NS_TO_S); }
+
+/** Return the total CPU time in seconds (both kernel and user time) that
+has been used so far by the currently executing thread.
+
+ at return CPU time used since this thread was created, in seconds, as
+a double precision floating point number.
+ at see cpuTime() **/
+inline double threadCpuTime()
+{ timespec ts;
+ clock_gettime(CLOCK_THREAD_CPUTIME_ID, &ts);
+ return (double)(timespecToNs(ts)*SimTK_NS_TO_S); }
+/**@}**/
+
+/** @defgroup ElapsedTime High-Resolution Elapsed Time Measurement and Sleep
+ @ingroup TimingFunctions
+
+These functions provide access to the system's high resolution interval timer,
+and highest precision sleep facility for unscheduling a thread until it is
+needed at a specific later time. The interval timer measures elapsed time from
+some arbitrary starting point (typically since the system was last booted). It
+is expected that this timer provides very precise measurement of short time
+intervals, but cannot be depended upon to measure very long periods without
+drifting. That is, it may not be synchronized to the system time of day.
+
+Generally it is most convenient to measure intervals as a floating point number
+of seconds, however this provides a variable amount of precision as the
+absolute value of the timer increases. For maximum precision, you can obtain
+the timer value as an integer number of nanoseconds instead. You can improve
+accuracy by subtacting the integer counts first before converting to seconds.
+The actual resolution is system-dependent, but it should be able to accurately
+measure elapsed times of 1ms or less, substantially less on some systems. **/
+/**@{**/
+
+/** Return current time on the high-resolution interval timer in nanoseconds,
+as a 64-bit integer count. Generally it is more convenient to use realTime()
+which reports the interval time in seconds instead, but the nanosecond count
+is best for maximum accuracy and consistency.
+
+ at return Elapsed nanoseconds since some arbitrary time, as a 64 bit integer
+count.
+ at see realTime(), nsToSec() **/
+inline long long realTimeInNs() {
+ timespec ts;
+ #ifdef CLOCK_MONOTONIC_RAW
+ clock_gettime(CLOCK_MONOTONIC_RAW, &ts);
+ #else
+ clock_gettime(CLOCK_MONOTONIC, &ts);
+ #endif
+ return timespecToNs(ts);
+}
+
+/** Return current time on the high-resolution interval timer in seconds. For
+maximum precision, you can improve repeatability and accuracy somewhat by
+obtaining the interval times as integer counts using realTimeInNs().
+
+ at return Elapsed seconds since some arbitrary time, as a double precision
+floating point number.
+ at see realTimeInNs() **/
+inline double realTime() {return nsToSec(realTimeInNs());}
+
+/** Sleep for the indicated number of nanoseconds, with the actual precision
+system dependent but intended to be the best achievable, hopefully less than
+5ms in all cases. However, when this wakes up you should not assume you
+reached the desired time; you could wake up a little earlier or a lot later.
+Alternatives to this are %SimTK's sleepInSec() function, or the Posix
+nanosleep() and usleep() functions.
+ at see sleepInSec() **/
+inline void sleepInNs(const long long& ns)
+{ timespec ts;
+ nsToTimespec(ns, ts);
+ nanosleep(&ts, 0); }
+
+/** Sleep for the indicated number of seconds, with the actual precision
+system dependent but intended to be the best achievable, hopefully less than
+5ms in all cases. However, when this wakes up you should not assume you reached
+the desired time; you could wake up a little earlier or a lot later.
+Alternatives to this are the %SimTK sleepInNs() function, or the Posix
+nanosleep() and usleep() functions.
+ at see sleepInNs() **/
+inline void sleepInSec(const double& seconds) {sleepInNs(secToNs(seconds));}
+/**@}**/
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_TIMING_H_
diff --git a/SimTKcommon/include/SimTKcommon/internal/Value.h b/SimTKcommon/include/SimTKcommon/internal/Value.h
new file mode 100644
index 0000000..b54d88d
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/Value.h
@@ -0,0 +1,107 @@
+#ifndef SimTK_SimTKCOMMON_VALUE_H_
+#define SimTK_SimTKCOMMON_VALUE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/internal/common.h"
+
+#include "SimTKcommon/internal/String.h"
+#include "SimTKcommon/internal/Exception.h"
+
+#include <limits>
+#include <typeinfo>
+#include <sstream>
+
+namespace SimTK {
+
+/**
+ * Abstract base class representing an arbitrary value of self-describing type.
+ */
+class AbstractValue {
+public:
+ AbstractValue() { }
+ virtual ~AbstractValue() { }
+
+ virtual String getTypeName() const = 0;
+ virtual String getValueAsString() const = 0;
+ virtual bool isCompatible(const AbstractValue&) const = 0;
+ virtual void compatibleAssign(const AbstractValue&) = 0;
+
+ AbstractValue& operator=(const AbstractValue& v) { compatibleAssign(v); return *this; }
+
+ virtual AbstractValue* clone() const = 0;
+};
+
+inline std::ostream&
+operator<<(std::ostream& o, const AbstractValue& v) { o << v.getValueAsString(); return o; }
+
+/**
+ * Templatized version of the abstract class, providing generic type-specific
+ * functionality that does not require specialization, with automatic conversion
+ * to the underlying type.
+ *
+ * Note that this class requires that type T supports an output operator "<<"
+ * so that we can serialize abstract values.
+ */
+template <class T> class Value : public AbstractValue {
+public:
+ Value() { } // contained value is default-constructed
+ explicit Value(const T& t) : thing(t) { }
+ // default copy, destructor
+
+ // Define assignment explicitly here to avoid implicitly calling AbstractValue's
+ // assignment operator.
+ Value& operator=(const Value& v) { thing = v.thing; return *this; }
+
+ Value& operator=(const T& t) { thing=t; return *this; }
+ operator const T&() const { return thing; } // automatic conversion to T
+ operator T&() { return thing; } // automatic conversion to T
+
+ const T& get() const { return thing; }
+
+ // two ways to assign to a new value
+ void set(const T& t) { thing = t; }
+ T& upd() { return thing; }
+
+ bool isCompatible(const AbstractValue& v) const { return isA(v); }
+ void compatibleAssign(const AbstractValue& v) {
+ if (!isA(v)) SimTK_THROW2(Exception::IncompatibleValues,v.getTypeName(),getTypeName());
+ *this = downcast(v);
+ }
+ String getTypeName() const { return NiceTypeName<T>::name(); }
+ // TODO: should have some general way to serialize these.
+ String getValueAsString() const
+ { return "Value<" + getTypeName() + ">"; }
+
+ AbstractValue* clone() const { return new Value(*this); }
+ SimTK_DOWNCAST(Value,AbstractValue);
+protected:
+ T thing;
+};
+
+
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_VALUE_H_
diff --git a/SimTKcommon/include/SimTKcommon/internal/VectorMath.h b/SimTKcommon/include/SimTKcommon/internal/VectorMath.h
new file mode 100644
index 0000000..e1adbed
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/VectorMath.h
@@ -0,0 +1,466 @@
+#ifndef SimTK_SimTKCOMMON_VECTOR_MATH_H_
+#define SimTK_SimTKCOMMON_VECTOR_MATH_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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/basics.h"
+#include "SimTKcommon/Simmatrix.h"
+
+#include <cmath> // for std:sin, sqrt, etc.
+#include <algorithm> // for std:sort, nth_element, etc.
+
+/** @file
+ * This file defines a large number of standard math functions that can be
+ * applied to vectors and matrices (both the large matrix and small matrix
+ * classes).
+ */
+
+namespace SimTK {
+
+// We can use a single definition for a number of functions that simply call a
+// function on each element, returning a value of the same type.
+// Note that some of these intentionally copy their argument for use as a temp.
+
+#define SimTK_ELEMENTWISE_FUNCTION(func) \
+template <class ELEM> \
+VectorBase<ELEM> func(const VectorBase<ELEM>& v) { \
+ const int size = v.size(); \
+ Vector_<ELEM> temp(size); \
+ for (int i = 0; i < size; ++i) \
+ temp[i] = std::func(v[i]); \
+ return temp; \
+} \
+template <class ELEM> \
+RowVectorBase<ELEM> func(const RowVectorBase<ELEM>& v){\
+ const int size = v.size(); \
+ RowVector_<ELEM> temp(size); \
+ for (int i = 0; i < size; ++i) \
+ temp[i] = std::func(v[i]); \
+ return temp; \
+} \
+template <class ELEM> \
+MatrixBase<ELEM> func(const MatrixBase<ELEM>& v) { \
+ const int rows = v.nrow(), cols = v.ncol(); \
+ Matrix_<ELEM> temp(rows, cols); \
+ for (int i = 0; i < rows; ++i) \
+ for (int j = 0; j < cols; ++j) \
+ temp(i, j) = std::func(v(i, j)); \
+ return temp; \
+} \
+template <int N, class ELEM> \
+Vec<N, ELEM> func(Vec<N, ELEM> v) { \
+ for (int i = 0; i < N; ++i) \
+ v[i] = std::func(v[i]); \
+ return v; \
+} \
+template <int N, class ELEM> \
+Row<N, ELEM> func(Row<N, ELEM> v) { \
+ for (int i = 0; i < N; ++i) \
+ v[i] = std::func(v[i]); \
+ return v; \
+} \
+template <int M, int N, class ELEM> \
+Mat<M, N, ELEM> func(Mat<M, N, ELEM> v) { \
+ for (int i = 0; i < M; ++i) \
+ for (int j = 0; j < N; ++j) \
+ v(i, j) = std::func(v(i, j)); \
+ return v; \
+} \
+template <int N, class ELEM> \
+SymMat<N, ELEM> func(SymMat<N, ELEM> v) { \
+ for (int i = 0; i < N; ++i) \
+ for (int j = 0; j <= i; ++j) \
+ v(i, j) = std::func(v(i, j)); \
+ return v; \
+} \
+
+SimTK_ELEMENTWISE_FUNCTION(exp)
+SimTK_ELEMENTWISE_FUNCTION(log)
+SimTK_ELEMENTWISE_FUNCTION(sqrt)
+SimTK_ELEMENTWISE_FUNCTION(sin)
+SimTK_ELEMENTWISE_FUNCTION(cos)
+SimTK_ELEMENTWISE_FUNCTION(tan)
+SimTK_ELEMENTWISE_FUNCTION(asin)
+SimTK_ELEMENTWISE_FUNCTION(acos)
+SimTK_ELEMENTWISE_FUNCTION(atan)
+SimTK_ELEMENTWISE_FUNCTION(sinh)
+SimTK_ELEMENTWISE_FUNCTION(cosh)
+SimTK_ELEMENTWISE_FUNCTION(tanh)
+
+#undef SimTK_ELEMENTWISE_FUNCTION
+
+// The abs() function.
+
+template <class ELEM>
+VectorBase<typename CNT<ELEM>::TAbs> abs(const VectorBase<ELEM>& v) {
+ return v.abs();
+}
+template <class ELEM>
+RowVectorBase<typename CNT<ELEM>::TAbs> abs(const RowVectorBase<ELEM>& v) {
+ return v.abs();
+}
+template <class ELEM>
+MatrixBase<typename CNT<ELEM>::TAbs> abs(const MatrixBase<ELEM>& v) {
+ return v.abs();
+}
+template <int N, class ELEM>
+Vec<N, typename CNT<ELEM>::TAbs> abs(const Vec<N, ELEM>& v) {
+ return v.abs();
+}
+template <int N, class ELEM>
+Row<N, typename CNT<ELEM>::TAbs> abs(const Row<N, ELEM>& v) {
+ return v.abs();
+}
+template <int M, int N, class ELEM>
+Mat<M, N, typename CNT<ELEM>::TAbs> abs(const Mat<M, N, ELEM>& v) {
+ return v.abs();
+}
+template <int N, class ELEM>
+SymMat<N, typename CNT<ELEM>::TAbs> abs(const SymMat<N, ELEM>& v) {
+ return v.abs();
+}
+
+// The sum() function.
+
+template <class ELEM>
+ELEM sum(const VectorBase<ELEM>& v) {
+ return v.sum();
+}
+template <class ELEM>
+ELEM sum(const RowVectorBase<ELEM>& v) {
+ return v.sum();
+}
+template <class ELEM>
+RowVectorBase<ELEM> sum(const MatrixBase<ELEM>& v) {
+ return v.sum();
+}
+template <int N, class ELEM>
+ELEM sum(const Vec<N, ELEM>& v) {
+ return v.sum();
+}
+template <int N, class ELEM>
+ELEM sum(const Row<N, ELEM>& v) {
+ return v.sum();
+}
+template <int M, int N, class ELEM>
+Row<N, ELEM> sum(const Mat<M, N, ELEM>& v) {
+ return v.sum();
+}
+template <int N, class ELEM>
+Row<N, ELEM> sum(const SymMat<N, ELEM>& v) {
+ return v.sum();
+}
+
+// The min() function.
+
+template <class ELEM>
+ELEM min(const VectorBase<ELEM>& v) {
+ const int size = v.size();
+ ELEM min = NTraits<ELEM>::getMostPositive();
+ for (int i = 0; i < size; ++i) {
+ ELEM val = v[i];
+ if (val < min)
+ min = val;
+ }
+ return min;
+}
+template <class ELEM>
+ELEM min(const RowVectorBase<ELEM>& v) {
+ const int size = v.size();
+ ELEM min = NTraits<ELEM>::getMostPositive();
+ for (int i = 0; i < size; ++i) {
+ ELEM val = v[i];
+ if (val < min)
+ min = val;
+ }
+ return min;
+}
+template <class ELEM>
+RowVectorBase<ELEM> min(const MatrixBase<ELEM>& v) {
+ int cols = v.ncol();
+ RowVectorBase<ELEM> temp(cols);
+ for (int i = 0; i < cols; ++i)
+ temp[i] = min(v(i));
+ return temp;
+}
+template <int N, class ELEM>
+ELEM min(const Vec<N, ELEM>& v) {
+ ELEM min = NTraits<ELEM>::getMostPositive();
+ for (int i = 0; i < N; ++i) {
+ ELEM val = v[i];
+ if (val < min)
+ min = val;
+ }
+ return min;
+}
+template <int N, class ELEM>
+ELEM min(const Row<N, ELEM>& v) {
+ ELEM min = NTraits<ELEM>::getMostPositive();
+ for (int i = 0; i < N; ++i) {
+ ELEM val = v[i];
+ if (val < min)
+ min = val;
+ }
+ return min;
+}
+template <int M, int N, class ELEM>
+Row<N, ELEM> min(const Mat<M, N, ELEM>& v) {
+ Row<N, ELEM> temp;
+ for (int i = 0; i < N; ++i)
+ temp[i] = min(v(i));
+ return temp;
+}
+template <int N, class ELEM>
+Row<N, ELEM> min(const SymMat<N, ELEM>& v) {
+ Row<N, ELEM> temp(~v.getDiag());
+ for (int i = 1; i < N; ++i)
+ for (int j = 0; j < i; ++j) {
+ ELEM value = v.getEltLower(i, j);
+ if (value < temp[i])
+ temp[i] = value;
+ if (value < temp[j])
+ temp[j] = value;
+ }
+ return temp;
+}
+
+// The max() function.
+
+template <class ELEM>
+ELEM max(const VectorBase<ELEM>& v) {
+ const int size = v.size();
+ ELEM max = NTraits<ELEM>::getMostNegative();
+ for (int i = 0; i < size; ++i) {
+ ELEM val = v[i];
+ if (val > max)
+ max = val;
+ }
+ return max;
+}
+template <class ELEM>
+ELEM max(const RowVectorBase<ELEM>& v) {
+ const int size = v.size();
+ ELEM max = NTraits<ELEM>::getMostNegative();
+ for (int i = 0; i < size; ++i) {
+ ELEM val = v[i];
+ if (val > max)
+ max = val;
+ }
+ return max;
+}
+template <class ELEM>
+RowVectorBase<ELEM> max(const MatrixBase<ELEM>& v) {
+ int cols = v.ncol();
+ RowVectorBase<ELEM> temp(cols);
+ for (int i = 0; i < cols; ++i)
+ temp[i] = max(v(i));
+ return temp;
+}
+template <int N, class ELEM>
+ELEM max(const Vec<N, ELEM>& v) {
+ ELEM max = NTraits<ELEM>::getMostNegative();
+ for (int i = 0; i < N; ++i) {
+ ELEM val = v[i];
+ if (val > max)
+ max = val;
+ }
+ return max;
+}
+template <int N, class ELEM>
+ELEM max(const Row<N, ELEM>& v) {
+ ELEM max = NTraits<ELEM>::getMostNegative();
+ for (int i = 0; i < N; ++i) {
+ ELEM val = v[i];
+ if (val > max)
+ max = val;
+ }
+ return max;
+}
+template <int M, int N, class ELEM>
+Row<N, ELEM> max(const Mat<M, N, ELEM>& v) {
+ Row<N, ELEM> temp;
+ for (int i = 0; i < N; ++i)
+ temp[i] = max(v(i));
+ return temp;
+}
+template <int N, class ELEM>
+Row<N, ELEM> max(const SymMat<N, ELEM>& v) {
+ Row<N, ELEM> temp(~v.getDiag());
+ for (int i = 1; i < N; ++i)
+ for (int j = 0; j < i; ++j) {
+ ELEM value = v.getEltLower(i, j);
+ if (value > temp[i])
+ temp[i] = value;
+ if (value > temp[j])
+ temp[j] = value;
+ }
+ return temp;
+}
+
+// The mean() function.
+
+template <class ELEM>
+ELEM mean(const VectorBase<ELEM>& v) {
+ return sum(v)/v.size();
+}
+template <class ELEM>
+ELEM mean(const RowVectorBase<ELEM>& v) {
+ return sum(v)/v.size();
+}
+template <class ELEM>
+RowVectorBase<ELEM> mean(const MatrixBase<ELEM>& v) {
+ return sum(v)/v.nrow();
+}
+template <int N, class ELEM>
+ELEM mean(const Vec<N, ELEM>& v) {
+ return sum(v)/N;
+}
+template <int N, class ELEM>
+ELEM mean(const Row<N, ELEM>& v) {
+ return sum(v)/N;
+}
+template <int M, int N, class ELEM>
+Row<N, ELEM> mean(const Mat<M, N, ELEM>& v) {
+ return sum(v)/M;
+}
+template <int N, class ELEM>
+Row<N, ELEM> mean(const SymMat<N, ELEM>& v) {
+ return sum(v)/N;
+}
+
+// The sort() function.
+
+template <class ELEM>
+VectorBase<ELEM> sort(const VectorBase<ELEM>& v) {
+ const int size = v.size();
+ VectorBase<ELEM> temp(v);
+ std::sort(temp.begin(), temp.end());
+ return temp;
+}
+template <class ELEM>
+RowVectorBase<ELEM> sort(const RowVectorBase<ELEM>& v) {
+ const int size = v.size();
+ RowVectorBase<ELEM> temp(v);
+ std::sort(temp.begin(), temp.end());
+ return temp;
+}
+template <class ELEM>
+MatrixBase<ELEM> sort(const MatrixBase<ELEM>& v) {
+ const int rows = v.nrow(), cols = v.ncol();
+ MatrixBase<ELEM> temp(v);
+ for (int i = 0; i < cols; ++i)
+ temp.updCol(i) = sort(temp.col(i));
+ return temp;
+}
+template <int N, class ELEM>
+Vec<N, ELEM> sort(Vec<N, ELEM> v) { // intentional copy of argument
+ ELEM* pointer = reinterpret_cast<ELEM*>(&v);
+ std::sort(pointer, pointer+N);
+ return v;
+}
+template <int N, class ELEM>
+Row<N, ELEM> sort(Row<N, ELEM> v) { // intentional copy of argument
+ ELEM* pointer = reinterpret_cast<ELEM*>(&v);
+ std::sort(pointer, pointer+N);
+ return v;
+}
+template <int M, int N, class ELEM>
+Mat<M, N, ELEM> sort(Mat<M, N, ELEM> v) { // intentional copy of argument
+ for (int i = 0; i < N; ++i)
+ v.col(i) = sort(v.col(i));
+ return v;
+}
+template <int N, class ELEM>
+Mat<N, N, ELEM> sort(const SymMat<N, ELEM>& v) {
+ return sort(Mat<N, N, ELEM>(v));
+}
+
+// The median() function.
+
+template <class ELEM, class RandomAccessIterator>
+ELEM median(RandomAccessIterator start, RandomAccessIterator end) {
+ const ptrdiff_t size = (ptrdiff_t)(end-start);
+ RandomAccessIterator mid = start+(size-1)/2;
+ std::nth_element(start, mid, end);
+ if (size%2 == 0 && mid+1 < end) {
+ // An even number of element. The median is the mean of the two middle elements.
+ // nth_element has given us the first of them and partially sorted the list.
+ // We need to scan through the rest of the list and find the next element in
+ // sorted order.
+
+ RandomAccessIterator min = mid+1;
+ for (RandomAccessIterator iter = mid+1; iter < end; iter++) {
+ if (*iter < *min)
+ min = iter;
+ }
+ return (*mid+*min)/2;
+ }
+ return *mid;
+}
+template <class ELEM>
+ELEM median(const VectorBase<ELEM>& v) {
+ VectorBase<ELEM> temp(v);
+ return median<ELEM>(temp.begin(), temp.end());
+}
+template <class ELEM>
+ELEM median(const RowVectorBase<ELEM>& v) {
+ RowVectorBase<ELEM> temp(v);
+ return median<ELEM>(temp.begin(), temp.end());
+}
+template <class ELEM>
+RowVectorBase<ELEM> median(const MatrixBase<ELEM>& v) {
+ int cols = v.ncol(), rows = v.nrow();
+ RowVectorBase<ELEM> temp(cols);
+ VectorBase<ELEM> column(rows);
+ for (int i = 0; i < cols; ++i) {
+ column = v.col(i);
+ temp[i] = median<ELEM>(column);
+ }
+ return temp;
+}
+template <int N, class ELEM>
+ELEM median(Vec<N, ELEM> v) { // intentional copy of argument
+ ELEM* pointer = reinterpret_cast<ELEM*>(&v);
+ return median<ELEM>(pointer, pointer+N);
+}
+template <int N, class ELEM>
+ELEM median(Row<N, ELEM> v) { // intentional copy of argument
+ ELEM* pointer = reinterpret_cast<ELEM*>(&v);
+ return median<ELEM>(pointer, pointer+N);
+}
+template <int M, int N, class ELEM>
+Row<N, ELEM> median(const Mat<M, N, ELEM>& v) {
+ Row<N, ELEM> temp;
+ for (int i = 0; i < N; ++i)
+ temp[i] = median(v(i));
+ return temp;
+}
+template <int N, class ELEM>
+Row<N, ELEM> median(const SymMat<N, ELEM>& v) {
+ return median(Mat<N, N, ELEM>(v));
+}
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_VECTOR_MATH_H_
diff --git a/SimTKcommon/include/SimTKcommon/internal/Xml.h b/SimTKcommon/include/SimTKcommon/internal/Xml.h
new file mode 100644
index 0000000..11002fe
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/Xml.h
@@ -0,0 +1,1609 @@
+#ifndef SimTK_SimTKCOMMON_XML_H_
+#define SimTK_SimTKCOMMON_XML_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * 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/internal/common.h"
+#include "SimTKcommon/internal/Array.h"
+#include "SimTKcommon/internal/String.h"
+
+#include <iterator>
+#include <iostream>
+
+namespace SimTK {
+
+// These are declared but never defined; all TinyXML code is hidden.
+class TiXmlNode;
+class TiXmlElement;
+class TiXmlAttribute;
+class TiXmlText;
+class TiXmlComment;
+class TiXmlUnknown;
+
+/** This class provides a minimalist capability for reading and writing XML
+documents, as files or strings. This is based with gratitude on the excellent
+open source XML parser TinyXML (http://www.grinninglizard.com/tinyxml/). Note
+that this is a <em>non-validating</em> parser, meaning it deals only with the
+XML file itself and not with a Document Type Definition (DTD), XML Schema, or
+any other description of the XML file's expected contents. Instead, the
+structure of your code that uses this class encodes the expected structure
+and contents of the XML document.
+
+Our in-memory model of an XML document is simplified even further than
+TinyXML's. There a lot to know about XML; you could start here:
+http://en.wikipedia.org/wiki/XML. However, everything you need to know in
+order to read and write XML documents with the SimTK::Xml class is described
+below.
+
+Much of the detailed documention is in the class Xml::Element; be sure to look
+there as well as at this overview.
+
+<h2>Our in-memory model of an XML document</h2>
+
+We consider an XML document to be a tree of "Nodes". There are only four
+types of nodes: Comments, Unknowns, %Text, and Elements. Only Elements can
+contain Text and other nodes, including recursively child Element nodes.
+Elements can also have "Attributes" which are name:value pairs (not nodes).
+
+The XML document as a whole is represented by an object of class Xml::Document.
+The Xml::Document object directly contains a short list of nodes, consisting
+only of Comments, Unknowns, and a single %Element called the "root element". The
+tag word associated with the root element is called the "root tag" and
+conventionally identifies the kind of document this is. For example, XML files
+produced by VTK begin with a root tag "<VTKFile>".
+
+We go to some pain to make sure every Xml::Document fits the above model so
+that you don't have to think about anything else. For example, if the file as
+read in has multiple root-level elements, or has document-level text, we will
+enclose all the element and text nodes within document start tag "<_Root>"
+and end tag "</_Root>" thus making it fit the description above. We call
+this "canonicalizing" the document.
+
+<h3>%Value Elements</h3>
+
+Element nodes can be classified into "value elements" and "compound
+elements". A value element is a "leaf" element (no child elements) that
+contains at most one Text node. For example, a document might contain value
+elements like these:
+ at code
+ <name>John Doe</name>
+ <rating>7.2</rating>
+ <winnings currency=euro>3429</winnings>
+ <preferences/>
+ <vector>1.2 -4 2e-3</vector>
+ at endcode
+All of these have a unique value so it makes sense to talk about "the" value
+of these elements (the empty "preferences" element has a null value). These
+are very common in XML documents, and the Xml::Element class makes them very
+easy to work with. For example, if Element elt is the "<vector>" element
+from the example, you could retrieve its value as a Vec3 like this:
+ at code
+ Vec3 v = elt.getValueAs<Vec3>();
+ at endcode
+This would automatically throw an error if the element wasn't a value element
+or if its value didn't have the right format to convert to a Vec3.
+
+Note that it is okay for a value element to have attributes; those are ignored
+in determining the element's value. Any element that is not a value element is
+a "compound element", meaning it has either child elements and/or more than
+one %Text node.
+
+<h2>Reading an XML document</h2>
+
+To read an XML document, you create an Xml::Document object and tell it to
+read in the document from a file or from a string. The document will be parsed
+and canonicalized into the in-memory model described above. Then to rummage
+around in the document, you ask the Xml::Document object for its root element,
+and check the root tag to see that it is the type of document you are
+expecting. You can check the root element's attributes, and then process its
+contents (child nodes). Iterators are provided for running through all the
+attributes, all the child nodes contained in the element, or all the child
+nodes of a particular type. For a child node that is an element,
+you check the tag and then pass the element to some piece of code that knows
+how to deal with that kind of element and its children recursively.
+
+Here is a complete example of reading in an Xml file "example.xml", printing
+the root tag and then the types of all the document-level nodes, in STL
+iterator style:
+ at code
+ Xml::Document doc("example.xml");
+ cout << "Root tag: " << ex.getRootTag() << endl;
+ for (Xml::node_iterator p=doc.node_begin(); p != doc.node_end(); ++p)
+ cout << "Node type: " << p->getNodeTypeAsString() << endl;
+ at endcode
+Exactly one of the above nodes will have type "ElementNode"; that is the
+root element. To print out the types of nodes contained in the root element,
+you could write:
+ at code
+ Xml::Element root = ex.getRootElement();
+ for (Xml::node_iterator p=root.node_begin(); p != root.node_end(); ++p)
+ cout << "Node type: " << p->getNodeTypeAsString() << endl;
+ at endcode
+
+(Some confessions: despite appearances, "Xml" is not a namespace, it is a
+class with the other Xml classes being internal classes of Xml. An object of
+type Xml is an XML document; the name Xml::Document is a typedef synonymous
+with Xml.)
+
+<h2>Writing an XML document</h2>
+
+You can insert, remove, and modify nodes and attributes in a document, or
+create a document from scratch. Then you can write the results in a
+"pretty-printed" or compact format to a file or a string; for pretty-printing
+you can override the default indentation string (four spaces). Whenever we
+write an XML document, we write it in canoncial format, regardless of how it
+looked when we found it.
+
+At the document level, you can only insert Comment and Unknown nodes. Text and
+Element nodes can be inserted only at the root element level and below.
+
+<h2>Details about XML</h2>
+
+This section provides detailed information about the syntax of XML files as
+we accept and produce them. You won't have to know these details to read and
+write XML files using the SimTK::Xml class, but you may find this helpful for
+when you have to look at an XML file in a text editor.
+
+<h3>Lexical elements</h3>
+
+(Ignore the quote characters below; those are present so I can get this text
+through Doxygen.)
+ - An XML document is a string of Unicode characters; all metadata is case
+ sensitive.
+ - The file begins with a "declaration" tag beginning with "<?xml" and ending
+ with "?>"
+ - Comments look like this: "<!--" \e anything "-->"
+ - The characters in an XML file represent \e markup and \e content
+ - Markup consists of "tags" delimited by "<" and ">", \e attributes denoted
+ by \e name="value", and character escapes delimited by "&" and ";".
+ - Tags come in three flavors: \e start tags like "<word>", \e end tags like
+ "</word>" and <em>empty element</em> tags like "<word/>". Tag words must
+ begin with a letter or an underscore and are case sensitive; "xml" is
+ reserved; don't use it.
+ - Attributes are recognized only in start tags, empty element tags, and
+ declaration tags. In standard XML the value must be quoted with single or
+ double quotes, but we'll supply missing quotes if there are none.
+ Attribute names are case sensitive and must be unique within a tag; but if
+ we see duplicates we'll just ignore all but the last.
+ - There are five pre-defined escapes: "<" and ">" representing "<"
+ and ">", "&" for ampersand, "'" for apostrophe (single quote)
+ and """ for double quote.
+ - There are also "numeric character reference" escapes of the form "&#nnnnn;"
+ (decimal) or "&#xnnnn;" (hex), with only as many digits as needed.
+ - %Text set off by "<![CDATA[" and "]]>" is interpreted as a raw byte stream.
+ - Tags that begin "<x" where x is not a letter or underscore and isn't one
+ of the above recognized patterns will be passed through uninterpreted.
+ - Anything else is Unicode text.
+
+<h3>File structure</h3>
+
+An XML file contains a single \e document which consists at the top level of
+ - a declaration
+ - comments and unknowns
+ - a root element
+ - more comments and unknowns
+
+Elements can be containers of other nodes and are thus the basis for the tree
+structure of XML files. Elements can contain:
+ - comments
+ - unknowns
+ - text
+ - child elements, recursively
+ - attributes
+
+A declaration (see below) also has attributes, but there are only three:
+version, encoding, and standalone ('yes' or 'no'). Unknowns are constructs
+found in the file that are not recognized; they might be errors but they are
+likely to be more sophisticated uses of XML that our feeble parser doesn't
+understand. Unknowns are tags where the tag word doesn't begin with a letter or
+underscore and isn't one of the very few other tags we recognize, like
+comments. As an example, a DTD tag like this would come through as an Unknown
+node here:
+ at code
+ <!DOCTYPE note SYSTEM "Note.dtd">
+ at endcode
+
+Here is the top-level structure we expect of a well-formed XML document, and
+we will impose this structure on XML documents that don't have it. This allows
+us to simplify the in-memory model as discussed above.
+ at code
+ <?xml version="1.0" encoding="UTF-8"?>
+ <!-- maybe comments and unknowns -->
+ <roottag attr=value ... >
+ ... contents ...
+ </roottag>
+ <!-- maybe comments and unknowns -->
+ at endcode
+That is, the first line should be a declaration, most commonly exactly the
+characters shown above, without the "standalone" attribute which will
+default to "yes". If we don't see a declaration when reading an XML
+document, we'll assume we read the one above. Then the document should contain
+exactly one root element representing the type of document and
+document-level attributes. The tag for the root element is not literally
+"roottag" but some name that makes sense for the given document. Note that the
+root element is an ordinary element so "contents" can contain text and child
+elements (as well as comments and unknowns).
+
+When reading an XML document, if it has exactly one document-level element and
+no document-level text, we'll take the document as-is. If there is
+more than one document-level element, or we find some document-level text,
+we'll assume that the root element is missing and act as though we had seen a
+root element "<_Root>" at the beginning and "</_Root>" at the end so
+the root tag will be "_Root". Note that this means that we will
+interpret even a plain text file as a well-formed XML document:
+ at code
+ A file consisting <?xml version="1.0" encoding="UTF-8" ?>
+ of just text ==> <_Root>
+ like this. A file consisting of just text like this.
+ </_Root>
+ at endcode
+The above XML document has a single document-level element and that element
+contains one Text node whose value is the original text.
+
+ at see Xml::Element, Xml::Node **/
+
+//------------------------------------------------------------------------------
+// XML
+//------------------------------------------------------------------------------
+class SimTK_SimTKCOMMON_EXPORT Xml {
+public:
+
+// These local classes are used to describe the contents of an XML document.
+class Attribute;
+class Node; // This is the abstract handle type for any node.
+class Comment; // These are the concrete node types.
+class Unknown; // "
+class Text; // "
+class Element; // "
+
+/** This typedef allows Xml::Document to be used as the type of the document
+which is more conventional than using just Xml, and provides future
+compatibility should we decide to upgrade Xml::Document to a class. **/
+typedef Xml Document;
+
+// This provides iteration over all the attributes found in a given element.
+class attribute_iterator;
+
+// This provides iteration over all the nodes, or nodes of a certain type,
+// at either the Xml document level or over the child nodes of an element.
+class node_iterator;
+
+// This provides iteration over all the element nodes that are children
+// of a given element, or over the subset of those child elements that has
+// a particular tag word.
+class element_iterator;
+
+/** The NodeType enum serves as the actual type of a node and as a filter
+for allowable node types during an iteration over nodes. We consider
+Element and Text nodes to be meaningful, while Comment and Unknown
+nodes are meaningless junk. However, you are free to extract some meaning
+from them if you know how. In particular, DTD nodes end up as Unknown. **/
+enum NodeType {
+ NoNode = 0x00, ///< Type of empty Node handle, or null filter
+ ElementNode = 0x01, ///< Element node type and only-Elements filter
+ TextNode = 0x02, ///< Text node type and only-Text nodes filter
+ CommentNode = 0x04, ///< Comment node type and only-Comments filter
+ UnknownNode = 0x08, ///< Unknown node type and only-Unknowns filter
+
+ NoJunkNodes = ElementNode|TextNode, ///< Filter out meaningless nodes
+ JunkNodes = CommentNode|UnknownNode, ///< Filter out meaningful nodes
+ AnyNodes = NoJunkNodes|JunkNodes ///< Allow all nodes
+};
+
+/** Translate a NodeType to a human-readable string. **/
+static String getNodeTypeAsString(NodeType type);
+
+/**@name Construction
+You can start with an empty Xml::Document or initialize it from a file. **/
+/*@{*/
+
+/** Create an empty XML Document with default declaration and default root
+element with tag "_Root".\ (You should invoke this as Xml::Document() instead
+of just Xml().)
+
+If you were to print out this document now you would see:
+ at code
+ <?xml version="1.0" encoding="UTF-8"?>
+ <_Root />
+ at endcode **/
+Xml();
+
+/** Create a new XML document and initialize it from the contents of the given
+file name.\ (You should invoke this as Xml::Document() instead
+of just Xml().)
+
+An exception will be thrown if the file doesn't exist or can't be
+parsed. @see readFromFile(), readFromString() **/
+explicit Xml(const String& pathname);
+
+/** Copy constructor makes a deep copy of the entire source document; nothing
+is shared between the source and the copy. **/
+Xml(const Xml::Document& source);
+
+/** Copy assignment frees all heap space associated with the current
+Xml::Document and then makes a deep copy of the source document; nothing is
+shared between the source and the copy. **/
+Xml::Document& operator=(const Xml::Document& souce);
+
+/** The destructor cleans up all heap space associated with this document. **/
+~Xml();
+
+/** Restore this document to its default-constructed state. **/
+void clear();
+/*@}*/
+
+/**@name Serializing and I/O
+These methods deal with conversion to and from the in-memory representation
+of the XML document from and to files and strings. **/
+/*@{*/
+/** Read the contents of this Xml::Document from the file whose pathname
+is supplied. This first clears the current document so the new one
+completely replaces the old one. @see readFromString() **/
+void readFromFile(const String& pathname);
+/** Write the contents of this in-memory Xml::Document to the file whose
+pathname is supplied. The file will be created if it doesn't exist,
+overwritten if it does exist. The file will be "pretty-printed" using the
+current indent string. @see setIndentString(), writeToString() **/
+void writeToFile(const String& pathname) const;
+/** Read the contents of this Xml::Document from the supplied string. This
+first clears the current document so the new one completely replaces the
+old one. @see readFromFile() **/
+void readFromString(const String& xmlDocument);
+/** Alternate form that reads from a null-terminated C string (char*)
+rather than a C++ string object. This would otherwise be implicitly
+converted to string first which would require copying. **/
+void readFromString(const char* xmlDocument);
+/** Write the contents of this in-memory Xml::Document to the supplied
+string. The string cleared first so will be completely overwritten.
+Normally the output is "pretty-printed" as it is for a file, but if you
+set \a compact to true the tabs and newlines will be suppressed to make
+a more compact representation. @see setIndentString(), writeToFile() **/
+void writeToString(String& xmlDocument, bool compact = false) const;
+/** Set the string to be used for indentation when we produce a
+"pretty-printed" serialized form of this document.\ The default is
+to use four spaces for each level of indentation.
+ at see writeToFile(), writeToString(), getIndentString() **/
+void setIndentString(const String& indent);
+/** Return the current value of the indent string.\ The default is
+four spaces. @see setIndentString() **/
+const String& getIndentString() const;
+
+/** Set global mode to control whether white space is preserved or condensed
+down to a single space (affects all subsequent document reads; not document
+specific). The default is to condense. **/
+static void setXmlCondenseWhiteSpace(bool shouldCondense);
+/** Return the current setting of the global "condense white space" option.
+Note that this option affects all Xml reads; it is not document specific. **/
+static bool isXmlWhiteSpaceCondensed();
+/*@}*/
+
+
+/**@name Top-level node manipulation
+These methods provide access to the top-level nodes, that is, those that are
+directly owned by the Xml::Document. Comment and Unknown nodes are allowed
+anywhere at the top level, but Text nodes are not allowed and there is just
+one distinguished Element node, the root element. If you want to add Text
+or Element nodes, add them to the root element rather than at the document
+level. **/
+/*@{*/
+
+/** Return an Element handle referencing the top-level element in this
+Xml::Document, known as the "root element". The tag word of this element is
+usually the type of document. This is the \e only top-level element; all others
+are its children and descendents. Once you have the root Element handle, you
+can also use any of the Element methods to manipulate it. If you need a
+node_iterator that refers to the root element (perhaps to use one of the
+top-level insert methods), use node_begin() with a NodeType filter:
+ at code
+ Xml::node_iterator rootp = Xml::node_begin(Xml::ElementNode);
+ at endcode
+That works since there is only one element at this level. **/
+Element getRootElement();
+
+/** Shortcut for getting the tag word of the root element which is usually
+the document type. This is the same as getRootElement().getElementTag(). **/
+const String& getRootTag() const;
+/** Shortcut for changing the tag word of the root element which is usually
+the document type. This is the same as getRootElement().setElementTag(tag). **/
+void setRootTag(const String& tag);
+
+/** Insert a top-level Comment or Unknown node just \e after the location
+indicated by the node_iterator, or at the end of the list if the iterator is
+node_end(). The iterator must refer to a top-level node. The Xml::Document
+takes over ownership of the Node which must be a Comment or Unknown node and
+must have been an orphan. The supplied Node handle will retain a reference
+to the node within the document and can still be used to make changes, but
+will no longer by an orphan. **/
+void insertTopLevelNodeAfter (const node_iterator& afterThis,
+ Node insertThis);
+/** Insert a top-level Comment or Unknown node just \e before the location
+indicated by the node_iterator. See insertTopLevelNodeAfter() for details. **/
+void insertTopLevelNodeBefore(const node_iterator& beforeThis,
+ Node insertThis);
+/** Delete the indicated top-level node, which must not be the root element,
+and must not be node_end(). That is, it must be a top-level Comment or
+Unknown node which will be removed from the Xml::Document and deleted. The
+iterator is invalid after this call; be sure not to use it again. Also, there
+must not be any handles referencing the now-deleted node. **/
+void eraseTopLevelNode(const node_iterator& deleteThis);
+/** Remove the indicated top-level node from the document, returning it as an
+orphan rather than erasing it. The node must not be the root element,
+and must not be node_end(). That is, it must be a top-level Comment or
+Unknown node which will be removed from the Xml::Document and returned as
+an orphan Node. The iterator is invalid after this call; be sure not to use it
+again. **/
+Node removeTopLevelNode(const node_iterator& removeThis);
+/*@}*/
+
+
+/**@name Iteration through top-level nodes (rarely used)
+If you want to run through this document's top-level nodes (of which the
+"root element" is one), these methods provide begin and end
+iterators. By default you'll see all the nodes (types Comment, Unknown,
+and the lone top-level Element) but you can restrict the node types that
+you'll see via the NodeType mask. Iteration is rarely used at this top level
+since you almost never care about about the Comment and Unknown nodes here and
+you can get to the root element directly using getRootElement().
+ at see getRootElement() **/
+/*@{*/
+/** Obtain an iterator to all the top-level nodes or a subset restricted via
+the \a allowed NodeType mask. **/
+node_iterator node_begin(NodeType allowed=AnyNodes);
+
+/** This node_end() iterator indicates the end of a sequence of nodes regardless
+of the NodeType restriction on the iterator being used. **/
+node_iterator node_end() const;
+/*@}*/
+
+/**@name XML Declaration attributes (rarely used)
+These methods deal with the mysterious XML "declaration" line that comes at the
+beginning of every XML document; that is the line that begins with "<?xml"
+and ends with "?>". There are at most three of these attributes and they have
+well-defined names that are always the same (default values shown):
+ - \e version = "1.0": to what version of the XML standard does this document
+ adhere?
+ - \e encoding = "UTF-8": what Unicode encoding is used to represent the
+ character in this document? Typically this is UTF-8, an 8-bit encoding in
+ which the first 128 codes match standard ASCII but where other characters
+ are represented in variable-length multibyte sequences.
+ - \e standalone = "yes": can this document be correctly parsed without
+ consulting other documents?
+
+You can examine and change these attributes with the methods in this section,
+however unless you really know what you're doing you should just leave the
+declaration alone; you'll get reasonable behavior automatically. **/
+/*@{*/
+/** Returns the Xml "version" attribute as a string (from the declaration
+line at the beginning of the document). **/
+String getXmlVersion() const;
+/** Returns the Xml "encoding" attribute as a string (from the declaration
+line at the beginning of the document). **/
+String getXmlEncoding() const;
+/** Returns the Xml "standalone" attribute as a bool (from the declaration
+line at the beginning of the document); default is true ("yes" in a file),
+meaning that the document can be parsed correctly without any other documents.
+We won't include "standalone" in the declaration line for any Xml documents
+we generate unless the value is false ("no" in a file). **/
+bool getXmlIsStandalone() const;
+
+/** Set the Xml "version" attribute; this will be written to the
+"declaration" line which is first in any Xml document. **/
+void setXmlVersion(const String& version);
+/** Set the Xml "encoding" attribute; this doesn't affect the in-memory
+representation but can affect how the document gets written out. **/
+void setXmlEncoding(const String& encoding);
+/** Set the Xml "standalone" attribute; this is normally true (corresponding
+to standalone="yes") and won't appear in the declaration line in that
+case when we write it out. If you set this to false then standalone="no"
+will appear in the declaration line when it is written. **/
+void setXmlIsStandalone(bool isStandalone);
+/*@}*/
+
+//------------------------------------------------------------------------------
+ private:
+friend class Node;
+
+class Impl; // a private, local class Xml::Impl
+const Impl& getImpl() const {assert(impl); return *impl;}
+Impl& updImpl() {assert(impl); return *impl;}
+
+Xml& unconst() const {return *const_cast<Xml*>(this);}
+
+Impl* impl; // This is the lone data member.
+};
+
+/** Output a "pretty printed" textual representation of the given Xml::Document
+to an std::ostream, using the document's current indent string for
+formatting. @see Xml::setIndentString()
+ at relates Xml **/
+// Do this inline so we don't have to pass the ostream through the API.
+inline std::ostream& operator<<(std::ostream& o, const Xml::Document& doc) {
+ String output;
+ doc.writeToString(output);
+ return o << output;
+}
+
+
+
+//------------------------------------------------------------------------------
+// XML ATTRIBUTE
+//------------------------------------------------------------------------------
+/** Elements can have attributes, which are name="value" pairs that appear
+within the element start tag in an XML document; this class represents the
+in-memory representation of one of those attributes and can be used to examine
+or modify the name or value. Attribute names within an element tag are unique.
+**/
+class SimTK_SimTKCOMMON_EXPORT Xml::Attribute {
+public:
+/** Default constructor creates a null Attribute handle. **/
+Attribute() : tiAttr(0) {}
+/** Create a new orphan Attribute, that is, an Attribute that is not
+owned by any Xml Element. **/
+Attribute(const String& name, const String& value);
+/** Copy constructor is shallow; that is, this handle will refer to the
+same attribute as the source. Note that this handle will provide write
+access to the underlying attribute, even if the source was const. **/
+Attribute(const Attribute& src) : tiAttr(src.tiAttr) {}
+/** Copy assignment is shallow; the handle is first cleared and then will
+refer to the same attribute as the source. Note that this handle will
+provide write access to the underlying attribute even if the source handle
+was const. @see clear() **/
+Attribute& operator=(const Attribute& src)
+{ if (&src!=this) {clear(); tiAttr=src.tiAttr;} return *this; }
+/** The Attribute handle destructor does not recover heap space so if you create
+orphan attributes and then don't put them in a document there will be a memory
+leak unless you explicitly destruct them first with clearOrphan(). **/
+~Attribute() {clear();}
+/** Is this handle currently holding an attribute? **/
+bool isValid() const {return tiAttr!=0;}
+/** Return true if this Attribute is an orphan, meaning that it is not
+empty, but is not owned by any element or top-level document. This is
+typically an Attribute object that has just been constructed, or one that
+has been cloned from another Attribute. **/
+bool isOrphan() const;
+/** If this is a valid attribute handle, get the name of the attribute. **/
+const String& getName() const;
+/** If this is a valid attribute handle, get the value of the attribute
+as a String, not including the quotes. **/
+const String& getValue() const;
+/** If this is a valid attribute handle, change its name.
+ at return A reference to this attribute that now has the new name. **/
+Attribute& setName(const String& name);
+/** If this is a valid attribute handle, change its value to the given
+String which should not be quoted.
+ at return A reference to this attribute that now has the new value. **/
+Attribute& setValue(const String& value);
+
+/** This method restores the Attribute handle to its default-constructed
+state but does not recover any heap space; use clearOrphan() if you know
+this attribute was never put into a document. **/
+void clear();
+/** This method explictly frees the heap space for an orphan attribute that
+was created but never inserted into a document. It is an error to call this
+if the attribute is in a document. **/
+void clearOrphan();
+
+/** Serialize this attribute to the given String. The output will be as it
+would appear in an XML file, i.e. name="value" or name='value' with special
+characters output as suitable entities. If you don't want it that way, use
+getName() and getValue() instead which return the raw strings. **/
+void writeToString(String& out) const;
+
+/** Comparison operators return true if the same attribute is being
+referenced or both handles are empty. Note that two different attributes
+with the same properties will not test equal by this criterion. **/
+bool operator==(const Attribute& attr) const {return tiAttr==attr.tiAttr;}
+bool operator!=(const Attribute& attr) const {return tiAttr!=attr.tiAttr;}
+
+//------------------------------------------------------------------------------
+ private:
+friend class Xml::attribute_iterator;
+friend class Xml::Element;
+
+explicit Attribute(TiXmlAttribute* attr) {tiAttr=attr;}
+const TiXmlAttribute& getTiAttr() const {assert(tiAttr);return *tiAttr;}
+TiXmlAttribute& updTiAttr() {assert(tiAttr);return *tiAttr;}
+
+// Careful; this does not clear the handle before replacing the pointer
+// so should not be used if this could be the owner handle of an attribute
+// that hasn't ever been added to a document. It is intended for use by
+// iterators, whose contained Attributes can never be owners.
+void setTiAttrPtr(TiXmlAttribute* attr) {tiAttr=attr;}
+const TiXmlAttribute* getTiAttrPtr() const {return tiAttr;}
+TiXmlAttribute* updTiAttrPtr() {return tiAttr;}
+
+Attribute& unconst() const {return *const_cast<Attribute*>(this);}
+
+TiXmlAttribute* tiAttr; // this is the lone data member
+};
+
+/** Output a textual representation of the given Attribute to an std::ostream.
+This will be in the form the Attribute would appear in an XML file; that is,
+name="value" or name='value' with entity substituion for odd characters,
+without surrounding blanks. @relates Xml::Attribute **/
+// Do this inline so we don't have to pass the ostream through the API.
+inline std::ostream& operator<<(std::ostream& o, const Xml::Attribute& attr) {
+ String output;
+ attr.writeToString(output);
+ return o << output;
+}
+
+
+
+//------------------------------------------------------------------------------
+// XML ATTRIBUTE ITERATOR
+//------------------------------------------------------------------------------
+/** This is a bidirectional iterator suitable for moving forward or backward
+within a list of Attributes within an Element, for writable access. **/
+class SimTK_SimTKCOMMON_EXPORT Xml::attribute_iterator
+: public std::iterator<std::bidirectional_iterator_tag, Xml::Attribute> {
+public:
+/** Default constructor creates an iterator that compares equal to
+attribute_end(). **/
+attribute_iterator() {}
+/** Construct this iterator to point to the same attribute as does the
+supplied Attribute handle (or attribute_end() if the handle is empty). **/
+explicit attribute_iterator(Attribute& attr) : attr(attr) {}
+/** Copy constructor takes an attribute_iterator that can be const, but that
+still allows writing to the Attribute. **/
+attribute_iterator(const attribute_iterator& src)
+: attr(src->updTiAttrPtr()) {}
+/** An iterator destructor never deletes the object to which it refers. **/
+~attribute_iterator() {attr.setTiAttrPtr(0);}
+/** Copy assignment takes an attribute_iterator that can be const, but that
+still allows writing to the Attribute. **/
+attribute_iterator& operator=(const attribute_iterator& src)
+{ attr.setTiAttrPtr(src->updTiAttrPtr()); return *this; }
+/** Prefix increment operator advances the iterator to the next attribute (or
+attribute_end() if it was already at the last attribute) and
+returns a reference to the now-incremented iterator. **/
+attribute_iterator& operator++(); // prefix
+/** Postfix increment operator advances the iterator to the next attribute (or
+attribute_end() if it was already at the last attribute) and
+returns an iterator still referencing the previous one. **/
+attribute_iterator operator++(int); // postfix
+/** Prefix decrement operator moves the iterator to the previous attribute (or
+attribute_end() if it was already at the first attribute) and
+returns a reference to the now-decremented iterator. **/
+attribute_iterator& operator--(); // prefix
+/** Postfix decrement operator moves the iterator to the previous attribute (or
+attribute_end() if it was already at the first attribute) and
+returns an iterator still referencing the original one. **/
+attribute_iterator operator--(int); // postfix
+
+// It's the iterator that's const in these next two methods; it still points
+// to a non-const object just like a char* const p.
+
+/** Return a writable reference to the Attribute referenced by this
+iterator; the handle will be invalid if the iterator was attribute_end(). **/
+Attribute& operator*() const {return const_cast<Attribute&>(attr);}
+/** Return a writable pointer to the Attribute referenced by this
+iterator; the pointer will never be null but the handle it points to will be
+invalid if the iterator was attribute_end(). **/
+Attribute* operator->() const {return const_cast<Attribute*>(&attr);}
+/** Comparison return true only if both iterators refer to the same in-memory
+attribute or both are at attribute_end(); iterators referencing two different
+attributes that happen to have identical properties will not test equal by
+these criteria. **/
+bool operator==(const attribute_iterator& other) const
+{ return other.attr==attr; }
+/** Uses same criteria as operator==(). **/
+bool operator!=(const attribute_iterator& other) const
+{ return other.attr!=attr; }
+
+//------------------------------------------------------------------------------
+ private:
+friend class Xml::Element;
+
+explicit attribute_iterator(TiXmlAttribute* ap) : attr(ap) {}
+
+Attribute attr; // the lone data member
+};
+
+
+
+//------------------------------------------------------------------------------
+// XML NODE
+//------------------------------------------------------------------------------
+/** Abstract handle for holding any kind of node in an XML tree. The concrete
+node handle types derived from Node are: Comment, Unknown, Text, and Element.
+Only an Element node may contain other nodes.
+
+A node may be classified by who owns it. There are three possibilities:
+ - Top-level node: The node belongs to the top-level Xml document and does
+ not have a parent node.
+ - Child node: The node belongs to an element, which may be the root element
+ or any lower-level element. The element that owns it is its "parent".
+ - Orphan node: The node is not yet part of any Xml document and does not
+ belong to an element. In that case the Node handle serves as the owner and
+ the node does not have a parent node.
+
+A Node handle may also be empty, meaning it refers to no node at all so there
+is nothing to own.
+
+Top-level nodes can only be Comment nodes, Unknown nodes, or the lone root
+Element node. Child nodes and orphans can be of Element and Text type also.
+Normally orphans exist only briefly during the time a new node is constructed
+and the time it is adopted by some element (usually in the same constructor)
+so you can ignore them for the most part. But if you must keep orphan nodes
+around, be aware that they must be referenced by only one handle at a time to
+avoid ownership conflicts. **/
+class SimTK_SimTKCOMMON_EXPORT Xml::Node {
+public:
+
+/**@name Construction and destruction
+These methods are mostly used by the derived node classes; Nodes are
+not generally created directly in user code. **/
+/*@{*/
+
+/** Create an empty Node handle that can be used to hold a reference to any
+kind of Node. **/
+Node() : tiNode(0) {}
+/** Copy constructor is shallow; that is, this handle will refer to the
+same node as the source. Note that this handle will provide write
+access to the underlying node, even if the source was const. **/
+Node(const Node& src) : tiNode(src.tiNode) {}
+/** Copy assignment is shallow; the handle is first cleared and then will
+refer to the same node as the source. Note that this handle will
+provide write access to the underlying node even if the source handle
+was const. @see clear() **/
+Node& operator=(const Node& src)
+{ if (&src!=this) {clear(); tiNode=src.tiNode;} return *this; }
+/** The clone() method makes a deep copy of this Node and its children and
+returns a new orphan Node with the same contents; ordinary assignment and
+copy construction is shallow. **/
+Node clone() const;
+/** The Node handle destructor does not recover heap space so if you create
+orphan nodes and then don't put them in a document there will be a memory
+leak unless you explicitly destruct them first with clearOrphan(). **/
+~Node() {clear();}
+/** This method restores the Node handle to its default-constructed
+state but does not recover any heap space; use clearOrphan() if you know
+this node was never put into a document. **/
+void clear();
+/** This method explictly frees the heap space for an orphan node that
+was created but never inserted into a document. It is an error to call this
+if the node is in a document. **/
+void clearOrphan();
+/*@}*/
+
+/**@name Node classification
+You can find out what concrete type of node this abstract Node handle is
+referring to (if any), who owns the node, and if it is owned by a parent
+element you can get access to the parent. **/
+/*@{*/
+
+/** Get the Xml::NodeType of this node. If this Node handle is empty, the
+returned NodeType will be "Xml::NoNode". **/
+NodeType getNodeType() const;
+
+/** Get the Node type as a string; an empty handle returns "NoNode". **/
+String getNodeTypeAsString() const;
+
+/** Return true if this Node handle is referencing some node, false if the
+Node handle is empty. **/
+bool isValid() const {return tiNode != 0;}
+
+/** Return true if this Node is owned by the top-level Xml document, false
+if the Node is owned by an Element or is an orphan, or if the Node handle
+is empty. **/
+bool isTopLevelNode() const;
+
+/** Return true if this Node is an orphan, meaning that it is not empty, but
+is not owned by any element or top-level document. This is typically a Node
+object that has just been constructed, or one that has been cloned from another
+Node. **/
+bool isOrphan() const;
+
+/** Return true if this node has a parent, i.e. it is owned by an element;
+the root element and other top-level nodes are owned by the document and thus
+do not have a parent. @see getParentElement() **/
+bool hasParentElement() const;
+
+/** Return a handle referencing this node's parent if it has one, otherwise
+throws an error; check first with hasParentElement() if you aren't sure. **/
+Element getParentElement();
+/*@}*/
+
+/**@name Access to node contents
+Usually contents inspection is handled at the concrete node class level;
+here we can only provide information for which you don't need to know what
+kind of node this is. **/
+/*@{*/
+
+/** Return a text value associated with this Node (\e not including its child
+nodes if any); the behavior depends on the NodeType. This is a convenience
+that saves downcasting a generic Node to a concrete type when all you want to
+do is dump out the text. It is not particularly useful for Element nodes. Here
+is what you get for each type of node:
+ - Comment: everything between "<!--" and "-->"
+ - Unknown: everything between "<" and ">"
+ - Text: the text
+ - Element: the element's tag word (\e not the element's value)
+ - None: (i.e., an empty handle) throw an error. **/
+const String& getNodeText() const;
+
+/** Serialize this node (and everything it contains) to the given String.
+The output will be "pretty printed" and terminated with a newline unless you
+specify \a compact = true in which case indents and newlines will be
+suppressed. Pretty printing uses the containing Xml::Document's indent string,
+if this Node is in a document, otherwise the default of four spaces for each
+indent level is used. **/
+void writeToString(String& out, bool compact=false) const;
+/*@}*/
+
+/** Comparing Nodes for equality means asking if the two Node handles are
+referring to exactly the same object; two different nodes that happen to have
+the same properties will not test equal by this criteria. **/
+bool operator==(const Node& other) const {return other.tiNode==tiNode;}
+/** Inequality test using same criteria as operator==(). **/
+bool operator!=(const Node& other) const {return other.tiNode!=tiNode;}
+
+
+//------------------------------------------------------------------------------
+ protected:
+/** @cond **/ // don't let Doxygen see these
+explicit Node(TiXmlNode* tiNode) : tiNode(tiNode) {}
+
+const TiXmlNode& getTiNode() const {assert(tiNode);return *tiNode;}
+TiXmlNode& updTiNode() {assert(tiNode);return *tiNode;}
+
+// Careful: these "Ptr" methods provide raw access to the contained
+// pointer without any cleanup or error checking. In particular,
+// setTiNodePtr() does not attempt to delete the current contents.
+void setTiNodePtr(TiXmlNode* node) {tiNode=node;}
+const TiXmlNode* getTiNodePtr() const {return tiNode;}
+TiXmlNode* updTiNodePtr() {return tiNode;}
+/** @endcond **/
+
+//------------------------------------------------------------------------------
+ private:
+friend class Xml;
+friend class Xml::Impl;
+friend class Xml::node_iterator;
+friend class Xml::Comment;
+friend class Xml::Unknown;
+friend class Xml::Text;
+friend class Xml::Element;
+
+Node& unconst() const {return *const_cast<Node*>(this);}
+
+TiXmlNode* tiNode; // the lone data member
+};
+
+/** Output a "pretty printed" textual representation of the given XML
+node (and all its contents) to an std::ostream. Pretty printing uses the
+indent string from the Node's containing Xml::Document, if this Node is in a
+document, otherwise the default of four spaces for each indent level is used.
+ at relates Xml::Node **/
+// Do this inline so we don't have to pass the ostream through the API.
+inline std::ostream& operator<<(std::ostream& o, const Xml::Node& xmlNode) {
+ String output;
+ xmlNode.writeToString(output);
+ return o << output;
+}
+
+
+
+//------------------------------------------------------------------------------
+// XML NODE ITERATOR
+//------------------------------------------------------------------------------
+/** This is a bidirectional iterator suitable for moving forward or backward
+within a list of Nodes, for writable access. By default we will iterate
+over all nodes but you can restrict the types at construction. **/
+class SimTK_SimTKCOMMON_EXPORT Xml::node_iterator
+: public std::iterator<std::bidirectional_iterator_tag, Xml::Node> {
+public:
+
+explicit node_iterator(NodeType allowed=AnyNodes)
+: allowed(allowed) {}
+explicit node_iterator(Node& node, NodeType allowed=AnyNodes)
+: node(node), allowed(allowed) {}
+
+/** Copy constructor takes a node_iterator that can be const, but that
+still allows writing to the Node. **/
+node_iterator(const node_iterator& src)
+: node(*src), allowed(src.allowed) {}
+/** An iterator destructor never deletes the object to which it refers. **/
+~node_iterator() {node.setTiNodePtr(0);}
+/** Copy assignment takes an node_iterator that can be const, but that
+still allows writing to the Node. **/
+node_iterator& operator=(const node_iterator& src)
+{ node = *src; allowed = src.allowed; return *this; }
+
+node_iterator& operator++(); // prefix
+node_iterator operator++(int); // postfix
+node_iterator& operator--(); // prefix
+node_iterator operator--(int); // postfix
+Node& operator*() {return node;}
+Node* operator->() {return &node;}
+// It's the iterator that's const; it still points to a non-const object
+// just like a char* const p.
+Node& operator*() const {return const_cast<Node&>(node);}
+Node* operator->() const {return const_cast<Node*>(&node);}
+bool operator==(const node_iterator& other) const {return other.node==node;}
+bool operator!=(const node_iterator& other) const {return other.node!=node;}
+
+//------------------------------------------------------------------------------
+ protected:
+explicit node_iterator(TiXmlNode* tiNode, NodeType allowed=AnyNodes)
+: node(tiNode), allowed(allowed) {}
+void reassign(TiXmlNode* tiNode)
+{ node.setTiNodePtr(tiNode); }
+
+//------------------------------------------------------------------------------
+ private:
+friend class Xml;
+friend class Xml::Node;
+friend class Xml::Element;
+friend class Xml::element_iterator;
+
+Node node; // data members
+NodeType allowed;
+};
+
+
+
+//------------------------------------------------------------------------------
+// XML ELEMENT ITERATOR
+//------------------------------------------------------------------------------
+/** This is a bidirectional iterator suitable for moving forward or backward
+within a list of Element nodes, for writable access. By default we will iterate
+over all elements in a list but you can restrict the element_iterator at
+construction to find only elements with a particular tag. **/
+class SimTK_SimTKCOMMON_EXPORT Xml::element_iterator
+: public Xml::node_iterator {
+public:
+
+/** This is the default constructor which leaves the element_iterator empty, and
+you can optionally set the type of Element which will be iterated over. **/
+explicit element_iterator(const String& tag="")
+: node_iterator(ElementNode), tag(tag) {}
+/** Constructor an element_iterator pointing to a given Element, and optionally
+set the type of Element which will be iterated over. **/
+inline explicit element_iterator(Element& elt, const String& tag=""); // below
+
+/** Copy constructor takes an element_iterator that can be const, but that
+still allows writing to the Element. **/
+element_iterator(const element_iterator& src)
+: node_iterator(src), tag(src.tag) {}
+
+/** Copy assignment takes an element_iterator that can be const, but that
+still allows writing to the Element. **/
+element_iterator& operator=(const element_iterator& src)
+{ upcast()=src; tag = src.tag; return *this; }
+
+element_iterator& operator++(); // prefix
+element_iterator operator++(int); // postfix
+element_iterator& operator--(); // prefix
+element_iterator operator--(int); // postfix
+inline Element& operator*() const; // below
+inline Element* operator->() const; // below
+
+bool operator==(const element_iterator& other) const
+{ return other.upcast()==upcast();}
+bool operator!=(const element_iterator& other) const
+{ return other.upcast()!=upcast();}
+
+//------------------------------------------------------------------------------
+ private:
+friend class Xml;
+friend class Xml::Element;
+
+explicit element_iterator(TiXmlElement* tiElt, const String& tag="")
+: node_iterator((TiXmlNode*)tiElt, ElementNode), tag(tag) {}
+void reassign(TiXmlElement* tiElt)
+{ upcast().reassign((TiXmlNode*)tiElt); }
+
+const node_iterator& upcast() const
+{ return *static_cast<const node_iterator*>(this); }
+node_iterator& upcast()
+{ return *static_cast<node_iterator*>(this); }
+
+String tag; // lone data member
+};
+
+
+
+
+//------------------------------------------------------------------------------
+// XML ELEMENT
+//------------------------------------------------------------------------------
+/** An element has (1) a tagword, (2) a map of (name,value) pairs called
+attributes, and (3) a list of child nodes. The tag word, which begins with an
+underscore or a letter, can serve as either the type or the name of the element
+depending on context. The nodes can be comments, unknowns, text, and child
+elements (recursively). It is common for "leaf" elements (elements with no
+child elements) to be supplied simply for their values, for example mass might
+be provided via an element "<mass> 29.3 </mass>". We call such elements "value
+elements" since they have a uniquely identifiable value similar to that of
+attributes. %Value elements have no more than one text node. They may have
+attributes, and may also have comment and unknown nodes but they cannot have
+any child elements. This class provides a special set of methods for dealing
+with value nodes very conveniently; they will fail if you attempt to use them
+on an element that is not a value element. **/
+class SimTK_SimTKCOMMON_EXPORT Xml::Element : public Xml::Node {
+public:
+
+/**@name Construction and destruction
+As discussed elsewhere, elements come in two varieties: value elements and
+compound elements. New value elements can be created easily since they are
+essentially just a name,value pair. Compound elements require a series of
+method calls to create the Element node and then add child nodes to it. In
+either case you may want to add attributes also. **/
+/*@{*/
+
+/** Create an empty Element handle; this is suitable only for holding
+references to other Elements. **/
+Element() : Node() {}
+
+/** Create a value element that uses the given tag word but is not yet part of
+any XML document, and optionally give it an inital value. Note that although
+you provide the initial value as a string, you can access it as any type T to
+which that string can be converted, using the getValueAs<T>() templatized
+method.
+
+If no initial value is provided, then the element will be empty so would print
+as "<tagWord />". If you provide a value (say "contents") here or add one
+later, it will print as "<tagWord>contents</tagWord>". In general you can add
+child elements and other node types with subsequent method calls; that would
+change this element from a value element to a compound element.
+ at see getValue(), updValue(), setValue() **/
+explicit Element(const String& tagWord, const String& value="");
+
+/** Create a new value element and set its initial value to the text
+equivalent of any type T for which a conversion construction String(T) is
+allowed (generally any type for which a stream insertion operator<<()
+exists).
+ at see getValueAs<T>(), setValueAs<T>()**/
+template <class T>
+Element(const String& tagWord, const T& value)
+{ new(this) Element(tagWord, String(value)); }
+
+/** The clone() method makes a deep copy of this Element and its children and
+returns a new orphan Element with the same contents; ordinary assignment and
+copy construction are shallow. **/
+Element clone() const;
+
+/** Get the element tag word. This may represent the name or type of the
+element depending on context. **/
+const String& getElementTag() const;
+/** Change the tag word that is used to bracket this element. **/
+void setElementTag(const String& tag);
+
+/** Insert a node into the list of this Element's children, just before the
+node pointed to by the supplied iterator (or at the end if the iterator
+is node_end()). The iterator must refer to a node that is a child of this
+Element. This Element takes over ownership of the node which must
+not already have a parent. **/
+void insertNodeBefore(const node_iterator& pos, Node node);
+/** Insert a node into the list of this Element's children, just after the
+node pointed to by the supplied iterator (or at the end if the iterator
+is node_end()). The iterator must refer to a node that is a child of this
+Element. This Element takes over ownership of the node which must
+not already have a parent. **/
+void insertNodeAfter(const node_iterator& pos, Node node);
+/** Delete the indicated node, which must be a child of this element,
+and must not be node_end(). The node will be removed from this element
+and deleted. The iterator is invalid after this call; be sure not to use it
+again. Also, there must not be any handles referencing the now-deleted
+node. **/
+void eraseNode(const node_iterator& deleteThis);
+/** Remove the indicated node from this element without erasing it, returning
+it as an orphan Node. The node must be a child of this element, and must not be
+node_end(). The node will be removed from this element and returned as an
+orphan. The iterator is invalid after this call; be sure not to use it
+again. **/
+Node removeNode(const node_iterator& removeThis);
+/*@}*/
+
+
+/**@name Value elements
+As described elsewhere, value elements are those that have no child elements
+and only a single Text node, whose contents can be considered as the element's
+value. Methods in this section allow you to work conveniently with value
+elements, getting direct access to the value string or interpreting it as
+some other type. You can easily modify the value by obtaining a writable
+refence to the String object that holds it. We provide methods for working
+with this element's value (if it is a value element) and with an element's
+children's values (if this element is compound). **/
+/*@{*/
+
+/** Determine whether this element qualifies as a "value element", defined
+as an element containing zero or one Text nodes and no child elements.
+You can treat a value element as you would an attribute -- it can be viewed
+as having a single value, which is just the value of its lone Text node
+(or a null string if it doesn't have any text). **/
+bool isValueElement() const;
+
+/** Get the text value of this value element. An error will be thrown if
+this is not a "value element". See the comments for this class for the
+definition of a "value element".
+ at note This does not return the same text as the base class method
+Node::getNodeText() does in the case of an element node; that returns the
+element tag word not its contents.
+ at see isValueElement(), setValue(), updValue() **/
+const String& getValue() const;
+
+/** Obtain a writable reference to the String containing the value of this
+value element. An error will be thrown if this is not a value element. If the
+element was initially empty and didn't contain a Text node, one will be added
+to it here with a null-string value so that we can return a reference to it.
+ at see isValueElement(), getValue() **/
+String& updValue();
+
+/** Set the text value of this value element. An error will be thrown if
+this is not a value element. If the element was initially empty and didn't
+contain a Text node, one will be added to it here so that we have a place to
+hold the \a value.
+ at see isValueElement(), setValueAs<T>() **/
+void setValue(const String& value);
+
+/** Set the value of this value element to the text equivalent of any type T
+for which a conversion construction String(T) is allowed (generally any
+type for which a stream insertion operator<<() exists). **/
+template <class T>
+void setValueAs(const T& value)
+{ setValue(String(value)); }
+
+/** Assuming this is a "value element", convert its text value to the type
+of the template argument T. It is an error if the text can not be converted,
+in its entirety, to a single object of type T. (But note that type T may
+be a container of some sort, like a Vector or Array.)
+ at tparam T A type that can be read from a stream using the ">>" operator.
+**/
+template <class T> T getValueAs() const
+{ T out; convertStringTo(getValue(),out); return out;}
+
+/** Alternate form of getValueAs() that avoids unnecessary copying and
+heap allocation for reading in large container objects. **/
+template <class T> void getValueAs(T& out) const
+{ convertStringTo(getValue(),out); }
+
+/** Get the text value of a child value element that \e must be present in
+this element. The child is identified by its tag; if there is more than one
+this refers to the first one. Then the element is expected to contain either
+zero or one Text nodes; if none we'll return a null string, otherwise
+the value of the Text node. Thus an element like "<tag>stuff</tag>" will
+have the value "stuff". An error will be thrown if either the element
+is not found or it is not a "value element". **/
+const String&
+getRequiredElementValue(const String& tag) const
+{ return unconst().getRequiredElement(tag).getValue(); }
+
+/** Get the text value of a child value element that \e may be present in
+this element, otherwise return a default string. If the child element is
+found, it must be a "value element" as defined above. **/
+String
+getOptionalElementValue(const String& tag, const String& def="") const
+{ const Element opt(unconst().getOptionalElement(tag));
+ return opt.isValid() ? opt.getValue() : def; }
+
+/** Convert the text value of a required child value element to the type
+of the template argument T. It is an error if the element is present but is
+not a value element, or if the text cannot be converted,
+in its entirety, to a single object of type T. (But note that type T may
+be a container of some sort, like a Vector or Array.)
+ at tparam T A type that can be read from a stream using the ">>" operator.
+ at param[in] tag The tag of the required child text element.
+ at return The value of the text element, converted to an object of type T. **/
+template <class T> T
+getRequiredElementValueAs(const String& tag) const
+{ T out; convertStringTo(unconst().getRequiredElementValue(tag), out);
+ return out; }
+
+/** Convert the text value of an optional child value element, if present, to
+the type of the template argument T. It is an error if the child element is
+present but is not a value element, or if the text cannot be
+converted, in its entirety, to a single object of type T. (But note that
+type T may be a container of some sort, like a Vector or Array.) If the
+child element is not present, then return a supplied default value of type T.
+ at tparam T A type that can be read from a stream with operator ">>".
+ at param[in] tag The tag of the optional child element.
+ at param[in] def The value of type T to return if child element is missing.
+ at return The value of element \a tag if it is present, otherwise a copy
+of the supplied default value \a def. **/
+template <class T> T
+getOptionalElementValueAs(const String& tag, const T& def) const
+{ const Element opt(unconst().getOptionalElement(tag));
+ if (!opt.isValid()) return def;
+ T out; convertStringTo(opt.getValue(), out); return out; }
+/*@}*/
+
+
+/**@name Attributes
+You can add, modify, and remove element attributes with the methods in this
+section. You can work directly with individual attributes by name, or you
+can iterate through the list of attributes. **/
+/*@{*/
+/** Return true if this element has an attribute of this name. **/
+bool hasAttribute(const String& name) const;
+
+/** Set the value of an attribute of this element, creating a new one if
+this is a new attribute name otherwise modifying an existing one. **/
+void setAttributeValue(const String& name, const String& value);
+
+/** Erase an attribute of this element if it exists, otherwise do nothing.
+If you need to know if the attribute exists, use hasAttribute(). There is
+no removeAttribute() that orphans an existing Attribute, but you can easily
+recreate one with the same name and value. **/
+void eraseAttribute(const String& name);
+
+/** Get the value of an attribute as a string and throw an error if that
+attribute is not present. **/
+const String&
+getRequiredAttributeValue(const String& name) const
+{ return unconst().getRequiredAttribute(name).getValue(); }
+
+/** Convert the text value of a required attribute to the type
+of the template argument T. It is an error if the text can not be converted,
+in its entirety, to a single object of type T. (But note that type T may
+be a container of some sort, like a Vec3.)
+ at tparam T A type that can be read from a stream using the ">>" operator.
+**/
+template <class T> T
+getRequiredAttributeValueAs(const String& name) const
+{ T out; convertStringTo(getRequiredAttributeValue(name),out); return out; }
+
+/** Get the value of an attribute as a string if the attribute is present in
+this element, otherwise return a supplied default value.
+ at param[in] name The name of the optional attribute.
+ at param[in] def The string to return if the attribute is missing.
+ at return The value of attribute \a name if it is present, otherwise a copy
+of the supplied default string \a def. **/
+String
+getOptionalAttributeValue(const String& name, const String& def="") const
+{ Attribute attr = unconst().getOptionalAttribute(name);
+ if (!attr.isValid()) return def;
+ return attr.getValue(); }
+
+/** Convert the value of an optional attribute, if present, from a string to
+the type of the template argument T. It is an error if the text can not be
+converted, in its entirety, to a single object of type T. (But note that type
+T may be a container of some sort, like a Vec3.) If the attribute is not
+present, then return a supplied default value of type T.
+ at tparam T A type that can be read from a stream with operator ">>".
+ at param[in] name The name of the optional attribute.
+ at param[in] def The value of type T to return if the attribute is missing.
+ at return The value of attribute \a name if it is present, otherwise a copy
+of the supplied default value \a def. **/
+template <class T> T
+getOptionalAttributeValueAs(const String& name, const T& def) const
+{ Attribute attr = unconst().getOptionalAttribute(name);
+ if (!attr.isValid()) return def;
+ T out; convertStringTo(attr.getValue(), out); return out; }
+
+/** Obtain an Attribute handle referencing a particular attribute of this
+element; an error will be thrown if no such attribute is present. **/
+Attribute getRequiredAttribute(const String& name);
+
+/** Obtain an Attribute handle referencing a particular attribute of this
+element specified by name, or an empty handle if no such attribute is
+present. **/
+Attribute getOptionalAttribute(const String& name);
+
+/** Return an array containing Attribute handles referencing all the
+attributes of this element. Attributes are returned in the order that
+they appear in the element tag. Attribute names within a tag are unique;
+if the source document had repeated attribute names only the last one
+to appear is retained and that's the only one we'll find here. This is
+just a shortcut for @code
+ Array_<Attribute>(attribute_begin(), attribute_end());
+ at endcode **/
+Array_<Attribute> getAllAttributes()
+{ return Array_<Attribute>(attribute_begin(), attribute_end()); }
+
+
+/** For iterating through all the attributes of this element. If there are no
+attributes then the returned attribute_iterator tests equal to
+attribute_end(). **/
+attribute_iterator attribute_begin();
+/** This attribute_end() iterator indicates the end of a sequence of
+attributes. **/
+attribute_iterator attribute_end() const;
+/*@}*/
+
+/**@name Compound elements
+Many elements contain child nodes, including other elements. When there is
+just a single child Text node and no child elements, we call the element a
+"value element" and it is easiest to work with using the methods in the
+"Value elements" section. When there are child elements and/or multiple Text
+nodes, the element is called a "compound element" and you need a way to
+iterate and recurse through its contents. The methods in this section support
+looking through all contained nodes, nodes of specified types, element nodes,
+or element nodes with a specified tags. You can obtain handles to child
+Nodes or Elements and then iterate through those recursively. **/
+/*@{*/
+
+/** Return true if this element has a child element with this tag. **/
+bool hasElement(const String& tag) const;
+/** See if this element has any child nodes, or any child nodes of the type(s)
+allowed by the NodeType filter if one is supplied. **/
+bool hasNode(NodeType allowed=AnyNodes) const;
+
+/** Get a reference to a child element that \e must be present in this
+element. The child is identified by its tag; if there is more than one
+only the first one is returned. If you want to see all children with this
+tag, use getAllElements() or use an element_iterator. **/
+Element getRequiredElement(const String& tag);
+
+/** Get a reference to a child element that \e may be present in this
+element; otherwise return an invalid Element handle. Test using the
+Element's isValid() method. **/
+Element getOptionalElement(const String& tag);
+
+/** Return an array containing Element handles referencing all the
+immediate child elements contained in this element, or all the child
+elements of a particular type (that is, with a given tag word). Elements
+are returned in the order they are seen in the document. This is just a
+shortcut for @code
+ Array_<Element>(element_begin(tag), element_end());
+ at endcode **/
+Array_<Element> getAllElements(const String& tag="")
+{ return Array_<Element>(element_begin(tag), element_end()); }
+
+/** Return an array containing Node handles referencing all the
+immediate child nodes contained in this element, or all the child
+nodes of a particular type or types. Nodes are returned in the order they
+are seen in the document. This is just a shortcut for @code
+ Array_<Node>(node_begin(allowed), node_end());
+ at endcode **/
+Array_<Node> getAllNodes(NodeType allowed=AnyNodes)
+{ return Array_<Node>(node_begin(allowed), node_end()); }
+
+/** For iterating through the immediate child elements of this element, or the
+child elements that have the indicated tag if one is supplied. If there are no
+children with the \a allowed tag then the returned element_iterator tests
+equal to element_end(). **/
+element_iterator element_begin(const String& tag="");
+/** This element_end() iterator indicates the end of any sequence of elements
+regardless of the tag restriction on the iterator being used. **/
+element_iterator element_end() const;
+
+/** For iterating through the immediate child nodes of this element, or the
+child nodes of the type(s) allowed by the NodeType filter if one is
+supplied. If there are no children of the \a allowed types then the returned
+node_iterator tests equal to node_end(). **/
+node_iterator node_begin(NodeType allowed=AnyNodes);
+/** This node_end() iterator indicates the end of any sequence of nodes
+regardless of the NodeType restriction on the iterator being used. **/
+node_iterator node_end() const;
+/*@}*/
+
+/**@name Conversion to Element from Node
+If you have a handle to a Node, such as would be returned by a node_iterator,
+you can check whether that Node is an Element and if so cast it to one. **/
+/*@{*/
+/** Test whether a given Node is an element node. **/
+static bool isA(const Node&);
+/** Recast a Node to a const Element, throwing an error if the Node is not
+actually an element node. @see isA() **/
+static const Element& getAs(const Node& node);
+/** Recast a writable Node to a writable Element, throwing an error if the
+Node is not actually an element node. @see isA() **/
+static Element& getAs(Node& node);
+/*@}*/
+
+//------------------------------------------------------------------------------
+ private:
+friend class Xml::Node;
+friend class Xml::element_iterator;
+
+explicit Element(TiXmlElement* tiElt)
+: Node(reinterpret_cast<TiXmlNode*>(tiElt)) {}
+
+TiXmlElement& updTiElement()
+{ return reinterpret_cast<TiXmlElement&>(updTiNode()); }
+const TiXmlElement& getTiElement() const
+{ return reinterpret_cast<const TiXmlElement&>(getTiNode()); }
+
+// Careful: these "Ptr" methods provide raw access to the contained
+// pointer without any cleanup or error checking. In particular,
+// setTiElementPtr() does not attempt to delete the current contents.
+const TiXmlElement* getTiElementPtr() const
+{ return reinterpret_cast<const TiXmlElement*>(getTiNodePtr()); }
+TiXmlElement* updTiElementPtr()
+{ return reinterpret_cast<TiXmlElement*>(updTiNodePtr()); }
+void setTiElementPtr(TiXmlElement* elt)
+{ setTiNodePtr(reinterpret_cast<TiXmlNode*>(elt)); }
+
+Element& unconst() const {return *const_cast<Element*>(this);}
+
+
+// no data members; see Node
+};
+
+
+
+// A few element_iterator inline definitions had to wait for Element to be
+// defined.
+inline Xml::element_iterator::element_iterator
+ (Xml::Element& elt, const String& tag)
+: Xml::node_iterator(elt, Xml::ElementNode), tag(tag) {}
+inline Xml::Element& Xml::element_iterator::operator*() const
+{ return Element::getAs(*upcast());}
+inline Xml::Element* Xml::element_iterator::operator->() const
+{ return &Element::getAs(*upcast());}
+
+
+
+
+//------------------------------------------------------------------------------
+// XML TEXT NODE
+//------------------------------------------------------------------------------
+/** This is the "leaf" content of an element. **/
+class SimTK_SimTKCOMMON_EXPORT Xml::Text : public Xml::Node {
+public:
+/** Create an empty Text node handle, suitable only for holding references
+to other Text nodes. **/
+Text() : Node() {}
+
+/** Create a new Text node with the given text; the node is not yet owned by
+any XML document. **/
+explicit Text(const String& text);
+
+/** The clone() method makes a deep copy of this Text node and returns a new
+orphan Text node with the same contents; ordinary assignment and copy
+construction are shallow. **/
+Text clone() const;
+
+/** Obtain a const reference to the String holding the value of this Text
+**/
+const String& getText() const;
+/** Obtain a writable reference to the String holding the value of this Text
+node; this can be used to alter the value. **/
+String& updText();
+
+/**@name Conversion to Text from Node
+If you have a handle to a Node, such as would be returned by a node_iterator,
+you can check whether that Node is a Text node and if so cast it to one. **/
+/*@{*/
+/** Test whether a given Node is an Text node. **/
+static bool isA(const Node&);
+/** Recast a Node to a const Text node, throwing an error if the Node is not
+actually a Text node. @see isA() **/
+static const Text& getAs(const Node& node);
+/** Recast a writable Node to a writable Text node, throwing an error if the
+Node is not actually a Text node. @see isA() **/
+static Text& getAs(Node& node);
+/*@}*/
+
+//------------------------------------------------------------------------------
+ private:
+// no data members; see Node
+
+explicit Text(TiXmlText* tiText)
+: Node(reinterpret_cast<TiXmlNode*>(tiText)) {}
+};
+
+
+
+//------------------------------------------------------------------------------
+// XML COMMENT NODE
+//------------------------------------------------------------------------------
+/** A comment contains only uninterpreted text. **/
+class SimTK_SimTKCOMMON_EXPORT Xml::Comment : public Xml::Node {
+public:
+/** Create an empty Comment node handle, suitable only for holding references
+to other Comment nodes. **/
+Comment() : Node() {}
+
+/** Create a new Comment node with the given text; the node is not yet owned by
+any XML document. Don't include the comment delimiters "<!--" and "-->" in the
+text; those will be added automatically if the document is serialized to a
+file or string. **/
+explicit Comment(const String& text);
+
+/** The clone() method makes a deep copy of this Comment node and returns a new
+orphan Comment node with the same contents; ordinary assignment and copy
+construction are shallow. **/
+Comment clone() const;
+
+/**@name Conversion to Comment from Node
+If you have a handle to a Node, such as would be returned by a node_iterator,
+you can check whether that Node is a Comment node and if so cast it to one. **/
+/*@{*/
+/** Test whether a given Node is an Comment node. **/
+static bool isA(const Node&);
+/** Recast a Node to a const Comment, throwing an error if the Node is not
+actually an Comment node. @see isA() **/
+static const Comment& getAs(const Node& node);
+/** Recast a writable Node to a writable Comment, throwing an error if the
+Node is not actually an Comment node. @see isA() **/
+static Comment& getAs(Node& node);
+/*@}*/
+
+//------------------------------------------------------------------------------
+ private:
+// no data members; see Node
+
+explicit Comment(TiXmlComment* tiComment)
+: Node(reinterpret_cast<TiXmlNode*>(tiComment)) {}
+};
+
+
+
+//------------------------------------------------------------------------------
+// XML UNKNOWN NODE
+//------------------------------------------------------------------------------
+/** This is something we don't understand but can carry around. **/
+class SimTK_SimTKCOMMON_EXPORT Xml::Unknown : public Xml::Node {
+public:
+/** Create an empty Unknown node handle, suitable only for holding references
+to other Unknown nodes. **/
+Unknown() : Node() {}
+
+/** Create a new Unknown node with the given contents; the node is not yet
+owned by any XML document. Don't include the tag delimiters "<" and ">" in the
+contents; those will be added automatically if the document is serialized to a
+file or string. That is, if you want "<!SOMETHING blah blah>", the contents
+you provide should be "!SOMETHING blah blah". **/
+explicit Unknown(const String& contents);
+
+/** Create a new Unknown node and append it to the list of nodes that are
+children of the given Element. The Element becomes the owner of the new
+Unknown node although the handle retains a reference to it. **/
+Unknown(Element& element, const String& contents)
+{ new(this) Unknown(contents);
+ element.insertNodeBefore(element.node_end(), *this); }
+
+/** The clone() method makes a deep copy of this Unknown node and returns a new
+orphan Unknown node with the same contents; ordinary assignment and copy
+construction are shallow. **/
+Unknown clone() const;
+
+/** Obtain the contents of this Unknown node. This is everything that would
+be between the "<" and ">" in the XML document. **/
+const String& getContents() const;
+/** Change the contents of this Unknown node. This is everything that would
+be between the "<" and ">" in the XML document. **/
+void setContents(const String& contents);
+
+/**@name Conversion to Unknown from Node
+If you have a handle to a Node, such as would be returned by a node_iterator,
+you can check whether that Node is an Unknown node and if so cast it to one. **/
+/*@{*/
+/** Test whether a given Node is an Unknown node. **/
+static bool isA(const Node&);
+/** Recast a Node to a const Unknown, throwing an error if the Node is not
+actually an Unknown node. @see isA() **/
+static const Unknown& getAs(const Node& node);
+/** Recast a writable Node to a writable Unknown, throwing an error if the
+Node is not actually an Unknown node. @see isA() **/
+static Unknown& getAs(Node& node);
+/*@}*/
+
+//------------------------------------------------------------------------------
+ private:
+// no data members; see Node
+
+explicit Unknown(TiXmlUnknown* tiUnknown)
+: Node(reinterpret_cast<TiXmlNode*>(tiUnknown)) {}
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_XML_H_
+
+
diff --git a/SimTKcommon/include/SimTKcommon/internal/common.h b/SimTKcommon/include/SimTKcommon/internal/common.h
new file mode 100644
index 0000000..91e97cb
--- /dev/null
+++ b/SimTKcommon/include/SimTKcommon/internal/common.h
@@ -0,0 +1,770 @@
+#ifndef SimTK_SimTKCOMMON_COMMON_H_
+#define SimTK_SimTKCOMMON_COMMON_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+Mandatory first inclusion for any Simbody source or header file.
+
+Every source and most header files using %SimTK must include this
+header as its \e first inclusion. Declarations and definitions that
+must be available and compiler-and machine-specific issues are dealt
+with here.
+
+This file must be includable from either C++ or ANSI C. It uses
+the ANSI-C++ macro "__cplusplus" for any code that will compile
+only under C++. **/
+
+// Provide doxygen documentation for the SimTK namespace.
+
+/**@namespace SimTK
+This is the top-level %SimTK namespace into which all %SimTK names are
+placed to avoid collision with other symbols. If you get tired of prefacing
+every symbol with "SimTK::", include the statement "using namespace SimTK;"
+at the beginning of your %SimTK-using compilation units. Any names which
+cannot be put in the namespace (macro names, for example) begin with the
+prefix "SimTK_" instead. **/
+
+// Define shared doxygen "modules" and sub-modules here. We'll put things
+// in them at various places when appropriate.
+
+/**@defgroup GlobalFunctions Global Functions in the SimTK namespace
+These are functions at the top level of the SimTK namespace, meaning
+that a function named funcName() is invoked as SimTK::funcName(), or
+just funcName() if there is a "using namespace SimTK;" statement in effect. **/
+
+/**@defgroup ScalarFunctions Scalar Functions
+ @ingroup GlobalFunctions
+These functions are overloaded to act on %SimTK scalar types and C++
+built-in types, including integral types when appropriate. **/
+
+/**@defgroup BitFunctions Bit-twiddling Functions
+ @ingroup GlobalFunctions
+These functions perform highly optimized bit-twiddling operations on
+the built-in integral types, and sometimes on the representations of
+floating point types as well. **/
+
+/**@defgroup Serialization Utilities for De/serializing
+ @ingroup GlobalFunctions
+These namespace-scope templatized utilities provide uniform serialization
+and deserialization behavior for built-in and SimTK-defined types. See
+SimTK::Xml for support of serialization to/from Xml files. **/
+
+/**@defgroup UniqueIndexTypes Type-Safe Integer Indices
+
+It is common to store objects or information about them in randomly-indexable
+arrays, and then to support maximum-performance selection by allowing the
+index to be used. We want these arrays indexable by simple ints for speed, but
+this quickly leads to APIs in which there are multiple int arguments in a
+function call, each intended to select a different kind of object. A common
+error when there is a series of identical argument types is to put them in
+the wrong order. To avoid that, we define unique index types here for
+accessing each category to help stay out of trouble.
+
+A unique index type is just a type-safe non-negative int, augmented with a
+"NaN" value called InvalidBLAH where BLAH is the type name. For most uses it
+will behave like an int, and it has an implicit conversion *to* int. Importantly
+though, it has no implicit conversion *from* int so you can't pass a plain int
+or any other Index type to an argument expecting a certain Index type. **/
+
+/*****************************/
+/* ANSI-C COMPATIBLE SECTION */
+/*****************************/
+
+/* Set up a few compile-time options that affect all SimTK Core headers. */
+
+/**
+ * This compile-time constant determines the default precision used everywhere
+ * in %SimTK Core code. Wherever a SimTK::Real, SimTK::Vector, SimTK::Matrix,
+ * etc. appears with no precision specified, it will have this underlying precision.
+ * We use 1==float, 2==double, 4==long double. Any other value will cause
+ * a compile time error. The default is 2, i.e., double precision.
+ */
+#ifndef SimTK_DEFAULT_PRECISION
+# define SimTK_DEFAULT_PRECISION 2
+#endif
+
+#if (SimTK_DEFAULT_PRECISION == 1)
+/** This type is for use in C; in C++ use SimTK::Real instead. */
+ typedef float SimTK_Real;
+#elif (SimTK_DEFAULT_PRECISION == 2)
+/** This type is for use in C; in C++ use SimTK::Real instead. */
+ typedef double SimTK_Real;
+#elif (SimTK_DEFAULT_PRECISION == 4)
+/** This type is for use in C; in C++ use SimTK::Real instead. */
+ typedef long double SimTK_Real;
+#else
+ #error ILLEGAL VALUE FOR DEFAULT PRECISION
+#endif
+
+#ifndef NDEBUG
+ #if defined(__cplusplus)
+ #include <cstdio>
+ #define SimTK_DEBUG(s) std::printf("DBG: " s)
+ #define SimTK_DEBUG1(s,a1) std::printf("DBG: " s,a1)
+ #define SimTK_DEBUG2(s,a1,a2) std::printf("DBG: " s,a1,a2)
+ #define SimTK_DEBUG3(s,a1,a2,a3) std::printf("DBG: " s,a1,a2,a3)
+ #define SimTK_DEBUG4(s,a1,a2,a3,a4) std::printf("DBG: " s,a1,a2,a3,a4)
+ #else
+ #include <stdio.h>
+ #define SimTK_DEBUG(s) printf("DBG: " s)
+ #define SimTK_DEBUG1(s,a1) printf("DBG: " s,a1)
+ #define SimTK_DEBUG2(s,a1,a2) printf("DBG: " s,a1,a2)
+ #define SimTK_DEBUG3(s,a1,a2,a3) printf("DBG: " s,a1,a2,a3)
+ #define SimTK_DEBUG4(s,a1,a2,a3,a4) printf("DBG: " s,a1,a2,a3,a4)
+ #endif
+#else
+ #define SimTK_DEBUG(s)
+ #define SimTK_DEBUG1(s,a1)
+ #define SimTK_DEBUG2(s,a1,a2)
+ #define SimTK_DEBUG3(s,a1,a2,a3)
+ #define SimTK_DEBUG4(s,a1,a2,a3,a4)
+#endif
+
+/*
+ * Shared libraries are messy in Visual Studio. We have to distinguish three
+ * cases:
+ * (1) this header is being used to build the SimTKcommon shared library (dllexport)
+ * (2) this header is being used by a *client* of the SimTKcommon shared
+ * library (dllimport)
+ * (3) we are building the SimTKcommon static library, or the client is
+ * being compiled with the expectation of linking with the
+ * SimTKcommon static library (nothing special needed)
+ * In the CMake script for building this library, we define one of the symbols
+ * SimTK_SimTKCOMMON_BUILDING_{SHARED|STATIC}_LIBRARY
+ * Client code normally has no special symbol defined, in which case we'll
+ * assume it wants to use the shared library. However, if the client defines
+ * the symbol SimTK_USE_STATIC_LIBRARIES we'll suppress the dllimport so
+ * that the client code can be linked with static libraries. Note that
+ * the client symbol is not library dependent, while the library symbols
+ * affect only the SimTKcommon library, meaning that other libraries can
+ * be clients of this one. However, we are assuming all-static or all-shared.
+ */
+
+#ifdef _WIN32
+ #ifdef _MSC_VER
+ #pragma warning(disable:4231) /*need to use 'extern' template explicit instantiation*/
+ #pragma warning(disable:4251) /*no DLL interface for type of member of exported class*/
+ #pragma warning(disable:4275) /*no DLL interface for base class of exported class*/
+ #pragma warning(disable:4345) /*warning about PODs being default-initialized*/
+ #endif
+ #if defined(SimTK_SimTKCOMMON_BUILDING_SHARED_LIBRARY)
+ #define SimTK_SimTKCOMMON_EXPORT __declspec(dllexport)
+ /* Keep MS VC++ quiet when it tries to instantiate incomplete template classes in a DLL. */
+ #ifdef _MSC_VER
+ #pragma warning(disable:4661)
+ #endif
+ #elif defined(SimTK_SimTKCOMMON_BUILDING_STATIC_LIBRARY) || defined(SimTK_USE_STATIC_LIBRARIES)
+ #define SimTK_SimTKCOMMON_EXPORT
+ #else
+ #define SimTK_SimTKCOMMON_EXPORT __declspec(dllimport) /*i.e., a client of a shared library*/
+ #endif
+ /* VC++ tries to be secure by leaving bounds checking on for STL containers
+ * even in Release mode. This macro exists to disable that feature and can
+ * result in a considerable speedup.
+ * CAUTION: every linked-together compilation unit must have this set the same
+ * way. Everyone who properly includes this file first is fine; but as of this
+ * writing Simmath's IpOpt doesn't do so.
+ * NOTE: Microsoft corrected this problem with VC10 -- the feature is
+ * disabled by default in that compiler and later.
+ */
+ /* (sherm 081204 disabling for now: doesn't work on VC++ 8 and is
+ * tricky on VC++ 9 because all libraries, including 3rd party, must
+ * be built the same way). Better to use the SimTK::Array_<T> class in
+ * place of the std::vector<T> class to get better performance.
+ #ifdef NDEBUG
+ #undef _SECURE_SCL
+ #define _SECURE_SCL 0
+ #endif
+ */
+#else
+ #define SimTK_SimTKCOMMON_EXPORT // Linux, Mac
+#endif
+
+/* Every SimTK Core library must provide these two routines, with the library
+ * name appearing after the "version_" and "about_".
+ */
+#if defined(__cplusplus)
+extern "C" {
+#endif
+ /** Obtain version information for the currently-loaded SimTKcommon library. */
+ SimTK_SimTKCOMMON_EXPORT void SimTK_version_SimTKcommon(int* major, int* minor, int* build);
+ /**
+ * Obtain "about" information for the currently-loaded SimTKcommon library.
+ * Available keywords are "version" (major.minor.build), "library",
+ * "type" (shared or static), "copyright", "svn_revision", "authors",
+ * "debug" (debug or release).
+ */
+ SimTK_SimTKCOMMON_EXPORT void SimTK_about_SimTKcommon(const char* key, int maxlen, char* value);
+#if defined(__cplusplus)
+}
+#endif
+
+/************************************/
+/* END OF ANSI-C COMPATIBLE SECTION */
+/************************************/
+
+#if defined(__cplusplus)
+
+#include <cstddef>
+#include <cassert>
+#include <cmath>
+#include <cfloat>
+#include <complex>
+#include <limits>
+#include <typeinfo>
+#include <algorithm>
+
+/* Transition macros for C++11 support. VC10 and VC11 have partial support for
+C++11, early VC's do not. Currently we're assuming no support from gcc. */
+#ifndef SWIG
+ #if _MSC_VER>=1700 /* VC11 or higher */
+ #define OVERRIDE_11 override
+ #define FINAL_11 final
+ #elif _MSC_VER==1600 /* VC10 */
+ #define OVERRIDE_11 override
+ #define FINAL_11 sealed
+ #else /* gcc or earlier VC */
+ #define OVERRIDE_11
+ #define FINAL_11
+ #endif
+#else /* Swigging */
+ #define OVERRIDE_11
+ #define FINAL_11
+#endif
+
+
+/* In Microsoft VC++ 11 (2012) and earlier these C99-compatible floating
+point functions are missing. We'll create them here and install them into
+namespace std. They were added in VC++ 12 (2013). */
+#if defined(_MSC_VER) && (_MSC_VER <= 1700) // VC++ 12 (2013, _MSC_VER=1800) added these
+namespace std {
+inline bool isfinite(float f) {return _finite(f) != 0;}
+inline bool isfinite(double d) {return _finite(d) != 0;}
+inline bool isfinite(long double l) {return _finite(l) != 0;}
+inline bool isnan(float f) {return _isnan(f) != 0;}
+inline bool isnan(double d) {return _isnan(d) != 0;}
+inline bool isnan(long double l) {return _isnan(l) != 0;}
+inline bool isinf(float f) {return std::abs(f)==std::numeric_limits<float>::infinity();}
+inline bool isinf(double d) {return std::abs(d)==std::numeric_limits<double>::infinity();}
+inline bool isinf(long double l) {return std::abs(l)==std::numeric_limits<double>::infinity();}
+inline bool signbit(float f) {return (*reinterpret_cast<unsigned*>(&f) & 0x80000000U) != 0;}
+inline bool signbit(double d) {return (*reinterpret_cast<unsigned long long*>(&d)
+ & 0x8000000000000000ULL) != 0;}
+inline bool signbit(long double l) {return (*reinterpret_cast<unsigned long long*>(&l)
+ & 0x8000000000000000ULL) != 0;}
+}
+#endif
+
+
+namespace SimTK {
+
+
+// This utility answers the question "if I put this integral value in an int and then
+// get it back, will its value be the same?".
+inline bool canStoreInInt(bool) {return true;}
+inline bool canStoreInInt(char) {return true;}
+inline bool canStoreInInt(unsigned char) {return true;}
+inline bool canStoreInInt(signed char) {return true;}
+inline bool canStoreInInt(short) {return true;}
+inline bool canStoreInInt(unsigned short) {return true;}
+inline bool canStoreInInt(int) {return true;}
+inline bool canStoreInInt(unsigned int u) {return (unsigned int)(int(u)) == u;}
+inline bool canStoreInInt(long i) {return long(int(i)) == i;}
+inline bool canStoreInInt(unsigned long u) {return (unsigned long)(int(u)) == u;}
+inline bool canStoreInInt(long long i) {return (long long)(int(i)) == i;}
+inline bool canStoreInInt(unsigned long long u) {return (unsigned long long)(int(u)) == u;}
+
+// This utility answers the question "is this integral value a nonnegative number
+// that can be stored in an int?".
+inline bool canStoreInNonnegativeInt(bool) {return true;}
+inline bool canStoreInNonnegativeInt(char c) {return c >= 0;}
+inline bool canStoreInNonnegativeInt(unsigned char) {return true;}
+inline bool canStoreInNonnegativeInt(signed char c) {return c >= 0;}
+inline bool canStoreInNonnegativeInt(short s) {return s >= 0;}
+inline bool canStoreInNonnegativeInt(unsigned short) {return true;}
+inline bool canStoreInNonnegativeInt(int i) {return i >= 0;}
+inline bool canStoreInNonnegativeInt(long l) {return canStoreInInt(l) && l >= 0;}
+inline bool canStoreInNonnegativeInt(long long l) {return canStoreInInt(l) && l >= 0;}
+inline bool canStoreInNonnegativeInt(unsigned int u) {return canStoreInInt(u);}
+inline bool canStoreInNonnegativeInt(unsigned long u) {return canStoreInInt(u);}
+inline bool canStoreInNonnegativeInt(unsigned long long u) {return canStoreInInt(u);}
+
+// This utility answers the question of whether an integer is suitable as a size
+// limited by the given maximum size. Signed types must be checked for being
+// nonegative; doing that with unsigned types leads to compiler warnings.
+
+// char can be signed or unsigned depending on the compiler; assume signed.
+inline bool isSizeInRange(char sz, char mx){return 0<=sz&&sz<=mx;}
+inline bool isSizeInRange(signed char sz, signed char mx){return 0<=sz&&sz<=mx;}
+inline bool isSizeInRange(short sz, short mx){return 0<=sz&&sz<=mx;}
+inline bool isSizeInRange(int sz, int mx){return 0<=sz&&sz<=mx;}
+inline bool isSizeInRange(long sz, long mx){return 0<=sz&&sz<=mx;}
+inline bool isSizeInRange(long long sz, long long mx){return 0<=sz&&sz<=mx;}
+inline bool isSizeInRange(unsigned char sz, unsigned char mx){return sz<=mx;}
+inline bool isSizeInRange(unsigned short sz, unsigned short mx){return sz<=mx;}
+inline bool isSizeInRange(unsigned int sz, unsigned int mx){return sz<=mx;}
+inline bool isSizeInRange(unsigned long sz, unsigned long mx){return sz<=mx;}
+inline bool isSizeInRange(unsigned long long sz, unsigned long long mx){return sz<=mx;}
+
+// This utility answers the question of whether an integer is suitable as an index
+// for an array limited by the given maximum size. Signed types must be checked for being
+// nonegative; doing that with unsigned types leads to compiler warnings. This is just
+// like the "size in range" check above except the maximum value allowed for an index
+// is one less that the size.
+
+// char can be signed or unsigned depending on the compiler; assume signed.
+inline bool isIndexInRange(char ix, char sz){return 0<=ix&&ix<sz;}
+inline bool isIndexInRange(signed char ix, signed char sz){return 0<=ix&&ix<sz;}
+inline bool isIndexInRange(short ix, short sz){return 0<=ix&&ix<sz;}
+inline bool isIndexInRange(int ix, int sz){return 0<=ix&&ix<sz;}
+inline bool isIndexInRange(long ix, long sz){return 0<=ix&&ix<sz;}
+inline bool isIndexInRange(long long ix, long long sz){return 0<=ix&&ix<sz;}
+inline bool isIndexInRange(unsigned char ix, unsigned char sz){return ix<sz;}
+inline bool isIndexInRange(unsigned short ix, unsigned short sz){return ix<sz;}
+inline bool isIndexInRange(unsigned int ix, unsigned int sz){return ix<sz;}
+inline bool isIndexInRange(unsigned long ix, unsigned long sz){return ix<sz;}
+inline bool isIndexInRange(unsigned long long ix, unsigned long long sz){return ix<sz;}
+
+// This utility answers the question: is this integral value nonnegative? The answer
+// is always true for unsigned types and you'll get a warning from some compilers if
+// you check.
+
+inline bool isNonnegative(bool) {return true;}
+// char can be signed or unsigned depending on the compiler; assume signed.
+inline bool isNonnegative(char n) {return n>=0;}
+inline bool isNonnegative(signed char n) {return n>=0;}
+inline bool isNonnegative(short n) {return n>=0;}
+inline bool isNonnegative(int n) {return n>=0;}
+inline bool isNonnegative(long n) {return n>=0;}
+inline bool isNonnegative(long long n) {return n>=0;}
+inline bool isNonnegative(unsigned char) {return true;}
+inline bool isNonnegative(unsigned short) {return true;}
+inline bool isNonnegative(unsigned int) {return true;}
+inline bool isNonnegative(unsigned long) {return true;}
+inline bool isNonnegative(unsigned long long){return true;}
+
+// A NaN-like value for unique index types created using the macro
+// SimTK_DEFINE_UNIQUE_INDEX_TYPE(). A unique, typed constant with
+// this numerical value is created for each index type.
+static const int InvalidIndex = -1111111111;
+}
+
+
+
+/**
+ * Use this macro to define a unique "Index" type which is just a type-safe
+ * non-negative int, augmented with a "NaN" value given by the predefined
+ * int constant SimTK::InvalidIndex. We also allow the Index to take on
+ * the value -1 if that is produced by a subtraction operation acting on a
+ * previously-valid Index, since that can occur during loops which are
+ * processed from the end towards the beginning. -1 is then allowed in
+ * comparision operators but not in any other operations, including further
+ * decrementing.
+ *
+ * No namespace is assumed for the newly-defined type; if you want the
+ * symbol in a namespace be sure to invoke the macro within that namespace.
+ * Make sure that the statement "#include <cassert>" appears somewhere before
+ * the point of invocation of this macro, because the defined Index type uses
+ * the assert() macro when in Debug mode.
+ *
+ * For most uses it will behave like an int, and it has an implicit
+ * conversion \e to int. Importantly though, it has no implicit conversion
+ * \e from int so you can't pass some other kind of number where a particular
+ * kind of Index was expected. This is used to create Index types
+ * which can be used as array indices but which prevent accidental mixing
+ * of types. Examples: SubsystemIndex, ConstraintIndex.
+ *
+ * If you create a type "ThingIndex" you will also get a constant of
+ * type ThingIndex named "InvalidThingIndex" which will be the initial
+ * value of any objects of type ThingIndex, and will have the same numerical
+ * value as SimTK::InvalidIndex.
+ */
+
+/** Define a global (that is, SimTK namespace level) Index class that is not
+exported in MS VC++ DLLs. **/
+#define SimTK_DEFINE_UNIQUE_INDEX_TYPE(NAME) \
+ SimTK_DEFINE_AND_EXPORT_UNIQUE_LOCAL_INDEX_TYPE(,,,NAME) \
+ static const NAME Invalid ## NAME;
+
+/** Define a global (that is, SimTK namespace level) Index class with a MS VC++
+"export" specification for DLLs. **/
+#define SimTK_DEFINE_AND_EXPORT_UNIQUE_INDEX_TYPE(EXPORT,NAME) \
+ SimTK_DEFINE_AND_EXPORT_UNIQUE_LOCAL_INDEX_TYPE(EXPORT,,,NAME) \
+ static const NAME Invalid ## NAME;
+
+/** Define a local Index class within a Parent class. **/
+#define SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(PARENT,NAME) \
+ SimTK_DEFINE_AND_EXPORT_UNIQUE_LOCAL_INDEX_TYPE(,PARENT,::,NAME)
+
+/** The most general form allows a MS VC++ "export" specification for DLLs,
+and a Parent class (with SEP=::) for local Index names. **/
+#define SimTK_DEFINE_AND_EXPORT_UNIQUE_LOCAL_INDEX_TYPE(EXPORT,PARENT,SEP,NAME) \
+class EXPORT NAME { \
+ int ix; \
+public: \
+ NAME() : ix(SimTK::InvalidIndex) { } \
+ explicit NAME(int i) : ix(i) {assert(i>=0 || i==SimTK::InvalidIndex);} \
+ explicit NAME(long l): ix((int)l) {assert(SimTK::canStoreInNonnegativeInt(l));} \
+ explicit NAME(unsigned int u) : ix((int)u) {assert(SimTK::canStoreInInt(u));} \
+ explicit NAME(unsigned long ul) : ix((int)ul) {assert(SimTK::canStoreInInt(ul));} \
+ operator int() const {return ix;} \
+ bool isValid() const {return ix>=0;} \
+ bool isValidExtended() const {return ix>=-1;} \
+ void invalidate(){ix=SimTK::InvalidIndex;} \
+ \
+ bool operator==(int i) const {assert(isValidExtended() && isValidExtended(i)); return ix==i;} \
+ bool operator==(short s) const{assert(isValidExtended() && isValidExtended(s)); return ix==(int)s;} \
+ bool operator==(long l) const {assert(isValidExtended() && isValidExtended(l)); return ix==(int)l;} \
+ bool operator==(unsigned int u) const {assert(isValidExtended() && isValid(u)); return ix==(int)u;} \
+ bool operator==(unsigned short us)const {assert(isValidExtended() && isValid(us)); return ix==(int)us;} \
+ bool operator==(unsigned long ul) const {assert(isValidExtended() && isValid(ul)); return ix==(int)ul;} \
+ bool operator!=(int i) const {return !operator==(i);} \
+ bool operator!=(short s) const {return !operator==(s);} \
+ bool operator!=(long l) const {return !operator==(l);} \
+ bool operator!=(unsigned int u) const {return !operator==(u);} \
+ bool operator!=(unsigned long ul) const {return !operator==(ul);} \
+ \
+ bool operator< (int i) const {assert(isValidExtended() && isValidExtended(i)); return ix<i;} \
+ bool operator< (short s) const{assert(isValidExtended() && isValidExtended(s)); return ix<(int)s;} \
+ bool operator< (long l) const {assert(isValidExtended() && isValidExtended(l)); return ix<(int)l;} \
+ bool operator< (unsigned int u) const {assert(isValidExtended() && isValid(u)); return ix<(int)u;} \
+ bool operator< (unsigned short us)const {assert(isValidExtended() && isValid(us)); return ix<(int)us;} \
+ bool operator< (unsigned long ul) const {assert(isValidExtended() && isValid(ul)); return ix<(int)ul;} \
+ bool operator>=(int i) const {return !operator<(i);} \
+ bool operator>=(short s) const {return !operator<(s);} \
+ bool operator>=(long l) const {return !operator<(l);} \
+ bool operator>=(unsigned int u) const {return !operator<(u);} \
+ bool operator>=(unsigned short us)const {return !operator<(us);} \
+ bool operator>=(unsigned long ul) const {return !operator<(ul);} \
+ \
+ bool operator> (int i) const {assert(isValidExtended() && isValidExtended(i)); return ix>i;} \
+ bool operator> (short s) const{assert(isValidExtended() && isValidExtended(s)); return ix>(int)s;} \
+ bool operator> (long l) const {assert(isValidExtended() && isValidExtended(l)); return ix>(int)l;} \
+ bool operator> (unsigned int u) const {assert(isValidExtended() && isValid(u)); return ix>(int)u;} \
+ bool operator> (unsigned short us)const {assert(isValidExtended() && isValid(us)); return ix>(int)us;} \
+ bool operator> (unsigned long ul) const {assert(isValidExtended() && isValid(ul)); return ix>(int)ul;} \
+ bool operator<=(int i) const {return !operator>(i);} \
+ bool operator<=(short s) const {return !operator>(s);} \
+ bool operator<=(long l) const {return !operator>(l);} \
+ bool operator<=(unsigned int u) const {return !operator>(u);} \
+ bool operator<=(unsigned short us)const {return !operator>(us);} \
+ bool operator<=(unsigned long ul) const {return !operator>(ul);} \
+ \
+ const NAME& operator++() {assert(isValid()); ++ix; return *this;} /*prefix */ \
+ NAME operator++(int) {assert(isValid()); ++ix; return NAME(ix-1);} /*postfix*/ \
+ const NAME& operator--() {assert(isValid()); --ix; return *this;} /*prefix */ \
+ NAME operator--(int) {assert(isValid()); --ix; return NAME(ix+1);} /*postfix*/ \
+ NAME next() const {assert(isValid()); return NAME(ix+1);} \
+ NAME prev() const {assert(isValid()); return NAME(ix-1);} /*might return -1*/ \
+ \
+ NAME& operator+=(int i) {assert(isValid() && isValidExtended(ix+i)); ix+=i; return *this;} \
+ NAME& operator-=(int i) {assert(isValid() && isValidExtended(ix-i)); ix-=i; return *this;} \
+ NAME& operator+=(short s){assert(isValid() && SimTK::canStoreInInt(s) && isValidExtended(ix+(int)s)); ix+=(int)s; return *this;} \
+ NAME& operator-=(short s){assert(isValid() && SimTK::canStoreInInt(s) && isValidExtended(ix-(int)s)); ix-=(int)s; return *this;} \
+ NAME& operator+=(long l) {assert(isValid() && SimTK::canStoreInInt(l) && isValidExtended(ix+(int)l)); ix+=(int)l; return *this;} \
+ NAME& operator-=(long l) {assert(isValid() && SimTK::canStoreInInt(l) && isValidExtended(ix-(int)l)); ix-=(int)l; return *this;} \
+ NAME& operator+=(unsigned int u) {assert(isValid()&& SimTK::canStoreInInt(u) && isValid(ix+(int)u)); ix+=(int)u; return *this;} \
+ NAME& operator-=(unsigned int u) {assert(isValid()&& SimTK::canStoreInInt(u) && isValidExtended(ix-(int)u)); ix-=(int)u; return *this;} \
+ NAME& operator+=(unsigned short us){assert(isValid()&& SimTK::canStoreInInt(us) && isValid(ix+(int)us)); ix+=(int)us; return *this;} \
+ NAME& operator-=(unsigned short us){assert(isValid()&& SimTK::canStoreInInt(us) && isValidExtended(ix-(int)us)); ix-=(int)us; return *this;} \
+ NAME& operator+=(unsigned long ul) {assert(isValid()&& SimTK::canStoreInInt(ul) && isValid(ix+(int)ul)); ix+=(int)ul; return *this;} \
+ NAME& operator-=(unsigned long ul) {assert(isValid()&& SimTK::canStoreInInt(ul) && isValidExtended(ix-(int)ul)); ix-=(int)ul; return *this;} \
+ \
+ static const NAME& Invalid() {static const NAME invalid; return invalid;} \
+ static bool isValid(int i) {return i>=0;} \
+ static bool isValid(short s){return s>=0;} \
+ static bool isValid(long l) {return SimTK::canStoreInNonnegativeInt(l);} \
+ static bool isValid(unsigned int u) {return SimTK::canStoreInInt(u);} \
+ static bool isValid(unsigned short) {return true;} \
+ static bool isValid(unsigned long ul) {return SimTK::canStoreInInt(ul);} \
+ static bool isValidExtended(int i) {return i>=-1;} \
+ static bool isValidExtended(short s){return s>=-1;} \
+ static bool isValidExtended(long l) {return SimTK::canStoreInInt(l) && l>=-1;} \
+ /* IndexTraits for use in Array_<T,X> with this as X; same as int */ \
+ typedef int size_type; \
+ typedef int difference_type; \
+ static size_type max_size() {return std::numeric_limits<int>::max();} \
+};
+
+/** Use this macro to generate a cast that is dynamic_cast in Debug builds
+but static_cast in Release builds, for uses where you don't want to
+pay for the extra safety. Caution: these are not necessarily equivalent for
+dynamic types that use multiple inheritance; don't use this macro in that
+case, and don't use it where you are using dynamic_cast on a pointer to
+check what type of derived object you're looking at. **/
+#ifndef NDEBUG
+ #define SimTK_DYNAMIC_CAST_DEBUG dynamic_cast // safe but slow
+#else
+ #define SimTK_DYNAMIC_CAST_DEBUG static_cast // unsafe but fast
+#endif
+
+/** Add public static method declaration in class derived from an abstract
+parent to assist in downcasting objects of the parent type to the derived
+type. **/
+#define SimTK_DOWNCAST(Derived,Parent) \
+ static bool isA(const Parent& p) \
+ { return dynamic_cast<const Derived*>(&p) != 0; } \
+ static const Derived& downcast(const Parent& p) \
+ { return SimTK_DYNAMIC_CAST_DEBUG<const Derived&>(p); } \
+ static Derived& updDowncast(Parent& p) \
+ { return SimTK_DYNAMIC_CAST_DEBUG<Derived&>(p); } \
+ static Derived& downcast(Parent& p) \
+ { return SimTK_DYNAMIC_CAST_DEBUG<Derived&>(p); }
+
+/** This is like SimTK_DOWNCAST except it allows for an intermediate "helper"
+class between Derived and Parent. **/
+#define SimTK_DOWNCAST2(Derived,Helper,Parent) \
+ static bool isA(const Parent& p) \
+ { return Helper::isA(p); } \
+ static const Derived& downcast(const Parent& p) \
+ { return static_cast<const Derived&>(Helper::downcast(p)); } \
+ static Derived& updDowncast(Parent& p) \
+ { return static_cast<Derived&>(Helper::downcast(p)); } \
+ static Derived& downcast(Parent& p) \
+ { return static_cast<Derived&>(Helper::downcast(p)); }
+
+
+/** Similar to the above but for private implementation abstract classes, that
+is, abstract class hierarchies where the virtual function table is hidden on
+the library side. **/
+#define SimTK_PIMPL_DOWNCAST(Derived, Parent) \
+ static bool isInstanceOf(const Parent&); \
+ static const Derived& downcast(const Parent&); \
+ static Derived& updDowncast(Parent&)
+
+namespace SimTK {
+
+/** This sub-namespace of SimTK is used for the exception types that are
+thrown by our error handing code. **/
+namespace Exception { }
+
+/** This is the default compiled-in floating point type for SimTK, either
+float or double. @see SimTK_DEFAULT_PRECISION **/
+typedef SimTK_Real Real;
+/** This is the default complex type for SimTK, with precision for the real
+and imaginary parts set to the compiled-in Real type. @see Real **/
+typedef std::complex<Real> Complex;
+/** An abbreviation for std::complex<float> for consistency with others. **/
+typedef std::complex<float> fComplex;
+/** An abbreviation for std::complex<double> for consistency with others. **/
+typedef std::complex<float> dComplex;
+
+
+// Forward declaration giving template defaults must come before any
+// other declarations.
+template <int M, class ELT=Real, int STRIDE=1> class Vec;
+template <int N, class ELT=Real, int STRIDE=1> class Row;
+template <int M, int N, class ELT=Real, int CS=M, int RS=1> class Mat;
+template <int M, class ELT=Real, int RS=1> class SymMat;
+
+/** A convenient struct for anything requiring an offset and length to specify
+a segment of some larger sequence. **/
+struct Segment {
+ Segment() : length(0), offset(0) { }
+ explicit Segment(int l, int ofs=0) : length(l), offset(ofs) {
+ assert(l>=0 && ofs>=0);
+ }
+ // default copy, assignment, destructor
+ int length;
+ int offset;
+};
+
+
+/** This is a special type used for causing invocation of a particular
+constructor or method overload that will avoid making a copy of the source
+(that is, perform a "shallow" copy rather than a "deep" copy). Typically these
+methods will have some dangerous side effects so make sure you know what you're
+doing. **/
+struct DontCopy {};
+/** This is a special type used for forcing invocation of a particularly
+dangerous constructor or method overload; don't use this unless you are an
+advanced user and know exactly what you're getting into. **/
+struct TrustMe {};
+
+/** This is a compile-time equivalent of "false", used in compile-time
+condition checking in templatized implementations. **/
+struct FalseType {};
+/** This is a compile-time equivalent of "true", used in compile-time
+condition checking in templatized implementations. **/
+struct TrueType {};
+
+/** This is an operator for and-ing compile-time truth types. */
+template <class L, class R> struct AndOpType {};
+template<> struct AndOpType<FalseType,FalseType> {typedef FalseType Result;};
+template<> struct AndOpType<FalseType,TrueType> {typedef FalseType Result;};
+template<> struct AndOpType<TrueType, FalseType> {typedef FalseType Result;};
+template<> struct AndOpType<TrueType, TrueType> {typedef TrueType Result;};
+
+/** This is an operator for or-ing compile-time truth types. */
+template <class L, class R> struct OrOpType {};
+template<> struct OrOpType<FalseType,FalseType> {typedef FalseType Result;};
+template<> struct OrOpType<FalseType,TrueType> {typedef TrueType Result;};
+template<> struct OrOpType<TrueType, FalseType> {typedef TrueType Result;};
+template<> struct OrOpType<TrueType, TrueType> {typedef TrueType Result;};
+
+/** This is an operator for exclusive or-ing compile-time truth types. */
+template <class L, class R> struct XorOpType {};
+template<> struct XorOpType<FalseType,FalseType> {typedef FalseType Result;};
+template<> struct XorOpType<FalseType,TrueType> {typedef TrueType Result;};
+template<> struct XorOpType<TrueType, FalseType> {typedef TrueType Result;};
+template<> struct XorOpType<TrueType, TrueType> {typedef FalseType Result;};
+
+/** Compile-time type test: is this one of the built-in integral types?. **/
+template <class T> struct IsIntegralType {
+ /** This typedef is TrueType if the template type T is an integral type;
+ otherwise it is FalseType. **/
+ typedef FalseType Result;
+ /** This compile-time constant bool is true if the template type T is an
+ integral type otherwise it is false. **/
+ static const bool result = false;
+};
+/** This macro must be invoked once for each of the built-in integral types to
+specialize the IsIntegralType struct template for those types. **/
+#define SimTK_SPECIALIZE_INTEGRAL_TYPE(T) \
+ template<> struct IsIntegralType<T> \
+ {typedef TrueType Result; static const bool result = true;}
+
+SimTK_SPECIALIZE_INTEGRAL_TYPE(bool);
+SimTK_SPECIALIZE_INTEGRAL_TYPE(char);
+// This causes problems when used with Qt which for some crazy
+// reason likes to make its own wchar_t rather than using the built in.
+// SimTK_SPECIALIZE_INTEGRAL_TYPE(wchar_t);
+SimTK_SPECIALIZE_INTEGRAL_TYPE(signed char);
+SimTK_SPECIALIZE_INTEGRAL_TYPE(unsigned char);
+SimTK_SPECIALIZE_INTEGRAL_TYPE(short);
+SimTK_SPECIALIZE_INTEGRAL_TYPE(unsigned short);
+SimTK_SPECIALIZE_INTEGRAL_TYPE(int);
+SimTK_SPECIALIZE_INTEGRAL_TYPE(unsigned int); // a.k.a. "unsigned"
+SimTK_SPECIALIZE_INTEGRAL_TYPE(long);
+SimTK_SPECIALIZE_INTEGRAL_TYPE(unsigned long);
+SimTK_SPECIALIZE_INTEGRAL_TYPE(long long);
+SimTK_SPECIALIZE_INTEGRAL_TYPE(unsigned long long);
+
+/** Compile-time type test: is this one of the built-in floating point types?. **/
+template <class T> struct IsFloatingType {
+ /** This typedef is TrueType if the template type T is a floating point type;
+ otherwise it is FalseType. **/
+ typedef FalseType Result;
+ /** This compile-time constant bool is true if the template type T is a
+ floating point type otherwise it is false. **/
+ static const bool result = false;
+};
+/** This macro must be invoked once for each of the built-in floating point
+types to specialize the IsFloatingType struct template for those types. **/
+#define SimTK_SPECIALIZE_FLOATING_TYPE(T) \
+ template<> struct IsFloatingType<T> \
+ {typedef TrueType Result; static const bool result = true;}
+
+SimTK_SPECIALIZE_FLOATING_TYPE(float);
+SimTK_SPECIALIZE_FLOATING_TYPE(double);
+SimTK_SPECIALIZE_FLOATING_TYPE(long double);
+
+/** Compile-time type test: is this the void type?. **/
+template <class T> struct IsVoidType {
+ /** This typedef is TrueType if the template type T is "void";
+ otherwise it is FalseType. **/
+ typedef FalseType Result;
+ /** This compile-time constant bool is true if the template type T is
+ "void" otherwise it is false. **/
+ static const bool result = false;
+};
+template<> struct IsVoidType<void>
+{typedef TrueType Result; static const bool result = true;};
+
+/** Compile-time test: is this one of the built-in "arithmetic" types, meaning
+an integral or floating type? **/
+template <class T> struct IsArithmeticType {
+ /** This typedef is TrueType if the template type T is one of the integral;
+ or floating point types, otherwise it is FalseType. **/
+ typedef OrOpType<typename IsIntegralType<T>::Result,
+ typename IsFloatingType<T>::Result> Result;
+ /** This compile-time constant bool is true if the template type T is
+ one of the integral or floating point types, otherwise it is false. **/
+ static const bool result = IsIntegralType<T>::result
+ || IsFloatingType<T>::result;
+};
+
+// This struct's sole use is to allow us to define the typedef
+// Is64BitPlatformType as equivalent to either TrueType or FalseType.
+template <bool is64Bit> struct Is64BitHelper {};
+template<> struct Is64BitHelper<true>
+{typedef TrueType Result; static const bool result = true;};
+template<> struct Is64BitHelper<false>
+{typedef FalseType Result; static const bool result = false;};
+
+/** Compile-time test: this typedef will be TrueType if this is a 64-bit
+platform, meaning that the size of a pointer is the same as the size of a
+long long; otherwise it will be FalseType and we have a 32-bit platform meaning
+that the size of a pointer is the same as an int. **/
+static const bool Is64BitPlatform = sizeof(size_t) > sizeof(int);
+typedef Is64BitHelper<Is64BitPlatform>::Result Is64BitPlatformType;
+
+
+/** In case you don't like the name you get from typeid(), you can specialize
+this class to provide a nicer name. This is typically used for error messages
+and testing. **/
+template <class T> struct NiceTypeName {
+ static const char* name() {return typeid(T).name();}
+};
+
+/** This specializes the name of a type to be exactly the text you use to
+specify it, rather than whatever ugly thing might result on different platforms
+from resolution of typedefs, default template arguments, etc. **/
+#define SimTK_NICETYPENAME_LITERAL(T) \
+template <> struct NiceTypeName< T > { \
+ static const char* name() { return #T; } \
+};
+
+// Some types for which we'd like to see nice type names.
+SimTK_NICETYPENAME_LITERAL(bool);
+SimTK_NICETYPENAME_LITERAL(char);
+// This causes problems when used with Qt which for some crazy
+// reason likes to make its own wchar_t rather than using the built in.
+// SimTK_NICETYPENAME_LITERAL(wchar_t);
+SimTK_NICETYPENAME_LITERAL(signed char);
+SimTK_NICETYPENAME_LITERAL(unsigned char);
+SimTK_NICETYPENAME_LITERAL(short);
+SimTK_NICETYPENAME_LITERAL(unsigned short);
+SimTK_NICETYPENAME_LITERAL(int);
+SimTK_NICETYPENAME_LITERAL(unsigned); // preferred to "unsigned int"
+SimTK_NICETYPENAME_LITERAL(long);
+SimTK_NICETYPENAME_LITERAL(unsigned long);
+SimTK_NICETYPENAME_LITERAL(long long);
+SimTK_NICETYPENAME_LITERAL(unsigned long long);
+SimTK_NICETYPENAME_LITERAL(float);
+SimTK_NICETYPENAME_LITERAL(double);
+SimTK_NICETYPENAME_LITERAL(long double);
+SimTK_NICETYPENAME_LITERAL(std::string);
+SimTK_NICETYPENAME_LITERAL(std::complex<float>);
+SimTK_NICETYPENAME_LITERAL(std::complex<double>);
+SimTK_NICETYPENAME_LITERAL(std::complex<long double>);
+SimTK_NICETYPENAME_LITERAL(SimTK::FalseType);
+SimTK_NICETYPENAME_LITERAL(SimTK::TrueType);
+
+} // namespace SimTK
+
+#endif /* C++ stuff */
+
+#endif /* SimTK_SimTKCOMMON_COMMON_H_ */
diff --git a/SimTKcommon/include/SimTKlapack.h b/SimTKcommon/include/SimTKlapack.h
new file mode 100644
index 0000000..a79a5a1
--- /dev/null
+++ b/SimTKcommon/include/SimTKlapack.h
@@ -0,0 +1,3215 @@
+#ifndef SimTK_FORTRAN_LAPACK_H_
+#define SimTK_FORTRAN_LAPACK_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: Michael Sherman, Christopher Bruns *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/*
+ * This header file contains const-correct function prototypes for C & C++
+ * programs calling the legacy (Fortran) interface for BLAS and LAPACK
+ * version 3. This header should work for almost any implementation of those
+ * routines, whether our SimTKlapack, Apple's Accelerate lapack & blas, Intel's
+ * Math Kernel Libraries (MKL), AMD's ACML, or Goto- or Atlas-generated
+ * libraries. This will also work with netlib's slow reference implementation,
+ * and should work even using the f2c-translated version of that because
+ * (although the code is in C) it still implements the Fortran calling
+ * sequence.
+ *
+ * CAUTION: THIS INTERFACE USES 32 BIT INTEGERS. So even though addresses
+ * are 8 bytes for a 64-bit lapack and blas, we're expecting all the integer
+ * arguments to remain 4 bytes.
+ *
+ * Do not confuse this interface with the CBLAS and CLAPACK interfaces which
+ * are C-friendly wrappers around the legacy interface. Here we are dealing
+ * with direct calls to the legacy routines (which are Fortran-like) from C
+ * and C++ programs.
+ *
+ * The basic rules for C programs calling Fortran-like routines with the
+ * convention we use (there are others) are:
+ *
+ * 1) Function names are in lower case and have an underscore appended to the
+ * name. For example, if a C program calls LAPACK's ZGEEV routine the call
+ * would be:
+ * zgeev_(...).
+ *
+ * 2) Fortran routines pass scalar arguments by reference. (except for
+ * character string "length" arguments that are normally hidden from
+ * FORTRAN programmers) Therefore a C program needs to pass pointers to
+ * scalar arguments. C++ code can just pass the arguments; they will be
+ * passed by reference automatically because of the declarations here.
+ *
+ * 3) In Fortran 2-D arrays are stored in column major format meaning that
+ * the matrix A = [ 1.0 2.0 ]
+ * [ 3.0 4.0 ]
+ * declared as A(2,2) would be stored in memory as 1.0, 3.0, 2.0, 4.0.
+ * While a C 2-D array declared as a[2][2], would be stored in
+ * row-major order as 1.0, 2.0, 3.0, 4.0. Therefore C programs may need to
+ * transpose 2D arrays before calling the Fortran interface. Note that
+ * SimTK Matrix objects are normally stored using the Fortran convention,
+ * so their data is directly compatible with Lapack.
+ *
+ * 4) The lengths of character strings need to be passed as additional
+ * arguments which are added to the end of the parameter list. For example,
+ * LAPACK's ZGEEV routine has two arguments which are character
+ * strings: JOBVL, JOBVR.
+ *
+ * ZGEEV(JOBVL, JOBVR, N, A, LDA, W, VL, LDVL, VR, LDVR,
+ * WORK, LWORK, RWORK, INFO)
+ *
+ * A C program calling ZGEEV would need to add two additional arguments
+ * at the end of the parameter list which contain the lengths of JOBVL, JOBVR
+ * arguments:
+ * char* jobvl = "N";
+ * char* jobvr = "Vectors";
+ * int len_jobvl = 1;
+ * int len_jobvr = 7;
+ * .
+ * .
+ * .
+ *
+ * zgeev_(jobvl, jobvr, &n, a, &lda, w, vl, &ldvl, vr, &ldvr,
+ * work, &lwork, rwork, &info,
+ * len_jobvl, len_jobvr);
+ * ^^^^^^^^ ^^^^^^^^
+ * additional arguments
+ *
+ * In practice, only the first character is used for any Lapack option so
+ * the length can always be passed as 1. Since these length arguments are
+ * at the end, they can have defaults in C++ and are set to 1 below so
+ * C++ programs do not need to be aware of the length arguments. But calls
+ * from C will typically end with ",1,1,1)" or whatever.
+ */
+
+/*
+ * We're going to define some temporary preprocessor macros here
+ * (SimTK_C_ and SimTK_Z_) to represent complex types.
+ * In C++ these will just be the built-in std::complex types. In C
+ * we'll either use a type supplied by the including module, or we'll
+ * declare complex types here if none are supplied. We assume the
+ * binary representation is the same in all cases:
+ * "float real,imag;" or "double real,imag;".
+ * We define an assortment of temporary macros for other argument
+ * passing situations.
+ * We'll undefine these temporary macros at the end of this header.
+ *
+ * Yes, we know this is ugly with all the macros. Think of it as a "header
+ * file generator" rather than a header file and it is more palatable. Our
+ * goal is to capture all the argument semantics here and then generate the
+ * right behavior.
+ */
+
+#ifdef __cplusplus
+
+ /* This is C++, just use the built-in complex types. */
+ #include <complex>
+ #define SimTK_C_ std::complex<float>
+ #define SimTK_Z_ std::complex<double>
+ #define SimTK_S_INPUT_(s) const float& s
+ #define SimTK_D_INPUT_(d) const double& d
+ #define SimTK_I_INPUT_(i) const int& i
+ #define SimTK_C_INPUT_(c) const std::complex<float>& c
+ #define SimTK_Z_INPUT_(z) const std::complex<double>& z
+
+ #define SimTK_S_INOUT_(s) float& s
+ #define SimTK_D_INOUT_(d) double& d
+ #define SimTK_I_INOUT_(i) int& i
+ #define SimTK_C_INOUT_(c) std::complex<float>& c
+ #define SimTK_Z_INOUT_(z) std::complex<double>& z
+
+ #define SimTK_S_OUTPUT_(s) float& s
+ #define SimTK_D_OUTPUT_(d) double& d
+ #define SimTK_I_OUTPUT_(i) int& i
+ #define SimTK_C_OUTPUT_(c) std::complex<float>& c
+ #define SimTK_Z_OUTPUT_(z) std::complex<double>& z
+
+ #define SimTK_FDIM_(n) const int& n /* a dimension, e.g. N,M,lda */
+ #define SimTK_FINC_(x) const int& inc##x /* increment, i.e. stride */
+ #define SimTK_FOPT_(c) const char& c /* an option, passed as a single char */
+ #define SimTK_CHAR_OUTPUT_(c) char& c /* returns a single char */
+ #define SimTK_FLEN_(c) int c##_len=1 /* dummy length parameter added by Fortran */
+ #define SimTK_INFO_ int& info /* returns error code */
+#else
+
+ /*
+ * This is C, not C++.
+ * Should check for 1999 standard C which has built-in SimTK_C_ type
+ * here. For now we allow type override via preprocessor symbol;
+ * users of 1999 C should provide these before including this file:
+ *
+ * #define SimTK_C_FLOAT_COMPLEX_TYPE_TO_USE float complex
+ * #define SimTK_C_DOUBLE_COMPLEX_TYPE_TO_USE double complex
+ *
+ */
+ #ifdef SimTK_C_FLOAT_COMPLEX_TYPE_TO_USE
+ #define SimTK_C_ SimTK_C_FLOAT_COMPLEX_TYPE_TO_USE
+ #else
+ typedef struct { float real, imag; } SimTK_float_complex;
+ #define SimTK_C_ SimTK_float_complex
+ #endif
+
+ #ifdef SimTK_C_DOUBLE_COMPLEX_TYPE_TO_USE
+ #define SimTK_Z_ SimTK_C_DOUBLE_COMPLEX_TYPE_TO_USE
+ #else
+ typedef struct { double real, imag; } SimTK_double_complex;
+ #define SimTK_Z_ SimTK_double_complex
+ #endif
+
+ #define SimTK_S_INPUT_(s) const float* s
+ #define SimTK_D_INPUT_(d) const double* d
+ #define SimTK_I_INPUT_(i) const int* i
+ #define SimTK_C_INPUT_(c) const SimTK_C_* c
+ #define SimTK_Z_INPUT_(z) const SimTK_Z_* z
+
+ #define SimTK_S_INOUT_(s) float* s
+ #define SimTK_D_INOUT_(d) double* d
+ #define SimTK_I_INOUT_(i) int* i
+ #define SimTK_C_INOUT_(c) SimTK_C_* c
+ #define SimTK_Z_INOUT_(z) SimTK_Z_* z
+
+ #define SimTK_S_OUTPUT_(s) float* s
+ #define SimTK_D_OUTPUT_(d) double* d
+ #define SimTK_I_OUTPUT_(i) int* i
+ #define SimTK_C_OUTPUT_(c) SimTK_C_* c
+ #define SimTK_Z_OUTPUT_(z) SimTK_Z_* z
+
+ #define SimTK_FDIM_(n) const int* n /* a dimension, e.g. N,M,lda */
+ #define SimTK_FINC_(x) const int* inc##x /* increment, i.e. stride */
+ #define SimTK_FOPT_(c) const char* c /* an option, passed as a single char */
+ #define SimTK_CHAR_OUTPUT_(c) char* c /* returns a single char */
+ #define SimTK_FLEN_(c) int c##_len /* dummy length parameter (must set to 1 in call) */
+ #define SimTK_INFO_ int *info /* returns error code */
+
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+ * These are the standard routines provided by all SimTK libraries so that
+ * various information about the particulars of the library can be extracted
+ * from the binary.
+ */
+void SimTK_version_SimTKlapack(int*,int*,int*);
+void SimTK_about_SimTKlapack(const char*, int, char*);
+
+/*
+ * These signatures define callouts to be made by some of the Lapack eigenvalue
+ * routines for selecting eigenvalue subsets.
+ */
+typedef int (* SimTK_SELECT_2S)(SimTK_S_INPUT_(wr), SimTK_S_INPUT_(wi));
+typedef int (* SimTK_SELECT_3F)(SimTK_S_INPUT_(ar), SimTK_S_INPUT_(ai), SimTK_S_INPUT_(b));
+typedef int (* SimTK_SELECT_2D)(SimTK_D_INPUT_(wr), SimTK_D_INPUT_(wi));
+typedef int (* SimTK_SELECT_3D)(SimTK_D_INPUT_(ar), SimTK_D_INPUT_(ai), SimTK_D_INPUT_(b));
+typedef int (* SimTK_SELECT_C) (SimTK_C_INPUT_(w));
+typedef int (* SimTK_SELECT_2C)(SimTK_C_INPUT_(a), SimTK_C_INPUT_(b));
+typedef int (* SimTK_SELECT_Z) (SimTK_Z_INPUT_(w));
+typedef int (* SimTK_SELECT_2Z)(SimTK_Z_INPUT_(a), SimTK_Z_INPUT_(b));
+
+/*******************************************************************************
+ * The BLAS routines. For documentation, see the LAPACK User's Guide, 3rd ed., *
+ * Appendix C "Quick Reference Guide to the BLAS", pg. 180-4. *
+ *******************************************************************************/
+
+/*
+ * ****************
+ * * BLAS Level 1 *
+ * ****************
+ *
+ * BLAS Level 1 functions (that is, value-returning methods).
+ *
+ * TODO: The following functions return complex values. This is OK in C++ but
+ * what about C?
+ * cdotu_, zdotu_ (complex dot product without conjugation)
+ * cdotc_, zdotc_ (complex dot product with conjugation)
+ *
+ * SimTKlapack 1.2 was compiled with gfortran and the -ff2c flag on Mac
+ * and Linux and g77 on Windows (gfortran still had issues on Windows). In
+ * either case the four routines below return SimTK_C_ or SimTK_Z_ type in
+ * C++ but for C programs they return a pointer to a SimTK_C_ or SimTK_Z_
+ * type as an extra parameter.
+ */
+/*
+#ifdef __cplusplus
+extern SimTK_C_ cdotu_(SimTK_FDIM_(n), const SimTK_C_ *x, SimTK_FINC_(x),
+ const SimTK_C_ *y, SimTK_FINC_(y));
+extern SimTK_Z_ zdotu_(SimTK_FDIM_(n), const SimTK_Z_ *x, SimTK_FINC_(x),
+ const SimTK_Z_ *y, SimTK_FINC_(y));
+extern SimTK_C_ cdotc_(SimTK_FDIM_(n), const SimTK_C_ *x, SimTK_FINC_(x),
+ const SimTK_C_ *y, SimTK_FINC_(y));
+extern SimTK_Z_ zdotc_(SimTK_FDIM_(n), const SimTK_Z_ *x, SimTK_FINC_(x),
+ const SimTK_Z_ *y, SimTK_FINC_(y));
+#else
+extern void cdotu_(SimTK_C_*, SimTK_FDIM_(n), const SimTK_C_ *x, SimTK_FINC_(x),
+ const SimTK_C_ *y, SimTK_FINC_(y));
+extern void zdotu_(SimTK_Z_*, SimTK_FDIM_(n), const SimTK_Z_ *x, SimTK_FINC_(x),
+ const SimTK_Z_ *y, SimTK_FINC_(y));
+extern void cdotc_(SimTK_C_*, SimTK_FDIM_(n), const SimTK_C_ *x, SimTK_FINC_(x),
+ const SimTK_C_ *y, SimTK_FINC_(y));
+extern void zdotc_(SimTK_Z_*, SimTK_FDIM_(n), const SimTK_Z_ *x, SimTK_FINC_(x),
+ const SimTK_Z_ *y, SimTK_FINC_(y));
+#endif
+*/
+
+extern float sdot_ (SimTK_FDIM_(n), const float *x, SimTK_FINC_(x), const float *y, SimTK_FINC_(y));
+extern double ddot_ (SimTK_FDIM_(n), const double *x, SimTK_FINC_(x), const double *y, SimTK_FINC_(y));
+extern double dsdot_ (SimTK_FDIM_(n), const float *x, SimTK_FINC_(x), const float *y, SimTK_FINC_(y));
+extern float sdsdot_(SimTK_FDIM_(n), SimTK_S_INPUT_(alpha),
+ const float *x, SimTK_FINC_(x), const float *y, SimTK_FINC_(y));
+
+/* Functions having prefixes S D SC DZ */
+extern float snrm2_(SimTK_FDIM_(n), const float *x, SimTK_FINC_(x));
+extern float sasum_(SimTK_FDIM_(n), const float *x, SimTK_FINC_(x));
+
+extern double dnrm2_(SimTK_FDIM_(n), const double *x, SimTK_FINC_(x));
+extern double dasum_(SimTK_FDIM_(n), const double *x, SimTK_FINC_(x));
+
+extern float scnrm2_(SimTK_FDIM_(n), const SimTK_C_ *x, SimTK_FINC_(x));
+extern float scasum_(SimTK_FDIM_(n), const SimTK_C_ *x, SimTK_FINC_(x));
+
+extern double dznrm2_(SimTK_FDIM_(n), const SimTK_Z_ *x, SimTK_FINC_(x));
+extern double dzasum_(SimTK_FDIM_(n), const SimTK_Z_ *x, SimTK_FINC_(x));
+
+/* Int functions having standard 4 prefixes I(S D C Z) */
+extern int isamax_(SimTK_FDIM_(n), const float *x, SimTK_FINC_(x));
+extern int idamax_(SimTK_FDIM_(n), const double *x, SimTK_FINC_(x));
+extern int icamax_(SimTK_FDIM_(n), const SimTK_C_ *x, SimTK_FINC_(x));
+extern int izamax_(SimTK_FDIM_(n), const SimTK_Z_ *x, SimTK_FINC_(x));
+
+/* BLAS Level 1 subroutines (that is, void methods). */
+
+/* Routines with standard 4 prefixes _(s, d, c, z) */
+extern void sswap_(SimTK_FDIM_(n), float *x, SimTK_FINC_(x), float *y, SimTK_FINC_(y));
+extern void dswap_(SimTK_FDIM_(n), double *x, SimTK_FINC_(x), double *y, SimTK_FINC_(y));
+extern void cswap_(SimTK_FDIM_(n), SimTK_C_ *x, SimTK_FINC_(x), SimTK_C_ *y, SimTK_FINC_(y));
+extern void zswap_(SimTK_FDIM_(n), SimTK_Z_ *x, SimTK_FINC_(x), SimTK_Z_ *y, SimTK_FINC_(y));
+
+/* assign y = x */
+extern void scopy_(SimTK_FDIM_(n), const float *x, SimTK_FINC_(x), float *y, SimTK_FINC_(y));
+extern void dcopy_(SimTK_FDIM_(n), const double *x, SimTK_FINC_(x), double *y, SimTK_FINC_(y));
+extern void ccopy_(SimTK_FDIM_(n), const SimTK_C_ *x, SimTK_FINC_(x), SimTK_C_ *y, SimTK_FINC_(y));
+extern void zcopy_(SimTK_FDIM_(n), const SimTK_Z_ *x, SimTK_FINC_(x), SimTK_Z_ *y, SimTK_FINC_(y));
+
+/* y += ax */
+extern void saxpy_(SimTK_FDIM_(n), SimTK_S_INPUT_(alpha), const float *x, SimTK_FINC_(x), float *y, SimTK_FINC_(y));
+extern void daxpy_(SimTK_FDIM_(n), SimTK_D_INPUT_(alpha), const double *x, SimTK_FINC_(x), double *y, SimTK_FINC_(y));
+extern void caxpy_(SimTK_FDIM_(n), SimTK_C_INPUT_(alpha), const SimTK_C_ *x, SimTK_FINC_(x), SimTK_C_ *y, SimTK_FINC_(y));
+extern void zaxpy_(SimTK_FDIM_(n), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *x, SimTK_FINC_(x), SimTK_Z_ *y, SimTK_FINC_(y));
+
+
+/*
+ * Routines with S and D prefix only
+ */
+
+/* a,b are in/out, c,s are output (all scalars) */
+extern void srotg_(SimTK_S_OUTPUT_(a), SimTK_S_OUTPUT_(b), SimTK_S_OUTPUT_(c), SimTK_S_OUTPUT_(s));
+extern void drotg_(SimTK_D_OUTPUT_(a), SimTK_D_OUTPUT_(b), SimTK_D_OUTPUT_(c), SimTK_D_OUTPUT_(s));
+
+/* all parameters are in/out */
+extern void srotmg_(SimTK_S_OUTPUT_(d1), SimTK_S_OUTPUT_(d2), SimTK_S_OUTPUT_(b1), SimTK_S_OUTPUT_(b2), float P[5]);
+extern void drotmg_(SimTK_D_OUTPUT_(d1), SimTK_D_OUTPUT_(d2), SimTK_D_OUTPUT_(b1), SimTK_D_OUTPUT_(b2), double P[5]);
+
+extern void srot_(SimTK_FDIM_(n), float *x, SimTK_FINC_(x), float *y, SimTK_FINC_(y), SimTK_S_INPUT_(c), SimTK_S_INPUT_(s));
+extern void drot_(SimTK_FDIM_(n), double *x, SimTK_FINC_(x), double *y, SimTK_FINC_(y), SimTK_D_INPUT_(c), SimTK_D_INPUT_(s));
+
+extern void srotm_(SimTK_FDIM_(n), float *x, SimTK_FINC_(x), float *y, SimTK_FINC_(y), const float P[5]);
+extern void drotm_(SimTK_FDIM_(n), double *x, SimTK_FINC_(x), double *y, SimTK_FINC_(y), const double P[5]);
+
+
+/*
+ * Routines with S D C Z CS and ZD prefixes
+ */
+extern void sscal_ (SimTK_FDIM_(n), SimTK_S_INPUT_(alpha), float *x, SimTK_FINC_(x));
+extern void dscal_ (SimTK_FDIM_(n), SimTK_D_INPUT_(alpha), double *x, SimTK_FINC_(x));
+extern void cscal_ (SimTK_FDIM_(n), SimTK_C_INPUT_(alpha), SimTK_C_ *x, SimTK_FINC_(x));
+extern void zscal_ (SimTK_FDIM_(n), SimTK_Z_INPUT_(alpha), SimTK_Z_ *x, SimTK_FINC_(x));
+extern void csscal_(SimTK_FDIM_(n), SimTK_S_INPUT_(alpha), SimTK_C_ *x, SimTK_FINC_(x));
+extern void zdscal_(SimTK_FDIM_(n), SimTK_D_INPUT_(alpha), SimTK_Z_ *x, SimTK_FINC_(x));
+
+/*
+ * Extra reference routines provided by ATLAS, but not mandated by the
+ * standard.
+ */
+extern void crotg_(SimTK_C_OUTPUT_(a), SimTK_C_INPUT_(b), SimTK_S_OUTPUT_(c), SimTK_C_OUTPUT_(s) );
+extern void zrotg_(SimTK_Z_OUTPUT_(a), SimTK_Z_INPUT_(b), SimTK_D_OUTPUT_(c), SimTK_Z_OUTPUT_(s) );
+extern void csrot_(SimTK_FDIM_(n), SimTK_C_ *x, SimTK_FINC_(x), SimTK_C_ *y, SimTK_FINC_(y),
+ SimTK_S_INPUT_(c), SimTK_S_INPUT_(s));
+extern void zdrot_(SimTK_FDIM_(n), SimTK_Z_ *x, SimTK_FINC_(x), SimTK_Z_ *y, SimTK_FINC_(y),
+ SimTK_D_INPUT_(c), SimTK_D_INPUT_(s));
+
+/*
+ *===========================================================================
+ * Prototypes for level 2 BLAS
+ *===========================================================================
+ */
+
+/*
+ *Routines with standard 4 prefixes _(S, D, C, Z)
+ */
+
+/* y = alpha A x + beta y */
+extern void sgemv_(SimTK_FOPT_(transA), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_S_INPUT_(alpha), const float *A, SimTK_FDIM_(lda), const float *x, SimTK_FINC_(x), SimTK_S_INPUT_(beta), float *Y, SimTK_FINC_(Y), SimTK_FLEN_(transA));
+extern void sgbmv_(SimTK_FOPT_(transA), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_S_INPUT_(alpha), const float *A, SimTK_FDIM_(lda), const float *x, SimTK_FINC_(x), SimTK_S_INPUT_(beta), float *Y, SimTK_FINC_(Y), SimTK_FLEN_(transA));
+extern void strmv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), const float *A, SimTK_FDIM_(lda), float *x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void stbmv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(k), const float *A, SimTK_FDIM_(lda), float *x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void stpmv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), const float *Ap, float *x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void strsv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), const float *A, SimTK_FDIM_(lda), float *x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void stbsv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(k), const float *A, SimTK_FDIM_(lda), float *x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void stpsv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), const float *Ap, float *x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+
+extern void dgemv_(SimTK_FOPT_(transA), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_D_INPUT_(alpha), const double *A, SimTK_FDIM_(lda), const double *x, SimTK_FINC_(x), SimTK_D_INPUT_(beta), double *Y, SimTK_FINC_(Y), SimTK_FLEN_(transA));
+extern void dgbmv_(SimTK_FOPT_(transA), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_D_INPUT_(alpha), const double *A, SimTK_FDIM_(lda), const double *x, SimTK_FINC_(x), SimTK_D_INPUT_(beta), double *Y, SimTK_FINC_(Y), SimTK_FLEN_(transA));
+extern void dtrmv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), const double *A, SimTK_FDIM_(lda), double *x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void dtbmv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(k), const double *A, SimTK_FDIM_(lda), double *x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void dtpmv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), const double *Ap, double *x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void dtrsv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), const double *A, SimTK_FDIM_(lda), double *x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void dtbsv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(k), const double *A, SimTK_FDIM_(lda), double *x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void dtpsv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), const double *Ap, double *x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+
+extern void cgemv_(SimTK_FOPT_(transA), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_INPUT_(alpha), const SimTK_C_ *A, SimTK_FDIM_(lda), const SimTK_C_ *x, SimTK_FINC_(x), SimTK_C_INPUT_(beta), SimTK_C_ *Y, SimTK_FINC_(Y), SimTK_FLEN_(transA));
+extern void cgbmv_(SimTK_FOPT_(transA), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_C_INPUT_(alpha), const SimTK_C_ *A, SimTK_FDIM_(lda), const SimTK_C_ *x, SimTK_FINC_(x), SimTK_C_INPUT_(beta), SimTK_C_ *Y, SimTK_FINC_(Y), SimTK_FLEN_(transA));
+extern void ctrmv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), const SimTK_C_ *A, SimTK_FDIM_(lda), SimTK_C_*x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void ctbmv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_C_ *A, SimTK_FDIM_(lda), SimTK_C_*x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void ctpmv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), const SimTK_C_ *Ap, SimTK_C_*x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void ctrsv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), const SimTK_C_ *A, SimTK_FDIM_(lda), SimTK_C_*x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void ctbsv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_C_ *A, SimTK_FDIM_(lda), SimTK_C_*x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void ctpsv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), const SimTK_C_ *Ap, SimTK_C_*x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+
+extern void zgemv_(SimTK_FOPT_(transA), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *A, SimTK_FDIM_(lda), const SimTK_Z_ *x, SimTK_FINC_(x), SimTK_Z_INPUT_(beta), SimTK_Z_ *Y, SimTK_FINC_(Y), SimTK_FLEN_(transA));
+extern void zgbmv_(SimTK_FOPT_(transA), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *A, SimTK_FDIM_(lda), const SimTK_Z_ *x, SimTK_FINC_(x), SimTK_Z_INPUT_(beta), SimTK_Z_ *Y, SimTK_FINC_(Y),
+ SimTK_FLEN_(transA));
+extern void ztrmv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), const SimTK_Z_ *A, SimTK_FDIM_(lda), SimTK_Z_*x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void ztbmv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_Z_ *A, SimTK_FDIM_(lda), SimTK_Z_*x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+extern void ztpmv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), const SimTK_Z_ *Ap, SimTK_Z_*x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+extern void ztrsv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), const SimTK_Z_ *A, SimTK_FDIM_(lda), SimTK_Z_*x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+extern void ztbsv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_Z_ *A, SimTK_FDIM_(lda), SimTK_Z_*x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void ztpsv_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(n), const SimTK_Z_ *Ap, SimTK_Z_*x, SimTK_FINC_(x), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+
+/*
+ * Routines with S and D prefixes only
+ */
+/* y = alpha A x + beta y */
+extern void ssymv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_S_INPUT_(alpha), const float *A, SimTK_FDIM_(lda), const float *x, SimTK_FINC_(x), SimTK_S_INPUT_(beta), float *y, SimTK_FINC_(y), SimTK_FLEN_(uplo));
+extern void ssbmv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_S_INPUT_(alpha), const float *A, SimTK_FDIM_(lda), const float *x, SimTK_FINC_(x), SimTK_S_INPUT_(beta), float *y, SimTK_FINC_(y), SimTK_FLEN_(uplo));
+extern void sspmv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_S_INPUT_(alpha), const float *Ap, const float *x, SimTK_FINC_(x), SimTK_S_INPUT_(beta), float *y, SimTK_FINC_(y), SimTK_FLEN_(uplo));
+
+extern void dsymv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_D_INPUT_(alpha), const double *A, SimTK_FDIM_(lda), const double *x, SimTK_FINC_(x), SimTK_D_INPUT_(beta), double *y, SimTK_FINC_(y), SimTK_FLEN_(uplo));
+extern void dsbmv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_D_INPUT_(alpha), const double *A, SimTK_FDIM_(lda), const double *x, SimTK_FINC_(x), SimTK_D_INPUT_(beta), double *y, SimTK_FINC_(y), SimTK_FLEN_(uplo));
+extern void dspmv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_D_INPUT_(alpha), const double *Ap, const double *x, SimTK_FINC_(x), SimTK_D_INPUT_(beta), double *y, SimTK_FINC_(y), SimTK_FLEN_(uplo));
+
+/* x,y are const, A is in/out */
+extern void sger_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_S_INPUT_(alpha), const float *x, SimTK_FINC_(x), const float *y, SimTK_FINC_(y), float *A, SimTK_FDIM_(lda));
+extern void ssyr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_S_INPUT_(alpha), const float *x, SimTK_FINC_(x), float *A, SimTK_FDIM_(lda), SimTK_FLEN_(uplo));
+extern void sspr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_S_INPUT_(alpha), const float *x, SimTK_FINC_(x), float *Ap, SimTK_FLEN_(uplo));
+extern void ssyr2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_S_INPUT_(alpha), const float *x, SimTK_FINC_(x), const float *y, SimTK_FINC_(y), float *A, SimTK_FDIM_(lda), SimTK_FLEN_(uplo));
+extern void sspr2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_S_INPUT_(alpha), const float *x, SimTK_FINC_(x), const float *y, SimTK_FINC_(y), float *A, SimTK_FLEN_(uplo));
+
+extern void dger_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_D_INPUT_(alpha), const double *x, SimTK_FINC_(x), const double *y, SimTK_FINC_(y), double *A, SimTK_FDIM_(lda));
+extern void dsyr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_D_INPUT_(alpha), const double *x, SimTK_FINC_(x), double *A, SimTK_FDIM_(lda), SimTK_FLEN_(uplo));
+extern void dspr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_D_INPUT_(alpha), const double *x, SimTK_FINC_(x), double *Ap, SimTK_FLEN_(uplo));
+extern void dsyr2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_D_INPUT_(alpha), const double *x, SimTK_FINC_(x), const double *y, SimTK_FINC_(y), double *A, SimTK_FDIM_(lda), SimTK_FLEN_(uplo));
+extern void dspr2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_D_INPUT_(alpha), const double *x, SimTK_FINC_(x), const double *y, SimTK_FINC_(y), double *A, SimTK_FLEN_(uplo));
+
+/*
+ * Routines with C and Z prefixes only
+ */
+/* y = alpha A x + beta y */
+extern void chemv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_INPUT_(alpha), const SimTK_C_ *A, SimTK_FDIM_(lda), const SimTK_C_ *x, SimTK_FINC_(x), SimTK_C_INPUT_(beta), SimTK_C_ *y, SimTK_FINC_(y), SimTK_FLEN_(uplo));
+extern void chbmv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_C_INPUT_(alpha), const SimTK_C_ *A, SimTK_FDIM_(lda), const SimTK_C_ *x, SimTK_FINC_(x), SimTK_C_INPUT_(beta), SimTK_C_ *y, SimTK_FINC_(y), SimTK_FLEN_(uplo));
+extern void chpmv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_INPUT_(alpha), const SimTK_C_ *Ap, const SimTK_C_ *x, SimTK_FINC_(x), SimTK_C_INPUT_(beta), SimTK_C_ *y, SimTK_FINC_(y), SimTK_FLEN_(uplo));
+
+extern void zhemv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *A, SimTK_FDIM_(lda), const SimTK_Z_ *x, SimTK_FINC_(x), SimTK_Z_INPUT_(beta), SimTK_Z_ *y, SimTK_FINC_(y), SimTK_FLEN_(uplo));
+extern void zhbmv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *A, SimTK_FDIM_(lda), const SimTK_Z_ *x, SimTK_FINC_(x), SimTK_Z_INPUT_(beta), SimTK_Z_ *y, SimTK_FINC_(y), SimTK_FLEN_(uplo));
+extern void zhpmv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *Ap, const SimTK_Z_ *x, SimTK_FINC_(x), SimTK_Z_INPUT_(beta), SimTK_Z_ *y, SimTK_FINC_(y), SimTK_FLEN_(uplo));
+
+/* x,y are const, A is in/out */
+extern void cgeru_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_INPUT_(alpha), const SimTK_C_ *x, SimTK_FINC_(x), const SimTK_C_ *y, SimTK_FINC_(y), SimTK_C_ *A, SimTK_FDIM_(lda));
+extern void cgerc_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_INPUT_(alpha), const SimTK_C_ *x, SimTK_FINC_(x), const SimTK_C_ *y, SimTK_FINC_(y), SimTK_C_ *A, SimTK_FDIM_(lda));
+extern void cher_ (SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_S_INPUT_(alpha), const SimTK_C_ *x, SimTK_FINC_(x), SimTK_C_ *A, SimTK_FDIM_(lda), SimTK_FLEN_(uplo));
+extern void chpr_ (SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_S_INPUT_(alpha), const SimTK_C_ *x, SimTK_FINC_(x), SimTK_C_ *A, SimTK_FLEN_(uplo));
+extern void cher2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_INPUT_(alpha), const SimTK_C_ *x, SimTK_FINC_(x), const SimTK_C_ *y, SimTK_FINC_(y), SimTK_C_ *A, SimTK_FDIM_(lda), SimTK_FLEN_(uplo));
+extern void chpr2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_INPUT_(alpha), const SimTK_C_ *x, SimTK_FINC_(x), const SimTK_C_ *y, SimTK_FINC_(y), SimTK_C_ *Ap, SimTK_FLEN_(uplo));
+
+extern void zgeru_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *x, SimTK_FINC_(x), const SimTK_Z_ *y, SimTK_FINC_(y), SimTK_Z_ *A, SimTK_FDIM_(lda));
+extern void zgerc_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *x, SimTK_FINC_(x), const SimTK_Z_ *y, SimTK_FINC_(y), SimTK_Z_ *A, SimTK_FDIM_(lda));
+extern void zher_ (SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_D_INPUT_(alpha), const SimTK_Z_ *x, SimTK_FINC_(x), SimTK_Z_ *A, SimTK_FDIM_(lda), SimTK_FLEN_(uplo));
+extern void zhpr_ (SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_D_INPUT_(alpha), const SimTK_Z_ *x, SimTK_FINC_(x), SimTK_Z_ *A, SimTK_FLEN_(uplo));
+extern void zher2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *x, SimTK_FINC_(x), const SimTK_Z_ *y, SimTK_FINC_(y), SimTK_Z_ *A, SimTK_FDIM_(lda), SimTK_FLEN_(uplo));
+extern void zhpr2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *x, SimTK_FINC_(x), const SimTK_Z_ *y, SimTK_FINC_(y), SimTK_Z_ *Ap, SimTK_FLEN_(uplo));
+
+/*
+ *===========================================================================
+ * Prototypes for level 3 BLAS
+ *===========================================================================
+ */
+
+/*
+ * Routines with standard 4 prefixes _(S, D, C, Z)
+ */
+/* A, B are input, C in/out for gemm, symm, syrk, syr2k */
+extern void sgemm_ (SimTK_FOPT_(transA), SimTK_FOPT_(transB), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_S_INPUT_(alpha), const float *A, SimTK_FDIM_(lda), const float *B, SimTK_FDIM_(ldb), SimTK_S_INPUT_(beta), float *C, SimTK_FDIM_(ldc), SimTK_FLEN_(transA), SimTK_FLEN_(transB));
+extern void ssymm_ (SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_S_INPUT_(alpha), const float *A, SimTK_FDIM_(lda), const float *B, SimTK_FDIM_(ldb), SimTK_S_INPUT_(beta), float *C, SimTK_FDIM_(ldc), SimTK_FLEN_(side), SimTK_FLEN_(uplo));
+extern void ssyrk_ (SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_S_INPUT_(alpha), const float *A, SimTK_FDIM_(lda), SimTK_S_INPUT_(beta), float *C, SimTK_FDIM_(ldc), SimTK_FLEN_(uplo), SimTK_FLEN_(transA));
+extern void ssyr2k_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_S_INPUT_(alpha), const float *A, SimTK_FDIM_(lda), const float *B, SimTK_FDIM_(ldb), SimTK_S_INPUT_(beta), float *C, SimTK_FDIM_(ldc), SimTK_FLEN_(uplo), SimTK_FLEN_(transA));
+
+extern void dgemm_ (SimTK_FOPT_(transA), SimTK_FOPT_(transB), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_D_INPUT_(alpha), const double *A, SimTK_FDIM_(lda),
+ const double *B, SimTK_FDIM_(ldb), SimTK_D_INPUT_(beta), double *C, SimTK_FDIM_(ldc), SimTK_FLEN_(transA), SimTK_FLEN_(transB));
+extern void dsymm_ (SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_D_INPUT_(alpha), const double *A, SimTK_FDIM_(lda), const double *B, SimTK_FDIM_(ldb), SimTK_D_INPUT_(beta), double *C, SimTK_FDIM_(ldc), SimTK_FLEN_(side), SimTK_FLEN_(uplo));
+extern void dsyrk_ (SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_D_INPUT_(alpha), const double *A, SimTK_FDIM_(lda), SimTK_D_INPUT_(beta), double *C, SimTK_FDIM_(ldc), SimTK_FLEN_(uplo), SimTK_FLEN_(transA));
+extern void dsyr2k_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_D_INPUT_(alpha), const double *A, SimTK_FDIM_(lda), const double *B, SimTK_FDIM_(ldb), SimTK_D_INPUT_(beta), double *C, SimTK_FDIM_(ldc), SimTK_FLEN_(uplo), SimTK_FLEN_(transA));
+
+extern void cgemm_ (SimTK_FOPT_(transA), SimTK_FOPT_(transB), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_C_INPUT_(alpha), const SimTK_C_ *A, SimTK_FDIM_(lda),
+ const SimTK_C_ *B, SimTK_FDIM_(ldb), SimTK_C_INPUT_(beta), SimTK_C_ *C, SimTK_FDIM_(ldc), SimTK_FLEN_(transA), SimTK_FLEN_(transB));
+extern void csymm_ (SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_INPUT_(alpha), const SimTK_C_ *A, SimTK_FDIM_(lda), const SimTK_C_ *B, SimTK_FDIM_(ldb), SimTK_C_INPUT_(beta), SimTK_C_ *C, SimTK_FDIM_(ldc), SimTK_FLEN_(side), SimTK_FLEN_(uplo));
+extern void csyrk_ (SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_C_INPUT_(alpha), const SimTK_C_ *A, SimTK_FDIM_(lda), SimTK_C_INPUT_(beta), SimTK_C_ *C, SimTK_FDIM_(ldc), SimTK_FLEN_(uplo), SimTK_FLEN_(transA));
+extern void csyr2k_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_C_INPUT_(alpha), const SimTK_C_ *A, SimTK_FDIM_(lda), const SimTK_C_ *B, SimTK_FDIM_(ldb), SimTK_C_INPUT_(beta), SimTK_C_ *C, SimTK_FDIM_(ldc), SimTK_FLEN_(uplo), SimTK_FLEN_(transA));
+
+extern void zgemm_ (SimTK_FOPT_(transA), SimTK_FOPT_(transB), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *A, SimTK_FDIM_(lda),
+ const SimTK_Z_ *B, SimTK_FDIM_(ldb), SimTK_Z_INPUT_(beta), SimTK_Z_ *C, SimTK_FDIM_(ldc), SimTK_FLEN_(transA), SimTK_FLEN_(transB));
+extern void zsymm_ (SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *A, SimTK_FDIM_(lda), const SimTK_Z_ *B, SimTK_FDIM_(ldb), SimTK_Z_INPUT_(beta), SimTK_Z_ *C, SimTK_FDIM_(ldc), SimTK_FLEN_(side), SimTK_FLEN_(uplo));
+extern void zsyrk_ (SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *A, SimTK_FDIM_(lda), SimTK_Z_INPUT_(beta), SimTK_Z_ *C, SimTK_FDIM_(ldc), SimTK_FLEN_(uplo), SimTK_FLEN_(trans));
+extern void zsyr2k_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *A, SimTK_FDIM_(lda), const SimTK_Z_ *B, SimTK_FDIM_(ldb), SimTK_Z_INPUT_(beta), SimTK_Z_ *C, SimTK_FDIM_(ldc), SimTK_FLEN_(uplo), SimTK_FLEN_(trans));
+
+/* A is input, B in/out for trmm and trsm */
+extern void strmm_(SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_S_INPUT_(alpha), const float *A, SimTK_FDIM_(lda), float *B, SimTK_FDIM_(ldb), SimTK_FLEN_(side), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void strsm_(SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_S_INPUT_(alpha), const float *A, SimTK_FDIM_(lda), float *B, SimTK_FDIM_(ldb), SimTK_FLEN_(side), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+
+extern void dtrmm_(SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_D_INPUT_(alpha), const double *A, SimTK_FDIM_(lda), double *B, SimTK_FDIM_(ldb), SimTK_FLEN_(side), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void dtrsm_(SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_D_INPUT_(alpha), const double *A, SimTK_FDIM_(lda), double *B, SimTK_FDIM_(ldb), SimTK_FLEN_(side), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+
+extern void ctrmm_(SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_INPUT_(alpha), const SimTK_C_ *A, SimTK_FDIM_(lda), SimTK_C_ *B, SimTK_FDIM_(ldb), SimTK_FLEN_(side), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void ctrsm_(SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_INPUT_(alpha), const SimTK_C_ *A, SimTK_FDIM_(lda), SimTK_C_ *B, SimTK_FDIM_(ldb), SimTK_FLEN_(side), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+
+extern void ztrmm_(SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *A, SimTK_FDIM_(lda), SimTK_Z_ *B, SimTK_FDIM_(ldb), SimTK_FLEN_(side), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+extern void ztrsm_(SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FOPT_(diag), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *A, SimTK_FDIM_(lda), SimTK_Z_ *B, SimTK_FDIM_(ldb), SimTK_FLEN_(side), SimTK_FLEN_(uplo), SimTK_FLEN_(transA), SimTK_FLEN_(diag));
+
+/*
+ * Routines with prefixes C and Z only
+ */
+/* A, B are input, C in/out for hemm, herk, her2k */
+extern void chemm_ (SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_INPUT_(alpha), const SimTK_C_ *A, SimTK_FDIM_(lda), const SimTK_C_ *B, SimTK_FDIM_(ldb), SimTK_C_INPUT_(beta), SimTK_C_ *C, SimTK_FDIM_(ldc), SimTK_FLEN_(side), SimTK_FLEN_(uplo));
+extern void cherk_ (SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_S_INPUT_(alpha), const SimTK_C_ *A, SimTK_FDIM_(lda), SimTK_S_INPUT_(beta), SimTK_C_ *C, SimTK_FDIM_(ldc), SimTK_FLEN_(uplo), SimTK_FLEN_(trans));
+extern void cher2k_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_C_INPUT_(alpha), const SimTK_C_ *A, SimTK_FDIM_(lda), const SimTK_C_ *B, SimTK_FDIM_(ldb), SimTK_S_INPUT_(beta), SimTK_C_ *C, SimTK_FDIM_(ldc), SimTK_FLEN_(uplo), SimTK_FLEN_(trans));
+
+extern void zhemm_ (SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *A, SimTK_FDIM_(lda), const SimTK_Z_ *B, SimTK_FDIM_(ldb), SimTK_Z_INPUT_(beta), SimTK_Z_ *C, SimTK_FDIM_(ldc), SimTK_FLEN_(side), SimTK_FLEN_(uplo));
+extern void zherk_ (SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_D_INPUT_(alpha), const SimTK_Z_ *A, SimTK_FDIM_(lda), SimTK_D_INPUT_(beta), SimTK_Z_ *C, SimTK_FDIM_(ldc), SimTK_FLEN_(uplo), SimTK_FLEN_(trans));
+extern void zher2k_(SimTK_FOPT_(uplo), SimTK_FOPT_(transA), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *A, SimTK_FDIM_(lda), const SimTK_Z_ *B, SimTK_FDIM_(ldb), SimTK_D_INPUT_(beta), SimTK_Z_ *C, SimTK_FDIM_(ldc), SimTK_FLEN_(uplo), SimTK_FLEN_(trans));
+
+/* END OF BLAS ROUTINES */
+
+
+
+
+
+
+/********************************************************************************
+ * The LAPACK routines. For documentation, see the LAPACK User's Guide, 3rd ed. *
+ ********************************************************************************/
+
+extern void cbdsqr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(ncvt), SimTK_FDIM_(nru), SimTK_FDIM_(ncc), float *d__, float *e, SimTK_C_ *vt, SimTK_FDIM_(ldvt), SimTK_C_ *u, SimTK_FDIM_(ldu), SimTK_C_ *c__, SimTK_FDIM_(ldc), float *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+extern void cgbbrd_(SimTK_FOPT_(vect), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(ncc), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_C_ *ab, SimTK_FDIM_(ldab), float *d__, float *e, SimTK_C_ *q, SimTK_FDIM_(ldq), SimTK_C_ *pt, SimTK_FDIM_(ldpt), SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(vect));
+
+extern void cgbcon_(SimTK_FOPT_(norm), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), const SimTK_C_ *ab, SimTK_FDIM_(ldab), const int *ipiv, SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(norm));
+
+extern void cgbequ_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), const SimTK_C_ *ab, SimTK_FDIM_(ldab), float *r__, float *c__, float *rowcnd, float *colcnd, float *amax, SimTK_INFO_);
+
+extern void cgbrfs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_FDIM_(nrhs), const SimTK_C_ *ab, SimTK_FDIM_(ldab), const SimTK_C_ *afb, SimTK_FDIM_(ldafb), const int *ipiv, const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void cgbsv_(SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_FDIM_(nrhs), SimTK_C_ *ab, SimTK_FDIM_(ldab), int *ipiv, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_);
+
+extern void cgbsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_FDIM_(nrhs), SimTK_C_ *ab, SimTK_FDIM_(ldab), SimTK_C_ *afb, SimTK_FDIM_(ldafb), int *ipiv, SimTK_CHAR_OUTPUT_(equed), float *r__, float *c__, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(rcond), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(trans), SimTK_FLEN_(equed));
+
+extern void cgbtf2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_C_ *ab, SimTK_FDIM_(ldab), int *ipiv, SimTK_INFO_);
+
+extern void cgbtrf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_C_ *ab, SimTK_FDIM_(ldab), int *ipiv, SimTK_INFO_);
+
+extern void cgbtrs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_FDIM_(nrhs), const SimTK_C_ *ab, SimTK_FDIM_(ldab), const int *ipiv, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void cgebak_(SimTK_FOPT_(job), SimTK_FOPT_(side), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_S_INPUT_(scale), SimTK_I_INPUT_(m), SimTK_C_ *v, SimTK_FDIM_(ldv), SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(side));
+
+extern void cgebal_(SimTK_FOPT_(job), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_I_OUTPUT_(ilo), SimTK_I_OUTPUT_(ihi), SimTK_S_OUTPUT_(scale), SimTK_INFO_, SimTK_FLEN_(job));
+
+extern void cgebd2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), float *d__, float *e, SimTK_C_ *tauq, SimTK_C_ *taup, SimTK_C_ *work, SimTK_INFO_);
+
+extern void cgebrd_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), float *d__, float *e, SimTK_C_ *tauq, SimTK_C_ *taup, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void cgecon_(SimTK_FOPT_(norm), SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(norm));
+
+extern void cgeequ_(SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), float *r__, float *c__, float *rowcnd, float *colcnd, float *amax, SimTK_INFO_);
+
+extern void cgees_(SimTK_FOPT_(jobvs), SimTK_FOPT_(sort), SimTK_SELECT_C select, SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), int *sdim, SimTK_C_ *w, SimTK_C_ *vs, SimTK_FDIM_(ldvs), SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, int *bwork, SimTK_INFO_, SimTK_FLEN_(jobvs), SimTK_FLEN_(sort));
+
+extern void cgeesx_(SimTK_FOPT_(jobvs), SimTK_FOPT_(sort), SimTK_SELECT_C select, SimTK_FOPT_(sense), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), int *sdim, SimTK_C_ *w, SimTK_C_ *vs, SimTK_FDIM_(ldvs), SimTK_S_OUTPUT_(rconde), SimTK_S_OUTPUT_(rcondv), SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, int *bwork, SimTK_INFO_, SimTK_FLEN_(jobvs), SimTK_FLEN_(sort), SimTK_FLEN_(sense));
+
+extern void cgeev_(SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *w, SimTK_C_ *vl, SimTK_FDIM_(ldvl), SimTK_C_ *vr, SimTK_FDIM_(ldvr), SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, SimTK_INFO_, SimTK_FLEN_(jobvl), SimTK_FLEN_(jobvr));
+
+extern void cgeevx_(SimTK_FOPT_(balanc), SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), SimTK_FOPT_(sense), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *w, SimTK_C_ *vl, SimTK_FDIM_(ldvl), SimTK_C_ *vr, SimTK_FDIM_(ldvr), SimTK_I_OUTPUT_(ilo), SimTK_I_OUTPUT_(ihi), SimTK_S_OUTPUT_(scale), float *abnrm, SimTK_S_OUTPUT_(rconde), SimTK_S_OUTPUT_(rcondv), SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, SimTK_INFO_, SimTK_FLEN_(balanc), SimTK_FLEN_(jobvl), SimTK_FLEN_(jobvr), SimTK_FLE [...]
+
+extern void cgegs_(SimTK_FOPT_(jobvsl), SimTK_FOPT_(jobvsr), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *alpha, SimTK_C_ *beta, SimTK_C_ *vsl, SimTK_FDIM_(ldvsl), SimTK_C_ *vsr, SimTK_FDIM_(ldvsr), SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, SimTK_INFO_, SimTK_FLEN_(jobvsl), SimTK_FLEN_(jobvsr));
+
+extern void cgegv_(SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *alpha, SimTK_C_ *beta, SimTK_C_ *vl, SimTK_FDIM_(ldvl), SimTK_C_ *vr, SimTK_FDIM_(ldvr), SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, SimTK_INFO_, SimTK_FLEN_(jobvsl), SimTK_FLEN_(jobvsr));
+
+
+extern void cgehd2_(SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_FDIM_(ihi), SimTK_C_ *a, SimTK_I_INPUT_(lda), SimTK_C_ *tau, SimTK_C_ *work, SimTK_INFO_);
+
+extern void cgehrd_(SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *tau, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void cgelq2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *tau, SimTK_C_ *work, SimTK_INFO_);
+
+extern void cgelqf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *tau, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void cgels_(SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(trans));
+extern void cgelss_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), float *s, SimTK_S_INPUT_(rcond), SimTK_I_OUTPUT_(rank), SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, SimTK_INFO_);
+
+/* gelsx is deprecated; use gelsy instead */
+extern void cgelsx_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), int *jpvt, SimTK_S_INPUT_(rcond), SimTK_I_OUTPUT_(rank), SimTK_C_ *work, float *rwork, SimTK_INFO_);
+extern void cgelsy_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), int *jpvt, SimTK_S_INPUT_(rcond), SimTK_I_OUTPUT_(rank), SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, SimTK_INFO_);
+
+extern void cgeql2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *tau, SimTK_C_ *work, SimTK_INFO_);
+
+extern void cgeqlf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *tau, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void cgeqp3_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), int *jpvt, SimTK_C_ *tau, SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, SimTK_INFO_);
+
+extern void cgeqpf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), int *jpvt, SimTK_C_ *tau, SimTK_C_ *work, float *rwork, SimTK_INFO_);
+
+extern void cgeqr2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *tau, SimTK_C_ *work, SimTK_INFO_);
+
+extern void cgeqrf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *tau, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void cgerfs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *af, SimTK_FDIM_(ldaf), const int *ipiv, const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void cgerq2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *tau, SimTK_C_ *work, SimTK_INFO_);
+
+extern void cgerqf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *tau, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void cgesc2_(SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *rhs, int *ipiv, int *jpiv, SimTK_S_OUTPUT_(scale));
+
+extern void cgesv_(SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_C_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_);
+
+extern void cgesvx_(SimTK_FOPT_(fact), SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *af, SimTK_FDIM_(ldaf), int *ipiv, SimTK_CHAR_OUTPUT_(equed), float *r__, float *c__, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(rcond), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(trans), SimTK_FLEN_(equed));
+
+
+extern void cgetc2_(SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), int *ipiv, int *jpiv, SimTK_INFO_);
+
+extern void cgetf2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_INFO_);
+
+extern void cgetrf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_INFO_);
+
+extern void cgetri_(SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), const int *ipiv, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void cgetrs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *a, SimTK_FDIM_(lda), const int *ipiv, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void cggbak_(SimTK_FOPT_(job), SimTK_FOPT_(side), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), const float *lscale, const float *rscale, SimTK_FDIM_(m), SimTK_C_ *v, SimTK_FDIM_(ldv), SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(side));
+
+extern void cggbal_(SimTK_FOPT_(job), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_I_OUTPUT_(ilo), SimTK_I_OUTPUT_(ihi), float *lscale, float *rscale, float *work, SimTK_INFO_, SimTK_FLEN_(job));
+
+extern void cgges_(SimTK_FOPT_(jobvsl), SimTK_FOPT_(jobvsr), SimTK_FOPT_(sort), SimTK_SELECT_2C selctg, SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), int *sdim, SimTK_C_ *alpha, SimTK_C_ *beta, SimTK_C_ *vsl, SimTK_FDIM_(ldvsl), SimTK_C_ *vsr, SimTK_FDIM_(ldvsr), SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, int *bwork, SimTK_INFO_, SimTK_FLEN_(jobvsl), SimTK_FLEN_(jobvsr), SimTK_FLEN_(sort));
+
+extern void cggesx_(SimTK_FOPT_(jobvsl), SimTK_FOPT_(jobvsr), SimTK_FOPT_(sort), SimTK_SELECT_2C selctg, SimTK_FOPT_(sense), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_I_OUTPUT_(sdim), SimTK_C_ *alpha, SimTK_C_ *beta, SimTK_C_ *vsl, SimTK_FDIM_(ldvsl), SimTK_C_ *vsr, SimTK_FDIM_(ldvsr), SimTK_S_OUTPUT_(rconde), SimTK_S_OUTPUT_(rcondv), SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, int *iwork, SimTK_FDIM_(liwork), int *bwork, SimTK_INFO_, SimTK [...]
+
+extern void cggev_(SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *alpha, SimTK_C_ *beta, SimTK_C_ *vl, SimTK_FDIM_(ldvl), SimTK_C_ *vr, SimTK_FDIM_(ldvr), SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, SimTK_INFO_, SimTK_FLEN_(jobvl), SimTK_FLEN_(jobvr));
+
+extern void cggevx_(SimTK_FOPT_(balanc), SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), SimTK_FOPT_(sense), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *alpha, SimTK_C_ *beta, SimTK_C_ *vl, SimTK_FDIM_(ldvl), SimTK_C_ *vr, SimTK_FDIM_(ldvr), SimTK_I_OUTPUT_(ilo), SimTK_I_OUTPUT_(ihi), float *lscale, float *rscale, SimTK_S_OUTPUT_(abnrm), SimTK_S_OUTPUT_(bbnrm), SimTK_S_OUTPUT_(rconde), SimTK_S_OUTPUT_(rcondv), SimTK_C_ *work, SimTK_FDIM_(lwork), float [...]
+
+extern void cggglm_(SimTK_FDIM_(n), SimTK_FDIM_(m), SimTK_FDIM_(p), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *d__, SimTK_C_ *x, SimTK_C_ *y, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void cgghrd_(SimTK_FOPT_(compq), SimTK_FOPT_(compz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *q, SimTK_FDIM_(ldq), SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_INFO_, SimTK_FLEN_(compq), SimTK_FLEN_(compz));
+
+extern void cgglse_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(p), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *c__, SimTK_C_ *d__, SimTK_C_ *x, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void cggqrf_(SimTK_FDIM_(n), SimTK_FDIM_(m), SimTK_FDIM_(p), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *taua, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *taub, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void cggrqf_(SimTK_FDIM_(m), SimTK_FDIM_(p), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *taua, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *taub, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void cggsvd_(SimTK_FOPT_(jobu), SimTK_FOPT_(jobv), SimTK_FOPT_(jobq), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(p), SimTK_I_OUTPUT_(k), SimTK_I_OUTPUT_(l), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), float *alpha, float *beta, SimTK_C_ *u, SimTK_FDIM_(ldu), SimTK_C_ *v, SimTK_FDIM_(ldv), SimTK_C_ *q, SimTK_FDIM_(ldq), SimTK_C_ *work, float *rwork, int *iwork, SimTK_INFO_, SimTK_FLEN_(jobu), SimTK_FLEN_(jobv), SimTK_FLEN_(jobq));
+
+extern void cggsvp_(SimTK_FOPT_(jobu), SimTK_FOPT_(jobv), SimTK_FOPT_(jobq), SimTK_FDIM_(m), SimTK_FDIM_(p), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_S_INPUT_(tola), SimTK_S_INPUT_(tolb), SimTK_I_OUTPUT_(k), SimTK_I_OUTPUT_(l), SimTK_C_ *u, SimTK_FDIM_(ldu), SimTK_C_ *v, SimTK_FDIM_(ldv), SimTK_C_ *q, SimTK_FDIM_(ldq), int *iwork, float *rwork, SimTK_C_ *tau, SimTK_C_ *work, SimTK_INFO_, SimTK_FLEN_(jobu), SimTK_FLEN_(jobv), SimTK_FLEN_(jobq));
+
+extern void cgtcon_(SimTK_FOPT_(norm), SimTK_FDIM_(n), SimTK_C_ *dl, const SimTK_C_ *d__, const SimTK_C_ *du, const SimTK_C_ *du2, const int *ipiv, SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), SimTK_C_ *work, SimTK_INFO_, SimTK_FLEN_(norm));
+
+extern void cgtrfs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *dl, const SimTK_C_ *d__, const SimTK_C_ *du, const SimTK_C_ *dlf, const SimTK_C_ *df, const SimTK_C_ *duf, const SimTK_C_ *du2, const int *ipiv, const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void cgtsv_(SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_C_ *dl, SimTK_C_ *d__, SimTK_C_ *du, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_);
+
+extern void cgtsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *dl, const SimTK_C_ *d__, const SimTK_C_ *du, SimTK_C_ *dlf, SimTK_C_ *df, SimTK_C_ *duf, SimTK_C_ *du2, int *ipiv, const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(rcond), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(trans));
+
+extern void cgttrf_(SimTK_FDIM_(n), SimTK_C_ *dl, SimTK_C_ *d__, SimTK_C_ *du, SimTK_C_ *du2, int *ipiv, SimTK_INFO_);
+
+extern void cgttrs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *dl, const SimTK_C_ *d__, const SimTK_C_ *du, const SimTK_C_ *du2, const int *ipiv, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void cgtts2_(int *itrans, SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *dl, const SimTK_C_ *d__, const SimTK_C_ *du, const SimTK_C_ *du2, const int *ipiv, SimTK_C_ *b, SimTK_FDIM_(ldb));
+
+extern void chbev_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_C_ *ab, SimTK_FDIM_(ldab), float *w, SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void chbevd_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_C_ *ab, SimTK_FDIM_(ldab), float *w, SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, SimTK_FDIM_(lrwork), int *iwork, SimTK_FDIM_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void chbevx_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_C_ *ab, SimTK_FDIM_(ldab), SimTK_C_ *q, SimTK_FDIM_(ldq), SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_S_INPUT_(abstol), SimTK_I_OUTPUT_(m), float *w, SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_C_ *work, float *rwork, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void chbgst_(SimTK_FOPT_(vect), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(ka), SimTK_FDIM_(kb), SimTK_C_ *ab, SimTK_FDIM_(ldab), const SimTK_C_ *bb, SimTK_FDIM_(ldbb), SimTK_C_ *x, SimTK_FDIM_(ldx), SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(vect), SimTK_FLEN_(uplo));
+
+extern void chbgv_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(ka), SimTK_FDIM_(kb), SimTK_C_ *ab, SimTK_FDIM_(ldab), SimTK_C_ *bb, SimTK_FDIM_(ldbb), float *w, SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void chbgvx_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(ka), SimTK_FDIM_(kb), SimTK_C_ *ab, SimTK_FDIM_(ldab), SimTK_C_ *bb, SimTK_FDIM_(ldbb), SimTK_C_ *q, SimTK_FDIM_(ldq), SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_S_INPUT_(abstol), SimTK_I_OUTPUT_(m), float *w, SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_C_ *work, float *rwork, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(ra [...]
+
+extern void chbtrd_(SimTK_FOPT_(vect), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_C_ *ab, SimTK_FDIM_(ldab), float *d__, float *e, SimTK_C_ *q, SimTK_FDIM_(ldq), SimTK_C_ *work, SimTK_INFO_, SimTK_FLEN_(vect), SimTK_FLEN_(uplo));
+
+extern void checon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), const int *ipiv, SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), SimTK_C_ *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cheev_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), float *w, SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void cheevd_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), float *w, SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, SimTK_FDIM_(lrwork), int *iwork, SimTK_FDIM_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void cheevr_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_S_INPUT_(abstol), SimTK_I_OUTPUT_(m), float *w, SimTK_C_ *z__, SimTK_FDIM_(ldz), int *isuppz, SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, SimTK_FDIM_(lrwork), int *iwork, SimTK_FDIM_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void cheevx_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_S_INPUT_(abstol), SimTK_I_OUTPUT_(m), float *w, SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void chegs2_(SimTK_I_INPUT_(itype), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void chegst_(SimTK_I_INPUT_(itype), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void chegv_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), float *w, SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void chegvd_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), float *w, SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, SimTK_FDIM_(lrwork), int *iwork, SimTK_FDIM_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void chegvx_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_S_INPUT_(abstol), SimTK_I_OUTPUT_(m), float *w, SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void cherfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *af, SimTK_FDIM_(ldaf), const int *ipiv, const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void chesv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_C_ *a, SimTK_FDIM_(lda), const int *ipiv, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void chesvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *af, SimTK_FDIM_(ldaf), int *ipiv, const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(rcond), float *ferr, float *berr, SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo));
+
+extern void chetf2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void chetrd_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), float *d__, float *e, SimTK_C_ *tau, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void chetrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void chetri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), const int *ipiv, SimTK_C_ *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void chetrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *a, SimTK_FDIM_(lda), const int *ipiv, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void chgeqz_(SimTK_FOPT_(job), SimTK_FOPT_(compq), SimTK_FOPT_(compz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *alpha, SimTK_C_ *beta, SimTK_C_ *q, SimTK_FDIM_(ldq), SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(comq), SimTK_FLEN_(compz));
+
+extern void chpcon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_C_ *ap, const int *ipiv, SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), SimTK_C_ *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void chpev_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *ap, float *w, SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void chpevd_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *ap, float *w, SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, SimTK_FDIM_(lrwork), int *iwork, SimTK_FDIM_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void chpevx_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *ap, SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_S_INPUT_(abstol), SimTK_I_OUTPUT_(m), float *w, SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_C_ *work, float *rwork, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void chpgst_(SimTK_I_INPUT_(itype), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *ap, const SimTK_C_ *bp, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void chpgv_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *ap, SimTK_C_ *bp, float *w, SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void chpgvd_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *ap, SimTK_C_ *bp, float *w, SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, SimTK_FDIM_(lrwork), int *iwork, SimTK_FDIM_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void chpgvx_( SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *ap, SimTK_C_ *bp, SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_S_INPUT_(abstol), SimTK_I_OUTPUT_(m), float *w, SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_C_ *work, float *rwork, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void chprfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *ap, const SimTK_C_ *afp, const int *ipiv, const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void chpsv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_C_ *ap, int *ipiv, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void chpsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *ap, const SimTK_C_ *afp, const int *ipiv, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(rcond), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo));
+
+extern void chptrd_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *ap, float *d__, float *e, SimTK_C_ *tau, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void chptrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *ap, int *ipiv, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void chptri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *ap, const int *ipiv, SimTK_C_ *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void chptrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *ap, const int *ipiv, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void chsein_(SimTK_FOPT_(side), SimTK_FOPT_(eigsrc), SimTK_FOPT_(initv), const int *select, SimTK_FDIM_(n), const SimTK_C_ *h__, SimTK_FDIM_(ldh), SimTK_C_ *w, SimTK_C_ *vl, SimTK_FDIM_(ldvl), SimTK_C_ *vr, SimTK_FDIM_(ldvr), SimTK_FDIM_(mm), SimTK_I_OUTPUT_(m), SimTK_C_ *work, float *rwork, int *ifaill, int *ifailr, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(eigsrc), SimTK_FLEN_(initv));
+
+extern void chseqr_(SimTK_FOPT_(job), SimTK_FOPT_(compz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_C_ *h__, SimTK_FDIM_(ldh), SimTK_C_ *w, SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(compz));
+
+extern void clabrd_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nb), SimTK_C_ *a, SimTK_FDIM_(lda), float *d__, float *e, SimTK_C_ *tauq, SimTK_C_ *taup, SimTK_C_ *x, SimTK_FDIM_(ldx), SimTK_C_ *y, SimTK_FDIM_(ldy));
+
+extern void clacgv_(SimTK_FDIM_(n), SimTK_C_ *x, SimTK_FINC_(x));
+
+extern void clacon_(SimTK_FDIM_(n), SimTK_C_ *v, SimTK_C_ *x, SimTK_S_OUTPUT_(est), SimTK_I_OUTPUT_(kase) );
+
+extern void clacn2_( SimTK_FDIM_(n), SimTK_C_ *v, SimTK_C_ *x, SimTK_S_OUTPUT_(est), SimTK_I_OUTPUT_(kase), int *isave );
+
+extern void clacp2_(SimTK_FOPT_(uplo), SimTK_FDIM_(m), SimTK_FDIM_(n), const float *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_FLEN_(uplo));
+
+extern void clacpy_(SimTK_FOPT_(uplo), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_FLEN_(uplo));
+
+extern void clacrm_(SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), const float *b, SimTK_FDIM_(ldb), const SimTK_C_ *c__, SimTK_FDIM_(ldc), float *rwork);
+
+extern void clacrt_(SimTK_FDIM_(n), SimTK_C_ *cx, SimTK_FINC_(x), SimTK_C_ *cy, SimTK_FINC_(y), const SimTK_C_ *c__, const SimTK_C_ *s);
+
+extern void claed0_(SimTK_I_INPUT_(qsiz), SimTK_FDIM_(n), float *d__, float *e, SimTK_C_ *q, SimTK_FDIM_(ldq), SimTK_C_ *qstore, SimTK_FDIM_(ldqs), float *rwork, int *iwork, SimTK_INFO_);
+
+extern void claed7_(SimTK_FDIM_(n), SimTK_I_INPUT_(cutpnt), SimTK_FDIM_(qsiz), SimTK_I_INPUT_(tlvls), SimTK_I_INPUT_(curlvl), SimTK_I_INPUT_(curpbm), float *d__, SimTK_C_ *q, SimTK_FDIM_(ldq), SimTK_S_INPUT_(rho), int *indxq, float *qstore, int *qptr, const int *prmptr, const int *perm, const int *givptr, const int *givcol, const float *givnum, SimTK_C_ *work, float *rwork, int *iwork, SimTK_INFO_);
+
+extern void claed8_(SimTK_I_OUTPUT_(k), SimTK_FDIM_(n), SimTK_FDIM_(qsiz), SimTK_C_ *q, SimTK_FDIM_(ldq), float *d__, SimTK_S_INPUT_(rho), SimTK_I_INPUT_(cutpnt), float *z__, float *dlamda, SimTK_C_ *q2, SimTK_FDIM_(ldq2), float *w, int *indxp, int *indx, const int *indxq, int *perm, int *givptr, int *givcol, float *givnum, SimTK_INFO_);
+
+extern void claein_(SimTK_I_INPUT_(rightv), SimTK_I_INPUT_(noinit), SimTK_FDIM_(n), const SimTK_C_ *h__, SimTK_FDIM_(ldh), SimTK_C_INPUT_(w), SimTK_C_ *v, SimTK_C_ *b, SimTK_FDIM_(ldb), float *rwork, SimTK_S_INPUT_(eps3), SimTK_S_INPUT_(smlnum), SimTK_INFO_);
+
+extern void claesy_(SimTK_C_INPUT_(a), SimTK_C_INPUT_(b), SimTK_C_INPUT_(c__), SimTK_C_OUTPUT_(rt1), SimTK_C_OUTPUT_(rt2), SimTK_C_OUTPUT_(evscal), SimTK_C_OUTPUT_(cs1), SimTK_C_OUTPUT_(sn1) );
+
+extern void claev2_(SimTK_C_INPUT_(a), SimTK_C_INPUT_(b), SimTK_C_INPUT_(c__), SimTK_S_OUTPUT_(rt1), SimTK_S_OUTPUT_(rt2), SimTK_S_OUTPUT_(cs1), SimTK_C_OUTPUT_(sn1) );
+
+extern void clags2_(SimTK_I_INPUT_(upper), SimTK_S_INPUT_(a1), SimTK_C_INPUT_(a2), SimTK_S_INPUT_(a3), SimTK_S_INPUT_(b1), SimTK_C_INPUT_(b2), SimTK_S_INPUT_(b3), SimTK_S_OUTPUT_(csu), SimTK_C_OUTPUT_(snu), SimTK_S_OUTPUT_(csv), SimTK_C_OUTPUT_(snv), SimTK_S_OUTPUT_(csq), SimTK_C_OUTPUT_(snq) );
+
+extern void clagtm_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_S_INPUT_(alpha), const SimTK_C_ *dl, const SimTK_C_ *d__, const SimTK_C_ *du, const SimTK_C_ *x, SimTK_FDIM_(ldx), SimTK_S_INPUT_(beta), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_FLEN_(trans));
+
+extern void clag2z_( SimTK_I_INPUT_(m), SimTK_I_INPUT_(n), SimTK_C_ *sa, SimTK_FDIM_(ldsa), const SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_INFO_);
+
+extern void clahef_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nb), SimTK_FDIM_(kb), SimTK_C_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_C_ *w, SimTK_FDIM_(ldw), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void clahqr_(SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_C_ *h__, SimTK_FDIM_(ldh), SimTK_C_ *w, SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_INFO_);
+
+extern void clahrd_(SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(nb), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *tau, SimTK_C_ *t, SimTK_FDIM_(ldt), SimTK_C_ *y, SimTK_FDIM_(ldy));
+
+extern void clahr2_(SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(nb), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *tau, SimTK_C_ *t, SimTK_FDIM_(ldt), SimTK_C_ *y, SimTK_FDIM_(ldy));
+
+extern void claic1_(SimTK_I_INPUT_(job), SimTK_FDIM_(j), const SimTK_C_ *x, SimTK_S_INPUT_(sest), const SimTK_C_ *w, SimTK_C_INPUT_(gamma), SimTK_S_OUTPUT_(sestpr), SimTK_C_OUTPUT_(s), SimTK_C_OUTPUT_(c__) );
+
+extern void clals0_(SimTK_I_INPUT_(icompq), SimTK_I_INPUT_(nl), SimTK_I_INPUT_(nr), SimTK_I_INPUT_(sqre), SimTK_FDIM_(nrhs), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *bx, SimTK_FDIM_(ldbx), int *perm, int *givptr, const int *givcol, SimTK_FDIM_(ldgcol), float *givnum, SimTK_FDIM_(ldgnum), float *poles, float *difl, float *difr, float *z__, SimTK_FDIM_(k), float *c__, float *s, float *rwork, SimTK_INFO_);
+
+extern void clalsa_(SimTK_I_INPUT_(icompq), SimTK_I_INPUT_(smlsiz), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *bx, SimTK_FDIM_(ldbx), float *u, SimTK_FDIM_(ldu), float *vt, SimTK_FDIM_(k), float *difl, float *difr, float *z__, float *poles, int *givptr, const int *givcol, SimTK_FDIM_(ldgcol), int *perm, float *givnum, float *c__, float *s, float *rwork, int *iwork, SimTK_INFO_);
+
+extern void clapll_(SimTK_FDIM_(n), SimTK_C_ *x, SimTK_FINC_(x), SimTK_C_ *y, SimTK_FINC_(y), SimTK_S_OUTPUT_(ssmin));
+
+extern void clapmt_(SimTK_I_INPUT_(forwrd), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_ *x, SimTK_FDIM_(ldx), SimTK_FDIM_(k));
+
+extern void claqgb_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_C_ *ab, SimTK_FDIM_(ldab), float *r__, float *c__, SimTK_S_OUTPUT_(rowcnd), SimTK_S_OUTPUT_(colcnd), SimTK_S_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(equed));
+
+extern void claqge_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), const float *r__, const float *c__, SimTK_S_INPUT_(rowcnd), SimTK_S_INPUT_(colcnd), SimTK_S_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(equed));
+
+extern void claqhb_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_C_ *ab, SimTK_FDIM_(ldab), float *s, SimTK_S_INPUT_(scond), SimTK_S_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void claqhe_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), const float *s, SimTK_S_INPUT_(scond), SimTK_S_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void claqhp_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *ap, const float *s, SimTK_S_INPUT_(scond), SimTK_S_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void claqp2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_I_INPUT_(offset), SimTK_C_ *a, SimTK_FDIM_(lda), int *jpvt, SimTK_C_ *tau, float *vn1, float *vn2, SimTK_C_ *work);
+
+extern void claqps_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_I_INPUT_(offset), SimTK_I_INPUT_(nb), SimTK_I_OUTPUT_(kb), SimTK_C_ *a, SimTK_FDIM_(lda), int *jpvt, SimTK_C_ *tau, float *vn1, float *vn2, SimTK_C_ *auxv, SimTK_C_ *f, SimTK_FDIM_(ldf));
+
+extern void claqr0_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_C_ *h, SimTK_FDIM_(ldh), SimTK_C_ *w, SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), SimTK_C_ *z, SimTK_FDIM_(ldz), SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_ );
+
+extern void claqr1_(SimTK_FDIM_(n), const SimTK_C_ *h, SimTK_FDIM_(ldh), SimTK_C_INPUT_(s1), SimTK_C_INPUT_(s2), SimTK_C_ *v );
+
+extern void claqr2_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ktop), SimTK_I_INPUT_(kbot), SimTK_I_INPUT_(nw), SimTK_C_ *h, SimTK_FDIM_(ldh), SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), SimTK_C_ *z, SimTK_FDIM_(ldz), SimTK_I_OUTPUT_(ns), SimTK_I_OUTPUT_(nd), SimTK_C_ *sh, SimTK_C_ *v, SimTK_FDIM_(ldv), SimTK_I_INPUT_(nh), SimTK_C_ *t, SimTK_FDIM_(ldt), SimTK_I_INPUT_(nv), SimTK_C_ *wv, SimTK_I_INPUT_(ldwv), SimTK_C_ *work, SimTK_FDIM_(lwork) );
+
+extern void claqr3_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ktop), SimTK_I_INPUT_(kbot), SimTK_I_INPUT_(nw), SimTK_C_ *h, SimTK_FDIM_(ldh), SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), SimTK_C_ *z, SimTK_FDIM_(ldz), SimTK_I_OUTPUT_(ns), SimTK_I_OUTPUT_(nd), SimTK_C_ *sh, SimTK_C_ *v, SimTK_FDIM_(ldv), SimTK_I_INPUT_(nh), SimTK_C_ *t, SimTK_FDIM_(ldt), SimTK_I_INPUT_(nv), SimTK_C_ *wv, SimTK_I_INPUT_(ldwv), SimTK_C_ *work, SimTK_FDIM_(lwork) );
+
+extern void claqr4_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_C_ *h, SimTK_FDIM_(ldh), SimTK_C_ *w, SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), SimTK_C_ *z, SimTK_FDIM_(ldz), SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_ );
+
+extern void claqr5_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_I_INPUT_(kacc22), SimTK_FDIM_(n), SimTK_I_INPUT_(ktop), SimTK_I_INPUT_(kbot), SimTK_I_INPUT_(nshifts), const SimTK_C_ *s, SimTK_C_ *h, SimTK_FDIM_(ldh), SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), SimTK_C_ *z, SimTK_FDIM_(ldz), SimTK_C_ *v, SimTK_FDIM_(ldv), SimTK_C_ *u, SimTK_FDIM_(ldu), SimTK_I_INPUT_(nh), SimTK_C_ *wh, SimTK_FDIM_(ldwh), SimTK_I_INPUT_(nv), SimTK_C_ *wv, SimTK_FDIM_(ldwv) );
+
+extern void claqsb_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_C_ *ab, SimTK_FDIM_(ldab), float *s, SimTK_S_INPUT_(scond), SimTK_S_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void claqsp_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *ap, const float *s, SimTK_S_INPUT_(scond), SimTK_S_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void claqsy_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), const float *s, SimTK_S_INPUT_(scond), SimTK_S_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void clar1v_(SimTK_FDIM_(n), SimTK_I_INPUT_(b1), SimTK_I_INPUT_(bn), SimTK_S_INPUT_(lambda), const float *d__, const float *l, const float *ld, const float *lld, SimTK_S_INPUT_(pivmin), SimTK_S_INPUT_(gaptol), SimTK_C_ *z__, SimTK_I_INPUT_(wantnc), SimTK_I_OUTPUT_(negcnt), SimTK_S_OUTPUT_(ztz), SimTK_S_OUTPUT_(mingma), SimTK_I_OUTPUT_(r__), int *isuppz, SimTK_S_OUTPUT_(nrminv), SimTK_S_OUTPUT_(resid), SimTK_S_OUTPUT_(rqcorr), float *work);
+
+extern void clar2v_(SimTK_FDIM_(n), SimTK_C_ *x, SimTK_C_ *y, SimTK_C_ *z__, SimTK_FINC_(x), const float *c__, const SimTK_C_ *s, SimTK_FINC_(c));
+
+extern void clarcm_(SimTK_FDIM_(m), SimTK_FDIM_(n), const float *a, SimTK_FDIM_(lda), const SimTK_C_ *b, SimTK_FDIM_(ldb), const SimTK_C_ *c__, SimTK_FDIM_(ldc), float *rwork);
+
+extern void clarf_(SimTK_FOPT_(side), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_C_ *v, SimTK_FINC_(v), SimTK_C_INPUT_(tau), SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_FLEN_(side));
+
+extern void clarfb_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FOPT_(direct), SimTK_FOPT_(storev), SimTK_I_OUTPUT_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_C_ *v, SimTK_FDIM_(ldv), const SimTK_C_ *t, SimTK_FDIM_(ldt), SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_FDIM_(ldwork), SimTK_FLEN_(side), SimTK_FLEN_(trans), SimTK_FLEN_(direct), SimTK_FLEN_(storev));
+
+extern void clarfg_(SimTK_FDIM_(n), SimTK_C_OUTPUT_(alpha), SimTK_C_ *x, int *incx, SimTK_C_OUTPUT_(tau) );
+
+extern void clarft_(SimTK_FOPT_(direct), SimTK_FOPT_(storev), SimTK_FDIM_(n), SimTK_I_INPUT_(k), SimTK_C_ *v, SimTK_FDIM_(ldv), const SimTK_C_ *tau, SimTK_C_ *t, SimTK_FDIM_(ldt), SimTK_FLEN_(direct), SimTK_FLEN_(storev));
+
+extern void clarfx_(SimTK_FOPT_(side), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_C_ *v, const SimTK_C_ *tau, SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_FLEN_(side));
+
+extern void clargv_(SimTK_FDIM_(n), SimTK_C_ *x, SimTK_FINC_(x), SimTK_C_ *y, SimTK_FINC_(y), float *c__, SimTK_FINC_(c));
+
+extern void clarnv_(SimTK_I_INPUT_(idist), int *iseed, SimTK_FDIM_(n), SimTK_C_ *x);
+
+extern void clarrv_(SimTK_FDIM_(n), SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), float *d__, float *l, SimTK_S_INPUT_(pivmin), const int *isplit, SimTK_FDIM_(m), SimTK_I_INPUT_(dol), SimTK_I_INPUT_(dou), SimTK_S_INPUT_(minrgp), SimTK_S_INPUT_(rtol1), SimTK_S_INPUT_(rtol2), float *w, float *werr, float *wgap, const int *iblock, const int *indexw, const float *gers, SimTK_C_ *z__, SimTK_FDIM_(ldz), int *isuppz, float *work, int *iwork, SimTK_INFO_);
+
+extern void clartg_(const SimTK_C_ *f, const SimTK_C_ *g, float *cs, SimTK_C_ *sn, SimTK_C_ *r__);
+
+extern void clartv_(SimTK_FDIM_(n), SimTK_C_ *x, SimTK_FINC_(x), SimTK_C_ *y, SimTK_FINC_(y), const float *c__, const SimTK_C_ *s, SimTK_FINC_(c));
+
+extern void clarz_(SimTK_FOPT_(side), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_I_INPUT_(L), const SimTK_C_ *v, SimTK_FINC_(v), SimTK_C_INPUT_(tau), SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_FLEN_(side));
+
+extern void clarzb_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FOPT_(direct), SimTK_FOPT_(storev), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(l), const SimTK_C_ *v, SimTK_FDIM_(ldv), const SimTK_C_ *t, SimTK_FDIM_(ldt), SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_FDIM_(ldwork), SimTK_FLEN_(side), SimTK_FLEN_(trans), SimTK_FLEN_(direct), SimTK_FLEN_(storev));
+
+extern void clarzt_(SimTK_FOPT_(direct), SimTK_FOPT_(storev), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_C_ *v, SimTK_FDIM_(ldv), const SimTK_C_ *tau, SimTK_C_ *t, SimTK_FDIM_(ldt), SimTK_FLEN_(direct), SimTK_FLEN_(storev));
+
+extern void clascl_(SimTK_FOPT_(type__), SimTK_FDIM_(kl), SimTK_FDIM_(ku), const float *cfrom, const float *cto, SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(type));
+
+extern void claset_(SimTK_FOPT_(uplo), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_INPUT_(alpha), SimTK_C_INPUT_(beta), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_FLEN_(uplo));
+
+extern void clasr_(SimTK_FOPT_(side), SimTK_FOPT_(pivot), SimTK_FOPT_(direct), SimTK_FDIM_(m), SimTK_FDIM_(n), const float *c__, const float *s, SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_FLEN_(side), SimTK_FLEN_(pivot), SimTK_FLEN_(direct));
+
+extern void classq_(SimTK_FDIM_(n), const SimTK_C_ *x, SimTK_FINC_(x), SimTK_S_OUTPUT_(scale), float *sumsq);
+
+extern void claswp_(SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_I_OUTPUT_(k1), SimTK_I_OUTPUT_(k2), const int *ipiv, SimTK_FINC_(x));
+
+extern void clasyf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_I_INPUT_(nb), SimTK_I_INPUT_(kb), SimTK_C_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_C_ *w, SimTK_FDIM_(ldw), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+
+
+
+extern void clatbs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FOPT_(normin), SimTK_FDIM_(n), SimTK_I_INPUT_(kd), const SimTK_C_ *ab, SimTK_FDIM_(ldab), SimTK_C_ *x, SimTK_S_OUTPUT_(scale), float *cnorm, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag), SimTK_FLEN_(normin));
+
+extern void clatdf_(SimTK_I_INPUT_(ijob), SimTK_FDIM_(n), const SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_C_ *rhs, SimTK_S_OUTPUT_(rdsum), SimTK_S_OUTPUT_(rdscal), const int *ipiv, const int *jpiv);
+
+extern void clatps_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FOPT_(normin), SimTK_FDIM_(n), const SimTK_C_ *ap, SimTK_C_ *x, SimTK_S_OUTPUT_(scale), float *cnorm, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag), SimTK_FLEN_(normin));
+
+extern void clatrd_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nb), SimTK_C_ *a, SimTK_FDIM_(lda), float *e, SimTK_C_ *tau, SimTK_C_ *w, SimTK_FDIM_(ldw), SimTK_FLEN_(uplo));
+
+extern void clatrs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FOPT_(normin), SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *x, SimTK_S_OUTPUT_(scale), float *cnorm, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag), SimTK_FLEN_(normin));
+
+extern void clatrz_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(l), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *tau, SimTK_C_ *work);
+
+extern void clatzm_(SimTK_FOPT_(side), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_C_ *v, SimTK_FINC_(v), SimTK_C_INPUT_(tau), SimTK_C_ *c1, SimTK_C_ *c2, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_FLEN_(side));
+
+extern void clauu2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void clauum_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cpbcon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), const SimTK_C_ *ab, SimTK_FDIM_(ldab), SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cpbequ_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_I_INPUT_(kd), const SimTK_C_ *ab, SimTK_FDIM_(ldab), SimTK_S_OUTPUT_(s), SimTK_S_OUTPUT_(scond), SimTK_S_OUTPUT_(amax), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cpbrfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), const SimTK_C_ *ab, SimTK_FDIM_(ldab), const SimTK_C_ *afb, SimTK_FDIM_(ldafb), const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cpbstf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_C_ *ab, SimTK_FDIM_(ldab), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cpbsv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), SimTK_C_ *ab, SimTK_FDIM_(ldab), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+
+
+extern void cpbsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), SimTK_C_ *ab, SimTK_FDIM_(ldab), SimTK_C_ *afb, SimTK_FDIM_(ldafb), SimTK_CHAR_OUTPUT_(equed), float *s, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), SimTK_S_INPUT_(rcond), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void cpbtf2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_C_ *ab, SimTK_FDIM_(ldab), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cpbtrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_C_ *ab, SimTK_FDIM_(ldab), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cpbtrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), const SimTK_C_ *ab, SimTK_FDIM_(ldab), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cpocon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cpoequ_(SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), float *s, SimTK_S_OUTPUT_(scond), SimTK_S_OUTPUT_(amax), SimTK_INFO_);
+
+extern void cporfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *af, SimTK_FDIM_(ldaf), const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cposv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cposvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *af, SimTK_FDIM_(ldaf), SimTK_CHAR_OUTPUT_(equed), float *s, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(rcond), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void cpotf2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cpotrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cpotri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cpotrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cppcon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_C_ *ap, SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cppequ_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_C_ *ap, float *s, SimTK_S_OUTPUT_(scond), SimTK_S_OUTPUT_(amax), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cpprfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *ap, const SimTK_C_ *afp, const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cppsv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_C_ *ap, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cppsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_C_ *ap, SimTK_C_ *afp, SimTK_CHAR_OUTPUT_(equed), float *s, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(rcond), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void cpptrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *ap, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cpptri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *ap, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cpptrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *ap, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cptcon_(SimTK_FDIM_(n), const float *d__, const SimTK_C_ *e, SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), float *rwork, SimTK_INFO_);
+
+extern void cptrfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *d__, const SimTK_C_ *e, const float *df, const SimTK_C_ *ef, const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cptsv_(SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *d__, SimTK_C_ *e, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_);
+
+extern void cptsvx_(SimTK_FOPT_(fact), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *d__, const SimTK_C_ *e, float *df, SimTK_C_ *ef, const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(rcond), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(fact));
+
+extern void cpttrf_(SimTK_FDIM_(n), float *d__, SimTK_C_ *e, SimTK_INFO_);
+
+extern void cpttrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *d__, const SimTK_C_ *e, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cptts2_(SimTK_I_INPUT_(iuplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *d__, const SimTK_C_ *e, SimTK_C_ *b, SimTK_FDIM_(ldb));
+
+extern void crot_(SimTK_FDIM_(n), SimTK_C_ *cx, SimTK_FINC_(x), SimTK_C_ *cy, SimTK_FINC_(y), const float *c__, const SimTK_C_ *s);
+
+extern void cspcon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_C_ *ap, const int *ipiv, SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), SimTK_C_ *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cspmv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_INPUT_(alpha), const SimTK_C_ *ap, const SimTK_C_ *x, SimTK_FINC_(x), SimTK_C_INPUT_(beta), SimTK_C_ *y, int *incy, SimTK_FLEN_(uplo));
+
+extern void cspr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_INPUT_(alpha), const SimTK_C_ *x, SimTK_FINC_(x), SimTK_C_ *ap, SimTK_FLEN_(uplo));
+
+extern void csprfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *ap, const SimTK_C_ *afp, const int *ipiv, const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cspsv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_C_ *ap, int *ipiv, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cspsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *ap, SimTK_C_ *afp, int *ipiv, const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(rcond), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo));
+
+extern void csptrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *ap, int *ipiv, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void csptri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *ap, const int *ipiv, SimTK_C_ *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void csptrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *ap, const int *ipiv, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void csrscl_(SimTK_FDIM_(n), SimTK_S_INPUT_(sa), SimTK_C_ *sx, SimTK_FINC_(x));
+
+extern void cstedc_(SimTK_FOPT_(compz), SimTK_FDIM_(n), float *d__, float *e, SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, int *lrwork, int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(compz));
+
+extern void cstein_(SimTK_FDIM_(n), const float *d__, const float *e, SimTK_FDIM_(m), const float *w, const int *iblock, const int *isplit, SimTK_C_ *z__, SimTK_FDIM_(ldz), float *work, int *iwork, int *ifail, SimTK_INFO_);
+
+extern void cstemr_( SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FDIM_(n), float *d, float *e, SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_I_OUTPUT_(m), float *w, SimTK_C_ *z, SimTK_FDIM_(ldz), SimTK_I_INPUT_(nzc), SimTK_I_OUTPUT_(isuppz), SimTK_I_OUTPUT_(tryrac), float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_FDIM_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range) );
+
+extern void csteqr_(SimTK_FOPT_(compz), SimTK_FDIM_(n), float *d__, float *e, SimTK_C_ *z__, SimTK_FDIM_(ldz), float *work, SimTK_INFO_, SimTK_FLEN_(compz));
+
+extern void csycon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), const int *ipiv, SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), SimTK_C_ *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void csymv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_INPUT_(alpha), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *x, SimTK_FINC_(x), SimTK_C_INPUT_(beta), SimTK_C_ *y, SimTK_FINC_(y), SimTK_FLEN_(uplo));
+
+extern void csyr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_INPUT_(alpha), const SimTK_C_ *x, SimTK_FINC_(x), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_FLEN_(uplo));
+
+extern void csyrfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *af, SimTK_FDIM_(ldaf), const int *ipiv, const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void csysv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_C_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void csysvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *af, SimTK_FDIM_(ldaf), int *ipiv, const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(rcond), float *ferr, float *berr, SimTK_C_ *work, SimTK_FDIM_(lwork), float *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo));
+
+extern void csytf2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void csytrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void csytri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), const int *ipiv, SimTK_C_ *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void csytrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *a, SimTK_FDIM_(lda), const int *ipiv, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void ctbcon_(SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(kd), const SimTK_C_ *ab, SimTK_FDIM_(ldab), SimTK_S_OUTPUT_(rcond), SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void ctbrfs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), const SimTK_C_ *ab, SimTK_FDIM_(ldab), const SimTK_C_ *b, SimTK_FDIM_(ldb), const SimTK_C_ *x, SimTK_FDIM_(ldx), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void ctbtrs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), const SimTK_C_ *ab, SimTK_FDIM_(ldab), const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void ctgevc_(SimTK_FOPT_(side), SimTK_FOPT_(howmny), const int *select, SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *vl, SimTK_FDIM_(ldvl), SimTK_C_ *vr, SimTK_FDIM_(ldvr), SimTK_I_INPUT_(mm), SimTK_I_OUTPUT_(m), SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(howmny));
+
+
+
+
+extern void ctgex2_(SimTK_I_INPUT_(wantq), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *q, SimTK_FDIM_(ldq), SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_I_INPUT_(j1), SimTK_INFO_);
+
+extern void ctgexc_(SimTK_I_INPUT_(wantq), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *q, SimTK_FDIM_(ldq), SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_I_OUTPUT_(ifst), SimTK_I_OUTPUT_(ilst), SimTK_INFO_);
+
+extern void ctgsen_(SimTK_I_INPUT_(ijob), SimTK_I_INPUT_(wantq), SimTK_I_INPUT_(wantz), const int *select, SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *alpha, SimTK_C_INPUT_(beta), SimTK_C_ *q, SimTK_FDIM_(ldq), SimTK_C_ *z__, SimTK_FDIM_(ldz), SimTK_I_OUTPUT_(m), SimTK_S_OUTPUT_(pl), SimTK_S_OUTPUT_(pr), float *dif, SimTK_C_ *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_);
+
+extern void ctgsja_(SimTK_FOPT_(jobu), SimTK_FOPT_(jobv), SimTK_FOPT_(jobq), SimTK_FDIM_(m), SimTK_FDIM_(p), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(l), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_S_INPUT_(tola), SimTK_S_INPUT_(tolb), float *alpha, float *beta, SimTK_C_ *u, SimTK_FDIM_(ldu), SimTK_C_ *v, SimTK_FDIM_(ldv), SimTK_C_ *q, SimTK_FDIM_(ldq), SimTK_C_ *work, SimTK_I_OUTPUT_(ncycle), SimTK_INFO_, SimTK_FLEN_(jobu), SimTK_FLEN_(jobv), SimTK_FLEN_(jobq));
+
+extern void ctgsna_(SimTK_FOPT_(job), SimTK_FOPT_(howmny), const int *select, SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *b, SimTK_FDIM_(ldb), const SimTK_C_ *vl, SimTK_FDIM_(ldvl), const SimTK_C_ *vr, SimTK_FDIM_(ldvr), float *s, float *dif, SimTK_FDIM_(mm), SimTK_I_OUTPUT_(m), SimTK_C_ *work, SimTK_FDIM_(lwork), int *iwork, SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(howmny));
+
+extern void ctgsy2_(SimTK_FOPT_(trans), SimTK_I_INPUT_(ijob), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *c__, SimTK_FDIM_(ldc), const SimTK_C_ *d__, SimTK_FDIM_(ldd), const SimTK_C_ *e, SimTK_FDIM_(lde), SimTK_C_ *f, SimTK_FDIM_(ldf), SimTK_S_OUTPUT_(scale), SimTK_S_OUTPUT_(rdsum), SimTK_S_OUTPUT_(rdscal), SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void ctgsyl_(SimTK_FOPT_(trans), SimTK_I_INPUT_(ijob), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *c__, SimTK_FDIM_(ldc), const SimTK_C_ *d__, SimTK_FDIM_(ldd), const SimTK_C_ *e, SimTK_FDIM_(lde), SimTK_C_ *f, SimTK_FDIM_(ldf), SimTK_S_OUTPUT_(scale), SimTK_S_OUTPUT_(dif), SimTK_C_ *work, SimTK_FDIM_(lwork), int *iwork, SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void ctpcon_(SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), const SimTK_C_ *ap, SimTK_S_OUTPUT_(rcond), SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void ctprfs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *ap, const SimTK_C_ *b, SimTK_FDIM_(ldb), const SimTK_C_ *x, SimTK_FDIM_(ldx), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void ctptri_(SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_C_ *ap, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void ctptrs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *ap, SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void ctrcon_(SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_S_OUTPUT_(rcond), SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void ctrevc_(SimTK_FOPT_(side), SimTK_FOPT_(howmny), const int *select, SimTK_FDIM_(n), SimTK_C_ *t, SimTK_FDIM_(ldt), SimTK_C_ *vl, SimTK_FDIM_(ldvl), SimTK_C_ *vr, SimTK_FDIM_(ldvr), SimTK_FDIM_(mm), SimTK_I_OUTPUT_(m), SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(howmny));
+
+extern void ctrexc_(SimTK_FOPT_(compq), SimTK_FDIM_(n), SimTK_C_ *t, SimTK_FDIM_(ldt), SimTK_C_ *q, SimTK_FDIM_(ldq), SimTK_I_INPUT_(ifst), SimTK_I_INPUT_(ilst), SimTK_INFO_, SimTK_FLEN_(compq));
+
+
+extern void ctrrfs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *b, SimTK_FDIM_(ldb), const SimTK_C_ *x, SimTK_FDIM_(ldx), float *ferr, float *berr, SimTK_C_ *work, float *rwork, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void ctrsen_(SimTK_FOPT_(job), SimTK_FOPT_(compq), const int *select, SimTK_FDIM_(n), SimTK_C_ *t, SimTK_FDIM_(ldt), SimTK_C_ *q, SimTK_FDIM_(ldq), SimTK_C_ *w, SimTK_I_OUTPUT_(m), SimTK_S_OUTPUT_(s), SimTK_S_OUTPUT_(sep), SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(compq));
+
+extern void ctrsna_(SimTK_FOPT_(job), SimTK_FOPT_(howmny), const int *select, SimTK_FDIM_(n), const SimTK_C_ *t, SimTK_FDIM_(ldt), const SimTK_C_ *vl, SimTK_FDIM_(ldvl), const SimTK_C_ *vr, SimTK_FDIM_(ldvr), float *s, float *sep, SimTK_FDIM_(mm), SimTK_I_OUTPUT_(m), SimTK_C_ *work, SimTK_FDIM_(ldwork), float *rwork, SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(howmny));
+
+extern void ctrsyl_(SimTK_FOPT_(trana), SimTK_FOPT_(tranb), SimTK_I_INPUT_(isgn), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_S_OUTPUT_(scale), SimTK_INFO_, SimTK_FLEN_(trana), SimTK_FLEN_(tranb));
+
+extern void ctrti2_(SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void ctrtri_(SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void ctrtrs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void ctzrqf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *tau, SimTK_INFO_);
+
+extern void ctzrzf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), SimTK_C_ *tau, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void cung2l_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *work, SimTK_INFO_);
+
+extern void cung2r_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *work, SimTK_INFO_);
+
+extern void cungbr_(SimTK_FOPT_(vect), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(vect));
+
+extern void cunghr_(SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *work, SimTK_FDIM_(lwork), int *info);
+
+extern void cungl2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *work, SimTK_INFO_);
+
+extern void cunglq_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void cungql_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void cungqr_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void cungr2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *work, SimTK_INFO_);
+
+extern void cungrq_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void cungtr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cunm2l_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void cunm2r_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void cunmbr_(SimTK_FOPT_(vect), SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(vect), SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void cunmhr_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void cunml2_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void cunmlq_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void cunmql_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void cunmqr_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void cunmr2_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void cunmr3_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_I_INPUT_(l), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void cunmrq_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void cunmrz_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(l), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void cunmtr_(SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), const SimTK_C_ *tau, SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(uplo), SimTK_FLEN_(trans));
+
+extern void cupgtr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_C_ *ap, const SimTK_C_ *tau, SimTK_C_ *q, SimTK_FDIM_(ldq), SimTK_C_ *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void cupmtr_(SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_C_ *ap, const SimTK_C_ *tau, SimTK_C_ *c__, SimTK_FDIM_(ldc), SimTK_C_ *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(uplo), SimTK_FLEN_(trans));
+
+extern void dbdsdc_(SimTK_FOPT_(uplo), SimTK_FOPT_(compq), SimTK_FDIM_(n), double *d__, double *e, double *u, SimTK_FDIM_(ldu), double *vt, SimTK_FDIM_(ldvt), double *q, int *iq, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(compq));
+
+extern void dbdsqr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(ncvt), SimTK_FDIM_(nru), SimTK_FDIM_(ncc), double *d__, double *e, double *vt, SimTK_FDIM_(ldvt), double *u, SimTK_FDIM_(ldu), double *c__, SimTK_FDIM_(ldc), double *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void ddisna_(SimTK_FOPT_(job), SimTK_FDIM_(m), SimTK_FDIM_(n), const double *d__, double *sep, SimTK_INFO_, SimTK_FLEN_(job));
+
+extern void dgbbrd_(SimTK_FOPT_(vect), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(ncc), SimTK_FDIM_(kl), SimTK_FDIM_(ku), double *ab, SimTK_FDIM_(ldab), double *d__, double *e, double *q, SimTK_FDIM_(ldq), double *pt, SimTK_FDIM_(ldpt), double *c__, SimTK_FDIM_(ldc), double *work, SimTK_INFO_, SimTK_FLEN_(vect));
+
+extern void dgbcon_(SimTK_FOPT_(norm), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), double *ab, SimTK_FDIM_(ldab), const int *ipiv, SimTK_D_INPUT_(anorm), SimTK_D_OUTPUT_(rcond), double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(norm));
+
+extern void dgbequ_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), double *ab, SimTK_FDIM_(ldab), double *r__, double *c__, double *rowcnd, double *colcnd, double *amax, SimTK_INFO_);
+
+extern void dgbrfs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_FDIM_(nrhs), double *ab, SimTK_FDIM_(ldab), double *afb, SimTK_FDIM_(ldafb), const int *ipiv, double *b, SimTK_FDIM_(ldb), double *x, SimTK_FDIM_(ldx), double *ferr, double *berr, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void dgbsv_(SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_FDIM_(nrhs), double *ab, SimTK_FDIM_(ldab), int *ipiv, double *b, SimTK_FDIM_(ldb), SimTK_INFO_);
+
+extern void dgbsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_FDIM_(nrhs), double *ab, SimTK_FDIM_(ldab), double *afb, SimTK_FDIM_(ldafb), int *ipiv, SimTK_CHAR_OUTPUT_(equed), double *r__, double *c__, double *b, SimTK_FDIM_(ldb), double *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(trans), SimTK_FLEN_(equed));
+
+extern void dgbtf2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), double *ab, SimTK_FDIM_(ldab), int *ipiv, SimTK_INFO_);
+
+extern void dgbtrf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), double *ab, SimTK_FDIM_(ldab), int *ipiv, SimTK_INFO_);
+
+extern void dgbtrs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_FDIM_(nrhs), double *ab, SimTK_FDIM_(ldab), const int *ipiv, double *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void dgebak_(SimTK_FOPT_(job), SimTK_FOPT_(side), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_D_INPUT_(scale), SimTK_I_INPUT_(m), double *v, SimTK_FDIM_(ldv), SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(side));
+
+extern void dgebal_(SimTK_FOPT_(job), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), SimTK_I_OUTPUT_(ilo), SimTK_I_OUTPUT_(ihi), SimTK_D_OUTPUT_(scale), SimTK_INFO_, SimTK_FLEN_(job));
+
+extern void dgebd2_(SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *d__, double *e, double *tauq, double *taup, double *work, SimTK_INFO_);
+
+extern void dgebrd_(SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *d__, double *e, double *tauq, double *taup, double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dgecon_(SimTK_FOPT_(norm), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), SimTK_D_INPUT_(anorm), SimTK_D_OUTPUT_(rcond), double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(norm));
+
+extern void dgeequ_(SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *r__, double *c__, double *rowcnd, double *colcnd, double *amax, SimTK_INFO_);
+
+extern void dgees_(SimTK_FOPT_(jobvs), SimTK_FOPT_(sort), SimTK_SELECT_2D select, SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), int *sdim, double *wr, double *wi, double *vs, SimTK_FDIM_(ldvs), double *work, SimTK_FDIM_(lwork), int *bwork, SimTK_INFO_, SimTK_FLEN_(jobvs), SimTK_FLEN_(sort));
+
+extern void dgeesx_(SimTK_FOPT_(jobvs), SimTK_FOPT_(sort), SimTK_SELECT_2D select, char *sense, SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), int *sdim, double *wr, double *wi, double *vs, SimTK_FDIM_(ldvs), SimTK_D_OUTPUT_(rconde), SimTK_D_OUTPUT_(rcondv), double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), int *bwork, SimTK_INFO_, SimTK_FLEN_(jobvs), SimTK_FLEN_(sort), SimTK_FLEN_(sense));
+
+extern void dgeev_(SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *wr, double *wi, double *vl, SimTK_FDIM_(ldvl), double *vr, SimTK_FDIM_(ldvr), double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(jobvl), SimTK_FLEN_(jobvr));
+
+extern void dgeevx_(SimTK_FOPT_(balanc), SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), char *sense, SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *wr, double *wi, double *vl, SimTK_FDIM_(ldvl), double *vr, SimTK_FDIM_(ldvr), SimTK_I_OUTPUT_(ilo), SimTK_I_OUTPUT_(ihi), SimTK_D_OUTPUT_(scale), double *abnrm, SimTK_D_OUTPUT_(rconde), SimTK_D_OUTPUT_(rcondv), double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_INFO_, SimTK_FLEN_(balanc), SimTK_FLEN_(jobvl), SimTK_FLEN_(jobvr), SimTK_FLEN_(sense));
+
+extern void dgegs_(SimTK_FOPT_(jobvsl), SimTK_FOPT_(jobvsr), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), double *alphar, double *alphai, double *beta, double *vsl, SimTK_FDIM_(ldvsl), double *vsr, SimTK_FDIM_(ldvsr), double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(jobvsl), SimTK_FLEN_(jobvsr));
+
+extern void dgegv_(SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), double *alphar, double *alphai, double *beta, double *vl, SimTK_FDIM_(ldvl), double *vr, SimTK_FDIM_(ldvr), double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(jobvsl), SimTK_FLEN_(jobvsr));
+
+extern void dgehd2_(SimTK_FDIM_(n), SimTK_FDIM_(ilo), SimTK_I_INPUT_(ihi), double *a, SimTK_I_INPUT_(lda), double *tau, double *work, SimTK_INFO_);
+
+extern void dgehrd_(SimTK_FDIM_(n), SimTK_FDIM_(ilo), SimTK_I_INPUT_(ihi), double *a, SimTK_I_INPUT_(lda), double *tau, double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dgelq2_(SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_INFO_);
+
+extern void dgelqf_(SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dgels_(SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void dgelsd_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), double *s, SimTK_D_INPUT_(rcond), SimTK_I_OUTPUT_(rank), double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_INFO_);
+
+extern void dgelss_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), double *s, SimTK_D_INPUT_(rcond), SimTK_I_OUTPUT_(rank), double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dgelsx_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), int *jpvt, SimTK_D_INPUT_(rcond), SimTK_I_OUTPUT_(rank), double *work, SimTK_INFO_);
+
+extern void dgelsy_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), int *jpvt, SimTK_D_INPUT_(rcond), SimTK_I_OUTPUT_(rank), double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dgeql2_(SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_INFO_);
+
+extern void dgeqlf_(SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dgeqp3_(SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), int *jpvt, double *tau, double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dgeqpf_(SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), int *jpvt, double *tau, double *work, SimTK_INFO_);
+
+extern void dgeqr2_(SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_INFO_);
+
+extern void dgeqrf_(SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dgerfs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *a, SimTK_FDIM_(lda), double *af, SimTK_FDIM_(ldaf), const int *ipiv, double *b, SimTK_FDIM_(ldb), double *x, SimTK_FDIM_(ldx), double *ferr, double *berr, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void dgerq2_(SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_INFO_);
+
+extern void dgerqf_(SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dgesc2_(SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *rhs, int *ipiv, int *jpiv, SimTK_D_OUTPUT_(scale));
+
+extern void dgesdd_(SimTK_FOPT_(jobz), SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *s, double *u, SimTK_FDIM_(ldu), double *vt, SimTK_FDIM_(ldvt), double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_INFO_, SimTK_FLEN_(jobz));
+
+extern void dgesv_(SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *a, SimTK_FDIM_(lda), int *ipiv, double *b, SimTK_FDIM_(ldb), SimTK_INFO_);
+
+extern void dgesvd_(SimTK_FOPT_(jobu), char *jobvt, SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *s, double *u, SimTK_FDIM_(ldu), double *vt, SimTK_FDIM_(ldvt), double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(jobu), SimTK_FLEN_(jobvt));
+
+extern void dgesvx_(SimTK_FOPT_(fact), SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *a, SimTK_FDIM_(lda), double *af, SimTK_FDIM_(ldaf), int *ipiv, SimTK_CHAR_OUTPUT_(equed), double *r__, double *c__, double *b, SimTK_FDIM_(ldb), double *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(trans), SimTK_FLEN_(equed));
+
+extern void dgetc2_(SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), int *ipiv, int *jpiv, SimTK_INFO_);
+
+extern void dgetf2_(SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), int *ipiv, SimTK_INFO_);
+
+extern void dgetrf_(SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), int *ipiv, SimTK_INFO_);
+
+extern void dgetri_(SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), const int *ipiv, double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dgetrs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *a, SimTK_FDIM_(lda), const int *ipiv, double *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void dggbak_(SimTK_FOPT_(job), SimTK_FOPT_(side), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), const double *lscale, const double *rscale, SimTK_FDIM_(m), double *v, SimTK_FDIM_(ldv), SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(side));
+
+extern void dggbal_(SimTK_FOPT_(job), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), SimTK_I_OUTPUT_(ilo), SimTK_I_OUTPUT_(ihi), double *lscale, double *rscale, double *work, SimTK_INFO_, SimTK_FLEN_(job));
+
+extern void dgges_(SimTK_FOPT_(jobvsl), SimTK_FOPT_(jobvsr), SimTK_FOPT_(sort), SimTK_SELECT_3D delctg, SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), int *sdim, double *alphar, double *alphai, SimTK_D_INPUT_(beta), double *vsl, SimTK_FDIM_(ldvsl), double *vsr, SimTK_FDIM_(ldvsr), double *work, SimTK_FDIM_(lwork), int *bwork, SimTK_INFO_, SimTK_FLEN_(jobvsl), SimTK_FLEN_(jobvsr), SimTK_FLEN_(sort));
+
+extern void dggesx_(SimTK_FOPT_(jobvsl), SimTK_FOPT_(jobvsr), SimTK_FOPT_(sort), SimTK_SELECT_3D delctg, SimTK_FOPT_(sense), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), SimTK_I_OUTPUT_(sdim), double *alphar, double *alphai, double *beta, double *vsl, SimTK_FDIM_(ldvsl), double *vsr, SimTK_FDIM_(ldvsr), SimTK_D_OUTPUT_(rconde), SimTK_D_OUTPUT_(rcondv), double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_FDIM_(liwork), int *bwork, SimTK_INFO_, SimTK_FLEN_(jobv [...]
+
+extern void dggev_(SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), double *alphar, double *alphai, double *beta, double *vl, SimTK_FDIM_(ldvl), double *vr, SimTK_FDIM_(ldvr), double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(jobvl), SimTK_FLEN_(jobvr));
+
+extern void dggevx_(SimTK_FOPT_(balanc), SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), SimTK_FOPT_(sense), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), double *alphar, double *alphai, double *beta, double *vl, SimTK_FDIM_(ldvl), double *vr, SimTK_FDIM_(ldvr), SimTK_I_OUTPUT_(ilo), SimTK_I_OUTPUT_(ihi), double *lscale, double *rscale, SimTK_D_OUTPUT_(abnrm), SimTK_D_OUTPUT_(bbnrm), SimTK_D_OUTPUT_(rconde), SimTK_D_OUTPUT_(rcondv), double *work, SimTK_FDIM_(lwork), i [...]
+
+extern void dggglm_(SimTK_FDIM_(n), SimTK_FDIM_(m), SimTK_FDIM_(p), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), double *d__, double *x, double *y, double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dgghrd_(SimTK_FOPT_(compq), SimTK_FOPT_(compz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), double *q, SimTK_FDIM_(ldq), double *z__, SimTK_FDIM_(ldz), SimTK_INFO_, SimTK_FLEN_(compq), SimTK_FLEN_(compz));
+
+extern void dgglse_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(p), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), double *c__, double *d__, double *x, double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dggqrf_(SimTK_FDIM_(n), SimTK_FDIM_(m), SimTK_FDIM_(p), double *a, SimTK_FDIM_(lda), double *taua, double *b, SimTK_FDIM_(ldb), double *taub, double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dggrqf_(SimTK_FDIM_(m), SimTK_FDIM_(p), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *taua, double *b, SimTK_FDIM_(ldb), double *taub, double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dggsvd_(SimTK_FOPT_(jobu), SimTK_FOPT_(jobv), SimTK_FOPT_(jobq), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(p), SimTK_I_OUTPUT_(k), SimTK_I_OUTPUT_(l), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), double *alpha, double *beta, double *u, SimTK_FDIM_(ldu), double *v, SimTK_FDIM_(ldv), double *q, SimTK_FDIM_(ldq), double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(jobu), SimTK_FLEN_(jobv), SimTK_FLEN_(jobq));
+
+extern void dggsvp_(SimTK_FOPT_(jobu), SimTK_FOPT_(jobv), SimTK_FOPT_(jobq), SimTK_FDIM_(m), SimTK_FDIM_(p), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), SimTK_D_INPUT_(tola), SimTK_D_INPUT_(tolb), SimTK_I_OUTPUT_(k), SimTK_I_OUTPUT_(l), double *u, SimTK_FDIM_(ldu), double *v, SimTK_FDIM_(ldv), double *q, SimTK_FDIM_(ldq), int *iwork, double *tau, double *work, SimTK_INFO_, SimTK_FLEN_(jobu), SimTK_FLEN_(jobv), SimTK_FLEN_(jobq));
+
+extern void dgtcon_(SimTK_FOPT_(norm), SimTK_FDIM_(n), double *dl, double *d__, double *du, double *du2, const int *ipiv, SimTK_D_INPUT_(anorm), SimTK_D_OUTPUT_(rcond), double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(norm));
+
+extern void dgtrfs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *dl, double *d__, double *du, double *dlf, double *df, double *duf, double *du2, const int *ipiv, double *b, SimTK_FDIM_(ldb), double *x, SimTK_FDIM_(ldx), double *ferr, double *berr, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void dgtsv_(SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *dl, double *d__, double *du, double *b, SimTK_FDIM_(ldb), int *info);
+
+extern void dgtsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *dl, double *d__, double *du, double *dlf, double *df, double *duf, double *du2, int *ipiv, double *b, SimTK_FDIM_(ldb), double *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(trans));
+
+extern void dgttrf_(SimTK_FDIM_(n), double *dl, double *d__, double *du, double *du2, int *ipiv, SimTK_INFO_);
+
+extern void dgttrs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *dl, double *d__, double *du, double *du2, const int *ipiv, double *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void dgtts2_(int *itrans, SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *dl, double *d__, double *du, double *du2, const int *ipiv, double *b, SimTK_FDIM_(ldb));
+
+extern void dhgeqz_(SimTK_FOPT_(job), SimTK_FOPT_(compq), SimTK_FOPT_(compz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), double *alphar, double *alphai, double *beta, double *q, SimTK_FDIM_(ldq), double *z__, SimTK_FDIM_(ldz), double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(compq), SimTK_FLEN_(compz));
+
+extern void dhsein_(SimTK_FOPT_(side), SimTK_FOPT_(eigsrc), SimTK_FOPT_(initv), const int *select, SimTK_FDIM_(n), const double *h__, SimTK_FDIM_(ldh), double *wr, double *wi, double *vl, SimTK_FDIM_(ldvl), double *vr, SimTK_FDIM_(ldvr), SimTK_FDIM_(mm), SimTK_I_OUTPUT_(m), double *work, int *ifaill, int *ifailr, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(eigsrc), SimTK_FLEN_(initv));
+
+extern double dlamch_(SimTK_FOPT_(cmach), SimTK_FLEN_(cmach));
+
+extern void dhseqr_(SimTK_FOPT_(job), SimTK_FOPT_(compz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), double *h__, SimTK_FDIM_(ldh), double *wr, double *wi, double *z__, SimTK_FDIM_(ldz), double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(comqz));
+
+extern int disnan( SimTK_D_INPUT_(din) );
+
+extern int dlaisnan( SimTK_D_INPUT_(din1), SimTK_D_INPUT_(din2) );
+
+extern void dlabad_(SimTK_D_INOUT_(small), SimTK_D_INOUT_(large));
+
+extern void dlabrd_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nb), double *a, SimTK_FDIM_(lda), double *d__, double *e, double *tauq, double *taup, double *x, SimTK_FDIM_(ldx), double *y, SimTK_FDIM_(ldy));
+
+extern void dlacon_(SimTK_FDIM_(n), double *v, double *x, int *isgn, SimTK_D_OUTPUT_(est), SimTK_I_OUTPUT_(kase) );
+
+extern void dlacn2_( SimTK_FDIM_(n), double *v, double *x, int *isgn, SimTK_D_INPUT_(est), SimTK_I_OUTPUT_(kase), int *isave );
+
+extern void dlacpy_(SimTK_FOPT_(uplo), SimTK_FDIM_(m), SimTK_FDIM_(n), const double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), SimTK_FLEN_(uplo));
+
+extern void dladiv_(SimTK_D_INPUT_(a), SimTK_D_INPUT_(b), SimTK_D_INPUT_(c__), SimTK_D_INPUT_(d__), double *p, double *q);
+
+extern void dlae2_(SimTK_D_INPUT_(a), SimTK_D_INPUT_(b), SimTK_D_INPUT_(c__), double *rt1, double *rt2);
+
+extern void dlaebz_(SimTK_I_INPUT_(ijob), SimTK_I_INPUT_(nitmax), SimTK_FDIM_(n), SimTK_I_INPUT_(mmax), SimTK_I_INPUT_(minp), SimTK_I_INPUT_(nbmin), SimTK_D_INPUT_(abstol), SimTK_D_INPUT_(reltol), SimTK_D_INPUT_(pivmin), const double *d__, const double *e, const double *e2, int *nval, double *ab, double *c__, SimTK_I_OUTPUT_(mout), int *nab, double *work, int *iwork, SimTK_INFO_);
+
+
+extern void dlaed0_(SimTK_I_INPUT_(icompq), SimTK_I_INPUT_(qsiz), SimTK_FDIM_(n), double *d__, double *e, double *q, SimTK_FDIM_(ldq), double *qstore, SimTK_FDIM_(ldqs), double *work, int *iwork, SimTK_INFO_);
+
+extern void dlaed1_(SimTK_FDIM_(n), double *d__, double *q, SimTK_FDIM_(ldq), int *indxq, SimTK_D_INPUT_(rho), SimTK_I_INPUT_(cutpnt), double *work, int *iwork, SimTK_INFO_);
+
+extern void dlaed2_(SimTK_FDIM_(k), SimTK_FDIM_(n), SimTK_I_INPUT_(n1), double *d__, double *q, SimTK_FDIM_(ldq), int *indxq, double *rho, double *z__, double *dlamda, double *w, double *q2, int *indx, int *indxc, int *indxp, int *coltyp, SimTK_INFO_);
+
+extern void dlaed3_(SimTK_FDIM_(k), SimTK_FDIM_(n), SimTK_I_INPUT_(n1), double *d__, double *q, SimTK_FDIM_(ldq), SimTK_D_INPUT_(rho), double *dlamda, const double *q2, const int *indx, const int *ctot, double *w, double *s, SimTK_INFO_);
+
+extern void dlaed4_(SimTK_FDIM_(n), SimTK_I_INPUT_(i__), const double *d__, const double *z__, double *delta, SimTK_D_INPUT_(rho), SimTK_D_OUTPUT_(dlam), SimTK_INFO_);
+
+extern void dlaed5_(SimTK_I_INPUT_(i__), const double *d__, const double *z__, double *delta, SimTK_D_INPUT_(rho), SimTK_D_OUTPUT_(dlam));
+
+extern void dlaed6_(SimTK_I_INPUT_(kniter), SimTK_I_INPUT_(orgati), SimTK_D_INPUT_(rho), const double *d__, const double *z__, SimTK_D_INPUT_(finit), SimTK_D_INPUT_(tau), SimTK_INFO_);
+
+extern void dlaed7_(SimTK_I_INPUT_(icompq), SimTK_FDIM_(n), SimTK_FDIM_(qsiz), SimTK_I_INPUT_(tlvls), SimTK_I_INPUT_(curlvl), SimTK_I_INPUT_(curpbm), double *d__, double *q, SimTK_FDIM_(ldq), int *indxq, SimTK_D_INPUT_(rho), SimTK_I_INPUT_(cutpnt), double *qstore, int *qptr, const int *prmptr, const int *perm, const int *givptr, const int *givcol, const double *givnum, double *work, int *iwork, SimTK_INFO_);
+
+extern void dlaed8_(SimTK_I_INPUT_(icompq), SimTK_I_OUTPUT_(k), SimTK_FDIM_(n), SimTK_FDIM_(qsiz), double *d__, double *q, SimTK_FDIM_(ldq), const int *indxq, SimTK_D_OUTPUT_(rho), SimTK_I_INPUT_(cutpnt), double *z__, double *dlamda, double *q2, SimTK_FDIM_(ldq2), double *w, int *perm, int *givptr, int *givcol, double *givnum, int *indxp, int *indx, SimTK_INFO_);
+
+extern void dlaed9_(SimTK_FDIM_(k), int *kstart, int *kstop, SimTK_FDIM_(n), double *d__, double *q, SimTK_FDIM_(ldq), double *rho, double *dlamda, double *w, double *s, SimTK_FDIM_(lds), SimTK_INFO_);
+
+extern void dlaeda_(SimTK_FDIM_(n), SimTK_I_INPUT_(tlvls), SimTK_I_INPUT_(curlvl), SimTK_I_INPUT_(curpbm), const int *prmptr, const int *perm, const int *givptr, const int *givcol, const double *givnum, const double *q, const int *qptr, double *z__, double *ztemp, SimTK_INFO_);
+
+extern void dlaein_(SimTK_I_INPUT_(rightv), SimTK_I_INPUT_(noinit), SimTK_FDIM_(n), const double *h__, SimTK_FDIM_(ldh), SimTK_D_INPUT_(wr), SimTK_D_INPUT_(wi), double *vr, double *vi, double *b, SimTK_FDIM_(ldb), double *work, SimTK_D_INPUT_(eps3), SimTK_D_INPUT_(smlnum), SimTK_D_INPUT_(bignum), SimTK_INFO_);
+
+extern void dlaev2_(SimTK_D_INPUT_(a), SimTK_D_INPUT_(b), SimTK_D_INPUT_(c__), SimTK_D_OUTPUT_(rt1), SimTK_D_OUTPUT_(rt2), SimTK_D_OUTPUT_(cs1), SimTK_D_OUTPUT_(sn1));
+
+extern void dlaexc_(SimTK_I_INPUT_(wantq), SimTK_FDIM_(n), double *t, SimTK_FDIM_(ldt), double *q, SimTK_FDIM_(ldq), SimTK_I_INPUT_(j1), SimTK_I_INPUT_(n1), SimTK_I_INPUT_(n2), double *work, SimTK_INFO_);
+
+extern void dlag2_(const double *a, SimTK_FDIM_(lda), const double *b, SimTK_FDIM_(ldb), SimTK_D_INPUT_(safmin), SimTK_D_OUTPUT_(scale1), SimTK_D_OUTPUT_(scale2), SimTK_D_OUTPUT_(wr1), SimTK_D_OUTPUT_(wr2), SimTK_D_OUTPUT_(wi) );
+
+extern void dlags2_(SimTK_I_INPUT_(upper), SimTK_D_INPUT_(a1), SimTK_D_INPUT_(a2), SimTK_D_INPUT_(a3), SimTK_D_INPUT_(b1), SimTK_D_INPUT_(b2), SimTK_D_INPUT_(b3), SimTK_D_OUTPUT_(csu), SimTK_D_OUTPUT_(snu), SimTK_D_OUTPUT_(csv), SimTK_D_OUTPUT_(snv), SimTK_D_OUTPUT_(csq), SimTK_D_OUTPUT_(snq) );
+
+extern void dlagtf_(SimTK_FDIM_(n), double *a, SimTK_D_INPUT_(lambda), double *b, double *c__, SimTK_D_INPUT_(tol), double *d__, int *in, SimTK_INFO_);
+
+extern void dlagtm_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_D_INPUT_(alpha), const double *dl, const double *d__, const double *du, const double *x, SimTK_FDIM_(ldx), SimTK_D_INPUT_(beta), double *b, SimTK_FDIM_(ldb), SimTK_FLEN_(trans));
+
+extern void dlag2s_( SimTK_I_INPUT_(m), SimTK_I_INPUT_(n), const double *a, SimTK_FDIM_(lda), float *sa, SimTK_FDIM_(ldsa), SimTK_INFO_);
+
+extern void dlagts_(SimTK_I_INPUT_(job), SimTK_FDIM_(n), const double *a, const double *b, const double *c__, const double *d__, const int *in, double *y, SimTK_D_OUTPUT_(tol), SimTK_INFO_);
+
+extern void dlagv2_(double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), double *alphar, double *alphai, double *beta, SimTK_D_OUTPUT_(csl), SimTK_D_OUTPUT_(snl), SimTK_D_OUTPUT_(csr), SimTK_D_OUTPUT_(snr));
+
+extern void dlahqr_(SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), double *h__, SimTK_FDIM_(ldh), double *wr, double *wi, SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), double *z__, SimTK_FDIM_(ldz), SimTK_INFO_);
+
+extern void dlahrd_(SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(nb), double *a, SimTK_FDIM_(lda), double *tau, double *t, SimTK_FDIM_(ldt), double *y, SimTK_FDIM_(ldy));
+
+extern void dlahr2_(SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(nb), double *a, SimTK_FDIM_(lda), double *tau, double *t, SimTK_FDIM_(ldt), double *y, SimTK_FDIM_(ldy));
+
+extern void dlaic1_(SimTK_I_INPUT_(job), SimTK_FDIM_(j), const double *x, SimTK_D_INPUT_(sest), const double *w, SimTK_D_INPUT_(gamma), SimTK_D_OUTPUT_(sestpr), SimTK_D_OUTPUT_(s), SimTK_D_OUTPUT_(c__) );
+
+extern void dlaln2_(SimTK_I_INPUT_(ltrans), SimTK_I_INPUT_(na), SimTK_I_INPUT_(nw), SimTK_D_INPUT_(smin), SimTK_D_INPUT_(ca), const double *a, SimTK_FDIM_(lda), SimTK_D_INPUT_(d1), SimTK_D_INPUT_(d2), SimTK_D_INPUT_(b), SimTK_FDIM_(ldb), SimTK_D_INPUT_(wr), SimTK_D_INPUT_(wi), double *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(scale), double *xnorm, SimTK_INFO_);
+
+extern void dlals0_(SimTK_I_INPUT_(icompq), SimTK_I_INPUT_(nl), SimTK_I_INPUT_(nr), SimTK_I_INPUT_(sqre), SimTK_FDIM_(nrhs), double *b, SimTK_FDIM_(ldb), double *bx, SimTK_FDIM_(ldbx), int *perm, int *givptr, const int *givcol, SimTK_FDIM_(ldgcol), double *givnum, SimTK_FDIM_(ldgnum), double *poles, double *difl, double *difr, double *z__, int *k, double *c__, double *s, double *work, SimTK_INFO_);
+
+extern void dlalsa_(SimTK_I_INPUT_(icompq), SimTK_I_INPUT_(smlsiz), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *b, SimTK_FDIM_(ldb), double *bx, SimTK_FDIM_(ldbx), double *u, SimTK_FDIM_(ldu), double *vt, SimTK_FDIM_(k), double *difl, double *difr, double *z__, double *poles, int *givptr, const int *givcol, SimTK_FDIM_(ldgcol), int *perm, double *givnum, double *c__, double *s, double *work, int *iwork, SimTK_INFO_);
+
+extern void dlalsd_(SimTK_FOPT_(uplo), SimTK_I_INPUT_(smlsiz), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *d__, double *e, double *b, SimTK_FDIM_(ldb), SimTK_D_INPUT_(rcond), SimTK_I_OUTPUT_(rank), double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dlamc1_(int *beta, int *t, int *rnd, int *ieee1);
+
+extern void dlamc2_(int *beta, int *t, int *rnd, double *eps, int *emin, double *rmin, int *emax, double *rmax);
+
+extern void dlamc4_(int *emin, double *start, int *base);
+
+extern void dlamc5_(int *beta, int *p, int *emin, int *ieee, int *emax, double *rmax);
+
+extern void dlamrg_( SimTK_I_INPUT_(n1), SimTK_I_INPUT_(n2), double *a, int *dtrd1, int *dtrd2, int *index);
+
+extern void dlanv2_(double *a, double *b, double *c__, double *d__, double *rt1r, double *rt1i, double *rt2r, double *rt2i, double *cs, double *sn);
+
+extern void dlapll_(SimTK_FDIM_(n), double *x, SimTK_FINC_(x), double *y, SimTK_FINC_(y), SimTK_D_OUTPUT_(ssmin));
+
+extern void dlapmt_(SimTK_I_INPUT_(forwrd), SimTK_FDIM_(m), SimTK_FDIM_(n), double *x, SimTK_FDIM_(ldx), SimTK_FDIM_(k));
+
+extern void dlaqgb_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), double *ab, SimTK_FDIM_(ldab), double *r__, double *c__, SimTK_D_OUTPUT_(rowcnd), SimTK_D_OUTPUT_(colcnd), SimTK_D_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(equed));
+
+extern void dlaqge_(SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *r__, double *c__, SimTK_D_INPUT_(rowcnd), SimTK_D_INPUT_(colcnd), SimTK_D_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(equed));
+
+extern void dlaqp2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_I_INPUT_(offset), double *a, SimTK_FDIM_(lda), int *jpvt, double *tau, double *vn1, double *vn2, double *work);
+
+extern void dlaqps_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_I_INPUT_(offset), SimTK_I_INPUT_(nb), SimTK_I_OUTPUT_(kb), double *a, SimTK_FDIM_(lda), int *jpvt, double *tau, double *vn1, double *vn2, double *auxv, double *f, SimTK_FDIM_(ldf));
+
+extern void dlaqr0_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), double *h, SimTK_FDIM_(ldh), double *wr, double *wi, SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), double *z, SimTK_FDIM_(ldz), double *work, SimTK_FDIM_(lwork), SimTK_INFO_ );
+
+extern void dlaqr1_(SimTK_FDIM_(n), const double *h, SimTK_FDIM_(ldh), SimTK_D_INPUT_(sr1), SimTK_D_INPUT_(si1), SimTK_D_INPUT_(sr2), SimTK_D_INPUT_(si2), double *v );
+
+extern void dlaqr2_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ktop), SimTK_I_INPUT_(kbot), SimTK_I_INPUT_(nw), double *h, SimTK_FDIM_(ldh), SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), double *z, SimTK_FDIM_(ldz), SimTK_I_OUTPUT_(ns), SimTK_I_OUTPUT_(nd), double *sr, double *si, double *v, SimTK_FDIM_(ldv), SimTK_I_INPUT_(nh), double *t, SimTK_FDIM_(ldt), SimTK_I_INPUT_(nv), double *wv, SimTK_I_INPUT_(ldwv), double *work, SimTK_FDIM_(lwork) );
+
+extern void dlaqr3_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ktop), SimTK_I_INPUT_(kbot), SimTK_I_INPUT_(nw), double *h, SimTK_FDIM_(ldh), SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), double *z, SimTK_FDIM_(ldz), SimTK_I_OUTPUT_(ns), SimTK_I_OUTPUT_(nd), double *sr, double *si, double *v, SimTK_FDIM_(ldv), SimTK_I_INPUT_(nh), double *t, SimTK_FDIM_(ldt), SimTK_I_INPUT_(nv), double *wv, SimTK_I_INPUT_(ldwv), double *work, SimTK_FDIM_(lwork) );
+
+extern void dlaqr4_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), double *h, SimTK_FDIM_(ldh), double *wr, double *wi, SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), double *z, SimTK_FDIM_(ldz), double *work, SimTK_FDIM_(lwork), SimTK_INFO_ );
+
+extern void dlaqr5_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_I_INPUT_(kacc22), SimTK_FDIM_(n), SimTK_I_INPUT_(ktop), SimTK_I_INPUT_(kbot), SimTK_I_INPUT_(nshifts), const double *sr, const double *si, double *h, SimTK_FDIM_(ldh), SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), double *z, SimTK_FDIM_(ldz), double *v, SimTK_FDIM_(ldv), double *u, SimTK_FDIM_(ldu), SimTK_I_INPUT_(nh), double *wh, SimTK_FDIM_(ldwh), SimTK_I_INPUT_(nv), double *wv, SimTK_FDIM_(ldwv) );
+
+extern void dlaqsb_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), double *ab, SimTK_FDIM_(ldab), double *s, SimTK_D_INPUT_(scond), SimTK_D_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(uplo), SimTK_FLEN_(equedk));
+
+extern void dlaqsp_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *ap, const double *s, SimTK_D_INPUT_(scond), SimTK_D_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void dlaqsy_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), const double *s, SimTK_D_INPUT_(scond), SimTK_D_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void dlaqtr_( SimTK_I_INPUT_(ltran), SimTK_I_INPUT_(lreal), SimTK_FDIM_(n), const double *t, SimTK_FDIM_(ldt), const double *b, const double *w, SimTK_D_OUTPUT_(scale), double *x, double *work, SimTK_INFO_);
+
+extern void dlar1v_(SimTK_FDIM_(n), SimTK_I_INPUT_(b1), SimTK_I_INPUT_(bn), SimTK_D_INPUT_(lambda), const double *d__, const double *l, const double *ld, const double *lld, const double *pivmin, const double *gaptol, double *z__, SimTK_I_INPUT_(wantc), SimTK_I_OUTPUT_(negcnt), SimTK_D_OUTPUT_(ztz), SimTK_D_OUTPUT_(mingma), SimTK_I_OUTPUT_(r__), int *isuppz, SimTK_D_OUTPUT_(nrminv), SimTK_D_OUTPUT_(resid), SimTK_D_OUTPUT_(rqcorr), double *work);
+
+extern void dlar2v_(SimTK_FDIM_(n), double *x, double *y, double *z__, SimTK_FINC_(x), const double *c__, const double *s, SimTK_FINC_(c));
+
+extern void dlarf_(SimTK_FOPT_(side), SimTK_FDIM_(m), SimTK_FDIM_(n), double *v, SimTK_FINC_(v), double *tau, double *c__, SimTK_FDIM_(ldc), double *work, SimTK_FLEN_(side));
+
+extern void dlarfb_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FOPT_(direct), SimTK_FOPT_(storev), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const double *v, SimTK_FDIM_(ldv), const double *t, SimTK_FDIM_(ldt), double *c__, SimTK_FDIM_(ldc), double *work, SimTK_FDIM_(ldwork), SimTK_FLEN_(side), SimTK_FLEN_(trans), SimTK_FLEN_(direct), SimTK_FLEN_(storev));
+
+extern void dlarfg_(SimTK_FDIM_(n), SimTK_D_OUTPUT_(alpha), double *x, SimTK_FINC_(x), SimTK_D_OUTPUT_(tau) );
+
+extern void dlarft_(SimTK_FOPT_(direct), SimTK_FOPT_(storev), SimTK_FDIM_(n), SimTK_I_INPUT_(k), double *v, SimTK_FDIM_(ldv), const double *tau, double *t, SimTK_FDIM_(ldt), SimTK_FLEN_(direct), SimTK_FLEN_(storev));
+
+extern void dlarfx_(SimTK_FOPT_(side), SimTK_FDIM_(m), SimTK_FDIM_(n), const double *v, const double *tau, double *c__, SimTK_FDIM_(ldc), double *work, SimTK_FLEN_(side));
+
+extern void dlargv_(SimTK_FDIM_(n), double *x, SimTK_FINC_(x), double *y, SimTK_FINC_(y), double *c__, SimTK_FINC_(c));
+
+extern void dlarnv_(SimTK_I_INPUT_(idist), int *iseed, SimTK_FDIM_(n), double *x);
+
+extern void dlarra_(SimTK_FDIM_(n), const double *d, double *e, double *e2, SimTK_D_INPUT_(spltol), SimTK_D_INPUT_(tnrm), SimTK_I_OUTPUT_(nsplit), int *isplit, SimTK_INFO_);
+
+extern void dlarrb_(SimTK_FDIM_(n), const double *d__, const double *lld, SimTK_I_INPUT_(ifirst), SimTK_I_INPUT_(ilast), SimTK_D_INPUT_(rtol1), SimTK_D_INPUT_(rtol2), SimTK_I_INPUT_(offset), double *w, double *wgap, double *werr, double *work, int *iwork, SimTK_D_INPUT_(pivmin), SimTK_D_INPUT_(spdiam), SimTK_I_INPUT_(twist), SimTK_INFO_);
+
+
+extern void dlarrc_(SimTK_FOPT_(jobt), SimTK_FDIM_(n), SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), const double *d, const double *e, SimTK_D_INPUT_(pivmin), SimTK_I_OUTPUT_(eigcnt), SimTK_I_OUTPUT_(lcnt), SimTK_I_OUTPUT_(rcnt), SimTK_INFO_, SimTK_FLEN_(jobt) );
+
+extern void dlarrd_( SimTK_FOPT_(range), SimTK_FOPT_(order), SimTK_FDIM_(n), SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), const double *gers, SimTK_D_INPUT_(reltol), const double *d, const double *e, const double *e2, SimTK_D_INPUT_(pivmin), SimTK_I_INPUT_(nsplit), const int *isplit, SimTK_I_OUTPUT_(m), double *w, double *werr, SimTK_D_OUTPUT_(wl), SimTK_D_OUTPUT_(wu), int *iblock, int *indexw, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(range), [...]
+
+extern void dlarre_(SimTK_FOPT_(range), SimTK_FDIM_(n), SimTK_D_OUTPUT_(vl), SimTK_D_OUTPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), double *d__, double *e, double *e2, SimTK_D_INPUT_(rtol1), SimTK_D_INPUT_(rtol2), SimTK_D_INPUT_(spltol), SimTK_I_OUTPUT_(nsplit), int *isplit, SimTK_I_OUTPUT_(m), double *w, double *werr, double *wgap, int *iblock, int *indexw, double *gers, double *pivmin, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(range) );
+
+
+extern void dlarrf_(SimTK_FDIM_(n), double *d__, double *l, double *ld, SimTK_I_INPUT_(clstrt), SimTK_I_INPUT_(clend), const double *w, double *wgap, double *werr, SimTK_D_INPUT_(spdiam), SimTK_D_INPUT_(clgapl), SimTK_D_INPUT_(clgapr), SimTK_D_INPUT_(pivmin), SimTK_D_OUTPUT_(sigma), double *dplus, double *lplus, double *work, SimTK_INFO_);
+
+
+extern void dlarrj_(SimTK_FDIM_(n), const double *d, const double *e2, SimTK_I_INPUT_(ifirst), SimTK_I_INPUT_(ilast), SimTK_D_INPUT_(rtol), SimTK_I_INPUT_(offset), double *w, double *werr, double *work, int *iwork, SimTK_D_INPUT_(pivmin), SimTK_D_INPUT_(spdiam), SimTK_INFO_);
+
+extern void dlarrk_( SimTK_FDIM_(n), SimTK_I_INPUT_(iw), SimTK_D_INPUT_(gl), SimTK_D_INPUT_(gu), const double *d, const double *e2, SimTK_D_INPUT_(pivmin), SimTK_D_INPUT_(reltol), SimTK_D_OUTPUT_(w), SimTK_D_OUTPUT_(werr), SimTK_INFO_);
+
+extern void dlarrr_( SimTK_FDIM_(n), const double *d__, double *e, SimTK_INFO_ );
+
+extern void dlarrv_(SimTK_FDIM_(n), SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), double *d__, double *l, SimTK_D_INPUT_(pivmin), const int *isplit, SimTK_FDIM_(m), SimTK_I_INPUT_(dol), SimTK_I_INPUT_(dou), SimTK_D_INPUT_(minrgp), SimTK_D_INPUT_(rtol1), SimTK_D_INPUT_(rtol2), double *w, double *werr, double *wgap, const int *iblock, const int *indexw, const double *gers, double *z__, SimTK_FDIM_(ldz), int *isuppz, double *work, int *iwork, SimTK_INFO_);
+
+extern void dlartg_(const double *f, const double *g, double *cs, double *sn, double *r__);
+
+extern void dlartv_(SimTK_FDIM_(n), double *x, SimTK_FINC_(x), double *y, SimTK_FINC_(y), const double *c__, const double *s, int *incc);
+
+extern void dlaruv_(int *iseed, SimTK_FDIM_(n), double *x);
+
+extern void dlarz_(SimTK_FOPT_(side), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_I_INPUT_(L), const double *v, SimTK_FINC_(v), SimTK_D_INPUT_(tau), double *c__, SimTK_FDIM_(ldc), double *work, SimTK_FLEN_(side));
+
+extern void dlarzb_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FOPT_(direct), SimTK_FOPT_(storev), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(l), const double *v, SimTK_FDIM_(ldv), const double *t, SimTK_FDIM_(ldt), double *c__, SimTK_FDIM_(ldc), double *work, SimTK_FDIM_(ldwork), SimTK_FLEN_(side), SimTK_FLEN_(trans), SimTK_FLEN_(direct), SimTK_FLEN_(storev));
+
+extern void dlarzt_(SimTK_FOPT_(direct), SimTK_FOPT_(storev), SimTK_FDIM_(n), SimTK_FDIM_(k), double *v, SimTK_FDIM_(ldv), const double *tau, double *t, SimTK_FDIM_(ldt), SimTK_FLEN_(direct), SimTK_FLEN_(storev));
+
+extern void dlas2_(double *f, double *g, double *h__, SimTK_D_OUTPUT_(ssmin), double *ssmax);
+
+extern void dlascl_(SimTK_FOPT_(type__), SimTK_FDIM_(kl), SimTK_FDIM_(ku), const double *cfrom, const double *cto, SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(type));
+
+extern void dlasd0_(SimTK_FDIM_(n), SimTK_I_INPUT_(sqre), double *d__, double *e, double *u, SimTK_FDIM_(ldu), double *vt, SimTK_FDIM_(ldvt), SimTK_I_INPUT_(smlsiz), int *iwork, double *work, SimTK_INFO_);
+
+extern void dlasd1_(SimTK_I_INPUT_(nl), SimTK_I_INPUT_(nr), SimTK_I_INPUT_(sqre), double *d__, SimTK_D_INPUT_(alpha), SimTK_D_INPUT_(beta), double *u, SimTK_FDIM_(ldu), double *vt, SimTK_FDIM_(ldvt), int *idxq, int *iwork, double *work, SimTK_INFO_);
+
+extern void dlasd2_(SimTK_I_INPUT_(nl), SimTK_I_INPUT_(nr), SimTK_I_INPUT_(sqre), SimTK_I_OUTPUT_(k), double *d__, double *z__, SimTK_D_INPUT_(alpha), double *beta, double *u, SimTK_FDIM_(ldu), double *vt, SimTK_FDIM_(ldvt), double *dsigma, double *u2, SimTK_FDIM_(ldu2), double *vt2, SimTK_FDIM_(ldvt2), int *idxp, int *idx, int *idxc, int *idxq, int *coltyp, SimTK_INFO_);
+
+extern void dlasd3_(SimTK_I_INPUT_(nl), SimTK_I_INPUT_(nr), SimTK_I_INPUT_(sqre), SimTK_I_INPUT_(k), double *d__, double *q, SimTK_FDIM_(ldq), const double *dsigma, const double *u, SimTK_FDIM_(ldu), const double *u2, SimTK_FDIM_(ldu2), const double *vt, SimTK_FDIM_(ldvt), const double *vt2, SimTK_FDIM_(ldvt2), const int *idxc, const int *ctot, const double *z__, SimTK_INFO_);
+
+extern void dlasd4_(SimTK_FDIM_(n), SimTK_I_INPUT_(i__), const double *d__, const double *z__, double *delta, SimTK_D_INPUT_(rho), SimTK_D_OUTPUT_(sigma), double *work, SimTK_INFO_);
+
+extern void dlasd5_(SimTK_I_INPUT_(i__), const double *d__, const double *z__, double *delta, SimTK_D_INPUT_(rho), SimTK_D_OUTPUT_(dsigma), double *work);
+
+extern void dlasd6_(SimTK_I_INPUT_(icompq), SimTK_I_INPUT_(nl), SimTK_I_INPUT_(nr), SimTK_I_INPUT_(sqre), double *d__, double *vf, double *vl, SimTK_D_INPUT_(alpha), SimTK_D_INPUT_(beta), int *idxq, int *perm, SimTK_I_OUTPUT_(givptr), SimTK_I_OUTPUT_(givcol), SimTK_FDIM_(ldgcol), double *givnum, SimTK_FDIM_(ldgnum), double *poles, double *difl, double *difr, double *z__, SimTK_I_OUTPUT_(k), SimTK_D_OUTPUT_(c__), SimTK_D_OUTPUT_(s), double *work, int *iwork, SimTK_INFO_);
+
+extern void dlasd7_(SimTK_I_INPUT_(icompq), SimTK_I_INPUT_(nl), SimTK_I_INPUT_(nr), SimTK_I_INPUT_(sqre), SimTK_I_OUTPUT_(k), double *d__, double *z__, double *zw, double *vf, double *vfw, double *vl, double *vlw, SimTK_D_INPUT_(alpha), SimTK_D_INPUT_(beta), double *dsigma, int *idx, int *idxp, const int *idxq, int *perm, SimTK_I_OUTPUT_(givptr), int *givcol, SimTK_FDIM_(LDGCOL), double *givnum, SimTK_FDIM_(ldgnum), SimTK_D_OUTPUT_(c__), SimTK_D_OUTPUT_(s), SimTK_INFO_);
+
+extern void dlasd8_(SimTK_I_INPUT_(icompq), SimTK_FDIM_(k), double *d__, const double *z__, double *vf, double *vl, double *difl, double *difr, SimTK_FDIM_(lddifr), const double *dsigma, double *work, SimTK_INFO_);
+
+extern void dlasd9_(SimTK_I_INPUT_(icompq), SimTK_FDIM_(ldu), SimTK_FDIM_(k), double *d__, const double *z__, double *vf, double *vl, double *difl, double *difr, double *dsigma, double *work, SimTK_INFO_);
+
+extern void dlasda_(SimTK_I_INPUT_(icompq), SimTK_I_INPUT_(smlsiz), SimTK_FDIM_(n), SimTK_I_INPUT_(sqre), double *d__, double *e, double *u, SimTK_FDIM_(ldu), double *vt, SimTK_FDIM_(k), double *difl, double *difr, double *z__, double *poles, int *givptr, int *givcol, SimTK_FDIM_(ldgcol), int *perm, double *givnum, double *c__, double *s, double *work, int *iwork, SimTK_INFO_);
+
+extern void dlasdq_(SimTK_FOPT_(uplo), SimTK_I_INPUT_(sqre), SimTK_FDIM_(n), SimTK_I_INPUT_(ncvt), SimTK_I_INPUT_(nru), SimTK_I_INPUT_(ncc), double *d__, double *e, double *vt, SimTK_FDIM_(ldvt), double *u, SimTK_FDIM_(ldu), double *c__, SimTK_FDIM_(ldc), double *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dlasdt_(SimTK_FDIM_(n), SimTK_I_OUTPUT_(lvl), SimTK_I_OUTPUT_(nd), int *inode, int *ndiml, int *ndimr, SimTK_I_INPUT_(msub) );
+
+extern void dlaset_(SimTK_FOPT_(uplo), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_D_INPUT_(alpha), SimTK_D_INPUT_(beta), double *a, SimTK_FDIM_(lda), SimTK_FLEN_(uplo));
+
+extern void dlasq1_(SimTK_FDIM_(n), double *d__, double *e, double *work, SimTK_INFO_);
+
+extern void dlasq2_(SimTK_FDIM_(n), double *z__, SimTK_INFO_);
+
+extern void dlasq3_(SimTK_I_INPUT_(i0), SimTK_I_INPUT_(n0), const double *z__, SimTK_I_INPUT_(pp), SimTK_D_OUTPUT_(dmin__), SimTK_D_OUTPUT_(sigma), SimTK_D_OUTPUT_(desig), SimTK_D_INPUT_(qmax), SimTK_I_OUTPUT_(nfail), SimTK_I_OUTPUT_(iter), SimTK_I_OUTPUT_(ndiv), SimTK_I_INPUT_(ieee)) ;
+
+extern void dlasq4_(SimTK_I_INPUT_(i0), SimTK_I_INPUT_(n0), const double *z__, SimTK_I_INPUT_(pp), SimTK_I_INPUT_(n0in), SimTK_D_INPUT_(dmin), SimTK_D_INPUT_(dmin1), SimTK_D_INPUT_(dmin2), SimTK_D_INPUT_(dn), SimTK_D_INPUT_(dn1), SimTK_D_INPUT_(dn2), SimTK_D_OUTPUT_(tau), SimTK_I_OUTPUT_(ttype) );
+
+extern void dlasq5_(SimTK_I_INPUT_(i0), SimTK_I_INPUT_(n0), const double *z__, SimTK_I_INPUT_(pp), SimTK_D_INPUT_(tau), SimTK_D_OUTPUT_(dmin), SimTK_D_OUTPUT_(dmin1), SimTK_D_OUTPUT_(dmin2), SimTK_D_OUTPUT_(dn), SimTK_D_OUTPUT_(dnm1), SimTK_D_OUTPUT_(dnm2), SimTK_I_INPUT_(ieee) );
+
+extern void dlasq6_(SimTK_I_INPUT_(i0), SimTK_I_INPUT_(n0), const double *z__, SimTK_I_INPUT_(pp), SimTK_D_OUTPUT_(dmin), SimTK_D_OUTPUT_(dmin1), SimTK_D_OUTPUT_(dmin2), SimTK_D_OUTPUT_(dn), SimTK_D_OUTPUT_(dnm1), SimTK_D_OUTPUT_(dnm2));
+
+extern void dlasr_(SimTK_FOPT_(side), SimTK_FOPT_(pivot), SimTK_FOPT_(direct), SimTK_FDIM_(m), SimTK_FDIM_(n), const double *c__, const double *s, double *a, SimTK_FDIM_(lda), SimTK_FLEN_(side), SimTK_FLEN_(pivot), SimTK_FLEN_(direct));
+
+extern void dlasrt_(SimTK_FOPT_(id), SimTK_FDIM_(n), double *d__, SimTK_INFO_, SimTK_FLEN_(id));
+
+extern void dlassq_(SimTK_FDIM_(n), const double *x, SimTK_FINC_(x), SimTK_D_OUTPUT_(scale), double *sumsq);
+
+extern void dlasv2_( SimTK_D_INPUT_(f), SimTK_D_INPUT_(g), SimTK_D_INPUT_(h__), SimTK_D_OUTPUT_(ssmin), SimTK_D_OUTPUT_(ssmax) , SimTK_D_OUTPUT_(snr) , SimTK_D_OUTPUT_(csr) , SimTK_D_OUTPUT_(snl) , SimTK_D_OUTPUT_(csl)) ;
+
+extern void dlaswp_(SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), SimTK_I_OUTPUT_(k1), SimTK_I_OUTPUT_(k2), const int *ipiv, SimTK_FINC_(x));
+
+extern void dlasy2_(SimTK_I_INPUT_(ltranl), SimTK_I_INPUT_(ltranr), SimTK_I_INPUT_(isgn), SimTK_I_INPUT_(n1), SimTK_I_INPUT_(n2), const double *tl, SimTK_FDIM_(ldtl), const double *tr, SimTK_FDIM_(ldtr), const double *b, SimTK_FDIM_(ldb), SimTK_D_OUTPUT_(scale), double *x, SimTK_FDIM_(ldx), double *xnorm, SimTK_INFO_);
+
+extern void dlasyf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_I_INPUT_(nb), SimTK_I_INPUT_(kb), double *a, SimTK_FDIM_(lda), int *ipiv, double *w, SimTK_FDIM_(ldw), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dlatbs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FOPT_(normin), SimTK_FDIM_(n), SimTK_I_INPUT_(kd), const double *ab, SimTK_FDIM_(ldab), double *x, SimTK_D_OUTPUT_(scale), double *cnorm, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag), SimTK_FLEN_(normin));
+
+extern void dlatdf_(SimTK_I_INPUT_(ijob), SimTK_FDIM_(n), const double *z__, SimTK_FDIM_(ldz), double *rhs, SimTK_D_OUTPUT_(rdsum), SimTK_D_OUTPUT_(rdscal), const int *ipiv, const int *jpiv);
+
+extern void dlatps_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FOPT_(normin), SimTK_FDIM_(n), const double *ap, double *x, SimTK_D_OUTPUT_(scale), double *cnorm, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag), SimTK_FLEN_(normin));
+
+extern void dlatrd_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nb), double *a, SimTK_FDIM_(lda), double *e, double *tau, double *w, SimTK_FDIM_(ldw), SimTK_FLEN_(uplo));
+
+extern void dlatrs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FOPT_(normin), SimTK_FDIM_(n), const double *a, SimTK_FDIM_(lda), double *x, SimTK_D_OUTPUT_(scale), double *cnorm, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag), SimTK_FLEN_(normin) );
+
+extern void dlatrz_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(l), double *a, SimTK_FDIM_(lda), double *tau, double *work);
+
+extern void dlatzm_(SimTK_FOPT_(side), SimTK_FDIM_(m), SimTK_FDIM_(n), const double *v, SimTK_FINC_(v), SimTK_D_INPUT_(tau), double *c1, double *c2, SimTK_FDIM_(ldc), double *work, SimTK_FLEN_(side));
+
+extern void dlauu2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dlauum_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dlazq3( SimTK_I_INPUT_(io), SimTK_I_INPUT_(no), const double *z, SimTK_I_INPUT_(pp), SimTK_D_OUTPUT_(dmin), SimTK_D_OUTPUT_(sigma), SimTK_D_OUTPUT_(desig), SimTK_D_INPUT_(qmax), SimTK_I_OUTPUT_(nfail), SimTK_I_OUTPUT_(iter), SimTK_I_OUTPUT_(ndiv), SimTK_I_INPUT_(ieee), SimTK_I_INPUT_(ttype), SimTK_D_OUTPUT_(dmin1), SimTK_D_OUTPUT_(dmin2), SimTK_D_OUTPUT_(dn), SimTK_D_OUTPUT_(dn1), SimTK_D_OUTPUT_(dn2), SimTK_D_OUTPUT_(tau) );
+
+extern void dlazq4( SimTK_I_INPUT_(io), SimTK_I_INPUT_(no), const double *z, SimTK_I_INPUT_(pp), SimTK_I_INPUT_(noin), SimTK_D_INPUT_(dmin), SimTK_D_INPUT_(dmin1), SimTK_D_INPUT_(dmin2), SimTK_D_INPUT_(dn), SimTK_D_INPUT_(dn1), SimTK_D_INPUT_(dn2), SimTK_D_OUTPUT_(tau), SimTK_I_OUTPUT_(ttype), SimTK_D_OUTPUT_(g) );
+
+extern void dopgtr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *ap, double *tau, double *q, SimTK_FDIM_(ldq), double *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dopmtr_(SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), double *ap, double *tau, double *c__, SimTK_FDIM_(ldc), double *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(uplo), SimTK_FLEN_(trans));
+
+extern void dorg2l_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_INFO_);
+
+extern void dorg2r_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_INFO_);
+
+extern void dorgbr_(SimTK_FOPT_(vect), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(vect));
+
+extern void dorghr_(SimTK_FDIM_(n), SimTK_FDIM_(ilo), SimTK_FDIM_(ihi), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dorgl2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_INFO_);
+
+extern void dorglq_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dorgql_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dorgqr_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dorgr2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_INFO_);
+
+extern void dorgrq_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dorgtr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dorm2l_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), double *a, SimTK_FDIM_(lda), double *tau, double *c__, SimTK_FDIM_(ldc), double *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void dorm2r_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), double *a, SimTK_FDIM_(lda), double *tau, double *c__, SimTK_FDIM_(ldc), double *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void dormbr_(SimTK_FOPT_(vect), SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), double *a, SimTK_FDIM_(lda), double *tau, double *c__, SimTK_FDIM_(ldc), double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(vect), SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void dormhr_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(ilo), SimTK_FDIM_(ihi), double *a, SimTK_FDIM_(lda), double *tau, double *c__, SimTK_FDIM_(ldc), double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void dorml2_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), double *a, SimTK_FDIM_(lda), double *tau, double *c__, SimTK_FDIM_(ldc), double *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void dormlq_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), double *a, SimTK_FDIM_(lda), double *tau, double *c__, SimTK_FDIM_(ldc), double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void dormql_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), double *a, SimTK_FDIM_(lda), double *tau, double *c__, SimTK_FDIM_(ldc), double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void dormqr_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), double *a, SimTK_FDIM_(lda), double *tau, double *c__, SimTK_FDIM_(ldc), double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void dormr2_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), double *a, SimTK_FDIM_(lda), double *tau, double *c__, SimTK_FDIM_(ldc), double *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void dormr3_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), int *l, double *a, SimTK_FDIM_(lda), double *tau, double *c__, SimTK_FDIM_(ldc), double *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void dormrq_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), double *a, SimTK_FDIM_(lda), double *tau, double *c__, SimTK_FDIM_(ldc), double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void dormrz_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(l), double *a, SimTK_FDIM_(lda), double *tau, double *c__, SimTK_FDIM_(ldc), double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void dormtr_(SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *tau, double *c__, SimTK_FDIM_(ldc), double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(uplo), SimTK_FLEN_(trans));
+
+extern void dpbcon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), const double *ab, SimTK_FDIM_(ldab), SimTK_D_INPUT_(anorm), SimTK_D_OUTPUT_(rcond), double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dpbequ_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_I_INPUT_(kd), const double *ab, SimTK_FDIM_(ldab), SimTK_D_OUTPUT_(s), SimTK_D_OUTPUT_(scond), SimTK_D_OUTPUT_(amax), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dpbrfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), const double *ab, SimTK_FDIM_(ldab), const double *afb, SimTK_FDIM_(ldafb), const double *b, SimTK_FDIM_(ldb), double *x, SimTK_FDIM_(ldx), double *ferr, double *berr, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dpbstf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), double *ab, SimTK_FDIM_(ldab), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dpbsv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), double *ab, SimTK_FDIM_(ldab), double *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dpbsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), double *ab, SimTK_FDIM_(ldab), double *afb, SimTK_FDIM_(ldafb), SimTK_CHAR_OUTPUT_(equed), double *s, double *b, SimTK_FDIM_(ldb), double *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void dpbtf2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), double *ab, SimTK_FDIM_(ldab), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dpbtrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), double *ab, SimTK_FDIM_(ldab), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dpbtrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), const double *ab, SimTK_FDIM_(ldab), double *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dpocon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const double *a, SimTK_FDIM_(lda), SimTK_D_INPUT_(anorm), SimTK_D_OUTPUT_(rcond), double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dpoequ_(SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *s, SimTK_D_OUTPUT_(scond), SimTK_D_OUTPUT_(amax), SimTK_INFO_);
+
+extern void dporfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *a, SimTK_FDIM_(lda), const double *af, SimTK_FDIM_(ldaf), const double *b, SimTK_FDIM_(ldb), double *x, SimTK_FDIM_(ldx), double *ferr, double *berr, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dposv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dposvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *a, SimTK_FDIM_(lda), double *af, SimTK_FDIM_(ldaf), SimTK_CHAR_OUTPUT_(equed), double *s, double *b, SimTK_FDIM_(ldb), double *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void dpotf2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dpotrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dpotri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dpotrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dppcon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const double *ap, SimTK_D_INPUT_(anorm), SimTK_D_OUTPUT_(rcond), double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dppequ_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const double *ap, double *s, SimTK_D_OUTPUT_(scond), SimTK_D_OUTPUT_(amax), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dpprfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *ap, const double *afp, const double *b, SimTK_FDIM_(ldb), double *x, SimTK_FDIM_(ldx), double *ferr, double *berr, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dppsv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *ap, double *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dppsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *ap, double *afp, SimTK_CHAR_OUTPUT_(equed), double *s, double *b, SimTK_FDIM_(ldb), double *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void dpptrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *ap, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dpptri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *ap, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dpptrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *ap, double *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dptcon_(SimTK_FDIM_(n), const double *d__, const double *e, SimTK_D_INPUT_(anorm), SimTK_D_OUTPUT_(rcond), double *work, SimTK_INFO_);
+
+extern void dpteqr_(SimTK_FOPT_(compz), SimTK_FDIM_(n), double *d__, double *e, double *z__, SimTK_FDIM_(ldz), double *work, SimTK_INFO_, SimTK_FLEN_(compz));
+
+extern void dptrfs_(SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *d__, const double *e, const double *df, const double *ef, const double *b, SimTK_FDIM_(ldb), double *x, SimTK_FDIM_(ldx), double *ferr, double *berr, double *work, SimTK_INFO_);
+
+extern void dptsv_(SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *d__, double *e, double *b, SimTK_FDIM_(ldb), SimTK_INFO_);
+
+extern void dptsvx_(SimTK_FOPT_(fact), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *d__, const double *e, double *df, double *ef, const double *b, SimTK_FDIM_(ldb), double *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, double *work, SimTK_INFO_, SimTK_FLEN_(fact));
+
+extern void dpttrf_(SimTK_FDIM_(n), double *d__, double *e, SimTK_INFO_);
+
+extern void dpttrs_(SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *d__, const double *e, double *b, SimTK_FDIM_(ldb), SimTK_INFO_);
+
+extern void dptts2_(SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *d__, const double *e, double *b, SimTK_FDIM_(ldb));
+
+extern void drscl_(SimTK_FDIM_(n), double *sa, double *sx, SimTK_FINC_(x));
+
+extern void dsbev_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), double *ab, SimTK_FDIM_(ldab), double *w, double *z__, SimTK_FDIM_(ldz), double *work, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void dsbevd_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), double *ab, SimTK_FDIM_(ldab), double *w, double *z__, SimTK_FDIM_(ldz), double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void dsbevx_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), double *ab, SimTK_FDIM_(ldab), double *q, SimTK_FDIM_(ldq), SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_D_INPUT_(abstol), SimTK_I_OUTPUT_(m), double *w, double *z__, SimTK_FDIM_(ldz), double *work, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void dsbgst_(SimTK_FOPT_(vect), SimTK_FOPT_(uplo), SimTK_FDIM_(n), int *ka, int *kb, double *ab, SimTK_FDIM_(ldab), double *bb, SimTK_FDIM_(ldbb), double *x, SimTK_FDIM_(ldx), double *work, SimTK_INFO_, SimTK_FLEN_(vect), SimTK_FLEN_(uplo));
+
+extern void dsbgv_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), int *ka, int *kb, double *ab, SimTK_FDIM_(ldab), double *bb, SimTK_FDIM_(ldbb), double *w, double *z__, SimTK_FDIM_(ldz), double *work, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void dsbgvd_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), int *ka, int *kb, double *ab, SimTK_FDIM_(ldab), double *bb, SimTK_FDIM_(ldbb), double *w, double *z__, SimTK_FDIM_(ldz), double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void dsbgvx_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), int *ka, int *kb, double *ab, SimTK_FDIM_(ldab), double *bb, SimTK_FDIM_(ldbb), double *q, SimTK_FDIM_(ldq), SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_D_INPUT_(abstol), SimTK_I_OUTPUT_(m), double *w, double *z__, SimTK_FDIM_(ldz), double *work, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void dsbtrd_(SimTK_FOPT_(vect), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), double *ab, SimTK_FDIM_(ldab), double *d__, double *e, double *q, SimTK_FDIM_(ldq), double *work, SimTK_INFO_, SimTK_FLEN_(vect), SimTK_FLEN_(uplo));
+
+extern void dsgesv_( SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *a, SimTK_FDIM_(lda), int *ipiv, const double *b, SimTK_FDIM_(ldb), double *x, SimTK_FDIM_(ldx), double *work, float *swork, SimTK_I_OUTPUT_(iter), SimTK_INFO_ );
+
+extern void dspcon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const double *ap, const int *ipiv, SimTK_D_INPUT_(anorm), SimTK_D_OUTPUT_(rcond), double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dspev_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *ap, double *w, double *z__, SimTK_FDIM_(ldz), double *work, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void dspevd_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *ap, double *w, double *z__, SimTK_FDIM_(ldz), double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void dspevx_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *ap, SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_D_INPUT_(abstol), SimTK_I_OUTPUT_(m), double *w, double *z__, SimTK_FDIM_(ldz), double *work, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void dspgst_(SimTK_I_INPUT_(itype), SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *ap, double *bp, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dspgv_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *ap, double *bp, double *w, double *z__, SimTK_FDIM_(ldz), double *work, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void dspgvd_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *ap, double *bp, double *w, double *z__, SimTK_FDIM_(ldz), double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void dspgvx_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *ap, double *bp, SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_D_INPUT_(abstol), SimTK_I_OUTPUT_(m), double *w, double *z__, SimTK_FDIM_(ldz), double *work, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void dsprfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *ap, const double *afp, const int *ipiv, const double *b, SimTK_FDIM_(ldb), double *x, SimTK_FDIM_(ldx), double *ferr, double *berr, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dspsv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *ap, int *ipiv, double *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dspsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *ap, double *afp, int *ipiv, const double *b, SimTK_FDIM_(ldb), double *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(face), SimTK_FLEN_(uplo));
+
+extern void dsptrd_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *ap, double *d__, double *e, double *tau, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dsptrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *ap, int *ipiv, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dsptri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *ap, const int *ipiv, double *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dsptrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *ap, const int *ipiv, double *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dstebz_(SimTK_FOPT_(range), SimTK_FOPT_(order), SimTK_FDIM_(n), SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_D_INPUT_(abstol), const double *d__, const double *e, SimTK_FDIM_(m), SimTK_I_OUTPUT_(nsplit), double *w, int *iblock, int *isplit, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(range), SimTK_FLEN_(order));
+
+extern void dstedc_(SimTK_FOPT_(compz), SimTK_FDIM_(n), double *d__, double *e, double *z__, SimTK_FDIM_(ldz), double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(compz));
+
+extern void dstegr_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FDIM_(n), double *d__, double *e, SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_D_INPUT_(abstol), SimTK_I_OUTPUT_(m), double *w, double *z__, SimTK_FDIM_(ldz), int *isuppz, double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range));
+
+extern void dstein_(SimTK_FDIM_(n), const double *d__, const double *e, SimTK_FDIM_(m), const double *w, const int *iblock, const int *isplit, double *z__, SimTK_FDIM_(ldz), double *work, int *iwork, int *ifail, SimTK_INFO_);
+
+extern void dstemr_( SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FDIM_(n), double *d, double *e, SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_I_OUTPUT_(m), double *w, double *z, SimTK_FDIM_(ldz), SimTK_I_INPUT_(nzc), SimTK_I_OUTPUT_(isuppz), SimTK_I_OUTPUT_(tryrac), double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_FDIM_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range) );
+
+extern void dsteqr_(SimTK_FOPT_(compz), SimTK_FDIM_(n), double *d__, double *e, double *z__, SimTK_FDIM_(ldz), double *work, SimTK_INFO_, SimTK_FLEN_(compz));
+
+extern void dsterf_(SimTK_FDIM_(n), double *d__, double *e, SimTK_INFO_);
+
+extern void dstev_(SimTK_FOPT_(jobz), SimTK_FDIM_(n), double *d__, double *e, double *z__, SimTK_FDIM_(ldz), double *work, SimTK_INFO_, SimTK_FLEN_(jobz));
+
+extern void dstevd_(SimTK_FOPT_(jobz), SimTK_FDIM_(n), double *d__, double *e, double *z__, SimTK_FDIM_(ldz), double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz));
+
+extern void dstevr_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FDIM_(n), double *d__, double *e, SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_D_INPUT_(abstol), SimTK_I_OUTPUT_(m), double *w, double *z__, SimTK_FDIM_(ldz), int *isuppz, double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range));
+
+extern void dstevx_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FDIM_(n), double *d__, double *e, SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_D_INPUT_(abstol), SimTK_I_OUTPUT_(m), double *w, double *z__, SimTK_FDIM_(ldz), double *work, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range));
+
+extern void dsycon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const double *a, SimTK_FDIM_(lda), const int *ipiv, SimTK_D_INPUT_(anorm), SimTK_D_OUTPUT_(rcond), double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dsyev_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *w, double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void dsyevd_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *w, double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void dsyevr_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_D_INPUT_(abstol), SimTK_I_OUTPUT_(m), double *w, double *z__, SimTK_FDIM_(ldz), int *isuppz, double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void dsyevx_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_D_INPUT_(abstol), SimTK_I_OUTPUT_(m), double *w, double *z__, SimTK_FDIM_(ldz), double *work, SimTK_FDIM_(lwork), int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void dsygs2_(SimTK_I_INPUT_(itype), SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dsygst_(SimTK_I_INPUT_(itype), SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dsygv_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), double *w, double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void dsygvd_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), double *w, double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void dsygvx_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_D_INPUT_(abstol), SimTK_I_OUTPUT_(m), double *w, double *z__, SimTK_FDIM_(ldz), double *work, SimTK_FDIM_(lwork), int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void dsyrfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *a, SimTK_FDIM_(lda), const double *af, SimTK_FDIM_(ldaf), const int *ipiv, const double *b, SimTK_FDIM_(ldb), double *x, SimTK_FDIM_(ldx), double *ferr, double *berr, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dsysv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *a, SimTK_FDIM_(lda), int *ipiv, double *b, SimTK_FDIM_(ldb), double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dsysvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *a, SimTK_FDIM_(lda), double *af, SimTK_FDIM_(ldaf), int *ipiv, const double *b, SimTK_FDIM_(ldb), double *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo));
+
+extern void dsytd2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *d__, double *e, double *tau, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dsytf2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), int *ipiv, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dsytrd_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *d__, double *e, double *tau, double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dsytrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), int *ipiv, double *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dsytri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const double *a, SimTK_FDIM_(lda), const int *ipiv, double *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dsytrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *a, SimTK_FDIM_(lda), int *ipiv, double *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void dtbcon_(SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(kd), const double *ab, SimTK_FDIM_(ldab), SimTK_D_OUTPUT_(rcond), double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void dtbrfs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), const double *ab, SimTK_FDIM_(ldab), const double *b, SimTK_FDIM_(ldb), const double *x, SimTK_FDIM_(ldx), double *ferr, double *berr, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void dtbtrs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), const double *ab, SimTK_FDIM_(ldab), const double *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void dtgevc_(SimTK_FOPT_(side), SimTK_FOPT_(howmny), const int *select, SimTK_FDIM_(n), const double *a, SimTK_FDIM_(lda), const double *b, SimTK_FDIM_(ldb), double *vl, SimTK_FDIM_(ldvl), double *vr, SimTK_FDIM_(ldvr), SimTK_I_INPUT_(mm), SimTK_FDIM_(m), double *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(howmny));
+
+extern void dtgex2_(SimTK_I_INPUT_(wantq), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), double *q, SimTK_FDIM_(ldq), double *z__, SimTK_FDIM_(ldz), SimTK_I_INPUT_(j1), SimTK_I_INPUT_(n1), SimTK_I_INPUT_(n2), double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dtgexc_(SimTK_I_INPUT_(wantq), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), double *q, SimTK_FDIM_(ldq), double *z__, SimTK_FDIM_(ldz), SimTK_I_OUTPUT_(ifst), SimTK_I_OUTPUT_(ilst), double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void dtgsen_(SimTK_I_INPUT_(ijob), SimTK_I_INPUT_(wantq), SimTK_I_INPUT_(wantz), const int *select, SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), double *alphar, double *alphai, double *beta, double *q, SimTK_FDIM_(ldq), double *z__, SimTK_FDIM_(ldz), SimTK_I_OUTPUT_(m), SimTK_D_OUTPUT_(pl), SimTK_D_OUTPUT_(pr), double *dif, double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_);
+
+extern void dtgsja_(SimTK_FOPT_(jobu), SimTK_FOPT_(jobv), SimTK_FOPT_(jobq), SimTK_FDIM_(m), SimTK_FDIM_(p), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(l), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), SimTK_D_INPUT_(tola), SimTK_D_INPUT_(tolb), double *alpha, double *beta, double *u, SimTK_FDIM_(ldu), double *v, SimTK_FDIM_(ldv), double *q, SimTK_FDIM_(ldq), double *work, SimTK_I_OUTPUT_(ncycle), SimTK_INFO_, SimTK_FLEN_(jobu), SimTK_FLEN_(jobv), SimTK_FLEN_(jobq));
+
+extern void dtgsna_(SimTK_FOPT_(job), SimTK_FOPT_(howmny), const int *select, SimTK_FDIM_(n), const double *a, SimTK_FDIM_(lda), const double *b, SimTK_FDIM_(ldb), const double *vl, SimTK_FDIM_(ldvl), const double *vr, SimTK_FDIM_(ldvr), double *s, double *dif, SimTK_FDIM_(mm), SimTK_FDIM_(m), double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(howmny));
+
+extern void dtgsy2_(SimTK_FOPT_(trans), SimTK_I_INPUT_(ijob), SimTK_FDIM_(m), SimTK_FDIM_(n), const double *a, SimTK_FDIM_(lda), const double *b, SimTK_FDIM_(ldb), double *c__, SimTK_FDIM_(ldc), const double *d__, SimTK_FDIM_(ldd), const double *e, SimTK_FDIM_(lde), double *f, SimTK_FDIM_(ldf), SimTK_D_OUTPUT_(scale), SimTK_D_OUTPUT_(rdsum), SimTK_D_OUTPUT_(rdscal), int *iwork, int *pq, SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void dtgsyl_(SimTK_FOPT_(trans), SimTK_I_INPUT_(ijob), SimTK_FDIM_(m), SimTK_FDIM_(n), const double *a, SimTK_FDIM_(lda), const double *b, SimTK_FDIM_(ldb), double *c__, SimTK_FDIM_(ldc), const double *d__, SimTK_FDIM_(ldd), const double *e, SimTK_FDIM_(lde), double *f, SimTK_FDIM_(ldf), SimTK_D_OUTPUT_(scale), double *dif, double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void dtpcon_(SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), const double *ap, SimTK_D_OUTPUT_(rcond), double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void dtprfs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *ap, const double *b, SimTK_FDIM_(ldb), const double *x, SimTK_FDIM_(ldx), double *ferr, double *berr, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void dtptri_(SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), double *ap, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void dtptrs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *ap, double *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void dtrcon_(SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), const double *a, SimTK_FDIM_(lda), SimTK_D_OUTPUT_(rcond), double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void dtrevc_(SimTK_FOPT_(side), SimTK_FOPT_(howmny), int *select, SimTK_FDIM_(n), double *t, SimTK_FDIM_(ldt), double *vl, SimTK_FDIM_(ldvl), double *vr, SimTK_FDIM_(ldvr), SimTK_FDIM_(mm), SimTK_FDIM_(m), double *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(howmny));
+
+extern void dtrexc_(SimTK_FOPT_(compq), SimTK_FDIM_(n), double *t, SimTK_FDIM_(ldt), double *q, SimTK_FDIM_(ldq), SimTK_I_OUTPUT_(ifst), SimTK_I_OUTPUT_(ilst), double *work, SimTK_INFO_, SimTK_FLEN_(compq));
+
+extern void dtrrfs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *a, SimTK_FDIM_(lda), const double *b, SimTK_FDIM_(ldb), const double *x, SimTK_FDIM_(ldx), double *ferr, double *berr, double *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void dtrsen_(SimTK_FOPT_(job), SimTK_FOPT_(compq), const int *select, SimTK_FDIM_(n), double *t, SimTK_FDIM_(ldt), double *q, SimTK_FDIM_(ldq), double *wr, double *wi, SimTK_FDIM_(m), SimTK_D_OUTPUT_(s), SimTK_D_OUTPUT_(sep), double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(compq));
+
+extern void dtrsna_(SimTK_FOPT_(job), SimTK_FOPT_(howmny), const int *select, SimTK_FDIM_(n), double *t, SimTK_FDIM_(ldt), double *vl, SimTK_FDIM_(ldvl), double *vr, SimTK_FDIM_(ldvr), double *s, double *sep, SimTK_FDIM_(mm), SimTK_FDIM_(m), double *work, SimTK_FDIM_(ldwork), int *iwork, SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(howmny));
+
+extern void dtrsyl_(SimTK_FOPT_(trana), SimTK_FOPT_(tranb), int *isgn, SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), double *c__, SimTK_FDIM_(ldc), SimTK_D_OUTPUT_(scale), SimTK_INFO_, SimTK_FLEN_(trana), SimTK_FLEN_(tranb));
+
+extern void dtrti2_(SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void dtrtri_(SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans));
+
+extern void dtrtrs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *a, SimTK_FDIM_(lda), double *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void dtzrqf_(SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *tau, SimTK_INFO_);
+
+extern void dtzrzf_(SimTK_FDIM_(m), SimTK_FDIM_(n), double *a, SimTK_FDIM_(lda), double *tau, double *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern int icmax1_(SimTK_FDIM_(n), const SimTK_C_ *cx, SimTK_FINC_(x));
+
+extern int ieeeck_(SimTK_FDIM_(ispec), SimTK_S_INPUT_(zero), SimTK_S_INPUT_(one));
+
+extern int ilaenv_(SimTK_FDIM_(ispec), const char *name__, const char *opts, SimTK_FDIM_(n1), SimTK_FDIM_(n2), SimTK_FDIM_(n3), SimTK_FDIM_(n4), SimTK_FLEN_(name), SimTK_FLEN_(opts));
+
+extern void ilaver_( SimTK_I_OUTPUT_(vers_major), SimTK_I_OUTPUT_(vers_minor), SimTK_I_OUTPUT_(vers_patch) );
+extern int iparmq_( SimTK_I_INPUT_(ispec), const char *name, const char *opts, SimTK_I_INPUT_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_I_INPUT_(lwork), SimTK_FLEN_(name), SimTK_FLEN_(opts));
+
+extern int izmax1_(SimTK_FDIM_(n), const SimTK_Z_ *cx, SimTK_FINC_(x));
+
+extern void sbdsdc_(SimTK_FOPT_(uplo), SimTK_FOPT_(compq), SimTK_FDIM_(n), float *d__, float *e, float *u, SimTK_FDIM_(ldu), float *vt, SimTK_FDIM_(ldvt), float *q, int *iq, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(compq));
+
+extern void sbdsqr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(ncvt), SimTK_FDIM_(nru), SimTK_FDIM_(ncc), float *d__, float *e, float *vt, SimTK_FDIM_(ldvt), float *u, SimTK_FDIM_(ldu), float *c__, SimTK_FDIM_(ldc), float *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void sdisna_(SimTK_FOPT_(job), SimTK_FDIM_(m), SimTK_FDIM_(n), const float *d__, float *sep, SimTK_INFO_, SimTK_FLEN_(job));
+
+extern void sgbbrd_(SimTK_FOPT_(vect), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(ncc), SimTK_FDIM_(kl), SimTK_FDIM_(ku), float *ab, SimTK_FDIM_(ldab), float *d__, float *e, float *q, SimTK_FDIM_(ldq), float *pt, SimTK_FDIM_(ldpt), float *c__, SimTK_FDIM_(ldc), float *work, SimTK_INFO_, SimTK_FLEN_(vect));
+
+extern void sgbcon_(SimTK_FOPT_(norm), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), float *ab, SimTK_FDIM_(ldab), const int *ipiv, SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(norm));
+
+extern void sgbequ_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), float *ab, SimTK_FDIM_(ldab), float *r__, float *c__, float *rowcnd, float *colcnd, float *amax, SimTK_INFO_);
+
+extern void sgbrfs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_FDIM_(nrhs), float *ab, SimTK_FDIM_(ldab), float *afb, SimTK_FDIM_(ldafb), const int *ipiv, float *b, SimTK_FDIM_(ldb), float *x, SimTK_FDIM_(ldx), float *ferr, float *berr, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void sgbsv_(SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_FDIM_(nrhs), float *ab, SimTK_FDIM_(ldab), int *ipiv, float *b, SimTK_FDIM_(ldb), SimTK_INFO_);
+
+extern void sgbsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_FDIM_(nrhs), float *ab, SimTK_FDIM_(ldab), float *afb, SimTK_FDIM_(ldafb), int *ipiv, SimTK_CHAR_OUTPUT_(equed), float *r__, float *c__, float *b, SimTK_FDIM_(ldb), float *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(rcond), float *ferr, float *berr, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(trans), SimTK_FLEN_(equed));
+
+extern void sgbtf2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), float *ab, SimTK_FDIM_(ldab), int *ipiv, SimTK_INFO_);
+
+extern void sgbtrf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), float *ab, SimTK_FDIM_(ldab), int *ipiv, SimTK_INFO_);
+
+extern void sgbtrs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_FDIM_(nrhs), float *ab, SimTK_FDIM_(ldab), const int *ipiv, float *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void sgebak_(SimTK_FOPT_(job), SimTK_FOPT_(side), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_S_INPUT_(scale), SimTK_I_INPUT_(m), float *v, SimTK_FDIM_(ldv), SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(side));
+
+extern void sgebal_(SimTK_FOPT_(job), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), SimTK_I_OUTPUT_(ilo), SimTK_I_OUTPUT_(ihi), SimTK_S_OUTPUT_(scale), SimTK_INFO_, SimTK_FLEN_(job));
+
+extern void sgebd2_(SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *d__, float *e, float *tauq, float *taup, float *work, SimTK_INFO_);
+
+extern void sgebrd_(SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *d__, float *e, float *tauq, float *taup, float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void sgecon_(SimTK_FOPT_(norm), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(norm));
+
+extern void sgeequ_(SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *r__, float *c__, float *rowcnd, float *colcnd, float *amax, int *info);
+
+extern void sgees_(SimTK_FOPT_(jobvs), SimTK_FOPT_(sort), SimTK_SELECT_2S select, SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), int *sdim, float *wr, float *wi, float *vs, SimTK_FDIM_(ldvs), float *work, SimTK_FDIM_(lwork), int *bwork, SimTK_INFO_, SimTK_FLEN_(jobvs), SimTK_FLEN_(sort));
+
+extern void sgeesx_(SimTK_FOPT_(jobvs), SimTK_FOPT_(sort), SimTK_SELECT_2S select, char *sense, SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), int *sdim, float *wr, float *wi, float *vs, SimTK_FDIM_(ldvs), SimTK_S_OUTPUT_(rconde), SimTK_S_OUTPUT_(rcondv), float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), int *bwork, SimTK_INFO_, SimTK_FLEN_(jobvs), SimTK_FLEN_(sort), SimTK_FLEN_(sense));
+
+extern void sgeev_(SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *wr, float *wi, float *vl, SimTK_FDIM_(ldvl), float *vr, SimTK_FDIM_(ldvr), float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(jobvl), SimTK_FLEN_(jobvr));
+
+extern void sgeevx_(SimTK_FOPT_(balanc), SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), char *sense, SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *wr, float *wi, float *vl, SimTK_FDIM_(ldvl), float *vr, SimTK_FDIM_(ldvr), SimTK_I_OUTPUT_(ilo), SimTK_I_OUTPUT_(ihi), SimTK_S_OUTPUT_(scale), float *abnrm, SimTK_S_OUTPUT_(rconde), SimTK_S_OUTPUT_(rcondv), float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_INFO_, SimTK_FLEN_(balanc), SimTK_FLEN_(jobvl), SimTK_FLEN_(jobvr), SimTK_FLEN_(alpahr));
+
+extern void sgegs_(SimTK_FOPT_(jobvsl), SimTK_FOPT_(jobvsr), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), float *alphar, float *alphai, float *beta, float *vsl, SimTK_FDIM_(ldvsl), float *vsr, SimTK_FDIM_(ldvsr), float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(jobvl), SimTK_FLEN_(jobvsr));
+
+extern void sgegv_(SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), float *alphar, float *alphai, float *beta, float *vl, SimTK_FDIM_(ldvl), float *vr, SimTK_FDIM_(ldvr), float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(jobvl), SimTK_FLEN_(jobvr));
+
+extern void sgehd2_(SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_INFO_);
+
+extern void sgehrd_(SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void sgelq2_(SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_INFO_);
+
+extern void sgelqf_(SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void sgels_(SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void sgelsd_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), float *s, SimTK_S_INPUT_(rcond), SimTK_I_OUTPUT_(rank), float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_INFO_);
+
+extern void sgelss_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), float *s, SimTK_S_INPUT_(rcond), SimTK_I_OUTPUT_(rank), float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void sgelsx_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), int *jpvt, SimTK_S_INPUT_(rcond), SimTK_I_OUTPUT_(rank), float *work, SimTK_INFO_);
+
+extern void sgelsy_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), int *jpvt, SimTK_S_INPUT_(rcond), SimTK_I_OUTPUT_(rank), float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void sgeql2_(SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_INFO_);
+
+extern void sgeqlf_(SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void sgeqp3_(SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), int *jpvt, float *tau, float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void sgeqpf_(SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), int *jpvt, float *tau, float *work, SimTK_INFO_);
+
+extern void sgeqr2_(SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_INFO_);
+
+extern void sgeqrf_(SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void sgerfs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *a, SimTK_FDIM_(lda), float *af, SimTK_FDIM_(ldaf), const int *ipiv, float *b, SimTK_FDIM_(ldb), float *x, SimTK_FDIM_(ldx), float *ferr, float *berr, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void sgerq2_(SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_INFO_);
+
+extern void sgerqf_(SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void sgesc2_(SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *rhs, int *ipiv, int *jpiv, SimTK_S_OUTPUT_(scale));
+
+extern void sgesdd_(SimTK_FOPT_(jobz), SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *s, float *u, SimTK_FDIM_(ldu), float *vt, SimTK_FDIM_(ldvt), float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_INFO_, SimTK_FLEN_(jobz));
+
+extern void sgesv_(SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *a, SimTK_FDIM_(lda), int *ipiv, float *b, SimTK_FDIM_(ldb), SimTK_INFO_);
+
+extern void sgesvd_(SimTK_FOPT_(jobu), char *jobvt, SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *s, float *u, SimTK_FDIM_(ldu), float *vt, SimTK_FDIM_(ldvt), float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(jobu), SimTK_FLEN_(jobvt));
+
+extern void sgesvx_(SimTK_FOPT_(fact), SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *a, SimTK_FDIM_(lda), float *af, SimTK_FDIM_(ldaf), int *ipiv, SimTK_CHAR_OUTPUT_(equed), float *r__, float *c__, float *b, SimTK_FDIM_(ldb), float *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(rcond), float *ferr, float *berr, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(trans), SimTK_FLEN_(equed));
+
+extern void sgetc2_(SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), int *ipiv, int *jpiv, SimTK_INFO_);
+
+extern void sgetf2_(SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), int *ipiv, SimTK_INFO_);
+
+extern void sgetrf_(SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), int *ipiv, SimTK_INFO_);
+
+extern void sgetri_(SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), const int *ipiv, float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void sgetrs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *a, SimTK_FDIM_(lda), const int *ipiv, float *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void sggbak_(SimTK_FOPT_(job), SimTK_FOPT_(side), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), const float *lscale, const float *rscale, SimTK_FDIM_(m), float *v, SimTK_FDIM_(ldv), SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(side));
+
+extern void sggbal_(SimTK_FOPT_(job), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), SimTK_I_OUTPUT_(ilo), SimTK_I_OUTPUT_(ihi), float *lscale, float *rscale, float *work, SimTK_INFO_, SimTK_FLEN_(job));
+
+extern void sgges_(SimTK_FOPT_(jobvsl), SimTK_FOPT_(jobvsr), SimTK_FOPT_(sort), SimTK_SELECT_3F selctg, SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), int *sdim, float *alphar, float *alphai, SimTK_S_INPUT_(eta), float *vsl, SimTK_FDIM_(ldvsl), float *vsr, SimTK_FDIM_(ldvsr), float *work, SimTK_FDIM_(lwork), int *bwork, SimTK_INFO_, SimTK_FLEN_(jobvsl), SimTK_FLEN_(jobvsr), SimTK_FLEN_(sort));
+
+extern void sggesx_(SimTK_FOPT_(jobvsl), SimTK_FOPT_(jobvsr), SimTK_FOPT_(sort), SimTK_SELECT_3F selctg, SimTK_FOPT_(sense), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), int *sdim, float *alphar, float *alphai, float *beta, float *vsl, SimTK_FDIM_(ldvsl), float *vsr, SimTK_FDIM_(ldvsr), SimTK_S_OUTPUT_(rconde), SimTK_S_OUTPUT_(rcondv), float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_FDIM_(liwork), int *bwork, SimTK_INFO_, SimTK_FLEN_(jobvsl), SimTK_FLEN_(job [...]
+
+extern void sggev_(SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), float *alphar, float *alphai, float *beta, float *vl, SimTK_FDIM_(ldvl), float *vr, SimTK_FDIM_(ldvr), float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(jobvsl), SimTK_FLEN_(jobvsr));
+
+extern void sggevx_(SimTK_FOPT_(balanc), SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), SimTK_FOPT_(sense), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), float *alphar, float *alphai, float *beta, float *vl, SimTK_FDIM_(ldvl), float *vr, SimTK_FDIM_(ldvr), SimTK_I_OUTPUT_(ilo), SimTK_I_OUTPUT_(ihi), float *lscale, float *rscale, SimTK_S_OUTPUT_(abnrm), SimTK_S_OUTPUT_(bbnrm), SimTK_S_OUTPUT_(rconde), SimTK_S_OUTPUT_(rcondv), float *work, SimTK_FDIM_(lwork), int *iwork, [...]
+
+extern void sggglm_(SimTK_FDIM_(n), SimTK_FDIM_(m), SimTK_FDIM_(p), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), float *d__, float *x, float *y, float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void sgghrd_(SimTK_FOPT_(compq), SimTK_FOPT_(compz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), float *q, SimTK_FDIM_(ldq), float *z__, SimTK_FDIM_(ldz), SimTK_INFO_, SimTK_FLEN_(compq), SimTK_FLEN_(compz));
+
+extern void sgglse_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(p), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), float *c__, float *d__, float *x, float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void sggqrf_(SimTK_FDIM_(n), SimTK_FDIM_(m), SimTK_FDIM_(p), float *a, SimTK_FDIM_(lda), float *taua, float *b, SimTK_FDIM_(ldb), float *taub, float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void sggrqf_(SimTK_FDIM_(m), SimTK_FDIM_(p), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *taua, float *b, SimTK_FDIM_(ldb), float *taub, float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void sggsvd_(SimTK_FOPT_(jobu), SimTK_FOPT_(jobv), SimTK_FOPT_(jobq), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(p), SimTK_I_OUTPUT_(k), SimTK_I_OUTPUT_(l), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), float *alpha, float *beta, float *u, SimTK_FDIM_(ldu), float *v, SimTK_FDIM_(ldv), float *q, SimTK_FDIM_(ldq), float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(jobu), SimTK_FLEN_(jobv), SimTK_FLEN_(jobq));
+
+extern void sggsvp_(SimTK_FOPT_(jobu), SimTK_FOPT_(jobv), SimTK_FOPT_(jobq), SimTK_FDIM_(m), SimTK_FDIM_(p), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), SimTK_S_INPUT_(tola), SimTK_S_INPUT_(tolb), SimTK_I_OUTPUT_(k), SimTK_I_OUTPUT_(l), float *u, SimTK_FDIM_(ldu), float *v, SimTK_FDIM_(ldv), float *q, SimTK_FDIM_(ldq), int *iwork, float *tau, float *work, SimTK_INFO_, SimTK_FLEN_(jobu), SimTK_FLEN_(jobv), SimTK_FLEN_(jobq));
+
+extern void sgtcon_(SimTK_FOPT_(norm), SimTK_FDIM_(n), float *dl, float *d__, float *du, float *du2, const int *ipiv, SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(norm));
+
+extern void sgtrfs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *dl, float *d__, float *du, float *dlf, float *df, float *duf, float *du2, const int *ipiv, float *b, SimTK_FDIM_(ldb), float *x, SimTK_FDIM_(ldx), float *ferr, float *berr, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void sgtsv_(SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *dl, float *d__, float *du, float *b, SimTK_FDIM_(ldb), SimTK_INFO_);
+
+extern void sgtsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *dl, float *d__, float *du, float *dlf, float *df, float *duf, float *du2, int *ipiv, float *b, SimTK_FDIM_(ldb), float *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(rcond), float *ferr, float *berr, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(trans));
+
+extern void sgttrf_(SimTK_FDIM_(n), float *dl, float *d__, float *du, float *du2, int *ipiv, SimTK_INFO_);
+
+extern void sgttrs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *dl, float *d__, float *du, float *du2, const int *ipiv, float *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void sgtts2_(int *itrans, SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *dl, float *d__, float *du, float *du2, const int *ipiv, float *b, SimTK_FDIM_(ldb));
+
+extern void shgeqz_(SimTK_FOPT_(job), SimTK_FOPT_(compq), SimTK_FOPT_(compz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), float *alphar, float *alphai, float *beta, float *q, SimTK_FDIM_(ldq), float *z__, SimTK_FDIM_(ldz), float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(compq), SimTK_FLEN_(compz));
+
+extern void shsein_(SimTK_FOPT_(side), SimTK_FOPT_(eigsrc), SimTK_FOPT_(initv), int *select, SimTK_FDIM_(n), const float *h__, SimTK_FDIM_(ldh), float *wr, float *wi, float *vl, SimTK_FDIM_(ldvl), float *vr, SimTK_FDIM_(ldvr), SimTK_FDIM_(mm), SimTK_I_OUTPUT_(m), float *work, int *ifaill, int *ifailr, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(eigsrc), SimTK_FLEN_(initv));
+
+extern float slamch_(SimTK_FOPT_(cmach), SimTK_FLEN_(cmach));
+
+extern void shseqr_(SimTK_FOPT_(job), SimTK_FOPT_(compz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), float *h__, SimTK_FDIM_(ldh), float *wr, float *wi, float *z__, SimTK_FDIM_(ldz), float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(compz));
+
+extern int sisnan( SimTK_S_INPUT_(sin) );
+
+extern int slaisnan( SimTK_S_INPUT_(sin1), SimTK_S_INPUT_(sin2) );
+
+extern void slabad_(SimTK_S_INOUT_(small), SimTK_S_INOUT_(large));
+
+extern void slabrd_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nb), float *a, SimTK_FDIM_(lda), float *d__, float *e, float *tauq, float *taup, float *x, SimTK_FDIM_(ldx), float *y, SimTK_FDIM_(ldy));
+
+extern void slacon_(SimTK_FDIM_(n), float *v, float *x, int *isgn, SimTK_S_OUTPUT_(est), SimTK_I_OUTPUT_(kase) );
+
+extern void slacn2_( SimTK_FDIM_(n), float *v, float *x, int *isgn, SimTK_S_INPUT_(est), SimTK_I_OUTPUT_(kase), int *isave );
+
+extern void slacpy_(SimTK_FOPT_(uplo), SimTK_FDIM_(m), SimTK_FDIM_(n), const float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), SimTK_FLEN_(uplo));
+
+extern void sladiv_(SimTK_S_INPUT_(a), SimTK_S_INPUT_(b), SimTK_S_INPUT_(c__), SimTK_S_INPUT_(d__), float *p, float *q);
+
+extern void slae2_(SimTK_S_INPUT_(a), SimTK_S_INPUT_(b), SimTK_S_INPUT_(c__), float *rt1, float *rt2);
+
+extern void slaebz_(SimTK_I_INPUT_(ijob), SimTK_I_INPUT_(nitmax), SimTK_FDIM_(n), SimTK_I_INPUT_(mmax), SimTK_I_INPUT_(minp), SimTK_I_INPUT_(nbmin), SimTK_S_INPUT_(abstol), SimTK_S_INPUT_(reltol), SimTK_S_INPUT_(pivmin), const float *d__, const float *e, const float *e2, int *nval, float *ab, float *c__, SimTK_I_OUTPUT_(mout), int *nab, float *work, int *iwork, SimTK_INFO_);
+
+extern void slaed0_(SimTK_I_INPUT_(icompq), SimTK_I_INPUT_(qsiz), SimTK_FDIM_(n), float *d__, float *e, float *q, SimTK_FDIM_(ldq), float *qstore, SimTK_FDIM_(ldqs), float *work, int *iwork, SimTK_INFO_);
+
+extern void slaed1_(SimTK_FDIM_(n), float *d__, float *q, SimTK_FDIM_(ldq), int *indxq, SimTK_S_INPUT_(rho), SimTK_I_INPUT_(cutpnt), float *work, int *iwork, SimTK_INFO_);
+
+extern void slaed2_(SimTK_FDIM_(k), SimTK_FDIM_(n), SimTK_I_INPUT_(n1), float *d__, float *q, SimTK_FDIM_(ldq), int *indxq, SimTK_S_INPUT_(rho), float *z__, float *dlamda, float *w, float *q2, int *indx, int *indxc, int *indxp, int *coltyp, SimTK_INFO_);
+
+extern void slaed3_(SimTK_FDIM_(k), SimTK_FDIM_(n), SimTK_I_INPUT_(n1), float *d__, float *q, SimTK_FDIM_(ldq), SimTK_S_INPUT_(rho), float *dlamda, const float *q2, const int *indx, const int *ctot, float *w, float *s, SimTK_INFO_);
+
+extern void slaed4_(SimTK_FDIM_(n), SimTK_I_INPUT_(i__), const float *d__, const float *z__, float *delta, SimTK_S_INPUT_(rho), SimTK_S_OUTPUT_(dlam), SimTK_INFO_);
+
+extern void slaed5_(SimTK_I_INPUT_(i__), const float *d__, const float *z__, float *delta, SimTK_S_INPUT_(rho), SimTK_S_OUTPUT_(dlam));
+
+extern void slaed6_(SimTK_I_INPUT_(kniter), SimTK_I_INPUT_(orgati), SimTK_S_INPUT_(rho), const float *d__, const float *z__, SimTK_S_INPUT_(finit), SimTK_S_INPUT_(tau), SimTK_INFO_);
+
+extern void slaed7_(SimTK_I_INPUT_(icompq), SimTK_FDIM_(n), SimTK_FDIM_(qsiz), SimTK_I_INPUT_(tlvls), SimTK_I_INPUT_(curlvl), SimTK_I_INPUT_(curpbm), float *d__, float *q, SimTK_FDIM_(ldq), int *indxq, SimTK_S_INPUT_(rho), SimTK_I_INPUT_(cutpnt), float *qstore, int *qptr, const int *prmptr, const int *perm, const int *givptr, const int *givcol, const float *givnum, float *work, int *iwork, SimTK_INFO_);
+
+extern void slaed8_(SimTK_I_INPUT_(icompq), SimTK_I_OUTPUT_(k), SimTK_FDIM_(n), SimTK_FDIM_(qsiz), float *d__, float *q, SimTK_FDIM_(ldq), const int *indxq, SimTK_S_OUTPUT_(rho), SimTK_I_INPUT_(cutpnt), float *z__, float *dlamda, float *q2, SimTK_FDIM_(ldq2), float *w, int *perm, int *givptr, int *givcol, float *givnum, int *indxp, int *indx, SimTK_INFO_);
+
+extern void slaed9_(SimTK_FDIM_(k), int *kstart, int *kstop, SimTK_FDIM_(n), float *d__, float *q, SimTK_FDIM_(ldq), float *rho, float *dlamda, float *w, float *s, SimTK_FDIM_(lds), SimTK_INFO_);
+
+extern void slaeda_(SimTK_FDIM_(n), SimTK_I_INPUT_(tlvls), SimTK_I_INPUT_(curlvl), SimTK_I_INPUT_(curpbm), const int *prmptr, const int *perm, const int *givptr, const int *givcol, const float *givnum, const float *q, const int *qptr, float *z__, float *ztemp, SimTK_INFO_);
+
+extern void slaein_( SimTK_I_INPUT_(rightv), SimTK_I_INPUT_(noinit), SimTK_FDIM_(n), const float *h__, SimTK_FDIM_(ldh), SimTK_S_INPUT_(wr), SimTK_S_INPUT_(wi), float *vr, float *vi, float *b, SimTK_FDIM_(ldb), float *work, SimTK_S_INPUT_(eps3), SimTK_S_INPUT_(smlnum), SimTK_S_INPUT_(bignum), SimTK_INFO_);
+
+extern void slaev2_(SimTK_S_INPUT_(a), SimTK_S_INPUT_(b), SimTK_S_INPUT_(c__), SimTK_S_OUTPUT_(rt1), SimTK_S_OUTPUT_(rt2), SimTK_S_OUTPUT_(cs1), SimTK_S_OUTPUT_(sn1));
+
+extern void slaexc_(SimTK_I_INPUT_(wantq), SimTK_FDIM_(n), float *t, SimTK_FDIM_(ldt), float *q, SimTK_FDIM_(ldq), SimTK_I_INPUT_(j1), SimTK_I_INPUT_(n1), SimTK_I_INPUT_(n2), float *work, SimTK_INFO_);
+
+extern void slag2_(const float *a, SimTK_FDIM_(lda), const float *b, SimTK_FDIM_(ldb), SimTK_S_INPUT_(safmin), SimTK_S_OUTPUT_(scale1), SimTK_S_OUTPUT_(scale2), SimTK_S_OUTPUT_(wr1), SimTK_S_OUTPUT_(wr2), SimTK_S_OUTPUT_(wi));
+
+extern void slags2_(SimTK_I_INPUT_(upper), SimTK_S_INPUT_(a1), SimTK_S_INPUT_(a2), SimTK_S_INPUT_(a3), SimTK_S_INPUT_(b1), SimTK_S_INPUT_(b2), SimTK_S_INPUT_(b3), SimTK_S_OUTPUT_(csu), SimTK_S_OUTPUT_(snu), SimTK_S_OUTPUT_(csv), SimTK_S_OUTPUT_(snv), SimTK_S_OUTPUT_(csq), SimTK_S_OUTPUT_(snq) );
+
+extern void slagtf_(SimTK_FDIM_(n), float *a, SimTK_S_INPUT_(lambda), float *b, float *c__, SimTK_S_INPUT_(tol), float *d__, int *in, SimTK_INFO_);
+
+extern void slagtm_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_S_INPUT_(alpha), const float *dl, const float *d__, const float *du, const float *x, SimTK_FDIM_(ldx), SimTK_S_INPUT_(beta), float *b, SimTK_FDIM_(ldb), SimTK_FLEN_(trans));
+
+extern void slag2d_( SimTK_I_INPUT_(m), SimTK_I_INPUT_(n), float *sa, SimTK_FDIM_(ldsa), const double *a, SimTK_FDIM_(lda), SimTK_INFO_);
+
+extern void slagts_(SimTK_I_INPUT_(job), SimTK_FDIM_(n), const float *a, const float *b, const float *c__, const float *d__, const int *in, float *y, SimTK_S_OUTPUT_(tol), SimTK_INFO_);
+
+extern void slagv2_( float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), float *alphar, float *alphai, float *beta, SimTK_S_OUTPUT_(csl), SimTK_S_OUTPUT_(snl), SimTK_S_OUTPUT_(csr), SimTK_S_OUTPUT_(snr));
+
+extern void slahqr_(SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), float *h__, SimTK_FDIM_(ldh), float *wr, float *wi, SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), float *z__, SimTK_FDIM_(ldz), SimTK_INFO_);
+
+extern void slahrd_(SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(nb), float *a, SimTK_FDIM_(lda), float *tau, float *t, SimTK_FDIM_(ldt), float *y, SimTK_FDIM_(ldy));
+
+extern void slahr2_(SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(nb), float *a, SimTK_FDIM_(lda), float *tau, float *t, SimTK_FDIM_(ldt), float *y, SimTK_FDIM_(ldy));
+
+extern void slaic1_(SimTK_I_INPUT_(job), SimTK_FDIM_(j), const float *x, SimTK_S_INPUT_(sest), const float *w, SimTK_S_INPUT_(gamma), SimTK_S_OUTPUT_(sestpr), SimTK_S_OUTPUT_(s), SimTK_S_OUTPUT_(c__) );
+
+extern void slaln2_(SimTK_I_INPUT_(ltrans), SimTK_I_INPUT_(na), SimTK_I_INPUT_(nw), SimTK_S_INPUT_(smin), SimTK_S_INPUT_(ca), const float *a, SimTK_FDIM_(lda), SimTK_S_INPUT_(d1), SimTK_S_INPUT_(d2), SimTK_S_INPUT_(b), SimTK_FDIM_(ldb), SimTK_S_INPUT_(wr), SimTK_S_INPUT_(wi), float *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(scale), float *xnorm, SimTK_INFO_);
+
+extern void slals0_(SimTK_I_INPUT_(icompq), SimTK_I_INPUT_(nl), SimTK_I_INPUT_(nr), SimTK_I_INPUT_(sqre), SimTK_FDIM_(nrhs), float *b, SimTK_FDIM_(ldb), float *bx, SimTK_FDIM_(ldbx), int *perm, int *givptr, const int *givcol, SimTK_FDIM_(ldgcol), float *givnum, SimTK_FDIM_(ldgnum), float *poles, float *difl, float *difr, float *z__, SimTK_FDIM_(k), float *c__, float *s, float *work, SimTK_INFO_);
+
+extern void slalsa_(SimTK_I_INPUT_(icompq), SimTK_I_INPUT_(smlsiz), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *b, SimTK_FDIM_(ldb), float *bx, SimTK_FDIM_(ldbx), float *u, SimTK_FDIM_(ldu), float *vt, SimTK_FDIM_(k), float *difl, float *difr, float *z__, float *poles, SimTK_FDIM_(givptr), const int *givcol, SimTK_FDIM_(ldgcol), int *perm, float *givnum, float *c__, float *s, float *work, int *iwork, SimTK_INFO_);
+
+extern void slalsd_(SimTK_FOPT_(uplo), SimTK_I_INPUT_(smlsiz), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *d__, float *e, float *b, SimTK_FDIM_(ldb), SimTK_S_INPUT_(rcond), SimTK_I_OUTPUT_(rank), float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void slaneg_( SimTK_FDIM_(n), SimTK_S_INPUT_(d), SimTK_S_INPUT_(lld), SimTK_S_INPUT_(sigma), SimTK_S_INPUT_(pivmin), SimTK_I_INPUT_(r) );
+
+extern double slangb_( SimTK_FOPT_(norm), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), const float *ab, SimTK_FDIM_(ldab), float *work, SimTK_FLEN_(norm) );
+
+extern double clangb_( SimTK_FOPT_(norm), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), const SimTK_C_ *ab, SimTK_FDIM_(ldab), float *work, SimTK_FLEN_(norm) );
+
+extern void dlaneg_( SimTK_FDIM_(n), SimTK_D_INPUT_(d), SimTK_D_INPUT_(lld), SimTK_D_INPUT_(sigma), SimTK_D_INPUT_(pivmin), SimTK_I_INPUT_(r) );
+
+extern double dlangb_( SimTK_FOPT_(norm), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), const double *ab, SimTK_FDIM_(ldab), double *work, SimTK_FLEN_(norm) );
+
+extern double zlangb_( SimTK_FOPT_(norm), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), const SimTK_Z_ *ab, SimTK_FDIM_(ldab), double *work, SimTK_FLEN_(norm) );
+
+extern double slange_( SimTK_FOPT_(norm), SimTK_FDIM_(m), SimTK_FDIM_(n), const float *a, SimTK_FDIM_(lda), float *work, SimTK_FLEN_(norm));
+extern double clange_( SimTK_FOPT_(norm), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), float *work, SimTK_FLEN_(norm));
+extern double dlange_( SimTK_FOPT_(norm), SimTK_FDIM_(m), SimTK_FDIM_(n), const double *a, SimTK_FDIM_(lda), double *work, SimTK_FLEN_(norm));
+extern double zlange_( SimTK_FOPT_(norm), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_Z_ *a, SimTK_FDIM_(lda), double *work, SimTK_FLEN_(norm));
+
+extern double slansb_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(k), const float *ab, SimTK_FDIM_(ldab), float *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo));
+
+extern double dlansb_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(k), const double *ab, SimTK_FDIM_(ldab), double *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo));
+
+extern double clansb_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_C_ *ab, SimTK_FDIM_(ldab), float *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo));
+
+extern double zlansb_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_Z_ *ab, SimTK_FDIM_(ldab), double *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo));
+
+
+extern double clanhb_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_C_ *ab, SimTK_FDIM_(ldab), float *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo));
+
+extern double zlanhb_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_Z_ *ab, SimTK_FDIM_(ldab), double *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo));
+
+extern double slansp_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FDIM_(n), const float *ap, float *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo));
+extern double dlansp_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FDIM_(n), const double *ap, double *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo));
+extern double clansp_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_C_ *ap, float *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo));
+extern double zlansp_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_Z_ *ap, double *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo));
+
+extern double slanhs_( SimTK_FOPT_(norm), SimTK_FDIM_(n), const float *a, SimTK_FDIM_(lda), float *work, SimTK_FLEN_(norm));
+extern double dlanhs_( SimTK_FOPT_(norm), SimTK_FDIM_(n), const double *a, SimTK_FDIM_(lda), double *work, SimTK_FLEN_(norm));
+extern double clanhs_( SimTK_FOPT_(norm), SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), float *work, SimTK_FLEN_(norm));
+extern double zlanhs_( SimTK_FOPT_(norm), SimTK_FDIM_(n), const SimTK_Z_ *a, SimTK_FDIM_(lda), double *work, SimTK_FLEN_(norm));
+
+extern double slangt_( SimTK_FOPT_(norm), SimTK_FDIM_(n), const float *dl, const float *d, const float *du, SimTK_FLEN_(norm));
+extern double dlangt_( SimTK_FOPT_(norm), SimTK_FDIM_(n), const double *dl, const double *d, const double *du, SimTK_FLEN_(norm));
+extern double clangt_( SimTK_FOPT_(norm), SimTK_FDIM_(n), const SimTK_C_ *dl, const SimTK_C_ *d, const SimTK_C_ *du, SimTK_FLEN_(norm));
+extern double zlangt_( SimTK_FOPT_(norm), SimTK_FDIM_(n), const SimTK_Z_ *dl, const SimTK_Z_ *d, const SimTK_Z_ *du, SimTK_FLEN_(norm));
+
+extern double clanhp_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_C_ *ap, float *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo));
+extern double zlanhp_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_Z_ *ap, double *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo));
+
+extern double clanht_( SimTK_FOPT_(norm), SimTK_FDIM_(n), const float *d, const SimTK_C_ *e, SimTK_FLEN_(norm) );
+extern double zlanht_( SimTK_FOPT_(norm), SimTK_FDIM_(n), const double *d, const SimTK_Z_ *e, SimTK_FLEN_(norm) );
+
+extern double slansy_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FDIM_(n), const float *a, SimTK_FDIM_(lda), float *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo));
+extern double dlansy_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FDIM_(n), const double *a, SimTK_FDIM_(lda), double *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo));
+extern double clansy_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), float *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo));
+extern double zlansy_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_Z_ *a, SimTK_FDIM_(lda), double *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo));
+
+extern double clanhe_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), float *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo));
+extern double zlanhe_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_Z_ *a, SimTK_FDIM_(lda), double *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo));
+
+extern double slantb_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(k), const float *ab, SimTK_FDIM_(ldab), float *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern double dlantb_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(k), const double *ab, SimTK_FDIM_(ldab), double *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern double clantb_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_C_ *ab, SimTK_FDIM_(ldab), float *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern double zlantb_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_Z_ *ab, SimTK_FDIM_(ldab), double *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern double slantp_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), const float *ap, float *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern double dlantp_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), const double *ap, double *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern double clantp_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), const SimTK_C_ *ap, float *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern double zlantp_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), const SimTK_Z_ *ap, double *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern double slantr_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(m), SimTK_FDIM_(n), const float *a, SimTK_FDIM_(lda), float *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern double dlantr_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(m), SimTK_FDIM_(n), const double *a, SimTK_FDIM_(lda), double *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern double clantr_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_C_ *a, SimTK_FDIM_(lda), float *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern double zlantr_( SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_Z_ *a, SimTK_FDIM_(lda), double *work, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern double slapy2_( SimTK_S_INPUT_(x), SimTK_S_INPUT_(y) );
+extern double dlapy2_( SimTK_D_INPUT_(x), SimTK_D_INPUT_(y) );
+extern double slapy3_( SimTK_S_INPUT_(x), SimTK_S_INPUT_(y), SimTK_S_INPUT_(z) );
+extern double dlapy3_( SimTK_D_INPUT_(x), SimTK_D_INPUT_(y), SimTK_D_INPUT_(z) );
+
+extern void chetd2_( SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_C_ *a, SimTK_FDIM_(lda), float *d, float *e, SimTK_C_ *tau, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zhetd2_( SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), double *d, double *e, SimTK_Z_ *tau, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void slamc1_(int *beta, int *t, int *rnd, int *ieee1);
+
+extern void slamc2_(int *beta, int *t, int *rnd, float *eps, int *emin, float *rmin, int *emax, float *rmax);
+
+extern void slamc4_(int *emin, float *start, int *base);
+
+extern void slamc5_(int *beta, int *p, int *emin, int *ieee, int *emax, float *rmax);
+
+extern void slamrg_( SimTK_I_INPUT_(n1), SimTK_I_INPUT_(n2), const float *a, SimTK_I_INPUT_(strd1), SimTK_I_INPUT_(strd2), int *index);
+
+extern void slanv2_(float *a, float *b, float *c__, float *d__, float *rt1r, float *rt1i, float *rt2r, float *rt2i, float *cs, float *sn);
+
+extern void slapll_(SimTK_FDIM_(n), float *x, SimTK_FINC_(x), float *y, SimTK_FINC_(y), SimTK_S_OUTPUT_(ssmin));
+
+extern void slapmt_(SimTK_I_INPUT_(forwrd), SimTK_FDIM_(m), SimTK_FDIM_(n), float *x, SimTK_FDIM_(ldx), SimTK_FDIM_(k));
+
+extern void slaqgb_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), float *ab, SimTK_FDIM_(ldab), float *r__, float *c__, SimTK_S_OUTPUT_(rowcnd), SimTK_S_OUTPUT_(colcnd), SimTK_S_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(equed));
+
+extern void slaqge_(SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *r__, float *c__, SimTK_S_INPUT_(rowcnd), SimTK_S_INPUT_(colcnd), SimTK_S_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(equed));
+
+extern void slaqp2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_I_INPUT_(offset), float *a, SimTK_FDIM_(lda), int *jpvt, float *tau, float *vn1, float *vn2, float *work);
+
+extern void slaqps_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_I_INPUT_(offset), SimTK_I_INPUT_(nb), SimTK_I_OUTPUT_(kb), float *a, SimTK_FDIM_(lda), int *jpvt, float *tau, float *vn1, float *vn2, float *auxv, float *f, SimTK_FDIM_(ldf));
+
+extern void slaqr0_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), float *h, SimTK_FDIM_(ldh), float *wr, float *wi, SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), float *z, SimTK_FDIM_(ldz), float *work, SimTK_FDIM_(lwork), SimTK_INFO_ );
+
+extern void slaqr1_(SimTK_FDIM_(n), const float *h, SimTK_FDIM_(ldh), SimTK_S_INPUT_(sr1), SimTK_S_INPUT_(si1), SimTK_S_INPUT_(sr2), SimTK_S_INPUT_(si2), float *v );
+
+extern void slaqr2_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ktop), SimTK_I_INPUT_(kbot), SimTK_I_INPUT_(nw), float *h, SimTK_FDIM_(ldh), SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), float *z, SimTK_FDIM_(ldz), SimTK_I_OUTPUT_(ns), SimTK_I_OUTPUT_(nd), float *sr, float *si, float *v, SimTK_FDIM_(ldv), SimTK_I_INPUT_(nh), float *t, SimTK_FDIM_(ldt), SimTK_I_INPUT_(nv), float *wv, SimTK_I_INPUT_(ldwv), float *work, SimTK_FDIM_(lwork) );
+
+extern void slaqr3_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ktop), SimTK_I_INPUT_(kbot), SimTK_I_INPUT_(nw), float *h, SimTK_FDIM_(ldh), SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), float *z, SimTK_FDIM_(ldz), SimTK_I_OUTPUT_(ns), SimTK_I_OUTPUT_(nd), float *sr, float *si, float *v, SimTK_FDIM_(ldv), SimTK_I_INPUT_(nh), float *t, SimTK_FDIM_(ldt), SimTK_I_INPUT_(nv), float *wv, SimTK_I_INPUT_(ldwv), float *work, SimTK_FDIM_(lwork) );
+
+extern void slaqr4_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), float *h, SimTK_FDIM_(ldh), float *wr, float *wi, SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), float *z, SimTK_FDIM_(ldz), float *work, SimTK_FDIM_(lwork), SimTK_INFO_ );
+
+extern void slaqr5_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_I_INPUT_(kacc22), SimTK_FDIM_(n), SimTK_I_INPUT_(ktop), SimTK_I_INPUT_(kbot), SimTK_I_INPUT_(nshifts), const float *sr, const float *si, float *h, SimTK_FDIM_(ldh), SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), float *z, SimTK_FDIM_(ldz), float *v, SimTK_FDIM_(ldv), float *u, SimTK_FDIM_(ldu), SimTK_I_INPUT_(nh), float *wh, SimTK_FDIM_(ldwh), SimTK_I_INPUT_(nv), float *wv, SimTK_FDIM_(ldwv) );
+
+extern void slaqsb_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), float *ab, SimTK_FDIM_(ldab), float *s, SimTK_S_INPUT_(scond), SimTK_S_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void slaqsp_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *ap, const float *s, SimTK_S_INPUT_(scond), SimTK_S_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void slaqsy_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), const float *s, SimTK_S_INPUT_(scond), SimTK_S_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void slaqtr_(SimTK_I_INPUT_(ltran), SimTK_I_INPUT_(lfloat), SimTK_FDIM_(n), const float *t, SimTK_FDIM_(ldt), const float *b, const float *w, SimTK_S_OUTPUT_(scale), float *x, float *work, SimTK_INFO_);
+
+extern void slar1v_(SimTK_FDIM_(n), SimTK_I_INPUT_(b1), SimTK_I_INPUT_(bn), SimTK_S_INPUT_(lambda), const float *d__, const float *l, const float *ld, const float *lld, const float *pivmin, const float *gaptol, float *z__, SimTK_I_INPUT_(wantc), SimTK_I_OUTPUT_(negcnt), SimTK_S_OUTPUT_(ztz), SimTK_S_OUTPUT_(mingma), SimTK_I_OUTPUT_(r__), int *isuppz, SimTK_S_OUTPUT_(nrminv), SimTK_S_OUTPUT_(resid), SimTK_S_OUTPUT_(rqcorr), float *work);
+
+extern void slar2v_(SimTK_FDIM_(n), float *x, float *y, float *z__, int *incx, const float *c__, const float *s, SimTK_FINC_(c));
+
+extern void slarf_(SimTK_FOPT_(side), SimTK_FDIM_(m), SimTK_FDIM_(n), float *v, SimTK_FINC_(v), float *tau, float *c__, SimTK_FDIM_(ldc), float *work, SimTK_FLEN_(side));
+
+extern void slarfb_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FOPT_(direct), SimTK_FOPT_(storev), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const float *v, SimTK_FDIM_(ldv), const float *t, SimTK_FDIM_(ldt), float *c__, SimTK_FDIM_(ldc), float *work, SimTK_FDIM_(ldwork), SimTK_FLEN_(side), SimTK_FLEN_(trans), SimTK_FLEN_(direct), SimTK_FLEN_(storev) );
+
+extern void slarfg_(SimTK_FDIM_(n), SimTK_S_OUTPUT_(alpha), float *x, SimTK_FINC_(x), SimTK_S_OUTPUT_(tau) );
+
+extern void slarft_(SimTK_FOPT_(direct), SimTK_FOPT_(storev), SimTK_FDIM_(n), SimTK_I_INPUT_(k), float *v, SimTK_FDIM_(ldv), const float *tau, float *t, SimTK_FDIM_(ldt), SimTK_FLEN_(direct), SimTK_FLEN_(storev));
+
+extern void slarfx_(SimTK_FOPT_(side), SimTK_FDIM_(m), SimTK_FDIM_(n), const float *v, const float *tau, float *c__, SimTK_FDIM_(ldc), float *work, SimTK_FLEN_(side));
+
+extern void slargv_(SimTK_FDIM_(n), float *x, SimTK_FINC_(x), float *y, SimTK_FINC_(y), float *c__, SimTK_FINC_(c));
+
+extern void slarnv_(SimTK_I_INPUT_(idist), int *iseed, SimTK_FDIM_(n), float *x);
+
+extern void slarra_(SimTK_FDIM_(n), const float *d, float *e, float *e2, SimTK_S_INPUT_(spltol), SimTK_S_INPUT_(tnrm), SimTK_I_OUTPUT_(nsplit), int *isplit, SimTK_INFO_);
+
+extern void slarrb_(SimTK_FDIM_(n), const float *d__, const float *lld, SimTK_I_INPUT_(ifirst), SimTK_I_INPUT_(ilast), SimTK_S_INPUT_(rtol1), SimTK_S_INPUT_(rtol2), SimTK_I_INPUT_(offset), float *w, float *wgap, float *werr, float *work, int *iwork, SimTK_S_INPUT_(pivmin), SimTK_S_INPUT_(spdiam), SimTK_I_INPUT_(twist), SimTK_INFO_);
+
+extern void slarrc_(SimTK_FOPT_(jobt), SimTK_FDIM_(n), SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), const float *d, const float *e, SimTK_S_INPUT_(pivmin), SimTK_I_OUTPUT_(eigcnt), SimTK_I_OUTPUT_(lcnt), SimTK_I_OUTPUT_(rcnt), SimTK_INFO_, SimTK_FLEN_(jobt) );
+
+extern void slarrd_( SimTK_FOPT_(range), SimTK_FOPT_(order), SimTK_FDIM_(n), SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), const float *gers, SimTK_S_INPUT_(reltol), const float *d, const float *e, const float *e2, SimTK_S_INPUT_(pivmin), SimTK_I_INPUT_(nsplit), const int *isplit, SimTK_I_OUTPUT_(m), float *w, float *werr, SimTK_S_OUTPUT_(wl), SimTK_S_OUTPUT_(wu), int *iblock, int *indexw, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(range), SimTK_F [...]
+
+extern void slarre_(SimTK_FOPT_(range), SimTK_FDIM_(n), SimTK_S_OUTPUT_(vl), SimTK_S_OUTPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), float *d__, float *e, float *e2, SimTK_S_INPUT_(rtol1), SimTK_S_INPUT_(rtol2), SimTK_S_INPUT_(spltol), SimTK_I_OUTPUT_(nsplit), int *isplit, SimTK_I_OUTPUT_(m), float *w, float *werr, float *wgap, int *iblock, int *indexw, float *gers, float *pivmin, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(range) );
+
+extern void slarrf_(SimTK_FDIM_(n), float *d__, float *l, float *ld, SimTK_I_INPUT_(clstrt), SimTK_I_INPUT_(clend), const float *w, float *wgap, float *werr, SimTK_S_INPUT_(spdiam), SimTK_S_INPUT_(clgapl), SimTK_S_INPUT_(clgapr), SimTK_S_INPUT_(pivmin), SimTK_S_OUTPUT_(sigma), float *dplus, float *lplus, float *work, SimTK_INFO_);
+
+extern void slarrj_(SimTK_FDIM_(n), const float *d, const float *e2, SimTK_I_INPUT_(ifirst), SimTK_I_INPUT_(ilast), SimTK_S_INPUT_(rtol), SimTK_I_INPUT_(offset), float *w, float *werr, float *work, int *iwork, SimTK_S_INPUT_(pivmin), SimTK_S_INPUT_(spdiam), SimTK_INFO_);
+
+extern void slarrk_( SimTK_FDIM_(n), SimTK_I_INPUT_(iw), SimTK_S_INPUT_(gl), SimTK_S_INPUT_(gu), const float *d, const float *e2, SimTK_S_INPUT_(pivmin), SimTK_S_INPUT_(reltol), SimTK_S_OUTPUT_(w), SimTK_S_OUTPUT_(werr), SimTK_INFO_);
+
+extern void slarrr_( SimTK_FDIM_(n), const float *d__, float *e, SimTK_INFO_ );
+
+extern void slarrv_(SimTK_FDIM_(n), SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), float *d__, float *l, SimTK_S_INPUT_(pivmin), const int *isplit, SimTK_FDIM_(m), SimTK_I_INPUT_(dol), SimTK_I_INPUT_(dou), SimTK_S_INPUT_(minrgp), SimTK_S_INPUT_(rtol1), SimTK_S_INPUT_(rtol2), float *w, float *werr, float *wgap, const int *iblock, const int *indexw, const float *gers, float *z__, SimTK_FDIM_(ldz), int *isuppz, float *work, int *iwork, SimTK_INFO_);
+
+extern void slartg_(const float *f, const float *g, float *cs, float *sn, float *r__);
+
+extern void slartv_(SimTK_FDIM_(n), float *x, SimTK_FINC_(x), float *y, SimTK_FINC_(y), const float *c__, const float *s, SimTK_FINC_(c));
+
+extern void slaruv_(int *iseed, SimTK_FDIM_(n), float *x);
+
+extern void slarz_(SimTK_FOPT_(side), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_I_INPUT_(L), const float *v, SimTK_FINC_(v), SimTK_S_INPUT_(tau), float *c__, SimTK_FDIM_(ldc), float *work, SimTK_FLEN_(side));
+
+extern void slarzb_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FOPT_(direct), SimTK_FOPT_(storev), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(l), const float *v, SimTK_FDIM_(ldv), const float *t, SimTK_FDIM_(ldt), float *c__, SimTK_FDIM_(ldc), float *work, SimTK_FDIM_(ldwork), SimTK_FLEN_(side), SimTK_FLEN_(trans), SimTK_FLEN_(direct), SimTK_FLEN_(storev));
+
+extern void slarzt_(SimTK_FOPT_(direct), SimTK_FOPT_(storev), SimTK_FDIM_(n), SimTK_FDIM_(k), float *v, SimTK_FDIM_(ldv), const float *tau, float *t, SimTK_FDIM_(ldt), SimTK_FLEN_(direct), SimTK_FLEN_(storev));
+
+extern void slas2_(float *f, float *g, float *h__, SimTK_S_OUTPUT_(ssmin), float *ssmax);
+
+extern void slascl_(SimTK_FOPT_(type__), SimTK_FDIM_(kl), SimTK_FDIM_(ku), const float *cfrom, const float *cto, SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(type));
+
+extern void slasd0_(SimTK_FDIM_(n), SimTK_I_INPUT_(sqre), float *d__, float *e, float *u, SimTK_FDIM_(ldu), float *vt, SimTK_FDIM_(ldvt), SimTK_I_INPUT_(smlsiz), int *iwork, float *work, SimTK_INFO_);
+
+extern void slasd1_(SimTK_I_INPUT_(nl), SimTK_I_INPUT_(nr), SimTK_I_INPUT_(sqre), float *d__, SimTK_S_INPUT_(alpha), SimTK_S_INPUT_(beta), float *u, SimTK_FDIM_(ldu), float *vt, SimTK_FDIM_(ldvt), int *idxq, int *iwork, float *work, SimTK_INFO_);
+
+extern void slasd2_(SimTK_I_INPUT_(nl), SimTK_I_INPUT_(nr), SimTK_I_INPUT_(sqre), SimTK_I_OUTPUT_(k), float *d__, float *z__, SimTK_S_INPUT_(alpha), SimTK_S_INPUT_(beta), float *u, SimTK_FDIM_(ldu), float *vt, SimTK_FDIM_(ldvt), float *dsigma, float *u2, SimTK_FDIM_(ldu2), float *vt2, SimTK_FDIM_(ldvt2), int *idxp, int *idx, int *idxc, int *idxq, int *coltyp, SimTK_INFO_);
+
+extern void slasd3_(SimTK_I_INPUT_(nl), SimTK_I_INPUT_(nr), SimTK_I_INPUT_(sqre), SimTK_I_INPUT_(k), float *d__, float *q, SimTK_FDIM_(ldq), const float *dsigma, const float *u, SimTK_FDIM_(ldu), const float *u2, SimTK_FDIM_(ldu2), const float *vt, SimTK_FDIM_(ldvt), const float *vt2, SimTK_FDIM_(ldvt2), const int *idxc, const int *ctot, const float *z__, SimTK_INFO_);
+
+
+extern void slasd4_(SimTK_FDIM_(n), SimTK_I_INPUT_(i__), const float *d__, const float *z__, float *delta, SimTK_S_INPUT_(rho), SimTK_S_OUTPUT_(sigma), float *work, SimTK_INFO_);
+
+extern void slasd5_( SimTK_I_INPUT_(i__), const float *d__, const float *z__, float *delta, SimTK_S_INPUT_(rho), SimTK_S_OUTPUT_(dsigma), float *work);
+
+extern void slasd6_(SimTK_I_INPUT_(icompq), SimTK_I_INPUT_(nl), SimTK_I_INPUT_(nr), SimTK_I_INPUT_(sqre), float *d__, float *vf, float *vl, SimTK_S_INPUT_(alpha), SimTK_S_INPUT_(beta), int *idxq, int *perm, SimTK_I_OUTPUT_(givptr), SimTK_I_OUTPUT_(givcol), SimTK_FDIM_(ldgcol), float *givnum, SimTK_FDIM_(ldgnum), float *poles, float *difl, float *difr, float *z__, SimTK_I_OUTPUT_(k), SimTK_S_OUTPUT_(c__), SimTK_S_OUTPUT_(s), float *work, int *iwork, SimTK_INFO_);
+
+extern void slasd7_( SimTK_I_INPUT_(icompq), SimTK_I_INPUT_(nl), SimTK_I_INPUT_(nr), SimTK_I_INPUT_(sqre), SimTK_I_OUTPUT_(k), float *d__, float *z__, float *zw, float *vf, float *vfw, float *vl, float *vlw, SimTK_S_INPUT_(alpha), SimTK_S_INPUT_(beta), float *dsigma, int *idx, int *idxp, const int *idxq, int *perm, SimTK_I_OUTPUT_(givptr), int *givcol, SimTK_FDIM_(LDGCOL), float *givnum, SimTK_FDIM_(ldgnum), SimTK_S_OUTPUT_(c__), SimTK_S_OUTPUT_(s), SimTK_INFO_);
+
+extern void slasd8_(SimTK_I_INPUT_(icompq), SimTK_FDIM_(k), float *d__, const float *z__, float *vf, float *vl, float *difl, float *difr, SimTK_FDIM_(lddifr), const float *dsigma, float *work, SimTK_INFO_);
+
+extern void slasd9_(SimTK_I_INPUT_(icompq), SimTK_FDIM_(ldu), SimTK_FDIM_(k), float *d__, float *z__, float *vf, float *vl, float *difl, float *difr, float *dsigma, float *work, SimTK_INFO_);
+
+extern void slasda_(SimTK_I_INPUT_(icompq), SimTK_I_INPUT_(smlsiz), SimTK_FDIM_(n), SimTK_I_INPUT_(sqre), float *d__, float *e, float *u, SimTK_FDIM_(ldu), float *vt, SimTK_FDIM_(k), float *difl, float *difr, float *z__, float *poles, int *givptr, int *givcol, SimTK_FDIM_(ldgcol), int *perm, float *givnum, float *c__, float *s, float *work, int *iwork, SimTK_INFO_);
+
+extern void slasdq_(SimTK_FOPT_(uplo), SimTK_I_INPUT_(sqre), SimTK_FDIM_(n), SimTK_I_INPUT_(ncvt), SimTK_I_INPUT_(nru), SimTK_I_INPUT_(ncc), float *d__, float *e, float *vt, SimTK_FDIM_(ldvt), float *u, SimTK_FDIM_(ldu), float *c__, SimTK_FDIM_(ldc), float *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void slasdt_( SimTK_FDIM_(n), SimTK_I_OUTPUT_(lvl), SimTK_I_OUTPUT_(nd), int *inode, int *ndiml, int *ndimr, SimTK_I_INPUT_(msub) );
+
+extern void slaset_(SimTK_FOPT_(uplo), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_S_INPUT_(alpha), SimTK_S_INPUT_(beta), float *a, SimTK_FDIM_(lda), SimTK_FLEN_(uplo));
+
+extern void slasq1_(SimTK_FDIM_(n), float *d__, float *e, float *work, SimTK_INFO_);
+
+extern void slasq2_(SimTK_FDIM_(n), float *z__, SimTK_INFO_);
+
+extern void slasq3_(SimTK_I_INPUT_(i0), SimTK_I_INPUT_(n0), const float *z__, SimTK_I_INPUT_(pp), SimTK_S_OUTPUT_(dmin__), SimTK_S_OUTPUT_(sigma), SimTK_S_OUTPUT_(desig), SimTK_S_INPUT_(qmax), SimTK_I_OUTPUT_(nfail), SimTK_I_OUTPUT_(iter), SimTK_I_OUTPUT_(ndiv), SimTK_I_INPUT_(ieee) );
+
+extern void slasq4_( SimTK_I_INPUT_(i0), SimTK_I_INPUT_(n0), const float *z__, SimTK_I_INPUT_(pp), SimTK_I_INPUT_(noin), SimTK_S_INPUT_(dmin), SimTK_S_INPUT_(dmin1), SimTK_S_INPUT_(dmin2), SimTK_S_INPUT_(dn), SimTK_S_INPUT_(dn1), SimTK_S_INPUT_(dn2), SimTK_S_OUTPUT_(tau), SimTK_I_OUTPUT_(ttype));
+
+extern void slasq5_(SimTK_I_INPUT_(i0), SimTK_I_INPUT_(n0), const float *z__, SimTK_I_INPUT_(pp), SimTK_S_INPUT_(tau), SimTK_S_OUTPUT_(dmin), SimTK_S_OUTPUT_(dmin1), SimTK_S_OUTPUT_(dmin2), SimTK_S_OUTPUT_(dn), SimTK_S_OUTPUT_(dnm1), SimTK_S_OUTPUT_(dnm2), SimTK_I_INPUT_(ieee) );
+
+extern void slasq6_(SimTK_I_INPUT_(i0), SimTK_I_INPUT_(n0), const float *z__, SimTK_I_INPUT_(pp), SimTK_S_OUTPUT_(dmin), SimTK_S_OUTPUT_(dmin1), SimTK_S_OUTPUT_(dmin2), SimTK_S_OUTPUT_(dn), SimTK_S_OUTPUT_(dnm1), SimTK_S_OUTPUT_(dnm2));
+
+extern void slasr_(SimTK_FOPT_(side), SimTK_FOPT_(pivot), SimTK_FOPT_(direct), SimTK_FDIM_(m), SimTK_FDIM_(n), const float *c__, const float *s, float *a, SimTK_FDIM_(lda), SimTK_FLEN_(side), SimTK_FLEN_(pivot), SimTK_FLEN_(direct));
+
+extern void slasrt_(SimTK_FOPT_(id), SimTK_FDIM_(n), float *d__, SimTK_INFO_, SimTK_FLEN_(id));
+
+extern void slassq_(SimTK_FDIM_(n), const float *x, SimTK_FINC_(x), SimTK_S_OUTPUT_(scale), float *sumsq);
+
+extern void slasv2_(SimTK_S_INPUT_(f), SimTK_S_INPUT_(g), SimTK_S_INPUT_(h__), SimTK_S_OUTPUT_(ssmin), SimTK_S_OUTPUT_(ssmax) , SimTK_S_OUTPUT_(snr) , SimTK_S_OUTPUT_(csr) , SimTK_S_OUTPUT_(snl) , SimTK_S_OUTPUT_(csl) );
+
+extern void slaswp_(SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), SimTK_I_OUTPUT_(k1), SimTK_I_OUTPUT_(k2), const int *ipiv, SimTK_FINC_(x));
+
+extern void slasy2_(SimTK_I_INPUT_(ltranl), SimTK_I_INPUT_(ltranr), SimTK_I_INPUT_(isgn), SimTK_I_INPUT_(n1), SimTK_I_INPUT_(n2), const float *tl, SimTK_FDIM_(ldtl), const float *tr, SimTK_FDIM_(ldtr), const float *b, SimTK_FDIM_(ldb), SimTK_S_OUTPUT_(scale), float *x, SimTK_FDIM_(ldx), float *xnorm, SimTK_INFO_);
+
+
+extern void slasyf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_I_INPUT_(nb), SimTK_I_INPUT_(kb), float *a, SimTK_FDIM_(lda), int *ipiv, float *w, SimTK_FDIM_(ldw), int *info, SimTK_FLEN_(uplo));
+
+extern void slatbs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FOPT_(normin), SimTK_FDIM_(n), SimTK_I_INPUT_(kd), const float *ab, SimTK_FDIM_(ldab), float *x, SimTK_S_OUTPUT_(scale), float *cnorm, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag), SimTK_FLEN_(normin) );
+
+extern void slatdf_(SimTK_I_INPUT_(ijob), SimTK_FDIM_(n), const float *z__, SimTK_FDIM_(ldz), float *rhs, SimTK_S_OUTPUT_(rdsum), SimTK_S_OUTPUT_(rdscal), const int *ipiv, const int *jpiv);
+
+extern void slatps_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FOPT_(normin), SimTK_FDIM_(n), const float *ap, float *x, SimTK_S_OUTPUT_(scale), float *cnorm, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag), SimTK_FLEN_(normin) );
+
+extern void slatrd_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nb), float *a, SimTK_FDIM_(lda), float *e, float *tau, float *w, SimTK_FDIM_(ldw), SimTK_FLEN_(uplo));
+
+extern void slatrs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FOPT_(normin), SimTK_FDIM_(n), const float *a, SimTK_FDIM_(lda), float *x, SimTK_S_OUTPUT_(scale), float *cnorm, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag), SimTK_FLEN_(normin));
+
+extern void slatrz_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(l), float *a, SimTK_FDIM_(lda), float *tau, float *work);
+
+extern void slatzm_(SimTK_FOPT_(side), SimTK_FDIM_(m), SimTK_FDIM_(n), const float *v, SimTK_FINC_(v), SimTK_S_INPUT_(tau), float *c1, float *c2, SimTK_FDIM_(ldc), float *work, SimTK_FLEN_(side));
+
+extern void slauu2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void slauum_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void slazq3( SimTK_I_INPUT_(io), SimTK_I_INPUT_(no), const float *z, SimTK_I_INPUT_(pp), SimTK_S_OUTPUT_(dmin), SimTK_S_OUTPUT_(sigma), SimTK_S_OUTPUT_(desig), SimTK_S_INPUT_(qmax), SimTK_I_OUTPUT_(nfail), SimTK_I_OUTPUT_(iter), SimTK_I_OUTPUT_(ndiv), SimTK_I_INPUT_(ieee), SimTK_I_INPUT_(ttype), SimTK_S_OUTPUT_(dmin1), SimTK_S_OUTPUT_(dmin2), SimTK_S_OUTPUT_(dn), SimTK_S_OUTPUT_(dn1), SimTK_S_OUTPUT_(dn2), SimTK_S_OUTPUT_(tau) );
+
+extern void slazq4( SimTK_I_INPUT_(io), SimTK_I_INPUT_(no), const float *z, SimTK_I_INPUT_(pp), SimTK_I_INPUT_(noin), SimTK_S_INPUT_(dmin), SimTK_S_INPUT_(dmin1), SimTK_S_INPUT_(dmin2), SimTK_S_INPUT_(dn), SimTK_S_INPUT_(dn1), SimTK_S_INPUT_(dn2), SimTK_S_OUTPUT_(tau), SimTK_I_OUTPUT_(ttype), SimTK_S_OUTPUT_(g) );
+
+extern void sopgtr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *ap, float *tau, float *q, SimTK_FDIM_(ldq), float *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void sopmtr_(SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), float *ap, float *tau, float *c__, SimTK_FDIM_(ldc), float *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(uplo), SimTK_FLEN_(trans));
+
+extern void sorg2l_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_INFO_);
+
+extern void sorg2r_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_INFO_);
+
+extern void sorgbr_(SimTK_FOPT_(vect), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_FDIM_(lwork), int *info, SimTK_FLEN_(vect));
+
+extern void sorghr_(SimTK_FDIM_(n), SimTK_FDIM_(ilo), SimTK_FDIM_(ihi), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void sorgl2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_INFO_);
+
+extern void sorglq_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void sorgql_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void sorgqr_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void sorgr2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_INFO_);
+
+extern void sorgrq_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void sorgtr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void sorm2l_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), float *a, SimTK_FDIM_(lda), float *tau, float *c__, SimTK_FDIM_(ldc), float *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void sorm2r_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), float *a, SimTK_FDIM_(lda), float *tau, float *c__, SimTK_FDIM_(ldc), float *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void sormbr_(SimTK_FOPT_(vect), SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), float *a, SimTK_FDIM_(lda), float *tau, float *c__, SimTK_FDIM_(ldc), float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(vect), SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void sormhr_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(ilo), SimTK_FDIM_(ihi), float *a, SimTK_FDIM_(lda), float *tau, float *c__, SimTK_FDIM_(ldc), float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void sorml2_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), float *a, SimTK_FDIM_(lda), float *tau, float *c__, SimTK_FDIM_(ldc), float *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void sormlq_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), float *a, SimTK_FDIM_(lda), float *tau, float *c__, SimTK_FDIM_(ldc), float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void sormql_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), float *a, SimTK_FDIM_(lda), float *tau, float *c__, SimTK_FDIM_(ldc), float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void sormqr_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), float *a, SimTK_FDIM_(lda), float *tau, float *c__, SimTK_FDIM_(ldc), float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void sormr2_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), float *a, SimTK_FDIM_(lda), float *tau, float *c__, SimTK_FDIM_(ldc), float *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void sormr3_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), int *l, float *a, SimTK_FDIM_(lda), float *tau, float *c__, SimTK_FDIM_(ldc), float *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void sormrq_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), float *a, SimTK_FDIM_(lda), float *tau, float *c__, SimTK_FDIM_(ldc), float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void sormrz_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(l), float *a, SimTK_FDIM_(lda), float *tau, float *c__, SimTK_FDIM_(ldc), float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void sormtr_(SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *tau, float *c__, SimTK_FDIM_(ldc), float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(uplo), SimTK_FLEN_(trans));
+
+extern void spbcon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), const float *ab, SimTK_FDIM_(ldab), SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void spbequ_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_I_INPUT_(kd), const float *ab, SimTK_FDIM_(ldab), SimTK_S_OUTPUT_(s), SimTK_S_OUTPUT_(scond), SimTK_S_OUTPUT_(amax), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void spbrfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), const float *ab, SimTK_FDIM_(ldab), const float *afb, SimTK_FDIM_(ldafb), const float *b, SimTK_FDIM_(ldb), float *x, SimTK_FDIM_(ldx), float *ferr, float *berr, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void spbstf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), float *ab, SimTK_FDIM_(ldab), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void spbsv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), float *ab, SimTK_FDIM_(ldab), float *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void spbsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), float *ab, SimTK_FDIM_(ldab), float *afb, SimTK_FDIM_(ldafb), SimTK_CHAR_OUTPUT_(equed), float *s, float *b, SimTK_FDIM_(ldb), float *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(rcond), float *ferr, float *berr, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void spbtf2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), float *ab, SimTK_FDIM_(ldab), SimTK_INFO_, SimTK_FLEN_(uplo) );
+
+extern void spbtrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), float *ab, SimTK_FDIM_(ldab), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void spbtrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), const float *ab, SimTK_FDIM_(ldab), float *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void spocon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const float *a, SimTK_FDIM_(lda), SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void spoequ_(SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *s, SimTK_S_OUTPUT_(scond), SimTK_S_OUTPUT_(amax), SimTK_INFO_);
+
+extern void sporfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *a, SimTK_FDIM_(lda), const float *af, SimTK_FDIM_(ldaf), const float *b, SimTK_FDIM_(ldb), float *x, SimTK_FDIM_(ldx), float *ferr, float *berr, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void sposv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void sposvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *a, SimTK_FDIM_(lda), float *af, SimTK_FDIM_(ldaf), SimTK_CHAR_OUTPUT_(equed), float *s, float *b, SimTK_FDIM_(ldb), float *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(rcond), float *ferr, float *berr, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void spotf2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void spotrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void spotri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void spotrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void sppcon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const float *ap, SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void sppequ_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const float *ap, float *s, SimTK_S_OUTPUT_(scond), SimTK_S_OUTPUT_(amax), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void spprfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *ap, const float *afp, const float *b, SimTK_FDIM_(ldb), float *x, SimTK_FDIM_(ldx), float *ferr, float *berr, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void sppsv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *ap, float *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void sppsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *ap, float *afp, SimTK_CHAR_OUTPUT_(equed), float *s, float *b, SimTK_FDIM_(ldb), float *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(rcond), float *ferr, float *berr, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void spptrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *ap, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void spptri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *ap, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void spptrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *ap, float *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void sptcon_(SimTK_FDIM_(n), const float *d__, const float *e, SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), float *work, SimTK_INFO_);
+
+extern void spteqr_(SimTK_FOPT_(compz), SimTK_FDIM_(n), float *d__, float *e, float *z__, SimTK_FDIM_(ldz), float *work, SimTK_INFO_, SimTK_FLEN_(compz));
+
+extern void sptrfs_(SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *d__, const float *e, const float *df, const float *ef, const float *b, SimTK_FDIM_(ldb), float *x, SimTK_FDIM_(ldx), float *ferr, float *berr, float *work, SimTK_INFO_);
+
+extern void sptsv_(SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *d__, float *e, float *b, SimTK_FDIM_(ldb), SimTK_INFO_);
+
+extern void sptsvx_(SimTK_FOPT_(fact), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *d__, const float *e, float *df, float *ef, const float *b, SimTK_FDIM_(ldb), float *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(rcond), float *ferr, float *berr, float *work, SimTK_INFO_, SimTK_FLEN_(fact));
+
+extern void spttrf_(SimTK_FDIM_(n), float *d__, float *e, SimTK_INFO_);
+
+extern void spttrs_(SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *d__, const float *e, float *b, SimTK_FDIM_(ldb), SimTK_INFO_);
+
+extern void sptts2_(SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *d__, const float *e, float *b, SimTK_FDIM_(ldb));
+
+extern void srscl_(SimTK_FDIM_(n), float *sa, float *sx, SimTK_FINC_(x));
+
+extern void ssbev_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), float *ab, SimTK_FDIM_(ldab), float *w, float *z__, SimTK_FDIM_(ldz), float *work, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void ssbevd_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), float *ab, SimTK_FDIM_(ldab), float *w, float *z__, SimTK_FDIM_(ldz), float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void ssbevx_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), float *ab, SimTK_FDIM_(ldab), float *q, SimTK_FDIM_(ldq), SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_S_INPUT_(abstol), SimTK_I_OUTPUT_(m), float *w, float *z__, SimTK_FDIM_(ldz), float *work, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void ssbgst_(SimTK_FOPT_(vect), SimTK_FOPT_(uplo), SimTK_FDIM_(n), int *ka, int *kb, float *ab, SimTK_FDIM_(ldab), float *bb, SimTK_FDIM_(ldbb), float *x, SimTK_FDIM_(ldx), float *work, SimTK_INFO_, SimTK_FLEN_(vect), SimTK_FLEN_(uplo));
+
+extern void ssbgv_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), int *ka, int *kb, float *ab, SimTK_FDIM_(ldab), float *bb, SimTK_FDIM_(ldbb), float *w, float *z__, SimTK_FDIM_(ldz), float *work, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void ssbgvd_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), int *ka, int *kb, float *ab, SimTK_FDIM_(ldab), float *bb, SimTK_FDIM_(ldbb), float *w, float *z__, SimTK_FDIM_(ldz), float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void ssbgvx_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), int *ka, int *kb, float *ab, SimTK_FDIM_(ldab), float *bb, SimTK_FDIM_(ldbb), float *q, SimTK_FDIM_(ldq), SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_S_INPUT_(abstol), SimTK_I_OUTPUT_(m), float *w, float *z__, SimTK_FDIM_(ldz), float *work, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void ssbtrd_(SimTK_FOPT_(vect), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), float *ab, SimTK_FDIM_(ldab), float *d__, float *e, float *q, SimTK_FDIM_(ldq), float *work, SimTK_INFO_, SimTK_FLEN_(vect), SimTK_FLEN_(uplo));
+
+extern void sspcon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const float *ap, const int *ipiv, SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void sspev_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *ap, float *w, float *z__, SimTK_FDIM_(ldz), float *work, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void sspevd_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *ap, float *w, float *z__, SimTK_FDIM_(ldz), float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void sspevx_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *ap, SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_S_INPUT_(abstol), SimTK_I_OUTPUT_(m), float *w, float *z__, SimTK_FDIM_(ldz), float *work, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void sspgst_(SimTK_I_INPUT_(itype), SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *ap, float *bp, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void sspgv_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *ap, float *bp, float *w, float *z__, SimTK_FDIM_(ldz), float *work, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void sspgvd_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *ap, float *bp, float *w, float *z__, SimTK_FDIM_(ldz), float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void sspgvx_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *ap, float *bp, SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_S_INPUT_(abstol), SimTK_I_OUTPUT_(m), float *w, float *z__, SimTK_FDIM_(ldz), float *work, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void ssprfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *ap, const float *afp, const int *ipiv, const float *b, SimTK_FDIM_(ldb), float *x, SimTK_FDIM_(ldx), float *ferr, float *berr, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void sspsv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *ap, int *ipiv, float *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void sspsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *ap, float *afp, int *ipiv, const float *b, SimTK_FDIM_(ldb), float *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(rcond), float *ferr, float *berr, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo));
+
+extern void ssptrd_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *ap, float *d__, float *e, float *tau, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void ssptrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *ap, int *ipiv, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void ssptri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *ap, const int *ipiv, float *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void ssptrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *ap, const int *ipiv, float *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void sstebz_(SimTK_FOPT_(range), SimTK_FOPT_(order), SimTK_FDIM_(n), SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_S_INPUT_(abstol), const float *d__, const float *e, SimTK_I_OUTPUT_(m), SimTK_I_OUTPUT_(nsplit), float *w, int *iblock, int *isplit, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(range), SimTK_FLEN_(order));
+
+extern void sstedc_(SimTK_FOPT_(compz), SimTK_FDIM_(n), float *d__, float *e, float *z__, SimTK_FDIM_(ldz), float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(compz));
+
+extern void sstegr_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FDIM_(n), float *d__, float *e, SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_S_INPUT_(abstol), SimTK_I_OUTPUT_(m), float *w, float *z__, SimTK_FDIM_(ldz), int *isuppz, float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range));
+
+extern void sstein_(SimTK_FDIM_(n), const float *d__, const float *e, SimTK_FDIM_(m), const float *w, const int *iblock, const int *isplit, float *z__, SimTK_FDIM_(ldz), float *work, int *iwork, int *ifail, SimTK_INFO_);
+
+extern void sstemr_( SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FDIM_(n), float *d, float *e, SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_I_OUTPUT_(m), float *w, float *z, SimTK_FDIM_(ldz), SimTK_I_INPUT_(nzc), SimTK_I_OUTPUT_(isuppz), SimTK_I_OUTPUT_(tryrac), float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_FDIM_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range) );
+
+extern void ssteqr_(SimTK_FOPT_(compz), SimTK_FDIM_(n), float *d__, float *e, float *z__, SimTK_FDIM_(ldz), float *work, SimTK_INFO_, SimTK_FLEN_(compz));
+
+extern void ssterf_(SimTK_FDIM_(n), float *d__, float *e, SimTK_INFO_);
+
+extern void sstev_(SimTK_FOPT_(jobz), SimTK_FDIM_(n), float *d__, float *e, float *z__, SimTK_FDIM_(ldz), float *work, SimTK_INFO_, SimTK_FLEN_(jobz));
+
+extern void sstevd_(SimTK_FOPT_(jobz), SimTK_FDIM_(n), float *d__, float *e, float *z__, SimTK_FDIM_(ldz), float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz));
+
+extern void sstevr_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FDIM_(n), float *d__, float *e, SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_S_INPUT_(abstol), SimTK_I_OUTPUT_(m), float *w, float *z__, SimTK_FDIM_(ldz), int *isuppz, float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range));
+
+extern void sstevx_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FDIM_(n), float *d__, float *e, SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_S_INPUT_(abstol), SimTK_I_OUTPUT_(m), float *w, float *z__, SimTK_FDIM_(ldz), float *work, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range));
+
+extern void ssycon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const float *a, SimTK_FDIM_(lda), const int *ipiv, SimTK_S_INPUT_(anorm), SimTK_S_OUTPUT_(rcond), float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void ssyev_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *w, float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void ssyevd_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *w, float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void ssyevr_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_S_INPUT_(abstol), SimTK_I_OUTPUT_(m), float *w, float *z__, SimTK_FDIM_(ldz), int *isuppz, float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void ssyevx_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_S_INPUT_(abstol), SimTK_I_OUTPUT_(m), float *w, float *z__, SimTK_FDIM_(ldz), float *work, SimTK_FDIM_(lwork), int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void ssygs2_(SimTK_I_INPUT_(itype), SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void ssygst_(SimTK_I_INPUT_(itype), SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void ssygv_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), float *w, float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void ssygvd_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), float *w, float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void ssygvx_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), SimTK_S_INPUT_(vl), SimTK_S_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_S_INPUT_(abstol), SimTK_I_OUTPUT_(m), float *w, float *z__, SimTK_FDIM_(ldz), float *work, SimTK_FDIM_(lwork), int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void ssyrfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *a, SimTK_FDIM_(lda), const float *af, SimTK_FDIM_(ldaf), const int *ipiv, const float *b, SimTK_FDIM_(ldb), float *x, SimTK_FDIM_(ldx), float *ferr, float *berr, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void ssysv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *a, SimTK_FDIM_(lda), int *ipiv, float *b, SimTK_FDIM_(ldb), float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void ssysvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *a, SimTK_FDIM_(lda), float *af, SimTK_FDIM_(ldaf), int *ipiv, const float *b, SimTK_FDIM_(ldb), float *x, SimTK_FDIM_(ldx), SimTK_S_OUTPUT_(rcond), float *ferr, float *berr, float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo));
+
+extern void ssytd2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *d__, float *e, float *tau, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void ssytf2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), int *ipiv, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void ssytrd_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *d__, float *e, float *tau, float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void ssytrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), int *ipiv, float *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void ssytri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const float *a, SimTK_FDIM_(lda), const int *ipiv, float *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void ssytrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), float *a, SimTK_FDIM_(lda), int *ipiv, float *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void stbcon_(SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(kd), const float *ab, SimTK_FDIM_(ldab), SimTK_S_OUTPUT_(rcond), float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void stbrfs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), const float *ab, SimTK_FDIM_(ldab), const float *b, SimTK_FDIM_(ldb), const float *x, SimTK_FDIM_(ldx), float *ferr, float *berr, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void stbtrs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), const float *ab, SimTK_FDIM_(ldab), const float *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void stgevc_(SimTK_FOPT_(side), SimTK_FOPT_(howmny), const int *select, SimTK_FDIM_(n), const float *a, SimTK_FDIM_(lda), const float *b, SimTK_FDIM_(ldb), float *vl, SimTK_FDIM_(ldvl), float *vr, SimTK_FDIM_(ldvr), SimTK_I_INPUT_(mm), SimTK_FDIM_(m), float *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(howmny));
+
+extern void stgex2_(SimTK_I_INPUT_(wantq), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), float *q, SimTK_FDIM_(ldq), float *z__, SimTK_FDIM_(ldz), SimTK_I_INPUT_(j1), SimTK_I_INPUT_(n1), SimTK_I_INPUT_(n2), float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void stgexc_(SimTK_I_INPUT_(wantq), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), float *q, SimTK_FDIM_(ldq), float *z__, SimTK_FDIM_(ldz), SimTK_I_OUTPUT_(ifst), SimTK_I_OUTPUT_(ilst), float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void stgsen_(SimTK_I_INPUT_(ijob), SimTK_I_INPUT_(wantq), SimTK_I_INPUT_(wantz), const int *select, SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), float *alphar, float *alphai, SimTK_S_INPUT_(beta), float *q, SimTK_FDIM_(ldq), float *z__, SimTK_FDIM_(ldz), SimTK_I_OUTPUT_(m), SimTK_S_OUTPUT_(pl), SimTK_S_OUTPUT_(pr), float *dif, float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_);
+
+extern void stgsja_(SimTK_FOPT_(jobu), SimTK_FOPT_(jobv), SimTK_FOPT_(jobq), SimTK_FDIM_(m), SimTK_FDIM_(p), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(l), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), SimTK_S_INPUT_(tola), SimTK_S_INPUT_(tolb), float *alpha, float *beta, float *u, SimTK_FDIM_(ldu), float *v, SimTK_FDIM_(ldv), float *q, SimTK_FDIM_(ldq), float *work, SimTK_I_OUTPUT_(ncycle), SimTK_INFO_, SimTK_FLEN_(jobu), SimTK_FLEN_(jobv), SimTK_FLEN_(jobq));
+
+extern void stgsna_(SimTK_FOPT_(job), SimTK_FOPT_(howmny), const int *select, SimTK_FDIM_(n), const float *a, SimTK_FDIM_(lda), const float *b, SimTK_FDIM_(ldb), const float *vl, SimTK_FDIM_(ldvl), const float *vr, SimTK_FDIM_(ldvr), float *s, float *dif, SimTK_FDIM_(mm), SimTK_FDIM_(m), float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(howmny));
+
+extern void stgsy2_(SimTK_FOPT_(trans), SimTK_I_INPUT_(ijob), SimTK_FDIM_(m), SimTK_FDIM_(n), const float *a, SimTK_FDIM_(lda), const float *b, SimTK_FDIM_(ldb), float *c__, SimTK_FDIM_(ldc), const float *d__, SimTK_FDIM_(ldd), const float *e, SimTK_FDIM_(lde), float *f, SimTK_FDIM_(ldf), SimTK_S_OUTPUT_(scale), SimTK_S_OUTPUT_(rdsum), SimTK_S_OUTPUT_(rdscal), int *iwork, int *pq, SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void stgsyl_(SimTK_FOPT_(trans), SimTK_I_INPUT_(ijob), SimTK_FDIM_(m), SimTK_FDIM_(n), const float *a, SimTK_FDIM_(lda), const float *b, SimTK_FDIM_(ldb), float *c__, SimTK_FDIM_(ldc), const float *d__, SimTK_FDIM_(ldd), const float *e, SimTK_FDIM_(lde), float *f, SimTK_FDIM_(ldf), SimTK_S_OUTPUT_(scale), float *dif, float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void stpcon_(SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), const float *ap, SimTK_S_OUTPUT_(rcond), float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void stprfs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *ap, const float *b, SimTK_FDIM_(ldb), const float *x, SimTK_FDIM_(ldx), float *ferr, float *berr, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void stptri_(SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), float *ap, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void stptrs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *ap, float *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void strcon_(SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), const float *a, SimTK_FDIM_(lda), SimTK_S_OUTPUT_(rcond), float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void strevc_(SimTK_FOPT_(side), SimTK_FOPT_(howmny), int *select, SimTK_FDIM_(n), float *t, SimTK_FDIM_(ldt), float *vl, SimTK_FDIM_(ldvl), float *vr, SimTK_FDIM_(ldvr), SimTK_FDIM_(mm), SimTK_FDIM_(m), float *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(howmny));
+
+extern void strexc_(SimTK_FOPT_(compq), SimTK_FDIM_(n), float *t, SimTK_FDIM_(ldt), float *q, SimTK_FDIM_(ldq), SimTK_I_OUTPUT_(ifst), SimTK_I_OUTPUT_(ilst), float *work, SimTK_INFO_, SimTK_FLEN_(compq));
+
+extern void strrfs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *a, SimTK_FDIM_(lda), const float *b, SimTK_FDIM_(ldb), const float *x, SimTK_FDIM_(ldx), float *ferr, float *berr, float *work, int *iwork, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void strsen_(SimTK_FOPT_(job), SimTK_FOPT_(compq), const int *select, SimTK_FDIM_(n), float *t, SimTK_FDIM_(ldt), float *q, SimTK_FDIM_(ldq), float *wr, float *wi, SimTK_FDIM_(m), SimTK_S_OUTPUT_(s), SimTK_S_OUTPUT_(sep), float *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(compq));
+
+extern void strsna_(SimTK_FOPT_(job), SimTK_FOPT_(howmny), const int *select, SimTK_FDIM_(n), float *t, SimTK_FDIM_(ldt), float *vl, SimTK_FDIM_(ldvl), float *vr, SimTK_FDIM_(ldvr), float *s, float *sep, SimTK_FDIM_(mm), SimTK_FDIM_(m), float *work, SimTK_FDIM_(ldwork), int *iwork, SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(howmny));
+
+extern void strsyl_(SimTK_FOPT_(trana), SimTK_FOPT_(tranb), int *isgn, SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), float *c__, SimTK_FDIM_(ldc), SimTK_S_OUTPUT_(scale), SimTK_INFO_, SimTK_FLEN_(trana), SimTK_FLEN_(tranb));
+
+extern void strti2_(SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void strtri_(SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void strtrs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const float *a, SimTK_FDIM_(lda), float *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void stzrqf_(SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *tau, SimTK_INFO_);
+
+extern void stzrzf_(SimTK_FDIM_(m), SimTK_FDIM_(n), float *a, SimTK_FDIM_(lda), float *tau, float *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void xerbla_(const char *srname, SimTK_INFO_, SimTK_FLEN_(srname));
+
+extern void zbdsqr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(ncvt), SimTK_FDIM_(nru), SimTK_FDIM_(ncc), double *d__, double *e, SimTK_Z_ *vt, SimTK_FDIM_(ldvt), SimTK_Z_ *u, SimTK_FDIM_(ldu), SimTK_Z_ *c__, SimTK_FDIM_(ldc), double *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zcgesv_( SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *a, SimTK_FDIM_(lda), int *ipiv, const SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), SimTK_Z_ *work, SimTK_C_ *swork, SimTK_I_OUTPUT_(iter), SimTK_INFO_ );
+
+extern void zdrscl_(SimTK_FDIM_(n), SimTK_D_INPUT_(sa), SimTK_Z_ *sx, SimTK_FINC_(x));
+
+extern void zgbbrd_(SimTK_FOPT_(vect), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(ncc), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_Z_ *ab, SimTK_FDIM_(ldab), double *d__, double *e, SimTK_Z_ *q, SimTK_FDIM_(ldq), SimTK_Z_ *pt, SimTK_FDIM_(ldpt), SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(vect));
+
+extern void zgbcon_(SimTK_FOPT_(norm), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_Z_ *ab, SimTK_FDIM_(ldab), const int *ipiv, SimTK_D_INPUT_(anorm), SimTK_D_OUTPUT_(rcond), SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(norm));
+
+extern void zgbequ_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_Z_ *ab, SimTK_FDIM_(ldab), double *r__, double *c__, double *rowcnd, double *colcnd, double *amax, SimTK_INFO_);
+
+extern void zgbrfs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_FDIM_(nrhs), SimTK_Z_ *ab, SimTK_FDIM_(ldab), SimTK_Z_ *afb, SimTK_FDIM_(ldafb), const int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void zgbsv_(SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_FDIM_(nrhs), SimTK_Z_ *ab, SimTK_FDIM_(ldab), int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_);
+
+extern void zgbsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_FDIM_(nrhs), SimTK_Z_ *ab, SimTK_FDIM_(ldab), SimTK_Z_ *afb, SimTK_FDIM_(ldafb), int *ipiv, SimTK_CHAR_OUTPUT_(equed), double *r__, double *c__, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(trans), SimTK_FLEN_(equed));
+
+extern void zgbtf2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_Z_ *ab, SimTK_FDIM_(ldab), int *ipiv, SimTK_INFO_);
+
+extern void zgbtrf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_Z_ *ab, SimTK_FDIM_(ldab), int *ipiv, SimTK_INFO_);
+
+extern void zgbtrs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_FDIM_(nrhs), SimTK_Z_ *ab, SimTK_FDIM_(ldab), const int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void zgebak_(SimTK_FOPT_(job), SimTK_FOPT_(side), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_D_INPUT_(scale), SimTK_I_INPUT_(m), SimTK_Z_ *v, SimTK_FDIM_(ldv), SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(side));
+
+extern void zgebal_(SimTK_FOPT_(job), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_I_OUTPUT_(ilo), SimTK_I_OUTPUT_(ihi), SimTK_D_OUTPUT_(scale), SimTK_INFO_, SimTK_FLEN_(job));
+
+extern void zgebd2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), double *d__, double *e, SimTK_Z_ *tauq, SimTK_Z_ *taup, SimTK_Z_ *work, SimTK_INFO_);
+
+extern void zgebrd_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), double *d__, double *e, SimTK_Z_ *tauq, SimTK_Z_ *taup, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void zgecon_(SimTK_FOPT_(norm), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_D_INPUT_(anorm), SimTK_D_OUTPUT_(rcond), SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(norm));
+
+extern void zgeequ_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), double *r__, double *c__, double *rowcnd, double *colcnd, double *amax, SimTK_INFO_);
+
+extern void zgees_(SimTK_FOPT_(jobvs), SimTK_FOPT_(sort), SimTK_SELECT_Z select, SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), int *sdim, SimTK_Z_ *w, SimTK_Z_ *vs, SimTK_FDIM_(ldvs), SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, int *bwork, SimTK_INFO_, SimTK_FLEN_(jobvs), SimTK_FLEN_(sort));
+
+extern void zgeesx_(SimTK_FOPT_(jobvs), SimTK_FOPT_(sort), SimTK_SELECT_Z select, char *sense, SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), int *sdim, SimTK_Z_ *w, SimTK_Z_ *vs, SimTK_FDIM_(ldvs), SimTK_D_OUTPUT_(rconde), SimTK_D_OUTPUT_(rcondv), SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, int *bwork, SimTK_INFO_, SimTK_FLEN_(jobvs), SimTK_FLEN_(sort), SimTK_FLEN_(sense));
+
+extern void zgeev_(SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *w, SimTK_Z_ *vl, SimTK_FDIM_(ldvl), SimTK_Z_ *vr, SimTK_FDIM_(ldvr), SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, SimTK_INFO_, SimTK_FLEN_(jobvl), SimTK_FLEN_(jobvr));
+
+extern void zgeevx_(SimTK_FOPT_(balanc), SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), char *sense, SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *w, SimTK_Z_ *vl, SimTK_FDIM_(ldvl), SimTK_Z_ *vr, SimTK_FDIM_(ldvr), SimTK_I_OUTPUT_(ilo), SimTK_I_OUTPUT_(ihi), SimTK_D_OUTPUT_(scale), double *abnrm, SimTK_D_OUTPUT_(rconde), SimTK_D_OUTPUT_(rcondv), SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, SimTK_INFO_, SimTK_FLEN_(balanc), SimTK_FLEN_(jobvl), SimTK_FLEN_(jobvr), SimTK_FLEN_(sense));
+
+extern void zgegs_(SimTK_FOPT_(jobvsl), SimTK_FOPT_(jobvsr), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *alpha, SimTK_Z_ *beta, SimTK_Z_ *vsl, SimTK_FDIM_(ldvsl), SimTK_Z_ *vsr, SimTK_FDIM_(ldvsr), SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, SimTK_INFO_, SimTK_FLEN_(jobvl), SimTK_FLEN_(jobvr));
+
+extern void zgegv_(SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *alpha, SimTK_Z_ *beta, SimTK_Z_ *vl, SimTK_FDIM_(ldvl), SimTK_Z_ *vr, SimTK_FDIM_(ldvr), SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, SimTK_INFO_, SimTK_FLEN_(jobvl), SimTK_FLEN_(jobvr));
+
+extern void zgehd2_(SimTK_FDIM_(n), SimTK_FDIM_(ilo), SimTK_I_INPUT_(ihi), SimTK_Z_ *a, SimTK_I_INPUT_(lda), SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_INFO_);
+
+extern void zgehrd_(SimTK_FDIM_(n), SimTK_FDIM_(ilo), SimTK_I_INPUT_(ihi), SimTK_Z_ *a, SimTK_I_INPUT_(lda), SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void zgelq2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_INFO_);
+
+extern void zgelqf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void zgels_(SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(trans));
+extern void zgelss_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), double *s, SimTK_D_INPUT_(rcond), SimTK_I_OUTPUT_(rank), SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, SimTK_INFO_);
+
+/* gelsx is deprecated; use gelsy instead */
+extern void zgelsx_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), int *jpvt, SimTK_D_INPUT_(rcond), SimTK_I_OUTPUT_(rank), SimTK_Z_ *work, double *rwork, SimTK_INFO_);
+extern void zgelsy_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), int *jpvt, SimTK_D_INPUT_(rcond), SimTK_I_OUTPUT_(rank), SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, SimTK_INFO_);
+
+extern void zgeql2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_INFO_);
+
+extern void zgeqlf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void zgeqp3_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), int *jpvt, SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, SimTK_INFO_);
+
+extern void zgeqpf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), int *jpvt, SimTK_Z_ *tau, SimTK_Z_ *work, double *rwork, SimTK_INFO_);
+
+extern void zgeqr2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_INFO_);
+
+extern void zgeqrf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void zgerfs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *af, SimTK_FDIM_(ldaf), const int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void zgerq2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_INFO_);
+
+extern void zgerqf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void zgesc2_(SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *rhs, int *ipiv, int *jpiv, SimTK_D_OUTPUT_(scale));
+
+extern void zgesv_(SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_);
+
+extern void zgesvx_(SimTK_FOPT_(fact), SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *af, SimTK_FDIM_(ldaf), int *ipiv, SimTK_CHAR_OUTPUT_(equed), double *r__, double *c__, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(trans), SimTK_FLEN_(equed));
+
+extern void zgetc2_(SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), int *ipiv, int *jpiv, SimTK_INFO_);
+
+extern void zgetf2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_INFO_);
+
+extern void zgetrf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_INFO_);
+
+extern void zgetri_(SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), const int *ipiv, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void zgetrs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_Z_ *a, SimTK_FDIM_(lda), const int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void zggbak_(SimTK_FOPT_(job), SimTK_FOPT_(side), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), const double *lscale, const double *rscale, SimTK_FDIM_(m), SimTK_Z_ *v, SimTK_FDIM_(ldv), SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(side));
+
+extern void zggbal_(SimTK_FOPT_(job), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_I_OUTPUT_(ilo), SimTK_I_OUTPUT_(ihi), double *lscale, double *rscale, double *work, SimTK_INFO_, SimTK_FLEN_(job));
+
+extern void zgges_(SimTK_FOPT_(jobvsl), SimTK_FOPT_(jobvsr), SimTK_FOPT_(sort), SimTK_SELECT_2Z delctg, SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), int *sdim, SimTK_Z_ *alpha, SimTK_Z_ *beta, SimTK_Z_ *vsl, SimTK_FDIM_(ldvsl), SimTK_Z_ *vsr, SimTK_FDIM_(ldvsr), SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, int *bwork, SimTK_INFO_, SimTK_FLEN_(jobvsl), SimTK_FLEN_(jobvsr), SimTK_FLEN_(sort));
+
+extern void zggesx_(SimTK_FOPT_(jobvsl), SimTK_FOPT_(jobvsr), SimTK_FOPT_(sort), SimTK_SELECT_2Z delctg, SimTK_FOPT_(sense), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_I_OUTPUT_(sdim), SimTK_Z_ *alpha, SimTK_Z_ *beta, SimTK_Z_ *vsl, SimTK_FDIM_(ldvsl), SimTK_Z_ *vsr, SimTK_FDIM_(ldvsr), SimTK_D_OUTPUT_(rconde), SimTK_D_OUTPUT_(rcondv), SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, int *iwork, SimTK_FDIM_(liwork), int *bwork, SimTK_INFO_, SimT [...]
+
+extern void zggev_(SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *alpha, SimTK_Z_ *beta, SimTK_Z_ *vl, SimTK_FDIM_(ldvl), SimTK_Z_ *vr, SimTK_FDIM_(ldvr), SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, SimTK_INFO_, SimTK_FLEN_(jobvl), SimTK_FLEN_(jobvr));
+
+extern void zggevx_(SimTK_FOPT_(balanc), SimTK_FOPT_(jobvl), SimTK_FOPT_(jobvr), SimTK_FOPT_(sense), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *alpha, SimTK_Z_ *beta, SimTK_Z_ *vl, SimTK_FDIM_(ldvl), SimTK_Z_ *vr, SimTK_FDIM_(ldvr), SimTK_I_OUTPUT_(ilo), SimTK_I_OUTPUT_(ihi), double *lscale, double *rscale, SimTK_D_OUTPUT_(abnrm), SimTK_D_OUTPUT_(bbnrm), SimTK_D_OUTPUT_(rconde), SimTK_D_OUTPUT_(rcondv), SimTK_Z_ *work, SimTK_FDIM_(lwork), doub [...]
+
+extern void zggglm_(SimTK_FDIM_(n), SimTK_FDIM_(m), SimTK_FDIM_(p), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *d__, SimTK_Z_ *x, SimTK_Z_ *y, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void zgghrd_(SimTK_FOPT_(compq), SimTK_FOPT_(compz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *q, SimTK_FDIM_(ldq), SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_INFO_, SimTK_FLEN_(compq), SimTK_FLEN_(compz));
+
+extern void zgglse_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(p), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *c__, SimTK_Z_ *d__, SimTK_Z_ *x, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void zggqrf_(SimTK_FDIM_(n), SimTK_FDIM_(m), SimTK_FDIM_(p), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *taua, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *taub, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void zggrqf_(SimTK_FDIM_(m), SimTK_FDIM_(p), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *taua, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *taub, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void zggsvd_(SimTK_FOPT_(jobu), SimTK_FOPT_(jobv), SimTK_FOPT_(jobq), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(p), SimTK_I_OUTPUT_(k), SimTK_I_OUTPUT_(l), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), double *alpha, double *beta, SimTK_Z_ *u, SimTK_FDIM_(ldu), SimTK_Z_ *v, SimTK_FDIM_(ldv), SimTK_Z_ *q, SimTK_FDIM_(ldq), SimTK_Z_ *work, double *rwork, int *iwork, SimTK_INFO_, SimTK_FLEN_(jobu), SimTK_FLEN_(jobv), SimTK_FLEN_(jobq));
+
+extern void zggsvp_(SimTK_FOPT_(jobu), SimTK_FOPT_(jobv), SimTK_FOPT_(jobq), SimTK_FDIM_(m), SimTK_FDIM_(p), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_D_INPUT_(tola), SimTK_D_INPUT_(tolb), SimTK_I_OUTPUT_(k), SimTK_I_OUTPUT_(l), SimTK_Z_ *u, SimTK_FDIM_(ldu), SimTK_Z_ *v, SimTK_FDIM_(ldv), SimTK_Z_ *q, SimTK_FDIM_(ldq), int *iwork, double *rwork, SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_INFO_, SimTK_FLEN_(jobu), SimTK_FLEN_(jobv), SimTK_FLEN_(jobq));
+
+extern void zgtcon_(SimTK_FOPT_(norm), SimTK_FDIM_(n), SimTK_Z_ *dl, SimTK_Z_ *d__, SimTK_Z_ *du, SimTK_Z_ *du2, const int *ipiv, SimTK_D_INPUT_(anorm),SimTK_D_OUTPUT_(rcond), SimTK_Z_ *work, SimTK_INFO_, SimTK_FLEN_(norm));
+
+extern void zgtrfs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *dl, SimTK_Z_ *d__, SimTK_Z_ *du, SimTK_Z_ *dlf, SimTK_Z_ *df, SimTK_Z_ *duf, SimTK_Z_ *du2, const int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void zgtsv_(SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *dl, SimTK_Z_ *d__, SimTK_Z_ *du, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_);
+
+extern void zgtsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *dl, SimTK_Z_ *d__, SimTK_Z_ *du, SimTK_Z_ *dlf, SimTK_Z_ *df, SimTK_Z_ *duf, SimTK_Z_ *du2, int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(trans));
+
+extern void zgttrf_(SimTK_FDIM_(n), SimTK_Z_ *dl, SimTK_Z_ *d__, SimTK_Z_ *du, SimTK_Z_ *du2, int *ipiv, SimTK_INFO_);
+
+extern void zgttrs_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *dl, SimTK_Z_ *d__, SimTK_Z_ *du, SimTK_Z_ *du2, const int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void zgtts2_(int *itrans, SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *dl, SimTK_Z_ *d__, SimTK_Z_ *du, SimTK_Z_ *du2, const int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb));
+
+extern void zhbev_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_Z_ *ab, SimTK_FDIM_(ldab), double *w, SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void zhbevd_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_Z_ *ab, SimTK_FDIM_(ldab), double *w, SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, SimTK_FDIM_(lrwork), int *iwork, SimTK_FDIM_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void zhbevx_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_Z_ *ab, SimTK_FDIM_(ldab), SimTK_Z_ *q, SimTK_FDIM_(ldq), SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_D_INPUT_(abstol), SimTK_FDIM_(m), double *w, SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_Z_ *work, double *rwork, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void zhbgst_(SimTK_FOPT_(vect), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(ka), SimTK_FDIM_(kb), SimTK_Z_ *ab, SimTK_FDIM_(ldab), SimTK_Z_ *bb, SimTK_FDIM_(ldbb), SimTK_Z_ *x, SimTK_FDIM_(ldx), SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(vect), SimTK_FLEN_(uplo));
+
+extern void zhbgv_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(ka), SimTK_FDIM_(kb), SimTK_Z_ *ab, SimTK_FDIM_(ldab), SimTK_Z_ *bb, SimTK_FDIM_(ldbb), double *w, SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void zhbgvx_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(ka), SimTK_FDIM_(kb), SimTK_Z_ *ab, SimTK_FDIM_(ldab), SimTK_Z_ *bb, SimTK_FDIM_(ldbb), SimTK_Z_ *q, SimTK_FDIM_(ldq), SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_D_INPUT_(abstol), SimTK_I_OUTPUT_(m), double *w, SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_Z_ *work, double *rwork, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_( [...]
+
+extern void zhbtrd_(SimTK_FOPT_(vect), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_Z_ *ab, SimTK_FDIM_(ldab), double *d__, double *e, SimTK_Z_ *q, SimTK_FDIM_(ldq), SimTK_Z_ *work, SimTK_INFO_, SimTK_FLEN_(vect), SimTK_FLEN_(uplo));
+
+extern void zhecon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), const int *ipiv, SimTK_D_INPUT_(anorm), SimTK_D_OUTPUT_(rcond), SimTK_Z_ *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zheev_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), double *w, SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void zheevd_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), double *w, SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, SimTK_FDIM_(lrwork), int *iwork, SimTK_FDIM_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void zheevr_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_D_INPUT_(abstol), SimTK_I_OUTPUT_(m), double *w, SimTK_Z_ *z__, SimTK_FDIM_(ldz), int *isuppz, SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, SimTK_FDIM_(lrwork), int *iwork, SimTK_FDIM_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void zheevx_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_D_INPUT_(abstol), SimTK_I_OUTPUT_(m), double *w, SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void zhegs2_(SimTK_I_INPUT_(itype), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zhegst_(SimTK_I_INPUT_(itype), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zhegv_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), double *w, SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void zhegvd_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), double *w, SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, int *lrwork, int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void zhegvx_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_D_INPUT_(abstol), SimTK_I_OUTPUT_(m), double *w, SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void zherfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *af, SimTK_FDIM_(ldaf), const int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zhesv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *a, SimTK_FDIM_(lda), const int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zhesvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *af, SimTK_FDIM_(ldaf), int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo));
+
+extern void zhetf2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zhetrd_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), double *d__, double *e, SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zhetrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zhetri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), const int *ipiv, SimTK_Z_ *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zhetrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_Z_ *a, SimTK_FDIM_(lda), const int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zhgeqz_(SimTK_FOPT_(job), SimTK_FOPT_(compq), SimTK_FOPT_(compz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *alpha, SimTK_Z_ *beta, SimTK_Z_ *q, SimTK_FDIM_(ldq), SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(compq), SimTK_FLEN_(compz));
+
+extern void zhpcon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_Z_ *ap, const int *ipiv, SimTK_D_INPUT_(anorm), SimTK_D_OUTPUT_(rcond), SimTK_Z_ *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zhpev_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *ap, double *w, SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void zhpevd_(SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *ap, double *w, SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, SimTK_FDIM_(lrwork), int *iwork, SimTK_FDIM_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void zhpevx_(SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *ap, SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_D_INPUT_(abstol), SimTK_I_OUTPUT_(m), double *w, SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_Z_ *work, double *rwork, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void zhpgst_(SimTK_I_INPUT_(itype), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *ap, const SimTK_Z_ *bp, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zhpgv_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *ap, SimTK_Z_ *bp, double *w, SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void zhpgvd_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *ap, SimTK_Z_ *bp, double *w, SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, SimTK_FDIM_(lrwork), int *iwork, SimTK_FDIM_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(uplo));
+
+extern void zhpgvx_(SimTK_I_INPUT_(itype), SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *ap, SimTK_Z_ *bp, SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_D_INPUT_(abstol), SimTK_I_OUTPUT_(m), double *w, SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_Z_ *work, double *rwork, int *iwork, int *ifail, SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range), SimTK_FLEN_(uplo));
+
+extern void zhprfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_Z_ *ap, const SimTK_Z_ *afp, const int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zhpsv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *ap, int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zhpsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_Z_ *ap, const SimTK_Z_ *afp, const int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo));
+
+extern void zhptrd_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *ap, double *d__, double *e, SimTK_Z_ *tau, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zhptrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *ap, int *ipiv, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zhptri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *ap, const int *ipiv, SimTK_Z_ *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zhptrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_Z_ *ap, const int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zhsein_(SimTK_FOPT_(side), SimTK_FOPT_(eigsrc), SimTK_FOPT_(initv), const int *select, SimTK_FDIM_(n), const SimTK_Z_ *h__, SimTK_FDIM_(ldh), SimTK_Z_ *w, SimTK_Z_ *vl, SimTK_FDIM_(ldvl), SimTK_Z_ *vr, SimTK_FDIM_(ldvr), SimTK_FDIM_(mm), SimTK_I_OUTPUT_(m), SimTK_Z_ *work, double *rwork, int *ifaill, int *ifailr, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(eigsrc), SimTK_FLEN_(initv));
+
+extern void zhseqr_(SimTK_FOPT_(job), SimTK_FOPT_(compz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_Z_ *h__, SimTK_FDIM_(ldh), SimTK_Z_ *w, SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(compz));
+
+extern void zlabrd_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(nb), SimTK_Z_ *a, SimTK_FDIM_(lda), double *d__, double *e, SimTK_Z_ *tauq, SimTK_Z_ *taup, SimTK_Z_ *x, SimTK_FDIM_(ldx), SimTK_Z_ *y, SimTK_FDIM_(ldy));
+
+extern void zlacgv_(SimTK_FDIM_(n), SimTK_Z_ *x, SimTK_FINC_(x));
+
+extern void zlacon_(SimTK_FDIM_(n), SimTK_Z_ *v, SimTK_Z_ *x, SimTK_D_OUTPUT_(est), SimTK_I_OUTPUT_(kase) );
+
+extern void zlacn2_( SimTK_FDIM_(n), SimTK_Z_ *v, SimTK_Z_ *x, SimTK_D_OUTPUT_(est), SimTK_I_OUTPUT_(kase), int *isave );
+
+extern void zlacp2_(SimTK_FOPT_(uplo), SimTK_FDIM_(m), SimTK_FDIM_(n), const double *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_FLEN_(uplo));
+
+extern void zlacpy_(SimTK_FOPT_(uplo), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_FLEN_(uplo));
+
+extern void zlacrm_(SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_Z_ *a, SimTK_FDIM_(lda), const double *b, SimTK_FDIM_(ldb), const SimTK_Z_ *c__, SimTK_FDIM_(ldc), double *rwork);
+
+extern void zlacrt_(SimTK_FDIM_(n), SimTK_Z_ *cx, SimTK_FINC_(x), SimTK_Z_ *cy, SimTK_FINC_(y), const SimTK_Z_ *c__, const SimTK_Z_ *s);
+
+extern void zlaed0_(SimTK_I_INPUT_(qsiz), SimTK_FDIM_(n), double *d__, const double *e, SimTK_Z_ *q, SimTK_FDIM_(ldq), SimTK_Z_ *qstore, SimTK_FDIM_(ldqs), double *rwork, int *iwork, SimTK_INFO_);
+
+extern void zlaed7_(SimTK_FDIM_(n), SimTK_I_INPUT_(cutpnt), SimTK_FDIM_(qsiz), SimTK_I_INPUT_(tlvls), SimTK_I_INPUT_(curlvl), SimTK_I_INPUT_(curpbm), double *d__, SimTK_Z_ *q, SimTK_FDIM_(ldq), SimTK_D_INPUT_(rho), int *indxq, double *qstore, int *qptr, const int *prmptr, const int *perm, const int *givptr, const int *givcol, const double *givnum, SimTK_Z_ *work, double *rwork, int *iwork, SimTK_INFO_);
+
+extern void zlaed8_(SimTK_I_OUTPUT_(k), SimTK_FDIM_(n), SimTK_FDIM_(qsiz), SimTK_Z_ *q, SimTK_FDIM_(ldq), double *d__, SimTK_D_OUTPUT_(rho), SimTK_I_INPUT_(cutpnt), double *z__, double *dlamda, SimTK_Z_ *q2, SimTK_FDIM_(ldq2), double *w, int *indxp, int *indx, const int *indxq, int *perm, int *givptr, int *givcol, double *givnum, SimTK_INFO_);
+
+extern void zlaein_(SimTK_I_INPUT_(rightv), SimTK_I_INPUT_(noinit), SimTK_FDIM_(n), const SimTK_Z_ *h__, SimTK_FDIM_(ldh), const SimTK_Z_ *w, SimTK_Z_ *v, SimTK_Z_ *b, SimTK_FDIM_(ldb), double *rwork, SimTK_D_INPUT_(eps3), SimTK_D_INPUT_(smlnum), SimTK_INFO_);
+
+extern void zlaesy_(SimTK_Z_INPUT_(a), SimTK_Z_INPUT_(b), SimTK_Z_INPUT_(c__), SimTK_Z_OUTPUT_(rt1), SimTK_Z_OUTPUT_(rt2), SimTK_Z_OUTPUT_(evscal), SimTK_Z_OUTPUT_(cs1), SimTK_Z_OUTPUT_(sn1));
+
+extern void zlaev2_(SimTK_Z_INPUT_(a), SimTK_Z_INPUT_(b), SimTK_Z_INPUT_(c__), SimTK_D_OUTPUT_(rt1), SimTK_D_OUTPUT_(rt2), SimTK_D_OUTPUT_(cs1), SimTK_Z_OUTPUT_(sn1));
+
+extern void zlags2_(SimTK_I_INPUT_(upper), SimTK_D_INPUT_(a1), SimTK_Z_INPUT_(a2), SimTK_D_INPUT_(a3), SimTK_D_INPUT_(b1), SimTK_Z_INPUT_(b2), SimTK_D_INPUT_(b3), SimTK_D_OUTPUT_(csu), SimTK_Z_OUTPUT_(snu), SimTK_D_OUTPUT_(csv), SimTK_Z_OUTPUT_(snv), SimTK_D_OUTPUT_(csq), SimTK_Z_OUTPUT_(snq) );
+
+extern void zlagtm_(SimTK_FOPT_(trans), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_D_INPUT_(alpha), const SimTK_Z_ *dl, const SimTK_Z_ *d__, const SimTK_Z_ *du, const SimTK_Z_ *x, SimTK_FDIM_(ldx), SimTK_D_INPUT_(beta), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_FLEN_(trans));
+
+extern void zlag2c_( SimTK_I_INPUT_(m), SimTK_I_INPUT_(n), const SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_C_ *sa, SimTK_FDIM_(ldsa), SimTK_INFO_);
+
+extern void zlahef_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nb), SimTK_FDIM_(kb), SimTK_Z_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_Z_ *w, SimTK_FDIM_(ldw), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zlahqr_(SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_Z_ *h__, SimTK_FDIM_(ldh), SimTK_Z_ *w, SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_INFO_);
+
+extern void zlahrd_(SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(nb), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *tau, SimTK_Z_ *t, SimTK_FDIM_(ldt), SimTK_Z_ *y, SimTK_FDIM_(ldy));
+
+extern void zlahr2_(SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(nb), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *tau, SimTK_Z_ *t, SimTK_FDIM_(ldt), SimTK_Z_ *y, SimTK_FDIM_(ldy));
+
+extern void zlaic1_(SimTK_I_INPUT_(job), SimTK_FDIM_(j), const SimTK_Z_ *x, SimTK_D_INPUT_(sest), const SimTK_Z_ *w, SimTK_Z_INPUT_(gamma), SimTK_D_OUTPUT_(sestpr), SimTK_Z_OUTPUT_(s), SimTK_Z_OUTPUT_(c__));
+
+extern void zlals0_(SimTK_I_INPUT_(icompq), SimTK_I_INPUT_(nl), SimTK_I_INPUT_(nr), SimTK_I_INPUT_(sqre), SimTK_FDIM_(nrhs), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *bx, SimTK_FDIM_(ldbx), int *perm, int *givptr, const int *givcol, SimTK_FDIM_(ldgcol), double *givnum, SimTK_FDIM_(ldgnum), double *poles, double *difl, double *difr, double *z__, SimTK_FDIM_(k), double *c__, double *s, double *rwork, SimTK_INFO_);
+
+extern void zlalsa_(SimTK_I_INPUT_(icompq), SimTK_I_INPUT_(smlsiz), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *bx, SimTK_FDIM_(ldbx), double *u, SimTK_FDIM_(ldu), double *vt, int *k, double *difl, double *difr, double *z__, double *poles, int *givptr, const int *givcol, SimTK_FDIM_(ldgcol), int *perm, double *givnum, double *c__, double *s, double *rwork, int *iwork, SimTK_INFO_);
+
+extern void zlapll_(SimTK_FDIM_(n), SimTK_Z_ *x, SimTK_FINC_(x), SimTK_Z_ *y, SimTK_FINC_(y), SimTK_D_OUTPUT_(ssmin));
+
+extern void zlapmt_(SimTK_I_INPUT_(forwrd), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *x, SimTK_FDIM_(ldx), SimTK_FDIM_(k));
+
+extern void zlaqgb_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(kl), SimTK_FDIM_(ku), SimTK_Z_ *ab, SimTK_FDIM_(ldab), double *r__, double *c__, SimTK_D_OUTPUT_(rowcnd), SimTK_D_OUTPUT_(colcnd), SimTK_D_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(equed) );
+
+extern void zlaqge_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), double *r__, double *c__, SimTK_D_INPUT_(rowcnd), SimTK_D_INPUT_(colcnd), SimTK_D_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(equed) );
+
+extern void zlaqhb_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_Z_ *ab, SimTK_FDIM_(ldab), double *s, SimTK_D_INPUT_(scond), SimTK_D_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(uplo), SimTK_FLEN_(equed) );
+
+extern void zlaqhe_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), const double *s, SimTK_D_INPUT_(scond), SimTK_D_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void zlaqhp_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *ap, const double *s, SimTK_D_INPUT_(scond), SimTK_D_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void zlaqp2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_I_INPUT_(offset), SimTK_Z_ *a, SimTK_FDIM_(lda), int *jpvt, SimTK_Z_ *tau, double *vn1, double *vn2, SimTK_Z_ *work);
+
+extern void zlaqps_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_I_INPUT_(offset), SimTK_I_INPUT_(nb), SimTK_I_OUTPUT_(kb), SimTK_Z_ *a, SimTK_FDIM_(lda), int *jpvt, SimTK_Z_ *tau, double *vn1, double *vn2, SimTK_Z_ *auxv, SimTK_Z_ *f, SimTK_FDIM_(ldf));
+
+extern void zlaqr0_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_Z_ *h, SimTK_FDIM_(ldh), SimTK_Z_ *w, SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), SimTK_Z_ *z, SimTK_FDIM_(ldz), SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_ );
+
+extern void zlaqr1_(SimTK_FDIM_(n), const SimTK_Z_ *h, SimTK_FDIM_(ldh), SimTK_Z_INPUT_(s1), SimTK_Z_INPUT_(s2), SimTK_Z_ *v );
+
+extern void zlaqr2_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ktop), SimTK_I_INPUT_(kbot), SimTK_I_INPUT_(nw), SimTK_Z_ *h, SimTK_FDIM_(ldh), SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), SimTK_Z_ *z, SimTK_FDIM_(ldz), SimTK_I_OUTPUT_(ns), SimTK_I_OUTPUT_(nd), SimTK_Z_ *sh, SimTK_Z_ *v, SimTK_FDIM_(ldv), SimTK_I_INPUT_(nh), SimTK_Z_ *t, SimTK_FDIM_(ldt), SimTK_I_INPUT_(nv), SimTK_Z_ *wv, SimTK_I_INPUT_(ldwv), SimTK_Z_ *work, SimTK_FDIM_(lwork) );
+
+extern void zlaqr3_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ktop), SimTK_I_INPUT_(kbot), SimTK_I_INPUT_(nw), SimTK_Z_ *h, SimTK_FDIM_(ldh), SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), SimTK_Z_ *z, SimTK_FDIM_(ldz), SimTK_I_OUTPUT_(ns), SimTK_I_OUTPUT_(nd), SimTK_Z_ *sh, SimTK_Z_ *v, SimTK_FDIM_(ldv), SimTK_I_INPUT_(nh), SimTK_Z_ *t, SimTK_FDIM_(ldt), SimTK_I_INPUT_(nv), SimTK_Z_ *wv, SimTK_I_INPUT_(ldwv), SimTK_Z_ *work, SimTK_FDIM_(lwork) );
+
+extern void zlaqr4_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_Z_ *h, SimTK_FDIM_(ldh), SimTK_Z_ *w, SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), SimTK_Z_ *z, SimTK_FDIM_(ldz), SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_ );
+
+extern void zlaqr5_( SimTK_I_INPUT_(wantt), SimTK_I_INPUT_(wantz), SimTK_I_INPUT_(kacc22), SimTK_FDIM_(n), SimTK_I_INPUT_(ktop), SimTK_I_INPUT_(kbot), SimTK_I_INPUT_(nshifts), const SimTK_Z_ *s, SimTK_Z_ *h, SimTK_FDIM_(ldh), SimTK_I_INPUT_(iloz), SimTK_I_INPUT_(ihiz), SimTK_Z_ *z, SimTK_FDIM_(ldz), SimTK_Z_ *v, SimTK_FDIM_(ldv), SimTK_Z_ *u, SimTK_FDIM_(ldu), SimTK_I_INPUT_(nh), SimTK_Z_ *wh, SimTK_FDIM_(ldwh), SimTK_I_INPUT_(nv), SimTK_Z_ *wv, SimTK_FDIM_(ldwv) );
+
+extern void zlaqsb_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_Z_ *ab, SimTK_FDIM_(ldab), double *s, SimTK_D_INPUT_(scond), SimTK_D_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void zlaqsp_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *ap, const double *s, SimTK_D_INPUT_(scond), SimTK_D_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(uplo), SimTK_FLEN_(equed) );
+
+extern void zlaqsy_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), const double *s, SimTK_D_INPUT_(scond), SimTK_D_INPUT_(amax), SimTK_CHAR_OUTPUT_(equed), SimTK_FLEN_(uplo), SimTK_FLEN_(equed) );
+
+extern void zlar1v_(SimTK_FDIM_(n), SimTK_I_INPUT_(b1), SimTK_I_INPUT_(bn), SimTK_D_INPUT_(lambda), const double *d__, const double *l, const double *ld, const double *lld, SimTK_D_INPUT_(pivmin), SimTK_D_INPUT_(gaptol), SimTK_Z_ *z__, SimTK_I_INPUT_(wantnc), SimTK_I_OUTPUT_(negcnt), SimTK_D_OUTPUT_(ztz), SimTK_D_OUTPUT_(mingma), SimTK_I_OUTPUT_(r__), int *isuppz, SimTK_D_OUTPUT_(nrminv), SimTK_D_OUTPUT_(resid), SimTK_D_OUTPUT_(rqcorr), double *work);
+
+
+extern void zlar2v_(SimTK_FDIM_(n), SimTK_Z_ *x, SimTK_Z_ *y, SimTK_Z_ *z__, SimTK_FINC_(x), const double *c__, const SimTK_Z_ *s, SimTK_FINC_(c));
+
+extern void zlarcm_(SimTK_FDIM_(m), SimTK_FDIM_(n), const double *a, SimTK_FDIM_(lda), const SimTK_Z_ *b, SimTK_FDIM_(ldb), const SimTK_Z_ *c__, SimTK_FDIM_(ldc), double *rwork);
+
+extern void zlarf_(SimTK_FOPT_(side), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_Z_ *v, SimTK_FINC_(v), SimTK_Z_INPUT_(tau), SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_FLEN_(side));
+
+extern void zlarfb_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FOPT_(direct), SimTK_FOPT_(storev), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_Z_ *v, SimTK_FDIM_(ldv), const SimTK_Z_ *t, SimTK_FDIM_(ldt), SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_FDIM_(ldwork), SimTK_FLEN_(side), SimTK_FLEN_(trans), SimTK_FLEN_(direct), SimTK_FLEN_(storev));
+
+extern void zlarfg_(SimTK_FDIM_(n), SimTK_Z_OUTPUT_(alpha), SimTK_Z_ *x, SimTK_FINC_(x), SimTK_Z_OUTPUT_(tau) );
+
+extern void zlarft_(SimTK_FOPT_(direct), SimTK_FOPT_(storev), SimTK_FDIM_(n), SimTK_I_INPUT_(k), SimTK_Z_ *v, SimTK_FDIM_(ldv), const SimTK_Z_ *tau, SimTK_Z_ *t, SimTK_FDIM_(ldt), SimTK_FLEN_(direct), SimTK_FLEN_(storev));
+
+extern void zlarfx_(SimTK_FOPT_(side), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_Z_ *v, const SimTK_Z_ *tau, SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_FLEN_(side));
+
+extern void zlargv_(SimTK_FDIM_(n), SimTK_Z_ *x, SimTK_FINC_(x), SimTK_Z_ *y, SimTK_FINC_(y), double *c__, SimTK_FINC_(c));
+
+extern void zlarnv_(SimTK_I_INPUT_(idist), int *iseed, SimTK_FDIM_(n), SimTK_Z_ *x);
+
+extern void zlarrv_(SimTK_FDIM_(n), SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), double *d__, double *l, SimTK_D_INPUT_(pivmin), const int *isplit, SimTK_FDIM_(m), SimTK_I_INPUT_(dol), SimTK_I_INPUT_(dou), SimTK_D_INPUT_(minrgp), SimTK_D_INPUT_(rtol1), SimTK_D_INPUT_(rtol2), double *w, double *werr, double *wgap, const int *iblock, const int *indexw, const double *gers, SimTK_Z_ *z__, SimTK_FDIM_(ldz), int *isuppz, double *work, int *iwork, SimTK_INFO_);
+
+
+extern void zlartg_(const SimTK_Z_ *f, const SimTK_Z_ *g, double *cs, SimTK_Z_ *sn, SimTK_Z_ *r__);
+
+extern void zlartv_(SimTK_FDIM_(n), SimTK_Z_ *x, SimTK_FINC_(x), SimTK_Z_ *y, SimTK_FINC_(y), const double *c__, const SimTK_Z_ *s, SimTK_FINC_(c));
+
+extern void zlarz_(SimTK_FOPT_(side), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_I_INPUT_(L), const SimTK_Z_ *v, SimTK_FINC_(v), SimTK_Z_INPUT_(tau), SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_FLEN_(side));
+
+extern void zlarzb_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FOPT_(direct), SimTK_FOPT_(storev), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(l), const SimTK_Z_ *v, SimTK_FDIM_(ldv), SimTK_Z_ *t, SimTK_FDIM_(ldt), SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_FDIM_(ldwork), SimTK_FLEN_(side), SimTK_FLEN_(trans), SimTK_FLEN_(direct), SimTK_FLEN_(storev));
+
+extern void zlarzt_(SimTK_FOPT_(direct), SimTK_FOPT_(storev), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_Z_ *v, SimTK_FDIM_(ldv), const SimTK_Z_ *tau, SimTK_Z_ *t, SimTK_FDIM_(ldt), SimTK_FLEN_(direct), SimTK_FLEN_(storev));
+
+extern void zlascl_(SimTK_FOPT_(type__), SimTK_FDIM_(kl), SimTK_FDIM_(ku), const double *cfrom, const double *cto, SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(type));
+
+extern void zlaset_(SimTK_FOPT_(uplo), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_INPUT_(alpha), SimTK_Z_INPUT_(beta), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_FLEN_(uplo));
+
+extern void zlasr_(SimTK_FOPT_(side), SimTK_FOPT_(pivot), SimTK_FOPT_(direct), SimTK_FDIM_(m), SimTK_FDIM_(n), const double *c__, const double *s, SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_FLEN_(side), SimTK_FLEN_(pivot), SimTK_FLEN_(direct));
+
+extern void zlassq_(SimTK_FDIM_(n), const SimTK_Z_ *x, SimTK_FINC_(x), SimTK_D_OUTPUT_(scale), double *sumsq);
+
+extern void zlaswp_(SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_I_OUTPUT_(k1), SimTK_I_OUTPUT_(k2), const int *ipiv, SimTK_FINC_(x));
+
+extern void zlasyf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_I_INPUT_(nb), SimTK_I_INPUT_(kb), SimTK_Z_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_Z_ *w, SimTK_FDIM_(ldw), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zlatbs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FOPT_(normin), SimTK_FDIM_(n), SimTK_I_INPUT_(kd), const SimTK_Z_ *ab, SimTK_FDIM_(ldab), SimTK_Z_ *x, SimTK_D_OUTPUT_(scale), double *cnorm, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag), SimTK_FLEN_(normin) );
+
+extern void zlatdf_(SimTK_I_INPUT_(ijob), SimTK_FDIM_(n), const SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_Z_ *rhs, SimTK_D_OUTPUT_(rdsum), SimTK_D_OUTPUT_(rdscal), const int *ipiv, const int *jpiv);
+
+extern void zlatps_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FOPT_(normin), SimTK_FDIM_(n), const SimTK_Z_ *ap, SimTK_Z_ *x, SimTK_D_OUTPUT_(scale), double *cnorm, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag), SimTK_FLEN_(normin) );
+
+extern void zlatrd_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nb), SimTK_Z_ *a, SimTK_FDIM_(lda), double *e, SimTK_Z_ *tau, SimTK_Z_ *w, SimTK_FDIM_(ldw), SimTK_FLEN_(uplo));
+
+extern void zlatrs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FOPT_(normin), SimTK_FDIM_(n), const SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *x, SimTK_D_OUTPUT_(scale), double *cnorm, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag), SimTK_FLEN_(normin));
+
+extern void zlatrz_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(l), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *tau, SimTK_Z_ *work);
+
+extern void zlatzm_(SimTK_FOPT_(side), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_Z_ *v, SimTK_FINC_(v), SimTK_Z_INPUT_(tau), SimTK_Z_ *c1, SimTK_Z_ *c2, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_FLEN_(side));
+
+extern void zlauu2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zlauum_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zpbcon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), const SimTK_Z_ *ab, SimTK_FDIM_(ldab), SimTK_D_INPUT_(anorm), SimTK_D_OUTPUT_(rcond), SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zpbequ_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_I_INPUT_(kd), const SimTK_Z_ *ab, SimTK_FDIM_(ldab), SimTK_D_OUTPUT_(s), SimTK_D_OUTPUT_(scond), SimTK_D_OUTPUT_(amax), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zpbrfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), const SimTK_Z_ *ab, SimTK_FDIM_(ldab), const SimTK_Z_ *afb, SimTK_FDIM_(ldafb), const SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zpbstf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_Z_ *ab, SimTK_FDIM_(ldab), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zpbsv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), SimTK_Z_ *ab, SimTK_FDIM_(ldab), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zpbsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), SimTK_Z_ *ab, SimTK_FDIM_(ldab), SimTK_Z_ *afb, SimTK_FDIM_(ldafb), SimTK_CHAR_OUTPUT_(equed), double *s, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void zpbtf2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_Z_ *ab, SimTK_FDIM_(ldab), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zpbtrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_Z_ *ab, SimTK_FDIM_(ldab), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zpbtrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), const SimTK_Z_ *ab, SimTK_FDIM_(ldab), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zpocon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_D_INPUT_(anorm), SimTK_D_OUTPUT_(rcond), SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zpoequ_(SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), double *s, SimTK_D_OUTPUT_(scond), SimTK_D_OUTPUT_(amax), SimTK_INFO_);
+
+extern void zporfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *af, SimTK_FDIM_(ldaf), const SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zposv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zposvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *af, SimTK_FDIM_(ldaf), SimTK_CHAR_OUTPUT_(equed), double *s, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void zpotf2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zpotrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zpotri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zpotrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zppcon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_Z_ *ap, SimTK_D_INPUT_(anorm), SimTK_D_OUTPUT_(rcond), SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zppequ_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_Z_ *ap, double *s, SimTK_D_OUTPUT_(scond), SimTK_D_OUTPUT_(amax), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zpprfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_Z_ *ap, const SimTK_Z_ *afp, const SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zppsv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *ap, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(fact));
+
+extern void zppsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *ap, SimTK_Z_ *afp, SimTK_CHAR_OUTPUT_(equed), double *s, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo), SimTK_FLEN_(equed));
+
+extern void zpptrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *ap, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zpptri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *ap, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zpptrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_Z_ *ap, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zptcon_(SimTK_FDIM_(n), const double *d__, const SimTK_Z_ *e, SimTK_D_INPUT_(anorm), SimTK_D_OUTPUT_(rcond), double *rwork, SimTK_INFO_);
+
+extern void zptrfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *d__, const SimTK_Z_ *e, const double *df, const SimTK_Z_ *ef, const SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zptsv_(SimTK_FDIM_(n), SimTK_FDIM_(nrhs), double *d__, SimTK_Z_ *e, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_);
+
+extern void zptsvx_(SimTK_FOPT_(fact), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *d__, const SimTK_Z_ *e, double *df, SimTK_Z_ *ef, const SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(fact));
+
+extern void zpttrf_(SimTK_FDIM_(n), double *d__, SimTK_Z_ *e, SimTK_INFO_);
+
+extern void zpttrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *d__, const SimTK_Z_ *e, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zptts2_(SimTK_I_INPUT_(iuplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const double *d__, const SimTK_Z_ *e, SimTK_Z_ *b, SimTK_FDIM_(ldb));
+
+extern void zrot_(SimTK_FDIM_(n), SimTK_Z_ *cx, SimTK_FINC_(x), SimTK_Z_ *cy, SimTK_FINC_(y), const double *c__, const SimTK_Z_ *s);
+
+extern void zspcon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_Z_ *ap, const int *ipiv, SimTK_D_INPUT_(anorm), SimTK_D_OUTPUT_(rcond), SimTK_Z_ *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zspmv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *ap, const SimTK_Z_ *x, SimTK_FINC_(x), SimTK_Z_ *beta, SimTK_Z_ *y, SimTK_FINC_(y), SimTK_FLEN_(uplo));
+
+extern void zspr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *x, SimTK_FINC_(x), SimTK_Z_ *ap, SimTK_FLEN_(uplo));
+
+extern void zsprfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_Z_ *ap, const SimTK_Z_ *afp, const int *ipiv, const SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zspsv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *ap, int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zspsvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_Z_ *ap, SimTK_Z_ *afp, int *ipiv, const SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo));
+
+extern void zsptrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *ap, int *ipiv, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zsptri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *ap, const int *ipiv, SimTK_Z_ *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zsptrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_Z_ *ap, const int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zstedc_(SimTK_FOPT_(compz), SimTK_FDIM_(n), double *d__, double *e, SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, int *lrwork, int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_, SimTK_FLEN_(compz));
+
+extern void zstein_(SimTK_FDIM_(n), const double *d__, const double *e, SimTK_FDIM_(m), const double *w, int *iblock, const int *isplit, const SimTK_Z_ *z__, SimTK_FDIM_(ldz), double *work, int *iwork, int *ifail, SimTK_INFO_);
+
+extern void zstemr_( SimTK_FOPT_(jobz), SimTK_FOPT_(range), SimTK_FDIM_(n), double *d, double *e, SimTK_D_INPUT_(vl), SimTK_D_INPUT_(vu), SimTK_I_INPUT_(il), SimTK_I_INPUT_(iu), SimTK_I_OUTPUT_(m), double *w, SimTK_Z_ *z, SimTK_FDIM_(ldz), SimTK_I_INPUT_(nzc), SimTK_I_OUTPUT_(isuppz), SimTK_I_OUTPUT_(tryrac), double *work, SimTK_FDIM_(lwork), int *iwork, SimTK_FDIM_(liwork), SimTK_INFO_, SimTK_FLEN_(jobz), SimTK_FLEN_(range) );
+
+extern void zsteqr_(SimTK_FOPT_(compz), SimTK_FDIM_(n), double *d__, double *e, SimTK_Z_ *z__, SimTK_FDIM_(ldz), double *work, SimTK_INFO_, SimTK_FLEN_(compz));
+
+extern void zsycon_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_Z_ *a, SimTK_FDIM_(lda), const int *ipiv, SimTK_D_INPUT_(anorm), SimTK_D_OUTPUT_(rcond), SimTK_Z_ *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zsymv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_INPUT_(alpha), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *x, SimTK_FINC_(x), SimTK_Z_INPUT_(beta), SimTK_Z_ *y, SimTK_FINC_(y), SimTK_FLEN_(uplo));
+
+extern void zsyr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_INPUT_(alpha), SimTK_Z_ *x, SimTK_FINC_(x), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_FLEN_(uplo));
+
+extern void zsyrfs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *af, SimTK_FDIM_(ldaf), const int *ipiv, const SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zsysv_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zsysvx_(SimTK_FOPT_(fact), SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *af, SimTK_FDIM_(ldaf), int *ipiv, const SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *x, SimTK_FDIM_(ldx), SimTK_D_OUTPUT_(rcond), double *ferr, double *berr, SimTK_Z_ *work, SimTK_FDIM_(lwork), double *rwork, SimTK_INFO_, SimTK_FLEN_(fact), SimTK_FLEN_(uplo));
+
+extern void zsytf2_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zsytrf_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zsytri_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_Z_ *a, SimTK_FDIM_(lda), const int *ipiv, SimTK_Z_ *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zsytrs_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), SimTK_Z_ *a, SimTK_FDIM_(lda), int *ipiv, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void ztbcon_(SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(kd), const SimTK_Z_ *ab, SimTK_FDIM_(ldab), SimTK_D_OUTPUT_(rcond), SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void ztbrfs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), const SimTK_Z_ *ab, SimTK_FDIM_(ldab), const SimTK_Z_ *b, SimTK_FDIM_(ldb), const SimTK_Z_ *x, SimTK_FDIM_(ldx), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void ztbtrs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(kd), SimTK_FDIM_(nrhs), const SimTK_Z_ *ab, SimTK_FDIM_(ldab), const SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void ztgevc_(SimTK_FOPT_(side), SimTK_FOPT_(howmny), const int *select, SimTK_FDIM_(n), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *vl, SimTK_FDIM_(ldvl), SimTK_Z_ *vr, SimTK_FDIM_(ldvr), SimTK_I_INPUT_(mm), SimTK_FDIM_(m), SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(howmny));
+
+extern void ztgex2_(SimTK_I_INPUT_(wantq), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *q, SimTK_FDIM_(ldq), SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_I_INPUT_(j1), SimTK_INFO_);
+
+extern void ztgexc_(SimTK_I_INPUT_(wantq), SimTK_I_INPUT_(wantz), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *q, SimTK_FDIM_(ldq), SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_I_OUTPUT_(ifst), SimTK_I_OUTPUT_(ilst), SimTK_INFO_);
+
+extern void ztgsen_(SimTK_I_INPUT_(ijob), SimTK_I_INPUT_(wantq), SimTK_I_INPUT_(wantz), const int *select, SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *alpha, SimTK_Z_ *beta, SimTK_Z_ *q, SimTK_FDIM_(ldq), SimTK_Z_ *z__, SimTK_FDIM_(ldz), SimTK_I_OUTPUT_(m), SimTK_D_OUTPUT_(pl), SimTK_D_OUTPUT_(pr), double *dif, SimTK_Z_ *work, SimTK_FDIM_(lwork), int *iwork, SimTK_I_INPUT_(liwork), SimTK_INFO_);
+
+extern void ztgsja_(SimTK_FOPT_(jobu), SimTK_FOPT_(jobv), SimTK_FOPT_(jobq), SimTK_FDIM_(m), SimTK_FDIM_(p), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(l), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_D_INPUT_(tola), SimTK_D_INPUT_(tolb), double *alpha, double *beta, SimTK_Z_ *u, SimTK_FDIM_(ldu), SimTK_Z_ *v, SimTK_FDIM_(ldv), SimTK_Z_ *q, SimTK_FDIM_(ldq), SimTK_Z_ *work, SimTK_I_OUTPUT_(ncycle), SimTK_INFO_, SimTK_FLEN_(jobu), SimTK_FLEN_(jobv), SimTK_FLEN_(jobq));
+
+extern void ztgsna_(SimTK_FOPT_(job), SimTK_FOPT_(howmny), const int *select, SimTK_FDIM_(n), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *b, SimTK_FDIM_(ldb), const SimTK_Z_ *vl, SimTK_FDIM_(ldvl), const SimTK_Z_ *vr, SimTK_FDIM_(ldvr), double *s, double *dif, SimTK_FDIM_(mm), SimTK_FDIM_(m), SimTK_Z_ *work, SimTK_FDIM_(lwork), int *iwork, SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(howmny));
+
+extern void ztgsy2_(SimTK_FOPT_(trans), SimTK_I_INPUT_(ijob), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *c__, SimTK_FDIM_(ldc), const SimTK_Z_ *d__, SimTK_FDIM_(ldd), const SimTK_Z_ *e, SimTK_FDIM_(lde), SimTK_Z_ *f, SimTK_FDIM_(ldf), SimTK_D_OUTPUT_(scale), SimTK_D_OUTPUT_(rdsum), SimTK_D_OUTPUT_(rdscal), SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void ztgsyl_(SimTK_FOPT_(trans), SimTK_I_INPUT_(ijob), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *c__, SimTK_FDIM_(ldc), const SimTK_Z_ *d__, SimTK_FDIM_(ldd), const SimTK_Z_ *e, SimTK_FDIM_(lde), SimTK_Z_ *f, SimTK_FDIM_(ldf), SimTK_D_OUTPUT_(scale), double *dif, SimTK_Z_ *work, SimTK_FDIM_(lwork), int *iwork, SimTK_INFO_, SimTK_FLEN_(trans));
+
+extern void ztpcon_(SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), const SimTK_Z_ *ap, SimTK_D_OUTPUT_(rcond), SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void ztprfs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_Z_ *ap, const SimTK_Z_ *b, SimTK_FDIM_(ldb), const SimTK_Z_ *x, SimTK_FDIM_(ldx), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void ztptri_(SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_Z_ *ap, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void ztptrs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_Z_ *ap, SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo),SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void ztrcon_(SimTK_FOPT_(norm), SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), const SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_D_OUTPUT_(rcond), SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(norm), SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void ztrevc_(SimTK_FOPT_(side), SimTK_FOPT_(howmny), const int *select, SimTK_FDIM_(n), SimTK_Z_ *t, SimTK_FDIM_(ldt), SimTK_Z_ *vl, SimTK_FDIM_(ldvl), SimTK_Z_ *vr, SimTK_FDIM_(ldvr), SimTK_FDIM_(mm), SimTK_FDIM_(m), SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(howmny));
+
+extern void ztrexc_(SimTK_FOPT_(compq), SimTK_FDIM_(n), SimTK_Z_ *t, SimTK_FDIM_(ldt), SimTK_Z_ *q, SimTK_FDIM_(ldq), SimTK_I_INPUT_(ifst), SimTK_I_INPUT_(ilst), SimTK_INFO_, SimTK_FLEN_(compq));
+
+extern void ztrrfs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *b, SimTK_FDIM_(ldb), const SimTK_Z_ *x, SimTK_FDIM_(ldx), double *ferr, double *berr, SimTK_Z_ *work, double *rwork, SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void ztrsen_(SimTK_FOPT_(job), SimTK_FOPT_(compq), const int *select, SimTK_FDIM_(n), SimTK_Z_ *t, SimTK_FDIM_(ldt), SimTK_Z_ *q, SimTK_FDIM_(ldq), SimTK_Z_ *w, SimTK_FDIM_(m), SimTK_D_OUTPUT_(s), SimTK_D_OUTPUT_(sep), SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(compq));
+
+extern void ztrsna_(SimTK_FOPT_(job), SimTK_FOPT_(howmny), const int *select, SimTK_FDIM_(n), SimTK_Z_ *t, SimTK_FDIM_(ldt), SimTK_Z_ *vl, SimTK_FDIM_(ldvl), SimTK_Z_ *vr, SimTK_FDIM_(ldvr), double *s, double *sep, SimTK_FDIM_(mm), SimTK_FDIM_(m), SimTK_Z_ *work, SimTK_FDIM_(ldwork), double *rwork, SimTK_INFO_, SimTK_FLEN_(job), SimTK_FLEN_(howmny));
+
+extern void ztrsyl_(char *tranA, char *tranB, int *isgn, SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_D_OUTPUT_(scale), SimTK_INFO_, SimTK_FLEN_(transA), SimTK_FLEN_(transB));
+
+extern void ztrti2_(SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void ztrtri_(SimTK_FOPT_(uplo), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(diag));
+
+extern void ztrtrs_(SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FOPT_(diag), SimTK_FDIM_(n), SimTK_FDIM_(nrhs), const SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *b, SimTK_FDIM_(ldb), SimTK_INFO_, SimTK_FLEN_(uplo), SimTK_FLEN_(trans), SimTK_FLEN_(diag));
+
+extern void ztzrqf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *tau, SimTK_INFO_);
+
+extern void ztzrzf_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void zung2l_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_INFO_);
+
+extern void zung2r_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_INFO_);
+
+extern void zungbr_(SimTK_FOPT_(vect), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(vect));
+
+extern void zunghr_(SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void zungl2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_INFO_);
+
+extern void zunglq_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void zungql_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void zungqr_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void zungr2_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_INFO_);
+
+extern void zungrq_(SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_);
+
+extern void zungtr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zunm2l_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void zunm2r_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void zunmbr_(SimTK_FOPT_(vect), SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(vect), SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void zunmhr_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_I_INPUT_(ilo), SimTK_I_INPUT_(ihi), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void zunml2_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void zunmlq_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void zunmql_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void zunmqr_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void zunmr2_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void zunmr3_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_I_INPUT_(l), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(trans));
+
+extern void zunmrq_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(uplo));
+
+extern void zunmrz_(SimTK_FOPT_(side), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), SimTK_FDIM_(k), SimTK_FDIM_(l), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(uplo));
+
+extern void zunmtr_(SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_Z_ *a, SimTK_FDIM_(lda), const SimTK_Z_ *tau, SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_FDIM_(lwork), SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(uplo), SimTK_FLEN_(trans));
+
+extern void zupgtr_(SimTK_FOPT_(uplo), SimTK_FDIM_(n), const SimTK_Z_ *ap, const SimTK_Z_ *tau, SimTK_Z_ *q, SimTK_FDIM_(ldq), SimTK_Z_ *work, SimTK_INFO_, SimTK_FLEN_(uplo));
+
+extern void zupmtr_(SimTK_FOPT_(side), SimTK_FOPT_(uplo), SimTK_FOPT_(trans), SimTK_FDIM_(m), SimTK_FDIM_(n), const SimTK_Z_ *ap, const SimTK_Z_ *tau, SimTK_Z_ *c__, SimTK_FDIM_(ldc), SimTK_Z_ *work, SimTK_INFO_, SimTK_FLEN_(side), SimTK_FLEN_(uplo), SimTK_FLEN_(trans));
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#undef SimTK_C_
+#undef SimTK_Z_
+#undef SimTK_FDIM_
+#undef SimTK_FOPT_
+#undef SimTK_FLEN_
+#undef SimTK_FINC_
+#undef SimTK_INFO_
+
+#endif /* SimTK_SIMTKLAPACK_H_ */
+
diff --git a/SimTKcommon/sharedTarget/CMakeLists.txt b/SimTKcommon/sharedTarget/CMakeLists.txt
new file mode 100644
index 0000000..2ef3eab
--- /dev/null
+++ b/SimTKcommon/sharedTarget/CMakeLists.txt
@@ -0,0 +1,54 @@
+## This whole directory exists just so I could define this extra
+## preprocessor value.
+
+ADD_DEFINITIONS(-DSimTK_SimTKCOMMON_BUILDING_SHARED_LIBRARY)
+
+IF(BUILD_UNVERSIONED_LIBRARIES)
+ ADD_LIBRARY(${SHARED_TARGET} SHARED
+ ${SOURCE_FILES} ${SOURCE_INCLUDE_FILES}
+ ${API_ABS_INCLUDE_FILES})
+
+ ADD_DEPENDENCIES(${SHARED_TARGET} PlatformFiles)
+
+ TARGET_LINK_LIBRARIES(${SHARED_TARGET} ${MATH_LIBS_TO_USE})
+
+ SET_TARGET_PROPERTIES(${SHARED_TARGET} PROPERTIES
+ PROJECT_LABEL "Code - ${SHARED_TARGET}"
+ SOVERSION ${SIMBODY_SONAME_VERSION})
+
+ # install library; on Windows .dll's go in bin directory
+
+ INSTALL(TARGETS ${SHARED_TARGET}
+ PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE
+ GROUP_READ GROUP_WRITE GROUP_EXECUTE
+ WORLD_READ WORLD_EXECUTE
+ LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+ ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+ RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} )
+ENDIF(BUILD_UNVERSIONED_LIBRARIES)
+
+IF(BUILD_VERSIONED_LIBRARIES)
+ ADD_LIBRARY(${SHARED_TARGET_VN} SHARED
+ ${SOURCE_FILES} ${SOURCE_INCLUDE_FILES}
+ ${API_ABS_INCLUDE_FILES})
+
+ ADD_DEPENDENCIES(${SHARED_TARGET_VN} PlatformFiles)
+
+ TARGET_LINK_LIBRARIES(${SHARED_TARGET_VN} ${MATH_LIBS_TO_USE_VN})
+
+ SET_TARGET_PROPERTIES(${SHARED_TARGET_VN} PROPERTIES
+ PROJECT_LABEL "Code - ${SHARED_TARGET_VN}"
+ SOVERSION ${SIMBODY_SONAME_VERSION})
+
+ # install library; on Windows .dll's go in bin directory
+
+ INSTALL(TARGETS ${SHARED_TARGET_VN}
+ PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE
+ GROUP_READ GROUP_WRITE GROUP_EXECUTE
+ WORLD_READ WORLD_EXECUTE
+ LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+ ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+ RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} )
+ENDIF(BUILD_VERSIONED_LIBRARIES)
+
+
diff --git a/SimTKcommon/src/About.cpp b/SimTKcommon/src/About.cpp
new file mode 100644
index 0000000..b26fd11
--- /dev/null
+++ b/SimTKcommon/src/About.cpp
@@ -0,0 +1,119 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * Defines the standard SimTK core "version" and "about" routines.
+ */
+
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+#include "SimTKcommon/internal/common.h"
+
+#include <string>
+#include <cstring>
+#include <cctype>
+
+#define STR(var) #var
+#define MAKE_VERSION_STRING(maj,min,build) STR(maj.min.build)
+#define MAKE_COPYRIGHT_STRING(y,a) \
+ "Copyright (c) " STR(y) " Stanford University, " STR(a)
+#define MAKE_STRING(a) STR(a)
+
+#define GET_VERSION_STRING \
+ MAKE_VERSION_STRING(SimTK_SimTKCOMMON_MAJOR_VERSION, \
+ SimTK_SimTKCOMMON_MINOR_VERSION, \
+ SimTK_SimTKCOMMON_PATCH_VERSION)
+
+#define GET_COPYRIGHT_STRING \
+ MAKE_COPYRIGHT_STRING(SimTK_SimTKCOMMON_COPYRIGHT_YEARS, \
+ SimTK_SimTKCOMMON_AUTHORS)
+
+#define GET_AUTHORS_STRING \
+ MAKE_STRING(SimTK_SimTKCOMMON_AUTHORS)
+
+#define GET_LIBRARY_STRING \
+ MAKE_STRING(SimTK_SimTKCOMMON_LIBRARY_NAME)
+
+#if defined(SimTK_SimTKCOMMON_BUILDING_SHARED_LIBRARY)
+ #define GET_TYPE_STRING "shared"
+#elif defined(SimTK_SimTKCOMMON_BUILDING_STATIC_LIBRARY)
+ #define GET_TYPE_STRING "static"
+#else
+ #define GET_TYPE_STRING "<unknown library type?!>"
+#endif
+
+#ifndef NDEBUG
+ #define GET_DEBUG_STRING "debug"
+#else
+ #define GET_DEBUG_STRING "release"
+#endif
+
+extern "C" {
+
+void SimTK_version_SimTKcommon(int* major, int* minor, int* patch) {
+ static const char* l = "SimTK library=" GET_LIBRARY_STRING;
+ static const char* t = "SimTK type=" GET_TYPE_STRING;
+ static const char* d = "SimTK debug=" GET_DEBUG_STRING;
+ static const char* v = "SimTK version=" GET_VERSION_STRING;
+ static const char* c = "SimTK copyright=" GET_COPYRIGHT_STRING;
+
+ if (major) *major = SimTK_SimTKCOMMON_MAJOR_VERSION;
+ if (minor) *minor = SimTK_SimTKCOMMON_MINOR_VERSION;
+ if (patch) *patch = SimTK_SimTKCOMMON_PATCH_VERSION;
+
+ // Force statics to be present in the binary (Release mode otherwise
+ // optimizes them away).
+ volatile int i=0;
+ if (i) { // never true, but compiler doesn't know ...
+ *major = *l + *t + *d + *v + *c;
+ }
+}
+
+void SimTK_about_SimTKcommon(const char* key, int maxlen, char* value) {
+ if (maxlen <= 0 || value==0) return;
+ value[0] = '\0'; // in case we don't find a match
+ if (key==0) return;
+
+ // downshift the key
+ std::string skey(key);
+ for (int i=0; i<(int)skey.size(); ++i)
+ skey[i] = std::tolower(skey[i]);
+
+ const char* v = 0;
+ if (skey == "version") v = GET_VERSION_STRING;
+ else if (skey == "library") v = GET_LIBRARY_STRING;
+ else if (skey == "type") v = GET_TYPE_STRING;
+ else if (skey == "copyright") v = GET_COPYRIGHT_STRING;
+ else if (skey == "authors") v = GET_AUTHORS_STRING;
+ else if (skey == "debug") v = GET_DEBUG_STRING;
+
+ if (v) {
+ std::strncpy(value,v,maxlen-1);
+ value[maxlen-1] = '\0'; // in case we ran out of room
+ }
+}
+
+}
diff --git a/SimTKcommon/src/AtomicInteger.cpp b/SimTKcommon/src/AtomicInteger.cpp
new file mode 100644
index 0000000..096d406
--- /dev/null
+++ b/SimTKcommon/src/AtomicInteger.cpp
@@ -0,0 +1,169 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/internal/AtomicInteger.h"
+#include "gmx_atomic.h"
+
+using namespace SimTK;
+
+AtomicInteger::AtomicInteger() {
+ atomic = new gmx_atomic_t();
+ gmx_atomic_set(reinterpret_cast<gmx_atomic_t*>(atomic), 0);
+}
+
+AtomicInteger::AtomicInteger(int value) {
+ atomic = new gmx_atomic_t();
+ gmx_atomic_set(reinterpret_cast<gmx_atomic_t*>(atomic), value);
+}
+
+AtomicInteger::~AtomicInteger() {
+ delete reinterpret_cast<gmx_atomic_t*>(atomic);
+}
+
+AtomicInteger& AtomicInteger::operator=(int value) {
+ gmx_atomic_set(reinterpret_cast<gmx_atomic_t*>(atomic), value);
+ return *this;
+}
+
+AtomicInteger::operator int() const {
+ return gmx_atomic_read(reinterpret_cast<gmx_atomic_t*>(atomic));
+}
+
+int AtomicInteger::operator++() {
+ return gmx_atomic_add_return(reinterpret_cast<gmx_atomic_t*>(atomic), 1);
+}
+
+int AtomicInteger::operator++(int) {
+ return gmx_atomic_fetch_add(reinterpret_cast<gmx_atomic_t*>(atomic), 1);
+}
+
+int AtomicInteger::operator--() {
+ return gmx_atomic_add_return(reinterpret_cast<gmx_atomic_t*>(atomic), -1);
+}
+
+int AtomicInteger::operator--(int) {
+ return gmx_atomic_fetch_add(reinterpret_cast<gmx_atomic_t*>(atomic), -1);
+}
+
+AtomicInteger& AtomicInteger::operator+=(int value) {
+ gmx_atomic_fetch_add(reinterpret_cast<gmx_atomic_t*>(atomic), value);
+ return *this;
+}
+
+AtomicInteger& AtomicInteger::operator-=(int value) {
+ gmx_atomic_fetch_add(reinterpret_cast<gmx_atomic_t*>(atomic), -value);
+ return *this;
+}
+
+AtomicInteger& AtomicInteger::operator*=(int value) {
+ gmx_atomic_t* a = reinterpret_cast<gmx_atomic_t*>(atomic);
+ int oldvalue, newvalue;
+ do {
+ oldvalue = gmx_atomic_read(a);
+ newvalue = oldvalue*value;
+ } while (gmx_atomic_cmpxchg(a, oldvalue, newvalue) != oldvalue);
+ return *this;
+}
+
+AtomicInteger& AtomicInteger::operator/=(int value) {
+ gmx_atomic_t* a = reinterpret_cast<gmx_atomic_t*>(atomic);
+ int oldvalue, newvalue;
+ do {
+ oldvalue = gmx_atomic_read(a);
+ newvalue = oldvalue/value;
+ } while (gmx_atomic_cmpxchg(a, oldvalue, newvalue) != oldvalue);
+ return *this;
+}
+
+AtomicInteger& AtomicInteger::operator%=(int value) {
+ gmx_atomic_t* a = reinterpret_cast<gmx_atomic_t*>(atomic);
+ int oldvalue, newvalue;
+ do {
+ oldvalue = gmx_atomic_read(a);
+ newvalue = oldvalue%value;
+ } while (gmx_atomic_cmpxchg(a, oldvalue, newvalue) != oldvalue);
+ return *this;
+}
+
+AtomicInteger& AtomicInteger::operator&=(int value) {
+ gmx_atomic_t* a = reinterpret_cast<gmx_atomic_t*>(atomic);
+ int oldvalue, newvalue;
+ do {
+ oldvalue = gmx_atomic_read(a);
+ newvalue = oldvalue&value;
+ } while (gmx_atomic_cmpxchg(a, oldvalue, newvalue) != oldvalue);
+ return *this;
+}
+
+AtomicInteger& AtomicInteger::operator|=(int value) {
+ gmx_atomic_t* a = reinterpret_cast<gmx_atomic_t*>(atomic);
+ int oldvalue, newvalue;
+ do {
+ oldvalue = gmx_atomic_read(a);
+ newvalue = oldvalue|value;
+ } while (gmx_atomic_cmpxchg(a, oldvalue, newvalue) != oldvalue);
+ return *this;
+}
+
+AtomicInteger& AtomicInteger::operator^=(int value) {
+ gmx_atomic_t* a = reinterpret_cast<gmx_atomic_t*>(atomic);
+ int oldvalue, newvalue;
+ do {
+ oldvalue = gmx_atomic_read(a);
+ newvalue = oldvalue^value;
+ } while (gmx_atomic_cmpxchg(a, oldvalue, newvalue) != oldvalue);
+ return *this;
+}
+
+AtomicInteger& AtomicInteger::operator<<=(int value) {
+ gmx_atomic_t* a = reinterpret_cast<gmx_atomic_t*>(atomic);
+ int oldvalue, newvalue;
+ do {
+ oldvalue = gmx_atomic_read(a);
+ newvalue = oldvalue<<value;
+ } while (gmx_atomic_cmpxchg(a, oldvalue, newvalue) != oldvalue);
+ return *this;
+}
+
+AtomicInteger& AtomicInteger::operator>>=(int value) {
+ gmx_atomic_t* a = reinterpret_cast<gmx_atomic_t*>(atomic);
+ int oldvalue, newvalue;
+ do {
+ oldvalue = gmx_atomic_read(a);
+ newvalue = oldvalue>>value;
+ } while (gmx_atomic_cmpxchg(a, oldvalue, newvalue) != oldvalue);
+ return *this;
+}
+
+bool AtomicInteger::operator==(int value) const {
+ return (gmx_atomic_read(reinterpret_cast<gmx_atomic_t*>(atomic)) == value);
+}
+
+bool AtomicInteger::operator!=(int value) const {
+ return (gmx_atomic_read(reinterpret_cast<gmx_atomic_t*>(atomic)) != value);
+}
+
+std::ostream& SimTK::operator<<(std::ostream& stream, const AtomicInteger& value) {
+ int i = value;
+ return stream << i;
+}
diff --git a/SimTKcommon/src/Parallel2DExecutor.cpp b/SimTKcommon/src/Parallel2DExecutor.cpp
new file mode 100644
index 0000000..74d6d29
--- /dev/null
+++ b/SimTKcommon/src/Parallel2DExecutor.cpp
@@ -0,0 +1,223 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "Parallel2DExecutorImpl.h"
+#include "SimTKcommon/internal/ParallelExecutor.h"
+#include <pthread.h>
+#include <utility>
+
+using std::pair;
+
+namespace SimTK {
+
+Parallel2DExecutorImpl::Parallel2DExecutorImpl(int gridSize, int numProcessors) : gridSize(gridSize), ownExecutor(true) {
+ numProcessors = std::min(numProcessors, gridSize/2);
+ if (numProcessors < 2)
+ executor = 0;
+ else
+ executor = new ParallelExecutor(numProcessors);
+ init(numProcessors);
+}
+Parallel2DExecutorImpl::Parallel2DExecutorImpl(int gridSize, ParallelExecutor& executor) : gridSize(gridSize), ownExecutor(false), executor(&executor) {
+ init(executor.getNumProcessors());
+}
+void Parallel2DExecutorImpl::init(int numProcessors) {
+ int bins;
+ if (numProcessors < 2) {
+ bins = 1;
+ }
+ else {
+
+ // Determine how many levels of subdivision to use.
+
+ int levels = 1;
+ while (1<<levels < numProcessors)
+ levels++;
+ levels++;
+ bins = 1<<levels;
+
+ // Build the set of squares.
+
+ squares.resize(bins-1);
+ addTriangle(0, 0, 0, levels);
+ }
+
+ // Find the range of indices in each bin.
+
+ binStart.resize(bins+1);
+ for (int i = 0; i < bins; ++i)
+ binStart[i] = (int) std::floor(0.5+i*gridSize/(double) bins);
+ binStart[bins] = gridSize;
+}
+Parallel2DExecutorImpl::~Parallel2DExecutorImpl() {
+ if (executor != 0 && ownExecutor)
+ delete executor;
+}
+void Parallel2DExecutorImpl::addSquare(int x, int y, int pass, int level) {
+ if (level == 0)
+ squares[pass-1].push_back(pair<int, int>(x, y));
+ else {
+ addSquare(2*x+0, 2*y+1, 2*pass+1, level-1);
+ addSquare(2*x+1, 2*y+2, 2*pass+1, level-1);
+ addSquare(2*x+0, 2*y+2, 2*pass+2, level-1);
+ addSquare(2*x+1, 2*y+1, 2*pass+2, level-1);
+ }
+}
+void Parallel2DExecutorImpl::addTriangle(int x, int y, int pass, int level) {
+ if (level > 1) {
+ addSquare(2*x, 2*y, 2*pass, level-1);
+ addTriangle(2*x, 2*y, 2*pass, level-1);
+ addTriangle(2*x+1, 2*y+1, 2*pass, level-1);
+ }
+}
+Parallel2DExecutorImpl* Parallel2DExecutorImpl::clone() const {
+ if (ownExecutor)
+ return new Parallel2DExecutorImpl(gridSize, executor->getNumProcessors());
+ return new Parallel2DExecutorImpl(gridSize, *executor);
+}
+ParallelExecutor& Parallel2DExecutorImpl::getExecutor() {
+ return *executor;
+}
+
+class Parallel2DExecutorImpl::TriangleTask : public ParallelExecutor::Task {
+public:
+ TriangleTask(const Parallel2DExecutorImpl& executor, Parallel2DExecutor::Task& task, Parallel2DExecutor::RangeType rangeType, int width, bool shouldInitialize, bool shouldFinish) :
+ executor(executor), task(task), rangeType(rangeType), width(width), shouldInitialize(shouldInitialize), shouldFinish(shouldFinish) {
+ }
+ void execute(int index) {
+ int start = executor.getBinStart(width*index);
+ int end = executor.getBinStart(width*(index+1));
+ switch (rangeType) {
+ case Parallel2DExecutor::FullMatrix:
+ for (int i = start; i < end; ++i)
+ for (int j = start; j < end; ++j)
+ task.execute(i, j);
+ return;
+ case Parallel2DExecutor::HalfMatrix:
+ for (int i = start; i < end; ++i)
+ for (int j = start; j < i; ++j)
+ task.execute(i, j);
+ return;
+ case Parallel2DExecutor::HalfPlusDiagonal:
+ for (int i = start; i < end; ++i)
+ for (int j = start; j <= i; ++j)
+ task.execute(i, j);
+ return;
+ }
+ }
+ void initialize() {
+ if (shouldInitialize)
+ task.initialize();
+ }
+ void finish() {
+ if (shouldFinish)
+ task.finish();
+ }
+private:
+ const Parallel2DExecutorImpl& executor;
+ Parallel2DExecutor::Task& task;
+ const Parallel2DExecutor::RangeType rangeType;
+ const int width;
+ const bool shouldInitialize, shouldFinish;
+};
+
+class Parallel2DExecutorImpl::SquareTask : public ParallelExecutor::Task {
+public:
+ SquareTask(const Parallel2DExecutorImpl& executor, Parallel2DExecutor::Task& task, const Array_<pair<int,int> >& squares, Parallel2DExecutor::RangeType rangeType, bool shouldInitialize, bool shouldFinish) :
+ executor(executor), task(task), squares(squares), rangeType(rangeType), shouldInitialize(shouldInitialize), shouldFinish(shouldFinish) {
+ }
+ void execute(int index) {
+ const pair<int,int>& square = squares[index];
+ int istart = executor.getBinStart(square.second+1);
+ int iend = executor.getBinStart(square.second+2);
+ int jstart = executor.getBinStart(square.first);
+ int jend = executor.getBinStart(square.first+1);
+ switch (rangeType) {
+ case Parallel2DExecutor::FullMatrix:
+ for (int i = istart; i < iend; ++i)
+ for (int j = jstart; j < jend; ++j) {
+ task.execute(i, j);
+ task.execute(j, i);
+ }
+ return;
+ case Parallel2DExecutor::HalfMatrix:
+ case Parallel2DExecutor::HalfPlusDiagonal:
+ for (int i = istart; i < iend; ++i)
+ for (int j = jstart; j < jend; ++j)
+ task.execute(i, j);
+ return;
+ }
+ }
+ void initialize() {
+ if (shouldInitialize)
+ task.initialize();
+ }
+ void finish() {
+ if (shouldFinish)
+ task.finish();
+ }
+private:
+ const Parallel2DExecutorImpl& executor;
+ Parallel2DExecutor::Task& task;
+ const Array_<pair<int,int> >& squares;
+ const Parallel2DExecutor::RangeType rangeType;
+ bool shouldInitialize, shouldFinish;
+};
+
+void Parallel2DExecutorImpl::execute(Parallel2DExecutor::Task& task, Parallel2DExecutor::RangeType rangeType) {
+ if (executor == 0) {
+ task.initialize();
+ TriangleTask(*this, task, rangeType, 1, false, false).execute(0);
+ task.finish();
+ return;
+ }
+ int bins = binStart.size()-1;
+
+ // Execute the blocks along the diagonal.
+
+ TriangleTask triangle(*this, task, rangeType, 2, true, false);
+ executor->execute(triangle, bins/2);
+
+ // Execute the square blocks in a series of passes.
+
+ for (int i = 0; i < (int)squares.size(); ++i) {
+ SquareTask square(*this, task, squares[i], rangeType, false, i == squares.size()-1);
+ executor->execute(square, squares[i].size());
+ }
+}
+
+Parallel2DExecutor::Parallel2DExecutor(int gridSize, int numProcessors) : HandleBase(new Parallel2DExecutorImpl(gridSize, numProcessors)) {
+}
+
+Parallel2DExecutor::Parallel2DExecutor(int gridSize, ParallelExecutor& executor) : HandleBase(new Parallel2DExecutorImpl(gridSize, executor)) {
+}
+
+void Parallel2DExecutor::execute(Task& task, RangeType rangeType) {
+ updImpl().execute(task, rangeType);
+}
+
+ParallelExecutor& Parallel2DExecutor::getExecutor() {
+ return updImpl().getExecutor();
+}
+
+} // namespace SimTK
diff --git a/SimTKcommon/src/Parallel2DExecutorImpl.h b/SimTKcommon/src/Parallel2DExecutorImpl.h
new file mode 100644
index 0000000..ece92a4
--- /dev/null
+++ b/SimTKcommon/src/Parallel2DExecutorImpl.h
@@ -0,0 +1,65 @@
+#ifndef SimTK_SimTKCOMMON_PARALLEL_2D_EXECUTOR_IMPL_H_
+#define SimTK_SimTKCOMMON_PARALLEL_2D_EXECUTOR_IMPL_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/internal/Parallel2DExecutor.h"
+#include "SimTKcommon/internal/Array.h"
+#include <utility>
+
+namespace SimTK {
+
+class ParallelExecutor;
+
+/**
+ * This is the internal implementation class for Parallel2DExecutor.
+ */
+
+class Parallel2DExecutorImpl : public PIMPLImplementation<Parallel2DExecutor, Parallel2DExecutorImpl> {
+public:
+ class TriangleTask;
+ class SquareTask;
+ Parallel2DExecutorImpl(int gridSize, int numProcessors);
+ Parallel2DExecutorImpl(int gridSize, ParallelExecutor& executor);
+ ~Parallel2DExecutorImpl();
+ void init(int numProcessors);
+ void addSquare(int x, int y, int pass, int level);
+ void addTriangle(int x, int y, int pass, int level);
+ Parallel2DExecutorImpl* clone() const;
+ void execute(Parallel2DExecutor::Task& task, Parallel2DExecutor::RangeType rangeType);
+ ParallelExecutor& getExecutor();
+ int getBinStart(int bin) const {
+ return binStart[bin];
+ }
+private:
+ const int gridSize;
+ Array_<Array_<std::pair<int,int> > > squares;
+ Array_<int> binStart;
+ ParallelExecutor* executor;
+ bool ownExecutor;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_PARALLEL_2D_EXECUTOR_IMPL_H_
diff --git a/SimTKcommon/src/ParallelExecutor.cpp b/SimTKcommon/src/ParallelExecutor.cpp
new file mode 100644
index 0000000..30a5d46
--- /dev/null
+++ b/SimTKcommon/src/ParallelExecutor.cpp
@@ -0,0 +1,220 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "ParallelExecutorImpl.h"
+#include "SimTKcommon/internal/ParallelExecutor.h"
+#include <pthread.h>
+
+namespace SimTK {
+
+static void* threadBody(void* args);
+
+ParallelExecutorImpl::ParallelExecutorImpl(int numThreads) : finished(false) {
+
+ // Construct all the threading related objects we will need.
+
+ SimTK_APIARGCHECK_ALWAYS(numThreads > 0, "ParallelExecutorImpl", "ParallelExecutorImpl", "Number of threads must be positive.");
+ threads.resize(numThreads);
+ pthread_mutex_init(&runLock, NULL);
+ pthread_cond_init(&runCondition, NULL);
+ pthread_cond_init(&waitCondition, NULL);
+ for (int i = 0; i < numThreads; ++i) {
+ threadInfo.push_back(new ThreadInfo(i, this));
+ pthread_create(&threads[i], NULL, threadBody, threadInfo[i]);
+ }
+}
+ParallelExecutorImpl::~ParallelExecutorImpl() {
+
+ // Notify the threads that they should exit.
+
+ pthread_mutex_lock(&runLock);
+ finished = true;
+ for (int i = 0; i < (int) threads.size(); ++i)
+ threadInfo[i]->running = true;
+ pthread_cond_broadcast(&runCondition);
+ pthread_mutex_unlock(&runLock);
+
+ // Wait until all the threads have finished.
+
+ for (int i = 0; i < (int) threads.size(); ++i)
+ pthread_join(threads[i], NULL);
+
+ // Clean up threading related objects.
+
+ pthread_mutex_destroy(&runLock);
+ pthread_cond_destroy(&runCondition);
+ pthread_cond_destroy(&waitCondition);
+}
+ParallelExecutorImpl* ParallelExecutorImpl::clone() const {
+ return new ParallelExecutorImpl(threads.size());
+}
+void ParallelExecutorImpl::execute(ParallelExecutor::Task& task, int times) {
+ if (times == 1 || threads.size() == 1) {
+ // Nothing is actually going to get done in parallel, so we might as well
+ // just execute the task directly and save the threading overhead.
+
+ task.initialize();
+ for (int i = 0; i < times; ++i)
+ task.execute(i);
+ task.finish();
+ return;
+ }
+
+ // Initialize fields to execute the new task.
+
+ pthread_mutex_lock(&runLock);
+ currentTask = &task;
+ currentTaskCount = times;
+ waitingThreadCount = 0;
+ for (int i = 0; i < (int) threadInfo.size(); ++i)
+ threadInfo[i]->running = true;
+
+ // Wake up the worker threads and wait until they finish.
+
+ pthread_cond_broadcast(&runCondition);
+ do {
+ pthread_cond_wait(&waitCondition, &runLock);
+ } while (waitingThreadCount < (int) threads.size());
+ pthread_mutex_unlock(&runLock);
+}
+void ParallelExecutorImpl::incrementWaitingThreads() {
+ pthread_mutex_lock(&runLock);
+ getCurrentTask().finish();
+ waitingThreadCount++;
+ if (waitingThreadCount == threads.size()) {
+ pthread_cond_signal(&waitCondition);
+ }
+ pthread_mutex_unlock(&runLock);
+}
+
+ThreadLocal<bool> ParallelExecutorImpl::isWorker(false);
+
+/**
+ * This function contains the code executed by the worker threads.
+ */
+
+void* threadBody(void* args) {
+ ParallelExecutorImpl::isWorker.upd() = true;
+ ThreadInfo& info = *reinterpret_cast<ThreadInfo*>(args);
+ ParallelExecutorImpl& executor = *info.executor;
+ int threadCount = executor.getThreadCount();
+ while (!executor.isFinished()) {
+
+ // Wait for a Task to come in.
+
+ pthread_mutex_lock(executor.getLock());
+ while (!info.running) {
+ pthread_cond_wait(executor.getCondition(), executor.getLock());
+ }
+ pthread_mutex_unlock(executor.getLock());
+ if (!executor.isFinished()) {
+
+ // Execute the task for all the indices belonging to this thread.
+
+ int count = executor.getCurrentTaskCount();
+ ParallelExecutor::Task& task = executor.getCurrentTask();
+ task.initialize();
+ int index = info.index;
+ try {
+ while (index < count) {
+ task.execute(index);
+ index += threadCount;
+ }
+ }
+ catch (std::exception ex) {
+ std::cerr <<"The parallel task threw an unhandled exception:"<< std::endl;
+ std::cerr <<ex.what()<< std::endl;
+ }
+ catch (...) {
+ std::cerr <<"The parallel task threw an error."<< std::endl;
+ }
+ info.running = false;
+ executor.incrementWaitingThreads();
+ }
+ }
+ delete &info;
+ return 0;
+}
+
+ParallelExecutor::ParallelExecutor(int numThreads) : HandleBase(new ParallelExecutorImpl(numThreads)) {
+}
+
+void ParallelExecutor::execute(Task& task, int times) {
+ updImpl().execute(task, times);
+}
+
+#ifdef __APPLE__
+ #include <sys/sysctl.h>
+ #include <dlfcn.h>
+#else
+ #ifdef __linux
+ #include <dlfcn.h>
+ #include <unistd.h>
+ #else
+ #include <windows.h>
+ #endif
+#endif
+
+int ParallelExecutor::getNumProcessors() {
+#ifdef __APPLE__
+ int ncpu,retval;
+ size_t len = 4;
+
+ retval = sysctlbyname( "hw.physicalcpu", &ncpu, &len, NULL, 0 );
+ if( retval == 0 ) {
+ return (ncpu );
+ } else {
+ return(1);
+ }
+#else
+#ifdef __linux
+ long nProcessorsOnline = sysconf(_SC_NPROCESSORS_ONLN);
+ if( nProcessorsOnline == -1 ) {
+ return(1);
+ } else {
+ return( (int)nProcessorsOnline );
+ }
+#else
+ // Windows
+
+ SYSTEM_INFO siSysInfo;
+ int ncpu;
+
+ // Copy the hardware information to the SYSTEM_INFO structure.
+
+ GetSystemInfo(&siSysInfo);
+
+ // Display the contents of the SYSTEM_INFO structure.
+
+ ncpu = siSysInfo.dwNumberOfProcessors;
+ if( ncpu < 1 ) ncpu = 1;
+ return(ncpu);
+#endif
+#endif
+}
+
+bool ParallelExecutor::isWorkerThread() {
+ return ParallelExecutorImpl::isWorker.get();
+}
+
+} // namespace SimTK
diff --git a/SimTKcommon/src/ParallelExecutorImpl.h b/SimTKcommon/src/ParallelExecutorImpl.h
new file mode 100644
index 0000000..18505e8
--- /dev/null
+++ b/SimTKcommon/src/ParallelExecutorImpl.h
@@ -0,0 +1,92 @@
+#ifndef SimTK_SimTKCOMMON_PARALLEL_EXECUTOR_IMPL_H_
+#define SimTK_SimTKCOMMON_PARALLEL_EXECUTOR_IMPL_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/internal/ParallelExecutor.h"
+#include "SimTKcommon/internal/ThreadLocal.h"
+#include "SimTKcommon/internal/Array.h"
+#include <pthread.h>
+
+namespace SimTK {
+
+class ParallelExecutorImpl;
+
+/**
+ * This class stores per-thread information used while executing a task.
+ */
+
+class ThreadInfo {
+public:
+ ThreadInfo(int index, ParallelExecutorImpl* executor) : index(index), executor(executor), running(false) {
+ }
+ const int index;
+ ParallelExecutorImpl* const executor;
+ bool running;
+};
+
+/**
+ * This is the internal implementation class for ParallelExecutor.
+ */
+
+class ParallelExecutorImpl : public PIMPLImplementation<ParallelExecutor, ParallelExecutorImpl> {
+public:
+ ParallelExecutorImpl(int numThreads);
+ ~ParallelExecutorImpl();
+ ParallelExecutorImpl* clone() const;
+ void execute(ParallelExecutor::Task& task, int times);
+ int getThreadCount() {
+ return threads.size();
+ }
+ ParallelExecutor::Task& getCurrentTask() {
+ return *currentTask;
+ }
+ int getCurrentTaskCount() {
+ return currentTaskCount;
+ }
+ bool isFinished() {
+ return finished;
+ }
+ pthread_mutex_t* getLock() {
+ return &runLock;
+ }
+ pthread_cond_t* getCondition() {
+ return &runCondition;
+ }
+ void incrementWaitingThreads();
+ static ThreadLocal<bool> isWorker;
+private:
+ bool finished;
+ pthread_mutex_t runLock;
+ pthread_cond_t runCondition, waitCondition;
+ Array_<pthread_t> threads;
+ Array_<ThreadInfo*> threadInfo;
+ ParallelExecutor::Task* currentTask;
+ int currentTaskCount;
+ int waitingThreadCount;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_PARALLEL_EXECUTOR_IMPL_H_
diff --git a/SimTKcommon/src/ParallelWorkQueue.cpp b/SimTKcommon/src/ParallelWorkQueue.cpp
new file mode 100644
index 0000000..bf70fda
--- /dev/null
+++ b/SimTKcommon/src/ParallelWorkQueue.cpp
@@ -0,0 +1,152 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "ParallelWorkQueueImpl.h"
+#include "SimTKcommon/internal/ParallelExecutor.h"
+#include <pthread.h>
+#include <utility>
+
+using std::queue;
+
+namespace SimTK {
+
+static void* threadBody(void* args) {
+ ParallelWorkQueueImpl& owner = *reinterpret_cast<ParallelWorkQueueImpl*>(args);
+ queue<ParallelWorkQueue::Task*>& taskQueue = owner.updTaskQueue();
+ pthread_mutex_t& queueLock = owner.getQueueLock();
+ pthread_cond_t& waitForTaskCondition = owner.getWaitCondition();
+ pthread_cond_t& queueFullCondition = owner.getQueueFullCondition();
+ bool decrementTaskCount = false;
+ while (!owner.isFinished() || !taskQueue.empty()) {
+ pthread_mutex_lock(&queueLock);
+ if (decrementTaskCount) {
+ owner.markTaskCompleted();
+ decrementTaskCount = false;
+ }
+ while (taskQueue.empty() && !owner.isFinished())
+ pthread_cond_wait(&waitForTaskCondition, &queueLock);
+ ParallelWorkQueue::Task* task = NULL;
+ if (!taskQueue.empty()) {
+ task = taskQueue.front();
+ taskQueue.pop();
+ }
+ pthread_cond_signal(&queueFullCondition);
+ pthread_mutex_unlock(&queueLock);
+ if (task != NULL) {
+ task->execute();
+ delete task;
+ decrementTaskCount = true;
+ }
+ }
+ if (decrementTaskCount) {
+ pthread_mutex_lock(&queueLock);
+ owner.markTaskCompleted();
+ pthread_mutex_unlock(&queueLock);
+ }
+ return 0;
+}
+
+ParallelWorkQueueImpl::ParallelWorkQueueImpl(int queueSize, int numThreads) : queueSize(queueSize), pendingTasks(0), finished(false) {
+ pthread_mutex_init(&queueLock, NULL);
+ pthread_cond_init(&waitForTaskCondition, NULL);
+ pthread_cond_init(&queueFullCondition, NULL);
+ threads.resize(numThreads);
+ for (int i = 0; i < numThreads; ++i)
+ pthread_create(&threads[i], NULL, threadBody, this);
+}
+
+ParallelWorkQueueImpl::~ParallelWorkQueueImpl() {
+ // Wait for the theads to finish.
+
+ pthread_mutex_lock(&queueLock);
+ finished = true;
+ pthread_cond_broadcast(&waitForTaskCondition);
+ pthread_mutex_unlock(&queueLock);
+ for (int i = 0; i < (int) threads.size(); ++i)
+ pthread_join(threads[i], NULL);
+
+ // Clean up memory.
+
+ pthread_mutex_destroy(&queueLock);
+ pthread_cond_destroy(&waitForTaskCondition);
+ pthread_cond_destroy(&queueFullCondition);
+}
+
+ParallelWorkQueueImpl* ParallelWorkQueueImpl::clone() const {
+ return new ParallelWorkQueueImpl(queueSize, threads.size());
+}
+
+void ParallelWorkQueueImpl::addTask(ParallelWorkQueue::Task* task) {
+ pthread_mutex_lock(&queueLock);
+ while ((int)taskQueue.size() >= queueSize)
+ pthread_cond_wait(&queueFullCondition, &queueLock);
+ taskQueue.push(task);
+ ++pendingTasks;
+ pthread_cond_signal(&waitForTaskCondition);
+ pthread_mutex_unlock(&queueLock);
+}
+
+void ParallelWorkQueueImpl::flush() {
+ pthread_mutex_lock(&queueLock);
+ while (pendingTasks > 0)
+ pthread_cond_wait(&queueFullCondition, &queueLock);
+ pthread_mutex_unlock(&queueLock);
+}
+
+queue<ParallelWorkQueue::Task*>& ParallelWorkQueueImpl::updTaskQueue() {
+ return taskQueue;
+}
+
+bool ParallelWorkQueueImpl::isFinished() const {
+ return finished;
+}
+
+pthread_mutex_t& ParallelWorkQueueImpl::getQueueLock() {
+ return queueLock;
+}
+
+pthread_cond_t& ParallelWorkQueueImpl::getWaitCondition() {
+ return waitForTaskCondition;
+}
+
+pthread_cond_t& ParallelWorkQueueImpl::getQueueFullCondition() {
+ return queueFullCondition;
+}
+
+void ParallelWorkQueueImpl::markTaskCompleted() {
+ --pendingTasks;
+ pthread_cond_signal(&queueFullCondition);
+}
+
+ParallelWorkQueue::ParallelWorkQueue(int queueSize, int numThreads) : HandleBase(new ParallelWorkQueueImpl(queueSize, numThreads)) {
+}
+
+void ParallelWorkQueue::addTask(Task* task) {
+ updImpl().addTask(task);
+}
+
+void ParallelWorkQueue::flush() {
+ updImpl().flush();
+}
+
+} // namespace SimTK
diff --git a/SimTKcommon/src/ParallelWorkQueueImpl.h b/SimTKcommon/src/ParallelWorkQueueImpl.h
new file mode 100644
index 0000000..40d3a05
--- /dev/null
+++ b/SimTKcommon/src/ParallelWorkQueueImpl.h
@@ -0,0 +1,66 @@
+#ifndef SimTK_SimTKCOMMON_PARALLEL_WORK_QUEUE_IMPL_H_
+#define SimTK_SimTKCOMMON_PARALLEL_WORK_QUEUE_IMPL_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/internal/ParallelWorkQueue.h"
+#include "SimTKcommon/internal/Array.h"
+#include <pthread.h>
+#include <queue>
+
+namespace SimTK {
+
+class ParallelExecutor;
+
+/**
+ * This is the internal implementation class for ParallelWorkQueue.
+ */
+
+class ParallelWorkQueueImpl : public PIMPLImplementation<ParallelWorkQueue, ParallelWorkQueueImpl> {
+public:
+ class ExecutorTask;
+ ParallelWorkQueueImpl(int queueSize, int numThreads);
+ ~ParallelWorkQueueImpl();
+ ParallelWorkQueueImpl* clone() const;
+ void addTask(ParallelWorkQueue::Task* task);
+ void flush();
+ std::queue<ParallelWorkQueue::Task*>& updTaskQueue();
+ bool isFinished() const;
+ pthread_mutex_t& getQueueLock();
+ pthread_cond_t& getWaitCondition();
+ pthread_cond_t& getQueueFullCondition();
+ void markTaskCompleted();
+private:
+ const int queueSize;
+ int pendingTasks;
+ bool finished;
+ std::queue<ParallelWorkQueue::Task*> taskQueue;
+ pthread_mutex_t queueLock;
+ pthread_cond_t waitForTaskCondition, queueFullCondition;
+ Array_<pthread_t> threads;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SimTKCOMMON_PARALLEL_WORK_QUEUE_IMPL_H_
diff --git a/SimTKcommon/src/Pathname.cpp b/SimTKcommon/src/Pathname.cpp
new file mode 100644
index 0000000..11d6975
--- /dev/null
+++ b/SimTKcommon/src/Pathname.cpp
@@ -0,0 +1,371 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/internal/common.h"
+#include "SimTKcommon/internal/String.h"
+#include "SimTKcommon/internal/Pathname.h"
+
+#include <string>
+using std::string;
+
+#include <cctype>
+using std::tolower;
+
+#include <iostream>
+#include <fstream>
+
+#ifdef _WIN32
+ #define WIN32_LEAN_AND_MEAN
+ #define NOMINMAX
+ #include <windows.h>
+ #include <direct.h>
+ #pragma warning(disable:4996) // getenv() is apparently unsafe
+#else
+ #ifdef __APPLE__
+ #include <mach-o/dyld.h>
+ #endif
+ #include <dlfcn.h>
+ #include <dirent.h>
+ #include <unistd.h>
+#endif
+
+
+using namespace SimTK;
+
+
+#ifdef _WIN32
+ static const bool IsWindows = true;
+ static const char MyPathSeparator = '\\';
+ static const char OtherPathSeparator = '/';
+#else
+ static const bool IsWindows = false;
+ static const char MyPathSeparator = '/';
+ static const char OtherPathSeparator = '\\';
+#endif
+
+char Pathname::getPathSeparatorChar() {
+ return MyPathSeparator;
+}
+
+string Pathname::getPathSeparator() {
+ return String(getPathSeparatorChar());
+}
+
+static void makeNativeSlashesInPlace(string& inout) {
+ for (unsigned i=0; i < inout.size(); ++i)
+ if (inout[i] == OtherPathSeparator)
+ inout[i] = MyPathSeparator;
+}
+
+static void addFinalSeparatorInPlace(string& inout) {
+ if (!inout.empty() && !Pathname::isPathSeparator(inout[inout.size()-1]))
+ inout += MyPathSeparator;
+}
+
+static bool beginsWithPathSeparator(const string& in) {
+ return !in.empty() && Pathname::isPathSeparator(in[0]);
+}
+
+// Remove the last segment of a path name and the separator. The
+// returned component does not include a separator.
+static void removeLastPathComponentInPlace(string& inout, string& component) {
+ component.clear();
+ if (inout.empty()) return;
+
+ const string::size_type lastSeparator = inout.find_last_of("/\\");
+ if (lastSeparator == string::npos) {
+ component = inout;
+ inout.clear();
+ } else {
+ component = inout.substr(lastSeparator+1);
+ inout.erase(lastSeparator);
+ }
+}
+
+static void removeDriveInPlace(string& inout, string& drive) {
+ drive.clear();
+ if (IsWindows && inout.size() >= 2 && inout[1]==':') {
+ drive = (char)tolower(inout[0]);
+ inout.erase(0,2);
+ }
+}
+
+
+// We assume a path name structure like this:
+// (1) Everything up to and including the final directory separator
+// character is the directory; the rest is the file name. On return
+// we fix the slashes in the directory name to suit the current
+// system ('\' for Windows, '/' otherwise).
+// (2) If the file name contains a ".", characters after the last
+// "." are the extension and the last "." is removed.
+// (5) What's left is the fileName.
+// We accept both "/" and "\" as separator characters. We leave the
+// case as it was supplied.
+// Leading and trailing white space is removed; embedded white space
+// remains.
+// Leading "X:" for some drive letter X is recognized on Windows as
+// a drive specification, otherwise the drive is the current drive.
+// That is then removed for further processing.
+// Absolute paths are designated like this:
+// Leading "/" means root relative (on the drive).
+// Leading "./" means current working directory relative (on the drive).
+// Leading "../" is interpreted as "./..".
+// Leading "@/" means relative to executable location.
+// Above leading characters are removed and replaced with the
+// full path name they represent, then the entire path is divided
+// into components. If a component is "." or "" (empty) it is
+// removed. If a component is ".." it and the previous component
+// if any are removed. (".." as a first component will report as
+// ill formed.)
+// If there is something ill-formed about the file name we'll return
+// false.
+void Pathname::deconstructPathname( const string& name,
+ bool& isAbsolutePath,
+ string& directory,
+ string& fileName,
+ string& extension)
+{
+ isAbsolutePath = false;
+ directory.erase(); fileName.erase(); extension.erase();
+
+ // Remove all the white space and make all the slashes be forward ones.
+ // (For Windows they'll be changed to backslashes later.)
+ String processed = String::trimWhiteSpace(name).replaceAllChar('\\', '/');
+ if (processed.empty())
+ return; // name consisted only of white space
+
+ string drive;
+ removeDriveInPlace(processed, drive);
+
+ // Now the drive if any has been removed and we're looking at
+ // the beginning of the path name.
+
+ // If the pathname in its entirety is just one of these, append
+ // a slash to avoid special cases below.
+ if (processed=="." || processed==".." || processed=="@")
+ processed += "/";
+
+ // If the path begins with "../" we'll make it ./../ to simplify handling.
+ if (processed.substr(0,3) == "../")
+ processed.insert(0, "./");
+
+ if (processed.substr(0,1) == "/") {
+ isAbsolutePath = true;
+ processed.erase(0,1);
+ if (drive.empty()) drive = getCurrentDriveLetter();
+ } else if (processed.substr(0,2) == "./") {
+ isAbsolutePath = true;
+ processed.replace(0,2,getCurrentWorkingDirectory(drive));
+ removeDriveInPlace(processed, drive);
+ } else if (processed.substr(0,2) == "@/") {
+ isAbsolutePath = true;
+ processed.replace(0,2,getThisExecutableDirectory());
+ removeDriveInPlace(processed, drive);
+ } else if (!drive.empty()) {
+ // Looks like a relative path name. But if it had an initial
+ // drive specification, e.g. X:something.txt, that is supposed
+ // to be interpreted relative to the current working directory
+ // on drive X, just as though it were X:./something.txt.
+ isAbsolutePath = true;
+ processed.insert(0, getCurrentWorkingDirectory(drive));
+ removeDriveInPlace(processed, drive);
+ }
+
+ // We may have picked up a new batch of backslashes above.
+ processed.replaceAllChar('\\', '/');
+
+ // Now we have the full path name if this is absolute, otherwise
+ // we're looking at a relative path name. In any case the last
+ // component is the file name if it isn't empty, ".", or "..".
+
+ // Process the ".." segments and eliminate meaningless ones
+ // as we go through.
+ Array_<string> segmentsInReverse;
+ bool isFinalSegment = true; // first time around might be the fileName
+ int numDotDotsSeen = 0;
+ while (!processed.empty()) {
+ string component;
+ removeLastPathComponentInPlace(processed, component);
+ if (component == "..")
+ ++numDotDotsSeen;
+ else if (!component.empty() && component != ".") {
+ if (numDotDotsSeen)
+ --numDotDotsSeen; // skip component
+ else if (isFinalSegment) fileName = component;
+ else segmentsInReverse.push_back(component);
+ }
+ isFinalSegment = false;
+ }
+
+ // Now we can put together the canonicalized directory.
+ if (isAbsolutePath) {
+ if (!drive.empty())
+ directory = drive + ":";
+ directory += "/";
+ }
+
+ for (int i = (int)segmentsInReverse.size()-1; i >= 0; --i)
+ directory += segmentsInReverse[i] + "/";
+
+ // Fix the slashes.
+ makeNativeSlashesInPlace(directory);
+
+ // If there is a .extension, strip it off.
+ string::size_type lastDot = fileName.rfind('.');
+ if (lastDot != string::npos) {
+ extension = fileName.substr(lastDot);
+ fileName.erase(lastDot);
+ }
+}
+
+bool Pathname::fileExists(const std::string& fileName) {
+ std::ifstream f(fileName.c_str());
+ return f.good();
+ // File is closed by destruction of f.
+}
+
+string Pathname::getDefaultInstallDir() {
+ string installDir;
+ #ifdef _WIN32
+ installDir = getEnvironmentVariable("ProgramFiles");
+ if (!installDir.empty()) {
+ makeNativeSlashesInPlace(installDir);
+ addFinalSeparatorInPlace(installDir);
+ } else
+ installDir = "c:\\Program Files\\";
+ #else
+ installDir = "/usr/local/";
+ #endif
+ return installDir;
+}
+
+string Pathname::addDirectoryOffset(const string& base, const string& offset) {
+ string cleanOffset = beginsWithPathSeparator(offset)
+ ? offset.substr(1) : offset; // remove leading path separator
+ addFinalSeparatorInPlace(cleanOffset); // add trailing / if non-empty
+ string result = base;
+ addFinalSeparatorInPlace(result);
+ result += cleanOffset;
+ return result;
+}
+
+string Pathname::getInstallDir(const std::string& envInstallDir,
+ const std::string& offsetFromDefaultInstallDir)
+{ std::string installDir = getEnvironmentVariable(envInstallDir);
+ if (!installDir.empty()) {
+ makeNativeSlashesInPlace(installDir);
+ addFinalSeparatorInPlace(installDir);
+ } else
+ installDir = addDirectoryOffset(getDefaultInstallDir(), offsetFromDefaultInstallDir);
+ return installDir;
+}
+
+string Pathname::getThisExecutablePath() {
+ char buf[2048];
+ #if defined(_WIN32)
+ const DWORD nBytes = GetModuleFileName((HMODULE)0, (LPTSTR)buf, sizeof(buf));
+ buf[0] = (char)tolower(buf[0]); // drive name
+ #elif defined(__APPLE__)
+ uint32_t sz = (uint32_t)sizeof(buf);
+ int status = _NSGetExecutablePath(buf, &sz);
+ assert(status==0); // non-zero means path longer than buf, sz says what's needed
+ #else // Linux
+ // This isn't automatically null terminated.
+ const size_t nBytes = readlink("/proc/self/exe", buf, sizeof(buf));
+ buf[nBytes] = '\0';
+ #endif
+ return string(buf);
+}
+
+string Pathname::getThisExecutableDirectory() {
+ string path = getThisExecutablePath();
+ string component;
+ removeLastPathComponentInPlace(path, component);
+ path += MyPathSeparator;
+ return path;
+}
+
+string Pathname::getCurrentDriveLetter() {
+ #ifdef _WIN32
+ const int which = _getdrive();
+ return string() + (char)('a' + which-1);
+ #else
+ return string();
+ #endif
+}
+
+string Pathname::getCurrentDrive() {
+ #ifdef _WIN32
+ return getCurrentDriveLetter() + ":";
+ #else
+ return string();
+ #endif
+}
+
+
+string Pathname::getCurrentWorkingDirectory(const string& drive) {
+ char buf[2048];
+
+ #ifdef _WIN32
+ const int which = drive.empty() ? 0 : (tolower(drive[0]) - 'a') + 1;
+ assert(which >= 0);
+ if (which != 0) {
+ // Make sure this drive exists.
+ const ULONG mask = _getdrives();
+ if (!(mask & (1<<(which-1))))
+ return getRootDirectory(drive);
+ }
+ _getdcwd(which, buf, sizeof(buf));
+ buf[0] = (char)tolower(buf[0]); // drive letter
+ #else
+ char* bufp = getcwd(buf, sizeof(buf));
+ assert(bufp != 0); // buf not big enough if this happens
+ #endif
+
+ string cwd(buf);
+ if (cwd.size() && cwd[cwd.size()-1] != MyPathSeparator)
+ cwd += MyPathSeparator;
+ return cwd;
+}
+
+string Pathname::getRootDirectory(const string& drive) {
+ #ifdef _WIN32
+ if (drive.empty())
+ return getCurrentDrive() + getPathSeparator();
+ return String(drive[0]).toLower() + ":" + getPathSeparator();
+ #else
+ return getPathSeparator();
+ #endif
+}
+
+bool Pathname::environmentVariableExists(const string& name) {
+ return getenv(name.c_str()) != 0;
+}
+
+string Pathname::getEnvironmentVariable(const string& name) {
+ char* value;
+ value = getenv(name.c_str());
+ return value ? string(value) : string();
+}
+
+
diff --git a/SimTKcommon/src/Plugin.cpp b/SimTKcommon/src/Plugin.cpp
new file mode 100644
index 0000000..0bed2d4
--- /dev/null
+++ b/SimTKcommon/src/Plugin.cpp
@@ -0,0 +1,275 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/internal/common.h"
+#include "SimTKcommon/internal/String.h"
+#include "SimTKcommon/internal/Pathname.h"
+#include "SimTKcommon/internal/Plugin.h"
+
+#include <string>
+using std::string;
+
+#include <cctype>
+using std::tolower;
+
+#include <iostream>
+
+#ifdef _WIN32
+ #define WIN32_LEAN_AND_MEAN
+ #define NOMINMAX
+ #include <windows.h>
+ #include <direct.h>
+ #pragma warning(disable:4996) // getenv() is apparently unsafe
+#else
+ #ifdef __APPLE__
+ #include <mach-o/dyld.h>
+ #endif
+ #include <dlfcn.h>
+ #include <dirent.h>
+ #include <unistd.h>
+#endif
+
+
+using namespace SimTK;
+
+Plugin::Plugin(const string& name) : m_handle(0) {
+ m_defaultName = name; // might be empty
+}
+
+Plugin::~Plugin() {
+ unload();
+}
+
+bool Plugin::load(const string& name) {
+ if (m_handle) {
+ m_lastMessage = "Plugin::load(): already loaded " + m_loadedPathname;
+ return false;
+ }
+
+ const string nameToUse = name.empty() ? m_defaultName : name;
+
+ if (nameToUse.empty()) {
+ m_lastMessage = "Plugin::load(): no pathname was supplied and there was no default.";
+ return false;
+ }
+
+ bool isAbsolutePath;
+ string directory, libPrefix, baseName, debugSuffix, extension;
+ if (!deconstructLibraryName(nameToUse, isAbsolutePath,
+ directory, libPrefix, baseName, debugSuffix, extension))
+ {
+ m_lastMessage = "Plugin::load(): illegal library name '" + nameToUse + "'.";
+ return false;
+ }
+
+ const std::string base = directory + getDynamicLibPrefix() + baseName;
+ if (isAbsolutePath) {
+ m_handle = loadDebugOrReleasePlugin(base, extension, m_loadedPathname, m_lastMessage);
+ } else {
+ for (unsigned i=0; !m_handle && i < m_searchPath.size(); ++i)
+ m_handle = loadDebugOrReleasePlugin(
+ m_searchPath[i] + base, extension, m_loadedPathname, m_lastMessage);
+ }
+
+ return m_handle != 0;
+}
+
+void Plugin::unload() {
+ if (m_handle) {
+ std::cout << "UNLOADING '" << m_loadedPathname << "'.\n";
+ unloadPlugin(m_handle);
+ m_handle = 0;
+ m_loadedPathname.clear();
+ }
+}
+
+// This is like deconstructPathname() but adds the assumption that
+// the pathname represents a library file. We assume further structure
+// for the "fileName" part: it may have a "lib" prefix and "_d"
+// debug suffix which are stripped off to reveal the baseName.
+// Also, this method returns false if there is no baseName
+// left either because the original path was a directory name
+// or because there was nothing left after stripping the prefix
+// or suffix.
+bool Plugin::deconstructLibraryName(const string& name,
+ bool& isAbsolutePath,
+ string& directory,
+ string& libPrefix,
+ string& baseName,
+ string& debugSuffix,
+ string& extension)
+{
+ libPrefix.erase(); baseName.erase(); debugSuffix.erase();
+
+ Pathname::deconstructPathname(name, isAbsolutePath, directory, baseName, extension);
+
+ // If there is a "lib" prefix, strip it off.
+ if (baseName.substr(0,3) == "lib") {
+ libPrefix = "lib";
+ baseName.erase(0,3);
+ }
+
+ // If there is a trailing "_d" or "_D", strip it off. Preserve
+ // the case in the debugSuffix.
+ if (baseName.size() >= 2) {
+ const char last2 = baseName[baseName.size()-2];
+ const char last = baseName[baseName.size()-1];
+ if (last2 == '_' && (last=='d' || last=='D')) {
+ debugSuffix = string("_") + last;
+ baseName.erase(baseName.size()-2);
+ }
+ }
+
+ // That must have left us with something in the baseName or
+ // this is not a legal library name.
+
+ return !baseName.empty();
+}
+
+
+#ifdef _WIN32
+static string getWindowsSystemMessage() {
+ LPVOID lpMsgBuf;
+ FormatMessage(
+ FORMAT_MESSAGE_ALLOCATE_BUFFER |
+ FORMAT_MESSAGE_FROM_SYSTEM |
+ FORMAT_MESSAGE_IGNORE_INSERTS,
+ NULL,
+ GetLastError(),
+ 0, // Default language
+ (LPTSTR) &lpMsgBuf,
+ 0,
+ NULL
+ );
+ const string retStr((const char*)lpMsgBuf);
+
+ LocalFree( lpMsgBuf );
+ return retStr;
+}
+#endif
+
+
+void* Plugin::loadPluginByFileName(const string& name, string& errorMessage) {
+ errorMessage.clear();
+
+ std::cout << "LOAD ATTEMPT: '" << name << "' ... ";
+
+ #ifdef _WIN32
+ // Tell Windows not to bother the user with ugly error boxes.
+ const UINT oldErrorMode = SetErrorMode(SEM_FAILCRITICALERRORS);
+ HMODULE handle = LoadLibrary(name.c_str());
+ SetErrorMode(oldErrorMode); // Restore previous error mode.
+ if (!handle) errorMessage = getWindowsSystemMessage();
+ #else
+ dlerror(); // clear pre-existing message if any
+ void* handle = dlopen(name.c_str(), RTLD_LAZY);
+ if (!handle) {
+ const char* msg = dlerror();
+ if (msg) errorMessage = string(msg);
+ }
+ #endif
+
+ std::cout << (handle ? "SUCCEEDED." : "FAILED!") << std::endl;
+
+ return handle;
+}
+
+// This attempts first to load the debug version of the library if we're
+// in Debug mode, but falls back to the Release if no Debug library is
+// available. If we're running in Release mode then no attempt is made
+// to load the Debug library.
+void* Plugin::loadDebugOrReleasePlugin(const string& base, const string& extension,
+ string& loadedFileName, string& errorMessage)
+{
+ void* handle = 0;
+ if (!getDynamicLibDebugSuffix().empty()) {
+ // Attempt to load the Debug library if it exists.
+ loadedFileName = base + getDynamicLibDebugSuffix() + getDynamicLibExtension();
+ handle = loadPluginByFileName(loadedFileName, errorMessage);
+ }
+ if (!handle) {
+ // Attempt to load the Release library.
+ loadedFileName = base + getDynamicLibExtension();
+ handle = loadPluginByFileName(loadedFileName, errorMessage);
+ }
+ if (!handle)
+ loadedFileName.clear();
+
+ return handle;
+}
+
+void Plugin::unloadPlugin(void* handle) {
+ // We're ignoring the return values here.
+ #ifdef _WIN32
+ (void)FreeLibrary((HMODULE)handle);
+ #else
+ (void)dlclose(handle);
+ #endif
+}
+
+void* Plugin::getSymbolAddress(void* handle, const string& name, string& errorMessage) {
+ void* address;
+ errorMessage.clear();
+
+ #ifdef _WIN32
+ address = GetProcAddress((HMODULE)handle, name.c_str());
+ if (!address)
+ errorMessage = getWindowsSystemMessage();
+ #else
+ dlerror(); // clear pre-existing message if any
+ address = dlsym(handle, name.c_str());
+ if (!address) {
+ const char* msg = dlerror();
+ if (msg) errorMessage = string(msg);
+ }
+ #endif
+
+ return address;
+}
+
+std::string Plugin::getDynamicLibPrefix() {
+ #ifdef _WIN32
+ return "";
+ #else
+ return "lib";
+ #endif
+}
+
+std::string Plugin::getDynamicLibExtension() {
+ #if defined(_WIN32)
+ return ".dll";
+ #elif defined(__APPLE__)
+ return ".dylib";
+ #else // linux
+ return ".so";
+ #endif
+}
+
+std::string Plugin::getDynamicLibDebugSuffix() {
+ #ifdef NDEBUG
+ return ""; // running in Release mode
+ #else
+ return "_d"; // running in Debug mode
+ #endif
+}
+
diff --git a/SimTKcommon/src/PrivateInstantiations.cpp b/SimTKcommon/src/PrivateInstantiations.cpp
new file mode 100644
index 0000000..2c711f7
--- /dev/null
+++ b/SimTKcommon/src/PrivateInstantiations.cpp
@@ -0,0 +1,48 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+#define SimTK_SIMTKCOMMON_DEFINING_POLYGONALMESH
+#define SimTK_SIMTKCOMMON_DEFINING_PARALLEL_EXECUTOR
+#define SimTK_SIMTKCOMMON_DEFINING_PARALLEL_2D_EXECUTOR
+#define SimTK_SIMTKCOMMON_DEFINING_PARALLEL_WORK_QUEUE
+#include "../Geometry/src/PolygonalMeshImpl.h"
+#include "ParallelExecutorImpl.h"
+#include "Parallel2DExecutorImpl.h"
+#include "ParallelWorkQueueImpl.h"
+#include "SimTKcommon/internal/PrivateImplementation_Defs.h"
+
+namespace SimTK {
+
+template class PIMPLHandle<PolygonalMesh, PolygonalMeshImpl, true>;
+template class PIMPLImplementation<PolygonalMesh, PolygonalMeshImpl>;
+
+template class PIMPLHandle<ParallelExecutor, ParallelExecutorImpl>;
+template class PIMPLImplementation<ParallelExecutor, ParallelExecutorImpl>;
+
+template class PIMPLHandle<Parallel2DExecutor, Parallel2DExecutorImpl>;
+template class PIMPLImplementation<Parallel2DExecutor, Parallel2DExecutorImpl>;
+
+template class PIMPLHandle<ParallelWorkQueue, ParallelWorkQueueImpl>;
+template class PIMPLImplementation<ParallelWorkQueue, ParallelWorkQueueImpl>;
+
+} // namespace SimTK
diff --git a/SimTKcommon/src/String.cpp b/SimTKcommon/src/String.cpp
new file mode 100644
index 0000000..1f82471
--- /dev/null
+++ b/SimTKcommon/src/String.cpp
@@ -0,0 +1,172 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This file contains the non-inline implementations of the SimTK::String
+ * class.
+ */
+
+#include "SimTKcommon/internal/common.h"
+#include "SimTKcommon/internal/ExceptionMacros.h"
+#include "SimTKcommon/internal/String.h"
+#include "SimTKcommon/internal/NTraits.h"
+#include "SimTKcommon/internal/Serialize.h"
+
+#include <string>
+#include <cctype>
+
+using SimTK::String;
+
+String::String(float r, const char* fmt) {
+ if (!isFinite(r)) {
+ if (isNaN(r)) {(*this)="NaN"; return;}
+ if (isInf(r)) {(*this)=(r<0?"-Inf":"Inf"); return;}
+ SimTK_ERRCHK2_ALWAYS(false, "SimTK::String(float)",
+ "Unrecognized non-finite value %g (0x%x).",
+ (double)r, *reinterpret_cast<const unsigned*>(&r));
+ return;
+ }
+ char buf[64]; sprintf(buf,fmt,r); (*this)=buf;
+}
+
+String::String(double r, const char* fmt) {
+ if (!isFinite(r)) {
+ if (isNaN(r)) {(*this)="NaN"; return;}
+ if (isInf(r)) {(*this)=(r<0?"-Inf":"Inf"); return;}
+ SimTK_ERRCHK2_ALWAYS(false, "SimTK::String(double)",
+ "Unrecognized non-finite value %g (0x%llx).",
+ r, *reinterpret_cast<const unsigned long long*>(&r));
+ return;
+ }
+ char buf[64]; sprintf(buf,fmt,r); (*this)=buf;
+}
+
+String::String(long double r, const char* fmt) {
+ if (!isFinite(r)) {
+ if (isNaN(r)) {(*this)="NaN"; return;}
+ if (isInf(r)) {(*this)=(r<0?"-Inf":"Inf"); return;}
+ SimTK_ERRCHK1_ALWAYS(false, "SimTK::String(long double)",
+ "Unrecognized non-finite value %lg.", r);
+ return;
+ }
+ char buf[128]; sprintf(buf,fmt,r); (*this)=buf;
+}
+
+static String cleanUp(const String& in) {
+ return String(in).trimWhiteSpace().toLower();
+}
+
+bool String::tryConvertToBool(bool& out) const {
+ const String adjusted = cleanUp(*this);
+ if (adjusted=="true") {out=true; return true;}
+ if (adjusted=="false") {out=false; return true;}
+ std::istringstream sstream(adjusted);
+ sstream >> out;
+ return !sstream.fail();
+}
+
+bool String::tryConvertToFloat(float& out) const {
+ const String adjusted = cleanUp(*this);
+ if (adjusted=="nan") {out=NTraits<float>::getNaN(); return true;}
+ if ( adjusted=="inf" || adjusted=="infinity"
+ || adjusted=="+inf" || adjusted=="+infinity")
+ { out = NTraits<float>::getInfinity(); return true;}
+ if (adjusted=="-inf" || adjusted=="-infinity")
+ { out = -NTraits<float>::getInfinity(); return true;}
+ std::istringstream sstream(adjusted);
+ sstream >> out;
+ return !sstream.fail();
+}
+
+bool String::tryConvertToDouble(double& out) const {
+ const String adjusted = cleanUp(*this);
+ if (adjusted=="nan") {out=NTraits<double>::getNaN(); return true;}
+ if ( adjusted=="inf" || adjusted=="infinity"
+ || adjusted=="+inf" || adjusted=="+infinity")
+ { out = NTraits<double>::getInfinity(); return true;}
+ if (adjusted=="-inf" || adjusted=="-infinity")
+ { out = -NTraits<double>::getInfinity(); return true;}
+ std::istringstream sstream(adjusted);
+ sstream >> out;
+ return !sstream.fail();
+}
+
+bool String::tryConvertToLongDouble(long double& out) const {
+ const String adjusted = cleanUp(*this);
+ if (adjusted=="nan") {out=NTraits<long double>::getNaN(); return true;}
+ if ( adjusted=="inf" || adjusted=="infinity"
+ || adjusted=="+inf" || adjusted=="+infinity")
+ { out = NTraits<long double>::getInfinity(); return true;}
+ if (adjusted=="-inf" || adjusted=="-infinity")
+ { out = -NTraits<long double>::getInfinity(); return true;}
+ std::istringstream sstream(adjusted);
+ sstream >> out;
+ return !sstream.fail();
+}
+
+
+
+String& String::toUpper() {
+ for (int i=0; i < size(); ++i)
+ (*this)[i] = (char)std::toupper((*this)[i]);
+ return *this;
+}
+
+String& String::toLower() {
+ for (int i=0; i < size(); ++i)
+ (*this)[i] = (char)std::tolower((*this)[i]);
+ return *this;
+}
+
+String& String::replaceAllChar(char oldChar, char newChar) {
+ for (int i=0; i < size(); ++i)
+ if ((*this)[i] == oldChar)
+ (*this)[i] = newChar;
+ return *this;
+}
+
+String& String::trimWhiteSpace() {
+ *this = trimWhiteSpace(*this);
+ return *this;
+}
+
+String String::trimWhiteSpace(const std::string& in) {
+ const int inz = (int)in.size();
+
+ // Find first non-white character position of "in".
+ int firstNonWhite = 0;
+ for ( ; firstNonWhite < inz; ++firstNonWhite)
+ if (!std::isspace(in[firstNonWhite])) break;
+
+ if (firstNonWhite == inz)
+ return String(); // "in" was all white space
+
+ // Find last non-white character position of "in".
+ int lastNonWhite = inz-1;
+ for ( ; lastNonWhite >= 0; --lastNonWhite)
+ if (!std::isspace(in[lastNonWhite])) break;
+
+ return String(in, firstNonWhite, (lastNonWhite+1) - firstNonWhite);
+}
+
+
diff --git a/SimTKcommon/src/Timing.cpp b/SimTKcommon/src/Timing.cpp
new file mode 100644
index 0000000..8ed4b0e
--- /dev/null
+++ b/SimTKcommon/src/Timing.cpp
@@ -0,0 +1,338 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * Defines any routines that have to be supplied to implement the features
+ * promised in Timing.h. Currently this consists of faking up the
+ * Posix timing routines when using the Microsoft Visual Studio C/C++
+ * compiler cl on Windows, or when running on Mac OSX.
+ */
+
+
+#include "SimTKcommon/internal/common.h"
+#include "SimTKcommon/internal/Timing.h"
+
+#include "pthread.h" // included in SimTK_SDK/include
+
+#include <string>
+#include <cstring>
+#include <cctype>
+#include <ctime>
+
+#include "errno.h"
+
+#if defined(_MSC_VER)
+ // Keeps MS VC++ quiet about sprintf, strcpy, etc.
+ #pragma warning(disable:4996)
+
+ #define WIN32_LEAN_AND_MEAN
+ #define NOMINMAX
+ #include <Windows.h>
+#elif defined(__APPLE__)
+ #include <unistd.h>
+ #include <sys/time.h>
+ #include <mach/mach.h>
+ #include <mach/mach_time.h>
+#endif
+
+// There are a billion (1e9) nanoseconds in a second.
+static const long long NsPerSec = 1000000000LL;
+
+// There are a million (1e6) nanoseconds in a millisecond.
+static const long long NsPerMs = 1000000LL;
+
+// There are a thousand (1e3) nanoseconds in a microsecond.
+static const long long NsPerUs = 1000LL;
+
+// There are 10 million (1e7) 100ns (hecto ns) ticks in a second.
+static const long long HectoNsPerSec = 10000000LL;
+
+// There are a million (1e6) microseconds in a second.
+static const long long UsPerSec = 1000000LL;
+
+// static getnstimeofday()
+// -----------------------
+// Returns the time of day in seconds and nanoseconds since 1-1-1970.
+// This is derived from code by Rolf Steenge, posted at
+// https://projects.coin-or.org/Cbc/ticket/45.
+// This method is the guts of CLOCK_REALTIME.
+#if defined(_MSC_VER)
+ // This is the time interval from the Win32 epoch 1/1/1601 to
+ // the Unix epoch 1/1/1970, measured in 100ns (hecto ns) ticks.
+ // The number of days between those dates (measuring them both
+ // using the same calendar) is 134774=(369 years)*365 + 89 leap days,
+ // or 11644473600 seconds (=134774*24*3600).
+ static const long long FILETIME_1970 = 11644473600LL*HectoNsPerSec;
+
+ // Return the value of a FILETIME reinterpreted as a
+ // long long integer count of the number of 100ns ("hecto ns")
+ // ticks since start of 1/1/1601 UTC.
+ static inline long long
+ filetimeToHectoNs(const FILETIME& ft) {
+ // Must do this by copying, not casting, to avoid alignment
+ // problems; FILETIME doesn't have to be 16 byte aligned.
+ LARGE_INTEGER large;
+ large.HighPart = ft.dwHighDateTime;
+ large.LowPart = ft.dwLowDateTime;
+ return large.QuadPart;
+ }
+
+ // Input is number of 100ns ticks since some base time; output is that
+ // same interval changed to seconds and nanoseconds in a timespec struct.
+ static inline void
+ hectoNsToTimespec(const long long& hecto, struct timespec& ts) {
+ ts.tv_sec = (long) (hecto / HectoNsPerSec); // seconds
+ ts.tv_nsec = (long) ((hecto % HectoNsPerSec) * 100LL); // nanoseconds
+ }
+
+ // Given time reported as a Win32 FILETIME measured from 1/1/1601, convert
+ // it to a timespec measured from 1/1/1970.
+ // CAUTION: note shift of epoch (measurement base) here.
+ static inline void filetimeToTimespec(const FILETIME& ft,
+ struct timespec& ts) {
+ long long hecto = filetimeToHectoNs(ft);
+ hectoNsToTimespec(hecto - FILETIME_1970, ts);
+ }
+
+ static int getnstimeofday(struct timespec *tp) {
+ if (!tp) return 0;
+ FILETIME ft;
+ GetSystemTimeAsFileTime(&ft); // 100-nanoseconds since 1-1-1601
+ filetimeToTimespec(ft, *tp); // now in ns since 1-1-1970
+ return 0;
+ }
+#elif defined(__APPLE__)
+ static int getnstimeofday(struct timespec *tp) {
+ if (!tp) return 0;
+ struct timeval tod;
+ gettimeofday(&tod, 0); // don't care about timezone
+ tp->tv_sec = tod.tv_sec;
+ tp->tv_nsec = 1000*tod.tv_usec;
+ return 0;
+ }
+#endif
+
+// static getperformancecounter()
+// ------------------------------
+// This is the guts of CLOCK_MONOTONIC (all variants). This is the number of
+// seconds and nanoseconds since an arbitrary start time, with a very high
+// resolution. This is for measuring short intervals very accurately; long
+// term it might drift away from the real time clock.
+#if defined(_MSC_VER)
+ static int getperformancecounter(struct timespec *tp) {
+ if (!tp) return 0;
+
+ // The frequency is not allowed to change after the system is started
+ // (according to Microsoft) so we just call this once.
+ static long long ticksPerSec = 0LL;
+ if (ticksPerSec==0LL) {
+ LARGE_INTEGER tps;
+ QueryPerformanceFrequency(&tps);
+ ticksPerSec = tps.QuadPart;
+ }
+
+ LARGE_INTEGER ticks;
+ if(!QueryPerformanceCounter(&ticks))
+ return EINVAL; // bad news
+
+ // Careful: don't try to simplify or rearrange these expressions;
+ // we're depending on integer arithmetic done in long long precision.
+ tp->tv_sec = (long) (ticks.QuadPart / ticksPerSec);
+ tp->tv_nsec =(long)((ticks.QuadPart % ticksPerSec)*NsPerSec / ticksPerSec);
+
+ return 0;
+ }
+#elif defined(__APPLE__)
+ static int getperformancecounter(struct timespec *tp) {
+ if (!tp) return 0;
+
+ static mach_timebase_info_data_t info = {0,0};
+
+ if (info.denom == 0)
+ mach_timebase_info(&info);
+
+ unsigned long long ticks = mach_absolute_time();
+ unsigned long long ticksInNs = (ticks * info.numer) / info.denom;
+
+ tp->tv_sec = (time_t)(ticksInNs / NsPerSec);
+ tp->tv_nsec = (long)(ticksInNs % NsPerSec);
+
+ return 0;
+ }
+#endif
+
+// static getprocesscputime()
+// --------------------------
+// This is the guts of CLOCK_PROCESS_CPUTIME_ID. It returns the number of
+// seconds and nanoseconds of cpu time used by all the threads of the
+// currently executing process since it started.
+#if defined(_MSC_VER)
+ static int getprocesscputime(struct timespec* tp) {
+ if (!tp) return 0;
+
+ FILETIME creationTime, exitTime, kernelTime, userTime;
+ if (!GetProcessTimes(GetCurrentProcess(),
+ &creationTime, &exitTime,
+ &kernelTime, &userTime))
+ return EINVAL;
+
+ long long ktime = filetimeToHectoNs(kernelTime);
+ long long utime = filetimeToHectoNs(userTime);
+ hectoNsToTimespec(ktime+utime, *tp);
+ return 0;
+ }
+#elif defined(__APPLE__)
+ static int getprocesscputime(struct timespec* tp) {
+ if (!tp) return 0;
+
+ task_basic_info deadThreadTimes; // for terminated threads
+ task_thread_times_info liveThreadTimes; // for live threads
+ mach_msg_type_number_t deadsz, livesz;
+ task_t currentProcess;
+ kern_return_t status;
+ currentProcess = mach_task_self();
+ deadsz = sizeof(deadThreadTimes)/sizeof(int);
+ livesz = sizeof(liveThreadTimes)/sizeof(int);
+ status = task_info(currentProcess, TASK_BASIC_INFO,
+ (task_info_t)&deadThreadTimes,&deadsz);
+ status = task_info(currentProcess, TASK_THREAD_TIMES_INFO,
+ (task_info_t)&liveThreadTimes,&livesz);
+
+ tp->tv_sec = (time_t)( deadThreadTimes.user_time.seconds
+ + deadThreadTimes.system_time.seconds
+ + liveThreadTimes.user_time.seconds
+ + liveThreadTimes.system_time.seconds);
+ tp->tv_nsec = (long)(1000*(deadThreadTimes.user_time.microseconds
+ + deadThreadTimes.system_time.microseconds
+ + liveThreadTimes.user_time.microseconds
+ + liveThreadTimes.system_time.microseconds));
+ while (tp->tv_nsec >= NsPerSec) {
+ ++tp->tv_sec;
+ tp->tv_nsec -= NsPerSec;
+ }
+ return 0;
+ }
+#endif
+
+// static getthreadcputime()
+// -------------------------
+// This is the guts of CLOCK_THREAD_CPUTIME_ID. It returns the number of
+// seconds and nanoseconds of cpu time used by the currently executing
+// thread since it started.
+#if defined(_MSC_VER)
+ static int getthreadcputime(struct timespec* tp) {
+ if (!tp) return 0;
+
+ FILETIME creationTime, exitTime, kernelTime, userTime;
+ if (!GetThreadTimes(GetCurrentThread(),
+ &creationTime, &exitTime,
+ &kernelTime, &userTime))
+ return EINVAL;
+
+ long long ktime = filetimeToHectoNs(kernelTime);
+ long long utime = filetimeToHectoNs(userTime);
+ hectoNsToTimespec(ktime+utime, *tp);
+ return 0;
+ }
+#elif defined(__APPLE__)
+ static int getthreadcputime(struct timespec* tp) {
+ if (!tp) return 0;
+
+ thread_basic_info info;
+ mach_msg_type_number_t infosz;
+ mach_port_t currentThread;
+ kern_return_t status;
+ currentThread = mach_thread_self(); // get "send rights"
+ infosz = sizeof(info)/sizeof(int);
+ status = thread_info(currentThread, THREAD_BASIC_INFO,
+ (thread_info_t)&info,&infosz);
+ // relinquish "send rights"
+ mach_port_deallocate(mach_task_self(), currentThread);
+
+ tp->tv_sec = (time_t)(info.user_time.seconds + info.system_time.seconds);
+ tp->tv_nsec = (long)(1000*( info.user_time.microseconds
+ + info.system_time.microseconds));
+ while (tp->tv_nsec >= NsPerSec) {
+ ++tp->tv_sec;
+ tp->tv_nsec -= NsPerSec;
+ }
+
+ return 0;
+ }
+#endif
+
+// int clock_gettime()
+// -------------------
+// Now define the Posix clock_gettime() function in terms of the above helpers.
+#if defined(_MSC_VER) || defined(__APPLE__)
+ int clock_gettime (clockid_t clock_id, struct timespec *tp) {
+ int retval = EINVAL;
+
+ switch (clock_id)
+ {
+ case CLOCK_REALTIME:
+ retval = getnstimeofday(tp);
+ break;
+ case CLOCK_MONOTONIC:
+ case CLOCK_MONOTONIC_HR: // "high resolution"
+ case CLOCK_MONOTONIC_RAW: // "not subject to NTP adjustments"
+ retval = getperformancecounter(tp);
+ break;
+ case CLOCK_PROCESS_CPUTIME_ID:
+ retval = getprocesscputime(tp);
+ break;
+ case CLOCK_THREAD_CPUTIME_ID:
+ retval = getthreadcputime(tp);
+ break;
+ default:
+ if (tp) {tp->tv_sec = 0; tp->tv_nsec = 0;}
+ retval = EINVAL;
+ break;
+ }
+
+ return retval;
+ }
+#endif
+
+// int nanosleep()
+// ---------------
+// Sleep for the amount of time required in req. If we return
+// early, rem is supposed to say how much time is left. However, our
+// implementation is noninterruptable so we'll always report a
+// zero remainder. Note that 0 is an allowable value and just means
+// "give up your time slice" which will allow another ready-to-run
+// thread to start if there is one but otherwise return immediately.
+#if defined(_MSC_VER)
+ int nanosleep(const struct timespec* req, struct timespec* rem) {
+ if (!req) return EINVAL;
+ const long long reqns = SimTK::timespecToNs(*req);
+ if (reqns < 0LL) return EINVAL;
+ // Round to the nearest ms.
+ static const long long halfMsInNs = NsPerMs/2;
+ const DWORD reqms = (DWORD)((reqns + halfMsInNs)/NsPerMs);
+ Sleep(reqms);
+ if (rem) {rem->tv_sec=0; rem->tv_nsec=0;}
+ return 0;
+ }
+#endif
+
diff --git a/SimTKcommon/src/Xml.cpp b/SimTKcommon/src/Xml.cpp
new file mode 100644
index 0000000..2ee030c
--- /dev/null
+++ b/SimTKcommon/src/Xml.cpp
@@ -0,0 +1,1067 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/internal/common.h"
+#include "SimTKcommon/internal/String.h"
+#include "SimTKcommon/internal/Plugin.h"
+#include "SimTKcommon/internal/Xml.h"
+
+#include "tinyxml.h"
+
+using namespace SimTK;
+
+//------------------------------------------------------------------------------
+// XML IMPL
+//------------------------------------------------------------------------------
+class Xml::Impl {
+public:
+ Impl() {} // not canonicalized yet
+ Impl(const String& pathname) {
+ readFromFile(pathname.c_str());
+ }
+ ~Impl() {
+ clear();
+ }
+
+ // Note that the copy must be canonicalized before use -- that's so we
+ // get our root element pointing correctly into the copy rather than
+ // at the source's root element.
+ Impl* clone() const {
+ Impl* newImpl = new Impl();
+ newImpl->m_tixml = m_tixml;
+ // root element isn't set yet
+ return newImpl;
+ }
+
+ void clear() {
+ m_tixml.Clear();
+ m_rootElement.clear();
+ }
+
+ void setIndentString(const String& indent)
+ { m_tixml.SetIndentString(indent); }
+ const String& getIndentString() const
+ { return m_tixml.GetIndentString(); }
+
+ void readFromFile(const String& pathname) {
+ clear();
+ m_tixml.SetValue(pathname);
+ bool loadOK = m_tixml.LoadFile();
+ SimTK_ERRCHK2_ALWAYS(loadOK, "Xml::readFromFile()",
+ "Failed to load the Xml file '%s' with error '%s'.",
+ pathname.c_str(), m_tixml.ErrorMsg().c_str());
+ }
+
+ void writeToFile(const String& pathname) const {
+ bool saveOK = m_tixml.SaveFile(pathname);
+ SimTK_ERRCHK2_ALWAYS(saveOK, "Xml::writeToFile()",
+ "Failed to write to the Xml file '%s' with error '%s'.",
+ pathname.c_str(), m_tixml.ErrorMsg().c_str());
+ }
+
+ void readFromString(const char* xmlDocument) {
+ clear();
+ m_tixml.Parse(xmlDocument);
+ SimTK_ERRCHK1_ALWAYS(!m_tixml.Error(), "Xml::readFromString()",
+ "Failed to parse the Xml string with error '%s'.",
+ m_tixml.ErrorMsg().c_str());
+ }
+
+ void writeToString(String& xmlDocument, bool compact) const {
+ TiXmlPrinter printer(xmlDocument);
+ if (compact) printer.SetStreamPrinting();
+ else printer.SetIndent(m_tixml.GetIndentChars());
+ m_tixml.Accept( &printer );
+ }
+
+ // Call this during construction and after a new Xml document has been
+ // parsed. It guarantees: (1) there is a Declaration record and it is
+ // the first node at the top level of the Xml document, and (2) there
+ // is no top-level text and only one top-level element and that is the
+ // "root" element whose tag name is the document type.
+ void canonicalizeDocument() {
+ TiXmlDeclaration* decl = addDeclarationIfNeeded();
+ TiXmlElement* root = addRootElementIfNeeded();
+
+ m_rootElement.setTiNodePtr(root);
+ }
+
+ const TiXmlDeclaration& getTiXmlDeclaration() const {
+ const TiXmlNode* decl = m_tixml.FirstChild();
+ assert(decl && decl->Type()==TiXmlNode::DECLARATION);
+ return *decl->ToDeclaration();
+ }
+
+ TiXmlDeclaration& updTiXmlDeclaration() {
+ TiXmlNode* decl = m_tixml.FirstChild();
+ assert(decl && decl->Type()==TiXmlNode::DECLARATION);
+ return *decl->ToDeclaration();
+ }
+
+
+ // The first thing in every Xml file should be the Declaration, that is,
+ // the line that looks something like
+ // <?xml version="1.0" encoding="UTF-8"?>
+ // That's because the encoding is needed to read the rest of the file.
+ // We'll look through the file to see if there is a declaration at
+ // the top level. If so, we'll move it to the first slot if necessary.
+ // If not, we'll create a default one and put it first. Either way we
+ // return a pointer to the declaration which will now be the first
+ // node in the Xml document.
+ TiXmlDeclaration* addDeclarationIfNeeded() {
+ TiXmlNode* child=m_tixml.FirstChild();
+ if (child && child->Type() == TiXmlNode::DECLARATION)
+ return child->ToDeclaration(); // the easy and most common case
+
+ // Otherwise hunt for the declaration.
+ for (; child; child=child->NextSibling())
+ if (child->Type() == TiXmlNode::DECLARATION)
+ break;
+
+ if (child)
+ m_tixml.DisconnectChild(child); // it's in the wrong place
+ else child = new TiXmlDeclaration("1.0", "UTF-8", "");
+
+ // Insert new node as the first in the document.
+ m_tixml.LinkBeginChild(child);
+ return child->ToDeclaration();
+ }
+
+ // If the supplied Xml document has zero or more than one top-level element,
+ // or has top-level text, we'll add <_Root> elements </_Root> to
+ // encapsulate all the top-level text and element nodes (this may also
+ // surround top-level comments and unknowns if they occur between the text
+ // and elements. A pointer to the root element (whether original or added)
+ // is returned.
+ TiXmlElement* addRootElementIfNeeded() {
+ // Find the first element or text node, and remember the node that
+ // preceded it since that's where we may be inserting the new root
+ // element.
+ TiXmlNode* nodeBeforeFirst = 0;
+ TiXmlNode* firstEltOrText = m_tixml.FirstChild();
+ while ( firstEltOrText &&
+ !(firstEltOrText->ToElement()||firstEltOrText->ToText()))
+ { nodeBeforeFirst = firstEltOrText;
+ firstEltOrText = firstEltOrText->NextSibling(); }
+
+ if (!firstEltOrText) {
+ // No top level element or text node. We'll just append an empty
+ // _Root element to the end of whatever's there.
+ TiXmlElement* root = new TiXmlElement("_Root");
+ m_tixml.LinkEndChild(root);
+ return root;
+ }
+
+ // There is at least one element or text node at the top level; the
+ // first one is pointed to by firstEltOrText, and the node just before
+ // that (if any) is pointed to by nodeBeforeFirst. Now find the last
+ // top-level element or text node.
+ TiXmlNode* lastEltOrText = m_tixml.LastChild();
+ while ( lastEltOrText &&
+ !(lastEltOrText->ToElement()||lastEltOrText->ToText()))
+ lastEltOrText = lastEltOrText->PreviousSibling();
+
+ assert(lastEltOrText); // should have at least re-found the first one!
+
+ // If the extremely likely case that the first and last are the same
+ // node and that node is an element, then the document is already in the
+ // right format and we don't have to do anything to it.
+ if (firstEltOrText==lastEltOrText && firstEltOrText->ToElement())
+ return firstEltOrText->ToElement();
+
+ // Now we know there is top-level text or more than one top-level
+ // element so we are going to have to surround everything between
+ // first and last with a new root element.
+ TiXmlElement* root = new TiXmlElement("_Root");
+
+ TiXmlNode* nextToMove = firstEltOrText;
+ while(true) {
+ assert(nextToMove); // can't happen!
+ TiXmlNode* moveMe = nextToMove;
+ nextToMove = moveMe->NextSibling();
+ root->LinkEndChild(m_tixml.DisconnectChild(moveMe));
+ // Did we just move the last element or text node?
+ if (moveMe == lastEltOrText) break;
+ }
+
+ // Now link the new root element right where we found the first
+ // element or text node.
+ if (nodeBeforeFirst) m_tixml.LinkAfterChild(nodeBeforeFirst, root);
+ else m_tixml.LinkBeginChild(root);
+
+ return root;
+ }
+
+ // The Tiny XML "document" is a TiXmlNode representing the entire XML
+ // object. We instead represent the document as an object of class Xml
+ // which is not itself a Node.
+ TiXmlDocument m_tixml;
+
+ // This references the root element withing the TinyXml document. It
+ // is filled in when the document is initially canonicalized.
+ Xml::Element m_rootElement;
+
+private:
+ Impl(const Impl&); // disable; use clone()
+ Impl& operator=(const Impl&); // "
+};
+
+
+
+//------------------------------------------------------------------------------
+// XML
+//------------------------------------------------------------------------------
+
+// Handy helper for weeding out unwanted nodes.
+static bool nodeTypeIsAllowed(Xml::NodeType allowed,
+ TiXmlNode::NodeType found) {
+ switch(found) {
+ case TiXmlNode::ELEMENT: return (allowed & Xml::ElementNode)!=0;
+ case TiXmlNode::TEXT: return (allowed & Xml::TextNode) !=0;
+ case TiXmlNode::COMMENT: return (allowed & Xml::CommentNode)!=0;
+ case TiXmlNode::UNKNOWN: return (allowed & Xml::UnknownNode)!=0;
+ default: return false;
+ }
+}
+
+/*static*/String Xml::getNodeTypeAsString(Xml::NodeType type) {
+ // Take care of special cases first.
+ switch(type) {
+ case NoNode: return "NoNode";
+ case NoJunkNodes: return "NoJunkNodes";
+ case JunkNodes: return "JunkNodes";
+ case AnyNodes: return "AnyNodes";
+ default: ; // fall through
+ }
+
+ // "Or" bits together in CUTE order.
+ String out;
+ if (type & CommentNode) out = "CommentNode";
+ if (type & UnknownNode)
+ { if (!out.empty()) out += "|"; out += "UnknownNode"; }
+ if (type & TextNode)
+ { if (!out.empty()) out += "|"; out += "TextNode"; }
+ if (type & ElementNode)
+ { if (!out.empty()) out += "|"; out += "ElementNode"; }
+
+ return out;
+}
+
+Xml::Xml() : impl(0) {
+ impl = new Impl();
+ impl->canonicalizeDocument();
+}
+
+Xml::Xml(const String& pathname) : impl(0) {
+ impl = new Impl(pathname);
+ impl->canonicalizeDocument();
+}
+
+Xml::Xml(const Xml& source) : impl(0) {
+ if (source.impl) {
+ impl = source.impl->clone();
+ impl->canonicalizeDocument();
+ }
+}
+
+Xml& Xml::operator=(const Xml& source) {
+ if (&source != this) {
+ delete impl; impl = 0;
+ if (source.impl) {
+ impl = source.impl->clone();
+ impl->canonicalizeDocument();
+ }
+ }
+ return *this;
+}
+
+Xml::~Xml() {
+ delete impl;
+ impl = 0;
+}
+
+
+void Xml::readFromFile(const String& pathname)
+{ updImpl().readFromFile(pathname.c_str());
+ updImpl().canonicalizeDocument(); }
+void Xml::writeToFile(const String& pathname) const
+{ getImpl().writeToFile(pathname.c_str()); }
+void Xml::readFromString(const char* xmlDocument)
+{ updImpl().readFromString(xmlDocument);
+ updImpl().canonicalizeDocument(); }
+void Xml::readFromString(const String& xmlDocument)
+{ readFromString(xmlDocument.c_str()); }
+void Xml::writeToString(String& xmlDocument, bool compact) const
+{ getImpl().writeToString(xmlDocument, compact); }
+void Xml::setIndentString(const String& indent)
+{ updImpl().setIndentString(indent); }
+const String& Xml::getIndentString() const
+{ return getImpl().getIndentString(); }
+
+/*static*/void Xml::setXmlCondenseWhiteSpace(bool shouldCondense)
+{ TiXmlBase::SetCondenseWhiteSpace(shouldCondense); }
+/*static*/bool Xml::isXmlWhiteSpaceCondensed()
+{ return TiXmlBase::IsWhiteSpaceCondensed(); }
+
+Xml::Element Xml::getRootElement()
+{ assert(getImpl().m_rootElement.isValid());
+ return updImpl().m_rootElement; }
+const String& Xml::getRootTag() const
+{ return unconst().getRootElement().getElementTag(); }
+void Xml::setRootTag(const String& tag)
+{ getRootElement().setElementTag(tag); }
+
+void Xml::insertTopLevelNodeAfter (const Xml::node_iterator& afterThis,
+ Xml::Node insertThis) {
+ const char* method = "Xml::insertTopLevelNodeAfter()";
+
+ // Check that the supplied Node is OK.
+ SimTK_ERRCHK_ALWAYS(insertThis.isValid(), method,
+ "The supplied Node handle was empty.");
+ SimTK_ERRCHK_ALWAYS(insertThis.isOrphan(), method,
+ "The Node was not an orphan so can't be inserted.");
+ SimTK_ERRCHK1_ALWAYS(Comment::isA(insertThis) || Unknown::isA(insertThis),
+ method, "The Node had NodeType %s, but only Comment and Unknown nodes"
+ " can be inserted at the topmost document level.",
+ insertThis.getNodeTypeAsString().c_str());
+
+ // If no iterator, add the Node to the end and we're done.
+ if (afterThis == node_end()) {
+ updImpl().m_tixml.LinkEndChild(insertThis.updTiNodePtr());
+ return;
+ }
+
+ // There is an iterator, make sure it's a top-level one.
+ SimTK_ERRCHK_ALWAYS(afterThis->isTopLevelNode(), method,
+ "The node_iterator did not refer to a top-level Node.");
+
+ updImpl().m_tixml.LinkAfterChild(afterThis->updTiNodePtr(),
+ insertThis.updTiNodePtr());
+}
+
+void Xml::insertTopLevelNodeBefore(const Xml::node_iterator& beforeThis,
+ Xml::Node insertThis) {
+ const char* method = "Xml::insertTopLevelNodeBefore()";
+
+ // Check that the supplied Node is OK.
+ SimTK_ERRCHK_ALWAYS(insertThis.isValid(), method,
+ "The supplied Node handle was empty.");
+ SimTK_ERRCHK_ALWAYS(insertThis.isOrphan(), method,
+ "The Node was not an orphan so can't be inserted.");
+ SimTK_ERRCHK1_ALWAYS(Comment::isA(insertThis) || Unknown::isA(insertThis),
+ method, "The Node had NodeType %s, but only Comment and Unknown nodes"
+ " can be inserted at the topmost document level.",
+ insertThis.getNodeTypeAsString().c_str());
+
+ // If no iterator, add the Node to the end and we're done.
+ if (beforeThis == node_end()) {
+ updImpl().m_tixml.LinkEndChild(insertThis.updTiNodePtr());
+ return;
+ }
+
+ // There is an iterator, make sure it's a top-level one.
+ SimTK_ERRCHK_ALWAYS(beforeThis->isTopLevelNode(), method,
+ "The node_iterator did not refer to a top-level Node.");
+
+ updImpl().m_tixml.LinkBeforeChild(beforeThis->updTiNodePtr(),
+ insertThis.updTiNodePtr());
+}
+
+void Xml::eraseTopLevelNode(const Xml::node_iterator& deleteThis) {
+ const char* method = "Xml::eraseTopLevelNode()";
+
+ // Check that the supplied iterator points to something.
+ SimTK_ERRCHK_ALWAYS(deleteThis != node_end(), method,
+ "The node_iterator is at node_end() so doesn't refer to a Node.");
+ // There is an iterator, make sure it's a top-level one.
+ SimTK_ERRCHK_ALWAYS(deleteThis->isTopLevelNode(), method,
+ "The node_iterator did not refer to a top-level Node.");
+ SimTK_ERRCHK1_ALWAYS(Comment::isA(*deleteThis) || Unknown::isA(*deleteThis),
+ method, "The Node had NodeType %s, but only Comment and Unknown nodes"
+ " can be erased at the topmost document level.",
+ deleteThis->getNodeTypeAsString().c_str());
+
+ updImpl().m_tixml.RemoveChild(deleteThis->updTiNodePtr());
+}
+
+Xml::Node Xml::removeTopLevelNode(const Xml::node_iterator& removeThis) {
+ const char* method = "Xml::removeTopLevelNode()";
+
+ // Check that the supplied iterator points to something.
+ SimTK_ERRCHK_ALWAYS(removeThis != node_end(), method,
+ "The node_iterator is at node_end() so doesn't refer to a Node.");
+ // There is an iterator, make sure it's a top-level one.
+ SimTK_ERRCHK_ALWAYS(removeThis->isTopLevelNode(), method,
+ "The node_iterator did not refer to a top-level Node.");
+ SimTK_ERRCHK1_ALWAYS(Comment::isA(*removeThis) || Unknown::isA(*removeThis),
+ method, "The Node had NodeType %s, but only Comment and Unknown nodes"
+ " can be removed at the topmost document level.",
+ removeThis->getNodeTypeAsString().c_str());
+
+ TiXmlNode* p = updImpl().m_tixml.DisconnectChild(removeThis->updTiNodePtr());
+ return Node(p);
+}
+
+
+String Xml::getXmlVersion() const
+{ return getImpl().getTiXmlDeclaration().Version(); }
+String Xml::getXmlEncoding() const
+{ return getImpl().getTiXmlDeclaration().Encoding(); }
+bool Xml::getXmlIsStandalone() const
+{ return String::toLower(getImpl().getTiXmlDeclaration().Standalone())!="no"; }
+
+void Xml::setXmlVersion(const String& version)
+{ updImpl().updTiXmlDeclaration().SetVersion(version.c_str()); }
+void Xml::setXmlEncoding(const String& encoding)
+{ updImpl().updTiXmlDeclaration().SetEncoding(encoding.c_str()); }
+// For the standalone case we set the string to "" rather than "yes" so that
+// the standalone attribute won't appear in the output declaration.
+void Xml::setXmlIsStandalone(bool isStandalone)
+{ updImpl().updTiXmlDeclaration().SetStandalone(isStandalone ? "" : "no"); }
+
+
+ // XML node_begin()
+Xml::node_iterator Xml::node_begin(NodeType allowed) {
+ TiXmlNode* first = updImpl().m_tixml.FirstChild();
+ while (first && !nodeTypeIsAllowed(allowed, first->Type()))
+ first = first->NextSibling();
+ return node_iterator(first, allowed);
+}
+
+ // XML node_end()
+Xml::node_iterator Xml::node_end() const
+{ return node_iterator(0); }
+
+
+
+//------------------------------------------------------------------------------
+// XML ATTRIBUTE
+//------------------------------------------------------------------------------
+Xml::Attribute::Attribute(const String& name, const String& value)
+: tiAttr(new TiXmlAttribute(name,value)) {}
+
+void Xml::Attribute::clear() {
+ tiAttr = 0;
+}
+
+void Xml::Attribute::clearOrphan() {
+ if (!tiAttr) return;
+ SimTK_ERRCHK_ALWAYS(isOrphan(), "Xml::Attribute::clearOrphan()",
+ "This Attribute is not an orphan (or it was already destructed"
+ " and now contains garbage).");
+ delete tiAttr; // not part of any document
+ tiAttr = 0;
+}
+
+// Note that the criteria for orphanhood is that the referenced
+// TiXmlAttr is not in a document.
+bool Xml::Attribute::isOrphan() const
+{ if (!isValid()) return false; // empty handle not considered an orphan
+ return tiAttr->GetDocument() == 0; }
+
+const String& Xml::Attribute::getName() const {
+ SimTK_ERRCHK_ALWAYS(isValid(), "Xml::Attribute::getName()",
+ "The attribute handle was empty.");
+ return getTiAttr().NameStr();
+}
+const String& Xml::Attribute::getValue() const {
+ SimTK_ERRCHK_ALWAYS(isValid(), "Xml::Attribute::getValue()",
+ "The attribute handle was empty.");
+ return getTiAttr().ValueStr();
+}
+
+Xml::Attribute& Xml::Attribute::setName(const String& name) {
+ SimTK_ERRCHK_ALWAYS(isValid(), "Xml::Attribute::setName()",
+ "The attribute handle was empty.");
+ updTiAttr().SetName(name);
+ return *this;
+}
+Xml::Attribute& Xml::Attribute::setValue(const String& value) {
+ SimTK_ERRCHK_ALWAYS(isValid(), "Xml::Attribute::setValue()",
+ "The attribute handle was empty.");
+ updTiAttr().SetValue(value);
+ return *this;
+}
+
+void Xml::Attribute::writeToString(String& out) const {
+ getTiAttr().Print(0,0,&out);
+}
+
+//------------------------------------------------------------------------------
+// XML ATTRIBUTE ITERATOR
+//------------------------------------------------------------------------------
+Xml::attribute_iterator& Xml::attribute_iterator::
+operator++() {
+ TiXmlAttribute* next = attr.updTiAttr().Next();
+ attr.setTiAttrPtr(next);
+ return *this;
+}
+
+Xml::attribute_iterator Xml::attribute_iterator::
+operator++(int) {
+ Attribute save(attr);
+ TiXmlAttribute* next = attr.updTiAttr().Next();
+ attr.setTiAttrPtr(next);
+ return attribute_iterator(save);
+}
+
+Xml::attribute_iterator& Xml::attribute_iterator::
+operator--() {
+ TiXmlAttribute* prev = attr.updTiAttr().Previous();
+ attr.setTiAttrPtr(prev);
+ return *this;
+}
+
+Xml::attribute_iterator Xml::attribute_iterator::
+operator--(int) {
+ Attribute save(attr);
+ TiXmlAttribute* prev = attr.updTiAttr().Previous();
+ attr.setTiAttrPtr(prev);
+ return attribute_iterator(save);
+}
+
+
+
+//------------------------------------------------------------------------------
+// XML NODE
+//------------------------------------------------------------------------------
+Xml::Node Xml::Node::clone() const {
+ TiXmlNode* newNode = 0;
+ if (tiNode) newNode = tiNode->Clone();
+ return Node(newNode);
+}
+
+void Xml::Node::clear() {
+ // TODO: Note that we do not clean up heap space here if this node is
+ // still an orphan. To do that requires that we reference count the tiNode
+ // so that we don't risk looking at deleted garbage in trying to determine
+ // orphanhood.
+ tiNode = 0;
+}
+
+void Xml::Node::clearOrphan() {
+ if (tiNode==0) return;
+ SimTK_ERRCHK_ALWAYS(isOrphan(), "Xml::Node::clearOrphan()",
+ "This Node is not an orphan (or it was already destructed and now"
+ " contains garbage).");
+ delete tiNode;
+ tiNode = 0;
+}
+
+
+Xml::NodeType Xml::Node::getNodeType() const {
+ if (!isValid()) return NoNode;
+ switch(getTiNode().Type()) {
+ case TiXmlNode::COMMENT: return CommentNode;
+ case TiXmlNode::UNKNOWN: return UnknownNode;
+ case TiXmlNode::TEXT: return TextNode;
+ case TiXmlNode::ELEMENT: return ElementNode;
+ default: SimTK_ASSERT1_ALWAYS(false,
+ "Xml::Node::getNodeType(): can't convert TinyXML node type %s to any"
+ " SimTK::Xml node type.", getTiNode().TypeName());
+ }
+ return NoNode;
+}
+
+String Xml::Node::
+getNodeTypeAsString() const {return Xml::getNodeTypeAsString(getNodeType());}
+
+const String& Xml::Node::getNodeText() const {
+ SimTK_ERRCHK_ALWAYS(isValid(), "Xml::Node::getText()",
+ "Can't get text from an empty Node handle.");
+
+ return getTiNode().ValueStr();
+}
+
+bool Xml::Node::isTopLevelNode() const
+{ if (!isValid()) return false;
+ const TiXmlNode& n = getTiNode();
+ return n.Parent() && n.Parent() == n.GetDocument(); }
+
+// Note that the criteria for orphanhood is that the *TiXmlNode* has no
+// parent. In SimTK::Xml, none of the top-level nodes are considered to have
+// a parent since the Xml document is not a node there, while in TinyXML
+// the TiXmlDocument is a TiXmlNode, so even top-level nodes have a parent.
+bool Xml::Node::isOrphan() const
+{ if (!isValid()) return false; // empty handle not considered an orphan
+ return getTiNode().Parent() == 0; }
+
+void Xml::Node::
+writeToString(String& out, bool compact) const {
+ if (!isValid()) {
+ out = "<!-- EMPTY NODE -->";
+ return;
+ }
+ TiXmlPrinter printer(out);
+ if (compact) printer.SetStreamPrinting();
+ getTiNode().Accept( &printer );
+}
+
+bool Xml::Node::hasParentElement() const
+{ if (!isValid()) return false;
+ const TiXmlNode* parent = getTiNode().Parent();
+ return parent && parent->ToElement(); }
+
+Xml::Element Xml::Node::getParentElement() {
+ SimTK_ERRCHK_ALWAYS(hasParentElement(),
+ "Xml::Node::hasParentElement()",
+ "This node does not have a parent element; it may be a top-level"
+ " node, an orphan, or just an empty Node handle.");
+ return Element(updTiNode().Parent()->ToElement());
+}
+
+
+//------------------------------------------------------------------------------
+// XML NODE ITERATOR
+//------------------------------------------------------------------------------
+Xml::node_iterator& Xml::node_iterator::
+operator++() {
+ TiXmlNode* next = node.updTiNode().NextSibling();
+ while (next && !nodeTypeIsAllowed(allowed, next->Type()))
+ next = next->NextSibling();
+ node = Node(next);
+ return *this;
+}
+
+Xml::node_iterator Xml::node_iterator::
+operator++(int) {
+ Node save(node);
+ TiXmlNode* next = node.updTiNode().NextSibling();
+ while (next && !nodeTypeIsAllowed(allowed, next->Type()))
+ next = next->NextSibling();
+ node = Node(next);
+ return node_iterator(save);
+}
+
+Xml::node_iterator& Xml::node_iterator::
+operator--() {
+ TiXmlNode* prev = node.updTiNode().PreviousSibling();
+ while (prev && !nodeTypeIsAllowed(allowed, prev->Type()))
+ prev = prev->PreviousSibling();
+ node = Node(prev);
+ return *this;
+}
+
+Xml::node_iterator Xml::node_iterator::
+operator--(int) {
+ Node save(node);
+ TiXmlNode* prev = node.updTiNode().PreviousSibling();
+ while (prev && !nodeTypeIsAllowed(allowed, prev->Type()))
+ prev = prev->PreviousSibling();
+ node = Node(prev);
+ return node_iterator(save);
+}
+
+
+
+//------------------------------------------------------------------------------
+// XML ELEMENT
+//------------------------------------------------------------------------------
+
+// Handy helper for weeding out unwanted nodes and elements.
+static bool elementIsAllowed(const String& tag,
+ const TiXmlElement* elt) {
+ if (elt==0) return false;
+ return tag.empty() || elt->ValueStr() == tag;
+}
+
+Xml::Element::Element(const String& tag, const String& value)
+: Node(new TiXmlElement(tag)) {
+ if (value.empty()) return;
+ // We need to add a Text node.
+ updTiElement().LinkEndChild(new TiXmlText(value));
+}
+
+Xml::Element Xml::Element::clone() const {
+ TiXmlElement* newElt = 0;
+ if (getTiElementPtr()) newElt = getTiElementPtr()->Clone()->ToElement();
+ return Element(newElt);
+}
+
+const String& Xml::Element::getElementTag() const
+{ return getTiNode().ValueStr(); }
+void Xml::Element::setElementTag(const String& type)
+{ updTiNode().SetValue(type); }
+
+bool Xml::Element::isValueElement() const {
+ if (unconst().element_begin() != element_end())
+ return false; // has child elements
+ node_iterator text = unconst().node_begin(TextNode);
+ return text == node_end() || ++text == node_end(); // zero or one
+}
+
+const String& Xml::Element::getValue() const {
+ static const String null;
+ SimTK_ERRCHK1_ALWAYS(isValueElement(), "Xml::Element::getValue()",
+ "Element <%s> is not a value element.", getElementTag().c_str());
+
+ node_iterator text = unconst().node_begin(TextNode);
+ return text == node_end() ? null : text->getNodeText();
+}
+
+// Must add a Text node now if this Element doesn't have one.
+String& Xml::Element::updValue() {
+ SimTK_ERRCHK1_ALWAYS(isValueElement(), "Xml::Element::getValue()",
+ "Element <%s> is not a value element.", getElementTag().c_str());
+
+ node_iterator text = node_begin(TextNode);
+ if (text != node_end()) return Text::getAs(*text).updText();
+
+ // We need to add a Text node.
+ TiXmlText* textp = new TiXmlText("");
+ updTiElement().LinkEndChild(textp);
+ return textp->UpdValueStr();
+}
+
+// If there is no Text node we'll add one; if there is just one we'll
+// change its value; otherwise report an error.
+void Xml::Element::setValue(const String& value) {
+ SimTK_ERRCHK1_ALWAYS(isValueElement(), "Xml::Element::setValue()",
+ "Element <%s> is not a value element.", getElementTag().c_str());
+
+ node_iterator text = node_begin(TextNode);
+ if (text == node_end()) updTiNode().LinkEndChild(new TiXmlText(value));
+ else text->updTiNode().SetValue(value);
+}
+
+bool Xml::Element::hasAttribute(const String& name) const
+{ return unconst().getOptionalAttribute(name).isValid(); }
+
+bool Xml::Element::hasElement(const String& tag) const
+{ return unconst().element_begin(tag) != element_end(); }
+
+bool Xml::Element::hasNode(NodeType allowed) const
+{ return unconst().node_begin(allowed) != node_end(); }
+
+Xml::Element Xml::Element::getRequiredElement(const String& tag) {
+ element_iterator p = element_begin(tag);
+ SimTK_ERRCHK2_ALWAYS(p != element_end(),
+ "Xml::Element::getRequiredElement()",
+ "Couldn't find required child element <%s> in element <%s>.",
+ tag.c_str(), getElementTag().c_str());
+ return *p;
+}
+
+Xml::Element Xml::Element::getOptionalElement(const String& tag) {
+ element_iterator p = element_begin(tag);
+ return p != element_end() ? *p : Element(0);
+}
+
+Xml::Attribute Xml::Element::getRequiredAttribute(const String& name) {
+ Attribute attr = getOptionalAttribute(name);
+ SimTK_ERRCHK2_ALWAYS(attr.isValid(),
+ "Xml::Element::getRequiredAttribute()",
+ "Couldn't find required attribute %s in element <%s>.",
+ name.c_str(), getElementTag().c_str());
+ return attr;
+}
+
+Xml::Attribute Xml::Element::getOptionalAttribute(const String& name) {
+ for (attribute_iterator p = attribute_begin();
+ p != attribute_end(); ++p)
+ if (p->getName() == name) return *p;
+ return Attribute(0);
+}
+
+
+void Xml::Element::setAttributeValue(const String& name, const String& value) {
+ SimTK_ERRCHK_ALWAYS(isValid(), "Xml::Element::setAttributeValue()",
+ "Can't add an attribute to an empty Element handle.");
+ updTiElement().SetAttribute(name,value);
+}
+
+
+void Xml::Element::eraseAttribute(const String& name) {
+ SimTK_ERRCHK_ALWAYS(isValid(), "Xml::Element::eraseAttribute()",
+ "Can't erase an attribute from an empty Element handle.");
+ updTiElement().RemoveAttribute(name);
+}
+
+/*static*/ bool Xml::Element::isA(const Xml::Node& node)
+{ if (!node.isValid()) return false;
+ return node.getTiNode().ToElement() != 0; }
+/*static*/const Xml::Element& Xml::Element::getAs(const Node& node)
+{ SimTK_ERRCHK1_ALWAYS(isA(node), "Xml::Element::getAs()",
+ "The given Node was not an Element; it is a %s. Use Element::isA()"
+ " to check before calling Element::getAs().",
+ node.getNodeTypeAsString().c_str());
+ return reinterpret_cast<const Element&>(node); }
+/*static*/Xml::Element& Xml::Element::getAs(Node& node)
+{ SimTK_ERRCHK1_ALWAYS(isA(node), "Xml::Element::getAs()",
+ "The given Node was not an Element; it is a %s. Use Element::isA()"
+ " to check before calling Element::getAs().",
+ node.getNodeTypeAsString().c_str());
+ return reinterpret_cast<Element&>(node); }
+
+void Xml::Element::insertNodeBefore(const node_iterator& beforeThis, Node node) {
+ const char* method = "Xml::Element::insertNodeBefore()";
+ const char* tag = getElementTag().c_str();
+
+ SimTK_ERRCHK1_ALWAYS(node.isValid(), method,
+ "The supplied Node handle was invalid so can't be inserted into"
+ " Element <%s>.", tag);
+ SimTK_ERRCHK1_ALWAYS(!node.hasParentElement(), method,
+ "The supplied Node already had a parent so can't be inserted into"
+ " Element <%s>.", tag);
+
+ if (beforeThis == node_end()) {
+ updTiNode().LinkEndChild(node.updTiNodePtr());
+ return;
+ }
+
+ SimTK_ERRCHK1_ALWAYS(beforeThis->getParentElement() == *this, method,
+ "The supplied node_iterator referred to a node that was not a"
+ "child node of this Element <%s>.", tag);
+
+ TiXmlNode* p = beforeThis->updTiNodePtr();
+ updTiNode().LinkBeforeChild(p, node.updTiNodePtr());
+}
+
+void Xml::Element::insertNodeAfter(const node_iterator& afterThis, Node node) {
+ const char* method = "Xml::Element::insertNodeAfter()";
+ const char* tag = getElementTag().c_str();
+
+ SimTK_ERRCHK1_ALWAYS(node.isValid(), method,
+ "The supplied Node handle was invalid so can't be inserted into"
+ " Element <%s>.", tag);
+ SimTK_ERRCHK1_ALWAYS(!node.hasParentElement(), method,
+ "The supplied Node already had a parent so can't be inserted into"
+ " Element <%s>.", tag);
+
+ if (afterThis == node_end()) {
+ updTiNode().LinkEndChild(node.updTiNodePtr());
+ return;
+ }
+
+ SimTK_ERRCHK1_ALWAYS(afterThis->getParentElement() == *this, method,
+ "The supplied node_iterator referred to a node that was not a"
+ "child node of this Element <%s>.", tag);
+
+ TiXmlNode* p = afterThis->updTiNodePtr();
+ updTiNode().LinkAfterChild(p, node.updTiNodePtr());
+}
+
+void Xml::Element::eraseNode(const Xml::node_iterator& deleteThis) {
+ const char* method = "Xml::Element::eraseNode()";
+
+ // Check that the supplied iterator points to something.
+ SimTK_ERRCHK_ALWAYS(deleteThis != node_end(), method,
+ "The node_iterator is at node_end() so doesn't refer to a Node.");
+ // There is an iterator, make sure it points to a child of this element.
+ SimTK_ERRCHK_ALWAYS(deleteThis->hasParentElement()
+ && deleteThis->getParentElement()==*this, method,
+ "The node_iterator did not refer to a child of this Element.");
+
+ updTiElement().RemoveChild(deleteThis->updTiNodePtr());
+}
+
+Xml::Node Xml::Element::removeNode(const Xml::node_iterator& removeThis) {
+ const char* method = "Xml::Element::removeNode()";
+
+ // Check that the supplied iterator points to something.
+ SimTK_ERRCHK_ALWAYS(removeThis != node_end(), method,
+ "The node_iterator is at node_end() so doesn't refer to a Node.");
+ // There is an iterator, make sure it points to a child of this element.
+ SimTK_ERRCHK_ALWAYS(removeThis->hasParentElement()
+ && removeThis->getParentElement()==*this, method,
+ "The node_iterator did not refer to a child of this Element.");
+
+ TiXmlNode* p = updTiElement().DisconnectChild(removeThis->updTiNodePtr());
+ return Xml::Node(p);
+}
+
+ // Element node_begin()
+Xml::node_iterator Xml::Element::node_begin(NodeType allowed) {
+ TiXmlNode* first = updTiNode().FirstChild();
+ while (first && !nodeTypeIsAllowed(allowed, first->Type()))
+ first = first->NextSibling();
+ return node_iterator(first, allowed);
+}
+
+ // Element node_end()
+Xml::node_iterator Xml::Element::node_end() const
+{ return node_iterator(0); }
+
+ // Element begin()
+Xml::element_iterator Xml::Element::
+element_begin(const String& tag) {
+ TiXmlElement* first = updTiNode().FirstChildElement();
+ while (first && !elementIsAllowed(tag, first))
+ first = first->NextSiblingElement();
+ return element_iterator(first, tag);
+}
+
+ // Element end()
+Xml::element_iterator Xml::Element::element_end() const
+{ return element_iterator(0);}
+
+ // Attribute begin()
+Xml::attribute_iterator Xml::Element::
+attribute_begin() {
+ TiXmlAttribute* first = updTiElement().FirstAttribute();
+ return attribute_iterator(first);
+}
+
+ // Attribute end()
+Xml::attribute_iterator Xml::Element::
+attribute_end() const
+{ return attribute_iterator(0); }
+
+
+
+//------------------------------------------------------------------------------
+// XML ELEMENT ITERATOR
+//------------------------------------------------------------------------------
+Xml::element_iterator& Xml::element_iterator::
+operator++() {
+ TiXmlElement* next = (*this)->updTiElement().NextSiblingElement();
+ while (next && !elementIsAllowed(tag,next))
+ next = next->NextSiblingElement();
+ reassign(next);
+ return *this;
+}
+
+Xml::element_iterator Xml::element_iterator::
+operator++(int) {
+ Element save(*(*this));
+ TiXmlElement* next = (*this)->updTiElement().NextSiblingElement();
+ while (next && !elementIsAllowed(tag,next))
+ next = next->NextSiblingElement();
+ reassign(next);
+ return element_iterator(save);
+}
+
+Xml::element_iterator& Xml::element_iterator::
+operator--() {
+ TiXmlElement* prev = (*this)->updTiElement().PreviousSiblingElement();
+ while (prev && !elementIsAllowed(tag,prev))
+ prev = prev->PreviousSiblingElement();
+ reassign(prev);
+ return *this;
+}
+
+Xml::element_iterator Xml::element_iterator::
+operator--(int) {
+ Element save(*(*this));
+ TiXmlElement* prev = (*this)->updTiElement().PreviousSiblingElement();
+ while (prev && !elementIsAllowed(tag,prev))
+ prev = prev->PreviousSiblingElement();
+ reassign(prev);
+ return element_iterator(save);
+}
+
+
+
+
+//------------------------------------------------------------------------------
+// XML TEXT NODE
+//------------------------------------------------------------------------------
+Xml::Text::Text(const String& text) : Node(new TiXmlText(text)) {}
+
+
+Xml::Text Xml::Text::clone() const {
+ TiXmlText* newText = 0;
+ if (getTiNodePtr()) newText = getTiNodePtr()->Clone()->ToText();
+ return Xml::Text(newText);
+}
+
+const String& Xml::Text::getText() const
+{ return getTiNode().ValueStr(); }
+String& Xml::Text::updText()
+{ return updTiNode().UpdValueStr(); }
+
+/*static*/ bool Xml::Text::isA(const Xml::Node& node)
+{ if (!node.isValid()) return false;
+ return node.getTiNode().ToText() != 0; }
+/*static*/const Xml::Text& Xml::Text::getAs(const Node& node)
+{ SimTK_ERRCHK1_ALWAYS(isA(node), "Xml::Text::getAs()",
+ "The given Node was not a Text node; it is a %s. Use Text::isA()"
+ " to check before calling Text::getAs().",
+ node.getNodeTypeAsString().c_str());
+ return reinterpret_cast<const Text&>(node); }
+/*static*/Xml::Text& Xml::Text::getAs(Node& node)
+{ SimTK_ERRCHK1_ALWAYS(isA(node), "Xml::Text::getAs()",
+ "The given Node was not a Text node; it is a %s. Use Text::isA()"
+ " to check before calling Text::getAs().",
+ node.getNodeTypeAsString().c_str());
+ return reinterpret_cast<Text&>(node); }
+
+
+
+//------------------------------------------------------------------------------
+// XML COMMENT NODE
+//------------------------------------------------------------------------------
+Xml::Comment::Comment(const String& text) : Node(new TiXmlComment(text)) {}
+
+Xml::Comment Xml::Comment::clone() const {
+ TiXmlComment* newComment = 0;
+ if (getTiNodePtr()) newComment = getTiNodePtr()->Clone()->ToComment();
+ return Xml::Comment(newComment);
+}
+
+/*static*/ bool Xml::Comment::isA(const Xml::Node& node)
+{ if (!node.isValid()) return false;
+ return node.getTiNode().ToComment() != 0; }
+/*static*/const Xml::Comment& Xml::Comment::getAs(const Node& node)
+{ SimTK_ERRCHK1_ALWAYS(isA(node), "Xml::Comment::getAs()",
+ "The given Node was not a Comment node; it is a %s. Use Comment::isA()"
+ " to check before calling Comment::getAs().",
+ node.getNodeTypeAsString().c_str());
+ return reinterpret_cast<const Comment&>(node); }
+/*static*/Xml::Comment& Xml::Comment::getAs(Node& node)
+{ SimTK_ERRCHK1_ALWAYS(isA(node), "Xml::Comment::getAs()",
+ "The given Node was not a Comment node; it is a %s. Use Comment::isA()"
+ " to check before calling Comment::getAs().",
+ node.getNodeTypeAsString().c_str());
+ return reinterpret_cast<Comment&>(node); }
+
+
+
+//------------------------------------------------------------------------------
+// XML UNKNOWN NODE
+//------------------------------------------------------------------------------
+Xml::Unknown::Unknown(const String& contents) : Node(new TiXmlUnknown())
+{ updTiNode().SetValue(contents); }
+
+Xml::Unknown Xml::Unknown::clone() const {
+ TiXmlUnknown* newUnknown = 0;
+ if (getTiNodePtr()) newUnknown = getTiNodePtr()->Clone()->ToUnknown();
+ return Xml::Unknown(newUnknown);
+}
+
+/*static*/ bool Xml::Unknown::isA(const Xml::Node& node)
+{ if (!node.isValid()) return false;
+ return node.getTiNode().ToUnknown() != 0; }
+/*static*/const Xml::Unknown& Xml::Unknown::getAs(const Node& node)
+{ SimTK_ERRCHK1_ALWAYS(isA(node), "Xml::Unknown::getAs()",
+ "The given Node was not an Unknown node; it is a %s. Use Unknown::isA()"
+ " to check before calling Unknown::getAs().",
+ node.getNodeTypeAsString().c_str());
+ return reinterpret_cast<const Unknown&>(node); }
+/*static*/Xml::Unknown& Xml::Unknown::getAs(Node& node)
+{ SimTK_ERRCHK1_ALWAYS(isA(node), "Xml::Unknown::getAs()",
+ "The given Node was not an Unknown node; it is a %s. Use Unknown::isA()"
+ " to check before calling Unknown::getAs().",
+ node.getNodeTypeAsString().c_str());
+ return reinterpret_cast<Unknown&>(node); }
+
diff --git a/SimTKcommon/src/gmx_atomic.h b/SimTKcommon/src/gmx_atomic.h
new file mode 100644
index 0000000..318796b
--- /dev/null
+++ b/SimTKcommon/src/gmx_atomic.h
@@ -0,0 +1,1596 @@
+/* -*- mode: c; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*-
+*
+* Copyright (c) 2004-2008, Erik Lindahl <lindahl at cbr.su.se>
+*
+* Unfortunately, some of the constructs in this file are _very_ sensitive
+* to compiler optimizations and architecture changes. If you find any such
+* errors, please send a message to lindahl at cbr.su.se to help us fix the
+* upstream version too.
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy
+* of this software and associated documentation files (the "Software"), to deal
+* in the Software without restriction, including without limitation the rights
+* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+* copies of the Software, and to permit persons to whom the Software is
+* furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in
+* all copies or substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+* THE SOFTWARE.
+*
+* And Hey:
+* Gnomes, ROck Monsters And Chili Sauce
+*/
+#ifndef _GMX_ATOMIC_H_
+#define _GMX_ATOMIC_H_
+
+/*! \file gmx_atomic.h
+ *
+ * @brief Atomic operations for fast SMP synchronization
+ *
+ * This file defines atomic integer operations and spinlocks for
+ * fast synchronization in performance-critical regions of gromacs.
+ *
+ * In general, the best option is to use functions without explicit
+ * locking, e.g. gmx_atomic_fetch_add() or gmx_atomic_cmpxchg().
+ *
+ * Not all architecture support atomic operations though inline assembly,
+ * and even if they do it might not be implemented here. In that case
+ * we use a fallback mutex implementation, so you can always count on
+ * the function interfaces working in Gromacs.
+ *
+ * Don't use spinlocks in non-performance-critical regions like file I/O.
+ * Since they always spin busy they would waste CPU cycles instead of
+ * properly yielding to a computation thread while waiting for the disk.
+ *
+ * Finally, note that all our spinlock operations are defined to return
+ * 0 if initialization or locking completes successfully.
+ * This is the opposite of some other implementations, but the same standard
+ * as used for pthread mutexes. So, if e.g. are trying to lock a spinlock,
+ * you will have gotten the lock if the return value is 0.
+ *
+ * gmx_spinlock_islocked(x) obviously still returns 1 if the lock is locked,
+ * and 0 if it is available, though...
+ */
+
+
+
+#include <stdio.h>
+
+#include <pthread.h>
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+#if 0
+} /* Avoids screwing up auto-indentation */
+#endif
+
+
+
+
+#if ( ( (defined(__GNUC__) || defined(__INTEL_COMPILER) || defined(__PATHSCALE__)) && \
+ (defined(i386) || defined(__x86_64__)) ) \
+ || defined (DOXYGEN) )
+
+/* This code is executed for x86 and x86-64, with these compilers:
+ * GNU
+ * Intel
+ * Pathscale
+ * All these support GCC-style inline assembly.
+ * We also use this section for the documentation.
+ */
+
+/*! \brief Memory barrier operation
+ *
+ * Modern CPUs rely heavily on out-of-order execution, and one common feature
+ * is that load/stores might be reordered. Also, when using inline assembly
+ * the compiler might already have loaded the variable we are changing into
+ * a register, so any update to memory won't be visible.
+ *
+ * This command creates a memory barrier, i.e. all memory results before
+ * it in the code should be visible to all memory operations after it - the
+ * CPU cannot propagate load/stores across it.
+ */
+#define gmx_atomic_memory_barrier() __asm__ __volatile__("": : :"memory")
+
+/* Only gcc and Intel support this check, otherwise set it to true (skip doc) */
+#if (!defined(__GNUC__) && !defined(__INTEL_COMPILER) && !defined DOXYGEN)
+#define __builtin_constant_p(i) (1)
+#endif
+
+
+/*! \brief Gromacs atomic operations datatype
+ *
+ * Portable synchronization primitives like mutexes are effective for
+ * many purposes, but usually not very high performance.
+ * One of the problem is that you have the overhead of a function call,
+ * and another is that Mutexes often have extra overhead to make the
+ * scheduling fair. Finally, if performance is important we don't want
+ * to suspend the thread if we cannot lock a mutex, but spin-lock at 100%
+ * CPU usage until the resources is available (e.g. increment a counter).
+ *
+ * These things can often be implemented with inline-assembly or other
+ * system-dependent functions, and we provide such functionality for the
+ * most common platforms. For portability we also have a fallback
+ * implementation using a mutex for locking.
+ *
+ * Performance-wise, the fastest solution is always to avoid locking
+ * completely (obvious, but remember it!). If you cannot do that, the
+ * next best thing is to use atomic operations that e.g. increment a
+ * counter without explicit locking. Spinlocks are useful to lock an
+ * entire region, but leads to more overhead and can be difficult to
+ * debug - it is up to you to make sure that only the thread owning the
+ * lock unlocks it!
+ *
+ * You should normally NOT use atomic operations for things like
+ * I/O threads. These should yield to other threads while waiting for
+ * the disk instead of spinning at 100% CPU usage.
+ *
+ * It is imperative that you use the provided routines for reading
+ * and writing, since some implementations require memory barriers before
+ * the CPU or memory sees an updated result. The structure contents is
+ * only visible here so it can be inlined for performance - it might
+ * change without further notice.
+ *
+ * \note No initialization is required for atomic variables.
+ *
+ * Currently, we have (real) atomic operations for:
+ *
+ * - x86 or x86_64, using GNU compilers
+ * - x86 or x86_64, using Intel compilers
+ * - x86 or x86_64, using Pathscale compilers
+ * - Itanium, using GNU compilers
+ * - Itanium, using Intel compilers
+ * - Itanium, using HP compilers
+ * - PowerPC, using GNU compilers
+ * - PowerPC, using IBM AIX compilers
+ * - PowerPC, using IBM compilers >=7.0 under Linux or Mac OS X.
+ */
+typedef struct gmx_atomic
+{
+ volatile int value; /*!< Volatile, to avoid compiler aliasing */
+}
+gmx_atomic_t;
+
+
+
+/*! \brief Gromacs spinlock
+ *
+ * Spinlocks provide a faster synchronization than mutexes,
+ * although they consume CPU-cycles while waiting. They are implemented
+ * with atomic operations and inline assembly whenever possible, and
+ * otherwise we use a fallback implementation where a spinlock is identical
+ * to a mutex (this is one of the reasons why you have to initialize them).
+ *
+ * There are no guarantees whatsoever about fair scheduling or
+ * debugging if you make a mistake and unlock a variable somebody
+ * else has locked - performance is the primary goal of spinlocks.
+ *
+ */
+typedef struct gmx_spinlock
+{
+ volatile unsigned int lock; /*!< Volatile, to avoid compiler aliasing */
+}
+gmx_spinlock_t;
+
+
+
+
+
+/*! \brief Spinlock static initializer
+ *
+ * This is used for static spinlock initialization, and has the same
+ * properties as GMX_THREAD_MUTEX_INITIALIZER has for mutexes.
+ * This is only for inlining in the gmx_thread.h header file. Whether
+ * it is 0, 1, or something else when unlocked depends on the platform.
+ * Don't assume anything about it. It might even be a mutex when using the
+ * fallback implementation!
+ */
+#define GMX_SPINLOCK_INITIALIZER { 1 }
+
+
+
+/*! \brief Return value of an atomic integer
+ *
+ * Also implements proper memory barriers when necessary.
+ * The actual implementation is system-dependent.
+ *
+ * \param a Atomic variable to read
+ * \return Integer value of the atomic variable
+ */
+#define gmx_atomic_read(a) ((a)->value)
+
+
+/*! \brief Write value to an atomic integer
+ *
+ * Also implements proper memory barriers when necessary.
+ * The actual implementation is system-dependent.
+ *
+ * \param a Atomic variable
+ * \param i Integer to set the atomic variable to.
+ */
+#define gmx_atomic_set(a,i) (((a)->value) = (i))
+
+
+/*! \brief Add integer to atomic variable
+ *
+ * Also implements proper memory barriers when necessary.
+ * The actual implementation is system-dependent.
+ *
+ * \param a atomic datatype to modify
+ * \param i integer to increment with. Use i<0 to subtract atomically.
+ *
+ * \return The new value (after summation).
+ */
+static inline int
+gmx_atomic_add_return(gmx_atomic_t * a,
+ volatile int i)
+{
+ int __i;
+
+ __i = i;
+ __asm__ __volatile__("lock ; xaddl %0, %1;"
+ :"=r"(i) :"m"(a->value), "0"(i));
+ return i + __i;
+}
+
+
+/*! \brief Add to variable, return the old value.
+ *
+ * This operation is quite useful for synchronization counters.
+ * By performing a fetchadd with N, a thread can e.g. reserve a chunk
+ * with the next N iterations, and the return value is the index
+ * of the first element to treat.
+ *
+ * Also implements proper memory barriers when necessary.
+ * The actual implementation is system-dependent.
+ *
+ * \param a atomic datatype to modify
+ * \param i integer to increment with. Use i<0 to subtract atomically.
+ *
+ * \return The value of the atomic variable before addition.
+ */
+static inline int
+gmx_atomic_fetch_add(gmx_atomic_t * a,
+ volatile int i)
+{
+ int __i;
+
+ __i = i;
+ __asm__ __volatile__("lock ; xaddl %0, %1;"
+ :"=r"(i) :"m"(a->value), "0"(i));
+ return i;
+}
+
+
+/*! \brief Atomic compare-exchange operation
+ *
+ * The \a old value is compared with the memory value in the atomic datatype.
+ * If the are identical, the atomic type is updated to the new value,
+ * and otherwise left unchanged.
+ *
+ * This is a very useful synchronization primitive: You can start by reading
+ * a value (without locking anything), perform some calculations, and then
+ * atomically try to update it in memory unless it has changed. If it has
+ * changed you will get an error return code - reread the new value
+ * an repeat the calculations in that case.
+ *
+ * \param a Atomic datatype ('memory' value)
+ * \param oldval Integer value read from the atomic type at an earlier point
+ * \param newval New value to write to the atomic type if it currently is
+ * identical to the old value.
+ *
+ * \return The value of the atomic memory variable in memory when this
+ * instruction was executed. This, if the operation succeeded the
+ * return value was identical to the \a old parameter, and if not
+ * it returns the updated value in memory so you can repeat your
+ * operations on it.
+ *
+ * \note The exchange occured if the return value is identical to \a old.
+ */
+static inline int
+gmx_atomic_cmpxchg(gmx_atomic_t * a,
+ int oldval,
+ int newval)
+{
+ volatile unsigned long prev;
+
+ __asm__ __volatile__("lock ; cmpxchgl %1,%2"
+ : "=a"(prev)
+ : "q"(newval), "m"(a->value), "0"(oldval)
+ : "memory");
+
+ return prev;
+}
+
+
+/*! \brief Initialize spinlock
+ *
+ * In theory you can call this from multiple threads, but remember
+ * that we don't check for errors. If the first thread proceeded to
+ * lock the spinlock after initialization, the second will happily
+ * overwrite the contents and unlock it without warning you.
+ *
+ * \param x Gromacs spinlock pointer.
+ */
+static inline void
+gmx_spinlock_init(gmx_spinlock_t * x)
+{
+ x->lock = 1;
+}
+
+
+
+/*! \brief Acquire spinlock
+ *
+ * This routine blocks until the spinlock is available, and
+ * the locks it again before returning.
+ *
+ * \param x Gromacs spinlock pointer
+ */
+static inline void
+gmx_spinlock_lock(gmx_spinlock_t * x)
+{
+ __asm__ __volatile__("\n1:\t"
+ "lock ; decb %0\n\t"
+ "jns 3f\n"
+ "2:\t"
+ "rep;nop\n\t"
+ "cmpb $0,%0\n\t"
+ "jle 2b\n\t"
+ "jmp 1b\n"
+ "3:\n\t"
+ :"=m" (x->lock) : : "memory");
+}
+
+
+/*! \brief Attempt to acquire spinlock
+ *
+ * This routine acquires the spinlock if possible, but if
+ * already locked it return an error code immediately.
+ *
+ * \param x Gromacs spinlock pointer
+ *
+ * \return 0 if the mutex was available so we could lock it,
+ * otherwise a non-zero integer (1) if the lock is busy.
+ */
+static inline int
+gmx_spinlock_trylock(gmx_spinlock_t * x)
+{
+ char old_value;
+
+ __asm__ __volatile__("xchgb %b0,%1"
+ :"=q" (old_value), "=m" (x->lock)
+ :"0" (0) : "memory");
+ return (old_value <= 0);
+}
+
+
+/*! \brief Release spinlock
+ *
+ * \param x Gromacs spinlock pointer
+ *
+ * Unlocks the spinlock, regardless if which thread locked it.
+ */
+static inline void
+gmx_spinlock_unlock(gmx_spinlock_t * x)
+{
+ char old_value = 1;
+
+ __asm__ __volatile__(
+ "xchgb %b0, %1"
+ :"=q" (old_value), "=m" (x->lock)
+ :"0" (old_value) : "memory"
+ );
+}
+
+
+/*! \brief Check if spinlock is locked
+ *
+ * This routine returns immediately with the lock status.
+ *
+ * \param x Gromacs spinlock pointer
+ *
+ * \return 1 if the spinlock is locked, 0 otherwise.
+ */
+static inline int
+gmx_spinlock_islocked(gmx_spinlock_t * x)
+{
+ return (*(volatile signed char *)(&(x)->lock) <= 0);
+}
+
+
+/*! \brief Wait for a spinlock to become available
+ *
+ * This routine blocks until the spinlock is unlocked,
+ * but in contrast to gmx_spinlock_lock() it returns without
+ * trying to lock the spinlock.
+ *
+ * \param x Gromacs spinlock pointer
+ */
+static inline void
+gmx_spinlock_wait(gmx_spinlock_t * x)
+{
+ do
+ {
+ gmx_atomic_memory_barrier();
+ }
+ while(gmx_spinlock_islocked(x));
+}
+
+
+#elif ( defined(__GNUC__) && (defined(__powerpc__) || defined(__ppc__)))
+/* PowerPC using proper GCC inline assembly.
+ * Recent versions of xlC (>=7.0) _partially_ support this, but since it is
+ * not 100% compatible we provide a separate implementation for xlC in
+ * the next section.
+ */
+
+/* Compiler-dependent stuff: GCC memory barrier */
+#define gmx_atomic_memory_barrier() __asm__ __volatile__("": : :"memory")
+
+
+
+typedef struct gmx_atomic
+{
+ volatile int value; /*!< Volatile, to avoid compiler aliasing */
+}
+gmx_atomic_t;
+
+
+typedef struct gmx_spinlock
+{
+ volatile unsigned int lock; /*!< Volatile, to avoid compiler aliasing */
+}
+gmx_spinlock_t;
+
+
+#define GMX_SPINLOCK_INITIALIZER { 0 }
+
+
+#define gmx_atomic_read(a) ((a)->value)
+#define gmx_atomic_set(a,i) (((a)->value) = (i))
+
+
+static inline int
+gmx_atomic_add_return(gmx_atomic_t * a,
+ int i)
+{
+ int t;
+
+ __asm__ __volatile__("1: lwarx %0,0,%2\n"
+ "\tadd %0,%1,%0\n"
+ "\tstwcx. %0,0,%2 \n"
+ "\tbne- 1b"
+ "\tisync\n"
+ : "=&r" (t)
+ : "r" (i), "r" (&a->value)
+ : "cc" , "memory");
+ return t;
+}
+
+
+
+static inline int
+gmx_atomic_fetch_add(gmx_atomic_t * a,
+ int i)
+{
+ int t;
+
+ __asm__ __volatile__("\teieio\n"
+ "1: lwarx %0,0,%2\n"
+ "\tadd %0,%1,%0\n"
+ "\tstwcx. %0,0,%2 \n"
+ "\tbne- 1b\n"
+ "\tisync\n"
+ : "=&r" (t)
+ : "r" (i), "r" (&a->value)
+ : "cc", "memory");
+
+ return (t - i);
+}
+
+
+static inline int
+gmx_atomic_cmpxchg(gmx_atomic_t * a,
+ int oldval,
+ int newval)
+{
+ int prev;
+
+ __asm__ __volatile__ ("1: lwarx %0,0,%2 \n"
+ "\tcmpw 0,%0,%3 \n"
+ "\tbne 2f \n"
+ "\tstwcx. %4,0,%2 \n"
+ "bne- 1b\n"
+ "\tsync\n"
+ "2:\n"
+ : "=&r" (prev), "=m" (a->value)
+ : "r" (&a->value), "r" (oldval), "r" (newval), "m" (a->value)
+ : "cc", "memory");
+
+ return prev;
+}
+
+static inline void
+gmx_spinlock_init(gmx_spinlock_t *x)
+{
+ x->lock = 0;
+}
+
+
+
+static inline void
+gmx_spinlock_lock(gmx_spinlock_t * x)
+{
+ unsigned int tmp;
+
+ __asm__ __volatile__("\tb 1f\n"
+ "2: lwzx %0,0,%1\n"
+ "\tcmpwi 0,%0,0\n"
+ "\tbne+ 2b\n"
+ "1: lwarx %0,0,%1\n"
+ "\tcmpwi 0,%0,0\n"
+ "\tbne- 2b\n"
+ "\tstwcx. %2,0,%1\n"
+ "bne- 2b\n"
+ "\tisync\n"
+ : "=&r"(tmp)
+ : "r"(&x->lock), "r"(1)
+ : "cr0", "memory");
+}
+
+
+static inline int
+gmx_spinlock_trylock(gmx_spinlock_t * x)
+{
+ unsigned int old, t;
+ unsigned int mask = 1;
+ volatile unsigned int *p = &x->lock;
+
+ __asm__ __volatile__("\teieio\n"
+ "1: lwarx %0,0,%4 \n"
+ "\tor %1,%0,%3 \n"
+ "\tstwcx. %1,0,%4 \n"
+ "\tbne 1b\n"
+ "\tsync\n"
+ : "=&r" (old), "=&r" (t), "=m" (*p)
+ : "r" (mask), "r" (p), "m" (*p)
+ : "cc", "memory");
+
+ return ((old & mask) != 0);
+}
+
+
+static inline void
+gmx_spinlock_unlock(gmx_spinlock_t * x)
+{
+ __asm__ __volatile__("\teieio\n": : :"memory");
+ x->lock = 0;
+}
+
+
+static inline int
+gmx_spinlock_islocked(gmx_spinlock_t * x)
+{
+ return ( x->lock != 0);
+}
+
+
+static inline void
+gmx_spinlock_wait(gmx_spinlock_t *x)
+{
+ do
+ {
+ gmx_atomic_memory_barrier();
+ }
+ while(gmx_spinlock_islocked(x));
+}
+
+
+
+#elif ( (defined(__IBM_GCC_ASM) || defined(__IBM_STDCPP_ASM)) && \
+ (defined(__powerpc__) || defined(__ppc__)))
+/* PowerPC using xlC inline assembly.
+ * Recent versions of xlC (>=7.0) _partially_ support GCC inline assembly
+ * if you use the option -qasm=gcc but we have had to hack things a bit, in
+ * particular when it comes to clobbered variables. Since this implementation
+ * _could_ be buggy, we have separated it from the known-to-be-working gcc
+ * one above.
+ */
+
+/* memory barrier - no idea how to create one with xlc! */
+#define gmx_atomic_memory_barrier()
+
+
+
+typedef struct gmx_atomic
+{
+ volatile int value; /*!< Volatile, to avoid compiler aliasing */
+}
+gmx_atomic_t;
+
+
+typedef struct gmx_spinlock
+{
+ volatile unsigned int lock; /*!< Volatile, to avoid compiler aliasing */
+}
+gmx_spinlock_t;
+
+
+#define GMX_SPINLOCK_INITIALIZER { 0 }
+
+
+#define gmx_atomic_read(a) ((a)->value)
+#define gmx_atomic_set(a,i) (((a)->value) = (i))
+
+
+static inline int
+gmx_atomic_add_return(gmx_atomic_t * a,
+ int i)
+{
+ int t;
+
+ __asm__ __volatile__("1: lwarx %0,0,%2 \n"
+ "\t add %0,%1,%0 \n"
+ "\t stwcx. %0,0,%2 \n"
+ "\t bne- 1b \n"
+ "\t isync \n"
+ : "=&r" (t)
+ : "r" (i), "r" (&a->value) );
+ return t;
+}
+
+
+
+static inline int
+gmx_atomic_fetch_add(gmx_atomic_t * a,
+ int i)
+{
+ int t;
+
+ __asm__ __volatile__("\t eieio\n"
+ "1: lwarx %0,0,%2 \n"
+ "\t add %0,%1,%0 \n"
+ "\t stwcx. %0,0,%2 \n"
+ "\t bne- 1b \n"
+ "\t isync \n"
+ : "=&r" (t)
+ : "r" (i), "r" (&a->value));
+
+ return (t - i);
+}
+
+
+static inline int
+gmx_atomic_cmpxchg(gmx_atomic_t * a,
+ int oldval,
+ int newval)
+{
+ int prev;
+
+ __asm__ __volatile__ ("1: lwarx %0,0,%2 \n"
+ "\t cmpw 0,%0,%3 \n"
+ "\t bne 2f \n"
+ "\t stwcx. %4,0,%2 \n"
+ "\t bne- 1b \n"
+ "\t sync \n"
+ "2: \n"
+ : "=&r" (prev), "=m" (a->value)
+ : "r" (&a->value), "r" (oldval), "r" (newval), "m" (a->value));
+
+ return prev;
+}
+
+static inline void
+gmx_spinlock_init(gmx_spinlock_t *x)
+{
+ x->lock = 0;
+}
+
+
+
+static inline void
+gmx_spinlock_lock(gmx_spinlock_t * x)
+{
+ unsigned int tmp;
+
+ __asm__ __volatile__("\t b 1f \n"
+ "2: lwzx %0,0,%1 \n"
+ "\t cmpwi 0,%0,0 \n"
+ "\t bne+ 2b \n"
+ "1: lwarx %0,0,%1 \n"
+ "\t cmpwi 0,%0,0 \n"
+ "\t bne- 2b \n"
+ "\t stwcx. %2,0,%1 \n"
+ "\t bne- 2b \n"
+ "\t isync\n"
+ : "=&r"(tmp)
+ : "r"(&x->lock), "r"(1));
+}
+
+
+static inline int
+gmx_spinlock_trylock(gmx_spinlock_t * x)
+{
+ unsigned int old, t;
+ unsigned int mask = 1;
+ volatile unsigned int *p = &x->lock;
+
+ __asm__ __volatile__("\t eieio\n"
+ "1: lwarx %0,0,%4 \n"
+ "\t or %1,%0,%3 \n"
+ "\t stwcx. %1,0,%4 \n"
+ "\t bne 1b \n"
+ "\t sync \n"
+ : "=&r" (old), "=&r" (t), "=m" (*p)
+ : "r" (mask), "r" (p), "m" (*p));
+
+ return ((old & mask) != 0);
+}
+
+
+static inline void
+gmx_spinlock_unlock(gmx_spinlock_t * x)
+{
+ __asm__ __volatile__("\t eieio \n");
+ x->lock = 0;
+}
+
+
+static inline void
+gmx_spinlock_islocked(gmx_spinlock_t * x)
+{
+ return ( x->lock != 0);
+}
+
+
+static inline void
+gmx_spinlock_wait(gmx_spinlock_t * x)
+{
+
+ do
+ {
+ gmx_atomic_memory_barrier();
+ }
+ while(gmx_spinlock_islocked(x));
+}
+
+
+
+
+#elif (defined(__ia64__) && (defined(__GNUC__) || defined(__INTEL_COMPILER)))
+/* ia64 with GCC or Intel compilers. Since we need to define everything through
+* cmpxchg and fetchadd on ia64, we merge the different compilers and only provide
+* different implementations for that single function.
+* Documentation? Check the gcc/x86 section.
+*/
+
+
+typedef struct gmx_atomic
+{
+ volatile int value; /*!< Volatile, to avoid compiler aliasing */
+}
+gmx_atomic_t;
+
+
+typedef struct gmx_spinlock
+{
+ volatile unsigned int lock; /*!< Volatile, to avoid compiler aliasing */
+}
+gmx_spinlock_t;
+
+
+#define GMX_SPINLOCK_INITIALIZER { 0 }
+
+
+#define gmx_atomic_read(a) ((a)->value)
+#define gmx_atomic_set(a,i) (((a)->value) = (i))
+
+
+
+/* Compiler thingies */
+#ifdef __INTEL_COMPILER
+void __memory_barrier(void);
+int _InterlockedCompareExchange(volatile int *dest, int xchg, int comp);
+unsigned __int64 __fetchadd4_rel(unsigned int *addend, const int increment);
+/* ia64 memory barrier */
+# define gmx_atomic_memory_barrier() __memory_barrier()
+/* ia64 cmpxchg */
+# define gmx_atomic_cmpxchg(a, oldval, newval) _InterlockedCompareExchange(&a->value,newval,oldval)
+/* ia64 fetchadd, but it only works with increments +/- 1,4,8,16 */
+# define gmx_ia64_fetchadd(a, inc) __fetchadd4_rel(a, inc)
+
+#elif defined __GNUC__
+/* ia64 memory barrier */
+# define gmx_atomic_memory_barrier() asm volatile ("":::"memory")
+/* ia64 cmpxchg */
+static inline int
+gmx_atomic_cmpxchg(gmx_atomic_t * a,
+ int oldval,
+ int newval)
+{
+ volatile int res;
+ asm volatile ("mov ar.ccv=%0;;" :: "rO"(oldval));
+ asm volatile ("cmpxchg4.acq %0=[%1],%2,ar.ccv":
+ "=r"(res) : "r"(&a->value), "r"(newval) : "memory");
+
+ return res;
+}
+
+
+/* fetchadd, but on ia64 it only works with increments +/- 1,4,8,16 */
+#define gmx_ia64_fetchadd(a, inc) \
+({ unsigned long res; \
+ asm volatile ("fetchadd4.rel %0=[%1],%2" \
+ : "=r"(res) : "r"(a), "i" (inc) : "memory"); \
+ res; \
+})
+
+
+#else /* Unknown compiler */
+# error Unknown ia64 compiler (not GCC or ICC) - modify gmx_thread.h!
+#endif
+
+
+
+static inline int
+gmx_atomic_add_return(gmx_atomic_t * a,
+ volatile int i)
+{
+ volatile int oldval,newval;
+ volatile int __i = i;
+
+ /* Use fetchadd if, and only if, the increment value can be determined
+ * at compile time (otherwise this check is optimized away) and it is
+ * a value supported by fetchadd (1,4,8,16,-1,-4,-8,-16).
+ */
+ if (__builtin_constant_p(i) &&
+ ( (__i == 1) || (__i == 4) || (__i == 8) || (__i == 16) ||
+ (__i == -1) || (__i == -4) || (__i == -8) || (__i == -16) ) )
+ {
+ oldval = gmx_ia64_fetchadd(a,__i);
+ newval = oldval + i;
+ }
+ else
+ {
+ /* Use compare-exchange addition that works with any value */
+ do
+ {
+ oldval = gmx_atomic_read(a);
+ newval = oldval + i;
+ }
+ while(gmx_atomic_cmpxchg(a,oldval,newval) != oldval);
+ }
+ return newval;
+}
+
+
+
+static inline int
+gmx_atomic_fetch_add(gmx_atomic_t * a,
+ volatile int i)
+{
+ volatile int oldval,newval;
+ volatile int __i = i;
+
+ /* Use ia64 fetchadd if, and only if, the increment value can be determined
+ * at compile time (otherwise this check is optimized away) and it is
+ * a value supported by fetchadd (1,4,8,16,-1,-4,-8,-16).
+ */
+ if (__builtin_constant_p(i) &&
+ ( (__i == 1) || (__i == 4) || (__i == 8) || (__i == 16) ||
+ (__i == -1) || (__i == -4) || (__i == -8) || (__i == -16) ) )
+ {
+ oldval = gmx_ia64_fetchadd(a,__i);
+ newval = oldval + i;
+ }
+ else
+ {
+ /* Use compare-exchange addition that works with any value */
+ do
+ {
+ oldval = gmx_atomic_read(a);
+ newval = oldval + i;
+ }
+ while(gmx_atomic_cmpxchg(a,oldval,newval) != oldval);
+ }
+ return oldval;
+}
+
+
+static inline void
+gmx_spinlock_init(gmx_spinlock_t *x)
+{
+ x->lock = 0;
+}
+
+
+static inline void
+gmx_spinlock_lock(gmx_spinlock_t * x)
+{
+ gmx_atomic_t *a = (gmx_atomic_t *) x;
+ unsigned long value;
+ value = gmx_atomic_cmpxchg(a, 0, 1);
+ if (value)
+ {
+ do
+ {
+ while (a->value != 0)
+ {
+ gmx_atomic_memory_barrier();
+ }
+ value = gmx_atomic_cmpxchg(a, 0, 1);
+ }
+ while (value);
+ }
+}
+
+
+static inline int
+gmx_spinlock_trylock(gmx_spinlock_t * x)
+{
+ return (gmx_atomic_cmpxchg((gmx_atomic_t *)x, 0, 1) != 0);
+}
+
+
+static inline void
+gmx_spinlock_unlock(gmx_spinlock_t * x)
+{
+ do
+ {
+ gmx_atomic_memory_barrier();
+ x->lock = 0;
+ }
+ while (0);
+}
+
+
+static inline int
+gmx_spinlock_islocked(gmx_spinlock_t * x)
+{
+ return (x->lock != 0);
+}
+
+
+static inline void
+gmx_spinlock_wait(gmx_spinlock_t * x)
+{
+
+ do
+ {
+ gmx_atomic_memory_barrier();
+ }
+ while(gmx_spinlock_islocked(x));
+}
+
+
+#undef gmx_ia64_fetchadd
+
+
+
+#elif (defined(__hpux) || defined(__HP_cc)) && defined(__ia64)
+/* HP compiler on ia64 */
+#include <machine/sys/inline.h>
+
+#define gmx_atomic_memory_barrier() _Asm_mf()
+
+#define gmx_hpia64_fetchadd(a, i) \
+ _Asm_fetchadd((_Asm_fasz)_FASZ_W,(_Asm_sem)_SEM_REL, \
+ (UInt32*)a,(unsigned int) i, \
+ (_Asm_ldhint)LDHINT_NONE)
+
+
+typedef struct gmx_atomic
+{
+ volatile int value; /*!< Volatile, to avoid compiler aliasing */
+}
+gmx_atomic_t;
+
+
+typedef struct gmx_spinlock
+{
+ volatile unsigned int lock; /*!< Volatile, to avoid compiler aliasing */
+}
+gmx_spinlock_t;
+
+
+static inline int
+gmx_atomic_cmpxchg(gmx_atomic_t * a,
+ int oldval,
+ int newval)
+{
+ int ret;
+
+ _Asm_mov_to_ar((_Asm_app_reg)_AREG_CCV,(Uint32)oldval,
+ (_Asm_fence)(_UP_CALL_FENCE | _UP_SYS_FENCE |
+ _DOWN_CALL_FENCE | _DOWN_SYS_FENCE));
+
+ ret = _Asm_cmpxchg((_Asm_sz)SZ_W,(_Asm_sem)_SEM_ACQ,(Uint32*)a,
+ (Uint32)newval,(_Asm_ldhint)_LDHINT_NONE);
+
+ return ret;
+}
+
+
+
+#define GMX_SPINLOCK_INITIALIZER { 0 }
+
+
+#define gmx_atomic_read(a) ((a)->value)
+#define gmx_atomic_set(a,i) (((a)->value) = (i))
+
+
+static inline void
+gmx_atomic_add_return(gmx_atomic_t * a,
+ int i)
+{
+ int old,new;
+ int __i = i;
+
+ /* On HP-UX we don't know any macro to determine whether the increment
+ * is known at compile time, but hopefully the call uses something simple
+ * like a constant, and then the optimizer should be able to do the job.
+ */
+ if ( (__i == 1) || (__i == 4) || (__i == 8) || (__i == 16) ||
+ (__i == -1) || (__i == -4) || (__i == -8) || (__i == -16) )
+ {
+ oldval = gmx_hpia64_fetchadd(a,__i);
+ newval = oldval + i;
+ }
+ else
+ {
+ /* Use compare-exchange addition that works with any value */
+ do
+ {
+ oldval = gmx_atomic_read(a);
+ newval = oldval + i;
+ }
+ while(gmx_atomic_cmpxchg(a,oldval,newval) != oldval);
+ }
+ return newval;
+}
+
+
+
+static inline int
+gmx_atomic_fetch_add(gmx_atomic_t * a,
+ int i)
+{
+ int oldval,newval;
+ int __i = i;
+
+ /* On HP-UX we don't know any macro to determine whether the increment
+ * is known at compile time, but hopefully the call uses something simple
+ * like a constant, and then the optimizer should be able to do the job.
+ */
+ if ( (__i == 1) || (__i == 4) || (__i == 8) || (__i == 16) ||
+ (__i == -1) || (__i == -4) || (__i == -8) || (__i == -16) )
+ {
+ oldval = gmx_hpia64_fetchadd(a,__i);
+ newval = oldval + i;
+ }
+ else
+ {
+ /* Use compare-exchange addition that works with any value */
+ do
+ {
+ oldval = gmx_atomic_read(a);
+ newval = oldval + i;
+ }
+ while(gmx_atomic_cmpxchg(a,oldval,newval) != oldval);
+ }
+ return oldval;
+}
+
+
+static inline void
+gmx_spinlock_init(gmx_spinlock_t *x)
+{
+ x->lock = 0;
+}
+
+
+
+
+
+static inline void
+gmx_spinlock_trylock(gmx_spinlock_t *x)
+{
+ int rc;
+
+ rc = _Asm_xchg((_Asm_sz)_SZ_W, (unsigned int *)x, 1
+ (_Asm_ldhit)_LDHINT_NONE);
+
+ return ( (rc>0) ? 1 : 0);
+}
+
+
+static inline void
+gmx_spinlock_lock(gmx_spinlock_t *x)
+{
+ int status = 1;
+
+ do
+ {
+ if( *((unsigned int *)x->lock) == 0 )
+ {
+ status = gmx_spinlock_trylock(x);
+ }
+ } while( status != 0);
+}
+
+
+static inline void
+gmx_spinlock_unlock(gmx_spinlock_t * x)
+{
+ _Asm_fetchadd((_Asm_fasz)_SZ_W,(_Asm_sem)_SEM_REL,
+ (unsigned int *)x,-1,(_Asm_ldhint)_LDHINT_NONE);
+}
+
+
+
+static inline void
+gmx_spinlock_islocked(gmx_spinlock_t * x)
+{
+ return ( x->lock != 0 );
+}
+
+
+
+static inline void
+gmx_spinlock_wait(gmx_spinlock_t * x)
+{
+ do
+ {
+ gmx_atomic_memory_barrier();
+ }
+ while(gmx_spinlock_islocked(x));
+}
+
+
+#undef gmx_hpia64_fetchadd
+
+
+
+#elif (defined(_MSC_VER) && (_MSC_VER >= 1200))
+/* Microsoft Visual C on x86, define taken from FFTW who got it from Morten Nissov */
+
+#include <windows.h>
+
+#define gmx_atomic_memory_barrier()
+
+
+typedef struct gmx_atomic
+{
+ LONG volatile value; /*!< Volatile, to avoid compiler aliasing */
+}
+gmx_atomic_t;
+
+
+typedef struct gmx_spinlock
+{
+ LONG volatile lock; /*!< Volatile, to avoid compiler aliasing */
+}
+gmx_spinlock_t;
+
+
+#define GMX_SPINLOCK_INITIALIZER { 0 }
+
+
+
+
+#define gmx_atomic_read(a) ((a)->value)
+#define gmx_atomic_set(a,i) (((a)->value) = (i))
+
+
+
+
+#define gmx_atomic_fetch_add(a, i) \
+ InterlockedExchangeAdd((LONG volatile *)a, (LONG) i)
+
+#define gmx_atomic_add_return(a, i) \
+ ( i + InterlockedExchangeAdd((LONG volatile *)a, (LONG) i) )
+
+#define gmx_atomic_cmpxchg(a, oldval, newval) \
+ InterlockedCompareExchange((LONG volatile *)a, (LONG) newval, (LONG) oldval)
+
+
+# define gmx_spinlock_lock(x) \
+ while((InterlockedCompareExchange((LONG volatile *)&x, 1, 0))!=0)
+
+
+#define gmx_spinlock_trylock(x) \
+ InterlockedCompareExchange((LONG volatile *)&x, 1, 0)
+
+
+static inline void
+gmx_spinlock_unlock(gmx_spinlock_t * x)
+{
+ x->lock = 0;
+}
+
+
+static inline int
+gmx_spinlock_islocked(gmx_spinlock_t * x)
+{
+ return (*(volatile signed char *)(&(x)->lock) != 0);
+}
+
+
+static inline void
+gmx_spinlock_wait(gmx_spinlock_t * x)
+{
+ while(gmx_spinlock_islocked(x))
+ {
+ Sleep(0);
+ }
+}
+
+
+
+#elif defined(__xlC__) && defined (_AIX)
+/* IBM xlC compiler on AIX */
+#include <sys/atomic_op.h>
+
+
+#define gmx_atomic_memory_barrier()
+
+
+typedef struct gmx_atomic
+{
+ volatile int value; /*!< Volatile, to avoid compiler aliasing */
+}
+gmx_atomic_t;
+
+
+typedef struct gmx_spinlock
+{
+ volatile unsigned int lock; /*!< Volatile, to avoid compiler aliasing */
+}
+gmx_spinlock_t;
+
+
+static inline int
+gmx_atomic_cmpxchg(gmx_atomic_t * a,
+ int oldval,
+ int newval)
+{
+ int t;
+
+ if(__check_lock((atomic_p)&a->value, oldval, newval))
+ {
+ /* Not successful - value had changed in memory. Reload value. */
+ t = a->value;
+ }
+ else
+ {
+ /* replacement suceeded */
+ t = oldval;
+ }
+ return t;
+}
+
+
+static inline void
+gmx_atomic_add_return(gmx_atomic_t * a,
+ int i)
+{
+ int oldval,newval;
+
+ do
+ {
+ oldval = gmx_atomic_read(a);
+ newval = oldval + i;
+ }
+ while(__check_lock((atomic_p)&a->value, oldval, newval));
+
+ return newval;
+}
+
+
+
+static inline void
+gmx_atomic_fetch_add(gmx_atomic_t * a,
+ int i)
+{
+ int oldval,newval;
+
+ do
+ {
+ oldval = gmx_atomic_read(a);
+ newval = oldval + i;
+ }
+ while(__check_lock((atomic_p)&a->value, oldval, newval));
+
+ return oldval;
+}
+
+
+static inline void
+gmx_spinlock_init(gmx_spinlock_t * x)
+{
+ __clear_lock((atomic_p)x,0);
+}
+
+
+static inline void
+gmx_spinlock_lock(gmx_spinlock_t * x)
+{
+ do
+ {
+ ;
+ }
+ while(__check_lock((atomic_p)x, 0, 1));
+}
+
+
+static inline void
+gmx_spinlock_trylock(gmx_spinlock_t * x)
+{
+ /* Return 0 if we got the lock */
+ return (__check_lock((atomic_p)x, 0, 1) != 0)
+}
+
+
+static inline void
+gmx_spinlock_unlock(gmx_spinlock_t * x)
+{
+ __clear_lock((atomic_p)x,0);
+}
+
+
+static inline void
+gmx_spinlock_islocked(gmx_spinlock_t * x)
+{
+ return (*((atomic_p)x) != 0);
+}
+
+
+static inline void
+gmx_spinlock_wait(gmx_spinlock_t * x)
+{
+ while(gmx_spinlock_islocked(x)) { ; }
+}
+
+
+#else
+/* No atomic operations, use mutex fallback. Documentation is in x86 section */
+
+
+#define gmx_atomic_memory_barrier()
+
+/* System mutex used for locking to guarantee atomicity */
+static pthread_mutex_t
+gmx_atomic_mutex = PTHREAD_MUTEX_INITIALIZER;
+
+
+typedef struct gmx_atomic
+{
+ int value;
+}
+gmx_atomic_t;
+
+#define gmx_spinlock_t pthread_mutex_t
+
+
+# define GMX_SPINLOCK_INITIALIZER PTHREAD_MUTEX_INITIALIZER
+
+/* Since mutexes guarantee memory barriers this works fine */
+#define gmx_atomic_read(a) ((a)->value)
+
+
+static inline void
+gmx_atomic_set(gmx_atomic_t * a,
+ int i)
+{
+ /* Mutexes here are necessary to guarantee memory visibility */
+ pthread_mutex_lock(&gmx_atomic_mutex);
+ a->value = i;
+ pthread_mutex_unlock(&gmx_atomic_mutex);
+}
+
+
+static inline int
+gmx_atomic_add_return(gmx_atomic_t * a,
+ int i)
+{
+ int t;
+ pthread_mutex_lock(&gmx_atomic_mutex);
+ t = a->value + i;
+ a->value = t;
+ pthread_mutex_unlock(&gmx_atomic_mutex);
+ return t;
+}
+
+
+static inline int
+gmx_atomic_fetch_add(gmx_atomic_t * a,
+ int i)
+{
+ int old_value;
+
+ pthread_mutex_lock(&gmx_atomic_mutex);
+ old_value = a->value;
+ a->value = old_value + i;
+ pthread_mutex_unlock(&gmx_atomic_mutex);
+ return old_value;
+}
+
+
+static inline int
+gmx_atomic_cmpxchg(gmx_atomic_t * a,
+ int oldv,
+ int newv)
+{
+ int t;
+
+ pthread_mutex_lock(&gmx_atomic_mutex);
+ t = a->value;
+ if (t == oldv)
+ {
+ a->value = newv;
+ }
+ pthread_mutex_unlock(&gmx_atomic_mutex);
+ return t;
+}
+
+
+#define gmx_spinlock_init(lock) pthread_mutex_init(lock)
+#define gmx_spinlock_lock(lock) pthread_mutex_lock(lock)
+#define gmx_spinlock_trylock(lock) pthread_mutex_trylock(lock)
+#define gmx_spinlock_unlock(lock) pthread_mutex_unlock(lock)
+
+static inline int
+gmx_spinlock_islocked(gmx_spinlock_t * x)
+{
+ int rc;
+
+ if(gmx_spinlock_trylock(x) != 0)
+ {
+ /* It was locked */
+ return 1;
+ }
+ else
+ {
+ /* We just locked it */
+ gmx_spinlock_unlock(x);
+ return 0;
+ }
+}
+
+
+static inline void
+gmx_spinlock_wait(gmx_spinlock_t * x)
+{
+ int rc;
+
+ gmx_spinlock_lock(x);
+ /* Got the lock now, so the waiting is over */
+ gmx_spinlock_unlock(x);
+}
+
+
+#endif
+
+
+
+
+/*! \brief Spinlock-based barrier type
+ *
+ * This barrier has the same functionality as the standard
+ * gmx_thread_barrier_t, but since it is based on spinlocks
+ * it provides faster synchronization at the cost of busy-waiting.
+ *
+ * Variables of this type should be initialized by calling
+ * gmx_spinlock_barrier_init() to set the number of threads
+ * that should be synchronized.
+ */
+typedef struct gmx_spinlock_barrier
+{
+ gmx_atomic_t count; /*!< Number of threads remaining */
+ int threshold; /*!< Total number of threads */
+ volatile int cycle; /*!< Current cycle (alternating 0/1) */
+}
+gmx_spinlock_barrier_t;
+
+
+
+
+/*! \brief Initialize spinlock-based barrier
+ *
+ * \param barrier Pointer to _spinlock_ barrier. Note that this is not
+ * the same datatype as the full, thread based, barrier.
+ * \param count Number of threads to synchronize. All threads
+ * will be released after \a count calls to
+ * gmx_spinlock_barrier_wait().
+ */
+static inline void
+gmx_spinlock_barrier_init(gmx_spinlock_barrier_t * barrier,
+ int count)
+{
+ barrier->threshold = count;
+ barrier->cycle = 0;
+ gmx_atomic_set(&(barrier->count),count);
+}
+
+
+
+
+/*! \brief Perform busy-waiting barrier synchronization
+*
+* This routine blocks until it has been called N times,
+* where N is the count value the barrier was initialized with.
+* After N total calls all threads return. The barrier automatically
+* cycles, and thus requires another N calls to unblock another time.
+*
+* Note that spinlock-based barriers are completely different from
+* standard ones (using mutexes and condition variables), only the
+* functionality and names are similar.
+*
+* \param barrier Pointer to previously create barrier.
+*
+* \return The last thread returns -1, all the others 0.
+*/
+static inline int
+gmx_spinlock_barrier_wait(gmx_spinlock_barrier_t * barrier)
+{
+ int cycle;
+ int status;
+
+ /* We don't need to lock or use atomic ops here, since the cycle index
+ * cannot change until after the last thread has performed the check
+ * further down. Further, they cannot reach this point in the next
+ * barrier iteration until all of them have been released, and that
+ * happens after the cycle value has been updated.
+ *
+ * No synchronization == fast synchronization.
+ */
+ cycle = barrier->cycle;
+
+ /* Decrement the count atomically and check if it is zero.
+ * This will only be true for the last thread calling us.
+ */
+ if( gmx_atomic_add_return( &(barrier->count), -1 ) == 0)
+ {
+ gmx_atomic_set(&(barrier->count), barrier->threshold);
+ barrier->cycle = !barrier->cycle;
+
+ status = -1;
+ }
+ else
+ {
+ /* Wait until the last thread changes the cycle index.
+ * We are both using a memory barrier, and explicit
+ * volatile pointer cast to make sure the compiler
+ * doesn't try to be smart and cache the contents.
+ */
+ do
+ {
+ gmx_atomic_memory_barrier();
+ }
+ while( *(volatile int *)(&(barrier->cycle)) == cycle);
+
+ status = 0;
+ }
+ return status;
+}
+
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif /* _GMX_ATOMIC_H_ */
diff --git a/SimTKcommon/src/tinyxml.cpp b/SimTKcommon/src/tinyxml.cpp
new file mode 100644
index 0000000..5588dd1
--- /dev/null
+++ b/SimTKcommon/src/tinyxml.cpp
@@ -0,0 +1,1906 @@
+/*
+www.sourceforge.net/projects/tinyxml
+Original code (2.0 and earlier )copyright (c) 2000-2006 Lee Thomason (www.grinninglizard.com)
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any
+damages arising from the use of this software.
+
+Permission is granted to anyone to use this software for any
+purpose, including commercial applications, and to alter it and
+redistribute it freely, subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must
+not claim that you wrote the original software. If you use this
+software in a product, an acknowledgment in the product documentation
+would be appreciated but is not required.
+
+2. Altered source versions must be plainly marked as such, and
+must not be misrepresented as being the original software.
+
+3. This notice may not be removed or altered from any source
+distribution.
+*/
+
+#include <cctype>
+
+#include <sstream>
+#include <iostream>
+
+#include "tinyxml.h"
+
+static const char* DefaultIndentChars = " ";
+
+
+// Microsoft compiler security
+static FILE* TiXmlFOpen( const char* filename, const char* mode )
+{
+ #if defined(_MSC_VER) && (_MSC_VER >= 1400 )
+ FILE* fp = 0;
+ errno_t err = fopen_s( &fp, filename, mode );
+ if ( !err && fp )
+ return fp;
+ return 0;
+ #else
+ return fopen( filename, mode );
+ #endif
+}
+
+namespace SimTK {
+
+
+//------------------------------------------------------------------------------
+// TiXml BASE
+//------------------------------------------------------------------------------
+
+// The goal of the seperate error file is to make the first
+// step towards localization. tinyxml (currently) only supports
+// english error messages, but the could now be translated.
+//
+// It also cleans up the code a bit.
+//
+
+const char* TiXmlBase::errorString[ TIXML_ERROR_STRING_COUNT ] =
+{
+ "No error",
+ "Error",
+ "Failed to open file",
+ "Memory allocation failed.",
+ "Error parsing Element.",
+ "Failed to read Element name",
+ "Error reading Element value.",
+ "Error reading Attributes.",
+ "Error: empty tag.",
+ "Error reading end tag.",
+ "Error parsing Unknown.",
+ "Error parsing Comment.",
+ "Error parsing Declaration.",
+ "Error document empty.",
+ "Error null (0) or unexpected EOF found in input stream.",
+ "Error parsing CDATA.",
+ "Error when TiXmlDocument added to document, because TiXmlDocument can only be at the root.",
+};
+
+
+bool TiXmlBase::condenseWhiteSpace = true;
+
+
+void TiXmlBase::EncodeString
+ (const String& str, String* outString, bool keepQuotes)
+{
+ int i=0;
+
+ while( i<(int)str.length() )
+ {
+ unsigned char c = (unsigned char) str[i];
+
+ if ( c == '&'
+ && i < ( (int)str.length() - 2 )
+ && str[i+1] == '#'
+ && str[i+2] == 'x' )
+ {
+ // Hexadecimal character reference.
+ // Pass through unchanged.
+ // © -- copyright symbol, for example.
+ //
+ // The -1 is a bug fix from Rob Laveaux. It keeps
+ // an overflow from happening if there is no ';'.
+ // There are actually 2 ways to exit this loop -
+ // while fails (error case) and break (semicolon found).
+ // However, there is no mechanism (currently) for
+ // this function to return an error.
+ while ( i<(int)str.length()-1 )
+ {
+ outString->append( str.c_str() + i, 1 );
+ ++i;
+ if ( str[i] == ';' )
+ break;
+ }
+ }
+ else if ( c == '&' )
+ {
+ outString->append( entity[0].str, entity[0].strLength );
+ ++i;
+ }
+ else if ( c == '<' )
+ {
+ outString->append( entity[1].str, entity[1].strLength );
+ ++i;
+ }
+ else if ( c == '>' )
+ {
+ outString->append( entity[2].str, entity[2].strLength );
+ ++i;
+ }
+ else if ( c == '\"' && !keepQuotes )
+ {
+ outString->append( entity[3].str, entity[3].strLength );
+ ++i;
+ }
+ else if ( c == '\'' && !keepQuotes )
+ {
+ outString->append( entity[4].str, entity[4].strLength );
+ ++i;
+ }
+ //sherm 20121009: if we preserved white space on the way in, preserve
+ //it on the way out also.
+ else if ( c < 32 && (condenseWhiteSpace || !IsWhiteSpace(c)))
+ {
+ // Easy pass at non-alpha/numeric/symbol
+ // Below 32 is symbolic.
+ char buf[ 32 ];
+
+ #if defined(TIXML_SNPRINTF)
+ TIXML_SNPRINTF( buf, sizeof(buf), "&#x%02X;", (unsigned) ( c & 0xff ) );
+ #else
+ sprintf( buf, "&#x%02X;", (unsigned) ( c & 0xff ) );
+ #endif
+
+ //*ME: warning C4267: convert 'size_t' to 'int'
+ //*ME: Int-Cast to make compiler happy ...
+ outString->append( buf, (int)strlen( buf ) );
+ ++i;
+ }
+ else
+ {
+ *outString += (char) c; // somewhat more efficient function call.
+ ++i;
+ }
+ }
+}
+
+
+
+//------------------------------------------------------------------------------
+// TiXml NODE
+//------------------------------------------------------------------------------
+
+TiXmlNode::TiXmlNode( NodeType _type ) : TiXmlBase()
+{
+ parent = 0;
+ type = _type;
+ firstChild = 0;
+ lastChild = 0;
+ prev = 0;
+ next = 0;
+}
+
+
+TiXmlNode::~TiXmlNode()
+{
+ TiXmlNode* node = firstChild;
+ TiXmlNode* temp = 0;
+
+ while ( node )
+ {
+ temp = node;
+ node = node->next;
+ delete temp;
+ }
+}
+
+
+void TiXmlNode::CopyTo( TiXmlNode* target ) const
+{
+ target->SetValue (value.c_str() );
+ target->userData = userData;
+}
+
+
+void TiXmlNode::Clear()
+{
+ TiXmlNode* node = firstChild;
+ TiXmlNode* temp = 0;
+
+ while ( node )
+ {
+ temp = node;
+ node = node->next;
+ delete temp;
+ }
+
+ firstChild = 0;
+ lastChild = 0;
+}
+
+
+TiXmlNode* TiXmlNode::LinkEndChild( TiXmlNode* node )
+{
+ assert( node );
+ assert( node->parent == 0 || node->parent == this );
+ assert( node->GetDocument() == 0 || node->GetDocument() == this->GetDocument() );
+
+ if ( node->Type() == TiXmlNode::DOCUMENT )
+ {
+ delete node;
+ if ( GetDocument() ) GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return 0;
+ }
+
+ node->parent = this;
+
+ node->prev = lastChild;
+ node->next = 0;
+
+ if ( lastChild )
+ lastChild->next = node;
+ else
+ firstChild = node; // it was an empty list.
+
+ lastChild = node;
+ return node;
+}
+
+
+TiXmlNode* TiXmlNode::InsertEndChild( const TiXmlNode& addThis )
+{
+ if ( addThis.Type() == TiXmlNode::DOCUMENT )
+ {
+ if ( GetDocument() ) GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return 0;
+ }
+ TiXmlNode* node = addThis.Clone();
+ if ( !node )
+ return 0;
+
+ return LinkEndChild( node );
+}
+
+
+TiXmlNode* TiXmlNode::LinkBeginChild( TiXmlNode* node )
+{
+ assert( node );
+ assert( node->parent == 0 || node->parent == this );
+ assert( node->GetDocument() == 0 || node->GetDocument() == this->GetDocument() );
+
+ if ( node->Type() == TiXmlNode::DOCUMENT )
+ {
+ delete node;
+ if ( GetDocument() ) GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return 0;
+ }
+
+ node->parent = this;
+
+ node->prev = 0;
+ node->next = firstChild;
+
+ if ( firstChild )
+ firstChild->prev = node;
+ else
+ lastChild = node; // it was an empty list.
+
+ firstChild = node;
+ return node;
+}
+
+// sherm 100319: I added this and reimplemented InsertBeforeChild using it.
+TiXmlNode* TiXmlNode::LinkBeforeChild
+ ( TiXmlNode* beforeThis, TiXmlNode* prependThis )
+{
+ assert( beforeThis && beforeThis->parent == this);
+ assert( prependThis );
+ assert( prependThis->parent == 0 || prependThis->parent == this );
+ assert( prependThis->GetDocument() == 0 || prependThis->GetDocument() == this->GetDocument() );
+
+ if ( prependThis->Type() == TiXmlNode::DOCUMENT )
+ {
+ delete prependThis;
+ if ( GetDocument() ) GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return 0;
+ }
+
+ prependThis->parent = this;
+
+ prependThis->next = beforeThis;
+ prependThis->prev = beforeThis->prev;
+ if ( beforeThis->prev )
+ {
+ beforeThis->prev->next = prependThis;
+ }
+ else
+ {
+ assert( firstChild == beforeThis );
+ firstChild = prependThis;
+ }
+ beforeThis->prev = prependThis;
+ return prependThis;
+}
+
+TiXmlNode* TiXmlNode::InsertBeforeChild( TiXmlNode* beforeThis, const TiXmlNode& addThis )
+{
+ return LinkBeforeChild(beforeThis, addThis.Clone());
+}
+
+// sherm 100319: I added this and reimplemented InsertAfterChild using it.
+TiXmlNode* TiXmlNode::LinkAfterChild( TiXmlNode* afterThis, TiXmlNode* addThis )
+{
+ assert( afterThis && afterThis->parent == this);
+ assert( addThis );
+ assert( addThis->parent == 0 || addThis->parent == this );
+ assert( addThis->GetDocument() == 0 || addThis->GetDocument() == this->GetDocument() );
+
+ if ( addThis->Type() == TiXmlNode::DOCUMENT )
+ {
+ delete addThis; // stop leak
+ if ( GetDocument() ) GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return 0;
+ }
+
+ addThis->parent = this;
+
+ addThis->prev = afterThis;
+ addThis->next = afterThis->next;
+ if ( afterThis->next )
+ {
+ afterThis->next->prev = addThis;
+ }
+ else
+ {
+ assert( lastChild == afterThis );
+ lastChild = addThis;
+ }
+ afterThis->next = addThis;
+ return addThis;
+}
+
+TiXmlNode* TiXmlNode::InsertAfterChild( TiXmlNode* afterThis, const TiXmlNode& addThis )
+{
+ return LinkAfterChild(afterThis, addThis.Clone());
+}
+
+
+TiXmlNode* TiXmlNode::ReplaceChild( TiXmlNode* replaceThis, const TiXmlNode& withThis )
+{
+ if ( replaceThis->parent != this )
+ return 0;
+
+ TiXmlNode* node = withThis.Clone();
+ if ( !node )
+ return 0;
+
+ node->next = replaceThis->next;
+ node->prev = replaceThis->prev;
+
+ if ( replaceThis->next )
+ replaceThis->next->prev = node;
+ else
+ lastChild = node;
+
+ if ( replaceThis->prev )
+ replaceThis->prev->next = node;
+ else
+ firstChild = node;
+
+ delete replaceThis;
+ node->parent = this;
+ return node;
+}
+
+TiXmlNode* TiXmlNode::DisconnectChild( TiXmlNode* disconnectThis )
+{
+ if ( disconnectThis->parent != this )
+ {
+ assert( 0 );
+ return 0;
+ }
+
+ if ( disconnectThis->next )
+ disconnectThis->next->prev = disconnectThis->prev;
+ else
+ lastChild = disconnectThis->prev;
+
+ if ( disconnectThis->prev )
+ disconnectThis->prev->next = disconnectThis->next;
+ else
+ firstChild = disconnectThis->next;
+
+ // Make this an only child, then an orphan.
+ disconnectThis->next = 0;
+ disconnectThis->prev = 0;
+ disconnectThis->parent = 0;
+
+ return disconnectThis;
+}
+
+bool TiXmlNode::RemoveChild( TiXmlNode* removeThis )
+{
+ if (DisconnectChild(removeThis) == 0) {
+ assert(!"couldn't disconnect node");
+ return false;
+ }
+
+ // We own the node now.
+ delete removeThis;
+ return true;
+}
+
+const TiXmlNode* TiXmlNode::FirstChild( const char * _value ) const
+{
+ const TiXmlNode* node;
+ for ( node = firstChild; node; node = node->next )
+ {
+ if ( strcmp( node->Value(), _value ) == 0 )
+ return node;
+ }
+ return 0;
+}
+
+
+const TiXmlNode* TiXmlNode::LastChild( const char * _value ) const
+{
+ const TiXmlNode* node;
+ for ( node = lastChild; node; node = node->prev )
+ {
+ if ( strcmp( node->Value(), _value ) == 0 )
+ return node;
+ }
+ return 0;
+}
+
+
+const TiXmlNode* TiXmlNode::IterateChildren( const TiXmlNode* previous ) const
+{
+ if ( !previous )
+ {
+ return FirstChild();
+ }
+ else
+ {
+ assert( previous->parent == this );
+ return previous->NextSibling();
+ }
+}
+
+
+const TiXmlNode* TiXmlNode::IterateChildren( const char * val, const TiXmlNode* previous ) const
+{
+ if ( !previous )
+ {
+ return FirstChild( val );
+ }
+ else
+ {
+ assert( previous->parent == this );
+ return previous->NextSibling( val );
+ }
+}
+
+
+const TiXmlNode* TiXmlNode::NextSibling( const char * _value ) const
+{
+ const TiXmlNode* node;
+ for ( node = next; node; node = node->next )
+ {
+ if ( strcmp( node->Value(), _value ) == 0 )
+ return node;
+ }
+ return 0;
+}
+
+
+const TiXmlNode* TiXmlNode::PreviousSibling( const char * _value ) const
+{
+ const TiXmlNode* node;
+ for ( node = prev; node; node = node->prev )
+ {
+ if ( strcmp( node->Value(), _value ) == 0 )
+ return node;
+ }
+ return 0;
+}
+
+const TiXmlElement* TiXmlNode::FirstChildElement() const
+{
+ const TiXmlNode* node;
+
+ for ( node = FirstChild();
+ node;
+ node = node->NextSibling() )
+ {
+ if ( node->ToElement() )
+ return node->ToElement();
+ }
+ return 0;
+}
+
+const TiXmlElement* TiXmlNode::LastChildElement() const
+{
+ const TiXmlNode* node;
+
+ for ( node = LastChild();
+ node;
+ node = node->PreviousSibling() )
+ {
+ if ( node->ToElement() )
+ return node->ToElement();
+ }
+ return 0;
+}
+
+const TiXmlElement* TiXmlNode::FirstChildElement( const char * _value ) const
+{
+ const TiXmlNode* node;
+
+ for ( node = FirstChild( _value );
+ node;
+ node = node->NextSibling( _value ) )
+ {
+ if ( node->ToElement() )
+ return node->ToElement();
+ }
+ return 0;
+}
+
+const TiXmlElement* TiXmlNode::LastChildElement( const char * _value ) const
+{
+ const TiXmlNode* node;
+
+ for ( node = LastChild( _value );
+ node;
+ node = node->PreviousSibling( _value ) )
+ {
+ if ( node->ToElement() )
+ return node->ToElement();
+ }
+ return 0;
+}
+
+
+const TiXmlElement* TiXmlNode::NextSiblingElement() const
+{
+ const TiXmlNode* node;
+
+ for ( node = NextSibling();
+ node;
+ node = node->NextSibling() )
+ {
+ if ( node->ToElement() )
+ return node->ToElement();
+ }
+ return 0;
+}
+
+const TiXmlElement* TiXmlNode::PreviousSiblingElement() const
+{
+ const TiXmlNode* node;
+
+ for ( node = PreviousSibling();
+ node;
+ node = node->PreviousSibling() )
+ {
+ if ( node->ToElement() )
+ return node->ToElement();
+ }
+ return 0;
+}
+
+const TiXmlElement* TiXmlNode::NextSiblingElement( const char * _value ) const
+{
+ const TiXmlNode* node;
+
+ for ( node = NextSibling( _value );
+ node;
+ node = node->NextSibling( _value ) )
+ {
+ if ( node->ToElement() )
+ return node->ToElement();
+ }
+ return 0;
+}
+
+const TiXmlElement* TiXmlNode::PreviousSiblingElement( const char * _value ) const
+{
+ const TiXmlNode* node;
+
+ for ( node = PreviousSibling( _value );
+ node;
+ node = node->PreviousSibling( _value ) )
+ {
+ if ( node->ToElement() )
+ return node->ToElement();
+ }
+ return 0;
+}
+
+const TiXmlDocument* TiXmlNode::GetDocument() const
+{
+ const TiXmlNode* node;
+
+ for( node = this; node; node = node->parent )
+ {
+ if ( node->ToDocument() )
+ return node->ToDocument();
+ }
+ return 0;
+}
+
+//------------------------------------------------------------------------------
+// TiXml ELEMENT
+//------------------------------------------------------------------------------
+
+TiXmlElement::TiXmlElement (const char * _value)
+ : TiXmlNode( TiXmlNode::ELEMENT )
+{
+ firstChild = lastChild = 0;
+ value = _value;
+}
+
+
+TiXmlElement::TiXmlElement( const String& _value )
+ : TiXmlNode( TiXmlNode::ELEMENT )
+{
+ firstChild = lastChild = 0;
+ value = _value;
+}
+
+
+TiXmlElement::TiXmlElement( const TiXmlElement& copy)
+ : TiXmlNode( TiXmlNode::ELEMENT )
+{
+ firstChild = lastChild = 0;
+ copy.CopyTo( this );
+}
+
+
+void TiXmlElement::operator=( const TiXmlElement& base )
+{
+ ClearThis();
+ base.CopyTo( this );
+}
+
+
+TiXmlElement::~TiXmlElement()
+{
+ ClearThis();
+}
+
+
+void TiXmlElement::ClearThis()
+{
+ Clear();
+ while( attributeSet.First() )
+ {
+ TiXmlAttribute* node = attributeSet.First();
+ attributeSet.Remove( node );
+ delete node;
+ }
+}
+
+
+const char* TiXmlElement::Attribute( const char* name ) const
+{
+ const TiXmlAttribute* node = attributeSet.Find( name );
+ if ( node )
+ return node->Value();
+ return 0;
+}
+
+
+const String* TiXmlElement::Attribute( const String& name ) const
+{
+ const TiXmlAttribute* node = attributeSet.Find( name );
+ if ( node )
+ return &node->ValueStr();
+ return 0;
+}
+
+
+const char* TiXmlElement::Attribute( const char* name, int* i ) const
+{
+ const char* s = Attribute( name );
+ if ( i )
+ {
+ if ( s ) {
+ *i = atoi( s );
+ }
+ else {
+ *i = 0;
+ }
+ }
+ return s;
+}
+
+
+const String* TiXmlElement::Attribute( const String& name, int* i ) const
+{
+ const String* s = Attribute( name );
+ if ( i )
+ {
+ if ( s ) {
+ *i = atoi( s->c_str() );
+ }
+ else {
+ *i = 0;
+ }
+ }
+ return s;
+}
+
+
+const char* TiXmlElement::Attribute( const char* name, double* d ) const
+{
+ const char* s = Attribute( name );
+ if ( d )
+ {
+ if ( s ) {
+ *d = atof( s );
+ }
+ else {
+ *d = 0;
+ }
+ }
+ return s;
+}
+
+
+const String* TiXmlElement::Attribute( const String& name, double* d ) const
+{
+ const String* s = Attribute( name );
+ if ( d )
+ {
+ if ( s ) {
+ *d = atof( s->c_str() );
+ }
+ else {
+ *d = 0;
+ }
+ }
+ return s;
+}
+
+
+int TiXmlElement::QueryIntAttribute( const char* name, int* ival ) const
+{
+ const TiXmlAttribute* node = attributeSet.Find( name );
+ if ( !node )
+ return TIXML_NO_ATTRIBUTE;
+ return node->QueryIntValue( ival );
+}
+
+
+int TiXmlElement::QueryIntAttribute( const String& name, int* ival ) const
+{
+ const TiXmlAttribute* node = attributeSet.Find( name );
+ if ( !node )
+ return TIXML_NO_ATTRIBUTE;
+ return node->QueryIntValue( ival );
+}
+
+
+int TiXmlElement::QueryDoubleAttribute( const char* name, double* dval ) const
+{
+ const TiXmlAttribute* node = attributeSet.Find( name );
+ if ( !node )
+ return TIXML_NO_ATTRIBUTE;
+ return node->QueryDoubleValue( dval );
+}
+
+
+int TiXmlElement::QueryDoubleAttribute( const String& name, double* dval ) const
+{
+ const TiXmlAttribute* node = attributeSet.Find( name );
+ if ( !node )
+ return TIXML_NO_ATTRIBUTE;
+ return node->QueryDoubleValue( dval );
+}
+
+
+void TiXmlElement::SetAttribute( const char * name, int val )
+{
+ char buf[64];
+ #if defined(TIXML_SNPRINTF)
+ TIXML_SNPRINTF( buf, sizeof(buf), "%d", val );
+ #else
+ sprintf( buf, "%d", val );
+ #endif
+ SetAttribute( name, buf );
+}
+
+
+void TiXmlElement::SetAttribute( const String& name, int val )
+{
+ std::ostringstream oss;
+ oss << val;
+ SetAttribute( name, oss.str() );
+}
+
+
+void TiXmlElement::SetDoubleAttribute( const char * name, double val )
+{
+ char buf[256];
+ #if defined(TIXML_SNPRINTF)
+ TIXML_SNPRINTF( buf, sizeof(buf), "%f", val );
+ #else
+ sprintf( buf, "%f", val );
+ #endif
+ SetAttribute( name, buf );
+}
+
+
+void TiXmlElement::SetAttribute( const char * cname, const char * cvalue )
+{
+ String _name( cname );
+ String _value( cvalue );
+
+ TiXmlAttribute* node = attributeSet.Find( _name );
+ if ( node )
+ {
+ node->SetValue( _value );
+ return;
+ }
+
+ TiXmlAttribute* attrib = new TiXmlAttribute( cname, cvalue );
+ if ( attrib )
+ {
+ attributeSet.Add( attrib );
+ }
+ else
+ {
+ TiXmlDocument* document = GetDocument();
+ if ( document ) document->SetError( TIXML_ERROR_OUT_OF_MEMORY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ }
+}
+
+void TiXmlElement::SetAttribute( const String& name, const String& _value )
+{
+ TiXmlAttribute* node = attributeSet.Find( name );
+ if ( node )
+ {
+ node->SetValue( _value );
+ return;
+ }
+
+ TiXmlAttribute* attrib = new TiXmlAttribute( name, _value );
+ if ( attrib )
+ {
+ attributeSet.Add( attrib );
+ }
+ else
+ {
+ TiXmlDocument* document = GetDocument();
+ if ( document ) document->SetError( TIXML_ERROR_OUT_OF_MEMORY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ }
+}
+
+
+void TiXmlElement::RemoveAttribute( const char * name )
+{
+ String str( name );
+ TiXmlAttribute* node = attributeSet.Find( str );
+
+ if ( node )
+ {
+ attributeSet.Remove( node );
+ delete node;
+ }
+}
+
+
+void TiXmlElement::Print( FILE* cfile, int depth ) const
+{
+ int i;
+ assert( cfile );
+ const char* indent = GetDocument()
+ ? GetDocument()->GetIndentChars()
+ : DefaultIndentChars;
+ for ( i=0; i<depth; i++ )
+ fprintf( cfile, "%s", indent );
+
+ fprintf( cfile, "<%s", value.c_str() );
+
+ const TiXmlAttribute* attrib;
+ for ( attrib = attributeSet.First(); attrib; attrib = attrib->Next() )
+ {
+ fprintf( cfile, " " );
+ attrib->Print( cfile, depth );
+ }
+
+ // There are 3 different formatting approaches:
+ // 1) An element without children is printed as a <foo /> node
+ // 2) An element with only a text child is printed as <foo> text </foo>
+ // 3) An element with children is printed on multiple lines.
+ TiXmlNode* node;
+ if ( !firstChild )
+ {
+ fprintf( cfile, " />" );
+ }
+ else if ( firstChild == lastChild && firstChild->ToText() )
+ {
+ fprintf( cfile, ">" );
+ firstChild->Print( cfile, depth + 1 );
+ fprintf( cfile, "</%s>", value.c_str() );
+ }
+ else
+ {
+ fprintf( cfile, ">" );
+
+ for ( node = firstChild; node; node=node->NextSibling() )
+ {
+ if ( !node->ToText() )
+ {
+ fprintf( cfile, "\n" );
+ }
+ node->Print( cfile, depth+1 );
+ }
+ fprintf( cfile, "\n" );
+ for( i=0; i<depth; ++i ) {
+ fprintf( cfile, "%s", indent );
+ }
+ fprintf( cfile, "</%s>", value.c_str() );
+ }
+}
+
+
+void TiXmlElement::CopyTo( TiXmlElement* target ) const
+{
+ // superclass:
+ TiXmlNode::CopyTo( target );
+
+ // Element class:
+ // Clone the attributes, then clone the children.
+ const TiXmlAttribute* attribute = 0;
+ for( attribute = attributeSet.First();
+ attribute;
+ attribute = attribute->Next() )
+ {
+ target->SetAttribute( attribute->Name(), attribute->Value() );
+ }
+
+ TiXmlNode* node = 0;
+ for ( node = firstChild; node; node = node->NextSibling() )
+ {
+ target->LinkEndChild( node->Clone() );
+ }
+}
+
+bool TiXmlElement::Accept( TiXmlVisitor* visitor ) const
+{
+ if ( visitor->VisitEnter( *this, attributeSet.First() ) )
+ {
+ for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() )
+ {
+ if ( !node->Accept( visitor ) )
+ break;
+ }
+ }
+ return visitor->VisitExit( *this );
+}
+
+
+TiXmlNode* TiXmlElement::Clone() const
+{
+ TiXmlElement* clone = new TiXmlElement( Value() );
+ if ( !clone )
+ return 0;
+
+ CopyTo( clone );
+ return clone;
+}
+
+
+const char* TiXmlElement::GetText() const
+{
+ const TiXmlNode* child = this->FirstChild();
+ if ( child ) {
+ const TiXmlText* childText = child->ToText();
+ if ( childText ) {
+ return childText->Value();
+ }
+ }
+ return 0;
+}
+
+//------------------------------------------------------------------------------
+// TiXml DOCUMENT
+//------------------------------------------------------------------------------
+
+TiXmlDocument::TiXmlDocument() : TiXmlNode( TiXmlNode::DOCUMENT )
+{
+ tabsize = 4;
+ useMicrosoftBOM = false;
+ indentString = DefaultIndentChars;
+ ClearError();
+}
+
+TiXmlDocument::TiXmlDocument( const char * documentName ) : TiXmlNode( TiXmlNode::DOCUMENT )
+{
+ tabsize = 4;
+ useMicrosoftBOM = false;
+ value = documentName;
+ indentString = DefaultIndentChars;
+ ClearError();
+}
+
+
+TiXmlDocument::TiXmlDocument( const String& documentName ) : TiXmlNode( TiXmlNode::DOCUMENT )
+{
+ tabsize = 4;
+ useMicrosoftBOM = false;
+ value = documentName;
+ indentString = DefaultIndentChars;
+ ClearError();
+}
+
+
+TiXmlDocument::TiXmlDocument( const TiXmlDocument& copy ) : TiXmlNode( TiXmlNode::DOCUMENT )
+{
+ copy.CopyTo( this );
+}
+
+
+void TiXmlDocument::operator=( const TiXmlDocument& copy )
+{
+ Clear();
+ copy.CopyTo( this );
+}
+
+
+bool TiXmlDocument::LoadFile( TiXmlEncoding encoding )
+{
+ // See STL_STRING_BUG below.
+ //StringToBuffer buf( value );
+
+ return LoadFile( Value(), encoding );
+}
+
+
+bool TiXmlDocument::SaveFile() const
+{
+ // See STL_STRING_BUG below.
+// StringToBuffer buf( value );
+//
+// if ( buf.buffer && SaveFile( buf.buffer ) )
+// return true;
+//
+// return false;
+ return SaveFile( Value() );
+}
+
+bool TiXmlDocument::LoadFile( const char* _filename, TiXmlEncoding encoding )
+{
+ // There was a really terrifying little bug here. The code:
+ // value = filename
+ // in the STL case, cause the assignment method of the String to
+ // be called. What is strange, is that the String had the same
+ // address as it's c_str() method, and so bad things happen. Looks
+ // like a bug in the Microsoft STL implementation.
+ // Add an extra string to avoid the crash.
+ String filename( _filename );
+ value = filename;
+
+ // reading in binary mode so that tinyxml can normalize the EOL
+ FILE* file = TiXmlFOpen( value.c_str (), "rb" );
+
+ if ( file )
+ {
+ bool result = LoadFile( file, encoding );
+ fclose( file );
+ return result;
+ }
+ else
+ {
+ SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return false;
+ }
+}
+
+bool TiXmlDocument::LoadFile( FILE* file, TiXmlEncoding encoding )
+{
+ if ( !file )
+ {
+ SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return false;
+ }
+
+ // Delete the existing data:
+ Clear();
+ location.Clear();
+
+ // Get the file size, so we can pre-allocate the string. HUGE speed impact.
+ long length = 0;
+ fseek( file, 0, SEEK_END );
+ length = ftell( file );
+ fseek( file, 0, SEEK_SET );
+
+ // Strange case, but good to handle up front.
+ if ( length <= 0 )
+ {
+ SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return false;
+ }
+
+ // If we have a file, assume it is all one big XML file, and read it in.
+ // The document parser may decide the document ends sooner than the entire file, however.
+ String data;
+ data.reserve( length );
+
+ // Subtle bug here. TinyXml did use fgets. But from the XML spec:
+ // 2.11 End-of-Line Handling
+ // <snip>
+ // <quote>
+ // ...the XML processor MUST behave as if it normalized all line breaks in external
+ // parsed entities (including the document entity) on input, before parsing, by translating
+ // both the two-character sequence #xD #xA and any #xD that is not followed by #xA to
+ // a single #xA character.
+ // </quote>
+ //
+ // It is not clear fgets does that, and certainly isn't clear it works cross platform.
+ // Generally, you expect fgets to translate from the convention of the OS to the c/unix
+ // convention, and not work generally.
+
+ /*
+ while( fgets( buf, sizeof(buf), file ) )
+ {
+ data += buf;
+ }
+ */
+
+ char* buf = new char[ length+1 ];
+ buf[0] = 0;
+
+ if ( fread( buf, length, 1, file ) != 1 ) {
+ delete [] buf;
+ SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return false;
+ }
+
+ const char* lastPos = buf;
+ const char* p = buf;
+
+ buf[length] = 0;
+ while( *p ) {
+ assert( p < (buf+length) );
+ if ( *p == 0xa ) {
+ // Newline character. No special rules for this. Append all the characters
+ // since the last string, and include the newline.
+ data.append( lastPos, (p-lastPos+1) ); // append, include the newline
+ ++p; // move past the newline
+ lastPos = p; // and point to the new buffer (may be 0)
+ assert( p <= (buf+length) );
+ }
+ else if ( *p == 0xd ) {
+ // Carriage return. Append what we have so far, then
+ // handle moving forward in the buffer.
+ if ( (p-lastPos) > 0 ) {
+ data.append( lastPos, p-lastPos ); // do not add the CR
+ }
+ data += (char)0xa; // a proper newline
+
+ if ( *(p+1) == 0xa ) {
+ // Carriage return - new line sequence
+ p += 2;
+ lastPos = p;
+ assert( p <= (buf+length) );
+ }
+ else {
+ // it was followed by something else...that is presumably characters again.
+ ++p;
+ lastPos = p;
+ assert( p <= (buf+length) );
+ }
+ }
+ else {
+ ++p;
+ }
+ }
+ // Handle any left over characters.
+ if ( p-lastPos ) {
+ data.append( lastPos, p-lastPos );
+ }
+ delete [] buf;
+ buf = 0;
+
+ Parse( data.c_str(), 0, encoding );
+
+ if ( Error() )
+ return false;
+ else
+ return true;
+}
+
+
+bool TiXmlDocument::SaveFile( const char * filename ) const
+{
+ // The old c stuff lives on...
+ FILE* fp = TiXmlFOpen( filename, "w" );
+ if ( fp )
+ {
+ bool result = SaveFile( fp );
+ fclose( fp );
+ return result;
+ }
+ return false;
+}
+
+
+bool TiXmlDocument::SaveFile( FILE* fp ) const
+{
+ if ( useMicrosoftBOM )
+ {
+ const unsigned char TIXML_UTF_LEAD_0 = 0xefU;
+ const unsigned char TIXML_UTF_LEAD_1 = 0xbbU;
+ const unsigned char TIXML_UTF_LEAD_2 = 0xbfU;
+
+ fputc( TIXML_UTF_LEAD_0, fp );
+ fputc( TIXML_UTF_LEAD_1, fp );
+ fputc( TIXML_UTF_LEAD_2, fp );
+ }
+ Print( fp, 0 );
+ return (ferror(fp) == 0);
+}
+
+
+void TiXmlDocument::CopyTo( TiXmlDocument* target ) const
+{
+ TiXmlNode::CopyTo( target );
+
+ target->error = error;
+ target->errorId = errorId;
+ target->errorDesc = errorDesc;
+ target->tabsize = tabsize;
+ target->errorLocation = errorLocation;
+ target->useMicrosoftBOM = useMicrosoftBOM;
+ target->indentString = indentString;
+
+ TiXmlNode* node = 0;
+ for ( node = firstChild; node; node = node->NextSibling() )
+ {
+ target->LinkEndChild( node->Clone() );
+ }
+}
+
+
+TiXmlNode* TiXmlDocument::Clone() const
+{
+ TiXmlDocument* clone = new TiXmlDocument();
+ if ( !clone )
+ return 0;
+
+ CopyTo( clone );
+ return clone;
+}
+
+
+void TiXmlDocument::Print( FILE* cfile, int depth ) const
+{
+ assert( cfile );
+ for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() )
+ {
+ node->Print( cfile, depth );
+ fprintf( cfile, "\n" );
+ }
+}
+
+
+bool TiXmlDocument::Accept( TiXmlVisitor* visitor ) const
+{
+ if ( visitor->VisitEnter( *this ) )
+ {
+ for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() )
+ {
+ if ( !node->Accept( visitor ) )
+ break;
+ }
+ }
+ return visitor->VisitExit( *this );
+}
+
+
+
+//------------------------------------------------------------------------------
+// TiXml ATTRIBUTE
+//------------------------------------------------------------------------------
+
+const TiXmlAttribute* TiXmlAttribute::Next() const
+{
+ // We are using knowledge of the sentinel. The sentinel
+ // have a value or name.
+ if ( next->value.empty() && next->name.empty() )
+ return 0;
+ return next;
+}
+
+/*
+TiXmlAttribute* TiXmlAttribute::Next()
+{
+ // We are using knowledge of the sentinel. The sentinel
+ // have a value or name.
+ if ( next->value.empty() && next->name.empty() )
+ return 0;
+ return next;
+}
+*/
+
+const TiXmlAttribute* TiXmlAttribute::Previous() const
+{
+ // We are using knowledge of the sentinel. The sentinel
+ // have a value or name.
+ if ( prev->value.empty() && prev->name.empty() )
+ return 0;
+ return prev;
+}
+
+/*
+TiXmlAttribute* TiXmlAttribute::Previous()
+{
+ // We are using knowledge of the sentinel. The sentinel
+ // have a value or name.
+ if ( prev->value.empty() && prev->name.empty() )
+ return 0;
+ return prev;
+}
+*/
+
+void TiXmlAttribute::Print( FILE* cfile, int /*depth*/, String* str ) const
+{
+ String n, v;
+
+ EncodeString( name, &n );
+ EncodeString( value, &v );
+
+ if (value.find ('\"') == String::npos) {
+ if ( cfile ) {
+ fprintf (cfile, "%s=\"%s\"", n.c_str(), v.c_str() );
+ }
+ if ( str ) {
+ (*str) += n; (*str) += "=\""; (*str) += v; (*str) += "\"";
+ }
+ }
+ else {
+ if ( cfile ) {
+ fprintf (cfile, "%s='%s'", n.c_str(), v.c_str() );
+ }
+ if ( str ) {
+ (*str) += n; (*str) += "='"; (*str) += v; (*str) += "'";
+ }
+ }
+}
+
+
+int TiXmlAttribute::QueryIntValue( int* ival ) const
+{
+ if ( TIXML_SSCANF( value.c_str(), "%d", ival ) == 1 )
+ return TIXML_SUCCESS;
+ return TIXML_WRONG_TYPE;
+}
+
+int TiXmlAttribute::QueryDoubleValue( double* dval ) const
+{
+ if ( TIXML_SSCANF( value.c_str(), "%lf", dval ) == 1 )
+ return TIXML_SUCCESS;
+ return TIXML_WRONG_TYPE;
+}
+
+void TiXmlAttribute::SetIntValue( int _value )
+{
+ char buf [64];
+ #if defined(TIXML_SNPRINTF)
+ TIXML_SNPRINTF(buf, sizeof(buf), "%d", _value);
+ #else
+ sprintf (buf, "%d", _value);
+ #endif
+ SetValue (buf);
+}
+
+void TiXmlAttribute::SetDoubleValue( double _value )
+{
+ char buf [256];
+ #if defined(TIXML_SNPRINTF)
+ TIXML_SNPRINTF( buf, sizeof(buf), "%lf", _value);
+ #else
+ sprintf (buf, "%lf", _value);
+ #endif
+ SetValue (buf);
+}
+
+int TiXmlAttribute::IntValue() const
+{
+ return atoi (value.c_str ());
+}
+
+double TiXmlAttribute::DoubleValue() const
+{
+ return atof (value.c_str ());
+}
+
+
+
+//------------------------------------------------------------------------------
+// TiXml COMMENT
+//------------------------------------------------------------------------------
+
+TiXmlComment::TiXmlComment( const TiXmlComment& copy ) : TiXmlNode( TiXmlNode::COMMENT )
+{
+ copy.CopyTo( this );
+}
+
+
+void TiXmlComment::operator=( const TiXmlComment& base )
+{
+ Clear();
+ base.CopyTo( this );
+}
+
+
+void TiXmlComment::Print( FILE* cfile, int depth ) const
+{
+ assert( cfile );
+ const char* indent = GetDocument()
+ ? GetDocument()->GetIndentChars()
+ : DefaultIndentChars;
+ for ( int i=0; i<depth; i++ )
+ fprintf( cfile, "%s", indent );
+
+ fprintf( cfile, "<!--%s-->", value.c_str() );
+}
+
+
+void TiXmlComment::CopyTo( TiXmlComment* target ) const
+{
+ TiXmlNode::CopyTo( target );
+}
+
+
+bool TiXmlComment::Accept( TiXmlVisitor* visitor ) const
+{
+ return visitor->Visit( *this );
+}
+
+
+TiXmlNode* TiXmlComment::Clone() const
+{
+ TiXmlComment* clone = new TiXmlComment();
+
+ if ( !clone )
+ return 0;
+
+ CopyTo( clone );
+ return clone;
+}
+
+//------------------------------------------------------------------------------
+// TiXml TEXT
+//------------------------------------------------------------------------------
+
+void TiXmlText::Print( FILE* cfile, int depth ) const
+{
+ assert( cfile );
+ if ( cdata )
+ {
+ int i;
+ fprintf( cfile, "\n" );
+ const char* indent = GetDocument()
+ ? GetDocument()->GetIndentChars()
+ : DefaultIndentChars;
+ for ( i=0; i<depth; i++ )
+ fprintf( cfile, "%s", indent );
+
+ fprintf( cfile, "<![CDATA[%s]]>\n", value.c_str() ); // unformatted output
+ }
+ else
+ {
+ String buffer;
+ // sherm: Third argument says OK to keep quotes here
+ EncodeString( value, &buffer, true );
+ fprintf( cfile, "%s", buffer.c_str() );
+ }
+}
+
+
+void TiXmlText::CopyTo( TiXmlText* target ) const
+{
+ TiXmlNode::CopyTo( target );
+ target->cdata = cdata;
+}
+
+
+bool TiXmlText::Accept( TiXmlVisitor* visitor ) const
+{
+ return visitor->Visit( *this );
+}
+
+
+TiXmlNode* TiXmlText::Clone() const
+{
+ TiXmlText* clone = 0;
+ clone = new TiXmlText( "" );
+
+ if ( !clone )
+ return 0;
+
+ CopyTo( clone );
+ return clone;
+}
+
+
+
+//------------------------------------------------------------------------------
+// TiXml DECLARATION
+//------------------------------------------------------------------------------
+
+TiXmlDeclaration::TiXmlDeclaration( const char * _version,
+ const char * _encoding,
+ const char * _standalone )
+ : TiXmlNode( TiXmlNode::DECLARATION )
+{
+ version = _version;
+ encoding = _encoding;
+ standalone = _standalone;
+}
+
+
+TiXmlDeclaration::TiXmlDeclaration( const String& _version,
+ const String& _encoding,
+ const String& _standalone )
+ : TiXmlNode( TiXmlNode::DECLARATION )
+{
+ version = _version;
+ encoding = _encoding;
+ standalone = _standalone;
+}
+
+
+TiXmlDeclaration::TiXmlDeclaration( const TiXmlDeclaration& copy )
+ : TiXmlNode( TiXmlNode::DECLARATION )
+{
+ copy.CopyTo( this );
+}
+
+
+void TiXmlDeclaration::operator=( const TiXmlDeclaration& copy )
+{
+ Clear();
+ copy.CopyTo( this );
+}
+
+
+void TiXmlDeclaration::Print( FILE* cfile, int /*depth*/, String* str ) const
+{
+ if ( cfile ) fprintf( cfile, "<?xml " );
+ if ( str ) (*str) += "<?xml ";
+
+ if ( !version.empty() ) {
+ if ( cfile ) fprintf (cfile, "version=\"%s\" ", version.c_str ());
+ if ( str ) { (*str) += "version=\""; (*str) += version; (*str) += "\" "; }
+ }
+ if ( !encoding.empty() ) {
+ if ( cfile ) fprintf (cfile, "encoding=\"%s\" ", encoding.c_str ());
+ if ( str ) { (*str) += "encoding=\""; (*str) += encoding; (*str) += "\" "; }
+ }
+ if ( !standalone.empty() ) {
+ if ( cfile ) fprintf (cfile, "standalone=\"%s\" ", standalone.c_str ());
+ if ( str ) { (*str) += "standalone=\""; (*str) += standalone; (*str) += "\" "; }
+ }
+ if ( cfile ) fprintf( cfile, "?>" );
+ if ( str ) (*str) += "?>";
+}
+
+
+void TiXmlDeclaration::CopyTo( TiXmlDeclaration* target ) const
+{
+ TiXmlNode::CopyTo( target );
+
+ target->version = version;
+ target->encoding = encoding;
+ target->standalone = standalone;
+}
+
+
+bool TiXmlDeclaration::Accept( TiXmlVisitor* visitor ) const
+{
+ return visitor->Visit( *this );
+}
+
+
+TiXmlNode* TiXmlDeclaration::Clone() const
+{
+ TiXmlDeclaration* clone = new TiXmlDeclaration();
+
+ if ( !clone )
+ return 0;
+
+ CopyTo( clone );
+ return clone;
+}
+
+//------------------------------------------------------------------------------
+// TiXml UNKNOWN
+//------------------------------------------------------------------------------
+
+void TiXmlUnknown::Print( FILE* cfile, int depth ) const
+{
+ const char* indent = GetDocument()
+ ? GetDocument()->GetIndentChars()
+ : DefaultIndentChars;
+ for ( int i=0; i<depth; i++ )
+ fprintf( cfile, "%s", indent );
+
+ fprintf( cfile, "<%s>", value.c_str() );
+}
+
+
+void TiXmlUnknown::CopyTo( TiXmlUnknown* target ) const
+{
+ TiXmlNode::CopyTo( target );
+}
+
+
+bool TiXmlUnknown::Accept( TiXmlVisitor* visitor ) const
+{
+ return visitor->Visit( *this );
+}
+
+
+TiXmlNode* TiXmlUnknown::Clone() const
+{
+ TiXmlUnknown* clone = new TiXmlUnknown();
+
+ if ( !clone )
+ return 0;
+
+ CopyTo( clone );
+ return clone;
+}
+
+
+
+//------------------------------------------------------------------------------
+// TiXml ATTRIBUTE SET
+//------------------------------------------------------------------------------
+
+TiXmlAttributeSet::TiXmlAttributeSet()
+{
+ sentinel.next = &sentinel;
+ sentinel.prev = &sentinel;
+}
+
+
+TiXmlAttributeSet::~TiXmlAttributeSet()
+{
+ assert( sentinel.next == &sentinel );
+ assert( sentinel.prev == &sentinel );
+}
+
+
+void TiXmlAttributeSet::Add( TiXmlAttribute* addMe )
+{
+ assert( !Find( String( addMe->Name() ) ) ); // Shouldn't be multiply adding to the set.
+
+ addMe->next = &sentinel;
+ addMe->prev = sentinel.prev;
+
+ sentinel.prev->next = addMe;
+ sentinel.prev = addMe;
+}
+
+void TiXmlAttributeSet::Remove( TiXmlAttribute* removeMe )
+{
+ TiXmlAttribute* node;
+
+ for( node = sentinel.next; node != &sentinel; node = node->next )
+ {
+ if ( node == removeMe )
+ {
+ node->prev->next = node->next;
+ node->next->prev = node->prev;
+ node->next = 0;
+ node->prev = 0;
+ return;
+ }
+ }
+ assert( 0 ); // we tried to remove a non-linked attribute.
+}
+
+
+const TiXmlAttribute* TiXmlAttributeSet::Find( const String& name ) const
+{
+ for( const TiXmlAttribute* node = sentinel.next; node != &sentinel; node = node->next )
+ {
+ if ( node->name == name )
+ return node;
+ }
+ return 0;
+}
+
+
+
+const TiXmlAttribute* TiXmlAttributeSet::Find( const char* name ) const
+{
+ for( const TiXmlAttribute* node = sentinel.next; node != &sentinel; node = node->next )
+ {
+ if ( strcmp( node->name.c_str(), name ) == 0 )
+ return node;
+ }
+ return 0;
+}
+
+
+std::istream& operator>> (std::istream & in, TiXmlNode & base)
+{
+ String tag;
+ tag.reserve( 8 * 1000 );
+ base.StreamIn( &in, &tag );
+
+ base.Parse( tag.c_str(), 0, TIXML_DEFAULT_ENCODING );
+ return in;
+}
+
+
+std::ostream& operator<< (std::ostream & out, const TiXmlNode & base)
+{
+ TiXmlPrinter printer;
+ printer.SetStreamPrinting();
+ base.Accept( &printer );
+ out << printer.Str();
+
+ return out;
+}
+
+
+String& operator<< (String& out, const TiXmlNode& base )
+{
+ TiXmlPrinter printer;
+ printer.SetStreamPrinting();
+ base.Accept( &printer );
+ out.append( printer.Str() );
+
+ return out;
+}
+
+
+bool TiXmlPrinter::VisitEnter( const TiXmlDocument& )
+{
+ return true;
+}
+
+bool TiXmlPrinter::VisitExit( const TiXmlDocument& )
+{
+ return true;
+}
+
+bool TiXmlPrinter::VisitEnter( const TiXmlElement& element, const TiXmlAttribute* firstAttribute )
+{
+ DoIndent();
+ buffer += "<";
+ buffer += element.Value();
+
+ for( const TiXmlAttribute* attrib = firstAttribute; attrib; attrib = attrib->Next() )
+ {
+ buffer += " ";
+ attrib->Print( 0, 0, &buffer );
+ }
+
+ if ( !element.FirstChild() )
+ {
+ buffer += " />";
+ DoLineBreak();
+ }
+ else
+ {
+ buffer += ">";
+ if ( element.FirstChild()->ToText()
+ && element.LastChild() == element.FirstChild()
+ && element.FirstChild()->ToText()->CDATA() == false )
+ {
+ simpleTextPrint = true;
+ // no DoLineBreak()!
+ }
+ else
+ {
+ DoLineBreak();
+ }
+ }
+ ++depth;
+ return true;
+}
+
+
+bool TiXmlPrinter::VisitExit( const TiXmlElement& element )
+{
+ --depth;
+ if ( !element.FirstChild() )
+ {
+ // nothing.
+ }
+ else
+ {
+ if ( simpleTextPrint )
+ {
+ simpleTextPrint = false;
+ }
+ else
+ {
+ DoIndent();
+ }
+ buffer += "</";
+ buffer += element.Value();
+ buffer += ">";
+ DoLineBreak();
+ }
+ return true;
+}
+
+
+bool TiXmlPrinter::Visit( const TiXmlText& text )
+{
+ if ( text.CDATA() )
+ {
+ DoIndent();
+ buffer += "<![CDATA[";
+ buffer += text.Value();
+ buffer += "]]>";
+ DoLineBreak();
+ }
+ else if ( simpleTextPrint )
+ {
+ String str;
+ // sherm: Third argument says OK to keep quotes here
+ TiXmlBase::EncodeString( text.ValueStr(), &str, true );
+ buffer += str;
+ }
+ else
+ {
+ DoIndent();
+ String str;
+ // sherm: Third argument says OK to keep quotes here
+ TiXmlBase::EncodeString( text.ValueStr(), &str, true );
+ buffer += str;
+ DoLineBreak();
+ }
+ return true;
+}
+
+
+bool TiXmlPrinter::Visit( const TiXmlDeclaration& declaration )
+{
+ DoIndent();
+ declaration.Print( 0, 0, &buffer );
+ DoLineBreak();
+ return true;
+}
+
+
+bool TiXmlPrinter::Visit( const TiXmlComment& comment )
+{
+ DoIndent();
+ buffer += "<!--";
+ buffer += comment.Value();
+ buffer += "-->";
+ DoLineBreak();
+ return true;
+}
+
+
+bool TiXmlPrinter::Visit( const TiXmlUnknown& unknown )
+{
+ DoIndent();
+ buffer += "<";
+ buffer += unknown.Value();
+ buffer += ">";
+ DoLineBreak();
+ return true;
+}
+
+
+} // namespace SimTK
diff --git a/SimTKcommon/src/tinyxml.h b/SimTKcommon/src/tinyxml.h
new file mode 100644
index 0000000..b28506c
--- /dev/null
+++ b/SimTKcommon/src/tinyxml.h
@@ -0,0 +1,1749 @@
+/*
+www.sourceforge.net/projects/tinyxml
+Original code (2.0 and earlier )copyright (c) 2000-2006 Lee Thomason (www.grinninglizard.com)
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any
+damages arising from the use of this software.
+
+Permission is granted to anyone to use this software for any
+purpose, including commercial applications, and to alter it and
+redistribute it freely, subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must
+not claim that you wrote the original software. If you use this
+software in a product, an acknowledgment in the product documentation
+would be appreciated but is not required.
+
+2. Altered source versions must be plainly marked as such, and
+must not be misrepresented as being the original software.
+
+3. This notice may not be removed or altered from any source
+distribution.
+*/
+
+
+#ifndef TINYXML_INCLUDED
+#define TINYXML_INCLUDED
+
+#ifdef _MSC_VER
+#define _CRT_SECURE_NO_WARNINGS
+#pragma warning( push )
+#pragma warning( disable : 4530 )
+#pragma warning( disable : 4786 )
+#endif
+
+#include "SimTKcommon/internal/common.h"
+#include "SimTKcommon/internal/String.h"
+
+using SimTK::String;
+
+#include <string>
+#include <iostream>
+#include <sstream>
+
+#include <cctype>
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+#include <cassert>
+
+// Help out windows:
+#if defined( _DEBUG ) && !defined( DEBUG )
+#define DEBUG
+#endif
+
+// Deprecated library function hell. Compilers want to use the
+// new safe versions. This probably doesn't fully address the problem,
+// but it gets closer. There are too many compilers for me to fully
+// test. If you get compilation troubles, undefine TIXML_SAFE
+#define TIXML_SAFE
+
+#ifdef TIXML_SAFE
+ #if defined(_MSC_VER) && (_MSC_VER >= 1400 )
+ // Microsoft visual studio, version 2005 and higher.
+ #define TIXML_SNPRINTF _snprintf_s
+ #define TIXML_SNSCANF _snscanf_s
+ #define TIXML_SSCANF sscanf_s
+ #elif defined(_MSC_VER) && (_MSC_VER >= 1200 )
+ // Microsoft visual studio, version 6 and higher.
+ //#pragma message( "Using _sn* functions." )
+ #define TIXML_SNPRINTF _snprintf
+ #define TIXML_SNSCANF _snscanf
+ #define TIXML_SSCANF sscanf
+ #elif defined(__GNUC__) && (__GNUC__ >= 3 )
+ // GCC version 3 and higher.s
+ //#warning( "Using sn* functions." )
+ #define TIXML_SNPRINTF snprintf
+ #define TIXML_SNSCANF snscanf
+ #define TIXML_SSCANF sscanf
+ #else
+ #define TIXML_SSCANF sscanf
+ #endif
+#endif
+
+namespace SimTK {
+
+class TiXmlDocument;
+class TiXmlElement;
+class TiXmlComment;
+class TiXmlUnknown;
+class TiXmlAttribute;
+class TiXmlText;
+class TiXmlDeclaration;
+class TiXmlParsingData;
+
+const int TIXML_MAJOR_VERSION = 2;
+const int TIXML_MINOR_VERSION = 5;
+const int TIXML_PATCH_VERSION = 3;
+
+/* Internal structure for tracking location of items
+ in the XML file.
+*/
+struct TiXmlCursor
+{
+ TiXmlCursor() { Clear(); }
+ void Clear() { row = col = -1; }
+
+ int row; // 0 based.
+ int col; // 0 based.
+};
+
+
+
+//------------------------------------------------------------------------------
+// TinyXML VISITOR
+//------------------------------------------------------------------------------
+/** If you call the Accept() method, it requires being passed a TiXmlVisitor
+class to handle callbacks. For nodes that contain other nodes (Document,
+Element) you will get called with a VisitEnter/VisitExit pair. Nodes that are
+always leaves are simple called with Visit().
+
+If you return 'true' from a Visit method, recursive parsing will continue. If
+you return false, <b>no children of this node or its sibilings</b> will be
+Visited.
+
+All flavors of Visit methods have a default implementation that returns 'true'
+(continue visiting). You need to only override methods that are interesting to
+you.
+
+Generally Accept() is called on the TiXmlDocument, although all nodes suppert
+Visiting.
+
+You should never change the document from a callback.
+
+ at sa TiXmlNode::Accept()
+*/
+class TiXmlVisitor
+{
+public:
+ virtual ~TiXmlVisitor() {}
+
+ /// Visit a document.
+ virtual bool VisitEnter( const TiXmlDocument& /*doc*/ ) { return true; }
+ /// Visit a document.
+ virtual bool VisitExit( const TiXmlDocument& /*doc*/ ) { return true; }
+
+ /// Visit an element.
+ virtual bool VisitEnter( const TiXmlElement& /*element*/, const TiXmlAttribute* /*firstAttribute*/ ) { return true; }
+ /// Visit an element.
+ virtual bool VisitExit( const TiXmlElement& /*element*/ ) { return true; }
+
+ /// Visit a declaration
+ virtual bool Visit( const TiXmlDeclaration& /*declaration*/ ) { return true; }
+ /// Visit a text node
+ virtual bool Visit( const TiXmlText& /*text*/ ) { return true; }
+ /// Visit a comment node
+ virtual bool Visit( const TiXmlComment& /*comment*/ ) { return true; }
+ /// Visit an unknown node
+ virtual bool Visit( const TiXmlUnknown& /*unknown*/ ) { return true; }
+};
+
+// Only used by Attribute::Query functions
+enum
+{
+ TIXML_SUCCESS,
+ TIXML_NO_ATTRIBUTE,
+ TIXML_WRONG_TYPE
+};
+
+
+// Used by the parsing routines.
+enum TiXmlEncoding
+{
+ TIXML_ENCODING_UNKNOWN,
+ TIXML_ENCODING_UTF8,
+ TIXML_ENCODING_LEGACY
+};
+
+const TiXmlEncoding TIXML_DEFAULT_ENCODING = TIXML_ENCODING_UNKNOWN;
+
+
+//------------------------------------------------------------------------------
+// TinyXML BASE
+//------------------------------------------------------------------------------
+/** TiXmlBase is a base class for every class in TinyXml. It does little except
+to establish that TinyXml classes can be printed and provide some utility
+functions.
+
+In XML, the document and elements can contain other elements and other types of
+nodes.
+
+ at verbatim
+A Document can contain: Declaration( leaf )
+ Element (container or leaf)
+ Comment (leaf)
+ Unknown (leaf)
+
+
+An Element can contain: Element (container or leaf)
+ Text (leaf)
+ Attributes (not on tree)
+ Comment (leaf)
+ Unknown (leaf)
+
+A Declaration contains: Three predefined attributes (not on tree, handled
+ separately from Element Attributes)
+ at endverbatim
+*/
+class TiXmlBase
+{
+ friend class TiXmlNode;
+ friend class TiXmlElement;
+ friend class TiXmlDocument;
+
+public:
+ TiXmlBase() : userData(0) {}
+ virtual ~TiXmlBase() {}
+
+ /** All TinyXml classes can print themselves to a filestream
+ or the string class. Either or both cfile and str can be null.
+
+ This is a formatted print, and will insert
+ tabs and newlines.
+
+ (For an unformatted stream, use the << operator.)
+ */
+ virtual void Print( FILE* cfile, int depth ) const = 0;
+
+ /** The world does not agree on whether white space should be kept or
+ not. In order to make everyone happy, these global, static functions
+ are provided to set whether or not TinyXml will condense all white space
+ into a single space or not. The default is to condense. Note changing this
+ value is not thread safe.
+ */
+ static void SetCondenseWhiteSpace( bool condense ) { condenseWhiteSpace = condense; }
+
+ /// Return the current white space setting.
+ static bool IsWhiteSpaceCondensed() { return condenseWhiteSpace; }
+
+ /** Return the position, in the original source file, of this node or
+ attribute. The row and column are 1-based. (That is the first row and
+ first column is 1,1). If the returns values are 0 or less, then the
+ parser does not have a row and column value.
+
+ Generally, the row and column value will be set when the
+ TiXmlDocument::Load(), TiXmlDocument::LoadFile(), or any
+ TiXmlNode::Parse() is called. It will NOT be set when the DOM was created
+ from operator>>.
+
+ The values reflect the initial load. Once the DOM is modified
+ programmatically (by adding or changing nodes and attributes) the new
+ values will NOT update to reflect changes in the document.
+
+ There is a minor performance cost to computing the row and column.
+ Computation can be disabled if TiXmlDocument::SetTabSize() is called with
+ 0 as the value.
+
+ @sa TiXmlDocument::SetTabSize() **/
+ int Row() const { return location.row + 1; }
+ int Column() const { return location.col + 1; } ///< See Row()
+
+ void SetUserData( void* user ) { userData = user; } ///< Set a pointer to arbitrary user data.
+ void* GetUserData() { return userData; } ///< Get a pointer to arbitrary user data.
+ const void* GetUserData() const { return userData; } ///< Get a pointer to arbitrary user data.
+
+ // Table that returns, for a given lead byte, the total number of bytes
+ // in the UTF-8 sequence.
+ static const int utf8ByteTable[256];
+
+ virtual const char* Parse( const char* p,
+ TiXmlParsingData* data,
+ TiXmlEncoding encoding /*= TIXML_ENCODING_UNKNOWN */ ) = 0;
+
+ /** Expands entities in a string. Note this should not contain the tag's
+ '<', '>', etc, or they will be transformed into entities!
+ sherm: added the option to preserve single and double quotes which
+ are OK in text but not tags **/
+ static void EncodeString( const String& str, String* out,
+ bool keepQuotes=false );
+
+ enum
+ {
+ TIXML_NO_ERROR = 0,
+ TIXML_ERROR,
+ TIXML_ERROR_OPENING_FILE,
+ TIXML_ERROR_OUT_OF_MEMORY,
+ TIXML_ERROR_PARSING_ELEMENT,
+ TIXML_ERROR_FAILED_TO_READ_ELEMENT_NAME,
+ TIXML_ERROR_READING_ELEMENT_VALUE,
+ TIXML_ERROR_READING_ATTRIBUTES,
+ TIXML_ERROR_PARSING_EMPTY,
+ TIXML_ERROR_READING_END_TAG,
+ TIXML_ERROR_PARSING_UNKNOWN,
+ TIXML_ERROR_PARSING_COMMENT,
+ TIXML_ERROR_PARSING_DECLARATION,
+ TIXML_ERROR_DOCUMENT_EMPTY,
+ TIXML_ERROR_EMBEDDED_NULL,
+ TIXML_ERROR_PARSING_CDATA,
+ TIXML_ERROR_DOCUMENT_TOP_ONLY,
+
+ TIXML_ERROR_STRING_COUNT
+ };
+
+protected:
+
+ static const char* SkipWhiteSpace( const char*, TiXmlEncoding encoding );
+ inline static bool IsWhiteSpace( char c )
+ {
+ return ( isspace( (unsigned char) c ) || c == '\n' || c == '\r' );
+ }
+ inline static bool IsWhiteSpace( int c )
+ {
+ if ( c < 256 )
+ return IsWhiteSpace( (char) c );
+ return false; // Again, only truly correct for English/Latin...but usually works.
+ }
+
+ static bool StreamWhiteSpace( std::istream * in, String * tag );
+ static bool StreamTo( std::istream * in, int character, String * tag );
+
+ /* Reads an XML name into the string provided. Returns
+ a pointer just past the last character of the name,
+ or 0 if the function has an error.
+ */
+ static const char* ReadName( const char* p, String* name, TiXmlEncoding encoding );
+
+ /* Reads text. Returns a pointer past the given end tag.
+ Wickedly complex options, but it keeps the (sensitive) code in one place.
+ */
+ static const char* ReadText
+ (const char* in, // where to start
+ String* text, // the string read
+ bool ignoreWhiteSpace, // whether to keep the white space
+ const char* endTag, // what ends this text
+ bool ignoreCase, // whether to ignore case in the end tag
+ TiXmlEncoding encoding); // the current encoding
+
+ // If an entity has been found, transform it into a character.
+ static const char* GetEntity( const char* in, char* value, int* length, TiXmlEncoding encoding );
+
+ // Get a character, while interpreting entities.
+ // The length can be from 0 to 4 bytes.
+ inline static const char* GetChar( const char* p, char* _value, int* length, TiXmlEncoding encoding )
+ {
+ assert( p );
+ if ( encoding == TIXML_ENCODING_UTF8 )
+ {
+ *length = utf8ByteTable[ *((const unsigned char*)p) ];
+ assert( *length >= 0 && *length < 5 );
+ }
+ else
+ {
+ *length = 1;
+ }
+
+ if ( *length == 1 )
+ {
+ if ( *p == '&' )
+ return GetEntity( p, _value, length, encoding );
+ *_value = *p;
+ return p+1;
+ }
+ else if ( *length )
+ {
+ //strncpy( _value, p, *length ); // lots of compilers don't like this function (unsafe),
+ // and the null terminator isn't needed
+ for( int i=0; p[i] && i<*length; ++i ) {
+ _value[i] = p[i];
+ }
+ return p + (*length);
+ }
+ else
+ {
+ // Not valid text.
+ return 0;
+ }
+ }
+
+ // Return true if the next characters in the stream are any of the endTag sequences.
+ // Ignore case only works for english, and should only be relied on when comparing
+ // to English words: StringEqual( p, "version", true ) is fine.
+ static bool StringEqual( const char* p,
+ const char* endTag,
+ bool ignoreCase,
+ TiXmlEncoding encoding );
+
+ static const char* errorString[ TIXML_ERROR_STRING_COUNT ];
+
+ TiXmlCursor location;
+
+ /// Field containing a generic user pointer
+ void* userData;
+
+ // None of these methods are reliable for any language except English.
+ // Good for approximation, not great for accuracy.
+ static int IsAlpha( unsigned char anyByte, TiXmlEncoding encoding );
+ static int IsAlphaNum( unsigned char anyByte, TiXmlEncoding encoding );
+ inline static int ToLower( int v, TiXmlEncoding encoding )
+ {
+ if ( encoding == TIXML_ENCODING_UTF8 )
+ {
+ if ( v < 128 ) return tolower( v );
+ return v;
+ }
+ else
+ {
+ return tolower( v );
+ }
+ }
+ static void ConvertUTF32ToUTF8( unsigned long input, char* output, int* length );
+
+private:
+ TiXmlBase( const TiXmlBase& ); // not implemented.
+ void operator=( const TiXmlBase& base ); // not allowed.
+
+ struct Entity
+ {
+ const char* str;
+ unsigned int strLength;
+ char chr;
+ };
+ enum
+ {
+ NUM_ENTITY = 5,
+ MAX_ENTITY_LENGTH = 6
+
+ };
+ static Entity entity[ NUM_ENTITY ];
+ static bool condenseWhiteSpace;
+};
+
+
+
+//------------------------------------------------------------------------------
+// TinyXML NODE
+//------------------------------------------------------------------------------
+/** The parent class for everything in the Document Object Model.
+(Except for attributes).
+Nodes have siblings, a parent, and children. A node can be
+in a document, or stand on its own. The type of a TiXmlNode
+can be queried, and it can be cast to its more defined type. **/
+class TiXmlNode : public TiXmlBase
+{
+ friend class TiXmlDocument;
+ friend class TiXmlElement;
+
+public:
+ /** An input stream operator, for every class. Tolerant of newlines and
+ formatting, but doesn't expect them. **/
+ friend std::istream& operator >> (std::istream& in, TiXmlNode& base);
+
+ /** An output stream operator, for every class. Note that this outputs
+ without any newlines or formatting, as opposed to Print(), which
+ includes tabs and new lines.
+
+ The operator<< and operator>> are not completely symmetric. Writing
+ a node to a stream is very well defined. You'll get a nice stream
+ of output, without any extra whitespace or newlines.
+
+ But reading is not as well defined. (As it always is.) If you create
+ a TiXmlElement (for example) and read that from an input stream,
+ the text needs to define an element or junk will result. This is
+ true of all input streams, but it's worth keeping in mind.
+
+ A TiXmlDocument will read nodes until it reads a root element, and
+ all the children of that root element. **/
+ friend std::ostream& operator<< (std::ostream& out, const TiXmlNode& base);
+
+ /** Appends the XML node or attribute to a String. **/
+ friend String& operator<< (String& out, const TiXmlNode& base );
+
+ /** The types of XML nodes supported by TinyXml. (All the
+ unsupported types are picked up by UNKNOWN.) **/
+ enum NodeType
+ {
+ DOCUMENT,
+ ELEMENT,
+ COMMENT,
+ UNKNOWN,
+ TEXT,
+ DECLARATION
+ };
+
+ static const char* nameOfNodeType(NodeType n) {
+ switch(n) {
+ case DOCUMENT: return "Document";
+ case ELEMENT: return "Element";
+ case COMMENT: return "Comment";
+ case UNKNOWN: return "Unknown";
+ case TEXT: return "Text";
+ case DECLARATION: return "Declaration";
+ };
+ return "UNRECOGNIZED NodeType";
+ }
+
+ virtual ~TiXmlNode();
+
+ /** The meaning of 'value' changes for the specific type of
+ TiXmlNode.
+ @verbatim
+ Document: filename of the xml file
+ Element: name of the element
+ Comment: the comment text
+ Unknown: the tag contents
+ Text: the text string
+ @endverbatim
+
+ The subclasses will wrap this function. **/
+ const char *Value() const { return value.c_str (); }
+
+ /** Return Value() as a String. If you only use STL,
+ this is more efficient than calling Value().
+ Only available in STL mode.
+ */
+ const String& ValueStr() const { return value; }
+ String& UpdValueStr() { return value; }
+
+ /** Changes the value of the node. Defined as:
+ @verbatim
+ Document: filename of the xml file
+ Element: name of the element (that is, the tag)
+ Comment: the comment text
+ Unknown: the tag contents
+ Text: the text string
+ @endverbatim **/
+ void SetValue(const char * _value) { value = _value;}
+
+ /// STL String form.
+ void SetValue( const String& _value ) { value = _value; }
+
+ /// Delete all the children of this node. Does not affect 'this'.
+ void Clear();
+
+ /// One step up the DOM.
+ TiXmlNode* Parent() { return parent; }
+ const TiXmlNode* Parent() const { return parent; }
+
+ const TiXmlNode* FirstChild() const { return firstChild; } ///< The first child of this node. Will be null if there are no children.
+ TiXmlNode* FirstChild() { return firstChild; }
+ const TiXmlNode* FirstChild( const char * value ) const; ///< The first child of this node with the matching 'value'. Will be null if none found.
+ /// The first child of this node with the matching 'value'. Will be null if none found.
+ TiXmlNode* FirstChild( const char * _value ) {
+ // Call through to the const version - safe since nothing is changed. Exiting syntax: cast this to a const (always safe)
+ // call the method, cast the return back to non-const.
+ return const_cast< TiXmlNode* > ((const_cast< const TiXmlNode* >(this))->FirstChild( _value ));
+ }
+ const TiXmlNode* LastChild() const { return lastChild; } /// The last child of this node. Will be null if there are no children.
+ TiXmlNode* LastChild() { return lastChild; }
+
+ const TiXmlNode* LastChild( const char * value ) const; /// The last child of this node matching 'value'. Will be null if there are no children.
+ TiXmlNode* LastChild( const char * _value ) {
+ return const_cast< TiXmlNode* > ((const_cast< const TiXmlNode* >(this))->LastChild( _value ));
+ }
+
+ const TiXmlNode* FirstChild( const String& _value ) const { return FirstChild (_value.c_str ()); }
+ TiXmlNode* FirstChild( const String& _value ) { return FirstChild (_value.c_str ()); }
+ const TiXmlNode* LastChild( const String& _value ) const { return LastChild (_value.c_str ()); }
+ TiXmlNode* LastChild( const String& _value ) { return LastChild (_value.c_str ()); }
+
+ /** An alternate way to walk the children of a node.
+ One way to iterate over nodes is:
+ @verbatim
+ for( child = parent->FirstChild(); child; child = child->NextSibling() )
+ @endverbatim
+
+ IterateChildren does the same thing with the syntax:
+ @verbatim
+ child = 0;
+ while( child = parent->IterateChildren( child ) )
+ @endverbatim
+
+ IterateChildren takes the previous child as input and finds
+ the next one. If the previous child is null, it returns the
+ first. IterateChildren will return null when done.
+ */
+ const TiXmlNode* IterateChildren( const TiXmlNode* previous ) const;
+ TiXmlNode* IterateChildren( const TiXmlNode* previous ) {
+ return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->IterateChildren( previous ) );
+ }
+
+ /// This flavor of IterateChildren searches for children with a particular 'value'
+ const TiXmlNode* IterateChildren( const char * value, const TiXmlNode* previous ) const;
+ TiXmlNode* IterateChildren( const char * _value, const TiXmlNode* previous ) {
+ return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->IterateChildren( _value, previous ) );
+ }
+
+ const TiXmlNode* IterateChildren( const String& _value, const TiXmlNode* previous ) const { return IterateChildren (_value.c_str (), previous); }
+ TiXmlNode* IterateChildren( const String& _value, const TiXmlNode* previous ) { return IterateChildren (_value.c_str (), previous); }
+
+ /** Add a new node related to this. Adds a child past the LastChild.
+ Returns a pointer to the new object or NULL if an error occured.
+ */
+ TiXmlNode* InsertEndChild( const TiXmlNode& addThis );
+
+
+ /** Add a new node related to this. Adds a child past the LastChild.
+
+ NOTE: the node to be added is passed by pointer, and will be
+ henceforth owned (and deleted) by tinyXml. This method is efficient
+ and avoids an extra copy, but should be used with care as it
+ uses a different memory model than the other insert functions.
+
+ @sa InsertEndChild
+ */
+ TiXmlNode* LinkEndChild( TiXmlNode* addThis );
+
+ /** Same but adds node to the beginning of the child list. **/
+ TiXmlNode* LinkBeginChild( TiXmlNode* prependThis );
+
+ /** Same but links node just before a given node. **/
+ TiXmlNode* LinkBeforeChild( TiXmlNode* beforeThis, TiXmlNode* prependThis );
+
+ /** Same but links node just after a given node. **/
+ TiXmlNode* LinkAfterChild( TiXmlNode* afterThis, TiXmlNode* addThis );
+
+ /** Add a new node related to this. Adds a child before the specified child.
+ Returns a pointer to the new object or NULL if an error occured.
+ */
+ TiXmlNode* InsertBeforeChild( TiXmlNode* beforeThis, const TiXmlNode& addThis );
+
+ /** Add a new node related to this. Adds a child after the specified child.
+ Returns a pointer to the new object or NULL if an error occured.
+ */
+ TiXmlNode* InsertAfterChild( TiXmlNode* afterThis, const TiXmlNode& addThis );
+
+ /** Replace a child of this node.
+ Returns a pointer to the new object or NULL if an error occured.
+ */
+ TiXmlNode* ReplaceChild( TiXmlNode* replaceThis, const TiXmlNode& withThis );
+
+ /** Disconnect the indicated child and all its children from the Xml
+ document but don't delete it. The disconnected node pointer (same as
+ the argument) is returned. You are responsible for deleting the node
+ yourself after this. **/
+ TiXmlNode* DisconnectChild( TiXmlNode* disconnectThis );
+
+ /** This disconnects the indicated child node and then deletes it. **/
+ bool RemoveChild( TiXmlNode* removeThis );
+
+ /// Navigate to a sibling node.
+ const TiXmlNode* PreviousSibling() const { return prev; }
+ TiXmlNode* PreviousSibling() { return prev; }
+
+ /// Navigate to a sibling node.
+ const TiXmlNode* PreviousSibling( const char * ) const;
+ TiXmlNode* PreviousSibling( const char *_prev ) {
+ return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->PreviousSibling( _prev ) );
+ }
+
+ const TiXmlNode* PreviousSibling( const String& _value ) const { return PreviousSibling (_value.c_str ()); }
+ TiXmlNode* PreviousSibling( const String& _value ) { return PreviousSibling (_value.c_str ()); }
+ const TiXmlNode* NextSibling( const String& _value) const { return NextSibling (_value.c_str ()); }
+ TiXmlNode* NextSibling( const String& _value) { return NextSibling (_value.c_str ()); }
+
+ /// Navigate to a sibling node.
+ const TiXmlNode* NextSibling() const { return next; }
+ TiXmlNode* NextSibling() { return next; }
+
+ /// Navigate to a sibling node with the given 'value'.
+ const TiXmlNode* NextSibling( const char * ) const;
+ TiXmlNode* NextSibling( const char* _next ) {
+ return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->NextSibling( _next ) );
+ }
+
+ /** Convenience function to get through elements.
+ Calls NextSibling and ToElement. Will skip all non-Element
+ nodes. Returns 0 if there is not another element. **/
+ const TiXmlElement* NextSiblingElement() const;
+ TiXmlElement* NextSiblingElement() {
+ return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->NextSiblingElement() );
+ }
+
+ /** Convenience function to get through elements.
+ Calls NextSibling and ToElement. Will skip all non-Element
+ nodes. Returns 0 if there is not another element. **/
+ const TiXmlElement* NextSiblingElement( const char * ) const;
+ TiXmlElement* NextSiblingElement( const char *_next ) {
+ return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->NextSiblingElement( _next ) );
+ }
+
+ const TiXmlElement* NextSiblingElement( const String& _value) const { return NextSiblingElement (_value.c_str ()); }
+ TiXmlElement* NextSiblingElement( const String& _value) { return NextSiblingElement (_value.c_str ()); }
+
+ /// Convenience function to get through elements.
+ const TiXmlElement* FirstChildElement() const;
+ TiXmlElement* FirstChildElement() {
+ return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->FirstChildElement() );
+ }
+
+ /// Convenience function to get through elements.
+ const TiXmlElement* FirstChildElement( const char * _value ) const;
+ TiXmlElement* FirstChildElement( const char * _value ) {
+ return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->FirstChildElement( _value ) );
+ }
+
+ const TiXmlElement* FirstChildElement( const String& _value ) const { return FirstChildElement (_value.c_str ()); }
+ TiXmlElement* FirstChildElement( const String& _value ) { return FirstChildElement (_value.c_str ()); }
+
+
+ /** Convenience function to get through elements.
+ Calls NextSibling and ToElement. Will skip all non-Element
+ nodes. Returns 0 if there is not another element. **/
+ const TiXmlElement* PreviousSiblingElement() const;
+ TiXmlElement* PreviousSiblingElement() {
+ return const_cast< TiXmlElement* >(
+ (const_cast< const TiXmlNode* >(this))->
+ PreviousSiblingElement() );
+ }
+
+ /** Convenience function to get through elements.
+ Calls NextSibling and ToElement. Will skip all non-Element
+ nodes. Returns 0 if there is not another element. **/
+ const TiXmlElement* PreviousSiblingElement( const char * ) const;
+ TiXmlElement* PreviousSiblingElement( const char *_next ) {
+ return const_cast< TiXmlElement* >(
+ (const_cast< const TiXmlNode* >(this))->
+ PreviousSiblingElement( _next ) );
+ }
+
+ const TiXmlElement* PreviousSiblingElement( const String& _value) const
+ { return PreviousSiblingElement (_value.c_str ()); }
+ TiXmlElement* PreviousSiblingElement( const String& _value)
+ { return PreviousSiblingElement (_value.c_str ()); }
+
+ /// Convenience function to get through elements.
+ const TiXmlElement* LastChildElement() const;
+ TiXmlElement* LastChildElement() {
+ return const_cast< TiXmlElement* >(
+ (const_cast< const TiXmlNode* >(this))->
+ LastChildElement() );
+ }
+
+ /// Convenience function to get through elements.
+ const TiXmlElement* LastChildElement( const char * _value ) const;
+ TiXmlElement* LastChildElement( const char * _value ) {
+ return const_cast< TiXmlElement* >(
+ (const_cast< const TiXmlNode* >(this))->
+ LastChildElement( _value ) );
+ }
+
+ const TiXmlElement* LastChildElement( const String& _value ) const
+ { return LastChildElement (_value.c_str ()); }
+ TiXmlElement* LastChildElement( const String& _value )
+ { return LastChildElement (_value.c_str ()); }
+
+
+ /** Query the type (as an enumerated value, above) of this node.
+ The possible types are: DOCUMENT, ELEMENT, COMMENT,
+ UNKNOWN, TEXT, and DECLARATION.
+ */
+ NodeType Type() const { return type; }
+ const char* TypeName() const {return nameOfNodeType(Type());}
+ String TypeNameStr() const {return String(TypeName());}
+
+ /** Return a pointer to the Document this node lives in.
+ Returns null if not in a document.
+ */
+ const TiXmlDocument* GetDocument() const;
+ TiXmlDocument* GetDocument() {
+ return const_cast<TiXmlDocument*>
+ ((const_cast<const TiXmlNode*>(this))->GetDocument());
+ }
+
+ /// Returns true if this node has no children.
+ bool NoChildren() const { return !firstChild; }
+
+ virtual const TiXmlDocument* ToDocument() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual const TiXmlElement* ToElement() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual const TiXmlComment* ToComment() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual const TiXmlUnknown* ToUnknown() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual const TiXmlText* ToText() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual const TiXmlDeclaration* ToDeclaration() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+
+ virtual TiXmlDocument* ToDocument() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual TiXmlElement* ToElement() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual TiXmlComment* ToComment() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual TiXmlUnknown* ToUnknown() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual TiXmlText* ToText() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual TiXmlDeclaration* ToDeclaration() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+
+ /** Create an exact duplicate of this node and return it. The memory must be deleted
+ by the caller. **/
+ virtual TiXmlNode* Clone() const = 0;
+
+ /** Accept a hierchical visit the nodes in the TinyXML DOM. Every node in the
+ XML tree will be conditionally visited and the host will be called back
+ via the TiXmlVisitor interface.
+
+ This is essentially a SAX interface for TinyXML. (Note however it doesn't re-parse
+ the XML for the callbacks, so the performance of TinyXML is unchanged by using this
+ interface versus any other.)
+
+ The interface has been based on ideas from:
+
+ - http://www.saxproject.org/
+ - http://c2.com/cgi/wiki?HierarchicalVisitorPattern
+
+ Which are both good references for "visiting".
+
+ An example of using Accept():
+ @verbatim
+ TiXmlPrinter printer;
+ tinyxmlDoc.Accept( &printer );
+ const char* xmlcstr = printer.CStr();
+ @endverbatim
+ */
+ virtual bool Accept( TiXmlVisitor* visitor ) const = 0;
+
+protected:
+ TiXmlNode( NodeType _type );
+
+ // Copy to the allocated object. Shared functionality between Clone, Copy constructor,
+ // and the assignment operator.
+ void CopyTo( TiXmlNode* target ) const;
+
+
+ // The real work of the input operator.
+ virtual void StreamIn( std::istream* in, String* tag ) = 0;
+
+ // Figure out what is at *p, and parse it. Returns null if it is not an xml node.
+ TiXmlNode* Identify( const char* start, TiXmlEncoding encoding );
+
+ TiXmlNode* parent;
+ NodeType type;
+
+ TiXmlNode* firstChild;
+ TiXmlNode* lastChild;
+
+ String value;
+
+ TiXmlNode* prev;
+ TiXmlNode* next;
+
+private:
+ TiXmlNode( const TiXmlNode& ); // not implemented.
+ void operator=( const TiXmlNode& base ); // not allowed.
+};
+
+
+
+//------------------------------------------------------------------------------
+// TinyXML ATTRIBUTE
+//------------------------------------------------------------------------------
+/** An attribute is a name-value pair. Elements have an arbitrary number of
+attributes, each with a unique name.
+
+ at note The attributes are not TiXmlNodes, since they are not part of the tinyXML
+document object model. There are other suggested ways to look at this problem.
+**/
+class TiXmlAttribute : public TiXmlBase
+{
+ friend class TiXmlAttributeSet;
+
+public:
+ /// Construct an empty attribute.
+ TiXmlAttribute() : TiXmlBase()
+ {
+ document = 0;
+ prev = next = 0;
+ }
+
+ /// String constructor.
+ TiXmlAttribute( const String& _name, const String& _value )
+ {
+ name = _name;
+ value = _value;
+ document = 0;
+ prev = next = 0;
+ }
+
+ /// Construct an attribute with a name and value.
+ TiXmlAttribute( const char * _name, const char * _value )
+ {
+ name = _name;
+ value = _value;
+ document = 0;
+ prev = next = 0;
+ }
+
+ const char* Name() const { return name.c_str(); } ///< Return the name of this attribute.
+ const char* Value() const { return value.c_str(); } ///< Return the value of this attribute.
+
+ // Get the String representation
+ const String& NameStr() const { return name; }
+ const String& ValueStr() const { return value; } ///< Return the value of this attribute.
+
+ int IntValue() const; ///< Return the value of this attribute, converted to an integer.
+ double DoubleValue() const; ///< Return the value of this attribute, converted to a double.
+
+
+ /** QueryIntValue examines the value string. It is an alternative to the
+ IntValue() method with richer error checking.
+ If the value is an integer, it is stored in 'value' and
+ the call returns TIXML_SUCCESS. If it is not
+ an integer, it returns TIXML_WRONG_TYPE.
+
+ A specialized but useful call. Note that for success it returns 0,
+ which is the opposite of almost all other TinyXml calls. **/
+ int QueryIntValue( int* _value ) const;
+ /** QueryDoubleValue examines the value string. See QueryIntValue(). */
+ int QueryDoubleValue( double* _value ) const;
+
+ void SetName( const char* _name ) { name = _name; } ///< Set the name of this attribute.
+ void SetValue( const char* _value ) { value = _value; } ///< Set the value.
+
+ void SetIntValue( int _value ); ///< Set the value from an integer.
+ void SetDoubleValue( double _value ); ///< Set the value from a double.
+
+ void SetName( const String& _name ) { name = _name; }
+ void SetValue( const String& _value ) { value = _value; }
+
+ /// Get the next sibling attribute in the DOM. Returns null at end.
+ const TiXmlAttribute* Next() const;
+ TiXmlAttribute* Next() {
+ return const_cast< TiXmlAttribute* >( (const_cast< const TiXmlAttribute* >(this))->Next() );
+ }
+
+ /// Get the previous sibling attribute in the DOM. Returns null at
+ /// beginning.
+ const TiXmlAttribute* Previous() const;
+ /// Writable form.
+ TiXmlAttribute* Previous() {
+ return const_cast< TiXmlAttribute* >( (const_cast< const TiXmlAttribute* >(this))->Previous() );
+ }
+
+ bool operator==( const TiXmlAttribute& rhs ) const { return rhs.name == name; }
+ bool operator<( const TiXmlAttribute& rhs ) const { return name < rhs.name; }
+ bool operator>( const TiXmlAttribute& rhs ) const { return name > rhs.name; }
+
+ /* Attribute parsing starts: first letter of the name
+ returns: the next char after the value end quote
+ */
+ virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding );
+
+ // Prints this Attribute to a FILE stream.
+ virtual void Print( FILE* cfile, int depth ) const {
+ Print( cfile, depth, 0 );
+ }
+ void Print( FILE* cfile, int depth, String* str ) const;
+
+ TiXmlDocument* GetDocument() {return document;}
+ const TiXmlDocument* GetDocument() const {return document;}
+
+ // [internal use]
+ // Set the document pointer so the attribute can report errors.
+ void SetDocument( TiXmlDocument* doc ) { document = doc; }
+
+private:
+ TiXmlAttribute( const TiXmlAttribute& ); // not implemented.
+ void operator=( const TiXmlAttribute& base ); // not allowed.
+
+ // A pointer back to a document, for error reporting.
+ TiXmlDocument* document;
+ String name;
+ String value;
+ TiXmlAttribute* prev;
+ TiXmlAttribute* next;
+};
+
+
+/* A class used to manage a group of attributes.
+ It is only used internally, both by the ELEMENT and the DECLARATION.
+
+ The set can be changed transparent to the Element and Declaration
+ classes that use it, but NOT transparent to the Attribute
+ which has to implement a next() and previous() method. Which makes
+ it a bit problematic and prevents the use of STL.
+
+ This version is implemented with circular lists because:
+ - I like circular lists
+ - it demonstrates some independence from the (typical) doubly linked list.
+*/
+class TiXmlAttributeSet
+{
+public:
+ TiXmlAttributeSet();
+ ~TiXmlAttributeSet();
+
+ void Add( TiXmlAttribute* attribute );
+ void Remove( TiXmlAttribute* attribute );
+
+ const TiXmlAttribute* First() const { return ( sentinel.next == &sentinel ) ? 0 : sentinel.next; }
+ TiXmlAttribute* First() { return ( sentinel.next == &sentinel ) ? 0 : sentinel.next; }
+ const TiXmlAttribute* Last() const { return ( sentinel.prev == &sentinel ) ? 0 : sentinel.prev; }
+ TiXmlAttribute* Last() { return ( sentinel.prev == &sentinel ) ? 0 : sentinel.prev; }
+
+ const TiXmlAttribute* Find( const char* _name ) const;
+ TiXmlAttribute* Find( const char* _name ) {
+ return const_cast< TiXmlAttribute* >( (const_cast< const TiXmlAttributeSet* >(this))->Find( _name ) );
+ }
+
+ const TiXmlAttribute* Find( const String& _name ) const;
+ TiXmlAttribute* Find( const String& _name ) {
+ return const_cast< TiXmlAttribute* >( (const_cast< const TiXmlAttributeSet* >(this))->Find( _name ) );
+ }
+
+
+private:
+ //*ME: Because of hidden/disabled copy-construktor in TiXmlAttribute
+ // (sentinel-element),
+ //*ME: this class must be also use a hidden/disabled copy-constructor !!!
+ TiXmlAttributeSet( const TiXmlAttributeSet& ); // not allowed
+ void operator=( const TiXmlAttributeSet& ); // not allowed (as TiXmlAttribute)
+
+ TiXmlAttribute sentinel;
+};
+
+
+
+//------------------------------------------------------------------------------
+// TinyXML ELEMENT
+//------------------------------------------------------------------------------
+/** The element is a container class. It has a value, the element name,
+ and can contain other elements, text, comments, and unknowns.
+ Elements also contain an arbitrary number of attributes.
+*/
+class TiXmlElement : public TiXmlNode
+{
+public:
+ /// Construct an element.
+ TiXmlElement (const char * in_value);
+
+ /// String constructor.
+ TiXmlElement( const String& _value );
+
+ TiXmlElement( const TiXmlElement& );
+
+ void operator=( const TiXmlElement& base );
+
+ virtual ~TiXmlElement();
+
+ /** Given an attribute name, Attribute() returns the value
+ for the attribute of that name, or null if none exists.
+ */
+ const char* Attribute( const char* name ) const;
+
+ /** Given an attribute name, Attribute() returns the value
+ for the attribute of that name, or null if none exists.
+ If the attribute exists and can be converted to an integer,
+ the integer value will be put in the return 'i', if 'i'
+ is non-null.
+ */
+ const char* Attribute( const char* name, int* i ) const;
+
+ /** Given an attribute name, Attribute() returns the value
+ for the attribute of that name, or null if none exists.
+ If the attribute exists and can be converted to an double,
+ the double value will be put in the return 'd', if 'd'
+ is non-null.
+ */
+ const char* Attribute( const char* name, double* d ) const;
+
+ /** QueryIntAttribute examines the attribute - it is an alternative to the
+ Attribute() method with richer error checking.
+ If the attribute is an integer, it is stored in 'value' and
+ the call returns TIXML_SUCCESS. If it is not
+ an integer, it returns TIXML_WRONG_TYPE. If the attribute
+ does not exist, then TIXML_NO_ATTRIBUTE is returned.
+ */
+ int QueryIntAttribute( const char* name, int* _value ) const;
+ /// QueryDoubleAttribute examines the attribute - see QueryIntAttribute().
+ int QueryDoubleAttribute( const char* name, double* _value ) const;
+ /// QueryFloatAttribute examines the attribute - see QueryIntAttribute().
+ int QueryFloatAttribute( const char* name, float* _value ) const {
+ double d;
+ int result = QueryDoubleAttribute( name, &d );
+ if ( result == TIXML_SUCCESS ) {
+ *_value = (float)d;
+ }
+ return result;
+ }
+
+ /** Template form of the attribute query which will try to read the
+ attribute into the specified type. Very easy, very powerful, but
+ be careful to make sure to call this with the correct type.
+
+ NOTE: This method doesn't work correctly for 'string' types.
+
+ @return TIXML_SUCCESS, TIXML_WRONG_TYPE, or TIXML_NO_ATTRIBUTE
+ */
+ template< typename T > int QueryValueAttribute
+ ( const String& name, T* outValue ) const
+ {
+ const TiXmlAttribute* node = attributeSet.Find( name );
+ if ( !node )
+ return TIXML_NO_ATTRIBUTE;
+
+ std::stringstream sstream( node->ValueStr() );
+ sstream >> *outValue;
+ if ( !sstream.fail() )
+ return TIXML_SUCCESS;
+ return TIXML_WRONG_TYPE;
+ }
+ /*
+ This is - in theory - a bug fix for "QueryValueAtribute returns truncated
+ String" but template specialization is hard to get working cross-compiler.
+ Leaving the bug for now.
+
+ // The above will fail for String because the space character is used as a
+ // separator. Specialize for strings. Bug [ 1695429 ] QueryValueAtribute
+ // returns truncated String
+ template<> int QueryValueAttribute( const String& name, String* outValue ) const
+ {
+ const TiXmlAttribute* node = attributeSet.Find( name );
+ if ( !node )
+ return TIXML_NO_ATTRIBUTE;
+ *outValue = node->ValueStr();
+ return TIXML_SUCCESS;
+ }
+ */
+
+ /** Sets an attribute of name to a given value. The attribute
+ will be created if it does not exist, or changed if it does. **/
+ void SetAttribute( const char* name, const char * _value );
+
+ const String* Attribute( const String& name ) const;
+ const String* Attribute( const String& name, int* i ) const;
+ const String* Attribute( const String& name, double* d ) const;
+ int QueryIntAttribute( const String& name, int* _value ) const;
+ int QueryDoubleAttribute( const String& name, double* _value ) const;
+
+ void SetAttribute( const String& name, const String& _value );
+ void SetAttribute( const String& name, int _value );
+
+ /** Sets an attribute of name to a given value. The attribute
+ will be created if it does not exist, or changed if it does. **/
+ void SetAttribute( const char * name, int value );
+
+ /** Sets an attribute of name to a given value. The attribute
+ will be created if it does not exist, or changed if it does. **/
+ void SetDoubleAttribute( const char * name, double value );
+
+ /** Deletes an attribute with the given name. **/
+ void RemoveAttribute( const char * name );
+ void RemoveAttribute( const String& name )
+ { RemoveAttribute (name.c_str ()); }
+
+ const TiXmlAttribute* FirstAttribute() const { return attributeSet.First(); } ///< Access the first attribute in this element.
+ TiXmlAttribute* FirstAttribute() { return attributeSet.First(); }
+ const TiXmlAttribute* LastAttribute() const { return attributeSet.Last(); } ///< Access the last attribute in this element.
+ TiXmlAttribute* LastAttribute() { return attributeSet.Last(); }
+
+ /** Convenience function for easy access to the text inside an element.
+ Although easy and concise, GetText() is limited compared to getting the
+ TiXmlText child and accessing it directly.
+
+ If the first child of 'this' is a TiXmlText, the GetText() returns the
+ character string of the Text node, else null is returned.
+
+ This is a convenient method for getting the text of simple contained text:
+ @verbatim
+ <foo>This is text</foo>
+ const char* str = fooElement->GetText();
+ @endverbatim
+
+ 'str' will be a pointer to "This is text".
+
+ Note that this function can be misleading. If the element foo was created
+ from this XML:
+ @verbatim
+ <foo><b>This is text</b></foo>
+ @endverbatim
+
+ then the value of str would be null. The first child node isn't a text
+ node, it is another element. From this XML:
+ @verbatim
+ <foo>This is <b>text</b></foo>
+ @endverbatim
+ GetText() will return "This is ".
+
+ WARNING: GetText() accesses a child node - don't become confused with the
+ similarly named TiXmlNode::ToText() which are safe type casts on the
+ referenced node. **/
+ const char* GetText() const;
+
+ /// Creates a new Element and returns it - the returned element is a copy.
+ virtual TiXmlNode* Clone() const;
+ // Print the Element to a FILE stream.
+ virtual void Print( FILE* cfile, int depth ) const;
+
+ /* Attribtue parsing starts: next char past '<'
+ returns: next char past '>'
+ */
+ virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding );
+
+ virtual const TiXmlElement* ToElement() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+ virtual TiXmlElement* ToElement() { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+
+ /** Walk the XML tree visiting this node and all of its children.
+ */
+ virtual bool Accept( TiXmlVisitor* visitor ) const;
+
+protected:
+
+ void CopyTo( TiXmlElement* target ) const;
+ void ClearThis(); // like clear, but initializes 'this' object as well
+
+ // Used to be public [internal use]
+ virtual void StreamIn( std::istream * in, String * tag );
+
+ /* [internal use]
+ Reads the "value" of the element -- another element, or text.
+ This should terminate with the current end tag.
+ */
+ const char* ReadValue( const char* in, TiXmlParsingData* prevData, TiXmlEncoding encoding );
+
+private:
+
+ TiXmlAttributeSet attributeSet;
+};
+
+
+
+
+//------------------------------------------------------------------------------
+// TinyXML COMMENT
+//------------------------------------------------------------------------------
+/** An XML comment.
+*/
+class TiXmlComment : public TiXmlNode
+{
+public:
+ /// Constructs an empty comment.
+ TiXmlComment() : TiXmlNode( TiXmlNode::COMMENT ) {}
+ /// Construct a comment from text.
+ explicit TiXmlComment( const char* _value ) : TiXmlNode( TiXmlNode::COMMENT ) {
+ SetValue( _value );
+ }
+ /// Construct a comment from text.
+ explicit TiXmlComment( const String& _value ) : TiXmlNode( TiXmlNode::COMMENT ) {
+ SetValue( _value );
+ }
+ TiXmlComment( const TiXmlComment& );
+ void operator=( const TiXmlComment& base );
+
+ virtual ~TiXmlComment() {}
+
+ /// Returns a copy of this Comment.
+ virtual TiXmlNode* Clone() const;
+ // Write this Comment to a FILE stream.
+ virtual void Print( FILE* cfile, int depth ) const;
+
+ /* Attribtue parsing starts: at the ! of the !--
+ returns: next char past '>'
+ */
+ virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding );
+
+ virtual const TiXmlComment* ToComment() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+ virtual TiXmlComment* ToComment() { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+
+ /** Walk the XML tree visiting this node and all of its children.
+ */
+ virtual bool Accept( TiXmlVisitor* visitor ) const;
+
+protected:
+ void CopyTo( TiXmlComment* target ) const;
+
+ // used to be public
+ virtual void StreamIn( std::istream * in, String * tag );
+// virtual void StreamOut( TIXML_OSTREAM * out ) const;
+
+private:
+
+};
+
+
+
+//------------------------------------------------------------------------------
+// TinyXML TEXT
+//------------------------------------------------------------------------------
+/** XML text. A text node can have 2 ways to output the next. "normal" output
+and CDATA. It will default to the mode it was parsed from the XML file and
+you generally want to leave it alone, but you can change the output mode with
+SetCDATA() and query it with CDATA().
+*/
+class TiXmlText : public TiXmlNode
+{
+ friend class TiXmlDocument;
+ friend class TiXmlElement;
+public:
+ /** Constructor for text element. By default, it is treated as
+ normal, encoded text. If you want it be output as a CDATA text
+ element, set the parameter _cdata to 'true'
+ */
+ explicit TiXmlText (const char * initValue ) : TiXmlNode (TiXmlNode::TEXT)
+ {
+ SetValue( initValue );
+ cdata = false;
+ }
+ /// Constructor.
+ explicit TiXmlText( const String& initValue ) : TiXmlNode (TiXmlNode::TEXT)
+ {
+ SetValue( initValue );
+ cdata = false;
+ }
+ virtual ~TiXmlText() {}
+
+ TiXmlText( const TiXmlText& copy ) : TiXmlNode( TiXmlNode::TEXT ) { copy.CopyTo( this ); }
+ void operator=( const TiXmlText& base ) { base.CopyTo( this ); }
+
+ // Write this text object to a FILE stream.
+ virtual void Print( FILE* cfile, int depth ) const;
+
+ /// Queries whether this represents text using a CDATA section.
+ bool CDATA() const { return cdata; }
+ /// Turns on or off a CDATA representation of text.
+ void SetCDATA( bool _cdata ) { cdata = _cdata; }
+
+ virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding );
+
+ virtual const TiXmlText* ToText() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+ virtual TiXmlText* ToText() { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+
+ /** Walk the XML tree visiting this node and all of its children.
+ */
+ virtual bool Accept( TiXmlVisitor* content ) const;
+
+protected :
+ /// [internal use] Creates a new Element and returns it.
+ virtual TiXmlNode* Clone() const;
+ void CopyTo( TiXmlText* target ) const;
+
+ bool Blank() const; // returns true if all white space and new lines
+ // [internal use]
+ virtual void StreamIn( std::istream * in, String * tag );
+
+private:
+ bool cdata; // true if this should be input and output as a CDATA style text element
+};
+
+
+
+
+//------------------------------------------------------------------------------
+// TinyXML DECLARATION
+//------------------------------------------------------------------------------
+/** In correct XML the declaration is the first entry in the file.
+ at verbatim
+ <?xml version="1.0" standalone="yes"?>
+ at endverbatim
+
+TinyXml will happily read or write files without a declaration,
+however. There are 3 possible attributes to the declaration:
+version, encoding, and standalone.
+
+Note: In this version of the code, the attributes are
+handled as special cases, not generic attributes, simply
+because there can only be at most 3 and they are always the same.
+*/
+class TiXmlDeclaration : public TiXmlNode
+{
+public:
+ /// Construct an empty declaration.
+ TiXmlDeclaration() : TiXmlNode( TiXmlNode::DECLARATION ) {}
+
+ /// Constructor.
+ TiXmlDeclaration( const String& _version,
+ const String& _encoding,
+ const String& _standalone );
+
+ /// Construct.
+ TiXmlDeclaration( const char* _version,
+ const char* _encoding,
+ const char* _standalone );
+
+ TiXmlDeclaration( const TiXmlDeclaration& copy );
+ void operator=( const TiXmlDeclaration& copy );
+
+ virtual ~TiXmlDeclaration() {}
+
+ void SetVersion(const char* version) {this->version = version;}
+ void SetEncoding(const char* encoding) {this->encoding = encoding;}
+ /// Better be "yes" or "no" but we won't check.
+ void SetStandalone(const char* standalone) {this->standalone = standalone;}
+
+ /// Version. Will return an empty string if none was found.
+ const char *Version() const { return version.c_str (); }
+ /// Encoding. Will return an empty string if none was found.
+ const char *Encoding() const { return encoding.c_str (); }
+ /// Is this a standalone document?
+ const char *Standalone() const { return standalone.c_str (); }
+
+ /// Creates a copy of this Declaration and returns it.
+ virtual TiXmlNode* Clone() const;
+ // Print this declaration to a FILE stream.
+ virtual void Print( FILE* cfile, int depth, String* str ) const;
+ virtual void Print( FILE* cfile, int depth ) const {
+ Print( cfile, depth, 0 );
+ }
+
+ virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding );
+
+ virtual const TiXmlDeclaration* ToDeclaration() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+ virtual TiXmlDeclaration* ToDeclaration() { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+
+ /** Walk the XML tree visiting this node and all of its children.
+ */
+ virtual bool Accept( TiXmlVisitor* visitor ) const;
+
+protected:
+ void CopyTo( TiXmlDeclaration* target ) const;
+ virtual void StreamIn( std::istream * in, String * tag );
+
+private:
+
+ String version;
+ String encoding;
+ String standalone;
+};
+
+
+
+//------------------------------------------------------------------------------
+// TinyXML UNKNOWN
+//------------------------------------------------------------------------------
+/** Any tag that tinyXml doesn't recognize is saved as an
+unknown. It is a tag of text, but should not be modified.
+It will be written back to the XML, unchanged, when the file
+is saved.
+
+DTD tags get thrown into TiXmlUnknowns.
+*/
+class TiXmlUnknown : public TiXmlNode
+{
+public:
+ TiXmlUnknown() : TiXmlNode( TiXmlNode::UNKNOWN ) {}
+ virtual ~TiXmlUnknown() {}
+
+ TiXmlUnknown( const TiXmlUnknown& copy ) : TiXmlNode( TiXmlNode::UNKNOWN ) { copy.CopyTo( this ); }
+ void operator=( const TiXmlUnknown& copy ) { copy.CopyTo( this ); }
+
+ /// Creates a copy of this Unknown and returns it.
+ virtual TiXmlNode* Clone() const;
+ // Print this Unknown to a FILE stream.
+ virtual void Print( FILE* cfile, int depth ) const;
+
+ virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding );
+
+ virtual const TiXmlUnknown* ToUnknown() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+ virtual TiXmlUnknown* ToUnknown() { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+
+ /** Walk the XML tree visiting this node and all of its children.
+ */
+ virtual bool Accept( TiXmlVisitor* content ) const;
+
+protected:
+ void CopyTo( TiXmlUnknown* target ) const;
+
+ virtual void StreamIn( std::istream * in, String * tag );
+
+private:
+
+};
+
+
+
+//------------------------------------------------------------------------------
+// TinyXML DOCUMENT
+//------------------------------------------------------------------------------
+/** Always the top level node. A document binds together all the
+XML pieces. It can be saved, loaded, and printed to the screen.
+The 'value' of a document node is the xml file name.
+*/
+class TiXmlDocument : public TiXmlNode
+{
+public:
+ /// Create an empty document, that has no name.
+ TiXmlDocument();
+ /// Create a document with a name. The name of the document is also the filename of the xml.
+ TiXmlDocument( const char * documentName );
+
+ /// Constructor.
+ TiXmlDocument( const String& documentName );
+
+ TiXmlDocument( const TiXmlDocument& copy );
+ void operator=( const TiXmlDocument& copy );
+
+ virtual ~TiXmlDocument() {}
+
+ void SetIndentString(const String& indent)
+ { indentString = indent; }
+ const String& GetIndentString() const
+ { return indentString; }
+ const char* GetIndentChars() const
+ { return indentString.c_str(); }
+
+ /** Load a file using the current document value.
+ Returns true if successful. Will delete any existing
+ document data before loading.
+ */
+ bool LoadFile( TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING );
+ /// Save a file using the current document value. Returns true if successful.
+ bool SaveFile() const;
+ /// Load a file using the given filename. Returns true if successful.
+ bool LoadFile( const char * filename, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING );
+ /// Save a file using the given filename. Returns true if successful.
+ bool SaveFile( const char * filename ) const;
+ /** Load a file using the given FILE*. Returns true if successful. Note that this method
+ doesn't stream - the entire object pointed at by the FILE*
+ will be interpreted as an XML file. TinyXML doesn't stream in XML from the current
+ file location. Streaming may be added in the future.
+ */
+ bool LoadFile( FILE*, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING );
+ /// Save a file using the given FILE*. Returns true if successful.
+ bool SaveFile( FILE* ) const;
+
+ bool LoadFile( const String& filename, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING )
+ {
+ return LoadFile( filename.c_str(), encoding );
+ }
+ bool SaveFile( const String& filename ) const
+ {
+ return SaveFile( filename.c_str() );
+ }
+
+ /** Parse the given null terminated block of xml data. Passing in an encoding to this
+ method (either TIXML_ENCODING_LEGACY or TIXML_ENCODING_UTF8 will force TinyXml
+ to use that encoding, regardless of what TinyXml might otherwise try to detect.
+ */
+ virtual const char* Parse( const char* p, TiXmlParsingData* data = 0, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING );
+
+ /** Get the root element -- the only top level element -- of the document.
+ In well formed XML, there should only be one. TinyXml is tolerant of
+ multiple elements at the document level.
+ */
+ const TiXmlElement* RootElement() const { return FirstChildElement(); }
+ TiXmlElement* RootElement() { return FirstChildElement(); }
+
+ /** If an error occurs, Error will be set to true. Also,
+ - The ErrorId() will contain the integer identifier of the error (not generally useful)
+ - The ErrorDesc() method will return the name of the error. (very useful)
+ - The ErrorRow() and ErrorCol() will return the location of the error (if known)
+ */
+ bool Error() const { return error; }
+
+ /** Contains the textual (english) description of the error if one
+ occurs. **/
+ const String& ErrorDesc() const { return errorDesc; }
+
+ /** Produce a formatted message from the error descriptor and the row
+ and column information. **/
+ String ErrorMsg() const {
+ return ErrorDesc() + " (line=" + String(ErrorRow())
+ + ", col=" + String(ErrorCol()) + ")";
+ }
+
+ /** Generally, you probably want the error string ( ErrorDesc() ). But if
+ you prefer the ErrorId, this function will fetch it. **/
+ int ErrorId() const { return errorId; }
+
+ /** Returns the location (if known) of the error. The first column is
+ column 1, and the first row is row 1. A value of 0 means the row and
+ column wasn't applicable (memory errors, for example, have no row/column)
+ or the parser lost the error. (An error in the error reporting, in that
+ case.)
+ @sa SetTabSize, Row, Column **/
+ int ErrorRow() const { return errorLocation.row+1; }
+ /** The column where the error occured. @sa ErrorRow() **/
+ int ErrorCol() const { return errorLocation.col+1; }
+
+ /** SetTabSize() allows the error reporting functions (ErrorRow() and
+ ErrorCol()) to report the correct values for row and column. It does not
+ change the output or input in any way.
+
+ By calling this method, with a tab size greater than 0, the row and column
+ of each node and attribute is stored when the file is loaded. Very useful
+ for tracking the DOM back in to the source file.
+
+ The tab size is required for calculating the location of nodes. If not
+ set, the default of 4 is used. The tabsize is set per document. Setting
+ the tabsize to 0 disables row/column tracking.
+
+ Note that row and column tracking is not supported when using operator>>.
+
+ The tab size needs to be enabled before the parse or load. Correct usage:
+ @verbatim
+ TiXmlDocument doc;
+ doc.SetTabSize( 8 );
+ doc.Load( "myfile.xml" );
+ @endverbatim
+
+ @sa Row, Column
+ */
+ void SetTabSize( int _tabsize ) { tabsize = _tabsize; }
+
+ int TabSize() const { return tabsize; }
+
+ /** If you have handled the error, it can be reset with this call. The error
+ state is automatically cleared if you Parse a new XML block.
+ */
+ void ClearError() { error = false;
+ errorId = 0;
+ errorDesc = "";
+ errorLocation.row = errorLocation.col = 0;
+ //errorLocation.last = 0;
+ }
+
+ /** Write the document to standard out using formatted printing ("pretty print"). */
+ void Print() const { Print( stdout, 0 ); }
+
+ /* Write the document to a string using formatted printing ("pretty print"). This
+ will allocate a character array (new char[]) and return it as a pointer. The
+ calling code pust call delete[] on the return char* to avoid a memory leak.
+ */
+ //char* PrintToMemory() const;
+
+ /// Print this Document to a FILE stream.
+ virtual void Print( FILE* cfile, int depth = 0 ) const;
+ // [internal use]
+ void SetError( int err, const char* errorLocation, TiXmlParsingData* prevData, TiXmlEncoding encoding );
+
+ virtual const TiXmlDocument* ToDocument() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+ virtual TiXmlDocument* ToDocument() { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+
+ /** Walk the XML tree visiting this node and all of its children.
+ */
+ virtual bool Accept( TiXmlVisitor* content ) const;
+
+protected :
+ // [internal use]
+ virtual TiXmlNode* Clone() const;
+ virtual void StreamIn( std::istream * in, String * tag );
+
+private:
+ void CopyTo( TiXmlDocument* target ) const;
+
+ bool error;
+ int errorId;
+ String errorDesc;
+ int tabsize;
+ TiXmlCursor errorLocation;
+ bool useMicrosoftBOM; // the UTF-8 BOM were found when read. Note this, and try to write.
+
+ // Added by sherm 20110106.
+ String indentString;
+};
+
+
+
+//------------------------------------------------------------------------------
+// TinyXML PRINTER
+//------------------------------------------------------------------------------
+/** Print to memory functionality. The TiXmlPrinter is useful when you need to:
+
+ -# Print to memory (especially in non-STL mode)
+ -# Control formatting (line endings, etc.)
+
+When constructed, the TiXmlPrinter is in its default "pretty printing" mode.
+Before calling Accept() you can call methods to control the printing
+of the XML document. After TiXmlNode::Accept() is called, the printed document
+can be accessed via the CStr(), Str(), and Size() methods.
+
+sherm: I added the ability to print to a given String to avoid a final copy.
+
+TiXmlPrinter uses the Visitor API.
+ at verbatim
+TiXmlPrinter printer;
+printer.SetIndent( "\t" );
+
+doc.Accept( &printer );
+fprintf( stdout, "%s", printer.CStr() );
+ at endverbatim
+*/
+class TiXmlPrinter : public TiXmlVisitor
+{
+public:
+ /** Construct a Printer that uses its own private String buffer. **/
+ TiXmlPrinter()
+ : depth( 0 ), simpleTextPrint( false ),
+ indent( " " ), lineBreak( "\n" ),
+ isBufferOwner(true), buffer(*new String()) {}
+
+ /** Construct a Printer that uses a pre-existing String as a buffer. **/
+ explicit TiXmlPrinter(String& buffer)
+ : depth( 0 ), simpleTextPrint( false ),
+ indent( " " ), lineBreak( "\n" ),
+ isBufferOwner(false), buffer(buffer)
+ { buffer.clear(); }
+
+ /** Destruct the buffer if this object is the owner of it. **/
+ ~TiXmlPrinter() {if (isBufferOwner) delete &buffer;}
+
+ virtual bool VisitEnter( const TiXmlDocument& doc );
+ virtual bool VisitExit( const TiXmlDocument& doc );
+
+ virtual bool VisitEnter( const TiXmlElement& element, const TiXmlAttribute* firstAttribute );
+ virtual bool VisitExit( const TiXmlElement& element );
+
+ virtual bool Visit( const TiXmlDeclaration& declaration );
+ virtual bool Visit( const TiXmlText& text );
+ virtual bool Visit( const TiXmlComment& comment );
+ virtual bool Visit( const TiXmlUnknown& unknown );
+
+ /** Set the indent characters for printing. By default 4 spaces
+ but tab (\t) is also useful, or null/empty string for no indentation.
+ */
+ void SetIndent( const char* _indent ) { indent = _indent ? _indent : "" ; }
+ /// Query the indention string.
+ const char* Indent() { return indent.c_str(); }
+ /** Set the line breaking string. By default set to newline (\n).
+ Some operating systems prefer other characters, or can be
+ set to the null/empty string for no indenation.
+ */
+ void SetLineBreak( const char* _lineBreak ) { lineBreak = _lineBreak ? _lineBreak : ""; }
+ /// Query the current line breaking string.
+ const char* LineBreak() { return lineBreak.c_str(); }
+
+ /** Switch over to "stream printing" which is the most dense formatting without
+ linebreaks. Common when the XML is needed for network transmission.
+ */
+ void SetStreamPrinting() { indent = "";
+ lineBreak = "";
+ }
+ /// Return the result.
+ const char* CStr() { return buffer.c_str(); }
+ /// Return the length of the result string.
+ size_t Size() { return buffer.size(); }
+
+ /// Return the result.
+ const String& Str() { return buffer; }
+
+private:
+ void DoIndent() {
+ for( int i=0; i<depth; ++i )
+ buffer += indent;
+ }
+ void DoLineBreak() {
+ buffer += lineBreak;
+ }
+
+ int depth;
+ bool simpleTextPrint;
+ String indent;
+ String lineBreak;
+
+ // true if Printer object owns this buffer heap space
+ bool isBufferOwner;
+ String& buffer;
+};
+
+} // namespace SimTK
+
+
+#ifdef _MSC_VER
+#pragma warning( pop )
+#endif
+
+#endif
+
diff --git a/SimTKcommon/src/tinyxmlparser.cpp b/SimTKcommon/src/tinyxmlparser.cpp
new file mode 100644
index 0000000..34be1b6
--- /dev/null
+++ b/SimTKcommon/src/tinyxmlparser.cpp
@@ -0,0 +1,1698 @@
+/*
+www.sourceforge.net/projects/tinyxml
+Original code (2.0 and earlier )copyright (c) 2000-2002 Lee Thomason (www.grinninglizard.com)
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any
+damages arising from the use of this software.
+
+Permission is granted to anyone to use this software for any
+purpose, including commercial applications, and to alter it and
+redistribute it freely, subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must
+not claim that you wrote the original software. If you use this
+software in a product, an acknowledgment in the product documentation
+would be appreciated but is not required.
+
+2. Altered source versions must be plainly marked as such, and
+must not be misrepresented as being the original software.
+
+3. This notice may not be removed or altered from any source
+distribution.
+*/
+
+#include <cctype>
+#include <cstddef>
+
+#include "tinyxml.h"
+
+//#define DEBUG_PARSER
+#if defined( DEBUG_PARSER )
+# if defined( DEBUG ) && defined( _MSC_VER )
+# include <windows.h>
+# define TIXML_LOG OutputDebugString
+# else
+# define TIXML_LOG printf
+# endif
+#endif
+
+
+namespace SimTK {
+
+// Note tha "PutString" hardcodes the same list. This
+// is less flexible than it appears. Changing the entries
+// or order will break putstring.
+TiXmlBase::Entity TiXmlBase::entity[ NUM_ENTITY ] =
+{
+ { "&", 5, '&' },
+ { "<", 4, '<' },
+ { ">", 4, '>' },
+ { """, 6, '\"' },
+ { "'", 6, '\'' }
+};
+
+// Bunch of unicode info at:
+// http://www.unicode.org/faq/utf_bom.html
+// Including the basic of this table, which determines the #bytes in the
+// sequence from the lead byte. 1 placed for invalid sequences --
+// although the result will be junk, pass it through as much as possible.
+// Beware of the non-characters in UTF-8:
+// ef bb bf (Microsoft "lead bytes")
+// ef bf be
+// ef bf bf
+
+static const unsigned char TIXML_UTF_LEAD_0 = 0xefU;
+static const unsigned char TIXML_UTF_LEAD_1 = 0xbbU;
+static const unsigned char TIXML_UTF_LEAD_2 = 0xbfU;
+
+const int TiXmlBase::utf8ByteTable[256] =
+{
+ // 0 1 2 3 4 5 6 7 8 9 a b c d e f
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x00
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x10
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x20
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x30
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x40
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x50
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x60
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x70 End of ASCII range
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x80 0x80 to 0xc1 invalid
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x90
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0xa0
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0xb0
+ 1, 1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // 0xc0 0xc2 to 0xdf 2 byte
+ 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // 0xd0
+ 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, // 0xe0 0xe0 to 0xef 3 byte
+ 4, 4, 4, 4, 4, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // 0xf0 0xf0 to 0xf4 4 byte, 0xf5 and higher invalid
+};
+
+
+void TiXmlBase::ConvertUTF32ToUTF8( unsigned long input, char* output, int* length )
+{
+ const unsigned long BYTE_MASK = 0xBF;
+ const unsigned long BYTE_MARK = 0x80;
+ const unsigned long FIRST_BYTE_MARK[7] = { 0x00, 0x00, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC };
+
+ if (input < 0x80)
+ *length = 1;
+ else if ( input < 0x800 )
+ *length = 2;
+ else if ( input < 0x10000 )
+ *length = 3;
+ else if ( input < 0x200000 )
+ *length = 4;
+ else
+ { *length = 0; return; } // This code won't covert this correctly anyway.
+
+ output += *length;
+
+ // Scary scary fall throughs.
+ switch (*length)
+ {
+ case 4:
+ --output;
+ *output = (char)((input | BYTE_MARK) & BYTE_MASK);
+ input >>= 6;
+ case 3:
+ --output;
+ *output = (char)((input | BYTE_MARK) & BYTE_MASK);
+ input >>= 6;
+ case 2:
+ --output;
+ *output = (char)((input | BYTE_MARK) & BYTE_MASK);
+ input >>= 6;
+ case 1:
+ --output;
+ *output = (char)(input | FIRST_BYTE_MARK[*length]);
+ }
+}
+
+
+/*static*/ int TiXmlBase::IsAlpha( unsigned char anyByte, TiXmlEncoding /*encoding*/ )
+{
+ // This will only work for low-ascii, everything else is assumed to be a valid
+ // letter. I'm not sure this is the best approach, but it is quite tricky trying
+ // to figure out alhabetical vs. not across encoding. So take a very
+ // conservative approach.
+
+// if ( encoding == TIXML_ENCODING_UTF8 )
+// {
+ if ( anyByte < 127 )
+ return isalpha( anyByte );
+ else
+ return 1; // What else to do? The unicode set is huge...get the english ones right.
+// }
+// else
+// {
+// return isalpha( anyByte );
+// }
+}
+
+
+/*static*/ int TiXmlBase::IsAlphaNum( unsigned char anyByte, TiXmlEncoding /*encoding*/ )
+{
+ // This will only work for low-ascii, everything else is assumed to be a valid
+ // letter. I'm not sure this is the best approach, but it is quite tricky trying
+ // to figure out alhabetical vs. not across encoding. So take a very
+ // conservative approach.
+
+// if ( encoding == TIXML_ENCODING_UTF8 )
+// {
+ if ( anyByte < 127 )
+ return isalnum( anyByte );
+ else
+ return 1; // What else to do? The unicode set is huge...get the english ones right.
+// }
+// else
+// {
+// return isalnum( anyByte );
+// }
+}
+
+
+class TiXmlParsingData
+{
+ friend class TiXmlDocument;
+public:
+ void Stamp( const char* now, TiXmlEncoding encoding );
+
+ const TiXmlCursor& Cursor() { return cursor; }
+
+private:
+ // Only used by the document!
+ TiXmlParsingData( const char* start, int _tabsize, int row, int col )
+ {
+ assert( start );
+ stamp = start;
+ tabsize = _tabsize;
+ cursor.row = row;
+ cursor.col = col;
+ }
+
+ TiXmlCursor cursor;
+ const char* stamp;
+ int tabsize;
+};
+
+
+void TiXmlParsingData::Stamp( const char* now, TiXmlEncoding encoding )
+{
+ assert( now );
+
+ // Do nothing if the tabsize is 0.
+ if ( tabsize < 1 )
+ {
+ return;
+ }
+
+ // Get the current row, column.
+ int row = cursor.row;
+ int col = cursor.col;
+ const char* p = stamp;
+ assert( p );
+
+ while ( p < now )
+ {
+ // Treat p as unsigned, so we have a happy compiler.
+ const unsigned char* pU = (const unsigned char*)p;
+
+ // Code contributed by Fletcher Dunn: (modified by lee)
+ switch (*pU) {
+ case 0:
+ // We *should* never get here, but in case we do, don't
+ // advance past the terminating null character, ever
+ return;
+
+ case '\r':
+ // bump down to the next line
+ ++row;
+ col = 0;
+ // Eat the character
+ ++p;
+
+ // Check for \r\n sequence, and treat this as a single character
+ if (*p == '\n') {
+ ++p;
+ }
+ break;
+
+ case '\n':
+ // bump down to the next line
+ ++row;
+ col = 0;
+
+ // Eat the character
+ ++p;
+
+ // Check for \n\r sequence, and treat this as a single
+ // character. (Yes, this bizarre thing does occur still
+ // on some arcane platforms...)
+ if (*p == '\r') {
+ ++p;
+ }
+ break;
+
+ case '\t':
+ // Eat the character
+ ++p;
+
+ // Skip to next tab stop
+ col = (col / tabsize + 1) * tabsize;
+ break;
+
+ case TIXML_UTF_LEAD_0:
+ if ( encoding == TIXML_ENCODING_UTF8 )
+ {
+ if ( *(p+1) && *(p+2) )
+ {
+ // In these cases, don't advance the column. These are
+ // 0-width spaces.
+ if ( *(pU+1)==TIXML_UTF_LEAD_1 && *(pU+2)==TIXML_UTF_LEAD_2 )
+ p += 3;
+ else if ( *(pU+1)==0xbfU && *(pU+2)==0xbeU )
+ p += 3;
+ else if ( *(pU+1)==0xbfU && *(pU+2)==0xbfU )
+ p += 3;
+ else
+ { p +=3; ++col; } // A normal character.
+ }
+ }
+ else
+ {
+ ++p;
+ ++col;
+ }
+ break;
+
+ default:
+ if ( encoding == TIXML_ENCODING_UTF8 )
+ {
+ // Eat the 1 to 4 byte utf8 character.
+ int step = TiXmlBase::utf8ByteTable[*((const unsigned char*)p)];
+ if ( step == 0 )
+ step = 1; // Error case from bad encoding, but handle gracefully.
+ p += step;
+
+ // Just advance one column, of course.
+ ++col;
+ }
+ else
+ {
+ ++p;
+ ++col;
+ }
+ break;
+ }
+ }
+ cursor.row = row;
+ cursor.col = col;
+ assert( cursor.row >= -1 );
+ assert( cursor.col >= -1 );
+ stamp = p;
+ assert( stamp );
+}
+
+
+const char* TiXmlBase::SkipWhiteSpace( const char* p, TiXmlEncoding encoding )
+{
+ if ( !p || !*p )
+ {
+ return 0;
+ }
+ if ( encoding == TIXML_ENCODING_UTF8 )
+ {
+ while ( *p )
+ {
+ const unsigned char* pU = (const unsigned char*)p;
+
+ // Skip the stupid Microsoft UTF-8 Byte order marks
+ if ( *(pU+0)==TIXML_UTF_LEAD_0
+ && *(pU+1)==TIXML_UTF_LEAD_1
+ && *(pU+2)==TIXML_UTF_LEAD_2 )
+ {
+ p += 3;
+ continue;
+ }
+ else if(*(pU+0)==TIXML_UTF_LEAD_0
+ && *(pU+1)==0xbfU
+ && *(pU+2)==0xbeU )
+ {
+ p += 3;
+ continue;
+ }
+ else if(*(pU+0)==TIXML_UTF_LEAD_0
+ && *(pU+1)==0xbfU
+ && *(pU+2)==0xbfU )
+ {
+ p += 3;
+ continue;
+ }
+
+ if ( IsWhiteSpace( *p ) || *p == '\n' || *p =='\r' ) // Still using old rules for white space.
+ ++p;
+ else
+ break;
+ }
+ }
+ else
+ {
+ while ( (*p && IsWhiteSpace( *p )) || *p == '\n' || *p =='\r' )
+ ++p;
+ }
+
+ return p;
+}
+
+/*static*/ bool TiXmlBase::StreamWhiteSpace( std::istream * in, String * tag )
+{
+ for( ;; )
+ {
+ if ( !in->good() ) return false;
+
+ int c = in->peek();
+ // At this scope, we can't get to a document. So fail silently.
+ if ( !IsWhiteSpace( c ) || c <= 0 )
+ return true;
+
+ *tag += (char) in->get();
+ }
+}
+
+/*static*/ bool TiXmlBase::StreamTo( std::istream * in, int character, String * tag )
+{
+ //assert( character > 0 && character < 128 ); // else it won't work in utf-8
+ while ( in->good() )
+ {
+ int c = in->peek();
+ if ( c == character )
+ return true;
+ if ( c <= 0 ) // Silent failure: can't get document at this scope
+ return false;
+
+ in->get();
+ *tag += (char) c;
+ }
+ return false;
+}
+
+// One of TinyXML's more performance demanding functions. Try to keep the memory overhead down. The
+// "assign" optimization removes over 10% of the execution time.
+//
+const char* TiXmlBase::ReadName( const char* p, String * name, TiXmlEncoding encoding )
+{
+ // Oddly, not supported on some comilers,
+ //name->clear();
+ // So use this:
+ *name = "";
+ assert( p );
+
+ // Names start with letters or underscores.
+ // Of course, in unicode, tinyxml has no idea what a letter *is*. The
+ // algorithm is generous.
+ //
+ // After that, they can be letters, underscores, numbers,
+ // hyphens, or colons. (Colons are valid ony for namespaces,
+ // but tinyxml can't tell namespaces from names.)
+ if ( p && *p
+ && ( IsAlpha( (unsigned char) *p, encoding ) || *p == '_' ) )
+ {
+ const char* start = p;
+ while( p && *p
+ && ( IsAlphaNum( (unsigned char ) *p, encoding )
+ || *p == '_'
+ || *p == '-'
+ || *p == '.'
+ || *p == ':' ) )
+ {
+ //(*name) += *p; // expensive
+ ++p;
+ }
+ if ( p-start > 0 ) {
+ name->assign( start, p-start );
+ }
+ return p;
+ }
+ return 0;
+}
+
+const char* TiXmlBase::GetEntity( const char* p, char* value, int* length, TiXmlEncoding encoding )
+{
+ // Presume an entity, and pull it out.
+ String ent;
+ int i;
+ *length = 0;
+
+ if ( *(p+1) && *(p+1) == '#' && *(p+2) )
+ {
+ unsigned long ucs = 0;
+ ptrdiff_t delta = 0;
+ unsigned mult = 1;
+
+ if ( *(p+2) == 'x' )
+ {
+ // Hexadecimal.
+ if ( !*(p+3) ) return 0;
+
+ const char* q = p+3;
+ q = strchr( q, ';' );
+
+ if ( !q || !*q ) return 0;
+
+ delta = q-p;
+ --q;
+
+ while ( *q != 'x' )
+ {
+ if ( *q >= '0' && *q <= '9' )
+ ucs += mult * (*q - '0');
+ else if ( *q >= 'a' && *q <= 'f' )
+ ucs += mult * (*q - 'a' + 10);
+ else if ( *q >= 'A' && *q <= 'F' )
+ ucs += mult * (*q - 'A' + 10 );
+ else
+ return 0;
+ mult *= 16;
+ --q;
+ }
+ }
+ else
+ {
+ // Decimal.
+ if ( !*(p+2) ) return 0;
+
+ const char* q = p+2;
+ q = strchr( q, ';' );
+
+ if ( !q || !*q ) return 0;
+
+ delta = q-p;
+ --q;
+
+ while ( *q != '#' )
+ {
+ if ( *q >= '0' && *q <= '9' )
+ ucs += mult * (*q - '0');
+ else
+ return 0;
+ mult *= 10;
+ --q;
+ }
+ }
+ if ( encoding == TIXML_ENCODING_UTF8 )
+ {
+ // convert the UCS to UTF-8
+ ConvertUTF32ToUTF8( ucs, value, length );
+ }
+ else
+ {
+ *value = (char)ucs;
+ *length = 1;
+ }
+ return p + delta + 1;
+ }
+
+ // Now try to match it.
+ for( i=0; i<NUM_ENTITY; ++i )
+ {
+ if ( strncmp( entity[i].str, p, entity[i].strLength ) == 0 )
+ {
+ assert( strlen( entity[i].str ) == entity[i].strLength );
+ *value = entity[i].chr;
+ *length = 1;
+ return ( p + entity[i].strLength );
+ }
+ }
+
+ // So it wasn't an entity, its unrecognized, or something like that.
+ *value = *p; // Don't put back the last one, since we return it!
+ //*length = 1; // Leave unrecognized entities - this doesn't really work.
+ // Just writes strange XML.
+ return p+1;
+}
+
+
+bool TiXmlBase::StringEqual( const char* p,
+ const char* tag,
+ bool ignoreCase,
+ TiXmlEncoding encoding )
+{
+ assert( p );
+ assert( tag );
+ if ( !p || !*p )
+ {
+ assert( 0 );
+ return false;
+ }
+
+ const char* q = p;
+
+ if ( ignoreCase )
+ {
+ while ( *q && *tag && ToLower( *q, encoding ) == ToLower( *tag, encoding ) )
+ {
+ ++q;
+ ++tag;
+ }
+
+ if ( *tag == 0 )
+ return true;
+ }
+ else
+ {
+ while ( *q && *tag && *q == *tag )
+ {
+ ++q;
+ ++tag;
+ }
+
+ if ( *tag == 0 ) // Have we found the end of the tag, and everything equal?
+ return true;
+ }
+ return false;
+}
+
+const char* TiXmlBase::ReadText( const char* p,
+ String * text,
+ bool trimWhiteSpace,
+ const char* endTag,
+ bool caseInsensitive,
+ TiXmlEncoding encoding )
+{
+ *text = "";
+ if ( !trimWhiteSpace // certain tags always keep whitespace
+ || !condenseWhiteSpace ) // if true, whitespace is always kept
+ {
+ // Keep all the white space.
+ while ( p && *p
+ && !StringEqual( p, endTag, caseInsensitive, encoding )
+ )
+ {
+ int len;
+ char cArr[4] = { 0, 0, 0, 0 };
+ p = GetChar( p, cArr, &len, encoding );
+ text->append( cArr, len );
+ }
+ }
+ else
+ {
+ bool whitespace = false;
+
+ // Remove leading white space:
+ p = SkipWhiteSpace( p, encoding );
+ while ( p && *p
+ && !StringEqual( p, endTag, caseInsensitive, encoding ) )
+ {
+ if ( *p == '\r' || *p == '\n' )
+ {
+ whitespace = true;
+ ++p;
+ }
+ else if ( IsWhiteSpace( *p ) )
+ {
+ whitespace = true;
+ ++p;
+ }
+ else
+ {
+ // If we've found whitespace, add it before the
+ // new character. Any whitespace just becomes a space.
+ if ( whitespace )
+ {
+ (*text) += ' ';
+ whitespace = false;
+ }
+ int len;
+ char cArr[4] = { 0, 0, 0, 0 };
+ p = GetChar( p, cArr, &len, encoding );
+ if ( len == 1 )
+ (*text) += cArr[0]; // more efficient
+ else
+ text->append( cArr, len );
+ }
+ }
+ }
+ if ( p )
+ p += strlen( endTag );
+ return p;
+}
+
+
+void TiXmlDocument::StreamIn( std::istream * in, String * tag )
+{
+ // The basic issue with a document is that we don't know what we're
+ // streaming. Read something presumed to be a tag (and hope), then
+ // identify it, and call the appropriate stream method on the tag.
+ //
+ // This "pre-streaming" will never read the closing ">" so the
+ // sub-tag can orient itself.
+
+ if ( !StreamTo( in, '<', tag ) )
+ {
+ SetError( TIXML_ERROR_PARSING_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return;
+ }
+
+ while ( in->good() )
+ {
+ int tagIndex = (int) tag->length();
+ while ( in->good() && in->peek() != '>' )
+ {
+ int c = in->get();
+ if ( c <= 0 )
+ {
+ SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN );
+ break;
+ }
+ (*tag) += (char) c;
+ }
+
+ if ( in->good() )
+ {
+ // We now have something we presume to be a node of
+ // some sort. Identify it, and call the node to
+ // continue streaming.
+ TiXmlNode* node = Identify( tag->c_str() + tagIndex, TIXML_DEFAULT_ENCODING );
+
+ if ( node )
+ {
+ node->StreamIn( in, tag );
+ bool isElement = node->ToElement() != 0;
+ delete node;
+ node = 0;
+
+ // If this is the root element, we're done. Parsing will be
+ // done by the >> operator.
+ if ( isElement )
+ {
+ return;
+ }
+ }
+ else
+ {
+ SetError( TIXML_ERROR, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return;
+ }
+ }
+ }
+ // We should have returned sooner.
+ SetError( TIXML_ERROR, 0, 0, TIXML_ENCODING_UNKNOWN );
+}
+
+
+const char* TiXmlDocument::Parse
+ ( const char* p, TiXmlParsingData* prevData, TiXmlEncoding encoding )
+{
+ ClearError();
+
+ // Parse away, at the document level. Since a document
+ // contains nothing but other tags, most of what happens
+ // here is skipping white space.
+ // sherm 100319: I changed this so that untagged top-level text is
+ // parsed as a Text node rather than a parsing error. CDATA text was
+ // already allowed at the top level so this seems more consistent.
+ if ( !p || !*p )
+ {
+ SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return 0;
+ }
+
+ // Note that, for a document, this needs to come
+ // before the while space skip, so that parsing
+ // starts from the pointer we are given.
+ location.Clear();
+ if ( prevData )
+ {
+ location.row = prevData->cursor.row;
+ location.col = prevData->cursor.col;
+ }
+ else
+ {
+ location.row = 0;
+ location.col = 0;
+ }
+ TiXmlParsingData data( p, TabSize(), location.row, location.col );
+ location = data.Cursor();
+
+ if ( encoding == TIXML_ENCODING_UNKNOWN )
+ {
+ // Check for the Microsoft UTF-8 lead bytes.
+ const unsigned char* pU = (const unsigned char*)p;
+ if ( *(pU+0) && *(pU+0) == TIXML_UTF_LEAD_0
+ && *(pU+1) && *(pU+1) == TIXML_UTF_LEAD_1
+ && *(pU+2) && *(pU+2) == TIXML_UTF_LEAD_2 )
+ {
+ encoding = TIXML_ENCODING_UTF8;
+ useMicrosoftBOM = true;
+ }
+ }
+
+ // Remember the start of white space in case we end up reading a text
+ // element in a "keep white space" mode.
+ const char* pWithWhiteSpace = p;
+ p = SkipWhiteSpace( p, encoding );
+ if ( !p )
+ {
+ SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return 0;
+ }
+
+ // sherm 100319: ignore all but the first Declaration
+ bool haveSeenDeclaration = false;
+ while ( p && *p )
+ {
+ TiXmlNode* node = 0;
+ if ( *p != '<' )
+ { // sherm 100319: I added this case by stealing the code from
+ // Element parsing; see above comment.
+ // Take what we have, make a text element.
+ TiXmlText* textNode = new TiXmlText( "" );
+
+ if ( !textNode )
+ {
+ SetError( TIXML_ERROR_OUT_OF_MEMORY, 0, 0, encoding );
+ return 0;
+ }
+
+ if ( TiXmlBase::IsWhiteSpaceCondensed() )
+ {
+ p = textNode->Parse( p, &data, encoding );
+ }
+ else
+ {
+ // Special case: we want to keep the white space
+ // so that leading spaces aren't removed.
+ p = textNode->Parse( pWithWhiteSpace, &data, encoding );
+ }
+
+ if ( !textNode->Blank() ) {
+ LinkEndChild( textNode );
+ node = textNode;
+ }
+ else
+ delete textNode;
+ }
+ else // We saw a '<', now identify what kind of tag it is.
+ {
+ TiXmlNode* node = Identify( p, encoding );
+ if ( node )
+ {
+ p = node->Parse( p, &data, encoding );
+ if (node->ToDeclaration()) {
+ if (haveSeenDeclaration) {
+ delete node; node=0; // ignore duplicate Declaration
+ } else
+ haveSeenDeclaration = true;
+ }
+ if (node)
+ LinkEndChild( node );
+ }
+ else
+ {
+ // If Identify fails then no further parsing is possible.
+ break;
+ }
+ }
+
+ // Did we get encoding info?
+ if ( encoding == TIXML_ENCODING_UNKNOWN
+ && node && node->ToDeclaration() )
+ {
+ TiXmlDeclaration* dec = node->ToDeclaration();
+ const char* enc = dec->Encoding();
+ assert( enc );
+
+ if ( *enc == 0 )
+ encoding = TIXML_ENCODING_UTF8;
+ else if ( StringEqual( enc, "UTF-8", true, TIXML_ENCODING_UNKNOWN ) )
+ encoding = TIXML_ENCODING_UTF8;
+ else if ( StringEqual( enc, "UTF8", true, TIXML_ENCODING_UNKNOWN ) )
+ encoding = TIXML_ENCODING_UTF8; // incorrect, but be nice
+ else
+ encoding = TIXML_ENCODING_LEGACY;
+ }
+
+ pWithWhiteSpace = p;
+ p = SkipWhiteSpace( p, encoding );
+ }
+
+ // Was this empty?
+ if ( !firstChild ) {
+ SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, encoding );
+ return 0;
+ }
+
+ // All is well.
+ return p;
+}
+
+void TiXmlDocument::SetError
+ ( int err, const char* pError, TiXmlParsingData* data, TiXmlEncoding encoding )
+{
+ // The first error in a chain is more accurate - don't set again!
+ if ( error )
+ return;
+
+ assert( err > 0 && err < TIXML_ERROR_STRING_COUNT );
+ error = true;
+ errorId = err;
+ errorDesc = errorString[ errorId ];
+
+ errorLocation.Clear();
+ if ( pError && data )
+ {
+ data->Stamp( pError, encoding );
+ errorLocation = data->Cursor();
+ }
+}
+
+// We're expecting a tag; identify what kind here and allocate the
+// appropriate type of node (but don't parse yet). We'll return null if
+// we can't find a "<" as the next non-whitespace character, or if we
+// find the "<" but it is followed by nothing but whitespace. Note that
+// we're only looking at the beginning of the tag, we won't check to see
+// if it is terminated properly.
+TiXmlNode* TiXmlNode::Identify( const char* p, TiXmlEncoding encoding )
+{
+ TiXmlNode* returnNode = 0;
+
+ p = SkipWhiteSpace( p, encoding );
+ if( !p || !*p || *p != '<' )
+ {
+ return 0; // couldn't find the tag-opening "<"
+ }
+
+ TiXmlDocument* doc = GetDocument();
+ p = SkipWhiteSpace( p, encoding );
+
+ if ( !p || !*p )
+ {
+ return 0; // a lone "<" at the end of the document
+ }
+
+ // What is this thing?
+ // - Elements: < X where X is a letter or underscore;
+ // white space optional between "<" and "X".
+ // - Comments: <!--
+ // - Declaration: <?xml
+ // - CDATA Text: <![CDATA[
+ // - All other tags are unknown to tinyxml.
+ //
+
+ const char* xmlHeader = { "<?xml" };
+ const char* commentHeader = { "<!--" };
+ const char* dtdHeader = { "<!" }; // treated as "unknown"
+ const char* cdataHeader = { "<![CDATA[" };
+
+ if ( StringEqual( p, xmlHeader, true, encoding ) )
+ {
+ #ifdef DEBUG_PARSER
+ TIXML_LOG( "XML parsing Declaration\n" );
+ #endif
+ returnNode = new TiXmlDeclaration();
+ }
+ else if ( StringEqual( p, commentHeader, false, encoding ) )
+ {
+ #ifdef DEBUG_PARSER
+ TIXML_LOG( "XML parsing Comment\n" );
+ #endif
+ returnNode = new TiXmlComment();
+ }
+ else if ( StringEqual( p, cdataHeader, false, encoding ) )
+ {
+ #ifdef DEBUG_PARSER
+ TIXML_LOG( "XML parsing CDATA\n" );
+ #endif
+ TiXmlText* text = new TiXmlText( "" );
+ text->SetCDATA( true );
+ returnNode = text;
+ }
+ else if ( StringEqual( p, dtdHeader, false, encoding ) )
+ {
+ #ifdef DEBUG_PARSER
+ TIXML_LOG( "XML parsing Unknown(1)\n" );
+ #endif
+ returnNode = new TiXmlUnknown();
+ }
+ else if ( IsAlpha( *(p+1), encoding )
+ || *(p+1) == '_' )
+ {
+ #ifdef DEBUG_PARSER
+ TIXML_LOG( "XML parsing Element\n" );
+ #endif
+ returnNode = new TiXmlElement( "" );
+ }
+ else
+ {
+ #ifdef DEBUG_PARSER
+ TIXML_LOG( "XML parsing Unknown(2)\n" );
+ #endif
+ returnNode = new TiXmlUnknown();
+ }
+
+ if ( returnNode )
+ {
+ // Set the parent, so it can report errors
+ returnNode->parent = this;
+ }
+ else
+ {
+ if ( doc )
+ doc->SetError( TIXML_ERROR_OUT_OF_MEMORY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ }
+ return returnNode;
+}
+
+
+void TiXmlElement::StreamIn (std::istream * in, String * tag)
+{
+ // We're called with some amount of pre-parsing. That is, some of "this"
+ // element is in "tag". Go ahead and stream to the closing ">"
+ while( in->good() )
+ {
+ int c = in->get();
+ if ( c <= 0 )
+ {
+ TiXmlDocument* document = GetDocument();
+ if ( document )
+ document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return;
+ }
+ (*tag) += (char) c ;
+
+ if ( c == '>' )
+ break;
+ }
+
+ if ( tag->length() < 3 ) return;
+
+ // Okay...if we are a "/>" tag, then we're done. We've read a complete tag.
+ // If not, identify and stream.
+
+ if ( tag->at( tag->length() - 1 ) == '>'
+ && tag->at( tag->length() - 2 ) == '/' )
+ {
+ // All good!
+ return;
+ }
+ else if ( tag->at( tag->length() - 1 ) == '>' )
+ {
+ // There is more. Could be:
+ // text
+ // cdata text (which looks like another node)
+ // closing tag
+ // another node.
+ for ( ;; )
+ {
+ StreamWhiteSpace( in, tag );
+
+ // Do we have text?
+ if ( in->good() && in->peek() != '<' )
+ {
+ // Yep, text.
+ TiXmlText text( "" );
+ text.StreamIn( in, tag );
+
+ // What follows text is a closing tag or another node.
+ // Go around again and figure it out.
+ continue;
+ }
+
+ // We now have either a closing tag...or another node.
+ // We should be at a "<", regardless.
+ if ( !in->good() ) return;
+ assert( in->peek() == '<' );
+ int tagIndex = (int) tag->length();
+
+ bool closingTag = false;
+ bool firstCharFound = false;
+
+ for( ;; )
+ {
+ if ( !in->good() )
+ return;
+
+ int c = in->peek();
+ if ( c <= 0 )
+ {
+ TiXmlDocument* document = GetDocument();
+ if ( document )
+ document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return;
+ }
+
+ if ( c == '>' )
+ break;
+
+ *tag += (char) c;
+ in->get();
+
+ // Early out if we find the CDATA id.
+ if ( c == '[' && tag->size() >= 9 )
+ {
+ size_t len = tag->size();
+ const char* start = tag->c_str() + len - 9;
+ if ( strcmp( start, "<![CDATA[" ) == 0 ) {
+ assert( !closingTag );
+ break;
+ }
+ }
+
+ if ( !firstCharFound && c != '<' && !IsWhiteSpace( c ) )
+ {
+ firstCharFound = true;
+ if ( c == '/' )
+ closingTag = true;
+ }
+ }
+ // If it was a closing tag, then read in the closing '>' to clean up the input stream.
+ // If it was not, the streaming will be done by the tag.
+ if ( closingTag )
+ {
+ if ( !in->good() )
+ return;
+
+ int c = in->get();
+ if ( c <= 0 )
+ {
+ TiXmlDocument* document = GetDocument();
+ if ( document )
+ document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return;
+ }
+ assert( c == '>' );
+ *tag += (char) c;
+
+ // We are done, once we've found our closing tag.
+ return;
+ }
+ else
+ {
+ // If not a closing tag, id it, and stream.
+ const char* tagloc = tag->c_str() + tagIndex;
+ TiXmlNode* node = Identify( tagloc, TIXML_DEFAULT_ENCODING );
+ if ( !node )
+ return;
+ node->StreamIn( in, tag );
+ delete node;
+ node = 0;
+
+ // No return: go around from the beginning: text, closing tag, or node.
+ }
+ }
+ }
+}
+
+
+const char* TiXmlElement::Parse
+ ( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding )
+{
+ p = SkipWhiteSpace( p, encoding );
+ TiXmlDocument* document = GetDocument();
+
+ if ( !p || !*p )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_PARSING_ELEMENT, 0, 0, encoding );
+ return 0;
+ }
+
+ if ( data )
+ {
+ data->Stamp( p, encoding );
+ location = data->Cursor();
+ }
+
+ if ( *p != '<' )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_PARSING_ELEMENT, p, data, encoding );
+ return 0;
+ }
+
+ p = SkipWhiteSpace( p+1, encoding );
+
+ // Read the name.
+ const char* pErr = p;
+
+ p = ReadName( p, &value, encoding );
+ if ( !p || !*p )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_FAILED_TO_READ_ELEMENT_NAME, pErr, data, encoding );
+ return 0;
+ }
+
+ String endTag ("</");
+ endTag += value;
+ endTag += ">";
+
+ // Check for and read attributes. Also look for an empty
+ // tag or an end tag.
+ while ( p && *p )
+ {
+ pErr = p;
+ p = SkipWhiteSpace( p, encoding );
+ if ( !p || !*p )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, pErr, data, encoding );
+ return 0;
+ }
+ if ( *p == '/' )
+ {
+ ++p;
+ // Empty tag.
+ if ( *p != '>' )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_PARSING_EMPTY, p, data, encoding );
+ return 0;
+ }
+ return (p+1);
+ }
+ else if ( *p == '>' )
+ {
+ // Done with attributes (if there were any.)
+ // Read the value -- which can include other
+ // elements -- read the end tag, and return.
+ ++p;
+ // Note this is an Element method, and will set the error if
+ // one happens.
+ p = ReadValue( p, data, encoding );
+ if ( !p || !*p ) {
+ // We were looking for the end tag, but found nothing.
+ // Fix for [ 1663758 ] Failure to report error on bad XML
+ if ( document ) document->SetError( TIXML_ERROR_READING_END_TAG, p, data, encoding );
+ return 0;
+ }
+
+ // We should find the end tag now
+ if ( StringEqual( p, endTag.c_str(), false, encoding ) )
+ {
+ p += endTag.length();
+ return p;
+ }
+ else
+ {
+ if ( document ) document->SetError( TIXML_ERROR_READING_END_TAG, p, data, encoding );
+ return 0;
+ }
+ }
+ else
+ {
+ // Try to read an attribute:
+ TiXmlAttribute* attrib = new TiXmlAttribute();
+ if ( !attrib )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_OUT_OF_MEMORY, pErr, data, encoding );
+ return 0;
+ }
+
+ attrib->SetDocument( document );
+ pErr = p;
+ p = attrib->Parse( p, data, encoding );
+
+ if ( !p || !*p )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_PARSING_ELEMENT, pErr, data, encoding );
+ delete attrib;
+ return 0;
+ }
+
+ // Handle the strange case of double attributes by ignoring
+ // all but the last one.
+ // sherm 100319: I fixed this
+ TiXmlAttribute* node = attributeSet.Find( attrib->NameStr() );
+ if ( node )
+ {
+ node->SetValue( attrib->Value() );
+ delete attrib;
+ } else
+ attributeSet.Add( attrib );
+ }
+ }
+ return p;
+}
+
+
+const char* TiXmlElement::ReadValue
+ ( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding )
+{
+ TiXmlDocument* document = GetDocument();
+
+ // Read in text and elements in any order.
+ const char* pWithWhiteSpace = p;
+ p = SkipWhiteSpace( p, encoding );
+
+ while ( p && *p )
+ {
+ if ( *p != '<' )
+ {
+ // Take what we have, make a text element.
+ TiXmlText* textNode = new TiXmlText( "" );
+
+ if ( !textNode )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_OUT_OF_MEMORY, 0, 0, encoding );
+ return 0;
+ }
+
+ if ( TiXmlBase::IsWhiteSpaceCondensed() )
+ {
+ p = textNode->Parse( p, data, encoding );
+ }
+ else
+ {
+ // Special case: we want to keep the white space
+ // so that leading spaces aren't removed.
+ p = textNode->Parse( pWithWhiteSpace, data, encoding );
+ }
+
+ if ( !textNode->Blank() )
+ LinkEndChild( textNode );
+ else
+ delete textNode;
+ }
+ else
+ {
+ // We hit a '<'
+ // Have we hit a new element or an end tag? This could also be
+ // a TiXmlText in the "CDATA" style.
+ if ( StringEqual( p, "</", false, encoding ) )
+ {
+ return p;
+ }
+ else
+ {
+ TiXmlNode* node = Identify( p, encoding );
+ if ( node )
+ {
+ p = node->Parse( p, data, encoding );
+ LinkEndChild( node );
+ }
+ else
+ {
+ return 0;
+ }
+ }
+ }
+ pWithWhiteSpace = p;
+ p = SkipWhiteSpace( p, encoding );
+ }
+
+ if ( !p )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_READING_ELEMENT_VALUE, 0, 0, encoding );
+ }
+ return p;
+}
+
+
+void TiXmlUnknown::StreamIn( std::istream * in, String * tag )
+{
+ while ( in->good() )
+ {
+ int c = in->get();
+ if ( c <= 0 )
+ {
+ TiXmlDocument* document = GetDocument();
+ if ( document )
+ document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return;
+ }
+ (*tag) += (char) c;
+
+ if ( c == '>' )
+ {
+ // All is well.
+ return;
+ }
+ }
+}
+
+
+const char* TiXmlUnknown::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding )
+{
+ TiXmlDocument* document = GetDocument();
+ p = SkipWhiteSpace( p, encoding );
+
+ if ( data )
+ {
+ data->Stamp( p, encoding );
+ location = data->Cursor();
+ }
+ if ( !p || !*p || *p != '<' )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_PARSING_UNKNOWN, p, data, encoding );
+ return 0;
+ }
+ ++p;
+ value = "";
+
+ while ( p && *p && *p != '>' )
+ {
+ value += *p;
+ ++p;
+ }
+
+ if ( !p )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_PARSING_UNKNOWN, 0, 0, encoding );
+ }
+ if ( *p == '>' )
+ return p+1;
+ return p;
+}
+
+void TiXmlComment::StreamIn( std::istream * in, String * tag )
+{
+ while ( in->good() )
+ {
+ int c = in->get();
+ if ( c <= 0 )
+ {
+ TiXmlDocument* document = GetDocument();
+ if ( document )
+ document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return;
+ }
+
+ (*tag) += (char) c;
+
+ if ( c == '>'
+ && tag->at( tag->length() - 2 ) == '-'
+ && tag->at( tag->length() - 3 ) == '-' )
+ {
+ // All is well.
+ return;
+ }
+ }
+}
+
+
+const char* TiXmlComment::Parse
+ ( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding )
+{
+ TiXmlDocument* document = GetDocument();
+ value = "";
+
+ p = SkipWhiteSpace( p, encoding );
+
+ if ( data )
+ {
+ data->Stamp( p, encoding );
+ location = data->Cursor();
+ }
+ const char* startTag = "<!--";
+ const char* endTag = "-->";
+
+ if ( !StringEqual( p, startTag, false, encoding ) )
+ {
+ document->SetError( TIXML_ERROR_PARSING_COMMENT, p, data, encoding );
+ return 0;
+ }
+ p += strlen( startTag );
+
+ // [ 1475201 ] TinyXML parses entities in comments
+ // Oops - ReadText doesn't work, because we don't want to parse the
+ // entities.
+ // p = ReadText( p, &value, false, endTag, false, encoding );
+ //
+ // from the XML spec:
+ /* [Definition:
+ Comments may appear anywhere in a document outside other markup; in
+ addition, they may appear within the document type declaration at places
+ allowed by the grammar. They are not part of the document's character data;
+ an XML processor MAY, but need not, make it possible for an application to
+ retrieve the text of comments. For compatibility, the string "--"
+ (double-hyphen) MUST NOT occur within comments.] Parameter entity
+ references MUST NOT be recognized within comments.
+
+ An example of a comment:
+
+ <!-- declarations for <head> & <body> -->
+ */
+
+ value = "";
+ // Keep all the white space.
+ while ( p && *p && !StringEqual( p, endTag, false, encoding ) )
+ {
+ value.append( p, 1 );
+ ++p;
+ }
+ if ( p && *p )
+ p += strlen( endTag );
+ else { // unclosed comment ate up the whole document
+ document->SetError( TIXML_ERROR_PARSING_COMMENT, p, data, encoding );
+ return 0;
+ }
+
+ return p;
+}
+
+
+const char* TiXmlAttribute::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding )
+{
+ p = SkipWhiteSpace( p, encoding );
+ if ( !p || !*p ) return 0;
+
+ if ( data )
+ {
+ data->Stamp( p, encoding );
+ location = data->Cursor();
+ }
+ // Read the name, the '=' and the value.
+ const char* pErr = p;
+ p = ReadName( p, &name, encoding );
+ if ( !p || !*p )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, pErr, data, encoding );
+ return 0;
+ }
+ p = SkipWhiteSpace( p, encoding );
+ if ( !p || !*p || *p != '=' )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding );
+ return 0;
+ }
+
+ ++p; // skip '='
+ p = SkipWhiteSpace( p, encoding );
+ if ( !p || !*p )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding );
+ return 0;
+ }
+
+ const char* end;
+ const char SINGLE_QUOTE = '\'';
+ const char DOUBLE_QUOTE = '\"';
+
+ if ( *p == SINGLE_QUOTE )
+ {
+ ++p;
+ end = "\'"; // single quote in string
+ p = ReadText( p, &value, false, end, false, encoding );
+ }
+ else if ( *p == DOUBLE_QUOTE )
+ {
+ ++p;
+ end = "\""; // double quote in string
+ p = ReadText( p, &value, false, end, false, encoding );
+ }
+ else
+ {
+ // All attribute values should be in single or double quotes.
+ // But this is such a common error that the parser will try
+ // its best, even without them.
+ value = "";
+ while ( p && *p // existence
+ && !IsWhiteSpace( *p ) && *p != '\n' && *p != '\r' // whitespace
+ && *p != '/' && *p != '>' ) // tag end
+ {
+ if ( *p == SINGLE_QUOTE || *p == DOUBLE_QUOTE ) {
+ // [ 1451649 ] Attribute values with trailing quotes not handled correctly
+ // We did not have an opening quote but seem to have a
+ // closing one. Give up and throw an error.
+ if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding );
+ return 0;
+ }
+ value += *p;
+ ++p;
+ }
+ }
+ return p;
+}
+
+
+void TiXmlText::StreamIn( std::istream * in, String * tag )
+{
+ while ( in->good() )
+ {
+ int c = in->peek();
+ if ( !cdata && (c == '<' ) )
+ {
+ return;
+ }
+ if ( c <= 0 )
+ {
+ TiXmlDocument* document = GetDocument();
+ if ( document )
+ document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return;
+ }
+
+ (*tag) += (char) c;
+ in->get(); // "commits" the peek made above
+
+ if ( cdata && c == '>' && tag->size() >= 3 ) {
+ size_t len = tag->size();
+ if ( (*tag)[len-2] == ']' && (*tag)[len-3] == ']' ) {
+ // terminator of cdata.
+ return;
+ }
+ }
+ }
+}
+
+
+const char* TiXmlText::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding )
+{
+ value = "";
+ TiXmlDocument* document = GetDocument();
+
+ if ( data )
+ {
+ data->Stamp( p, encoding );
+ location = data->Cursor();
+ }
+
+ const char* const startTag = "<![CDATA[";
+ const char* const endTag = "]]>";
+
+ if ( cdata || StringEqual( p, startTag, false, encoding ) )
+ {
+ cdata = true;
+
+ if ( !StringEqual( p, startTag, false, encoding ) )
+ {
+ document->SetError( TIXML_ERROR_PARSING_CDATA, p, data, encoding );
+ return 0;
+ }
+ p += strlen( startTag );
+
+ // Keep all the white space, ignore the encoding, etc.
+ while ( p && *p
+ && !StringEqual( p, endTag, false, encoding )
+ )
+ {
+ value += *p;
+ ++p;
+ }
+
+ String dummy;
+ p = ReadText( p, &dummy, false, endTag, false, encoding );
+ return p;
+ }
+ else
+ {
+ bool ignoreWhite = true;
+
+ const char* end = "<";
+ p = ReadText( p, &value, ignoreWhite, end, false, encoding );
+ if ( p )
+ return p-1; // don't truncate the '<'
+ return 0;
+ }
+}
+
+
+void TiXmlDeclaration::StreamIn( std::istream * in, String * tag )
+{
+ while ( in->good() )
+ {
+ int c = in->get();
+ if ( c <= 0 )
+ {
+ TiXmlDocument* document = GetDocument();
+ if ( document )
+ document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return;
+ }
+ (*tag) += (char) c;
+
+ if ( c == '>' )
+ {
+ // All is well.
+ return;
+ }
+ }
+}
+
+
+const char* TiXmlDeclaration::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding _encoding )
+{
+ p = SkipWhiteSpace( p, _encoding );
+ // Find the beginning, find the end, and look for
+ // the stuff in-between.
+ TiXmlDocument* document = GetDocument();
+ if ( !p || !*p || !StringEqual( p, "<?xml", true, _encoding ) )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_PARSING_DECLARATION, 0, 0, _encoding );
+ return 0;
+ }
+ if ( data )
+ {
+ data->Stamp( p, _encoding );
+ location = data->Cursor();
+ }
+ p += 5;
+
+ version = "";
+ encoding = "";
+ standalone = "";
+
+ while ( p && *p )
+ {
+ if ( *p == '>' )
+ {
+ ++p;
+ return p;
+ }
+
+ p = SkipWhiteSpace( p, _encoding );
+ if ( StringEqual( p, "version", true, _encoding ) )
+ {
+ TiXmlAttribute attrib;
+ p = attrib.Parse( p, data, _encoding );
+ version = attrib.Value();
+ }
+ else if ( StringEqual( p, "encoding", true, _encoding ) )
+ {
+ TiXmlAttribute attrib;
+ p = attrib.Parse( p, data, _encoding );
+ encoding = attrib.Value();
+ }
+ else if ( StringEqual( p, "standalone", true, _encoding ) )
+ {
+ TiXmlAttribute attrib;
+ p = attrib.Parse( p, data, _encoding );
+ standalone = attrib.Value();
+ }
+ else
+ {
+ // Read over whatever it is.
+ while( p && *p && *p != '>' && !IsWhiteSpace( *p ) )
+ ++p;
+ }
+ }
+ return 0;
+}
+
+bool TiXmlText::Blank() const
+{
+ for ( int i=0; i< (int)value.length(); i++ )
+ if ( !IsWhiteSpace( value[i] ) )
+ return false;
+ return true;
+}
+
+
+} // namespace SimTK
+
diff --git a/SimTKcommon/staticTarget/CMakeLists.txt b/SimTKcommon/staticTarget/CMakeLists.txt
new file mode 100644
index 0000000..c3e90fe
--- /dev/null
+++ b/SimTKcommon/staticTarget/CMakeLists.txt
@@ -0,0 +1,43 @@
+## This whole directory exists just so I could define this extra
+## preprocessor values.
+
+ADD_DEFINITIONS(-DSimTK_SimTKCOMMON_BUILDING_STATIC_LIBRARY
+ -DSimTK_USE_STATIC_LIBRARIES)
+
+IF(BUILD_UNVERSIONED_LIBRARIES)
+ ADD_LIBRARY(${STATIC_TARGET} STATIC
+ ${SOURCE_FILES} ${SOURCE_INCLUDE_FILES}
+ ${API_ABS_INCLUDE_FILES})
+
+ ADD_DEPENDENCIES(${STATIC_TARGET} PlatformFiles)
+
+ TARGET_LINK_LIBRARIES(${STATIC_TARGET} ${MATH_LIBS_TO_USE})
+
+ SET_TARGET_PROPERTIES(${STATIC_TARGET} PROPERTIES
+ PROJECT_LABEL "Code - ${STATIC_TARGET}")
+
+ INSTALL(TARGETS ${STATIC_TARGET}
+ PERMISSIONS OWNER_READ OWNER_WRITE GROUP_READ GROUP_WRITE WORLD_READ
+ ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ RUNTIME DESTINATION ${CMAKE_INSTALL_FULL_BINDIR} )
+ENDIF(BUILD_UNVERSIONED_LIBRARIES)
+
+IF(BUILD_VERSIONED_LIBRARIES)
+ ADD_LIBRARY(${STATIC_TARGET_VN} STATIC
+ ${SOURCE_FILES} ${SOURCE_INCLUDE_FILES}
+ ${API_ABS_INCLUDE_FILES})
+
+ ADD_DEPENDENCIES(${STATIC_TARGET_VN} PlatformFiles)
+
+ TARGET_LINK_LIBRARIES(${STATIC_TARGET_VN} ${MATH_LIBS_TO_USE_VN})
+
+ SET_TARGET_PROPERTIES(${STATIC_TARGET_VN} PROPERTIES
+ PROJECT_LABEL "Code - ${STATIC_TARGET_VN}")
+
+ INSTALL(TARGETS ${STATIC_TARGET_VN}
+ PERMISSIONS OWNER_READ OWNER_WRITE GROUP_READ GROUP_WRITE WORLD_READ
+ ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ RUNTIME DESTINATION ${CMAKE_INSTALL_FULL_BINDIR} )
+ENDIF(BUILD_VERSIONED_LIBRARIES)
diff --git a/SimTKcommon/tests/BNTTest.cpp b/SimTKcommon/tests/BNTTest.cpp
new file mode 100644
index 0000000..ebab60e
--- /dev/null
+++ b/SimTKcommon/tests/BNTTest.cpp
@@ -0,0 +1,381 @@
+/* -------------------------------------------------------------------------- *
+ * SimTK Simbody: SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+//#define SimTK_DEFAULT_PRECISION 1
+//#define SimTK_DEFAULT_PRECISION 2
+//#define SimTK_DEFAULT_PRECISION 4
+
+#include "SimTKcommon.h"
+
+#include <iostream>
+#include <iomanip>
+#include <limits>
+#include <complex>
+#include <cstdio>
+#include <cmath>
+
+using std::cout;
+using std::endl;
+using std::setprecision;
+using std::complex;
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS((cond), "Assertion failed");}
+// Assert that two real-valued calculations should produce the same result to within a few
+// machine roundoff errors.
+#define ASSERT_EPS(x,y) {ASSERT(std::abs((x)-(y))<=(4*Eps));}
+// Assert that two complex-valued calculations should produce the same result (in both real and
+// imaginary components) to within a few machine roundoff errors.
+#define ASSERT_EPSX(x,y) {ASSERT_EPS((x).real(),(y).real()); \
+ ASSERT_EPS((x).imag(),(y).imag());}
+
+using namespace SimTK;
+
+// use this to keep the compiler from whining about divides by zero
+static Real getRealZero();
+
+int main() {
+ try
+ { const Real zero = 0., one = 1., two = 2.;
+ const Complex oneTwo(1.,2.);
+ const Complex threeFour(3.,4.);
+ const complex<float> fcinf = CNT< complex<float> >::getInfinity();
+
+ // nan tests
+ const Real nan = CNT<Real>::getNaN();
+ const float fnan = NTraits<float>::getNaN();
+ const double dnan = NTraits<double>::getNaN();
+ const long double lnan = NTraits<long double>::getNaN();
+ const std::complex<float> cfnan(fnan, fnan);
+ const std::complex<double> cdnan(3, dnan);
+ const std::complex<long double> clnan(lnan, 0L);
+ const conjugate<float> jfnan(fnan, 0.09f);
+ const conjugate<double> jdnan(3, dnan);
+ const conjugate<long double> jlnan(lnan, lnan);
+ const negator<Real>& nzero = reinterpret_cast<const negator<Real>&>(zero);
+ const negator<Real>& ntwo = reinterpret_cast<const negator<Real>&>(two);
+ const negator<float>& nfnan = reinterpret_cast<const negator<float>&>(fnan);
+ const negator< std::complex<float> >& ncfnan = reinterpret_cast<const negator<std::complex<float> >&>(cfnan);
+ const negator< conjugate<long double> >& njlnan = reinterpret_cast<const negator<conjugate<long double> >&>(jlnan);
+
+ writeUnformatted(std::cout, fcinf);
+ Array_< negator<Complex> > arrc;
+ arrc.push_back(oneTwo);
+ arrc.push_back(threeFour);
+ arrc.push_back(fcinf);
+ writeUnformatted(cout, arrc); cout << endl;
+ writeUnformatted(cout, Vec3(1,2,3)); cout << endl;
+ Vector vxxx(Vec3(4,NaN,-3));
+ writeUnformatted(cout, vxxx); cout << endl;
+ cout << vxxx << "\n";
+ writeUnformatted(cout, Mat34( 1, 2, 3, 4,
+ 5, NaN, 7, 8,
+ Infinity, 10, -Infinity, 12 ));
+ cout << endl;
+ writeUnformatted(cout, SymMat33( 1,
+ 2, 3,
+ NaN, 5, 6 )); cout << endl;
+
+ Matrix mxxx(Mat34( 1, 2, 3, 4,
+ 5, NaN, 7, 8,
+ Infinity, 10, -Infinity, 12 ));
+ writeUnformatted(cout, mxxx); cout << endl;
+ cout << mxxx;
+
+ double inval;
+ std::stringstream ss("1.3 nan 4 6 -3 -inf");
+ while (readUnformatted(ss, inval))
+ cout << "'" << String(inval) << "'\n";
+
+ std::stringstream ss2("1 2 3 nan 4 inf");
+ readUnformatted(ss2, arrc);
+ cout << "arrc=" << arrc << endl;
+ writeUnformatted(cout, arrc);
+
+ ss2.clear(); ss2.seekg(0, std::ios::beg);
+ Vec6 myv6;
+ readUnformatted(ss2, myv6);
+ writeUnformatted(cout << "myv6=", myv6);
+ ss2.clear(); ss2.seekg(0, std::ios::beg);
+ Matrix mym23(2,3);
+ fillUnformatted(ss2, mym23);
+ writeUnformatted(cout << "\nmym23=", mym23);
+ ss2.clear(); ss2.seekg(0, std::ios::beg);
+ Array_<float> axx(2), ayy(4);
+ ArrayView_<float> avxx(axx), avyy(ayy);
+ readUnformatted(ss2, avxx);
+ readUnformatted(ss2, avyy);
+ writeUnformatted(cout << "u axx=",axx); cout<<endl;
+ writeUnformatted(cout << "u ayy=",ayy); cout<<endl;
+ writeFormatted(cout << "f axx=",axx); cout<<endl;
+ writeFormatted(cout << "f ayy=",ayy); cout<<endl;
+
+ ASSERT(isNaN(nan));
+ ASSERT(isNaN(-nan));
+ ASSERT(isNaN(NaN)); // SimTK::NaN
+ ASSERT(isNaN(-NaN)); // SimTK::NaN
+ ASSERT(!isNaN(one));
+ ASSERT(!isNaN(Infinity));
+ ASSERT(isNaN(0./getRealZero()));
+ ASSERT(!isNaN(1./getRealZero())); // Infinity
+
+ ASSERT(isNaN(fnan)); ASSERT(isNaN(dnan)); // float,double
+ ASSERT(isNaN(cfnan)); ASSERT(isNaN(cdnan)); // complex<float,double>
+ ASSERT(!isNaN(fcinf)); // complex infinity, not NaN
+ ASSERT(isNaN(jfnan)); ASSERT(isNaN(jdnan)); // conjugate<float,double>
+
+ // Check negator behavior
+ ASSERT(nzero == zero); ASSERT(-nzero == zero);
+ ASSERT(ntwo == -two); ASSERT(-ntwo == two);
+ ASSERT(isNaN(nfnan)); ASSERT(isNaN(-nfnan));
+ ASSERT(!isNaN(nzero)); ASSERT(!isNaN(-ntwo));
+ ASSERT(isNaN(ncfnan)); ASSERT(isNaN(-ncfnan));
+
+
+ cout << "one=" << one << " two=" << two << endl;
+ cout << "oneTwo=" << oneTwo << " threeFour=" << threeFour << endl;
+
+ cout << "negator(one)=" << negator<Real>(one) << endl;
+ cout << "reinterp<negator>(one)=" << reinterpret_cast<const negator<Real>&>(one) << endl;
+
+ cout << "fcinf=" << fcinf << endl;
+ cout << "nan=" << nan << endl;
+
+ cout << "negator<Real>::inf=" << CNT<negator<Real> >::getInfinity() << endl;
+ cout << "negator<Real>::nan=" << CNT<negator<Real> >::getNaN() << endl;
+ //cout << "conj(one)=" << SimTK::conj(one) << " conj(oneTwo)=" << SimTK::conj(oneTwo) << endl;
+
+ const conjugate<Real> conj34(threeFour);
+ cout << "conj34=" << conj34 << endl;
+ cout << "complex(conj34)=" <<
+ std::complex<Real>(conj34.real(),conj34.imag()) << endl;
+ cout << "-conj34=" << -conj34 << endl;
+
+ const conjugate<Real>& reconj34 =
+ reinterpret_cast<const conjugate<Real>&>(threeFour);
+ cout << "reconj34=" << reconj34 << endl;
+
+ const negator<conjugate<Real> > negconj34(conj34);
+ cout << "negconj34=" << negconj34 << endl;
+ cout << "negconj34.real()=" << negconj34.real() << endl;
+ cout << "negconj34.imag()=" << negconj34.imag() << endl;
+ const conjugate<Real> crn = (conjugate<Real>)negconj34;
+ cout << "Conj(negconj34)=" << crn << endl;
+
+ typedef negator<conjugate<Real> > NCR;
+ typedef NCR::TWithoutNegator NCRWN;
+ cout << "NCR is a " << typeid(NCR).name() << endl;
+ cout << "NCRWN is a " << typeid(NCRWN).name() << endl;
+
+ const NCR& negreconj34 =
+ reinterpret_cast<const negator<conjugate<Real> >&>(reconj34);
+ cout << "negreconj34=" << negreconj34 << endl;
+ cout << " ... is a " << typeid(negreconj34).name() << endl;
+ cout << "negreconj34.normalize()=" << negreconj34.normalize() << endl;
+ cout << " ... is a " << typeid(negreconj34.normalize()).name() << endl;
+ cout << "(noneg)negreconj34="
+ << CNT<NCR>::castAwayNegatorIfAny(negreconj34) << endl;
+ cout << " ... is a "
+ << typeid(CNT<NCR>::castAwayNegatorIfAny(negreconj34)).name() << endl;
+ const NCRWN& nnn = CNT<NCR>::castAwayNegatorIfAny(negreconj34);
+ cout << "(noneg)negreconj34.normalize()="
+ << CNT<NCRWN>::normalize(nnn) << endl;
+
+ const negator<conjugate<Real> >& nc_threeFour
+ = reinterpret_cast<const negator<conjugate<Real> >&>(threeFour);
+ cout << "nc_threeFour=" << nc_threeFour << " conj(.)="
+ << CNT<negator<conjugate<Real> > >::transpose(nc_threeFour) << endl;
+
+ cout << "NC<C> nan=" << CNT<negator<conjugate<Real> > >::getNaN() << endl;
+ cout << "NC<C> inf=" << CNT<negator<conjugate<Real> > >::getInfinity() << endl;
+
+ cout << "negator<complex<float>>*long double=" <<
+ typeid( negator< complex<float> >::Result<long double>::Mul ).name() << endl;
+ negator< complex<long double> > nlc =
+ negator< complex<float> >::Result<long double>::Mul(complex<long double>(1,2));
+ cout << "nlc=" << nlc << endl;
+
+ cout << "NegConjugate<double>*float=" <<
+ typeid( negator<conjugate<double> >::Result<float>::Mul ).name() << endl;
+ negator<conjugate<double> > ncdc =
+ negator< conjugate<double> >::Result<float>::Mul(complex<double>(9,10));
+ cout << "ncdc=" << ncdc << endl;
+
+ cout << "NegConjugate<float>*complex<double>=" <<
+ typeid( negator<conjugate<float> >::Result< complex<double> >::Mul ).name() << endl;
+ negator< complex<double> > ndc =
+ negator<conjugate<float> >::Result< complex<double> >::Mul(complex<double>(.1,.2));
+ cout << "ndc=" << ndc << endl;
+
+ negator<Complex> x(Complex(Real(7.1),Real(1.7))), y;
+ y = x; y *= Real(2);
+ cout << "x=" << x << "(y=x)*=2. =" << y << endl;
+ cout << "x*2.=" << x*2. << endl;
+ cout << "x*y=" << x*y << endl;
+ cout << "x+y=" << x+y << endl;
+ cout << "x-y=" << x-y << endl;
+ cout << "x+(-y)=" << x+(-y) << endl;
+
+ // In gcc 4.1.2, if you remove this output line then the
+ // corresponding ASSERT below it will fail!
+ cout << "-(-x)+y=" << -(-x)+y << endl;
+ ASSERT_EPSX(x+y, -(-x)+y);
+
+ ASSERT_EPSX(x+y, -((-x)+(-y)));
+ ASSERT_EPSX(x+y, x-(-y));
+ ASSERT_EPSX(x-y, x+(-y));
+ ASSERT_EPSX(x-y, -(y-x));
+ ASSERT_EPSX(x-y, -(-x+y));
+ ASSERT_EPSX(-(x+y), (-x)-y);
+ ASSERT_EPSX(-(x+y), -x-y);
+
+ ASSERT_EPSX(x*y, (-x)*(-y));
+ ASSERT_EPSX(x*y, -x*-y);
+ ASSERT_EPSX(-(x*y), -x*y);
+ ASSERT_EPSX(-(x*y), x*-y);
+
+ ASSERT_EPSX(x/y, (-x)/(-y));
+ ASSERT_EPSX(x/y, -x/-y);
+ ASSERT_EPSX(-(x/y), -x/y);
+ ASSERT_EPSX(-(x/y), x/-y);
+
+ cout << "x+2=" << x+Complex(2,0) << endl;
+ Complex zz = operator+(x,Complex(2,0));
+ cout << "zz=op+(x,2)=" << zz << endl;
+
+ cout << "negator<Complex> x=" << x << endl;
+ cout << "square(x)=" << square(x) << " x*x=" << x*x << " diff=" << square(x)-x*x <<endl;
+ cout << "cube(x)=" << cube(x) << " x*x*x=" << x*x*x << " diff=" << cube(x)-x*x*x <<endl;
+
+ Real pp=27, nn=-14, zzz=0;
+ cout << "Real: sign(27)=" << sign(pp) << " sign(-14)=" << sign(nn) << " sign(0)=" << sign(zzz) << endl;
+ cout << "negator<Real>: sign(27)="
+ << sign(negator<Real>::recast(pp)) << " sign(-14)=" << sign(negator<Real>::recast(nn))
+ << " sign(0)=" << sign(negator<Real>::recast(zzz)) << endl;
+
+
+ // Check mixed-mode complex & conjugate operators
+ complex<float> cff;
+ complex<double> dff;
+ complex<long double> lff;
+ cff * 3.; 3.*cff; cff /3.; 3./cff; cff + 3.;3.+cff;cff-3.;3.-cff;
+ dff * 3.f; 3.f*dff; dff / 3.f; 3.f/dff;dff + 3.f;3.f+dff;dff-3.f;3.f-dff;
+ lff * 3.; 3.*lff; lff/3.; 3./lff;lff + 3.;3.+lff;lff-3.;3.-lff;
+ cff*3;lff/3;3/dff;3+cff;
+ conjugate<float> ccf;
+ conjugate<double> dcf;
+ conjugate<long double> lcf;
+ ccf * 3.; 3.*ccf; ccf /3.; 3./ccf; ccf + 3.;3.+ccf;ccf-3.;3.-ccf;
+ dcf * 3.f; 3.f*dcf; dcf / 3.f; 3.f/dcf;dcf + 3.f;3.f+dcf;dcf-3.f;3.f-dcf;
+ lcf * 3.; 3.*lcf; lcf/3.; 3./lcf;lcf + 3.;3.+lcf;lcf-3.;3.-lcf;
+
+ // Constants in various precisions
+#define STRZ_(X) #X
+#define STRZ(X) STRZ_(X)
+
+
+ printf("\nCONSTANTS IN DEFAULT REAL PRECISION\n");
+ printf("NumDigits=%d, LosslessNumDigits=%d\n", NumDigitsReal, LosslessNumDigitsReal);
+ cout << "e^(i*pi)+1=" << std::pow(E, I*Pi)+1 << endl;
+ cout << "NaN=" << setprecision(LosslessNumDigitsReal) << NaN << endl;
+ cout << "Infinity=" << setprecision(LosslessNumDigitsReal) << Infinity << endl;
+
+ cout << "Eps=" << setprecision(LosslessNumDigitsReal) << Eps << endl;
+ cout << "SqrtEps=" << setprecision(LosslessNumDigitsReal) << SqrtEps << endl;
+ cout << "TinyReal=" << setprecision(LosslessNumDigitsReal) << TinyReal << endl;
+ cout << "SignificantReal=" << setprecision(LosslessNumDigitsReal) << SignificantReal << endl;
+ cout << "LeastPositiveReal=" << setprecision(LosslessNumDigitsReal) << LeastPositiveReal << endl;
+ cout << "MostPositiveReal=" << setprecision(LosslessNumDigitsReal) << MostPositiveReal << endl;
+ cout << "LeastNegativeReal=" << setprecision(LosslessNumDigitsReal) << LeastNegativeReal << endl;
+ cout << "MostNegativeReal=" << setprecision(LosslessNumDigitsReal) << MostNegativeReal << endl;
+ cout << "Zero=" << setprecision(LosslessNumDigitsReal) << Zero << endl;
+ cout << "One=" << setprecision(LosslessNumDigitsReal) << One << endl;
+ cout << "MinusOne=" << setprecision(LosslessNumDigitsReal) << MinusOne << endl;
+ cout << "Two=" << setprecision(LosslessNumDigitsReal) << Two << endl;
+ cout << "Three=" << setprecision(LosslessNumDigitsReal) << Three << endl;
+ cout << "OneHalf=" << setprecision(LosslessNumDigitsReal) << OneHalf << endl;
+ cout << "OneThird=" << setprecision(LosslessNumDigitsReal) << OneThird << endl;
+ cout << "OneFourth=" << setprecision(LosslessNumDigitsReal) << OneFourth << endl;
+ cout << "OneFifth=" << setprecision(LosslessNumDigitsReal) << OneFifth << endl;
+ cout << "OneSixth=" << setprecision(LosslessNumDigitsReal) << OneSixth << endl;
+ cout << "OneSeventh=" << setprecision(LosslessNumDigitsReal) << OneSeventh << endl;
+ cout << "OneEighth=" << setprecision(LosslessNumDigitsReal) << OneEighth << endl;
+ cout << "OneNinth=" << setprecision(LosslessNumDigitsReal) << OneNinth << endl;
+ cout << "Pi=" << setprecision(LosslessNumDigitsReal) << Pi << endl;
+ cout << "OneOverPi=" << setprecision(LosslessNumDigitsReal) << OneOverPi << endl;
+ cout << "E=" << setprecision(LosslessNumDigitsReal) << E << endl;
+ cout << "Log2E=" << setprecision(LosslessNumDigitsReal) << Log2E << endl;
+ cout << "Log10E=" << setprecision(LosslessNumDigitsReal) << Log10E << endl;
+ cout << "Sqrt2=" << setprecision(LosslessNumDigitsReal) << Sqrt2 << endl;
+ cout << "OneOverSqrt2=" << setprecision(LosslessNumDigitsReal) << OneOverSqrt2 << endl;
+ cout << "Sqrt3=" << setprecision(LosslessNumDigitsReal) << Sqrt3 << endl;
+ cout << "OneOverSqrt3=" << setprecision(LosslessNumDigitsReal) << OneOverSqrt3 << endl;
+ cout << "CubeRoot2=" << setprecision(LosslessNumDigitsReal) << CubeRoot2 << endl;
+ cout << "CubeRoot3=" << setprecision(LosslessNumDigitsReal) << CubeRoot3 << endl;
+ cout << "Ln2=" << setprecision(LosslessNumDigitsReal) << Ln2 << endl;
+ cout << "Ln10=" << setprecision(LosslessNumDigitsReal) << Ln10 << endl;
+ cout << "I=" << setprecision(LosslessNumDigitsReal) << I << endl;
+
+ printf("\nSOME CONSTANTS IN VARIOUS PRECISIONS\n");
+
+ printf("PI=%s\n", STRZ(SimTK_PI));
+ cout << "f=" << setprecision(NTraits<float>::getNumDigits()+2) << NTraits<float>::getPi()
+ << " d=" << setprecision(NTraits<double>::getNumDigits()+2) << NTraits<double>::getPi()
+ << " ld=" << setprecision(NTraits<long double>::getNumDigits()+2) << NTraits<long double>::getPi() << endl;
+
+ std::printf("1/sqrt(2)=%.18Lg\n", 1/SimTK_SQRT2);
+ cout << "f=" << setprecision(NTraits<float>::getNumDigits()+2) << NTraits<float>::getOneOverSqrt2()
+ << " d=" << setprecision(NTraits<double>::getNumDigits()+2) << NTraits<double>::getOneOverSqrt2()
+ << " ld=" << setprecision(NTraits<long double>::getNumDigits()+2) << NTraits<long double>::getOneOverSqrt2() << endl;
+
+ printf("Eps f=%.16Lg d=%.16Lg ld=%.16Lg\n",
+ (long double)NTraits<float>::getEps(),
+ (long double)NTraits<double>::getEps(),
+ NTraits<long double>::getEps());
+
+ printf("SqrtEps f=%.16Lg d=%.16Lg ld=%.16Lg\n",
+ (long double)NTraits<float>::getSqrtEps(),
+ (long double)NTraits<double>::getSqrtEps(),
+ NTraits<long double>::getSqrtEps());
+
+ printf("Significant f=%.16Lg d=%.16Lg ld=%.16Lg\n",
+ (long double)NTraits<float>::getSignificant(),
+ (long double)NTraits<double>::getSignificant(),
+ NTraits<long double>::getSignificant());
+
+ printf("Tiny f=%.16Lg d=%.16Lg ld=%.16Lg\n",
+ (long double)NTraits<float>::getTiny(),
+ (long double)NTraits<double>::getTiny(),
+ NTraits<long double>::getTiny());
+
+ } catch(const std::exception& e) {
+ std::cout << "exception: " << e.what() << std::endl;
+ return 1;
+ }
+
+ return 0; // success
+}
+
+// Try to prevent a smart optimizer from noticing this zero.
+static Real getRealZero() {
+ return std::sin(Real(0));
+}
diff --git a/SimTKcommon/tests/CMakeLists.txt b/SimTKcommon/tests/CMakeLists.txt
new file mode 100644
index 0000000..f8c738e
--- /dev/null
+++ b/SimTKcommon/tests/CMakeLists.txt
@@ -0,0 +1,56 @@
+# Adhoc tests are those test or demo programs which are not intended,
+# or not ready, to be part of the regression suite.
+ADD_SUBDIRECTORY(adhoc)
+
+# Generate regression tests.
+#
+# This is boilerplate code for generating a set of executables, one per
+# .cpp file in a "tests" directory. These are for test cases which
+# have been engineered to be suitable as part of the nightly regression
+# test suite. Ideally, they know their correct answers and return
+# a simple thumbs-up/thumbs-down result, although some regressions
+# may be present and useful just to determine if they compile and
+# run to completion.
+#
+# For IDEs that can deal with PROJECT_LABEL properties (at least
+# Visual Studio) the projects for building each of these adhoc
+# executables will be labeled "Regr - TheTestName" if a file
+# TheTestName.cpp is found in this directory.
+#
+# We check the BUILD_TESTS_AND_EXAMPLES_{SHARED,STATIC} variables to determine
+# whether to build dynamically linked, statically linked, or both
+# versions of the executable.
+
+FILE(GLOB REGR_TESTS "*.cpp")
+FOREACH(TEST_PROG ${REGR_TESTS})
+ GET_FILENAME_COMPONENT(TEST_ROOT ${TEST_PROG} NAME_WE)
+
+ IF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+ # Link with shared library
+ ADD_EXECUTABLE(${TEST_ROOT} ${TEST_PROG})
+ SET_TARGET_PROPERTIES(${TEST_ROOT}
+ PROPERTIES
+ PROJECT_LABEL "Test_Regr - ${TEST_ROOT}")
+ TARGET_LINK_LIBRARIES(${TEST_ROOT}
+ ${TEST_SHARED_TARGET})
+ ADD_TEST(${TEST_ROOT} ${EXECUTABLE_OUTPUT_PATH}/${TEST_ROOT})
+ ENDIF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+
+ IF (BUILD_STATIC_LIBRARIES AND BUILD_TESTS_AND_EXAMPLES_STATIC)
+ # Link with static library
+ SET(TEST_STATIC ${TEST_ROOT}Static)
+ ADD_EXECUTABLE(${TEST_STATIC} ${TEST_PROG})
+ SET_TARGET_PROPERTIES(${TEST_STATIC}
+ PROPERTIES
+ COMPILE_FLAGS "-DSimTK_USE_STATIC_LIBRARIES"
+ PROJECT_LABEL "Test_Regr - ${TEST_STATIC}")
+ TARGET_LINK_LIBRARIES(${TEST_STATIC}
+ ${TEST_STATIC_TARGET})
+ ADD_TEST(${TEST_STATIC} ${EXECUTABLE_OUTPUT_PATH}/${TEST_STATIC})
+ ENDIF()
+
+ENDFOREACH(TEST_PROG ${REGR_TESTS})
+
+
+
+
diff --git a/SimTKcommon/tests/MatVecTest.cpp b/SimTKcommon/tests/MatVecTest.cpp
new file mode 100644
index 0000000..d52ef0d
--- /dev/null
+++ b/SimTKcommon/tests/MatVecTest.cpp
@@ -0,0 +1,553 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/SmallMatrix.h"
+#include "SimTKcommon/Testing.h"
+
+#include <cstdio>
+#include <iostream>
+#include <iomanip>
+using std::cout;
+using std::endl;
+using std::setw;
+
+#include <complex>
+using std::complex;
+using std::sin;
+using std::cos;
+
+using namespace SimTK;
+
+static void dummy();
+
+//typedef Vec<3,Complex> CVec3;
+//static CVec3 f(CVec3 v) {
+ // return CVec3();
+//}
+
+static Complex f(Complex x) {
+ return std::sin(x);
+}
+
+// TODO: move lots of tests here and check the answers
+void testNegator() {
+ Vec3 vvv(1,2,3); Row3 rrr(1,2,3);
+ SimTK_TEST(vvv-vvv == Vec3(0)); // these are exact results
+ SimTK_TEST(-vvv-vvv == (-2*vvv));
+ SimTK_TEST(rrr-rrr == Row3(0));
+ SimTK_TEST(-rrr-rrr == (-2*rrr));
+}
+
+void testElementwiseOps() {
+ Vec3 vvv(1,2,3); Vec3 www(7,9,2);
+ Row3 rrr(1,2,3); Row3 sss(5,4,10);
+
+ Vec3 mv=vvv.elementwiseMultiply(www);
+ Vec3 dv=vvv.elementwiseDivide(www);
+ SimTK_TEST_EQ(mv, Vec3(7,18,6));
+ SimTK_TEST_EQ(dv, Vec3(Real(1)/7, Real(2)/9, Real(3)/2));
+
+ Row3 mr=rrr.elementwiseMultiply(sss);
+ Row3 dr=rrr.elementwiseDivide(sss);
+ SimTK_TEST_EQ(mr, Row3(5,8,30));
+ SimTK_TEST_EQ(dr, Row3(Real(1)/5, Real(2)/4, Real(3)/10));
+
+ Mat22 mmm(1, 2,
+ 3, 4);
+ Mat22 nnn(7, 9,
+ 2, 3);
+ SymMat22 yyy(1,
+ 2, 3);
+ SymMat22 zzz(5,
+ 4, 10);
+
+ Mat22 mm=mmm.elementwiseMultiply(nnn);
+ Mat22 dm=mmm.elementwiseDivide(nnn);
+ SymMat22 my=yyy.elementwiseMultiply(zzz);
+ SymMat22 dy=yyy.elementwiseDivide(zzz);
+
+ SimTK_TEST_EQ(mm, Mat22(7,18,6,12));
+ SimTK_TEST_EQ(dm, Mat22(Real(1)/7,Real(2)/9,Real(3)/2,Real(4)/3));
+ SimTK_TEST_EQ(my, SymMat22(5,8,30));
+ SimTK_TEST_EQ(dy, SymMat22(Real(1)/5,Real(2)/4,Real(3)/10));
+}
+
+void testSums() {
+ Mat22 m(1,2,
+ 3,4);
+ SimTK_TEST_EQ(m.colSum(), Row2(4,6));
+ SimTK_TEST_EQ(m.rowSum(), Vec2(3,7));
+ SimTK_TEST(m.sum() == m.colSum()); // should be exact
+
+ SymMat22 y(3, /*4*/
+ 4, 5);
+ SimTK_TEST_EQ(y.colSum(), Row2(7,9)); // same for real, sym
+ SimTK_TEST_EQ(y.rowSum(), Vec2(7,9));
+ SimTK_TEST(y.sum() == y.colSum()); // should be exact
+
+ Mat22 sm(y); // create fully populated symmetric matrix
+ SimTK_TEST_EQ(sm.rowSum(), y.rowSum());
+ SimTK_TEST_EQ(sm.colSum(), y.colSum());
+
+ Mat<2,2,Complex> mc(1+2*I, 3+4*I,
+ 5+6*I, 7+8*I);
+ typedef Row<2,Complex> CRow2;
+ typedef Vec<2,Complex> CVec2;
+ SimTK_TEST_EQ(mc.colSum(), CRow2(6+8*I, 10+12*I));
+ SimTK_TEST_EQ(mc.rowSum(), CVec2(4+6*I, 12+14*I));
+ SimTK_TEST(mc.sum() == mc.colSum()); // should be exact
+
+ // Row sum and col sum for Hermitian are conjugates; not the same.
+ SymMat<2,Complex> yc( 1, /*3+6*I*/
+ 3-6*I, 4);
+ SimTK_TEST_EQ(yc.colSum(), CRow2(4-6*I, 7+6*I));
+ SimTK_TEST_EQ(yc.rowSum(), CVec2(4+6*I, 7-6*I));
+ SimTK_TEST(yc.sum() == yc.colSum()); // should be exact
+
+ Mat<2,2,Complex> smc(yc); // create fully populated symmetric matrix
+ SimTK_TEST_EQ(smc.rowSum(), yc.rowSum());
+ SimTK_TEST_EQ(smc.colSum(), yc.colSum());
+}
+
+void testMiscellaneous()
+{
+ cout << std::setprecision(16);
+ cout << "f(.3)=" << f(Complex(0.3)) << endl;
+ Real h = 1e-20;
+ cout << "f(.3 + i*h)/h=" << f(Complex(0.3,h)) / h << endl;
+
+ cout << CNT< Mat<2,3, Vec<2> > >::getNaN() << endl;
+
+ Mat<2,3, Vec<2, Mat<2,2,Complex> > > isThisNaN;
+ cout << "isThisNan? " << isThisNaN << endl;
+
+ const Complex mdc[] = {
+ Complex(1.,2.), Complex(3.,4.), Complex(5.,6.), Complex(7.,8.),
+ Complex(9.,10.), Complex(10.,11.), Complex(.1,.26), Complex(.3,.45),
+ Complex(.5,.64), Complex(.7,.83), Complex(.9,.102), Complex(.10,.111)
+ };
+
+ cout << "*** TEST COMPLEX DOT PRODUCT ***" << endl;
+ Vec<3,Complex> vdot(mdc), wdot(&mdc[3]);
+ Row<3,Complex> rdot(mdc), sdot(&mdc[3]);
+ cout << "v=" << vdot << " w=" << wdot << endl;
+ cout << "r=" << rdot << " s=" << sdot << endl;
+ cout << "v.normalize()=" << vdot.normalize() << endl;
+ cout << "r.normalize()=" << rdot.normalize() << endl;
+
+
+ cout << "--- dot() global function:dot(v,w), rw, vs, rs should be the same" << endl;
+ cout << "vw=" << dot(vdot,wdot) << " rw" << dot(rdot,wdot)
+ << " vs" << dot(vdot,sdot) << " rs" << dot(rdot,sdot) << endl;
+ cout << "--- dot operator* requires row*col meaning Hermitian transpose with sign changes" << endl;
+ cout << "vw=" << ~vdot*wdot << " rw" << rdot*wdot
+ << " vs" << ~vdot*~sdot << " rs" << rdot*~sdot << endl;
+
+ cout << endl << "*** TEST COMPLEX OUTER PRODUCT ***" << endl;
+ cout << "--- outer() global function:dot(v,w), rw, vs, rs should be the same" << endl;
+ cout << "vw=" << outer(vdot,wdot) << " rw" << outer(rdot,wdot)
+ << " vs" << outer(vdot,sdot) << " rs" << outer(rdot,sdot) << endl;
+ cout << "--- outer operator* requires col*row meaning Hermitian transpose with sign changes" << endl;
+ cout << "vw=" << vdot*~wdot << " rw" << ~rdot*~wdot
+ << " vs" << vdot*sdot << " rs" << ~rdot*sdot << endl;
+
+ cout << "*** TEST COMPLEX CROSS PRODUCT ***" << endl;
+ cout << "--- cross() global function:dot(v,w), rw, vs, rs should be the same" << endl;
+ cout << "vw=" << cross(vdot,wdot) << " rw" << cross(rdot,wdot)
+ << " vs" << cross(vdot,sdot) << " rs" << cross(rdot,sdot) << endl;
+ cout << "--- cross operator% involves NO sign changes, but returns row if either arg is a row" << endl;
+ cout << "vw=" << vdot%wdot << " rw" << rdot%wdot
+ << " vs" << vdot%sdot << " rs" << rdot%sdot << endl;
+
+ cout << "*** TEST crossMat() ***" << endl;
+ Mat<3,3,Complex> vcross(crossMat(vdot));
+ Mat<3,3,Complex> rcross(crossMat(rdot));
+ cout << "--- crossMat 3d should be same whether made from row or vec" << endl;
+ cout << "vdot%wdot=" << vdot%wdot << endl;
+ cout << "crossMat(v)=" << vcross << "crossMat(r)=" << rcross;
+ cout << "crossMat(v)*w=" << crossMat(vdot)*wdot << " vcross*w=" << vcross*wdot << endl;
+
+
+ Vec<2,Complex> vdot2 = vdot.getSubVec<2>(0);
+ Vec<2,Complex> wdot2 = wdot.getSubVec<2>(0);
+ Row<2,Complex> rdot2 = rdot.getSubRow<2>(0);
+ Row<2,Complex> vcross2(crossMat(vdot2));
+ Row<2,Complex> rcross2(crossMat(rdot2));
+
+ cout << "--- crossMat 2d should be same whether made from row or vec" << endl;
+ cout << "vdot2, wdot2=" << vdot2 << ", " << wdot2 << " vdot2%wdot2=" << vdot2%wdot2 << endl;
+ cout << "crossMat(v2)=" << vcross2 << "crossMat(r2)=" << rcross2;
+ cout << "crossMat(v2)*w2=" << crossMat(vdot2)*wdot2 << " vcross2*w2=" << vcross2*wdot2 << endl;
+
+ cout << "*********\n";
+
+
+
+ Mat<2,5,float> m25f( 1, 2, 3, 4, 5,
+ 6, 7, 8, 9, 10 );
+ cout << "Mat<2,5,float>=" << m25f;
+ cout << "Mat<2,5,float>.normalize()=" << m25f.normalize();
+ cout << "Mat<2,5,float>.sqrt()=" << m25f.sqrt();
+
+ const Mat<1,5,Vec<2,float> >& m15v2f =
+ *reinterpret_cast<const Mat<1,5,Vec<2,float> >*>(&m25f);
+ cout << " m25f@" << &m25f << " m15v2f@" << &m15v2f << endl;
+ cout << "Mat<1,5,Vec<2,float> >=" << m15v2f;;
+ cout << "Mat<1,5,Vec<2,float> >.normalize()=" << m15v2f.normalize();
+
+ const Real twoXthree[] = { 1, 2, 3,
+ 4, 5, 6 };
+ const Real threeXone[] = { .1, .001, .00001 };
+ const Real sym33[] = { 1,
+ 2, 3,
+ 4, -5, 6 };
+ SymMat<3> sm3(sym33);
+ cout << "SymMat<3> sm3=" << sm3;
+ Mat<3,3> m33sm3;
+ for (int i=0; i<3; ++i)
+ for (int j=0; j<=i; ++j)
+ m33sm3(i,j) = m33sm3(j,i) = sm3(i,j);
+ cout << "Mat33(sm3)=" << m33sm3;
+ cout << "sm3*3=" << sm3*3;
+ cout << "sm3+100=" << sm3+100;
+ cout << "m33sm3+100=" << m33sm3+100;
+ cout << "sm3.normalize()=" << sm3.normalize();
+ cout << "m33sm3.normalize()=" << m33sm3.normalize();
+ cout << "sm3+=100:" << (sm3+=100.);
+ cout << "m33sm3+=100:" << (m33sm3+=100.);
+
+ Mat<3,3,Complex> whole(mdc);
+ SymMat<3,Complex,9> sym = SymMat<3,Complex,9>().setFromLower(whole);
+ cout << "whole=" << whole << endl;
+ cout << "sym =" << sym << "(pos~)sym =" << sym.positionalTranspose() << endl;
+
+ cout << "whole.real()=" << whole.real();
+ cout << "whole.imag()=" << whole.imag();
+ cout << "sym.real()=" << sym.real();
+ cout << "sym.imag()=" << sym.imag();
+
+
+
+ Mat<3,4,Complex> mdcp(mdc); cout << "*** Data looks like this: " << mdcp;
+ SymMat<4,negator<Complex> > symp(reinterpret_cast<const negator<conjugate<double> >*>(mdc));
+ cout << " 4x4 Sym<Neg<cmplx>> from (negator<conj>)pointer to data gives this:" << symp;
+ cout << " sym.real()=" << symp.real();
+ cout << " sym.imag()=" << symp.imag();
+ cout << " ~sym.imag()=" << ~symp.imag();
+ cout << "pos~(sym.imag())=" << symp.imag().positionalTranspose();
+ cout << "(pos~sym).imag()=" << symp.positionalTranspose().imag();
+ cout << " -sym.imag()=" << -symp.imag();
+
+ symp(2,1).real() = 99.;
+ cout << "after sym(2,1).real=99, sym=" << symp;
+
+ symp.updPositionalTranspose().imag()(3,1)=123.;
+ cout << "after (pos~sym).imag()(3,1)=123, (pos~sym).imag()=" << symp.positionalTranspose().imag();
+ cout << " ... sym=" << symp;
+
+ Mat<2,3, SymMat<3,Complex> > weird(Row<3,SymMat<3,Complex> >( sym, -sym, sym ),
+ Row<3,SymMat<3,Complex> >( sym, sym, sym ));
+ cout << "weird=" << weird;
+ weird *= 2.;
+ cout << " weird*=2: " << weird;
+ cout << " weird(1)=" << weird(1) << endl;
+ cout << " weird(0,1)=" << weird(0,1) << " [0][1]=" << weird[0][1] << endl;
+
+ cout << " typename(weird)=" << typeid(weird).name() << endl;
+ cout << " typename(weird.real)=" << typeid(weird.real()).name() << endl;
+ cout << " typename(weird.imag)=" << typeid(weird.imag()).name() << endl;
+
+ cout << " weird.real()=" << weird.real();
+ cout << " weird.imag()=" << weird.imag();
+
+ cout << "sizeof(sym<3,cplx>)=" << sizeof(sym) << " sizeof(mat<2,3,sym>=" << sizeof(weird) << endl;
+
+ Mat<2,3> m23(twoXthree);
+ Mat<3,1> m31(threeXone);
+ cout << "m23=" << m23 << endl;
+ cout << "m31=" << m31 << endl;
+ cout << "m23*-m31=" << m23*-m31 << endl;
+ cout << "~ ~m31 * ~-m23=" << ~((~m31)*(~-m23)) << endl;
+
+ Mat<2,3,Complex> c23(m23);
+ Mat<3,1,Complex> c31(m31);
+ cout << "c23=" << c23 << endl;
+ cout << "c31=" << c31 << endl;
+ cout << "c23*c31=" << c23*-c31 << endl;
+ cout << " ~c31 * ~-c23=" << (~c31)*(~-c23) << endl;
+ cout << "~ ~-c31 * ~c23=" << ~((~-c31)*(~c23)) << endl;
+
+
+ Mat<3,4> m34;
+ Mat<3,4,Complex> cm34;
+
+
+ cm34 = mdc;
+ m34 = cm34.real();
+
+ cout << "Mat<3,4,Complex> cm34=" << cm34 << endl;
+ cout << "cm34.diag()=" << cm34.diag() << endl;
+
+ cout << "cm34 + cm34=" << cm34+cm34 << endl; //INTERNAL COMPILER ERROR IN Release MODE
+ cout << "~cm34 * 1000=" << ~cm34 * 1000. << endl;
+
+ cout << "m34=" << m34 << endl;
+ m34 =19.123;
+ cout << "after m34=19.123, m34=" << m34 << endl;
+
+ const double ddd[] = { 11, 12, 13, 14, 15, 16 };
+ const complex<float> ccc[] = { complex<float>(1.,2.),
+ complex<float>(3.,4.),
+ complex<float>(5.,6.),
+ complex<float>(7.,8.) };
+ Vec<2,complex<float>,1> cv2(ccc);
+ cout << "cv2 from array=" << cv2 << endl;
+ cv2 = Vec<2,complex<float> >(complex<float>(1.,2.), complex<float>(3.,4.));
+ cout << "cv2 after assignment=" << cv2 << endl;
+
+ cout << "cv2.real()=" << cv2.real() << " cv2.imag()=" << cv2.imag() << endl;
+
+ Vec<2,negator<complex<float> >,1>& negCv2 = (Vec<2,negator<complex<float> >,1>&)cv2;
+ Vec<2,conjugate<float>,1>& conjCv2 = (Vec<2,conjugate<float>,1>&)cv2;
+ Vec<2,negator<conjugate<float> >,1>& negConjCv2 = (Vec<2,negator<conjugate<float> >,1>&)cv2;
+
+
+
+ Vec<2,complex<float> > testMe = cv2;
+ cout << "testMe=cv2 (init)=" << testMe << endl;
+ testMe = cv2;
+ cout << "testMe=cv2 (assign)=" << testMe << endl;
+
+
+ cout << "(cv2+cv2)/complex<float>(1000,0):" << (cv2 + cv2) / complex<float>(1000,0) << endl;
+ cout << "(cv2+cv2)/1000.f:" << (cv2 + cv2) / 1000.f << endl;
+ cout << "(cv2+cv2)/1000.:" << (cv2 + cv2) / 1000. << endl;
+ cout << "(cv2+cv2)/1000.L:" << (cv2 + cv2) / 1000.L << endl;
+ cout << "(cv2+cv2)/1000:" << (cv2 + cv2) / 1000 << endl;
+
+ cout << "negCv2=" << negCv2 << endl;
+ cout << "conjCv2=" << conjCv2 << endl;
+ cout << "negConjCv2=" << negConjCv2 << endl;
+ cout << "cv2+negCv2=" << cv2+negCv2 << endl;
+
+ negConjCv2 = complex<float>(8,9);
+ cout << "AFTER negConjCv2 = (8,9):" << endl;
+ cout << " cv2=" << cv2 << endl;
+ cout << " negCv2=" << negCv2 << endl;
+ cout << " conjCv2=" << conjCv2 << endl;
+ cout << " negConjCv2=" << negConjCv2 << endl;
+
+ cout << "cv2: " << cv2 << endl;
+ cout << "cv2T: " << cv2.transpose() << endl;
+ cout << "-cv2: " << -cv2 << endl;
+ cout << "~cv2: " << ~cv2 << endl;
+ cout << "-~cv2: " << -(~cv2) << endl;
+ cout << "~-cv2: " << ~(-cv2) << endl;
+ cout << "~-cv2*10000: " << (~(-cv2))*10000.f << endl;
+
+ (~cv2)[1]=complex<float>(101.1f,202.3f);
+ cout << "after ~cv2[1]=(101.1f,202.3f), cv2= " << cv2 << endl;
+ (-(~cv2))[1]=complex<float>(11.1f,22.3f);
+ cout << "after -~cv2[1]=(11.1f,22.3f), cv2= " << cv2 << endl;
+
+ Vec<3> dv3(ddd), ddv3(ddd+3);
+ dv3[2] = 1000;
+ cout << "dv3=" << dv3 << " ddv3=" << ddv3 << endl;
+ cout << "100(ddv3-dv3)/1000=" << 100.* (ddv3 - dv3) / 1000. << endl;
+
+ Vec<3> xxx(dv3); cout << "copy of dv3 xxx=" << xxx << endl;
+ Vec<3> yyy(*ddd);cout << "copy of *ddd yyy=" << yyy << endl;
+
+ cout << "dv3.norm()=" << dv3.norm() << endl;
+ cout << "cv2=" << cv2 << " cv2.norm()=" << cv2.norm() << endl;
+
+ const Vec<2> v2c[] = {Vec<2>(ddd),Vec<2>(ddd+1)};
+ Vec<2, Vec<2> > vflt(v2c);
+ cout << "vflt 2xvec2=" << vflt << endl;
+ cout << "10.*vflt=" << 10.*vflt << endl;
+ cout << "vflt*10.=" << vflt*10. << endl;
+
+ int ivals[] = {0x10, 0x20, 0x30, 0x40};
+ Vec<4> iv(ivals);
+ cout << "iv=" << iv << endl;
+
+ Vec<2, Vec<2> > v22;
+ v22 = Vec<2>(&ivals[2]);
+ cout << "v22=" << v22 << endl;
+
+
+ // Test dot product
+ {
+ double d[] = {1,2,3,4,5,6,7,8};
+
+
+ Vec<2> v1(&d[0]), v2(&d[2]);
+ Row<2> r1(&d[4]), r2(&d[6]);
+ Vec<2>::TNeg& nv1 = (Vec<2>::TNeg&)v1;
+
+ negator<double> nd(100); cout << endl << "nd=" << nd << endl;
+ cout << "nv1=" << nv1 << endl;
+ cout << "nv1*nd=" << nv1*nd << endl;
+ cout << "nd*nv1=" << nd*nv1 << endl;
+ cout << "nv1/nd=" << nv1/nd << endl << endl;
+
+
+ cout << "v1,v2=" << v1 << v2 << endl;
+ cout << "r1,r2=" << r1 << r2 << endl;
+ cout << "dot r1*v1 =" << dot(r1,v1) << endl;
+ cout << "dot r1*nv1=" << dot(r1,nv1) << endl;
+ cout << "r1*v1 =" << r1*v1 << endl;
+ cout << "r1*nv1=" << r1*nv1 << endl;
+
+ // outer product
+ cout << " outer v1*r1=" << v1*r1 << endl;
+ cout << " outer nv1*r1=" << nv1*r1 << endl;
+
+ // cross product (2d)
+ cout << "cross(v1,v2)=" << cross(v1,v2) << endl;
+ cout << "v1 % v2=" << v1 % v2 << endl;
+ cout << "cross(r1,v2)=" << cross(r1,v2) << endl;
+ cout << "r1 % v2=" << r1 % v2 << endl;
+ cout << "cross(v1,r2)=" << cross(v1,r2) << endl;
+ cout << "v1 % r2=" << v1 % r2 << endl;
+ cout << "cross(r1,r2)=" << cross(r1,r2) << endl;
+ cout << "r1 % r2=" << r1 % r2 << endl;
+
+ // do the cross products with 3d routines
+ Vec3 v13(v1[0],v1[1],0), v23(v2[0],v2[1],0);
+ Row3 r13(r1[0],r1[1],0), r23(r2[0],r2[1],0);
+ cout << "cross(v13,v23)=" << cross(v13,v23) << endl;
+ cout << "v13 % v23=" << v13 % v23 << endl;
+ cout << "cross(r13,v23)=" << cross(r13,v23) << endl;
+ cout << "r13 % v23=" << r13 % v23 << endl;
+ cout << "cross(v13,r23)=" << cross(v13,r23) << endl;
+ cout << "v13 % r23=" << v13 % r23 << endl;
+ cout << "cross(r13,r23)=" << cross(r13,r23) << endl;
+ cout << "r13 % r23=" << r13 % r23 << endl;
+
+ v13[2]=7;
+ cout << "v13=" << v13 << " 2*v13+.001=" << 2*v13+.001 << endl;
+ cout << "v13 % (2*v13)=" << v13 % (2*v13) << endl;
+ cout << "v13 % (2*v13+.001)=" << v13 % (2*v13+.001) << endl;
+
+ cout << endl;
+
+ // test constructors
+ Mat<2,3> mvcols( v1, ~r1, v2 );
+ Mat<3,2> mvrows( ~v1,
+ r1,
+ r2 );
+
+ cout << "mvcols=" << mvcols << endl;
+ cout << "mvrows=" << mvrows << endl;
+
+ Vec<3,float> v2f(39.f, 40.f, 50.L);
+ cout << "v2f=" << v2f << endl;
+
+ cout << "v2f.drop1(0)=" << v2f.drop1(0) << endl;
+ cout << "v2f.drop1(1)=" << v2f.drop1(1) << endl;
+ cout << "v2f.drop1(2)=" << v2f.drop1(2) << endl;
+
+ cout << "v2f.append1(3.3f)=" << v2f.append1(3.3f) << endl;
+ cout << "v2f.append1((short)1)=" << v2f.append1((short)1) << endl;
+ cout << "v2f.drop1(1).append1((unsigned short)0x10)=" << v2f.drop1(1).append1((unsigned short)0x10) << endl;
+ cout << "(~v2f).drop1(1).append1((unsigned short)0x10)=" << (~v2f).drop1(1).append1((unsigned short)0x10) << endl;
+
+ cout << "v2f.insert1(0, 23.f)=" << v2f.insert1(0, 23.f) << endl;
+ cout << "v2f.insert1(1, 23.f)=" << v2f.insert1(1, 23.f) << endl;
+ cout << "v2f.insert1(2, 23.f)=" << v2f.insert1(2, 23.f) << endl;
+ cout << "v2f.insert1(3, 23.f)=" << v2f.insert1(3, 23.f) << endl;
+ cout << "v2f.insert1(2, 23.f).drop1(2)=" << v2f.insert1(2, 23.f).drop1(2) << endl;
+
+ cout << "(~v2f).insert1(2, 23.f).drop1(2)=" << (~v2f).insert1(2, 23.f).drop1(2) << endl;
+
+ Mat33 m33( Row3(1, 2, 3),
+ Row3(4, 5, 6),
+ Row3(.003f, 9.62L, 41.1) );
+ cout << "m33=" << m33 << endl;
+ cout << "v13=" << v13 << endl;
+ cout << "m33*v13=" << m33*v13 << endl;
+ cout << "~m33*v13=" << (~m33)*v13 << endl;
+ cout << "~v13*~m33=" << ~v13*(~m33) << endl;
+ }
+
+ Mat<4,3> ident43;
+ ident43 = 1;
+ cout << "ident43=" << ident43 << endl;
+
+ Mat<4,3> negid43 = -ident43; // requires implicit conversion from Mat<negator<Real>>
+ cout << "negid43=" << negid43 << endl;
+
+ // Absolute value
+ const Vec<3> vorig(1.,-2.,-3.);
+ cout << "vorig=" << vorig << " vorig.abs()=" << vorig.abs() << endl;
+ const Vec<2, Vec<3> > v2orig(vorig,-vorig);
+ cout << "v2orig=" << v2orig << " v2orig.abs()=" << v2orig.abs() << endl;
+
+ Vec<3> nvorig = -vorig;
+ Vec<2, Vec<3> > nv2orig = -v2orig;
+
+ Mat<3,2> morig(vorig,-vorig);
+ cout << "morig=" << morig << " morig.abs()=" << morig.abs() << endl;
+
+ //Mat<2,2, Vec<3> > m2orig(v2orig, (Vec<2, Vec<3> >)-v2orig);
+ //cout << "m2orig=" << m2orig << " m2orig.abs()=" << m2orig.abs() << endl;
+
+ cout << "vorig.getSubVec<2>(1)=" << vorig.getSubVec<2>(1) << endl;
+
+ negid43.updSubMat<2,2>(2,1) = -27.;
+ cout << "after negid43.updSubMat<2,2>(2,1) = -27., negid43=" << negid43;
+
+ cout << "negid43[2].getSubRow<2>(1)=" << negid43[2].getSubRow<2>(1) << endl;
+
+ cout << "CHECK DIAGONAL LENGTH FOR RECTANGULAR MATRICES" << endl;
+ Mat<3,2, Row3, 1, 2> H;
+ cout << "H[" << H.nrow() << "," << H.ncol() << "]" << endl;
+ cout << "H.diag()[" << H.diag().nrow() << "," << H.diag().ncol() << "]" << endl;
+ Mat<3,2, Row3, 1, 2>::TransposeType Ht;
+ cout << "Ht[" << Ht.nrow() << "," << Ht.ncol() << "]" << endl;
+ cout << "Ht.diag()[" << Ht.diag().nrow() << "," << Ht.diag().ncol() << "]" << endl;
+}
+
+void testMatInverse() {
+ Matrix m = Test::randMatrix(20,20);
+ Matrix mi = m.invert();
+ Matrix id(20,20); id=1; // identity
+ SimTK_TEST_EQ_SIZE(m*mi, id, 20);
+}
+
+
+int main() {
+ SimTK_START_TEST("MatVecTest");
+
+ SimTK_SUBTEST(testSums);
+ SimTK_SUBTEST(testNegator);
+ SimTK_SUBTEST(testElementwiseOps);
+ SimTK_SUBTEST(testMiscellaneous);
+ SimTK_SUBTEST(testMatInverse);
+
+ SimTK_END_TEST();
+}
+
+
+
diff --git a/SimTKcommon/tests/OrientationTest.cpp b/SimTKcommon/tests/OrientationTest.cpp
new file mode 100644
index 0000000..19059c9
--- /dev/null
+++ b/SimTKcommon/tests/OrientationTest.cpp
@@ -0,0 +1,423 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Adhoc tests for rotation-related classes.
+ */
+
+#include "SimTKcommon.h"
+
+#include <string>
+#include <typeinfo>
+#include <iostream>
+#include <iomanip>
+#include <cmath>
+#include <limits>
+using std::cout;
+using std::endl;
+
+using namespace SimTK;
+
+static Rotation rotate1(int i, Real a);
+
+float feps = std::numeric_limits<float>::epsilon();
+double deps = std::numeric_limits<double>::epsilon();
+
+// What is the smallest a for which acos(cos(a))=a to machine precision?
+double ferr(float angle) {return double((double)std::acos(std::cos(angle))-(double)angle)/(std::fabs((double)angle)+deps);}
+double derr(double angle) {return (std::acos(std::cos(angle))-angle)/(std::fabs(angle)+deps);}
+double err(double a, double aest) {return std::fabs(aest-a)/std::max(std::fabs(a), deps);}
+void cosTest() {
+
+ float sfeps = std::sqrt(feps);
+ double sdeps = std::sqrt(deps);
+
+ float csf = std::cos(sfeps);
+ double csd = std::cos(sdeps);
+
+ cout << "feps=" << feps << " sfeps=" << sfeps << " cos(sf)-1" << csf-1 << endl;
+ cout << "deps=" << deps << " sdeps=" << sdeps << " cos(sd)-1" << csd-1 << endl;
+
+ double pi2 = std::acos(0.);
+ double maxs=0, maxc=0, maxt=0;
+ for (int i=-10000001; i<=10000001; ++i) {
+ double a = pi2 - 1.03e-9*double(i)*pi2 + 0.237e-9;
+ double s = std::sin(a), c = std::cos(a);
+ maxs = std::max(maxs, err(a, std::asin(s)));
+ maxc = std::max(maxc, err(a, std::acos(c)));
+ maxt = std::max(maxt, err(a, std::atan2(s,c)));
+ }
+ printf("near pi/2: esin=%g ecos=%g etan2=%g\n", maxs, maxc, maxt);
+
+ double maxs2=0, maxc2=0, maxt2=0;
+ for (int i=-10000001; i<=10000001; ++i) {
+ double a = 1.03e-9*double(i)*pi2 + 0.237e-9;
+ double s = std::sin(a), c = std::cos(a);
+ maxs2 = std::max(maxs2, err(a, std::asin(s)));
+ maxc2 = std::max(maxc2, err(a, std::acos(c)));
+ maxt2 = std::max(maxt2, err(a, std::atan2(s,c)));
+ }
+ printf("near zero: esin=%g ecos=%g etan2=%g\n", maxs2, maxc2, maxt2);
+
+}
+
+void quatTest() {
+ const Real pi2 = std::acos(Real(0));
+ Vec4 avOrig(-.1-1e-4, 7e1,-.2,.1);
+ Quaternion q1,q2;
+ q1.setQuaternionFromAngleAxis(avOrig);
+ cout << "q1=" << q1 << endl;
+ Vec4 av1 = q1.convertQuaternionToAngleAxis();
+ cout << std::setprecision(18);
+ cout << "avOrig=" << avOrig << " av1=" << av1 << endl;
+ q2.setQuaternionFromAngleAxis(av1);
+ cout << "q2-q1=" << q2-q1 << endl;
+ Vec4 av2 = q2.convertQuaternionToAngleAxis();
+ cout << "av2-av1=" << av2-av1 << endl;
+
+ Rotation r1(q1);
+ Vec4 av3 = r1.convertRotationToAngleAxis();
+ cout << "av3=" << av3 << " av3-av1=" << av3-av1 << endl;
+ Quaternion q3;q3.setQuaternionFromAngleAxis(av3);
+ Rotation r2(q3);
+ cout << "norm(r2*~r1)-sqrt(3)=" << (r2*~r1).norm()-std::sqrt(3.) << endl;
+
+}
+
+void orthoTest(String msg, const Rotation& R) {
+ cout << msg << endl;
+ cout << "cols=" << R(0).norm()-1 << ", " << R(1).norm()-1 << ", " << R(2).norm()-1 << endl;
+ cout << "rows=" << R[0].norm()-1 << ", " << R[1].norm()-1 << ", " << R[2].norm()-1 << endl;
+ cout << "perp=" << dot(R(0),R(1)) << ", " << dot(R(1),R(2)) << ", " << dot(R(0),R(2)) << endl;
+}
+
+void f(const CoordinateAxis& ax) { printf("which = %d\n", (int)ax); }
+void f(CoordinateAxis::XCoordinateAxis) { printf("X\n"); }
+void f(CoordinateAxis::YCoordinateAxis) { printf("Y\n"); }
+void f(CoordinateAxis::ZCoordinateAxis) { printf("Z\n"); }
+
+int a[]={9,10,11};
+
+int main() {
+ quatTest();
+
+ cout << "-----------------\n";
+ cout << "X,Y,Z=" << XAxis << "," <<YAxis << "," <<ZAxis << endl;
+ printf("%d %d %d\n", (int)XAxis, (int)YAxis, (int)ZAxis);
+
+ f(XAxis); f(YAxis); f(ZAxis);
+ printf("index: %d %d %d\n", a[XAxis], a[YAxis], a[ZAxis]);
+
+try {
+ UnitVec3 u;
+ cout << "should be NaN: u()=" << u << endl;
+ cout << "typename(u)=" << typeid(u).name() << " ~u=" << typeid(~u).name() << endl;
+
+ u = UnitVec3(1,2,3);
+ cout << "u=UnitVec(1,2,3): " << u << " transposed: " << ~u << endl;
+ cout << "u[2]=" << u[2] << " u(2)=" << u(2) << endl;
+ cout << "(~u)[2]=" << (~u)[2] << " (~u)(2)=" << (~u)(2) << endl;
+
+ cout << "u.perp()=" << u.perp() << " ~u*u.perp()=" << ~u*u.perp() << endl;
+ cout << "(~u).perp()=" << (~u).perp() << " (~u).perp()*u=" << (~u).perp()*u << endl;
+
+ Vec3 vv(u);
+ Vec3 w = u;
+ cout << "Vec3 vv(u)=" << vv << " Vec3 w=u, w=" << w << endl;
+
+ // these lines shouldn't compile
+ //u = Vec3(1,2,3);
+ //u += UnitVec3(1,2,3);
+ //u[2] = 5.;
+ //u(2) = 5.;
+ //u=0.;
+ // end of lines which shouldn't compile
+
+
+ Rotation R;
+ cout << "should be identity: R()=" << R;
+ cout << "typename(R)=" << typeid(R).name() << " ~R=" << typeid(~R).name() << endl;
+
+ Transform X;
+ cout << "should be identity: X()=" << X;
+ cout << "typename(X)=" << typeid(X).name() << " ~X=" << typeid(~X).name() << endl;
+
+ R = Rotation( u, ZAxis );
+
+ cout << "Rotation with u as z axis (norm=" << R.norm() << "): " << R;
+ cout << "~R: " << ~R;
+ cout << "typename(R[1])=" << typeid(R[1]).name() << " R(1)=" << typeid(R(1)).name() << endl;
+
+ X = Transform(R, Vec3(-1, 2, 20));
+ cout << "Transform X=" << X;
+ cout << " X.R=" << X.R() << " X.p=" << X.p() << " X.pInv=" << X.pInv() << endl;
+ cout << " X.x=" << X.x() << " y=" << X.y() << " z=" << X.z() << endl;
+ cout << "Transform ~X=" << ~X;
+ cout << " (~X).R=" << (~X).R() << " (~X).p=" << (~X).p() << " (~X).pInv=" << (~X).pInv() << endl;
+ cout << " (~X).x=" << (~X).x() << " y=" << (~X).y() << " z=" << (~X).z() << endl;
+ cout << "Transform X asMat34():" << X.asMat34();
+ cout << "Transform X toMat34():" << X.toMat34();
+ cout << "Transform X toMat44():" << X.toMat44();
+ cout << "Transform ~X toMat34():" << (~X).toMat34();
+ cout << "Transform ~X toMat44():" << (~X).toMat44();
+ cout << "should be identity: X*~X=" << X*~X;
+ cout << "should be identity: ~X*X=" << ~X*X;
+
+ Vec3 v(1,2,3);
+ cout << "v=" << v << endl;
+ cout << "X.R()*v=" << X.R()*v << endl;
+ cout << "X*v=" << X*v << endl;
+ cout << "X*[v,0]=" << X*v.append1(0) << endl;
+ cout << "X*[v,1]=" << X*v.append1(1) << endl;
+ cout << "~X*(X*v)=" << ~X*(X*v) << endl;
+ cout << "~X*(X*[v,0])=" << ~X*(X*v.append1(0)) << endl;
+ cout << "~X*(X*[v,1])=" << ~X*(X*v.append1(1)) << endl;
+
+ Rotation rx( 0.1, XAxis );
+ Rotation ry( 0.17, YAxis );
+ Rotation rz( 0.31, ZAxis );
+ Rotation rxoldy( SpaceRotationSequence, 0.1, XAxis, 0.17, YAxis );
+ Rotation rxyz = rz*ry*rx; // space fixed
+ // reverse order to use body fixed like space fixed
+ Rotation r123( BodyRotationSequence, 0.31, ZAxis, 0.17, YAxis, 0.1, XAxis );
+
+ cout << "Rotation r123:" << r123;
+ cout << "Rotation r123.asMat33():" << r123.asMat33();
+ cout << "Rotation r123.toMat33():" << r123.toMat33();
+
+ cout << "InvRotation ~r123:" << (~r123);
+ cout << "InvRotation ~r123.asMat33():" << (~r123).asMat33();
+ cout << "InvRotation ~r123.toMat33():" << (~r123).toMat33();
+
+ cout << "ry(.17)*rx(.1)=" << ry*rx;
+ cout << "rxoldy(.1,.17)=" << rxoldy;
+ cout << "norm(rxoldy-ry*rx)=" << (rxoldy-ry*rx).norm() << endl;
+ cout << "rxyz=" << rxyz;
+ cout << "r123=" << r123;
+
+ // Test inverse assignments, copy construct
+ Rotation invr123(~r123);
+ Rotation invr123eq;
+ invr123eq = ~r123;
+
+ cout << "Check inverse assignment/copy." << endl;
+ cout << "norm invr123*r123-identity=" << (invr123*r123-Mat33(1)).norm() << endl;
+ cout << "norm invr123eq*r123-identity=" << (invr123eq*r123-Mat33(1)).norm() << endl;
+
+ Rotation bodyXY( BodyRotationSequence, 0.03, XAxis, 0.11, YAxis, 0.0, ZAxis );
+ Rotation aboutYthenX( SpaceRotationSequence, 0.11, YAxis, 0.03, XAxis );
+
+ cout << "bodyXY(.03,.11)=" << bodyXY;
+ cout << "aboutYthenoldX(.11,.03)=" << aboutYthenX;
+
+ Rotation bodyZX( BodyRotationSequence, 0.07, ZAxis, 0.0, YAxis, -0.22, XAxis );
+ Rotation aboutZThenNewX( BodyRotationSequence, 0.07, ZAxis, -0.22, XAxis );
+ Rotation aboutZ( 0.07, ZAxis );
+ Rotation aboutXaboutZ = Rotation(-0.22, aboutZ*Vec3(1,0,0)) * aboutZ;
+ cout << "bodyZX(.07,-.22)=" << bodyZX;
+ cout << "aboutZThenNewX(.07,-.22)=" << aboutZThenNewX;
+ cout << "aboutXaboutZ(.07,-.22)=" << aboutXaboutZ;
+ cout << " diff norm=" << (aboutXaboutZ-aboutZThenNewX).norm() << endl;
+
+ // Check all space-fixed sequences vs composed single rotations.
+ cout << "SXY err=" << ( Rotation( SpaceRotationSequence, 0.13, XAxis, -0.29, YAxis ) - Rotation( -0.29, YAxis )*Rotation( 0.13, XAxis) ).norm() << endl;
+ cout << "SXZ err=" << ( Rotation( SpaceRotationSequence, 0.13, XAxis, -0.29, ZAxis ) - Rotation( -0.29, ZAxis )*Rotation( 0.13, XAxis) ).norm() << endl;
+ cout << "SZX err=" << ( Rotation( SpaceRotationSequence, 0.13, ZAxis, -0.29, XAxis ) - Rotation( -0.29, XAxis )*Rotation( 0.13, ZAxis) ).norm() << endl;
+ cout << "SYZ err=" << ( Rotation( SpaceRotationSequence, 0.13, YAxis, -0.29, ZAxis ) - Rotation( -0.29, ZAxis )*Rotation( 0.13, YAxis) ).norm() << endl;
+ cout << "SZY err=" << ( Rotation( SpaceRotationSequence, 0.13, ZAxis, -0.29, YAxis ) - Rotation( -0.29, YAxis )*Rotation( 0.13, ZAxis) ).norm() << endl;
+
+ // Check all body-fixed sequences vs. transformed & composed single rotations.
+ const Real a1=-.23, a2=1.09;
+ const Rotation ax( a1, XAxis ); const Vec3 x(1,0,0);
+ const Rotation ay( a1, YAxis ); const Vec3 y(0,1,0);
+ const Rotation az( a1, ZAxis ); const Vec3 z(0,0,1);
+
+ cout << "BXY err=" << ( Rotation( BodyRotationSequence, a1, XAxis, a2, YAxis ) - Rotation(a2,ax*y)*ax ).norm() << endl;
+ cout << "BYX err=" << ( Rotation( BodyRotationSequence, a1, YAxis, a2, XAxis ) - Rotation(a2,ay*x)*ay ).norm() << endl;
+ cout << "BXZ err=" << ( Rotation( BodyRotationSequence, a1, XAxis, a2, ZAxis ) - Rotation(a2,ax*z)*ax ).norm() << endl;
+ cout << "BZX err=" << ( Rotation( BodyRotationSequence, a1, ZAxis, a2, XAxis ) - Rotation(a2,az*x)*az ).norm() << endl;
+ cout << "BYZ err=" << ( Rotation( BodyRotationSequence, a1, YAxis, a2, ZAxis ) - Rotation(a2,ay*z)*ay ).norm() << endl;
+ cout << "BZY err=" << ( Rotation( BodyRotationSequence, a1, ZAxis, a2, YAxis ) - Rotation(a2,az*y)*az ).norm() << endl;
+
+ cout << "rotate1(0,.03)=" << ( rotate1(0,.03) - Rotation(.03,XAxis) ).norm() << endl;
+ cout << "rotate1(1,.03)=" << ( rotate1(1,.03) - Rotation(.03,YAxis) ).norm() << endl;
+ cout << "rotate1(2,.03)=" << ( rotate1(2,.03) - Rotation(.03,ZAxis) ).norm() << endl;
+
+ cout << "-1 % 3=" << -1 % 3 << endl;
+
+ const Transform X_01( Rotation( .03, Vec3(1,.1,.7) ), Vec3(1,2,3) );
+ const Real masses[] = {1,2,3,4,5};
+ const Real mtot = Vector(5,masses).sum();
+ const Vec3 stations000[] = {Vec3(.1,.2,.3), Vec3(-2,-9,1), Vec3(.01,.02,.05),
+ Vec3(1,-1,1), Vec3(0,0,0)};
+ const Vector_<Vec3> s000(5, stations000);
+ const Vec3 com000 = (~Vector(5,masses) * s000)/mtot;
+ Vector_<Vec3> s123(s000);
+ for (int i=0;i<5;++i) s123[i] = ~X_01*s123[i];
+ const Vec3 com123 = (~Vector(5,masses) * s123)/mtot;
+ Inertia I000(0), I123(0);
+ for (int i=0; i<5; ++i) I000 += Inertia(s000[i],masses[i]);
+ for (int i=0; i<5; ++i) I123 += Inertia(s123[i],masses[i]);
+ cout << "mtot=" << mtot << endl;
+ cout << "com000=" << com000 << endl;
+ cout << "com123=" << com123 << endl;
+ cout << "I000=" << I000;
+ cout << "I123=" << I123;
+
+ MassProperties mp(mtot, com000, I000);
+ cout << mp;
+ Real scale = std::sqrt(I123.toMat33().diag().normSqr()/3); cout << "inertia scale=" << scale << endl;
+ cout << "norm(I000->123-I123)/rms(I123)="
+ << (mp.calcTransformedInertia(X_01).toMat33()-I123.toMat33()).norm()/scale
+ << endl;
+
+
+ Rotation R_AB( BodyRotationSequence, 0.31, ZAxis, 0.17, YAxis, 0.1, XAxis );
+ Rotation R_BC( BodyRotationSequence, -123.3, ZAxis, 41.1, YAxis, 14, XAxis );
+
+ Rotation Rtmp;
+ cout << "R_AB*R_BC=" << R_AB*R_BC;
+ Rtmp = R_AB;
+ cout << "R_AB*=R_BC=" << (Rtmp*=R_BC);
+ cout << "R_AB*~R_BC=" << R_AB*~R_BC;
+ cout << "R_AB/R_BC=" << R_AB/R_BC;
+ Rtmp=R_AB;
+ cout << "R_AB/=R_BC=" << (Rtmp/=R_BC);
+
+ cout << "COMPARE ROTATIONS" << endl;
+ Rotation R_GB, R_GX; Real backOut;
+ R_GB.setRotationFromAngleAboutNonUnitVector( 0.17, Vec3(1,2,3) );
+ backOut = R_GB.convertRotationToAngleAxis()[0];
+ cout << " in=0.17 radians, out=" << backOut << " err=" << std::abs(backOut-0.17) << endl;
+
+ R_GB.setRotationFromAngleAboutNonUnitVector( 0.17+1e-13, Vec3(1,2,3) );
+ R_GX.setRotationFromAngleAboutNonUnitVector( 0.17, Vec3(1,2,3) );
+ cout << " 0.17+1e-13:0.17 isSameToPrecision? " << R_GB.isSameRotationToWithinAngleOfMachinePrecision(R_GX)
+ << " isSameToAngle(1e-12)? " << R_GB.isSameRotationToWithinAngle(R_GX, 1e-12) << endl;
+ R_GB.setRotationFromAngleAboutNonUnitVector( 0.17+1e-15, Vec3(1,2,3) );
+ R_GX.setRotationFromAngleAboutNonUnitVector( 0.17, Vec3(1,2,3) );
+ cout << " 0.17+1e-15:0.17 isSameToPrecision? " << R_GB.isSameRotationToWithinAngleOfMachinePrecision(R_GX)
+ << " isSameToAngle(1e-18)? " << R_GB.isSameRotationToWithinAngle(R_GX, 1e-18) << endl;
+
+ const Real pi2 = Pi/2;
+ const Real pi2x = -pi2 + 1e-8;
+ cout << "pi2x=pi2-" << pi2-pi2x << " sin(pi2x)-1=" << std::sin(pi2x)-1 << endl;
+ const Vec3 vin(-3, pi2x, 0.1);
+ Rotation b123( BodyRotationSequence, vin[0], XAxis, vin[1], YAxis, vin[2], ZAxis );
+ Mat33 m123=b123; m123[0][0] += 1e-14; m123[1][2] += 1e-14;
+ //b123 = Rotation::trustMe(m123);
+ //cout << "bad b123*~b123 angle=" << (b123*~b123).convertToAngleAxis()[0] << endl;
+ //b123 = Rotation(m123);
+ //cout << "good b123*~b123.norm()=" << (b123*~b123).convertToAngleAxis()[0] << endl;
+ b123 *= R_GX;
+ b123 *= ~R_GX;
+ cout << "b123=" << b123;
+ Rotation cleanb123(b123.asMat33());
+ // b123=cleanb123;
+
+ cout << "cos(pi2x)=" << cos(pi2x) << "cos(pi2)=" << cos(pi2) << endl;
+ cout << "atan2(02/00)-pi2=" << atan2(b123[0][2],b123[0][0])-pi2 << endl;
+ Vec3 v123 = b123.convertThreeAxesRotationToThreeAngles( BodyRotationSequence, XAxis, YAxis, ZAxis );
+ Vec4 aax = b123.convertRotationToAngleAxis();
+ cout << "vin=" << vin << "\nvout=" << v123 << endl;
+ Rotation b123x( BodyRotationSequence, v123[0], XAxis, v123[1], YAxis, v123[2], ZAxis );
+ Vec4 aax2 = (~b123*b123x).convertRotationToAngleAxis();
+ cout << " aax2=" << aax2 << endl;
+
+ Rotation chrisRot( UnitVec3( Vec3(1,1,0) ), XAxis, Vec3(0,1,0), YAxis );
+ cout << "chrisRot+y=" << chrisRot;
+ chrisRot = Rotation( UnitVec3( Vec3(1,1,0) ), XAxis, Vec3(0,-1,0), YAxis );
+ cout << "chrisRot-y=" << chrisRot;
+ chrisRot = Rotation( UnitVec3( Vec3(1,1,0) ), XAxis, Vec3(0,0,1), YAxis );
+ cout << "chrisRot+z=" << chrisRot;
+ return 0;
+}
+catch(const Exception::Base& e) {
+ std::cout << e.getMessage() << std::endl;
+}
+}
+
+static Rotation rotate1(int i, Real a) {
+ assert(0 <= i && i < 3);
+ int j=(i+1)%3, k=(i+2)%3;
+ Real s=std::sin(a), c=std::cos(a);
+ Mat33 m;
+ m(i,i)=1; m(i,j)=m(j,i)=m(i,k)=m(k,i)=0;
+ m(j,j)=m(k,k)=c;
+ m(k,j)=s; m(j,k)=-s;
+ return Rotation(m,true);
+}
+
+
+// Two-angle space-fixed rotations.
+
+//0,1
+static Rotation aboutXThenOldY(const Real& xInRad, const Real& yInRad) {
+ const Real s0 = std::sin(xInRad), c0 = std::cos(xInRad);
+ const Real s1 = std::sin(yInRad), c1 = std::cos(yInRad);
+ const Mat33 m( c1 , s0*s1 , c0*s1 ,
+ 0 , c0 , -s0 ,
+ -s1 , s0*c1 , c0*c1 ); return Rotation(m,true);
+}
+//2,0
+static Rotation aboutZThenOldX(const Real& zInRad, const Real& xInRad) {
+ const Real s1 = std::sin(xInRad), c1 = std::cos(xInRad);
+ const Real s0 = std::sin(zInRad), c0 = std::cos(zInRad);
+ const Mat33 m( c0 , -s0 , 0 ,
+ s0*c1 , c0*c1 , -s1 ,
+ s0*s1 , c0*s1 , c1 ); return Rotation(m,true);
+}
+//1,2
+static Rotation aboutYThenOldZ(const Real& yInRad, const Real& zInRad) {
+ const Real s0 = std::sin(yInRad), c0 = std::cos(yInRad);
+ const Real s1 = std::sin(zInRad), c1 = std::cos(zInRad);
+ const Mat33 m( c0*c1 , -s1 , s0*c1 ,
+ c0*s1 , c1 , s0*s1 ,
+ -s0 , 0 , c0 ); return Rotation(m,true);
+}
+
+//1,0
+static Rotation aboutYThenOldX(const Real& yInRad, const Real& xInRad) {
+ const Real s1 = std::sin(xInRad), c1 = std::cos(xInRad);
+ const Real s0 = std::sin(yInRad), c0 = std::cos(yInRad);
+ const Mat33 m( c0 , 0 , s0 ,
+ s0*s1 , c1 , -c0*s1 ,
+ -s0*c1 , s1 , c0*c1 ); return Rotation(m,true);
+}
+//0,2
+static Rotation aboutXThenOldZ(const Real& xInRad, const Real& zInRad) {
+ const Real s0 = std::sin(xInRad), c0 = std::cos(xInRad);
+ const Real s1 = std::sin(zInRad), c1 = std::cos(zInRad);
+ const Mat33 m( c1 , -c0*s1 , s0*s1 ,
+ s1 , c0*c1 , -s0*c1 ,
+ 0 , s0 , c0 ); return Rotation(m,true);
+}
+
+//2,1
+static Rotation aboutZThenOldY(const Real& zInRad, const Real& yInRad) {
+ const Real s1 = std::sin(yInRad), c1 = std::cos(yInRad);
+ const Real s0 = std::sin(zInRad), c0 = std::cos(zInRad);
+ const Mat33 m( c0*c1 , -s0*c1 , s1 ,
+ s0 , c0 , 0 ,
+ -c0*s1 , s0*s1 , c1 ); return Rotation(m,true);
+}
diff --git a/SimTKcommon/tests/PolynomialTest.cpp b/SimTKcommon/tests/PolynomialTest.cpp
new file mode 100644
index 0000000..5756d2a
--- /dev/null
+++ b/SimTKcommon/tests/PolynomialTest.cpp
@@ -0,0 +1,445 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+using std::cout;
+using std::endl;
+using namespace SimTK;
+
+/**
+ * Determine if two complex numbers are equal to within the specified tolerance.
+ */
+
+bool equal(Complex expected, Complex found, Real tol) {
+ Real diffr = std::abs(expected.real()-found.real());
+ Real diffi = std::abs(expected.imag()-found.imag());
+ Real scaler = std::max(std::abs(expected.real()), std::abs(found.real()));
+ Real scalei = std::max(std::abs(expected.imag()), std::abs(found.imag()));
+ if (expected.real() == 0.0)
+ scaler = 1.0;
+ if (expected.imag() == 0.0)
+ scalei = 1.0;
+ return (diffr < tol*scaler && diffi < tol*scalei);
+}
+
+/**
+ * Determine if two roots of a quadratic polynomial are equal. This is a more stringent test then the above routine,
+ * since the quadratic solver has very high accuracy.
+ */
+
+bool equal2(Complex expected, Complex found) {
+ if (expected.imag() == 0.0 && found.imag() != 0.0)
+ return false; // If we expect a real number, require the number found to be precisely real as well.
+ return equal(expected, found, std::sqrt(NTraits<Real>::getEps()));
+}
+
+/**
+ * Find the roots of the specified polynomial with real coefficients, and compare them to the expected ones.
+ */
+
+void testQuadratic(Vec3 coeff, Vec<2,Complex> expected) {
+ Vec<2,Complex> found;
+ PolynomialRootFinder::findRoots(coeff, found);
+ ASSERT((equal2(expected[0], found[0]) && equal2(expected[1], found[1])) || (equal2(expected[0], found[1]) && equal2(expected[1], found[0])))
+}
+
+/**
+ * Find the roots of the specified polynomial with complex coefficients, and compare them to the expected ones.
+ */
+
+void testQuadratic(Vec<3,Complex> coeff, Vec<2,Complex> expected) {
+ Vec<2,Complex> found;
+ PolynomialRootFinder::findRoots(coeff, found);
+ ASSERT((equal2(expected[0], found[0]) && equal2(expected[1], found[1])) || (equal2(expected[0], found[1]) && equal2(expected[1], found[0])))
+}
+
+/**
+ * Generate a polynomial that has the specified roots, then solve it and compare.
+ */
+
+void testQuadraticRealRoots(Vec2 roots) {
+ static Random::Gaussian random(0.0, 100.0);
+ Vec3 coeff(1.0, -roots[0]-roots[1], roots[0]*roots[1]);
+ coeff *= random.getValue();
+ testQuadratic(coeff, Vec<2,Complex>(roots[0], roots[1]));
+}
+
+/**
+ * Generate a polynomial whose roots are a complex conjugate pair, then solve it and compare.
+ */
+
+void testQuadraticConjugateRoots(Complex root) {
+ static Random::Gaussian random(0.0, 1000.0);
+ Vec3 coeff(1.0, -2.0*root.real(), norm(root));
+ coeff *= random.getValue();
+ testQuadratic(coeff, Vec<2,Complex>(root, conj(root)));
+}
+
+/**
+ * Generate a polynomial that has the specified roots, then solve it and compare.
+ */
+
+void testQuadraticComplexRoots(Vec<2,Complex>roots) {
+ static Random::Gaussian random(0.0, 100.0);
+ Vec<3,Complex> coeff(1.0, -roots[0]-roots[1], roots[0]*roots[1]);
+ coeff *= Complex(random.getValue(), random.getValue());
+ testQuadratic(coeff, roots);
+}
+
+/**
+ * Find the roots of the specified polynomial with real coefficients, and compare them to the expected ones.
+ */
+
+void testCubic(Vec4 coeff, Vec<3,Complex> expected) {
+ Vec<3,Complex> found;
+ PolynomialRootFinder::findRoots(coeff, found);
+ Real tol = 1e-4;
+ if (equal(expected[0], found[0], tol))
+ ASSERT((equal(expected[1], found[1], tol) && equal(expected[2], found[2], tol)) || (equal(expected[1], found[2], tol) && equal(expected[2], found[1], tol)))
+ else if (equal(expected[0], found[1], tol))
+ ASSERT((equal(expected[1], found[2], tol) && equal(expected[2], found[0], tol)) || (equal(expected[1], found[0], tol) && equal(expected[2], found[2], tol)))
+ else if (equal(expected[0], found[2], tol))
+ ASSERT((equal(expected[1], found[0], tol) && equal(expected[2], found[1], tol)) || (equal(expected[1], found[1], tol) && equal(expected[2], found[0], tol)))
+ else
+ ASSERT(false)
+}
+
+/**
+ * Find the roots of the specified polynomial with complex coefficients, and compare them to the expected ones.
+ */
+
+void testCubic(Vec<4,Complex> coeff, Vec<3,Complex> expected) {
+ Vec<3,Complex> found;
+ PolynomialRootFinder::findRoots(coeff, found);
+ Real tol = 1e-4;
+ if (equal(expected[0], found[0], tol))
+ ASSERT((equal(expected[1], found[1], tol) && equal(expected[2], found[2], tol)) || (equal(expected[1], found[2], tol) && equal(expected[2], found[1], tol)))
+ else if (equal(expected[0], found[1], tol))
+ ASSERT((equal(expected[1], found[2], tol) && equal(expected[2], found[0], tol)) || (equal(expected[1], found[0], tol) && equal(expected[2], found[2], tol)))
+ else if (equal(expected[0], found[2], tol))
+ ASSERT((equal(expected[1], found[0], tol) && equal(expected[2], found[1], tol)) || (equal(expected[1], found[1], tol) && equal(expected[2], found[0], tol)))
+ else
+ ASSERT(false)
+}
+
+/**
+ * Generate a polynomial that has the specified roots, then solve it and compare.
+ */
+
+void testCubicRealRoots(Vec3 roots) {
+ static Random::Gaussian random(0.0, 100.0);
+ Vec4 coeff(1.0, -roots[0]-roots[1]-roots[2], roots[0]*roots[1]+roots[1]*roots[2]+roots[2]*roots[0], -roots[0]*roots[1]*roots[2]);
+ coeff *= random.getValue();
+ testCubic(coeff, Vec<3,Complex>(roots[0], roots[1], roots[2]));
+}
+
+/**
+ * Generate a polynomial whose roots are a complex conjugate pair plus a single real root, then solve it and compare.
+ */
+
+void testCubicConjugateRoots(Complex root1, Real root2) {
+ static Random::Gaussian random(0.0, 1000.0);
+ Vec4 coeff(1.0, -2.0*root1.real()-root2, norm(root1)+2.0*root1.real()*root2,-norm(root1)*root2);
+ coeff *= random.getValue();
+ testCubic(coeff, Vec<3,Complex>(root1, conj(root1), root2));
+}
+
+/**
+ * Generate a polynomial that has the specified roots, then solve it and compare.
+ */
+
+void testCubicComplexRoots(Vec<3,Complex> roots) {
+ static Random::Gaussian random(0.0, 100.0);
+ Vec<4,Complex> coeff(1.0, -roots[0]-roots[1]-roots[2], roots[0]*roots[1]+roots[1]*roots[2]+roots[2]*roots[0], -roots[0]*roots[1]*roots[2]);
+ coeff *= random.getValue();
+ testCubic(coeff, Vec<3,Complex>(roots[0], roots[1], roots[2]));
+}
+
+// Evaluate a real-coefficient polynomial at the given complex value. Should
+// evaluate to zero if this is a root.
+Complex evalPoly(Vector_<Real>& coefficients, const Complex& value) {
+ Complex sum = 0.0;
+ for (int j = 0; j < coefficients.size(); ++j)
+ sum = sum*value+coefficients[j];
+ return sum;
+}
+
+// Evaluate a complex-coefficient polynomial at the given complex value. Should
+// evaluate to zero if this is a root.
+Complex evalPoly(Vector_<Complex>& coefficients, const Complex& value) {
+ Complex sum = 0.0;
+ for (int j = 0; j < coefficients.size(); ++j)
+ sum = sum*value+coefficients[j];
+ return sum;
+}
+
+/**
+ * Given a polynomial and a list of roots, verify that they really are roots.
+ */
+void verifyRoots(Vector_<Real>& coefficients, Vector_<Complex>& roots, Real tol=1e-2) {
+ for (int i = 0; i < roots.size(); ++i) {
+ Complex sum = evalPoly(coefficients, roots[i]);
+ ASSERT(equal(0.0, sum, tol))
+ }
+}
+
+/**
+ * Given a polynomial and a list of roots, verify that they really are roots.
+ */
+
+void verifyRoots(Vector_<Complex>& coefficients, Vector_<Complex>& roots, Real tol=1e-2) {
+ for (int i = 0; i < roots.size(); ++i) {
+ Complex sum = evalPoly(coefficients, roots[i]);
+ ASSERT(equal(0.0, sum, tol))
+ }
+}
+
+/**
+ * Perform a variety of tests solving quadratic polynomials.
+ */
+
+void testSolveQuadratic() {
+
+ // Try a few fixed tests.
+
+ testQuadraticRealRoots(Vec2(0.0, 0.0));
+ testQuadraticRealRoots(Vec2(1.0, 1.0));
+ testQuadraticRealRoots(Vec2(0.0, 5.0));
+ testQuadraticRealRoots(Vec2(10.0, -5.0));
+ testQuadratic(Vec3(1.0, 0.0, 0.0), Vec<2,Complex>(0.0, 0.0));
+
+ // Try cases with a single, repeated real root.
+
+ static Random::Gaussian random(0.0, 100.0);
+ for (int i = 0; i < 1000; ++i) {
+ Real root = random.getValue();
+ testQuadraticRealRoots(Vec2(root, root));
+ }
+
+ // Try cases with two distinct real roots.
+
+ for (int i = 0; i < 1000; ++i)
+ testQuadraticRealRoots(Vec2(random.getValue(), random.getValue()));
+
+ // Try cases whose roots are complex conjugates.
+
+ for (int i = 0; i < 1000; ++i)
+ testQuadraticConjugateRoots(Complex(random.getValue(), random.getValue()));
+
+ // Try cases with two distinct complex roots.
+
+ for (int i = 0; i < 1000; ++i)
+ testQuadraticComplexRoots(Vec<2,Complex>(Complex(random.getValue(), random.getValue()), Complex(random.getValue(), random.getValue())));
+
+ // Verify that if the leading coefficient is zero, it throws an exception.
+
+ try {
+ testQuadratic(Vec3(0.0, 1.0, 1.0), Vec<2,Complex>(0.0, 0.0));
+ ASSERT(false)
+ }
+ catch (PolynomialRootFinder::ZeroLeadingCoefficient ex) {
+ }
+ try {
+ testQuadratic(Vec<3,Complex>(0.0, 1.0, 1.0), Vec<2,Complex>(0.0, 0.0));
+ ASSERT(false)
+ }
+ catch (PolynomialRootFinder::ZeroLeadingCoefficient ex) {
+ }
+}
+
+/**
+ * Perform a variety of tests solving cubic polynomials.
+ */
+
+void testSolveCubic() {
+
+ // Try a few fixed tests.
+
+ testCubicRealRoots(Vec3(0.0, 0.0, 0.0));
+ testCubicRealRoots(Vec3(1.0, 1.0, 1.0));
+ testCubicRealRoots(Vec3(0.0, 5.0, 5.0));
+ testCubicRealRoots(Vec3(10.0, -5.0, 100.0));
+ testCubic(Vec4(1.0, 0.0, 0.0, 0.0), Vec<3,Complex>(0.0, 0.0, 0.0));
+
+ // Try cases with a single, repeated real root.
+
+ static Random::Gaussian random(0.0, 100.0);
+ for (int i = 0; i < 1000; ++i) {
+ Real root = random.getValue();
+ testCubicRealRoots(Vec3(root, root, root));
+ }
+
+ // Try cases with two distinct real roots.
+
+ for (int i = 0; i < 1000; ++i) {
+ Real root = random.getValue();
+ testCubicRealRoots(Vec3(root, root, random.getValue()));
+ }
+
+ // Try cases with three distinct real roots.
+
+ for (int i = 0; i < 1000; ++i)
+ testCubicRealRoots(Vec3(random.getValue(), random.getValue(), random.getValue()));
+
+ // Try cases whose roots include a complex conjugates pair.
+
+ for (int i = 0; i < 1000; ++i)
+ testCubicConjugateRoots(Complex(random.getValue(), random.getValue()), random.getValue());
+
+ // Try cases with three distinct complex roots.
+
+ for (int i = 0; i < 1000; ++i)
+ testCubicComplexRoots(Vec<3,Complex>(Complex(random.getValue(), random.getValue()), Complex(random.getValue(), random.getValue()), Complex(random.getValue(), random.getValue())));
+
+ // Verify that if the leading coefficient is zero, it throws an exception.
+
+ try {
+ testCubic(Vec4(0.0, 0.0, 0.0, 0.0), Vec<3,Complex>(0.0, 0.0, 0.0));
+ ASSERT(false)
+ }
+ catch (PolynomialRootFinder::ZeroLeadingCoefficient ex) {
+ }
+ try {
+ testCubic(Vec<4,Complex>(0.0, 0.0, 0.0, 0.0), Vec<3,Complex>(0.0, 0.0, 0.0));
+ ASSERT(false)
+ }
+ catch (PolynomialRootFinder::ZeroLeadingCoefficient ex) {
+ }
+}
+
+/**
+ * Test solving polynomials of arbitrary degree.
+ */
+
+void testSolveArbitrary() {
+ static Random::Uniform uniform(2.0, 7.0);
+ static Random::Gaussian random(0.0, 100.0);
+
+ // Test random polynomials of various degrees with real coefficients.
+
+ Vector realCoeff;
+ Vector_<Complex> roots;
+ for (int i = 0; i < 1000; ++i) {
+ int length = uniform.getIntValue();
+ realCoeff.resize(length);
+ roots.resize(length-1);
+ for (int j = 0; j < length; ++j)
+ realCoeff[j] = random.getValue();
+ PolynomialRootFinder::findRoots(realCoeff, roots);
+ verifyRoots(realCoeff, roots);
+ }
+
+ // Test random polynomials of various degrees with complex coefficients.
+
+ Vector_<Complex> complexCoeff;
+ for (int i = 0; i < 1000; ++i) {
+ int length = uniform.getIntValue();
+ complexCoeff.resize(length);
+ roots.resize(length-1);
+ for (int j = 0; j < length; ++j)
+ complexCoeff[j] = Complex(random.getValue(), random.getValue());
+ PolynomialRootFinder::findRoots(complexCoeff, roots);
+ verifyRoots(complexCoeff, roots);
+ }
+
+ // Verify that if the leading coefficient is zero, it throws an exception.
+
+ try {
+ realCoeff[0] = 0.0;
+ PolynomialRootFinder::findRoots(realCoeff, roots);
+ ASSERT(false)
+ }
+ catch (PolynomialRootFinder::ZeroLeadingCoefficient ex) {
+ }
+ try {
+ complexCoeff[0] = 0.0;
+ PolynomialRootFinder::findRoots(complexCoeff, roots);
+ ASSERT(false)
+ }
+ catch (PolynomialRootFinder::ZeroLeadingCoefficient ex) {
+ }
+}
+
+// Sherm 20130410:
+// This 6th order polynomial was generated by Ellipsoid's findNearestPoint()
+// method and caused the Jenkins-Traub polynomial solver to fail
+// catastrophically, returning no roots, while many nearly identical
+// polynomials solved perfectly. I put in a fix (see rpoly.cpp) and to make
+// sure it stays fixed I'm recording the bad polynomial here.
+void testReallyAwfulEllipsoidPolynomial() {
+ Vector good1(7), good2(7), bad(7);
+ Vector_<Complex> roots(6);
+
+ good1[0] = 1.0000000000000000;
+ good1[1] = 0.093099999999999988;
+ good1[2] = 0.00057211940879762883;
+ good1[3] = 1.4343090324181468e-006;
+ good1[4] = 1.6097307763625053e-009;
+ good1[5] = 6.6189348690786845e-013;
+ good1[6] = -3.0418139616145048e-018;
+ PolynomialRootFinder::findRoots(good1, roots);
+ verifyRoots(good1, roots, 1e-10);
+
+ good2[0] = 1.0000000000000000;
+ good2[1] = 0.021700000000000004;
+ good2[2] = 0.00013355532616269355;
+ good2[3] = 1.8068473626980300e-007;
+ good2[4] = 6.2414644021223684e-011;
+ good2[5] = 6.4036661086410838e-015;
+ good2[6] = -6.8627441483352105e-022;
+ PolynomialRootFinder::findRoots(good2, roots);
+ verifyRoots(good2, roots, 1e-10);
+
+ // This one breaks the original Jenkins-Traub method. Sure doesn't look
+ // much different than the ones above.
+ bad[0] = 1.0000000000000000;
+ bad[1] = 0.021700000000000004;
+ bad[2] = 2.9889970904696875e-005;
+ bad[3] = 1.0901272298136685e-008;
+ bad[4] = -4.4822782160985054e-012;
+ bad[5] = -2.6193432740351220e-015;
+ bad[6] = -3.0900602527225053e-019;
+ PolynomialRootFinder::findRoots(bad, roots);
+ verifyRoots(bad, roots, 1e-10);
+
+}
+
+int main() {
+ try {
+ testSolveQuadratic();
+ testSolveCubic();
+ testSolveArbitrary();
+ testReallyAwfulEllipsoidPolynomial();
+ } catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ return 0;
+}
+
+
diff --git a/SimTKcommon/tests/RandomTest.cpp b/SimTKcommon/tests/RandomTest.cpp
new file mode 100644
index 0000000..74308f7
--- /dev/null
+++ b/SimTKcommon/tests/RandomTest.cpp
@@ -0,0 +1,235 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+using std::cout;
+using std::endl;
+using std::sqrt;
+using namespace SimTK;
+
+/**
+ * Given the number of values expected and found in a set of bins, verify that the distribution is correct.
+ */
+
+void verifyDistribution(int expected[], int found[], int bins) {
+ for (int i = 0; i < bins; ++i) {
+ Real dev = sqrt((Real) expected[i]);
+ ASSERT(found[i] >= expected[i]-4*dev && found[i] <= expected[i]+4*dev)
+ }
+}
+
+/**
+ * Given a set of Reals, verify that they satisfy a uniform distribution between 0 and 1.
+ */
+
+void verifyUniformDistribution(Real min, Real max, Real value[], int length) {
+ int expected[10], found[10];
+ for (int i = 0; i < 10; ++i) {
+ expected[i] = length/10;
+ found[i] = 0;
+ }
+ for (int i = 0; i < length; ++i) {
+ ASSERT(value[i] >= min)
+ ASSERT(value[i] < max)
+ int index = (int) ((value[i]-min)*10/(max-min));
+ found[index]++;
+ }
+ verifyDistribution(expected, found, 10);
+}
+
+/**
+ * Given a set of ints, verify that they satisfy a uniform distribution between 0 and max.
+ */
+
+void verifyUniformDistribution(int min, int max, int value[], int length) {
+ int range = max-min;
+ int* expected = new int[range];
+ int* found = new int[range];
+ for (int i = 0; i < range; ++i) {
+ expected[i] = length/range;
+ found[i] = 0;
+ }
+ for (int i = 0; i < length; ++i) {
+ ASSERT(value[i] >= min)
+ ASSERT(value[i] < max)
+ found[value[i]-min]++;
+ }
+ verifyDistribution(expected, found, range);
+ delete[] expected;
+ delete[] found;
+}
+
+/**
+ * Given a set of values, verify that they satisfy a Gaussian distribution.
+ */
+
+void verifyGaussianDistribution(Real mean, Real stddev, Real value[], int length) {
+ int expected[6], found[6];
+ expected[0] = expected[5] = (int) (0.0228*length);
+ expected[1] = expected[4] = (int) (0.1587*length-expected[0]);
+ expected[2] = expected[3] = (int) (0.5*length-expected[1]);
+ for (int i = 0; i < 6; ++i)
+ found[i] = 0;
+ for (int i = 0; i < length; ++i) {
+ Real val = (value[i]-mean)/stddev;
+ if (val < -2)
+ found[0]++;
+ else if (val < -1)
+ found[1]++;
+ else if (val < 0)
+ found[2]++;
+ else if (val < 1)
+ found[3]++;
+ else if (val < 2)
+ found[4]++;
+ else
+ found[5]++;
+ }
+ verifyDistribution(expected, found, 6);
+}
+
+void testUniform() {
+ Random::Uniform rand;
+ ASSERT(rand.getMin() == 0.0)
+ ASSERT(rand.getMax() == 1.0)
+
+ // Try generating a bunch of random numbers, and make sure they are distributed uniformly between 0 and 1.
+
+ Real value[2001];
+ value[2000] = 123.4;
+ rand.setSeed(1);
+ for (int i = 0; i < 2000; ++i)
+ value[i] = rand.getValue();
+ verifyUniformDistribution(0.0, 1.0, value, 2000);
+
+ // Reset the random number generator, and make sure it produces the same values again.
+
+ rand.setSeed(1);
+ for (int i = 0; i < 2000; ++i)
+ ASSERT(value[i] == rand.getValue())
+
+ // Now try asking for a whole array at a time, and verify that it still gives the same results.
+
+ Real value2[2001];
+ value2[2000] = 567.8;
+ rand.setSeed(1);
+ rand.fillArray(value2, 2000);
+ for (int i = 0; i < 2000; ++i)
+ ASSERT(value[i] == value2[i])
+
+ // Set the seed to a different value, and verify that the results are different.
+
+ rand.setSeed(2);
+ rand.fillArray(value2, 2000);
+ for (int i = 0; i < 2000; ++i)
+ ASSERT(value[i] != value2[i])
+
+ // Change the range and test the distribution.
+
+ rand.setMin(5.0);
+ rand.setMax(20.0);
+ ASSERT(rand.getMin() == 5.0)
+ ASSERT(rand.getMax() == 20.0)
+ rand.fillArray(value2, 2000);
+ verifyUniformDistribution(5.0, 20.0, value2, 2000);
+
+ // Try generating uniform integers.
+
+ int value3[2001];
+ value3[2000] = -99;
+ rand.setSeed(3);
+ for (int i = 0; i < 2000; ++i)
+ value3[i] = rand.getIntValue();
+ verifyUniformDistribution(5, 20, value3, 2000);
+
+ // Verify that if we do not explicitly set the seed, every Random object is initialized with a different seed.
+
+ Random::Uniform rand1, rand2;
+ rand1.fillArray(value, 2000);
+ rand2.fillArray(value2, 2000);
+ for (int i = 0; i < 2000; ++i)
+ ASSERT(value[i] != value2[i])
+
+ // Make sure none of the above operations has overwritten the final array element.
+
+ ASSERT(value[2000] == 123.4)
+ ASSERT(value2[2000] = 567.8)
+ ASSERT(value3[2000] = -99)
+}
+
+void testGaussian() {
+ Random::Gaussian rand;
+ ASSERT(rand.getMean() == 0.0)
+ ASSERT(rand.getStdDev() == 1.0)
+
+ // Try generating a bunch of Gaussian random numbers, and check the distribution.
+
+ Real value[2001];
+ value[2000] = 123.4;
+ rand.setSeed(1);
+ for (int i = 0; i < 2000; ++i)
+ value[i] = rand.getValue();
+ verifyGaussianDistribution(0.0, 1.0, value, 2000);
+
+ // Try getting a whole array at a time.
+
+ Real value2[2001];
+ value2[2000] = 567.8;
+ rand.setSeed(1);
+ rand.fillArray(value2, 2000);
+ for (int i = 0; i < 2000; ++i)
+ ASSERT(value[i] == value2[i])
+
+ // Change the parameters and test the distribution.
+
+ rand.setMean(10.0);
+ rand.setStdDev(7.0);
+ ASSERT(rand.getMean() == 10.0)
+ ASSERT(rand.getStdDev() == 7.0)
+ rand.fillArray(value2, 2000);
+ verifyGaussianDistribution(10.0, 7.0, value2, 2000);
+
+ // Make sure none of the above operations has overwritten the final array element.
+
+ ASSERT(value[2000] == 123.4)
+ ASSERT(value2[2000] = 567.8)
+}
+
+int main() {
+ try {
+ testUniform();
+ testGaussian();
+ } catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ cout << "Done" << endl;
+ return 0;
+}
+
+
diff --git a/SimTKcommon/tests/RotationTest.cpp b/SimTKcommon/tests/RotationTest.cpp
new file mode 100644
index 0000000..84d5059
--- /dev/null
+++ b/SimTKcommon/tests/RotationTest.cpp
@@ -0,0 +1,745 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Paul Mitiguy *
+ * Contributors: Michael Sherman *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Tests for the classes defined in Rotation.h.
+ */
+
+//-----------------------------------------------------------------------------
+#include "SimTKcommon.h"
+#include "SimTKcommon/Testing.h"
+
+#include <iostream>
+using std::cout; using std::endl;
+
+//-----------------------------------------------------------------------------
+using namespace SimTK;
+
+bool doRequiredTasks();
+void WriteStringToScreen( const char outputString[] ) { std::cout << outputString; } // fputs( outputString,stdout ); if include<stdio>
+
+//-------------------------------------------------------------------
+int main() {
+
+ // Default value is program failed
+ bool programSucceeded = false;
+
+ // It is a good programming practice to do little in the main function of a program.
+ // The try-catch code in this main routine catches exceptions thrown by functions in the
+ // try block, e.g., catching an exception that occurs when a NULL pointer is de-referenced.
+ try {
+ // Do the required programming tasks
+ programSucceeded = doRequiredTasks();
+ }
+ // This catch statement handles certain types of exceptions
+ catch( const std::exception& e ) {
+ WriteStringToScreen( "\n\n Error: Programming error encountered.\n The exception thrown is: " );
+ WriteStringToScreen( e.what() );
+ WriteStringToScreen( "\n\n" );
+ }
+ // The exception-declaration statement (...) handles any type of exception,
+ // including C exceptions and system/application generated exceptions.
+ // This includes exceptions such as memory protection and floating-point violations.
+ // An ellipsis catch handler must be the last handler for its try block.
+ catch( ... ) {
+ WriteStringToScreen( "\n\n Error: Programming error encountered.\n An unhandled exception was thrown.\n\n" );
+ }
+
+ if (!programSucceeded)
+ WriteStringToScreen("\nError: a test failed.\n");
+
+ // The value returned by the main function is the exit status of the program.
+ // A normal program exit returns 0 (other return values usually signal an error).
+ return programSucceeded == true ? 0 : 1;
+}
+
+
+//-------------------------------------------------------------------
+// Prototypes for methods in this test
+//-------------------------------------------------------------------
+bool testRotationOneAxis( const Real angle, const CoordinateAxis& axis );
+bool testRotationTwoAxes( const BodyOrSpaceType bodyOrSpace, const Real angle1, const CoordinateAxis& axis1, const Real angle2, const CoordinateAxis &axis2 );
+bool testRotationThreeAxes( const BodyOrSpaceType bodyOrSpace, const Real angle1, const CoordinateAxis& axis1, const Real angle2, const CoordinateAxis &axis2, const Real angle3, const CoordinateAxis &axis3 );
+bool testQuaternion( Real e0, Real e1, Real e2, Real e3 );
+bool testSetRotationToBodyFixedXYZ();
+
+bool testInverseRotation1Angle( Real angle, Real theta );
+bool testInverseRotation2Angle( Real angle1, Real theta1, Real angle2, Real theta2 );
+bool testInverseRotation3AngleTwoAxes( Real angle1, Real theta1, Real angle2, Real theta2, Real angle3, Real theta3 );
+bool testInverseRotation3AngleThreeAxes( Real angle1, Real theta1, Real angle2, Real theta2, Real angle3, Real theta3 );
+
+
+bool exhaustiveTestof1AngleRotation();
+bool exhaustiveTestof2AngleRotation();
+bool exhaustiveTestof3AngleRotation();
+bool exhaustiveTestof3AngleTwoAxesRotationNearSingularity();
+bool exhaustiveTestof3AngleThreeAxesRotationNearSingularity();
+bool exhaustiveTestofQuaternions();
+
+bool testRotationFromTwoGivenAxes( const Vec3& vi, const CoordinateAxis& ai, const Vec3& vj, const CoordinateAxis& aj);
+bool testReexpressSymMat33();
+
+//-------------------------------------------------------------------
+bool doRequiredTasks( ) {
+
+ // Use the next Rotation to test against (simple theta-lambda rotation)
+ Rotation testRotation;
+
+ // Test default constructor
+ Rotation defaultRotationConstructor;
+ testRotation.setRotationToIdentityMatrix();
+ bool test = defaultRotationConstructor.areAllRotationElementsSameToMachinePrecision( testRotation );
+
+ // Test copy constructor
+ testRotation.setRotationFromAngleAboutNonUnitVector( 1.0, Vec3(0.2, 0.4, 0.6) );
+ Rotation rotationCopyConstructor( testRotation );
+ test = test && rotationCopyConstructor.areAllRotationElementsSameToMachinePrecision( testRotation );
+
+ // Test operator =
+ Rotation rotationOperatorEqual = testRotation;
+ test = test && rotationOperatorEqual.areAllRotationElementsSameToMachinePrecision( testRotation );
+
+ // Test rotation by angle about arbitrary CoordinateAxis
+ testRotation.setRotationFromAngleAboutNonUnitVector( 0.1, Vec3(1.0, 0.0, 0.0) );
+ CoordinateAxis coordAxis = XAxis;
+ Rotation rotationCoordAxis( 0.1, coordAxis );
+ test = test && rotationCoordAxis.areAllRotationElementsSameToMachinePrecision( testRotation );
+
+ Real testTheta = rotationCoordAxis.convertOneAxisRotationToOneAngle( coordAxis );
+ test = test && fabs(0.1 - testTheta) < 10*SignificantReal;
+
+ // Test rotation by angle about XAxis, YAxis, ZAxis
+ test = test && testRotationOneAxis( 0.2, XAxis );
+ test = test && testRotationOneAxis( -0.2, XAxis );
+ test = test && testRotationOneAxis( 2.1, YAxis );
+ test = test && testRotationOneAxis( -2.1, YAxis );
+ test = test && testRotationOneAxis( 3.1, ZAxis );
+ test = test && testRotationOneAxis( -3.1, ZAxis );
+
+ // Test rotation with two angles and two axes XX, XY, XZ
+ test = test && testRotationTwoAxes( BodyRotationSequence, 0.2, XAxis, 0.3, XAxis );
+ test = test && testRotationTwoAxes( SpaceRotationSequence, 0.2, XAxis, 0.3, XAxis );
+ test = test && testRotationTwoAxes( BodyRotationSequence, 1.2, XAxis,-1.3, YAxis );
+ test = test && testRotationTwoAxes( SpaceRotationSequence, 1.2, XAxis,-1.3, YAxis );
+ test = test && testRotationTwoAxes( BodyRotationSequence, -3.1, XAxis, 1.2, ZAxis );
+ test = test && testRotationTwoAxes( SpaceRotationSequence, -3.1, XAxis, 1.2, ZAxis );
+
+ // Test rotation with two angles and two axes YX, YY, YZ
+ test = test && testRotationTwoAxes( BodyRotationSequence, 1.2, YAxis, 0.3, XAxis );
+ test = test && testRotationTwoAxes( SpaceRotationSequence, 1.2, YAxis, 0.3, XAxis );
+ test = test && testRotationTwoAxes( BodyRotationSequence, 2.2, YAxis,-1.3, YAxis );
+ test = test && testRotationTwoAxes( SpaceRotationSequence, 2.2, YAxis,-1.3, YAxis );
+ test = test && testRotationTwoAxes( BodyRotationSequence, -3.1, YAxis, 1.2, ZAxis );
+ test = test && testRotationTwoAxes( SpaceRotationSequence, -3.1, YAxis, 1.2, ZAxis );
+
+ // Test rotation with two angles and two axes ZX, ZY, ZZ
+ test = test && testRotationTwoAxes( BodyRotationSequence, 1.2, ZAxis, 0.3, XAxis );
+ test = test && testRotationTwoAxes( SpaceRotationSequence, 1.2, ZAxis, 0.3, XAxis );
+ test = test && testRotationTwoAxes( BodyRotationSequence, 2.2, ZAxis,-1.3, YAxis );
+ test = test && testRotationTwoAxes( SpaceRotationSequence, 2.2, ZAxis,-1.3, YAxis );
+ test = test && testRotationTwoAxes( BodyRotationSequence, -3.1, ZAxis, 1.2, ZAxis );
+ test = test && testRotationTwoAxes( SpaceRotationSequence, -3.1, ZAxis, 1.2, ZAxis );
+
+ // Test the construction of rotations from two given axes.
+ const UnitVec3 vi(0.01, 0.02, .9);
+ const Vec3 vj(-0.5, 0.5, 0.2);
+
+ test = test && testRotationFromTwoGivenAxes(vi, XAxis, vj, YAxis);
+ test = test && testRotationFromTwoGivenAxes(vi, YAxis, vj, XAxis);
+
+ test = test && testRotationFromTwoGivenAxes(vi, YAxis, vj, ZAxis);
+ test = test && testRotationFromTwoGivenAxes(vi, ZAxis, vj, YAxis);
+
+ test = test && testRotationFromTwoGivenAxes(vi, ZAxis, vj, XAxis);
+ test = test && testRotationFromTwoGivenAxes(vi, XAxis, vj, ZAxis);
+
+ // Test rotation with three angles and three axes XXX, XXY, XXZ, XYX, XYY, XYZ, XZX, XZY, XZZ
+ test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, XAxis, 0.3, XAxis, 0.4, XAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, XAxis, 0.3, XAxis, 0.4, XAxis );
+ test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, XAxis,-1.3, XAxis,-1.4, YAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, XAxis,-1.3, XAxis,-1.4, YAxis );
+ test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, XAxis, 1.2, XAxis, 1.3, ZAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, XAxis, 1.2, XAxis, 1.3, ZAxis );
+
+ test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, XAxis, 0.3, YAxis, 0.4, XAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, XAxis, 0.3, YAxis, 0.4, XAxis );
+ test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, XAxis,-1.3, YAxis,-1.4, YAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, XAxis,-1.3, YAxis,-1.4, YAxis );
+ test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, XAxis, 1.2, YAxis, 1.3, ZAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, XAxis, 1.2, YAxis, 1.3, ZAxis );
+
+ test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, XAxis, 0.3, ZAxis, 0.4, XAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, XAxis, 0.3, ZAxis, 0.4, XAxis );
+ test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, XAxis,-1.3, ZAxis,-1.4, YAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, XAxis,-1.3, ZAxis,-1.4, YAxis );
+ test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, XAxis, 1.2, ZAxis, 1.3, ZAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, XAxis, 1.2, ZAxis, 1.3, ZAxis );
+
+ // Test rotation with three angles and three axes YXX, YXY, YXZ, YYX, YYY, YYZ, YZX, YZY, YZZ
+ test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, YAxis, 0.3, XAxis, 0.4, XAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, YAxis, 0.3, XAxis, 0.4, XAxis );
+ test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, YAxis,-1.3, XAxis,-1.4, YAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, YAxis,-1.3, XAxis,-1.4, YAxis );
+ test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, YAxis, 1.2, XAxis, 1.3, ZAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, YAxis, 1.2, XAxis, 1.3, ZAxis );
+
+ test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, YAxis, 0.3, YAxis, 0.4, XAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, YAxis, 0.3, YAxis, 0.4, XAxis );
+ test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, YAxis,-1.3, YAxis,-1.4, YAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, YAxis,-1.3, YAxis,-1.4, YAxis );
+ test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, YAxis, 1.2, YAxis, 1.3, ZAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, YAxis, 1.2, YAxis, 1.3, ZAxis );
+
+ test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, YAxis, 0.3, ZAxis, 0.4, XAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, YAxis, 0.3, ZAxis, 0.4, XAxis );
+ test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, YAxis,-1.3, ZAxis,-1.4, YAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, YAxis,-1.3, ZAxis,-1.4, YAxis );
+ test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, YAxis, 1.2, ZAxis, 1.3, ZAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, YAxis, 1.2, ZAxis, 1.3, ZAxis );
+
+ // Test rotation with three angles and three axes ZXX, ZXY, ZXZ, ZYX, ZYY, ZYZ, ZZX, ZZY, ZZZ
+ test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, ZAxis, 0.3, XAxis, 0.4, XAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, ZAxis, 0.3, XAxis, 0.4, XAxis );
+ test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, ZAxis,-1.3, XAxis,-1.4, YAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, ZAxis,-1.3, XAxis,-1.4, YAxis );
+ test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, ZAxis, 1.2, XAxis, 1.3, ZAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, ZAxis, 1.2, XAxis, 1.3, ZAxis );
+
+ test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, ZAxis, 0.3, YAxis, 0.4, XAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, ZAxis, 0.3, YAxis, 0.4, XAxis );
+ test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, ZAxis,-1.3, YAxis,-1.4, YAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, ZAxis,-1.3, YAxis,-1.4, YAxis );
+ test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, ZAxis, 1.2, YAxis, 1.3, ZAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, ZAxis, 1.2, YAxis, 1.3, ZAxis );
+
+ test = test && testRotationThreeAxes( BodyRotationSequence, 0.2, ZAxis, 0.3, ZAxis, 0.4, XAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, 0.2, ZAxis, 0.3, ZAxis, 0.4, XAxis );
+ test = test && testRotationThreeAxes( BodyRotationSequence, 1.2, ZAxis,-1.3, ZAxis,-1.4, YAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, 1.2, ZAxis,-1.3, ZAxis,-1.4, YAxis );
+ test = test && testRotationThreeAxes( BodyRotationSequence, -3.1, ZAxis, 1.2, ZAxis, 1.3, ZAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, -3.1, ZAxis, 1.2, ZAxis, 1.3, ZAxis );
+
+ // This used to fail
+ test = test && testRotationThreeAxes( BodyRotationSequence, -3.2288591161895095, XAxis, -3.1415926535897931, YAxis, -3.1415926535897931, XAxis );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, -3.2288591161895095, XAxis, -3.1415926535897931, YAxis, -3.1415926535897931, XAxis );
+
+ // Test Rotation quaternion methods.
+ test = test && testQuaternion( 0.5, 0.1, 0.2, 0.3 );
+ test = test && testQuaternion(-0.5, 0.1, 0.2, -0.3 );
+
+ Quaternion unnorm(Vec4(1,2,3,4), true); // don't do this at home
+ SimTK_TEST_NOTEQ(unnorm.norm(), Real(1)); // shouldn't have normalized
+ Quaternion fixedUp;
+ fixedUp = unnorm.normalize();
+ SimTK_TEST_EQ(fixedUp.norm(), Real(1));
+ unnorm.normalizeThis();
+ SimTK_TEST_EQ(unnorm.norm(), Real(1));
+
+
+ // Test construction of nearby orthogonal rotation matrix from a generic Mat33.
+ Rotation nearbyRotation( testRotation.asMat33() );
+ test = test && nearbyRotation.areAllRotationElementsSameToMachinePrecision( testRotation );
+
+ // Exhaustive test of 1-angle, 2-angle, and 3-angle rotations
+ test = test && exhaustiveTestof1AngleRotation();
+ test = test && exhaustiveTestof2AngleRotation();
+ test = test && exhaustiveTestof3AngleRotation();
+
+ // Exhaustive test of 3-angle rotations near singularity
+ test = test && exhaustiveTestof3AngleTwoAxesRotationNearSingularity();
+ test = test && exhaustiveTestof3AngleThreeAxesRotationNearSingularity();
+
+ // Exhaustive test of Quaterions
+ test = test && exhaustiveTestofQuaternions();
+
+ // Test special handling of body-fixed XYZ.
+ test = test && testSetRotationToBodyFixedXYZ();
+
+ // Test out special code for rotating symmetric matrices.
+ test = test && testReexpressSymMat33();
+
+ return test;
+}
+
+
+//-------------------------------------------------------------------
+bool testRotationOneAxis( const Real angle, const CoordinateAxis& axis ) {
+
+ // Form rotation about specified axis
+ Rotation rotationSpecified;
+ if( axis == XAxis ) rotationSpecified.setRotationFromAngleAboutX( angle );
+ if( axis == YAxis ) rotationSpecified.setRotationFromAngleAboutY( angle );
+ if( axis == ZAxis ) rotationSpecified.setRotationFromAngleAboutZ( angle );
+
+ // Form equivalent rotation by another means
+ Real unitX = axis == XAxis ? 1 : 0;
+ Real unitY = axis == YAxis ? 1 : 0;
+ Real unitZ = axis == ZAxis ? 1 : 0;
+ UnitVec3 unitVector( unitX, unitY, unitZ );
+ Rotation testRotation( angle, unitVector );
+
+ // Test to see if they are the same
+ bool test = rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );
+
+ // Do the inverse problem to back out the angle
+ const Real theta = rotationSpecified.convertOneAxisRotationToOneAngle( axis );
+
+ // Create a Rotation matrix with the backed-out angle and compare to the original Rotation matrix
+ testRotation.setRotationFromAngleAboutAxis( theta, axis );
+ test = test && rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );
+
+ // Conversion should produce angle = theta if angle is in proper range (-pi < angle <= pi)
+ test = test && testInverseRotation1Angle( angle, theta );
+
+ return test;
+}
+
+
+//-------------------------------------------------------------------
+bool testRotationTwoAxes( const BodyOrSpaceType bodyOrSpace, const Real angle1, const CoordinateAxis& axis1, const Real angle2, const CoordinateAxis &axis2 ) {
+
+ // Form rotation about specified axes
+ Rotation rotationSpecified( bodyOrSpace, angle1, axis1, angle2, axis2 );
+
+ // Form equivalent rotation by another means
+ Rotation AB( angle1, axis1 );
+ Rotation BC( angle2, axis2 );
+ Rotation testRotation = (bodyOrSpace == BodyRotationSequence) ? AB * BC : BC * AB;
+
+ // Test to see if they are the same
+ bool test = rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );
+
+ // Do the inverse problem to back out the angles
+ const Vec2 testVec = rotationSpecified.convertTwoAxesRotationToTwoAngles( bodyOrSpace, axis1, axis2 );
+ const Real theta1 = testVec[0];
+ const Real theta2 = testVec[1];
+
+ // Create a Rotation matrix with the backed-out angles and compare to the original Rotation matrix
+ testRotation.setRotationFromTwoAnglesTwoAxes( bodyOrSpace, theta1, axis1, theta2, axis2 );
+ test = test && rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );
+
+ // Conversion should produce same angles for for appropriate ranges of angle1 and angle2
+ if( axis1.isSameAxis(axis2) )
+ test = test && testInverseRotation1Angle( angle1+angle2, theta1+theta2 );
+ else
+ test = test && testInverseRotation2Angle( angle1,theta1, angle2,theta2 );
+
+ return test;
+}
+
+
+//-------------------------------------------------------------------
+bool testRotationThreeAxes( const BodyOrSpaceType bodyOrSpace, const Real angle1, const CoordinateAxis& axis1, const Real angle2, const CoordinateAxis &axis2, const Real angle3, const CoordinateAxis &axis3 ) {
+
+ // Form rotation about specified axes
+ Rotation rotationSpecified( bodyOrSpace, angle1, axis1, angle2, axis2, angle3, axis3 );
+
+ // Form equivalent rotation by another means
+ Rotation AB( angle1, axis1 );
+ Rotation BC( angle2, axis2 );
+ Rotation CD( angle3, axis3 );
+ Rotation testRotation = (bodyOrSpace == BodyRotationSequence) ? AB * BC * CD : CD * BC * AB;
+
+ // Test to see if they are the same
+ bool test = rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );
+
+ // Do the inverse problem to back out the angles
+ const Vec3 testVec = rotationSpecified.convertThreeAxesRotationToThreeAngles( bodyOrSpace, axis1, axis2, axis3 );
+ const Real theta1 = testVec[0];
+ const Real theta2 = testVec[1];
+ const Real theta3 = testVec[2];
+
+ // Create a Rotation matrix with the backed-out angles and compare to the original Rotation matrix
+ testRotation.setRotationFromThreeAnglesThreeAxes( bodyOrSpace, theta1, axis1, theta2, axis2, theta3, axis3 );
+ test = test && rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );
+
+ // Conversion should produce same angles for for appropriate ranges of angle1 and angle2
+ if( axis1.areAllSameAxes(axis2,axis3) )
+ test = test && testInverseRotation1Angle( angle1+angle2+angle3, theta1+theta2+theta3 );
+ else if( axis1.isSameAxis(axis2) )
+ test = test && testInverseRotation2Angle( angle1+angle2, theta1+theta1, angle3,theta3 );
+ else if( axis2.isSameAxis(axis3) )
+ test = test && testInverseRotation2Angle( angle1,theta1, angle2+angle3, theta2+theta3 );
+ else if( axis1.isSameAxis(axis3) )
+ test = test && testInverseRotation3AngleTwoAxes( angle1,theta1, angle2,theta2, angle3,theta3 );
+ else
+ test = test && testInverseRotation3AngleThreeAxes( angle1,theta1, angle2,theta2, angle3,theta3 );
+
+ return test;
+}
+
+
+//-------------------------------------------------------------------
+bool testInverseRotation1Angle( Real angle, Real theta ) {
+ bool test = true;
+ bool angleInProperRange = ( -SimTK_PI <= angle && angle <= SimTK_PI );
+ if( angleInProperRange )
+ test = fabs( angle - theta ) < 10*SignificantReal;
+ return test;
+}
+
+
+//-------------------------------------------------------------------
+bool testInverseRotation2Angle( Real angle1, Real theta1, Real angle2, Real theta2 ) {
+ bool test = true;
+ bool angle1InProperRange = ( -SimTK_PI <= angle1 && angle1 <= SimTK_PI );
+ bool angle2InProperRange = ( -SimTK_PI <= angle2 && angle2 <= SimTK_PI );
+ if( angle1InProperRange && angle2InProperRange ) {
+ test = test && fabs( angle1 - theta1 ) < 10*SignificantReal;
+ test = test && fabs( angle2 - theta2 ) < 10*SignificantReal;
+ }
+ return test;
+}
+
+
+//-------------------------------------------------------------------
+bool testInverseRotation3AngleTwoAxes( Real angle1, Real theta1, Real angle2, Real theta2, Real angle3, Real theta3 ) {
+ bool test = true;
+ bool angle1InProperRange = ( -SimTK_PI <= angle1 && angle1 <= SimTK_PI );
+ bool angle2InProperRange = ( 0 <= angle2 && angle2 <= SimTK_PI );
+ bool angle3InProperRange = ( -SimTK_PI <= angle3 && angle3 <= SimTK_PI );
+ if( angle1InProperRange && angle2InProperRange && angle3InProperRange ) {
+ test = test && fabs( angle1 - theta1 ) < 10*SignificantReal;
+ test = test && fabs( angle2 - theta2 ) < 10*SignificantReal;
+ test = test && fabs( angle3 - theta3 ) < 10*SignificantReal;
+
+ // Test needs to be modified if near singularity
+ const Real singularity = 0.0;
+ if( test == false && fabs(angle2-singularity) <= SignificantReal ) {
+ const Real angle1PlusAngle3 = angle1 + angle3;
+ const Real theta1PlusTheta3 = theta1 + theta3;
+ bool angleSumInProperRange = ( -SimTK_PI <= angle1PlusAngle3 && angle1PlusAngle3 <= SimTK_PI );
+ if( angleSumInProperRange == false ) test = true;
+ else {
+ test = fabs( angle2 - theta2 ) < 10*SignificantReal;
+ test = test && fabs( angle1PlusAngle3 - theta1PlusTheta3 ) < 10*SignificantReal;
+ }
+ }
+ }
+ return test;
+}
+
+
+//-------------------------------------------------------------------
+bool testInverseRotation3AngleThreeAxes( Real angle1, Real theta1, Real angle2, Real theta2, Real angle3, Real theta3 ) {
+ bool test = true;
+ bool angle1InProperRange = ( -SimTK_PI <= angle1 && angle1 <= SimTK_PI );
+ bool angle2InProperRange = (-0.5*SimTK_PI <= angle2 && angle2 <= 0.5*SimTK_PI );
+ bool angle3InProperRange = ( -SimTK_PI <= angle3 && angle3 <= SimTK_PI );
+ if( angle1InProperRange && angle2InProperRange && angle3InProperRange ) {
+ test = test && fabs( angle1 - theta1 ) < 10*SignificantReal;
+ test = test && fabs( angle2 - theta2 ) < 10*SignificantReal;
+ test = test && fabs( angle3 - theta3 ) < 10*SignificantReal;
+
+ // Test needs to be modified if near singularity
+ const Real singularity = 0.5*SimTK_PI;
+ if( test == false && fabs(angle2-singularity) <= SignificantReal ) {
+ const Real angle1PlusAngle3 = angle1 + angle3;
+ const Real theta1PlusTheta3 = theta1 + theta3;
+ bool angleSumInProperRange = ( -SimTK_PI <= angle1PlusAngle3 && angle1PlusAngle3 <= SimTK_PI );
+ if( angleSumInProperRange == false ) test = true;
+ else {
+ test = fabs( angle2 - theta2 ) < 10*SignificantReal;
+ test = test && fabs( angle1PlusAngle3 - theta1PlusTheta3 ) < 10*SignificantReal;
+ }
+ }
+ }
+ return test;
+}
+
+
+//-------------------------------------------------------------------
+bool testQuaternion( Real e0, Real e1, Real e2, Real e3 )
+{
+ // Construct quaternion and normalize it
+ Quaternion qe0e1e2e3( e0, e1, e2, e3 );
+
+ // Convert quaternion to a Rotation matrix
+ Rotation rotationSpecified( qe0e1e2e3 );
+
+ // Convert Rotation back to quaternion
+ Quaternion qTest = rotationSpecified.convertRotationToQuaternion();
+
+ // Convert quaternion to a Rotation matrix
+ Rotation testRotation; testRotation.setRotationFromQuaternion( qTest );
+
+ // Test to see if they are the same
+ bool test = rotationSpecified.areAllRotationElementsSameToMachinePrecision( testRotation );
+
+ return test;
+}
+
+//-------------------------------------------------------------------
+bool testSetRotationToBodyFixedXYZ() {
+ bool test = true;
+ const Real q0=.123, q1=-.234, q2=.787;
+ Rotation R0, R1, R2;
+ // The general case and special case implementation should produce
+ // the same result.
+ R0.setRotationFromThreeAnglesThreeAxes(
+ BodyRotationSequence, q0, XAxis, q1, YAxis, q2, ZAxis);
+ R1.setRotationToBodyFixedXYZ(Vec3(q0,q1,q2));
+ R2.setRotationToBodyFixedXYZ(
+ Vec3(std::cos(q0),std::cos(q1),std::cos(q2)),
+ Vec3(std::sin(q0),std::sin(q1),std::sin(q2)));
+
+ test = test && R0.areAllRotationElementsSameToMachinePrecision(R1);
+ test = test && R0.areAllRotationElementsSameToMachinePrecision(R2);
+ test = test && R1.areAllRotationElementsSameToMachinePrecision(R2);
+
+ return test;
+}
+
+
+//-------------------------------------------------------------------
+bool exhaustiveTestof1AngleRotation( ) {
+ bool test = true;
+
+ // Range to check angles
+ Real negativeStartAngle = convertDegreesToRadians( -385 );
+ Real positiveStartAngle = convertDegreesToRadians( 385 );
+ Real incrementAngle = convertDegreesToRadians( 0.5 );
+
+ // Test each axis
+ for( int i=0; i<=2; i++ ) {
+ CoordinateAxis axisi = CoordinateAxis::getCoordinateAxis(i);
+ for( Real anglei = negativeStartAngle; anglei < positiveStartAngle; anglei += incrementAngle )
+ test = test && testRotationOneAxis( anglei, axisi );
+ }
+ return test;
+}
+
+
+//-------------------------------------------------------------------
+bool exhaustiveTestof2AngleRotation( ) {
+ bool test = true;
+
+ // Range to check angles
+ Real negativeStartAngle = convertDegreesToRadians( -200 );
+ Real positiveStartAngle = convertDegreesToRadians( 200 );
+ Real incrementAngle = convertDegreesToRadians( 10.0 );
+
+ // Test each axis
+ for( int i=0; i<=2; i++ ) {
+ CoordinateAxis axisi = CoordinateAxis::getCoordinateAxis(i);
+
+ for( int j=0; j<=2; j++ ) {
+ CoordinateAxis axisj = CoordinateAxis::getCoordinateAxis(j);
+
+ for( Real anglei = negativeStartAngle; anglei < positiveStartAngle; anglei += incrementAngle )
+ for( Real anglej = negativeStartAngle; anglej < positiveStartAngle; anglej += incrementAngle ) {
+ test = test && testRotationTwoAxes( BodyRotationSequence, anglei, axisi, anglej, axisj );
+ test = test && testRotationTwoAxes( SpaceRotationSequence, anglei, axisi, anglej, axisj );
+ }
+ }
+ }
+ return test;
+}
+
+
+//-------------------------------------------------------------------
+bool exhaustiveTestof3AngleRotation( ) {
+ bool test = true;
+
+ // Range to check angles
+ Real negativeStartAngle = convertDegreesToRadians( -200 );
+ Real positiveStartAngle = convertDegreesToRadians( 200 );
+ Real incrementAngle = convertDegreesToRadians( 40.0 );
+
+ // Test each axis
+ for( int i=0; i<=2; i++ ) {
+ CoordinateAxis axisi = CoordinateAxis::getCoordinateAxis(i);
+
+ for( int j=0; j<=2; j++ ) {
+ CoordinateAxis axisj = CoordinateAxis::getCoordinateAxis(j);
+
+ for( int k=0; k<=2; k++ ) {
+ CoordinateAxis axisk = CoordinateAxis::getCoordinateAxis(k);
+
+ for( Real anglei = negativeStartAngle; anglei < positiveStartAngle; anglei += incrementAngle )
+ for( Real anglej = negativeStartAngle; anglej < positiveStartAngle; anglej += incrementAngle )
+ for( Real anglek = negativeStartAngle; anglek < positiveStartAngle; anglek += incrementAngle ) {
+ test = test && testRotationThreeAxes( BodyRotationSequence, anglei, axisi, anglej, axisj, anglek, axisk );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, anglei, axisi, anglej, axisj, anglek, axisk );
+ }
+ }
+ }
+ }
+ return test;
+}
+
+
+//-------------------------------------------------------------------
+bool exhaustiveTestof3AngleTwoAxesRotationNearSingularity() {
+ bool test = true;
+
+ // Range to check angles anglei and anglek
+ Real negativeStartAngle = convertDegreesToRadians( -200 );
+ Real positiveStartAngle = convertDegreesToRadians( 200 );
+ Real incrementStartAngle = convertDegreesToRadians( 20.0 );
+
+ // Range to check around singularity
+ Real negativeSinglAngle = convertDegreesToRadians( 0 - 1.0E-11 );
+ Real positiveSinglAngle = convertDegreesToRadians( 0 + 1.0E-11 );
+ Real incrementSinglAngle = convertDegreesToRadians( 1.0E-12 );
+
+ // Test each axis
+ for( int i=0; i<=2; i++ ) {
+ CoordinateAxis axisi = CoordinateAxis::getCoordinateAxis(i);
+
+ for( int j=0; j<=2; j++ ) {
+ CoordinateAxis axisj = CoordinateAxis::getCoordinateAxis(j);
+
+ for( int k=0; k<=2; k++ ) {
+ CoordinateAxis axisk = CoordinateAxis::getCoordinateAxis(k);
+
+ for( Real anglei = negativeStartAngle; anglei < positiveStartAngle; anglei += incrementStartAngle )
+ for( Real anglej = negativeSinglAngle; anglej < positiveSinglAngle; anglej += incrementSinglAngle )
+ for( Real anglek = negativeStartAngle; anglek < positiveStartAngle; anglek += incrementStartAngle ) {
+ test = test && testRotationThreeAxes( BodyRotationSequence, anglei, axisi, anglej, axisj, anglek, axisk );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, anglei, axisi, anglej, axisj, anglek, axisk );
+ }
+ }
+ }
+ }
+ return test;
+}
+
+
+//-------------------------------------------------------------------
+bool exhaustiveTestof3AngleThreeAxesRotationNearSingularity() {
+ bool test = true;
+
+ // Range to check angles anglei and anglek
+ Real negativeStartAngle = convertDegreesToRadians( -200 );
+ Real positiveStartAngle = convertDegreesToRadians( 200 );
+ Real incrementStartAngle = convertDegreesToRadians( 20.0 );
+
+ // Range to check around singularity
+ Real negativeSinglAngle = convertDegreesToRadians( 90 - 1.0E-11 );
+ Real positiveSinglAngle = convertDegreesToRadians( 90 + 1.0E-11 );
+ Real incrementSinglAngle = convertDegreesToRadians( 1.0E-12 );
+
+ // Test each axis
+ for( int i=0; i<=2; i++ ) {
+ CoordinateAxis axisi = CoordinateAxis::getCoordinateAxis(i);
+
+ for( int j=0; j<=2; j++ ) {
+ CoordinateAxis axisj = CoordinateAxis::getCoordinateAxis(j);
+
+ for( int k=0; k<=2; k++ ) {
+ CoordinateAxis axisk = CoordinateAxis::getCoordinateAxis(k);
+
+ for( Real anglei = negativeStartAngle; anglei < positiveStartAngle; anglei += incrementStartAngle )
+ for( Real anglej = negativeSinglAngle; anglej < positiveSinglAngle; anglej += incrementSinglAngle )
+ for( Real anglek = negativeStartAngle; anglek < positiveStartAngle; anglek += incrementStartAngle ) {
+ test = test && testRotationThreeAxes( BodyRotationSequence, anglei, axisi, anglej, axisj, anglek, axisk );
+ test = test && testRotationThreeAxes( SpaceRotationSequence, anglei, axisi, anglej, axisj, anglek, axisk );
+ }
+ }
+ }
+ }
+ return test;
+}
+
+
+//-------------------------------------------------------------------
+bool exhaustiveTestofQuaternions() {
+ bool test = true;
+
+ for( Real e0 = -1; e0 <= 1; e0 += 0.2 )
+ for( Real e1 = -1; e1 <= 1; e1 += 0.2 )
+ for( Real e2 = -1; e2 <= 1; e2 += 0.2 )
+ for( Real e3 = -1; e3 <= 1; e3 += 0.2 )
+ test = test && testQuaternion( e0, e1, e2, e3 );
+
+ return test;
+}
+
+//-------------------------------------------------------------------
+// Here we need to check that the resulting rotation has determinant 1 (a previous bug
+// left it with determinant -1) and that the j'th axis is at least pointing in the
+// direction of the given vj.
+bool testRotationFromTwoGivenAxes( const Vec3& vi, const CoordinateAxis& ai, const Vec3& vj, const CoordinateAxis& aj) {
+ bool test = true;
+
+ // This makes a Rotation with vi as axis i, but axis j will only be in the general direction of vj.
+ const Rotation testRotation(UnitVec3(vi), ai, vj, aj);
+
+ test = test && std::fabs(det(testRotation) - 1) <= SignificantReal;
+
+ test = test && dot(testRotation(ai), vi) > 0;
+ test = test && dot(testRotation(aj), vj) > 0;
+
+ return test;
+}
+
+//-------------------------------------------------------------------
+// Test the tricked-out code used to rotation a symmetric dyadic
+// matrix S_AA=R_AB*S_BB*R_BA.
+bool testReexpressSymMat33() {
+ bool test = true;
+
+ const Rotation R_AB(Test::randRotation());
+ const SymMat33 S_BB(Test::randSymMat<3>());
+ const Mat33 M_BB(S_BB);
+
+ const SymMat33 S_AA = R_AB.reexpressSymMat33(S_BB);
+ const Mat33 M_AA = R_AB*M_BB*~R_AB;
+
+ const Mat33 MS_AA(S_AA);
+ test = test && (MS_AA-M_AA).norm() <= SignificantReal;
+
+ // Now put it back with an InverseRotation.
+ const SymMat33 isS_BB = (~R_AB).reexpressSymMat33(S_AA);
+ test = test && (S_BB-isS_BB).norm() <= SignificantReal;
+
+ const Rotation I; // identity
+ const SymMat33 S_BB_still = I.reexpressSymMat33(S_BB);
+ test = test && (S_BB_still-S_BB).norm() <= SignificantReal;
+
+ // Test symmetric matrix multiply (doesn't belong here).
+ const SymMat33 S1(Test::randSymMat<3>()), S2(Test::randSymMat<3>());
+ const Mat33 M1(S1), M2(S2);
+ const Mat33 S(S1*S2);
+ const Mat33 M(M1*M2);
+ test = test && (S-M).norm() <= SignificantReal;
+
+ const SymMat<3,Complex> SC1(Test::randComplex(),
+ Test::randComplex(), Test::randComplex(),
+ Test::randComplex(), Test::randComplex(), Test::randComplex() );
+ const SymMat<3,Complex> SC2(Test::randComplex(),
+ Test::randComplex(), Test::randComplex(),
+ Test::randComplex(), Test::randComplex(), Test::randComplex() );
+
+ SimTK_TEST_EQ(SC1.elt(1,0), conj(SC1.elt(0,1)));
+ SimTK_TEST_EQ(SC1.elt(2,0), conj(SC1.elt(0,2)));
+ SimTK_TEST_EQ(SC1.elt(1,2), conj(SC1.elt(2,1)));
+
+ const Mat<3,3,Complex> MC1(SC1), MC2(SC2);
+ const Mat<3,3,Complex> SC(SC1*SC2);
+ const Mat<3,3,Complex> MC(MC1*MC2);
+ SimTK_TEST_EQ(SC, MC);
+
+ return test;
+}
diff --git a/SimTKcommon/tests/SFMTTest.cpp b/SimTKcommon/tests/SFMTTest.cpp
new file mode 100644
index 0000000..2711d27
--- /dev/null
+++ b/SimTKcommon/tests/SFMTTest.cpp
@@ -0,0 +1,62 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "../../Random/src/SFMT.h"
+
+#include <cassert>
+#include <iostream>
+using namespace SimTK_SFMT;
+using std::cout;
+using std::endl;
+
+int main() {
+ try {
+ SFMTData* sfmt = createSFMTData();
+
+ // Read off the first five numbers generated by SFMT, and compare them to the expected values provided with the library.
+
+ init_gen_rand(1234, *sfmt);
+ unsigned int expected[] = {3440181298u, 1564997079u, 1510669302u, 2930277156u, 1452439940u};
+ for (int i = 0; i < 5; ++i)
+ assert(gen_rand32(*sfmt) == expected[i]);
+
+ // Make sure that getting numbers a whole array at a time gives the same values.
+
+ int length = get_min_array_size32();
+ if (length < 5)
+ length = 5;
+ uint32_t* values = new uint32_t[length];
+ init_gen_rand(1234, *sfmt);
+ fill_array32(values, length, *sfmt);
+ for (int i = 0; i < 5; ++i)
+ assert(values[i] == expected[i]);
+ deleteSFMTData(sfmt);
+ delete[] values;
+ } catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ return 0;
+}
+
+
diff --git a/SimTKcommon/tests/SpatialAlgebraTest.cpp b/SimTKcommon/tests/SpatialAlgebraTest.cpp
new file mode 100644
index 0000000..6c9250f
--- /dev/null
+++ b/SimTKcommon/tests/SpatialAlgebraTest.cpp
@@ -0,0 +1,188 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "SimTKcommon/Testing.h"
+
+using namespace SimTK;
+
+#include <iostream>
+#include <iomanip>
+using std::cout;
+using std::endl;
+using std::setw;
+
+void testPhiMatrix() {
+ const Vec3 p = Test::randVec3();
+ PhiMatrix phi(p);
+ SpatialVec v1(Test::randSpatialVec());
+ SpatialMat m1(Test::randSpatialMat());
+
+ SimTK_TEST_EQ( phi*v1, SpatialVec(v1[0] + p%v1[1], v1[1]));
+ SimTK_TEST_EQ(~phi*v1, SpatialVec(v1[0], v1[1] - p%v1[0]));
+
+ SimTK_TEST_EQ(phi*v1, phi.toSpatialMat()*v1);
+ SimTK_TEST_EQ(phi*m1, phi.toSpatialMat()*m1);
+
+ SimTK_TEST_EQ(~phi*v1, (~phi).toSpatialMat()*v1);
+ SimTK_TEST_EQ(m1*~phi, m1*(~phi).toSpatialMat());
+}
+
+// TODO: this isn't a real regression test but it does catch
+// compilation problems.
+void testMiscSpatialAlgebra() {
+ SpatialVec v; SpatialRow r; SpatialMat m;
+ Mat33 m33(1,2,3,
+ 4,5,6,
+ 7,8,9);
+ Mat22 m22(10,20,
+ 30,40);
+ cout << endl << "TEST: uninitialized should be NaN in Debug, random crap in Release" << endl;
+ cout << "rawVec=" << v << " rawRow=" << r << " rawMat=" << m;
+
+ cout << "SpatialMat::NRows=" << SpatialMat::NRows
+ << " SpatialMat::ArgDepth=" << CNT<SpatialMat>::ArgDepth
+ << " CNT<SpatialMat>::ArgDepth=" << SpatialMat::ArgDepth
+ << " SpatialRow::ArgDepth=" << SpatialRow::ArgDepth
+ << " SpatialVec::ArgDepth=" << SpatialVec::ArgDepth
+ << " Mat33::ArgDepth=" << Mat33::ArgDepth
+ << " CNT<Real>::ArgDepth=" << CNT<Real>::ArgDepth
+ << endl;
+
+ cout << endl << "TEST: set to 1; element wise for col/row; diagonal for mat" << endl;
+ v=1;r=1;m=1;
+ cout << "v=1:" << v << " r=1:" << r << " m=1:" << m;
+
+ SpatialVec sv( Vec3(1,2,-3), Vec3(.1,-.2,.3) );
+ Vec6& sv6 = Vec6::updAs(&sv[0][0]);
+ cout << "sv (" << &sv << ")=" << sv << endl;
+ cout << "sv6(" << &sv6 << ")=" << sv6 << endl;
+ cout << "sv.normalize() =" << sv.normalize() << endl;
+ cout << "sv6.normalize()=" << sv6.normalize() << endl;
+
+ SpatialVec::TNeg& nsv = sv.updNegate();
+ Vec6::TNeg& nsv6 = Vec6::TNeg::updAs(&nsv[0][0]);
+ cout << "nsv (" << &nsv << ")=" << nsv << endl;
+ cout << "nsv6(" << &nsv6 << ")=" << nsv6 << endl;
+ cout << "nsv.normalize() =" << nsv.normalize() << endl;
+ cout << "nsv6.normalize()=" << nsv6.normalize() << endl;
+
+ sv = sv.normalize();
+ cout << "after sv=sv.normalize(), sv=" << sv << endl;
+ cout << "now nsv+1=" << nsv+1 << ", (nsv+1).normalize()=" << (nsv+1).normalize() << endl;
+ nsv = (nsv+1).normalize();
+ cout << "after nsv=(nsv+1).normalize(), nsv=" << nsv << endl;
+
+ CNT<Vec3>::Result<double>::Mul xxx;
+
+ cout << endl << "TEST: v+v=" << v+v;
+ cout << endl << "TEST: v*3.=" << v*3.;
+ //cout << endl << "TEST: 3./v=" << 3./v;
+
+
+ cout << endl << "TEST: 3.f*v=" << 3.f*v;
+ cout << endl << "TEST: 3*v*2=" << 3*v*2;
+ cout << endl << "TEST: v/negator<Real>(3.)=" << v/negator<Real>(3.);
+ cout << endl << "TEST: v/complex<Real>(2.,3.)=" << v/complex<Real>(2.,3.);
+ cout << endl << "TEST: v/conjugate<Real>(2.,3.)=" << v/conjugate<Real>(2.,3.);
+ const conjugate<Real> cr23(2.,3.);
+ cout << endl << "TEST: v*negator<conjugate<Real>(2.,3.)>="
+ << v*negator<conjugate<Real> >::recast(cr23);
+ cout << endl << "TEST: v/negator<conjugate<Real>(2.,3.)>="
+ << v/negator<conjugate<Real> >::recast(cr23);
+ //cout << endl << "TEST: v-v=" << v-v;
+ //cout << endl << "TEST: r+r=" << r+r;
+ //cout << endl << "TEST: r-r=" << r-r;
+ //cout << endl << "TEST: m+m=" << m+m;
+ //cout << endl << "TEST: m-m=" << m-m;
+
+ // CONFORMING MULTIPLY
+
+ cout << endl << "TEST: multiply by identity matrix" << endl;
+ cout << "m*v:" << m*v << endl;
+ cout << " r*m:" << r*m << endl;
+ cout << " m*m:" << m*m;
+ cout << " m*m22:" << m*m22;
+
+ cout << endl << "TEST: scalar multiply, left and right" << endl;
+ cout << "v*9=" << v*9 << " 2*v=" << 2*v << endl;
+ cout << "r*9=" << r*9 << " 2*r=" << 2*r << endl;
+ cout << "m*9=" << m*9 << " 2*m=" << 2*m << endl;
+
+ cout << endl << "TEST: dot product" << endl;
+ cout << "r*v=" << r*v << " ~v*~r=" << ~v*~r << endl;
+
+ cout << endl << "TEST: outer product" << endl;
+ cout << "v*r=" << v*r << endl;
+ cout << " ~r*~v=" << ~r*~v << endl;
+
+ // NONCONFORMING MULTIPLY
+
+ cout << endl << "TEST: m*m33=" << m * m33 << " ~m*m33=" << ~m*m33;
+ cout << endl << "TEST: m33*m=" << m33 * m;
+ cout << endl << "TEST: r*m33=" << r * m33 << " ~v*m33=" << ~v*m33;
+ cout << endl << "TEST: m33*v=" << m33 * v << endl << endl;
+
+ Row<2, Mat33> m1233(Mat33(2), Mat33(3));
+ Vec3 v310(10);
+ SpatialRow sr = ~v310 * m1233;
+ cout << "v310=" << v310 << " m1233=" << m1233;
+ cout << "~v310*m1233=" << sr << endl;
+
+ cout << endl << "TEST: element multiply by non-scalar" << endl;
+ Row<2> rnew = r * Vec3(1,2,3);
+ cout << "r*vec(1,2,3)=" << rnew << endl;
+
+ // SYMMAT
+ SymMat<3> sy3 = SymMat<3>().setFromLower(Mat<3,3>(2, 99, 99,
+ 3, 4, 99,
+ 5, 6, 7));
+ SymMat<3> sy3d(-5);
+ cout << "sy3=" << sy3 << " sy3d=" << sy3d;
+ cout << "sy3+sy3d=" << sy3+sy3d;
+
+ // TRICKY CASE: 1x1 matrix, 1-row, 1-vec
+ Mat<1, 2, Row3> m11(Row3(1,2,3), Row3(4,5,6));
+ Row<1> r1(-.1);
+ Vec<2, Real> v1(10,20);
+ cout << "r1=" << r1 << " v1=" << v1 << " m11=" << m11;
+ cout << "m11*v1=" << m11*v1 << endl;
+ cout << "r1*m11=" << r1*m11 << endl;;
+ //cout << "r2*v1=" << r1*v1 << endl;
+ //cout << "v1*r1=" << v1*r1 << endl;;
+
+
+ MassProperties mprops(23, Vec3(1,2,3), UnitInertia::brick(.1,.2,.3));
+ cout << "MassProperties: " << mprops;
+ cout << "MassProperties.toSpatialMat: " << mprops.toSpatialMat();
+ cout << "MassProperties.toMat66: " << mprops.toMat66();
+}
+
+
+int main()
+{
+ SimTK_START_TEST("SpatialAlgebraTest");
+ SimTK_SUBTEST(testPhiMatrix);
+ SimTK_SUBTEST(testMiscSpatialAlgebra);
+ SimTK_END_TEST();
+}
diff --git a/SimTKcommon/tests/StateTest.cpp b/SimTKcommon/tests/StateTest.cpp
new file mode 100644
index 0000000..d0ff773
--- /dev/null
+++ b/SimTKcommon/tests/StateTest.cpp
@@ -0,0 +1,257 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Run the State class few some paces.
+ */
+
+#include "SimTKcommon.h"
+#include "SimTKcommon/Testing.h"
+
+#include <string>
+#include <iostream>
+#include <exception>
+#include <cmath>
+using std::cout;
+using std::endl;
+
+using namespace SimTK;
+
+
+
+
+//void testLowestModified() {
+// const SubsystemIndex Sub0(0), Sub1(1);
+// State s;
+// s.setNumSubsystems(2);
+// SimTK_TEST(s.getSystemStage()==Stage::Empty);
+// SimTK_TEST(s.getSubsystemStage(Sub0)==Stage::Empty && s.getSubsystemStage(Sub1)==Stage::Empty);
+// SimTK_TEST(s.getLowestStageModified()==Stage::Topology);
+//
+// const DiscreteVariableIndex dvxModel = s.allocateDiscreteVariable(Sub1, Stage::Model, new Value<Real>(2));
+//
+// // "realize" Topology stage
+// s.advanceSubsystemToStage(Sub0, Stage::Topology);
+// s.advanceSubsystemToStage(Sub1, Stage::Topology);
+// s.advanceSystemToStage(Stage::Topology);
+// SimTK_TEST(s.getLowestStageModified()==Stage::Topology); // shouldn't have changed
+//
+// const DiscreteVariableIndex dvxInstance = s.allocateDiscreteVariable(Sub0, Stage::Instance, new Value<int>(-4));
+//
+// // A Model-stage variable must be allocated before Topology is realized, and this condition
+// // should be tested even in Release mode.
+// try {
+// s.allocateDiscreteVariable(Sub0, Stage::Model, new Value<int>(0));
+// SimTK_TEST(!"Shouldn't have allowed allocation of Model-stage var here");
+// } catch (...) {}
+//
+// // "realize" Model stage
+// s.advanceSubsystemToStage(Sub0, Stage::Model);
+// s.advanceSubsystemToStage(Sub1, Stage::Model);
+// s.advanceSystemToStage(Stage::Model);
+// SimTK_TEST(s.getSystemStage() == Stage::Model);
+//
+// SimTK_TEST(s.getLowestStageModified()==Stage::Topology); // shouldn't have changed
+// s.resetLowestStageModified();
+// SimTK_TEST(s.getLowestStageModified()==Stage::Instance); // i.e., lowest invalid stage
+//
+// // This variable invalidates Instance stage, so shouldn't change anything now.
+// SimTK_TEST(Value<int>::downcast(s.getDiscreteVariable(Sub0, dvxInstance))==-4);
+// Value<int>::downcast(s.updDiscreteVariable(Sub0, dvxInstance)) = 123;
+// SimTK_TEST(Value<int>::downcast(s.getDiscreteVariable(Sub0, dvxInstance))== 123);
+//
+// SimTK_TEST(s.getSystemStage()==Stage::Model);
+// SimTK_TEST(s.getLowestStageModified()==Stage::Instance);
+//
+// // This variable invalidates Model Stage, so should back up the stage to Topology,
+// // invalidating Model.
+// Value<Real>::downcast(s.updDiscreteVariable(Sub1, dvxModel)) = 29;
+// SimTK_TEST(s.getSubsystemStage(Sub1)==Stage::Topology);
+// SimTK_TEST(s.getLowestStageModified()==Stage::Model);
+//
+// // Now realize Model stage again; shouldn't affect lowestStageModified.
+// s.advanceSubsystemToStage(Sub0, Stage::Model);
+// s.advanceSubsystemToStage(Sub1, Stage::Model);
+// s.advanceSystemToStage(Stage::Model);
+// SimTK_TEST(s.getSystemStage() == Stage::Model);
+//
+// SimTK_TEST(s.getLowestStageModified()==Stage::Model); // shouldn't have changed
+//}
+
+void testCacheValidity() {
+ const SubsystemIndex Sub0(0), Sub1(1);
+ State s;
+ s.setNumSubsystems(2);
+
+ // Allocate a Model stage-invalidating state variable.
+ const DiscreteVariableIndex dvxModel = s.allocateDiscreteVariable(Sub1, Stage::Model, new Value<Real>(2));
+
+ // This cache entry depends on Model stage state and is guaranteed to be valid at Time stage.
+ // In between (at Model or Instance stage) it *may* be valid if explicitly marked so.
+ const CacheEntryIndex cx = s.allocateCacheEntry(Sub0,
+ Stage::Model, Stage::Time, new Value<int>(41));
+
+ // "realize" Topology stage
+ s.advanceSubsystemToStage(Sub0, Stage::Topology);
+ s.advanceSubsystemToStage(Sub1, Stage::Topology);
+ s.advanceSystemToStage(Stage::Topology);
+
+ // Shouldn't be able to access cache entry here because this is less than its
+ // "depends on" stage.
+ SimTK_TEST_MUST_THROW(s.getCacheEntry(Sub0, cx));
+
+
+ // "realize" Model stage
+ s.advanceSubsystemToStage(Sub0, Stage::Model);
+ s.advanceSubsystemToStage(Sub1, Stage::Model);
+ s.advanceSystemToStage(Stage::Model);
+
+ // "realize" Instance stage
+ s.advanceSubsystemToStage(Sub0, Stage::Instance);
+ s.advanceSubsystemToStage(Sub1, Stage::Instance);
+ s.advanceSystemToStage(Stage::Instance);
+
+ // Although the cache entry *could* be valid at this point,
+ // no one has said so, so we expect it to throw.
+ SimTK_TEST_MUST_THROW(s.getCacheEntry(Sub0, cx));
+
+ // If we say it is valid, we should be able to obtain its value.
+ s.markCacheValueRealized(Sub0, cx);
+ SimTK_TEST(Value<int>::downcast(s.getCacheEntry(Sub0, cx)) == 41);
+
+ // Now modify a Model-stage state variable and realize again. This
+ // shoudl have invalidated the cache entry.
+ Value<Real>::updDowncast(s.updDiscreteVariable(Sub1, dvxModel)) = 9;
+ // "realize" Model stage
+ s.advanceSubsystemToStage(Sub0, Stage::Model);
+ s.advanceSubsystemToStage(Sub1, Stage::Model);
+ s.advanceSystemToStage(Stage::Model);
+
+ SimTK_TEST(!s.isCacheValueRealized(Sub0, cx));
+
+ SimTK_TEST_MUST_THROW(s.getCacheEntry(Sub0, cx));
+
+ // "calculate" the cache entry and mark it valid.
+ Value<int>::updDowncast(s.updCacheEntry(Sub0,cx)) =
+ (int)(2 * Value<Real>::downcast(s.getDiscreteVariable(Sub1, dvxModel)));
+ s.markCacheValueRealized(Sub0, cx);
+
+ SimTK_TEST(Value<int>::downcast(s.getCacheEntry(Sub0, cx)) == 18);
+
+ // Now modify the Model-stage variable again, but realize through
+ // Time stage. We should be able to access the cache entry without
+ // explicitly marking it valid.
+ Value<Real>::updDowncast(s.updDiscreteVariable(Sub1, dvxModel)) = -100;
+ s.advanceSubsystemToStage(Sub0, Stage::Model);
+ s.advanceSubsystemToStage(Sub1, Stage::Model);
+ s.advanceSystemToStage(Stage::Model);
+ s.advanceSubsystemToStage(Sub0, Stage::Instance);
+ s.advanceSubsystemToStage(Sub1, Stage::Instance);
+ s.advanceSystemToStage(Stage::Instance);
+ s.advanceSubsystemToStage(Sub0, Stage::Time);
+ s.advanceSubsystemToStage(Sub1, Stage::Time);
+ s.advanceSystemToStage(Stage::Time);
+
+ SimTK_TEST(Value<int>::downcast(s.getCacheEntry(Sub0, cx)) == 18);
+
+}
+
+void testMisc() {
+ State s;
+ s.setNumSubsystems(1);
+ s.advanceSubsystemToStage(SubsystemIndex(0), Stage::Topology);
+
+ // Can't ask for the time before Stage::Topology, but if you could it would be NaN.
+ s.advanceSystemToStage(Stage::Topology);
+
+ // Advancing to Stage::Topology sets t=0.
+ cout << "AFTER ADVANCE TO TOPOLOGY, t=" << s.getTime() << endl;
+
+ SimTK_TEST(s.getTime()==0);
+
+ Vector v3(3), v2(2);
+ QIndex q1 = s.allocateQ(SubsystemIndex(0), v3);
+ QIndex q2 = s.allocateQ(SubsystemIndex(0), v2);
+
+ EventTriggerByStageIndex e1 = s.allocateEventTrigger(SubsystemIndex(0), Stage::Position, 3);
+ EventTriggerByStageIndex e2 = s.allocateEventTrigger(SubsystemIndex(0), Stage::Instance, 2);
+
+ printf("q1,2=%d,%d\n", (int)q1, (int)q2);
+ printf("e1,2=%d,%d\n", (int)e1, (int)e2);
+
+ //cout << s;
+
+ DiscreteVariableIndex dv = s.allocateDiscreteVariable(SubsystemIndex(0), Stage::Dynamics, new Value<int>(5));
+
+ s.advanceSubsystemToStage(SubsystemIndex(0), Stage::Model);
+ //long dv2 = s.allocateDiscreteVariable(SubsystemIndex(0), Stage::Position, new Value<int>(5));
+
+ Value<int>::downcast(s.updDiscreteVariable(SubsystemIndex(0), dv)) = 71;
+ cout << s.getDiscreteVariable(SubsystemIndex(0), dv) << endl;
+
+
+ s.advanceSystemToStage(Stage::Model);
+
+ cout << "AFTER ADVANCE TO MODEL, t=" << s.getTime() << endl;
+
+ // Event triggers are available at Instance stage.
+ s.advanceSubsystemToStage(SubsystemIndex(0), Stage::Instance);
+ s.advanceSystemToStage(Stage::Instance);
+
+ printf("ntriggers=%d, by stage:\n", s.getNEventTriggers());
+ for (int j=0; j<Stage::NValid; ++j) {
+ Stage g = Stage(j);
+ cout << g.getName() << ": " << s.getNEventTriggersByStage(g) << endl;
+ }
+
+ printf("subsys 0 by stage:\n");
+ for (int j=0; j<Stage::NValid; ++j) {
+ Stage g = Stage(j);
+ cout << g.getName() << ": " << s.getNEventTriggersByStage(SubsystemIndex(0),g) << endl;
+ }
+ //cout << "State s=" << s;
+
+ s.clear();
+ //cout << "after clear(), State s=" << s;
+}
+
+int main() {
+ int major,minor,build;
+ char out[100];
+ const char* keylist[] = { "version", "library", "type", "debug", "authors", "copyright", "svn_revision", 0 };
+
+ SimTK_version_SimTKcommon(&major,&minor,&build);
+ std::printf("==> SimTKcommon library version: %d.%d.%d\n", major, minor, build);
+ std::printf(" SimTK_about_SimTKcommon():\n");
+ for (const char** p = keylist; *p; ++p) {
+ SimTK_about_SimTKcommon(*p, 100, out);
+ std::printf(" about(%s)='%s'\n", *p, out);
+ }
+
+
+ SimTK_START_TEST("StateTest");
+ //SimTK_SUBTEST(testLowestModified);
+ SimTK_SUBTEST(testCacheValidity);
+ SimTK_SUBTEST(testMisc);
+ SimTK_END_TEST();
+}
diff --git a/SimTKcommon/tests/TestArray.cpp b/SimTKcommon/tests/TestArray.cpp
new file mode 100644
index 0000000..10bef98
--- /dev/null
+++ b/SimTKcommon/tests/TestArray.cpp
@@ -0,0 +1,868 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/*
+ * These are regression tests for the SimTK::Array_<T,X> class.
+ */
+
+#include "SimTKcommon.h"
+#include "SimTKcommon/Testing.h"
+
+#include <vector>
+#include <sstream>
+#include <iterator>
+#include <iostream>
+using std::cout;
+using std::endl;
+using std::cin;
+
+
+using namespace SimTK;
+
+// Output an std::vector<T>
+template <class T>
+std::ostream& operator<<(std::ostream& o, std::vector<T>& v) {
+ o << '<';
+ if (!v.empty()) {
+ o << v.front();
+ for (unsigned i=1; i < v.size(); ++i)
+ o << ' ' << v[i];
+ }
+ return o << '>';
+}
+
+//// Input an Array_<T>
+//template <class T, class X> inline std::istream&
+//operator>>(std::istream& i, Array_<T,X>& a) {
+// if (a.isOwner()) {
+// a.clear();
+// while(!i.eof()) {
+// a.push_back(); // default construct new element
+// i >> a.back(); // input last element
+// }
+// } else { // non-owner
+// typedef typename Array_<T,X>::size_type size_type;
+// for (size_type e(0); e < a.size() && !i.eof(); ++e)
+// i >> a[e];
+// }
+// return i;
+//}
+//template <class T, class X> inline std::istream&
+//operator>>(std::istream& i, ArrayView_<T,X>& a)
+//{ return i >> (Array_<T,X>&)a; }
+
+template <class T>
+class OtherArray_ : public Array_<T> {
+public:
+ typedef typename Array_<T>::size_type size_type;
+
+ OtherArray_() : Array_<T>() {}
+ OtherArray_(size_type n, const T& v) : Array_<T>(n,v) {}
+};
+
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(TestIx);
+
+// This index type has a max size of 4 for testing out-of-space
+// checks.
+class SmallIx {
+public:
+ SmallIx() : ix(0xff) {}
+ explicit SmallIx(unsigned char i) : ix(i) {}
+
+ SmallIx& operator++()
+ { assert(ix<max_size()); ++ix; return *this;}
+ SmallIx operator++(int)
+ { assert(ix<max_size()); const SmallIx x=*this; ++ix; return x;}
+ SmallIx& operator--()
+ { assert(ix>0); --ix; return *this;}
+ SmallIx operator--(int)
+ { assert(ix>0); const SmallIx x=*this; ++ix; return x;}
+
+ // These are required for any class to be used an index type.
+ operator unsigned char() const {return ix;}
+ typedef unsigned char size_type;
+ typedef signed char difference_type;
+ static size_type max_size() {return 4;}
+private:
+ unsigned char ix;
+};
+
+namespace SimTK {
+template <> struct NiceTypeName<SmallIx> {
+ static const char* name() {return "SmallIx";}
+};
+}
+
+class Counter {
+public:
+ Counter() : count(0) {}
+ Counter& operator=(int i) {count=i; return *this;}
+ Counter& operator++() {++count; return *this;}
+ Counter operator++(int) {const Counter c=*this; ++count; return c;}
+ Counter& reset() {count=0; return *this;}
+ operator int() const {return count;}
+private:
+ mutable int count;
+};
+inline std::ostream&
+operator<<(std::ostream& o, const Counter& c) {
+ return o << (int)c;
+}
+
+// This class is a T but augmented with counters that track the number
+// of calls to constructors, assignment, and the destructor.
+template <class T>
+struct Count {
+ Count() {++defCtor;}
+ Count(const Count& c) : val(c.val) {++copyCtor;}
+ Count& operator=(const Count& c) {val=c.val; ++copyAssign; return *this;}
+ ~Count() {++dtor;}
+
+ // Conversion from T.
+ Count(const T& t) : val(t) {++initCtor;}
+ // Assign from T.
+ Count& operator=(const T& t) {val=t; ++initAssign; return *this;}
+
+
+ bool operator==(const Count& other) const {return val==other.val;}
+ bool operator!=(const Count& other) const {return val!=other.val;}
+
+ static void dumpCounts(const char* msg) {
+ cout << msg << ":";
+ cout << " defCtor=" << Count<int>::defCtor;
+ cout << " initCtor=" << Count<int>::initCtor;
+ cout << " copyCtor=" << Count<int>::copyCtor;
+ cout << " initAssign=" << Count<int>::initAssign;
+ cout << " copyAssign=" << Count<int>::copyAssign;
+ cout << " dtor=" << Count<int>::dtor;
+ cout << endl;
+ }
+
+ static bool isReset()
+ { return !(defCtor||initCtor||copyCtor||initAssign||copyAssign||dtor); }
+
+ T val;
+
+ static void reset() {defCtor=initCtor=copyCtor=initAssign=copyAssign=dtor=0;}
+ static Counter defCtor;
+ static Counter initCtor;
+ static Counter copyCtor;
+ static Counter initAssign;
+ static Counter copyAssign;
+ static Counter dtor;
+};
+template <class T> inline std::ostream&
+operator<<(std::ostream& o, const Count<T>& c) {
+ return o << c.val;
+}
+template <class T> Counter Count<T>::defCtor;
+template <class T> Counter Count<T>::initCtor;
+template <class T> Counter Count<T>::copyCtor;
+template <class T> Counter Count<T>::initAssign;
+template <class T> Counter Count<T>::copyAssign;
+template <class T> Counter Count<T>::dtor;
+
+// Instantiate the whole class to check for compilation problems.
+namespace SimTK {
+template class Array_<int>;
+template class Array_<std::string, unsigned char>;
+};
+
+#ifdef _MSC_VER // gcc 4.1.2 had trouble with this
+// Instantiate templatized methods
+typedef std::set<float>::const_iterator inputIt; // not a random access iterator
+
+// Constructors.
+template Array_<float,int>::Array_(const Array_<float,int>&);
+template Array_<float,int>::Array_(const float*,const float*);
+
+// Assignment.
+template void
+Array_<float,int>::assign(const float*,const float*);
+template void
+Array_<double,int>::assign(const inputIt&, const inputIt&);
+template Array_<float,int>&
+Array_<float,int>::operator=(const Array_<float,int>&);
+template Array_<double,int>&
+Array_<double,int>::operator=(const std::vector<float>&);
+
+// Insertion
+template float*
+Array_<float,int>::insert(float*, const float*, const float*);
+template float*
+Array_<float,short>::insert(float*, const inputIt&, const inputIt&);
+
+// Comparison
+template bool SimTK::operator==(const ArrayViewConst_<float,int>&,
+ const ArrayViewConst_<float,unsigned>&);
+#endif
+
+
+void testConstruction() {
+ const int data[] = {5,3,-2,27,9};
+ const char uchar[] = {'f','i','t','z'};
+ Array_<int> nothing;
+ Array_<int> def(5);
+ Array_<int> intWithInt(data, data+5);
+ Array_<char> charWithChar(uchar, uchar+4);
+ Array_<int> intWithChar(uchar, uchar+4);
+ Array_<char> charWithInt(data, data+5);
+ cout << "nothing=" << nothing << endl;
+ cout << "def=" << def << endl;
+ cout << "intWithInt=" << intWithInt << endl;
+ cout << "charWithChar=" << charWithChar << endl;
+ cout << "intWithChar=" << intWithChar << endl;
+ cout << "charWithInt=" << charWithInt << endl;
+
+ Array_< Count<int> > cint(data, data+5);
+ Count<int>::dumpCounts("cint(data,data+5)");
+ Count<int>::reset();
+
+ const Count<int> counts[] = {3,4,5};
+ Count<int>::reset();
+ Array_< Count<int> > ccnt(counts, counts+3);
+ Count<int>::dumpCounts("ccnt(counts,counts+3)");
+ Count<int>::reset();
+
+ Array_< Count<int> > cint2(cint);
+ Count<int>::dumpCounts("cint2(cint)");
+ Count<int>::reset();
+
+ cint2 = ccnt;
+ Count<int>::dumpCounts("cint2=ccnt");
+ Count<int>::reset();
+ cout << "cint2=" << cint2 << endl;
+
+ Array_<int,SmallIx> ismall0;
+ cout << "default constructed Array_<int> begin()=" << ismall0.begin()
+ << " end()=" << ismall0.end()
+ << " capacity()=" << (int)ismall0.capacity()
+ << endl;
+
+ std::vector<int> ivec0;
+ cout << "default constructed std::vector<int>"
+ << " capacity()=" << ivec0.capacity()
+ << endl;
+
+ Array_<int,SmallIx> ismall(3);
+ Array_<int,SmallIx> imaxsz(data, data+4);
+ cout << "ismall0=" << ismall0 << endl;
+ cout << "ismall=" << ismall << endl;
+ cout << "imaxsz=" << imaxsz << endl;
+
+ new(ismall.raw_push_back()) int(27);
+ cout << "ismall after raw_push_back():" << ismall << endl;
+
+ SimTK_TEST_MUST_THROW_DEBUG(imaxsz.push_back()); // already full
+
+ // Check null assignments.
+ ismall = ismall0; // src is null
+ ismall0 = imaxsz; // dest was null
+ ismall = Array_<int,SmallIx>(); // both null
+
+ cout << "sizeof(Array_<int,bool>)=" << sizeof(Array_<int,bool>) << endl;
+ cout << "sizeof(Array_<int,char>)=" << sizeof(Array_<int,unsigned char>) << endl;
+ cout << "sizeof(Array_<int,short>)=" << sizeof(Array_<int,unsigned short>) << endl;
+ cout << "sizeof(Array_<int>)=" << sizeof(Array_<int>) << endl;
+ cout << "sizeof(std::vector<int>)=" << sizeof(std::vector<int>) << endl;
+ cout << "sizeof(Array_<int,long long>)=" << sizeof(Array_<int,long long>) << endl;
+
+ Array_<String, TestIx> strings(6, "woohoo");
+ cout << "strings=" << strings << endl;
+ strings.push_back("last");
+ for (int i=0; i<5; ++i) {
+ strings.insert(strings.end(), 2, "ins" + String(i));
+ cout << strings.size() << ":" << strings.capacity()
+ << ":" << strings << endl;
+ }
+ cout << "strings=" << strings << endl;
+
+ Array_<String, TestIx>::reverse_iterator p = strings.rbegin();
+ while (p != strings.rend())
+ cout << " " << *p++;
+ cout << endl;
+
+ const int ownerData[] = {7, 77, 777, 7777, 77777};
+ std::vector<int> owner(ownerData, ownerData+5);
+ std::vector<unsigned> unowner(owner.begin(), owner.end());
+ Array_<int> shared; shared.shareData(&owner[1], &owner[4]);
+ cout << "vector before=" << owner << endl;
+ cout << "shared before=" << shared << endl;
+ shared[2] = 29;
+ cout << "shared after=" << shared << endl;
+ cout << "vector after=" << owner << endl;
+ cout << "shared(1,2)=" << shared(1,2) << endl;
+
+ Array_<int> copyOfOwner(owner);
+ cout << "copyOfOwner=" << copyOfOwner << endl;
+ Array_<unsigned short,char> weirdCopy(owner);
+ cout << "weirdCopy=" << weirdCopy << endl;
+ copyOfOwner = unowner;
+ cout << "copyOfOwner=unowner=" << copyOfOwner << endl;
+
+ Array_<unsigned> shareOfUnowner(unowner, DontCopy());
+ cout << "shareOfUnowner=" << shareOfUnowner << endl;
+
+ shareOfUnowner(1,3) = Array_<unsigned>(3,88);
+ cout << "shareOfUnowner=" << shareOfUnowner << endl;
+
+ OtherArray_<int> oa(5, -4);
+ cout << "oa=" << oa << endl;
+}
+
+static void toArray(const Array_<int>& a) {
+ cout << "toArray= " << a << " &a[0]=" << &a[0] << endl;
+}
+static void toArrayView(const ArrayView_<int>& av) {
+ cout << "toArrayView= " << av << " &av[0]=" << &av[0] << endl;
+}
+static void toArrayViewConst(const ArrayViewConst_<int>& ca) {
+ cout << "toArrayViewConst=" << ca << " &ca[0]=" << &ca[0] << endl;
+}
+void testConversion() {
+ const int p[] = {1,2,3,4,5,6};
+ std::vector<int> v(p,p+6);
+ cout << "v=" << v << " &v[0]=" << &v[0] << endl;
+ Array_<int> a(v);
+ ArrayView_<int> av(v);
+ ArrayViewConst_<int> ca(v);
+ cout << "a= " << a << " &a[0]=" << &a[0] << endl;
+ cout << "av=" << av << " &av[0]=" << &av[0] << endl;
+ cout << "ca=" << ca << " &ca[0]=" << &ca[0] << endl;
+
+ toArray(ArrayView_<int>(v));
+ toArrayView(v);
+ toArrayViewConst(v);
+}
+
+// ArrayView assignment can't change the size of the target ArrayView,
+// and the semantics are elementwise assignment, not destruct-then-copy-
+// construct as for resizeable Array (or std::vector) assignment.
+void testArrayViewAssignment() {
+ const int data[5] = {10, 100, -23, 4, -99};
+ Array_<int> adata(data, data+5); // copies of original data
+ std::vector<int> vdata(data, data+5);
+
+ Count<int>::reset();
+ Array_< Count<int> > acnt(data, data+5);
+ SimTK_TEST(!( Count<int>::defCtor ||Count<int>::copyCtor
+ ||Count<int>::copyAssign||Count<int>::dtor));
+ SimTK_TEST(Count<int>::initCtor == 5);
+
+ Count<int>::reset();
+ acnt = adata; // clear() then construct from int
+ SimTK_TEST(!( Count<int>::defCtor ||Count<int>::copyCtor
+ ||Count<int>::copyAssign));
+ SimTK_TEST(Count<int>::dtor==5 && Count<int>::initCtor==5);
+
+ Count<int>::reset();
+ Array_< Count<int> > acopy(3); // default constructed
+ SimTK_TEST(Count<int>::defCtor == 3);
+ acopy = acnt; // destruct 3, copy construct 5
+ SimTK_TEST(Count<int>::dtor==3 && Count<int>::copyCtor==5);
+
+ Count<int>::reset();
+ // this is an initialization, not an assignment
+ ArrayView_< Count<int> > avcnt = acnt(1,2); // shares 2nd & 3rd elts
+ SimTK_TEST(Count<int>::isReset()); // nothing should have happened
+ SimTK_TEST(avcnt.size()==2);
+ SimTK_TEST(avcnt[0]==acnt[1]&&avcnt[1]==acnt[2]);
+ SimTK_TEST(&avcnt[0]==&acnt[1]&& &avcnt[1]==&acnt[2]);
+
+ // This assignment should fail because the source has too many elements.
+ SimTK_TEST_MUST_THROW(avcnt = adata);
+ // This one should succeed, with 2 calls to Count<int>::op=(int).
+ Count<int>::reset();
+ avcnt.assign(adata.begin(), adata.begin()+2);
+ SimTK_TEST(!(Count<int>::defCtor||Count<int>::copyCtor
+ ||Count<int>::copyAssign));
+ SimTK_TEST(Count<int>::initAssign == 2);
+
+ // This assignment should fail because of overlap between source and
+ // destination.
+ SimTK_TEST_MUST_THROW(avcnt = acnt(0,2));
+ // But this succeeds because no overlap.
+ Count<int>::reset();
+ avcnt = acnt(3,2); // sets acnt(1,2)=acnt(3,2) with 2 copy assigns
+ SimTK_TEST(!(Count<int>::copyCtor||Count<int>::dtor));
+ SimTK_TEST(Count<int>::copyAssign == 2);
+ //was: int data[5] = {10, 100, -23, 4, -99};
+ int modified[5] = {10, 4, -99, 4, -99}; // should now be
+ SimTK_TEST(avcnt[0]==4&&avcnt[1]==-99);
+ SimTK_TEST(acnt == Array_<int>(modified, modified+5));
+
+ // Check behavior of assign(first,last1) for input, forward, and
+ // random access iterators.
+ int someSpace[5] = {123, 1, 12, -9, 14};
+ ArrayView_<int> avSpace(someSpace, someSpace+5);
+ std::vector<int> aVec(avSpace.begin(), avSpace.end()); // copy
+ std::set<int> aSet(avSpace.begin(), avSpace.end()); // copy & sort
+ SimTK_TEST(&avSpace[0] == someSpace); //must be sharing space
+ // Test fill first.
+ avSpace = 19; SimTK_TEST(avSpace==Array_<int>(5, 19));
+ avSpace.fill(-3); SimTK_TEST(avSpace==Array_<int>(5, -3));
+ SimTK_TEST_MUST_THROW_DEBUG(avSpace.assign(12, 999));
+ avSpace.assign(5,999); SimTK_TEST(avSpace==Array_<int>(5,999));
+ // Assign from pointers
+ avSpace.assign(data, data+5); SimTK_TEST(avSpace==vdata);
+ avSpace = 999;
+ // Assign from random_access_iterators
+ avSpace.assign(vdata.begin(),vdata.end());SimTK_TEST(avSpace==vdata);
+ avSpace = 999;
+ // Assign from bidirectional_interator
+ avSpace.assign(aSet.begin(), aSet.end());
+ SimTK_TEST(avSpace==std::vector<int>(aSet.begin(),aSet.end()));
+
+ SimTK_TEST_MUST_THROW(avSpace.assign(data, data+3));
+ SimTK_TEST_MUST_THROW(avSpace.assign(vdata.begin(), vdata.begin()+3));
+ std::set<int>::iterator sp = aSet.begin(); ++sp; ++sp;
+ SimTK_TEST_MUST_THROW(avSpace.assign(aSet.begin(), sp));
+
+ //TODO: test input iterators, and source too big problems
+}
+
+void testInsert() {
+ const int data1[3] = {7, -2, 3};
+ const int data2[4] = {101, 121, -111, 321};
+ int wdata[3] = {99, 9999, 999}; // writable
+
+ Array_<int> a(data2,data2+4); // copy the data
+ SimTK_TEST(&a[0] != data2);
+ ArrayViewConst_<int> avc(data2, data2+4); // share the data
+ SimTK_TEST(&avc[1] == data2+1);
+
+ Array_<int> aw(wdata, wdata+3, DontCopy()); // shared
+ SimTK_TEST(&aw[0] == wdata);
+
+ // Can't insert into non-owner.
+ SimTK_TEST_MUST_THROW(aw.insert(aw.begin(), avc.begin(), avc.end()));
+ // Unless we're inserting zero elements; that's allowed.
+ aw.insert(&aw[1], avc.begin(), avc.begin());
+
+ Array_<int> ac(data1, data1+3);
+ std::vector<int> vc(data1, data1+3);
+ ac.insert(&ac[1], &a[1], &a[1]+2);
+ vc.insert(vc.begin()+1, &a[1], &a[1]+2);
+ SimTK_TEST(ac.size()==5);
+ SimTK_TEST(ac == vc); // 7, 121, -111, -2, 3
+
+ // insert vc onto beginning and end of ac
+ ac.insert(ac.begin(), vc.begin(), vc.end());
+ ac.insert(ac.end(), vc.begin(), vc.end());
+ SimTK_TEST(ac.size()==15);
+ SimTK_TEST(ac(0,5)==vc && ac(5,5)==vc && ac(10,5)==vc);
+
+ // Shrink ac back down to 5 again.
+ ac.erase(ac.begin()+2, ac.begin()+12);
+ SimTK_TEST(ac == vc);
+ SimTK_TEST(ac.allocated() >= 15);
+ ac.shrink_to_fit();
+ SimTK_TEST(ac.allocated() < 15);
+ SimTK_TEST(ac == vc); // make sure we didn't lose the data
+
+ // Now try some null insertions
+ Array_<int> null;
+ ac.insert(ac.begin(), null.begin(), null.end());
+ ac.insert(ac.begin()+2, null.begin(), null.end());
+ ac.insert(ac.end(), null.begin(), null.end());
+ ac.insert(ac.begin(), 0, 929);
+ ac.insert(ac.begin()+2, 0, 929);
+ ac.insert(ac.end(), 0, 929);
+ SimTK_TEST(ac == vc);
+
+ ArrayView_<int> null2;
+ null.insert(null.begin(), null2.begin(), null2.end());
+ SimTK_TEST(null.empty() && null2.empty());
+
+ // How about inserting into a null array?
+ null.insert(null.begin(), 3, 987);
+ SimTK_TEST(null == std::vector<int>(3,987));
+ null.deallocate(); // back to null
+ SimTK_TEST(null.data()==0 && null.size()==0 && null.allocated()==0);
+
+ null.insert(null.begin(), ac.begin(), ac.end());
+ SimTK_TEST(null == vc);
+ null.deallocate();
+
+ // Fill in a bunch of 1000's in the middle, erase the beginning and
+ // end, and make sure we see just the 1000's.
+ ac.insert(ac.begin()+2, 99, 1000);
+ ac.erase(ac.begin(), ac.begin()+2);
+ ac.erase(ac.end()-3, ac.end());
+ SimTK_TEST(ac == Array_<int>(99,1000));
+
+}
+
+// A bool index type is more or less useless in real life but was handy for
+// catching obscure implementation bugs having to do with index_type vs.
+// size_type.
+void testBoolIndex() {
+ SimTK_TEST((Array_<int,long>().empty()));
+ SimTK_TEST(!(Array_<int,long>(1L,99).empty()));
+ SimTK_TEST((Array_<int,long>(1L,99)[0] == 99));
+
+ Array_<std::string, bool> wisdom(2);
+ SimTK_TEST(wisdom[true] == ""); SimTK_TEST(wisdom[false] == "");
+ wisdom[true] = "this too shall pass";
+ wisdom[false] = "don't worry it's not loaded";
+ SimTK_TEST(wisdom.size() == 2);
+ SimTK_TEST(wisdom.max_size() == 2);
+ SimTK_TEST(wisdom.capacity() >= 2);
+ SimTK_TEST(wisdom.allocated() == wisdom.capacity());
+ SimTK_TEST(wisdom.data() != 0);
+ SimTK_TEST(wisdom.data() == wisdom.cdata());
+ SimTK_TEST(wisdom.begin() == wisdom.data());
+ SimTK_TEST(wisdom.cbegin() == wisdom.cdata());
+ SimTK_TEST(wisdom.end() == wisdom.begin()+2);
+ SimTK_TEST(wisdom.cend() == wisdom.cbegin()+2);
+
+ SimTK_TEST(wisdom[false] == "don't worry it's not loaded");
+ SimTK_TEST(wisdom[true] == "this too shall pass");
+ SimTK_TEST(wisdom.at(false) == "don't worry it's not loaded");
+ SimTK_TEST(wisdom.at(true) == "this too shall pass");
+
+ cout << "wisdom=" << wisdom << endl;
+ cout << "wisdom(false,1)=" << wisdom(false,1) << endl;
+ cout << "wisdom(true,1)=" << wisdom(true,1) << endl;
+ cout << "wisdom(true,0)=" << wisdom(true,0) << endl;
+
+ // Subarrays are fixed size; can't assign a 1-element vector to
+ // a 2 element subarray.
+ SimTK_TEST_MUST_THROW_DEBUG(
+ wisdom(false,2) = std::vector<const char*>(1,"whatever"));
+
+ const std::vector<const char*> vrel(2,"it's all relative");
+ wisdom(false,2) = vrel;
+ cout << "wisdom=" << wisdom << endl;
+
+ // Test all the comparison operators Array vs. std::vector.
+ SimTK_TEST(wisdom == vrel); SimTK_TEST(vrel == wisdom);
+ SimTK_TEST(wisdom <= vrel); SimTK_TEST(vrel <= wisdom);
+ SimTK_TEST(wisdom >= vrel); SimTK_TEST(vrel >= wisdom);
+ SimTK_TEST(wisdom(false,1) < vrel);
+ SimTK_TEST(wisdom(true,1) < vrel);
+ SimTK_TEST(wisdom(0,0) < vrel);
+ SimTK_TEST(wisdom(true,1) < vrel);
+ SimTK_TEST(wisdom(false,1) != vrel);
+ SimTK_TEST(wisdom != std::vector<const char*>(2,"it's all absolute"));
+ wisdom[true] = "z comes after i";
+ SimTK_TEST(wisdom > vrel); SimTK_TEST(vrel < wisdom);
+
+
+ SimTK_TEST_MUST_THROW_DEBUG(wisdom(true,2));
+ SimTK_TEST_MUST_THROW_DEBUG(wisdom.push_back("more brilliance"));
+}
+
+// It should be possible to assign to an Array_ from an std::set or std::map
+// even though you can't subtract their bidirectional_iterators.
+void testNonRandomIterator() {
+ const int someInts[] = {30,40,10,20,30,7,5};
+ std::set<int> iset(someInts, someInts+7);
+ std::vector<int> sortUniq(iset.begin(), iset.end()); // the right answer
+
+ Array_<int> iarr(iset.begin(), iset.end());
+ iarr.assign(iset.begin(), iset.end()); // must increment to count
+ SimTK_TEST(iarr == sortUniq);
+
+ Array_<int> iarr2(sortUniq.begin(), sortUniq.end());
+ iarr2.assign(sortUniq.begin(), sortUniq.end()); // can subtract iterators
+ SimTK_TEST(iarr2 == sortUniq);
+ iarr2.assign(iarr.begin(), iarr.end()); // can subtract pointers
+ SimTK_TEST(iarr2 == sortUniq);
+
+ // The standard requires this to match the constructor that creates
+ // n copies of an initial value -- it must NOT match the templatized
+ // InputIterator form because these are integral types.
+ Array_<int> dummy1((char)3, 'A'); // 3*65
+ SimTK_TEST(dummy1 == Array_<int>(3, (int)'A'));
+ Array_<int> dummy2(4U, 129U); // 4*129
+ SimTK_TEST(dummy2 == Array_<int>(4, 129));
+
+ // This should use the constant-time std::swap specialization that is
+ // provided in the Array.h header file.
+ std::swap(dummy1, dummy2);
+ SimTK_TEST(dummy2 == Array_<int>(3, (int)'A'));
+ SimTK_TEST(dummy1 == Array_<int>(4, 129));
+
+ // assign() and insert() should behave like the constructor.
+ dummy1.assign((char)2, 'B');
+ dummy1.insert(dummy1.begin()+1, (char)3, 'C');
+ const int d1answer[] = {(int)'B',(int)'C',(int)'C',(int)'C',(int)'B'};
+ SimTK_TEST((dummy1 == Array_<int,unsigned short>(d1answer, d1answer+5)));
+
+ // Test fill().
+ dummy1.fill(7);
+ SimTK_TEST((dummy1 == Array_<int>(5, 7))); // i.e., 5 7's
+
+
+ // This is too much data and should be detectable for any forward iterator.
+ typedef Array_<int,SmallIx> AType;
+ SimTK_TEST_MUST_THROW_DEBUG(
+ AType small(iset.begin(), iset.end())); // bidirectional
+ SimTK_TEST_MUST_THROW_DEBUG(
+ AType small(sortUniq.begin(), sortUniq.end())); // random access
+ SimTK_TEST_MUST_THROW_DEBUG(
+ AType small(iarr.begin(), iarr.end())); // pointer
+
+}
+
+// Input iterators require special handling in the implementation because
+// it can't be determined for them how many elements are in a range
+// [first,last1) because to increment an iterator is to consume it. This is
+// the only case where a bulk constructor, insert(), or assign() must be
+// done with multiple space reallocations (basically like a series of
+// one-element push_back() calls).
+void testInputIterator() {
+ const int answerData[]={10,12,-14,5,203,-232,1,2,3,4};
+ const Array_<int,char> answer(answerData,answerData+10);
+ const Array_<int,short> smallAnswer(answerData+4, answerData+8);
+ std::istringstream inp1("10 12 -14 5 203 -232 1 2 3 4");
+ std::istringstream inp2("10 12 -14 5 203 -232 1 2 3 4");
+ std::istringstream smallInp("203 -232 1 2"); // fits in SmallIx
+ typedef std::istream_iterator<int> Iter;
+ Iter p1(inp1), p2(inp2), psmall(smallInp);
+ Array_<int> readin(p1, Iter()); // like begin(), end()
+ SimTK_TEST(readin == answer);
+
+ // This shouldn't work because there are too many elements.
+ typedef Array_<int,SmallIx> SmallArray;
+ SimTK_TEST_MUST_THROW_DEBUG(SmallArray tooSmall(p2, Iter()));
+
+ // This should be OK.
+ SmallArray okSmall(psmall, Iter());
+ SimTK_TEST(okSmall == smallAnswer);
+
+ Array_<float> farray;
+ const float farray_ans1[] = {-1.5f,3e4f,.125f,11,4e-7f};
+ std::istringstream fin1("[ -1.5, 3e4 ,.125 , 11,4e-7 ]");
+ fin1 >> farray;
+ SimTK_TEST(!fin1.fail());
+ SimTK_TEST(farray == std::vector<float>(farray_ans1, farray_ans1+5));
+
+ // Replace middle three elements.
+ ArrayView_<float> fmid(farray(1,3));
+ const float farray_ans2[] = {-1.5f,910,920,9200,4e-7f};
+ std::istringstream fin2(" 9.1e2 9.2e2 9.2e3 ignore me");
+ fin2 >> fmid;
+ SimTK_TEST(!fin2.fail());
+ SimTK_TEST(farray == Array_<float>(farray_ans2, farray_ans2+5));
+
+ std::istringstream fin3(" 9.1e2 9.2e2"); fin3 >> fmid;
+ SimTK_TEST(fin3.fail()); // wrong size
+
+ std::istringstream fin4(" 9.1e2 9.2e2,9.2e3 "); fin4 >> fmid;
+ SimTK_TEST(fin4.fail()); // inconsistent use of commas
+
+ std::istringstream fin5("(9.1e2,9.2e2,9.2e3 "); fin5 >> fmid;
+ SimTK_TEST(fin5.fail()); // missing paren
+
+ std::istringstream fin6("{9.1e2,9.2e2,9.2e3]"); fin6 >> fmid;
+ SimTK_TEST(fin6.fail()); // mismatched delimiters
+
+ std::istringstream fin7("{9.1e2,9.2e2,9.2e3,}"); fin7 >> fmid;
+ SimTK_TEST(fin7.fail()); // trailing comma
+
+ std::istringstream fin8(" 9.1e2,9.2e2,9.2e3,"); fin8 >> fmid;
+ SimTK_TEST(!fin8.fail()); // trailing comma OK here because we got our fill
+
+}
+
+// Reduce the loop count by 50X in Debug.
+static const int Outer = 500000
+#ifndef NDEBUG
+ / 50
+#endif
+ ;
+
+static const int Inner = 1000;
+void testSpeedStdVector() {
+ std::vector<int> v;
+ v.reserve(Inner);
+
+ for (int i=0; i < Outer; ++i) {
+ v.clear();
+ for (int i=0; i < Inner; ++i)
+ v.push_back(i);
+ }
+
+ int sum;
+ for (int i=0; i < Outer; ++i) {
+ sum = i;
+ for (unsigned i=0; i < v.size(); ++i)
+ sum += v[i];
+ }
+ cout << "std::vector sum=" << sum << endl;
+}
+
+void testSpeedSimTKArray() {
+ Array_<int> v;
+ v.reserve(Inner);
+
+ for (int i=0; i < Outer; ++i) {
+ v.clear();
+ for (int i=0; i < Inner; ++i)
+ v.push_back(i);
+ }
+
+ int sum;
+ for (int i=0; i < Outer; ++i) {
+ sum = i;
+ for (unsigned i=0; i < v.size(); ++i)
+ sum += v[i];
+ }
+ cout << "Array sum=" << sum << endl;
+}
+
+void testNiceTypeName() {
+ cout << "Is64BitPlatform=" << NiceTypeName<Is64BitPlatformType>::name() << endl;
+ cout << "packed_size_type<bool>="
+ << NiceTypeName<ArrayIndexPackType<bool>::packed_size_type>::name() << endl;
+ cout << "packed_size_type<char>="
+ << NiceTypeName<ArrayIndexPackType<char>::packed_size_type>::name() << endl;
+ cout << "packed_size_type<signed char>="
+ << NiceTypeName<ArrayIndexPackType<signed char>::packed_size_type>::name() << endl;
+ cout << "packed_size_type<unsigned char>="
+ << NiceTypeName<ArrayIndexPackType<unsigned char>::packed_size_type>::name() << endl;
+ cout << "packed_size_type<short>="
+ << NiceTypeName<ArrayIndexPackType<short>::packed_size_type>::name() << endl;
+ cout << "packed_size_type<unsigned short>="
+ << NiceTypeName<ArrayIndexPackType<unsigned short>::packed_size_type>::name() << endl;
+ cout << "packed_size_type<int>="
+ << NiceTypeName<ArrayIndexPackType<int>::packed_size_type>::name() << endl;
+ cout << "packed_size_type<unsigned>="
+ << NiceTypeName<ArrayIndexPackType<unsigned>::packed_size_type>::name() << endl;
+ cout << "packed_size_type<long>="
+ << NiceTypeName<ArrayIndexPackType<long>::packed_size_type>::name() << endl;
+ cout << "packed_size_type<unsigned long long>="
+ << NiceTypeName<ArrayIndexPackType<unsigned long long>::packed_size_type>::name() << endl;
+ cout << NiceTypeName< Array_<String,char> >::name() << endl;
+}
+
+// The Array_ class is supposed to make better use of memory than does
+// std::vector when the index type is smaller than a pointer.
+void testMemoryFootprint() {
+ // These conditions should apply on any 32- or 64-bit platform.
+ SimTK_TEST(sizeof(Array_<int>) <= sizeof(std::vector<int>));
+ SimTK_TEST(sizeof(Array_<int,bool>) < sizeof(std::vector<int>));
+ SimTK_TEST(sizeof(Array_<int,char>) < sizeof(std::vector<int>));
+ SimTK_TEST(sizeof(Array_<int,signed char>) < sizeof(std::vector<int>));
+ SimTK_TEST(sizeof(Array_<int,unsigned char>) < sizeof(std::vector<int>));
+ SimTK_TEST(sizeof(Array_<int,short>) < sizeof(std::vector<int>));
+ SimTK_TEST(sizeof(Array_<int,unsigned short>) < sizeof(std::vector<int>));
+
+ // Since an int is smaller than a pointer here we will do better than
+ // any 3-pointer implementation. And we shouldn't be worse then normal
+ // for long longs.
+ if (Is64BitPlatform) {
+ SimTK_TEST(sizeof(Array_<int,int>) < sizeof(std::vector<int>));
+ SimTK_TEST(sizeof(Array_<int,unsigned>) < sizeof(std::vector<int>));
+ SimTK_TEST(sizeof(Array_<int,long long>) <= sizeof(std::vector<int>));
+ SimTK_TEST(sizeof(Array_<int,unsigned long long>) <= sizeof(std::vector<int>));
+ }
+
+ // We don't know if long will be 32 or 64 bit on any given 64 bit
+ // implementation (it is 32 bits for MSVC and 64 for gcc). But it is
+ // always 32 bits on a 32 bit implementation so we shouldn't be doing
+ // any worse here.
+ SimTK_TEST(sizeof(Array_<int,long>) <= sizeof(std::vector<int>));
+
+ // Check that packing is working right.
+ // ints and larger are treated the same for 32 vs 64. (longs are
+ // wobblers though so we don't check here)
+ SimTK_TEST(sizeof(Array_<int>::packed_size_type)==sizeof(int));
+ SimTK_TEST(sizeof(Array_<int,int>::packed_size_type)==sizeof(int));
+ SimTK_TEST(sizeof(Array_<int,unsigned int>::packed_size_type)==sizeof(int));
+ SimTK_TEST(sizeof(Array_<int,long long>::packed_size_type)==sizeof(long long));
+ SimTK_TEST(sizeof(Array_<int,unsigned long long>::packed_size_type)==sizeof(long long));
+ if (Is64BitPlatform) {
+ // Small types are packed into an int on 64 bit platform.
+ SimTK_TEST(sizeof(Array_<int,bool>::packed_size_type)==sizeof(int));
+ SimTK_TEST(sizeof(Array_<int,char>::packed_size_type)==sizeof(int));
+ SimTK_TEST(sizeof(Array_<int,signed char>::packed_size_type)==sizeof(int));
+ SimTK_TEST(sizeof(Array_<int,unsigned char>::packed_size_type)==sizeof(int));
+ SimTK_TEST(sizeof(Array_<int,short>::packed_size_type)==sizeof(int));
+ SimTK_TEST(sizeof(Array_<int,unsigned short>::packed_size_type)==sizeof(int));
+ } else {
+ // Small types are packed into a short on 32 bit platform.
+ SimTK_TEST(sizeof(Array_<int,bool>::packed_size_type)==sizeof(short));
+ SimTK_TEST(sizeof(Array_<int,char>::packed_size_type)==sizeof(short));
+ SimTK_TEST(sizeof(Array_<int,signed char>::packed_size_type)==sizeof(short));
+ SimTK_TEST(sizeof(Array_<int,unsigned char>::packed_size_type)==sizeof(short));
+ SimTK_TEST(sizeof(Array_<int,short>::packed_size_type)==sizeof(short));
+ SimTK_TEST(sizeof(Array_<int,unsigned short>::packed_size_type)==sizeof(short));
+ }
+
+ // Now we'll bravely insist that we know how these should be packed.
+ if (Is64BitPlatform) {
+ SimTK_TEST(sizeof(Array_<int>)==16);
+ SimTK_TEST(sizeof(Array_<int,bool>)==16);
+ SimTK_TEST(sizeof(Array_<int,char>)==16);
+ SimTK_TEST(sizeof(Array_<int,signed char>)==16);
+ SimTK_TEST(sizeof(Array_<int,unsigned char>)==16);
+ SimTK_TEST(sizeof(Array_<int,short>)==16);
+ SimTK_TEST(sizeof(Array_<int,unsigned short>)==16);
+ SimTK_TEST(sizeof(Array_<int,int>)==16);
+ SimTK_TEST(sizeof(Array_<int,unsigned>)==16);
+ SimTK_TEST(sizeof(Array_<int,long>)<=24);
+ SimTK_TEST(sizeof(Array_<int,unsigned long>)<=24);
+ SimTK_TEST(sizeof(Array_<int,long long>)==24);
+ SimTK_TEST(sizeof(Array_<int,unsigned long long>)==24);
+ } else { // 32 bit platform
+ SimTK_TEST(sizeof(Array_<int>)==12);
+ SimTK_TEST(sizeof(Array_<int,bool>)==8);
+ SimTK_TEST(sizeof(Array_<int,char>)==8);
+ SimTK_TEST(sizeof(Array_<int,signed char>)==8);
+ SimTK_TEST(sizeof(Array_<int,unsigned char>)==8);
+ SimTK_TEST(sizeof(Array_<int,short>)==8);
+ SimTK_TEST(sizeof(Array_<int,unsigned short>)==8);
+ SimTK_TEST(sizeof(Array_<int,int>)==12);
+ SimTK_TEST(sizeof(Array_<int,unsigned>)==12);
+ SimTK_TEST(sizeof(Array_<int,long>)<=12);
+ SimTK_TEST(sizeof(Array_<int,unsigned long>)<=12);
+ // These don't make sense on a 32 bit platform, but they work. The
+ // size will be 20 or 24 depending on how the compiler aligns the
+ // 8-byte integers after the pointer.
+ SimTK_TEST(sizeof(Array_<int,long long>)<=24);
+ SimTK_TEST(sizeof(Array_<int,unsigned long long>)<=24);
+ }
+}
+
+int main() {
+
+ SimTK_START_TEST("TestArray");
+
+ SimTK_SUBTEST(testInsert);
+ SimTK_SUBTEST(testArrayViewAssignment);
+ SimTK_SUBTEST(testInputIterator);
+ SimTK_SUBTEST(testNiceTypeName);
+ SimTK_SUBTEST(testMemoryFootprint);
+ SimTK_SUBTEST(testConstruction);
+ SimTK_SUBTEST(testConversion);
+ SimTK_SUBTEST(testBoolIndex);
+ SimTK_SUBTEST(testNonRandomIterator);
+ SimTK_SUBTEST(testSpeedStdVector);
+ SimTK_SUBTEST(testSpeedSimTKArray);
+
+ SimTK_END_TEST();
+}
+
diff --git a/SimTKcommon/tests/TestAtomicInteger.cpp b/SimTKcommon/tests/TestAtomicInteger.cpp
new file mode 100644
index 0000000..b538815
--- /dev/null
+++ b/SimTKcommon/tests/TestAtomicInteger.cpp
@@ -0,0 +1,152 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+using std::cout;
+using std::endl;
+using namespace SimTK;
+using namespace std;
+
+void testOperators() {
+ AtomicInteger a = 5;
+ ASSERT(a == 5);
+ ASSERT(a != 6);
+ a = 6;
+ ASSERT(a == 6);
+ ASSERT(++a == 7);
+ ASSERT(a == 7);
+ ASSERT(a++ == 7);
+ ASSERT(a == 8);
+ ASSERT(--a == 7);
+ ASSERT(a == 7);
+ ASSERT(a-- == 7);
+ ASSERT(a == 6);
+ a += 2;
+ ASSERT(a == 8);
+ a -= 5;
+ ASSERT(a == 3);
+ a *= -5;
+ ASSERT(a == -15);
+ a /= 3;
+ ASSERT(a == -5);
+ int i = a;
+ ASSERT(i == -5);
+ a = 3;
+ a |= 4;
+ ASSERT(a = 7);
+ a &= 13;
+ ASSERT(a == 5);
+ a ^= 12;
+ ASSERT(a == 9);
+ a %= 7;
+ ASSERT(a == 2);
+ a <<= 2;
+ ASSERT(a == 8);
+ a >>= 1;
+ ASSERT(a == 4);
+}
+
+void testParallelExecution() {
+ AtomicInteger index;
+ vector<int> flags(10000);
+ for (int i = 0; i < (int)flags.size(); ++i)
+ flags[i] = 0;
+ ParallelExecutor executor;
+
+ // See if the ++ operator is properly atomic.
+
+ class SetFlagTask : public ParallelExecutor::Task {
+ public:
+ SetFlagTask(vector<int>& flags, AtomicInteger& index) : flags(flags), index(index) {
+ }
+ void execute(int i) {
+ flags[index++]++;
+ }
+ private:
+ vector<int>& flags;
+ AtomicInteger& index;
+ };
+ for (int i = 0; i < 100; ++i) {
+ SetFlagTask task(flags, index);
+ index = 0;
+ executor.execute(task, 5000);
+ ASSERT(index == 5000);
+ for (int j = 0; j < (int)flags.size(); ++j)
+ ASSERT(flags[j] == (j < 5000 ? i+1 : 0));
+ }
+
+ // See if the += operator is properly atomic.
+
+ class IncrementTask : public ParallelExecutor::Task {
+ public:
+ IncrementTask(AtomicInteger& index) : index(index) {
+ }
+ void execute(int i) {
+ index += 2;
+ }
+ private:
+ AtomicInteger& index;
+ };
+ for (int i = 0; i < 100; ++i) {
+ IncrementTask task(index);
+ index = 0;
+ executor.execute(task, 5000);
+ ASSERT(index == 10000);
+ }
+
+ // See if the *= operator is properly atomic.
+
+ class MultiplyTask : public ParallelExecutor::Task {
+ public:
+ MultiplyTask(AtomicInteger& index) : index(index) {
+ }
+ void execute(int i) {
+ index *= (i%500 == 0 ? 2 : -1);
+ }
+ private:
+ AtomicInteger& index;
+ };
+ for (int i = 0; i < 100; ++i) {
+ MultiplyTask task(index);
+ index = 1;
+ executor.execute(task, 4999);
+ ASSERT(index == -1024);
+ }
+}
+
+int main() {
+ try {
+ testOperators();
+ testParallelExecution();
+ } catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ cout << "Done" << endl;
+ return 0;
+}
diff --git a/SimTKcommon/tests/TestBigMatrix.cpp b/SimTKcommon/tests/TestBigMatrix.cpp
new file mode 100644
index 0000000..4cf648f
--- /dev/null
+++ b/SimTKcommon/tests/TestBigMatrix.cpp
@@ -0,0 +1,281 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "SimTKcommon/Testing.h"
+
+#include <iostream>
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+using std::cout;
+using std::endl;
+using namespace SimTK;
+using namespace std;
+
+template <class T, int N>
+void testVector(const T& value, const Vec<N>& expected) {
+ ASSERT(value.size() == N);
+ for (int i = 0; i < N; ++i) {
+ if (isNaN(expected[i])) {
+ ASSERT(isNaN(value[i]));
+ }
+ else {
+ ASSERT(value[i] == expected[i]);
+ }
+ }
+}
+
+template <class T, int M, int N>
+void testMatrix(const T& value, const Mat<M, N>& expected) {
+ ASSERT(value.nrow() == M);
+ ASSERT(value.ncol() == N);
+ for (int i = 0; i < M; ++i)
+ for (int j = 0; j < N; ++j) {
+ if (isNaN(expected(i, j))) {
+ ASSERT(isNaN(value(i, j)));
+ }
+ else {
+ ASSERT(value(i, j) == expected(i, j));
+ }
+ }
+}
+
+
+void testMatDivision() {
+ Mat22 m1( 4, 0,
+ 0, 1);
+ Mat22 oom1( .25, 0,
+ 0, 1 );
+ Mat<2,2,Mat22> m2(Mat22( 2, 0,
+ 0, 3 ));
+ Mat<2,2,Mat22> oom2(Mat22( .5, 0,
+ 0, OneThird));
+
+ SimTK_TEST_EQ(1/m1, oom1);
+ SimTK_TEST_EQ(1/m2, oom2);
+}
+
+static void f(const Vec3& v) {
+ cout << "f(v)=" << v << endl;
+}
+
+void testTransform() {
+ Transform X;
+ Rotation R;
+ Mat33 m;
+ Vec3 v;
+ Vec<3,Real,6> vs(1,2,3); // funny stride
+ Vec<4,Real,9> vs2(1,2,3,0); // funny stride
+ f(vs);
+
+ SimTK_TEST(X*vs == -(X*-vs));
+ SimTK_TEST(X*vs2 == -(X*-vs2));
+
+ SimTK_TEST(R*vs == -(R*-vs));
+ SimTK_TEST(~vs*R == -(-~vs*R));
+}
+
+// Make sure we can instantiate all of these successfully.
+namespace SimTK {
+template class MatrixBase<double>;
+template class VectorBase<double>;
+template class RowVectorBase<double>;
+template class MatrixView_<double>;
+template class VectorView_<double>;
+template class RowVectorView_<double>;
+template class Matrix_<double>;
+template class Vector_<double>;
+template class RowVector_<double>;
+
+template class MatrixBase<negator<double> >;
+template class VectorBase<negator<double> >;
+template class RowVectorBase<negator<double> >;
+template class MatrixView_<negator<double> >;
+template class VectorView_<negator<double> >;
+template class RowVectorView_<negator<double> >;
+template class Matrix_<negator<double> >;
+template class Vector_<negator<double> >;
+template class RowVector_<negator<double> >;
+}
+
+int main() {
+ try {
+ // Currently, this only tests a small number of operations that were recently added.
+ // It should be expanded into a more comprehensive test of the big matrix classes.
+
+ testMatDivision();
+ testTransform();
+
+ Matrix m(Mat22(1, 2, 3, 4));
+ testMatrix<Matrix,2,2>(m, Mat22(1, 2, 3, 4));
+ m += 3;
+ testMatrix<Matrix,2,2>(m, Mat22(4, 2, 3, 7));
+ m -= 3;
+ testMatrix<Matrix,2,2>(m, Mat22(1, 2, 3, 4));
+ testMatrix<Matrix,2,2>(m-1, Mat22(0, 2, 3, 3));
+ testMatrix<Matrix,2,2>(m+1, Mat22(2, 2, 3, 5));
+ testMatrix<Matrix,2,2>(1-m, Mat22(0, -2, -3, -3));
+ testMatrix<Matrix,2,2>(1+m, Mat22(2, 2, 3, 5));
+ Vector v(Vec3(1, 2, 3));
+ testVector(v, Vec3(1, 2, 3));
+ v += 2;
+ testVector(v, Vec3(3, 4, 5));
+ v -= 2;
+ testVector(v, Vec3(1, 2, 3));
+ testVector(v-1, Vec3(0, 1, 2));
+ testVector(v+1, Vec3(2, 3, 4));
+ testVector(1-v, Vec3(0, -1, -2));
+ testVector(1+v, Vec3(2, 3, 4));
+ RowVector r(Row3(1, 2, 3));
+ testVector(r, Vec3(1, 2, 3));
+ r += 2;
+ testVector(r, Vec3(3, 4, 5));
+ r -= 2;
+ testVector(r, Vec3(1, 2, 3));
+ testVector(r-1, Vec3(0, 1, 2));
+ testVector(r+1, Vec3(2, 3, 4));
+ testVector(1-r, Vec3(0, -1, -2));
+ testVector(1+r, Vec3(2, 3, 4));
+
+ Matrix mm( Mat23( 1, 2, 3,
+ 7, 8, 9 ) );
+ testMatrix<Matrix,2,3>(mm, Mat23(1,2,3,7,8,9));
+
+ // Test copying a column or row of a Matrix into
+ // a Vector or RowVector.
+
+ // Test assignment constructor
+ Vector vv = mm(1); testVector(vv, Vec2(2,8));
+ // Test copy assignment
+ vv = mm(0); testVector(vv, Vec2(1,7));
+ // Test assignment constructor
+ RowVector rr = mm[1]; testVector(rr, Vec3(7,8,9));
+ // Test copy assignment
+ rr = mm[0]; testVector(rr, Vec3(1,2,3));
+
+ // Test copying a row into a Vector and column into RowVector.
+
+ // Test assignment (copy) constructor
+ RowVector rrr = ~mm(1);
+ testVector(rrr, Vec2(2,8));
+ // Test copy assignment
+ rrr = ~mm(0); testVector(rrr, Vec2(1,7));
+
+ // Test assignment (copy) constructor
+ Vector vvv = ~mm[1];
+ testVector(vvv, Vec3(7,8,9));
+ // Test copy assignment
+ vvv = ~mm[0]; testVector(vvv, Vec3(1,2,3));
+
+ // Test creating a Matrix that shares space with an Array
+
+ // Easy case: sizeof(element) == sizeof(scalar)
+ Array_<Real> rarrmat;
+ rarrmat.push_back(1.1); rarrmat.push_back(2.2); // col(0)
+ rarrmat.push_back(3.3); rarrmat.push_back(4.4); // col(1)
+ Matrix rmatrix(2,2, 2/*lda*/, &rarrmat[0]);
+ testMatrix<Matrix,2,2>(rmatrix, Mat22(1.1, 3.3,
+ 2.2, 4.4));
+
+ // Here sizeof(element) != sizeof(scalar)
+ Array_<SpatialVec> svarrmat;
+ svarrmat.push_back(SpatialVec(Vec3(1,2,3),Vec3(4,5,6)));
+ svarrmat.push_back(SpatialVec(Vec3(1.1,2.1,3.1),Vec3(4.1,5.1,6.1)));
+ svarrmat.push_back(SpatialVec(Vec3(1.2,2.2,3.2),Vec3(4.2,5.2,6.2)));
+ svarrmat.push_back(SpatialVec(Vec3(1.3,2.3,3.3),Vec3(4.3,5.3,6.3)));
+ const int szInScalars = sizeof(SpatialVec)/sizeof(Real);
+ Matrix_<SpatialVec> svmatrix(2,2, 2*szInScalars/*lda*/,
+ (Real*)&svarrmat[0]);
+ Matrix_<SpatialVec> svmatans(2,2);
+ svmatans(0,0) = svarrmat[0]; svmatans(1,0)=svarrmat[1];
+ svmatans(0,1) = svarrmat[2]; svmatans(1,1)=svarrmat[3];
+ SimTK_TEST_EQ_TOL(svmatrix, svmatans, 1e-16); // should be exact
+
+ // Test creating a Vector that shares space with an Array
+
+ // Easy case: sizeof(element) == sizeof(scalar)
+ Array_<Real> rarray;
+ rarray.push_back(1.1); rarray.push_back(2.2); rarray.push_back(3.3);
+ Vector rvector(3, &rarray[0], true);
+ testVector(rarray, Vec3(1.1,2.2,3.3));
+
+ // Here sizeof(element) != sizeof(scalar)
+ Array_<SpatialVec> svarray;
+ svarray.push_back(SpatialVec(Vec3(1,2,3),Vec3(4,5,6)));
+ svarray.push_back(SpatialVec(Vec3(1.1,2.1,3.1),Vec3(4.1,5.1,6.1)));
+ svarray.push_back(SpatialVec(Vec3(1.2,2.2,3.2),Vec3(4.2,5.2,6.2)));
+ Vector_<SpatialVec> svvector(3, (Real*)&svarray[0], true);
+ Vector_<SpatialVec> svanswer(3);
+ svanswer[0]=svarray[0];svanswer[1]=svarray[1];svanswer[2]=svarray[2];
+ SimTK_TEST_EQ_TOL(svvector, svanswer, 1e-16); // should be exact
+
+ // Create 0-width slices of Matrix that has general shape,
+ // vector shape, and row vector shape. This caused trouble before
+ // because vector and row shapes use 1d matrix storage; when making
+ // a 0-width slice of those they have to go back to general shape.
+ // Note that you are allowed to index off the bottom and right if
+ // you make a zero-width slice.
+
+ Matrix general(3, 4);
+ MatrixView gslice1 = general(1,1,0,2); // middle
+ SimTK_TEST(gslice1.nrow()==0 && gslice1.ncol()==2);
+ MatrixView gslice2 = general(1,1,1,0); // middle
+ SimTK_TEST(gslice2.nrow()==1 && gslice2.ncol()==0);
+ MatrixView gslice3 = general(0,0,3,0); // left side
+ SimTK_TEST(gslice3.nrow()==3 && gslice3.ncol()==0);
+ MatrixView gslice4 = general(0,0,0,4); // top
+ SimTK_TEST(gslice4.nrow()==0 && gslice4.ncol()==4);
+ MatrixView gslice5 = general(3,0,0,4); // off the bottom
+ SimTK_TEST(gslice5.nrow()==0 && gslice5.ncol()==4);
+ MatrixView gslice6 = general(0,4,3,0); // off the right side
+ SimTK_TEST(gslice6.nrow()==3 && gslice6.ncol()==0);
+ MatrixView gslice7 = general(0,0,0,0);
+ SimTK_TEST(gslice7.nrow()==0 && gslice7.ncol()==0);
+ MatrixView gslice8 = general(1,2,0,0);
+ SimTK_TEST(gslice8.nrow()==0 && gslice8.ncol()==0);
+ MatrixView gslice9 = general(2,3,0,0);
+ SimTK_TEST(gslice9.nrow()==0 && gslice9.ncol()==0);
+
+ MatrixView vector = general(0,1,3,1);
+ SimTK_TEST(vector.nrow()==3 && vector.ncol()==1);
+ MatrixView vslice1 = vector(0,0,3,0);
+ SimTK_TEST(vslice1.nrow()==3 && vslice1.ncol()==0);
+ MatrixView vslice2 = vector(0,0,0,0);
+ SimTK_TEST(vslice2.nrow()==0 && vslice2.ncol()==0);
+ MatrixView vslice3 = vector(2,0,1,0);
+ SimTK_TEST(vslice3.nrow()==1 && vslice3.ncol()==0);
+ MatrixView vslice4 = vector(3,0,0,1); // off the bottom
+ SimTK_TEST(vslice4.nrow()==0 && vslice4.ncol()==1);
+ MatrixView vslice5 = vector(0,1,3,0); // off the right
+ SimTK_TEST(vslice5.nrow()==3 && vslice5.ncol()==0);
+ vslice5 = Matrix(3,0);
+
+
+ } catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ cout << "Done" << endl;
+ return 0;
+}
diff --git a/SimTKcommon/tests/TestFunction.cpp b/SimTKcommon/tests/TestFunction.cpp
new file mode 100644
index 0000000..757e8b3
--- /dev/null
+++ b/SimTKcommon/tests/TestFunction.cpp
@@ -0,0 +1,210 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+// Note: this file was moved from Simmath to SimTKcommon 20100601; see the
+// Simmath repository for earlier history.
+
+#include "SimTKcommon.h"
+#include "SimTKcommon/Testing.h"
+
+
+// We'll use some std::vectors to check interoperability between Array_<T>
+// and std::vector<T>.
+#include <vector>
+
+using namespace SimTK;
+using namespace std;
+
+const Real TOL = 1e-10;
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+void assertEqual(Real val1, Real val2, double tol=TOL) {
+ ASSERT(abs(val1-val2) < tol);
+}
+
+template <int N>
+void assertEqual(Vec<N> val1, Vec<N> val2, double tol=TOL) {
+ for (int i = 0; i < N; ++i)
+ ASSERT(abs(val1[i]-val2[i]) < tol);
+}
+
+template <class T>
+void assertEqual(Vector_<T> val1, Vector_<T> val2, double tol=TOL) {
+ ASSERT(val1.size() == val2.size());
+ for (int i = 0; i < val1.size(); ++i)
+ assertEqual(val1[i], val2[i], tol);
+}
+
+void testConstant() {
+ Function_<Vec3>::Constant f(Vec3(1, 2, 3), 2);
+ ASSERT(f.getArgumentSize() == 2);
+ Vector x(2);
+ assertEqual(Vec3(1, 2, 3), f.calcValue(x));
+ Array_<int> derivComponents(1);
+ const Vec3 df = f.calcDerivative(derivComponents, x);
+ assertEqual(Vec3(0), df);
+}
+
+void testLinear() {
+ Vector_<Vec3> coeff(3);
+ coeff[0] = Vec3(1, 2, 3);
+ coeff[1] = Vec3(4, 3, 2);
+ coeff[2] = Vec3(-1, -2, -3);
+ Function_<Vec3>::Linear f(coeff);
+ ASSERT(f.getArgumentSize() == 2);
+ assertEqual(Vec3(-1, -2, -3), f.calcValue(Vector(Vec2(0, 0))));
+ assertEqual(Vec3(0, 0, 0), f.calcValue(Vector(Vec2(1, 0))));
+ assertEqual(Vec3(-2.5, -2.5, -2.5), f.calcValue(Vector(Vec2(0.5, -0.5))));
+ std::vector<int> derivComponents(1);
+ derivComponents[0] = 1;
+ assertEqual(Vec3(4, 3, 2), f.calcDerivative(derivComponents, Vector(Vec2(1, 0))));
+ std::vector<int> derivComponents2(2);
+ assertEqual(Vec3(0, 0, 0), f.calcDerivative(derivComponents2, Vector(Vec2(1, 0))));
+}
+
+void testPolynomial() {
+ Vector_<Vec3> coeff(3);
+ coeff[0] = Vec3(1, 2, 3);
+ coeff[1] = Vec3(4, 3, 2);
+ coeff[2] = Vec3(-1, -2, -3);
+ Function_<Vec3>::Polynomial f(coeff);
+ ASSERT(f.getArgumentSize() == 1);
+ assertEqual(Vec3(-1, -2, -3), f.calcValue(Vector(Vec1(0))));
+ assertEqual(Vec3(4, 3, 2), f.calcValue(Vector(Vec1(1))));
+ assertEqual(Vec3(11, 12, 13), f.calcValue(Vector(Vec1(2))));
+ std::vector<int> derivComponents(1);
+ assertEqual(Vec3(4, 3, 2), f.calcDerivative(derivComponents, Vector(Vec1(0))));
+ assertEqual(Vec3(6, 7, 8), f.calcDerivative(derivComponents, Vector(Vec1(1))));
+ assertEqual(Vec3(8, 11, 14), f.calcDerivative(derivComponents, Vector(Vec1(2))));
+ std::vector<int> derivComponents2(2);
+ assertEqual(Vec3(2, 4, 6), f.calcDerivative(derivComponents2, Vector(Vec1(0))));
+ assertEqual(Vec3(2, 4, 6), f.calcDerivative(derivComponents2, Vector(Vec1(1))));
+ std::vector<int> derivComponents3(3);
+ assertEqual(Vec3(0, 0, 0), f.calcDerivative(derivComponents3, Vector(Vec1(1))));
+}
+
+void testRealFunction() {
+ Vector coeff(3);
+ coeff[0] = 1.0;
+ coeff[1] = 4.0;
+ coeff[2] = -1.0;
+ Function::Linear f(coeff);
+ ASSERT(f.getArgumentSize() == 2);
+ assertEqual(-1, f.calcValue(Vector(Vec2(0, 0))));
+ assertEqual(0, f.calcValue(Vector(Vec2(1, 0))));
+ assertEqual(-2.5, f.calcValue(Vector(Vec2(0.5, -0.5))));
+ Array_<int> derivComponents(1);
+ derivComponents[0] = 1;
+ assertEqual(4, f.calcDerivative(derivComponents, Vector(Vec2(1, 0))));
+ Array_<int> derivComponents2(2);
+ assertEqual(0, f.calcDerivative(derivComponents2, Vector(Vec2(1, 0))));
+}
+
+void testSinusoid() {
+ Real a=11.23, w=1.1, p=Pi/4;
+ Vector t1(1,.23), t2(1,-3.2), t3(1,14.1);
+ Function::Sinusoid s1(a,w,p);
+ SimTK_TEST_EQ(s1.calcValue(Vector(1,0.)), a*std::sin(p));
+ SimTK_TEST_EQ(s1.calcValue(t1), a*std::sin(w*t1[0]+p));
+ SimTK_TEST_EQ(s1.calcValue(t2), a*std::sin(w*t2[0]+p));
+ SimTK_TEST_EQ(s1.calcValue(t3), a*std::sin(w*t3[0]+p));
+
+ // Do enough of these to make sure we reach the general forumula.
+ Array_<int> deriv; // 0th derivative is function
+ SimTK_TEST_EQ(s1.calcDerivative(deriv, t1), s1.calcValue(t1)); // 0th
+ deriv.push_back(0); // 1st deriv
+ SimTK_TEST_EQ(s1.calcDerivative(deriv, t2), a*w*std::cos(w*t2[0]+p));
+ deriv.push_back(0); // 2nd deriv
+ SimTK_TEST_EQ(s1.calcDerivative(deriv, t3), -a*w*w*std::sin(w*t3[0]+p));
+ deriv.push_back(0); // 3rd deriv
+ SimTK_TEST_EQ(s1.calcDerivative(deriv, t1), -a*w*w*w*std::cos(w*t1[0]+p));
+ deriv.push_back(0); // 4th deriv
+ SimTK_TEST_EQ(s1.calcDerivative(deriv, t1), a*w*w*w*w*std::sin(w*t1[0]+p));
+ deriv.push_back(0); // 5th deriv
+ SimTK_TEST_EQ(s1.calcDerivative(deriv, t2), a*w*w*w*w*w*std::cos(w*t2[0]+p));
+ deriv.push_back(0); // 6th deriv
+ SimTK_TEST_EQ(s1.calcDerivative(deriv, t2), -a*w*w*w*w*w*w*std::sin(w*t2[0]+p));
+ deriv.push_back(0); // 7th deriv
+ SimTK_TEST_EQ(s1.calcDerivative(deriv, t2), -a*w*w*w*w*w*w*w*std::cos(w*t2[0]+p));
+}
+
+void testStep() {
+ Function::Step s1(-1,1,0,1); // y in [-1,1] as x in [0,1]
+ SimTK_TEST(s1.calcValue(Vector(1,Zero)) == -1); // x0 -> y0
+ SimTK_TEST(s1.calcValue(Vector(1,One)) == 1); // x1 -> y1
+ SimTK_TEST(s1.calcValue(Vector(1,OneHalf)) == 0); // 1/2 -> (y1+y0)/2
+ SimTK_TEST(s1.calcValue(Vector(1,Real(-29))) == -1);
+ SimTK_TEST(s1.calcValue(Vector(1,Real(234.3))) == 1);
+
+ // First & second derivs should be zero at either end.
+ Array_<int> derivOrder1(1);
+ SimTK_TEST(s1.calcDerivative(derivOrder1, Vector(1,Zero)) == 0);
+ SimTK_TEST(s1.calcDerivative(derivOrder1, Vector(1,One)) == 0);
+ SimTK_TEST(s1.calcDerivative(derivOrder1, Vector(1,Real(-29))) == 0);
+ SimTK_TEST(s1.calcDerivative(derivOrder1, Vector(1,Real(234.3))) == 0);
+ Array_<int> derivOrder2(2);
+ SimTK_TEST(s1.calcDerivative(derivOrder2, Vector(1,Zero)) == 0);
+ SimTK_TEST(s1.calcDerivative(derivOrder2, Vector(1,One)) == 0);
+ SimTK_TEST(s1.calcDerivative(derivOrder2, Vector(1,Real(-29))) == 0);
+ SimTK_TEST(s1.calcDerivative(derivOrder2, Vector(1,Real(234.3))) == 0);
+ Array_<int> derivOrder3(3); // don't know much about 3rd derivative
+ SimTK_TEST(s1.calcDerivative(derivOrder3, Vector(1,Real(-29))) == 0);
+ SimTK_TEST(s1.calcDerivative(derivOrder3, Vector(1,Real(234.3))) == 0);
+
+ // Try a more general step with x0,x1 reversed also.
+ // Here y goes from -221.3 to 47.9 as x goes from 1000 down to -333.
+ const Real y0=-221.3, y1=47.9, x0=1000, x1=-333;
+ Function::Step s2(y0,y1,x0,x1);
+ SimTK_TEST(s2.calcValue(Vector(1,x0)) == y0); // x0 -> y0
+ SimTK_TEST(s2.calcValue(Vector(1,x1)) == y1); // x1 -> y1
+ SimTK_TEST_EQ(s2.calcValue(Vector(1,(x1+x0)/2)), (y1+y0)/2); // (x1+x0)/2 -> (y1+y0)/2
+ SimTK_TEST(s2.calcValue(Vector(1,x0+100)) == y0); // note sign
+ SimTK_TEST(s2.calcValue(Vector(1,x1-100)) == y1);
+
+ // Calculate 3rd deriv by differencing 2nd
+ const Real x = -22.701, dx = 1e-6;
+ const Real d2m=s2.calcDerivative(derivOrder2, Vector(1, x-dx));
+ const Real d2p=s2.calcDerivative(derivOrder2, Vector(1, x+dx));
+ const Real d3approx = (d2p-d2m)/(2*dx); // approx 10 digits
+ SimTK_TEST_EQ_TOL(s2.calcDerivative(derivOrder3, Vector(1,x)),
+ d3approx, 1e-8);
+
+ // Try interpolating a Vec3
+ Function_<Vec3>::Step sv(Vec3(1,2,3), Vec3(4,5,6), 0, 1);
+ SimTK_TEST(sv.calcValue(Vector(1,OneHalf)) == Vec3(2.5,3.5,4.5));
+ SimTK_TEST(sv.calcDerivative(derivOrder2, Vector(1, -29.3)) == Vec3(0));
+}
+
+int main () {
+ SimTK_START_TEST("TestFunction");
+
+ SimTK_SUBTEST(testConstant);
+ SimTK_SUBTEST(testLinear);
+ SimTK_SUBTEST(testPolynomial);
+ SimTK_SUBTEST(testSinusoid);
+ SimTK_SUBTEST(testRealFunction);
+ SimTK_SUBTEST(testStep);
+
+ SimTK_END_TEST();
+}
diff --git a/SimTKcommon/tests/TestMassProperties.cpp b/SimTKcommon/tests/TestMassProperties.cpp
new file mode 100644
index 0000000..f363144
--- /dev/null
+++ b/SimTKcommon/tests/TestMassProperties.cpp
@@ -0,0 +1,387 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "SimTKcommon/Testing.h"
+
+#include <iostream>
+using std::cout;
+using std::endl;
+
+using namespace SimTK;
+
+// We use cross product and cross product matrices extensively in shifting
+// mass properties around.
+
+void testCrossProduct() {
+ // These are exactly representable in base 2.
+ Vec3 w(1.25, 3, -2.5), v(-2.75, 2.125, 5);
+ Vec<3,float> wf(1.25, 3, -2.5), vf(-2.75, 2.125, 5);
+ Vec3 wxv = w % v, vxw = v % w;
+ Vec<3,float> wxvf = wf % vf, vxwf = vf % wf;
+ SimTK_TEST(wxv == Vec3(20.3125, 0.625, 10.90625));
+ SimTK_TEST(wxvf == (Vec<3,float>(20.3125, 0.625, 10.90625)));
+ SimTK_TEST(vxw == -wxv);
+ SimTK_TEST(vxwf == -wxvf);
+
+ // Cross product involving one or more rows returns a row.
+ SimTK_TEST((~w)%v == ~wxv);
+ SimTK_TEST(w % ~v == ~wxv);
+ SimTK_TEST((~w) % (~v) == ~wxv);
+
+ SimTK_TEST( crossMat(w) == Mat33( 0, 2.5, 3,
+ -2.5, 0, -1.25,
+ -3, 1.25, 0) );
+
+ SimTK_TEST( crossMatSq(w) == ~crossMat(w)*crossMat(w) );
+
+ const Mat33 full33 = Test::randMat33();
+ const SymMat33 sym33 = Test::randSymMat33();
+ const Mat33 fsym33(sym33);
+ SimTK_TEST(fsym33 == sym33);
+
+ SimTK_TEST_EQ(v % full33, Mat33( v%full33(0), v%full33(1), v%full33(2) ));
+ SimTK_TEST_EQ(v % full33, crossMat(v)*full33);
+ SimTK_TEST_EQ(v % sym33, Mat33( v%fsym33(0), v%fsym33(1), v%fsym33(2) ));
+ SimTK_TEST_EQ(v % sym33, crossMat(v)*fsym33);
+
+ // m % v == m * crossMat(v) == -~(v % ~m)
+ SimTK_TEST_EQ(full33 % v, full33 * crossMat(v));
+ SimTK_TEST_EQ(full33 % v, -~(v % ~full33));
+ SimTK_TEST_EQ(full33 % v, ~(-v % ~full33));
+ SimTK_TEST_EQ(sym33 % v, fsym33 * crossMat(v));
+ SimTK_TEST_EQ(sym33 % v, -~(v % sym33));
+ SimTK_TEST_EQ(sym33 % v, ~(-v % sym33));
+
+ // gratuitous det() test
+ SimTK_TEST_EQ(det(sym33), det(fsym33));
+
+ // 2D
+
+ // These are exactly representable in base 2.
+ Vec2 w2(1.25, 3), v2(-2.75, 2.125);
+ Vec<2,float> w2f(1.25, 3), v2f(-2.75, 2.125);
+ Real wxv2 = w2 % v2, vxw2 = v2 % w2;
+ float wxv2f = w2f % v2f, vxw2f = v2f % w2f;
+ // 2D cross product is the z component of the 3D cross product where
+ // the z coordinates of the two vectors are zero (i.e., they lie in the x-y plane).
+ SimTK_TEST(wxv2 == (Vec3(w2[0],w2[1],0) % Vec3(v2[0],v2[1],0))[2]);
+ SimTK_TEST(wxv2f == (Vec<3,float>(w2f[0],w2f[1],0) % Vec<3,float>(v2f[0],v2f[1],0))[2]);
+ SimTK_TEST(vxw2 == -wxv2);
+ SimTK_TEST(vxw2f == -wxv2f);
+
+ // Cross product involving one or more rows returns same result in 2d.
+ SimTK_TEST((~w2)%v2 == wxv2);
+ SimTK_TEST(w2 % ~v2 == wxv2);
+ SimTK_TEST((~w2) % (~v2) == wxv2);
+
+ SimTK_TEST( crossMat(w2) == Row2( -w2[1], w2[0] ) );
+ SimTK_TEST( crossMat(w2)*v2 == w2 % v2 );
+
+ const Mat22 full22 = Test::randMat<2,2>();
+ const SymMat22 sym22 = Test::randSymMat<2>();
+ const Mat22 fsym22(sym22);
+ SimTK_TEST(fsym22 == sym22);
+
+ /* These don't exist in 2D
+ SimTK_TEST_EQ(v2 % full22, Row2( v2%full22(0), v2%full22(1) ));
+ SimTK_TEST_EQ(v2 % full22, crossMat(v2)*full22);
+ SimTK_TEST_EQ(v2 % sym22, Row2( v2%fsym22(0), v2%fsym22(1) ));
+ SimTK_TEST_EQ(v2 % sym22, crossMat(v2)*fsym22);
+
+ // m % v == m * crossMat(v) == -~(v % ~m)
+ SimTK_TEST_EQ(full22 % v2, full22 * crossMat(v2));
+ SimTK_TEST_EQ(full22 % v2, -~(v2 % ~full22));
+ SimTK_TEST_EQ(full22 % v2, ~(-v2 % ~full22));
+ SimTK_TEST_EQ(sym22 % v2, fsym22 * crossMat(v2));
+ SimTK_TEST_EQ(sym22 % v2, -~(v2 % sym22));
+ SimTK_TEST_EQ(sym22 % v2, ~(-v2 % sym22));
+ */
+
+ // gratuitous det() test
+ SimTK_TEST_EQ(det(sym22), det(fsym22));
+}
+
+void testInertia() {
+ const Real mass = std::abs(Test::randReal());
+ const UnitInertia_<Real> G( Vec3(1,2,2.5), // moments
+ Vec3(0.1,0.2,0.3) ); // products
+ const Real Gtrace = 1+2+2.5;
+
+ SimTK_TEST(G.trace() == Gtrace); // should be exact because .5 is power of 2
+
+ const Inertia_<Real> I = mass*G;
+ SymMat33 sI = I.asSymMat33();
+ Mat33 mI = I.toMat33();
+
+ SimTK_TEST_EQ( sI, mass*SymMat33(1,
+ 0.1, 2,
+ 0.2, 0.3, 2.5) );
+
+ SimTK_TEST(mI.isExactlySymmetric());
+ SimTK_TEST(mI == Mat33(sI));
+ SimTK_TEST(sI == SymMat33(mI));
+
+ SimTK_TEST_EQ( I.trace(), mass*Gtrace );
+
+ // Test Inertia*scalar
+ const Real s = std::abs(Test::randReal()) + 1;
+ SimTK_TEST_EQ( (I*s).toMat33(), I.toMat33()*s );
+ SimTK_TEST_EQ( (s*I).toMat33(), I.toMat33()*s );
+ SimTK_TEST_EQ( (G*s).toMat33(), G.toMat33()*s );
+ SimTK_TEST_EQ( (s*G).toMat33(), G.toMat33()*s );
+ SimTK_TEST_EQ( (I*s).asSymMat33(), I.asSymMat33()*s );
+ SimTK_TEST_EQ( (s*I).asSymMat33(), I.asSymMat33()*s );
+ SimTK_TEST_EQ( (G*s).asSymMat33(), G.asSymMat33()*s );
+ SimTK_TEST_EQ( (s*G).asSymMat33(), G.asSymMat33()*s );
+
+ // Test Inertia*vec (I*w)
+ const Vec3 w = Test::randVec3();
+ SimTK_TEST_EQ( I*w, I.toMat33()*w );
+ SimTK_TEST_EQ( G*w, G.toMat33()*w );
+ SimTK_TEST_EQ( I*w, I.asSymMat33()*w );
+ SimTK_TEST_EQ( G*w, G.asSymMat33()*w );
+
+ // Test inertia rotation
+
+ const Rotation R = Test::randRotation();
+ Inertia_<Real> mIR = Inertia_<Real>(~R*I.toMat33()*R);
+
+ SimTK_TEST_EQ(mIR.asSymMat33(), I.reexpress(R).asSymMat33());
+ SimTK_TEST_EQ(I.asSymMat33(), mIR.reexpress(~R).asSymMat33());
+
+ Inertia_<Real> J=I;
+ J.reexpressInPlace(R);
+ SimTK_TEST_EQ(J.asSymMat33(), mIR.asSymMat33());
+
+ // Test inertia shifting
+
+ // Calculate the inertia of a point mass with the same mass
+ // we used above.
+ const Vec3 pLoc = Test::randVec3();
+ const SymMat33 psG = crossMatSq(pLoc); // unit inertia
+ const SymMat33 psI = mass*psG; // inertia
+ const UnitInertia_<Real> pG(psG);
+ const Inertia_<Real> pI(psI);
+
+ // Assuming I and G are central, shifting them to pLoc should be
+ // the same as adding the point inertias above.
+ SimTK_TEST_EQ( G.shiftFromCentroid(pLoc), G + pG );
+ SimTK_TEST_EQ( I.shiftFromMassCenter(pLoc, mass), I + pI );
+
+ // Now try in place shifts and shifting back.
+ UnitInertia_<Real> Gshft(G); Gshft.shiftFromCentroidInPlace(pLoc);
+ Inertia_<Real> Ishft(I); Ishft.shiftFromMassCenterInPlace(pLoc, mass);
+ SimTK_TEST_EQ(Gshft, G+pG);
+ SimTK_TEST_EQ(Ishft, I+pI);
+
+ SimTK_TEST_EQ(Gshft.shiftToCentroid(pLoc), G);
+ SimTK_TEST_EQ(Ishft.shiftToMassCenter(pLoc, mass), I);
+
+ Gshft.shiftToCentroidInPlace(pLoc);
+ Ishft.shiftToMassCenterInPlace(pLoc, mass);
+ SimTK_TEST_EQ(Gshft, G);
+ SimTK_TEST_EQ(Ishft, I);
+
+ // Check that we catch bad inertias in debug mode
+#ifndef NDEBUG
+ Inertia bad1;
+ SimTK_TEST_MUST_THROW(bad1 = Inertia(1,2,NaN));
+ SimTK_TEST_MUST_THROW(bad1 = Inertia(1,2,-.000003)); // negative diag
+ SimTK_TEST_MUST_THROW(bad1 = Inertia(5,1,2)); // triangle inequality violated
+
+ // A little slop should be allowed for the triangle inequality.
+ Real tooMuchSlop = 1e-3;
+ Real okSlop = SignificantReal;
+ SimTK_TEST_MUST_THROW(bad1 = Inertia(1, 2+tooMuchSlop, 1));
+ bad1 = Inertia(1, 2+okSlop, 1);
+
+
+#endif
+
+}
+
+// Calculate the lower half of vx*F where vx is the cross product matrix
+// of v and F is a full 3x3 matrix. This result would normally be a full
+// 3x3 but for the uses below we know we're only going to need the diagonal
+// and lower triangle so we can save some flops by working this out by hand.
+// The method is templatized so that it will work on a transposed matrix
+// as efficiently as an untransposed one. (18 flops)
+template <class P, int CS, int RS>
+static inline SymMat<3,P>
+halfCross(const Vec<3,P>& v, const Mat<3,3,P,CS,RS>& F) {
+ return SymMat<3,P>
+ ( v[1]*F(2,0)-v[2]*F(1,0),
+ v[2]*F(0,0)-v[0]*F(2,0), v[2]*F(0,1)-v[0]*F(2,1),
+ v[0]*F(1,0)-v[1]*F(0,0), v[0]*F(1,1)-v[1]*F(0,1), v[0]*F(1,2)-v[1]*F(0,2) );
+}
+
+// Calculate the lower half of G*vx where G is a full 3x3 matrix and vx
+// is the cross product matrix of v. See comment above for details.
+// (18 flops)
+template <class P, int CS, int RS>
+static inline SymMat<3,P>
+halfCross(const Mat<3,3,P,CS,RS>& G, const Vec<3,P>& v) {
+ return SymMat<3,P>
+ ( v[2]*G(0,1)-v[1]*G(0,2),
+ v[2]*G(1,1)-v[1]*G(1,2), v[0]*G(1,2)-v[2]*G(1,0),
+ v[2]*G(2,1)-v[1]*G(2,2), v[0]*G(2,2)-v[2]*G(2,0), v[1]*G(2,0)-v[0]*G(2,1) );
+}
+
+// This method computes the lower half of the difference vx*F-G*vx using
+// the same methods as above, but done together in order to pull out the
+// common v terms. This is 33 flops, down from 42 if you call the two
+// methods above and add them.
+template <class P, int CS1, int RS1, int CS2, int RS2>
+static inline SymMat<3,P>
+halfCrossDiff(const Vec<3,P>& v, const Mat<3,3,P,CS1,RS1>& F, const Mat<3,3,P,CS2,RS2>& G) {
+ return SymMat<3,P>
+ ( v[1]*(F(2,0)+G(0,2)) - v[2]*(F(1,0)+G(0,1)),
+ v[2]*(F(0,0)-G(1,1)) - v[0]*F(2,0) + v[1]*G(1,2),
+ v[2]*(F(0,1)+G(1,0)) - v[0]*(F(2,1)+G(1,2)),
+ v[0]*F(1,0) - v[2]*G(2,1) - v[1]*(F(0,0)-G(2,2)),
+ v[0]*(F(1,1)-G(2,2)) - v[1]*F(0,1) + v[2]*G(2,0),
+ v[0]*(F(1,2)+G(2,1)) - v[1]*(F(0,2)+G(2,0)) );
+}
+
+void testHalfCross() {
+ const Vec3 v = Test::randVec3();
+ const Mat33 F = Test::randMat33();
+ const Mat33 G = Test::randMat33();
+ const Mat33 vx = crossMat(v);
+ const Mat33 vxF = v % F;
+ const Mat33 Gxv = G % v;
+
+ SimTK_TEST_EQ(vxF, vx * F);
+ SimTK_TEST_EQ(Gxv, G * vx);
+
+ const SymMat33 hvxF( vxF(0,0),
+ vxF(1,0), vxF(1,1),
+ vxF(2,0), vxF(2,1), vxF(2,2) );
+ SimTK_TEST_EQ(halfCross(v, F), hvxF);
+
+ const SymMat33 hGxv( Gxv(0,0),
+ Gxv(1,0), Gxv(1,1),
+ Gxv(2,0), Gxv(2,1), Gxv(2,2) );
+ SimTK_TEST_EQ(halfCross(G, v), hGxv);
+
+ const SymMat33 hdiff = hvxF-hGxv;
+ SimTK_TEST_EQ(halfCrossDiff(v, F, G), hdiff);
+
+}
+
+void testArticulatedInertia() {
+ const SymMat33 mass = Test::randSymMat33();
+ const Mat33 massMoment = Test::randMat33();
+ const SymMat33 inertia = crossMatSq(Test::randVec3());
+
+ ArticulatedInertia abi(mass,massMoment,inertia);
+
+ const SpatialMat mabi = abi.toSpatialMat();
+ SimTK_TEST( mabi(0,0) == inertia );
+ SimTK_TEST( mabi(0,1) == massMoment );
+ SimTK_TEST( mabi(1,0) == ~massMoment );
+ SimTK_TEST( mabi(1,1) == mass );
+
+ const Vec3 shiftVec = Test::randVec3();
+ const SpatialMat shiftMat(Mat33(1), crossMat(shiftVec),
+ Mat33(0), Mat33(1));
+ const SpatialMat mabiShiftedManually = shiftMat*mabi*~shiftMat;
+
+ const PhiMatrix phi(shiftVec);
+ const SpatialMat mabiShiftedByPhi = phi*mabi*~phi;
+
+ SimTK_TEST_EQ(mabiShiftedByPhi, mabiShiftedManually);
+
+ const ArticulatedInertia shiftAbi = abi.shift(shiftVec);
+
+ SimTK_TEST_EQ(shiftAbi.toSpatialMat(), mabiShiftedManually);
+
+ SimTK_TEST_EQ(shiftAbi.shift(-shiftVec).toSpatialMat(),
+ abi.toSpatialMat());
+
+ abi.shiftInPlace(shiftVec);
+ SimTK_TEST_EQ(abi.toSpatialMat(), mabiShiftedManually);
+
+ SimTK_TEST_EQ(abi.shiftInPlace(-shiftVec).toSpatialMat(), mabi);
+}
+
+void testManualABIShift(const ArticulatedInertia& abi, const Array_<Vec3>& shifts, Real& out) {
+ SpatialMat mabi = abi.toSpatialMat();
+
+ SpatialMat shiftMat(Mat33(1));
+ for (unsigned i=0; i<shifts.size(); ++i) {
+ shiftMat(0,1) = crossMat(shifts[i]);
+ mabi = shiftMat * mabi * ~shiftMat;
+ }
+ out = mabi(1,1)(2,2);
+}
+
+void testPhiABIShift(const ArticulatedInertia& abi, const Array_<Vec3>& shifts, Real& out) {
+ SpatialMat mabi = abi.toSpatialMat();
+
+ for (unsigned i=0; i<shifts.size(); ++i) {
+ const PhiMatrix phi(shifts[i]);
+ mabi = phi * mabi * ~phi;
+ }
+ out = mabi(1,1)(2,2);
+}
+
+void testFastABIShift(const ArticulatedInertia& abi_in, const Array_<Vec3>& shifts, Real& out) {
+ ArticulatedInertia abi(abi_in);
+ for (unsigned i=0; i<shifts.size(); ++i)
+ abi = abi.shift(shifts[i]);
+ out = abi.getMass()(2,2);
+}
+
+Array_<Vec3> shifts;
+#ifdef NDEBUG
+ #define NSHIFTS 2000000
+#else
+ #define NSHIFTS 100000
+#endif
+
+int main() {
+ SimTK_START_TEST("TestMassProperties");
+
+ SimTK_SUBTEST(testCrossProduct);
+ SimTK_SUBTEST(testInertia);
+ SimTK_SUBTEST(testHalfCross);
+ SimTK_SUBTEST(testArticulatedInertia);
+
+ // Speed tests.
+ shifts.resize(NSHIFTS);
+ for (int i=0; i<NSHIFTS; ++i) shifts[i] = Test::randVec3();
+ const SymMat33 mass = Test::randSymMat33();
+ const Mat33 massMoment = Test::randMat33();
+ const SymMat33 inertia = crossMatSq(Test::randVec3());
+ const ArticulatedInertia abi(mass,massMoment,inertia);
+ Real out1, out2, out3;
+ SimTK_SUBTEST3(testManualABIShift, abi, shifts, out1);
+ SimTK_SUBTEST3(testPhiABIShift, abi, shifts, out2);
+ SimTK_SUBTEST3(testFastABIShift, abi, shifts, out3);
+ SimTK_TEST_EQ(out1, out2); SimTK_TEST_EQ(out2, out3);
+
+ SimTK_END_TEST();
+}
+
diff --git a/SimTKcommon/tests/TestParallel2DExecutor.cpp b/SimTKcommon/tests/TestParallel2DExecutor.cpp
new file mode 100644
index 0000000..2297e4c
--- /dev/null
+++ b/SimTKcommon/tests/TestParallel2DExecutor.cpp
@@ -0,0 +1,138 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+using std::cout;
+using std::endl;
+using namespace SimTK;
+using namespace std;
+
+class SetFlagTask : public Parallel2DExecutor::Task {
+public:
+ SetFlagTask(Array_<Array_<int> >& flags, int& count) : flags(flags), count(count) {
+ }
+ void execute(int i, int j) {
+ flags[i][j]++;
+ localCount.upd()++;
+ }
+ void initialize() {
+ localCount.upd() = 0;
+ }
+ void finish() {
+ count += localCount.get();
+ }
+private:
+ Array_<Array_<int> >& flags;
+ int& count;
+ ThreadLocal<int> localCount;
+};
+
+void clearFlags(Array_<Array_<int> >& flags) {
+ int numFlags = flags.size();
+ for (int i = 0; i < numFlags; ++i) {
+ flags[i].resize(numFlags);
+ for (int j = 0; j < numFlags; ++j)
+ flags[i][j] = 0;
+ }
+}
+
+void testParallelExecution() {
+ int numFlags = 100;
+ Parallel2DExecutor executor(numFlags);
+ Array_<Array_<int> > flags(numFlags);
+ clearFlags(flags);
+ for (int iter = 0; iter < 100; ++iter) {
+ int count = 0;
+ SetFlagTask task(flags, count);
+ executor.execute(task, Parallel2DExecutor::FullMatrix);
+ ASSERT(count == numFlags*numFlags);
+ for (int i = 0; i < numFlags; ++i)
+ for (int j = 0; j < numFlags; ++j)
+ ASSERT(flags[i][j] == iter+1);
+ }
+ clearFlags(flags);
+ for (int iter = 0; iter < 100; ++iter) {
+ int count = 0;
+ SetFlagTask task(flags, count);
+ executor.execute(task, Parallel2DExecutor::HalfMatrix);
+ ASSERT(count == numFlags*(numFlags-1)/2);
+ for (int i = 0; i < numFlags; ++i)
+ for (int j = 0; j < numFlags; ++j)
+ ASSERT(flags[i][j] == (j < i ? iter+1 : 0));
+ }
+ clearFlags(flags);
+ for (int iter = 0; iter < 100; ++iter) {
+ int count = 0;
+ SetFlagTask task(flags, count);
+ executor.execute(task, Parallel2DExecutor::HalfPlusDiagonal);
+ ASSERT(count == numFlags*(numFlags+1)/2);
+ for (int i = 0; i < numFlags; ++i)
+ for (int j = 0; j < numFlags; ++j)
+ ASSERT(flags[i][j] == (j <= i ? iter+1 : 0));
+ }
+}
+
+void testSingleThreadedExecution() {
+ int numFlags = 100;
+ Parallel2DExecutor executor(numFlags, 1);
+ Array_<Array_<int> > flags(numFlags);
+ int count = 0;
+ SetFlagTask task(flags, count);
+ clearFlags(flags);
+ executor.execute(task, Parallel2DExecutor::FullMatrix);
+ ASSERT(count == numFlags*numFlags);
+ for (int i = 0; i < numFlags; ++i)
+ for (int j = 0; j < numFlags; ++j)
+ ASSERT(flags[i][j] == 1);
+ count = 0;
+ clearFlags(flags);
+ executor.execute(task, Parallel2DExecutor::HalfMatrix);
+ ASSERT(count == numFlags*(numFlags-1)/2);
+ for (int i = 0; i < numFlags; ++i)
+ for (int j = 0; j < numFlags; ++j)
+ ASSERT(flags[i][j] == (j < i ? 1 : 0));
+ count = 0;
+ clearFlags(flags);
+ executor.execute(task, Parallel2DExecutor::HalfPlusDiagonal);
+ ASSERT(count == numFlags*(numFlags+1)/2);
+ for (int i = 0; i < numFlags; ++i)
+ for (int j = 0; j < numFlags; ++j)
+ ASSERT(flags[i][j] == (j <= i ? 1 : 0));
+}
+
+int main() {
+ try {
+ testParallelExecution();
+ testSingleThreadedExecution();
+ } catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ cout << "Done" << endl;
+ return 0;
+}
diff --git a/SimTKcommon/tests/TestParallelExecutor.cpp b/SimTKcommon/tests/TestParallelExecutor.cpp
new file mode 100644
index 0000000..8abd28f
--- /dev/null
+++ b/SimTKcommon/tests/TestParallelExecutor.cpp
@@ -0,0 +1,104 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+using std::cout;
+using std::endl;
+using namespace SimTK;
+using namespace std;
+
+bool isParallel;
+
+class SetFlagTask : public ParallelExecutor::Task {
+public:
+ SetFlagTask(Array_<int>& flags, int& count) : flags(flags), count(count) {
+ }
+ void execute(int index) {
+ flags[index]++;
+ localCount.upd()++;
+ ASSERT(ParallelExecutor::isWorkerThread() == isParallel);
+ }
+ void initialize() {
+ localCount.upd() = 0;
+ ASSERT(ParallelExecutor::isWorkerThread() == isParallel);
+ }
+ void finish() {
+ count += localCount.get();
+ ASSERT(ParallelExecutor::isWorkerThread() == isParallel);
+ }
+private:
+ Array_<int>& flags;
+ int& count;
+ ThreadLocal<int> localCount;
+};
+
+void testParallelExecution() {
+ const int numFlags = 100;
+ Array_<int> flags(numFlags);
+ isParallel = (ParallelExecutor::getNumProcessors() > 1);
+ ParallelExecutor executor;
+ ASSERT(!ParallelExecutor::isWorkerThread());
+ for (int i = 0; i < 100; ++i) {
+ int count = 0;
+ SetFlagTask task(flags, count);
+ for (int j = 0; j < numFlags; ++j)
+ flags[j] = 0;
+ executor.execute(task, numFlags-10);
+ ASSERT(count == numFlags-10);
+ for (int j = 0; j < numFlags; ++j)
+ ASSERT(flags[j] == (j < numFlags-10 ? 1 : 0));
+ }
+ ASSERT(!ParallelExecutor::isWorkerThread());
+}
+
+void testSingleThreadedExecution() {
+ const int numFlags = 100;
+ Array_<int> flags(numFlags);
+ isParallel = false;
+ ParallelExecutor executor(1); // Specify only a single thread.
+ int count = 0;
+ SetFlagTask task(flags, count);
+ for (int j = 0; j < numFlags; ++j)
+ flags[j] = 0;
+ executor.execute(task, numFlags-10);
+ ASSERT(count == numFlags-10);
+ for (int j = 0; j < numFlags; ++j)
+ ASSERT(flags[j] == (j < numFlags-10 ? 1 : 0));
+}
+
+int main() {
+ try {
+ testParallelExecution();
+ testSingleThreadedExecution();
+ } catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ cout << "Done" << endl;
+ return 0;
+}
diff --git a/SimTKcommon/tests/TestParallelWorkQueue.cpp b/SimTKcommon/tests/TestParallelWorkQueue.cpp
new file mode 100644
index 0000000..874dea4
--- /dev/null
+++ b/SimTKcommon/tests/TestParallelWorkQueue.cpp
@@ -0,0 +1,68 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+using std::cout;
+using std::endl;
+using namespace SimTK;
+using namespace std;
+
+class SetFlagTask : public ParallelWorkQueue::Task {
+public:
+ SetFlagTask(Array_<int>& flags, int index) : flags(flags), index(index) {
+ }
+ void execute() {
+ ASSERT(!flags[index]);
+ flags[index] = true;
+ }
+private:
+ Array_<int>& flags;
+ int index;
+};
+
+void testParallelExecution() {
+ const int numFlags = 500;
+ Array_<int> flags(numFlags, false);
+ ParallelWorkQueue queue(10);
+ for (int i = 0; i < numFlags-10; i++)
+ queue.addTask(new SetFlagTask(flags, i));
+ queue.flush();
+ for (int i = 0; i < numFlags; i++)
+ ASSERT(flags[i] == (i < numFlags-10));
+}
+
+int main() {
+ try {
+ testParallelExecution();
+ } catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ cout << "Done" << endl;
+ return 0;
+}
diff --git a/SimTKcommon/tests/TestPlugin.cpp b/SimTKcommon/tests/TestPlugin.cpp
new file mode 100644
index 0000000..7f2216c
--- /dev/null
+++ b/SimTKcommon/tests/TestPlugin.cpp
@@ -0,0 +1,240 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "SimTKcommon/Testing.h"
+
+#include "SimTKcommon/internal/Plugin.h"
+
+#include <iostream>
+#include <string>
+#include <cstdio>
+using std::cout;
+using std::endl;
+using std::string;
+using std::printf;
+
+using namespace SimTK;
+
+class ExportedClass;
+class MyPlugin : public Plugin {
+public:
+ explicit MyPlugin(const std::string& name)
+ : Plugin(name) {
+ addSearchDirectory(Pathname::getInstallDir("SimTK_INSTALL_DIR", "SimTK")
+ + "/lib/plugins/");
+ }
+
+ SimTK_PLUGIN_DEFINE_FUNCTION1(int,sayHi,const std::string&);
+ SimTK_PLUGIN_DEFINE_FUNCTION(ExportedClass*,TestRuntimeDLL_makeExportedClass);
+ SimTK_PLUGIN_DEFINE_FUNCTION(void, NotPresent);
+
+private:
+};
+
+void testDeconstructFileName() {
+ string name, directory, libPrefix, baseName, debugSuffix, extension;
+ bool isAbsPath;
+ const std::string s = Pathname::getPathSeparator();
+ const std::string d = Pathname::getCurrentDriveLetter();
+ const std::string dd = d.empty() ? std::string() : d + ":";
+ const std::string cwd = Pathname::getCurrentWorkingDirectory();
+ const std::string xd = Pathname::getThisExecutableDirectory();
+
+
+ //printf("'%s': %s %s|%s|%s|%s|%s\n", name.c_str(),
+ // isAbsPath?"ABS":"REL", directory.c_str(), libPrefix.c_str(), baseName.c_str(),
+ // debugSuffix.c_str(), extension.c_str());
+
+
+ directory=libPrefix=baseName=debugSuffix=extension="junk";
+ name = " \t \n \r "; // Illegal because nothing but white space
+ SimTK_TEST(!Plugin::deconstructLibraryName(name,
+ isAbsPath, directory, libPrefix, baseName, debugSuffix, extension));
+ SimTK_TEST(!isAbsPath && directory.empty()&&libPrefix.empty()&&baseName.empty()
+ &&debugSuffix.empty()&&extension.empty());
+
+ directory=libPrefix=baseName=debugSuffix=extension="junk";
+ name = " This.Is.Not.A.Suffix_A.dylib "; // OK
+ SimTK_TEST(Plugin::deconstructLibraryName(name,
+ isAbsPath, directory, libPrefix, baseName, debugSuffix, extension));
+ SimTK_TEST(!isAbsPath && directory.empty() && libPrefix.empty()
+ && baseName=="This.Is.Not.A.Suffix_A"
+ && debugSuffix.empty() && extension==".dylib");
+
+ directory=libPrefix=baseName=debugSuffix=extension="junk";
+ name = "/usr/local/lib/"; // illegal because no file name
+ SimTK_TEST(!Plugin::deconstructLibraryName(name,
+ isAbsPath, directory, libPrefix, baseName, debugSuffix, extension));
+ SimTK_TEST(isAbsPath
+ && directory==(dd+s+"usr"+s+"local"+s+"lib"+s)
+ && libPrefix.empty()&&baseName.empty()
+ && debugSuffix.empty()&&extension.empty());
+
+ directory=libPrefix=baseName=debugSuffix=extension="junk";
+ name = "lib_D"; // illegal because nothing left after lib prefix and debug suffix
+ SimTK_TEST(!Plugin::deconstructLibraryName(name,
+ isAbsPath, directory, libPrefix, baseName, debugSuffix, extension));
+ SimTK_TEST(!isAbsPath && directory.empty()&&libPrefix=="lib"&&baseName.empty()
+ &&debugSuffix=="_D"&&extension.empty());
+
+ directory=libPrefix=baseName=debugSuffix=extension="junk";
+ name = "/mylibrary_D."; // OK (empty file type suffix)
+ SimTK_TEST(Plugin::deconstructLibraryName(name,
+ isAbsPath, directory, libPrefix, baseName, debugSuffix, extension));
+ SimTK_TEST(isAbsPath
+ && directory==(dd+s) && libPrefix.empty()&&baseName=="mylibrary"
+ && debugSuffix=="_D"&&extension==".");
+
+ directory=libPrefix=baseName=debugSuffix=extension="junk";
+ name = "./first/more/../../filename";
+ SimTK_TEST(Plugin::deconstructLibraryName(name,
+ isAbsPath, directory, libPrefix, baseName, debugSuffix, extension));
+ SimTK_TEST(isAbsPath
+ && directory==cwd
+ && libPrefix.empty()&&baseName=="filename"
+ && debugSuffix.empty()&&extension.empty());
+
+ directory=libPrefix=baseName=debugSuffix=extension="junk";
+ name = "@/../../filename"; // executable path-relative name
+ SimTK_TEST(Plugin::deconstructLibraryName(name,
+ isAbsPath, directory, libPrefix, baseName, debugSuffix, extension));
+
+ SimTK_TEST(isAbsPath
+ //&& directory==xd
+ && libPrefix.empty()&&baseName=="filename"
+ && debugSuffix.empty()&&extension.empty());
+
+ directory=libPrefix=baseName=debugSuffix=extension="junk";
+ name = "a"; // OK (very short file name)
+ SimTK_TEST(Plugin::deconstructLibraryName(name,
+ isAbsPath, directory, libPrefix, baseName, debugSuffix, extension));
+ SimTK_TEST(!isAbsPath && directory.empty()&&libPrefix.empty()&&baseName=="a"
+ &&debugSuffix.empty()&&extension.empty());
+
+#ifdef _WIN32
+ // Disk specifiers are only recognized on Windows.
+ directory=libPrefix=baseName=debugSuffix=extension="junk";
+ name = " c:\\Program Files\\lib\\libMyPlugIn_d.dll \n "; // OK
+ SimTK_TEST(Plugin::deconstructLibraryName(name,
+ isAbsPath, directory, libPrefix, baseName, debugSuffix, extension));
+ SimTK_TEST(isAbsPath &&
+ directory==("c:"+s+"Program Files"+s+"lib"+s) && libPrefix=="lib"
+ && baseName=="MyPlugIn" && debugSuffix=="_d" && extension==".dll");
+
+ directory=libPrefix=baseName=debugSuffix=extension="junk";
+ name = dd + "mydir/relative.txt"; // cwd relative to specified disk
+ SimTK_TEST(Plugin::deconstructLibraryName(name,
+ isAbsPath, directory, libPrefix, baseName, debugSuffix, extension));
+ SimTK_TEST(isAbsPath
+ && directory==(cwd + "mydir" + s)
+ && libPrefix.empty()&&baseName=="relative"
+ && debugSuffix.empty()&&extension==".txt");
+#endif
+}
+
+void testPathname() {
+#ifdef _WIN32
+ SimTK_TEST(Pathname::getPathSeparatorChar()=='\\');
+ SimTK_TEST(Pathname::getPathSeparator()=="\\");
+ SimTK_TEST(Pathname::getCurrentDriveLetter().size() == 1);
+ SimTK_TEST(Pathname::getCurrentDrive()==Pathname::getCurrentDriveLetter()+":");
+ SimTK_TEST(Pathname::getRootDirectory() == Pathname::getCurrentDrive() + "\\");
+#else
+ SimTK_TEST(Pathname::getPathSeparatorChar()=='/');
+ SimTK_TEST(Pathname::getPathSeparator()=="/");
+ SimTK_TEST(Pathname::getCurrentDriveLetter().size() == 0);
+ SimTK_TEST(Pathname::getCurrentDrive()=="");
+ SimTK_TEST(Pathname::getRootDirectory() == "/");
+#endif
+
+ std::string name;
+ bool isAbsPath;
+ std::string directory, fileName, extension;
+ const std::string curDrive =
+ Pathname::getCurrentDriveLetter().empty()
+ ? std::string()
+ : Pathname::getCurrentDriveLetter()+":";
+ const std::string sep = Pathname::getPathSeparator();
+
+ directory=fileName=extension="junk"; isAbsPath=false;
+ name = "/topdir/seconddir/myFileName.ext";
+ Pathname::deconstructPathname(name, isAbsPath, directory, fileName, extension);
+ SimTK_TEST(isAbsPath
+ && directory==curDrive+sep+"topdir"+sep+"seconddir"+sep
+ && fileName=="myFileName" && extension==".ext");
+
+ directory=fileName=extension="junk"; isAbsPath=true;
+ name = "topdir/seconddir/myFileName.ext";
+ Pathname::deconstructPathname(name, isAbsPath, directory, fileName, extension);
+ SimTK_TEST(!isAbsPath
+ && directory=="topdir"+sep+"seconddir"+sep
+ && fileName=="myFileName" && extension==".ext");
+}
+
+void testPlugin() {
+ MyPlugin myPlug("c:\\temp\\TestRuntimeDLL_d.dll");
+
+ if (myPlug.load()) {
+ std::cout << "fellas length is " << myPlug.sayHi("fellas") << std::endl;
+ myPlug.TestRuntimeDLL_makeExportedClass();
+ } else
+ std::cout << "Plugin load failed, err=" << myPlug.getLastErrorMessage() << std::endl;
+
+ SimTK_TEST_MUST_THROW(myPlug.NotPresent());
+
+ if (myPlug.isLoaded()) {
+ SimTK_TEST(myPlug.has_TestRuntimeDLL_makeExportedClass());
+ SimTK_TEST(!myPlug.has_NotPresent());
+ }
+
+ myPlug.unload();
+ myPlug.addSearchDirectory("@");
+ myPlug.addSearchDirectory(".");
+ myPlug.addSearchDirectory("..");
+ myPlug.addSearchDirectory("/temp");
+ myPlug.addSearchDirectory("c:/temp");
+ myPlug.load("TestRuntimeDLL.so");
+}
+
+int main() {
+ cout << "Path of this executable: '" << Pathname::getThisExecutablePath() << "'\n";
+ cout << "Executable directory: '" << Pathname::getThisExecutableDirectory() << "'\n";
+ cout << "Current working directory: '" << Pathname::getCurrentWorkingDirectory() << "'\n";
+ cout << "Current drive letter: '" << Pathname::getCurrentDriveLetter() << "'\n";
+ cout << "Current drive: '" << Pathname::getCurrentDrive() << "'\n";
+ cout << "Path separator: '" << Pathname::getPathSeparator() << "'\n";
+ cout << "Default install dir: '" << Pathname::getDefaultInstallDir() << "'\n";
+ cout << "installDir(SimTK_INSTALL_DIR,/LunchTime/snacks): '"
+ << Pathname::getInstallDir("SimTK_INSTALL_DIR", "/LunchTime/snacks") << "'\n";
+ cout << "installDir(LocalAppData,SimTK): '"
+ << Pathname::getInstallDir("LocalAppData", "SimTK") << "'\n";
+ SimTK_START_TEST("TestPlugin");
+
+ SimTK_SUBTEST(testPathname);
+ SimTK_SUBTEST(testDeconstructFileName);
+ SimTK_SUBTEST(testPlugin);
+
+ SimTK_END_TEST();
+}
+
diff --git a/SimTKcommon/tests/TestPolygonalMesh.cpp b/SimTKcommon/tests/TestPolygonalMesh.cpp
new file mode 100644
index 0000000..052b511
--- /dev/null
+++ b/SimTKcommon/tests/TestPolygonalMesh.cpp
@@ -0,0 +1,122 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+using std::cout;
+using std::endl;
+using namespace SimTK;
+using namespace std;
+
+void testCreateMesh() {
+ PolygonalMesh mesh;
+ ASSERT(mesh.getNumFaces() == 0);
+ ASSERT(mesh.getNumVertices() == 0);
+ ASSERT(mesh.addVertex(Vec3(0)) == 0);
+ ASSERT(mesh.addVertex(Vec3(0, 1, 0)) == 1);
+ ASSERT(mesh.addVertex(Vec3(0, 0, 1)) == 2);
+ Array_<int> v;
+ v.push_back(1);
+ v.push_back(2);
+ v.push_back(0);
+ ASSERT(mesh.addFace(v) == 0);
+ ASSERT(mesh.getNumFaces() == 1);
+ ASSERT(mesh.getNumVertices() == 3);
+ ASSERT(mesh.getFaceVertex(0, 0) == 1);
+ ASSERT(mesh.getFaceVertex(0, 1) == 2);
+ ASSERT(mesh.getFaceVertex(0, 2) == 0);
+ ASSERT(mesh.getVertexPosition(0) == Vec3(0));
+ ASSERT(mesh.getVertexPosition(1) == Vec3(0, 1, 0));
+ ASSERT(mesh.getVertexPosition(2) == Vec3(0, 0, 1));
+
+ // Make sure copy and assignment are shallow.
+ PolygonalMesh mesh2(mesh); // shallow copy construction
+ ASSERT(&mesh2.getImpl() == &mesh.getImpl());
+ ASSERT(mesh.getImplHandleCount()==2);
+
+ PolygonalMesh mesh3;
+ mesh3 = mesh; // shallow assignment
+ ASSERT(&mesh3.getImpl() == &mesh.getImpl());
+ ASSERT(mesh.getImplHandleCount()==3);
+
+ PolygonalMesh mesh4;
+ mesh4.copyAssign(mesh); // deep copy
+ ASSERT(mesh4.getNumVertices() == mesh.getNumVertices());
+ ASSERT(&mesh4.getImpl() != &mesh.getImpl());
+ ASSERT(mesh4.getImplHandleCount()==1);
+ ASSERT(mesh.getImplHandleCount()==3);
+}
+
+void testLoadObjFile() {
+ string file;
+ file += "# This is a comment\n";
+ file += "v -1.0 1.0 2.0\n";
+ file += "v -2.0 2.0 3.0\n";
+ file += "v -3.0 3.0 \\\n";
+ file += "4.0\n";
+ file += "v -4.0 4.0 5.0\n";
+ file += "v -5.0 5.0 6.0\n";
+ file += "f 1 2 3\n";
+ file += "f 2// 3/4/5 4//2\n";
+ file += "f -3 -2/3/4 -1\n";
+ file += "f 1 3\\\n";
+ file += "5 2\n";
+ PolygonalMesh mesh;
+ stringstream stream(file);
+ mesh.loadObjFile(stream);
+ ASSERT(mesh.getNumVertices() == 5);
+ ASSERT(mesh.getNumFaces() == 4);
+ for (int i = 0; i < mesh.getNumVertices(); i++) {
+ const Vec3& pos = mesh.getVertexPosition(i);
+ ASSERT(pos[0] == -(i+1));
+ ASSERT(pos[1] == i+1);
+ ASSERT(pos[2] == i+2);
+ }
+ for (int i = 0; i < 3; i++) {
+ ASSERT(mesh.getNumVerticesForFace(i) == 3);
+ ASSERT(mesh.getFaceVertex(i, 0) == i);
+ ASSERT(mesh.getFaceVertex(i, 1) == i+1);
+ ASSERT(mesh.getFaceVertex(i, 2) == i+2);
+ }
+ ASSERT(mesh.getNumVerticesForFace(3) == 4);
+ ASSERT(mesh.getFaceVertex(3, 0) == 0);
+ ASSERT(mesh.getFaceVertex(3, 1) == 2);
+ ASSERT(mesh.getFaceVertex(3, 2) == 4);
+ ASSERT(mesh.getFaceVertex(3, 3) == 1);
+}
+
+int main() {
+ try {
+ testCreateMesh();
+ testLoadObjFile();
+ } catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ cout << "Done" << endl;
+ return 0;
+}
diff --git a/SimTKcommon/tests/TestPrivateImplementation.cpp b/SimTKcommon/tests/TestPrivateImplementation.cpp
new file mode 100644
index 0000000..d936076
--- /dev/null
+++ b/SimTKcommon/tests/TestPrivateImplementation.cpp
@@ -0,0 +1,398 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "SimTKcommon/internal/PrivateImplementation.h"
+// we'll include PrivateImplementation_Defs.h later to make sure
+// none of the client-visible stuff is dependent on it.
+
+#include <iostream>
+using std::cout; using std::endl;
+
+#include <string>
+using std::string;
+
+using namespace SimTK;
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+
+ /////////////////
+ // CLIENT SIDE //
+ /////////////////
+
+// The Handles defined here represent ones that would be present in
+// user-includable header files. They should contain no hint of their
+// implementations, except for a forward-declared name!
+
+// This is a simple, concrete class which has been split into a
+// Handle and an Implementation. It has ordinary object (value)
+// semantics meaning that copy construction an copy assignment
+// produce independent objects, not an additional reference to an
+// existing object.
+
+class Concrete_Impl;
+class Concrete : public PIMPLHandle<Concrete, Concrete_Impl> {
+public:
+ Concrete(); // default constructor allocates an implementation
+
+ // The implementation object can store a string.
+ void setString(const string n);
+ const string& getString() const;
+};
+
+// This is the identical class except it has reference semantics.
+class ConcreteRef_Impl;
+class ConcreteRef : public PIMPLHandle<ConcreteRef, ConcreteRef_Impl, true> {
+public:
+ ConcreteRef(); // default constructor allocates an implementation
+
+ // The implementation object can store a string.
+ void setString(const string n);
+ const string& getString() const;
+};
+
+
+class MyHandle_Impl;
+class MyHandle : public PIMPLHandle<MyHandle, MyHandle_Impl, false> {
+public:
+ MyHandle();
+ explicit MyHandle(MyHandle_Impl* p) : HandleBase(p) { }
+
+ void setName(const string n);
+ const string& getName() const;
+};
+std::ostream& operator<<(std::ostream& o, const MyHandle& h) {
+ o << "MyHandle ";
+ if (h.isEmptyHandle()) return o << "IS EMPTY\n";
+
+ o << "(name=" << h.getName() << "):";
+ return o << static_cast<const MyHandle::ParentHandle&>(h);
+}
+
+class DerivedHandle_Impl;
+class DerivedHandle : public MyHandle {
+public:
+ DerivedHandle();
+ explicit DerivedHandle(DerivedHandle_Impl*);
+
+ void setReal(Real);
+ Real getReal() const;
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(DerivedHandle,DerivedHandle_Impl,MyHandle);
+};
+std::ostream& operator<<(std::ostream& o, const DerivedHandle& h) {
+ o << "DerivedHandle ";
+ if (h.isEmptyHandle()) return o << "IS EMPTY\n";
+
+ o << "(real=" << h.getReal() << "):";
+ return o << h.upcast();
+}
+
+class DerDerivedHandle_Impl;
+class DerDerivedHandle : public DerivedHandle {
+public:
+ DerDerivedHandle();
+ explicit DerDerivedHandle(DerDerivedHandle_Impl*);
+
+ void setString(string);
+ string getString() const;
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(DerDerivedHandle,DerDerivedHandle_Impl,DerivedHandle);
+};
+std::ostream& operator<<(std::ostream& o, const DerDerivedHandle& h) {
+ o << "DerDerivedHandle ";
+ if (h.isEmptyHandle()) return o << "IS EMPTY\n";
+
+ o << "(string=" << h.getString() << "):";
+ return o << h.upcast();
+}
+
+
+int main() {
+ try
+ {
+ Concrete c1, c2;
+ ConcreteRef cr1, cr2;
+
+ // Handles should consist precisely of one pointer.
+ ASSERT(sizeof(c1) == sizeof(void*));
+ ASSERT(sizeof(cr1) == sizeof(void*));
+
+ ASSERT(c1.getImplHandleCount()==1);
+ ASSERT(c2.getImplHandleCount()==1);
+ ASSERT(cr1.getImplHandleCount()==1);
+ ASSERT(cr2.getImplHandleCount()==1);
+
+ ASSERT(!c1.isEmptyHandle());
+ ASSERT(c2.isOwnerHandle());
+ ASSERT(!c1.isSameHandle(c2));
+ ASSERT(c2.isSameHandle(c2));
+
+ ASSERT(!cr1.isEmptyHandle());
+ ASSERT(cr2.isOwnerHandle());
+ ASSERT(!cr1.isSameHandle(cr2));
+ ASSERT(cr2.isSameHandle(cr2));
+
+ c1.setString("c1"); cr1.setString("cr1");
+ c2.setString("c2"); cr2.setString("cr2");
+ ASSERT(c1.getString() == "c1");
+ ASSERT(c2.getString() == "c2");
+ ASSERT(cr1.getString() == "cr1");
+ ASSERT(cr2.getString() == "cr2");
+
+ Concrete copyOfc2(c2); ConcreteRef copyOfcr2(cr2);
+ Concrete copyOfc2ViaAssign; ConcreteRef copyOfcr2ViaAssign;
+ copyOfc2ViaAssign = c2;
+ copyOfc2ViaAssign.clearHandle();
+ copyOfc2ViaAssign = c2;
+
+ ASSERT(c2.getImplHandleCount()==1);
+ ASSERT(copyOfc2.getImplHandleCount()==1);
+ ASSERT(copyOfc2ViaAssign.getImplHandleCount()==1);
+
+ ASSERT(cr2.getImplHandleCount()==2);
+ ASSERT(copyOfcr2.getImplHandleCount()==2);
+
+ // can't do reference assign to an owner handle
+ copyOfcr2ViaAssign.clearHandle();
+ copyOfcr2ViaAssign = cr2;
+
+ ASSERT(copyOfc2.isOwnerHandle() && copyOfc2ViaAssign.isOwnerHandle());
+ ASSERT(!copyOfcr2.isOwnerHandle() && !copyOfcr2ViaAssign.isOwnerHandle());
+ ASSERT(cr2.getImplHandleCount()==3);
+ ASSERT(copyOfcr2.getImplHandleCount()==3);
+ ASSERT(copyOfcr2ViaAssign.getImplHandleCount()==3);
+
+
+ ASSERT(&c2.getImpl() != ©Ofc2.getImpl());
+ ASSERT(&c2.getImpl() != ©Ofc2ViaAssign.getImpl());
+ ASSERT(&cr2.getImpl() == ©Ofcr2.getImpl());
+ ASSERT(&cr2.getImpl() == ©Ofcr2ViaAssign.getImpl());
+
+
+ ASSERT(copyOfc2.getString() == c2.getString());
+ ASSERT(copyOfc2ViaAssign.getString() == c2.getString());
+ ASSERT(copyOfcr2.getString() == cr2.getString());
+ ASSERT(copyOfcr2ViaAssign.getString() == cr2.getString());
+
+ copyOfc2.setString("copyOfc2");
+ ASSERT(c2.getString() == "c2" && copyOfc2.getString() == "copyOfc2");
+ ASSERT(copyOfc2ViaAssign.getString() == "c2");
+
+ copyOfcr2.setString("copyOfcr2");
+ ASSERT(cr2.getString() == "copyOfcr2");
+ ASSERT(copyOfcr2ViaAssign.getString() == "copyOfcr2");
+
+
+ copyOfc2ViaAssign.setString("copyOfc2ViaAssign");
+ ASSERT(copyOfc2ViaAssign.getString() == "copyOfc2ViaAssign");
+ ASSERT(c2.getString() == "c2" && copyOfc2.getString() == "copyOfc2");
+
+ c1.clearHandle(); ASSERT(c1.isEmptyHandle()); ASSERT(!c1.isOwnerHandle());
+ cr1.clearHandle(); ASSERT(cr1.isEmptyHandle()); ASSERT(!cr1.isOwnerHandle());
+
+ c1.referenceAssign(c2); // now c1 & c2 point to same impl
+ cr1.copyAssign(cr2); // while cr1 & cr2 point to different implementations
+ ASSERT(&c1.getImpl() == &c2.getImpl()); ASSERT(c1.getImplHandleCount() == 2);
+ ASSERT(&cr1.getImpl() != &cr2.getImpl()); ASSERT(cr1.getImplHandleCount() == 1);
+ ASSERT(cr2.getImplHandleCount() == 3);
+ c1.setString("setThroughC1"); cr1.setString("setThroughCR1");
+
+ ASSERT(c1.getString() == "setThroughC1" && c2.getString() == "setThroughC1");
+ ASSERT(cr1.getString() == "setThroughCR1" && cr2.getString() == "copyOfcr2");
+
+ // TODO: regression tests for hierarchical handle classes
+
+ // TODO: regression tests for client-side custom implementations
+
+ // TODO: code below is not yet in regression test form. However, these should not
+ // thrown any exceptions.
+ MyHandle h;
+ h.setName("Fred");
+ cout << "h=" << h;
+
+ // Make empty handle
+ MyHandle empty(0);
+ h.disown(empty);
+ cout << "after disown h=" << h;
+ cout << " empty=" << empty;
+
+ MyHandle h2 = h;
+ cout << "h2=" << h2;
+
+ DerivedHandle hderived;
+ hderived.setName("Joe");
+ cout << "hderived=" << hderived;
+
+ MyHandle h3 = hderived;
+ cout << "h3=" << h3;
+ cout << "downcast h3.getReal()=" << DerivedHandle::downcast(h3).getReal() << endl;
+
+ DerivedHandle d2(hderived);
+ cout << "d2=" << d2;
+
+ DerivedHandle dempty(0);
+ cout << "dempty=" << dempty;
+
+ dempty = DerivedHandle::downcast(d2);
+ cout << "dempty=h, dempty=" << dempty;
+
+ DerDerivedHandle dd; dd.setString("hi there");
+ cout << "dd=" << dd;
+
+ cout << "dd.upcast() =" << dd.upcast();
+ cout << "dd.upcast^2()= "<< dd.upcast().upcast();
+
+ cout << "sizeof(MyHandle)=" << sizeof(MyHandle)
+ << " sizeof(DerivedHandle)=" << sizeof(DerivedHandle)
+ << " sizeof(DerDerivedHandle)=" << sizeof(DerDerivedHandle)
+ << endl;
+
+
+ }
+ catch (const std::exception& e) {
+ printf("EXCEPTION THROWN: %s\n", e.what());
+ return 1;
+ }
+ catch (...) {
+ printf("UNKNOWN EXCEPTION THROWN\n");
+ return 1;
+ }
+
+ return 0;
+}
+
+
+
+ //////////////////
+ // LIBRARY SIDE //
+ //////////////////
+
+// Code here would normally be in some other compilation unit and available
+// only as a binary.
+
+#include "SimTKcommon/internal/PrivateImplementation_Defs.h"
+
+ // CONCRETE IMPL //
+class Concrete_Impl : public PIMPLImplementation<Concrete,Concrete_Impl> {
+public:
+ Concrete_Impl* clone() const {return new Concrete_Impl(*this);}
+ string s;
+ friend class Concrete;
+};
+
+namespace SimTK {
+template class PIMPLHandle<Concrete,Concrete_Impl>;
+template class PIMPLImplementation<Concrete,Concrete_Impl>;
+}
+
+ // CONCRETE REF IMPL //
+class ConcreteRef_Impl : public PIMPLImplementation<ConcreteRef,ConcreteRef_Impl> {
+public:
+ ConcreteRef_Impl* clone() const {return new ConcreteRef_Impl(*this);}
+ string s;
+ friend class ConcreteRef;
+};
+
+namespace SimTK {
+template class PIMPLHandle<ConcreteRef,ConcreteRef_Impl,true>;
+template class PIMPLImplementation<ConcreteRef,ConcreteRef_Impl>;
+}
+
+ // CONCRETE & CONCRETE REF HANDLE IMPLEMENTATIONS //
+Concrete::Concrete() : HandleBase(new Concrete_Impl()) { }
+ConcreteRef::ConcreteRef() : HandleBase(new ConcreteRef_Impl()) { }
+
+void Concrete::setString(const string n) {updImpl().s = n;}
+void ConcreteRef::setString(const string n) {updImpl().s = n;}
+
+const string& Concrete::getString() const {return getImpl().s;}
+const string& ConcreteRef::getString() const {return getImpl().s;}
+
+
+ // OTHER IMPLs and HANDLE IMPLEMENTATIONS //
+
+class MyHandle_Impl : public PIMPLImplementation<MyHandle,MyHandle_Impl> {
+public:
+ virtual ~MyHandle_Impl() { }
+ virtual MyHandle_Impl* clone() const {return new MyHandle_Impl(*this);}
+
+private:
+ string name;
+ friend class MyHandle;
+};
+
+namespace SimTK {
+template class PIMPLHandle<MyHandle,MyHandle_Impl>;
+template class PIMPLImplementation<MyHandle,MyHandle_Impl>;
+}
+
+class DerivedHandle_Impl : public MyHandle_Impl {
+public:
+ virtual DerivedHandle_Impl* clone() const {return new DerivedHandle_Impl(*this);}
+
+private:
+ Real r;
+ friend class DerivedHandle;
+};
+
+class DerDerivedHandle_Impl : public DerivedHandle_Impl {
+public:
+ virtual DerDerivedHandle_Impl* clone() const {return new DerDerivedHandle_Impl(*this);}
+
+private:
+ string s;
+ friend class DerDerivedHandle;
+};
+
+MyHandle::MyHandle() : HandleBase(new MyHandle_Impl()) {
+}
+
+void MyHandle::setName(const string n) {
+ updImpl().name = n;
+}
+
+const string& MyHandle::getName() const {
+ return getImpl().name;
+}
+
+DerivedHandle::DerivedHandle() : MyHandle(new DerivedHandle_Impl()) {
+ setReal(27);
+}
+
+DerivedHandle::DerivedHandle(DerivedHandle_Impl* p) : MyHandle(p) { }
+
+void DerivedHandle::setReal(Real rr) {updImpl().r=rr;}
+Real DerivedHandle::getReal() const {return getImpl().r;}
+
+DerDerivedHandle::DerDerivedHandle() : DerivedHandle(new DerDerivedHandle_Impl()) {
+ setString("default dd string");
+ setReal(22.345);
+}
+DerDerivedHandle::DerDerivedHandle(DerDerivedHandle_Impl* p) : DerivedHandle(p) { }
+
+void DerDerivedHandle::setString(string s) {updImpl().s=s;}
+string DerDerivedHandle::getString() const {return getImpl().s;}
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(DerivedHandle,DerivedHandle_Impl,MyHandle);
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(DerDerivedHandle,DerDerivedHandle_Impl,DerivedHandle);
diff --git a/SimTKcommon/tests/TestScalar.cpp b/SimTKcommon/tests/TestScalar.cpp
new file mode 100644
index 0000000..22d4872
--- /dev/null
+++ b/SimTKcommon/tests/TestScalar.cpp
@@ -0,0 +1,766 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "SimTKcommon/Testing.h"
+
+#include <iostream>
+using std::cout;
+using std::endl;
+
+using namespace SimTK;
+
+
+void testIsNaN() {
+ const float fltRegular = -12.34f;
+ const double dblRegular = -12.34;
+ const float fltNaN = NTraits<float>::getNaN();
+ const double dblNaN = NTraits<double>::getNaN();
+ const float nfltNaN = -fltNaN;
+ const double ndblNaN = -dblNaN;
+
+ SimTK_TEST(isNaN(fltNaN) && isNaN(dblNaN));
+ SimTK_TEST(isNaN(nfltNaN) && isNaN(ndblNaN));
+ SimTK_TEST(!isNaN(fltRegular) && !isNaN(dblRegular));
+
+ std::complex<float> cflt(fltRegular, -2*fltRegular);
+ std::complex<double> cdbl(dblRegular, -2*dblRegular);
+ conjugate<float> cjflt(fltRegular, -2*fltRegular);
+ conjugate<double> cjdbl(dblRegular, -2*dblRegular);
+
+ SimTK_TEST(!isNaN(cflt) && !isNaN(cdbl));
+ SimTK_TEST(!isNaN(cjflt) && !isNaN(cjdbl));
+
+ // Reference the same memory as a negator of its contents.
+ const negator<float>& nflt = reinterpret_cast<const negator<float>&>(fltRegular);
+ const negator<double>& ndbl = reinterpret_cast<const negator<double>&>(dblRegular);
+ negator<std::complex<float> >& ncflt = reinterpret_cast<negator<std::complex<float> >&> (cflt);
+ negator<std::complex<double> >& ncdbl = reinterpret_cast<negator<std::complex<double> >&>(cdbl);
+ negator<conjugate<float> >& ncjflt = reinterpret_cast<negator<conjugate<float> >&> (cjflt);
+ negator<conjugate<double> >& ncjdbl = reinterpret_cast<negator<conjugate<double> >&> (cjdbl);
+
+ // Test that negators are working properly.
+ SimTK_TEST_EQ(nflt, -fltRegular);
+ SimTK_TEST_EQ(ndbl, -dblRegular);
+ SimTK_TEST_EQ(ncflt, -cflt);
+ SimTK_TEST_EQ(-ncflt, cflt);
+ SimTK_TEST_EQ(ncjflt, -cjflt);
+ SimTK_TEST_EQ(-ncjflt, cjflt);
+
+ SimTK_TEST(!isNaN(nflt) && !isNaN(ndbl));
+ SimTK_TEST(!isNaN(ncflt) && !isNaN(ncdbl));
+ SimTK_TEST(!isNaN(ncjflt) && !isNaN(ncjdbl));
+
+ // Should be NaN if either or both parts are NaN.
+ cflt = std::complex<float>(cflt.real(), fltNaN);
+ cdbl = std::complex<double>(cdbl.real(), dblNaN);
+ cjflt = conjugate<float>(cjflt.real(), fltNaN);
+ cjdbl = conjugate<double>(cjdbl.real(), dblNaN);
+
+ // Imaginary only is NaN.
+ SimTK_TEST(isNaN(cflt) && isNaN(cdbl));
+ SimTK_TEST(isNaN(cjflt) && isNaN(cjdbl));
+ SimTK_TEST(isNaN(ncflt) && isNaN(ncdbl));
+ SimTK_TEST(isNaN(ncjflt) && isNaN(ncjdbl));
+
+ cflt = std::complex<float>(fltNaN, cflt.imag());
+ cdbl = std::complex<double>(dblNaN, cdbl.imag());
+ cjflt = conjugate<float>(fltNaN, cjflt.imag());
+ cjdbl = conjugate<double>(dblNaN, cjdbl.imag());
+
+ // Both parts are NaN.
+ SimTK_TEST(isNaN(cflt) && isNaN(cdbl));
+ SimTK_TEST(isNaN(cjflt) && isNaN(cjdbl));
+ SimTK_TEST(isNaN(ncflt) && isNaN(ncdbl));
+ SimTK_TEST(isNaN(ncjflt) && isNaN(ncjdbl));
+
+ // Restore imaginary part to normal.
+ cflt = std::complex<float>(cflt.real(), fltRegular);
+ cdbl = std::complex<double>(cdbl.real(), dblRegular);
+ cjflt = conjugate<float>(cjflt.real(), fltRegular);
+ cjdbl = conjugate<double>(cjdbl.real(), dblRegular);
+
+ // Real part only is NaN;
+ SimTK_TEST(isNaN(cflt) && isNaN(cdbl));
+ SimTK_TEST(isNaN(cjflt) && isNaN(cjdbl));
+ SimTK_TEST(isNaN(ncflt) && isNaN(ncdbl));
+ SimTK_TEST(isNaN(ncjflt) && isNaN(ncjdbl));
+}
+
+void testIsInf() {
+ const float fltRegular = -12.34f;
+ const double dblRegular = -12.34;
+ const float fltInf = NTraits<float>::getInfinity();
+ const double dblInf = NTraits<double>::getInfinity();
+ const float mfltInf = -fltInf;
+ const double mdblInf = -dblInf;
+ const negator<float>& nfltInf = reinterpret_cast<const negator<float>&>(fltInf);
+ const negator<double>& ndblInf = reinterpret_cast<const negator<double>&>(dblInf);
+
+ SimTK_TEST(nfltInf == -fltInf);
+ SimTK_TEST(ndblInf == -dblInf);
+
+ SimTK_TEST(isInf(fltInf) && isInf(dblInf));
+ SimTK_TEST(isInf(mfltInf) && isInf(mdblInf));
+ SimTK_TEST(isInf(nfltInf) && isInf(ndblInf));
+ SimTK_TEST(!isInf(fltRegular) && !isInf(dblRegular));
+
+ std::complex<float> cflt(fltRegular, -2*fltRegular);
+ std::complex<double> cdbl(dblRegular, -2*dblRegular);
+ conjugate<float> cjflt(fltRegular, -2*fltRegular);
+ conjugate<double> cjdbl(dblRegular, -2*dblRegular);
+
+ SimTK_TEST(!isInf(cflt) && !isInf(cdbl));
+ SimTK_TEST(!isInf(cjflt) && !isInf(cjdbl));
+
+ // Reference the same memory as a negator of its contents.
+ const negator<float>& nflt = reinterpret_cast<const negator<float>&>(fltRegular);
+ const negator<double>& ndbl = reinterpret_cast<const negator<double>&>(dblRegular);
+ negator<std::complex<float> >& ncflt = reinterpret_cast<negator<std::complex<float> >&> (cflt);
+ negator<std::complex<double> >& ncdbl = reinterpret_cast<negator<std::complex<double> >&>(cdbl);
+ negator<conjugate<float> >& ncjflt = reinterpret_cast<negator<conjugate<float> >&> (cjflt);
+ negator<conjugate<double> >& ncjdbl = reinterpret_cast<negator<conjugate<double> >&> (cjdbl);
+
+ // Test that negators are working properly.
+ SimTK_TEST_EQ(nflt, -fltRegular);
+ SimTK_TEST_EQ(ndbl, -dblRegular);
+ SimTK_TEST_EQ(ncflt, -cflt);
+ SimTK_TEST_EQ(-ncflt, cflt);
+ SimTK_TEST_EQ(ncjflt, -cjflt);
+ SimTK_TEST_EQ(-ncjflt, cjflt);
+
+ SimTK_TEST(!isInf(nflt) && !isInf(ndbl));
+ SimTK_TEST(!isInf(ncflt) && !isInf(ncdbl));
+ SimTK_TEST(!isInf(ncjflt) && !isInf(ncjdbl));
+
+ // Should be Inf if either or both parts are Inf, as long as neither
+ // part is NaN.
+ cflt = std::complex<float>(cflt.real(), fltInf);
+ cdbl = std::complex<double>(cdbl.real(), dblInf);
+ cjflt = conjugate<float>(cjflt.real(), fltInf);
+ cjdbl = conjugate<double>(cjdbl.real(), dblInf);
+
+ // Imaginary only is Inf.
+ SimTK_TEST(isInf(cflt) && isInf(cdbl));
+ SimTK_TEST(isInf(cjflt) && isInf(cjdbl));
+ SimTK_TEST(isInf(ncflt) && isInf(ncdbl));
+ SimTK_TEST(isInf(ncjflt) && isInf(ncjdbl));
+
+ cflt = std::complex<float>(fltInf, cflt.imag());
+ cdbl = std::complex<double>(dblInf, cdbl.imag());
+ cjflt = conjugate<float>(fltInf, cjflt.imag());
+ cjdbl = conjugate<double>(dblInf, cjdbl.imag());
+
+ // Both parts are Inf.
+ SimTK_TEST(isInf(cflt) && isInf(cdbl));
+ SimTK_TEST(isInf(cjflt) && isInf(cjdbl));
+ SimTK_TEST(isInf(ncflt) && isInf(ncdbl));
+ SimTK_TEST(isInf(ncjflt) && isInf(ncjdbl));
+
+ // Restore imaginary part to normal.
+ cflt = std::complex<float>(cflt.real(), fltRegular);
+ cdbl = std::complex<double>(cdbl.real(), dblRegular);
+ cjflt = conjugate<float>(cjflt.real(), fltRegular);
+ cjdbl = conjugate<double>(cjdbl.real(), dblRegular);
+
+ // Real part only is Inf;
+ SimTK_TEST(isInf(cflt) && isInf(cdbl));
+ SimTK_TEST(isInf(cjflt) && isInf(cjdbl));
+ SimTK_TEST(isInf(ncflt) && isInf(ncdbl));
+ SimTK_TEST(isInf(ncjflt) && isInf(ncjdbl));
+
+ // Set real part to minus infinity.
+ cflt = std::complex<float>(mfltInf, cflt.imag());
+ cdbl = std::complex<double>(mdblInf, cdbl.imag());
+ cjflt = conjugate<float>(mfltInf, cjflt.imag());
+ cjdbl = conjugate<double>(mdblInf, cjdbl.imag());
+
+ SimTK_TEST(isInf(cflt) && isInf(cdbl));
+ SimTK_TEST(isInf(cjflt) && isInf(cjdbl));
+ SimTK_TEST(isInf(ncflt) && isInf(ncdbl));
+ SimTK_TEST(isInf(ncjflt) && isInf(ncjdbl));
+
+ // Set real part to NaN.
+ const float fltNaN = NTraits<float>::getNaN();
+ const double dblNaN = NTraits<double>::getNaN();
+ cflt = std::complex<float>(fltNaN, cflt.imag());
+ cdbl = std::complex<double>(dblNaN, cdbl.imag());
+ cjflt = conjugate<float>(fltNaN, cjflt.imag());
+ cjdbl = conjugate<double>(dblNaN, cjdbl.imag());
+
+ SimTK_TEST(!isInf(cflt) && !isInf(cdbl));
+ SimTK_TEST(!isInf(cjflt) && !isInf(cjdbl));
+ SimTK_TEST(!isInf(ncflt) && !isInf(ncdbl));
+ SimTK_TEST(!isInf(ncjflt) && !isInf(ncjdbl));
+}
+
+void testIsFinite() {
+ const float fltRegular = -12.34f;
+ const double dblRegular = -12.34;
+ const float fltNaN = NTraits<float>::getNaN();
+ const double dblNaN = NTraits<double>::getNaN();
+ const float nfltNaN = -fltNaN;
+ const double ndblNaN = -dblNaN;
+ const float fltInf = NTraits<float>::getInfinity();
+ const double dblInf = NTraits<double>::getInfinity();
+ const float mfltInf = -fltInf;
+ const double mdblInf = -dblInf;
+
+ SimTK_TEST(isFinite(fltRegular) && isFinite(dblRegular));
+ SimTK_TEST(!isFinite(fltNaN) && !isFinite(dblNaN));
+ SimTK_TEST(!isFinite(fltInf) && !isFinite(dblInf));
+ SimTK_TEST(!isFinite(mfltInf) && !isFinite(mdblInf));
+
+ std::complex<float> cflt(fltRegular, -2*fltRegular);
+ std::complex<double> cdbl(dblRegular, -2*dblRegular);
+ conjugate<float> cjflt(fltRegular, -2*fltRegular);
+ conjugate<double> cjdbl(dblRegular, -2*dblRegular);
+
+ SimTK_TEST(isFinite(cflt) && isFinite(cdbl));
+ SimTK_TEST(isFinite(cjflt) && isFinite(cjdbl));
+
+ // Reference the same memory as a negator of its contents.
+ const negator<float>& nflt = reinterpret_cast<const negator<float>&>(fltRegular);
+ const negator<double>& ndbl = reinterpret_cast<const negator<double>&>(dblRegular);
+ negator<std::complex<float> >& ncflt = reinterpret_cast<negator<std::complex<float> >&> (cflt);
+ negator<std::complex<double> >& ncdbl = reinterpret_cast<negator<std::complex<double> >&>(cdbl);
+ negator<conjugate<float> >& ncjflt = reinterpret_cast<negator<conjugate<float> >&> (cjflt);
+ negator<conjugate<double> >& ncjdbl = reinterpret_cast<negator<conjugate<double> >&> (cjdbl);
+
+ // Test that negators are working properly.
+ SimTK_TEST_EQ(nflt, -fltRegular);
+ SimTK_TEST_EQ(ndbl, -dblRegular);
+ SimTK_TEST_EQ(ncflt, -cflt);
+ SimTK_TEST_EQ(-ncflt, cflt);
+ SimTK_TEST_EQ(ncjflt, -cjflt);
+ SimTK_TEST_EQ(-ncjflt, cjflt);
+
+ SimTK_TEST(isFinite(nflt) && isFinite(ndbl));
+ SimTK_TEST(isFinite(ncflt) && isFinite(ncdbl));
+ SimTK_TEST(isFinite(ncjflt) && isFinite(ncjdbl));
+
+ // Should be finite only if both parts are finite.
+ cflt = std::complex<float>(cflt.real(), fltInf);
+ cdbl = std::complex<double>(cdbl.real(), mdblInf);
+ cjflt = conjugate<float>(cjflt.real(), fltNaN);
+ cjdbl = conjugate<double>(cjdbl.real(), dblInf);
+
+ // Imaginary only is NaN.
+ SimTK_TEST(!isFinite(cflt) && !isFinite(cdbl));
+ SimTK_TEST(!isFinite(cjflt) && !isFinite(cjdbl));
+ SimTK_TEST(!isFinite(ncflt) && !isFinite(ncdbl));
+ SimTK_TEST(!isFinite(ncjflt) && !isFinite(ncjdbl));
+
+ cflt = std::complex<float> (fltInf, cflt.imag());
+ cdbl = std::complex<double>(mdblInf, cdbl.imag());
+ cjflt = conjugate<float> (fltNaN, cjflt.imag());
+ cjdbl = conjugate<double> (dblInf, cjdbl.imag());
+
+ // Both parts are non-finite.
+ SimTK_TEST(!isFinite(cflt) && !isFinite(cdbl));
+ SimTK_TEST(!isFinite(cjflt) && !isFinite(cjdbl));
+ SimTK_TEST(!isFinite(ncflt) && !isFinite(ncdbl));
+ SimTK_TEST(!isFinite(ncjflt) && !isFinite(ncjdbl));
+
+ // Restore imaginary part to normal.
+ cflt = std::complex<float>(cflt.real(), fltRegular);
+ cdbl = std::complex<double>(cdbl.real(), dblRegular);
+ cjflt = conjugate<float>(cjflt.real(), fltRegular);
+ cjdbl = conjugate<double>(cjdbl.real(), dblRegular);
+
+ // Real part only is non-finite;
+ SimTK_TEST(!isFinite(cflt) && !isFinite(cdbl));
+ SimTK_TEST(!isFinite(cjflt) && !isFinite(cjdbl));
+ SimTK_TEST(!isFinite(ncflt) && !isFinite(ncdbl));
+ SimTK_TEST(!isFinite(ncjflt) && !isFinite(ncjdbl));
+}
+
+void testSignBit() {
+ const unsigned char ucm=0xff, ucz=0, ucp=27;
+ const unsigned short usm=0xffff, usz=0, usp=2342;
+ const unsigned int uim=0xffffffff, uiz=0, uip=2342344;
+ const unsigned long ulm=(unsigned long)-23423L, ulz=0, ulp=234234UL;
+ const unsigned long long ullm=(unsigned long long)-234234234LL, ullz=0, ullp=234234234ULL;
+
+ SimTK_TEST(!(signBit(ucm)||signBit(ucz)||signBit(ucp)));
+ SimTK_TEST(!(signBit(usm)||signBit(usz)||signBit(usp)));
+ SimTK_TEST(!(signBit(uim)||signBit(uiz)||signBit(uip)));
+ SimTK_TEST(!(signBit(ulm)||signBit(ulz)||signBit(ulp)));
+ SimTK_TEST(!(signBit(ullm)||signBit(ullz)||signBit(ullp)));
+
+ // Note that signBit(char) doesn't exist.
+
+ const signed char cm=-23, cz=0, cp=99;
+ const short sm=-1234, sz=0, sp=23423;
+ const int im=-2342343, iz=0, ip=29472383;
+ const long lm=-43488, lz=0, lp=3454545;
+ const long long llm=-2342342343433LL, llz=0, llp=874578478478574LL;
+
+ SimTK_TEST(signBit(cm) && !(signBit(cz)||signBit(cp)));
+ SimTK_TEST(signBit(sm) && !(signBit(sz)||signBit(sp)));
+ SimTK_TEST(signBit(im) && !(signBit(iz)||signBit(ip)));
+ SimTK_TEST(signBit(lm) && !(signBit(lz)||signBit(lp)));
+ SimTK_TEST(signBit(llm) && !(signBit(llz)||signBit(llp)));
+
+ const float fm=-12398.34f, fz=0, fp=4354.331f;
+ const double dm=-234234.454, dz=0, dp=345345.2342;
+ float mfz=-fz; double mdz=-dz;// -0 for some compilers
+
+ SimTK_TEST(signBit(fm) && !(signBit(fz)||signBit(fp)));
+ SimTK_TEST(signBit(dm) && !(signBit(dz)||signBit(dp)));
+
+ // Can't be sure whether the compiler will actually have produced
+ // a minus zero here.
+ // SimTK_TEST(signBit(mfz) && signBit(mdz));
+
+ // Note: signBit of negated float or double should be the
+ // *same* as the underlying float or double; it is the
+ // interpretation of that bit that is supposed to be
+ // different.
+ const negator<float>& nfm=reinterpret_cast<const negator<float>&>(fm);
+ const negator<float>& nfz=reinterpret_cast<const negator<float>&>(fz);
+ const negator<float>& nfp=reinterpret_cast<const negator<float>&>(fp);
+ const negator<float>& nmfz=reinterpret_cast<const negator<float>&>(mfz);
+ const negator<double>& ndm=reinterpret_cast<const negator<double>&>(dm);
+ const negator<double>& ndz=reinterpret_cast<const negator<double>&>(dz);
+ const negator<double>& ndp=reinterpret_cast<const negator<double>&>(dp);
+ const negator<double>& nmdz=reinterpret_cast<const negator<double>&>(mdz);
+
+ SimTK_TEST(signBit(nfm) && !(signBit(nfz)||signBit(nfp)));
+ SimTK_TEST(signBit(ndm) && !(signBit(ndz)||signBit(ndp)));
+ SimTK_TEST(signBit(nmfz)==signBit(mfz) && signBit(nmdz)==signBit(mdz));
+
+ const float fltInf = NTraits<float>::getInfinity();
+ const double dblInf = NTraits<double>::getInfinity();
+ const float mfltInf = -fltInf;
+ const double mdblInf = -dblInf;
+
+ SimTK_TEST(!signBit(fltInf) && !signBit(dblInf));
+ SimTK_TEST(signBit(mfltInf) && signBit(mdblInf));
+}
+
+
+void testSign() {
+ const unsigned char ucm=0xff, ucz=0, ucp=27;
+ const unsigned short usm=0xffff, usz=0, usp=2342;
+ const unsigned int uim=0xffffffff, uiz=0, uip=2342344;
+ const unsigned long ulm=(unsigned long)-23423L, ulz=0, ulp=234234UL;
+ const unsigned long long ullm=(unsigned long long)-234234234LL, ullz=0, ullp=234234234ULL;
+
+ SimTK_TEST(sign(ucm)==1 && sign(ucz)==0 && sign(ucp)==1);
+ SimTK_TEST(sign(usm)==1 && sign(usz)==0 && sign(usp)==1);
+ SimTK_TEST(sign(uim)==1 && sign(uiz)==0 && sign(uip)==1);
+ SimTK_TEST(sign(ulm)==1 && sign(ulz)==0 && sign(ulp)==1);
+ SimTK_TEST(sign(ullm)==1 && sign(ullz)==0 && sign(ullp)==1);
+
+ // Note that sign(char) doesn't exist.
+
+ const signed char cm=-23, cz=0, cp=99;
+ const short sm=-1234, sz=0, sp=23423;
+ const int im=-2342343, iz=0, ip=29472383;
+ const long lm=-43488, lz=0, lp=3454545;
+ const long long llm=-2342342343433LL, llz=0, llp=874578478478574LL;
+
+ SimTK_TEST(sign(cm)==-1 && sign(cz)==0 && sign(cp)==1);
+ SimTK_TEST(sign(sm)==-1 && sign(sz)==0 && sign(sp)==1);
+ SimTK_TEST(sign(im)==-1 && sign(iz)==0 && sign(ip)==1);
+ SimTK_TEST(sign(lm)==-1 && sign(lz)==0 && sign(lp)==1);
+ SimTK_TEST(sign(llm)==-1 && sign(llz)==0 && sign(llp)==1);
+
+ const float fm=-12398.34f, fz=0, fp=4354.331f;
+ const double dm=-234234.454, dz=0, dp=345345.2342;
+ float mfz=-fz; double mdz=-dz;// -0
+
+ SimTK_TEST(sign(fm)==-1 && sign(fz)==0 && sign(fp)==1);
+ SimTK_TEST(sign(dm)==-1 && sign(dz)==0 && sign(dp)==1);
+ SimTK_TEST(sign(mfz)==0 && sign(mdz)==0); // doesn't matter if it's -0
+
+ // Note: sign of negated float or double should be the
+ // *opposite* as the underlying float or double.
+ const negator<float>& nfm=reinterpret_cast<const negator<float>&>(fm);
+ const negator<float>& nfz=reinterpret_cast<const negator<float>&>(fz);
+ const negator<float>& nfp=reinterpret_cast<const negator<float>&>(fp);
+ const negator<float>& nmfz=reinterpret_cast<const negator<float>&>(mfz);
+ const negator<double>& ndm=reinterpret_cast<const negator<double>&>(dm);
+ const negator<double>& ndz=reinterpret_cast<const negator<double>&>(dz);
+ const negator<double>& ndp=reinterpret_cast<const negator<double>&>(dp);
+ const negator<double>& nmdz=reinterpret_cast<const negator<double>&>(mdz);
+
+ SimTK_TEST(sign(nfm)==1 && sign(nfz)==0 && sign(nfp)==-1);
+ SimTK_TEST(sign(ndm)==1 && sign(ndz)==0 && sign(ndp)==-1);
+ SimTK_TEST(sign(nmfz)==0 && sign(nmdz)==0); // doesn't matter if it's -0
+
+ const float fltInf = NTraits<float>::getInfinity();
+ const double dblInf = NTraits<double>::getInfinity();
+ const float mfltInf = -fltInf;
+ const double mdblInf = -dblInf;
+ const negator<float>& nfltInf = reinterpret_cast<const negator<float>&>(fltInf);
+ const negator<double>& ndblInf = reinterpret_cast<const negator<double>&>(dblInf);
+
+ SimTK_TEST(sign(fltInf)==1 && sign(dblInf)==1);
+ SimTK_TEST(sign(mfltInf)==-1 && sign(mdblInf)==-1);
+ SimTK_TEST(sign(nfltInf)==-1 && sign(ndblInf)==-1);
+}
+
+void testSquareAndCube() {
+ const float fval = -23.33f;
+ const double dval = -234443.441;
+ const negator<float>& nfval = reinterpret_cast<const negator<float>&>(fval);
+ const negator<double>& ndval = reinterpret_cast<const negator<double>&>(dval);
+
+ // Basic test.
+ SimTK_TEST_EQ(square(fval), fval*fval);
+ SimTK_TEST_EQ(square(dval), dval*dval);
+ SimTK_TEST_EQ(cube(fval), fval*fval*fval);
+ SimTK_TEST_EQ(cube(dval), dval*dval*dval);
+
+ // Test scalar negators.
+ SimTK_TEST_EQ(square(nfval), nfval*nfval);
+ SimTK_TEST_EQ(square(nfval), fval*fval);
+ SimTK_TEST_EQ(square(ndval), ndval*ndval);
+ SimTK_TEST_EQ(square(ndval), dval*dval);
+ SimTK_TEST_EQ(cube(nfval), nfval*nfval*nfval);
+ SimTK_TEST_EQ(cube(nfval), -fval*fval*fval);
+ SimTK_TEST_EQ(cube(ndval), ndval*ndval*ndval);
+ SimTK_TEST_EQ(cube(ndval), -dval*dval*dval);
+
+ // Create complex and conjugate values.
+
+ std::complex<float> fc(-234.343f, 45345e7f);
+ std::complex<double> dc(-234.343, 45345e7);
+ conjugate<float> fcj(-19.1e3f, -454.234f);
+ conjugate<double> dcj(-19.1e3, -454.234);
+
+ // Manual conjugates
+ std::complex<float> fcmj(fcj.real(), fcj.imag());
+ std::complex<double> dcmj(dcj.real(), dcj.imag());
+ SimTK_TEST(fcj == fcmj); // sign change only; should be exact
+ SimTK_TEST(dcj == dcmj);
+ SimTK_TEST_EQ(fcj*fcj, fcmj*fcmj);
+ SimTK_TEST_EQ(dcj*dcj, dcmj*dcmj);
+ SimTK_TEST_EQ(fcj*fcj*fcj, fcmj*fcmj*fcmj);
+ SimTK_TEST_EQ(dcj*dcj*dcj, dcmj*dcmj*dcmj);
+
+ // Negators of complex an conjugate.
+ negator<std::complex<float> >& nfc = reinterpret_cast<negator<std::complex<float> >&> (fc);
+ negator<std::complex<double> >& ndc = reinterpret_cast<negator<std::complex<double> >&>(dc);
+ negator<conjugate<float> >& nfcj = reinterpret_cast<negator<conjugate<float> >&> (fcj);
+ negator<conjugate<double> >& ndcj = reinterpret_cast<negator<conjugate<double> >&> (dcj);
+
+ // Change of sign should be exact.
+ SimTK_TEST(nfc == -fc);
+ SimTK_TEST(ndc == -dc);
+ SimTK_TEST(nfcj == -fcj);
+ SimTK_TEST(ndcj == -dcj);
+
+
+ // Basic complex and conjugate tests.
+ SimTK_TEST_EQ(square(fc), fc*fc);
+ SimTK_TEST_EQ(cube(fc), fc*fc*fc);
+ SimTK_TEST_EQ(square(dc), dc*dc);
+ SimTK_TEST_EQ(cube(dc), dc*dc*dc);
+ SimTK_TEST_EQ(square(fcj), fcj*fcj);
+ SimTK_TEST_EQ(cube(fcj), fcj*fcj*fcj);
+ SimTK_TEST_EQ(square(dcj), dcj*dcj);
+ SimTK_TEST_EQ(cube(dcj), dcj*dcj*dcj);
+
+ // Tests involving negators of complex and conjugate.
+ SimTK_TEST_EQ(square(nfc), nfc*nfc);
+ SimTK_TEST_EQ(square(nfc), fc*fc);
+ SimTK_TEST_EQ(square(ndc), ndc*ndc);
+ SimTK_TEST_EQ(square(ndc), dc*dc);
+
+ SimTK_TEST_EQ(cube(nfc), nfc*nfc*nfc);
+ SimTK_TEST_EQ(cube(nfc), -fc*fc*fc);
+ SimTK_TEST_EQ(cube(ndc), ndc*ndc*ndc);
+ SimTK_TEST_EQ(cube(ndc), -dc*dc*dc);
+
+ SimTK_TEST_EQ(square(nfcj), nfcj*nfcj);
+ SimTK_TEST_EQ(square(nfcj), fcj*fcj);
+ SimTK_TEST_EQ(square(ndcj), ndcj*ndcj);
+ SimTK_TEST_EQ(square(ndcj), dcj*dcj);
+
+ SimTK_TEST_EQ(cube(nfcj), nfcj*nfcj*nfcj);
+ SimTK_TEST_EQ(cube(nfcj), -fcj*fcj*fcj);
+ SimTK_TEST_EQ(cube(ndcj), ndcj*ndcj*ndcj);
+ SimTK_TEST_EQ(cube(ndcj), -dcj*dcj*dcj);
+}
+
+void testIsNumericallyEqual() {
+ const float f=1.234f, fn=1.234f+1e-5f, fe=1.234f+1e-9f;
+ const double d=1.234, dn=1.234 +1e-12, de=1.234 +1e-15;
+ const negator<float>& nf=negator<float>::recast(f);
+ const negator<float>& nfn=negator<float>::recast(fn);
+ const negator<float>& nfe=negator<float>::recast(fe);
+
+ SimTK_TEST(isNumericallyEqual(f,f))
+ SimTK_TEST(isNumericallyEqual(f,fe));
+ SimTK_TEST(!isNumericallyEqual(f,fn));
+ SimTK_TEST(isNumericallyEqual(f,fn,1e-4f));
+ SimTK_TEST(!isNumericallyEqual(f,fn,1e-6f));
+
+ SimTK_TEST(CNT<float>::isNumericallyEqual(f,f));
+ SimTK_TEST(CNT<float>::isNumericallyEqual(f,fe));
+ SimTK_TEST(!CNT<float>::isNumericallyEqual(f,fn));
+ SimTK_TEST(CNT<float>::isNumericallyEqual(f,fn,1e-4f));
+ SimTK_TEST(!CNT<float>::isNumericallyEqual(f,fn,1e-6f));
+
+ SimTK_TEST(nf.isNumericallyEqual(nf));
+ SimTK_TEST(nf.isNumericallyEqual(-f));
+ SimTK_TEST(!nf.isNumericallyEqual(f));
+
+ SimTK_TEST(isNumericallyEqual(1000*f,1234));
+ SimTK_TEST(isNumericallyEqual(1234,1000*f));
+ SimTK_TEST(isNumericallyEqual(1000*fe,1234));
+ SimTK_TEST(isNumericallyEqual(1234,1000*fe));
+ SimTK_TEST(!isNumericallyEqual(1000*fn,1234));
+ SimTK_TEST(!isNumericallyEqual(1234,1000*fn));
+
+ SimTK_TEST(isNumericallyEqual(d,d));
+ SimTK_TEST(isNumericallyEqual(d,de));
+ SimTK_TEST(!isNumericallyEqual(d,dn));
+ SimTK_TEST(isNumericallyEqual(1000*d,1234));
+ SimTK_TEST(isNumericallyEqual(1234,1000*d));
+ SimTK_TEST(isNumericallyEqual(1000*de,1234));
+ SimTK_TEST(isNumericallyEqual(1234,1000*de));
+ SimTK_TEST(!isNumericallyEqual(1000*dn,1234));
+ SimTK_TEST(!isNumericallyEqual(1234,1000*dn));
+
+ // Mixed should use float tolerance
+ SimTK_TEST(isNumericallyEqual(fe,de));
+ SimTK_TEST(!isNumericallyEqual((double)fe,de));
+}
+
+void testClamp() {
+ const int i4=4;
+ const double d325=3.25;
+ const float fn325=-3.25;
+
+ // int
+ SimTK_TEST(clamp(4,i4,4)==4);
+ SimTK_TEST(clamp(0,i4,9)==4);
+ SimTK_TEST(clamp(5,i4,9)==5);
+ SimTK_TEST(clamp(-7,i4,-5)==-5);
+
+ // double
+ SimTK_TEST(clamp(3.25,d325,3.25)==3.25);
+ SimTK_TEST(clamp(0.,d325,9.)==3.25);
+ SimTK_TEST(clamp(5.,d325,9.)==5);
+ SimTK_TEST(clamp(-7.,d325,-5.)==-5);
+
+ // float
+ SimTK_TEST(clamp(-3.25f,fn325,-3.25f)==-3.25);
+ SimTK_TEST(clamp(-9.f,fn325,0.f)==-3.25f);
+ SimTK_TEST(clamp(-9.f,fn325,-5.f)==-5);
+ SimTK_TEST(clamp(5.f,fn325,7.f)==5);
+
+ // Test methods that take integer bounds.
+ SimTK_TEST(clamp(0,d325,9)==3.25);
+ SimTK_TEST(clamp(5,d325,9)==5);
+ SimTK_TEST(clamp(-7,d325,-5)==-5);
+
+ SimTK_TEST(clamp(-9,fn325,0)==-3.25);
+ SimTK_TEST(clamp(-9,fn325,-5)==-5);
+ SimTK_TEST(clamp(5,fn325,7)==5);
+
+ SimTK_TEST(clamp(0.,d325,9)==3.25);
+ SimTK_TEST(clamp(5.,d325,9)==5);
+ SimTK_TEST(clamp(-7.,d325,-5)==-5);
+
+ SimTK_TEST(clamp(-9.f,fn325,0)==-3.25);
+ SimTK_TEST(clamp(-9.f,fn325,-5)==-5);
+ SimTK_TEST(clamp(5.f,fn325,7)==5);
+
+ SimTK_TEST(clamp(0,d325,9.)==3.25);
+ SimTK_TEST(clamp(5,d325,9.)==5);
+ SimTK_TEST(clamp(-7,d325,-5.)==-5);
+
+ SimTK_TEST(clamp(-9,fn325,0.f)==-3.25);
+ SimTK_TEST(clamp(-9,fn325,-5.f)==-5);
+ SimTK_TEST(clamp(5,fn325,7.f)==5);
+
+ int i; double d; float f;
+ i=i4;
+ SimTK_TEST(clampInPlace(-2,i,3)==3 && i==3);
+ d=d325;
+ SimTK_TEST(clampInPlace(-2.,d,3.)==3 && d==3);
+ f=fn325;
+ SimTK_TEST(clampInPlace(-2,f,3)==-2 && f==-2);
+
+ // Do a test for each of the less-common supported types.
+ char c='j'; unsigned char uc=3; signed char sc=-2;
+ SimTK_TEST(clamp('a',c,'e')=='e');
+ SimTK_TEST(clamp('a',c,'z')=='j');
+ SimTK_TEST(clamp((unsigned char)4,uc,(unsigned char)5)==4);
+ SimTK_TEST(clamp((signed char)-7,sc,(signed char)-1)==-2);
+
+ short s=-32000; unsigned short us=17; unsigned ui=4023456789U;
+ SimTK_TEST(clamp((short)-29000,s,(short)400)==-29000);
+ SimTK_TEST(clamp((unsigned short)4,us,(unsigned short)15)==15);
+ SimTK_TEST(clamp(100000000U,ui,4010000000U)==4010000000U);
+
+ long l=-234234L; unsigned long ul=293493849UL;
+ long long ll=-123456789123LL; unsigned long long ull=123456789123ULL;
+ SimTK_TEST(clamp(-1000000L,l,-200000L)==-234234);
+ SimTK_TEST(clamp(1000000UL,ul,4000000000UL)==293493849);
+ SimTK_TEST(clamp(-100000000000LL,ll,27LL)==-100000000000LL);
+ SimTK_TEST(clamp(-1000000000000LL,ll,27LL)==-123456789123LL);
+}
+
+void testStep() {
+ // double
+ SimTK_TEST(stepUp(0.)==0 && stepUp(.5)==.5 && stepUp(1.)==1);
+ SimTK_TEST(0 < stepUp(.3) && stepUp(.3) < .5);
+ SimTK_TEST(.5 < stepUp(.7) && stepUp(.7) < 1);
+ SimTK_TEST(stepDown(0.)==1 && stepDown(.5)==.5 && stepDown(1.)==0);
+ SimTK_TEST(.5 < stepDown(.3) && stepDown(.3) < 1);
+ SimTK_TEST(0 < stepDown(.7) && stepDown(.7) < .5);
+ SimTK_TEST(dstepUp(0.)==0 && dstepUp(.5)>0 && dstepUp(1.)==0);
+ SimTK_TEST(dstepDown(0.)==0 && dstepDown(.5)<0 && dstepDown(1.)==0);
+ SimTK_TEST(d2stepUp(0.)==0 && d2stepUp(1.)==0);
+ SimTK_TEST(d2stepDown(0.)==0 && d2stepDown(1.)==0);
+ // float
+ SimTK_TEST(stepUp(0.f)==0 && stepUp(.5f)==.5f && stepUp(1.f)==1);
+ SimTK_TEST(0 < stepUp(.3f) && stepUp(.3f) < .5f);
+ SimTK_TEST(.5f < stepUp(.7f) && stepUp(.7f) < 1);
+ SimTK_TEST(stepDown(0.f)==1 && stepDown(.5f)==.5f && stepDown(1.f)==0);
+ SimTK_TEST(.5f < stepDown(.3f) && stepDown(.3f) < 1);
+ SimTK_TEST(0 < stepDown(.7f) && stepDown(.7f) < .5f);
+ SimTK_TEST(dstepUp(0.f)==0 && dstepUp(.5f)>0 && dstepUp(1.f)==0);
+ SimTK_TEST(dstepDown(0.f)==0 && dstepDown(.5f)<0 && dstepDown(1.f)==0);
+ SimTK_TEST(d2stepUp(0.f)==0 && d2stepUp(1.f)==0);
+ SimTK_TEST(d2stepDown(0.f)==0 && d2stepDown(1.f)==0);
+ // long double
+ SimTK_TEST(stepUp(0.L)==0 && stepUp(.5L)==.5L && stepUp(1.L)==1);
+ SimTK_TEST(0 < stepUp(.3L) && stepUp(.3L) < .5L);
+ SimTK_TEST(.5L < stepUp(.7L) && stepUp(.7L) < 1);
+ SimTK_TEST(stepDown(0.L)==1 && stepDown(.5L)==.5L && stepDown(1.L)==0);
+ SimTK_TEST(.5L < stepDown(.3L) && stepDown(.3L) < 1);
+ SimTK_TEST(0 < stepDown(.7L) && stepDown(.7L) < .5L);
+ SimTK_TEST(dstepUp(0.L)==0 && dstepUp(.5L)>0 && dstepUp(1.L)==0);
+ SimTK_TEST(dstepDown(0.L)==0 && dstepDown(.5L)<0 && dstepDown(1.L)==0);
+ SimTK_TEST(d2stepUp(0.L)==0 && d2stepUp(1.L)==0);
+ SimTK_TEST(d2stepDown(0.L)==0 && d2stepDown(1.L)==0);
+
+ // int is treated as a double, but only for stepUp()/stepDown()
+ SimTK_TEST(stepUp(0)==0 && stepUp(1)==1);
+ SimTK_TEST(stepDown(0)==1 && stepDown(1)==0);
+
+ // Don't know anything analytic about d3 but can test with finite
+ // differencing below.
+
+ // Central difference estimates should give around 10
+ // decimal places in double, 4 in float.
+ const double dupEst = (stepUp(.799+1e-6)-stepUp(.799-1e-6))/2e-6;
+ const double ddnEst = (stepDown(.799+1e-6)-stepDown(.799-1e-6))/2e-6;
+ const double d2upEst = (dstepUp(.723+1e-6)-dstepUp(.723-1e-6))/2e-6;
+ const double d2dnEst = (dstepDown(.723+1e-6)-dstepDown(.723-1e-6))/2e-6;
+ const double d3upEst = (d2stepUp(.123+1e-6)-d2stepUp(.123-1e-6))/2e-6;
+ const double d3dnEst = (d2stepDown(.123+1e-6)-d2stepDown(.123-1e-6))/2e-6;
+ SimTK_TEST_EQ_TOL(dstepUp(.799), dupEst, 1e-8);
+ SimTK_TEST_EQ_TOL(dstepDown(.799), ddnEst, 1e-8);
+ SimTK_TEST_EQ_TOL(d2stepUp(.723), d2upEst, 1e-8);
+ SimTK_TEST_EQ_TOL(d2stepDown(.723), d2dnEst, 1e-8);
+ SimTK_TEST_EQ_TOL(d3stepUp(.123), d3upEst, 1e-8);
+ SimTK_TEST_EQ_TOL(d3stepDown(.123), d3dnEst, 1e-8);
+
+ const float fdupEst = (stepUp(.699f+1e-3f)-stepUp(.699f-1e-3f))/2e-3f;
+ const float fddnEst = (stepDown(.699f+1e-3f)-stepDown(.699f-1e-3f))/2e-3f;
+ const float fd2upEst = (dstepUp(.623f+1e-3f)-dstepUp(.623f-1e-3f))/2e-3f;
+ const float fd2dnEst = (dstepDown(.623f+1e-3f)-dstepDown(.623f-1e-3f))/2e-3f;
+ const float fd3upEst = (d2stepUp(.211f+1e-3f)-d2stepUp(.211f-1e-3f))/2e-3f;
+ const float fd3dnEst = (d2stepDown(.211f+1e-3f)-d2stepDown(.211f-1e-3f))/2e-3f;
+ SimTK_TEST_EQ_TOL(dstepUp(.699f), fdupEst, 1e-3);
+ SimTK_TEST_EQ_TOL(dstepDown(.699f), fddnEst, 1e-3);
+ SimTK_TEST_EQ_TOL(d2stepUp(.623f), fd2upEst, 1e-3);
+ SimTK_TEST_EQ_TOL(d2stepDown(.623f), fd2dnEst, 1e-3);
+ SimTK_TEST_EQ_TOL(d3stepUp(.211f), fd3upEst, 1e-3);
+ SimTK_TEST_EQ_TOL(d3stepDown(.211f), fd3dnEst, 1e-3);
+
+
+ // y = stepAny(y0,yrange,x0,1/xrange, x)
+ // y goes from -1 to 1 as x goes from 0 to 1, exact arithmetic.
+ SimTK_TEST(stepAny(-1,2,0,1,0.) == -1);
+ SimTK_TEST(stepAny(-1,2,0,1,.5) == 0);
+ SimTK_TEST(stepAny(-1,2,0,1,1.) == 1);
+ SimTK_TEST(stepAny(-1,2,0,1,0.f) == -1);
+ SimTK_TEST(stepAny(-1,2,0,1,.5f) == 0);
+ SimTK_TEST(stepAny(-1,2,0,1,1.f) == 1);
+ SimTK_TEST(stepAny(-1,2,0,1,0.L) == -1);
+ SimTK_TEST(stepAny(-1,2,0,1,.5L) == 0);
+ SimTK_TEST(stepAny(-1,2,0,1,1.L) == 1);
+
+ // y goes from -7 down to -14 as x goes from -3.1 up to +429.3.
+ const double x0=-3.1, x1=429.3, y0=-7., y1=-14.;
+ const double xr=(x1-x0), ooxr=1/xr, yr=(y1-y0);
+ SimTK_TEST_EQ(stepAny(y0,yr,x0,ooxr,-3.1), y0);
+ SimTK_TEST_EQ(stepAny(y0,yr,x0,ooxr,429.3), y1);
+ SimTK_TEST_EQ(stepAny(y0,yr,x0,ooxr,x0+xr/2), y0+yr/2);
+
+ const float fx0=-3.1f, fx1=429.3f, fy0=-7.f, fy1=-14.f;
+ const float fxr=(fx1-fx0), fooxr=1/fxr, fyr=(fy1-fy0);
+ SimTK_TEST_EQ(stepAny(fy0,fyr,fx0,fooxr,-3.1f),fy0);
+ SimTK_TEST_EQ(stepAny(fy0,fyr,fx0,fooxr,429.3f),fy1);
+ SimTK_TEST_EQ(stepAny(fy0,fyr,fx0,fooxr,fx0+fxr/2),fy0+fyr/2);
+
+ // Check derivatives
+ const double danyEst =
+ (stepAny(y0,yr,x0,ooxr,.799+1e-6)-stepAny(y0,yr,x0,ooxr,.799-1e-6))/2e-6;
+ const double d2anyEst =
+ (dstepAny(yr,x0,ooxr,.723+1e-6)-dstepAny(yr,x0,ooxr,.723-1e-6))/2e-6;
+ const double d3anyEst =
+ (d2stepAny(yr,x0,ooxr,.123+1e-6)-d2stepAny(yr,x0,ooxr,.123-1e-6))/2e-6;
+ SimTK_TEST_EQ_TOL(dstepAny(yr,x0,ooxr,.799), danyEst, 1e-8);
+ SimTK_TEST_EQ_TOL(d2stepAny(yr,x0,ooxr,.723), d2anyEst, 1e-8);
+ SimTK_TEST_EQ_TOL(d3stepAny(yr,x0,ooxr,.123), d3anyEst, 1e-8);
+
+ const float fdanyEst =
+ (stepAny(fy0,fyr,fx0,fooxr,.799f+1e-3f)
+ -stepAny(fy0,fyr,fx0,fooxr,.799f-1e-3f))/2e-3f;
+ const float fd2anyEst =
+ (dstepAny(fyr,fx0,fooxr,.723f+1e-3f)
+ -dstepAny(fyr,fx0,fooxr,.723f-1e-3f))/2e-3f;
+ const float fd3anyEst =
+ (d2stepAny(fyr,fx0,fooxr,.123f+1e-3f)
+ -d2stepAny(fyr,fx0,fooxr,.123f-1e-3f))/2e-3f;
+ SimTK_TEST_EQ_TOL(dstepAny(fyr,fx0,fooxr,.799f), fdanyEst, 1e-3);
+ SimTK_TEST_EQ_TOL(d2stepAny(fyr,fx0,fooxr,.723f), fd2anyEst, 1e-3);
+ SimTK_TEST_EQ_TOL(d3stepAny(fyr,fx0,fooxr,.123f), fd3anyEst, 1e-3);
+}
+
+int main() {
+ SimTK_START_TEST("TestScalar");
+
+ SimTK_SUBTEST(testIsNaN);
+ SimTK_SUBTEST(testIsInf);
+ SimTK_SUBTEST(testIsFinite);
+ SimTK_SUBTEST(testSignBit);
+ SimTK_SUBTEST(testSign);
+ SimTK_SUBTEST(testSquareAndCube);
+ SimTK_SUBTEST(testIsNumericallyEqual);
+ SimTK_SUBTEST(testClamp);
+ SimTK_SUBTEST(testStep);
+
+ SimTK_END_TEST();
+}
+
diff --git a/SimTKcommon/tests/TestSimulation.cpp b/SimTKcommon/tests/TestSimulation.cpp
new file mode 100644
index 0000000..6257cce
--- /dev/null
+++ b/SimTKcommon/tests/TestSimulation.cpp
@@ -0,0 +1,875 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/*
+ * Here we'll build a very simple System containing a simple
+ * Subsystem, and integrate with a simple integrator. This avoids all the usual
+ * Simmath and Simbody trappings and lets us just check out the underlying
+ * simulation architecture.
+ */
+
+#include "SimTKcommon.h"
+#include "SimTKcommon/internal/SystemGuts.h"
+
+#include <iostream>
+using std::cout;
+using std::endl;
+
+using namespace SimTK;
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS((cond), "Assertion failed.");}
+#define ASSERT_EQ(v1,v2) \
+ {SimTK_ASSERT_ALWAYS(numericallyEqual((v1),(v2)), \
+ "Values should have been numerically equivalent.");}
+
+// Scale by the magnitude of the quantities being compared, so that we don't
+// ask for unreasonable precision. For magnitudes near zero, we'll be satisfied
+// if both are very small without demanding that they must also be relatively
+// close. That is, we use a relative tolerance for big numbers and an absolute
+// tolerance for small ones.
+bool numericallyEqual(float v1, float v2) {
+ const float scale = std::max(std::max(std::abs(v1), std::abs(v2)), 0.1f);
+ return std::abs(v1-v2) < scale*NTraits<float>::getSignificant();
+}
+bool numericallyEqual(double v1, double v2) {
+ const double scale = std::max(std::max(std::abs(v1), std::abs(v2)), 0.1);
+ return std::abs(v1-v2) < scale*NTraits<double>::getSignificant();
+}
+template <class P>
+bool numericallyEqual(const std::complex<P>& v1, const std::complex<P>& v2) {
+ return numericallyEqual(v1.real(), v2.real())
+ && numericallyEqual(v1.imag(), v2.imag());
+}
+template <class P>
+bool numericallyEqual(const conjugate<P>& v1, const conjugate<P>& v2) {
+ return numericallyEqual(v1.real(), v2.real())
+ && numericallyEqual(v1.imag(), v2.imag());
+}
+template <class P>
+bool numericallyEqual(const std::complex<P>& v1, const conjugate<P>& v2) {
+ return numericallyEqual(v1.real(), v2.real())
+ && numericallyEqual(v1.imag(), v2.imag());
+}
+template <class P>
+bool numericallyEqual(const conjugate<P>& v1, const std::complex<P>& v2) {
+ return numericallyEqual(v1.real(), v2.real())
+ && numericallyEqual(v1.imag(), v2.imag());
+}
+template <class P>
+bool numericallyEqual(const negator<P>& v1, const negator<P>& v2) {
+ return numericallyEqual(-v1, -v2); // P, P
+}
+template <class P>
+bool numericallyEqual(const P& v1, const negator<P>& v2) {
+ return numericallyEqual(-v1, -v2); // P, P
+}
+template <class P>
+bool numericallyEqual(const negator<P>& v1, const P& v2) {
+ return numericallyEqual(-v1, -v2); // P, P
+}
+template <class P>
+bool numericallyEqual(const negator<std::complex<P> >& v1, const conjugate<P>& v2) {
+ return numericallyEqual(-v1, -v2); // complex, conjugate
+}
+template <class P>
+bool numericallyEqual(const negator<conjugate<P> >& v1, const std::complex<P>& v2) {
+ return numericallyEqual(-v1, -v2); // conjugate, complex
+}
+template <class P>
+bool numericallyEqual(const std::complex<P>& v1, const negator<conjugate<P> >& v2) {
+ return numericallyEqual(-v1, -v2); // complex, conjugate
+}
+template <class P>
+bool numericallyEqual(const conjugate<P>& v1, const negator<std::complex<P> >& v2) {
+ return numericallyEqual(-v1, -v2); // conjugate, complex
+}
+
+namespace SimTK {
+typedef std::map<EventId, SubsystemIndex> EventRegistry;
+std::ostream& operator<<(std::ostream& o, const EventRegistry&) {return o;}
+}
+
+class SystemSubsystemGuts : public Subsystem::Guts {
+public:
+ SystemSubsystemGuts() {}
+
+ const EventRegistry& getEventRegistry(const State& s) const
+ { assert(eventRegistry >= 0);
+ return Value<EventRegistry>::downcast(getCacheEntry(s, eventRegistry)); }
+ EventRegistry& updEventRegistry(const State& s) const
+ { assert(eventRegistry >= 0);
+ return Value<EventRegistry>::downcast(updCacheEntry(s, eventRegistry)); }
+
+ // implementations of Subsystem::Guts virtuals
+ SystemSubsystemGuts* cloneImpl() const OVERRIDE_11
+ { return new SystemSubsystemGuts(*this); }
+ int realizeSubsystemTopologyImpl(State& s) const OVERRIDE_11 {
+ eventRegistry = allocateCacheEntry(s, Stage::Instance,
+ new Value<EventRegistry>());
+ return 0;
+ }
+
+private:
+ mutable CacheEntryIndex eventRegistry;
+};
+
+
+class SystemSubsystem : public Subsystem {
+public:
+ SystemSubsystem() {
+ adoptSubsystemGuts(new SystemSubsystemGuts());
+ }
+
+ void registerEventsToSubsystem(const State& s, const Subsystem::Guts& sub,
+ EventId start, int nEvents) const
+ { EventRegistry& er = getGuts().updEventRegistry(s);
+ SubsystemIndex sx = sub.getMySubsystemIndex();
+ for (int i=start; i < start+nEvents; ++i)
+ er[EventId(i)] = sx;
+ }
+
+ const EventRegistry& getEventRegistry(const State& s) const
+ { return getGuts().getEventRegistry(s); }
+
+private:
+ const SystemSubsystemGuts& getGuts() const
+ { return dynamic_cast<const SystemSubsystemGuts&>(getSubsystemGuts());}
+ SystemSubsystemGuts& updGuts()
+ { return dynamic_cast<SystemSubsystemGuts&>(updSubsystemGuts());}
+};
+
+
+class TestSystemGuts : public System::Guts {
+public:
+ const SystemSubsystem& getSystemSubsystem() const {return syssub;}
+ SystemSubsystem& updSystemSubsystem() {return syssub;}
+
+ // implementations of System::Guts virtuals
+ TestSystemGuts* cloneImpl() const OVERRIDE_11
+ { return new TestSystemGuts(*this); }
+
+ bool prescribeQImpl(State& state) const OVERRIDE_11
+ {
+ return false;
+ }
+ bool prescribeUImpl(State& state) const OVERRIDE_11
+ {
+ return false;
+ }
+ void projectQImpl(State& state, Vector& yerrest, const ProjectOptions& opts,
+ ProjectResults& result) const OVERRIDE_11
+ {
+ result.setExitStatus(ProjectResults::Succeeded);
+ }
+ void projectUImpl(State& state, Vector& yerrest, const ProjectOptions& opts,
+ ProjectResults& result) const OVERRIDE_11
+ {
+ result.setExitStatus(ProjectResults::Succeeded);
+ }
+ void handleEventsImpl
+ (State& s, Event::Cause cause, const Array_<EventId>& eventIds,
+ const HandleEventsOptions& options, HandleEventsResults& results) const
+ OVERRIDE_11
+ {
+ cout << "handleEventsImpl t=" << s.getTime()
+ << " cause=" << Event::getCauseName(cause) << endl;
+
+ if (eventIds.empty()) {
+ for (SubsystemIndex sx(0); sx < getNumSubsystems(); ++sx) {
+ const Subsystem& sub = getSubsystem(sx);
+ sub.getSubsystemGuts().handleEvents(s, cause, eventIds,
+ options, results);
+ if (results.getExitStatus()==HandleEventsResults::Failed)
+ break;
+ }
+ return;
+ }
+
+ // If there are EventIds, dole them out to the owning subsystem.
+
+ const EventRegistry& registry = getSystemSubsystem().getEventRegistry(s);
+
+ std::map<SubsystemIndex, Array_<EventId> > eventsPerSub;
+ for (EventId eid(0); eid < eventIds.size(); ++eid)
+ eventsPerSub[ registry.find(eid)->second ].push_back(eid);
+
+ std::map<SubsystemIndex, Array_<EventId> >::const_iterator
+ i = eventsPerSub.begin();
+ for (; i != eventsPerSub.end(); ++i) {
+ const Subsystem& sub = getSubsystem(i->first);
+ sub.getSubsystemGuts().handleEvents(s, cause, i->second,
+ options, results);
+ if (results.getExitStatus()==HandleEventsResults::Failed)
+ break;
+ }
+ }
+
+ int reportEventsImpl(const State& s, Event::Cause cause,
+ const Array_<EventId>& eventIds) const OVERRIDE_11
+ {
+ cout << "reportEventsImpl t=" << s.getTime() << " cause="
+ << Event::getCauseName(cause) << endl;
+ return 0;
+ }
+
+private:
+ SystemSubsystem syssub;
+};
+
+class TestSystem : public System {
+public:
+ TestSystem() {
+ adoptSystemGuts(new TestSystemGuts());
+ adoptSubsystem(updGuts().updSystemSubsystem());
+ }
+
+ const SystemSubsystem& getSystemSubsystem() const
+ { return getGuts().getSystemSubsystem(); }
+ SystemSubsystem& updSystemSubsystem()
+ { return updGuts().updSystemSubsystem(); }
+
+ void registerEventsToSubsystem(const State& s, const Subsystem::Guts& sub,
+ EventId start, int nEvents) const
+ {
+ const SystemSubsystem& syssub = getGuts().getSystemSubsystem();
+ syssub.registerEventsToSubsystem(s,sub,start,nEvents);
+ }
+
+ static const TestSystem& getAs(const System& sys)
+ { assert(dynamic_cast<const Guts*>(&sys.getSystemGuts()));
+ return static_cast<const TestSystem&>(sys); }
+ static TestSystem& updAs(System& sys)
+ { assert(dynamic_cast<const Guts*>(&sys.getSystemGuts()));
+ return static_cast<TestSystem&>(sys); }
+private:
+ const TestSystemGuts& getGuts() const
+ { return dynamic_cast<const TestSystemGuts&>(getSystemGuts());}
+ TestSystemGuts& updGuts()
+ { return dynamic_cast<TestSystemGuts&>(updSystemGuts());}
+};
+
+
+class TestSubsystemGuts : public Subsystem::Guts {
+ struct StateVars {
+ QIndex myQs;
+ UIndex myUs;
+ };
+ struct CacheEntries {
+ CacheEntryIndex qSumCacheIx, uSumCacheIx;
+ EventTriggerByStageIndex timeTriggerIx, velTriggerIx;
+ };
+ friend std::ostream& operator<<(std::ostream& o, const CacheEntries&);
+ friend std::ostream& operator<<(std::ostream& o, const StateVars&);
+public:
+ TestSubsystemGuts() {}
+
+ const Vec3& getQ3(const State& s) const {return Vec3::getAs(&getQ(s)[getStateVars(s).myQs]);}
+ const Vec3& getU3(const State& s) const {return Vec3::getAs(&getU(s)[getStateVars(s).myQs]);}
+ const Vec3& getQDot3(const State& s) const {return Vec3::getAs(&getQDot(s)[getStateVars(s).myQs]);}
+ const Vec3& getUDot3(const State& s) const {return Vec3::getAs(&getUDot(s)[getStateVars(s).myUs]);}
+ const Vec3& getQDotDot3(const State& s) const {return Vec3::getAs(&getQDotDot(s)[getStateVars(s).myQs]);}
+ Real getQSum(const State& s) const {return Value<Real>::downcast(getCacheEntry(s,getCacheEntries(s).qSumCacheIx));}
+ Real getUSum(const State& s) const {return Value<Real>::downcast(getCacheEntry(s,getCacheEntries(s).uSumCacheIx));}
+
+ Real getTimeTrigger1(const State& s) const {return getEventTriggersByStage(s, Stage::Time)[getCacheEntries(s).timeTriggerIx];}
+ Real getTimeTrigger2(const State& s) const {return getEventTriggersByStage(s, Stage::Time)[getCacheEntries(s).timeTriggerIx+1];}
+ Real getVelTrigger(const State& s) const {return getEventTriggersByStage(s, Stage::Velocity)[getCacheEntries(s).velTriggerIx];}
+
+ Vec3& updQ3(State& s) const {return Vec3::updAs(&updQ(s)[getStateVars(s).myQs]);}
+ Vec3& updU3(State& s) const {return Vec3::updAs(&updU(s)[getStateVars(s).myUs]);}
+
+ // implementations of Subsystem::Guts virtuals
+
+ TestSubsystemGuts* cloneImpl() const OVERRIDE_11
+ { return new TestSubsystemGuts(*this); }
+
+
+ int realizeSubsystemTopologyImpl(State& s) const OVERRIDE_11 {
+ myStateVars = allocateCacheEntry(s, Stage::Model, new Value<StateVars>());
+ myCacheEntries = allocateCacheEntry(s, Stage::Instance, new Value<CacheEntries>());
+ return 0;
+ }
+
+
+ int realizeSubsystemModelImpl(State& s) const OVERRIDE_11 {
+ StateVars& vars = updStateVars(s);
+ vars.myQs = allocateQ(s, Vector(Vec3(0)));
+ vars.myUs = allocateU(s, Vector(Vec3(0)));
+ return 0;
+ }
+
+ int realizeSubsystemInstanceImpl(const State& s) const OVERRIDE_11 {
+ CacheEntries& cache = updCacheEntries(s);
+ cache.qSumCacheIx = allocateCacheEntry(s, Stage::Position, new Value<Real>(0));
+ cache.uSumCacheIx = allocateCacheEntry(s, Stage::Velocity, new Value<Real>(0));
+ cache.timeTriggerIx = allocateEventTriggersByStage(s, Stage::Time, 2);
+ cache.velTriggerIx = allocateEventTriggersByStage(s, Stage::Velocity, 1);
+
+ getTestSystem().registerEventsToSubsystem(s, *this, EventId(cache.timeTriggerIx), 2);
+ getTestSystem().registerEventsToSubsystem(s, *this, EventId(cache.velTriggerIx), 1);
+ return 0;
+ }
+
+ int realizeSubsystemTimeImpl(const State& s) const OVERRIDE_11 {
+ const Real TriggerTime1 = .6789, TriggerTime2 = 1.234;
+ updTimeTrigger1(s) = s.getTime() - TriggerTime1;
+ updTimeTrigger2(s) = s.getTime() - TriggerTime2;
+ return 0;
+ }
+
+ int realizeSubsystemPositionImpl(const State& s) const OVERRIDE_11 {
+ updQSum(s) = sum(getQ3(s));
+ return 0;
+ }
+
+ int realizeSubsystemVelocityImpl(const State& s) const OVERRIDE_11 {
+ const Real TriggerUSum = 5;
+ updQDot3(s) = getU3(s);
+ const Real usum = updUSum(s) = sum(getU3(s));
+ updVelTrigger(s) = usum - TriggerUSum;
+ return 0;
+ }
+
+ int realizeSubsystemAccelerationImpl(const State& s) const OVERRIDE_11 {
+ updQDotDot3(s) = updUDot3(s) = Vec3(1,2,3);
+ return 0;
+ }
+
+ void handleEventsImpl(State& s, Event::Cause cause,
+ const Array_<EventId>& eventIds,
+ const HandleEventsOptions& options,
+ HandleEventsResults& results) const OVERRIDE_11
+ {
+ cout << "**** TestSubsystem::handleEventsImpl t=" << s.getTime()
+ << " acc=" << options.getAccuracy()
+ << " eventIds=";
+ for (unsigned i=0; i < eventIds.size(); ++i)
+ cout << " " << eventIds[i];
+ cout << " ****" << endl;
+
+ // Pretend we changed a position to test lowestModifiedStage
+ // calculation. Try to hide our duplicity by realizing it again.
+ s.invalidateAllCacheAtOrAbove(Stage::Position);
+ getSystem().realize(s, Stage::Velocity);
+ results.setExitStatus(HandleEventsResults::Succeeded);
+ }
+
+ void reportEventsImpl(const State&, Event::Cause,
+ const Array_<EventId>& eventIds) const OVERRIDE_11
+ {
+ }
+
+
+private:
+ Vec3& updQDot3(const State& s) const {return Vec3::updAs(&updQDot(s)[getStateVars(s).myQs]);}
+ Vec3& updUDot3(const State& s) const {return Vec3::updAs(&updUDot(s)[getStateVars(s).myUs]);}
+ Vec3& updQDotDot3(const State& s) const {return Vec3::updAs(&updQDotDot(s)[getStateVars(s).myQs]);}
+ Real& updQSum(const State& s) const {return Value<Real>::downcast(updCacheEntry(s,getCacheEntries(s).qSumCacheIx));}
+ Real& updUSum(const State& s) const {return Value<Real>::downcast(updCacheEntry(s,getCacheEntries(s).uSumCacheIx));}
+ Real& updTimeTrigger1(const State& s) const {return updEventTriggersByStage(s, Stage::Time)[getCacheEntries(s).timeTriggerIx];}
+ Real& updTimeTrigger2(const State& s) const {return updEventTriggersByStage(s, Stage::Time)[getCacheEntries(s).timeTriggerIx+1];}
+ Real& updVelTrigger(const State& s) const {return updEventTriggersByStage(s, Stage::Velocity)[getCacheEntries(s).velTriggerIx];}
+
+ const StateVars& getStateVars(const State& s) const
+ { assert(myStateVars >= 0);
+ return Value<StateVars>::downcast(getCacheEntry(s,myStateVars)); }
+ const CacheEntries& getCacheEntries(const State& s) const
+ { assert(myCacheEntries >= 0);
+ return Value<CacheEntries>::downcast(getCacheEntry(s,myCacheEntries)); }
+ StateVars& updStateVars (const State& s) const
+ { assert(myStateVars >= 0);
+ return Value<StateVars>::downcast(updCacheEntry(s,myStateVars)); }
+ CacheEntries& updCacheEntries(const State& s) const
+ { assert(myCacheEntries >= 0);
+ return Value<CacheEntries>::downcast(updCacheEntry(s,myCacheEntries)); }
+
+ const TestSystem& getTestSystem() const {return TestSystem::getAs(getSystem());}
+ TestSystem& updTestSystem() {return TestSystem::updAs(updSystem());}
+
+ // TOPOLOGY STATE VARIABLES //
+
+ Array_<EventHandler*> eventHandlers;
+ mutable Array_<EventReporter*> eventReporters;
+
+ // TOPOLOGY CACHE //
+ mutable CacheEntryIndex myStateVars;
+ mutable CacheEntryIndex myCacheEntries;
+
+};
+
+// This Subsystem has 3 q's and 3 u's of its own, as well as whatever State
+// variables its Measures require.
+class TestSubsystem : public Subsystem {
+public:
+ TestSubsystem(System& sys) {
+ adoptSubsystemGuts(new TestSubsystemGuts());
+ sys.adoptSubsystem(*this);
+ }
+
+ Real getQSum(const State& s) {return getGuts().getQSum(s);}
+ Real getUSum(const State& s) {return getGuts().getUSum(s);}
+private:
+ const TestSubsystemGuts& getGuts() const
+ { return dynamic_cast<const TestSubsystemGuts&>(getSubsystemGuts());}
+ TestSubsystemGuts& updGuts()
+ { return dynamic_cast<TestSubsystemGuts&>(updSubsystemGuts());}
+};
+
+// Find the event triggers at a particular stage that changed sign since
+// they were last recorded in events0.
+static void findEvents(const State& state, Stage g, const Vector& triggers0,
+ Array_<EventId>& triggered)
+{
+ const int n = state.getNEventTriggersByStage(g);
+ const int start = state.getEventTriggerStartByStage(g); // location within triggers0 Vector
+
+ const Vector& stageTriggers = state.getEventTriggersByStage(g);
+
+ triggered.clear();
+ for (int i=0; i < n; ++i) {
+ const EventId allStageId = EventId(start + i);
+ if (sign(triggers0[allStageId]) != sign(stageTriggers[i]))
+ triggered.push_back(allStageId);
+ }
+}
+
+static Real accuracy = 1e-6;
+static Real timescale;
+
+static bool handleEvents(const System& sys, State& state, Stage g,
+ const Array_<EventId>& triggered)
+{
+ if (triggered.empty())
+ return false;
+
+ cout << "==> Handling " << triggered.size() << " events at Stage " << g << ":";
+ for (unsigned i=0; i < triggered.size(); ++i)
+ cout << " " << triggered[i];
+ cout << endl;
+
+ bool shouldTerminate = false;
+ HandleEventsOptions options(accuracy);
+ HandleEventsResults results;
+
+ Array_<StageVersion> stageVersions;
+ state.getSystemStageVersions(stageVersions);
+ cout << "BEFORE handling stage versions=\n";
+ cout << stageVersions << "\n";
+ sys.handleEvents(state, Event::Cause::Triggered, triggered,
+ options, results);
+ state.getSystemStageVersions(stageVersions);
+ cout << "AFTER handling stage versions=\n";
+ cout << stageVersions << "\n";
+ cout << "Results lowestStage=" << results.getLowestModifiedStage() <<"\n";
+
+ if (results.getExitStatus()==HandleEventsResults::ShouldTerminate) {
+ cout << "==> Event at Stage " << g
+ << " requested termination at t=" << state.getTime() << endl;
+ shouldTerminate = true;
+ }
+
+ return shouldTerminate;
+}
+
+template <class T>
+class MySinCos : public Measure_<T> {
+public:
+ SimTK_MEASURE_HANDLE_PREAMBLE(MySinCos, Measure_<T>);
+
+ SimTK_MEASURE_HANDLE_POSTSCRIPT(MySinCos, Measure_<T>);
+};
+
+
+template <class T>
+class MySinCos<T>::Implementation : public Measure_<T>::Implementation {
+public:
+ Implementation()
+ : Measure_<T>::Implementation(T(Vec2(0)), 1) {}
+
+ // Default copy constructor, destructor, copy assignment are fine.
+
+ // Implementations of virtual methods.
+ Implementation* cloneVirtual() const {return new Implementation(*this);}
+ int getNumTimeDerivativesVirtual() const {return 0;}
+ Stage getDependsOnStageVirtual(int order) const
+ { return Stage::Time; }
+
+ void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
+ {
+ SimTK_ASSERT1_ALWAYS(derivOrder==0,
+ "MySinCos::Implementation::calcCachedValueVirtual():"
+ " derivOrder %d seen but only 0 allowed.", derivOrder);
+
+ value[0] = std::sin(s.getTime());
+ value[1] = std::cos(s.getTime());
+ }
+};
+
+template <class T>
+class MyRealMeasure : public Measure_<T> {
+public:
+ SimTK_MEASURE_HANDLE_PREAMBLE(MyRealMeasure, Measure);
+
+ SimTK_MEASURE_HANDLE_POSTSCRIPT(MyRealMeasure, Measure);
+};
+
+template <class T>
+class MyRealMeasure<T>::Implementation : public Measure_<T>::Implementation {
+public:
+ Implementation* cloneVirtual() const OVERRIDE_11
+ { return new Implementation(*this); }
+ int getNumTimeDerivativesVirtual() const OVERRIDE_11
+ { return 0; }
+ Stage getDependsOnStageVirtual(int order) const OVERRIDE_11
+ { return Stage::Time; }
+
+};
+
+
+void testOne() {
+ TestSystem sys;
+ TestSubsystem subsys(sys);
+
+ // Add a Result measure to the system subsystem. This depends on
+ // Position stage and invalidates Dynamics and later stages.
+ Measure_<Vector>::Result vectorResult(sys.updSystemSubsystem(),
+ Stage::Position, Stage::Dynamics);
+
+ MeasureIndex vectorResultIx = vectorResult.getSubsystemMeasureIndex();
+ cout << "vectorResult index="
+ << vectorResultIx << endl;
+
+ Measure_<Vector>::Result myVecRes = Measure_<Vector>::Result::getAs(
+ sys.updSystemSubsystem().getMeasure(vectorResultIx));
+
+ Measure::Result result(sys.updSystemSubsystem(),
+ Stage::Time, Stage::Position);
+ Measure::Result autoResult(sys.updSystemSubsystem(),
+ Stage::Time, Stage::Position);
+ autoResult.setIsPresumedValidAtDependsOnStage(true);
+
+ Measure::Zero zero(subsys);
+ Measure::Constant three(subsys, 3);
+ Measure_<Vec3>::Constant v3const(subsys, Vec3(1,2,3));
+ Measure::Sinusoid cos2pit(subsys, 1, 2*Pi, Pi/2);
+
+ // Integrate the cos(2pi*t) measure with IC=0; should give sin(2pi*t)/2pi.
+ Measure::Integrate sin2pitOver2pi(subsys, cos2pit, zero);
+
+ // These two compute -cos(t), sin(t) by integrating sin(t), cos(t) with
+ // initial conditions -1,0.
+ Measure_<Vec2>::Constant cossinInit(subsys, Vec2(-1,0));
+ MySinCos<Vec2> mysincos(subsys);
+ Measure_<Vec2>::Integrate cossin(subsys, mysincos, cossinInit);
+
+ Measure_<Vector>::Constant vcossinInit(subsys, Vector(Vec2(-1,0)));
+ MySinCos<Vector> vmysincos(subsys);
+ Measure_<Vector>::Integrate vcossin(subsys, vmysincos, vcossinInit,
+ Vector(2,Zero));
+ Measure_<Vector>::Delay vcossin_delaypt1(subsys, vcossin, .1);
+
+ Measure_<Real>::Minimum minCos2pit(subsys, cos2pit);
+ Measure_<Real>::Maximum maxCos2pit(subsys, cos2pit);
+ Measure_<Real>::MinAbs minAbsCos2pit(subsys, cos2pit);
+ Measure_<Real>::MaxAbs maxAbsCos2pit(subsys, cos2pit);
+
+ Measure::Differentiate dInteg(subsys, sin2pitOver2pi);
+ dInteg.setForceUseApproximation(true);
+
+ Measure::Time tMeasure;
+ Measure::Time tSubMeas(subsys);
+
+ Measure::Delay tDelayed(subsys, tSubMeas, 0.01);
+
+ Measure::Scale t1000(subsys, 1000, tSubMeas);
+
+ Measure::Variable mv(subsys, Stage::Position, 29);
+ cout << "mv def value=" << mv.getDefaultValue() << endl;
+ mv.setDefaultValue(-19);
+ cout << "mv def value now=" << mv.getDefaultValue() << endl;
+
+ cout << "Measure::Zero=" << Measure::Zero().getValue(State()) << endl;
+ cout << "Measure::One=" << Measure::One().getValue(State()) << endl;
+
+ Measure::Plus vplus(subsys, mv, cos2pit);
+ Measure::Minus vminus(subsys, mv, cos2pit);
+
+ Measure::Plus vplus2;
+ vplus2.deepAssign(vplus);
+
+ Measure m;
+ m = cos2pit;
+
+
+
+ cout << "vplus ref count=" << vplus.getRefCount() << endl;
+ cout << "vplus2 ref count=" << vplus2.getRefCount() << endl;
+
+
+ State state = sys.realizeTopology();
+ cout << "sys topo version=" << sys.getSystemTopologyCacheVersion() << "\n";
+ cout << "state topo version=" << state.getSystemTopologyStageVersion() << "\n";
+ sys.invalidateSystemTopologyCache();
+ sys.realizeTopology();
+ cout << "sys topo version=" << sys.getSystemTopologyCacheVersion() << "\n";
+ // Use sneaky loophole since we know state is still good.
+ state.setSystemTopologyStageVersion(sys.getSystemTopologyCacheVersion());
+ sys.realizeModel(state);
+
+ m = zero;
+ cout << "m=" << m.getValue(state) << endl;
+
+
+ cout << "uWeights=" << state.getUWeights() << "\n";
+ cout << "zWeights=" << state.getZWeights() << "\n";
+
+ state.updUWeights()[1] = 9;
+ state.updZWeights() = 21;
+
+ cout << "uWeights=" << state.getUWeights() << "\n";
+ cout << "zWeights=" << state.getZWeights() << "\n";
+
+ sys.realize(state,Stage::Instance);
+
+ cout << "qerrWeights=" << state.getQErrWeights() << "\n";
+ cout << "uerrWeights=" << state.getUErrWeights() << "\n";
+
+ State dupState = state;
+ cout << "dup uWeights=" << state.getUWeights() << "\n";
+ cout << "dup zWeights=" << state.getZWeights() << "\n";
+ cout << "dup qerrWeights=" << state.getQErrWeights() << "\n";
+ cout << "dup uerrWeights=" << state.getUErrWeights() << "\n";
+
+ // Allocate vectorResult and initialize it. (Can't mark it valid yet.)
+ vectorResult.updValue(state).resize(3);
+ vectorResult.updValue(state) = Vector(Vec3(1,2,3));
+
+ result.updValue(state) = 1.234;
+ autoResult.updValue(state) = 4.321;
+
+
+ state.setTime(1.234);
+ sys.realize(state, Stage::Time);
+ cout << "Initially, tMeasure=" << tMeasure.getValue(state)
+ << " tSubMeas=" << tSubMeas.getValue(state)
+ << " 1000*tMeasure=" << t1000.getValue(state)
+ << endl;
+
+ Measure_<Mat22>::One m22Ident;
+ cout << "Measure_<Mat22>::One=" << m22Ident.getValue(state) << endl;
+
+ cout << "mv after realizeTopo=" << mv.getValue(state) << endl;
+
+ cout << "cossinInit=" << cossinInit.getValue(state) << endl;
+
+
+ State s2,s3;
+ s2 = state; // new copies of variables
+ s2 = state; // should do only assignments w/o heap allocation
+
+ // Explicit midpoint steps.
+ const Real h = .001;
+ const int nSteps = 2000;
+ const int outputInterval = 100;
+ state.setTime(0);
+
+ ASSERT(state.getTime()==0);
+
+
+ //initialize()
+ sys.realize(state, Stage::Position);
+ cout << "mv+cos2pit=" << vplus.getValue(state) << endl;
+ cout << "mv-cos2pit=" << vminus.getValue(state) << endl;
+
+ cout << "Sys stage after realize(Pos):"
+ << state.getSystemStage().getName() << endl;
+
+ mv.setValue(state, 1.234);
+
+ cout << "Sys stage after mv=1.234:"
+ << state.getSystemStage().getName() << endl;
+
+ cout << "mv is now=" << mv.getValue(state) << endl;
+
+ sys.realize(state, Stage::Position);
+
+ cout << "Realized Position:\n";
+ vectorResult.markAsValid(state);
+ cout << "vectorResult=" << vectorResult.getValue(state) << endl;
+ cout << "myVecRes=" << myVecRes.getValue(state) << endl;
+ result.markAsValid(state);
+ cout << "result=" << result.getValue(state) << endl;
+ // Shouldn't need to mark this one.
+ cout << "autoResult=" << autoResult.getValue(state) << endl;
+
+ // Fill in statics above.
+ timescale = sys.getDefaultTimeScale();
+
+ sys.realize(state, Stage::Acceleration);
+
+ cout << "Now stage=" << state.getSystemStage() << endl;
+ vectorResult.setValue(state, Vector(5,9));
+ cout << "After vectorResult.setValue(), vectorResult="
+ << vectorResult.getValue(state) << endl;
+ cout << "... but stage=" << state.getSystemStage() << endl;
+
+ cossin.setValue(state, cossinInit.getValue(state));
+ vcossin.setValue(state, vcossinInit.getValue(state));
+
+ // Handler is allowed to throw an exception if it fails since we don't
+ // have a way to recover.
+ HandleEventsOptions handleOpts;
+ HandleEventsResults results;
+ sys.handleEvents(state, Event::Cause::Initialization,
+ Array_<EventId>(), handleOpts, results);
+ SimTK_ERRCHK_ALWAYS(
+ results.getExitStatus()!=HandleEventsResults::ShouldTerminate,
+ "Integrator::initialize()",
+ "An initialization event handler requested termination.");
+
+ sys.realize(state, Stage::Acceleration);
+ state.autoUpdateDiscreteVariables(); // ??
+
+ for (int i=0; i <= nSteps; ++i) {
+
+ if (i % outputInterval == 0) {
+ sys.realize(state, Stage::Report);
+ cout << "\ntMeasure=" << tMeasure.getValue(state)
+ << " d/dt tMeasure=" << tMeasure.getValue(state,1)
+ << " d3/dt3 tMeasure=" << tMeasure.getValue(state,3)
+ << " 1000*tSubMeas=" << t1000.getValue(state)
+ << " t=" << state.getTime() << endl;
+ cout << " tDelayed=" << tDelayed.getValue(state) << endl;
+ cout << "q=" << state.getQ() << " u=" << state.getU() << endl;
+ cout << "qSum=" << subsys.getQSum(state) << " uSum=" << subsys.getUSum(state) << endl;
+ cout << "three=" << three.getValue(state) << " v3const=" << v3const.getValue(state) << endl;
+ cout << "cos2pit=" << cos2pit.getValue(state)
+ << " cos(2pi*t)=" << std::cos(2*Pi*state.getTime()) << endl;
+ cout << "Min(cos2pit)=" << minCos2pit.getValue(state)
+ << " @t=" << minCos2pit.getTimeOfExtremeValue(state) << endl;
+ cout << "Max(cos2pit)=" << maxCos2pit.getValue(state)
+ << " @t=" << maxCos2pit.getTimeOfExtremeValue(state) << endl;
+ cout << "MinAbs(cos2pit)=" << minAbsCos2pit.getValue(state)
+ << " @t=" << minAbsCos2pit.getTimeOfExtremeValue(state) << endl;
+ cout << "MaxAbs(cos2pit)=" << maxAbsCos2pit.getValue(state)
+ << " @t=" << maxAbsCos2pit.getTimeOfExtremeValue(state) << endl;
+ cout << "sin2pitOver2pi=" << sin2pitOver2pi.getValue(state)
+ << " sin(2pi*t)/2pi=" << std::sin(2*Pi*state.getTime())/(2*Pi) << endl;
+ cout << "d/dt sin2pitOver2pi="
+ << sin2pitOver2pi.getValue(state,1) << endl;
+ cout << "dInteg="
+ << dInteg.getValue(state) << endl;
+ cout << "cossin=" << cossin.getValue(state) << "\n";
+ cout << "vcossin=" << vcossin.getValue(state) << "\n";
+ cout << "vcossin delay .1=" << vcossin_delaypt1.getValue(state) << "\n";
+ }
+
+ if (i == nSteps)
+ break;
+
+ const Real h2 = h/2;
+ const Vector ydot0 = state.getYDot();
+ const Vector triggers0 = state.getEventTriggers();
+ Array_<EventId> triggered;
+
+ // Commit the values for the discrete variable updates calculated
+ // at the end of the previous step. This includes both explicitly
+ // discrete variables and continuous variables which are defined
+ // by algebraic rather than differential equations, such as
+ // prescribed motions. This requires that all calculations have
+ // been performed already using the *updated* values, *not* the
+ // state values; that permits us to perform this update without
+ // invalidating any cache entries.
+ state.autoUpdateDiscreteVariables();
+
+ // First integrator stage: unconstrained continuous system only.
+ state.updY() += h2*ydot0;
+ state.updTime() += h2;
+ sys.realize(state, Stage::Time);
+ sys.prescribeQ(state);
+ sys.realize(state, Stage::Position);
+ sys.prescribeU(state);
+ sys.realize(state);
+
+ // Second (final) integrator stage.
+ // 1. Unconstrained continuous system.
+ const Vector& ydot = state.getYDot();
+ state.updY() += h2*ydot; // that is, y = y0 + h*(ydot0+ydot)/2
+ state.updTime() += h2;
+ sys.realize(state, Stage::Time);
+ sys.prescribeQ(state);
+
+ // 2. Deal with time-dependent events.
+ findEvents(state, Stage::Time, triggers0, triggered);
+ if (handleEvents(sys, state, Stage::Time, triggered))
+ break;
+
+ sys.realize(state, Stage::Position);
+ sys.prescribeU(state);
+
+ // 3a. Project position-dependent constraints.
+ Vector temp;
+ ProjectOptions opts(accuracy);
+ ProjectResults results;
+ sys.projectQ(state, temp, opts, results);
+
+ // 3b. Handle position-dependent events.
+ findEvents(state, Stage::Position, triggers0, triggered);
+ if (handleEvents(sys, state, Stage::Position, triggered))
+ break;
+
+ // 4a. Project velocity-dependent constraints.
+ sys.realize(state, Stage::Velocity);
+ sys.projectU(state, temp, opts, results);
+
+ // 4b. Handle velocity-dependent events.
+ findEvents(state, Stage::Velocity, triggers0, triggered);
+ if (handleEvents(sys, state, Stage::Velocity, triggered))
+ break;
+
+ // 5. Handle dynamics- and acceleration-dependent events.
+ sys.realize(state, Stage::Dynamics);
+ findEvents(state, Stage::Dynamics, triggers0, triggered);
+ if (handleEvents(sys, state, Stage::Dynamics, triggered))
+ break;
+ sys.realize(state, Stage::Acceleration);
+ findEvents(state, Stage::Acceleration, triggers0, triggered);
+ if (handleEvents(sys, state, Stage::Acceleration, triggered))
+ break;
+
+ // Ensure State is realized through Acceleration Stage.
+ sys.realize(state, Stage::Acceleration);
+ }
+}
+
+int main() {
+ try {
+ testOne();
+ } catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ cout << "Done" << endl;
+ return 0;
+}
+
diff --git a/SimTKcommon/tests/TestSmallMatrix.cpp b/SimTKcommon/tests/TestSmallMatrix.cpp
new file mode 100644
index 0000000..608262f
--- /dev/null
+++ b/SimTKcommon/tests/TestSmallMatrix.cpp
@@ -0,0 +1,502 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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 "SimTKcommon/Testing.h"
+
+#include <iostream>
+
+using std::cout;
+using std::endl;
+using namespace SimTK;
+using namespace std;
+
+// A matrix operation of size N can be expected to achieve an
+// accuracy of about N*tol where tol is the expected accuracy
+// of a scalar operation.
+
+template <int N>
+void testInverse() {
+ Mat<N,N> identity(1);
+ Mat<N,N> mat = Test::randMat<N,N>();
+ SimTK_TEST_EQ_SIZE(mat*mat.invert(), identity, N);
+ mat = ~mat;
+ SimTK_TEST_EQ_SIZE(mat*mat.invert(), identity, N);
+ SimTK_TEST_EQ_SIZE((-mat)*(-mat).invert(), identity, N);
+}
+
+void testDotProducts() {
+ Vec3 v1(1, 2, 3);
+ Vec3 v2(-1, -2, -3);
+ Row3 r1(0.1, 0.2, 0.3);
+ Row3 r2(-0.1, -0.2, -0.3);
+ SimTK_TEST_EQ(dot(v1, v2), -14.0);
+ SimTK_TEST_EQ(dot(r1, r2), -0.14);
+ SimTK_TEST_EQ(dot(v1, r2), -1.4);
+ SimTK_TEST_EQ(dot(r1, v2), -1.4);
+ SimTK_TEST_EQ(r1*v2, -1.4);
+ SpatialVec sv(Vec3(1, 2, 3), Vec3(4, 5, 6));
+ SpatialRow sr(Row3(1, 2, 3), Row3(4, 5, 6));
+ SimTK_TEST_EQ(sr*sv, 91.0);
+}
+
+void testCrossProducts() {
+ const Vec3 w = Test::randVec3();
+ const Vec3 v = Test::randVec3();
+ const Vec3 wp = UnitVec3(w).perp() * Test::randReal();
+ const Vec3 vp = UnitVec3(v).perp() * Test::randReal();
+ const Mat33 vx = crossMat(v);
+ const Mat33 wx = crossMat(w);
+ const Mat33 vpx = crossMat(vp);
+ const Mat33 wpx = crossMat(wp);
+
+ const Mat33 m33 = Test::randMat33();
+ const Mat34 m = Test::randMat<3,4>();
+ const Mat43 mt = ~m;
+ const Mat<3,1> m1 = Test::randMat<3,1>();
+
+
+ SimTK_TEST_EQ( w%v, Vec3(w[1]*v[2]-w[2]*v[1],
+ w[2]*v[0]-w[0]*v[2],
+ w[0]*v[1]-w[1]*v[0]) );
+
+ SimTK_TEST_EQ( w % v, cross( w, v));
+ SimTK_TEST_EQ( ~w % ~v, -cross( ~v, ~w));
+
+ SimTK_TEST_EQ( wx*v, w % v );
+ SimTK_TEST_EQ( crossMat(~w), wx );
+ SimTK_TEST_EQ( crossMat(-w), ~wx );
+ SimTK_TEST_EQ( crossMatSq(w)*v, -w % (w%v) );
+
+ // cross(vector, matrix) (columnwise)
+ Mat34 c = v % m;
+ SimTK_TEST_EQ(c(0), v%m(0));
+ SimTK_TEST_EQ(c(1), v%m(1));
+ SimTK_TEST_EQ(c(2), v%m(2));
+ SimTK_TEST_EQ(c(3), v%m(3));
+ SimTK_TEST_EQ(c, vx*m);
+ SimTK_TEST_EQ(c, (~v)%m); // row same as col here
+
+ Mat<3,1> c1 = v % m1;
+ SimTK_TEST_EQ(c1(0), v%m1(0));
+
+ // cross(matrix, vector) (rowwise)
+ Mat43 cr = mt % w;
+ SimTK_TEST_EQ(cr[0], mt[0]%w);
+ SimTK_TEST_EQ(cr[1], mt[1]%w);
+ SimTK_TEST_EQ(cr[2], mt[2]%w);
+ SimTK_TEST_EQ(cr[3], mt[3]%w);
+ SimTK_TEST_EQ(cr, mt*wx);
+ SimTK_TEST_EQ(cr, mt % (~w)); // row same as col here
+
+ SimTK_TEST_EQ( vx * m33 * vx, v % m33 % v );
+}
+
+// Individually test 2x2, 3x3, and 4x4 because the
+// smaller sizes may have specialized inline operators.
+void testSymMat() {
+ // 2x2
+ const Vec3 a = Test::randVec3();
+ const Vec<2> v = Test::randVec<2>();
+ SymMat<2> sm( a[0],
+ a[1], a[2] );
+ Mat<2,2> m( a[0], a[1],
+ a[1], a[2] );
+
+ SimTK_TEST_EQ( (Mat<2,2>(sm)), m );
+ SimTK_TEST_EQ( sm, SymMat<2>().setFromSymmetric(m) );
+
+ SimTK_TEST_EQ( sm*v, m*v );
+ SimTK_TEST_EQ( ~v*sm, ~v*m );
+
+ // 3x3
+ const Vec<6> a3 = Test::randVec<6>();
+ const Vec<3> v3 = Test::randVec<3>();
+ SymMat<3> sm3( a3[0],
+ a3[1], a3[2],
+ a3[3], a3[4], a3[5]);
+ Mat<3,3> m3( a3[0], a3[1], a3[3],
+ a3[1], a3[2], a3[4],
+ a3[3], a3[4], a3[5]);
+
+ SimTK_TEST_EQ( (Mat<3,3>(sm3)), m3 );
+ SimTK_TEST_EQ( sm3, SymMat<3>().setFromSymmetric(m3) );
+
+ SimTK_TEST_EQ( sm3*v3, m3*v3 );
+ SimTK_TEST_EQ( ~v3*sm3, ~v3*m3 );
+
+ // 4x4 (hopefully the general case)
+ const Vec<10> a4 = Test::randVec<10>();
+ const Vec<4> v4 = Test::randVec<4>();
+ SymMat<4> sm4( a4[0],
+ a4[1], a4[2],
+ a4[3], a4[4], a4[5],
+ a4[6], a4[7], a4[8], a4[9]);
+ Mat<4,4> m4( a4[0], a4[1], a4[3], a4[6],
+ a4[1], a4[2], a4[4], a4[7],
+ a4[3], a4[4], a4[5], a4[8],
+ a4[6], a4[7], a4[8], a4[9]);
+
+ SimTK_TEST_EQ( (Mat<4,4>(sm4)), m4 );
+ SimTK_TEST_EQ( sm4, SymMat<4>().setFromSymmetric(m4) );
+
+ SimTK_TEST_EQ( sm4*v4, m4*v4 );
+ SimTK_TEST_EQ( ~v4*sm4, ~v4*m4 );
+
+
+ // Complex is tricky for symmetric (really Hermitian) matrices because
+ // the diagonals must be real and the corresponding off-diagonals are
+ // complex conjugate pairs, NOT the same value even though the off
+ // diagonal data is only stored once.
+ const Vec<3,Complex> ac( Test::randComplex(), Test::randComplex(), Test::randComplex() );
+ const Vec<2,Complex> vc( Test::randComplex(), Test::randComplex() );
+ SymMat<2,Complex> smc( ac[0],
+ ac[1], ac[2] );
+ Mat<2,2,Complex> mc( ac[0].real(), std::conj(ac[1]),
+ ac[1], ac[2].real() );
+
+ // This constructor has to figure out how to generate a conjugate
+ // element for the upper right in the full Mat.
+ Mat<2,2,Complex> sm2mc(smc);
+ SimTK_TEST_EQ( sm2mc, mc );
+ SimTK_TEST_EQ( smc, (SymMat<2,Complex>().setFromSymmetric(mc)) );
+
+ SimTK_TEST_EQ( smc*vc, mc*vc );
+ SimTK_TEST_EQ( ~vc*smc, ~vc*mc );
+
+ SimTK_TEST_EQ( ~smc*vc, ~mc*vc );
+ SimTK_TEST_EQ( ~smc*vc, smc*vc );
+}
+
+void testNumericallyEqual() {
+ Mat<3,4,float> fm1(1), fm1e(1), fm1n(1), fm1nz(1);
+ fm1e(1,1) += (float)(0.5*fm1.getDefaultTolerance()); // should test equal
+ fm1n(2,2) += (float)(2 *fm1.getDefaultTolerance()); // should test not equal
+ fm1nz(1,3) = (float)(2 *fm1.getDefaultTolerance()); // should test not equal
+
+ const Mat<3,4,float> fmident34( 1, 0, 0, 0,
+ 0, 1, 0, 0,
+ 0, 0, 1, 0 );
+
+
+
+ SimTK_TEST(fm1==fmident34); // exact
+ SimTK_TEST(fm1.isNumericallyEqual(fm1));
+ SimTK_TEST(fm1.isNumericallyEqual(fm1e));
+ SimTK_TEST(!fm1.isNumericallyEqual(fm1n));
+ SimTK_TEST(!fm1.isNumericallyEqual(fm1nz));
+ SimTK_TEST((fm1.getSubMat<3,3>(0,0).isNumericallyEqual(fm1nz.getSubMat<3,3>(0,0))));
+ SimTK_TEST((fm1e.getSubMat<3,3>(0,0).isNumericallyEqual(fm1nz.getSubMat<3,3>(0,0))));
+ SimTK_TEST(!(fm1n.getSubMat<3,3>(0,0).isNumericallyEqual(fm1nz.getSubMat<3,3>(0,0))));
+
+ // Tightening tolerance should make fm1e not equal.
+ SimTK_TEST(!fm1.isNumericallyEqual(fm1e, .3*fm1.getDefaultTolerance()));
+ SimTK_TEST(fm1.isNumericallyEqual(1.f));
+ SimTK_TEST(fm1e.isNumericallyEqual(1.f));
+ SimTK_TEST(!fm1n.isNumericallyEqual(1.f));
+
+ Mat<3,4,double> dm1(1), dfm1e(fm1e);
+ // Try mixed-precision.
+ SimTK_TEST(dm1.isNumericallyEqual(fm1));
+ SimTK_TEST(dm1.isNumericallyEqual(fm1e)); // because should use float tolerance
+ SimTK_TEST(!dm1.isNumericallyEqual(dfm1e)); // because should use double tolerance
+ SimTK_TEST(dm1.isNumericallyEqual(dfm1e, fm1e.getDefaultTolerance())); // force float tolerance
+
+ // Repeat for symmetric matrix.
+
+ SymMat<3,float> fs1(1), fs1e(1), fs1n(1), fs1nz(1);
+ fs1e(1,1) += (float)(0.5*fs1.getDefaultTolerance()); // should test equal
+ fs1n(2,2) += (float)(2 *fs1.getDefaultTolerance()); // should test not equal
+ fs1nz(2,1) = (float)(2 *fs1.getDefaultTolerance()); // should test not equal
+
+ const SymMat<3,float> fsident3( 1,
+ 0, 1,
+ 0, 0, 1 );
+
+ SimTK_TEST(fs1==fsident3); // exact
+ SimTK_TEST(fs1.isNumericallyEqual(fs1));
+ SimTK_TEST(fs1.isNumericallyEqual(fs1e));
+ SimTK_TEST(!fs1.isNumericallyEqual(fs1n));
+ SimTK_TEST(!fs1.isNumericallyEqual(fs1nz));
+ // Tightening tolerance should make fs1e not equal.
+ SimTK_TEST(!fs1.isNumericallyEqual(fs1e, .3*fs1.getDefaultTolerance()));
+ SimTK_TEST(fs1.isNumericallyEqual(1.f));
+ SimTK_TEST(fs1e.isNumericallyEqual(1.f));
+ SimTK_TEST(!fs1n.isNumericallyEqual(1.f));
+
+ SymMat<3,double> ds1(1), dfs1e(fs1e);
+ // Try mixed-precision.
+ SimTK_TEST(ds1.isNumericallyEqual(fs1));
+ SimTK_TEST(ds1.isNumericallyEqual(fs1e)); // because should use float tolerance
+ SimTK_TEST(!ds1.isNumericallyEqual(dfs1e)); // because should use double tolerance
+ SimTK_TEST(ds1.isNumericallyEqual(dfs1e, fs1e.getDefaultTolerance())); // force float tolerance
+
+ // Check Vec and Row.
+
+ Vec<3,float> fv1(1), fv1e(1), fv1n(1); // should be 1 1 1
+ Row<3,float> fr1(1);
+ fv1e[1] += (float)(0.5*fv1.getDefaultTolerance()); // should test equal
+ fv1n[0] += (float)(2 *fv1.getDefaultTolerance()); // should test not equal
+
+ const Vec<3,float> fone(1,1,1);
+ SimTK_TEST(fv1==fone); // exact
+ SimTK_TEST(fv1.isNumericallyEqual(fv1));
+ SimTK_TEST(fv1.isNumericallyEqual(fv1e));
+ SimTK_TEST(!fv1.isNumericallyEqual(fv1n));
+
+ SimTK_TEST(fr1==~fone); // exact
+ SimTK_TEST(fr1.isNumericallyEqual(~fv1));
+ SimTK_TEST(fr1.isNumericallyEqual(~fv1e));
+ SimTK_TEST(!fr1.isNumericallyEqual(~fv1n));
+
+ // Check symmetry tests in Mat.
+ Mat<2,7,double> notSquare(0); // can't be symmetric
+ SimTK_TEST(!notSquare.isExactlySymmetric());
+ SimTK_TEST(!notSquare.isNumericallySymmetric());
+
+ Mat<3,3,float> f33(1), // exactly symmetric
+ f33e(1), // numerically symmetric
+ f33n(1); // too sloppy
+ f33e(1,2) += (float)(0.5*f33.getDefaultTolerance()); // should test equal
+ f33n(2,0) += (float)(2 *f33.getDefaultTolerance()); // should test not equal
+
+ SimTK_TEST(f33.isExactlySymmetric());
+ SimTK_TEST(!f33e.isExactlySymmetric());
+ SimTK_TEST(!f33n.isExactlySymmetric());
+ SimTK_TEST(f33.isNumericallySymmetric());
+ SimTK_TEST(f33e.isNumericallySymmetric());
+ SimTK_TEST(!f33n.isNumericallySymmetric());
+
+ // Things are trickier for complex matrices where symmetry means
+ // Hermitian (conjugate) symmetry.
+ // This one has *positional* symmetry, not Hermitian.
+ Mat<2,2, std::complex<double> > mcp( std::complex<double>(1,2), std::complex<double>(3,4),
+ std::complex<double>(3,4), std::complex<double>(5,6) );
+ // This one is Hermitian.
+ Mat<2,2, std::complex<double> > mch( std::complex<double>(1,0), std::complex<double>(3,-4),
+ std::complex<double>(3,4), std::complex<double>(5,0) );
+
+ SimTK_TEST(!mcp.isExactlySymmetric());
+ SimTK_TEST(!mcp.isNumericallySymmetric());
+
+ SimTK_TEST(mch.isExactlySymmetric());
+ SimTK_TEST(mch.isNumericallySymmetric());
+
+ // This should be OK because mch is symmetric.
+ SymMat<2, std::complex<double> > symTest;
+ symTest.setFromSymmetric(mch);
+
+ mch(0,1) += 0.5*mch.getDefaultTolerance();
+ SimTK_TEST(!mch.isExactlySymmetric());
+ SimTK_TEST(mch.isNumericallySymmetric());
+
+ // This should be OK because mch is almost symmetric.
+ symTest.setFromSymmetric(mch);
+
+ mch(0,1) += 5*mch.getDefaultTolerance();
+ SimTK_TEST(!mch.isExactlySymmetric());
+ SimTK_TEST(!mch.isNumericallySymmetric());
+
+
+ // This should throw in Debug mode because mch is too far off.
+ #ifndef NDEBUG
+ SimTK_TEST_MUST_THROW(symTest.setFromSymmetric(mch));
+ #endif
+
+}
+
+static bool isXAxis(const UnitVec3& test) {return test==UnitVec3(1,0,0);}
+static bool isNegZAxis(const UnitVec3& test) {return test==UnitVec3(0,0,-1);}
+
+void testUnitVec() {
+ SimTK_TEST_EQ(Vec3(UnitVec3(1,1,0)), Vec3(Sqrt2/2,Sqrt2/2,0));
+ SimTK_TEST(UnitVec3(XAxis) == UnitVec3(1,0,0));
+ SimTK_TEST(UnitVec3(YAxis) == UnitVec3(0,1,0));
+ SimTK_TEST(UnitVec3(ZAxis) == UnitVec3(0,0,1));
+ SimTK_TEST(UnitVec3(NegXAxis) == UnitVec3(-1,0,0));
+ SimTK_TEST(UnitVec3(NegYAxis) == UnitVec3(0,-1,0));
+ SimTK_TEST(UnitVec3(NegZAxis) == UnitVec3(0,0,-1));
+
+ SimTK_TEST(isXAxis(XAxis)); // implicit conversion
+ SimTK_TEST(!isXAxis(YAxis)); // implicit conversion
+ SimTK_TEST(isNegZAxis(NegZAxis)); // implicit conversion
+ SimTK_TEST(!isNegZAxis(NegYAxis)); // implicit conversion
+ SimTK_TEST(!isNegZAxis(ZAxis)); // implicit conversion
+
+ SimTK_TEST(XAxis.dotProduct(XAxis) == 1);
+ SimTK_TEST(XAxis.dotProduct(YAxis) == 0);
+ SimTK_TEST(XAxis.dotProduct(ZAxis) == 0);
+ SimTK_TEST(ZAxis.dotProduct(YAxis) == 0);
+
+ SimTK_TEST(XAxis.crossProductSign(XAxis) == 0);
+
+ SimTK_TEST(XAxis.crossProductAxis(YAxis) == ZAxis);
+ SimTK_TEST(XAxis.crossProductSign(YAxis) == 1);
+
+ SimTK_TEST(XAxis.crossProductAxis(ZAxis) == YAxis);
+ SimTK_TEST(XAxis.crossProductSign(ZAxis) == -1);
+
+ SimTK_TEST(ZAxis.crossProductAxis(XAxis) == YAxis);
+ SimTK_TEST(ZAxis.crossProductSign(XAxis) == 1);
+
+ int sign;
+ CoordinateAxis axis = ZAxis.crossProduct(YAxis, sign);
+ SimTK_TEST(CoordinateDirection(axis,sign) == NegXAxis);
+
+ SimTK_TEST(CoordinateDirection(YAxis,CoordinateDirection::Negative())
+ == NegYAxis);
+
+ SimTK_TEST(-XAxis == NegXAxis);
+ SimTK_TEST(-YAxis == NegYAxis);
+ SimTK_TEST(-ZAxis == NegZAxis);
+ SimTK_TEST(-ZAxis != NegYAxis);
+ SimTK_TEST(ZAxis == -NegZAxis);
+ SimTK_TEST(-(-NegXAxis) == -XAxis);
+ SimTK_TEST(-(-NegYAxis) == -(-(-YAxis)));
+
+ SimTK_TEST(NegXAxis.crossProductAxis(NegYAxis) == ZAxis);
+ SimTK_TEST(NegYAxis.crossProductAxis(ZAxis) == XAxis);
+ SimTK_TEST(NegYAxis.crossProductSign(ZAxis) == -1);
+ SimTK_TEST(NegYAxis.crossProductSign(NegZAxis) == 1);
+
+
+}
+
+// Test append/drop/insert rows and columns.
+void testAppendRowCol() {
+ Mat34 m34( 1, 2, 3, 4,
+ 5, 6, 7, 8,
+ 9, 10, 11, 12);
+
+ Mat44 m44 = m34.appendRow(Row4(-1, -2, -3, -4));
+ SimTK_TEST(m44==Mat44( 1, 2, 3, 4,
+ 5, 6, 7, 8,
+ 9, 10, 11, 12,
+ -1, -2, -3, -4));
+ m44 = m34.insertRow(0, Row4(-1, -2, -3, -4));
+ SimTK_TEST(m44==Mat44( -1, -2, -3, -4,
+ 1, 2, 3, 4,
+ 5, 6, 7, 8,
+ 9, 10, 11, 12));
+
+ m44 = m34.insertRow(1, Row4(-1, -2, -3, -4));
+ SimTK_TEST(m44==Mat44( 1, 2, 3, 4,
+ -1, -2, -3, -4,
+ 5, 6, 7, 8,
+ 9, 10, 11, 12));
+
+ m44 = m34.insertRow(3, Row4(-1, -2, -3, -4));
+ SimTK_TEST(m44==m34.appendRow(Row4(-1, -2, -3, -4)));
+
+ Mat35 m35 = m34.appendCol(Vec3(-6, -7, -8));
+ SimTK_TEST(m35==Mat35( 1, 2, 3, 4, -6,
+ 5, 6, 7, 8, -7,
+ 9, 10, 11, 12, -8));
+
+ m35 = m34.insertCol(0, Vec3(-6, -7, -8));
+ SimTK_TEST(m35==Mat35( -6, 1, 2, 3, 4,
+ -7, 5, 6, 7, 8,
+ -8, 9, 10, 11, 12));
+
+ m35 = m34.insertCol(2, Vec3(-6, -7, -8));
+ SimTK_TEST(m35==Mat35( 1, 2, -6, 3, 4,
+ 5, 6, -7, 7, 8,
+ 9, 10, -8, 11, 12));
+
+ m35 = m34.insertCol(4, Vec3(-6, -7, -8));
+ SimTK_TEST(m35==m34.appendCol(Vec3(-6, -7, -8)));
+
+ Mat45 m45 = m34.appendRowCol(Row5(-1, -2, -3, -4, -5),
+ Vec4(-6, -7, -8, -9));
+ SimTK_TEST(m45==Mat45( Row5(1, 2, 3, 4, -6),
+ Row5(5, 6, 7, 8, -7),
+ Row5(9, 10, 11, 12, -8),
+ Row5(-1, -2, -3, -4, -9)));
+
+ m45 = m34.insertRowCol(0,0, Row5(-1, -2, -3, -4, -5),
+ Vec4(-6, -7, -8, -9));
+ SimTK_TEST(m45==Mat45( Row5(-6, -2, -3, -4, -5),
+ Row5(-7, 1, 2, 3, 4),
+ Row5(-8, 5, 6, 7, 8),
+ Row5(-9, 9, 10, 11, 12)));
+
+
+
+ m45 = m34.insertRowCol(1,2, Row5(-1, -2, -3, -4, -5),
+ Vec4(-6, -7, -8, -9));
+ SimTK_TEST(m45==Mat45( Row5( 1, 2, -6, 3, 4),
+ Row5(-1, -2, -7, -4, -5),
+ Row5( 5, 6, -8, 7, 8),
+ Row5( 9, 10, -9, 11, 12)));
+
+ m45 = m34.insertRowCol(3,4, Row5(-1, -2, -3, -4, -5),
+ Vec4(-6, -7, -8, -9));
+ SimTK_TEST(m45== m34.appendRowCol(Row5(-1, -2, -3, -4, -5),
+ Vec4(-6, -7, -8, -9)));
+
+ Mat24 m24_0 = m34.dropRow(0);
+ Mat24 m24_1 = m34.dropRow(1);
+ Mat24 m24_2 = m34.dropRow(2);
+
+ SimTK_TEST(m24_0==Mat24(5, 6, 7, 8,
+ 9, 10, 11, 12));
+ SimTK_TEST(m24_1==Mat24(1, 2, 3, 4,
+ 9, 10, 11, 12));
+ SimTK_TEST(m24_2==Mat24(1, 2, 3, 4,
+ 5, 6, 7, 8));
+
+ Mat33 m33_0 = m34.dropCol(0);
+ Mat33 m33_1 = m34.dropCol(1);
+ Mat33 m33_2 = m34.dropCol(2);
+ Mat33 m33_3 = m34.dropCol(3);
+
+ SimTK_TEST(m33_0==Mat33( 2, 3, 4,
+ 6, 7, 8,
+ 10, 11, 12));
+ SimTK_TEST(m33_1==Mat33( 1, 3, 4,
+ 5, 7, 8,
+ 9, 11, 12));
+ SimTK_TEST(m33_2==Mat33( 1, 2, 4,
+ 5, 6, 8,
+ 9, 10, 12));
+ SimTK_TEST(m33_3==Mat33( 1, 2, 3,
+ 5, 6, 7,
+ 9, 10, 11));
+
+}
+
+int main() {
+ SimTK_START_TEST("TestSmallMatrix");
+ SimTK_SUBTEST(testSymMat);
+ SimTK_SUBTEST(testInverse<1>);
+ SimTK_SUBTEST(testInverse<2>);
+ SimTK_SUBTEST(testInverse<3>);
+ SimTK_SUBTEST(testInverse<5>);
+ SimTK_SUBTEST(testInverse<10>);
+ SimTK_SUBTEST(testDotProducts);
+ SimTK_SUBTEST(testCrossProducts);
+ SimTK_SUBTEST(testNumericallyEqual);
+ SimTK_SUBTEST(testUnitVec);
+ SimTK_SUBTEST(testAppendRowCol);
+ SimTK_END_TEST();
+}
diff --git a/SimTKcommon/tests/TestVectorMath.cpp b/SimTKcommon/tests/TestVectorMath.cpp
new file mode 100644
index 0000000..bec99f3
--- /dev/null
+++ b/SimTKcommon/tests/TestVectorMath.cpp
@@ -0,0 +1,357 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS((cond), "Assertion failed");}
+#define ASSERT_EQUAL(val1, val2) {ASSERT(std::abs((val1)-(val2)) < 1e-10);}
+
+using std::cout;
+using std::endl;
+using namespace SimTK;
+using namespace std;
+
+static Real sqrt(int a) {return std::sqrt(Real(a));}
+static Real sin(int a) {return std::sin(Real(a));}
+static Real cos(int a) {return std::cos(Real(a));}
+static Real tan(int a) {return std::tan(Real(a));}
+static Real asin(int a) {return std::asin(Real(a));}
+static Real acos(int a) {return std::acos(Real(a));}
+static Real atan(int a) {return std::atan(Real(a));}
+static Real exp(int a) {return std::exp(Real(a));}
+static Real log(int a) {return std::log(Real(a));}
+static Real cosh(int a) {return std::cosh(Real(a));}
+static Real sinh(int a) {return std::sinh(Real(a));}
+static Real tanh(int a) {return std::tanh(Real(a));}
+
+template <class T, int N>
+void testVector(const T& value, const Vec<N>& expected) {
+ ASSERT(value.size() == N);
+ for (int i = 0; i < N; ++i) {
+ if (isNaN(expected[i])) {
+ ASSERT(isNaN(value[i]));
+ }
+ else {
+ ASSERT_EQUAL(value[i], expected[i]);
+ }
+ }
+}
+
+template <class T, int M, int N>
+void testMatrix(const T& value, const Mat<M, N>& expected) {
+ ASSERT(value.nrow() == M);
+ ASSERT(value.ncol() == N);
+ for (int i = 0; i < M; ++i)
+ for (int j = 0; j < N; ++j) {
+ if (isNaN(expected(i, j))) {
+ ASSERT(isNaN(value(i, j)));
+ }
+ else {
+ ASSERT_EQUAL(value(i, j), expected(i, j));
+ }
+ }
+}
+
+template <class T, int N>
+void testSymMat(const T& value, const SymMat<N>& expected) {
+ ASSERT(value.nrow() == N);
+ for (int i = 0; i < N; ++i)
+ for (int j = 0; j <= i; ++j) {
+ if (isNaN(expected(i, j))) {
+ ASSERT(isNaN(value(i, j)));
+ }
+ else {
+ ASSERT_EQUAL(value(i, j), expected(i, j));
+ }
+ }
+}
+
+int main() {
+ try {
+ // Create a bunch of vectors and matrices that will be used for testing.
+
+ Vector vector(5);
+ vector[0] = -1;
+ vector[1] = 2;
+ vector[2] = -3;
+ vector[3] = 4;
+ vector[4] = -5;
+ RowVector rowvector = ~vector;
+ Vec5 vec = Vec5(-1, 2, -3, 4, -5);
+ Row5 row = Row5(-1, 2, -3, 4, -5);
+ Matrix matrix(2, 3);
+ matrix[0][0] = -1;
+ matrix[0][1] = 2;
+ matrix[0][2] = -3;
+ matrix[1][0] = 4;
+ matrix[1][1] = -5;
+ matrix[1][2] = 6;
+ Mat<2,3> mat = Mat<2,3>(-1, 2, -3,
+ 4, -5, 6);
+ Real temp[] = {-1, 2, -3};
+ SymMat<2> symmat = SymMat<2>(temp);
+
+ // Test the abs function.
+
+ Vec5 expectedVec = Vec5(1, 2, 3, 4, 5);
+ Mat<2,3> expectedMat = Mat<2,3>(1, 2, 3,
+ 4, 5, 6);
+ testVector(abs(vector), expectedVec);
+ testVector(abs(rowvector), expectedVec);
+ testVector(abs(vec), expectedVec);
+ testVector(abs(row), expectedVec);
+ testMatrix<Matrix,2,3>(abs(matrix), expectedMat);
+ testMatrix<Mat<2,3>,2,3>(abs(mat), expectedMat);
+ testSymMat(abs(symmat), SymMat<2>(Mat22(1, 2,
+ 2, 3)));
+
+ // Test the exp function.
+
+ expectedVec = Vec5(exp(-1), exp(2), exp(-3), exp(4), exp(-5));
+ expectedMat = Mat<2,3>(exp(-1), exp( 2), exp(-3),
+ exp( 4), exp(-5), exp( 6));
+ testVector(exp(vector), expectedVec);
+ testVector(exp(rowvector), expectedVec);
+ testVector(exp(vec), expectedVec);
+ testVector(exp(row), expectedVec);
+ testMatrix<Matrix,2,3>(exp(matrix), expectedMat);
+ testMatrix<Mat<2,3>,2,3>(exp(mat), expectedMat);
+ testSymMat(exp(symmat), SymMat<2>(Mat22(exp(-1), exp( 2),
+ exp( 2), exp(-3))));
+
+ // Test the log function.
+
+ expectedVec = Vec5(log(-1), log(2), log(-3), log(4), log(-5));
+ expectedMat = Mat<2,3>(log(-1), log(2), log(-3), log(4), log(-5), log(6));
+ testVector(log(vector), expectedVec);
+ testVector(log(rowvector), expectedVec);
+ testVector(log(vec), expectedVec);
+ testVector(log(row), expectedVec);
+ testMatrix<Matrix,2,3>(log(matrix), expectedMat);
+ testMatrix<Mat<2,3>,2,3>(log(mat), expectedMat);
+ testSymMat(log(symmat), SymMat<2>(Mat22(log(-1), log(2),
+ log(2), log(-3))));
+
+ // Test the sqrt function.
+
+ expectedVec = Vec5(sqrt(-1), sqrt(2), sqrt(-3), sqrt(4), sqrt(-5));
+ expectedMat = Mat<2,3>(sqrt(-1), sqrt(2), sqrt(-3), sqrt(4), sqrt(-5), sqrt(6));
+ testVector(sqrt(vector), expectedVec);
+ testVector(sqrt(rowvector), expectedVec);
+ testVector(sqrt(vec), expectedVec);
+ testVector(sqrt(row), expectedVec);
+ testMatrix<Matrix,2,3>(sqrt(matrix), expectedMat);
+ testMatrix<Mat<2,3>,2,3>(sqrt(mat), expectedMat);
+ testSymMat(sqrt(symmat), SymMat<2>(Mat22(sqrt(-1), sqrt(2),
+ sqrt(2), sqrt(-3))));
+
+ // Test the sin function.
+
+ expectedVec = Vec5(sin(-1), sin(2), sin(-3), sin(4), sin(-5));
+ expectedMat = Mat<2,3>(sin(-1), sin(2), sin(-3), sin(4), sin(-5), sin(6));
+ testVector(sin(vector), expectedVec);
+ testVector(sin(rowvector), expectedVec);
+ testVector(sin(vec), expectedVec);
+ testVector(sin(row), expectedVec);
+ testMatrix<Matrix,2,3>(sin(matrix), expectedMat);
+ testMatrix<Mat<2,3>,2,3>(sin(mat), expectedMat);
+ testSymMat(sin(symmat), SymMat<2>(Mat22(sin(-1), sin(2),
+ sin(2), sin(-3))));
+
+ // Test the cos function.
+
+ expectedVec = Vec5(cos(-1), cos(2), cos(-3), cos(4), cos(-5));
+ expectedMat = Mat<2,3>(cos(-1), cos(2), cos(-3), cos(4), cos(-5), cos(6));
+ testVector(cos(vector), expectedVec);
+ testVector(cos(rowvector), expectedVec);
+ testVector(cos(vec), expectedVec);
+ testVector(cos(row), expectedVec);
+ testMatrix<Matrix,2,3>(cos(matrix), expectedMat);
+ testMatrix<Mat<2,3>,2,3>(cos(mat), expectedMat);
+ testSymMat(cos(symmat), SymMat<2>(Mat22(cos(-1), cos(2),
+ cos(2), cos(-3))));
+
+ // Test the tan function.
+
+ expectedVec = Vec5(tan(-1), tan(2), tan(-3), tan(4), tan(-5));
+ expectedMat = Mat<2,3>(tan(-1), tan(2), tan(-3), tan(4), tan(-5), tan(6));
+ testVector(tan(vector), expectedVec);
+ testVector(tan(rowvector), expectedVec);
+ testVector(tan(vec), expectedVec);
+ testVector(tan(row), expectedVec);
+ testMatrix<Matrix,2,3>(tan(matrix), expectedMat);
+ testMatrix<Mat<2,3>,2,3>(tan(mat), expectedMat);
+ testSymMat(tan(symmat), SymMat<2>(Mat22(tan(-1), tan(2),
+ tan(2), tan(-3))));
+
+ // Test the asin function.
+
+ expectedVec = Vec5(asin(-1), asin(2), asin(-3), asin(4), asin(-5));
+ expectedMat = Mat<2,3>(asin(-1), asin(2), asin(-3), asin(4), asin(-5), asin(6));
+ testVector(asin(vector), expectedVec);
+ testVector(asin(rowvector), expectedVec);
+ testVector(asin(vec), expectedVec);
+ testVector(asin(row), expectedVec);
+ testMatrix<Matrix,2,3>(asin(matrix), expectedMat);
+ testMatrix<Mat<2,3>,2,3>(asin(mat), expectedMat);
+ testSymMat(asin(symmat), SymMat<2>(Mat22(asin(-1), asin(2),
+ asin(2), asin(-3))));
+
+ // Test the asin function.
+
+ expectedVec = Vec5(acos(-1), acos(2), acos(-3), acos(4), acos(-5));
+ expectedMat = Mat<2,3>(acos(-1), acos(2), acos(-3), acos(4), acos(-5), acos(6));
+ testVector(acos(vector), expectedVec);
+ testVector(acos(rowvector), expectedVec);
+ testVector(acos(vec), expectedVec);
+ testVector(acos(row), expectedVec);
+ testMatrix<Matrix,2,3>(acos(matrix), expectedMat);
+ testMatrix<Mat<2,3>,2,3>(acos(mat), expectedMat);
+ testSymMat(acos(symmat), SymMat<2>(Mat22(acos(-1), acos(2),
+ acos(2), acos(-3))));
+
+ // Test the atan function.
+
+ expectedVec = Vec5(atan(-1), atan(2), atan(-3), atan(4), atan(-5));
+ expectedMat = Mat<2,3>(atan(-1), atan(2), atan(-3), atan(4), atan(-5), atan(6));
+ testVector(atan(vector), expectedVec);
+ testVector(atan(rowvector), expectedVec);
+ testVector(atan(vec), expectedVec);
+ testVector(atan(row), expectedVec);
+ testMatrix<Matrix,2,3>(atan(matrix), expectedMat);
+ testMatrix<Mat<2,3>,2,3>(atan(mat), expectedMat);
+ testSymMat(atan(symmat), SymMat<2>(Mat22(atan(-1), atan(2),
+ atan(2), atan(-3))));
+
+ // Test the sinh function.
+
+ expectedVec = Vec5(sinh(-1), sinh(2), sinh(-3), sinh(4), sinh(-5));
+ expectedMat = Mat<2,3>(sinh(-1), sinh(2), sinh(-3), sinh(4), sinh(-5), sinh(6));
+ testVector(sinh(vector), expectedVec);
+ testVector(sinh(rowvector), expectedVec);
+ testVector(sinh(vec), expectedVec);
+ testVector(sinh(row), expectedVec);
+ testMatrix<Matrix,2,3>(sinh(matrix), expectedMat);
+ testMatrix<Mat<2,3>,2,3>(sinh(mat), expectedMat);
+ testSymMat(sinh(symmat), SymMat<2>(Mat22(sinh(-1), sinh(2),
+ sinh(2), sinh(-3))));
+
+ // Test the cosh function.
+
+ expectedVec = Vec5(cosh(-1), cosh(2), cosh(-3), cosh(4), cosh(-5));
+ expectedMat = Mat<2,3>(cosh(-1), cosh(2), cosh(-3), cosh(4), cosh(-5), cosh(6));
+ testVector(cosh(vector), expectedVec);
+ testVector(cosh(rowvector), expectedVec);
+ testVector(cosh(vec), expectedVec);
+ testVector(cosh(row), expectedVec);
+ testMatrix<Matrix,2,3>(cosh(matrix), expectedMat);
+ testMatrix<Mat<2,3>,2,3>(cosh(mat), expectedMat);
+ testSymMat(cosh(symmat), SymMat<2>(Mat22(cosh(-1), cosh(2),
+ cosh(2), cosh(-3))));
+
+ // Test the tanh function.
+
+ expectedVec = Vec5(tanh(-1), tanh(2), tanh(-3), tanh(4), tanh(-5));
+ expectedMat = Mat<2,3>(tanh(-1), tanh(2), tanh(-3), tanh(4), tanh(-5), tanh(6));
+ testVector(tanh(vector), expectedVec);
+ testVector(tanh(rowvector), expectedVec);
+ testVector(tanh(vec), expectedVec);
+ testVector(tanh(row), expectedVec);
+ testMatrix<Matrix,2,3>(tanh(matrix), expectedMat);
+ testMatrix<Mat<2,3>,2,3>(tanh(mat), expectedMat);
+ testSymMat(tanh(symmat), SymMat<2>(Mat22(tanh(-1), tanh(2),
+ tanh(2), tanh(-3))));
+
+ // Test the sum function.
+
+ ASSERT_EQUAL(sum(vector), -3);
+ ASSERT_EQUAL(sum(rowvector), -3);
+ ASSERT_EQUAL(sum(vec), -3);
+ ASSERT_EQUAL(sum(row), -3);
+ testVector(sum(matrix), Vec3(3, -3, 3));
+ testVector(sum(mat), Vec3(3, -3, 3));
+ testVector(sum(symmat), Vec2(1, -1));
+
+ // Test the min function.
+
+ ASSERT_EQUAL(min(vector), -5);
+ ASSERT_EQUAL(min(rowvector), -5);
+ ASSERT_EQUAL(min(vec), -5);
+ ASSERT_EQUAL(min(row), -5);
+ testVector(min(matrix), Vec3(-1, -5, -3));
+ testVector(min(mat), Vec3(-1, -5, -3));
+ testVector(min(symmat), Vec2(-1, -3));
+
+ // Test the max function.
+
+ ASSERT_EQUAL(max(vector), 4);
+ ASSERT_EQUAL(max(rowvector), 4);
+ ASSERT_EQUAL(max(vec), 4);
+ ASSERT_EQUAL(max(row), 4);
+ testVector(max(matrix), Vec3(4, 2, 6));
+ testVector(max(mat), Vec3(4, 2, 6));
+ testVector(max(symmat), Vec2(2, 2));
+
+ // Test the mean function.
+
+ ASSERT_EQUAL(mean(vector), -0.6);
+ ASSERT_EQUAL(mean(rowvector), -0.6);
+ ASSERT_EQUAL(mean(vec), -0.6);
+ ASSERT_EQUAL(mean(row), -0.6);
+ testVector(mean(matrix), Vec3(1.5, -1.5, 1.5));
+ testVector(mean(mat), Vec3(1.5, -1.5, 1.5));
+ testVector(mean(symmat), Vec2(0.5, -0.5));
+
+ // Test the sort function.
+
+ expectedVec = Vec5(-5, -3, -1, 2, 4);
+ expectedMat = Mat<2,3>(-1, -5, -3, 4, 2, 6);
+ testVector(sort(vector), expectedVec);
+ testVector(sort(rowvector), expectedVec);
+ testVector(sort(vec), expectedVec);
+ testVector(sort(row), expectedVec);
+ testMatrix<Matrix,2,3>(sort(matrix), expectedMat);
+ testMatrix<Mat<2,3>,2,3>(sort(mat), expectedMat);
+ testMatrix<Mat<2,2>,2,2>(sort(symmat), Mat22(-1, -3,
+ 2, 2));
+
+ // Test the median function.
+
+ ASSERT_EQUAL(median(vector), -1);
+ ASSERT_EQUAL(median(rowvector), -1);
+ ASSERT_EQUAL(median(vec), -1);
+ ASSERT_EQUAL(median(row), -1);
+ testVector(median(matrix), Vec3(1.5, -1.5, 1.5));
+ testVector(median(mat), Vec3(1.5, -1.5, 1.5));
+ testVector(median(symmat), Vec2(0.5, -0.5));
+ ASSERT_EQUAL(median(Vec6(6, 1, 5, 2, 4, 3)), 3.5);
+ } catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ cout << "Done" << endl;
+ return 0;
+}
+
diff --git a/SimTKcommon/tests/TestXml.cpp b/SimTKcommon/tests/TestXml.cpp
new file mode 100644
index 0000000..837585b
--- /dev/null
+++ b/SimTKcommon/tests/TestXml.cpp
@@ -0,0 +1,340 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "SimTKcommon/Testing.h"
+
+#include "SimTKcommon/internal/Xml.h"
+
+#include <iostream>
+#include <string>
+#include <cstdio>
+using std::cout;
+using std::cin;
+using std::endl;
+using std::string;
+using std::printf;
+
+using namespace SimTK;
+
+// This example is from Wikipedia's XML entry.
+const char* xmlPainting =
+"<?xml version='1.0' encoding='UTF-8'?>\n"
+"<!-- a top-level comment -->\n"
+"<!-- a multiline\n comment\n third line -->\n"
+" \n" // line should be ignored
+"but something like this is top level text and will need to get \n"
+"moved into a new '_Root' element\n"
+"<painting artist='Raphael' artist='metoo'>\n"
+" <img src=\"madonna.jpg\" alt='Foligno Madonna, by Raphael'/>\n"
+" <!-- What follows is a so-called 'caption' -->\n"
+" <caption>This is Raphael's \"Foligno\" Madonna, painted in\n"
+" <date>1511</date>-<date>1512</date>.\n"
+" <![CDATA[some non-Unicode text]]>\n"
+" < !SOMETHING but this tag is unknown> \n"
+" </caption>\n"
+" This part is just plain old text.\n"
+" As is this \"quoted\" thing.\n"
+"</painting>\n"
+"<!whazzis unknown junk>\n"
+"<!-- final comment -->";
+
+const char* xmlPlainTextFile =
+"That is, the first line should be a declaration, most commonly exactly the\n"
+"characters shown above, without the \"standalone\" attribute which will\n"
+"default to \"yes\" anyway. If we don't see a declaration when reading an XML\n"
+"document, we'll assume we read the one above. Then the document should contain\n"
+"exactly one top-level (root) element representing the type of document &\n"
+"document-level attributes.\n";
+
+const char* xmlUnclosedComment =
+"<?xml version='1.0' encoding='UTF-8'?>\n"
+" <!-- What follows is a so-called 'caption' ->\n" // UNCLOSED!
+"<!whazzis unknown junk>";
+
+const char* xmlJustAComment =
+"<!-- this is the entire contents -->\n";
+
+const char* xmlEmpty = " \n \n \t "; // white space only
+
+
+static void showElement(Xml::Element elt, const String& indent="") {
+ cout << indent << "ELEMENT WITH TAG '" << elt.getElementTag() << "':\n";
+
+ // Show attributes
+ Xml::attribute_iterator ap = elt.attribute_begin();
+ for (; ap != elt.attribute_end(); ++ap)
+ cout << indent << " ATTR '" << ap->getName() << "'='"
+ << ap->getValue() << "'\n";
+
+ // Show all contents
+ Xml::node_iterator p = elt.node_begin();
+ for (; p != elt.node_end(); ++p) {
+ cout << indent << p->getNodeTypeAsString() << endl;
+ if (p->getNodeType() == Xml::ElementNode)
+ showElement(Xml::Element::getAs(*p), indent + " ");
+ }
+ cout << indent << "END OF ELEMENT.\n";
+}
+
+void testXmlFromString() {
+ Xml::Document fromString;
+ fromString.readFromString(xmlJustAComment);
+ cout << "Just a comment: '" << fromString << "'\n";
+
+ fromString.readFromString(xmlPlainTextFile);
+ cout << "Plain text file: '" << fromString << "'\n";
+
+ // Note that the "condense white space" setting is global, not
+ // document-specific.
+ Xml::Document preserveWhite;
+ Xml::setXmlCondenseWhiteSpace(false);
+ SimTK_TEST(!Xml::isXmlWhiteSpaceCondensed());
+ preserveWhite.readFromString(xmlPlainTextFile);
+ cout << "Plain text file with white space preserved (raw): "
+ << preserveWhite.getRootElement().getValue() << "\n";
+ cout << "... (formatted with condense=false): "
+ << preserveWhite << "\n";
+ Xml::setXmlCondenseWhiteSpace(true);
+ cout << "... (formatted with condense=true): "
+ << preserveWhite << "\n";
+
+ SimTK_TEST_MUST_THROW(fromString.readFromString(xmlEmpty));
+ SimTK_TEST_MUST_THROW(fromString.readFromString(xmlUnclosedComment));
+
+ fromString.readFromString(String(xmlPainting));
+ cout << "Painting: '" << fromString << "'\n";
+
+ cout << "Doc type is: " << fromString.getRootTag() << endl;
+ cout << " version: " << fromString.getXmlVersion() << endl;
+ cout << " encoding: " << fromString.getXmlEncoding() << endl;
+ cout << " standalone: "
+ << String(fromString.getXmlIsStandalone() ? "yes" : "no") << endl;
+
+ cout << "All nodes in doc:\n";
+ Xml::node_iterator np = fromString.node_begin();
+ for (; np != fromString.node_end(); ++np)
+ cout << " " << np->getNodeTypeAsString() << endl;
+
+
+ Xml::Element root = fromString.getRootElement();
+
+ cout << "hasNode()=" << root.hasNode() << endl;
+ cout << "hasNode(Comment)=" << root.hasNode(Xml::CommentNode) << endl;
+ cout << "hasNode(Unknown)=" << root.hasNode(Xml::UnknownNode) << endl;
+
+ showElement(root);
+
+ Xml::node_iterator p = root.node_begin(Xml::NoJunkNodes);
+ for (; p != root.node_end(); ++p)
+ cout << p->getNodeTypeAsString() << endl;
+
+ cout << "Caption elements:\n";
+ Xml::element_iterator ep(root.element_begin("caption"));
+ for (; ep != root.element_end(); ++ep)
+ cout << ep->getNodeTypeAsString() << ": <" << ep->getElementTag()
+ << ">" << endl;
+
+ cout << "All elements:\n";
+ Array_<Xml::Element> all = root.getAllElements();
+ for (unsigned i=0; i < all.size(); ++i)
+ cout << "<" << all[i].getElementTag() << ">" << endl;
+
+
+ Array_<Xml::Node> allNodes(root.node_begin(Xml::NoJunkNodes),
+ root.node_end());
+ for (unsigned i=0; i < allNodes.size(); ++i)
+ cout << "Node " << allNodes[i].getNodeTypeAsString() << endl;
+
+ String prettyString, compactString;
+ fromString.writeToString(prettyString);
+ cout << "String pretty: " << prettyString.size()
+ << "\n'" << prettyString << "'\n";
+
+ fromString.writeToString(compactString, true); // compact
+ cout << "String compact: " << compactString.size()
+ << "\n'" << compactString << "'\n";
+
+ SimTK_TEST(compactString.size() < prettyString.size());
+
+ cout << "painting.allNode=" << root.getRequiredElement("painting")
+ .getAllNodes() << endl;
+ cout << "painting.img.allAttr=" <<
+ root.getRequiredElement("painting").getRequiredElement("img")
+ .getAllAttributes() << endl;
+ //fromString.writeToFile("TestXml.xml");
+
+ //Xml ex("TestXml.xml");
+ //cout << "Document tag: " << ex.getDocumentTag() << endl;
+ //for (Xml::nodu;e_iterator xp=ex.node_begin(); xp != ex.node_end(); ++xp)
+ // cout << "Node type: " << xp->getNodeTypeAsString() << endl;
+
+ //PolygonalMesh mesh;
+ //mesh.loadVtpFile("arm_r_humerus.vtp");
+ //cout << "num vertices=" << mesh.getNumVertices() << " faces="
+ // << mesh.getNumFaces() << endl;
+}
+
+void testXmlFromScratch() {
+ Xml::Document scratch;
+ scratch.setRootTag("MyDoc");
+ cout << scratch;
+
+ Xml::Comment c("This is a comment.");
+ Xml::Unknown u("!GODONLY knows what this is!!");
+ Xml::Text t("This is some\ntext on two lines with trailing blanks ");
+ Xml::Element e("elementTag");
+
+ // We're never going to use this one so its heap space will
+ // leak if we don't explicitly call clearOrphan().
+ Xml::Element neverMind("neverMind");
+
+ cout << "initially e='" << e.getValue() << "'" << endl;
+ e.updValue() += "AVALUE:";
+ cout << "then e='" << e.getValue() << "'" << endl;
+
+ e.setAttributeValue("attr1", String(Vec2(9,-9)));
+
+ cout << "attr1 is " << e.getRequiredAttributeValueAs<Vec2>("attr1") << endl;
+
+ cout << "isOrphan? " << String(c.isOrphan()) << ":" << c;
+ cout << "isOrphan? " << String(u.isOrphan()) << ":" << u;
+ cout << "isOrphan? " << String(t.isOrphan()) << ":" << t;
+ cout << "isOrphan? " << String(e.isOrphan()) << ":" << e;
+
+ e.setValue("this is the only value");
+ e.updValue() += " (but then I added this)";
+ cout << "e value=" << e.getValue() << endl;
+ cout << "e = " << e << endl;
+ e.setValue("9 10 -3.2e-4");
+ cout << "e value=" << e.getValueAs< Array_<float> >() << endl;
+ cout << "e = " << e << endl;
+
+ scratch.insertTopLevelNodeAfter(scratch.node_begin(), c);
+ cout << "isOrphan? " << String(c.isOrphan()) << ":" << c;
+ cout << scratch;
+
+ scratch.insertTopLevelNodeBefore(scratch.node_begin(Xml::ElementNode), u);
+ cout << "isOrphan? " << String(u.isOrphan()) << ":" << u;
+ cout << scratch;
+
+ scratch.insertTopLevelNodeBefore(scratch.node_begin(),
+ Xml::Comment("This should be at the top of the file, except declaration."));
+ cout << scratch;
+
+ Xml scratch2;
+ scratch2 = scratch; // deep copy
+
+ scratch.eraseTopLevelNode(scratch.node_begin());
+ cout << "First node gone (scratch)?\n" << scratch;
+ cout << "First node still there (scratch2)?\n" << scratch2;
+
+ Xml::Element e2("anotherElt", Vec3(.1,.2,.3));
+ cout << e2;
+ e.insertNodeAfter(e.element_end(), e2);
+ cout << "now owns anotherElt:\n" << e;
+
+ Xml::Element root = scratch.getRootElement();
+ root.insertNodeAfter(root.node_begin(), e);
+ cout << scratch;
+
+ scratch.setIndentString("..");
+ cout << scratch;
+
+ Xml::Element ecopy = e.clone();
+ ecopy.setElementTag("elementTagCopy");
+ cout << "COPY of e: " << ecopy;
+
+ //scratch.writeToFile("scratch.xml");
+
+ e.eraseNode(e.element_begin("anotherElt"));
+ cout << "in-place removal of anotherElt from e: " << scratch;
+ cout << "COPY of e: " << ecopy;
+
+ root.insertNodeAfter(root.element_begin("elementTag"), ecopy);
+ cout << "After copy insert, scratch=" << scratch;
+
+ Xml::Node extract = root.removeNode(root.element_begin("elementTagCopy"));
+ cout << "Extracted copy: " << extract << endl;
+ cout << "Now scratch=" << scratch << endl;
+ cout << "Duplicate scratch=" << Xml::Document(scratch) << endl;
+
+ neverMind.clearOrphan();
+}
+
+#define SHOWIT(something) \
+ cout << String(#something) << String("'") << something << String("'\n")
+
+void testStringConvert() {
+ SimTK_TEST(convertStringTo<int>(" 239\n ")==239);
+ SimTK_TEST(convertStringTo<String>(" lunch box\n") == " lunch box\n");
+ SimTK_TEST(convertStringTo<std::string>(" lunch box\n") == " lunch box\n");
+ SimTK_TEST(convertStringTo<int>("1234")==1234);
+ SimTK_TEST(convertStringTo<unsigned>("01234")==1234);
+ SimTK_TEST(convertStringTo<float>("1234.5")==1234.5);
+ SimTK_TEST_MUST_THROW(convertStringTo<char*>(" lunch box\n"));
+ SimTK_TEST_MUST_THROW(convertStringTo<int>(" 234 j"));
+ SimTK_TEST_MUST_THROW(convertStringTo<int>("345.5"));
+
+ SimTK_TEST(convertStringTo< std::complex<double> >("(-4,22)")
+ ==std::complex<double>(-4,22));
+ SimTK_TEST(convertStringTo< Vec3 >("1 2 3") == Vec3(1,2,3));
+ SimTK_TEST(convertStringTo< Vec3 >("1, 2 , 3") == Vec3(1,2,3));
+ SimTK_TEST(convertStringTo< Vec3 >("[ -3 , 5, 6 ] ")== Vec3(-3,5,6));
+ SimTK_TEST(convertStringTo< Vec3 >("( -3 5 -6 ) ")== Vec3(-3,5,-6));
+ SimTK_TEST(convertStringTo< Vec3 >(" ~ [ -3 , 5, 6 ] ")== Vec3(-3,5,6));
+ SimTK_TEST(convertStringTo< Vec3 >("~( -3 5 -6 ) ")== Vec3(-3,5,-6));
+ SimTK_TEST_MUST_THROW(convertStringTo< Vec3 >("( -3 5 -6 ] "));
+ SimTK_TEST_MUST_THROW(convertStringTo< Vec3 >(" -3 5 -6 ] "));
+ SimTK_TEST_MUST_THROW(convertStringTo< Vec3 >(" ~ -3 5 -6 "));
+ typedef Vec<2,std::complex<float> > fCVec2;
+ SimTK_TEST(convertStringTo<fCVec2>("[(1,2) (3,4)]")
+ == fCVec2(std::complex<float>(1,2), std::complex<float>(3,4)));
+
+ Array_<int> a = convertStringTo< Array_<int> >("1 2 3 4");
+
+ Array_<float> af(2);
+ SimTK_TEST_MUST_THROW( // because ArrayView_ is fixed size (2)
+ String(" -.25, .5, 29.2e4 ").convertTo<ArrayView_<float> >(af));
+ // But this should work because an Array_ can be resized.
+ String(" -.25, .5, 29.2e4 ").convertTo<Array_<float> >(af);
+ SimTK_TEST(af[0]==-.25 && af[1]==.5 && af[2]==292000);
+
+}
+
+
+int main() {
+ cout << "Path of this executable: '" << Pathname::getThisExecutablePath() << "'\n";
+ cout << "Executable directory: '" << Pathname::getThisExecutableDirectory() << "'\n";
+ cout << "Current working directory: '" << Pathname::getCurrentWorkingDirectory() << "'\n";
+
+
+ SimTK_START_TEST("TestXml");
+
+ SimTK_SUBTEST(testStringConvert);
+ SimTK_SUBTEST(testXmlFromString);
+ SimTK_SUBTEST(testXmlFromScratch);
+
+ SimTK_END_TEST();
+}
+
diff --git a/SimTKcommon/tests/adhoc/BigMatrixTest.cpp b/SimTKcommon/tests/adhoc/BigMatrixTest.cpp
new file mode 100644
index 0000000..70cec5f
--- /dev/null
+++ b/SimTKcommon/tests/adhoc/BigMatrixTest.cpp
@@ -0,0 +1,859 @@
+/* -------------------------------------------------------------------------- *
+ * SimTK Simbody: SimTKcommon *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 <cstdlib> // for rand()
+#include <cstdio>
+#include <ctime>
+#include <string>
+#include <complex>
+#include <iostream>
+using std::cout;
+using std::endl;
+using std::complex;
+
+using namespace SimTK;
+
+// Numerical Recipes declarations.
+namespace NR {
+ template <class DP>
+ void lubksb(const int N, const DP* a/*N,N*/, const int* indx/*N*/,
+ DP* b/*N*/);
+
+ template <class DP>
+ void ludcmp(const int N, DP* a/*N,N*/, int* indx/*N*/, DP &d);
+
+ template <class DP>
+ void luinvert(int N, DP* a/*N,N*/, DP* y/*N,N*/);
+}
+
+// Some explicit instantiations just to make sure everything's there.
+namespace SimTK {
+template class Matrix_<Real>;
+template class Vector_<Complex>;
+template class RowVector_< conjugate<float> >;
+//template class MatrixBase< Mat<3,4,Vec2> >;
+
+template class MatrixView_< complex<double> >;
+template class VectorView_< negator<float> >;
+template class RowVectorView_< negator< conjugate<float> > >;
+}
+
+template <class NT>
+void dump(const String& s, const Matrix_<NT>& mm) {
+ cout << s << ":" << endl;
+ for (int i=0; i<mm.nrow(); ++i) {
+ for (int j=0; j<mm.ncol(); ++j)
+ cout << mm(i,j) << " ";
+ cout << endl;
+ }
+}
+
+template <class NT>
+void dump(const String& s, const MatrixView_<NT>& mv)
+ { dump(s,(const Matrix_<NT>&)mv); }
+
+extern "C" {
+ void SimTK_version_SimTKlapack(int* major, int* minor, int* build);
+ void SimTK_about_SimTKlapack(const char* key, int maxlen, char* value);
+}
+
+void testSums() {
+ Matrix m(Mat22(1,2,
+ 3,4));
+ SimTK_TEST_EQ(m.colSum(), RowVector(Row2(4,6)));
+ SimTK_TEST_EQ(m.rowSum(), Vector(Vec2(3,7)));
+ SimTK_TEST_EQ(m.sum(), m.colSum()); // should be exact
+
+ Matrix_<Complex> mc(Mat<2,2,Complex>(1+2*I, 3+4*I,
+ 5+6*I, 7+8*I));
+ typedef Row<2,Complex> CRow2;
+ typedef Vec<2,Complex> CVec2;
+ typedef RowVector_<Complex> CRowVector;
+ typedef Vector_<Complex> CVector;
+ SimTK_TEST_EQ(mc.colSum(), CRowVector(CRow2(6+8*I, 10+12*I)));
+ SimTK_TEST_EQ(mc.rowSum(), CVector(CVec2(4+6*I, 12+14*I)));
+ SimTK_TEST_EQ(mc.sum(), mc.colSum()); // should be exact
+}
+
+void testCharacter() {
+ MatrixCommitment mc;
+ // mc = MatrixCommitment::Symmetric();
+ //mc.commitOutline(MatrixOutline::Square);
+ cout << mc;
+ cout << mc.calcDefaultCharacter(0,0);
+ Matrix m(mc);
+ m.resize(2,3);
+ m = 5;
+ m.dump("m");
+ cout << "NOW: " << m.getMatrixCharacter();
+ cout << "m.diag()=" << m.diag() << endl;
+ cout << "m.diag() " << m.diag().getMatrixCharacter();
+ cout << "m[1]=" << m[1] << endl;
+ Matrix mm = m*3;
+ cout << "mm=" << mm;
+
+ MatrixView mmv = mm(0,1,2,2);
+ cout << "mmv(mm(0,1,2,2):" << mmv;
+ cout << "mm(0,0,2,2):" << mm(0,0,2,2);
+ mmv.clear();
+ mmv = mm(0,0,1,2);
+ cout << "mmv.clear(), then mmv=mm(0,0,1,2): " << mmv;
+
+
+ mc = MatrixCommitment( MatrixStructure(MatrixStructure::Triangular, MatrixStructure::Upper) );
+ cout << "mc=" << mc << " actual=" << mc.calcDefaultCharacter(0,0);
+ Matrix t(mc);
+ t.resize(5,3);
+ t.dump("t");
+
+ t.clear();
+ t.commitTo(MatrixStructure(MatrixStructure::Hermitian, MatrixStructure::Upper));
+ t.dump("t now hermitian, prior to resize");
+ cout << "t commit=" << t.getCharacterCommitment() << "t actual=" << t.getMatrixCharacter();
+ t.resize(3,5);
+ t.dump("t after resize");
+ cout << "t commit=" << t.getCharacterCommitment() << "t actual=" << t.getMatrixCharacter();
+ t = 123;
+ t(0,2)=-2;
+ t.dump("t now hermitian");
+ cout << "t commit=" << t.getCharacterCommitment() << "t actual=" << t.getMatrixCharacter();
+
+ Vector v(10);
+ for (int i=0; i<10; ++i) v[i] = i*.1;
+ v[4] = -17.3;
+ cout << "v commitment: " << v.getCharacterCommitment();
+ cout << "v character: " << v.getMatrixCharacter();
+ cout << "v=" << v << endl;
+ Vector w1(10, Real(1));
+ cout << "weights w1=" << w1 << endl;
+ int worst;
+ cout << "|w1*v|_rms=" << v.weightedNormRMS(w1,&worst) << "\n";
+ cout << "(worst=" << worst << ")\n";
+ cout << "|w1*v|_inf=" << v.weightedNormInf(w1,&worst) << "\n";
+ cout << "(worst=" << worst << ")\n";
+ cout << "|v|_rms=" << v.normRMS(&worst) << "\n";
+ cout << "(worst=" << worst << ")\n";
+ cout << "|v|_inf=" << v.normInf(&worst) << "\n";
+ cout << "(worst=" << worst << ")\n";
+ cout << "2nd sig: |v|_rms=" << v.normRMS() << "\n";
+ cout << "2nd sig: |v|_inf=" << v.normInf() << "\n";
+
+ w1(9) = 100;
+ cout << "weights w1=" << w1 << endl;
+ cout << "|w1*v|_rms=" << v.weightedNormRMS(w1,&worst) << "\n";
+ cout << "(worst=" << worst << ")\n";
+ cout << "|w1*v|_inf=" << v.weightedNormInf(w1,&worst) << "\n";
+ cout << "(worst=" << worst << ")\n";
+ cout << "2nd sig: |w1*v|_rms=" << v.weightedNormRMS(w1) << "\n";
+ cout << "2nd sig: |w1*v|_inf=" << v.weightedNormInf(w1) << "\n";
+
+ cout << "rms(zero length)=" << Vector().normRMS(&worst) << "\n";
+ cout << " worst=" << worst << "\n";
+ cout << "inf(zero length)=" << Vector().normInf(&worst) << "\n";
+ cout << " worst=" << worst << "\n";
+
+
+ Array_<int> vx;
+ vx.push_back(2); vx.push_back(5); vx.push_back(7); vx.push_back(8);
+
+ VectorView vxx = v(vx);
+ cout << "vxx character: " << vxx.getMatrixCharacter();
+ cout << "vxx=" << vxx << endl;
+ cout << "vxx(1,2)=" << vxx(1,2) << endl;
+
+ Complex cmplx[] = {Complex(1,2), Complex(3,4), Complex(-.2,.3),
+ Complex(-100,200), Complex(20,40), Complex(-.001,.002)};
+
+ ComplexMatrix cm(2,3,2,cmplx); // shared data is in column order
+ cout << "cm(2,3,lda=2,cmplx)=" << cm;
+
+ ComplexMatrix cm2(2,3,cmplx); // init constructor takes data in row order
+ cout << "cm(2,3,cmplx)=" << cm2;
+
+ // Share data but note that it is row order. Using MatrixBase to get to
+ // a general constructor.
+ MatrixCharacter::LapackFull mchar(2,3);
+ mchar.setStorage(MatrixStorage(MatrixStorage::NoPacking,MatrixStorage::RowOrder));
+ MatrixBase<Complex> cm3(MatrixCommitment(), mchar, 3, cmplx);
+ cout << "cm3(RowOrder,2,3,lda=3,cmplx)=" << cm3;
+ cm3(0,1)=99;
+ cout << "cm3(0,1)=99 ->" << cm3;
+ cout << "cm (should have changed too) ->" << cm;
+ cout << "cm2 (should not have changed) ->" << cm2;
+}
+
+// Test "scalar" multiply for Vectors and RowVectors that have CNT types
+// as elements.
+// NOTE: the Vector elements and the "scalar" CNT must conform for this
+// to work; nonconforming will cause obscure compile errors.
+void testScalarMultiply() {
+ cout << "\n------ TEST SCALAR MULTIPLY ------\n";
+ Mat33 m33( .03, .04, .05,
+ .06, .08, .09,
+ .07, .10, .11 );
+ Vector_< SpatialVec > vs(3, SpatialVec(Vec3(1,2,3), Vec3(4,5,6)));
+ cout << "vs=" << vs << endl;
+ cout << "vs*SpatialRow=" << vs*SpatialRow(Row3(.1)) << endl;
+ cout << "SpatialRow*vs=" << SpatialRow(Row3(.1))*vs << endl;
+ cout << "m33 * SpatialVec=" << m33 * SpatialVec(Vec3(1,2,3), Vec3(4,5,6)) << endl;
+ cout << "m33 * vs=" << SpatialMat(m33) * vs << endl; // note cast to conforming diagonal matrix
+ cout << "SpatialRow * m33=" << ~SpatialVec(Vec3(1,2,3), Vec3(4,5,6)) * m33 << endl;
+ cout << "~vs*m33=" << ~vs * SpatialMat(m33) << endl; // note cast to conforming diagonal matrix
+ RowVector_< SymMat22 > rv(3, SymMat22(1,2,3));
+ cout << "rv=" << rv << endl;
+ cout << "rv*Vec2=" << rv*Vec2(.1,.2) << endl;
+ cout << "Row2*rv=" << Row2(.1,.2)*rv << endl;
+ cout << "------ END TEST SCALAR MULTIPLY ------\n\n";
+}
+
+
+void testAjaysBlock() {
+ cout << "\n------ TEST AJAY'S BLOCK ------\n";
+ const int nu =7, nm=4;
+ Matrix J(6,nu);
+ for (int i=0; i<6; ++i)
+ for (int j=0; j<nu; ++j)
+ J(i,j) = 1000*i+j;
+ Matrix t = ~J(0,3,3,nm);
+ cout << "J=" << J << endl;
+ cout << "t=" << t;
+ cout << "\n------ END TEST AJAY'S BLOCK ------\n";
+}
+
+int main()
+{
+ try {
+ SimTK_DEBUG("Running BigMatrixTest ...\n");
+
+ testSums();
+
+ Matrix assignToMe(5,4);
+ assignToMe.elementwiseAssign(1.);
+ std::cout << "assignToMe=" << assignToMe;
+ assignToMe.elementwiseAssign(14);
+ std::cout << "assignToMe=" << assignToMe;
+
+ testAjaysBlock();
+ testScalarMultiply();
+ testCharacter();
+
+ int major,minor,build;
+ char out[100];
+ const char* keylist[] = { "version", "library", "type", "debug", "authors", "copyright", "svn_revision", 0 };
+
+ //SimTK_version_SimTKlapack(&major,&minor,&build);
+ //std::printf("SimTKlapack library version: %d.%d.%d\n", major, minor, build);
+
+
+ SimTK_version_SimTKcommon(&major,&minor,&build);
+ std::printf("==> SimTKcommon library version: %d.%d.%d\n", major, minor, build);
+ std::printf(" SimTK_about_SimTKcommon():\n");
+ for (const char** p = keylist; *p; ++p) {
+ SimTK_about_SimTKcommon(*p, 100, out);
+ std::printf(" about(%s)='%s'\n", *p, out);
+ }
+
+ const Real vvvdata[] = {1,2,.1,.2,9,10,-22,-23,-24,25};
+ Vector vvv(10,vvvdata);
+ cout << "vvv=" << vvv << endl;
+ cout << "vvv(2,5)=" << vvv(2,5) << endl;
+ Vector vvv25;
+ vvv25.viewAssign(vvv(2,5));
+ cout << "vvv25=" << vvv25 << endl;
+ cout << "vvv(2,0)=" << vvv(2,0) << endl;
+ Vector vvv20;
+ vvv20.viewAssign(vvv(2,0));
+ cout << "vvv20=" << vvv20 << endl;
+ cout << "vvv(0,0)=" << vvv(0,0) << endl;
+ Vector vvv00;
+ vvv00.viewAssign(vvv(0,0));
+ cout << "vvv00=" << vvv00 << endl;
+ Vector vb;
+ vvv00 = vb;
+ cout << "after vvv00=vb [null], vvv00(" << vvv00.nrow() << "," << vvv00.ncol() << ")=" << vvv00 << endl;
+
+ const Complex mdc[] = { Complex(1.,2.),
+ Complex(3.,4.),
+ Complex(5.,6.),
+ Complex(7.,8.),
+ Complex(9.,10.),
+ Complex(10.,11.),
+ Complex(.1,.26),
+ Complex(.3,.45),
+ Complex(.5,.64),
+ Complex(.7,.83),
+ Complex(.9,.102),
+ Complex(.10,.111)
+ };
+ Matrix_<Complex> md(2,2,mdc);
+ cout << "2x2 complex Matrix md=" << md;
+ Mat<2,2,Complex> md_mat(mdc);
+ cout << "2x2 complex Mat md_mat=" << md_mat;
+ cout << " sizeof(Complex)=" << sizeof(Complex) << " sizeof(md_mat)=" << sizeof(md_mat) << endl;
+ Matrix_<Mat<2,2,Complex> > mm22c;
+ Mat<2,2, Mat<2,2,Complex> > mm22c_mat;
+ cout << " sizeof(mm22c_mat)=" << sizeof(mm22c_mat) << " should be " << 16*sizeof(Complex) << endl;
+ mm22c.resize(2,2); cout << " sizeof(mm22c(2,2))=" << ((char*)(&mm22c(1,1)(1,1)+1)) - ((char*)&mm22c(0,0)(0,0)) << endl;
+
+ cout << "scalar md.normRMS: " << md.normRMS() << endl;
+ try {
+ cout << "nonscalar mm22c.normRMS: " << mm22c.normRMS() << endl;
+ } catch(const std::exception& e) {
+ cout << "SHOULD FAIL FOR NONSCALAR ELEMENTS: " << e.what() << endl;
+ }
+
+
+ mm22c.setToZero(); cout << "mm22c after setToZero():" << mm22c;
+ mm22c.setToNaN(); cout << "mm22c after setToNaN():" << mm22c;
+
+ mm22c.dump("**** mm22c ****");
+
+ cout << "~md=" << ~md;
+ cout << "~md(1)=" << ~md(1) << endl;
+ cout << "(~md)(1)=" << (~md)(1) << endl;
+ cout << "~md[1]=" << ~md[1] << endl;
+ cout << "(~md)[1]=" << (~md)[1] << endl;
+
+ dump("2x2 complex md",md);
+ dump("md(0,1,2,1)",md(0,1,2,1));
+ const ComplexMatrixView& mvc = md(0,1,2,1);
+ dump("mvc=md(0,1,2,1)", mvc);
+ md(1,0) *= 10.;
+ dump("md after md(1,0) *= 10.",md);
+ md(0,1,2,1) *= Complex(10.,100.);
+ dump("md after md(0,1,2,1)*=(10+100i)",md);
+
+ Matrix_<Complex> mm(3,4);
+ mm = 2390.;
+ cout << "after [3x4 complex] mm = 2390, mm: " << mm << endl;
+ for (int i=0; i<mm.nrow(); ++i) {
+ for (int j=0; j<mm.ncol(); ++j)
+ mm(i,j)=(i+1)*(j+1);
+ }
+ cout << "after mm(i,j)=(i+1)*(j+1), mm: " << mm << endl;
+
+ Vector mmColScale(4), mmRowScale(3);
+ mmColScale[0]=1; mmColScale[1]=10; mmColScale[2]=100; mmColScale[3]=1000;
+ mmRowScale[0]=-1000; mmRowScale[1]=-100; mmRowScale[2]=-10;
+ Vector_<double> mmRowScaleR(3); for(int i=0;i<3;++i) mmRowScaleR[i]=(float)mmRowScale[i];
+
+ cout << "mm=" << mm << " mmColScale=" << mmColScale << endl;
+ mm.colScaleInPlace(mmColScale);
+ cout << "after col scale, mm=" << mm;
+
+ mm.colScaleInPlace(mmColScale.elementwiseInvert());
+ cout << "after col UNscale mm=" << mm;
+
+ cout << " mmRowScale=" << mmRowScale << endl;
+ mm.rowScaleInPlace(mmRowScale);
+ cout << "after row scale, mm=" << mm;
+
+ mm.rowScaleInPlace(mmRowScale.elementwiseInvert());
+ cout << "after row UNscale mm=" << mm;
+
+ mm.rowScaleInPlace(mmRowScaleR);
+ cout << "after LONG DOUBLE row scale mm=" << mm;
+
+ cout << "mm.rowScale(double)=" << mm.rowScale(mmRowScaleR);
+ cout << "type(mm.rowScale(double))=" << typeid(mm.rowScale(mmRowScaleR)).name() << endl;
+
+ Vector_<Vec2> mmVCol(4); for (int i=0; i<4; ++i) mmVCol[i] = Vec2(4*i, 4*i+1);
+ cout << "mm Vec2 colScale=" << mmVCol << endl;
+ cout << "mm.colScale(Vec2)=" << mm.colScale(mmVCol);
+ cout << "type(mm.colScale(Vec2))=" << typeid(mm.colScale(mmVCol)).name() << endl;
+
+ mm *= 1000.; dump("mm(3,4) after *=1000", mm);
+ mm.dump("*** mm ***");
+
+ cout << "mm using << op: " << mm;
+ cout << "~mm: " << ~mm;
+ cout << "mm.norm()=" << mm.norm() << endl;
+ cout << "mm(3)=" << mm(3) << endl;
+ cout << "mm[2]=" << mm[2] << endl;
+ cout << "mm + 2*(-mm)=" << mm + 2*(-mm) << endl;
+
+ Matrix_<Complex> mm2 = mm;
+ cout << "mm2=" << mm2;
+ cout << "mm2(1,1,1,2)=" << mm2(1,1,1,2);
+ mm2(1,1,1,2)=7;
+ cout << "after mm2(1112)=7, mm2=" << mm2;
+
+ cout << "mm=" << mm;
+ (~mm)(1,1,1,2)=99;
+ cout << "after (~mm)(1112)=99, mm=" << mm;
+
+ cout << "\n--- DIAGONAL TEST ---\n";
+ cout << "mm2.diag()=" << mm2.diag() << endl;
+
+ mm2 = 99;
+ cout << "after mm2=99: mm2=" << mm2;
+
+ mm2(0,2,2,2) = std::complex<double>(-99,-99);
+ cout << "after mm2(0,2,2,2) = (-99,-99): mm2=" << mm2;
+
+ mm2.updDiag() *= .001;
+ cout << "after mm2.updDiag()*=.001: mm2=" << mm2;
+
+ // scalar assign to a row-shaped matrix should set only
+ // the first element to the scalar and all else zero
+ // (since that's the diagonal), while
+ // scalar assign to a row should set all the elements.
+ mm2(0,0,1,mm2.ncol()) = 1; // row shaped block of first row
+ mm2[mm2.nrow()-1] = 1; // last row
+ cout << "after mm2(0,0,1,n)=1 and mm2[m-1]=1: mm2=" << mm2;
+
+ // ditto for columns
+ mm2(0,1,mm2.nrow(),1) = 2;
+ mm2(2) = 2;
+
+ cout << "after mm2(0,1,m,1)=2 and mm2(2)=2: mm2=" << mm2;
+
+
+ cout << "\n-------- ASSIGN TEST --------\n";
+ Matrix_< Vec<2,Complex> > rr(2,3);
+ for (int i=0;i<2;++i) for (int j=0;j<3;++j)
+ rr(i,j) = Vec<2,Complex>(mdc[i]*(j+1), mdc[i]*(j-1));
+ Matrix_< Vec<2,Complex> > rrAssign;
+ (rrAssign = rr(0,1,2,2)) *= 1000.;
+ cout << "rr=" << rr;
+ cout << "(rrAssign=rr(0,1,2,2)) *= 1000.; rrAssign=" << rrAssign;
+ cout << "rr=" << rr;
+ (rrAssign.viewAssign(rr(0,1,2,2))) *= 100.;
+ cout << "(rrAssign.viewAssign(rr(0,1,2,2))) *= 100; rrAssign=" << rrAssign;
+ cout << "rr=" << rr;
+ cout << "-------- END ASSIGN TEST --------\n\n";
+
+ cout << "\n-------- RESIZE KEEP TEST --------\n";
+ Vector resizeMe(5); for (int i=0; i<5; ++i) resizeMe[i]=i;
+ cout << "resizeMe=" << resizeMe << endl;
+ resizeMe.resize(10);
+ cout << "after resize(10), resizeMe=" << resizeMe << endl;
+ Vector resizeMe2(5); for (int i=0; i<5; ++i) resizeMe2[i]=i;
+ cout << "resizeMe2=" << resizeMe2 << endl;
+ resizeMe2.resizeKeep(10);
+ cout << "after resizeKeep(10), resizeMe2=" << resizeMe2 << endl;
+ Matrix resizeMem(2,3); for (int i=0; i<2; ++i) for (int j=0; j<3; ++j) resizeMem(i,j)=(i+1)*(j+1);
+ cout << "resizeMem(2,3)=" << resizeMem << endl;
+ resizeMem.resizeKeep(3,5);
+ cout << "after resizeKeep(3,5), resizeMem=" << resizeMem << endl;
+ resizeMem.resizeKeep(2,2);
+ cout << "after resizeKeep(2,2), resizeMem=" << resizeMem << endl;
+ resizeMem.resize(3,4);
+ cout << "after resize(3,4), resizeMem=" << resizeMem << endl;
+ cout << "-------- END RESIZE KEEP TEST --------\n\n";
+
+ Mat<3,4,Complex> cm34;
+ for (int i=0; i<3; ++i)
+ for (int j=0; j<4; ++j)
+ cm34(i,j) = mdc[i+j*3];
+ cout << "Mat<3,4,Complex>=" << cm34 << endl;
+ Vec<4,Complex> cv4(&mdc[6]);
+ cout << "Vec<4,Complex>=" << cv4 << endl;
+ cout << "Mat<3,4>*Vec<4>=" << cm34*cv4 << endl;
+ cout << "Mat<3,4>*Mat<4,3>=" << cm34*~cm34;
+
+ complex<float> zzzz(1.,2.);
+ conjugate<float> jjjj(0.3f,0.4f);
+ negator<float> nnnn(7.1);
+ cout << "zzzz=" << zzzz << " jjjj=" << jjjj << " nnnn=" << nnnn << endl;
+ cout << "zzzz*jjjj=" << zzzz*jjjj << endl;
+
+ Matrix_<Complex> cMatrix34(3,4);
+ Vector_<Complex> cVector4(4);
+ for (int i=0; i<3; ++i) for (int j=0; j<4; ++j) cMatrix34(i,j)=cm34(i,j);
+ for (int i=0; i<4; ++i) cVector4[i] = cv4[i];
+ cout << "cMatrix34=" << cMatrix34;
+ cout << "cVector4=" << cVector4 << endl;
+ cout << "cMatrix34*cVector4=" << cMatrix34*cVector4 << endl;
+ cout << "cMatrix34*cMatrix43=" << cMatrix34*~cMatrix34;
+
+ Matrix_<Complex> cMatrix34N = -cMatrix34;
+
+ //TODO: not allowed yet
+ //Matrix_<Complex> cMatrix34H = ~cMatrix34;
+
+ Vector vv(4), ww;
+ vv[0] = 1.; vv[1] = 2.; vv[2] = 3.; vv[3] = 4.;
+ cout << "vv(4)=" << vv << endl;
+ vv *= 9.; cout << "vv*=9:" << vv << endl;
+
+ Vector vvvv;
+ vvvv = vv[2] * vv;
+ cout << "vvvv = vv[2]*vv = " << vvvv << endl;
+
+ Matrix_<Mat<2,2, Mat<2,2,double> > > mmm(2,1);
+ mmm = Mat<2,2, Mat<2,2,double> >(Mat<2,2,double>(1));
+ cout << "*****>>> mmm=" << mmm << endl;
+
+ Matrix mnm(4,2), nn;
+ cout << "mnm(4,2)=" << mnm;
+ mnm(0) = vv;
+ mnm(1) = -0.01 * vv;
+ cout << "mnm(vv,-.01*vv)=" << mnm;
+ cout << "===> mnm.abs()=" << mnm.abs();
+ cout << "mnm(1)=" << mnm(1) << endl;
+ cout << "mnm(1).abs()=" << mnm(1).abs() << endl;
+ cout << "mnm[1]=" << mnm[1] << endl;
+ cout << "mnm[1].abs()=" << mnm[1].abs() << endl;
+
+ ww = vv;
+ ww *= 0.1;
+ vv += ww;
+ cout << "ww=vv, *=0.1:" << ww << endl;
+ cout << "vv+=ww:" << vv << endl;
+
+ const Real rdata[]={1,2,3,
+ 9,.1,14,
+ 2,6,9};
+ Matrix_<negator<Real> > A(3,3, (negator<Real>*)rdata);
+
+ Matrix AI = A.invert();
+ cout << "A=" << A << "AI=" << AI << " A*AI=" << A*AI;
+
+ cout << "A(1,2).real()=" << A(1,2).real() << endl;
+
+ Matrix AH = ~A;
+ cout << "~A=" << ~A << "inv(~A)=" << (~A).invert() << "~(inv(A))=" << ~AI;
+
+ A.invertInPlace();
+ cout << "after A=inv(A), A=" << A << "norm(A-AI)=" << (A-AI).norm() << endl;
+
+ A.dump("*** A ***");
+ AI.dump("*** AI ***");
+
+ Mat<3,3,negator<Real> > smallNegA((negator<Real>*)rdata);
+ Mat<3,3,negator<Real> >::TInvert smallNegAI(smallNegA.invert());
+
+ cout << "smallNegA=" << smallNegA << " inv(smallNegA)=" << smallNegAI;
+ cout << "smallNegA*inv(smallNegA)=" << smallNegA*smallNegAI << "NORM="
+ << (smallNegA*smallNegAI).norm() << endl;
+ cout << "inverse(smallNegA)=" << inverse(smallNegA) << endl;
+
+ negator<Real> nnn = smallNegA(0,0)-smallNegA(1,1);
+ Real nnnr = smallNegA(0,0)-smallNegA(1,1);
+ cout << "negator nnn=" << nnn << " real nnnr=" << nnnr << endl;
+
+ nnn = smallNegA(0,1)-smallNegA(1,0);
+ nnnr = smallNegA(0,1)-smallNegA(1,0);
+ cout << "negator nnn=" << nnn << " real nnnr=" << nnnr << endl;
+
+ cout << "det(smallNegA)=" << det(smallNegA)
+ << " det(inv(smallNegA))=" << det(smallNegAI) << endl;
+
+ const Real cjdata[]={1,1, 2,2, 3,3, 4,4,
+ 9,9, .1,.1, 14,14, 22,22,
+ 2,2, 6,6, 9,9, 11,11,
+ .2,.2, .7,.7, 5,5, 10,10};
+
+ // General Lapack inverse.
+ Mat<4,4,conjugate<Real> > smallConjA4((conjugate<Real>*)cjdata);
+ Mat<4,4,conjugate<Real> >::TInvert smallConjAI4(smallConjA4.invert());
+
+
+ cout << "smallConjA4=" << smallConjA4 << " inv(smallConjA4)=" << smallConjAI4;
+ cout << "smallConjA4*inv(smallConjA4)=" << smallConjA4*smallConjAI4;
+ cout << "NORM=" << (smallConjA4*smallConjAI4).norm() << endl;
+ cout << "inverse(smallConjA4)=" << inverse(smallConjA4) << endl;
+ cout << "norm(inverse-lapackInverse4)=" << (inverse(smallConjA4)-lapackInverse(smallConjA4)).norm() << endl;
+
+ cout << "det(smallConjA4)=" << det(smallConjA4)
+ << " det(inv(smallConjA4))=" << det(smallConjAI4) << endl;
+
+ // Specialized inverse.
+ Mat<3,3,conjugate<Real> > smallConjA3((conjugate<Real>*)cjdata);
+ Mat<3,3,conjugate<Real> >::TInvert smallConjAI3(smallConjA3.invert());
+
+ cout << "smallConjA3=" << smallConjA3 << " inv(smallConjA3)=" << smallConjAI3;
+
+ cout << "smallConjA3*inv(smallConjA3)=" << smallConjA3*smallConjAI3 << "NORM="
+ << (smallConjA3*smallConjAI3).norm() << endl;
+
+ cout << "inverse(smallNegA3)=" << inverse(smallConjA3) << endl;
+
+ cout << "norm(inverse-lapackInverse3)=" << (inverse(smallConjA3)-lapackInverse(smallConjA3)).norm() << endl;
+
+ cout << "det(smallConjA3)=" << det(smallConjA3)
+ << " det(inv(smallConjA3))=" << det(smallConjAI3) << endl;
+
+ cout << "Mat<1,1,conj>=" << smallConjA3.getSubMat<1,1>(1,0) << "inv(...)=" <<
+ smallConjA3.getSubMat<1,1>(1,0).invert();
+ cout << "Mat11*inv(Mat11)=" <<
+ smallConjA3.getSubMat<1,1>(1,0)*smallConjA3.getSubMat<1,1>(1,0).invert();
+ cout << "Mat<2,2,conj>=" << smallConjA3.getSubMat<2,2>(0,0) << "inv(...)=" <<
+ smallConjA3.getSubMat<2,2>(0,0).invert();
+ cout << "Mat22*inv(Mat22)=" <<
+ smallConjA3.getSubMat<2,2>(0,0)*smallConjA3.getSubMat<2,2>(0,0).invert();
+
+ try {
+ const double ddd[] = { 11, 12, 13, 14, 15, 16 };
+ const float fddd[] = { 11, 12, 13, 14, 15, 16 };
+ const complex<float> ccc[] = { complex<float>(1.,2.),
+ complex<float>(3.,4.),
+ complex<float>(5.,6.),
+ complex<float>(7.,8.) };
+ Vec< 2,complex<float> > cv2(ccc);
+ cout << "cv2=" << cv2 << endl;
+ cout << "(cv2+cv2)/1000:" << (cv2 + cv2) / complex<float>(1000,0)
+ << endl;
+
+ cout << "cv2: " << cv2 << endl;
+ cout << "cv2T: " << cv2.transpose() << endl;
+ cout << "-cv2: " << -cv2 << endl;
+ cout << "~cv2: " << ~cv2 << endl;
+ cout << "-~cv2: " << -(~cv2) << endl;
+ cout << "~-cv2: " << ~(-cv2) << endl;
+ cout << "~-cv2*10000: " << (~(-cv2))*10000.f << endl;
+
+ (~cv2)[1]=complex<float>(101.1f,202.3f);
+ cout << "after ~cv2[1]=(101.1f,202.3f), cv2= " << cv2 << endl;
+ -(~cv2)[1]=complex<float>(11.1f,22.3f);
+ cout << "after -~cv2[1]=(11.1f,22.3f), cv2= " << cv2 << endl;
+
+ Vec<3,float> dv2(fddd), ddv2(fddd+3);
+ dv2[2] = 1000;
+ cout << 100.* (ddv2 - dv2) / 1000. << endl;
+
+ cout << "dv2=" << dv2 << " dv2.norm()=" << dv2.norm() << endl;
+ cout << "cv2=" << cv2 << " cv2.norm()=" << cv2.norm() << endl;
+
+ dv2 = 100*dv2;
+
+ const Vec<3,float> v3c[] = {Vec<3,float>(fddd),Vec<3,float>(fddd+1)};
+
+ Vector_< Vec<2, Vec<3,float> > > vflt(2);
+ vflt[0] = Vec<2, Vec<3,float> >(v3c);
+ vflt[1] = vflt[0]*100;
+ vflt[1] = 100*vflt[0];
+ cout << "vflt 2xvec3=" << vflt << endl;
+
+
+ cout << "vflt.getNScalarsPerElement()=" << vflt.getNScalarsPerElement() << endl;
+ cout << "vflt.getPackedSizeofElement()=" << vflt.getPackedSizeofElement() << endl;
+ cout << "sizeof(vflt)=" << sizeof(vflt) << " sizeof(vflt[0])=" << sizeof(vflt[0]) << endl;
+ cout << "vflt.hasContiguousData()=" << vflt.hasContiguousData() << endl;
+ cout << "vflt.getContiguousScalarDataLength()=" << vflt.getContiguousScalarDataLength() << endl;
+
+ const float* p = vflt.getContiguousScalarData();
+ cout << "vflt's raw data: " << endl;
+ for (int i=0; i<vflt.getContiguousScalarDataLength(); ++i)
+ cout << " " << p[i];
+ cout << endl;
+
+ float* newData = new float[12];
+ float* oldData;
+ for (int i=0; i<12; ++i) newData[i]=(float)-i;
+ vflt.swapOwnedContiguousScalarData(newData, 12, oldData);
+
+ cout << "after data swap, vflt=" << vflt << endl;
+ cout << "old data =";
+ for (int i=0; i<12; ++i) cout << " " << oldData[i];
+ cout << endl;
+ delete[] oldData;
+
+ }
+ catch(const Exception::Base& b)
+ {
+ cout << b.getMessage() << endl;
+ }
+
+ typedef double P;
+ const int N = 1000;
+ const int LUP = 1;
+ Matrix_<P> big(N,N);
+ for (int j=0; j<N; ++j)
+ for (int i=0; i<N; ++i)
+ big(i,j) = 1+(float)std::rand()/RAND_MAX;
+ cout << "big.norm()=" << big.norm() << endl;
+
+
+ cout << "INVERTING " << LUP << "x" << N << "x" << N
+ << (sizeof(P)==4 ? std::string(" float") : std::string(" double")) << endl << std::flush;
+
+ Matrix_<P> flip(N,N), nrflip(N,N);
+
+ std::clock_t start = std::clock();
+ for (int i=0; i<LUP; ++i)
+ flip = big.invert();
+ cout << "... DONE -- " << (double)(std::clock()-start)/CLOCKS_PER_SEC << " seconds" << endl << std::flush;
+ cout << "initial norm=" << big.norm() << " invert.norm()=" << flip.norm() << endl << std::flush;
+
+ Matrix_<P> nrbig(N,N); nrbig=big;
+ cout << "NOW INVERT WITH NR, initial norm=" << nrbig.norm() << endl << std::flush;
+ start = std::clock();
+ for (int i=0; i<LUP; ++i) {
+ nrbig=big;
+ NR::luinvert(N, &nrbig(0,0), &nrflip(0,0));
+ }
+ cout << "... DONE -- " << (double)(std::clock()-start)/CLOCKS_PER_SEC << " seconds" << endl << std::flush;
+ cout << " nrinverse.norm()=" << nrflip.norm() << endl << std::flush;
+
+ cout << "big.norm()=" << big.norm() << " flip.norm()=" << flip.norm() << endl << std::flush;
+ Matrix_<P> ans(N,N);
+ cout << "Multiplying ..." << endl << std::flush;
+ start = std::clock();
+ for (int i=0; i<LUP; ++i)
+ Lapack::gemm('n','n',N,N,N,P(1),&big(0,0),N,&flip(0,0),N,P(0),&ans(0,0),N);
+ cout << "... DONE -- " << (double)(std::clock()-start)/CLOCKS_PER_SEC << " seconds" << endl << std::flush;
+ cout << "RMS (big*flip).norm() - 1=" << std::sqrt(ans.normSqr()/N)-1. << endl << std::flush;
+
+ cout << "Multiplying with NR result ..." << endl << std::flush;
+ start = std::clock();
+ for (int i=0; i<LUP; ++i)
+ Lapack::gemm('n','n',N,N,N,P(1),&big(0,0),N,&nrflip(0,0),N,P(0),&ans(0,0),N);
+ cout << "... DONE -- " << (double)(std::clock()-start)/CLOCKS_PER_SEC << " seconds" << endl << std::flush;
+ cout << "RMS (big*nrflip).norm() - 1=" << std::sqrt(ans.normSqr()/N)-1. << endl << std::flush;
+
+ cout << "Multiplying by hand ..." << endl << std::flush;
+ start = std::clock();
+ const P* bigp = &big(0,0);
+ const P* flipp = &flip(0,0);
+ P* ansp = &ans(0,0);
+ for (int l=0; l<LUP; ++l) {
+ for (int j=0; j<N; ++j) {
+ for (int i=0; i<N; ++i) {
+ P sum = P(0); const int jN=j*N;
+ for (int k=0; k<N; ++k)
+ sum += bigp[k*N+i] * flipp[jN+k];
+ ansp[jN+i] = sum;
+ }
+ }
+ }
+ cout << "... DONE -- " << (double)(std::clock()-start)/CLOCKS_PER_SEC << " seconds" << endl << std::flush;
+ cout << "RMS (big*flip).norm() - 1=" << std::sqrt(ans.normSqr()/N)-1. << endl << std::flush;
+ return 0;
+ }
+ catch (const std::exception& e) {
+ cout << e.what() << endl;
+ }
+}
+
+// Numerical Recipes version 2.11 LU decomp and inversion via backsolve.
+// This is the C++ version modified to use column-ordered consecutive
+// storage.
+
+namespace NR {
+
+// Return 1d index for column ordered matrix with leading dim N.
+#define X(i,j) j*N+i
+
+template <class DP>
+void luinvert(const int N, DP* a/*N,N*/, DP* y/*N,N*/) {
+ assert(a && y);
+ int* indx = new int[N];
+ DP d;
+ NR::ludcmp(N, a, indx, d);
+ for (int j=0; j<N; ++j) {
+ DP* col = &y[X(0,j)];
+ for (int i=0; i<N; ++i) col[i]=DP(0);
+ col[j] = DP(1);
+ NR::lubksb(N,a,indx,col); // writes directly into y
+ }
+
+ delete[] indx;
+}
+
+template <class DP>
+void lubksb(const int N, const DP* a/*N,N*/, const int* indx/*N*/,
+ DP* b/*N*/)
+{
+ int i,ii=0,ip,j;
+ DP sum;
+
+ for (i=0;i<N;i++) {
+ ip=indx[i];
+ sum=b[ip];
+ b[ip]=b[i];
+ if (ii != 0)
+ for (j=ii-1;j<i;j++) sum -= a[X(i,j)]*b[j];
+ else if (sum != 0.0)
+ ii=i+1;
+ b[i]=sum;
+ }
+ for (i=N-1;i>=0;i--) {
+ sum=b[i];
+ for (j=i+1;j<N;j++) sum -= a[X(i,j)]*b[j];
+ b[i]=sum/a[X(i,i)];
+ }
+}
+
+template <class DP>
+void ludcmp(const int N, DP* a/*N,N*/, int* indx/*N*/, DP &d)
+{
+ const DP TINY=DP(1.0e-20);
+ int i,imax,j,k;
+ DP big,dum,sum,temp;
+
+ DP* vv = new DP[N];
+ d=DP(1);
+ for (i=0;i<N;i++) {
+ big=0.0;
+ for (j=0;j<N;j++)
+ if ((temp=fabs(a[X(i,j)])) > big) big=temp;
+ if (big == 0.0) {
+ std::cerr << "Singular matrix in routine ludcmp" << endl;
+ assert(false);
+ exit(1);
+ }
+ vv[i]=DP(1)/big;
+ }
+ for (j=0;j<N;j++) {
+ for (i=0;i<j;i++) {
+ sum=a[X(i,j)];
+ for (k=0;k<i;k++) sum -= a[X(i,k)]*a[X(k,j)];
+ a[X(i,j)]=sum;
+ }
+ big=0.0;
+ for (i=j;i<N;i++) {
+ sum=a[X(i,j)];
+ for (k=0;k<j;k++) sum -= a[X(i,k)]*a[X(k,j)];
+ a[X(i,j)]=sum;
+ if ((dum=vv[i]*fabs(sum)) >= big) {
+ big=dum;
+ imax=i;
+ }
+ }
+ if (j != imax) {
+ for (k=0;k<N;k++) {
+ dum=a[X(imax,k)];
+ a[X(imax,k)]=a[X(j,k)];
+ a[X(j,k)]=dum;
+ }
+ d = -d;
+ vv[imax]=vv[j];
+ }
+ indx[j]=imax;
+ if (a[X(j,j)] == 0.0) a[X(j,j)]=TINY;
+ if (j != N-1) {
+ dum=DP(1)/(a[X(j,j)]);
+ for (i=j+1;i<N;i++) a[X(i,j)] *= dum;
+ }
+ }
+
+ delete[] vv;
+}
+
+
+} // namespace NR
+
diff --git a/SimTKcommon/tests/adhoc/CMakeLists.txt b/SimTKcommon/tests/adhoc/CMakeLists.txt
new file mode 100644
index 0000000..c0ba61d
--- /dev/null
+++ b/SimTKcommon/tests/adhoc/CMakeLists.txt
@@ -0,0 +1,46 @@
+# Generate adhoc tests.
+#
+# This is boilerplate code for generating a set of executables, one per
+# .cpp file in a "tests/adhoc" subdirectory. These are for "loose" test
+# cases, not suitable as regression tests but perhaps useful to the
+# developer or as demos. Unlike the similar boilerplate code in the "tests"
+# directory, this does not generate CMake ADD_TEST commands so these
+# will never run automatically.
+#
+# For IDEs that can deal with PROJECT_LABEL properties (at least
+# Visual Studio) the projects for building each of these adhoc
+# executables will be labeled "Adhoc - TheTestName" if a file
+# TheTestName.cpp is found in this directory.
+#
+# We check the BUILD_TESTS_AND_EXAMPLES_{SHARED,STATIC} variables to determine
+# whether to build dynamically linked, statically linked, or both
+# versions of the executable.
+
+
+FILE(GLOB ADHOC_TESTS "*.cpp")
+FOREACH(TEST_PROG ${ADHOC_TESTS})
+ GET_FILENAME_COMPONENT(TEST_ROOT ${TEST_PROG} NAME_WE)
+
+ IF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+ # Link with shared library
+ ADD_EXECUTABLE(${TEST_ROOT} ${TEST_PROG})
+ SET_TARGET_PROPERTIES(${TEST_ROOT}
+ PROPERTIES
+ PROJECT_LABEL "Test_Adhoc - ${TEST_ROOT}")
+ TARGET_LINK_LIBRARIES(${TEST_ROOT}
+ ${TEST_SHARED_TARGET})
+ ENDIF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+
+ IF (BUILD_STATIC_LIBRARIES AND BUILD_TESTS_AND_EXAMPLES_STATIC)
+ # Link with static library
+ SET(TEST_STATIC ${TEST_ROOT}Static)
+ ADD_EXECUTABLE(${TEST_STATIC} ${TEST_PROG})
+ SET_TARGET_PROPERTIES(${TEST_STATIC}
+ PROPERTIES
+ COMPILE_FLAGS "-DSimTK_USE_STATIC_LIBRARIES"
+ PROJECT_LABEL "Test_Adhoc - ${TEST_STATIC}")
+ TARGET_LINK_LIBRARIES(${TEST_STATIC}
+ ${TEST_STATIC_TARGET})
+ ENDIF()
+
+ENDFOREACH(TEST_PROG ${ADHOC_TESTS})
diff --git a/SimTKmath/CMakeLists.txt b/SimTKmath/CMakeLists.txt
new file mode 100644
index 0000000..520fc4e
--- /dev/null
+++ b/SimTKmath/CMakeLists.txt
@@ -0,0 +1,209 @@
+#---------------------------------------------------
+# Simmath
+#
+# Creates SimTK Core library, base name=SimTKmath.
+# Default libraries are shared & optimized. Variants
+# are created for static (_static) and debug (_d) and
+# provision is made for an optional "namespace" (ns)
+# and version number (vn).
+#
+# Windows:
+# [ns_]SimTKmath[_vn][_d].dll
+# [ns_]SimTKmath[_vn][_d].lib
+# [ns_]SimTKmath[_vn]_static[_d].lib
+# Unix:
+# lib[ns_]SimTKmath[_vn][_d].so
+# lib[ns_]SimTKmath[_vn]_static[_d].a
+#
+# All libraries are installed in
+# %ProgramFiles%\SimTK\lib (Windows)
+# /usr/local/SimTK/lib (UNIX)
+#
+#----------------------------------------------------
+
+PROJECT (SimTKmath)
+
+# SimTKmath depends on PlatformFiles and SimTKcommon only.
+INCLUDE_DIRECTORIES(${PLATFORM_INCLUDE_DIRECTORIES}
+ ${SimTKCOMMON_INCLUDE_DIRECTORIES})
+
+# code is in "./src" and in each of these subdirectories/src
+SET(SIMMATH_SOURCE_SUBDIRS LinearAlgebra Integrators Optimizers Geometry)
+
+# Collect up information about the version of the SimTKmath library
+# we're building and make it available to the code so it can be built
+# into the binaries. This also determines the versioned library names
+# in which case all dependencies must use the same version.
+
+SET(SIMMATH_MAJOR_VERSION ${SIMBODY_MAJOR_VERSION})
+SET(SIMMATH_MINOR_VERSION ${SIMBODY_MINOR_VERSION})
+SET(SIMMATH_PATCH_VERSION ${SIMBODY_PATCH_VERSION})
+
+
+# Report the version number to the CMake UI. Don't include the
+# patch version if it is zero.
+SET(PATCH_VERSION_STRING)
+IF(SIMMATH_PATCH_VERSION)
+ SET(PATCH_VERSION_STRING ".${SIMMATH_PATCH_VERSION}")
+ENDIF()
+
+SET(SIMMATH_VERSION
+ "${SIMMATH_MAJOR_VERSION}.${SIMMATH_MINOR_VERSION}${PATCH_VERSION_STRING}")
+
+# This is the suffix if we're building and depending on versioned libraries.
+SET(VN "_${SIMMATH_VERSION}")
+
+SET(SIMMATH_COPYRIGHT_YEARS "2005-10")
+
+# underbar separated list of dotted authors, no spaces or commas
+SET(SIMMATH_AUTHORS "Jack.Middleton_Michael.Sherman")
+
+SET (SIMMATH_SVN_REVISION ${SIMBODY_SVN_REVISION})
+
+ADD_DEFINITIONS(-DSimTK_SIMMATH_LIBRARY_NAME=${SimTKMATH_LIBRARY_NAME}
+ -DSimTK_SIMMATH_MAJOR_VERSION=${SIMMATH_MAJOR_VERSION}
+ -DSimTK_SIMMATH_MINOR_VERSION=${SIMMATH_MINOR_VERSION}
+ -DSimTK_SIMMATH_PATCH_VERSION=${SIMMATH_PATCH_VERSION})
+
+IF(NEED_QUOTES)
+ ADD_DEFINITIONS(-DSimTK_SIMMATH_SVN_REVISION="${SIMMATH_SVN_REVISION}"
+ -DSimTK_SIMMATH_COPYRIGHT_YEARS="${SIMMATH_COPYRIGHT_YEARS}"
+ -DSimTK_SIMMATH_AUTHORS="${SIMMATH_AUTHORS}")
+ELSE(NEED_QUOTES)
+ ADD_DEFINITIONS(-DSimTK_SIMMATH_SVN_REVISION=${SIMMATH_SVN_REVISION}
+ -DSimTK_SIMMATH_COPYRIGHT_YEARS=${SIMMATH_COPYRIGHT_YEARS}
+ -DSimTK_SIMMATH_AUTHORS=${SIMMATH_AUTHORS})
+ENDIF(NEED_QUOTES)
+
+# -DSimTK_SIMMATH_TYPE has to be defined in the target subdirectories.
+# -Dsimmath_EXPORTS defined automatically when Windows DLL build is being done.
+
+SET(SHARED_TARGET ${SimTKMATH_LIBRARY_NAME})
+SET(STATIC_TARGET ${SimTKMATH_LIBRARY_NAME}_static)
+SET(SHARED_TARGET_VN ${SimTKMATH_LIBRARY_NAME}${VN})
+SET(STATIC_TARGET_VN ${SimTKMATH_LIBRARY_NAME}${VN}_static)
+
+# On Unix or Cygwin we have to add the debug suffix manually
+IF (UNIX AND CMAKE_BUILD_TYPE MATCHES Debug)
+ SET(SHARED_TARGET ${SHARED_TARGET}_d)
+ SET(STATIC_TARGET ${STATIC_TARGET}_d)
+ SET(SHARED_TARGET_VN ${SHARED_TARGET_VN}_d)
+ SET(STATIC_TARGET_VN ${STATIC_TARGET_VN}_d)
+ENDIF (UNIX AND CMAKE_BUILD_TYPE MATCHES Debug)
+
+## Test against the unversioned libraries if they are being built;
+## otherwise against the versioned libraries.
+IF(BUILD_UNVERSIONED_LIBRARIES)
+ SET(TEST_SHARED_TARGET ${SHARED_TARGET})
+ SET(TEST_STATIC_TARGET ${STATIC_TARGET})
+ELSE(BUILD_UNVERSIONED_LIBRARIES)
+ SET(TEST_SHARED_TARGET ${SHARED_TARGET_VN})
+ SET(TEST_STATIC_TARGET ${STATIC_TARGET_VN})
+ENDIF(BUILD_UNVERSIONED_LIBRARIES)
+
+# These are all the places to search for header files which are
+# to be part of the API.
+SET(API_INCLUDE_DIRS) # start empty
+SET(SimTKMATH_INCLUDE_DIRS) # start empty
+FOREACH(subdir . ${SIMMATH_SOURCE_SUBDIRS})
+ # append
+ SET(API_INCLUDE_DIRS ${API_INCLUDE_DIRS}
+ ${PROJECT_SOURCE_DIR}/${subdir}/include
+ ${PROJECT_SOURCE_DIR}/${subdir}/include/simmath
+ ${PROJECT_SOURCE_DIR}/${subdir}/include/simmath/internal)
+
+ # Referencing headers must always be done relative to this level.
+ SET(SimTKMATH_INCLUDE_DIRS ${SimTKMATH_INCLUDE_DIRS}
+ ${PROJECT_SOURCE_DIR}/${subdir}/include)
+ENDFOREACH(subdir)
+
+# Include the Simmath API include directories now so that Simmath code
+# can use them.
+INCLUDE_DIRECTORIES(${SimTKMATH_INCLUDE_DIRS})
+
+# And pass API include directories up to the parent so subsequent libraries
+# can find the headers too.
+SET(SimTKMATH_INCLUDE_DIRECTORIES ${SimTKMATH_INCLUDE_DIRS}
+ PARENT_SCOPE)
+
+
+# We'll need both *relative* path names, starting with
+# their API_INCLUDE_DIRS, and absolute pathnames.
+SET(API_REL_INCLUDE_FILES) # start these out empty
+SET(API_ABS_INCLUDE_FILES)
+
+FOREACH(dir ${API_INCLUDE_DIRS})
+ FILE(GLOB fullpaths ${dir}/*.h) # returns full pathnames
+ SET(API_ABS_INCLUDE_FILES ${API_ABS_INCLUDE_FILES} ${fullpaths})
+
+ FOREACH(pathname ${fullpaths})
+ GET_FILENAME_COMPONENT(filename ${pathname} NAME)
+ SET(API_REL_INCLUDE_FILES ${API_REL_INCLUDE_FILES}
+ ${dir}/${filename})
+ ENDFOREACH(pathname)
+ENDFOREACH(dir)
+
+# collect up source files
+SET(SOURCE_FILES) # empty
+SET(SOURCE_INCLUDE_FILES) # for dependency tracking only, I believe (sherm)
+SET(SOURCE_INCLUDE_DIRS) # in case some low-level source needs a -I include dir
+SET(SOURCE_GROUPS)
+SET(SOURCE_GROUP_FILES)
+
+# first process all the source subdirectories
+FOREACH(subdir ${SIMMATH_SOURCE_SUBDIRS})
+ add_subdirectory(${subdir})
+ENDFOREACH(subdir)
+
+if (SOURCE_GROUPS)
+ list(LENGTH SOURCE_GROUPS NGROUPS)
+ math(EXPR lastgrpnum ${NGROUPS}-1)
+ foreach( grpnum RANGE 0 ${lastgrpnum} )
+ list(GET SOURCE_GROUPS ${grpnum} grp)
+ list(GET SOURCE_GROUP_FILES ${grpnum} grpfile)
+ source_group("${grp}" FILES "${grpfile}")
+ endforeach()
+endif()
+
+# then process ./src
+FILE(GLOB src_files ./src/*.cpp)
+FILE(GLOB incl_files ./src/*.h)
+SET(SOURCE_FILES ${SOURCE_FILES} ${src_files}) #append
+SET(SOURCE_INCLUDE_FILES ${SOURCE_INCLUDE_FILES} ${incl_files})
+
+# Add low-level source include directories if any.
+INCLUDE_DIRECTORIES(${SOURCE_INCLUDE_DIRS})
+
+#
+# Installation
+#
+
+# libraries and examples are installed from their subdirectories; headers here
+
+# install headers
+FILE(GLOB CORE_HEADERS include/*.h */include/*.h)
+FILE(GLOB TOP_HEADERS include/simmath/*.h */include/simmath/*.h)
+FILE(GLOB INTERNAL_HEADERS include/simmath/internal/*.h */include/simmath/internal/*.h)
+
+INSTALL_FILES(/${SIMBODY_INCLUDE_INSTALL_DIR}/ FILES ${CORE_HEADERS})
+INSTALL_FILES(/${SIMBODY_INCLUDE_INSTALL_DIR}/simmath/ FILES ${TOP_HEADERS})
+INSTALL_FILES(/${SIMBODY_INCLUDE_INSTALL_DIR}/simmath/internal FILES ${INTERNAL_HEADERS})
+
+FILE(GLOB SIMMATH_DOCS doc/*.pdf doc/*.txt)
+INSTALL(FILES ${SIMMATH_DOCS} DESTINATION ${CMAKE_INSTALL_DOCDIR})
+
+# These are at the end because we want them processed after
+# all the various variables have been set above.
+
+IF(BUILD_STATIC_LIBRARIES)
+ ADD_SUBDIRECTORY( staticTarget )
+ENDIF()
+ADD_SUBDIRECTORY( sharedTarget )
+
+IF( BUILD_EXAMPLES )
+ ADD_SUBDIRECTORY( examples )
+ENDIF( BUILD_EXAMPLES )
+
+IF( BUILD_TESTING )
+ ADD_SUBDIRECTORY( tests )
+ENDIF( BUILD_TESTING )
diff --git a/SimTKmath/Geometry/CMakeLists.txt b/SimTKmath/Geometry/CMakeLists.txt
new file mode 100644
index 0000000..29e7cd6
--- /dev/null
+++ b/SimTKmath/Geometry/CMakeLists.txt
@@ -0,0 +1,10 @@
+# add in local source and headers here
+FILE(GLOB src_files ./src/*.cpp)
+FILE(GLOB incl_files ./src/*.h)
+
+# append to the local scope copy, and then copy up to parent scope
+list(APPEND SOURCE_FILES ${src_files})
+set(SOURCE_FILES ${SOURCE_FILES} PARENT_SCOPE)
+
+list(APPEND SOURCE_INCLUDE_FILES ${incl_files})
+set(SOURCE_INCLUDE_FILES ${SOURCE_INCLUDE_FILES} PARENT_SCOPE)
diff --git a/SimTKmath/Geometry/include/simmath/internal/BicubicSurface.h b/SimTKmath/Geometry/include/simmath/internal/BicubicSurface.h
new file mode 100644
index 0000000..d736f07
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/BicubicSurface.h
@@ -0,0 +1,605 @@
+#ifndef SimTK_BICUBIC_SURFACE_H_
+#define SimTK_BICUBIC_SURFACE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Matthew Millard, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+This file defines the BicubicSurface class, and the BicubicFunction class
+that uses it to create a two-argument Function object. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_BicubicHermitePatch.h"
+#include "simmath/internal/Geo_BicubicBezierPatch.h"
+
+#include <limits>
+
+namespace SimTK {
+
+//==============================================================================
+// CLASS BICUBIC SURFACE
+//==============================================================================
+/** This class will create a smooth surface that approximates a two-argument
+function F(X,Y) from a given set of samples of that function on a rectangular
+grid with regular or irregular spacing. This is useful both for function
+interpolation and to provide height-mapped terrain surfaces. See the related
+class BicubicFunction if you need to satisfy the SimTK::Function interface.
+A single BicubicSurface can be shared among multiple accessors and threads
+once constructed.
+
+ at image html BicubicSurface1.png "A single-patch bicubic surface"
+
+A bicubic surface interpolation is used to approximate the function
+between the sample points. That is desirable for simulation use because it is
+continuous up to the second derivative, providing smoothly varying first
+derivatives, and a very smooth surface. The third derivatives will be
+discontinuous between grid boundaries; all higher derivatives are zero.
+
+The user need only provide two vectors x and y defining the sample points,
+and a matrix f that defines the value of the function at each sample (you can
+think of that as the height Z of the surface over the X-Y plane). If the
+samples along both axes are regularly spaced, x and y can be defined just by
+giving the spacing; otherwise, the sample locations are given explicitly.
+
+<h3>Usage</h3>
+The following code generates an unsmoothed and smoothed surface using a 3x4
+grid of bicubic patches from a matrix of 4x5 irregularly-spaced sample points:
+ at code
+ const int Nx = 4, Ny = 5;
+ const Real xData[Nx] = { .1, 1, 2, 4 };
+ const Real yData[Ny] = { -3, -2, 0, 1, 3 };
+ const Real fData[Nx*Ny] = { 1, 2, 3, 3, 2,
+ 1.1, 2.1, 3.1, 3.1, 2.1,
+ 1, 2, 7, 3, 2,
+ 1.2, 2.2, 3.2, 3.2, 2.2 };
+ const Vector x(Nx, xData);
+ const Vector y(Ny, yData);
+ const Matrix f(Nx,Ny, fData);
+ BicubicSurface surfaceThroughSamples(x, y, f);
+ BicubicSurface smoothedSurface(x, y, f, 1);
+
+ // Evaluate the surface at (.5,.5):
+ Real val = smoothedSurface.calcValue(Vec2(.5,.5));
+ at endcode
+
+When accessing the surface repeatedly, you can significantly improve performance
+by maintaining a "hint" object that allows for very fast repeated access to the
+same patch, a very common access pattern. Alternatively, use a BicubicFunction
+as the interface to your BicubicSurface; in that case the hint is managed for
+you automatically. Here is an example of explicit hint usage:
+ at code
+ PatchHint hint;
+ val = smoothedSurface.calcValue(Vec2(.5,.6), hint);
+ val = smoothedSurface.calcValue(Vec2(.51,.61), hint);
+ at endcode
+
+Additional methods are provided for obtaining the surface normals, and all the
+surface partial derivatives. Advanced users can obtain principal curvature
+directions, and performance statistics, among other specialized functions.
+
+If you want to visualize the surface, you can ask it to generate a polygonal
+mesh (with optional control over the resolution). That mesh can be used to
+generate a DecorativeMesh compatible with the Simbody Visualizer. This example
+creates a mesh of the smoothed surface, then places it on the Ground body so
+that the Visualizer will pick it up.
+ at code
+ PolygonalMesh smoothedMesh = smoothedSurface.createPolygonalMesh();
+ matter.Ground().addBodyDecoration(Vec3(0), // place at origin
+ DecorativeMesh(smoothedMesh)
+ .setRepresentation(DecorativeGeometry::Wireframe)
+ .setColor(Blue));
+ at endcode
+
+<h3>Discussion</h3>
+Graphically if the defining sample vectors and matrices were laid next to each
+other consistently with how the surface is computed the diagram would look
+like this: <pre>
+ y(0) y(1) ... y(ny-1)
+ ------ ------ --------
+ x(0) | f(0,0) f(0,1) ... f(0,ny-1)
+ x(1) | f(1,0) f(1,1) ... f(1,ny-1)
+ . | . . .
+ . | . . .
+ . | . . .
+ x(nx-1) | f(nx-1,0) f(nx-1,1) f(nx-1,ny-1)
+</pre>
+such that f(i,j)=F(x(i),y(j)).
+
+Note that the each XY location can only have a unique value associated with it
+-- cave-like structures cannot be represented using this interpolation method.
+
+A bicubic surface interpolation requires the partial derivatives
+fx=Df/Dx, fy=Df/Dy and fxy=Dfx/Dy=D^2f/DxDy at each of the grid points. If you
+already know those, you can provide them directly. However, in most cases you
+will only know the sample points and not the derivatives. You can provide
+just the sample points and BicubicSurface will estimate the derivatives
+automatically. For the interested reader, these partial derivatives are
+computed by fitting splines through the points provided, and then taking
+derivatives of splines.
+
+These splines will pass through the points exactly when the smoothness
+parameter of the surface is set to 0, and will be interpolated using natural
+cubic splines, meaning that the curvature will be zero at the boundaries.
+When the smoothness parameter is greater than zero, the surface
+will be "relaxed" using the algorithm provided by SplineFitter, and will not
+exactly pass through the points given, but will smoothly come close to the
+points. The smoothness parameter can thus
+be used to generate a surface that smoothly interpolates noisy surface data.
+
+Here is a Wikipedia entry that describes the basic approach:
+http://en.wikipedia.org/wiki/Bicubic_interpolation
+
+ at author Matthew Millard, Michael Sherman
+
+ at see SplineFitter for implementation notes regarding smoothing.
+ at see BicubicFunction **/
+class SimTK_SIMMATH_EXPORT BicubicSurface {
+public:
+ class PatchHint; // See below for definition of PatchHint.
+
+ /** Construct an uninitialized BicubicSurface handle. This can be filled
+ in later by assignment. **/
+ BicubicSurface() : guts(0) {}
+ /** Destructor deletes the underlying surface if there are no more handles
+ referencing it, otherwise does nothing. **/
+ ~BicubicSurface();
+ /** Copy constructor makes a shallow copy of \a source; the new handle
+ will reference the same underlying surface as does \a source. **/
+ BicubicSurface(const BicubicSurface& source);
+ /** Copy assignment is shallow; it makes this handle reference the same
+ underlying surface as does \a source. If this handle was currently
+ referencing a different surface, that surface will be destructed if that
+ was the last reference to it. **/
+ BicubicSurface& operator=(const BicubicSurface& source);
+
+
+ /** Construct a bicubic surface that approximates F(X,Y) given samples
+ f(i,j) with the sample locations in f defined by the vectors x and
+ y. The smoothness parameter controls how closely the surface approaches the
+ grid points specified in matrix f, with the default being that the surface
+ will pass exactly through those points.
+
+ @param[in] x
+ Vector of sample locations along the X axis (minimum 4 values). Must be
+ monotonically increasing (no duplicates).
+ @param[in] y
+ Vector of sample locations along the Y axis (minimum 4 values). Must be
+ monotonically increasing (no duplicates).
+ @param[in] f
+ Matrix of function values (or surface heights) evaluated at the grid
+ points formed by x and y (dimension x.size() by y.size()), such that
+ f(i,j) is F(x[i],y[j]) where F is the function being approximated here.
+ @param[in] smoothness
+ A value of 0 will force surface to pass through all of the
+ points in f(x,y). As smoothness increases, the surface will
+ become smoother and smoother but will increasingly deviate from the
+ points stored in matrix \a f. (Optional, default is 0.)
+
+ If your sample points are regularly spaced, use the other constructor. **/
+ BicubicSurface(const Vector& x, const Vector& y, const Matrix& f,
+ Real smoothness=0);
+
+ /** Construct a bicubic surface that approximates F(X,Y) given samples
+ f(i,j) over a grid with regular spacing in both the x and y directions. The
+ smoothness parameter controls how closely the surface approaches the
+ grid points specified in matrix f, with the default being that the surface
+ will pass exactly through those points.
+
+ @param[in] XY
+ A Vec2 giving the (x0,y0) sample location associated with the (0,0)
+ grid position in matrix \a f.
+ @param[in] spacing
+ A Vec2 giving regular spacing along the x and y directions; both
+ entries must be greater than 0. The (i,j)th sample location is then
+ taken to be XY + (i*spacing[0], j*spacing[1]).
+ @param[in] f
+ Matrix of function values (or surface heights) evaluated at points of
+ the x-y plane regularly sampled using the supplied spacings. Can be
+ rectangular but must have minimum dimension 4x4. Here
+ f(i,j)=F(XY[0]+i*spacing[0],XY[1]+j*spacing[1]) where F is the function
+ being approximated.
+ @param[in] smoothness
+ A value of 0 will force surface to pass through all of the
+ points in \a f. As smoothness increases, the surface will
+ become smoother and smoother but will increasingly deviate from the
+ points stored in matrix \a f. (Optional, default is 0.)
+
+ If your sample points are not regularly spaced, use the other constructor
+ which allows for specified sample points. **/
+ BicubicSurface(const Vec2& XY, const Vec2& spacing,
+ const Matrix& f, Real smoothness=0);
+
+ /** Calculate the value of the surface at a particular XY coordinate.
+ @param[in] XY
+ A Vec2 giving the (X,Y) point at which F(X,Y) is to be evaluated.
+ @param[in,out] hint
+ Information saved from an earlier invocation of calcValue(),
+ calcUnitNormal(), or calcDerivative() that is used to reduce
+ execution time.
+ @return The interpolated value of the function at point (X,Y).
+
+ Cost is minimal for repeated access to the same point, and considerably
+ reduced if access is to the same patch. We also take advantage of
+ a regularly-spaced grid if there is one to avoid searching for the right
+ patch. **/
+ Real calcValue(const Vec2& XY, PatchHint& hint) const;
+
+ /** This is a slow-but-convenient version of calcValue() since it does
+ not provide for a PatchHint. See the other signature for a much faster
+ version. **/
+ Real calcValue(const Vec2& XY) const;
+
+ /** Calculate the outward unit normal to the surface at a particular XY
+ coordinate.
+ @param[in] XY
+ A Vec2 giving the (X,Y) point at which the normal is to be evaluated.
+ @param[in,out] hint
+ Information saved from an earlier invocation of calcValue(),
+ calcUnitNormal(), or calcDerivative() that is used to reduce execution
+ time.
+ @return The outward unit normal at point (X,Y).
+
+ This requires evaluating the first derivatives of the patch, constructing
+ tangents, finding their cross product and normalizing. **/
+ UnitVec3 calcUnitNormal(const Vec2& XY, PatchHint& hint) const;
+
+ /** This is a slow-but-convenient version of calcUnitNormal() since it does
+ not provide for a PatchHint. See the other signature for a much faster
+ version. **/
+ UnitVec3 calcUnitNormal(const Vec2& XY) const;
+
+ /** Calculate a partial derivative of this function at a particular point.
+ Which derivative to take is specified by listing the input components
+ (0==x, 1==y) with which to take it. For example, if derivComponents=={0},
+ that indicates a first derivative with respective to argument x.
+ If derivComponents=={0, 0, 0}, that indicates a third derivative with
+ respective to argument x. If derivComponents=={0, 1}, that indicates
+ a partial second derivative with respect to x and y, that is Df(x,y)/DxDy.
+ (We use capital D to indicate partial derivative.)
+
+ @param[in] derivComponents
+ The input components with respect to which the derivative should be
+ taken. Its size must be less than or equal to the value returned by
+ getMaxDerivativeOrder().
+ @param[in] XY
+ The vector of two input arguments that define the XY location on the
+ surface.
+ @param[in,out] hint
+ Information saved from an earlier invocation of calcValue(),
+ calcUnitNormal(), or calcDerivative() that is used to reduce
+ execution time.
+ @return The interpolated value of the selected function partial derivative
+ for arguments (X,Y).
+
+ See comments in calcValue() for a discussion of cost and how the hint
+ is used to reduce the cost. **/
+ Real calcDerivative(const Array_<int>& derivComponents,
+ const Vec2& XY, PatchHint& hint) const;
+
+ /** This is a slow-but-convenient version of calcDerivative() since it
+ does not provide for a PatchHint. See the other signature for a much faster
+ version. **/
+ Real calcDerivative(const Array_<int>& derivComponents,
+ const Vec2& XY) const;
+
+ /** The surface interpolation only works within the grid defined by the
+ vectors x and y used in the constructor. This function checks to see if an
+ XYval is within the defined bounds of this particular BicubicSurface.
+
+ @param[in] XY The vector of exactly 2 input arguments that define the XY
+ location on the surface.
+ @return \c true if the point is in range, \c false otherwise.
+
+ An attempt to invoke calcValue() or calcDerivative() on an out-of-range
+ point will raise an exception; use this method to check first if you
+ are not sure. **/
+ bool isSurfaceDefined(const Vec2& XY) const;
+
+ /** Return the lowest XY pair for which this surface is defined; that is
+ the point (xmin,ymin). **/
+ Vec2 getMinXY() const;
+ /** Return the highest XY pair for which this surface is defined; that is
+ the point (xmax,ymax). **/
+ Vec2 getMaxXY() const;
+
+ /** Create a mesh that can be used to visualize this surface. The default
+ resolution will generate four quads (2x2) per patch. Set \a resolution to
+ the number of times you want each patch subdivided. The default is 1; set
+ resolution=0 to get just one quad per patch, resolution=3 would give
+ 16 quads (4x4) per patch.
+
+ The resulting mesh has its origin at (0,0,0), not at (x[0],y[0],0) as
+ you might expect. X,Y,Z directions match the surface description. **/
+ PolygonalMesh createPolygonalMesh(Real resolution=1) const;
+
+ //--------------------------------------------------------------------------
+ /**@name Statistics
+ This class keeps track of the number of surface accesses made (using
+ either calcValue() or calcDerivative(), and how many of those were
+ resolved successfully using some or all of the hint information.
+ Methods in this section allow access to those statistics. Note that
+ these statistics include accesses from all users of this surface. **/
+ /**@{**/
+ /** This is the total number of calls made to either calcValue() or
+ calcDerivative(). **/
+ int getNumAccesses() const;
+ /** This is the number of accesses which specified a point whose
+ information was already available in the hint. Note that if different
+ information is requested about the point, and that information is not
+ already available, we count that as "same patch" but not "same point".
+ These accesses are resolved with essentially no computation. **/
+ int getNumAccessesSamePoint() const;
+ /** This is the number of accesses which specified a new point on the
+ same patch as was already present in the hint, or asked for new information
+ about the same point. These accesses are resolved without having to search
+ for the patch, and without having to compute patch information. However,
+ specific point information still must be calculated. **/
+ int getNumAccessesSamePatch() const;
+ /** This is the number of accesses which specified on a point that was
+ not on the patch currently in the hint, but was close enough that we did
+ not have to do a general search. This also applies if the point is on an
+ edge since those don't require searching either. So these accesses avoided
+ searching, but still required patch and point information to be computed,
+ which can be expensive. **/
+ int getNumAccessesNearbyPatch() const;
+ /** Reset all statistics to zero. Note that statistics are mutable so you
+ do not have to have write access to the surface. Any user of this surface
+ can reset statistics and we make no attempt to handle simultaneous access
+ by multiple threads in any careful manner. **/
+ void resetStatistics() const;
+ /**@}**/
+
+
+ //--------------------------------------------------------------------------
+ /**@name Advanced methods
+ The constructors here assume you have already computed the function values
+ and derivatives. Most users should use the constructors that construct
+ this information automatically from given data points. **/
+ /**@{**/
+
+ /** (Advanced) A constructor for a bicubic surface that sets the partial
+ derivatives of the surface to the values specified by fx, fy, and fxy.
+
+ @param[in] x vector of X grid points (minimum 2 values)
+ @param[in] y vector of Y grid points (minimum 2 values)
+ @param[in] f matrix of the surface heights evaluated at the grid formed
+ by x and y (minumum 2x2)
+ @param[in] fx matrix of partial derivative of f w.r.t to x (minumum 2x2)
+ @param[in] fy matrix of partial derivative of f w.r.t to y (minumum 2x2)
+ @param[in] fxy matrix of partial derivative of f w.r.t to x,y (minumum 2x2)
+ **/
+ BicubicSurface(const Vector& x, const Vector& y, const Matrix& f,
+ const Matrix& fx, const Matrix& fy, const Matrix& fxy);
+ /** (Advanced) Same, but with regular grid spacing. **/
+ BicubicSurface(const Vec2& XY, const Vec2& spacing, const Matrix& f,
+ const Matrix& fx, const Matrix& fy, const Matrix& fxy);
+
+ /** (Advanced) For use with Hertz contact at a point Q we need to know the
+ surface normal and principal curvature magnitudes and directions. This can
+ be viewed as an approximating paraboloid at Q in a frame P where OP=Q, Pz is
+ the outward-facing unit normal to the surface at Q, Px is the direction of
+ maximum curvature and Py is the direction of minimum curvature.
+ k=(kmax,kmin) are the returned curvatures with kmax >= kmin > 0. The
+ equation of the resulting paraboloid in the P frame is
+ -2z = kmax*x^2 + kmin*y^2.
+
+ @param[in] XY
+ The Vec2 that defines the desired location on the surface such that the
+ contact point Q=F(X,Y).
+ @param[in,out] hint
+ Information saved from an earlier invocation of this or another
+ hint-using method in this class.
+ @param[out] X_SP
+ The frame of the paraboloid P, measured and expressed in the surface
+ local frame S. X_SP.p() is Q, X_SP.x() is the calculated direction of
+ maximum curvature kmax; y() is the direction of minimum curvature kmin;
+ z is the outward facing normal at Q.
+ @param[out] k
+ The maximum (k[0]) and minimum (k[1]) curvatures of the surface (and
+ paraboloid P) at point Q.
+ **/
+ void calcParaboloid(const Vec2& XY, PatchHint& hint,
+ Transform& X_SP, Vec2& k) const;
+ /** (Advanced) This is a slow-but-convenient version of calcParaboloid()
+ since it does not provide for a PatchHint. See the other signature for a
+ much faster version. **/
+ void calcParaboloid(const Vec2& XY, Transform& X_SP, Vec2& k) const;
+
+ /** (Advanced) Get the number of individual bicubic patches used to form
+ this surface, as the dimensions along each side of a rectangular grid.
+ There are nx X ny patches with indices in [0..nx-1, 0..ny-1]. **/
+ void getNumPatches(int& nx, int& ny) const;
+
+ /** (Advanced) Select a patch by its (x,y) position in the rectangular
+ grid of individual bicubic patches from which this surface is constructed,
+ returning it as a Hermite patch. Cost is roughly 110 flops. **/
+ Geo::BicubicHermitePatch calcHermitePatch(int x, int y) const;
+
+ /** (Advanced) Select a patch by its (x,y) position in the rectangular
+ grid of individual bicubic patches from which this surface is constructed,
+ returning it as a Bezier patch. Cost is roughly 330 flops. **/
+ Geo::BicubicBezierPatch calcBezierPatch(int x, int y) const;
+ /**@}**/
+
+ //--------------------------------------------------------------------------
+ /**@name Bookkeeping
+ Methods in this section are administrative and most users will not need
+ to use them. **/
+ /**@{**/
+
+ /** Return \c true if this is an empty handle meaning that it does not
+ currently refer to any surface. This is the state the handle will have
+ after default construction or a call to clear(). **/
+ bool isEmpty() const {return guts==0;}
+
+ /** Return this handle to its default-constructed state, meaning that
+ it will not refer to any surface. If the handle was referencing some
+ surface, and that was the last reference to that surface, then the
+ surface will be destructed. After a call to clear(), isEmpty() will
+ return \c true. **/
+ void clear();
+ /**@}**/
+
+ /** @cond **/ // Hide from Doxygen.
+ class Guts; // Opaque implementation class.
+ const BicubicSurface::Guts& getGuts() const
+ { assert(guts); return *guts; }
+ /** @endcond **/
+private:
+ BicubicSurface::Guts* guts;
+}; // END class BicubicSurface
+
+
+
+//==============================================================================
+// CLASS BICUBIC FUNCTION :: PATCH HINT
+//==============================================================================
+/** This object is used to hold precalculated data about the most recently
+accessed patch to accelerate the common case of repeated access to the same
+patch or to nearby patches. **/
+class SimTK_SIMMATH_EXPORT BicubicSurface::PatchHint {
+public:
+ /** Creates an empty PatchHint, meaning it contains no meaningful
+ hint information. **/
+ PatchHint();
+ /** Copy an existing PatchHint to create a new one with the same
+ contents. If \a source is empty, the new one will be also. **/
+ PatchHint(const PatchHint& source);
+ /** Set the contents of this PatchHint to be the same as that of
+ \a source. If \a source is empty, this one will be empty after the
+ assignment. **/
+ PatchHint& operator=(const PatchHint& source);
+ /** Destruct this PatchHint. **/
+ ~PatchHint();
+
+ /** Return \c true if this object currently contains no meaningful
+ hint information. **/
+ bool isEmpty() const;
+ /** Erase any information currently stored in this %PatchHint. After this
+ call isEmpty() will return \c true. **/
+ void clear();
+
+ /** @cond **/ // Hide from Doxygen
+ class Guts; // Hidden implementation of PatchHint.
+ const Guts& getGuts() const {return *guts;}
+ Guts& updGuts() {return *guts;}
+ /** @endcond **/
+private:
+ Guts* guts;
+};
+
+
+
+//==============================================================================
+// CLASS BICUBIC FUNCTION
+//==============================================================================
+
+/** This is a two-argument Function built using a shared BicubicSurface and
+managing current state to optimize for localized access. Each
+distinct use of the BicubicSurface should create its own BicubicFunction,
+which is a lightweight wrapper around the BicubicSurface. This allows for
+localized access pattern optimization to be effective for each use of the
+surface.
+
+<h3>Thread safety</h3>
+BicubicFunction is \e not thread-safe, but the underlying BicubicSurface is.
+Each thread should thus have a private BicubicFunction that it uses to access
+the shared surface.
+
+ at author Matthew Millard, Michael Sherman **/
+class SimTK_SIMMATH_EXPORT BicubicFunction : public Function_<Real> {
+public:
+ /** Create a BicubicFunction referencing the given BicubicSurface, which
+ is shared not copied. **/
+ BicubicFunction(const BicubicSurface& surface) : surface(surface) {}
+
+ /** Return a reference to the BicubicSurface object being used by this
+ BicubicFunction. **/
+ const BicubicSurface& getBicubicSurface() const {return surface;}
+
+ /** Calculate the value of the function at a particular XY coordinate. Note
+ that XY must be a vector with only 2 elements in it (because this is a
+ 2-argument function), anything else will throw an exception. This is the
+ required implementation of the Function base class pure virtual.
+
+ @param[in] XY the 2-Vector of input arguments X and Y.
+ @return The interpolated value of the function at point (X,Y). **/
+ virtual Real calcValue(const Vector& XY) const {
+ SimTK_ERRCHK1(XY.size()==2, "BicubicFunction::calcValue()",
+ "The argument Vector XY must have exactly 2 elements but had %d.",
+ XY.size());
+ return surface.calcValue(Vec2(XY[0],XY[1]), hint);
+ }
+
+ /** Calculate a partial derivative of this function at a particular point.
+ Which derivative to take is specified by listing the input components
+ (0==x, 1==y) with which to take it. For example, if derivComponents=={0},
+ that indicates a first derivative with respective to argument x.
+ If derivComponents=={0, 0, 0}, that indicates a third derivative with
+ respective to argument x. If derivComponents=={0, 1}, that indicates
+ a partial second derivative with respect to x and y, that is Df(x,y)/DxDy.
+ (We use capital D to indicate partial derivative.)
+
+ @param[in] derivComponents
+ The input components with respect to which the derivative should be
+ taken. Each entry must be 0 or 1, and if there are 4 or more entries
+ the result will be zero since the surface has only 3 non-zero
+ derivatives.
+ @param[in] XY
+ The vector of two input arguments that define the XY location on the
+ surface.
+ @return The interpolated value of the selected function partial derivative
+ for arguments (X,Y). **/
+ virtual Real calcDerivative(const Array_<int>& derivComponents,
+ const Vector& XY) const {
+ SimTK_ERRCHK1(XY.size()==2, "BicubicFunction::calcDerivative()",
+ "The argument Vector XY must have exactly 2 elements but had %d.",
+ XY.size());
+ return surface.calcDerivative(derivComponents, Vec2(XY[0],XY[1]), hint);
+ }
+
+ /** This implements the Function base class pure virtual; here it
+ always returns 2 (X and Y). **/
+ virtual int getArgumentSize() const {return 2;}
+
+ /** This implements the Function base class pure virtual specifying how
+ many derivatives can be taken of this function; here it is unlimited.
+ However, note that a bicubic surface is continuous up to the second
+ derivative, discontinuous at the third, and zero for any derivatives equal
+ to or higher than the fourth. **/
+ virtual int getMaxDerivativeOrder() const
+ { return std::numeric_limits<int>::max(); }
+private:
+ BicubicSurface surface;
+ mutable BicubicSurface::PatchHint hint;
+};
+
+
+
+}; //namespace
+//=============================================================================
+//=============================================================================
+
+#endif // SimTK_BICUBIC_SURFACE_H_
diff --git a/SimTKmath/Geometry/include/simmath/internal/CollisionDetectionAlgorithm.h b/SimTKmath/Geometry/include/simmath/internal/CollisionDetectionAlgorithm.h
new file mode 100644
index 0000000..316671f
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/CollisionDetectionAlgorithm.h
@@ -0,0 +1,271 @@
+#ifndef SimTK_SIMMATH_COLLISION_DETECTION_ALGORITHM_H_
+#define SimTK_SIMMATH_COLLISION_DETECTION_ALGORITHM_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/internal/ContactGeometry.h"
+#include "simmath/internal/Contact.h"
+
+#include <map>
+
+namespace SimTK {
+
+/** A CollisionDetectionAlgorithm implements an algorithm for detecting overlaps
+between pairs of ContactGeometry objects, and creating Contact objects based on
+them. This class is used internally by GeneralContactSubsystem, and there
+usually is no reason to access it directly. The exception is if you are
+defining a new ContactGeometry subclass. In that case, you will also need to
+define one or more CollisionDetectionAlgorithms to detect collisions with your
+new geometry type, then register it calling registerAlgorithm(). **/
+class SimTK_SIMMATH_EXPORT CollisionDetectionAlgorithm {
+public:
+ class HalfSpaceSphere;
+ class SphereSphere;
+ class HalfSpaceEllipsoid;
+ class HalfSpaceTriangleMesh;
+ class SphereTriangleMesh;
+ class TriangleMeshTriangleMesh;
+ class ConvexConvex;
+ virtual ~CollisionDetectionAlgorithm() {}
+ /**
+ * Identify contacts between a pair of bodies.
+ *
+ * @param index1 the index of the first body within its contact set
+ * @param object1 the ContactGeometry for the first body
+ * @param transform1 the location and orientation of the first body in the
+ * ground frame
+ * @param index2 the index of the second body within its contact set
+ * @param object2 the ContactGeometry for the second body
+ * @param transform2 the location and orientation of the second body in the
+ * ground frame
+ * @param contacts if the bodies overlap, a Contact should be added to
+ * this for each distinct contact between them. (Multiple
+ * contacts may exist if one of the bodies is concave.)
+ */
+ virtual void processObjects
+ (ContactSurfaceIndex index1, const ContactGeometry& object1,
+ const Transform& transform1,
+ ContactSurfaceIndex index2, const ContactGeometry& object2,
+ const Transform& transform2,
+ Array_<Contact>& contacts) const = 0;
+ /**
+ * Register a CollisionDetectionAlgorithm to be used for identifying
+ * contacts between bodies of two specific types.
+ *
+ * @param type1 the type identifier for the ContactGeometry subclass
+ * the algorithm expects as the first body
+ * @param type2 the type identifier for the ContactGeometry subclass
+ * the algorithm expects as the second body
+ * @param algorithm the algorithm to use for bodies of the specified types
+ */
+ static void registerAlgorithm(ContactGeometryTypeId type1,
+ ContactGeometryTypeId type2,
+ CollisionDetectionAlgorithm* algorithm);
+ /**
+ * Get the CollisionDetectionAlgorithm to use for identifying contacts
+ * between bodies of two specific types.
+ *
+ * @param type1 the type id of the first body's ContactGeometry
+ * @param type2 the type id of the second body's ContactGeometry
+ * @return the CollisionDetectionAlgorithm to use, or NULL if no suitable
+ * algorithm has been registered
+ */
+ static CollisionDetectionAlgorithm*
+ getAlgorithm(ContactGeometryTypeId type1, ContactGeometryTypeId type2);
+private:
+ struct AlgorithmMap
+ : public std::map<std::pair<ContactGeometryTypeId, ContactGeometryTypeId>,
+ CollisionDetectionAlgorithm*>
+ {
+ ~AlgorithmMap();
+ };
+
+ static AlgorithmMap algorithmMap;
+};
+
+/**
+ * This algorithm detects contacts between a ContactGeometry::HalfSpace and a
+ * ContactGeometry::Sphere.
+ */
+class SimTK_SIMMATH_EXPORT CollisionDetectionAlgorithm::HalfSpaceSphere
+: public CollisionDetectionAlgorithm {
+public:
+ virtual ~HalfSpaceSphere() {}
+ void processObjects
+ (ContactSurfaceIndex index1, const ContactGeometry& object1,
+ const Transform& transform1,
+ ContactSurfaceIndex index2, const ContactGeometry& object2,
+ const Transform& transform2,
+ Array_<Contact>& contacts) const;
+};
+
+/**
+ * This algorithm detects contacts between a ContactGeometry::HalfSpace and a
+ * ContactGeometry::Ellipsoid.
+ */
+class SimTK_SIMMATH_EXPORT CollisionDetectionAlgorithm::HalfSpaceEllipsoid
+: public CollisionDetectionAlgorithm {
+public:
+ virtual ~HalfSpaceEllipsoid() {}
+ void processObjects
+ (ContactSurfaceIndex index1, const ContactGeometry& object1,
+ const Transform& transform1,
+ ContactSurfaceIndex index2, const ContactGeometry& object2,
+ const Transform& transform2,
+ Array_<Contact>& contacts) const;
+};
+
+/**
+ * This algorithm detects contacts between two ContactGeometry::Sphere objects.
+ */
+class SimTK_SIMMATH_EXPORT CollisionDetectionAlgorithm::SphereSphere
+: public CollisionDetectionAlgorithm {
+public:
+ virtual ~SphereSphere() {}
+ void processObjects
+ (ContactSurfaceIndex index1, const ContactGeometry& object1,
+ const Transform& transform1,
+ ContactSurfaceIndex index2, const ContactGeometry& object2,
+ const Transform& transform2,
+ Array_<Contact>& contacts) const;
+};
+
+/**
+ * This algorithm detects contacts between a ContactGeometry::HalfSpace and a
+ * ContactGeometry::TriangleMesh.
+ */
+class SimTK_SIMMATH_EXPORT CollisionDetectionAlgorithm::HalfSpaceTriangleMesh
+: public CollisionDetectionAlgorithm {
+public:
+ virtual ~HalfSpaceTriangleMesh() {}
+ void processObjects
+ (ContactSurfaceIndex index1, const ContactGeometry& object1,
+ const Transform& transform1,
+ ContactSurfaceIndex index2, const ContactGeometry& object2,
+ const Transform& transform2,
+ Array_<Contact>& contacts) const;
+private:
+ void processBox(const ContactGeometry::TriangleMesh& mesh,
+ const ContactGeometry::TriangleMesh::OBBTreeNode& node,
+ const Transform& transform, const Vec3& axisDir,
+ Real xoffset, std::set<int>& insideFaces) const;
+ void addAllTriangles(const ContactGeometry::TriangleMesh::OBBTreeNode& node,
+ std::set<int>& insideFaces) const;
+};
+
+/**
+ * This algorithm detects contacts between a ContactGeometry::Sphere and a
+ * ContactGeometry::TriangleMesh.
+ */
+class SimTK_SIMMATH_EXPORT CollisionDetectionAlgorithm::SphereTriangleMesh
+: public CollisionDetectionAlgorithm {
+public:
+ virtual ~SphereTriangleMesh() {}
+ void processObjects
+ (ContactSurfaceIndex index1, const ContactGeometry& object1,
+ const Transform& transform1,
+ ContactSurfaceIndex index2, const ContactGeometry& object2,
+ const Transform& transform2,
+ Array_<Contact>& contacts) const;
+private:
+ void processBox(const Vec3& center, Real radius2,
+ const ContactGeometry::TriangleMesh& mesh,
+ const ContactGeometry::TriangleMesh::OBBTreeNode& node,
+ std::set<int>& insideFaces) const;
+};
+
+/**
+ * This algorithm detects contacts between two ContactGeometry::TriangleMesh
+ * objects.
+ */
+class SimTK_SIMMATH_EXPORT CollisionDetectionAlgorithm::TriangleMeshTriangleMesh
+: public CollisionDetectionAlgorithm {
+public:
+ virtual ~TriangleMeshTriangleMesh() {}
+ void processObjects
+ (ContactSurfaceIndex index1, const ContactGeometry& object1,
+ const Transform& transform1,
+ ContactSurfaceIndex index2, const ContactGeometry& object2,
+ const Transform& transform2,
+ Array_<Contact>& contacts) const;
+private:
+ void processNodes(const ContactGeometry::TriangleMesh& mesh1,
+ const ContactGeometry::TriangleMesh& mesh2,
+ const ContactGeometry::TriangleMesh::OBBTreeNode& node1,
+ const ContactGeometry::TriangleMesh::OBBTreeNode& node2,
+ const OrientedBoundingBox& node2Bounds,
+ const Transform& transform, std::set<int>& triangles1,
+ std::set<int>& triangles2) const;
+ void findInsideTriangles(const ContactGeometry::TriangleMesh& mesh,
+ const ContactGeometry::TriangleMesh& otherMesh,
+ const Transform& transform,
+ std::set<int>& triangles) const;
+ void tagFaces(const ContactGeometry::TriangleMesh& mesh,
+ Array_<int>& faceType, std::set<int>& triangles,
+ int index, int depth) const;
+ static const int OUTSIDE = -1;
+ static const int UNKNOWN = 0;
+ static const int BOUNDARY = 1;
+ static const int INSIDE = 2;
+};
+
+/**
+ * This algorithm detects contacts between two ContactGeometry::Convex objects.
+ */
+class SimTK_SIMMATH_EXPORT CollisionDetectionAlgorithm::ConvexConvex
+: public CollisionDetectionAlgorithm {
+public:
+ virtual ~ConvexConvex() {}
+ void processObjects
+ (ContactSurfaceIndex index1, const ContactGeometry& object1,
+ const Transform& transform1,
+ ContactSurfaceIndex index2, const ContactGeometry& object2,
+ const Transform& transform2,
+ Array_<Contact>& contacts) const;
+private:
+ static Vec3 computeSupport(const ContactGeometry& object1,
+ const ContactGeometry& object2,
+ const Transform& transform, UnitVec3 direction);
+ static void addContact
+ (ContactSurfaceIndex index1, ContactSurfaceIndex index2,
+ const ContactGeometry& object1,
+ const ContactGeometry& object2,
+ const Transform& transform1, const Transform& transform2,
+ const Transform& transform12,
+ Vec3 point1, Vec3 point2, Array_<Contact>& contacts);
+ static Vec6 computeErrorVector(const ContactGeometry& object1,
+ const ContactGeometry& object2,
+ Vec3 pos1, Vec3 pos2,
+ const Transform& transform12);
+ static Mat66 computeJacobian(const ContactGeometry& object1,
+ const ContactGeometry& object2,
+ Vec3 pos1, Vec3 pos2,
+ const Transform& transform12);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_COLLISION_DETECTION_ALGORITHM_H_
diff --git a/SimTKmath/Geometry/include/simmath/internal/Contact.h b/SimTKmath/Geometry/include/simmath/internal/Contact.h
new file mode 100644
index 0000000..a05b772
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/Contact.h
@@ -0,0 +1,563 @@
+#ifndef SimTK_SIMMATH_CONTACT_H_
+#define SimTK_SIMMATH_CONTACT_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+
+namespace SimTK {
+
+
+/** @class SimTK::ContactSurfaceIndex
+This defines a unique index for all the contact surfaces being handled
+either by a ContactTrackerSubsystem or within a single ContactSet of a
+GeneralContactSubsystem. **/
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(ContactSurfaceIndex);
+
+/** @class SimTK::ContactId
+This is a unique integer Id assigned to each contact pair when we first
+begin to track it. The Id persists for as long as we are tracking this pair
+of surfaces; it is the basis for our notions of "the same contact" and
+"continuing contact". After we stop tracking a contact pair, its Id will not
+refer to any contact pair and any given Id will not be reused for a very long
+time; hence, these will typically be large numbers even if there are only a
+small number of contacts at any given moment. The Id is unique only within an
+instance of ContactTrackerSubsystem. **/
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(ContactId);
+
+/** @class SimTK::ContactTypeId
+This is a small integer that serves as the unique typeid for each type
+of concrete Contact class. This is used to select an appropriate contact
+response method for a given type of Contact. This Id is shared by all
+threads of a given program execution but you can't expect to get the same Id
+for different programs or different executions of the same program. **/
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(ContactTypeId);
+
+
+class ContactImpl;
+class UntrackedContactImpl;
+class BrokenContactImpl;
+class CircularPointContactImpl;
+class EllipticalPointContactImpl;
+class TriangleMeshContactImpl;
+class PointContactImpl;
+
+
+//==============================================================================
+// CONTACT
+//==============================================================================
+/** A Contact contains information about the spatial relationship between two
+surfaces that are near, or in contact with, each other. It usually is created
+by a ContactTracker or CollisionDetectionAlgorithm, and is retrieved from a
+ContactTrackerSubsystem or GeneralContactSubsystem.
+
+The base class records only the indices of the two surfaces that are in
+contact, and the relative Transform between them at the time the Contact
+was recorded. ContactTrackers or CollisionDetectionAlgorithms which
+characterize contacts in more complex ways will return objects that are
+subclasses of Contact that provide additional information. **/
+class SimTK_SIMMATH_EXPORT Contact {
+public:
+ /** The Contact::Condition tracks the status of a Contact through its
+ lifetime. **/
+ enum Condition {
+ Unknown, ///< this is an illegal value
+ Untracked, ///< this pair not yet being tracked; might not contact
+ Anticipated,///< we expect these to contact soon
+ NewContact, ///< first time seen; needs a ContactId assigned
+ Ongoing, ///< was new or ongoing before; still in contact now
+ Broken ///< was new or ongoing before; no longer in contact
+ };
+ /** Returns a human-readable name corresponding to the given Condition;
+ useful for debugging. If the Condition is unrecognized the method will
+ return some text to that effect rather than crashing. **/
+ static const char* nameOfCondition(Condition);
+
+ /** The default constructor creates an empty handle. **/
+ Contact() : impl(0) {}
+ /** Copy constructor is shallow and reference-counted; this handle will
+ point to the same object as does the \a source. **/
+ Contact(const Contact& source);
+ /** Destructor clears the handle, deleting the referenced object if this
+ was the last reference. **/
+ ~Contact() {clear();}
+ /** Copy assignment is shallow and reference-counted; this handle will
+ point to the same object as does the \a source. **/
+ Contact& operator=(const Contact& source);
+ /** Clear this handle, deleting the referenced object if this
+ was the last reference. **/
+ void clear();
+ /** See if this handle is empty. **/
+ bool isEmpty() const {return impl==0;}
+
+ /** Get the persistent ContactId that has been assigned to this Contact
+ object if there is one (otherwise this will be invalid -- you can check
+ with isValid(). **/
+ ContactId getContactId() const;
+ /** Find out the current condition of this Contact object. **/
+ Condition getCondition() const;
+ /** Get the first surface involved in the contact, specified by
+ its index within its contact set or ContactTrackerSubsystem. **/
+ ContactSurfaceIndex getSurface1() const;
+ /** Get the second surface involved in the contact, specified by
+ its index within its contact set or ContactTrackerSubsystem. **/
+ ContactSurfaceIndex getSurface2() const;
+ /** Return the transform X_S1S2 giving the pose of surface 2's frame
+ measured and expressed in surface 1's frame, recorded at the time this
+ Contact object was calculated. **/
+ const Transform& getTransform() const;
+
+ /** Set the ContactId for this Contact object. This must persist over the
+ lifetime of a single contact event. **/
+ Contact& setContactId(ContactId id);
+ /** Set the current Condition. **/
+ Contact& setCondition(Condition condition);
+ /** Set the surfaces tracked by this Contact object. **/
+ Contact& setSurfaces(ContactSurfaceIndex surf1, ContactSurfaceIndex surf2);
+ /** Set the surface-to-surface relative transform X_S1S2. **/
+ Contact& setTransform(const Transform& X_S1S2);
+
+ /** Return a unique small integer corresponding to the concrete type
+ of Contact object being referenced by this handle. **/
+ ContactTypeId getTypeId() const;
+
+ /** This creates a new ContactId starting from 1 and increasing for a very
+ long time (to a billion or so) before repeating. ContactId 0 is never
+ returned and this call is thread-safe. **/
+ static ContactId createNewContactId();
+
+ const ContactImpl& getImpl() const {assert(impl); return *impl;}
+ ContactImpl& updImpl() {assert(impl); return *impl;}
+protected:
+ explicit Contact(ContactImpl* impl);
+private:
+ ContactImpl* impl;
+};
+
+inline std::ostream& operator<<(std::ostream& o, const Contact& c) {
+ o << "Contact id=" << c.getContactId()
+ << " (typeId=" << c.getTypeId() << "):\n";
+ o << " surf1,surf2=" << c.getSurface1() << ","
+ << c.getSurface2() << "\n";
+ o << " condition=" << Contact::nameOfCondition(c.getCondition()) << "\n";
+ return o;
+}
+
+
+
+//==============================================================================
+// UNTRACKED CONTACT
+//==============================================================================
+/** This subclass of Contact represents a pair of contact surfaces that are
+not yet being tracked; there is no ContactId for them. We don't yet know what
+kind of Contact object would be appropriate for them, so this is a placeholder
+until a ContactTracker replaces it with something meaningful. The contact
+condition for one of these is always "Untracked". **/
+class SimTK_SIMMATH_EXPORT UntrackedContact : public Contact {
+public:
+ /** Default constructor creates an empty handle. **/
+ UntrackedContact() {}
+ /** Create an UntrackedContact object.
+ @param surf1 the index of the first surface involved in the contact,
+ specified by its index within its contact set
+ @param surf2 the index of the second surface involved in the contact,
+ specified by its index within its contact set **/
+ UntrackedContact(ContactSurfaceIndex surf1, ContactSurfaceIndex surf2);
+
+ /** Determine whether a Contact object is an UntrackedContact. **/
+ static bool isInstance(const Contact& contact);
+ /** Obtain the unique small-integer id for the UntrackedContact class. **/
+ static ContactTypeId classTypeId();
+
+private:
+ const UntrackedContactImpl& getImpl() const
+ { assert(isInstance(*this));
+ return reinterpret_cast<const UntrackedContactImpl&>
+ (Contact::getImpl()); }
+};
+
+
+
+//==============================================================================
+// BROKEN CONTACT
+//==============================================================================
+/** This subclass of Contact represents a pair of contact surfaces that were
+in contact (meaning within cutoff range) but have now gone out of range. This
+is the last time we will use this ContactId. The only parameters here are the
+surfaces and the separation distance (> cutoff). If someone cares, the
+separation distance can be used to estimate the time at which contact was
+broken. **/
+class SimTK_SIMMATH_EXPORT BrokenContact : public Contact {
+public:
+ /** Create a BrokenContact object.
+ @param surf1 The index of the first surface involved in the contact.
+ @param surf2 The index of the second surface involved in the contact.
+ @param X_S1S2 The surface-to-surface relative transform.
+ @param separation The minimum distance between the surfaces, with
+ separation > cutoff >= 0 always. **/
+ BrokenContact(ContactSurfaceIndex surf1, ContactSurfaceIndex surf2,
+ const Transform& X_S1S2, Real separation);
+
+ /** Get the separation (> cutoff >= 0) between the two surfaces at the time
+ we decided the contact had been broken. Note that the sign convention is
+ opposite from \e depth which is negative when separated. **/
+ Real getSeparation() const;
+
+ /** Determine whether a Contact object is a BrokenContact. **/
+ static bool isInstance(const Contact& contact);
+ /** Obtain the unique small-integer id for the BrokenContact class. **/
+ static ContactTypeId classTypeId();
+
+private:
+ const BrokenContactImpl& getImpl() const
+ { assert(isInstance(*this));
+ return reinterpret_cast<const BrokenContactImpl&>(Contact::getImpl()); }
+};
+
+
+
+//==============================================================================
+// CIRCULAR POINT CONTACT
+//==============================================================================
+/** This subclass of Contact represents a contact between two non-conforming
+surfaces 1 and 2 that initially meet at a point where each surface has a
+uniform radius of curvature in all directions (R1 and R2), like a sphere (inside
+or outside) or a halfspace, resulting in a contact region with circular
+symmetry. One of the objects may be concave, with negative curvature, as long
+as the two objects are not conforming.
+
+The undeformed geometry is characterized here by the effective radius
+R=1/(1/R1+1/R2), a normal vector z defining the penetration direction, a scalar
+penetration depth d (d>0 when surfaces overlap), and a patch origin point OP
+located centered on the patch normal such that each undeformed surface is d/2
+up or down the normal from OP. **/
+class SimTK_SIMMATH_EXPORT CircularPointContact : public Contact {
+public:
+ /** Create a CircularPointContact object.
+ @param surf1 the index of the first surface involved in the contact
+ @param radius1 surf1's uniform radius at the contact initiation point
+ @param surf2 the index of the second surface involved in the contact
+ @param radius2 surf2's uniform radius at the contact initiation point
+ @param X_S1S2 the surface-to-surface relative transform
+ @param radius the effective combined radius to use
+ @param depth the penetration depth d (>0) or separation distance
+ (<0); surfaces are at +/- d/2 from the origin, up and
+ down the normal
+ @param origin_S1 origin point for the contact patch frame, in S1
+ @param normal_S1 the common normal at onset, pointing from surface1 to
+ surface2, expressed in S1. This is the z axis of the
+ patch frame. **/
+ CircularPointContact
+ (ContactSurfaceIndex surf1, Real radius1,
+ ContactSurfaceIndex surf2, Real radius2,
+ const Transform& X_S1S2, Real radius, Real depth,
+ const Vec3& origin_S1, const UnitVec3& normal_S1);
+
+ /** Get the radius of surface1 at the contact point. **/
+ Real getRadius1() const;
+ /** Get the radius of surface2 at the contact point. **/
+ Real getRadius2() const;
+ /** Get the precalculated effective radius R at the contact point, where
+ R=1/(1/R1+1/R2). **/
+ Real getEffectiveRadius() const;
+ /** Get the penetration depth (>0) or separation distance (<0), also known
+ as the "approach". This is defined as the minimum distance you would need to
+ translate surface2 along the normal vector to make the surfaces just touch
+ at their contact points without overlap. **/
+ Real getDepth() const;
+ /** Get the origin OP of the contact patch frame P, in S1. **/
+ const Vec3& getOrigin() const;
+ /** Get the z axis of the contact patch frame, which is the common surface
+ normal at the initial contact point, pointing outward from surface1 towards
+ surface2 at initial contact. This is a unit vector expressed in S1. **/
+ const UnitVec3& getNormal() const;
+
+ /** Determine whether a Contact object is a CircularPointContact. **/
+ static bool isInstance(const Contact& contact);
+ static const CircularPointContact& getAs(const Contact& contact)
+ { assert(isInstance(contact));
+ return static_cast<const CircularPointContact&>(contact); }
+ static CircularPointContact& updAs(Contact& contact)
+ { assert(isInstance(contact));
+ return static_cast<CircularPointContact&>(contact); }
+
+ /** Get the unique small-integer id for the CircularPointContact class. **/
+ static ContactTypeId classTypeId();
+
+private:
+ const CircularPointContactImpl& getImpl() const
+ { assert(isInstance(*this));
+ return reinterpret_cast<const CircularPointContactImpl&>
+ (Contact::getImpl()); }
+};
+
+
+
+//==============================================================================
+// ELLIPTICAL POINT CONTACT
+//==============================================================================
+/** This subclass of Contact represents a contact between two non-conforming
+surfaces 1 and 2 that initially meet at a point and where each surface has two
+principal curvatures (maximum and minimum) in perpendicular directions. The
+prototypical example is ellipsoid-ellipsoid contact, but this includes a wide
+range of contacts between smooth surfaces, where the surfaces have two
+continuous spatial derivatives at the contact point. The contact plane on
+each surface is parameterized by its principal curvature directions x,y with the
+surface's contact point at the origin, and the common normal as the z axis. The
+surface is thus approximated by a paraboloid z=Ax^2+By^2 for which A=kx/2,
+B=ky/2 where kx,ky are the curvatures in the x,y directions. Here A>=0, A>=B,
+but B can be negative indicating a hyperbolic paraboloid (saddle). Each surface
+is parameterized separately: the z axes are along the same line, but the x,y
+axes are relatively rotated about z by an angle theta, with
+cos(theta)=dot(x1,x2)=dot(y1,y2).
+
+The surface of relative separation of the two surfaces will also be a
+paraboloid, sharing the z axis with the contact surfaces but having its own
+relative principal curvatures and directions. The undeformed contact is
+ultimately characterized by this relative paraboloid and a penetration depth
+d (d>0 when surfaces overlap, <0 when separated), in a contact frame where
+x,y are the relative principal curvature directions, z is the common normal
+oriented to point away from surface1, and the origin OP is centered such that
+the surface2 contact point is at O-(d/2)z and surface1 contact point is at
+O+(d/2)z.
+
+<h3>References</h3>
+ - Johnson, K.L. "Contact Mechanics", Cambridge University Press, 1985
+ (corrected ed. 1987), sec. 4.1, pp. 84-88.
+ - Goldsmith, W. "Impact", Dover, 2001, sec. 4.2, pp. 83-85.
+**/
+class SimTK_SIMMATH_EXPORT EllipticalPointContact : public Contact {
+public:
+ /** Create a EllipticalPointContact object.
+ @param surf1 the index of the first surface involved in the contact
+ @param surf2 the index of the second surface involved in the contact
+ @param X_S1S2 the surface-to-surface relative transform
+ @param X_S1C contact paraboloid coordinate frame C in S1 frame; x is
+ kmax direction, y is kmin direction, z points away from
+ surf1; origin OC is at midpoint between contact
+ points on the two surfaces
+ @param k maximum and minimum curvatures kmax,kmin of the
+ relative contact paraboloid
+ @param depth penetration depth d(>0) or separation (<0); surf1
+ contact pt at OC+(d/2)z, surf2 contact pt at OC-(d/2)z
+ **/
+ EllipticalPointContact
+ (ContactSurfaceIndex surf1, ContactSurfaceIndex surf2,
+ const Transform& X_S1S2,
+ const Transform& X_S1C, const Vec2& k, Real depth);
+
+ /** Get the relative curvatures at the contact point, ordered kmax,kmin
+ with kmax >= kmin. Note that it is possible that kmin < 0. **/
+ const Vec2& getCurvatures() const;
+ /** Get the frame C in which the contact paraboloid is expressed, as the
+ transform X_S1C. The Cx axis is the direction of maximum relative
+ curvature kmax, Cy is the direction of minimum curvature kmin, and Cz
+ is the contact normal direct away from S1's surface. The origin OC is
+ a point centered between the contact points on the two surfaces; those
+ points are at +/- depth/2 along Cz away from OC. **/
+ const Transform& getContactFrame() const;
+ /** Get the penetration depth (>0) or separation distance (<0), also known
+ as the "approach". This is defined as the minimum distance you would need to
+ translate surface2 along the normal vector to make the surfaces just touch
+ at their contact points without overlap. **/
+ Real getDepth() const;
+
+ /** Determine whether a Contact object is an EllipticalPointContact. **/
+ static bool isInstance(const Contact& contact);
+ static const EllipticalPointContact& getAs(const Contact& contact)
+ { assert(isInstance(contact));
+ return static_cast<const EllipticalPointContact&>(contact); }
+ static EllipticalPointContact& updAs(Contact& contact)
+ { assert(isInstance(contact));
+ return static_cast<EllipticalPointContact&>(contact); }
+
+ /** Get the unique small-integer id for the CircularPointContact class. **/
+ static ContactTypeId classTypeId();
+
+private:
+ const EllipticalPointContactImpl& getImpl() const
+ { assert(isInstance(*this));
+ return reinterpret_cast<const EllipticalPointContactImpl&>
+ (Contact::getImpl()); }
+};
+
+
+
+
+//==============================================================================
+// TRIANGLE MESH CONTACT
+//==============================================================================
+/** This subclass of Contact is used when one or both of the ContactGeometry
+objects is a TriangleMesh. It stores a list of every face on each object
+that is partly or completely inside the other one. **/
+class SimTK_SIMMATH_EXPORT TriangleMeshContact : public Contact {
+public:
+ /** Create a TriangleMeshContact object.
+ @param surf1 the index of the first surface involved in the contact,
+ specified by its index within its contact set
+ @param surf2 the index of the second surface involved in the contact,
+ specified by its index within its contact set
+ @param X_S1S2 the transform giving surf2's frame measured and expressed
+ in surf1's frame
+ @param faces1 the indices of all faces in the first surface which are
+ inside the second one
+ @param faces2 the indices of all faces in the second surface which are
+ inside the first one **/
+ TriangleMeshContact(ContactSurfaceIndex surf1,
+ ContactSurfaceIndex surf2,
+ const Transform& X_S1S2,
+ const std::set<int>& faces1,
+ const std::set<int>& faces2);
+
+ /** Get the indices of all faces of surface1 that are partly or completely
+ inside surface2. If surface1 is not a TriangleMesh, this will return an
+ empty set. **/
+ const std::set<int>& getSurface1Faces() const;
+ /** Get the indices of all faces of surface2 that are partly or completely
+ inside surface1. If surface2 is not a TriangleMesh, this will return an
+ empty set. **/
+ const std::set<int>& getSurface2Faces() const;
+
+ /** Determine whether a Contact object is a TriangleMeshContact. **/
+ static bool isInstance(const Contact& contact);
+ /** Recast a triangle mesh given as a generic Contact object to a
+ const reference to a concrete TriangleMeshContact object. **/
+ static const TriangleMeshContact& getAs(const Contact& contact)
+ { assert(isInstance(contact));
+ return static_cast<const TriangleMeshContact&>(contact); }
+ /** Recast a triangle mesh given as a generic Contact object to a
+ writable reference to a concrete TriangleMeshContact object. **/
+ static TriangleMeshContact& updAs(Contact& contact)
+ { assert(isInstance(contact));
+ return static_cast<TriangleMeshContact&>(contact); }
+
+ /** Obtain the unique small-integer id for the TriangleMeshContact
+ class. **/
+ static ContactTypeId classTypeId();
+
+private:
+ const TriangleMeshContactImpl& getImpl() const
+ { assert(isInstance(*this));
+ return reinterpret_cast<const TriangleMeshContactImpl&>
+ (Contact::getImpl()); }
+};
+
+
+
+
+//==============================================================================
+// POINT CONTACT
+//==============================================================================
+/** OBSOLETE -- use CircularPointContact or EllipticalPointContact.
+ * This subclass of Contact represents a symmetric contact centered at a single
+ * point, such as between two spheres or a sphere and a half space. It
+ * characterizes the contact by the center location and radius of the contact
+ * patch, the normal vector, and the penetration depth.
+ */
+class SimTK_SIMMATH_EXPORT PointContact : public Contact {
+public:
+ /**
+ * Create a PointContact object representing a general (elliptical) contact.
+ *
+ * @param surf1 the index of the first surface involved in the contact,
+ * specified by its index within its contact set
+ * @param surf2 the index of the second surface involved in the contact,
+ * specified by its index within its contact set
+ * @param location the location where the two surfaces touch, specified in
+ * the ground frame
+ * @param normal the surface normal at the contact location. This is
+ * specified in the ground frame, and points outward
+ * from surface1 towards surface2
+ * @param radius1 the first principal relative radius of curvature of the contact surface
+ * @param radius2 the second principal relative radius of curvature of the contact surface
+ * @param depth the penetration depth
+ */
+ PointContact(ContactSurfaceIndex surf1, ContactSurfaceIndex surf2,
+ Vec3& location, Vec3& normal, Real radius1, Real radius2, Real depth);
+ /**
+ * Create a PointContact object representing a circularly symmetric contact.
+ *
+ * @param surf1 the index of the first surface involved in the contact,
+ * specified by its index within its contact set
+ * @param surf2 the index of the second surface involved in the contact,
+ * specified by its index within its contact set
+ * @param location the location where the two surfaces touch, specified in
+ * the ground frame
+ * @param normal the surface normal at the contact location. This is
+ * specified in the ground frame, and points outward
+ * from surface1 towards surface2
+ * @param radius the relative radius of curvature of the contact surface
+ * @param depth the penetration depth
+ */
+ PointContact(ContactSurfaceIndex surf1, ContactSurfaceIndex surf2,
+ Vec3& location, Vec3& normal, Real radius, Real depth);
+ /**
+ * The location where the two surfaces touch, specified in the ground frame.
+ * More precisely, the contact region is represented as a circular patch
+ * centered at this point and perpendicular to the normal vector.
+ */
+ Vec3 getLocation() const;
+ /**
+ * Get the surface normal at the contact location. This is specified in the
+ * ground frame, and points outward from surface1 towards surface2.
+ */
+ Vec3 getNormal() const;
+ /**
+ * Get the first principal relative radius of curvature of the contact surface.
+ */
+ Real getRadiusOfCurvature1() const;
+ /**
+ * Get the second principal relative radius of curvature of the contact surface.
+ */
+ Real getRadiusOfCurvature2() const;
+ /**
+ * Get the effective relative radius of curvature of the contact surface. This is equal to
+ * sqrt(R1*R2), where R1 and R2 are the principal relative radii of curvature.
+ */
+ Real getEffectiveRadiusOfCurvature() const;
+ /**
+ * Get the penetration depth. This is defined as the minimum distance you
+ * would need to translate one surface along the normal vector to make the
+ * surfaces no longer overlap.
+ */
+ Real getDepth() const;
+ /**
+ * Determine whether a Contact object is a PointContact.
+ */
+ static bool isInstance(const Contact& contact);
+ /**
+ * Obtain the unique small-integer id for the PointContact class.
+ */
+ static ContactTypeId classTypeId();
+
+private:
+ const PointContactImpl& getImpl() const
+ { assert(isInstance(*this));
+ return reinterpret_cast<const PointContactImpl&>(Contact::getImpl()); }
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_CONTACT_H_
diff --git a/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h b/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h
new file mode 100644
index 0000000..38fcd41
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/ContactGeometry.h
@@ -0,0 +1,1517 @@
+#ifndef SimTK_SIMMATH_CONTACT_GEOMETRY_H_
+#define SimTK_SIMMATH_CONTACT_GEOMETRY_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: Ian Stavness, Andreas Scholz *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Defines the ContactGeometry class and its API-visible local subclasses for
+individual contact shapes. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/internal/OrientedBoundingBox.h"
+#include "simmath/internal/Geodesic.h"
+#include "simmath/internal/BicubicSurface.h"
+
+#include <cassert>
+
+namespace SimTK {
+
+/** @class SimTK::ContactGeometryTypeId
+This is a unique integer type for quickly identifying specific types of
+contact geometry for fast lookup purposes. **/
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(ContactGeometryTypeId);
+
+class ContactGeometryImpl;
+class OBBTreeNodeImpl;
+class OBBTree;
+class Plane;
+
+
+
+//==============================================================================
+// CONTACT GEOMETRY
+//==============================================================================
+/** A ContactGeometry object describes the shape of all or part of the boundary
+of a solid object, for the purpose of modeling with Simbody physical
+effects that occur at the surface of that object, such as contact and
+wrapping forces. Surfaces may be finite or infinite (e.g. a halfspace).
+Surfaces may be smooth or discrete (polyhedral). Smooth surfaces are defined
+implicitly as f(P)=0 (P=[px,py,pz]), and optionally may provide a surface
+parameterization P=f(u,v). An implicit representation is valid for any P;
+parametric representations may have limited validity, singular points, or may
+be defined only in a local neighborhood.
+
+A variety of operators are implemented by each specific surface type. Some of
+these are designed to support efficient implementation of higher-level
+algorithms that deal in pairs of interacting objects, such as broad- and
+narrow-phase contact and minimum-distance calculations.
+
+The idea here is to collect all the important knowledge about a particular
+kind of geometric shape in one place, adding operators as needed to support
+new algorithms from time to time.
+
+All surfaces provide these operations:
+ - find closest point to a given point
+ - find intersection with a given ray
+ - find most extreme point in a given direction
+ - return the outward-facing surface normal at a point
+ - generate a polygonal mesh that approximates the surface
+ - return a unique integer id that may be used to quickly determine the
+ concrete type of a generic surface
+
+Finite surfaces provide
+ - a bounding sphere that encloses the entire surface
+ - a bounding volume hierarchy with tight-fitting leaf nodes containing
+ only simple primitives
+
+Smooth surfaces provide
+ - Min/max curvatures and directions
+ - Calculate a geodesic between two points on the surface, or
+ starting at a point for a given direction and length
+
+Individual surface types generally support additional operations that may
+be used by specialized algorithms that know they are working with that
+particular kind of surface. For example, an algorithm for determining
+ellipsoid-halfspace contact is likely to take advantage of special properties
+of both surfaces.
+
+We do not require
+detailed solid geometry, but neither can the surface be treated without some
+information about the solid it bounds. For example, for contact we must know
+which side of the surface is the "inside". However, we don't need a fully
+consistent treatment of the solid; for ease of modeling we require only that
+the surface behave properly in those locations at which it is evaluated at run
+time. The required behavior may vary depending on the algorithm using it.
+
+This is the base class for surface handles; user code will typically
+reference one of the local classes it defines instead for specific shapes. **/
+class SimTK_SIMMATH_EXPORT ContactGeometry {
+public:
+class HalfSpace;
+class Cylinder;
+class Sphere;
+class Ellipsoid;
+class SmoothHeightMap;
+class TriangleMesh;
+class Torus;
+
+// TODO
+class Cone;
+
+/** Base class default constructor creates an empty handle. **/
+ContactGeometry() : impl(0) {}
+/** Copy constructor makes a deep copy. **/
+ContactGeometry(const ContactGeometry& src);
+/** Copy assignment makes a deep copy. **/
+ContactGeometry& operator=(const ContactGeometry& src);
+/** Base class destructor deletes the implementation object.\ Note that this
+is not virtual; handles should consist of just a pointer to the
+implementation. **/
+~ContactGeometry();
+
+/** Generate a DecorativeGeometry that matches the shape of this ContactGeometry **/
+DecorativeGeometry createDecorativeGeometry() const;
+
+/** Given a point, find the nearest point on the surface of this object. If
+multiple points on the surface are equally close to the specified point, this
+may return any of them.
+ at param[in] position The point in question.
+ at param[out] inside On exit, this is set to true if the specified point is
+ inside this object, false otherwise.
+ at param[out] normal On exit, this contains the surface normal at the
+ returned point.
+ at return The point on the surface of the object which is closest to the
+specified point. **/
+Vec3 findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const;
+
+/** Given a query point Q, find the nearest point P on the surface of this
+object, looking only down the local gradient. Thus we cannot guarantee that P
+is the globally nearest point; if you need that use the findNearestPoint()
+method. However, this method is extremely fast since it only needs to find the
+locally nearest point. It is best suited for use when you know P is not too
+far from the surface.
+
+ at param[in] pointQ The query point Q, assumed to be somewhere not too far
+ from the surface.
+ at return A point P on the surface, at which the surface normal is aligned with
+the line from P to Q.
+
+This method is very cheap if query point Q is already on the surface to within
+a very tight tolerance; in that case it will simply return P=Q. **/
+Vec3 projectDownhillToNearestPoint(const Vec3& pointQ) const;
+
+/** Track the closest point between this implicit surface and a given line, or
+the point of deepest penetration if the line intersects the surface.
+We are given a guess at the closest point, and search only downhill from that
+guess so we can't guarantee we actually are returning the globally closest.
+However, the method does run very fast and is well suited to continuous
+tracking where nothing dramatic happens from call to call.
+
+If the line intersects the surface, we return the closest perpendicular point,
+\e not one of the intersection points. The perpendicular point will be the
+point of \e most separation rather than least. This behavior makes the
+signed separation distance a smooth function that passes through zero as the
+line approaches, contacts, and penetrates the surface. We return that signed
+distance as the \a height argument.
+
+ at param[in] pointOnLine Any point through which the line passes.
+ at param[in] directionOfLine A unit vector giving the direction of the line.
+ at param[in] startingGuessForClosestPoint
+ A point on the implicit surface that is a good guess for the closest
+ (or most deeply penetrated) point.
+ at param[out] newClosestPointOnSurface
+ This is the point of least distance to the line if the surface and line are
+ separated; the point of most distance to the line if the line intersects
+ the surface.
+ at param[out] closestPointOnLine
+ The is the corresponding point on the line that is the closest (furthest)
+ from \a newClosestPointOnSurface.
+ at param[out] height
+ This is the signed height of the closest point on the line over the
+ surface tangent plane at \a newClosestPointOnSurface. This is positive
+ when \a closestPointOnLine is in the direction of the outward normal,
+ negative if it is in the opposite direction. If we successfully found the
+ point we sought, a negative height means the extreme point on the line
+ is inside the object bounded by this surface.
+
+ at returns \c true if it succeeds in finding a point that meets the criteria to
+a strict tolerance. Otherwise the method has gotten stuck in a local minimum
+meaning the initial guess wasn't good enough.
+
+In case we fail to find a good point, we'll still return \e some points on the
+surface and the line that reduced the error function. Sometimes those are
+useful for visualizing what went wrong.
+
+<h3>Theory</h3>
+We are looking for a point P on the surface where a line N passing through P
+parallel to the normal at P intersects the given line L and is perpendicular to
+L there. Thus there are two degrees of freedom (location of P on the
+surface), and there are two equations to solve. Let Q and R be the closest
+approach points of the lines N and L respectively. We require that the following
+two conditions hold:
+ - lines N and L are perpendicular
+ - points Q and R are coincident
+
+To be precise we solve the following nonlinear system of three equations:
+<pre>
+err(P) = 0
+where
+err(P) = [ n . l ]
+ [ (R-Q) . (n X l) ]
+ [ f(P) ]
+In the above n is the unit normal vector at P, l is a unit vector aligned with
+the query line L, and f(P) is the implicit surface function that keeps P on the
+surface.
+</pre>
+**/
+bool trackSeparationFromLine(const Vec3& pointOnLine,
+ const UnitVec3& directionOfLine,
+ const Vec3& startingGuessForClosestPoint,
+ Vec3& newClosestPointOnSurface,
+ Vec3& closestPointOnLine,
+ Real& height) const;
+
+
+
+/** Determine whether this object intersects a ray, and if so, find the
+intersection point.
+ at param[in] origin The position at which the ray begins.
+ at param[in] direction The ray direction.
+ at param[out] distance If an intersection is found, the distance from the ray
+ origin to the intersection point is stored in this.
+ Otherwise, it is left unchanged.
+ at param[out] normal If an intersection is found, the surface normal of the
+ intersection point is stored in this. Otherwise, it is
+ left unchanged.
+ at return \c true if an intersection is found, \c false otherwise. **/
+bool intersectsRay(const Vec3& origin, const UnitVec3& direction,
+ Real& distance, UnitVec3& normal) const;
+
+/** Get a bounding sphere which completely encloses this object.
+ at param[out] center On exit, this contains the location of the center of the
+ bounding sphere.
+ at param[out] radius On exit, this contains the radius of the bounding
+ sphere. **/
+void getBoundingSphere(Vec3& center, Real& radius) const;
+
+/** Returns \c true if this is a smooth surface, meaning that it can provide
+meaningful curvature information and continuous derivatives with respect to its
+parameterization. **/
+bool isSmooth() const;
+
+/** Compute the principal curvatures and their directions, and the surface
+normal, at a given point on a smooth surface.
+ at param[in] point
+ A point at which to compute the curvature.
+ at param[out] curvature
+ On return, this will contain the maximum (curvature[0]) and minimum
+ (curvature[1]) curvatures of the surface at the point.
+ at param[out] orientation
+ On return, this will contain the orientation of the surface at the given
+ point as follows: the x axis along the direction of maximum curvature, the
+ y axis along the direction of minimum curvature, and the z axis along the
+ surface normal. These vectors are expressed in the surface's coordinate
+ frame.
+
+Non-smooth surfaces will not implement this method and will throw an exception
+if you call it. **/
+void calcCurvature(const Vec3& point, Vec2& curvature,
+ Rotation& orientation) const;
+
+/** Our smooth surfaces define a function f(P)=0 that provides an implicit
+representation of the surface. P=(x,y,z) is any point in space expressed in
+the surface's coordinate frame S (that is, given by a vector P-So, expressed in
+S). The function is positive inside the object, 0 on the surface, and negative
+outside the object. The returned Function object supports first and second
+partial derivatives with respect to the three function arguments x, y, and z.
+Evaluation of the function and its derivatives is cheap.
+
+Non-smooth surfaces will not implement this method and will throw an exception
+if you call it. **/
+const Function& getImplicitFunction() const;
+
+/** Calculate the value of the implicit surface function, at a given point.
+ at param[in] point
+ A point at which to compute the surface value.
+ at return
+ The value of the implicit surface function at the point. **/
+Real calcSurfaceValue(const Vec3& point) const;
+
+/** Calculate the implicit surface outward facing unit normal at the given
+point. This is determined using the implicit surface function gradient
+so is undefined if the point is at a singular point of the implicit function.
+An example is a point along the center line of
+a cylinder. Rather than return a NaN unit normal in these cases, which
+would break many algorithms that are searching around for valid points, we'll
+return the normal from a nearby, hopefully non-singular point. If that doesn't
+work, we'll return an arbitrary direction. If you want to know whether you're
+at a singular point, obtain the gradient directly with calcSurfaceGradient()
+and check its length. **/
+UnitVec3 calcSurfaceUnitNormal(const Vec3& point) const;
+
+/** Calculate the gradient of the implicit surface function, at a given point.
+ at param[in] point
+ A point at which to compute the surface gradient.
+ at return
+ The gradient of the implicit surface function at the point. **/
+Vec3 calcSurfaceGradient(const Vec3& point) const;
+
+/** Calculate the hessian of the implicit surface function, at a given point.
+ at param[in] point
+ A point at which to compute the surface Hessian.
+ at return
+ The Hessian of the implicit surface function at the point. **/
+Mat33 calcSurfaceHessian(const Vec3& point) const;
+
+/** For an implicit surface, return the Gaussian curvature at the point p whose
+implicit surface function gradient g(p) and Hessian H(p) are supplied. It is
+not required that p is on the surface but the results will be meaningless if
+the gradient and Hessian were not calculated at the same point.
+
+Here is the formula:
+<pre>
+ ~grad(f) * Adjoint(H) * grad(f)
+ Kg = --------------------------------
+ |grad(f)|^4
+</pre>
+where grad(f) is Df/Dx, Hessian H is D grad(f)/Dx and Adjoint is a 3x3
+matrix A where A(i,j)=determinant(H with row i and column j removed).
+Ref: Goldman, R. "Curvature formulas for implicit curves and surfaces",
+Comp. Aided Geometric Design 22 632-658 (2005).
+
+Gaussian curvature is the product of the two principal curvatures, Kg=k1*k2.
+So for example, the Gaussian curvature anywhere on a sphere is 1/r^2. Note
+that despite the name, Gaussian curvature has units of 1/length^2 rather than
+curvature units of 1/length.
+
+Here is what the (symmetric) adjoint matrix looks like:
+<pre>
+adjH = [ fyy*fzz - fyz^2, fxz*fyz - fxy*fzz, fxy*fyz - fxz*fyy ]
+ [ (1,2), fxx*fzz - fxz^2, fxy*fxz - fxx*fyz ]
+ [ (1,3), (2,3), fxx*fyy - fxy^2 ]
+</pre>
+**/
+Real calcGaussianCurvature(const Vec3& gradient,
+ const Mat33& Hessian) const;
+
+/** This signature is for convenience; use the other one to save time if you
+already have the gradient and Hessian available for this point. See the other
+signature for documentation. **/
+Real calcGaussianCurvature(const Vec3& point) const {
+ return calcGaussianCurvature(calcSurfaceGradient(point),
+ calcSurfaceHessian(point));
+}
+
+/** For an implicit surface, return the curvature k of the surface at a given
+point p in a given direction tp. Make sure the point is on the surface and the
+direction vector lies in the tangent plane and has unit length |tp| = 1. Then
+<pre>
+k = ~tp * H * tp ,
+</pre>
+where H is the Hessian matrix evaluated at p.
+**/
+Real calcSurfaceCurvatureInDirection(const Vec3& point, const UnitVec3& direction) const;
+
+/** Returns \c true if this surface is known to be convex. This can be true
+for smooth or polygonal surfaces. **/
+bool isConvex() const;
+
+/** Given a direction expressed in the surface's frame S, return the point P on
+the surface that is the furthest in that direction (or one of those points if
+there is more than one). This will be the point such that dot(P-So, direction)
+is maximal for the surface (where So is the origin of the surface). This is
+particularly useful for convex surfaces and should be very fast for them. **/
+Vec3 calcSupportPoint(UnitVec3 direction) const;
+
+/** ContactTrackerSubsystem uses this id for fast identification of specific
+surface shapes. **/
+ContactGeometryTypeId getTypeId() const;
+
+/** Calculate surface curvature at a point using differential geometry as
+suggested by Harris 2006, "Curvature of ellipsoids and other surfaces" Ophthal.
+Physiol. Opt. 26:497-501, although the equations here come directly from
+Harris' reference Struik 1961, Lectures on Classical Differential Geometry,
+2nd ed. republished by Dover 1988. Equation and page numbers below are from
+Struik.
+
+This method works for any smooth surface for which there is a local (u,v)
+surface parameterization; it is not restricted to ellipsoids or convex shapes,
+and (u,v) must be distinct but do not have to be perpendicular. Both must be
+perpendicular to the surface normal.
+
+First fundamental form: I = E du^2 + 2F dudv + G dv^2
+<pre> E = dxdu^2, F = ~dxdu * dxdv, G=dxdv^2 </pre>
+
+Second fundamental form: II = e du^2 + 2f dudv + g dv^2
+<pre> e = - ~d2xdu2 * nn, f = - ~d2xdudv * nn, g = - ~d2xdv2 * nn </pre>
+
+Given a direction t=dv/du, curvature k is
+<pre>
+ II e + 2f t + g t^2
+ k = -- = ---------------- (eq. 6-3)
+ I E + 2F t + G t^2
+</pre>
+
+We want minimum and maximum values for k to get principal curvatures. We can
+find those as the solutions to dk/dt=0.
+<pre> dk/dt = (E + 2Ft + Gt^2)(f+gt) - (e + 2ft + gt^2)(F+Gt) </pre>
+
+When dk/dt=0, k =(f+gt)/(F+Gt) = (e+ft)/(E+Ft) (eq. 6-4). That provides a
+quadratic equation for the two values of t:
+<pre> A t^2 + B t + C = 0 </pre>
+where A=Fg-Gf, B=Eg-Ge, C=Ef-Fe (eq. 6-5a).
+
+In case the u and v tangent directions are the min and max curvature directions
+(on a sphere, for example), they must be perpendicular so F=f=0 (eq. 6-6). Then
+the curvatures are ku = e/E and kv = g/G (eq. 6-8).
+
+We're going to return principal curvatures kmax and kmin such that kmax >= kmin,
+along with the perpendicular tangent unit directions dmax,dmin that are the
+corresponding principal curvature directions, oriented so that (dmax,dmin,nn)
+form a right-handed coordinate frame.
+
+Cost: given a point P, normalized normal nn, unnormalized u,v tangents and
+second derivatives <pre>
+ curvatures: ~115 flops
+ directions: ~50 flops
+ ----
+ ~165 flops
+</pre> **/
+static Vec2 evalParametricCurvature(const Vec3& P, const UnitVec3& nn,
+ const Vec3& dPdu, const Vec3& dPdv,
+ const Vec3& d2Pdu2, const Vec3& d2Pdv2,
+ const Vec3& d2Pdudv,
+ Transform& X_EP);
+
+/** This utility method is useful for characterizing the relative geometry of
+two locally-smooth surfaces in contact, in a way that is useful for later
+application of Hertz compliant contact theory for generating forces. We assume
+that contact points Q1 on surface1 and Q2 on surface2 have been determined with
+the following properties:
+ - the surface normals are aligned but opposite
+ - points Q1 and Q2 are separated only along the normal (no tangential
+ separation)
+
+Then the local regions near Q1 and Q2 may be fit with paraboloids P1 and P2
+that have their origins at Q1 and Q2, and have the same normals and curvatures
+at the origins as do the original surfaces. We will behave here as though
+Q1 and Q2 are coincident in space at a point Q; imagine sliding them along
+the normal until that happens. Now we define the equations of P1 and P2 in
+terms of the maximum and minimum curvatures of surface1 and surface2 at Q:<pre>
+ P1: -2z = kmax1 x1^2 + kmin1 y1^2
+ P2: 2z = kmax2 x2^2 + kmin2 y2^2
+</pre>
+Although the origin Q and z direction are shared, the x,y directions for the
+two paraboloids, though in the same plane z=0, are relatively rotated. Note
+that the kmins might be negative; the surfaces do not have to be convex.
+
+For Hertz contact, we need to know the difference (relative) surface
+between the two paraboloids. The difference is a paraboloid P with equation
+<pre>
+ P: -2z = kmax x^2 + kmin y^2
+</pre>
+It shares the origin Q and z direction (oriented as for P1), but has its
+own principal directions x,y which are coplanar with x1,y1 and x2,y2 but
+rotated into some unknown composite orientation. The purpose of this method
+is to calculate kmax and kmin, and optionally (depending which signature you
+call), x and y, the directions of maximum and minimum curvature (resp.). The
+curvature directions are also the principal axes of the contact ellipse formed
+by the deformed surfaces, so are necessary (for example) if you want to draw
+that ellipse.
+
+Cost is about 220 flops. If you don't need the curvature directions, call the
+other overloaded signature which returns only kmax and kmin and takes only
+about 1/3 as long.
+
+ at param[in] R_SP1
+ The orientation of the P1 paraboloid's frame, expressed in some frame S
+ (typically the frame of the surface to which P1 is fixed). R_SP1.x() is
+ the direction of maximum curvature; y() is minimum curvature; z is the
+ contact normal pointing away from surface 1.
+ at param[in] k1
+ The maximum (k1[0]) and minimum (k1[1]) curvatures for P1 at the contact
+ point Q1 on surface1. Negative curvatures are handled correctly here but
+ may cause trouble for your force model if the resulting contact is
+ conforming.
+ at param[in] x2
+ The direction of maximum curvature for paraboloid P2. \a x2 must be in the
+ x1,y1 plane provided in \a R_SP1 and expressed in the S frame.
+ at param[in] k2
+ The maximum (k2[0]) and minimum (k2[1]) curvatures for P2 at the contact
+ point Q2 on surface2. Negative curvatures are handled correctly here but
+ may cause trouble for your force model if the resulting contact is
+ conforming.
+ at param[out] R_SP
+ The orientation of the difference paraboloid P's frame, expressed in the
+ same S frame as was used for P1. R_SP.x() is the direction of maximum
+ curvature of P at the contact point; y() is the minimum curvature
+ direction; z() is the unchanged contact normal pointing away from surface1.
+ at param[out] k
+ The maximum (k[0]) and minimum(k[1]) curvatures for the difference
+ paraboloid P at the contact point Q. If either of these is negative or
+ zero then the surfaces are conforming and you can't use a point contact
+ force model. Note that if k1>0 and k2>0 (i.e. surfaces are convex at Q)
+ then k>0 too. If some of the surface curvatures are concave, it is still
+ possible that k>0, depending on the relative curvatures.
+
+ at see The other signature for combineParaboloids() that is much cheaper if
+you just need the curvatures \a k but not the directions \a R_SP. **/
+static void combineParaboloids(const Rotation& R_SP1, const Vec2& k1,
+ const UnitVec3& x2, const Vec2& k2,
+ Rotation& R_SP, Vec2& k);
+
+/** This is a much faster version of combineParaboloids() for when you just
+need the curvatures of the difference paraboloid, but not the directions
+of those curvatures. Cost is about 70 flops. See the other overload of
+this method for details. **/
+static void combineParaboloids(const Rotation& R_SP1, const Vec2& k1,
+ const UnitVec3& x2, const Vec2& k2,
+ Vec2& k);
+
+
+/** @name Geodesic Evaluators **/
+/**@{**/
+
+/** Given two points, find a geodesic curve connecting them.
+If a preferred starting point is provided, find the geodesic curve that
+is closest to that point. Otherwise, find the shortest length geodesic.
+
+ at param[in] xP Coordinates of starting point P, in S.
+ at param[in] xQ Coordinates of ending point Q, in S.
+ at param[in] xSP (Optional) Coordinates of a preferred point for the geodesic to be near
+ at param[in] options Parameters related to geodesic calculation
+ at param[out] geod On exit, this contains a geodesic between P and Q.
+**/
+void initGeodesic(const Vec3& xP, const Vec3& xQ, const Vec3& xSP,
+ const GeodesicOptions& options, Geodesic& geod) const;
+
+
+/** Given the current positions of two points P and Q moving on this surface,
+and the previous geodesic curve G' connecting prior locations P' and Q' of
+those same two points, return the geodesic G between P and Q that is closest in
+length to the previous one. If multiple equal-length geodesics are possible
+(rare; between poles of a sphere, for example) then the one best matching the
+direction of the previous geodesic is selected.
+
+ at param[in] xP Coordinates of starting point P, in S.
+ at param[in] xQ Coordinates of ending point Q, in S.
+ at param[in] prevGeod The previous geodesic to which the new one should be
+ similar.
+ at param[in] options Parameters controlling the computation of the
+ new geodesic.
+ at param[out] geod On exit, this contains a geodesic between P and Q.
+
+<h3>Algorithm</h3>
+The handling of continuity here enforces our desire to have the length of a
+geodesic change smoothly as its end points move over the surface. This also
+permits us to accelerate geodesic finding by using starting guesses that are
+extrapolated from the previous result. If we find that the new geodesic has
+changed length substantially from the previous one, we will flag the result
+as uncertain and the caller should reduce the integration step size.
+
+First, classify the previous geodesic G' as "direct" or "indirect". Direct
+means that both tangents tP' and tQ' are approximately aligned with the vector
+r_PQ'. Note that as points P and Q get close together, as occurs prior to
+a cable liftoff, the geodesic between them always becomes direct and in fact
+just prior to liftoff it is the straight line from P to Q.
+
+If G' was indirect, then G is the geodesic connecting P and Q that has tP
+closest to tP' and about the same length as G'. We use G' data to initialize
+our computation of G and perform a local search to find the closest match.
+
+If G' was direct, then we look at the direction of r_PQ. If it is still aligned
+with the previous tP', then we calculate G from P to Q starting in direction
+tP', with roughly length s'. If it is anti-aligned, P and Q have swapped
+positions, presumably because they should have lifted off. In that case
+we calculate G from P to Q in direction -tP', and report that the geodesic
+has flipped. In that case you can consider the length s to be negative and
+use the value of s as an event witness for liftoff events.
+
+**/
+// XXX if xP and xQ are the exact end-points of prevGeod; then geod = prevGeod;
+void continueGeodesic(const Vec3& xP, const Vec3& xQ, const Geodesic& prevGeod,
+ const GeodesicOptions& options, Geodesic& geod) const;
+
+/** Produce a straight-line approximation to the (presumably short) geodesic
+between two points on this implicit surface. We do not check here whether it
+is reasonable to treat this geodesic as a straight line; we assume the caller
+has made that determination.
+
+We are given points P and Q and choose
+P' and Q' as the nearest downhill points on the surface to the given points. Then the
+returned geodesic line runs from P' to Q'. The Geodesic object will contain only
+the start and end points of the geodesic, with all the necessary information
+filled in. The normals nP and nQ are calculated from the surface points P' and
+Q'. The binormal direction is calculated using a preferred direction vector d
+(see below) as bP=normalize(d X nP) and bQ=normalize(d X nQ). Then the tangents
+are tP=nP X bP and tQ=nQ X bQ.
+
+The preferred direction d is calculated as follows: if P' and Q' are
+numerically indistinguishable (as defined by
+Geo::Point::pointsAreNumericallyCoincident()), we'll use the given
+\a defaultDirection if there is one, otherwise d is an arbitrary
+perpendicular to nP. If P' and Q' are numerically distinguishable, we
+instead set d = normalize(Q-P).
+
+When P' and Q' are \e numerically coincident, we will shift both of them to
+the midpoint (P'+Q')/2 so that the geodesic end points become \e exactly
+coincident and the resulting geodesic has exactly zero length. There will
+still be two points returned in the Geodesic object but they will be
+identical. **/
+void makeStraightLineGeodesic(const Vec3& xP, const Vec3& xQ,
+ const UnitVec3& defaultDirectionIfNeeded,
+ const GeodesicOptions& options, Geodesic& geod) const;
+
+
+/** Compute a geodesic curve starting at the given point, starting in the
+ * given direction, and terminating at the given length.
+
+ at param[in] xP Coordinates of the starting point for the geodesic.
+ at param[in] tP The starting tangent direction for the geodesic.
+ at param[in] terminatingLength The length that the resulting geodesic should have.
+ at param[in] options Parameters related to geodesic calculation
+ at param[out] geod On exit, this contains the calculated geodesic
+**/
+// XXX what to do if tP is not in the tangent plane at P -- project it?
+void shootGeodesicInDirectionUntilLengthReached
+ (const Vec3& xP, const UnitVec3& tP, const Real& terminatingLength,
+ const GeodesicOptions& options, Geodesic& geod) const;
+
+/** Given an already-calculated geodesic on this surface connecting points
+P and Q, fill in the sensitivity of point P with respect to a change of
+tangent direction at Q. If there are interior points stored with the geodesic,
+then we'll calculate the interior sensitivities also.
+
+ at param[in,out] geodesic An already-calculated geodesic.
+ at param[in] initSensitivity
+ Initial conditions for the Jacobi field calculation. If this is the whole
+ geodesic then the initial conditions are (0,1) for the sensitivity and
+ its arc length derivative. However, if we are continuing from another
+ geodesic, then the end sensitivity for that geodesic is the initial
+ conditions for this one.
+**/
+void calcGeodesicReverseSensitivity
+ (Geodesic& geodesic,
+ const Vec2& initSensitivity = Vec2(0,1)) const; // j, jdot at end point
+
+
+/** Compute a geodesic curve starting at the given point, starting in the
+ * given direction, and terminating when it hits the given plane.
+
+ at param[in] xP Coordinates of the starting point for the geodesic.
+ at param[in] tP The starting tangent direction for the geodesic.
+ at param[in] terminatingPlane The plane in which the end point of the resulting geodesic should lie.
+ at param[in] options Parameters related to geodesic calculation
+ at param[out] geod On exit, this contains the calculated geodesic
+**/
+// XXX what to do if tP is not in the tangent plane at P -- project it?
+// XXX what to do if we don't hit the plane
+void shootGeodesicInDirectionUntilPlaneHit(const Vec3& xP, const UnitVec3& tP,
+ const Plane& terminatingPlane, const GeodesicOptions& options,
+ Geodesic& geod) const;
+
+
+/** Utility method to find geodesic between P and Q using split geodesic
+method with initial shooting directions tPhint and -tQhint. **/
+void calcGeodesic(const Vec3& xP, const Vec3& xQ,
+ const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const;
+
+/** Utility method to find geodesic between P and Q using the orthogonal
+method, with initial direction tPhint and initial length lengthHint. **/
+void calcGeodesicUsingOrthogonalMethod(const Vec3& xP, const Vec3& xQ,
+ const Vec3& tPhint, Real lengthHint, Geodesic& geod) const;
+
+/** This signature makes a guess at the initial direction and length
+and then calls the other signature. **/
+void calcGeodesicUsingOrthogonalMethod(const Vec3& xP, const Vec3& xQ,
+ Geodesic& geod) const
+{
+ const Vec3 r_PQ = xQ - xP;
+ const Real lengthHint = r_PQ.norm();
+ const UnitVec3 n = calcSurfaceUnitNormal(xP);
+ // Project r_PQ into the tangent plane.
+ const Vec3 t_PQ = r_PQ - (~r_PQ*n)*n;
+ const Real tLength = t_PQ.norm();
+ const UnitVec3 tPhint =
+ tLength != 0 ? UnitVec3(t_PQ/tLength, true)
+ : n.perp(); // some arbitrary perpendicular to n
+ calcGeodesicUsingOrthogonalMethod(xP, xQ, Vec3(tPhint), lengthHint, geod);
+}
+
+
+/**
+ * Utility method to calculate the "geodesic error" between one geodesic
+ * shot from P in the direction tP and another geodesic shot from Q in the
+ * direction tQ. We optionally return the resulting "kinked" geodesic in
+ * case anyone wants it; if the returned error is below tolerance then that
+ * geodesic is the good one.
+ **/
+Vec2 calcSplitGeodError(const Vec3& P, const Vec3& Q,
+ const UnitVec3& tP, const UnitVec3& tQ,
+ Geodesic* geod=0) const;
+
+
+
+/** Analytically compute a geodesic curve starting at the given point, starting in the
+ * given direction, and terminating at the given length. Only possible for a few simple
+ * shapes, such as spheres and cylinders.
+
+ at param[in] xP Coordinates of the starting point for the geodesic.
+ at param[in] tP The starting tangent direction for the geodesic.
+ at param[in] terminatingLength The length that the resulting geodesic should have.
+ at param[in] options Parameters related to geodesic calculation
+ at param[out] geod On exit, this contains the calculated geodesic
+**/
+// XXX what to do if tP is not in the tangent plane at P -- project it?
+void shootGeodesicInDirectionUntilLengthReachedAnalytical
+ (const Vec3& xP, const UnitVec3& tP, const Real& terminatingLength,
+ const GeodesicOptions& options, Geodesic& geod) const;
+
+
+/** Analytically compute a geodesic curve starting at the given point, starting in the
+ * given direction, and terminating when it hits the given plane. Only possible
+ * for a few simple shapes, such as spheres and cylinders.
+
+ at param[in] xP Coordinates of the starting point for the geodesic.
+ at param[in] tP The starting tangent direction for the geodesic.
+ at param[in] terminatingPlane The plane in which the end point of the resulting geodesic should lie.
+ at param[in] options Parameters related to geodesic calculation
+ at param[out] geod On exit, this contains the calculated geodesic
+**/
+// XXX what to do if tP is not in the tangent plane at P -- project it?
+// XXX what to do if we don't hit the plane
+void shootGeodesicInDirectionUntilPlaneHitAnalytical(const Vec3& xP, const UnitVec3& tP,
+ const Plane& terminatingPlane, const GeodesicOptions& options,
+ Geodesic& geod) const;
+
+
+/** Utility method to analytically find geodesic between P and Q with initial shooting
+ directions tPhint and tQhint. Only possible for a few simple shapes, such as spheres
+ and cylinders.
+ **/
+void calcGeodesicAnalytical(const Vec3& xP, const Vec3& xQ,
+ const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const;
+
+/**
+ * Utility method to analytically calculate the "geodesic error" between one geodesic
+ * shot from P in the direction tP and another geodesic shot from Q in the
+ * direction tQ. We optionally return the resulting "kinked" geodesic in
+ * case anyone wants it; if the returned error is below tolerance then that
+ * geodesic is the good one. Only possible for a few simple shapes, such as spheres
+ and cylinders.
+ **/
+Vec2 calcSplitGeodErrorAnalytical(const Vec3& P, const Vec3& Q,
+ const UnitVec3& tP, const UnitVec3& tQ,
+ Geodesic* geod=0) const;
+
+/**@}**/
+
+
+/** @name Geodesic-related Debugging **/
+/**@{**/
+
+
+
+/** Get the plane associated with the
+ geodesic hit plane event handler **/
+const Plane& getPlane() const;
+/** Set the plane associated with the
+ geodesic hit plane event handler **/
+void setPlane(const Plane& plane) const;
+/** Get the geodesic for access by visualizer **/
+const Geodesic& getGeodP() const;
+/** Get the geodesic for access by visualizer **/
+const Geodesic& getGeodQ() const;
+const int getNumGeodesicsShot() const;
+void addVizReporter(ScheduledEventReporter* reporter) const;
+/**@}**/
+
+
+
+explicit ContactGeometry(ContactGeometryImpl* impl); /**< Internal use only. **/
+bool isOwnerHandle() const; /**< Internal use only. **/
+bool isEmptyHandle() const; /**< Internal use only. **/
+bool hasImpl() const {return impl != 0;} /**< Internal use only. **/
+/** Internal use only. **/
+const ContactGeometryImpl& getImpl() const {assert(impl); return *impl;}
+/** Internal use only. **/
+ContactGeometryImpl& updImpl() {assert(impl); return *impl; }
+
+protected:
+ContactGeometryImpl* impl; /**< Internal use only. **/
+};
+
+
+
+//==============================================================================
+// HALF SPACE
+//==============================================================================
+/** This ContactGeometry subclass represents an object that occupies the
+entire half-space x>0. This is useful for representing walls and floors. **/
+class SimTK_SIMMATH_EXPORT ContactGeometry::HalfSpace : public ContactGeometry {
+public:
+HalfSpace();
+
+/** Return true if the supplied ContactGeometry object is a halfspace. **/
+static bool isInstance(const ContactGeometry& geo)
+{ return geo.getTypeId()==classTypeId(); }
+/** Cast the supplied ContactGeometry object to a const halfspace. **/
+static const HalfSpace& getAs(const ContactGeometry& geo)
+{ assert(isInstance(geo)); return static_cast<const HalfSpace&>(geo); }
+/** Cast the supplied ContactGeometry object to a writable halfspace. **/
+static HalfSpace& updAs(ContactGeometry& geo)
+{ assert(isInstance(geo)); return static_cast<HalfSpace&>(geo); }
+
+/** Obtain the unique id for HalfSpace contact geometry. **/
+static ContactGeometryTypeId classTypeId();
+
+class Impl; /**< Internal use only. **/
+const Impl& getImpl() const; /**< Internal use only. **/
+Impl& updImpl(); /**< Internal use only. **/
+};
+
+
+
+//==============================================================================
+// CYLINDER
+//==============================================================================
+/** This ContactGeometry subclass represents a cylinder centered at the
+origin, with radius r in the x-y plane, and infinite length along z.
+TODO: should allow finite length to be specified. **/
+class SimTK_SIMMATH_EXPORT ContactGeometry::Cylinder : public ContactGeometry {
+public:
+explicit Cylinder(Real radius);
+Real getRadius() const;
+void setRadius(Real radius);
+
+/** Return true if the supplied ContactGeometry object is a cylinder. **/
+static bool isInstance(const ContactGeometry& geo)
+{ return geo.getTypeId()==classTypeId(); }
+/** Cast the supplied ContactGeometry object to a const cylinder. **/
+static const Cylinder& getAs(const ContactGeometry& geo)
+{ assert(isInstance(geo)); return static_cast<const Cylinder&>(geo); }
+/** Cast the supplied ContactGeometry object to a writable cylinder. **/
+static Cylinder& updAs(ContactGeometry& geo)
+{ assert(isInstance(geo)); return static_cast<Cylinder&>(geo); }
+
+/** Obtain the unique id for Cylinder contact geometry. **/
+static ContactGeometryTypeId classTypeId();
+
+class Impl; /**< Internal use only. **/
+const Impl& getImpl() const; /**< Internal use only. **/
+Impl& updImpl(); /**< Internal use only. **/
+};
+
+
+
+//==============================================================================
+// SPHERE
+//==============================================================================
+/** This ContactGeometry subclass represents a sphere centered at the
+origin. **/
+class SimTK_SIMMATH_EXPORT ContactGeometry::Sphere : public ContactGeometry {
+public:
+explicit Sphere(Real radius);
+Real getRadius() const;
+void setRadius(Real radius);
+
+/** Return true if the supplied ContactGeometry object is a sphere. **/
+static bool isInstance(const ContactGeometry& geo)
+{ return geo.getTypeId()==classTypeId(); }
+/** Cast the supplied ContactGeometry object to a const sphere. **/
+static const Sphere& getAs(const ContactGeometry& geo)
+{ assert(isInstance(geo)); return static_cast<const Sphere&>(geo); }
+/** Cast the supplied ContactGeometry object to a writable sphere. **/
+static Sphere& updAs(ContactGeometry& geo)
+{ assert(isInstance(geo)); return static_cast<Sphere&>(geo); }
+
+/** Obtain the unique id for Sphere contact geometry. **/
+static ContactGeometryTypeId classTypeId();
+
+class Impl; /**< Internal use only. **/
+const Impl& getImpl() const; /**< Internal use only. **/
+Impl& updImpl(); /**< Internal use only. **/
+};
+
+
+
+//==============================================================================
+// ELLIPSOID
+//==============================================================================
+/** This ContactGeometry subclass represents an ellipsoid centered at the
+origin, with its principal axes pointing along the x, y, and z axes and
+half dimensions a,b, and c (all > 0) along those axes, respectively. The
+implicit equation f(x,y,z)=0 of the ellipsoid surface is <pre>
+ f(x,y,z) = Ax^2+By^2+Cz^2 - 1
+ where A=1/a^2, B=1/b^2, C=1/c^2
+</pre>
+A,B, and C are the squares of the principal curvatures ka=1/a, kb=1/b, and
+kc=1/c.
+
+The interior of the ellipsoid consists of all points such that f(x,y,z)<0 and
+points exterior satisfy f(x,y,z)>0. The region around any point (x,y,z) on an
+ellipsoid surface is locally an elliptic paraboloid with equation <pre>
+ -2 z' = kmax x'^2 + kmin y'^2
+</pre>
+where z' is measured along the the outward unit normal n at (x,y,z), x' is
+measured along the the unit direction u of maximum curvature, and y' is
+measured along the unit direction v of minimum curvature. kmax,kmin are the
+curvatures with kmax >= kmin > 0. The signs of the mutually perpendicular
+vectors u and v are chosen so that (u,v,n) forms a right-handed coordinate
+system for the paraboloid. **/
+class SimTK_SIMMATH_EXPORT ContactGeometry::Ellipsoid : public ContactGeometry {
+public:
+/** Construct an Ellipsoid given its three principal half-axis dimensions a,b,c
+(all positive) along the local x,y,z directions respectively. The curvatures
+(reciprocals of radii) are precalculated here at a cost of about 30 flops. **/
+explicit Ellipsoid(const Vec3& radii);
+/** Obtain the three half-axis dimensions a,b,c used to define this
+ellipsoid. **/
+const Vec3& getRadii() const;
+/** Set the three half-axis dimensions a,b,c (all positive) used to define this
+ellipsoid, overriding the current radii and recalculating the principal
+curvatures at a cost of about 30 flops.
+ at param[in] radii The three half-dimensions of the ellipsoid, in the
+ ellipsoid's local x, y, and z directions respectively. **/
+void setRadii(const Vec3& radii);
+
+/** For efficiency we precalculate the principal curvatures whenever the
+ellipsoid radii are set; this avoids having to repeatedly perform these three
+expensive divisions at runtime. The curvatures are ka=1/a, kb=1/b, and kc=1/c
+so that the ellipsoid's implicit equation can be written Ax^2+By^2+Cz^2=1,
+with A=ka^2, etc. **/
+const Vec3& getCurvatures() const;
+
+/** Given a point \a P =(x,y,z) on the ellipsoid surface, return the unique unit
+outward normal to the ellipsoid at that point. If \a P is not on the surface,
+the result is the same as for the point obtained by scaling the vector
+\a P - O until it just touches the surface. That is, we compute
+P'=findPointInThisDirection(P) and then return the normal at P'. Cost is about
+40 flops regardless of whether P was initially on the surface.
+ at param[in] P A point on the ellipsoid surface, measured and expressed in the
+ ellipsoid's local frame. See text for what happens if \a P is
+ not actually on the ellipsoid surface.
+ at return The outward-facing unit normal at point \a P (or at the surface point
+pointed to by \a P).
+ at see findPointInSameDirection() **/
+UnitVec3 findUnitNormalAtPoint(const Vec3& P) const;
+
+/** Given a unit direction \a n, find the unique point P on the ellipsoid
+surface at which the outward-facing normal is \a n. Cost is about 40 flops.
+ at param[in] n The unit vector for which we want to find a match on the
+ ellipsoid surface, expressed in the ellipsoid's local frame.
+ at return The point on the ellipsoid's surface at which the outward-facing
+normal is the same as \a n. The point is measured and expressed in the
+ellipsoid's local frame. **/
+Vec3 findPointWithThisUnitNormal(const UnitVec3& n) const;
+
+/** Given a direction d defined by the vector Q-O for an arbitrary point in
+space Q=(x,y,z)!=O, find the unique point P on the ellipsoid surface that is
+in direction d from the ellipsoid origin O. That is, P=s*d for some scalar
+s > 0 such that f(P)=0. Cost is about 40 flops.
+ at param[in] Q A point in space measured from the ellipsoid origin but not the
+ origin.
+ at return P, the intersection of the ray in the direction Q-O with the ellipsoid
+surface **/
+Vec3 findPointInSameDirection(const Vec3& Q) const;
+
+/** Given a point Q on the surface of the ellipsoid, find the approximating
+paraboloid at Q in a frame P where OP=Q, Pz is the outward-facing unit
+normal to the ellipsoid at Q, Px is the direction of maximum curvature
+and Py is the direction of minimum curvature. k=(kmax,kmin) are the returned
+curvatures with kmax >= kmin > 0. The equation of the resulting paraboloid
+in the P frame is -2z = kmax*x^2 + kmin*y^2. Cost is about 260 flops; you can
+save a little time if you already know the normal at Q by using the other
+overloaded signature for this method.
+
+ at warning It is up to you to make sure that Q is actually on the ellipsoid
+surface. If it is not you will quietly get a meaningless result.
+
+ at param[in] Q A point on the surface of this ellipsoid, measured and
+ expressed in the ellipsoid's local frame.
+ at param[out] X_EP The frame of the paraboloid P, measured and expressed in
+ the ellipsoid local frame E. X_EP.p() is \a Q, X_EP.x()
+ is the calculated direction of maximum curvature kmax; y()
+ is the direction of minimum curvature kmin; z is the
+ outward facing normal at \a Q.
+ at param[out] k The maximum (k[0]) and minimum (k[1]) curvatures of the
+ ellipsoid (and paraboloid P) at point \a Q.
+ at see findParaboloidAtPointWithNormal() **/
+void findParaboloidAtPoint(const Vec3& Q, Transform& X_EP, Vec2& k) const;
+
+/** If you already have both a point and the unit normal at that point, this
+will save about 40 flops by trusting that you have provided the correct normal;
+be careful -- no one is going to check that you got this right. The results are
+meaningless if the point and normal are not consistent. Cost is about 220 flops.
+ at see findParaboloidAtPoint() for details **/
+void findParaboloidAtPointWithNormal(const Vec3& Q, const UnitVec3& n,
+ Transform& X_EP, Vec2& k) const;
+
+/** Return true if the supplied ContactGeometry object is an Ellipsoid. **/
+static bool isInstance(const ContactGeometry& geo)
+{ return geo.getTypeId()==classTypeId(); }
+/** Cast the supplied ContactGeometry object to a const Ellipsoid. **/
+static const Ellipsoid& getAs(const ContactGeometry& geo)
+{ assert(isInstance(geo)); return static_cast<const Ellipsoid&>(geo); }
+/** Cast the supplied ContactGeometry object to a writable Ellipsoid. **/
+static Ellipsoid& updAs(ContactGeometry& geo)
+{ assert(isInstance(geo)); return static_cast<Ellipsoid&>(geo); }
+
+/** Obtain the unique id for Ellipsoid contact geometry. **/
+static ContactGeometryTypeId classTypeId();
+
+class Impl; /**< Internal use only. **/
+const Impl& getImpl() const; /**< Internal use only. **/
+Impl& updImpl(); /**< Internal use only. **/
+};
+
+
+
+//==============================================================================
+// SMOOTH HEIGHT MAP
+//==============================================================================
+/** This ContactGeometry subclass represents a smooth surface fit through a
+set of sampled points using bicubic patches to provide C2 continuity. It is
+particularly useful as a bounded terrain. The boundary is an axis-aligned
+rectangle in the local x-y plane. Within the boundary, every (x,y) location has
+a unique height z, so caves and overhangs cannot be represented.
+
+The surface is parameterized as z=f(x,y) where x,y,z are measured in
+the surface's local coordinate frame. This can also be described as the
+implicit function F(x,y,z)=f(x,y)-z=0, as though this were an infinitely thick
+slab in the -z direction below the surface. **/
+class SimTK_SIMMATH_EXPORT
+ContactGeometry::SmoothHeightMap : public ContactGeometry {
+public:
+/** Create a SmoothHeightMap surface using an already-existing BicubicSurface.
+The BicubicSurface object is referenced, not copied, so it may be shared with
+other users. **/
+explicit SmoothHeightMap(const BicubicSurface& surface);
+
+/** Return a reference to the BicubicSurface object being used by this
+SmoothHeightMap. **/
+const BicubicSurface& getBicubicSurface() const;
+
+/** (Advanced) Return a reference to the oriented bounding box tree for this
+surface. **/
+const OBBTree& getOBBTree() const;
+
+/** Return true if the supplied ContactGeometry object is a SmoothHeightMap. **/
+static bool isInstance(const ContactGeometry& geo)
+{ return geo.getTypeId()==classTypeId(); }
+/** Cast the supplied ContactGeometry object to a const SmoothHeightMap. **/
+static const SmoothHeightMap& getAs(const ContactGeometry& geo)
+{ assert(isInstance(geo)); return static_cast<const SmoothHeightMap&>(geo); }
+/** Cast the supplied ContactGeometry object to a writable SmoothHeightMap. **/
+static SmoothHeightMap& updAs(ContactGeometry& geo)
+{ assert(isInstance(geo)); return static_cast<SmoothHeightMap&>(geo); }
+
+/** Obtain the unique id for SmoothHeightMap contact geometry. **/
+static ContactGeometryTypeId classTypeId();
+
+class Impl; /**< Internal use only. **/
+const Impl& getImpl() const; /**< Internal use only. **/
+Impl& updImpl(); /**< Internal use only. **/
+};
+
+
+
+//==============================================================================
+// TRIANGLE MESH
+//==============================================================================
+/** This ContactGeometry subclass represents an arbitrary shape described by a
+mesh of triangular faces. The mesh surface must satisfy the following
+requirements:
+ - It must be closed, so that any point can unambiguously be classified as
+ either inside or outside.
+ - It may not intersect itself anywhere, even at a single point.
+ - It must be an oriented manifold.
+ - The vertices for each face must be ordered counter-clockwise when viewed
+ from the outside. That is, if v0, v1, and v2 are the locations of the
+ three vertices for a face, the cross product (v1-v0)%(v2-v0) must point
+ outward.
+ - The length of every edge must be non-zero.
+
+It is your responsibility to ensure that any mesh you create meets these
+requirements. The constructor will detect many incorrect meshes and signal them
+by throwing an exception, but it is not guaranteed to detect all possible
+problems. If a mesh fails to satisfy any of these requirements, the results of
+calculations performed with it are undefined. For example, collisions involving
+it might fail to be detected, or contact forces on it might be calculated
+incorrectly. **/
+class SimTK_SIMMATH_EXPORT ContactGeometry::TriangleMesh
+: public ContactGeometry {
+public:
+class OBBTreeNode;
+/** Create a TriangleMesh.
+ at param vertices The positions of all vertices in the mesh.
+ at param faceIndices The indices of the vertices that make up each face. The
+ first three elements are the vertices in the first face,
+ the next three elements are the vertices in the second
+ face, etc.
+ at param smooth If true, the mesh will be treated as a smooth surface, and
+ normal vectors will be smoothly interpolated between
+ vertices. If false, it will be treated as a faceted mesh
+ with a constant normal vector over each face. **/
+TriangleMesh(const ArrayViewConst_<Vec3>& vertices, const ArrayViewConst_<int>& faceIndices, bool smooth=false);
+/** Create a TriangleMesh based on a PolygonalMesh object. If any faces of the
+PolygonalMesh have more than three vertices, they are automatically
+triangulated.
+ at param mesh The PolygonalMesh from which to construct a triangle mesh.
+ at param smooth If true, the mesh will be treated as a smooth surface, and
+ normal vectors will be smoothly interpolated between vertices.
+ If false, it will be treated as a faceted mesh with a constant
+ normal vector over each face. **/
+explicit TriangleMesh(const PolygonalMesh& mesh, bool smooth=false);
+/** Get the number of edges in the mesh. **/
+int getNumEdges() const;
+/** Get the number of faces in the mesh. **/
+int getNumFaces() const;
+/** Get the number of vertices in the mesh. **/
+int getNumVertices() const;
+/** Get the position of a vertex in the mesh.
+ at param index The index of the vertex to get.
+ at return The position of the specified vertex. **/
+const Vec3& getVertexPosition(int index) const;
+/** Get the index of one of the edges of a face. Edge 0 connects vertices 0
+and 1. Edge 1 connects vertices 1 and 2. Edge 2 connects vertices 0 and 2.
+ at param face The index of the face.
+ at param edge The index of the edge within the face (0, 1, or 2).
+ at return The index of the specified edge. **/
+int getFaceEdge(int face, int edge) const;
+/** Get the index of one of the vertices of a face.
+ at param face The index of the face.
+ at param vertex The index of the vertex within the face (0, 1, or 2).
+ at return The index of the specified vertex. **/
+int getFaceVertex(int face, int vertex) const;
+/** Get the index of one of the faces shared by an edge
+ at param edge The index of the edge.
+ at param face The index of the face within the edge (0 or 1).
+ at return The index of the specified face. **/
+int getEdgeFace(int edge, int face) const;
+/** Get the index of one of the vertices shared by an edge.
+ at param edge The index of the edge.
+ at param vertex The index of the vertex within the edge (0 or 1).
+ at return The index of the specified vertex. **/
+int getEdgeVertex(int edge, int vertex) const;
+/** Find all edges that intersect a vertex.
+ at param vertex The index of the vertex.
+ at param edges The indices of all edges intersecting the vertex will be added
+ to this. **/
+void findVertexEdges(int vertex, Array_<int>& edges) const;
+/** Get the normal vector for a face. This points outward from the mesh.
+ at param face The index of the face. **/
+const UnitVec3& getFaceNormal(int face) const;
+/** Get the area of a face.
+ at param face The index of the face. **/
+Real getFaceArea(int face) const;
+/** Calculate the location of a point on the surface, in the local frame of
+the TriangleMesh. Cost is 11 flops.
+ at param face The index of the face containing the point.
+ at param uv The point within the face, specified by its barycentric uv
+ coordinates. **/
+Vec3 findPoint(int face, const Vec2& uv) const;
+/** Calculate the location of a face's centroid, that is, the point uv=(1/3,1/3)
+which is the average of the three vertex locations. This is a common special
+case of findPoint() that can be calculated more quickly (7 flops).
+ at param face The index of the face whose centroid is of interest. **/
+Vec3 findCentroid(int face) const;
+/** Calculate the normal vector at a point on the surface.
+ at param face The index of the face containing the point.
+ at param uv The point within the face, specified by its barycentric uv
+ coordinates. **/
+UnitVec3 findNormalAtPoint(int face, const Vec2& uv) const;
+/** Given a point, find the nearest point on the surface of this object. If
+multiple points on the surface are equally close to the specified point, this
+may return any of them.
+ at param position The point in question.
+ at param inside On exit, this is set to true if the specified point is
+ inside this object, false otherwise.
+ at param normal On exit, this contains the surface normal at the returned
+ point.
+ at return The point on the surface of the object which is closest to the
+specified point. **/
+Vec3 findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const;
+/** Given a point, find the nearest point on the surface of this object. If
+multiple points on the surface are equally close to the specified point, this
+may return any of them.
+ at param position The point in question.
+ at param inside On exit, this is set to true if the specified point is
+ inside this object, false otherwise.
+ at param face On exit, this contains the index of the face containing the
+ returned point.
+ at param uv On exit, this contains the barycentric coordinates (u and v)
+ of the returned point within its face.
+ at return The point on the surface of the object which is closest to the
+specified point. **/
+Vec3 findNearestPoint(const Vec3& position, bool& inside, int& face, Vec2& uv) const;
+
+/** Given a point and a face of this object, find the point of the face that is
+nearest the given point. If multiple points on the face are equally close to
+the specified point, this may return any of them.
+ at param position The point in question.
+ at param face The face to be examined.
+ at param uv On exit, this contains the barycentric coordinates (u and v)
+ of the returned point within the face.
+ at return The face point, in the surface's frame, that is closest to the
+specified point. **/
+Vec3 findNearestPointToFace(const Vec3& position, int face, Vec2& uv) const;
+
+
+/** Determine whether this mesh intersects a ray, and if so, find the
+intersection point.
+ at param origin The position at which the ray begins.
+ at param direction The ray direction.
+ at param distance If an intersection is found, the distance from the ray origin
+ to the intersection point is stored in this. Otherwise, it is
+ left unchanged.
+ at param normal If an intersection is found, the surface normal of the
+ intersection point is stored in this. Otherwise, it is left
+ unchanged.
+ at return \c true if an intersection is found, \c false otherwise. **/
+bool intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance, UnitVec3& normal) const;
+/** Determine whether this mesh intersects a ray, and if so, find what face it
+hit.
+ at param origin The position at which the ray begins.
+ at param direction The ray direction.
+ at param distance If an intersection is found, the distance from the ray origin
+ to the intersection point is stored in this. Otherwise, it is
+ left unchanged.
+ at param face If an intersection is found, the index of the face hit by the
+ ray is stored in this. Otherwise, it is left unchanged.
+ at param uv If an intersection is found, the barycentric coordinates (u
+ and v) of the intersection point within the hit face are
+ stored in this. Otherwise, it is left unchanged.
+ at return \c true if an intersection is found, \c false otherwise. **/
+bool intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance, int& face, Vec2& uv) const;
+/** Get the OBBTreeNode which forms the root of this mesh's Oriented Bounding
+Box Tree. **/
+OBBTreeNode getOBBTreeNode() const;
+
+/** Generate a PolygonalMesh from this TriangleMesh; useful mostly for debugging
+because you can create a DecorativeMesh from this and then look at it. **/
+PolygonalMesh createPolygonalMesh() const;
+
+/** Return true if the supplied ContactGeometry object is a triangle mesh. **/
+static bool isInstance(const ContactGeometry& geo)
+{ return geo.getTypeId()==classTypeId(); }
+/** Cast the supplied ContactGeometry object to a const triangle mesh. **/
+static const TriangleMesh& getAs(const ContactGeometry& geo)
+{ assert(isInstance(geo)); return static_cast<const TriangleMesh&>(geo); }
+/** Cast the supplied ContactGeometry object to a writable triangle mesh. **/
+static TriangleMesh& updAs(ContactGeometry& geo)
+{ assert(isInstance(geo)); return static_cast<TriangleMesh&>(geo); }
+
+/** Obtain the unique id for TriangleMesh contact geometry. **/
+static ContactGeometryTypeId classTypeId();
+
+class Impl; /**< Internal use only. **/
+const Impl& getImpl() const; /**< Internal use only. **/
+Impl& updImpl(); /**< Internal use only. **/
+};
+
+
+
+//==============================================================================
+// TRIANGLE MESH :: OBB TREE NODE
+//==============================================================================
+/** This class represents a node in the Oriented Bounding Box Tree for a
+TriangleMesh. Each node has an OrientedBoundingBox that fully encloses all
+triangles contained within it or its children. This is a binary tree: each
+non-leaf node has two children. Triangles are stored only in the leaf nodes. **/
+class SimTK_SIMMATH_EXPORT ContactGeometry::TriangleMesh::OBBTreeNode {
+public:
+OBBTreeNode(const OBBTreeNodeImpl& impl);
+/** Get the OrientedBoundingBox which encloses all triangles in this node or
+its children. **/
+const OrientedBoundingBox& getBounds() const;
+/** Get whether this is a leaf node. **/
+bool isLeafNode() const;
+/** Get the first child node. Calling this on a leaf node will produce an
+exception. **/
+const OBBTreeNode getFirstChildNode() const;
+/** Get the second child node. Calling this on a leaf node will produce an
+exception. **/
+const OBBTreeNode getSecondChildNode() const;
+/** Get the indices of all triangles contained in this node. Calling this on a
+non-leaf node will produce an exception. **/
+const Array_<int>& getTriangles() const;
+/** Get the number of triangles inside this node. If this is not a leaf node,
+this is the total number of triangles contained by all children of this
+node. **/
+int getNumTriangles() const;
+
+private:
+const OBBTreeNodeImpl* impl;
+};
+
+//==============================================================================
+// TORUS
+//==============================================================================
+/** This ContactGeometry subclass represents a torus centered at the
+origin with the axial direction aligned to the z-axis. It is defined by
+a torusRadius (radius of the circular centerline of the torus, measured
+from the origin), and a tubeRadius (radius of the torus cross-section:
+perpendicular distance from the circular centerline to the surface). **/
+class SimTK_SIMMATH_EXPORT ContactGeometry::Torus : public ContactGeometry {
+public:
+Torus(Real torusRadius, Real tubeRadius);
+Real getTorusRadius() const;
+void setTorusRadius(Real radius);
+Real getTubeRadius() const;
+void setTubeRadius(Real radius);
+
+/** Return true if the supplied ContactGeometry object is a torus. **/
+static bool isInstance(const ContactGeometry& geo)
+{ return geo.getTypeId()==classTypeId(); }
+/** Cast the supplied ContactGeometry object to a const torus. **/
+static const Torus& getAs(const ContactGeometry& geo)
+{ assert(isInstance(geo)); return static_cast<const Torus&>(geo); }
+/** Cast the supplied ContactGeometry object to a writable torus. **/
+static Torus& updAs(ContactGeometry& geo)
+{ assert(isInstance(geo)); return static_cast<Torus&>(geo); }
+
+/** Obtain the unique id for Torus contact geometry. **/
+static ContactGeometryTypeId classTypeId();
+
+class Impl; /**< Internal use only. **/
+const Impl& getImpl() const; /**< Internal use only. **/
+Impl& updImpl(); /**< Internal use only. **/
+};
+
+
+
+
+//==============================================================================
+// GEODESIC EVALUATOR helper classes
+//==============================================================================
+
+
+/**
+ * A simple plane class
+ **/
+class Plane {
+public:
+ Plane() : m_normal(1,0,0), m_offset(0) { }
+ Plane(const Vec3& normal, const Real& offset)
+ : m_normal(normal), m_offset(offset) { }
+
+ Real getDistance(const Vec3& pt) const {
+ return ~m_normal*pt - m_offset;
+ }
+
+ Vec3 getNormal() const {
+ return m_normal;
+ }
+
+ Real getOffset() const {
+ return m_offset;
+ }
+
+private:
+ Vec3 m_normal;
+ Real m_offset;
+}; // class Plane
+
+
+/**
+ * A event handler to terminate integration when geodesic hits the plane.
+ * For use with a ParticleConSurfaceSystem
+ **/
+class GeodHitPlaneEvent : public TriggeredEventHandler {
+public:
+ GeodHitPlaneEvent()
+ : TriggeredEventHandler(Stage::Position) { }
+
+ explicit GeodHitPlaneEvent(const Plane& aplane)
+ : TriggeredEventHandler(Stage::Position) {
+ plane = aplane;
+ }
+
+ // event is triggered if distance of geodesic endpoint to plane is zero
+ Real getValue(const State& state) const {
+ if (!enabled) {
+ return 1;
+ }
+ Vec3 endpt(&state.getQ()[0]);
+ Real dist = plane.getDistance(endpt);
+// std::cout << "dist = " << dist << std::endl;
+ return dist;
+ }
+
+ // This method is called whenever this event occurs.
+ void handleEvent(State& state, Real accuracy, bool& shouldTerminate) const {
+ if (!enabled) {
+ return;
+ }
+
+ // This should be triggered when geodesic endpoint to plane is zero.
+ Vec3 endpt;
+ const Vector& q = state.getQ();
+ endpt[0] = q[0]; endpt[1] = q[1]; endpt[2] = q[2];
+ Real dist = plane.getDistance(endpt);
+
+// ASSERT(std::abs(dist) < 0.01 );
+ shouldTerminate = true;
+// std::cout << "hit plane!" << std::endl;
+ }
+
+ void setPlane(const Plane& aplane) const {
+ plane = aplane;
+ }
+
+ const Plane& getPlane() const {
+ return plane;
+ }
+
+ const void setEnabled(bool enabledFlag) {
+ enabled = enabledFlag;
+ }
+
+ const bool isEnabled() {
+ return enabled;
+ }
+
+private:
+ mutable Plane plane;
+ bool enabled;
+
+}; // class GeodHitPlaneEvent
+
+/**
+ * This class generates decoration for contact points and straight line path segments
+ **/
+class PathDecorator : public DecorationGenerator {
+public:
+ PathDecorator(const Vector& x, const Vec3& O, const Vec3& I, const Vec3& color) :
+ m_x(x), m_O(O), m_I(I), m_color(color) { }
+
+ virtual void generateDecorations(const State& state,
+ Array_<DecorativeGeometry>& geometry) {
+// m_system.realize(state, Stage::Position);
+
+ Vec3 P, Q;
+ P[0] = m_x[0]; P[1] = m_x[1]; P[2] = m_x[2];
+ Q[0] = m_x[3]; Q[1] = m_x[4]; Q[2] = m_x[5];
+
+ geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(m_O));
+ geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(P));
+ geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(Q));
+ geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(m_I));
+
+ geometry.push_back(DecorativeLine(m_O,P)
+ .setColor(m_color)
+ .setLineThickness(2));
+ geometry.push_back(DecorativeLine(Q,m_I)
+ .setColor(m_color)
+ .setLineThickness(2));
+
+ }
+
+private:
+ const Vector& m_x; // x = ~[P Q]
+ const Vec3& m_O;
+ const Vec3& m_I;
+ const Vec3& m_color;
+ Rotation R_plane;
+ Vec3 offset;
+}; // class DecorationGenerator
+
+
+/**
+ * This class generates decoration for a plane
+ **/
+class PlaneDecorator : public DecorationGenerator {
+public:
+ PlaneDecorator(const Plane& plane, const Vec3& color) :
+ m_plane(plane), m_color(color) { }
+
+ virtual void generateDecorations(const State& state,
+ Array_<DecorativeGeometry>& geometry) {
+// m_system.realize(state, Stage::Position);
+
+ // draw plane
+ R_plane.setRotationFromOneAxis(UnitVec3(m_plane.getNormal()),
+ CoordinateAxis::XCoordinateAxis());
+ offset = 0;
+ offset[0] = m_plane.getOffset();
+ geometry.push_back(
+ DecorativeBrick(Vec3(Real(.01),1,1))
+ .setTransform(Transform(R_plane, R_plane*offset))
+ .setColor(m_color)
+ .setOpacity(Real(.2)));
+ }
+
+private:
+ const Plane& m_plane;
+ const Vec3& m_color;
+ Rotation R_plane;
+ Vec3 offset;
+}; // class DecorationGenerator
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_CONTACT_GEOMETRY_H_
diff --git a/SimTKmath/Geometry/include/simmath/internal/ContactTracker.h b/SimTKmath/Geometry/include/simmath/internal/ContactTracker.h
new file mode 100644
index 0000000..b426fa7
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/ContactTracker.h
@@ -0,0 +1,602 @@
+#ifndef SimTK_SIMMATH_CONTACT_TRACKER_SUBSYSTEM_H_
+#define SimTK_SIMMATH_CONTACT_TRACKER_SUBSYSTEM_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/internal/Contact.h"
+
+namespace SimTK {
+
+//==============================================================================
+// CONTACT TRACKER
+//==============================================================================
+/** A ContactTracker implements an algorithm for detecting overlaps or
+potential overlaps between pairs of ContactGeometry objects, and managing
+Contact objects that track individual contacts as they evolve through
+time. This class is used internally by ContractTrackerSubsystem and there
+usually is no reason to access it directly. The exception is if you are
+defining a new ContactGeometry subclass. In that case, you will also need to
+define one or more ContactTrackers to detect collisions with your new geometry
+type, then register it with the ContactTrackerSubsystem.
+
+The result of a ContactTracker when applied to a pair of contact
+surfaces, is either a determination that the surfaces are not in contact,
+or a Contact object describing their contact interaction. There are different
+types of these Contact objects (for example, PointContact, LineContact,
+MeshContact) and the same algorithm may result in different kinds of Contact
+under different circumstances. At each evaluation, the caller passes in the
+previous Contact object, if any, that was associated with two ContactSurfaces,
+then receives an update from the algorithm.
+
+Note that ContactTrackers that manage dissimilar geometry type pairs expect
+the two types in a particular order, e.g. (halfspace,sphere) rather than
+(sphere,halfspace) but are used for all contacts involving that pair of
+types. It is up to the ContactTrackerSubsystem to ensure that the contact
+surfaces are presented in the correct order regardless of how they are
+encountered. The Contact objects that are created and managed by trackers
+always have their (surface1,surface2) pairs in the order required by the
+tracker that handles those types. **/
+class SimTK_SIMMATH_EXPORT ContactTracker {
+public:
+class HalfSpaceSphere;
+class HalfSpaceEllipsoid;
+class SphereSphere;
+class HalfSpaceTriangleMesh;
+class SphereTriangleMesh;
+class TriangleMeshTriangleMesh;
+class ConvexImplicitPair;
+class GeneralImplicitPair;
+
+/** Base class constructor for use by the concrete classes. **/
+ContactTracker(ContactGeometryTypeId typeOfSurface1,
+ ContactGeometryTypeId typeOfSurface2)
+: m_surfaceTypes(typeOfSurface1, typeOfSurface2)
+{
+}
+
+/** Return the pair of contact geometry type ids handled by this tracker,
+in the order that they must be presented to the tracker's methods. **/
+const std::pair<ContactGeometryTypeId,ContactGeometryTypeId>&
+getContactGeometryTypeIds() const {return m_surfaceTypes;}
+
+virtual ~ContactTracker() {}
+
+/** The ContactTrackerSubsystem will invoke this method for any pair of
+contact surfaces that is already being tracked, or for which the static broad
+phase analysis indicated that they might be in contact now. Only position
+information is available. Note that the arguments and Contact object surfaces
+must be ordered by geometry type id as required by this tracker. **/
+virtual bool trackContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Contact& currentStatus) const = 0;
+
+/** The ContactTrackerSubsystem will invoke this method for any tracked pair of
+contact surfaces that is still not in contact after trackContact() looked at
+it, or any untracked pair for which the dynamic broad phase indicated that
+they might be in contact within the interval of interest. Position, velocity,
+and acceleration information may be used. Ordering must be correct as
+discussed for trackContact(). **/
+virtual bool predictContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& predictedStatus) const = 0;
+
+/** At the beginning of a simulation we will have no past information to
+help disambiguate tricky contact situations. This method may use current
+position and velocity information in heuristics for guessing the contact
+status between the indicated pair of surfaces. Ordering must be correct as
+discussed for trackContact(). **/
+virtual bool initializeContact
+ (const Transform& X_GS1, const SpatialVec& V_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& contactStatus) const = 0;
+
+/** Given two shapes for which implicit functions are known, and a rough-guess
+contact point for each shape (each measured and expressed in its own surface's
+frame), refine those contact points to obtain the nearest
+pair that satisfies contact conditions to a requested accuracy. For separated
+objects, these will be the points of closest approach between the surfaces; for
+contacting objects these are the points of maximum penetration.
+
+In implicit form there are six unknowns (spatial coordinates of the contact
+points). The six contact conditions we use are:
+ - 2 equations: Point P is on the surface of shape A and point Q is on the
+ surface of shape B (that is, the implicit functions both return zero).
+ - 2 equations: The normal vector at point P is perpendicular to the
+ tangent plane at point Q.
+ - 2 equations: The separation vector (P-Q) is zero or perpendicular to the
+ tangent plane at P.
+
+Note that these equations could be satisfied by incorrect points that have the
+opposite normals because the perpendicularity conditions can't distinguish n
+from -n. We are depending on having an initial guess that is good enough so
+that we find the correct solution by going downhill from there. Don't try to
+use this if you don't have a reasonably good guess already. For \e convex
+implicit surfaces you can use estimateConvexImplicitPairContactUsingMPR() to
+get a good start if the surfaces are in contact.
+
+ at returns \c true if the requested accuracy is achieved but returns its best
+attempt at the refined points regardless. **/
+static bool refineImplicitPair
+ (const ContactGeometry& shapeA, Vec3& pointP_A, // in/out
+ const ContactGeometry& shapeB, Vec3& pointQ_B, // in/out
+ const Transform& X_AB, Real accuracyRequested,
+ Real& accuracyAchieved, int& numIterations);
+
+/** Calculate the error function described in refineImplicitPair(). **/
+static Vec6 findImplicitPairError
+ (const ContactGeometry& shapeA, const Vec3& pointP,
+ const ContactGeometry& shapeB, const Vec3& pointQ,
+ const Transform& X_AB);
+
+/** Calculate the partial derivatives of the findImplicitPairError() error
+function with respect to the locations of the two points in their own surface's
+frame. This might be an approximation of the derivative; it needs only to be
+good enough for refineImplicitPair() to get usable directional information. **/
+static Mat66 calcImplicitPairJacobian
+ (const ContactGeometry& shapeA, const Vec3& pointP,
+ const ContactGeometry& shapeB, const Vec3& pointQ,
+ const Transform& X_AB, const Vec6& err0);
+
+/** Use Minkowski Portal Refinement (XenoCollide method by G. Snethen) to
+generate a reasonably good starting estimate of the contact points between
+two \e convex implicit shapes that may be in contact. MPR cannot find those
+points if the surfaces are separated. Returns \c false if the two shapes
+are definitely \e not in contact (MPR found a separating plane); in that case
+the returned direction is the separating plane normal and the points are the
+support points that prove separation. Otherwise, there \e might be contact
+and the points are estimates of the contact point on each surface, determined
+roughly to the requested accuracy. You still have to refine these and it might
+turn out there is no contact after all. **/
+static bool estimateConvexImplicitPairContactUsingMPR
+ (const ContactGeometry& shapeA, const ContactGeometry& shapeB,
+ const Transform& X_AB,
+ Vec3& pointP_A, Vec3& pointQ_B, UnitVec3& dirInA,
+ int& numIterations);
+
+
+//--------------------------------------------------------------------------
+ private:
+// This tracker should be called only for surfaces of these two types,
+// in this order.
+std::pair<ContactGeometryTypeId,ContactGeometryTypeId> m_surfaceTypes;
+};
+
+
+
+//==============================================================================
+// HALFSPACE-SPHERE CONTACT TRACKER
+//==============================================================================
+/** This ContactTracker handles contacts between a ContactGeometry::HalfSpace
+and a ContactGeometry::Sphere, in that order. **/
+class SimTK_SIMMATH_EXPORT ContactTracker::HalfSpaceSphere
+: public ContactTracker {
+public:
+HalfSpaceSphere()
+: ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
+ ContactGeometry::Sphere::classTypeId()) {}
+
+virtual ~HalfSpaceSphere() {}
+
+virtual bool trackContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Contact& currentStatus) const;
+
+virtual bool predictContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& predictedStatus) const;
+
+virtual bool initializeContact
+ (const Transform& X_GS1, const SpatialVec& V_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& contactStatus) const;
+};
+
+
+
+//==============================================================================
+// HALFSPACE-ELLIPSOID CONTACT TRACKER
+//==============================================================================
+/** This ContactTracker handles contacts between a ContactGeometry::HalfSpace
+and a ContactGeometry::Ellipsoid, in that order. **/
+class SimTK_SIMMATH_EXPORT ContactTracker::HalfSpaceEllipsoid
+: public ContactTracker {
+public:
+HalfSpaceEllipsoid()
+: ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
+ ContactGeometry::Ellipsoid::classTypeId()) {}
+
+virtual ~HalfSpaceEllipsoid() {}
+
+virtual bool trackContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Contact& currentStatus) const;
+
+virtual bool predictContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& predictedStatus) const;
+
+virtual bool initializeContact
+ (const Transform& X_GS1, const SpatialVec& V_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& contactStatus) const;
+};
+
+
+
+//==============================================================================
+// SPHERE-SPHERE CONTACT TRACKER
+//==============================================================================
+/** This ContactTracker handles contacts between two ContactGeometry::Sphere
+objects. **/
+class SimTK_SIMMATH_EXPORT ContactTracker::SphereSphere
+: public ContactTracker {
+public:
+SphereSphere()
+: ContactTracker(ContactGeometry::Sphere::classTypeId(),
+ ContactGeometry::Sphere::classTypeId()) {}
+
+virtual ~SphereSphere() {}
+
+virtual bool trackContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Contact& currentStatus) const;
+
+virtual bool predictContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& predictedStatus) const;
+
+virtual bool initializeContact
+ (const Transform& X_GS1, const SpatialVec& V_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& contactStatus) const;
+};
+
+
+
+//==============================================================================
+// HALFSPACE-TRIANGLE MESH CONTACT TRACKER
+//==============================================================================
+/** This ContactTracker handles contacts between a ContactGeometry::HalfSpace
+and a ContactGeometry::TriangleMesh, in that order. **/
+class SimTK_SIMMATH_EXPORT ContactTracker::HalfSpaceTriangleMesh
+: public ContactTracker {
+public:
+HalfSpaceTriangleMesh()
+: ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
+ ContactGeometry::TriangleMesh::classTypeId()) {}
+
+virtual ~HalfSpaceTriangleMesh() {}
+
+virtual bool trackContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1,
+ const ContactGeometry& surface1, // the half space
+ const Transform& X_GS2,
+ const ContactGeometry& surface2, // the mesh
+ Real cutoff,
+ Contact& currentStatus) const;
+
+virtual bool predictContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& predictedStatus) const;
+
+virtual bool initializeContact
+ (const Transform& X_GS1, const SpatialVec& V_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& contactStatus) const;
+
+private:
+void processBox(const ContactGeometry::TriangleMesh& mesh,
+ const ContactGeometry::TriangleMesh::OBBTreeNode& node,
+ const Transform& X_HM, const UnitVec3& hsNormal_M,
+ Real hsFaceHeight_M, std::set<int>& insideFaces) const;
+void addAllTriangles(const ContactGeometry::TriangleMesh::OBBTreeNode& node,
+ std::set<int>& insideFaces) const;
+};
+
+
+
+//==============================================================================
+// SPHERE - TRIANGLE MESH CONTACT TRACKER
+//==============================================================================
+/** This ContactTracker handles contacts between a ContactGeometry::Sphere
+and a ContactGeometry::TriangleMesh, in that order. **/
+class SimTK_SIMMATH_EXPORT ContactTracker::SphereTriangleMesh
+: public ContactTracker {
+public:
+SphereTriangleMesh()
+: ContactTracker(ContactGeometry::Sphere::classTypeId(),
+ ContactGeometry::TriangleMesh::classTypeId()) {}
+
+virtual ~SphereTriangleMesh() {}
+
+virtual bool trackContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1,
+ const ContactGeometry& surface1, // the sphere
+ const Transform& X_GS2,
+ const ContactGeometry& surface2, // the mesh
+ Real cutoff,
+ Contact& currentStatus) const;
+
+virtual bool predictContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& predictedStatus) const;
+
+virtual bool initializeContact
+ (const Transform& X_GS1, const SpatialVec& V_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& contactStatus) const;
+
+private:
+void processBox
+ (const ContactGeometry::TriangleMesh& mesh,
+ const ContactGeometry::TriangleMesh::OBBTreeNode& node,
+ const Vec3& center_M, Real radius2,
+ std::set<int>& insideFaces) const ;
+};
+
+
+
+//==============================================================================
+// TRIANGLE MESH - TRIANGLE MESH CONTACT TRACKER
+//==============================================================================
+/** This ContactTracker handles contacts between two
+ContactGeometry::TriangleMesh surfaces. **/
+class SimTK_SIMMATH_EXPORT ContactTracker::TriangleMeshTriangleMesh
+: public ContactTracker {
+public:
+TriangleMeshTriangleMesh()
+: ContactTracker(ContactGeometry::TriangleMesh::classTypeId(),
+ ContactGeometry::TriangleMesh::classTypeId()) {}
+
+virtual ~TriangleMeshTriangleMesh() {}
+
+virtual bool trackContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1,
+ const ContactGeometry& surface1, // mesh1
+ const Transform& X_GS2,
+ const ContactGeometry& surface2, // mesh2
+ Real cutoff,
+ Contact& currentStatus) const;
+
+virtual bool predictContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& predictedStatus) const;
+
+virtual bool initializeContact
+ (const Transform& X_GS1, const SpatialVec& V_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& contactStatus) const;
+
+private:
+void findIntersectingFaces
+ (const ContactGeometry::TriangleMesh& mesh1,
+ const ContactGeometry::TriangleMesh& mesh2,
+ const ContactGeometry::TriangleMesh::OBBTreeNode& node1,
+ const ContactGeometry::TriangleMesh::OBBTreeNode& node2,
+ const OrientedBoundingBox& node2Bounds_M1,
+ const Transform& X_M1M2,
+ std::set<int>& insideFaces1,
+ std::set<int>& insideFaces2) const;
+void findBuriedFaces
+ (const ContactGeometry::TriangleMesh& mesh,
+ const ContactGeometry::TriangleMesh& otherMesh,
+ const Transform& X_OM,
+ std::set<int>& insideFaces) const;
+void tagFaces(const ContactGeometry::TriangleMesh& mesh,
+ Array_<int>& faceType,
+ std::set<int>& triangles,
+ int index,
+ int depth) const;
+};
+
+
+//==============================================================================
+// CONVEX IMPLICIT SURFACE PAIR CONTACT TRACKER
+//==============================================================================
+/** This ContactTracker handles contacts between two smooth, convex objects
+by using their implicit functions. Create one of these for each possible
+pair that you want handled this way. **/
+class SimTK_SIMMATH_EXPORT ContactTracker::ConvexImplicitPair
+: public ContactTracker {
+public:
+ConvexImplicitPair(ContactGeometryTypeId type1, ContactGeometryTypeId type2)
+: ContactTracker(type1, type2) {}
+
+virtual ~ConvexImplicitPair() {}
+
+virtual bool trackContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Contact& currentStatus) const;
+
+virtual bool predictContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& predictedStatus) const;
+
+virtual bool initializeContact
+ (const Transform& X_GS1, const SpatialVec& V_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& contactStatus) const;
+};
+
+
+//==============================================================================
+// GENERAL IMPLICIT SURFACE PAIR CONTACT TRACKER
+//==============================================================================
+/** (TODO: not implemented yet) This ContactTracker handles contacts between
+two arbitrary smooth surfaces
+by using their implicit functions, with no shape restrictions. Each surface
+must provide a bounding hierarchy with "safe" leaf objects, meaning that
+interactions between a leaf of each surface yield at most one solution.
+
+Create one of these for each possible pair that you want handled this way. **/
+class SimTK_SIMMATH_EXPORT ContactTracker::GeneralImplicitPair
+: public ContactTracker {
+public:
+GeneralImplicitPair(ContactGeometryTypeId type1, ContactGeometryTypeId type2)
+: ContactTracker(type1, type2) {}
+
+virtual ~GeneralImplicitPair() {}
+
+virtual bool trackContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Contact& currentStatus) const;
+
+virtual bool predictContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& predictedStatus) const;
+
+virtual bool initializeContact
+ (const Transform& X_GS1, const SpatialVec& V_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& contactStatus) const;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_CONTACT_TRACKER_SUBSYSTEM_H_
diff --git a/SimTKmath/Geometry/include/simmath/internal/GCVSPLUtil.h b/SimTKmath/Geometry/include/simmath/internal/GCVSPLUtil.h
new file mode 100644
index 0000000..bdb0342
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/GCVSPLUtil.h
@@ -0,0 +1,132 @@
+#ifndef SimTK_SIMMATH_GCVSPL_UTIL_H_
+#define SimTK_SIMMATH_GCVSPL_UTIL_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+
+// These are global functions.
+int SimTK_gcvspl_(const SimTK::Real *, const SimTK::Real *, int *, const SimTK::Real *, const SimTK::Real *, int *, int *,
+ int *, int *, SimTK::Real *, SimTK::Real *, int *, SimTK::Real *, int *);
+SimTK::Real SimTK_splder_(int *, int *, int *, SimTK::Real *, const SimTK::Real *, const SimTK::Real *, int *, SimTK::Real *, int);
+
+namespace SimTK {
+
+/**
+ * This class provides entry points for using the GCVSPL algorithm in terms of SimTK data types.
+ * In most cases, you should use the SplineFitter class rather than invoking this class directly.
+ * For details, see Woltring, H.J. (1986), A FORTRAN package for generalized, cross-validatory
+ * spline smoothing and differentiation. Advances in Engineering Software 8(2):104-113.
+ */
+
+class SimTK_SIMMATH_EXPORT GCVSPLUtil {
+public:
+ static void gcvspl(const Vector& x, const Vector& y, const Vector& wx, Real wy, int m, int md, Real val, Vector& c, Vector& wk, int& ier);
+ template <int K>
+ static void gcvspl(const Vector& x, const Vector_<Vec<K> >& y, const Vector& wx, Vec<K> wy, int m, int md, Real val, Vector_<Vec<K> >& c, Vector& wk, int& ier);
+ static Real splder(int derivOrder, int degree, Real t, const Vector& x, const Vector& coeff);
+ template <int K>
+ static Vec<K> splder(int derivOrder, int degree, Real t, const Vector& x, const Vector_<Vec<K> >& coeff);
+};
+
+template <int K>
+void GCVSPLUtil::gcvspl(const Vector& x, const Vector_<Vec<K> >& y, const Vector& wx, Vec<K> wy, int degree, int md, Real val, Vector_<Vec<K> >& c, Vector& wk, int& ier) {
+ SimTK_APIARGCHECK_ALWAYS(degree > 0 && degree%2==1, "GCVSPLUtil", "gcvspl", "degree must be positive and odd");
+ SimTK_APIARGCHECK_ALWAYS(y.size() >= x.size(), "GCVSPLUtil", "gcvspl", "y is shorter than x");
+ SimTK_APIARGCHECK_ALWAYS(wx.size() >= x.size(), "GCVSPLUtil", "gcvspl", "wx and x must be the same size");
+ SimTK_APIARGCHECK_ALWAYS(x.hasContiguousData(), "GCVSPLUtil", "gcvspl", "x must have contiguous storage (i.e. not be a view)");
+ SimTK_APIARGCHECK_ALWAYS(wk.hasContiguousData(), "GCVSPLUtil", "gcvspl", "wk must have contiguous storage (i.e. not be a view)");
+
+ // Create various temporary variables.
+
+ int m = (degree+1)/2;
+ int n = x.size();
+ int ny = y.size();
+ Vector yvec(ny*K);
+ int index = 0;
+ for (int j = 0; j < K; ++j)
+ for (int i = 0; i < ny; ++i)
+ yvec[index++] = y[i][j];
+ int nc = n*K;
+ Vector cvec(nc);
+ wk.resize(6*(m*n+1)+n);
+ int k = K;
+
+ // Invoke GCV.
+
+ SimTK_gcvspl_(&x[0], &yvec[0], &ny, &wx[0], &wy[0], &m, &n, &k, &md, &val, &cvec[0], &n, &wk[0], &ier);
+ if (ier != 0) {
+ SimTK_APIARGCHECK_ALWAYS(n >= 2*m, "GCVSPLUtil", "gcvspl", "Too few data points");
+ SimTK_APIARGCHECK_ALWAYS(ier != 2, "GCVSPLUtil", "gcvspl", "The values in x must be strictly increasing");
+ SimTK_APIARGCHECK_ALWAYS(ier == 0, "GCVSPLUtil", "gcvspl", "GCVSPL returned an error code");
+ }
+ c.resize(n);
+ index = 0;
+ for (int j = 0; j < K; ++j)
+ for (int i = 0; i < n; ++i)
+ c[i][j] = cvec[index++];
+}
+
+template <int K>
+Vec<K> GCVSPLUtil::splder(int derivOrder, int degree, Real t, const Vector& x, const Vector_<Vec<K> >& coeff) {
+ assert(derivOrder >= 0);
+ assert(t >= x[0] && t <= x[x.size()-1]);
+ assert(x.size() == coeff.size());
+ assert(degree > 0 && degree%2==1);
+ assert(x.hasContiguousData());
+
+ // Create various temporary variables.
+
+
+ Vec<K> result;
+ int m = (degree+1)/2;
+ int n = x.size();
+ int interval = (int) ceil(n*(t-x[0])/(x[n-1]-x[0]));
+
+ const int MaxCheapM = 32;
+ Real qbuf[2*MaxCheapM];
+
+ Real *q = qbuf; // tentatively
+ if (m > MaxCheapM)
+ q = new Real[2*m]; // use heap instead; don't forget to delete
+
+ int offset = (int) (&coeff[1][0]-&coeff[0][0]);
+
+ // Evaluate the spline one component at a time.
+
+ for (int i = 0; i < K; ++i)
+ result[i] = SimTK_splder_(&derivOrder, &m, &n, &t, &x[0], &coeff[0][i], &interval, q, offset);
+
+ if (m > MaxCheapM)
+ delete[] q;
+
+ return result;
+}
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_GCVSPL_UTIL_H_
+
+
diff --git a/SimTKmath/Geometry/include/simmath/internal/Geo.h b/SimTKmath/Geometry/include/simmath/internal/Geo.h
new file mode 100644
index 0000000..87d04e7
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/Geo.h
@@ -0,0 +1,419 @@
+#ifndef SimTK_SIMMATH_GEO_H_
+#define SimTK_SIMMATH_GEO_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Defines geometric primitive shapes and algorthms. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+
+#include <cassert>
+#include <cmath>
+#include <algorithm>
+
+namespace SimTK {
+
+//==============================================================================
+// GEO
+//==============================================================================
+/** The Geo class collects geometric primitives intended to deal with raw,
+fixed-size geometric shapes occupying minimal memory and providing
+maximum performance through small inline methods and larger high performance
+algorithms. Subclasses collect algorithms relevant to particular shapes.
+There are no virtual methods or class hierarchies here; each subclass is a
+"POD" (plain old data) class. The general idea is to make it so that these
+common methods are implemented in only one place in Simbody.
+
+The Geo class itself is dataless and provides only static methods. It is also
+used as a namespace for geometric primitives to allow these names to be used
+elsewhere for more significant objects. **/
+class SimTK_SIMMATH_EXPORT Geo {
+public:
+template <class P> class Point_;
+template <class P> class Sphere_;
+template <class P> class LineSeg_;
+template <class P> class Line_;
+template <class P> class Plane_;
+template <class P> class Circle_;
+template <class P> class Box_;
+template <class P> class AlignedBox_;
+template <class P> class OrientedBox_;
+template <class P> class Triangle_;
+template <class P> class CubicHermiteCurve_;
+template <class P> class BicubicHermitePatch_;
+template <class P> class CubicBezierCurve_;
+template <class P> class BicubicBezierPatch_;
+
+typedef Point_<Real> Point;
+typedef Sphere_<Real> Sphere;
+typedef LineSeg_<Real> LineSeg;
+typedef Line_<Real> Line;
+typedef Plane_<Real> Plane;
+typedef Circle_<Real> Circle;
+typedef Box_<Real> Box;
+typedef AlignedBox_<Real> AlignedBox;
+typedef OrientedBox_<Real> OrientedBox;
+typedef Triangle_<Real> Triangle;
+typedef CubicHermiteCurve_<Real> CubicHermiteCurve;
+typedef BicubicHermitePatch_<Real> BicubicHermitePatch;
+typedef CubicBezierCurve_<Real> CubicBezierCurve;
+typedef BicubicBezierPatch_<Real> BicubicBezierPatch;
+
+/**@name Differential geometry of curve segments
+
+These methods calculate geometric quantities from given parametric ones, in an
+arbitrary parameter u, such that points on the curve segment are given by
+P(u), with 0<=u<=1. We consider the direction of increasing arc length to be
+the same as the direction of u. These are utility methods that can be used
+with any parametric curve. The idea is that you use the curve evaluators
+to determine the arguments here, using the same u value for each of them. If
+you don't use the same u value you'll get meaningless results.
+
+The primary reference for this material is the book "Lectures on Classical
+Differential Geometry, 2nd ed." (chapter 1) by Dirk Struik, 1961, republished
+by Dover in 1988. Notation and equation numbers are from that reference.
+Two exceptions: (1) we use c for the curvature vector rather than Struik's bold
+k, so we can use k for the scalar value of curvature, and (2) we define the
+curve normal n to point \e away (outward) from the center of curvature, while
+Struik defined it to point inward. Using our definition, if you have a
+parametric circle the normal points towards the outside, which is more
+conventional and analogous to surface normals. Our right-handed curve frame
+is thus x,y,z=n,t,b rather than Struik's "moving trihedron" frame t,n,b. For
+us the binormal b=n X t, while with Struik's definition it is t X n. Note that
+only the normal is reversed from Struik's, the tangent and binormal vectors
+are the same. **/
+/**@{**/
+
+/** Given the parametric derivative Pu(u)=dP/du, determine whether the point
+P(u) is at a cusp, that is, a place where the arc length s does not change when
+parameter u does, so ds/du=0 (within tolerance). Note that an inflection point,
+where the curvature is zero, is not a cusp; see isInflectionPoint(). Cost is 6
+flops. **/
+template <class RealP, int S> static bool
+isCusp(const Vec<3,RealP,S>& Pu)
+{ return Pu.normSqr() < getDefaultTolSqr<RealP>(); }
+
+/** Given the parametric derivatives Pu(u)=dP/du, and Puu(u)=d2P/du2 determine
+whether point P(u) is at an inflection point on the curve, that is, a flat
+place where there is no curvature (within tolerance). We will also return true
+if this point is a cusp, where curvature is not defined. Cost is 15 flops.
+
+<h3>Theory</h3>
+If Puu is zero or parallel to Pu (meaning it changes the parametric tangent's
+length but not its direction) then we are at an inflection point. We consider
+a cusp to be an inflection point even though curvature is undefined there,
+so we define an inflection point to be any place where |Pu X Puu|==0 to
+within tolerance.
+
+ at see isCusp() **/
+template <class RealP, int S> static bool
+isInflectionPoint(const Vec<3,RealP,S>& Pu, const Vec<3,RealP,S>& Puu)
+{ return (Pu % Puu).normSqr() < getDefaultTolSqr<RealP>(); }
+
+/** Calculate the unit tangent vector t=dP/ds, given Pu=dP/du. This is
+undefined at a cusp (Pu==0). See Struik, eq. 2-2.
+Cost is about 40 flops. **/
+template <class RealP, int S> static UnitVec<RealP,1>
+calcUnitTangent(const Vec<3,RealP,S>& Pu) {
+ const RealP dsdu = Pu.norm(); // ~25 flops
+ SimTK_ERRCHK(dsdu >= getDefaultTol<RealP>(),
+ "Geo::calcUnitTangent()", "Unit tangent undefined at a cusp.");
+
+ return UnitVec<RealP,1>(Pu/dsdu, true); // ~13 flops
+}
+
+/** Return the curvature vector c=dt/ds=d2P/ds2, given Pu=dP/du and
+Puu=d2P/du2. This vector points \e towards the center of curvature, with
+length equal to the magnitude of the curvature (it's not a unit vector).
+Curvature is undefined at a cusp (where Pu==0).
+Since we define the curve unit normal n to point \e away
+from the center of curvature (see above), we have c=-k*n where k is the
+(scalar) curvature. Cost is about 30 flops.
+
+<h3>Theory</h3>
+See Struik, eqn. 4-3, 4-4. Let prime denote differentiation with respect to
+arclength: <pre>
+ u' = du/ds = 1/|Pu|
+ u'' = -(~Pu Puu)/Pu^4 = -(~Pu Puu) * u'^4
+ t = P' = Pu u' = Pu/|Pu|
+ c = t' = P'' = Puu u'^2 + Pu u''
+ = Puu/Pu^2 - Pu (~Pu Puu)/Pu^4
+ = -k * n, k is signed curvature, n is unit normal
+</pre>
+Note that c can be used to determine the magnitude |k|, but you can't get
+the sign until the normal n has been defined. We're going to define n from c,
+so that n=-c/|c| so dot(c,n) is always negative meaning that k is always
+positive for us. **/
+template <class RealP, int S> static Vec<3,RealP>
+calcCurvatureVector(const Vec<3,RealP,S>& Pu, const Vec<3,RealP,S>& Puu) {
+ const RealP Pu2 = Pu.normSqr(); // (ds/du)^2, 5 flops
+ SimTK_ERRCHK(Pu2 >= getDefaultTolSqr<RealP>(),
+ "Geo::calcCurvatureVector()", "Curvature undefined at a cusp.");
+ const RealP PuPuu = dot(Pu,Puu);
+ const RealP uPrimeSqr = 1/Pu2; // ~10 flops
+ const RealP u2Prime = -PuPuu * square(uPrimeSqr); // 8 flops
+ return uPrimeSqr*Puu + u2Prime*Pu; // 9 flops
+}
+
+/** In our definition, the unit normal vector n points in the "outward"
+direction, that is, it points away from the center of curvature (opposite
+the curvature vector c so n=-c/|c|). This convention is the opposite of
+Struik's, where he has the normal point in the same direction
+as the curvature vector. The normal is undefined at a cusp (Pu(u)==0), and is
+an arbitrary perpendicular to the tangent at an inflection point (a flat
+place where there is no curvature, i.e. |Pu(u) X Puu(u)|==0). If the curve is
+a straight line then every point is an inflection point, so the normal is
+arbitrary everywhere. Cost is about 80 flops.
+ at see calcCurvatureVector() for theory. **/
+template <class RealP, int S> static UnitVec<RealP,1>
+calcUnitNormal(const Vec<3,RealP,S>& Pu, const Vec<3,RealP,S>& Puu) {
+ typedef UnitVec<RealP,1> UnitVec3P;
+
+ const RealP Pu2 = Pu.normSqr(); // (ds/du)^2, 5 flops
+ SimTK_ERRCHK(Pu2 >= getDefaultTolSqr<RealP>(),
+ "Geo::calcUnitNormal()", "The normal is undefined at a cusp.");
+
+ // Now check if we're at an inflection point, meaning |Pu X Puu|==0.
+ // Use this handy identity from Struik eq. 3-9 to avoid calculating
+ // the cross product: (Pu X Puu)^2 = Pu^2Puu^2 - (~Pu*Puu)^2
+ const RealP Puu2 = Puu.normSqr(); // 5 flops
+ const RealP PuPuu = dot(Pu,Puu); // 5 flops
+ const RealP PuXPuu2 = Pu2*Puu2 - square(PuPuu); // 3 flops
+ if (PuXPuu2 < getDefaultTolSqr<RealP>()) // 1 flop
+ return UnitVec3P(Pu).perp();
+ // Calculate the curvature vector, negate, and normalize.
+ const RealP uPrimeSqr = 1/Pu2; // ~10 flops
+ const RealP u2Prime = -PuPuu * square(uPrimeSqr); // 3 flops
+ const Vec<3,RealP> c = uPrimeSqr*Puu + u2Prime*Pu; // 9 flops
+ return UnitVec3P(-c); // ~40 flops
+}
+
+/** Return the the curvature k (always positive), and a frame
+whose origin is a point along the curve, x axis is the outward unit normal n,
+y is the unit tangent t, and z=x X y is the binormal b, which is a normal
+to the osculating plane. So the vectors n,t,b form a right-handed set; this
+convention is different from Struik's since he has n pointing the opposite
+direction. This frame is undefined at a cusp (Pu==0), and the normal is
+arbitrary at an inflection point (|Pu(u) X Puu(u)|==0) or if the
+curve is a line. Cost is about 115 flops.
+**/
+template <class RealP, int S> static RealP
+calcCurveFrame(const Vec<3,RealP,S>& P,
+ const Vec<3,RealP,S>& Pu,
+ const Vec<3,RealP,S>& Puu,
+ Transform_<RealP>& X_FP) {
+ typedef UnitVec<RealP,1> UnitVec3P;
+
+ const RealP Pu2 = Pu.normSqr(); // (ds/du)^2, 5 flops
+ SimTK_ERRCHK(Pu2 >= getDefaultTolSqr<RealP>(),
+ "Geo::calcCurveFrame()", "Curve frame is undefined at a cusp.");
+
+ // Set the point P(u) as the frame origin.
+ X_FP.updP() = P;
+
+ // Calculate the unit tangent t, our y axis.
+ const RealP uPrimeSqr = 1/Pu2; // ~10 flops
+ const RealP uPrime = std::sqrt(uPrimeSqr); // ~20 flops
+ const UnitVec3P t(uPrime*Pu, true); // 3 flops
+
+ // Next calculate unit normal n, our x axis. See calcUnitNormal() above
+ // for theory.
+ const RealP Puu2 = Puu.normSqr(); // 5 flops
+ const RealP PuPuu = dot(Pu,Puu); // 5 flops
+ const RealP PuXPuu2 = Pu2*Puu2 - square(PuPuu); // 3 flops
+ UnitVec3P n; // unit normal
+ RealP k; // curvature magnitude
+ if (PuXPuu2 < getDefaultTolSqr<RealP>()) { // 1 flop
+ k = 0;
+ n = t.perp(); // arbitrary
+ } else {
+ // Calculate the curvature vector, negate, and normalize.
+ const RealP u2Prime = -PuPuu * square(uPrimeSqr); // 8 flops
+ const Vec<3,RealP> c = uPrimeSqr*Puu + u2Prime*Pu; // 9 flops
+ k = c.norm(); // curvature >= 0, ~25 flops
+ n = UnitVec3P((-1/k)*c, true); // ~13 flops
+ }
+
+ // Finally calculate the binormal, our z axis. No need to normalize
+ // here because n and t are perpendicular unit vectors.
+ const UnitVec3P b(n % t, true); // 9 flops
+
+ // Construct the coordinate frame without normalizing.
+ X_FP.updR().setRotationFromUnitVecsTrustMe(n,t,b);
+
+ return k;
+}
+
+/** Return k^2, the square of the scalar curvature k, given Pu=dP/du and
+Puu=d2P/du2. Using our definition for the curve normal n (see above), k is
+always positive so the curvature is the positive square root of the value
+returned here. Curvature is undefined at a cusp (where Pu==0) and is zero
+at an inflection point (|Pu X Puu|==0). Cost is about 30 flops.
+
+<h3>Theory</h3>
+<pre>
+ |Pu X Puu|^2
+ k^2 = |c|^2 = ------------
+ |Pu|^6
+</pre> See Struik, pg. 17, eq. 5-5a. **/
+template <class RealP, int S> static RealP
+calcCurvatureSqr(const Vec<3,RealP,S>& Pu, const Vec<3,RealP,S>& Puu) {
+ const RealP Pu2 = Pu.normSqr(); // (ds/du)^2, 5 flops
+ SimTK_ERRCHK(Pu2 >= getDefaultTolSqr<RealP>(),
+ "Geo::calcCurvatureSqr()", "Curvature is undefined at a cusp.");
+ const RealP num = cross(Pu,Puu).normSqr(); // 14 flops
+ const RealP den = cube(Pu2); // 2 flops
+ return num/den; // ~10 flops
+}
+
+/** Return tau, the torsion or "second curvature" given Pu=dP/du,
+Puu=d2P/du2, Puuu=d3P/du3. Torsion is a signed quantity related to the
+rate of change of the osculating plane binormal b, with db/ds=tau*n where
+n is the "outward" unit normal (see above). Torsion is undefined at either a
+cusp (where Pu==0) or an inflection point (where |Pu X Puu|==0). Cost is about
+30 flops.
+
+<h3>Theory</h3>
+<pre>
+ ~(Pu X Puu) * Puuu
+ tau = ------------------
+ |Pu X Puu|^2
+</pre> See Struik, pg. 17, eq. 5-5b and discussion on page 16. **/
+template <class RealP, int S> static RealP
+calcTorsion(const Vec<3,RealP,S>& Pu, const Vec<3,RealP,S>& Puu,
+ const Vec<3,RealP,S>& Puuu) {
+ const Vec<3,RealP> PuXPuu = cross(Pu,Puu); // 9 flops
+ const RealP PuXPuu2 = PuXPuu.normSqr(); // 5 flops
+ SimTK_ERRCHK(PuXPuu2 >= getDefaultTolSqr<RealP>(), "Geo::calcTorsion()",
+ "Torsion is undefined at a cusp or inflection point.");
+ const RealP num = dot(PuXPuu, Puuu); // 5 flops
+ return num/PuXPuu2; // ~10 flops
+}
+/**@}**/
+
+/**@name Lines **/
+
+/** Find the points of closest approach on two lines L0 and L1, each
+represented by an origin point and a direction. Points and vectors must be in
+a common frame. We return point x0 on L0 and x1 on L1 such that the distance
+|x1-x0| is the smallest for any points on the two lines. If the lines are
+parallel or nearly so (all points same distance) we'll pick the point
+on each line closest to midway between the origins as the closest points and
+return and indication that the returned points weren't unique.
+
+ at param[in] p0 The origin point of line L0, that is, any point
+ through which line L0 passes.
+ at param[in] d0 A unit vector giving the direction of L0.
+ at param[in] p1 The origin point of line L1.
+ at param[in] d1 A unit vector giving the direction of L1.
+ at param[out] x0 The point of L0 that is closest to L1.
+ at param[out] x1 The point of L1 that is closest to L2.
+ at param[out] linesAreParallel
+ True if the lines were treated as effectively parallel.
+
+Cost is about 65 flops. **/
+template <class RealP> static void
+findClosestPointsOfTwoLines
+ (const Vec<3,RealP>& p0, const UnitVec<RealP,1>& d0,
+ const Vec<3,RealP>& p1, const UnitVec<RealP,1>& d1,
+ Vec<3,RealP>& x0, Vec<3,RealP>& x1, bool& linesAreParallel)
+{
+ const Vec<3,RealP> w = p1-p0; // vector from p0 to p1; 3 flops
+ const RealP s2Theta = (d0 % d1).normSqr(); // sin^2(angle); 14 flops
+ const RealP d = dot(w,d0); // 5 flops
+ const RealP e = -dot(w,d1); // 6 flops
+
+ RealP t0, t1; // line parameters of closest points
+ if (s2Theta < square(NTraits<RealP>::getSignificant())) { // 3 flops
+ // Lines are parallel. Return a point on each line midway between
+ // the origin points.
+ linesAreParallel = true; // parallel
+ t0 = d/2; t1 = e/2; // 2 flops
+ } else {
+ linesAreParallel = false;
+ const RealP cTheta = dot(d0,d1); // cos(angle between lines); 5 flops
+ const RealP oos2Theta = 1/s2Theta; // about 10 flops
+ t0 = (e*cTheta + d) * oos2Theta; // 3 flops
+ t1 = (d*cTheta + e) * oos2Theta; // 3 flops
+ }
+
+ x0 = p0 + t0*d0; // 6 flops
+ x1 = p1 + t1*d1; // 6 flops
+}
+
+/**@name Miscellaneous utilities **/
+/**@{**/
+
+/** Return the default tolerance to use for degeneracy tests and other tests
+for "too small" or "near enough" that arise in dealing with geometry primitives.
+The value depends on the precision being used; we use the SimTK constant
+SignificantReal which is eps^(7/8) where eps is the resolution of the
+template argument RealP (which must be \c float or \c double) That
+makes this tolerance around 2e-14 in double precision and 9e-7 in float. **/
+template <class RealP> static RealP getDefaultTol()
+{ return NTraits<RealP>::getSignificant(); }
+/** Returns the square of the default tolerance.
+ at see getDefaultTol() **/
+template <class RealP> static RealP getDefaultTolSqr()
+{ return square(getDefaultTol<RealP>()); }
+
+/** Return machine precision for floating point calculations at precision
+RealP. **/
+template <class RealP> static RealP getEps()
+{ return NTraits<RealP>::getEps(); }
+/** Return a NaN (not a number) at precision RealP. **/
+template <class RealP> static RealP getNaN()
+{ return NTraits<RealP>::getNaN(); }
+/** Return Infinity at precision RealP.\ You can negate this for -Infinity. **/
+template <class RealP> static RealP getInfinity()
+{ return NTraits<RealP>::getInfinity(); }
+
+/** Stretch a dimension by a given tolerance amount. The result is the
+given \a length increased by at least an absolute amount \a tol, or by a
+relative amount length*tol if length > 1. Don't call this with \a tol less
+than machine precision or an exception will be thrown. Cost is 3 flops. **/
+template <class RealP> static RealP stretchBy(RealP length, RealP tol) {
+ SimTK_ERRCHK2(tol >= getEps<RealP>(), "Geo::stretchBy()",
+ "The supplied tolerance %g is too small; must be at least %g"
+ " for significance at this precision.",
+ (double)tol, (double)getEps<RealP>());
+
+ return length + std::max(length*tol, tol);
+}
+
+/** Stretch a dimension using the default tolerance for this precision as
+the tolerance in stretchBy(). Cost is 3 flops. **/
+template <class RealP> static RealP stretch(RealP length)
+{ return stretchBy(length, getDefaultTol<RealP>()); }
+
+/**@}**/
+
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_GEO_H_
diff --git a/SimTKmath/Geometry/include/simmath/internal/Geo_BicubicBezierPatch.h b/SimTKmath/Geometry/include/simmath/internal/Geo_BicubicBezierPatch.h
new file mode 100644
index 0000000..d102025
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/Geo_BicubicBezierPatch.h
@@ -0,0 +1,523 @@
+#ifndef SimTK_SIMMATH_GEO_BICUBIC_BEZIER_PATCH_H_
+#define SimTK_SIMMATH_GEO_BICUBIC_BEZIER_PATCH_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Provides primitive operations for a single bicubic Bezier patch using either
+single or double precision. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_CubicBezierCurve.h"
+
+#include <cassert>
+#include <cmath>
+#include <algorithm>
+
+namespace SimTK {
+
+
+//==============================================================================
+// GEO BICUBIC BEZIER PATCH
+//==============================================================================
+/** A primitive useful for computations involving a single bicubic Bezier
+patch. Note that a bicubic Bezier spline surface would not necessarily be
+composed of these, but could use the static methods here for patch
+computations.
+
+<h3>Theory</h3>
+The primary reference for this implementation is the book "Geometric Modeling,
+3rd ed." by Michael E. Mortenson, Industrial Press 2006, chapter 8. We follow
+Mortenson's notation here (with some name changes) and equation numbers are
+from the text. See CubicHermiteCurve_ and BicubicHermitePatch_ comments for
+introductory material; here we add the Bezier description to the algebraic and
+Hermite (geometric) forms described there.
+
+We use B for Bezier control points (rather than P), and H for Hermite
+coefficients (rather than B). We call the Bezier basis matrix Mb (same as
+Mortenson) but call the Hermite basis matrix Mh (rather than Mf).
+
+The 16 control points are laid out like this, matching Mortenson, with the
+Hermite coefficient matrix for comparison:
+<pre>
+ [ b11 b12 b13 b14 ] u=0 [ h00 h01 w00 w01 ] u=dp/du
+ B = [ b21 b22 b23 b24 ] . H = [ h10 h11 w10 w11 ] w=dp/dw
+ [ b31 b32 b33 b34 ] . [ u00 u01 t00 t01 ] t=d2p/dudw
+ [ b41 b42 b43 b44 ] u=1 [ u10 u11 t10 t11 ] ("twist")
+ w=0 . . w=1
+</pre>
+We store those in a 4x4 hypermatrix; subtract one from each control point
+index to get the matrix indices (sorry).
+
+To convert between Bezier and Hermite or algebraic forms, use
+<pre>
+ A = Mb B ~Mb (although Mb is symmetric so ~Mb=Mb)
+ H = (Mh^-1 Mb) B ~(Mh^-1 Mb)
+ B = (Mb^-1 Mh) H ~(Mb^-1 Mh)
+</pre>
+where the parenthesized (very simple) matrices are given explicitly in the
+Theory section for CubicBezierCurve_. The results are:
+<pre>
+ [ b11 b14 3(b12-b11) 3(b14-b13) ]
+H = [ b41 b44 3(b42-b41) 3(b44-b43) ]
+ [ 3(b21-b11) 3(b24-b14) 9(b22-b21-{b12-b11}) 9({b24-b14}+b13-b23) ]
+ [ 3(b41-b31) 3(b44-b34) 9({b42-b41}+b31-b32) 9({b44-b34}+b33-b43) ]
+</pre>
+The braces in the twist terms mark common subexpressions, so the
+conversion from Bezier to Hermite takes 3x28=84 flops (all entries are
+3-vectors).
+<pre>
+ [ h00 h00+w00/3 h01-w01/3 h01 ]
+B = [h00+u00/3 {h00+u00/3}+w00/3+t00/9 {h01+u01/3}-w01/3-t01/9 h01+u01/3 ]
+ [h10-u10/3 {h10-u10/3}+w10/3-t10/9 {h11-u11/3}-w11/3+t11/9 h11-u11/3 ]
+ [ h10 h10+w10/3 h11-w11/3 h11 ]
+</pre>
+Exploiting the common subexpressions in braces, conversion from Hermite to
+Bezier takes 3x32=96 flops. **/
+template <class P>
+class Geo::BicubicBezierPatch_ {
+typedef P RealP;
+typedef Vec<3,RealP> Vec3P;
+
+public:
+/** Construct an uninitialized patch; control points will be garbage. **/
+BicubicBezierPatch_() {}
+
+/** Construct a bicubic Bezier patch using the given control points B. See the
+class documentation for how this matrix is defined. If instead you
+have algebraic or Hermite coefficients, you can first convert them to Bezier
+control points with the calcBFromA() or calcHFromA() provided here. **/
+explicit BicubicBezierPatch_(const Mat<4,4,Vec3P>& controlPoints)
+: B(controlPoints) {}
+
+
+/** Evaluate a point P(u,w) on this patch given values for the
+parameters u and w in [0,1]. Values outside this range are permitted but do
+not lie on the patch. Cost is 123 flops. **/
+Vec3P evalP(RealP u, RealP w) const {return evalPUsingB(B,u,w);}
+
+/** Evaluate the tangents Pu=dP/du, Pw=dP/dw on this patch given values for the
+parameters u and w in [0,1]. Values outside this range are permitted but do not
+lie on the curve segment. Cost is 248 flops. **/
+void evalP1(RealP u, RealP w, Vec3P& Pu, Vec3P& Pw) const
+{ return evalP1UsingB(B,u,w,Pu,Pw); }
+
+/** Evaluate the second derivatives Puu=d2P/du2, Pww=d2P/dw2, and cross
+derivative Puw=Pwu=d2P/dudw on this patch given values for the parameters u
+and w in [0,1]. Values outside this range are permitted but do not lie on the
+curve segment. Cost is 363 flops. **/
+void evalP2(RealP u, RealP w, Vec3P& Puu, Vec3P& Puw, Vec3P& Pww) const
+{ evalP2UsingB(B,u,w,Puu,Puw,Pww); }
+
+/** Evaluate the third derivatives Puuu=d3P/du3, Pwww=d3P/dw3, and cross
+derivatives Puuw=Pwuu=Puwu=d3P/du2dw and Puww=Pwwu=Pwuw=d3P/dudw2 on this patch
+given values for the parameters u and w in [0,1]. Cost is 468 flops. All
+higher derivatives of a cubic patch are zero. **/
+void evalP3(RealP u, RealP w, Vec3P& Puuu, Vec3P& Puuw,
+ Vec3P& Puww, Vec3P& Pwww) const
+{ evalP3UsingB(B,u,w,Puuu,Puuw,Puww,Pwww); }
+
+/** Return a reference to the Bezier control points B that are
+stored in this object. See the documentation for this class to see how the
+returned matrix of control points is defined. **/
+const Mat<4,4,Vec3P>& getControlPoints() const {return B;}
+/** Return a writable reference to the Bezier control points B that are
+stored in this object. See the documentation for this class to see how the
+returned matrix of control points is defined. **/
+Mat<4,4,Vec3P>& updControlPoints() {return B;}
+/** Calculate the algebraic coefficients A from the stored Bezier control
+points. See the documentation for BicubicHermitePatch_ to see how the
+returned matrix of coefficients is defined. Cost is 240 flops. **/
+Mat<4,4,Vec3P> calcAlgebraicCoefficients() const {return calcAFromB(B);}
+/** Calculate the Hermite coefficients H from the stored
+Bezier control points. See the documentation for this class to see how the
+returned matrix of coefficients is defined. Cost is 84 flops. **/
+Mat<4,4,Vec3P> calcHermiteCoefficients() const {return calcHFromB(B);}
+
+/** Return the u=0 boundary curve as a Bezier curve segment. The control points
+are just the first row of the patch control points B so no computation is
+required. **/
+CubicBezierCurve_<P> getBoundaryCurveU0() const
+{ return CubicBezierCurve_<P>(B[0]); } // b11 b12 b13 b14
+/** Return the u=1 boundary curve as a Bezier curve segment. The control points
+are just the last row of the patch control points B so no computation is
+required. **/
+CubicBezierCurve_<P> getBoundaryCurveU1() const
+{ return CubicBezierCurve_<P>(B[3]); } // b41 b42 b43 b44
+/** Return the w=0 boundary curve as a Bezier curve segment. The control points
+are just the first column of the patch control points B so no computation is
+required. **/
+CubicBezierCurve_<P> getBoundaryCurveW0() const
+{ return CubicBezierCurve_<P>(B(0)); } // b11 b21 b31 b41
+/** Return the w=1 boundary curve as a Bezier curve segment. The control points
+are just the last column of the patch control points B so no computation is
+required. **/
+CubicBezierCurve_<P> getBoundaryCurveW1() const
+{ return CubicBezierCurve_<P>(B(3)); } // b14 b24 b34 b44
+
+/** Given a particular value u0 for patch coordinate u, create a cubic Bezier
+curve segment P(w)=P(u0,w) for the isoparametric curve along the patch at fixed
+u=u0. Cost is 93 flops. **/
+CubicBezierCurve_<P> calcIsoCurveU(RealP u0) const
+{ return calcIsoCurveU(B, u0); }
+/** Given a particular value w0 for patch coordinate w, create a cubic Bezier
+curve segment P(u)=P(u,w0) for the isoparametric curve along the patch at fixed
+w=w0. Cost is 93 flops. **/
+CubicBezierCurve_<P> calcIsoCurveW(RealP w0) const
+{ return calcIsoCurveW(B, w0); }
+
+/** Split this patch into two along the u direction, along an isoparametric
+curve of constant u=t such that 0 < t < 1. On return, \a patch0 coincides with
+the u=0..t subpatch, and \a patch1 coincides with the u=t..1 subpatch. Each of
+the new patches is reparameterized so that its u parameter goes from 0 to 1,
+and the w direction remains parameterized as before. This method is only
+allowed for tol <= t <= 1-tol where tol is the default tolerance for this
+precision.
+
+ at param[in] u The u parameter of splitting isoparametric curve
+ (tol <= u <= 1-tol).
+ at param[out] patch0 Subpatch covering domain [0..u,0..1] (contains 0,0 corner).
+ at param[out] patch1 Subpatch covering domain [u..1,0..1] (contains 1,1 corner).
+
+Cost is 180 flops, or 120 flops if u=1/2. **/
+void splitU(RealP u, BicubicBezierPatch_<P>& patch0,
+ BicubicBezierPatch_<P>& patch1) const {
+ const RealP tol = getDefaultTol<RealP>();
+ SimTK_ERRCHK1(tol <= u && u <= 1-tol, "Geo::BicubicBezierPatch::splitU()",
+ "Can't split patch at parameter u=%g; it is either out of range or"
+ " too close to an edge.", (double)u);
+ CubicBezierCurve_<P> l,h;
+ if (u==RealP(0.5)) // bisecting
+ for (int i=0; i<4; ++i) {
+ CubicBezierCurve_<P>(B(i)).bisect(l,h);
+ patch0.B(i) = l.getControlPoints();
+ patch1.B(i) = h.getControlPoints();
+ }
+ else
+ for (int i=0; i<4; ++i) {
+ CubicBezierCurve_<P>(B(i)).split(u,l,h);
+ patch0.B(i) = l.getControlPoints();
+ patch1.B(i) = h.getControlPoints();
+ }
+}
+
+/** Split this patch into two along the w direction, along an isoparametric
+curve of constant w=t such that 0 < t < 1. On return, \a patch0 coincides with
+the w=0..t subpatch, and \a patch1 coincides with the w=t..1 subpatch. Each of
+the new patches is reparameterized so that its w parameter goes from 0 to 1,
+and the u direction remains parameterized as before. This method is only
+allowed for tol <= t <= 1-tol where tol is the default tolerance for this
+precision.
+
+ at param[in] w The w parameter of splitting isoparametric curve
+ (tol <= w <= 1-tol).
+ at param[out] patch0 Subpatch covering domain [0..1,0..w] (contains 0,0 corner).
+ at param[out] patch1 Subpatch covering domain [0..1,w..1] (contains 1,1 corner).
+
+Cost is 180 flops, or 120 flops if w=1/2. **/
+void splitW(RealP w, BicubicBezierPatch_<P>& patch0,
+ BicubicBezierPatch_<P>& patch1) const {
+ const RealP tol = getDefaultTol<RealP>();
+ SimTK_ERRCHK1(tol <= w && w <= 1-tol, "Geo::BicubicBezierPatch::splitW()",
+ "Can't split patch at parameter w=%g; it is either out of range or"
+ " too close to an edge.", (double)w);
+ CubicBezierCurve_<P> l,h;
+ if (w==RealP(0.5)) // bisecting
+ for (int i=0; i<4; ++i) {
+ CubicBezierCurve_<P>(B[i]).bisect(l,h);
+ patch0.B[i] = l.getControlPoints().positionalTranspose();
+ patch1.B[i] = h.getControlPoints().positionalTranspose();
+ }
+ else
+ for (int i=0; i<4; ++i) {
+ CubicBezierCurve_<P>(B[i]).split(w,l,h);
+ patch0.B[i] = l.getControlPoints().positionalTranspose();
+ patch1.B[i] = h.getControlPoints().positionalTranspose();
+ }
+}
+
+
+/** Split this patch into four subpatches at a particular parametric point
+(u,w) such that 0 < u,w < 1, with each resulting subpatch covering part of the
+original parametric domain in each direction. The subpatches are reparametrized
+to have [0..1,0..1] domains. This method is only allowed for
+tol <= u,w <= 1-tol where tol is the default tolerance for this precision.
+
+ at param[in] u The u parameter of the splitting point (tol <= u <= 1-tol).
+ at param[in] w The w parameter of the splitting point (tol <= w <= 1-tol).
+ at param[out] patch00 Subpatch covering domain [0..u,0..w] (contains 0,0 corner).
+ at param[out] patch01 Subpatch covering domain [0..u,w..1] (contains 0,1 corner).
+ at param[out] patch10 Subpatch covering domain [u..1,0..w] (contains 1,0 corner).
+ at param[out] patch11 Subpatch covering domain [u..1,w..1] (contains 1,1 corner).
+
+Cost is 360, 420, or 540 flops depending on whether both, one, or neither
+parameter is 1/2. **/
+void split(RealP u, RealP w, BicubicBezierPatch_<P>& patch00,
+ BicubicBezierPatch_<P>& patch01,
+ BicubicBezierPatch_<P>& patch10,
+ BicubicBezierPatch_<P>& patch11) const {
+ const RealP tol = getDefaultTol<RealP>();
+ SimTK_ERRCHK2((tol <= u && u <= 1-tol) && (tol <= w && w <= 1-tol),
+ "Geo::BicubicBezierPatch::split()",
+ "Can't split patch at parametric point u,w=%g,%g; it is either out of"
+ " range or too close to an edge.", (double)u, (double)w);
+
+ BicubicBezierPatch_<P> patch0, patch1; // results of the first split
+
+ // We split once along one direction, and then twice along the other, so
+ // make sure the second direction is the cheap bisecting one.
+ if (u == Real(0.5)) {
+ splitW(w,patch0,patch1); // 120 or 180 flops
+ patch0.splitU(u, patch00, patch10); // 120 flops
+ patch1.splitU(u, patch01, patch11); // 120 flops
+ } else {
+ splitU(u,patch0,patch1); // 180 flops
+ patch0.splitW(w, patch00, patch01); // 120 or 180 flops
+ patch1.splitW(w, patch10, patch11); // 120 or 180 flops
+ }
+}
+
+/** Return a sphere that surrounds the entire patch in the 0<= u,w <=1
+range.
+
+ at bug Currently we use the fact that the patch is enclosed within the convex
+hull of its control points and generate the minimum bounding sphere that
+includes all control points; that is not necessarily the
+tightest-fitting sphere. **/
+Geo::Sphere_<P> calcBoundingSphere() const
+{ const ArrayViewConst_<Vec3P> points(&B(0,0), &B(0,0)+16); // no copy
+ return Geo::Point_<P>::calcBoundingSphere(points); }
+
+/** Return an axis-aligned bounding box (AABB) that surrounds the entire patch
+segment in the 0<= u,w <=1 range.
+
+ at bug Currently we use the fact that the patch is enclosed within the convex
+hull of its control points and generate the minimum bounding box that
+includes all control points; that is not necessarily the
+tightest-fitting box. **/
+Geo::AlignedBox_<P> calcAxisAlignedBoundingBox() const
+{ const ArrayViewConst_<Vec3P> points(&B(0,0), &B(0,0)+16); // no copy
+ return Geo::Point_<P>::calcAxisAlignedBoundingBox(points); }
+
+/** Return an oriented bounding box (OBB) that surrounds the entire curve
+segment in the 0<= u,w <=1 range.
+
+ at bug Currently we use the fact that the patch is enclosed within the convex
+hull of its control points and generate an oriented bounding box that
+includes all control points; that is not necessarily the
+tightest-fitting box. **/
+Geo::OrientedBox_<P> calcOrientedBoundingBox() const
+{ const ArrayViewConst_<Vec3P> points(&B(0,0), &B(0,0)+16); // no copy
+ return Geo::Point_<P>::calcOrientedBoundingBox(points); }
+
+/**@name Utility methods
+These static methods work with given control points. **/
+/**@{**/
+
+/** Given Bezier control points B and values for the curve parameters
+u and w in [0..1], return the point P(u,w)=Fb(u)*B*~Fb(w) at that location,
+where Fb is a vector of Bezier basis functions. Cost is
+3x35+18=123 flops. **/
+static Vec3P evalPUsingB(const Mat<4,4,Vec3P>& B, RealP u, RealP w) {
+ Row<4,P> Fbu = CubicBezierCurve_<P>::calcFb(u); // 9 flops
+ Row<4,P> Fbw = CubicBezierCurve_<P>::calcFb(w); // 9 flops
+ return Fbu * B * ~Fbw; // 3x35 flops
+}
+
+/** Given Bezier control points B and values for the curve parameters
+u and w in [0..1], return the tangents Pu(u,w)=dFb(u)*B*~Fb(w) and
+Pw(u,w)=Fb(u)*B*~dFb(w) at
+that location. Cost is 3x70+38=248 flops. **/
+static void evalP1UsingB(const Mat<4,4,Vec3P>& B, RealP u, RealP w,
+ Vec3P& Pu, Vec3P& Pw) {
+ Row<4,P> Fbu = CubicBezierCurve_<P>::calcFb(u); // 9 flops
+ Row<4,P> Fbw = CubicBezierCurve_<P>::calcFb(w); // 9 flops
+ Row<4,P> dFbu = CubicBezierCurve_<P>::calcDFb(u); // 10 flops
+ Row<4,P> dFbw = CubicBezierCurve_<P>::calcDFb(w); // 10 flops
+ Pu = dFbu * B * ~Fbw; // 3x35
+ Pw = Fbu * B * ~dFbw; // 3x35
+}
+
+/** Given Bezier control points B and values for the curve parameters
+u and w in [0..1], return the second derivatives Puu(u,w)=d2Fb(u)*B*~Fb(w),
+Puw(u,w)=dFb(u)*B*~dFb(w) and Pww(u,w)=Fb(u)*B*~d2Fb(w) at that location.
+Cost is 3x105+48=363 flops. **/
+static void evalP2UsingB(const Mat<4,4,Vec3P>& B, RealP u, RealP w,
+ Vec3P& Puu, Vec3P& Puw, Vec3P& Pww) {
+ Row<4,P> Fbu = CubicBezierCurve_<P>::calcFb(u); // 9 flops
+ Row<4,P> Fbw = CubicBezierCurve_<P>::calcFb(w); // 9 flops
+ Row<4,P> dFbu = CubicBezierCurve_<P>::calcDFb(u); // 10 flops
+ Row<4,P> dFbw = CubicBezierCurve_<P>::calcDFb(w); // 10 flops
+ Row<4,P> d2Fbu = CubicBezierCurve_<P>::calcD2Fb(u); // 5 flops
+ Row<4,P> d2Fbw = CubicBezierCurve_<P>::calcD2Fb(w); // 5 flops
+ Puu = d2Fbu * B * ~Fbw; // 3x35
+ Puw = dFbu * B * ~dFbw; // 3x35
+ Pww = Fbu * B * ~d2Fbw; // 3x35
+}
+
+/** Given Bezier control points B and values for the curve parameters
+u and w in [0..1], return the third derivatives Puuu(u,w)=d3Fb(u)*B*~Fb(w),
+Puuw(u,w)=d2Fb(u)*B*~dFb(w), Puww(u,w)=dFb(u)*B*~d2Fb(w) and
+Pwww(u,w)=Fb(u)*B*~d3Fb(w) at that location. Cost is 3x140+48=468 flops. **/
+static void evalP3UsingB(const Mat<4,4,Vec3P>& B, RealP u, RealP w,
+ Vec3P& Puuu, Vec3P& Puuw, Vec3P& Puww, Vec3P& Pwww) {
+ Row<4,P> Fbu = CubicBezierCurve_<P>::calcFb(u); // 9 flops
+ Row<4,P> Fbw = CubicBezierCurve_<P>::calcFb(w); // 9 flops
+ Row<4,P> dFbu = CubicBezierCurve_<P>::calcDFb(u); // 10 flops
+ Row<4,P> dFbw = CubicBezierCurve_<P>::calcDFb(w); // 10 flops
+ Row<4,P> d2Fbu = CubicBezierCurve_<P>::calcD2Fb(u); // 5 flops
+ Row<4,P> d2Fbw = CubicBezierCurve_<P>::calcD2Fb(w); // 5 flops
+ Row<4,P> d3Fbu = CubicBezierCurve_<P>::calcD3Fb(u); // 0
+ Row<4,P> d3Fbw = CubicBezierCurve_<P>::calcD3Fb(w); // 0
+ Puuu = d3Fbu * B * ~Fbw; // 3x35
+ Puuw = d2Fbu * B * ~dFbw; // 3x35
+ Puww = dFbu * B * ~d2Fbw; // 3x35
+ Pwww = Fbu * B * ~d3Fbw; // 3x35
+}
+
+/** Given a particular value u0 for patch coordinate u, create a cubic Bezier
+curve segment P(w)=P(u0,w) for the isoparametric curve along the patch at fixed
+u=u0. Cost is 3x28+9=93 flops. **/
+static CubicBezierCurve_<P>
+calcIsoCurveU(const Mat<4,4,Vec3P>& B, RealP u0)
+{ const Row<4,Vec3P> Bu0 = CubicBezierCurve_<P>::calcFb(u0) * B;
+ return CubicBezierCurve_<P>(Bu0); }
+
+/** Given a particular value w0 for patch coordinate w, create a cubic Bezier
+curve segment P(u)=P(u,w0) for the isoparametric curve along the patch at fixed
+w=w0. Cost is 3x28+9=93 flops. **/
+static CubicBezierCurve_<P>
+calcIsoCurveW(const Mat<4,4,Vec3P>& B, RealP w0)
+{ const Vec<4,Vec3P> Bw0 = B * ~CubicBezierCurve_<P>::calcFb(w0);
+ return CubicBezierCurve_<P>(Bw0); }
+
+/** Given the Bezier control points B, return the equivalent vector algebraic
+coefficients A. All coefficients are 3-vectors. Cost is 3x80=240 flops. **/
+static Mat<4,4,Vec3P> calcAFromB(const Mat<4,4,Vec3P>& B) {
+ typedef const Vec3P& Coef;
+ Coef b11=B(0,0), b12=B(0,1), b13=B(0,2), b14=B(0,3),
+ b21=B(1,0), b22=B(1,1), b23=B(1,2), b24=B(1,3),
+ b31=B(2,0), b32=B(2,1), b33=B(2,2), b34=B(2,3),
+ b41=B(3,0), b42=B(3,1), b43=B(3,2), b44=B(3,3);
+ // First calculate Mb*B:
+ // a b c d
+ // e f g h
+ // p q r s
+ // b11 b12 b13 b14
+ Vec3P a= b41-b11+3*(b21-b31), b= b42-b12+3*(b22-b32), // 3x16 flops
+ c= b43-b13+3*(b23-b33), d= b44-b14+3*(b24-b34);
+ Vec3P e= 3*(b11+b31)-6*b21, f= 3*(b12+b32)-6*b22, // 3x16 flops
+ g= 3*(b13+b33)-6*b23, h= 3*(b14+b34)-6*b24;
+ Vec3P p= 3*(b21-b11), q= 3*(b22-b12), // 3x8 flops
+ r= 3*(b23-b13), s= 3*(b24-b14);
+
+ // Then calculate (Mb*B)*~Mb. (3x40 more flops)
+ return Mat<4,4,Vec3P>
+ ( d-a+3*(b-c), 3*(a+c)-6*b, 3*(b-a), a,
+ h-e+3*(f-g), 3*(e+g)-6*f, 3*(f-e), e,
+ s-p+3*(q-r), 3*(p+r)-6*q, 3*(q-p), p,
+ b14-b11+3*(b12-b13), 3*(b11+b13)-6*b12, 3*(b12-b11), b11 );
+}
+
+/** Given the vector algebraic coefficients A, return the equivalent Bezier
+control points B. All coefficients are 3-vectors. Cost is 3x72=216 flops. **/
+static Mat<4,4,Vec3P> calcBFromA(const Mat<4,4,Vec3P>& A) {
+ typedef const Vec3P& Coef;
+ Coef a33=A(0,0), a32=A(0,1), a31=A(0,2), a30=A(0,3),
+ a23=A(1,0), a22=A(1,1), a21=A(1,2), a20=A(1,3),
+ a13=A(2,0), a12=A(2,1), a11=A(2,2), a10=A(2,3),
+ a03=A(3,0), a02=A(3,1), a01=A(3,2), a00=A(3,3);
+ // First calculate Mb^-1*A:
+ // a03 a02 a01 a00
+ // a b c d
+ // e f g h
+ // p q r s
+ Vec3P a=a13/3+a03, b=a12/3+a02, c=a11/3+a01, d=a10/3+a00; // 3x8 flops
+ Vec3P e=(a23+2*a13)/3+a03, f=(a22+2*a12)/3+a02, // 3x16 flops
+ g=(a21+2*a11)/3+a01, h=(a20+2*a10)/3+a00;
+ Vec3P p=a33+a23+a13+a03, q=a32+a22+a12+a02, // 3x12 flops
+ r=a31+a21+a11+a01, s=a30+a20+a10+a00;
+
+
+ // Then calculate (Mb^-1*A)*Mb^-T (3x36 more flops)
+ return Mat<4,4,Vec3P>
+ ( a00, a01/3+a00, (a02+2*a01)/3+a00, a03+a02+a01+a00,
+ d, c/3+d, (b+2*c)/3+d, a+b+c+d,
+ h, g/3+h, (f+2*g)/3+h, e+f+g+h,
+ s, r/3+s, (q+2*r) /3+s, p+q+r+s );
+}
+
+/** Given the Bezier control points B, return the equivalent vector Hermite
+coefficients H. All coefficients are 3-vectors. Cost is 3x28=84 flops. **/
+static Mat<4,4,Vec3P> calcHFromB(const Mat<4,4,Vec3P>& B) {
+ typedef const Vec3P& Coef;
+ Coef b11=B(0,0), b12=B(0,1), b13=B(0,2), b14=B(0,3),
+ b21=B(1,0), b22=B(1,1), b23=B(1,2), b24=B(1,3),
+ b31=B(2,0), b32=B(2,1), b33=B(2,2), b34=B(2,3),
+ b41=B(3,0), b42=B(3,1), b43=B(3,2), b44=B(3,3);
+
+ // First calculate a few temps -- see class comments. (3x4 flops)
+ Vec3P b12mb11=b12-b11, b24mb14=b24-b14, b42mb41=b42-b41, b44mb34=b44-b34;
+
+ // Then calculate (Mh^-1 Mb) B ~(Mh^-1 Mb). (3x24 more flops)
+ return Mat<4,4,Vec3P>
+ ( b11, b14, 3*b12mb11, 3*(b14-b13),
+ b41, b44, 3*b42mb41, 3*(b44-b43),
+ 3*(b21-b11), 3*b24mb14, 9*(b22-b21-b12mb11), 9*(b24mb14+b13-b23),
+ 3*(b41-b31), 3*b44mb34, 9*(b42mb41+b31-b32), 9*(b44mb34+b33-b43) );
+}
+
+/** Given the vector Hermite coefficients H, return the equivalent Bezier
+control points B. All coefficients are 3-vectors. Cost is 3x32=96 flops. **/
+static Mat<4,4,Vec3P> calcBFromH(const Mat<4,4,Vec3P>& H) {
+ typedef const Vec3P& Coef;
+ Coef h00=H(0,0), h01=H(0,1), w00=H(0,2), w01=H(0,3),
+ h10=H(1,0), h11=H(1,1), w10=H(1,2), w11=H(1,3),
+ u00=H(2,0), u01=H(2,1), t00=H(2,2), t01=H(2,3),
+ u10=H(3,0), u11=H(3,1), t10=H(3,2), t11=H(3,3);
+
+ // First calculate a few temps -- see class comments. (3x8 flops)
+ Vec3P tmp00=h00+u00/3, tmp01=h01+u01/3,
+ tmp10=h10-u10/3, tmp11=h11-u11/3;
+
+ // Then calculate (Mb^-1 Mh) H ~(Mb^-1 Mh). (3x24 more flops)
+ return Mat<4,4,Vec3P>
+ ( h00, h00+w00/3, h01-w01/3, h01,
+ tmp00, tmp00+w00/3+t00/9, tmp01-w01/3-t01/9, tmp01,
+ tmp10, tmp10+w10/3-t10/9, tmp11-w11/3+t11/9, tmp11,
+ h10, h10+w10/3, h11-w11/3, h11 );
+}
+/**@}**/
+
+private:
+Mat<4,4,Vec3P> B; // 16 Bezier control points; see above for definition
+};
+
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_GEO_BICUBIC_BEZIER_PATCH_H_
diff --git a/SimTKmath/Geometry/include/simmath/internal/Geo_BicubicHermitePatch.h b/SimTKmath/Geometry/include/simmath/internal/Geo_BicubicHermitePatch.h
new file mode 100644
index 0000000..3725017
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/Geo_BicubicHermitePatch.h
@@ -0,0 +1,288 @@
+#ifndef SimTK_SIMMATH_GEO_BICUBIC_HERMITE_PATCH_H_
+#define SimTK_SIMMATH_GEO_BICUBIC_HERMITE_PATCH_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Provides primitive operations for a single bicubic Hermite patch using either
+single or double precision. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+
+#include <cassert>
+#include <cmath>
+#include <algorithm>
+
+namespace SimTK {
+
+
+//==============================================================================
+// GEO BICUBIC HERMITE PATCH
+//==============================================================================
+/** A primitive useful for computations involving a single bicubic Hermite
+patch. Note that a bicubic Hermite spline surface would not necessarily be
+composed of these, but could use the static methods here for patch
+computations.
+
+<h3>Theory</h3>
+The primary reference for this implementation is the book "Geometric Modeling,
+3rd ed." by Michael E. Mortenson, Industrial Press 2006, chapter 7. We follow
+Mortenson's notation here (with some name changes) and equation numbers are
+from the text. See CubicHermiteCurve_ comments for
+introductory material; here we add the code needed for a bicubic Hermite
+surface, leaning on the cubic Hermite curve code whenever possible. Note that
+a bicubic surface is bounded by cubic curves and every cross section is a
+cubic curve. This class deals with a bicubic patch in algebraic or Hermite
+(geometric) form. The algebraic form is stored since most operations are faster
+when the algebraic coefficients are available. Methods are available to
+convert quickly between the algebraic and Hermite forms. See
+BicubicBezierPatch_ for additional code for conversions to/from Bezier form
+and additional operations that are better performed on Bezier control points.
+
+We use H for Hermite coefficients, rather than B, to avoid confusion with
+Bezier coefficients. We call the Hermite basis matrix Mh (rather than Mf).
+With that name change, the algebraic and Hermite basis matrices match
+Mortenson's:
+<pre>
+ [ a33 a32 a31 a30 ] [ h00 h01 w00 w01 ] u=dp/du
+ A = [ a23 a22 a21 a20 ] H = [ h10 h11 w10 w11 ] w=dp/dw
+ [ a13 a12 a11 a10 ] [ u00 u01 t00 t01 ] t=d2p/dudw
+ [ a03 a02 a01 a00 ] [ u10 u11 t10 t11 ] ("twist")
+</pre>
+We store those in a 4x4 hypermatrix with Vec3 elements. Note that the element
+indices do \e not match the matrix indices; instead they have different
+meanings as defined by Mortenson. For the algebraic coefficients we have
+aij = A[3-i][3-j].
+
+The patch is parameterized by u,w each in
+range [0..1]. A point P(u,w) on the patch can be calculated as
+<pre>
+ P(u,w) = U A ~W = sum_ij( aij u^i w^j )
+ = U Mh H ~Mh ~W
+</pre>
+where U and W are row vectors U=[u^3 u^2 u 1], W=[w^3 w^2 w 1]. From that you
+can see how to interconvert between the two forms:
+<pre>
+ A = Mh H ~Mh
+ H = Mh^-1 A ~Mh^-1
+</pre>
+The Mh matrix is presented explicitly in CubicHermiteCurve_; it has a very
+simple structure. This can be used to work out low cost interconversions:
+
+**/
+template <class P>
+class Geo::BicubicHermitePatch_ {
+typedef P RealP;
+typedef Vec<3,RealP> Vec3P;
+
+public:
+/** Construct an uninitialized patch; control points will be garbage. **/
+BicubicHermitePatch_() {}
+/** Construct a bicubic Hermite patch using the given geometry matrix B. **/
+explicit BicubicHermitePatch_(const Mat<4,4,Vec3P>& A) : A(A) {}
+/** Return a reference to the algebraic coefficients A that are
+stored in this object. See the documentation for this class to see how the
+returned matrix of coefficients is defined. **/
+const Mat<4,4,Vec3P>& getAlgebraicCoefficients() const {return A;}
+/** Calculate the Hermite coefficients H from the stored
+algebraic coefficients. See the documentation for this class to see how the
+returned matrix of coefficients is defined. Cost is 168 flops. **/
+Mat<4,4,Vec3P> calcHermiteCoefficients() const {return calcHFromA(A);}
+
+/** Evaluate a point P(u,w) on this patch given values for the
+parameters u and w in [0,1]. Values outside this range are permitted but do
+not lie on the patch. Cost is 94 flops. **/
+Vec3P evalP(RealP u, RealP w) const {return evalPUsingA(A,u,w);}
+
+/** Evaluate the tangents Pu=dP/du, Pw=dP/dw on this patch given values for the
+parameters u and w in [0,1]. Values outside this range are permitted but do not
+lie on the curve segment. Cost is 148 flops. **/
+void evalP1(RealP u, RealP w, Vec3P& Pu, Vec3P& Pw) const
+{ return evalP1UsingA(A,u,w,Pu,Pw); }
+
+/** Evaluate the second derivatives Puu=d2P/du2, Pww=d2P/dw2, and cross
+derivative Puw=Pwu=d2P/dudw on this patch given values for the parameters u
+and w in [0,1]. Values outside this range are permitted but do not lie on the
+curve segment. Cost is 172 flops. **/
+void evalP2(RealP u, RealP w, Vec3P& Puu, Vec3P& Puw, Vec3P& Pww) const
+{ evalP2UsingA(A,u,w,Puu,Puw,Pww); }
+
+/** Evaluate the third derivatives Puuu=d3P/du3, Pwww=d3P/dw3, and cross
+derivatives Puuw=Pwuu=Puwu=d3P/du2dw and Puww=Pwwu=Pwuw=d3P/dudw2 on this patch
+given values for the parameters u and w in [0,1]. Cost is 142 flops. All
+higher derivatives of a cubic patch are zero. **/
+void evalP3(RealP u, RealP w, Vec3P& Puuu, Vec3P& Puuw,
+ Vec3P& Puww, Vec3P& Pwww) const
+{ evalP3UsingA(A,u,w,Puuu,Puuw,Puww,Pwww); }
+
+/**@name Utility methods
+These static methods work with given coefficients. **/
+/**@{**/
+
+/** Given the vector Hermite coefficients H, return the algebraic
+coefficients A. All coefficients are 3-vectors. Cost is 3x70=210 flops. **/
+static Mat<4,4,Vec3P> calcAFromH(const Mat<4,4,Vec3P>& H) {
+ typedef const Vec3P& Coef;
+ Coef h00=H(0,0), h01=H(0,1), w00=H(0,2), w01=H(0,3),
+ h10=H(1,0), h11=H(1,1), w10=H(1,2), w11=H(1,3),
+ u00=H(2,0), u01=H(2,1), t00=H(2,2), t01=H(2,3),
+ u10=H(3,0), u11=H(3,1), t10=H(3,2), t11=H(3,3);
+ // First calculate Mh*H:
+ // a b c d
+ // p q r s
+ // u00 u01 t00 t01
+ // h00 h01 w00 w01
+ Vec3P a=2*(h00-h10)+u00+u10, b=2*(h01-h11)+u01+u11,
+ c=2*(w00-w10)+t00+t10, d=2*(w01-w11)+t01+t11;
+ Vec3P p=3*(h10-h00)-2*u00-u10, q=3*(h11-h01)-2*u01-u11,
+ r=3*(w10-w00)-2*t00-t10, s=3*(w11-w01)-2*t01-t11;
+
+ // Then calculate (Mh*H)*~Mh.
+ Vec3P bma=b-a, qmp=q-p;
+ return Mat<4,4,Vec3P>
+ ( c+d-2*bma, 3*bma-2*c-d, c, a,
+ r+s-2*qmp, 3*qmp-2*r-s, r, p,
+ 2*(u00-u01)+t00+t01, 3*(u01-u00)-2*t00-t01, t00, u00,
+ 2*(h00-h01)+w00+w01, 3*(h01-h00)-2*w00-w01, w00, h00 );
+}
+
+/** Given the vector algebraic coefficients A, return the Hermite coefficients
+H. All coefficients are 3-vectors. Cost is 3x56=168 flops. **/
+static Mat<4,4,Vec3P> calcHFromA(const Mat<4,4,Vec3P>& A) {
+ typedef const Vec3P& Coef;
+ Coef a33=A(0,0), a32=A(0,1), a31=A(0,2), a30=A(0,3),
+ a23=A(1,0), a22=A(1,1), a21=A(1,2), a20=A(1,3),
+ a13=A(2,0), a12=A(2,1), a11=A(2,2), a10=A(2,3),
+ a03=A(3,0), a02=A(3,1), a01=A(3,2), a00=A(3,3);
+ // First calculate Mh^-1*A:
+ // a03 a02 a01 a00
+ // a b c d
+ // a13 a12 a11 a10
+ // p q r s
+ Vec3P a=a33+a23+a13+a03, b=a32+a22+a12+a02, // 3x12 flops
+ c=a31+a21+a11+a01, d=a30+a20+a10+a00;
+ Vec3P p=3*a33+2*a23+a13, q=3*a32+2*a22+a12, // 3x16 flops
+ r=3*a31+2*a21+a11, s=3*a30+2*a20+a10;
+
+ // Then calculate (Mh^-1*A)*Mh^-T (3x28 flops)
+ return Mat<4,4,Vec3P>
+ ( a00, a03+a02+a01+a00, a01, 3*a03+2*a02+a01,
+ d, a+b+c+d, c, 3*a+2*b+c,
+ a10, a13+a12+a11+a10, a11, 3*a13+2*a12+a11,
+ s, p+q+r+s, r, 3*p+2*q+r );
+}
+
+/** Given vector algebraic coefficients A and values for the curve parameters
+u and w in [0..1], return the point P(u,w)=U*A*~W at that location. Cost is
+3x30+4=94 flops. **/
+static Vec3P evalPUsingA(const Mat<4,4,Vec3P>& A, RealP u, RealP w) {
+ typedef const Vec3P& Coef;
+ Coef a33=A(0,0), a32=A(0,1), a31=A(0,2), a30=A(0,3),
+ a23=A(1,0), a22=A(1,1), a21=A(1,2), a20=A(1,3),
+ a13=A(2,0), a12=A(2,1), a11=A(2,2), a10=A(2,3),
+ a03=A(3,0), a02=A(3,1), a01=A(3,2), a00=A(3,3);
+
+ const RealP u2 = u*u, u3 = u*u2, w2 = w*w, w3 = w*w2;
+ Vec3P p = u3*(a33*w3 + a32*w2 + a31*w + a30)
+ + u2*(a23*w3 + a22*w2 + a21*w + a20)
+ + u *(a13*w3 + a12*w2 + a11*w + a10)
+ + (a03*w3 + a02*w2 + a01*w + a00);
+ return p;
+}
+
+/** Given vector algebraic coefficients A and values for the curve parameters
+u and w in [0..1], return the tangents Pu(u,w)=dU*A*~W and Pw(u,w)=U*A*~dW at
+that location. Cost is 3x48+4=148 flops. **/
+static void evalP1UsingA(const Mat<4,4,Vec3P>& A, RealP u, RealP w,
+ Vec3P& Pu, Vec3P& Pw) {
+ typedef const Vec3P& Coef;
+ Coef a33=A(0,0), a32=A(0,1), a31=A(0,2), a30=A(0,3),
+ a23=A(1,0), a22=A(1,1), a21=A(1,2), a20=A(1,3),
+ a13=A(2,0), a12=A(2,1), a11=A(2,2), a10=A(2,3),
+ a03=A(3,0), a02=A(3,1), a01=A(3,2);
+
+ const RealP u2 = u*u, u3 = u*u2, w2 = w*w, w3 = w*w2;
+ Pu = 3*u2*(a33*w3 + a32*w2 + a31*w + a30)
+ + 2* u*(a23*w3 + a22*w2 + a21*w + a20)
+ + (a13*w3 + a12*w2 + a11*w + a10);
+ Pw = 3*w2*(u3*a33 + u2*a23 + u*a13 + a03)
+ + 2* w*(u3*a32 + u2*a22 + u*a12 + a02)
+ + (u3*a31 + u2*a21 + u*a11 + a01);
+}
+
+/** Given vector algebraic coefficients A and values for the curve parameters
+u and w in [0..1], return the second derivatives Puu(u,w)=d2U*A*~W,
+Puw(u,w)=dU*A*~dW and Pww(u,w)=U*A*~d2W at that location.
+Cost is 3x56+4=172 flops. **/
+static void evalP2UsingA(const Mat<4,4,Vec3P>& A, RealP u, RealP w,
+ Vec3P& Puu, Vec3P& Puw, Vec3P& Pww) {
+ typedef const Vec3P& Coef;
+ Coef a33=A(0,0), a32=A(0,1), a31=A(0,2), a30=A(0,3),
+ a23=A(1,0), a22=A(1,1), a21=A(1,2), a20=A(1,3),
+ a13=A(2,0), a12=A(2,1), a11=A(2,2),
+ a03=A(3,0), a02=A(3,1);
+
+ const RealP u2 = u*u, u3 = u*u2, w2 = w*w, w3 = w*w2;
+ Puu = 6*u*(a33*w3 + a32*w2 + a31*w + a30)
+ + 2 *(a23*w3 + a22*w2 + a21*w + a20);
+ Pww = 6*w*(u3*a33 + u2*a23 + u*a13 + a03)
+ + 2 *(u3*a32 + u2*a22 + u*a12 + a02);
+ Puw = 3*u2*(3*a33*w2 + 2*a32*w + a31)
+ + 2*u *(3*a23*w2 + 2*a22*w + a21)
+ + (3*a13*w2 + 2*a12*w + a11);
+}
+
+/** Given vector algebraic coefficients A and values for the curve parameters
+u and w in [0..1], return the third derivatives Puuu(u,w)=d3U*A*~W,
+Puuw(u,w)=d2U*A*~dW, Puww(u,w)=dU*A*~d2W and Pwww(u,w)=U*A*~d3W at that
+location. Cost is 3x46+4=142 flops. **/
+static void evalP3UsingA(const Mat<4,4,Vec3P>& A, RealP u, RealP w,
+ Vec3P& Puuu, Vec3P& Puuw, Vec3P& Puww, Vec3P& Pwww) {
+ typedef const Vec3P& Coef;
+ Coef a33=A(0,0), a32=A(0,1), a31=A(0,2), a30=A(0,3),
+ a23=A(1,0), a22=A(1,1), a21=A(1,2), a20=A(1,3),
+ a13=A(2,0), a12=A(2,1),
+ a03=A(3,0), a02=A(3,1);
+
+ const RealP u2 = u*u, u3 = u*u2, w2 = w*w, w3 = w*w2;
+ Puuu = 6*(a33*w3 + a32*w2 + a31*w + a30);
+ Pwww = 6*(u3*a33 + u2*a23 + u*a13 + a03);
+ Puuw = 6*u*(3*a33*w2 + 2*a32*w + a31)
+ + 6*a23*w2 + 4*a22*w + 2*a21;
+ Puww = 6*w*(3*u2*a33 + 2*u*a23 + a13)
+ + 6*u2*a32 + 4*u*a22 + 2*a12;
+}
+/**@}**/
+
+private:
+Mat<4,4,Vec3P> A; // algebraic coefficients
+};
+
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_GEO_BICUBIC_HERMITE_PATCH_H_
diff --git a/SimTKmath/Geometry/include/simmath/internal/Geo_Box.h b/SimTKmath/Geometry/include/simmath/internal/Geo_Box.h
new file mode 100644
index 0000000..7142511
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/Geo_Box.h
@@ -0,0 +1,375 @@
+#ifndef SimTK_SIMMATH_GEO_BOX_H_
+#define SimTK_SIMMATH_GEO_BOX_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Defines primitive operations involving 3d rectangular boxes. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+
+#include <cassert>
+#include <cmath>
+#include <algorithm>
+
+namespace SimTK {
+
+//==============================================================================
+// GEO BOX
+//==============================================================================
+/** A 3d rectangular box aligned with an unspecified frame F and centered at
+that frame's origin. The box has a local frame B, centered at the box center
+and oriented along the box edges, and B==F. We keep track of the relative
+edge lengths to facilitate short-to-long processing. **/
+template <class P>
+class Geo::Box_ {
+typedef P RealP;
+typedef Vec<3,P> Vec3P;
+typedef Mat<3,3,P> Mat33P;
+typedef Rotation_<P> RotationP;
+typedef Transform_<P> TransformP;
+
+public:
+/** Construct an uninitialized Box object; the dimensions will be garbage. **/
+Box_() {}
+/** Construct a Box with the given nonnegative half-dimensions. Cost is 4
+flops to sort the edges. **/
+Box_(const Vec3P& halfLengths) {setHalfLengths(halfLengths);}
+
+/** Change the half-dimensions of this box. Dimensions must be nonnegative.
+Cost is 4 flops to sort the edges. **/
+Box_& setHalfLengths(const Vec3P& halfLengths) {
+ SimTK_ERRCHK3(halfLengths >= 0, "Geo::Box_::setHalfLengths()",
+ "Half lengths must be nonnegative; got %g,%g,%g.",
+ (double)halfLengths[0],(double)halfLengths[1],(double)halfLengths[2]);
+ h = halfLengths;
+ sortEdges();
+ return *this;
+}
+
+/** Change the half-dimensions of this box by adding the given vector. The
+result must be nonnegative. Cost is 7 flops, including resorting the edges. **/
+Box_& addToHalfLengths(const Vec3P& incr) {
+ h += incr;
+ SimTK_ERRCHK3(h >= 0, "Geo::Box_::addToHalfLengths()",
+ "Half lengths must be nonnegative but were %g,%g,%g after change.",
+ (double)h[0],(double)h[1],(double)h[2]);
+ sortEdges();
+ return *this;
+}
+
+/** Return the half-lengths of this box as a Vec3 from the center to the
+first quadrant vertex. **/
+const Vec3P& getHalfLengths() const {return h;}
+
+/** Get lengths in order shortest to longest; 0 is shortest, 2 is longest. **/
+RealP getOrderedHalfLength(int i) const {
+ SimTK_INDEXCHECK(i, 3, "Geo::Box_::getOrderedHalfLength()");
+ return h[order[i]];
+}
+
+/** Get axes in order shortest to longest; 0 is shortest, 2 is longest. **/
+CoordinateAxis getOrderedAxis(int i) const {
+ SimTK_INDEXCHECK(i, 3, "Geo::Box_::getOrderedAxis()");
+ return CoordinateAxis(order[i]);
+}
+
+/** Calculate the volume of this box. Cost is 4 flops. **/
+RealP findVolume() const {return 8*h[0]*h[1]*h[2];}
+/** Calculate the surface area of this box, ignoring degeneracy (meaning that
+all pairs of sides are counted even if coincident). Cost is 6 flops. **/
+RealP findArea() const {return 8*(h[0]*h[1] + h[0]*h[2] + h[1]*h[2]);}
+
+/** Given a point measured and expressed in the box frame, determine whether
+it is inside the box (we count touching the surface as inside). The point
+must be measured from the box center. Cost is about 5 flops. **/
+bool containsPoint(const Vec3P& pt) const {
+ const Vec3P absPt = pt.abs(); // reflect to first quadrant
+ return absPt <= h;
+}
+
+/** Return the square of the distance from this box to a given point whose
+location is measured from and expressed in the box frame (at the box center).
+If the point is on or inside the box the returned distance is zero. Cost is
+about 14 flops. **/
+RealP findDistanceSqrToPoint(const Vec3P& pt) const {
+ const Vec3P absPt = pt.abs(); // reflect to first quadrant
+ RealP d2 = 0;
+ if (absPt[0] > h[0]) d2 += square(absPt[0]-h[0]);
+ if (absPt[1] > h[1]) d2 += square(absPt[1]-h[1]);
+ if (absPt[2] > h[2]) d2 += square(absPt[2]-h[2]);
+ return d2;
+}
+
+/** Return the square of the distance from this box to a given sphere whose
+center location is measured from and expressed in the box frame (at the box
+center). If the sphere intersects the box the returned distance is zero. Cost
+is about 17 flops. **/
+RealP findDistanceSqrToSphere(const Geo::Sphere_<P>& sphere) const {
+ const Vec3P absCtr = sphere.getCenter().abs(); // reflect to first quadrant
+ const Vec3P grow = h + sphere.getRadius(); // 3 flops
+ RealP d2 = 0;
+ if (absCtr[0] > grow[0]) d2 += square(absCtr[0]-grow[0]);
+ if (absCtr[1] > grow[1]) d2 += square(absCtr[1]-grow[1]);
+ if (absCtr[2] > grow[2]) d2 += square(absCtr[2]-grow[2]);
+ return d2;
+}
+
+/** Return the square of the distance from this box to an axis-aligned box whose
+center location is measured from and expressed in this box frame (at the box
+center). If the boxes intersect the returned distance is zero. Cost
+is about 17 flops. **/
+RealP findDistanceSqrToAlignedBox(const Geo::AlignedBox_<P>& aab) const {
+ const Vec3P absCtr = aab.getCenter().abs(); // reflect to first quadrant
+ const Vec3P grow = h + aab.getHalfLengths();
+ RealP d2 = 0;
+ if (absCtr[0] > grow[0]) d2 += square(absCtr[0]-grow[0]);
+ if (absCtr[1] > grow[1]) d2 += square(absCtr[1]-grow[1]);
+ if (absCtr[2] > grow[2]) d2 += square(absCtr[2]-grow[2]);
+ return d2;
+}
+
+/** Given a sphere with center measured and expressed in the box frame, return
+true if the box and sphere intersect. We are treating both objects as solids,
+so we'll say yes even if one object completely contains the other. We also
+return true if they are just touching. Cost is about 8 flops. **/
+bool intersectsSphere(const Geo::Sphere_<P>& sphere) const {
+ const Vec3P absCtr = sphere.getCenter().abs(); // reflect to first quadrant
+ const RealP r = sphere.getRadius();
+ if (absCtr[0] > h[0]+r) return false;
+ if (absCtr[1] > h[1]+r) return false;
+ if (absCtr[2] > h[2]+r) return false;
+ return true;
+}
+
+/** Given an aligned box with center measured and expressed in the from of
+this box, return true if the two boxes intersect. We are treating both objects
+as solids, so we'll say yes even if one box completely contains the other. We
+also return true if they are just touching. Cost is about 8 flops. **/
+bool intersectsAlignedBox(const Geo::AlignedBox_<P>& aab) const {
+ const Vec3P absCtr = aab.getCenter().abs(); // reflect to first quadrant
+ const Vec3P& aabh = aab.getHalfLengths();
+ if (absCtr[0] > h[0]+aabh[0]) return false;
+ if (absCtr[1] > h[1]+aabh[1]) return false;
+ if (absCtr[2] > h[2]+aabh[2]) return false;
+ return true;
+}
+
+/** Given an oriented box whose pose is measured and expressed in the frame
+of this box, return true if the two boxes intersect. We are treating both
+objects as solids, so we'll say yes even if one box completely contains the
+other. We also return true if they are just touching. This is an exact but
+fairly expensive test if the boxes are separated; if you don't mind some
+false positives, use mayIntersectOrientedBox() instead. Cost is about 200
+flops worst case (when boxes are intersecting) although it can return
+\c false in as few as 16 flops. **/
+SimTK_SIMMATH_EXPORT bool
+intersectsOrientedBox(const Geo::OrientedBox_<P>& ob) const;
+
+/** Given an oriented box whose pose is measured and expressed in the frame
+of this box, return true if the two boxes may be intersecting. Only relatively
+cheap operations are performed at the expense of returning false positives
+sometimes (allegedly less than 10% of the time). If you need an exact
+determination, use intersectsOrientedBox(). Cost is about 75 flops worst
+case (when boxes appear to be intersecting) but can return \c false in as few
+as 16 flops. **/
+SimTK_SIMMATH_EXPORT bool
+mayIntersectOrientedBox(const Geo::OrientedBox_<P>& ob) const;
+
+
+private:
+// Call this whenever an edge length changes. Each axis will appear once.
+void sortEdges() {
+ CoordinateAxis shortest = XAxis, longest = ZAxis;
+ if (h[YAxis] < h[shortest]) shortest=YAxis;
+ if (h[ZAxis] < h[shortest]) shortest=ZAxis;
+ if (h[XAxis] > h[longest]) longest=XAxis;
+ if (h[YAxis] > h[longest]) longest=YAxis;
+ order[0] = shortest; order[2] = longest;
+ order[1] = shortest.getThirdAxis(longest); // not shortest or longest
+}
+
+int intersectsOrientedBoxHelper(const OrientedBox_<P>& O,
+ Mat33P& absR_BO,
+ Vec3P& absP_BO) const;
+
+Vec3P h; // half-dimensions of the box
+unsigned char order[3]; // 0,1,2 reordered short to long
+};
+
+
+
+//==============================================================================
+// GEO ALIGNED BOX
+//==============================================================================
+/** A 3d box aligned with an unspecified frame F and centered at a given
+point measured from that frame's origin. The box frame B is aligned with F
+but the origin Bo is shifted from Fo. **/
+template <class P>
+class SimTK_SIMMATH_EXPORT Geo::AlignedBox_ {
+typedef P RealP;
+typedef Vec<3,RealP> Vec3P;
+
+public:
+/** Construct an uninitialized AlignedBox object; the dimensions and location
+will be garbage. **/
+AlignedBox_() {}
+/** Construct an AlignedBox with the given box shape with the center located
+as given. **/
+AlignedBox_(const Vec3P& center, const Geo::Box_<P>& box)
+: center(center), box(box) {}
+/** Construct an AlignedBox with the given center location and
+half-dimensions. **/
+AlignedBox_(const Vec3P& center, const Vec3P& halfLengths)
+: center(center), box(halfLengths) {}
+
+/** Change the center location of this box. **/
+AlignedBox_& setCenter(const Vec3P& center)
+{ this->center=center; return *this; }
+
+/** Change the dimensions of this box. **/
+AlignedBox_& setHalfLengths(const Vec3P& halfLengths)
+{ box.setHalfLengths(halfLengths); return *this; }
+
+/** Return the location of the center of this box (box frame origin). **/
+const Vec3P& getCenter() const {return center;}
+/** Return a writable reference to the center location of this box. **/
+Vec3P& updCenter() {return center;}
+/** Return the half-lengths of this box as a Vec3 from the center to the
+first quadrant vertex. **/
+const Vec3P& getHalfLengths() const {return box.getHalfLengths();}
+// no updHalfLengths()
+const Box_<P>& getBox() const {return box;}
+Box_<P>& updBox() {return box;}
+
+/** Given a point measured and expressed in the base frame F, determine whether
+it is strictly contained in the box (just touching doesn't count). Cost is
+about 8 flops. **/
+bool containsPoint(const Vec3P& pt_F) const
+{ return box.containsPoint(pt_F - center); } // shift to box frame B
+
+/** Stretch this box in place by a small amount to ensure that there will
+be no roundoff problems if this is used as a bounding box. The amount to
+stretch depends on the default tolerance for this precision, the dimensions,
+and the position of the box in space. A very large box, or a box that is
+very far from the origin, must be stretched more than a small one at the
+origin. Cost is 6 flops.
+ at see Geo class for tolerance information. **/
+AlignedBox_& stretchBoundary() {
+ const RealP tol = Geo::getDefaultTol<P>();
+ const RealP maxdim = max(getCenter().abs());
+ const RealP maxrad = max(getHalfLengths());
+ const RealP scale = std::max(maxdim, maxrad);
+ const RealP incr = std::max(scale*Geo::getEps<P>(), tol);
+ box.addToHalfLengths(Vec3P(incr));
+ return *this;
+}
+
+private:
+Vec3P center;
+Geo::Box_<P> box;
+};
+
+
+//==============================================================================
+// GEO ORIENTED BOX
+//==============================================================================
+/** TODO: A 3d box oriented and positioned with respect to an unspecified
+frame F. **/
+template <class P>
+class SimTK_SIMMATH_EXPORT Geo::OrientedBox_ {
+typedef P RealP;
+typedef Vec<3,P> Vec3P;
+typedef Rotation_<P> RotationP;
+typedef Transform_<P> TransformP;
+
+public:
+/** Construct an uninitialized OrientedBox object; the dimensions and pose
+will be garbage. **/
+OrientedBox_() {}
+/** Construct an OrientedBox with the given box shape with positioned and
+oriented according to the given Transform X_FB which gives the box local
+frame B (at the box center) in an unspecifed frame F. **/
+OrientedBox_(const TransformP& X_FB, const Geo::Box_<P>& box)
+: X_FB(X_FB), box(box) {}
+/** Construct an OrientedBox with the given location and
+half-dimensions. **/
+OrientedBox_(const TransformP& X_FB, const Vec3P& halfLengths)
+: X_FB(X_FB), box(halfLengths) {}
+
+
+/** Change the pose of this box. **/
+OrientedBox_& setTransform(const TransformP& newX_FB)
+{ X_FB=newX_FB; return *this; }
+
+/** Change the dimensions of this box. **/
+OrientedBox_& setHalfLengths(const Vec3P& halfLengths)
+{ box.setHalfLengths(halfLengths); return *this; }
+
+const Vec3P& getCenter() const {return X_FB.p();}
+Vec3P& updCenter() {return X_FB.updP();}
+const RotationP& getOrientation() const {return X_FB.R();}
+RotationP& updOrientation() {return X_FB.updR();}
+const TransformP& getTransform() const {return X_FB;}
+TransformP& updTransform() {return X_FB;}
+const Vec3P& getHalfLengths() const {return box.getHalfLengths();}
+// no updHalfLengths()
+const Box_<P>& getBox() const {return box;}
+Box_<P>& updBox() {return box;}
+
+
+/** Given a point measured and expressed in the base frame F, determine whether
+it is strictly contained in the box (just touching doesn't count). Cost is
+about 23 flops. **/
+bool containsPoint(const Vec3P& pt_F) const
+{ return box.containsPoint(~X_FB*pt_F); } // shift to box frame B
+
+/** Stretch this box in place by a small amount to ensure that there will
+be no roundoff problems if this is used as a bounding box. The amount to
+stretch depends on the default tolerance for this precision, the dimensions,
+and the position of the box in space. A very large box, or a box that is
+very far from the origin, must be stretched more than a small one at the
+origin. Cost is 6 flops.
+ at see Geo class for tolerance information. **/
+OrientedBox_& stretchBoundary() {
+ const RealP tol = Geo::getDefaultTol<P>();
+ const RealP maxdim = max(getCenter().abs());
+ const RealP maxrad = max(getHalfLengths());
+ const RealP scale = std::max(maxdim, maxrad);
+ const RealP incr = std::max(scale*Geo::getEps<P>(), tol);
+ box.addToHalfLengths(Vec3P(incr));
+ return *this;
+}
+
+private:
+TransformP X_FB;
+Geo::Box_<P> box;
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_GEO_BOX_H_
diff --git a/SimTKmath/Geometry/include/simmath/internal/Geo_CubicBezierCurve.h b/SimTKmath/Geometry/include/simmath/internal/Geo_CubicBezierCurve.h
new file mode 100644
index 0000000..291c402
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/Geo_CubicBezierCurve.h
@@ -0,0 +1,526 @@
+#ifndef SimTK_SIMMATH_GEO_CUBIC_BEZIER_CURVE_H_
+#define SimTK_SIMMATH_GEO_CUBIC_BEZIER_CURVE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Provides primitive operations for a single bicubic Bezier curve using either
+single or double precision. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_Point.h"
+#include "simmath/internal/Geo_Sphere.h"
+#include "simmath/internal/Geo_Box.h"
+
+#include <cassert>
+#include <cmath>
+#include <algorithm>
+
+namespace SimTK {
+
+
+//==============================================================================
+// GEO CUBIC BEZIER CURVE
+//==============================================================================
+/** This is a primitive useful for computations involving a single cubic Bezier
+curve segment. Objects of this class contain the Bezier control points, but
+these can easily be converted to algebraic or Hermite coefficients. A useful
+feature of the Bezier control points representation is that the curve (not
+necessarily planar) lies within the convex hull of the four control points. We
+can use that fact to create a bounding sphere or oriented bounding box around
+the curve. We can check whether the control points are already convex to ensure
+that the contained curve is well behaved, and subdivide if not.
+
+Note that a cubic Bezier spline (made up of multiple segments) would not
+necessarily be composed of these because a spline can be contructed more
+compactly with shared end points. However, the primitive and inline methods
+here can be used for fast curve segment computations.
+
+<h3>Theory</h3>
+The primary reference for this implementation is the book "Geometric Modeling,
+3rd ed." by Michael E. Mortenson, Industrial Press 2006, chapter 4. We follow
+Mortenson's notation here (with some name changes) and equation numbers are
+from the text. See CubicHermiteCurve_ comments for an introduction; here we
+add the Bezier description to the algebraic and Hermite (geometric) forms
+described there.
+
+The curve is parameterized by a scalar u in [0..1], such that points on the
+curve, and their derivatives with respect to u are given by <pre>
+ P(u) = B0(u) b0 + B1(u) b1 + B2(u) b2 + B3(u) b3 (4.2)
+ Pu(u) = B0u(u) b0 + B1u(u) b1 + B2u(u) b2 + B3u(u) b3
+ Puu(u) = B0uu(u) b0 + B1uu(u) b1 + B2uu(u) b2 + B3uu(u) b3
+ Puuu(u) = B0uuu(u) b0 + B1uuu(u) b1 + B2uuu(u) b2 + B3uuu(u) b3
+</pre> where the Bi's are the (scalar) Bernstein polynomials given by
+<pre>
+ B0(u) = (1-u)^3 = -u^3 + 3u^2 - 3u + 1 (4.5)
+ B1(u) = 3 u (1-u)^2 = 3u^3 - 6u^2 + 3u
+ B2(u) = 3 u^2(1-u) = -3u^3 + 3u^2
+ B3(u) = u^3 = u^3
+
+ B0u(u) = -3u^2 + 6u - 3 B0uu(u) = -6u + 6 B0uuu(u) = -6
+ B1u(u) = 9u^2 - 12u + 3 B1uu(u) = 18u - 12 B1uuu(u) = 18
+ B2u(u) = -9u^2 + 6u B2uu(u) = -18u + 6 B2uuu(u) = -18
+ B3u(u) = 3u^2 B3uu(u) = 6u B3uuu(u) = 6
+</pre>
+In matrix notation, let Fb=[B0 B1 B2 B3], and U=[u^3 u^2 u 1]. Then
+ Fb = U Mb
+where Mb, the Bezier basis transformation matrix, and its inverse are:
+<pre>
+ [ -1 3 -3 1 ] [ 0 0 0 1 ]
+ Mb = [ 3 -6 3 0 ] inv(Mb) = [ 0 0 1/3 1 ]
+ [ -3 3 0 0 ] [ 0 1/3 2/3 1 ]
+ [ 1 0 0 0 ] [ 1 1 1 1 ]
+</pre>
+Now we can write the algebraic, Hermite, and Bezier forms in matrix notation.
+Let A=~[a3 a2 a1 a0], H=~[h0 h1 hu0 hu1], B=~[b0 b1 b2 b3]. We have <pre>
+ P(u) = U A = U Mh H = U Mb B (4.7)
+ A = Mh H = Mb B
+ H = inv(Mh) A = inv(Mh) Mb B (4.8)
+ B = inv(Mb) A = inv(Mb) Mh H (4.9)
+</pre>
+where these equations show how to convert among the algebraic, Hermite,
+and Bezier forms. Note that while U, Fb, Mh, and Mb are ordinary matrices, A, H,
+and B are hypermatrices since their elements are 3-vectors. Multiplying out
+the matrix products gives: <pre>
+ [ 1 0 0 0 ] [ 1 0 0 0 ]
+ Mh^-1 Mb = [ 0 0 0 1 ] Mb^-1 Mh = [ 1 0 1/3 0 ]
+ [ -3 3 0 0 ] [ 0 1 0 -1/3 ]
+ [ 0 0 -3 3 ] [ 0 1 0 0 ]
+</pre>
+Because of the sparsity of the matrices and the many common subexpressions
+above, it saves a considerable amount of computation to work out the necessary
+products by hand, and this implementation does that. For example, to find the
+Bezier control points B given the Hermite coefficients H, or vice versa, the
+matrix-vector multiply would take 3x28=84 flops, while the hand-worked versions
+are: <pre>
+ [h0 ] -1 [ b0 ] [b0] -1 [ h0 ]
+ H = [h1 ] = Mh Mb B = [ b3 ] B = [b1] = Mb Mh H = [h0 + hu0/3]
+ [hu0] [3 (b1-b0)] [b2] [h1 - hu1/3]
+ [hu1] [3 (b3-b2)] [b3] [ h1 ]
+</pre>
+which instead take 3x4=12 flops, 7X faster. Conversion between Bezier
+and algebraic is a little more expensive: <pre>
+ [ b3-b0 + 3 (b1-b2) ] [ a0 ]
+ A = Mb B = [ 3 (b0+b2) - 6 b1 ] B = Mb^-1 A = [ a1/3 + a0 ]
+ [ 3 (b1-b0) ] [ (a2 + 2 a1)/3 + a0 ]
+ [ b0 ] [ a3 + a2 + a1 + a0 ]
+</pre>
+which take about 3x10=30 flops, still almost 3X faster than a matrix-vector
+multiply.
+
+ at see CubicHermiteCurve_, BicubicBezierPatch_, BicubicHermitePatch_
+**/
+template <class P>
+class Geo::CubicBezierCurve_ {
+typedef P RealP;
+typedef Vec<3,RealP> Vec3P;
+typedef UnitVec<P,1> UnitVec3P;
+typedef Rotation_<P> RotationP;
+typedef Transform_<P> TransformP;
+
+public:
+/** Construct an uninitialized curve; control points will be garbage. **/
+CubicBezierCurve_() {}
+
+/** Construct a cubic Bezier curve using the given control points
+B=[b0 b1 b2 b3]. **/
+template <int S>
+explicit CubicBezierCurve_(const Vec<4,Vec3P,S>& controlPoints)
+: B(controlPoints) {}
+
+/** Alternate signature accepts a Row of control points, although they are
+stored internally as a Vec. **/
+template <int S>
+explicit CubicBezierCurve_(const Row<4,Vec3P,S>& controlPoints)
+: B(controlPoints.positionalTranspose()) {}
+
+/** Return a reference to the Bezier control points B=[b0 b1 b2 b3] that are
+stored in this object. **/
+const Vec<4,Vec3P>& getControlPoints() const {return B;}
+/** Calculate the algebraic coefficients A=[a3 a2 a1 a0] from the stored
+Bezier control points. Cost is 30 flops. **/
+Vec<4,Vec3P> calcAlgebraicCoefficients() const {return calcAFromB(B);}
+/** Calculate the Hermite (geometric) coefficients H=[h0 h1 hu0 hu1] from the
+stored Bezier control points. Cost is 12 flops. **/
+Vec<4,Vec3P> calcHermiteCoefficients() const {return calcHFromB(B);}
+/** Evaluate a point on this curve given a value for parameter u in [0,1].
+Values outside this range are permitted but do not lie on the curve segment.
+Cost is 20 flops. **/
+Vec3P evalP(RealP u) const {return evalPUsingB(B,u);}
+/** Evaluate the tangent Pu=dP/du on this curve given a value for parameter u
+in [0,1]. Values outside this range are permitted but do not lie on the curve
+segment. Cost is 15 flops. **/
+Vec3P evalPu(RealP u) const {return evalPuUsingB(B,u);}
+/** Evaluate the second derivative Puu=d2P/du2 on this curve given a value for
+parameter u in [0,1]. Values outside this range are permitted but do not lie on
+the curve segment. Cost is 10 flops. **/
+Vec3P evalPuu(RealP u) const {return evalPuuUsingB(B,u);}
+/** Evaluate the third derivative Puuu=d3P/du3 on this curve. Parameter u is
+ignored here since the 3rd derivative of a cubic curve is a constant. Cost is
+3 flops. **/
+Vec3P evalPuuu(RealP u) const {return evalPuuuUsingB(B,u);}
+
+/** Return ds/du, the change in arc length per change in curve parameter.
+This is the magnitude of the tangent vector Pu=dP/du. Cost is about 40
+flops. **/
+RealP calcDsdu(RealP u) const {return evalPu(u).norm();}
+
+/** The unit tangent vector t=dP/ds where s is the arc length. This is
+undefined at a cusp (Pu(u)==0). Cost is about 55 flops.
+ at see Geo::calcUnitTangent() for more information. **/
+UnitVec3P calcUnitTangent(RealP u) const {
+ const Vec3P Pu=evalPu(u); // 15 flops
+ return Geo::calcUnitTangent(Pu); // ~40 flops
+}
+
+/** The curvature vector c=dt/ds where t is the unit tangent vector
+(t=dP/ds) and s is arclength. Cost is about 55 flops.
+ at see Geo::calcCurvatureVector() for more information. **/
+Vec3P calcCurvatureVector(RealP u) const {
+ const Vec3P Pu=evalPu(u), Puu=evalPuu(u); // 25 flops
+ return Geo::calcCurvatureVector(Pu,Puu); // ~30 flops
+}
+
+/** Return k^2, the square of the scalar curvature k at the point P(u) on
+the curve. Curvature is undefined at a cusp (where Pu==0) and is zero
+at an inflection point (|Pu X Puu|==0). Cost is about 31 flops. **/
+RealP calcCurvatureSqr(RealP u) {
+ const Vec3P Pu=evalPu(u), Puu=evalPuu(u); // 25 flops
+ return Geo::calcCurvatureSqr(Pu,Puu); // ~30 flops
+}
+
+/** Return tau, the torsion or "second curvature". Torsion is a signed quantity
+related to the rate of change of the osculating plane binormal b, with
+db/ds=tau*n where n is the "outward" unit normal. Torsion is undefined at
+either a cusp (where Pu==0) or an inflection point (where |Pu X Puu|==0). Cost
+is about 30 flops.
+ at see Geo::calcTorsion() for more information. **/
+RealP calcTorsion(RealP u) {
+ const Vec3P Pu=evalPu(u), Puu=evalPuu(u), Puuu=evalPuuu(u); // 28 flops
+ return Geo::calcTorsion(Pu,Puu,Puuu);
+}
+
+
+/** In our definition, the unit normal vector n points in the "outward"
+direction, that is, it points away from the center of curvature (opposite
+the curvature vector). The normal is undefined at a cusp (Pu(u)==0), and
+arbitrary at an inflection point (|Pu X Puu|==0). If the curve is a straight
+line then every point has Puu==0, so the normal is arbitrary everywhere.
+Cost is about 105 flops.
+ at see Geo::calcUnitNormal() for more information. **/
+UnitVec3P calcUnitNormal(RealP u) const {
+ const Vec3P Pu=evalPu(u), Puu=evalPuu(u); // 25 flops
+ return Geo::calcUnitNormal(Pu,Puu); // ~80 flops
+}
+
+/** Return the magnitude of the curvature (always positive), and a frame
+whose origin is a point along the curve, x axis is the outward unit normal n,
+y is the unit tangent t, and z=x X y is the binormal b, which is a normal
+to the osculating plane. So the vectors n,t,b form a right-handed set; this
+convention is different from Struik's since he has n pointing the opposite
+direction. This frame is undefined at a cusp (Pu==0), and the normal is
+arbitrary at an inflection point (Puu(u)==0) or if the
+curve is a line (Puu==0 everywhere). Cost is about 160 flops.
+**/
+RealP calcCurveFrame(RealP u, TransformP& X_FP) const {
+ const Vec3P Pval=evalP(u), Pu=evalPu(u), Puu=evalPuu(u); // 45 flops
+ return Geo::calcCurveFrame(Pval,Pu,Puu,X_FP);
+}
+
+/** Split this curve into two at a point u=t such that 0 < t < 1, such that
+the first curve coincides with the u=0..t segment of this curve, and the
+second coincides with the u=t..1 segment. Each of the new curves is
+reparameterized so that its curve parameter goes from 0 to 1. This method
+is only allowed for tol <= t <= 1-tol where tol is the default tolerance
+for this precision. Cost is 3x15=45 flops. **/
+void split(RealP u, CubicBezierCurve_<P>& left,
+ CubicBezierCurve_<P>& right) const {
+ const RealP tol = getDefaultTol<RealP>();
+ SimTK_ERRCHK1(tol <= u && u <= 1-tol, "Geo::CubicBezierCurve::split()",
+ "Can't split curve at parameter %g; it is either out of range or"
+ " too close to an end point.", (double)u);
+
+ const RealP u1 = 1-u;
+ const Vec3P p01 = u1*B[0] + u*B[1]; // 3x9 flops
+ const Vec3P p12 = u1*B[1] + u*B[2];
+ const Vec3P p23 = u1*B[2] + u*B[3];
+ left.B[0] = B[0];
+ left.B[1] = p01;
+ left.B[2] = u1*p01 + u*p12; // 3x3 flops
+
+ right.B[3] = B[3];
+ right.B[2] = p23;
+ right.B[1] = u1*p12 + u*p23;
+ left.B[3] = right.B[0] = u1*left.B[2] + u*right.B[1]; // 3x3 flops
+}
+
+/** Split this curve into two at the point u=1/2 (halfway in parameter space,
+not necessarily in arclength). This is a faster special case
+of the split() method. Cost is 3x10=30 flops. **/
+void bisect(CubicBezierCurve_<P>& left,
+ CubicBezierCurve_<P>& right) const {
+ const Vec3P p01 = (B[0] + B[1])/2; // 3x6 flops
+ const Vec3P p12 = (B[1] + B[2])/2;
+ const Vec3P p23 = (B[2] + B[3])/2;
+ left.B[0] = B[0];
+ left.B[1] = p01;
+ left.B[2] = (p01 + p12)/2; // 3x2 flops
+
+ right.B[3] = B[3];
+ right.B[2] = p23;
+ right.B[1] = (p12 + p23)/2;
+ left.B[3] = right.B[0] = (left.B[2] + right.B[1])/2; // 3x2 flops
+}
+
+
+/** Return a sphere that surrounds the entire curve segment in the u=[0..1]
+range. We use the fact that the curve is enclosed within the convex hull of
+its control points and generate the minimum bounding sphere that includes all
+four control points. **/
+Geo::Sphere_<P> calcBoundingSphere() const
+{ return Geo::Point_<P>::calcBoundingSphere(B[0],B[1],B[2],B[3]); }
+
+/** Return an axis-aligned bounding box (AABB) that surrounds the entire curve
+segment in the u=[0..1] range. We use the fact that the curve is enclosed
+within the convex hull of its control points and generate the minimum
+axis-aligned box that includes all four control points. **/
+Geo::AlignedBox_<P> calcAxisAlignedBoundingBox() const
+{ const ArrayViewConst_<Vec3P> points(&B[0], &B[0]+4); // no copy or heap use
+ return Geo::Point_<P>::calcAxisAlignedBoundingBox(points); }
+
+/** Return an oriented bounding box (OBB) that surrounds the entire curve
+segment in the u=[0..1] range. We use the fact that the curve is enclosed
+within the convex hull of its control points and generate an oriented bounding
+box that includes all four control points. **/
+Geo::OrientedBox_<P> calcOrientedBoundingBox() const
+{ const ArrayViewConst_<Vec3P> points(&B[0], &B[0]+4); // no copy or heap use
+ return Geo::Point_<P>::calcOrientedBoundingBox(points); }
+
+/**@name Utility methods
+These static methods provide operations useful for working with cubic Bezier
+curves. See the CubicHermiteCurve_ class for related operations. **/
+/**@{**/
+/** Calculate the Bernstein basis functions Fb=[B0..B3] for a given value of
+the parameter u. This is an optimized calculation of U*Mb, taking 9 flops. **/
+static Row<4,P> calcFb(RealP u) {
+ const RealP u2 = u*u, u3 = u*u2; // powers of u
+ const RealP u1 = 1-u, u12=u1*u1, u13=u1*u12; // powers of 1-u
+ return Row<4,P>(u13, 3*u*u12, 3*u2*u1, u3);
+}
+
+/** Calculate first derivatives dFb=[B0u..B3u] of the Bernstein basis functions
+for a given value of the parameter u. Cost is 10 flops. **/
+static Row<4,P> calcDFb(RealP u) {
+ const RealP u6=6*u, u2 = u*u, u23 = 3*u2, u29 = 9*u2;
+ return Row<4,P>(u6-u23-3, u29-12*u+3, u6-u29, u23);
+}
+
+/** Calculate second derivatives d2Fb=[B0uu..B3uu] of the Bernstein basis
+functions for a given value of the parameter u. Cost is 5 flops. **/
+static Row<4,P> calcD2Fb(RealP u) {
+ const RealP u6 = 6*u, u18 = 18*u;
+ return Row<4,P>(6-u6, u18-12, 6-u18, u6);
+}
+
+/** Calculate third derivatives d3Fb=[B0uuu..B3uuu] of the Bernstein basis
+functions for a given value of the parameter u. For a cubic curve this is
+just a constant. Cost is 0 flops. **/
+static Row<4,P> calcD3Fb(RealP u) {
+ return Row<4,P>(-6, 18, -18, 6);
+}
+
+/** Given the Bezier control points B=~[b0 b1 b2 b3], return the algebraic
+coefficients A=~[a3 a2 a1 a0]. All coefficients are 3-vectors. Cost is 30
+flops. **/
+template <int S>
+static Vec<4,Vec3P> calcAFromB(const Vec<4,Vec3P,S>& B) {
+ const Vec3P& b0=B[0]; const Vec3P& b1=B[1]; // aliases for beauty
+ const Vec3P& b2=B[2]; const Vec3P& b3=B[3];
+ return Vec<4,Vec3P>(b3-b0+3*(b1-b2), 3*(b0+b2)-6*b1, 3*(b1-b0), b0);
+}
+
+/** Given the algebraic coefficients A=~[a3 a2 a1 a0], return the Bezier
+control points B=~[b0 b1 b2 b3]. All coefficients are 3-vectors. Cost is 27
+flops. **/
+template <int S>
+static Vec<4,Vec3P> calcBFromA(const Vec<4,Vec3P,S>& A) {
+ const Vec3P& a3=A[0]; const Vec3P& a2=A[1]; // aliases for beauty
+ const Vec3P& a1=A[2]; const Vec3P& a0=A[3];
+ return Vec<4,Vec3P>(a0, a1/3 + a0, (a2+2*a1)/3 + a0, a3+a2+a1+a0);
+}
+
+/** Given the Bezier control points B=~[b0 b1 b2 b3], return the Hermite
+coefficients H=~[h0 h1 hu0 hu1]. All coefficients are 3-vectors. Cost is 12
+flops. **/
+template <int S>
+static Vec<4,Vec3P> calcHFromB(const Vec<4,Vec3P,S>& B) {
+ const Vec3P& b0=B[0]; const Vec3P& b1=B[1]; // aliases for beauty
+ const Vec3P& b2=B[2]; const Vec3P& b3=B[3];
+ return Vec<4,Vec3P>(b0, b3, 3*(b1-b0), 3*(b3-b2));
+}
+
+/** Given the Hermite coefficients H=~[h0 h1 hu0 hu1], return the Bezier
+control points B=~[b0 b1 b2 b3]. All coefficients are 3-vectors. Cost is 12
+flops. **/
+template <int S>
+static Vec<4,Vec3P> calcBFromH(const Vec<4,Vec3P,S>& H) {
+ const Vec3P& h0= H[0]; const Vec3P& h1= H[1]; // aliases for beauty
+ const Vec3P& hu0=H[2]; const Vec3P& hu1=H[3];
+ return Vec<4,Vec3P>(h0, h0 + hu0/3, h1 - hu1/3, h1);
+}
+
+/** Given Bezier control points B and a value for the curve parameter u, return
+the point P(u) at that location. Cost is 30 flops. Note that if you need to
+do this for the same curve more than twice, it is cheaper to convert to
+algebraic form using calcAFromB() (30 flops) and then evaluate using A
+(20 flops). **/
+template <int S>
+static Vec3P evalPUsingB(const Vec<4,Vec3P,S>& B, RealP u) {
+ return calcFb(u)*B; // 9 + 3*7 = 30 flops
+}
+/** Given Bezier control points B and a value for the curve parameter u, return
+the first derivative Pu(u)=dP/du at that location. Cost is 31 flops. Note that
+if you need to do this for the same curve more than once, it is cheaper to
+convert to algebraic form using calcAFromB() (30 flops) and then evaluate using
+A (15 flops). **/
+template <int S>
+static Vec3P evalPuUsingB(const Vec<4,Vec3P,S>& B, RealP u) {
+ return calcDFb(u)*B; // 10 + 3*7 = 31 flops
+}
+/** Given Bezier control points B and a value for the curve parameter u, return
+the second derivative Puu(u)=d2P/du2 at that location. Cost is 26 flops. Note
+that if you need to do this for the same curve more than once, it is cheaper to
+convert to algebraic form using calcAFromB() (30 flops) and then evaluate using
+A (10 flops). **/
+template <int S>
+static Vec3P evalPuuUsingB(const Vec<4,Vec3P,S>& B, RealP u) {
+ return calcD2Fb(u)*B; // 5 + 3*7 = 26 flops
+}
+/** Given Bezier control points B and a value for the curve parameter u, return
+the third derivative Puuu(u)=d3P/du3 at that location. Cost is 21 flops. Note
+that if you need to do this for the same curve more than once, it is cheaper to
+convert to algebraic form using calcAFromB() (30 flops) and then evaluate using
+A (3 flops). **/
+template <int S>
+static Vec3P evalPuuuUsingB(const Vec<4,Vec3P,S>& B, RealP u) {
+ return calcD3Fb(u)*B; // 0 + 3*7 = 21 flops
+}
+/** Obtain the Bezier basis matrix Mb explicitly. This is mostly useful for
+testing since specialized routines can save a lot of CPU time over working
+directly in matrix form. This is a constant matrix so there is no computation
+cost. The matrix is symmetric although we return a full 4x4 here. **/
+static Mat<4,4,P> getMb() {
+ return Mat<4,4,P>( -1, 3, -3, 1,
+ 3, -6, 3, 0,
+ -3, 3, 0, 0,
+ 1, 0, 0, 0);
+}
+
+/** Form the product of the Bezier basis matrix Mb and a 4-vector, exploiting
+the structure of Mb. Since Mb is symmetric you can also use this for
+multiplication by a row from the left, i.e. ~b*Mb=~(~Mb*b)=~(Mb*b).
+Cost is 10 flops. **/
+template <int S>
+static Vec<4,P> multiplyByMb(const Vec<4,P,S>& b) {
+ const RealP b0=b[0], b1=b[1], b2=b[2], b3=b[3];
+ return Vec<4,P>(3*(b1-b2)+b3-b0, 3*(b0+b2)-6*b1, 3*(b1-b0), b0);
+}
+
+/** Obtain the inverse inv(Mb) of the Bezier basis matrix explicitly. This is
+mostly useful for testing since specialized routines can save a lot of CPU time
+over working directly in matrix form. This is a constant matrix so there is no
+computation cost. The matrix is symmetric although we return a full 4x4
+here. **/
+static Mat<4,4,P> getMbInv() {
+ return Mat<4,4,P>( 0, 0, 0, 1,
+ 0, 0, P(1)/3, 1,
+ 0, P(1)/3, P(2)/3, 1,
+ 1, 1, 1, 1 );
+}
+
+/** Form the product of the inverse inv(Mb) of the Bezier basis matrix Mb and a
+4-vector, exploiting the structure of inv(Mb). Since inv(Mb) is symmetric you
+can also use this for multiplication by a row from the left, i.e.
+~b*Mb^-1=~(Mb^-T*b)=~(Mb^-1*b). Cost is 9 flops. **/
+template <int S>
+static Vec<4,P> multiplyByMbInv(const Vec<4,P,S>& b) {
+ const RealP b0=b[0], b1=b[1], b2=b[2], b3=b[3];
+ return Vec<4,P>(b3, b2/3+b3, (b1+2*b2)/3+b3, b0+b1+b2+b3);
+}
+
+/** Obtain the product Mh^-1*Mb explicitly; this is the matrix used for
+conversion from Bezier to Hermite bases since H=Mh^-1 Mb B and is the inverse
+of the matrix Mb^-1*Mh. This is mostly useful for testing since specialized
+routines can save a lot of CPU time over working directly in matrix form.
+There is a very efficient method for forming matrix-vector products with this
+matrix. This is a constant matrix so there is no computation cost.
+ at see multiplyByMhInvMb(), getMbInvMh() **/
+static Mat<4,4,P> getMhInvMb() {
+ return Mat<4,4,P>( 1, 0, 0, 0,
+ 0, 0, 0, 1,
+ -3, 3, 0, 0,
+ 0, 0, -3, 3 );
+}
+/** Given a vector v, form the product inv(Mh)*Mb*v, exploiting the structure
+of the constant matrix inv(Mh)*Mb (not symmetric). Cost is 4 flops. **/
+template <int S>
+static Vec<4,P> multiplyByMhInvMb(const Vec<4,P,S>& v) {
+ const RealP v0=v[0], v1=v[1], v2=v[2], v3=v[3];
+ return Vec<4,P>(v0, v3, 3*(v1-v0), 3*(v3-v2));
+}
+
+/** Obtain the product Mb^-1*Mh explicitly; this is the matrix used for
+conversion from Hermite to Bezier bases since B=Mb^-1 Mh H and is the inverse
+of the matrix Mh^-1*Mb. This matrix is not symmetric. This method is mostly
+useful for testing since specialized routines can save a lot of CPU time over
+working directly in matrix form. There is a very efficient method for forming
+matrix-vector products with this matrix. This is a constant matrix so there is
+no computation cost here.
+ at see multiplyByMhInvMb(), getMhInvMb() **/
+static Mat<4,4,P> getMbInvMh() {
+ return Mat<4,4,P>( 1, 0, 0, 0,
+ 1, 0, P(1)/3, 0,
+ 0, 1, 0, P(-1)/3,
+ 0, 1, 0, 0 );
+}
+/** Given a vector v, form the product inv(Mb)*Mh*v, exploiting the structure
+of the constant matrix inv(Mb)*Mh (not symmetric). Cost is 4 flops. **/
+template <int S>
+static Vec<4,P> multiplyByMbInvMh(const Vec<4,P,S>& v) {
+ const RealP v0=v[0], v1=v[1], v2=v[2], v3=v[3];
+ return Vec<4,P>(v0, v0+v2/3, v1-v3/3, v1);
+}
+/**@}**/
+
+private:
+Vec<4,Vec3P> B;
+};
+
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_GEO_CUBIC_BEZIER_CURVE_H_
diff --git a/SimTKmath/Geometry/include/simmath/internal/Geo_CubicHermiteCurve.h b/SimTKmath/Geometry/include/simmath/internal/Geo_CubicHermiteCurve.h
new file mode 100644
index 0000000..390ec0d
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/Geo_CubicHermiteCurve.h
@@ -0,0 +1,334 @@
+#ifndef SimTK_SIMMATH_GEO_CUBIC_HERMITE_CURVE_H_
+#define SimTK_SIMMATH_GEO_CUBIC_HERMITE_CURVE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Provides primitive operations for a single cubic Hermite curve using either
+single or double precision. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+
+#include <cassert>
+#include <cmath>
+#include <algorithm>
+
+namespace SimTK {
+
+
+//==============================================================================
+// GEO CUBIC HERMITE CURVE
+//==============================================================================
+/** A primitive useful for computations involving a single cubic Hermite
+curve segment in algebraic or geometric (Hermite) form. Objects of this class
+contain the algebraic coefficients because most operations are more efficient
+in that form, but methods are provided for easy conversion to or from Hermite
+form and for working directly with the Hermite form.
+
+Note that a cubic Hermite spline (made up of multiple segments) would not
+necessarily be composed of these because they can be contructed more compactly
+with shared end points. However, the primitive and inline methods here can be
+used for fast curve segment computations.
+
+<h3>Theory</h3>
+The primary reference for this implementation is the book "Geometric Modeling,
+3rd ed." by Michael E. Mortenson, Industrial Press 2006, chapter 3. We follow
+Mortenson's notation here (with a few exceptions) and equation numbers are
+from the text. We're using h's for the Hermite coefficients rather than
+Mortenson's b's to avoid confusion with Bezier control points B, so that we
+can use A for algebraic, H for Hermite, and B for Bezier coefficient matrices.
+
+The curve is parameterized by a scalar u in [0..1], such that points on the
+curve, and their derivatives with respect to u are given by <pre>
+ P(u) = a3 u^3 + a2 u^2 + a1 u + a0 (3.2)
+ Pu(u) = 3 a3 u^2 + 2 a2 u + a1
+ Puu(u) = 6 a3 u + 2 a2
+ Puuu(u) = 6 a3
+</pre> where Pu=dP/du, Puu=d2P/du2, Puuu=d3P/du3. Note that all higher
+derivatives are zero for a cubic. The 3-vectors ai are the algebraic
+coefficients, and that is the algebraic form of this cubic curve. The Hermite,
+or geometric, form is parameterized by position and tangent at the end points,
+given by <pre>
+ h0=P(0)=a0, h1=P(1)=a3+a2+a1+a0, hu0=Pu(0)=a1, hu1=Pu(1)=3*a3+2*a2+a1
+</pre>
+These define the Hermite coefficients h0, h1, hu0, hu1. In this form the
+curve's points are <pre>
+ P(u) = F1(u) h0 + F2(u) h1 + F3(u) hu0 + F4(u) hu1 (3.6)
+ Pu(u) = F1u(u) h0 + F2u(u) h1 + F3u(u) hu0 + F4u(u) hu1
+ Puu(u) = F1uu(u) h0 + F2uu(u) h1 + F3uu(u) hu0 + F4uu(u) hu1
+ Puuu(u) = F1uuu(u) h0 + F2uuu(u) h1 + F3uuu(u) hu0 + F4uuu(u) hu1
+</pre> where the Fi's are the (scalar) Hermite basis functions given by
+<pre>
+ F1(u) = 2 u^3 - 3 u^2 + 1
+ F2(u) = -2 u^3 + 3 u^2 (3.4)
+ F3(u) = u^3 - 2 u^2 + u
+ F4(u) = u^3 - u^2
+
+ F1u(u) = 6 u^2 - 6 u F1uu(u) = 12 u - 6 F1uuu(u) = 12 (3.7)
+ F2u(u) = -6 u^2 + 6 u F2uu(u) = -12 u + 6 F2uuu(u) = -12 (3.8)
+ F3u(u) = 3 u^2 - 4 u + 1 F3uu(u) = 6 u - 4 F3uuu(u) = 6
+ F4u(u) = 3 u^2 - 2 u F4uu(u) = 6 u - 2 F4uuu(u) = 6
+</pre>
+In matrix notation, let Fh=[F1 F2 F3 F4], and U=[u^3 u^2 u 1]. Then
+ Fh = U Mh
+where Mh, the Hermite basis transformation matrix, and its inverse are:
+<pre>
+ [ 2 -2 1 1 ] [ 0 0 0 1 ]
+ Mh = [-3 3 -2 -1 ] inv(Mh) = [ 1 1 1 1 ] (3.18)
+ [ 0 0 1 0 ] [ 0 0 1 0 ] (3.23)
+ [ 1 0 0 0 ] [ 3 2 1 0 ]
+</pre>
+(Mortenson calls this matrix "Mf".) Now we can write the algebraic and Hermite
+forms in matrix notation. Let A=~[a3 a2 a1 a0], H=~[h0 h1 hu0 hu1]. We have
+<pre>
+ P(u) = U A
+ = U Mh H
+ A = Mh H (3.20-22)
+ H = inv(Mh) A
+</pre>
+where the last two equations show how to convert between the algebraic and
+geometric forms. Note that while U, Fh, and Mh are ordinary matrices, A and H
+are hypermatrices since their elements are 3-vectors.
+
+Because of the sparsity of the matrices and the many common subexpressions
+above, it saves a considerable amount of computation to work out the necessary
+products by hand, and this implementation does that. For example, to find the
+algebraic coefficients A given the Hermite coefficients H the matrix-vector
+multiply Mh*H would take 3x28=84 flops, while the hand-worked version is:
+<pre>
+ [ a3 ] [ 2 (h0 - h1) + hu0 + hu1 ]
+ A = [ a2 ] = [-3 (h0 - h1) - 2 hu0 - hu1 ]
+ [ a1 ] [ hu0 ]
+ [ a0 ] [ h0 ]
+</pre> which instead takes 3x8=24 flops, 3.5X faster.
+
+ at see CubicBezierCurve_, BicubicHermitePatch_, BicubicBezierPatch_
+**/
+template <class P>
+class Geo::CubicHermiteCurve_ {
+typedef P RealP;
+typedef Vec<3,RealP> Vec3P;
+
+public:
+/** Construct an uninitialized curve; coefficients will be garbage. **/
+CubicHermiteCurve_() {}
+/** Construct a cubic Hermite curve using the given algebraic coefficients
+A=[a3 a2 a1 a0], such that points on the curve are P(u)=sum_i(ai*u^i). If you
+have Hermite coefficients H, convert them to algebraic using static method
+calcAFromH(). **/
+explicit CubicHermiteCurve_(const Vec<4,Vec3P>& A) : A(A) {}
+/** Return a reference to the algebraic coefficients A=[a3 a2 a1 a0] that are
+stored in this object. **/
+const Vec<4,Vec3P>& getAlgebraicCoefficients() const {return A;}
+/** Calculate the Hermite coefficients H=[h0 h1 hu0 hu1] from the stored
+algebraic coefficients. Here h0=P(0), h1=P(1), hu0=(dP/du)(0), hu1=(dP/du)(1)
+where P(u) is the curve evaluation function. Cost is 21 flops. **/
+Vec<4,Vec3P> calcGeometricCoefficients() const {return calcHFromA(A);}
+/** Evaluate a point P(u) on this curve given a value for parameter u in [0,1].
+Values outside this range are permitted but do not lie on the curve segment.
+Cost is 20 flops. **/
+Vec3P evalP(RealP u) const {return evalPUsingA(A,u);}
+/** Evaluate the tangent Pu=dP/du on this curve given a value for parameter u
+in [0,1]. Values outside this range are permitted but do not lie on the curve
+segment. Cost is 15 flops. **/
+Vec3P evalPu(RealP u) const {return evalPuUsingA(A,u);}
+/** Evaluate the second derivative Puu=d2P/du2 on this curve given a value for
+parameter u in [0,1]. Values outside this range are permitted but do not lie on
+the curve segment. Cost is 10 flops. **/
+Vec3P evalPuu(RealP u) const {return evalPuuUsingA(A,u);}
+/** Evaluate the third derivative Puuu=d3P/du3 on this curve. Parameter u is
+ignored here since the 3rd derivative of a cubic curve is a constant. Cost is
+3 flops. **/
+Vec3P evalPuuu(RealP u) const {return evalPuuuUsingA(A,u);}
+
+/**@name Utility methods
+These static methods provide operations useful for working with cubic Hermite
+curves. **/
+/**@{**/
+/** Return the row vector U=[u^3 u^2 u 1]. Cost is 2 flops. **/
+static Row<4,P> calcU(RealP u) {
+ const RealP u2 = u*u;
+ return Row<4,P>(u*u2, u2, u, 1);
+}
+
+/** Calculate the Hermite basis functions Fh=[F1..F4] for a given value of the
+parameter u. This is an optimized calculation of U*Mh, taking 10 flops. **/
+static Row<4,P> calcFh(RealP u) {
+ const RealP u2 = u*u, u3 = u*u2;
+ const RealP t = 3*u2 - 2*u3;
+ return Row<4,P>(1-t, t, u3-2*u2+u, u3-u2);
+}
+
+/** Calculate first derivatives Fhu=[F1u..F4u] of the Hermite basis functions
+for a given value of the parameter u. Cost is 10 flops. **/
+static Row<4,P> calcFhu(RealP u) {
+ const RealP u2 = u*u, u23=3*u2;
+ const RealP dt = 6*(u-u2);
+ return Row<4,P>(-dt, dt, u23-4*u+1, u23-2*u);
+}
+
+/** Calculate second derivatives Fhuu=[F1uu..F4uu] of the Hermite basis
+functions for a given value of the parameter u. Cost is 6 flops. **/
+static Row<4,P> calcFhuu(RealP u) {
+ const RealP u6 = 6*u;
+ const RealP ddt = 6 - 12*u;
+ return Row<4,P>(-ddt, ddt, u6-4, u6-2);
+}
+
+/** Calculate third derivatives Fhuuu=[F1uuu..F4uuu] of the Hermite basis
+functions for a given value of the parameter u. For a cubic curve the third
+derivative is a constants so the cost is 0 flops. **/
+static Row<4,P> calcFhuuu(RealP u) {
+ return Row<4,P>(12, -12, 6, 6);
+}
+
+/** Given the Hermite coefficients H=~[h0 h1 hu0 hu1], return the algebraic
+coefficients A=~[a3 a2 a1 a0]. All coefficients are 3-vectors. Cost is 24
+flops. **/
+static Vec<4,Vec3P> calcAFromH(const Vec<4,Vec3P>& H) {
+ const Vec3P& h0= H[0]; const Vec3P& h1= H[1]; // aliases for beauty
+ const Vec3P& hu0=H[2]; const Vec3P& hu1=H[3];
+ const Vec3P h01 = h0 - h1;
+ return Vec<4,Vec3P>( 2*h01 + hu0 + hu1,
+ -3*h01 - 2*hu0 - hu1,
+ hu0,
+ h0 );
+}
+
+/** Given the algebraic coefficients A=~[a3 a2 a1 a0], return the Hermite
+coefficients H=~[h0 h1 hu0 hu1]. All coefficients are 3-vectors. Cost is 21
+flops. **/
+static Vec<4,Vec3P> calcHFromA(const Vec<4,Vec3P>& A) {
+ const Vec3P& a3=A[0]; const Vec3P& a2=A[1]; // aliases for beauty
+ const Vec3P& a1=A[2]; const Vec3P& a0=A[3];
+ return Vec<4,Vec3P>(a0, a3+a2+a1+a0, a1, 3*a3+2*a2+a1);
+}
+
+/** Given algebraic coefficients A and a value for the curve parameter u, return
+the point P(u) at that location. Cost is 20 flops. **/
+static Vec3P evalPUsingA(const Vec<4,Vec3P>& A, RealP u) {
+ const RealP u2 = u*u, u3 = u*u2;
+ return u3*A[0] + u2*A[1] + u*A[2] + A[3]; // (vectors)
+}
+/** Given algebraic coefficients A and a value for the curve parameter u, return
+the first derivative Pu(u)=dP/du at that location. Cost is 15 flops. **/
+static Vec3P evalPuUsingA(const Vec<4,Vec3P>& A, RealP u) {
+ return (3*u*u)*A[0] + (2*u)*A[1] + A[2];
+}
+/** Given algebraic coefficients A and a value for the curve parameter u, return
+the second derivative Puu(u)=d2P/du2 at that location. Cost is 10 flops. **/
+static Vec3P evalPuuUsingA(const Vec<4,Vec3P>& A, RealP u) {
+ return (6*u)*A[0] + 2*A[1];
+}
+/** Given algebraic coefficients A and a value for the curve parameter u, return
+the third derivative Puuu(u)=d3P/du3 at that location. The parameter u is
+ignored since the third derivative of a cubic is just a constant. Cost is
+3 flops. **/
+static Vec3P evalPuuuUsingA(const Vec<4,Vec3P>& A, RealP u) {
+ return 6*A[0];
+}
+
+/** Given Hermite coefficients H and a value for the curve parameter u, return
+the point P(u) at that location. Cost is 31 flops. Note that if you need to
+do this for the same curve more than twice, it is cheaper to convert to
+algebraic form using calcAFromH() (24 flops) and then evaluate using A
+(20 flops). **/
+static Vec3P evalPUsingH(const Vec<4,Vec3P>& H, RealP u) {
+ return calcFh(u)*H; // 10 + 3*7 = 31 flops
+}
+/** Given Hermite coefficients H and a value for the curve parameter u, return
+the first derivative Pu(u)=dP/du at that location. Cost is 31 flops. Note that
+if you need to do this for the same curve more than once, it is cheaper to
+convert to algebraic form using calcAFromH() (24 flops) and then evaluate using
+A (15 flops). **/
+static Vec3P evalPuUsingH(const Vec<4,Vec3P>& H, RealP u) {
+ return calcFhu(u)*H; // 10 + 3*7 = 31 flops
+}
+/** Given Hermite coefficients H and a value for the curve parameter u, return
+the second derivative Puu(u)=d2P/du2 at that location. Cost is 27 flops. Note
+that if you need to do this for the same curve more than once, it is cheaper to
+convert to algebraic form using calcAFromH() (24 flops) and then evaluate using
+A (10 flops). **/
+static Vec3P evalPuuUsingH(const Vec<4,Vec3P>& H, RealP u) {
+ return calcFhuu(u)*H; // 6 + 3*7 = 27 flops
+}
+/** Given Hermite coefficients H and a value for the curve parameter u, return
+the third derivative Puuu(u)=d3P/du3 at that location. Cost is 21 flops. Note
+that if you need to do this for the same curve more than once, it is cheaper to
+convert to algebraic form using calcAFromH() (24 flops) and then evaluate using
+A (3 flops). **/
+static Vec3P evalPuuuUsingH(const Vec<4,Vec3P>& H, RealP u) {
+ return calcFhuuu(u)*H; // 0 + 3*7 = 21 flops
+}
+
+/** Obtain the Hermite basis matrix Mh explicitly. This is mostly useful for
+testing since specialized routines can save a lot of CPU time over working
+directly in matrix form. This is a constant matrix so there is no computation
+cost. **/
+static Mat<4,4,P> getMh() {
+ return Mat<4,4,P>( 2, -2, 1, 1,
+ -3, 3, -2, -1,
+ 0, 0, 1, 0,
+ 1, 0, 0, 0 );
+}
+
+/** Form the product of the Hermite basis matrix Mh and a 4-vector, exploiting
+the structure of Mh (which is not symmetric). Cost is 8 flops. **/
+static Vec<4,P> multiplyByMh(const Vec<4,P>& v) {
+ const RealP v0=v[0], v1=v[1], v2=v[2], v3=v[3];
+ const RealP v01 = v0-v1;
+ return Vec<4,P>(2*v01+v2+v3, -3*v01-2*v2-v3, v2, v0);
+}
+
+/** Obtain the inverse inv(Mh) of the Hermite basis matrix explicitly. This is
+mostly useful for testing since specialized routines can save a lot of CPU time
+over working directly in matrix form. This is a constant matrix so there is no
+computation cost. **/
+static Mat<4,4,P> getMhInv() {
+ return Mat<4,4,P>( 0, 0, 0, 1,
+ 1, 1, 1, 1,
+ 0, 0, 1, 0,
+ 3, 2, 1, 0 );
+}
+
+/** Form the product of the inverse Hermite basis matrix inv(Mh) and a
+4-vector, exploiting the structure of inv(Mh) (which is not symmetric).
+Cost is 7 flops. **/
+static Vec<4,P> multiplyByMhInv(const Vec<4,P>& v) {
+ const RealP v0=v[0], v1=v[1], v2=v[2], v3=v[3];
+ return Vec<4,P>(v3, v0+v1+v2+v3, v2, 3*v0+2*v1+v2);
+}
+/**@}**/
+
+private:
+Vec<4,Vec3P> A;
+};
+
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_GEO_CUBIC_HERMITE_CURVE_H_
diff --git a/SimTKmath/Geometry/include/simmath/internal/Geo_LineSeg.h b/SimTKmath/Geometry/include/simmath/internal/Geo_LineSeg.h
new file mode 100644
index 0000000..4740e25
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/Geo_LineSeg.h
@@ -0,0 +1,149 @@
+#ifndef SimTK_SIMMATH_GEO_LINESEG_H_
+#define SimTK_SIMMATH_GEO_LINESEG_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Collects primitive operations involving line segments. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_Sphere.h"
+
+#include <cassert>
+#include <cmath>
+#include <algorithm>
+
+namespace SimTK {
+
+
+//==============================================================================
+// GEO LINESEG
+//==============================================================================
+/** A 3d line segment primitive represented by its end points
+in an unspecified frame, and a collection of line segment-related utility
+methods. We support a one-parameter t representation of the line segment, where
+t=0 gives the first end point and t=1 gives the second end point. **/
+template <class P>
+class SimTK_SIMMATH_EXPORT Geo::LineSeg_ {
+typedef P RealP;
+typedef Vec<3,RealP> Vec3P;
+
+public:
+/** Construct an uninitialized LineSeg object; the end points will
+be garbage (NaN in Debug builds). **/
+LineSeg_() {}
+
+/** Construct a LineSeg with the given end points. When an orientation is
+needed we define the line segment to go from \a e0 to \a e1. **/
+LineSeg_(const Vec3P& e0, const Vec3P& e1)
+{ setEndpoints(e0,e1); }
+
+/** Change the end points of this line segment. **/
+LineSeg_& setEndpoints(const Vec3P& e0, const Vec3P& e1)
+{ e[0]=e0; e[1]=e1; return *this; }
+
+/** Change one end point of this line segment. **/
+LineSeg_& setEndpoint(int which, const Vec3P& p)
+{ assert(which==0 || which==1); e[which] = p; return *this; }
+
+/** Determine whether this line segment is degenerate to a given tolerance,
+meaning that its length is less than or equal to the tolerance. Use a tolerance
+of zero if you want to check only for exact degeneracy. By default the
+tolerance is SignificantReal for the precision in use, about 2e-14 in double,
+9e-7 in float. Cost is 10 flops. **/
+bool isDegenerate(RealP tol = Geo::getDefaultTol<P>()) const
+{ assert(tol >= 0); return calcLengthSqr() <= square(tol); }
+
+/** Get the location of an end point.\ Order is the same as construction. You
+can use operator[] instead for a more compact notation. **/
+const Vec3P& getEndpoint(int which) const
+{ assert(which==0 || which==1); return e[which]; }
+
+/** Get a writable reference to the location of an end point.\ Order is the
+same as construction. You can use operator[] instead for a more compact
+notation. **/
+Vec3P& updEndpoint(int which)
+{ assert(which==0 || which==1); return e[which]; }
+
+/** Access an end point by indexing the line segment. **/
+const Vec3P& operator[](int which) const {return getEndpoint(which);}
+/** Get writable access to an end point by indexing the line segment. **/
+Vec3P& operator[](int which) {return updEndpoint(which);}
+
+/** Calculate the length of this line segment (expensive). Cost is about
+40 flops. **/
+RealP calcLength() const {return (e[1]-e[0]).norm(); }
+
+/** Calculate the square of the length of this line segment (cheap). Cost
+is 8 flops. **/
+RealP calcLengthSqr() const {return (e[1]-e[0]).normSqr(); }
+
+/** Return a point along the line segment given by its coordinate t, where
+t==0 gives end point 0, t==1 gives end point 1 and other values interpolate
+or extrapolate linearly. Note that values outside the range [0,1] are not
+on the line segment. Cost is 10 flops. **/
+Vec3P findPoint(RealP t) const
+{ return t*e[0] + (1-t)*e[1]; }
+
+/** Return the center point of the line segment; this is the same as
+findPoint(1/2) but faster. Cost is 4 flops. **/
+Vec3P findMidpoint() const
+{ return (e[0]+e[1]) / RealP(2); }
+
+/** Calculate a minimal bounding sphere for this line segment. This is the
+sphere whose center is the segment midpoint and whose radius is half the
+segment length, plus a little slop. Cost is about 45 flops. **/
+Sphere_<P> calcBoundingSphere() const
+{ return Geo::Point_<P>::calcBoundingSphere(e[0],e[1]); }
+
+/** Find the distance between this line segment and a point expressed in the
+same frame. Cost is XXX flops. **/
+RealP findDistanceToPoint(const Vec3P& p2) const
+{SimTK_ASSERT_ALWAYS(!"implemented",
+"Geo::LineSeg_::findDistanceToPoint(): Not implemented yet.");
+return Geo::getNaN<P>();}
+
+/** Find the square of the distance between this line segment and a point
+expressed in the same frame. Cost is XXX flops. **/
+RealP findDistanceToPointSqr(const Vec3P& p2) const
+{SimTK_ASSERT_ALWAYS(!"implemented",
+"Geo::LineSeg_::findDistanceToPointSqr(): Not implemented yet.");
+return Geo::getNaN<P>();}
+
+/**@name Line segment-related utilities
+These static methods work with points or collections of points. **/
+/**@{**/
+
+/**@}**/
+
+private:
+Vec3P e[2];
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_GEO_LINESEG_H_
diff --git a/SimTKmath/Geometry/include/simmath/internal/Geo_Point.h b/SimTKmath/Geometry/include/simmath/internal/Geo_Point.h
new file mode 100644
index 0000000..946984c
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/Geo_Point.h
@@ -0,0 +1,474 @@
+#ifndef SimTK_SIMMATH_GEO_POINT_H_
+#define SimTK_SIMMATH_GEO_POINT_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Defines primitive computations involving points. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+
+#include <cassert>
+#include <cmath>
+#include <algorithm>
+
+namespace SimTK {
+
+//==============================================================================
+// GEO POINT
+//==============================================================================
+/** A 3d point primitive represented by a Vec3 from the origin of an unspecified
+frame, and a collection of point-related utility methods. **/
+template <class P>
+class Geo::Point_ {
+typedef P RealP;
+typedef Vec<3,P> Vec3P;
+typedef Mat<3,3,P> Mat33P;
+typedef SymMat<3,P> SymMat33P;
+typedef UnitVec<P,1> UnitVec3P;
+typedef Rotation_<P> RotationP;
+typedef Transform_<P> TransformP;
+
+public:
+/** Construct an uninitialized Point object; the location will be garbage. **/
+Point_() {}
+/** Construct a Point with the given location.\ Also serves as implicit
+conversion from Vec3 to Geo::Point. **/
+Point_(const Vec3P& location) : p(location) {}
+
+/** Change the location of this point. **/
+Point_& setLocation(const Vec3P& location) {p=location; return *this;}
+
+/** Get the location of this Point. **/
+const Vec3P& getLocation() const {return p;}
+
+/** Calculate the distance between this point and another one whose location is
+expressed in the same frame (expensive). Cost is about 30 flops. **/
+RealP calcDistance(const Vec3P& p2) const
+{ return calcDistance(p, p2); }
+
+/** Find the square of the distance between this point and another one whose
+location is expressed in the same frame (cheap). Cost is 8 flops. **/
+RealP findDistanceSqr(const Vec3P& p2) const
+{ return findDistanceSqr(p, p2); }
+
+/**@name Miscellaneous point-related utilities
+These static methods work with points or collections of points. Collections
+of points are represented either as an Array of point locations or as
+an indirect Array of pointers to point locations, which can save a lot of
+copying for large point sets. **/
+/**@{**/
+
+/** Calculate the distance between two points (expensive). Cost is about
+30 flops. **/
+static RealP calcDistance(const Vec3P& p1, const Vec3P& p2)
+{ return std::sqrt(findDistanceSqr(p1,p2)); }
+
+/** Find the square of the distance between two points (cheap). Cost is
+8 flops. **/
+static RealP findDistanceSqr(const Vec3P& p1, const Vec3P& p2)
+{ return (p2-p1).normSqr(); }
+
+/** Find the point midway between two points. Cost is 4 flops. **/
+static Vec3P findMidpoint(const Vec3P& p1, const Vec3P& p2)
+{ return (p1+p2)/2; }
+
+/** Determine whether two points whose locations are known to an accuracy
+\a tol are numerically indistinguishable. We define this to mean that they
+are so close that a perturbation of their measure numbers by no more than tol
+could make them coincident. If a measure number has value x, we define a
+tol-sized perturbation to be max(tol, |x|*tol). We use the default
+tolerance if none is supplied. Cost is about 20 flops.
+ at see Geo::getDefaultTol() **/
+static bool pointsAreNumericallyCoincident(const Vec3P& p1, const Vec3P& p2)
+{
+ return pointsAreNumericallyCoincident(p1,p2,Geo::getDefaultTol<RealP>());
+}
+/** Alternate signature with explicitly-supplied tolerance. **/
+static bool pointsAreNumericallyCoincident
+ (const Vec3P& p1, const Vec3P& p2, RealP tol)
+{
+ const RealP maxcoord = std::max(max(p1.abs()),max(p2.abs())); // ~7 flops
+ const RealP scale = std::max(tol, maxcoord*tol); // 2 flops
+ return findDistanceSqr(p1,p2) < square(scale); // 10 flops
+}
+
+
+/** Given a set of points, find the one that is the furthest in a given
+direction, and return its index and location along that direction. There must
+be at least one point in the set. **/
+SimTK_SIMMATH_EXPORT static void
+findSupportPoint(const Array_<Vec3P>& points, const UnitVec3P& direction,
+ int& most, RealP& mostCoord);
+
+/** Alternate signature taking an array of pointers to points rather than the
+points themselves. **/
+SimTK_SIMMATH_EXPORT static void
+findSupportPointIndirect(const Array_<const Vec3P*>& points,
+ const UnitVec3P& direction,
+ int& most, RealP& mostCoord);
+
+/** Given a set of points, find the two points that are the most extreme along
+a given direction (not necessarily distinct), and return their indices and
+locations along the given direction. There must be at least one point
+in the set. **/
+SimTK_SIMMATH_EXPORT static void
+findExtremePoints(const Array_<Vec3P>& points, const UnitVec3P& direction,
+ int& least, int& most,
+ RealP& leastCoord, RealP& mostCoord);
+
+/** Alternate signature taking an array of pointers to points rather than the
+points themselves. **/
+SimTK_SIMMATH_EXPORT static void
+findExtremePointsIndirect(const Array_<const Vec3P*>& points,
+ const UnitVec3P& direction,
+ int& least, int& most,
+ RealP& leastCoord, RealP& mostCoord);
+
+/** Given a set of points, calculate the centroid (average location) of those
+points. Cost is about 3*n+10 flops for n points. **/
+SimTK_SIMMATH_EXPORT static Vec3P
+calcCentroid(const Array_<Vec3P>& points_F);
+
+/** Alternate signature taking an array of pointers to points rather than the
+points themselves. **/
+SimTK_SIMMATH_EXPORT static Vec3P
+calcCentroidIndirect(const Array_<const Vec3P*>& points_F);
+
+/** Given a set of points, calculate the centroid (average location) and
+covariance matrix of those points. **/
+SimTK_SIMMATH_EXPORT static void
+calcCovariance(const Array_<Vec3P>& points_F,
+ Vec3P& centroid, SymMat33P& covariance);
+
+/** Alternate signature taking an array of pointers to points rather than the
+points themselves. **/
+SimTK_SIMMATH_EXPORT static void
+calcCovarianceIndirect(const Array_<const Vec3P*>& points_F,
+ Vec3P& centroid, SymMat33P& covariance);
+
+/** Given a set of points in an unspecified frame F, find the principal
+component directions describing the distribution of the points in space. The
+result is a frame P with origin at the centroid, x axis along the direction
+of maximum dispersion, y axis along the direction of minimum dispersion, and
+z=x X y. Note that clustering of points affects the directions. **/
+SimTK_SIMMATH_EXPORT static void
+calcPrincipalComponents(const Array_<Vec3P>& points_F,
+ TransformP& X_FP);
+
+/** Alternate signature taking an array of pointers to points rather than the
+points themselves. **/
+SimTK_SIMMATH_EXPORT static void
+calcPrincipalComponentsIndirect(const Array_<const Vec3P*>& points_F,
+ TransformP& X_FP);
+
+/**@}**/
+
+/**@name Axis-aligned bounding box creation
+These static methods create a minimal axis-aligned box that includes all
+of a set of given points. **/
+/**@{**/
+
+/** Given a set of points, find the six points that are the most extreme along
+the axial directions (not necessarily distinct points). Return the indices of
+the extreme points and the locations of the box corners. Note that the corners
+do not necessarily correspond to any points in the set. There must be at least
+one point in the set. **/
+SimTK_SIMMATH_EXPORT static void
+findAxisAlignedExtremePoints(const Array_<Vec3P>& points,
+ int least[3], int most[3],
+ Vec3P& low, Vec3P& high);
+
+/** Alternate signature taking an array of pointers to points rather than the
+points themselves. **/
+SimTK_SIMMATH_EXPORT static void
+findAxisAlignedExtremePointsIndirect(const Array_<const Vec3P*>& points,
+ int least[3], int most[3],
+ Vec3P& low, Vec3P& high);
+
+/** Calculate the smallest axis-aligned bounding box including all n given
+points. Cost is O(n). **/
+SimTK_SIMMATH_EXPORT static Geo::AlignedBox_<P>
+calcAxisAlignedBoundingBox(const Array_<Vec3P>& points,
+ Array_<int>& support);
+
+/** Alternate signature doesn't return support points. **/
+static Geo::AlignedBox_<P>
+calcAxisAlignedBoundingBox(const Array_<Vec3P>& points)
+{ Array_<int> support;
+ return calcAxisAlignedBoundingBox(points,support); }
+
+/** Alternate signature taking an array of pointers to points rather than the
+points themselves. **/
+SimTK_SIMMATH_EXPORT static Geo::AlignedBox_<P>
+calcAxisAlignedBoundingBoxIndirect(const Array_<const Vec3P*>& points,
+ Array_<int>& support);
+
+/** Alternate signature doesn't return support points. **/
+static Geo::AlignedBox_<P>
+calcAxisAlignedBoundingBoxIndirect(const Array_<const Vec3P*>& points)
+{ Array_<int> support;
+ return calcAxisAlignedBoundingBoxIndirect(points,support); }
+
+/**@}**/
+
+/**@name Oriented bounding box creation
+These static methods create a tight-fitting oriented bounding box (OBB) that
+includes all of a set of given points. The OBB is not guaranteed to be minimal
+but will usually be very good. You can optionally obtain the set of support
+points that determined the size of the box. **/
+/**@{**/
+
+/** Given a set of points, find the six points that are the most extreme along
+specified orientation directions (not necessarily distinct points). The points
+are given in an arbitrary frame F. We have an oriented "box" frame B given
+by its orientation in F, R_FB. The origin of the B frame is coincident with
+the F frame. We'll find the points that are the most extreme along the B frame
+axis directions, and we'll also return the corner points <em>in B</em> (that
+is, the points having minimum and maximum x,y,z values in B). Note that the
+corners do not necessarily correspond to any points in the set. If you want to
+know where the corners are in F, just compute R_FB*low_B and R_FB*high_B on
+return. There must be at least one point in the given set. **/
+SimTK_SIMMATH_EXPORT static void
+findOrientedExtremePoints(const Array_<Vec3P>& points_F,
+ const RotationP& R_FB,
+ int least[3], int most[3],
+ Vec3P& low_B, Vec3P& high_B);
+
+/** Alternate signature taking an array of pointers to points rather than the
+points themselves. **/
+SimTK_SIMMATH_EXPORT static void
+findOrientedExtremePointsIndirect(const Array_<const Vec3P*>& points_F,
+ const RotationP& R_FB,
+ int least[3], int most[3],
+ Vec3P& low_B, Vec3P& high_B);
+
+/** Calculate a tight-fitting oriented bounding box (OBB) that includes all
+n given points. The OBB is not guaranteed to be minimal but will usually be
+very good unless you suppress optimization to save runtime. Cost is O(n). **/
+SimTK_SIMMATH_EXPORT static Geo::OrientedBox_<P>
+calcOrientedBoundingBox(const Array_<Vec3P>& points,
+ Array_<int>& support,
+ bool optimize=true);
+
+/** Alternate signature doesn't return support points. **/
+static Geo::OrientedBox_<P>
+calcOrientedBoundingBox(const Array_<Vec3P>& points)
+{ Array_<int> support;
+ return calcOrientedBoundingBox(points,support); }
+
+/** Alternate signature taking an array of pointers to points rather than the
+points themselves. **/
+SimTK_SIMMATH_EXPORT static Geo::OrientedBox_<P>
+calcOrientedBoundingBoxIndirect(const Array_<const Vec3P*>& points,
+ Array_<int>& support,
+ bool optimize=true);
+
+/** Alternate signature doesn't return support points. **/
+static Geo::OrientedBox_<P>
+calcOrientedBoundingBoxIndirect(const Array_<const Vec3P*>& points,
+ bool optimize=true)
+{ Array_<int> support;
+ return calcOrientedBoundingBoxIndirect(points,support,optimize); }
+/**@}**/
+
+/**@name Sphere-related utilities
+These static methods work with spheres or collections of spheres.
+
+<h3>Bounding spheres</h3>
+Bounding sphere methods calculate the smallest sphere around a given set of
+points such that no point is outside the sphere, although some may be on
+its surface. How many and specifically which points were actually used to
+define the sphere can be returned; there will never be more than 4. This
+information is primarily used to construct bounding sphere algorithms; users
+normally just need the sphere so can use the simpler signatures.
+
+Bounding sphere methods address roundoff by stretching the sphere enough to
+guarantee that all points are strictly inside the sphere and that later tests
+can produce only false positives not false negatives which might cause a
+contact to be missed. To do that we have to account not just for machine
+precision, but for relative errors caused by spheres of large radius or
+spheres that are located far from the origin. These adjustments ensure that
+if a test point appears numerically to be outside the sphere, it really cannot
+contact anything that is inside the sphere.
+
+We use a bounding sphere method due originally to Emo Welzl that computes a
+near-perfect minimal bounding sphere around a set of points with expected O(n)
+run time. Our implementation has been extensively modified to deal with
+singular cases so you do not have to precondition the points before asking
+for their bounding sphere.
+
+We also provide a conventional fast and dumb approximate bounding sphere using
+Ritter's method as described in Christer Ericson's book. This is mostly
+useful for testing the Welzl method's accuracy and performance and should
+not generally be used. A Welzl bounding sphere should never be larger than
+a Ritter sphere and should normally be substantially smaller. **/
+/**@{**/
+
+/** Create a tiny bounding sphere around a single point. The center is the
+point and the radius is tiny but non-zero. **/
+static Sphere_<P> calcBoundingSphere(const Vec3P& p)
+{ return Sphere_<P>(p, 0).stretchBoundary(); }
+
+
+/** Create a minimal bounding sphere around two points. Some care is taken
+to avoid roundoff problems if the points are far from the origin or very
+close together. **/
+static Sphere_<P> calcBoundingSphere(const Vec3P& p0, const Vec3P& p1) {
+ Array_<int> which;
+ return calcBoundingSphere(p0,p1,which);
+}
+
+
+/** Create a minimal bounding sphere around three points. **/
+static Sphere_<P> calcBoundingSphere
+ (const Vec3P& p0, const Vec3P& p1, const Vec3P& p2) {
+ Array_<int> which;
+ return calcBoundingSphere(p0,p1,p2,false,which);
+}
+
+/** Create a minimal bounding sphere around four points. **/
+static Sphere_<P> calcBoundingSphere
+ (const Vec3P& p0, const Vec3P& p1, const Vec3P& p2, const Vec3P& p3) {
+ Array_<int> which;
+ return calcBoundingSphere(p0,p1,p2,p3,false,which);
+}
+
+/** Create a minimal bounding sphere around a collection of n points.
+This has expected O(n) performance and usually yields a near-perfect
+bounding sphere. **/
+static Sphere_<P> calcBoundingSphere(const Array_<Vec3P>& points) {
+ Array_<int> which;
+ return calcBoundingSphere(points, which);
+}
+
+/** This signature takes an std::vector rather than a SimTK::Array_; no
+extra copying is required. **/
+static Sphere_<P>
+calcBoundingSphere(const std::vector<Vec3P>& points) {
+ return calcBoundingSphere // no copy done here
+ (ArrayViewConst_<Vec3P>(points));
+}
+
+/** Create a minimal bounding sphere around a collection of n points, given
+indirectly as an array of pointers. This has expected O(n) performance and
+yields a perfect bounding sphere. **/
+static Sphere_<P> calcBoundingSphereIndirect(const Array_<const Vec3P*>& points) {
+ Array_<int> which;
+ return calcBoundingSphereIndirect(points, which);
+}
+
+/** This signature takes an std::vector rather than a SimTK::Array_; no
+extra copying is required. **/
+static Sphere_<P>
+calcBoundingSphere(const std::vector<const Vec3P*>& points) {
+ return calcBoundingSphereIndirect // no copy done here
+ (ArrayViewConst_<const Vec3P*>(points));
+}
+
+/** Create one-point bounding sphere and return the (trivial) support
+point, of which there is always one. **/
+static Sphere_<P> calcBoundingSphere(const Vec3P& p0, Array_<int>& which)
+{ which.clear(); which.push_back(0);
+ return Sphere_<P>(p0,0).stretchBoundary(); }
+
+/** Create a minimum sphere around two points. The center is the
+midpoint, and the radius is roughly half the distance between the points,
+possibly expanded in the face of roundoff to ensure that neither point tests
+outside. There will be two support points for the circle unless the given
+points are very close to one another. In that case, we treat these as a single
+point and report in \a which that only 1 point was used to define the sphere.
+Points far from the origin will produce a larger sphere because of roundoff.
+Cost is about 45 flops. **/
+SimTK_SIMMATH_EXPORT static Sphere_<P>
+calcBoundingSphere(const Vec3P& p0, const Vec3P& p1, Array_<int>& which);
+
+/** Create a minimum sphere around three points. There can be 1, 2, or 3
+support points returned in \a which. You can optionally force use of the
+3-point circumsphere, which will not always be minimal. Even if
+\a forceCircumsphere is set \c true, if the points are
+singular (coincident, collinear, coplanar) then it may not be
+possible to generate a circumsphere and fewer support points will be used. **/
+SimTK_SIMMATH_EXPORT static Sphere_<P>
+calcBoundingSphere(const Vec3P& p0, const Vec3P& p1, const Vec3P& p2,
+ bool forceCircumsphere, Array_<int>& which);
+
+/** Create a minimum sphere around four points. There can be 1, 2, 3, or 4
+support points returned in \a which. You can optionally force use of the
+4-point circumsphere, which will not always be minimal. Even if
+\a forceCircumsphere is set \c true, if the points are
+singular (coincident, collinear, coplanar, cospherical) then it may not be
+possible to generate a circumsphere and fewer support points will be used. **/
+SimTK_SIMMATH_EXPORT static Sphere_<P>
+calcBoundingSphere(const Vec3P& p0, const Vec3P& p1, const Vec3P& p2,
+ const Vec3P& p3, bool forceCircumsphere, Array_<int>& which);
+
+/** Create an optimal minimum sphere around a collection of n points. This has
+expected O(n) performance and yields a near-perfect minimum sphere. There can be
+1, 2, 3, or 4 support points used to define the sphere and \a which reports
+which of the input points were used. **/
+SimTK_SIMMATH_EXPORT static Sphere_<P>
+calcBoundingSphere(const Array_<Vec3P>& points, Array_<int>& which);
+
+/** Alternate signature works with an array of pointers to points. **/
+SimTK_SIMMATH_EXPORT static Sphere_<P>
+calcBoundingSphereIndirect(const Array_<const Vec3P*>& points,
+ Array_<int>& which);
+
+/** Calculate an approximate bounding sphere.\ You should normally use
+calcBoundingSphere() which will give a smaller sphere. **/
+SimTK_SIMMATH_EXPORT static Sphere_<P>
+calcApproxBoundingSphere(const Array_<Vec3P>& points);
+
+/** This signature takes an std::vector rather than a SimTK::Array_; no
+extra copying is required. **/
+static Sphere_<P>
+calcApproxBoundingSphere(const std::vector<Vec3P>& points) {
+ return calcApproxBoundingSphere // no copy done here
+ (ArrayViewConst_<Vec3P>(points));
+}
+
+/** Alternate signature works with an array of pointers to points. **/
+SimTK_SIMMATH_EXPORT static Sphere_<P>
+calcApproxBoundingSphereIndirect(const Array_<const Vec3P*>& points);
+
+/** This signature takes an std::vector rather than a SimTK::Array_; no
+extra copying is required. **/
+static Sphere_<P>
+calcApproxBoundingSphereIndirect(const std::vector<const Vec3P*>& points) {
+ return calcApproxBoundingSphereIndirect // no copy done here
+ (ArrayViewConst_<const Vec3P*>(points));
+}
+/**@}**/
+
+
+private:
+Vec3P p;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_GEO_POINT_H_
diff --git a/SimTKmath/Geometry/include/simmath/internal/Geo_Sphere.h b/SimTKmath/Geometry/include/simmath/internal/Geo_Sphere.h
new file mode 100644
index 0000000..f4d0aaa
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/Geo_Sphere.h
@@ -0,0 +1,125 @@
+#ifndef SimTK_SIMMATH_GEO_SPHERE_H_
+#define SimTK_SIMMATH_GEO_SPHERE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Defines primitive operations on spheres. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+
+#include <cassert>
+#include <cmath>
+#include <algorithm>
+
+namespace SimTK {
+
+
+//==============================================================================
+// GEO SPHERE
+//==============================================================================
+/** A geometric primitive representing a sphere by its radius and center
+point, and a collection of sphere-related utility methods. **/
+template <class P>
+class Geo::Sphere_ {
+typedef P RealP;
+typedef Vec<3,P> Vec3P;
+typedef Vec<4,P> Vec4P;
+public:
+/** Construct an uninitialized Sphere object; the center point and radius
+will be garbage. **/
+Sphere_() {}
+/** Construct a sphere from its center location and radius. **/
+Sphere_(const Vec3P& center, RealP radius)
+: cr(center[0], center[1], center[2], radius) {assert(radius>=0);}
+/** Change the radius of this sphere. **/
+Sphere_& setRadius(RealP radius)
+{ assert(radius>=0); cr[3]=radius; return *this; }
+/** Change the center location of this sphere. **/
+Sphere_& setCenter(const Vec3P& center)
+{ Vec3P::updAs(&cr[0])=center; return *this; }
+
+/** Modify this sphere to scale its radius by a fractional amount f,
+that is we set radius to f*radius.
+ at return A reference to this now-resized sphere. **/
+Sphere_& scaleBy(RealP f)
+{ setRadius(f*getRadius()); return *this; }
+
+/** Stretch this sphere in place by a small amount to ensure that there will
+be no roundoff problems if this is used as a bounding sphere. The amount to
+stretch depends on the default tolerance for this precision, the radius, and
+the position of the sphere in space. A very large sphere, or a sphere that is
+very far from the origin, must be stretched more than a small one at the
+origin. Cost is 6 flops.
+ at see Geo class for tolerance information. **/
+Sphere_& stretchBoundary() {
+ const RealP tol = Geo::getDefaultTol<P>();
+ const RealP maxdim = max(getCenter().abs());
+ const RealP scale = std::max(maxdim, getRadius());
+ updRadius() += std::max(scale*Geo::getEps<P>(), tol);
+ return *this;
+}
+
+/** Return the volume of this sphere (4/3 pi r^3). **/
+RealP findVolume() const
+{ return (RealP(4)/3) * NTraits<P>::getPi() * cube(getRadius()); }
+/** Return the surface area of this sphere (4 pi r^2). **/
+RealP findArea() const
+{ return 4 * NTraits<P>::getPi() * square(getRadius()); }
+
+/** Return true if a given point is strictly outside this sphere. Just touching
+the sphere does not qualify. **/
+bool isPointOutside(const Vec3P& p) const {
+ const RealP r2 = Geo::Point_<P>::findDistanceSqr(p, getCenter());
+ return r2 > square(getRadius());
+}
+/** Return true if a given point is more than a given tolerance outside the
+sphere. **/
+bool isPointOutside(const Vec3P& p, RealP tol) const {
+ assert(tol >= 0);
+ const RealP r2 = Geo::Point_<P>::findDistanceSqr(p, getCenter());
+ return r2 > square(getRadius()+tol);
+}
+/** Get the location of the sphere's center point. **/
+const Vec3P& getCenter() const {return Vec3P::getAs(&cr[0]);}
+/** Get a writable reference to the sphere's center point. **/
+Vec3P& updCenter() {return Vec3P::updAs(&cr[0]);}
+/** Get the sphere's radius. **/
+RealP getRadius() const {return cr[3];}
+/** Get a writable reference to the sphere's radius. **/
+RealP& updRadius() {return cr[3];}
+
+
+private:
+// Store together to make sure the compiler doesn't introduce any padding.
+// cr[0..2] is the center point, cr[3] is the radius
+Vec4P cr;
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_GEO_SPHERE_H_
diff --git a/SimTKmath/Geometry/include/simmath/internal/Geo_Triangle.h b/SimTKmath/Geometry/include/simmath/internal/Geo_Triangle.h
new file mode 100644
index 0000000..c2ce987
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/Geo_Triangle.h
@@ -0,0 +1,173 @@
+#ifndef SimTK_SIMMATH_GEO_TRIANGLE_H_
+#define SimTK_SIMMATH_GEO_TRIANGLE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Defines primitive operations on triangles. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_Point.h"
+#include "simmath/internal/Geo_LineSeg.h"
+
+#include <cassert>
+#include <cmath>
+#include <algorithm>
+
+namespace SimTK {
+
+//==============================================================================
+// GEO TRIANGLE
+//==============================================================================
+/** A geometric primitive representing a triangle by its vertices as points
+in some unspecified frame, and a collection of triangle-related utility
+methods. We support a u-v barycentric parameterization for the triangle. **/
+template <class P>
+class Geo::Triangle_ {
+typedef P RealP;
+typedef Vec<2,P> Vec2P;
+typedef Vec<3,P> Vec3P;
+typedef UnitVec<P,1> UnitVec3P;
+public:
+/** Construct an uninitialized Triangle object; the vertices will
+be garbage. **/
+Triangle_() {}
+/** Construct a triangle from its vertices. **/
+Triangle_(const Vec3P& v0, const Vec3P& v1, const Vec3P& v2)
+{ setVertices(v0,v1,v2); }
+/** Construct a triangle from vertices stored in array which is presumed
+to contain at least three points. **/
+explicit Triangle_(const Vec3P* vertices)
+{ setVertices(vertices); }
+/** Construct a triangle from an indirect list of vertices stored in array
+which is presumed to contain at least three pointers to points. **/
+explicit Triangle_(const Vec3P** vertexPointers)
+{ setVertices(*vertexPointers[0], *vertexPointers[1], *vertexPointers[2]); }
+
+/** Change one vertex of this triangle. **/
+Triangle_& setVertex(int i, const Vec3P& p)
+{ assert(0<=i && i<3); v[i] = p; return *this; }
+/** Change the vertices of this triangle. **/
+Triangle_& setVertices(const Vec3P& v0, const Vec3P& v1, const Vec3P& v2)
+{ v[0]=v0; v[1]=v1; v[2]=v2; return *this; }
+/** Change the vertices of this triangle, taking the new ones from an array
+which is presumed to contain at least three points. **/
+Triangle_& setVertices(const Vec3P* vertices)
+{ v[0]=vertices[0]; v[1]=vertices[1]; v[2]=vertices[2]; return *this; }
+
+/** Access a vertex by index.\ Order is the same as construction. You can use
+operator[] instead for a more compact notation. **/
+const Vec3P& getVertex(int i) const
+{ SimTK_INDEXCHECK(i,3,"Geo::Triangle_::getVertex()");
+ return v[i]; }
+/** Get writable access to a vertex by index.\ Order is the same as
+construction. You can use operator[] instead for a more compact notation.**/
+Vec3P& updVertex(int i)
+{ SimTK_INDEXCHECK(i,3,"Geo::Triangle_::updVertex()");
+ return v[i]; }
+
+/** Access a vertex by indexing the triangle. **/
+const Vec3P& operator[](int i) const {return getVertex(i);}
+/** Get writable access to a vertex by indexing the triangle. **/
+Vec3P& operator[](int i) {return updVertex(i);}
+
+/** Return a LineSeg_ containing an edge of this triangle, with edges numbered
+in a counterclockwise direction so that edge0 is v0v1, edge1 is v1v2, and
+edge2 is v2v0. **/
+LineSeg_<P> getEdge(int i) const
+{ SimTK_INDEXCHECK(i,3,"Geo::Triangle_::getEdge()");
+ return LineSeg_<P>(v[i],v[(i+1)%3]); }
+
+/** Return a point on the triangle's face given by its (u,v) coordinates.
+Cost is 17 flops. **/
+Vec3P findPoint(const Vec2P& uv) const
+{ return uv[0]*v[0] + uv[1]*v[1] + (1-uv[0]-uv[1])*v[2]; }
+
+/** Return the centroid point on the triangle's face; this is the same as
+findPoint(1/3,1/3) but faster. Cost is 9 flops. **/
+Vec3P findCentroid() const
+{ return (v[0]+v[1]+v[2]) / RealP(3); }
+
+/** Calculate the unit normal to the triangle face taking the vertices in
+counterclockwise order. Cost is about 50 flops. **/
+UnitVec3P calcUnitNormal() const
+{ return UnitVec3P((v[1]-v[0]) % (v[2]-v[0])); }
+
+/** Calculate the smallest bounding sphere enclosing the three vertices of
+this triangle. We guarantee that no vertex is outside the sphere, but for
+numerical reasons it is possible for the sphere to be a little too big. **/
+Sphere_<P> calcBoundingSphere() const
+{ return Geo::Point_<P>::calcBoundingSphere(v[0],v[1],v[2]); }
+
+/** Return the area of this triangle. Cost is about 40 flops. **/
+RealP calcArea() const
+{ return ((v[1]-v[0]) % (v[2]-v[0])).norm() / 2; }
+
+/** Return the square of the area of this triangle. Cost is 23 flops. **/
+RealP calcAreaSqr() const
+{ return ((v[1]-v[0]) % (v[2]-v[0])).normSqr() / 4; }
+
+/** Given a location in space, find the point of this triangular face that
+is closest to that location. If the answer is not unique then one of the
+equidistant points is returned. **/
+Vec3P findNearestPoint(const Vec3P& position, Vec2P& uv) const
+{SimTK_ASSERT_ALWAYS(!"implemented",
+"Geo::Triangle_::findNearestPoint(): Not implemented yet.");
+return Vec3P();}
+
+/** Determine whether a given ray intersects this triangle. TODO: is a
+perfect hit on the boundary an intersection? **/
+bool intersectsRay(const Vec3P& origin, const UnitVec3P& direction,
+ RealP& distance, Vec2P& uv) const
+{SimTK_ASSERT_ALWAYS(!"implemented",
+"Geo::Triangle_::intersectsRay(): Not implemented yet."); return false;}
+
+/** Determine yes/no whether this triangle overlaps another one. Note that
+exactly touching is not overlapping. **/
+SimTK_SIMMATH_EXPORT bool overlapsTriangle(const Triangle_<P>& other) const;
+/** Determine whether this triangle intersects another one, and if so then
+if they are not coplanar return the line segment representing their
+intersection. If the triangles are coplanar and intersect we'll return true
+and set \a isCoplanar true, but not return a line segment. Note that the
+triangles may meet at a point so the line segment may be degenerate in any
+case. **/
+SimTK_SIMMATH_EXPORT bool intersectsTriangle(const Triangle_<P>& other, LineSeg_<P>& seg,
+ bool& isCoplanar) const;
+
+/**@name Triangle-related utilities
+These static methods work with triangles or collections of triangles. **/
+/**@{**/
+
+/**@}**/
+
+private:
+// Vertices in the order they were supplied in the constructor.
+Vec3P v[3];
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_GEO_TRIANGLE_H_
diff --git a/SimTKmath/Geometry/include/simmath/internal/Geodesic.h b/SimTKmath/Geometry/include/simmath/internal/Geodesic.h
new file mode 100644
index 0000000..d23225c
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/Geodesic.h
@@ -0,0 +1,318 @@
+#ifndef SimTK_SIMMATH_GEODESIC_H_
+#define SimTK_SIMMATH_GEODESIC_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Ian Stavness, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+This file defines the Geodesic class. **/
+
+//==============================================================================
+// GEODESIC CLASS
+//==============================================================================
+
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+
+namespace SimTK {
+
+/** This class stores a geodesic curve after it has been determined. The curve
+is represented by a discrete set of Frenet frames along its arc length, with
+each frame providing a point on the curve, the tangent along the curve
+there, surface normal, and binormal. The number of points is determined by
+the accuracy to which the geodesic was calculated and the complexity of
+the surface. For analytical geodesics, we'll sample the curve to generate
+enough points for visualization but the accuracy will be machine precision.
+The first point is at arclength s=0, the last is at the
+actual length of the geodesic. We call the first point P and the last
+point Q, and the geodesic arc length increases from P to Q, with the
+tangent always pointing in the direction of increasing arc length. **/
+class SimTK_SIMMATH_EXPORT Geodesic {
+public:
+ /** Construct an empty geodesic. **/
+ Geodesic() {clear();}
+
+ int getNumPoints() const {return (int)frenetFrames.size(); }
+
+ /** Frenet frame of geodesic at arc length s:
+ - origin: point on geodesic at s
+ - z axis: outward surface (unit) normal n at s
+ - y axis: unit tangent t at s in direction of increasing arc length
+ - x axis: unit binormal at s: b=t X n (x=y X z)
+
+ Note that this convention is different from that of a curve, because
+ our normal vector always points outwards from the surface. **/
+ const Array_<Transform>& getFrenetFrames() const {return frenetFrames;}
+ Array_<Transform>& updFrenetFrames() {return frenetFrames;}
+ void addFrenetFrame(const Transform& Kf) {frenetFrames.push_back(Kf);}
+
+ Array_<Real>& updArcLengths() {return arcLengths;}
+ const Array_<Real>& getArcLengths() const {return arcLengths;}
+ void addArcLength(Real s) {arcLengths.push_back(s);}
+
+ Array_<Real>& updCurvatures() {return curvature;}
+ const Array_<Real>& getCurvatures() const {return curvature;}
+ void addCurvature(Real kappa) {curvature.push_back(kappa);}
+
+ Array_<Vec2>& updDirectionalSensitivityPtoQ()
+ { return directionalSensitivityPtoQ; }
+ const Array_<Vec2>& getDirectionalSensitivityPtoQ() const
+ { return directionalSensitivityPtoQ; }
+ void addDirectionalSensitivityPtoQ(const Vec2& jP) {
+ directionalSensitivityPtoQ.push_back(jP);
+ }
+
+ Array_<Vec2>& updDirectionalSensitivityQtoP()
+ { return directionalSensitivityQtoP; }
+ const Array_<Vec2>& getDirectionalSensitivityQtoP() const
+ { return directionalSensitivityQtoP; }
+ void addDirectionalSensitivityQtoP(const Vec2& jQ) {
+ directionalSensitivityQtoP.push_back(jQ);
+ }
+
+ Array_<Vec2>& updPositionalSensitivityPtoQ()
+ { return positionalSensitivityPtoQ; }
+ const Array_<Vec2>& getPositionalSensitivityPtoQ() const
+ { return positionalSensitivityPtoQ; }
+ void addPositionalSensitivityPtoQ(const Vec2& jtP) {
+ positionalSensitivityPtoQ.push_back(jtP);
+ }
+
+ Array_<Vec2>& updPositionalSensitivityQtoP()
+ { return positionalSensitivityQtoP; }
+ const Array_<Vec2>& getPositionalSensitivityQtoP() const
+ { return positionalSensitivityQtoP; }
+ void addPositionalSensitivityQtoP(const Vec2& jtQ) {
+ positionalSensitivityQtoP.push_back(jtQ);
+ }
+
+ void setTorsionAtP(Real tauP) {torsionAtP = tauP;}
+ void setTorsionAtQ(Real tauQ) {torsionAtQ = tauQ;}
+ void setBinormalCurvatureAtP(Real muP) {binormalCurvatureAtP = muP;}
+ void setBinormalCurvatureAtQ(Real muQ) {binormalCurvatureAtQ = muQ;}
+
+ /** Return the total arc length of this geodesic curve. Will return zero
+ if no curve has been calculated. **/
+ Real getLength() const {return arcLengths.empty() ? 0 : arcLengths.back();}
+
+ /** Given the time derivatives of the surface coordinates of P and Q,
+ calculate the rate of change of length of this geodesic. Only the
+ components of \a xdotP and \a xdotQ along the curve tangent directions
+ can affect its length; moves in the normal or binormal direction leave
+ the length unchanged. **/
+ Real calcLengthDot(const Vec3& xdotP, const Vec3& xdotQ) const
+ { return ~xdotQ*getTangentQ() - ~xdotP*getTangentP(); }
+
+ /** Return the location on the surface of the geodesic's starting point P,
+ measured and expressed in the surface frame S. **/
+ const Vec3& getPointP() const {return frenetFrames.front().p();}
+ /** Return the location on the surface of the geodesic's ending point Q,
+ measured and expressed in the surface frame S. **/
+ const Vec3& getPointQ() const {return frenetFrames.back().p();}
+
+ /** Return the surface outward unit normal at P, which is aligned
+ with the curve normal there but will have opposite sign if the
+ geodesic curvature is positive at P. **/
+ const UnitVec3& getNormalP() const {return frenetFrames.front().z();}
+ /** Return the surface outward unit normal at Q, which is aligned
+ with the curve normal there but will have opposite sign if the
+ geodesic curvature is positive at Q. **/
+ const UnitVec3& getNormalQ() const {return frenetFrames.back().z();}
+
+ /** Return the unit tangent to the geodesic at P, pointing in the
+ direction of increasing arc length parameters (i.e., towards Q). **/
+ const UnitVec3& getTangentP() const {return frenetFrames.front().y();}
+ /** Return the unit tangent to the geodesic at Q, pointing in the
+ direction of increasing arc length parameters (i.e., away from P). **/
+ const UnitVec3& getTangentQ() const {return frenetFrames.back().y();}
+
+ /** Return the unit binormal vector to the curve at P, defined as
+ bP = tP X nP. **/
+ const UnitVec3& getBinormalP() const {return frenetFrames.front().x();}
+ /** Return the unit binormal vector to the curve at Q, defined as
+ bQ = tQ X nQ. **/
+ const UnitVec3& getBinormalQ() const {return frenetFrames.back().x();}
+
+ /** Return the geodesic normal curvature at P, defined to be positive when
+ the surface is convex in the curve tangent direction at P, negative if the
+ surface is concave in that direction. This is a scalar
+ kappaP=-dot(DtP/ds,nP). Note that the geodesic curvature is
+ the same as the surface curvature in the curve tangent direction. **/
+ Real getCurvatureP() const {return curvature.front();}
+ /** Return the geodesic normal curvature at Q, defined to be positive when
+ the surface is convex in the curve tangent direction at Q, negative if the
+ surface is concave in that direction. This is a scalar
+ kappaQ=-dot(DtQ/ds,nQ). Note that the geodesic curvature is
+ the same as the surface curvature in the curve tangent direction, and
+ remember that the curve tangent at Q points \e away from P, that is, off
+ the end of the geodesic. **/
+ Real getCurvatureQ() const {return curvature.back();}
+
+ /** Return the geodesic torsion at P, that is, the twisting of the
+ Frenet frame as you move along the tangent towards Q. The sign follows
+ the right hand rule with your thumb directed along the tangent. This
+ is a scalar tauP=-dot(DbP/Ds,nP). **/
+ Real getTorsionP() const {return torsionAtP;}
+ /** Return the geodesic torsion at Q, that is, the twisting of the
+ Frenet frame as you move along the tangent away from P, that is, off the
+ end of the geodesic. The sign follows the right hand rule with your thumb
+ directed along the tangent. This is a scalar tauQ=-dot(DbQ/Ds,nQ). **/
+ Real getTorsionQ() const {return torsionAtQ;}
+
+ /** Return the \e surface curvature in the binormal direction at P; don't
+ confuse this with the geodesic torsion at P. Surface curvature in a
+ direction is a property of the surface independent of any curve. **/
+ Real getBinormalCurvatureP() const {return binormalCurvatureAtP;}
+ /** Return the \e surface curvature in the binormal direction at Q; don't
+ confuse this with the geodesic torsion at Q. Surface curvature in a
+ direction is a property of the surface independent of any curve. **/
+ Real getBinormalCurvatureQ() const {return binormalCurvatureAtQ;}
+
+ /** Return jP, the Jacobi field term giving the sensitivity of the P end
+ of the geodesic to changes in tangent direction at the Q end, assuming
+ the geodesic length is fixed. This is a scalar jP=dot(DP/DthetaQ,bP) where
+ thetaQ is a right-hand-rule rotation about the normal nQ at Q. Caution:
+ jP and jQ have opposite signs. **/
+ Real getJacobiP() const {return directionalSensitivityQtoP.front()[0];}
+ /** Return jQ, the Jacobi field term giving the sensitivity of the Q end
+ of the geodesic to changes in tangent direction at the P end, assuming
+ the geodesic length is fixed. This is a scalar jQ=-dot(DQ/DthetaP,bQ)
+ where thetaP is a right-hand-rule rotation about the normal nP at P.
+ Note that jQ has the opposite sign from jP. **/
+ Real getJacobiQ() const {return directionalSensitivityPtoQ.back()[0];}
+
+ /** Return the derivative of jP with respect to s, the arc length of the
+ geodesic (which always runs from P to Q). That is jPDot = d/ds jP. **/
+ // Note sign change here -- we calculated this by integrating backwards
+ // so the arc length we used had the opposite sign from the true arc
+ // length parameter.
+ Real getJacobiPDot() const {return -directionalSensitivityQtoP.front()[1];}
+ /** Return the derivative of jQ with respect to s, the arc length of the
+ geodesic. That is, jQDot = d/ds jQ. **/
+ Real getJacobiQDot() const {return directionalSensitivityPtoQ.back()[1];}
+
+ // XXX testing
+ Real getJacobiTransP() const {return positionalSensitivityQtoP.front()[0];}
+ Real getJacobiTransQ() const {return positionalSensitivityPtoQ.back()[0];}
+ Real getJacobiTransPDot() const {return -positionalSensitivityQtoP.front()[1];}
+ Real getJacobiTransQDot() const {return positionalSensitivityPtoQ.back()[1];}
+
+ /** Clear the data in this geodesic, returning it to its default-constructed
+ state, although memory remains allocated. **/
+ void clear() {
+ arcLengths.clear();
+ frenetFrames.clear();
+ directionalSensitivityPtoQ.clear();
+ directionalSensitivityQtoP.clear();
+ positionalSensitivityPtoQ.clear();
+ positionalSensitivityQtoP.clear();
+ curvature.clear();
+ torsionAtP = torsionAtQ = NaN;
+ binormalCurvatureAtP = binormalCurvatureAtQ = NaN;
+ convexFlag = shortestFlag = false;
+ initialStepSizeHint = achievedAccuracy = NaN;
+ }
+
+ void setIsConvex(bool isConvex) {convexFlag = isConvex;}
+ void setIsShortest(bool isShortest) {shortestFlag = isShortest;}
+ void setInitialStepSizeHint(Real sz) {initialStepSizeHint=sz;}
+ void setAchievedAccuracy(Real acc) {achievedAccuracy=acc;}
+
+ bool isConvex() const {return convexFlag;}
+ bool isShortest() const {return shortestFlag;}
+ Real getInitialStepSizeHint() const {return initialStepSizeHint;}
+ Real getAchievedAccuracy() const {return achievedAccuracy;}
+
+ void dump(std::ostream& o) const;
+
+private:
+ // All these arrays are the same length when the geodesic is complete.
+ Array_<Real> arcLengths; // arc length coord corresponding to point
+ Array_<Transform> frenetFrames; // see above for more info
+ Array_<Vec2> directionalSensitivityPtoQ; // jQ and jQdot
+ Array_<Vec2> directionalSensitivityQtoP; // jP and -jPdot
+ Array_<Vec2> positionalSensitivityPtoQ; // jtQ and jtQdot
+ Array_<Vec2> positionalSensitivityQtoP; // jtP and -jtPdot
+ Array_<Real> curvature; // normal curvature kappa in tangent direction
+ // These are only calculated at the end points.
+ Real torsionAtP, torsionAtQ; // torsion tau (only at ends)
+ Real binormalCurvatureAtP, binormalCurvatureAtQ;
+
+ // This flag is set true if curvature[i]>=0 for all i.
+ bool convexFlag; // is this geodesic over a convex surface?
+
+ bool shortestFlag; // XXX is this geodesic the shortest one of the surface?
+ Real initialStepSizeHint; // the initial step size to be tried when integrating this geodesic
+ Real achievedAccuracy; // the accuracy to which this geodesic curve has been calculated
+};
+
+
+/**
+ * This class generates decoration (line segments) for a geodesic curve
+ */
+class GeodesicDecorator : public DecorationGenerator {
+public:
+ GeodesicDecorator(const Geodesic& geod, const Vec3& color) :
+ m_geod(geod), m_color(color) { }
+
+ virtual void generateDecorations(const State& state,
+ Array_<DecorativeGeometry>& geometry) {
+// m_system.realize(state, Stage::Position);
+
+ // draw connected line segments from pts
+ const Array_<Transform>& Kfs = m_geod.getFrenetFrames();
+ Vec3 prevPt;
+ for (int i = 0; i < (int) Kfs.size(); ++i) {
+ Vec3 cur = Kfs[i].p();
+ if (i > 0) {
+ geometry.push_back(
+ DecorativeLine(prevPt, cur)
+ .setColor(m_color)
+ .setLineThickness(3));
+ }
+ prevPt = cur;
+
+ geometry.push_back(DecorativeFrame(Real(.2)).setTransform(Kfs[i]));
+ }
+ }
+
+private:
+ const Geodesic& m_geod;
+ const Vec3& m_color;
+};
+
+
+
+
+/**
+ * This class stores options for calculating geodesics
+ */
+class GeodesicOptions {
+ // XXX stub
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_GEODESIC_H_
diff --git a/SimTKmath/Geometry/include/simmath/internal/GeodesicIntegrator.h b/SimTKmath/Geometry/include/simmath/internal/GeodesicIntegrator.h
new file mode 100644
index 0000000..ba975bb
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/GeodesicIntegrator.h
@@ -0,0 +1,402 @@
+#ifndef SimTK_SIMMATH_GEODESIC_INTEGRATOR_H_
+#define SimTK_SIMMATH_GEODESIC_INTEGRATOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * Contains a stripped-down low overhead numerical integrator for use with
+ * small, fixed-sized sets of Differential-Algebraic equations such as arise
+ * when computing geodesics over smooth surfaces.
+ */
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+
+namespace SimTK {
+
+/** This is a stripped-down numerical integrator for small ODE or DAE problems
+whose size is known at compile time, with no provision for discrete variables,
+event detection, or interpolation. You cannot use this integrator to advance
+a Simbody System; see Integrator instead. Everything is defined in this header
+file so that the integration can proceed with virtually no overhead. Templates
+are used rather than run-time polymorphism, so there are no virtual function
+calls. The system of equations is given as a template object that must
+implement particular methods which the compiler may inline if they are simple
+enough.
+
+<h3>Class Equations</h3>
+This integrator is instantiated with a class that encapsulates the system of
+equations to be solved, and must provide compile time constants and methods
+with the following signatures:
+ at code
+class MyEquations {
+public:
+ enum { NQ=5, // number of 2nd order equations
+ NC=3, // total number of position & velocity constraints
+ N = 2*NQ }; // total number of states y
+
+ // Calculate state derivatives ydot given time and state y.
+ void calcDerivs(Real t, const Vec<N>& y, Vec<N>& ydot) const;
+
+ // Calculate amount by which the given time and state y violate the
+ // constraints and return the constraint errors in cerr.
+ void calcConstraintErrors(Real t, const Vec<N>& y, Vec<NC>& cerr) const;
+
+ // Given a time and state y, ensure that the state satisfies the constraints
+ // to within the indicated absolute tolerance, by performing the shortest
+ // (i.e. least squares) projection of the state back to the constraint
+ // manifold. Return false if the desired tolerance cannot be achieved.
+ // Otherwise (true return), a subsequent call to calcConstraintErrors()
+ // would return each |cerr[i]|<=consTol.
+ bool projectIfNeeded(Real consTol, Real t, Vec<N>& y) const;
+};
+
+ at endcode
+
+<h3>Usage</h3>
+
+ at code
+// Create an object of a type that satisfies the Equations description above.
+MyEquations eqns(...); // ... is whatever arguments you require.
+// Instantiate an integrator for this system of equations and specify the
+// integration accuracy and constraint tolerance.
+GeodesicIntegrator<MyEquations> integ(eqns, accuracy, constraintTol);
+// Initialize; will project constraints if necessary.
+integ.initialize(t0, y0);
+// Integrate to time finalTime, getting output every completed step.
+while (true) {
+ std::cout << "t=" << integ.getTime() << " y=" << integ.getY() << "\n";
+ if (integ.getTime() == finalTime)
+ break;
+ integ.takeOneStep(finalTime);
+}
+ at endcode
+
+<h3>Mathematical Overview</h3>
+
+This is an explicit, variable-step integrator solving a 2nd-order DAE
+structured as an ODE-on-a-manifold system[1] like this:
+<pre>
+ (1) udot = f(t,q,u) NQ dynamic differential equations
+ (2) qdot = u NQ kinematic differential equations
+ (3) 0 = c(t,q,u) NC constraints
+</pre>
+Here the "dot" suffix indicates differentiation with respect to the independent
+variable t which we'll refer to as time here although it can be anything (for
+geodesic calculations it is arc length). We'll
+call the second order variables q the "position variables", and their time
+derivatives u the "velocity variables". Collected together we call the state
+y={q,u}. At the beginning of a step, we expect to have been given initial
+conditions t0,q0,u0 such that |c(t0,q0,u0)|<=tol. The user provides the accuracy
+requirement and constraint tolerance. We solve the system to that accuracy while
+keeping the constraints within tolerance. The integrator returns after taking
+a successful step which may involve trial evaluations that are retracted.
+
+By "ODE on a manifold" we mean that the ODE (1,2) automatically satisfies the
+condition that IF c==0, THEN cdot=0, where
+<pre>
+ cdot=Dc/Dt + Dc/Dq*qdot + Dc/Du*udot
+</pre>
+This means that satisfaction of the acceleration-level constraints is built
+into the dynamic differential equations (1) so that we need only deal with
+relatively slow drift of the solution away from the position and velocity
+constraint manifolds.
+
+To handle the constraint drift we use the method of coordinate projection and
+expect the supplied Equations object to be able to perform a least-squares
+projection of a state (q,u) to move it onto the constraint manifolds.
+
+[1] Hairer, Lubich, Wanner, "Geometric Numerical Integration:
+Structure-Preserving Algorithms for Ordinary Differential Equations", 2nd ed.,
+section IV.4, pg 109ff, Springer, 2006. **/
+template <class Eqn>
+class GeodesicIntegrator {
+public:
+ enum { NQ = Eqn::NQ, NC = Eqn::NC, N = 2*NQ };
+
+ /** Construct an integrator for the given set of equations \a eqn, which are
+ to be solved to the given \a accuracy, with constraints maintained to
+ within the given \a constraintTol. **/
+ GeodesicIntegrator(const Eqn& eqn, Real accuracy, Real constraintTol)
+ : m_eqn(eqn), m_accuracy(accuracy), m_consTol(constraintTol),
+ m_hInit(NaN), m_hLast(NaN), m_hNext(Real(0.1)), m_nInitialize(0)
+ { reinitializeCounters(); }
+
+ /** Call this once before taking a series of steps. This sets the initial
+ conditions, and calculates the starting derivatives and constraint errors.
+ The constraints must be satisfied already by the given state; an error
+ is thrown if not. **/
+ void initialize(Real t, const Vec<N>& y) {
+ ++m_nInitialize;
+ reinitializeCounters();
+ m_hInit = m_hLast = NaN;
+ m_hNext = Real(0.1); // override if you have a better idea
+ m_t = t; m_y = y;
+ if (!m_eqn.projectIfNeeded(m_consTol, m_t, m_y)) {
+ Vec<NC> cerr;
+ m_eqn.calcConstraintErrors(m_t, m_y, cerr);
+ const Real consErr = calcNormInf(cerr);
+ SimTK_ERRCHK2_ALWAYS(!"projection failed",
+ "GeodesicIntegrator::initialize()",
+ "Couldn't project constraints to tol=%g;"
+ " largest error was %g.", m_consTol, consErr);
+ }
+ m_eqn.calcDerivs(m_t, m_y, m_ydot);
+ }
+
+ /** Set initial time and state prior to integrating. State derivatives
+ and constraint errors are calculated and an error is thrown if the
+ constraints are not already satisifed to the required tolerance. **/
+ void setTimeAndState(Real t, const Vec<N>& y) {
+ m_t = t; m_y = y;
+ m_eqn.calcDerivs(m_t, m_y, m_ydot);
+ Vec<NC> cerr;
+ m_eqn.calcConstraintErrors(m_t, m_y, cerr);
+ const Real consErr = calcNormInf(cerr);
+ if (consErr > m_consTol)
+ SimTK_ERRCHK2_ALWAYS(!"constraints not satisfied",
+ "GeodesicIntegrator::setTimeAndState()",
+ "Supplied state failed to satisfy constraints to tol=%g;"
+ " largest error was %g.", m_consTol, consErr);
+ }
+
+ /** Use this if you think you know a better initial step size to try than
+ the default. **/
+ void setNextStepSizeToTry(Real h) {m_hNext=h;}
+ /** Return the size of the next time step the integrator will attempt on
+ the next call to takeOneStep(). **/
+ Real getNextStepSizeToTry() const {return m_hNext;}
+
+ /** Return the accuracy requirement as set in the constructor. **/
+ Real getRequiredAccuracy() const {return m_accuracy;}
+ /** Return the constraint tolerance as set in the constructor. **/
+ Real getConstraintTolerance() const {return m_consTol;}
+
+ /** Return the size of the first accepted step to be taken after the most
+ recent initialize() call. **/
+ Real getActualInitialStepSizeTaken() const {return m_hInit;}
+
+ /** Return the number of successful time steps taken since the most recent
+ initialize() call. **/
+ int getNumStepsTaken() const {return m_nStepsTaken;}
+ /** Return the total number of steps that were attempted since the most
+ recent initialize() call. In general this will be more than the number
+ of steps taken since some will be rejected. **/
+ int getNumStepsAttempted() const {return m_nStepsAttempted;}
+ /** How many steps were rejected because they did not satisfy the
+ accuracy requirement, since the most recent initalize() call. This is
+ common but for non-stiff systems should be only a modest fraction of the
+ number of steps taken. **/
+ int getNumErrorTestFailures() const {return m_nErrtestFailures;}
+ /** How many steps were rejected because the projectIfNeeded() method was
+ unable to satisfy the constraint tolerance (since the most recent
+ initialize() call). This should be very rare. **/
+ int getNumProjectionFailures() const {return m_nProjectionFailures;}
+
+ /** Return the number of calls to initialize() since construction of this
+ integrator object. **/
+ int getNumInitializations() const {return m_nInitialize;}
+
+ /** Advance time and state by one error-controlled step and return, but
+ in no case advance past t=tStop. The integrator's internal time,
+ state, and state derivatives are advanced to the end of the step. If this
+ step reaches \a tStop, the returned time will be \e exactly tStop. **/
+ void takeOneStep(Real tStop);
+
+ /** Return the current time. **/
+ const Real& getTime() const {return m_t;}
+ /** Return the complete current state as a Vec<N>. **/
+ const Vec<N>& getY() const {return m_y;}
+ /** Return just the "position" variables q from the current state. **/
+ const Vec<NQ>& getQ() const {return Vec<NQ>::getAs(&m_y[0]);}
+ /** Return just the "velocity" variables u from the current state. **/
+ const Vec<NQ>& getU() const {return Vec<NQ>::getAs(&m_y[NQ]);}
+ /** Return the complete set of time derivatives of the current state. **/
+ const Vec<N>& getYDot() const {return m_ydot;}
+ /** Return just the derivatives qdot of the "position" variables q. **/
+ const Vec<NQ>& getQDot() const {return Vec<NQ>::getAs(&m_ydot[0]);}
+ /** Return just the derivatives udot of the "velocity" variables u. **/
+ const Vec<NQ>& getUDot() const {return Vec<NQ>::getAs(&m_ydot[NQ]);}
+
+ /** This is a utility routine that returns the infinity norm (maximum
+ absolute value) contained in a fixed-size, scalar Vec. **/
+ template <int Z> static Real calcNormInf(const Vec<Z>& v) {
+ Real norm = 0;
+ for (int i=0; i < Z; ++i) {
+ Real aval = std::abs(v[i]);
+ if (aval > norm) norm = aval;
+ }
+ return norm;
+ }
+
+ /** This is a utility routine that returns the RMS norm of a fixed-size,
+ scalar Vec. **/
+ template <int Z> static Real calcNormRMS(const Vec<Z>& v) {
+ Real norm = 0;
+ for (int i=0; i< Z; ++i)
+ norm += square(v[i]);
+ return std::sqrt(norm/Z);
+ }
+
+private:
+ void takeRKMStep(Real h, Vec<N>& y1, Vec<N>& y1err) const;
+
+ const Eqn& m_eqn; // The DAE system to be solved.
+ Real m_accuracy; // Absolute accuracy requirement for y.
+ Real m_consTol; // Absolute tolerance for constraint errors.
+
+ Real m_t; // Current value of the independent variable.
+ Vec<N> m_y; // Current q,u in that order.
+
+ Real m_hInit; // Actual initial step taken.
+ Real m_hLast; // Last step taken.
+ Real m_hNext; // max step size to try next
+
+ Vec<N> m_ydot; // ydot(t,y)
+
+ // Counters.
+ int m_nInitialize; // zeroed on construction only
+ void reinitializeCounters() {
+ m_nStepsTaken=m_nStepsAttempted=0;
+ m_nErrtestFailures=m_nProjectionFailures=0;
+ }
+ int m_nStepsTaken;
+ int m_nStepsAttempted;
+ int m_nErrtestFailures;
+ int m_nProjectionFailures;
+};
+
+template <class Eqn> void
+GeodesicIntegrator<Eqn>::takeOneStep(Real tStop) {
+ const Real Safety = Real(0.9), MinShrink = Real(0.1), MaxGrow = Real(5);
+ const Real HysteresisLow = Real(0.9), HysteresisHigh = Real(1.2);
+ const Real MaxStretch = Real(0.1);
+ const Real hMin = m_t <= 1 ? SignificantReal : SignificantReal*m_t;
+ const Real hStretch = MaxStretch*m_hNext;
+
+ // Figure out the target ending time for the next step. Choosing time
+ // rather than step size lets us end at exactly tStop.
+ Real t1 = m_t + m_hNext; // this is the usual case
+ // If a small stretching of the next step would get us to tStop, try
+ // to make it all the way in one step.
+ if (t1 + hStretch > tStop)
+ t1 = tStop;
+
+ Real h, errNorm;
+ Vec<N> y1, y1err;
+
+ // Try smaller and smaller step sizes if necessary until we get one
+ // that satisfies the error requirement, and for which projection
+ // succeeds.
+ while (true) {
+ ++m_nStepsAttempted;
+ h = t1 - m_t; assert(h>0);
+ takeRKMStep(h, y1, y1err);
+ errNorm = calcNormInf(y1err);
+ if (errNorm > m_accuracy) {
+ ++m_nErrtestFailures;
+ // Failed to achieve required accuracy at this step size h.
+ SimTK_ERRCHK4_ALWAYS(h > hMin,
+ "GeodesicIntegrator::takeOneStep()",
+ "Accuracy %g worse than required %g at t=%g with step size"
+ " h=%g; can't go any smaller.", errNorm, m_accuracy, m_t, h);
+
+ // Shrink step by (acc/err)^(1/4) for 4th order.
+ Real hNew = Safety * h * std::sqrt(std::sqrt(m_accuracy/errNorm));
+ hNew = clamp(MinShrink*h, hNew, HysteresisLow*h);
+ t1 = m_t + hNew;
+ continue;
+ }
+ // Accuracy achieved. Can we satisfy the constraints?
+ if (m_eqn.projectIfNeeded(m_consTol, t1, y1))
+ break; // all good
+
+ // Constraint projection failed. Hopefully that's because the
+ // step was too big.
+ ++m_nProjectionFailures;
+
+ SimTK_ERRCHK3_ALWAYS(h > hMin,
+ "GeodesicIntegrator::takeOneStep()",
+ "Projection failed to reach constraint tolerance %g at t=%g "
+ "with step size h=%g; can't shrink step further.",
+ m_consTol, m_t, h);
+
+ const Real hNew = MinShrink*h;
+ t1 = m_t + hNew;
+ }
+
+ // We achieved desired accuracy at step size h, and satisfied the
+ // constraints. (t1,y1) is now the final time and state; calculate
+ // state derivatives which will be used at the start of the next step.
+ ++m_nStepsTaken;
+ if (m_nStepsTaken==1) m_hInit = h; // that was the initial step
+ m_t = t1; m_y = y1; m_hLast = h;
+ m_eqn.calcDerivs(m_t, m_y, m_ydot);
+
+ // If the step we just took ended right at tStop, don't use it to
+ // predict a new step size; instead we'll just use the same hNext we
+ // would have used here if it weren't for tStop.
+ if (t1 < tStop) {
+ // Possibly grow step for next time.
+ Real hNew = errNorm == 0 ? MaxGrow*h
+ : Safety * h * std::sqrt(std::sqrt(m_accuracy/errNorm));
+ if (hNew < HysteresisHigh*h) hNew = h; // don't bother
+ hNew = std::min(hNew, MaxGrow*h);
+ m_hNext = hNew;
+ }
+}
+
+template <class Eqn> void
+GeodesicIntegrator<Eqn>::takeRKMStep(Real h, Vec<N>& y1, Vec<N>& y1err) const {
+ const Real h2=h/2, h3=h/3, h6=h/6, h8=h/8;
+ const Real t0=m_t, t1=m_t+h;
+ const Vec<N>& y0 = m_y;
+ const Vec<N>& f0 = m_ydot;
+ Vec<N> f1, f2, f3, f4, ysave;
+ m_eqn.calcDerivs(t0+h3, y0 + h3* f0, f1);
+ m_eqn.calcDerivs(t0+h3, y0 + h6*(f0 + f1), f2);
+ m_eqn.calcDerivs(t0+h2, y0 + h8*(f0 + 3*f2), f3);
+
+ // We'll need this for error estimation.
+ ysave = y0 + h2*(f0 - 3*f2 + 4*f3);
+ m_eqn.calcDerivs(t1, ysave, f4);
+
+ // Final value. This is the 4th order accurate estimate for
+ // y1=y(t0+h)+O(h^5): y1 = y0 + (h/6)*(f0 + 4 f3 + f4).
+ // Don't evaluate here yet because we might reject this step or we
+ // might need to do a projection.
+ y1 = y0 + h6*(f0 + 4*f3 + f4);
+
+ // This is an embedded 3rd-order estimate y1hat=y(t0+h)+O(h^4).
+ // y1hat = y0 + (h/10)*(f0 + 3 f2 + 4 f3 + 2 f4)
+ // We don't actually have any need for y1hat, just its 4th-order
+ // error estimate y1hat-y1=(1/5)(y1-ysave) (easily verified from the above).
+
+ for (int i=0; i<N; ++i)
+ y1err[i] = std::abs(y1[i]-ysave[i]) / 5;
+}
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_GEODESIC_INTEGRATOR_H_
diff --git a/SimTKmath/Geometry/include/simmath/internal/OBBTree.h b/SimTKmath/Geometry/include/simmath/internal/OBBTree.h
new file mode 100644
index 0000000..d79b6ef
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/OBBTree.h
@@ -0,0 +1,112 @@
+#ifndef SimTK_SIMMATH_OBB_TREE_H_
+#define SimTK_SIMMATH_OBB_TREE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Defines an oriented bounding box tree with generalized leaves. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_Box.h"
+#include "simmath/internal/Geo_BicubicBezierPatch.h"
+
+#include <cassert>
+
+namespace SimTK {
+
+//==============================================================================
+// OBB LEAF
+//==============================================================================
+/** TODO **/
+class OBBLeaf {
+public:
+ virtual ~OBBLeaf() {}
+};
+
+//==============================================================================
+// OBB NODE
+//==============================================================================
+/** TODO **/
+class SimTK_SIMMATH_EXPORT OBBNode {
+public:
+ OBBNode() : contents(0) {clear();}
+ ~OBBNode() {clear();}
+
+ void clear() {
+ delete contents; contents=0;
+ x0=y0=nx=ny=-1;
+ children.clear();
+ }
+
+ bool isLeaf() const {return children.empty();}
+ int getNumChildren() const {return (int)children.size();}
+ const OBBNode& getChild(int i) const {return children[i];}
+ OBBNode& updChild(int i) {return children[i];}
+
+ // A box enclosing the contents.
+ Geo::OrientedBox box;
+ int depth; // 0 is root
+ int height; // a leaf is 0, node is max of children+1
+
+ // A cone enclosing the entire range of normals.
+ UnitVec3 normal; // central normal
+ Real coneHalfAngle; // 0<=a<=pi, pi/2 makes a halfspace
+
+ // An arbitrary point on the contained surface, used in
+ // distance queries where distance to box is min distance, distance
+ // to point is max distance.
+ Vec3 pointOnSurface;
+
+ int x0,y0; // Range of patches in this node
+ int nx,ny;
+
+ Array_<OBBNode> children;
+
+ // If no children, leaf contents:
+ OBBLeaf* contents; // non-null only for leaf (NOT USED YET)
+ Vec2 centerUW; // (u,w) parameters of patch center
+ Vec2 dims; // half-u, half-w sizes
+ Geo::BicubicBezierPatch patch; // TODO: no need to keep this around
+};
+
+
+//==============================================================================
+// OBB TREE
+//==============================================================================
+/** TODO **/
+class SimTK_SIMMATH_EXPORT OBBTree {
+public:
+ const OBBNode& getRoot() const {return root;}
+ OBBNode& updRoot() {return root;}
+private:
+ OBBNode root;
+};
+
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_OBB_TREE_H_
diff --git a/SimTKmath/Geometry/include/simmath/internal/OrientedBoundingBox.h b/SimTKmath/Geometry/include/simmath/internal/OrientedBoundingBox.h
new file mode 100644
index 0000000..97a1557
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/OrientedBoundingBox.h
@@ -0,0 +1,106 @@
+#ifndef SimTK_SIMMATH_ORIENTED_BOUNDING_BOX_H_
+#define SimTK_SIMMATH_ORIENTED_BOUNDING_BOX_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+
+namespace SimTK {
+
+/**
+ * This class represents a rectangular box with arbitrary position and
+ * orientation. It is used in collision detection as a bounding volume for
+ * geometry of various types.
+ *
+ * An OrientedBoundingBox is defined by a Transform that specifies its position
+ * and orientation, and a Vec3 that specifies its size. In the reference frame
+ * defined by the Transform, one corner is at the origin and the opposite
+ * corner is at the point returned by getSize().
+ */
+class SimTK_SIMMATH_EXPORT OrientedBoundingBox {
+public:
+ OrientedBoundingBox();
+ /**
+ * Create an OrientedBoundingBox.
+ *
+ * @param transform specifies the position and orientation of the box
+ * @param size specifies the dimensions of the box
+ */
+ OrientedBoundingBox(const Transform& transform, const Vec3& size);
+ /**
+ * Create an OrientedBoundingBox which encloses a set of points.
+ */
+ explicit OrientedBoundingBox(const Vector_<Vec3>& points);
+ /**
+ * Get the position and orientation of the box.
+ */
+ const Transform& getTransform() const;
+ /**
+ * Get the dimensions of the box.
+ */
+ const Vec3& getSize() const;
+ /**
+ * Determine whether a point is inside the box.
+ */
+ bool containsPoint(const Vec3& point) const;
+ /**
+ * Determine whether this box intersects another bounding box at any point.
+ */
+ bool intersectsBox(const OrientedBoundingBox& box) const;
+ /**
+ * Determine whether a ray intersects this bounding box.
+ *
+ * @param origin the position at which the ray begins
+ * @param direction the ray direction
+ * @param distance if an intersection is found, the distance from the ray
+ * origin to the intersection point is stored in this.
+ * Otherwise, it is left unchanged.
+ * @return true if an intersection is found, false otherwise
+ */
+ bool intersectsRay(const Vec3& origin, const UnitVec3& direction,
+ Real& distance) const;
+ /**
+ * Given a point in space, find the point inside the bounding box which is
+ * nearest to it.
+ */
+ Vec3 findNearestPoint(const Vec3& position) const;
+ /**
+ * Get the locations of the eight corners of the box.
+ *
+ * @param corners the corner locations are stored in this array
+ */
+ void getCorners(Vec3 corners[8]) const;
+private:
+ Real calculateVolume(const Vector_<Vec3>& points, const Rotation& rotation);
+ Transform transform;
+ Vec3 size;
+};
+
+SimTK_SIMMATH_EXPORT OrientedBoundingBox
+operator*(const Transform& t, const OrientedBoundingBox& box);
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_ORIENTED_BOUNDING_BOX_H_
diff --git a/SimTKmath/Geometry/include/simmath/internal/ParticleConSurfaceSystem.h b/SimTKmath/Geometry/include/simmath/internal/ParticleConSurfaceSystem.h
new file mode 100644
index 0000000..915f1b8
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/ParticleConSurfaceSystem.h
@@ -0,0 +1,151 @@
+#ifndef SimTK_SIMBODY_PARTICLECONSURFACESYSTEM_H_
+#define SimTK_SIMBODY_PARTICLECONSURFACESYSTEM_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Ian Stavness, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+
+/**
+ * This is a System that represents the dynamics of a particle moving
+ * along a smooth surface. It is used to integrate Geodesic curves along
+ * implicit surfaces. The particle on surface constraint is stabilized
+ * with coordinate projection. (Implementation based on PendulumSystem.h)
+ *
+ **/
+
+#include "simmath/internal/BicubicSurface.h" // XXX compiler needed this
+#include "simmath/internal/ContactGeometry.h"
+#include "SimTKcommon.h"
+#include "SimTKcommon/internal/SystemGuts.h"
+
+namespace SimTK {
+
+class ParticleConSurfaceSystem;
+class ParticleConSurfaceSystemGuts: public System::Guts {
+ friend class ParticleConSurfaceSystem;
+
+ // TOPOLOGY STATE
+ SubsystemIndex subsysIndex;
+
+ // TOPOLOGY CACHE
+// mutable DiscreteVariableIndex massIndex, lengthIndex, gravityIndex;
+ DiscreteVariableIndex geodesicIndex, geometryIndex;
+ mutable QIndex q0;
+ mutable UIndex u0;
+ mutable QErrIndex qerr0;
+ mutable UErrIndex uerr0;
+ mutable UDotErrIndex udoterr0;
+ mutable EventTriggerByStageIndex event0;
+
+public:
+ ParticleConSurfaceSystemGuts(const ContactGeometryImpl& geom)
+ : Guts(), geom(geom) {
+ // Index types set themselves invalid on construction.
+ }
+
+ inline const ParticleConSurfaceSystem& getParticleConSurfaceSystem() const;
+
+ SubsystemIndex getSubsysIndex() const {
+ return subsysIndex;
+ }
+
+ /*virtual*/ParticleConSurfaceSystemGuts* cloneImpl() const {return new ParticleConSurfaceSystemGuts(*this);}
+
+ /////////////////////////////////////////////////////////
+ // Implementation of continuous DynamicSystem virtuals //
+ /////////////////////////////////////////////////////////
+
+ /*virtual*/int realizeTopologyImpl(State&) const;
+ /*virtual*/int realizeModelImpl(State&) const;
+ /*virtual*/int realizeInstanceImpl(const State&) const;
+ /*virtual*/int realizePositionImpl(const State&) const;
+ /*virtual*/int realizeVelocityImpl(const State&) const;
+ /*virtual*/int realizeDynamicsImpl(const State&) const;
+ /*virtual*/int realizeAccelerationImpl(const State&) const;
+
+ // qdot==u here so these are just copies
+ /*virtual*/void multiplyByNImpl(const State& state, const Vector& u,
+ Vector& dq) const {dq=u;}
+ /*virtual*/void multiplyByNTransposeImpl(const State& state, const Vector& fq,
+ Vector& fu) const {fu=fq;}
+ /*virtual*/void multiplyByNPInvImpl(const State& state, const Vector& dq,
+ Vector& u) const {u=dq;}
+ /*virtual*/void multiplyByNPInvTransposeImpl(const State& state, const Vector& fu,
+ Vector& fq) const {fq=fu;}
+
+ // No prescribed motion.
+ /*virtual*/bool prescribeQImpl(State&) const {return false;}
+ /*virtual*/bool prescribeUImpl(State&) const {return false;}
+
+ // No constraints.
+ /*virtual*/void projectQImpl(State&, Vector& qErrEst,
+ const ProjectOptions& options, ProjectResults& results) const;
+ /*virtual*/void projectUImpl(State&, Vector& uErrEst,
+ const ProjectOptions& options, ProjectResults& results) const;
+private:
+ const ContactGeometryImpl& geom;
+}; // class ParticleConSurfaceSystemGuts
+
+
+
+class ParticleConSurfaceSystem: public System {
+public:
+ ParticleConSurfaceSystem(const ContactGeometryImpl& geom) : System()
+ {
+ adoptSystemGuts(new ParticleConSurfaceSystemGuts(geom));
+ DefaultSystemSubsystem defsub(*this);
+ updGuts().subsysIndex = defsub.getMySubsystemIndex();
+
+ setHasTimeAdvancedEvents(false);
+ }
+
+ const ParticleConSurfaceSystemGuts& getGuts() const {
+ return SimTK_DYNAMIC_CAST_DEBUG<const ParticleConSurfaceSystemGuts&>
+ (getSystemGuts());
+ }
+
+ ParticleConSurfaceSystemGuts& updGuts() {
+ return SimTK_DYNAMIC_CAST_DEBUG<ParticleConSurfaceSystemGuts&>
+ (updSystemGuts());
+ }
+
+ void setDefaultTimeAndState(Real t, const Vector& q, const Vector& u) {
+ const ParticleConSurfaceSystemGuts& guts = getGuts();
+ updDefaultState().updU(guts.subsysIndex) = u;
+ updDefaultState().updQ(guts.subsysIndex) = q;
+ updDefaultState().updTime() = t;
+ }
+
+
+}; // class ParticleConSurfaceSystem
+
+
+inline const ParticleConSurfaceSystem& ParticleConSurfaceSystemGuts::
+getParticleConSurfaceSystem() const {
+ return static_cast<const ParticleConSurfaceSystem&>(getSystem());
+}
+
+
+} // namespace SimTK
+
+#endif /*SimTK_SIMBODY_PARTICLECONSURFACESYSTEM_H_*/
diff --git a/SimTKmath/Geometry/include/simmath/internal/Spline.h b/SimTKmath/Geometry/include/simmath/internal/Spline.h
new file mode 100644
index 0000000..2809037
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/Spline.h
@@ -0,0 +1,205 @@
+#ifndef SimTK_SIMMATH_SPLINE_H_
+#define SimTK_SIMMATH_SPLINE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-13 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/internal/GCVSPLUtil.h"
+
+#include <limits>
+
+namespace SimTK {
+
+/** This class implements a non-uniform Bezier curve. It requires the spline
+degree to be odd (linear, cubic, quintic, etc.), but supports arbitrarily high
+degrees. Only spline curves are supported, not surfaces or higher dimensional
+objects, but the curve may be defined in an arbitrary dimensional space.
+That is, a Spline_ is a Function_ that calculates an arbitrary number of
+output values based on a single input value. The template argument must be
+either Real or Vec<N> from some integer N. The name "Spline" (with no trailing
+"_") may be used as a synonym for Spline_<Real>.
+
+Most users should generate Spline_ objects using SplineFitter which can be
+used to generate spline curves through data points rather than requiring
+Bezier control points.
+
+ at see SplineFitter for best-fitting a spline through sampled data.
+ at see BicubicSurface for fitting a smooth surface to 2D sample data.
+**/
+template <class T>
+class Spline_ : public Function_<T> {
+public:
+ /** Create a Spline_ object based on a set of control points.\ See
+ SplineFitter for a nicer way to create these objects.
+
+ @param[in] degree
+ The degree of the spline to create. This must be a positive odd value.
+ @param[in] x
+ Values of the independent variable for each Bezier control point.
+ @param[in] y
+ Values of the dependent variables for each Bezier control point.
+ **/
+ Spline_(int degree, const Vector& x, const Vector_<T>& y)
+ : impl(new SplineImpl(degree, x, y)) {}
+
+ /** Default constructor creates an empty %Spline_ handle; not very
+ useful. **/
+ Spline_() : impl(NULL) {}
+
+ /** Copy constructor is shallow and reference-counted; that is, the new
+ %Spline_ refers to the same object as does \a source. **/
+ Spline_(const Spline_& source) : impl(source.impl)
+ { if (impl) impl->referenceCount++; }
+
+ /** Copy assignment is shallow and reference-counted; that is, after the
+ assignment this %Spline_ refers to the same object as does \a source. **/
+ Spline_& operator=(const Spline_& source) {
+ if (impl) {
+ impl->referenceCount--;
+ if (impl->referenceCount == 0)
+ delete impl;
+ }
+ impl = source.impl;
+ if (impl) impl->referenceCount++;
+ return *this;
+ }
+
+ /** Destructor decrements the reference count and frees the heap space if
+ this is the last reference. **/
+ ~Spline_() {
+ if (impl) {
+ impl->referenceCount--;
+ if (impl->referenceCount == 0)
+ delete impl;
+ }
+ }
+
+ /** Calculate the values of the dependent variables at a particular value
+ of the independent variable.
+ @param[in] x The value of the independent variable.
+ @returns The corresponding values of the dependent variables. **/
+ T calcValue(Real x) const {
+ assert(impl);
+ return impl->getValue(x);
+ }
+
+ /** Calculate a derivative of the spline function with respect to its
+ independent variable, at the given value.
+ @param[in] order
+ Which derivative? order==1 returns first derivative, order==2 is second,
+ etc. Must be >= 1.
+ @param[in] x
+ The value of the independent variable about which the derivative is
+ taken.
+ @returns
+ The \a order'th derivative of the dependent variables at \a x. **/
+ T calcDerivative(int order, Real x) const {
+ assert(impl);
+ assert(order > 0);
+ return impl->getDerivative(order, x);
+ }
+
+ /** Get the locations (that is, the values of the independent variable) for
+ each of the Bezier control points. **/
+ const Vector& getControlPointLocations() const {
+ assert(impl);
+ return impl->x;
+ }
+ /** Get the values of the dependent variables at each of the Bezier control
+ points. **/
+ const Vector_<T>& getControlPointValues() const {
+ assert(impl);
+ return impl->y;
+ }
+
+ /** Get the degree of the spline. **/
+ int getSplineDegree() const {
+ assert(impl);
+ return impl->degree;
+ }
+
+ /** Alternate signature provided to implement the generic Function_
+ interface expects a one-element Vector for the independent variable. **/
+ T calcValue(const Vector& x) const OVERRIDE_11 {
+ assert(x.size() == 1);
+ return calcValue(x[0]);
+ }
+ /** Alternate signature provided to implement the generic Function_
+ interface expects an awkward \a derivComponents argument, and
+ takes a one-element Vector for the independent variable. Because
+ Spline_ only allows a single independent variable, all elements of
+ derivComponents should be 0; only its length determines the order of the
+ derivative to calculate. **/
+ T calcDerivative(const Array_<int>& derivComponents, const Vector& x) const
+ OVERRIDE_11
+ { assert(x.size() == 1);
+ return calcDerivative((int)derivComponents.size(), x[0]); }
+ /** For the Function_ style interface, this provides compatibility
+ with std::vector. No copying or heap allocation is required. **/
+ T calcDerivative(const std::vector<int>& derivComponents,
+ const Vector& x) const
+ { assert(x.size() == 1);
+ return calcDerivative((int)derivComponents.size(), x[0]); }
+
+ /** Required by the Function_ interface. **/
+ int getArgumentSize() const OVERRIDE_11 {return 1;}
+ /** Required by the Function_ interface. **/
+ int getMaxDerivativeOrder() const OVERRIDE_11
+ { return std::numeric_limits<int>::max(); }
+
+private:
+ class SplineImpl;
+ SplineImpl* impl;
+};
+
+/** Provide a convenient name for a scalar-valued Spline_. **/
+typedef Spline_<Real> Spline;
+
+/** This is the implementation class that supports the Spline_ interface. **/
+template <class T>
+class Spline_<T>::SplineImpl {
+public:
+ SplineImpl(int degree, const Vector& x, const Vector_<T>& y)
+ : referenceCount(1), degree(degree), x(x), y(y) {}
+ ~SplineImpl() {
+ assert(referenceCount == 0);
+ }
+ T getValue(Real t) const {
+ return GCVSPLUtil::splder(0, degree, t, x, y);
+ }
+ T getDerivative(int derivOrder, Real t) const {
+ return GCVSPLUtil::splder(derivOrder, degree, t, x, y);
+ }
+ int referenceCount;
+ int degree;
+ Vector x;
+ Vector_<T> y;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_SPLINE_H_
+
+
diff --git a/SimTKmath/Geometry/include/simmath/internal/SplineFitter.h b/SimTKmath/Geometry/include/simmath/internal/SplineFitter.h
new file mode 100644
index 0000000..1f65713
--- /dev/null
+++ b/SimTKmath/Geometry/include/simmath/internal/SplineFitter.h
@@ -0,0 +1,208 @@
+#ifndef SimTK_SIMMATH_SPLINE_FITTER_H_
+#define SimTK_SIMMATH_SPLINE_FITTER_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/internal/Spline.h"
+#include "simmath/internal/GCVSPLUtil.h"
+
+namespace SimTK {
+
+/** Given a set of data points, this class creates a Spline_ which interpolates
+or approximates them. The data points are assumed to represent a smooth curve
+plus uncorrelated additive noise. It attempts to separate these from each other
+and return a Spline_ which represents the original curve without noise.
+
+The fitting is done based on a <i>smoothing parameter</i>. When the parameter
+is 0, the spline will exactly interpolate the data points. Larger values of the
+smoothing parameter produce smoother curves that may vary more from the original
+data. Since you generally do not know in advance what value for the smoothing
+parameter is "best", several different methods are provided for selecting it
+automatically.
+
+If you have no prior information about the structure of the input data, call
+fitFromGCV(): @code
+ SplineFitter<Vec3> fitter = SplineFitter::fitFromGCV(degree, x, y);
+ Spline_<Vec3> spline = fitter.getSpline();
+ at endcode
+
+This chooses a value of the smoothing parameter to minimize the <i>Generalized
+Cross Validation</i> function. It also estimates the true mean squared error of
+the data the the number of degrees of freedom of the residual (that is, the
+number of degrees of freedom not explained by the spline), which can be queried
+by calling getMeanSquaredError() and getDegreesOfFreedom(). Alternatively, if
+you have prior knowledge of the error variance or residual degrees of freedom,
+you can call fitFromErrorVariance() or fitFromDOF() instead. Finally, you can
+explicitly specify the smoothing parameter to use by calling
+fitForSmoothingParameter().
+
+For more information on the GCVSPL algorithm, see Woltring, H.J. (1986),
+"A FORTRAN package for generalized, cross-validatory spline smoothing and
+differentiation." Advances in Engineering Software 8(2):104-113. Also, while
+this class provides access to the most important features of the algorithm,
+there are a few advanced options which it does not expose directly. If you need
+those options, you can access them using the GCVSPLUtil class. **/
+template <class T>
+class SplineFitter {
+public:
+ SplineFitter(const SplineFitter& copy) : impl(copy.impl) {
+ impl->referenceCount++;
+ }
+ SplineFitter operator=(const SplineFitter& copy) {
+ impl = copy.impl;
+ impl->referenceCount++;
+ return *this;
+ }
+ ~SplineFitter() {
+ impl->referenceCount--;
+ if (impl->referenceCount == 0)
+ delete impl;
+ }
+
+ /** Perform a fit, choosing a value of the smoothing parameter that
+ minimizes the Generalized Cross Validation function.
+ @param[in] degree The degree of the spline to create. This must be a
+ positive, odd integer.
+ @param[in] x The value of the independent variable for each data point.
+ @param[in] y The values of the dependent variables for each data point.
+ @return A SplineFitter object containing the desired Spline_. **/
+ static SplineFitter fitFromGCV
+ (int degree, const Vector& x, const Vector_<T>& y) {
+ Vector_<T> coeff;
+ Vector wk;
+ int ier;
+ GCVSPLUtil::gcvspl(x, y, Vector(x.size(), 1.0), T(1),
+ degree, 2, 0, coeff, wk, ier);
+ return SplineFitter<T>(new SplineFitterImpl
+ (degree, Spline_<T>(degree, x, coeff), wk[3], wk[4], wk[2]));
+ }
+
+ /** Perform a fit, choosing a value of the smoothing parameter based on the
+ known error variance in the data.
+ @param[in] degree The degree of the spline to create. This must be a
+ positive, odd integer.
+ @param[in] x The value of the independent variable for each data point.
+ @param[in] y The values of the dependent variables for each data point.
+ @param[in] error The variance of the error in the data.
+ @return A SplineFitter object containing the desired Spline_. **/
+ static SplineFitter fitFromErrorVariance
+ (int degree, const Vector& x, const Vector_<T>& y, Real error) {
+ Vector_<T> coeff;
+ Vector wk;
+ int ier;
+ GCVSPLUtil::gcvspl(x, y, Vector(x.size(), 1.0), T(1),
+ degree, 3, error, coeff, wk, ier);
+ return SplineFitter<T>(new SplineFitterImpl
+ (degree, Spline_<T>(degree, x, coeff), wk[3], wk[4], wk[2]));
+ }
+
+ /** Perform a fit, choosing a value of the smoothing parameter based on the
+ expect number of degrees of freedom of the residual.
+ @param[in] degree The degree of the spline to create. This must be a
+ positive, odd value.
+ @param[in] x The value of the independent variable for each data point.
+ @param[in] y The values of the dependent variables for each data point.
+ @param[in] dof The expected number of degrees of freedom.
+ @return A SplineFitter object containing the desired Spline_. **/
+ static SplineFitter fitFromDOF
+ (int degree, const Vector& x, const Vector_<T>& y, Real dof) {
+ Vector_<T> coeff;
+ Vector wk;
+ int ier;
+ GCVSPLUtil::gcvspl(x, y, Vector(x.size(), 1.0), T(1),
+ degree, 4, dof, coeff, wk, ier);
+ return SplineFitter<T>(new SplineFitterImpl
+ (degree, Spline_<T>(degree, x, coeff), wk[3], wk[4], wk[2]));
+ }
+
+ /** Perform a fit, using a specified fixed value for the smoothing
+ parameter.
+ @param[in] degree The degree of the spline to create. This must be a
+ positive, odd value.
+ @param[in] x The value of the independent variable for each data point.
+ @param[in] y The values of the dependent variables for each data point.
+ @param[in] p The value of the smoothing parameter.
+ @return A SplineFitter object containing the desired Spline_. **/
+ static SplineFitter fitForSmoothingParameter
+ (int degree, const Vector& x, const Vector_<T>& y, Real p) {
+ Vector_<T> coeff;
+ Vector wk;
+ int ier;
+ GCVSPLUtil::gcvspl(x, y, Vector(x.size(), 1.0), T(1),
+ degree, 1, p, coeff, wk, ier);
+ return SplineFitter<T>(new SplineFitterImpl
+ (degree, Spline_<T>(degree, x, coeff), wk[3], wk[4], wk[2]));
+ }
+ /**
+ * Get the Spline_ that was generated by the fitting.
+ */
+ const Spline_<T>& getSpline() {
+ return impl->spline;
+ }
+ /**
+ * Get the smoothing parameter that was used for the fitting.
+ */
+ Real getSmoothingParameter() {
+ return impl->p;
+ }
+ /**
+ * Get the estimate of the true mean squared error in the data that was determined by the fitting.
+ */
+ Real getMeanSquaredError() {
+ return impl->error;
+ }
+ /**
+ * Get the estimate of the number of degrees of freedom of the residual that was determined by the fitting.
+ */
+ Real getDegreesOfFreedom() {
+ return impl->dof;
+ }
+private:
+ class SplineFitterImpl;
+ SplineFitter(SplineFitterImpl *impl) : impl(impl) {
+ }
+ SplineFitterImpl* impl;
+};
+
+template <class T>
+class SplineFitter<T>::SplineFitterImpl {
+public:
+ SplineFitterImpl(int degree, const Spline_<T>& spline, Real p, Real error, Real dof) : referenceCount(1), degree(degree), spline(spline), p(p), error(error), dof(dof) {
+ }
+ ~SplineFitterImpl() {
+ assert(referenceCount == 0);
+ }
+ int referenceCount;
+ int degree;
+ Spline_<T> spline;
+ Real p, error, dof;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_SPLINE_FITTER_H_
+
+
diff --git a/SimTKmath/Geometry/src/BicubicSurface.cpp b/SimTKmath/Geometry/src/BicubicSurface.cpp
new file mode 100644
index 0000000..e477518
--- /dev/null
+++ b/SimTKmath/Geometry/src/BicubicSurface.cpp
@@ -0,0 +1,1112 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Matthew Millard, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+This file contains the library-side implementations of classes
+BicubicSurface, BicubicSurface::Guts, and BicubicSurface::PatchHint. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/internal/BicubicSurface.h"
+#include "simmath/internal/SplineFitter.h"
+#include "simmath/internal/ContactGeometry.h"
+
+#include "BicubicSurface_Guts.h"
+
+#include <algorithm>
+
+using namespace SimTK;
+using namespace std;
+
+//==============================================================================
+// BICUBIC SURFACE
+//==============================================================================
+
+// This is just a handle for BicubicSurface::Guts, which is the shared,
+// underlying surface representation. Here we just manage the reference
+// count and otherwise forward all requests to the Guts class.
+
+// Default constructor is inline; just sets guts pointer to null.
+
+// Destructor: delete guts if this is the last referencing handle.
+BicubicSurface::~BicubicSurface() {clear();}
+
+// Copy constructor is shallow.
+BicubicSurface::BicubicSurface(const BicubicSurface& source) {
+ guts = source.guts; // copy pointer
+ if (guts) guts->incrReferenceCount();
+}
+
+// Copy assignment is shallow.
+BicubicSurface& BicubicSurface::operator=(const BicubicSurface& source) {
+ if (guts != source.guts) {
+ clear();
+ guts = source.guts; // copy pointer
+ if (guts) guts->incrReferenceCount();
+ }
+ return *this;
+}
+
+// Return handle to its default-constructed state, deleting guts if this was
+// the last reference.
+void BicubicSurface::clear() {
+ if (guts && guts->decrReferenceCount()==0)
+ delete guts;
+ guts = 0;
+}
+
+// Constructor for irregularly spaced samples.
+BicubicSurface::BicubicSurface
+ (const Vector& x, const Vector& y, const Matrix& f, Real smoothness)
+: guts(0) {
+ guts = new BicubicSurface::Guts(x,y,f,smoothness);
+ guts->incrReferenceCount(); // will be 1
+}
+
+// Constructor for regularly spaced samples.
+BicubicSurface::BicubicSurface
+ (const Vec2& XY, const Vec2& spacing, const Matrix& f, Real smoothness)
+: guts(0) {
+ guts = new BicubicSurface::Guts(XY,spacing,f,smoothness);
+ guts->incrReferenceCount(); // will be 1
+}
+
+// Constructor from known patch derivatives.
+BicubicSurface::BicubicSurface
+ (const Vector& x, const Vector& y, const Matrix& f,
+ const Matrix& fx, const Matrix& fy, const Matrix& fxy)
+: guts(0) {
+ guts = new BicubicSurface::Guts(x,y,f,fx,fy,fxy);
+ guts->incrReferenceCount(); // will be 1
+}
+
+// Constructor from known patch derivatives with regular spacing.
+BicubicSurface::BicubicSurface
+ (const Vec2& XY, const Vec2& spacing, const Matrix& f,
+ const Matrix& fx, const Matrix& fy, const Matrix& fxy)
+: guts(0) {
+ guts = new BicubicSurface::Guts(XY,spacing,f,fx,fy,fxy);
+ guts->incrReferenceCount(); // will be 1
+}
+
+// calcValue(), the fast version.
+Real BicubicSurface::calcValue(const Vec2& XY, PatchHint& hint) const {
+ SimTK_ERRCHK_ALWAYS(!isEmpty(), "BicubicSurface::calcValue()",
+ "This method can't be called on an empty handle.");
+ return guts->calcValue(XY, hint);
+}
+
+// calcValue(), the slow version.
+Real BicubicSurface::calcValue(const Vec2& XY) const {
+ PatchHint hint; // create an empty hint
+ return calcValue(XY, hint);
+}
+
+// calcDerivative(), the fast version.
+Real BicubicSurface::calcDerivative
+ (const Array_<int>& components, const Vec2& XY, PatchHint& hint) const {
+ SimTK_ERRCHK_ALWAYS(!isEmpty(), "BicubicSurface::calcDerivative()",
+ "This method can't be called on an empty handle.");
+ return guts->calcDerivative(components, XY, hint);
+}
+
+// calcDerivative(), the slow version.
+Real BicubicSurface::calcDerivative
+ (const Array_<int>& components, const Vec2& XY) const {
+ PatchHint hint; // create an empty hint
+ return calcDerivative(components, XY, hint);
+}
+
+UnitVec3 BicubicSurface::calcUnitNormal(const Vec2& XY, PatchHint& hint) const {
+ const Array_<int> dx(1,0), dy(1,1);
+ const Real fx = calcDerivative(dx,XY,hint);
+ const Real fy = calcDerivative(dy,XY,hint);
+ return UnitVec3(-fx, -fy, 1); // (1,0,fx) X (0,1,fy)
+}
+UnitVec3 BicubicSurface::calcUnitNormal(const Vec2& XY) const {
+ PatchHint hint;
+ return calcUnitNormal(XY,hint);
+}
+
+void BicubicSurface::calcParaboloid
+ (const Vec2& XY, PatchHint& hint, Transform& X_SP, Vec2& k) const
+{
+ SimTK_ERRCHK_ALWAYS(!isEmpty(), "BicubicSurface::calcParaboloid()",
+ "This method can't be called on an empty handle.");
+ guts->calcParaboloid(XY, hint, X_SP, k);
+}
+
+void BicubicSurface::calcParaboloid
+ (const Vec2& XY, Transform& X_SP, Vec2& k) const
+{
+ PatchHint hint;
+ calcParaboloid(XY, hint, X_SP, k);
+}
+
+void BicubicSurface::getNumPatches(int& nx, int& ny) const
+{ guts->getNumPatches(nx,ny); }
+
+Geo::BicubicHermitePatch BicubicSurface::
+calcHermitePatch(int x, int y) const
+{ PatchHint hint; return guts->calcHermitePatch(x,y, hint); }
+
+Geo::BicubicBezierPatch BicubicSurface::
+calcBezierPatch(int x, int y) const
+{ PatchHint hint; return guts->calcBezierPatch(x,y, hint); }
+
+bool BicubicSurface::isSurfaceDefined(const Vec2& XY) const
+{ return getGuts().isSurfaceDefined(XY); }
+
+Vec2 BicubicSurface::getMinXY() const {
+ const BicubicSurface::Guts& guts = getGuts();
+ return Vec2(guts._x[0],guts._y[0]);
+}
+
+Vec2 BicubicSurface::getMaxXY() const {
+ const BicubicSurface::Guts& guts = getGuts();
+ return Vec2(guts._x[guts._x.size()-1],guts._y[guts._y.size()-1]);
+}
+
+PolygonalMesh BicubicSurface::createPolygonalMesh(Real resolution) const {
+ PolygonalMesh mesh;
+ getGuts().createPolygonalMesh(resolution, mesh);
+ return mesh;
+}
+
+int BicubicSurface::getNumAccesses() const
+{ return getGuts().numAccesses; }
+
+int BicubicSurface::getNumAccessesSamePoint() const
+{ return getGuts().numAccessesSamePoint; }
+
+int BicubicSurface::getNumAccessesSamePatch() const
+{ return getGuts().numAccessesSamePatch; }
+
+int BicubicSurface::getNumAccessesNearbyPatch() const
+{ return getGuts().numAccessesNearbyPatch; }
+
+void BicubicSurface::resetStatistics() const
+{ return getGuts().resetStatistics(); }
+
+
+
+//==============================================================================
+// BICUBIC SURFACE :: GUTS
+//==============================================================================
+
+// This is the constructor for irregularly-spaced samples.
+BicubicSurface::Guts::Guts(const Vector& aX, const Vector& aY,
+ const Matrix& af, Real smoothness)
+{
+ construct();
+
+ _x.resize(aX.size()); _y.resize(aY.size());
+ _x = aX; _y = aY;
+
+ _hasRegularSpacing = false;
+
+ constructFromSplines(af, smoothness);
+}
+
+
+// This is the constructor for regularly spaced samples.
+BicubicSurface::Guts::Guts
+ (const Vec2& XY, const Vec2& spacing, const Matrix& af, Real smoothness)
+{
+ construct();
+
+ // Check for reasonable spacing.
+ SimTK_ERRCHK2_ALWAYS(spacing > 0,
+ "BicubicSurface::BicubicSurface(XY,spacing,af,smoothness)",
+ "A BicubicSurface requires positive spacing in both x and y"
+ " but spacing was %g and %g.", spacing[0], spacing[1]);
+
+ const int nx = af.nrow(), ny = af.ncol();
+ _x.resize(nx); _y.resize(ny);
+
+ for (int i=0; i < nx; ++i)
+ _x[i] = XY[0] + i*spacing[0];
+ for (int j=0; j < ny; ++j)
+ _y[j] = XY[1] + j*spacing[1];
+
+ _hasRegularSpacing = true;
+ _spacing = spacing;
+
+ constructFromSplines(af, smoothness);
+}
+
+// This implementation is shared by the regular and irregular constructors.
+// We expect _x and _y already to have been filled in with the grid sample
+// locations (either as supplied or as generated from regular spacing).
+void BicubicSurface::Guts::
+constructFromSplines(const Matrix& af, Real smoothness)
+{
+ const int nx = af.nrow(), ny = af.ncol();
+
+ // Check for sufficient sample size.
+ SimTK_ERRCHK2_ALWAYS(af.nrow() >= 4 && af.ncol() >= 4,
+ "BicubicSurface::BicubicSurface()",
+ "A BicubicSurface requires at least 4 sample in x and y directions"
+ " but grid dimensions were %d and %d.", nx, ny);
+
+ SimTK_ERRCHK4_ALWAYS(_x.size() == nx && _y.size() == ny,
+ "BicubicSurface::BicubicSurface()",
+ "Number of samples must match the grid dimension (%d X %d) but"
+ "the number of supplied sample points was %d X %d.",
+ nx, ny, _x.size(), _y.size());
+
+ // We're checking this even for generated regularly-spaced samples
+ // to catch the rare case that the spacing was so small that it
+ // produced two identical sample locations.
+
+ SimTK_ERRCHK_ALWAYS( isMonotonicallyIncreasing(_x)
+ && isMonotonicallyIncreasing(_y),
+ "BicubicSurface::BicubicSurface()",
+ "Sample vectors must each be monotonically increasing.");
+
+ // The grid data stores a Vec4 at each grid point with (possibly smoothed)
+ // function value f, and derivatives fx, fy, fxy in that order.
+ _ff.resize(nx, ny);
+
+ _debug = false;
+
+ // These temporaries are needed for indexing into the Spline functions.
+ Vector coord(1);
+ const Array_<int> deriv1(1,0); // just one zero to pick 1st derivative
+
+ // This temporary will hold either the original function values or the
+ // smoothed ones.
+ Matrix fSmooth(nx,ny);
+
+ // Smoothing strategy: we want something that is symmetric in x and y
+ // so that you will get the same surface if you rotate the grid 90 degrees
+ // to exchange the meaning of x and y. To accomplish that, we smooth
+ // the grid separately along the rows and columns, and then average the
+ // results to produce a new grid to which we fit the surface.
+
+ if (smoothness > 0) {
+ // This temp holds the function values as they look after smoothing
+ // in the x direction (that is, down the columns of constant y).
+ Matrix xf(nx,ny);
+
+ // Smooth position data along lines of constant y.
+ for(int j=0; j < ny; ++j){
+ Spline_<Real> xspline = SplineFitter<Real>::fitForSmoothingParameter
+ (3,_x,af(j),smoothness).getSpline();
+ for(int i=0; i < nx; ++i){
+ coord[0] = _x[i];
+ xf(i,j) = xspline.calcValue(coord);
+ }
+ }
+
+ // Smooth position data along lines of constant x, then average the
+ // result with the corresponding value from smoothing the other way.
+ for(int i=0; i < nx; ++i){
+ Spline_<Real> yspline = SplineFitter<Real>::fitForSmoothingParameter
+ (3,_y,~af[i],smoothness).getSpline();
+ for(int j=0; j < ny; ++j){
+ coord[0] = _y[j];
+ const Real yfij = yspline.calcValue(coord);
+ fSmooth(i,j) = (xf(i,j) + yfij) / 2; // average xf,xy
+ }
+ }
+ } else {
+ // Not smoothing.
+ fSmooth = af;
+ }
+
+ // Now fill in the f and fx entries in our internal grid by exactly
+ // fitting a spline to the already-smoothed data.
+ for(int j=0; j < ny; ++j){
+ Spline_<Real> xspline = SplineFitter<Real>::fitForSmoothingParameter
+ (3,_x,fSmooth(j),0).getSpline();
+ for(int i=0; i < nx; ++i){
+ coord[0] = _x[i];
+ Vec4& fij = _ff(i,j);
+ fij[F] = fSmooth(i,j);
+ fij[Fx] = xspline.calcDerivative(deriv1,coord);
+ }
+ }
+
+ // Compute fy and fxy by fitting splines along the rows.
+ // Note that we are using the already-smoothed value of f here.
+ Vector tmpRow(ny);
+ for(int i=0; i < nx; ++i){
+ // Fit splines along rows of constant x to go exactly through the
+ // already-smoothed sample points in order to get fy=Df/Dy.
+ Spline_<Real> yspline = SplineFitter<Real>::fitForSmoothingParameter
+ (3,_y,~fSmooth[i],0).getSpline();
+
+ // Fit splines along rows of constant x to interpolate fx in the y
+ // direction to give fxy=Dfx/Dy.
+ for (int j=0; j<_y.size(); ++j) tmpRow[j] = _ff(i,j)[Fx];
+ Spline_<Real> ydxspline = SplineFitter<Real>::fitForSmoothingParameter
+ (3,_y,tmpRow,0).getSpline();
+
+ for(int j=0; j < ny; ++j){
+ coord[0] = _y[j];
+ Vec4& fij = _ff(i,j);
+ fij[Fy] = yspline.calcDerivative(deriv1,coord);
+ fij[Fxy] = ydxspline.calcDerivative(deriv1,coord);
+ }
+ }
+}
+
+// This is the advanced constructor where everything is known already.
+BicubicSurface::Guts::Guts
+ (const Vector& aX, const Vector& aY, const Matrix& af,
+ const Matrix& afx, const Matrix& afy, const Matrix& afxy)
+{
+ construct();
+
+ _x.resize(aX.size()); _y.resize(aY.size());
+ _x = aX; _y = aY;
+
+ _hasRegularSpacing = false;
+
+ constructFromKnownFunction(af, afx, afy, afxy);
+}
+
+BicubicSurface::Guts::Guts
+ (const Vec2& XY, const Vec2& spacing, const Matrix& af,
+ const Matrix& afx, const Matrix& afy, const Matrix& afxy)
+{
+ construct();
+
+ // Check for reasonable spacing.
+ SimTK_ERRCHK2_ALWAYS(spacing > 0,
+ "BicubicSurface::BicubicSurface(XY,spacing,af,smoothness)",
+ "A BicubicSurface requires positive spacing in both x and y"
+ " but spacing was %g and %g.", spacing[0], spacing[1]);
+
+ const int nx = af.nrow(), ny = af.ncol();
+ _x.resize(nx); _y.resize(ny);
+
+ for (int i=0; i < nx; ++i)
+ _x[i] = XY[0] + i*spacing[0];
+ for (int j=0; j < ny; ++j)
+ _y[j] = XY[1] + j*spacing[1];
+
+ _hasRegularSpacing = true;
+ _spacing = spacing;
+
+ constructFromKnownFunction(af, afx, afy, afxy);
+}
+
+// Expects _x and _y already to be filled in.
+void BicubicSurface::Guts::
+constructFromKnownFunction
+ (const Matrix& af, const Matrix& afx, const Matrix& afy,
+ const Matrix& afxy)
+{
+ const int nx = af.nrow(), ny = af.ncol();
+
+ // Check for sufficient sample size.
+ SimTK_ERRCHK2_ALWAYS(nx >= 2 && ny >= 2,
+ "BicubicSurface::BicubicSurface(f,fx,fy,fxy)",
+ "A BicubicSurface requires at least 2 sample in x and y directions"
+ " but grid dimensions were %d and %d.", nx, ny);
+
+ SimTK_ERRCHK4_ALWAYS(_x.size() == nx && _y.size() == ny,
+ "BicubicSurface::BicubicSurface(f,fx,fy,fxy)",
+ "Number of samples must match the grid dimension (%d X %d) but"
+ "the number of supplied sample points was %d X %d.",
+ nx, ny, _x.size(), _y.size());
+
+ SimTK_ERRCHK2_ALWAYS( afx.nrow() == nx && afx.ncol() == ny
+ && afy.nrow() == nx && afy.ncol() == ny
+ && afxy.nrow() == nx && afxy.ncol() == ny,
+ "BicubicSurface::BicubicSurface(f,fx,fy,fxy)",
+ "All the derivative sample matrices must match the grid dimension"
+ " (%d X %d).", nx, ny);
+
+ SimTK_ERRCHK_ALWAYS( isMonotonicallyIncreasing(_x)
+ && isMonotonicallyIncreasing(_y),
+ "BicubicSurface::BicubicSurface(f,fx,fy,fxy)",
+ "Sample vectors must each be monotonically increasing.");
+
+ _ff.resize(nx,ny);
+
+ _debug = false;
+
+ //Copy the data into our packed data structure.
+ for(int i=0; i < nx; ++i) {
+ for(int j=0; j < ny; ++j){
+ Vec4& fij = _ff(i,j);
+ fij[F] = af(i,j);
+ fij[Fx] = afx(i,j);
+ fij[Fy] = afy(i,j);
+ fij[Fxy] = afxy(i,j);
+ }
+ }
+}
+
+//_____________________________________________________________________________
+
+Real BicubicSurface::Guts::calcValue(const Vec2& aXY, PatchHint& hint) const
+{
+ getFdF(aXY,0,hint); // just function value
+ const PatchHint::Guts& h = hint.getGuts();
+ assert(h.xy == aXY && h.level >= 0);
+ return h.f;
+}
+
+
+Real BicubicSurface::Guts::calcDerivative
+ (const Array_<int>& aDerivComponents, const Vec2& aXY, PatchHint& hint) const
+{
+ const int wantLevel = (int)aDerivComponents.size();
+
+ if (wantLevel == 0)
+ return calcValue(aXY, hint); // "0th" deriv is the function value
+
+ for (int i=0; i < wantLevel; ++i) {
+ SimTK_ERRCHK2_ALWAYS(aDerivComponents[i]==0 || aDerivComponents[i]==1,
+ "BicubicSurface::calcDerivative()",
+ "Component %d was %d but must be 0 or 1 for x or y.",
+ i, aDerivComponents[i]);
+ }
+
+ if (wantLevel > 3)
+ return 0; // 4th and higher derivatives are all zero
+
+ getFdF(aXY, wantLevel, hint);
+ const PatchHint::Guts& h = hint.getGuts();
+ assert(h.xy == aXY && h.level >= wantLevel);
+
+ if (aDerivComponents.size() == 1)
+ return aDerivComponents[0]==0 ? h.fx : h.fy; // fx : fy
+
+ if (aDerivComponents.size() == 2) {
+ if (aDerivComponents[0]==0) //x
+ return aDerivComponents[1]==0 ? h.fxx : h.fxy; // fxx:fxy
+ else //y (fyx==fxy)
+ return aDerivComponents[1]==0 ? h.fxy : h.fyy; // fyx:fyy
+ }
+
+ // Third derivative.
+ if (aDerivComponents[0]==0) { //x
+ if (aDerivComponents[1]==0) // xx
+ return aDerivComponents[2]==0 ? h.fxxx : h.fxxy; // fxxx:fxxy
+ else // xy (fxyx==fxxy)
+ return aDerivComponents[2]==0 ? h.fxxy : h.fxyy; // fxyx:fxyy
+ } else { //y (fyx==fxy)
+ if (aDerivComponents[1]==0) // yx (fyxx==fxxy, fyxy==fxyy)
+ return aDerivComponents[2]==0 ? h.fxxy : h.fxyy; // fyxx:fyxy
+ else // yy (fyyx==fxyy)
+ return aDerivComponents[2]==0 ? h.fxyy : h.fyyy; // fyyx:fyyy
+ }
+}
+
+// Cost is patch evaluation + about 200 flops.
+void BicubicSurface::Guts::calcParaboloid
+ (const Vec2& aXY, PatchHint& hint, Transform& X_SP, Vec2& k) const
+{
+ getFdF(aXY, 2, hint); // calculate through 2nd derivatives
+ const PatchHint::Guts& h = hint.getGuts();
+ assert(h.xy == aXY && h.level >= 2);
+
+ const Vec3 P = aXY.append1(h.f);
+ const Vec3 dPdx(1,0,h.fx);
+ const Vec3 dPdy(0,1,h.fy);
+ const UnitVec3 nn(-h.fx, -h.fy, 1); // dPdx X dPdy (normalizing, ~35 flops)
+ const Vec3 d2Pdx2(0,0,h.fxx);
+ const Vec3 d2Pdy2(0,0,h.fyy);
+ const Vec3 d2Pdxdy(0,0,h.fxy);
+
+ // TODO: could save a little time here by taking advantage of the known
+ // sparsity of these vectors (probably not worth the trouble).
+ k = ContactGeometry::evalParametricCurvature
+ (P,nn,dPdx,dPdy,d2Pdx2,d2Pdy2,d2Pdxdy,X_SP);
+}
+
+bool BicubicSurface::Guts::isSurfaceDefined(const Vec2& XYval) const
+{
+ const bool valueDefined =
+ (_x[0] <= XYval[0] && XYval[0] <= _x[_x.size()-1])
+ && (_y[0] <= XYval[1] && XYval[1] <= _y[_y.size()-1]);
+
+ return valueDefined;
+}
+
+/* This function ensures that the given hint contains correct information for
+the patch indexed (x0,y0). */
+void BicubicSurface::Guts::
+getPatchInfoIfNeeded(int x0, int y0, BicubicSurface::PatchHint::Guts& h) const {
+ const int x1 = x0+1, y1 = y0+1;
+
+ // Compute Bicubic coefficients only if we're in a new patch
+ // else use the old ones, because this is an expensive step!
+ if( !(h.x0 == x0 && h.y0 == y0) ) {
+ // The hint is no good at all since it is for the wrong patch.
+ h.clear();
+ h.x0 = x0; h.y0 = y0;
+
+ // Compute the scaling of the new patch. Note that neither patch
+ // dimension can be zero since we don't allow duplicates in x or y.
+ h.xS = _x(x1)-_x(x0);
+ h.yS = _y(y1)-_y(y0);
+ h.ooxS = 1/h.xS; h.ooxS2 = h.ooxS*h.ooxS; h.ooxS3=h.ooxS*h.ooxS2;
+ h.ooyS = 1/h.yS; h.ooyS2 = h.ooyS*h.ooyS; h.ooyS3=h.ooyS*h.ooyS2;
+
+ // Form the vector f and multiply Ainv*f to form coefficient vector a.
+
+ const Vec4& f00 = _ff(x0,y0);
+ const Vec4& f01 = _ff(x0,y1);
+ const Vec4& f10 = _ff(x1,y0);
+ const Vec4& f11 = _ff(x1,y1);
+
+ h.fV[0] = f00[F];
+ h.fV[1] = f10[F];
+ h.fV[2] = f01[F];
+ h.fV[3] = f11[F];
+
+ // Can't precalculate these scaled values because the same grid point
+ // is used for up to four different patches, each scaled differently.
+ h.fV[4] = f00[Fx]*h.xS;
+ h.fV[5] = f10[Fx]*h.xS;
+ h.fV[6] = f01[Fx]*h.xS;
+ h.fV[7] = f11[Fx]*h.xS;
+
+ h.fV[8] = f00[Fy]*h.yS;
+ h.fV[9] = f10[Fy]*h.yS;
+ h.fV[10] = f01[Fy]*h.yS;
+ h.fV[11] = f11[Fy]*h.yS;
+
+ h.fV[12] = f00[Fxy]*h.xS*h.yS;
+ h.fV[13] = f10[Fxy]*h.xS*h.yS;
+ h.fV[14] = f01[Fxy]*h.xS*h.yS;
+ h.fV[15] = f11[Fxy]*h.xS*h.yS;
+
+ getCoefficients(h.fV,h.a);
+ }
+}
+
+
+/**
+This function computes the surface value and all derivatives (because it is
+cheap to compute these derivatives once the bicubic interpolation
+coefficients have been solved for). These values are stored in mutable
+function members so that if a user repeatedly asks for information about
+the same location the stored data is supplied. Additionally, if the user
+is staying within a single patch, the the coefficients required for this
+patch are only computed once, and they are re-used until a location outside
+the patch is requested.
+
+ at param aXY the X,Y location of interest
+ at param fV an empty vector that is set to the values of f,fx,fy and fxy of
+ the patch corners
+ f(0,0) f(1,0) f(0,1) f(1,1),
+ fx(0,0) fx(1,0) fx(0,1) fx(1,1),
+ fy(0,0) fy(1,0) fy(0,1) fy(1,1),
+ fxy(0,0) fxy(1,0) fxy(0,1) fxy(1,1)
+ at param aijV an empty vector that is set to the coefficient values of the
+ bicubic surface polynomials for the patch that the current
+ XY location resides in. The coefficients are returned in this
+ order
+
+ a00,a10,a20,a30,a01,a11,a21,a31,a02,a12,a22,a32,a03,a13,a23,a33
+
+ at param aFdF an empty vector that is set to the 10 values of f and its partial
+derivatives at the point XY. These values are stored in the following order:
+ f(x,y) fx(x,y) fy(x,y) fxy(x,y) fxx(x,y) fyy(x,y)
+ fxxx(x,y) fxxy(x,y) fyyy(x,y) fxyy(x,y)
+*/
+void BicubicSurface::Guts::
+getFdF(const Vec2& aXY, int wantLevel, PatchHint& hint) const {
+ ++numAccesses; // All surface accesses come through here.
+
+ //0. Check if the surface is defined for the XY value given.
+ SimTK_ERRCHK6_ALWAYS(isSurfaceDefined(aXY),
+ "BicubicSurface::getFdF (private fcn)",
+ "BicubicSurface is not defined at requested location (%g,%g)."
+ " The surface is valid from x[%g %g], y[%g %g].", aXY(0), aXY(1),
+ _x[0], _x[_x.size()-1], _y[0], _y[_y.size()-1]);
+
+ // -1 means just do the patch
+ // 0 means patch and function value
+ // 1 means add 1st deriv, 2 is 2nd, 3 is 3rd
+ assert(-1 <= wantLevel && wantLevel <= 3);
+
+ BicubicSurface::PatchHint::Guts& h = hint.updGuts();
+
+ //1. Check to see if we have already computed values for the requested point.
+ if(h.level >= wantLevel && aXY == h.xy){
+ ++numAccessesSamePoint;
+ return;
+ }
+
+ // Nope. We're at least changing points.
+ h.xy = aXY;
+ h.level = -1; // we don't know anything about this point
+
+ // We're going to feed calcLowerBoundIndex() our best guess as to the
+ // patch this point is on. For regularly-spaced grid points we can find
+ // it exactly, except for some possible roundoff. Otherwise the best we
+ // can do is supply the current index from the hint.
+ int pXidx = h.x0, pYidx = h.y0;
+ if (_hasRegularSpacing) {
+ pXidx = clamp(0, (int)std::floor((h.xy[0]-_x[0])/_spacing[0]),
+ _x.size()-2); // can't be last index
+ pYidx = clamp(0, (int)std::floor((h.xy[1]-_y[0])/_spacing[1]),
+ _y.size()-2);
+ }
+
+ // Compute the indices that define the patch containing this value.
+ int howResolvedX, howResolvedY;
+ const int x0 = calcLowerBoundIndex(_x,aXY[0],pXidx,howResolvedX);
+ const int x1 = x0+1;
+ const int y0 = calcLowerBoundIndex(_y,aXY[1],pYidx,howResolvedY);
+ const int y1 = y0+1;
+
+ // 0->same patch, 1->nearby patch, 2->had to search
+ const int howResolved = std::max(howResolvedX, howResolvedY);
+ if (howResolved == 0) ++numAccessesSamePatch;
+ else if (howResolved == 1) ++numAccessesNearbyPatch;
+
+ // Compute Bicubic coefficients only if we're in a new patch
+ // else use the old ones, because this is an expensive step!
+ getPatchInfoIfNeeded(x0,y0,h);
+
+ // At this point we know that the hint contains valid patch information,
+ // but it contains no valid point information.
+
+ if (wantLevel == -1)
+ return; // caller just wanted patch info
+
+ const Vec<16>& a = h.a; // abbreviate for convenience
+
+
+ // Compute where in the patch we are. This has to
+ // be done everytime the location within the patch changes
+ // ... which has happened if this code gets executed.
+
+ //--------------------------------------------------------------------------
+ // Evaluate function value f (38 flops).
+
+ // 8 flops
+ const Real xpt = (aXY(0)-_x(x0))*h.ooxS, xpt2=xpt*xpt, xpt3=xpt*xpt2;
+ const Real ypt = (aXY(1)-_y(y0))*h.ooyS, ypt2=ypt*ypt, ypt3=ypt*ypt2;
+ // 12 flops
+ const Mat44 mx(a[ 0], a[ 1]*xpt, a[ 2]*xpt2, a[ 3]*xpt3,
+ a[ 4], a[ 5]*xpt, a[ 6]*xpt2, a[ 7]*xpt3,
+ a[ 8], a[ 9]*xpt, a[10]*xpt2, a[11]*xpt3,
+ a[12], a[13]*xpt, a[14]*xpt2, a[15]*xpt3);
+ // 12 flops
+ const Vec4 xsum = mx.rowSum();
+ // 6 flops
+ h.f = xsum[0] + ypt*xsum[1] + ypt2*xsum[2] + ypt3*xsum[3];
+ h.level = 0; // function value is ready
+ if (wantLevel == 0)
+ return;
+
+ //--------------------------------------------------------------------------
+ // Evaluate first derivatives fx, fy (43 flops).
+
+ // fy is 9 flops
+ const Real dypt = h.ooyS, dypt2= 2*ypt*h.ooyS, dypt3= 3*ypt2*h.ooyS;
+ h.fy = dypt*xsum[1] + dypt2*xsum[2] + dypt3*xsum[3];
+
+ // fx is 34 flops
+ const Real dxpt=h.ooxS, dxpt2=2*xpt*h.ooxS, dxpt3=3*xpt2*h.ooxS;
+ const Mat43 mdx(a[ 1]*dxpt, a[ 2]*dxpt2, a[ 3]*dxpt3,
+ a[ 5]*dxpt, a[ 6]*dxpt2, a[ 7]*dxpt3,
+ a[ 9]*dxpt, a[10]*dxpt2, a[11]*dxpt3,
+ a[13]*dxpt, a[14]*dxpt2, a[15]*dxpt3);
+ const Vec4 dxsum = mdx.rowSum();
+ h.fx = dxsum[0] + ypt*dxsum[1] + ypt2*dxsum[2] + ypt3*dxsum[3];
+ h.level = 1; // first derivatives are ready
+ if (wantLevel == 1)
+ return;
+
+
+ //--------------------------------------------------------------------------
+ // Evaluate second derivatives fxy, fxx, fyy (40 flops).
+
+ // fxy, fyy are 11 flops
+ h.fxy = dypt*dxsum[1] + dypt2*dxsum[2] + dypt3*dxsum[3];
+ const Real dyypt2=2*h.ooyS2, dyypt3=6*ypt*h.ooyS2;
+ h.fyy = dyypt2*xsum[2] + dyypt3*xsum[3];
+
+ // fxx is 29 flops
+ const Real dxxpt2=2*h.ooxS2, dxxpt3=6*xpt*h.ooxS2;
+ const Mat42 mdxx(a[ 2]*dxxpt2, a[ 3]*dxxpt3,
+ a[ 6]*dxxpt2, a[ 7]*dxxpt3,
+ a[10]*dxxpt2, a[11]*dxxpt3,
+ a[14]*dxxpt2, a[15]*dxxpt3);
+ const Vec4 dxxsum = mdxx.rowSum();
+ h.fxx = dxxsum[0] + ypt*dxxsum[1] + ypt2*dxxsum[2] + ypt3*dxxsum[3];
+ h.level = 2; // second derivatives are ready
+ if (wantLevel == 2)
+ return;
+
+ //--------------------------------------------------------------------------
+ // Evaluate third derivatives fxxx, fxxy, fyyy, fxyy (21 flops).
+
+ // 10 flops
+ const Real dyyypt3=6*h.ooyS3;
+ h.fyyy = dyyypt3*xsum[3];
+ h.fxyy = dyypt2*dxsum[2] + dyypt3*dxsum[3];
+ h.fxxy = dypt*dxxsum[1] + dypt2*dxxsum[2] + dypt3*dxxsum[3];
+
+ // 11 flops
+ const Real dxxxpt3=6*h.ooxS3;
+ const Vec4 mdxxx(a[ 3]*dxxxpt3,
+ a[ 7]*dxxxpt3,
+ a[11]*dxxxpt3,
+ a[15]*dxxxpt3);
+ h.fxxx = mdxxx[0] + ypt* mdxxx[1] + ypt2*mdxxx[2] + ypt3*mdxxx[3];
+ h.level = 3; // third derivatives are ready
+
+ if(_debug == true){
+ cout<<" getFdF" << endl;
+ cout <<"Member variables" <<endl;
+ cout << "_x" << _x << endl;
+ cout << "\n"<<endl;
+ cout << "_y" << _y << endl;
+ cout << "\n"<<endl;
+ cout << "_ff" << _ff << endl;
+ cout << "\n"<<endl;
+
+ cout <<" Intermediate variables " << endl;
+ cout <<"XY: " << aXY << endl;
+ printf("[x0 x1], [y0 y1]: [%d %d],[%d %d]\n",x0,x1,y0,y1);
+ printf("[x0V x1V], [y0V y1V]: [%f %f],[%f %f]\n",_x(x0),_x(x1),_y(y0),_y(y1));
+ cout <<" xS " << h.xS << " yS " << h.yS << endl;
+ printf("(xp,yp): (%f %f)\n",xpt,ypt);
+ cout << "\n\n\n"<<endl;
+ }
+}
+
+
+/** Given a search value aVal and an n-vector aVec (n>=2) containing
+monotonically increasing values (no duplicates), this method finds the unique
+pair of indices (i,i+1) such that either
+ - aVec[i] <= aVal < aVec[i+1], or
+ - aVec[n-2] < aVal == aVec[n-1].
+
+The following preconditions must be satisfied by the arguments to this
+method:
+ - aVec.size() >= 2
+ - aVal >= aVec[0]
+ - aVal <= aVec[n-1]
+
+
+aVec = [0.1 0.2 0.3 0.4 0.5];
+aVal = 0.125
+idxLB = calcLowerBoundIndex(aVec,aVal,-1);
+
+Then idxLB should be 0. Some effort has been put into making this code
+efficient, as it is expected that very large vectors could be used in this
+function. If the data is evenly spaced, the index is computed. If not data
+near a previous index (set by the user) is searched. If the data is still
+not found a binary search is performed
+
+ at params aVec: A SimTK vector containing monotonically increasing data
+ at params aVal: A value bounded by the numbers in the vector
+ at params pIdx: A hint index provided by the user (of this function)
+ at returns int: The index of the entry in the vector that is the as close to
+ aVal without exceeding it.
+
+*/
+
+// Return true if aVal is on the patch (really the line) between
+// aVec[indxL] and aVec[indxL+1]. By "on the patch" we mean that
+// aVec[indxL] <= aVal < aVec[indxL+1] unless this is the last patch in
+// which case we allow aVec[indxL] < aVal <= aVec[indxL+1].
+static bool isOnPatch(const Vector& aVec, int indxL, Real aVal) {
+ assert(aVec.size() >= 2);
+ const int maxLB = aVec.size() - 2;
+ assert(0 <= indxL && indxL <= maxLB);
+
+ const Real low = aVec[indxL], high = aVec[indxL+1];
+
+ if (aVal < low || aVal > high)
+ return false;
+
+ // Here we know low <= aVal <= high.
+ if (aVal < high)
+ return true;
+
+ // Here we know low < aVal == high. This is only allowed on the last patch.
+ return indxL == maxLB;
+}
+
+// howResolved: 0->same patch, 1->nearby patch, 2->search
+int BicubicSurface::Guts::
+calcLowerBoundIndex(const Vector& aVec, Real aVal, int pIdx,
+ int& howResolved) const {
+ int idxLB = -1;
+ bool idxComputed = false;
+
+ assert(aVec.size() >= 2);
+
+ // Because we're trying to find the lower index, it can't be the very
+ // last knot.
+ const int maxLB = aVec.size() - 2;
+
+ // 1. Do a local search around the previous index, if one is given.
+ if(0 <= pIdx && pIdx <= maxLB) {
+ // Are we still on the same patch? Caution -- can't be equal to the
+ // upper knot unless it is the last one.
+ if (isOnPatch(aVec, pIdx, aVal)) {
+ howResolved = 0;
+ return pIdx;
+ }
+
+ // Not on the same patch, how about adjacent patches?
+ if (aVal < aVec[pIdx]) {
+ // Value moved below the current patch.
+ if (pIdx > 0 && isOnPatch(aVec, pIdx-1, aVal)) {
+ howResolved = 1;
+ return pIdx-1; // found it next door!
+ }
+ } else if (aVal >= aVec[pIdx+1]) {
+ // Value moved above the current patch.
+ if (pIdx < maxLB && isOnPatch(aVec, pIdx+1, aVal)) {
+ howResolved = 1;
+ return pIdx+1; // found it next door!
+ }
+ }
+ }
+
+ // Either we didn't have a previous index to try, or it didn't help.
+
+ // 2. Check the end points. We'll count these as "nearby patches" since
+ // they are about the same amount of work.
+ if (aVal <= aVec[0]) {
+ howResolved = 1;
+ return 0;
+ }
+ if (aVal >= aVec[maxLB+1]) {
+ howResolved = 1;
+ return maxLB;
+ }
+
+ // 3. If still not found, use binary search to find the appropriate index.
+
+ // std::upper_bound returns the index of the first element that
+ // is strictly greater than aVal (one after the last element
+ // if aVal is exactly equal to the last knot).
+ const Real* upper =
+ std::upper_bound(&aVec[0], &aVec[0] + aVec.size(), aVal);
+ const int upperIx = clamp(0, (int)(upper-&aVec[1]), maxLB);
+
+ howResolved = 2;
+ return upperIx;
+}
+
+/**
+This function will compute the coefficients aij for a bicubic patch that has
+the values of f, fx, fy and fxy defined at its four corners. This function is
+relatively expensive, as it involves multiplying a 16x16 matrix by a 16x1,
+so it is called only when absolutely necessary.
+
+ at param f Vector f defining the values of f, fx, fy and fxy at the four
+ corners of the patch. Vector f is in this order:
+ f(0,0) f(1,0) f(0,1) f(1,1),
+ fx(0,0) fx(1,0) fx(0,1) fx(1,1),
+ fy(0,0) fy(1,0) fy(0,1) fy(1,1),
+ fxy(0,0) fxy(1,0) fxy(0,1) fxy(1,1)
+ at param aV An empty vector for which the coefficients aij are written into
+ in the following order:
+
+ a00,a10,a20,a30,a01,a11,a21,a31,a02,a12,a22,a32,a03,a13,a23,a33
+*/
+void BicubicSurface::Guts::
+getCoefficients(const Vec<16>& fV, Vec<16>& aV) const {
+ // This is what the full matrix inverse looks like (copied here from
+ // Wikipedia). It is very sparse and contains only a few unique values
+ // so the matrix-vector product can be done very cheaply if worked out
+ // in painstaking detail (with some help from Maple). The full
+ // matrix-vector product takes 496 flops; we can do it in 80.
+ /*
+ const Real Ainv[] = {
+ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ -3, 3, 0, 0, -2,-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 2,-2, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, -3, 3, 0, 0, -2,-1, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 2,-2, 0, 0, 1, 1, 0, 0,
+ -3, 0, 3, 0, 0, 0, 0, 0, -2, 0,-1, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, -3, 0, 3, 0, 0, 0, 0, 0, -2, 0,-1, 0,
+ 9,-9,-9, 9, 6, 3,-6,-3, 6,-6, 3,-3, 4, 2, 2, 1,
+ -6, 6, 6,-6, -3,-3, 3, 3, -4, 4,-2, 2, -2,-2,-1,-1,
+ 2, 0,-2, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 2, 0,-2, 0, 0, 0, 0, 0, 1, 0, 1, 0,
+ -6, 6, 6,-6, -4,-2, 4, 2, -3, 3,-3, 3, -2,-1,-2,-1,
+ 4,-4,-4, 4, 2, 2,-2,-2, 2,-2, 2,-2, 1, 1, 1, 1
+ };
+ Mat<16,16> AinvM(Ainv);
+ aV = AinvM*fV; // So cool that I can do this in C++! Go Sherm!
+ */
+
+
+ // Matt's masterful Maple work, plus a little manual hacking by Sherm
+ // produced this version:
+
+ // Caution: note index change.
+ Real f1=fV[0], f2=fV[1], f3=fV[2], f4=fV[3], f5=fV[4], f6=fV[5],
+ f7=fV[6], f8=fV[7], f9=fV[8], f10=fV[9], f11=fV[10], f12=fV[11],
+ f13=fV[12], f14=fV[13], f15=fV[14], f16=fV[15];
+
+ // 54 add/subtract
+ // 26 multiplies
+ Real f86 = f8-f6, f1211=f12-f11, f109=f10-f9, f75=f7-f5;
+ Real f1234 = f1-f2-f3+f4;
+ Real t312 = 2*f86;
+ Real t311 = 2*f1211;
+ Real t310 = 3*f86 - 2*f14;
+ Real t309 = 3*f1211 - 2*f15;
+ Real t10 = 2*f9;
+ Real t308 = f14 - 2*f10 + t10;
+ Real t307 = 3*f109 - f14;
+ Real t306 = 3*f75 - f15;
+ Real t15 = 2*f5;
+ Real t305 = t15 - 2*f7 + f13 + f15;
+ Real t289 = -2*f13;
+ Real t304 = t289 - 6*f1234 - f16;
+ Real t302 = -3*f1;
+ Real t301 = 2*f1;
+ aV(0) = f1;
+ aV(1) = f5;
+ aV(2) = t302 + 3*f2 - t15 - f6;
+ aV(3) = t301 - 2*f2 + f5 + f6;
+ aV(4) = f9;
+ aV(5) = f13;
+ aV(6) = t289 + t307;
+ aV(7) = f13 + t308;
+ aV(8) = t302 + 3*f3 - t10 - f11;
+ aV(9) = t289 + t306;
+ aV(10) = 9*f1234 + 6*(f5-f7+f9-f10) + 4*f13 + f16 - t309 - t310;
+ aV(11) = 4*f109 + t304 + t306 + t310 + t311;
+ aV(12) = t301 - 2*f3 + f9 + f11;
+ aV(13) = t305;
+ aV(14) = 4*f75 + t304 + t307 + t309 + t312;
+ aV(15) = 4*f1234 + f16 + t305 + t308 - t311 - t312;
+
+ if(_debug == true){
+ cout << "getCoefficients" << endl;
+ cout << "\tfV" << fV << endl;
+ cout << "\taijV" << aV << endl;
+ }
+}
+
+// We're going to generate quads in strips like this:
+// * <- * <- * <- *
+// v ^ v ^ v ^
+// * -> * -> * -> *
+// Each patch will generate the same number of quads so there will be more
+// quads where the surface is denser.
+void BicubicSurface::Guts::
+createPolygonalMesh(Real resolution, PolygonalMesh& mesh) const {
+ PatchHint hint;
+ // Number of patches in x and y direction.
+ const int nxpatch = _x.size()-1;
+ const int nypatch = _y.size()-1;
+ // n is the number of subdivisions per patch
+ const int n = std::max(1 + (int)(resolution+.5), 1); // round
+ const int nx = nxpatch*n + 1; // number of vertices along each row
+ const int ny = nypatch*n + 1;
+ // These will alternately serve as previous and current.
+ Array_<int> row1(ny), row2(ny);
+ Array_<int>* prevRowVerts = &row1;
+ Array_<int>* curRowVerts = &row2;
+ Vector xVals(nx), yVals(ny);
+
+ // Calculate the x and y sample values.
+ int nxt = 0;
+ for (int px=0; px < nxpatch; ++px) {
+ const Real xlo = _x[px], xhi = _x[px+1];
+ const Real xs = xhi - xlo, width = xs/n;
+ // For each quad within patch px
+ for (int qx=0; qx < n; ++qx)
+ xVals[nxt++] = xlo + qx*width;
+ }
+ xVals[nxt++] = _x[_x.size()-1];
+ assert(nxt == nx);
+
+ nxt = 0;
+ for (int py=0; py < nypatch; ++py) {
+ const Real ylo = _y[py], yhi = _y[py+1];
+ const Real ys = yhi - ylo, width = ys/n;
+ // For each quad within patch py
+ for (int qx=0; qx < n; ++qx)
+ yVals[nxt++] = ylo + qx*width;
+ }
+ yVals[nxt++] = _y[_y.size()-1];
+ assert(nxt == ny);
+
+ // Fill in the zeroth row.
+ Vec2 pt; pt[0] = xVals[0];
+ for (int j=0; j < ny; ++j) {
+ pt[1] = yVals[j];
+ const Real z = calcValue(pt, hint);
+ (*prevRowVerts)[j] = mesh.addVertex(Vec3(pt[0],pt[1],z));
+ }
+
+ // For each remaining row, generate vertices and then a strip of faces.
+ Array_<int> face(4);
+ for (int i=1; i < nx; ++i) {
+ Vec2 pt; pt[0] = xVals[i];
+ for (int j=0; j < ny; ++j) {
+ pt[1] = yVals[j];
+ const Real z = calcValue(pt, hint);
+ (*curRowVerts)[j] = mesh.addVertex(Vec3(pt[0],pt[1],z));
+ }
+ for (int j=1; j < ny; ++j) {
+ face[0] = (*curRowVerts)[j-1]; // counterclockwise
+ face[1] = (*curRowVerts)[j];
+ face[2] = (*prevRowVerts)[j];
+ face[3] = (*prevRowVerts)[j-1];
+ mesh.addFace(face);
+ }
+ std::swap(prevRowVerts, curRowVerts);
+ }
+}
+
+//==============================================================================
+// BICUBIC SURFACE :: PATCH HINT
+//==============================================================================
+
+// Here we ensure that there is always a valid PatchHint::Guts object present
+// and forward all operations to it.
+
+BicubicSurface::PatchHint::PatchHint() : guts(new PatchHint::Guts) {}
+BicubicSurface::PatchHint::~PatchHint() {delete guts;}
+
+BicubicSurface::PatchHint::PatchHint(const PatchHint& src)
+: guts(new PatchHint::Guts(*src.guts)) {}
+
+BicubicSurface::PatchHint&
+BicubicSurface::PatchHint::operator=(const PatchHint& src)
+{ *guts = *src.guts; return *this; }
+
+bool BicubicSurface::PatchHint::isEmpty() const {return guts->isEmpty();}
+void BicubicSurface::PatchHint::clear() {guts->clear();}
+
+
+
+
diff --git a/SimTKmath/Geometry/src/BicubicSurface_Guts.h b/SimTKmath/Geometry/src/BicubicSurface_Guts.h
new file mode 100644
index 0000000..b392079
--- /dev/null
+++ b/SimTKmath/Geometry/src/BicubicSurface_Guts.h
@@ -0,0 +1,359 @@
+#ifndef SimTK_BICUBIC_SURFACE_GUTS_H_
+#define SimTK_BICUBIC_SURFACE_GUTS_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Matthew Millard, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+This file defines the internal BicubicSurface::Guts class, which is the private
+implementation class of BicubicSurface, and BicubicSurface::PatchHint::Guts
+which is used to improve performance for repeated access to the same patch or
+nearby patches. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_BicubicHermitePatch.h"
+#include "simmath/internal/Geo_BicubicBezierPatch.h"
+#include "simmath/internal/BicubicSurface.h"
+
+#include <cassert>
+
+namespace SimTK {
+
+//==============================================================================
+// CLASS BICUBIC SURFACE :: PATCH HINT :: GUTS
+//==============================================================================
+/** This object is used to hold precalculated data about the most recently
+accessed patch to accelerate the common case of repeated access to the same
+patch or to nearby patches. **/
+class BicubicSurface::PatchHint::Guts {
+public:
+ Guts() {clear();}
+ // Default copy constructor, copy assignment, destructor
+
+ bool isEmpty() const {return x0 < 0;}
+ void clear() {
+ x0=y0=-1;
+ level = -2; // means "everything is invalid"
+ #ifndef NDEBUG
+ xS=ooxS=ooxS2=ooxS3=yS=ooyS=ooyS2=ooyS3 = NaN;
+ f=fx=fy=fxy=fxx=fyy=fxxx=fxxy=fyyy=fxyy = NaN;
+ #endif
+ }
+
+
+ // Patch information.
+ // This is valid whenever level >= -1 and does not change
+ // for repeated access anywhere within the same patch.
+ int x0, y0; // Indices of the lower-left corner of the patch.
+ // These are the precalculated patch dimensions and their reciprocals.
+ // xScale=x[x0+1]-x[x0], yScale=y[y0+1]-y[y0].
+ Real xS, ooxS, ooxS2, ooxS3;
+ Real yS, ooyS, ooyS2, ooyS3;
+ // These are the bicubic patch coefficients a00-a33 for this patch, in
+ // column order (a00 a10 a20 a30 a01 a11 ...).
+ Vec<16> a;
+ // These are the scaled function values at the corners of this patch, in
+ // the order f00,f10,f01,f11,
+ // fx00,fx10,fx01,fx11,
+ // fy00,fy10,fy01,fy11,
+ // fxy00,fxy10,fxy01,fxy11
+ Vec<16> fV;
+
+ // Point information.
+ // This information records the results of the last
+ // access to the above patch, which was point (x,y,f(x,y)). This will
+ // change whenever a new point is accessed, even if it is on the same patch.
+
+ // -2: nothing,
+ // -1: patch info only,
+ // 0: patch info and function value only,
+ // 1: patch info, function value, and 1st derivs,
+ // 2: ... plus 2nd derivatives,
+ // 3: ... plus 3rd derivatives.
+ int level;
+ Vec2 xy;
+ Real f; // f(x,y); valid if level >= 0.
+
+ // These are valid only if level has reached the value indicated.
+ Real fx, fy; // level >= 1
+ Real fxy, fxx, fyy; // level >= 2 (fyx==fxy)
+ Real fxxx, fxxy, fyyy, fxyy; // level == 3 (fyxx==fxyx==fxxy,
+ // fyyx==fyxy==fxyy)
+};
+
+
+
+//==============================================================================
+// CLASS BICUBIC SURFACE :: GUTS
+//==============================================================================
+// Private implementation of BicubicSurface.
+class SimTK_SIMMATH_EXPORT BicubicSurface::Guts {
+friend class BicubicSurface;
+public:
+ // Construct an uninitialized BicubicSurface object. This can be filled
+ // in later by assignment. Reference count is zero after construction.
+ Guts() {construct();}
+ // This class should not be destructed until the reference count drops
+ //to zero.
+ ~Guts() {assert(referenceCount==0);}
+
+
+ // Implementation of the specified-sample points constructor.
+ Guts(const Vector& x, const Vector& y, const Matrix& f,
+ Real smoothness);
+
+ // Implementation the regularly-sampled constructor.
+ Guts(const Vec2& XY, const Vec2& spacing,
+ const Matrix& f, Real smoothness);
+
+ // Implementation of the advanced constructor that gives all derivatives
+ // at each grid point.
+ Guts(const Vector& x, const Vector& y, const Matrix& f,
+ const Matrix& fx, const Matrix& fy, const Matrix& fxy);
+ Guts(const Vec2& XY, const Vec2& spacing, const Matrix& f,
+ const Matrix& fx, const Matrix& fy, const Matrix& fxy);
+
+ // Calculate the value of the surface at a particular XY coordinate.
+ Real calcValue(const Vec2& XY, PatchHint& hint) const;
+
+ // Calculate a partial derivative of this function at a particular XY
+ // coordinate.
+ Real calcDerivative(const Array_<int>& derivComponents,
+ const Vec2& XY, PatchHint& hint) const;
+
+ // Calculate a paraboloid and the max/min principal curvatures at a contact
+ // point at XY.
+ void calcParaboloid
+ (const Vec2& XY, PatchHint& hint, Transform& X_SP, Vec2& k) const;
+
+ void getNumPatches(int& nx, int &ny) const {
+ nx = _ff.nrow()-1;
+ ny = _ff.ncol()-1;
+ }
+
+ // Need to match out-of-order algebraic coefficients to the required
+ // matrix form:
+ // [ a33 a32 a31 a30 ]
+ // A = [ a23 a22 a21 a20 ]
+ // [ a13 a12 a11 a10 ]
+ // [ a03 a02 a01 a00 ]
+ // in which each entry is a 3-vector,
+ // given scalar algebraic coefficients for the z coordinates in this order:
+ // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
+ // a00 a10 a20 a30 a01 a11 a21 a31 a02 a12 a22 a32 a03 a13 a23 a33
+ //
+ // All x,y coefficients will be zero except for the constant and linear
+ // terms a00, a01, a10.
+ Geo::BicubicHermitePatch
+ calcHermitePatch(int i, int j, BicubicSurface::PatchHint& hint) const {
+ int nx, ny; getNumPatches(nx,ny);
+ SimTK_ERRCHK4_ALWAYS(0<=i && i<nx && 0<=j && j<ny,
+ "BicubicSurface::calcHermitePatch()",
+ "Patch coordinates (%d,%d) out of range [0..%d,0..%d].",
+ i,j,nx-1,ny-1);
+ BicubicSurface::PatchHint::Guts& h = hint.updGuts();
+ getPatchInfoIfNeeded(i,j,h); // need patch info
+ const Vec<16>& a = h.a;
+ const Vec3
+ a33(0,0,a[15]), a32(0,0,a[11]), a31(0, 0, a[7]), a30( 0, 0, a[3]),
+ a23(0,0,a[14]), a22(0,0,a[10]), a21(0, 0, a[6]), a20( 0, 0, a[2]),
+ a13(0,0,a[13]), a12(0,0,a[9] ), a11(0, 0, a[5]), a10( h.xS, 0, a[1]),
+ a03(0,0,a[12]), a02(0,0,a[8] ), a01(0,h.yS,a[4]), a00(_x[i],_y[j],a[0]);
+
+ const Mat<4,4,Vec3> A(a33, a32, a31, a30,
+ a23, a22, a21, a20,
+ a13, a12, a11, a10,
+ a03, a02, a01, a00);
+ return Geo::BicubicHermitePatch(A);
+ }
+
+ // Return as a Bezier patch. Currently we just create an algebraic patch
+ // which is cheap (~110 flops), and then do an expensive algebraic-to-Bezier
+ // conversion (~220 flops). I don't know of a cheaper approach.
+ Geo::BicubicBezierPatch
+ calcBezierPatch(int i, int j, BicubicSurface::PatchHint& hint) const {
+ const Geo::BicubicHermitePatch hpatch = calcHermitePatch(i,j,hint);
+ const Mat<4,4,Vec3>& A = hpatch.getAlgebraicCoefficients();
+ const Mat<4,4,Vec3> B = Geo::BicubicBezierPatch::calcBFromA(A);
+ return Geo::BicubicBezierPatch(B);
+ }
+
+ // Determine if a point is within the defined surface.
+ bool isSurfaceDefined(const Vec2& XY) const;
+
+ // Put a visualization mesh into the given PolygonalMesh argument.
+ void createPolygonalMesh(Real resolution, PolygonalMesh& mesh) const;
+
+ int getReferenceCount() const {return referenceCount;}
+ void incrReferenceCount() const {++referenceCount;}
+ int decrReferenceCount() const
+ {assert(referenceCount);return --referenceCount;}
+
+ // return The total number of elements in the matrix f
+ int getFnelt() const {return (int)_ff.nelt();}
+ // return The number of elements in the vector X
+ int getXnelt() const {return (int)_x.nelt();}
+ // return The number of elements in the vector Y
+ int getYnelt() const {return (int)_y.nelt();}
+
+ // Methods for debugging and testing
+
+ // Sets a flag that prints a lot of useful debugging data to the screen
+ // when this class is used.
+ void setDebug(bool aDebug) {_debug = aDebug;}
+
+ // return the matrix ff, which defines the grid points that the surface
+ // actually passes through, and derivatives fx,fy,fxy.
+ const Matrix_<Vec4>& getff() const {return _ff;}
+
+ // return the vector x, which defines one side of the mesh grid for
+ // which f(x,y) is defined.
+ const Vector& getx() const {return _x;}
+
+ // return the vector y, which defines one side of the mesh grid for
+ // which f(x,y) is defined.
+ const Vector& gety() const {return _y;}
+
+ /** Return the function values for the patch containing a particular point.
+ @param XY an (X,Y) location within the grid
+ @return The 16x1 vector that defines the values of f,fx,fy, and fxy at the
+ corners. The vector is given in the following order
+
+ f(0,0) f(1,0) f(0,1) f(1,1),
+ fx(0,0) fx(1,0) fx(0,1) fx(1,1),
+ fy(0,0) fy(1,0) fy(0,1) fy(1,1),
+ fxy(0,0) fxy(1,0) fxy(0,1) fxy(1,1)
+ **/
+ Vec<16> getPatchFunctionVector(const Vec2& XY) const {
+ BicubicSurface::PatchHint hint;
+ getFdF(XY,-1,hint); // just need patch info
+ const BicubicSurface::PatchHint::Guts& h = hint.getGuts();
+ return h.fV;
+
+ }
+
+ /** Return the patch coefficients for the patch containing a particular
+ point.
+ @param XY an (X,Y) location within the grid
+ @return The 16x1 vector that defines the values of the coefficients aij of
+ the bicubic surface interpolation in this patch. The coefficient
+ vector is given in the following order
+
+ a00,a10,a20,a30,a01,a11,a21,a31,a02,a12,a22,a32,a03,a13,a23,a33
+ */
+ Vec<16> getPatchBicubicCoefficients(const Vec2& XY) const {
+ BicubicSurface::PatchHint hint;
+ getFdF(XY,-1,hint); // just need patch info
+ const BicubicSurface::PatchHint::Guts& h = hint.getGuts();
+ return h.a;
+ }
+
+
+private:
+ int calcLowerBoundIndex(const Vector& vecV, Real value, int pIdx,
+ int& howResolved) const;
+ void getCoefficients(const Vec<16>& f, Vec<16>& aV) const;
+ void getFdF(const Vec2& aXY, int wantLevel,
+ BicubicSurface::PatchHint& hint) const;
+ void getPatchInfoIfNeeded(int x0, int y0,
+ BicubicSurface::PatchHint::Guts& h) const;
+
+ // This is called from each constructor to initialize this object.
+ void construct() {
+ referenceCount = 0;
+ resetStatistics();
+ _hasRegularSpacing = false;
+ _debug = false;
+ }
+
+ // Return true if the entries in this vector are monotonically increasing
+ // (no duplicates allowed).
+ static bool isMonotonicallyIncreasing(const Vector& v) {
+ for (int i=1; i < v.size(); ++i)
+ if (v[i] <= v[i-1]) return false;
+ return true;
+ }
+
+ // Shared by the irregular and regular-spaced constructors.
+ void constructFromSplines
+ (const Matrix& f, Real smoothness);
+ void constructFromKnownFunction
+ (const Matrix& f, const Matrix& fx, const Matrix& fy,
+ const Matrix& fxy);
+
+//=============================================================================
+// MEMBER VARIABLES
+//=============================================================================
+private:
+ // This class is reference counted and is not destructed until the
+ // reference count goes to zero.
+ mutable int referenceCount;
+
+ // Interesting statistics about the use of this surface.
+ mutable int numAccesses;
+ mutable int numAccessesSamePoint;
+ mutable int numAccessesSamePatch;
+ mutable int numAccessesNearbyPatch;
+ void resetStatistics() const
+ { numAccesses = numAccessesSamePoint = 0;
+ numAccessesSamePatch = numAccessesNearbyPatch = 0; }
+
+ // PROPERTIES
+ // Array of values for the independent variables (i.e., the spline knot
+ // sequence). Each array must be monotonically increasing, of size nx and
+ // ny elements. These are filled in even if the grid is regularly
+ // spaced, although we'll use the spacing to avoid a search. The values
+ // here define the actual grid points; if roundoff causes a calculated
+ // grid location to be different, these take priority.
+ Vector _x, _y;
+
+ // If the grid has regular spacing we remember the specified spacing
+ // here and use it instead of searching when we need to move to a new patch.
+ // Note that the grid values are _x[0]+i*spacing[0] and _y[0]+j*spacing[1],
+ // but you still have to check in _x and _y to avoid roundoff problems.
+ bool _hasRegularSpacing;
+ Vec2 _spacing;
+
+ // 2D nx X ny z values that correspond to the values at the grid defined
+ // by x and y, and the partial differentials at those grid points. The
+ // entries at each grid point are ordered f, fx, fy, fxy.
+ enum {F=0, Fx=1, Fy=2, Fxy=3};
+ Matrix_<Vec4> _ff;
+
+ //A private debugging flag - if set to true, a lot of useful debugging
+ //data will be printed tot the screen
+ bool _debug;
+
+
+//=============================================================================
+}; // END class BicubicSurface::Guts
+
+
+
+}; //namespace
+//=============================================================================
+//=============================================================================
+
+#endif // SimTK_BICUBIC_SURFACE_GUTS_H_
diff --git a/SimTKmath/Geometry/src/CollisionDetectionAlgorithm.cpp b/SimTKmath/Geometry/src/CollisionDetectionAlgorithm.cpp
new file mode 100644
index 0000000..dc16aa8
--- /dev/null
+++ b/SimTKmath/Geometry/src/CollisionDetectionAlgorithm.cpp
@@ -0,0 +1,798 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 <set>
+
+using std::map;
+using std::pair;
+using std::set;
+
+namespace SimTK {
+
+//==============================================================================
+// COLLISION DETECTION ALGORITHM
+//==============================================================================
+
+CollisionDetectionAlgorithm::AlgorithmMap::~AlgorithmMap() {
+ // Clean up algorithms to satisfy valgrind
+ for (iterator i = begin(); i != end(); ++i)
+ delete i->second;
+}
+
+CollisionDetectionAlgorithm::AlgorithmMap
+ CollisionDetectionAlgorithm::algorithmMap;
+
+static int registerStandardAlgorithms() {
+ CollisionDetectionAlgorithm::registerAlgorithm
+ (ContactGeometry::HalfSpace::classTypeId(),
+ ContactGeometry::Sphere::classTypeId(),
+ new CollisionDetectionAlgorithm::HalfSpaceSphere());
+ CollisionDetectionAlgorithm::registerAlgorithm
+ (ContactGeometry::Sphere::classTypeId(),
+ ContactGeometry::Sphere::classTypeId(),
+ new CollisionDetectionAlgorithm::SphereSphere());
+ CollisionDetectionAlgorithm::registerAlgorithm
+ (ContactGeometry::HalfSpace::classTypeId(),
+ ContactGeometry::Ellipsoid::classTypeId(),
+ new CollisionDetectionAlgorithm::HalfSpaceEllipsoid());
+ CollisionDetectionAlgorithm::registerAlgorithm
+ (ContactGeometry::Ellipsoid::classTypeId(),
+ ContactGeometry::Sphere::classTypeId(),
+ new CollisionDetectionAlgorithm::ConvexConvex());
+ CollisionDetectionAlgorithm::registerAlgorithm
+ (ContactGeometry::Ellipsoid::classTypeId(),
+ ContactGeometry::Ellipsoid::classTypeId(),
+ new CollisionDetectionAlgorithm::ConvexConvex());
+ CollisionDetectionAlgorithm::registerAlgorithm
+ (ContactGeometry::HalfSpace::classTypeId(),
+ ContactGeometry::TriangleMesh::classTypeId(),
+ new CollisionDetectionAlgorithm::HalfSpaceTriangleMesh());
+ CollisionDetectionAlgorithm::registerAlgorithm
+ (ContactGeometry::Sphere::classTypeId(),
+ ContactGeometry::TriangleMesh::classTypeId(),
+ new CollisionDetectionAlgorithm::SphereTriangleMesh());
+ CollisionDetectionAlgorithm::registerAlgorithm
+ (ContactGeometry::TriangleMesh::classTypeId(),
+ ContactGeometry::TriangleMesh::classTypeId(),
+ new CollisionDetectionAlgorithm::TriangleMeshTriangleMesh());
+ return 1;
+}
+
+static int staticInitializer = registerStandardAlgorithms();
+
+void CollisionDetectionAlgorithm::registerAlgorithm
+ (ContactGeometryTypeId type1, ContactGeometryTypeId type2,
+ CollisionDetectionAlgorithm* algorithm) {
+ algorithmMap[std::make_pair(type1,type2)] = algorithm;
+}
+
+CollisionDetectionAlgorithm* CollisionDetectionAlgorithm::
+getAlgorithm(ContactGeometryTypeId type1, ContactGeometryTypeId type2) {
+ AlgorithmMap::const_iterator
+ iter = algorithmMap.find(std::make_pair(type1, type2));
+ if (iter == algorithmMap.end())
+ return NULL;
+ return iter->second;
+
+}
+
+
+
+//==============================================================================
+// HALF SPACE - SPHERE
+//==============================================================================
+void CollisionDetectionAlgorithm::HalfSpaceSphere::processObjects
+ (ContactSurfaceIndex index1, const ContactGeometry& object1,
+ const Transform& transform1,
+ ContactSurfaceIndex index2, const ContactGeometry& object2,
+ const Transform& transform2,
+ Array_<Contact>& contacts) const
+{
+ const ContactGeometry::Sphere& sphere =
+ ContactGeometry::Sphere::getAs(object2);
+ // Location of the sphere in the half-space's coordinate frame
+ Vec3 location = (~transform1)*transform2.p();
+ Real r = sphere.getRadius();
+ Real depth = r+location[0];
+ if (depth > 0) {
+ // They are overlapping.
+
+ Vec3 normal = transform1.R()*Vec3(-1, 0, 0);
+ Vec3 contactLocation =
+ transform1*Vec3(depth/2, location[1], location[2]);
+ contacts.push_back(PointContact(index1, index2, contactLocation,
+ normal, r, depth));
+ }
+}
+
+
+
+//==============================================================================
+// SPHERE - SPHERE
+//==============================================================================
+void CollisionDetectionAlgorithm::SphereSphere::processObjects
+ (ContactSurfaceIndex index1, const ContactGeometry& object1,
+ const Transform& transform1,
+ ContactSurfaceIndex index2, const ContactGeometry& object2,
+ const Transform& transform2,
+ Array_<Contact>& contacts) const
+{
+ const ContactGeometry::Sphere& sphere1 =
+ ContactGeometry::Sphere::getAs(object1);
+ const ContactGeometry::Sphere& sphere2 =
+ ContactGeometry::Sphere::getAs(object2);
+ Vec3 delta = transform2.p()-transform1.p();
+ Real dist = delta.norm();
+ if (dist == 0)
+ return; // No sensible way to deal with this.
+ Real r1 = sphere1.getRadius();
+ Real r2 = sphere2.getRadius();
+ Real depth = r1+r2-dist;
+ if (depth > 0) {
+ // They are overlapping.
+
+ Real radius = r1*r2/(r1+r2);
+ Vec3 normal = delta/dist;
+ Vec3 location = transform1.p()+(r1-depth/2)*normal;
+ contacts.push_back(PointContact(index1, index2, location, normal, radius, depth));
+ }
+}
+
+
+
+//==============================================================================
+// HALF SPACE - ELLIPSOID
+//==============================================================================
+void CollisionDetectionAlgorithm::HalfSpaceEllipsoid::processObjects
+ (ContactSurfaceIndex index1, const ContactGeometry& object1,
+ const Transform& transform1,
+ ContactSurfaceIndex index2, const ContactGeometry& object2,
+ const Transform& transform2,
+ Array_<Contact>& contacts) const
+{
+ const ContactGeometry::Ellipsoid& ellipsoid =
+ ContactGeometry::Ellipsoid::getAs(object2);
+
+ const Vec3& radii = ellipsoid.getRadii();
+ const Vec3 r2(radii[0]*radii[0], radii[1]*radii[1], radii[2]*radii[2]);
+ const Vec3 ri2(1/r2[0], 1/r2[1], 1/r2[2]);
+ // Transform giving half space frame in the ellipsoid's frame.
+ const Transform trans = (~transform1)*transform2;
+ Vec3 normal = ~trans.R()*Vec3(-1, 0, 0);
+ Vec3 location(normal[0]*r2[0], normal[1]*r2[1], normal[2]*r2[2]);
+ location /= -std::sqrt( normal[0]*location[0]
+ + normal[1]*location[1]
+ + normal[2]*location[2]);
+ Real depth = (trans*location)[0];
+ if (depth > 0) {
+ // They are overlapping. We need to calculate the principal radii of
+ // curvature.
+
+ Vec3 v1 = ~trans.R()*Vec3(0, 1, 0);
+ Vec3 v2 = ~trans.R()*Vec3(0, 0, 1);
+ Real dxx = v1[0]*v1[0]*ri2[0] + v1[1]*v1[1]*ri2[1] + v1[2]*v1[2]*ri2[2];
+ Real dyy = v2[0]*v2[0]*ri2[0] + v2[1]*v2[1]*ri2[1] + v2[2]*v2[2]*ri2[2];
+ Real dxy = v1[0]*v2[0]*ri2[0] + v1[1]*v2[1]*ri2[1] + v1[2]*v2[2]*ri2[2];
+ Vec<2, complex<Real> > eigenvalues;
+ PolynomialRootFinder::findRoots(Vec3(1, -dxx-dyy, dxx*dyy-dxy*dxy),
+ eigenvalues);
+ Vec3 contactNormal = transform2.R()*normal;
+ Vec3 contactPoint = transform2*(location+(depth/2)*normal);
+ contacts.push_back(PointContact(index1, index2, contactPoint,
+ contactNormal,
+ 1/std::sqrt(eigenvalues[0].real()),
+ 1/std::sqrt(eigenvalues[1].real()),
+ depth));
+ }
+}
+
+
+
+//==============================================================================
+// HALF SPACE - TRIANGLE MESH
+//==============================================================================
+void CollisionDetectionAlgorithm::HalfSpaceTriangleMesh::processObjects
+ (ContactSurfaceIndex index1, const ContactGeometry& object1,
+ const Transform& X_GH,
+ ContactSurfaceIndex index2, const ContactGeometry& object2,
+ const Transform& X_GM,
+ Array_<Contact>& contacts) const
+{
+ const ContactGeometry::TriangleMesh& mesh =
+ ContactGeometry::TriangleMesh::getAs(object2);
+ // Transform giving mesh (S2) frame in the halfspace (S1) frame.
+ const Transform X_HM = (~X_GH)*X_GM;
+ set<int> insideFaces;
+ Vec3 axisDir = ~X_HM.R()*Vec3(-1, 0, 0);
+ Real xoffset = ~axisDir*(~X_HM*Vec3(0));
+ processBox(mesh, mesh.getOBBTreeNode(), X_HM, axisDir, xoffset, insideFaces);
+ if (insideFaces.size() > 0)
+ contacts.push_back(TriangleMeshContact(index1, index2, X_HM,
+ set<int>(), insideFaces));
+}
+
+void CollisionDetectionAlgorithm::HalfSpaceTriangleMesh::processBox
+ (const ContactGeometry::TriangleMesh& mesh,
+ const ContactGeometry::TriangleMesh::OBBTreeNode& node,
+ const Transform& X_HM, const Vec3& axisDir, Real xoffset,
+ set<int>& insideFaces) const
+{ // First check against the node's bounding box.
+
+ OrientedBoundingBox bounds = node.getBounds();
+ const Vec3 b = bounds.getSize() / 2;
+ Vec3 boxCenter = bounds.getTransform()*b;
+ Real radius = ~b*(~bounds.getTransform().R()*axisDir).abs();
+ Real dist = ~axisDir*boxCenter-xoffset;
+ if (dist > radius)
+ return;
+ if (dist < -radius) {
+ addAllTriangles(node, insideFaces);
+ return;
+ }
+
+ // If it is not a leaf node, check its children.
+
+ if (!node.isLeafNode()) {
+ processBox(mesh, node.getFirstChildNode(), X_HM, axisDir, xoffset, insideFaces);
+ processBox(mesh, node.getSecondChildNode(), X_HM, axisDir, xoffset, insideFaces);
+ return;
+ }
+
+ // Check the triangles.
+
+ const Array_<int>& triangles = node.getTriangles();
+ const Row3 xdir = X_HM.R().row(0);
+ const Real tx = X_HM.p()[0];
+ for (int i = 0; i < (int) triangles.size(); i++) {
+ if (xdir*mesh.getVertexPosition(mesh.getFaceVertex(triangles[i], 0))+tx > 0)
+ insideFaces.insert(triangles[i]);
+ else if (xdir*mesh.getVertexPosition(mesh.getFaceVertex(triangles[i], 1))+tx > 0)
+ insideFaces.insert(triangles[i]);
+ else if (xdir*mesh.getVertexPosition(mesh.getFaceVertex(triangles[i], 2))+tx > 0)
+ insideFaces.insert(triangles[i]);
+ }
+}
+
+void CollisionDetectionAlgorithm::HalfSpaceTriangleMesh::addAllTriangles
+ (const ContactGeometry::TriangleMesh::OBBTreeNode& node,
+ std::set<int>& insideFaces) const
+{
+ if (node.isLeafNode()) {
+ const Array_<int>& triangles = node.getTriangles();
+ for (int i = 0; i < (int) triangles.size(); i++)
+ insideFaces.insert(triangles[i]);
+ }
+ else {
+ addAllTriangles(node.getFirstChildNode(), insideFaces);
+ addAllTriangles(node.getSecondChildNode(), insideFaces);
+ }
+}
+
+
+
+//==============================================================================
+// SPHERE - TRIANGLE MESH
+//==============================================================================
+void CollisionDetectionAlgorithm::SphereTriangleMesh::processObjects
+ (ContactSurfaceIndex index1, const ContactGeometry& object1, // sphere
+ const Transform& X_GS,
+ ContactSurfaceIndex index2, const ContactGeometry& object2, // mesh
+ const Transform& X_GM,
+ Array_<Contact>& contacts) const
+{
+ const ContactGeometry::Sphere& sphere =
+ ContactGeometry::Sphere::getAs(object1);
+ const ContactGeometry::TriangleMesh& mesh =
+ ContactGeometry::TriangleMesh::getAs(object2);
+
+ const Transform X_SM = ~X_GS*X_GM;
+ // Want the sphere center measured and expressed in the mesh frame.
+ const Vec3 p_MC = (~X_SM).p();
+ set<int> insideFaces;
+ processBox(p_MC, sphere.getRadius()*sphere.getRadius(),
+ mesh, mesh.getOBBTreeNode(), insideFaces);
+ if (insideFaces.size() > 0)
+ contacts.push_back(TriangleMeshContact(index1, index2, X_SM,
+ set<int>(), insideFaces));
+}
+
+void CollisionDetectionAlgorithm::SphereTriangleMesh::processBox
+ (const Vec3& center, Real radius2,
+ const ContactGeometry::TriangleMesh& mesh,
+ const ContactGeometry::TriangleMesh::OBBTreeNode& node,
+ set<int>& insideFaces) const
+{ // First check against the node's bounding box.
+
+ Vec3 nearestPoint = node.getBounds().findNearestPoint(center);
+ if ((nearestPoint-center).normSqr() > radius2)
+ return;
+
+ // If it is not a leaf node, check its children.
+
+ if (!node.isLeafNode()) {
+ processBox(center, radius2, mesh, node.getFirstChildNode(), insideFaces);
+ processBox(center, radius2, mesh, node.getSecondChildNode(), insideFaces);
+ return;
+ }
+
+ // Check the triangles.
+
+ const Array_<int>& triangles = node.getTriangles();
+ for (int i = 0; i < (int) triangles.size(); i++) {
+ Vec2 uv;
+ Vec3 nearestPoint = mesh.findNearestPointToFace
+ (center, triangles[i], uv);
+ if ((nearestPoint-center).normSqr() < radius2)
+ insideFaces.insert(triangles[i]);
+ }
+}
+
+
+
+//==============================================================================
+// TRIANGLE MESH - TRIANGLE MESH
+//==============================================================================
+void CollisionDetectionAlgorithm::TriangleMeshTriangleMesh::
+processObjects
+ (ContactSurfaceIndex index1, const ContactGeometry& object1,
+ const Transform& X_GM1,
+ ContactSurfaceIndex index2, const ContactGeometry& object2,
+ const Transform& X_GM2,
+ Array_<Contact>& contacts) const
+{
+ const ContactGeometry::TriangleMesh& mesh1 =
+ ContactGeometry::TriangleMesh::getAs(object1);
+ const ContactGeometry::TriangleMesh& mesh2 =
+ ContactGeometry::TriangleMesh::getAs(object2);
+
+ // Get mesh2's frame measured and expressed in mesh1's frame.
+ const Transform X_M1M2 = (~X_GM1)*X_GM2;
+ set<int> triangles1;
+ set<int> triangles2;
+ OrientedBoundingBox mesh2Bounds = X_M1M2*mesh2.getOBBTreeNode().getBounds();
+ processNodes(mesh1, mesh2, mesh1.getOBBTreeNode(), mesh2.getOBBTreeNode(),
+ mesh2Bounds, X_M1M2, triangles1, triangles2);
+ if (triangles1.size() == 0)
+ return; // No intersection.
+
+ // There was an intersection. We now need to identify every triangle and vertex of each mesh that is inside the other mesh.
+
+ findInsideTriangles(mesh1, mesh2, ~X_M1M2, triangles1);
+ findInsideTriangles(mesh2, mesh1, X_M1M2, triangles2);
+ contacts.push_back(TriangleMeshContact(index1, index2, X_M1M2,
+ triangles1, triangles2));
+}
+
+void CollisionDetectionAlgorithm::TriangleMeshTriangleMesh::
+processNodes
+ (const ContactGeometry::TriangleMesh& mesh1,
+ const ContactGeometry::TriangleMesh& mesh2,
+ const ContactGeometry::TriangleMesh::OBBTreeNode& node1,
+ const ContactGeometry::TriangleMesh::OBBTreeNode& node2,
+ const OrientedBoundingBox& node2Bounds,
+ const Transform& X_M1M2,
+ set<int>& triangles1,
+ set<int>& triangles2) const
+{ // See if the bounding boxes intersect.
+
+ if (!node1.getBounds().intersectsBox(node2Bounds))
+ return;
+
+ // If either node is not a leaf node, process the children recursively.
+
+ if (!node1.isLeafNode()) {
+ if (!node2.isLeafNode()) {
+ OrientedBoundingBox firstChildBounds = X_M1M2*node2.getFirstChildNode().getBounds();
+ OrientedBoundingBox secondChildBounds = X_M1M2*node2.getSecondChildNode().getBounds();
+ processNodes(mesh1, mesh2, node1.getFirstChildNode(), node2.getFirstChildNode(), firstChildBounds, X_M1M2, triangles1, triangles2);
+ processNodes(mesh1, mesh2, node1.getFirstChildNode(), node2.getSecondChildNode(), secondChildBounds, X_M1M2, triangles1, triangles2);
+ processNodes(mesh1, mesh2, node1.getSecondChildNode(), node2.getFirstChildNode(), firstChildBounds, X_M1M2, triangles1, triangles2);
+ processNodes(mesh1, mesh2, node1.getSecondChildNode(), node2.getSecondChildNode(), secondChildBounds, X_M1M2, triangles1, triangles2);
+ }
+ else {
+ processNodes(mesh1, mesh2, node1.getFirstChildNode(), node2, node2Bounds, X_M1M2, triangles1, triangles2);
+ processNodes(mesh1, mesh2, node1.getSecondChildNode(), node2, node2Bounds, X_M1M2, triangles1, triangles2);
+ }
+ return;
+ }
+ else if (!node2.isLeafNode()) {
+ OrientedBoundingBox firstChildBounds = X_M1M2*node2.getFirstChildNode().getBounds();
+ OrientedBoundingBox secondChildBounds = X_M1M2*node2.getSecondChildNode().getBounds();
+ processNodes(mesh1, mesh2, node1, node2.getFirstChildNode(), firstChildBounds, X_M1M2, triangles1, triangles2);
+ processNodes(mesh1, mesh2, node1, node2.getSecondChildNode(), secondChildBounds, X_M1M2, triangles1, triangles2);
+ return;
+ }
+
+ // These are both leaf nodes, so check triangles for intersections.
+
+ const Array_<int>& node1triangles = node1.getTriangles();
+ const Array_<int>& node2triangles = node2.getTriangles();
+ for (int i = 0; i < (int) node2triangles.size(); i++) {
+ Vec3 a1 = X_M1M2*mesh2.getVertexPosition(mesh2.getFaceVertex(node2triangles[i], 0));
+ Vec3 a2 = X_M1M2*mesh2.getVertexPosition(mesh2.getFaceVertex(node2triangles[i], 1));
+ Vec3 a3 = X_M1M2*mesh2.getVertexPosition(mesh2.getFaceVertex(node2triangles[i], 2));
+ Geo::Triangle A(a1,a2,a3);
+ for (int j = 0; j < (int) node1triangles.size(); j++) {
+ const Vec3& b1 = mesh1.getVertexPosition(mesh1.getFaceVertex(node1triangles[j], 0));
+ const Vec3& b2 = mesh1.getVertexPosition(mesh1.getFaceVertex(node1triangles[j], 1));
+ const Vec3& b3 = mesh1.getVertexPosition(mesh1.getFaceVertex(node1triangles[j], 2));
+ Geo::Triangle B(b1,b2,b3);
+ if (A.overlapsTriangle(B)) {
+ // The triangles intersect.
+ triangles1.insert(node1triangles[j]);
+ triangles2.insert(node2triangles[i]);
+ }
+ }
+ }
+}
+
+void CollisionDetectionAlgorithm::TriangleMeshTriangleMesh::
+findInsideTriangles(const ContactGeometry::TriangleMesh& mesh, // M
+ const ContactGeometry::TriangleMesh& otherMesh, // O
+ const Transform& X_OM,
+ set<int>& triangles) const
+{ // Find which triangles are inside.
+ const int Unknown = UNKNOWN; // work around gcc bug
+ Array_<int> faceType(mesh.getNumFaces(), Unknown);
+ for (set<int>::iterator iter = triangles.begin();
+ iter != triangles.end(); ++iter)
+ faceType[*iter] = BOUNDARY;
+
+ for (int i = 0; i < (int) faceType.size(); i++) {
+ if (faceType[i] == UNKNOWN) {
+ // Trace a ray from its center to determine whether it is inside.
+ const Vec3 origin_O = X_OM * mesh.findCentroid(i);
+ const UnitVec3 direction_O = X_OM.R()* mesh.getFaceNormal(i);
+ Real distance;
+ int face;
+ Vec2 uv;
+ if ( otherMesh.intersectsRay(origin_O, direction_O, distance,
+ face, uv)
+ && ~direction_O*otherMesh.getFaceNormal(face) > 0)
+ {
+ faceType[i] = INSIDE;
+ triangles.insert(i);
+ } else
+ faceType[i] = OUTSIDE;
+
+ // Recursively mark adjacent triangles.
+ tagFaces(mesh, faceType, triangles, i, 0);
+ }
+ }
+}
+
+//TODO: the following method uses depth-first recursion to iterate through
+//unmarked faces. For a large mesh this was observed to produce a stack
+//overflow in OpenSim. Here we limit the recursion depth; after we get that
+//deep we'll pop back out and do another expensive intersectsRay() test in
+//the method above.
+static const int MaxRecursionDepth = 500;
+
+void CollisionDetectionAlgorithm::TriangleMeshTriangleMesh::
+tagFaces(const ContactGeometry::TriangleMesh& mesh,
+ Array_<int>& faceType,
+ set<int>& triangles,
+ int index,
+ int depth) const
+{
+ for (int i = 0; i < 3; i++) {
+ int edge = mesh.getFaceEdge(index, i);
+ int face = (mesh.getEdgeFace(edge, 0) == index ? mesh.getEdgeFace(edge, 1) : mesh.getEdgeFace(edge, 0));
+ if (faceType[face] == UNKNOWN) {
+ faceType[face] = faceType[index];
+ if (faceType[index] > 0)
+ triangles.insert(face);
+ if (depth < MaxRecursionDepth)
+ tagFaces(mesh, faceType, triangles, face, depth+1);
+ }
+ }
+}
+
+
+
+//==============================================================================
+// CONVEX - CONVEX
+//==============================================================================
+// This is an implementation based on the Minkowski Portal Refinement method
+// used by XenoCollide. See G. Snethen, �Xenocollide: Complex collision made
+// simple,� in Game Programming Gems 7, 2008. MPR is used to obtain an initial
+// guess for the contact location which is then refined to numerical precision
+// by using the smooth representation of the colliding objects.
+
+Vec3 CollisionDetectionAlgorithm::ConvexConvex::
+computeSupport(const ContactGeometry& obj1,
+ const ContactGeometry& obj2,
+ const Transform& transform, UnitVec3 direction) {
+ return obj1.calcSupportPoint(direction)
+ - transform*obj2.calcSupportPoint(~transform.R()*-direction);
+}
+
+void CollisionDetectionAlgorithm::ConvexConvex::processObjects
+ (ContactSurfaceIndex index1, const ContactGeometry& obj1,
+ const Transform& transform1,
+ ContactSurfaceIndex index2, const ContactGeometry& obj2,
+ const Transform& transform2,
+ Array_<Contact>& contacts) const
+{
+ Transform transform = ~transform1*transform2;
+
+ // Compute a point that is known to be inside the Minkowski difference, and
+ // a ray directed from that point to the origin.
+
+ Vec3 v0 = ( computeSupport(obj1, obj2, transform, UnitVec3(1, 0, 0))
+ + computeSupport(obj1, obj2, transform, UnitVec3(-1, 0, 0))) / 2;
+ if (v0 == 0.0) {
+ // This is a pathological case: the two objects are directly on top of
+ // each other with their centers at exactly the same place. Just
+ // return *some* vaguely plausible contact.
+
+ Vec3 point1 = obj1.calcSupportPoint(UnitVec3(1, 0, 0));
+ Vec3 point2 = obj2.calcSupportPoint(~transform.R()*UnitVec3(-1, 0, 0));
+ addContact(index1, index2, obj1, obj2, transform1, transform2,
+ transform, point1, point2, contacts);
+ return;
+ }
+
+ // Select three points that define the initial portal.
+
+ UnitVec3 dir1 = UnitVec3(-v0);
+ Vec3 v1 = computeSupport(obj1, obj2, transform, dir1);
+ if (~v1*dir1 <= 0.0)
+ return;
+ if (v1%v0 == 0.0) {
+ Vec3 point1 = obj1.calcSupportPoint(dir1);
+ Vec3 point2 = obj2.calcSupportPoint(~transform.R()*-dir1);
+ addContact(index1, index2, obj1, obj2, transform1, transform2,
+ transform, point1, point2, contacts);
+ return;
+ }
+ UnitVec3 dir2 = UnitVec3(v1%v0);
+ Vec3 v2 = computeSupport(obj1, obj2, transform, dir2);
+ if (~v2*dir2 <= 0.0)
+ return;
+ UnitVec3 dir3 = UnitVec3((v1-v0)%(v2-v0));
+ if (~dir3*v0 > 0) {
+ UnitVec3 swap1 = dir1;
+ Vec3 swap2 = v1;
+ dir1 = dir2;
+ v1 = v2;
+ dir2 = swap1;
+ v2 = swap2;
+ dir3 = -dir3;
+ }
+ Vec3 v3 = computeSupport(obj1, obj2, transform, dir3);
+ if (~v3*dir3 <= 0.0)
+ return;
+ while (true) {
+ if (~v0*(v1%v3) < -1e-14) {
+ dir2 = dir3;
+ v2 = v3;
+ }
+ else if (~v0*(v3%v2) < -1e-14) {
+ dir1 = dir3;
+ v1 = v3;
+ }
+ else
+ break;
+ dir3 = UnitVec3((v1-v0)%(v2-v0));
+ v3 = computeSupport(obj1, obj2, transform, dir3);
+ }
+
+ // We have a portal that the origin ray passes through. Now we need to
+ // refine it.
+
+ while (true) {
+ UnitVec3 portalDir = UnitVec3((v2-v1)%(v3-v1));
+ if (~portalDir*v0 > 0)
+ portalDir = -portalDir;
+ Real dist1 = ~portalDir*v1;
+ Vec3 v4 = computeSupport(obj1, obj2, transform, portalDir);
+ Real dist4 = ~portalDir*v4;
+ if (dist1 >= 0.0) {
+ // The origin is inside the portal, so we have an intersection.
+ // Compute the barycentric coordinates of the origin in the outer
+ // face of the portal.
+
+ Vec3 origin = v0+v0*(~portalDir*(v1-v0)/(~portalDir*v0));
+ Real totalArea = ((v2-v1)%(v3-v1)).norm();
+ Real area1 = ~portalDir*((v2-origin)%(v3-origin));
+ Real area2 = ~portalDir*((v3-origin)%(v1-origin));
+ Real u = area1/totalArea;
+ Real v = area2/totalArea;
+ Real w = 1-u-v;
+
+ // Compute the contact properties.
+
+ Vec3 point1 = u*obj1.calcSupportPoint(dir1)
+ + v*obj1.calcSupportPoint(dir2)
+ + w*obj1.calcSupportPoint(dir3);
+ Vec3 point2 = u*obj2.calcSupportPoint(~transform.R()*-dir1)
+ + v*obj2.calcSupportPoint(~transform.R()*-dir2)
+ + w*obj2.calcSupportPoint(~transform.R()*-dir3);
+ addContact(index1, index2, obj1, obj2,
+ transform1, transform2, transform,
+ point1, point2, contacts);
+ return;
+ }
+ if (dist4 <= 0.0)
+ return;
+ Vec3 cross = v4%v0;
+ if (~v1*cross > 0.0) {
+ if (~v2*cross > 0.0) {
+ dir1 = portalDir;
+ v1 = v4;
+ }
+ else {
+ dir3 = portalDir;
+ v3 = v4;
+ }
+ }
+ else {
+ if (~v3*cross > 0.0) {
+ dir2 = portalDir;
+ v2 = v4;
+ }
+ else {
+ dir1 = portalDir;
+ v1 = v4;
+ }
+ }
+ }
+}
+
+void CollisionDetectionAlgorithm::ConvexConvex::addContact
+ (ContactSurfaceIndex index1, ContactSurfaceIndex index2,
+ const ContactGeometry& object1,
+ const ContactGeometry& object2,
+ const Transform& transform1, const Transform& transform2,
+ const Transform& transform12,
+ Vec3 point1, Vec3 point2, Array_<Contact>& contacts) {
+ // We have a rough estimate of the contact points. Use Newton iteration to
+ // refine them.
+
+ Vec6 err = computeErrorVector(object1, object2, point1, point2, transform12);
+ while (err.norm() > Real(1e-12)) {
+ Mat66 J = computeJacobian(object1, object2, point1, point2, transform12);
+ FactorQTZ qtz;
+ qtz.factor(Matrix(J), Real(1e-6));
+ Vector deltaVec(6);
+ qtz.solve(Vector(err), deltaVec);
+ Vec6 delta(&deltaVec[0]);
+
+ // Line search for safety in case starting guess bad.
+
+ Real f = 2; // scale back factor
+ Vec3 point1old = point1, point2old = point2;
+ Vec6 errold = err;
+ do {
+ f /= 2;
+ point1 = point1old - f*delta.getSubVec<3>(0);
+ point2 = point2old - f*delta.getSubVec<3>(3);
+ err = computeErrorVector(object1, object2, point1, point2,
+ transform12);
+ } while (err.norm() > errold.norm());
+ if (f < 0.1) {
+ // We're clearly outside the region where Newton iteration is going
+ // to work properly. Just project the points onto the surfaces and
+ // then exit.
+
+ bool inside;
+ UnitVec3 normal;
+ point1 = object1.findNearestPoint(point1, inside, normal);
+ point2 = object2.findNearestPoint(point2, inside, normal);
+ break;
+ }
+ }
+
+ // Compute the curvature of the two surfaces.
+
+ Vec2 curvature1, curvature2;
+ Rotation orientation1, orientation2;
+ object1.calcCurvature(point1, curvature1, orientation1);
+ object2.calcCurvature(point2, curvature2, orientation2);
+ Vec2 curvature;
+ UnitVec3 maxDir2(transform12.R()*orientation2(0));
+ ContactGeometry::combineParaboloids(orientation1, curvature1,
+ maxDir2, curvature2, curvature);
+
+ // Record the contact.
+
+ Vec3 p1 = transform1*point1;
+ Vec3 p2 = transform2*point2;
+ Vec3 position = (p1+p2)/2;
+ UnitVec3 normal(p1-p2);
+ contacts.push_back(PointContact(index1, index2, position, normal,
+ 1/curvature[0], 1/curvature[1],
+ (p1-p2).norm()));
+}
+
+Vec6 CollisionDetectionAlgorithm::ConvexConvex::computeErrorVector
+ (const ContactGeometry& object1,
+ const ContactGeometry& object2,
+ Vec3 pos1, Vec3 pos2, const Transform& transform12) {
+ // Compute the function value and normal vector for each object.
+
+ const Function& f1 = object1.getImplicitFunction();
+ const Function& f2 = object2.getImplicitFunction();
+ Vector x(3);
+ Array_<int> components(1);
+ Vec3 grad1, grad2;
+ Vec3::updAs(&x[0]) = pos1;
+ for (int i = 0; i < 3; i++) {
+ components[0] = i;
+ grad1[i] = f1.calcDerivative(components, x);
+ }
+ Real error1 = f1.calcValue(x);
+ Vec3::updAs(&x[0]) = pos2;
+ for (int i = 0; i < 3; i++) {
+ components[0] = i;
+ grad2[i] = f2.calcDerivative(components, x);
+ }
+ Real error2 = f2.calcValue(x);
+
+ // Construct a coordinate frame for each object.
+
+ UnitVec3 n1(-grad1);
+ UnitVec3 n2(-transform12.R()*grad2);
+ UnitVec3 u1(fabs(n1[0]) > 0.5 ? n1%Vec3(0, 1, 0) : n1%Vec3(1, 0, 0));
+ UnitVec3 u2(fabs(n2[0]) > 0.5 ? n2%Vec3(0, 1, 0) : n2%Vec3(1, 0, 0));
+ Vec3 v1 = n1%u1; // Already a unit vector, so we don't need to normalize it.
+ Vec3 v2 = n2%u2;
+
+ // Compute the error vector. The components indicate, in order, that n1
+ // must be perpendicular to both tangents of object 2, that the separation
+ // vector should be zero or perpendicular to the tangents of object 1, and
+ // that both points should be on their respective surfaces.
+
+ Vec3 delta = pos1-transform12*pos2;
+ return Vec6(~n1*u2, ~n1*v2, ~delta*u1, ~delta*v1, error1, error2);
+}
+
+Mat66 CollisionDetectionAlgorithm::ConvexConvex::computeJacobian
+ (const ContactGeometry& object1,
+ const ContactGeometry& object2,
+ Vec3 pos1, Vec3 pos2, const Transform& transform12) {
+ Real dt = Real(1e-7);
+ Vec6 err0 = computeErrorVector(object1, object2, pos1, pos2, transform12);
+ Vec3 d1 = dt*Vec3(1, 0, 0);
+ Vec3 d2 = dt*Vec3(0, 1, 0);
+ Vec3 d3 = dt*Vec3(0, 0, 1);
+ Vec6 err1 = computeErrorVector(object1, object2, pos1+d1, pos2, transform12)
+ - err0;
+ Vec6 err2 = computeErrorVector(object1, object2, pos1+d2, pos2, transform12)
+ - err0;
+ Vec6 err3 = computeErrorVector(object1, object2, pos1+d3, pos2, transform12)
+ - err0;
+ Vec6 err4 = computeErrorVector(object1, object2, pos1, pos2+d1, transform12)
+ - err0;
+ Vec6 err5 = computeErrorVector(object1, object2, pos1, pos2+d2, transform12)
+ - err0;
+ Vec6 err6 = computeErrorVector(object1, object2, pos1, pos2+d3, transform12)
+ - err0;
+ return Mat66(err1, err2, err3, err4, err5, err6) / dt;
+}
+
+} // namespace SimTK
+
diff --git a/SimTKmath/Geometry/src/Contact.cpp b/SimTKmath/Geometry/src/Contact.cpp
new file mode 100644
index 0000000..d5008ea
--- /dev/null
+++ b/SimTKmath/Geometry/src/Contact.cpp
@@ -0,0 +1,276 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/internal/Contact.h"
+
+#include "ContactImpl.h"
+
+#include <set>
+
+using namespace SimTK;
+using std::set;
+
+
+
+//==============================================================================
+// CONTACT
+//==============================================================================
+
+/*static*/ const char* Contact::
+nameOfCondition(Condition cond) {
+ switch(cond) {
+ case Unknown: return "Unknown";
+ case Untracked: return "Untracked";
+ case Anticipated: return "Untracked";
+ case NewContact: return "NewContact";
+ case Ongoing: return "Ongoing";
+ case Broken: return "Broken";
+ default: break;
+ }
+ return "INVALID CONTACT CONDITION";
+}
+
+Contact::Contact(ContactImpl* impl) : impl(impl) {
+ if (impl)
+ impl->m_referenceCount++;
+}
+
+Contact::Contact(const Contact& src) : impl(src.impl) {
+ if (impl)
+ impl->m_referenceCount++;
+}
+
+void Contact::clear() {
+ if (impl) {
+ impl->m_referenceCount--;
+ if (impl->m_referenceCount == 0)
+ delete impl;
+ impl = 0;
+ }
+}
+
+Contact& Contact::operator=(const Contact& src) {
+ clear();
+ if (src.impl) {
+ impl = src.impl;
+ impl->m_referenceCount++;
+ }
+ return *this;
+}
+
+ContactId Contact::getContactId() const {return getImpl().m_id;}
+Contact::Condition Contact::getCondition() const {return getImpl().m_condition;}
+ContactSurfaceIndex Contact::getSurface1() const {return getImpl().m_surf1;}
+ContactSurfaceIndex Contact::getSurface2() const {return getImpl().m_surf2;}
+const Transform& Contact::getTransform() const {return getImpl().m_X_S1S2;}
+ContactTypeId Contact::getTypeId() const {return getImpl().getTypeId();}
+
+Contact& Contact::setContactId(ContactId id)
+{ updImpl().m_id = id; return *this; }
+Contact& Contact::setCondition(Condition condition)
+{ updImpl().m_condition = condition; return *this; }
+Contact& Contact::setSurfaces
+ (ContactSurfaceIndex surf1, ContactSurfaceIndex surf2)
+{ updImpl().m_surf1 = surf1; updImpl().m_surf2 = surf2; return *this; }
+Contact& Contact::setTransform(const Transform& X_S1S2)
+{ updImpl().m_X_S1S2 = X_S1S2; return *this; }
+
+/*static*/ ContactId Contact::createNewContactId()
+{ return ContactImpl::createNewContactId(); }
+
+
+
+//==============================================================================
+// UNTRACKED CONTACT
+//==============================================================================
+UntrackedContact::UntrackedContact
+ (ContactSurfaceIndex surf1, ContactSurfaceIndex surf2)
+: Contact(new UntrackedContactImpl(surf1, surf2)) {}
+
+/*static*/ bool UntrackedContact::isInstance(const Contact& contact) {
+ return (dynamic_cast<const UntrackedContactImpl*>(&contact.getImpl()) != 0);
+}
+
+/*static*/ ContactTypeId UntrackedContact::classTypeId()
+{ return UntrackedContactImpl::classTypeId(); }
+
+
+
+//==============================================================================
+// BROKEN CONTACT
+//==============================================================================
+BrokenContact::BrokenContact
+ (ContactSurfaceIndex surf1, ContactSurfaceIndex surf2,
+ const Transform& X_S1S2, Real separation)
+: Contact(new BrokenContactImpl(surf1, surf2, X_S1S2, separation)) {}
+
+/*static*/ bool BrokenContact::isInstance(const Contact& contact) {
+ return (dynamic_cast<const BrokenContactImpl*>(&contact.getImpl()) != 0);
+}
+
+/*static*/ ContactTypeId BrokenContact::classTypeId()
+{ return BrokenContactImpl::classTypeId(); }
+
+Real BrokenContact::getSeparation() const
+{ return getImpl().separation; }
+
+
+
+//==============================================================================
+// CIRCULAR POINT CONTACT
+//==============================================================================
+CircularPointContact::CircularPointContact
+ (ContactSurfaceIndex surf1, Real radius1,
+ ContactSurfaceIndex surf2, Real radius2,
+ const Transform& X_S1S2, Real radius, Real depth,
+ const Vec3& origin_S1, const UnitVec3& normal_S1)
+: Contact(new CircularPointContactImpl(surf1,radius1,surf2,radius2,X_S1S2,
+ radius,depth,origin_S1,normal_S1)) {}
+
+Real CircularPointContact::getRadius1() const
+{ return getImpl().radius1; }
+Real CircularPointContact::getRadius2() const
+{ return getImpl().radius2; }
+Real CircularPointContact::getEffectiveRadius() const
+{ return getImpl().radiusEff; }
+Real CircularPointContact::getDepth() const
+{ return getImpl().depth; }
+const Vec3& CircularPointContact::getOrigin() const
+{ return getImpl().origin_S1; }
+const UnitVec3& CircularPointContact::getNormal() const
+{ return getImpl().normal_S1; }
+
+bool CircularPointContact::isInstance(const Contact& contact) {
+ return (dynamic_cast<const CircularPointContactImpl*>(&contact.getImpl()) != 0);
+}
+
+/*static*/ ContactTypeId CircularPointContact::classTypeId()
+{ return CircularPointContactImpl::classTypeId(); }
+
+
+
+//==============================================================================
+// ELLIPTICAL POINT CONTACT
+//==============================================================================
+EllipticalPointContact::EllipticalPointContact
+ (ContactSurfaceIndex surf1, ContactSurfaceIndex surf2,
+ const Transform& X_S1S2, const Transform& X_S1C,
+ const Vec2& k, Real depth)
+: Contact(new EllipticalPointContactImpl(surf1,surf2,X_S1S2,X_S1C,
+ k,depth)) {}
+
+const Vec2& EllipticalPointContact::getCurvatures() const
+{ return getImpl().k; }
+const Transform& EllipticalPointContact::getContactFrame() const
+{ return getImpl().X_S1C; }
+Real EllipticalPointContact::getDepth() const
+{ return getImpl().depth; }
+
+bool EllipticalPointContact::isInstance(const Contact& contact) {
+ return (dynamic_cast<const EllipticalPointContactImpl*>
+ (&contact.getImpl()) != 0);
+}
+
+/*static*/ ContactTypeId EllipticalPointContact::classTypeId()
+{ return EllipticalPointContactImpl::classTypeId(); }
+
+
+
+//==============================================================================
+// TRIANGLE MESH CONTACT & IMPL
+//==============================================================================
+TriangleMeshContact::TriangleMeshContact
+ (ContactSurfaceIndex surf1, ContactSurfaceIndex surf2,
+ const Transform& X_S1S2,
+ const std::set<int>& faces1, const std::set<int>& faces2)
+: Contact(new TriangleMeshContactImpl(surf1, surf2, X_S1S2,
+ faces1, faces2)) {}
+
+const set<int>& TriangleMeshContact::getSurface1Faces() const
+{ return getImpl().faces1; }
+const set<int>& TriangleMeshContact::getSurface2Faces() const
+{ return getImpl().faces2; }
+
+/*static*/ bool TriangleMeshContact::isInstance(const Contact& contact)
+{ return (dynamic_cast<const TriangleMeshContactImpl*>(&contact.getImpl())
+ != 0); }
+
+/*static*/ ContactTypeId TriangleMeshContact::classTypeId()
+{ return TriangleMeshContactImpl::classTypeId(); }
+
+TriangleMeshContactImpl::TriangleMeshContactImpl
+ (ContactSurfaceIndex surf1, ContactSurfaceIndex surf2,
+ const Transform& X_S1S2,
+ const set<int>& faces1, const set<int>& faces2)
+: ContactImpl(surf1, surf2, X_S1S2), faces1(faces1), faces2(faces2) {}
+
+
+
+
+//==============================================================================
+// POINT CONTACT & IMPL (OBSOLETE)
+//==============================================================================
+PointContact::PointContact
+ (ContactSurfaceIndex surf1, ContactSurfaceIndex surf2,
+ Vec3& location, Vec3& normal, Real radius1, Real radius2, Real depth)
+: Contact(new PointContactImpl(surf1, surf2, location, normal, radius1, radius2, depth)) {}
+PointContact::PointContact
+ (ContactSurfaceIndex surf1, ContactSurfaceIndex surf2,
+ Vec3& location, Vec3& normal, Real radius, Real depth)
+: Contact(new PointContactImpl(surf1, surf2, location, normal, radius, depth)) {}
+
+Vec3 PointContact::getLocation() const
+{ return getImpl().location; }
+Vec3 PointContact::getNormal() const
+{ return getImpl().normal; }
+Real PointContact::getRadiusOfCurvature1() const
+{ return getImpl().radius1; }
+Real PointContact::getRadiusOfCurvature2() const
+{ return getImpl().radius2; }
+Real PointContact::getEffectiveRadiusOfCurvature() const
+{ return getImpl().effectiveRadius; }
+Real PointContact::getDepth() const
+{ return getImpl().depth; }
+
+/*static*/ bool PointContact::isInstance(const Contact& contact) {
+ return (dynamic_cast<const PointContactImpl*>(&contact.getImpl()) != 0);
+}
+
+/*static*/ ContactTypeId PointContact::classTypeId()
+{ return PointContactImpl::classTypeId(); }
+
+PointContactImpl::PointContactImpl
+ (ContactSurfaceIndex surf1, ContactSurfaceIndex surf2,
+ Vec3& location, Vec3& normal, Real radius1, Real radius2, Real depth)
+: ContactImpl(surf1, surf2), location(location), normal(normal),
+ radius1(radius1), radius2(radius2), effectiveRadius(std::sqrt(radius1*radius2)), depth(depth) {}
+
+PointContactImpl::PointContactImpl
+ (ContactSurfaceIndex surf1, ContactSurfaceIndex surf2,
+ Vec3& location, Vec3& normal, Real radius, Real depth)
+: ContactImpl(surf1, surf2), location(location), normal(normal),
+ radius1(radius), radius2(radius), effectiveRadius(radius), depth(depth) {}
+
+
diff --git a/SimTKmath/Geometry/src/ContactGeometry.cpp b/SimTKmath/Geometry/src/ContactGeometry.cpp
new file mode 100644
index 0000000..69ac69b
--- /dev/null
+++ b/SimTKmath/Geometry/src/ContactGeometry.cpp
@@ -0,0 +1,2189 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman, Ian Stavness *
+ * Contributors: Andreas Scholz, Matthew Millard *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_Point.h"
+#include "simmath/internal/Geo_Sphere.h"
+#include "simmath/internal/Geo_BicubicBezierPatch.h"
+#include "simmath/internal/BicubicSurface.h"
+#include "simmath/internal/ParticleConSurfaceSystem.h"
+#include "simmath/Differentiator.h"
+#include "simmath/RungeKutta3Integrator.h"
+#include "simmath/RungeKuttaMersonIntegrator.h"
+#include "simmath/RungeKuttaFeldbergIntegrator.h"
+#include "simmath/VerletIntegrator.h"
+#include "simmath/CPodesIntegrator.h"
+#include "simmath/TimeStepper.h"
+#include "simmath/internal/ContactGeometry.h"
+#include "simmath/internal/GeodesicIntegrator.h"
+
+#include "ContactGeometryImpl.h"
+#include "GeodesicEquations.h"
+
+#include <iostream>
+#include <cmath>
+#include <map>
+#include <set>
+
+using namespace SimTK;
+using std::map;
+using std::pair;
+using std::set;
+using std::string;
+using std::cout; using std::endl;
+
+#define USE_NEW_INTEGRATOR
+
+//==============================================================================
+// CONTACT GEOMETRY
+//==============================================================================
+
+ContactGeometry::ContactGeometry(ContactGeometryImpl* impl) : impl(impl) {
+ assert(impl);
+ impl->setMyHandle(*this);
+}
+
+ContactGeometry::~ContactGeometry() {
+ if (isOwnerHandle())
+ delete impl;
+ impl = 0;
+}
+
+bool ContactGeometry::isOwnerHandle() const {
+ return (impl == 0 || impl->getMyHandle() == this);
+}
+
+bool ContactGeometry::isEmptyHandle() const {
+ return (impl == 0);
+}
+
+ContactGeometry::ContactGeometry(const ContactGeometry& src) : impl(0) {
+ if (src.impl) {
+ impl = src.impl->clone();
+ impl->setMyHandle(*this);
+ }
+}
+
+ContactGeometry& ContactGeometry::operator=(const ContactGeometry& src) {
+ if (&src != this) {
+ if (isOwnerHandle())
+ delete impl;
+ impl = 0;
+ if (src.impl) {
+ impl = src.impl->clone();
+ impl->setMyHandle(*this);
+ }
+ }
+ return *this;
+}
+
+ContactGeometryTypeId ContactGeometry::
+getTypeId() const {return getImpl().getTypeId();}
+
+DecorativeGeometry ContactGeometry::createDecorativeGeometry() const {
+ return getImpl().createDecorativeGeometry();
+}
+
+bool ContactGeometry::intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance, UnitVec3& normal) const {
+ return getImpl().intersectsRay(origin, direction, distance, normal);
+}
+
+Vec3 ContactGeometry::findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const {
+ return getImpl().findNearestPoint(position, inside, normal);
+}
+
+Vec3 ContactGeometry::projectDownhillToNearestPoint(const Vec3& Q) const {
+ return getImpl().projectDownhillToNearestPoint(Q);
+}
+
+bool ContactGeometry::
+trackSeparationFromLine(const Vec3& pointOnLine,
+ const UnitVec3& directionOfLine,
+ const Vec3& startingGuessForClosestPoint,
+ Vec3& newClosestPointOnSurface,
+ Vec3& closestPointOnLine,
+ Real& height) const {
+ return getImpl().trackSeparationFromLine(pointOnLine,directionOfLine,
+ startingGuessForClosestPoint,newClosestPointOnSurface,
+ closestPointOnLine,height);
+}
+
+
+void ContactGeometry::getBoundingSphere(Vec3& center, Real& radius) const {
+ getImpl().getBoundingSphere(center, radius);
+}
+
+bool ContactGeometry::isSmooth() const {return getImpl().isSmooth();}
+bool ContactGeometry::isConvex() const {return getImpl().isConvex();}
+
+void ContactGeometry::calcCurvature(const Vec3& point, Vec2& curvature,
+ Rotation& orientation) const
+{ getImpl().calcCurvature(point, curvature, orientation); }
+
+const Function& ContactGeometry::getImplicitFunction() const
+{ return getImpl().getImplicitFunction(); }
+
+
+Real ContactGeometry::calcSurfaceValue(const Vec3& point) const {
+ return getImpl().calcSurfaceValue(point);
+}
+
+UnitVec3 ContactGeometry::calcSurfaceUnitNormal(const Vec3& point) const {
+ return getImpl().calcSurfaceUnitNormal(point);
+}
+
+Vec3 ContactGeometry::calcSurfaceGradient(const Vec3& point) const {
+ return getImpl().calcSurfaceGradient(point);
+}
+
+Mat33 ContactGeometry::calcSurfaceHessian(const Vec3& point) const {
+ return getImpl().calcSurfaceHessian(point);
+}
+
+Real ContactGeometry::calcGaussianCurvature(const Vec3& gradient,
+ const Mat33& Hessian) const {
+ return getImpl().calcGaussianCurvature(gradient,Hessian);
+}
+
+Real ContactGeometry::calcSurfaceCurvatureInDirection(const Vec3& point, const UnitVec3& direction) const {
+ return getImpl().calcSurfaceCurvatureInDirection(point, direction);
+}
+
+Vec3 ContactGeometry::calcSupportPoint(UnitVec3 direction) const
+{ return getImpl().calcSupportPoint(direction); }
+
+
+//------------------------------------------------------------------------------
+// EVAL PARAMETRIC CURVATURE
+//------------------------------------------------------------------------------
+/*static*/Vec2 ContactGeometry::
+evalParametricCurvature(const Vec3& P, const UnitVec3& nn,
+ const Vec3& dPdu, const Vec3& dPdv,
+ const Vec3& d2Pdu2, const Vec3& d2Pdv2,
+ const Vec3& d2Pdudv,
+ Transform& X_EP)
+{
+ // All this is 42 flops
+ Real E = ~dPdu*dPdu, F = ~dPdu*dPdv, G = ~dPdv*dPdv;
+ Real e =-(~d2Pdu2*nn), f =-(~d2Pdudv*nn), g =-(~d2Pdv2*nn);
+ Real A = F*g-G*f, B = E*g-G*e, C = E*f-F*e;
+
+ Real kmax, kmin;
+ UnitVec3 dmax;
+ if (std::abs(F) < SignificantReal) {
+ Real ku = e/E, kv = g/G; // two divides ~20 flops
+ if (ku < kv) {
+ kmax=kv, kmin=ku;
+ dmax=UnitVec3(dPdv); // normalizing, ~35 flops
+ } else {
+ kmax=ku, kmin=kv;
+ dmax=UnitVec3(dPdu); // normalizing, ~35 flops
+ }
+ } else {
+ // ~40 flops
+ // t = (-b +/- sqrt(b^2-4ac)) / 2a
+ // Discriminant must be nonnegative for real surfaces
+ // but could be slightly negative due to numerical noise.
+ Real sqrtd = std::sqrt(std::max(B*B - 4*A*C, Real(0)));
+ Vec2 t = Vec2(sqrtd - B, -sqrtd - B) / (2*A);
+
+ // Two divides + misc: ~30 flops
+ Real kr = (e + f*t[0])/(E+F*t[0]); // Struik, eq. 6-4, pg 80
+ Real ks = (e + f*t[1])/(E+F*t[1]); // (works only because these are extremes)
+ // otherwise use eq. 6-3.
+
+ if (kr < ks) {
+ kmax=ks, kmin=kr;
+ dmax = UnitVec3(t[1]*dPdv + dPdu); // Sdir, normalizing, ~40 flops
+ } else {
+ kmax=kr, kmin=ks;
+ dmax = UnitVec3(t[0]*dPdv + dPdu); // Rdir, normalizing, ~40 flops
+ }
+ }
+
+ // y=z%x ensures right handed; already unit vec (9 flops)
+ UnitVec3 dmin = UnitVec3(nn % dmax, true);
+ X_EP.updR().setRotationFromUnitVecsTrustMe(dmax, dmin, nn);
+ X_EP.updP() = P; // the origin point
+
+ return Vec2(kmax, kmin);
+}
+
+
+//------------------------------------------------------------------------------
+// COMBINE PARABOLOIDS
+//------------------------------------------------------------------------------
+// See the documentation in the header file for a complete description of
+// what's being calculated here. This comment adds implementation information
+// that isn't relevant to the API user.
+//
+// Given two paraboloids P1 and P2 sharing a common normal z and origin,
+// compute the paraboloid that represents their difference, and express that
+// paraboloid in a frame that has been rotated around z so that x and y
+// coincide with the principal curvature directions. P1 and P2 may be
+// elliptic (kmax>=kmin>=0) or hyperbolic (kmax>=0>kmin). If the surfaces are
+// non-conforming, their difference will be elliptic with kmax>=kmin>0.
+//
+// We assume the paraboloids represent surfaces and that each has its z axis
+// oriented away from the surface, pointing outside the "bowl" of the elliptic
+// paraboloid or away from the convex direction of a hyperbolic paraboloid.
+// That's the opposite sense from a standard paraboloid parameterization.
+// The z axes are antiparallel. We will return the resulting difference
+// paraboloid in a frame whose z axis is coincident with P1's z axis, and thus
+// antiparallel to P2's z axis.
+//
+// P1: z = -(kmax1/2 x1^2 + kmin1/2 y1^2)
+// P2: z = kmax2/2 x2^2 + kmin2/2 y2^2
+// P: z = -( kmax/2 x^2 + kmin/2 y^2)
+// Thus the right-handed coordinate frames are:
+// P1: (x1,y1, z)
+// P2: (x2,y2,-z)
+// P: ( x, y, z)
+// The above distinctions don't matter a whole lot for the implementation here,
+// but still, I thought you might like to know anyway.
+//
+// Cost is about 70 flops to get the curvatures kmax,kmin. Then if you want
+// the curvature directions too it costs another 150 flops.
+
+// This local static helper method calculates the curvatures and returns
+// intermediates necessary for calculating the directions, but doesn't actually
+// calculate them. So we use only about 70 flops here.
+static void combineParaboloidsHelper
+ (const Rotation& R_SP1, const Vec2& k1,
+ const UnitVec3& x2, const Vec2& k2,
+ Real& cos2w, Real& sin2w, Real& kdiff1, Real& kdiff2, Vec2& k)
+{
+ const UnitVec3& x1 = R_SP1.x(); // P1 kmax direction
+ const UnitVec3& y1 = R_SP1.y(); // P1 kmin direction
+ const UnitVec3& z = R_SP1.z(); // P1, P, -P2 normal
+
+ const Real ksum1 = k1[0]+k1[1], ksum2 = k2[0]+k2[1]; // 4 flops
+ kdiff1 = k1[0]-k1[1], kdiff2 = k2[0]-k2[1];
+
+ // w is angle between x1, x2 max curvature directions defined
+ // using right hand rule rotation of x1 about z until it is
+ // coincident with x2. But ... we want -90 <= w <= 90, meaning
+ // cos(w) >= 0. If necessary we flip x2 180 degrees around z,
+ // since -x2 is an equally good max curvature direction.
+ const Real dotx1x2 = dot(x1,x2); // 5 flops
+ const UnitVec3 x2p = dotx1x2 < 0 ? -x2 : x2;
+ const Real cosw = std::abs(dotx1x2);
+ const Real sinw = dot(cross(x1,x2p), z); // signed, 14 flops
+
+ // We'll need cos(2w), sin(2w); luckily these are easy to get (5 flops).
+ cos2w = 2*square(cosw) - 1; // double angle formulas
+ sin2w = 2*sinw*cosw;
+
+ // Compute min/max curvatures of the difference surface.
+ // See KL Johnson 1987 Ch. 4 and Appendix 2, and J-F Antoine, et al.
+ // 2006 pg 661. ~35 flops
+ const Real ksum = ksum1 + ksum2;
+ const Real kdiff = std::sqrt(square(kdiff1) + square(kdiff2)
+ + 2*kdiff1*kdiff2*cos2w);
+ k = Vec2(ksum + kdiff, ksum - kdiff)/2; // kmax, kmin (4 flops)
+}
+
+// This is the full version that calculates the curvatures for 70 flops
+// then spends another 150 to get the curvature directions.
+/*static*/ void ContactGeometry::combineParaboloids
+ (const Rotation& R_SP1, const Vec2& k1,
+ const UnitVec3& x2, const Vec2& k2,
+ Rotation& R_SP, Vec2& k)
+{
+ Real cos2w, sin2w, kdiff1, kdiff2;
+ combineParaboloidsHelper(R_SP1, k1, x2, k2,
+ cos2w, sin2w, kdiff1, kdiff2, k);
+
+ // Now find the rotated coordinate system by solving for the
+ // angle -90 <= alpha <= 90 by which we need to rotate the x1 axis
+ // about z to align it with the x axis. See KL Johnson Appendix 2
+ // again, noting that beta = theta-alpha, then solving for tan(2 alpha).
+ // This is about 130 flops.
+ const Real yy = kdiff2*sin2w, xx = kdiff2*cos2w + kdiff1;
+ Real a = std::atan2(yy,xx) / 2; // yy==xx==0 -> a=0
+ Real cosa = std::cos(a), sina = std::sin(a);
+
+ // Perform the actual rotations of x1,y1 to get x,y (18 flops)
+ const UnitVec3& x1 = R_SP1.x(); // P1 kmax direction
+ const UnitVec3& y1 = R_SP1.y(); // P1 kmin direction
+ const UnitVec3& z = R_SP1.z(); // P1, P, -P2 normal
+ R_SP.setRotationFromUnitVecsTrustMe(UnitVec3(cosa*x1 + sina*y1, true),
+ UnitVec3(cosa*y1 - sina*x1, true),
+ z);
+}
+
+// This is the abridged version that costs only 70 flops but gives you just
+// the curvatures without the curvature directions.
+/*static*/ void ContactGeometry::combineParaboloids
+ (const Rotation& R_SP1, const Vec2& k1,
+ const UnitVec3& x2, const Vec2& k2,
+ Vec2& k)
+{
+ Real cos2w, sin2w, kdiff1, kdiff2; // unneeded
+ combineParaboloidsHelper(R_SP1, k1, x2, k2,
+ cos2w, sin2w, kdiff1, kdiff2, k);
+}
+
+//------------------------------------------------------------------------------
+// PROJECT DOWNHILL TO NEAREST POINT
+//------------------------------------------------------------------------------
+
+// Find the "local" nearest point to p on this surface. The algorithm assumes that
+// p is close to the surface and uses Newton's method to proceed downhill until a
+// feasible surface point is found.
+//
+// The nearest point, x, is a point on the surface whose normal points toward the
+// initial point, p. The nearest point, x, must satisfy the following conditions:
+// (1) surf(x) = 0
+// (2) ~r_px*t = 0
+// (3) ~r_px*b = 0
+// where surf(x) is the implicit surface function, r_px = (p-x), and
+// t and b are orthogonal vectors that span the tangent plane at x.
+//
+// Condition (1) ensures that x is on the surface, and conditions (2) and (3)
+// ensure that the line between p and x is perpendicular to the surface
+// tangent plane at x.
+//
+// At point p, we choose arbitrary basis vectors tP and bP that are perpendicular
+// to the gradient at p. The tangent plane at x is spanned by t and b as:
+// t := tP - n*(~tP*n)
+// b := bP - n*(~bP*n)
+// where n is the unit normal vector at x
+//
+// Newton's method solves err(x) = 0 iteratively as
+//
+// f(x + dx) ~= f(x) + J dx = 0
+//
+// The jacobian J has the following rows:
+// J(1) = dsurf/dx = ~g
+// J(2) = ~t*Rx + ~r*Tx
+// J(3) = ~b*Rx + ~r*Bx
+//
+// n := g/norm(g) (unit normal vector)
+// H := dg/dx (the Hessian of the surface at x)
+// Rx := dr/dx = -I
+// Nx := dn/dx = dn/dg*dg/dx = -(I - n*(~n))*H/norm(g) // negative due to sign convention of gradient
+// Tx := dt/dx = -Nx*(~tP*n) -n*(~tP*Nx)
+// Bx := db/dx = -Nx*(~bP*n) -n*(~bP*Nx)
+
+Vec3 ContactGeometryImpl::
+projectDownhillToNearestPoint(const Vec3& Q) const {
+
+ // Newton solver settings
+ const Real ftol = SignificantReal;
+
+ // Check for immediate return.
+ if (std::abs(calcSurfaceValue(Q)) <= ftol)
+ return Q;
+
+ // construct arbitrary frame at p
+ UnitVec3 nP = calcSurfaceUnitNormal(Q);
+ SimTK_ASSERT_ALWAYS(!nP.isNaN(), "gradient is undefined at the query point.");
+ UnitVec3 tP = nP.perp();
+ UnitVec3 bP(tP%nP);
+
+ // Estimate a scale for the local neighborhood of this surface by
+ // using the larger curvature in the t or b direction. We want to take
+ // conservative steps that never move by more than a fraction of the
+ // scale to avoid jumping out of the local minimum.
+ const Real kt = calcSurfaceCurvatureInDirection(Q, tP);
+ const Real kb = calcSurfaceCurvatureInDirection(Q, bP);
+ const Real maxK = std::max(std::abs(kt),std::abs(kb));
+ const Real scale = std::min(1/maxK, Real(1000)); // keep scale reasonable
+ const Real MaxMove = Real(.25); // Limit one move to 25% of smaller radius.
+
+ const Real xtol = Real(1e-12); // TODO: what should these be in single
+ const Real minlam = Real(1e-9); // precision?
+ const int maxNewtonIterations = 30;
+
+ Vec3 x(Q); // initialize to query point
+ Vec3 f, dx, xold, g, r, t, b;
+ Mat33 J, H, Nx, Tx, Bx;
+ Real gNorm, rmsError, rmsErrorOld, xchg, lam;
+
+ // initialized to query point, therefore first step is in gradient direction
+ f[0] = calcSurfaceValue(x);
+ f[1] = 0;
+ f[2] = 0;
+
+ rmsError = std::sqrt(f.normSqr()/3);
+ //std::cout << "BEFORE Q=" << Q << ", f=" << f << ", frms=" << rmsError << std::endl;
+
+ int cnt = 0;
+ do {
+ if (rmsError <= ftol) { // found solution
+ //std::cout << "CONVERGED in " << cnt << " steps, frms=" << rmsError << std::endl;
+ break;
+ }
+
+ g = calcSurfaceGradient(x); // non-normalized gradient vector
+ H = calcSurfaceHessian(x);
+ gNorm = g.norm();
+ UnitVec3 n(-g/gNorm, true);
+
+ // calculate frame at x
+ r = Q-x;
+ t = tP-n*(~tP*n); // project tP to tangent plane at x
+ b = bP-n*(~bP*n); // project bP to tangent plane at x
+
+ SimTK_ASSERT_ALWAYS(t.norm() > Real(1e-6),
+ "t is aligned with the normal vector at the current point.");
+ SimTK_ASSERT_ALWAYS(b.norm() > Real(1e-6),
+ "b is aligned with the with normal vector at the current point.");
+
+ // calculate error
+ f[0] = calcSurfaceValue(x);
+ f[1] = ~r*t;
+ f[2] = ~r*b;
+
+ // calculate derivatives
+ Nx = -((Mat33(1) - n*(~n))/(gNorm))*H; // negative due to sign convention of gradient
+ Tx = -Nx*(~tP*n) - n*(~tP*Nx);
+ Bx = -Nx*(~bP*n) - n*(~bP*Nx);
+
+ J[0] = ~g;
+ J[1] = -(~t) + ~r*Tx;
+ J[2] = -(~b) + ~r*Bx;
+
+ dx = J.invert()*f;
+ const Real dxrms = std::sqrt(dx.normSqr()/3);
+
+ rmsErrorOld = rmsError;
+ xold = x;
+
+ // Backtracking. Limit the starting step size if dx is too big.
+ lam = std::min(Real(1), MaxMove*(scale/dxrms));
+ if (lam < 1) {
+ //std::cout << "PROJECT: LIMITED STEP: iter=" << cnt
+ // << " lam=" << lam << endl;
+ }
+ while (true) {
+ x = xold - lam*dx;
+
+ n = calcSurfaceUnitNormal(x);
+ r = Q-x;
+ t = tP-n*(~tP*n); // project tP to tangent plane at x
+ b = bP-n*(~bP*n); // project bP to tangent plane at x
+
+ f[0] = calcSurfaceValue(x);
+ f[1] = ~r*t;
+ f[2] = ~r*b;
+
+ rmsError = std::sqrt(f.normSqr()/3);
+ if (rmsError > rmsErrorOld && lam > minlam) {
+ lam = lam / 2;
+ } else {
+ break;
+ }
+ }
+
+ //std::cout << cnt << ": AFTER x-=" << lam << "*dx, x=" << x
+ // << ", f=" << f << ", frms=" << rmsError << std::endl;
+
+ if (rmsError > ftol) {
+ xchg = dxrms*lam; // roughly, how much we changed x
+ if (xchg < xtol) { // check step size
+ std::cout << "PROJECT: STALLED on step size, xchg=" << xchg
+ << " frms=" << rmsError << std::endl;
+ break;
+ }
+ }
+
+ cnt++;
+ if (cnt > maxNewtonIterations) {
+// SimTK_ASSERT_ALWAYS(false,"Newton solve did not converge, max iterations taken");
+ std::cout << "PROJECT: MAX iterations taken" << std::endl;
+ break; // Return whatever we got.
+ }
+
+ rmsErrorOld = rmsError;
+
+ } while (true);
+
+ return x;
+}
+
+
+
+//------------------------------------------------------------------------------
+// TRACK SEPARATION FROM LINE
+//------------------------------------------------------------------------------
+
+class TrackLineSeparationJacobian : public Differentiator::JacobianFunction {
+public:
+ TrackLineSeparationJacobian(const ContactGeometryImpl& geom,
+ const Vec3& p, const UnitVec3& e)
+ : Differentiator::JacobianFunction(3,3), geom(geom), p(p), e(e) { }
+
+ int f(const Vector& xvec, Vector& f) const OVERRIDE_11 {
+ const Vec3& x = Vec3::getAs(&xvec[0]);
+ UnitVec3 n; Vec3 closestPointOnLine; // not used
+ const Vec3 eps = calcExtremePointError(x, n, closestPointOnLine);
+
+ f[0] = eps[0]; f[1] = eps[1]; f[2] = eps[2];
+ return 0;
+ }
+
+ // Given a point x, return the value of the "closest point" error of x.
+ // We return the outward unit normal n at x, and the line's closest point R
+ // as side effects in case you are interested.
+ Vec3 calcExtremePointError(const Vec3& x, UnitVec3& n, Vec3& R) const {
+ n = geom.calcSurfaceUnitNormal(x);
+
+ Vec3 Q; // Q is the point of the normal line closest to L
+ bool linesAreParallel;
+ Geo::findClosestPointsOfTwoLines(x, n, p, e,
+ Q, R, linesAreParallel);
+
+ if (linesAreParallel)
+ cout << "findClosest: PARALLEL!!!" << endl;
+
+ Vec3 errf( ~n * e, // normal and line should be perpendicular
+ ~(R-Q) * (n % e), // no separation along common perpendicular
+ geom.calcSurfaceValue(x) ); // x should be on the surface
+
+ return errf;
+ }
+
+private:
+ const ContactGeometryImpl& geom;
+ const Vec3 p; // a point on the line
+ const UnitVec3 e; // a unit vector along the line
+};
+
+// This method is required to search downhill only from the starting guess.
+// The Newton iteration is throttled back accordingly if it gets too
+// agressive.
+bool ContactGeometryImpl::
+trackSeparationFromLine(const Vec3& pointOnLine,
+ const UnitVec3& directionOfLine,
+ const Vec3& startingGuessForClosestPoint,
+ Vec3& x, // the new extreme point
+ Vec3& closestPointOnLine,
+ Real& height) const
+{
+ // Newton solver settings.
+
+ // RMS error must reach sqrt of this value (squared for speed).
+ const Real Ftol2 = square(SignificantReal);
+
+ // Limit the number of Newton steps. We don't
+ // count steps that we limited because we were nervous about the size of
+ // the change, so this value is *very* generous. It should never take
+ // more than 7 full Newton iterations to solve to machine precision.
+ const int MaxNewtonIterations = 20;
+ // Make sure there is at least some limit on the total number of steps
+ // including ones we throttled back on purpose, just as a sanity check.
+ const int MaxTotalIterations = 50;
+ // We won't take a step unless it reduces the error and we'll take a
+ // fractional step if necessary. If we have to use a fraction smaller than
+ // this we're probably at a local minimum and it's time to give up.
+ const Real MinStepFrac = Real(1e-6);
+ // If the norm of a change to X during a step isn't at least this fraction
+ // of a full-scale change (see scaling below), we'll treat that as an
+ // independent reason to give up (squared for speed).
+ const Real Xtol2 = square(Real(1e-12)); // TODO: single precision?
+ // We'll calculate a length scale for the local patch of this object and
+ // then limit any moves we make to this fraction of that scale to avoid
+ // jumping out of one local minimum into another. This can cause a series
+ // of slowly-converging steps to be taken at the beginning.
+ const Real MaxMove2 = square(Real(.25)); // Limit one move to 25% of smaller radius.
+
+ // Create an object that can calculate the error function for this
+ // surface against the given line.
+ TrackLineSeparationJacobian extremePointJac
+ (*this, pointOnLine, directionOfLine);
+
+ // Initialize the extreme point to the given value.
+ x = startingGuessForClosestPoint;
+ UnitVec3 nX; // normal at x
+ Vec3 f = extremePointJac.calcExtremePointError(x,
+ nX, closestPointOnLine);
+ Real frms2 = f.normSqr(); // initial error
+
+ if (frms2 <= Ftol2) {
+ //cout << "TRACK: already at extreme point with frms="
+ // << std::sqrt(frms2) << endl;
+ height = ~(closestPointOnLine - x) * nX;
+ return true; // Success
+ }
+
+ // We are going to have to move the starting point to find the new
+ // extreme point.
+
+ // Estimate a scale for the local neighborhood of this surface by
+ // sampling the curvature around x, taking the largest curvature we find.
+ // We want to take conservative steps that never move by more than a
+ // fraction of the scale to avoid jumping out of the local minimum.
+ // TODO: could calculate the actual max curvature here but it is
+ // more expensive.
+ const UnitVec3 tX = nX.perp(); // any perpendicular to the normal at x
+ const UnitVec3 bX(tX % nX, true); // another tangent vector
+ // using the larger curvature in the t or b direction.
+ const Real kt = calcSurfaceCurvatureInDirection(x, tX);
+ const Real kb = calcSurfaceCurvatureInDirection(x, bX);
+ const Real maxK = std::max(std::abs(kt),std::abs(kb));
+ const Real scale2 = square(clamp(Real(0.1), 1/maxK, Real(1000))); // keep scale reasonable
+
+ //cout << "TRACK START: line p0=" << pointOnLine << " d=" << directionOfLine << "\n";
+ //cout << " starting x=" << x << " nX=" << nX << " scale est=" << std::sqrt(scale2) << "\n";
+ //cout << " err=" << f << " rms=" << std::sqrt(frms2)
+ // << " closest line pt=" << closestPointOnLine
+ // << " height=" << ~(closestPointOnLine - x) * nX << "\n";
+
+ Differentiator diff(extremePointJac);
+
+ int stepCount = 0, limitedStepCount = 0;
+ bool succeeded = false;
+ do {
+ ++stepCount; // we're going to take a step now
+ const Matrix Jnum = diff.calcJacobian((Vector)x,
+ Differentiator::ForwardDifference);
+ const Mat33& J = Mat33::getAs(&Jnum(0,0));
+
+ // This is the full step that the Newton would like us to take. We
+ // might not use all of it.
+ const Vec3 dx = J.invert()*f;
+ const Real dxrms2 = dx.normSqr()/3;
+
+ //cout << "det(J)=" << det(J)
+ // << "full dxrms=" << std::sqrt(dxrms2) << " dx=" << dx << endl;
+
+ const Vec3 xOld = x; // Save previous solution & its norm.
+ const Real frms2Old = frms2;
+
+ // Backtracking. Limit the starting step size if dx is too big.
+ // Calculate the square of the step fraction.
+ const Real stepFrac2 = std::min(Real(1), MaxMove2*(scale2/dxrms2));
+ Real stepFrac = 1;
+ if (stepFrac2 < 1) {
+ stepFrac = std::sqrt(stepFrac2); // not done often
+ //cout << "TRACK: LIMITED STEP: iter=" << stepCount
+ // << " stepFrac=" << stepFrac << endl;
+ ++limitedStepCount;
+ }
+ Real xchgrms2; // norm^2 of the actual change we make to X
+ while (true) {
+ x = xOld - stepFrac*dx;
+ xchgrms2 = stepFrac2*dxrms2; // = |stepFrac*dx|^2 / 3
+
+ f = extremePointJac.calcExtremePointError(x, nX, closestPointOnLine);
+ frms2 = f.normSqr()/3;
+ if (frms2 < frms2Old || stepFrac <= MinStepFrac)
+ break;
+
+ stepFrac /= 2;
+ }
+
+ //cout << stepCount << ": TRACK lam=" << stepFrac << " |lam*dx|=" << (stepFrac*dx).norm()
+ // << " lam*dx=" << stepFrac*dx << "-> new x=" << x << "\n";
+ //cout << " |f|=" << std::sqrt(frms2) << " f=" << f << "\n";
+
+ if (frms2 <= Ftol2) { // found solution
+ //cout << "TRACK CONVERGED in " << stepCount << " steps, frms="
+ // << std::sqrt(frms2) << endl;
+ succeeded = true;
+ break;
+ }
+
+ // This method is supposed to converge quickly -- if we're not making
+ // substantial progress in each step, give it up.
+ if (frms2 >= 0.999*frms2Old) {
+ if (frms2 > frms2Old) { // Oops -- made it worse.
+ x = xOld; // Repair the damage.
+ f = extremePointJac.calcExtremePointError(x, nX, closestPointOnLine);
+ frms2 = f.normSqr()/3;
+ }
+ //cout << "TRACK FAILED at " << stepCount << " steps, frms="
+ // << std::sqrt(frms2) << endl;
+ break;
+ }
+
+ // We took a step and made an improvement but haven't converged yet.
+
+ if (xchgrms2 < Xtol2) { // check step size
+ //std::cout << "TRACK: STALLED on step size, xchg=" << std::sqrt(xchgrms2)
+ // << " frms=" << std::sqrt(frms2) << std::endl;
+ break;
+ }
+
+ if (stepCount-limitedStepCount > MaxNewtonIterations
+ || stepCount > MaxTotalIterations) {
+ //cout << "TRACK: MAX iterations taken" << endl;
+ break; // Return whatever we got.
+ }
+
+ } while (true);
+
+ if (!succeeded)
+ x = projectDownhillToNearestPoint(x); // push to surface
+
+ height = ~(closestPointOnLine - x) * nX;
+
+ //cout << "TRACK END: x=" << x << " nX=" << nX << "\n";
+ //cout << " err=" << f << " rms=" << std::sqrt(frms2)
+ // << " closest line pt=" << closestPointOnLine
+ // << " height=" << height << "\n";
+ return succeeded;
+}
+
+//==============================================================================
+// GEODESIC EVALUATORS in CONTACT GEOMETRY
+//==============================================================================
+
+
+void ContactGeometry::initGeodesic(const Vec3& xP, const Vec3& xQ,
+ const Vec3& xSP, const GeodesicOptions& options, Geodesic& geod) const
+{
+ // TODO
+}
+
+
+// Given two points and previous geodesic curve close to the points, find
+// a geodesic curve connecting the points that is close to the previous geodesic.
+void ContactGeometry::
+continueGeodesic(const Vec3& xP, const Vec3& xQ, const Geodesic& prevGeod,
+ const GeodesicOptions& options, Geodesic& geod) const {
+ getImpl().continueGeodesic(xP, xQ, prevGeod, options, geod);
+}
+
+void ContactGeometry::
+makeStraightLineGeodesic(const Vec3& xP, const Vec3& xQ,
+ const UnitVec3& defaultDirectionIfNeeded,
+ const GeodesicOptions& options, Geodesic& geod) const {
+ getImpl().makeStraightLineGeodesic(xP, xQ, defaultDirectionIfNeeded,
+ options, geod);
+}
+
+
+// Utility method to find geodesic between P and Q
+// with starting directions tPhint and tQhint
+// XXX tangent basis should be formed from previous geodesic
+void ContactGeometry::calcGeodesic(const Vec3& xP, const Vec3& xQ,
+ const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const {
+ getImpl().calcGeodesic(xP, xQ, tPhint, tQhint, geod);
+}
+
+void ContactGeometry::calcGeodesicUsingOrthogonalMethod
+ (const Vec3& xP, const Vec3& xQ,
+ const Vec3& tPhint, Real lengthHint, Geodesic& geod) const {
+ getImpl().calcGeodesicUsingOrthogonalMethod
+ (xP, xQ, tPhint, lengthHint, geod);
+}
+
+// Compute a geodesic curve starting at the given point, starting in the given
+// direction, and terminating at the given plane.
+// XXX what to do if tP is not in the tangent plane at P -- project it?
+// XXX what to do if we don't hit the plane
+void ContactGeometry::
+shootGeodesicInDirectionUntilPlaneHit(const Vec3& xP, const UnitVec3& tP,
+ const Plane& terminatingPlane, const GeodesicOptions& options,
+ Geodesic& geod) const {
+ getImpl().shootGeodesicInDirectionUntilPlaneHit(xP, tP, terminatingPlane,
+ options, geod);
+}
+
+
+// Compute a geodesic curve of the given length, starting at the given point and
+// in the given direction.
+// XXX what to do if tP is not in the tangent plane at P -- project it?
+void ContactGeometry::
+shootGeodesicInDirectionUntilLengthReached(const Vec3& xP, const UnitVec3& tP,
+ const Real& terminatingLength, const GeodesicOptions& options,
+ Geodesic& geod) const {
+ getImpl().shootGeodesicInDirection(xP, tP, terminatingLength, options, geod);
+}
+
+void ContactGeometry::
+calcGeodesicReverseSensitivity(Geodesic& geodesic, const Vec2& initSensitivity)
+ const
+{
+ getImpl().calcGeodesicReverseSensitivity(geodesic, initSensitivity);
+}
+
+
+void ContactGeometry::
+shootGeodesicInDirectionUntilLengthReachedAnalytical(const Vec3& xP, const UnitVec3& tP,
+ const Real& terminatingLength, const GeodesicOptions& options, Geodesic& geod) const {
+ getImpl().shootGeodesicInDirectionUntilLengthReachedAnalytical(xP, tP,
+ terminatingLength, options, geod);
+}
+
+void ContactGeometry::
+shootGeodesicInDirectionUntilPlaneHitAnalytical(const Vec3& xP, const UnitVec3& tP,
+ const Plane& terminatingPlane, const GeodesicOptions& options,
+ Geodesic& geod) const {
+ getImpl().shootGeodesicInDirectionUntilPlaneHitAnalytical(xP, tP,
+ terminatingPlane, options, geod);
+}
+
+void ContactGeometry::
+calcGeodesicAnalytical(const Vec3& xP, const Vec3& xQ,
+ const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const {
+ getImpl().calcGeodesicAnalytical(xP, xQ, tPhint, tQhint, geod);
+}
+
+
+Vec2 ContactGeometry::calcSplitGeodError(const Vec3& xP, const Vec3& xQ,
+ const UnitVec3& tP, const UnitVec3& tQ,
+ Geodesic* geod) const {
+ return getImpl().calcSplitGeodError(xP, xQ, tP, tQ, geod);
+}
+
+Vec2 ContactGeometry::calcSplitGeodErrorAnalytical(const Vec3& xP, const Vec3& xQ,
+ const UnitVec3& tP, const UnitVec3& tQ,
+ Geodesic* geod) const {
+ return getImpl().calcSplitGeodErrorAnalytical(xP, xQ, tP, tQ, geod);
+}
+
+const Plane& ContactGeometry::getPlane() const { return getImpl().getPlane(); }
+void ContactGeometry::setPlane(const Plane& plane) const { getImpl().setPlane(plane); }
+const Geodesic& ContactGeometry::getGeodP() const { return getImpl().getGeodP(); }
+const Geodesic& ContactGeometry::getGeodQ() const { return getImpl().getGeodQ(); }
+const int ContactGeometry::getNumGeodesicsShot() const { return getImpl().getNumGeodesicsShot(); }
+void ContactGeometry::addVizReporter(ScheduledEventReporter* reporter) const {
+ getImpl().addVizReporter(reporter);
+}
+
+
+
+//==============================================================================
+// CONTACT GEOMETRY IMPL
+//==============================================================================
+
+
+
+//------------------------------------------------------------------------------
+// CALC SURFACE VALUE
+//------------------------------------------------------------------------------
+Real ContactGeometryImpl::
+calcSurfaceValue(const Vec3& p) const {
+ const Vector point(p); // required by Function
+ return getImplicitFunction().calcValue(point);
+}
+
+
+
+//------------------------------------------------------------------------------
+// CALC SURFACE GRADIENT
+//------------------------------------------------------------------------------
+Vec3 ContactGeometryImpl::
+calcSurfaceGradient(const Vec3& p) const {
+ const Vector point(p); // required by Function
+ const Function& f = getImplicitFunction();
+
+ // Arguments to get first derivative from the calcDerivative interface.
+ // Avoid heap allocation by making ArrayViews of stack data.
+ const int x = 0; const ArrayViewConst_<int> fx(&x, &x+1);
+ const int y = 1; const ArrayViewConst_<int> fy(&y, &y+1);
+ const int z = 2; const ArrayViewConst_<int> fz(&z, &z+1);
+
+ Vec3 grad;
+ // Note that the gradient may point inward or outward depending on the
+ // sign convention used by the implicit surface function.
+ grad[0] = f.calcDerivative(fx,point);
+ grad[1] = f.calcDerivative(fy,point);
+ grad[2] = f.calcDerivative(fz,point);
+ return grad;
+}
+
+
+
+//------------------------------------------------------------------------------
+// CALC SURFACE UNIT NORMAL
+//------------------------------------------------------------------------------
+UnitVec3 ContactGeometryImpl::
+calcSurfaceUnitNormal(const Vec3& p) const {
+ // Implicit surface functions may have singularities away from the surface,
+ // such as a point along the central axis of a cylinder. This would
+ // produce a NaN unit normal; we'll instead move the point slightly to
+ // return a valid nearby normal. This helps algorithms that are looking
+ // for the surface to get there rather than blow up.
+ Vec3 grad = calcSurfaceGradient(p);
+ Real gradMag = grad.norm();
+
+ if (gradMag < TinyReal) {
+ // Try perturbing in x, y, or z and take the first one that has a
+ // non-zero gradient.
+ for (int i=0; i < 3; ++i) {
+ Vec3 phat(p); phat[i] += SqrtEps;
+ grad=calcSurfaceGradient(phat);
+ gradMag = grad.norm();
+ if (gradMag >= TinyReal)
+ break;
+ }
+ if (gradMag < TinyReal) {
+ // We're desperate now. Pull a normal out of our hat.
+ grad=Vec3(Real(1),Real(1.1),Real(1.2)); gradMag=grad.norm();
+ }
+ }
+
+ // Implicit surfaces are defined as positive inside and negative outside
+ // therefore normal is the negative of the gradient.
+ // TODO: this should be changed to the opposite convention.
+ return UnitVec3(-grad/gradMag, true);
+}
+
+
+
+//------------------------------------------------------------------------------
+// CALC SURFACE HESSIAN
+//------------------------------------------------------------------------------
+// Note: Hessian is symmetric, although we're filling in all 9 elements here.
+// TODO: use a SymMat33.
+Mat33 ContactGeometryImpl::
+calcSurfaceHessian(const Vec3& p) const {
+ const Vector point(p); // required by Function
+ const Function& f = getImplicitFunction();
+
+ // Arguments to get second derivatives from the calcDerivative interface.
+ // No heap allocation here.
+ const int xx[] = {0,0}; ArrayViewConst_<int> fxx(xx,xx+2);
+ const int xy[] = {0,1}; ArrayViewConst_<int> fxy(xy,xy+2);
+ const int xz[] = {0,2}; ArrayViewConst_<int> fxz(xz,xz+2);
+ //const int yx[] = {1,0}; ArrayViewConst_<int> fyx(yx,yx+2);
+ const int yy[] = {1,1}; ArrayViewConst_<int> fyy(yy,yy+2);
+ const int yz[] = {1,2}; ArrayViewConst_<int> fyz(yz,yz+2);
+ //const int zx[] = {2,0}; ArrayViewConst_<int> fzx(zx,zx+2);
+ //const int zy[] = {2,1}; ArrayViewConst_<int> fzy(zy,zy+2);
+ const int zz[] = {2,2}; ArrayViewConst_<int> fzz(zz,zz+2);
+
+ Mat33 hess;
+
+ hess(0,0) = f.calcDerivative(fxx,point);
+ hess(0,1) = f.calcDerivative(fxy,point);
+ hess(0,2) = f.calcDerivative(fxz,point);
+ hess(1,0) = hess(0,1);
+ hess(1,1) = f.calcDerivative(fyy,point);
+ hess(1,2) = f.calcDerivative(fyz,point);
+ hess(2,0) = hess(0,2);
+ hess(2,1) = hess(1,2);
+ hess(2,2) = f.calcDerivative(fzz,point);
+
+ return hess;
+}
+
+
+
+//------------------------------------------------------------------------------
+// CALC GAUSSIAN CURVATURE
+//------------------------------------------------------------------------------
+Real ContactGeometryImpl::
+calcGaussianCurvature(const Vec3& g, const Mat33& H) const {
+ // Calculate the adjoint matrix. Watch the signs!
+ Mat33 A;
+ A(0,0) = H(1,1)*H(2,2) - square(H(1,2)); // fyy*fzz - fyz^2
+ A(0,1) = H(0,2)*H(1,2) - H(0,1)*H(2,2); // fxz*fyz - fxy*fzz
+ A(0,2) = H(0,1)*H(1,2) - H(0,2)*H(1,1); // fxy*fyz - fxz*fyy
+ A(1,0) = A(0,1);
+ A(1,1) = H(0,0)*H(2,2) - square(H(0,2)); // fxx*fzz - fxz^2
+ A(1,2) = H(0,1)*H(0,2) - H(0,0)*H(1,2); // fxy*fxz - fxx*fyz
+ A(2,0) = A(0,2);
+ A(2,1) = A(1,2);
+ A(2,2) = H(0,0)*H(1,1) - square(H(0,1)); // fxx*fyy - fxy^2
+
+ Real Kg = ~g * (A*g) / square(g.normSqr()); // |g|^4
+ return Kg;
+}
+
+
+
+//------------------------------------------------------------------------------
+// CALC SURFACE CURVATURE IN A DIRECTION
+//------------------------------------------------------------------------------
+Real ContactGeometryImpl::
+calcSurfaceCurvatureInDirection(const Vec3& point,
+ const UnitVec3& direction) const
+{
+ const UnitVec3 n = calcSurfaceUnitNormal(point);
+ const Vec3 g = calcSurfaceGradient(point);
+ const Mat33 H = calcSurfaceHessian(point);
+ const Real knum = ~direction*H*direction; // numerator
+ if (std::abs(knum) < TinyReal)
+ return 0; // don't want to return 0/0.
+
+ const Real k = knum/(~g*n);
+
+ return k;
+}
+
+
+
+
+
+// used in numerical differentiation. TODO: what value for single precision?
+static const Real estimatedGeodesicAccuracy = Real(1e-12);
+static const Real pauseBetweenGeodIterations = 0; // sec, used in newton solver
+
+
+// This local class is used to Calcualte the split geodesic error
+// given scalar angles at P and Q
+class ContactGeometryImpl::SplitGeodesicError: public Differentiator::JacobianFunction {
+
+public:
+ SplitGeodesicError(int nf, int ny, const ContactGeometryImpl& geom,
+ const Vec3& xP, const Vec3& xQ,
+ const Vec3& tPhint, const Vec3& tQhint)
+ : Differentiator::JacobianFunction(nf, ny),
+ geom(geom),
+ P(xP), Q(xQ),
+ R_SP(geom.calcTangentBasis(P, tPhint)),
+ R_SQ(geom.calcTangentBasis(Q, tQhint)) { }
+
+ // x = ~[thetaP, thetaQ]
+ int f(const Vector& x, Vector& fx) const {
+ UnitVec3 tP = calcUnitTangentVec(x[0], R_SP);
+ UnitVec3 tQ = calcUnitTangentVec(x[1], R_SQ);
+
+ Vec2 geodErr = geom.calcSplitGeodError(P, Q, tP, tQ);
+
+ // error between geodesic end points at plane
+ fx[0] = geodErr[0];
+ fx[1] = geodErr[1];
+ return 0;
+ }
+
+
+private:
+ const ContactGeometryImpl& geom;
+ const Vec3& P;
+ const Vec3& Q;
+ const Rotation R_SP;
+ const Rotation R_SQ;
+
+}; // class SplitGeodesicError
+
+// This local class is used to calculate the orthogonal geodesic error
+// Jacobian given angle theta and length.
+class ContactGeometryImpl::OrthoGeodesicError: public Differentiator::JacobianFunction {
+
+public:
+ OrthoGeodesicError(const ContactGeometryImpl& geom,
+ const Vec3& xP, const Vec3& xQ) :
+ Differentiator::JacobianFunction(2, 2),
+ geom(geom), P(xP), Q(xQ) { }
+
+ // x = ~[thetaP, length]
+ int f(const Vector& x, Vector& fx) const {
+ Geodesic geod;
+ Vec2 geodErr = geom.calcOrthogonalGeodError(P, Q, x[0], x[1], geod);
+
+ // error between geodesic end points at plane
+ fx[0] = geodErr[0];
+ fx[1] = geodErr[1];
+ return 0;
+ }
+
+
+private:
+ const ContactGeometryImpl& geom;
+ const Vec3 P;
+ const Vec3 Q;
+}; // class OrthoGeodesicError
+
+
+//------------------------------------------------------------------------------
+// INIT GEODESIC
+//------------------------------------------------------------------------------
+
+void ContactGeometryImpl::initGeodesic(const Vec3& xP, const Vec3& xQ,
+ const Vec3& xSP, const GeodesicOptions& options, Geodesic& geod) const
+{
+ // TODO
+}
+
+
+
+//------------------------------------------------------------------------------
+// CONTINUE GEODESIC
+//------------------------------------------------------------------------------
+// Given two points and a previous geodesic curve close to the points, find
+// a geodesic curve connecting the points that is close to the previous
+// geodesic. See header or doxygen for algorithmic details.
+// Note that the geodesic runs from P' to Q', which are closest-point
+// projections of the initial points which might not be on the surface.
+void ContactGeometryImpl::
+continueGeodesic(const Vec3& xP, const Vec3& xQ, const Geodesic& prevGeod,
+ const GeodesicOptions& options, Geodesic& geod) const
+{
+ const Real StraightLineGeoFrac = Real(1e-5); // a straight line
+
+ const Vec3 P = projectDownhillToNearestPoint(xP);
+ const Vec3 Q = projectDownhillToNearestPoint(xQ);
+
+ //cout << "continueGeo(P=" << P << ",Q=" << Q << ")\n";
+
+ // If P and Q are bit-identical to P and Q from the previous geodesic,
+ // then the new one is just a copy of the previous one. This is especially
+ // likely when doing numerical Jacobians since many value remain unchanged.
+ if (prevGeod.getNumPoints()
+ && prevGeod.getPointP()==P && prevGeod.getPointQ()==Q) {
+ geod = prevGeod;
+ cout << "REUSING OLD GEODESIC of length="
+ << prevGeod.getLength() << "\n";
+ return;
+ }
+
+ const Vec3 PQ = Q-P;
+ const Real PQlength = PQ.norm();
+ const UnitVec3 PQdir =
+ PQlength == 0 ? UnitVec3(XAxis) : UnitVec3(PQ/PQlength, true);
+ cout << " PQlen=" << PQlength << " PQdir=" << PQdir << "\n";
+
+ // If the length is less than this fraction of the maximum radius of
+ // curvature (1/kdP) then the geodesic is indistinguishable from a
+ // straight line.
+ // TODO: if the previous geodesic was very long and came all the way
+ // around this will incorrectly switch to a very short geodesic. Does
+ // that matter?
+ const Real kdP = std::abs(calcSurfaceCurvatureInDirection(P,PQdir));
+ if (PQlength*kdP <= StraightLineGeoFrac) {
+ cout << "STRAIGHT LINE GEO length=" << PQlength << "\n";
+ makeStraightLineGeodesic(P, Q, PQdir, options, geod);
+ return;
+ }
+
+ // If there is no previous geodesic the best we can do is to start in
+ // direction PQdir, and guess length |PQ|.
+ if (!prevGeod.getNumPoints()) {
+ calcGeodesicUsingOrthogonalMethod(P, Q, PQdir, PQlength, geod);
+ //calcGeodesicAnalytical(P, Q, PQ, PQ, geod);
+ return;
+ }
+
+ // First classify the previous geodesic as direct or indirect. Direct is
+ // a strict classification; only if the end tangents are aligned with the
+ // PQ line to within an allowed cone angle is it direct.
+ const Real CosMaxDirectAngle = Real(0.9); // about 25 degrees
+
+ // Find maximum curvature of previous geodesic to use as a length scale.
+ // TODO: should store this with the geodesic so we can use any intermediate
+ // points also. TODO: use binormal curvature also?
+ const Real maxK = std::max(std::abs(prevGeod.getCurvatureP()),
+ std::abs(prevGeod.getCurvatureQ()));
+
+ // We consider a geodesic straight when the separation of its end points
+ // is a small fraction of the radius of curvature because then the chord
+ // and the arc are the same length to a MUCH smaller tolerance than that.
+ const Vec3 prevPQ = prevGeod.getPointQ() - prevGeod.getPointP();
+ const Real prevPQlen = prevPQ.norm();
+ const bool isPrevStraightLine = prevPQlen*maxK <= StraightLineGeoFrac;
+
+ const UnitVec3 eprevPQ = isPrevStraightLine
+ ? prevGeod.getTangentP()
+ : UnitVec3(prevPQ/prevPQlen, true);
+
+ const Real cosConetP = dot(prevGeod.getTangentP(), eprevPQ);
+ const Real cosConetQ = dot(prevGeod.getTangentQ(), eprevPQ);
+ const bool isDirect = std::min(cosConetP,cosConetQ) >= CosMaxDirectAngle;
+
+ UnitVec3 tPhint = prevGeod.getTangentP(); // might flip
+ UnitVec3 tQhint = prevGeod.getTangentQ();
+ Real sHint = std::max(prevGeod.getLength(), PQlength);
+
+ if (isDirect) {
+ if (~tPhint*PQdir < 0 && ~tQhint*PQdir < 0) {
+ tPhint = PQdir;
+ tQhint = PQdir;
+ sHint = PQlength;
+ cout << "GEODESIC FLIPPED. Prev len was "
+ << prevGeod.getLength() << endl;
+ }
+ }
+
+ calcGeodesicUsingOrthogonalMethod(P, Q, tPhint, sHint, geod);
+ //calcGeodesicAnalytical(P, Q, tPhint, tQhint, geod);
+}
+
+
+
+//------------------------------------------------------------------------------
+// MAKE STRAIGHT LINE GEODESIC
+//------------------------------------------------------------------------------
+void ContactGeometryImpl::
+makeStraightLineGeodesic(const Vec3& xP, const Vec3& xQ,
+ const UnitVec3& defaultDirectionIfNeeded,
+ const GeodesicOptions& options, Geodesic& geod) const
+{
+ geod.clear();
+
+ Vec3 Pprime = projectDownhillToNearestPoint(xP);
+ Vec3 Qprime = projectDownhillToNearestPoint(xQ);
+
+ const bool isZeroLength =
+ Geo::Point::pointsAreNumericallyCoincident(Pprime,Qprime);
+
+ UnitVec3 d;
+ Real length;
+ if (isZeroLength) {
+ const Vec3 mid = Geo::Point::findMidpoint(Pprime, Qprime);
+ Pprime = Qprime = projectDownhillToNearestPoint(mid);
+ d = defaultDirectionIfNeeded;
+ length = 0;
+ } else {
+ const Vec3 PQ = Qprime - Pprime;
+ length = PQ.norm();
+ d = UnitVec3(PQ/length, true);
+ }
+
+ const UnitVec3 nP = calcSurfaceUnitNormal(Pprime);
+ const UnitVec3 nQ = calcSurfaceUnitNormal(Qprime);
+ const Rotation RP(nP, ZAxis, d, YAxis);
+ const Rotation RQ(nQ, ZAxis, d, YAxis);
+ geod.addFrenetFrame(Transform(RP, Pprime));
+ geod.addFrenetFrame(Transform(RQ, Qprime));
+ geod.addArcLength(0);
+ geod.addArcLength(length);
+ geod.addCurvature(calcSurfaceCurvatureInDirection(Pprime, RP.y()));
+ geod.addCurvature(calcSurfaceCurvatureInDirection(Qprime, RQ.y()));
+ geod.addDirectionalSensitivityPtoQ(Vec2(0,1));
+ geod.addDirectionalSensitivityPtoQ(Vec2(length,1));
+ geod.addDirectionalSensitivityQtoP(Vec2(length,1));
+ geod.addDirectionalSensitivityQtoP(Vec2(0,1));
+ geod.addPositionalSensitivityPtoQ(Vec2(1,0));
+ geod.addPositionalSensitivityPtoQ(Vec2(1,0));
+ geod.addPositionalSensitivityQtoP(Vec2(1,0));
+ geod.addPositionalSensitivityQtoP(Vec2(1,0));
+ geod.setBinormalCurvatureAtP(calcSurfaceCurvatureInDirection(Pprime, RP.x()));
+ geod.setBinormalCurvatureAtQ(calcSurfaceCurvatureInDirection(Qprime, RQ.x()));
+
+ //TODO: We're estimating torsion here as the change in binormal per unit
+ // of arc length, in the -n direction. If length is zero we'll just say
+ // the torsion is zero although that is wrong.
+ const Real ooLength = isZeroLength ? Real(0) : 1/length;
+ const Vec3 bChg = geod.getBinormalQ()-geod.getBinormalP();
+ geod.setTorsionAtP( (~nP * bChg) * ooLength );
+ geod.setTorsionAtQ( (~nQ * bChg) * ooLength );
+
+ geod.setIsConvex(geod.getCurvatureP() >= 0 && geod.getCurvatureQ() >= 0);
+ geod.setIsShortest(true);
+ geod.setInitialStepSizeHint(NaN); // no clue
+ geod.setAchievedAccuracy(Geo::getDefaultTol<Real>());
+}
+
+
+
+//------------------------------------------------------------------------------
+// SHOOT GEODESIC IN DIRECTION
+//------------------------------------------------------------------------------
+// Utility method used by shootGeodesicInDirectionUntilPlaneHit
+// and shootGeodesicInDirectionUntilLengthReached.
+// P is not necessarily on the surface and tP is not necessarily tangent
+// to the surface, so the first thing we do is project P to the nearest
+// surface point P', calculate the outward unit normal n' at P', then
+// project tP to tP' by removing any component it has in the n' direction,
+// then renormalizing.
+static const Real IntegratorAccuracy = Real(1e-6); // TODO: how to choose?
+static const Real IntegratorConstraintTol = Real(1e-10);
+void ContactGeometryImpl::
+shootGeodesicInDirection(const Vec3& P, const UnitVec3& tP,
+ const Real& finalTime, const GeodesicOptions& options,
+ Geodesic& geod) const {
+
+ // integrator settings
+ const Real startTime = 0;
+
+ ++numGeodesicsShot;
+
+ // Initialize state
+ State sysState = ptOnSurfSys->getDefaultState();
+ sysState.setTime(startTime);
+ Vector& q = sysState.updQ();
+ Vector& u = sysState.updU();
+ q[0] = P[0]; q[1] = P[1]; q[2] = P[2];
+ u[0] = tP[0]; u[1] = tP[1]; u[2] = tP[2];
+
+ // Jacobi field states
+ q[3] = 0;
+ u[3] = 1;
+
+ // Setup integrator to integrate until terminatingLength
+ //RungeKutta3Integrator integ(*ptOnSurfSys);
+ RungeKuttaMersonIntegrator integ(*ptOnSurfSys);
+ //RungeKuttaFeldbergIntegrator integ(*ptOnSurfSys);
+ integ.setAccuracy(IntegratorAccuracy);
+ integ.setConstraintTolerance(IntegratorConstraintTol);
+ integ.setFinalTime(finalTime);
+ integ.setReturnEveryInternalStep(true); // save geodesic knot points
+
+ // Setup timestepper in order to handle event when geodesic hits the plane
+ TimeStepper ts(*ptOnSurfSys, integ);
+ ts.setReportAllSignificantStates(true);
+ ts.initialize(sysState);
+
+ // Simulate it, and record geodesic knot points after each step
+ // Terminate when geodesic hits the plane
+ Integrator::SuccessfulStepStatus status;
+ int stepcnt = 0;
+ geod.setIsConvex(true); // Set false if we see negative curvature anywhere.
+ while (true) {
+ // Final time is already reported by the time we see end of simulation;
+ // don't duplicate the last step.
+ if ((status=ts.stepTo(Infinity)) == Integrator::EndOfSimulation)
+ break;
+
+ // If we stopped just for a report, don't add that to the geodesic;
+ // that may happen if a visualization reporter has been hung on this
+ // system for watching the geodesic grow.
+ if (status == Integrator::ReachedReportTime)
+ continue;
+
+ // Careful -- integrator has its own internal copies of the state and
+ // the one returned won't always be the same one so you need to get a
+ // fresh reference each time.
+ const State& state = integ.getState();
+ const Real s = state.getTime();
+ geod.addArcLength(s);
+
+ const Vec3& pt = Vec3::getAs(&state.getQ()[0]);
+ const UnitVec3 n = calcSurfaceUnitNormal(pt);
+ const Vec3& tangent = Vec3::getAs(&state.getU()[0]);
+ // Rotation will orthogonalize so x direction we get may not be
+ // exactly the same as what we supply here.
+ const Transform frenetFrame(Rotation(n, ZAxis, tangent, YAxis), pt);
+ geod.addFrenetFrame(frenetFrame);
+ geod.addDirectionalSensitivityPtoQ(Vec2(state.getQ()[3],
+ state.getU()[3]));
+ geod.addPositionalSensitivityPtoQ(Vec2(NaN,NaN)); // XXX
+ const Real kappa = calcSurfaceCurvatureInDirection(pt, frenetFrame.y());
+ geod.addCurvature(kappa);
+ if (kappa < 0)
+ geod.setIsConvex(false);
+
+ ++stepcnt;
+ }
+
+ geod.setBinormalCurvatureAtP(
+ calcSurfaceCurvatureInDirection(geod.getPointP(),geod.getBinormalP()));
+ geod.setBinormalCurvatureAtQ(
+ calcSurfaceCurvatureInDirection(geod.getPointQ(),geod.getBinormalQ()));
+
+ //TODO: numerical torsion estimate for now
+ const Array_<Transform>& frenet = geod.getFrenetFrames();
+ const Array_<Real>& arcLen = geod.getArcLengths();
+ const int last = geod.getNumPoints()-1;
+ const Real lFirst = arcLen[1];
+ const Real lLast = arcLen[last] - arcLen[last-1];
+ const Real tauP = lFirst==0 ? Real(0)
+ : -dot(frenet[0].z(),
+ (frenet[1].x()-frenet[0].x())) / lFirst; // dbP/ds
+ const Real tauQ = lLast==0 ? Real(0)
+ : -dot(frenet[last].z(),
+ (frenet[last].x()-frenet[last-1].x())) / lLast;
+
+ geod.setTorsionAtP(tauP); geod.setTorsionAtQ(tauQ);
+
+ geod.setIsShortest(false); // TODO
+ geod.setAchievedAccuracy(IntegratorAccuracy); // TODO: accuracy of length?
+ // TODO: better to use something like the second-to-last step, or average
+ // excluding initial and last steps, so that we don't have to start small.
+ geod.setInitialStepSizeHint(integ.getActualInitialStepSizeTaken());
+}
+
+void ContactGeometryImpl::
+shootGeodesicInDirection2(const Vec3& P, const UnitVec3& tP,
+ const Real& finalArcLength, const GeodesicOptions& options,
+ Geodesic& geod) const {
+
+ // integrator settings
+ const Real startArcLength = 0;
+
+ GeodesicOnImplicitSurface eqns(*this);
+ GeodesicIntegrator<GeodesicOnImplicitSurface>
+ integ(eqns,IntegratorAccuracy,IntegratorConstraintTol);
+ static const int N = GeodesicOnImplicitSurface::N;
+
+ ++numGeodesicsShot;
+
+ integ.initialize(startArcLength, eqns.getInitialState(P,tP));
+ // Aliases for the integrators internal time and state variables.
+ const Vec<N>& y = integ.getY();
+ const Real& s = integ.getTime(); // arc length
+
+ // Simulate it, and record geodesic knot points after each step
+ int stepcnt = 0;
+ geod.setIsConvex(true); // Set false if we see negative curvature anywhere.
+ while (true) {
+ // Record a knot point.
+ geod.addArcLength(s);
+ const Vec3& pt = eqns.getP(y);
+ const UnitVec3 n = calcSurfaceUnitNormal(pt);
+ const Vec3& tangent = eqns.getV(y);
+ // Rotation will orthogonalize so x direction we get may not be
+ // exactly the same as what we supply here.
+ const Transform frenetFrame(Rotation(n, ZAxis, tangent, YAxis), pt);
+ geod.addFrenetFrame(frenetFrame);
+ geod.addDirectionalSensitivityPtoQ(Vec2(eqns.getJRot(y),
+ eqns.getJRotDot(y)));
+ geod.addPositionalSensitivityPtoQ(Vec2(eqns.getJTrans(y),
+ eqns.getJTransDot(y)));
+ const Real kappa = calcSurfaceCurvatureInDirection(pt, frenetFrame.y());
+ geod.addCurvature(kappa);
+ if (kappa < 0) geod.setIsConvex(false);
+
+ if (s == finalArcLength)
+ break;
+
+ integ.takeOneStep(finalArcLength);
+ ++stepcnt;
+ }
+
+ //printf("RKM acc=%g tol=%g: %d/%d steps, errtest=%d projfail=%d\n",
+ // integ.getRequiredAccuracy(), integ.getConstraintTolerance(),
+ // integ.getNumStepsTaken(), integ.getNumStepsAttempted(),
+ // integ.getNumErrorTestFailures(), integ.getNumProjectionFailures());
+
+ geod.setBinormalCurvatureAtP(
+ calcSurfaceCurvatureInDirection(geod.getPointP(),geod.getBinormalP()));
+ geod.setBinormalCurvatureAtQ(
+ calcSurfaceCurvatureInDirection(geod.getPointQ(),geod.getBinormalQ()));
+
+ //TODO: numerical torsion estimate for now
+ const Array_<Transform>& frenet = geod.getFrenetFrames();
+ const Array_<Real>& arcLen = geod.getArcLengths();
+ const int last = geod.getNumPoints()-1;
+ const Real lFirst = arcLen[1];
+ const Real lLast = arcLen[last] - arcLen[last-1];
+ const Real tauP = lFirst==0 ? Real(0)
+ : -dot(frenet[0].z(),
+ (frenet[1].x()-frenet[0].x())) / lFirst; // dbP/ds
+ const Real tauQ = lLast==0 ? Real(0)
+ : -dot(frenet[last].z(),
+ (frenet[last].x()-frenet[last-1].x())) / lLast;
+
+ geod.setTorsionAtP(tauP); geod.setTorsionAtQ(tauQ);
+
+ geod.setIsShortest(false); // TODO
+ geod.setAchievedAccuracy(IntegratorAccuracy); // TODO: accuracy of length?
+ // TODO: better to use something like the second-to-last step, or average
+ // excluding initial and last steps, so that we don't have to start small.
+ geod.setInitialStepSizeHint(integ.getActualInitialStepSizeTaken());
+}
+
+
+//------------------------------------------------------------------------------
+// CALC GEODESIC REVERSE SENSITIVITY
+//------------------------------------------------------------------------------
+// After a geodesic has been calculated, this method integrates backwards
+// to fill in the missing reverse Jacobi term.
+void ContactGeometryImpl::
+calcGeodesicReverseSensitivity(Geodesic& geod, const Vec2& initJacobi) const {
+
+ // Don't look for a plane.
+ geodHitPlaneEvent->setEnabled(false);
+
+ // integrator settings
+
+ //RungeKutta3Integrator integ(ptOnSurfSys);
+ RungeKuttaMersonIntegrator integ(*ptOnSurfSys);
+ integ.setAccuracy(IntegratorAccuracy);
+ integ.setConstraintTolerance(IntegratorConstraintTol);
+ State sysState = ptOnSurfSys->getDefaultState();
+ Vector& q = sysState.updQ();
+ Vector& u = sysState.updU();
+
+ // Initial Jacobi field states.
+ q[3] = initJacobi[0];
+ u[3] = initJacobi[1];
+
+ Array_<Vec2>& jQ = geod.updDirectionalSensitivityQtoP();
+ jQ.resize(geod.getNumPoints());
+ jQ.back() = initJacobi;
+
+ for (int step=geod.getNumPoints()-1; step >= 1; --step) {
+ // Curve goes from P to Q. We have to integrate backwards from Q to P.
+ const Transform& QFrenet = geod.getFrenetFrames()[step];
+ const Transform& PFrenet = geod.getFrenetFrames()[step-1];
+ const Real sQ = geod.getArcLengths()[step];
+ const Real sP = geod.getArcLengths()[step-1];
+ const Vec3& Q = QFrenet.p();
+ const UnitVec3& tQ = QFrenet.x(); // we'll reverse this
+ const Vec3& P = PFrenet.p();
+ const UnitVec3& tP = PFrenet.x();
+
+ // Initialize state
+ sysState.setTime(0);
+ q[0] = Q[0]; q[1] = Q[1]; q[2] = Q[2];
+ u[0] = -tQ[0]; u[1] = -tQ[1]; u[2] = -tQ[2];
+ q[3] = jQ[step][0];
+ u[3] = jQ[step][1];
+
+ const Real arcLength = sQ-sP; // how far to integrate
+
+ integ.initialize(sysState);
+
+ Integrator::SuccessfulStepStatus status;
+ do {
+ status = integ.stepTo(arcLength);
+ if (status == Integrator::StartOfContinuousInterval)
+ continue;
+ if (integ.getTime() < arcLength) {
+ printf("integ to %g returned early at %g with status=%s\n",
+ arcLength, integ.getTime(),
+ Integrator::getSuccessfulStepStatusString(status).c_str());
+ }
+ } while (integ.getTime() < arcLength);
+
+ // Save Jacobi field value.
+ const State& state = integ.getState();
+ jQ[step-1] = Vec2(state.getQ()[3],state.getU()[3]);
+ }
+}
+
+void ContactGeometryImpl::
+calcGeodesicReverseSensitivity2
+ (Geodesic& geod, const Vec2& initJRot, const Vec2& initJTrans) const {
+
+ GeodesicOnImplicitSurface eqns(*this);
+ GeodesicIntegrator<GeodesicOnImplicitSurface>
+ integ(eqns,IntegratorAccuracy,IntegratorConstraintTol);
+ static const int N = GeodesicOnImplicitSurface::N;
+
+ integ.initialize(0,
+ eqns.getInitialState(geod.getPointQ(),-geod.getTangentQ()));
+ // Aliases for the integrators internal time and state variables.
+ const Vec<N>& y = integ.getY();
+ const Real& s = integ.getTime(); // arc length
+
+ // These two arrays need to be filled in to complete the geodesic.
+ Array_<Vec2>& jrP = geod.updDirectionalSensitivityQtoP();
+ jrP.resize(geod.getNumPoints());
+ jrP.back() = initJRot;
+
+ Array_<Vec2>& jtP = geod.updPositionalSensitivityQtoP();
+ jtP.resize(geod.getNumPoints());
+ jtP.back() = initJTrans;
+
+ for (int step=geod.getNumPoints()-1; step >= 1; --step) {
+ // Curve goes from P to Q. We have to integrate backwards from Q to P.
+ const Transform& QFrenet = geod.getFrenetFrames()[step];
+ const Transform& PFrenet = geod.getFrenetFrames()[step-1];
+ const Real sQ = geod.getArcLengths()[step];
+ const Real sP = geod.getArcLengths()[step-1];
+ const Vec3& Q = QFrenet.p();
+ const UnitVec3& tQ = QFrenet.x(); // we'll reverse this
+ const Vec3& P = PFrenet.p();
+ const UnitVec3& tP = PFrenet.x();
+
+ // Initialize state
+ Vec<N> yInit;
+ eqns.updP(yInit) = Q;
+ eqns.updV(yInit) = -tQ.asVec3();
+ eqns.updJRot(yInit) = jrP[step][0];
+ eqns.updJRotDot(yInit) = jrP[step][1];
+ eqns.updJTrans(yInit) = jtP[step][0];
+ eqns.updJTransDot(yInit) = jtP[step][1];
+
+ const Real arcLength = sQ-sP; // how far to integrate
+
+ integ.setTimeAndState(0, yInit);
+ integ.setNextStepSizeToTry(arcLength);
+
+ do { // usually we'll be able to do this in one step
+ integ.takeOneStep(arcLength);
+ //if (s < arcLength) printf("integ to %g returned early at %g\n", arcLength, s);
+ } while (s < arcLength);
+
+ // Save Jacobi field values.
+ jrP[step-1] = Vec2(eqns.getJRot(y), eqns.getJRotDot(y));
+ jtP[step-1] = Vec2(eqns.getJTrans(y), eqns.getJTransDot(y));
+ }
+
+ //printf("REVERSE acc=%g tol=%g: %d/%d steps, errtest=%d projfail=%d\n",
+ // integ.getRequiredAccuracy(), integ.getConstraintTolerance(),
+ // integ.getNumStepsTaken(), integ.getNumStepsAttempted(),
+ // integ.getNumErrorTestFailures(), integ.getNumProjectionFailures());
+}
+
+
+// Compute a geodesic curve starting at the given point, starting in the given
+// direction, and terminating at the given plane.
+// XXX what to do if tP is not in the tangent plane at P -- project it?
+// XXX what to do if we don't hit the plane
+void ContactGeometryImpl::
+shootGeodesicInDirectionUntilPlaneHit(const Vec3& xP, const UnitVec3& tP,
+ const Plane& terminatingPlane, const GeodesicOptions& options,
+ Geodesic& geod) const {
+ geodHitPlaneEvent->setEnabled(true);
+ geodHitPlaneEvent->setPlane(terminatingPlane);
+ // TODO: need a reasonable max length
+ const Real MaxLength = /*Infinity*/100;
+ shootGeodesicInDirection(xP, tP, MaxLength, options, geod);
+}
+
+
+// Compute a geodesic curve of the given length, starting at the given point and
+// in the given direction.
+// XXX what to do if tP is not in the tangent plane at P -- project it?
+void ContactGeometryImpl::
+shootGeodesicInDirectionUntilLengthReached(const Vec3& xP, const UnitVec3& tP,
+ const Real& terminatingLength, const GeodesicOptions& options,
+ Geodesic& geod) const {
+ geodHitPlaneEvent->setEnabled(false);
+#ifdef USE_NEW_INTEGRATOR
+ shootGeodesicInDirection2(xP, tP, terminatingLength, options, geod);
+#else
+ shootGeodesicInDirection(xP, tP, terminatingLength, options, geod);
+#endif
+}
+
+
+
+static Real cleanUpH(Real hEst, Real y0) {
+ volatile Real temp = y0+hEst;
+ return temp-y0;
+}
+
+static Real maxabs(Vec2 x) {
+ return std::max(std::abs(x[0]), std::abs(x[1]));
+}
+
+static Real maxabsdiff(Vec2 x, Vec2 xold) {
+ return std::max(std::abs(x[0]-xold[0])/std::max(x[0],Real(1)),
+ std::abs(x[1]-xold[1])/std::max(x[1],Real(1)));
+}
+
+
+
+//------------------------------------------------------------------------------
+// CALC GEODESIC
+//------------------------------------------------------------------------------
+// Utility method to find geodesic between P and Q
+// with starting directions tPhint and tQhint
+// XXX tangent basis should be formed from previous geodesic
+void ContactGeometryImpl::calcGeodesic(const Vec3& xP, const Vec3& xQ,
+ const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const {
+
+ // Newton solver settings. TODO: single precision?
+ const Real ftol = Real(1e-9);
+ const Real xtol = Real(1e-9);
+ const Real minlam = Real(1e-9);
+ const int maxNewtonIterations = 25;
+
+ // reset counter
+ numGeodesicsShot = 0;
+
+ // define basis
+ R_SP = calcTangentBasis(xP, tPhint);
+ R_SQ = calcTangentBasis(xQ, tQhint);
+
+ // calculate plane bisecting P and Q, and use as termination condition for integrator
+ UnitVec3 normal(xQ - xP);
+ Real offset = (~(xP+xQ)*normal)/2 ;
+ geodHitPlaneEvent->setPlane(Plane(normal, offset));
+
+ Mat22 J;
+ Vec2 x, xold, dx, Fx;
+
+ // initial conditions
+ x[0] = Pi/2; // thetaP
+ x[1] = -Pi/2; // thetaQ: shoot toward P, i.e. in the opposite direction of tQ
+ Real f, fold, lam = 1;
+
+// splitGeodErr = new SplitGeodesicError(2, 2, *const_cast<ContactGeometry*>(this),
+// xP, xQ, tPhint, tQhint);
+// splitGeodErr->setEstimatedAccuracy(estimatedGeodesicAccuracy);
+// Differentiator diff( *const_cast<SplitGeodesicError*>(splitGeodErr));
+
+// splitGeodErr->f(x, Fx);
+ Fx = calcSplitGeodError(xP, xQ, x[0], x[1]);
+ if (vizReporter != NULL) {
+ vizReporter->handleEvent(ptOnSurfSys->getDefaultState());
+ sleepInSec(pauseBetweenGeodIterations);
+ }
+
+ f = std::sqrt(~Fx*Fx);
+
+ for (int i = 0; i < maxNewtonIterations; ++i) {
+ if (maxabs(Fx) < ftol) {
+ //std::cout << "geodesic converged in " << i << " iterations" << std::endl;
+// std::cout << "err = " << Fx << std::endl;
+ break;
+ }
+// diff.calcJacobian(x, Fx, J, Differentiator::ForwardDifference);
+ J = calcSplitGeodErrorJacobian(xP, xQ, x[0], x[1],
+ Differentiator::ForwardDifference);
+ dx = J.invert()*Fx;
+
+ fold = f;
+ xold = x;
+
+ // backtracking
+ lam = 1;
+ while (true) {
+ x = xold - lam*dx;
+// splitGeodErr->f(x, Fx);
+ Fx = calcSplitGeodError(xP, xQ, x[0], x[1]);
+ f = std::sqrt(~Fx*Fx);
+ if (f > fold && lam > minlam) {
+ lam = lam / 2;
+ } else {
+ break;
+ }
+ }
+ if (maxabsdiff(x,xold) < xtol) {
+ std::cout << "converged on step size after " << i << " iterations" << std::endl;
+ std::cout << "err = " << Fx << std::endl;
+ break;
+ }
+
+ if (vizReporter != NULL) {
+ vizReporter->handleEvent(ptOnSurfSys->getDefaultState());
+ sleepInSec(pauseBetweenGeodIterations);
+ }
+
+ }
+
+ // Finish each geodesic with reverse Jacobi field.
+ calcGeodesicReverseSensitivity(geodP,
+ geodQ.getDirectionalSensitivityPtoQ().back());
+ calcGeodesicReverseSensitivity(geodQ,
+ geodP.getDirectionalSensitivityPtoQ().back());
+
+ mergeGeodesics(geodP, geodQ, geod);
+}
+
+
+
+//------------------------------------------------------------------------------
+// CALC GEODESIC USING ORTHOGONAL METHOD
+//------------------------------------------------------------------------------
+void ContactGeometryImpl::calcGeodesicUsingOrthogonalMethod
+ (const Vec3& xP, const Vec3& xQ,
+ const Vec3& tPhint, Real lengthHint, Geodesic& geod) const
+{
+ const Vec3 P = projectDownhillToNearestPoint(xP);
+ const Vec3 Q = projectDownhillToNearestPoint(xQ);
+
+ // Newton solver settings. TODO: single precision values?
+ const Real ftol = Real(1e-9);
+ const Real xtol = Real(1e-12);
+ const Real minlam = Real(1e-3);
+ const int MaxIterations = 25;
+
+ bool useNewtonIteration = true;
+
+ // reset counter
+ numGeodesicsShot = 0;
+
+ // Define basis. This will not change during the solution.
+ R_SP = calcTangentBasis(P, tPhint);
+
+ Mat22 J;
+ Vec2 x, xold, dx, Fx;
+ Matrix JMat;
+ // initial conditions
+ x[0] = Pi/2; // thetaP
+ x[1] = lengthHint;
+ Real f, fold, dist, lam = 1;
+
+ Fx = calcOrthogonalGeodError(P, Q, x[0], x[1], geod);
+ if (vizReporter != NULL) {
+ vizReporter->handleEvent(ptOnSurfSys->getDefaultState());
+ sleepInSec(pauseBetweenGeodIterations);
+ }
+
+ //OrthoGeodesicError orthoErr(*this, P, Q);
+ //orthoErr.setEstimatedAccuracy(1e-12); // TODO
+ //Differentiator diff(orthoErr);
+
+ //cout << "Using " << (useNewtonIteration ? "NEWTON" : "FIXED POINT")
+ // << " iteration\n";
+
+ for (int i = 0; i < MaxIterations; ++i) {
+ f = std::sqrt(~Fx*Fx);
+ const Vec3 r_QQhat = geod.getPointQ()-Q;
+ dist = r_QQhat.norm();
+ //std::cout << "ORTHO x= " << x << " err = " << Fx << " |err|=" << f
+ // << " dist=" << dist << std::endl;
+ if (f <= ftol) {
+ // std::cout << "ORTHO geodesic converged in "
+ // << i << " iterations with err=" << f << std::endl;
+ break;
+ }
+
+ if (useNewtonIteration) {
+ // This numerical Jacobian is very bad; CentralDifference is
+ // required in order to produce a reasonable one.
+ //diff.calcJacobian(Vector(x), Vector(Fx), JMat,
+ // Differentiator::CentralDifference);
+ //J = Mat22::getAs(&JMat(0,0));
+
+ //XXX: numerical calculation of j and jdot
+ //Geodesic geod0, geod1;
+ //calcOrthogonalGeodError(P, Q, x[0]-1e-5, x[1], geod0);
+ //calcOrthogonalGeodError(P, Q, x[0]+1e-5, x[1], geod1);
+ //Vec3 qdiff = geod1.getPointQ() - geod0.getPointQ();
+ //Real num_j = dot(qdiff,geod0.getBinormalQ())/2e-5;
+ //printf("Jacobi Q num=%g, analytic=%g\n",
+ // num_j, geod0.getJacobiQ());
+
+ //calcOrthogonalGeodError(P, Q, x[0]-1e-5, x[1]+1e-5, geod0);
+ //calcOrthogonalGeodError(P, Q, x[0]+1e-5, x[1]+1e-5, geod1);
+ //Vec3 qdiff2 = geod1.getPointQ() - geod0.getPointQ();
+ //Real num_j2 = dot(qdiff2,geod0.getBinormalQ())/2e-5;
+
+ //Real num_jd = (num_j2-num_j)/1e-5;
+ //printf("Jacobi dot Q num=%g, analytic=%g\n",
+ // num_jd, geod0.getJacobiQDot());
+
+ //XXX: numerical calculation of jt and jtdot
+ //Geodesic geod0, geod1;
+ //calcOrthogonalGeodError(P-1e-5*geod.getBinormalP(), Q, x[0], x[1], geod0);
+ //calcOrthogonalGeodError(P+1e-5*geod.getBinormalP(), Q, x[0], x[1], geod1);
+ //Vec3 qdiff = geod1.getPointQ() - geod0.getPointQ();
+ //Real num_jt = dot(qdiff,geod0.getBinormalQ())/2e-5;
+ //printf("Jacobi Trans Q num=%g, analytic=%g\n",
+ // num_jt, geod0.getJacobiTransQ());
+
+ //calcOrthogonalGeodError(P-1e-5*geod.getBinormalP(), Q, x[0], x[1]+1e-5, geod0);
+ //calcOrthogonalGeodError(P+1e-5*geod.getBinormalP(), Q, x[0], x[1]+1e-5, geod1);
+ //Vec3 qdiff2 = geod1.getPointQ() - geod0.getPointQ();
+ //Real num_jt2 = dot(qdiff2,geod0.getBinormalQ())/2e-5;
+
+ //Real num_jtd = (num_jt2-num_jt)/1e-5;
+ //printf("Jacobi dot Q num=%g, analytic=%g\n",
+ // num_jtd, geod0.getJacobiTransQDot());
+
+ const Real jt = geod.getJacobiTransQ();
+ const Real jtd = geod.getJacobiTransQDot();
+
+ const Real eh = ~r_QQhat*geod.getNormalQ();
+ const Real es = ~r_QQhat*geod.getTangentQ();
+ const Real eb = ~r_QQhat*geod.getBinormalQ();
+ const Real j = geod.getJacobiQ();
+ const Real jd = geod.getJacobiQDot();
+ const Real tau = geod.getTorsionQ();
+ const Real kappa = geod.getCurvatureQ();
+ const Real mu = geod.getBinormalCurvatureQ();
+
+ // printf("eh=%g es=%g, eb=%g, j=%g, tau=%g, kappa=%g, mu=%g\n",
+ // eh,es,eb,j,tau,kappa,mu);
+
+ Mat22 newJ( -(jd*es+j*(mu*eh-1)), -tau*eh,
+ -(j*tau*eh-jd*eb), 1-kappa*eh );
+
+ //cout << " J=" << J;
+ //cout << "newJ=" << newJ;
+
+ //dx = J.invert()*Fx; // Newton
+ dx = newJ.invert()*Fx;
+ } else {
+ // fixed point -- feed error back to variables
+ dx = Vec2(Fx[0]/geod.getJacobiQ(), Fx[1]);
+ }
+
+ //cout << "f=" << f << "-> dx=" << dx << endl;
+
+ fold = f;
+ xold = x;
+
+ // backtracking. Limit angle changes to around 22 degrees.
+ const Real dtheta = std::abs(dx[0]);
+ lam = 1;
+ if (dtheta > Pi/8) {
+ lam = (Pi/8)/dtheta;
+ //cout << "ORTHO: lam reduced to " << lam << "\n";
+ }
+
+ while (true) {
+ x = xold - lam*dx;
+ //cout << "at lam=" << lam << " x-lam*dx=" << x << endl;
+ // Negative length means flip direction.
+ Vec2 xadj = x;
+ bool flipped = false;
+ if (x[1] < 0) {
+ cout << "NEGATIVE LENGTH; x=" << x << "; flipping\n";
+ cout << "P=" << P << " Q=" << Q << " Q-P=" << Q-P
+ << "d=" << (Q-P).norm() << "\n";
+ cout << "tP=" << R_SP.x() <<
+ "(Q-P).tP=" << ~(Q-P)*R_SP.x() << "\n";
+ if (x[1] < -SqrtEps) {
+ xadj[0] -= Pi; xadj[1] = -xadj[1]; // flip
+ flipped = true;
+ } else xadj[1]=0; // ignore
+ }
+ Fx = calcOrthogonalGeodError(P, Q, xadj[0], xadj[1],geod);
+ if (flipped) Fx[1] = -Fx[1];
+ f = std::sqrt(~Fx*Fx);
+ dist = (geod.getPointQ()-Q).norm();
+ //cout << "step size=" << lam << " errNorm=" << f << " dist=" << dist << endl;
+ if (f > fold && lam > minlam) {
+ lam = lam / 2;
+ } else {
+ break;
+ }
+ }
+ if (f > ftol && maxabsdiff(x,xold) < xtol) {
+ std::cout << "ORTHO terminated on too-small step size after "
+ << i << " iterations" << std::endl;
+ std::cout << "err = " << Fx << std::endl;
+ break;
+ }
+
+ if (vizReporter != NULL) {
+ vizReporter->handleEvent(ptOnSurfSys->getDefaultState());
+ sleepInSec(pauseBetweenGeodIterations);
+ }
+
+ }
+ if (f > ftol)
+ std::cout << "### ORTHO geodesic DIVERGED in "
+ << MaxIterations << " iterations with err=" << f << std::endl;
+
+ // Finish each geodesic with reverse Jacobi field.
+#ifdef USE_NEW_INTEGRATOR
+ calcGeodesicReverseSensitivity2(geod, Vec2(0,1), Vec2(1,0));
+#else
+ calcGeodesicReverseSensitivity(geod, Vec2(0,1));
+#endif
+
+}
+
+
+void ContactGeometryImpl::shootGeodesicInDirectionUntilLengthReachedAnalytical(const Vec3& xP, const UnitVec3& tP,
+ const Real& terminatingLength, const GeodesicOptions& options, Geodesic& geod) const {
+ std::cout << "warning: no analytical shootGeodesic for ContactGeometry base class, computing numerically." << std::endl;
+ shootGeodesicInDirectionUntilLengthReached(xP, tP, terminatingLength, options, geod);
+}
+
+void ContactGeometryImpl::shootGeodesicInDirectionUntilPlaneHitAnalytical(const Vec3& xP, const UnitVec3& tP,
+ const Plane& terminatingPlane, const GeodesicOptions& options,
+ Geodesic& geod) const {
+ std::cout << "warning: no analytical shootGeodesic for ContactGeometry base class, computing numerically." << std::endl;
+ shootGeodesicInDirectionUntilPlaneHit(xP, tP, terminatingPlane, options, geod);
+}
+
+void ContactGeometryImpl::calcGeodesicAnalytical(const Vec3& xP, const Vec3& xQ,
+ const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const {
+ std::cout << "warning: no analytical calcGeodesic for ContactGeometry base class, computing numerically." << std::endl;
+ calcGeodesic(xP, xQ, tPhint, tQhint, geod);
+}
+
+// Calculate the "geodesic error" for thetaP and thetaQ, and return the
+// resulting (kinked) geodesic if the supplied pointer is non-null.
+Vec2 ContactGeometryImpl::
+calcSplitGeodError(const Vec3& xP, const Vec3& xQ,
+ Real thetaP, Real thetaQ,
+ Geodesic* geodesic) const
+{
+ UnitVec3 tP = calcUnitTangentVec(thetaP, R_SP);
+ UnitVec3 tQ = calcUnitTangentVec(thetaQ, R_SQ);
+ return calcSplitGeodError(xP, xQ, tP, tQ, geodesic);
+}
+
+// Calculate the "geodesic error" for tP and tQ
+Vec2 ContactGeometryImpl::
+calcSplitGeodError(const Vec3& xP, const Vec3& xQ,
+ const UnitVec3& tP, const UnitVec3& tQ,
+ Geodesic* geodesic) const
+{
+ geodP.clear();
+ geodQ.clear();
+
+ GeodesicOptions opts;
+ shootGeodesicInDirectionUntilPlaneHit(xP, tP,
+ geodHitPlaneEvent->getPlane(), opts, geodP);
+ shootGeodesicInDirectionUntilPlaneHit(xQ, tQ,
+ geodHitPlaneEvent->getPlane(), opts, geodQ);
+
+ // Finish each geodesic with reverse Jacobi field.
+ calcGeodesicReverseSensitivity(geodP,
+ geodQ.getDirectionalSensitivityPtoQ().back());
+ calcGeodesicReverseSensitivity(geodQ,
+ geodP.getDirectionalSensitivityPtoQ().back());
+
+ if (geodesic)
+ mergeGeodesics(geodP, geodQ, *geodesic);
+
+ return calcError(geodP, geodQ);
+}
+
+//------------------------------------------------------------------------------
+// CALC ORTHOGONAL GEOD ERROR
+//------------------------------------------------------------------------------
+// Calculate the "orthogonal error" for thetaP with given arc length.
+// The error is
+// err(theta,s) = [ dot(Qhat-Q, bQhat) ]
+// [ dot(Qhat-Q, tQhat) ]
+Vec2 ContactGeometryImpl::
+calcOrthogonalGeodError(const Vec3& xP, const Vec3& xQ,
+ Real thetaP, Real length,
+ Geodesic& geod) const
+{
+ const UnitVec3 tP = calcUnitTangentVec(thetaP, R_SP);
+
+ geod.clear();
+
+ GeodesicOptions opts;
+
+ Vec3 Qhat;
+ if (length < 1e-3) { // TODO
+ Qhat = xP + length*tP;
+ this->makeStraightLineGeodesic(xP, Qhat, tP, opts, geod);
+ } else {
+ shootGeodesicInDirectionUntilLengthReached(xP, tP, length, opts, geod);
+ Qhat = geod.getPointQ();
+ }
+ const Vec3 r_QQhat = Qhat - xQ;
+
+ const Real eb = ~r_QQhat * geod.getBinormalQ(); // length errors
+ const Real es = ~r_QQhat * geod.getTangentQ();
+
+ return Vec2(eb, es);
+}
+
+// Calculate the "geodesic error" for tP and tQ
+Vec2 ContactGeometryImpl::
+calcSplitGeodErrorAnalytical(const Vec3& xP, const Vec3& xQ,
+ const UnitVec3& tP, const UnitVec3& tQ,
+ Geodesic* geodesic) const
+{
+ geodP.clear();
+ geodQ.clear();
+
+ GeodesicOptions opts;
+ shootGeodesicInDirectionUntilPlaneHitAnalytical(xP, tP,
+ geodHitPlaneEvent->getPlane(), opts, geodP);
+ shootGeodesicInDirectionUntilPlaneHitAnalytical(xQ, tQ,
+ geodHitPlaneEvent->getPlane(), opts, geodQ);
+
+ // Finish each geodesic with reverse Jacobi field.
+ calcGeodesicReverseSensitivity(geodP,
+ geodQ.getDirectionalSensitivityPtoQ().back());
+ calcGeodesicReverseSensitivity(geodQ,
+ geodP.getDirectionalSensitivityPtoQ().back());
+
+ if (geodesic)
+ mergeGeodesics(geodP, geodQ, *geodesic);
+
+ return calcError(geodP, geodQ);
+}
+
+
+
+// Calculate the "geodesic jacobian" by numerical perturbation
+Mat22 ContactGeometryImpl::
+calcSplitGeodErrorJacobian(const Vec3& xP, const Vec3& xQ,
+ const Real& thetaP, const Real& thetaQ, Differentiator::Method order) const {
+
+// UnitVec3 tP = calcUnitTangentVecGG(thetaP, R_SP);
+// UnitVec3 tQ = calcUnitTangentVecGG(thetaQ, R_SQ);
+//
+// geodP.clear();
+// geodQ.clear();
+//
+// GeodesicOptions opts;
+// shootGeodesicInDirectionUntilPlaneHit(xP, tP,
+// geodHitPlaneEvent->getPlane(), opts, geodP);
+// shootGeodesicInDirectionUntilPlaneHit(xQ, tQ,
+// geodHitPlaneEvent->getPlane(), opts, geodQ);
+
+ GeodesicOptions opts;
+
+ UnitVec3 tP, tQ;
+
+ Vec2 fy0 = calcError(geodP, geodQ);
+
+ Mat22 dfdy;
+ Vec2 fyptmp, fymtmp;
+ Real hEst, h, accFactor, YMin;
+
+ Geodesic geodPtmp;
+ Geodesic geodQtmp;
+
+ if (order == 1) {
+ accFactor = std::sqrt(estimatedGeodesicAccuracy);
+ } else {
+ accFactor = std::pow(estimatedGeodesicAccuracy, OneThird);
+ }
+ YMin = Real(0.1);
+
+ Vec2 y0(thetaP, thetaQ);
+
+ // perturb thetaP
+ hEst = accFactor*std::max(std::abs(thetaP), YMin);
+ h = cleanUpH(hEst, thetaP);
+
+ // positive perturb
+ tP = calcUnitTangentVec(thetaP+h, R_SP);
+ shootGeodesicInDirectionUntilPlaneHit(xP, tP,
+ geodHitPlaneEvent->getPlane(), opts, geodPtmp);
+ fyptmp = calcError(geodPtmp, geodQ);
+
+ if (order==1) {
+ dfdy(0) = (fyptmp-fy0)/h;
+ } else {
+
+ geodPtmp.clear();
+ tP = calcUnitTangentVec(thetaP-h, R_SP);
+ shootGeodesicInDirectionUntilPlaneHit(xP, tP,
+ geodHitPlaneEvent->getPlane(), opts, geodPtmp);
+ fymtmp = calcError(geodPtmp, geodQ);
+
+ dfdy(0) = (fyptmp-fymtmp)/(2*h);
+ }
+
+ // perturb thetaQ
+ hEst = accFactor*std::max(std::abs(thetaQ), YMin);
+ h = cleanUpH(hEst, thetaP);
+
+ // positive perturb
+ tQ = calcUnitTangentVec(thetaQ+h, R_SQ);
+ shootGeodesicInDirectionUntilPlaneHit(xQ, tQ,
+ geodHitPlaneEvent->getPlane(), opts, geodQtmp);
+ fyptmp = calcError(geodP, geodQtmp);
+
+ if (order==1) {
+ dfdy(1) = (fyptmp-fy0)/h;
+ } else {
+
+ geodQtmp.clear();
+ tQ = calcUnitTangentVec(thetaQ-h, R_SQ);
+ shootGeodesicInDirectionUntilPlaneHit(xQ, tQ,
+ geodHitPlaneEvent->getPlane(), opts, geodQtmp);
+ fymtmp = calcError(geodP, geodQtmp);
+
+ dfdy(1) = (fyptmp-fymtmp)/(2*h);
+ }
+
+ return dfdy;
+}
+
+Vec2 ContactGeometryImpl::
+calcError(const Geodesic& geodP, const Geodesic& geodQ) const {
+ const Transform& Fphat = geodP.getFrenetFrames().back();
+ const Transform& Fqhat = geodQ.getFrenetFrames().back();
+ const Vec3& Phat = Fphat.p(); const Vec3& Qhat = Fqhat.p();
+ const UnitVec3& tPhat = Fphat.x(); const UnitVec3& tQhat = Fqhat.x();
+ const UnitVec3& bPhat = Fphat.y(); const UnitVec3& bQhat = Fqhat.y();
+ const UnitVec3& nPhat = Fphat.z(); const UnitVec3& nQhat = Fqhat.z();
+
+ // Error is separation distance along mutual b direction, and angle by
+ // which the curves fail to connect smoothly.
+ Vec2 geodErr(~(bPhat - bQhat) * (Phat - Qhat), ~bPhat * tQhat);
+ return geodErr;
+}
+
diff --git a/SimTKmath/Geometry/src/ContactGeometryImpl.h b/SimTKmath/Geometry/src/ContactGeometryImpl.h
new file mode 100644
index 0000000..e813d14
--- /dev/null
+++ b/SimTKmath/Geometry/src/ContactGeometryImpl.h
@@ -0,0 +1,1146 @@
+#ifndef SimTK_SIMMATH_CONTACT_GEOMETRY_IMPL_H_
+#define SimTK_SIMMATH_CONTACT_GEOMETRY_IMPL_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_Sphere.h"
+#include "simmath/internal/OBBTree.h"
+#include "simmath/internal/ParticleConSurfaceSystem.h"
+#include "simmath/Differentiator.h"
+#include "simmath/internal/ContactGeometry.h"
+
+#include <limits>
+
+namespace SimTK {
+
+class SplitGeodesicError;
+
+
+//==============================================================================
+// CONTACT GEOMETRY IMPL
+//==============================================================================
+class SimTK_SIMMATH_EXPORT ContactGeometryImpl {
+public:
+ ContactGeometryImpl()
+ : myHandle(0), ptOnSurfSys(0), geodHitPlaneEvent(0), vizReporter(0),
+ splitGeodErr(0), numGeodesicsShot(0)
+ {
+ createParticleOnSurfaceSystem();
+ }
+ ContactGeometryImpl(const ContactGeometryImpl& source)
+ : myHandle(0), ptOnSurfSys(0), geodHitPlaneEvent(0), vizReporter(0),
+ splitGeodErr(0), numGeodesicsShot(0)
+ {}
+
+ virtual ~ContactGeometryImpl() {
+ clearMyHandle();
+ clearParticleOnSurfaceSystem();
+ }
+
+ /* Create a new ContactGeometryTypeId and return this unique integer
+ (thread safe). Each distinct type of ContactGeometry should use this to
+ initialize a static variable for that concrete class. */
+ static ContactGeometryTypeId createNewContactGeometryTypeId()
+ { static AtomicInteger nextAvailableId = 1;
+ return ContactGeometryTypeId(nextAvailableId++); }
+
+ virtual ContactGeometryImpl* clone() const = 0;
+ virtual ContactGeometryTypeId getTypeId() const = 0;
+
+ virtual DecorativeGeometry createDecorativeGeometry() const {
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "ContactGeometryImpl", "createDecorativeGeometry()");
+ }
+
+ virtual Vec3 findNearestPoint(const Vec3& position, bool& inside,
+ UnitVec3& normal) const = 0;
+
+ virtual bool intersectsRay(const Vec3& origin, const UnitVec3& direction,
+ Real& distance, UnitVec3& normal) const = 0;
+
+ virtual void getBoundingSphere(Vec3& center, Real& radius) const = 0;
+
+
+ virtual bool isSmooth() const = 0;
+ virtual bool isConvex() const = 0;
+ virtual bool isFinite() const = 0;
+
+ // Smooth surfaces only.
+ virtual void calcCurvature(const Vec3& point, Vec2& curvature,
+ Rotation& orientation) const
+ { SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "ContactGeometryImpl", "calcCurvature()"); }
+
+ // Smooth surfaces only.
+ virtual const Function& getImplicitFunction() const
+ { SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "ContactGeometryImpl", "getImplicitFunction()"); }
+
+ // Convex surfaces only.
+ virtual Vec3 calcSupportPoint(UnitVec3 direction) const
+ { SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "ContactGeometryImpl", "calcSupportPoint()"); }
+
+ const OBBTree& getOBBTree() const {return obbTree;}
+
+
+ Real calcSurfaceValue(const Vec3& point) const;
+ UnitVec3 calcSurfaceUnitNormal(const Vec3& point) const;
+ Vec3 calcSurfaceGradient(const Vec3& point) const;
+ Mat33 calcSurfaceHessian(const Vec3& point) const;
+ Real calcGaussianCurvature(const Vec3& gradient,
+ const Mat33& Hessian) const;
+ Real calcSurfaceCurvatureInDirection(const Vec3& point, const UnitVec3& direction) const;
+
+ Vec3 projectDownhillToNearestPoint(const Vec3& Q) const;
+
+ bool trackSeparationFromLine(const Vec3& pointOnLine,
+ const UnitVec3& directionOfLine,
+ const Vec3& startingGuessForClosestPoint,
+ Vec3& newClosestPointOnSurface,
+ Vec3& closestPointOnLine,
+ Real& height) const;
+
+ // Geodesic evaluators
+
+
+ // Given two points, find a geodesic curve connecting them.
+ void initGeodesic(const Vec3& xP, const Vec3& xQ, const Vec3& xSP,
+ const GeodesicOptions& options, Geodesic& geod) const;
+
+
+ // Given two points and previous geodesic curve close to the points, find
+ // a geodesic curve connecting the points that is close to the previous geodesic.
+ void continueGeodesic(const Vec3& xP, const Vec3& xQ, const Geodesic& prevGeod,
+ const GeodesicOptions& options, Geodesic& geod) const;
+
+ // Given two points (which should be close together) create a two-point
+ // geodesic that is a straight line between the points.
+ void makeStraightLineGeodesic(const Vec3& xP, const Vec3& xQ,
+ const UnitVec3& defaultDirectionIfNeeded,
+ const GeodesicOptions& options, Geodesic& geod) const;
+
+ // Compute a geodesic curve starting at the given point, starting in the
+ // given direction, and terminating at the given length.
+ void shootGeodesicInDirectionUntilLengthReached(const Vec3& xP, const UnitVec3& tP,
+ const Real& terminatingLength, const GeodesicOptions& options, Geodesic& geod) const;
+
+ // Compute a geodesic curve starting at the given point, starting in the
+ // given direction, and terminating when it hits the given plane.
+ void shootGeodesicInDirectionUntilPlaneHit(const Vec3& xP, const UnitVec3& tP,
+ const Plane& terminatingPlane, const GeodesicOptions& options,
+ Geodesic& geod) const;
+
+ // Utility method to find geodesic between P and Q with initial shooting
+ // directions tPhint and tQhint
+ void calcGeodesic(const Vec3& xP, const Vec3& xQ,
+ const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const;
+
+ // Use a method that generates orthogonal error conditions in the tangent
+ // and binormal directions.
+ void calcGeodesicUsingOrthogonalMethod
+ (const Vec3& xP, const Vec3& xQ,
+ const Vec3& tPhint, Real lengthHint, Geodesic& geod) const;
+
+ // Utility method to calculate the "geodesic error" between one geodesic
+ // shot from P in the direction tP and another geodesic shot from Q in the
+ // direction tQ.
+ Vec2 calcSplitGeodError(const Vec3& xP, const Vec3& xQ,
+ const UnitVec3& tP, const UnitVec3& tQ,
+ Geodesic* geod=0) const;
+
+ // analytical versions of the geodesic API methods
+
+ virtual void shootGeodesicInDirectionUntilLengthReachedAnalytical(const Vec3& xP, const UnitVec3& tP,
+ const Real& terminatingLength, const GeodesicOptions& options, Geodesic& geod) const;
+
+ virtual void shootGeodesicInDirectionUntilPlaneHitAnalytical(const Vec3& xP, const UnitVec3& tP,
+ const Plane& terminatingPlane, const GeodesicOptions& options,
+ Geodesic& geod) const;
+
+ virtual void calcGeodesicAnalytical(const Vec3& xP, const Vec3& xQ,
+ const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const;
+
+ Vec2 calcSplitGeodErrorAnalytical(const Vec3& P, const Vec3& Q,
+ const UnitVec3& tP, const UnitVec3& tQ,
+ Geodesic* geod=0) const;
+
+
+ // Utility method to calculate the "geodesic error" between the end-points
+ // of two geodesics.
+ Vec2 calcError(const Geodesic& geodP, const Geodesic& geodQ) const;
+
+
+ // Calculate a unit tangent vector based on angle theta measured from the
+ // binormal axis of the given rotation matrix
+ static UnitVec3 calcUnitTangentVec(const Real& theta, const Rotation& R_GS) {
+ UnitVec3 tR_S(std::sin(theta), std::cos(theta), 0);
+ return R_GS*tR_S;
+ }
+
+ // Temporary utility method for creating a continuous geodesic object
+ // from the two "split geodesics"
+ // XXX to be replaced by actual copy / append functionalily in Geodesic.h
+ static void mergeGeodesics(const Geodesic& geodP, const Geodesic& geodQ,
+ Geodesic& geod) {
+
+ mergeGeodFrames(geodP.getFrenetFrames(), geodQ.getFrenetFrames(),
+ geod.updFrenetFrames());
+ mergeArcLengths(geodP.getArcLengths(), geodQ.getArcLengths(),
+ geod.updArcLengths());
+ mergeJacobiField(geodP.getDirectionalSensitivityPtoQ(),
+ geodQ.getDirectionalSensitivityQtoP(),
+ geod.updDirectionalSensitivityPtoQ());
+ mergeJacobiField(geodP.getDirectionalSensitivityQtoP(),
+ geodQ.getDirectionalSensitivityPtoQ(),
+ geod.updDirectionalSensitivityQtoP());
+ // TODO: calculate QtoP
+ geod.setIsConvex(geodP.isConvex() && geodQ.isConvex());
+ // TODO: what to do with "is shortest"?
+ // Take the smaller of the two suggested step sizes.
+ geod.setInitialStepSizeHint(std::min(geodP.getInitialStepSizeHint(),
+ geodQ.getInitialStepSizeHint()));
+ // Take the larger (sloppier) of the two accuracies.
+ geod.setAchievedAccuracy(std::max(geodP.getAchievedAccuracy(),
+ geodQ.getAchievedAccuracy()));
+
+ }
+
+ // Temporary utility method for joining geodP and the reverse of geodQ.
+ // Optionally negate the Q vector (used for tangents). The last entries
+ // for both geodesics are supposed to represent the same point on the
+ // combined geodesic so we'll average them. The final geodesic will
+ // thus have one fewer point than the sum of the two half-geodesics.
+ // Note convention for Transform<->Frenet Frame mapping:
+ // z = normal, y = tangent, x = binormal (tXn)
+ static void mergeGeodFrames(const Array_<Transform>& geodP,
+ const Array_<Transform>& geodQ,
+ Array_<Transform>& geod)
+ {
+ const int sizeP = geodP.size(), sizeQ = geodQ.size();
+ assert(sizeP > 0 && sizeQ > 0);
+ geod.clear();
+ geod.reserve(sizeP+sizeQ-1);
+ geod = geodP; // the first part is the same as P
+
+ const Transform& Pf = geodP.back();
+ const Transform& Qf = geodQ.back();
+ // Recalculate the midpoint by averaging the ends.
+ Vec3 midpoint = (Pf.p() + Qf.p())/2;
+ // TODO: get exact normal at midpoint from surface
+ UnitVec3 midnormal = UnitVec3((Pf.z() + Qf.z())/2);
+ Vec3 midtangent = (Pf.y() - Qf.y())/2; // approx
+
+ // Replace the midpoint Frenet frame with one at the average of
+ // the P and Q endpoints, using the calculated normal as the z axis,
+ // and the average tangent as y axis. We'll let Rotation
+ // perpendicularize this, so the actual y axis may deviate slightly
+ // from the calculated average normal. Then b=y X z is the x axis.
+ geod.back() = Transform(Rotation(midnormal, ZAxis, midtangent, YAxis),
+ midpoint);
+
+ // Now run through the Q-side geodesic backwards reversing its
+ // tangent (y) and binormal (x) vectors while preserving the point
+ // and surface normal. That's a 180 degree rotation about z so it
+ // is still right handed!
+ for (int i = sizeQ-2; i >= 0; --i) {
+ const Transform& F = geodQ[i]; // old Frenet frame
+ Transform Frev; // Same, but with x,y directions negated
+ Frev.updR().setRotationFromUnitVecsTrustMe(-F.x(), -F.y(), F.z());
+ Frev.updP() = F.p();
+ geod.push_back(Frev);
+ }
+ }
+
+ // Temporary utility for merging the arc lengths (knot coordinates) for
+ // two half-geodesics. The last entry in each list is supposed to be the
+ // same point.
+ static void mergeArcLengths(const Array_<Real>& knotsP,
+ const Array_<Real>& knotsQ,
+ Array_<Real>& knots)
+ {
+ const int sizeP = knotsP.size(), sizeQ = knotsQ.size();
+ assert(sizeP > 0 && sizeQ > 0);
+ knots.clear();
+ knots.reserve(sizeP+sizeQ-1);
+ knots = knotsP; // the first part is the same as P
+
+ const Real lengthP = knotsP.back(), lengthQ = knotsQ.back();
+ for (int i = sizeQ-2; i >= 0; --i)
+ knots.push_back(lengthP + (lengthQ-knotsQ[i]));
+ }
+
+ static void mergeJacobiField(const Array_<Vec2>& jP,
+ const Array_<Vec2>& jQ,
+ Array_<Vec2>& j)
+ {
+ const int sizeP = jP.size(), sizeQ = jQ.size();
+ assert(sizeP > 0 && sizeQ > 0);
+ j.clear();
+ j.reserve(sizeP+sizeQ-1);
+ j = jP; // the first part is the same as P
+ //TODO: this is the wrong-direction field!
+ for (int i = sizeQ-2; i >= 0; --i)
+ j.push_back(jQ[i]);
+ }
+
+
+ // Utility method to used by calcGeodesicInDirectionUntilPlaneHit
+ // and calcGeodesicInDirectionUntilLengthReached
+ void shootGeodesicInDirection(const Vec3& P, const UnitVec3& tP,
+ const Real& finalTime, const GeodesicOptions& options,
+ Geodesic& geod) const;
+ void shootGeodesicInDirection2(const Vec3& P, const UnitVec3& tP,
+ const Real& finalTime, const GeodesicOptions& options,
+ Geodesic& geod) const;
+
+ // Utility method to integrate geodesic backwards to fill in the Q to P
+ // directional sensitivity.
+ void calcGeodesicReverseSensitivity
+ (Geodesic& geod, const Vec2& initJacobi) const;
+ void calcGeodesicReverseSensitivity2
+ (Geodesic& geod, const Vec2& initJRot, const Vec2& initJTrans) const;
+
+ // Utility method to calculate the "geodesic error" between one geodesic
+ // shot from P in the direction thetaP and another geodesic shot from Q in the
+ // direction thetaQ given the pre-calculated member basis R_SP, R_SQ.
+ // We optionally return the resulting "kinked" geodesic, which is the real
+ // one if the returned errors are below tolerance.
+ Vec2 calcSplitGeodError(const Vec3& xP, const Vec3& xQ,
+ Real thetaP, Real thetaQ,
+ Geodesic* geodesic=0) const;
+
+ // Utility method to calculate the "orthogonal error" between a geodesic
+ // shot from P in the direction thetaP with given length and a point Q.
+ // We are actually shooting the geodesic here and we'll return it in
+ // the indicated variable. Note that we *do not* do the backwards
+ // integration for the reverse directional sensitivity; if you want to
+ // return this as the final geodesic be sure to fill in that term.
+ Vec2 calcOrthogonalGeodError(const Vec3& xP, const Vec3& xQ,
+ Real thetaP, Real length,
+ Geodesic& geodesic) const;
+
+ // Utility method to calculate the "geodesic error jacobian" between one geodesic
+ // shot from P in the direction thetaP and another geodesic shot from Q in the
+ // direction thetaQ given the pre-calculated member basis R_SP, R_SQ
+ Mat22 calcSplitGeodErrorJacobian(const Vec3& P, const Vec3& Q,
+ const Real& thetaP, const Real& thetaQ, Differentiator::Method method) const;
+
+ // Compute rotation matrix using the normal at the given point and the
+ // given direction.
+ Rotation calcTangentBasis(const Vec3& point, const Vec3& dir) const {
+ const UnitVec3 n = calcSurfaceUnitNormal(point);
+ Rotation R_GS;
+ R_GS.setRotationFromTwoAxes(n, ZAxis, dir, XAxis);
+ return R_GS;
+ }
+
+ // Get the plane associated with the geodesic hit plane event handler
+ const Plane& getPlane() const {
+ return geodHitPlaneEvent->getPlane();
+ }
+
+ // Set the plane associated with the geodesic hit plane event handler
+ void setPlane(const Plane& plane) const {
+ return geodHitPlaneEvent->setPlane(plane);
+ }
+
+
+ // Get the geodesic for access by visualizer
+ const Geodesic& getGeodP() const {
+ return geodP;
+ }
+
+ // Get the geodesic for access by visualizer
+ const Geodesic& getGeodQ() const {
+ return geodQ;
+ }
+
+ const int getNumGeodesicsShot() const {
+ return numGeodesicsShot;
+ }
+
+ void addVizReporter(ScheduledEventReporter* reporter) const {
+ vizReporter = reporter;
+ ptOnSurfSys->addEventReporter(vizReporter); // takes ownership
+ ptOnSurfSys->realizeTopology();
+ }
+
+ class SplitGeodesicError; // local class
+ friend class SplitGeodesicError;
+ class OrthoGeodesicError; // local class
+ friend class OrthoGeodesicError;
+
+ void createParticleOnSurfaceSystem() {
+ ptOnSurfSys = new ParticleConSurfaceSystem(*this);
+ geodHitPlaneEvent = new GeodHitPlaneEvent();
+ ptOnSurfSys->addEventHandler(geodHitPlaneEvent); // takes ownership
+ ptOnSurfSys->realizeTopology();
+ }
+
+ void clearParticleOnSurfaceSystem() {
+ delete ptOnSurfSys;
+ ptOnSurfSys = 0;
+ geodHitPlaneEvent = 0; // was deleted by the system
+ }
+
+ ContactGeometry* getMyHandle() {return myHandle;}
+ void setMyHandle(ContactGeometry& h) {myHandle = &h;}
+ void clearMyHandle() {myHandle = 0;}
+protected:
+ ContactGeometry* myHandle;
+ OBBTree obbTree;
+
+ ParticleConSurfaceSystem* ptOnSurfSys;
+ GeodHitPlaneEvent* geodHitPlaneEvent; // don't delete this
+ mutable ScheduledEventReporter* vizReporter; // don't delete this
+ mutable SplitGeodesicError* splitGeodErr;
+
+ // temporary objects
+ mutable Geodesic geodP;
+ mutable Geodesic geodQ;
+ mutable Rotation R_SP;
+ mutable Rotation R_SQ;
+ mutable int numGeodesicsShot;
+
+};
+
+
+
+//==============================================================================
+// HALF SPACE IMPL
+//==============================================================================
+class HalfSpaceImplicitFunction : public Function {
+public:
+ HalfSpaceImplicitFunction() : ownerp(0) {}
+ HalfSpaceImplicitFunction(const ContactGeometry::HalfSpace::Impl& owner)
+ : ownerp(&owner) {}
+ void setOwner(const ContactGeometry::HalfSpace::Impl& owner) {ownerp=&owner;}
+
+ // Value is positive for x>0.
+ Real calcValue(const Vector& P) const {return P[0];}
+ // First derivative w.r.t. x is 1, all else is zero.
+ Real calcDerivative(const Array_<int>& components,
+ const Vector& P) const
+ { if (components.empty()) return calcValue(P);
+ if (components.size()==1 && components[0]==0) return 1;
+ return 0; }
+
+ int getArgumentSize() const {return 3;}
+ int getMaxDerivativeOrder() const
+ { return std::numeric_limits<int>::max(); }
+private:
+ const ContactGeometry::HalfSpace::Impl* ownerp; // just a ref.; don't delete
+};
+
+
+class ContactGeometry::HalfSpace::Impl : public ContactGeometryImpl {
+public:
+ Impl() : ContactGeometryImpl() {
+ }
+ ContactGeometryImpl* clone() const {
+ return new Impl();
+ }
+
+ ContactGeometryTypeId getTypeId() const {return classTypeId();}
+
+ DecorativeGeometry createDecorativeGeometry() const;
+ Vec3 findNearestPoint(const Vec3& position, bool& inside,
+ UnitVec3& normal) const;
+ bool intersectsRay(const Vec3& origin, const UnitVec3& direction,
+ Real& distance, UnitVec3& normal) const;
+ void getBoundingSphere(Vec3& center, Real& radius) const;
+
+ bool isSmooth() const {return true;}
+ bool isConvex() const {return false;}
+ bool isFinite() const {return false;}
+
+ // Curvature is zero everywhere. Since the half plane occupies x>0 in
+ // its own frame, the surface normal is -x, and -x,y,-z forms a right
+ // handed set.
+ void calcCurvature(const Vec3& point, Vec2& curvature,
+ Rotation& orientation) const
+ { curvature = 0;
+ orientation.setRotationFromUnitVecsTrustMe
+ (UnitVec3(-XAxis), UnitVec3(YAxis), UnitVec3(-ZAxis));
+ }
+
+ const Function& getImplicitFunction() const {return function;}
+
+ static ContactGeometryTypeId classTypeId() {
+ static const ContactGeometryTypeId id =
+ createNewContactGeometryTypeId();
+ return id;
+ }
+private:
+ HalfSpaceImplicitFunction function;
+};
+
+
+
+//==============================================================================
+// CYLINDER IMPL
+//==============================================================================
+class CylinderImplicitFunction : public Function {
+public:
+ CylinderImplicitFunction() : ownerp(0) {}
+ CylinderImplicitFunction(const ContactGeometry::Cylinder::Impl& owner)
+ : ownerp(&owner) {}
+ void setOwner(const ContactGeometry::Cylinder::Impl& owner) {ownerp=&owner;}
+ Real calcValue(const Vector& x) const;
+ Real calcDerivative(const Array_<int>& derivComponents,
+ const Vector& x) const;
+ int getArgumentSize() const {return 3;}
+ int getMaxDerivativeOrder() const
+ { return std::numeric_limits<int>::max(); }
+private:
+ const ContactGeometry::Cylinder::Impl* ownerp; // just a reference; don't delete
+};
+
+class ContactGeometry::Cylinder::Impl : public ContactGeometryImpl {
+public:
+ explicit Impl(Real radius) : radius(radius) {
+ function.setOwner(*this);
+ }
+
+ ContactGeometryImpl* clone() const {
+ return new Impl(radius);
+ }
+ Real getRadius() const {
+ return radius;
+ }
+ void setRadius(Real r) {
+ radius = r;
+ }
+
+ ContactGeometryTypeId getTypeId() const {return classTypeId();}
+
+ DecorativeGeometry createDecorativeGeometry() const;
+ Vec3 findNearestPoint(const Vec3& position, bool& inside,
+ UnitVec3& normal) const;
+ bool intersectsRay(const Vec3& origin, const UnitVec3& direction,
+ Real& distance, UnitVec3& normal) const;
+ void getBoundingSphere(Vec3& center, Real& radius) const;
+
+ bool isSmooth() const {return true;}
+ bool isConvex() const {return true;}
+ bool isFinite() const {return false;}
+
+ Vec3 calcSupportPoint(UnitVec3 direction) const {
+ assert(false);
+ return Vec3(NaN);
+ }
+
+ void calcCurvature(const Vec3& point, Vec2& curvature,
+ Rotation& orientation) const;
+
+ virtual void shootGeodesicInDirectionUntilLengthReachedAnalytical(const Vec3& xP, const UnitVec3& tP,
+ const Real& terminatingLength, const GeodesicOptions& options, Geodesic& geod) const;
+
+ virtual void shootGeodesicInDirectionUntilPlaneHitAnalytical(const Vec3& xP, const UnitVec3& tP,
+ const Plane& terminatingPlane, const GeodesicOptions& options,
+ Geodesic& geod) const;
+
+ virtual void calcGeodesicAnalytical(const Vec3& xP, const Vec3& xQ,
+ const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const;
+
+ const Function& getImplicitFunction() const {
+ return function;
+ }
+
+ static ContactGeometryTypeId classTypeId() {
+ static const ContactGeometryTypeId id =
+ createNewContactGeometryTypeId();
+ return id;
+ }
+private:
+ Real radius;
+ CylinderImplicitFunction function;
+};
+
+
+
+//==============================================================================
+// SPHERE IMPL
+//==============================================================================
+class SphereImplicitFunction : public Function {
+public:
+ SphereImplicitFunction() : ownerp(0) {}
+ SphereImplicitFunction(const ContactGeometry::Sphere::Impl& owner)
+ : ownerp(&owner) {}
+ void setOwner(const ContactGeometry::Sphere::Impl& owner) {ownerp=&owner;}
+ Real calcValue(const Vector& x) const;
+ Real calcDerivative(const Array_<int>& derivComponents,
+ const Vector& x) const;
+ int getArgumentSize() const {return 3;}
+ int getMaxDerivativeOrder() const
+ { return std::numeric_limits<int>::max(); }
+private:
+ const ContactGeometry::Sphere::Impl* ownerp; // just a reference; don't delete
+};
+
+class ContactGeometry::Sphere::Impl : public ContactGeometryImpl {
+public:
+ explicit Impl(Real radius) : radius(radius) {
+ function.setOwner(*this);
+ createOBBTree();
+ }
+
+ ContactGeometryImpl* clone() const {
+ return new Impl(radius);
+ }
+ Real getRadius() const {
+ return radius;
+ }
+ void setRadius(Real r) {
+ radius = r;
+ }
+
+ ContactGeometryTypeId getTypeId() const {return classTypeId();}
+
+ DecorativeGeometry createDecorativeGeometry() const;
+ Vec3 findNearestPoint(const Vec3& position, bool& inside,
+ UnitVec3& normal) const;
+ bool intersectsRay(const Vec3& origin, const UnitVec3& direction,
+ Real& distance, UnitVec3& normal) const;
+ void getBoundingSphere(Vec3& center, Real& radius) const;
+
+ bool isSmooth() const {return true;}
+ bool isConvex() const {return true;}
+ bool isFinite() const {return true;}
+
+ Vec3 calcSupportPoint(UnitVec3 direction) const {
+ return radius*direction;
+ }
+ void calcCurvature(const Vec3& point, Vec2& curvature,
+ Rotation& orientation) const;
+
+ virtual void shootGeodesicInDirectionUntilLengthReachedAnalytical(const Vec3& xP, const UnitVec3& tP,
+ const Real& terminatingLength, const GeodesicOptions& options, Geodesic& geod) const;
+
+ virtual void shootGeodesicInDirectionUntilPlaneHitAnalytical(const Vec3& xP, const UnitVec3& tP,
+ const Plane& terminatingPlane, const GeodesicOptions& options,
+ Geodesic& geod) const;
+
+ virtual void calcGeodesicAnalytical(const Vec3& xP, const Vec3& xQ,
+ const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const;
+
+ const Function& getImplicitFunction() const {
+ return function;
+ }
+
+ static ContactGeometryTypeId classTypeId() {
+ static const ContactGeometryTypeId id =
+ createNewContactGeometryTypeId();
+ return id;
+ }
+private:
+ void createOBBTree();
+
+ Real radius;
+ SphereImplicitFunction function;
+};
+
+
+
+//==============================================================================
+// ELLIPSOID IMPL
+//==============================================================================
+class EllipsoidImplicitFunction : public Function {
+public:
+ EllipsoidImplicitFunction() : ownerp(0) {}
+ EllipsoidImplicitFunction(const ContactGeometry::Ellipsoid::Impl& owner)
+ : ownerp(&owner) {}
+ void setOwner(const ContactGeometry::Ellipsoid::Impl& owner) {ownerp=&owner;}
+ Real calcValue(const Vector& x) const;
+ Real calcDerivative(const Array_<int>& derivComponents,
+ const Vector& x) const;
+ int getArgumentSize() const {return 3;}
+ int getMaxDerivativeOrder() const
+ { return std::numeric_limits<int>::max(); }
+private:
+ const ContactGeometry::Ellipsoid::Impl* ownerp;// just a ref.; don't delete
+};
+
+class ContactGeometry::Ellipsoid::Impl : public ContactGeometryImpl {
+public:
+ explicit Impl(const Vec3& radii)
+ : radii(radii),
+ curvatures(Vec3(1/radii[0],1/radii[1],1/radii[2]))
+ { function.setOwner(*this);
+ createOBBTree(); }
+
+ ContactGeometryImpl* clone() const {return new Impl(radii);}
+ const Vec3& getRadii() const {return radii;}
+ void setRadii(const Vec3& r)
+ { radii = r; curvatures = Vec3(1/r[0],1/r[1],1/r[2]); }
+
+ const Vec3& getCurvatures() const {return curvatures;}
+
+ // See below.
+ inline Vec3 findPointWithThisUnitNormal(const UnitVec3& n) const;
+ inline Vec3 findPointInSameDirection(const Vec3& Q) const;
+ inline UnitVec3 findUnitNormalAtPoint(const Vec3& Q) const;
+
+ // Cost is findParaboloidAtPointWithNormal + about 40 flops.
+ void findParaboloidAtPoint(const Vec3& Q, Transform& X_EP, Vec2& k) const
+ { findParaboloidAtPointWithNormal(Q,findUnitNormalAtPoint(Q),X_EP,k); }
+
+ void findParaboloidAtPointWithNormal(const Vec3& Q, const UnitVec3& n,
+ Transform& X_EP, Vec2& k) const;
+
+ ContactGeometryTypeId getTypeId() const {return classTypeId();}
+
+ DecorativeGeometry createDecorativeGeometry() const;
+ Vec3 findNearestPoint(const Vec3& position, bool& inside,
+ UnitVec3& normal) const;
+ bool intersectsRay(const Vec3& origin, const UnitVec3& direction,
+ Real& distance, UnitVec3& normal) const;
+ void getBoundingSphere(Vec3& center, Real& radius) const;
+
+ bool isSmooth() const {return true;}
+ bool isConvex() const {return true;}
+ bool isFinite() const {return true;}
+
+ // The point furthest in this direction is the unique point whose outward
+ // normal is this direction.
+ Vec3 calcSupportPoint(UnitVec3 direction) const {
+ return findPointWithThisUnitNormal(direction);
+ }
+ void calcCurvature(const Vec3& point, Vec2& curvature,
+ Rotation& orientation) const;
+ const Function& getImplicitFunction() const {
+ return function;
+ }
+
+ static ContactGeometryTypeId classTypeId() {
+ static const ContactGeometryTypeId id =
+ createNewContactGeometryTypeId();
+ return id;
+ }
+private:
+ void createOBBTree();
+
+
+ Vec3 radii;
+ // The curvatures are calculated whenever the radii are set.
+ Vec3 curvatures; // (1/radii[0], 1/radii[1], 1/radii[2])
+ EllipsoidImplicitFunction function;
+};
+
+// Given an ellipsoid and a unit normal direction, find the unique point on the
+// ellipsoid whose outward normal matches. The unnormalized normal at a point
+// p=[x y z] is the gradient of the ellipsoid's implicit equation there:
+// (1) n(p) = grad(f(p)) = 2*[x/a^2, y/b^2, z/c^2]
+// If we had that, we'd have
+// (2) p=[n[0]*a^2, n[1]*b^2, n[2]*c^2]/2,
+// but instead we're given the normalized normal that has been divided
+// by the length of n:
+// (3) nn = n/|n| = s * [x/a^2, y/b^2, z/c^2]
+// where s = 2/|n|.
+//
+// We can solve for s using the fact that x,y,z must lie on the ellipsoid so
+// |x/a,y/b,z/c|=1. Construct the vector
+// v = [nn[0]*a, nn[1]*b, nn[2]*c] = s*[x/a, y/b, z/c]
+// Now we have |v|=s. So n/2 = nn/|v| and we can use equation (2) to solve
+// for p. Cost is about 40 flops.
+inline Vec3 ContactGeometry::Ellipsoid::Impl::
+findPointWithThisUnitNormal(const UnitVec3& nn) const {
+ const Real& a=radii[0]; const Real& b=radii[1]; const Real& c=radii[2];
+ const Vec3 v = Vec3(nn[0]*a, nn[1]*b, nn[2]*c);
+ const Vec3 p = Vec3( v[0]*a, v[1]*b, v[2]*c) / v.norm();
+ return p;
+}
+
+// Given a point Q=(x,y,z) measured from ellipse center O, find the intersection
+// of the ray d=Q-O with the ellipse surface. This just requires scaling the
+// direction vector d by a factor s so that f(s*d)=0, that is,
+// s*|x/a y/b z/c|=1 => s = 1/|x/a y/b z/c|
+// Cost is about 40 flops.
+inline Vec3 ContactGeometry::Ellipsoid::Impl::
+findPointInSameDirection(const Vec3& Q) const {
+ Real s = 1/Vec3(Q[0]*curvatures[0],
+ Q[1]*curvatures[1],
+ Q[2]*curvatures[2]).norm();
+ return s*Q;
+}
+
+// The implicit equation of the ellipsoid surface is f(x,y,z)=0 where
+// f(x,y,z) = (ka x)^2 + (kb y)^2 (kc z)^2 - 1. Points p inside the ellipsoid
+// have f(p)<0, outside f(p)>0. f defines a field in space; its positive
+// gradient [df/dx df/dy df/dz] points outward. So, given an ellipsoid with
+// principal curvatures ka,kb,kc and a point Q allegedly on the ellipsoid, the
+// outward normal (unnormalized) n at that point is
+// n(p) = grad(f(p)) = 2*[ka^2 x, kb^2 y, kc^2 z]
+// so the unit norm we're interested in is nn=n/|n| (the "2" drops out).
+// If Q is not on the ellipsoid this is equivalent to scaling the ray Q-O
+// until it hits the ellipsoid surface at Q'=s*Q, and then reporting the outward
+// normal at Q' instead.
+// Cost is about 40 flops.
+inline UnitVec3 ContactGeometry::Ellipsoid::Impl::
+findUnitNormalAtPoint(const Vec3& Q) const {
+ const Vec3 kk(square(curvatures[0]), square(curvatures[1]),
+ square(curvatures[2]));
+ return UnitVec3(kk[0]*Q[0], kk[1]*Q[1], kk[2]*Q[2]);
+}
+
+
+
+//==============================================================================
+// SMOOTH HEIGHT MAP IMPL
+//==============================================================================
+class SmoothHeightMapImplicitFunction : public Function {
+public:
+ SmoothHeightMapImplicitFunction() : ownerp(0) {}
+ SmoothHeightMapImplicitFunction
+ (const ContactGeometry::SmoothHeightMap::Impl& owner)
+ : ownerp(&owner) {}
+ void setOwner(const ContactGeometry::SmoothHeightMap::Impl& owner)
+ { ownerp=&owner; }
+ Real calcValue(const Vector& x) const;
+ Real calcDerivative(const Array_<int>& derivComponents,
+ const Vector& x) const;
+ int getArgumentSize() const {return 3;}
+ int getMaxDerivativeOrder() const
+ { return std::numeric_limits<int>::max(); }
+private:
+ // just a reference; don't delete
+ const ContactGeometry::SmoothHeightMap::Impl* ownerp;
+};
+
+
+
+class ContactGeometry::SmoothHeightMap::Impl : public ContactGeometryImpl {
+public:
+ explicit Impl(const BicubicSurface& surface);
+
+ ContactGeometryImpl* clone() const {
+ return new Impl(surface);
+ }
+
+ const BicubicSurface& getBicubicSurface() const {return surface;}
+ BicubicSurface::PatchHint& updHint() const {return hint;}
+
+ ContactGeometryTypeId getTypeId() const {return classTypeId();}
+
+ DecorativeGeometry createDecorativeGeometry() const;
+ Vec3 findNearestPoint(const Vec3& position, bool& inside,
+ UnitVec3& normal) const;
+
+ bool intersectsRay(const Vec3& origin, const UnitVec3& direction,
+ Real& distance, UnitVec3& normal) const;
+
+ void getBoundingSphere(Vec3& center, Real& radius) const {
+ center = boundingSphere.getCenter();
+ radius = boundingSphere.getRadius();
+ }
+
+ bool isSmooth() const {return true;}
+ bool isConvex() const {return false;}
+ bool isFinite() const {return true;}
+
+ Vec3 calcSupportPoint(UnitVec3 direction) const {
+ assert(false);
+ return Vec3(NaN);
+ }
+
+ // We ignore the z coordinate here and just return the curvature of
+ // the unique point at (x,y).
+ void calcCurvature(const Vec3& point, Vec2& curvature,
+ Rotation& orientation) const {
+ Transform X_SP;
+ surface.calcParaboloid(Vec2(point[0],point[1]), hint, X_SP, curvature);
+ orientation = X_SP.R();
+ }
+
+ const Function& getImplicitFunction() const {return implicitFunction;}
+
+ static ContactGeometryTypeId classTypeId() {
+ static const ContactGeometryTypeId id =
+ createNewContactGeometryTypeId();
+ return id;
+ }
+private:
+ void createBoundingVolumes();
+ // The given OBBNode is assigned this range of patches. If there is
+ // more than one patch in the range, it will dole those out to its
+ // children recursively until the leaves each have responsibility for
+ // one patch. Then we'll deal with that patch, which may have to be
+ // subdivided into submission.
+ void splitPatches(int x0,int y0, int nx, int ny,
+ OBBNode& node, int depth,
+ Array_<const Vec3*>* parentControlPoints=0) const;
+
+ // The supplied OBBNode has responsibility for the given subpatch, which
+ // may need further subdivision.
+ void assignPatch(const Geo::BicubicBezierPatch& patch,
+ OBBNode& node, int depth,
+ Array_<const Vec3*>* parentControlPoints=0) const;
+
+
+ BicubicSurface surface;
+ mutable BicubicSurface::PatchHint hint;
+ Geo::Sphere boundingSphere;
+ SmoothHeightMapImplicitFunction implicitFunction;
+};
+
+
+
+
+//==============================================================================
+// OBB TREE NODE IMPL
+//==============================================================================
+class OBBTreeNodeImpl {
+public:
+ OBBTreeNodeImpl() : child1(NULL), child2(NULL) {
+ }
+ OBBTreeNodeImpl(const OBBTreeNodeImpl& copy);
+ ~OBBTreeNodeImpl();
+ OrientedBoundingBox bounds;
+ OBBTreeNodeImpl* child1;
+ OBBTreeNodeImpl* child2;
+ Array_<int> triangles;
+ int numTriangles;
+ Vec3 findNearestPoint(const ContactGeometry::TriangleMesh::Impl& mesh,
+ const Vec3& position, Real cutoff2, Real& distance2,
+ int& face, Vec2& uv) const;
+ bool intersectsRay(const ContactGeometry::TriangleMesh::Impl& mesh,
+ const Vec3& origin, const UnitVec3& direction,
+ Real& distance, int& face, Vec2& uv) const;
+};
+
+
+
+//==============================================================================
+// TRIANGLE MESH IMPL
+//==============================================================================
+class ContactGeometry::TriangleMesh::Impl : public ContactGeometryImpl {
+public:
+ class Edge;
+ class Face;
+ class Vertex;
+
+ Impl(const ArrayViewConst_<Vec3>& vertexPositions,
+ const ArrayViewConst_<int>& faceIndices, bool smooth);
+ Impl(const PolygonalMesh& mesh, bool smooth);
+ ContactGeometryImpl* clone() const {
+ return new Impl(*this);
+ }
+
+ ContactGeometryTypeId getTypeId() const {return classTypeId();}
+
+ DecorativeGeometry createDecorativeGeometry() const;
+ Vec3 findPoint(int face, const Vec2& uv) const;
+ Vec3 findCentroid(int face) const;
+ UnitVec3 findNormalAtPoint(int face, const Vec2& uv) const;
+ Vec3 findNearestPoint(const Vec3& position, bool& inside,
+ UnitVec3& normal) const;
+ Vec3 findNearestPoint(const Vec3& position, bool& inside, int& face,
+ Vec2& uv) const;
+ Vec3 findNearestPointToFace(const Vec3& position, int face, Vec2& uv) const;
+ bool intersectsRay(const Vec3& origin, const UnitVec3& direction,
+ Real& distance, UnitVec3& normal) const;
+ bool intersectsRay(const Vec3& origin, const UnitVec3& direction,
+ Real& distance, int& face, Vec2& uv) const;
+ void getBoundingSphere(Vec3& center, Real& radius) const;
+
+ bool isSmooth() const {return false;}
+ bool isConvex() const {return false;}
+ bool isFinite() const {return true;}
+
+ void createPolygonalMesh(PolygonalMesh& mesh) const;
+
+ static ContactGeometryTypeId classTypeId() {
+ static const ContactGeometryTypeId id =
+ createNewContactGeometryTypeId();
+ return id;
+ }
+private:
+ void init(const Array_<Vec3>& vertexPositions, const Array_<int>& faceIndices);
+ void createObbTree(OBBTreeNodeImpl& node, const Array_<int>& faceIndices);
+ void splitObbAxis(const Array_<int>& parentIndices,
+ Array_<int>& child1Indices,
+ Array_<int>& child2Indices, int axis);
+ void findBoundingSphere(Vec3* point[], int p, int b,
+ Vec3& center, Real& radius);
+ friend class ContactGeometry::TriangleMesh;
+ friend class OBBTreeNodeImpl;
+
+ Array_<Edge> edges;
+ Array_<Face> faces;
+ Array_<Vertex> vertices;
+ Vec3 boundingSphereCenter;
+ Real boundingSphereRadius;
+ OBBTreeNodeImpl obb;
+ bool smooth;
+};
+
+
+
+//==============================================================================
+// TriangleMeshImpl EDGE
+//==============================================================================
+class ContactGeometry::TriangleMesh::Impl::Edge {
+public:
+ Edge(int vert1, int vert2, int face1, int face2) {
+ vertices[0] = vert1;
+ vertices[1] = vert2;
+ faces[0] = face1;
+ faces[1] = face2;
+ }
+ int vertices[2];
+ int faces[2];
+};
+
+
+
+//==============================================================================
+// TriangleMeshImpl FACE
+//==============================================================================
+class ContactGeometry::TriangleMesh::Impl::Face {
+public:
+ Face(int vert1, int vert2, int vert3,
+ const Vec3& normal, Real area)
+ : normal(normal), area(area) {
+ vertices[0] = vert1;
+ vertices[1] = vert2;
+ vertices[2] = vert3;
+ }
+ int vertices[3];
+ int edges[3];
+ UnitVec3 normal;
+ Real area;
+};
+
+
+
+//==============================================================================
+// TriangleMeshImpl VERTEX
+//==============================================================================
+class ContactGeometry::TriangleMesh::Impl::Vertex {
+public:
+ Vertex(Vec3 pos) : pos(pos), firstEdge(-1) {
+ }
+ Vec3 pos;
+ UnitVec3 normal;
+ int firstEdge;
+};
+
+
+
+//==============================================================================
+// TORUS IMPL
+//==============================================================================
+class TorusImplicitFunction : public Function {
+public:
+ TorusImplicitFunction() : ownerp(0) {}
+ TorusImplicitFunction(const ContactGeometry::Torus::Impl& owner)
+ : ownerp(&owner) {}
+ void setOwner(const ContactGeometry::Torus::Impl& owner) {ownerp=&owner;}
+ Real calcValue(const Vector& x) const;
+ Real calcDerivative(const Array_<int>& derivComponents,
+ const Vector& x) const;
+ int getArgumentSize() const {return 3;}
+ int getMaxDerivativeOrder() const
+ { return std::numeric_limits<int>::max(); }
+private:
+ const ContactGeometry::Torus::Impl* ownerp; // just a reference; don't delete
+};
+
+class ContactGeometry::Torus::Impl : public ContactGeometryImpl {
+public:
+ explicit Impl(Real torusRadius, Real tubeRadius) :
+ torusRadius(torusRadius), tubeRadius(tubeRadius) {
+ function.setOwner(*this);
+ }
+
+ ContactGeometryImpl* clone() const {
+ return new Impl(torusRadius, tubeRadius);
+ }
+ Real getTorusRadius() const {
+ return torusRadius;
+ }
+ void setTorusRadius(Real r) {
+ torusRadius = r;
+ }
+ Real getTubeRadius() const {
+ return tubeRadius;
+ }
+ void setTubeRadius(Real r) {
+ tubeRadius = r;
+ }
+
+ ContactGeometryTypeId getTypeId() const {return classTypeId();}
+
+ DecorativeGeometry createDecorativeGeometry() const;
+ bool intersectsRay(const Vec3& origin, const UnitVec3& direction,
+ Real& distance, UnitVec3& normal) const;
+ void getBoundingSphere(Vec3& center, Real& radius) const;
+
+ void createPolygonalMesh(PolygonalMesh& mesh) const;
+
+ bool isSmooth() const {return true;}
+ bool isConvex() const {return false;}
+ bool isFinite() const {return true;}
+
+ Vec3 calcSupportPoint(UnitVec3 direction) const;
+
+ void calcCurvature(const Vec3& point, Vec2& curvature,
+ Rotation& orientation) const;
+
+ Vec3 findNearestPoint(const Vec3& position, bool& inside,
+ UnitVec3& normal) const;
+
+// TODO
+// virtual void shootGeodesicInDirectionUntilLengthReachedAnalytical(const Vec3& xP, const UnitVec3& tP,
+// const Real& terminatingLength, const GeodesicOptions& options, Geodesic& geod) const;
+//
+// virtual void shootGeodesicInDirectionUntilPlaneHitAnalytical(const Vec3& xP, const UnitVec3& tP,
+// const Plane& terminatingPlane, const GeodesicOptions& options,
+// Geodesic& geod) const;
+//
+// virtual void calcGeodesicAnalytical(const Vec3& xP, const Vec3& xQ,
+// const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const;
+
+ const Function& getImplicitFunction() const {
+ return function;
+ }
+
+ static ContactGeometryTypeId classTypeId() {
+ static const ContactGeometryTypeId id =
+ createNewContactGeometryTypeId();
+ return id;
+ }
+private:
+ Real torusRadius;
+ Real tubeRadius;
+ TorusImplicitFunction function;
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_CONTACT_GEOMETRY_IMPL_H_
diff --git a/SimTKmath/Geometry/src/ContactGeometry_Cylinder.cpp b/SimTKmath/Geometry/src/ContactGeometry_Cylinder.cpp
new file mode 100644
index 0000000..ac169e4
--- /dev/null
+++ b/SimTKmath/Geometry/src/ContactGeometry_Cylinder.cpp
@@ -0,0 +1,315 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Ian Stavness *
+ * Contributors: Michael Sherman, Andreas Scholz *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_Point.h"
+#include "simmath/internal/Geo_Sphere.h"
+#include "simmath/internal/ContactGeometry.h"
+
+#include "ContactGeometryImpl.h"
+
+#include <iostream>
+#include <cmath>
+#include <map>
+#include <set>
+
+using namespace SimTK;
+using std::map;
+using std::pair;
+using std::set;
+using std::string;
+using std::cout; using std::endl;
+
+//==============================================================================
+// CONTACT GEOMETRY :: CYLINDER & IMPL
+//==============================================================================
+
+ContactGeometry::Cylinder::Cylinder(Real radius)
+: ContactGeometry(new Cylinder::Impl(radius)) {}
+
+/*static*/ ContactGeometryTypeId ContactGeometry::Cylinder::classTypeId()
+{ return ContactGeometry::Cylinder::Impl::classTypeId(); }
+
+Real ContactGeometry::Cylinder::getRadius() const {
+ return getImpl().getRadius();
+}
+
+void ContactGeometry::Cylinder::setRadius(Real radius) {
+ updImpl().setRadius(radius);
+}
+
+const ContactGeometry::Cylinder::Impl& ContactGeometry::Cylinder::getImpl() const {
+ assert(impl);
+ return static_cast<const Cylinder::Impl&>(*impl);
+}
+
+ContactGeometry::Cylinder::Impl& ContactGeometry::Cylinder::updImpl() {
+ assert(impl);
+ return static_cast<Cylinder::Impl&>(*impl);
+}
+
+DecorativeGeometry ContactGeometry::Cylinder::Impl::createDecorativeGeometry() const {
+ DecorativeCylinder cyl(radius, radius*2);
+ // DecorativeCylinder's axis is defined as the y-axis,
+ // whereas ContactGeometry::Cylinder axis is defined as the z-axis
+ cyl.setTransform(Rotation(Pi/2, XAxis));
+ return cyl;
+}
+
+Vec3 ContactGeometry::Cylinder::Impl::findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const {
+
+ normal = calcSurfaceUnitNormal(position);
+
+ // long axis is z-axis, project to x-y plane
+ Vec2 xy_position(position(0), position(1));
+ inside = (xy_position.normSqr() <= radius*radius);
+
+ // nearestPoint = point_on_surface_in_xy_plane + height_in_z
+ Vec3 nearestPoint = normal*radius + Vec3(0,0,position(2));
+
+ return nearestPoint;
+}
+
+bool ContactGeometry::Cylinder::Impl::intersectsRay
+ (const Vec3& origin, const UnitVec3& direction,
+ Real& distance, UnitVec3& normal) const
+{
+ // cylinder axis is z-axis, project to x-y plane
+ const Vec3 xy_vec(direction(0),direction(1),0);
+ const Real xy_vec_norm = xy_vec.norm();
+ const UnitVec3 xy_direction(xy_vec/xy_vec_norm, true); // don't renormalize
+ const Vec3 xy_origin(origin(0),origin(1),0);
+ Real xy_distance;
+ Real b = -~xy_direction*xy_origin;
+ Real c = xy_origin.normSqr() - radius*radius;
+ if (c > 0) {
+ // Ray origin is outside cylinder.
+
+ if (b <= 0)
+ return false; // Ray points away from axis of cylinder.
+ Real d = b*b - c;
+ if (d < 0)
+ return false;
+ Real root = std::sqrt(d);
+ xy_distance = b - root;
+ }
+ else {
+ // Ray origin is inside cylinder.
+
+ Real d = b*b - c;
+ if (d < 0)
+ return false;
+ xy_distance = b + std::sqrt(d);
+ }
+ distance = xy_distance/xy_vec_norm;
+ normal = UnitVec3(xy_origin+xy_distance*xy_direction);
+ return true;
+}
+
+void ContactGeometry::Cylinder::Impl::getBoundingSphere
+ (Vec3& center, Real& radius) const {
+ center = Vec3(0);
+ radius = Infinity;
+}
+
+void ContactGeometry::Cylinder::Impl::
+calcCurvature(const Vec3& point, Vec2& curvature, Rotation& orientation) const {
+
+ // long axis (direction of min curvature) points in <0,0,1>
+ orientation = Rotation(calcSurfaceUnitNormal(point), ZAxis, Vec3(0, 0, 1), YAxis);
+ curvature[0] = 1/radius;
+ curvature[1] = 0;
+
+}
+
+// Sample geodesic between two points P and Q on a cylinder analytically.
+static void setGeodesicToHelicalArc(Real R, Real phiP, Real angle, Real m, Real c, Geodesic& geod)
+{
+ // Clear current geodesic.
+ geod.clear();
+
+ const Real sqrt1m2 = sqrt(1+m*m); // Avoid repeated calculation.
+ const Real kappa = 1 / (R*(1+m*m)); // Curvature in tangent direction.
+ const Real kb = 1 / (R*(1+1/(m*m))); // Curvature in binormal direction
+ // (slope is 1/m).
+ const Real tau = m*kappa; // Torsion (signed).
+
+ // Arc length of the helix. Always
+ const Real L = R * sqrt1m2 * std::abs(angle);
+
+ // Orientation of helix.
+ const Real orientation = angle < 0 ? Real(-1) : Real(1);
+
+ // TODO: Make this generic, so long geodesics are sampled more than short ones.
+ const int numGeodesicSamples = 12;
+ const Real deltaPhi = std::abs(angle / Real(numGeodesicSamples-1));
+
+ for (int i = 0; i < numGeodesicSamples; ++i)
+ {
+ // Watch out: Angle phi has an offset phiP
+ Real phi = Real(i)*angle/Real(numGeodesicSamples-1) + phiP;
+ const Real sphi = sin(phi), cphi = cos(phi);
+
+ // Evaluate helix.
+ Vec3 p( R*cphi, R*sphi, R*m*(phi - phiP) + c);
+
+ // We'll normalize so UnitVec3 doesn't have to do it.
+ UnitVec3 t((orientation/sqrt1m2)*Vec3(-sphi, cphi, m), true);
+ UnitVec3 n(Vec3(cphi, sphi, 0), true);
+
+ // Though not needed, we use an orthogonalizing constructor for the rotation.
+ geod.addFrenetFrame(Transform(Rotation(n, ZAxis, t, YAxis), p));
+
+ // Current arc length s.
+ Real s = R * sqrt1m2 * (Real(i)*deltaPhi);
+ geod.addArcLength(s);
+ geod.addCurvature(kappa);
+
+ // Solve the scalar Jacobi equation
+ //
+ // j''(s) + K(s)*j(s) = 0 , (1)
+ //
+ // where K is the Gaussian curvature and (.)' := d(.)/ds denotes differentiation
+ // with respect to the arc length s. Then, j is the directional sensitivity and
+ // we obtain the corresponding variational vector field by multiplying b*j. For
+ // a cylinder, K = 0 and the solution of equation (1) becomes
+ //
+ // j = s (2)
+ // j' = 1 , (3)
+ //
+ // so the Jacobi field increases linearly in s.
+
+ // Forward directional sensitivity from P to Q
+ Vec2 jPQ(s, 1);
+ geod.addDirectionalSensitivityPtoQ(jPQ);
+
+ // Backwards directional sensitivity from Q to P
+ Vec2 jQP(L-s, 1);
+ geod.addDirectionalSensitivityQtoP(jQP);
+
+
+ // TODO: positional sensitivity
+ geod.addPositionalSensitivityPtoQ(Vec2(NaN));
+ geod.addPositionalSensitivityQtoP(Vec2(NaN));
+ }
+
+ // Only compute torsion and binormal curvature at the end points.
+ geod.setTorsionAtP(tau); geod.setTorsionAtQ(tau);
+ geod.setBinormalCurvatureAtP(kb); geod.setBinormalCurvatureAtQ(kb);
+
+ geod.setIsConvex(true); // Curve on cylinder is always convex.
+
+ geod.setIsShortest(false); // TODO
+ geod.setAchievedAccuracy(SignificantReal); // TODO: accuracy of length?
+// geod.setInitialStepSizeHint(integ.getActualInitialStepSizeTaken()); // TODO
+}
+
+// Compute geodesic between two points P and Q on a cylinder analytically. Since a geodesic on a
+// cylinder is a helix it is parameterized by
+//
+// [ R * cos(phi) ]
+// p(phi) = [ R * sin(phi) ]
+// [ R * m * phi + c ]
+//
+// where R is the radius of the cylinder, phi parameterizes the opening angle of the helix, m is
+// the slope and c is an offset. We define the geodesic from P to Q, hence c = Pz.
+void ContactGeometry::Cylinder::Impl::
+calcGeodesicAnalytical(const Vec3& xP, const Vec3& xQ,
+ const Vec3& tPhint, const Vec3& tQhint,
+ Geodesic& geod) const
+{
+ // Compute angle between P and Q. Save both the positive (right handed) and the negative
+ // (left handed) angle.
+ Real phiP = atan2(xP[1], xP[0]);
+ Real phiQ = atan2(xQ[1], xQ[0]);
+
+ Real temp = phiQ - phiP;
+ Real angleRightHanded, angleLeftHanded;
+
+ // Left-handed angle will always be negative, right-handed angle will be positive.
+ if (temp >= 0) {
+ angleRightHanded = temp;
+ angleLeftHanded = temp - 2*Pi;
+ }
+
+ else {
+ angleLeftHanded = temp;
+ angleRightHanded = temp + 2*Pi;
+ }
+
+ // Compute "moment" of tPhint at P and tQhint at Q around z-Axis.
+ // Make sure tPhint and tQhint are unit vectors, otherwise moments are scaled.
+ Real MP = xP[0]*tPhint[1] - xP[1]*tPhint[0];
+ Real MQ = xQ[0]*tQhint[1] - xQ[1]*tQhint[0];
+
+ // Average moment.
+ Real M = (MP + MQ) / 2;
+
+ // Decide whether helix is right or left handed. The sign of angle stores the
+ // information about the orientation (right handed, if positive)
+ Real angle;
+ if (M >= 0) {
+ angle = angleRightHanded;
+ }
+
+ else {
+ angle = angleLeftHanded;
+ }
+
+ // Offset and slope.
+ Real c = xP[2];
+ Real m = (xQ[2] - xP[2]) / (angle * radius);
+
+ setGeodesicToHelicalArc(radius, phiP, angle, m, c, geod);
+}
+
+void ContactGeometry::Cylinder::Impl::shootGeodesicInDirectionUntilLengthReachedAnalytical(const Vec3& xP, const UnitVec3& tP,
+ const Real& terminatingLength, const GeodesicOptions& options, Geodesic& geod) const {
+
+ //TODO for Andreas :)
+}
+
+void ContactGeometry::Cylinder::Impl::shootGeodesicInDirectionUntilPlaneHitAnalytical(const Vec3& xP, const UnitVec3& tP,
+ const Plane& terminatingPlane, const GeodesicOptions& options,
+ Geodesic& geod) const {
+
+ //TODO for Andreas :)
+}
+
+Real CylinderImplicitFunction::
+calcValue(const Vector& x) const {
+ return 1-(x[0]*x[0]+x[1]*x[1])/square(ownerp->getRadius());
+}
+
+Real CylinderImplicitFunction::
+calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
+ if (derivComponents.size() == 1 && derivComponents[0] < 2)
+ return -2*x[derivComponents[0]]/square(ownerp->getRadius());
+ if (derivComponents.size() == 2 &&
+ derivComponents[0] == derivComponents[1] &&
+ derivComponents[0] < 2 )
+ return -2/square(ownerp->getRadius());
+ return 0;
+}
diff --git a/SimTKmath/Geometry/src/ContactGeometry_Ellipsoid.cpp b/SimTKmath/Geometry/src/ContactGeometry_Ellipsoid.cpp
new file mode 100644
index 0000000..97f07cd
--- /dev/null
+++ b/SimTKmath/Geometry/src/ContactGeometry_Ellipsoid.cpp
@@ -0,0 +1,397 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_Point.h"
+#include "simmath/internal/Geo_Sphere.h"
+#include "simmath/internal/ContactGeometry.h"
+
+#include "ContactGeometryImpl.h"
+
+#include <iostream>
+#include <cmath>
+#include <map>
+#include <set>
+
+using namespace SimTK;
+using std::map;
+using std::pair;
+using std::set;
+using std::string;
+using std::cout; using std::endl;
+
+
+//==============================================================================
+// CONTACT GEOMETRY :: ELLIPSOID & IMPL
+//==============================================================================
+
+ContactGeometry::Ellipsoid::Ellipsoid(const Vec3& radii)
+: ContactGeometry(new Ellipsoid::Impl(radii)) {}
+
+void ContactGeometry::Ellipsoid::setRadii(const Vec3& radii)
+{ updImpl().setRadii(radii); }
+
+/*static*/ ContactGeometryTypeId ContactGeometry::Ellipsoid::classTypeId()
+{ return ContactGeometry::Ellipsoid::Impl::classTypeId(); }
+
+const Vec3& ContactGeometry::Ellipsoid::getRadii() const
+{ return getImpl().getRadii(); }
+
+const Vec3& ContactGeometry::Ellipsoid::getCurvatures() const
+{ return getImpl().getCurvatures(); }
+
+UnitVec3 ContactGeometry::Ellipsoid::
+findUnitNormalAtPoint(const Vec3& Q) const
+{ return getImpl().findUnitNormalAtPoint(Q); }
+
+Vec3 ContactGeometry::Ellipsoid::
+findPointWithThisUnitNormal(const UnitVec3& nn) const
+{ return getImpl().findPointWithThisUnitNormal(nn); }
+
+Vec3 ContactGeometry::Ellipsoid::
+findPointInSameDirection(const Vec3& Q) const
+{ return getImpl().findPointInSameDirection(Q); }
+
+void ContactGeometry::Ellipsoid::
+findParaboloidAtPoint(const Vec3& Q, Transform& X_EP, Vec2& k) const
+{ return getImpl().findParaboloidAtPoint(Q,X_EP,k); }
+
+void ContactGeometry::Ellipsoid::
+findParaboloidAtPointWithNormal(const Vec3& Q, const UnitVec3& nn,
+ Transform& X_EP, Vec2& k) const
+{ return getImpl().findParaboloidAtPointWithNormal(Q,nn,X_EP,k); }
+
+
+const ContactGeometry::Ellipsoid::Impl& ContactGeometry::Ellipsoid::
+getImpl() const {
+ assert(impl);
+ return static_cast<const Ellipsoid::Impl&>(*impl);
+}
+
+ContactGeometry::Ellipsoid::Impl& ContactGeometry::Ellipsoid::
+updImpl() {
+ assert(impl);
+ return static_cast<Ellipsoid::Impl&>(*impl);
+}
+
+DecorativeGeometry ContactGeometry::Ellipsoid::Impl::createDecorativeGeometry() const {
+ return DecorativeEllipsoid(radii);
+}
+
+// Given a point Q on an ellipsoid, with outward unit normal nn at Q: find the
+// principal curvatures at the point and their directions. The result is a
+// coordinate frame with origin Q, z axis the ellipsoid normal nn at Q, x axis
+// is the direction dmax of maximum curvature kmax, y axis the direction dmin
+// of minimum curvature kmin, such that [dmax dmin n] forms a right-handed set.
+// This is equivalent to fitting an elliptic paraboloid
+// z = -kmax/2 x^2 -kmin/2 y^2 to the ellipsoid at point Q. Note that for
+// an ellipsoid we have kmax>=kmin>0.
+//
+// We'll find the ellipse on the central plane perpendicular to the normal by
+// intersecting the plane equation with the ellipsoid equation but working in
+// the plane frame P=[u v n], where u and v are arbitrary axes in the plane.
+// Our goal is to obtain an equation for the ellipse in P and then rotate the
+// P frame about its normal until we get the ellipse in standard form
+// Ru^2+Sv^2=1 in which case d/R and d/S are the ellipsoid curvatures (d is the
+// distance from the point on the ellipsoid to the plane).
+// ref: McArthur, Neil. "Principal radii of curvature at a point on an
+// ellipsoid", Mathematical Notes 24 pp. xvi-xvii, 1929.
+//
+// In its own frame E=[x y z] the ellipsoid surface is the set of points such
+// that
+// ~e * diag(A,B,C) * e = 1
+// where e is a vector expressed in E. The plane is the set of points
+// satisfying ~e * n = 0. We can write rotation matrix R_EP=[u v n] where
+// u,v,n are expressed in E. Now we can put the ellipsoid in P:
+// ~(R_EP*p) * diag(A,B,C) * (R_EP*p) = 1
+// We can intersect that with the plane just by dropping the n coordinate of
+// p so p=[u v 0] (u,v scalars here), and the intersection equation is
+// A(u*ux + v*vx)^2 + B(u*uy+v*vy)^2 + C(u*uz + v*vz)^2 = 1
+// which is
+// R u^2 + S v^2 + T u*v = 1
+// with
+// R = A ux^2 + B uy^2 + C uz^2
+// S = A vx^2 + B vy^2 + C vz^2
+// T = 2(A ux*vx + B uy*vy + C uz*vz)
+//
+// We want to find a rotation about n that eliminates the cross term Tuv,
+// leaving us with
+// R' u'^2 + S' v'^2 = 1
+// for new constants R' and S' and new basis u' and v'.
+//
+// Method
+// ------
+// We'll calculate an angle theta where theta=0 would be along u and
+// theta=pi/2 would be along v. Then theta+pi/2 is a perpendicular direction
+// that has the other curvature extreme. Per "Dr Rob" at Mathforum.org 2000:
+// t2t = tan(2*theta) = T/(R-S)
+// theta = atan(t2t)/2, c = cos(theta), s = sin(theta)
+// R' = Rc^2 + Tsc + Ss^2 (theta direction)
+// S' = Rs^2 - Tsc + Sc^2 (theta+pi/2 direction)
+// Directions are u' = c*u + s*v, v' = c*v - s*u; these are automatically unit
+// vectors.
+//
+// Optimization
+// ------------
+// The above requires an atan() to get 2*theta then sin & cos(theta) at
+// a cost of about 120 flops. We can use half angle formulas to work
+// exclusively with 2*theta, but then we'll have to normalize u' and v'
+// at the end:
+// t2t = tan(2*theta) = T/(R-S)
+// c2t = cos(2*theta) = 1/sqrt(1 + t2t^2)
+// s2t = sin(2*theta) = t2t*cos2t;
+// 2*R' = R+S + Rc2t - Sc2t + Ts2t
+// 2*S' = R+S - Rc2t + Sc2t - Ts2t
+// By multiplying the u',v' formulas above by 2*c we change the lengths
+// but get expressions that are easily converted to double angles:
+// u' = normalize((1+c2t)*u + s2t*v)
+// v' = normalize((1+c2t)*v - s2t*u)
+// (but actually v' is n X u' which is cheap). This saves about 30
+// flops over the straightforward method above.
+//
+// Cost: given a point and normalized normal
+// curvatures ~160 flops
+// directions ~ 60 flops more
+// ----
+// ~220 flops
+//
+// So: Given an ellipsoid in its own frame E, with equation Ax^2+By^2+Cz^2=1, a
+// point Q=(x,y,z) on its surface, and the unit outward normal vector nn at Q,
+// return (kmax,kmin) the principal curvatures at Q, and a Transform with
+// x=dmax, y=dmin, z=nn, O=Q that gives the principal curvature directions.
+// (Note: A=1/a^2, B=1/b^2, C=1/c^2 where a,b,c are the ellipsoid radii.)
+void ContactGeometry::Ellipsoid::Impl::
+findParaboloidAtPointWithNormal(const Vec3& Q, const UnitVec3& nn,
+ Transform& X_EP, Vec2& k) const
+{
+ const Real A = square(curvatures[0]), B = square(curvatures[1]),
+ C = square(curvatures[2]);
+
+ // Sanity checks in debug.
+ SimTK_ERRCHK(std::abs(A*Q[0]*Q[0]+B*Q[1]*Q[1]+C*Q[2]*Q[2]-1) < SqrtEps,
+ "ContactGeometry::Ellipsoid::findParaboloidAtPointWithNormal()",
+ "The given point was not on the surface of the ellipsoid.");
+ SimTK_ERRCHK((nn-findUnitNormalAtPoint(Q)).normSqr() < SqrtEps,
+ "ContactGeometry::Ellipsoid::findParaboloidAtPointWithNormal()",
+ "The given normal was not consistent with the given point.");
+
+ UnitVec3 tu = nn.perp(); // ~40 flops
+ UnitVec3 tv(nn % tu, true); // y = z X x for plane, already normalized (9 flops)
+
+ // 27 flops to get R,S,T
+ Real R= A*square(tu[0]) + B*square(tu[1]) + C*square(tu[2]);
+ Real S= A*square(tv[0]) + B*square(tv[1]) + C*square(tv[2]);
+ Real T=2*(A*tu[0]*tv[0] + B*tu[1]*tv[1] + C*tu[2]*tv[2]);
+
+ // T will be zero for spheres (A=B=C) and for various "clean" points
+ // on the ellipsoid where tu[i]*tv[i]==0, i=0,1,2. In that case we
+ // already have the ellipse we're looking for with R,S.
+ // R==S means curvature is the same in every direction (that's called
+ // an "umbilic" point). In that case tu and tv are good directions.
+ // I *believe* R==S -> T==0 but I don't have a proof.
+ Real kmax2, kmin2; // squared curvatures of ellipse
+ UnitVec3 dmax;
+ if (std::abs(R-S) < SignificantReal*std::max(R,S)) {
+ kmax2 = kmin2 = (R+S)/2;
+ dmax = tu;
+ } else if (std::abs(T) < SignificantReal) {
+ if (R < S) kmax2=S, dmax=tv, kmin2=R;
+ else kmax2=R, dmax=tu, kmin2=S;
+ } else { // T,R-S both nonzero
+ Real tan2t = T/(R-S); // ~20 flops
+ Real cos2t = 1/std::sqrt(1 + square(tan2t)); // ~40 flops
+ Real sin2t = tan2t*cos2t; // 1 flop
+ // 11 flops here
+ Real term = R*cos2t-S*cos2t+T*sin2t;
+ Real Rp = (R+S + term)/2;
+ Real Sp = (R+S - term)/2;
+
+ // Sort into kmax, kmin; at most one normalization done below
+ if (Rp < Sp) {
+ kmax2=Sp, kmin2=Rp;
+ dmax = UnitVec3((1+cos2t)*tv - sin2t*tu); // Sdir, must normalize, ~50 flops
+ } else {
+ kmax2=Rp,kmin2=Sp;
+ dmax = UnitVec3((1+cos2t)*tu + sin2t*tv); // Rdir, must normalize, ~50 flops
+ }
+ }
+
+ Real d = ~Q * nn; // distance along normal from center to point on ellipsoid (5 flops)
+ Real kmax = d * kmax2, kmin = d * kmin2; // surface curvatures (2 flops)
+
+ X_EP.updP() = Q; // the origin point
+ Rotation& R_EP = X_EP.updR();
+ // 9 flops
+ UnitVec3 dmin = UnitVec3(nn % dmax, true); // y=z%x ensures right handedness (already unit vector too)
+ R_EP.setRotationFromUnitVecsTrustMe(dmax, dmin, nn);
+
+ k = Vec2(kmax, kmin);
+}
+
+
+// Peter E. says he implemented this from David Eberly's web site
+// http://www.geometrictools.com/Documentation/DistancePointToEllipsoid.pdf
+// Eberly says he got it from John Hart's article in Graphics Gems 4, page
+// 113 "Distance to an Ellipsoid". Both Eberly and Hart recommend using a
+// Newton iteration to solve this problem because the largest root is directly
+// downhill given appropriate starting points, which they provide. However,
+// the implementation here uses a direct solution of the 6th-order polynomial
+// then searches for the largest real root. That is likely to be *much* slower
+// than the recommended approach, although that should be measured.
+//
+// I asked Peter and he said he did not try and reject the Newton approach;
+// he just took the direct approach. I believe the Newton method would be
+// *much* faster, but Eberly hints that there are special cases that can
+// cause convergence troubles and must be dealt with carefully. If the
+// existing routine turns out to be a bottleneck, it would be worth revisiting
+// this implementation. -- Sherm 20110203.
+//
+// TODO: use faster method?
+Vec3 ContactGeometry::Ellipsoid::Impl::
+findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const {
+ Real a2 = radii[0]*radii[0];
+ Real b2 = radii[1]*radii[1];
+ Real c2 = radii[2]*radii[2];
+ Real a4 = a2*a2;
+ Real b4 = b2*b2;
+ Real c4 = c2*c2;
+ Real px2 = position[0]*position[0];
+ Real py2 = position[1]*position[1];
+ Real pz2 = position[2]*position[2];
+ Real a2b2 = a2*b2;
+ Real b2c2 = b2*c2;
+ Real a2c2 = a2*c2;
+ Real a2b2c2 = a2b2*c2;
+ Vector coeff(7);
+ coeff[0] = 1;
+ coeff[1] = 2*(a2+b2+c2);
+ coeff[2] = -(a2*px2+b2*py2+c2*pz2) + a4+b4+c4 + 4*(a2b2+b2c2+a2c2);
+ coeff[3] = -2*((a2b2+a2c2)*px2+(a2b2+b2c2)*py2+(b2c2+a2c2)*pz2) + 2*(a4*(b2+c2)+b4*(a2+c2)+c4*(a2+b2)) + 8*a2b2c2;
+ coeff[4] = -a2*(b4+4*b2c2+c4)*px2-b2*(a4+4*a2c2+c4)*py2-c2*(a4+4*a2b2+b4)*pz2 + 4*(a2+b2+c2)*a2b2c2 + a4*b4+a4*c4+b4*c4;
+ coeff[5] = 2*a2b2c2*(-(b2+c2)*px2-(a2+c2)*py2-(a2+b2)*pz2 + a2b2+b2c2+a2c2);
+ coeff[6] = a2b2c2*(-b2c2*px2-a2c2*py2-a2b2*pz2+a2b2c2);
+ Vector_<complex<Real> > roots(6);
+ PolynomialRootFinder::findRoots(coeff, roots);
+ Real root = NTraits<Real>::getMostNegative();
+ for (int i = 0; i < 6; i++)
+ if (fabs(roots[i].imag()) < 1e-10 && (roots[i].real()) > (root))
+ root = roots[i].real();
+ Vec3 result(position[0]*a2/(root+a2), position[1]*b2/(root+b2), position[2]*c2/(root+c2));
+ Vec3 ri2(1/a2, 1/b2, 1/c2);
+ inside = (position[0]*position[0]*ri2[0] + position[1]*position[1]*ri2[1] + position[2]*position[2]*ri2[2] < 1.0);
+ normal = UnitVec3(result[0]*ri2[0], result[1]*ri2[1], result[2]*ri2[2]);
+ return result;
+}
+
+// Peter says he took this algorithm from Art of Illusion but can't remember
+// where it came from. It is similar to an algorithm presented in this thread:
+// http://www.ogre3d.org/forums/viewtopic.php?f=2&t=26442&start=0
+// and is most likely a special case of the general ray-quadric intersection
+// method presented by Cychosz and Waggenspack in Graphics Gems III, pg. 275,
+// "Intersecting a ray with a quadric surface."
+bool ContactGeometry::Ellipsoid::Impl::intersectsRay
+ (const Vec3& origin, const UnitVec3& direction,
+ Real& distance, UnitVec3& normal) const
+{
+ Real rx2 = radii[0]*radii[0];
+ Real sy = rx2/(radii[1]*radii[1]);
+ Real sz = rx2/(radii[2]*radii[2]);
+ Vec3 scaledDir(direction[0], sy*direction[1], sz*direction[2]);
+ Real b = -(~scaledDir*origin);
+ Real c = origin[0]*origin[0] + sy*origin[1]*origin[1] + sz*origin[2]*origin[2] - rx2;
+ if (c > 0) {
+ // Ray origin is outside ellipsoid.
+
+ if (b <= 0)
+ return false; // Ray points away from the ellipsoid.
+ Real a = ~scaledDir*direction;;
+ Real d = b*b - a*c;
+ if (d < 0)
+ return false;
+ distance = (b - std::sqrt(d))/a;
+ }
+ else {
+ // Ray origin is inside ellipsoid.
+
+ Real a = ~scaledDir*direction;;
+ Real d = b*b - a*c;
+ if (d < 0)
+ return false;
+ distance = (b + std::sqrt(d))/a;
+ }
+ Vec3 pos = origin+distance*direction;
+ normal = UnitVec3(pos[0], pos[1]*sy, pos[2]*sz);
+ return true;
+}
+
+void ContactGeometry::Ellipsoid::Impl::
+getBoundingSphere(Vec3& center, Real& radius) const {
+ center = Vec3(0);
+ radius = max(radii);
+}
+
+void ContactGeometry::Ellipsoid::Impl::
+calcCurvature(const Vec3& point, Vec2& curvature, Rotation& orientation) const {
+ Transform transform;
+ findParaboloidAtPoint(point, transform, curvature);
+ orientation = transform.R();
+}
+
+
+//TODO: just an axis-aligned leaf box for now
+void ContactGeometry::Ellipsoid::Impl::createOBBTree() {
+ OBBNode& root = obbTree.updRoot();
+ root.box.setHalfLengths(radii);
+ root.normal = UnitVec3(XAxis); // doesn't matter
+ root.coneHalfAngle = Pi; // has all possible normals
+ root.pointOnSurface = Vec3(radii[0],0,0); // doesn't matter
+ root.children.clear(); // This is a leaf
+
+ // Leaf contents.
+ root.centerUW = Vec2(0,0);
+ root.dims = Vec2(Pi, Pi/2); // u in [-Pi,Pi], v in [-Pi/2,Pi/2]
+}
+
+Real EllipsoidImplicitFunction::
+calcValue(const Vector& x) const {
+ const Vec3& radii = ownerp->getRadii();
+ return 1-x[0]*x[0]/(radii[0]*radii[0])-x[1]*x[1]/(radii[1]*radii[1])-x[2]*x[2]/(radii[2]*radii[2]);
+}
+
+Real EllipsoidImplicitFunction::
+calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
+ const Vec3& radii = ownerp->getRadii();
+ if (derivComponents.size() == 1) {
+ int c = derivComponents[0];
+ return -2*x[c]/(radii[c]*radii[c]);
+ }
+ if ( derivComponents.size() == 2
+ && derivComponents[0] == derivComponents[1]) {
+ int c = derivComponents[0];
+ return -2/(radii[c]*radii[c]);
+ }
+ // A mixed second derivative, or any higher derivative is zero.
+ return 0;
+}
diff --git a/SimTKmath/Geometry/src/ContactGeometry_HalfSpace.cpp b/SimTKmath/Geometry/src/ContactGeometry_HalfSpace.cpp
new file mode 100644
index 0000000..fa6040b
--- /dev/null
+++ b/SimTKmath/Geometry/src/ContactGeometry_HalfSpace.cpp
@@ -0,0 +1,96 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/ContactGeometry.h"
+
+#include "ContactGeometryImpl.h"
+
+#include <iostream>
+#include <cmath>
+#include <map>
+#include <set>
+
+using namespace SimTK;
+using std::map;
+using std::pair;
+using std::set;
+using std::string;
+using std::cout; using std::endl;
+
+
+//==============================================================================
+// CONTACT GEOMETRY :: HALF SPACE & IMPL
+//==============================================================================
+ContactGeometry::HalfSpace::HalfSpace()
+: ContactGeometry(new HalfSpace::Impl()) {}
+
+/*static*/ ContactGeometryTypeId ContactGeometry::HalfSpace::classTypeId()
+{ return ContactGeometry::HalfSpace::Impl::classTypeId(); }
+
+DecorativeGeometry ContactGeometry::HalfSpace::Impl::createDecorativeGeometry() const {
+ return DecorativeBrick(Vec3(Real(0.01),1,1));
+}
+
+// Point position is given in the half space frame.
+Vec3 ContactGeometry::HalfSpace::Impl::findNearestPoint
+ (const Vec3& position, bool& inside, UnitVec3& normal) const {
+ inside = (position[0] >= 0);
+ normal = -UnitVec3(XAxis); // this does not require normalization
+ return Vec3(0, position[1], position[2]);
+}
+
+bool ContactGeometry::HalfSpace::Impl::intersectsRay
+ (const Vec3& origin, const UnitVec3& direction,
+ Real& distance, UnitVec3& normal) const
+{
+ if (std::abs(direction[0]) < SignificantReal)
+ return false; // ray is parallel to halfspace surface
+
+ const Real t = origin[0]/direction[0];
+ if (t > 0)
+ return false; // ray points away from surface
+
+ distance = -t;
+ normal = -UnitVec3(XAxis); // cheap; no normalization required
+ return true;
+}
+
+void ContactGeometry::HalfSpace::Impl::getBoundingSphere
+ (Vec3& center, Real& radius) const
+{ center = Vec3(0);
+ radius = Infinity; }
+
+const ContactGeometry::HalfSpace::Impl& ContactGeometry::HalfSpace::
+getImpl() const {
+ assert(impl);
+ return static_cast<const HalfSpace::Impl&>(*impl);
+}
+
+ContactGeometry::HalfSpace::Impl& ContactGeometry::HalfSpace::
+updImpl() {
+ assert(impl);
+ return static_cast<HalfSpace::Impl&>(*impl);
+}
diff --git a/SimTKmath/Geometry/src/ContactGeometry_SmoothHeightMap.cpp b/SimTKmath/Geometry/src/ContactGeometry_SmoothHeightMap.cpp
new file mode 100644
index 0000000..13bb8c0
--- /dev/null
+++ b/SimTKmath/Geometry/src/ContactGeometry_SmoothHeightMap.cpp
@@ -0,0 +1,244 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Matthew Millard *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_Point.h"
+#include "simmath/internal/Geo_Sphere.h"
+#include "simmath/internal/Geo_BicubicBezierPatch.h"
+#include "simmath/internal/BicubicSurface.h"
+#include "simmath/internal/ContactGeometry.h"
+
+#include "ContactGeometryImpl.h"
+
+#include <iostream>
+#include <cmath>
+#include <map>
+#include <set>
+
+using namespace SimTK;
+using std::map;
+using std::pair;
+using std::set;
+using std::string;
+using std::cout; using std::endl;
+
+
+//==============================================================================
+// CONTACT GEOMETRY :: SMOOTH HEIGHT MAP & IMPL
+//==============================================================================
+
+ContactGeometry::SmoothHeightMap::
+SmoothHeightMap(const BicubicSurface& surface)
+: ContactGeometry(new SmoothHeightMap::Impl(surface)) {}
+
+/*static*/ ContactGeometryTypeId ContactGeometry::SmoothHeightMap::
+classTypeId()
+{ return ContactGeometry::SmoothHeightMap::Impl::classTypeId(); }
+
+const BicubicSurface& ContactGeometry::SmoothHeightMap::
+getBicubicSurface() const {return getImpl().getBicubicSurface();}
+
+const OBBTree& ContactGeometry::SmoothHeightMap::
+getOBBTree() const {return getImpl().getOBBTree();}
+
+const ContactGeometry::SmoothHeightMap::Impl& ContactGeometry::SmoothHeightMap::
+getImpl() const {
+ assert(impl);
+ return static_cast<const SmoothHeightMap::Impl&>(*impl);
+}
+
+ContactGeometry::SmoothHeightMap::Impl& ContactGeometry::SmoothHeightMap::
+updImpl() {
+ assert(impl);
+ return static_cast<SmoothHeightMap::Impl&>(*impl);
+}
+
+// This is the main constructor.
+ContactGeometry::SmoothHeightMap::Impl::
+Impl(const BicubicSurface& surface)
+: surface(surface) {
+ implicitFunction.setOwner(*this);
+
+ createBoundingVolumes();
+
+}
+
+void ContactGeometry::SmoothHeightMap::Impl::
+assignPatch(const Geo::BicubicBezierPatch& patch,
+ OBBNode& node, int depth,
+ Array_<const Vec3*>* parentControlPoints) const
+{
+ const Mat<4,4,Vec3>& nodeB = patch.getControlPoints();
+ const Vec2& nodeB11 = nodeB(0,0).getSubVec<2>(0); // just x,y
+ const Vec2& nodeB44 = nodeB(3,3).getSubVec<2>(0);
+ node.centerUW = (nodeB11+nodeB44)/2;
+ node.dims = (nodeB11-nodeB44).abs()/2;
+
+ // For now just split 4 ways; need to be done recursively based on
+ // flatness of patch.
+ node.children.resize(4);
+ patch.split(0.5,0.5,node.children[0].patch, node.children[1].patch,
+ node.children[2].patch, node.children[3].patch);
+ Array_<const Vec3*> myControlPoints;
+ for (int c=0; c<4; ++c) {
+ OBBNode& child = node.children[c];
+ child.depth = depth+1;
+ child.height = 0;
+ child.box = child.patch.calcOrientedBoundingBox();
+ const Mat<4,4,Vec3>& B = child.patch.getControlPoints();
+ const Vec2& b11 = B(0,0).getSubVec<2>(0); // just x,y
+ const Vec2& b44 = B(3,3).getSubVec<2>(0);
+ child.centerUW = (b11+b44)/2;
+ child.dims = (b11-b44).abs()/2;
+ for (int i=0; i<4; ++i)
+ for (int j=0; j<4; ++j)
+ myControlPoints.push_back(&B(i,j));
+ }
+
+ node.depth = depth;
+ node.height = 1 + std::max(node.children[0].height,
+ std::max(node.children[1].height,
+ std::max(node.children[2].height,
+ node.children[3].height)));
+ node.box = Geo::Point::calcOrientedBoundingBoxIndirect(myControlPoints);
+
+ if (parentControlPoints)
+ for (unsigned i=0; i<myControlPoints.size(); ++i)
+ parentControlPoints->push_back(myControlPoints[i]);
+}
+
+void ContactGeometry::SmoothHeightMap::Impl::
+splitPatches(int x0,int y0, int nx, int ny,
+ OBBNode& node, int depth,
+ Array_<const Vec3*>* parentControlPoints) const {
+ assert(nx>0 && ny>0 && depth>=0);
+
+
+ node.x0=0; node.y0=0; node.nx=nx; node.ny=ny;
+ if (nx==1 && ny==1) {
+ assignPatch(surface.calcBezierPatch(x0,y0), node, depth,
+ parentControlPoints);
+ return;
+ }
+
+ // Add two children.
+ node.children.resize(2);
+ Array_<const Vec3*> myControlPoints;
+
+ // Split on the long direction
+ if (nx > ny) {
+ splitPatches(x0, y0, nx/2, ny, node.children[0],
+ depth+1, &myControlPoints);
+ splitPatches(x0+nx/2, y0, nx-nx/2, ny, node.children[1],
+ depth+1, &myControlPoints);
+ } else {
+ splitPatches(x0, y0, nx, ny/2, node.children[0],
+ depth+1, &myControlPoints);
+ splitPatches(x0, y0+ny/2, nx, ny-ny/2, node.children[1],
+ depth+1, &myControlPoints);
+ }
+ node.depth = depth;
+ node.height = 1 + std::max(node.children[0].height,
+ node.children[1].height);
+
+ node.box = Geo::Point::calcOrientedBoundingBoxIndirect(myControlPoints);
+
+ if (parentControlPoints) {
+ for (unsigned i=0; i<myControlPoints.size(); ++i)
+ parentControlPoints->push_back(myControlPoints[i]);
+ }
+}
+
+void ContactGeometry::SmoothHeightMap::Impl::
+createBoundingVolumes() {
+ // Temporarily convert the surface into a set of Bezier patches (using
+ // a lot more memory than the original).
+
+ int nx,ny; surface.getNumPatches(nx,ny);
+ OBBNode& root = obbTree.updRoot();
+ splitPatches(0,0,nx,ny,root,0);
+
+
+ // Create bounding sphere.
+ // TODO: fake this using mesh; this needs to be done correctly instead
+ // by the BicubicSurface itself. Using 5 subdivisions per patch.
+ PolygonalMesh mesh = surface.createPolygonalMesh(5);
+
+ // Collect all the vertices.
+ const int n = mesh.getNumVertices();
+ Array_<const Vec3*> points(n);
+ for (int i=0; i<n; ++i)
+ points[i] = &mesh.getVertexPosition(i);
+ boundingSphere = Geo::Point::calcBoundingSphereIndirect(points);
+ // Add 10% as a hack to make it less likely we'll miss part of the surface.
+ boundingSphere.updRadius() *= Real(1.1);
+}
+
+DecorativeGeometry ContactGeometry::SmoothHeightMap::Impl::createDecorativeGeometry() const {
+ return DecorativeMesh(surface.createPolygonalMesh());
+}
+
+Vec3 ContactGeometry::SmoothHeightMap::Impl::
+findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const {
+ assert(false);
+ return Vec3(NaN);
+}
+
+bool ContactGeometry::SmoothHeightMap::Impl::intersectsRay
+ (const Vec3& origin, const UnitVec3& direction,
+ Real& distance, UnitVec3& normal) const
+{
+ assert(false);
+ return true;
+}
+
+Real SmoothHeightMapImplicitFunction::
+calcValue(const Vector& p) const {
+ const BicubicSurface& surf = ownerp->getBicubicSurface();
+ BicubicSurface::PatchHint& hint = ownerp->updHint();
+ const Real z = surf.calcValue(Vec2(p[0],p[1]), hint);
+ //TODO: this is negated from convention
+ return z - p[2]; // negative outside, positive inside
+}
+
+// First deriv with respect to p[2] (z component) is -1 to match the above
+// implicit function definition, all higher derivs are
+// with respect to that component are 0.
+Real SmoothHeightMapImplicitFunction::
+calcDerivative(const Array_<int>& derivComponents, const Vector& p) const {
+ if (derivComponents.empty()) return calcValue(p);
+ if (derivComponents.size() == 1 && derivComponents[0]==2)
+ return -1;
+ for (unsigned i=0; i<derivComponents.size(); ++i)
+ if (derivComponents[i]==2) return 0;
+
+ // We're asking only for derivatives in x and y.
+ const BicubicSurface& surf = ownerp->getBicubicSurface();
+ BicubicSurface::PatchHint& hint = ownerp->updHint();
+ const Real d = surf.calcDerivative(derivComponents, Vec2(p[0],p[1]), hint);
+ return d;
+}
+
+
diff --git a/SimTKmath/Geometry/src/ContactGeometry_Sphere.cpp b/SimTKmath/Geometry/src/ContactGeometry_Sphere.cpp
new file mode 100644
index 0000000..c662dbf
--- /dev/null
+++ b/SimTKmath/Geometry/src/ContactGeometry_Sphere.cpp
@@ -0,0 +1,324 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: Andreas Scholz, Ian Stavness *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_Point.h"
+#include "simmath/internal/Geo_Sphere.h"
+#include "simmath/internal/ContactGeometry.h"
+
+#include "ContactGeometryImpl.h"
+
+#include <iostream>
+#include <cmath>
+#include <map>
+#include <set>
+
+using namespace SimTK;
+using std::map;
+using std::pair;
+using std::set;
+using std::string;
+using std::cout; using std::endl;
+
+
+//==============================================================================
+// CONTACT GEOMETRY :: SPHERE & IMPL
+//==============================================================================
+
+ContactGeometry::Sphere::Sphere(Real radius)
+: ContactGeometry(new Sphere::Impl(radius)) {}
+
+/*static*/ ContactGeometryTypeId ContactGeometry::Sphere::classTypeId()
+{ return ContactGeometry::Sphere::Impl::classTypeId(); }
+
+Real ContactGeometry::Sphere::getRadius() const {
+ return getImpl().getRadius();
+}
+
+void ContactGeometry::Sphere::setRadius(Real radius) {
+ updImpl().setRadius(radius);
+}
+
+const ContactGeometry::Sphere::Impl& ContactGeometry::Sphere::getImpl() const {
+ assert(impl);
+ return static_cast<const Sphere::Impl&>(*impl);
+}
+
+ContactGeometry::Sphere::Impl& ContactGeometry::Sphere::updImpl() {
+ assert(impl);
+ return static_cast<Sphere::Impl&>(*impl);
+}
+
+DecorativeGeometry ContactGeometry::Sphere::Impl::createDecorativeGeometry() const {
+ return DecorativeSphere(radius);
+}
+
+Vec3 ContactGeometry::Sphere::Impl::findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const {
+ inside = (position.normSqr() <= radius*radius);
+ normal = UnitVec3(position); // expensive -- normalizing
+ return normal*radius;
+}
+
+bool ContactGeometry::Sphere::Impl::intersectsRay
+ (const Vec3& origin, const UnitVec3& direction,
+ Real& distance, UnitVec3& normal) const
+{
+ Real b = -~direction*origin;;
+ Real c = origin.normSqr() - radius*radius;
+ if (c > 0) {
+ // Ray origin is outside sphere.
+
+ if (b <= 0)
+ return false; // Ray points away from center of sphere.
+ Real d = b*b - c;
+ if (d < 0)
+ return false;
+ Real root = std::sqrt(d);
+ distance = b - root;
+ }
+ else {
+ // Ray origin is inside sphere.
+
+ Real d = b*b - c;
+ if (d < 0)
+ return false;
+ distance = b + std::sqrt(d);
+ }
+ normal = UnitVec3(origin+distance*direction);
+ return true;
+}
+
+void ContactGeometry::Sphere::Impl::
+getBoundingSphere(Vec3& center, Real& radius) const {
+ center = Vec3(0);
+ radius = this->radius;
+}
+
+void ContactGeometry::Sphere::Impl::
+calcCurvature(const Vec3& point, Vec2& curvature, Rotation& orientation) const {
+ orientation = Rotation(UnitVec3(point), ZAxis, fabs(point[0]) > 0.5 ? Vec3(0, 1, 0) : Vec3(1, 0, 0), XAxis);
+ curvature = 1/radius;
+}
+
+//TODO: just an axis-aligned leaf box for now
+void ContactGeometry::Sphere::Impl::createOBBTree() {
+ OBBNode& root = obbTree.updRoot();
+ root.box.setHalfLengths(Vec3(radius));
+ root.normal = UnitVec3(XAxis); // doesn't matter
+ root.coneHalfAngle = Pi; // has all possible normals
+ root.pointOnSurface = Vec3(radius,0,0); // doesn't matter
+ root.children.clear(); // This is a leaf
+
+ // Leaf contents.
+ root.centerUW = Vec2(0,0);
+ root.dims = Vec2(Pi, Pi/2); // u in [-Pi,Pi], v in [-Pi/2,Pi/2]
+}
+
+
+// Compute geodesic between two points P and Q on a sphere analytically. Since a geodesic on a
+// sphere is a great circle it is parameterized by
+//
+// p(phi) = R * (e1*cos(phi) + e2*sin(phi)) ,
+//
+// where R is the radius of the sphere and the angle phi parameterizes the great circle with
+// respect to an orthonormal basis {e1, e2}. By definition P = p(0) and the geodesic goes from
+// P to Q, where Q = p(angle). Make sure e1 . e2 = 0 and |e1| = |e2| = 1.
+static void setGeodesicToArc(const UnitVec3& e1, const UnitVec3& e2,
+ Real R, Real angle, Geodesic& geod)
+{
+ // Check if e1 and e2 are orthogonal.
+ assert(std::abs(~e1*e2) <= SignificantReal);
+
+ // Clear current geodesic.
+ geod.clear();
+
+ // TODO: Make this generic, so long geodesics are sampled more than short ones.
+ const int numGeodesicSamples = 12;
+
+ // Total arc length and orientation.
+ const Real orientation = Real(sign(angle));
+ const Real L = R*angle*orientation;
+
+ // Increment of phi in loop.
+ const Real deltaPhi = std::abs(angle / Real(numGeodesicSamples-1));
+
+ const Real k = 1/R; // curvature
+ for (int i = 0; i < numGeodesicSamples; ++i){
+ Real phi = Real(i)*angle / Real(numGeodesicSamples-1);
+ const Real sphi = sin(phi), cphi = cos(phi);
+
+ // Trust me, this is already normalized by definition of the input.
+ UnitVec3 n(e1*cphi + e2*sphi, true);
+
+ Vec3 p = R*n;
+
+ // t = dp/dphi, hence pointing into direction of increasing phi.
+ Vec3 t = (-e1*sphi + e2*cphi)*orientation;
+
+ // Though not needed, we use an orthogonalizing constructor for the rotation.
+ geod.addFrenetFrame(Transform(Rotation(n, ZAxis, t, YAxis), p));
+
+ // Current arc length s.
+ Real s = R*Real(i)*deltaPhi;
+ geod.addArcLength(s);
+
+ // Solve the scalar Jacobi equation
+ //
+ // j''(s) + K(s)*j(s) = 0 , (1)
+ //
+ // where K is the Gaussian curvature and (.)' := d(.)/ds denotes differentiation
+ // with respect to the arc length s. Then, j is the directional sensitivity and
+ // we obtain the corresponding variational vector field by multiplying b*j. For
+ // a sphere, K = R^(-2) and the solution of equation (1) becomes
+ //
+ // j = R * sin(1/R * s) (2)
+ // j' = cos(1/R * s) , (3)
+ //
+ // where equation (2) is the standard solution of a non-damped oscillator. Its
+ // period is 2*pi*R and its amplitude is R.
+
+ // Forward directional sensitivity from P to Q
+ Vec2 jPQ(R*sin(k * s), cos(k * s));
+ geod.addDirectionalSensitivityPtoQ(jPQ);
+
+ // Backwards directional sensitivity from Q to P
+ Vec2 jQP(R*sin(k * (L-s)), cos(k * (L-s)));
+ geod.addDirectionalSensitivityQtoP(jQP);
+
+
+ // TODO: positional sensitivity
+ geod.addPositionalSensitivityPtoQ(Vec2(NaN));
+ geod.addPositionalSensitivityQtoP(Vec2(NaN));
+
+ geod.addCurvature(k);
+ }
+ geod.setTorsionAtP(0); geod.setTorsionAtQ(0);
+ geod.setBinormalCurvatureAtP(k); geod.setBinormalCurvatureAtQ(k);
+
+ geod.setIsConvex(true); // Curve on sphere is always convex.
+ geod.setIsShortest(false); // TODO
+ geod.setAchievedAccuracy(SignificantReal); // TODO: accuracy of length?
+// geod.setInitialStepSizeHint(integ.getActualInitialStepSizeTaken()); // TODO
+}
+
+
+void ContactGeometry::Sphere::Impl::
+calcGeodesicAnalytical(const Vec3& xP, const Vec3& xQ,
+ const Vec3& tPhint, const Vec3& tQhint,
+ Geodesic& geod) const
+{
+ // Build an orthonormal basis {e1, e2, e3}.
+ const UnitVec3 e1(xP), e_OQ(xQ);
+ const Vec3 arcAxis = e1 % e_OQ;
+
+ const Real sinAngle = arcAxis.norm();
+ const Real cosAngle = ~e1*e_OQ;
+
+ UnitVec3 e3(arcAxis/sinAngle, true);
+
+ // Tangent vectors tP and tQ at P and Q corresponding to a positive rotation
+ // of the arc around e3.
+ UnitVec3 tP(e3 % e1, true);
+ UnitVec3 tQ(e3 % e_OQ, true);
+
+ // Average moment of of hint vectors applied e3.
+ Real MP = ~(e1 % tPhint)*e3;
+ Real MQ = ~(e_OQ % tQhint)*e3;
+ Real M = (MP + MQ) / 2;
+
+ // Small angle between e_OP and e_OQ corresponding to a short geodesic.
+ Real temp = atan2(sinAngle, cosAngle);
+ Real angleRightHanded, angleLeftHanded;
+
+ // Left-handed angle will always be negative, right-handed angle will be positive.
+ if (temp >= 0) {
+ angleRightHanded = temp;
+ angleLeftHanded = temp - 2*Pi;
+ }
+
+ else {
+ angleLeftHanded = temp;
+ angleRightHanded = temp + 2*Pi;
+ }
+
+ // Orientation of arc. A negative angle means a left-handed rotation around e3.
+ Real angle;
+ if (M >= 0) {
+ angle = angleRightHanded;
+ }
+
+ else {
+ angle = angleLeftHanded;
+ }
+
+ // Create the last unit vector to form the orthonormal basis to describe the arc.
+ UnitVec3 e2(e3 % e1, true);
+
+ setGeodesicToArc(e1, e2, radius, angle, geod);
+}
+
+void ContactGeometry::Sphere::Impl::shootGeodesicInDirectionUntilLengthReachedAnalytical(const Vec3& xP, const UnitVec3& tP,
+ const Real& terminatingLength, const GeodesicOptions& options, Geodesic& geod) const {
+
+ UnitVec3 e_OP(xP);
+ Real angle = terminatingLength/radius;
+
+ setGeodesicToArc(e_OP, tP, radius, angle, geod);
+}
+
+void ContactGeometry::Sphere::Impl::shootGeodesicInDirectionUntilPlaneHitAnalytical(const Vec3& xP, const UnitVec3& tP,
+ const Plane& terminatingPlane, const GeodesicOptions& options,
+ Geodesic& geod) const {
+
+ UnitVec3 e_OP(xP);
+
+ // solve ~( e_OP * cos(t) + tP * sin(t) - pt_on_plane )*plane_normal = 0
+ // for sphere plane offset is zero, therefore pt_on_plane = 0
+ Real a = ~e_OP*terminatingPlane.getNormal();
+ Real b = ~tP*terminatingPlane.getNormal();
+ Real alpha = std::atan2(a,b);
+ Real angle = (alpha > 0 ? Pi-alpha : -alpha);
+// std::cout << "a=" << a << ", b=" << b << ", alpha = " << alpha << std::endl;
+
+ setGeodesicToArc(e_OP, tP, radius, angle, geod);
+}
+
+
+Real SphereImplicitFunction::
+calcValue(const Vector& x) const {
+ return 1-(x[0]*x[0]+x[1]*x[1]+x[2]*x[2])/square(ownerp->getRadius());
+}
+
+Real SphereImplicitFunction::
+calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
+ if (derivComponents.size() == 1)
+ return -2*x[derivComponents[0]]/square(ownerp->getRadius());
+ if ( derivComponents.size() == 2
+ && derivComponents[0] == derivComponents[1])
+ return -2/square(ownerp->getRadius());
+ // A mixed second derivative, or any higher derivative is zero.
+ return 0;
+}
+
diff --git a/SimTKmath/Geometry/src/ContactGeometry_Torus.cpp b/SimTKmath/Geometry/src/ContactGeometry_Torus.cpp
new file mode 100644
index 0000000..1645472
--- /dev/null
+++ b/SimTKmath/Geometry/src/ContactGeometry_Torus.cpp
@@ -0,0 +1,237 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Ian Stavness *
+ * Contributors: Michael Sherman, Andreas Scholz *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_Point.h"
+#include "simmath/internal/Geo_Sphere.h"
+#include "simmath/internal/ContactGeometry.h"
+
+#include "ContactGeometryImpl.h"
+
+#include <iostream>
+#include <cmath>
+#include <map>
+#include <set>
+
+using namespace SimTK;
+using std::map;
+using std::pair;
+using std::set;
+using std::string;
+using std::cout; using std::endl;
+
+
+//==============================================================================
+// CONTACT GEOMETRY :: TORUS & IMPL
+//==============================================================================
+
+ContactGeometry::Torus::Torus(Real torusRadius, Real tubeRadius)
+: ContactGeometry(new Torus::Impl(torusRadius, tubeRadius)) {}
+
+/*static*/ ContactGeometryTypeId ContactGeometry::Torus::classTypeId()
+{ return ContactGeometry::Torus::Impl::classTypeId(); }
+
+Real ContactGeometry::Torus::getTorusRadius() const {
+ return getImpl().getTorusRadius();
+}
+
+void ContactGeometry::Torus::setTorusRadius(Real radius) {
+ updImpl().setTorusRadius(radius);
+}
+
+Real ContactGeometry::Torus::getTubeRadius() const {
+ return getImpl().getTubeRadius();
+}
+
+void ContactGeometry::Torus::setTubeRadius(Real radius) {
+ updImpl().setTubeRadius(radius);
+}
+
+const ContactGeometry::Torus::Impl& ContactGeometry::Torus::getImpl() const {
+ assert(impl);
+ return static_cast<const Torus::Impl&>(*impl);
+}
+
+ContactGeometry::Torus::Impl& ContactGeometry::Torus::updImpl() {
+ assert(impl);
+ return static_cast<Torus::Impl&>(*impl);
+}
+
+DecorativeGeometry ContactGeometry::Torus::Impl::createDecorativeGeometry() const {
+ PolygonalMesh mesh;
+ createPolygonalMesh(mesh);
+ return DecorativeMesh(mesh);
+}
+
+
+Vec3 ContactGeometry::Torus::Impl::
+findNearestPoint(const Vec3& Q, bool& inside, UnitVec3& normal) const {
+
+ UnitVec3 Zdir(0,0,1);
+
+ // find point P on circle in x-y plane that traces the centroid of the torus
+ Vec3 P;
+ Vec3 Qproj = Q - Zdir*(~Q*Zdir);
+ Real normQproj = Qproj.norm();
+
+ if (std::abs(normQproj) < SimTK::Eps) {
+ // Q is along z-axis, therefore there is a locus of closest points,
+ // we arbitrarily choose the one in the x-z plane
+ P = Vec3(torusRadius,0,0);
+ }
+ else {
+ P = Qproj/normQproj*torusRadius;
+ }
+
+ // find direction from centroid of tube to query point, and find near point Qhat
+ UnitVec3 Qdir(Q-P);
+ Vec3 Qhat = P + Qdir*tubeRadius;
+
+ return Qhat;
+}
+
+//TODO
+bool ContactGeometry::Torus::Impl::intersectsRay
+ (const Vec3& origin, const UnitVec3& direction,
+ Real& distance, UnitVec3& normal) const
+{
+ SimTK_ASSERT_ALWAYS(false, "ContactGeometry::Torus::Impl::intersectsRay unimplemented");
+ return false;
+}
+
+void ContactGeometry::Torus::Impl::getBoundingSphere
+ (Vec3& center, Real& radius) const {
+ center = Vec3(0);
+ radius = tubeRadius + torusRadius;
+}
+
+// Create a polygonal mesh for this torus using parameterization as follows:
+// u = [0, 2*Pi] traces a circle in the x-y plane with radius torusRadius,
+// which is the centroid of the torus. A point P on this circle is
+// given by P = torusRadius*~[cos(u) sin(u) 0].
+// v = [0, 2*Pi] traces a circle arond the cross-section (or tube) of the
+// torus with radius tubeRadius, at a given u. A point Q on this circle
+// is given by Q = (torusRadius + tubeRadius*cos(v))*e1 + tubeRadius*(~[0 0 1]*sin(v))
+// where e1 = ~[sin(u) cos(u) 0]. The tube circle is in a plane spanned
+// by e1 and the z-axis.
+void ContactGeometry::Torus::Impl::createPolygonalMesh(PolygonalMesh& mesh) const {
+ // TODO add resolution argument
+ const int numSides = 12; //*resolution;
+ const int numSlices = 36; //*resolution;
+
+ // add vertices
+ for (int i = 0; i < numSlices; ++i) {
+ Real u = Real((i*2*SimTK_PI)/numSlices);
+ UnitVec3 e1(std::sin(u), std::cos(u), 0); // torus circle aligned with z-axis (z-axis through hole)
+ for (int j = 0; j < numSides; ++j) {
+ Real v = Real((j*2*SimTK_PI)/numSides);
+ Vec3 vtx = (torusRadius + tubeRadius*std::cos(v))*e1 + tubeRadius*std::sin(v)*Vec3(0,0,1); // use ZAXIS?
+ mesh.addVertex(vtx);
+ }
+ }
+
+ // add faces, be careful to wrap indices for the last slice
+ int numVertices = mesh.getNumVertices();
+// cout << "num verts = " << numVertices << endl;
+ for (int i = 0; i < numVertices; ++i) {
+// cout << "v" << i << ": " << mesh.getVertexPosition(i) << endl;
+ // define counter-clockwise quad faces
+ Array_<int> faceIndices;
+ faceIndices.push_back(i); // u_i,v_i
+ faceIndices.push_back((i+1)%numVertices); // u_i, v_i+1
+ faceIndices.push_back((i+1+numSides)%numVertices); // u_i+1, v_i+1
+ faceIndices.push_back((i+numSides)%numVertices); // u_i+1, v_i
+ mesh.addFace(faceIndices);
+ }
+
+}
+
+//TODO
+void ContactGeometry::Torus::Impl::
+calcCurvature(const Vec3& point, Vec2& curvature, Rotation& orientation) const {
+ SimTK_ASSERT_ALWAYS(false, "ContactGeometry::Torus::Impl::calcCurvature unimplemented");
+}
+
+//TODO
+Vec3 ContactGeometry::Torus::Impl::
+calcSupportPoint(UnitVec3 direction) const {
+ SimTK_ASSERT_ALWAYS(false, "ContactGeometry::Torus::Impl::calcSupportPoint unimplemented");
+ return Vec3(0);
+}
+
+Real TorusImplicitFunction::
+calcValue(const Vector& x) const {
+ return 1-(square(ownerp->getTorusRadius()-std::sqrt(x[0]*x[0]+x[1]*x[1]))+x[2]*x[2])/
+ square(ownerp->getTubeRadius());
+}
+
+Real TorusImplicitFunction::
+calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
+ // first derivatives
+ if (derivComponents.size() == 1) {
+ if (derivComponents[0]<2) {
+ Real sqrt_xy = std::sqrt(x[0]*x[0] + x[1]*x[1]);
+ return 2*x[derivComponents[0]]*(ownerp->getTorusRadius() - sqrt_xy)/
+ (square(ownerp->getTubeRadius())*sqrt_xy);
+ }
+ else
+ return -2*x[2]/square(ownerp->getTubeRadius());
+ }
+
+ // second derivatives
+ if (derivComponents.size() == 2) {
+ if (derivComponents[0] < 2) { // fx_ fy_
+ if (derivComponents[1] < 2) {
+ Real tubeRadiusSq = square(ownerp->getTubeRadius());
+ Real xy = x[0]*x[0] + x[1]*x[1];
+ Real sqrt_xy = std::sqrt(xy);
+ Real den = tubeRadiusSq*xy*sqrt_xy;
+ if (derivComponents[0]==derivComponents[1]) { // fxx or fyy
+ int idx = derivComponents[1]==0; // if 0 then idx=1, if 1 then idx=0
+ Real num = 2*ownerp->getTorusRadius()*x[idx]*x[idx];
+ return num/den - 2/tubeRadiusSq;
+ }
+ else { // fxy or fyx
+ return - 2*ownerp->getTorusRadius()*x[0]*x[1]/den;
+ }
+ }
+ else // fxz = fyz = 0
+ return 0;
+ }
+ else { // fz_
+ if (derivComponents[1] == 2) // fzz
+ return -2/square(ownerp->getTubeRadius());
+ else // fzx = fzy = 0
+ return 0;
+ }
+ }
+
+ //TODO higher order derivatives
+ SimTK_ASSERT1_ALWAYS(!"derivative not implemented",
+ "Implicit Torus implements 1st&2nd derivs only but %d deriv requested.",
+ derivComponents.size());
+ return 0;
+}
+
diff --git a/SimTKmath/Geometry/src/ContactGeometry_TriangleMesh.cpp b/SimTKmath/Geometry/src/ContactGeometry_TriangleMesh.cpp
new file mode 100644
index 0000000..064f816
--- /dev/null
+++ b/SimTKmath/Geometry/src/ContactGeometry_TriangleMesh.cpp
@@ -0,0 +1,1028 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_Point.h"
+#include "simmath/internal/Geo_Sphere.h"
+#include "simmath/internal/ContactGeometry.h"
+
+#include "ContactGeometryImpl.h"
+
+#include <iostream>
+#include <cmath>
+#include <map>
+#include <set>
+
+using namespace SimTK;
+using std::map;
+using std::pair;
+using std::set;
+using std::string;
+using std::cout; using std::endl;
+
+//==============================================================================
+// CONTACT GEOMETRY :: TRIANGLE MESH
+//==============================================================================
+
+ContactGeometry::TriangleMesh::TriangleMesh
+ (const ArrayViewConst_<Vec3>& vertices,
+ const ArrayViewConst_<int>& faceIndices, bool smooth)
+: ContactGeometry(new TriangleMesh::Impl(vertices, faceIndices, smooth)) {}
+
+ContactGeometry::TriangleMesh::TriangleMesh
+ (const PolygonalMesh& mesh, bool smooth)
+: ContactGeometry(new TriangleMesh::Impl(mesh, smooth)) {}
+
+/*static*/ ContactGeometryTypeId ContactGeometry::TriangleMesh::classTypeId()
+{ return ContactGeometry::TriangleMesh::Impl::classTypeId(); }
+
+
+int ContactGeometry::TriangleMesh::getNumEdges() const {
+ return getImpl().edges.size();
+}
+
+int ContactGeometry::TriangleMesh::getNumFaces() const {
+ return getImpl().faces.size();
+}
+
+int ContactGeometry::TriangleMesh::getNumVertices() const {
+ return getImpl().vertices.size();
+}
+
+const Vec3& ContactGeometry::TriangleMesh::getVertexPosition(int index) const {
+ assert(index >= 0 && index < getNumVertices());
+ return getImpl().vertices[index].pos;
+}
+
+int ContactGeometry::TriangleMesh::getFaceEdge(int face, int edge) const {
+ assert(face >= 0 && face < getNumFaces());
+ assert(edge >= 0 && edge < 3);
+ return getImpl().faces[face].edges[edge];
+}
+
+int ContactGeometry::TriangleMesh::getFaceVertex(int face, int vertex) const {
+ assert(face >= 0 && face < getNumFaces());
+ assert(vertex >= 0 && vertex < 3);
+ return getImpl().faces[face].vertices[vertex];
+}
+
+int ContactGeometry::TriangleMesh::getEdgeFace(int edge, int face) const {
+ assert(edge >= 0 && edge < getNumEdges());
+ assert(face >= 0 && face < 2);
+ return getImpl().edges[edge].faces[face];
+}
+
+int ContactGeometry::TriangleMesh::getEdgeVertex(int edge, int vertex) const {
+ assert(edge >= 0 && edge < getNumEdges());
+ assert(vertex >= 0 && vertex < 2);
+ return getImpl().edges[edge].vertices[vertex];
+}
+
+const UnitVec3& ContactGeometry::TriangleMesh::getFaceNormal(int face) const {
+ assert(face >= 0 && face < getNumFaces());
+ return getImpl().faces[face].normal;
+}
+
+Real ContactGeometry::TriangleMesh::getFaceArea(int face) const {
+ assert(face >= 0 && face < getNumFaces());
+ return getImpl().faces[face].area;
+}
+
+void ContactGeometry::TriangleMesh::
+findVertexEdges(int vertex, Array_<int>& edges) const {
+ // Begin at an arbitrary edge which intersects the vertex.
+
+ int firstEdge = getImpl().vertices[vertex].firstEdge;
+ int previousEdge = firstEdge;
+ int previousFace = getImpl().edges[firstEdge].faces[0];
+
+ // Walk around the vertex, using each edge to find the next face and each
+ // face to find the next edge.
+
+ do {
+ edges.push_back(previousEdge);
+ const ContactGeometry::TriangleMesh::Impl::Edge&
+ edge = getImpl().edges[previousEdge];
+ int nextFace = (edge.faces[0] == previousFace ? edge.faces[1]
+ : edge.faces[0]);
+ const ContactGeometry::TriangleMesh::Impl::Face&
+ face = getImpl().faces[nextFace];
+ int nextEdge;
+ if ( face.edges[0] != previousEdge
+ && (face.vertices[0] == vertex || face.vertices[1] == vertex))
+ nextEdge = face.edges[0];
+ else if ( face.edges[1] != previousEdge
+ && (face.vertices[1] == vertex || face.vertices[2] == vertex))
+ nextEdge = face.edges[1];
+ else
+ nextEdge = face.edges[2];
+ previousEdge = nextEdge;
+ previousFace = nextFace;
+ } while (previousEdge != firstEdge);
+}
+
+Vec3 ContactGeometry::TriangleMesh::findPoint(int face, const Vec2& uv) const {
+ return getImpl().findPoint(face, uv);
+}
+
+Vec3 ContactGeometry::TriangleMesh::findCentroid(int face) const {
+ return getImpl().findCentroid(face);
+}
+
+UnitVec3 ContactGeometry::TriangleMesh::
+findNormalAtPoint(int face, const Vec2& uv) const {
+ return getImpl().findNormalAtPoint(face, uv);
+}
+
+Vec3 ContactGeometry::TriangleMesh::findNearestPoint
+ (const Vec3& position, bool& inside, UnitVec3& normal) const {
+ return getImpl().findNearestPoint(position, inside, normal);
+}
+
+Vec3 ContactGeometry::TriangleMesh::findNearestPoint
+ (const Vec3& position, bool& inside, int& face, Vec2& uv) const {
+ return getImpl().findNearestPoint(position, inside, face, uv);
+}
+
+Vec3 ContactGeometry::TriangleMesh::findNearestPointToFace
+ (const Vec3& position, int face, Vec2& uv) const {
+ return getImpl().findNearestPointToFace(position, face, uv);
+}
+
+bool ContactGeometry::TriangleMesh::intersectsRay
+ (const Vec3& origin, const UnitVec3& direction, Real& distance,
+ UnitVec3& normal) const {
+ return getImpl().intersectsRay(origin, direction, distance, normal);
+}
+
+bool ContactGeometry::TriangleMesh::intersectsRay
+ (const Vec3& origin, const UnitVec3& direction, Real& distance, int& face,
+ Vec2& uv) const {
+ return getImpl().intersectsRay(origin, direction, distance, face, uv);
+}
+
+ContactGeometry::TriangleMesh::OBBTreeNode
+ContactGeometry::TriangleMesh::getOBBTreeNode() const {
+ return OBBTreeNode(getImpl().obb);
+}
+
+PolygonalMesh ContactGeometry::TriangleMesh::createPolygonalMesh() const {
+ PolygonalMesh mesh;
+ getImpl().createPolygonalMesh(mesh);
+ return mesh;
+}
+
+const ContactGeometry::TriangleMesh::Impl&
+ContactGeometry::TriangleMesh::getImpl() const {
+ assert(impl);
+ return static_cast<const TriangleMesh::Impl&>(*impl);
+}
+
+ContactGeometry::TriangleMesh::Impl&
+ContactGeometry::TriangleMesh::updImpl() {
+ assert(impl);
+ return static_cast<TriangleMesh::Impl&>(*impl);
+}
+
+
+
+//==============================================================================
+// CONTACT GEOMETRY :: TRIANGLE MESH :: IMPL
+//==============================================================================
+
+DecorativeGeometry ContactGeometry::TriangleMesh::Impl::createDecorativeGeometry() const {
+ PolygonalMesh mesh;
+ createPolygonalMesh(mesh);
+ return DecorativeMesh(mesh);
+}
+
+Vec3 ContactGeometry::TriangleMesh::Impl::findPoint
+ (int face, const Vec2& uv) const {
+ const Face& f = faces[face];
+ return uv[0] * vertices[f.vertices[0]].pos
+ + uv[1] * vertices[f.vertices[1]].pos
+ + (1-uv[0]-uv[1])* vertices[f.vertices[2]].pos;
+}
+
+// same as findPoint(face, (1/3,1/3)) but faster
+Vec3 ContactGeometry::TriangleMesh::Impl::findCentroid(int face) const {
+ const Face& f = faces[face];
+ return ( vertices[f.vertices[0]].pos
+ + vertices[f.vertices[1]].pos
+ + vertices[f.vertices[2]].pos) / 3;
+}
+
+UnitVec3 ContactGeometry::TriangleMesh::Impl::findNormalAtPoint
+ (int face, const Vec2& uv) const {
+ const Face& f = faces[face];
+ if (smooth)
+ return UnitVec3( uv[0] * vertices[f.vertices[0]].normal
+ + uv[1] * vertices[f.vertices[1]].normal
+ + (1-uv[0]-uv[1])* vertices[f.vertices[2]].normal);
+ return f.normal;
+}
+
+Vec3 ContactGeometry::TriangleMesh::Impl::
+findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const {
+ int face;
+ Vec2 uv;
+ Vec3 nearestPoint = findNearestPoint(position, inside, face, uv);
+ normal = findNormalAtPoint(face, uv);
+ return nearestPoint;
+}
+
+Vec3 ContactGeometry::TriangleMesh::Impl::
+findNearestPoint(const Vec3& position, bool& inside, int& face, Vec2& uv) const
+{
+ Real distance2;
+ Vec3 nearestPoint = obb.findNearestPoint(*this, position, MostPositiveReal, distance2, face, uv);
+ Vec3 delta = position-nearestPoint;
+ inside = (~delta*faces[face].normal < 0);
+ return nearestPoint;
+}
+
+bool ContactGeometry::TriangleMesh::Impl::
+intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance,
+ UnitVec3& normal) const {
+ int face;
+ Vec2 uv;
+ if (!intersectsRay(origin, direction, distance, face, uv))
+ return false;
+ normal = findNormalAtPoint(face, uv);
+ return true;
+}
+
+bool ContactGeometry::TriangleMesh::Impl::
+intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance,
+ int& face, Vec2& uv) const {
+ Real boundsDistance;
+ if (!obb.bounds.intersectsRay(origin, direction, boundsDistance))
+ return false;
+ return obb.intersectsRay(*this, origin, direction, distance, face, uv);
+}
+
+void ContactGeometry::TriangleMesh::Impl::
+getBoundingSphere(Vec3& center, Real& radius) const {
+ center = boundingSphereCenter;
+ radius = boundingSphereRadius;
+}
+
+void ContactGeometry::TriangleMesh::Impl::
+createPolygonalMesh(PolygonalMesh& mesh) const {
+ for (unsigned vx=0; vx < vertices.size(); ++vx)
+ mesh.addVertex(vertices[vx].pos);
+ for (unsigned fx=0; fx < faces.size(); ++fx) {
+ const Face& face = faces[fx];
+ const ArrayViewConst_<int> verts(face.vertices, face.vertices+3);
+ mesh.addFace(verts);
+ }
+}
+
+ContactGeometry::TriangleMesh::Impl::Impl
+ (const ArrayViewConst_<Vec3>& vertexPositions,
+ const ArrayViewConst_<int>& faceIndices, bool smooth)
+: ContactGeometryImpl(), smooth(smooth) {
+ init(vertexPositions, faceIndices);
+}
+
+ContactGeometry::TriangleMesh::Impl::Impl
+ (const PolygonalMesh& mesh, bool smooth)
+: ContactGeometryImpl(), smooth(smooth)
+{ // Create the mesh, triangulating faces as necessary.
+ Array_<Vec3> vertexPositions;
+ Array_<int> faceIndices;
+ for (int i = 0; i < mesh.getNumVertices(); i++)
+ vertexPositions.push_back(mesh.getVertexPosition(i));
+ for (int i = 0; i < mesh.getNumFaces(); i++) {
+ int numVert = mesh.getNumVerticesForFace(i);
+ if (numVert < 3)
+ continue; // Ignore it.
+ if (numVert == 3) {
+ faceIndices.push_back(mesh.getFaceVertex(i, 0));
+ faceIndices.push_back(mesh.getFaceVertex(i, 1));
+ faceIndices.push_back(mesh.getFaceVertex(i, 2));
+ }
+ else if (numVert == 4) {
+ // Split it into two triangles.
+
+ faceIndices.push_back(mesh.getFaceVertex(i, 0));
+ faceIndices.push_back(mesh.getFaceVertex(i, 1));
+ faceIndices.push_back(mesh.getFaceVertex(i, 2));
+ faceIndices.push_back(mesh.getFaceVertex(i, 2));
+ faceIndices.push_back(mesh.getFaceVertex(i, 3));
+ faceIndices.push_back(mesh.getFaceVertex(i, 0));
+ }
+ else {
+ // Add a vertex at the center, then split it into triangles.
+
+ Vec3 center(0);
+ for (int j = 0; j < numVert; j++)
+ center += vertexPositions[mesh.getFaceVertex(i, j)];
+ center /= numVert;
+ vertexPositions.push_back(center);
+ int newIndex = vertexPositions.size()-1;
+ for (int j = 0; j < numVert-1; j++) {
+ faceIndices.push_back(mesh.getFaceVertex(i, j));
+ faceIndices.push_back(mesh.getFaceVertex(i, j+1));
+ faceIndices.push_back(newIndex);
+ }
+ }
+ }
+ init(vertexPositions, faceIndices);
+
+ // Make sure the mesh normals are oriented correctly.
+
+ Vec3 origin(0);
+ for (int i = 0; i < 3; i++)
+ origin += vertices[faces[0].vertices[i]].pos;
+ origin /= 3; // this is the face centroid
+
+ const UnitVec3 direction = -faces[0].normal;
+ // Calculate a ray origin that is guaranteed to be outside the
+ // mesh. If the topology is right (face 0 normal points outward), we'll be
+ // outside on the side containing face 0. If it is wrong, we'll be outside
+ // on the opposite side of the mesh. Then we'll shoot a ray back along the
+ // direction we came from (that is, towards the interior of the mesh from
+ // outside). We'll hit *some* face. If the topology is right, the hit
+ // face's normal will be pointing back at us. If it is wrong, the face
+ // normal will also be pointing inwards, in roughly the same direction as
+ // the ray.
+ origin -= max(obb.bounds.getSize())*direction;
+ Real distance;
+ int face;
+ Vec2 uv;
+ bool intersects = intersectsRay(origin, direction, distance, face, uv);
+ assert(intersects);
+ // Now dot the hit face normal with the ray direction; correct topology
+ // will have them pointing in more-or-less opposite directions.
+ if (dot(faces[face].normal, direction) > 0) {
+ // We need to invert the mesh topology.
+
+ for (int i = 0; i < (int) faces.size(); i++) {
+ Face& f = faces[i];
+ int temp = f.vertices[0];
+ f.vertices[0] = f.vertices[1];
+ f.vertices[1] = temp;
+ temp = f.edges[1];
+ f.edges[1] = f.edges[2];
+ f.edges[2] = temp;
+ f.normal *= -1;
+ }
+ for (int i = 0; i < (int) vertices.size(); i++)
+ vertices[i].normal *= -1;
+ }
+}
+
+void ContactGeometry::TriangleMesh::Impl::init
+ (const Array_<Vec3>& vertexPositions, const Array_<int>& faceIndices)
+{ SimTK_APIARGCHECK_ALWAYS(faceIndices.size()%3 == 0,
+ "ContactGeometry::TriangleMesh::Impl", "TriangleMesh::Impl",
+ "The number of indices must be a multiple of 3.");
+ int numFaces = faceIndices.size()/3;
+
+ // Create the vertices.
+
+ for (int i = 0; i < (int) vertexPositions.size(); i++)
+ vertices.push_back(Vertex(vertexPositions[i]));
+
+ // Create the faces and build lists of all the edges.
+
+ map<pair<int, int>, int> forwardEdges;
+ map<pair<int, int>, int> backwardEdges;
+ for (int i = 0; i < numFaces; i++) {
+ int start = i*3;
+ int v1 = faceIndices[start], v2 = faceIndices[start+1],
+ v3 = faceIndices[start+2];
+ SimTK_APIARGCHECK1_ALWAYS
+ ( v1 >= 0 && v1 < (int) vertices.size()
+ && v2 >= 0 && v2 < (int) vertices.size()
+ && v3 >= 0 && v3 < (int) vertices.size(),
+ "ContactGeometry::TriangleMesh::Impl", "TriangleMesh::Impl",
+ "Face %d contains a vertex with an illegal index.", i);
+ Vec3 cross = (vertexPositions[v2]-vertexPositions[v1])
+ % (vertexPositions[v3]-vertexPositions[v1]);
+ Real norm = cross.norm();
+ cross *= Real(1)/norm;
+ SimTK_APIARGCHECK1_ALWAYS(norm > 0,
+ "ContactGeometry::TriangleMesh::Impl", "TriangleMesh::Impl",
+ "Face %d is degenerate.", i);
+ faces.push_back(Face(v1, v2, v3, cross, norm/2));
+ int edges[3][2] = {{v1, v2}, {v2, v3}, {v3, v1}};
+ for (int j = 0; j < 3; j++) {
+ SimTK_APIARGCHECK1_ALWAYS(edges[j][0] != edges[j][1],
+ "ContactGeometry::TriangleMesh::Impl", "TriangleMesh::Impl",
+ "Vertices %d appears twice in a single face.", edges[j][0]);
+ if (edges[j][0] < edges[j][1]) {
+ SimTK_APIARGCHECK2_ALWAYS
+ (forwardEdges.find(pair<int, int>(edges[j][0], edges[j][1]))
+ == forwardEdges.end(),
+ "ContactGeometry::TriangleMesh::Impl", "TriangleMesh::Impl",
+ "Multiple faces have an edge between vertices %d and %d"
+ " in the same order.", edges[j][0], edges[j][1]);
+ forwardEdges[pair<int, int>(edges[j][0], edges[j][1])] = i;
+ }
+ else {
+ SimTK_APIARGCHECK2_ALWAYS
+ (backwardEdges.find(pair<int, int>(edges[j][1], edges[j][0]))
+ == backwardEdges.end(),
+ "ContactGeometry::TriangleMesh::Impl", "TriangleMesh::Impl",
+ "Multiple faces have an edge between vertices %d and %d"
+ " in the same order.", edges[j][1], edges[j][0]);
+ backwardEdges[pair<int, int>(edges[j][1], edges[j][0])] = i;
+ }
+ }
+ }
+
+ // Create the edges.
+
+ SimTK_APIARGCHECK_ALWAYS(forwardEdges.size() == backwardEdges.size(),
+ "ContactGeometry::TriangleMesh::Impl", "TriangleMesh::Impl",
+ "Each edge must be shared by exactly two faces.");
+ for (map<pair<int, int>, int>::iterator iter = forwardEdges.begin();
+ iter != forwardEdges.end(); ++iter) {
+ int vert1 = iter->first.first;
+ int vert2 = iter->first.second;
+ int face1 = iter->second;
+ map<pair<int, int>, int>::iterator iter2 =
+ backwardEdges.find(pair<int, int>(vert1, vert2));
+ SimTK_APIARGCHECK_ALWAYS(iter2 != backwardEdges.end(),
+ "ContactGeometry::TriangleMesh::Impl", "TriangleMesh::Impl",
+ "Each edge must be shared by exactly two faces.");
+ int face2 = iter2->second;
+ edges.push_back(Edge(vert1, vert2, face1, face2));
+ }
+
+ // Record the edges for each face.
+
+ for (int i = 0; i < (int) edges.size(); i++) {
+ Edge& edge = edges[i];
+ int f[2] = {edge.faces[0], edge.faces[1]};
+ for (int j = 0; j < 2; j++) {
+ Face& face = faces[f[j]];
+ if ((edge.vertices[0] == face.vertices[0] || edge.vertices[0] == face.vertices[1]) &&
+ (edge.vertices[1] == face.vertices[0] || edge.vertices[1] == face.vertices[1]))
+ face.edges[0] = i;
+ else if ((edge.vertices[0] == face.vertices[1] || edge.vertices[0] == face.vertices[2]) &&
+ (edge.vertices[1] == face.vertices[1] || edge.vertices[1] == face.vertices[2]))
+ face.edges[1] = i;
+ else if ((edge.vertices[0] == face.vertices[2] || edge.vertices[0] == face.vertices[0]) &&
+ (edge.vertices[1] == face.vertices[2] || edge.vertices[1] == face.vertices[0]))
+ face.edges[2] = i;
+ else
+ SimTK_ASSERT_ALWAYS(false,
+ "Face and edge vertices are inconsistent.");
+ }
+ }
+
+ // Record a single edge for each vertex.
+
+ for (int i = 0; i < (int) edges.size(); i++) {
+ vertices[edges[i].vertices[0]].firstEdge = i;
+ vertices[edges[i].vertices[1]].firstEdge = i;
+ }
+ for (int i = 0; i < (int) vertices.size(); i++)
+ SimTK_APIARGCHECK1_ALWAYS(vertices[i].firstEdge >= 0,
+ "ContactGeometry::TriangleMesh::Impl", "TriangleMesh::Impl",
+ "Vertex %d is not part of any face.", i);
+
+ // Calculate a normal for each vertex.
+
+ Vector_<Vec3> vertNorm(vertices.size(), Vec3(0));
+ for (int i = 0; i < (int) faces.size(); i++) {
+ const Face& f = faces[i];
+ UnitVec3 edgeDir[3];
+ for (int j = 0; j < 3; j++) {
+ edgeDir[j] = UnitVec3( vertices[f.vertices[(j+1)%3]].pos
+ - vertices[f.vertices[j]].pos);
+ }
+ for (int j = 0; j < 3; j++) {
+ Real angle = std::acos(~edgeDir[j]*edgeDir[(j+2)%3]);
+ vertNorm[f.vertices[j]] += f.normal*angle;
+ }
+ }
+ for (int i = 0; i < (int) vertices.size(); i++)
+ vertices[i].normal = UnitVec3(vertNorm[i]);
+
+ // Create the OBBTree.
+
+ Array_<int> allFaces(faces.size());
+ for (int i = 0; i < (int) allFaces.size(); i++)
+ allFaces[i] = i;
+ createObbTree(obb, allFaces);
+
+ // Find the bounding sphere.
+ Array_<const Vec3*> points(vertices.size());
+ for (int i = 0; i < (int) vertices.size(); i++)
+ points[i] = &vertices[i].pos;
+ const Geo::Sphere bnd = Geo::Point::calcBoundingSphereIndirect(points);
+ boundingSphereCenter = bnd.getCenter();
+ boundingSphereRadius = bnd.getRadius();
+}
+
+void ContactGeometry::TriangleMesh::Impl::createObbTree
+ (OBBTreeNodeImpl& node, const Array_<int>& faceIndices)
+{ // Find all vertices in the node and build the OrientedBoundingBox.
+ node.numTriangles = faceIndices.size();
+ set<int> vertexIndices;
+ for (int i = 0; i < (int) faceIndices.size(); i++)
+ for (int j = 0; j < 3; j++)
+ vertexIndices.insert(faces[faceIndices[i]].vertices[j]);
+ Vector_<Vec3> points((int)vertexIndices.size());
+ int index = 0;
+ for (set<int>::iterator iter = vertexIndices.begin();
+ iter != vertexIndices.end(); ++iter)
+ points[index++] = vertices[*iter].pos;
+ node.bounds = OrientedBoundingBox(points);
+ if (faceIndices.size() > 3) {
+
+ // Order the axes by size.
+
+ int axisOrder[3];
+ const Vec3& size = node.bounds.getSize();
+ if (size[0] > size[1]) {
+ if (size[0] > size[2]) {
+ axisOrder[0] = 0;
+ if (size[1] > size[2]) {
+ axisOrder[1] = 1;
+ axisOrder[2] = 2;
+ }
+ else {
+ axisOrder[1] = 2;
+ axisOrder[2] = 1;
+ }
+ }
+ else {
+ axisOrder[0] = 2;
+ axisOrder[1] = 0;
+ axisOrder[2] = 1;
+ }
+ }
+ else if (size[0] > size[2]) {
+ axisOrder[0] = 1;
+ axisOrder[1] = 0;
+ axisOrder[2] = 2;
+ }
+ else {
+ if (size[1] > size[2]) {
+ axisOrder[0] = 1;
+ axisOrder[1] = 2;
+ }
+ else {
+ axisOrder[0] = 2;
+ axisOrder[1] = 1;
+ }
+ axisOrder[2] = 0;
+ }
+
+ // Try splitting along each axis.
+
+ for (int i = 0; i < 3; i++) {
+ Array_<int> child1Indices, child2Indices;
+ splitObbAxis(faceIndices, child1Indices, child2Indices,
+ axisOrder[i]);
+ if (child1Indices.size() > 0 && child2Indices.size() > 0) {
+ // It was successfully split, so create the child nodes.
+
+ node.child1 = new OBBTreeNodeImpl();
+ node.child2 = new OBBTreeNodeImpl();
+ createObbTree(*node.child1, child1Indices);
+ createObbTree(*node.child2, child2Indices);
+ return;
+ }
+ }
+ }
+
+ // This is a leaf node.
+
+ node.triangles.insert(node.triangles.begin(), faceIndices.begin(),
+ faceIndices.end());
+}
+
+void ContactGeometry::TriangleMesh::Impl::splitObbAxis
+ (const Array_<int>& parentIndices, Array_<int>& child1Indices,
+ Array_<int>& child2Indices, int axis)
+{ // For each face, find its minimum and maximum extent along the axis.
+ Vector minExtent(parentIndices.size());
+ Vector maxExtent(parentIndices.size());
+ for (int i = 0; i < (int) parentIndices.size(); i++) {
+ int* vertexIndices = faces[parentIndices[i]].vertices;
+ Real minVal = vertices[vertexIndices[0]].pos[axis];
+ Real maxVal = vertices[vertexIndices[0]].pos[axis];
+ minVal = std::min(minVal, vertices[vertexIndices[1]].pos[axis]);
+ maxVal = std::max(maxVal, vertices[vertexIndices[1]].pos[axis]);
+ minExtent[i] = std::min(minVal, vertices[vertexIndices[2]].pos[axis]);
+ maxExtent[i] = std::max(maxVal, vertices[vertexIndices[2]].pos[axis]);
+ }
+
+ // Select a split point that tries to put as many faces as possible
+ // entirely on one side or the other.
+
+ Real split = (median(minExtent)+median(maxExtent)) / 2;
+
+ // Choose a side for each face.
+
+ for (int i = 0; i < (int) parentIndices.size(); i++) {
+ if (maxExtent[i] <= split)
+ child1Indices.push_back(parentIndices[i]);
+ else if (minExtent[i] >= split)
+ child2Indices.push_back(parentIndices[i]);
+ else if (0.5*(minExtent[i]+maxExtent[i]) <= split)
+ child1Indices.push_back(parentIndices[i]);
+ else
+ child2Indices.push_back(parentIndices[i]);
+ }
+}
+
+Vec3 ContactGeometry::TriangleMesh::Impl::findNearestPointToFace
+ (const Vec3& position, int face, Vec2& uv) const {
+ // Calculate the distance between a point in space and a face of the mesh.
+ // This algorithm is based on a description by David Eberly found at
+ // http://www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf.
+
+ const ContactGeometry::TriangleMesh::Impl::Face& fc = faces[face];
+ const Vec3& vert1 = vertices[fc.vertices[0]].pos;
+ const Vec3& vert2 = vertices[fc.vertices[1]].pos;
+ const Vec3& vert3 = vertices[fc.vertices[2]].pos;
+ const Vec3 e0 = vert2-vert1;
+ const Vec3 e1 = vert3-vert1;
+ const Vec3 delta = vert1-position;
+ const Real a = e0.normSqr();
+ const Real b = ~e0*e1;
+ const Real c = e1.normSqr();
+ const Real d = ~e0*delta;
+ const Real e = ~e1*delta;
+ const Real f = delta.normSqr();
+ const Real det = a*c-b*b;
+ Real s = b*e-c*d;
+ Real t = b*d-a*e;
+ if (s+t <= det) {
+ if (s < 0) {
+ if (t < 0) {
+ // Region 4
+
+ if (d < 0) {
+ s = (-d >= a ? 1 : -d/a);
+ t = 0;
+ }
+ else {
+ s = 0;
+ t = (e >= 0 ? 0 : (-e >= c ? 1 : -e/c));
+ }
+ }
+ else {
+ // Region 3
+
+ s = 0;
+ t = (e >= 0 ? 0 : (-e >= c ? 1 : -e/c));
+ }
+ }
+ else if (t < 0) {
+ // Region 5
+
+ s = (d >= 0 ? 0 : (-d >= a ? 1 : -d/a));
+ t = 0;
+ }
+ else {
+ // Region 0
+
+ const Real invDet = Real(1)/det;
+ s *= invDet;
+ t *= invDet;
+ }
+ }
+ else {
+ if (s < 0) {
+ // Region 2
+
+ Real temp0 = b+d;
+ Real temp1 = c+e;
+ if (temp1 > temp0) {
+ Real numer = temp1-temp0;
+ Real denom = a-2*b+c;
+ s = (numer >= denom ? 1 : numer/denom);
+ t = 1-s;
+ }
+ else {
+ s = 0;
+ t = (temp1 <= 0 ? 1 : (e >= 0 ? 0 : -e/c));
+ }
+ }
+ else if (t < 0) {
+ // Region 6
+
+ Real temp0 = b+e;
+ Real temp1 = a+d;
+ if (temp1 > temp0) {
+ Real numer = temp1-temp0;
+ Real denom = a-2*b+c;
+ t = (numer >= denom ? 1 : numer/denom);
+ s = 1-t;
+ }
+ else {
+ s = (temp1 <= 0 ? 1 : (e >= 0 ? 0 : -d/a));
+ t = 0;
+ }
+ }
+ else {
+ // Region 1
+
+ const Real numer = c+e-b-d;
+ if (numer <= 0)
+ s = 0;
+ else {
+ const Real denom = a-2*b+c;
+ s = (numer >= denom ? 1 : numer/denom);
+ }
+ t = 1-s;
+ }
+ }
+ uv = Vec2(1-s-t, s);
+ return vert1 + s*e0 + t*e1;
+}
+
+
+//==============================================================================
+// OBB TREE NODE IMPL
+//==============================================================================
+
+OBBTreeNodeImpl::OBBTreeNodeImpl(const OBBTreeNodeImpl& copy)
+: bounds(copy.bounds), triangles(copy.triangles),
+ numTriangles(copy.numTriangles) {
+ if (copy.child1 == NULL) {
+ child1 = NULL;
+ child2 = NULL;
+ }
+ else {
+ child1 = new OBBTreeNodeImpl(*copy.child1);
+ child2 = new OBBTreeNodeImpl(*copy.child2);
+ }
+}
+
+OBBTreeNodeImpl::~OBBTreeNodeImpl() {
+ if (child1 != NULL)
+ delete child1;
+ if (child2 != NULL)
+ delete child2;
+}
+
+Vec3 OBBTreeNodeImpl::findNearestPoint
+ (const ContactGeometry::TriangleMesh::Impl& mesh,
+ const Vec3& position, Real cutoff2,
+ Real& distance2, int& face, Vec2& uv) const
+{
+ Real tol = 100*Eps;
+ if (child1 != NULL) {
+ // Recursively check the child nodes.
+
+ Real child1distance2 = MostPositiveReal,
+ child2distance2 = MostPositiveReal;
+ int child1face, child2face;
+ Vec2 child1uv, child2uv;
+ Vec3 child1point, child2point;
+ Real child1BoundsDist2 =
+ (child1->bounds.findNearestPoint(position)-position).normSqr();
+ Real child2BoundsDist2 =
+ (child2->bounds.findNearestPoint(position)-position).normSqr();
+ if (child1BoundsDist2 < child2BoundsDist2) {
+ if (child1BoundsDist2 < cutoff2) {
+ child1point = child1->findNearestPoint(mesh, position, cutoff2, child1distance2, child1face, child1uv);
+ if (child2BoundsDist2 < child1distance2 && child2BoundsDist2 < cutoff2)
+ child2point = child2->findNearestPoint(mesh, position, cutoff2, child2distance2, child2face, child2uv);
+ }
+ }
+ else {
+ if (child2BoundsDist2 < cutoff2) {
+ child2point = child2->findNearestPoint(mesh, position, cutoff2, child2distance2, child2face, child2uv);
+ if (child1BoundsDist2 < child2distance2 && child1BoundsDist2 < cutoff2)
+ child1point = child1->findNearestPoint(mesh, position, cutoff2, child1distance2, child1face, child1uv);
+ }
+ }
+ if ( child1distance2 <= child2distance2*(1+tol)
+ && child2distance2 <= child1distance2*(1+tol)) {
+ // Decide based on angle which one to use.
+
+ if ( std::abs(~(child1point-position)*mesh.faces[child1face].normal)
+ > std::abs(~(child2point-position)*mesh.faces[child2face].normal))
+ child2distance2 = MostPositiveReal;
+ else
+ child1distance2 = MostPositiveReal;
+ }
+ if (child1distance2 < child2distance2) {
+ distance2 = child1distance2;
+ face = child1face;
+ uv = child1uv;
+ return child1point;
+ }
+ else {
+ distance2 = child2distance2;
+ face = child2face;
+ uv = child2uv;
+ return child2point;
+ }
+ }
+ // This is a leaf node, so check each triangle for its distance to the point.
+
+ distance2 = MostPositiveReal;
+ Vec3 nearestPoint;
+ for (int i = 0; i < (int) triangles.size(); i++) {
+ Vec2 triangleUV;
+ Vec3 p = mesh.findNearestPointToFace(position, triangles[i], triangleUV);
+ Vec3 offset = p-position;
+ // TODO: volatile to work around compiler bug
+ volatile Real d2 = offset.normSqr();
+ if (d2 < distance2 || (d2 < distance2*(1+tol) && std::abs(~offset*mesh.faces[triangles[i]].normal) > std::abs(~offset*mesh.faces[face].normal))) {
+ nearestPoint = p;
+ distance2 = d2;
+ face = triangles[i];
+ uv = triangleUV;
+ }
+ }
+ return nearestPoint;
+}
+
+bool OBBTreeNodeImpl::
+intersectsRay(const ContactGeometry::TriangleMesh::Impl& mesh,
+ const Vec3& origin, const UnitVec3& direction, Real& distance,
+ int& face, Vec2& uv) const {
+ if (child1 != NULL) {
+ // Recursively check the child nodes.
+
+ Real child1distance, child2distance;
+ int child1face, child2face;
+ Vec2 child1uv, child2uv;
+ bool child1intersects = child1->bounds.intersectsRay(origin, direction, child1distance);
+ bool child2intersects = child2->bounds.intersectsRay(origin, direction, child2distance);
+ if (child1intersects) {
+ if (child2intersects) {
+ // The ray intersects both child nodes. First check the closer one.
+
+ if (child1distance < child2distance) {
+ child1intersects = child1->intersectsRay(mesh, origin, direction, child1distance, child1face, child1uv);
+ if (!child1intersects || child2distance < child1distance)
+ child2intersects = child2->intersectsRay(mesh, origin, direction, child2distance, child2face, child2uv);
+ }
+ else {
+ child2intersects = child2->intersectsRay(mesh, origin, direction, child2distance, child2face, child2uv);
+ if (!child2intersects || child1distance < child2distance)
+ child1intersects = child1->intersectsRay(mesh, origin, direction, child1distance, child1face, child1uv);
+ }
+ }
+ else
+ child1intersects = child1->intersectsRay(mesh, origin, direction, child1distance, child1face, child1uv);
+ }
+ else if (child2intersects)
+ child2intersects = child2->intersectsRay(mesh, origin, direction, child2distance, child2face, child2uv);
+
+ // If either one had an intersection, return the closer one.
+
+ if (child1intersects && (!child2intersects || child1distance < child2distance)) {
+ distance = child1distance;
+ face = child1face;
+ uv = child1uv;
+ return true;
+ }
+ if (child2intersects) {
+ distance = child2distance;
+ face = child2face;
+ uv = child2uv;
+ return true;
+ }
+ return false;
+ }
+
+ // This is a leaf node, so check each triangle for an intersection with the
+ // ray.
+
+ bool foundIntersection = false;
+ for (int i = 0; i < (int) triangles.size(); i++) {
+ const UnitVec3& faceNormal = mesh.faces[triangles[i]].normal;
+ Real vd = ~faceNormal*direction;
+ if (vd == 0.0)
+ continue; // The ray is parallel to the plane.
+ const Vec3& vert1 = mesh.vertices[mesh.faces[triangles[i]].vertices[0]].pos;
+ Real v0 = ~faceNormal*(vert1-origin);
+ Real t = v0/vd;
+ if (t < 0)
+ continue; // Ray points away from plane of triangle.
+ if (foundIntersection && t >= distance)
+ continue; // We already have a closer intersection.
+
+ // Determine whether the intersection point is inside the triangle by projecting onto
+ // a plane and computing the barycentric coordinates.
+
+ Vec3 ri = origin+direction*t;
+ const Vec3& vert2 = mesh.vertices[mesh.faces[triangles[i]].vertices[1]].pos;
+ const Vec3& vert3 = mesh.vertices[mesh.faces[triangles[i]].vertices[2]].pos;
+ int axis1, axis2;
+ if (std::abs(faceNormal[1]) > std::abs(faceNormal[0])) {
+ if (std::abs(faceNormal[2]) > std::abs(faceNormal[1])) {
+ axis1 = 0;
+ axis2 = 1;
+ }
+ else {
+ axis1 = 0;
+ axis2 = 2;
+ }
+ }
+ else {
+ if (std::abs(faceNormal[2]) > std::abs(faceNormal[0])) {
+ axis1 = 0;
+ axis2 = 1;
+ }
+ else {
+ axis1 = 1;
+ axis2 = 2;
+ }
+ }
+ Vec2 pos(ri[axis1]-vert1[axis1], ri[axis2]-vert1[axis2]);
+ Vec2 edge1(vert1[axis1]-vert2[axis1], vert1[axis2]-vert2[axis2]);
+ Vec2 edge2(vert1[axis1]-vert3[axis1], vert1[axis2]-vert3[axis2]);
+ Real denom = Real(1)/(edge1%edge2);
+ edge2 *= denom;
+ Real v = edge2%pos;
+ if (v < 0 || v > 1)
+ continue;
+ edge1 *= denom;
+ Real w = pos%edge1;
+ if (w < 0 || w > 1)
+ continue;
+ Real u = 1-v-w;
+ if (u < 0 || u > 1)
+ continue;
+
+ // It intersects.
+
+ distance = t;
+ face = triangles[i];
+ uv = Vec2(u, v);
+ foundIntersection = true;
+ }
+ return foundIntersection;
+}
+
+
+
+
+//==============================================================================
+// CONTACT GEOMETRY :: TRIANGLE MESH :: OBB TREE NODE
+//==============================================================================
+
+ContactGeometry::TriangleMesh::OBBTreeNode::
+OBBTreeNode(const OBBTreeNodeImpl& impl) : impl(&impl) {}
+
+const OrientedBoundingBox&
+ContactGeometry::TriangleMesh::OBBTreeNode::getBounds() const {
+ return impl->bounds;
+}
+
+bool ContactGeometry::TriangleMesh::OBBTreeNode::isLeafNode() const {
+ return (impl->child1 == NULL);
+}
+
+const ContactGeometry::TriangleMesh::OBBTreeNode
+ContactGeometry::TriangleMesh::OBBTreeNode::getFirstChildNode() const {
+ SimTK_ASSERT_ALWAYS(impl->child1,
+ "Called getFirstChildNode() on a leaf node");
+ return OBBTreeNode(*impl->child1);
+}
+
+const ContactGeometry::TriangleMesh::OBBTreeNode
+ContactGeometry::TriangleMesh::OBBTreeNode::getSecondChildNode() const {
+ SimTK_ASSERT_ALWAYS(impl->child2,
+ "Called getFirstChildNode() on a leaf node");
+ return OBBTreeNode(*impl->child2);
+}
+
+const Array_<int>& ContactGeometry::TriangleMesh::OBBTreeNode::
+getTriangles() const {
+ SimTK_ASSERT_ALWAYS(impl->child2 == NULL,
+ "Called getTriangles() on a non-leaf node");
+ return impl->triangles;
+}
+
+int ContactGeometry::TriangleMesh::OBBTreeNode::getNumTriangles() const {
+ return impl->numTriangles;
+}
+
diff --git a/SimTKmath/Geometry/src/ContactImpl.h b/SimTKmath/Geometry/src/ContactImpl.h
new file mode 100644
index 0000000..48bc794
--- /dev/null
+++ b/SimTKmath/Geometry/src/ContactImpl.h
@@ -0,0 +1,259 @@
+#ifndef SimTK_SIMMATH_CONTACT_IMPL_H_
+#define SimTK_SIMMATH_CONTACT_IMPL_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/internal/Contact.h"
+
+namespace SimTK {
+
+
+//==============================================================================
+// CONTACT IMPL
+//==============================================================================
+/** This is the internal implementation base class for Contact. **/
+class ContactImpl {
+public:
+ ContactImpl(ContactSurfaceIndex surf1,
+ ContactSurfaceIndex surf2,
+ Contact::Condition condition=Contact::Unknown)
+ : m_referenceCount(0), m_condition(condition),
+ m_id(), m_surf1(surf1), m_surf2(surf2), m_X_S1S2() {}
+
+ ContactImpl(ContactSurfaceIndex surf1,
+ ContactSurfaceIndex surf2,
+ const Transform& X_S1S2,
+ Contact::Condition condition=Contact::Unknown)
+ : m_referenceCount(0), m_condition(condition),
+ m_id(), m_surf1(surf1), m_surf2(surf2), m_X_S1S2(X_S1S2) {}
+
+ void setTransform(const Transform& X_S1S2) {m_X_S1S2 = X_S1S2;}
+ const Transform& getTransform() const {return m_X_S1S2;}
+
+ void setCondition(Contact::Condition cond) {m_condition=cond;}
+ Contact::Condition getCondition() const {return m_condition;}
+
+ void setContactId(ContactId id) {m_id=id;}
+ ContactId getContactId() const {return m_id;}
+
+ virtual ~ContactImpl() {
+ assert(m_referenceCount == 0);
+ }
+ virtual ContactTypeId getTypeId() const = 0;
+
+ /* Create a new ContactTypeId and return this unique small integer
+ (thread safe). Each distinct type of Contact should use this to
+ initialize a static variable for that concrete class. */
+ static ContactTypeId createNewContactTypeId()
+ { static AtomicInteger nextAvailableId = 1;
+ return ContactTypeId(nextAvailableId++); }
+
+
+ /* Create a new ContactId and return this unique integer
+ (thread safe). Each distinct type of Contact should use this to
+ initialize a static variable for that concrete class. This will
+ roll over at approximately 1 billion. */
+ static ContactId createNewContactId()
+ { static AtomicInteger nextAvailableId = 1;
+ const int MaxContactId = 999999999; // 1 billion-1
+ const int id = nextAvailableId++;
+ // Other threads might get a few more high-numbered ids here before
+ // we reset the next available to 1, but since only one thread gets
+ // exactly MaxContactId as its id, only one will execute the reset.
+ if (id == MaxContactId)
+ nextAvailableId = 1;
+ return ContactId(id); }
+
+protected:
+friend class Contact;
+
+ mutable int m_referenceCount;
+ Contact::Condition m_condition;
+ ContactId m_id;
+ ContactSurfaceIndex m_surf1,
+ m_surf2;
+ Transform m_X_S1S2;
+};
+
+
+
+//==============================================================================
+// UNTRACKED CONTACT IMPL
+//==============================================================================
+/** This is the internal implementation class for UntrackedContact. **/
+class UntrackedContactImpl : public ContactImpl {
+public:
+ UntrackedContactImpl(ContactSurfaceIndex surf1, ContactSurfaceIndex surf2)
+ : ContactImpl(surf1, surf2, Contact::Untracked) {}
+
+ ContactTypeId getTypeId() const {return classTypeId();}
+ static ContactTypeId classTypeId() {
+ static const ContactTypeId tid = createNewContactTypeId();
+ return tid;
+ }
+};
+
+
+//==============================================================================
+// BROKEN CONTACT IMPL
+//==============================================================================
+/** This is the internal implementation class for BrokenContact. **/
+class BrokenContactImpl : public ContactImpl {
+public:
+ BrokenContactImpl
+ (ContactSurfaceIndex surf1, ContactSurfaceIndex surf2,
+ const Transform& X_S1S2, Real separation)
+ : ContactImpl(surf1, surf2, X_S1S2, Contact::Broken),
+ separation(separation)
+ {
+ }
+
+ ContactTypeId getTypeId() const {return classTypeId();}
+ static ContactTypeId classTypeId() {
+ static const ContactTypeId tid = createNewContactTypeId();
+ return tid;
+ }
+
+private:
+friend class BrokenContact;
+ Real separation;
+};
+
+
+
+//==============================================================================
+// CIRCULAR POINT CONTACT IMPL
+//==============================================================================
+/** This is the internal implementation class for CircularPointContact. **/
+class CircularPointContactImpl : public ContactImpl {
+public:
+ CircularPointContactImpl
+ (ContactSurfaceIndex surf1, Real radius1,
+ ContactSurfaceIndex surf2 ,Real radius2,
+ const Transform& X_S1S2, Real radiusEff, Real depth,
+ const Vec3& origin_S1, const UnitVec3& normal_S1)
+ : ContactImpl(surf1, surf2, X_S1S2),
+ radius1(radius1), radius2(radius2), radiusEff(radiusEff),
+ depth(depth), origin_S1(origin_S1), normal_S1(normal_S1) {}
+
+ ContactTypeId getTypeId() const {return classTypeId();}
+ static ContactTypeId classTypeId() {
+ static const ContactTypeId tid = createNewContactTypeId();
+ return tid;
+ }
+
+private:
+friend class CircularPointContact;
+ Real radius1, radius2, radiusEff, depth;
+ Vec3 origin_S1;
+ UnitVec3 normal_S1;
+};
+
+
+
+//==============================================================================
+// ELLIPTICAL POINT CONTACT IMPL
+//==============================================================================
+/** This is the internal implementation class for EllipticalPointContact. **/
+class EllipticalPointContactImpl : public ContactImpl {
+public:
+ EllipticalPointContactImpl
+ (ContactSurfaceIndex surf1, ContactSurfaceIndex surf2,
+ const Transform& X_S1S2, const Transform& X_S1C,
+ const Vec2& k, Real depth)
+ : ContactImpl(surf1, surf2, X_S1S2),
+ X_S1C(X_S1C), k(k), depth(depth) {}
+
+ ContactTypeId getTypeId() const {return classTypeId();}
+ static ContactTypeId classTypeId() {
+ static const ContactTypeId tid = createNewContactTypeId();
+ return tid;
+ }
+
+private:
+friend class EllipticalPointContact;
+ Transform X_S1C;
+ Vec2 k; // kmax, kmin
+ Real depth;
+};
+
+
+
+//==============================================================================
+// TRIANGLE MESH IMPL
+//==============================================================================
+/** This is the internal implementation class for TriangleMeshContact. **/
+class TriangleMeshContactImpl : public ContactImpl {
+public:
+ TriangleMeshContactImpl(ContactSurfaceIndex surf1,
+ ContactSurfaceIndex surf2,
+ const Transform& X_S1S2,
+ const std::set<int>& faces1,
+ const std::set<int>& faces2);
+
+ ContactTypeId getTypeId() const {return classTypeId();}
+ static ContactTypeId classTypeId() {
+ static const ContactTypeId tid = createNewContactTypeId();
+ return tid;
+ }
+
+private:
+friend class TriangleMeshContact;
+
+ const std::set<int> faces1;
+ const std::set<int> faces2;
+};
+
+
+
+//==============================================================================
+// POINT CONTACT IMPL
+//==============================================================================
+/** This is the internal implementation class for PointContact. **/
+class PointContactImpl : public ContactImpl {
+public:
+ PointContactImpl(ContactSurfaceIndex surf1, ContactSurfaceIndex surf2,
+ Vec3& location, Vec3& normal, Real radius1, Real radius2, Real depth);
+ PointContactImpl(ContactSurfaceIndex surf1, ContactSurfaceIndex surf2,
+ Vec3& location, Vec3& normal, Real radius, Real depth);
+
+ ContactTypeId getTypeId() const {return classTypeId();}
+ static ContactTypeId classTypeId() {
+ static const ContactTypeId tid = createNewContactTypeId();
+ return tid;
+ }
+
+private:
+friend class PointContact;
+
+ Vec3 location, normal;
+ Real radius1, radius2, effectiveRadius, depth;
+};
+
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_CONTACT_IMPL_H_
diff --git a/SimTKmath/Geometry/src/ContactTracker.cpp b/SimTKmath/Geometry/src/ContactTracker.cpp
new file mode 100644
index 0000000..9118ba9
--- /dev/null
+++ b/SimTKmath/Geometry/src/ContactTracker.cpp
@@ -0,0 +1,1479 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-13 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 <algorithm>
+using std::pair; using std::make_pair;
+#include <iostream>
+using std::cout; using std::endl;
+#include <set>
+
+// Define this if you want to see voluminous output from MPR (XenoCollide)
+//#define MPR_DEBUG
+
+
+namespace SimTK {
+
+//==============================================================================
+// CONTACT TRACKER
+//==============================================================================
+
+
+//------------------------------------------------------------------------------
+// ESTIMATE IMPLICIT PAIR CONTACT USING MPR
+//------------------------------------------------------------------------------
+// Generate a rough guess at the contact points. Point P is returned in A's
+// frame and point Q is in B's frame, but all the work here is done in frame A.
+// See Snethen, G. "XenoCollide: Complex Collision Made Simple", Game
+// Programming Gems 7, pp.165-178.
+//
+// Note that we intend to use this on smooth convex objects. That means there
+// can be no guarantee about how many iterations this will take to converge; it
+// may take a very long time for objects that are just barely touching. On the
+// other hand we only need an approximate answer because we're going to polish
+// the solution to machine precision using a Newton iteration afterwards.
+
+namespace {
+
+// Given a direction in shape A's frame, calculate the support point of the
+// Minkowski difference shape A-B in that direction. To do that we find A's
+// support point P in that direction, and B's support point Q in the opposite
+// direction; i.e. what would be -B's support in the original direction. Then
+// return the vector v=(P-Q) expressed in A's frame.
+struct Support {
+ Support(const ContactGeometry& shapeA, const ContactGeometry& shapeB,
+ const Transform& X_AB, const UnitVec3& dirInA)
+ : shapeA(shapeA), shapeB(shapeB), X_AB(X_AB)
+ { computeSupport(dirInA); }
+
+ // Calculate a new set of supports for the same shapes.
+ void computeSupport(const UnitVec3& dirInA) {
+ const UnitVec3 dirInB = ~X_AB.R() * dirInA; // 15 flops
+ dir = dirInA;
+ A = shapeA.calcSupportPoint( dirInA); // varies; 40 flops for ellipsoid
+ B = shapeB.calcSupportPoint(-dirInB); // "
+ v = A - X_AB*B; // 21 flops
+ depth = dot(v,dir); // 5 flops
+ }
+
+ Support& operator=(const Support& src) {
+ dir = src.dir; A = src.A; B = src.B; v = src.v;
+ return *this;
+ }
+
+ void swap(Support& other) {
+ std::swap(dir, other.dir);
+ std::swap(A,other.A); std::swap(B,other.B); std::swap(v,other.v);
+ }
+
+ void getResult(Vec3& pointP_A, Vec3& pointQ_B, UnitVec3& normalInA) const
+ { pointP_A = A; pointQ_B = B; normalInA = dir; }
+
+ UnitVec3 dir; // A support direction, exp in A
+ Vec3 A; // support point in direction dir on shapeA, expressed in A
+ Vec3 B; // support point in direction -dir on shapeB, expressed in B
+ Vec3 v; // support point A-B in Minkowski difference shape, exp. in A
+ Real depth; // v . dir (positive when origin is below support plane)
+
+private:
+ const ContactGeometry& shapeA;
+ const ContactGeometry& shapeB;
+ const Transform& X_AB;
+};
+
+const Real MPRAccuracy = Real(.05); // 5% of the surface dimensions
+const Real SqrtMPRAccuracy = Real(.2236); // roughly sqrt(MPRAccuracy)
+}
+
+
+/*static*/ bool ContactTracker::
+estimateConvexImplicitPairContactUsingMPR
+ (const ContactGeometry& shapeA, const ContactGeometry& shapeB,
+ const Transform& X_AB,
+ Vec3& pointP, Vec3& pointQ, UnitVec3& dirInA, int& numIterations)
+{
+ const Rotation& R_AB = X_AB.R();
+ numIterations = 0;
+
+ // Get some cheap, rough scaling information.
+ // TODO: ideally this would be done using local curvature information.
+ // A reasonable alternative would be to use the smallest dimension of the
+ // shape's oriented bounding box.
+ // We're going to assume the smallest radius is 1/4 of the bounding
+ // sphere radius.
+ Vec3 cA, cB; Real rA, rB;
+ shapeA.getBoundingSphere(cA,rA); shapeB.getBoundingSphere(cB,rB);
+ const Real lengthScale = Real(0.25)*std::min(rA,rB);
+ const Real areaScale = Real(1.5)*square(lengthScale); // ~area of octant
+
+ // If we determine the depth to within a small fraction of the scale,
+ // or localize the contact area to a small fraction of the surface area,
+ // that is good enough and we can stop.
+ const Real depthGoal = MPRAccuracy*lengthScale;
+ const Real areaGoal = SqrtMPRAccuracy*areaScale;
+
+ #ifdef MPR_DEBUG
+ printf("\nMPR acc=%g: r=%g, depthGoal=%g areaGoal=%g\n",
+ MPRAccuracy, std::min(rA,rB), depthGoal, areaGoal);
+ #endif
+
+ // Phase 1: Portal Discovery
+ // -------------------------
+
+ // Compute an interior point v0 that is known to be inside the Minkowski
+ // difference, and a ray dir1 directed from that point to the origin.
+ const Vec3 v0 = ( Support(shapeA, shapeB, X_AB, UnitVec3( XAxis)).v
+ + Support(shapeA, shapeB, X_AB, UnitVec3(-XAxis)).v)/2;
+
+ if (v0 == 0) {
+ // This is a pathological case: the two objects are directly on top of
+ // each other with their centers at exactly the same place. Just
+ // return *some* vaguely plausible contact.
+ pointP = shapeA.calcSupportPoint( UnitVec3( XAxis));
+ pointQ = shapeB.calcSupportPoint(~R_AB*UnitVec3(-XAxis)); // in B
+ dirInA = XAxis;
+ return true;
+ }
+
+
+ // Support 1's direction is initially the "origin ray" that points from
+ // interior point v0 to the origin.
+ Support s1(shapeA, shapeB, X_AB, UnitVec3(-v0));
+
+ // Test for NaN once and get out to avoid getting stuck in loops below.
+ if (isNaN(s1.depth)) {
+ pointP = pointQ = NaN;
+ dirInA = UnitVec3();
+ return false;
+ }
+
+ if (s1.depth <= 0) { // origin outside 1st support plane
+ s1.getResult(pointP, pointQ, dirInA);
+ return false;
+ }
+
+ if (s1.v % v0 == 0) { // v0 perpendicular to support plane; origin inside
+ s1.getResult(pointP, pointQ, dirInA);
+ return true;
+ }
+
+ // Find support point perpendicular to plane containing origin, interior
+ // point v0, and first support v1.
+ Support s2(shapeA, shapeB, X_AB, UnitVec3(s1.v % v0));
+ if (s2.depth <= 0) { // origin is outside 2nd support plane
+ s2.getResult(pointP, pointQ, dirInA);
+ return false;
+ }
+
+ // Find support perpendicular to plane containing interior point v0 and
+ // first two support points v1 and v2. Make sure it is on the side that
+ // is closer to the origin; fix point ordering if necessary.
+ UnitVec3 d3 = UnitVec3((s1.v-v0)%(s2.v-v0));
+ if (~d3*v0 > 0) { // oops -- picked the wrong side
+ s1.swap(s2);
+ d3 = -d3;
+ }
+ Support s3(shapeA, shapeB, X_AB, d3);
+ if (s3.depth <= 0) { // origin is outside 3rd support plane
+ s3.getResult(pointP, pointQ, dirInA);
+ return false;
+ }
+
+ // We now have a candidate portal (triangle v1,v2,v3). We have to refine it
+ // until the origin ray -v0 intersects the candidate. Check against the
+ // three planes of the tetrahedron that contain v0. By construction above
+ // we know the origin is inside the v0,v1,v2 face already.
+
+ // We should find a candidate portal very fast, probably in 1 or 2
+ // iterations. We'll allow an absurdly large number of
+ // iterations and then abort just to make sure we don't get stuck in an
+ // infinite loop.
+ const Real MaxCandidateIters = 100; // should never get anywhere near this
+ int candidateIters = 0;
+ while (true) {
+ ++candidateIters;
+ SimTK_ERRCHK_ALWAYS(candidateIters <= MaxCandidateIters,
+ "ContactTracker::estimateConvexImplicitPairContactUsingMPR()",
+ "Unable to find a candidate portal; should never happen.");
+
+ if (~v0*(s1.v % s3.v) < -SignificantReal)
+ s2 = s3; // origin outside v0,v1,v3 face; replace v2
+ else if (~v0*(s3.v % s2.v) < -SignificantReal)
+ s1 = s3; // origin outside v0,v2,v3 face; replace v1
+ else
+ break;
+
+ // Choose new candidate. The keepers are in v1 and v2; get a new v3.
+ s3.computeSupport(UnitVec3((s1.v-v0) % (s2.v-v0)));
+ }
+
+ // Phase 2: Portal Refinement
+ // --------------------------
+
+ // We have a portal (triangle v1,v2,v3) that the origin ray passes through.
+ // Now we need to refine v1,v2,v3 until we have portal such that the origin
+ // is inside the tetrahedron v0,v1,v2,v3.
+
+ const int MinTriesToFindSeparatingPlane = 5;
+ const int MaxTriesToImproveContact = 5;
+ int triesToFindSeparatingPlane=0, triesToImproveContact=0;
+ while (true) {
+ ++numIterations;
+
+ // Get the normal to the portal, oriented in the general direction of
+ // the origin ray (i.e., the outward normal).
+ const Vec3 portalVec = (s2.v-s1.v) % (s3.v-s1.v);
+ const Real portalArea = portalVec.norm(), ooPortalArea = 1/portalArea;
+ UnitVec3 portalDir(portalVec*ooPortalArea, true);
+ if (~portalDir*v0 > 0)
+ portalDir = -portalDir;
+
+ // Any portal vertex is a vector from the origin to the portal plane.
+ // Dot one of them with the portal outward normal to get the origin
+ // depth (positive if inside).
+ const Real depth = ~s1.v*portalDir;
+
+ // Find new support in portal direction.
+ Support s4(shapeA, shapeB, X_AB, portalDir);
+ if (s4.depth <= 0) { // origin is outside new support plane
+ s4.getResult(pointP, pointQ, dirInA);
+ return false;
+ }
+
+ const Real depthChange = std::abs(s4.depth - depth);
+
+
+ bool mustReturn=false, okToReturn=false;
+ if (depth >= 0) { // We found a contact.
+ mustReturn = (++triesToImproveContact >= MaxTriesToImproveContact);
+ okToReturn = true;
+ } else { // No contact yet.
+ okToReturn =
+ (++triesToFindSeparatingPlane >= MinTriesToFindSeparatingPlane);
+ mustReturn = false;
+ }
+ bool accuracyAchieved =
+ (depthChange <= depthGoal || portalArea <= areaGoal);
+
+ #ifdef MPR_DEBUG
+ printf(" depth=%g, change=%g area=%g changeFrac=%g areaFrac=%g\n",
+ depth, depthChange, portalArea,
+ depthChange/depthGoal, portalArea/areaGoal);
+ printf(" accuracyAchieved=%d okToReturn=%d mustReturn=%d\n",
+ accuracyAchieved, okToReturn, mustReturn);
+ #endif
+
+ if (mustReturn || (okToReturn && accuracyAchieved)) {
+ // The origin is inside the portal, so we have an intersection.
+ // Compute the barycentric coordinates of the origin ray's
+ // intersection with the portal, and map back to the two surfaces.
+ const Vec3 origin = v0+v0*(~portalDir*(s1.v-v0)/(~portalDir*v0));
+ const Real area1 = ~portalDir*((s2.v-origin)%(s3.v-origin));
+ const Real area2 = ~portalDir*((s3.v-origin)%(s1.v-origin));
+ const Real u = area1*ooPortalArea;
+ const Real v = area2*ooPortalArea;
+ const Real w = 1-u-v;
+
+ // Compute the contact points in their own shape's frame.
+ pointP = u*s1.A + v*s2.A + w*s3.A;
+ pointQ = u*s1.B + v*s2.B + w*s3.B;
+ dirInA = portalDir;
+ return true;
+ }
+
+ // We know the origin ray entered the (v1,v2,v3,v4) tetrahedron via the
+ // (v1,v2,v3) face (the portal). New portal is the face that it exits.
+ const Vec3 v4v0 = s4.v % v0;
+ if (~s1.v * v4v0 > 0) {
+ if (~s2.v * v4v0 > 0) s1 = s4; // v4,v2,v3 (new portal)
+ else s3 = s4; // v1,v2,v4
+ } else {
+ if (~s3.v * v4v0 > 0) s2 = s4; // v1,v4,v3
+ else s1 = s4; // v4,v2,v3 again
+ }
+ }
+}
+
+
+//------------------------------------------------------------------------------
+// REFINE IMPLICIT PAIR
+//------------------------------------------------------------------------------
+// We have a rough estimate of the contact points. Use Newton iteration to
+// refine them. If the surfaces are separated, this will find the points of
+// closest approach. If contacting, this will find the points of maximium
+// penetration.
+
+// Returns true if the desired accuracy is achieved, regardless of whether the
+// surfaces are separated or in contact.
+/*static*/ bool ContactTracker::
+refineImplicitPair
+ (const ContactGeometry& shapeA, Vec3& pointP, // in/out (in A)
+ const ContactGeometry& shapeB, Vec3& pointQ, // in/out (in B)
+ const Transform& X_AB, Real accuracyRequested,
+ Real& accuracyAchieved, int& numIterations)
+{
+ // If the initial guess is very bad, or the ellipsoids pathological we
+ // may have to crawl along for a while at the beginning.
+ const int MaxSlowIterations = 8;
+ const int MaxIterations = MaxSlowIterations + 8;
+ const Real MinStepFrac = Real(1e-6); // if we can't take at least this
+ // fraction of Newton step, give it up
+
+ Vec6 err = findImplicitPairError(shapeA, pointP, shapeB, pointQ, X_AB);
+ accuracyAchieved = err.norm();
+ Vector errVec(6, &err[0], true); // share space with err
+
+ Mat66 J; // to hold the Jacobian
+ Matrix JMat(6,6,6,&J(0,0)); // share space with J
+
+ Vec6 delta;
+ Vector deltaVec(6, &delta[0], true); // share space with delta
+
+ numIterations = 0;
+ while ( accuracyAchieved > accuracyRequested
+ && numIterations < MaxIterations)
+ {
+ ++numIterations;
+ J = calcImplicitPairJacobian(shapeA, pointP, shapeB, pointQ,
+ X_AB, err);
+
+ // Try to use LU factorization; fall back to QTZ if singular.
+ FactorLU lu(JMat);
+ if (!lu.isSingular())
+ lu.solve(errVec, deltaVec); // writes into delta also
+ else {
+ FactorQTZ qtz(JMat, SqrtEps);
+ qtz.solve(errVec, deltaVec); // writes into delta also
+ }
+
+ // Line search for safety in case starting guess bad. Don't accept
+ // any move that makes things worse.
+ Real f = 2; // scale back factor
+ Vec3 oldP = pointP, oldQ = pointQ;
+ Real oldAccuracyAchieved = accuracyAchieved;
+ do {
+ f /= 2;
+ pointP = oldP - f*delta.getSubVec<3>(0);
+ pointQ = oldQ - f*delta.getSubVec<3>(3);
+ err = findImplicitPairError(shapeA, pointP, shapeB, pointQ, X_AB);
+ accuracyAchieved = err.norm();
+ } while (accuracyAchieved > oldAccuracyAchieved && f > MinStepFrac);
+
+ const bool noProgressMade = (accuracyAchieved > oldAccuracyAchieved);
+
+ if (noProgressMade) { // Restore best points and fall through.
+ pointP = oldP;
+ pointQ = oldQ;
+ }
+
+ if ( noProgressMade
+ || (f < 1 && numIterations >= MaxSlowIterations)) // Too slow.
+ {
+ // We don't appear to be getting anywhere. Just project the
+ // points onto the surfaces and then exit.
+ bool inside;
+ UnitVec3 normal;
+ pointP = shapeA.findNearestPoint(pointP, inside, normal);
+ pointQ = shapeB.findNearestPoint(pointQ, inside, normal);
+ err = findImplicitPairError(shapeA, pointP, shapeB, pointQ, X_AB);
+ accuracyAchieved = err.norm();
+ break;
+ }
+ }
+
+ return accuracyAchieved <= accuracyRequested;
+}
+
+
+//------------------------------------------------------------------------------
+// FIND IMPLICIT PAIR ERROR
+//------------------------------------------------------------------------------
+// We're given two implicitly-defined shapes A and B and candidate surface
+// contact points P (for A) and Q (for B). There are six equations that real
+// contact points should satisfy. Here we return the errors in each of those
+// equations; if all six terms are zero these are contact points.
+//
+// Per profiling, this gets called a lot at runtime, so keep it tight!
+/*static*/ Vec6 ContactTracker::
+findImplicitPairError
+ (const ContactGeometry& shapeA, const Vec3& pointP, // in A
+ const ContactGeometry& shapeB, const Vec3& pointQ, // in B
+ const Transform& X_AB)
+{
+ // Compute the function value and normal vector for each object.
+
+ const Function& fA = shapeA.getImplicitFunction();
+ const Function& fB = shapeB.getImplicitFunction();
+
+ // Avoid some heap allocations by using stack arrays.
+ Vec3 xData;
+ Vector x(3, &xData[0], true); // shares space with xdata
+
+ int compData; // just one integer
+ ArrayView_<int> components(&compData, &compData+1);
+
+ Vec3 gradA, gradB;
+ xData = pointP; // writes into Vector x also
+ for (int i = 0; i < 3; i++) {
+ components[0] = i;
+ gradA[i] = fA.calcDerivative(components, x);
+ }
+ Real errorA = fA.calcValue(x);
+ xData = pointQ; // writes into Vector x also
+ for (int i = 0; i < 3; i++) {
+ components[0] = i;
+ gradB[i] = fB.calcDerivative(components, x);
+ }
+ Real errorB = fB.calcValue(x);
+
+ // Construct a coordinate frame for each object.
+ // TODO: this needs some work to make sure it is as stable as possible
+ // so the perpendicularity errors are stable as the solution advances
+ // and especially as the Jacobian is calculated by perturbation.
+
+ UnitVec3 nA(-gradA);
+ UnitVec3 nB(X_AB.R()*(-gradB));
+ UnitVec3 uA(std::abs(nA[0])>Real(0.5)? nA%Vec3(0, 1, 0) : nA%Vec3(1, 0, 0));
+ UnitVec3 uB(std::abs(nB[0])>Real(0.5)? nB%Vec3(0, 1, 0) : nB%Vec3(1, 0, 0));
+ Vec3 vA = nA%uA; // Already a unit vector, so we don't need to normalize it.
+ Vec3 vB = nB%uB;
+
+ // Compute the error vector. The components indicate, in order, that nA
+ // must be perpendicular to both tangents of object B, that the separation
+ // vector should be zero or perpendicular to the tangents of object A, and
+ // that both points should be on their respective surfaces.
+
+ Vec3 delta = pointP-X_AB*pointQ;
+ return Vec6(~nA*uB, ~nA*vB, ~delta*uA, ~delta*vA, errorA, errorB);
+}
+
+
+//------------------------------------------------------------------------------
+// CALC IMPLICIT PAIR JACOBIAN
+//------------------------------------------------------------------------------
+// Differentiate the findImplicitPairError() with respect to changes in the
+// locations of points P and Q in their own surface frames.
+
+/*static*/ Mat66 ContactTracker::calcImplicitPairJacobian
+ (const ContactGeometry& shapeA, const Vec3& pointP,
+ const ContactGeometry& shapeB, const Vec3& pointQ,
+ const Transform& X_AB, const Vec6& err0)
+{
+ Real dt = SqrtEps;
+
+ Vec3 d1 = dt*Vec3(1, 0, 0);
+ Vec3 d2 = dt*Vec3(0, 1, 0);
+ Vec3 d3 = dt*Vec3(0, 0, 1);
+ Vec6 err1 = findImplicitPairError(shapeA, pointP+d1, shapeB, pointQ, X_AB)
+ - err0;
+ Vec6 err2 = findImplicitPairError(shapeA, pointP+d2, shapeB, pointQ, X_AB)
+ - err0;
+ Vec6 err3 = findImplicitPairError(shapeA, pointP+d3, shapeB, pointQ, X_AB)
+ - err0;
+ Vec6 err4 = findImplicitPairError(shapeA, pointP, shapeB, pointQ+d1, X_AB)
+ - err0;
+ Vec6 err5 = findImplicitPairError(shapeA, pointP, shapeB, pointQ+d2, X_AB)
+ - err0;
+ Vec6 err6 = findImplicitPairError(shapeA, pointP, shapeB, pointQ+d3, X_AB)
+ - err0;
+ return Mat66(err1, err2, err3, err4, err5, err6) / dt;
+
+ // This is a Central Difference derivative (you should use a somewhat
+ // larger dt in this case). However, I haven't seen any evidence that this
+ // helps, even for some very eccentric ellipsoids. (sherm 20130408)
+ //Vec6 err1 = findImplicitPairError(shapeA, pointP+d1, shapeB, pointQ, X_AB)
+ // - findImplicitPairError(shapeA, pointP-d1, shapeB, pointQ, X_AB);
+ //Vec6 err2 = findImplicitPairError(shapeA, pointP+d2, shapeB, pointQ, X_AB)
+ // - findImplicitPairError(shapeA, pointP-d2, shapeB, pointQ, X_AB);
+ //Vec6 err3 = findImplicitPairError(shapeA, pointP+d3, shapeB, pointQ, X_AB)
+ // - findImplicitPairError(shapeA, pointP-d3, shapeB, pointQ, X_AB);
+ //Vec6 err4 = findImplicitPairError(shapeA, pointP, shapeB, pointQ+d1, X_AB)
+ // - findImplicitPairError(shapeA, pointP, shapeB, pointQ-d1, X_AB);
+ //Vec6 err5 = findImplicitPairError(shapeA, pointP, shapeB, pointQ+d2, X_AB)
+ // - findImplicitPairError(shapeA, pointP, shapeB, pointQ-d2, X_AB);
+ //Vec6 err6 = findImplicitPairError(shapeA, pointP, shapeB, pointQ+d3, X_AB)
+ // - findImplicitPairError(shapeA, pointP, shapeB, pointQ-d3, X_AB);
+ //return Mat66(err1, err2, err3, err4, err5, err6) / (2*dt);
+}
+
+
+
+//==============================================================================
+// HALFSPACE-SPHERE CONTACT TRACKER
+//==============================================================================
+// Cost is 21 flops if no contact, 67 with contact.
+bool ContactTracker::HalfSpaceSphere::trackContact
+ (const Contact& priorStatus,
+ const Transform& X_GH,
+ const ContactGeometry& geoHalfSpace,
+ const Transform& X_GS,
+ const ContactGeometry& geoSphere,
+ Real cutoff,
+ Contact& currentStatus) const
+{
+ SimTK_ASSERT
+ ( geoHalfSpace.getTypeId()==ContactGeometry::HalfSpace::classTypeId()
+ && geoSphere.getTypeId()==ContactGeometry::Sphere::classTypeId(),
+ "ContactTracker::HalfSpaceSphere::trackContact()");
+
+ // No need for an expensive dynamic cast here; we know what we have.
+ const ContactGeometry::Sphere& sphere =
+ ContactGeometry::Sphere::getAs(geoSphere);
+
+ const Rotation R_HG = ~X_GH.R(); // inverse rotation; no flops
+
+ // p_HC is vector from H origin to S's center C
+ const Vec3 p_HC = R_HG*(X_GS.p() - X_GH.p()); // 18 flops
+
+ // Calculate depth of sphere center C given that the halfspace occupies
+ // all of x>0 space.
+ const Real r = sphere.getRadius();
+ const Real depth = p_HC[0] + r; // 1 flop
+
+ if (depth <= -cutoff) { // 2 flops
+ currentStatus.clear(); // not touching
+ return true; // successful return
+ }
+
+ // Calculate the rest of the X_HS transform as required by Contact.
+ const Transform X_HS(R_HG*X_GS.R(), p_HC); // 45 flops
+ const UnitVec3 normal_H(Vec3(-1,0,0), true); // 0 flops
+ const Vec3 origin_H = Vec3(depth/2, p_HC[1], p_HC[2]); // 1 flop
+ // The surfaces are contacting (or close enough to be interesting).
+ // The sphere's radius is also the effective radius.
+ currentStatus = CircularPointContact(priorStatus.getSurface1(), Infinity,
+ priorStatus.getSurface2(), r,
+ X_HS, r, depth, origin_H, normal_H);
+ return true; // success
+}
+
+bool ContactTracker::HalfSpaceSphere::predictContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& predictedStatus) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactTracker::HalfSpaceSphere::predictContact() not implemented yet.");
+ return false; }
+
+bool ContactTracker::HalfSpaceSphere::initializeContact
+ (const Transform& X_GS1, const SpatialVec& V_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& contactStatus) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactTracker::HalfSpaceSphere::initializeContact() not implemented yet.");
+ return false; }
+
+
+
+//==============================================================================
+// HALFSPACE-ELLIPSOID CONTACT TRACKER
+//==============================================================================
+// Cost is ~135 flops if no contact, ~425 with contact.
+// The contact point on the ellipsoid must be the unique point that has its
+// outward-facing normal in the opposite direction of the half space normal.
+// We can find that point very fast and see how far it is from the half
+// space surface. If it is close enough, we'll evaluate the curvatures at
+// that point in preparation for generating forces with Hertz theory.
+bool ContactTracker::HalfSpaceEllipsoid::trackContact
+ (const Contact& priorStatus,
+ const Transform& X_GH,
+ const ContactGeometry& geoHalfSpace,
+ const Transform& X_GE,
+ const ContactGeometry& geoEllipsoid,
+ Real cutoff,
+ Contact& currentStatus) const
+{
+ SimTK_ASSERT
+ ( geoHalfSpace.getTypeId()==ContactGeometry::HalfSpace::classTypeId()
+ && geoEllipsoid.getTypeId()==ContactGeometry::Ellipsoid::classTypeId(),
+ "ContactTracker::HalfSpaceEllipsoid::trackContact()");
+
+ // No need for an expensive dynamic cast here; we know what we have.
+ const ContactGeometry::Ellipsoid& ellipsoid =
+ ContactGeometry::Ellipsoid::getAs(geoEllipsoid);
+
+ // Our half space occupies the +x half so the normal is -x.
+ const Transform X_HE = ~X_GH*X_GE; // 63 flops
+ // Halfspace normal is -x, so the ellipsoid normal we're looking for is
+ // in the half space's +x direction.
+ const UnitVec3& n_E = (~X_HE.R()).x(); // halfspace normal in E
+ const Vec3 Q_E = ellipsoid.findPointWithThisUnitNormal(n_E); // 40 flops
+ const Vec3 Q_H = X_HE*Q_E; // Q measured from half space origin (18 flops)
+ const Real depth = Q_H[0]; // x > 0 is penetrated
+
+ if (depth <= -cutoff) { // 2 flops
+ currentStatus.clear(); // not touching
+ return true; // successful return
+ }
+
+ // The surfaces are contacting (or close enough to be interesting).
+ // The ellipsoid's principal curvatures k at the contact point are also
+ // the curvatures of the contact paraboloid since the half space doesn't
+ // add anything interesting.
+ Transform X_EQ; Vec2 k;
+ ellipsoid.findParaboloidAtPointWithNormal(Q_E, n_E, X_EQ, k); // 220 flops
+
+ // We have the contact paraboloid expressed in frame Q but Qz=n_E has the
+ // wrong sign since we have to express it using the half space normal.
+ // We have to end up with a right handed frame, so one of x or y has
+ // to be negated too. (6 flops)
+ Rotation& R_EQ = X_EQ.updR();
+ R_EQ.setRotationColFromUnitVecTrustMe(ZAxis, -R_EQ.z()); // changing X_EQ
+ R_EQ.setRotationColFromUnitVecTrustMe(XAxis, -R_EQ.x());
+
+ // Now the frame is pointing in the right direction. Measure and express in
+ // half plane frame, then shift origin to half way between contact point
+ // Q on the undeformed ellipsoid and the corresponding contact point P
+ // on the undeformed half plane surface. It's easier to do this shift
+ // in H since it is in the -Hx direction.
+ Transform X_HC = X_HE*X_EQ; X_HC.updP()[0] -= depth/2; // 65 flops
+
+ currentStatus = EllipticalPointContact(priorStatus.getSurface1(),
+ priorStatus.getSurface2(),
+ X_HE, X_HC, k, depth);
+ return true; // success
+}
+
+bool ContactTracker::HalfSpaceEllipsoid::predictContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& predictedStatus) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactTracker::HalfSpaceEllipsoid::predictContact() not implemented yet.");
+ return false; }
+
+bool ContactTracker::HalfSpaceEllipsoid::initializeContact
+ (const Transform& X_GS1, const SpatialVec& V_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& contactStatus) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactTracker::HalfSpaceEllipsoid::initializeContact() not implemented yet.");
+ return false; }
+
+
+
+//==============================================================================
+// SPHERE-SPHERE CONTACT TRACKER
+//==============================================================================
+// Cost is 12 flops if no contact, 139 if contact
+bool ContactTracker::SphereSphere::trackContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1,
+ const ContactGeometry& geoSphere1,
+ const Transform& X_GS2,
+ const ContactGeometry& geoSphere2,
+ Real cutoff, // 0 for contact
+ Contact& currentStatus) const
+{
+ SimTK_ASSERT
+ ( geoSphere1.getTypeId()==ContactGeometry::Sphere::classTypeId()
+ && geoSphere2.getTypeId()==ContactGeometry::Sphere::classTypeId(),
+ "ContactTracker::SphereSphere::trackContact()");
+
+ // No need for an expensive dynamic casts here; we know what we have.
+ const ContactGeometry::Sphere& sphere1 =
+ ContactGeometry::Sphere::getAs(geoSphere1);
+ const ContactGeometry::Sphere& sphere2 =
+ ContactGeometry::Sphere::getAs(geoSphere2);
+
+ currentStatus.clear();
+
+ // Find the vector from sphere center C1 to C2, expressed in G.
+ const Vec3 p_12_G = X_GS2.p() - X_GS1.p(); // 3 flops
+ const Real d2 = p_12_G.normSqr(); // 5 flops
+ const Real r1 = sphere1.getRadius();
+ const Real r2 = sphere2.getRadius();
+ const Real rr = r1 + r2; // 1 flop
+
+ // Quick check. If separated we can return nothing, unless we were
+ // in contact last time in which case we have to return one last
+ // Contact indicating that contact has been broken and by how much.
+ if (d2 > square(rr+cutoff)) { // 3 flops
+ if (!priorStatus.getContactId().isValid())
+ return true; // successful return: still separated
+
+ const Real separation = std::sqrt(d2) - rr; // > cutoff, ~25 flops
+ const Transform X_S1S2(~X_GS1.R()*X_GS2.R(),
+ ~X_GS1.R()*p_12_G); // 60 flops
+ currentStatus = BrokenContact(priorStatus.getSurface1(),
+ priorStatus.getSurface2(),
+ X_S1S2, separation);
+ return true;
+ }
+
+ const Real d = std::sqrt(d2); // ~20 flops
+ if (d < SignificantReal) { // 1 flop
+ // TODO: If the centers are coincident we should use past information
+ // to determine the most likely normal. For now just fail.
+ return false;
+ }
+
+ const Transform X_S1S2(~X_GS1.R()*X_GS2.R(),
+ ~X_GS1.R()*p_12_G);// 60 flops
+ const Vec3& p_12 = X_S1S2.p(); // center-to-center vector in S1
+
+ const Real depth = rr - d; // >0 for penetration (1 flop)
+ const Real r = r1*r2/rr; // r=r1r2/(r1+r2)=1/(1/r1+1/r2) ~20 flops
+
+ const UnitVec3 normal_S1(p_12/d, true); // 1/ + 3* = ~20 flops
+ const Vec3 origin_S1 = (r1 - depth/2)*normal_S1; // 5 flops
+
+ // The surfaces are contacting (or close enough to be interesting).
+ currentStatus = CircularPointContact(priorStatus.getSurface1(), r1,
+ priorStatus.getSurface2(), r2,
+ X_S1S2, r, depth,
+ origin_S1, normal_S1);
+ return true; // success
+}
+
+bool ContactTracker::SphereSphere::predictContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& predictedStatus) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactTracker::SphereSphere::predictContact() not implemented yet.");
+ return false; }
+
+bool ContactTracker::SphereSphere::initializeContact
+ (const Transform& X_GS1, const SpatialVec& V_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& contactStatus) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactTracker::SphereSphere::initializeContact() not implemented yet.");
+ return false; }
+
+
+
+
+//==============================================================================
+// HALFSPACE - TRIANGLE MESH CONTACT TRACKER
+//==============================================================================
+// Cost is TODO
+bool ContactTracker::HalfSpaceTriangleMesh::trackContact
+ (const Contact& priorStatus,
+ const Transform& X_GH,
+ const ContactGeometry& geoHalfSpace,
+ const Transform& X_GM,
+ const ContactGeometry& geoMesh,
+ Real cutoff,
+ Contact& currentStatus) const
+{
+ SimTK_ASSERT_ALWAYS
+ ( ContactGeometry::HalfSpace::isInstance(geoHalfSpace)
+ && ContactGeometry::TriangleMesh::isInstance(geoMesh),
+ "ContactTracker::HalfSpaceTriangleMesh::trackContact()");
+
+ // We can't handle a "proximity" test, only penetration.
+ SimTK_ASSERT_ALWAYS(cutoff==0,
+ "ContactTracker::HalfSpaceTriangleMesh::trackContact()");
+
+ // No need for an expensive dynamic cast here; we know what we have.
+ const ContactGeometry::TriangleMesh& mesh =
+ ContactGeometry::TriangleMesh::getAs(geoMesh);
+
+ // Transform giving mesh (S2) frame in the halfspace (S1) frame.
+ const Transform X_HM = (~X_GH)*X_GM;
+
+ // Normal is halfspace -x direction; xdir is first column of R_MH.
+ // That's a unit vector and -unitvec is also a unit vector so this
+ // doesn't require normalization.
+ const UnitVec3 hsNormal_M = -(~X_HM.R()).x();
+ // Find the height of the halfspace face along the normal, measured
+ // from the mesh origin.
+ const Real hsFaceHeight_M = dot((~X_HM).p(), hsNormal_M);
+ // Now collect all the faces that are all or partially below the
+ // halfspace surface.
+ std::set<int> insideFaces;
+ processBox(mesh, mesh.getOBBTreeNode(), X_HM,
+ hsNormal_M, hsFaceHeight_M, insideFaces);
+
+ if (insideFaces.empty()) {
+ currentStatus.clear(); // not touching
+ return true; // successful return
+ }
+
+ currentStatus = TriangleMeshContact(priorStatus.getSurface1(),
+ priorStatus.getSurface2(),
+ X_HM,
+ std::set<int>(), insideFaces);
+ return true; // success
+}
+
+bool ContactTracker::HalfSpaceTriangleMesh::predictContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& predictedStatus) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactTracker::HalfSpaceTriangleMesh::predictContact() not implemented yet.");
+ return false; }
+
+bool ContactTracker::HalfSpaceTriangleMesh::initializeContact
+ (const Transform& X_GS1, const SpatialVec& V_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& contactStatus) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactTracker::HalfSpaceTriangleMesh::initializeContact() not implemented yet.");
+ return false; }
+
+
+
+// Check a single OBB and its contents (recursively) against the halfspace,
+// appending any penetrating faces to the insideFaces list.
+void ContactTracker::HalfSpaceTriangleMesh::processBox
+ (const ContactGeometry::TriangleMesh& mesh,
+ const ContactGeometry::TriangleMesh::OBBTreeNode& node,
+ const Transform& X_HM, const UnitVec3& hsNormal_M, Real hsFaceHeight_M,
+ std::set<int>& insideFaces) const
+{ // First check against the node's bounding box.
+
+ const OrientedBoundingBox& bounds = node.getBounds();
+ const Transform& X_MB = bounds.getTransform(); // box frame in mesh
+ const Vec3 p_BC = bounds.getSize()/2; // from box origin corner to center
+ // Express the half space normal in the box frame, then reflect it into
+ // the first (+,+,+) quadrant where it is the normal of a different
+ // but symmetric and more convenient half space.
+ const UnitVec3 octant1hsNormal_B = (~X_MB.R()*hsNormal_M).abs();
+ // Dot our octant1 radius p_BC with our octant1 normal to get
+ // the extent of the box from its center in the direction of the octant1
+ // reflection of the halfspace.
+ const Real extent = dot(p_BC, octant1hsNormal_B);
+ // Compute the height of the box center over the mesh origin,
+ // measured along the real halfspace normal.
+ const Vec3 boxCenter_M = X_MB*p_BC;
+ const Real boxCenterHeight_M = dot(boxCenter_M, hsNormal_M);
+ // Subtract the halfspace surface position to get the height of the
+ // box center over the halfspace.
+ const Real boxCenterHeight = boxCenterHeight_M - hsFaceHeight_M;
+ if (boxCenterHeight >= extent)
+ return; // no penetration
+ if (boxCenterHeight <= -extent) {
+ addAllTriangles(node, insideFaces); // box is entirely in halfspace
+ return;
+ }
+
+ // Box is partially penetrated into halfspace. If it is not a leaf node,
+ // check its children.
+ if (!node.isLeafNode()) {
+ processBox(mesh, node.getFirstChildNode(), X_HM, hsNormal_M,
+ hsFaceHeight_M, insideFaces);
+ processBox(mesh, node.getSecondChildNode(), X_HM, hsNormal_M,
+ hsFaceHeight_M, insideFaces);
+ return;
+ }
+
+ // This is a leaf OBB node that is penetrating, so some of its triangles
+ // may be penetrating.
+ const Array_<int>& triangles = node.getTriangles();
+ for (int i = 0; i < (int) triangles.size(); i++) {
+ for (int vx=0; vx < 3; ++vx) {
+ const int vertex = mesh.getFaceVertex(triangles[i], vx);
+ const Vec3& vertexPos = mesh.getVertexPosition(vertex);
+ const Real vertexHeight_M = dot(vertexPos, hsNormal_M);
+ if (vertexHeight_M < hsFaceHeight_M) {
+ insideFaces.insert(triangles[i]);
+ break; // done with this face
+ }
+ }
+ }
+}
+
+void ContactTracker::HalfSpaceTriangleMesh::addAllTriangles
+ (const ContactGeometry::TriangleMesh::OBBTreeNode& node,
+ std::set<int>& insideFaces) const
+{
+ if (node.isLeafNode()) {
+ const Array_<int>& triangles = node.getTriangles();
+ for (int i = 0; i < (int) triangles.size(); i++)
+ insideFaces.insert(triangles[i]);
+ }
+ else {
+ addAllTriangles(node.getFirstChildNode(), insideFaces);
+ addAllTriangles(node.getSecondChildNode(), insideFaces);
+ }
+}
+
+
+
+
+
+//==============================================================================
+// SPHERE - TRIANGLE MESH CONTACT TRACKER
+//==============================================================================
+// Cost is TODO
+bool ContactTracker::SphereTriangleMesh::trackContact
+ (const Contact& priorStatus,
+ const Transform& X_GS,
+ const ContactGeometry& geoSphere,
+ const Transform& X_GM,
+ const ContactGeometry& geoMesh,
+ Real cutoff,
+ Contact& currentStatus) const
+{
+ SimTK_ASSERT_ALWAYS
+ ( ContactGeometry::Sphere::isInstance(geoSphere)
+ && ContactGeometry::TriangleMesh::isInstance(geoMesh),
+ "ContactTracker::SphereTriangleMesh::trackContact()");
+
+ // We can't handle a "proximity" test, only penetration.
+ SimTK_ASSERT_ALWAYS(cutoff==0,
+ "ContactTracker::SphereTriangleMesh::trackContact()");
+
+ // No need for an expensive dynamic cast here; we know what we have.
+ const ContactGeometry::Sphere& sphere =
+ ContactGeometry::Sphere::getAs(geoSphere);
+ const ContactGeometry::TriangleMesh& mesh =
+ ContactGeometry::TriangleMesh::getAs(geoMesh);
+
+ // Transform giving mesh (M) frame in the sphere (S) frame.
+ const Transform X_SM = ~X_GS*X_GM;
+
+ // Want the sphere center measured and expressed in the mesh frame.
+ const Vec3 p_MC = (~X_SM).p();
+ std::set<int> insideFaces;
+ processBox(mesh, mesh.getOBBTreeNode(), p_MC, square(sphere.getRadius()),
+ insideFaces);
+
+ if (insideFaces.empty()) {
+ currentStatus.clear(); // not touching
+ return true; // successful return
+ }
+
+ currentStatus = TriangleMeshContact(priorStatus.getSurface1(),
+ priorStatus.getSurface2(),
+ X_SM,
+ std::set<int>(), insideFaces);
+ return true; // success
+}
+
+// Check a single OBB and its contents (recursively) against the sphere
+// whose center location in M and radius squared is given, appending any
+// penetrating faces to the insideFaces list.
+void ContactTracker::SphereTriangleMesh::processBox
+ (const ContactGeometry::TriangleMesh& mesh,
+ const ContactGeometry::TriangleMesh::OBBTreeNode& node,
+ const Vec3& center_M, Real radius2,
+ std::set<int>& insideFaces) const
+{ // First check against the node's bounding box.
+
+ const Vec3 nearest_M = node.getBounds().findNearestPoint(center_M);
+ if ((nearest_M-center_M).normSqr() >= radius2)
+ return; // no intersection possible
+
+ // Bounding box is penetrating. If it's not a leaf node, check its children.
+ if (!node.isLeafNode()) {
+ processBox(mesh, node.getFirstChildNode(), center_M, radius2,
+ insideFaces);
+ processBox(mesh, node.getSecondChildNode(), center_M, radius2,
+ insideFaces);
+ return;
+ }
+
+ // This is a leaf node that may be penetrating; check the triangles.
+ const Array_<int>& triangles = node.getTriangles();
+ for (unsigned i = 0; i < triangles.size(); i++) {
+ Vec2 uv;
+ Vec3 nearest_M = mesh.findNearestPointToFace
+ (center_M, triangles[i], uv);
+ if ((nearest_M-center_M).normSqr() < radius2)
+ insideFaces.insert(triangles[i]);
+ }
+}
+
+bool ContactTracker::SphereTriangleMesh::predictContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& predictedStatus) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactTracker::SphereTriangleMesh::predictContact() not implemented yet.");
+ return false; }
+
+bool ContactTracker::SphereTriangleMesh::initializeContact
+ (const Transform& X_GS1, const SpatialVec& V_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& contactStatus) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactTracker::SphereTriangleMesh::initializeContact() not implemented yet.");
+ return false; }
+
+
+
+
+
+
+//==============================================================================
+// TRIANGLE MESH - TRIANGLE MESH CONTACT TRACKER
+//==============================================================================
+// Cost is TODO
+bool ContactTracker::TriangleMeshTriangleMesh::trackContact
+ (const Contact& priorStatus,
+ const Transform& X_GM1,
+ const ContactGeometry& geoMesh1,
+ const Transform& X_GM2,
+ const ContactGeometry& geoMesh2,
+ Real cutoff,
+ Contact& currentStatus) const
+{
+ SimTK_ASSERT_ALWAYS
+ ( ContactGeometry::TriangleMesh::isInstance(geoMesh1)
+ && ContactGeometry::TriangleMesh::isInstance(geoMesh2),
+ "ContactTracker::TriangleMeshTriangleMesh::trackContact()");
+
+ // We can't handle a "proximity" test, only penetration.
+ SimTK_ASSERT_ALWAYS(cutoff==0,
+ "ContactTracker::TriangleMeshTriangleMesh::trackContact()");
+
+ // No need for an expensive dynamic cast here; we know what we have.
+ const ContactGeometry::TriangleMesh& mesh1 =
+ ContactGeometry::TriangleMesh::getAs(geoMesh1);
+ const ContactGeometry::TriangleMesh& mesh2 =
+ ContactGeometry::TriangleMesh::getAs(geoMesh2);
+
+ // Transform giving mesh2 (M2) frame in the mesh1 (M1) frame.
+ const Transform X_M1M2 = ~X_GM1*X_GM2;
+ std::set<int> insideFaces1, insideFaces2;
+
+ // Get M2's bounding box in M1's frame.
+ const OrientedBoundingBox
+ mesh2Bounds_M1 = X_M1M2*mesh2.getOBBTreeNode().getBounds();
+
+ // Find the faces that are actually intersecting faces on the other
+ // surface (this doesn't yet include faces that may be completely buried).
+ findIntersectingFaces(mesh1, mesh2,
+ mesh1.getOBBTreeNode(), mesh2.getOBBTreeNode(),
+ mesh2Bounds_M1, X_M1M2, insideFaces1, insideFaces2);
+
+ // It should never be the case that one set of faces is empty and the
+ // other isn't, however it is conceivable that roundoff error could cause
+ // this to happen so we'll check both lists.
+ if (insideFaces1.empty() && insideFaces2.empty()) {
+ currentStatus.clear(); // not touching
+ return true; // successful return
+ }
+
+ // There was an intersection. We now need to identify every triangle and
+ // vertex of each mesh that is inside the other mesh. We found the border
+ // intersections above; now we have to fill in the buried faces.
+ findBuriedFaces(mesh1, mesh2, ~X_M1M2, insideFaces1);
+ findBuriedFaces(mesh2, mesh1, X_M1M2, insideFaces2);
+
+ currentStatus = TriangleMeshContact(priorStatus.getSurface1(),
+ priorStatus.getSurface2(),
+ X_M1M2,
+ insideFaces1, insideFaces2);
+ return true; // success
+}
+
+void ContactTracker::TriangleMeshTriangleMesh::
+findIntersectingFaces
+ (const ContactGeometry::TriangleMesh& mesh1,
+ const ContactGeometry::TriangleMesh& mesh2,
+ const ContactGeometry::TriangleMesh::OBBTreeNode& node1,
+ const ContactGeometry::TriangleMesh::OBBTreeNode& node2,
+ const OrientedBoundingBox& node2Bounds_M1,
+ const Transform& X_M1M2,
+ std::set<int>& triangles1,
+ std::set<int>& triangles2) const
+{ // See if the bounding boxes intersect.
+
+ if (!node1.getBounds().intersectsBox(node2Bounds_M1))
+ return;
+
+ // If either node is not a leaf node, process the children recursively.
+
+ if (!node1.isLeafNode()) {
+ if (!node2.isLeafNode()) {
+ const OrientedBoundingBox firstChildBounds =
+ X_M1M2*node2.getFirstChildNode().getBounds();
+ const OrientedBoundingBox secondChildBounds =
+ X_M1M2*node2.getSecondChildNode().getBounds();
+ findIntersectingFaces(mesh1, mesh2, node1.getFirstChildNode(), node2.getFirstChildNode(), firstChildBounds, X_M1M2, triangles1, triangles2);
+ findIntersectingFaces(mesh1, mesh2, node1.getFirstChildNode(), node2.getSecondChildNode(), secondChildBounds, X_M1M2, triangles1, triangles2);
+ findIntersectingFaces(mesh1, mesh2, node1.getSecondChildNode(), node2.getFirstChildNode(), firstChildBounds, X_M1M2, triangles1, triangles2);
+ findIntersectingFaces(mesh1, mesh2, node1.getSecondChildNode(), node2.getSecondChildNode(), secondChildBounds, X_M1M2, triangles1, triangles2);
+ }
+ else {
+ findIntersectingFaces(mesh1, mesh2, node1.getFirstChildNode(), node2, node2Bounds_M1, X_M1M2, triangles1, triangles2);
+ findIntersectingFaces(mesh1, mesh2, node1.getSecondChildNode(), node2, node2Bounds_M1, X_M1M2, triangles1, triangles2);
+ }
+ return;
+ }
+ else if (!node2.isLeafNode()) {
+ const OrientedBoundingBox firstChildBounds =
+ X_M1M2*node2.getFirstChildNode().getBounds();
+ const OrientedBoundingBox secondChildBounds =
+ X_M1M2*node2.getSecondChildNode().getBounds();
+ findIntersectingFaces(mesh1, mesh2, node1, node2.getFirstChildNode(), firstChildBounds, X_M1M2, triangles1, triangles2);
+ findIntersectingFaces(mesh1, mesh2, node1, node2.getSecondChildNode(), secondChildBounds, X_M1M2, triangles1, triangles2);
+ return;
+ }
+
+ // These are both leaf nodes, so check triangles for intersections.
+
+ const Array_<int>& node1triangles = node1.getTriangles();
+ const Array_<int>& node2triangles = node2.getTriangles();
+ for (unsigned i = 0; i < node2triangles.size(); i++) {
+ const int face2 = node2triangles[i];
+ Vec3 a1 = X_M1M2*mesh2.getVertexPosition(mesh2.getFaceVertex(face2, 0));
+ Vec3 a2 = X_M1M2*mesh2.getVertexPosition(mesh2.getFaceVertex(face2, 1));
+ Vec3 a3 = X_M1M2*mesh2.getVertexPosition(mesh2.getFaceVertex(face2, 2));
+ const Geo::Triangle A(a1,a2,a3);
+ for (unsigned j = 0; j < node1triangles.size(); j++) {
+ const int face1 = node1triangles[j];
+ const Vec3& b1 = mesh1.getVertexPosition(mesh1.getFaceVertex(face1, 0));
+ const Vec3& b2 = mesh1.getVertexPosition(mesh1.getFaceVertex(face1, 1));
+ const Vec3& b3 = mesh1.getVertexPosition(mesh1.getFaceVertex(face1, 2));
+ const Geo::Triangle B(b1,b2,b3);
+ if (A.overlapsTriangle(B))
+ { // The triangles intersect.
+ triangles1.insert(face1);
+ triangles2.insert(face2);
+ }
+ }
+ }
+}
+
+static const int Outside = -1;
+static const int Unknown = 0;
+static const int Boundary = 1;
+static const int Inside = 2;
+
+void ContactTracker::TriangleMeshTriangleMesh::
+findBuriedFaces(const ContactGeometry::TriangleMesh& mesh, // M
+ const ContactGeometry::TriangleMesh& otherMesh, // O
+ const Transform& X_OM,
+ std::set<int>& insideFaces) const
+{
+ // Find which faces are inside.
+ // We're passed in the list of Boundary faces, that is, those faces of
+ // "mesh" that intersect faces of "otherMesh".
+ Array_<int> faceType(mesh.getNumFaces(), Unknown);
+ for (std::set<int>::iterator iter = insideFaces.begin();
+ iter != insideFaces.end(); ++iter)
+ faceType[*iter] = Boundary;
+
+ for (int i = 0; i < (int) faceType.size(); i++) {
+ if (faceType[i] == Unknown) {
+ // Trace a ray from its center to determine whether it is inside.
+ const Vec3 origin_O = X_OM * mesh.findCentroid(i);
+ const UnitVec3 direction_O = X_OM.R()* mesh.getFaceNormal(i);
+ Real distance;
+ int face;
+ Vec2 uv;
+ if ( otherMesh.intersectsRay(origin_O, direction_O, distance,
+ face, uv)
+ && ~direction_O*otherMesh.getFaceNormal(face) > 0)
+ {
+ faceType[i] = Inside;
+ insideFaces.insert(i);
+ } else
+ faceType[i] = Outside;
+
+ // Recursively mark adjacent inside or outside Faces.
+ tagFaces(mesh, faceType, insideFaces, i, 0);
+ }
+ }
+}
+
+//TODO: the following method uses depth-first recursion to iterate through
+//unmarked faces. For a large mesh this was observed to produce a stack
+//overflow in OpenSim. Here we limit the recursion depth; after we get that
+//deep we'll pop back out and do another expensive intersectsRay() test in
+//the method above.
+static const int MaxRecursionDepth = 500;
+
+void ContactTracker::TriangleMeshTriangleMesh::
+tagFaces(const ContactGeometry::TriangleMesh& mesh,
+ Array_<int>& faceType,
+ std::set<int>& triangles,
+ int index,
+ int depth) const
+{
+ for (int i = 0; i < 3; i++) {
+ const int edge = mesh.getFaceEdge(index, i);
+ const int face = (mesh.getEdgeFace(edge, 0) == index
+ ? mesh.getEdgeFace(edge, 1)
+ : mesh.getEdgeFace(edge, 0));
+ if (faceType[face] == Unknown) {
+ faceType[face] = faceType[index];
+ if (faceType[index] > 0)
+ triangles.insert(face);
+ if (depth < MaxRecursionDepth)
+ tagFaces(mesh, faceType, triangles, face, depth+1);
+ }
+ }
+}
+
+
+bool ContactTracker::TriangleMeshTriangleMesh::predictContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& predictedStatus) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactTracker::TriangleMeshTriangleMesh::predictContact() not implemented yet.");
+ return false; }
+
+bool ContactTracker::TriangleMeshTriangleMesh::initializeContact
+ (const Transform& X_GS1, const SpatialVec& V_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& contactStatus) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactTracker::TriangleMeshTriangleMesh::initializeContact() not implemented yet.");
+ return false; }
+
+
+
+//==============================================================================
+// CONVEX IMPLICIT SURFACE PAIR CONTACT TRACKER
+//==============================================================================
+// This will return an elliptical point contact.
+bool ContactTracker::ConvexImplicitPair::trackContact
+ (const Contact& priorStatus,
+ const Transform& X_GA,
+ const ContactGeometry& shapeA,
+ const Transform& X_GB,
+ const ContactGeometry& shapeB,
+ Real cutoff,
+ Contact& currentStatus) const
+{
+ SimTK_ASSERT_ALWAYS
+ ( shapeA.isConvex() && shapeA.isSmooth()
+ && shapeB.isConvex() && shapeB.isSmooth(),
+ "ContactTracker::ConvexImplicitPair::trackContact()");
+
+ // We'll work in the shape A frame.
+ const Transform X_AB = ~X_GA*X_GB; // 63 flops
+ const Rotation& R_AB = X_AB.R();
+
+ // 1. Get a rough guess at the contact points P and Q and contact normal.
+ Vec3 pointP_A, pointQ_B; // on A and B, resp.
+ UnitVec3 norm_A;
+ int numMPRIters;
+ const bool mightBeContact = estimateConvexImplicitPairContactUsingMPR
+ (shapeA, shapeB, X_AB,
+ pointP_A, pointQ_B, norm_A, numMPRIters);
+
+ #ifdef MPR_DEBUG
+ std::cout << "MPR: " << (mightBeContact?"MAYBE":"NO") << std::endl;
+ std::cout << " P=" << X_GA*pointP_A << " Q=" << X_GB*pointQ_B << std::endl;
+ std::cout << " N=" << X_GA.R()*norm_A << std::endl;
+ #endif
+
+ if (!mightBeContact) {
+ currentStatus.clear(); // definitely not touching
+ return true; // successful return
+ }
+
+ // 2. Refine the contact points to near machine precision.
+ const Real accuracyRequested = SignificantReal;
+ Real accuracyAchieved; int numNewtonIters;
+ bool converged = refineImplicitPair(shapeA, pointP_A, shapeB, pointQ_B,
+ X_AB, accuracyRequested, accuracyAchieved, numNewtonIters);
+
+ const Vec3 pointQ_A = X_AB*pointQ_B; // Q on B, measured & expressed in A
+
+ // 3. Compute the curvatures and surface normals of the two surfaces at
+ // P and Q. Once we have the first normal we can check whether there was
+ // actually any contact and duck out early if not.
+ Rotation R_AP; Vec2 curvatureP;
+ shapeA.calcCurvature(pointP_A, curvatureP, R_AP);
+
+ // If the surfaces are in contact then the vector from Q on surface B
+ // (supposedly inside A) to P on surface A (supposedly inside B) should be
+ // aligned with the outward normal on A.
+ const Real depth = dot(pointP_A-pointQ_A, R_AP.z());
+
+ #ifdef MPR_DEBUG
+ printf("MPR %2d iters, Newton %2d iters->accuracy=%g depth=%g\n",
+ numMPRIters, numNewtonIters, accuracyAchieved, depth);
+ #endif
+
+ if (depth <= 0) {
+ currentStatus.clear(); // not touching
+ return true; // successful return
+ }
+
+ // The surfaces are in contact.
+
+ Rotation R_BQ; Vec2 curvatureQ;
+ shapeB.calcCurvature(pointQ_B, curvatureQ, R_BQ);
+ const UnitVec3 maxDirB_A(R_AB*R_BQ.x()); // re-express in A
+
+ // 4. Compute the effective contact frame C and corresponding relative
+ // curvatures.
+ Transform X_AC; Vec2 curvatureC;
+
+ // Define the contact frame origin to be at the midpoint of P and Q.
+ X_AC.updP() = (pointP_A+pointQ_A)/2;
+
+ // Determine the contact frame orientations and composite curvatures.
+ ContactGeometry::combineParaboloids(R_AP, curvatureP,
+ maxDirB_A, curvatureQ,
+ X_AC.updR(), curvatureC);
+
+ // 5. Return the elliptical point contact for force generation.
+ currentStatus = EllipticalPointContact(priorStatus.getSurface1(),
+ priorStatus.getSurface2(),
+ X_AB, X_AC, curvatureC, depth);
+ return true; // success
+}
+
+
+bool ContactTracker::ConvexImplicitPair::predictContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& predictedStatus) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactTracker::ConvexImplicitPair::predictContact() not implemented yet.");
+ return false; }
+
+bool ContactTracker::ConvexImplicitPair::initializeContact
+ (const Transform& X_GS1, const SpatialVec& V_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& contactStatus) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactTracker::ConvexImplicitPair::initializeContact() not implemented yet.");
+ return false; }
+
+
+
+
+//==============================================================================
+// GENERAL IMPLICIT SURFACE PAIR CONTACT TRACKER
+//==============================================================================
+// This will return an elliptical point contact. TODO: should return a set
+// of contacts.
+//TODO: not implemented yet -- this is just the Convex-convex code here.
+bool ContactTracker::GeneralImplicitPair::trackContact
+ (const Contact& priorStatus,
+ const Transform& X_GA,
+ const ContactGeometry& shapeA,
+ const Transform& X_GB,
+ const ContactGeometry& shapeB,
+ Real cutoff,
+ Contact& currentStatus) const
+{
+ SimTK_ASSERT_ALWAYS
+ ( shapeA.isSmooth() && shapeB.isSmooth(),
+ "ContactTracker::GeneralImplicitPair::trackContact()");
+
+ // TODO: this won't work unless the shapes are actually convex.
+ return ConvexImplicitPair(shapeA.getTypeId(),shapeB.getTypeId())
+ .trackContact(priorStatus, X_GA, shapeA, X_GB, shapeB,
+ cutoff, currentStatus);
+}
+
+
+bool ContactTracker::GeneralImplicitPair::predictContact
+ (const Contact& priorStatus,
+ const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& predictedStatus) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactTracker::GeneralImplicitPair::predictContact() not implemented yet.");
+ return false; }
+
+bool ContactTracker::GeneralImplicitPair::initializeContact
+ (const Transform& X_GS1, const SpatialVec& V_GS1,
+ const ContactGeometry& surface1,
+ const Transform& X_GS2, const SpatialVec& V_GS2,
+ const ContactGeometry& surface2,
+ Real cutoff,
+ Real intervalOfInterest,
+ Contact& contactStatus) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactTracker::GeneralImplicitPair::initializeContact() not implemented yet.");
+ return false; }
+
+
+
+
+} // namespace SimTK
+
diff --git a/SimTKmath/Geometry/src/GCVSPL README.txt b/SimTKmath/Geometry/src/GCVSPL README.txt
new file mode 100755
index 0000000..82d4a38
--- /dev/null
+++ b/SimTKmath/Geometry/src/GCVSPL README.txt
@@ -0,0 +1,31 @@
+C version of GCVSPL
+
+This is a version of Herman Woltring's classic GCVSPL fortran program
+that has been translated to c. Herman's original fortran code has been
+translated with the free fortran to c translator, f2c. (For more
+information on f2c consult netlib at research.att.com where f2c is
+archived.)
+
+I have set f2c to translate the program to ANSI c. The test program
+included here is a hand coded version of Herman's original test program.
+This is included to show how gcvspl should be accessed in c. Note that
+the code produced by f2c is almost unreadable but it has compiled and
+run for me on a number of different operating systems (Unix,DOS,Mac).
+I did the hand coded example because even though the f2c conversion of
+the original worked it was unreadable.
+
+When you use gcvspl you with need the f2c.h file included here. Only a small
+number of the data types and structures defined in this file are actually
+used in the gcvspl.c code, so you can remove alot of the content of this
+file if you like.
+
+Lastly, there is an interesting addition to gcvspl that Ton van den
+Bogert mentioned awhile back. You can set the cutoff frequency of gcvspl
+or inquire what it selected as the cutoff frequency when it is run in
+automatic mode. This is not demonstrated in this code. However, I
+have implemented both these options in my gait package in the
+interface code to gcvspl (in the file filter.for of the program ANZ).
+Look in the same directory as this archive for more info on how get
+those files.
+
+dwight meglan
\ No newline at end of file
diff --git a/SimTKmath/Geometry/src/GCVSPLUtil.cpp b/SimTKmath/Geometry/src/GCVSPLUtil.cpp
new file mode 100644
index 0000000..b7dba66
--- /dev/null
+++ b/SimTKmath/Geometry/src/GCVSPLUtil.cpp
@@ -0,0 +1,36 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/GCVSPLUtil.h"
+
+namespace SimTK {
+
+void GCVSPLUtil::gcvspl(const Vector& x, const Vector& y, const Vector& wx, Real wy, int degree, int md, Real val, Vector& c, Vector& wk, int& ier) {
+ gcvspl(x, reinterpret_cast<const Vector_<Vec1>&>(y), wx, Vec1(wy), degree, md, val, reinterpret_cast<Vector_<Vec1>&>(c), wk, ier);
+}
+
+Real GCVSPLUtil::splder(int derivOrder, int degree, Real t, const Vector& x, const Vector& coeff) {
+ return splder(derivOrder, degree, t, x, reinterpret_cast<const Vector_<Vec1>&>(coeff))[0];
+}
+
+} // namespace SimTK
diff --git a/SimTKmath/Geometry/src/Geo.cpp b/SimTKmath/Geometry/src/Geo.cpp
new file mode 100644
index 0000000..e1cdd0f
--- /dev/null
+++ b/SimTKmath/Geometry/src/Geo.cpp
@@ -0,0 +1,63 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Non-inline static methods from the Geo class. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_Point.h"
+#include "simmath/internal/Geo_LineSeg.h"
+#include "simmath/internal/Geo_Box.h"
+#include "simmath/internal/Geo_CubicHermiteCurve.h"
+#include "simmath/internal/Geo_BicubicHermitePatch.h"
+#include "simmath/internal/Geo_CubicBezierCurve.h"
+#include "simmath/internal/Geo_BicubicBezierPatch.h"
+
+namespace SimTK {
+
+//==============================================================================
+// GEO
+//==============================================================================
+
+// Template instantiations for subclasses that don't have their own source
+// files.
+
+template class Geo::LineSeg_<float>;
+template class Geo::LineSeg_<double>;
+
+template class Geo::CubicHermiteCurve_<float>;
+template class Geo::CubicHermiteCurve_<double>;
+
+template class Geo::BicubicHermitePatch_<float>;
+template class Geo::BicubicHermitePatch_<double>;
+
+template class Geo::CubicBezierCurve_<float>;
+template class Geo::CubicBezierCurve_<double>;
+
+template class Geo::BicubicBezierPatch_<float>;
+template class Geo::BicubicBezierPatch_<double>;
+
+
+} // End of namespace SimTK
\ No newline at end of file
diff --git a/SimTKmath/Geometry/src/Geo_Box.cpp b/SimTKmath/Geometry/src/Geo_Box.cpp
new file mode 100644
index 0000000..ec5622e
--- /dev/null
+++ b/SimTKmath/Geometry/src/Geo_Box.cpp
@@ -0,0 +1,288 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Non-inline static methods from the Geo::Box_ class. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_Box.h"
+#include "simmath/internal/Geo_Sphere.h"
+
+#include <cstdio>
+#include <iostream>
+using std::cout; using std::endl;
+
+namespace SimTK {
+
+//==============================================================================
+// GEO :: BOX
+//==============================================================================
+
+// For testing whether this box intersects a given oriented box, we use the
+// separating axis method first described in Gottschalk, S., Lin, MC,
+// Manocha, D, "OBBTree: a hierarchical structure for rapid interference
+// detection." Proceedings of the 23rd Annual Conference on Computer
+// Graphics and Interactive Techniques, pp. 171-180, 1996. This is a series
+// of 15 tests used to determine whether there exists a separating plane
+// between the boxes. The possible plane normals are the 3 faces of each box,
+// and the 9 possible cross products of the three edge directions from each
+// box. In each case we project the boxes onto the plane normal lines
+// and see whether the projections overlap. If they don't, we have found a
+// separating plane and can return right away. This can be sped up by doing
+// only the first six face tests and accepting the occasional false positive
+// (allegedly less than 10%); we provide a method for doing that abbreviated
+// version of the test as well as the full method.
+
+// If the boxes have a parallel edge, they have to be handled specially because
+// the cross product of those edges will be zero. Then the projections of both
+// the box dimensions and the center-to-center distance will be zero, creating
+// a false appearance of having found a separating plane. Christer Ericson
+// advocates use of a "fudge factor" to ensure a conservative result, but David
+// Eberly tackles it more directly by noting that if any edge is parallel then
+// one of the six faces *must* be the separating plane if there is one. So if
+// you've done six tests and not found a separating plane there is no need to
+// check the edge cross products; the boxes do intersect. We follow Eberly's
+// approach.
+
+
+
+// First check only the B and O face normals as possible separating planes. If
+// we find one of those works the boxes can't be intersecting, otherwise they
+// might be. Ordering for speed:
+// - first check faces of this box B, because we're working in B frame
+// making these tests cheaper
+// - when working on a box, test smallest dimension first because that
+// covers the most space in one test
+// For a few extra flops (1 per axis), we can also spot whether one box center
+// is completely inside the other and if so report early that the boxes are
+// definitely intersecting. Worst case here is about 74 flops. Minimum is 16
+// flops if the first axis checked is a separating axis.
+
+// Private helper method returns 0 if a separating plane is found (definitely
+// no intersection), 2 if the centers overlap (definitely an intersection)
+// and 1 otherwise (probably an intersection, but maybe not).
+
+// It is convenient to calculate the absolute value of the Transform matrix
+// because those are reused. We'll do that here and return the results in case
+// the caller needs to use them. Note that the absolute value of a Rotation
+// matrix is not necessarily a Rotation since the resulting vectors might not
+// even be perpendicular.
+template <class P> int Geo::Box_<P>::
+intersectsOrientedBoxHelper(const OrientedBox_<P>& O,
+ Mat33P& absR_BO,
+ Vec3P& absP_BO) const {
+ const RotationP& R_BO = O.getTransform().R();
+ const Vec3P& p_BO = O.getTransform().p(); // center location
+
+ absR_BO = R_BO.asMat33().abs(); // about 6 flops
+ absP_BO = p_BO.abs(); // about 2 flops
+ const Geo::Box_<P>& ob = O.getBox();
+ const Vec3P& oh = ob.getHalfLengths();
+
+ bool centerOisInsideB = true; // set false when we see a counterexample
+
+ // Try each face of B in increasing order of box dimension so that we
+ // start with the skinniest slab of box B. Cost is 8 flops per axis.
+ // "V" below is the 1st quadrant (+++) vertex of each box.
+ for (int i=0; i<3; ++i) {
+ const CoordinateAxis b = getOrderedAxis(i);
+ const RealP rb = h[b]; // proj. p_BV onto axis b
+ const RealP ro = dot(oh, absR_BO[b]); // proj. p_OV onto axis b (in O)
+ const RealP d = absP_BO[b]; // ctr-ctr distance along b
+ if (d > rb+ro)
+ return 0; // axis b is a separating plane normal
+ if (d > rb)
+ centerOisInsideB = false; // saw a counterexample
+ }
+
+ if (centerOisInsideB)
+ return 2; // definitely does intersect!
+
+ bool centerBisInsideO = true;
+
+ // Try each face of O in increasing order of box dimension so that we
+ // start with the skinniest slab of box O. Cost is 14 flops per axis.
+ for (int i=0; i<3; ++i) {
+ const CoordinateAxis o = ob.getOrderedAxis(i);
+ // Note: round brackets indexing selects a column of R_BO1.
+ const RealP rb = dot(h, absR_BO(o)); // proj. p_BV onto axis o (in B)
+ const RealP ro = oh[o]; // proj. of p_OV onto axis o
+ const RealP d = std::abs(dot(p_BO, R_BO(o))); //B-O dist along o (in B)
+ if (d > rb+ro)
+ return 0; // axis o is a separating plane normal
+ if (d > ro)
+ centerBisInsideO = false; // saw a counterexample
+ }
+
+ if (centerBisInsideO)
+ return 2; // definitely does intersect!
+
+ return 1; // probably intersect
+}
+
+// Worst case 74 flops.
+template <class P> bool Geo::Box_<P>::
+mayIntersectOrientedBox(const Geo::OrientedBox_<P>& ob) const {
+ Mat33P absR_BO; Vec3P absP_BO;
+ const int result = intersectsOrientedBoxHelper(ob, absR_BO, absP_BO);
+ return result != 0;
+}
+
+// Worst case 74 + 9 + 9*12 = 191 flops.
+template <class P> bool Geo::Box_<P>::
+intersectsOrientedBox(const Geo::OrientedBox_<P>& ob) const {
+ Mat33P absR_BO;
+ Vec3P absP_BO;
+ const int result = intersectsOrientedBoxHelper(ob, absR_BO, absP_BO);
+ if (result == 0) return false;
+ else if (result == 2) return true;
+
+ // Indeterminate -- we still don't know if there is an intersection.
+
+ const RotationP& R_BO = ob.getTransform().R();
+ const Vec3P& p_BO = ob.getTransform().p(); // center location
+ const Vec3P& oh = ob.getHalfLengths();
+
+ // First we have to determine whether the boxes have a pair of edges that
+ // are (almost) parallel. If so, one of the cross product tests below will
+ // produce a near-zero separating vector candidate. Projections onto that
+ // line will yield zero lengths and we'll end up comparing noise to noise
+ // to see if we found a separating vector, producing unacceptable false
+ // negatives. On the other hand, if there is a pair of parallel edges, then
+ // one of the face normals we just tested would have to be a separating
+ // plane if there is one (draw a picture). In that case we know that we
+ // are intersecting and can return true now. This also serves as a fast
+ // out for the common case of parallel boxes.
+
+ // If there is a parallel edge, one of the entries for O's axes in |R_BO|
+ // will be within noise of one of B's axes, that is, 100, 010, or 001.
+ // David Eberly's algorithm looks for a "1" as cos(near zero) but that's a
+ // bad idea because sin() and cos() are very flat near 1. It is better to
+ // look for "0", as sin(near zero) because sin(theta)~=theta for small
+ // angles. So we pick an angle tolerance that we declare is small enough to
+ // be considered parallel, and then see if a column of |R_BO| has two
+ // entries that size or smaller. It costs 9 flops to do this test.
+
+ const RealP tol = Geo::getDefaultTol<P>(); // TODO: too tight?
+ for (int i=0; i<3; ++i) {
+ int numZeroes=0;
+ const Vec3P& v = absR_BO(i); // get column; row would work too
+ if (v[0] < tol) ++numZeroes; // 3 flops
+ if (v[1] < tol) ++numZeroes;
+ if (v[2] < tol) ++numZeroes;
+ assert(numZeroes < 3); // can't happen in a Rotation!
+ if (numZeroes==2)
+ return true; // boxes intersect
+ }
+
+ // There are no parallel edges. Check the 9 remaining possibilites for
+ // separating axes formed by the cross product of an edge from B and an
+ // edge from O. B's edges are 100,010,001 so the cross products simplify
+ // considerably: 100 X [o0 o1 o2] = [ 0 -o2 o1]
+ // 010 X [o0 o1 o2] = [ o2 0 -o0]
+ // 001 X [o0 o1 o2] = [-o1 o0 0]
+ // I don't know of any particularly good ordering for this step.
+
+ RealP rb, ro; // Projections of box dimensions on separating vector.
+ RealP d; // Projection of center-center distance on separating vector.
+
+ const Mat33P& R = R_BO.asMat33(); // abbreviations
+ const Vec3P& p = p_BO;
+ const Mat33P& Rabs = absR_BO;
+
+ // Each of these tests takes about 12 flops.
+ rb = h[1] *Rabs(2, 0)+h[2] *Rabs(1, 0); // b0 X oO
+ ro = oh[1]*Rabs(0, 2)+oh[2]*Rabs(0, 1);
+ d = std::abs(p[2]*R(1, 0) - p[1]*R(2, 0));
+ if (d > rb+ro) return false;
+
+ rb = h[1] *Rabs(2, 1)+h[2] *Rabs(1, 1); // b0 X o1
+ ro = oh[0]*Rabs(0, 2)+oh[2]*Rabs(0, 0);
+ d = std::abs(p[2]*R(1, 1) - p[1]*R(2, 1));
+ if (d > rb+ro) return false;
+
+ rb = h[1] *Rabs(2, 2)+h[2] *Rabs(1, 2); // b0 X o2
+ ro = oh[0]*Rabs(0, 1)+oh[1]*Rabs(0, 0);
+ d = std::abs(p[2]*R(1, 2) - p[1]*R(2, 2));
+ if (d > rb+ro) return false;
+
+ rb = h[0] *Rabs(2, 0)+h[2] *Rabs(0, 0); // b1 X oO
+ ro = oh[1]*Rabs(1, 2)+oh[2]*Rabs(1, 1);
+ d = std::abs(p[0]*R(2, 0) - p[2]*R(0, 0));
+ if (d > rb+ro) return false;
+
+ rb = h[0] *Rabs(2, 1)+h[2] *Rabs(0, 1); // b1 X o1
+ ro = oh[0]*Rabs(1, 2)+oh[2]*Rabs(1, 0);
+ d = std::abs(p[0]*R(2, 1) - p[2]*R(0, 1));
+ if (d > rb+ro) return false;
+
+ rb = h[0] *Rabs(2, 2)+h[2] *Rabs(0, 2); // b1 X o2
+ ro = oh[0]*Rabs(1, 1)+oh[1]*Rabs(1, 0);
+ d = std::abs(p[0]*R(2, 2) - p[2]*R(0, 2));
+ if (d > rb+ro) return false;
+
+ rb = h[0] *Rabs(1, 0)+h[1] *Rabs(0, 0); // b2 X oO
+ ro = oh[1]*Rabs(2, 2)+oh[2]*Rabs(2, 1);
+ d = std::abs(p[1]*R(0, 0) - p[0]*R(1, 0));
+ if (d > rb+ro) return false;
+
+ rb = h[0] *Rabs(1, 1)+h[1] *Rabs(0, 1); // b2 X o1
+ ro = oh[0]*Rabs(2, 2)+oh[2]*Rabs(2, 0);
+ d = std::abs(p[1]*R(0, 1) - p[0]*R(1, 1));
+ if (d > rb+ro) return false;
+
+ rb = h[0] *Rabs(1, 2)+h[1] *Rabs(0, 2); // b2 X o2
+ ro = oh[0]*Rabs(2, 1)+oh[1]*Rabs(2, 0);
+ d = std::abs(p[1]*R(0, 2) - p[0]*R(1, 2));
+ if (d > rb+ro) return false;
+
+ return true; // The boxes intersect.
+}
+
+
+
+
+// Explicit instantiations for float and double.
+template class Geo::Box_<float>;
+template class Geo::Box_<double>;
+
+//==============================================================================
+// GEO :: ALIGNED BOX
+//==============================================================================
+// Explicit instantiations for float and double.
+template class Geo::AlignedBox_<float>;
+template class Geo::AlignedBox_<double>;
+
+//==============================================================================
+// GEO :: ORIENTED BOX
+//==============================================================================
+
+
+
+// Explicit instantiations for float and double.
+template class Geo::OrientedBox_<float>;
+template class Geo::OrientedBox_<double>;
+
+} // End of namespace SimTK
\ No newline at end of file
diff --git a/SimTKmath/Geometry/src/Geo_Point.cpp b/SimTKmath/Geometry/src/Geo_Point.cpp
new file mode 100644
index 0000000..be69f81
--- /dev/null
+++ b/SimTKmath/Geometry/src/Geo_Point.cpp
@@ -0,0 +1,1379 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Non-inline static methods from the Geo::Point_ class. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/LinearAlgebra.h"
+#include "simmath/Optimizer.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_Point.h"
+#include "simmath/internal/Geo_Box.h"
+#include "simmath/internal/Geo_Sphere.h"
+
+#include <cstdio>
+#include <iostream>
+using std::cout; using std::endl;
+
+namespace SimTK {
+
+//==============================================================================
+// GEO :: POINT
+//==============================================================================
+
+// These local helpers find the minimum or maximum of several values and
+// return which one was the extreme value.
+template <class P>
+inline static void minOf(P a, P b, P& minVal, int& which) {
+ minVal=a; which=0;
+ if (b<minVal) minVal=b, which=1;
+}
+
+template <class P>
+inline static void minOf(P a, P b, P c, P& minVal, int& which) {
+ minVal=a; which=0;
+ if (b<minVal) minVal=b, which=1;
+ if (c<minVal) minVal=c, which=2;
+}
+
+template <class P>
+inline static void minOf(P a, P b, P c, P d, P& minVal, int& which) {
+ minVal=a; which=0;
+ if (b<minVal) minVal=b, which=1;
+ if (c<minVal) minVal=c, which=2;
+ if (d<minVal) minVal=d, which=3;
+}
+template <class P>
+inline static void maxOf(P a, P b, P& maxVal, int& which) {
+ maxVal=a; which=0;
+ if (b>maxVal) maxVal=b, which=1;
+}
+
+template <class P>
+inline static void maxOf(P a, P b, P c, P& maxVal, int& which) {
+ maxVal=a; which=0;
+ if (b>maxVal) maxVal=b, which=1;
+ if (c>maxVal) maxVal=c, which=2;
+}
+
+template <class P>
+inline static void maxOf(P a, P b, P c, P d, P& maxVal, int& which) {
+ maxVal=a; which=0;
+ if (b>maxVal) maxVal=b, which=1;
+ if (c>maxVal) maxVal=c, which=2;
+ if (d>maxVal) maxVal=d, which=3;
+}
+
+
+// This helper replaces the 0-based indices in "which" with corresponding
+// indices in "map". We assume map is long enough.
+inline static void fixWhich(const int* map, Array_<int>& which) {
+ for (unsigned i=0; i<which.size(); ++i)
+ which[i] = map[which[i]];
+}
+
+// Given an array of point locations, create an indirect array of pointers
+// to those point locations.
+template <class P> void
+makeIndirect(const Array_< Vec<3,P> >& points,
+ Array_<const Vec<3,P>*>& pointsIndirect) {
+ pointsIndirect.resize(points.size());
+ for (unsigned i=0; i < points.size(); ++i)
+ pointsIndirect[i] = &points[i];
+}
+
+
+// Given a set of points, find the one that is the furthest in a given
+// direction. There must be at least one point in the set.
+template <class P> /*static*/ void Geo::Point_<P>::
+findSupportPoint(const Array_<Vec3P>& points, const UnitVec3P& direction,
+ int& most, RealP& maxCoord) {
+ SimTK_APIARGCHECK(!points.empty(),"Geo::Point_", "findSupportPoint()",
+ "There must be at least one point in the set.");
+ most=0; maxCoord = dot(points[0],direction);
+ for (int i=1; i < (int)points.size(); ++i) {
+ const RealP coord = dot(points[i],direction);
+ if (coord > maxCoord) most=i, maxCoord=coord;
+ }
+}
+
+template <class P> /*static*/ void Geo::Point_<P>::
+findSupportPointIndirect(const Array_<const Vec3P*>& points,
+ const UnitVec3P& direction,
+ int& most, RealP& maxCoord) {
+ SimTK_APIARGCHECK(!points.empty(),"Geo::Point_",
+ "findSupportPointIndirect()",
+ "There must be at least one point in the set.");
+ most=0; maxCoord = dot(*points[0],direction);
+ for (int i=1; i < (int)points.size(); ++i) {
+ const RealP coord = dot(*points[i],direction);
+ if (coord > maxCoord) most=i, maxCoord=coord;
+ }
+}
+
+// Given a set of points, find the two points that are the most extreme along
+// a given direction (not necessarily distinct). There must be at least one
+// point in the set.
+template <class P> /*static*/ void Geo::Point_<P>::
+findExtremePoints(const Array_<Vec3P>& points, const UnitVec3P& direction,
+ int& least, int& most, RealP& leastCoord, RealP& mostCoord) {
+ SimTK_APIARGCHECK(!points.empty(),"Geo::Point_", "findExtremePoints()",
+ "There must be at least one point in the set.");
+ least=most=0;
+ leastCoord = dot(points[0],direction); mostCoord=leastCoord;
+ for (int i=1; i < (int)points.size(); ++i) {
+ const Vec3P& p = points[i];
+ const RealP coord = dot(p,direction);
+ if (coord < leastCoord) least=i, leastCoord=coord;
+ if (coord > mostCoord) most=i, mostCoord=coord;
+ }
+}
+
+template <class P> /*static*/ void Geo::Point_<P>::
+findExtremePointsIndirect(const Array_<const Vec3P*>& points,
+ const UnitVec3P& direction,
+ int& least, int& most,
+ RealP& leastCoord, RealP& mostCoord) {
+ SimTK_APIARGCHECK(!points.empty(),"Geo::Point_",
+ "findExtremePointsIndirect()",
+ "There must be at least one point in the set.");
+ least=most=0;
+ leastCoord = dot(*points[0],direction); mostCoord=leastCoord;
+ for (int i=1; i < (int)points.size(); ++i) {
+ const Vec3P& p = *points[i];
+ const RealP coord = dot(p,direction);
+ if (coord < leastCoord) least=i, leastCoord=coord;
+ if (coord > mostCoord) most=i, mostCoord=coord;
+ }
+}
+
+template <class P> /*static*/ Vec<3,P> Geo::Point_<P>::
+calcCentroid(const Array_<Vec3P>& points) {
+ SimTK_APIARGCHECK(!points.empty(),"Geo::Point_", "calcCentroid()",
+ "There must be at least one point in the set.");
+
+ const int n = (int)points.size();
+ const RealP oon = RealP(1)/n; // ~10 flops
+
+ Vec3P centroid(0);
+ for (int i=0; i < n; ++i)
+ centroid += points[i]; // 3 flops
+ centroid *= oon;
+
+ return centroid;
+}
+
+template <class P> /*static*/ Vec<3,P> Geo::Point_<P>::
+calcCentroidIndirect(const Array_<const Vec3P*>& points) {
+ SimTK_APIARGCHECK(!points.empty(),"Geo::Point_", "calcCentroidIndirect()",
+ "There must be at least one point in the set.");
+
+ const int n = (int)points.size();
+ const RealP oon = RealP(1)/n; // ~10 flops
+
+ Vec3P centroid(0);
+ for (int i=0; i < n; ++i)
+ centroid += *points[i]; // 3 flops
+ centroid *= oon;
+
+ return centroid;
+}
+
+template <class P> /*static*/ void Geo::Point_<P>::
+calcCovariance(const Array_<Vec3P>& points_F,
+ Vec3P& centroid, SymMat33P& covariance) {
+ Array_<const Vec3P*> indirect_F;
+ makeIndirect(points_F, indirect_F);
+ calcCovarianceIndirect(indirect_F, centroid, covariance);
+}
+
+template <class P> /*static*/ void Geo::Point_<P>::
+calcCovarianceIndirect(const Array_<const Vec3P*>& points_F,
+ Vec3P& centroid, SymMat33P& covariance) {
+ SimTK_APIARGCHECK(!points_F.empty(),"Geo::Point_",
+ "calcCovarianceIndirect()",
+ "There must be at least one point in the set.");
+
+ const int n = (int)points_F.size();
+ const RealP oon = RealP(1)/n; // ~10 flops
+
+ // Pass 1: find the centroid
+ centroid = RealP(0);
+ for (int i=0; i < n; ++i)
+ centroid += *points_F[i]; // 3 flops
+ centroid *= oon;
+
+ // Pass 2: calculate the covariance matrix.
+ covariance.setToZero();
+ Vec3P& diag = covariance.updDiag();
+ Vec3P& lower = covariance.updLower(); // 1,0 2,0 2,1
+ for (int i=0; i < n; ++i) {
+ const Vec3P p = *points_F[i] - centroid;
+ diag[0] += p[0]*p[0]; diag[1] += p[1]*p[1]; diag[2] += p[2]*p[2];
+ lower[0] += p[0]*p[1]; lower[1] += p[0]*p[2]; lower[2] += p[1]*p[2];
+ }
+
+ covariance *= oon;
+}
+
+
+template <class P> /*static*/ void Geo::Point_<P>::
+calcPrincipalComponents(const Array_<Vec3P>& points_F,
+ TransformP& X_FP) {
+ Array_<const Vec3P*> indirect_F;
+ makeIndirect(points_F, indirect_F);
+ calcPrincipalComponentsIndirect(indirect_F, X_FP);
+}
+
+template <class P> /*static*/ void Geo::Point_<P>::
+calcPrincipalComponentsIndirect(const Array_<const Vec3P*>& points_F,
+ TransformP& X_FP) {
+ SimTK_APIARGCHECK(!points_F.empty(),"Geo::Point_",
+ "calcPrincipalComponentsIndirect()",
+ "There must be at least one point in the set.");
+
+ Vec3P centroid;
+ SymMat33P covariance;
+ calcCovarianceIndirect(points_F, X_FP.updP(), covariance);
+
+ // Calculate eigenvalues and eigenvectors.
+ const Mat33P cov33(covariance);
+ Matrix_<P> cov(cov33);
+ Vector_< std::complex<P> > evals;
+ Matrix_< std::complex<P> > evecs;
+ Eigen(cov).getAllEigenValuesAndVectors(evals, evecs);
+
+ // Find the largest and smallest eigenvalues and corresponding vectors.
+ const Vec3P vals(evals[0].real(), evals[1].real(), evals[2].real());
+ int minIx, maxIx; RealP minVal, maxVal;
+ minOf(vals[0], vals[1], vals[2], minVal, minIx);
+ maxOf(vals[0], vals[1], vals[2], maxVal, maxIx);
+
+ if (maxIx == minIx) // eigenvalues must all be the same
+ maxIx = (minIx+1) % 3; // pick a different axis
+
+ // Eigenvectors for a real symmetric matrix are perpendicular and
+ // normalized already.
+ UnitVec3P longAxis( Vec3P(evecs(0,maxIx).real(), evecs(1,maxIx).real(),
+ evecs(2,maxIx).real()),
+ true); // "trust me" to prevent normalizing
+ UnitVec3P shortAxis(Vec3P(evecs(0,minIx).real(), evecs(1,minIx).real(),
+ evecs(2,minIx).real()),
+ true);
+ X_FP.updR() = RotationP(longAxis, XAxis, shortAxis, YAxis);
+}
+
+//==============================================================================
+// CALC AXIS ALIGNED BOUNDING BOX
+//==============================================================================
+
+
+template <class P> /*static*/ void Geo::Point_<P>::
+findAxisAlignedExtremePoints(const Array_<Vec3P>& points,
+ int least[3], int most[3],
+ Vec3P& low, Vec3P& high) {
+ SimTK_APIARGCHECK(!points.empty(),"Geo::Point_",
+ "findAxisAlignedExtremePoints()",
+ "There must be at least one point in the set.");
+
+ low=points[0]; high=points[0];
+ for (int i=0; i<3; i++) least[i]=most[i]=0; // point 0 most extreme so far
+
+ for (int i=1; i < (int)points.size(); ++i) {
+ const Vec3P& p = points[i];
+ if (p[0] < low [0]) low [0]=p[0], least[0]=i;
+ if (p[0] > high[0]) high[0]=p[0], most [0]=i;
+ if (p[1] < low [1]) low [1]=p[1], least[1]=i;
+ if (p[1] > high[1]) high[1]=p[1], most [1]=i;
+ if (p[2] < low [2]) low [2]=p[2], least[2]=i;
+ if (p[2] > high[2]) high[2]=p[2], most [2]=i;
+ }
+}
+
+// This signature taking an array of pointers to points rather than the
+// points themselves.
+
+template <class P> /*static*/ void Geo::Point_<P>::
+findAxisAlignedExtremePointsIndirect(const Array_<const Vec3P*>& points,
+ int least[3], int most[3],
+ Vec3P& low, Vec3P& high) {
+ SimTK_APIARGCHECK(!points.empty(),"Geo::Point_",
+ "findAxisAlignedExtremePointsIndirect()",
+ "There must be at least one point in the set.");
+
+ low=(*points[0]); high=(*points[0]);
+ for (int i=0; i<3; i++) least[i]=most[i]=0; // point 0 most extreme so far
+
+ for (int i=1; i < (int)points.size(); ++i) {
+ const Vec3P& p = (*points[i]);
+ if (p[0] < low [0]) low [0]=p[0], least[0]=i;
+ if (p[0] > high[0]) high[0]=p[0], most [0]=i;
+ if (p[1] < low [1]) low [1]=p[1], least[1]=i;
+ if (p[1] > high[1]) high[1]=p[1], most [1]=i;
+ if (p[2] < low [2]) low [2]=p[2], least[2]=i;
+ if (p[2] > high[2]) high[2]=p[2], most [2]=i;
+ }
+}
+
+
+// This signature taking an array of pointers to points rather than the
+// points themselves.
+template <class P> /*static*/
+Geo::AlignedBox_<P> Geo::Point_<P>::
+calcAxisAlignedBoundingBoxIndirect(const Array_<const Vec<3,P>*>& points,
+ Array_<int>& support) {
+ const unsigned npoints = points.size();
+ if (npoints == 0) {
+ const P inf = NTraits<P>::getInfinity();
+ support.clear();
+ return Geo::AlignedBox_<P>(Vec3P(inf),Vec3P(0));
+ }
+
+ // Find the extreme points along each of the coordinate axes.
+ int least[3], most[3];
+ Vec3P low, high;
+ findAxisAlignedExtremePointsIndirect(points, least, most, low, high);
+
+ // Sort the support points and eliminate duplicates.
+ std::set<int> supportSet;
+ for (unsigned i=0; i<3; ++i) {
+ supportSet.insert(least[i]);
+ supportSet.insert(most[i]);
+ }
+ support.assign(supportSet.begin(), supportSet.end());
+
+ const Vec3P ctr = (low+high)/2;
+ // Make sure nothing gets left out due to roundoff.
+ const Vec3P hdim(std::max(high[0]-ctr[0], ctr[0]-low[0]),
+ std::max(high[1]-ctr[1], ctr[1]-low[1]),
+ std::max(high[2]-ctr[2], ctr[2]-low[2]));
+
+ return AlignedBox_<P>(ctr, hdim).stretchBoundary();
+}
+
+
+// This signature takes an array of points, creates an array of pointers to
+// those points and calls the other signature.
+template <class P> /*static*/
+Geo::AlignedBox_<P> Geo::Point_<P>::
+calcAxisAlignedBoundingBox(const Array_< Vec<3,P> >& points,
+ Array_<int>& support) {
+ Array_<const Vec<3,P>*> indirect;
+ makeIndirect(points, indirect);
+ return calcAxisAlignedBoundingBoxIndirect(indirect, support);
+}
+
+//==============================================================================
+// CALC ORIENTED BOUNDING BOX
+//==============================================================================
+
+template <class P> /*static*/ void Geo::Point_<P>::
+findOrientedExtremePoints(const Array_<Vec3P>& points_F,
+ const RotationP& R_FB,
+ int least[3], int most[3],
+ Vec3P& low_B, Vec3P& high_B) {
+ SimTK_APIARGCHECK(!points_F.empty(),"Geo::Point_",
+ "findOrientedExtremePoints()",
+ "There must be at least one point in the set.");
+ const RotationP R_BF = ~R_FB;
+
+ low_B=R_BF*points_F[0]; high_B=R_BF*points_F[0];
+ for (int i=0; i<3; i++) least[i]=most[i]=0; // point 0 most extreme so far
+
+ for (int i=1; i < (int)points_F.size(); ++i) {
+ const Vec3P p = R_BF*points_F[i];
+ if (p[0] < low_B [0]) low_B [0]=p[0], least[0]=i;
+ if (p[0] > high_B[0]) high_B[0]=p[0], most [0]=i;
+ if (p[1] < low_B [1]) low_B [1]=p[1], least[1]=i;
+ if (p[1] > high_B[1]) high_B[1]=p[1], most [1]=i;
+ if (p[2] < low_B [2]) low_B [2]=p[2], least[2]=i;
+ if (p[2] > high_B[2]) high_B[2]=p[2], most [2]=i;
+ }
+}
+
+
+template <class P> /*static*/ void Geo::Point_<P>::
+findOrientedExtremePointsIndirect(const Array_<const Vec3P*>& points_F,
+ const RotationP& R_FB,
+ int least[3], int most[3],
+ Vec3P& low_B, Vec3P& high_B) {
+ SimTK_APIARGCHECK(!points_F.empty(),"Geo::Point_",
+ "findOrientedExtremePointsIndirect()",
+ "There must be at least one point in the set.");
+ const RotationP R_BF = ~R_FB;
+
+ low_B=R_BF*(*points_F[0]); high_B=R_BF*(*points_F[0]);
+ for (int i=0; i<3; i++) least[i]=most[i]=0; // point 0 most extreme so far
+
+ for (int i=1; i < (int)points_F.size(); ++i) {
+ const Vec3P p = R_BF*(*points_F[i]);
+ if (p[0] < low_B [0]) low_B [0]=p[0], least[0]=i;
+ if (p[0] > high_B[0]) high_B[0]=p[0], most [0]=i;
+ if (p[1] < low_B [1]) low_B [1]=p[1], least[1]=i;
+ if (p[1] > high_B[1]) high_B[1]=p[1], most [1]=i;
+ if (p[2] < low_B [2]) low_B [2]=p[2], least[2]=i;
+ if (p[2] > high_B[2]) high_B[2]=p[2], most [2]=i;
+ }
+}
+
+// Differentiate the above function being careful to note the low accuracy
+// when running in single precision.
+template <class P>
+class VolumeGradient : public Differentiator::GradientFunction {
+ typedef Vec<3,P> Vec3P;
+public:
+ VolumeGradient(const Array_<const Vec3P*>& points,
+ const Rotation_<P>& R_FB0)
+ : Differentiator::GradientFunction(3,(Real)Geo::getDefaultTol<P>()),
+ p(&points), R_FB0(R_FB0) {}
+
+ // This function calculates the volume as a function of rotation angles
+ // relative to the starting frame B0.
+ int f(const Vector& angles, Real& volume) const {
+ Vec3P a; a[0] = P(angles[0]); a[1] = P(angles[1]); a[2] = P(angles[2]);
+ Rotation_<P> R_B0B(BodyRotationSequence,
+ a[0], XAxis, a[1], YAxis, a[2], ZAxis);
+ int least[3], most[3];
+ Vec<3,P> low_B, high_B;
+ Geo::Point_<P>::findOrientedExtremePointsIndirect
+ (*p, R_FB0*R_B0B, least, most, low_B, high_B);
+ const Vec<3,P> extent(high_B - low_B);
+ volume = (Real)(extent[0]*extent[1]*extent[2]);
+ return 0;
+ }
+private:
+ const Array_<const Vec3P*>* p; // points to array of points
+ const Rotation_<P> R_FB0; // starting orientation
+};
+
+
+template <class P> /*static*/
+Geo::OrientedBox_<P> Geo::Point_<P>::
+calcOrientedBoundingBoxIndirect(const Array_<const Vec3P*>& points_F,
+ Array_<int>& support,
+ bool optimize) {
+ TransformP X_FB0;
+ //TODO: this is not a good initial guess because it is sensitive to
+ // point clustering and distribution of interior points. Should start
+ // with a convex hull, get the mass properties of that and use principal
+ // moments as directions.
+ calcPrincipalComponentsIndirect(points_F, X_FB0);
+
+ // We'll update these as we go.
+ RotationP R_FB = X_FB0.R();
+ Vec3P center_F = X_FB0.p();
+
+ int least[3], most[3];
+ Vec3P low_B, high_B;
+ findOrientedExtremePointsIndirect(points_F, R_FB,
+ least, most, low_B, high_B);
+
+ // Initial guess at OBB.
+ Vec3P extent_B = high_B - low_B;
+ RealP volume = extent_B[0]*extent_B[1]*extent_B[2];
+ center_F = R_FB*(high_B+low_B)/2;
+
+ // This is a very abbreviated steepest-descent optimizer. Starting with
+ // R_FB0 it uses three Euler angles a as parameters for an incremental
+ // rotation R_B0B(a) and calculates volume of the box whose orientation
+ // is R_FB(a)=R_FB0*R_B0B(a). It calculates the downhill gradient once,
+ // then does a line search along that gradient and quits as soon as it
+ // stops making significant progress.
+ if (optimize) {
+ VolumeGradient<P> grad(points_F, R_FB);
+ Differentiator diff(grad);
+ Vector g;
+ diff.calcGradient(Vector(3, Real(0)), (Real)volume, g);
+ Vec3P dir; dir[0]=P(g[0]); dir[1]=P(g[1]); dir[2]=P(g[2]);
+
+ // Gradient has units of volume/radian.
+ // Set initial step to attempt a 10% volume reduction.
+ RealP dirNorm = dir.norm();
+ RealP incr = volume/(10*dirNorm);
+ const RealP MinImprovement = RealP(.001); // .1% or give up
+ RealP minIncr = incr / 1000000;
+ RealP step = 0;
+ for (int i=0; i < 20; ++i) {
+ step -= incr;
+ Vec3P a(step*dir); // angles away from B0
+ Rotation_<P> R_B0B(BodyRotationSequence,
+ a[0], XAxis, a[1], YAxis, a[2], ZAxis);
+ Rotation_<P> tryR_FB = X_FB0.R()*R_B0B;
+ int tryLeast[3], tryMost[3];
+ Vec3P tryLow_B, tryHigh_B;
+ findOrientedExtremePointsIndirect
+ (points_F, tryR_FB, tryLeast, tryMost, tryLow_B, tryHigh_B);
+ Vec3P tryExtent_B = tryHigh_B - tryLow_B;
+ RealP tryVol = tryExtent_B[0]*tryExtent_B[1]*tryExtent_B[2];
+
+ if (tryVol < volume) {
+ const RealP improvement = (volume-tryVol)/volume;
+ for (int j=0; j<3; ++j)
+ least[j]=tryLeast[j], most[j]=tryMost[j];
+ R_FB = tryR_FB;
+ extent_B = tryExtent_B;
+ center_F = R_FB*(tryHigh_B+tryLow_B)/2;
+ volume = tryVol;
+ if (improvement < MinImprovement)
+ break;
+ incr *= RealP(1.5); // grow slowly
+ continue;
+ }
+
+ // Volume got worse.
+ step += incr; // back to previous best
+ if (incr <= minIncr)
+ break;
+ incr /= 10; // shrink fast
+ }
+ }
+
+ // Sort the support points and eliminate duplicates.
+ std::set<int> supportSet;
+ for (unsigned i=0; i<3; ++i) {
+ supportSet.insert(least[i]);
+ supportSet.insert(most[i]);
+ }
+ support.assign(supportSet.begin(), supportSet.end());
+
+ OrientedBox_<P> obb(TransformP(R_FB,center_F), extent_B/2);
+ return obb.stretchBoundary();
+}
+
+// This signature takes an array of points rather than an array of pointers
+// to points. Allocate a temporary array of pointers and then call the other
+// signature.
+template <class P> /*static*/
+Geo::OrientedBox_<P> Geo::Point_<P>::
+calcOrientedBoundingBox(const Array_<Vec3P>& points,
+ Array_<int>& support,
+ bool optimize) {
+ Array_<const Vec3P*> indirect;
+ makeIndirect(points, indirect);
+ return calcOrientedBoundingBoxIndirect(indirect,support,optimize);
+}
+
+
+
+//==============================================================================
+// CALC BOUNDING SPHERE - 2 POINTS
+//==============================================================================
+/* I bet you think this is easy! Unfortunately if the points in question are
+far from the origin (at 100, say) but close together, we can't compute the
+separation very accurately (around 100*eps for this example). So the resulting
+sphere can be too big (annoying but OK) or too small (very bad). Also, if the
+line length is <= tol, we want to consider these a single point rather than
+two supporting points for the sphere; in that case we pick one of the points
+and center the sphere around that with radius tol. About 45 flops. */
+template <class P> /*static*/
+Geo::Sphere_<P> Geo::Point_<P>::
+calcBoundingSphere(const Vec3P& p0, const Vec3P& p1, Array_<int>& which) {
+ const RealP tol = getDefaultTol<P>();
+
+ // Choose the tentative center, subject to scaled roundoff.
+ const Vec3P ctr = (p0 + p1)/2; // 6 flops
+
+ // Rather than using (p0-p1)/2 as the radius which can result in a sphere
+ // that doesn't include the points by a large margin (I tried it), measure
+ // the actual distances from the center to each point and use the larger
+ // as the radius. If that radius is <= tol/2 (meaning line length <= tol)
+ // then we'll just move the center to the closer point, set the radius to
+ // tol or more (depending on expected roundoff) and return with 1 support
+ // point (i.e., we consider this a point not a line).
+ const RealP p0d2 = (p0-ctr).normSqr(); // squared dist from center
+ const RealP p1d2 = (p1-ctr).normSqr(); // (8 flops each)
+
+ RealP rad;
+ which.clear();
+ if (p0d2 >= p1d2) {
+ rad = std::sqrt(p0d2); // 20 flops
+ if (rad <= tol/2) { // 2 flops
+ which.push_back(1);
+ return Sphere_<P>(p1, 0).stretchBoundary();
+ }
+ } else { // p1d2 > p0d2
+ rad = std::sqrt(p1d2); // same cost here
+ if (rad <= tol/2) {
+ which.push_back(0);
+ return Sphere_<P>(p0, 0).stretchBoundary();
+ }
+ }
+ // use both points
+ which.push_back(0); which.push_back(1);
+ return Sphere_<P>(ctr, rad).stretchBoundary();
+}
+
+
+
+//==============================================================================
+// CALC BOUNDING SPHERE - 3 POINTS
+//==============================================================================
+/* (Note that the 3- and 4-point methods from Nicolas Capens compute only the
+sphere that passes through all points (the circumsphere), which can be *way*
+too big.)
+
+This method produces the minimum bounding sphere around three points (i.e.,
+a triangle) for all cases except some very, very small triangles where it will
+give a small, but perhaps not minimal, bounding sphere. It is
+modified from Christer Ericson's blog: "Minimum bounding circle (sphere) for
+a triangle (tetrahedron)" July 27, 2007.
+http://realtimecollisiondetection.net/blog/?p=20
+Accessed 12/12/2011 and reimplemented by Sherm. The main change is handling of
+singular triangles and roundoff problems.
+
+Cost is about 110 flops for a typical case.
+
+Implementation
+--------------
+We have a triangle with vertices a,b,c with no restrictions on their placement
+(i.e., they can be singular in several different ways). Algorithm:
+ - Calculate the area of the triangle.
+ - If the area is very small:
+ - Find the longest edge from ab, ac, bc.
+ - Drop the vertex that is not used in the longest edge.
+ - Calculate the bounding sphere of the longest edge.
+ - If the dropped vertex is outside, stretch the sphere to include it.
+ - Return that sphere as the result (might not be perfect but will
+ be very good).
+ - Else (area is OK):
+ - Calculate barycentric coordinates s,t, u=1-s-t of the
+ circumsphere center P (the point equidistant to all vertices).
+ - If s,t, or u < 0, P lies outside the triangle and a 2-point sphere
+ will be smaller. Drop the corresponding vertex and return the
+ bounding sphere for the remaining edge (2 supporting points).
+ - Otherwise return the circumsphere (3 supporting points).
+ - There is an option to force use of the circumsphere regardless of
+ where P ends up; sometimes that is needed by the 4-point method.
+
+How to calculate the circumsphere:
+ We express center P as P=a + s*(b-a) + t*(c-a). This point is to be
+ equidistant from all vertices, giving us two equations for s and t:
+ (P-b)^2 = (P-a)^2, (P-c)^2 = (P-a)^2
+ Substituting the definition of P gives us the following system of equations
+ for s and t:
+ [ab^2 abac] [s] [ab^2/2]
+ [abac ac^2] [t] = [ac^2/2] or M x = b
+ We'll use Cramer's rule to solve the equation, which requires computing
+ determinants of three matrices: M, Ms, Mt where the latter are M with its
+ 1st or 2nd column replaced by b. Let dm=det(M), ds=det(Ms), dt=det(Mt).
+ Then s=ds/dm, t=dt/dm. Determinant dm is related to the area of the
+ triangle by 2*a = sqrt(dm), so dm = 4 a^2. Note that dm can't be negative
+ except possibly due to roundoff error. Below we'll actually calculate
+ 2*dm, 2*ds, etc. since that saves some multiplies.
+*/
+template <class P> /*static*/
+Geo::Sphere_<P> Geo::Point_<P>::
+calcBoundingSphere(const Vec3P& a, const Vec3P& b, const Vec3P& c,
+ bool forceCircumsphere, Array_<int>& which) {
+ const RealP tol = Geo::getDefaultTol<P>();
+
+ const Vec3P ab = b - a, ac = c - a; // 6 flops
+ const RealP ab2 = ab.normSqr(), ac2 = ac.normSqr(); // 10 flops
+ const RealP abac = dot(ab, ac); // == |ab||ac|cos theta 5 flops
+ const RealP ab2ac2 = ab2*ac2; // 1 flop
+
+ // The expression below is equivalent to
+ // dm2 = 2*dm = 2 * (|ab||ac|)^2 * (1-cos^2 theta)
+ // where theta is the angle between side ab and ac. That quantity can't
+ // be negative except for roundoff error. Note that the area of this
+ // triangle is area = 1/2 sqrt(ab2*ac2-square(abac)) so dm2 = 8*area^2
+ const RealP dm2 = 2*(ab2ac2 - square(abac)); // 3 flops
+
+ // We'll use one of three ways to find the center point and determine
+ // the set of supporting vertices (in which):
+ // 1) singular (2 or 1 support vertices)
+ // 2) non-singular with 2 support vertices
+ // 3) non-singular using circumsphere (3 support vertices)
+
+ Vec3P ctr;
+
+ // Consider the triangle singular if its area is less than tol.
+ if (dm2 <= 8*square(tol)) { // 3 flops
+ // Triangle is near singular or very small.
+ const Vec3P bc = c - b;
+ const RealP bc2 = bc.normSqr();
+ // Find the longest edge.
+ RealP maxLen2; int maxEdge;
+ maxOf(ab2,ac2,bc2,maxLen2,maxEdge);
+ // Now create a sphere around that edge using 2 or 1 support points,
+ // but we're only going to keep the center.
+ Sphere_<P> edgeSphere; int map[2];
+ switch(maxEdge) {
+ case 0: map[0]=0; map[1]=1; // (a,b) drop c
+ edgeSphere=calcBoundingSphere(a,b,which); break;
+ case 1: map[0]=0; map[1]=2; // (a,c) drop b
+ edgeSphere=calcBoundingSphere(a,c,which); break;
+ case 2: map[0]=1; map[1]=2; // (b,c) drop a
+ edgeSphere=calcBoundingSphere(b,c,which); break;
+ };
+ fixWhich(map, which); // fix the indices
+ ctr = edgeSphere.getCenter();
+ } else {
+ // Triangle is non-singular. It is still possible that we won't need
+ // all 3 points to be on the sphere surface. If one or more of the
+ // barycentric coordinates is negative, it means that the circumsphere
+ // center is outside the triangle. Pick the most negative, then drop
+ // the opposite vertex. The circumsphere around the remaining edge will
+ // include the dropped one for free.
+
+ // s controls P's height over ac, t over ab, u=1-s-t over bc.
+ const RealP ds2 = ab2ac2 - ac2*abac; // 2 flops
+ const RealP dt2 = ab2ac2 - ab2*abac; // 2 flops
+ const RealP du2 = dm2-ds2-dt2; // 2 flops
+ const RealP oodm2 = 1/dm2; // (> 0) ~10 flops
+ const RealP s = ds2*oodm2, t = dt2*oodm2, u = du2*oodm2; // 3 flops
+
+ RealP minBary; int minBaryIx;
+ minOf(s,t,u,minBary,minBaryIx); // 2 flops
+
+ if (!forceCircumsphere && minBary <= 0) { // 1 flop
+ Sphere_<P> edgeSphere; int map[2];
+ switch(minBaryIx) {
+ case 0: // s is the most negative
+ map[0]=0; map[1]=2; // sphere around ac includes b
+ edgeSphere=calcBoundingSphere(a,c,which);
+ break;
+ case 1: // t is the most negative
+ map[0]=0; map[1]=1; // sphere around ab includes c
+ edgeSphere=calcBoundingSphere(a,b,which);
+ break;
+ case 2: // u=1-s-t is the most negative
+ map[0]=1; map[1]=2; // sphere around bc includes a
+ edgeSphere=calcBoundingSphere(b,c,which);
+ break;
+ };
+ fixWhich(map, which);
+ ctr = edgeSphere.getCenter();
+ } else { // s,t,u > 0 or we're forced to use circumsphere
+ // All barycentric coordinates are positive. The circumsphere's
+ // center will be inside the triangle and thus of minimal size
+ // (unless we were forced to use the circumsphere).
+ which.clear();
+ which.push_back(0); which.push_back(1); which.push_back(2);
+ const Vec3P aToCenter = s*ab + t*ac; // 9 flops
+ ctr = a + aToCenter; // 3 flops
+ }
+ }
+
+ // All methods lead here. At this point we have the support points
+ // and center point chosen but still need to pick the radius.
+
+ // This cleans up ugly roundoff errors that cause the center not to be
+ // exactly equidistant from the points. This makes sure that the radius
+ // touches the outermost point with the rest inside. This is
+ // expensive but worth it to ensure a trouble-free sphere.
+ RealP rmax2; int rmaxIx;
+ maxOf((a-ctr).normSqr(),(b-ctr).normSqr(),(c-ctr).normSqr(),
+ rmax2, rmaxIx); // 27 flops
+ const RealP rad = std::sqrt(rmax2); // 20 flops
+
+ // If the max radius point wasn't in the support set, we have to add it.
+ if ( which.size() < 3
+ && std::find(which.begin(), which.end(), rmaxIx) == which.end())
+ which.push_back(rmaxIx);
+
+ return Sphere_<P>(ctr, rad).stretchBoundary();
+}
+
+
+
+//==============================================================================
+// CALC BOUNDING SPHERE - 4 POINTS
+//==============================================================================
+/* This is a 4 point bounding sphere calculator based on Crister Ericson's
+suggestion on his blog page -- see the 3 point routine above. The derivation
+here was done by Sherm since Ericson didn't work out the 4-point case.
+
+Cost is about 190 flops in a typical case where all 4 points are used.
+
+Implementation
+--------------
+We have a tetrahedron with vertices a,b,c,d with no restrictions on their
+placement (i.e., they can be singular in a bunch of different ways). Algorithm:
+ - Calculate the volume of the tetrahedron.
+ - If the volume is very small:
+ - Find the face of largest area from abc, abd, acd, bcd.
+ - Drop the vertex that is not used in the largest face.
+ - Calculate the bounding sphere of the largest face.
+ - If the dropped vertex is outside, stretch the sphere to include it.
+ - Return that sphere as the result (might not be perfect but will
+ be very good).
+ - Else (volume is OK):
+ - Calculate barycentric coordinates s,t,u, v=1-s-t-u of the
+ circumsphere center P (the point equidistant to all vertices).
+ - If s,t,u, or v < 0, P lies outside the tetrahedron and a 3-point
+ sphere will be smaller. Drop the corresponding vertex and return the
+ bounding sphere for the remaining face. (3 support points).
+ - Otherwise return the circumsphere (4 support points).
+ - There is an option to force use of the circumsphere regardless of
+ where P ends up.
+
+How to calculate the circumsphere:
+ We express center P as P=a + s*(b-a) + t*(c-a) + u*(d-a). This point is to
+ be equidistant from all vertices, giving us 3 equations for s, t, and u:
+ (P-b)^2 = (P-a)^2, (P-c)^2 = (P-a)^2, (P-d)^2 = (P-a)^2
+ Substituting the definition of P gives us the following system of equations
+ for s, t, and u:
+ [ab^2 abac abad] [s] [ab^2/2]
+ [abac ac^2 acad] [t] = [ac^2/2] or M x = b
+ [abad acad ad^2] [u] = [ad^2/2]
+ We'll use Cramer's rule to solve the equation, which requires computing
+ determinants of four matrices: M, Ms, Mt, Mu where the latter are M with
+ its 1st, 2nd, or 3rd column replaced by b. Let dm=det(M), ds=det(Ms),
+ dt=det(Mt), du=det(Mu). Then s=ds/dm, t=dt/dm, u=du/dm. There are going to
+ be lots of common subexpressions, and we can pick up face areas along the
+ way. Also, determinant dm is related to the volume of the tetrahedron by
+ 6*v = sqrt(dm), so dm = 36 v^2. Note that dm can't be negative except
+ possibly due to roundoff error. Below we'll work with 2*dm, 2*ds, etc.
+ because it saves some multiplies.
+*/
+template <class P> /*static*/
+Geo::Sphere_<P> Geo::Point_<P>::
+calcBoundingSphere(const Vec3P& a, const Vec3P& b,
+ const Vec3P& c, const Vec3P& d,
+ bool forceCircumsphere, Array_<int>& which) {
+ const RealP tol = Geo::getDefaultTol<P>();
+
+ const Vec3P ab = b-a, ac = c-a, ad = d-a; // 9 flops
+ const RealP abac = dot(ab, ac); // == |ab||ac|cos theta ( 5 flops)
+ const RealP abad = dot(ab, ad); // == |ab||ad|cos theta ( 5 flops)
+ const RealP acad = dot(ac, ad); // == |ac||ad|cos theta ( 5 flops)
+ // 25 flops in the next two lines.
+ const RealP ab2=ab.normSqr(), ac2=ac.normSqr(), ad2=ad.normSqr();
+ const RealP abac2=square(abac), abad2=square(abad), acad2=square(acad);
+ const RealP ab2ac2=ab2*ac2, ab2ad2=ab2*ad2, ac2ad2=ac2*ad2;
+ const RealP ab2ac2ad2=ab2ac2*ad2;
+
+ const RealP dm2 = 2*( ab2ac2ad2 // 10 flops
+ - ab2*acad2
+ - ad2*abac2
+ - ac2*abad2
+ + 2*abac*abad*acad); // 72 v^2
+
+ // We'll use one of three ways to find the center point and determine
+ // the set of supporting vertices (in which):
+ // 1) singular (3, 2, or 1 support vertices)
+ // 2) non-singular with 3 support vertices
+ // 3) non-singular using circumsphere (4 support vertices)
+
+ Vec3P ctr;
+
+ // Consider the tetrahedron singular if its volume is less than tol.
+ if (dm2 <= 72*square(tol)) { // 3 flops
+ // Tetrahedron is near singular or very small.
+ const Vec3P bc = c-b, cd = d-c;
+ const RealP bccd = dot(bc, cd), bccd2 = square(bccd);
+ const RealP bc2=bc.normSqr(), cd2=cd.normSqr(), bc2cd2=bc2*cd2;
+ // Find the largest face.The expressions below are equivalent to, for
+ // example: (|ab||ac|)^2 * (1-cos^2 theta)
+ // where theta is the angle between side ab and ac. Note that the area
+ // of this triangle is area = 1/2 sqrt(ab2*ac2-square(abac)).
+ const RealP abc4Area2 = ab2ac2-abac2; // 4*area(abc)^2
+ const RealP abd4Area2 = ab2ad2-abad2; // 4*area(abd)^2
+ const RealP acd4Area2 = ac2ad2-acad2; // 4*area(acd)^2
+ const RealP bcd4Area2 = bc2cd2-bccd2; // 4*area(bcd)^2
+ RealP maxArea2; int maxFace=0;
+ maxOf(abc4Area2,abd4Area2,acd4Area2,bcd4Area2,maxArea2,maxFace);
+ // Now create a sphere around that edge using 3,2, or 1 support points,
+ // but we're only going to keep the center.
+ Sphere_<P> faceSphere; int map[3];
+ switch(maxFace) {
+ case 0: map[0]=0; map[1]=1; map[2]=2; //abc, drop d
+ faceSphere=calcBoundingSphere(a,b,c,false,which); break;
+ case 1: map[0]=0; map[1]=1; map[2]=3; //abd, drop c
+ faceSphere=calcBoundingSphere(a,b,d,false,which); break;
+ case 2: map[0]=0; map[1]=2; map[2]=3; //acd, drop b
+ faceSphere=calcBoundingSphere(a,c,d,false,which); break;
+ case 3: map[0]=1; map[1]=2; map[2]=3; //bcd, drop a
+ faceSphere=calcBoundingSphere(b,c,d,false,which); break;
+ };
+ fixWhich(map, which); // fix the indices
+ ctr = faceSphere.getCenter();
+ } else {
+ // Tetrahedron is non-singular. It is still possible that we won't need
+ // all four points to be on the sphere surface. If one or more of the
+ // barycentric coordinates is negative, it means that the circumsphere
+ // center is outside the tetrahedron. Pick the most negative, then drop
+ // the opposite vertex. The circumsphere around the remaining face will
+ // include the dropped one for free.
+
+ // s controls height over acd, t over abd, u over abc, 1-s-t-u over bcd.
+ const RealP ds2 = ab2ac2ad2 // 12 flops
+ + ac2*abad*acad
+ + ad2*abac*acad
+ - ab2*acad2
+ - ac2ad2*abac
+ - ac2ad2*abad;
+ const RealP dt2 = ab2ac2ad2 // 12 flops
+ + ab2*abad*acad
+ + ad2*abac*abad
+ - ac2*abad2
+ - ab2ad2*acad
+ - ab2ad2*abac;
+ const RealP du2 = ab2ac2ad2 // 12 flops
+ + ab2*abac*acad
+ + ac2*abac*abad
+ - ad2*abac2
+ - ab2ac2*acad
+ - ab2ac2*abad;
+ const RealP dv2 = dm2-ds2-dt2-du2; // 3 flops
+ const RealP oodm2 = 1/dm2; // (> 0) ~10 flops
+ const RealP s=ds2*oodm2, t=dt2*oodm2, u=du2*oodm2, v=dv2*oodm2;
+ // 4 flops
+
+ RealP minBary; int minBaryIx;
+ minOf(s,t,u,v,minBary,minBaryIx); // 3 flops
+
+ if (!forceCircumsphere && minBary <= 0) { // 1 flop
+ Sphere_<P> faceSphere; int map[3];
+ switch(minBaryIx) {
+ case 0: // s is the most negative
+ map[0]=0; map[1]=2; map[2]=3; // sphere around acd includes b
+ faceSphere=calcBoundingSphere(a,c,d,false,which);
+ if (which.size()<3 && faceSphere.isPointOutside(b))
+ faceSphere=calcBoundingSphere(a,c,d,true,which);
+ break;
+ case 1: // t is the most negative
+ map[0]=0; map[1]=1; map[2]=3; // sphere around abd includes c
+ faceSphere=calcBoundingSphere(a,b,d,false,which);
+ if (which.size()<3 && faceSphere.isPointOutside(c))
+ faceSphere=calcBoundingSphere(a,b,d,true,which);
+ break;
+ case 2: // u is the most negative
+ map[0]=0; map[1]=1; map[2]=2; // sphere around abc includes d
+ faceSphere=calcBoundingSphere(a,b,c,false,which);
+ if (which.size()<3 && faceSphere.isPointOutside(d))
+ faceSphere=calcBoundingSphere(a,b,c,true,which);
+ break;
+ case 3: // v is the most negative
+ map[0]=1; map[1]=2; map[2]=3; // sphere around bcd includes a
+ faceSphere=calcBoundingSphere(b,c,d,false,which);
+ if (which.size()<3 && faceSphere.isPointOutside(d))
+ faceSphere=calcBoundingSphere(b,c,d,true,which);
+ break;
+ };
+ fixWhich(map, which);
+ ctr = faceSphere.getCenter();
+ } else { // s,t,u,v > 0 or we're forced to use circumsphere
+ // All barycentric coordinates are positive. The circumsphere's
+ // center will be inside the tetrahedron and thus of minimal size
+ // (unless we were forced to use the circumsphere).
+ which.clear();
+ which.push_back(0); which.push_back(1);
+ which.push_back(2); which.push_back(3);
+ const Vec3P aToCenter = s*ab + t*ac + u*ad; // 12 flops
+ ctr = a + aToCenter; // 3 flops
+ }
+ }
+
+ // All methods lead here. At this point we have the support points
+ // and center point chosen but still need to pick the radius.
+
+ // This cleans up ugly roundoff errors that cause the center not to be
+ // exactly equidistant from the points. This makes sure that the radius
+ // touches the outermost point with the rest inside. This is
+ // expensive but worth it to ensure a trouble-free sphere.
+ RealP rmax2; int rmaxIx;
+ maxOf((a-ctr).normSqr(),(b-ctr).normSqr(),
+ (c-ctr).normSqr(),(d-ctr).normSqr(),
+ rmax2, rmaxIx); // 35 flops
+ const RealP rad = std::sqrt(rmax2); // 20 flops
+
+ // If the max radius point wasn't in the support set, we have to add it.
+ if ( which.size() < 4
+ && std::find(which.begin(), which.end(), rmaxIx) == which.end())
+ which.push_back(rmaxIx);
+
+ return Sphere_<P>(ctr, rad).stretchBoundary(); // 6 flops
+}
+
+
+
+//==============================================================================
+// CALC BOUNDING SPHERE - N POINTS
+//==============================================================================
+// This is called recursively to calculate the minimum bounding sphere for a
+// set of points (like the vertices of a mesh). It uses an algorithm developed
+// by Emo Welzl which, despite appearances, has O(n) expected running time.
+// We tried the implementation by Nicolas Capens at
+// http://www.flipcode.com/archives/Smallest_Enclosing_Spheres.shtml.
+// As described there, the algorithm is highly susceptible to numerical
+// instabilities. Bernd Gartner describes an improved version in "Fast and
+// robust smallest enclosing balls", Proc. 7th Annual ACM European Symposium on
+// Algorithms, v. 1643 Lecture Notes in Computer Science, pp. 325-338, 1999.
+// The implementation here follows Gartner's "pivoting" method which uses
+// Welzl's recursive algorithm only for maximum sets of 5 points. But here the
+// primitives have been reworked so that they deal nicely with singularities
+// and roundoff. TODO: there are still rare problems that crop up in random-
+// point tests. With about 5 million random sets of 1000 points (see TestGeo)
+// I found a case that went into an infinite loop (now falls back to Ritter
+// sphere after enough attempts). I have also seen the
+// minimal sphere come out considerably larger than the crude Ritter sphere
+// generated by calcApproxBoundingSphere(); this is also very rare but bears
+// investigating. On average I see the Welzl sphere's volume about 20% smaller
+// than Ritter's. (sherm 20111227)
+
+// It is possible that we didn't need as support points all the bIn points
+// we were given, but instead used a smaller number, bActual. In that case
+// we have to make sure that the *first* bActual are the support points.
+// So we'll look at the entries [bActual..bIn-1]; if one is now part of the
+// support set we'll swap it with a now-unused point in [0..bActual-1].
+static void moveSupportToFront(int bIn, const Array_<int>& support,
+ Array_<int>& ix) {
+ const int bActual = (int)support.size();
+ for (int i=bActual; i < bIn; ++i) {
+ const int* s = std::find(support.begin(), support.end(), ix[i]);
+ if (s == support.end()) continue; // not being used
+ // ix[i] is part of the support set
+ bool swapped=false; // There has to be one to swap with!
+ for (int j=0; j < bActual; ++j) {
+ const int* ss = std::find(support.begin(), support.end(), ix[j]);
+ if (ss == support.end()) {
+ // ix[j] is not part of the support set; swap with i
+ std::swap(ix[i], ix[j]); swapped=true;
+ break;
+ }
+ }
+ assert(swapped); // can't happen
+ }
+}
+
+template <class P> static
+Geo::Sphere_<P>
+findWelzlSphere(const Array_<const Vec<3,P>*>& p, Array_<int>& ix,
+ int bIn, Array_<int>& which, int recursionLevel) {
+
+ // The first bIn points should be an independent support set, and we're
+ // hoping to calculate their circumsphere here. Although in theory the
+ // algorithm wouldn't have gotten here if all bIn points weren't
+ // independent, roundoff issues may cause the underlying primitive to
+ // disagree. So we might use only a subset of the available points, and
+ // "which" will list the ones we used. If necessary we'll rearrange the
+ // first few entries in "ix" to make sure that the indices of the new
+ // support set come first.
+
+ Geo::Sphere_<P> minSphere;
+
+ switch(bIn) {
+ // Create a bounding sphere for 0, 1, 2, 3, or 4 points. Note which
+ // points were actually used, and how many. The original algorithm returned
+ // when four points are used, but we still have to check that we have the
+ // *right* four points because our primitives might not have used all
+ // the points they were given earlier and we need to make sure the new
+ // 4-point circumsphere includes the points that were dropped.
+ case 0:
+ minSphere = Geo::Sphere_<P>(Vec<3,P>(NTraits<P>::getInfinity()),0);
+ which.clear();
+ break;
+ case 1:
+ minSphere = Geo::Point_<P>::calcBoundingSphere(*p[ix[0]],which);
+ break;
+ case 2:
+ minSphere = Geo::Point_<P>::calcBoundingSphere
+ (*p[ix[0]],*p[ix[1]],which);
+ break;
+ case 3:
+ minSphere = Geo::Point_<P>::calcBoundingSphere
+ (*p[ix[0]],*p[ix[1]],*p[ix[2]],false,which);
+ break;
+ case 4:
+ minSphere = Geo::Point_<P>::calcBoundingSphere
+ (*p[ix[0]],*p[ix[1]],*p[ix[2]],*p[ix[3]],false,which);
+ break;
+ }
+
+ // We have a sphere that surrounds all bIn points but might not have
+ // used them all as support in "which".
+
+ // Fix the indices in which (the primitives number from 0).
+ fixWhich(ix.begin(), which);
+ // Make sure the support set is at the front of ix.
+ moveSupportToFront(bIn, which, ix);
+
+ if (bIn==4) {
+ // There can only be 4 or 5 points. If 4 we're done.
+ if (ix.size()==4 || !minSphere.isPointOutside(*p[ix[4]]))
+ return minSphere; // all the points already enclosed
+ // We failed with the current "left out" point in ix[4]. We'll try
+ // the other four possibilities until one works.
+ for (int i=0; i<4; ++i) {
+ for (int j = 4; j > 0; --j) // rotate last point in
+ std::swap(ix[j], ix[j-1]);
+ minSphere = Geo::Point_<P>::calcBoundingSphere
+ (*p[ix[0]],*p[ix[1]],*p[ix[2]],*p[ix[3]],false,which);
+ fixWhich(ix.begin(), which);
+ if (!minSphere.isPointOutside(*p[ix[4]])) {
+ moveSupportToFront(bIn, which, ix);
+ return minSphere;
+ }
+ }
+ // If we get here then no sphere around 4 points included the 5th.
+ // That should be impossible and I've never seen it happen. But ...
+ // I can't prove it would never happen given finite arithmetic so
+ // just in case we'll generate a Ritter sphere, which will always work.
+ Array_<const Vec<3,P>*> ritterPoints;
+ for (int i=0; i < (int)ix.size(); ++i)
+ ritterPoints.push_back(p[ix[i]]);
+ minSphere =
+ Geo::Point_<P>::calcApproxBoundingSphereIndirect(ritterPoints);
+ which.clear(); // we don't know
+ return minSphere;
+ }
+
+
+ // The indices of the support points are within the first bIn entries in
+ // ix, although they may not be in the first bActual slots. The first bIn
+ // may therefore contain some unused points we already processed
+ // but didn't need. Now run through all subsequent points and update the
+ // sphere to include them if necessary, each time ensuring that all
+ // previous points remain included.
+
+ for (int i = bIn; i < (int)ix.size(); ++i) {
+ // We expect the sphere already to have been enlarged to deal with
+ // roundoff so we can do an exact test here.
+ if (minSphere.isPointOutside(*p[ix[i]])) {
+ // This point is outside the current bounding sphere.
+ // Move it to the start of the list *without* reordering.
+ for (int j = i; j > 0; --j)
+ std::swap(ix[j], ix[j-1]);
+
+ // Update the bounding sphere, taking the new point into account
+ // and ensuring that the resulting sphere also includes all the
+ // previous points as well. No more than 4 support points are ever
+ // needed.
+ ArrayView_<int> toBoundIx(ix.begin(), &ix[i]+1);
+ minSphere = findWelzlSphere<P>(p, toBoundIx,
+ std::min(bIn+1,4),
+ which, recursionLevel+1);
+
+ }
+ }
+
+ return minSphere; // Already stretched for roundoff.
+}
+
+// This signature takes an array of points, creates an array of pointers to
+// those points and calls the other signature.
+template <class P> /*static*/
+Geo::Sphere_<P> Geo::Point_<P>::
+calcBoundingSphere(const Array_<Vec3P>& points, Array_<int>& which) {
+ Array_<const Vec3P*> indirect;
+ makeIndirect(points, indirect);
+ return calcBoundingSphereIndirect(indirect, which);
+}
+
+// This was the outer block for the basic Emo Welzl "move to front" algorithm
+// (algorithm 1 in Gartner's paper). I found it unreliable and sometimes slow
+// in practice and switched to Gartner's "pivoting" algorithm below.
+
+//template <class P> /*static*/
+//Geo::Sphere_<P> Geo::Point_<P>::
+//calcBoundingSphere(const Array_<const Vec3P*>& points, Array_<int>& which) {
+// const unsigned npoints = points.size();
+//
+// // Allocate and initialize an array of point indices. These will get
+// // moved around during the computation.
+// Array_<int> ix(npoints); for (unsigned i=0; i<npoints; ++i) ix[i] = i;
+//
+// if (npoints < 10) {
+// // Not worth rearranging.
+// return findWelzlSphere<P>(points, ix, 1, which, 0);
+// }
+//
+// // There are enough points that we'll try to improve the ordering so that
+// // the bounding sphere gets large quickly. This optimization helps *a lot*
+// // for large numbers of points.
+//
+// // Find the six points that have the most extreme
+// // x,y, and z coordinates (not necessarily six unique points) and move
+// // them to the front so they get processed first.
+// Vec3P lo=*points[0], hi=*points[0]; // initialize extremes
+// int ilo[3], ihi[3]; for (int i=0; i<3; ++i) ilo[i]=ihi[i]=0;
+// for (unsigned i=0; i<points.size(); ++i) {
+// const Vec3P& p = *points[i];
+// if (p[0] > hi[0]) hi[0]=p[0], ihi[0]=i;
+// if (p[0] < lo[0]) lo[0]=p[0], ilo[0]=i;
+// if (p[1] > hi[1]) hi[1]=p[1], ihi[1]=i;
+// if (p[1] < lo[1]) lo[1]=p[1], ilo[1]=i;
+// if (p[2] > hi[2]) hi[2]=p[2], ihi[2]=i;
+// if (p[2] < lo[2]) lo[2]=p[2], ilo[2]=i;
+// }
+// // Find the nx <= 6 unique extreme points.
+// std::set<int> pending;
+// pending.insert(ilo, ilo+3); pending.insert(ihi, ihi+3);
+// const int nx = pending.size();
+// // Go through the first n points. If the point is already an extreme,
+// // remove it from the pending list. If not, swap it with one of the
+// // extreme points.
+// for (int i=0; i < nx; ++i) {
+// std::set<int>::iterator p = pending.find(ix[i]);
+// if (p != pending.end()) {
+// pending.erase(p);
+// continue;
+// }
+// p = pending.begin(); // first unmoved extreme
+// const int extremeIx = *p;
+// pending.erase(p);
+// std::swap(ix[i], ix[extremeIx]);
+// }
+//
+// return findWelzlSphere<P>(points, ix, 1, which, 0);
+//}
+
+template <class P> /*static*/
+Geo::Sphere_<P> Geo::Point_<P>::
+calcBoundingSphereIndirect(const Array_<const Vec3P*>& points,
+ Array_<int>& which) {
+ const unsigned npoints = points.size();
+ if (npoints == 0)
+ return Geo::Sphere_<P>(Vec<3,P>(NTraits<P>::getInfinity()),0);
+
+ // Allocate and initialize an array of point indices. These will get
+ // moved around during the computation.
+ Array_<int> ix(npoints); for (unsigned i=0; i<npoints; ++i) ix[i] = i;
+
+ // Sherm 20111225: there are still very obscure cases where this
+ // code may get stuck in a cycle and iterate forever. It usually only
+ // takes a few iterations to finish, so 100 means something is very wrong.
+ // In that case we'll give up and return a Ritter sphere.
+ const int MaxIters = 100;
+
+ unsigned nxt=0; // "t" in Gartner's paper
+ Geo::Sphere_<P> minSphere = calcBoundingSphere(*points[ix[nxt++]], which);
+
+ // Loop until all the points are inside minSphere.
+ int s=1; // number of support points
+ for(int iters=0; ; ++iters) {
+ // Find worst point.
+ const Vec3P& center = minSphere.getCenter();
+ RealP maxDist2=0; unsigned worst=0;
+ for (unsigned i=nxt; i < ix.size(); ++i) {
+ const RealP d2 = (*points[ix[i]] - center).normSqr();
+ if (d2 > maxDist2) maxDist2=d2, worst=i;
+ }
+ if (maxDist2 <= square(minSphere.getRadius()))
+ break;
+
+ if (iters == MaxIters) {
+ minSphere = calcApproxBoundingSphereIndirect(points);
+ which.clear(); // we don't know
+ break;
+ }
+
+ // Point worst (>=nxt) is outside the current bounding sphere.
+ // Move it to the start of the list *without* reordering.
+ for (int j = worst; j > 0; --j)
+ std::swap(ix[j], ix[j-1]);
+
+ // Make a list of the initial s+1 points, s <= 4.
+ ArrayView_<int> toBoundIx(ix.begin(), &ix[s]+1);
+
+ nxt = s+1; // we're going to bound points 0..s
+ s=std::min(s+1,4); // number of supports to try for next
+ minSphere = findWelzlSphere<P>(points, toBoundIx, s, which, 0);
+
+ }
+
+ return minSphere;
+}
+
+// This signature takes an array of points, creates an array of pointers to
+// those points and calls the other signature.
+template <class P> /*static*/
+Geo::Sphere_<P> Geo::Point_<P>::
+calcApproxBoundingSphere(const Array_<Vec3P>& points) {
+ Array_<const Vec3P*> indirect;
+ makeIndirect(points, indirect);
+ return calcApproxBoundingSphereIndirect(indirect);
+}
+
+// Calculate a Ritter sphere using the method described by Christer Ericson
+// in Real Time Collision Detection, Elsevier 2005, pp. 89-91. This method
+// makes one pass to find some reasonably far apart points, then starts with
+// a sphere around those two points. Then it makes another pass growing the
+// sphere to just include the current sphere plus the new point.
+template <class P> /*static*/
+Geo::Sphere_<P> Geo::Point_<P>::
+calcApproxBoundingSphereIndirect(const Array_<const Vec3P*>& points) {
+ const unsigned npoints = points.size();
+ if (npoints == 0)
+ return Geo::Sphere_<P>(Vec<3,P>(0),0);
+
+ // Find the most-separated pair of points along one of the coordinate axes.
+ Vec3P lo=*points[0], hi=*points[0]; // initialize extremes
+ int ilo[3], ihi[3]; for (int i=0; i<3; ++i) ilo[i]=ihi[i]=0;
+ for (unsigned i=0; i<points.size(); ++i) {
+ const Vec3P& p = *points[i];
+ if (p[0] > hi[0]) hi[0]=p[0], ihi[0]=i;
+ if (p[0] < lo[0]) lo[0]=p[0], ilo[0]=i;
+ if (p[1] > hi[1]) hi[1]=p[1], ihi[1]=i;
+ if (p[1] < lo[1]) lo[1]=p[1], ilo[1]=i;
+ if (p[2] > hi[2]) hi[2]=p[2], ihi[2]=i;
+ if (p[2] < lo[2]) lo[2]=p[2], ilo[2]=i;
+ }
+ const RealP xdist2 = (*points[ihi[0]]-*points[ilo[0]]).normSqr();
+ const RealP ydist2 = (*points[ihi[0]]-*points[ilo[0]]).normSqr();
+ const RealP zdist2 = (*points[ihi[0]]-*points[ilo[0]]).normSqr();
+ RealP maxVal; int which;
+ maxOf(xdist2, ydist2, zdist2, maxVal, which);
+ const Vec3P& pmin = *points[ilo[which]];
+ const Vec3P& pmax = *points[ihi[which]];
+
+ Geo::Sphere_<P> minSphere;
+ Vec3P& ctr = minSphere.updCenter(); // aliases
+ RealP& rad = minSphere.updRadius();
+ ctr = (pmin+pmax)/2;
+ // Calculating radius this way ensures that roundoff won't leave one of
+ // the points outside. We'll do a final roundoff adjustment at the end.
+ rad = std::sqrt(std::max((pmax-ctr).normSqr(),
+ (pmin-ctr).normSqr()));
+
+ // Now run through all the points again and grow the sphere if necessary.
+ // This requires moving the center too so we don't grow more than needed.
+ for (unsigned i=0; i<points.size(); ++i) {
+ const Vec3P& p = *points[i];
+ const Vec3P ctr2p = p-ctr;
+ const RealP dist2 = ctr2p.normSqr();
+ if (dist2 > square(rad)) {
+ const RealP dist = std::sqrt(dist2);
+ const RealP newrad = (dist + rad)/2;
+ ctr += (newrad - rad)/dist * ctr2p; // has roundoff issues
+ rad = newrad;
+ // Make sure we didn't miss the new point due to roundoff.
+ const RealP newdist2 = (p-ctr).normSqr();
+ if (newdist2 > square(rad))
+ rad = std::sqrt(newdist2);
+ }
+ }
+
+ return minSphere.stretchBoundary();
+}
+
+// Explicit instantiations for float and double.
+template class Geo::Point_<float>;
+template class Geo::Point_<double>;
+
+
+} // End of namespace SimTK
\ No newline at end of file
diff --git a/SimTKmath/Geometry/src/Geo_Sphere.cpp b/SimTKmath/Geometry/src/Geo_Sphere.cpp
new file mode 100644
index 0000000..bb116d5
--- /dev/null
+++ b/SimTKmath/Geometry/src/Geo_Sphere.cpp
@@ -0,0 +1,49 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Non-inline static methods from the Geo::Sphere_ class. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_Point.h"
+#include "simmath/internal/Geo_Sphere.h"
+
+#include <cstdio>
+#include <iostream>
+using std::cout; using std::endl;
+
+namespace SimTK {
+
+//==============================================================================
+// GEO :: SPHERE
+//==============================================================================
+
+
+// Explicit instantiations for float and double.
+template class Geo::Sphere_<float>;
+template class Geo::Sphere_<double>;
+
+
+} // End of namespace SimTK
\ No newline at end of file
diff --git a/SimTKmath/Geometry/src/Geo_Triangle.cpp b/SimTKmath/Geometry/src/Geo_Triangle.cpp
new file mode 100644
index 0000000..d73703e
--- /dev/null
+++ b/SimTKmath/Geometry/src/Geo_Triangle.cpp
@@ -0,0 +1,632 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Non-inline static methods from the Geo::Triangle class. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_LineSeg.h"
+#include "simmath/internal/Geo_Triangle.h"
+
+namespace SimTK {
+
+//==============================================================================
+// GEO :: TRIANGLE
+//==============================================================================
+
+// These are defined below.
+template <class RealP>
+static bool tri_tri_overlap_test_3d
+ (const RealP p1[3], const RealP q1[3], const RealP r1[3],
+ const RealP p2[3], const RealP q2[3], const RealP r2[3]);
+
+template <class RealP>
+static bool tri_tri_intersection_test_3d
+ (const RealP p1[3], const RealP q1[3], const RealP r1[3],
+ const RealP p2[3], const RealP q2[3], const RealP r2[3],
+ bool& coplanar, RealP source[3], RealP target[3]);
+
+template <class P>
+bool Geo::Triangle_<P>::
+overlapsTriangle(const Triangle_<P>& other) const {
+ return tri_tri_overlap_test_3d<P>
+ (&v[0][0], &v[1][0], &v[2][0],
+ &other.v[0][0], &other.v[1][0], &other.v[2][0]);
+}
+
+template <class P>
+bool Geo::Triangle_<P>::
+intersectsTriangle(const Triangle_<P>& other, LineSeg_<P>& seg,
+ bool& isCoplanar) const {
+ return tri_tri_intersection_test_3d<P>
+ (&v[0][0], &v[1][0], &v[2][0],
+ &other.v[0][0], &other.v[1][0], &other.v[2][0],
+ isCoplanar, &seg[0][0], &seg[1][0]);
+}
+
+
+//==============================================================================
+// Triangle-triangle overlap routines
+//==============================================================================
+/*
+*
+* Triangle-Triangle Overlap Test Routines
+* July, 2002
+* Updated December 2003
+*
+* This file contains C implementation of algorithms for
+* performing two and three-dimensional triangle-triangle intersection test
+* The algorithms and underlying theory are described in
+*
+* "Fast and Robust Triangle-Triangle Overlap Test
+* Using Orientation Predicates" P. Guigue - O. Devillers
+*
+* Journal of Graphics Tools, 8(1), 2003
+*
+* Several geometric predicates are defined. Their parameters are all
+* points. Each point is an array of two or three double precision
+* floating point numbers. The geometric predicates implemented in
+* this file are:
+*
+* bool tri_tri_overlap_test_3d(p1,q1,r1,p2,q2,r2)
+* bool tri_tri_overlap_test_2d(p1,q1,r1,p2,q2,r2)
+*
+* bool tri_tri_intersection_test_3d(p1,q1,r1,p2,q2,r2,
+* coplanar,source,target)
+*
+* is a version that computes the segment of intersection when
+* the triangles overlap (and are not coplanar)
+*
+* each function returns true if the triangles (including their
+* boundary) intersect, otherwise false
+*
+*
+* Other information are available from the Web page
+* http://www.acm.org/jgt/papers/GuigueDevillers03/
+*
+*/
+
+
+// These static functions are from the original C implementation, with minor
+// modifications. "RealP" will be instantiated for float and double.
+
+
+template <class RealP>
+static bool coplanar_tri_tri3d
+ (const RealP p1[3], const RealP q1[3], const RealP r1[3],
+ const RealP p2[3], const RealP q2[3], const RealP r2[3],
+ const RealP N1[3], const RealP N2[3]);
+
+template <class RealP>
+static bool tri_tri_overlap_test_2d
+ (const RealP p1[2], const RealP q1[2], const RealP r1[2],
+ const RealP p2[2], const RealP q2[2], const RealP r2[2]);
+
+
+/* coplanar returns whether the triangles are coplanar
+* source and target are the endpoints of the segment of
+* intersection if it exists)
+*/
+
+
+/* some 3D macros */
+
+#define CROSS(dest,v1,v2) \
+ dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
+ dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
+ dest[2]=v1[0]*v2[1]-v1[1]*v2[0];
+
+#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])
+
+
+
+#define SUB(dest,v1,v2) dest[0]=v1[0]-v2[0]; \
+ dest[1]=v1[1]-v2[1]; \
+ dest[2]=v1[2]-v2[2];
+
+
+#define SCALAR(dest,alpha,v) dest[0] = alpha * v[0]; \
+ dest[1] = alpha * v[1]; \
+ dest[2] = alpha * v[2];
+
+
+
+#define CHECK_MIN_MAX(p1,q1,r1,p2,q2,r2) {\
+ SUB(v1,p2,q1)\
+ SUB(v2,p1,q1)\
+ CROSS(N1,v1,v2)\
+ SUB(v1,q2,q1)\
+ if (DOT(v1,N1) > 0) return false;\
+ SUB(v1,p2,p1)\
+ SUB(v2,r1,p1)\
+ CROSS(N1,v1,v2)\
+ SUB(v1,r2,p1) \
+ if (DOT(v1,N1) > 0) return false;\
+ else return true; }
+
+
+
+/* Permutation in a canonical form of T2's vertices */
+
+#define TRI_TRI_3D(p1,q1,r1,p2,q2,r2,dp2,dq2,dr2) { \
+ if (dp2 > 0) { \
+ if (dq2 > 0) CHECK_MIN_MAX(p1,r1,q1,r2,p2,q2) \
+ else if (dr2 > 0) CHECK_MIN_MAX(p1,r1,q1,q2,r2,p2)\
+ else CHECK_MIN_MAX(p1,q1,r1,p2,q2,r2) }\
+ else if (dp2 < 0) { \
+ if (dq2 < 0) CHECK_MIN_MAX(p1,q1,r1,r2,p2,q2)\
+ else if (dr2 < 0) CHECK_MIN_MAX(p1,q1,r1,q2,r2,p2)\
+ else CHECK_MIN_MAX(p1,r1,q1,p2,q2,r2)\
+ } else { \
+ if (dq2 < 0) { \
+ if (dr2 >= 0) CHECK_MIN_MAX(p1,r1,q1,q2,r2,p2)\
+ else CHECK_MIN_MAX(p1,q1,r1,p2,q2,r2)\
+ } \
+ else if (dq2 > 0) { \
+ if (dr2 > 0) CHECK_MIN_MAX(p1,r1,q1,p2,q2,r2)\
+ else CHECK_MIN_MAX(p1,q1,r1,q2,r2,p2)\
+ } \
+ else { \
+ if (dr2 > 0) CHECK_MIN_MAX(p1,q1,r1,r2,p2,q2)\
+ else if (dr2 < 0) CHECK_MIN_MAX(p1,r1,q1,r2,p2,q2)\
+ else return coplanar_tri_tri3d(p1,q1,r1,p2,q2,r2,N1,N2);\
+ }}}
+
+
+//==============================================================================
+// Three-dimensional Triangle-Triangle Overlap Test
+//==============================================================================
+template <class RealP> static bool
+tri_tri_overlap_test_3d(const RealP p1[3], const RealP q1[3], const RealP r1[3],
+ const RealP p2[3], const RealP q2[3], const RealP r2[3])
+{
+ RealP dp1, dq1, dr1, dp2, dq2, dr2;
+ RealP v1[3], v2[3];
+ RealP N1[3], N2[3];
+
+ /* Compute distance signs of p1, q1 and r1 to the plane of
+ triangle(p2,q2,r2) */
+
+
+ SUB(v1,p2,r2)
+ SUB(v2,q2,r2)
+ CROSS(N2,v1,v2)
+
+ SUB(v1,p1,r2)
+ dp1 = DOT(v1,N2);
+ SUB(v1,q1,r2)
+ dq1 = DOT(v1,N2);
+ SUB(v1,r1,r2)
+ dr1 = DOT(v1,N2);
+
+ if (((dp1 * dq1) > 0) && ((dp1 * dr1) > 0)) return false;
+
+ /* Compute distance signs of p2, q2 and r2 to the plane of
+ triangle(p1,q1,r1) */
+
+
+ SUB(v1,q1,p1)
+ SUB(v2,r1,p1)
+ CROSS(N1,v1,v2)
+
+ SUB(v1,p2,r1)
+ dp2 = DOT(v1,N1);
+ SUB(v1,q2,r1)
+ dq2 = DOT(v1,N1);
+ SUB(v1,r2,r1)
+ dr2 = DOT(v1,N1);
+
+ if (((dp2 * dq2) > 0) && ((dp2 * dr2) > 0)) return false;
+
+ /* Permutation in a canonical form of T1's vertices */
+
+
+ if (dp1 > 0) {
+ if (dq1 > 0) TRI_TRI_3D(r1,p1,q1,p2,r2,q2,dp2,dr2,dq2)
+ else if (dr1 > 0) TRI_TRI_3D(q1,r1,p1,p2,r2,q2,dp2,dr2,dq2)
+ else TRI_TRI_3D(p1,q1,r1,p2,q2,r2,dp2,dq2,dr2)
+ } else if (dp1 < 0) {
+ if (dq1 < 0) TRI_TRI_3D(r1,p1,q1,p2,q2,r2,dp2,dq2,dr2)
+ else if (dr1 < 0) TRI_TRI_3D(q1,r1,p1,p2,q2,r2,dp2,dq2,dr2)
+ else TRI_TRI_3D(p1,q1,r1,p2,r2,q2,dp2,dr2,dq2)
+ } else {
+ if (dq1 < 0) {
+ if (dr1 >= 0) TRI_TRI_3D(q1,r1,p1,p2,r2,q2,dp2,dr2,dq2)
+ else TRI_TRI_3D(p1,q1,r1,p2,q2,r2,dp2,dq2,dr2)
+ }
+ else if (dq1 > 0) {
+ if (dr1 > 0) TRI_TRI_3D(p1,q1,r1,p2,r2,q2,dp2,dr2,dq2)
+ else TRI_TRI_3D(q1,r1,p1,p2,q2,r2,dp2,dq2,dr2)
+ }
+ else {
+ if (dr1 > 0) TRI_TRI_3D(r1,p1,q1,p2,q2,r2,dp2,dq2,dr2)
+ else if (dr1 < 0) TRI_TRI_3D(r1,p1,q1,p2,r2,q2,dp2,dr2,dq2)
+ else return coplanar_tri_tri3d(p1,q1,r1,p2,q2,r2,N1,N2);
+ }
+ }
+};
+
+
+
+template <class RealP> static bool
+coplanar_tri_tri3d(const RealP p1[3], const RealP q1[3], const RealP r1[3],
+ const RealP p2[3], const RealP q2[3], const RealP r2[3],
+ const RealP normal_1[3], const RealP normal_2[3]){
+
+ RealP P1[2],Q1[2],R1[2];
+ RealP P2[2],Q2[2],R2[2];
+
+ RealP n_x, n_y, n_z;
+
+ n_x = ((normal_1[0]<0)?-normal_1[0]:normal_1[0]);
+ n_y = ((normal_1[1]<0)?-normal_1[1]:normal_1[1]);
+ n_z = ((normal_1[2]<0)?-normal_1[2]:normal_1[2]);
+
+
+ /* Projection of the triangles in 3D onto 2D such that the area of
+ the projection is maximized. */
+
+
+ if (( n_x > n_z ) && ( n_x >= n_y )) {
+ // Project onto plane YZ
+
+ P1[0] = q1[2]; P1[1] = q1[1];
+ Q1[0] = p1[2]; Q1[1] = p1[1];
+ R1[0] = r1[2]; R1[1] = r1[1];
+
+ P2[0] = q2[2]; P2[1] = q2[1];
+ Q2[0] = p2[2]; Q2[1] = p2[1];
+ R2[0] = r2[2]; R2[1] = r2[1];
+
+ } else if (( n_y > n_z ) && ( n_y >= n_x )) {
+ // Project onto plane XZ
+
+ P1[0] = q1[0]; P1[1] = q1[2];
+ Q1[0] = p1[0]; Q1[1] = p1[2];
+ R1[0] = r1[0]; R1[1] = r1[2];
+
+ P2[0] = q2[0]; P2[1] = q2[2];
+ Q2[0] = p2[0]; Q2[1] = p2[2];
+ R2[0] = r2[0]; R2[1] = r2[2];
+
+ } else {
+ // Project onto plane XY
+
+ P1[0] = p1[0]; P1[1] = p1[1];
+ Q1[0] = q1[0]; Q1[1] = q1[1];
+ R1[0] = r1[0]; R1[1] = r1[1];
+
+ P2[0] = p2[0]; P2[1] = p2[1];
+ Q2[0] = q2[0]; Q2[1] = q2[1];
+ R2[0] = r2[0]; R2[1] = r2[1];
+ }
+
+ return tri_tri_overlap_test_2d(P1,Q1,R1,P2,Q2,R2);
+
+};
+
+
+//==============================================================================
+// 3D Triangle-Triangle Intersection
+//==============================================================================
+/*
+ This macro is called when the triangles surely intersect
+ It constructs the segment of intersection of the two triangles
+ if they are not coplanar.
+*/
+#define CONSTRUCT_INTERSECTION(p1,q1,r1,p2,q2,r2) { \
+ SUB(v1,q1,p1) \
+ SUB(v2,r2,p1) \
+ CROSS(N,v1,v2) \
+ SUB(v,p2,p1) \
+ if (DOT(v,N) > 0) {\
+ SUB(v1,r1,p1) \
+ CROSS(N,v1,v2) \
+ if (DOT(v,N) <= 0) { \
+ SUB(v2,q2,p1) \
+ CROSS(N,v1,v2) \
+ if (DOT(v,N) > 0) { \
+ SUB(v1,p1,p2) \
+ SUB(v2,p1,r1) \
+ alpha = DOT(v1,N2) / DOT(v2,N2); \
+ SCALAR(v1,alpha,v2) \
+ SUB(source,p1,v1) \
+ SUB(v1,p2,p1) \
+ SUB(v2,p2,r2) \
+ alpha = DOT(v1,N1) / DOT(v2,N1); \
+ SCALAR(v1,alpha,v2) \
+ SUB(target,p2,v1) \
+ return true; \
+ } else { \
+ SUB(v1,p2,p1) \
+ SUB(v2,p2,q2) \
+ alpha = DOT(v1,N1) / DOT(v2,N1); \
+ SCALAR(v1,alpha,v2) \
+ SUB(source,p2,v1) \
+ SUB(v1,p2,p1) \
+ SUB(v2,p2,r2) \
+ alpha = DOT(v1,N1) / DOT(v2,N1); \
+ SCALAR(v1,alpha,v2) \
+ SUB(target,p2,v1) \
+ return true; \
+ } \
+ } else { \
+ return false; \
+ } \
+ } else { \
+ SUB(v2,q2,p1) \
+ CROSS(N,v1,v2) \
+ if (DOT(v,N) < 0) { \
+ return false; \
+ } else { \
+ SUB(v1,r1,p1) \
+ CROSS(N,v1,v2) \
+ if (DOT(v,N) >= 0) { \
+ SUB(v1,p1,p2) \
+ SUB(v2,p1,r1) \
+ alpha = DOT(v1,N2) / DOT(v2,N2); \
+ SCALAR(v1,alpha,v2) \
+ SUB(source,p1,v1) \
+ SUB(v1,p1,p2) \
+ SUB(v2,p1,q1) \
+ alpha = DOT(v1,N2) / DOT(v2,N2); \
+ SCALAR(v1,alpha,v2) \
+ SUB(target,p1,v1) \
+ return true; \
+ } else { \
+ SUB(v1,p2,p1) \
+ SUB(v2,p2,q2) \
+ alpha = DOT(v1,N1) / DOT(v2,N1); \
+ SCALAR(v1,alpha,v2) \
+ SUB(source,p2,v1) \
+ SUB(v1,p1,p2) \
+ SUB(v2,p1,q1) \
+ alpha = DOT(v1,N2) / DOT(v2,N2); \
+ SCALAR(v1,alpha,v2) \
+ SUB(target,p1,v1) \
+ return true; \
+ }}}}
+
+
+
+#define TRI_TRI_INTER_3D(p1,q1,r1,p2,q2,r2,dp2,dq2,dr2) { \
+ if (dp2 > 0) { \
+ if (dq2 > 0) CONSTRUCT_INTERSECTION(p1,r1,q1,r2,p2,q2) \
+ else if (dr2 > 0) CONSTRUCT_INTERSECTION(p1,r1,q1,q2,r2,p2)\
+ else CONSTRUCT_INTERSECTION(p1,q1,r1,p2,q2,r2) }\
+ else if (dp2 < 0) { \
+ if (dq2 < 0) CONSTRUCT_INTERSECTION(p1,q1,r1,r2,p2,q2)\
+ else if (dr2 < 0) CONSTRUCT_INTERSECTION(p1,q1,r1,q2,r2,p2)\
+ else CONSTRUCT_INTERSECTION(p1,r1,q1,p2,q2,r2)\
+ } else { \
+ if (dq2 < 0) { \
+ if (dr2 >= 0) CONSTRUCT_INTERSECTION(p1,r1,q1,q2,r2,p2)\
+ else CONSTRUCT_INTERSECTION(p1,q1,r1,p2,q2,r2)\
+ } \
+ else if (dq2 > 0) { \
+ if (dr2 > 0) CONSTRUCT_INTERSECTION(p1,r1,q1,p2,q2,r2)\
+ else CONSTRUCT_INTERSECTION(p1,q1,r1,q2,r2,p2)\
+ } \
+ else { \
+ if (dr2 > 0) CONSTRUCT_INTERSECTION(p1,q1,r1,r2,p2,q2)\
+ else if (dr2 < 0) CONSTRUCT_INTERSECTION(p1,r1,q1,r2,p2,q2)\
+ else { \
+ coplanar = true; \
+ return coplanar_tri_tri3d(p1,q1,r1,p2,q2,r2,N1,N2);\
+ } \
+ }} }
+
+
+/*
+ The following version computes the segment of intersection of the
+ two triangles if it exists.
+ coplanar returns whether the triangles are coplanar
+ source and target are the endpoints of the line segment of intersection
+*/
+
+template <class RealP> static bool
+tri_tri_intersection_test_3d(const RealP p1[3], const RealP q1[3], const RealP r1[3],
+ const RealP p2[3], const RealP q2[3], const RealP r2[3],
+ bool& coplanar,
+ RealP source[3], RealP target[3] )
+
+{
+ RealP dp1, dq1, dr1, dp2, dq2, dr2;
+ RealP v1[3], v2[3], v[3];
+ RealP N1[3], N2[3], N[3];
+ RealP alpha;
+
+ // Compute distance signs of p1, q1 and r1
+ // to the plane of triangle(p2,q2,r2)
+
+
+ SUB(v1,p2,r2)
+ SUB(v2,q2,r2)
+ CROSS(N2,v1,v2)
+
+ SUB(v1,p1,r2)
+ dp1 = DOT(v1,N2);
+ SUB(v1,q1,r2)
+ dq1 = DOT(v1,N2);
+ SUB(v1,r1,r2)
+ dr1 = DOT(v1,N2);
+
+ if (((dp1 * dq1) > 0) && ((dp1 * dr1) > 0)) return false;
+
+ // Compute distance signs of p2, q2 and r2
+ // to the plane of triangle(p1,q1,r1)
+
+
+ SUB(v1,q1,p1)
+ SUB(v2,r1,p1)
+ CROSS(N1,v1,v2)
+
+ SUB(v1,p2,r1)
+ dp2 = DOT(v1,N1);
+ SUB(v1,q2,r1)
+ dq2 = DOT(v1,N1);
+ SUB(v1,r2,r1)
+ dr2 = DOT(v1,N1);
+
+ if (((dp2 * dq2) > 0) && ((dp2 * dr2) > 0)) return false;
+
+ // Permutation in a canonical form of T1's vertices
+
+
+ if (dp1 > 0) {
+ if (dq1 > 0) TRI_TRI_INTER_3D(r1,p1,q1,p2,r2,q2,dp2,dr2,dq2)
+ else if (dr1 > 0) TRI_TRI_INTER_3D(q1,r1,p1,p2,r2,q2,dp2,dr2,dq2)
+
+ else TRI_TRI_INTER_3D(p1,q1,r1,p2,q2,r2,dp2,dq2,dr2)
+ } else if (dp1 < 0) {
+ if (dq1 < 0) TRI_TRI_INTER_3D(r1,p1,q1,p2,q2,r2,dp2,dq2,dr2)
+ else if (dr1 < 0) TRI_TRI_INTER_3D(q1,r1,p1,p2,q2,r2,dp2,dq2,dr2)
+ else TRI_TRI_INTER_3D(p1,q1,r1,p2,r2,q2,dp2,dr2,dq2)
+ } else {
+ if (dq1 < 0) {
+ if (dr1 >= 0) TRI_TRI_INTER_3D(q1,r1,p1,p2,r2,q2,dp2,dr2,dq2)
+ else TRI_TRI_INTER_3D(p1,q1,r1,p2,q2,r2,dp2,dq2,dr2)
+ }
+ else if (dq1 > 0) {
+ if (dr1 > 0) TRI_TRI_INTER_3D(p1,q1,r1,p2,r2,q2,dp2,dr2,dq2)
+ else TRI_TRI_INTER_3D(q1,r1,p1,p2,q2,r2,dp2,dq2,dr2)
+ }
+ else {
+ if (dr1 > 0) TRI_TRI_INTER_3D(r1,p1,q1,p2,q2,r2,dp2,dq2,dr2)
+ else if (dr1 < 0) TRI_TRI_INTER_3D(r1,p1,q1,p2,r2,q2,dp2,dr2,dq2)
+ else {
+ // triangles are co-planar
+
+ coplanar = true;
+ return coplanar_tri_tri3d(p1,q1,r1,p2,q2,r2,N1,N2);
+ }
+ }
+ }
+};
+
+
+
+
+//==============================================================================
+// 2D Triangle-Triangle Overlap Test
+//==============================================================================
+
+/* some 2D macros */
+
+#define ORIENT_2D(a, b, c) ((a[0]-c[0])*(b[1]-c[1])-(a[1]-c[1])*(b[0]-c[0]))
+
+
+#define INTERSECTION_TEST_VERTEX(P1, Q1, R1, P2, Q2, R2) {\
+ if (ORIENT_2D(R2,P2,Q1) >= 0)\
+ if (ORIENT_2D(R2,Q2,Q1) <= 0)\
+ if (ORIENT_2D(P1,P2,Q1) > 0) {\
+ if (ORIENT_2D(P1,Q2,Q1) <= 0) return true; \
+ else return false;} else {\
+ if (ORIENT_2D(P1,P2,R1) >= 0)\
+ if (ORIENT_2D(Q1,R1,P2) >= 0) return true; \
+ else return false;\
+ else return false;}\
+ else \
+ if (ORIENT_2D(P1,Q2,Q1) <= 0)\
+ if (ORIENT_2D(R2,Q2,R1) <= 0)\
+ if (ORIENT_2D(Q1,R1,Q2) >= 0) return true; \
+ else return false;\
+ else return false;\
+ else return false;\
+ else\
+ if (ORIENT_2D(R2,P2,R1) >= 0) \
+ if (ORIENT_2D(Q1,R1,R2) >= 0)\
+ if (ORIENT_2D(P1,P2,R1) >= 0) return true;\
+ else return false;\
+ else \
+ if (ORIENT_2D(Q1,R1,Q2) >= 0) {\
+ if (ORIENT_2D(R2,R1,Q2) >= 0) return true; \
+ else return false; }\
+ else return false; \
+ else return false; \
+ };
+
+
+
+#define INTERSECTION_TEST_EDGE(P1, Q1, R1, P2, Q2, R2) { \
+ if (ORIENT_2D(R2,P2,Q1) >= 0) {\
+ if (ORIENT_2D(P1,P2,Q1) >= 0) { \
+ if (ORIENT_2D(P1,Q1,R2) >= 0) return true; \
+ else return false;} else { \
+ if (ORIENT_2D(Q1,R1,P2) >= 0){ \
+ if (ORIENT_2D(R1,P1,P2) >= 0) return true; else return false;} \
+ else return false; } \
+ } else {\
+ if (ORIENT_2D(R2,P2,R1) >= 0) {\
+ if (ORIENT_2D(P1,P2,R1) >= 0) {\
+ if (ORIENT_2D(P1,R1,R2) >= 0) return true; \
+ else {\
+ if (ORIENT_2D(Q1,R1,R2) >= 0) return true; else return false;}}\
+ else return false; }\
+ else return false; }}
+
+
+// This is a helper function for tri_tri_overlap_test_2d.
+template <class RealP> static bool
+ccw_tri_tri_intersection_2d(const RealP p1[2], const RealP q1[2], const RealP r1[2],
+ const RealP p2[2], const RealP q2[2], const RealP r2[2]) {
+ if ( ORIENT_2D(p2,q2,p1) >= 0 ) {
+ if ( ORIENT_2D(q2,r2,p1) >= 0 ) {
+ if ( ORIENT_2D(r2,p2,p1) >= 0 ) return true;
+ else INTERSECTION_TEST_EDGE(p1,q1,r1,p2,q2,r2)
+ } else {
+ if ( ORIENT_2D(r2,p2,p1) >= 0 )
+ INTERSECTION_TEST_EDGE(p1,q1,r1,r2,p2,q2)
+ else INTERSECTION_TEST_VERTEX(p1,q1,r1,p2,q2,r2)}}
+ else {
+ if ( ORIENT_2D(q2,r2,p1) >= 0 ) {
+ if ( ORIENT_2D(r2,p2,p1) >= 0 )
+ INTERSECTION_TEST_EDGE(p1,q1,r1,q2,r2,p2)
+ else INTERSECTION_TEST_VERTEX(p1,q1,r1,q2,r2,p2)}
+ else INTERSECTION_TEST_VERTEX(p1,q1,r1,r2,p2,q2)}
+};
+
+template <class RealP> static bool
+tri_tri_overlap_test_2d(const RealP p1[2], const RealP q1[2], const RealP r1[2],
+ const RealP p2[2], const RealP q2[2], const RealP r2[2]) {
+ if ( ORIENT_2D(p1,q1,r1) < 0 )
+ if ( ORIENT_2D(p2,q2,r2) < 0 )
+ return ccw_tri_tri_intersection_2d(p1,r1,q1,p2,r2,q2);
+ else
+ return ccw_tri_tri_intersection_2d(p1,r1,q1,p2,q2,r2);
+ else
+ if ( ORIENT_2D(p2,q2,r2) < 0 )
+ return ccw_tri_tri_intersection_2d(p1,q1,r1,p2,r2,q2);
+ else
+ return ccw_tri_tri_intersection_2d(p1,q1,r1,p2,q2,r2);
+
+};
+
+// Explicit instantiations for float and double.
+template class Geo::Triangle_<float>;
+template class Geo::Triangle_<double>;
+
+
+} // End of namespace SimTK
\ No newline at end of file
diff --git a/SimTKmath/Geometry/src/Geodesic.cpp b/SimTKmath/Geometry/src/Geodesic.cpp
new file mode 100644
index 0000000..a12e4e0
--- /dev/null
+++ b/SimTKmath/Geometry/src/Geodesic.cpp
@@ -0,0 +1,523 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Ian Stavness, Michael Sherman *
+ * Contributors: Andreas Scholz *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/internal/Geodesic.h"
+#include "simmath/internal/GeodesicIntegrator.h"
+#include "simmath/internal/ContactGeometry.h"
+
+#include "GeodesicEquations.h"
+
+#include <iostream>
+#include <cmath>
+
+using namespace SimTK;
+
+// Make sure these compile correctly.
+namespace SimTK {
+template class GeodesicIntegrator<GeodesicOnImplicitSurface>;
+template class GeodesicIntegrator<GeodesicOnParametricSurface>;
+}
+
+
+//==============================================================================
+// GEODESIC
+//==============================================================================
+
+void Geodesic::dump(std::ostream& o) const {
+ o << "Geodesic: " << getNumPoints() << " points, length="
+ << getLength() << "\n";
+ bool hasQtoP = !directionalSensitivityQtoP.empty();
+ if (!hasQtoP)
+ o << " QtoP Jacobi fields not available\n";
+ for (int i=0; i < getNumPoints(); ++i) {
+ o << " Point at s=" << arcLengths[i] << ":\n";
+ o << " p=" << frenetFrames[i].p() << " t="
+ << frenetFrames[i].x() << "\n";
+ o << " jrP=" << directionalSensitivityPtoQ[i][0];
+ o << " jtP=" << positionalSensitivityPtoQ[i][0];
+ if (hasQtoP) {
+ o << " jrQ=" << directionalSensitivityQtoP[i][0];
+ o << " jtQ=" << positionalSensitivityQtoP[i][0];
+ }
+ o << std::endl;
+ }
+}
+
+//==============================================================================
+// GEODESIC ON IMPLICIT SURFACE
+//==============================================================================
+
+// See class header for documentation.
+void GeodesicOnImplicitSurface::
+calcDerivs(Real t, const Vec<N>& y, Vec<N>& ydot) const {
+ const Vec3& p = getP(y); // rename state variables
+ const Vec3& v = getV(y);
+ const Real& jr = getJRot(y);
+ const Real& jt = getJTrans(y);
+
+ // Evaluate the surface at p.
+ const Vec3 g = geom.calcSurfaceGradient(p);
+ const Mat33 H = geom.calcSurfaceHessian(p);
+ Real Kg = geom.calcGaussianCurvature(g,H);
+
+ const Real Gdotv = ~v*(H*v);
+ const Real L = Gdotv/(~g*g); // Lagrange multiplier
+
+ // We have qdot = u; that part is easy.
+ updQ(ydot) = getU(y);
+
+ // These together are the udots.
+ Vec3& a = updV(ydot); // d/dt v
+ Real& jrdd = updJRotDot(ydot); // d/dt jdr
+ Real& jtdd = updJTransDot(ydot); // d/dt jdt
+
+ a = -L*g;
+ jrdd = -Kg*jr;
+ jtdd = -Kg*jt;
+}
+
+
+// See class header for documentation.
+void GeodesicOnImplicitSurface::
+calcConstraintErrors(Real t, const Vec<N>& y, Vec<NC>& cerr) const {
+ const Vec3& p = getP(y);
+ const Vec3& v = getV(y);
+ // This is the perr() equation that says the point must be on the surface.
+ cerr[0] = geom.calcSurfaceValue(p);
+ // These are the two verr() equations. The first is the derivative of
+ // the above point-on-surface holonomic constraint above. The second is
+ // a nonholonomic velocity constraint restricting the velocity along
+ // the curve to be 1.
+ cerr[1] = ~geom.calcSurfaceGradient(p)*v;
+ cerr[2] = v.norm() - 1;
+}
+
+// Given a state y drive the infinity norm of the position and velocity
+// constraint errors to consTol or below by adjusting y.
+bool GeodesicOnImplicitSurface::
+projectIfNeeded(Real consTol, Real t, Vec<N>& y) const {
+ const int MaxIter = 10; // should take *far* fewer
+ const Real OvershootFac = Real(0.1); // try to do better than consTol
+
+ const Real tryTol = consTol * OvershootFac;
+ Vec3& p = updP(y); // aliases for the state variables
+ Vec3& v = updV(y);
+
+ // Fix the position constraint first. This is a Newton interation
+ // that modifies only the point location to make sure it remains on
+ // the surface. No position projection is done if we're already at
+ // tryTol, which is a little tighter than the requested consTol.
+
+ // NOTE: (sherm) I don't think this is exactly the right projection.
+ // Here we project down the gradient, but the final result won't
+ // be exactly the nearest point on the surface if the gradient changes
+ // direction on the way down. For correcting small errors this is
+ // probably completely irrelevant since the starting and final gradient
+ // directions will be the same.
+
+ Real perr, ptolAchieved;
+ int piters=0;
+ while (true) {
+ perr = geom.calcSurfaceValue(p);
+ ptolAchieved = std::abs(perr);
+ if (ptolAchieved <= tryTol || piters==MaxIter)
+ break;
+
+ ++piters;
+ // We want a least squares solution dp to ~g*dp=perr which we
+ // get using the pseudoinverse: dp=pinv(~g)*perr, where
+ // pinv(~g) = g*inv(~g*g).
+ const Vec3 g = geom.calcSurfaceGradient(p);
+ const Vec3 pinvgt = g/(~g*g);
+ const Vec3 dp = pinvgt*perr;
+
+ p -= dp; // updates the state
+ }
+
+
+ // Now the velocities. There are two velocity constraints that have
+ // to be satisfied simultaneously. They are (1) the time derivative of
+ // the perr equation which we just solved, and (2) the requirement that
+ // the velocity magnitude be 1. So verr=~[ ~g*v, |v|-1 ]. You might
+ // think these need to be solved simultaneously to find the least
+ // squares dv, but dv can be determined by two orthogonal projections.
+ // The allowable velocity vectors form a unit circle whose normal is
+ // in the gradient direction. The least squares dv is the shortest
+ // vector from the end point of v to that cicle. To find the closest
+ // point on the unit circle, first project the vector v onto the
+ // circle's plane by the shortest path (remove the normal component).
+ // Then stretch the result to unit length.
+ // First we solve the linear least squares problem ~g*(v+dv0)=0 for
+ // dv0, and set v0=v+dv0. Then set vfinal = v0/|v0|, giving dv=vfinal-v.
+
+ // We're going to project velocities unconditionally because we
+ // would have to evaluate the constraint anyway to see if it is
+ // violated and that is most of the computation we need to fix it.
+
+ const Vec3 g = geom.calcSurfaceGradient(p);
+ const Vec3 pinvgt = g/(~g*g);
+ const Real perrdot = ~g*v;
+
+ const Vec3 dv0 = pinvgt*perrdot;
+ const Vec3 v0 = v - dv0; // fix direction
+ v = v0/v0.norm(); // fix length; updates state
+
+ const bool success = (ptolAchieved <= consTol);
+ return success;
+}
+
+
+
+
+
+//==============================================================================
+// PARTICLE CON SURFACE SYSTEM GUTS
+//==============================================================================
+/*
+ * This system is a 3d particle mass constrained to move along a surface
+ * with no applied force (other than the constraint reaction force normal
+ * to the surface). With no applied for the particle traces a geodesic along
+ * the surface.
+ *
+ *
+ * The DAE for a generic multibody system is:
+ * qdot = Nu
+ * M udot = f - ~A lambda
+ * A udot = b
+ * perr(t,q) = 0
+ * verr(t,q,u) = 0
+ *
+ * Let r be the 3d coordinates of the particle
+ * g(r) be the implicit surface function
+ * G be the gradient of the implicit surface function
+ * H be the hessian of the implicit surface function
+ *
+ * We will express implicit surface constraint as
+ * g(r) = 0 (perr)
+ * Gr' = 0 (verr)
+ * Gr'' = -G'r' = - ~r'Hr' (aerr)
+ *
+ * So the matrix A = G and b = -r'Hr', and the
+ * equations of motion are:
+ * [ M ~G ] [ r'' ] [ 0 ]
+ * [ G 0 ] [ L ] = [ - ~r'Hr' ]
+ * where L (the Lagrange multiplier) is proportional to
+ * the constraint force.
+ *
+ * solving for L,
+ * L = (GM\~G)\~r'Hr'
+ * r'' = - M\~G(GM\~G)\~r'Hr'
+ *
+ * where ~A = transpose of A
+ * and A\ = inverse of A
+ */
+int ParticleConSurfaceSystemGuts::realizeTopologyImpl(State& s) const {
+ // Generalized coordinates:
+ // q0, q1, q2, q3 = x, y, z, j
+ // u0, u1, u2, u3 = x', y', z', j'
+ const Vector init(4, Real(0));
+ q0 = s.allocateQ(subsysIndex, init);
+ u0 = s.allocateU(subsysIndex, init);
+ return 0;
+}
+int ParticleConSurfaceSystemGuts::realizeModelImpl(State& s) const {
+ return 0;
+}
+int ParticleConSurfaceSystemGuts::realizeInstanceImpl(const State& s) const {
+ qerr0 = s.allocateQErr(subsysIndex, 1);
+ uerr0 = s.allocateUErr(subsysIndex, 2);
+ udoterr0 = s.allocateUDotErr(subsysIndex, 2); // and multiplier
+// event0 = s.allocateEvent(subsysIndex, Stage::Position, 3);
+ return 0;
+}
+int ParticleConSurfaceSystemGuts::realizePositionImpl(const State& s) const {
+ const Vector& q = s.getQ(subsysIndex);
+ const Vec3& point = Vec3::getAs(&q[0]);
+
+ // This is the perr() equation that says the point must be on the surface.
+ s.updQErr(subsysIndex)[0] = geom.calcSurfaceValue(point);
+ return 0;
+}
+
+int ParticleConSurfaceSystemGuts::realizeVelocityImpl(const State& s) const {
+ const Vector& q = s.getQ(subsysIndex);
+ const Vector& u = s.getU(subsysIndex);
+ const Vec3& p = Vec3::getAs(&q[0]); // point
+ const Vec3& dpdt = Vec3::getAs(&u[0]); // point's velocity on surface
+
+ Vector& qdot = s.updQDot(subsysIndex);
+
+ // Calculate qdots. They are just generalized speeds here.
+ qdot = u;
+
+ // These are the two verr() equations. The first is the derivative of
+ // the point-on-surface holonomic constraint defined in realizePositionImpl
+ // above. The second is a nonholonomic velocity constraint restricting
+ // the velocity along the curve to be 1.
+ s.updUErr(subsysIndex)[0] = ~geom.calcSurfaceGradient(p)*dpdt;
+ s.updUErr(subsysIndex)[1] = dpdt.norm() - 1;
+ return 0;
+}
+
+int ParticleConSurfaceSystemGuts::realizeDynamicsImpl(const State& s) const {
+ return 0;
+}
+
+int ParticleConSurfaceSystemGuts::realizeAccelerationImpl(const State& s) const {
+
+ // XXX assume unit mass
+
+ const Vector& q = s.getQ(subsysIndex);
+ const Vec3& p = Vec3::getAs(&q[0]); // point
+ const Real& j = q[3]; // Jacobi field value
+
+ const Vector& u = s.getU(subsysIndex);
+ const Vec3& v = Vec3::getAs(&u[0]); // point's velocity on surface
+
+ Vector& udot = s.updUDot(subsysIndex);
+ Vec3& a = Vec3::updAs(&udot[0]);
+ Real& jdotdot = udot[3]; // Jacobi field 2nd derivative
+
+ const Real ep = geom.calcSurfaceValue(p); // position constraint error
+ const Vec3 GT = geom.calcSurfaceGradient(p);
+ const Real ev = ~GT*v; // d/dt ep
+ const Mat33 H = geom.calcSurfaceHessian(p);
+ const Real Gdotv = ~v*(H*v);
+ const Real L = Gdotv/(~GT*GT);
+ a = GT*-L; // fills in udot
+
+ // Now evaluate the Jacobi field.
+ const Real Kg = geom.calcGaussianCurvature(GT,H);
+ jdotdot = -Kg*j; // sets udot[3]
+
+ // Qdotdots are just udots here.
+ s.updQDotDot() = udot;
+
+ s.updMultipliers(subsysIndex)[0] = L;
+
+ // This is the aerr() equation.
+ s.updUDotErr(subsysIndex)[0] = ~GT*a + Gdotv;
+ s.updUDotErr(subsysIndex)[1] = (~v*a)/u.norm();
+ return 0;
+}
+
+static Real wrms(const Vector& y, const Vector& w) {
+ Real sumsq = 0;
+ for (int i=0; i<y.size(); ++i)
+ sumsq += square(y[i]*w[i]);
+ return std::sqrt(sumsq/y.size());
+}
+
+/*
+ * Here we want to remove any constraint errors from the current state,
+ * and project out any component of the integrator's error estimate
+ * perpendicular to the constraint manifold. We will do this sequentially
+ * rather than handling position and velocity simultaneously.
+ */
+// qerrest is in/out
+void ParticleConSurfaceSystemGuts::projectQImpl(State& s, Vector& qerrest,
+ const ProjectOptions& opts,
+ ProjectResults& results) const
+
+{
+ const Real consAccuracy = opts.getRequiredAccuracy();
+
+ // These are convenient aliases for state entries. Note that they are
+ // "live" since they refer directly into the state we are changing.
+
+ const Vector& qerr = s.getQErr(subsysIndex);
+ const Real& ep = qerr[0];
+
+ Vector& q = s.updQ(subsysIndex);
+ Vec3& p = Vec3::updAs(&q[0]);
+
+ realize(s, Stage::Position); // recalc QErr (ep)
+// std::cout << "BEFORE wperr=" << ep << std::endl;
+
+ Real qchg;
+ Vec3 dp(0);
+ int cnt = 0;
+ do {
+ Vec3 g = geom.calcSurfaceGradient(p);
+
+ // dp = Pinv*ep, where Pinv = ~P/(P*~P) and P=~g
+ Row3 P = ~g;
+ dp = ~P/(P*~P)*ep;
+// dp = (g/(~g*g))*ep;
+
+ qchg = std::sqrt(dp.normSqr()/3); // rms norm
+
+ p -= dp; // updates the state
+
+ s.invalidateAll(Stage::Position); // force realize position
+ realize(s, Stage::Position); // recalc QErr (ep)
+
+// std::cout << cnt << ": AFTER q-=dq, q=" << q << ", wperr=" << ep << ", wqchg=" << qchg << std::endl;
+ cnt++;
+
+ //sleepInSec(0.5);
+ if (cnt > 10) {
+ results.setExitStatus(ProjectResults::FailedToConverge);
+ return;
+ }
+
+ } while (std::abs(ep) > consAccuracy && qchg >= 0.01*consAccuracy);
+
+/*
+ // Now do error estimates.
+ if (qerrest.size()) {
+ Vec3& eq = Vec3::updAs(&qerrest[0]);
+
+ Vec3 g = geom.calcSurfaceGradient(p);
+
+ // qperp = Pinv*P*eq, where Pinv = ~P/(P*~P) and P=~g
+ Vec3 qperp = (g/(~g*g))*(~g*eq);
+
+// std::cout << "ERREST before=" << qerrest
+// << " wrms=" << std::sqrt(qerrest.normSqr()/q.size()) << std::endl;
+// std::cout << "P*eq=" << ~g*eq << std::endl;
+
+ eq -= qperp;
+
+// std::cout << "ERREST after=" << qerrest
+// << " wrms=" << std::sqrt(qerrest.normSqr()/q.size()) << std::endl;
+// std::cout << "P*eq=" << ~g*eq << std::endl;
+ }
+*/
+ results.setExitStatus(ProjectResults::Succeeded);
+}
+
+// uerrest is in/out
+void ParticleConSurfaceSystemGuts::projectUImpl(State& s, Vector& uerrest,
+ const ProjectOptions& opts, ProjectResults& results) const
+{
+ const Real consAccuracy = opts.getRequiredAccuracy();
+
+ const Vector& uerr = s.getUErr(subsysIndex); // set up aliases
+ const Vec2& ev = Vec2::getAs(&uerr[0]);
+
+ const Vector& q = s.getQ(subsysIndex);
+ const Vec3& p = Vec3::getAs(&q[0]);
+
+ Vector& u = s.updU(subsysIndex);
+ Vec3& v = Vec3::updAs(&u[0]);
+
+ realize(s, Stage::Velocity); // calculate UErr (ev)
+// std::cout << "vBEFORE wperr=" << ep << std::endl;
+// std::cout << "vBEFORE wverr=" << ev << std::endl;
+
+// // Do velocity projection at current values of q, which should have
+// // been projected already.
+ Vec3 g = geom.calcSurfaceGradient(p);
+
+ // dv = Pinv*ev, where Pinv = ~P/(P*~P) and P=~g
+// Vec3 dv = (g/(~g*g))*ev[0];
+ Row3 P = ~g;
+ Vec3 dv = ~P/(P*~P)*ev[0];
+//
+// // force unit speed
+ const UnitVec3 newv(v - dv);
+ v = Vec3(newv);
+// v -= dv;
+
+ s.invalidateAll(Stage::Velocity); // force realize velocity
+ realize(s, Stage::Velocity); // recalc UErr
+// std::cout << "vAFTER wverr=" << ev << std::endl;
+
+// XXX combined u projection not working...
+
+// Real vchg;
+// Vec3 dv(0);
+// Mat23 V;
+// int cnt = 0;
+// V[0] = ~g; // Pnonholo = ~g;
+// do {
+// V[1] = ~v/v.norm(); // Vholo
+//
+//
+// // du = Vinv*ev, where Vinv = ~V/(V*~V) and V = ~[Pnonholo Vholo]
+// Mat22 VVT = V*~V;
+// VVT.invert();
+// dv = (~V)*VVT*ev;
+//
+//// std::cout << "V = " << V << std::endl;
+//// std::cout << "v = " << v << ", dv = " << dv << std::endl;
+//
+//// dv = ~V/(V*~V)*ev;
+//
+// vchg = std::sqrt(dv.normSqr()/3); // wrms norm
+//
+//// s.updU(subsysIndex)[0] -= du[0];
+//// s.updU(subsysIndex)[1] -= du[1];
+//// s.updU(subsysIndex)[2] -= du[2];
+//
+// v -= dv;
+//
+// s.invalidateAll(Stage::Velocity); // force realize velocity
+// realize(s, Stage::Velocity); // recalc UErr (ev)
+//
+// std::cout << cnt << ": AFTER v-=dv verr=" << ev << " vchg=" << vchg << std::endl;
+//// std::cout << "v = " << v << ", dv = " << dv << std::endl;
+//
+// cnt++;
+//
+// sleep(0.5);
+// if (cnt > 10) {
+// results.setExitStatus(ProjectResults::FailedToConverge);
+// return;
+// }
+//
+//
+// } while (std::max(std::abs(ev[0]), std::abs(ev[1])) > consAccuracy &&
+// vchg >= 0.01*consAccuracy);
+
+/*
+ // Now do error estimates.
+ if (uerrest.size()) {
+ Vec3& eu = Vec3::updAs(&uerrest[0]);
+
+ // uperp = Vinv*(V*eu), where V=P, Pinv = ~P/(P*~P) and P=~n
+ Vec3 uperp = (g/(~g*g))*(~g*eu);
+
+// std::cout << "ERREST before=" << uerrest
+// << " wrms=" << std::sqrt(uerrest.normSqr()/u.size()) << std::endl;
+// std::cout << " VW*eu=" << ~n*eu << std::endl;
+
+ eu -= uperp;
+
+// std::cout << "ERREST after=" << uerrest
+// << " wrms=" << std::sqrt(uerrest.normSqr()/u.size()) << std::endl;
+// std::cout << " VW*eu=" << ~n*eu << std::endl;
+ }
+*/
+ results.setExitStatus(ProjectResults::Succeeded);
+// std::cout << "norm(u) = " << s.getU(subsysIndex).norm() << std::endl;
+}
+
+
+
+
+
+
diff --git a/SimTKmath/Geometry/src/GeodesicEquations.h b/SimTKmath/Geometry/src/GeodesicEquations.h
new file mode 100644
index 0000000..b16420f
--- /dev/null
+++ b/SimTKmath/Geometry/src/GeodesicEquations.h
@@ -0,0 +1,254 @@
+#ifndef SimTK_SIMMATH_GEODESIC_EQUATIONS_H_
+#define SimTK_SIMMATH_GEODESIC_EQUATIONS_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Ian Stavness, Michael Sherman *
+ * Contributors: Andreas Scholz *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+// This is an internal header file; not part of the API. These are the
+// classes representing the differential equations that must be solved to
+// calculate geodesics over general smooth surfaces.
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/internal/Geodesic.h"
+#include "simmath/internal/GeodesicIntegrator.h"
+#include "simmath/internal/ContactGeometry.h"
+
+#include "ContactGeometryImpl.h"
+
+#include <iostream>
+#include <cmath>
+
+namespace SimTK {
+
+//==============================================================================
+// GEODESIC ON IMPLICIT SURFACE
+//==============================================================================
+/* This class satisfies the requirements for an Equations template argument
+to the GeodesicIntegrator class. These are the differential-algebraic
+equations that need to be solved to generate an arc length-parameterized
+geodesic over an arbitrary implicit surface, starting at a given point and
+tangent direction. We also calculate the geodesic's Jacobi field[2,3] which
+provides scalars relating changes in direction or position at the start to
+their effects on the location of the end point.
+
+Geodesic equations [1]
+------------------
+This system can be viewed as a 3d particle mass constrained to move along a
+surface with a unit velocity and no applied force except the constraint
+reaction force normal to the surface. With no applied force the particle traces
+a geodesic along the surface.
+
+The DAE for a generic multibody system is:
+ (1) qdot = Nu
+ (2) M udot + ~G lambda = f
+ (3) G udot = b
+ (4) perr(t,q) = 0
+ (5) verr(t,q,u) = 0
+
+Let p be the 3d coordinates of the particle (=q)
+ v = p' = dp/ds the particle's velocity (=u)
+ a = p'' = dv/ds the particle's acceleration (=udot)
+ S(p) the implicit surface function
+ g(p) = ~DS/Dp the gradient of the implicit surface function (3x1)
+ H(p) = Dg/Dp the Hessian of the implicit surface function (3x3 symmetric)
+ s is arc length along the geodesic curve
+
+Notation
+ D = partial derivative
+ ~A = transpose of matrix A
+ A\ = inverse of matrix A
+
+We express the implicit surface constraint and its derivatives with respect
+to the independent variable s:
+ (6) S(p) = 0 (perr)
+ (7) ~g v = 0 (verr)
+ (8) ~g a = -~g'v = - ~v H v (aerr)
+
+There is also a non-holonomic (velocity) constraint required to maintain the
+tangential velocity's magnitude to 1, ensuring that we get an arc length
+parameterization of the geodesic:
+ (9) |v| = 1 (verr)
+
+So in eqns. (2,3) the matrix M=I (unit mass particle), G = ~g and b = -vHv, and
+the equations of motion are:
+ [ I g ] [ a ] [ 0 ]
+ (10) [ ~g 0 ] [ L ] = [ - ~vHv ]
+where L (the Lagrange multiplier) is proportional to the constraint force.
+There is no multiplier associated with equation (9) since there are no
+tangential forces.
+
+Solving for L and then a gives:
+ L = (~g g)\ ~vHv
+ a = - g L
+
+
+Jacobi field equations [2,3]
+----------------------
+There are two second order differential
+equations to solve for the rotational and translational Jacobi field
+amplitudes:
+ (1) jr'' + Kg*jr = 0, with jr(0)=0, jr'(0)=1
+ (2) jt'' + Kg*jt = 0, with jt(0)=1, jt'(0)=0
+where jr=jr(s), jt=jt(s). Kg=Kg(s) is the Gaussian curvature of the surface
+evaluated at s along the curve.
+
+
+Constraint projection
+---------------------
+Although the equations of motion satisfy the constraints at the acceleration
+level, integration error will cause the solution to drift away from the
+position and velocity constraint manifolds. Unlike ref. [1] which uses
+Baumgarte stabilization, we use the more tractable coordinate projection
+method[4] to prevent this drift. This requires that at each step we perform
+a least-squares projection of the state variables back onto the constraint
+manifolds. Coordinate projection guarantees that the constraints are always
+satisfied, and the solution to the differential equations is improved.
+
+See the implementation of the projectIfNeeded() method for details.
+
+
+References
+----------
+[1] De Sapio, V., Khatib, O., Delp, S. Least action principles and their
+application to constrained and task-level problems in robotics and�
+biomechanics. Multibody System Dynamics 19(3):303 (2008), section 3.1.
+[2] Do Carmo, M.P. Differential Geometry of Curves and Surfaces,
+Chapter 5-5 Jacobi Fields and Conjugate Points. Prentice Hall (1976).
+[3] For real understanding, see Andreas Scholz' master's thesis (2012).
+[4] Eich, E. Convergence results for a coordinate projection method applied
+to mechanical systems with algebraic constraints. Siam Journal on Numerical
+Analysis 30(5):1467 (1993).
+*/
+class SimTK_SIMMATH_EXPORT GeodesicOnImplicitSurface {
+public:
+ // state y = q | u = px py pz jr jt | vx vy vz jrd jtd
+ // NQ, NC are required by the GeodesicIntegrator
+ enum { D = 3, // 3 coordinates for a point on an implicit surface.
+ NJ = 2, // Number of Jacobi field equations.
+ NQ = D+NJ, // Number of 2nd order equations.
+ N = 2*NQ, // Number of differential equations.
+ NC = 3 }; // 3 constraints: point on surface, point velocity
+ // along surface, unit velocity
+
+ GeodesicOnImplicitSurface(const ContactGeometryImpl& geom)
+ : geom(geom) {}
+
+ // This method is required by the GeodesicIntegrator.
+ // Calculate state derivatives ydot(t,y).
+ // See above for documentation.
+ void calcDerivs(Real t, const Vec<N>& y, Vec<N>& ydot) const;
+
+ // This method is required by the GeodesicIntegrator.
+ // Evaluate the position and velocity constraint errors cerr(t,y).
+ // See above for documentation.
+ void calcConstraintErrors(Real t, const Vec<N>& y, Vec<NC>& cerr) const;
+
+ // This method is required by the GeodesicIntegrator.
+ // Given a state y drive the infinity norm of the position and velocity
+ // constraint errors to consTol or below by adjusting y.
+ bool projectIfNeeded(Real consTol, Real t, Vec<N>& y) const;
+
+ // Utility routine for filling in the initial state given a starting
+ // point and direction. Note that there is no guarantee that the resulting
+ // state satisfies the constraints.
+ static Vec<N> getInitialState(const Vec3& P, const UnitVec3& tP) {
+ Vec<N> y;
+ updP(y) = P; updV(y) = tP.asVec3();
+ updJRot(y) = 0; updJRotDot(y) = 1;
+ updJTrans(y) = 1; updJTransDot(y) = 0;
+ return y;
+ }
+
+ // These define the meanings of the state variables & derivatives.
+ static const Vec<NQ>& getQ(const Vec<N>& y) {return Vec<NQ>::getAs(&y[0]);}
+ static Vec<NQ>& updQ(Vec<N>& y) {return Vec<NQ>::updAs(&y[0]);}
+
+ static const Vec<NQ>& getU(const Vec<N>& y) {return Vec<NQ>::getAs(&y[NQ]);}
+ static Vec<NQ>& updU(Vec<N>& y) {return Vec<NQ>::updAs(&y[NQ]);}
+
+
+ // Extract the point location from a full state y.
+ static const Vec3& getP(const Vec<N>& y) {return Vec3::getAs(&y[0]);}
+ static Vec3& updP(Vec<N>& y) {return Vec3::updAs(&y[0]);}
+ // Extract the point velocity from a full state y.
+ static const Vec3& getV(const Vec<N>& y) {return Vec3::getAs(&y[NQ]);}
+ static Vec3& updV(Vec<N>& y) {return Vec3::updAs(&y[NQ]);}
+
+ // Extract the value of the rotational Jacobi field from a state y.
+ static const Real& getJRot(const Vec<N>& y) {return y[D];}
+ static Real& updJRot(Vec<N>& y) {return y[D];}
+ static const Real& getJRotDot(const Vec<N>& y) {return y[NQ+D];}
+ static Real& updJRotDot(Vec<N>& y) {return y[NQ+D];}
+ // Extract the value of the translational Jacobi field from a state y.
+ static const Real& getJTrans(const Vec<N>& y) {return y[D+1];}
+ static Real& updJTrans(Vec<N>& y) {return y[D+1];}
+ static const Real& getJTransDot(const Vec<N>& y) {return y[NQ+D+1];}
+ static Real& updJTransDot(Vec<N>& y) {return y[NQ+D+1];}
+
+private:
+ const ContactGeometryImpl& geom;
+};
+
+
+
+//==============================================================================
+// GEODESIC ON PARAMETRIC SURFACE
+//==============================================================================
+
+/* This class satisfies the requirements for an Equations template argument
+to the GeodesicIntegrator class.
+TODO!
+*/
+class GeodesicOnParametricSurface {
+public:
+ // state y = q | u = u v jr jt | ud vd jdr jdt
+ enum { D = 2, // 3 coordinates for a point on an implicit surface.
+ NJ = 2, // Number of Jacobi field equations.
+ NQ = D+NJ, // Number of 2nd order equations.
+ N = 2*NQ, // Number of differential equations.
+ NC = 1 }; // maintain unit velocity
+
+ GeodesicOnParametricSurface(const ContactGeometryImpl& geom)
+ : geom(geom) {}
+
+ void calcDerivs(Real t, const Vec<N>& y, Vec<N>& ydot) const {
+ assert(!"not implemented");
+ }
+
+ void calcConstraintErrors(Real t, const Vec<N>& y, Vec<NC>& cerr) const {
+ assert(!"not implemented");
+ }
+
+ bool projectIfNeeded(Real consTol, Real t, Vec<N>& y) const {
+ assert(!"not implemented");
+ return false;
+ }
+private:
+ const ContactGeometryImpl& geom;
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_INLINE_INTEGRATOR_H_
diff --git a/SimTKmath/Geometry/src/OBBTree.cpp b/SimTKmath/Geometry/src/OBBTree.cpp
new file mode 100644
index 0000000..a6d46ec
--- /dev/null
+++ b/SimTKmath/Geometry/src/OBBTree.cpp
@@ -0,0 +1,33 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/internal/OBBTree.h"
+
+using namespace SimTK;
+
+//==============================================================================
+// OBB TREE
+//==============================================================================
+
diff --git a/SimTKmath/Geometry/src/OrientedBoundingBox.cpp b/SimTKmath/Geometry/src/OrientedBoundingBox.cpp
new file mode 100644
index 0000000..17fb2f8
--- /dev/null
+++ b/SimTKmath/Geometry/src/OrientedBoundingBox.cpp
@@ -0,0 +1,387 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/LinearAlgebra.h"
+#include "simmath/internal/OrientedBoundingBox.h"
+
+namespace SimTK {
+
+OrientedBoundingBox::OrientedBoundingBox() {
+}
+
+OrientedBoundingBox::OrientedBoundingBox
+ (const Transform& transform, const Vec3& size)
+: transform(transform), size(size) {
+}
+
+const Transform& OrientedBoundingBox::getTransform() const {
+ return transform;
+}
+
+const Vec3& OrientedBoundingBox::getSize() const {
+ return size;
+}
+
+OrientedBoundingBox::OrientedBoundingBox(const Vector_<Vec3>& points) {
+ SimTK_APIARGCHECK(points.size() > 0, "OrientedBoundingBox",
+ "OrientedBoundingBox", "No points passed to constructor");
+
+ // Construct the covariance matrix of the points.
+
+ Vec3 center = mean(points);
+ Vector_<Vec3> p = points-center;
+ Mat33 c(0);
+ for (int i = 0; i < p.size(); i++)
+ for (int j = 0; j < 3; j++)
+ for (int k = 0; k < 3; k++)
+ c(j, k) += p[i][j]*p[i][k];
+ c *= Real(1)/p.size();
+
+ // Find the eigenvectors, which will be our initial guess for the axes of
+ // the box.
+
+ Vector_<std::complex<Real> > eigenvalues;
+ Matrix_<std::complex<Real> > eigenvectors;
+ Eigen(Matrix(c)).getAllEigenValuesAndVectors(eigenvalues, eigenvectors);
+ Vec3 axes[3];
+ for (int i = 0; i < 2; i++)
+ for (int j = 0; j < 3; j++)
+ axes[i][j] = eigenvectors(j, i).real();
+
+ // Now try optimizing the rotation to give a better fit.
+
+ Rotation rot(UnitVec3(axes[0]), XAxis, axes[1], YAxis);
+ Real volume = calculateVolume(points, rot);
+ for (Real step = Real(0.1); step > Real(0.01); step /= 2) {
+ bool improved = true;
+ while (improved) {
+ Rotation trialRotation[6];
+ trialRotation[0].setRotationFromAngleAboutX(step);
+ trialRotation[1].setRotationFromAngleAboutX(-step);
+ trialRotation[2].setRotationFromAngleAboutY(step);
+ trialRotation[3].setRotationFromAngleAboutY(-step);
+ trialRotation[4].setRotationFromAngleAboutZ(step);
+ trialRotation[5].setRotationFromAngleAboutZ(-step);
+ improved = false;
+ for (int i = 0; i < 6; i++) {
+ trialRotation[i] = trialRotation[i]*rot;
+ Real trialVolume = calculateVolume(points, trialRotation[i]);
+ if (trialVolume < volume) {
+ rot = trialRotation[i];
+ volume = trialVolume;
+ improved = true;
+ }
+ }
+ }
+ }
+
+ // Find the extent along each axis.
+
+ axes[0] = Vec3(rot.col(0));
+ axes[1] = Vec3(rot.col(1));
+ axes[2] = Vec3(rot.col(2));
+ Vec3 minExtent = Vec3(MostPositiveReal);
+ Vec3 maxExtent = Vec3(MostNegativeReal);
+ for (int i = 0; i < points.size(); i++) {
+ for (int j = 0; j < 3; j++) {
+ minExtent[j] = std::min(minExtent[j], ~axes[j]*points[i]);
+ maxExtent[j] = std::max(maxExtent[j], ~axes[j]*points[i]);
+ }
+ }
+
+ // Create the bounding box.
+
+ size = maxExtent-minExtent;
+ Vec3 tol = Real(1e-5)*size;
+ for (int i = 0; i < 3; i++)
+ tol[i] = std::max(tol[i], Real(1e-10));
+ size += 2*tol;
+ transform = Transform(rot, rot*(minExtent-tol));
+}
+
+Real OrientedBoundingBox::calculateVolume
+ (const Vector_<Vec3>& points, const Rotation& rotation) {
+ Vec3 minExtent = Vec3(MostPositiveReal);
+ Vec3 maxExtent = Vec3(MostNegativeReal);
+ for (int i = 0; i < points.size(); i++) {
+ Vec3 p = ~rotation*points[i];
+ for (int j = 0; j < 3; j++) {
+ minExtent[j] = std::min(minExtent[j], p[j]);
+ maxExtent[j] = std::max(maxExtent[j], p[j]);
+ }
+ }
+ Vec3 size = maxExtent-minExtent+Vec3(Real(2e-10));
+ return size[0]*size[1]*size[2];
+}
+
+bool OrientedBoundingBox::containsPoint(const Vec3& point) const {
+ Vec3 p = ~transform*point;
+ return (p[0] >= 0 && p[0] <= size[0] &&
+ p[1] >= 0 && p[1] <= size[1] &&
+ p[2] >= 0 && p[2] <= size[2]);
+}
+
+bool OrientedBoundingBox::intersectsBox(const OrientedBoundingBox& box) const {
+ // Precalculate various quantities.
+
+ // From the other box's frame to this one's
+ const Transform t = ~getTransform()*box.getTransform();
+ const Mat33& r = t.R().asMat33();
+ const Mat33 rabs = r.abs();
+ const Vec3 a = getSize()/2;
+ const Vec3 b = box.getSize()/2;
+ const Vec3 center1 = a;
+ const Vec3 center2 = t*b;
+ const Vec3 d = center2-center1;
+
+ // Now perform a series of 15 tests where we project each box onto an axis
+ // and see if they overlap. This is described in Gottschalk, S., Lin, MC,
+ // Manocha, D, "OBBTree: a hierarchical structure for rapid interference
+ // detection." Proceedings of the 23rd Annual Conference on Computer
+ // Graphics and Interactive Techniques, pp. 171-180, 1996. We also perform
+ // an additional check which allows an early acceptance if the center of
+ // one box is inside the other one.
+
+ // First check the three axes of this box.
+
+ bool accept = true;
+ for (int i = 0; i < 3; i++) {
+ Real ra = a[i];
+ Real rb = rabs.row(i)*b;
+ Real distance = std::abs(d[i]);
+ if (distance > ra+rb)
+ return false;
+ if (distance > ra)
+ accept = false;
+ }
+ if (accept)
+ return true;
+
+ // Now check the three axes of the other box.
+
+ accept = true;
+ for (int i = 0; i < 3; i++) {
+ Real ra = ~a*rabs.col(i);
+ Real rb = b[i];
+ Real distance = std::abs(d[0]*r(0, i)+d[1]*r(1, i)+d[2]*r(2, i));
+ if (distance > ra+rb)
+ return false;
+ if (distance > rb)
+ accept = false;
+ }
+ if (accept)
+ return true;
+
+ // Now check the nine axes formed from cross products of one axis from each
+ // box.
+
+ {
+ Real ra = a[1]*rabs(2, 0)+a[2]*rabs(1, 0);
+ Real rb = b[1]*rabs(0, 2)+b[2]*rabs(0, 1);
+ if (std::abs(d[2]*r(1, 0) - d[1]*r(2, 0)) > ra+rb)
+ return false;
+ }
+ {
+ Real ra = a[1]*rabs(2, 1)+a[2]*rabs(1, 1);
+ Real rb = b[0]*rabs(0, 2)+b[2]*rabs(0, 0);
+ if (std::abs(d[2]*r(1, 1) - d[1]*r(2, 1)) > ra+rb)
+ return false;
+ }
+ {
+ Real ra = a[1]*rabs(2, 2)+a[2]*rabs(1, 2);
+ Real rb = b[0]*rabs(0, 1)+b[1]*rabs(0, 0);
+ if (std::abs(d[2]*r(1, 2) - d[1]*r(2, 2)) > ra+rb)
+ return false;
+ }
+ {
+ Real ra = a[0]*rabs(2, 0)+a[2]*rabs(0, 0);
+ Real rb = b[1]*rabs(1, 2)+b[2]*rabs(1, 1);
+ if (std::abs(d[0]*r(2, 0) - d[2]*r(0, 0)) > ra+rb)
+ return false;
+ }
+ {
+ Real ra = a[0]*rabs(2, 1)+a[2]*rabs(0, 1);
+ Real rb = b[0]*rabs(1, 2)+b[2]*rabs(1, 0);
+ if (std::abs(d[0]*r(2, 1) - d[2]*r(0, 1)) > ra+rb)
+ return false;
+ }
+ {
+ Real ra = a[0]*rabs(2, 2)+a[2]*rabs(0, 2);
+ Real rb = b[0]*rabs(1, 1)+b[1]*rabs(1, 0);
+ if (std::abs(d[0]*r(2, 2) - d[2]*r(0, 2)) > ra+rb)
+ return false;
+ }
+
+ {
+ Real ra = a[0]*rabs(1, 0)+a[1]*rabs(0, 0);
+ Real rb = b[1]*rabs(2, 2)+b[2]*rabs(2, 1);
+ if (std::abs(d[1]*r(0, 0) - d[0]*r(1, 0)) > ra+rb)
+ return false;
+ }
+ {
+ Real ra = a[0]*rabs(1, 1)+a[1]*rabs(0, 1);
+ Real rb = b[0]*rabs(2, 2)+b[2]*rabs(2, 0);
+ if (std::abs(d[1]*r(0, 1) - d[0]*r(1, 1)) > ra+rb)
+ return false;
+ }
+ {
+ Real ra = a[0]*rabs(1, 2)+a[1]*rabs(0, 2);
+ Real rb = b[0]*rabs(2, 1)+b[1]*rabs(2, 0);
+ if (std::abs(d[1]*r(0, 2) - d[0]*r(1, 2)) > ra+rb)
+ return false;
+ }
+ return true;
+}
+
+bool OrientedBoundingBox::intersectsRay
+ (const Vec3& origin, const UnitVec3& direction, Real& distance) const {
+ // Transform the ray to the bounding box's reference frame.
+
+ Vec3 orig = ~getTransform()*origin;
+ UnitVec3 dir = ~getTransform().R()*direction;
+
+ // Check it against each plane that defines a side of the box.
+
+ Real minDist = MostNegativeReal;
+ Real maxDist = MostPositiveReal;
+ if (dir[0] == 0.0) {
+ if (orig[0] < 0 || orig[0] > getSize()[0])
+ return false;
+ }
+ else {
+ Real dist1 = -orig[0]/dir[0];
+ Real dist2 = (getSize()[0]-orig[0])/dir[0];
+ if (dist1 < dist2) {
+ if (dist1 > minDist)
+ minDist = dist1;
+ if (dist2 < maxDist)
+ maxDist = dist2;
+ }
+ else {
+ if (dist2 > minDist)
+ minDist = dist2;
+ if (dist1 < maxDist)
+ maxDist = dist1;
+ }
+ if (minDist > maxDist || maxDist < 0.0)
+ return false;
+ }
+ if (dir[1] == 0.0) {
+ if (orig[1] < 0 || orig[1] > getSize()[1])
+ return false;
+ }
+ else {
+ Real dist1 = -orig[1]/dir[1];
+ Real dist2 = (getSize()[1]-orig[1])/dir[1];
+ if (dist1 < dist2) {
+ if (dist1 > minDist)
+ minDist = dist1;
+ if (dist2 < maxDist)
+ maxDist = dist2;
+ }
+ else {
+ if (dist2 > minDist)
+ minDist = dist2;
+ if (dist1 < maxDist)
+ maxDist = dist1;
+ }
+ if (minDist > maxDist || maxDist < 0.0)
+ return false;
+ }
+ if (dir[2] == 0.0) {
+ if (orig[2] < 0 || orig[2] > getSize()[2])
+ return false;
+ }
+ else {
+ Real dist1 = -orig[2]/dir[2];
+ Real dist2 = (getSize()[2]-orig[2])/dir[2];
+ if (dist1 < dist2) {
+ if (dist1 > minDist)
+ minDist = dist1;
+ if (dist2 < maxDist)
+ maxDist = dist2;
+ }
+ else {
+ if (dist2 > minDist)
+ minDist = dist2;
+ if (dist1 < maxDist)
+ maxDist = dist1;
+ }
+ if (minDist > maxDist || maxDist < 0.0)
+ return false;
+ }
+ if (minDist > 0)
+ distance = minDist;
+ else
+ distance = 0;
+ return true;
+}
+
+Vec3 OrientedBoundingBox::findNearestPoint(const Vec3& position) const {
+ // Transform the point to the bounding box's reference frame.
+
+ Vec3 p = ~getTransform()*position;
+
+ // Find the nearest point in the box.
+
+ if (p[0] < 0)
+ p[0] = 0;
+ if (p[0] > getSize()[0])
+ p[0] = getSize()[0];
+ if (p[1] < 0)
+ p[1] = 0;
+ if (p[1] > getSize()[1])
+ p[1] = getSize()[1];
+ if (p[2] < 0)
+ p[2] = 0;
+ if (p[2] > getSize()[2])
+ p[2] = getSize()[2];
+
+ // Transform it back again.
+
+ return getTransform()*p;
+}
+
+void OrientedBoundingBox::getCorners(Vec3 corners[8]) const {
+ Vec3 dx = size[0]*transform.R().col(0);
+ Vec3 dy = size[1]*transform.R().col(1);
+ Vec3 dz = size[2]*transform.R().col(2);
+ corners[0] = transform.p();
+ corners[1] = corners[0]+dx;
+ corners[2] = corners[0]+dy;
+ corners[3] = corners[1]+dy;
+ corners[4] = corners[0]+dz;
+ corners[5] = corners[1]+dz;
+ corners[6] = corners[2]+dz;
+ corners[7] = corners[3]+dz;
+}
+
+OrientedBoundingBox
+operator*(const Transform& t, const OrientedBoundingBox& box) {
+ return OrientedBoundingBox(t*box.getTransform(), box.getSize());
+}
+
+} // namespace SimTK
+
diff --git a/SimTKmath/Geometry/src/gcvspl.cpp b/SimTKmath/Geometry/src/gcvspl.cpp
new file mode 100755
index 0000000..fd0e36b
--- /dev/null
+++ b/SimTKmath/Geometry/src/gcvspl.cpp
@@ -0,0 +1,1097 @@
+/* gcvspl.f -- translated by f2c (version of 16 February 1991 0:35:15).
+ You must link the resulting object file with the libraries:
+ -lF77 -lI77 -lm -lc (in that order)
+*/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+
+int SimTK_SIMMATH_EXPORT SimTK_gcvspl_(const SimTK_Real *, const SimTK_Real *, int *, const SimTK_Real *, const SimTK_Real *, int *, int *,
+ int *, int *, SimTK_Real *, SimTK_Real *, int *, SimTK_Real *, int *);
+
+SimTK_Real SimTK_SIMMATH_EXPORT SimTK_splder_(int *, int *, int *, SimTK_Real *, const SimTK_Real *, const SimTK_Real *, int *, SimTK_Real *, int);
+
+#define abs(x) ((x) >= 0 ? (x) : -(x))
+#define min(a,b) ((a) <= (b) ? (a) : (b))
+#define max(a,b) ((a) >= (b) ? (a) : (b))
+
+
+/* Table of constant values */
+
+static SimTK_Real c_b6 = SimTK_Real(1e-15);
+
+int SimTK_gcvspl_(const SimTK_Real *x, const SimTK_Real *y, int *ny,
+ const SimTK_Real *wx, const SimTK_Real *wy, int *m, int *n, int *k,
+ int *md, SimTK_Real *val, SimTK_Real *c, int *nc, SimTK_Real *
+ wk, int *ier)
+{
+
+ int m2 = 0;
+ int nm1 = 0;
+ SimTK_Real el = 0.;
+
+ /* System generated locals */
+ int y_dim1, y_offset, c_dim1, c_offset, i__1;
+
+ /* Local variables */
+ int nm2m1, nm2p1;
+ extern SimTK_Real splc_(int *, int *, int *, const SimTK_Real *,
+ int *, const SimTK_Real *, const SimTK_Real *, int *, SimTK_Real *,
+ SimTK_Real *, SimTK_Real *, SimTK_Real *, int *, SimTK_Real *,
+ SimTK_Real *, SimTK_Real *, SimTK_Real *, SimTK_Real *);
+ extern /* Subroutine */ int prep_(int *, int *, const SimTK_Real *,
+ const SimTK_Real *, SimTK_Real *, SimTK_Real *);
+ int i, j;
+ SimTK_Real alpha;
+ extern /* Subroutine */ int basis_(int *, int *, const SimTK_Real *,
+ SimTK_Real *, SimTK_Real *, SimTK_Real *);
+ SimTK_Real r1, r2, r3, r4;
+ int ib;
+ SimTK_Real gf2, gf1, gf3, gf4;
+ int iwe;
+ SimTK_Real err;
+
+
+ /* Parameter adjustments */
+ --wk;
+
+ c_dim1 = *nc;
+ c_offset = c_dim1 + 1;
+ c -= c_offset;
+
+ --wy;
+ --wx;
+
+ y_dim1 = *ny;
+ y_offset = y_dim1 + 1;
+ y -= y_offset;
+
+ --x;
+
+ /* Function Body */
+
+
+ *ier = 0;
+ if (abs(*md) > 4 ||
+ *md == 0 ||
+ ( abs(*md) == 1 && *val < 0. ) ||
+ ( abs(*md) == 3 && *val < 0. ) ||
+ ( abs(*md) == 4 && (*val < 0. || *val > (SimTK_Real) (*n - *m)))) {
+ *ier = 3;
+ return 0;
+ }
+ if (*md > 0) {
+ m2 = *m << 1;
+ nm1 = *n - 1;
+ } else {
+ if (m2 != *m << 1 || nm1 != *n - 1) {
+ *ier = 3;
+ return 0;
+ }
+ }
+ if (*m <= 0 || *n < m2) {
+ *ier = 1;
+ return 0;
+ }
+ if (wx[1] <= 0.) {
+ *ier = 2;
+ }
+ i__1 = *n;
+ for (i = 2; i <= i__1; ++i) {
+ if (wx[i] <= 0. || x[i - 1] >= x[i]) {
+ *ier = 2;
+ }
+ if (*ier != 0) {
+ return 0;
+ }
+ }
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ if (wy[j] <= 0.) {
+ *ier = 2;
+ }
+ if (*ier != 0) {
+ return 0;
+ }
+ }
+
+
+ nm2p1 = *n * (m2 + 1);
+ nm2m1 = *n * (m2 - 1);
+ ib = nm2p1 + 7;
+ iwe = ib + nm2m1;
+
+ if (*md > 0) {
+ basis_(m, n, &x[1], &wk[ib], &r1, &wk[7]);
+ prep_(m, n, &x[1], &wx[1], &wk[iwe], &el);
+ el /= r1;
+ }
+ if (abs(*md) != 1) {
+ goto L20;
+ }
+ r1 = *val;
+ goto L100;
+
+
+L20:
+ if (*md < -1) {
+ r1 = wk[4];
+ } else {
+ r1 = 1 / el;
+ }
+ r2 = r1 * 2;
+ gf2 = splc_(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r2, &
+ c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]);
+L40:
+ gf1 = splc_(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r1, &
+ c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]);
+ if (gf1 > gf2) {
+ goto L50;
+ }
+ if (wk[4] <= 0) {
+ goto L100;
+ }
+ r2 = r1;
+ gf2 = gf1;
+ r1 /= 2.;
+ goto L40;
+L50:
+ r3 = r2 * 2;
+L60:
+ gf3 = splc_(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r3, &
+ c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]);
+ if (gf3 > gf2) {
+ goto L70;
+ }
+ if (wk[4] >= SimTK_Real(999999999999999.88)) {
+ goto L100;
+ }
+ r2 = r3;
+ gf2 = gf3;
+ r3 *= 2.;
+ goto L60;
+L70:
+ r2 = r3;
+ gf2 = gf3;
+ alpha = (r2 - r1) / SimTK_Real(1.618033983);
+ r4 = r1 + alpha;
+ r3 = r2 - alpha;
+ gf3 = splc_(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r3, &
+ c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]);
+ gf4 = splc_(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r4, &
+ c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]);
+L80:
+ if (gf3 <= gf4) {
+ r2 = r4;
+ gf2 = gf4;
+ err = (r2 - r1) / (r1 + r2);
+ if (err * err + 1 == 1 || err <= SimTK_Real(1e-6)) {
+ goto L90;
+ }
+ r4 = r3;
+ gf4 = gf3;
+ alpha /= SimTK_Real(1.618033983);
+ r3 = r2 - alpha;
+ gf3 = splc_(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r3, &
+ c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]
+ );
+ } else {
+ r1 = r3;
+ gf1 = gf3;
+ err = (r2 - r1) / (r1 + r2);
+ if (err * err + 1 == 1 || err <= SimTK_Real(1e-6)) {
+ goto L90;
+ }
+ r3 = r4;
+ gf3 = gf4;
+ alpha /= SimTK_Real(1.618033983);
+ r4 = r1 + alpha;
+ gf4 = splc_(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r4, &
+ c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]
+ );
+ }
+ goto L80;
+L90:
+ r1 = (r1 + r2) / 2;
+
+
+L100:
+ gf1 = splc_(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r1, &
+ c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]);
+
+ return 0;
+}
+
+
+
+
+/* BASIS.FOR, 1985-06-03 */
+
+int basis_(int *m, int *n, const SimTK_Real *x, SimTK_Real
+ *b, SimTK_Real *bl, SimTK_Real *q)
+{
+ /* System generated locals */
+ int b_dim1, b_offset, q_offset, i__1, i__2, i__3, i__4;
+ SimTK_Real d__1;
+
+ /* Local variables */
+ int nmip1, i, j, k, l;
+ SimTK_Real u, v, y;
+ int j1, j2, m2, ir, mm1, mp1;
+ SimTK_Real arg;
+
+
+
+ /* Parameter adjustments */
+ q_offset = 1 - *m;
+ q -= q_offset;
+
+ b_dim1 = *m - 1 - (1 - *m) + 1;
+ b_offset = 1 - *m + b_dim1;
+ b -= b_offset;
+
+ --x;
+
+ if (*m == 1) {
+ i__1 = *n;
+ for (i = 1; i <= i__1; ++i) {
+ b[i * b_dim1] = 1.;
+ }
+ *bl = 1.;
+ return 0;
+ }
+
+ mm1 = *m - 1;
+ mp1 = *m + 1;
+ m2 = *m << 1;
+ i__1 = *n;
+ for (l = 1; l <= i__1; ++l) {
+ i__2 = *m;
+ for (j = -mm1; j <= i__2; ++j) {
+ q[j] = 0.;
+ }
+ q[mm1] = 1.;
+ if (l != 1 && l != *n) {
+ q[mm1] = 1 / (x[l + 1] - x[l - 1]);
+ }
+ arg = x[l];
+ i__2 = m2;
+ for (i = 3; i <= i__2; ++i) {
+ ir = mp1 - i;
+ v = q[ir];
+ if (l < i) {
+ i__3 = i;
+ for (j = l + 1; j <= i__3; ++j) {
+ u = v;
+ v = q[ir + 1];
+ q[ir] = u + (x[j] - arg) * v;
+ ++ir;
+ }
+ }
+ i__3 = l - i + 1;
+ j1 = max(i__3,1);
+ i__3 = l - 1, i__4 = *n - i;
+ j2 = min(i__3,i__4);
+ if (j1 <= j2) {
+ if (i < m2) {
+ i__3 = j2;
+ for (j = j1; j <= i__3; ++j) {
+ y = x[i + j];
+ u = v;
+ v = q[ir + 1];
+ q[ir] = u + (v - u) * (y - arg) / (y - x[j]);
+ ++ir;
+ }
+ } else {
+ i__3 = j2;
+ for (j = j1; j <= i__3; ++j) {
+ u = v;
+ v = q[ir + 1];
+ q[ir] = (arg - x[j]) * u + (x[i + j] - arg) * v;
+ ++ir;
+ }
+ }
+ }
+ nmip1 = *n - i + 1;
+ if (nmip1 < l) {
+ i__3 = l - 1;
+ for (j = nmip1; j <= i__3; ++j) {
+ u = v;
+ v = q[ir + 1];
+ q[ir] = (arg - x[j]) * u + v;
+ ++ir;
+ }
+ }
+ }
+ i__2 = mm1;
+ for (j = -mm1; j <= i__2; ++j) {
+ b[j + l * b_dim1] = q[j];
+ }
+ }
+
+ i__1 = mm1;
+ for (i = 1; i <= i__1; ++i) {
+ i__2 = mm1;
+ for (k = i; k <= i__2; ++k) {
+ b[-k + i * b_dim1] = 0.;
+ b[k + (*n + 1 - i) * b_dim1] = 0.;
+ }
+ }
+
+ *bl = 0.;
+ i__1 = *n;
+ for (i = 1; i <= i__1; ++i) {
+ i__2 = mm1;
+ for (k = -mm1; k <= i__2; ++k) {
+ *bl += (d__1 = b[k + i * b_dim1], abs(d__1));
+ }
+ }
+ *bl /= *n;
+
+ return 0;
+}
+
+
+
+/* PREP.FOR, 1985-07-04 */
+
+
+int prep_(int *m, int *n, const SimTK_Real *x, const SimTK_Real *
+ w, SimTK_Real *we, SimTK_Real *el)
+{
+ /* System generated locals */
+ int i__1, i__2, i__3;
+ SimTK_Real d__1;
+
+ /* Local variables */
+ SimTK_Real f;
+ int i, j, k, l;
+ SimTK_Real y, f1;
+ int i1, i2, m2;
+ SimTK_Real ff;
+ int jj, jm, kl, nm, ku;
+ SimTK_Real wi;
+ int n2m, mp1, i2m1, inc, i1p1, m2m1, m2p1;
+
+
+
+/* WE(-M:M,N) */
+ /* Parameter adjustments */
+ --we;
+ --w;
+ --x;
+
+ /* Function Body */
+ m2 = *m << 1;
+ mp1 = *m + 1;
+ m2m1 = m2 - 1;
+ m2p1 = m2 + 1;
+ nm = *n - *m;
+ f1 = -1.;
+ if (*m != 1) {
+ i__1 = *m;
+ for (i = 2; i <= i__1; ++i) {
+ f1 = -f1 * i;
+ }
+ i__1 = m2m1;
+ for (i = mp1; i <= i__1; ++i) {
+ f1 *= i;
+ }
+ }
+
+ i1 = 1;
+ i2 = *m;
+ jm = mp1;
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ inc = m2p1;
+ if (j > nm) {
+ f1 = -f1;
+ f = f1;
+ } else {
+ if (j < mp1) {
+ inc = 1;
+ f = f1;
+ } else {
+ f = f1 * (x[j + *m] - x[j - *m]);
+ }
+ }
+ if (j > mp1) {
+ ++i1;
+ }
+ if (i2 < *n) {
+ ++i2;
+ }
+ jj = jm;
+ ff = f;
+ y = x[i1];
+ i1p1 = i1 + 1;
+ i__2 = i2;
+ for (i = i1p1; i <= i__2; ++i) {
+ ff /= y - x[i];
+ }
+ we[jj] = ff;
+ jj += m2;
+ i2m1 = i2 - 1;
+ if (i1p1 <= i2m1) {
+ i__2 = i2m1;
+ for (l = i1p1; l <= i__2; ++l) {
+ ff = f;
+ y = x[l];
+ i__3 = l - 1;
+ for (i = i1; i <= i__3; ++i) {
+ ff /= y - x[i];
+ }
+ i__3 = i2;
+ for (i = l + 1; i <= i__3; ++i) {
+ ff /= y - x[i];
+ }
+ we[jj] = ff;
+ jj += m2;
+ }
+ }
+ ff = f;
+ y = x[i2];
+ i__2 = i2m1;
+ for (i = i1; i <= i__2; ++i) {
+ ff /= y - x[i];
+ }
+ we[jj] = ff;
+ jj += m2;
+ jm += inc;
+ }
+
+ kl = 1;
+ n2m = m2p1 * *n + 1;
+ i__1 = *m;
+ for (i = 1; i <= i__1; ++i) {
+ ku = kl + *m - i;
+ i__2 = ku;
+ for (k = kl; k <= i__2; ++k) {
+ we[k] = 0.;
+ we[n2m - k] = 0.;
+ }
+ kl += m2p1;
+ }
+
+ jj = 0;
+ *el = 0.;
+ i__1 = *n;
+ for (i = 1; i <= i__1; ++i) {
+ wi = w[i];
+ i__2 = m2p1;
+ for (j = 1; j <= i__2; ++j) {
+ ++jj;
+ we[jj] /= wi;
+ *el += (d__1 = we[jj], abs(d__1));
+ }
+ }
+ *el /= *n;
+
+ return 0;
+}
+
+
+
+/* SPLC.FOR, 1985-12-12 */
+
+SimTK_Real splc_(int *m, int *n, int *k, const SimTK_Real *y, int *
+ ny, const SimTK_Real *wx, const SimTK_Real *wy, int *mode, SimTK_Real *val,
+ SimTK_Real *p, SimTK_Real *eps, SimTK_Real *c, int *nc,
+ SimTK_Real *stat, SimTK_Real *b, SimTK_Real *we, SimTK_Real *el,
+ SimTK_Real *bwe)
+{
+ /* System generated locals */
+ int y_dim1, y_offset, c_dim1, c_offset, b_dim1, b_offset, we_dim1,
+ we_offset, bwe_dim1, bwe_offset, i__1, i__2, i__3, i__4;
+ SimTK_Real ret_val, d__1;
+
+ /* Local variables */
+ int i, j, l;
+ extern SimTK_Real trinv_(SimTK_Real *, SimTK_Real *, int *, int *)
+ ;
+ SimTK_Real dp;
+ int km;
+ SimTK_Real dt;
+ int kp;
+ extern /* Subroutine */ int bandet_(SimTK_Real *, int *, int *),
+ bansol_(SimTK_Real *, const SimTK_Real *, int *, SimTK_Real *,
+ int *, int *, int *, int *);
+ SimTK_Real pel, esn, trn;
+
+
+
+/* *** Check on p-value */
+
+ /* Parameter adjustments */
+ bwe_dim1 = *m - (-(*m)) + 1;
+ bwe_offset = -(*m) + bwe_dim1;
+ bwe -= bwe_offset;
+
+ we_dim1 = *m - (-(*m)) + 1;
+ we_offset = -(*m) + we_dim1;
+ we -= we_offset;
+
+ b_dim1 = *m - 1 - (1 - *m) + 1;
+ b_offset = 1 - *m + b_dim1;
+ b -= b_offset;
+
+ --stat;
+
+ c_dim1 = *nc;
+ c_offset = c_dim1 + 1;
+ c -= c_offset;
+
+ --wy;
+ --wx;
+
+ y_dim1 = *ny;
+ y_offset = y_dim1 + 1;
+ y -= y_offset;
+
+ /* Function Body */
+ dp = *p;
+ stat[4] = *p;
+ pel = *p * *el;
+ if (pel < *eps) {
+ dp = *eps / *el;
+ stat[4] = 0.;
+ }
+ if (pel * *eps > 1.) {
+ dp = 1 / (*el * *eps);
+ stat[4] = dp;
+ }
+
+ i__1 = *n;
+ for (i = 1; i <= i__1; ++i) {
+ i__2 = *m, i__3 = i - 1;
+ km = -min(i__2,i__3);
+ i__2 = *m, i__3 = *n - i;
+ kp = min(i__2,i__3);
+ i__2 = kp;
+ for (l = km; l <= i__2; ++l) {
+ if (abs(l) == *m) {
+ bwe[l + i * bwe_dim1] = dp * we[l + i * we_dim1];
+ } else {
+ bwe[l + i * bwe_dim1] = b[l + i * b_dim1] + dp * we[l + i *
+ we_dim1];
+ }
+ }
+ }
+
+ bandet_(&bwe[bwe_offset], m, n);
+ bansol_(&bwe[bwe_offset], &y[y_offset], ny, &c[c_offset], nc, m, n, k);
+ stat[3] = trinv_(&we[we_offset], &bwe[bwe_offset], m, n) * dp;
+ trn = stat[3] / *n;
+
+ esn = 0.;
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *n;
+ for (i = 1; i <= i__2; ++i) {
+ dt = -y[i + j * y_dim1];
+ i__3 = *m - 1, i__4 = i - 1;
+ km = -min(i__3,i__4);
+ i__3 = *m - 1, i__4 = *n - i;
+ kp = min(i__3,i__4);
+ i__3 = kp;
+ for (l = km; l <= i__3; ++l) {
+ dt += b[l + i * b_dim1] * c[i + l + j * c_dim1];
+ }
+ esn += dt * dt * wx[i] * wy[j];
+ }
+ }
+ esn /= *n * *k;
+
+ stat[6] = esn / trn;
+ stat[1] = stat[6] / trn;
+ stat[2] = esn;
+ if (abs(*mode) != 3) {
+ stat[5] = stat[6] - esn;
+ if (abs(*mode) == 1) {
+ ret_val = 0.;
+ }
+ if (abs(*mode) == 2) {
+ ret_val = stat[1];
+ }
+ if (abs(*mode) == 4) {
+ ret_val = (d__1 = stat[3] - *val, abs(d__1));
+ }
+ } else {
+ stat[5] = esn - *val * (trn * 2 - 1);
+ ret_val = stat[5];
+ }
+
+ return ret_val;
+}
+
+
+
+/* BANDET.FOR, 1985-06-03 */
+
+int bandet_(SimTK_Real *e, int *m, int *n)
+{
+ /* System generated locals */
+ int e_dim1, e_offset, i__1, i__2, i__3, i__4;
+
+ /* Local variables */
+ int i, k, l;
+ SimTK_Real di, dl;
+ int mi, km, lm;
+ SimTK_Real du;
+
+
+
+ /* Parameter adjustments */
+
+ e_dim1 = *m - (-(*m)) + 1;
+ e_offset = -(*m) + e_dim1;
+ e -= e_offset;
+
+ /* Function Body */
+ if (*m <= 0) {
+ return 0;
+ }
+ i__1 = *n;
+ for (i = 1; i <= i__1; ++i) {
+ di = e[i * e_dim1];
+ i__2 = *m, i__3 = i - 1;
+ mi = min(i__2,i__3);
+ if (mi >= 1) {
+ i__2 = mi;
+ for (k = 1; k <= i__2; ++k) {
+ di -= e[-k + i * e_dim1] * e[k + (i - k) * e_dim1];
+ }
+ e[i * e_dim1] = di;
+ }
+ i__2 = *m, i__3 = *n - i;
+ lm = min(i__2,i__3);
+ if (lm >= 1) {
+ i__2 = lm;
+ for (l = 1; l <= i__2; ++l) {
+ dl = e[-l + (i + l) * e_dim1];
+ i__3 = *m - l, i__4 = i - 1;
+ km = min(i__3,i__4);
+ if (km >= 1) {
+ du = e[l + i * e_dim1];
+ i__3 = km;
+ for (k = 1; k <= i__3; ++k) {
+ du -= e[-k + i * e_dim1] * e[l + k + (i - k) * e_dim1]
+ ;
+ dl -= e[-l - k + (l + i) * e_dim1] * e[k + (i - k) *
+ e_dim1];
+ }
+ e[l + i * e_dim1] = du;
+ }
+ e[-l + (i + l) * e_dim1] = dl / di;
+ }
+ }
+ }
+
+ return 0;
+}
+
+
+
+/* BANSOL.FOR, 1985-12-12 */
+
+int bansol_(SimTK_Real *e, const SimTK_Real *y, int *ny,
+ SimTK_Real *c, int *nc, int *m, int *n, int *k)
+{
+ /* System generated locals */
+ int e_dim1, e_offset, y_dim1, y_offset, c_dim1, c_offset, i__1, i__2,
+ i__3, i__4;
+
+ /* Local variables */
+ SimTK_Real d;
+ int i, j, l, mi, nm1;
+
+
+
+/* *** Check on special cases: M=0, M=1, M>1 */
+
+ /* Parameter adjustments */
+
+ c_dim1 = *nc;
+ c_offset = c_dim1 + 1;
+ c -= c_offset;
+
+ y_dim1 = *ny;
+ y_offset = y_dim1 + 1;
+ y -= y_offset;
+
+ e_dim1 = *m - (-(*m)) + 1;
+ e_offset = -(*m) + e_dim1;
+ e -= e_offset;
+
+ /* Function Body */
+ nm1 = *n - 1;
+ if ((i__1 = *m - 1) < 0) {
+ goto L10;
+ } else if (i__1 == 0) {
+ goto L40;
+ } else {
+ goto L80;
+ }
+
+L10:
+ i__1 = *n;
+ for (i = 1; i <= i__1; ++i) {
+ i__2 = *k;
+ for (j = 1; j <= i__2; ++j) {
+ c[i + j * c_dim1] = y[i + j * y_dim1] / e[i * e_dim1];
+ }
+ }
+ return 0;
+
+L40:
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ c[j * c_dim1 + 1] = y[j * y_dim1 + 1];
+ i__2 = *n;
+ for (i = 2; i <= i__2; ++i) {
+ c[i + j * c_dim1] = y[i + j * y_dim1] - e[i * e_dim1 - 1] * c[i -
+ 1 + j * c_dim1];
+ }
+ c[*n + j * c_dim1] /= e[*n * e_dim1];
+ for (i = nm1; i >= 1; --i) {
+ c[i + j * c_dim1] = (c[i + j * c_dim1] - e[i * e_dim1 + 1] * c[i
+ + 1 + j * c_dim1]) / e[i * e_dim1];
+ }
+ }
+ return 0;
+
+
+L80:
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ c[j * c_dim1 + 1] = y[j * y_dim1 + 1];
+ i__2 = *n;
+ for (i = 2; i <= i__2; ++i) {
+ i__3 = *m, i__4 = i - 1;
+ mi = min(i__3,i__4);
+ d = y[i + j * y_dim1];
+ i__3 = mi;
+ for (l = 1; l <= i__3; ++l) {
+ d -= e[-l + i * e_dim1] * c[i - l + j * c_dim1];
+ }
+ c[i + j * c_dim1] = d;
+ }
+ c[*n + j * c_dim1] /= e[*n * e_dim1];
+ for (i = nm1; i >= 1; --i) {
+ i__2 = *m, i__3 = *n - i;
+ mi = min(i__2,i__3);
+ d = c[i + j * c_dim1];
+ i__2 = mi;
+ for (l = 1; l <= i__2; ++l) {
+ d -= e[l + i * e_dim1] * c[i + l + j * c_dim1];
+ }
+ c[i + j * c_dim1] = d / e[i * e_dim1];
+ }
+ }
+ return 0;
+}
+
+
+
+/* TRINV.FOR, 1985-06-03 */
+
+
+SimTK_Real trinv_(SimTK_Real *b, SimTK_Real *e, int *m, int *n)
+{
+ /* System generated locals */
+ int b_dim1, b_offset, e_dim1, e_offset, i__1, i__2, i__3;
+ SimTK_Real ret_val;
+
+ /* Local variables */
+ int i, j, k;
+ SimTK_Real dd, dl;
+ int mi;
+ SimTK_Real du;
+ int mn, mp;
+
+
+
+/* *** Assess central 2*M+1 bands of E**-1 and store in array E */
+
+ /* Parameter adjustments */
+
+ e_dim1 = *m - (-(*m)) + 1;
+ e_offset = -(*m) + e_dim1;
+ e -= e_offset;
+
+ b_dim1 = *m - (-(*m)) + 1;
+ b_offset = -(*m) + b_dim1;
+ b -= b_offset;
+
+ /* Function Body */
+ e[*n * e_dim1] = 1 / e[*n * e_dim1];
+ for (i = *n - 1; i >= 1; --i) {
+ i__1 = *m, i__2 = *n - i;
+ mi = min(i__1,i__2);
+ dd = 1 / e[i * e_dim1];
+ i__1 = mi;
+ for (k = 1; k <= i__1; ++k) {
+ e[k + *n * e_dim1] = e[k + i * e_dim1] * dd;
+ e[-k + e_dim1] = e[-k + (k + i) * e_dim1];
+ }
+ dd += dd;
+ for (j = mi; j >= 1; --j) {
+ du = 0.;
+ dl = 0.;
+ i__1 = mi;
+ for (k = 1; k <= i__1; ++k) {
+ du -= e[k + *n * e_dim1] * e[j - k + (i + k) * e_dim1];
+ dl -= e[-k + e_dim1] * e[k - j + (i + j) * e_dim1];
+ }
+ e[j + i * e_dim1] = du;
+ e[-j + (j + i) * e_dim1] = dl;
+ dd -= e[j + *n * e_dim1] * dl + e[-j + e_dim1] * du;
+ }
+ e[i * e_dim1] = dd / 2;
+ }
+
+ dd = 0.;
+ i__1 = *n;
+ for (i = 1; i <= i__1; ++i) {
+ i__2 = *m, i__3 = i - 1;
+ mn = -min(i__2,i__3);
+ i__2 = *m, i__3 = *n - i;
+ mp = min(i__2,i__3);
+ i__2 = mp;
+ for (k = mn; k <= i__2; ++k) {
+ dd += b[k + i * b_dim1] * e[-k + (k + i) * e_dim1];
+ }
+ }
+ ret_val = dd;
+ i__1 = *m;
+ for (k = 1; k <= i__1; ++k) {
+ e[k + *n * e_dim1] = 0.;
+ e[-k + e_dim1] = 0.;
+ }
+ return ret_val;
+}
+
+
+
+/* SPLDER.FOR, 1985-06-11 */
+
+
+SimTK_Real SimTK_splder_(int *ider, int *m, int *n, SimTK_Real *t,
+ const SimTK_Real *x, const SimTK_Real *c, int *l, SimTK_Real *q, int coffset)
+{
+ /* System generated locals */
+ int i__1, i__2;
+ SimTK_Real ret_val;
+
+ /* Local variables */
+ int lk1i1;
+ SimTK_Real xjki;
+ int i, j, k;
+ SimTK_Real z;
+ int i1, j1, k1, j2, m2, ii, jj, ki, jl, lk, mi, nk, lm, ml, jm,
+ ir, ju;
+ extern /* Subroutine */ int search_(int *, const SimTK_Real *, SimTK_Real *,
+ int *);
+ SimTK_Real tt;
+ int lk1, mp1, m2m1, jin, nki, npm, lk1i, nki1;
+
+
+
+/* *** Derivatives of IDER.ge.2*M are alway zero */
+
+ /* Parameter adjustments */
+
+ --q;
+ c -= coffset;
+ --x;
+
+ /* Function Body */
+ m2 = *m << 1;
+ k = m2 - *ider;
+ if (k < 1) {
+ ret_val = 0.;
+ return ret_val;
+ }
+
+ search_(n, &x[1], t, l);
+
+ tt = *t;
+ mp1 = *m + 1;
+ npm = *n + *m;
+ m2m1 = m2 - 1;
+ k1 = k - 1;
+ nk = *n - k;
+ lk = *l - k;
+ lk1 = lk + 1;
+ lm = *l - *m;
+ jl = *l + 1;
+ ju = *l + m2;
+ ii = *n - m2;
+ ml = -(*l);
+ i__1 = ju;
+ for (j = jl; j <= i__1; ++j) {
+ if (j >= mp1 && j <= npm) {
+ q[j + ml] = c[coffset*(j - *m)];
+ } else {
+ q[j + ml] = 0.;
+ }
+ }
+
+ if (*ider > 0) {
+ jl -= m2;
+ ml += m2;
+ i__1 = *ider;
+ for (i = 1; i <= i__1; ++i) {
+ ++jl;
+ ++ii;
+ j1 = max(1,jl);
+ j2 = min(*l,ii);
+ mi = m2 - i;
+ j = j2 + 1;
+ if (j1 <= j2) {
+ i__2 = j2;
+ for (jin = j1; jin <= i__2; ++jin) {
+ --j;
+ jm = ml + j;
+ q[jm] = (q[jm] - q[jm - 1]) / (x[j + mi] - x[j]);
+ }
+ }
+ if (jl >= 1) {
+ goto L6;
+ }
+ i1 = i + 1;
+ j = ml + 1;
+ if (i1 <= ml) {
+ i__2 = ml;
+ for (jin = i1; jin <= i__2; ++jin) {
+ --j;
+ q[j] = -q[j - 1];
+ }
+ }
+L6:
+ ;
+ }
+ i__1 = k;
+ for (j = 1; j <= i__1; ++j) {
+ q[j] = q[j + *ider];
+ }
+ }
+
+ if (k1 >= 1) {
+ i__1 = k1;
+ for (i = 1; i <= i__1; ++i) {
+ nki = nk + i;
+ ir = k;
+ jj = *l;
+ ki = k - i;
+ nki1 = nki + 1;
+ if (*l >= nki1) {
+ i__2 = *l;
+ for (j = nki1; j <= i__2; ++j) {
+ q[ir] = q[ir - 1] + (tt - x[jj]) * q[ir];
+ --jj;
+ --ir;
+ }
+ }
+ lk1i = lk1 + i;
+ j1 = max(1,lk1i);
+ j2 = min(*l,nki);
+ if (j1 <= j2) {
+ i__2 = j2;
+ for (j = j1; j <= i__2; ++j) {
+ xjki = x[jj + ki];
+ z = q[ir];
+ q[ir] = z + (xjki - tt) * (q[ir - 1] - z) / (xjki - x[jj])
+ ;
+ --ir;
+ --jj;
+ }
+ }
+ if (lk1i <= 0) {
+ jj = ki;
+ lk1i1 = 1 - lk1i;
+ i__2 = lk1i1;
+ for (j = 1; j <= i__2; ++j) {
+ q[ir] += (x[jj] - tt) * q[ir - 1];
+ --jj;
+ --ir;
+ }
+ }
+ }
+ }
+
+ z = q[k];
+ if (*ider > 0) {
+ i__1 = m2m1;
+ for (j = k; j <= i__1; ++j) {
+ z *= j;
+ }
+ }
+ ret_val = z;
+ return ret_val;
+}
+
+
+
+/* SEARCH.FOR, 1985-06-03 */
+
+
+int search_(int *n, const SimTK_Real *x, SimTK_Real *t,
+ int *l)
+{
+ int il, iu;
+
+ /* Parameter adjustments */
+
+ --x;
+
+ /* Function Body */
+
+ if (*t < x[1]) {
+ *l = 0;
+ return 0;
+ }
+ if (*t >= x[*n]) {
+ *l = *n;
+ return 0;
+ }
+ *l = max(*l,1);
+ if (*l >= *n) {
+ *l = *n - 1;
+ }
+
+ if (*t >= x[*l]) {
+ goto L5;
+ }
+ --(*l);
+ if (*t >= x[*l]) {
+ return 0;
+ }
+
+ il = 1;
+L3:
+ iu = *l;
+L4:
+ *l = (il + iu) / 2;
+ if (iu - il <= 1) {
+ return 0;
+ }
+ if (*t < x[*l]) {
+ goto L3;
+ }
+ il = *l;
+ goto L4;
+L5:
+ if (*t < x[*l + 1]) {
+ return 0;
+ }
+ ++(*l);
+ if (*t < x[*l + 1]) {
+ return 0;
+ }
+ il = *l + 1;
+ iu = *n;
+ goto L4;
+
+}
diff --git a/SimTKmath/Integrators/CMakeLists.txt b/SimTKmath/Integrators/CMakeLists.txt
new file mode 100644
index 0000000..e982c06
--- /dev/null
+++ b/SimTKmath/Integrators/CMakeLists.txt
@@ -0,0 +1,19 @@
+# visit the CPodes directory to gather up source and tests
+add_subdirectory(src/CPodes)
+
+# add in local source and headers here
+FILE(GLOB src_files ./src/*.cpp)
+FILE(GLOB incl_files ./src/*.h)
+
+# append to the local scope copy, and then copy up to parent scope
+list(APPEND SOURCE_FILES ${src_files})
+set(SOURCE_FILES ${SOURCE_FILES} PARENT_SCOPE)
+
+list(APPEND SOURCE_INCLUDE_FILES ${incl_files})
+set(SOURCE_INCLUDE_FILES ${SOURCE_INCLUDE_FILES} PARENT_SCOPE)
+
+# We don't have any include dirs to add here but we have to pass
+# up anything added in a subdirectory.
+set(SOURCE_INCLUDE_DIRS ${SOURCE_INCLUDE_DIRS} PARENT_SCOPE)
+set(SOURCE_GROUPS ${SOURCE_GROUPS} PARENT_SCOPE)
+set(SOURCE_GROUP_FILES ${SOURCE_GROUP_FILES} PARENT_SCOPE)
diff --git a/SimTKmath/Integrators/include/simmath/CPodesIntegrator.h b/SimTKmath/Integrators/include/simmath/CPodesIntegrator.h
new file mode 100644
index 0000000..631b594
--- /dev/null
+++ b/SimTKmath/Integrators/include/simmath/CPodesIntegrator.h
@@ -0,0 +1,80 @@
+#ifndef SimTK_SIMMATH_CPODES_INTEGRATOR_H_
+#define SimTK_SIMMATH_CPODES_INTEGRATOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+
+#include "simmath/Integrator.h"
+#include "simmath/internal/SimTKcpodes.h"
+
+namespace SimTK {
+
+class CPodesIntegratorRep;
+
+/**
+ * This is an Integrator based on the CPODES library. It is an error controlled, variable order
+ * implicit integrator. It provides a good combination of accuracy, stability, and speed, and
+ * is a good choice for integrating stiff problems.
+ *
+ * When creating a CPodesIntegrator, you can specify various options for how to perform
+ * the implicit integration: the linear multistep method to use (Adams or BDF), and the nonlinear system
+ * iteration type (Newton iteration or functional iteration). For stiff problems, the recommended choices
+ * are BDF with Newton iteration. For non-stiff problems, using Adams and/or functional iteration may
+ * provide better performance. Note that Adams is <i>never</i> recommended for systems that include
+ * constraints.
+ */
+
+class SimTK_SIMMATH_EXPORT CPodesIntegrator : public Integrator {
+public:
+ /**
+ * Create a CPodesIntegrator for integrating a System.
+ */
+ explicit CPodesIntegrator(const System& sys, CPodes::LinearMultistepMethod method=CPodes::BDF);
+ /**
+ * Create a CPodesIntegrator for integrating a System. The nonlinear system iteration type is chosen automatically
+ * based on the linear multistep method: Newton iteration for BDF (the default), and functional iteration for Adams.
+ */
+ CPodesIntegrator(const System& sys, CPodes::LinearMultistepMethod method, CPodes::NonlinearSystemIterationType iterationType);
+ /**
+ * CPODES provides its own mechanism for projecting the system onto the constraint manifold. By default,
+ * CPodesIntegrator uses the System's project() method for doing projection, which is usually more
+ * efficient. Invoking this method tells it to use the CPODES mechanism instead.
+ *
+ * This method must be invoked before the integrator is initialized. Invoking it after initialization
+ * will produce an exception.
+ */
+ void setUseCPodesProjection();
+ /**
+ * Restrict the integrator to lower orders than it is otherwise capable of (up to 12 for Adams, 5 for BDF). This method
+ * may only be used to decrease the maximum order permitted, never to increase it. Once you specify an order limit, calling it
+ * again with a larger value will fail.
+ */
+ void setOrderLimit(int order);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_CPODES_INTEGRATOR_H_
diff --git a/SimTKmath/Integrators/include/simmath/ExplicitEulerIntegrator.h b/SimTKmath/Integrators/include/simmath/ExplicitEulerIntegrator.h
new file mode 100644
index 0000000..c9e0112
--- /dev/null
+++ b/SimTKmath/Integrators/include/simmath/ExplicitEulerIntegrator.h
@@ -0,0 +1,58 @@
+#ifndef SimTK_SIMMATH_EXPLICIT_EULER_INTEGRATOR_H_
+#define SimTK_SIMMATH_EXPLICIT_EULER_INTEGRATOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/Integrator.h"
+
+namespace SimTK {
+
+class ExplicitEulerIntegratorRep;
+
+/**
+ * This is an Integrator based on the explicit Euler algorithm. It is an
+ * error controlled, first order explicit integrator. This is one of the simplest
+ * integrators possible. As such, it is useful as a test case, but usually is
+ * a bad choice for real simulations.
+ */
+
+class SimTK_SIMMATH_EXPORT ExplicitEulerIntegrator : public Integrator {
+public:
+ /**
+ * Create an ExplicitEulerIntegrator for integrating a System with variable sized steps.
+ */
+ explicit ExplicitEulerIntegrator(const System& sys);
+ /**
+ * Create an ExplicitEulerIntegrator for integrating a System with fixed sized steps.
+ */
+ ExplicitEulerIntegrator(const System& sys, Real stepSize);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_EXPLICIT_EULER_INTEGRATOR_H_
+
+
diff --git a/SimTKmath/Integrators/include/simmath/Integrator.h b/SimTKmath/Integrators/include/simmath/Integrator.h
new file mode 100644
index 0000000..6f4ec89
--- /dev/null
+++ b/SimTKmath/Integrators/include/simmath/Integrator.h
@@ -0,0 +1,419 @@
+#ifndef SimTK_SIMMATH_INTEGRATOR_H_
+#define SimTK_SIMMATH_INTEGRATOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This is the header file that user code should include to pick up the
+ * SimTK Simmath numerical integration tools.
+ */
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+
+namespace SimTK {
+
+
+class IntegratorRep;
+
+/** An Integrator is an object that can advance the State of a System
+through time. This is an abstract class. Subclasses implement a variety of
+different integration methods, which vary in their speed, accuracy, stability,
+and various other properties.
+
+<h3>Usage</h3>
+
+An Integrator is most often used in combination with a TimeStepper. The
+TimeStepper automates much of the work of using an Integrator: invoking it
+repeatedly to advance time, calling event handlers, reintializing the
+Integrator as necessary, and so on. A typical use of an Integrator generally
+resembles the following:
+
+ at code
+ // Instantiate an Integrator subclass which is appropriate for your problem.
+ VerletIntegrator integ(system);
+ // Set configuration options on the Integrator.
+ integ.setAccuracy(1e-3);
+ // Create a TimeStepper and use it to run the simulation.
+ TimeStepper stepper(system, integ);
+ stepper.initialize(initialState);
+ stepper.stepTo(finalTime);
+ at endcode
+
+<h3>Mathematical Overview</h3>
+
+Given a continuous system of differential equations for state variables y, and
+optionally a manifold (set of algebraic equations) on which the solution must
+lie, an Integrator object will advance that system through time. If the full
+system is continuous, this is sufficient. However, most interesting systems
+consist of both continuous and discrete equations, in which case the overall
+time advancement is handled by a TimeStepper object which will use an
+Integrator as an "inner loop" for advancing the system across continuous
+intervals between discrete updates. In that case, in addition to continuous
+state variables y there will be a set of discrete variables d which are held
+constant during an integration interval.
+
+The continuous part of the system is an ODE-on-a-manifold system suitable for
+solution via coordinate projection[1], structured like this:
+ (1) y' = f(d;t,y) differential equations
+ (2) 0 = c(d;t,y) algebraic equations (manifold is c=0)
+ (3) e = e(d;t,y) event triggers (watch for zero crossings)
+with initial conditions t0,y0 such that c=0. The "d;" is a reminder that the
+overall system is dependent on discrete variables d as well as y, but d cannot
+change during a continuous interval.
+
+By "ODE on a manifold" we mean that the ODE (1) automatically satisfies the
+condition that IF c==0, THEN c'=0, where
+ c'=partial(c)/partial(t) + [partial(c)/partial(y)]*y'.
+This is a less stringent condition than an ODE with "first integral" invariant,
+in which c'=0 regardless of whether c=0.
+
+[1] Hairer, Lubich, Wanner, "Geometric Numerical Integration:
+Structure-Preserving Algorithms for Ordinary Differential Equations", 2nd ed.,
+section IV.4, pg 109ff, Springer, 2006.
+
+The discrete variables d are updated by the time stepper upon occurence of
+specific events, which terminate a continuous interval. The Integrator detects
+these using a set of scalar-valued event trigger functions shown as equation
+(3) above. An event trigger function for a particular event must be designed so
+that it has a zero crossing when the event occurs. The integrator can thus
+watch for sign changes in event triggers and terminate the current step when a
+zero crossing occurs, notifying the system and giving it a chance to handle the
+event; that is, update its state variables discontinuously.
+
+The zero crossings of continuous event trigger functions will be isolated
+quickly; discontinuous ones have to be "binary chopped" which is more expensive
+but they will still work.
+
+We are given a set of weights W for the y's, and a set of tolerances T for the
+constraint errors. Given an accuracy specification (like 0.1%), the integrators
+here are expected to solve for y(t) such that the local error
+|W*yerr|_RMS <= accuracy, and |T*c(t,y)|_RMS <= accuracy at all times.
+
+TODO: isolation tolerances for witnesses; dealing with simultaneity.
+**/
+class SimTK_SIMMATH_EXPORT Integrator {
+public:
+ Integrator() : rep(0) { }
+ ~Integrator();
+
+ // These are the exceptions that can be thrown by this class.
+
+ class InitializationFailed;
+ class StepSizeTooSmall;
+ class StepFailed;
+ class TriedToAdvancePastFinalTime;
+ class CantAskForEventInfoWhenNoEventTriggered;
+
+ /// Get the name of this integration method
+ const char* getMethodName() const;
+ /// Get the minimum order this Integrator may use
+ int getMethodMinOrder() const;
+ /// Get the maximum order this Integrator may use
+ int getMethodMaxOrder() const;
+ /// Get whether this Integrator provides error control. An error controlled Integrator will dynamically adjust its
+ /// step size to maintain the level of accuracy specified with setAccuracy(). An Integrator which does not provide
+ /// error control cannot do this, and will usually ignore the value specified with setAccuracy().
+ bool methodHasErrorControl() const;
+
+ /// Supply the integrator with a starting state. This must be called before the first call to stepBy() or stepTo().
+ /// The specified state is copied into the Integrator's internally maintained current state; subsequent changes to
+ /// the State object passed in here will not affect the integration.
+ void initialize(const State& state);
+
+ /// After an event handler has made a discontinuous change to the Integrator's
+ /// "advanced state", this method must be called to reinitialize the Integrator.
+ /// Event handlers can do varying amounts of damage and some events will require no
+ /// reinitialization, or minimal reinitialization, depending on details
+ /// of the particular integration method. So after the handler has
+ /// mangled our State, we tell the Integrator the lowest Stage which was
+ /// changed and allow the Integrator to figure out how much reinitialization
+ /// to do.
+ ///
+ /// If "shouldTerminate" is passed in true, the Integrator will wrap
+ /// things up and report that the end of the simulation has been reached.
+ void reinitialize(Stage stage, bool shouldTerminate);
+
+ /// Return a State corresponding to the "current" time at the end of the last call to
+ /// stepTo() or stepBy(). This may be an interpolated value earlier than getAdvancedState().
+ const State& getState() const;
+ /// Get the time of the current State. This is equivalent to calling getState().getTime().
+ Real getTime() const {return getState().getTime();}
+
+ /// Get whether getState() will return an interpolated state or just the same thing as getAdvancedState() does.
+ /// In most cases, you should not have reason to care whether the state is interpolated or not.
+ bool isStateInterpolated() const;
+
+ /// Return the state representing the trajectory point to which the
+ /// integrator has irreversibly advanced. This may be later than the
+ /// state return by getState().
+ const State& getAdvancedState() const;
+ /// Get the time of the advanced State. This is equivalent to calling getAdvancedState().getTime().
+ Real getAdvancedTime() const {return getAdvancedState().getTime();}
+
+ /// Get a non-const reference to the advanced state.
+ State& updAdvancedState();
+
+ /// Get the accuracy which is being used for error control. Usually this is the same value that was
+ /// specified to setAccuracy().
+ Real getAccuracyInUse() const;
+ /// Get the constraint tolerance which is being used for error control. Usually this is the same value that was
+ /// specified to setConstraintTolerance().
+ Real getConstraintToleranceInUse() const;
+
+ /// When a step is successful, it will return an indication of what
+ /// caused it to stop where it did. When unsuccessful it will throw
+ /// an exception so you won't see any return value.
+ ///
+ /// When return of control is due ONLY to reaching a report time,
+ /// (status is ReachedReportTime) the integrator's getState() method may return an
+ /// interpolated value at an earlier time than its getAdvancedState()
+ /// method would return. For the other returns, and whenever the report
+ /// time coincides with the end of an internal step, getState() and
+ /// getAdvancedState() will be identical.
+ ///
+ /// Note: we ensure algorithmically that no report time, scheduled time,
+ /// or final time t can occur *within* an event window, that is, we will
+ /// never have t_low < t < t_high for any interesting t. Further, t_report,
+ /// t_scheduled and t_final can coincide with t_high but only t_report can
+ /// be at t_low. The interior of t_low:t_high is a "no man's land" where we
+ /// don't understand the solution, so must be avoided.
+ enum SuccessfulStepStatus {
+ /// stopped only to report; might be interpolated
+ ReachedReportTime =1,
+ /// localized an event; this is the *before* state (interpolated)
+ ReachedEventTrigger =2,
+ /// reached the limit provided in stepTo() (scheduled event)
+ ReachedScheduledEvent=3,
+ /// user requested control whenever an internal step is successful
+ TimeHasAdvanced =4,
+ /// took a lot of internal steps but didn't return control yet
+ ReachedStepLimit =5,
+ /// termination; don't call again
+ EndOfSimulation =6,
+ /// the beginning of a continuous interval: either the start of the
+ /// simulation, or t_high after an event handler has modified the state.
+ StartOfContinuousInterval=7,
+ InvalidSuccessfulStepStatus = -1
+ };
+ /// Get a human readable description of the reason a step returned.
+ static String getSuccessfulStepStatusString(SuccessfulStepStatus);
+
+ /// Integrate the System until something happens which requires outside processing, and return a status code describing what happened.
+ /// @param reportTime the time of the next scheduled report
+ /// @param scheduledEventTime the time of the next scheduled event
+ SuccessfulStepStatus stepTo(Real reportTime, Real scheduledEventTime=Infinity);
+ /// Integrate the System until something happens which requires outside processing, and return a status code describing what happened.
+ /// @param interval the interval from the current time (as returned by getTime()) until the next scheduled report
+ /// @param scheduledEventTime the time of the next scheduled event
+ SuccessfulStepStatus stepBy(Real interval, Real scheduledEventTime=Infinity);
+
+
+ /// Get the window (tLow, tHigh] within which one or more events have been localized. This may only be called
+ /// when stepTo() or stepBy() has returned ReachedEventTrigger.
+ Vec2 getEventWindow() const;
+ /// Get the IDs of all events which have been localized within the event window. This may only be called
+ /// when stepTo() or stepBy() has returned ReachedEventTrigger.
+ const Array_<EventId>& getTriggeredEvents() const;
+ /// Get the estimated times of all events which have been localized within the event window. This may only be called
+ /// when stepTo() or stepBy() has returned ReachedEventTrigger.
+ const Array_<Real>& getEstimatedEventTimes() const;
+ /// Get EventTriggers describing the events which have been localized within the event window. This may only be called
+ /// when stepTo() or stepBy() has returned ReachedEventTrigger.
+ const Array_<Event::Trigger>& getEventTransitionsSeen() const;
+
+
+ // TERMINATION //
+
+ /// Once the simulation has ended, getTerminationReason() may be called to find out what caused it to end.
+ enum TerminationReason {
+ /// The simulation reached the time specified by setFinalTime().
+ ReachedFinalTime = 1,
+ /// An error occurred which the Integrator was unable to handle.
+ AnUnrecoverableErrorOccurred = 2,
+ /// An event handler function requested that the simulation terminate immediately.
+ EventHandlerRequestedTermination = 3,
+ /// This will be returned if getTerminationReason() is called before the simulation has ended.
+ InvalidTerminationReason = -1
+ };
+
+ /// Get whether the simulation has terminated. If this returns true, you should not
+ /// call stepTo() or stepBy() again.
+ bool isSimulationOver() const;
+
+ /// Get the reason the simulation terminated. This should only be called if
+ /// isSimulationOver() returns true.
+ TerminationReason getTerminationReason() const;
+
+ /// Get a human readable description of the termination reason.
+ static String getTerminationReasonString(TerminationReason);
+
+ /// Reset all statistics to zero.
+ void resetAllStatistics();
+
+ /// Get the size of the first successful step after the last initialize() call.
+ Real getActualInitialStepSizeTaken() const;
+
+ /// Get the size of the most recent successful step.
+ Real getPreviousStepSizeTaken() const;
+
+ /// Get the step size that will be attempted first on the next call to stepTo() or stepBy().
+ Real getPredictedNextStepSize() const;
+
+ /// Get the total number of steps that have been attempted (successfully or unsuccessfully) since
+ /// the last call to resetAllStatistics().
+ int getNumStepsAttempted() const;
+ /// Get the total number of steps that have been successfully taken since the last call to resetAllStatistics().
+ int getNumStepsTaken() const;
+ /// Get the total number of state realizations that have been performed since the last call to resetAllStatistics().
+ int getNumRealizations() const;
+ /// Get the total number of times a state positions Q have been projected
+ /// since the last call to resetAllStatistics().
+ int getNumQProjections() const;
+ /// Get the total number of times a state velocities U have been projected
+ /// since the last call to resetAllStatistics().
+ int getNumUProjections() const;
+ /// Get the total number of times a state has been projected (counting
+ /// both Q and U projections) since the last call to resetAllStatistics().
+ int getNumProjections() const;
+ /// Get the number of attempted steps that have failed due to the error being unacceptably high since
+ /// the last call to resetAllStatistics().
+ int getNumErrorTestFailures() const;
+ /// Get the number of attempted steps that failed due to non-convergence of
+ /// internal step iterations. This is most common with iterative methods
+ /// but can occur if for some reason a step can't be completed. It is reset
+ /// to zero by resetAllStatistics.
+ int getNumConvergenceTestFailures() const;
+ /// Get the number of attempted steps that have failed due to an error when realizing the state since
+ /// the last call to resetAllStatistics().
+ int getNumRealizationFailures() const;
+ /// Get the number of attempted steps that have failed due to an error
+ /// when projecting the state positions (Q) since the last call to
+ /// resetAllStatistics().
+ int getNumQProjectionFailures() const;
+ /// Get the number of attempted steps that have failed due to an error
+ /// when projecting the state velocities (U) since the last call to
+ /// resetAllStatistics().
+ int getNumUProjectionFailures() const;
+ /// Get the number of attempted steps that have failed due to an error
+ /// when projecting the state (either a Q- or U-projection) since
+ /// the last call to resetAllStatistics().
+ int getNumProjectionFailures() const;
+ /// For iterative methods, get the number of internal step iterations in steps that led to
+ /// convergence (not necessarily successful steps). Reset to zero by resetAllStatistics().
+ int getNumConvergentIterations() const;
+ /// For iterative methods, get the number of internal step iterations in steps that did not lead
+ /// to convergence. Reset to zero by resetAllStatistics().
+ int getNumDivergentIterations() const;
+ /// For iterative methods, this is the total number of internal step iterations taken regardless
+ /// of whether those iterations led to convergence or to successful steps. This is the sum of
+ /// the number of convergent and divergent iterations which are available separately.
+ int getNumIterations() const;
+
+ /// Set the time at which the simulation should end. The default is infinity. Some integrators may
+ /// not support this option.
+ void setFinalTime(Real tFinal);
+ /// Set the initial step size that should be attempted. The default depends on the integration method.
+ /// Some integrators may not support this option.
+ void setInitialStepSize(Real hinit);
+ /// Set the minimum step size that should ever be used. The default depends on the integration method.
+ /// Some integrators may not support this option.
+ void setMinimumStepSize(Real hmin);
+ /// Set the maximum step size that should ever be used. The default depends on the integration method.
+ /// Some integrators may not support this option.
+ void setMaximumStepSize(Real hmax);
+
+ /// Set the integrator to use a single fixed step size for all steps. This
+ /// is exactly equivalent to calling setInitialStepSize(),
+ /// setMinimumStepSize(), and setMaximumStepSize(), passing the same value
+ /// to each one. This will therefore not work correctly if the integrator
+ /// does not support minimum and/or maximum step sizes.
+ void setFixedStepSize(Real stepSize);
+
+ /// Set the overall accuracy that should be used for integration. If the
+ /// Integrator does not support error control (methodHasErrorControl()
+ /// returns false), this may have no effect.
+ void setAccuracy(Real accuracy);
+ /// Set the tolerance within which constraints must be satisfied.
+ void setConstraintTolerance(Real consTol);
+ /// (Advanced) Use infinity norm (maximum absolute value) instead of
+ /// default RMS norm to evaluate whether accuracy has been achieved for
+ /// states and for constraint tolerance. The infinity norm is more strict
+ /// but may permit use of a looser accuracy request.
+ void setUseInfinityNorm(bool useInfinityNorm);
+ /// (Advanced) Are we currently using the infinity norm?
+ bool isInfinityNormInUse() const;
+
+
+ /// Set the maximum number of steps that may be taken within a single call
+ /// to stepTo() or stepBy(). If this many internal steps occur before
+ /// reaching the report time, it will return control with a ReachedStepLimit
+ /// status. If nSteps <= 0, the number of steps will be unlimited.
+ void setInternalStepLimit(int nSteps);
+
+ /// Set whether the Integrator should return from stepTo() or stepBy() after
+ /// every internal step, even if no event has occurred and the report time
+ /// has not been reached. The default is false.
+ void setReturnEveryInternalStep(bool shouldReturn);
+
+ /// Set whether the system should be projected back to the constraint
+ /// manifold after every step. If this is false, projection will only be
+ /// performed when the constraint error exceeds the allowed tolerance.
+ void setProjectEveryStep(bool forceProject);
+ /// Set whether the Integrator is permitted to return interpolated states
+ /// for reporting purposes which may be less accurate than the "real"
+ /// states that form the trajectory. Setting this to false may
+ /// significantly affect performance, since the Integrator will be forced
+ /// to decrease its step size at every scheduled reporting time.
+ ///
+ /// This option is generally only meaningful if interpolated states are
+ /// less accurate than other states on the trajectory. If an Integrator can
+ /// produce interpolated states that have the same accuracy as the rest of
+ /// the trajectory, it may ignore this option.
+ void setAllowInterpolation(bool shouldInterpolate);
+ /// Set whether interpolated states should be projected back to the
+ /// constraint manifold after interpolation is performed. The default is
+ /// "true".
+ void setProjectInterpolatedStates(bool shouldProject);
+ /// (Advanced) Constraint projection may use an out-of-date iteration
+ /// matrix for efficiency. You can force strict use of a current iteration
+ /// matrix recomputed at each iteration if you want.
+ void setForceFullNewton(bool forceFullNewton);
+
+ /// OBSOLETE: use getSuccessfulStepStatusString().
+ static String successfulStepStatusString(SuccessfulStepStatus stat)
+ { return getSuccessfulStepStatusString(stat); }
+
+protected:
+ const IntegratorRep& getRep() const {assert(rep); return *rep;}
+ IntegratorRep& updRep() {assert(rep); return *rep;}
+
+ // opaque implementation for binary compatibility
+ IntegratorRep* rep;
+ friend class IntegratorRep;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_INTEGRATOR_H_
diff --git a/SimTKmath/Integrators/include/simmath/RungeKutta2Integrator.h b/SimTKmath/Integrators/include/simmath/RungeKutta2Integrator.h
new file mode 100644
index 0000000..1a92d21
--- /dev/null
+++ b/SimTKmath/Integrators/include/simmath/RungeKutta2Integrator.h
@@ -0,0 +1,49 @@
+#ifndef SimTK_SIMMATH_RUNGE_KUTTA_2_INTEGRATOR_H_
+#define SimTK_SIMMATH_RUNGE_KUTTA_2_INTEGRATOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/Integrator.h"
+
+namespace SimTK {
+class RungeKutta2IntegratorRep;
+
+/**
+ * This is a 2nd order Runge-Kutta Integrator using coefficients that are
+ * also known as the explicit trapezoid rule. It is an error controlled,
+ * second order, two stage explicit integrator with an embedded 1st order
+ * error estimate.
+ */
+class SimTK_SIMMATH_EXPORT RungeKutta2Integrator : public Integrator {
+public:
+ explicit RungeKutta2Integrator(const System& sys);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_RUNGE_KUTTA_2_INTEGRATOR_H_
+
+
diff --git a/SimTKmath/Integrators/include/simmath/RungeKutta3Integrator.h b/SimTKmath/Integrators/include/simmath/RungeKutta3Integrator.h
new file mode 100644
index 0000000..32c6a0f
--- /dev/null
+++ b/SimTKmath/Integrators/include/simmath/RungeKutta3Integrator.h
@@ -0,0 +1,49 @@
+#ifndef SimTK_SIMMATH_RUNGE_KUTTA_3_INTEGRATOR_H_
+#define SimTK_SIMMATH_RUNGE_KUTTA_3_INTEGRATOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/Integrator.h"
+
+namespace SimTK {
+class RungeKutta3IntegratorRep;
+
+/**
+ * This is a 3rd order Runge-Kutta Integrator using coefficents from J.C. Butcher's
+ * book "The Numerical Analysis of Ordinary Differential Equations", John Wiley & Sons,
+ * 1987, page 325. It is an error controlled, third order, three stage explicit integrator
+ * with an embedded 2nd order error estimate.
+ */
+class SimTK_SIMMATH_EXPORT RungeKutta3Integrator : public Integrator {
+public:
+ explicit RungeKutta3Integrator(const System& sys);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_RUNGE_KUTTA_3_INTEGRATOR_H_
+
+
diff --git a/SimTKmath/Integrators/include/simmath/RungeKuttaFeldbergIntegrator.h b/SimTKmath/Integrators/include/simmath/RungeKuttaFeldbergIntegrator.h
new file mode 100644
index 0000000..66b2018
--- /dev/null
+++ b/SimTKmath/Integrators/include/simmath/RungeKuttaFeldbergIntegrator.h
@@ -0,0 +1,49 @@
+#ifndef SimTK_SIMMATH_RUNGE_KUTTA_FELDBERG_INTEGRATOR_H_
+#define SimTK_SIMMATH_RUNGE_KUTTA_FELDBERG_INTEGRATOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/Integrator.h"
+
+namespace SimTK {
+
+/**
+ * This is an Integrator based on the Runge-Kutta-Feldberg algorithm. It is an
+ * error controlled, fifth order explicit integrator.
+ */
+
+class RungeKuttaFeldbergIntegratorRep;
+
+class SimTK_SIMMATH_EXPORT RungeKuttaFeldbergIntegrator : public Integrator {
+public:
+ explicit RungeKuttaFeldbergIntegrator(const System& sys);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_RUNGE_KUTTA_FELDBERG_INTEGRATOR_H_
+
+
diff --git a/SimTKmath/Integrators/include/simmath/RungeKuttaMersonIntegrator.h b/SimTKmath/Integrators/include/simmath/RungeKuttaMersonIntegrator.h
new file mode 100644
index 0000000..373e55a
--- /dev/null
+++ b/SimTKmath/Integrators/include/simmath/RungeKuttaMersonIntegrator.h
@@ -0,0 +1,49 @@
+#ifndef SimTK_SIMMATH_RUNGE_KUTTA_MERSON_INTEGRATOR_H_
+#define SimTK_SIMMATH_RUNGE_KUTTA_MERSON_INTEGRATOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/Integrator.h"
+
+namespace SimTK {
+
+/**
+ * This is an Integrator based on the Runge-Kutta-Merson algorithm. It is an
+ * error controlled, fourth order explicit integrator.
+ */
+
+class RungeKuttaMersonIntegratorRep;
+
+class SimTK_SIMMATH_EXPORT RungeKuttaMersonIntegrator : public Integrator {
+public:
+ explicit RungeKuttaMersonIntegrator(const System& sys);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_RUNGE_KUTTA_MERSON_INTEGRATOR_H_
+
+
diff --git a/SimTKmath/Integrators/include/simmath/SemiExplicitEuler2Integrator.h b/SimTKmath/Integrators/include/simmath/SemiExplicitEuler2Integrator.h
new file mode 100644
index 0000000..40f6600
--- /dev/null
+++ b/SimTKmath/Integrators/include/simmath/SemiExplicitEuler2Integrator.h
@@ -0,0 +1,100 @@
+#ifndef SimTK_SIMMATH_SEMI_EXPLICIT_EULER_2_INTEGRATOR_H_
+#define SimTK_SIMMATH_SEMI_EXPLICIT_EULER_2_INTEGRATOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/Integrator.h"
+
+namespace SimTK {
+
+class SemiExplicitEuler2IntegratorRep;
+
+/** This is an implementation of a variable-step, first-order semi-explicit
+Euler method, also known as semi-implicit Euler or symplectic Euler.
+
+The fixed-step, first-order semi-explicit Euler method is very popular for game
+engines such as ODE, and Simbody offers that method as
+SemiExplicitEulerIntegrator. See the documentation there for a discussion and
+theory. This integrator is an attempt to extend that method to give some
+error control. For equivalent performance to the fixed-step method, this one
+needs to run at twice the step size (because it takes two substeps). Be sure
+to set its maximum allowable step large enough, and use a very loose accuracy
+setting.
+
+<h3>Theory</h3>
+
+See SemiExplicitEulerIntegrator for the theory behind the underlying first
+order method. Here we use Richardson Extrapolation (step doubling) to get an
+error estimate. See Solving Ordinary Differential Equations I: Nonstiff
+Problems, 2nd rev. ed., Hairer, Norsett, & Wanner, section II.4, pp 164-165.
+Note that we are not using the local extrapolation trick to get a higher-order
+result; see below for why.
+
+The idea is to take two steps of length h, and a single big step of length 2h,
+and combine the results. Starting at t=t0 we can get to t2=t0+2h two different
+ways: <pre>
+ u1 = u0 + h udot(q0,u0) ub = u0 + 2h udot(q0,u0)
+ q1 = q0 + h N(q0)u1 and qb = q0 + 2h N(q0)ub
+ u2 = u1 + h udot(q1,u1) (b = big step)
+ q2 = q1 + h N(q1)u2
+</pre> Note that we can use udot(q0,u0) in both methods so it doesn't cost much
+to calculate both solutions y2(t2)=(q2,u2) and yb(t2)=(qb,ub).
+
+Now assume the true solution at t2 is y(t2)=(q,u). Then <pre>
+ y = y2 + 2C h^2 + O(h^3)
+ y = yb + C(2h)^2 + O(h^3)
+</pre> where C is some unknown constant of the underlying method. Ignoring the
+3rd order terms, subtract these two equations to get: <pre>
+ y2-yb = 2Ch^2
+</pre> which is the 2nd order error term for y2. We can use that for error
+control.
+
+Hairer suggests using that error term to improve on y2 to get a better estimate
+for y (this is called "local extrapolation") : <pre>
+ y2' = y2 + (y2-yb) = 2 y2 - yb
+ y = y2' + O(h^3) <-- we don't actually do this
+</pre> y2' is a 2nd order accurate estimate for y(t2). But ... we don't have an
+error estimate for the revised value and we don't actually know that it is more
+accurate. In fact, although this sounds lovely in theory, in practice we will
+often be running at close to a stability boundary and using the "big step" value
+to update the result causes stability problems. So we'll skip the local
+extrapolation and stick with the first order result from the two half-steps.
+
+ at author Michael Sherman
+**/
+
+class SimTK_SIMMATH_EXPORT SemiExplicitEuler2Integrator : public Integrator {
+public:
+ /** Create a SemiExplicitEuler2Integrator for integrating a System with
+ variable size steps. **/
+ SemiExplicitEuler2Integrator(const System& sys);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_SEMI_EXPLICIT_EULER_2_INTEGRATOR_H_
+
+
diff --git a/SimTKmath/Integrators/include/simmath/SemiExplicitEulerIntegrator.h b/SimTKmath/Integrators/include/simmath/SemiExplicitEulerIntegrator.h
new file mode 100644
index 0000000..3854aed
--- /dev/null
+++ b/SimTKmath/Integrators/include/simmath/SemiExplicitEulerIntegrator.h
@@ -0,0 +1,108 @@
+#ifndef SimTK_SIMMATH_SEMI_EXPLICIT_EULER_INTEGRATOR_H_
+#define SimTK_SIMMATH_SEMI_EXPLICIT_EULER_INTEGRATOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/Integrator.h"
+
+namespace SimTK {
+
+class SemiExplicitEulerIntegratorRep;
+
+/** This is an implementation of the fixed-step Semi-Explicit Euler method, also
+known as Semi-Implicit Euler or Symplectic Euler.
+
+This method is very popular for game engines such as ODE, because it is
+a first order method that is more stable than Explicit Euler. However, the
+implementation used in gaming is fully explicit, which is only correct if
+there are no velocity-dependent force elements. Coriolis forces, damping,
+etc. would require an implicit solution which is generally too slow for
+real time simulation.
+
+ at note There is no error estimator for this integrator so it cannot adjust the
+step size. It is given a fixed step size on construction and will use that
+unless you change it. The step size may be reduced to isolate events, and the
+method is capable of interpolation (linear) if you want reports at shorter
+intervals than the step size.
+
+<h3>Theory</h3>
+
+The correct implementation of this method is described in Geometric Numerical
+Integration, Hairer, Lubich & Wanner 2006, page 3. The method applies to
+a system of differential equations that can be partitioned like
+this: <pre>
+ udot = udot(q,u)
+ qdot = qdot(q,u)
+ = N(q)u in Simbody
+</pre> Semi-Implicit Euler treats one variable implicitly, and the other
+explicitly, producing two different possible forms: <pre>
+Form 1:
+ q1 = q0 + h qdot(q1,u0) <-- implicit in q
+ u1 = u0 + h udot(q1,u0)
+Form 2:
+ u1 = u0 + h udot(q0,u1) <-- implicit in u
+ q1 = q0 + h qdot(q0,u1)
+</pre> If udot were udot(q) only, Form 2 would be <pre>
+ u1 = u0 + h udot(q0) <-- if no velocity dependence
+ q1 = q0 + h N(q0)u1
+</pre> That would be a correct, yet fully explicit implementation of Symplectic
+Euler. Instead, the standard gaming implementation is <pre>
+ u1 = u0 + h udot(q0,u0) <-- wrong; should be implicit in u
+ q1 = q0 + h N(q0)u1
+</pre> This differs from a true Symplectic Euler by however much the velocity-
+dependent forces change from u0 to u1. If they are very small, no great loss.
+That may be less likely in internal coordinates though. Making this implicit
+would be quite expensive since we don't have analytical partial derivatives
+of udot available.
+
+Form 1 on the other hand could be implemented efficiently as a true
+semi-implicit method: <pre>
+ q1 = q0 + h N(q1)u0 <-- implicit in q
+ u1 = u0 + h udot(q1,u0)
+</pre> The directional partial derivatives D N(q)*u0 / D q are easily calculated
+for the block diagonal matrix N which in many cases is just an identity matrix.
+A disadvantage is that accelerations are unknown at (q0,u0) and (q1,u1); if
+those are needed (very common!) a second evaluation of the accelerations
+would be required. For that expense, a second order integration method could
+have been used instead. I also don't know whether this form would have the same
+stability properties in practice as the other one; it would be interesting to
+experiment. However, for now we use Form 2 in the gamer's fully-explicit style.
+This will not work well in the presence of huge damping forces and probably not
+for very high rotation rates either!
+**/
+
+class SimTK_SIMMATH_EXPORT SemiExplicitEulerIntegrator : public Integrator {
+public:
+ /** Create a SemiExplicitEulerIntegrator for integrating a System with
+ fixed size steps. **/
+ SemiExplicitEulerIntegrator(const System& sys, Real stepSize);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_SEMI_EXPLICIT_EULER_INTEGRATOR_H_
+
+
diff --git a/SimTKmath/Integrators/include/simmath/TimeStepper.h b/SimTKmath/Integrators/include/simmath/TimeStepper.h
new file mode 100644
index 0000000..1496fe1
--- /dev/null
+++ b/SimTKmath/Integrators/include/simmath/TimeStepper.h
@@ -0,0 +1,125 @@
+#ifndef SimTK_SIMMATH_TIMESTEPPER_H_
+#define SimTK_SIMMATH_TIMESTEPPER_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/Integrator.h"
+
+namespace SimTK {
+class Integrator;
+
+/**
+ * This class uses an Integrator to advance a System through time. For example:
+ *
+ * <pre>
+ * TimeStepper stepper(system, integrator);
+ * stepper.initialize(initialState);
+ * stepper.stepTo(finalTime);
+ * </pre>
+ *
+ * stepTo() invokes the Integrator to advance time. It detects events which may occur, calls event handlers as
+ * appropriate, then invokes the Integrator again to continue advancing time until the target time is reached.
+ */
+class SimTK_SIMMATH_EXPORT TimeStepper {
+public:
+ /**
+ * Create a TimeStepper to advance a System.
+ *
+ * This constructor leaves the Integrator unspecified. You therefore must call setIntegrator()
+ * before calling initialize().
+ */
+ explicit TimeStepper(const System& system);
+ /**
+ * Create a TimeStepper to advance a System using an Integrator.
+ */
+ TimeStepper(const System& system, Integrator& integrator);
+ ~TimeStepper();
+ /**
+ * Set the Integrator this TimeStepper will use to advance the System.
+ */
+ void setIntegrator(Integrator& integrator);
+ /**
+ * Get the Integrator being used to advance the System.
+ */
+ const Integrator& getIntegrator() const;
+ /**
+ * Get a non-const reference to the Integrator being used to advance the System.
+ */
+ Integrator& updIntegrator();
+ /**
+ * Get whether the TimeStepper should report every significant state returned by the Integrator.
+ * If this is true, stepTo() will return whenever the Integrator reports a significant state,
+ * such as when an event occurs or the start of a new continuous interval. If this is false,
+ * stepTo() will only return when the specified time has been reached or when the simulation is
+ * terminated.
+ */
+ bool getReportAllSignificantStates() const;
+ /**
+ * Set whether the TimeStepper should report every significant state returned by the Integrator.
+ * If this is true, stepTo() will return whenever the Integrator reports a significant state,
+ * such as when an event occurs or the start of a new continuous interval. If this is false,
+ * stepTo() will only return when the specified time has been reached or when the simulation is
+ * terminated.
+ */
+ void setReportAllSignificantStates(bool b);
+ /**
+ * Supply the time stepper with a starting state. This must be called after the Integrator has
+ * been set, and before the first call to stepTo().
+ *
+ * The specified state is copied into the Integrator's internally maintained "advanced" state;
+ * subsequent changes to the State object passed in here will not affect the simulation.
+ */
+ void initialize(const State&);
+ /**
+ * Get the current State of the System being integrated. Usually this will correspond to the
+ * time specified in the most recent call to stepTo(). It may be an interpolated state which
+ * is earlier than the Integrator's "advanced" state.
+ */
+ const State& getState() const;
+ /**
+ * Get the current time of the System being integrated. This is identical to calling getState().getTime().
+ */
+ Real getTime() const {return getState().getTime();}
+ /**
+ * Use the Integrator to advance the System up to the specified time. This method will repeatedly
+ * invoke the Integrator as necessary, handling any events which occur. The TimeStepper must be
+ * initialized by calling initialize() before this method may be called.
+ *
+ * When this method returns, the System will usually have been advanced all the way to the specified
+ * final time. There are situations where it may return sooner, however: if setReportAllSignificantStates()
+ * was set to true, and a significant state occurred; if {@link Integrator#setFinalTime setFinalTime()} was invoked on the Integrator,
+ * and the final time was reached; or if an event handler requested that the simulation terminate
+ * immediately.
+ */
+ Integrator::SuccessfulStepStatus stepTo(Real time);
+private:
+ class TimeStepperRep* rep;
+ friend class TimeStepperRep;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_TIMESTEPPER_H_
diff --git a/SimTKmath/Integrators/include/simmath/VerletIntegrator.h b/SimTKmath/Integrators/include/simmath/VerletIntegrator.h
new file mode 100644
index 0000000..bf342b0
--- /dev/null
+++ b/SimTKmath/Integrators/include/simmath/VerletIntegrator.h
@@ -0,0 +1,81 @@
+#ifndef SimTK_SIMMATH_VERLET_INTEGRATOR_H_
+#define SimTK_SIMMATH_VERLET_INTEGRATOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/Integrator.h"
+
+namespace SimTK {
+
+class VerletIntegratorRep;
+
+/**
+ * This is an Integrator based on the velocity Verlet algorithm. It is a third order, semi-explicit integrator.
+ * Velocity independent forces only need to be evaluated once per time step, but forces which depend on velocity
+ * must be evaluated multiple times. This makes it a very efficient algorithm for systems where most of the time
+ * is spent evaluating forces that depend only on position.
+ *
+ * Although this is a third order integrator, the velocities reported at each time step are only accurate to lower
+ * order. If any forces depend on velocity, this may lead to a reduction in the overall accuracy of integration,
+ * since the forces being integrated are less accurate than the method used to integrate them. Whether this
+ * actually happens in a particular case depends on the magnitude of the velocity dependent forces, how sensitive
+ * they are to errors in velocity, and how the system is affected by those forces.
+ *
+ * When this integrator is used with fixed size time steps, it is symplectic. This means that it is extremely
+ * good at conserving energy, and will usually produce much less variation in energy than most other integrators
+ * would at the same step size. This makes it a good choice for problems where accurate energy conservation over
+ * long time periods is important. To use it in this way, use the constructor which takes a step size, or call
+ * setFixedStepSize().
+ *
+ * Alternatively, it may be used in variable step mode, in which case the step size is selected based on the
+ * accuracy specified by calling setAccuracy(). In this case it is no longer symplectic, so the energy will
+ * fluctuate more over time. Because the step size is adjusted based on the local error in each step, however,
+ * the trajectory will generally be more accurate in variable step size mode than would be obtained with the
+ * same number of fixed size steps spanning the same amount of time.
+ *
+ * Another possible strategy is to set only a maximum step size but not a minimum step size. It should be chosen
+ * such that most time steps will use the fixed maximum step size, but smaller steps can be used when necessary to
+ * preserve accuracy. This leads to fairly good long term energy conservation, while still maintaining reasonable
+ * accuracy when unusually large forces transiently occur.
+ */
+
+class SimTK_SIMMATH_EXPORT VerletIntegrator : public Integrator {
+public:
+ /**
+ * Create a VerletIntegrator for integrating a System with variable size steps.
+ */
+ explicit VerletIntegrator(const System& sys);
+ /**
+ * Create a VerletIntegrator for integrating a System with fixed size steps.
+ */
+ VerletIntegrator(const System& sys, Real stepSize);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_VERLET_INTEGRATOR_H_
+
+
diff --git a/SimTKmath/Integrators/include/simmath/internal/SimTKcpodes.h b/SimTKmath/Integrators/include/simmath/internal/SimTKcpodes.h
new file mode 100644
index 0000000..e65db66
--- /dev/null
+++ b/SimTKmath/Integrators/include/simmath/internal/SimTKcpodes.h
@@ -0,0 +1,421 @@
+#ifndef SimTK_SIMMATH_CPODES_H_
+#define SimTK_SIMMATH_CPODES_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This is the header file that user code should include to pick up the
+ * SimTK C++ interface to the Sundials CPODES coordinate-projection
+ * integrator.
+ */
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+
+#include <cstdio> // Needed for "FILE".
+
+namespace SimTK {
+
+/**
+ * This abstract class defines the system to be integrated with SimTK
+ * CPodes. Note that this defines a client-side virtual function table
+ * which must be used only on the client side. Library-side access to
+ * these virtual functions is done only through a set
+ * of equivalent static functions (provided in this same header files)
+ * whose addresses can be reliably "tossed over the fence" to the library
+ * side without compromising binary compatibility.
+ */
+class SimTK_SIMMATH_EXPORT CPodesSystem {
+public:
+ virtual ~CPodesSystem() {}
+ // The default implementations of these virtual functions
+ // just throw an "unimplemented virtual function" exception.
+
+ // At least one of these two must be supplied by the concrete class.
+ virtual int explicitODE(Real t, const Vector& y, Vector& fout) const;
+ virtual int implicitODE(Real t, const Vector& y, const Vector& yp,
+ Vector& fout) const;
+
+ virtual int constraint(Real t, const Vector& y,
+ Vector& cout) const;
+ virtual int project(Real t, const Vector& ycur, Vector& corr,
+ Real epsProj, Vector& err) const; // err is in/out
+ virtual int quadrature(Real t, const Vector& y,
+ Vector& qout) const;
+ virtual int root(Real t, const Vector& y, const Vector& yp,
+ Vector& gout) const;
+ virtual int weight(const Vector& y, Vector& weights) const;
+ virtual void errorHandler(int error_code, const char* module,
+ const char* function, char* msg) const;
+
+ //TODO: Jacobian functions
+};
+
+
+// These static functions are private to the current (client-side) compilation
+// unit. They are used to navigate the client-side CPodesSystem virtual function
+// table, which cannot be done on the library side. Note that these are defined
+// in the SimTK namespace so don't need "SimTK" in their names.
+static int explicitODE_static(const CPodesSystem& sys,
+ Real t, const Vector& y, Vector& fout)
+ { return sys.explicitODE(t,y,fout); }
+
+static int implicitODE_static(const CPodesSystem& sys,
+ Real t, const Vector& y, const Vector& yp, Vector& fout)
+ { return sys.implicitODE(t,y,yp,fout); }
+
+static int constraint_static(const CPodesSystem& sys,
+ Real t, const Vector& y, Vector& cout)
+ { return sys.constraint(t,y,cout); }
+
+static int project_static(const CPodesSystem& sys,
+ Real t, const Vector& ycur, Vector& corr,
+ Real epsProj, Vector& err)
+ { return sys.project(t,ycur,corr,epsProj,err); }
+
+static int quadrature_static(const CPodesSystem& sys,
+ Real t, const Vector& y, Vector& qout)
+ { return sys.quadrature(t,y,qout); }
+
+static int root_static(const CPodesSystem& sys,
+ Real t, const Vector& y, const Vector& yp, Vector& gout)
+ { return sys.root(t,y,yp,gout); }
+
+static int weight_static(const CPodesSystem& sys,
+ const Vector& y, Vector& weights)
+ { return sys.weight(y,weights); }
+
+static void errorHandler_static(const CPodesSystem& sys,
+ int error_code, const char* module,
+ const char* function, char* msg)
+ { sys.errorHandler(error_code,module,function,msg); }
+
+/**
+ * This is a straightforward translation of the Sundials CPODES C
+ * interface into C++. The class CPodes represents a single instance
+ * of a CPODES integrator, and handles the associated memory internally.
+ * Methods here are identical to the corresponding CPODES functions (with
+ * the "CPode" prefix removed) but are const-correct and use SimTK
+ * Vector & Real rather than Sundials N_Vector and realtype.
+ */
+class SimTK_SIMMATH_EXPORT CPodes {
+public:
+ // no default constructor
+ // copy constructor and default assignment are suppressed
+
+ enum ODEType {
+ UnspecifiedODEType=0,
+ ExplicitODE,
+ ImplicitODE
+ };
+
+ enum LinearMultistepMethod {
+ UnspecifiedLinearMultistepMethod=0,
+ BDF,
+ Adams
+ };
+
+ enum NonlinearSystemIterationType {
+ UnspecifiedNonlinearSystemIterationType=0,
+ Newton,
+ Functional
+ };
+
+ enum ToleranceType {
+ UnspecifiedToleranceType=0,
+ ScalarScalar,
+ ScalarVector,
+ WeightFunction
+ };
+
+ enum ProjectionNorm {
+ UnspecifiedProjectionNorm=0,
+ L2Norm,
+ ErrorNorm
+ };
+
+ enum ConstraintLinearity {
+ UnspecifiedConstraintLinearity=0,
+ Linear,
+ Nonlinear
+ };
+
+ enum ProjectionFactorizationType {
+ UnspecifiedProjectionFactorizationType=0,
+ ProjectWithLU,
+ ProjectWithQR,
+ ProjectWithSchurComplement,
+ ProjectWithQRPivot // for handling redundancy
+ };
+
+ enum StepMode {
+ UnspecifiedStepMode=0,
+ Normal,
+ OneStep,
+ NormalTstop,
+ OneStepTstop
+ };
+
+ explicit CPodes
+ (ODEType ode=UnspecifiedODEType,
+ LinearMultistepMethod lmm=UnspecifiedLinearMultistepMethod,
+ NonlinearSystemIterationType nls=UnspecifiedNonlinearSystemIterationType)
+ {
+ // Perform construction of the CPodesRep on the library side.
+ librarySideCPodesConstructor(ode, lmm, nls);
+ // But fill in function pointers from the client side.
+ clientSideCPodesConstructor();
+ }
+
+ // Values for 'flag' return values. These are just the "normal" return
+ // values; there are many more which are all negative and represent
+ // error conditions.
+ static const int Success = 0;
+ static const int TstopReturn = 1;
+ static const int RootReturn = 2;
+ static const int Warning = 99;
+ static const int TooMuchWork = -1;
+ static const int TooClose = -27;
+
+ // These values should be used by user routines. "Success" is the
+ // same as above. A positive return value means "recoverable error",
+ // i.e., CPodes should cut the step size and try again, while a
+ // negative number means "unrecoverable error" which will kill
+ // CPODES altogether with a CP_xxx_FAIL error. The particular numerical
+ // values here have no significance, just + vs. -.
+ static const int RecoverableError = 9999;
+ static const int UnrecoverableError = -9999;
+
+ ~CPodes();
+
+ // Depending on the setting of ode_type at construction, init()
+ // and reInit() will tell CPodes to use either the explicitODE()
+ // or implicitODE() function from the CPodesSystem, so the user
+ // MUST have overridden at least one of those virtual methods.
+ int init(CPodesSystem& sys,
+ Real t0, const Vector& y0, const Vector& yp0,
+ ToleranceType tt, Real reltol, void* abstol);
+
+ int reInit(CPodesSystem& sys,
+ Real t0, const Vector& y0, const Vector& yp0,
+ ToleranceType tt, Real reltol, void* abstol);
+
+ // This tells CPodes to make use of the user's constraint()
+ // method from CPodesSystem, and perform projection internally.
+ int projInit(ProjectionNorm, ConstraintLinearity,
+ const Vector& ctol);
+
+ // This tells CPodes to make use of the user's project()
+ // method from CPodesSystem.
+ int projDefine();
+
+ // These tell CPodes to make use of the user's quadrature()
+ // method from CPodesSystem.
+ int quadInit(const Vector& q0);
+ int quadReInit(const Vector& q0);
+
+ // This tells CPodes to make use of the user's root() method
+ // from CPodesSystem.
+ int rootInit(int nrtfn);
+
+ // This tells CPodes to make use of the user's errorHandler()
+ // method from CPodesSystem.
+ int setErrHandlerFn();
+
+ // These tells CPodes to make use of the user's weight()
+ // method from CPodesSystem.
+ int setEwtFn();
+
+ // TODO: these routines should enable methods that are defined
+ // in the CPodesSystem, but a proper interface to the Jacobian
+ // routines hasn't been implemented yet.
+ int dlsSetJacFn(void* jac, void* jac_data);
+ int dlsProjSetJacFn(void* jacP, void* jacP_data);
+
+
+ int step(Real tout, Real* tret,
+ Vector& y_inout, Vector& yp_inout, StepMode=Normal);
+
+ int setErrFile(FILE* errfp);
+ int setMaxOrd(int maxord);
+ int setMaxNumSteps(int mxsteps);
+ int setMaxHnilWarns(int mxhnil);
+ int setStabLimDet(bool stldet) ;
+ int setInitStep(Real hin);
+ int setMinStep(Real hmin);
+ int setMaxStep(Real hmax);
+ int setStopTime(Real tstop);
+ int setMaxErrTestFails(int maxnef);
+
+ int setMaxNonlinIters(int maxcor);
+ int setMaxConvFails(int maxncf);
+ int setNonlinConvCoef(Real nlscoef);
+
+ int setProjUpdateErrEst(bool proj_err);
+ int setProjFrequency(int proj_freq);
+ int setProjTestCnstr(bool test_cnstr);
+ int setProjLsetupFreq(int proj_lset_freq);
+ int setProjNonlinConvCoef(Real prjcoef);
+
+ int setQuadErrCon(bool errconQ,
+ int tol_typeQ, Real reltolQ, void* abstolQ);
+
+ int setTolerances(int tol_type, Real reltol, void* abstol);
+
+ int setRootDirection(Array_<int>& rootdir);
+
+ int getDky(Real t, int k, Vector& dky);
+
+ int getQuad(Real t, Vector& yQout);
+ int getQuadDky(Real t, int k, Vector& dky);
+
+ int getWorkSpace(int* lenrw, int* leniw);
+ int getNumSteps(int* nsteps);
+ int getNumFctEvals(int* nfevals);
+ int getNumLinSolvSetups(int* nlinsetups);
+ int getNumErrTestFails(int* netfails);
+ int getLastOrder(int* qlast);
+ int getCurrentOrder(int* qcur);
+ int getNumStabLimOrderReds(int* nslred);
+ int getActualInitStep(Real* hinused);
+ int getLastStep(Real* hlast);
+ int getCurrentStep(Real* hcur);
+ int getCurrentTime(Real* tcur);
+ int getTolScaleFactor(Real* tolsfac);
+ int getErrWeights(Vector& eweight);
+ int getEstLocalErrors(Vector& ele) ;
+ int getNumGEvals(int* ngevals);
+ int getRootInfo(int* rootsfound);
+ int getRootWindow(Real* tLo, Real* tHi);
+ int getIntegratorStats(int* nsteps,
+ int* nfevals, int* nlinsetups,
+ int* netfails, int* qlast,
+ int* qcur, Real* hinused, Real* hlast,
+ Real* hcur, Real* tcur);
+
+ int getNumNonlinSolvIters(int* nniters);
+ int getNumNonlinSolvConvFails(int* nncfails);
+ int getNonlinSolvStats(int* nniters, int* nncfails);
+ int getProjNumProj(int* nproj);
+ int getProjNumCnstrEvals(int* nce);
+ int getProjNumLinSolvSetups(int* nsetupsP);
+ int getProjNumFailures(int* nprf) ;
+ int getProjStats(int* nproj,
+ int* nce, int* nsetupsP,
+ int* nprf);
+ int getQuadNumFunEvals(int* nqevals);
+ int getQuadErrWeights(Vector& eQweight);
+ char* getReturnFlagName(int flag);
+
+
+ int dlsGetWorkSpace(int* lenrwLS, int* leniwLS);
+ int dlsGetNumJacEvals(int* njevals);
+ int dlsGetNumFctEvals(int* nfevalsLS);
+ int dlsGetLastFlag(int* flag);
+ char* dlsGetReturnFlagName(int flag);
+
+ int dlsProjGetNumJacEvals(int* njPevals);
+ int dlsProjGetNumFctEvals(int* ncevalsLS);
+
+ int lapackDense(int N);
+ int lapackBand(int N, int mupper, int mlower);
+ int lapackDenseProj(int Nc, int Ny, ProjectionFactorizationType);
+
+private:
+ // This is how we get the client-side virtual functions to
+ // be callable from library-side code while maintaining binary
+ // compatibility.
+ typedef int (*ExplicitODEFunc)(const CPodesSystem&,
+ Real t, const Vector& y, Vector& fout);
+ typedef int (*ImplicitODEFunc)(const CPodesSystem&,
+ Real t, const Vector& y, const Vector& yp,
+ Vector& fout);
+ typedef int (*ConstraintFunc) (const CPodesSystem&,
+ Real t, const Vector& y, Vector& cout);
+ typedef int (*ProjectFunc) (const CPodesSystem&,
+ Real t, const Vector& ycur, Vector& corr,
+ Real epsProj, Vector& err);
+ typedef int (*QuadratureFunc) (const CPodesSystem&,
+ Real t, const Vector& y, Vector& qout);
+ typedef int (*RootFunc) (const CPodesSystem&,
+ Real t, const Vector& y, const Vector& yp,
+ Vector& gout);
+ typedef int (*WeightFunc) (const CPodesSystem&,
+ const Vector& y, Vector& weights);
+ typedef void (*ErrorHandlerFunc)(const CPodesSystem&,
+ int error_code, const char* module,
+ const char* function, char* msg);
+
+ // Note that these routines do not tell CPodes to use the supplied
+ // functions. They merely provide the client-side addresses of functions
+ // which understand how to find the user's virtual functions, should those
+ // actually be provided. Control over whether to actually call any of these
+ // is handled elsewhere, with user-visible methods. These private methods
+ // are to be called only upon construction of the CPodes object here. They
+ // are not even dependent on which user-supplied concrete CPodesSystem is
+ // being used.
+ void registerExplicitODEFunc(ExplicitODEFunc);
+ void registerImplicitODEFunc(ImplicitODEFunc);
+ void registerConstraintFunc(ConstraintFunc);
+ void registerProjectFunc(ProjectFunc);
+ void registerQuadratureFunc(QuadratureFunc);
+ void registerRootFunc(RootFunc);
+ void registerWeightFunc(WeightFunc);
+ void registerErrorHandlerFunc(ErrorHandlerFunc);
+
+
+ // This is the library-side part of the CPodes constructor. This must
+ // be done prior to the client side construction.
+ void librarySideCPodesConstructor(ODEType, LinearMultistepMethod, NonlinearSystemIterationType);
+
+ // Note that this routine MUST be called from client-side code so that
+ // it picks up exactly the static routines above which will agree with
+ // the client about the layout of the CPodesSystem virtual function table.
+ void clientSideCPodesConstructor() {
+ registerExplicitODEFunc(explicitODE_static);
+ registerImplicitODEFunc(implicitODE_static);
+ registerConstraintFunc(constraint_static);
+ registerProjectFunc(project_static);
+ registerQuadratureFunc(quadrature_static);
+ registerRootFunc(root_static);
+ registerWeightFunc(weight_static);
+ registerErrorHandlerFunc(errorHandler_static);
+ }
+
+ // FOR INTERNAL USE ONLY
+private:
+ class CPodesRep* rep;
+ friend class CPodesRep;
+
+ const CPodesRep& getRep() const {assert(rep); return *rep;}
+ CPodesRep& updRep() {assert(rep); return *rep;}
+
+ // Suppress copy constructor and default assigment operator.
+ CPodes(const CPodes&);
+ CPodes& operator=(const CPodes&);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_CPODES_H_
diff --git a/SimTKmath/Integrators/src/AbstractIntegratorRep.cpp b/SimTKmath/Integrators/src/AbstractIntegratorRep.cpp
new file mode 100644
index 0000000..d691670
--- /dev/null
+++ b/SimTKmath/Integrators/src/AbstractIntegratorRep.cpp
@@ -0,0 +1,825 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "AbstractIntegratorRep.h"
+
+using namespace SimTK;
+
+AbstractIntegratorRep::AbstractIntegratorRep
+ (Integrator* handle, const System& sys, int minOrder, int maxOrder,
+ const std::string& methodName, bool hasErrorControl)
+: IntegratorRep(handle, sys), minOrder(minOrder), maxOrder(maxOrder),
+ methodName(methodName), hasErrorControl(hasErrorControl) {}
+
+
+
+//==============================================================================
+// METHOD INITIALIZE
+//==============================================================================
+void AbstractIntegratorRep::methodInitialize(const State& state) {
+ initialized = true;
+ if (userInitStepSize != -1)
+ currentStepSize = userInitStepSize;
+ else
+ currentStepSize = getDynamicSystemTimescale()/10;
+ if (userMinStepSize != -1)
+ currentStepSize = std::max(currentStepSize, userMinStepSize);
+ if (userMaxStepSize != -1)
+ currentStepSize = std::min(currentStepSize, userMaxStepSize);
+ lastStepSize = currentStepSize;
+ actualInitialStepSizeTaken = (hasErrorControl ? NaN : currentStepSize);
+ resetMethodStatistics();
+ }
+
+
+
+//==============================================================================
+// CREATE INTERPOLATED STATE
+//==============================================================================
+// Create an interpolated state at time t, which is between tPrev and tCurrent.
+// If we haven't yet delivered an interpolated state in this interval, we have
+// to initialize its discrete part from the advanced state.
+void AbstractIntegratorRep::createInterpolatedState(Real t) {
+ const System& system = getSystem();
+ const State& advanced = getAdvancedState();
+ State& interp = updInterpolatedState();
+ interp = advanced; // pick up discrete stuff.
+
+ // Hermite interpolation requires state derivatives so we must realize
+ // end-of-step derivatives if they haven't already been realized.
+ realizeStateDerivatives(advanced);
+
+ interpolateOrder3(getPreviousTime(), getPreviousY(), getPreviousYDot(),
+ advanced.getTime(), advanced.getY(), advanced.getYDot(),
+ t, interp.updY());
+ interp.updTime() = t;
+
+ if (userProjectInterpolatedStates == 0) {
+ system.realize(interp, Stage::Time);
+ system.prescribeQ(interp);
+ system.realize(interp, Stage::Position);
+ system.prescribeU(interp);
+ system.realize(interp, Stage::Velocity);
+ return;
+ }
+
+ // We may need to project onto constraint manifold. Allow project()
+ // to throw an exception if it fails since there is no way to recover here.
+ realizeAndProjectKinematicsWithThrow(interp, ProjectOptions::LocalOnly);
+}
+
+
+//==============================================================================
+// BACK UP ADVANCED STATE BY INTERPOLATION
+//==============================================================================
+// Interpolate the advanced state back to an earlier part of the interval,
+// forgetting about the rest of the interval. This is necessary, for
+// example after we have localized an event trigger to an interval tLow:tHigh
+// where tHigh < tAdvanced.
+void AbstractIntegratorRep::backUpAdvancedStateByInterpolation(Real t) {
+ const System& system = getSystem();
+ State& advanced = updAdvancedState();
+ Vector yinterp, dummy;
+
+ assert(getPreviousTime() <= t && t <= advanced.getTime());
+
+ // Hermite interpolation requires state derivatives so we must realize
+ // end-of-step derivatives if they haven't already been realized.
+ realizeStateDerivatives(advanced);
+ interpolateOrder3(getPreviousTime(), getPreviousY(), getPreviousYDot(),
+ advanced.getTime(), advanced.getY(), advanced.getYDot(),
+ t, yinterp);
+ advanced.updY() = yinterp;
+ advanced.updTime() = t;
+
+ // Ignore any user request not to project interpolated states here -- this
+ // is the actual advanced state which will be propagated through the
+ // rest of the trajectory so we can't allow it not to satisfy the
+ // constraints!
+ // But it is OK if it just *barely* satisfies the constraints so we
+ // won't get carried away if the user isn't being finicky about it.
+
+ // Project position constraints if warranted. Allow project()
+ // to throw an exception if it fails since there is no way to recover here.
+
+ realizeAndProjectKinematicsWithThrow(advanced, ProjectOptions::LocalOnly);
+}
+
+
+
+//==============================================================================
+// ATTEMPT DAE STEP
+//==============================================================================
+// This is the default implementation of this virtual method. Most integrators
+// can use this method and just override attemptODEStep(). Some will have to
+// override this, however.
+bool AbstractIntegratorRep::attemptDAEStep
+ (Real t1, Vector& yErrEst, int& errOrder, int& numIterations)
+{
+ const System& system = getSystem();
+ State& advanced = updAdvancedState();
+
+ bool ODEconverged = false;
+ try {
+ numIterations = 1; // so non-iterative ODEs can forget about this
+ ODEconverged = attemptODEStep(t1, yErrEst, errOrder, numIterations);
+ } catch (...) {return false;}
+
+ if (!ODEconverged)
+ return false;
+
+ // The ODE step did not throw an exception and says it converged,
+ // meaning its error estimate is worth a look.
+ int worstOne;
+ Real errNorm = calcErrorNorm(advanced, yErrEst, worstOne);
+
+ // If the estimated error is extremely bad, don't attempt the
+ // projection. If we're near the edge, though, the projection may
+ // clean up the error estimate enough to allow the step to be
+ // accepted. We'll define "near the edge" to mean that a half-step
+ // would have succeeded where this one failed. If the current error
+ // norm is eStep then a half step would have given us an error of
+ // eHalf = eStep/(2^p). We want to try the projection as long as
+ // eHalf <= accuracy, i.e., eStep <= 2^p * accuracy.
+ if (errNorm > std::pow(Real(2),errOrder)*getAccuracyInUse())
+ return true; // this step converged, but isn't worth projecting
+
+ // The ODE error estimate is good enough or at least worth trying
+ // to salvage via projection. If the constraint violation is
+ // extreme, however, we must not attempt to project it. The goal
+ // here is to ensure that the Newton iteration in projection is
+ // well behaved, running near its quadratic convergence regime.
+ // Thus we'll consider failure to reach sqrt(consTol) to be extreme.
+ // To guard against numerically large values of consTol, we'll
+ // always permit projection if we come within 2X of consTol. Examples:
+ // consTol projectionLimit
+ // 1e-12 1e-6
+ // 1e-4 1e-2
+ // 0.01 0.1
+ // 0.1 0.316
+ // 0.5 1
+ // 1 2
+ const Real projectionLimit =
+ std::max(2*getConstraintToleranceInUse(),
+ std::sqrt(getConstraintToleranceInUse()));
+
+ bool anyChanges;
+ if (!localProjectQAndQErrEstNoThrow(advanced, yErrEst, anyChanges,
+ projectionLimit))
+ return false; // convergence failure for this step
+
+ // q's satisfy the position constraint manifold. Now work on u's.
+
+ system.prescribeU(advanced);
+ system.realize(advanced, Stage::Velocity);
+
+ // Now try velocity constraint projection. Nothing will happen if
+ // velocity constraints are already satisfied unless user has set the
+ // ForceProjection option.
+
+ if (!localProjectUAndUErrEstNoThrow(advanced, yErrEst, anyChanges,
+ projectionLimit))
+ return false; // convergence failure for this step
+
+ // ODE step and projection (if any) were successful, although
+ // the accuracy requirement may not have been met.
+ return true;
+}
+
+
+
+//==============================================================================
+// STEP TO
+//==============================================================================
+Integrator::SuccessfulStepStatus
+AbstractIntegratorRep::stepTo(Real reportTime, Real scheduledEventTime) {
+ try {
+ assert(initialized);
+ assert(reportTime >= getState().getTime());
+ assert(scheduledEventTime >= getState().getTime());
+
+ // If this is the start of a continuous interval, return immediately so
+ // the current state will be seen as part of the trajectory.
+ if (startOfContinuousInterval) {
+ startOfContinuousInterval = false;
+ setStepCommunicationStatus(StepHasBeenReturnedNoEvent);
+ return Integrator::StartOfContinuousInterval;
+ }
+
+ // tMax is the time beyond which we cannot advance internally.
+ const Real finalTime = (userFinalTime == -1.0 ? Infinity : userFinalTime);
+ Real tMax = std::min(scheduledEventTime, finalTime);
+
+ // tReturn is the next scheduled time to return control to the user.
+ // Events may cause an earlier return.
+ const Real tReturn = std::min(reportTime, tMax);
+
+ if (userAllowInterpolation == 0)
+ tMax = tReturn;
+
+ // Count the number of internal steps taken during this call to stepTo().
+ // Normally this is limited by maxNumInternalSteps, meaning even if
+ // we don't reach reportTime we'll return after that many steps.
+ int internalStepsTaken = 0;
+
+ bool wasLastStep=false;
+ for(;;) { // MAIN STEPPING LOOP
+
+ // At this point the system's advancedState is the one to which
+ // we last advanced successfully. It has been realized through the
+ // Acceleration Stage. Behavior depends on the current state of
+ // the step communication state machine.
+
+ switch (getStepCommunicationStatus()) {
+ case FinalTimeHasBeenReturned:
+ // Copy advanced state to previous just so the error message
+ // in the catch() below will show the right time.
+ saveStateAndDerivsAsPrevious(getAdvancedState());
+ SimTK_ERRCHK2_ALWAYS(!"EndOfSimulation already returned",
+ "Integrator::stepTo()",
+ "Attempted stepTo(t=%g) but final time %g had already been "
+ "reached and returned."
+ "\nCheck for Integrator::EndOfSimulation status, or use the "
+ "Integrator::initialize() method to restart.",
+ reportTime, finalTime);
+ break;
+
+ case StepHasBeenReturnedNoEvent:
+ if (getAdvancedTime() >= finalTime) {
+ setUseInterpolatedState(false);
+ setStepCommunicationStatus(FinalTimeHasBeenReturned);
+ terminationReason = Integrator::ReachedFinalTime;
+ return Integrator::EndOfSimulation;
+ }
+ // need to advance
+ break;
+
+ case CompletedInternalStepWithEvent: {
+ // Time has been advanced, but the step ended by triggering an
+ // event at tAdvanced (==tHigh). We will return the last-good
+ // state at tLow<tHigh, but first we need to dispatch any pending
+ // reports that are supposed to occur before tLow in which case
+ // we interpolate back and return control before moving on.
+ if (reportTime <= getEventWindowLow()) {
+ // Report time reached: take a brief time-out to make an
+ // interpolated report. After that we'll come right back here
+ // with the user having advanced the reportTime (hopefully).
+ if (reportTime < getAdvancedTime()) {
+ createInterpolatedState(reportTime);
+ setUseInterpolatedState(true);
+ }
+ else
+ setUseInterpolatedState(false);
+ // No change to step communication status -- this doesn't
+ // count as reporting the current step since it is earlier
+ // than advancedTime.
+ return Integrator::ReachedReportTime;
+ }
+
+ // The advanced state is at tHigh, but needs event handling.
+
+ // Report the last "pre-event" state.
+ createInterpolatedState(getEventWindowLow());
+ setUseInterpolatedState(true);
+ setStepCommunicationStatus(StepHasBeenReturnedWithEvent);
+ return Integrator::ReachedEventTrigger;
+ }
+
+ case StepHasBeenReturnedWithEvent:
+ setUseInterpolatedState(false);
+ // Fall through to the next case.
+
+ case CompletedInternalStepNoEvent: {
+ // Time has been advanced. If there is a report due before
+ // tAdvanced, then we interpolate back and return control before
+ // moving on.
+ if (reportTime <= getAdvancedTime()) {
+ // Report time reached: take a brief time-out to make an
+ // interpolated report. After that we'll come right back here
+ // with the user having advanced the reportTime (hopefully).
+ if (reportTime < getAdvancedTime()) {
+ createInterpolatedState(reportTime);
+ setUseInterpolatedState(true);
+ // No change to step communication status -- this doesn't
+ // count as reporting the current step since it is earlier
+ // than advancedTime.
+ }
+ else {
+ // This counts as reporting the current step since we're
+ // exactly at advancedTime.
+ setUseInterpolatedState(false);
+ setStepCommunicationStatus(StepHasBeenReturnedNoEvent);
+ }
+
+ return Integrator::ReachedReportTime;
+ }
+
+ // Here we know advancedTime < reportTime. But there may be other
+ // reasons to return.
+
+ Integrator::SuccessfulStepStatus reportReason =
+ Integrator::InvalidSuccessfulStepStatus;
+ setUseInterpolatedState(false);
+
+ if (getAdvancedTime() >= scheduledEventTime) {
+ reportReason = Integrator::ReachedScheduledEvent;
+ } else if (userReturnEveryInternalStep == 1) {
+ reportReason = Integrator::TimeHasAdvanced;
+ } else if (getAdvancedTime() >= finalTime) {
+ reportReason = Integrator::ReachedReportTime;
+ } else if ( userInternalStepLimit > 0
+ && internalStepsTaken >= userInternalStepLimit) {
+ // Last-ditch excuse: too much work.
+ reportReason = Integrator::ReachedStepLimit; // too much work
+ }
+
+ if (reportReason != Integrator::InvalidSuccessfulStepStatus) {
+ setStepCommunicationStatus(StepHasBeenReturnedNoEvent);
+ return reportReason;
+ }
+
+ // no return required; need to advance time
+ break;
+ }
+
+ default:
+ assert(!"unrecognized stepCommunicationStatus");
+ }
+
+ // If a report or event is requested at the current time, return
+ // immediately.
+ if (getState().getTime() == reportTime)
+ return Integrator::ReachedReportTime;
+ if (getState().getTime() == scheduledEventTime)
+ return Integrator::ReachedScheduledEvent;
+
+ // At this point we know we are going to have to advance the state,
+ // meaning that the current state will become the previous one. We
+ // need to
+ // (1) ensure that all needed computations have been performed
+ // (2) update the auto-update discrete variables, whose update is not
+ // permitted to change any of the results calculated in (1)
+ // (3) save the continuous pieces of the current state for integrator
+ // restarts and interpolation.
+ // The continuous state variables will be updated below only when we
+ // make irreversible progress. Otherwise we'll use the saved ones to
+ // put things back the way we found them after any failures.
+
+ // Record the current time and state as the previous values.
+ saveTimeAndStateAsPrevious(getAdvancedState());
+ // Ensure that all derivatives and other derived quantities are known,
+ // including discrete state updates.
+ realizeStateDerivatives(getAdvancedState());
+ // Record derivative information and witness function values for step
+ // restarts.
+ saveStateDerivsAsPrevious(getAdvancedState());
+
+ // Derivatives at start of step have now been saved. We can update
+ // discrete state variables whose update is not permitted to cause a
+ // discontinuous change in the trajectory. These are implemented via
+ // auto-update state variables for which we just swap the cache
+ // entry (used to calculate most recent derivatives) with the
+ // corresponding state variable. Examples: record new min or max
+ // value; updated memory for delay measures.
+
+ // Swap the discrete state update cache entries with their state
+ // variables. Other than those cache entries, no other calculations
+ // are invalidated.
+ updAdvancedState().autoUpdateDiscreteVariables();
+
+ //---------------- TAKE ONE STEP --------------------
+ // Now take a step and see whether an event occurred.
+ bool eventOccurred = takeOneStep(tMax, reportTime);
+ //---------------------------------------------------
+
+ ++internalStepsTaken;
+ ++statsStepsTaken;
+ setStepCommunicationStatus(eventOccurred
+ ? CompletedInternalStepWithEvent
+ : CompletedInternalStepNoEvent);
+ } // END OF MAIN STEP LOOP
+
+ //NOTREACHED
+ assert(!"can't get here!!");
+
+ } catch (const std::exception& e) {
+ setAdvancedState(getPreviousTime(),getPreviousY()); // restore
+ SimTK_THROW2(Integrator::StepFailed, getAdvancedState().getTime(), e.what());
+ } catch (...) {
+ setAdvancedState(getPreviousTime(),getPreviousY()); // restore
+ SimTK_THROW2(Integrator::StepFailed, getAdvancedState().getTime(), "UNKNOWN EXCEPTION");
+ }
+
+ // can't happen
+ return Integrator::InvalidSuccessfulStepStatus;
+}
+
+
+
+//==============================================================================
+// ADJUST STEP SIZE
+//==============================================================================
+// This is the default implementation for the virtual method adjustStepSize().
+//
+// Adjust the step size for next time based on the accuracy achieved this
+// time. Returns true ("success") if the new step is at least as big as the
+// current one.
+bool AbstractIntegratorRep::adjustStepSize
+ (Real err, int errOrder, bool hWasArtificiallyLimited)
+{
+ const Real Safety = Real(0.9), MinShrink = Real(0.1), MaxGrow = 5;
+ const Real HysteresisLow = Real(0.9), HysteresisHigh = Real(1.2);
+
+ Real newStepSize;
+
+ // First, make a first guess at the next step size to use based on
+ // the supplied error norm. Watch out for NaN!
+ if (!isFinite(err)) // e.g., integrand returned NaN
+ newStepSize = MinShrink * currentStepSize;
+ else if (err==0) // a "perfect" step; can happen if no dofs for example
+ newStepSize = MaxGrow * currentStepSize;
+ else // choose best step for skating just below the desired accuracy
+ newStepSize = Safety * currentStepSize
+ * std::pow(getAccuracyInUse()/err, 1/Real(errOrder));
+
+ // If the new step is bigger than the old, don't make the change if the
+ // old one was small for some unimportant reason (like reached a reporting
+ // interval). Also, don't grow the step size if the change would be very
+ // small; better to keep the step size stable in that case (maybe just
+ // for aesthetic reasons).
+ if (newStepSize > currentStepSize) {
+ if ( hWasArtificiallyLimited
+ || newStepSize < HysteresisHigh*currentStepSize)
+ newStepSize = currentStepSize;
+ }
+
+ // If we're supposed to shrink the step size but the one we have actually
+ // achieved the desired accuracy last time, we won't change the step now.
+ // Otherwise, if we are going to shrink the step, let's not be shy -- we'll
+ // shrink it by at least a factor of HysteresisLow.
+ if (newStepSize < currentStepSize) {
+ if (err <= getAccuracyInUse())
+ newStepSize = currentStepSize; // not this time
+ else newStepSize = std::min(newStepSize, HysteresisLow*currentStepSize);
+ }
+
+ // Keep the size change within the allowable bounds.
+ newStepSize = std::min(newStepSize, MaxGrow*currentStepSize);
+ newStepSize = std::max(newStepSize, MinShrink*currentStepSize);
+
+ // Apply user-requested limits on min and max step size.
+ if (userMinStepSize != -1)
+ newStepSize = std::max(newStepSize, userMinStepSize);
+ if (userMaxStepSize != -1)
+ newStepSize = std::min(newStepSize, userMaxStepSize);
+
+ //TODO: this is an odd definition of success. It only works because we're
+ //refusing the shrink the step size above if accuracy was achieved.
+ bool success = (newStepSize >= currentStepSize);
+ currentStepSize = newStepSize;
+ return success;
+}
+
+
+
+//==============================================================================
+// TAKE ONE STEP
+//==============================================================================
+// Just prior to this call, advanced.t, advanced.y, advanced.ydot were copied
+// into the "previous" members so that they can be used for restarts.
+// Advanced has been realized through Acceleration stage.
+// This is a private method.
+bool AbstractIntegratorRep::takeOneStep(Real tMax, Real tReport)
+{
+ Real t1;
+ State& advanced = updAdvancedState();
+
+ // These are the pre-recorded starting values. State "advanced" will
+ // get mangled during trial steps but these remain unchanged.
+ const Real t0 = getPreviousTime();
+ const Vector& y0 = getPreviousY();
+ const Vector& ydot0 = getPreviousYDot();
+ const Vector& uScale = getPreviousUScale();
+ const Vector& zScale = getPreviousZScale();
+
+ const int nq = advanced.getNQ(),
+ nu = advanced.getNU(),
+ nz = advanced.getNZ(),
+ ny = nq+nu+nz;
+
+ Vector yErrEst(ny);
+ bool stepSucceeded = false;
+ do {
+ // If we lose more than a small fraction of the step size we wanted
+ // to take due to a need to stop at tMax, make a note of that so the
+ // step size adjuster won't try to grow from the current step.
+ bool hWasArtificiallyLimited = false;
+ if (tMax < t0 + 0.95*currentStepSize) {
+ hWasArtificiallyLimited = true;
+ t1 = tMax; // tMax much smaller than current step size
+ } else if (tMax > t0 + 1.001*currentStepSize)
+ t1 = t0 + currentStepSize; // tMax too big to reach in one step
+ else
+ t1 = tMax; // tMax is roughly t0+currentStepSize; try for it
+
+ SimTK_ERRCHK1_ALWAYS(t1 > t0, "AbstractIntegrator::takeOneStep()",
+ "Unable to advance time past %g.", t0);
+
+ int errOrder;
+ int numIterations=1; // non-iterative methods can ignore this
+ //--------------------------------------------------------------------
+ bool converged = attemptDAEStep(t1, yErrEst, errOrder, numIterations);
+ //--------------------------------------------------------------------
+ Real errNorm=NaN; int worstY=-1;
+ if (converged) {
+ errNorm = (hasErrorControl ? calcErrorNorm(advanced,yErrEst,worstY)
+ : Real(0));
+ statsConvergentIterations += numIterations;
+ } else {
+ errNorm = Infinity; // step didn't converge so error is *very* bad!
+ ++statsConvergenceTestFailures;
+ statsDivergentIterations += numIterations;
+ }
+
+ // TODO: this isn't right for a non-error controlled integrator that
+ // didn't converge, if there is such a thing.
+ stepSucceeded = (hasErrorControl ? adjustStepSize(errNorm, errOrder,
+ hWasArtificiallyLimited)
+ : true);
+ if (!stepSucceeded)
+ statsErrorTestFailures++;
+ else { // step succeeded
+ lastStepSize = t1-t0;
+ if (isNaN(actualInitialStepSizeTaken))
+ actualInitialStepSizeTaken = lastStepSize;
+ }
+ } while (!stepSucceeded);
+
+ // The step succeeded. Check for event triggers. If there aren't any, we're
+ // done with the step. Otherwise our goal will be to find the "exact" time
+ // at which the first event occurs within the current interval. Then we
+ // will advance the system to that time only and forget about the rest of
+ // the interval.
+ //
+ // Here's how this is done:
+ // First, determine which events trigger across the *whole* interval. At
+ // least one of those events, and no other events, are eventually going to
+ // be reported as having triggered after localization, and the trigger type
+ // will be the original one. That is, we don't care *what* we see during
+ // localization (which could be viewed as more accurate); we have already
+ // decided which events are candidates. Any trigger that came and went
+ // during the current interval has missed the boat permanently; that's a
+ // user error (bad design of event trigger or maybe excessively loose
+ // accuracy). Second, with the list of candidates and their transitions in
+ // hand we want to find the time at which the first one(s) trigger, to
+ // within a localization window (tlo,thi]; that is, we do not see the
+ // event triggering at tlo but we do see it triggering at thi, and
+ // (thi-tlo)<=w for some time window w. We then chop back the advancedState
+ // by interpolation to thi and return to the caller who will deal with
+ // interpolations needed for reporting, then a final interpolation at tlo.
+ // Note that we should never actually return the state at thi as part of
+ // the trajectory, since it is invalid until the event handler is called
+ // to fix it.
+
+ const Vector& e0 = getPreviousEventTriggers();
+ if (e0.size() == 0) {
+ // There are no witness functions to worry about in this system.
+ return false;
+ }
+
+ // TODO: this is indiscriminately evaluating expensive accelerations
+ // that are only needed if there are acceleration-level witness functions.
+ realizeStateDerivatives(getAdvancedState());
+ const Vector& e1 = getAdvancedState().getEventTriggers();
+ assert(e0.size() == e1.size() &&
+ e0.size() == getAdvancedState().getNEventTriggers());
+
+ const Real MinWindow =
+ SignificantReal * std::max(Real(1), getAdvancedTime());
+ Array_<SystemEventTriggerIndex> eventCandidates, newEventCandidates;
+ Array_<Event::Trigger>
+ eventCandidateTransitions, newEventCandidateTransitions;
+ Array_<Real> eventTimeEstimates, newEventTimeEstimates;
+
+ Real earliestTimeEst, narrowestWindow;
+
+ findEventCandidates(e0.size(), 0, 0, t0, e0, t1, e1, 1., MinWindow,
+ eventCandidates, eventTimeEstimates,
+ eventCandidateTransitions,
+ earliestTimeEst, narrowestWindow);
+
+ if (eventCandidates.empty()) {
+ // This is the normal return.
+ return false;
+ }
+
+ Real tLow = t0;
+ Real tHigh = t1;
+
+ if ( (tHigh-tLow) <= narrowestWindow
+ && !(tLow < tReport && tReport < tHigh))
+ {
+ Array_<EventId> ids;
+ findEventIds(eventCandidates, ids);
+ setTriggeredEvents(tLow, tHigh, ids, eventTimeEstimates,
+ eventCandidateTransitions);
+ // localized already; advanced state is right (tHigh==tAdvanced)
+ return true;
+ }
+
+ // We now have a list of candidates in the (tLow,tHigh] interval, but that
+ // interval is too wide. We have to narrow the interval until all the
+ // triggered events in the interval are happy with the interval width.
+ // From above we have earliestTimeEst which is the time at which we
+ // think the first event is triggering.
+
+ Vector eLow = e0, eHigh = e1;
+ Real bias = 1; // neutral
+
+ // There is an event in (tLow,tHigh], with the eariest occurrence
+ // estimated at tMid=earliestTimeEst, tLow<tMid<tHigh.
+ // Decide whether the earliest occurence is actually in the
+ // (tLow,tMid] interval or (tMid,tHigh].
+
+ // Remember which side of the interval the root estimate was in over
+ // the last two iterations. -1 => (tLow,tMid], 1 => (tMid,tHigh], 0 => not
+ // valid yet.
+ int sideTwoItersAgo=0, sidePrevIter=0;
+ do {
+ if (sideTwoItersAgo != 0 && sidePrevIter != 0) {
+ if (sideTwoItersAgo != sidePrevIter)
+ bias = 1; // this is good; alternating intervals
+ else
+ bias = sidePrevIter < 0 ? bias/2 : bias*2;
+ }
+
+ const Real tMid = (tLow < tReport && tReport < tHigh)
+ ? tReport : earliestTimeEst;
+
+ createInterpolatedState(tMid);
+
+ // Failure to evaluate at the interpolated state is a disaster of some
+ // kind, not something we expect to be able to recover from, so this
+ // will throw an exception if it fails.
+ realizeStateDerivatives(getInterpolatedState());
+
+ const Vector& eMid = getInterpolatedState().getEventTriggers();
+
+ // TODO: should search in the wider interval first
+
+ // First guess: it is in (tLow,tMid].
+ findEventCandidates(e0.size(), &eventCandidates,
+ &eventCandidateTransitions,
+ tLow, eLow, tMid, eMid, bias, MinWindow,
+ newEventCandidates, newEventTimeEstimates,
+ newEventCandidateTransitions,
+ earliestTimeEst, narrowestWindow);
+
+ if (!newEventCandidates.empty()) {
+ sideTwoItersAgo = sidePrevIter;
+ sidePrevIter = -1;
+
+ // We guessed right -- tMid is our new tHigh, earliestTimeEst is
+ // our new tMid, narrowestWindow is our localization requirement.
+ tHigh = tMid; eHigh = eMid;
+ eventCandidates = newEventCandidates;
+ eventTimeEstimates = newEventTimeEstimates;
+ // these will still be the original transitions, but only the ones
+ // which are still candidates are retained
+ eventCandidateTransitions = newEventCandidateTransitions;
+ continue;
+ }
+
+ // Nope. It must be in the upper part of the interval (tMid,tHigh].
+ findEventCandidates(e0.size(), &eventCandidates,
+ &eventCandidateTransitions,
+ tMid, eMid, tHigh, eHigh, bias, MinWindow,
+ newEventCandidates, newEventTimeEstimates,
+ newEventCandidateTransitions,
+ earliestTimeEst, narrowestWindow);
+
+ // TODO: I think this can happen if we land exactly on a zero in eMid.
+ assert(!newEventCandidates.empty());
+
+ sideTwoItersAgo = sidePrevIter;
+ sidePrevIter = 1;
+
+ tLow = tMid; eLow = eMid;
+ eventCandidates = newEventCandidates;
+ eventTimeEstimates = newEventTimeEstimates;
+ // These will still be the original transitions, but only the ones
+ // which are still candidates are retained.
+ eventCandidateTransitions = newEventCandidateTransitions;
+
+ } while ((tHigh-tLow) > narrowestWindow);
+
+ Array_<EventId> ids;
+ findEventIds(eventCandidates, ids);
+ setTriggeredEvents(tLow, tHigh, ids, eventTimeEstimates,
+ eventCandidateTransitions);
+
+ // We have to throw away all of the advancedState that occurred after
+ // tHigh, by interpolating back to tHigh.
+ // TODO: I think a smarter algorithm could avoid this last realize() half
+ // the time since we may have just interpolated and realized this end
+ // of the interval above.
+ if (tHigh < getAdvancedTime()) {
+ backUpAdvancedStateByInterpolation(tHigh);
+ // Failure to realize here should never happen since we already
+ // succeeded all the way to advancedTime. So if this fails it
+ // will throw an exception that will kill the simulation.
+ realizeStateDerivatives(getAdvancedState());
+ }
+
+ return true;
+}
+
+
+
+//==============================================================================
+// STATUS & MISC
+//==============================================================================
+
+Real AbstractIntegratorRep::getActualInitialStepSizeTaken() const {
+ return actualInitialStepSizeTaken;
+}
+
+Real AbstractIntegratorRep::getPreviousStepSizeTaken() const {
+ return lastStepSize;
+}
+
+Real AbstractIntegratorRep::getPredictedNextStepSize() const {
+ return currentStepSize;
+}
+
+int AbstractIntegratorRep::getNumStepsAttempted() const {
+ assert(initialized);
+ return statsStepsAttempted;
+}
+
+int AbstractIntegratorRep::getNumStepsTaken() const {
+ assert(initialized);
+ return statsStepsTaken;
+}
+
+int AbstractIntegratorRep::getNumErrorTestFailures() const {
+ return statsErrorTestFailures;
+}
+int AbstractIntegratorRep::getNumConvergenceTestFailures() const {
+ return statsConvergenceTestFailures;
+}
+int AbstractIntegratorRep::getNumConvergentIterations() const {
+ return statsConvergentIterations;
+}
+int AbstractIntegratorRep::getNumDivergentIterations() const {
+ return statsDivergentIterations;
+}
+int AbstractIntegratorRep::getNumIterations() const {
+ return statsConvergentIterations + statsDivergentIterations;
+}
+
+void AbstractIntegratorRep::resetMethodStatistics() {
+ statsStepsTaken = 0;
+ statsStepsAttempted = 0;
+ statsErrorTestFailures = 0;
+ statsConvergenceTestFailures = 0;
+ statsConvergentIterations = 0;
+ statsDivergentIterations = 0;
+}
+
+const char* AbstractIntegratorRep::getMethodName() const {
+ return methodName.c_str();
+}
+
+int AbstractIntegratorRep::getMethodMinOrder() const {
+ return minOrder;
+}
+
+int AbstractIntegratorRep::getMethodMaxOrder() const {
+ return maxOrder;
+}
+
+bool AbstractIntegratorRep::methodHasErrorControl() const {
+ return hasErrorControl;
+}
diff --git a/SimTKmath/Integrators/src/AbstractIntegratorRep.h b/SimTKmath/Integrators/src/AbstractIntegratorRep.h
new file mode 100644
index 0000000..1e04141
--- /dev/null
+++ b/SimTKmath/Integrators/src/AbstractIntegratorRep.h
@@ -0,0 +1,171 @@
+#ifndef SimTK_SIMMATH_ABSTRACT_INTEGRATOR_REP_H_
+#define SimTK_SIMMATH_ABSTRACT_INTEGRATOR_REP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/Integrator.h"
+
+#include "IntegratorRep.h"
+
+namespace SimTK {
+
+/**
+ * This class implements most of the generic functionality needed for an
+ * Integrator, leaving only the actual integration method to be implemented
+ * by the subclass. This is the parent class of several different integrators.
+ *
+ * There are default implementations for everything but the ODE formula.
+ */
+
+class AbstractIntegratorRep : public IntegratorRep {
+public:
+ AbstractIntegratorRep(Integrator* handle, const System& sys,
+ int minOrder, int maxOrder,
+ const std::string& methodName,
+ bool hasErrorControl);
+
+ void methodInitialize(const State&);
+
+ Integrator::SuccessfulStepStatus
+ stepTo(Real reportTime, Real scheduledEventTime);
+
+ Real getActualInitialStepSizeTaken() const;
+ Real getPreviousStepSizeTaken() const;
+ Real getPredictedNextStepSize() const;
+ int getNumStepsAttempted() const;
+ int getNumStepsTaken() const;
+ int getNumErrorTestFailures() const;
+ int getNumConvergenceTestFailures() const;
+ int getNumConvergentIterations() const;
+ int getNumDivergentIterations() const;
+ int getNumIterations() const;
+ void resetMethodStatistics();
+ const char* getMethodName() const;
+ int getMethodMinOrder() const;
+ int getMethodMaxOrder() const;
+ bool methodHasErrorControl() const;
+
+protected:
+ /*
+ * Given initial values for time, all the continuous variables y=(q,u,z) and
+ * their derivatives (not necessarily what's in advancedState currently),
+ * take a trial step of size h=(t1-t0), optimistically storing the result
+ * in advancedState. The starting values have been saved in the parent
+ * IntegratorRep class and are accessible with getPreviousTime(),
+ * getPreviousY() and getPreviousYDot().
+ *
+ * Also estimate the absolute error in each element of y,
+ * and store them in yErrEst. Returns true if the step converged (always
+ * true for non-iterative methods), false otherwise. The number of internal
+ * iterations just for this step is return in numIterations, which should
+ * always be 1 for non-iterative methods.
+ *
+ * This is a DAE step, meaning that coordinate projections should be done
+ * (including their effect on the error estimate) prior to returning.
+ * The default implementation calls the "raw" ODE integrator and then
+ * handles the necessary projections; if that's OK for your method then
+ * you only have to implement attemptODEStep(). Otherwise, you should
+ * override the attemptDAEStep() and deal carefully with the DAE-specific
+ * issues yourself.
+ *
+ * The return value is true if the step converged; that tells the caller
+ * to look at the error estimate. If the step doesn't converge, the error
+ * estimate is meaningless and the step will be rejected.
+ */
+ virtual bool attemptDAEStep
+ (Real t1, Vector& yErrEst, int& errOrder, int& numIterations);
+
+ // Any integrator that doesn't override the above attemptDAEStep() method
+ // must override at least the ODE part here. The method must take an ODE
+ // step modifying y in advancedState, return false for failure to converge,
+ // or return true and an estimate of the absolute error in each element of
+ // the advancedState y variables. The integrator should not attempt to
+ // evaluate derivatives at the final y value because we want to project
+ // onto the position and velocity constraint manifolds first so the
+ // derivative calculation would have been wasted.
+ virtual bool attemptODEStep
+ (Real t1, Vector& yErrEst, int& errOrder, int& numIterations)
+ {
+ SimTK_ERRCHK_ALWAYS(!"Unimplemented virtual function",
+ "AbstractIntegratorRep::attemptODEStep()",
+ "This virtual function was called but wasn't defined. Every"
+ " concrete integrator must override attemptODEStep() or override"
+ " attemptDAEStep() which calls it.");
+ return false;
+ }
+
+
+ /**
+ * Evaluate the error that occurred in the step we just attempted, and
+ * select a new step size accordingly. The default implementation should
+ * work well for most integrators.
+ *
+ * @param err
+ * The error estimate from the step that was just attempted.
+ * @param errOrder
+ * The order of the error estimator so we know what the effect of a
+ * step size change would be on the error we see next time.
+ * @param hWasArtificiallyLimited
+ * Tells whether the step size was artificially reduced due to a
+ * scheduled event time. If this is true, we will never attempt to
+ * increase the step size.
+ * @return
+ * True if the step should be accepted, false if it should be rejected
+ * and retried with a smaller step size.
+ */
+ virtual bool adjustStepSize(Real err, int errOrder,
+ bool hWasArtificiallyLimited);
+ /**
+ * Create an interpolated state at time t, which is between the previous
+ * and advanced times. The default implementation uses third order
+ * Hermite spline interpolation.
+ */
+ virtual void createInterpolatedState(Real t);
+ /**
+ * Interpolate the advanced state back to an earlier part of the interval,
+ * forgetting about the rest of the interval. This is necessary, for
+ * example, after we have localized an event trigger to an interval
+ * tLow:tHigh where tHigh < tAdvanced. The default implementation uses
+ * third order Hermite spline interpolation.
+ */
+ virtual void backUpAdvancedStateByInterpolation(Real t);
+ int statsStepsTaken, statsStepsAttempted, statsErrorTestFailures, statsConvergenceTestFailures;
+
+ // Iterative methods should count iterations and then classify them as
+ // iterations that led to successful convergence and those that didn't.
+ int statsConvergentIterations, statsDivergentIterations;
+private:
+ bool takeOneStep(Real tMax, Real tReport);
+ bool initialized, hasErrorControl;
+ Real currentStepSize, lastStepSize, actualInitialStepSizeTaken;
+ int minOrder, maxOrder;
+ std::string methodName;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_ABSTRACT_INTEGRATOR_REP_H_
diff --git a/SimTKmath/Integrators/src/CPodes/CMakeLists.txt b/SimTKmath/Integrators/src/CPodes/CMakeLists.txt
new file mode 100644
index 0000000..3088169
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/CMakeLists.txt
@@ -0,0 +1,25 @@
+# visit the sundials directory to gather up source and tests
+add_subdirectory(sundials)
+
+# add in local source and headers here
+FILE(GLOB src_files ./*.cpp)
+FILE(GLOB incl_files ./*.h)
+
+# append to the local scope copy, and then copy up to parent scope
+list(APPEND SOURCE_FILES ${src_files})
+set(SOURCE_FILES ${SOURCE_FILES} PARENT_SCOPE)
+
+list(APPEND SOURCE_INCLUDE_FILES ${incl_files})
+set(SOURCE_INCLUDE_FILES ${SOURCE_INCLUDE_FILES} PARENT_SCOPE)
+
+list(APPEND SOURCE_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/sundials/include)
+set(SOURCE_INCLUDE_DIRS ${SOURCE_INCLUDE_DIRS} PARENT_SCOPE)
+
+# just pass these up
+set(SOURCE_GROUPS ${SOURCE_GROUPS} PARENT_SCOPE)
+set(SOURCE_GROUP_FILES ${SOURCE_GROUP_FILES} PARENT_SCOPE)
+
+# define tests that depend on sources here
+if(BUILD_TESTING)
+ add_subdirectory(tests)
+endif(BUILD_TESTING)
diff --git a/SimTKmath/Integrators/src/CPodes/SimTKcpodes.cpp b/SimTKmath/Integrators/src/CPodes/SimTKcpodes.cpp
new file mode 100644
index 0000000..64cc27c
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/SimTKcpodes.cpp
@@ -0,0 +1,762 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This is the private (library side) implementation of the CPodes
+ * handle class and its actual representation CPodesRep.
+ */
+
+#include "simmath/internal/SimTKcpodes.h"
+#include "nvector_SimTK.h"
+#include "cpodes/cpodes.h"
+#include "cpodes/cpodes_dense.h"
+#include "cpodes/cpodes_lapack_exports.h"
+
+#include <limits>
+
+namespace SimTK {
+
+class CPodesRep {
+public:
+ CPodesRep()
+ : useImplicitODEFunction(false), cpode_mem(0), sysp(0), myHandle(0)
+ {
+ zeroFunctionPointers();
+ }
+
+ CPodesRep(int ode_type, int lmm_type, int nls_type)
+ : useImplicitODEFunction(ode_type == CP_IMPL), cpode_mem(0), sysp(0), myHandle(0)
+ {
+ cpode_mem = CPodeCreate(ode_type, lmm_type, nls_type);
+ }
+
+ ~CPodesRep() {
+ if (cpode_mem)
+ CPodeFree(&cpode_mem);
+ }
+
+ const CPodesSystem& getCPodesSystem() const {return *sysp;}
+
+ // Client-side function pointers
+ CPodes::ExplicitODEFunc explicitODEFunc;
+ CPodes::ImplicitODEFunc implicitODEFunc;
+ CPodes::ConstraintFunc constraintFunc;
+ CPodes::ProjectFunc projectFunc;
+ CPodes::QuadratureFunc quadratureFunc;
+ CPodes::RootFunc rootFunc;
+ CPodes::WeightFunc weightFunc;
+ CPodes::ErrorHandlerFunc errorHandlerFunc;
+
+ void zeroFunctionPointers() {
+ explicitODEFunc = 0;
+ implicitODEFunc = 0;
+ constraintFunc = 0;
+ projectFunc = 0;
+ quadratureFunc = 0;
+ rootFunc = 0;
+ weightFunc = 0;
+ errorHandlerFunc = 0;
+ }
+
+ void setMyHandle(CPodes& cp) {myHandle = &cp;}
+ const CPodes& getMyHandle() const {assert(myHandle); return *myHandle;}
+ void clearMyHandle() {myHandle=0;}
+private:
+ bool useImplicitODEFunction;
+ void* cpode_mem;
+ const CPodesSystem* sysp;
+
+ friend class CPodes;
+ CPodes* myHandle; // The owner handle of this Rep.
+
+ void* getSystemForSundials() const {
+ return (void*)const_cast<CPodesSystem*>(sysp);
+ }
+};
+
+
+//////////////////////
+// CPODES FUNCTIONS //
+//////////////////////
+
+// These functions satisfy the C signatures required by Sundials CPODES. They provide
+// translation between SimTK data types and those understood by Sundials and CPODES.
+// These in turn call (indirectly) user-supplied C++ code that was used in
+// defining concrete classes derived from the user-visible CPodesSystem abstract
+// class. Note that we *never* access the client-side CPodesSystem virtual
+// function table from library code. CPodesRep contains an alternate table which
+// will be correct even if the client side is out of date with respect to the
+// library.
+
+static int explicitODEWrapper(realtype t, N_Vector nv_y, N_Vector nv_fout, void* f_data) {
+ const Vector& y = N_Vector_SimTK::getVector(nv_y);
+ Vector& fout = N_Vector_SimTK::updVector(nv_fout);
+ const CPodesRep& rep = *reinterpret_cast<const CPodesRep*>(f_data);
+ return rep.explicitODEFunc(rep.getCPodesSystem(), t, y, fout);
+}
+
+static int implicitODEWrapper(realtype t, N_Vector nv_y, N_Vector nv_yp, N_Vector nv_fout, void* f_data) {
+ const Vector& y = N_Vector_SimTK::getVector(nv_y);
+ const Vector& yp = N_Vector_SimTK::getVector(nv_yp);
+ Vector& fout = N_Vector_SimTK::updVector(nv_fout);
+ const CPodesRep& rep = *reinterpret_cast<const CPodesRep*>(f_data);
+ return rep.implicitODEFunc(rep.getCPodesSystem(), t, y, yp, fout);
+}
+
+static int constraintWrapper(realtype t, N_Vector nv_y, N_Vector nv_cout, void* c_data) {
+ const Vector& y = N_Vector_SimTK::getVector(nv_y);
+ Vector& cout = N_Vector_SimTK::updVector(nv_cout);
+ const CPodesRep& rep = *reinterpret_cast<const CPodesRep*>(c_data);
+ return rep.constraintFunc(rep.getCPodesSystem(), t, y, cout);
+}
+
+static int projectWrapper(realtype t, N_Vector nv_ycur,
+ N_Vector nv_corr,
+ realtype epsProj, N_Vector nv_err, void *pdata)
+{
+ const Vector& ycur = N_Vector_SimTK::getVector(nv_ycur);
+ Vector& corr = N_Vector_SimTK::updVector(nv_corr);
+ Vector& err = N_Vector_SimTK::updVector(nv_err);
+ const CPodesRep& rep = *reinterpret_cast<const CPodesRep*>(pdata);
+ return rep.projectFunc(rep.getCPodesSystem(), t, ycur, corr, epsProj, err);
+}
+
+static int quadratureWrapper(realtype t, N_Vector nv_y,
+ N_Vector nv_qout, void *q_data)
+{
+ const Vector& y = N_Vector_SimTK::getVector(nv_y);
+ Vector& qout = N_Vector_SimTK::updVector(nv_qout);
+ const CPodesRep& rep = *reinterpret_cast<const CPodesRep*>(q_data);
+ return rep.quadratureFunc(rep.getCPodesSystem(), t, y, qout);
+}
+
+static int rootWrapper(realtype t, N_Vector nv_y, N_Vector nv_yp,
+ realtype *goutp, void *g_data)
+{
+ const Vector& y = N_Vector_SimTK::getVector(nv_y);
+ const Vector& yp = N_Vector_SimTK::getVector(nv_yp);
+
+ const CPodesRep& rep = *reinterpret_cast<const CPodesRep*>(g_data);
+
+ // Create a temporary Vector to hold the result. This is awkward
+ // but we don't know here how much space to allocate since for
+ // some odd reason the output array here isn't an N_Vector.
+ Vector gout_tmp; // the root routine will have to resize this
+ const int flag = rep.rootFunc(rep.getCPodesSystem(), t, y, yp, gout_tmp);
+ for (int i=0; i<gout_tmp.size(); ++i)
+ goutp[i] = gout_tmp[i];
+ return flag;
+}
+
+static int weightWrapper(N_Vector nv_y, N_Vector nv_ewt, void *e_data) {
+ const Vector& y = N_Vector_SimTK::getVector(nv_y);
+ Vector& ewt = N_Vector_SimTK::updVector(nv_ewt);
+ const CPodesRep& rep = *reinterpret_cast<const CPodesRep*>(e_data);
+ return rep.weightFunc(rep.getCPodesSystem(), y, ewt);
+}
+
+static void errorHandlerWrapper(int error_code,
+ const char *module, const char *function,
+ char *msg, void *eh_data)
+{
+ const CPodesRep& rep = *reinterpret_cast<const CPodesRep*>(eh_data);
+ return rep.errorHandlerFunc(rep.getCPodesSystem(), error_code,module,function,msg);
+}
+
+////////////////////////////////////////
+// CLASS SimTK::CPodes IMPLEMENTATION //
+////////////////////////////////////////
+
+// These static functions map SimTK::CPodes enumerated types to C CPodes #defines.
+static int mapODEType(CPodes::ODEType ode) {
+ switch(ode) {
+ case CPodes::ExplicitODE: return CP_EXPL;
+ case CPodes::ImplicitODE: return CP_IMPL;
+ default: return std::numeric_limits<int>::min();
+ }
+}
+
+static int mapLinearMultistepMethod(CPodes::LinearMultistepMethod lmm) {
+ switch(lmm) {
+ case CPodes::BDF: return CP_BDF;
+ case CPodes::Adams: return CP_ADAMS;
+ default: return std::numeric_limits<int>::min();
+ }
+}
+
+static int mapNonlinearSystemIterationType(CPodes::NonlinearSystemIterationType nls) {
+ switch(nls) {
+ case CPodes::Functional: return CP_FUNCTIONAL;
+ case CPodes::Newton: return CP_NEWTON;
+ default: return std::numeric_limits<int>::min();
+ }
+}
+
+static int mapToleranceType(CPodes::ToleranceType type) {
+ switch(type) {
+ case CPodes::ScalarScalar: return CP_SS;
+ case CPodes::ScalarVector: return CP_SV;
+ case CPodes::WeightFunction: return CP_WF;
+ default: return std::numeric_limits<int>::min();
+ }
+}
+
+static int mapProjectionNorm(CPodes::ProjectionNorm norm) {
+ switch(norm) {
+ case CPodes::L2Norm: return CP_PROJ_L2NORM;
+ case CPodes::ErrorNorm: return CP_PROJ_ERRNORM;
+ default: return std::numeric_limits<int>::min();
+ }
+}
+
+static int mapConstraintLinearity(CPodes::ConstraintLinearity lin) {
+ switch(lin) {
+ case CPodes::Linear: return CP_CNSTR_LIN;
+ case CPodes::Nonlinear: return CP_CNSTR_NONLIN;
+ default: return std::numeric_limits<int>::min();
+ }
+}
+
+
+static int mapProjectionFactorizationType(CPodes::ProjectionFactorizationType ft) {
+ switch(ft) {
+ case CPodes::ProjectWithLU: return CPDIRECT_LU;
+ case CPodes::ProjectWithQR: return CPDIRECT_QR;
+ case CPodes::ProjectWithSchurComplement: return CPDIRECT_SC;
+ case CPodes::ProjectWithQRPivot: return CPDIRECT_QRP;
+ default: return std::numeric_limits<int>::min();
+ }
+}
+
+static int mapStepMode(CPodes::StepMode mode) {
+ switch(mode) {
+ case CPodes::Normal: return CP_NORMAL;
+ case CPodes::OneStep: return CP_ONE_STEP;
+ case CPodes::NormalTstop: return CP_NORMAL_TSTOP;
+ case CPodes::OneStepTstop: return CP_ONE_STEP_TSTOP;
+ default: return std::numeric_limits<int>::min();
+ }
+}
+
+// The actual constructor is defined on the client side but
+// calls this library-side routine to do most of the work. (Registration of
+// user functions has to be done on the client side.)
+void CPodes::librarySideCPodesConstructor
+ (ODEType ode, LinearMultistepMethod lmm, NonlinearSystemIterationType nls)
+{
+ if (ode == UnspecifiedODEType) ode = ExplicitODE;
+ if (lmm == UnspecifiedLinearMultistepMethod) lmm = BDF;
+ if (nls == UnspecifiedNonlinearSystemIterationType)
+ nls = (ode==ExplicitODE ? Functional : Newton);
+
+ rep = new CPodesRep(mapODEType(ode),
+ mapLinearMultistepMethod(lmm),
+ mapNonlinearSystemIterationType(nls));
+ rep->setMyHandle(*this);
+}
+
+CPodes::~CPodes() {
+ if (rep && rep->myHandle==this) // this is the owner handle
+ delete rep;
+ rep = 0;
+}
+
+int CPodes::init(CPodesSystem& sys,
+ Real t0, const Vector& y0, const Vector& yp0,
+ ToleranceType tol_type, Real reltol, void* abstol)
+{
+ updRep().sysp = &sys;
+
+ N_Vector_SimTK nv_y0(y0); // references, not copies
+ N_Vector_SimTK nv_yp0(yp0);
+ return CPodeInit(updRep().cpode_mem,
+ getRep().useImplicitODEFunction
+ ? (void*)implicitODEWrapper
+ : (void*)explicitODEWrapper,
+ (void*)rep,
+ t0, &nv_y0, &nv_yp0,
+ mapToleranceType(tol_type),reltol,abstol);
+}
+
+int CPodes::reInit(CPodesSystem& sys,
+ Real t0, const Vector& y0, const Vector& yp0,
+ ToleranceType tol_type, Real reltol, void* abstol)
+{
+ updRep().sysp = &sys;
+
+ N_Vector_SimTK nv_y0(y0); // references, not copies
+ N_Vector_SimTK nv_yp0(yp0);
+ return CPodeReInit(updRep().cpode_mem,
+ getRep().useImplicitODEFunction
+ ? (void*)implicitODEWrapper
+ : (void*)explicitODEWrapper,
+ (void*)rep,
+ t0, &nv_y0, &nv_yp0,
+ mapToleranceType(tol_type),reltol,abstol);
+}
+
+int CPodes::projInit(ProjectionNorm norm, ConstraintLinearity lin,
+ const Vector& ctol)
+{
+ N_Vector_SimTK nv_ctol(ctol);
+ return CPodeProjInit(updRep().cpode_mem,
+ mapProjectionNorm(norm),
+ mapConstraintLinearity(lin),
+ constraintWrapper, (void*)rep,
+ &nv_ctol);
+}
+
+int CPodes::projDefine() {
+ return CPodeProjDefine(updRep().cpode_mem, projectWrapper, (void*)rep);
+}
+
+int CPodes::quadInit(const Vector& q0) {
+ N_Vector_SimTK nv_q0(q0);
+ return CPodeQuadInit(updRep().cpode_mem, quadratureWrapper, (void*)rep,
+ &nv_q0);
+}
+
+int CPodes::quadReInit(const Vector& q0) {
+ N_Vector_SimTK nv_q0(q0);
+ return CPodeQuadReInit(updRep().cpode_mem, quadratureWrapper, (void*)rep,
+ &nv_q0);
+}
+
+int CPodes::rootInit(int nrtfn) {
+ return CPodeRootInit(updRep().cpode_mem, nrtfn, rootWrapper, (void*)rep);
+}
+
+int CPodes::setErrHandlerFn() {
+ return CPodeSetErrHandlerFn(updRep().cpode_mem, errorHandlerWrapper, (void*)rep);
+}
+int CPodes::setErrFile(FILE* errfp) {
+ return CPodeSetErrFile(updRep().cpode_mem,errfp);
+}
+
+int CPodes::setEwtFn() {
+ return CPodeSetEwtFn(updRep().cpode_mem, weightWrapper, (void*)rep);
+}
+int CPodes::setMaxOrd(int maxord) {
+ return CPodeSetMaxOrd(updRep().cpode_mem,maxord);
+}
+int CPodes::setMaxNumSteps(int mxsteps) {
+ return CPodeSetMaxNumSteps(updRep().cpode_mem,(long)mxsteps);
+}
+int CPodes::setMaxHnilWarns(int mxhnil) {
+ return CPodeSetMaxHnilWarns(updRep().cpode_mem,mxhnil);
+}
+int CPodes::setStabLimDet(bool stldet) {
+ return CPodeSetStabLimDet(updRep().cpode_mem,(booleantype)stldet);
+}
+int CPodes::setInitStep(Real hin) {
+ return CPodeSetInitStep(updRep().cpode_mem,hin);
+}
+int CPodes::setMinStep(Real hmin) {
+ return CPodeSetMinStep(updRep().cpode_mem,hmin);
+}
+int CPodes::setMaxStep(Real hmax) {
+ return CPodeSetMaxStep(updRep().cpode_mem,hmax);
+}
+int CPodes::setStopTime(Real tstop) {
+ return CPodeSetStopTime(updRep().cpode_mem,tstop);
+}
+int CPodes::setMaxErrTestFails(int maxnef) {
+ return CPodeSetMaxErrTestFails(updRep().cpode_mem,maxnef);
+}
+
+int CPodes::setMaxNonlinIters(int maxcor) {
+ return CPodeSetMaxNonlinIters(updRep().cpode_mem,maxcor);
+}
+int CPodes::setMaxConvFails(int maxncf) {
+ return CPodeSetMaxConvFails(updRep().cpode_mem,maxncf);
+}
+int CPodes::setNonlinConvCoef(Real nlscoef) {
+ return CPodeSetNonlinConvCoef(updRep().cpode_mem,nlscoef);
+}
+
+int CPodes::setProjUpdateErrEst(bool proj_err) {
+ return CPodeSetProjUpdateErrEst(updRep().cpode_mem,(booleantype)proj_err);
+}
+int CPodes::setProjFrequency(int proj_freq) {
+ return CPodeSetProjFrequency(updRep().cpode_mem,(long)proj_freq);
+}
+int CPodes::setProjTestCnstr(bool test_cnstr) {
+ return CPodeSetProjTestCnstr(updRep().cpode_mem,(booleantype)test_cnstr);
+}
+int CPodes::setProjLsetupFreq(int proj_lset_freq) {
+ return CPodeSetProjLsetupFreq(updRep().cpode_mem,(long)proj_lset_freq);
+}
+int CPodes::setProjNonlinConvCoef(Real prjcoef) {
+ return CPodeSetProjNonlinConvCoef(updRep().cpode_mem,prjcoef);
+}
+
+int CPodes::setQuadErrCon(bool errconQ,
+ int tol_typeQ, Real reltolQ, void* abstolQ) {
+ return CPodeSetQuadErrCon(updRep().cpode_mem,(booleantype)errconQ,
+ tol_typeQ,reltolQ,abstolQ);
+}
+
+int CPodes::setTolerances(int tol_type, Real reltol, void* abstol) {
+ return CPodeSetTolerances(updRep().cpode_mem,tol_type,reltol,abstol);
+}
+
+int CPodes::setRootDirection(Array_<int>& rootdir) {
+ int* array = new int[rootdir.size()];
+ for (int i = 0; i < (int)rootdir.size(); ++i)
+ array[i] = rootdir[i];
+ int result = CPodeSetRootDirection(updRep().cpode_mem, array);
+ delete[] array;
+ return result;
+}
+
+int CPodes::step(Real tout, Real* tret,
+ Vector& yout, Vector& ypout, StepMode mode)
+{
+ // The pretty code commented out here heap allocates the N_VectorContent_SimTK
+ // objects, which are then deleted in the N_Vector_SimTK destructor:
+ // N_Vector_SimTK nv_yout(yout);
+ // N_Vector_SimTK nv_ypout(ypout);
+ // Instead, we'll use stack allocated Content objects, and
+ // construct the N_Vectors by hand here avoiding the per-step
+ // heap allocation/deallocation, which gets tiresome after a while.
+
+ N_VectorContent_SimTK content_yout(yout);
+ N_VectorContent_SimTK content_ypout(ypout);
+ _generic_N_Vector nv_yout, nv_ypout;
+ nv_yout.content = (void*)&content_yout;
+ nv_ypout.content = (void*)&content_ypout;
+ nv_yout.ops = nv_ypout.ops = N_Vector_Ops_SimTK::getSundialsOpsPtr();
+
+ return CPode(updRep().cpode_mem,tout,tret,
+ &nv_yout, &nv_ypout,
+ mapStepMode(mode));
+}
+
+int CPodes::getDky(Real t, int k, Vector& dky) {
+ N_Vector_SimTK nv_dky(dky);
+ return CPodeGetDky(updRep().cpode_mem,t,k,&nv_dky);
+}
+
+int CPodes::getQuad(Real t, Vector& yQout) {
+ N_Vector_SimTK nv_yQout(yQout);
+ return CPodeGetQuad(updRep().cpode_mem,t,&nv_yQout);
+}
+int CPodes::getQuadDky(Real t, int k, Vector& dky) {
+ N_Vector_SimTK nv_dky(dky);
+ return CPodeGetQuadDky(updRep().cpode_mem,t,k,&nv_dky);
+}
+
+int CPodes::getWorkSpace(int* lenrw, int* leniw) {
+ long llenrw, lleniw;
+ int stat = CPodeGetWorkSpace(updRep().cpode_mem,&llenrw,&lleniw);
+ *lenrw = (int)llenrw; *leniw = (int)lleniw;
+ return stat;
+}
+int CPodes::getNumSteps(int* nsteps) {
+ long lnsteps;
+ int stat = CPodeGetNumSteps(updRep().cpode_mem,&lnsteps);
+ *nsteps = (int)lnsteps;
+ return stat;
+}
+int CPodes::getNumFctEvals(int* nfevals) {
+ long lnfevals;
+ int stat = CPodeGetNumFctEvals(updRep().cpode_mem,&lnfevals);
+ *nfevals = (int)lnfevals;
+ return stat;
+}
+int CPodes::getNumLinSolvSetups(int* nlinsetups) {
+ long lnlinsetups;
+ int stat = CPodeGetNumLinSolvSetups(updRep().cpode_mem,&lnlinsetups);
+ *nlinsetups = (int)lnlinsetups;
+ return stat;
+}
+int CPodes::getNumErrTestFails(int* netfails) {
+ long lnetfails;
+ int stat = CPodeGetNumErrTestFails(updRep().cpode_mem,&lnetfails);
+ *netfails = (int)lnetfails;
+ return stat;
+}
+int CPodes::getLastOrder(int* qlast) {
+ return CPodeGetLastOrder(updRep().cpode_mem,qlast);
+}
+int CPodes::getCurrentOrder(int* qcur) {
+ return CPodeGetCurrentOrder(updRep().cpode_mem,qcur);
+}
+int CPodes::getNumStabLimOrderReds(int* nslred) {
+ long lnslred;
+ int stat = CPodeGetNumStabLimOrderReds(updRep().cpode_mem,&lnslred);
+ *nslred = (int)lnslred;
+ return stat;
+}
+int CPodes::getActualInitStep(Real* hinused) {
+ return CPodeGetActualInitStep(updRep().cpode_mem,hinused);
+}
+int CPodes::getLastStep(Real* hlast) {
+ return CPodeGetLastStep(updRep().cpode_mem,hlast);
+}
+int CPodes::getCurrentStep(Real* hcur) {
+ return CPodeGetCurrentStep(updRep().cpode_mem,hcur);
+}
+int CPodes::getCurrentTime(Real* tcur) {
+ return CPodeGetCurrentTime(updRep().cpode_mem,tcur);
+}
+int CPodes::getTolScaleFactor(Real* tolsfac) {
+ return CPodeGetTolScaleFactor(updRep().cpode_mem,tolsfac);
+}
+int CPodes::getErrWeights(Vector& eweight) {
+ N_Vector_SimTK nv_eweight(eweight);
+ return CPodeGetErrWeights(updRep().cpode_mem, &nv_eweight);
+}
+int CPodes::getEstLocalErrors(Vector& ele) {
+ N_Vector_SimTK nv_ele(ele);
+ return CPodeGetEstLocalErrors(updRep().cpode_mem, &nv_ele);
+}
+int CPodes::getNumGEvals(int* ngevals) {
+ long lngevals;
+ int stat = CPodeGetNumGEvals(updRep().cpode_mem,&lngevals);
+ *ngevals = (int)lngevals;
+ return stat;
+}
+int CPodes::getRootInfo(int* rootsfound) {
+ return CPodeGetRootInfo(updRep().cpode_mem,rootsfound);
+}
+int CPodes::getRootWindow(Real* tLo, Real* tHi) {
+ return CPodeGetRootWindow(updRep().cpode_mem,tLo,tHi);
+}
+int CPodes::getIntegratorStats(int* nsteps,
+ int* nfevals, int* nlinsetups,
+ int* netfails, int* qlast,
+ int* qcur, Real* hinused, Real* hlast,
+ Real* hcur, Real* tcur)
+{
+ long lnsteps, lnfevals, lnlinsetups, lnetfails;
+ int stat = CPodeGetIntegratorStats(updRep().cpode_mem,&lnsteps,
+ &lnfevals,&lnlinsetups,
+ &lnetfails,qlast,
+ qcur,hinused,hlast,
+ hcur,tcur);
+ *nsteps = (int)lnsteps; *nfevals = (int)lnfevals;
+ *nlinsetups = (int)lnlinsetups; *netfails = (int)lnetfails;
+ return stat;
+}
+
+int CPodes::getNumNonlinSolvIters(int* nniters) {
+ long lnniters;
+ int stat = CPodeGetNumNonlinSolvIters(updRep().cpode_mem,&lnniters);
+ *nniters = (int)lnniters;
+ return stat;
+}
+int CPodes::getNumNonlinSolvConvFails(int* nncfails) {
+ long lnncfails;
+ int stat = CPodeGetNumNonlinSolvConvFails(updRep().cpode_mem,&lnncfails);
+ *nncfails = (int)lnncfails;
+ return stat;
+}
+int CPodes::getNonlinSolvStats(int* nniters, int* nncfails)
+{
+ long lnniters, lnncfails;
+ int stat = CPodeGetNonlinSolvStats(updRep().cpode_mem,&lnniters,&lnncfails);
+ *nniters = (int)lnniters; *nncfails = (int)lnncfails;
+ return stat;
+}
+int CPodes::getProjNumProj(int* nproj) {
+ long lnproj;
+ int stat = CPodeGetProjNumProj(updRep().cpode_mem,&lnproj);
+ *nproj = (int)lnproj;
+ return stat;
+}
+int CPodes::getProjNumCnstrEvals(int* nce) {
+ long lnce;
+ int stat = CPodeGetProjNumCnstrEvals(updRep().cpode_mem,&lnce);
+ *nce = (int)lnce;
+ return stat;
+}
+int CPodes::getProjNumLinSolvSetups(int* nsetupsP) {
+ long lnsetupsP;
+ int stat = CPodeGetProjNumLinSolvSetups(updRep().cpode_mem,&lnsetupsP);
+ *nsetupsP = (int)lnsetupsP;
+ return stat;
+}
+int CPodes::getProjNumFailures(int* nprf) {
+ long lnprf;
+ int stat = CPodeGetProjNumFailures(updRep().cpode_mem,&lnprf);
+ *nprf = (int)lnprf;
+ return stat;
+}
+int CPodes::getProjStats(int* nproj,
+ int* nce, int* nsetupsP,
+ int* nprf)
+{
+ long lnproj,lnce,lnsetupsP,lnprf;
+ int stat = CPodeGetProjStats(updRep().cpode_mem,&lnproj,&lnce,&lnsetupsP,&lnprf);
+ *nproj=(int)lnproj; *nce=(int)lnce;
+ *nsetupsP=(int)lnsetupsP; *nprf=(int)lnprf;
+ return stat;
+}
+int CPodes::getQuadNumFunEvals(int* nqevals) {
+ long lnqevals;
+ int stat = CPodeGetQuadNumFunEvals(updRep().cpode_mem,&lnqevals);
+ *nqevals = (int)lnqevals;
+ return stat;
+}
+int CPodes::getQuadErrWeights(Vector& eQweight) {
+ N_Vector_SimTK nv_eQweight(eQweight);
+ return CPodeGetQuadErrWeights(updRep().cpode_mem, &nv_eQweight);
+}
+char* CPodes::getReturnFlagName(int flag) {
+ return CPodeGetReturnFlagName(flag);
+}
+
+int CPodes::dlsSetJacFn(void* jac, void* jac_data) {
+ return CPDlsSetJacFn(updRep().cpode_mem,jac,jac_data);
+}
+int CPodes::dlsGetWorkSpace(int* lenrwLS, int* leniwLS) {
+ long llenrwLS, lleniwLS;
+ int stat = CPDlsGetWorkSpace(updRep().cpode_mem,&llenrwLS,&lleniwLS);
+ *lenrwLS = (int)llenrwLS; *leniwLS = (int)lleniwLS;
+ return stat;
+}
+int CPodes::dlsGetNumJacEvals(int* njevals) {
+ long lnjevals;
+ int stat = CPDlsGetNumJacEvals(updRep().cpode_mem,&lnjevals);
+ *njevals = (int)lnjevals;
+ return stat;
+}
+int CPodes::dlsGetNumFctEvals(int* nfevalsLS) {
+ long lnfevalsLS;
+ int stat = CPDlsGetNumFctEvals(updRep().cpode_mem,&lnfevalsLS);
+ *nfevalsLS = (int)lnfevalsLS;
+ return stat;
+}
+int CPodes::dlsGetLastFlag(int* flag) {
+ return CPDlsGetLastFlag(updRep().cpode_mem,flag);
+}
+char* CPodes::dlsGetReturnFlagName(int flag) {
+ return CPDlsGetReturnFlagName(flag);
+}
+int CPodes::dlsProjSetJacFn(void* jacP, void* jacP_data) {
+ return CPDlsProjSetJacFn(updRep().cpode_mem,jacP,jacP_data);
+}
+int CPodes::dlsProjGetNumJacEvals(int* njPevals) {
+ long lnjPevals;
+ int stat = CPDlsProjGetNumJacEvals(updRep().cpode_mem,&lnjPevals);
+ *njPevals = (int)lnjPevals;
+ return stat;
+}
+int CPodes::dlsProjGetNumFctEvals(int* ncevalsLS) {
+ long lncevalsLS;
+ int stat = CPDlsProjGetNumFctEvals(updRep().cpode_mem,&lncevalsLS);
+ *ncevalsLS = (int)lncevalsLS;
+ return stat;
+}
+
+int CPodes::lapackDense(int N) {
+ return CPLapackDense(updRep().cpode_mem,N);
+}
+int CPodes::lapackBand(int N, int mupper, int mlower) {
+ return CPLapackBand(updRep().cpode_mem,N,mupper,mlower);
+}
+int CPodes::lapackDenseProj(int Nc, int Ny, ProjectionFactorizationType fact_type) {
+ return CPLapackDenseProj(updRep().cpode_mem,Nc,Ny,
+ mapProjectionFactorizationType(fact_type));
+}
+
+
+
+// Client-side function registration
+void CPodes::registerExplicitODEFunc(CPodes::ExplicitODEFunc f) {
+ updRep().explicitODEFunc = f;
+}
+void CPodes::registerImplicitODEFunc(CPodes::ImplicitODEFunc f) {
+ updRep().implicitODEFunc = f;
+}
+void CPodes::registerConstraintFunc(CPodes::ConstraintFunc f) {
+ updRep().constraintFunc = f;
+}
+void CPodes::registerProjectFunc(CPodes::ProjectFunc f) {
+ updRep().projectFunc = f;
+}
+void CPodes::registerQuadratureFunc(CPodes::QuadratureFunc f) {
+ updRep().quadratureFunc = f;
+}
+void CPodes::registerRootFunc(CPodes::RootFunc f) {
+ updRep().rootFunc = f;
+}
+void CPodes::registerWeightFunc(CPodes::WeightFunc f) {
+ updRep().weightFunc = f;
+}
+void CPodes::registerErrorHandlerFunc(CPodes::ErrorHandlerFunc f) {
+ updRep().errorHandlerFunc = f;
+}
+
+/////////////////////////////////
+// CPodesSystem IMPLEMENTATION //
+/////////////////////////////////
+
+// These are the default implementations for the CPodesSystem virtual functions.
+// Note that this DOES NOT cause binary compatibility problems. The addresses of
+// these functions will be supplied from the library side, but these addresses will
+// get filled in to the default virtual function table on the *client* side which
+// knows where to put each function by name.
+
+int CPodesSystem::explicitODE(Real, const Vector&, Vector&) const {
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "CPodesSystem", "explicitODE");
+ return std::numeric_limits<int>::min();
+}
+int CPodesSystem::implicitODE(Real, const Vector&, const Vector&, Vector&) const {
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "CPodesSystem", "implicitODE");
+ return std::numeric_limits<int>::min();
+}
+
+int CPodesSystem::constraint(Real, const Vector&, Vector&) const {
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "CPodesSystem", "constraint");
+ return std::numeric_limits<int>::min();
+}
+
+int CPodesSystem::project(Real, const Vector&, Vector&, Real, Vector&) const {
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "CPodesSystem", "project");
+ return std::numeric_limits<int>::min();
+}
+
+int CPodesSystem::quadrature(Real, const Vector&, Vector&) const {
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "CPodesSystem", "quadrature");
+ return std::numeric_limits<int>::min();
+}
+
+int CPodesSystem::root(Real, const Vector&, const Vector&, Vector&) const {
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "CPodesSystem", "root");
+ return std::numeric_limits<int>::min();
+}
+
+int CPodesSystem::weight(const Vector&, Vector&) const {
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "CPodesSystem", "weight");
+ return std::numeric_limits<int>::min();
+}
+
+void CPodesSystem::errorHandler(int, const char*, const char*, char*) const {
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "CPodesSystem", "errorHandler");
+}
+
+} // namespace SimTK
+
+
diff --git a/SimTKmath/Integrators/src/CPodes/nvector_SimTK.cpp b/SimTKmath/Integrators/src/CPodes/nvector_SimTK.cpp
new file mode 100644
index 0000000..7f59f0a
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/nvector_SimTK.cpp
@@ -0,0 +1,538 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This is the implementation of N_Vector_SimTK anad related classes. It
+ * is a concrete implementation of the generic interface defined by
+ * Sundials for its abstract data structure N_Vector. The idea here is to
+ * provide the needed operations using the SimTK Vector class.
+ */
+
+#include "simmath/internal/SimTKcpodes.h"
+#include "nvector_SimTK.h"
+
+using SimTK::Real;
+using SimTK::Vector;
+
+
+// These are the "virtual functions" required by the N_Vector interface.
+// Each is an implementation of one of the function types defined in
+// the Sundials C struct _generic_N_Vector_Ops. At startup we'll initialize
+// a single static "ops" object and use it for all the N_Vector_SimTK's
+// that ever get created.
+
+static N_Vector nvclone_SimTK(N_Vector);
+static N_Vector nvcloneempty_SimTK(N_Vector);
+static void nvdestroy_SimTK(N_Vector);
+static void nvspace_SimTK(N_Vector, long int*, long int*);
+static realtype* nvgetarraypointer_SimTK(N_Vector);
+static void nvsetarraypointer_SimTK(realtype*, N_Vector);
+static void nvlinearsum_SimTK(realtype, N_Vector, realtype, N_Vector, N_Vector);
+static void nvconst_SimTK(realtype, N_Vector);
+static void nvprod_SimTK(N_Vector, N_Vector, N_Vector);
+static void nvdiv_SimTK(N_Vector, N_Vector, N_Vector);
+static void nvscale_SimTK(realtype, N_Vector, N_Vector);
+static void nvabs_SimTK(N_Vector, N_Vector);
+static void nvinv_SimTK(N_Vector, N_Vector);
+static void nvaddconst_SimTK(N_Vector, realtype, N_Vector);
+static realtype nvdotprod_SimTK(N_Vector, N_Vector);
+static realtype nvmaxnorm_SimTK(N_Vector);
+static realtype nvwrmsnorm_SimTK(N_Vector, N_Vector);
+static realtype nvwrmsnormmask_SimTK(N_Vector, N_Vector, N_Vector);
+static realtype nvmin_SimTK(N_Vector);
+static realtype nvwl2norm_SimTK(N_Vector, N_Vector);
+static realtype nvl1norm_SimTK(N_Vector);
+static void nvcompare_SimTK(realtype, N_Vector, N_Vector);
+static booleantype nvinvtest_SimTK(N_Vector, N_Vector);
+static booleantype nvconstrmask_SimTK(N_Vector, N_Vector, N_Vector);
+static realtype nvminquotient_SimTK(N_Vector, N_Vector);
+
+// The default constructor for this static const member takes care
+// of initializing all the function pointers.
+const N_Vector_Ops_SimTK N_Vector_Ops_SimTK::Ops;
+
+// This is the default constructor. It is used exactly once to initialize
+// the static const member variable above.
+N_Vector_Ops_SimTK::N_Vector_Ops_SimTK() {
+ nvclone = nvclone_SimTK;
+ nvcloneempty = nvcloneempty_SimTK;
+ nvdestroy = nvdestroy_SimTK;
+ nvspace = nvspace_SimTK;
+ nvgetarraypointer = nvgetarraypointer_SimTK;
+ nvsetarraypointer = nvsetarraypointer_SimTK;
+ nvlinearsum = nvlinearsum_SimTK;
+ nvconst = nvconst_SimTK;
+ nvprod = nvprod_SimTK;
+ nvdiv = nvdiv_SimTK;
+ nvscale = nvscale_SimTK;
+ nvabs = nvabs_SimTK;
+ nvinv = nvinv_SimTK;
+ nvaddconst = nvaddconst_SimTK;
+ nvdotprod = nvdotprod_SimTK;
+ nvmaxnorm = nvmaxnorm_SimTK;
+ nvwrmsnorm = nvwrmsnorm_SimTK;
+ nvwrmsnormmask = nvwrmsnormmask_SimTK;
+ nvmin = nvmin_SimTK;
+ nvwl2norm = nvwl2norm_SimTK;
+ nvl1norm = nvl1norm_SimTK;
+ nvcompare = nvcompare_SimTK;
+ nvinvtest = nvinvtest_SimTK;
+ nvconstrmask = nvconstrmask_SimTK;
+ nvminquotient = nvminquotient_SimTK;
+}
+
+//////////////////////////////
+// DEFINITIONS OF OPERATORS //
+//////////////////////////////
+
+// N_VClone
+// Allocate a new N_Vector of the same size & type as
+// existing vector w, but do not copy the data.
+static N_Vector
+nvclone_SimTK(N_Vector w) {
+ return N_Vector_SimTK::nvclone(w);
+}
+
+// N_VCloneEmpty
+// Allocate a new N_Vector of the same type as
+// existing vector w, but allocate no space.
+static N_Vector
+nvcloneempty_SimTK(N_Vector w) {
+ assert(N_Vector_SimTK::isA(w));
+ return new N_Vector_SimTK();
+}
+
+// N_VDestroy
+// Free all space associated with a given N_Vector. This
+// may or may not destroy the underlying data, depending
+// on whether the given N_Vector is the owner.
+static void
+nvdestroy_SimTK(N_Vector v) {
+ delete N_Vector_SimTK::updDowncast(v);
+}
+
+// N_VSpace
+// Returns the storage requirements for one N_Vector, specified
+// as a number of 'realtype' objects and a number of 'int'
+// objects.
+// sherm 061128: I discussed this with Radu and he confirmed
+// that the routine does not make sense and is never used by
+// Sundials for anything significant. It may show up in some
+// kind of unused storage estimate. So I'm just returning
+// the length as the number of reals and "1" as the number
+// of ints.
+static void
+nvspace_SimTK(N_Vector v, long int* lrw, long int* liw) {
+ *lrw = N_Vector_SimTK::getVector(v).size();
+ *liw = 1; // doesn't mean anything
+}
+
+// N_VGetArrayPointer
+// This returns the address of the first entry in the N_Vector's
+// data, with the built-in assumption that the data is stored
+// contiguously as an array of reals.
+static realtype*
+nvgetarraypointer_SimTK(N_Vector nvx) {
+ Vector& x = N_Vector_SimTK::updVector(nvx);
+ return x.updContiguousScalarData();
+}
+
+// N_VSetArrayPointer
+// Replace the data portion of an N_Vector with a new one. This
+// assumes that the existing N_Vector uses contiguous data
+// and is the owner of its data. Note: we are NOT going to
+// delete the old data here; we're assuming that the N_Vector
+// user is holding onto a pointer to it obtained with
+// N_VGetArrayPointer above. If that's not right then there
+// is going to be a leak here.
+static void
+nvsetarraypointer_SimTK(realtype* vdata, N_Vector nvz) {
+ Vector& z = N_Vector_SimTK::updVector(nvz);
+ assert(z.hasContiguousData());
+ realtype* oldData;
+ z.swapOwnedContiguousScalarData(vdata, z.size(), oldData);
+ // don't do anything with oldData here
+}
+
+// N_VLinearSum
+// z = ax + by
+// This will blow up if x and y aren't the same size.
+// z will get resized if necessary.
+static void
+nvlinearsum_SimTK(realtype a, N_Vector nvx, realtype b, N_Vector nvy, N_Vector nvz) {
+ const Vector& x = N_Vector_SimTK::getVector(nvx);
+ const Vector& y = N_Vector_SimTK::getVector(nvy);
+ Vector& z = N_Vector_SimTK::updVector(nvz);
+
+ assert(y.size() == x.size() && z.size() == x.size());
+
+ z = a*x + b*y;
+}
+
+// N_VConst
+// Set all components of vector z to value c.
+static void
+nvconst_SimTK(realtype c, N_Vector nvz) {
+ Vector& z = N_Vector_SimTK::updVector(nvz);
+ z = c;
+}
+
+// N_VProd
+// z = x .* y (componentwise multiply)
+static void
+nvprod_SimTK(N_Vector nvx, N_Vector nvy, N_Vector nvz) {
+ const Vector& x = N_Vector_SimTK::getVector(nvx);
+ const Vector& y = N_Vector_SimTK::getVector(nvy);
+ Vector& z = N_Vector_SimTK::updVector(nvz);
+
+ const int sz = x.size();
+ assert(y.size() == sz && z.size() == sz);
+
+ // TODO: should be a built-in Vector operation for this
+ const Real* xp = x.getContiguousScalarData();
+ const Real* yp = y.getContiguousScalarData();
+ Real* zp = z.updContiguousScalarData();
+
+ for (int i=0; i<sz; ++i)
+ zp[i] = xp[i]*yp[i];
+}
+
+// N_VDiv
+// z = x ./ y (componentwise divide)
+static void
+nvdiv_SimTK(N_Vector nvx, N_Vector nvy, N_Vector nvz) {
+ const Vector& x = N_Vector_SimTK::getVector(nvx);
+ const Vector& y = N_Vector_SimTK::getVector(nvy);
+ Vector& z = N_Vector_SimTK::updVector(nvz);
+
+ const int sz = x.size();
+ assert(y.size() == sz && z.size() == sz);
+
+ // TODO: should be a built-in Vector operation for this
+ const Real* xp = x.getContiguousScalarData();
+ const Real* yp = y.getContiguousScalarData();
+ Real* zp = z.updContiguousScalarData();
+
+ for (int i=0; i<sz; ++i)
+ zp[i] = xp[i]/yp[i];
+}
+
+// N_VScale
+// z = c*x
+static void
+nvscale_SimTK(realtype c, N_Vector nvx, N_Vector nvz) {
+ const Vector& x = N_Vector_SimTK::getVector(nvx);
+ Vector& z = N_Vector_SimTK::updVector(nvz);
+
+ assert(z.size() == x.size());
+ z = c*x;
+}
+
+// N_VAbs
+// Set zi = |xi|, that is, componentwise absolute value.
+static void
+nvabs_SimTK(N_Vector nvx, N_Vector nvz) {
+ const Vector& x = N_Vector_SimTK::getVector(nvx);
+ Vector& z = N_Vector_SimTK::updVector(nvz);
+
+ assert(z.size() == x.size());
+ z = x.abs();
+}
+
+// N_VInv
+// zi = 1/xi, that is, componentwise inversion.
+static void
+nvinv_SimTK(N_Vector nvx, N_Vector nvz) {
+ const Vector& x = N_Vector_SimTK::getVector(nvx);
+ Vector& z = N_Vector_SimTK::updVector(nvz);
+
+ const int sz = x.size();
+ assert(z.size() == sz);
+
+ // TODO: should be a built-in Vector operation for this
+ const Real* xp = x.getContiguousScalarData();
+ Real* zp = z.updContiguousScalarData();
+
+ for (int i=0; i<sz; ++i)
+ zp[i] = 1/xp[i];
+}
+
+// N_VAddConst
+// zi = xi + b, that is, componentwise scalar addition.
+static void
+nvaddconst_SimTK(N_Vector nvx, realtype b, N_Vector nvz) {
+ const Vector& x = N_Vector_SimTK::getVector(nvx);
+ Vector& z = N_Vector_SimTK::updVector(nvz);
+
+ const int sz = x.size();
+ assert(z.size() == sz);
+
+ // TODO: should be a built-in Vector operation for this
+ const Real* xp = x.getContiguousScalarData();
+ Real* zp = z.updContiguousScalarData();
+ for (int i=0; i<sz; ++i)
+ zp[i] = xp[i] + b;
+}
+
+// N_VDotProd
+// result = dot(x,y)
+static realtype
+nvdotprod_SimTK(N_Vector nvx, N_Vector nvy) {
+ const Vector& x = N_Vector_SimTK::getVector(nvx);
+ const Vector& y = N_Vector_SimTK::getVector(nvy);
+
+ assert(y.size() == x.size());
+
+ return ~x * y;
+}
+
+// N_VMaxNorm
+// result = max_i |xi|, that is, return the absolute value
+// of the element whose absolute value is largest.
+static realtype
+nvmaxnorm_SimTK(N_Vector nvx) {
+ const Vector& x = N_Vector_SimTK::getVector(nvx);
+
+ const int sz = x.size();
+ assert(sz > 0);
+
+ // TODO: should be a built-in Vector operation for this
+ const Real* xp = x.getContiguousScalarData();
+ Real maxabs = std::abs(xp[0]);
+ for (int i=1; i<sz; ++i) {
+ const Real absval = std::abs(xp[i]);
+ if (absval > maxabs) maxabs=absval;
+ }
+ return maxabs;
+}
+
+// N_VWrmsNorm
+// result = sqrt( sum_i((xi*wi)^2)/n )
+// that is, weighted RMS norm of x.
+static realtype
+nvwrmsnorm_SimTK(N_Vector nvx, N_Vector nvw) {
+ const Vector& x = N_Vector_SimTK::getVector(nvx);
+ const Vector& w = N_Vector_SimTK::getVector(nvw);
+
+ const int sz = x.size();
+ assert(sz > 0 && w.size() == sz);
+
+ // TODO: should be a built-in Vector operation for this
+ const Real* xp = x.getContiguousScalarData();
+ const Real* wp = w.getContiguousScalarData();
+
+ Real sumsq = 0;
+ for (int i=0; i<sz; ++i) {
+ const Real xw = xp[i]*wp[i];
+ sumsq += xw*xw;
+ }
+ return std::sqrt( sumsq / sz );
+}
+
+// N_VWrmsNormMask
+// result = sqrt( sum_id[i]!=0((xi*wi)^2)/n );
+// that is, weighted RMS norm of x including only those terms
+// where id[i] != 0.
+static realtype
+nvwrmsnormmask_SimTK(N_Vector nvx, N_Vector nvw, N_Vector nvid) {
+ const Vector& x = N_Vector_SimTK::getVector(nvx);
+ const Vector& w = N_Vector_SimTK::getVector(nvw);
+ const Vector& id = N_Vector_SimTK::getVector(nvid);
+
+ const int sz = x.size();
+ assert(sz > 0 && w.size()==sz && id.size()==sz);
+
+ const Real* xp = x.getContiguousScalarData();
+ const Real* wp = w.getContiguousScalarData();
+ const Real* idp = id.getContiguousScalarData();
+
+ Real sumsq = 0;
+ for (int i=0; i<sz; ++i)
+ if (idp[i] != 0) {
+ const Real xw = xp[i]*wp[i];
+ sumsq += xw*xw;
+ }
+ return std::sqrt( sumsq / sz );
+}
+
+// N_VMin
+// Return the smallest element of x.
+static realtype
+nvmin_SimTK(N_Vector nvx) {
+ const Vector& x = N_Vector_SimTK::getVector(nvx);
+
+ const int sz = x.size();
+ assert(sz > 0);
+
+ // TODO: should be a built-in Vector operation for this
+ const Real* xp = x.getContiguousScalarData();
+
+ Real minelt = xp[0];
+ for (int i=1; i<sz; ++i)
+ if (xp[i] < minelt) minelt=xp[i];
+ return minelt;
+}
+
+// N_VWL2Norm
+// result = sqrt( sum_i( (xi*wi)^2 ) )
+// Return the weighted Euclidean L2 norm of x.
+static realtype
+nvwl2norm_SimTK(N_Vector nvx, N_Vector nvw) {
+ const Vector& x = N_Vector_SimTK::getVector(nvx);
+ const Vector& w = N_Vector_SimTK::getVector(nvw);
+
+ const int sz = x.size();
+ assert(w.size() == sz);
+
+ // TODO: should be a built-in Vector operation for this
+ const Real* xp = x.getContiguousScalarData();
+ const Real* wp = w.getContiguousScalarData();
+
+ Real sumsq = 0;
+ for (int i=0; i<sz; ++i) {
+ const Real xw = xp[i]*wp[i];
+ sumsq += xw*xw;
+ }
+ return std::sqrt(sumsq);
+}
+
+// N_VL1Norm
+// result = sum_i |xi|
+// Return the L1 norm of x (sum of absolute values).
+static realtype
+nvl1norm_SimTK(N_Vector nvx) {
+ const Vector& x = N_Vector_SimTK::getVector(nvx);
+
+ const int sz = x.size();
+
+ // TODO: should be a built-in Vector operation for this
+ const Real* xp = x.getContiguousScalarData();
+
+ Real sumabs = 0;
+ for (int i=0; i<sz; ++i) {
+ sumabs += std::abs(xp[i]);
+ }
+ return sumabs;
+}
+
+// N_VCompare
+// Compare components of x to scalar c and return
+// z such that zi=1 if |xi|>=c, else 0.
+static void
+nvcompare_SimTK(realtype c, N_Vector nvx, N_Vector nvz) {
+ const Vector& x = N_Vector_SimTK::getVector(nvx);
+ Vector& z = N_Vector_SimTK::updVector(nvz);
+
+ const int sz = x.size();
+ assert(z.size() == sz);
+
+ const Real* xp = x.getContiguousScalarData();
+ Real* zp = z.updContiguousScalarData();
+
+ for (int i=0; i<sz; ++i)
+ zp[i] = Real(std::abs(xp[i]) >= c ? 1 : 0);
+}
+
+// N_VInvTest
+// Set z[i] = 1/x[i] if ALL x[i] != 0 and return TRUE.
+// If ANY x[i]==0 return FALSE with z undefined.
+static booleantype
+nvinvtest_SimTK(N_Vector nvx, N_Vector nvz) {
+ const Vector& x = N_Vector_SimTK::getVector(nvx);
+ Vector& z = N_Vector_SimTK::updVector(nvz);
+
+ const int sz = x.size();
+ assert(z.size() == sz);
+
+ const Real* xp = x.getContiguousScalarData();
+ Real* zp = z.updContiguousScalarData();
+
+ for (int i=0; i<sz; ++i) {
+ if (xp[i] == 0) return FALSE;
+ zp[i] = 1/xp[i];
+ }
+ return TRUE;
+}
+
+// N_VConstrMask
+// Perform constraint tests on xi based on ci:
+// ci == 2 => xi > 0
+// ci == 1 => xi >= 0
+// ci == 0 => xi is unconstrained
+// ci == -1 => xi <= 0
+// ci == -2 => xi < 0
+// We set mask entry mi to 1 if xi failed its constraint, 0 otherwise.
+// Return TRUE if all passed (=> m==0), else FALSE.
+static booleantype
+nvconstrmask_SimTK(N_Vector nvc, N_Vector nvx, N_Vector nvm) {
+ const Vector& c = N_Vector_SimTK::getVector(nvc);
+ const Vector& x = N_Vector_SimTK::getVector(nvx);
+ Vector& m = N_Vector_SimTK::updVector(nvm);
+
+ const int sz = x.size();
+ assert(c.size()==sz && m.size()==sz);
+
+ m = 0; // assume success
+ booleantype allGood = TRUE;
+
+ const Real* cp = c.getContiguousScalarData();
+ const Real* xp = x.getContiguousScalarData();
+ Real* mp = m.updContiguousScalarData();
+
+ for (int i=0; i<sz; ++i) {
+ if (cp[i]==0) continue;
+ if (cp[i]== 2 && xp[i] > 0) continue;
+ if (cp[i]== 1 && xp[i] >= 0) continue;
+ if (cp[i]==-1 && xp[i] <= 0) continue;
+ if (cp[i]==-2 && xp[i] < 0) continue;
+ mp[i] = 1;
+ allGood = FALSE;
+ }
+
+ return allGood;
+}
+
+// N_VMinQuotient
+// Return the minimum of the quotients num_i/denom_i skipping
+// any elements where denom_i==0. If all the denom_i are zero,
+// return BIG_REAL.
+static realtype
+nvminquotient_SimTK(N_Vector nvnum, N_Vector nvdenom) {
+ const Vector& num = N_Vector_SimTK::getVector(nvnum);
+ const Vector& denom = N_Vector_SimTK::getVector(nvdenom);
+
+ const int sz = num.size();
+ assert(denom.size() == sz);
+
+ const Real* nump = num.getContiguousScalarData();
+ const Real* denomp = denom.getContiguousScalarData();
+
+ Real result = BIG_REAL;
+ for (int i=0; i<sz; ++i) {
+ if (denomp[i] == 0) continue;
+ const Real quot = nump[i]/denomp[i];
+ if (quot < result) result=quot;
+ }
+
+ return result;
+}
diff --git a/SimTKmath/Integrators/src/CPodes/nvector_SimTK.h b/SimTKmath/Integrators/src/CPodes/nvector_SimTK.h
new file mode 100644
index 0000000..0d63287
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/nvector_SimTK.h
@@ -0,0 +1,212 @@
+#ifndef SimTK_CPODES_NVECTOR_SimTK_H_
+#define SimTK_CPODES_NVECTOR_SimTK_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This header defines a Sundials N_Vector implementation which uses
+ * SimTK's Vector class internally.
+ */
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+
+#include "sundials/sundials_nvector.h"
+
+#include <cassert>
+
+using SimTK::Vector;
+
+// This is the object to which the generic N_Vector struct's "content"
+// entry points.
+class N_VectorContent_SimTK {
+public:
+ N_VectorContent_SimTK()
+ : treatAsConst(false), ownVector(true), data(new Vector()) {
+ }
+
+ N_VectorContent_SimTK(const Vector& v)
+ : treatAsConst(true), ownVector(false),
+ data(const_cast<Vector*>(&v)) {
+ }
+
+ N_VectorContent_SimTK(Vector& v)
+ : treatAsConst(false), ownVector(false), data(&v) {
+ }
+
+ // Copy constructor makes a deep (new) copy.
+ N_VectorContent_SimTK(const N_VectorContent_SimTK& nv)
+ : treatAsConst(false), ownVector(true), data(new Vector(*nv.data)) {
+ }
+
+ // Assignment fails if target is const, otherwise reallocates as needed.
+ N_VectorContent_SimTK& operator=(const N_VectorContent_SimTK& nv) {
+ if (&nv != this) {
+ assert(!treatAsConst);
+ *data = *nv.data;
+ }
+ return *this;
+ }
+
+ ~N_VectorContent_SimTK() {
+ if (ownVector) delete data;
+ data = 0;
+ }
+
+ const Vector& getVector() const {
+ assert(data);
+ return *data;
+ }
+
+ Vector& updVector() {
+ assert(data);
+ assert(!treatAsConst);
+ return *data;
+ }
+
+private:
+ bool treatAsConst; // is this logically const?
+ bool ownVector; // if true, destruct Vector along with this
+ Vector* data;
+
+};
+
+// This singleton class defines the operations needed by the abstract
+// N_Vector in terms of operations performed on SimTK Vectors.
+// The "virtual function table" is filled in once and used
+// in *all* N_Vector_SimTK objects.
+class N_Vector_Ops_SimTK : public _generic_N_Vector_Ops {
+public:
+ // Return a reference to the singleton object. You can use
+ // the address of this reference to check whether a given N_Vector
+ // is really an N_Vector_SimTK.
+ static const _generic_N_Vector_Ops& getOps() {
+ return Ops;
+ }
+
+ // Sundials isn't const correct so it is going to want a
+ // non-const pointer to the Ops class here, which is const.
+ // We'll just have to trust it.
+ static _generic_N_Vector_Ops* getSundialsOpsPtr() {
+ return const_cast<N_Vector_Ops_SimTK*>(&Ops);
+ }
+
+private:
+ N_Vector_Ops_SimTK(); // initialize
+ SUNDIALS_EXPORT static const N_Vector_Ops_SimTK Ops;
+};
+
+// This is the object to which an N_Vector points when we are using
+// SimTK's N_Vector implementation.
+class N_Vector_SimTK : public _generic_N_Vector {
+public:
+ // Default constructor creates an owned, empty, resizable vector.
+ N_Vector_SimTK() {
+ content = (void*)new N_VectorContent_SimTK();
+ ops = N_Vector_Ops_SimTK::getSundialsOpsPtr();
+ }
+
+ // This constructor produces a read-only *reference* to an existing Vector.
+ // Destruction of the N_Vector_SimTK leaves the original Vector as it was.
+ N_Vector_SimTK(const Vector& v) {
+ content = (void*)new N_VectorContent_SimTK(v);
+ ops = N_Vector_Ops_SimTK::getSundialsOpsPtr();
+ }
+
+ // This constructor produces a writable *reference* to an existing
+ // Vector. Destruction of the N_Vector_SimTK leaves the original
+ // Vector in existence, although its value may have changed.
+ N_Vector_SimTK(Vector& v) {
+ content = (void*)new N_VectorContent_SimTK(v);
+ ops = N_Vector_Ops_SimTK::getSundialsOpsPtr();
+ }
+
+ // Copy constructor makes a deep copy. The result will be an owned,
+ // resizable vector that will evaporate upon destruction of the
+ // N_Vector_SimTK object.
+ N_Vector_SimTK(const N_Vector_SimTK& nv) {
+ assert(nv.ops == &N_Vector_Ops_SimTK::getOps());
+ content = (void*)new N_VectorContent_SimTK(nv.getContent());
+ ops = N_Vector_Ops_SimTK::getSundialsOpsPtr();
+ }
+
+ // Assignment will fail if this isn't writable.
+ N_Vector_SimTK& operator=(const N_Vector_SimTK& nv) {
+ assert( ops == &N_Vector_Ops_SimTK::getOps()
+ && nv.ops == &N_Vector_Ops_SimTK::getOps());
+ if (&nv != this)
+ updContent() = nv.getContent();
+ return *this;
+ }
+
+ ~N_Vector_SimTK() {
+ assert(ops == &N_Vector_Ops_SimTK::getOps());
+ delete reinterpret_cast<N_VectorContent_SimTK*>(content);
+ content = 0;
+ ops = 0;
+ }
+
+ static bool isA(const N_Vector nv) {
+ return nv && (nv->ops == &N_Vector_Ops_SimTK::getOps());
+ }
+
+ static const N_Vector_SimTK* downcast(const N_Vector nv) {
+ assert(isA(nv));
+ return reinterpret_cast<const N_Vector_SimTK*>(nv);
+ }
+
+ static N_Vector_SimTK* updDowncast(N_Vector nv) {
+ assert(isA(nv));
+ return reinterpret_cast<N_Vector_SimTK*>(nv);
+ }
+
+ static N_Vector_SimTK* nvclone(const N_Vector nv) {
+ assert(isA(nv));
+ const N_Vector_SimTK& nvs = *reinterpret_cast<const N_Vector_SimTK*>(nv);
+ return new N_Vector_SimTK(nvs);
+ }
+
+ static const Vector& getVector(const N_Vector nv) {
+ assert(isA(nv));
+ const N_Vector_SimTK& nvs = *reinterpret_cast<const N_Vector_SimTK*>(nv);
+ return nvs.getContent().getVector();
+ }
+
+ static Vector& updVector(N_Vector nv) {
+ assert(isA(nv));
+ N_Vector_SimTK& nvs = *reinterpret_cast<N_Vector_SimTK*>(nv);
+ return nvs.updContent().updVector();
+ }
+
+private:
+ const N_VectorContent_SimTK& getContent() const {
+ return *reinterpret_cast<const N_VectorContent_SimTK*>(content);
+ }
+ N_VectorContent_SimTK& updContent() {
+ return *reinterpret_cast<N_VectorContent_SimTK*>(content);
+ }
+
+};
+
+#endif // SimTK_NVECTOR_SimTK_H_
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/CMakeLists.txt b/SimTKmath/Integrators/src/CPodes/sundials/CMakeLists.txt
new file mode 100644
index 0000000..cf7f4a6
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/CMakeLists.txt
@@ -0,0 +1,30 @@
+
+# add in all sundials source and headers here
+FILE(GLOB src_files ./src/*/*.c)
+FILE(GLOB incl_files ./include/*/*.h)
+
+foreach( src ${src_files} )
+ list(APPEND SOURCE_GROUPS "Source Files\\CPodes")
+ list(APPEND SOURCE_GROUP_FILES "${src}")
+endforeach()
+
+foreach( incl ${incl_files} )
+ list(APPEND SOURCE_GROUPS "Header Files\\CPodes")
+ list(APPEND SOURCE_GROUP_FILES "${incl}")
+endforeach()
+
+set(SOURCE_GROUPS ${SOURCE_GROUPS} PARENT_SCOPE)
+set(SOURCE_GROUP_FILES ${SOURCE_GROUP_FILES} PARENT_SCOPE)
+
+# append to the local scope copy, and then copy up to parent scope
+list(APPEND SOURCE_FILES ${src_files})
+set(SOURCE_FILES ${SOURCE_FILES} PARENT_SCOPE)
+
+list(APPEND SOURCE_INCLUDE_FILES ${incl_files})
+set(SOURCE_INCLUDE_FILES ${SOURCE_INCLUDE_FILES} PARENT_SCOPE)
+
+
+# build tests that depend on source at this level
+IF( BUILD_TESTING )
+ add_subdirectory(tests)
+ENDIF( BUILD_TESTING )
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/INSTALL_NOTES.txt b/SimTKmath/Integrators/src/CPodes/sundials/INSTALL_NOTES.txt
new file mode 100644
index 0000000..f02a730
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/INSTALL_NOTES.txt
@@ -0,0 +1,494 @@
+ SUNDIALS Installation Instructions
+ Release 2.3.0, August 2006
+
+
+These are generic installation instructions. For complete installation instructions,
+consult the user guide for any of the SUNDIALS solvers.
+
+Contents:
+1. Basic Installation
+2. Installation Names
+3. Compilers and Options
+ 3.1. General options
+ 3.2. Options for Fortran support
+ 3.3. Options for MPI support
+ 3.4. Options for library support
+ 3.5. Options for Matlab support
+ 3.6. Environment variables
+4. Configuration examples
+
+1. Basic Installation
+=====================
+
+The SUNDIALS suite (or individual solvers) are distributed as compressed archives
+(.tgz). The name of the distribution archive is of the form 'solver'-x.y.z.tgz,
+where 'solver' is one of: 'sundials', 'cvode', 'cvodes', 'ida', or 'kinsol',
+and x.y.z represents the version number (of the SUNDIALS suite or of the individual
+solver).
+
+To begin the installation, first uncompress and expand the sources, by issuing
+
+ % tar xzf solver-x.y.z.tgz
+
+This will extract source files under a directory 'solver'-x.y.z.
+
+The installation procedure outlined below will work on commodity linux/unix
+systems without modification. However, users are still encouraged to carefully read
+the entire chapter before attempting to install the SUNDIALS suite, in case
+non-default choices are desired for compilers, compilation options, or the like.
+In lieu of reading the option list below, the user may invoke the configuration
+script with the help flag to view a complete listing of available options, which
+may be done by issuing
+
+ % ./configure --help
+
+from within the directory created above.
+
+In the remainder of this chapter, we make the following distinctions:
+
+'srcdir'
+
+ is the directory 'solver'-x.y.z created above; i.e., the
+ directory containing the SUNDIALS sources.
+
+'builddir'
+
+ is the directory under which SUNDIALS is built; i.e., the directory from within
+ which the configure command is issued. Usually, this is the same as 'srcdir'.
+
+'instdir'
+
+ is the directory under which the SUNDIALS exported header files and libraries will
+ be installed. Typically, header files are exported under a directory 'instdir'/include
+ while libraries are installed under 'instdir'/lib, with 'instdir' specified with the
+ --prefix flag to configure. See below for more details on the installation directories,
+ including the special cases of the SUNDIALS examples and the sundialsTB toolbox.
+
+Note: The installation directory 'instdir' should NOT be the same as the source directory
+ 'srcdir'.
+
+The installation steps for SUNDIALS can be as simple as
+
+ % tar xzf solver-x.y.z.tgz
+ % cd solver-x.y.z
+ % ./configure
+ % make
+ % make install
+
+in which case the SUNDIALS header files and libraries are installed under /usr/local/include
+and /usr/local/lib, respectively. Note that, by default, neither the example programs nor the
+{\sundialsTB toolbox are built and installed.
+
+If disk space is a priority, then to delete all temporary files created by building SUNDIALS, issue
+
+ % make clean
+
+To prepare the SUNDIALS distribution for a new install (using, for example, different options and/or
+installation destinations), issue
+
+ % make distclean
+
+
+2. Installation Names
+=====================
+
+By default, 'make install' will install the SUNDIALS libraries under 'libdir' and the public
+header files under 'includedir'. The default values for these directories are 'instdir'/lib
+and 'instdir'/include, respectively, but can be changed using the configure script options
+--prefix, --exec-prefix, --includedir, and --libdir (see below). For example, a global
+installation of SUNDIALS on a *NIX system could be accomplished using
+
+ % ./configure --prefix=/opt/sundials-2.3.0
+
+Although all installed libraries reside under 'libdir', the public header files are further
+organized into subdirectories under 'includedir'.
+
+The installed libraries and exported header files are listed for reference in the table below.
+
+A typical user program need not explicitly include any of the shared SUNDIALS header files
+from under the 'includedir'/sundials directory since they are explicitly included by the
+appropriate solver header files (e.g., cvode_dense.h includes sundials_dense.h).
+However, it is both legal and safe to do so (e.g., the functions declared in
+sundials_smalldense.h could be used in building a preconditioner.
+
+The SUNDIALS libraries and header files are summarized below (names are relative to 'libdir'
+for libraries and to 'includedir' for header files)
+
+SHARED module
+ header files: sundials/sundials_types.h sundials/sundials_math.h
+ sundials/sundials_config.h sundias/sundials_nvector.h
+ sundials/sunials_smalldense.h sundials/sundials_dense.h
+ sundials/sundials_iterative.h sundials/sundials_band.h
+ sundials/sundials_spbcgs.h sundials/sundials_sptfqmr.h
+ sundials/sundials_spgmr.h
+
+
+NVECTOR_SERIAL module
+ libraries: libsundials_nvecserial.{a,so} libsundials_fnvecserial.a
+ header files: nvector/nvector_serial.h
+
+
+NVECTOR_PARALLEL module
+ libraries: libsundials_nvecparallel.{a,so} libsundials_fnvecparallel.a
+ header files: nvector/nvector_parallel.h
+
+
+CVODE module
+ libraries: libsundials_cvode.{a,so} libsundials_fcvode.a
+ header files: cvode/cvode.h
+ cvode/cvode_dense.h cvode/cvode_band.h
+ cvode/cvode_diag.h cvode/cvode_spils.h
+ cvode/cvode_bandpre.h cvode/cvode_bbdpre.h
+ cvode/cvode_spgmr.h cvode/cvode_spbcgs.h
+ cvode/cvode_sptfqmr.h cvode/cvode_impl.h
+
+
+CVODES module
+ library: libsundials_cvodes.{a,so}
+ header files: cvodes/cvodes.h
+ cvodes/cvodes_dense.h cvodes/cvodes_band.h
+ cvodes/cvodes_diag.h cvodes/cvodes_spils.h
+ cvodes/cvodes_bandpre.h cvodes/cvodes_bbdpre.h
+ cvodes/cvodes_spgmr.h cvodes/cvodes_spbcgs.h
+ cvodes/cvodes_sptfqmr.h cvodes/cvodes_impl.h
+ cvodes/cvodea_impl.h
+
+IDA module
+ library: libsundials_ida.{a,so}
+ header files: ida/ida.h
+ ida/ida_dense.h ida/ida_band.h
+ ida/ida_spils.h ida/ida_spgmr.h
+ ida/ida_spbcgs.h ida/ida_sptfqmr.h
+ ida/ida_bbdpre.h ida/ida_impl.h
+
+
+KINSOL module
+ libraries: libsundials_kinsol.{a,so} libsundials_fkinsol.a
+ header files: kinsol/kinsol.h
+ kinsol/kinsol_dense.h kinsol/kinsol_band.h
+ kinsol/kinsol_spils.h kinsol/kinsol_spgmr.h
+ kinsol/kinsol_spbcgs.h kinsol/kinsol_sptfqmr.h
+ kinsol/kinsol_bbdpre.h kinsol/kinsol_impl.h
+
+
+
+3. Compilers and Options
+========================
+
+Some systems require unusual options for compilation or linking that the `configure'
+script does not know about. Run `./configure --help' for details on some of the
+pertinent environment variables.
+
+You can give `configure' initial values for these variables by setting them in the
+environment. You can do that on the command line like this:
+
+ % ./configure CC=gcc CFLAGS=-O2 F77=g77 FFLAGS=-O
+
+Here is a detailed description of the configure options that are pertinent to SUNDIALS.
+In what follows, 'build_tree' is the directory from where 'configure' was invoked.
+
+3.1. General options
+--------------------
+
+--help
+-h
+
+ print a summary of the options to `configure', and exit.
+
+--quiet
+--silent
+-q
+
+ do not print messages saying which checks are being made. To
+ suppress all normal output, redirect it to `/dev/null' (any error
+ messages will still be shown).
+
+
+--prefix=PREFIX
+
+ Location for architecture-independent files.
+ Default: PREFIX=/usr/local
+
+--exec-prefix=EPREFIX
+
+ Location for architecture-dependent files.
+ Default: EPREFIX=/usr/local
+
+--includedir=DIR
+
+ Alternate location for header files.
+ Default: DIR=PREFIX/include
+
+--libdir=DIR
+
+ Alternate location for libraries.
+ Default: DIR=EPREFIX/lib
+
+--disable-solver
+
+ Although each existing solver module is built by default, support for a
+ given solver can be explicitly disabled using this option.
+ The valid values for solver are: cvode, cvodes,
+ ida, and kinsol.
+
+--enable-examples
+
+ Available example programs are not built by default. Use this option
+ to enable compilation of all pertinent example programs. Upon completion of
+ the 'make' command, the example executables will be created under solver-specific
+ subdirectories of 'builddir'/examples:
+
+ 'builddir'/examples/'solver'/serial : serial C examples
+ 'builddir'/examples/'solver'/parallel : parallel C examples
+ 'builddir'/examples/'solver'/fcmix_serial : serial Fortran examples
+ 'builddir'/examples/'solver'/fcmix_parallel : parallel Fortran examples
+
+ Note: Some of these subdirectories may not exist depending upon the
+ solver and/or the configuration options given.
+
+--with-examples-instdir=EXINSTDIR
+
+ Alternate location for example executables and sample output files (valid only if
+ examples are enabled). Note that installtion of example files can be completely disabled
+ by issuing EXINSTDIR=no (in case building the examples is desired only as a test of
+ the sundials libraries).
+
+ Default: DIR=EPREFIX/examples
+
+--with-cppflags=ARG
+
+ Specify additional C preprocessor flags
+ (e.g., ARG=-I<include_dir> if necessary header files are located in nonstandard locations).
+
+--with-cflags=ARG
+
+ Specify additional C compilation flags.
+
+--with-ldflags=ARG
+
+ Specify additional linker flags
+ (e.g., ARG=-L<lib_dir> if required libraries are located in nonstandard locations).
+
+--with-libs=ARG
+
+ Specify additional libraries to be used
+ (e.g., ARG=-l<foo> to link with the library named libfoo.a or libfoo.so).
+
+--with-precision=ARG
+
+ By default, sundials will define a real number (internally referred to as
+ realtype) to be a double-precision floating-point numeric data type (double
+ C-type); however, this option may be used to build sundials with realtype
+ alternatively defined as a single-precision floating-point numeric data type
+ (float C-type) if ARG=single, or as a long double C-type if ARG=extended.
+
+ Default: ARG=double
+
+3.2. Options for Fortran support
+--------------------------------
+
+--disable-f77
+
+ Using this option will disable all F77 support. The fcvode, fkinsol and
+ fnvector modules will not be built regardless of availability.
+
+--with-fflags=ARG
+
+ Specify additional F77 compilation flags.
+
+
+The configuration script will attempt to automatically determine the
+function name mangling scheme required by the specified F77 compiler, but the
+following two options may be used to override the default behavior.
+
+--with-f77underscore=ARG
+
+ This option pertains to the fkinsol, fcvode and fnvector F/C interface
+ modules and is used to specify the number of underscores to append to
+ function names so F77 routines can properly link with the associated
+ sundials libraries. Valid values for ARG are: none, one, and two.
+
+ Default: ARG=one
+
+--with-f77case=ARG
+
+ Use this option to specify whether the external names of the fkinsol,
+ fcvode and fnvector F/C interface functions should be lowercase
+ or uppercase so F77 routines can properly link with the associated sundials
+ libraries. Valid values for ARG are: lower and upper.
+
+ Default: ARG=lower
+
+3.3. Options for MPI support
+----------------------------
+
+The following configuration options are only applicable to the parallel sundials packages:
+
+
+--disable-mpi
+
+ Using this option will completely disable MPI support.
+
+--with-mpicc=ARG
+--with-mpif77=ARG
+
+ By default, the configuration utility script will use the MPI compiler
+ scripts named mpicc and mpif77 to compile the parallelized
+ sundials subroutines; however, for reasons of compatibility, different
+ executable names may be specified via the above options. Also, ARG=no
+ can be used to disable the use of MPI compiler scripts, thus causing
+ the serial C and F compilers to be used to compile the parallelized
+ sundials functions and examples.
+
+--with-mpi-root=MPIDIR
+
+ This option may be used to specify which MPI implementation should be used.
+ The sundials configuration script will automatically check under the
+ subdirectories MPIDIR/include and MPIDIR/lib for the necessary
+ header files and libraries. The subdirectory MPIDIR/bin will also be
+ searched for the C and F MPI compiler scripts, unless the user uses
+ --with-mpicc=no or --with-mpif77=no.
+
+--with-mpi-incdir=INCDIR
+--with-mpi-libdir=LIBDIR
+--with-mpi-libs=LIBS
+
+ These options may be used if the user would prefer not to use a preexisting
+ MPI compiler script, but instead would rather use a serial complier and
+ provide the flags necessary to compile the MPI-aware subroutines in
+ sundials.
+
+ Often an MPI implementation will have unique library names and so it may
+ be necessary to specify the appropriate libraries to use (e.g., LIBS=-lmpich).
+
+ Default: INCDIR=MPIDIR/include, LIBDIR=MPIDIR/lib and LIBS=-lmpi
+
+--with-mpi-flags=ARG
+
+ Specify additional MPI-specific flags.
+
+3.4. Options for library support
+--------------------------------
+
+By default, only static libraries are built, but the following option
+may be used to build shared libraries on supported platforms.
+
+--enable-shared
+
+ Using this particular option will result in both static and shared versions
+ of the available sundials libraries being built if the systsupports
+ shared libraries. To build only shared libraries also specify --disable-static.
+
+ Note: The fcvode and fkinsol libraries can only be built as static
+ libraries because they contain references to externally defined symbols, namely
+ user-supplied F77 subroutines. Although the F77 interfaces to the serial and
+ parallel implementations of the supplied nvector module do not contain any
+ unresolvable external symbols, the libraries are still built as static libraries
+ for the purpose of consistency.
+
+3.5. Options for Matlab support
+-------------------------------
+
+The following options are relevant only for configuring and building
+the sundialsTB Matlab toolbox:
+
+--enable-sundialsTB
+
+ The sundialsTB Matlab toolbox is not built by default. Use this option
+ to enable configuration and compilation of the mex files. Upon completion
+ of the make command, the following mex files will be created:
+ 'builddir'/sundialsTB/cvodes/cvm/cvm.'mexext'
+ 'builddir'/sundialsTB/idas/idm/idm.'mexext'
+ 'builddir'/sundialsTB/kinsol/kim/kim.'mexext'
+ where 'mexext' is the platform-specific extension of mex files.
+
+--with-sundialsTB-instdir=STBINSTDIR
+
+ Alternate location for the installed sundialsTB toolbox (valid only if
+ sundialsTB is enabled). As for the example programs, installation of sundialsTB can
+ be completely disabled by issuing STBINSTDIR=no (in case building the toolbox is desired
+ but its installtion will be done manually afterwards). Otherwise, all required sundialsTB
+ files will be installed under the directory STBINSTDIR/sundialsTB.
+
+ Default: DIR=MATLAB/toolbox (see below for the definition of MATLAB).
+
+--with-matlab=MATLAB
+
+ This option can be used to specify the locatin of the Matlab installation.
+ If it is defined, the environment variable MATLAB will be used. Otherwise,
+ the configure system will attempt to infer MATLAB from the location of
+ the matlab executable.
+
+--with-mex=ARG
+
+ Specify the mex compiler to be used.
+
+ Default: ARG=MATLAB/bin/mex
+
+--with-mexopts=ARG
+
+ Specify the mex options file to be used.
+
+ Default: Standard Matlab mex options file.
+
+3.6. Environment variables
+--------------------------
+
+The following environment variables can be locally (re)defined for use during the
+configuration of sundials. See the next section for illustrations of these.
+
+CC
+
+F77
+
+ Since the configuration script uses the first C and F77 compilers found in
+ the current executable search path, then each relevant shell variable (CC
+ and F77) must be locally (re)defined in order to use a different compiler.
+ For example, to use xcc (executable name of chosen compiler) as the C
+ language compiler, use CC=xcc in the configure step.
+
+CFLAGS
+
+FFLAGS
+
+ Use these environment variables to override the default C and F77 compilation flags.
+
+4. Configuration examples
+=========================
+
+The following examples are meant to help demonstrate proper usage of the configure options.
+
+To build SUNDIALS using the default C and Fortran compilers, and default mpicc and mpif77
+parallel compilers, enable compilation of examples, build the Matlab mex files for sundialsTB,
+and install it under /home/myname/matlab/sundialsTB, use
+
+ % ./configure --prefix=/home/myname/sundials --enable-examples \
+ --enable-sundialsTB --with-sundialsTB-instdir=/home/myname/matlab
+
+To disable installation of the examples, use:
+ % ./configure --prefix=/home/myname/sundials \
+ --enable-examples --with-examples-instdir=no \
+ --enable-sundialsTB --with-sundialsTB-instdir=/home/myname/matlab
+
+
+The following example builds SUNDIALS using gcc as the serial C compiler, g77 as the serial
+Fortran compiler, mpicc as the parallel C compiler, mpif77 as the parallel Fortran compiler,
+and appends the -g3 compilaton flag to the list of default flags:
+
+ % ./configure CC=gcc F77=g77 --with-cflags=-g3 --with-fflags=-g3 \
+ --with-mpicc=/usr/apps/mpich/1.2.4/bin/mpicc \
+ --with-mpif77=/usr/apps/mpich/1.2.4/bin/mpif77
+
+The next example again builds SUNDIALS using gcc as the serial C compiler, but the
+--with-mpicc=no option explicitly disables the use of the corresponding MPI compiler
+script. In addition, since the --with-mpi-root option is given, the compilation flags
+-I/usr/apps/mpich/1.2.4/include and -L/usr/apps/mpich/1.2.4/lib are passed to gcc when
+compiling the MPI-enabled functions.
+The --disable-examples option explicitly disables the examples (which means a Fortran
+compiler is not required).
+The --with-mpi-libs option is required so that the configure script can check if gcc
+can link with the appropriate MPI library.
+
+ % ./configure CC=gcc --disable-examples --with-mpicc=no \
+ --with-mpi-root=/usr/apps/mpich/1.2.4 \
+ --with-mpi-libs=-lmpich
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/LICENSE.txt b/SimTKmath/Integrators/src/CPodes/sundials/LICENSE.txt
new file mode 100644
index 0000000..ecfe265
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/LICENSE.txt
@@ -0,0 +1,63 @@
+Copyright (c) 2002, The Regents of the University of California.
+Produced at the Lawrence Livermore National Laboratory.
+Written by S.D. Cohen, A.C. Hindmarsh, R. Serban,
+ D. Shumaker, and A.G. Taylor.
+UCRL-CODE-155951 (CVODE)
+UCRL-CODE-155950 (CVODES)
+UCRL-CODE-155952 (IDA)
+UCRL-CODE-155953 (KINSOL)
+All rights reserved.
+
+This file is part of SUNDIALS.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+1. Redistributions of source code must retain the above copyright
+notice, this list of conditions and the disclaimer below.
+
+2. Redistributions in binary form must reproduce the above copyright
+notice, this list of conditions and the disclaimer (as noted below)
+in the documentation and/or other materials provided with the
+distribution.
+
+3. Neither the name of the UC/LLNL nor the names of its contributors
+may be used to endorse or promote products derived from this software
+without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+REGENTS OF THE UNIVERSITY OF CALIFORNIA, THE U.S. DEPARTMENT OF ENERGY
+OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+Additional BSD Notice
+---------------------
+1. This notice is required to be provided under our contract with
+the U.S. Department of Energy (DOE). This work was produced at the
+University of California, Lawrence Livermore National Laboratory
+under Contract No. W-7405-ENG-48 with the DOE.
+
+2. Neither the United States Government nor the University of
+California nor any of their employees, makes any warranty, express
+or implied, or assumes any liability or responsibility for the
+accuracy, completeness, or usefulness of any information, apparatus,
+product, or process disclosed, or represents that its use would not
+infringe privately-owned rights.
+
+3. Also, reference herein to any specific commercial products,
+process, or services by trade name, trademark, manufacturer or
+otherwise does not necessarily constitute or imply its endorsement,
+recommendation, or favoring by the United States Government or the
+University of California. The views and opinions of authors expressed
+herein do not necessarily state or reflect those of the United States
+Government or the University of California, and shall not be used for
+advertising or product endorsement purposes.
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/README.txt b/SimTKmath/Integrators/src/CPodes/sundials/README.txt
new file mode 100644
index 0000000..ea08de8
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/README.txt
@@ -0,0 +1,53 @@
+ SUNDIALS
+ SUite of Nonlinear and DIfferential/ALgebraic equation Solvers
+ Release 2.3.0, November 2006
+ Aaron Collier, Alan Hindmarsh, and Radu Serban
+ Center for Applied Scientific Computing, LLNL
+
+
+The family of solvers referred to as SUNDIALS consists of solvers CVODE
+(for ODE systems), CVODES (ODE with sensitivity analysis capabilities),
+IDA (for differential-algebraic systems), and KINSOL (for nonlinear
+algebraic systems).
+
+The various solvers of this family share many subordinate modules.
+For this reason, it is organized as a family, with a directory structure
+that exploits that sharing. Each individual solver includes documentation
+on installation, along with full usage documentation.
+
+The following is a list of the solver packages presently available.
+
+CVODE: A solver for stiff and nonstiff ODE systems y' = f(t,y).
+
+CVODES: A solver for stiff and non-stiff ODE systems with sensitivity
+ analysis capabilities.
+
+IDA: A solver for differential-algebraic systems F(t,y,y') = 0.
+
+KINSOL: A solver for nonlinear algebraic systems F(u) = 0.
+
+Warning to users who receive more than one of these individual solvers
+at different times: The mixing of old and new versions SUNDIALS may fail.
+To avoid such failures, obtain all desired solvers at the same time.
+
+For installation directions see the file INSTALL_NOTES.
+
+For additional information on a particular solver, see the README file
+in the solver directory (e.g. src/cvode/README).
+
+ Release history
+
++----------+------------------------------------------------------+
+| | SUNDIALS | Solver version |
+| Date | +----------+----------+----------+----------+
+| | release | CVODE | CVODES | IDA | KINSOL |
++----------+----------+----------+----------+----------+----------+
+| Jul 2002 | 1.0 | 2.0 | 1.0 | 2.0 | 2.0 |
+| Dec 2004 | 2.0 | 2.2.0 | 2.1.0 | 2.2.0 | 2.2.0 |
+| Jan 2005 | 2.0.1 | 2.2.1 | 2.1.1 | 2.2.1 | 2.2.1 |
+| Mar 2005 | 2.0.2 | 2.2.2 | 2.1.2 | 2.2.2 | 2.2.2 |
+| Apr 2005 | 2.1.0 | 2.3.0 | 2.2.0 | 2.3.0 | 2.3.0 |
+| May 2005 | 2.1.1 | 2.3.0 | 2.3.0 | 2.3.0 | 2.3.0 |
+| Mar 2006 | 2.2.0 | 2.4.0 | 2.4.0 | 2.4.0 | 2.4.0 |
+| Nov 2006 | 2.3.0 | 2.5.0 | 2.5.0 | 2.5.0 | 2.5.0 |
++----------+----------+----------+----------+----------+----------+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes.h b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes.h
new file mode 100644
index 0000000..836d3d5
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes.h
@@ -0,0 +1,1288 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.6 $
+ * $Date: 2007/03/20 14:33:17 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the interface file for the main CPODES integrator.
+ * -----------------------------------------------------------------
+ *
+ * CPODES is used to solve numerically the ordinary initial value
+ * problem with invariants:
+ *
+ * y' = f(t,y) or F(t,y,y') = 0
+ * c(t,y) = 0 c(t,y) = 0
+ * y(t0) = y0 y(t0) = y0; y'(t0) = yp0
+ *
+ * where t0, y0, yp0 in R^N, f: R x R^N -> R^N, F: R x R^N x R^N -> R^N
+ * and c: R x R^N -> R^M.
+ *
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _CPODES_H
+#define _CPODES_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <stdio.h>
+#include <sundials/sundials_nvector.h>
+
+/*
+ * =================================================================
+ * C P O D E C O N S T A N T S
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Inputs to CPodeCreate, CPodeInit, CPodeReInit, and CPode.
+ * -----------------------------------------------------------------
+ * Symbolic constants for the lmm_type and nls_type inputs to
+ * CPodeCreate, the ode_type and tol_type inputs to CPodeInit and
+ * CPodeReInit, the cnstr_type input to CPodeProjInit, as well as
+ * the input mode to CPode, are given below.
+ *
+ * lmm_type: The user of the CPODES package specifies whether to use
+ * the CP_ADAMS (Adams-Moulton) or CP_BDF (Backward Differentiation
+ * Formula) linear multistep method. The BDF method is recommended
+ * for stiff problems, and the CP_ADAMS method is recommended for
+ * nonstiff problems.
+ *
+ * nls_type: At each internal time step, a nonlinear equation must
+ * be solved. The user can specify either CP_FUNCTIONAL iteration,
+ * which does not require linear algebra, or a CP_NEWTON iteration,
+ * which requires the solution of linear systems. In the CP_NEWTON
+ * case, the user also specifies a CPODES linear solver.
+ * CP_NEWTON is recommended in case of stiff problems.
+ *
+ * ode_type: The ODE system can be given either in explicit form,
+ * y' = f(t,y) (in which case ode = CP_EXPL) or in implicit form
+ * (in which case ode = CP_IMPL).
+ *
+ * tol_type: This parameter specifies the relative and absolute
+ * tolerance types to be used. The CP_SS tolerance type means a
+ * scalar relative and absolute tolerance. The CP_SV tolerance type
+ * means a scalar relative tolerance and a vector absolute tolerance
+ * (a potentially different absolute tolerance for each vector
+ * component). The CP_WF tolerance type means that the user provides
+ * a function (of type CPEwtFn) to set the error weight vector.
+ *
+ * proj_type: If performing projection on the invariant manifold,
+ * this parameter specifies whether to use the internal algorithm
+ * or a user-provided projection function. The valid values are
+ * CP_PROJ_USER and CP_PROJ_INERNAL.
+ * A value proj_type = CP_PROJ_USER indicates that a function to
+ * perform the projection is supplied by the user.
+ * A value proj_type = CP_PROJ_INTERNAL indicates that the internal
+ * CPODES projection algorithm is to be used. In this case, the
+ * user must specify the constraint function and a linear solver
+ * to be used.
+ *
+ * proj_norm: Type of the norm in which projection is to be performed.
+ * The valid values are CP_PROJ_L2NORM (in which case projection
+ * is done in Euclidian norm) and CP_PROJ_ERRNORM (in which case
+ * the projection is done in WRMS norm).
+ *
+ * cnstr_type: If the internal projection algorithm is used, then
+ * cnstr_type specifies the type of constraints.
+ * If the constraints are linear (cnstr_type = CP_CNSTR_LIN),
+ * then an improved error estimate for the projected method
+ * (obtained by projecting the error estimate for the original
+ * method) is available at no additional cost and will always be used.
+ * If the constraints are nonlinear (cnstr_type = CP_CNSTR_NONLIN),
+ * obtaining the global projection operator requires an additional
+ * evaluation of the constraint Jacobian and constructing its
+ * More-Penrose pseudo-inverse. In this case, the default is
+ * to use the error estimate of the unprojected method (unless
+ * indicated otherwise by the user).
+ *
+ * mode: The mode input parameter to CPode indicates the job of the
+ * solver for the next user step. The CP_NORMAL mode is to have
+ * the solver take internal steps until it has reached or just
+ * passed the user specified tout parameter. The solver then
+ * interpolates in order to return an approximate value of y(tout).
+ * The CP_ONE_STEP option tells the solver to just take one internal
+ * step and return the solution at the point reached by that step.
+ * The CP_NORMAL_TSTOP and CP_ONE_STEP_TSTOP modes are similar to
+ * CP_NORMAL and CP_ONE_STEP, respectively, except that the
+ * integration never proceeds past the value tstop (specified
+ * through the routine CPodeSetStopTime).
+ * -----------------------------------------------------------------
+ */
+
+/* lmm_type */
+#define CP_ADAMS 1
+#define CP_BDF 2
+
+/* nls_type */
+#define CP_FUNCTIONAL 1
+#define CP_NEWTON 2
+
+/* ode_type */
+#define CP_EXPL 1
+#define CP_IMPL 2
+
+/* tol_type */
+#define CP_SS 1
+#define CP_SV 2
+#define CP_WF 3
+
+/* proj_type */
+#define CP_PROJ_USER 1
+#define CP_PROJ_INTERNAL 2
+
+/* proj_norm */
+#define CP_PROJ_L2NORM 1
+#define CP_PROJ_ERRNORM 2
+
+/* cnstr_type */
+#define CP_CNSTR_LIN 1
+#define CP_CNSTR_NONLIN 2
+
+/* mode */
+#define CP_NORMAL 1
+#define CP_ONE_STEP 2
+#define CP_NORMAL_TSTOP 3
+#define CP_ONE_STEP_TSTOP 4
+
+/*
+ * ----------------------------------------
+ * CPODES return flags
+ * ----------------------------------------
+ */
+
+#define CP_SUCCESS 0
+#define CP_TSTOP_RETURN 1
+#define CP_ROOT_RETURN 2
+
+#define CP_WARNING 99
+
+#define CP_TOO_MUCH_WORK -1
+#define CP_TOO_MUCH_ACC -2
+#define CP_ERR_FAILURE -3
+#define CP_CONV_FAILURE -4
+
+#define CP_LINIT_FAIL -5
+#define CP_LSETUP_FAIL -6
+#define CP_LSOLVE_FAIL -7
+#define CP_ODEFUNC_FAIL -8
+#define CP_FIRST_ODEFUNC_ERR -9
+#define CP_REPTD_ODEFUNC_ERR -10
+#define CP_UNREC_ODEFUNC_ERR -11
+#define CP_RTFUNC_FAIL -12
+
+#define CP_MEM_FAIL -20
+#define CP_MEM_NULL -21
+#define CP_ILL_INPUT -22
+#define CP_NO_MALLOC -23
+#define CP_BAD_K -24
+#define CP_BAD_T -25
+#define CP_BAD_DKY -26
+#define CP_TOO_CLOSE -27
+
+#define CP_NO_QUAD -30
+#define CP_QUADFUNC_FAIL -31
+#define CP_FIRST_QUADFUNC_ERR -32
+#define CP_REPTD_QUADFUNC_ERR -33
+#define CP_UNREC_QUADFUNC_ERR -34
+
+#define CP_BAD_IS -40
+#define CP_NO_SENS -41
+#define CP_SENSFUNC_FAIL -42
+#define CP_FIRST_SENSFUNC_ERR -43
+#define CP_REPTD_SENSFUNC_ERR -44
+#define CP_UNREC_SENSFUNC_ERR -45
+
+#define CP_PLINIT_FAIL -50
+#define CP_PLSETUP_FAIL -51
+#define CP_PLSOLVE_FAIL -52
+#define CP_CNSTRFUNC_FAIL -53
+#define CP_PROJFUNC_FAIL -54
+#define CP_PROJ_FAILURE -55
+#define CP_REPTD_CNSTRFUNC_ERR -56
+#define CP_REPTD_PROJFUNC_ERR -57
+
+#define CP_FIRST_CNSTRFUNC_ERR -60
+#define CP_NO_RECOVERY -61
+#define CP_LINESEARCH_FAIL -62
+
+/*
+ * =================================================================
+ * F U N C T I O N T Y P E S
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Type : CPRhsFn
+ * -----------------------------------------------------------------
+ * If the ODE is given in explicit form, the function which defines
+ * the right-hand side of the ODE system y'=f(t,y) must have type
+ * CPRhsFn.
+ * Such a function takes as input the independent variable value t,
+ * and the dependent variable vector y. It stores the result of
+ * f(t,y) in the vector fout. The y and fout arguments are of type
+ * N_Vector. (Allocation of memory for ydot is handled within CODES)
+ * The f_data parameter is the same as the f_data parameter set by
+ * the user through the CPodeInit or CPodeReInit functions. This
+ * user-supplied pointer is passed to the user's fun function every
+ * time it is called.
+ *
+ * A CPRhsFn should return 0 if successful, a negative value if
+ * an unrecoverable error occured, and a positive value if a
+ * recoverable error (e.g. invalid y values) occured.
+ * If an unrecoverable occured, the integration is halted.
+ * If a recoverable error occured, then (in most cases) CPODES
+ * will try to correct and retry.
+ * -----------------------------------------------------------------
+ */
+
+typedef int (*CPRhsFn)(realtype t, N_Vector y,
+ N_Vector fout, void *f_data);
+
+/*
+ * ----------------------------------------------------------------
+ * Type : CPResFn
+ * ----------------------------------------------------------------
+ * If the ODE is given in implicit form, the function which
+ * defines the ODE system F(t,y,y')=0 must have type CPResFn.
+ *
+ * A CPResFn takes as input the independent variable value t,
+ * the dependent variable vector y, and the derivative (with
+ * respect to t) of the y vector, yp. It stores the result of
+ * F(t,y,y') in the vector fout. The y, yp, and fout arguments are
+ * of type N_Vector. The f_data parameter is the pointer f_data
+ * passed by the user to the CPodeInit or CPodeReInit functions.
+ * This user-supplied pointer is passed to the user's fun function
+ * every time it is called.
+ *
+ * A CPResFn function should return a value of 0 if successful, a
+ * positive value if a recoverable error occured (e.g. y has an
+ * illegal value), or a negative value if a nonrecoverable error
+ * occured. In the latter case, the program halts. If a recoverable
+ * error occured, then (in most cases) the integrator will attempt
+ * to correct and retry.
+ * ----------------------------------------------------------------
+ */
+
+typedef int (*CPResFn)(realtype t, N_Vector y, N_Vector yp,
+ N_Vector fout, void *f_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Type : CPCnstrFn
+ * -----------------------------------------------------------------
+ * The function cfun defining the invariant constraints c(t,y) = 0
+ * must have type CPCnstrFn.
+ *
+ * A CPCnstrFn takes as input the independent variable value t and
+ * the dependent variable vector y. It stores the result of c(t,y)
+ * in the vector cout. The y and cout arguments are of type
+ * N_Vector. (Allocation of memory for cout is handled within CPODES)
+ * The c_data parameter is the same as the c_data parameter set by
+ * the user through the CPodeProjDefineConstraints routine.
+ * This user-supplied pointer is passed to the user's cfun function
+ * every time it is called.
+ *
+ * A CPCnstrFn should return 0 if successful, a negative value if
+ * an unrecoverable error occured, and a positive value if a
+ * recoverable error (e.g. invalid y values) occured.
+ * If an unrecoverable occured, the integration is halted.
+ * If a recoverable error occured, then (in most cases) CPODES
+ * will try to correct and retry.
+ * -----------------------------------------------------------------
+ */
+
+typedef int (*CPCnstrFn)(realtype t, N_Vector y,
+ N_Vector cout, void *c_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Type : CPProjFn
+ * -----------------------------------------------------------------
+ * A user-supplied function to performs the projection onto the
+ * invariant manifold c(t,y)=0, must have type CPProjFn.
+ *
+ * A CPProjFn takes as input the independent variable t and the
+ * current (corrected) variable vector ycur.
+ * It must compute a correction vector corr such that
+ * y = ycurr + corr lies on the manifold (i.e. c(t,y)=0).
+ * The value epsProj is provided to be used in the stopping test
+ * of a nonlinear solver iteration (the iterations should be
+ * terminated when the WRMS norm of the current iterate update
+ * is below epsProj).
+ *
+ * Note that, if the projection is orthogonal then it can be written as
+ * y = P * ycur + alpha(t), with P^2 = P,
+ * then the projected error estimate is
+ * errP = P * err
+ * and ||errP|| <= ||err||.
+ * The vector err contains a (scaled) version of the current error
+ * estimate. CPProjFn may also compute the projected error estimate
+ * and overwrite the vector err with errP. Otherwise, it should leave
+ * err unchanged.
+ *
+ * If err is NULL, a CPProjFn function should attempt NO projection
+ * of the error estimate (in this case, it was called from within
+ * the computation of consistent initial conditions).
+ *
+ * A CPProjFn should return 0 if successful, a negative value if
+ * an unrecoverable error occured, and a positive value if a
+ * recoverable error (e.g. invalid y values) occured.
+ * If an unrecoverable occured, the integration is halted.
+ * If a recoverable error occured, then (in most cases) CPODES
+ * will try to correct and retry.
+ * -----------------------------------------------------------------
+ * NOTE: If the user's projection routine needs other quantities,
+ * they are accessible as follows: the error weight vector
+ * can be obtained by calling CPodeGetErrWeights. The unit
+ * roundoff is available as UNIT_ROUNDOFF defined in
+ * sundials_types.h.
+ * -----------------------------------------------------------------
+ */
+
+typedef int (*CPProjFn)(realtype t, N_Vector ycur, N_Vector corr,
+ realtype epsProj, N_Vector err, void *pdata);
+
+/*
+ * -----------------------------------------------------------------
+ * Type : CPQuadFn
+ * -----------------------------------------------------------------
+ * The qfun function which defines the right hand side of the
+ * quadrature equations q' = fQ(t,y) must have type CPQuadFn.
+ * It takes as input the value of the independent variable t and
+ * the vector of states y and must store the result of fQ in qout.
+ * (Allocation of memory for qout is handled by CPODES).
+ * The q_data parameter is the same as the q_data parameter
+ * set by the user through the CPodeQuadInit function and is
+ * passed to the qfun function every time it is called.
+ *
+ * A CPQuadFn should return 0 if successful, a negative value if
+ * an unrecoverable error occured, and a positive value if a
+ * recoverable error (e.g. invalid y values) occured.
+ * If an unrecoverable occured, the integration is halted.
+ * If a recoverable error occured, then (in most cases) CPODES
+ * will try to correct and retry.
+ * -----------------------------------------------------------------
+ */
+
+typedef int (*CPQuadFn)(realtype t, N_Vector y,
+ N_Vector qout, void *q_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Type : CPRootFn
+ * -----------------------------------------------------------------
+ * A function g, which defines a set of functions g_i(t,y,y') whose
+ * roots are sought during the integration, must have type CPRootFn.
+ * The function g takes as input the independent variable value
+ * t, the dependent variable vector y, and its derivative yp=y'.
+ * It stores the nrtfn values g_i(t,y,y') in the realtype array gout.
+ * (Allocation of memory for gout is handled within CPODES.)
+ * The g_data parameter is the same as that passed by the user
+ * to the CPodeRootInit routine. This user-supplied pointer is
+ * passed to the user's g function every time it is called.
+ *
+ * A CPRootFn should return 0 if successful or a non-zero value
+ * if an error occured (in which case the integration will be halted).
+ * -----------------------------------------------------------------
+ */
+
+typedef int (*CPRootFn)(realtype t, N_Vector y, N_Vector yp,
+ realtype *gout, void *g_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Type : CPEwtFn
+ * -----------------------------------------------------------------
+ * A function e, which sets the error weight vector ewt, must have
+ * type CPEwtFn.
+ * The function e takes as input the current dependent variable y.
+ * It must set the vector of error weights used in the WRMS norm:
+ *
+ * ||y||_WRMS = sqrt [ 1/N * sum ( ewt_i * y_i)^2 ]
+ *
+ * Typically, the vector ewt has components:
+ *
+ * ewt_i = 1 / (reltol * |y_i| + abstol_i)
+ *
+ * The e_data parameter is the same as that passed by the user
+ * to the CPodeSetEwtFn routine. This user-supplied pointer is
+ * passed to the user's e function every time it is called.
+ * A CPEwtFn e must return 0 if the error weight vector has been
+ * successfuly set and a non-zero value otherwise.
+ * -----------------------------------------------------------------
+ */
+
+typedef int (*CPEwtFn)(N_Vector y, N_Vector ewt, void *e_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Type : CPErrHandlerFn
+ * -----------------------------------------------------------------
+ * A function eh, which handles error messages, must have type
+ * CPErrHandlerFn.
+ * The function eh takes as input the error code, the name of the
+ * module reporting the error, the error message, and a pointer to
+ * user data, the same as that passed to CPodeSetErrHandlerFn.
+ *
+ * All error codes are negative, except CP_WARNING which indicates
+ * a warning (the solver continues).
+ *
+ * A CPErrHandlerFn has no return value.
+ * -----------------------------------------------------------------
+ */
+
+typedef void (*CPErrHandlerFn)(int error_code,
+ const char *module, const char *function,
+ char *msg, void *eh_data);
+
+/*
+ * =================================================================
+ * U S E R - C A L L A B L E F U N C T I O N S
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPodeCreate
+ * -----------------------------------------------------------------
+ * CPodeCreate creates an internal memory block for a problem to
+ * be solved by CPODES.
+ *
+ * ode_type - form in which the ODE system is provided.
+ * The legal values are CP_EXPL or CP_IMPL (see above).
+ *
+ * lmm_type - type of linear multistep method to be used.
+ * The legal values are CP_ADAMS and CP_BDF (see above).
+ *
+ * nls_type - type of iteration used to solve the nonlinear
+ * system that arises during each internal time step.
+ * The legal values are CP_FUNCTIONAL and CP_NEWTON
+ * for ode_type = CP_EXPL and only CP_NEWTON for
+ * ode_type = CP_IMPL.
+ *
+ * If successful, CPodeCreate returns a pointer to initialized
+ * problem memory. This pointer should be passed to CPodeInit.
+ * If an initialization error occurs, CPodeCreate prints an error
+ * message to standard err and returns NULL.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void *CPodeCreate(int ode_type, int lmm_type, int nls_type);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPodeInit
+ * -----------------------------------------------------------------
+ * CPodeInit allocates and initializes memory for a problem to be
+ * solved by CPODES.
+ *
+ * cpode_mem - pointer to CPODES memory returned by CPodeCreate.
+ *
+ * fun - name of the C function defining the ODE system.
+ * Depending on the ODE type (see CPodeCreate), fun
+ * should be of type CPRhsFn (ode_type=CP_EXPL) or of
+ * type CPResFn (ode_type=CP_IMPL).
+ *
+ * f_data - pointer to user data that will be passed to the
+ * fun function every time it is called.
+ *
+ * t0 - initial value of t.
+ *
+ * y0 - initial condition vector y(t0).
+ *
+ * yp0 - initial condition vector y'(t0).
+ *
+ * tol_type - type of tolerances to be used. The legal values are:
+ * CP_SS (scalar relative and absolute tolerances),
+ * CP_SV (scalar relative tolerance and vector
+ * absolute tolerance).
+ * CP_WF (indicates that the user will provide a
+ * function to evaluate the error weights.
+ * In this case, reltol and abstol are ignored.)
+ *
+ * reltol - scalar relative tolerance scalar.
+ *
+ * abstol - pointer to the absolute tolerance scalar or
+ * an N_Vector of absolute tolerances.
+ *
+ * The parameters tol_type, reltol, and abstol define a vector of
+ * error weights, ewt, with components
+ * ewt[i] = 1/(reltol*abs(y[i]) + abstol) if tol_type = CP_SS
+ * ewt[i] = 1/(reltol*abs(y[i]) + abstol[i]) if tol_type = CP_SV.
+ * This vector is used in all error and convergence tests, which
+ * use a weighted RMS norm on all error-like vectors v:
+ * WRMSnorm(v) = sqrt( (1/N) sum(i=1..N) (v[i]*ewt[i])^2 ),
+ * where N is the problem dimension.
+ *
+ * Return flag:
+ * CP_SUCCESS if successful
+ * CP_MEM_NULL if the CPODES memory was NULL
+ * CP_MEM_FAIL if a memory allocation failed
+ * CP_ILL_INPUT f an argument has an illegal value.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPodeInit(void *cpode_mem,
+ void *fun, void *f_data,
+ realtype t0, N_Vector y0, N_Vector yp0,
+ int tol_type, realtype reltol, void *abstol);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPodeReInit
+ * -----------------------------------------------------------------
+ * CPodeReInit re-initializes CPODES for the solution of a problem,
+ * where a prior call to CPodeInit has been made with the same
+ * problem size N. CPodeReInit performs the same input checking
+ * and initializations that CPodeInit does.
+ * But it does no memory allocation, assuming that the existing
+ * internal memory is sufficient for the new problem.
+ *
+ * The use of CPodeReInit requires that the maximum method order,
+ * maxord, is no larger for the new problem than for the problem
+ * specified in the last call to CPodeInit. This condition is
+ * automatically fulfilled if the multistep method parameter lmm_type
+ * is unchanged (or changed from CP_ADAMS to CP_BDF) and the default
+ * value for maxord is specified.
+ *
+ * All of the arguments to CPodeReInit have names and meanings
+ * identical to those of CPodeInit.
+ *
+ * The return value of CPodeReInit is equal to CP_SUCCESS = 0 if
+ * there were no errors; otherwise it is a negative int equal to:
+ * CP_MEM_NULL indicating cpode_mem was NULL (i.e.,
+ * CPodeCreate has not been called).
+ * CP_NO_MALLOC indicating that cpode_mem has not been
+ * allocated (i.e., CPodeInit has not been
+ * called).
+ * CP_ILL_INPUT indicating an input argument was illegal
+ * (including an attempt to increase maxord).
+ * In case of an error return, an error message is also printed.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPodeReInit(void *cpode_mem,
+ void *fun, void *f_data,
+ realtype t0, N_Vector y0, N_Vector yp0,
+ int tol_type, realtype reltol, void *abstol);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPodeProjInit
+ * -----------------------------------------------------------------
+ * CPodeProjInit initializes the internal CPODES coordinate
+ * projection algorithm. It must be called after CPodeCreate and
+ * CPodeInit.
+ *
+ * The arguments are as follows:
+ *
+ * cpode_mem - pointer to CPODES memory returned by CPodeCreate.
+ *
+ * proj_norm - the norm in which projection is to be done. Legal
+ * values are CP_PROJ_L2NORM and CP_PROJ_ERRNORM.
+ *
+ * cnstr_type - the type of constraints.
+ * CP_CNSTR_LIN : linear constraints.
+ * CP_CNSTR_NONLIN: nonlinear constraints.
+ *
+ * cfun - name of the user-supplied function (type CPCnstrFn)
+ * defining the invariant manifold.
+ *
+ * c_data - pointer to user data that will be passed to the
+ * cfun function every time it is called.
+ *
+ * ctol - a vector of "absolute tolerances" for the constraints.
+ * In default operation, this vector is only used as
+ * a template for cloning other vectors. However, if
+ * enabled through CPodeSetProjTestCnstr, these values,
+ * together with reltol, are used to compute the
+ * constraint WL2 norm and a projection will be
+ * perfomed only if ||c(t)|_WL2 >= 1.0
+ *
+ * The return value of CPodeProjInit is equal to CP_SUCCESS = 0 if
+ * there were no errors; otherwise it is a negative int equal to:
+ * CP_MEM_NULL - cpode_mem was NULL
+ * (i.e., CPodeCreate has not been called).
+ * CP_NO_MALLOC - cpode_mem has not been allocated
+ * (i.e., CPodeInit has not been called).
+ * CP_MEM_FAIL - a memory allocation failed.
+ * CP_ILL_INPUT - an input argument was illegal.
+ * In case of an error return, an error message is also printed.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPodeProjInit(void *cpode_mem, int proj_norm,
+ int cnstr_type, CPCnstrFn cfun, void *c_data,
+ N_Vector ctol);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPodeProjDefine
+ * -----------------------------------------------------------------
+ * CPodeProjDefine initializes coordinate projection using a funciton
+ * provided by the user. It must be called after CPodeInit.
+ *
+ * The arguments are as follows:
+ *
+ * cpode_mem - pointer to CPODES memory returned by CPodeCreate.
+ *
+ * pfun - name of the user-supplied function (type CPProjFn)
+ * which will perform the projection.
+ *
+ * p_data - pointer to user data that will be passed to the
+ * pfun function every time it is called.
+ *
+ * The return value of CPodeProjDefine is CP_SUCCESS if there were
+ * no errors, or CP_MEM_NULL if the cpode_mem argument was NULL
+ * (i.e., CPodeCreate has not been called).
+ * In case of an error return, an error message is also printed.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPodeProjDefine(void *cpode_mem, CPProjFn pfun, void *p_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPodeQuadInit and CPodeQuadReInit
+ * -----------------------------------------------------------------
+ * CVodeQuadInit allocates and initializes memory related to
+ * quadrature integration.
+ *
+ * cpode_mem - pointer to CPODES memory returned by CPodeCreate
+ *
+ * qfun - the user-provided integrand routine.
+ *
+ * q_data - pointer to user data that will be passed to the
+ * qfun function every time it is called.
+ *
+ * q0 - N_Vector with initial values for quadratures
+ * (typically q0 has all zero components).
+ *
+ * CPodeQuadReInit re-initializes CPODES's quadrature related memory
+ * for a problem, assuming it has already been allocated in prior calls
+ * to CPodeInit and CPodeQuadInit. All problem specification inputs
+ * are checked for errors. The number of quadratures Nq is assumed to
+ * be unchanged since the previous call to CPodeQuadInit.
+ *
+ * Return values:
+ * CP_SUCCESS if successful
+ * CP_MEM_NULL if the CPODES memory was NULL
+ * CP_MEM_FAIL if a memory allocation failed
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPodeQuadInit(void *cpode_mem, CPQuadFn qfun, void *q_data, N_Vector q0);
+SUNDIALS_EXPORT int CPodeQuadReInit(void *cpode_mem, CPQuadFn qfun, void *q_data, N_Vector q0);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPodeRootInit
+ * -----------------------------------------------------------------
+ * CPodeRootInit initializes a rootfinding problem to be solved
+ * during the integration of the ODE system. It must be called
+ * after CPodeCreate, and before CPode. The arguments are:
+ *
+ * cpode_mem - pointer to CPODES memory returned by CPodeCreate.
+ *
+ * nrtfn - number of functions g_i, an int >= 0.
+ *
+ * gfun - name of user-supplied function, of type CPRootFn,
+ * defining the functions g_i whose roots are sought.
+ *
+ * g_data - pointer to user data that will be passed to the
+ * gfun function every time it is called.
+ *
+ * If a new problem is to be solved with a call to CPodeReInit,
+ * where the new problem has no root functions but the prior one
+ * did, then call CPodeRootInit with nrtfn = 0.
+ *
+ * The return value of CPodeRootInit is CP_SUCCESS = 0 if there were
+ * no errors; otherwise it is a negative int equal to:
+ * CP_MEM_NULL - indicating cpode_mem was NULL.
+ * CP_MEM_FAIL - indicating a memory allocation failed.
+ * CP_ILL_INPUT - indicating nrtfn > 0 but gfun = NULL.
+ * In case of an error return, an error message is also printed.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPodeRootInit(void *cpode_mem, int nrtfn, CPRootFn gfun, void *g_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPodeCalcIC
+ * -----------------------------------------------------------------
+ * CPodeCalcIC calculates corrected initial conditions that are
+ * consistent with the invariant constraints and (for implicit-form
+ * ODEs) with the ODE system itself. It first projects the initial
+ * guess for the state vector (given by the user through CPodeInit
+ * or CPodeReInit) and then, if necessary, computes a state derivative
+ * vector as solution of F(t0, y0, y0') = 0.
+ *
+ * Note: If the initial conditions satisfy both the constraints and
+ * (for implicit-form ODEs) the ODE itself, a call to CPodeCalcIC
+ * is NOT necessary.
+ *
+ * A call to CpodeCalcIC must be preceded by a successful call to
+ * CPodeInit or CPodeReInit for the given ODE problem, and by a
+ * successful call to the linear system solver specification
+ * routine.
+ *
+ * A call to CPodeCalcIC should precede the call(s) to CPode for
+ * the given problem.
+ *
+ * If successful, CPodeCalcIC stores internally the corrected
+ * initial conditions which will be used to start the integration
+ * at the first call to CPode.
+ *
+ * The only argument to CPodeCalcIC is the pointer to the CPODE
+ * memory block returned by CPodeCreate.
+ *
+ * The return value of CPodeCalcIC is one of the following:
+ *
+ * CP_SUCCESS
+ * CP_MEM_NULL
+ * CP_NO_MALLOC
+ * CP_ILL_INPUT
+ * CP_LINIT_FAIL
+ * CP_PLINIT_FAIL
+ * CP_FIRST_CNSTRFUNC_ERR
+ * CP_PROJ_FAILURE
+ * CP_CNSTRFUNC_FAIL
+ * CP_PROJFUNC_FAIL
+ * CP_PLSETUP_FAIL
+ * CP_PLSOLVE_FAIL
+ * CP_NO_RECOVERY
+ *
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPodeCalcIC(void *cpode_mem);
+
+/*
+ * -----------------------------------------------------------------
+ * Integrator optional input specification functions
+ * -----------------------------------------------------------------
+ * The following functions can be called to set optional inputs
+ * to values other than the defaults given below:
+ *
+ * Function | Optional input / [ default value ]
+ * -----------------------------------------------------------------
+ * |
+ * CPodeSetErrHandlerFn | user-provided ErrHandler function.
+ * | [internal]
+ * |
+ * CPodeSetErrFile | the file pointer for an error file
+ * | where all CPODES warning and error
+ * | messages will be written if the default
+ * | internal error handling function is used.
+ * | This parameter can be stdout (standard
+ * | output), stderr (standard error), or a
+ * | file pointer (corresponding to a user
+ * | error file opened for writing) returned
+ * | by fopen.
+ * | If not called, then all messages will
+ * | be written to the standard error stream.
+ * | [stderr]
+ * |
+ * -----------------------------------------------------------------
+ * |
+ * CPodeSetEwtFn | user-provided EwtSet function efun and
+ * | a pointer to user data that will be
+ * | passed to the user's efun function
+ * | every time efun is called.
+ * | [internal]
+ * | [NULL]
+ * |
+ * CPodeSetMaxOrd | maximum LMM order to be used by the
+ * | solver.
+ * | [12 for Adams , 5 for BDF]
+ * |
+ * CPodeSetMaxNumSteps | maximum number of internal steps to be
+ * | taken by the solver in its attempt to
+ * | reach tout.
+ * | [500]
+ * |
+ * CPodeSetMaxHnilWarns | maximum number of warning messages
+ * | issued by the solver that t+h==t on the
+ * | next internal step. A value of -1 means
+ * | no such messages are issued.
+ * | [10]
+ * |
+ * CPodeSetStabLimDet | flag to turn on/off stability limit
+ * | detection (TRUE = on, FALSE = off).
+ * | When BDF is used and order is 3 or
+ * | greater, CPsldet is called to detect
+ * | stability limit. If limit is detected,
+ * | the order is reduced.
+ * | [FALSE]
+ * |
+ * CPodeSetInitStep | initial step size.
+ * | [estimated by CPODES]
+ * |
+ * CPodeSetMinStep | minimum absolute value of step size
+ * | allowed.
+ * | [0.0]
+ * |
+ * CPodeSetMaxStep | maximum absolute value of step size
+ * | allowed.
+ * | [infinity]
+ * |
+ * CPodeSetStopTime | the independent variable value past
+ * | which the solution is not to proceed.
+ * | [infinity]
+ * |
+ * CPodeSetMaxErrTestFails | Maximum number of error test failures
+ * | in attempting one step.
+ * | [7]
+ * |
+ * -----------------------------------------------------------------
+ * |
+ * CPodeSetMaxNonlinIters | Maximum number of nonlinear solver
+ * | iterations at one solution.
+ * | [3]
+ * |
+ * CPodeSetMaxConvFails | Maximum number of convergence failures
+ * | allowed in attempting one step.
+ * | [10]
+ * |
+ * CPodeSetNonlinConvCoef | Coefficient in the nonlinear
+ * | convergence test.
+ * | [0.1]
+ * |
+ * -----------------------------------------------------------------
+ * |
+ * CPodeSetProjUpdateErrEst| toggles ON/OFF projection of the
+ * | error estimate.
+ * | [TRUE]
+ * |
+ * CPodeSetProjFrequency | frequency with which the projection
+ * | step is performed. A value of 1
+ * | indicates that the projection step
+ * | will be performed at every step.
+ * | A value of 0 will disable projection.
+ * | [1]
+ * |
+ * CPodeSetProjTestCnstr | if TRUE, the internal projection
+ * | function will be performed only if
+ * | the constraint violation is larger
+ * | than the prescribed tolerances.
+ * | Otherwise, the tolerances are ignored
+ * | and projection is always performed.
+ * | [FALSE]
+ * |
+ * CPodeSetProjLsetupFreq | frequency with which the linear
+ * | solver setup function is called
+ * | (i.e. frequency of constraint
+ * | Jacobian evaluations). The default
+ * | value of 1 forces a Jacobian
+ * | evaluation before every single
+ * | projection step.
+ * | [1]
+ * |
+ * CPodeSetProjNonlinConvCoef | Coefficient in the nonlinear
+ * | convergence test (for projection).
+ * | [0.1]
+ * |
+ * -----------------------------------------------------------------
+ * |
+ * CPodeSetQuadErrCon | are quadrature variables considered in
+ * | the error control?
+ * | If yes, set tolerances for quadrature
+ * | integration.
+ * | [errconQ = FALSE]
+ * | [no tolerances]
+ * |
+ * -----------------------------------------------------------------
+ * |
+ * CPodeSetTolerances | Changes the integration tolerances
+ * | between calls to CPode.
+ * | [set by CPodeInit/CPodeReInit]
+ * |
+ * ----------------------------------------------------------------
+ * |
+ * CPodeSetRootDirection | Specifies the direction of zero
+ * | crossings to be monitored
+ * | [both directions]
+ * |
+ * -----------------------------------------------------------------
+ * Return flag:
+ * CP_SUCCESS if successful
+ * CP_MEM_NULL if the CPODES memory is NULL
+ * CP_ILL_INPUT if an argument has an illegal value
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPodeSetErrHandlerFn(void *cpode_mem, CPErrHandlerFn ehfun, void *eh_data);
+SUNDIALS_EXPORT int CPodeSetErrFile(void *cpode_mem, FILE *errfp);
+
+SUNDIALS_EXPORT int CPodeSetEwtFn(void *cpode_mem, CPEwtFn efun, void *e_data);
+SUNDIALS_EXPORT int CPodeSetMaxOrd(void *cpode_mem, int maxord);
+SUNDIALS_EXPORT int CPodeSetMaxNumSteps(void *cpode_mem, long int mxsteps);
+SUNDIALS_EXPORT int CPodeSetMaxHnilWarns(void *cpode_mem, int mxhnil);
+SUNDIALS_EXPORT int CPodeSetStabLimDet(void *cpode_mem, booleantype stldet);
+SUNDIALS_EXPORT int CPodeSetInitStep(void *cpode_mem, realtype hin);
+SUNDIALS_EXPORT int CPodeSetMinStep(void *cpode_mem, realtype hmin);
+SUNDIALS_EXPORT int CPodeSetMaxStep(void *cpode_mem, realtype hmax);
+SUNDIALS_EXPORT int CPodeSetStopTime(void *cpode_mem, realtype tstop);
+SUNDIALS_EXPORT int CPodeSetMaxErrTestFails(void *cpode_mem, int maxnef);
+
+SUNDIALS_EXPORT int CPodeSetMaxNonlinIters(void *cpode_mem, int maxcor);
+SUNDIALS_EXPORT int CPodeSetMaxConvFails(void *cpode_mem, int maxncf);
+SUNDIALS_EXPORT int CPodeSetNonlinConvCoef(void *cpode_mem, realtype nlscoef);
+
+SUNDIALS_EXPORT int CPodeSetProjUpdateErrEst(void *cpode_mem, booleantype proj_err);
+SUNDIALS_EXPORT int CPodeSetProjFrequency(void *cpode_mem, long int proj_freq);
+SUNDIALS_EXPORT int CPodeSetProjTestCnstr(void *cpode_mem, booleantype test_cnstr);
+SUNDIALS_EXPORT int CPodeSetProjLsetupFreq(void *cpode_mem, long int proj_lset_freq);
+SUNDIALS_EXPORT int CPodeSetProjNonlinConvCoef(void *cpode_mem, realtype prjcoef);
+
+SUNDIALS_EXPORT int CPodeSetQuadErrCon(void *cpode_mem, booleantype errconQ,
+ int tol_typeQ, realtype reltolQ, void *abstolQ);
+
+SUNDIALS_EXPORT int CPodeSetTolerances(void *cpode_mem,
+ int tol_type, realtype reltol, void *abstol);
+
+SUNDIALS_EXPORT int CPodeSetRootDirection(void *cpode_mem, int *rootdir);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPode
+ * -----------------------------------------------------------------
+ * CPode integrates the ODE over an interval in t.
+ * If mode is CP_NORMAL, then the solver integrates from its
+ * current internal t value to a point at or beyond tout, then
+ * interpolates to t = tout and returns y(tout) in the user-
+ * allocated vector yout. If mode is CP_ONE_STEP, then the solver
+ * takes one internal time step and returns in yout the value of
+ * y at the new internal time. In this case, tout is used only
+ * during the first call to CPode to determine the direction of
+ * integration and the rough scale of the t variable. If mode is
+ * CP_NORMAL_TSTOP or CP_ONE_STEP_TSTOP, then CPode returns the
+ * solution at tstop if that comes sooner than tout or the end of
+ * the next internal step, respectively. In any case,
+ * the time reached by the solver is placed in (*tret). The
+ * user is responsible for allocating the memory for this value.
+ *
+ * cpode_mem is the pointer to CPODE memory returned by
+ * CPodeCreate.
+ *
+ * tout is the next time at which a computed solution is desired.
+ *
+ * tret is a pointer to a real location. CPode sets (*tret) to
+ * the time reached by the solver and returns yout=y(*tret)
+ * and ypout=y'(*tret).
+ *
+ * yout is the computed solution vector. In CP_NORMAL mode with no
+ * errors and no roots found, yout=y(tout).
+ *
+ * ypout is the computed derivative vector.
+ *
+ * mode is CP_NORMAL, CP_ONE_STEP, CP_NORMAL_TSTOP, or
+ * CP_ONE_STEP_TSTOP. These four modes are described above.
+ *
+ * Here is a brief description of each return value:
+ *
+ * CP_SUCCESS: CPode succeeded and no roots were found.
+ *
+ * CP_ROOT_RETURN: CPode succeeded, and found one or more roots.
+ * If nrtfn > 1, call CPodeGetRootInfo to see
+ * which g_i were found to have a root at (*tret).
+ *
+ * CP_TSTOP_RETURN: CPode succeeded and returned at tstop.
+ *
+ * CP_MEM_NULL: The cpode_mem argument was NULL.
+ *
+ * CP_NO_MALLOC: cpode_mem was not allocated.
+ *
+ * CP_ILL_INPUT: One of the inputs to CPode is illegal. This
+ * includes the situation when a component of the
+ * error weight vectors becomes < 0 during
+ * internal time-stepping. It also includes the
+ * situation where a root of one of the root
+ * functions was found both at t0 and very near t0.
+ * The ILL_INPUT flag will also be returned if the
+ * linear solver routine CP--- (called by the user
+ * after calling CPodeCreate) failed to set one of
+ * the linear solver-related fields in cpode_mem or
+ * if the linear solver's init routine failed. In
+ * any case, the user should see the printed
+ * error message for more details.
+ *
+ * CP_TOO_MUCH_WORK: The solver took mxstep internal steps but
+ * could not reach tout. The default value for
+ * mxstep is MXSTEP_DEFAULT = 500.
+ *
+ * CP_TOO_MUCH_ACC: The solver could not satisfy the accuracy
+ * demanded by the user for some internal step.
+ *
+ * CP_ERR_FAILURE: Error test failures occurred too many times
+ * (= MXNEF = 7) during one internal time step or
+ * occurred with |h| = hmin.
+ *
+ * CP_CONV_FAILURE: Convergence test failures occurred too many
+ * times (= MXNCF = 10) during one internal time
+ * step or occurred with |h| = hmin.
+ *
+ * CP_LINIT_FAIL: The linear solver's initialization function
+ * failed.
+ *
+ * CP_LSETUP_FAIL: The linear solver's setup routine failed in an
+ * unrecoverable manner.
+ *
+ * CP_LSOLVE_FAIL: The linear solver's solve routine failed in an
+ * unrecoverable manner.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPode(void *cpode_mem, realtype tout, realtype *tret,
+ N_Vector yout, N_Vector ypout, int mode);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPodeGetDky
+ * -----------------------------------------------------------------
+ * CPodeGetDky computes the kth derivative of the y function at
+ * time t, where tn-hu <= t <= tn, tn denotes the current
+ * internal time reached, and hu is the last internal step size
+ * successfully used by the solver. The user may request
+ * k=0, 1, ..., qu, where qu is the order last used. The
+ * derivative vector is returned in dky. This vector must be
+ * allocated by the caller. It is only legal to call this
+ * function after a successful return from CPode.
+ *
+ * cpode_mem is the pointer to CPODES memory returned by
+ * CPodeCreate.
+ *
+ * t is the time at which the kth derivative of y is evaluated.
+ * The legal range for t is [tn-hu,tn] as described above.
+ *
+ * k is the order of the derivative of y to be computed. The
+ * legal range for k is [0,qu] as described above.
+ *
+ * dky is the output derivative vector [((d/dy)^k)y](t).
+ *
+ * The return value for CPodeGetDky is one of:
+ *
+ * CP_SUCCESS: CPodeGetDky succeeded.
+ *
+ * CP_BAD_K: k is not in the range 0, 1, ..., qu.
+ *
+ * CP_BAD_T: t is not in the interval [tn-hu,tn].
+ *
+ * CP_BAD_DKY: The dky argument was NULL.
+ *
+ * CP_MEM_NULL: The cpode_mem argument was NULL.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPodeGetDky(void *cpode_mem, realtype t, int k, N_Vector dky);
+
+/*
+ * -----------------------------------------------------------------
+ * Quadrature integration solution extraction routines
+ * -----------------------------------------------------------------
+ * The following functions can be called to obtain the quadrature
+ * variables (or derivatives of them) after a successful integration
+ * step. If quadratures were not computed, they return CP_NO_QUAD.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPodeGetQuad(void *cpode_mem, realtype t, N_Vector yQout);
+SUNDIALS_EXPORT int CPodeGetQuadDky(void *cpode_mem, realtype t, int k, N_Vector dky);
+
+
+/*
+ * -----------------------------------------------------------------
+ * IC calculation optional output extraction functions
+ * -----------------------------------------------------------------
+ * CPodeGetConsistentIC returns the consistent initial conditions
+ * computed by CPodeCalcIC
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPodeGetConsistentIC(void *cpode_mem, N_Vector yy0, N_Vector yp0);
+
+/*
+ * -----------------------------------------------------------------
+ * Integrator optional output extraction functions
+ * -----------------------------------------------------------------
+ * The following functions can be called to get optional outputs
+ * and statistics related to the main integrator.
+ * -----------------------------------------------------------------
+ * CPodeGetWorkSpace returns the CPODES real and integer workspaces
+ * CPodeGetNumSteps returns the cumulative number of internal
+ * steps taken by the solver
+ * CPodeGetNumFctEvals returns the number of calls to the user's
+ * fun function
+ * CPodeGetNumLinSolvSetups returns the number of calls made to
+ * the linear solver's setup routine
+ * CPodeGetNumErrTestFails returns the number of local error test
+ * failures that have occured
+ * CPodeGetLastOrder returns the order used during the last
+ * internal step
+ * CPodeGetCurrentOrder returns the order to be used on the next
+ * internal step
+ * CPodeGetNumStabLimOrderReds returns the number of order
+ * reductions due to stability limit detection
+ * CPodeGetActualInitStep returns the actual initial step size
+ * used by CPODES
+ * CPodeGetLastStep returns the step size for the last internal step
+ * CPodeGetCurrentStep returns the step size to be attempted on
+ * the next internal step
+ * CPodeGetCurrentTime returns the current internal time reached
+ * by the solver
+ * CPodeGetTolScaleFactor returns a suggested factor by which the
+ * user's tolerances should be scaled when too much accuracy has
+ * been requested for some internal step
+ * CPodeGetErrWeights returns the current error weight vector.
+ * The user must allocate space for eweight.
+ * CPodeGetEstLocalErrors returns the vector of estimated local
+ * errors. The user must allocate space for ele.
+ * CPodeGetNumGEvals returns the number of calls to the user's
+ * gfun function (for rootfinding)
+ * CPodeGetRootInfo returns the indices for which g_i was found to
+ * have a root. The user must allocate space for rootsfound.
+ * For i = 0 ... nrtfn-1, rootsfound[i] = 1 if g_i has a root,
+ * and = 0 if not.
+ * CPodeGetRootWindow returns the most recent (tLo,tHi] window in
+ * which a g_i was found to have a root.
+ * CPodeGetIntegratorStats retruns most of the optional outputs as
+ * a group.
+ * CPodeGet* return values:
+ * CP_SUCCESS if succesful
+ * CP_MEM_NULL if the CPODES memory was NULL
+ * CP_NO_SLDET if stability limit was not turned on
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPodeGetWorkSpace(void *cpode_mem, long int *lenrw, long int *leniw);
+SUNDIALS_EXPORT int CPodeGetNumSteps(void *cpode_mem, long int *nsteps);
+SUNDIALS_EXPORT int CPodeGetNumFctEvals(void *cpode_mem, long int *nfevals);
+SUNDIALS_EXPORT int CPodeGetNumLinSolvSetups(void *cpode_mem, long int *nlinsetups);
+SUNDIALS_EXPORT int CPodeGetNumErrTestFails(void *cpode_mem, long int *netfails);
+SUNDIALS_EXPORT int CPodeGetLastOrder(void *cpode_mem, int *qlast);
+SUNDIALS_EXPORT int CPodeGetCurrentOrder(void *cpode_mem, int *qcur);
+SUNDIALS_EXPORT int CPodeGetNumStabLimOrderReds(void *cpode_mem, long int *nslred);
+SUNDIALS_EXPORT int CPodeGetActualInitStep(void *cpode_mem, realtype *hinused);
+SUNDIALS_EXPORT int CPodeGetLastStep(void *cpode_mem, realtype *hlast);
+SUNDIALS_EXPORT int CPodeGetCurrentStep(void *cpode_mem, realtype *hcur);
+SUNDIALS_EXPORT int CPodeGetCurrentTime(void *cpode_mem, realtype *tcur);
+SUNDIALS_EXPORT int CPodeGetTolScaleFactor(void *cpode_mem, realtype *tolsfac);
+SUNDIALS_EXPORT int CPodeGetErrWeights(void *cpode_mem, N_Vector eweight);
+SUNDIALS_EXPORT int CPodeGetEstLocalErrors(void *cpode_mem, N_Vector ele);
+SUNDIALS_EXPORT int CPodeGetNumGEvals(void *cpode_mem, long int *ngevals);
+SUNDIALS_EXPORT int CPodeGetRootInfo(void *cpode_mem, int *rootsfound);
+SUNDIALS_EXPORT int CPodeGetRootWindow(void *cpode_mem, realtype *tlo, realtype *thi);
+SUNDIALS_EXPORT int CPodeGetIntegratorStats(void *cpode_mem, long int *nsteps,
+ long int *nfevals, long int *nlinsetups,
+ long int *netfails, int *qlast,
+ int *qcur, realtype *hinused, realtype *hlast,
+ realtype *hcur, realtype *tcur);
+
+/*
+ * -----------------------------------------------------------------
+ * Nonlinear solver optional output extraction functions
+ * -----------------------------------------------------------------
+ * The following functions can be called to get optional outputs
+ * and statistics related to the nonlinear solver.
+ * -----------------------------------------------------------------
+ * CPodeGetNumNonlinSolvIters returns the number of nonlinear
+ * solver iterations performed.
+ * CPodeGetNumNonlinSolvConvFails returns the number of nonlinear
+ * convergence failures.
+ * CPodeGetNonlinSolvStats returns the nonlinear solver optional
+ * outputs in a group.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPodeGetNumNonlinSolvIters(void *cpode_mem, long int *nniters);
+SUNDIALS_EXPORT int CPodeGetNumNonlinSolvConvFails(void *cpode_mem, long int *nncfails);
+SUNDIALS_EXPORT int CPodeGetNonlinSolvStats(void *cpode_mem, long int *nniters,
+ long int *nncfails);
+
+
+/*
+ * -----------------------------------------------------------------
+ * Projection optional output extraction functions
+ * -----------------------------------------------------------------
+ * The following functions can be called to get optional outputs
+ * and statistics related to the projection step.
+ * -----------------------------------------------------------------
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPodeGetProjNumProj(void *cpode_mem, long int *nproj);
+SUNDIALS_EXPORT int CPodeGetProjNumCnstrEvals(void *cpode_mem, long int *nce);
+SUNDIALS_EXPORT int CPodeGetProjNumLinSolvSetups(void *cpode_mem, long int *nsetupsP);
+SUNDIALS_EXPORT int CPodeGetProjNumFailures(void *cpode_mem, long int *nprf);
+SUNDIALS_EXPORT int CPodeGetProjStats(void *cpode_mem, long int *nproj,
+ long int *nce, long int *nsetupsP,
+ long int *nprf);
+
+/*
+ * -----------------------------------------------------------------
+ * Quadrature integration optional output extraction functions
+ * -----------------------------------------------------------------
+ * The following functions can be called to get optional outputs
+ * and statistics related to the integration of quadratures.
+ * -----------------------------------------------------------------
+ * CPodeGetQuadNumFunEvals returns the number of calls to the
+ * user function qfun defining the integrand
+ * CPodeGetQuadErrWeights returns the vector of error weights for
+ * the quadrature variables. The user must allocate space for ewtQ.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPodeGetQuadNumFunEvals(void *cpode_mem, long int *nqevals);
+SUNDIALS_EXPORT int CPodeGetQuadErrWeights(void *cpode_mem, N_Vector eQweight);
+
+/*
+ * -----------------------------------------------------------------
+ * The following function returns the name of the constant
+ * associated with a CPODES return flag
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT char *CPodeGetReturnFlagName(int flag);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPodeFree
+ * -----------------------------------------------------------------
+ * CPodeFree frees the problem memory cpode_mem allocated by
+ * CPodeCreate and CPodeInit. Its only argument is the pointer
+ * cpode_mem returned by CPodeCreate.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void CPodeFree(void **cpode_mem);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPodeQuadFree
+ * -----------------------------------------------------------------
+ * CPodeQuadFree frees the problem memory in cpode_mem allocated
+ * for quadrature integration. Its only argument is the pointer
+ * cpode_mem returned by CPodeCreate.
+ * Note that CPodeQuadFree is called by CPodeFree.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void CPodeQuadFree(void *cpode_mem);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_band.h b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_band.h
new file mode 100644
index 0000000..2656d5b
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_band.h
@@ -0,0 +1,60 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.3 $
+ * $Date: 2006/11/29 00:05:05 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the header file for the CPODES band linear solver, CPBAND.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _CPBAND_H
+#define _CPBAND_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <cpodes/cpodes_direct.h>
+#include <sundials/sundials_band.h>
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPBand
+ * -----------------------------------------------------------------
+ * A call to the CPBand function links the main CPODES integrator
+ * with the CPBAND linear solver.
+ *
+ * cpode_mem is the pointer to the integrator memory returned by
+ * CPodeCreate.
+ *
+ * N is the size of the ODE system.
+ *
+ * mupper is the upper bandwidth of the band Jacobian
+ * approximation.
+ *
+ * mlower is the lower bandwidth of the band Jacobian
+ * approximation.
+ *
+ * The return value of CPBand is one of:
+ * CPDIRECT_SUCCESS if successful
+ * CPDIRECT_MEM_NULL if the CPODES memory was NULL
+ * CPDIRECT_MEM_FAIL if there was a memory allocation failure
+ * CPDIRECT_ILL_INPUT if a required vector operation is missing
+ * or if a bandwidth has an illegal value.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPBand(void *cpode_mem, int N, int mupper, int mlower);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_bandpre.h b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_bandpre.h
new file mode 100644
index 0000000..9e29083
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_bandpre.h
@@ -0,0 +1,253 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.3 $
+ * $Date: 2006/11/29 00:05:05 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the header file for the CPBANDPRE module, which
+ * provides a banded difference quotient Jacobian-based
+ * preconditioner and solver routines for use with CPSPGMR,
+ * CPSPBCG, or CPSPTFQMR.
+ *
+ * Summary:
+ * These routines provide a band matrix preconditioner based on
+ * difference quotients of the ODE right-hand side function f
+ * (for explicit-form ODEs) or on the residual function F (for
+ * implicit-form ODEs).
+ * The user supplies parameters
+ * mu = upper half-bandwidth (number of super-diagonals)
+ * ml = lower half-bandwidth (number of sub-diagonals)
+ * The routines generate a band matrix of bandwidth ml + mu + 1
+ * and use this to form a preconditioner for use with one of the
+ * CSPILS iterative linear solvers. Although this matrix is intended
+ * to approximate the Jacobian df/dy (respectively dF/dy + gamma*dF/dy,
+ * it may be a very crude approximation. The true Jacobian need not
+ * be banded, or its true bandwith may be larger than ml + mu + 1,
+ * as long as the banded approximation generated here is sufficiently
+ * accurate to speed convergence as a preconditioner.
+ *
+ * Usage:
+ * The following is a summary of the usage of this module.
+ * Details of the calls to CPodeCreate, CPodeMalloc, CPSp*,
+ * and CPode are available in the User Guide.
+ * To use these routines, the sequence of calls in the user
+ * main program should be as follows:
+ *
+ * #include <cpodes/cpodes_bandpre.h>
+ * #include <nvector_serial.h>
+ * ...
+ * void *bp_data;
+ * ...
+ * Set y0
+ * ...
+ * cpode_mem = CPodeCreate(...);
+ * ier = CPodeMalloc(...);
+ * ...
+ * bp_data = CPBandPrecAlloc(cpode_mem, N, mu, ml);
+ * ...
+ * flag = CPBPSptfqmr(cpode_mem, pretype, maxl, bp_data);
+ * -or-
+ * flag = CPBPSpgmr(cpode_mem, pretype, maxl, bp_data);
+ * -or-
+ * flag = CPBPSpbcg(cpode_mem, pretype, maxl, bp_data);
+ * ...
+ * flag = CPode(...);
+ * ...
+ * CPBandPrecFree(&bp_data);
+ * ...
+ * Free y0
+ * ...
+ * CPodeFree(cpode_mem);
+ *
+ * Notes:
+ * (1) Include this file for the CPBandPrecData type definition.
+ * (2) In the CPBandPrecAlloc call, the arguments N is the
+ * problem dimension.
+ * (3) In the CPBPSp* call, the user is free to specify
+ * the input pretype and the optional input maxl. The last
+ * argument must be the pointer returned by CPBandPrecAlloc.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _CPBANDPRE_H
+#define _CPBANDPRE_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <sundials/sundials_nvector.h>
+
+/* CPBANDPRE return values */
+
+#define CPBANDPRE_SUCCESS 0
+#define CPBANDPRE_PDATA_NULL -11
+#define CPBANDPRE_FUNC_UNRECVR -12
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPBandPrecAlloc
+ * -----------------------------------------------------------------
+ * CPBandPrecAlloc allocates and initializes a CPBandPrecData
+ * structure to be passed to CPSp* (and subsequently used
+ * by CPBandPrecSetup and CPBandPrecSolve).
+ *
+ * The parameters of CPBandPrecAlloc are as follows:
+ *
+ * cpode_mem is the pointer to CPODES memory returned by CPodeCreate.
+ *
+ * N is the problem size.
+ *
+ * mu is the upper half bandwidth.
+ *
+ * ml is the lower half bandwidth.
+ *
+ * CPBandPrecAlloc returns the storage pointer of type
+ * CPBandPrecData, or NULL if the request for storage cannot be
+ * satisfied.
+ *
+ * NOTE: The band preconditioner assumes a serial implementation
+ * of the NVECTOR package. Therefore, CPBandPrecAlloc will
+ * first test for a compatible N_Vector internal
+ * representation by checking for required functions.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void *CPBandPrecAlloc(void *cpode_mem, int N, int mu, int ml);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPBPSptfqmr
+ * -----------------------------------------------------------------
+ * CPBPSptfqmr links the CPBANDPPRE preconditioner to the CPSPTFQMR
+ * linear solver. It performs the following actions:
+ * 1) Calls the CPSPTFQMR specification routine and attaches the
+ * CPSPTFQMR linear solver to the integrator memory;
+ * 2) Sets the preconditioner data structure for CPSPTFQMR
+ * 3) Sets the preconditioner setup routine for CPSPTFQMR
+ * 4) Sets the preconditioner solve routine for CPSPTFQMR
+ *
+ * Its first 3 arguments are the same as for CPSptfqmr (see
+ * cpsptfqmr.h). The last argument is the pointer to the CPBANDPPRE
+ * memory block returned by CPBandPrecAlloc. Note that the user need
+ * not call CPSptfqmr.
+ *
+ * Possible return values are:
+ * CPSPILS_SUCCESS if successful
+ * CPSPILS_MEM_NULL if the CPODES memory was NULL
+ * CPSPILS_LMEM_NULL if the CPSPILS memory was NULL
+ * CPSPILS_MEM_FAIL if there was a memory allocation failure
+ * CPSPILS_ILL_INPUT if a required vector operation is missing
+ * CPBANDPRE_PDATA_NULL if the bp_data was NULL
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPBPSptfqmr(void *cpode_mem, int pretype, int maxl, void *p_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPBPSpbcg
+ * -----------------------------------------------------------------
+ * CPBPSpbcg links the CPBANDPPRE preconditioner to the CPSPBCG
+ * linear solver. It performs the following actions:
+ * 1) Calls the CPSPBCG specification routine and attaches the
+ * CPSPBCG linear solver to the integrator memory;
+ * 2) Sets the preconditioner data structure for CPSPBCG
+ * 3) Sets the preconditioner setup routine for CPSPBCG
+ * 4) Sets the preconditioner solve routine for CPSPBCG
+ *
+ * Its first 3 arguments are the same as for CPSpbcg (see
+ * cpspbcg.h). The last argument is the pointer to the CPBANDPPRE
+ * memory block returned by CPBandPrecAlloc. Note that the user need
+ * not call CPSpbcg.
+ *
+ * Possible return values are:
+ * CPSPILS_SUCCESS if successful
+ * CPSPILS_MEM_NULL if the CPODEs memory was NULL
+ * CPSPILS_LMEM_NULL if the CPSPILS memory was NULL
+ * CPSPILS_MEM_FAIL if there was a memory allocation failure
+ * CPSPILS_ILL_INPUT if a required vector operation is missing
+ * CPBANDPRE_PDATA_NULL if the bp_data was NULL
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPBPSpbcg(void *cpode_mem, int pretype, int maxl, void *p_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPBPSpgmr
+ * -----------------------------------------------------------------
+ * CPBPSpgmr links the CPBANDPPRE preconditioner to the CPSPGMR
+ * linear solver. It performs the following actions:
+ * 1) Calls the CPSPGMR specification routine and attaches the
+ * CPSPGMR linear solver to the integrator memory;
+ * 2) Sets the preconditioner data structure for CPSPGMR
+ * 3) Sets the preconditioner setup routine for CPSPGMR
+ * 4) Sets the preconditioner solve routine for CPSPGMR
+ *
+ * Its first 3 arguments are the same as for CPSpgmr (see
+ * cpspgmr.h). The last argument is the pointer to the CPBANDPPRE
+ * memory block returned by CPBandPrecAlloc. Note that the user need
+ * not call CPSpgmr.
+ *
+ * Possible return values are:
+ * CPSPILS_SUCCESS if successful
+ * CPSPILS_MEM_NULL if the CPODES memory was NULL
+ * CPSPILS_LMEM_NULL if the CPSPILS memory was NULL
+ * CPSPILS_MEM_FAIL if there was a memory allocation failure
+ * CPSPILS_ILL_INPUT if a required vector operation is missing
+ * CPBANDPRE_PDATA_NULL if the bp_data was NULL
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPBPSpgmr(void *cpode_mem, int pretype, int maxl, void *p_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPBandPrecFree
+ * -----------------------------------------------------------------
+ * CPBandPrecFree frees the memory allocated by CPBandPrecAlloc
+ * in the argument bp_data.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void CPBandPrecFree(void **bp_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Optional output functions : CPBandPrecGet*
+ * -----------------------------------------------------------------
+ * CPBandPrecGetWorkSpace returns the real and integer work space used
+ * by CPBANDPRE.
+ * CPBandPrecGetNumFctEvals returns the number of calls made from
+ * CPBANDPRE to the user's ODE function.
+ *
+ * The return value of CPBandPrecGet* is one of:
+ * CPBANDPRE_SUCCESS if successful
+ * CPBANDPRE_PDATA_NULL if the bp_data memory was NULL
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPBandPrecGetWorkSpace(void *bp_data, long int *lenrwLS, long int *leniwLS);
+SUNDIALS_EXPORT int CPBandPrecGetNumFctEvals(void *bp_data, long int *nfevalsBP);
+
+/*
+ * -----------------------------------------------------------------
+ * The following function returns the name of the constant
+ * associated with a CPBANDPRE return flag
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT char *CPBandPrecGetReturnFlagName(int flag);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_bbdpre.h b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_bbdpre.h
new file mode 100644
index 0000000..41d9037
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_bbdpre.h
@@ -0,0 +1,408 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.3 $
+ * $Date: 2006/11/29 00:05:05 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the header file for the CPBBDPRE module, for a
+ * band-block-diagonal preconditioner, i.e. a block-diagonal
+ * matrix with banded blocks, for use with one of the CPSPILS
+ * linear solvers and the parallel implementation of the NVECTOR
+ * module.
+ *
+ * Summary:
+ *
+ * These routines provide a preconditioner matrix that is
+ * block-diagonal with banded blocks. The blocking corresponds to the
+ * distribution of the dependent variable vector y among the processors.
+ * Each preconditioner block is generated from the Jacobian of the local
+ * part (on the current processor) of a given function g(t,y) approximating
+ * f(t,y) (for explicit-form ODEs) or G(t,y,y') approximating F(t,y,y')
+ * (for implicit-form ODEs). The blocks are generated by a difference
+ * quotient scheme on each processor independently. This scheme utilizes
+ * an assumed banded structure with given half-bandwidths, mudq and mldq.
+ * However, the banded Jacobian block kept by the scheme has half-bandwiths
+ * mukeep and mlkeep, which may be smaller.
+ *
+ * The user's calling program should have the following form:
+ *
+ * #include <nvector_parallel.h>
+ * #include <cpodes/cpodes_bbdpre.h>
+ * ...
+ * void *cpode_mem;
+ * void *bbd_data;
+ * ...
+ * Set y0
+ * ...
+ * cpode_mem = CPodeCreate(...);
+ * ier = CPodeMalloc(...);
+ * ...
+ * bbd_data = CPBBDPrecAlloc(cpode_mem, Nlocal, mudq ,mldq,
+ * mukeep, mlkeep, dqrely, gloc, cfn);
+ * flag = CPBBDSpgmr(cpode_mem, pretype, maxl, bbd_data);
+ * -or-
+ * flag = CPBBDSpbcg(cpode_mem, pretype, maxl, bbd_data);
+ * -or-
+ * flag = CPBBDSptfqmr(cpode_mem, pretype, maxl, bbd_data);
+ * ...
+ * ier = CPode(...);
+ * ...
+ * CPBBDPrecFree(&bbd_data);
+ * ...
+ * CPodeFree(...);
+ *
+ * Free y0
+ *
+ * The user-supplied routines required are:
+ *
+ * f or F - function defining the ODE right-hand side f(t,y)
+ * or the ODE residual F(t,y,y')
+ *
+ * gloc or Gloc - function defining the approximation g(t,y)
+ * or G(t,y,y')
+ *
+ * cfn - function to perform communication need for gloc.
+ *
+ * Notes:
+ *
+ * 1) This header file is included by the user for the definition
+ * of the CPBBDData type and for needed function prototypes.
+ *
+ * 2) The CPBBDPrecAlloc call includes half-bandwiths mudq and mldq
+ * to be used in the difference quotient calculation of the
+ * approximate Jacobian. They need not be the true
+ * half-bandwidths of the Jacobian of the local block of g,
+ * when smaller values may provide a greater efficiency.
+ * Also, the half-bandwidths mukeep and mlkeep of the retained
+ * banded approximate Jacobian block may be even smaller,
+ * to reduce storage and computation costs further.
+ * For all four half-bandwidths, the values need not be the
+ * same on every processor.
+ *
+ * 3) The actual name of the user's f (or F) function is passed to
+ * CPodeMalloc, and the names of the user's gloc (or Gloc) and
+ * cfn functions are passed to CPBBDPrecAlloc.
+ *
+ * 4) The pointer to the user-defined data block f_data, which is
+ * set through CPodeSetFdata is also available to the user in
+ * gloc/Gloc and cfn.
+ *
+ * 5) For the CPSpgmr solver, the Gram-Schmidt type gstype,
+ * is left to the user to specify through CPSpgmrSetGStype.
+ *
+ * 6) Optional outputs specific to this module are available by
+ * way of routines listed below. These include work space sizes
+ * and the cumulative number of gloc calls. The costs
+ * associated with this module also include nsetups banded LU
+ * factorizations, nlinsetups cfn calls, and npsolves banded
+ * backsolve calls, where nlinsetups and npsolves are
+ * integrator/CPSPGMR/CPSPBCG/CPSPTFQMR optional outputs.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _CPBBDPRE_H
+#define _CPBBDPRE_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <sundials/sundials_nvector.h>
+
+/* CPBBDPRE return values */
+
+#define CPBBDPRE_SUCCESS 0
+#define CPBBDPRE_PDATA_NULL -11
+#define CPBBDPRE_FUNC_UNRECVR -12
+
+/*
+ * -----------------------------------------------------------------
+ * Types: CPBBDLocalRhsFn and CPBBDLocalResFn
+ * -----------------------------------------------------------------
+ *
+ * For ODEs in explicit form, i.e., y' = f(t,y), the user must
+ * supply a function g(t,y) which approximates the right-hand side
+ * function f for the system y'=f(t,y), and which is computed locally
+ * (without interprocess communication). The case where g is
+ * mathematically identical to f is allowed. The implementation of
+ * this function must have type CPBBDLocalRhsFn.
+ *
+ * This function takes as input the local vector size Nlocal, the
+ * independent variable value t, the local real dependent variable
+ * vector y, and a pointer to the user-defined data block f_data.
+ * It is to compute the local part of g(t,y) and store this in the
+ * vector gout. Allocation of memory for y and g is handled within
+ * the preconditioner module. The f_data parameter is the same as
+ * that specified by the user through the CPodeSetFdata routine.
+ *
+ * A CPBBDLocalRhsFn should return 0 if successful, a positive value
+ * if a recoverable error occurred, and a negative value if an
+ * unrecoverable error occurred.
+ *
+ * -----------------------------------------------------------------
+ *
+ * For ODEs in implicit form, the user must supply a function
+ * G(t,y,y') which approximates the function F for the system
+ * F(t,y,y') = 0, and which is computed locally (without interprocess
+ * communication. The case where G is mathematically identical to F
+ * is allowed. The implementation of this function must have type
+ * CPBBDLocalResFn.
+ *
+ * This function takes as input the independent variable value t,
+ * the current solution vector y, the current solution derivative
+ * vector yp, and a pointer to the user-defined data block f_data.
+ * It is to compute the local part of G(t,y,y') and store it in the
+ * vector gout. Providing memory for y, yp, and gout is handled within
+ * this preconditioner module. It is expected that this routine will
+ * save communicated data in work space defined by the user and made
+ * available to the preconditioner function for the problem. The f_data
+ * parameter is the same as that passed by the user to the CPodeSetFdata
+ * routine.
+ *
+ * A CPBBDLocalResFn Gres is to return an int, defined in the same
+ * way as for the residual function: 0 (success), +1 or -1 (fail).
+ * -----------------------------------------------------------------
+ */
+
+typedef int (*CPBBDLocalRhsFn)(int Nlocal, realtype t,
+ N_Vector y,
+ N_Vector gout, void *f_data);
+
+typedef int (*CPBBDLocalResFn)(int Nlocal, realtype t,
+ N_Vector y, N_Vector yp,
+ N_Vector gout, void *f_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Type: CPBBDCommFn
+ * -----------------------------------------------------------------
+ * The user may supply a function of type CPBBDCommFn which performs
+ * all interprocess communication necessary to evaluate the
+ * approximate right-hand side function described above.
+ *
+ * This function takes as input the local vector size Nlocal,
+ * the independent variable value t, the dependent variable
+ * vector y, and a pointer to the user-defined data block f_data.
+ * The f_data parameter is the same as that specified by the user
+ * through the CPodeSetFdata routine. A CPBBDCommFn cfn is
+ * expected to save communicated data in space defined within the
+ * structure f_data. A CPBBDCommFn cfn does not have a return value.
+ *
+ * Each call to the CPBBDCommFn cfn is preceded by a call to the
+ * CPRhsFn f (or CPResFn F) with the same (t, y, y') arguments
+ * (where y'=NULL for explicit-form ODEs). Thus cfn can omit any
+ * communications done by f (or F) if relevant to the evaluation of
+ * the local approximation. If all necessary communication was done
+ * by f (respectively F), the user can pass NULL for cfn in
+ * CPBBDPrecAlloc (see below).
+ *
+ * A CPBBDCommFn should return 0 if successful, a positive value if
+ * a recoverable error occurred, and a negative value if an
+ * unrecoverable error occurred.
+ * -----------------------------------------------------------------
+ */
+
+typedef int (*CPBBDCommFn)(int Nlocal, realtype t,
+ N_Vector y, N_Vector yp, void *f_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPBBDPrecAlloc
+ * -----------------------------------------------------------------
+ * CPBBDPrecAlloc allocates and initializes a CPBBDData structure
+ * to be passed to CPSp* (and used by CPBBDPrecSetup and
+ * CPBBDPrecSolve).
+ *
+ * The parameters of CPBBDPrecAlloc are as follows:
+ *
+ * cpode_mem - pointer to the integrator memory.
+ *
+ * Nlocal - length of the local vectors on the current processor.
+ *
+ * mudq, mldq - upper and lower half-bandwidths to be used in the
+ * difference quotient computation of the local Jacobian block.
+ *
+ * mukeep, mlkeep - upper and lower half-bandwidths of the retained
+ * banded approximation to the local Jacobian block.
+ *
+ * dqrely - optional input. It is the relative increment in components
+ * of y used in the difference quotient approximations. To specify
+ * the default value (square root of the unitroundoff), pass 0.
+ *
+ * gloc - name of the user-supplied function g(t,y) or G(t,y,y') that
+ * approximates f (respectively F) and whose local Jacobian blocks
+ * are to form the preconditioner.
+ *
+ * cfn - name of the user-defined function that performs necessary
+ * interprocess communication for the execution of gloc.
+ *
+ * CPBBDPrecAlloc returns the storage allocated (type *void), or NULL
+ * if the request for storage cannot be satisfied.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void *CPBBDPrecAlloc(void *cpode_mem, int Nlocal,
+ int mudq, int mldq, int mukeep, int mlkeep,
+ realtype dqrely,
+ void *gloc, CPBBDCommFn cfn);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPBBDSptfqmr
+ * -----------------------------------------------------------------
+ * CPBBDSptfqmr links the CPBBDPRE preconditioner to the CPSPTFQMR
+ * linear solver. It performs the following actions:
+ * 1) Calls the CPSPTFQMR specification routine and attaches the
+ * CPSPTFQMR linear solver to the integrator memory;
+ * 2) Sets the preconditioner data structure for CPSPTFQMR
+ * 3) Sets the preconditioner setup routine for CPSPTFQMR
+ * 4) Sets the preconditioner solve routine for CPSPTFQMR
+ *
+ * Its first 3 arguments are the same as for CPSptfqmr (see
+ * cpsptfqmr.h). The last argument is the pointer to the CPBBDPRE
+ * memory block returned by CPBBDPrecAlloc. Note that the user need
+ * not call CPSptfqmr.
+ *
+ * Possible return values are:
+ * CPSPILS_SUCCESS if successful
+ * CPSPILS_MEM_NULL if the CPODES memory was NULL
+ * CPSPILS_LMEM_NULL if the CPSPILS memory was NULL
+ * CPSPILS_MEM_FAIL if there was a memory allocation failure
+ * CPSPILS_ILL_INPUT if a required vector operation is missing
+ * CPBBDPRE_PDATA_NULL if the bbd_data was NULL
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPBBDSptfqmr(void *cpode_mem, int pretype, int maxl, void *bbd_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPBBDSpbcg
+ * -----------------------------------------------------------------
+ * CPBBDSpbcg links the CPBBDPRE preconditioner to the CPSPBCG
+ * linear solver. It performs the following actions:
+ * 1) Calls the CPSPBCG specification routine and attaches the
+ * CPSPBCG linear solver to the integrator memory;
+ * 2) Sets the preconditioner data structure for CPSPBCG
+ * 3) Sets the preconditioner setup routine for CPSPBCG
+ * 4) Sets the preconditioner solve routine for CPSPBCG
+ *
+ * Its first 3 arguments are the same as for CPSpbcg (see
+ * cpspbcg.h). The last argument is the pointer to the CPBBDPRE
+ * memory block returned by CPBBDPrecAlloc. Note that the user need
+ * not call CPSpbcg.
+ *
+ * Possible return values are:
+ * CPSPILS_SUCCESS if successful
+ * CPSPILS_MEM_NULL if the CPODES memory was NULL
+ * CPSPILS_LMEM_NULL if the CPSPILS memory was NULL
+ * CPSPILS_MEM_FAIL if there was a memory allocation failure
+ * CPSPILS_ILL_INPUT if a required vector operation is missing
+ * CPBBDPRE_PDATA_NULL if the bbd_data was NULL
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPBBDSpbcg(void *cpode_mem, int pretype, int maxl, void *bbd_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPBBDSpgmr
+ * -----------------------------------------------------------------
+ * CPBBDSpgmr links the CPBBDPRE preconditioner to the CPSPGMR
+ * linear solver. It performs the following actions:
+ * 1) Calls the CPSPGMR specification routine and attaches the
+ * CPSPGMR linear solver to the integrator memory;
+ * 2) Sets the preconditioner data structure for CPSPGMR
+ * 3) Sets the preconditioner setup routine for CPSPGMR
+ * 4) Sets the preconditioner solve routine for CPSPGMR
+ *
+ * Its first 3 arguments are the same as for CPSpgmr (see
+ * cpspgmr.h). The last argument is the pointer to the CPBBDPRE
+ * memory block returned by CPBBDPrecAlloc. Note that the user need
+ * not call CPSpgmr.
+ *
+ * Possible return values are:
+ * CPSPILS_SUCCESS if successful
+ * CPSPILS_MEM_NULL if the CPODES memory was NULL
+ * CPSPILS_LMEM_NULL if the CPSPILS memory was NULL
+ * CPSPILS_MEM_FAIL if there was a memory allocation failure
+ * CPSPILS_ILL_INPUT if a required vector operation is missing
+ * CPBBDPRE_PDATA_NULL if the bbd_data was NULL
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPBBDSpgmr(void *cpode_mem, int pretype, int maxl, void *bbd_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPBBDPrecReInit
+ * -----------------------------------------------------------------
+ * CPBBDPrecReInit re-initializes the BBDPRE module when solving a
+ * sequence of problems of the same size with CPSPGMR/CPBBDPRE or
+ * CPSPBCG/CPBBDPRE or CPSPTFQMR/CPBBDPRE provided there is no change
+ * in Nlocal, mukeep, or mlkeep. After solving one problem, and after
+ * calling CPodeReInit to re-initialize the integrator for a subsequent
+ * problem, call CPBBDPrecReInit. Then call CPSpgmrSet* or CPSpbcgSet*
+ * or CPSptfqmrSet* functions if necessary for any changes to CPSPGMR,
+ * CPSPBCG, or CPSPTFQMR parameters, before calling CPode.
+ *
+ * The first argument to CPBBDPrecReInit must be the pointer pdata
+ * that was returned by CPBBDPrecAlloc. All other arguments have
+ * the same names and meanings as those of CPBBDPrecAlloc.
+ *
+ * The return value of CPBBDPrecReInit is CPBBDPRE_SUCCESS, indicating
+ * success, or CPBBDPRE_PDATA_NULL if bbd_data was NULL.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPBBDPrecReInit(void *bbd_data, int mudq, int mldq,
+ realtype dqrely, void *gloc, CPBBDCommFn cfn);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPBBDPrecFree
+ * -----------------------------------------------------------------
+ * CPBBDPrecFree frees the memory block bbd_data allocated by the
+ * call to CPBBDAlloc.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void CPBBDPrecFree(void **bbd_data);
+
+/*
+ * -----------------------------------------------------------------
+ * BBDPRE optional output extraction routines
+ * -----------------------------------------------------------------
+ * CPBBDPrecGetWorkSpace returns the BBDPRE real and integer work space
+ * sizes.
+ * CPBBDPrecGetNumGfnEvals returns the number of calls to gfn.
+ *
+ * The return value of CPBBDPrecGet* is one of:
+ * CPBBDPRE_SUCCESS if successful
+ * CPBBDPRE_PDATA_NULL if the bbd_data memory was NULL
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPBBDPrecGetWorkSpace(void *bbd_data, long int *lenrwLS, long int *leniwLS);
+SUNDIALS_EXPORT int CPBBDPrecGetNumGfnEvals(void *bbd_data, long int *ngevalsBBDP);
+
+/*
+ * -----------------------------------------------------------------
+ * The following function returns the name of the constant
+ * associated with a CPBBDPRE return flag
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT char *CPBBDPrecGetReturnFlagName(int flag);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_dense.h b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_dense.h
new file mode 100644
index 0000000..e887ba7
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_dense.h
@@ -0,0 +1,83 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.3 $
+ * $Date: 2006/11/29 00:05:05 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the header file for the CPODES dense linear solver, CPDENSE.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _CPDENSE_H
+#define _CPDENSE_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <cpodes/cpodes_direct.h>
+#include <sundials/sundials_dense.h>
+
+/*
+ * =================================================================
+ * E X P O R T E D F U N C T I O N S
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPDense
+ * -----------------------------------------------------------------
+ * A call to the CPDense function links the main integrator with
+ * the CPDENSE linear solver.
+ *
+ * cpode_mem is the pointer to the integrator memory returned by
+ * CPodeCreate.
+ *
+ * N is the size of the ODE system.
+ *
+ * The return value of CPDense is one of:
+ * CPDIRECT_SUCCESS if successful
+ * CPDIRECT_MEM_NULL if the CPODES memory was NULL
+ * CPDIRECT_MEM_FAIL if there was a memory allocation failure
+ * CPDIRECT_ILL_INPUT if a required vector operation is missing
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPDense(void *cpode_mem, int N);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPDenseProj
+ * -----------------------------------------------------------------
+ * A call to the CPDenseProj function links the main integrator with
+ * the CPDENSE linear solver.
+ *
+ * cpode_mem the pointer to the integrator memory returned by
+ * CPodeCreate.
+ * Nc the number of constraints
+ * Ny the number of states (size of the ODE system).
+ * fact_type the type of factorization used for the constraint
+ * Jcobian G. Legal values are CPDIRECT_LU and CPDIRECT_LQ.
+ *
+ * The return value of CPDense is one of:
+ * CPDIRECT_SUCCESS if successful
+ * CPDIRECT_MEM_NULL if the CPODES memory was NULL
+ * CPDIRECT_MEM_FAIL if there was a memory allocation failure
+ * CPDIRECT_ILL_INPUT if a required vector operation is missing
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPDenseProj(void *cpode_mem, int Nc, int Ny, int fact_type);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_direct.h b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_direct.h
new file mode 100644
index 0000000..7827065
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_direct.h
@@ -0,0 +1,438 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/29 00:05:06 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * Common header file for the direct linear solvers in CPODES.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _CPDIRECT_H
+#define _CPDIRECT_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <sundials/sundials_direct.h>
+#include <sundials/sundials_nvector.h>
+
+/*
+ * =================================================================
+ * C P D I R E C T C O N S T A N T S
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * CPDIRECT input constants
+ * -----------------------------------------------------------------
+ * fact_type: is the type of constraint Jacobian factorization used
+ * for the projection. For independent constraints (i.e. Jacobian
+ * with full row rank) any of the following three options can be
+ * used (although their computational cost varies, depending on
+ * the number of states and constraints)
+ * CPDIRECT_LU: use LU decomposition of G^T.
+ * CPDIRECT_QR: use QR decomposition of G^T
+ * CPDIRECT_SC: use Schur complement
+ * If it is known (or suspected) that some constraints are redundant,
+ * the following option should be used:
+ * CPDIRECT_QRP: use QR with column pivoting on G^T.
+ * -----------------------------------------------------------------
+ */
+
+/* fact_type */
+
+#define CPDIRECT_LU 1
+#define CPDIRECT_QR 2
+#define CPDIRECT_SC 3
+#define CPDIRECT_QRP 4
+
+/*
+ * -----------------------------------------------------------------
+ * CPDIRECT return values
+ * -----------------------------------------------------------------
+ */
+
+#define CPDIRECT_SUCCESS 0
+#define CPDIRECT_MEM_NULL -1
+#define CPDIRECT_LMEM_NULL -2
+#define CPDIRECT_ILL_INPUT -3
+#define CPDIRECT_MEM_FAIL -4
+
+/* Additional last_flag values */
+
+#define CPDIRECT_JACFUNC_UNRECVR -5
+#define CPDIRECT_JACFUNC_RECVR -6
+
+/*
+ * =================================================================
+ * F U N C T I O N T Y P E S
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Types: CPDlsDenseJacExplFn and CPDlsDenseJacImplFn
+ * -----------------------------------------------------------------
+ *
+ * If the ODE is given in explicit form, a dense Jacobian
+ * approximation function Jac must be of type CPDlsDenseJacFn.
+ * Its parameters are:
+ *
+ * N is the problem size.
+ *
+ * Jac is the dense matrix (of type DlsMat) that will be loaded
+ * by a CPDlsDenseJacFn with an approximation to the Jacobian
+ * matrix J = (df_i/dy_j) at the point (t,y).
+ *
+ * t is the current value of the independent variable.
+ *
+ * y is the current value of the dependent variable vector,
+ * namely the predicted value of y(t).
+ *
+ * fy is the vector f(t,y).
+ *
+ * jac_data is a pointer to user data - the same as the jac_data
+ * parameter passed to CPDlsSetJacFn.
+ *
+ * tmp1, tmp2, and tmp3 are pointers to memory allocated for
+ * vectors of length N which can be used by a CPDlsDenseJacFn
+ * as temporary storage or work space.
+ *
+ * A CPDlsDenseJacFn should return 0 if successful, a positive
+ * value if a recoverable error occurred, and a negative value if
+ * an unrecoverable error occurred.
+ *
+ * -----------------------------------------------------------------
+ *
+ * If the ODE is given in implicit form, a dense Jacobian
+ * approximation function djac must be of type CPDlsDenseJacImplFn.
+ * Its parameters are:
+ *
+ * N is the problem size, and length of all vector arguments.
+ *
+ * t is the current value of the independent variable t.
+ *
+ * y is the current value of the dependent variable vector,
+ * namely the predicted value of y(t).
+ *
+ * yp is the current value of the derivative vector y',
+ * namely the predicted value of y'(t).
+ *
+ * f is the residual vector F(tt,yy,yp).
+ *
+ * gm is the scalar in the system Jacobian, proportional to
+ * the step size h.
+ *
+ * jac_data is a pointer to user Jacobian data - the same as the
+ * jdata parameter passed to CPDenseSetJacFn.
+ *
+ * Jac is the dense matrix (of type DenseMat) to be loaded by
+ * an CPDenseJacImplFn routine with an approximation to the
+ * system Jacobian matrix
+ * J = dF/dy' + gamma*dF/dy
+ * at the given point (t,y,y'), where the ODE system is
+ * given by F(t,y,y') = 0. Jac is preset to zero, so only
+ * the nonzero elements need to be loaded. See note below.
+ *
+ * tmp1, tmp2, tmp3 are pointers to memory allocated for
+ * N_Vectors which can be used by an CPDenseJacImplFn routine
+ * as temporary storage or work space.
+ *
+ * A CPDenseJacImplFn should return
+ * 0 if successful,
+ * a positive int if a recoverable error occurred, or
+ * a negative int if a nonrecoverable error occurred.
+ * In the case of a recoverable error return, the integrator will
+ * attempt to recover by reducing the stepsize (which changes cj).
+ *
+ * -----------------------------------------------------------------
+ *
+ * NOTE: The following are two efficient ways to load a dense Jac:
+ * (1) (with macros - no explicit data structure references)
+ * for (j=0; j < Neq; j++) {
+ * col_j = DENSE_COL(Jac,j);
+ * for (i=0; i < Neq; i++) {
+ * generate J_ij = the (i,j)th Jacobian element
+ * col_j[i] = J_ij;
+ * }
+ * }
+ * (2) (without macros - explicit data structure references)
+ * for (j=0; j < Neq; j++) {
+ * col_j = (Jac->data)[j];
+ * for (i=0; i < Neq; i++) {
+ * generate J_ij = the (i,j)th Jacobian element
+ * col_j[i] = J_ij;
+ * }
+ * }
+ * A third way, using the DENSE_ELEM(A,i,j) macro, is much less
+ * efficient in general. It is only appropriate for use in small
+ * problems in which efficiency of access is NOT a major concern.
+ *
+ * NOTE: If the user's Jacobian routine needs other quantities,
+ * they are accessible as follows: hcur (the current stepsize)
+ * and ewt (the error weight vector) are accessible through
+ * CPodeGetCurrentStep and CPodeGetErrWeights, respectively
+ * (see cvode.h). The unit roundoff is available as
+ * UNIT_ROUNDOFF defined in sundials_types.h.
+ *
+ * -----------------------------------------------------------------
+ */
+
+
+typedef int (*CPDlsDenseJacExplFn)(int N, realtype t,
+ N_Vector y, N_Vector fy,
+ DlsMat Jac, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+typedef int (*CPDlsDenseJacImplFn)(int N, realtype t, realtype gm,
+ N_Vector y, N_Vector yp, N_Vector r,
+ DlsMat Jac, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+
+/*
+ * -----------------------------------------------------------------
+ * Types: CPDlsBandJacExplFn and CPDlsBandJacImplFn
+ * -----------------------------------------------------------------
+ *
+ * If the ODE is given in explicit form, a band Jacobian
+ * approximation function Jac must be of type CPDlsBandJacExplFn.
+ * Its parameters are:
+ *
+ * N is the length of all vector arguments.
+ *
+ * mupper is the upper half-bandwidth of the approximate banded
+ * Jacobian. This parameter is the same as the mupper parameter
+ * passed by the user to the linear solver initialization function.
+ *
+ * mlower is the lower half-bandwidth of the approximate banded
+ * Jacobian. This parameter is the same as the mlower parameter
+ * passed by the user to the linear solver initialization function.
+ *
+ * t is the current value of the independent variable.
+ *
+ * y is the current value of the dependent variable vector,
+ * namely the predicted value of y(t).
+ *
+ * fy is the vector f(t,y).
+ *
+ * Jac is the band matrix (of type DlsMat) that will be loaded
+ * by a CPDlsBandJacFn with an approximation to the Jacobian matrix
+ * Jac = (df_i/dy_j) at the point (t,y).
+ * jac_data is a pointer to user data - the same as the jac_data
+ * parameter passed to CPDlsSetJacFn.
+ *
+ * A CPDlsBandJacExplFn should return 0 if successful, a positive
+ * value if a recoverable error occurred, and a negative value if
+ * an unrecoverable error occurred.
+ *
+ * -----------------------------------------------------------------
+ *
+ * If the ODE is given in explicit form, a band Jacobian
+ * approximation function Jac must be of type CPDlsBandJacImplFn.
+ * Its parameters are:
+ *
+ * N is the problem size, and length of all vector arguments.
+ *
+ * mupper is the upper bandwidth of the banded Jacobian matrix.
+ *
+ * mlower is the lower bandwidth of the banded Jacobian matrix.
+ *
+ * t is the current value of the independent variable t.
+ *
+ * y is the current value of the dependent variable vector,
+ * namely the predicted value of y(t).
+ *
+ * yp is the current value of the derivative vector y',
+ * namely the predicted value of y'(t).
+ *
+ * r is the residual vector F(tt,yy,yp).
+ *
+ * gm is the scalar in the system Jacobian, proportional to
+ * the step size h.
+ *
+ * jac_data is a pointer to user Jacobian data - the same as the
+ * jdata parameter passed to CPBandSetJacFn.
+ *
+ * J is the band matrix (of type BandMat) to be loaded by
+ * with an approximation to the system Jacobian matrix
+ * J = dF/dy' + gamma*dF/dy
+ * at the given point (t,y,y'), where the ODE system is
+ * given by F(t,y,y') = 0. Jac is preset to zero, so only
+ * the nonzero elements need to be loaded. See note below.
+ *
+ * tmp1, tmp2, tmp3 are pointers to memory allocated for
+ * N_Vectors which can be used by an CPBandJacImplFn routine
+ * as temporary storage or work space.
+ *
+ * A CPDlsBandJacImplFn should return 0 if successful, a positive
+ * value if a recoverable error occurred, and a negative value if
+ * an unrecoverable error occurred.
+ *
+ * -----------------------------------------------------------------
+ *
+ * NOTE: Three efficient ways to load J are:
+ *
+ * (1) (with macros - no explicit data structure references)
+ * for (j=0; j < n; j++) {
+ * col_j = BAND_COL(Jac,j);
+ * for (i=j-mupper; i <= j+mlower; i++) {
+ * generate J_ij = the (i,j)th Jacobian element
+ * BAND_COL_ELEM(col_j,i,j) = J_ij;
+ * }
+ * }
+ *
+ * (2) (with BAND_COL macro, but without BAND_COL_ELEM macro)
+ * for (j=0; j < n; j++) {
+ * col_j = BAND_COL(Jac,j);
+ * for (k=-mupper; k <= mlower; k++) {
+ * generate J_ij = the (i,j)th Jacobian element, i=j+k
+ * col_j[k] = J_ij;
+ * }
+ * }
+ *
+ * (3) (without macros - explicit data structure references)
+ * offset = Jac->smu;
+ * for (j=0; j < n; j++) {
+ * col_j = ((Jac->data)[j])+offset;
+ * for (k=-mupper; k <= mlower; k++) {
+ * generate J_ij = the (i,j)th Jacobian element, i=j+k
+ * col_j[k] = J_ij;
+ * }
+ * }
+ * Caution: Jac->smu is generally NOT the same as mupper.
+ *
+ * The BAND_ELEM(A,i,j) macro is appropriate for use in small
+ * problems in which efficiency of access is NOT a major concern.
+ *
+ * NOTE: If the user's Jacobian routine needs other quantities,
+ * they are accessible as follows: hcur (the current stepsize)
+ * and ewt (the error weight vector) are accessible through
+ * CPodeGetCurrentStep and CPodeGetErrWeights, respectively
+ * (see cvode.h). The unit roundoff is available as
+ * UNIT_ROUNDOFF defined in sundials_types.h
+ *
+ * tmp1, tmp2, and tmp3 are pointers to memory allocated for
+ * vectors of length N which can be used by a CPDlsBandJacFn
+ * as temporary storage or work space.
+ *
+ * -----------------------------------------------------------------
+ */
+
+typedef int (*CPDlsBandJacExplFn)(int N, int mupper, int mlower,
+ realtype t, N_Vector y, N_Vector fy,
+ DlsMat Jac, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+typedef int (*CPDlsBandJacImplFn)(int N, int mupper, int mlower,
+ realtype t, realtype gm,
+ N_Vector y, N_Vector yp, N_Vector r,
+ DlsMat Jac, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+
+/*
+ * -----------------------------------------------------------------
+ * Type: CPDlsDenseProjJacFn
+ * -----------------------------------------------------------------
+ *
+ *
+ * -----------------------------------------------------------------
+ */
+
+typedef int (*CPDlsDenseProjJacFn)(int Nc, int Ny,
+ realtype t, N_Vector y, N_Vector cy,
+ DlsMat Jac, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2);
+
+
+
+/*
+ * =================================================================
+ * E X P O R T E D F U N C T I O N S
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Optional inputs to a CPDIRECT linear solver
+ * for implicit integration
+ * -----------------------------------------------------------------
+ *
+ * CPDlsSetJacFn specifies the Jacobian approximation routine to be
+ * used. When using dense Jacobians, a user-supplied jac routine must
+ * be of type CPDlsDenseJacFn. When using banded Jacobians, a
+ * user-supplied jac routine must be of type CPDlsBandJacFn.
+ * By default, a difference quotient approximation, supplied with this
+ * solver is used.
+ * CPDlsSetJacFn also specifies a pointer to user data which is
+ * passed to the user's jac routine every time it is called.
+ *
+ * The return value of CPDlsSetJacFn is one of:
+ * CPDIRECT_SUCCESS if successful
+ * CPDIRECT_MEM_NULL if the CPODES memory was NULL
+ * CPDIRECT_LMEM_NULL if the linear solver memory was NULL
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPDlsSetJacFn(void *cvode_mem, void *jac, void *jac_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Optional outputs from a CPDIRECT linear solver
+ * for implicit integration
+ * -----------------------------------------------------------------
+ *
+ * CPDlsGetWorkSpace returns the real and integer workspace used
+ * by the direct linear solver.
+ * CPDlsGetNumJacEvals returns the number of calls made to the
+ * Jacobian evaluation routine jac.
+ * CPDlsGetNumFctEvals returns the number of calls to the user
+ * f routine due to finite difference Jacobian
+ * evaluation.
+ * CPDlsGetLastFlag returns the last error flag set by any of
+ * the CPDIRECT interface functions.
+ *
+ * The return value of CPDlsGet* is one of:
+ * CPDIRECT_SUCCESS if successful
+ * CPDIRECT_MEM_NULL if the CPODES memory was NULL
+ * CPDIRECT_LMEM_NULL if the linear solver memory was NULL
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPDlsGetWorkSpace(void *cvode_mem, long int *lenrwLS, long int *leniwLS);
+SUNDIALS_EXPORT int CPDlsGetNumJacEvals(void *cvode_mem, long int *njevals);
+SUNDIALS_EXPORT int CPDlsGetNumFctEvals(void *cvode_mem, long int *nfevalsLS);
+SUNDIALS_EXPORT int CPDlsGetLastFlag(void *cvode_mem, int *flag);
+
+/*
+ * -----------------------------------------------------------------
+ * The following function returns the name of the constant
+ * associated with a CPDIRECT return flag
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT char *CPDlsGetReturnFlagName(int flag);
+
+/*
+ * -----------------------------------------------------------------
+ * Optional I/O functions for a CPDIRECT linear solver
+ * for projection
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPDlsProjSetJacFn(void *cpode_mem, void *jacP, void *jacP_data);
+
+SUNDIALS_EXPORT int CPDlsProjGetNumJacEvals(void *cpode_mem, long int *njPevals);
+SUNDIALS_EXPORT int CPDlsProjGetNumFctEvals(void *cpode_mem, long int *ncevalsLS);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_lapack.h b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_lapack.h
new file mode 100644
index 0000000..79a7ca2
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_lapack.h
@@ -0,0 +1,31 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.3 $
+ * $Date: 2006/11/29 00:05:06 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * Header file for the CPODES dense linear solver CPLAPACK.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _CPLAPACK_H
+#define _CPLAPACK_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <sundials/sundials_lapack.h>
+#include <cpodes/cpodes_lapack_exports.h>
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_lapack_exports.h b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_lapack_exports.h
new file mode 100644
index 0000000..4ad0bda
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_lapack_exports.h
@@ -0,0 +1,111 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.3 $
+ * $Date: 2006/11/29 00:05:06 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * Header file for the CPODES dense linear solver CPLAPACK.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _CPLAPACK_EXPORTS_H
+#define _CPLAPACK_EXPORTS_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <cpodes/cpodes_direct.h>
+
+/*
+ * =================================================================
+ * E X P O R T E D F U N C T I O N S
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPLapackDense
+ * -----------------------------------------------------------------
+ * A call to the CPLapackDense function links the main integrator
+ * with the CPLAPACK linear solver using dense Jacobians.
+ *
+ * cpode_mem is the pointer to the integrator memory returned by
+ * CPodeCreate.
+ *
+ * N is the size of the ODE system.
+ *
+ * The return value of CPLapackDense is one of:
+ * CPDIRECT_SUCCESS if successful
+ * CPDIRECT_MEM_NULL if the CPODES memory was NULL
+ * CPDIRECT_MEM_FAIL if there was a memory allocation failure
+ * CPDIRECT_ILL_INPUT if a required vector operation is missing
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPLapackDense(void *cpode_mem, int N);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPLapackBand
+ * -----------------------------------------------------------------
+ * A call to the CPLapackBand function links the main integrator
+ * with the CPLAPACK linear solver using banded Jacobians.
+ *
+ * cpode_mem is the pointer to the integrator memory returned by
+ * CPodeCreate.
+ *
+ * N is the size of the ODE system.
+ *
+ * mupper is the upper bandwidth of the band Jacobian approximation.
+ *
+ * mlower is the lower bandwidth of the band Jacobian approximation.
+ *
+ * The return value of CPLapackBand is one of:
+ * CPDIRECT_SUCCESS if successful
+ * CPDIRECT_MEM_NULL if the CPODES memory was NULL
+ * CPDIRECT_MEM_FAIL if there was a memory allocation failure
+ * CPDIRECT_ILL_INPUT if a required vector operation is missing or
+ * if a bandwidth has an illegal value.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPLapackBand(void *cpode_mem, int N, int mupper, int mlower);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPLapackDenseProj
+ * -----------------------------------------------------------------
+ * A call to the CPLapackDenseProj function links the main integrator
+ * with the CPLAPACK linear solver using dense Jacobians.
+ *
+ * cpode_mem the pointer to the integrator memory returned by
+ * CPodeCreate.
+ * Nc the number of constraints
+ * Ny the number of states (size of the ODE system).
+ * fact_type the type of factorization used for the constraint
+ * Jacobian G. Legal values are CPDIRECT_LU, CPDIRECT_QR,
+ * and CPDIRECT_SC.
+ *
+ * The return value of CPLapackDenseProj is one of:
+ * CPDIRECT_SUCCESS if successful
+ * CPDIRECT_MEM_NULL if the CPODES memory was NULL
+ * CPDIRECT_MEM_FAIL if there was a memory allocation failure
+ * CPDIRECT_ILL_INPUT if a required vector operation is missing
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPLapackDenseProj(void *cpode_mem, int Nc, int Ny, int fact_type);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_spbcgs.h b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_spbcgs.h
new file mode 100644
index 0000000..aafd13b
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_spbcgs.h
@@ -0,0 +1,67 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/29 00:05:06 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the header file for the CPODES scaled preconditioned
+ * Bi-CGSTAB linear solver, CPSPBCG.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _CPSPBCG_H
+#define _CPSPBCG_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <cpodes/cpodes_spils.h>
+#include <sundials/sundials_spbcgs.h>
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPSpbcg
+ * -----------------------------------------------------------------
+ * A call to the CPSpbcg function links the main CPODES integrator
+ * with the CPSPBCG linear solver.
+ *
+ * cpode_mem is the pointer to the integrator memory returned by
+ * CPodeCreate.
+ *
+ * pretype is the type of user preconditioning to be done.
+ * This must be one of the four enumeration constants
+ * PREC_NONE, PREC_LEFT, PREC_RIGHT, or PREC_BOTH defined
+ * in iterative.h. These correspond to no preconditioning,
+ * left preconditioning only, right preconditioning
+ * only, and both left and right preconditioning,
+ * respectively.
+ *
+ * maxl is the maximum Krylov dimension. This is an
+ * optional input to the CPSPBCG solver. Pass 0 to
+ * use the default value CPSPBCG_MAXL=5.
+ *
+ * The return value of CPSpbcg is one of:
+ * CPSPILS_SUCCESS if successful
+ * CPSPILS_MEM_NULL if the CPODES memory was NULL
+ * CPSPILS_MEM_FAIL if there was a memory allocation failure
+ * CPSPILS_ILL_INPUT if a required vector operation is missing
+ * The above constants are defined in cpodes_spils.h
+ *
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPSpbcg(void *cpode_mem, int pretype, int maxl);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_spgmr.h b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_spgmr.h
new file mode 100644
index 0000000..f96bdd8
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_spgmr.h
@@ -0,0 +1,68 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/29 00:05:06 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the header file for the CPODES scaled preconditioned GMRES
+ * linear solver, CPSPGMR.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _CPSPGMR_H
+#define _CPSPGMR_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <cpodes/cpodes_spils.h>
+#include <sundials/sundials_spgmr.h>
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPSpgmr
+ * -----------------------------------------------------------------
+ * A call to the CPSpgmr function links the main CPODES integrator
+ * with the CPSPGMR linear solver.
+ *
+ * cpode_mem is the pointer to the integrator memory returned by
+ * CPodeCreate.
+ *
+ * pretype is the type of user preconditioning to be done.
+ * This must be one of the four enumeration constants
+ * PREC_NONE, PREC_LEFT, PREC_RIGHT, or PREC_BOTH defined
+ * in sundials_iterative.h.
+ * These correspond to no preconditioning,
+ * left preconditioning only, right preconditioning
+ * only, and both left and right preconditioning,
+ * respectively.
+ *
+ * maxl is the maximum Krylov dimension. This is an
+ * optional input to the CPSPGMR solver. Pass 0 to
+ * use the default value CPSPGMR_MAXL=5.
+ *
+ * The return value of CPSpgmr is one of:
+ * CPSPILS_SUCCESS if successful
+ * CPSPILS_MEM_NULL if the CPODES memory was NULL
+ * CPSPILS_MEM_FAIL if there was a memory allocation failure
+ * CPSPILS_ILL_INPUT if a required vector operation is missing
+ * The above constants are defined in cpodes_spils.h
+ *
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPSpgmr(void *cpode_mem, int pretype, int maxl);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_spils.h b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_spils.h
new file mode 100644
index 0000000..2eabf06
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_spils.h
@@ -0,0 +1,391 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/29 00:05:06 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the common header file for the Scaled, Preconditioned
+ * Iterative Linear Solvers in CPODES.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _CPSPILS_H
+#define _CPSPILS_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <sundials/sundials_iterative.h>
+#include <sundials/sundials_nvector.h>
+
+/*
+ * =================================================================
+ * C P S P I L S C O N S T A N T S
+ * =================================================================
+ */
+
+/* CPSPILS return values */
+
+#define CPSPILS_SUCCESS 0
+#define CPSPILS_MEM_NULL -1
+#define CPSPILS_LMEM_NULL -2
+#define CPSPILS_ILL_INPUT -3
+#define CPSPILS_MEM_FAIL -4
+
+/*
+ * =================================================================
+ * F U N C T I O N T Y P E S
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Type : CPSpilsPrecSetupExplFn and CPSpilsPrecSetupImplFn
+ * -----------------------------------------------------------------
+ *
+ * Explicit ODE case
+ *
+ * The user-supplied preconditioner setup function PrecSetup and
+ * the user-supplied preconditioner solve function PrecSolve
+ * together must define left and right preconditoner matrices
+ * P1 and P2 (either of which may be trivial), such that the
+ * product P1*P2 is an approximation to the Newton matrix
+ * M = I - gamma*J. Here J is the system Jacobian J = df/dy,
+ * and gamma is a scalar proportional to the integration step
+ * size h. The solution of systems P z = r, with P = P1 or P2,
+ * is to be carried out by the PrecSolve function, and PrecSetup
+ * is to do any necessary setup operations.
+ *
+ * The user-supplied preconditioner setup function PrecSetup
+ * is to evaluate and preprocess any Jacobian-related data
+ * needed by the preconditioner solve function PrecSolve.
+ * This might include forming a crude approximate Jacobian,
+ * and performing an LU factorization on the resulting
+ * approximation to M. This function will not be called in
+ * advance of every call to PrecSolve, but instead will be called
+ * only as often as necessary to achieve convergence within the
+ * Newton iteration. If the PrecSolve function needs no
+ * preparation, the PrecSetup function can be NULL.
+ *
+ * For greater efficiency, the PrecSetup function may save
+ * Jacobian-related data and reuse it, rather than generating it
+ * from scratch. In this case, it should use the input flag jok
+ * to decide whether to recompute the data, and set the output
+ * flag *jcurPtr accordingly.
+ *
+ * Each call to the PrecSetup function is preceded by a call to
+ * the RhsFn f with the same (t,y) arguments. Thus the PrecSetup
+ * function can use any auxiliary data that is computed and
+ * saved by the f function and made accessible to PrecSetup.
+ *
+ * A function PrecSetup must have the prototype given below.
+ * Its parameters are as follows:
+ *
+ * t is the current value of the independent variable.
+ *
+ * y is the current value of the dependent variable vector,
+ * namely the predicted value of y(t).
+ *
+ * fy is the vector f(t,y).
+ *
+ * jok is an input flag indicating whether Jacobian-related
+ * data needs to be recomputed, as follows:
+ * jok == FALSE means recompute Jacobian-related data
+ * from scratch.
+ * jok == TRUE means that Jacobian data, if saved from
+ * the previous PrecSetup call, can be reused
+ * (with the current value of gamma).
+ * A Precset call with jok == TRUE can only occur after
+ * a call with jok == FALSE.
+ *
+ * jcurPtr is a pointer to an output integer flag which is
+ * to be set by PrecSetup as follows:
+ * Set *jcurPtr = TRUE if Jacobian data was recomputed.
+ * Set *jcurPtr = FALSE if Jacobian data was not recomputed,
+ * but saved data was reused.
+ *
+ * gamma is the scalar appearing in the Newton matrix.
+ *
+ * P_data is a pointer to user data - the same as the P_data
+ * parameter passed to the CP*SetPreconditioner function.
+ *
+ * tmp1, tmp2, and tmp3 are pointers to memory allocated
+ * for N_Vectors which can be used by
+ * CPSpilsPrecSetupFn as temporary storage or
+ * work space.
+ *
+ * NOTE: If the user's preconditioner needs other quantities,
+ * they are accessible as follows: hcur (the current stepsize)
+ * and ewt (the error weight vector) are accessible through
+ * CPodeGetCurrentStep and CPodeGetErrWeights, respectively).
+ * The unit roundoff is available as UNIT_ROUNDOFF defined in
+ * sundials_types.h.
+ *
+ * Returned value:
+ * The value to be returned by the PrecSetup function is a flag
+ * indicating whether it was successful. This value should be
+ * 0 if successful,
+ * > 0 for a recoverable error (step will be retried),
+ * < 0 for an unrecoverable error (integration is halted).
+ *
+ * -----------------------------------------------------------------
+ *
+ * Implicit ODE case
+ *
+ *
+ * -----------------------------------------------------------------
+ */
+
+typedef int (*CPSpilsPrecSetupExplFn)(realtype t, N_Vector y, N_Vector fy,
+ booleantype jok, booleantype *jcurPtr,
+ realtype gamma, void *P_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+
+typedef int (*CPSpilsPrecSetupImplFn)(realtype t, N_Vector y, N_Vector yp, N_Vector r,
+ realtype gamma, void *P_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+
+/*
+ * -----------------------------------------------------------------
+ * Type : CPSpilsPrecSolveExplFn and CPSpilsPrecSolvImplFn
+ * -----------------------------------------------------------------
+ *
+ * Explicit ODE case
+ *
+ * The user-supplied preconditioner solve function PrecSolve
+ * is to solve a linear system P z = r in which the matrix P is
+ * one of the preconditioner matrices P1 or P2, depending on the
+ * type of preconditioning chosen.
+ *
+ * A function PrecSolve must have the prototype given below.
+ * Its parameters are as follows:
+ *
+ * t is the current value of the independent variable.
+ *
+ * y is the current value of the dependent variable vector.
+ *
+ * fy is the vector f(t,y).
+ *
+ * b is the right-hand side vector of the linear system.
+ *
+ * x is the output vector computed by PrecSolve.
+ *
+ * gamma is the scalar appearing in the Newton matrix.
+ *
+ * delta is an input tolerance for use by PSolve if it uses
+ * an iterative method in its solution. In that case,
+ * the residual vector Res = r - P z of the system
+ * should be made less than delta in weighted L2 norm,
+ * i.e., sqrt [ Sum (Res[i]*ewt[i])^2 ] < delta.
+ * Note: the error weight vector ewt can be obtained
+ * through a call to the routine CPodeGetErrWeights.
+ *
+ * lr is an input flag indicating whether PrecSolve is to use
+ * the left preconditioner P1 or right preconditioner
+ * P2: lr = 1 means use P1, and lr = 2 means use P2.
+ *
+ * P_data is a pointer to user data - the same as the P_data
+ * parameter passed to the CP*SetPreconditioner function.
+ *
+ * tmp is a pointer to memory allocated for an N_Vector
+ * which can be used by PSolve for work space.
+ *
+ * Returned value:
+ * The value to be returned by the PrecSolve function is a flag
+ * indicating whether it was successful. This value should be
+ * 0 if successful,
+ * positive for a recoverable error (step will be retried),
+ * negative for an unrecoverable error (integration is halted).
+ *
+ * -----------------------------------------------------------------
+ *
+ * Implicit case ODE
+ *
+ * -----------------------------------------------------------------
+ */
+
+typedef int (*CPSpilsPrecSolveExplFn)(realtype t, N_Vector y, N_Vector fy,
+ N_Vector b, N_Vector x,
+ realtype gamma, realtype delta,
+ int lr, void *P_data, N_Vector tmp);
+
+typedef int (*CPSpilsPrecSolveImplFn)(realtype t, N_Vector y, N_Vector yp, N_Vector r,
+ N_Vector b, N_Vector x,
+ realtype gamma, realtype delta,
+ void *P_data, N_Vector tmp);
+
+/*
+ * -----------------------------------------------------------------
+ * Type : CPSpilsJacTimesVecExplFn and CPSpilsJacTimesVecImplFn
+ * -----------------------------------------------------------------
+ *
+ * Explicit ODE case
+ *
+ * The user-supplied function jtimes is to generate the product
+ * J*v for given v, where J is the Jacobian df/dy, or an
+ * approximation to it, and v is a given vector. It should return
+ * 0 if successful a positive value for a recoverable error or
+ * a negative value for an unrecoverable failure.
+ *
+ * A function jtimes must have the prototype given below. Its
+ * parameters are as follows:
+ *
+ * v is the N_Vector to be multiplied by J.
+ *
+ * Jv is the output N_Vector containing J*v.
+ *
+ * t is the current value of the independent variable.
+ *
+ * y is the current value of the dependent variable
+ * vector.
+ *
+ * fy is the vector f(t,y).
+ *
+ * jac_data is a pointer to user Jacobian data, the same as the
+ * jac_data parameter passed to the CP*SetJacTimesVecFn
+ * function.
+ *
+ * tmp is a pointer to memory allocated for an N_Vector
+ * which can be used by Jtimes for work space.
+ *
+ * -----------------------------------------------------------------
+ *
+ * Implicit ODE case
+ *
+ * -----------------------------------------------------------------
+ */
+
+typedef int (*CPSpilsJacTimesVecExplFn)(realtype t, N_Vector y, N_Vector fy,
+ N_Vector v, N_Vector Jv, void *jac_data,
+ N_Vector tmp);
+
+typedef int (*CPSpilsJacTimesVecImplFn)(realtype t, realtype gm,
+ N_Vector y, N_Vector yp, N_Vector r,
+ N_Vector v, N_Vector Jv, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2);
+
+/*
+ * =================================================================
+ * U S E R - C A L L A B L E F U N C T I O N S
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Optional inputs to the CPSPILS linear solver
+ * -----------------------------------------------------------------
+ *
+ * CPSpilsSetPrecType resets the type of preconditioner, pretype,
+ * from the value previously set.
+ * This must be one of PREC_NONE, PREC_LEFT,
+ * PREC_RIGHT, or PREC_BOTH.
+ *
+ * CPSpilsSetGSType specifies the type of Gram-Schmidt
+ * orthogonalization to be used. This must be one of
+ * the two enumeration constants MODIFIED_GS or
+ * CLASSICAL_GS defined in iterative.h. These correspond
+ * to using modified Gram-Schmidt and classical
+ * Gram-Schmidt, respectively.
+ * Default value is MODIFIED_GS.
+ *
+ * CPSpilsSetMaxl resets the maximum Krylov subspace size, maxl,
+ * from the value previously set.
+ * An input value <= 0, gives the default value.
+ *
+ * CPSpilsSetDelt specifies the factor by which the tolerance on
+ * the nonlinear iteration is multiplied to get a
+ * tolerance on the linear iteration.
+ * Default value is 0.05.
+ *
+ * CPSpilsSetPreconditioner specifies the PrecSetup and PrecSolve functions.
+ * as well as a pointer to user preconditioner data.
+ * This pointer is passed to PrecSetup and PrecSolve
+ * every time these routines are called.
+ * Default is NULL for al three arguments.
+ *
+ * CPSpilsSetJacTimesVecFn specifies the jtimes function and a pointer to
+ * user Jacobian data. This pointer is passed to jtimes every
+ * time the jtimes routine is called.
+ * Default is to use an internal finite difference
+ * approximation routine.
+ *
+ * The return value of CPSpilsSet* is one of:
+ * CPSPILS_SUCCESS if successful
+ * CPSPILS_MEM_NULL if the CPODES memory was NULL
+ * CPSPILS_LMEM_NULL if the CPSPILS memory was NULL
+ * CPSPILS_ILL_INPUT if an input has an illegal value
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPSpilsSetPrecType(void *cpode_mem, int pretype);
+SUNDIALS_EXPORT int CPSpilsSetGSType(void *cpode_mem, int gstype);
+SUNDIALS_EXPORT int CPSpilsSetMaxl(void *cpode_mem, int maxl);
+SUNDIALS_EXPORT int CPSpilsSetDelt(void *cpode_mem, realtype delt);
+SUNDIALS_EXPORT int CPSpilsSetPreconditioner(void *cpode_mem, void *pset, void *psolve, void *P_data);
+SUNDIALS_EXPORT int CPSpilsSetJacTimesVecFn(void *cpode_mem, void *jtimes, void *jac_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Optional outputs from the CPSPILS linear solver
+ * -----------------------------------------------------------------
+ * CPSpilsGetWorkSpace returns the real and integer workspace used
+ * by the SPILS module.
+ *
+ * CPSpilsGetNumPrecEvals returns the number of preconditioner
+ * evaluations, i.e. the number of calls made
+ * to PrecSetup with jok==FALSE.
+ *
+ * CPSpilsGetNumPrecSolves returns the number of calls made to
+ * PrecSolve.
+ *
+ * CPSpilsGetNumLinIters returns the number of linear iterations.
+ *
+ * CPSpilsGetNumConvFails returns the number of linear
+ * convergence failures.
+ *
+ * CPSpilsGetNumJtimesEvals returns the number of calls to jtimes.
+ *
+ * CPSpilsGetNumRhsEvals returns the number of calls to the user
+ * f routine due to finite difference Jacobian
+ * times vector evaluation.
+ *
+ * CPSpilsGetLastFlag returns the last error flag set by any of
+ * the CPSPILS interface functions.
+ *
+ * The return value of CPSpilsGet* is one of:
+ * CPSPILS_SUCCESS if successful
+ * CPSPILS_MEM_NULL if the CPODES memory was NULL
+ * CPSPILS_LMEM_NULL if the CPSPILS memory was NULL
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPSpilsGetWorkSpace(void *cpode_mem, long int *lenrwLS, long int *leniwLS);
+SUNDIALS_EXPORT int CPSpilsGetNumPrecEvals(void *cpode_mem, long int *npevals);
+SUNDIALS_EXPORT int CPSpilsGetNumPrecSolves(void *cpode_mem, long int *npsolves);
+SUNDIALS_EXPORT int CPSpilsGetNumLinIters(void *cpode_mem, long int *nliters);
+SUNDIALS_EXPORT int CPSpilsGetNumConvFails(void *cpode_mem, long int *nlcfails);
+SUNDIALS_EXPORT int CPSpilsGetNumJtimesEvals(void *cpode_mem, long int *njvevals);
+SUNDIALS_EXPORT int CPSpilsGetNumFctEvals(void *cpode_mem, long int *nfevalsLS);
+SUNDIALS_EXPORT int CPSpilsGetLastFlag(void *cpode_mem, int *flag);
+
+/*
+ * -----------------------------------------------------------------
+ * The following function returns the name of the constant
+ * associated with a CPSPILS return flag
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT char *CPSpilsGetReturnFlagName(int flag);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_sptfqmr.h b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_sptfqmr.h
new file mode 100644
index 0000000..4ac522a
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/cpodes/cpodes_sptfqmr.h
@@ -0,0 +1,66 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/29 00:05:06 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the header file for the CPODES scaled preconditioned TFQMR
+ * linear solver, CPSPTFQMR.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _CPSPTFQMR_H
+#define _CPSPTFQMR_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <cpodes/cpodes_spils.h>
+#include <sundials/sundials_sptfqmr.h>
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPSptfqmr
+ * -----------------------------------------------------------------
+ * A call to the CPSptfqmr function links the main CPODES integrator
+ * with the CPSPTFQMR linear solver.
+ *
+ * cpode_mem is the pointer to the integrator memory returned by
+ * CPodeCreate.
+ *
+ * pretype is the type of user preconditioning to be done.
+ * This must be one of the four enumeration constants
+ * PREC_NONE, PREC_LEFT, PREC_RIGHT, or PREC_BOTH defined
+ * in iterative.h. These correspond to no preconditioning,
+ * left preconditioning only, right preconditioning
+ * only, and both left and right preconditioning,
+ * respectively.
+ *
+ * maxl is the maximum Krylov dimension. This is an
+ * optional input to the CPSPTFQMR solver. Pass 0 to
+ * use the default value CPSPILS_MAXL=5.
+ *
+ * The return value of CPSptfqmr is one of:
+ * CPSPILS_SUCCESS if successful
+ * CPSPILS_MEM_NULL if the CPODES memory was NULL
+ * CPSPILS_MEM_FAIL if there was a memory allocation failure
+ * CPSPILS_ILL_INPUT if a required vector operation is missing
+ * The above constants are defined in cpodes_spils.h
+ *
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int CPSptfqmr(void *cpode_mem, int pretype, int maxl);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/nvector/nvector_parallel.h b/SimTKmath/Integrators/src/CPodes/sundials/include/nvector/nvector_parallel.h
new file mode 100644
index 0000000..f8a006c
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/nvector/nvector_parallel.h
@@ -0,0 +1,314 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/29 00:05:07 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Scott D. Cohen, Alan C. Hindmarsh, Radu Serban,
+ * and Aaron Collier @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the main header file for the MPI-enabled implementation
+ * of the NVECTOR module.
+ *
+ * Part I contains declarations specific to the parallel
+ * implementation of the supplied NVECTOR module.
+ *
+ * Part II defines accessor macros that allow the user to efficiently
+ * use the type N_Vector without making explicit references to the
+ * underlying data structure.
+ *
+ * Part III contains the prototype for the constructor
+ * N_VNew_Parallel as well as implementation-specific prototypes
+ * for various useful vector operations.
+ *
+ * Notes:
+ *
+ * - The definition of the generic N_Vector structure can be
+ * found in the header file sundials_nvector.h.
+ *
+ * - The definition of the type realtype can be found in the
+ * header file sundials_types.h, and it may be changed (at the
+ * configuration stage) according to the user's needs.
+ * The sundials_types.h file also contains the definition
+ * for the type booleantype.
+ *
+ * - N_Vector arguments to arithmetic vector operations need not
+ * be distinct. For example, the following call:
+ *
+ * N_VLinearSum_Parallel(a,x,b,y,y);
+ *
+ * (which stores the result of the operation a*x+b*y in y)
+ * is legal.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _NVECTOR_PARALLEL_H
+#define _NVECTOR_PARALLEL_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <mpi.h>
+
+#include <sundials/sundials_nvector.h>
+
+
+/*
+ * -----------------------------------------------------------------
+ * PART I: PARALLEL implementation of N_Vector
+ * -----------------------------------------------------------------
+ */
+
+/* define MPI data types */
+
+#if defined(SUNDIALS_SINGLE_PRECISION)
+
+#define PVEC_REAL_MPI_TYPE MPI_FLOAT
+
+#elif defined(SUNDIALS_DOUBLE_PRECISION)
+
+#define PVEC_REAL_MPI_TYPE MPI_DOUBLE
+
+#elif defined(SUNDIALS_EXTENDED_PRECISION)
+
+#define PVEC_REAL_MPI_TYPE MPI_LONG_DOUBLE
+
+#endif
+
+#define PVEC_INTEGER_MPI_TYPE MPI_LONG
+
+/* parallel implementation of the N_Vector 'content' structure
+ contains the global and local lengths of the vector, a pointer
+ to an array of 'realtype components', the MPI communicator,
+ and a flag indicating ownership of the data */
+
+struct _N_VectorContent_Parallel {
+ long int local_length; /* local vector length */
+ long int global_length; /* global vector length */
+ booleantype own_data; /* ownership of data */
+ realtype *data; /* local data array */
+ MPI_Comm comm; /* pointer to MPI communicator */
+};
+
+typedef struct _N_VectorContent_Parallel *N_VectorContent_Parallel;
+
+/*
+ * -----------------------------------------------------------------
+ * PART II: macros NV_CONTENT_P, NV_DATA_P, NV_OWN_DATA_P,
+ * NV_LOCLENGTH_P, NV_GLOBLENGTH_P,NV_COMM_P, and NV_Ith_P
+ * -----------------------------------------------------------------
+ * In the descriptions below, the following user declarations
+ * are assumed:
+ *
+ * N_Vector v;
+ * long int v_len, s_len, i;
+ *
+ * (1) NV_CONTENT_P
+ *
+ * This routines gives access to the contents of the parallel
+ * vector N_Vector.
+ *
+ * The assignment v_cont = NV_CONTENT_P(v) sets v_cont to be
+ * a pointer to the parallel N_Vector content structure.
+ *
+ * (2) NV_DATA_P, NV_OWN_DATA_P, NV_LOCLENGTH_P, NV_GLOBLENGTH_P,
+ * and NV_COMM_P
+ *
+ * These routines give access to the individual parts of
+ * the content structure of a parallel N_Vector.
+ *
+ * The assignment v_data = NV_DATA_P(v) sets v_data to be
+ * a pointer to the first component of the local data for
+ * the vector v. The assignment NV_DATA_P(v) = data_v sets
+ * the component array of v to be data_V by storing the
+ * pointer data_v.
+ *
+ * The assignment v_llen = NV_LOCLENGTH_P(v) sets v_llen to
+ * be the length of the local part of the vector v. The call
+ * NV_LOCLENGTH_P(v) = llen_v sets the local length
+ * of v to be llen_v.
+ *
+ * The assignment v_glen = NV_GLOBLENGTH_P(v) sets v_glen to
+ * be the global length of the vector v. The call
+ * NV_GLOBLENGTH_P(v) = glen_v sets the global length of v to
+ * be glen_v.
+ *
+ * The assignment v_comm = NV_COMM_P(v) sets v_comm to be the
+ * MPI communicator of the vector v. The assignment
+ * NV_COMM_C(v) = comm_v sets the MPI communicator of v to be
+ * comm_v.
+ *
+ * (3) NV_Ith_P
+ *
+ * In the following description, the components of the
+ * local part of an N_Vector are numbered 0..n-1, where n
+ * is the local length of (the local part of) v.
+ *
+ * The assignment r = NV_Ith_P(v,i) sets r to be the value
+ * of the ith component of the local part of the vector v.
+ * The assignment NV_Ith_P(v,i) = r sets the value of the
+ * ith local component of v to be r.
+ *
+ * Note: When looping over the components of an N_Vector v, it is
+ * more efficient to first obtain the component array via
+ * v_data = NV_DATA_P(v) and then access v_data[i] within the
+ * loop than it is to use NV_Ith_P(v,i) within the loop.
+ * -----------------------------------------------------------------
+ */
+
+#define NV_CONTENT_P(v) ( (N_VectorContent_Parallel)(v->content) )
+
+#define NV_LOCLENGTH_P(v) ( NV_CONTENT_P(v)->local_length )
+
+#define NV_GLOBLENGTH_P(v) ( NV_CONTENT_P(v)->global_length )
+
+#define NV_OWN_DATA_P(v) ( NV_CONTENT_P(v)->own_data )
+
+#define NV_DATA_P(v) ( NV_CONTENT_P(v)->data )
+
+#define NV_COMM_P(v) ( NV_CONTENT_P(v)->comm )
+
+#define NV_Ith_P(v,i) ( NV_DATA_P(v)[i] )
+
+/*
+ * -----------------------------------------------------------------
+ * PART III: functions exported by nvector_parallel
+ *
+ * CONSTRUCTORS:
+ * N_VNew_Parallel
+ * N_VNewEmpty_Parallel
+ * N_VMake_Parallel
+ * N_VCloneVectorArray_Parallel
+ * N_VCloneVectorArrayEmpty_Parallel
+ * DESTRUCTORS:
+ * N_VDestroy_Parallel
+ * N_VDestroyVectorArray_Parallel
+ * OTHER:
+ * N_VPrint_Parallel
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Function : N_VNew_Parallel
+ * -----------------------------------------------------------------
+ * This function creates and allocates memory for a parallel vector.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT N_Vector N_VNew_Parallel(MPI_Comm comm,
+ long int local_length,
+ long int global_length);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : N_VNewEmpty_Parallel
+ * -----------------------------------------------------------------
+ * This function creates a new parallel N_Vector with an empty
+ * (NULL) data array.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT N_Vector N_VNewEmpty_Parallel(MPI_Comm comm,
+ long int local_length,
+ long int global_length);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : N_VMake_Parallel
+ * -----------------------------------------------------------------
+ * This function creates and allocates memory for a parallel vector
+ * with a user-supplied data array.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT N_Vector N_VMake_Parallel(MPI_Comm comm,
+ long int local_length,
+ long int global_length,
+ realtype *v_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : N_VCloneVectorArray_Parallel
+ * -----------------------------------------------------------------
+ * This function creates an array of 'count' PARALLEL vectors by
+ * cloning a given vector w.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT N_Vector *N_VCloneVectorArray_Parallel(int count, N_Vector w);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : N_VCloneVectorArrayEmpty_Parallel
+ * -----------------------------------------------------------------
+ * This function creates an array of 'count' PARALLEL vectors each
+ * with an empty (NULL) data array by cloning w.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT N_Vector *N_VCloneVectorArrayEmpty_Parallel(int count, N_Vector w);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : N_VDestroyVectorArray_Parallel
+ * -----------------------------------------------------------------
+ * This function frees an array of N_Vector created with
+ * N_VCloneVectorArray_Parallel or N_VCloneVectorArrayEmpty_Parallel.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void N_VDestroyVectorArray_Parallel(N_Vector *vs, int count);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : N_VPrint_Parallel
+ * -----------------------------------------------------------------
+ * This function prints the content of a parallel vector to stdout.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void N_VPrint_Parallel(N_Vector v);
+
+/*
+ * -----------------------------------------------------------------
+ * parallel implementations of the vector operations
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT N_Vector N_VCloneEmpty_Parallel(N_Vector w);
+SUNDIALS_EXPORT N_Vector N_VClone_Parallel(N_Vector w);
+SUNDIALS_EXPORT void N_VDestroy_Parallel(N_Vector v);
+SUNDIALS_EXPORT void N_VSpace_Parallel(N_Vector v, long int *lrw, long int *liw);
+SUNDIALS_EXPORT realtype *N_VGetArrayPointer_Parallel(N_Vector v);
+SUNDIALS_EXPORT void N_VSetArrayPointer_Parallel(realtype *v_data, N_Vector v);
+SUNDIALS_EXPORT void N_VLinearSum_Parallel(realtype a, N_Vector x, realtype b, N_Vector y, N_Vector z);
+SUNDIALS_EXPORT void N_VConst_Parallel(realtype c, N_Vector z);
+SUNDIALS_EXPORT void N_VProd_Parallel(N_Vector x, N_Vector y, N_Vector z);
+SUNDIALS_EXPORT void N_VDiv_Parallel(N_Vector x, N_Vector y, N_Vector z);
+SUNDIALS_EXPORT void N_VScale_Parallel(realtype c, N_Vector x, N_Vector z);
+SUNDIALS_EXPORT void N_VAbs_Parallel(N_Vector x, N_Vector z);
+SUNDIALS_EXPORT void N_VInv_Parallel(N_Vector x, N_Vector z);
+SUNDIALS_EXPORT void N_VAddConst_Parallel(N_Vector x, realtype b, N_Vector z);
+SUNDIALS_EXPORT realtype N_VDotProd_Parallel(N_Vector x, N_Vector y);
+SUNDIALS_EXPORT realtype N_VMaxNorm_Parallel(N_Vector x);
+SUNDIALS_EXPORT realtype N_VWrmsNorm_Parallel(N_Vector x, N_Vector w);
+SUNDIALS_EXPORT realtype N_VWrmsNormMask_Parallel(N_Vector x, N_Vector w, N_Vector id);
+SUNDIALS_EXPORT realtype N_VMin_Parallel(N_Vector x);
+SUNDIALS_EXPORT realtype N_VWL2Norm_Parallel(N_Vector x, N_Vector w);
+SUNDIALS_EXPORT realtype N_VL1Norm_Parallel(N_Vector x);
+SUNDIALS_EXPORT void N_VCompare_Parallel(realtype c, N_Vector x, N_Vector z);
+SUNDIALS_EXPORT booleantype N_VInvTest_Parallel(N_Vector x, N_Vector z);
+SUNDIALS_EXPORT booleantype N_VConstrMask_Parallel(N_Vector c, N_Vector x, N_Vector m);
+SUNDIALS_EXPORT realtype N_VMinQuotient_Parallel(N_Vector num, N_Vector denom);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/nvector/nvector_serial.h b/SimTKmath/Integrators/src/CPodes/sundials/include/nvector/nvector_serial.h
new file mode 100644
index 0000000..4301a68
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/nvector/nvector_serial.h
@@ -0,0 +1,265 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/29 00:05:07 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Scott D. Cohen, Alan C. Hindmarsh, Radu Serban,
+ * and Aaron Collier @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the header file for the serial implementation of the
+ * NVECTOR module.
+ *
+ * Part I contains declarations specific to the serial
+ * implementation of the supplied NVECTOR module.
+ *
+ * Part II defines accessor macros that allow the user to
+ * efficiently use the type N_Vector without making explicit
+ * references to the underlying data structure.
+ *
+ * Part III contains the prototype for the constructor N_VNew_Serial
+ * as well as implementation-specific prototypes for various useful
+ * vector operations.
+ *
+ * Notes:
+ *
+ * - The definition of the generic N_Vector structure can be found
+ * in the header file sundials_nvector.h.
+ *
+ * - The definition of the type 'realtype' can be found in the
+ * header file sundials_types.h, and it may be changed (at the
+ * configuration stage) according to the user's needs.
+ * The sundials_types.h file also contains the definition
+ * for the type 'booleantype'.
+ *
+ * - N_Vector arguments to arithmetic vector operations need not
+ * be distinct. For example, the following call:
+ *
+ * N_VLinearSum_Serial(a,x,b,y,y);
+ *
+ * (which stores the result of the operation a*x+b*y in y)
+ * is legal.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _NVECTOR_SERIAL_H
+#define _NVECTOR_SERIAL_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <sundials/sundials_nvector.h>
+
+/*
+ * -----------------------------------------------------------------
+ * PART I: SERIAL implementation of N_Vector
+ * -----------------------------------------------------------------
+ */
+
+/* serial implementation of the N_Vector 'content' structure
+ contains the length of the vector, a pointer to an array
+ of 'realtype' components, and a flag indicating ownership of
+ the data */
+
+struct _N_VectorContent_Serial {
+ long int length;
+ booleantype own_data;
+ realtype *data;
+};
+
+typedef struct _N_VectorContent_Serial *N_VectorContent_Serial;
+
+/*
+ * -----------------------------------------------------------------
+ * PART II: macros NV_CONTENT_S, NV_DATA_S, NV_OWN_DATA_S,
+ * NV_LENGTH_S, and NV_Ith_S
+ * -----------------------------------------------------------------
+ * In the descriptions below, the following user declarations
+ * are assumed:
+ *
+ * N_Vector v;
+ * long int i;
+ *
+ * (1) NV_CONTENT_S
+ *
+ * This routines gives access to the contents of the serial
+ * vector N_Vector.
+ *
+ * The assignment v_cont = NV_CONTENT_S(v) sets v_cont to be
+ * a pointer to the serial N_Vector content structure.
+ *
+ * (2) NV_DATA_S NV_OWN_DATA_S and NV_LENGTH_S
+ *
+ * These routines give access to the individual parts of
+ * the content structure of a serial N_Vector.
+ *
+ * The assignment v_data = NV_DATA_S(v) sets v_data to be
+ * a pointer to the first component of v. The assignment
+ * NV_DATA_S(v) = data_V sets the component array of v to
+ * be data_v by storing the pointer data_v.
+ *
+ * The assignment v_len = NV_LENGTH_S(v) sets v_len to be
+ * the length of v. The call NV_LENGTH_S(v) = len_v sets
+ * the length of v to be len_v.
+ *
+ * (3) NV_Ith_S
+ *
+ * In the following description, the components of an
+ * N_Vector are numbered 0..n-1, where n is the length of v.
+ *
+ * The assignment r = NV_Ith_S(v,i) sets r to be the value of
+ * the ith component of v. The assignment NV_Ith_S(v,i) = r
+ * sets the value of the ith component of v to be r.
+ *
+ * Note: When looping over the components of an N_Vector v, it is
+ * more efficient to first obtain the component array via
+ * v_data = NV_DATA_S(v) and then access v_data[i] within the
+ * loop than it is to use NV_Ith_S(v,i) within the loop.
+ * -----------------------------------------------------------------
+ */
+
+#define NV_CONTENT_S(v) ( (N_VectorContent_Serial)(v->content) )
+
+#define NV_LENGTH_S(v) ( NV_CONTENT_S(v)->length )
+
+#define NV_OWN_DATA_S(v) ( NV_CONTENT_S(v)->own_data )
+
+#define NV_DATA_S(v) ( NV_CONTENT_S(v)->data )
+
+#define NV_Ith_S(v,i) ( NV_DATA_S(v)[i] )
+
+/*
+ * -----------------------------------------------------------------
+ * PART III: functions exported by nvector_serial
+ *
+ * CONSTRUCTORS:
+ * N_VNew_Serial
+ * N_VNewEmpty_Serial
+ * N_VMake_Serial
+ * N_VCloneVectorArray_Serial
+ * N_VCloneVectorArrayEmpty_Serial
+ * DESTRUCTORS:
+ * N_VDestroy_Serial
+ * N_VDestroyVectorArray_Serial
+ * OTHER:
+ * N_VPrint_Serial
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Function : N_VNew_Serial
+ * -----------------------------------------------------------------
+ * This function creates and allocates memory for a serial vector.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT N_Vector N_VNew_Serial(long int vec_length);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : N_VNewEmpty_Serial
+ * -----------------------------------------------------------------
+ * This function creates a new serial N_Vector with an empty (NULL)
+ * data array.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT N_Vector N_VNewEmpty_Serial(long int vec_length);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : N_VMake_Serial
+ * -----------------------------------------------------------------
+ * This function creates and allocates memory for a serial vector
+ * with a user-supplied data array.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT N_Vector N_VMake_Serial(long int vec_length, realtype *v_data);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : N_VCloneVectorArray_Serial
+ * -----------------------------------------------------------------
+ * This function creates an array of 'count' SERIAL vectors by
+ * cloning a given vector w.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT N_Vector *N_VCloneVectorArray_Serial(int count, N_Vector w);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : N_VCloneVectorArrayEmpty_Serial
+ * -----------------------------------------------------------------
+ * This function creates an array of 'count' SERIAL vectors each
+ * with an empty (NULL) data array by cloning w.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT N_Vector *N_VCloneVectorArrayEmpty_Serial(int count, N_Vector w);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : N_VDestroyVectorArray_Serial
+ * -----------------------------------------------------------------
+ * This function frees an array of SERIAL vectors created with
+ * N_VCloneVectorArray_Serial or N_VCloneVectorArrayEmpty_Serial.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void N_VDestroyVectorArray_Serial(N_Vector *vs, int count);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : N_VPrint_Serial
+ * -----------------------------------------------------------------
+ * This function prints the content of a serial vector to stdout.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void N_VPrint_Serial(N_Vector v);
+
+/*
+ * -----------------------------------------------------------------
+ * serial implementations of various useful vector operations
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT N_Vector N_VCloneEmpty_Serial(N_Vector w);
+SUNDIALS_EXPORT N_Vector N_VClone_Serial(N_Vector w);
+SUNDIALS_EXPORT void N_VDestroy_Serial(N_Vector v);
+SUNDIALS_EXPORT void N_VSpace_Serial(N_Vector v, long int *lrw, long int *liw);
+SUNDIALS_EXPORT realtype *N_VGetArrayPointer_Serial(N_Vector v);
+SUNDIALS_EXPORT void N_VSetArrayPointer_Serial(realtype *v_data, N_Vector v);
+SUNDIALS_EXPORT void N_VLinearSum_Serial(realtype a, N_Vector x, realtype b, N_Vector y, N_Vector z);
+SUNDIALS_EXPORT void N_VConst_Serial(realtype c, N_Vector z);
+SUNDIALS_EXPORT void N_VProd_Serial(N_Vector x, N_Vector y, N_Vector z);
+SUNDIALS_EXPORT void N_VDiv_Serial(N_Vector x, N_Vector y, N_Vector z);
+SUNDIALS_EXPORT void N_VScale_Serial(realtype c, N_Vector x, N_Vector z);
+SUNDIALS_EXPORT void N_VAbs_Serial(N_Vector x, N_Vector z);
+SUNDIALS_EXPORT void N_VInv_Serial(N_Vector x, N_Vector z);
+SUNDIALS_EXPORT void N_VAddConst_Serial(N_Vector x, realtype b, N_Vector z);
+SUNDIALS_EXPORT realtype N_VDotProd_Serial(N_Vector x, N_Vector y);
+SUNDIALS_EXPORT realtype N_VMaxNorm_Serial(N_Vector x);
+SUNDIALS_EXPORT realtype N_VWrmsNorm_Serial(N_Vector x, N_Vector w);
+SUNDIALS_EXPORT realtype N_VWrmsNormMask_Serial(N_Vector x, N_Vector w, N_Vector id);
+SUNDIALS_EXPORT realtype N_VMin_Serial(N_Vector x);
+SUNDIALS_EXPORT realtype N_VWL2Norm_Serial(N_Vector x, N_Vector w);
+SUNDIALS_EXPORT realtype N_VL1Norm_Serial(N_Vector x);
+SUNDIALS_EXPORT void N_VCompare_Serial(realtype c, N_Vector x, N_Vector z);
+SUNDIALS_EXPORT booleantype N_VInvTest_Serial(N_Vector x, N_Vector z);
+SUNDIALS_EXPORT booleantype N_VConstrMask_Serial(N_Vector c, N_Vector x, N_Vector m);
+SUNDIALS_EXPORT realtype N_VMinQuotient_Serial(N_Vector num, N_Vector denom);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_band.h b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_band.h
new file mode 100644
index 0000000..1ff6c55
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_band.h
@@ -0,0 +1,174 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.4 $
+ * $Date: 2006/11/29 00:05:07 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Alan C. Hindmarsh and Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the header file for a generic BAND linear solver
+ * package, based on the DlsMat type defined in sundials_direct.h.
+ *
+ * There are two sets of band solver routines listed in
+ * this file: one set uses type DlsMat defined below and the
+ * other set uses the type realtype ** for band matrix arguments.
+ * Routines that work with the type DlsMat begin with "Band".
+ * Routines that work with realtype ** begin with "band"
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _SUNDIALS_BAND_H
+#define _SUNDIALS_BAND_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <sundials/sundials_direct.h>
+
+/*
+ * -----------------------------------------------------------------
+ * Function : BandGBTRF
+ * -----------------------------------------------------------------
+ * Usage : ier = BandGBTRF(A, p);
+ * if (ier != 0) ... A is singular
+ * -----------------------------------------------------------------
+ * BandGBTRF performs the LU factorization of the N by N band
+ * matrix A. This is done using standard Gaussian elimination
+ * with partial pivoting.
+ *
+ * A successful LU factorization leaves the "matrix" A and the
+ * pivot array p with the following information:
+ *
+ * (1) p[k] contains the row number of the pivot element chosen
+ * at the beginning of elimination step k, k=0, 1, ..., N-1.
+ *
+ * (2) If the unique LU factorization of A is given by PA = LU,
+ * where P is a permutation matrix, L is a lower triangular
+ * matrix with all 1's on the diagonal, and U is an upper
+ * triangular matrix, then the upper triangular part of A
+ * (including its diagonal) contains U and the strictly lower
+ * triangular part of A contains the multipliers, I-L.
+ *
+ * BandGBTRF returns 0 if successful. Otherwise it encountered
+ * a zero diagonal element during the factorization. In this case
+ * it returns the column index (numbered from one) at which
+ * it encountered the zero.
+ *
+ * Important Note: A must be allocated to accommodate the increase
+ * in upper bandwidth that occurs during factorization. If
+ * mathematically, A is a band matrix with upper bandwidth mu and
+ * lower bandwidth ml, then the upper triangular factor U can
+ * have upper bandwidth as big as smu = MIN(n-1,mu+ml). The lower
+ * triangular factor L has lower bandwidth ml. Allocate A with
+ * call A = BandAllocMat(N,mu,ml,smu), where mu, ml, and smu are
+ * as defined above. The user does not have to zero the "extra"
+ * storage allocated for the purpose of factorization. This will
+ * handled by the BandGBTRF routine.
+ *
+ * BandGBTRF is only a wrapper around bandGBTRF. All work is done
+ * in bandGBTRF works directly on the data in the DlsMat A (i.e.,
+ * the field cols).
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int BandGBTRF(DlsMat A, int *p);
+SUNDIALS_EXPORT int bandGBTRF(realtype **a, int n, int mu, int ml, int smu, int *p);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : BandGBTRS
+ * -----------------------------------------------------------------
+ * Usage : BandGBTRS(A, p, b);
+ * -----------------------------------------------------------------
+ * BandGBTRS solves the N-dimensional system A x = b using
+ * the LU factorization in A and the pivot information in p
+ * computed in BandGBTRF. The solution x is returned in b. This
+ * routine cannot fail if the corresponding call to BandGBTRF
+ * did not fail.
+ *
+ * BandGBTRS is only a wrapper around bandGBTRS which does all the
+ * work directly on the data in the DlsMat A (i.e., the field cols).
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void BandGBTRS(DlsMat A, int *p, realtype *b);
+SUNDIALS_EXPORT void bandGBTRS(realtype **a, int n, int smu, int ml, int *p, realtype *b);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : BandZero
+ * -----------------------------------------------------------------
+ * Usage : BandZero(A);
+ * -----------------------------------------------------------------
+ * A(i,j) <- 0.0, j-(A->mu) <= i <= j+(A->ml).
+ *
+ * BandZero is a wrapper around bandZero which accesses the data of
+ * the DlsMat A (i.e. the field cols)
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void BandZero(DlsMat A);
+SUNDIALS_EXPORT void bandZero(realtype **a, int n, int mu, int ml, int smu);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : BandCopy
+ * -----------------------------------------------------------------
+ * Usage : BandCopy(A, B, copymu, copyml);
+ * -----------------------------------------------------------------
+ * BandCopy copies the submatrix with upper and lower bandwidths
+ * copymu, copyml of the N by N band matrix A into the N by N
+ * band matrix B.
+ *
+ * BandCopy is a wrapper around bandCopy which accesses the data
+ * in the DlsMat A and B (i.e. the fields cols)
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void BandCopy(DlsMat A, DlsMat B, int copymu, int copyml);
+SUNDIALS_EXPORT void bandCopy(realtype **a, realtype **b, int n, int a_smu, int b_smu,
+ int copymu, int copyml);
+
+/*
+ * -----------------------------------------------------------------
+ * Function: BandScale
+ * -----------------------------------------------------------------
+ * Usage : BandScale(c, A);
+ * -----------------------------------------------------------------
+ * A(i,j) <- c*A(i,j), j-(A->mu) <= i <= j+(A->ml).
+ *
+ * BandScale is a wrapper around bandScale which performs the actual
+ * scaling by accessing the data in the DlsMat A (i.e. the field
+ * cols).
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void BandScale(realtype c, DlsMat A);
+SUNDIALS_EXPORT void bandScale(realtype c, realtype **a, int n, int mu, int ml, int smu);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : BandAddI
+ * -----------------------------------------------------------------
+ * Usage : BandAddI(A);
+ * -----------------------------------------------------------------
+ * A(j,j) <- A(j,j)+1.0, 0 <= j <= (A->size)-1.
+ *
+ * BandAddI is a wrapper around bandAddI which performs the actual
+ * work by accessing the data in the DlsMat A (i.e. the field cols)
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void BandAddI(DlsMat A);
+SUNDIALS_EXPORT void bandAddI(realtype **a, int n, int smu);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_config.h b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_config.h
new file mode 100644
index 0000000..bbfd5ed
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_config.h
@@ -0,0 +1,96 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/08 00:48:24 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Aaron Collier and Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2005, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ *------------------------------------------------------------------
+ * SUNDIALS configuration header file
+ *------------------------------------------------------------------
+ */
+#ifndef SimTK_SIMMATH_SUNDIALS_CONFIG_H_
+#define SimTK_SIMMATH_SUNDIALS_CONFIG_H_
+
+/***** ADDED BY SHERM 20061121 */
+#include "SimTKcommon.h"
+
+/* Keeps MS VC++ 8 quiet about sprintf, strcpy, etc. (sherm) */
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+// No need to expose any of this; SimTK users must access CPodes
+// through the C++ API.
+#ifdef _WIN32
+ #if defined(SimTK_SIMMATH_BUILDING_SHARED_LIBRARY)
+ #define SUNDIALS_EXPORT __declspec(dllexport)
+ #elif defined(SimTK_SIMMATH_BUILDING_STATIC_LIBRARY) || defined(SimTK_USE_STATIC_LIBRARIES)
+ #define SUNDIALS_EXPORT
+ #else
+ /* i.e., a client of a shared library */
+ #define SUNDIALS_EXPORT __declspec(dllimport)
+ #endif
+#else
+ /* Linux, Mac */
+ #define SUNDIALS_EXPORT
+#endif
+
+/* Define precision of SUNDIALS data type 'realtype'
+ * Depending on the precision level, one of the following
+ * three macros will be defined:
+ * #define SUNDIALS_SINGLE_PRECISION 1
+ * #define SUNDIALS_DOUBLE_PRECISION 1
+ * #define SUNDIALS_EXTENDED_PRECISION 1
+ */
+#ifdef SimTK_DEFAULT_PRECISION
+ #if (SimTK_DEFAULT_PRECISION == 1)
+ #define SUNDIALS_SINGLE_PRECISION 1
+ #elif (SimTK_DEFAULT_PRECISION == 2)
+ #define SUNDIALS_DOUBLE_PRECISION 1
+ #elif (SimTK_DEFAULT_PRECISION == 4)
+ #define SUNDIALS_EXTENDED_PRECISION 1
+ #endif
+#else
+ #define SUNDIALS_DOUBLE_PRECISION 1
+#endif
+
+/***** END OF SHERM'S ADDITIONS */
+
+/* Define SUNDIALS version number */
+#define SUNDIALS_PACKAGE_VERSION "2.3.0"
+
+/* FCMIX: Define Fortran name-mangling macro
+ * Depending on the inferred scheme, one of the following
+ * six macros will be defined:
+ * #define F77_FUNC(name,NAME) name
+ * #define F77_FUNC(name,NAME) name ## _
+ * #define F77_FUNC(name,NAME) name ## __
+ * #define F77_FUNC(name,NAME) NAME
+ * #define F77_FUNC(name,NAME) NAME ## _
+ * #define F77_FUNC(name,NAME) NAME ## __
+ */
+#define F77_FUNC(name,NAME) name ## _
+#define F77_FUNC_(name,NAME) name ## _
+
+
+/* Use generic math functions
+ * If it was decided that generic math functions can be used, then
+ * #define SUNDIALS_USE_GENERIC_MATH 1
+ * otherwise
+ * #define SUNDIALS_USE_GENERIC_MATH 0
+ */
+#define SUNDIALS_USE_GENERIC_MATH 1
+
+/* FNVECTOR: Allow user to specify different MPI communicator
+ * If it was found that the MPI implementation supports MPI_Comm_f2c, then
+ * #define SUNDIALS_MPI_COMM_F2C 1
+ * otherwise
+ * #define SUNDIALS_MPI_COMM_F2C 0
+ */
+
+#endif // SimTK_SIMMATH_SUNDIALS_CONFIG_H_
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_config.in b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_config.in
new file mode 100644
index 0000000..c9fc8df
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_config.in
@@ -0,0 +1,66 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2007/01/24 01:05:12 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Aaron Collier and Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2005, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ *------------------------------------------------------------------
+ * SUNDIALS configuration header file
+ *------------------------------------------------------------------
+ */
+
+/* Define SUNDIALS version number */
+#define SUNDIALS_PACKAGE_VERSION "@PACKAGE_VERSION@"
+
+/* FCMIX: Define Fortran name-mangling macro
+ * Depending on the inferred scheme, one of the following
+ * six macros will be defined:
+ * #define F77_FUNC(name,NAME) name
+ * #define F77_FUNC(name,NAME) name ## _
+ * #define F77_FUNC(name,NAME) name ## __
+ * #define F77_FUNC(name,NAME) NAME
+ * #define F77_FUNC(name,NAME) NAME ## _
+ * #define F77_FUNC(name,NAME) NAME ## __
+ */
+ at F77_MANGLE_MACRO1@
+ at F77_MANGLE_MACRO2@
+
+/* Define precision of SUNDIALS data type 'realtype'
+ * Depending on the precision level, one of the following
+ * three macros will be defined:
+ * #define SUNDIALS_SINGLE_PRECISION 1
+ * #define SUNDIALS_DOUBLE_PRECISION 1
+ * #define SUNDIALS_EXTENDED_PRECISION 1
+ */
+ at PRECISION_LEVEL@
+
+/* Use generic math functions
+ * If it was decided that generic math functions can be used, then
+ * #define SUNDIALS_USE_GENERIC_MATH 1
+ * otherwise
+ * #define SUNDIALS_USE_GENERIC_MATH 0
+ */
+ at GENERIC_MATH_LIB@
+
+/* FNVECTOR: Allow user to specify different MPI communicator
+ * If it was found that the MPI implementation supports MPI_Comm_f2c, then
+ * #define SUNDIALS_MPI_COMM_F2C 1
+ * otherwise
+ * #define SUNDIALS_MPI_COMM_F2C 0
+ */
+ at F77_MPI_COMM_F2C@
+
+/* Mark SUNDIALS API functions for export/import
+ * When building shared SUNDIALS libraries under Windows, use
+ * #define SUNDIALS_EXPORT __declspec(dllexport)
+ * When linking to shared SUNDIALS libraries under Windows, use
+ * #define SUNDIALS_EXPORT __declspec(dllimport)
+ * In all other cases (other platforms or static libraries under
+ * Windows), the SUNDIALS_EXPORT macro is empty
+ */
+ at SUNDIALS_EXPORT@
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_dense.h b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_dense.h
new file mode 100644
index 0000000..20460ac
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_dense.h
@@ -0,0 +1,207 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.6 $
+ * $Date: 2006/11/29 00:05:07 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the header file for a generic package of DENSE matrix
+ * operations, based on the DlsMat type defined in sundials_direct.h.
+ *
+ * There are two sets of dense solver routines listed in
+ * this file: one set uses type DlsMat defined below and the
+ * other set uses the type realtype ** for dense matrix arguments.
+ * Routines that work with the type DlsMat begin with "Dense".
+ * Routines that work with realtype** begin with "dense".
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _SUNDIALS_DENSE_H
+#define _SUNDIALS_DENSE_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <sundials/sundials_direct.h>
+
+/*
+ * -----------------------------------------------------------------
+ * Functions: DenseGETRF and DenseGETRS
+ * -----------------------------------------------------------------
+ * DenseGETRF performs the LU factorization of the M by N dense
+ * matrix A. This is done using standard Gaussian elimination
+ * with partial (row) pivoting. Note that this applies only
+ * to matrices with M >= N and full column rank.
+ *
+ * A successful LU factorization leaves the matrix A and the
+ * pivot array p with the following information:
+ *
+ * (1) p[k] contains the row number of the pivot element chosen
+ * at the beginning of elimination step k, k=0, 1, ..., N-1.
+ *
+ * (2) If the unique LU factorization of A is given by PA = LU,
+ * where P is a permutation matrix, L is a lower trapezoidal
+ * matrix with all 1's on the diagonal, and U is an upper
+ * triangular matrix, then the upper triangular part of A
+ * (including its diagonal) contains U and the strictly lower
+ * trapezoidal part of A contains the multipliers, I-L.
+ *
+ * For square matrices (M=N), L is unit lower triangular.
+ *
+ * DenseGETRF returns 0 if successful. Otherwise it encountered
+ * a zero diagonal element during the factorization. In this case
+ * it returns the column index (numbered from one) at which
+ * it encountered the zero.
+ *
+ * DenseGETRS solves the N-dimensional system A x = b using
+ * the LU factorization in A and the pivot information in p
+ * computed in DenseGETRF. The solution x is returned in b. This
+ * routine cannot fail if the corresponding call to DenseGETRF
+ * did not fail.
+ * DenseGETRS does NOT check for a square matrix!
+ *
+ * -----------------------------------------------------------------
+ * DenseGETRF and DenseGETRS are simply wrappers around denseGETRF
+ * and denseGETRS, respectively, which perform all the work by
+ * directly accessing the data in the DlsMat A (i.e. the field cols)
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int DenseGETRF(DlsMat A, int *p);
+SUNDIALS_EXPORT void DenseGETRS(DlsMat A, int *p, realtype *b);
+
+SUNDIALS_EXPORT int denseGETRF(realtype **a, int m, int n, int *p);
+SUNDIALS_EXPORT void denseGETRS(realtype **a, int n, int *p, realtype *b);
+
+/*
+ * -----------------------------------------------------------------
+ * Functions : DensePOTRF and DensePOTRS
+ * -----------------------------------------------------------------
+ * DensePOTRF computes the Cholesky factorization of a real symmetric
+ * positive definite matrix A.
+ * -----------------------------------------------------------------
+ * DensePOTRS solves a system of linear equations A*X = B with a
+ * symmetric positive definite matrix A using the Cholesky factorization
+ * A = L*L**T computed by DensePOTRF.
+ *
+ * -----------------------------------------------------------------
+ * DensePOTRF and DensePOTRS are simply wrappers around densePOTRF
+ * and densePOTRS, respectively, which perform all the work by
+ * directly accessing the data in the DlsMat A (i.e. the field cols)
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int DensePOTRF(DlsMat A);
+SUNDIALS_EXPORT void DensePOTRS(DlsMat A, realtype *b);
+
+SUNDIALS_EXPORT int densePOTRF(realtype **a, int m);
+SUNDIALS_EXPORT void densePOTRS(realtype **a, int m, realtype *b);
+
+/*
+ * -----------------------------------------------------------------
+ * Functions : DenseGEQRF and DenseORMQR
+ * -----------------------------------------------------------------
+ * DenseGEQRF computes a QR factorization of a real M-by-N matrix A:
+ * A = Q * R (with M>= N).
+ *
+ * DenseGEQRF requires a temporary work vector wrk of length M.
+ * -----------------------------------------------------------------
+ * DenseORMQR computes the product w = Q * v where Q is a real
+ * orthogonal matrix defined as the product of k elementary reflectors
+ *
+ * Q = H(1) H(2) . . . H(k)
+ *
+ * as returned by DenseGEQRF. Q is an M-by-N matrix, v is a vector
+ * of length N and w is a vector of length M (with M>=N).
+ *
+ * DenseORMQR requires a temporary work vector wrk of length M.
+ *
+ * -----------------------------------------------------------------
+ * DenseGEQRF and DenseORMQR are simply wrappers around denseGEQRF
+ * and denseORMQR, respectively, which perform all the work by
+ * directly accessing the data in the DlsMat A (i.e. the field cols)
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int DenseGEQRF(DlsMat A, realtype *beta, realtype *wrk);
+SUNDIALS_EXPORT int DenseORMQR(DlsMat A, realtype *beta, realtype *vn, realtype *vm,
+ realtype *wrk);
+
+SUNDIALS_EXPORT int denseGEQRF(realtype **a, int m, int n, realtype *beta, realtype *v);
+SUNDIALS_EXPORT int denseORMQR(realtype **a, int m, int n, realtype *beta,
+ realtype *v, realtype *w, realtype *wrk);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : DenseZero
+ * -----------------------------------------------------------------
+ * DenseZero sets all the elements of the M-by-N matrix A to 0.0.
+ *
+ * DenseZero is a wrapper around denseZero which accesses the data of
+ * the DlsMat A (i.e. the field cols)
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void DenseZero(DlsMat A);
+SUNDIALS_EXPORT void denseZero(realtype **a, int m, int n);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : DenseCopy
+ * -----------------------------------------------------------------
+ * DenseCopy copies the contents of the M-by-N matrix A into the
+ * M-by-N matrix B.
+ *
+ * DenseCopy is a wrapper around denseCopy which accesses the data
+ * in the DlsMat A and B (i.e. the fields cols)
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void DenseCopy(DlsMat A, DlsMat B);
+SUNDIALS_EXPORT void denseCopy(realtype **a, realtype **b, int m, int n);
+
+/*
+ * -----------------------------------------------------------------
+ * Function: DenseScale
+ * -----------------------------------------------------------------
+ * DenseScale scales the elements of the M-by-N matrix A by the
+ * constant c and stores the result back in A.
+ *
+ * DenseScale is a wrapper around denseScale which performs the actual
+ * scaling by accessing the data in the DlsMat A (i.e. the field
+ * cols).
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void DenseScale(realtype c, DlsMat A);
+SUNDIALS_EXPORT void denseScale(realtype c, realtype **a, int m, int n);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : DenseAddI
+ * -----------------------------------------------------------------
+ * DenseAddI adds 1.0 to the main diagonal (A_ii, i=1,2,...,N-1) of
+ * the M-by-N matrix A (M>= N) and stores the result back in A.
+ * DenseAddI is typically used with square matrices.
+ * DenseAddI does not check for M >= N and therefore a segmentation
+ * fault will occur if M < N!
+ *
+ * DenseAddI is a wrapper around denseAddI which performs the actual
+ * work by accessing the data in the DlsMat A (i.e. the field cols)
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void DenseAddI(DlsMat A);
+SUNDIALS_EXPORT void denseAddI(realtype **a, int n);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_direct.h b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_direct.h
new file mode 100644
index 0000000..3ad8ebb
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_direct.h
@@ -0,0 +1,297 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/29 00:05:07 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This header file contains definitions and declarations for use by
+ * generic direct linear solvers for Ax = b. It defines types for
+ * dense and banded matrices and corresponding accessor macros.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _SUNDIALS_DIRECT_H
+#define _SUNDIALS_DIRECT_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <sundials/sundials_types.h>
+
+/*
+ * =================================================================
+ * C O N S T A N T S
+ * =================================================================
+ */
+
+/*
+ * SUNDIALS_DENSE: dense matrix
+ * SUNDIALS_BAND: banded matrix
+ */
+
+#define SUNDIALS_DENSE 1
+#define SUNDIALS_BAND 2
+
+/*
+ * ==================================================================
+ * Type definitions
+ * ==================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Type : DlsMat
+ * -----------------------------------------------------------------
+ * The type DlsMat is defined to be a pointer to a structure
+ * with various sizes, a data field, and an array of pointers to
+ * the columns which defines a dense or band matrix for use in
+ * direct linear solvers. The M and N fields indicates the number
+ * of rows and columns, respectively. The data field is a one
+ * dimensional array used for component storage. The cols field
+ * stores the pointers in data for the beginning of each column.
+ * -----------------------------------------------------------------
+ * For DENSE matrices, the relevant fields in DlsMat are:
+ * type = SUNDIALS_DENSE
+ * M - number of rows
+ * N - number of columns
+ * ldim - leading dimension (ldim >= M)
+ * data - pointer to a contiguous block of realtype variables
+ * ldata - length of the data array =ldim*N
+ * cols - array of pointers. cols[j] points to the first element
+ * of the j-th column of the matrix in the array data.
+ *
+ * The elements of a dense matrix are stored columnwise (i.e columns
+ * are stored one on top of the other in memory).
+ * If A is of type DlsMat, then the (i,j)th element of A (with
+ * 0 <= i < M and 0 <= j < N) is given by (A->data)[j*n+i].
+ *
+ * The DENSE_COL and DENSE_ELEM macros below allow a user to access
+ * efficiently individual matrix elements without writing out explicit
+ * data structure references and without knowing too much about the
+ * underlying element storage. The only storage assumption needed is
+ * that elements are stored columnwise and that a pointer to the
+ * jth column of elements can be obtained via the DENSE_COL macro.
+ * -----------------------------------------------------------------
+ * For BAND matrices, the relevant fields in DlsMat are:
+ * type = SUNDIALS_BAND
+ * M - number of rows
+ * N - number of columns
+ * mu - upper bandwidth, 0 <= mu <= min(M,N)
+ * ml - lower bandwidth, 0 <= ml <= min(M,N)
+ * s_mu - storage upper bandwidth, mu <= s_mu <= N-1.
+ * The dgbtrf routine writes the LU factors into the storage
+ * for A. The upper triangular factor U, however, may have
+ * an upper bandwidth as big as MIN(N-1,mu+ml) because of
+ * partial pivoting. The s_mu field holds the upper
+ * bandwidth allocated for A.
+ * ldim - leading dimension (ldim >= s_mu)
+ * data - pointer to a contiguous block of realtype variables
+ * ldata - length of the data array =ldim*(s_mu+ml+1)
+ * cols - array of pointers. cols[j] points to the first element
+ * of the j-th column of the matrix in the array data.
+ *
+ * The BAND_COL, BAND_COL_ELEM, and BAND_ELEM macros below allow a
+ * user to access individual matrix elements without writing out
+ * explicit data structure references and without knowing too much
+ * about the underlying element storage. The only storage assumption
+ * needed is that elements are stored columnwise and that a pointer
+ * into the jth column of elements can be obtained via the BAND_COL
+ * macro. The BAND_COL_ELEM macro selects an element from a column
+ * which has already been isolated via BAND_COL. The macro
+ * BAND_COL_ELEM allows the user to avoid the translation
+ * from the matrix location (i,j) to the index in the array returned
+ * by BAND_COL at which the (i,j)th element is stored.
+ * -----------------------------------------------------------------
+ */
+
+typedef struct _DlsMat {
+ int type;
+ int M;
+ int N;
+ int ldim;
+ int mu;
+ int ml;
+ int s_mu;
+ realtype *data;
+ int ldata;
+ realtype **cols;
+} *DlsMat;
+
+/*
+ * ==================================================================
+ * Data accessor macros
+ * ==================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * DENSE_COL and DENSE_ELEM
+ * -----------------------------------------------------------------
+ *
+ * DENSE_COL(A,j) references the jth column of the M-by-N dense
+ * matrix A, 0 <= j < N. The type of the expression DENSE_COL(A,j)
+ * is (realtype *). After the assignment in the usage above, col_j
+ * may be treated as an array indexed from 0 to M-1. The (i,j)-th
+ * element of A is thus referenced by col_j[i].
+ *
+ * DENSE_ELEM(A,i,j) references the (i,j)th element of the dense
+ * M-by-N matrix A, 0 <= i < M ; 0 <= j < N.
+ *
+ * -----------------------------------------------------------------
+ */
+
+#define DENSE_COL(A,j) ((A->cols)[j])
+#define DENSE_ELEM(A,i,j) ((A->cols)[j][i])
+
+/*
+ * -----------------------------------------------------------------
+ * BAND_COL, BAND_COL_ELEM, and BAND_ELEM
+ * -----------------------------------------------------------------
+ *
+ * BAND_COL(A,j) references the diagonal element of the jth column
+ * of the N by N band matrix A, 0 <= j <= N-1. The type of the
+ * expression BAND_COL(A,j) is realtype *. The pointer returned by
+ * the call BAND_COL(A,j) can be treated as an array which is
+ * indexed from -(A->mu) to (A->ml).
+ *
+ * BAND_COL_ELEM references the (i,j)th entry of the band matrix A
+ * when used in conjunction with BAND_COL. The index (i,j) should
+ * satisfy j-(A->mu) <= i <= j+(A->ml).
+ *
+ * BAND_ELEM(A,i,j) references the (i,j)th element of the M-by-N
+ * band matrix A, where 0 <= i,j <= N-1. The location (i,j) should
+ * further satisfy j-(A->mu) <= i <= j+(A->ml).
+ *
+ * -----------------------------------------------------------------
+ */
+
+#define BAND_COL(A,j) (((A->cols)[j])+(A->s_mu))
+#define BAND_COL_ELEM(col_j,i,j) (col_j[(i)-(j)])
+#define BAND_ELEM(A,i,j) ((A->cols)[j][(i)-(j)+(A->s_mu)])
+
+/*
+ * ==================================================================
+ * Exported function prototypes (functions working on dlsMat)
+ * ==================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Function: NewDenseMat
+ * -----------------------------------------------------------------
+ * NewDenseMat allocates memory for an M-by-N dense matrix and
+ * returns the storage allocated (type DlsMat). NewDenseMat
+ * returns NULL if the request for matrix storage cannot be
+ * satisfied. See the above documentation for the type DlsMat
+ * for matrix storage details.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT DlsMat NewDenseMat(int M, int N);
+
+/*
+ * -----------------------------------------------------------------
+ * Function: NewBandMat
+ * -----------------------------------------------------------------
+ * NewBandMat allocates memory for an M-by-N band matrix
+ * with upper bandwidth mu, lower bandwidth ml, and storage upper
+ * bandwidth smu. Pass smu as follows depending on whether A will
+ * be LU factored:
+ *
+ * (1) Pass smu = mu if A will not be factored.
+ *
+ * (2) Pass smu = MIN(N-1,mu+ml) if A will be factored.
+ *
+ * NewBandMat returns the storage allocated (type DlsMat) or
+ * NULL if the request for matrix storage cannot be satisfied.
+ * See the documentation for the type DlsMat for matrix storage
+ * details.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT DlsMat NewBandMat(int N, int mu, int ml, int smu);
+
+/*
+ * -----------------------------------------------------------------
+ * Functions: DestroyMat
+ * -----------------------------------------------------------------
+ * DestroyMat frees the memory allocated by NewDenseMat or NewBandMat
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void DestroyMat(DlsMat A);
+
+/*
+ * -----------------------------------------------------------------
+ * Function: NewIntArray
+ * -----------------------------------------------------------------
+ * NewIntArray allocates memory an array of N integers and returns
+ * the pointer to the memory it allocates. If the request for
+ * memory storage cannot be satisfied, it returns NULL.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int *NewIntArray(int N);
+
+/*
+ * -----------------------------------------------------------------
+ * Function: NewRealArray
+ * -----------------------------------------------------------------
+ * NewRealArray allocates memory an array of N realtype and returns
+ * the pointer to the memory it allocates. If the request for
+ * memory storage cannot be satisfied, it returns NULL.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT realtype *NewRealArray(int N);
+
+/*
+ * -----------------------------------------------------------------
+ * Function: DestroyArray
+ * -----------------------------------------------------------------
+ * DestroyArray frees memory allocated by NewIntArray or by
+ * NewRealArray.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void DestroyArray(void *p);
+
+/*
+ * -----------------------------------------------------------------
+ * Functions: PrintMat
+ * -----------------------------------------------------------------
+ * This function prints the M-by-N (dense or band) matrix A to
+ * standard output as it would normally appear on paper.
+ * It is intended as debugging tools with small values of M and N.
+ * The elements are printed using the %g/%lg/%Lg option.
+ * A blank line is printed before and after the matrix.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void PrintMat(DlsMat A);
+
+/*
+ * ==================================================================
+ * Exported function prototypes (functions working on realtype**)
+ * ==================================================================
+ */
+
+SUNDIALS_EXPORT realtype **newDenseMat(int m, int n);
+SUNDIALS_EXPORT realtype **newBandMat(int n, int smu, int ml);
+SUNDIALS_EXPORT void destroyMat(realtype **a);
+SUNDIALS_EXPORT int *newIntArray(int n);
+SUNDIALS_EXPORT realtype *newRealArray(int m);
+SUNDIALS_EXPORT void destroyArray(void *v);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_fnvector.h b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_fnvector.h
new file mode 100644
index 0000000..bbc9a95
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_fnvector.h
@@ -0,0 +1,41 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.1 $
+ * $Date: 2006/07/05 15:27:52 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Radu Serban and Aaron Collier @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This file (companion of nvector.h) contains definitions
+ * needed for the initialization of vector operations in Fortran.
+ * -----------------------------------------------------------------
+ */
+
+
+#ifndef _FNVECTOR_H
+#define _FNVECTOR_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#ifndef _SUNDIALS_CONFIG_H
+#define _SUNDIALS_CONFIG_H
+#include <sundials/sundials_config.h>
+#endif
+
+/* SUNDIALS solver IDs */
+
+#define FCMIX_CVODE 1
+#define FCMIX_IDA 2
+#define FCMIX_KINSOL 3
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_iterative.h b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_iterative.h
new file mode 100644
index 0000000..b90d7b3
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_iterative.h
@@ -0,0 +1,242 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/29 00:05:07 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Scott D. Cohen and Alan C. Hindmarsh @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This header file contains declarations intended for use by
+ * generic iterative solvers of Ax = b. The enumeration gives
+ * symbolic names for the type of preconditioning to be used.
+ * The function type declarations give the prototypes for the
+ * functions to be called within an iterative linear solver, that
+ * are responsible for
+ * multiplying A by a given vector v (ATimesFn), and
+ * solving the preconditioner equation Pz = r (PSolveFn).
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _ITERATIVE_H
+#define _ITERATIVE_H
+
+#include <sundials/sundials_nvector.h>
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+
+/*
+ * -----------------------------------------------------------------
+ * enum : types of preconditioning
+ * -----------------------------------------------------------------
+ * PREC_NONE : The iterative linear solver should not use
+ * preconditioning.
+ *
+ * PREC_LEFT : The iterative linear solver uses preconditioning on
+ * the left only.
+ *
+ * PREC_RIGHT : The iterative linear solver uses preconditioning on
+ * the right only.
+ *
+ * PREC_BOTH : The iterative linear solver uses preconditioning on
+ * both the left and the right.
+ * -----------------------------------------------------------------
+ */
+
+enum { PREC_NONE, PREC_LEFT, PREC_RIGHT, PREC_BOTH };
+
+/*
+ * -----------------------------------------------------------------
+ * enum : types of Gram-Schmidt routines
+ * -----------------------------------------------------------------
+ * MODIFIED_GS : The iterative solver uses the modified
+ * Gram-Schmidt routine ModifiedGS listed in this
+ * file.
+ *
+ * CLASSICAL_GS : The iterative solver uses the classical
+ * Gram-Schmidt routine ClassicalGS listed in this
+ * file.
+ * -----------------------------------------------------------------
+ */
+
+enum { MODIFIED_GS = 1, CLASSICAL_GS = 2 };
+
+/*
+ * -----------------------------------------------------------------
+ * Type: ATimesFn
+ * -----------------------------------------------------------------
+ * An ATimesFn multiplies Av and stores the result in z. The
+ * caller is responsible for allocating memory for the z vector.
+ * The parameter A_data is a pointer to any information about A
+ * which the function needs in order to do its job. The vector v
+ * is unchanged. An ATimesFn returns 0 if successful and a
+ * non-zero value if unsuccessful.
+ * -----------------------------------------------------------------
+ */
+
+typedef int (*ATimesFn)(void *A_data, N_Vector v, N_Vector z);
+
+/*
+ * -----------------------------------------------------------------
+ * Type: PSolveFn
+ * -----------------------------------------------------------------
+ * A PSolveFn solves the preconditioner equation Pz = r for the
+ * vector z. The caller is responsible for allocating memory for
+ * the z vector. The parameter P_data is a pointer to any
+ * information about P which the function needs in order to do
+ * its job. The parameter lr is input, and indicates whether P
+ * is to be taken as the left preconditioner or the right
+ * preconditioner: lr = 1 for left and lr = 2 for right.
+ * If preconditioning is on one side only, lr can be ignored.
+ * The vector r is unchanged.
+ * A PSolveFn returns 0 if successful and a non-zero value if
+ * unsuccessful. On a failure, a negative return value indicates
+ * an unrecoverable condition, while a positive value indicates
+ * a recoverable one, in which the calling routine may reattempt
+ * the solution after updating preconditioner data.
+ * -----------------------------------------------------------------
+ */
+
+typedef int (*PSolveFn)(void *P_data, N_Vector r, N_Vector z, int lr);
+
+/*
+ * -----------------------------------------------------------------
+ * Function: ModifiedGS
+ * -----------------------------------------------------------------
+ * ModifiedGS performs a modified Gram-Schmidt orthogonalization
+ * of the N_Vector v[k] against the p unit N_Vectors at
+ * v[k-1], v[k-2], ..., v[k-p].
+ *
+ * v is an array of (k+1) N_Vectors v[i], i=0, 1, ..., k.
+ * v[k-1], v[k-2], ..., v[k-p] are assumed to have L2-norm
+ * equal to 1.
+ *
+ * h is the output k by k Hessenberg matrix of inner products.
+ * This matrix must be allocated row-wise so that the (i,j)th
+ * entry is h[i][j]. The inner products (v[i],v[k]),
+ * i=i0, i0+1, ..., k-1, are stored at h[i][k-1]. Here
+ * i0=MAX(0,k-p).
+ *
+ * k is the index of the vector in the v array that needs to be
+ * orthogonalized against previous vectors in the v array.
+ *
+ * p is the number of previous vectors in the v array against
+ * which v[k] is to be orthogonalized.
+ *
+ * new_vk_norm is a pointer to memory allocated by the caller to
+ * hold the Euclidean norm of the orthogonalized vector v[k].
+ *
+ * If (k-p) < 0, then ModifiedGS uses p=k. The orthogonalized
+ * v[k] is NOT normalized and is stored over the old v[k]. Once
+ * the orthogonalization has been performed, the Euclidean norm
+ * of v[k] is stored in (*new_vk_norm).
+ *
+ * ModifiedGS returns 0 to indicate success. It cannot fail.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int ModifiedGS(N_Vector *v, realtype **h, int k, int p,
+ realtype *new_vk_norm);
+
+/*
+ * -----------------------------------------------------------------
+ * Function: ClassicalGS
+ * -----------------------------------------------------------------
+ * ClassicalGS performs a classical Gram-Schmidt
+ * orthogonalization of the N_Vector v[k] against the p unit
+ * N_Vectors at v[k-1], v[k-2], ..., v[k-p]. The parameters v, h,
+ * k, p, and new_vk_norm are as described in the documentation
+ * for ModifiedGS.
+ *
+ * temp is an N_Vector which can be used as workspace by the
+ * ClassicalGS routine.
+ *
+ * s is a length k array of realtype which can be used as
+ * workspace by the ClassicalGS routine.
+ *
+ * ClassicalGS returns 0 to indicate success. It cannot fail.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int ClassicalGS(N_Vector *v, realtype **h, int k, int p,
+ realtype *new_vk_norm, N_Vector temp, realtype *s);
+
+/*
+ * -----------------------------------------------------------------
+ * Function: QRfact
+ * -----------------------------------------------------------------
+ * QRfact performs a QR factorization of the Hessenberg matrix H.
+ *
+ * n is the problem size; the matrix H is (n+1) by n.
+ *
+ * h is the (n+1) by n Hessenberg matrix H to be factored. It is
+ * stored row-wise.
+ *
+ * q is an array of length 2*n containing the Givens rotations
+ * computed by this function. A Givens rotation has the form:
+ * | c -s |
+ * | s c |.
+ * The components of the Givens rotations are stored in q as
+ * (c, s, c, s, ..., c, s).
+ *
+ * job is a control flag. If job==0, then a new QR factorization
+ * is performed. If job!=0, then it is assumed that the first
+ * n-1 columns of h have already been factored and only the last
+ * column needs to be updated.
+ *
+ * QRfact returns 0 if successful. If a zero is encountered on
+ * the diagonal of the triangular factor R, then QRfact returns
+ * the equation number of the zero entry, where the equations are
+ * numbered from 1, not 0. If QRsol is subsequently called in
+ * this situation, it will return an error because it could not
+ * divide by the zero diagonal entry.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int QRfact(int n, realtype **h, realtype *q, int job);
+
+/*
+ * -----------------------------------------------------------------
+ * Function: QRsol
+ * -----------------------------------------------------------------
+ * QRsol solves the linear least squares problem
+ *
+ * min (b - H*x, b - H*x), x in R^n,
+ *
+ * where H is a Hessenberg matrix, and b is in R^(n+1).
+ * It uses the QR factors of H computed by QRfact.
+ *
+ * n is the problem size; the matrix H is (n+1) by n.
+ *
+ * h is a matrix (computed by QRfact) containing the upper
+ * triangular factor R of the original Hessenberg matrix H.
+ *
+ * q is an array of length 2*n (computed by QRfact) containing
+ * the Givens rotations used to factor H.
+ *
+ * b is the (n+1)-vector appearing in the least squares problem
+ * above.
+ *
+ * On return, b contains the solution x of the least squares
+ * problem, if QRsol was successful.
+ *
+ * QRsol returns a 0 if successful. Otherwise, a zero was
+ * encountered on the diagonal of the triangular factor R.
+ * In this case, QRsol returns the equation number (numbered
+ * from 1, not 0) of the zero entry.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int QRsol(int n, realtype **h, realtype *q, realtype *b);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_lapack.h b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_lapack.h
new file mode 100644
index 0000000..719fbf1
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_lapack.h
@@ -0,0 +1,158 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.4 $
+ * $Date: 2006/11/29 00:05:07 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the header file for a generic package of direct matrix
+ * operations for use with BLAS/LAPACK.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _SUNDIALS_LAPACK_H
+#define _SUNDIALS_LAPACK_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <sundials/sundials_direct.h>
+
+/*
+ * ==================================================================
+ * Exported functions
+ * ==================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Functions: LapackDenseZero and LapackBandZero
+ * -----------------------------------------------------------------
+ * These functions set all the elements of the M-by-N matrix A to 0.0.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void LapackDenseZero(DlsMat A);
+SUNDIALS_EXPORT void LapackBandZero(DlsMat A);
+
+/*
+ * -----------------------------------------------------------------
+ * Functions: LapackDenseAddI and LapackBandAddI
+ * -----------------------------------------------------------------
+ * These functions add 1.0 to the main diagonal (A_ii, i=1,2,...,N-1)
+ * of the M-by-N matrix A (M>= N) and stores the result back in A.
+ * They are typically used with square matrices.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void LapackDenseAddI(DlsMat A);
+SUNDIALS_EXPORT void LapackBandAddI(DlsMat A);
+
+/*
+ * ==================================================================
+ * Blas and Lapack functions
+ * ==================================================================
+ */
+
+#if defined(F77_FUNC)
+
+#define dcopy_f77 F77_FUNC(dcopy, DCOPY)
+#define dscal_f77 F77_FUNC(dscal, DSCAL)
+#define dgemv_f77 F77_FUNC(dgemv, DGEMV)
+#define dtrsv_f77 F77_FUNC(dtrsv, DTRSV)
+#define dsyrk_f77 F77_FUNC(dsyrk, DSKYR)
+
+#define dgbtrf_f77 F77_FUNC(dgbtrf, DGBTRF)
+#define dgbtrs_f77 F77_FUNC(dgbtrs, DGBTRS)
+#define dgetrf_f77 F77_FUNC(dgetrf, DGETRF)
+#define dgetrs_f77 F77_FUNC(dgetrs, DGETRS)
+#define dgeqp3_f77 F77_FUNC(dgeqp3, DGEQP3)
+#define dgeqrf_f77 F77_FUNC(dgeqrf, DGEQRF)
+#define dormqr_f77 F77_FUNC(dormqr, DORMQR)
+#define dpotrf_f77 F77_FUNC(dpotrf, DPOTRF)
+#define dpotrs_f77 F77_FUNC(dpotrs, DPOTRS)
+
+#else
+
+#define dcopy_f77 dcopy_
+#define dscal_f77 dscal_
+#define dgemv_f77 dgemv_
+#define dtrsv_f77 dtrsv_
+#define dsyrk_f77 dsyrk_
+
+#define dgbtrf_f77 dgbtrf_
+#define dgbtrs_f77 dgbtrs_
+#define dgeqp3_f77 dgeqp3_
+#define dgeqrf_f77 dgeqrf_
+#define dgetrf_f77 dgetrf_
+#define dgetrs_f77 dgetrs_
+#define dormqr_f77 dormqr_
+#define dpotrf_f77 dpotrf_
+#define dpotrs_f77 dpotrs_
+
+#endif
+
+/* Level-1 BLAS */
+
+extern void dcopy_f77(int *n, const double *x, const int *inc_x, double *y, const int *inc_y);
+extern void dscal_f77(int *n, const double *alpha, double *x, const int *inc_x);
+
+/* Level-2 BLAS */
+
+extern void dgemv_f77(const char *trans, int *m, int *n, const double *alpha, const double *a,
+ int *lda, const double *x, int *inc_x, const double *beta, double *y, int *inc_y,
+ int len_trans);
+
+extern void dtrsv_f77(const char *uplo, const char *trans, const char *diag, const int *n,
+ const double *a, const int *lda, double *x, const int *inc_x,
+ int len_uplo, int len_trans, int len_diag);
+
+/* Level-3 BLAS */
+
+extern void dsyrk_f77(const char *uplo, const char *trans, const int *n, const int *k,
+ const double *alpha, const double *a, const int *lda, const double *beta,
+ const double *c, const int *ldc, int len_uplo, int len_trans);
+
+/* LAPACK */
+
+extern void dgbtrf_f77(const int *m, const int *n, const int *kl, const int *ku,
+ double *ab, int *ldab, int *ipiv, int *info);
+
+extern void dgbtrs_f77(const char *trans, const int *n, const int *kl, const int *ku, const int *nrhs,
+ double *ab, const int *ldab, int *ipiv, double *b, const int *ldb,
+ int *info, int len_trans);
+
+
+extern void dgeqp3_f77(const int *m, const int *n, double *a, const int *lda, int *jpvt, double *tau,
+ double *work, const int *lwork, int *info);
+
+extern void dgeqrf_f77(const int *m, const int *n, double *a, const int *lda, double *tau, double *work,
+ const int *lwork, int *info);
+
+extern void dgetrf_f77(const int *m, const int *n, double *a, int *lda, int *ipiv, int *info);
+
+extern void dgetrs_f77(const char *trans, const int *n, const int *nrhs, double *a, const int *lda,
+ int *ipiv, double *b, const int *ldb, int *info, int len_trans);
+
+
+extern void dormqr_f77(const char *side, const char *trans, const int *m, const int *n, const int *k,
+ double *a, const int *lda, double *tau, double *c, const int *ldc,
+ double *work, const int *lwork, int *info, int len_side, int len_trans);
+
+extern void dpotrf_f77(const char *uplo, const int *n, double *a, int *lda, int *info, int len_uplo);
+
+extern void dpotrs_f77(const char *uplo, const int *n, const int *nrhs, double *a, const int *lda,
+ double *b, const int *ldb, int * info, int len_uplo);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_math.h b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_math.h
new file mode 100644
index 0000000..99de085
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_math.h
@@ -0,0 +1,139 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/29 00:05:07 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Scott D. Cohen, Alan C. Hindmarsh and
+ * Aaron Collier @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the header file for a simple C-language math library. The
+ * routines listed here work with the type realtype as defined in
+ * the header file sundials_types.h.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _SUNDIALSMATH_H
+#define _SUNDIALSMATH_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <sundials/sundials_types.h>
+
+/*
+ * -----------------------------------------------------------------
+ * Macros : MIN and MAX
+ * -----------------------------------------------------------------
+ * MIN(A,B) returns the minimum of A and B
+ *
+ * MAX(A,B) returns the maximum of A and B
+ *
+ * SQR(A) returns A^2
+ * -----------------------------------------------------------------
+ */
+
+#ifndef MIN
+#define MIN(A, B) ((A) < (B) ? (A) : (B))
+#endif
+
+#ifndef MAX
+#define MAX(A, B) ((A) > (B) ? (A) : (B))
+#endif
+
+#ifndef SQR
+#define SQR(A) ((A)*(A))
+#endif
+
+#ifndef ABS
+#define ABS RAbs
+#endif
+
+#ifndef SQRT
+#define SQRT RSqrt
+#endif
+
+#ifndef EXP
+#define EXP RExp
+#endif
+
+/*
+ * -----------------------------------------------------------------
+ * Function : RPowerI
+ * -----------------------------------------------------------------
+ * Usage : int exponent;
+ * realtype base, ans;
+ * ans = RPowerI(base,exponent);
+ * -----------------------------------------------------------------
+ * RPowerI returns the value of base^exponent, where base is of type
+ * realtype and exponent is of type int.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT realtype RPowerI(realtype base, int exponent);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : RPowerR
+ * -----------------------------------------------------------------
+ * Usage : realtype base, exponent, ans;
+ * ans = RPowerR(base,exponent);
+ * -----------------------------------------------------------------
+ * RPowerR returns the value of base^exponent, where both base and
+ * exponent are of type realtype. If base < ZERO, then RPowerR
+ * returns ZERO.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT realtype RPowerR(realtype base, realtype exponent);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : RSqrt
+ * -----------------------------------------------------------------
+ * Usage : realtype sqrt_x;
+ * sqrt_x = RSqrt(x);
+ * -----------------------------------------------------------------
+ * RSqrt(x) returns the square root of x. If x < ZERO, then RSqrt
+ * returns ZERO.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT realtype RSqrt(realtype x);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : RAbs (a.k.a. ABS)
+ * -----------------------------------------------------------------
+ * Usage : realtype abs_x;
+ * abs_x = RAbs(x);
+ * -----------------------------------------------------------------
+ * RAbs(x) returns the absolute value of x.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT realtype RAbs(realtype x);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : RExp (a.k.a. EXP)
+ * -----------------------------------------------------------------
+ * Usage : realtype exp_x;
+ * exp_x = RExp(x);
+ * -----------------------------------------------------------------
+ * RExp(x) returns e^x (base-e exponential function).
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT realtype RExp(realtype x);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_nvector.h b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_nvector.h
new file mode 100644
index 0000000..5d09381
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_nvector.h
@@ -0,0 +1,373 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/29 00:05:07 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Radu Serban and Aaron Collier @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the header file for a generic NVECTOR package.
+ * It defines the N_Vector structure (_generic_N_Vector) which
+ * contains the following fields:
+ * - an implementation-dependent 'content' field which contains
+ * the description and actual data of the vector
+ * - an 'ops' filed which contains a structure listing operations
+ * acting on such vectors
+ *
+ * Part I of this file contains type declarations for the
+ * _generic_N_Vector and _generic_N_Vector_Ops structures, as well
+ * as references to pointers to such structures (N_Vector).
+ *
+ * Part II of this file contains the prototypes for the vector
+ * functions which operate on N_Vector.
+ *
+ * At a minimum, a particular implementation of an NVECTOR must
+ * do the following:
+ * - specify the 'content' field of N_Vector,
+ * - implement the operations on those N_Vectors,
+ * - provide a constructor routine for new vectors
+ *
+ * Additionally, an NVECTOR implementation may provide the following:
+ * - macros to access the underlying N_Vector data
+ * - a constructor for an array of N_Vectors
+ * - a constructor for an empty N_Vector (i.e., a new N_Vector with
+ * a NULL data pointer).
+ * - a routine to print the content of an N_Vector
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _NVECTOR_H
+#define _NVECTOR_H
+
+#include <sundials/sundials_types.h>
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+/*
+ * -----------------------------------------------------------------
+ * Generic definition of N_Vector
+ * -----------------------------------------------------------------
+ */
+
+/* Forward reference for pointer to N_Vector_Ops object */
+typedef struct _generic_N_Vector_Ops *N_Vector_Ops;
+
+/* Forward reference for pointer to N_Vector object */
+typedef struct _generic_N_Vector *N_Vector;
+
+/* Define array of N_Vectors */
+typedef N_Vector *N_Vector_S;
+
+/* Structure containing function pointers to vector operations */
+struct _generic_N_Vector_Ops {
+ N_Vector (*nvclone)(N_Vector);
+ N_Vector (*nvcloneempty)(N_Vector);
+ void (*nvdestroy)(N_Vector);
+ void (*nvspace)(N_Vector, long int *, long int *);
+ realtype* (*nvgetarraypointer)(N_Vector);
+ void (*nvsetarraypointer)(realtype *, N_Vector);
+ void (*nvlinearsum)(realtype, N_Vector, realtype, N_Vector, N_Vector);
+ void (*nvconst)(realtype, N_Vector);
+ void (*nvprod)(N_Vector, N_Vector, N_Vector);
+ void (*nvdiv)(N_Vector, N_Vector, N_Vector);
+ void (*nvscale)(realtype, N_Vector, N_Vector);
+ void (*nvabs)(N_Vector, N_Vector);
+ void (*nvinv)(N_Vector, N_Vector);
+ void (*nvaddconst)(N_Vector, realtype, N_Vector);
+ realtype (*nvdotprod)(N_Vector, N_Vector);
+ realtype (*nvmaxnorm)(N_Vector);
+ realtype (*nvwrmsnorm)(N_Vector, N_Vector);
+ realtype (*nvwrmsnormmask)(N_Vector, N_Vector, N_Vector);
+ realtype (*nvmin)(N_Vector);
+ realtype (*nvwl2norm)(N_Vector, N_Vector);
+ realtype (*nvl1norm)(N_Vector);
+ void (*nvcompare)(realtype, N_Vector, N_Vector);
+ booleantype (*nvinvtest)(N_Vector, N_Vector);
+ booleantype (*nvconstrmask)(N_Vector, N_Vector, N_Vector);
+ realtype (*nvminquotient)(N_Vector, N_Vector);
+};
+
+/*
+ * -----------------------------------------------------------------
+ * A vector is a structure with an implementation-dependent
+ * 'content' field, and a pointer to a structure of vector
+ * operations corresponding to that implementation.
+ * -----------------------------------------------------------------
+ */
+
+struct _generic_N_Vector {
+ void *content;
+ struct _generic_N_Vector_Ops *ops;
+};
+
+/*
+ * -----------------------------------------------------------------
+ * Functions exported by NVECTOR module
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * N_VClone
+ * Creates a new vector of the same type as an existing vector.
+ * It does not copy the vector, but rather allocates storage for
+ * the new vector.
+ *
+ * N_VCloneEmpty
+ * Creates a new vector of the same type as an existing vector,
+ * but does not allocate storage.
+ *
+ * N_VDestroy
+ * Destroys a vector created with N_VClone.
+ *
+ * N_VSpace
+ * Returns space requirements for one N_Vector (type 'realtype' in
+ * lrw and type 'long int' in liw).
+ *
+ * N_VGetArrayPointer
+ * Returns a pointer to the data component of the given N_Vector.
+ * NOTE: This function assumes that the internal data is stored
+ * as a contiguous 'realtype' array. This routine is only used in
+ * the solver-specific interfaces to the dense and banded linear
+ * solvers, as well as the interfaces to the banded preconditioners
+ * distributed with SUNDIALS.
+ *
+ * N_VSetArrayPointer
+ * Overwrites the data field in the given N_Vector with a user-supplied
+ * array of type 'realtype'.
+ * NOTE: This function assumes that the internal data is stored
+ * as a contiguous 'realtype' array. This routine is only used in
+ * the interfaces to the dense linear solver.
+ *
+ * N_VLinearSum
+ * Performs the operation z = a*x + b*y
+ *
+ * N_VConst
+ * Performs the operation z[i] = c for i = 0, 1, ..., N-1
+ *
+ * N_VProd
+ * Performs the operation z[i] = x[i]*y[i] for i = 0, 1, ..., N-1
+ *
+ * N_VDiv
+ * Performs the operation z[i] = x[i]/y[i] for i = 0, 1, ..., N-1
+ *
+ * N_VScale
+ * Performs the operation z = c*x
+ *
+ * N_VAbs
+ * Performs the operation z[i] = |x[i]| for i = 0, 1, ..., N-1
+ *
+ * N_VInv
+ * Performs the operation z[i] = 1/x[i] for i = 0, 1, ..., N-1
+ * This routine does not check for division by 0. It should be
+ * called only with an N_Vector x which is guaranteed to have
+ * all non-zero components.
+ *
+ * N_VAddConst
+ * Performs the operation z[i] = x[i] + b for i = 0, 1, ..., N-1
+ *
+ * N_VDotProd
+ * Returns the dot product of two vectors:
+ * sum (i = 0 to N-1) {x[i]*y[i]}
+ *
+ * N_VMaxNorm
+ * Returns the maximum norm of x:
+ * max (i = 0 to N-1) ABS(x[i])
+ *
+ * N_VWrmsNorm
+ * Returns the weighted root mean square norm of x with weight
+ * vector w:
+ * sqrt [(sum (i = 0 to N-1) {(x[i]*w[i])^2})/N]
+ *
+ * N_VWrmsNormMask
+ * Returns the weighted root mean square norm of x with weight
+ * vector w, masked by the elements of id:
+ * sqrt [(sum (i = 0 to N-1) {(x[i]*w[i]*msk[i])^2})/N]
+ * where msk[i] = 1.0 if id[i] > 0 and
+ * msk[i] = 0.0 if id[i] < 0
+ *
+ * N_VMin
+ * Returns the smallest element of x:
+ * min (i = 0 to N-1) x[i]
+ *
+ * N_VWL2Norm
+ * Returns the weighted Euclidean L2 norm of x with weight
+ * vector w:
+ * sqrt [(sum (i = 0 to N-1) {(x[i]*w[i])^2})]
+ *
+ * N_VL1Norm
+ * Returns the L1 norm of x:
+ * sum (i = 0 to N-1) {ABS(x[i])}
+ *
+ * N_VCompare
+ * Performs the operation
+ * z[i] = 1.0 if ABS(x[i]) >= c i = 0, 1, ..., N-1
+ * 0.0 otherwise
+ *
+ * N_VInvTest
+ * Performs the operation z[i] = 1/x[i] with a test for
+ * x[i] == 0.0 before inverting x[i].
+ * This routine returns TRUE if all components of x are non-zero
+ * (successful inversion) and returns FALSE otherwise.
+ *
+ * N_VConstrMask
+ * Performs the operation :
+ * m[i] = 1.0 if constraint test fails for x[i]
+ * m[i] = 0.0 if constraint test passes for x[i]
+ * where the constraint tests are as follows:
+ * If c[i] = +2.0, then x[i] must be > 0.0.
+ * If c[i] = +1.0, then x[i] must be >= 0.0.
+ * If c[i] = -1.0, then x[i] must be <= 0.0.
+ * If c[i] = -2.0, then x[i] must be < 0.0.
+ * This routine returns a boolean FALSE if any element failed
+ * the constraint test, TRUE if all passed. It also sets a
+ * mask vector m, with elements equal to 1.0 where the
+ * corresponding constraint test failed, and equal to 0.0
+ * where the constraint test passed.
+ * This routine is specialized in that it is used only for
+ * constraint checking.
+ *
+ * N_VMinQuotient
+ * Performs the operation :
+ * minq = min ( num[i]/denom[i]) over all i such that
+ * denom[i] != 0.
+ * This routine returns the minimum of the quotients obtained
+ * by term-wise dividing num[i] by denom[i]. A zero element
+ * in denom will be skipped. If no such quotients are found,
+ * then the large value BIG_REAL is returned.
+ *
+ * -----------------------------------------------------------------
+ *
+ * The following table lists the vector functions used by
+ * different modules in SUNDIALS. The symbols in the table
+ * have the following meaning:
+ * S - called by the solver;
+ * D - called by the dense linear solver module
+ * B - called by the band linear solver module
+ * Di - called by the diagonal linear solver module
+ * I - called by the iterative linear solver module
+ * BP - called by the band preconditioner module
+ * BBDP - called by the band-block diagonal preconditioner module
+ * F - called by the Fortran-to-C interface
+ *
+ * ------------------------------------------------
+ * MODULES
+ * NVECTOR ------------------------------------------------
+ * FUNCTIONS CVODE/CVODES IDA KINSOL
+ * -----------------------------------------------------------------
+ * N_VClone S Di I S I BBDP S I BBDP
+ * -----------------------------------------------------------------
+ * N_VCloneEmpty F F F
+ * -----------------------------------------------------------------
+ * N_VDestroy S Di I S I BBDP S I BBDP
+ * -----------------------------------------------------------------
+ * N_VSpace S S S
+ * -----------------------------------------------------------------
+ * N_VGetArrayPointer D B BP BBDP F D B BBDP BBDP F
+ * -----------------------------------------------------------------
+ * N_VSetArrayPointer D F D F
+ * -----------------------------------------------------------------
+ * N_VLinearSum S D Di I S D I S I
+ * -----------------------------------------------------------------
+ * N_VConst S I S I I
+ * -----------------------------------------------------------------
+ * N_VProd S Di I S I S I
+ * -----------------------------------------------------------------
+ * N_VDiv S Di I S I S I
+ * -----------------------------------------------------------------
+ * N_VScale S D B Di I BP BBDP S D B I BBDP S I BBDP
+ * -----------------------------------------------------------------
+ * N_VAbs S S S
+ * -----------------------------------------------------------------
+ * N_VInv S Di S S
+ * -----------------------------------------------------------------
+ * N_VAddConst S Di S
+ * -----------------------------------------------------------------
+ * N_VDotProd I I I
+ * -----------------------------------------------------------------
+ * N_VMaxNorm S S S
+ * -----------------------------------------------------------------
+ * N_VWrmsNorm S D B I BP BBDP S
+ * -----------------------------------------------------------------
+ * N_VWrmsNormMask S
+ * -----------------------------------------------------------------
+ * N_VMin S S S
+ * -----------------------------------------------------------------
+ * N_VWL2Norm S I
+ * -----------------------------------------------------------------
+ * N_VL1Norm I
+ * -----------------------------------------------------------------
+ * N_VCompare Di S
+ * -----------------------------------------------------------------
+ * N_VInvTest Di
+ * -----------------------------------------------------------------
+ * N_VConstrMask S S
+ * -----------------------------------------------------------------
+ * N_VMinQuotient S S
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT N_Vector N_VClone(N_Vector w);
+SUNDIALS_EXPORT N_Vector N_VCloneEmpty(N_Vector w);
+SUNDIALS_EXPORT void N_VDestroy(N_Vector v);
+SUNDIALS_EXPORT void N_VSpace(N_Vector v, long int *lrw, long int *liw);
+SUNDIALS_EXPORT realtype *N_VGetArrayPointer(N_Vector v);
+SUNDIALS_EXPORT void N_VSetArrayPointer(realtype *v_data, N_Vector v);
+SUNDIALS_EXPORT void N_VLinearSum(realtype a, N_Vector x, realtype b, N_Vector y, N_Vector z);
+SUNDIALS_EXPORT void N_VConst(realtype c, N_Vector z);
+SUNDIALS_EXPORT void N_VProd(N_Vector x, N_Vector y, N_Vector z);
+SUNDIALS_EXPORT void N_VDiv(N_Vector x, N_Vector y, N_Vector z);
+SUNDIALS_EXPORT void N_VScale(realtype c, N_Vector x, N_Vector z);
+SUNDIALS_EXPORT void N_VAbs(N_Vector x, N_Vector z);
+SUNDIALS_EXPORT void N_VInv(N_Vector x, N_Vector z);
+SUNDIALS_EXPORT void N_VAddConst(N_Vector x, realtype b, N_Vector z);
+SUNDIALS_EXPORT realtype N_VDotProd(N_Vector x, N_Vector y);
+SUNDIALS_EXPORT realtype N_VMaxNorm(N_Vector x);
+SUNDIALS_EXPORT realtype N_VWrmsNorm(N_Vector x, N_Vector w);
+SUNDIALS_EXPORT realtype N_VWrmsNormMask(N_Vector x, N_Vector w, N_Vector id);
+SUNDIALS_EXPORT realtype N_VMin(N_Vector x);
+SUNDIALS_EXPORT realtype N_VWL2Norm(N_Vector x, N_Vector w);
+SUNDIALS_EXPORT realtype N_VL1Norm(N_Vector x);
+SUNDIALS_EXPORT void N_VCompare(realtype c, N_Vector x, N_Vector z);
+SUNDIALS_EXPORT booleantype N_VInvTest(N_Vector x, N_Vector z);
+SUNDIALS_EXPORT booleantype N_VConstrMask(N_Vector c, N_Vector x, N_Vector m);
+SUNDIALS_EXPORT realtype N_VMinQuotient(N_Vector num, N_Vector denom);
+
+/*
+ * -----------------------------------------------------------------
+ * Additional functions exported by NVECTOR module
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * N_VCloneEmptyVectorArray
+ * Creates (by cloning 'w') an array of 'count' empty N_Vectors
+ *
+ * N_VCloneVectorArray
+ * Creates (by cloning 'w') an array of 'count' N_Vectors
+ *
+ * N_VDestroyVectorArray
+ * Frees memory for an array of 'count' N_Vectors that was
+ * created by a call to N_VCloneVectorArray
+ *
+ * These functions are used by the SPGMR iterative linear solver
+ * module and by the CVODES and IDAS solvers.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT N_Vector *N_VCloneEmptyVectorArray(int count, N_Vector w);
+SUNDIALS_EXPORT N_Vector *N_VCloneVectorArray(int count, N_Vector w);
+SUNDIALS_EXPORT void N_VDestroyVectorArray(N_Vector *vs, int count);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_spbcgs.h b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_spbcgs.h
new file mode 100644
index 0000000..d569d1d
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_spbcgs.h
@@ -0,0 +1,199 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/29 00:05:07 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Peter Brown and Aaron Collier @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2004, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the header file for the implementation of the scaled,
+ * preconditioned Bi-CGSTAB (SPBCG) iterative linear solver.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _SPBCG_H
+#define _SPBCG_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <sundials/sundials_iterative.h>
+
+/*
+ * -----------------------------------------------------------------
+ * Types: struct SpbcgMemRec and struct *SpbcgMem
+ * -----------------------------------------------------------------
+ * A variable declaration of type struct *SpbcgMem denotes a pointer
+ * to a data structure of type struct SpbcgMemRec. The SpbcgMemRec
+ * structure contains numerous fields that must be accessed by the
+ * SPBCG linear solver module.
+ *
+ * l_max maximum Krylov subspace dimension that SpbcgSolve will
+ * be permitted to use
+ *
+ * r vector (type N_Vector) which holds the scaled, preconditioned
+ * linear system residual
+ *
+ * r_star vector (type N_Vector) which holds the initial scaled,
+ * preconditioned linear system residual
+ *
+ * p, q, u and Ap vectors (type N_Vector) used for workspace by
+ * the SPBCG algorithm
+ *
+ * vtemp scratch vector (type N_Vector) used as temporary vector
+ * storage
+ * -----------------------------------------------------------------
+ */
+
+typedef struct {
+
+ int l_max;
+
+ N_Vector r_star;
+ N_Vector r;
+ N_Vector p;
+ N_Vector q;
+ N_Vector u;
+ N_Vector Ap;
+ N_Vector vtemp;
+
+} SpbcgMemRec, *SpbcgMem;
+
+/*
+ * -----------------------------------------------------------------
+ * Function : SpbcgMalloc
+ * -----------------------------------------------------------------
+ * SpbcgMalloc allocates additional memory needed by the SPBCG
+ * linear solver module.
+ *
+ * l_max maximum Krylov subspace dimension that SpbcgSolve will
+ * be permitted to use
+ *
+ * vec_tmpl implementation-specific template vector (type N_Vector)
+ * (created using either N_VNew_Serial or N_VNew_Parallel)
+ *
+ * If successful, SpbcgMalloc returns a non-NULL memory pointer. If
+ * an error occurs, then a NULL pointer is returned.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT SpbcgMem SpbcgMalloc(int l_max, N_Vector vec_tmpl);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : SpbcgSolve
+ * -----------------------------------------------------------------
+ * SpbcgSolve solves the linear system Ax = b by means of a scaled
+ * preconditioned Bi-CGSTAB (SPBCG) iterative method.
+ *
+ * mem pointer to an internal memory block allocated during a
+ * prior call to SpbcgMalloc
+ *
+ * A_data pointer to a data structure containing information
+ * about the coefficient matrix A (passed to user-supplied
+ * function referenced by atimes (function pointer))
+ *
+ * x vector (type N_Vector) containing initial guess x_0 upon
+ * entry, but which upon return contains an approximate solution
+ * of the linear system Ax = b (solution only valid if return
+ * value is either SPBCG_SUCCESS or SPBCG_RES_REDUCED)
+ *
+ * b vector (type N_Vector) set to the right-hand side vector b
+ * of the linear system (undisturbed by function)
+ *
+ * pretype variable (type int) indicating the type of
+ * preconditioning to be used (see sundials_iterative.h)
+ *
+ * delta tolerance on the L2 norm of the scaled, preconditioned
+ * residual (if return value == SPBCG_SUCCESS, then
+ * ||sb*P1_inv*(b-Ax)||_L2 <= delta)
+ *
+ * P_data pointer to a data structure containing preconditioner
+ * information (passed to user-supplied function referenced
+ * by psolve (function pointer))
+ *
+ * sx vector (type N_Vector) containing positive scaling factors
+ * for x (pass sx == NULL if scaling NOT required)
+ *
+ * sb vector (type N_Vector) containing positive scaling factors
+ * for b (pass sb == NULL if scaling NOT required)
+ *
+ * atimes user-supplied routine responsible for computing the
+ * matrix-vector product Ax (see sundials_iterative.h)
+ *
+ * psolve user-supplied routine responsible for solving the
+ * preconditioned linear system Pz = r (ignored if
+ * pretype == PREC_NONE) (see sundials_iterative.h)
+ *
+ * res_norm pointer (type realtype*) to the L2 norm of the
+ * scaled, preconditioned residual (if return value
+ * is either SPBCG_SUCCESS or SPBCG_RES_REDUCED, then
+ * *res_norm = ||sb*P1_inv*(b-Ax)||_L2, where x is
+ * the computed approximate solution, sb is the diagonal
+ * scaling matrix for the right-hand side b, and P1_inv
+ * is the inverse of the left-preconditioner matrix)
+ *
+ * nli pointer (type int*) to the total number of linear
+ * iterations performed
+ *
+ * nps pointer (type int*) to the total number of calls made
+ * to the psolve routine
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int SpbcgSolve(SpbcgMem mem, void *A_data, N_Vector x, N_Vector b,
+ int pretype, realtype delta, void *P_data, N_Vector sx,
+ N_Vector sb, ATimesFn atimes, PSolveFn psolve,
+ realtype *res_norm, int *nli, int *nps);
+
+/* Return values for SpbcgSolve */
+
+#define SPBCG_SUCCESS 0 /* SPBCG algorithm converged */
+#define SPBCG_RES_REDUCED 1 /* SPBCG did NOT converge, but the
+ residual was reduced */
+#define SPBCG_CONV_FAIL 2 /* SPBCG algorithm failed to converge */
+#define SPBCG_PSOLVE_FAIL_REC 3 /* psolve failed recoverably */
+#define SPBCG_ATIMES_FAIL_REC 4 /* atimes failed recoverably */
+#define SPBCG_PSET_FAIL_REC 5 /* pset faild recoverably */
+
+#define SPBCG_MEM_NULL -1 /* mem argument is NULL */
+#define SPBCG_ATIMES_FAIL_UNREC -2 /* atimes returned failure flag */
+#define SPBCG_PSOLVE_FAIL_UNREC -3 /* psolve failed unrecoverably */
+#define SPBCG_PSET_FAIL_UNREC -4 /* pset failed unrecoverably */
+
+/*
+ * -----------------------------------------------------------------
+ * Function : SpbcgFree
+ * -----------------------------------------------------------------
+ * SpbcgFree frees the memory allocated by a call to SpbcgMalloc.
+ * It is illegal to use the pointer mem after a call to SpbcgFree.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void SpbcgFree(SpbcgMem mem);
+
+/*
+ * -----------------------------------------------------------------
+ * Macro : SPBCG_VTEMP
+ * -----------------------------------------------------------------
+ * This macro provides access to the vector r in the
+ * memory block of the SPBCG module. The argument mem is the
+ * memory pointer returned by SpbcgMalloc, of type SpbcgMem,
+ * and the macro value is of type N_Vector.
+ *
+ * Note: Only used by IDA (r contains P_inverse F if nli_inc == 0).
+ * -----------------------------------------------------------------
+ */
+
+#define SPBCG_VTEMP(mem) (mem->r)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_spgmr.h b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_spgmr.h
new file mode 100644
index 0000000..c557acd
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_spgmr.h
@@ -0,0 +1,296 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/29 00:05:07 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Scott D. Cohen, Alan C. Hindmarsh and
+ * Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the header file for the implementation of SPGMR Krylov
+ * iterative linear solver. The SPGMR algorithm is based on the
+ * Scaled Preconditioned GMRES (Generalized Minimal Residual)
+ * method.
+ *
+ * The SPGMR algorithm solves a linear system A x = b.
+ * Preconditioning is allowed on the left, right, or both.
+ * Scaling is allowed on both sides, and restarts are also allowed.
+ * We denote the preconditioner and scaling matrices as follows:
+ * P1 = left preconditioner
+ * P2 = right preconditioner
+ * S1 = diagonal matrix of scale factors for P1-inverse b
+ * S2 = diagonal matrix of scale factors for P2 x
+ * The matrices A, P1, and P2 are not required explicitly; only
+ * routines that provide A, P1-inverse, and P2-inverse as
+ * operators are required.
+ *
+ * In this notation, SPGMR applies the underlying GMRES method to
+ * the equivalent transformed system
+ * Abar xbar = bbar , where
+ * Abar = S1 (P1-inverse) A (P2-inverse) (S2-inverse) ,
+ * bbar = S1 (P1-inverse) b , and xbar = S2 P2 x .
+ *
+ * The scaling matrices must be chosen so that vectors S1
+ * P1-inverse b and S2 P2 x have dimensionless components.
+ * If preconditioning is done on the left only (P2 = I), by a
+ * matrix P, then S2 must be a scaling for x, while S1 is a
+ * scaling for P-inverse b, and so may also be taken as a scaling
+ * for x. Similarly, if preconditioning is done on the right only
+ * (P1 = I, P2 = P), then S1 must be a scaling for b, while S2 is
+ * a scaling for P x, and may also be taken as a scaling for b.
+ *
+ * The stopping test for the SPGMR iterations is on the L2 norm of
+ * the scaled preconditioned residual:
+ * || bbar - Abar xbar ||_2 < delta
+ * with an input test constant delta.
+ *
+ * The usage of this SPGMR solver involves supplying two routines
+ * and making three calls. The user-supplied routines are
+ * atimes (A_data, x, y) to compute y = A x, given x,
+ * and
+ * psolve (P_data, x, y, lr)
+ * to solve P1 x = y or P2 x = y for x, given y.
+ * The three user calls are:
+ * mem = SpgmrMalloc(lmax, vec_tmpl);
+ * to initialize memory,
+ * flag = SpgmrSolve(mem,A_data,x,b,...,
+ * P_data,s1,s2,atimes,psolve,...);
+ * to solve the system, and
+ * SpgmrFree(mem);
+ * to free the memory created by SpgmrMalloc.
+ * Complete details for specifying atimes and psolve and for the
+ * usage calls are given in the paragraphs below and in iterative.h.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _SPGMR_H
+#define _SPGMR_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <sundials/sundials_iterative.h>
+
+/*
+ * -----------------------------------------------------------------
+ * Types: SpgmrMemRec, SpgmrMem
+ * -----------------------------------------------------------------
+ * SpgmrMem is a pointer to an SpgmrMemRec which contains
+ * the memory needed by SpgmrSolve. The SpgmrMalloc routine
+ * returns a pointer of type SpgmrMem which should then be passed
+ * in subsequent calls to SpgmrSolve. The SpgmrFree routine frees
+ * the memory allocated by SpgmrMalloc.
+ *
+ * l_max is the maximum Krylov dimension that SpgmrSolve will be
+ * permitted to use.
+ *
+ * V is the array of Krylov basis vectors v_1, ..., v_(l_max+1),
+ * stored in V[0], ..., V[l_max], where l_max is the second
+ * parameter to SpgmrMalloc. Each v_i is a vector of type
+ * N_Vector.
+ *
+ * Hes is the (l_max+1) x l_max Hessenberg matrix. It is stored
+ * row-wise so that the (i,j)th element is given by Hes[i][j].
+ *
+ * givens is a length 2*l_max array which represents the
+ * Givens rotation matrices that arise in the algorithm. The
+ * Givens rotation matrices F_0, F_1, ..., F_j, where F_i is
+ *
+ * 1
+ * 1
+ * c_i -s_i <--- row i
+ * s_i c_i
+ * 1
+ * 1
+ *
+ * are represented in the givens vector as
+ * givens[0]=c_0, givens[1]=s_0, givens[2]=c_1, givens[3]=s_1,
+ * ..., givens[2j]=c_j, givens[2j+1]=s_j.
+ *
+ * xcor is a vector (type N_Vector) which holds the scaled,
+ * preconditioned correction to the initial guess.
+ *
+ * yg is a length (l_max+1) array of realtype used to hold "short"
+ * vectors (e.g. y and g).
+ *
+ * vtemp is a vector (type N_Vector) used as temporary vector
+ * storage during calculations.
+ * -----------------------------------------------------------------
+ */
+
+typedef struct _SpgmrMemRec {
+
+ int l_max;
+
+ N_Vector *V;
+ realtype **Hes;
+ realtype *givens;
+ N_Vector xcor;
+ realtype *yg;
+ N_Vector vtemp;
+
+} SpgmrMemRec, *SpgmrMem;
+
+/*
+ * -----------------------------------------------------------------
+ * Function : SpgmrMalloc
+ * -----------------------------------------------------------------
+ * SpgmrMalloc allocates the memory used by SpgmrSolve. It
+ * returns a pointer of type SpgmrMem which the user of the
+ * SPGMR package should pass to SpgmrSolve. The parameter l_max
+ * is the maximum Krylov dimension that SpgmrSolve will be
+ * permitted to use. The parameter vec_tmpl is a pointer to an
+ * N_Vector used as a template to create new vectors by duplication.
+ * This routine returns NULL if there is a memory request failure.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT SpgmrMem SpgmrMalloc(int l_max, N_Vector vec_tmpl);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : SpgmrSolve
+ * -----------------------------------------------------------------
+ * SpgmrSolve solves the linear system Ax = b using the SPGMR
+ * method. The return values are given by the symbolic constants
+ * below. The first SpgmrSolve parameter is a pointer to memory
+ * allocated by a prior call to SpgmrMalloc.
+ *
+ * mem is the pointer returned by SpgmrMalloc to the structure
+ * containing the memory needed by SpgmrSolve.
+ *
+ * A_data is a pointer to information about the coefficient
+ * matrix A. This pointer is passed to the user-supplied function
+ * atimes.
+ *
+ * x is the initial guess x_0 upon entry and the solution
+ * N_Vector upon exit with return value SPGMR_SUCCESS or
+ * SPGMR_RES_REDUCED. For all other return values, the output x
+ * is undefined.
+ *
+ * b is the right hand side N_Vector. It is undisturbed by this
+ * function.
+ *
+ * pretype is the type of preconditioning to be used. Its
+ * legal possible values are enumerated in iterativ.h. These
+ * values are PREC_NONE=0, PREC_LEFT=1, PREC_RIGHT=2, and
+ * PREC_BOTH=3.
+ *
+ * gstype is the type of Gram-Schmidt orthogonalization to be
+ * used. Its legal values are enumerated in iterativ.h. These
+ * values are MODIFIED_GS=0 and CLASSICAL_GS=1.
+ *
+ * delta is the tolerance on the L2 norm of the scaled,
+ * preconditioned residual. On return with value SPGMR_SUCCESS,
+ * this residual satisfies || s1 P1_inv (b - Ax) ||_2 <= delta.
+ *
+ * max_restarts is the maximum number of times the algorithm is
+ * allowed to restart.
+ *
+ * P_data is a pointer to preconditioner information. This
+ * pointer is passed to the user-supplied function psolve.
+ *
+ * s1 is an N_Vector of positive scale factors for P1-inv b, where
+ * P1 is the left preconditioner. (Not tested for positivity.)
+ * Pass NULL if no scaling on P1-inv b is required.
+ *
+ * s2 is an N_Vector of positive scale factors for P2 x, where
+ * P2 is the right preconditioner. (Not tested for positivity.)
+ * Pass NULL if no scaling on P2 x is required.
+ *
+ * atimes is the user-supplied function which performs the
+ * operation of multiplying A by a given vector. Its description
+ * is given in iterative.h.
+ *
+ * psolve is the user-supplied function which solves a
+ * preconditioner system Pz = r, where P is P1 or P2. Its full
+ * description is given in iterativ.h. The psolve function will
+ * not be called if pretype is NONE; in that case, the user
+ * should pass NULL for psolve.
+ *
+ * res_norm is a pointer to the L2 norm of the scaled,
+ * preconditioned residual. On return with value SPGMR_SUCCESS or
+ * SPGMR_RES_REDUCED, (*res_norm) contains the value
+ * || s1 P1_inv (b - Ax) ||_2 for the computed solution x.
+ * For all other return values, (*res_norm) is undefined. The
+ * caller is responsible for allocating the memory (*res_norm)
+ * to be filled in by SpgmrSolve.
+ *
+ * nli is a pointer to the number of linear iterations done in
+ * the execution of SpgmrSolve. The caller is responsible for
+ * allocating the memory (*nli) to be filled in by SpgmrSolve.
+ *
+ * nps is a pointer to the number of calls made to psolve during
+ * the execution of SpgmrSolve. The caller is responsible for
+ * allocating the memory (*nps) to be filled in by SpgmrSolve.
+ *
+ * Note: Repeated calls can be made to SpgmrSolve with varying
+ * input arguments. If, however, the problem size N or the
+ * maximum Krylov dimension l_max changes, then a call to
+ * SpgmrMalloc must be made to obtain new memory for SpgmrSolve
+ * to use.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int SpgmrSolve(SpgmrMem mem, void *A_data, N_Vector x, N_Vector b,
+ int pretype, int gstype, realtype delta,
+ int max_restarts, void *P_data, N_Vector s1,
+ N_Vector s2, ATimesFn atimes, PSolveFn psolve,
+ realtype *res_norm, int *nli, int *nps);
+
+
+/* Return values for SpgmrSolve */
+
+#define SPGMR_SUCCESS 0 /* Converged */
+#define SPGMR_RES_REDUCED 1 /* Did not converge, but reduced
+ norm of residual */
+#define SPGMR_CONV_FAIL 2 /* Failed to converge */
+#define SPGMR_QRFACT_FAIL 3 /* QRfact found singular matrix */
+#define SPGMR_PSOLVE_FAIL_REC 4 /* psolve failed recoverably */
+#define SPGMR_ATIMES_FAIL_REC 5 /* atimes failed recoverably */
+#define SPGMR_PSET_FAIL_REC 6 /* pset faild recoverably */
+
+#define SPGMR_MEM_NULL -1 /* mem argument is NULL */
+#define SPGMR_ATIMES_FAIL_UNREC -2 /* atimes returned failure flag */
+#define SPGMR_PSOLVE_FAIL_UNREC -3 /* psolve failed unrecoverably */
+#define SPGMR_GS_FAIL -4 /* Gram-Schmidt routine faiuled */
+#define SPGMR_QRSOL_FAIL -5 /* QRsol found singular R */
+#define SPGMR_PSET_FAIL_UNREC -6 /* pset failed unrecoverably */
+
+/*
+ * -----------------------------------------------------------------
+ * Function : SpgmrFree
+ * -----------------------------------------------------------------
+ * SpgmrMalloc frees the memory allocated by SpgmrMalloc. It is
+ * illegal to use the pointer mem after a call to SpgmrFree.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void SpgmrFree(SpgmrMem mem);
+
+/*
+ * -----------------------------------------------------------------
+ * Macro: SPGMR_VTEMP
+ * -----------------------------------------------------------------
+ * This macro provides access to the work vector vtemp in the
+ * memory block of the SPGMR module. The argument mem is the
+ * memory pointer returned by SpgmrMalloc, of type SpgmrMem,
+ * and the macro value is of type N_Vector.
+ * On a return from SpgmrSolve with *nli = 0, this vector
+ * contains the scaled preconditioned initial residual,
+ * s1 * P1_inverse * (b - A x_0).
+ * -----------------------------------------------------------------
+ */
+
+#define SPGMR_VTEMP(mem) (mem->vtemp)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_sptfqmr.h b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_sptfqmr.h
new file mode 100644
index 0000000..b1f31cc
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_sptfqmr.h
@@ -0,0 +1,255 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/29 00:05:07 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Aaron Collier @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2005, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the header file for the implementation of the scaled
+ * preconditioned Transpose-Free Quasi-Minimal Residual (SPTFQMR)
+ * linear solver.
+ *
+ * The SPTFQMR algorithm solves a linear system of the form Ax = b.
+ * Preconditioning is allowed on the left (PREC_LEFT), right
+ * (PREC_RIGHT), or both (PREC_BOTH). Scaling is allowed on both
+ * sides. We denote the preconditioner and scaling matrices as
+ * follows:
+ * P1 = left preconditioner
+ * P2 = right preconditioner
+ * S1 = diagonal matrix of scale factors for P1-inverse b
+ * S2 = diagonal matrix of scale factors for P2 x
+ * The matrices A, P1, and P2 are not required explicitly; only
+ * routines that provide A, P1-inverse, and P2-inverse as operators
+ * are required.
+ *
+ * In this notation, SPTFQMR applies the underlying TFQMR method to
+ * the equivalent transformed system:
+ * Abar xbar = bbar, where
+ * Abar = S1 (P1-inverse) A (P2-inverse) (S2-inverse),
+ * bbar = S1 (P1-inverse) b, and
+ * xbar = S2 P2 x.
+ *
+ * The scaling matrices must be chosen so that vectors
+ * S1 P1-inverse b and S2 P2 x have dimensionless components. If
+ * preconditioning is done on the left only (P2 = I), by a matrix P,
+ * then S2 must be a scaling for x, while S1 is a scaling for
+ * P-inverse b, and so may also be taken as a scaling for x.
+ * Similarly, if preconditioning is done on the right only (P1 = I,
+ * P2 = P), then S1 must be a scaling for b, while S2 is a scaling
+ * for P x, and may also be taken as a scaling for b.
+ *
+ * The stopping test for the SPTFQMR iterations is on the L2-norm of
+ * the scaled preconditioned residual:
+ * || bbar - Abar xbar ||_2 < delta
+ * with an input test constant delta.
+ *
+ * The usage of this SPTFQMR solver involves supplying two routines
+ * and making three calls. The user-supplied routines are:
+ * atimes(A_data, x, y) to compute y = A x, given x,
+ * and
+ * psolve(P_data, x, y, lr) to solve P1 x = y or P2 x = y for x,
+ * given y.
+ * The three user calls are:
+ * mem = SptfqmrMalloc(lmax, vec_tmpl);
+ * to initialize memory
+ * flag = SptfqmrSolve(mem, A_data, x, b, pretype, delta, P_data,
+ * sx, sb, atimes, psolve, res_norm, nli, nps);
+ * to solve the system, and
+ * SptfqmrFree(mem);
+ * to free the memory allocated by SptfqmrMalloc().
+ * Complete details for specifying atimes() and psolve() and for the
+ * usage calls are given in the paragraphs below and in the header
+ * file sundials_iterative.h.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _SPTFQMR_H
+#define _SPTFQMR_H
+
+#include <sundials/sundials_iterative.h>
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+
+/*
+ * -----------------------------------------------------------------
+ * Types: struct SptfqmrMemRec and struct *SptfqmrMem
+ * -----------------------------------------------------------------
+ * A variable declaration of type struct *SptfqmrMem denotes a pointer
+ * to a data structure of type struct SptfqmrMemRec. The SptfqmrMemRec
+ * structure contains numerous fields that must be accessed by the
+ * SPTFQMR linear solver module.
+ *
+ * l_max maximum Krylov subspace dimension that SptfqmrSolve will
+ * be permitted to use
+ *
+ * r_star vector (type N_Vector) which holds the initial scaled,
+ * preconditioned linear system residual
+ *
+ * q/d/v/p/u/r vectors (type N_Vector) used for workspace by
+ * the SPTFQMR algorithm
+ *
+ * vtemp1/vtemp2/vtemp3 scratch vectors (type N_Vector) used as
+ * temporary storage
+ * -----------------------------------------------------------------
+ */
+
+typedef struct {
+
+ int l_max;
+
+ N_Vector r_star;
+ N_Vector q;
+ N_Vector d;
+ N_Vector v;
+ N_Vector p;
+ N_Vector *r;
+ N_Vector u;
+ N_Vector vtemp1;
+ N_Vector vtemp2;
+ N_Vector vtemp3;
+
+} SptfqmrMemRec, *SptfqmrMem;
+
+/*
+ * -----------------------------------------------------------------
+ * Function : SptfqmrMalloc
+ * -----------------------------------------------------------------
+ * SptfqmrMalloc allocates additional memory needed by the SPTFQMR
+ * linear solver module.
+ *
+ * l_max maximum Krylov subspace dimension that SptfqmrSolve will
+ * be permitted to use
+ *
+ * vec_tmpl implementation-specific template vector (type N_Vector)
+ * (created using either N_VNew_Serial or N_VNew_Parallel)
+ *
+ * If successful, SptfqmrMalloc returns a non-NULL memory pointer. If
+ * an error occurs, then a NULL pointer is returned.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT SptfqmrMem SptfqmrMalloc(int l_max, N_Vector vec_tmpl);
+
+/*
+ * -----------------------------------------------------------------
+ * Function : SptfqmrSolve
+ * -----------------------------------------------------------------
+ * SptfqmrSolve solves the linear system Ax = b by means of a scaled
+ * preconditioned Transpose-Free Quasi-Minimal Residual (SPTFQMR)
+ * method.
+ *
+ * mem pointer to an internal memory block allocated during a
+ * prior call to SptfqmrMalloc
+ *
+ * A_data pointer to a data structure containing information
+ * about the coefficient matrix A (passed to user-supplied
+ * function referenced by atimes (function pointer))
+ *
+ * x vector (type N_Vector) containing initial guess x_0 upon
+ * entry, but which upon return contains an approximate solution
+ * of the linear system Ax = b (solution only valid if return
+ * value is either SPTFQMR_SUCCESS or SPTFQMR_RES_REDUCED)
+ *
+ * b vector (type N_Vector) set to the right-hand side vector b
+ * of the linear system (undisturbed by function)
+ *
+ * pretype variable (type int) indicating the type of
+ * preconditioning to be used (see sundials_iterative.h)
+ *
+ * delta tolerance on the L2 norm of the scaled, preconditioned
+ * residual (if return value == SPTFQMR_SUCCESS, then
+ * ||sb*P1_inv*(b-Ax)||_L2 <= delta)
+ *
+ * P_data pointer to a data structure containing preconditioner
+ * information (passed to user-supplied function referenced
+ * by psolve (function pointer))
+ *
+ * sx vector (type N_Vector) containing positive scaling factors
+ * for x (pass sx == NULL if scaling NOT required)
+ *
+ * sb vector (type N_Vector) containing positive scaling factors
+ * for b (pass sb == NULL if scaling NOT required)
+ *
+ * atimes user-supplied routine responsible for computing the
+ * matrix-vector product Ax (see sundials_iterative.h)
+ *
+ * psolve user-supplied routine responsible for solving the
+ * preconditioned linear system Pz = r (ignored if
+ * pretype == PREC_NONE) (see sundials_iterative.h)
+ *
+ * res_norm pointer (type realtype*) to the L2 norm of the
+ * scaled, preconditioned residual (if return value
+ * is either SPTFQMR_SUCCESS or SPTFQMR_RES_REDUCED, then
+ * *res_norm = ||sb*P1_inv*(b-Ax)||_L2, where x is
+ * the computed approximate solution, sb is the diagonal
+ * scaling matrix for the right-hand side b, and P1_inv
+ * is the inverse of the left-preconditioner matrix)
+ *
+ * nli pointer (type int*) to the total number of linear
+ * iterations performed
+ *
+ * nps pointer (type int*) to the total number of calls made
+ * to the psolve routine
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT int SptfqmrSolve(SptfqmrMem mem, void *A_data, N_Vector x, N_Vector b,
+ int pretype, realtype delta, void *P_data, N_Vector sx,
+ N_Vector sb, ATimesFn atimes, PSolveFn psolve,
+ realtype *res_norm, int *nli, int *nps);
+
+/* Return values for SptfqmrSolve */
+
+#define SPTFQMR_SUCCESS 0 /* SPTFQMR algorithm converged */
+#define SPTFQMR_RES_REDUCED 1 /* SPTFQMR did NOT converge, but the
+ residual was reduced */
+#define SPTFQMR_CONV_FAIL 2 /* SPTFQMR algorithm failed to converge */
+#define SPTFQMR_PSOLVE_FAIL_REC 3 /* psolve failed recoverably */
+#define SPTFQMR_ATIMES_FAIL_REC 4 /* atimes failed recoverably */
+#define SPTFQMR_PSET_FAIL_REC 5 /* pset faild recoverably */
+
+#define SPTFQMR_MEM_NULL -1 /* mem argument is NULL */
+#define SPTFQMR_ATIMES_FAIL_UNREC -2 /* atimes returned failure flag */
+#define SPTFQMR_PSOLVE_FAIL_UNREC -3 /* psolve failed unrecoverably */
+#define SPTFQMR_PSET_FAIL_UNREC -4 /* pset failed unrecoverably */
+
+/*
+ * -----------------------------------------------------------------
+ * Function : SptfqmrFree
+ * -----------------------------------------------------------------
+ * SptfqmrFree frees the memory allocated by a call to SptfqmrMalloc.
+ * It is illegal to use the pointer mem after a call to SptfqmrFree.
+ * -----------------------------------------------------------------
+ */
+
+SUNDIALS_EXPORT void SptfqmrFree(SptfqmrMem mem);
+
+/*
+ * -----------------------------------------------------------------
+ * Macro : SPTFQMR_VTEMP
+ * -----------------------------------------------------------------
+ * This macro provides access to the work vector vtemp1 in the
+ * memory block of the SPTFQMR module. The argument mem is the
+ * memory pointer returned by SptfqmrMalloc, of type SptfqmrMem,
+ * and the macro value is of type N_Vector.
+ *
+ * Note: Only used by IDA (vtemp1 contains P_inverse F if
+ * nli_inc == 0).
+ * -----------------------------------------------------------------
+ */
+
+#define SPTFQMR_VTEMP(mem) (mem->vtemp1)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_types.h b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_types.h
new file mode 100644
index 0000000..75f6429
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/include/sundials/sundials_types.h
@@ -0,0 +1,119 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/29 00:05:07 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Scott Cohen, Alan Hindmarsh, Radu Serban, and
+ * Aaron Collier @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ *------------------------------------------------------------------
+ * This header file exports two types: realtype and booleantype,
+ * as well as the constants TRUE and FALSE.
+ *
+ * Users should include the header file sundials_types.h in every
+ * program file and use the exported name realtype instead of
+ * float, double or long double.
+ *
+ * The constants SUNDIALS_SINGLE_PRECISION, SUNDIALS_DOUBLE_PRECISION
+ * and SUNDIALS_LONG_DOUBLE_PRECISION indicate the underlying data
+ * type of realtype. It is set at the configuration stage.
+ *
+ * The legal types for realtype are float, double and long double.
+ *
+ * The macro RCONST gives the user a convenient way to define
+ * real-valued constants. To use the constant 1.0, for example,
+ * the user should write the following:
+ *
+ * #define ONE RCONST(1.0)
+ *
+ * If realtype is defined as a double, then RCONST(1.0) expands
+ * to 1.0. If realtype is defined as a float, then RCONST(1.0)
+ * expands to 1.0F. If realtype is defined as a long double,
+ * then RCONST(1.0) expands to 1.0L. There is never a need to
+ * explicitly cast 1.0 to (realtype).
+ *------------------------------------------------------------------
+ */
+
+#ifndef _SUNDIALSTYPES_H
+#define _SUNDIALSTYPES_H
+
+#include <sundials/sundials_config.h>
+#include <float.h>
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+
+/*
+ *------------------------------------------------------------------
+ * Type realtype
+ * Macro RCONST
+ * Constants BIG_REAL, SMALL_REAL, and UNIT_ROUNDOFF
+ *------------------------------------------------------------------
+ */
+
+#if defined(SUNDIALS_SINGLE_PRECISION)
+
+typedef float realtype;
+# define RCONST(x) x##F
+# define BIG_REAL FLT_MAX
+# define SMALL_REAL FLT_MIN
+# define UNIT_ROUNDOFF FLT_EPSILON
+
+#elif defined(SUNDIALS_DOUBLE_PRECISION)
+
+typedef double realtype;
+# define RCONST(x) x
+# define BIG_REAL DBL_MAX
+# define SMALL_REAL DBL_MIN
+# define UNIT_ROUNDOFF DBL_EPSILON
+
+#elif defined(SUNDIALS_EXTENDED_PRECISION)
+
+typedef long double realtype;
+# define RCONST(x) x##L
+# define BIG_REAL LDBL_MAX
+# define SMALL_REAL LDBL_MIN
+# define UNIT_ROUNDOFF LDBL_EPSILON
+
+#endif
+
+/*
+ *------------------------------------------------------------------
+ * Type : booleantype
+ *------------------------------------------------------------------
+ * Constants : FALSE and TRUE
+ *------------------------------------------------------------------
+ * ANSI C does not have a built-in boolean data type. Below is the
+ * definition for a new type called booleantype. The advantage of
+ * using the name booleantype (instead of int) is an increase in
+ * code readability. It also allows the programmer to make a
+ * distinction between int and boolean data. Variables of type
+ * booleantype are intended to have only the two values FALSE and
+ * TRUE which are defined below to be equal to 0 and 1,
+ * respectively.
+ *------------------------------------------------------------------
+ */
+
+#ifndef booleantype
+#define booleantype int
+#endif
+
+#ifndef FALSE
+#define FALSE 0
+#endif
+
+#ifndef TRUE
+#define TRUE 1
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/LICENSE.txt b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/LICENSE.txt
new file mode 100644
index 0000000..4391935
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/LICENSE.txt
@@ -0,0 +1,59 @@
+Copyright (c) 2006, The Regents of the University of California.
+Produced at the Lawrence Livermore National Laboratory.
+Written by Radu Serban
+UCRL-CODE-xxxxxx
+All rights reserved.
+
+This file is part of CPODES.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+1. Redistributions of source code must retain the above copyright
+notice, this list of conditions and the disclaimer below.
+
+2. Redistributions in binary form must reproduce the above copyright
+notice, this list of conditions and the disclaimer (as noted below)
+in the documentation and/or other materials provided with the
+distribution.
+
+3. Neither the name of the UC/LLNL nor the names of its contributors
+may be used to endorse or promote products derived from this software
+without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+REGENTS OF THE UNIVERSITY OF CALIFORNIA, THE U.S. DEPARTMENT OF ENERGY
+OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+Additional BSD Notice
+---------------------
+1. This notice is required to be provided under our contract with
+the U.S. Department of Energy (DOE). This work was produced at the
+University of California, Lawrence Livermore National Laboratory
+under Contract No. W-7405-ENG-48 with the DOE.
+
+2. Neither the United States Government nor the University of
+California nor any of their employees, makes any warranty, express
+or implied, or assumes any liability or responsibility for the
+accuracy, completeness, or usefulness of any information, apparatus,
+product, or process disclosed, or represents that its use would not
+infringe privately-owned rights.
+
+3. Also, reference herein to any specific commercial products,
+process, or services by trade name, trademark, manufacturer or
+otherwise does not necessarily constitute or imply its endorsement,
+recommendation, or favoring by the United States Government or the
+University of California. The views and opinions of authors expressed
+herein do not necessarily state or reflect those of the United States
+Government or the University of California, and shall not be used for
+advertising or product endorsement purposes.
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/README.txt b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/README.txt
new file mode 100644
index 0000000..73068bd
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/README.txt
@@ -0,0 +1,139 @@
+ CPODES
+ Release 0.0.1, December 2006
+ Radu Serban
+ Center for Applied Scientific Computing, LLNL
+
+CPODES is a solver for stiff and nonstiff ODE IVP with invariants,
+given in either explicit or implict form. In other words, CPODES solves:
+
+ Explicit-form or Implicit-form
+
+DE: y' = f(t,y) F(t,y,y') = 0
+CN: c(t,y) = 0 c(t,y) = 0
+IC: y(t0) = y0 y(t0) = y0; y'(t0) = yp0; F(t0,t0,yp0) = 0
+
+where t0, y0, yp0 in R^N, f: R x R^N -> R^N, F: R x R^N x R^N -> R^N
+and c: R x R^N -> R^M. The initial conditions must be lie on the
+invariant manifold (i.e. c(t0, y0) = 0).
+
+CPODES is part of a software family called SUNDIALS: SUite of Nonlinear
+and DIfferential/ALgebraic equation Solvers. This suite consists of
+CVODE, CVODES, KINSOL, IDA, and CPODES. The directory structure of the
+package reflects this family relationship.
+
+CPODES can be used both on serial and parallel (MPI) computers, the main
+difference being in the NVECTOR module of vector kernels. The desired
+version is obtained when compiling the example files by linking the
+appropriate library of NVECTOR kernels. In the parallel version,
+communication between processors is done with the MPI (Message Passage
+Interface) system.
+
+The integration method in CPODES is based on variable-order, variable-step
+implicit linear multistep methods, implemented with predictor-corrector
+polynomials represented through Nordsieck history arrays.
+CPODES provides both a nonstiff option (Adams method) and a stiff method
+(Backward Differentiation Formula). The method order for the Adams option
+varies between 1 and 12, while for the BDF method it varies between 1 and 5.
+The resulting nonlinear systems can be solved either with a functional
+iteration (appropriate for nonstiff problems and available ONLY for ODEs
+in explicit-form) or with a (modified) Newton method (appropriate for stiff
+problems). Therefore, it is recommended to use BDF+Newton for stiff problems
+and Adams+Functional for nonstiff problems (if possible).
+
+When the nonlinear systems are solved using the Newton method option,
+a linear solver module must be attached to CPODES. CPODEs provides both
+direct and preconditioned Krylov (iterative) linear solvers. The following
+direct solvers are currently available: dense and banded direct linear solver
+based on internal SUNDIALS implementations and Lapack-based linear solvers
+(both dense and banded). Three different iterative solvers are available:
+scaled preconditioned GMRES (SPGMR), scaled preconditioned BiCGStab (SPBCG),
+and scaled preconditioned TFQMR (SPTFQMR). When CPODES is used in conjunction
+with the serial NVECTOR module, any of the above linear solvers can be employed.
+However, when CPODES is used with the parallel NVECTOR module, only the Krylov
+linear solvers are available. For the serial version, CPODES also provides
+a banded preconditioner module called CPBANDPRE available for use with the
+Krylov solvers, while for the parallel version there is a preconditioner
+module called CPBBDPRE which provides a band-block-diagonal preconditioner.
+
+For any of the above linear solvers, the user has the option of providing
+Jacobian information, but all of them also provide a default difference
+quotient Jacobian approximation. For explicit-form ODEs, the Jacobian
+required by CPODES is J = df/dy, while for implicit-form ODEs, it is
+J = J = dF/dy' + gamma*dF/dy, where gamma is a scalar proportional to
+the current integration step size. The value gamma is set by CPODES and
+it should not be modified by a function evaluationg the Jacobian. Note also
+that the iterative linear solvers are matrix-free, in the sense that they
+only require Jacobian-vector products.
+
+CPODES ensures that the solution lies on the invariant manifold by performing
+a so-called coordinate projection. The user has the option of providing their
+own projection function, but CPODES provides an internal projection algorithm.
+The CPODES internal projection algorithm applies a pseudo-inverse (if the user
+indicates that the constraints are linear) or a performs Moore-Penrose iterations
+(for nonlinear constraints). In either case, the pseudo inverses are never
+computed explicitly, but rather as solutions of linear systems in so-called
+optimization matrix form. For the solution of these special-form linear systems,
+CPODES offers several different methods, based either on a Schur-complement
+approach or on various decompositions of the transposed of the constraint
+Jacobian: LU, QR, or QR with column pivoting, the latter being applicable to
+the case in which some of the constraints are known (or suspected) to be
+redundant. Currently, only the dense and Lapack dense linear solvers in
+SUNDIALS provide support for the linear algebra required in the projection
+algorithm.
+
+Therefore, in an attempt to advance the solution to a new point in time (i.e.
+taking a new integration step), CPODES performs the following operations:
+ - predict solution
+ - solve nonlinear system and correct solution
+ - project solution
+ - test error
+ - select order and step size for next step
+and includes several recovery attempts in case there are convergence failures
+(or difficulties) in the nonlinear solver or in the projection step, or if the
+solution fails to satisfy the error test.
+
+Other features offered by CPODES include:
+ - dense output: an approximate solution (or derivatives of the solution up to
+ the current method order) can be obtained for any time within the last step.
+ - integration of pure quadrature equations: CPODES can treat more efficiently
+ the integration of equations of the type yq' = q(t,y) if these are specified
+ as such. This can be used to evaluate integrals I = \int_t0^t q(t,y) dt
+ as I = yq(t), where yq' = q(t,y), yq(t0) = 0.
+ - rootfinding: additional user-specified functions can be monitored and the
+ solver will return whenever a root of such a function is encountered.
+ - stability limit detection: since BDF methods of order higher than 2 are not
+ absolutely stable. CPODES provides a mechanism which attempts to detect,
+ in a direct manner, the presence of a stability region boundary that is
+ limiting the step sizes in the presence of a weakly damped oscillation
+ and reduce the BDF order.
+
+
+A. Documentation
+----------------
+
+/sundials/doc/cpodes contains PDF files for the CPODES User Guide (cps_guide.pdf)
+and the CPODES Examples (cps_examples.pdf) documents.
+
+B. Installation
+---------------
+
+For basic installation instructions see the file /sundials/INSTALL_NOTES.
+For complete installation instructions see the "CPODES Installation Procedure"
+chapter in the CPODES User Guide.
+
+C. References
+-------------
+
+[1] R. Serban, "User Documentation for CPODES"
+ LLLNL technical report UCRL-SM-xxxxxx, Month Year.
+
+
+D. Releases
+-----------
+
+v. 0.0.1 - Dec. 2006
+
+
+E. Revision History
+-------------------
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/TODO.txt b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/TODO.txt
new file mode 100644
index 0000000..21506c8
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/TODO.txt
@@ -0,0 +1,36 @@
+Not implemented yet:
+- various functions needed for projection in WRMS norm
+ (in cpodes_dense and cpodes_lapack)
+- function to compute consistent y' for implicit-form ODEs
+ (in cpodes_ic)
+- add some more optional I/O functions for IC calculation
+
+More tests needed:
+- test quadrature integration
+- test BAND linear solver for CP_IMPL case
+- test BANDPRE and BBDPRE preconditioner modules
+- recheck SPGMR, SPBCG, and SPTFQMR (particularly the CP_IMPL case) and test.
+
+Under consideration:
+- do I want to add a function to change from its default value of 1.0 the
+ variable dqincfac (cpSpilsDQjtvImpl in cpodes_spils), like it's done in IDA?
+- automatic stiff-nonstiff switch (through nonlinear solver switch)?
+ We could add a Lipschitz constant estimator for this. But what about the
+ influence of problem size (when using DQ Jacobians) or the relative cost
+ of fct/Jac. evals (when the user provides the Jacobian)?
+
+NOTES:
+
+- At this time, unless we allocate one additional vector, we cannot return the
+ proper estimated local error vector if the error estimate was actually projected.
+ (i.e. we still return acor/tq[2] when we really should return P*acor/tq[2], but we
+ do not have P*acor anymore -- it was stored into a temporary vector).
+
+- To save an additional function evaluation at the projected state (i.e. finding
+ yP' such that yP' = f(t,yP) or F(t, yP', yP) = 0, the latter also requiring
+ a nonlinear solve with a Jacobian which is not readily available), we use a
+ special trick in applying the projection correction (i.e. we do something slightly
+ different than it's done for the application of the NLS correction) => potential
+ problems when using Adams.
+
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes.c b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes.c
new file mode 100644
index 0000000..780a19c
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes.c
@@ -0,0 +1,4361 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.9 $
+ * $Date: 2007/10/26 21:51:29 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for the main CPODES integrator.
+ * It is independent of the CPODES linear solver in use.
+ * -----------------------------------------------------------------
+ *
+ * CONTENTS
+ *
+ * Exported functions:
+ * CPodeCreate - create CPODES solver object
+ * CPodeInit - initialize solver
+ * CPodeReInit - re-initialize solver
+ * CPodeProjInit - initialize internal projection algorithm
+ * CPodeProjDefine - initialize user-provided projection
+ * CPode - main solver function
+ * CPodeGetDky - dense output function
+ * CPodeFree - memory deallocation
+ *
+ * Private functions
+ * - Memory allocation/deallocation and initialization functions
+ * - Initial step size evaluation functions
+ * - Main step function
+ * - LMM-related functions
+ * - Nonlinear and error test failure handlers
+ * - Succesful step completion functions
+ * - BDF stability limit detection functions
+ * - Projection functions
+ * - Internal error weight evaluation functions
+ * - Error reporting functions
+ */
+
+/*
+ * =================================================================
+ * IMPORTED HEADER FILES
+ * =================================================================
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
+
+#include "cpodes_private.h"
+#include <sundials/sundials_math.h>
+
+/*
+ * =================================================================
+ * FUNCTION SPECIFIC CONSTANTS
+ * =================================================================
+ */
+
+/*
+ * Algorithmic constants
+ * ---------------------
+ */
+
+/* constants used in estimating an initial step size */
+#define HLB_FACTOR RCONST(100.0)
+#define HUB_FACTOR RCONST(0.1)
+#define H_BIAS HALF
+#define H_MAXITERS 4
+
+/* constants used in taking a step */
+#define THRESH RCONST(1.5)
+#define ETAMX1 RCONST(10000.0)
+#define ETAMX2 RCONST(10.0)
+#define ETAMX3 RCONST(10.0)
+#define ETAMXF RCONST(0.2)
+#define ETAMIN RCONST(0.1)
+#define ADDON RCONST(0.000001)
+#define BIAS1 RCONST(6.0)
+#define BIAS2 RCONST(6.0)
+#define BIAS3 RCONST(10.0)
+
+/*
+ * =================================================================
+ * PROTOTYPES FOR PRIVATE FUNCTIONS
+ * =================================================================
+ */
+
+/* Memory allocation/deallocation and initialization functions */
+static booleantype cpCheckNvector(N_Vector tmpl);
+static booleantype cpAllocVectors(CPodeMem cp_mem, N_Vector tmpl, int tol);
+static void cpFreeVectors(CPodeMem cp_mem);
+static booleantype cpProjAlloc(CPodeMem cp_mem, N_Vector c_tmpl, N_Vector s_tmpl);
+static void cpProjFree(CPodeMem cp_mem);
+static booleantype cpQuadAlloc(CPodeMem cp_mem, N_Vector q_tmpl);
+static void cpQuadFree(CPodeMem cp_mem);
+
+static int cpInitialSetup(CPodeMem cp_mem);
+
+/* Initial step size evaluation functions */
+static int cpHin(CPodeMem cp_mem, realtype tout);
+static realtype cpUpperBoundH0(CPodeMem cp_mem, realtype tdist);
+static int cpHinExpl(CPodeMem cp_mem, realtype hlb, realtype hub, int sign, realtype *h0);
+static int cpYppNorm(CPodeMem cp_mem, realtype hg, realtype *yppnorm);
+static int cpHinImpl(CPodeMem cp_mem, realtype tdist, realtype *h0);
+
+/* Main step function */
+static int cpStep(CPodeMem cp_mem);
+
+/* Functions acting on Nordsieck history array */
+static void cpPredict(CPodeMem cp_mem);
+static void cpCorrect(CPodeMem cp_mem);
+
+/* Quadrature correction function */
+static int cpQuadNls(CPodeMem cp_mem, realtype saved_t, int *ncfPtr);
+
+/* LMM-related functions */
+static void cpAdjustParams(CPodeMem cp_mem);
+static void cpAdjustOrder(CPodeMem cp_mem, int deltaq);
+static void cpAdjustAdams(CPodeMem cp_mem, int deltaq);
+static void cpAdjustBDF(CPodeMem cp_mem, int deltaq);
+static void cpIncreaseBDF(CPodeMem cp_mem);
+static void cpDecreaseBDF(CPodeMem cp_mem);
+static void cpSet(CPodeMem cp_mem);
+static void cpSetAdams(CPodeMem cp_mem);
+static realtype cpAdamsStart(CPodeMem cp_mem, realtype m[]);
+static void cpAdamsFinish(CPodeMem cp_mem, realtype m[], realtype M[], realtype hsum);
+static realtype cpAltSum(int iend, realtype a[], int k);
+static void cpSetBDF(CPodeMem cp_mem);
+static void cpSetTqBDF(CPodeMem cp_mem, realtype hsum, realtype alpha0,
+ realtype alpha0_hat, realtype xi_inv, realtype xistar_inv);
+
+/* Error test function */
+static int cpDoErrorTest(CPodeMem cp_mem, realtype saved_t, realtype acor_norm,
+ int *nefPtr, realtype *dsmPtr);
+
+/* Succesful step completion functions */
+static void cpCompleteStep(CPodeMem cp_mem);
+static void cpPrepareNextStep(CPodeMem cp_mem, realtype dsm);
+static void cpSetEta(CPodeMem cp_mem);
+static realtype cpComputeEtaqm1(CPodeMem cp_mem);
+static realtype cpComputeEtaqp1(CPodeMem cp_mem);
+static void cpChooseEta(CPodeMem cp_mem);
+static int cpHandleFailure(CPodeMem cp_mem,int flag);
+
+/* BDF stability limit detection functions */
+static void cpBDFStab(CPodeMem cp_mem);
+static int cpSLdet(CPodeMem cp_mem);
+
+/* Internal error weight evaluation functions */
+static int cpEwtSetSS(CPodeMem cp_mem, N_Vector ycur, N_Vector weight);
+static int cpEwtSetSV(CPodeMem cp_mem, N_Vector ycur, N_Vector weight);
+static int cpQuadEwtSet(CPodeMem cp_mem, N_Vector qcur, N_Vector weightQ);
+static int cpQuadEwtSetSS(CPodeMem cp_mem, N_Vector qcur, N_Vector weightQ);
+static int cpQuadEwtSetSV(CPodeMem cp_mem, N_Vector qcur, N_Vector weightQ);
+
+/* Function for combined norms */
+static realtype cpQuadUpdateNorm(CPodeMem cp_mem, realtype old_nrm,
+ N_Vector xQ, N_Vector wQ);
+static realtype cpQuadUpdateDsm(CPodeMem cp_mem, realtype old_dsm,
+ realtype dsmQ);
+
+/* Error reporting functions */
+static void cpErrHandler(int error_code, const char *module,
+ const char *function, char *msg, void *data);
+
+/*
+ * =================================================================
+ * EXPORTED FUNCTIONS
+ * =================================================================
+ */
+
+/*
+ * CPodeCreate
+ *
+ * CPodeCreate creates an internal memory block for a problem to
+ * be solved by CPODES.
+ * If successful, CPodeCreate returns a pointer to the problem memory.
+ * This pointer should be passed to CPodeInit.
+ * If an initialization error occurs, CPodeCreate prints an error
+ * message to standard err and returns NULL.
+ */
+
+void *CPodeCreate(int ode_type, int lmm_type, int nls_type)
+{
+ int maxord;
+ CPodeMem cp_mem;
+
+ /* Test inputs */
+ if ((ode_type != CP_EXPL) && (ode_type != CP_IMPL)) {
+ cpProcessError(NULL, 0, "CPODES", "CPodeCreate", MSGCP_BAD_ODE);
+ return(NULL);
+ }
+ if ((lmm_type != CP_ADAMS) && (lmm_type != CP_BDF)) {
+ cpProcessError(NULL, 0, "CPODES", "CPodeCreate", MSGCP_BAD_LMM);
+ return(NULL);
+ }
+ if ((nls_type != CP_FUNCTIONAL) && (nls_type != CP_NEWTON)) {
+ cpProcessError(NULL, 0, "CPODES", "CPodeCreate", MSGCP_BAD_NLS);
+ return(NULL);
+ }
+ if ((ode_type == CP_IMPL) && (nls_type == CP_FUNCTIONAL)) {
+ cpProcessError(NULL, 0, "CPODES", "CPodeCreate", MSGCP_BAD_ODE_NLS);
+ return(NULL);
+ }
+
+ /* Allocate space for solver object */
+ cp_mem = NULL;
+ cp_mem = (CPodeMem) malloc(sizeof(struct CPodeMemRec));
+ if (cp_mem == NULL) {
+ cpProcessError(NULL, 0, "CPODES", "CPodeCreate", MSGCP_CPMEM_FAIL);
+ return(NULL);
+ }
+
+ maxord = (lmm_type == CP_ADAMS) ? ADAMS_Q_MAX : BDF_Q_MAX;
+
+ /* Copy input parameters into cp_mem */
+ cp_mem->cp_ode_type = ode_type;
+ cp_mem->cp_lmm_type = lmm_type;
+ cp_mem->cp_nls_type = nls_type;
+
+ /* Set uround */
+ cp_mem->cp_uround = UNIT_ROUNDOFF;
+
+ /* Initialize required values */
+ cp_mem->cp_fi = NULL;
+ cp_mem->cp_fe = NULL;
+ cp_mem->cp_f_data = NULL;
+
+ /* Set default values for integrator optional inputs */
+ cp_mem->cp_efun = cpEwtSet;
+ cp_mem->cp_e_data = (void *) cp_mem;
+ cp_mem->cp_ehfun = cpErrHandler;
+ cp_mem->cp_eh_data = (void *) cp_mem;
+ cp_mem->cp_errfp = stderr;
+ cp_mem->cp_qmax = maxord;
+ cp_mem->cp_mxstep = MXSTEP_DEFAULT;
+ cp_mem->cp_mxhnil = MXHNIL_DEFAULT;
+ cp_mem->cp_sldeton = FALSE;
+ cp_mem->cp_hin = ZERO;
+ cp_mem->cp_hmin = HMIN_DEFAULT;
+ cp_mem->cp_hmax_inv = HMAX_INV_DEFAULT;
+ cp_mem->cp_tstopset = FALSE;
+ cp_mem->cp_maxcor = NLS_MAXCOR;
+ cp_mem->cp_maxnef = MXNEF;
+ cp_mem->cp_maxncf = MXNCF;
+ cp_mem->cp_nlscoef = NLS_TEST_COEF;
+
+ /* Set the linear solver addresses to NULL. */
+ cp_mem->cp_linit = NULL;
+ cp_mem->cp_lsetup = NULL;
+ cp_mem->cp_lsolve = NULL;
+ cp_mem->cp_lfree = NULL;
+ cp_mem->cp_lmem = NULL;
+ cp_mem->cp_lsetup_exists = FALSE;
+
+ /* Initialize projection variables */
+ cp_mem->cp_proj_enabled = FALSE;
+ cp_mem->cp_proj_type = CP_PROJ_INTERNAL;
+ cp_mem->cp_cnstr_type = CP_CNSTR_NONLIN;
+ cp_mem->cp_cfun = NULL;
+ cp_mem->cp_c_data = NULL;
+ cp_mem->cp_pfun = NULL;
+ cp_mem->cp_p_data = NULL;
+ cp_mem->cp_prjcoef = PRJ_TEST_COEF;
+ cp_mem->cp_maxcorP = PRJ_MAXCOR;
+ cp_mem->cp_project_err = TRUE;
+ cp_mem->cp_test_cnstr = FALSE;
+ cp_mem->cp_proj_freq = 1;
+ cp_mem->cp_lsetupP_freq = PRJ_MSBLS;
+ cp_mem->cp_maxnpf = MXNPF;
+
+ /* Initialize quadrature variables */
+ cp_mem->cp_quadr = FALSE;
+ cp_mem->cp_qfun = NULL;
+ cp_mem->cp_q_data = NULL;
+ cp_mem->cp_errconQ = FALSE;
+
+ /* Set the linear solver addresses to NULL. */
+ cp_mem->cp_linitP = NULL;
+ cp_mem->cp_lsetupP = NULL;
+ cp_mem->cp_lsolveP = NULL;
+ cp_mem->cp_lfreeP = NULL;
+ cp_mem->cp_lmemP = NULL;
+ cp_mem->cp_lsetupP_exists = FALSE;
+
+ /* Initialize root finding variables */
+ cp_mem->cp_doRootfinding = FALSE;
+ cp_mem->cp_nrtfn = 0;
+ cp_mem->cp_glo = NULL;
+ cp_mem->cp_ghi = NULL;
+ cp_mem->cp_grout = NULL;
+ cp_mem->cp_gactive = NULL;
+ cp_mem->cp_iroots = NULL;
+ cp_mem->cp_rootdir = NULL;
+ cp_mem->cp_gfun = NULL;
+ cp_mem->cp_g_data = NULL;
+
+ /* Set the saved value qmax_alloc */
+ cp_mem->cp_qmax_alloc = maxord;
+ cp_mem->cp_qmax_allocQ = maxord;
+
+ /* Initialize lrw and liw */
+ cp_mem->cp_lrw = 58 + 2*L_MAX + NUM_TESTS;
+ cp_mem->cp_liw = 40;
+
+ /* No mallocs have been done yet */
+ cp_mem->cp_MallocDone = FALSE;
+ cp_mem->cp_VabstolMallocDone = FALSE;
+ cp_mem->cp_projMallocDone = FALSE;
+ cp_mem->cp_quadMallocDone = FALSE;
+ cp_mem->cp_VabstolQMallocDone = FALSE;
+ cp_mem->cp_rootMallocDone = FALSE;
+
+ /* Return pointer to CPODES memory block */
+
+ return((void *)cp_mem);
+}
+
+/*-----------------------------------------------------------------*/
+
+#define ode_type (cp_mem->cp_ode_type)
+#define lmm_type (cp_mem->cp_lmm_type)
+#define nls_type (cp_mem->cp_nls_type)
+#define lrw (cp_mem->cp_lrw)
+#define liw (cp_mem->cp_liw)
+
+/*-----------------------------------------------------------------*/
+
+/*
+ * CPodeInit
+ *
+ * CPodeInit allocates and initializes memory for a problem. All
+ * problem inputs are checked for errors. If any error occurs during
+ * initialization, it is reported to the file whose file pointer is
+ * errfp and an error flag is returned. Otherwise, it returns CP_SUCCESS
+ */
+
+int CPodeInit(void *cpode_mem,
+ void *fun, void *f_data,
+ realtype t0, N_Vector y0, N_Vector yp0,
+ int tol_type, realtype reltol, void *abstol)
+{
+ CPodeMem cp_mem;
+ booleantype nvectorOK, allocOK, neg_abstol;
+ long int lrw1, liw1;
+ int i,k;
+
+ /* Check cpode_mem */
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeInit", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Check for legal input parameters */
+ if (fun == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeInit", MSGCP_NULL_F);
+ return(CP_ILL_INPUT);
+ }
+ if (y0==NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeInit", MSGCP_NULL_Y0);
+ return(CP_ILL_INPUT);
+ }
+ if ( (ode_type==CP_IMPL) && (yp0==NULL) ) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeInit", MSGCP_NULL_YP0);
+ return(CP_ILL_INPUT);
+ }
+ if ((tol_type != CP_SS) && (tol_type != CP_SV) && (tol_type != CP_WF)) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeInit", MSGCP_BAD_ITOL);
+ return(CP_ILL_INPUT);
+ }
+ /* Test if all required vector operations are implemented */
+ nvectorOK = cpCheckNvector(y0);
+ if(!nvectorOK) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeInit", MSGCP_BAD_NVECTOR);
+ return(CP_ILL_INPUT);
+ }
+
+ /* Test tolerances */
+ if (tol_type != CP_WF) {
+
+ if (abstol == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeInit", MSGCP_NULL_ABSTOL);
+ return(CP_ILL_INPUT);
+ }
+
+ if (reltol < ZERO) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeInit", MSGCP_BAD_RELTOL);
+ return(CP_ILL_INPUT);
+ }
+
+ if (tol_type == CP_SS)
+ neg_abstol = (*((realtype *)abstol) < ZERO);
+ else
+ neg_abstol = (N_VMin((N_Vector)abstol) < ZERO);
+
+ if (neg_abstol) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeInit", MSGCP_BAD_ABSTOL);
+ return(CP_ILL_INPUT);
+ }
+
+ }
+
+ /* Set space requirements for one N_Vector */
+ if (y0->ops->nvspace != NULL) {
+ N_VSpace(y0, &lrw1, &liw1);
+ } else {
+ lrw1 = 0;
+ liw1 = 0;
+ }
+ cp_mem->cp_lrw1 = lrw1;
+ cp_mem->cp_liw1 = liw1;
+
+ /* Allocate the vectors (using y0 as a template) */
+ allocOK = cpAllocVectors(cp_mem, y0, tol_type);
+ if (!allocOK) {
+ cpProcessError(cp_mem, CP_MEM_FAIL, "CPODES", "CPodeInit", MSGCP_MEM_FAIL);
+ return(CP_MEM_FAIL);
+ }
+
+ /*
+ * All error checking is complete at this point
+ */
+
+ /* Copy tolerances into memory */
+ cp_mem->cp_tol_type = tol_type;
+ cp_mem->cp_reltol = reltol;
+
+ if (tol_type == CP_SS) {
+ cp_mem->cp_Sabstol = *((realtype *)abstol);
+ } else if (tol_type == CP_SV) {
+ N_VScale(ONE, (N_Vector)abstol, cp_mem->cp_Vabstol);
+ }
+
+ /* Copy the input parameters into CPODES state */
+ if (ode_type == CP_EXPL) cp_mem->cp_fe = (CPRhsFn) fun;
+ else cp_mem->cp_fi = (CPResFn) fun;
+ cp_mem->cp_f_data = f_data;
+ cp_mem->cp_tn = t0;
+
+ /* Set step parameters */
+ cp_mem->cp_q = 1;
+ cp_mem->cp_L = 2;
+ cp_mem->cp_qwait = cp_mem->cp_L;
+ cp_mem->cp_etamax = ETAMX1;
+ cp_mem->cp_qu = 0;
+ cp_mem->cp_hu = ZERO;
+ cp_mem->cp_tolsf = ONE;
+
+ /* Initialize the history array zn */
+ N_VScale(ONE, y0, cp_mem->cp_zn[0]);
+ if(ode_type==CP_IMPL) N_VScale(ONE, yp0, cp_mem->cp_zn[1]);
+
+ /* Initialize all the counters */
+ cp_mem->cp_nst = 0;
+ cp_mem->cp_nfe = 0;
+ cp_mem->cp_ncfn = 0;
+ cp_mem->cp_netf = 0;
+ cp_mem->cp_nni = 0;
+ cp_mem->cp_nsetups = 0;
+ cp_mem->cp_nhnil = 0;
+ cp_mem->cp_nstlset = 0;
+ cp_mem->cp_nscon = 0;
+ cp_mem->cp_nge = 0;
+
+ cp_mem->cp_irfnd = 0;
+
+ cp_mem->cp_nproj = 0;
+ cp_mem->cp_nprf = 0;
+ cp_mem->cp_nce = 0;
+ cp_mem->cp_nstlprj = 0;
+ cp_mem->cp_nsetupsP = 0;
+ cp_mem->cp_first_proj = TRUE;
+
+ /* Initialize other integrator optional outputs */
+ cp_mem->cp_h0u = ZERO;
+ cp_mem->cp_next_h = ZERO;
+ cp_mem->cp_next_q = 0;
+
+ /*
+ * Initialize Stablilty Limit Detection data.
+ * NOTE: We do this even if stab lim det was not turned on yet.
+ * This way, the user can turn it on at any later time.
+ */
+ cp_mem->cp_nor = 0;
+ for (i = 1; i <= 5; i++)
+ for (k = 1; k <= 3; k++)
+ cp_mem->cp_ssdat[i-1][k-1] = ZERO;
+
+ /* Problem has been successfully initialized */
+ cp_mem->cp_MallocDone = TRUE;
+
+ return(CP_SUCCESS);
+}
+
+/*-----------------------------------------------------------------*/
+
+#define lrw1 (cp_mem->cp_lrw1)
+#define liw1 (cp_mem->cp_liw1)
+
+/*-----------------------------------------------------------------*/
+
+/*
+ * CPodeReInit
+ *
+ * CPodeReInit re-initializes CPODE's memory for a problem, assuming
+ * it has already been allocated in a prior CPodeInit call.
+ * All problem specification inputs are checked for errors.
+ * If any error occurs during initialization, it is reported to the
+ * file whose file pointer is errfp.
+ * The return value is CP_SUCCESS = 0 if no errors occurred, or
+ * a negative value otherwise.
+ */
+
+int CPodeReInit(void *cpode_mem,
+ void *fun, void *f_data,
+ realtype t0, N_Vector y0, N_Vector yp0,
+ int tol_type, realtype reltol, void *abstol)
+{
+ CPodeMem cp_mem;
+ booleantype neg_abstol;
+ int i,k;
+
+ /* Check cpode_mem */
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeReInit", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Check if cpode_mem was allocated */
+ if (cp_mem->cp_MallocDone == FALSE) {
+ cpProcessError(cp_mem, CP_NO_MALLOC, "CPODES", "CPodeReInit", MSGCP_NO_MALLOC);
+ return(CP_NO_MALLOC);
+ }
+
+ /* Check for legal input parameters */
+ if (fun == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeReInit", MSGCP_NULL_F);
+ return(CP_ILL_INPUT);
+ }
+ if (y0 == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeReInit", MSGCP_NULL_Y0);
+ return(CP_ILL_INPUT);
+ }
+ if ( (ode_type==CP_IMPL) && (yp0==NULL) ) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeReInit", MSGCP_NULL_YP0);
+ return(CP_ILL_INPUT);
+ }
+ if ((tol_type != CP_SS) && (tol_type != CP_SV) && (tol_type != CP_WF)) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeReInit", MSGCP_BAD_ITOL);
+ return(CP_ILL_INPUT);
+ }
+
+ /* Test tolerances */
+ if (tol_type != CP_WF) {
+
+ if (abstol == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeReInit", MSGCP_NULL_ABSTOL);
+ return(CP_ILL_INPUT);
+ }
+
+ if (reltol < ZERO) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeReInit", MSGCP_BAD_RELTOL);
+ return(CP_ILL_INPUT);
+ }
+
+ if (tol_type == CP_SS) {
+ neg_abstol = (*((realtype *)abstol) < ZERO);
+ } else {
+ neg_abstol = (N_VMin((N_Vector)abstol) < ZERO);
+ }
+
+ if (neg_abstol) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeReInit", MSGCP_BAD_ABSTOL);
+ return(CP_ILL_INPUT);
+ }
+
+ }
+
+ /*
+ * All error checking is complete at this point
+ */
+
+ /* Copy tolerances into memory */
+ cp_mem->cp_tol_type = tol_type;
+ cp_mem->cp_reltol = reltol;
+
+ if (tol_type == CP_SS) {
+ cp_mem->cp_Sabstol = *((realtype *)abstol);
+ } else if (tol_type == CP_SV) {
+ if ( !(cp_mem->cp_VabstolMallocDone) ) {
+ cp_mem->cp_Vabstol = N_VClone(y0);
+ lrw += lrw1;
+ liw += liw1;
+ cp_mem->cp_VabstolMallocDone = TRUE;
+ }
+ N_VScale(ONE, (N_Vector)abstol, cp_mem->cp_Vabstol);
+ }
+
+ /* Copy the input parameters into CPODES state */
+ if (ode_type == CP_EXPL) cp_mem->cp_fe = (CPRhsFn) fun;
+ else cp_mem->cp_fi = (CPResFn) fun;
+ cp_mem->cp_f_data = f_data;
+ cp_mem->cp_tn = t0;
+
+ /* Set step parameters */
+ cp_mem->cp_q = 1;
+ cp_mem->cp_L = 2;
+ cp_mem->cp_qwait = cp_mem->cp_L;
+ cp_mem->cp_etamax = ETAMX1;
+ cp_mem->cp_qu = 0;
+ cp_mem->cp_hu = ZERO;
+ cp_mem->cp_tolsf = ONE;
+
+ /* Initialize the history array zn */
+ N_VScale(ONE, y0, cp_mem->cp_zn[0]);
+ if(ode_type==CP_IMPL) N_VScale(ONE, yp0, cp_mem->cp_zn[1]);
+
+ /* Initialize all the counters */
+ cp_mem->cp_nst = 0;
+ cp_mem->cp_nfe = 0;
+ cp_mem->cp_ncfn = 0;
+ cp_mem->cp_netf = 0;
+ cp_mem->cp_nni = 0;
+ cp_mem->cp_nsetups = 0;
+ cp_mem->cp_nhnil = 0;
+ cp_mem->cp_nstlset = 0;
+ cp_mem->cp_nscon = 0;
+ cp_mem->cp_nge = 0;
+
+ cp_mem->cp_irfnd = 0;
+
+ cp_mem->cp_nproj = 0;
+ cp_mem->cp_nprf = 0;
+ cp_mem->cp_nce = 0;
+ cp_mem->cp_nstlprj = 0;
+ cp_mem->cp_nsetupsP = 0;
+ cp_mem->cp_first_proj = TRUE;
+
+ /* Initialize other integrator optional outputs */
+ cp_mem->cp_h0u = ZERO;
+ cp_mem->cp_next_h = ZERO;
+ cp_mem->cp_next_q = 0;
+
+ /* Initialize Stablilty Limit Detection data */
+ cp_mem->cp_nor = 0;
+ for (i = 1; i <= 5; i++)
+ for (k = 1; k <= 3; k++)
+ cp_mem->cp_ssdat[i-1][k-1] = ZERO;
+
+ /* Problem has been successfully re-initialized */
+ return(CP_SUCCESS);
+}
+
+/*-----------------------------------------------------------------*/
+
+/*
+ * CPodeProjInit
+ *
+ */
+
+int CPodeProjInit(void *cpode_mem, int proj_norm,
+ int cnstr_type, CPCnstrFn cfun, void *c_data,
+ N_Vector ctol)
+{
+ CPodeMem cp_mem;
+ booleantype allocOK;
+
+ /* Check cpode_mem */
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeProjInit", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Check if cpode_mem was allocated */
+ if (cp_mem->cp_MallocDone == FALSE) {
+ cpProcessError(cp_mem, CP_NO_MALLOC, "CPODES", "CPodeProjInit", MSGCP_NO_MALLOC);
+ return(CP_NO_MALLOC);
+ }
+
+ /* Check for legal input parameters */
+ if ((proj_norm != CP_PROJ_L2NORM) && (proj_norm != CP_PROJ_ERRNORM) ) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeProjInit", MSGCP_BAD_NORM);
+ return(CP_ILL_INPUT);
+ }
+ if ((cnstr_type != CP_CNSTR_LIN) && (cnstr_type != CP_CNSTR_NONLIN) ) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeProjInit", MSGCP_BAD_CNSTR);
+ return(CP_ILL_INPUT);
+ }
+ if (cfun == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeProjInit", MSGCP_NULL_C);
+ return(CP_ILL_INPUT);
+ }
+ if (ctol == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeProjInit", MSGCP_CTOL_NULL);
+ return(CP_ILL_INPUT);
+ }
+
+ /* Allocate memory for vectors used in the projection algorithm,
+ using ctol and tempv as templates */
+ if (!cp_mem->cp_projMallocDone) {
+ allocOK = cpProjAlloc(cp_mem, ctol, cp_mem->cp_tempv);
+ if (!allocOK) {
+ cpProcessError(cp_mem, CP_MEM_FAIL, "CPODES", "CPodeProjInit", MSGCP_MEM_FAIL);
+ return(CP_MEM_FAIL);
+ }
+ cp_mem->cp_projMallocDone = TRUE;
+ }
+
+ /* Set variable values in CPODES memory block */
+ cp_mem->cp_proj_norm = proj_norm;
+ cp_mem->cp_cnstr_type = cnstr_type;
+ cp_mem->cp_cfun = cfun;
+ cp_mem->cp_c_data = c_data;
+
+ /* Copy 1/ctol into memory */
+ N_VScale(ONE, ctol, cp_mem->cp_ctol);
+ N_VInv(cp_mem->cp_ctol, cp_mem->cp_ctol);
+
+ /* internal projection is now enabled */
+ cp_mem->cp_proj_type = CP_PROJ_INTERNAL;
+ cp_mem->cp_proj_enabled = TRUE;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeProjDefine
+ */
+
+int CPodeProjDefine(void *cpode_mem, CPProjFn pfun, void *p_data)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeProjDefine", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ cp_mem->cp_pfun = pfun;
+ cp_mem->cp_p_data = p_data;
+
+ /* user-defined projection is now enabled */
+ cp_mem->cp_proj_type = CP_PROJ_USER;
+ cp_mem->cp_proj_enabled = TRUE;
+
+ return(CP_SUCCESS);
+}
+
+/*-----------------------------------------------------------------*/
+
+/*
+ * CPodeQuadInit
+ *
+ * CPodeQuadInit allocates and initializes quadrature related
+ * memory for a problem. All problem specification inputs are
+ * checked for errors. If any error occurs during initialization,
+ * it is reported to the error handler function.
+ * The return value is CP_SUCCESS = 0 if no errors occurred, or
+ * a negative value otherwise.
+ */
+
+int CPodeQuadInit(void *cpode_mem, CPQuadFn qfun, void *q_data, N_Vector q0)
+{
+ CPodeMem cp_mem;
+ booleantype allocOK;
+ long int lrw1Q, liw1Q;
+
+ /* Check cpode_mem */
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeQuadInit", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Set space requirements for one N_Vector */
+ N_VSpace(q0, &lrw1Q, &liw1Q);
+ cp_mem->cp_lrw1Q = lrw1Q;
+ cp_mem->cp_liw1Q = liw1Q;
+
+ /* Allocate the vectors (using q0 as a template) */
+ allocOK = cpQuadAlloc(cp_mem, q0);
+ if (!allocOK) {
+ cpProcessError(cp_mem, CP_MEM_FAIL, "CPODES", "CPodeQuadInit", MSGCP_MEM_FAIL);
+ return(CP_MEM_FAIL);
+ }
+
+ /* Initialize znQ[0] in the history array */
+ N_VScale(ONE, q0, cp_mem->cp_znQ[0]);
+
+ /* Copy the input parameters into CPODES state */
+ cp_mem->cp_qfun = qfun;
+ cp_mem->cp_q_data = q_data;
+
+ /* Initialize counters */
+ cp_mem->cp_nqe = 0;
+ cp_mem->cp_netfQ = 0;
+
+ /* Quadrature integration turned ON */
+ cp_mem->cp_quadr = TRUE;
+ cp_mem->cp_quadMallocDone = TRUE;
+
+ /* Quadrature initialization was successfull */
+ return(CP_SUCCESS);
+}
+
+/*-----------------------------------------------------------------*/
+
+#define lrw1Q (cp_mem->cp_lrw1Q)
+#define liw1Q (cp_mem->cp_liw1Q)
+
+/*-----------------------------------------------------------------*/
+
+/*
+ * CPodeQuadReInit
+ *
+ * CPodeQuadReInit re-initializes CPODES's quadrature related memory
+ * for a problem, assuming it has already been allocated in prior
+ * calls to CPodeMalloc and CPodeQuadInit.
+ * All problem specification inputs are checked for errors.
+ * If any error occurs during initialization, it is reported to the
+ * error handler function.
+ * The return value is CP_SUCCESS = 0 if no errors occurred, or
+ * a negative value otherwise.
+ */
+
+int CPodeQuadReInit(void *cpode_mem, CPQuadFn qfun, void *q_data, N_Vector q0)
+{
+ CPodeMem cp_mem;
+
+ /* Check cpode_mem */
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeQuadReInit", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Ckeck if quadrature was initialized? */
+ if (cp_mem->cp_quadMallocDone == FALSE) {
+ cpProcessError(cp_mem, CP_NO_QUAD, "CPODES", "CPodeQuadReInit", MSGCP_NO_QUAD);
+ return(CP_NO_QUAD);
+ }
+
+ /* Initialize znQ[0] in the history array */
+ N_VScale(ONE, q0, cp_mem->cp_znQ[0]);
+
+ /* Copy the input parameters into CPODES state */
+ cp_mem->cp_qfun = qfun;
+ cp_mem->cp_q_data = q_data;
+
+ /* Initialize counters */
+ cp_mem->cp_nqe = 0;
+ cp_mem->cp_netfQ = 0;
+
+ /* Quadrature integration turned ON */
+ cp_mem->cp_quadr = TRUE;
+
+ /* Quadrature re-initialization was successfull */
+ return(CP_SUCCESS);
+}
+
+/*
+ * =================================================================
+ * READIBILITY REPLACEMENTS
+ * =================================================================
+ */
+
+#define fi (cp_mem->cp_fi)
+#define fe (cp_mem->cp_fe)
+#define f_data (cp_mem->cp_f_data)
+#define efun (cp_mem->cp_efun)
+#define e_data (cp_mem->cp_e_data)
+#define qmax (cp_mem->cp_qmax)
+#define mxstep (cp_mem->cp_mxstep)
+#define mxhnil (cp_mem->cp_mxhnil)
+#define sldeton (cp_mem->cp_sldeton)
+#define hin (cp_mem->cp_hin)
+#define hmin (cp_mem->cp_hmin)
+#define hmax_inv (cp_mem->cp_hmax_inv)
+#define istop (cp_mem->cp_istop)
+#define tstop (cp_mem->cp_tstop)
+#define tstopset (cp_mem->cp_tstopset)
+#define maxncf (cp_mem->cp_maxncf)
+#define maxnef (cp_mem->cp_maxnef)
+#define nlscoef (cp_mem->cp_nlscoef)
+#define tol_type (cp_mem->cp_tol_type)
+#define reltol (cp_mem->cp_reltol)
+#define Sabstol (cp_mem->cp_Sabstol)
+#define Vabstol (cp_mem->cp_Vabstol)
+
+#define uround (cp_mem->cp_uround)
+#define zn (cp_mem->cp_zn)
+#define ewt (cp_mem->cp_ewt)
+#define y (cp_mem->cp_y)
+#define yp (cp_mem->cp_yp)
+#define acor (cp_mem->cp_acor)
+#define tempv (cp_mem->cp_tempv)
+#define ftemp (cp_mem->cp_ftemp)
+#define q (cp_mem->cp_q)
+#define qprime (cp_mem->cp_qprime)
+#define next_q (cp_mem->cp_next_q)
+#define qwait (cp_mem->cp_qwait)
+#define L (cp_mem->cp_L)
+#define h (cp_mem->cp_h)
+#define hprime (cp_mem->cp_hprime)
+#define next_h (cp_mem->cp_next_h)
+#define eta (cp_mem->cp_eta)
+#define etaqm1 (cp_mem->cp_etaqm1)
+#define etaq (cp_mem->cp_etaq)
+#define etaqp1 (cp_mem->cp_etaqp1)
+#define nscon (cp_mem->cp_nscon)
+#define hscale (cp_mem->cp_hscale)
+#define tn (cp_mem->cp_tn)
+#define tau (cp_mem->cp_tau)
+#define tq (cp_mem->cp_tq)
+#define l (cp_mem->cp_l)
+#define p (cp_mem->cp_p)
+#define rl1 (cp_mem->cp_rl1)
+#define gamma (cp_mem->cp_gamma)
+#define gammap (cp_mem->cp_gammap)
+#define gamrat (cp_mem->cp_gamrat)
+#define acnrm (cp_mem->cp_acnrm)
+#define etamax (cp_mem->cp_etamax)
+#define nst (cp_mem->cp_nst)
+#define nfe (cp_mem->cp_nfe)
+#define ncfn (cp_mem->cp_ncfn)
+#define netf (cp_mem->cp_netf)
+#define nhnil (cp_mem->cp_nhnil)
+#define qu (cp_mem->cp_qu)
+#define hu (cp_mem->cp_hu)
+#define saved_tq5 (cp_mem->cp_saved_tq5)
+#define indx_acor (cp_mem->cp_indx_acor)
+#define tolsf (cp_mem->cp_tolsf)
+#define lsetup_exists (cp_mem->cp_lsetup_exists)
+#define nor (cp_mem->cp_nor)
+#define ssdat (cp_mem->cp_ssdat)
+
+#define proj_enabled (cp_mem->cp_proj_enabled)
+#define proj_freq (cp_mem->cp_proj_freq)
+#define proj_type (cp_mem->cp_proj_type)
+#define cnstr_type (cp_mem->cp_cnstr_type)
+#define cfun (cp_mem->cp_cfun)
+#define c_data (cp_mem->cp_c_data)
+#define pfun (cp_mem->cp_pfun)
+#define p_data (cp_mem->cp_p_data)
+#define yC (cp_mem->cp_yC)
+#define acorP (cp_mem->cp_acorP)
+#define errP (cp_mem->cp_errP)
+#define ctol (cp_mem->cp_ctol)
+#define ctemp (cp_mem->cp_ctemp)
+#define tempvP1 (cp_mem->cp_tempvP1)
+#define tempvP2 (cp_mem->cp_tempvP2)
+#define nstlprj (cp_mem->cp_nstlprj)
+#define applyProj (cp_mem->cp_applyProj)
+
+#define quadr (cp_mem->cp_quadr)
+#define qfun (cp_mem->cp_qfun)
+#define q_data (cp_mem->cp_q_data)
+#define errconQ (cp_mem->cp_errconQ)
+#define tol_typeQ (cp_mem->cp_tol_typeQ)
+#define reltolQ (cp_mem->cp_reltolQ)
+#define SabstolQ (cp_mem->cp_SabstolQ)
+#define VabstolQ (cp_mem->cp_VabstolQ)
+#define znQ (cp_mem->cp_znQ)
+#define ewtQ (cp_mem->cp_ewtQ)
+#define acorQ (cp_mem->cp_acorQ)
+#define yQ (cp_mem->cp_yQ)
+#define tempvQ (cp_mem->cp_tempvQ)
+#define acnrmQ (cp_mem->cp_acnrmQ)
+#define nqe (cp_mem->cp_nqe)
+#define netfQ (cp_mem->cp_netfQ)
+#define quadMallocDone (cp_mem->cp_quadMallocDone)
+
+#define doRootfinding (cp_mem->cp_doRootfinding)
+#define tlo (cp_mem->cp_tlo)
+#define thi (cp_mem->cp_thi)
+#define tretlast (cp_mem->cp_tretlast)
+#define toutc (cp_mem->cp_toutc)
+#define taskc (cp_mem->cp_taskc)
+#define irfnd (cp_mem->cp_irfnd)
+
+/*-----------------------------------------------------------------*/
+
+/*
+ * CPode
+ *
+ * This routine is the main driver of the CPODES package.
+ *
+ * It integrates over a time interval defined by the user, by calling
+ * cpStep to do internal time steps.
+ *
+ * The first time that CPode is called for a successfully initialized
+ * problem, it computes a tentative initial step size h.
+ *
+ * CPode supports four modes, specified by mode: CP_NORMAL, CP_ONE_STEP,
+ * CP_NORMAL_TSTOP, and CP_ONE_STEP_TSTOP.
+ * In the CP_NORMAL mode, the solver steps until it reaches or passes tout
+ * and then interpolates to obtain y(tout).
+ * In the CP_ONE_STEP mode, it takes one internal step and returns.
+ * CP_NORMAL_TSTOP and CP_ONE_STEP_TSTOP are similar to CP_NORMAL and CP_ONE_STEP,
+ * respectively, but the integration never proceeds past tstop (which
+ * must have been defined through a call to CPodeSetStopTime).
+ */
+
+int CPode(void *cpode_mem, realtype tout, realtype *tret,
+ N_Vector yout, N_Vector ypout, int mode)
+{
+ CPodeMem cp_mem;
+ long int nstloc;
+ int retval, hflag, kflag, istate, ier, task, irfndp;
+ realtype troundoff, rh, nrm;
+
+ /*
+ * -------------------------------------
+ * 1. Check and process inputs
+ * -------------------------------------
+ */
+
+ /* Check if cpode_mem exists */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPode", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Check if cpode_mem was allocated */
+ if (cp_mem->cp_MallocDone == FALSE) {
+ cpProcessError(cp_mem, CP_NO_MALLOC, "CPODES", "CPode", MSGCP_NO_MALLOC);
+ return(CP_NO_MALLOC);
+ }
+
+ /* Check for yout != NULL */
+ if (yout == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPode", MSGCP_YOUT_NULL);
+ return(CP_ILL_INPUT);
+ }
+ y = yout;
+
+ /* Check for ypout != NULL */
+ if (ypout == NULL ) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPode", MSGCP_YPOUT_NULL);
+ return(CP_ILL_INPUT);
+ }
+ yp = ypout;
+
+ /* Check for tret != NULL */
+ if (tret == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPode", MSGCP_TRET_NULL);
+ return(CP_ILL_INPUT);
+ }
+
+ /* Check for valid mode */
+ if ((mode != CP_NORMAL) &&
+ (mode != CP_ONE_STEP) &&
+ (mode != CP_NORMAL_TSTOP) &&
+ (mode != CP_ONE_STEP_TSTOP) ) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPode", MSGCP_BAD_MODE);
+ return(CP_ILL_INPUT);
+ }
+
+ /* Split mode into task and istop */
+ if ((mode == CP_NORMAL_TSTOP) || (mode == CP_ONE_STEP_TSTOP)) {
+ if ( tstopset == FALSE ) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPode", MSGCP_NO_TSTOP);
+ return(CP_ILL_INPUT);
+ }
+ istop = TRUE;
+ } else {
+ istop = FALSE;
+ }
+ if ((mode == CP_NORMAL) || (mode == CP_NORMAL_TSTOP)) {
+ task = CP_NORMAL; toutc = tout;
+ } else {
+ task = CP_ONE_STEP;
+ }
+ taskc = task;
+
+ /*
+ * ----------------------------------------
+ * 2. Initializations performed only at
+ * the first step (nst=0):
+ * - initial setup
+ * - compute initial step size
+ * - check for approach to tstop
+ * - check for approach to a root
+ * - scale zn[1] by h
+ * ----------------------------------------
+ */
+
+ if (nst == 0) {
+
+ /* Initial setup */
+ ier = cpInitialSetup(cp_mem);
+ if (ier!= CP_SUCCESS) return(ier);
+
+ /* In CP_EXPL mode, call fun at (t0, y0) to set zn[1] = f'(t0,y0) */
+ if (ode_type == CP_EXPL) {
+ retval = fe(tn, zn[0], zn[1], f_data);
+ nfe++;
+ if (retval < 0) {
+ cpProcessError(cp_mem, CP_ODEFUNC_FAIL, "CPODES", "CPode", MSGCP_ODEFUNC_FAILED, tn);
+ return(CP_ODEFUNC_FAIL);
+ }
+ if (retval > 0) {
+ cpProcessError(cp_mem, CP_FIRST_ODEFUNC_ERR, "CPODES", "CPode", MSGCP_ODEFUNC_FIRST);
+ return(CP_FIRST_ODEFUNC_ERR);
+ }
+ }
+
+ /* If computing any quadratures, call qfun at (t0,y0) to set znQ[1] = q'(t0,y0) */
+ if (quadr) {
+ retval = qfun(tn, zn[0], znQ[1], q_data);
+ nqe++;
+ if (retval < 0) {
+ cpProcessError(cp_mem, CP_QUADFUNC_FAIL, "CPODES", "CPode", MSGCP_QUADFUNC_FAILED, tn);
+ return(CP_QUADFUNC_FAIL);
+ }
+ if (retval > 0) {
+ cpProcessError(cp_mem, CP_FIRST_QUADFUNC_ERR, "CPODES", "CPode", MSGCP_QUADFUNC_FIRST);
+ return(CP_FIRST_QUADFUNC_ERR);
+ }
+ }
+
+ /* Set initial h (from H0 or cpHin). */
+ h = hin;
+ if ( (h != ZERO) && ((tout-tn)*h < ZERO) ) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPode", MSGCP_BAD_H0);
+ return(CP_ILL_INPUT);
+ }
+ if (h == ZERO) {
+ hflag = cpHin(cp_mem, tout);
+ if (hflag != CP_SUCCESS) {
+ istate = cpHandleFailure(cp_mem, hflag);
+ return(istate);
+ }
+ }
+ rh = ABS(h)*hmax_inv;
+ if (rh > ONE) h /= rh;
+ if (ABS(h) < hmin) h *= hmin/ABS(h);
+
+ /* Check for approach to tstop */
+ if (istop) {
+ if ( (tstop - tn)*h < ZERO ) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPode", MSGCP_BAD_TSTOP, tn);
+ return(CP_ILL_INPUT);
+ }
+ if ( (tn + h - tstop)*h > ZERO )
+ h = (tstop - tn)*(ONE-FOUR*uround);
+ }
+
+ cp_mem->cp_h0u = h;
+
+ /* Check for zeros of root function g at and near t0. If found at t0 we'll
+ advance time a little to see whether the zeroes go away. If not, we'll
+ deactivate the g's that stay zero. On return we will have marked the
+ end of the "previous" rootfinding interval as thi, with ghi=g(thi). */
+ if (doRootfinding) {
+ retval = cpRcheck1(cp_mem);
+ if (retval == CP_RTFUNC_FAIL) {
+ cpProcessError(cp_mem, CP_RTFUNC_FAIL, "CPODES", "cpRcheck1", MSGCP_RTFUNC_FAILED, tn);
+ return(CP_RTFUNC_FAIL);
+ }
+ }
+
+ /* Scale zn[1] and (if needed) znQ[1] by h */
+ hscale = h;
+ hprime = h;
+ N_VScale(h, zn[1], zn[1]);
+ if (quadr) N_VScale(h, znQ[1], znQ[1]);
+
+ }
+
+ /*
+ * ------------------------------------------------------
+ * 3. At following steps, perform stop tests:
+ * - check for root in last step
+ * - check if we passed tstop
+ * - check if we passed tout (NORMAL mode)
+ * - check if current tn was returned (ONE_STEP mode)
+ * - check if we are close to tstop
+ * (adjust step size if needed)
+ * -------------------------------------------------------
+ */
+
+ if (nst > 0) {
+
+ /* Estimate an infinitesimal time interval to be used as
+ a roundoff for time quantities (based on current time
+ and step size) */
+ troundoff = FUZZ_FACTOR*uround*(ABS(tn) + ABS(h));
+
+ /* First, check for a root in the last step taken, other than the
+ last root found, if any. If task = CP_ONE_STEP and y(tn) was not
+ returned because of an intervening root, return y(tn) now. */
+ if (doRootfinding) {
+
+ irfndp = irfnd;
+ /* This may find a root at the start of the interval in which case
+ (tlo,thi] brackets it and glo and ghi are set. If no root then thi and
+ ghi are set ready for the call to cpRcheck3(). */
+ retval = cpRcheck2(cp_mem);
+
+ if (retval == CP_RTFUNC_FAIL) {
+ cpProcessError(cp_mem, CP_RTFUNC_FAIL, "CPODES", "cpRcheck2", MSGCP_RTFUNC_FAILED, tlo);
+ return(CP_RTFUNC_FAIL);
+ } else if (retval == RTFOUND) {
+ tretlast = *tret = thi;
+ return(CP_ROOT_RETURN);
+ }
+
+ /* If tn is distinct from tretlast (within roundoff),
+ check remaining interval for roots */
+ if ( ABS(tn - tretlast) > troundoff ) {
+
+ /* Search for a root between the old thi and tn, updating tlo,thi. */
+ retval = cpRcheck3(cp_mem);
+
+ if (retval == CP_SUCCESS) { /* no root found */
+ irfnd = 0;
+ if ((irfndp == 1) && (task == CP_ONE_STEP)) {
+ tretlast = *tret = tn;
+ cpGetSolution(cp_mem, tn, yout, ypout);
+ return(CP_SUCCESS);
+ }
+ } else if (retval == RTFOUND) { /* a new root was found */
+ irfnd = 1;
+ tretlast = *tret = thi;
+ return(CP_ROOT_RETURN);
+ } else if (retval == CP_RTFUNC_FAIL) { /* g failed */
+ cpProcessError(cp_mem, CP_RTFUNC_FAIL, "CPODES", "cpRcheck3", MSGCP_RTFUNC_FAILED, thi);
+ return(CP_RTFUNC_FAIL);
+ }
+
+ }
+
+ } /* end of root stop check */
+
+ /* Test for tn past tstop */
+ if ( istop && ((tstop - tn)*h < ZERO) ) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPode", MSGCP_BAD_TSTOP, tn);
+ return(CP_ILL_INPUT);
+ }
+
+ /* In CP_NORMAL mode, test if tout was reached */
+ if ( (task == CP_NORMAL) && ((tn-tout)*h >= ZERO) ) {
+ tretlast = *tret = tout;
+ ier = cpGetSolution(cp_mem, tout, yout, ypout);
+ if (ier != CP_SUCCESS) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPode", MSGCP_BAD_TOUT, tout);
+ return(CP_ILL_INPUT);
+ }
+ return(CP_SUCCESS);
+ }
+
+ /* In CP_ONE_STEP mode, test if tn was returned */
+ if ( task == CP_ONE_STEP && ABS(tn - tretlast) > troundoff ) {
+ tretlast = *tret = tn;
+ cpGetSolution(cp_mem, tn, yout, ypout);
+ return(CP_SUCCESS);
+ }
+
+ /* Test for tn at tstop or near tstop */
+ if ( istop ) {
+
+ if ( ABS(tn - tstop) <= troundoff) {
+ ier = cpGetSolution(cp_mem, tstop, yout, ypout);
+ if (ier != CP_SUCCESS) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPode", MSGCP_BAD_TSTOP, tn);
+ return(CP_ILL_INPUT);
+ }
+ tretlast = *tret = tstop;
+ return(CP_TSTOP_RETURN);
+ }
+
+ /* If next step would overtake tstop, adjust stepsize */
+ if ( (tn + hprime - tstop)*h > ZERO ) {
+ hprime = (tstop - tn)*(ONE-FOUR*uround);
+ eta = hprime/h;
+ }
+
+ }
+
+ }
+
+ /*
+ * --------------------------------------------------
+ * 4. Looping point for internal steps
+ *
+ * 4.1. check for errors (too many steps, too much
+ * accuracy requested, step size too small)
+ * 4.2. take a new step (call cpStep)
+ * 4.3. stop on error
+ * 4.4. perform stop tests:
+ * - check for root in last step
+ * - check if tout was passed
+ * - check if close to tstop
+ * - check if in ONE_STEP mode (must return)
+ * --------------------------------------------------
+ */
+
+ nstloc = 0;
+ loop {
+
+ next_h = h;
+ next_q = q;
+
+ /* Reset and check ewt, ewtQ */
+ if (nst > 0) {
+
+ ier = efun(zn[0], ewt, e_data);
+ if (ier != 0) {
+ if (tol_type == CP_WF) cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPode", MSGCP_EWT_NOW_FAIL, tn);
+ else cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPode", MSGCP_EWT_NOW_BAD, tn);
+ istate = CP_ILL_INPUT;
+ tretlast = *tret = tn;
+ cpGetSolution(cp_mem, tn, yout, ypout);
+ break;
+ }
+
+ if (quadr && errconQ) {
+ ier = cpQuadEwtSet(cp_mem, znQ[0], ewtQ);
+ if(ier != 0) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPode", MSGCP_EWTQ_NOW_BAD, tn);
+ istate = CP_ILL_INPUT;
+ tretlast = *tret = tn;
+ N_VScale(ONE, zn[0], yout);
+ break;
+ }
+ }
+
+ }
+
+ /* Check for too many steps */
+ if (nstloc >= mxstep) {
+ cpProcessError(cp_mem, CP_TOO_MUCH_WORK, "CPODES", "CPode", MSGCP_MAX_STEPS, tn);
+ istate = CP_TOO_MUCH_WORK;
+ tretlast = *tret = tn;
+ cpGetSolution(cp_mem, tn, yout, ypout);
+ break;
+ }
+
+ /* Check for too much accuracy requested */
+ nrm = N_VWrmsNorm(zn[0], ewt);
+ if (quadr && errconQ) nrm = cpQuadUpdateNorm(cp_mem, nrm, znQ[0], ewtQ);
+ tolsf = uround * nrm;
+ if (tolsf > ONE) {
+ cpProcessError(cp_mem, CP_TOO_MUCH_ACC, "CPODES", "CPode", MSGCP_TOO_MUCH_ACC, tn);
+ istate = CP_TOO_MUCH_ACC;
+ tretlast = *tret = tn;
+ cpGetSolution(cp_mem, tn, yout, ypout);
+ tolsf *= TWO;
+ break;
+ } else {
+ tolsf = ONE;
+ }
+
+ /* Check for h below roundoff level in tn */
+ if (tn + h == tn) {
+ nhnil++;
+ if (nhnil <= mxhnil)
+ cpProcessError(cp_mem, CP_WARNING, "CPODES", "CPode", MSGCP_HNIL, tn, h);
+ if (nhnil == mxhnil)
+ cpProcessError(cp_mem, CP_WARNING, "CPODES", "CPode", MSGCP_HNIL_DONE);
+ }
+
+ /* Call cpStep to take a step */
+ kflag = cpStep(cp_mem);
+
+ /* Process failed step cases, and exit loop */
+ if (kflag != CP_SUCCESS) {
+ istate = cpHandleFailure(cp_mem, kflag);
+ tretlast = *tret = tn;
+ cpGetSolution(cp_mem, tn, yout, ypout);
+ break;
+ }
+
+ nstloc++;
+
+ /* Check for root in step just taken. */
+ if (doRootfinding) {
+ /* This will move tlo up to the previous thi, then move thi to the
+ end of the current step and then use (tlo,thi] as the initial search
+ interval. */
+ retval = cpRcheck3(cp_mem);
+
+ if (retval == RTFOUND) { /* A new root was found */
+ irfnd = 1;
+ istate = CP_ROOT_RETURN;
+ tretlast = *tret = thi;
+ break;
+ } else if (retval == CP_RTFUNC_FAIL) { /* g failed */
+ cpProcessError(cp_mem, CP_RTFUNC_FAIL, "CPODES", "cpRcheck3", MSGCP_RTFUNC_FAILED, thi);
+ istate = CP_RTFUNC_FAIL;
+ break;
+ }
+
+ }
+
+ /* In NORMAL mode, check if tout reached */
+ if ( (task == CP_NORMAL) && (tn-tout)*h >= ZERO ) {
+ istate = CP_SUCCESS;
+ tretlast = *tret = tout;
+ (void) cpGetSolution(cp_mem, tout, yout, ypout);
+ next_q = qprime;
+ next_h = hprime;
+ break;
+ }
+
+ /* Check if tn is at tstop or near tstop */
+ if ( istop ) {
+
+ troundoff = FUZZ_FACTOR*uround*(ABS(tn) + ABS(h));
+ if ( ABS(tn - tstop) <= troundoff) {
+ (void) cpGetSolution(cp_mem, tstop, yout, ypout);
+ tretlast = *tret = tstop;
+ istate = CP_TSTOP_RETURN;
+ break;
+ }
+
+ if ( (tn + hprime - tstop)*h > ZERO ) {
+ hprime = (tstop - tn)*(ONE-FOUR*uround);
+ eta = hprime/h;
+ }
+
+ }
+
+ /* In ONE_STEP mode, copy y and exit loop */
+ if (task == CP_ONE_STEP) {
+ istate = CP_SUCCESS;
+ tretlast = *tret = tn;
+ cpGetSolution(cp_mem, tn, yout, ypout);
+ next_q = qprime;
+ next_h = hprime;
+ break;
+ }
+
+ } /* end looping for internal steps */
+
+ return(istate);
+}
+
+/*-----------------------------------------------------------------*/
+
+/*
+ * CPodeGetDky
+ *
+ * This routine computes the k-th derivative of the interpolating
+ * polynomial at the time t and stores the result in the vector dky.
+ * The formula is:
+ * q
+ * dky = SUM c(j,k) * (t - tn)^(j-k) * h^(-j) * zn[j] ,
+ * j=k
+ * where c(j,k) = j*(j-1)*...*(j-k+1), q is the current order, and
+ * zn[j] is the j-th column of the Nordsieck history array.
+ *
+ */
+
+int CPodeGetDky(void *cpode_mem, realtype t, int k, N_Vector dky)
+{
+ realtype s, c, r;
+ realtype tfuzz, tp, tn1;
+ int i, j;
+ CPodeMem cp_mem;
+
+ /* Check all inputs for legality */
+
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetDky", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (dky == NULL) {
+ cpProcessError(cp_mem, CP_BAD_DKY, "CPODES", "CPodeGetDky", MSGCP_NULL_DKY);
+ return(CP_BAD_DKY);
+ }
+
+ if ((k < 0) || (k > q)) {
+ cpProcessError(cp_mem, CP_BAD_K, "CPODES", "CPodeGetDky", MSGCP_BAD_K);
+ return(CP_BAD_K);
+ }
+
+ /* Allow for some slack */
+ tfuzz = FUZZ_FACTOR * uround * (ABS(tn) + ABS(hu));
+ if (hu < ZERO) tfuzz = -tfuzz;
+ tp = tn - hu - tfuzz;
+ tn1 = tn + tfuzz;
+ if ((t-tp)*(t-tn1) > ZERO) {
+ cpProcessError(cp_mem, CP_BAD_T, "CPODES", "CPodeGetDky", MSGCP_BAD_T, t, tn-hu, tn);
+ return(CP_BAD_T);
+ }
+
+ /* Sum the differentiated interpolating polynomial */
+
+ s = (t - tn) / h;
+ for (j=q; j >= k; j--) {
+ c = ONE;
+ for (i=j; i >= j-k+1; i--) c *= i;
+ if (j == q) {
+ N_VScale(c, zn[q], dky);
+ } else {
+ N_VLinearSum(c, zn[j], s, dky, dky);
+ }
+ }
+ if (k == 0) return(CP_SUCCESS);
+ r = RPowerI(h,-k);
+ N_VScale(r, dky, dky);
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetQuad
+ *
+ * This routine extracts quadrature solution into yQout.
+ * This is just a wrapper that calls CPodeGetQuadDky with k=0
+ */
+
+int CPodeGetQuad(void *cpode_mem, realtype t, N_Vector yQout)
+{
+ return(CPodeGetQuadDky(cpode_mem,t,0,yQout));
+}
+
+/*
+ * CPodeGetQuadDky
+ *
+ * CPodeQuadDky computes the kth derivative of the yQ function at
+ * time t, where tn-hu <= t <= tn, tn denotes the current
+ * internal time reached, and hu is the last internal step size
+ * successfully used by the solver. The user may request
+ * k=0, 1, ..., qu, where qu is the current order.
+ * The derivative vector is returned in dky. This vector
+ * must be allocated by the caller. It is only legal to call this
+ * function after a successful return from CPode with quadrature
+ * computation enabled.
+ */
+
+int CPodeGetQuadDky(void *cpode_mem, realtype t, int k, N_Vector dkyQ)
+{
+ realtype s, c, r;
+ realtype tfuzz, tp, tn1;
+ int i, j;
+ CPodeMem cp_mem;
+
+ /* Check all inputs for legality */
+
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetQuadDky", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if(quadr != TRUE) {
+ cpProcessError(cp_mem, CP_NO_QUAD, "CPODES", "CPodeGetQuadDky", MSGCP_NO_QUAD);
+ return(CP_NO_QUAD);
+ }
+
+ if (dkyQ == NULL) {
+ cpProcessError(cp_mem, CP_BAD_DKY, "CPODES", "CPodeGetQuadDky", MSGCP_NULL_DKY);
+ return(CP_BAD_DKY);
+ }
+
+ if ((k < 0) || (k > q)) {
+ cpProcessError(cp_mem, CP_BAD_K, "CPODES", "CPodeGetQuadDky", MSGCP_BAD_K);
+ return(CP_BAD_K);
+ }
+
+ /* Allow for some slack */
+ tfuzz = FUZZ_FACTOR * uround * (ABS(tn) + ABS(hu));
+ if (hu < ZERO) tfuzz = -tfuzz;
+ tp = tn - hu - tfuzz;
+ tn1 = tn + tfuzz;
+ if ((t-tp)*(t-tn1) > ZERO) {
+ cpProcessError(cp_mem, CP_BAD_T, "CPODES", "CPodeGetQuadDky", MSGCP_BAD_T);
+ return(CP_BAD_T);
+ }
+
+ /* Sum the differentiated interpolating polynomial */
+
+ s = (t - tn) / h;
+ for (j=q; j >= k; j--) {
+ c = ONE;
+ for (i=j; i >= j-k+1; i--) c *= i;
+ if (j == q) {
+ N_VScale(c, znQ[q], dkyQ);
+ } else {
+ N_VLinearSum(c, znQ[j], s, dkyQ, dkyQ);
+ }
+ }
+ if (k == 0) return(CP_SUCCESS);
+ r = RPowerI(h,-k);
+ N_VScale(r, dkyQ, dkyQ);
+ return(CP_SUCCESS);
+
+}
+
+/*
+ * CPodeFree
+ *
+ * This routine frees the problem memory allocated by CPodeInit.
+ * Such memory includes all the vectors allocated by cpAllocVectors,
+ * and the memory lmem for the linear solver (deallocated by a call
+ * to lfree).
+ */
+
+void CPodeFree(void **cpode_mem)
+{
+ CPodeMem cp_mem;
+
+ if (*cpode_mem == NULL) return;
+
+ cp_mem = (CPodeMem) (*cpode_mem);
+
+ cpFreeVectors(cp_mem);
+
+ CPodeQuadFree(cp_mem);
+
+ if (nls_type == CP_NEWTON && cp_mem->cp_lfree != NULL)
+ cp_mem->cp_lfree(cp_mem);
+
+ if (cp_mem->cp_lfreeP != NULL)
+ cp_mem->cp_lfreeP(cp_mem);
+
+ if (cp_mem->cp_rootMallocDone) cpRootFree(cp_mem);
+
+ if (cp_mem->cp_projMallocDone) cpProjFree(cp_mem);
+
+ free(*cpode_mem);
+ *cpode_mem = NULL;
+}
+
+/*
+ * CPodeQuadFree
+ *
+ * CPodeQuadFree frees the problem memory in cpode_mem allocated
+ * for quadrature integration. Its only argument is the pointer
+ * cpode_mem returned by CPodeCreate.
+ */
+
+void CPodeQuadFree(void *cpode_mem)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem == NULL) return;
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if(quadMallocDone) {
+ cpQuadFree(cp_mem);
+ quadMallocDone = FALSE;
+ quadr = FALSE;
+ }
+}
+
+
+/*
+ * =================================================================
+ * PRIVATE FUNCTIONS
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Memory allocation/deallocation and initialization functions
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * cpCheckNvector
+ * This routine checks if all required vector operations are present.
+ * If any of them is missing it returns FALSE.
+ */
+
+static booleantype cpCheckNvector(N_Vector tmpl)
+{
+ if((tmpl->ops->nvclone == NULL) ||
+ (tmpl->ops->nvdestroy == NULL) ||
+ (tmpl->ops->nvlinearsum == NULL) ||
+ (tmpl->ops->nvconst == NULL) ||
+ (tmpl->ops->nvprod == NULL) ||
+ (tmpl->ops->nvdiv == NULL) ||
+ (tmpl->ops->nvscale == NULL) ||
+ (tmpl->ops->nvabs == NULL) ||
+ (tmpl->ops->nvinv == NULL) ||
+ (tmpl->ops->nvaddconst == NULL) ||
+ (tmpl->ops->nvmaxnorm == NULL) ||
+ (tmpl->ops->nvwrmsnorm == NULL) ||
+ (tmpl->ops->nvmin == NULL))
+ return(FALSE);
+ else
+ return(TRUE);
+}
+
+/*
+ * cpAllocVectors
+ *
+ * This routine allocates the CPODES vectors ewt, acor, tempv, ftemp,
+ * and zn[0], ..., zn[maxord]. If tol_type=CP_SV, it also allocates
+ * space for Vabstol. If all memory allocations are successful,
+ * cpAllocVectors returns TRUE. Otherwise all allocated memory is
+ * freed and cpAllocVectors returns FALSE. This routine also sets the
+ * optional outputs lrw and liw, which are (respectively) the lengths
+ * of the real and integer work spaces allocated here.
+ */
+
+static booleantype cpAllocVectors(CPodeMem cp_mem, N_Vector tmpl, int tol)
+{
+ int i, j;
+
+ /* Allocate ewt, acor, tempv, ftemp */
+
+ ewt = N_VClone(tmpl);
+ if (ewt == NULL) return(FALSE);
+
+ acor = N_VClone(tmpl);
+ if (acor == NULL) {
+ N_VDestroy(ewt);
+ return(FALSE);
+ }
+
+ tempv = N_VClone(tmpl);
+ if (tempv == NULL) {
+ N_VDestroy(ewt);
+ N_VDestroy(acor);
+ return(FALSE);
+ }
+
+ ftemp = N_VClone(tmpl);
+ if (ftemp == NULL) {
+ N_VDestroy(tempv);
+ N_VDestroy(ewt);
+ N_VDestroy(acor);
+ return(FALSE);
+ }
+
+ /* Allocate zn[0] ... zn[qmax] */
+
+ for (j=0; j <= qmax; j++) {
+ zn[j] = N_VClone(tmpl);
+ if (zn[j] == NULL) {
+ N_VDestroy(ewt);
+ N_VDestroy(acor);
+ N_VDestroy(tempv);
+ N_VDestroy(ftemp);
+ for (i=0; i < j; i++) N_VDestroy(zn[i]);
+ return(FALSE);
+ }
+ }
+
+ /* Update solver workspace lengths */
+ lrw += (qmax + 5)*lrw1;
+ liw += (qmax + 5)*liw1;
+
+ if (tol == CP_SV) {
+ Vabstol = N_VClone(tmpl);
+ if (Vabstol == NULL) {
+ N_VDestroy(ewt);
+ N_VDestroy(acor);
+ N_VDestroy(tempv);
+ N_VDestroy(ftemp);
+ for (i=0; i <= qmax; i++) N_VDestroy(zn[i]);
+ return(FALSE);
+ }
+ lrw += lrw1;
+ liw += liw1;
+ cp_mem->cp_VabstolMallocDone = TRUE;
+ }
+
+ /* Store the value of qmax used here */
+ cp_mem->cp_qmax_alloc = qmax;
+
+ return(TRUE);
+}
+
+/*
+ * cpFreeVectors
+ *
+ * This routine frees the CPODES vectors allocated in cpAllocVectors.
+ */
+
+static void cpFreeVectors(CPodeMem cp_mem)
+{
+ int j, maxord;
+
+ maxord = cp_mem->cp_qmax_alloc;
+
+ N_VDestroy(ewt);
+ N_VDestroy(acor);
+ N_VDestroy(tempv);
+ N_VDestroy(ftemp);
+ for(j=0; j <= maxord; j++) N_VDestroy(zn[j]);
+
+ lrw -= (maxord + 5)*lrw1;
+ liw -= (maxord + 5)*liw1;
+
+ if (cp_mem->cp_VabstolMallocDone) {
+ N_VDestroy(Vabstol);
+ lrw -= lrw1;
+ liw -= liw1;
+ }
+}
+
+/*
+ * cpProjAlloc allocates memory for the internal projection functions.
+ */
+
+static booleantype cpProjAlloc(CPodeMem cp_mem, N_Vector c_tmpl, N_Vector s_tmpl)
+{
+
+ /* Vectors cloned from c_tmpl (length M) */
+
+ ctol = N_VClone(c_tmpl);
+ if (ctol == NULL) return(FALSE);
+
+ ctemp = N_VClone(c_tmpl);
+ if (ctemp == NULL) {
+ N_VDestroy(ctol);
+ return(FALSE);
+ }
+
+ tempvP1 = N_VClone(c_tmpl);
+ if (tempvP1 == NULL) {
+ N_VDestroy(ctol);
+ N_VDestroy(ctemp);
+ return(FALSE);
+ }
+
+ tempvP2 = N_VClone(c_tmpl);
+ if (tempvP2 == NULL) {
+ N_VDestroy(ctol);
+ N_VDestroy(ctemp);
+ N_VDestroy(tempvP1);
+ return(FALSE);
+ }
+
+ /* Vectors cloned from s_tmpl (length N) */
+
+ acorP = N_VClone(s_tmpl);
+ if (acorP == NULL) {
+ N_VDestroy(ctol);
+ N_VDestroy(ctemp);
+ N_VDestroy(tempvP1);
+ N_VDestroy(tempvP2);
+ return(FALSE);
+ }
+
+ yC = N_VClone(s_tmpl);
+ if (yC == NULL) {
+ N_VDestroy(ctol);
+ N_VDestroy(ctemp);
+ N_VDestroy(tempvP1);
+ N_VDestroy(tempvP2);
+ N_VDestroy(acorP);
+ return(FALSE);
+ }
+
+ errP = N_VClone(s_tmpl);
+ if (errP == NULL) {
+ N_VDestroy(ctol);
+ N_VDestroy(ctemp);
+ N_VDestroy(tempvP1);
+ N_VDestroy(tempvP2);
+ N_VDestroy(acorP);
+ N_VDestroy(yC);
+ return(FALSE);
+ }
+
+
+ return(TRUE);
+}
+
+/*
+ * cpProjFree frees the memory allocated in cpProjAlloc.
+ */
+
+static void cpProjFree(CPodeMem cp_mem)
+{
+
+ N_VDestroy(ctol);
+ N_VDestroy(ctemp);
+ N_VDestroy(tempvP1);
+ N_VDestroy(tempvP2);
+ N_VDestroy(acorP);
+ N_VDestroy(yC);
+ N_VDestroy(errP);
+
+ lrw -= lrw1;
+ liw -= liw1;
+
+}
+
+/*
+ * cpQuadAlloc allocates memory for the quadrature integration
+ *
+ * NOTE: Space for ewtQ is allocated even when errconQ=FALSE,
+ * although in this case, ewtQ is never used. The reason for this
+ * decision is to allow the user to re-initialize the quadrature
+ * computation with errconQ=TRUE, after an initialization with
+ * errconQ=FALSE, without new memory allocation within
+ * CPodeQuadReInit.
+ */
+
+static booleantype cpQuadAlloc(CPodeMem cp_mem, N_Vector q_tmpl)
+{
+ int i, j;
+
+ /* Allocate ewtQ */
+ ewtQ = N_VClone(q_tmpl);
+ if (ewtQ == NULL) {
+ return(FALSE);
+ }
+
+ /* Allocate acorQ */
+ acorQ = N_VClone(q_tmpl);
+ if (acorQ == NULL) {
+ N_VDestroy(ewtQ);
+ return(FALSE);
+ }
+
+ /* Allocate yQ */
+ yQ = N_VClone(q_tmpl);
+ if (yQ == NULL) {
+ N_VDestroy(ewtQ);
+ N_VDestroy(acorQ);
+ return(FALSE);
+ }
+
+ /* Allocate tempvQ */
+ tempvQ = N_VClone(q_tmpl);
+ if (tempvQ == NULL) {
+ N_VDestroy(ewtQ);
+ N_VDestroy(acorQ);
+ N_VDestroy(yQ);
+ return(FALSE);
+ }
+
+ /* Allocate zQn[0] ... zQn[maxord] */
+
+ for (j=0; j <= qmax; j++) {
+ znQ[j] = N_VClone(q_tmpl);
+ if (znQ[j] == NULL) {
+ N_VDestroy(ewtQ);
+ N_VDestroy(acorQ);
+ N_VDestroy(yQ);
+ N_VDestroy(tempvQ);
+ for (i=0; i < j; i++) N_VDestroy(znQ[i]);
+ return(FALSE);
+ }
+ }
+
+ /* Store the value of qmax used here */
+ cp_mem->cp_qmax_allocQ = qmax;
+
+ /* Update solver workspace lengths */
+ lrw += (qmax + 5)*lrw1Q;
+ liw += (qmax + 5)*liw1Q;
+
+ return(TRUE);
+}
+
+/*
+ * cpQuadFree frees the memory allocated in cpQuadAlloc.
+ */
+
+static void cpQuadFree(CPodeMem cp_mem)
+{
+ int j, maxord;
+
+ maxord = cp_mem->cp_qmax_allocQ;
+
+ N_VDestroy(ewtQ);
+ N_VDestroy(acorQ);
+ N_VDestroy(yQ);
+ N_VDestroy(tempvQ);
+
+ for (j=0; j<=maxord; j++) N_VDestroy(znQ[j]);
+
+ lrw -= (maxord + 5)*lrw1Q;
+ liw -= (maxord + 5)*liw1Q;
+
+ if (cp_mem->cp_VabstolQMallocDone) {
+ N_VDestroy(VabstolQ);
+ lrw -= lrw1Q;
+ liw -= liw1Q;
+ }
+
+ cp_mem->cp_VabstolQMallocDone = FALSE;
+}
+
+/*
+ * cpInitialSetup
+ *
+ * This routine performs input consistency checks at the first step.
+ * If needed, it also checks the linear solver module and calls the
+ * linear solver initialization routine.
+ */
+
+static int cpInitialSetup(CPodeMem cp_mem)
+{
+ int ier;
+
+ /* Did the user provide efun? */
+ if (tol_type != CP_WF) {
+ efun = cpEwtSet;
+ e_data = (void *)cp_mem;
+ } else {
+ if (efun == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "cpInitialSetup", MSGCP_NO_EFUN);
+ return(CP_ILL_INPUT);
+ }
+ }
+
+ /* Evaluate error weights at initial time */
+ ier = efun(zn[0], ewt, e_data);
+ if (ier != 0) {
+ if (tol_type == CP_WF) cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "cpInitialSetup", MSGCP_EWT_FAIL);
+ else cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "cpInitialSetup", MSGCP_BAD_EWT);
+ return(CP_ILL_INPUT);
+ }
+
+ /* Evaluate quadrature error weights */
+ if (quadr && errconQ) {
+ ier = cpQuadEwtSet(cp_mem, znQ[0], ewtQ);
+ if (ier != 0) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPInitialSetup", MSGCP_BAD_EWTQ);
+ return(CP_ILL_INPUT);
+ }
+ }
+
+ if (!quadr) errconQ = FALSE;
+
+ /* Check if lsolve function exists (if needed)
+ and call linit function (if it exists) */
+ if (nls_type == CP_NEWTON) {
+ if (cp_mem->cp_lsolve == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "cpInitialSetup", MSGCP_LSOLVE_NULL);
+ return(CP_ILL_INPUT);
+ }
+ if (cp_mem->cp_linit != NULL) {
+ ier = cp_mem->cp_linit(cp_mem);
+ if (ier != 0) {
+ cpProcessError(cp_mem, CP_LINIT_FAIL, "CPODES", "cpInitialSetup", MSGCP_LINIT_FAIL);
+ return(CP_LINIT_FAIL);
+ }
+ }
+ }
+
+ /* Tests related to coordinate projection */
+ if (proj_enabled) {
+
+ switch (proj_type) {
+
+ case CP_PROJ_USER:
+
+ /* Check if user provided the projection function */
+ if ( pfun == NULL ) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "cpInitialSetup", MSGCP_NO_PFUN);
+ return(CP_ILL_INPUT);
+ }
+
+ break;
+
+ case CP_PROJ_INTERNAL:
+
+ /* Check if user provided the constraint function */
+ if (cfun == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "cpInitialSetup", MSGCP_NO_CFUN);
+ return(CP_ILL_INPUT);
+ }
+ /* Check lsolveP exists */
+ if ( cp_mem->cp_lsolveP == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "cpInitialSetup", MSGCP_PLSOLVE_NULL);
+ return(CP_ILL_INPUT);
+ }
+ /* Call linitP if it exists */
+ if ( cp_mem->cp_linitP != NULL ) {
+ ier = cp_mem->cp_linitP(cp_mem);
+ if (ier != 0) {
+ cpProcessError(cp_mem, CP_LINIT_FAIL, "CPODES", "cpInitialSetup", MSGCP_PLINIT_FAIL);
+ return(CP_PLINIT_FAIL);
+ }
+ }
+
+ break;
+
+ }
+
+ }
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Initial step size evaluation functions
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * cpHin
+ *
+ * This routine computes a tentative initial step size h0.
+ * If tout is too close to tn (= t0), then cpHin returns CP_TOO_CLOSE
+ * and h remains uninitialized.
+ *
+ */
+
+static int cpHin(CPodeMem cp_mem, realtype tout)
+{
+ int sign, retval;
+ realtype hlb, hub, hg, h0;
+ realtype tdiff, tdist, tround;
+
+ /* If tout is too close to tn, give up */
+ if ((tdiff = tout-tn) == ZERO) return(CP_TOO_CLOSE);
+ sign = (tdiff > ZERO) ? 1 : -1;
+ tdist = ABS(tdiff);
+ tround = uround * MAX(ABS(tn), ABS(tout));
+ if (tdist < TWO*tround) return(CP_TOO_CLOSE);
+
+ /* Set lower and upper bounds on h0*/
+ hlb = HLB_FACTOR * tround;
+ hub = cpUpperBoundH0(cp_mem, tdist);
+
+ /* Compute geometric mean of lower and upper bounds */
+ hg = RSqrt(hlb*hub);
+
+ /* If the bounds cross each other, use hg as initial step size */
+ if (hub < hlb) {
+ if (sign == -1) h = -hg;
+ else h = hg;
+ return(CP_SUCCESS);
+ }
+
+ /* Estimate initial step size */
+ if (ode_type == CP_EXPL) retval = cpHinExpl(cp_mem, hlb, hub, sign, &h0);
+ else retval = cpHinImpl(cp_mem, tdist, &h0);
+
+ /* Apply bounds and attach sign */
+ if (h0 < hlb) h0 = hlb;
+ if (h0 > hub) h0 = hub;
+ if (sign == -1) h0 = -h0;
+ h = h0;
+
+ return(retval);
+}
+
+/*
+ * cpUpperBoundH0
+ *
+ * This routine sets an upper bound on abs(h0) based on
+ * tdist = tn - t0 and the values of y[i]/y'[i] and yQ[i]/yQ'[i].
+ */
+
+static realtype cpUpperBoundH0(CPodeMem cp_mem, realtype tdist)
+{
+ realtype hub_inv, hubQ_inv, hub;
+ N_Vector temp1, temp2;
+ N_Vector tempQ1, tempQ2;
+
+ /*
+ * Bound based on |y0|/|y0'| -- allow at most an increase of
+ * HUB_FACTOR in y0 (based on a forward Euler step). The weight
+ * factor is used as a safeguard against zero components in y0.
+ */
+
+ temp1 = tempv;
+ temp2 = acor;
+
+ N_VAbs(zn[0], temp2);
+ efun(zn[0], temp1, e_data);
+ N_VInv(temp1, temp1);
+ N_VLinearSum(HUB_FACTOR, temp2, ONE, temp1, temp1);
+
+ N_VAbs(zn[1], temp2);
+
+ N_VDiv(temp2, temp1, temp1);
+ hub_inv = N_VMaxNorm(temp1);
+
+ /* Bound based on |yQ|/|yQ'| */
+
+ if (quadr && errconQ) {
+
+ tempQ1 = tempvQ;
+ tempQ2 = acorQ;
+
+ N_VAbs(znQ[0], tempQ2);
+ cpQuadEwtSet(cp_mem, znQ[0], tempQ1);
+ N_VInv(tempQ1, tempQ1);
+ N_VLinearSum(HUB_FACTOR, tempQ2, ONE, tempQ1, tempQ1);
+
+ N_VAbs(znQ[1], tempQ2);
+
+ N_VDiv(tempQ2, tempQ1, tempQ1);
+ hubQ_inv = N_VMaxNorm(tempQ1);
+
+ if (hubQ_inv > hub_inv) hub_inv = hubQ_inv;
+
+ }
+
+ /*
+ * bound based on tdist -- allow at most a step of magnitude
+ * HUB_FACTOR * tdist
+ */
+
+ hub = HUB_FACTOR*tdist;
+
+ /* Use the smaler of the two */
+
+ if (hub*hub_inv > ONE) hub = ONE/hub_inv;
+
+ return(hub);
+}
+
+/*
+ * cpHinExpl
+ *
+ * This routine computes a tentative initial step size h0.
+ * If the ODE function fails unrecoverably, cpHinExpl returns CP_ODEFUNC_FAIL.
+ * If the ODE function fails recoverably too many times and recovery is
+ * not possible, cpHinExpl returns CP_REPTD_ODEFUNC_ERR.
+ * Otherwise, cpHinExpl sets h0 to the chosen value and returns CP_SUCCESS.
+ *
+ * The algorithm used seeks to find h0 as a solution of
+ * (WRMS norm of (h0^2 ypp / 2)) = 1,
+ * where ypp = estimated second derivative of y.
+ *
+ * We start with an initial estimate equal to the geometric mean of the
+ * lower and upper bounds on the step size.
+ *
+ * Loop up to H_MAXITERS times to find h0.
+ * Stop if new and previous values differ by a factor < 2.
+ * Stop if hnew/hg > 2 after one iteration, as this probably means
+ * that the ypp value is bad because of cancellation error.
+ *
+ * For each new proposed hg, we allow H_MAXITERS attempts to
+ * resolve a possible recoverable failure from f() by reducing
+ * the proposed stepsize by a factor of 0.2. If a legal stepsize
+ * still cannot be found, fall back on a previous value if possible,
+ * or else return CP_REPTD_ODEFUNC_ERR.
+ *
+ * Finally, we apply a bias (0.5).
+ */
+
+static int cpHinExpl(CPodeMem cp_mem, realtype hlb, realtype hub, int sign, realtype *h0)
+{
+ int retval, count1, count2;
+ realtype hg, hgs, hs, hnew, hrat, yppnorm;
+ booleantype hgOK, hnewOK;
+
+ /* Initial estimate = geometric mean of computed bounds */
+ hg = RSqrt(hlb*hub);
+
+ /* Outer loop */
+
+ hnewOK = FALSE;
+ hs = hg; /* safeguard against 'uninitialized variable' warning */
+
+ for(count1 = 1; count1 <= H_MAXITERS; count1++) {
+
+ /* Attempts to estimate ypp */
+
+ hgOK = FALSE;
+
+ for (count2 = 1; count2 <= H_MAXITERS; count2++) {
+ hgs = hg*sign;
+ retval = cpYppNorm(cp_mem, hgs, &yppnorm);
+ /* If fun() or qfun() failed unrecoverably, give up */
+ if (retval < 0) return(retval);
+ /* If successful, we can use ypp */
+ if (retval == CP_SUCCESS) {hgOK = TRUE; break;}
+ /* fun() or qfun() failed recoverably; cut step size and test it again */
+ hg *= PT2;
+ }
+
+ /* If fun() or qfun() failed recoverably H_MAXITERS times */
+
+ if (!hgOK) {
+ /* Exit if this is the first or second pass. No recovery possible */
+ if (count1 <= 2) {
+ if (retval == ODEFUNC_RECVR) return(CP_REPTD_ODEFUNC_ERR);
+ if (retval == QUADFUNC_RECVR) return(CP_REPTD_QUADFUNC_ERR);
+ }
+ /* We have a fall-back option. The value hs is a previous hnew which
+ passed through f(). Use it and break */
+ hnew = hs;
+ break;
+ }
+
+ /* The proposed step size is feasible. Save it. */
+ hs = hg;
+
+ /* If the stopping criteria was met, or if this is the last pass, stop */
+ if ( (hnewOK) || (count1 == H_MAXITERS)) {hnew = hg; break;}
+
+ /* Propose new step size */
+ hnew = (yppnorm*hub*hub > TWO) ? RSqrt(TWO/yppnorm) : RSqrt(hg*hub);
+ hrat = hnew/hg;
+
+ /* Accept hnew if it does not differ from hg by more than a factor of 2 */
+ if ((hrat > HALF) && (hrat < TWO)) {
+ hnewOK = TRUE;
+ }
+
+ /* After one pass, if ypp seems to be bad, use fall-back value. */
+ if ((count1 > 1) && (hrat > TWO)) {
+ hnew = hg;
+ hnewOK = TRUE;
+ }
+
+ /* Send this value back through f() */
+ hg = hnew;
+
+ }
+
+ /* Apply bias factor */
+ *h0 = H_BIAS*hnew;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * cpYppNorm
+ *
+ * When the ODE is given in explicit form, this routine computes
+ * an estimate of the second derivative of y using a difference
+ * quotient, and returns its WRMS norm.
+ */
+
+static int cpYppNorm(CPodeMem cp_mem, realtype hg, realtype *yppnorm)
+{
+ int retval;
+
+ /* y <- h*y'(t) + y(t) */
+ N_VLinearSum(hg, zn[1], ONE, zn[0], y);
+
+ /* tempv <- fun(t+h, h*y'(t)+y(t)) */
+ retval = fe(tn+hg, y, tempv, f_data);
+ nfe++;
+ if (retval < 0) return(CP_ODEFUNC_FAIL);
+ if (retval > 0) return(ODEFUNC_RECVR);
+
+ /* tempvQ <- qfun(t+h, h*y'(t)+y(t)) */
+ if (quadr && errconQ) {
+ retval = qfun(tn+hg, y, tempvQ, q_data);
+ nqe++;
+ if (retval < 0) return(CP_QUADFUNC_FAIL);
+ if (retval > 0) return(QUADFUNC_RECVR);
+ }
+
+ /* tempv <- fun(t+h, h*y'(t)+y(t)) - y'(t) */
+ /* tempv <- ypp */
+ N_VLinearSum(ONE, tempv, -ONE, zn[1], tempv);
+ N_VScale(ONE/hg, tempv, tempv);
+
+ /* tempvQ <- qfun(t+h, h*y'(t)+y(t)) - yQ'(t) */
+ /* tempvQ <- yQdd */
+ if (quadr && errconQ) {
+ N_VLinearSum(ONE, tempvQ, -ONE, znQ[1], tempvQ);
+ N_VScale(ONE/hg, tempvQ, tempvQ);
+ }
+
+ /* Estimate ||y''|| */
+ *yppnorm = N_VWrmsNorm(tempv, ewt);
+ if (quadr && errconQ) *yppnorm = cpQuadUpdateNorm(cp_mem, *yppnorm, tempvQ, ewtQ);
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * cpHinImpl
+ *
+ * For ODE systems given in implicit form, use
+ * h0 = min ( 0.001 * tdist , 0.5 / ||yp(t0)||_wrms , 0.5 / ||yQp(t0)||_wrms )
+ */
+
+static int cpHinImpl(CPodeMem cp_mem, realtype tdist, realtype *h0)
+{
+ realtype ypnorm;
+
+ *h0 = PT001 * tdist;
+
+ ypnorm = N_VWrmsNorm(zn[1], ewt);
+ if (quadr && errconQ) ypnorm = cpQuadUpdateNorm(cp_mem, ypnorm, znQ[1], ewtQ);
+
+ if (ypnorm > HALF/(*h0)) *h0 = HALF/ypnorm;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Main step function
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * cpStep
+ *
+ * This routine performs one internal cpode step, from tn to tn + h.
+ * It calls other routines to do all the work.
+ *
+ * The main operations done here are as follows:
+ * - preliminary adjustments if a new step size was chosen;
+ * - prediction of the Nordsieck history array zn at tn + h;
+ * - setting of multistep method coefficients and test quantities;
+ * - solution of the nonlinear system;
+ * - testing the local error;
+ * - updating zn and other state data if successful;
+ * - resetting stepsize and order for the next step.
+ * - if SLDET is on, check for stability, reduce order if necessary.
+ * On a failure in the nonlinear system solution or error test, the
+ * step may be reattempted, depending on the nature of the failure.
+ */
+
+static int cpStep(CPodeMem cp_mem)
+{
+ realtype saved_t, dsm, dsmQ;
+ int ncf, npf, nef;
+ int nflag, kflag, pflag, eflag, qflag;
+ booleantype doProjection;
+
+ saved_t = tn;
+ ncf = npf = nef = 0;
+ nflag = FIRST_CALL;
+
+#ifdef CPODES_DEBUG
+ printf("\nStep # %ld t = %lg\n",nst,tn);
+#endif
+
+ /* If the step size was changed, adjust method parameters */
+ if ((nst > 0) && (hprime != h)) {
+
+#ifdef CPODES_DEBUG
+ printf(" Adjust parameters\n");
+ printf(" qprime = %d hprime = %g\n", qprime, hprime);
+#endif
+
+ cpAdjustParams(cp_mem);
+
+#ifdef CPODES_DEBUG
+ printf(" Done adjust parameters\n");
+#endif
+
+ }
+
+ /* Looping point for attempts to take a step */
+
+ loop {
+
+ /* Prediction */
+
+ cpPredict(cp_mem);
+ cpSet(cp_mem);
+
+#ifdef CPODES_DEBUG
+ printf(" Predict\n");
+#endif
+
+ /* Solve the nonlinear system to correct the states */
+
+#ifdef CPODES_DEBUG
+ printf(" Nonlinear solver\n");
+#endif
+
+ kflag = cpNls(cp_mem, nflag, saved_t, &ncf);
+
+#ifdef CPODES_DEBUG
+ printf(" Nonlinear solver return flag = %d\n",kflag);
+#endif
+
+ if (kflag == PREDICT_AGAIN) {
+ /* If we need to predict again, set nflag=PREV_CONV_FAIL */
+ nflag = PREV_CONV_FAIL;
+ continue;
+ } else if (kflag != CP_SUCCESS) {
+ /* Recovery is not possible. */
+ return(kflag);
+ }
+
+ /* Nonlinear solve successful */
+ ncf = 0;
+
+ /* Do we need to perform projection? */
+
+#ifdef CPODES_DEBUG
+ printf(" Perform projection? ");
+#endif
+
+ doProjection = proj_enabled && (proj_freq > 0) &&
+ ( (nst==0) || (nst >= nstlprj + proj_freq) );
+ applyProj = FALSE;
+
+#ifdef CPODES_DEBUG
+ printf("%d\n",doProjection);
+#endif
+
+ /* If needed, perform the projection step */
+
+ if ( doProjection ) {
+
+#ifdef CPODE_DEBUG
+ printf(" Projection\n");
+#endif
+
+ pflag = cpDoProjection(cp_mem, saved_t, &npf);
+
+#ifdef CPODES_DEBUG
+ printf(" Projection return flag = %d\n",pflag);
+#endif
+
+ if (pflag == PREDICT_AGAIN) {
+ /* If we need to predict again, set nflag=PREV_PROJ_FAIL */
+ nflag = PREV_PROJ_FAIL;
+ continue;
+ } else if (pflag != CP_SUCCESS) {
+ /* Recovery is not possible. */
+ return(pflag);
+ }
+
+ /* Projection successful */
+ npf = 0;
+
+ }
+
+ /* Perform error test */
+
+#ifdef CPODES_DEBUG
+ printf(" Error test\n");
+#endif
+
+ eflag = cpDoErrorTest(cp_mem, saved_t, acnrm, &nef, &dsm);
+
+#ifdef CPODES_DEBUG
+ printf(" Error test return flag = %d\n",eflag);
+#endif
+
+ if (eflag == PREDICT_AGAIN) {
+ /* If we need to predict again set nflag=PREV_ERR_FAIL */
+ nflag = PREV_ERR_FAIL;
+ continue;
+ } else if (eflag != CP_SUCCESS) {
+ /* Recovery is not possible */
+ return(eflag);
+ }
+
+ /* State error test successful */
+ nef = 0;
+
+ /* Deal with quadrature variables if needed */
+
+ if (quadr) {
+
+ /* Correct quadrature variables */
+
+#ifdef CPODES_DEBUG
+ printf(" Correct quadratures\n");
+#endif
+
+ qflag = cpQuadNls(cp_mem, saved_t, &ncf);
+
+#ifdef CPODES_DEBUG
+ printf(" Correct quadratures return flag = %d\n",qflag);
+#endif
+
+ if (qflag == PREDICT_AGAIN) {
+ /* If we need to predict again, set nflag=PREV_CONV_FAIL */
+ nflag = PREV_CONV_FAIL;
+ continue;
+ } else if (qflag != CP_SUCCESS) {
+ /* Recovery is not possible. */
+ return(qflag);
+ }
+
+ /* Quadrature correction successful */
+ ncf = 0;
+
+ /* If needed, perform error test on quadrature variables */
+
+ if (errconQ) {
+
+#ifdef CPODES_DEBUG
+ printf(" Quadratures error test\n");
+#endif
+
+ eflag = cpDoErrorTest(cp_mem, saved_t, acnrmQ, &nef, &dsmQ);
+
+#ifdef CPODES_DEBUG
+ printf(" Quadratures error test return flag = %d\n",eflag);
+#endif
+
+ if (eflag == PREDICT_AGAIN) {
+ /* If we need to predict again set nflag=PREV_ERR_FAIL */
+ nflag = PREV_ERR_FAIL;
+ continue;
+ } else if (eflag != CP_SUCCESS) {
+ /* Recovery is not possible */
+ return(eflag);
+ }
+
+ /* Quadrature error test successful */
+ nef = 0;
+
+ /* Update 'dsm' with 'dsmQ' (to be used in cpPrepareNextStep) */
+ dsm = cpQuadUpdateDsm(cp_mem, dsm, dsmQ);
+
+ }
+
+ }
+
+ /* Everything went fine. Make final updates and exit loop */
+ nstlprj = nst;
+ break;
+
+ }
+
+ /* Apply corrections to Nordsieck history arrays,
+ * update data, and
+ * consider change of step and/or order. */
+ cpCorrect(cp_mem);
+ cpCompleteStep(cp_mem);
+ cpPrepareNextStep(cp_mem, dsm);
+
+ /* If Stablilty Limit Detection is turned on
+ * check if order must be reduced */
+ if (sldeton) cpBDFStab(cp_mem);
+
+ etamax = (nst <= SMALL_NST) ? ETAMX2 : ETAMX3;
+
+ /* Finally, we rescale the acor array to be
+ * the estimated local error vector. */
+ N_VScale(ONE/tq[2], acor, acor);
+ if (quadr) N_VScale(ONE/tq[2], acorQ, acorQ);
+
+ return(CP_SUCCESS);
+
+}
+
+/*
+ * cpGetSolution
+ *
+ * This routine evaluates y(t) and y'(t) as the value and derivative of
+ * the interpolating polynomial at the independent variable t, and stores
+ * the results in the vectors yret and ypret.
+ *
+ * The formulas are:
+ * q
+ * yret = zn[0] + SUM (t - tn)^j * h^(-j) * zn[j] ,
+ * j=1
+ * and
+ * q
+ * ypret = SUM j * (t - tn)^(j-1) * h^(-j) * zn[j] ,
+ * j=1
+ *
+ * This function is called by CPode() with t = tout, t = tn, or t = tstop.
+ *
+ */
+
+int cpGetSolution(void *cpode_mem, realtype t, N_Vector yret, N_Vector ypret)
+{
+ realtype s, c, d;
+ realtype tfuzz, tp, tn1;
+ int j;
+ CPodeMem cp_mem;
+
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Allow for some slack */
+ tfuzz = FUZZ_FACTOR * uround * (ABS(tn) + ABS(hu));
+ if (hu < ZERO) tfuzz = -tfuzz;
+ tp = tn - hu - tfuzz;
+ tn1 = tn + tfuzz;
+ if ((t-tp)*(t-tn1) > ZERO) return(CP_BAD_T);
+
+ /* Initialize yret = zn[0] and ypret = 0 */
+ N_VScale(ONE, zn[0], yret);
+ N_VConst(ZERO, ypret);
+
+ /* Accumulate multiples of columns zn[j] into yret and ypret. */
+ s = (t - tn) / h;
+ c = s;
+ d = ONE/h;
+ for (j=1; j<=q; j++) {
+ N_VLinearSum(ONE, yret, c, zn[j], yret);
+ N_VLinearSum(ONE, ypret, d, zn[j], ypret);
+ d = (j+1)*c/h;
+ c *= s;
+ }
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Functions acting on the Nordsieck history array
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * cpPredict
+ *
+ * This routine advances tn by the tentative step size h, and computes
+ * the predicted array z_n(0), which is overwritten on zn.
+ * The prediction of zn is done by repeated additions.
+ * In TSTOP mode, it is possible for tn + h to be past tstop by roundoff,
+ * and in that case, we reset tn (after incrementing by h) to tstop.
+ */
+
+static void cpPredict(CPodeMem cp_mem)
+{
+ int j, k;
+
+ tn += h;
+ if (istop) {
+ if ((tn - tstop)*h > ZERO) tn = tstop;
+ }
+
+ for (k = 1; k <= q; k++)
+ for (j = q; j >= k; j--)
+ N_VLinearSum(ONE, zn[j-1], ONE, zn[j], zn[j-1]);
+
+ if (quadr) {
+ for (k = 1; k <= q; k++)
+ for (j = q; j >= k; j--)
+ N_VLinearSum(ONE, znQ[j-1], ONE, znQ[j], znQ[j-1]);
+ }
+
+}
+
+/*
+ * cpRescale
+ *
+ * This routine rescales the Nordsieck array by multiplying the
+ * jth column zn[j] by eta^j, j = 1, ..., q. Then the value of
+ * h is rescaled by eta, and hscale is reset to h.
+ *
+ * This function is used in cpNls, cpDoProjection, and cpDoerrorTest
+ */
+
+void cpRescale(CPodeMem cp_mem)
+{
+ int j;
+ realtype factor;
+
+ factor = eta;
+ for (j=1; j <= q; j++) {
+ N_VScale(factor, zn[j], zn[j]);
+ if (quadr) N_VScale(factor, znQ[j], znQ[j]);
+ factor *= eta;
+ }
+ h = hscale * eta;
+ next_h = h;
+ hscale = h;
+ nscon = 0;
+}
+
+/*
+ * cpRestore
+ *
+ * This routine restores the value of tn to saved_t and undoes the
+ * prediction. After execution of cpRestore, the Nordsieck array zn has
+ * the same values as before the call to cpPredict.
+ *
+ * This function is used in cpNls, cpDoProjection, and cpDoerrorTest
+ */
+
+void cpRestore(CPodeMem cp_mem, realtype saved_t)
+{
+ int j, k;
+
+ tn = saved_t;
+
+ for (k = 1; k <= q; k++)
+ for (j = q; j >= k; j--)
+ N_VLinearSum(ONE, zn[j-1], -ONE, zn[j], zn[j-1]);
+
+ if (quadr) {
+ for (k = 1; k <= q; k++)
+ for (j = q; j >= k; j--)
+ N_VLinearSum(ONE, znQ[j-1], -ONE, znQ[j], znQ[j-1]);
+ }
+
+}
+
+/*
+ * cpCorrect
+ *
+ * This routine applies the corrections to the zn array.
+ * The correction to zn is done by repeated additions.
+ */
+
+static void cpCorrect(CPodeMem cp_mem)
+{
+ int j;
+
+ for (j=0; j <= q; j++)
+ N_VLinearSum(l[j], acor, ONE, zn[j], zn[j]);
+
+ if (applyProj) {
+ for (j=0; j <= q; j++)
+ N_VLinearSum(p[j], acorP, ONE, zn[j], zn[j]);
+ }
+
+ if (quadr) {
+ for (j=0; j <= q; j++)
+ N_VLinearSum(l[j], acorQ, ONE, znQ[j], znQ[j]);
+ }
+
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Quadrature correction function
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * cpQuadNls
+ *
+ * This function attempts to correct the quadrature variables.
+ *
+ * The only way this function can fail is through the user's quad
+ * function. If quad fails unrecoverably we return immediately. If
+ * it fails with a recoverable error, we consider a new prediction.
+ *
+ * We count any failure here as a convergence failure (we do not
+ * distinguish these from convergence failures in the nonlinear
+ * solver for the state correction).
+ *
+ * NOTE: This is technically not a nonlinear solver, but this name is
+ * used to reflect the similarity to cpNls.
+ */
+
+static int cpQuadNls(CPodeMem cp_mem, realtype saved_t, int *ncfPtr)
+{
+ int retval;
+
+ /* Save quadrature correction in acorQ */
+ retval = qfun(tn, y, acorQ, q_data);
+ nqe++;
+
+ if (retval == 0) {
+
+ /* Find correction */
+ N_VLinearSum(h, acorQ, -ONE, znQ[1], acorQ);
+ N_VScale(rl1, acorQ, acorQ);
+
+ /* Compute its WRMS norm */
+ acnrmQ = N_VWrmsNorm(acorQ, ewtQ);
+
+ /* Apply correction to quadrature variables */
+ N_VLinearSum(ONE, znQ[0], ONE, acorQ, yQ);
+
+ return(CP_SUCCESS);
+
+ }
+
+ /* Increment ncfn and restore zn */
+ ncfn++;
+ cpRestore(cp_mem, saved_t);
+
+ if (retval < 0) return(CP_QUADFUNC_FAIL);
+
+ /* At this point, qfun had a recoverable error; increment ncf */
+ (*ncfPtr)++;
+ etamax = ONE;
+
+ /* If we had maxncf failures or |h| = hmin,
+ return CP_REPTD_QUADFUNC_ERR. */
+ if ((ABS(h) <= hmin*ONEPSM) || (*ncfPtr == maxncf))
+ return(CP_REPTD_QUADFUNC_ERR);
+
+ /* Reduce step size; return to reattempt the step */
+ eta = MAX(ETACF, hmin / ABS(h));
+ cpRescale(cp_mem);
+
+ return(PREDICT_AGAIN);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * LMM-related functions
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * cpAdjustParams
+ *
+ * This routine is called when a change in step size was decided upon,
+ * and it handles the required adjustments to the history array zn.
+ * If there is to be a change in order, we call cpAdjustOrder and reset
+ * q, L = q+1, and qwait. Then in any case, we call cpRescale, which
+ * resets h and rescales the Nordsieck array.
+ */
+
+static void cpAdjustParams(CPodeMem cp_mem)
+{
+ if (qprime != q) {
+ cpAdjustOrder(cp_mem, qprime-q);
+ q = qprime;
+ L = q+1;
+ qwait = L;
+ }
+ cpRescale(cp_mem);
+}
+
+/*
+ * cpAdjustOrder
+ *
+ * This routine is a high level routine which handles an order
+ * change by an amount deltaq (= +1 or -1). If a decrease in order
+ * is requested and q==2, then the routine returns immediately.
+ * Otherwise cpAdjustAdams or cpAdjustBDF is called to handle the
+ * order change (depending on the value of lmm_type).
+ */
+
+static void cpAdjustOrder(CPodeMem cp_mem, int deltaq)
+{
+ if ((q==2) && (deltaq != 1)) return;
+
+ switch(lmm_type){
+ case CP_ADAMS:
+ cpAdjustAdams(cp_mem, deltaq);
+ break;
+ case CP_BDF:
+ cpAdjustBDF(cp_mem, deltaq);
+ break;
+ }
+}
+
+/*
+ * cpAdjustAdams
+ *
+ * This routine adjusts the history array on a change of order q by
+ * deltaq, in the case that lmm_type == CP_ADAMS.
+ */
+
+static void cpAdjustAdams(CPodeMem cp_mem, int deltaq)
+{
+ int i, j;
+ realtype xi, hsum;
+
+ /* On an order increase, set new column of zn to zero and return */
+
+ if (deltaq==1) {
+ N_VConst(ZERO, zn[L]);
+ if (quadr) N_VConst(ZERO, znQ[L]);
+ return;
+ }
+
+ /*
+ * On an order decrease, each zn[j] is adjusted by a multiple of zn[q].
+ * The coeffs. in the adjustment are the coeffs. of the polynomial:
+ * x
+ * q * INT { u * ( u + xi_1 ) * ... * ( u + xi_{q-2} ) } du
+ * 0
+ * where xi_j = [t_n - t_(n-j)]/h => xi_0 = 0
+ */
+
+ for (i=0; i <= qmax; i++) l[i] = ZERO;
+ l[1] = ONE;
+ hsum = ZERO;
+ for (j=1; j <= q-2; j++) {
+ hsum += tau[j];
+ xi = hsum / hscale;
+ for (i=j+1; i >= 1; i--) l[i] = l[i]*xi + l[i-1];
+ }
+
+ for (j=1; j <= q-2; j++) l[j+1] = q * (l[j] / (j+1));
+
+ for (j=2; j < q; j++)
+ N_VLinearSum(-l[j], zn[q], ONE, zn[j], zn[j]);
+
+ if (quadr) {
+ for (j=2; j < q; j++)
+ N_VLinearSum(-l[j], znQ[q], ONE, znQ[j], znQ[j]);
+ }
+
+}
+
+/*
+ * cpAdjustBDF
+ *
+ * This is a high level routine which handles adjustments to the
+ * history array on a change of order by deltaq in the case that
+ * lmm_type == CP_BDF. cpAdjustBDF calls cpIncreaseBDF if deltaq = +1 and
+ * cpDecreaseBDF if deltaq = -1 to do the actual work.
+ */
+
+static void cpAdjustBDF(CPodeMem cp_mem, int deltaq)
+{
+ switch(deltaq) {
+ case 1 :
+ cpIncreaseBDF(cp_mem);
+ return;
+ case -1:
+ cpDecreaseBDF(cp_mem);
+ return;
+ }
+}
+
+/*
+ * cpIncreaseBDF
+ *
+ * This routine adjusts the history array on an increase in the
+ * order q in the case that lmm_type == CP_BDF.
+ * A new column zn[q+1] is set equal to a multiple of the saved
+ * vector (= acor) in zn[indx_acor]. Then each zn[j] is adjusted by
+ * a multiple of zn[q+1]. The coefficients in the adjustment are the
+ * coefficients of the polynomial x*x*(x+xi_1)*...*(x+xi_j),
+ * where xi_j = [t_n - t_(n-j)]/h.
+ */
+
+static void cpIncreaseBDF(CPodeMem cp_mem)
+{
+ realtype alpha0, alpha1, prod, xi, xiold, hsum, A1;
+ int i, j;
+
+ for (i=0; i <= qmax; i++) l[i] = ZERO;
+ l[2] = alpha1 = prod = xiold = ONE;
+ alpha0 = -ONE;
+ hsum = hscale;
+ if (q > 1) {
+ for (j=1; j < q; j++) {
+ hsum += tau[j+1];
+ xi = hsum / hscale;
+ prod *= xi;
+ alpha0 -= ONE / (j+1);
+ alpha1 += ONE / xi;
+ for (i=j+2; i >= 2; i--) l[i] = l[i]*xiold + l[i-1];
+ xiold = xi;
+ }
+ }
+ A1 = (-alpha0 - alpha1) / prod;
+
+ /*
+ * zn[indx_acor] contains the value Delta_n = y_n - y_n(0)
+ * This value was stored there at the previous successful
+ * step (in cpCompleteStep)
+ *
+ * A1 contains dbar = (1/xi* - 1/xi_q)/prod(xi_j)
+ */
+
+ N_VScale(A1, zn[indx_acor], zn[L]);
+ for (j=2; j <= q; j++)
+ N_VLinearSum(l[j], zn[L], ONE, zn[j], zn[j]);
+
+ if (quadr) {
+ N_VScale(A1, znQ[indx_acor], znQ[L]);
+ for (j=2; j <= q; j++)
+ N_VLinearSum(l[j], znQ[L], ONE, znQ[j], znQ[j]);
+ }
+
+}
+
+/*
+ * cpDecreaseBDF
+ *
+ * This routine adjusts the history array on a decrease in the
+ * order q in the case that lmm_type == CP_BDF.
+ * Each zn[j] is adjusted by a multiple of zn[q]. The coefficients
+ * in the adjustment are the coefficients of the polynomial
+ * x*x*(x+xi_1)*...*(x+xi_j), where xi_j = [t_n - t_(n-j)]/h.
+ */
+
+static void cpDecreaseBDF(CPodeMem cp_mem)
+{
+ realtype hsum, xi;
+ int i, j;
+
+ for (i=0; i <= qmax; i++) l[i] = ZERO;
+ l[2] = ONE;
+ hsum = ZERO;
+ for(j=1; j <= q-2; j++) {
+ hsum += tau[j];
+ xi = hsum /hscale;
+ for (i=j+2; i >= 2; i--) l[i] = l[i]*xi + l[i-1];
+ }
+
+ for(j=2; j < q; j++)
+ N_VLinearSum(-l[j], zn[q], ONE, zn[j], zn[j]);
+
+ if (quadr) {
+ for (j=2; j < q; j++)
+ N_VLinearSum(-l[j], znQ[q], ONE, znQ[j], znQ[j]);
+ }
+
+}
+
+/*
+ * cpSet
+ *
+ * This routine is a high level routine which calls cpSetAdams or
+ * cpSetBDF to set the coefficients l of the polynomial Lambda
+ * (used in applying the NLS correction to zn), the coefficients p
+ * of the polynomial Phi (used in applying the projection correction
+ * to zn), the test quantity array tq (used in the NLS convergence
+ * test and in the error test), and the related variables rl1, gamma,
+ * and gamrat.
+ */
+
+static void cpSet(CPodeMem cp_mem)
+{
+ switch(lmm_type) {
+ case CP_ADAMS:
+ cpSetAdams(cp_mem);
+ break;
+ case CP_BDF:
+ cpSetBDF(cp_mem);
+ break;
+ }
+ rl1 = ONE / l[1];
+ gamma = h * rl1;
+ if (nst == 0) gammap = gamma;
+ gamrat = (nst > 0) ? gamma / gammap : ONE; /* protect x / x != 1.0 */
+}
+
+/*
+ * cpSetAdams
+ *
+ * This routine handles the computation of l, p, and tq for the
+ * case lmm_type == CP_ADAMS.
+ *
+ * The components of the array l are the coefficients of a
+ * polynomial Lambda(x) = l_0 + l_1 x + ... + l_q x^q, that
+ * satisfies the following q+1 conditions:
+ * Lambda(-1) = 0, Lambda(0) = 1
+ * Lambda'(-xi_i) = 0, i=1,2,...,q-1.
+ * Here x = [t - t_n] /h and xi_i = [t_n - t_(n-i)] / h.
+ *
+ * For the Adams case, the polynomial Phi(x) is identical to Lambda(x)
+ * and therefore p_i = l_i, i=0,1,2,...,q.
+ *
+ * The array tq is set to test quantities used in the convergence
+ * test, the error test, and the selection of h at a new order.
+ */
+
+static void cpSetAdams(CPodeMem cp_mem)
+{
+ realtype m[L_MAX], M[3], hsum;
+ int i;
+
+ if (q == 1) {
+
+ l[0] = l[1] = tq[1] = tq[5] = ONE;
+ tq[2] = TWO;
+ tq[3] = TWELVE;
+ tq[4] = nlscoef * tq[2]; /* = 0.1 * tq[2] */
+
+ } else {
+
+ hsum = cpAdamsStart(cp_mem, m);
+
+ M[0] = cpAltSum(q-1, m, 1);
+ M[1] = cpAltSum(q-1, m, 2);
+
+ cpAdamsFinish(cp_mem, m, M, hsum);
+
+ }
+
+ for(i=0; i<=q; i++) p[i] = l[i];
+
+ return;
+}
+
+/*
+ * cpAdamsStart
+ *
+ * This routine generates in m[] the coefficients of the product
+ * polynomial needed for the Adams l and tq coefficients for q > 1.
+ */
+
+static realtype cpAdamsStart(CPodeMem cp_mem, realtype m[])
+{
+ realtype hsum, xi_inv, sum;
+ int i, j;
+
+ hsum = h;
+ m[0] = ONE;
+ for (i=1; i <= q; i++) m[i] = ZERO;
+ for (j=1; j < q; j++) {
+ if ((j==q-1) && (qwait == 1)) {
+ sum = cpAltSum(q-2, m, 2);
+ tq[1] = m[q-2] / (q * sum);
+ }
+ xi_inv = h / hsum;
+ for (i=j; i >= 1; i--) m[i] += m[i-1] * xi_inv;
+ hsum += tau[j];
+ /* The m[i] are coefficients of product(1 to j) (1 + x/xi_i) */
+ }
+ return(hsum);
+}
+
+/*
+ * cpAdamsFinish
+ *
+ * This routine completes the calculation of the Adams l and tq.
+ */
+
+static void cpAdamsFinish(CPodeMem cp_mem, realtype m[], realtype M[], realtype hsum)
+{
+ int i;
+ realtype M0_inv, xi, xi_inv;
+
+ M0_inv = ONE / M[0];
+
+ l[0] = ONE;
+ for (i=1; i <= q; i++) l[i] = M0_inv * (m[i-1] / i);
+ xi = hsum / h;
+ xi_inv = ONE / xi;
+
+ tq[2] = xi * M[0] / M[1];
+ tq[5] = xi / l[q];
+
+ if (qwait == 1) {
+ for (i=q; i >= 1; i--) m[i] += m[i-1] * xi_inv;
+ M[2] = cpAltSum(q, m, 2);
+ tq[3] = L * M[0] / M[2];
+ }
+
+ tq[4] = nlscoef * tq[2];
+}
+
+/*
+ * cpAltSum
+ *
+ * cpAltSum returns the value of the alternating sum
+ * sum (i= 0 ... iend) [ (-1)^i * (a[i] / (i + k)) ].
+ * If iend < 0 then cpAltSum returns 0.
+ * This operation is needed to compute the integral, from -1 to 0,
+ * of a polynomial x^(k-1) M(x) given the coefficients of M(x).
+ */
+
+static realtype cpAltSum(int iend, realtype a[], int k)
+{
+ int i, sign;
+ realtype sum;
+
+ if (iend < 0) return(ZERO);
+
+ sum = ZERO;
+ sign = 1;
+ for (i=0; i <= iend; i++) {
+ sum += sign * (a[i] / (i+k));
+ sign = -sign;
+ }
+ return(sum);
+}
+
+/*
+ * cpSetBDF
+ *
+ * This routine computes the coefficients l and tq in the case
+ * lmm_type == CP_BDF. cpSetBDF calls cpSetTqBDF to set the test
+ * quantity array tq.
+ *
+ * The components of the array l are the coefficients of a
+ * polynomial Lambda(x) = l_0 + l_1 x + ... + l_q x^q, given by
+ * q-1
+ * Lambda(x) = (1 + x / xi*_q) * PRODUCT (1 + x / xi_i)
+ * i=1
+ *
+ * The components of the array p are the coefficients of a
+ * polynomial Phi(x) = p_0 + p_1 x + ... + p_q x^q, given by
+ * q
+ * Phi(x) = PRODUCT (1 + x / xi_i)
+ * i=1
+ *
+ * Here x = [t - t_n] /h and xi_i = [t_n - t_(n-i)] / h.
+ *
+ * The array tq is set to test quantities used in the convergence
+ * test, the error test, and the selection of h at a new order.
+ */
+
+static void cpSetBDF(CPodeMem cp_mem)
+{
+ realtype alpha0, alpha0_hat, xi_inv, xistar_inv, hsum;
+ int i,j;
+
+ l[0] = l[1] = ONE;
+ p[0] = p[1] = ONE;
+ xi_inv = xistar_inv = ONE;
+ for (i=2; i <= q; i++) {
+ l[i] = ZERO;
+ p[i] = ZERO;
+ }
+ alpha0 = alpha0_hat = -ONE;
+ hsum = h;
+ if (q > 1) {
+ for (j=2; j < q; j++) {
+ hsum += tau[j-1];
+ xi_inv = h / hsum;
+ alpha0 -= ONE / j;
+ /* The l[i] and p[i] are coefficients of product(1 to j) (1 + x/xi_i) */
+ for(i=j; i >= 1; i--) {
+ l[i] += l[i-1]*xi_inv;
+ p[i] += p[i-1]*xi_inv;
+ }
+ }
+
+ /* j = q */
+ alpha0 -= ONE / q;
+ xistar_inv = -l[1] - alpha0;
+ hsum += tau[q-1];
+ xi_inv = h / hsum;
+ alpha0_hat = -l[1] - xi_inv;
+ for (i=q; i >= 1; i--) {
+ l[i] += l[i-1]*xistar_inv;
+ p[i] += p[i-1]*xi_inv;
+ }
+ }
+
+ cpSetTqBDF(cp_mem, hsum, alpha0, alpha0_hat, xi_inv, xistar_inv);
+}
+
+/*
+ * cpSetTqBDF
+ *
+ * This routine sets the test quantity array tq in the case
+ * lmm_type == CP_BDF.
+ */
+
+static void cpSetTqBDF(CPodeMem cp_mem, realtype hsum, realtype alpha0,
+ realtype alpha0_hat, realtype xi_inv, realtype xistar_inv)
+{
+ realtype A1, A2, A3, A4, A5, A6;
+ realtype C, CPrime, CPrimePrime;
+
+ A1 = ONE - alpha0_hat + alpha0;
+ A2 = ONE + q * A1;
+ tq[2] = ABS(alpha0 * (A2 / A1));
+ tq[5] = ABS((A2) / (l[q] * xi_inv/xistar_inv));
+ if (qwait == 1) {
+ C = xistar_inv / l[q];
+ A3 = alpha0 + ONE / q;
+ A4 = alpha0_hat + xi_inv;
+ CPrime = A3 / (ONE - A4 + A3);
+ tq[1] = ABS(CPrime / C);
+ hsum += tau[q];
+ xi_inv = h / hsum;
+ A5 = alpha0 - (ONE / (q+1));
+ A6 = alpha0_hat - xi_inv;
+ CPrimePrime = A2 / (ONE - A6 + A5);
+ tq[3] = ABS(CPrimePrime * xi_inv * (q+2) * A5);
+ }
+ tq[4] = nlscoef * tq[2];
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Error test
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * cpDoErrorTest
+ *
+ * This routine performs the local error test. It is called to test
+ * both the sates and the quadrature variables, with different values
+ * of acor_norm and dsmPtr.
+ *
+ * The weighted local error norm dsm is loaded into *dsmPtr, and
+ * we test whether dsm <= 1.
+ *
+ * If the test passes, cpDoErrorTest returns CP_SUCCESS.
+ *
+ * If the test fails, we undo the step just taken (call cpRestore) and
+ *
+ * - if maxnef error test failures have occurred or if ABS(h) = hmin,
+ * we return CP_ERR_FAILURE.
+ *
+ * - if more than MXNEF1 error test failures have occurred, an order
+ * reduction is forced. If already at order 1, restart by reloading
+ * zn from scratch.
+ *
+ * - otherwise, set return PREDICT_AGAIN, telling cpStep to retry the
+ * step (with nflag = PREV_ERR_FAIL).
+ *
+ * Note that we lump all error test failures together (i.e. we do not
+ * distinguish between failures due to states or to quadratures).
+ */
+
+static int cpDoErrorTest(CPodeMem cp_mem, realtype saved_t, realtype acor_norm,
+ int *nefPtr, realtype *dsmPtr)
+{
+ realtype dsm;
+
+ /* If est. local error norm dsm passes test, return CP_SUCCESS */
+ dsm = acor_norm / tq[2];
+ *dsmPtr = dsm;
+
+#ifdef CPODES_DEBUG
+ printf(" dsm = %lg\n",dsm);
+#endif
+
+ if (dsm <= ONE) return(CP_SUCCESS);
+
+ /* Test failed: increment counters and restore zn array */
+ (*nefPtr)++;
+ netf++;
+ cpRestore(cp_mem, saved_t);
+
+ /* At maxnef failures or |h| = hmin, return CP_ERR_FAILURE */
+ if ((ABS(h) <= hmin*ONEPSM) || (*nefPtr == maxnef)) return(CP_ERR_FAILURE);
+
+ /* Set etamax = 1 to prevent step size increase at end of this step */
+ etamax = ONE;
+
+ /* Set h ratio eta from dsm, rescale, and return for retry of step */
+ if (*nefPtr <= MXNEF1) {
+ eta = ONE / (RPowerR(BIAS2*dsm,ONE/L) + ADDON);
+ eta = MAX(ETAMIN, MAX(eta, hmin / ABS(h)));
+ if (*nefPtr >= SMALL_NEF) eta = MIN(eta, ETAMXF);
+ cpRescale(cp_mem);
+ return(PREDICT_AGAIN);
+ }
+
+ /* After MXNEF1 failures, force an order reduction and retry step */
+ if (q > 1) {
+ eta = MAX(ETAMIN, hmin / ABS(h));
+ cpAdjustOrder(cp_mem,-1);
+ L = q;
+ q--;
+ qwait = L;
+ cpRescale(cp_mem);
+ return(PREDICT_AGAIN);
+ }
+
+ /* If already at order 1, restart: reload zn from scratch */
+ eta = MAX(ETAMIN, hmin / ABS(h));
+ h *= eta;
+ next_h = h;
+ hscale = h;
+ qwait = LONG_WAIT;
+ nscon = 0;
+ N_VScale(eta, zn[1], zn[1]);
+ if (quadr) N_VScale(eta, znQ[1], znQ[1]);
+
+ return(PREDICT_AGAIN);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Succesful step completion functions
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * cpCompleteStep
+ *
+ * This routine performs various update operations when the solution
+ * has passed the local error test.
+ * We increment the step counter nst, record the values hu and qu,
+ * and update the tau array.
+ * The tau[i] are the last q values of h, with tau[1] the most recent.
+ * The counter qwait is decremented, and if qwait == 1 (and q < qmax)
+ * we save acor and tq[5] for a possible order increase.
+ */
+
+static void cpCompleteStep(CPodeMem cp_mem)
+{
+ int i;
+
+ nst++;
+ nscon++;
+ hu = h;
+ qu = q;
+
+ for (i=q; i >= 2; i--) tau[i] = tau[i-1];
+ if ((q==1) && (nst > 1)) tau[2] = tau[1];
+ tau[1] = h;
+
+ /* If necessary, store Delta_n in zn[qmax] to be used in order increase
+ *
+ * This actually will be Delta_{n-1} in the ELTE at q+1 since it happens at
+ * the next to last step of order q before a possible one at order q+1
+ */
+
+ qwait--;
+ if ((qwait == 1) && (q != qmax)) {
+ N_VScale(ONE, acor, zn[qmax]);
+ if (quadr) N_VScale(ONE, acorQ, znQ[qmax]);
+ saved_tq5 = tq[5];
+ indx_acor = qmax;
+ }
+}
+
+/*
+ * CPprepareNextStep
+ *
+ * This routine handles the setting of stepsize and order for the
+ * next step -- hprime and qprime. Along with hprime, it sets the
+ * ratio eta = hprime/h. It also updates other state variables
+ * related to a change of step size or order.
+ */
+
+static void cpPrepareNextStep(CPodeMem cp_mem, realtype dsm)
+{
+ /* If etamax = 1, defer step size or order changes */
+ if (etamax == ONE) {
+ qwait = MAX(qwait, 2);
+ qprime = q;
+ hprime = h;
+ eta = ONE;
+ return;
+ }
+
+ /* etaq is the ratio of new to old h at the current order */
+ etaq = ONE /(RPowerR(BIAS2*dsm,ONE/L) + ADDON);
+
+ /* If no order change, adjust eta and acor in cpSetEta and return */
+ if (qwait != 0) {
+ eta = etaq;
+ qprime = q;
+ cpSetEta(cp_mem);
+ return;
+ }
+
+ /* If qwait = 0, consider an order change. etaqm1 and etaqp1 are
+ the ratios of new to old h at orders q-1 and q+1, respectively.
+ cpChooseEta selects the largest; cpSetEta adjusts eta and acor */
+ qwait = 2;
+ etaqm1 = cpComputeEtaqm1(cp_mem);
+ etaqp1 = cpComputeEtaqp1(cp_mem);
+ cpChooseEta(cp_mem);
+ cpSetEta(cp_mem);
+}
+
+/*
+ * CPsetEta
+ *
+ * This routine adjusts the value of eta according to the various
+ * heuristic limits and the optional input hmax.
+ */
+
+static void cpSetEta(CPodeMem cp_mem)
+{
+
+ /* If eta below the threshhold THRESH, reject a change of step size */
+ if (eta < THRESH) {
+ eta = ONE;
+ hprime = h;
+ } else {
+ /* Limit eta by etamax and hmax, then set hprime */
+ eta = MIN(eta, etamax);
+ eta /= MAX(ONE, ABS(h)*hmax_inv*eta);
+ hprime = h * eta;
+ if (qprime < q) nscon = 0;
+ }
+
+}
+
+/*
+ * cpComputeEtaqm1
+ *
+ * This routine computes and returns the value of etaqm1 for a
+ * possible decrease in order by 1.
+ */
+
+static realtype cpComputeEtaqm1(CPodeMem cp_mem)
+{
+ realtype ddn;
+
+ etaqm1 = ZERO;
+ if (q > 1) {
+ ddn = N_VWrmsNorm(zn[q], ewt) / tq[1];
+ if ( quadr && errconQ) ddn = cpQuadUpdateNorm(cp_mem, ddn, znQ[q], ewtQ);
+ etaqm1 = ONE/(RPowerR(BIAS1*ddn, ONE/q) + ADDON);
+ }
+ return(etaqm1);
+}
+
+/*
+ * cpComputeEtaqp1
+ *
+ * This routine computes and returns the value of etaqp1 for a
+ * possible increase in order by 1.
+ */
+
+static realtype cpComputeEtaqp1(CPodeMem cp_mem)
+{
+ realtype dup, cquot;
+
+ etaqp1 = ZERO;
+ if (q != qmax) {
+ cquot = (tq[5] / saved_tq5) * RPowerI(h/tau[2], L);
+ N_VLinearSum(-cquot, zn[qmax], ONE, acor, tempv);
+ dup = N_VWrmsNorm(tempv, ewt);
+ if ( quadr && errconQ ) {
+ N_VLinearSum(-cquot, znQ[qmax], ONE, acorQ, tempvQ);
+ dup = cpQuadUpdateNorm(cp_mem, dup, tempvQ, ewtQ);
+ }
+ dup = dup / tq[3];
+ etaqp1 = ONE / (RPowerR(BIAS3*dup, ONE/(L+1)) + ADDON);
+ }
+ return(etaqp1);
+}
+
+/*
+ * cpChooseEta
+ * Given etaqm1, etaq, etaqp1 (the values of eta for qprime =
+ * q - 1, q, or q + 1, respectively), this routine chooses the
+ * maximum eta value, sets eta to that value, and sets qprime to the
+ * corresponding value of q. If there is a tie, the preference
+ * order is to (1) keep the same order, then (2) decrease the order,
+ * and finally (3) increase the order. If the maximum eta value
+ * is below the threshhold THRESH, the order is kept unchanged and
+ * eta is set to 1.
+ */
+
+static void cpChooseEta(CPodeMem cp_mem)
+{
+ realtype etam;
+
+ etam = MAX(etaqm1, MAX(etaq, etaqp1));
+
+ if (etam < THRESH) {
+ eta = ONE;
+ qprime = q;
+ return;
+ }
+
+ if (etam == etaq) {
+
+ eta = etaq;
+ qprime = q;
+
+ } else if (etam == etaqm1) {
+
+ eta = etaqm1;
+ qprime = q - 1;
+
+ } else {
+
+ eta = etaqp1;
+ qprime = q + 1;
+
+ if (lmm_type == CP_BDF) {
+
+ /*
+ * Store Delta_n in zn[qmax] to be used in order increase
+ *
+ * This happens at the last step of order q before an increase
+ * to order q+1, so it represents Delta_n in the ELTE at q+1
+ */
+
+ N_VScale(ONE, acor, zn[qmax]);
+ if (quadr && errconQ) N_VScale(ONE, acorQ, znQ[qmax]);
+
+ }
+
+ }
+
+}
+
+/*
+ * cpHandleFailure
+ *
+ * This routine prints error messages for all cases of failure by
+ * cpHin and cpStep. It returns to CPode the value that CPode is
+ * to return to the user.
+ */
+
+static int cpHandleFailure(CPodeMem cp_mem, int flag)
+{
+
+ /* Depending on flag, print error message and return error flag */
+ switch (flag) {
+ case CP_ERR_FAILURE:
+ cpProcessError(cp_mem, CP_ERR_FAILURE, "CPODES", "CPode", MSGCP_ERR_FAILS, tn, h);
+ break;
+ case CP_CONV_FAILURE:
+ cpProcessError(cp_mem, CP_CONV_FAILURE, "CPODES", "CPode", MSGCP_CONV_FAILS, tn, h);
+ break;
+ case CP_LSETUP_FAIL:
+ cpProcessError(cp_mem, CP_LSETUP_FAIL, "CPODES", "CPode", MSGCP_SETUP_FAILED, tn);
+ break;
+ case CP_LSOLVE_FAIL:
+ cpProcessError(cp_mem, CP_LSOLVE_FAIL, "CPODES", "CPode", MSGCP_SOLVE_FAILED, tn);
+ break;
+ case CP_ODEFUNC_FAIL:
+ cpProcessError(cp_mem, CP_ODEFUNC_FAIL, "CPODES", "CPode", MSGCP_ODEFUNC_FAILED, tn);
+ break;
+ case CP_UNREC_ODEFUNC_ERR:
+ cpProcessError(cp_mem, CP_UNREC_ODEFUNC_ERR, "CPODES", "CPode", MSGCP_ODEFUNC_UNREC, tn);
+ break;
+ case CP_REPTD_ODEFUNC_ERR:
+ cpProcessError(cp_mem, CP_REPTD_ODEFUNC_ERR, "CPODES", "CPode", MSGCP_ODEFUNC_REPTD, tn);
+ break;
+ case CP_RTFUNC_FAIL:
+ cpProcessError(cp_mem, CP_RTFUNC_FAIL, "CPODES", "CPode", MSGCP_RTFUNC_FAILED, tn);
+ break;
+ case CP_TOO_CLOSE:
+ cpProcessError(cp_mem, CP_TOO_CLOSE, "CPODES", "CPode", MSGCP_TOO_CLOSE);
+ break;
+ case CP_PROJ_FAILURE:
+ cpProcessError(cp_mem, CP_PROJ_FAILURE, "CPODES", "CPode", MSGCP_PROJ_FAILS, tn, h);
+ break;
+ case CP_CNSTRFUNC_FAIL:
+ cpProcessError(cp_mem, CP_CNSTRFUNC_FAIL, "CPODES", "CPode", MSGCP_CNSTRFUNC_FAILED, tn);
+ break;
+ case CP_REPTD_CNSTRFUNC_ERR:
+ cpProcessError(cp_mem, CP_REPTD_CNSTRFUNC_ERR, "CPODES", "CPode", MSGCP_CNSTRFUNC_REPTD, tn);
+ break;
+ case CP_PROJFUNC_FAIL:
+ cpProcessError(cp_mem, CP_PROJFUNC_FAIL, "CPODES", "CPode", MSGCP_PROJFUNC_FAILED, tn);
+ break;
+ case CP_REPTD_PROJFUNC_ERR:
+ cpProcessError(cp_mem, CP_REPTD_PROJFUNC_ERR, "CPODES", "CPode", MSGCP_PROJFUNC_REPTD, tn);
+ break;
+ case CP_PLSETUP_FAIL:
+ cpProcessError(cp_mem, CP_PLSETUP_FAIL, "CPODES", "CPode", MSGCP_PLSETUP_FAILED, tn);
+ break;
+ case CP_PLSOLVE_FAIL:
+ cpProcessError(cp_mem, CP_PLSOLVE_FAIL, "CPODES", "CPode", MSGCP_PLSOLVE_FAILED, tn);
+ break;
+ default:
+ return(CP_SUCCESS);
+ }
+
+ return(flag);
+
+}
+
+/*
+ * -----------------------------------------------------------------
+ * BDF stability limit detection functions
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * cpBDFStab
+ *
+ * This routine handles the BDF Stability Limit Detection Algorithm
+ * STALD. It is called if lmm_type = CP_BDF and the SLDET option is on.
+ * If the order is 3 or more, the required norm data is saved.
+ * If a decision to reduce order has not already been made, and
+ * enough data has been saved, cpSLdet is called. If it signals
+ * a stability limit violation, the order is reduced, and the step
+ * size is reset accordingly.
+ */
+
+void cpBDFStab(CPodeMem cp_mem)
+{
+ int i,k, ldflag, factorial;
+ realtype sq, sqm1, sqm2;
+
+ /* If order is 3 or greater, then save scaled derivative data,
+ push old data down in i, then add current values to top. */
+
+ if (q >= 3) {
+ for (k = 1; k <= 3; k++)
+ { for (i = 5; i >= 2; i--) ssdat[i][k] = ssdat[i-1][k]; }
+ factorial = 1;
+ for (i = 1; i <= q-1; i++) factorial *= i;
+ sq = factorial*q*(q+1)*acnrm/tq[5];
+ sqm1 = factorial*q*N_VWrmsNorm(zn[q], ewt);
+ sqm2 = factorial*N_VWrmsNorm(zn[q-1], ewt);
+ ssdat[1][1] = sqm2*sqm2;
+ ssdat[1][2] = sqm1*sqm1;
+ ssdat[1][3] = sq*sq;
+ }
+
+ if (qprime >= q) {
+
+ /* If order is 3 or greater, and enough ssdat has been saved,
+ nscon >= q+5, then call stability limit detection routine. */
+
+ if ( (q >= 3) && (nscon >= q+5) ) {
+ ldflag = cpSLdet(cp_mem);
+ if (ldflag > 3) {
+ /* A stability limit violation is indicated by
+ a return flag of 4, 5, or 6.
+ Reduce new order. */
+ qprime = q-1;
+ eta = etaqm1;
+ eta = MIN(eta,etamax);
+ eta = eta/MAX(ONE,ABS(h)*hmax_inv*eta);
+ hprime = h*eta;
+ nor = nor + 1;
+ }
+ }
+ }
+ else {
+ /* Otherwise, let order increase happen, and
+ reset stability limit counter, nscon. */
+ nscon = 0;
+ }
+}
+
+/*
+ * cpSLdet
+ *
+ * This routine detects stability limitation using stored scaled
+ * derivatives data. cpSLdet returns the magnitude of the
+ * dominate characteristic root, rr. The presents of a stability
+ * limit is indicated by rr > "something a little less then 1.0",
+ * and a positive kflag. This routine should only be called if
+ * order is greater than or equal to 3, and data has been collected
+ * for 5 time steps.
+ *
+ * Returned values:
+ * kflag = 1 -> Found stable characteristic root, normal matrix case
+ * kflag = 2 -> Found stable characteristic root, quartic solution
+ * kflag = 3 -> Found stable characteristic root, quartic solution,
+ * with Newton correction
+ * kflag = 4 -> Found stability violation, normal matrix case
+ * kflag = 5 -> Found stability violation, quartic solution
+ * kflag = 6 -> Found stability violation, quartic solution,
+ * with Newton correction
+ *
+ * kflag < 0 -> No stability limitation,
+ * or could not compute limitation.
+ *
+ * kflag = -1 -> Min/max ratio of ssdat too small.
+ * kflag = -2 -> For normal matrix case, vmax > vrrt2*vrrt2
+ * kflag = -3 -> For normal matrix case, The three ratios
+ * are inconsistent.
+ * kflag = -4 -> Small coefficient prevents elimination of quartics.
+ * kflag = -5 -> R value from quartics not consistent.
+ * kflag = -6 -> No corrected root passes test on qk values
+ * kflag = -7 -> Trouble solving for sigsq.
+ * kflag = -8 -> Trouble solving for B, or R via B.
+ * kflag = -9 -> R via sigsq[k] disagrees with R from data.
+ */
+
+static int cpSLdet(CPodeMem cp_mem)
+{
+ int i, k, j, it, kmin, kflag = 0;
+ realtype rat[5][4], rav[4], qkr[4], sigsq[4], smax[4], ssmax[4];
+ realtype drr[4], rrc[4],sqmx[4], qjk[4][4], vrat[5], qc[6][4], qco[6][4];
+ realtype rr, rrcut, vrrtol, vrrt2, sqtol, rrtol;
+ realtype smink, smaxk, sumrat, sumrsq, vmin, vmax, drrmax, adrr;
+ realtype tem, sqmax, saqk, qp, s, sqmaxk, saqj, sqmin;
+ realtype rsa, rsb, rsc, rsd, rd1a, rd1b, rd1c;
+ realtype rd2a, rd2b, rd3a, cest1, corr1;
+ realtype ratp, ratm, qfac1, qfac2, bb, rrb;
+
+ /* The following are cutoffs and tolerances used by this routine */
+
+ rrcut = RCONST(0.98);
+ vrrtol = RCONST(1.0e-4);
+ vrrt2 = RCONST(5.0e-4);
+ sqtol = RCONST(1.0e-3);
+ rrtol = RCONST(1.0e-2);
+
+ rr = ZERO;
+
+ /* Index k corresponds to the degree of the interpolating polynomial. */
+ /* k = 1 -> q-1 */
+ /* k = 2 -> q */
+ /* k = 3 -> q+1 */
+
+ /* Index i is a backward-in-time index, i = 1 -> current time, */
+ /* i = 2 -> previous step, etc */
+
+ /* get maxima, minima, and variances, and form quartic coefficients */
+
+ for (k=1; k<=3; k++) {
+ smink = ssdat[1][k];
+ smaxk = ZERO;
+
+ for (i=1; i<=5; i++) {
+ smink = MIN(smink,ssdat[i][k]);
+ smaxk = MAX(smaxk,ssdat[i][k]);
+ }
+
+ if (smink < TINY*smaxk) {
+ kflag = -1;
+ return(kflag);
+ }
+ smax[k] = smaxk;
+ ssmax[k] = smaxk*smaxk;
+
+ sumrat = ZERO;
+ sumrsq = ZERO;
+ for (i=1; i<=4; i++) {
+ rat[i][k] = ssdat[i][k]/ssdat[i+1][k];
+ sumrat = sumrat + rat[i][k];
+ sumrsq = sumrsq + rat[i][k]*rat[i][k];
+ }
+ rav[k] = PT25 * sumrat;
+ vrat[k] = ABS(PT25*sumrsq - rav[k]*rav[k]);
+
+ qc[5][k] = ssdat[1][k]*ssdat[3][k] - ssdat[2][k]*ssdat[2][k];
+ qc[4][k] = ssdat[2][k]*ssdat[3][k] - ssdat[1][k]*ssdat[4][k];
+ qc[3][k] = ZERO;
+ qc[2][k] = ssdat[2][k]*ssdat[5][k] - ssdat[3][k]*ssdat[4][k];
+ qc[1][k] = ssdat[4][k]*ssdat[4][k] - ssdat[3][k]*ssdat[5][k];
+
+ for (i=1; i<=5; i++) {
+ qco[i][k] = qc[i][k];
+ }
+ } /* End of k loop */
+
+ /* Isolate normal or nearly-normal matrix case. Three quartic will
+ have common or nearly-common roots in this case.
+ Return a kflag = 1 if this procedure works. If three root
+ differ more than vrrt2, return error kflag = -3. */
+
+ vmin = MIN(vrat[1],MIN(vrat[2],vrat[3]));
+ vmax = MAX(vrat[1],MAX(vrat[2],vrat[3]));
+
+ if(vmin < vrrtol*vrrtol) {
+ if (vmax > vrrt2*vrrt2) {
+ kflag = -2;
+ return(kflag);
+ } else {
+ rr = (rav[1] + rav[2] + rav[3])/THREE;
+
+ drrmax = ZERO;
+ for(k = 1;k<=3;k++) {
+ adrr = ABS(rav[k] - rr);
+ drrmax = MAX(drrmax, adrr);
+ }
+ if (drrmax > vrrt2) {
+ kflag = -3;
+ }
+
+ kflag = 1;
+
+ /* can compute charactistic root, drop to next section */
+
+ }
+ } else {
+
+ /* use the quartics to get rr. */
+
+ if (ABS(qco[1][1]) < TINY*ssmax[1]) {
+ kflag = -4;
+ return(kflag);
+ }
+
+ tem = qco[1][2]/qco[1][1];
+ for(i=2; i<=5; i++) {
+ qco[i][2] = qco[i][2] - tem*qco[i][1];
+ }
+
+ qco[1][2] = ZERO;
+ tem = qco[1][3]/qco[1][1];
+ for(i=2; i<=5; i++) {
+ qco[i][3] = qco[i][3] - tem*qco[i][1];
+ }
+ qco[1][3] = ZERO;
+
+ if (ABS(qco[2][2]) < TINY*ssmax[2]) {
+ kflag = -4;
+ return(kflag);
+ }
+
+ tem = qco[2][3]/qco[2][2];
+ for(i=3; i<=5; i++) {
+ qco[i][3] = qco[i][3] - tem*qco[i][2];
+ }
+
+ if (ABS(qco[4][3]) < TINY*ssmax[3]) {
+ kflag = -4;
+ return(kflag);
+ }
+
+ rr = -qco[5][3]/qco[4][3];
+
+ if (rr < TINY || rr > HUNDRED) {
+ kflag = -5;
+ return(kflag);
+ }
+
+ for(k=1; k<=3; k++) {
+ qkr[k] = qc[5][k] + rr*(qc[4][k] + rr*rr*(qc[2][k] + rr*qc[1][k]));
+ }
+
+ sqmax = ZERO;
+ for(k=1; k<=3; k++) {
+ saqk = ABS(qkr[k])/ssmax[k];
+ if (saqk > sqmax) sqmax = saqk;
+ }
+
+ if (sqmax < sqtol) {
+ kflag = 2;
+
+ /* can compute charactistic root, drop to "given rr,etc" */
+
+ } else {
+
+ /* do Newton corrections to improve rr. */
+
+ for(it=1; it<=3; it++) {
+ for(k=1; k<=3; k++) {
+ qp = qc[4][k] + rr*rr*(THREE*qc[2][k] + rr*FOUR*qc[1][k]);
+ drr[k] = ZERO;
+ if (ABS(qp) > TINY*ssmax[k]) drr[k] = -qkr[k]/qp;
+ rrc[k] = rr + drr[k];
+ }
+
+ for(k=1; k<=3; k++) {
+ s = rrc[k];
+ sqmaxk = ZERO;
+ for(j=1; j<=3; j++) {
+ qjk[j][k] = qc[5][j] + s*(qc[4][j] +
+ s*s*(qc[2][j] + s*qc[1][j]));
+ saqj = ABS(qjk[j][k])/ssmax[j];
+ if (saqj > sqmaxk) sqmaxk = saqj;
+ }
+ sqmx[k] = sqmaxk;
+ }
+
+ sqmin = sqmx[1]; kmin = 1;
+ for(k=2; k<=3; k++) {
+ if (sqmx[k] < sqmin) {
+ kmin = k;
+ sqmin = sqmx[k];
+ }
+ }
+ rr = rrc[kmin];
+
+ if (sqmin < sqtol) {
+ kflag = 3;
+ /* can compute charactistic root */
+ /* break out of Newton correction loop and drop to "given rr,etc" */
+ break;
+ } else {
+ for(j=1; j<=3; j++) {
+ qkr[j] = qjk[j][kmin];
+ }
+ }
+ } /* end of Newton correction loop */
+
+ if (sqmin > sqtol) {
+ kflag = -6;
+ return(kflag);
+ }
+ } /* end of if (sqmax < sqtol) else */
+ } /* end of if(vmin < vrrtol*vrrtol) else, quartics to get rr. */
+
+ /* given rr, find sigsq[k] and verify rr. */
+ /* All positive kflag drop to this section */
+
+ for(k=1; k<=3; k++) {
+ rsa = ssdat[1][k];
+ rsb = ssdat[2][k]*rr;
+ rsc = ssdat[3][k]*rr*rr;
+ rsd = ssdat[4][k]*rr*rr*rr;
+ rd1a = rsa - rsb;
+ rd1b = rsb - rsc;
+ rd1c = rsc - rsd;
+ rd2a = rd1a - rd1b;
+ rd2b = rd1b - rd1c;
+ rd3a = rd2a - rd2b;
+
+ if (ABS(rd1b) < TINY*smax[k]) {
+ kflag = -7;
+ return(kflag);
+ }
+
+ cest1 = -rd3a/rd1b;
+ if (cest1 < TINY || cest1 > FOUR) {
+ kflag = -7;
+ return(kflag);
+ }
+ corr1 = (rd2b/cest1)/(rr*rr);
+ sigsq[k] = ssdat[3][k] + corr1;
+ }
+
+ if (sigsq[2] < TINY) {
+ kflag = -8;
+ return(kflag);
+ }
+
+ ratp = sigsq[3]/sigsq[2];
+ ratm = sigsq[1]/sigsq[2];
+ qfac1 = PT25 * (q*q - ONE);
+ qfac2 = TWO/(q - ONE);
+ bb = ratp*ratm - ONE - qfac1*ratp;
+ tem = ONE - qfac2*bb;
+
+ if (ABS(tem) < TINY) {
+ kflag = -8;
+ return(kflag);
+ }
+
+ rrb = ONE/tem;
+
+ if (ABS(rrb - rr) > rrtol) {
+ kflag = -9;
+ return(kflag);
+ }
+
+ /* Check to see if rr is above cutoff rrcut */
+ if (rr > rrcut) {
+ if (kflag == 1) kflag = 4;
+ if (kflag == 2) kflag = 5;
+ if (kflag == 3) kflag = 6;
+ }
+
+ /* All positive kflag returned at this point */
+
+ return(kflag);
+
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Internal error weight evaluation functions
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * cpEwtSet
+ *
+ * This routine is responsible for setting the error weight vector ewt,
+ * according to tol_type, as follows:
+ *
+ * (1) ewt[i] = 1 / (reltol * ABS(ycur[i]) + *abstol), i=0,...,neq-1
+ * if tol_type = CP_SS
+ * (2) ewt[i] = 1 / (reltol * ABS(ycur[i]) + abstol[i]), i=0,...,neq-1
+ * if tol_type = CP_SV
+ *
+ * cpEwtSet returns 0 if ewt is successfully set as above to a
+ * positive vector and -1 otherwise. In the latter case, ewt is
+ * considered undefined.
+ *
+ * All the real work is done in the routines cpEwtSetSS, cpEwtSetSV.
+ */
+
+int cpEwtSet(N_Vector ycur, N_Vector weight, void *edata)
+{
+ CPodeMem cp_mem;
+ int flag = 0;
+
+ /* edata points to cp_mem here */
+ cp_mem = (CPodeMem) edata;
+
+ switch(tol_type) {
+ case CP_SS:
+ flag = cpEwtSetSS(cp_mem, ycur, weight);
+ break;
+ case CP_SV:
+ flag = cpEwtSetSV(cp_mem, ycur, weight);
+ break;
+ }
+
+ return(flag);
+}
+
+/*
+ * cpEwtSetSS
+ *
+ * This routine sets ewt as decribed above in the case tol_type = CP_SS.
+ * It tests for non-positive components before inverting. cpEwtSetSS
+ * returns 0 if ewt is successfully set to a positive vector
+ * and -1 otherwise. In the latter case, ewt is considered undefined.
+ */
+
+static int cpEwtSetSS(CPodeMem cp_mem, N_Vector ycur, N_Vector weight)
+{
+ N_VAbs(ycur, tempv);
+ N_VScale(reltol, tempv, tempv);
+ N_VAddConst(tempv, Sabstol, tempv);
+ if (N_VMin(tempv) <= ZERO) return(-1);
+ N_VInv(tempv, weight);
+ return(0);
+}
+
+/*
+ * cpEwtSetSV
+ *
+ * This routine sets ewt as decribed above in the case tol_type = CP_SV.
+ * It tests for non-positive components before inverting. cpEwtSetSV
+ * returns 0 if ewt is successfully set to a positive vector
+ * and -1 otherwise. In the latter case, ewt is considered undefined.
+ */
+
+static int cpEwtSetSV(CPodeMem cp_mem, N_Vector ycur, N_Vector weight)
+{
+ N_VAbs(ycur, tempv);
+ N_VLinearSum(reltol, tempv, ONE, Vabstol, tempv);
+ if (N_VMin(tempv) <= ZERO) return(-1);
+ N_VInv(tempv, weight);
+ return(0);
+}
+
+/*
+ * cpQuadEwtSet
+ *
+ */
+
+static int cpQuadEwtSet(CPodeMem cp_mem, N_Vector qcur, N_Vector weightQ)
+{
+ int flag=0;
+
+ switch (tol_typeQ) {
+ case CP_SS:
+ flag = cpQuadEwtSetSS(cp_mem, qcur, weightQ);
+ break;
+ case CP_SV:
+ flag = cpQuadEwtSetSV(cp_mem, qcur, weightQ);
+ break;
+ }
+
+ return(flag);
+
+}
+
+/*
+ * cpQuadEwtSetSS
+ *
+ */
+
+static int cpQuadEwtSetSS(CPodeMem cp_mem, N_Vector qcur, N_Vector weightQ)
+{
+ N_VAbs(qcur, tempvQ);
+ N_VScale(reltolQ, tempvQ, tempvQ);
+ N_VAddConst(tempvQ, SabstolQ, tempvQ);
+ if (N_VMin(tempvQ) <= ZERO) return(-1);
+ N_VInv(tempvQ, weightQ);
+
+ return(0);
+}
+
+/*
+ * cpQuadEwtSetSV
+ *
+ */
+
+static int cpQuadEwtSetSV(CPodeMem cp_mem, N_Vector qcur, N_Vector weightQ)
+{
+ N_VAbs(qcur, tempvQ);
+ N_VLinearSum(reltolQ, tempvQ, ONE, VabstolQ, tempvQ);
+ if (N_VMin(tempvQ) <= ZERO) return(-1);
+ N_VInv(tempvQ, weightQ);
+
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Updated WRMS norms
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * cpQuadUpdateNorm
+ *
+ * Updates the norm old_nrm to account for all quadratures.
+ */
+
+static realtype cpQuadUpdateNorm(CPodeMem cp_mem, realtype old_nrm,
+ N_Vector xQ, N_Vector wQ)
+{
+ realtype qnrm;
+
+ qnrm = N_VWrmsNorm(xQ, wQ);
+ if (old_nrm > qnrm) return(old_nrm);
+ else return(qnrm);
+}
+
+/*
+ * cpQuadUpdateDsm
+ *
+ * This routine updates the local error norm dsm with quadrature
+ * related information. Used only if quadratures are computed
+ * with FULL error control.
+ *
+ * Returns the maximum over the wheighted local error norms.
+ */
+
+static realtype cpQuadUpdateDsm(CPodeMem cp_mem, realtype old_dsm,
+ realtype dsmQ)
+{
+ if ( old_dsm > dsmQ ) return(old_dsm);
+ else return(dsmQ);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Error reporting functions
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * cpProcessError is a high level error handling function
+ * - if cp_mem==NULL it prints the error message to stderr
+ * - otherwise, it sets-up and calls the error hadling function
+ * pointed to by cp_ehfun
+ */
+
+#define ehfun (cp_mem->cp_ehfun)
+#define eh_data (cp_mem->cp_eh_data)
+
+void cpProcessError(CPodeMem cp_mem,
+ int error_code, const char *module, const char *fname,
+ const char *msgfmt, ...)
+{
+ va_list ap;
+ char msg[256];
+
+ /* Initialize the argument pointer variable
+ (msgfmt is the last required argument to cpProcessError) */
+
+ va_start(ap, msgfmt);
+
+ if (cp_mem == NULL) { /* We write to stderr */
+
+#ifndef NO_FPRINTF_OUTPUT
+ fprintf(stderr, "\n[%s ERROR] %s\n ", module, fname);
+ fprintf(stderr, msgfmt, 0); // extra arg avoids gcc warnings
+ fprintf(stderr, "\n\n");
+#endif
+
+ } else { /* We can call ehfun */
+
+ /* Compose the message */
+
+ vsprintf(msg, msgfmt, ap);
+
+ /* Call ehfun */
+
+ ehfun(error_code, module, fname, msg, eh_data);
+
+ }
+
+ /* Finalize argument processing */
+
+ va_end(ap);
+
+ return;
+
+}
+
+/* cpErrHandler is the default error handling function.
+ It sends the error message to the stream pointed to by cp_errfp */
+
+#define errfp (cp_mem->cp_errfp)
+
+static void cpErrHandler(int error_code, const char *module,
+ const char *function, char *msg, void *data)
+{
+ CPodeMem cp_mem;
+ char err_type[10];
+
+ /* data points to cp_mem here */
+
+ cp_mem = (CPodeMem) data;
+
+ if (error_code == CP_WARNING)
+ sprintf(err_type,"WARNING");
+ else
+ sprintf(err_type,"ERROR");
+
+#ifndef NO_FPRINTF_OUTPUT
+ if (errfp!=NULL) {
+ fprintf(errfp,"\n[%s %s] %s\n",module,err_type,function);
+ fprintf(errfp," %s\n\n",msg);
+ }
+#endif
+
+ return;
+}
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_band.c b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_band.c
new file mode 100644
index 0000000..cbec221
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_band.c
@@ -0,0 +1,404 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/22 00:12:48 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for the CPBAND linear solver.
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * =================================================================
+ * IMPORTED HEADER FILES
+ * =================================================================
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <cpodes/cpodes_band.h>
+#include "cpodes_direct_impl.h"
+#include "cpodes_private.h"
+
+#include <sundials/sundials_math.h>
+
+/*
+ * =================================================================
+ * PROTOTYPES FOR PRIVATE FUNCTIONS
+ * =================================================================
+ */
+
+/* CPBAND linit, lsetup, lsolve, and lfree routines */
+static int cpBandInit(CPodeMem cp_mem);
+static int cpBandSetup(CPodeMem cp_mem, int convfail,
+ N_Vector yP, N_Vector ypP, N_Vector fctP,
+ booleantype *jcurPtr,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+static int cpBandSolve(CPodeMem cp_mem, N_Vector b, N_Vector weight,
+ N_Vector yC, N_Vector ypC, N_Vector fctC);
+static void cpBandFree(CPodeMem cp_mem);
+
+/*
+ * =================================================================
+ * READIBILITY REPLACEMENTS
+ * =================================================================
+ */
+
+#define ode_type (cp_mem->cp_ode_type)
+#define lmm_type (cp_mem->cp_lmm_type)
+#define fe (cp_mem->cp_fe)
+#define fi (cp_mem->cp_fi)
+#define f_data (cp_mem->cp_f_data)
+#define uround (cp_mem->cp_uround)
+#define nst (cp_mem->cp_nst)
+#define tn (cp_mem->cp_tn)
+#define h (cp_mem->cp_h)
+#define gamma (cp_mem->cp_gamma)
+#define gammap (cp_mem->cp_gammap)
+#define gamrat (cp_mem->cp_gamrat)
+#define ewt (cp_mem->cp_ewt)
+#define tempv (cp_mem->cp_tempv)
+
+#define linit (cp_mem->cp_linit)
+#define lsetup (cp_mem->cp_lsetup)
+#define lsolve (cp_mem->cp_lsolve)
+#define lfree (cp_mem->cp_lfree)
+#define lmem (cp_mem->cp_lmem)
+#define lsetup_exists (cp_mem->cp_lsetup_exists)
+
+#define mtype (cpdls_mem->d_type)
+#define n (cpdls_mem->d_n)
+#define jacE (cpdls_mem->d_bjacE)
+#define jacI (cpdls_mem->d_bjacI)
+#define M (cpdls_mem->d_M)
+#define mu (cpdls_mem->d_mu)
+#define ml (cpdls_mem->d_ml)
+#define smu (cpdls_mem->d_smu)
+#define pivots (cpdls_mem->d_pivots)
+#define savedJ (cpdls_mem->d_savedJ)
+#define nstlj (cpdls_mem->d_nstlj)
+#define nje (cpdls_mem->d_nje)
+#define nfeDQ (cpdls_mem->d_nfeDQ)
+#define J_data (cpdls_mem->d_J_data)
+#define last_flag (cpdls_mem->d_last_flag)
+
+/*
+ * =================================================================
+ * EXPORTED FUNCTIONS
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * CPBand
+ * -----------------------------------------------------------------
+ * This routine initializes the memory record and sets various function
+ * fields specific to the band linear solver module. CPBand first calls
+ * the existing lfree routine if this is not NULL. It then sets the
+ * cp_linit, cp_lsetup, cp_lsolve, and cp_lfree fields in (*cpode_mem)
+ * to be CPBandInit, CPBandSetup, CPBandSolve, and CPBandFree,
+ * respectively. It allocates memory for a structure of type
+ * CPDlsMemRec and sets the cp_lmem field in (*cpode_mem) to the
+ * address of this structure. It sets lsetup_exists in (*cpode_mem) to be
+ * TRUE, d_mu to be mupper, d_ml to be mlower, and initializes the d_bjacE
+ * and d_bjacI to NULL.
+ * Finally, it allocates memory for M, savedJ, and pivot. The CPBand
+ * return value is SUCCESS = 0, LMEM_FAIL = -1, or LIN_ILL_INPUT = -2.
+ *
+ * NOTE: The band linear solver assumes a serial implementation
+ * of the NVECTOR package. Therefore, CPBand will first
+ * test for compatible a compatible N_Vector internal
+ * representation by checking that the function
+ * N_VGetArrayPointer exists.
+ * -----------------------------------------------------------------
+ */
+
+int CPBand(void *cpode_mem, int N, int mupper, int mlower)
+{
+ CPodeMem cp_mem;
+ CPDlsMem cpdls_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPDIRECT_MEM_NULL, "CPBAND", "CPBand", MSGD_CPMEM_NULL);
+ return(CPDIRECT_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Test if the NVECTOR package is compatible with the BAND solver */
+ if (tempv->ops->nvgetarraypointer == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_ILL_INPUT, "CPBAND", "CPBand", MSGD_BAD_NVECTOR);
+ return(CPDIRECT_ILL_INPUT);
+ }
+
+ if (lfree != NULL) lfree(cp_mem);
+
+ /* Set four main function fields in cp_mem */
+ linit = cpBandInit;
+ lsetup = cpBandSetup;
+ lsolve = cpBandSolve;
+ lfree = cpBandFree;
+
+ /* Get memory for CPDlsMemRec */
+ cpdls_mem = NULL;
+ cpdls_mem = (CPDlsMem) malloc(sizeof(CPDlsMemRec));
+ if (cpdls_mem == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPBAND", "CPBand", MSGD_MEM_FAIL);
+ return(CPDIRECT_MEM_FAIL);
+ }
+
+ /* Set matrix type */
+ mtype = SUNDIALS_BAND;
+
+ /* Set default Jacobian routine and Jacobian data */
+ jacE = NULL;
+ jacI = NULL;
+ J_data = NULL;
+
+ last_flag = CPDIRECT_SUCCESS;
+ lsetup_exists = TRUE;
+
+ /* Load problem dimension */
+ n = N;
+
+ /* Load half-bandwiths in cpdls_mem */
+ ml = mlower;
+ mu = mupper;
+
+ /* Test ml and mu for legality */
+ if ((ml < 0) || (mu < 0) || (ml >= N) || (mu >= N)) {
+ cpProcessError(cp_mem, CPDIRECT_ILL_INPUT, "CPBAND", "CPBand", MSGD_BAD_SIZES);
+ return(CPDIRECT_ILL_INPUT);
+ }
+
+ /* Set extended upper half-bandwith for M (required for pivoting) */
+ smu = MIN(N-1, mu + ml);
+
+ /* Allocate memory for M, savedJ, and pivot arrays */
+ M = NULL;
+ pivots = NULL;
+ savedJ = NULL;
+
+ M = NewBandMat(N, mu, ml, smu);
+ if (M == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPBAND", "CPBand", MSGD_MEM_FAIL);
+ free(cpdls_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ pivots = NewIntArray(N);
+ if (pivots == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPBAND", "CPBand", MSGD_MEM_FAIL);
+ DestroyMat(M);
+ free(cpdls_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ if (ode_type == CP_EXPL) {
+ savedJ = NewBandMat(N, mu, ml, mu);
+ if (savedJ == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPBAND", "CPBand", MSGD_MEM_FAIL);
+ DestroyMat(M);
+ DestroyArray(pivots);
+ free(cpdls_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ }
+
+ /* Attach linear solver memory to integrator memory */
+ lmem = cpdls_mem;
+
+ return(CPDIRECT_SUCCESS);
+}
+
+/*
+ * =================================================================
+ * PRIVATE FUNCTIONS FOR IMPLICIT INTEGRATION
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * cpBandInit
+ * -----------------------------------------------------------------
+ * This routine does remaining initializations specific to the band
+ * linear solver.
+ * -----------------------------------------------------------------
+ */
+
+static int cpBandInit(CPodeMem cp_mem)
+{
+ CPDlsMem cpdls_mem;
+
+ cpdls_mem = (CPDlsMem) lmem;
+
+ nje = 0;
+ nfeDQ = 0;
+ nstlj = 0;
+
+ if (ode_type == CP_EXPL && jacE == NULL) {
+ jacE = cpDlsBandDQJacExpl;
+ J_data = cp_mem;
+ }
+
+ if (ode_type == CP_IMPL && jacI == NULL) {
+ jacI = cpDlsBandDQJacImpl;
+ J_data = cp_mem;
+ }
+
+ last_flag = CPDIRECT_SUCCESS;
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpBandSetup
+ * -----------------------------------------------------------------
+ * This routine does the setup operations for the band linear solver.
+ * It makes a decision whether or not to call the Jacobian evaluation
+ * routine based on various state variables, and if not it uses the
+ * saved copy (for explicit ODE only). In any case, it constructs
+ * the Newton matrix M = I - gamma*J or M = F_y' - gamma*F_y, updates
+ * counters, and calls the band LU factorization routine.
+ * -----------------------------------------------------------------
+ */
+
+static int cpBandSetup(CPodeMem cp_mem, int convfail,
+ N_Vector yP, N_Vector ypP, N_Vector fctP,
+ booleantype *jcurPtr,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ CPDlsMem cpdls_mem;
+ booleantype jbad, jok;
+ realtype dgamma;
+ int ier, retval;
+
+ cpdls_mem = (CPDlsMem) lmem;
+
+ switch (ode_type) {
+
+ case CP_EXPL:
+
+ /* Use nst, gamma/gammap, and convfail to set J eval. flag jok */
+ dgamma = ABS((gamma/gammap) - ONE);
+ jbad = (nst == 0) || (nst > nstlj + CPD_MSBJ) ||
+ ((convfail == CP_FAIL_BAD_J) && (dgamma < CPD_DGMAX)) ||
+ (convfail == CP_FAIL_OTHER);
+ jok = !jbad;
+
+ if (jok) {
+
+ /* If jok = TRUE, use saved copy of J */
+ *jcurPtr = FALSE;
+ BandCopy(savedJ, M, mu, ml);
+
+ } else {
+
+ /* If jok = FALSE, call jac routine for new J value */
+ nje++;
+ nstlj = nst;
+ *jcurPtr = TRUE;
+
+ BandZero(M);
+ retval = jacE(n, mu, ml, tn, yP, fctP, M, J_data, tmp1, tmp2, tmp3);
+ if (retval == 0) {
+ BandCopy(M, savedJ, mu, ml);
+ }else if (retval < 0) {
+ cpProcessError(cp_mem, CPDIRECT_JACFUNC_UNRECVR, "CPBAND", "CPBandSetup", MSGD_JACFUNC_FAILED);
+ last_flag = CPDIRECT_JACFUNC_UNRECVR;
+ return(-1);
+ }else if (retval > 0) {
+ last_flag = CPDIRECT_JACFUNC_RECVR;
+ return(1);
+ }
+
+ }
+
+ /* Scale and add I to get M = I - gamma*J */
+ BandScale(-gamma, M);
+ BandAddI(M);
+
+ break;
+
+ case CP_IMPL:
+
+ BandZero(M);
+ retval = jacI(n, mu, ml, tn, gamma, yP, ypP, fctP, M, J_data, tmp1, tmp2, tmp3);
+ if (retval < 0) {
+ cpProcessError(cp_mem, CPDIRECT_JACFUNC_UNRECVR, "CPBAND", "CPBandSetup", MSGD_JACFUNC_FAILED);
+ last_flag = CPDIRECT_JACFUNC_UNRECVR;
+ return(-1);
+ } else if (retval > 0) {
+ last_flag = CPDIRECT_JACFUNC_RECVR;
+ return(+1);
+ }
+
+ break;
+
+ }
+
+ /* Do LU factorization of M */
+ ier = BandGBTRF(M, pivots);
+
+ /* Return 0 if the LU was complete; otherwise return 1 */
+ last_flag = ier;
+ if (ier>0) return(1);
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpBandSolve
+ * -----------------------------------------------------------------
+ * This routine handles the solve operation for the band linear solver
+ * by calling the band backsolve routine. The return value is 0.
+ * -----------------------------------------------------------------
+ */
+
+static int cpBandSolve(CPodeMem cp_mem, N_Vector b, N_Vector weight,
+ N_Vector yC, N_Vector ypC, N_Vector fctC)
+{
+ CPDlsMem cpdls_mem;
+ realtype *bd;
+
+ cpdls_mem = (CPDlsMem) lmem;
+
+ bd = N_VGetArrayPointer(b);
+
+ BandGBTRS(M, pivots, bd);
+
+ /* For BDF, scale the correction to account for change in gamma */
+ if ((lmm_type == CP_BDF) && (gamrat != ONE)) {
+ N_VScale(TWO/(ONE + gamrat), b, b);
+ }
+
+ last_flag = CPDIRECT_SUCCESS;
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpBandFree
+ * -----------------------------------------------------------------
+ * This routine frees memory specific to the band linear solver.
+ * -----------------------------------------------------------------
+ */
+
+static void cpBandFree(CPodeMem cp_mem)
+{
+ CPDlsMem cpdls_mem;
+
+ cpdls_mem = (CPDlsMem) lmem;
+
+ DestroyMat(M);
+ DestroyArray(pivots);
+ if (ode_type == CP_EXPL) DestroyMat(savedJ);
+ free(cpdls_mem);
+ cpdls_mem = NULL;
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_bandpre.c b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_bandpre.c
new file mode 100644
index 0000000..029dc46
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_bandpre.c
@@ -0,0 +1,764 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.4 $
+ * $Date: 2007/01/29 17:35:23 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This file contains implementations of the banded difference
+ * quotient Jacobian-based preconditioner and solver routines for
+ * use with the CPSPILS linear solvers.
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * =================================================================
+ * IMPORTED HEADER FILES
+ * =================================================================
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <cpodes/cpodes_sptfqmr.h>
+#include <cpodes/cpodes_spbcgs.h>
+#include <cpodes/cpodes_spgmr.h>
+
+#include <sundials/sundials_math.h>
+
+#include "cpodes_bandpre_impl.h"
+#include "cpodes_private.h"
+
+/*
+ * =================================================================
+ * FUNCTION SPECIFIC CONSTANTS
+ * =================================================================
+ */
+
+#define MIN_INC_MULT RCONST(1000.0)
+
+/*
+ * =================================================================
+ * PROTOTYPES FOR PRIVATE FUNCTIONS
+ * =================================================================
+ */
+
+/* cpBandPrecSetupExpl and cpBandPrecSetupImpl */
+
+static int cpBandPrecSetupExpl(realtype t, N_Vector y, N_Vector fy,
+ booleantype jok, booleantype *jcurPtr,
+ realtype gamma, void *bp_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+
+static int cpBandPrecSetupImpl(realtype t, N_Vector y, N_Vector yp, N_Vector r,
+ realtype gamma, void *bp_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+
+/* cpBandPrecSolveExpl and cpBandPrecSolveImpl */
+
+static int cpBandPrecSolveExpl(realtype t, N_Vector y, N_Vector fy,
+ N_Vector b, N_Vector x,
+ realtype gamma, realtype delta,
+ int lr, void *bp_data, N_Vector tmp);
+
+static int cpBandPrecSolveImpl(realtype t, N_Vector y, N_Vector yp, N_Vector r,
+ N_Vector b, N_Vector x,
+ realtype gamma, realtype delta,
+ void *bp_data, N_Vector tmp);
+
+/* Difference quotient Jacobian calculation routines */
+
+static int cpBandPDQJacExpl(CPBandPrecData pdata,
+ realtype t, N_Vector y, N_Vector fy,
+ N_Vector ftemp, N_Vector ytemp);
+
+static int cpBandPDQJacImpl(CPBandPrecData pdata,
+ realtype t, realtype gamma,
+ N_Vector y, N_Vector yp, N_Vector r,
+ N_Vector ftemp, N_Vector ytemp, N_Vector yptemp);
+
+/* Redability replacements */
+
+#define ode_type (cp_mem->cp_ode_type)
+#define vec_tmpl (cp_mem->cp_tempv)
+
+/*
+ * =================================================================
+ * EXPORTED FUNCTIONS
+ * =================================================================
+ */
+
+void *CPBandPrecAlloc(void *cpode_mem, int N, int mu, int ml)
+{
+ CPodeMem cp_mem;
+ CPBandPrecData pdata;
+ int mup, mlp, storagemu;
+
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, 0, "CPBANDPRE", "CPBandPrecAlloc", MSGBP_CPMEM_NULL);
+ return(NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Test if the NVECTOR package is compatible with the BAND preconditioner */
+ if(vec_tmpl->ops->nvgetarraypointer == NULL) {
+ cpProcessError(cp_mem, 0, "CPBANDPRE", "CPBandPrecAlloc", MSGBP_BAD_NVECTOR);
+ return(NULL);
+ }
+
+ pdata = NULL;
+ pdata = (CPBandPrecData) malloc(sizeof *pdata); /* Allocate data memory */
+ if (pdata == NULL) {
+ cpProcessError(cp_mem, 0, "CPBANDPRE", "CPBandPrecAlloc", MSGBP_MEM_FAIL);
+ return(NULL);
+ }
+
+ /* Load pointers and bandwidths into pdata block. */
+ pdata->cpode_mem = cpode_mem;
+ pdata->N = N;
+ pdata->mu = mup = MIN(N-1, MAX(0,mu));
+ pdata->ml = mlp = MIN(N-1, MAX(0,ml));
+
+ /* Initialize nfeBP counter */
+ pdata->nfeBP = 0;
+
+ /* Allocate memory for saved banded Jacobian approximation. */
+ pdata->savedJ = NULL;
+ pdata->savedJ = NewBandMat(N, mup, mlp, mup);
+ if (pdata->savedJ == NULL) {
+ free(pdata); pdata = NULL;
+ cpProcessError(cp_mem, 0, "CPBANDPRE", "CPBandPrecAlloc", MSGBP_MEM_FAIL);
+ return(NULL);
+ }
+
+ /* Allocate memory for banded preconditioner. */
+ storagemu = MIN(N-1, mup+mlp);
+ pdata->savedP = NULL;
+ pdata->savedP = NewBandMat(N, mup, mlp, storagemu);
+ if (pdata->savedP == NULL) {
+ DestroyMat(pdata->savedJ);
+ free(pdata); pdata = NULL;
+ cpProcessError(cp_mem, 0, "CPBANDPRE", "CPBandPrecAlloc", MSGBP_MEM_FAIL);
+ return(NULL);
+ }
+
+ /* Allocate memory for pivot array. */
+ pdata->pivots = NULL;
+ pdata->pivots = NewIntArray(N);
+ if (pdata->savedJ == NULL) {
+ DestroyMat(pdata->savedP);
+ DestroyMat(pdata->savedJ);
+ free(pdata); pdata = NULL;
+ cpProcessError(cp_mem, 0, "CPBANDPRE", "CPBandPrecAlloc", MSGBP_MEM_FAIL);
+ return(NULL);
+ }
+
+ return((void *) pdata);
+}
+
+int CPBPSptfqmr(void *cpode_mem, int pretype, int maxl, void *p_data)
+{
+ CPodeMem cp_mem;
+ int flag;
+
+ flag = CPSptfqmr(cpode_mem, pretype, maxl);
+ if(flag != CPSPILS_SUCCESS) return(flag);
+
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if ( p_data == NULL ) {
+ cpProcessError(cp_mem, CPBANDPRE_PDATA_NULL, "CPBANDPRE", "CPBPSptfqmr", MSGBP_PDATA_NULL);
+ return(CPBANDPRE_PDATA_NULL);
+ }
+
+ switch (ode_type) {
+ case CP_EXPL:
+ flag = CPSpilsSetPreconditioner(cpode_mem, (void *)cpBandPrecSetupExpl, (void *)cpBandPrecSolveExpl, p_data);
+ break;
+ case CP_IMPL:
+ flag = CPSpilsSetPreconditioner(cpode_mem, (void *)cpBandPrecSetupImpl, (void *)cpBandPrecSolveImpl, p_data);
+ break;
+ }
+ if(flag != CPSPILS_SUCCESS) return(flag);
+
+ return(CPSPILS_SUCCESS);
+}
+
+int CPBPSpbcg(void *cpode_mem, int pretype, int maxl, void *p_data)
+{
+ CPodeMem cp_mem;
+ int flag;
+
+ flag = CPSpbcg(cpode_mem, pretype, maxl);
+ if(flag != CPSPILS_SUCCESS) return(flag);
+
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if ( p_data == NULL ) {
+ cpProcessError(cp_mem, CPBANDPRE_PDATA_NULL, "CPBANDPRE", "CPBPSpbcg", MSGBP_PDATA_NULL);
+ return(CPBANDPRE_PDATA_NULL);
+ }
+
+ switch (ode_type) {
+ case CP_EXPL:
+ flag = CPSpilsSetPreconditioner(cpode_mem, (void *)cpBandPrecSetupExpl, (void *)cpBandPrecSolveExpl, p_data);
+ break;
+ case CP_IMPL:
+ flag = CPSpilsSetPreconditioner(cpode_mem, (void *)cpBandPrecSetupImpl, (void *)cpBandPrecSolveImpl, p_data);
+ break;
+ }
+ if(flag != CPSPILS_SUCCESS) return(flag);
+
+ return(CPSPILS_SUCCESS);
+}
+
+int CPBPSpgmr(void *cpode_mem, int pretype, int maxl, void *p_data)
+{
+ CPodeMem cp_mem;
+ int flag;
+
+ flag = CPSpgmr(cpode_mem, pretype, maxl);
+ if(flag != CPSPILS_SUCCESS) return(flag);
+
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if ( p_data == NULL ) {
+ cpProcessError(cp_mem, CPBANDPRE_PDATA_NULL, "CPBANDPRE", "CPBPSpgmr", MSGBP_PDATA_NULL);
+ return(CPBANDPRE_PDATA_NULL);
+ }
+
+ switch (ode_type) {
+ case CP_EXPL:
+ flag = CPSpilsSetPreconditioner(cpode_mem, (void *)cpBandPrecSetupExpl, (void *)cpBandPrecSolveExpl, p_data);
+ break;
+ case CP_IMPL:
+ flag = CPSpilsSetPreconditioner(cpode_mem, (void *)cpBandPrecSetupImpl, (void *)cpBandPrecSolveImpl, p_data);
+ break;
+ }
+ if(flag != CPSPILS_SUCCESS) return(flag);
+
+ return(CPSPILS_SUCCESS);
+}
+
+void CPBandPrecFree(void **bp_data)
+{
+ CPBandPrecData pdata;
+
+ if (*bp_data == NULL) return;
+
+ pdata = (CPBandPrecData) (*bp_data);
+ DestroyMat(pdata->savedJ);
+ DestroyMat(pdata->savedP);
+ DestroyArray(pdata->pivots);
+
+ free(*bp_data);
+ *bp_data = NULL;
+
+}
+
+int CPBandPrecGetWorkSpace(void *bp_data, long int *lenrwBP, long int *leniwBP)
+{
+ CPBandPrecData pdata;
+ int N, ml, mu, smu;
+
+ if ( bp_data == NULL ) {
+ cpProcessError(NULL, CPBANDPRE_PDATA_NULL, "CPBANDPRE", "CPBandPrecGetWorkSpace", MSGBP_PDATA_NULL);
+ return(CPBANDPRE_PDATA_NULL);
+ }
+
+ pdata = (CPBandPrecData) bp_data;
+
+ N = pdata->N;
+ mu = pdata->mu;
+ ml = pdata->ml;
+ smu = MIN( N-1, mu + ml);
+
+ *leniwBP = pdata->N;
+ *lenrwBP = N * ( 2*ml + smu + mu + 2 );
+
+ return(CPBANDPRE_SUCCESS);
+}
+
+int CPBandPrecGetNumFctEvals(void *bp_data, long int *nfevalsBP)
+{
+ CPBandPrecData pdata;
+
+ if (bp_data == NULL) {
+ cpProcessError(NULL, CPBANDPRE_PDATA_NULL, "CPBANDPRE", "CPBandPrecGetNumFctEvals", MSGBP_PDATA_NULL);
+ return(CPBANDPRE_PDATA_NULL);
+ }
+
+ pdata = (CPBandPrecData) bp_data;
+
+ *nfevalsBP = pdata->nfeBP;
+
+ return(CPBANDPRE_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * CPBandPrecGetReturnFlagName
+ * -----------------------------------------------------------------
+ */
+
+char *CPBandPrecGetReturnFlagName(int flag)
+{
+ char *name;
+
+ name = (char *)malloc(30*sizeof(char));
+
+ switch(flag) {
+ case CPBANDPRE_SUCCESS:
+ sprintf(name,"CPBANDPRE_SUCCESS");
+ break;
+ case CPBANDPRE_PDATA_NULL:
+ sprintf(name,"CPBANDPRE_PDATA_NULL");
+ break;
+ case CPBANDPRE_FUNC_UNRECVR:
+ sprintf(name,"CPBANDPRE_FUNC_UNRECVR");
+ break;
+ default:
+ sprintf(name,"NONE");
+ }
+
+ return(name);
+}
+
+/*
+ * =================================================================
+ * PRIVATE FUNCTIONS
+ * =================================================================
+ */
+
+/* Readability Replacements */
+
+#define N (pdata->N)
+#define mu (pdata->mu)
+#define ml (pdata->ml)
+#define pivots (pdata->pivots)
+#define savedJ (pdata->savedJ)
+#define savedP (pdata->savedP)
+#define nfeBP (pdata->nfeBP)
+
+/*
+ * -----------------------------------------------------------------
+ * cpBandPrecSetupExpl
+ * -----------------------------------------------------------------
+ * Together cpBandPrecSetupExpl and cpBandPrecSolveExpl use a banded
+ * difference quotient Jacobian to create a preconditioner.
+ * cpBandPrecSetupExpl calculates a new J, if necessary, then
+ * calculates P = I - gamma*J, and does an LU factorization of P.
+ *
+ * The parameters of cpBandPrecSetupExpl are as follows:
+ *
+ * t is the current value of the independent variable.
+ *
+ * y is the current value of the dependent variable vector,
+ * namely the predicted value of y(t).
+ *
+ * fy is the vector f(t,y).
+ *
+ * jok is an input flag indicating whether Jacobian-related
+ * data needs to be recomputed, as follows:
+ * jok == FALSE means recompute Jacobian-related data
+ * from scratch.
+ * jok == TRUE means that Jacobian data from the
+ * previous cpBandPrecSetupExpl call will be reused
+ * (with the current value of gamma).
+ * A cpBandPrecSetupExpl call with jok == TRUE should only
+ * occur after a call with jok == FALSE.
+ *
+ * *jcurPtr is a pointer to an output integer flag which is
+ * set by cpBandPrecsetupExpl as follows:
+ * *jcurPtr = TRUE if Jacobian data was recomputed.
+ * *jcurPtr = FALSE if Jacobian data was not recomputed,
+ * but saved data was reused.
+ *
+ * gamma is the scalar appearing in the Newton matrix.
+ *
+ * bp_data is a pointer to preconditoner data - the same as the
+ * bp_data parameter passed to CPSp*.
+ *
+ * tmp1, tmp2, and tmp3 are pointers to memory allocated
+ * for vectors of length N for work space. This
+ * routine uses only tmp1 and tmp2.
+ *
+ * The value to be returned by the cpBandPrecSetupExpl function is
+ * 0 if successful, or
+ * 1 if the band factorization failed.
+ * -----------------------------------------------------------------
+ */
+
+static int cpBandPrecSetupExpl(realtype t, N_Vector y, N_Vector fy,
+ booleantype jok, booleantype *jcurPtr,
+ realtype gamma, void *bp_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ CPBandPrecData pdata;
+ CPodeMem cp_mem;
+ int ier, retval;
+
+ /* Assume matrix and pivots have already been allocated. */
+ pdata = (CPBandPrecData) bp_data;
+
+ cp_mem = (CPodeMem) pdata->cpode_mem;
+
+ if (jok) {
+
+ /* If jok = TRUE, use saved copy of J. */
+ *jcurPtr = FALSE;
+ BandCopy(savedJ, savedP, mu, ml);
+
+ } else {
+
+ /* If jok = FALSE, call cpBandPDQJac for new J value. */
+ *jcurPtr = TRUE;
+ BandZero(savedJ);
+
+ retval = cpBandPDQJacExpl(pdata, t, y, fy, tmp1, tmp2);
+ if (retval < 0) {
+ cpProcessError(cp_mem, CPBANDPRE_FUNC_UNRECVR, "CPBANDPRE", "cpBandPrecSetupExpl", MSGBP_FUNC_FAILED);
+ return(-1);
+ }
+ if (retval > 0) {
+ return(1);
+ }
+
+ BandCopy(savedJ, savedP, mu, ml);
+
+ }
+
+ /* Scale and add I to get savedP = I - gamma*J. */
+ BandScale(-gamma, savedP);
+ BandAddI(savedP);
+
+ /* Do LU factorization of matrix. */
+ ier = BandGBTRF(savedP, pivots);
+
+ /* Return 0 if the LU was complete; otherwise return 1. */
+ if (ier > 0) return(1);
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpBandPrecSolveExpl
+ * -----------------------------------------------------------------
+ * cpBandPrecSolveExpl solves a linear system P x = b, where P is the
+ * matrix computed by cpBandPrecSetupExpl.
+ *
+ * The parameters of cpBandPrecSolveExpl used here are as follows:
+ *
+ * b is the right-hand side vector of the linear system.
+ *
+ * bp_data is a pointer to preconditioner data - the same as the
+ * bp_data parameter passed to CPSp*.
+ *
+ * x is the output vector computed by cpBandPrecSolveExpl.
+ *
+ * The value returned by the cpBandPrecSolveExpl function is always 0,
+ * indicating success.
+ * -----------------------------------------------------------------
+ */
+
+static int cpBandPrecSolveExpl(realtype t, N_Vector y, N_Vector fy,
+ N_Vector b, N_Vector x,
+ realtype gamma, realtype delta,
+ int lr, void *bp_data, N_Vector tmp)
+{
+ CPBandPrecData pdata;
+ realtype *xd;
+
+ /* Assume matrix and pivots have already been allocated. */
+ pdata = (CPBandPrecData) bp_data;
+
+ /* Copy b to x. */
+ N_VScale(ONE, b, x);
+
+ /* Do band backsolve on the vector z. */
+ xd = N_VGetArrayPointer(x);
+
+ BandGBTRS(savedP, pivots, xd);
+
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpBandPrecSetupImpl
+ * -----------------------------------------------------------------
+ * Together cpBandPrecSetupImpl and cpBandPrecSolveImpl use a banded
+ * difference quotient Jacobian to create a preconditioner.
+ * cpBandPrecSetupImpl calculates a new J = dF/dy + gamma*dF/dy'
+ * and does an LU factorization of P.
+ * -----------------------------------------------------------------
+ */
+
+static int cpBandPrecSetupImpl(realtype t, N_Vector y, N_Vector yp, N_Vector r,
+ realtype gamma, void *bp_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ CPBandPrecData pdata;
+ CPodeMem cp_mem;
+ int ier, retval;
+
+ /* Assume matrix and pivots have already been allocated. */
+ pdata = (CPBandPrecData) bp_data;
+
+ cp_mem = (CPodeMem) pdata->cpode_mem;
+
+ BandZero(savedJ);
+ retval = cpBandPDQJacImpl(pdata, t, gamma, y, yp, r, tmp1, tmp2, tmp3);
+ if (retval < 0) {
+ cpProcessError(cp_mem, CPBANDPRE_FUNC_UNRECVR, "CPBANDPRE", "cpBandPrecSetupImpl", MSGBP_FUNC_FAILED);
+ return(-1);
+ }
+ if (retval > 0) {
+ return(1);
+ }
+
+ /* Do LU factorization of matrix. */
+ ier = BandGBTRF(savedP, pivots);
+
+ /* Return 0 if the LU was complete; otherwise return 1. */
+ if (ier > 0) return(1);
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpBandPrecSolveImpl
+ * -----------------------------------------------------------------
+ * cpBandPrecSolveImpl solves a linear system P x = b, where P is the
+ * matrix computed by cpBandPrecSetupImpl.
+ *
+ * The parameters of cpBandPrecSolveImpl used here are as follows:
+ *
+ * b is the right-hand side vector of the linear system.
+ *
+ * bp_data is a pointer to preconditioner data - the same as the
+ * bp_data parameter passed to CPSp*.
+ *
+ * x is the output vector computed by cpBandPrecSolveImpl.
+ *
+ * The value returned by the cpBandPrecSolveImpl function is always 0,
+ * indicating success.
+ * -----------------------------------------------------------------
+ */
+
+static int cpBandPrecSolveImpl(realtype t, N_Vector y, N_Vector yp, N_Vector r,
+ N_Vector b, N_Vector x,
+ realtype gamma, realtype delta,
+ void *bp_data, N_Vector tmp)
+{
+ CPBandPrecData pdata;
+ realtype *xd;
+
+ /* Assume matrix and pivots have already been allocated. */
+ pdata = (CPBandPrecData) bp_data;
+
+ /* Copy b to x. */
+ N_VScale(ONE, b, x);
+
+ /* Do band backsolve on the vector z. */
+ xd = N_VGetArrayPointer(x);
+
+ BandGBTRS(savedP, pivots, xd);
+
+ return(0);
+}
+
+/*
+ * =================================================================
+ * DQ LOCAL JACOBIAN APROXIMATIONS
+ * =================================================================
+ */
+
+#define ewt (cp_mem->cp_ewt)
+#define uround (cp_mem->cp_uround)
+#define h (cp_mem->cp_h)
+#define fe (cp_mem->cp_fe)
+#define fi (cp_mem->cp_fi)
+#define f_data (cp_mem->cp_f_data)
+
+/*
+ * -----------------------------------------------------------------
+ * cpBandPDQJacExpl
+ * -----------------------------------------------------------------
+ * This routine generates a banded difference quotient approximation to
+ * the Jacobian of f(t,y). It assumes that a band matrix of type
+ * BandMat is stored column-wise, and that elements within each column
+ * are contiguous. This makes it possible to get the address of a column
+ * of J via the macro BAND_COL and to write a simple for loop to set
+ * each of the elements of a column in succession.
+ * -----------------------------------------------------------------
+ */
+
+static int cpBandPDQJacExpl(CPBandPrecData pdata,
+ realtype t, N_Vector y, N_Vector fy,
+ N_Vector ftemp, N_Vector ytemp)
+{
+ CPodeMem cp_mem;
+ realtype fnorm, minInc, inc, inc_inv, srur;
+ int group, i, j, width, ngroups, i1, i2;
+ realtype *col_j, *ewt_data, *fy_data, *ftemp_data, *y_data, *ytemp_data;
+ int retval;
+
+ cp_mem = (CPodeMem) pdata->cpode_mem;
+
+ /* Obtain pointers to the data for ewt, fy, ftemp, y, ytemp. */
+ ewt_data = N_VGetArrayPointer(ewt);
+ fy_data = N_VGetArrayPointer(fy);
+ ftemp_data = N_VGetArrayPointer(ftemp);
+ y_data = N_VGetArrayPointer(y);
+ ytemp_data = N_VGetArrayPointer(ytemp);
+
+ /* Load ytemp with y = predicted y vector. */
+ N_VScale(ONE, y, ytemp);
+
+ /* Set minimum increment based on uround and norm of f. */
+ srur = RSqrt(uround);
+ fnorm = N_VWrmsNorm(fy, ewt);
+ minInc = (fnorm != ZERO) ?
+ (MIN_INC_MULT * ABS(h) * uround * N * fnorm) : ONE;
+
+ /* Set bandwidth and number of column groups for band differencing. */
+ width = ml + mu + 1;
+ ngroups = MIN(width, N);
+
+ /* Loop over column groups. */
+ for (group = 1; group <= ngroups; group++) {
+
+ /* Increment all y_j in group. */
+ for(j = group-1; j < N; j += width) {
+ inc = MAX(srur*ABS(y_data[j]), minInc/ewt_data[j]);
+ ytemp_data[j] += inc;
+ }
+
+ /* Evaluate f with incremented y. */
+
+ retval = fe(t, ytemp, ftemp, f_data);
+ nfeBP++;
+ if (retval != 0) return(retval);
+
+ /* Restore ytemp, then form and load difference quotients. */
+ for (j = group-1; j < N; j += width) {
+ ytemp_data[j] = y_data[j];
+ col_j = BAND_COL(savedJ,j);
+ inc = MAX(srur*ABS(y_data[j]), minInc/ewt_data[j]);
+ inc_inv = ONE/inc;
+ i1 = MAX(0, j-mu);
+ i2 = MIN(j+ml, N-1);
+ for (i=i1; i <= i2; i++)
+ BAND_COL_ELEM(col_j,i,j) = inc_inv * (ftemp_data[i] - fy_data[i]);
+ }
+ }
+
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpBandPDQJacImpl
+ * -----------------------------------------------------------------
+ * This routine generates a banded difference quotient approximation
+ * to the Jacobian dF/dy + gamma*dF/dy' and loads it directly into
+ * savedP. It assumes that a band matrix of type BandMat is stored
+ * column-wise, and that elements within each column are contiguous.
+ * This makes it possible to get the address of a column of J via
+ * the macro BAND_COL and to write a simple for loop to set each of
+ * the elements of a column in succession.
+ * -----------------------------------------------------------------
+ */
+
+static int cpBandPDQJacImpl(CPBandPrecData pdata,
+ realtype t, realtype gamma,
+ N_Vector y, N_Vector yp, N_Vector r,
+ N_Vector ftemp, N_Vector ytemp, N_Vector yptemp)
+
+{
+ CPodeMem cp_mem;
+ realtype inc, inc_inv, yj, ypj, srur, ewtj;
+ realtype *y_data, *yp_data, *ewt_data;
+ realtype *ytemp_data, *yptemp_data, *ftemp_data, *r_data, *col_j;
+ int i, j, i1, i2, width, group, ngroups;
+ int retval = 0;
+
+ cp_mem = (CPodeMem) pdata->cpode_mem;
+
+ /* Obtain pointers to the data for all vectors used. */
+ ewt_data = N_VGetArrayPointer(ewt);
+ r_data = N_VGetArrayPointer(r);
+ y_data = N_VGetArrayPointer(y);
+ yp_data = N_VGetArrayPointer(yp);
+ ftemp_data = N_VGetArrayPointer(ftemp);
+ ytemp_data = N_VGetArrayPointer(ytemp);
+ yptemp_data = N_VGetArrayPointer(yptemp);
+
+ /* Initialize ytemp and yptemp. */
+ N_VScale(ONE, y, ytemp);
+ N_VScale(ONE, yp, yptemp);
+
+ /* Compute miscellaneous values for the Jacobian computation. */
+ srur = RSqrt(uround);
+ width = ml + mu + 1;
+ ngroups = MIN(width, N);
+
+ /* Loop over column groups. */
+ for (group=1; group <= ngroups; group++) {
+
+ /* Increment all y[j] and yp[j] for j in this group. */
+ for (j=group-1; j<N; j+=width) {
+ yj = y_data[j];
+ ypj = yp_data[j];
+ ewtj = ewt_data[j];
+
+ /* Set increment inc to yj based on sqrt(uround)*abs(yj), with
+ adjustments using ypj and ewtj if this is small, and a further
+ adjustment to give it the same sign as h*ypj. */
+ inc = MAX( srur * MAX( ABS(yj), ABS(h*ypj) ) , ONE/ewtj );
+
+ if (h*ypj < ZERO) inc = -inc;
+ inc = (yj + inc) - yj;
+
+ /* Increment yj and ypj. */
+ ytemp_data[j] += gamma*inc;
+ yptemp_data[j] += inc;
+ }
+
+ /* Call ODE fct. with incremented arguments. */
+ retval = fi(t, ytemp, yptemp, ftemp, f_data);
+ nfeBP++;
+ if (retval != 0) break;
+
+ /* Loop over the indices j in this group again. */
+ for (j=group-1; j<N; j+=width) {
+
+ /* Reset ytemp and yptemp components that were perturbed. */
+ yj = ytemp_data[j] = y_data[j];
+ ypj = yptemp_data[j] = yp_data[j];
+ col_j = BAND_COL(savedP, j);
+ ewtj = ewt_data[j];
+
+ /* Set increment inc exactly as above. */
+ inc = MAX( srur * MAX( ABS(yj), ABS(h*ypj) ) , ONE/ewtj );
+ if (h*ypj < ZERO) inc = -inc;
+ inc = (yj + inc) - yj;
+
+ /* Load the difference quotient Jacobian elements for column j. */
+ inc_inv = ONE/inc;
+ i1 = MAX(0, j-mu);
+ i2 = MIN(j+ml,N-1);
+ for (i=i1; i<=i2; i++)
+ BAND_COL_ELEM(col_j,i,j) = inc_inv*(ftemp_data[i]-r_data[i]);
+ }
+
+ }
+
+ return(retval);
+}
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_bandpre_impl.h b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_bandpre_impl.h
new file mode 100644
index 0000000..e201a5c
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_bandpre_impl.h
@@ -0,0 +1,69 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/22 00:12:48 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * Implementation header file for the CPBANDPRE module.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _CPBANDPRE_IMPL_H
+#define _CPBANDPRE_IMPL_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <cpodes/cpodes_bandpre.h>
+#include <sundials/sundials_band.h>
+
+/*
+ * -----------------------------------------------------------------
+ * Type: CPBandPrecData
+ * -----------------------------------------------------------------
+ */
+
+typedef struct {
+
+ /* Data set by user in CPBandPrecAlloc */
+ int N;
+ int ml, mu;
+
+ /* Data set by CPBandPrecSetup */
+ DlsMat savedJ;
+ DlsMat savedP;
+ int *pivots;
+
+ /* Function evaluations for DQ Jacobian approximation */
+ long int nfeBP;
+
+ /* Pointer to cpode_mem */
+ void *cpode_mem;
+
+} *CPBandPrecData;
+
+/*
+ * -----------------------------------------------------------------
+ * CPBANDPRE error messages
+ * -----------------------------------------------------------------
+ */
+
+#define MSGBP_CPMEM_NULL "Integrator memory is NULL."
+#define MSGBP_MEM_FAIL "A memory request failed."
+#define MSGBP_BAD_NVECTOR "A required vector operation is not implemented."
+#define MSGBP_PDATA_NULL "BandPrecData is NULL."
+#define MSGBP_FUNC_FAILED "The ODE function failed in an unrecoverable manner."
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_bbdpre.c b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_bbdpre.c
new file mode 100644
index 0000000..36c6741
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_bbdpre.c
@@ -0,0 +1,902 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.3 $
+ * $Date: 2006/11/24 19:09:18 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This file contains implementations of routines for a
+ * band-block-diagonal preconditioner, i.e. a block-diagonal
+ * matrix with banded blocks, for use with CPODES, a CPSPILS linear
+ * solver, and the parallel implementation of NVECTOR.
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * =================================================================
+ * IMPORTED HEADER FILES
+ * =================================================================
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <cpodes/cpodes_sptfqmr.h>
+#include <cpodes/cpodes_spbcgs.h>
+#include <cpodes/cpodes_spgmr.h>
+
+#include "cpodes_bbdpre_impl.h"
+#include "cpodes_private.h"
+
+#include <sundials/sundials_math.h>
+
+/*
+ * =================================================================
+ * FUNCTION SPECIFIC CONSTANTS
+ * =================================================================
+ */
+
+#define MIN_INC_MULT RCONST(1000.0)
+
+/*
+ * =================================================================
+ * PROTOTYPES FOR PRIVATE FUNCTIONS
+ * =================================================================
+ */
+
+/* cpBBDPrecSetupExpl and cpBBDPrecSetupImpl */
+
+static int cpBBDPrecSetupExpl(realtype t, N_Vector y, N_Vector fy,
+ booleantype jok, booleantype *jcurPtr,
+ realtype gamma, void *bbd_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+
+static int cpBBDPrecSetupImpl(realtype t, N_Vector y, N_Vector yp, N_Vector r,
+ realtype gamma, void *bbd_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+
+/* cpBBDPrecSolveExpl and cpBBDPrecSolveImpl */
+
+static int cpBBDPrecSolveExpl(realtype t, N_Vector y, N_Vector fy,
+ N_Vector b, N_Vector x,
+ realtype gamma, realtype delta,
+ int lr, void *bbd_data, N_Vector tmp);
+
+static int cpBBDPrecSolveImpl(realtype t, N_Vector y, N_Vector yp, N_Vector r,
+ N_Vector b, N_Vector x,
+ realtype gamma, realtype delta,
+ void *bbd_data, N_Vector tmp);
+
+/* Difference quotient Jacobian calculation routines */
+
+static int cpBBDDQJacExpl(CPBBDPrecData pdata, realtype t,
+ N_Vector y, N_Vector gy,
+ N_Vector ytemp, N_Vector gtemp);
+
+static int cpBBDDQJacImpl(CPBBDPrecData pdata, realtype tt, realtype gamma,
+ N_Vector yy, N_Vector yp, N_Vector gref,
+ N_Vector ytemp, N_Vector yptemp, N_Vector gtemp);
+
+/* Redability replacements */
+
+#define ode_type (cp_mem->cp_ode_type)
+#define uround (cp_mem->cp_uround)
+#define vec_tmpl (cp_mem->cp_tempv)
+
+/*
+ * =================================================================
+ * EXPORTED FUNCTIONS
+ * =================================================================
+ */
+
+void *CPBBDPrecAlloc(void *cpode_mem, int Nlocal,
+ int mudq, int mldq, int mukeep, int mlkeep,
+ realtype dqrely,
+ void *gloc, CPBBDCommFn cfn)
+{
+ CPodeMem cp_mem;
+ CPBBDPrecData pdata;
+ N_Vector tmp4;
+ int muk, mlk, storage_mu;
+
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, 0, "CPBBDPRE", "CPBBDPrecAlloc", MSGBBDP_CPMEM_NULL);
+ return(NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Test if the NVECTOR package is compatible with the BLOCK BAND preconditioner */
+ if(vec_tmpl->ops->nvgetarraypointer == NULL) {
+ cpProcessError(cp_mem, 0, "CPBBDPRE", "CPBBDPrecAlloc", MSGBBDP_BAD_NVECTOR);
+ return(NULL);
+ }
+
+ /* Allocate data memory */
+ pdata = NULL;
+ pdata = (CPBBDPrecData) malloc(sizeof *pdata);
+ if (pdata == NULL) {
+ cpProcessError(cp_mem, 0, "CPBBDPRE", "CPBBDPrecAlloc", MSGBBDP_MEM_FAIL);
+ return(NULL);
+ }
+
+ /* Set pointers to gloc and cfn; load half-bandwidths */
+
+ pdata->cpode_mem = cpode_mem;
+
+ switch (ode_type) {
+ case CP_EXPL:
+ pdata->glocE = (CPBBDLocalRhsFn) gloc;
+ pdata->glocI = NULL;
+ break;
+ case CP_IMPL:
+ pdata->glocI = (CPBBDLocalResFn) gloc;
+ pdata->glocE = NULL;
+ break;
+ }
+
+ pdata->cfn = cfn;
+
+ pdata->mudq = MIN(Nlocal-1, MAX(0,mudq));
+ pdata->mldq = MIN(Nlocal-1, MAX(0,mldq));
+
+ muk = MIN(Nlocal-1, MAX(0,mukeep));
+ mlk = MIN(Nlocal-1, MAX(0,mlkeep));
+ pdata->mukeep = muk;
+ pdata->mlkeep = mlk;
+
+ /* Allocate memory for saved Jacobian */
+ pdata->savedJ = NewBandMat(Nlocal, muk, mlk, muk);
+ if (pdata->savedJ == NULL) {
+ free(pdata); pdata = NULL;
+ cpProcessError(cp_mem, 0, "CPBBDPRE", "CPBBDPrecAlloc", MSGBBDP_MEM_FAIL);
+ return(NULL);
+ }
+
+ /* Allocate memory for preconditioner matrix */
+ storage_mu = MIN(Nlocal-1, muk + mlk);
+ pdata->savedP = NULL;
+ pdata->savedP = NewBandMat(Nlocal, muk, mlk, storage_mu);
+ if (pdata->savedP == NULL) {
+ DestroyMat(pdata->savedJ);
+ free(pdata); pdata = NULL;
+ cpProcessError(cp_mem, 0, "CPBBDPRE", "CPBBDPrecAlloc", MSGBBDP_MEM_FAIL);
+ return(NULL);
+ }
+ /* Allocate memory for pivots */
+ pdata->pivots = NULL;
+ pdata->pivots = NewIntArray(Nlocal);
+ if (pdata->savedJ == NULL) {
+ DestroyMat(pdata->savedP);
+ DestroyMat(pdata->savedJ);
+ free(pdata); pdata = NULL;
+ cpProcessError(cp_mem, 0, "CPBBDPRE", "CPBBDPrecAlloc", MSGBBDP_MEM_FAIL);
+ return(NULL);
+ }
+ /* Allocate tmp4 for use by cpBBDDQJacImpl */
+ tmp4 = NULL;
+ tmp4 = N_VClone(vec_tmpl);
+ if (tmp4 == NULL){
+ DestroyMat(pdata->savedP);
+ DestroyMat(pdata->savedJ);
+ DestroyArray(pdata->pivots);
+ free(pdata); pdata = NULL;
+ cpProcessError(cp_mem, 0, "CPBBDPRE", "CPBBDPrecAlloc", MSGBBDP_MEM_FAIL);
+ return(NULL);
+ }
+ pdata->tmp4 = tmp4;
+
+ /* Set pdata->dqrely based on input dqrely (0 implies default). */
+ pdata->dqrely = (dqrely > ZERO) ? dqrely : RSqrt(uround);
+
+ /* Store Nlocal to be used in cpBBDPrecSetupExpl and cpBBDPrecSetupImpl */
+ pdata->n_local = Nlocal;
+
+ /* Set work space sizes and initialize nge */
+ pdata->rpwsize = Nlocal*(muk + 2*mlk + storage_mu + 2);
+ pdata->ipwsize = Nlocal;
+ pdata->nge = 0;
+
+ return((void *)pdata);
+}
+
+int CPBBDSptfqmr(void *cpode_mem, int pretype, int maxl, void *bbd_data)
+{
+ CPodeMem cp_mem;
+ int flag;
+
+ flag = CPSptfqmr(cpode_mem, pretype, maxl);
+ if(flag != CPSPILS_SUCCESS) return(flag);
+
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (bbd_data == NULL) {
+ cpProcessError(cp_mem, CPBBDPRE_PDATA_NULL, "CPBBDPRE", "CPBBDSptfqmr", MSGBBDP_PDATA_NULL);
+ return(CPBBDPRE_PDATA_NULL);
+ }
+
+ switch (ode_type) {
+ case CP_EXPL:
+ flag = CPSpilsSetPreconditioner(cpode_mem, (void *)cpBBDPrecSetupExpl, (void *)cpBBDPrecSolveExpl, bbd_data);
+ break;
+ case CP_IMPL:
+ flag = CPSpilsSetPreconditioner(cpode_mem, (void *)cpBBDPrecSetupImpl, (void *)cpBBDPrecSolveImpl, bbd_data);
+ break;
+ }
+ if(flag != CPSPILS_SUCCESS) return(flag);
+
+ return(CPSPILS_SUCCESS);
+}
+
+int CPBBDSpbcg(void *cpode_mem, int pretype, int maxl, void *bbd_data)
+{
+ CPodeMem cp_mem;
+ int flag;
+
+ flag = CPSpbcg(cpode_mem, pretype, maxl);
+ if(flag != CPSPILS_SUCCESS) return(flag);
+
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (bbd_data == NULL) {
+ cpProcessError(cp_mem, CPBBDPRE_PDATA_NULL, "CPBBDPRE", "CPBBDSpbcg", MSGBBDP_PDATA_NULL);
+ return(CPBBDPRE_PDATA_NULL);
+ }
+
+ switch (ode_type) {
+ case CP_EXPL:
+ flag = CPSpilsSetPreconditioner(cpode_mem, (void *)cpBBDPrecSetupExpl, (void *)cpBBDPrecSolveExpl, bbd_data);
+ break;
+ case CP_IMPL:
+ flag = CPSpilsSetPreconditioner(cpode_mem, (void *)cpBBDPrecSetupImpl, (void *)cpBBDPrecSolveImpl, bbd_data);
+ break;
+ }
+ if(flag != CPSPILS_SUCCESS) return(flag);
+
+ return(CPSPILS_SUCCESS);
+}
+
+int CPBBDSpgmr(void *cpode_mem, int pretype, int maxl, void *bbd_data)
+{
+ CPodeMem cp_mem;
+ int flag;
+
+ flag = CPSpgmr(cpode_mem, pretype, maxl);
+ if(flag != CPSPILS_SUCCESS) return(flag);
+
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (bbd_data == NULL) {
+ cpProcessError(cp_mem, CPBBDPRE_PDATA_NULL, "CPBBDPRE", "CPBBDSpgmr", MSGBBDP_PDATA_NULL);
+ return(CPBBDPRE_PDATA_NULL);
+ }
+
+ switch (ode_type) {
+ case CP_EXPL:
+ flag = CPSpilsSetPreconditioner(cpode_mem, (void *)cpBBDPrecSetupExpl, (void *)cpBBDPrecSolveExpl, bbd_data);
+ break;
+ case CP_IMPL:
+ flag = CPSpilsSetPreconditioner(cpode_mem, (void *)cpBBDPrecSetupImpl, (void *)cpBBDPrecSolveImpl, bbd_data);
+ break;
+ }
+ if(flag != CPSPILS_SUCCESS) return(flag);
+
+ return(CPSPILS_SUCCESS);
+}
+
+int CPBBDPrecReInit(void *bbd_data, int mudq, int mldq,
+ realtype dqrely,
+ void *gloc, CPBBDCommFn cfn)
+{
+ CPBBDPrecData pdata;
+ CPodeMem cp_mem;
+ int Nlocal;
+
+ if (bbd_data == NULL) {
+ cpProcessError(NULL, CPBBDPRE_PDATA_NULL, "CPBBDPRE", "CPBBDPrecReInit", MSGBBDP_PDATA_NULL);
+ return(CPBBDPRE_PDATA_NULL);
+ }
+ pdata = (CPBBDPrecData) bbd_data;
+ cp_mem = (CPodeMem) pdata->cpode_mem;
+
+ /* Set pointers to gloc and cfn; load half-bandwidths */
+ switch (ode_type) {
+ case CP_EXPL:
+ pdata->glocE = (CPBBDLocalRhsFn) gloc;
+ pdata->glocI = NULL;
+ break;
+ case CP_IMPL:
+ pdata->glocI = (CPBBDLocalResFn) gloc;
+ pdata->glocE = NULL;
+ break;
+ }
+ pdata->cfn = cfn;
+ Nlocal = pdata->n_local;
+ pdata->mudq = MIN(Nlocal-1, MAX(0,mudq));
+ pdata->mldq = MIN(Nlocal-1, MAX(0,mldq));
+
+ /* Set pdata->dqrely based on input dqrely (0 implies default). */
+ pdata->dqrely = (dqrely > ZERO) ? dqrely : RSqrt(uround);
+
+ /* Re-initialize nge */
+ pdata->nge = 0;
+
+ return(CPBBDPRE_SUCCESS);
+}
+
+void CPBBDPrecFree(void **bbd_data)
+{
+ CPBBDPrecData pdata;
+
+ if (*bbd_data == NULL) return;
+
+ pdata = (CPBBDPrecData) (*bbd_data);
+ DestroyMat(pdata->savedJ);
+ DestroyMat(pdata->savedP);
+ DestroyArray(pdata->pivots);
+ N_VDestroy(pdata->tmp4);
+
+ free(*bbd_data);
+ *bbd_data = NULL;
+
+}
+
+int CPBBDPrecGetWorkSpace(void *bbd_data, long int *lenrwBBDP, long int *leniwBBDP)
+{
+ CPBBDPrecData pdata;
+
+ if (bbd_data == NULL) {
+ cpProcessError(NULL, CPBBDPRE_PDATA_NULL, "CPBBDPRE", "CPBBDPrecGetWorkSpace", MSGBBDP_PDATA_NULL);
+ return(CPBBDPRE_PDATA_NULL);
+ }
+
+ pdata = (CPBBDPrecData) bbd_data;
+
+ *lenrwBBDP = pdata->rpwsize;
+ *leniwBBDP = pdata->ipwsize;
+
+ return(CPBBDPRE_SUCCESS);
+}
+
+int CPBBDPrecGetNumGfnEvals(void *bbd_data, long int *ngevalsBBDP)
+{
+ CPBBDPrecData pdata;
+
+ if (bbd_data == NULL) {
+ cpProcessError(NULL, CPBBDPRE_PDATA_NULL, "CPBBDPRE", "CPBBDPrecGetNumGfnEvals", MSGBBDP_PDATA_NULL);
+ return(CPBBDPRE_PDATA_NULL);
+ }
+
+ pdata = (CPBBDPrecData) bbd_data;
+
+ *ngevalsBBDP = pdata->nge;
+
+ return(CPBBDPRE_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * CPBBDPrecGetReturnFlagName
+ * -----------------------------------------------------------------
+ */
+
+char *CPBBDPrecGetReturnFlagName(int flag)
+{
+ char *name;
+
+ name = (char *)malloc(30*sizeof(char));
+
+ switch(flag) {
+ case CPBBDPRE_SUCCESS:
+ sprintf(name,"CPBBDPRE_SUCCESS");
+ break;
+ case CPBBDPRE_PDATA_NULL:
+ sprintf(name,"CPBBDPRE_PDATA_NULL");
+ break;
+ case CPBBDPRE_FUNC_UNRECVR:
+ sprintf(name,"CPBBDPRE_FUNC_UNRECVR");
+ break;
+ default:
+ sprintf(name,"NONE");
+ }
+
+ return(name);
+}
+
+/*
+ * =================================================================
+ * PRIVATE FUNCTIONS
+ * =================================================================
+ */
+
+/* Readability Replacements */
+
+#define Nlocal (pdata->n_local)
+#define mudq (pdata->mudq)
+#define mldq (pdata->mldq)
+#define mukeep (pdata->mukeep)
+#define mlkeep (pdata->mlkeep)
+#define dqrely (pdata->dqrely)
+#define glocE (pdata->glocE)
+#define glocI (pdata->glocI)
+#define cfn (pdata->cfn)
+#define savedJ (pdata->savedJ)
+#define savedP (pdata->savedP)
+#define pivots (pdata->pivots)
+#define nge (pdata->nge)
+
+/*
+ * -----------------------------------------------------------------
+ * Function: cpBBDPrecSetupExpl
+ * -----------------------------------------------------------------
+ * cpBBDPrecSetupExpl generates and factors a banded block of the
+ * preconditioner matrix on each processor, via calls to the
+ * user-supplied glocE and cfn functions. It uses difference
+ * quotient approximations to the Jacobian elements.
+ *
+ * cpBBDPrecSetupExpl calculates a new J,if necessary, then
+ * calculates P = I - gamma*J, and does an LU factorization of P.
+ *
+ * The parameters of cpBBDPrecSetupExpl used here are as follows:
+ *
+ * t is the current value of the independent variable.
+ *
+ * y is the current value of the dependent variable vector,
+ * namely the predicted value of y(t).
+ *
+ * fy is the vector f(t,y).
+ *
+ * jok is an input flag indicating whether Jacobian-related
+ * data needs to be recomputed, as follows:
+ * jok == FALSE means recompute Jacobian-related data
+ * from scratch.
+ * jok == TRUE means that Jacobian data from the
+ * previous cpBBDPrecSetupExpl call can be reused
+ * (with the current value of gamma).
+ * A cpBBDPrecSetupExpl call with jok == TRUE should only
+ * occur after a call with jok == FALSE.
+ *
+ * jcurPtr is a pointer to an output integer flag which is
+ * set by cpBBDPrecSetupExpl as follows:
+ * *jcurPtr = TRUE if Jacobian data was recomputed.
+ * *jcurPtr = FALSE if Jacobian data was not recomputed,
+ * but saved data was reused.
+ *
+ * gamma is the scalar appearing in the Newton matrix.
+ *
+ * bbd_data is a pointer to user data returned by CPBBDPrecAlloc.
+ *
+ * tmp1, tmp2, and tmp3 are pointers to memory allocated for
+ * NVectors which are be used by cpBBDPrecSetupExpl
+ * as temporary storage or work space.
+ *
+ * Return value:
+ * The value returned by this cpBBDPrecSetupExpl function is the int
+ * 0 if successful,
+ * 1 for a recoverable error (step will be retried).
+ * -----------------------------------------------------------------
+ */
+
+static int cpBBDPrecSetupExpl(realtype t, N_Vector y, N_Vector fy,
+ booleantype jok, booleantype *jcurPtr,
+ realtype gamma, void *bbd_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ int ier;
+ CPBBDPrecData pdata;
+ CPodeMem cp_mem;
+ int retval;
+
+ pdata = (CPBBDPrecData) bbd_data;
+
+ cp_mem = (CPodeMem) pdata->cpode_mem;
+
+ if (jok) {
+
+ /* If jok = TRUE, use saved copy of J */
+ *jcurPtr = FALSE;
+ BandCopy(savedJ, savedP, mukeep, mlkeep);
+
+ } else {
+
+ /* Otherwise call cpBBDDQJacExpl for new J value */
+ *jcurPtr = TRUE;
+ BandZero(savedJ);
+
+ retval = cpBBDDQJacExpl(pdata, t, y, tmp1, tmp2, tmp3);
+ if (retval < 0) {
+ cpProcessError(cp_mem, CPBBDPRE_FUNC_UNRECVR, "CPBBDPRE", "cpBBDPrecSetup", MSGBBDP_FUNC_FAILED);
+ return(-1);
+ }
+ if (retval > 0) {
+ return(1);
+ }
+
+ BandCopy(savedJ, savedP, mukeep, mlkeep);
+
+ }
+
+ /* Scale and add I to get P = I - gamma*J */
+ BandScale(-gamma, savedP);
+ BandAddI(savedP);
+
+ /* Do LU factorization of P in place */
+ ier = BandGBTRF(savedP, pivots);
+
+ /* Return 0 if the LU was complete; otherwise return 1 */
+ if (ier > 0) return(1);
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Function: cpBBDPrecSolveExpl
+ * -----------------------------------------------------------------
+ * cpBBDPrecSolveExpl solves a linear system P z = r, with the
+ * band-block-diagonal preconditioner matrix P generated and
+ * factored by cpBBDPrecSetupExpl.
+ *
+ * The parameters of cpBBDPrecSolveExpl used here are as follows:
+ *
+ * r - right-hand side vector of the linear system.
+ *
+ * bbd_data - pointer to the preconditioner data returned by
+ * CPBBDPrecAlloc.
+ *
+ * z - output vector computed by cpBBDPrecSolveExpl.
+ *
+ * The value returned by the cpBBDPrecSolveExpl function is always
+ * 0, indicating success.
+ * -----------------------------------------------------------------
+ */
+
+static int cpBBDPrecSolveExpl(realtype t, N_Vector y, N_Vector fy,
+ N_Vector b, N_Vector x,
+ realtype gamma, realtype delta,
+ int lr, void *bbd_data, N_Vector tmp)
+{
+ CPBBDPrecData pdata;
+ realtype *xd;
+
+ pdata = (CPBBDPrecData) bbd_data;
+
+ /* Copy b to x, then do backsolve and return */
+ N_VScale(ONE, b, x);
+
+ xd = N_VGetArrayPointer(x);
+
+ BandGBTRS(savedP, pivots, xd);
+
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Function : cpBBDPrecSetupImpl
+ * -----------------------------------------------------------------
+ * cpBBDPrecSetupImpl generates a band-block-diagonal preconditioner
+ * matrix, where the local block (on this processor) is a band
+ * matrix. Each local block is computed by a difference quotient
+ * scheme via calls to the user-supplied routines glocal, gcomm.
+ * After generating the block in the band matrix savedP, this routine
+ * does an LU factorization in place in savedP.
+ *
+ * The cpBBDPrecSetupImpl parameters used here are as follows:
+ *
+ * t is the current value of the independent variable t.
+ *
+ * yy is the current value of the dependent variable vector,
+ * namely the predicted value of y(t).
+ *
+ * yp is the current value of the derivative vector y',
+ * namely the predicted value of y'(t).
+ *
+ * gamma is the scalar in the system Jacobian, proportional to h.
+ *
+ * bbd_data is a pointer to user preconditioner data returned by
+ * CPBBDPrecAlloc.
+ *
+ * tmp1, tmp2, tmp3 are pointers to vectors of type N_Vector,
+ * used for temporary storage or work space.
+ *
+ * Return value:
+ * The value returned by cpBBDPrecSetupImpl function is a int
+ * flag indicating whether it was successful. This value is
+ * 0 if successful,
+ * > 0 for a recoverable error (step will be retried), or
+ * < 0 for a nonrecoverable error (step fails).
+ * -----------------------------------------------------------------
+ */
+
+static int cpBBDPrecSetupImpl(realtype t, N_Vector y, N_Vector yp, N_Vector r,
+ realtype gamma, void *bbd_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ int ier, retval;
+ CPBBDPrecData pdata;
+ CPodeMem cp_mem;
+
+ pdata =(CPBBDPrecData) bbd_data;
+
+ cp_mem = (CPodeMem) pdata->cpode_mem;
+
+ /* Call cpBBDDQJacImpl for a new Jacobian calculation and store in savedP. */
+ BandZero(savedP);
+ retval = cpBBDDQJacImpl(pdata, t, gamma, y, yp,
+ tmp1, tmp2, tmp3, pdata->tmp4);
+ if (retval < 0) {
+ cpProcessError(cp_mem, CPBBDPRE_FUNC_UNRECVR, "CPBBDPRE", "cpBBDPrecSetupImpl", MSGBBDP_FUNC_FAILED);
+ return(-1);
+ }
+ if (retval > 0) {
+ return(+1);
+ }
+
+ /* Do LU factorization of preconditioner block in place (in savedP). */
+ ier = BandGBTRF(savedP, pivots);
+
+ /* Return 0 if the LU was complete, or +1 otherwise. */
+ if (ier > 0) return(+1);
+ return(0);
+}
+
+
+/*
+ * -----------------------------------------------------------------
+ * Function: cpBBDPrecSolveImpl
+ * -----------------------------------------------------------------
+ * The function cpBBDPrecSolve computes a solution to the linear
+ * system P x = b, where P is the left preconditioner defined by
+ * the routine cpBBDPrecSetupImpl.
+ *
+ * The cpBBDPrecSolveImpl parameters used here are as follows:
+ *
+ * b is the input right-hand side vector r.
+ *
+ * x is the computed solution vector.
+ *
+ * bbd_data is a pointer to user preconditioner data returned
+ * by CPBBDPrecAlloc.
+ *
+ * The arguments t, y, yp, r, gamma, delta, and tmp are NOT used.
+ *
+ * cpBBDPrecSolveImpl always returns 0, indicating success.
+ * -----------------------------------------------------------------
+ */
+
+static int cpBBDPrecSolveImpl(realtype t, N_Vector y, N_Vector yp, N_Vector r,
+ N_Vector b, N_Vector x,
+ realtype gamma, realtype delta,
+ void *bbd_data, N_Vector tmp)
+{
+ CPBBDPrecData pdata;
+ realtype *xd;
+
+ pdata = (CPBBDPrecData) bbd_data;
+
+ /* Copy b to x, do the backsolve, and return. */
+ N_VScale(ONE, b, x);
+
+ xd = N_VGetArrayPointer(x);
+
+ BandGBTRS(savedP, pivots, xd);
+
+ return(0);
+}
+
+/*
+ * =================================================================
+ * DQ LOCAL JACOBIAN APROXIMATIONS
+ * =================================================================
+ */
+
+#define ewt (cp_mem->cp_ewt)
+#define h (cp_mem->cp_h)
+#define f_data (cp_mem->cp_f_data)
+
+/*
+ * -----------------------------------------------------------------
+ * Function: cpBBDDQJacExpl
+ * -----------------------------------------------------------------
+ * This routine generates a banded difference quotient approximation
+ * to the local block of the Jacobian of g(t,y). It assumes that a
+ * band matrix of type BandMat is stored columnwise, and that elements
+ * within each column are contiguous. All matrix elements are generated
+ * as difference quotients, by way of calls to the user routine glocE.
+ * By virtue of the band structure, the number of these calls is
+ * bandwidth + 1, where bandwidth = mldq + mudq + 1.
+ * But the band matrix kept has bandwidth = mlkeep + mukeep + 1.
+ * This routine also assumes that the local elements of a vector are
+ * stored contiguously.
+ * -----------------------------------------------------------------
+ */
+
+static int cpBBDDQJacExpl(CPBBDPrecData pdata, realtype t,
+ N_Vector y, N_Vector gy,
+ N_Vector ytemp, N_Vector gtemp)
+{
+ CPodeMem cp_mem;
+ realtype gnorm, minInc, inc, inc_inv;
+ int group, i, j, width, ngroups, i1, i2;
+ realtype *y_data, *ewt_data, *gy_data, *gtemp_data, *ytemp_data, *col_j;
+ int retval;
+
+ cp_mem = (CPodeMem) pdata->cpode_mem;
+
+ /* Load ytemp with y = predicted solution vector */
+ N_VScale(ONE, y, ytemp);
+
+ /* Call cfn and glocE to get base value of g(t,y) */
+ if (cfn != NULL) {
+ retval = cfn(Nlocal, t, y, NULL, f_data);
+ if (retval != 0) return(retval);
+ }
+
+ retval = glocE(Nlocal, t, ytemp, gy, f_data);
+ nge++;
+ if (retval != 0) return(retval);
+
+ /* Obtain pointers to the data for various vectors */
+ y_data = N_VGetArrayPointer(y);
+ gy_data = N_VGetArrayPointer(gy);
+ ewt_data = N_VGetArrayPointer(ewt);
+ ytemp_data = N_VGetArrayPointer(ytemp);
+ gtemp_data = N_VGetArrayPointer(gtemp);
+
+ /* Set minimum increment based on uround and norm of g */
+ gnorm = N_VWrmsNorm(gy, ewt);
+ minInc = (gnorm != ZERO) ?
+ (MIN_INC_MULT * ABS(h) * uround * Nlocal * gnorm) : ONE;
+
+ /* Set bandwidth and number of column groups for band differencing */
+ width = mldq + mudq + 1;
+ ngroups = MIN(width, Nlocal);
+
+ /* Loop over groups */
+ for (group=1; group <= ngroups; group++) {
+
+ /* Increment all y_j in group */
+ for(j=group-1; j < Nlocal; j+=width) {
+ inc = MAX(dqrely*ABS(y_data[j]), minInc/ewt_data[j]);
+ ytemp_data[j] += inc;
+ }
+
+ /* Evaluate g with incremented y */
+ retval = glocE(Nlocal, t, ytemp, gtemp, f_data);
+ nge++;
+ if (retval != 0) return(retval);
+
+ /* Restore ytemp, then form and load difference quotients */
+ for (j=group-1; j < Nlocal; j+=width) {
+ ytemp_data[j] = y_data[j];
+ col_j = BAND_COL(savedJ,j);
+ inc = MAX(dqrely*ABS(y_data[j]), minInc/ewt_data[j]);
+ inc_inv = ONE/inc;
+ i1 = MAX(0, j-mukeep);
+ i2 = MIN(j+mlkeep, Nlocal-1);
+ for (i=i1; i <= i2; i++)
+ BAND_COL_ELEM(col_j,i,j) =
+ inc_inv * (gtemp_data[i] - gy_data[i]);
+ }
+ }
+
+ return(0);
+}
+
+
+/*
+ * -----------------------------------------------------------------
+ * cpBBDDQJacImpl
+ * -----------------------------------------------------------------
+ * This routine generates a banded difference quotient approximation
+ * to the local block of the Jacobian of G(t,y,y'). It assumes that
+ * a band matrix of type BandMat is stored column-wise, and that
+ * elements within each column are contiguous.
+ *
+ * All matrix elements are generated as difference quotients, by way
+ * of calls to the user routine glocI. By virtue of the band
+ * structure, the number of these calls is bandwidth + 1, where
+ * bandwidth = mldq + mudq + 1. But the band matrix kept has
+ * bandwidth = mlkeep + mukeep + 1. This routine also assumes that
+ * the local elements of a vector are stored contiguously.
+ *
+ * Return values are: 0 (success), > 0 (recoverable error),
+ * or < 0 (nonrecoverable error).
+ * -----------------------------------------------------------------
+ */
+
+static int cpBBDDQJacImpl(CPBBDPrecData pdata, realtype t, realtype gamma,
+ N_Vector y, N_Vector yp, N_Vector gref,
+ N_Vector ytemp, N_Vector yptemp, N_Vector gtemp)
+{
+ CPodeMem cp_mem;
+ realtype inc, inc_inv;
+ int retval;
+ int group, i, j, width, ngroups, i1, i2;
+ realtype *ydata, *ypdata, *ytempdata, *yptempdata, *grefdata, *gtempdata;
+ realtype *ewtdata;
+ realtype *col_j, yj, ypj, ewtj;
+
+ cp_mem = (CPodeMem) pdata->cpode_mem;
+
+ /* Initialize ytemp and yptemp. */
+ N_VScale(ONE, y, ytemp);
+ N_VScale(ONE, yp, yptemp);
+
+ /* Obtain pointers as required to the data array of vectors. */
+ ydata = N_VGetArrayPointer(y);
+ ypdata = N_VGetArrayPointer(yp);
+ gtempdata = N_VGetArrayPointer(gtemp);
+ ewtdata = N_VGetArrayPointer(ewt);
+ ytempdata = N_VGetArrayPointer(ytemp);
+ yptempdata= N_VGetArrayPointer(yptemp);
+ grefdata = N_VGetArrayPointer(gref);
+
+ /* Call cfn and glocI to get base value of G(t,y,y'). */
+ if (cfn != NULL) {
+ retval = cfn(Nlocal, t, y, yp, f_data);
+ if (retval != 0) return(retval);
+ }
+
+ retval = glocI(Nlocal, t, y, yp, gref, f_data);
+ nge++;
+ if (retval != 0) return(retval);
+
+ /* Set bandwidth and number of column groups for band differencing. */
+ width = mldq + mudq + 1;
+ ngroups = MIN(width, Nlocal);
+
+ /* Loop over groups. */
+ for(group = 1; group <= ngroups; group++) {
+
+ /* Loop over the components in this group. */
+ for(j = group-1; j < Nlocal; j += width) {
+ yj = ydata[j];
+ ypj = ypdata[j];
+ ewtj = ewtdata[j];
+
+ /* Set increment inc to yj based on rel_yy*abs(yj), with
+ adjustments using ypj and ewtj if this is small, and a further
+ adjustment to give it the same sign as hh*ypj. */
+ inc = dqrely*MAX(ABS(yj), MAX( ABS(h*ypj), ONE/ewtj));
+ if (h*ypj < ZERO) inc = -inc;
+ inc = (yj + inc) - yj;
+
+ /* Increment yj and ypj. */
+ ytempdata[j] += gamma*inc;
+ yptempdata[j] += inc;
+
+ }
+
+ /* Evaluate G with incremented y and yp arguments. */
+ retval = glocI(Nlocal, t, ytemp, yptemp, gtemp, f_data);
+ nge++;
+ if (retval != 0) return(retval);
+
+ /* Loop over components of the group again; restore ytemp and yptemp. */
+ for(j = group-1; j < Nlocal; j += width) {
+ yj = ytempdata[j] = ydata[j];
+ ypj = yptempdata[j] = ypdata[j];
+ ewtj = ewtdata[j];
+
+ /* Set increment inc as before .*/
+ inc = dqrely*MAX(ABS(yj), MAX( ABS(h*ypj), ONE/ewtj));
+ if (h*ypj < ZERO) inc = -inc;
+ inc = (yj + inc) - yj;
+
+ /* Form difference quotients and load into savedP. */
+ inc_inv = ONE/inc;
+ col_j = BAND_COL(savedP,j);
+ i1 = MAX(0, j-mukeep);
+ i2 = MIN(j+mlkeep, Nlocal-1);
+ for(i = i1; i <= i2; i++)
+ BAND_COL_ELEM(col_j,i,j) = inc_inv * (gtempdata[i] - grefdata[i]);
+ }
+ }
+
+ return(0);
+}
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_bbdpre_impl.h b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_bbdpre_impl.h
new file mode 100644
index 0000000..f2e869c
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_bbdpre_impl.h
@@ -0,0 +1,79 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/22 00:12:48 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * Implementation header file for the CPBBDPRE module.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _CPBBDPRE_IMPL_H
+#define _CPBBDPRE_IMPL_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <cpodes/cpodes_bbdpre.h>
+#include <sundials/sundials_band.h>
+
+/*
+ * -----------------------------------------------------------------
+ * Type: CPBBDPrecData
+ * -----------------------------------------------------------------
+ */
+
+typedef struct {
+
+ /* passed by user to CPBBDPrecAlloc and used by PrecSetup/PrecSolve */
+ int mudq, mldq, mukeep, mlkeep;
+ realtype dqrely;
+ CPBBDLocalRhsFn glocE;
+ CPBBDLocalResFn glocI;
+ CPBBDCommFn cfn;
+
+ /* additional N_Vector needed by cpBBDPrecSetupImpl */
+ N_Vector tmp4;
+
+ /* set by CPBBDPrecSetup and used by CPBBDPrecSolve */
+ DlsMat savedJ;
+ DlsMat savedP;
+ int *pivots;
+
+ /* set by CPBBDPrecAlloc and used by CPBBDPrecSetup */
+ int n_local;
+
+ /* available for optional output */
+ long int rpwsize;
+ long int ipwsize;
+ long int nge;
+
+ /* pointer to cpode_mem */
+ void *cpode_mem;
+
+} *CPBBDPrecData;
+
+/*
+ * -----------------------------------------------------------------
+ * CPBBDPRE error messages
+ * -----------------------------------------------------------------
+ */
+
+#define MSGBBDP_CPMEM_NULL "Integrator memory is NULL."
+#define MSGBBDP_MEM_FAIL "A memory request failed."
+#define MSGBBDP_BAD_NVECTOR "A required vector operation is not implemented."
+#define MSGBBDP_PDATA_NULL "BBDPrecData is NULL."
+#define MSGBBDP_FUNC_FAILED "The gloc or cfn routine failed in an unrecoverable manner."
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_dense.c b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_dense.c
new file mode 100644
index 0000000..34dd51b
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_dense.c
@@ -0,0 +1,1155 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.4 $
+ * $Date: 2007/10/26 21:48:38 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for the CPDENSE linear solver.
+ * -----------------------------------------------------------------
+ */
+
+
+/*
+ * TODO:
+ *
+ * cpdQRcomputeKD
+ * cpdSCcomputeKD
+ */
+
+
+/*
+ * =================================================================
+ * IMPORTED HEADER FILES
+ * =================================================================
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <cpodes/cpodes_dense.h>
+#include "cpodes_direct_impl.h"
+#include "cpodes_private.h"
+
+#include <sundials/sundials_math.h>
+
+/*
+ * =================================================================
+ * PROTOTYPES FOR PRIVATE FUNCTIONS
+ * =================================================================
+ */
+
+/* CPDENSE linit, lsetup, lsolve, and lfree routines */
+
+static int cpDenseInit(CPodeMem cp_mem);
+static int cpDenseSetup(CPodeMem cp_mem, int convfail,
+ N_Vector yP, N_Vector ypP, N_Vector fctP,
+ booleantype *jcurPtr,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+static int cpDenseSolve(CPodeMem cp_mem, N_Vector b, N_Vector weight,
+ N_Vector yC, N_Vector ypC, N_Vector fctC);
+static void cpDenseFree(CPodeMem cp_mem);
+
+/* CPDENSE linitP, lsetupP, lsolveP, lmultP, and lfreeP routines */
+static int cpDenseProjInit(CPodeMem cp_mem);
+static int cpDenseProjSetup(CPodeMem cp_mem, N_Vector y, N_Vector cy,
+ N_Vector c_tmp1, N_Vector c_tmp2, N_Vector s_tmp1);
+static int cpDenseProjSolve(CPodeMem cp_mem, N_Vector b, N_Vector x,
+ N_Vector y, N_Vector cy,
+ N_Vector c_tmp1, N_Vector s_tmp1);
+static void cpDenseProjMult(CPodeMem cp_mem, N_Vector x, N_Vector Gx);
+static void cpDenseProjFree(CPodeMem cp_mem);
+
+/* Private functions for LU, QR, and SC projection */
+static void cpdLUcomputeKI(CPodeMem cp_mem);
+static void cpdLUcomputeKD(CPodeMem cp_mem, N_Vector d);
+static void cpdQRcomputeKD(CPodeMem cp_mem, N_Vector d);
+static void cpdSCcomputeKI(CPodeMem cp_mem);
+static void cpdSCcomputeKD(CPodeMem cp_mem, N_Vector d);
+
+/*
+ * =================================================================
+ * READIBILITY REPLACEMENTS
+ * =================================================================
+ */
+
+#define ode_type (cp_mem->cp_ode_type)
+#define lmm_type (cp_mem->cp_lmm_type)
+#define fe (cp_mem->cp_fe)
+#define fi (cp_mem->cp_fi)
+#define f_data (cp_mem->cp_f_data)
+#define uround (cp_mem->cp_uround)
+#define nst (cp_mem->cp_nst)
+#define tn (cp_mem->cp_tn)
+#define h (cp_mem->cp_h)
+#define gamma (cp_mem->cp_gamma)
+#define gammap (cp_mem->cp_gammap)
+#define gamrat (cp_mem->cp_gamrat)
+#define ewt (cp_mem->cp_ewt)
+#define tempv (cp_mem->cp_tempv)
+
+#define linit (cp_mem->cp_linit)
+#define lsetup (cp_mem->cp_lsetup)
+#define lsolve (cp_mem->cp_lsolve)
+#define lfree (cp_mem->cp_lfree)
+#define lmem (cp_mem->cp_lmem)
+#define lsetup_exists (cp_mem->cp_lsetup_exists)
+
+#define pnorm (cp_mem->cp_proj_norm)
+
+#define linitP (cp_mem->cp_linitP)
+#define lsetupP (cp_mem->cp_lsetupP)
+#define lsolveP (cp_mem->cp_lsolveP)
+#define lmultP (cp_mem->cp_lmultP)
+#define lfreeP (cp_mem->cp_lfreeP)
+#define lmemP (cp_mem->cp_lmemP)
+#define lsetupP_exists (cp_mem->cp_lsetupP_exists)
+
+#define mtype (cpdls_mem->d_type)
+#define n (cpdls_mem->d_n)
+#define jacE (cpdls_mem->d_djacE)
+#define jacI (cpdls_mem->d_djacI)
+#define M (cpdls_mem->d_M)
+#define savedJ (cpdls_mem->d_savedJ)
+#define pivots (cpdls_mem->d_pivots)
+#define nstlj (cpdls_mem->d_nstlj)
+#define nje (cpdls_mem->d_nje)
+#define nfeDQ (cpdls_mem->d_nfeDQ)
+#define J_data (cpdls_mem->d_J_data)
+#define last_flag (cpdls_mem->d_last_flag)
+
+#define nc (cpdlsP_mem->d_nc)
+#define ny (cpdlsP_mem->d_ny)
+#define jacP (cpdlsP_mem->d_jacP)
+#define JP_data (cpdlsP_mem->d_JP_data)
+#define ftype (cpdlsP_mem->d_ftype)
+#define G (cpdlsP_mem->d_G)
+#define savedG (cpdlsP_mem->d_savedG)
+#define K (cpdlsP_mem->d_K)
+#define pivotsP (cpdlsP_mem->d_pivotsP)
+#define beta (cpdlsP_mem->d_beta)
+#define nstljP (cpdlsP_mem->d_nstljP)
+#define njeP (cpdlsP_mem->d_njeP)
+#define nceDQ (cpdlsP_mem->d_nceDQ)
+
+/*
+ * =================================================================
+ * EXPORTED FUNCTIONS
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * CPDense
+ * -----------------------------------------------------------------
+ * This routine initializes the memory record and sets various function
+ * fields specific to the dense linear solver module. CPDense first
+ * calls the existing lfree routine if this is not NULL. Then it sets
+ * the cp_linit, cp_lsetup, cp_lsolve, cp_lfree fields in (*cpode_mem)
+ * to be cpDenseInit, cpDenseSetup, cpDenseSolve, and cpDenseFree,
+ * respectively. It allocates memory for a structure of type
+ * CPDlsMemRec and sets the cp_lmem field in (*cpode_mem) to the
+ * address of this structure. It sets lsetup_exists in (*cpode_mem) to
+ * TRUE, and the d_jac field to the default cpDlsDenseDQJac.
+ * Finally, it allocates memory for M, pivots, and (if needed) savedJ.
+ * The return value is SUCCESS = 0, or LMEM_FAIL = -1.
+ *
+ * NOTE: The dense linear solver assumes a serial implementation
+ * of the NVECTOR package. Therefore, CPDense will first
+ * test for compatible a compatible N_Vector internal
+ * representation by checking that N_VGetArrayPointer and
+ * N_VSetArrayPointer exist.
+ * -----------------------------------------------------------------
+ */
+
+int CPDense(void *cpode_mem, int N)
+{
+ CPodeMem cp_mem;
+ CPDlsMem cpdls_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPDIRECT_MEM_NULL, "CPDENSE", "CPDense", MSGD_CPMEM_NULL);
+ return(CPDIRECT_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Test if the NVECTOR package is compatible with the DENSE solver */
+ if (tempv->ops->nvgetarraypointer == NULL ||
+ tempv->ops->nvsetarraypointer == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_ILL_INPUT, "CPDENSE", "CPDense", MSGD_BAD_NVECTOR);
+ return(CPDIRECT_ILL_INPUT);
+ }
+
+ if (lfree !=NULL) lfree(cp_mem);
+
+ /* Set four main function fields in cp_mem */
+ linit = cpDenseInit;
+ lsetup = cpDenseSetup;
+ lsolve = cpDenseSolve;
+ lfree = cpDenseFree;
+
+ /* Get memory for CPDlsMemRec */
+ cpdls_mem = NULL;
+ cpdls_mem = (CPDlsMem) malloc(sizeof(CPDlsMemRec));
+ if (cpdls_mem == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPDENSE", "CPDense", MSGD_MEM_FAIL);
+ return(CPDIRECT_MEM_FAIL);
+ }
+
+ /* Set matrix type */
+ mtype = SUNDIALS_DENSE;
+
+ /* Set default Jacobian routine and Jacobian data */
+ jacE = NULL;
+ jacI = NULL;
+ J_data = NULL;
+
+ last_flag = CPDIRECT_SUCCESS;
+ lsetup_exists = TRUE;
+
+ /* Set problem dimension */
+ n = N;
+
+ /* Allocate memory for M, pivot array, and (if needed) savedJ */
+ M = NULL;
+ pivots = NULL;
+ savedJ = NULL;
+
+ M = NewDenseMat(N, N);
+ if (M == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPDENSE", "CPDense", MSGD_MEM_FAIL);
+ free(cpdls_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ pivots = NewIntArray(N);
+ if (pivots == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPDENSE", "CPDense", MSGD_MEM_FAIL);
+ DestroyMat(M);
+ free(cpdls_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ if (ode_type == CP_EXPL) {
+ savedJ = NewDenseMat(N, N);
+ if (savedJ == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPDENSE", "CPDense", MSGD_MEM_FAIL);
+ DestroyMat(M);
+ DestroyArray(pivots);
+ free(cpdls_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ }
+
+ /* Attach linear solver memory to integrator memory */
+ lmem = cpdls_mem;
+
+ return(CPDIRECT_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * CPDenseProj
+ * -----------------------------------------------------------------
+ * This routine initializes the memory record and sets various function
+ * fields specific to the dense linear solver module for the projection.
+ *
+ * NOTE: The dense linear solver assumes a serial implementation
+ * of the NVECTOR package. Therefore, CPDense will first
+ * test for compatible a compatible N_Vector internal
+ * representation by checking that N_VGetArrayPointer and
+ * N_VSetArrayPointer exist.
+ * -----------------------------------------------------------------
+ */
+
+int CPDenseProj(void *cpode_mem, int Nc, int Ny, int fact_type)
+{
+ CPodeMem cp_mem;
+ CPDlsProjMem cpdlsP_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPDIRECT_MEM_NULL, "CPDENSE", "CPDenseProj", MSGD_CPMEM_NULL);
+ return(CPDIRECT_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Test if the NVECTOR package is compatible with the DENSE solver */
+ if (tempv->ops->nvgetarraypointer == NULL ||
+ tempv->ops->nvsetarraypointer == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_ILL_INPUT, "CPDENSE", "CPDenseProj", MSGD_BAD_NVECTOR);
+ return(CPDIRECT_ILL_INPUT);
+ }
+
+ /* Check if fact_type has a legal value */
+ if ( (fact_type != CPDIRECT_LU) && (fact_type != CPDIRECT_QR) && (fact_type != CPDIRECT_SC) ) {
+ cpProcessError(cp_mem, CPDIRECT_ILL_INPUT, "CPDENSE", "CPDenseProj", MSGD_BAD_FACT);
+ return(CPDIRECT_ILL_INPUT);
+ }
+
+ if (lfreeP !=NULL) lfreeP(cp_mem);
+
+ /* Set the five function fields in cp_mem */
+ linitP = cpDenseProjInit;
+ lsetupP = cpDenseProjSetup;
+ lsolveP = cpDenseProjSolve;
+ lmultP = cpDenseProjMult;
+ lfreeP = cpDenseProjFree;
+
+ /* Get memory for CPDlsProjMemRec */
+ cpdlsP_mem = NULL;
+ cpdlsP_mem = (CPDlsProjMem) malloc(sizeof(CPDlsProjMemRec));
+ if (cpdlsP_mem == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPDENSE", "CPDenseProj", MSGD_MEM_FAIL);
+ return(CPDIRECT_MEM_FAIL);
+ }
+
+ lsetupP_exists = TRUE;
+
+ /* Initialize all internal pointers to NULL */
+ G = NULL;
+ K = NULL;
+ pivotsP = NULL;
+ beta = NULL;
+
+ /* Allocate memory for G and other work space */
+ G = NewDenseMat(Ny, Nc);
+ if (G == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPDENSE", "CPDenseProj", MSGD_MEM_FAIL);
+ free(cpdlsP_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ savedG = NewDenseMat(Ny, Nc);
+ if (savedG == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPDENSE", "CPDenseProj", MSGD_MEM_FAIL);
+ DestroyMat(G);
+ free(cpdlsP_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+
+ /* Allocate additional work space, depending on factorization */
+ switch(fact_type) {
+
+ case CPDIRECT_LU:
+ /* Allocate space for pivotsP and K */
+ pivotsP = NewIntArray(Nc);
+ if (pivotsP == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPDENSE", "CPDenseProj", MSGD_MEM_FAIL);
+ DestroyMat(savedG);
+ DestroyMat(G);
+ free(cpdlsP_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ K = NewDenseMat(Ny-Nc, Ny-Nc);
+ if (K == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPDENSE", "CPDenseProj", MSGD_MEM_FAIL);
+ DestroyArray(pivotsP);
+ DestroyMat(savedG);
+ DestroyMat(G);
+ free(cpdlsP_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ break;
+
+ case CPDIRECT_QR:
+ /* Allocate space for beta */
+ beta = NewRealArray(Nc);
+ if (beta == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPDENSE", "CPDenseProj", MSGD_MEM_FAIL);
+ DestroyMat(savedG);
+ DestroyMat(G);
+ free(cpdlsP_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ /* If projecting in WRMS norm, allocate space for K=Q^T*D^(-1)*Q */
+ if (pnorm == CP_PROJ_ERRNORM) {
+ K = NewDenseMat(Nc, Nc);
+ if (K == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPDENSE", "CPDenseProj", MSGD_MEM_FAIL);
+ DestroyArray(beta);
+ DestroyMat(savedG);
+ DestroyMat(G);
+ free(cpdlsP_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ }
+ break;
+
+ case CPDIRECT_SC:
+ /* Allocate space for K = G * D^(-1) * G^T */
+ K = NewDenseMat(Nc, Nc);
+ if (K == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPDENSE", "CPDenseProj", MSGD_MEM_FAIL);
+ DestroyMat(savedG);
+ DestroyMat(G);
+ free(cpdlsP_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+
+ break;
+
+ }
+
+ /* Set default Jacobian routine and Jacobian data */
+ jacP = NULL;
+ JP_data = NULL;
+
+ lsetupP_exists = TRUE;
+
+ /* Copy inputs into memory */
+ nc = Nc; /* number of constraints */
+ ny = Ny; /* number of states */
+ ftype = fact_type; /* factorization type */
+
+ /* Attach linear solver memory to integrator memory */
+ lmemP = cpdlsP_mem;
+
+ return(CPDIRECT_SUCCESS);
+}
+
+/*
+ * =================================================================
+ * PRIVATE FUNCTIONS FOR IMPLICIT INTEGRATION
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * cpDenseInit
+ * -----------------------------------------------------------------
+ * This routine does remaining initializations specific to the dense
+ * linear solver.
+ * -----------------------------------------------------------------
+ */
+
+static int cpDenseInit(CPodeMem cp_mem)
+{
+ CPDlsMem cpdls_mem;
+
+ cpdls_mem = (CPDlsMem) lmem;
+
+ nje = 0;
+ nfeDQ = 0;
+ nstlj = 0;
+
+ if (ode_type == CP_EXPL && jacE == NULL) {
+ jacE = cpDlsDenseDQJacExpl;
+ J_data = cp_mem;
+ }
+
+ if (ode_type == CP_IMPL && jacI == NULL) {
+ jacI = cpDlsDenseDQJacImpl;
+ J_data = cp_mem;
+ }
+
+ last_flag = CPDIRECT_SUCCESS;
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpDenseSetup
+ * -----------------------------------------------------------------
+ * This routine does the setup operations for the dense linear solver.
+ * It makes a decision whether or not to call the Jacobian evaluation
+ * routine based on various state variables, and if not it uses the
+ * saved copy (for explicit ODE only). In any case, it constructs
+ * the Newton matrix M = I - gamma*J or M = F_y' - gamma*F_y, updates
+ * counters, and calls the dense LU factorization routine.
+ * -----------------------------------------------------------------
+ */
+
+static int cpDenseSetup(CPodeMem cp_mem, int convfail,
+ N_Vector yP, N_Vector ypP, N_Vector fctP,
+ booleantype *jcurPtr,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ booleantype jbad, jok;
+ realtype dgamma;
+ long int ier;
+ CPDlsMem cpdls_mem;
+ int retval;
+
+ cpdls_mem = (CPDlsMem) lmem;
+
+ switch (ode_type) {
+
+ case CP_EXPL:
+
+ /* Use nst, gamma/gammap, and convfail to set J eval. flag jok */
+ dgamma = ABS((gamma/gammap) - ONE);
+ jbad = (nst == 0) || (nst > nstlj + CPD_MSBJ) ||
+ ((convfail == CP_FAIL_BAD_J) && (dgamma < CPD_DGMAX)) ||
+ (convfail == CP_FAIL_OTHER);
+ jok = !jbad;
+
+ /* Test if it is enough to use a saved Jacobian copy */
+ if (jok) {
+ *jcurPtr = FALSE;
+ DenseCopy(savedJ, M);
+ } else {
+ nstlj = nst;
+ *jcurPtr = TRUE;
+ DenseZero(M);
+ retval = jacE(n, tn, yP, fctP, M, J_data, tmp1, tmp2, tmp3);
+ nje++;
+ if (retval < 0) {
+ cpProcessError(cp_mem, CPDIRECT_JACFUNC_UNRECVR, "CPDENSE", "cpDenseSetup", MSGD_JACFUNC_FAILED);
+ last_flag = CPDIRECT_JACFUNC_UNRECVR;
+ return(-1);
+ }
+ if (retval > 0) {
+ last_flag = CPDIRECT_JACFUNC_RECVR;
+ return(1);
+ }
+ DenseCopy(M, savedJ);
+ }
+
+ /* Scale and add I to get M = I - gamma*J */
+ DenseScale(-gamma, M);
+ DenseAddI(M);
+
+ break;
+
+ case CP_IMPL:
+
+ /* Initialize Jacobian to 0 and call Jacobian function */
+ DenseZero(M);
+ retval = jacI(n, tn, gamma, yP, ypP, fctP, M, J_data, tmp1, tmp2, tmp3);
+ nje++;
+ if (retval < 0) {
+ cpProcessError(cp_mem, CPDIRECT_JACFUNC_UNRECVR, "CPDENSE", "cpDenseSetup", MSGD_JACFUNC_FAILED);
+ last_flag = CPDIRECT_JACFUNC_UNRECVR;
+ return(-1);
+ }
+ if (retval > 0) {
+ last_flag = CPDIRECT_JACFUNC_RECVR;
+ return(1);
+ }
+
+ break;
+
+ }
+
+ /* Do LU factorization of M */
+ ier = DenseGETRF(M, pivots);
+
+ /* Return 0 if the LU was complete; otherwise return 1 */
+ last_flag = ier;
+ if (ier > 0) return(1);
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpDenseSolve
+ * -----------------------------------------------------------------
+ * This routine handles the solve operation for the dense linear solver
+ * by calling the dense backsolve routine. The returned value is 0.
+ * -----------------------------------------------------------------
+ */
+
+static int cpDenseSolve(CPodeMem cp_mem, N_Vector b, N_Vector weight,
+ N_Vector yC, N_Vector ypC, N_Vector fctC)
+{
+ CPDlsMem cpdls_mem;
+ realtype *bd;
+
+ cpdls_mem = (CPDlsMem) lmem;
+
+ bd = N_VGetArrayPointer(b);
+
+ DenseGETRS(M, pivots, bd);
+
+ /* For BDF, scale the correction to account for change in gamma */
+ if ((lmm_type == CP_BDF) && (gamrat != ONE)) {
+ N_VScale(TWO/(ONE + gamrat), b, b);
+ }
+
+ last_flag = CPDIRECT_SUCCESS;
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpDenseFree
+ * -----------------------------------------------------------------
+ * This routine frees memory specific to the dense linear solver.
+ * -----------------------------------------------------------------
+ */
+
+static void cpDenseFree(CPodeMem cp_mem)
+{
+ CPDlsMem cpdls_mem;
+
+ cpdls_mem = (CPDlsMem) lmem;
+
+ DestroyMat(M);
+ DestroyArray(pivots);
+ if (ode_type == CP_EXPL) DestroyMat(savedJ);
+ free(cpdls_mem);
+ cpdls_mem = NULL;
+}
+
+/*
+ * =================================================================
+ * PRIVATE FUNCTIONS FOR PROJECTION
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * cpDenseProjInit
+ * -----------------------------------------------------------------
+ * This routine does remaining initializations specific to the dense
+ * linear solver.
+ * -----------------------------------------------------------------
+ */
+
+static int cpDenseProjInit(CPodeMem cp_mem)
+{
+ CPDlsProjMem cpdlsP_mem;
+
+ cpdlsP_mem = (CPDlsProjMem) lmemP;
+
+ njeP = 0;
+ nceDQ = 0;
+ nstljP = 0;
+
+ if (jacP == NULL) {
+ jacP = cpDlsDenseProjDQJac;
+ JP_data = cp_mem;
+ }
+
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpDenseProjSetup
+ * -----------------------------------------------------------------
+ * This routine does the setup operations for the dense linear solver.
+ * It calls the Jacobian evaluation routine and, depending on ftype,
+ * it performs various factorizations.
+ * -----------------------------------------------------------------
+ */
+
+static int cpDenseProjSetup(CPodeMem cp_mem, N_Vector y, N_Vector cy,
+ N_Vector c_tmp1, N_Vector c_tmp2, N_Vector s_tmp1)
+{
+ long int ier;
+ CPDlsProjMem cpdlsP_mem;
+ realtype **g_mat, *col_i, *s_tmpd;
+ long int i, j, k;
+ int retval;
+
+ cpdlsP_mem = (CPDlsProjMem) lmemP;
+
+ g_mat = G->cols;
+
+ /*
+ * Initialize Jacobian matrix to 0 and call Jacobian function
+ * G will contain the Jacobian transposed.
+ */
+ DenseZero(G);
+ retval = jacP(nc, ny, tn, y, cy, G, JP_data, c_tmp1, c_tmp2);
+ njeP++;
+ if (retval < 0) {
+ cpProcessError(cp_mem, CPDIRECT_JACFUNC_UNRECVR, "CPDENSE", "cpDenseProjSetup", MSGD_JACFUNC_FAILED);
+ return(-1);
+ } else if (retval > 0) {
+ return(1);
+ }
+
+ /* Save Jacobian before factorization for possible use by lmultP */
+ DenseCopy(G, savedG);
+
+ /* Factorize G, depending on ftype */
+ switch (ftype) {
+
+ case CPDIRECT_LU:
+
+ /*
+ * LU factorization of G^T
+ *
+ * P*G^T = | U1^T | * L^T
+ * | U2^T |
+ *
+ * After factorization, P is encoded in pivotsP and
+ * G^T is overwritten with U1 (nc by nc unit upper triangular),
+ * U2 ( nc by ny-nc rectangular), and L (nc by nc lower triangular).
+ *
+ * Return 1 if factorization failed.
+ */
+ ier = DenseGETRF(G, pivotsP);
+ if (ier > 0) return(1);
+
+ /*
+ * Build S = U1^{-1} * U2 (in place, S overwrites U2)
+ * For each row j of G, j = nc,...,ny-1, perform
+ * a backward substitution (row version).
+ *
+ * After this step, G^T contains U1, S, and L.
+ */
+ for (j=nc; j<ny; j++) {
+ for (i=nc-2; i>=0; i--) {
+ col_i = g_mat[i];
+ for (k=i+1; k<nc; k++) g_mat[i][j] -= col_i[k]*g_mat[k][j];
+ }
+ }
+
+ /*
+ * Build K = D1 + S^T * D2 * S
+ * Compute and store only the lower triangular part of K.
+ */
+ if (pnorm == CP_PROJ_L2NORM) cpdLUcomputeKI(cp_mem);
+ else cpdLUcomputeKD(cp_mem, s_tmp1);
+
+ /*
+ * Perform Cholesky decomposition of K (in place, gaxpy version)
+ *
+ * K = C*C^T
+ *
+ * After factorization, the lower triangular part of K contains C.
+ *
+ * Return 1 if factorization failed.
+ */
+ ier = DensePOTRF(K);
+ if (ier > 0) return(1);
+
+ break;
+
+ case CPDIRECT_QR:
+
+ /*
+ * Thin QR factorization of G^T
+ *
+ * G^T = Q * R
+ *
+ * After factorization, the upper trianguler part of G^T
+ * contains the matrix R. The lower trapezoidal part of
+ * G^T, together with the array beta, encodes the orthonormal
+ * columns of Q as elementary reflectors.
+ */
+
+ /* Use s_tmp1 as workspace */
+ s_tmpd = N_VGetArrayPointer(s_tmp1);
+ ier = DenseGEQRF(G, beta, s_tmpd);
+
+ /* If projecting in WRMS norm */
+ if (pnorm == CP_PROJ_ERRNORM) {
+ /* Build K = Q^T * D^(-1) * Q */
+ cpdQRcomputeKD(cp_mem, s_tmp1);
+ /* Perform Cholesky decomposition of K */
+ ier = DensePOTRF(K);
+ if (ier > 0) return(1);
+ }
+
+ break;
+
+ case CPDIRECT_SC:
+
+ /*
+ * Build K = G*D^(-1)*G^T
+ * Compute and store only the lower triangular part of K.
+ */
+ if (pnorm == CP_PROJ_L2NORM) cpdSCcomputeKI(cp_mem);
+ else cpdSCcomputeKD(cp_mem, s_tmp1);
+
+ /*
+ * Perform Cholesky decomposition of K (in place, gaxpy version)
+ *
+ * K = C*C^T
+ *
+ * After factorization, the lower triangular part of K contains C.
+ *
+ * Return 1 if factorization failed.
+ */
+ ier = DensePOTRF(K);
+ if (ier > 0) return(1);
+
+ break;
+
+ }
+
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpDenseProjSolve
+ * -----------------------------------------------------------------
+ * This routine handles the solve operation for the dense linear
+ * solver. The returned value is 0.
+ * -----------------------------------------------------------------
+ */
+
+static int cpDenseProjSolve(CPodeMem cp_mem, N_Vector b, N_Vector x,
+ N_Vector y, N_Vector cy,
+ N_Vector c_tmp1, N_Vector s_tmp1)
+{
+ CPDlsProjMem cpdlsP_mem;
+ realtype **g_mat, *bd, *xd, *col_i, *s_tmpd;
+ realtype *ewt_data, *d_data, *da_data, tmp;
+ long int nd, i, k, pk;
+
+ cpdlsP_mem = (CPDlsProjMem) lmemP;
+
+ g_mat = G->cols;
+
+ ewt_data = N_VGetArrayPointer(ewt);
+ bd = N_VGetArrayPointer(b);
+ xd = N_VGetArrayPointer(x);
+ d_data = N_VGetArrayPointer(s_tmp1);
+
+ nd = ny - nc;
+
+ /* Solve the linear system, depending on ftype */
+ switch (ftype) {
+
+ case CPDIRECT_LU:
+
+ /*
+ * Solve L*U1*alpha = bd
+ * (a) solve L*beta = bd using fwd. subst. (row version)
+ * (b) solve U1*alpha = beta using bckwd. subst (row version)
+ * where L^T and U1^T are stored in G[0...nc-1][0...nc-1].
+ * beta and then alpha overwrite bd.
+ */
+ bd[0] /= g_mat[0][0];
+ for (i=1; i<nc; i++) {
+ col_i = g_mat[i];
+ for (k=0; k<i; k++) bd[i] -= col_i[k]*bd[k];
+ bd[i] /= col_i[i];
+ }
+ for (i=nc-2; i>=0; i--) {
+ col_i = g_mat[i];
+ for (k=i+1; k<nc; k++) bd[i] -= col_i[k]*bd[k];
+ }
+
+ /*
+ * Compute S^T * (D1 * alpha)
+ * alpha is stored in bd.
+ * S^T is stored in g_mat[nc...ny-1][0...nc-1].
+ * Store result in x2 = x[nc...ny-1].
+ */
+ if (pnorm == CP_PROJ_ERRNORM) {
+
+ /* Load squared error weights into d */
+ for (k=0; k<ny; k++) d_data[k] = ewt_data[k] * ewt_data[k];
+ /* Permute elements of d, based on pivotsP. Swap d[k] and d[pivotsP[k]]. */
+ for (k=0; k<nc; k++) {
+ pk = pivotsP[k];
+ if (pk != k) {
+ tmp = d_data[k];
+ d_data[k] = d_data[pk];
+ d_data[pk] = tmp;
+ }
+ }
+ /* Compute D1*alpha and store it into da_data */
+ da_data = N_VGetArrayPointer(c_tmp1);
+ for(k=0; k<nc; k++) da_data[k] = d_data[k] * bd[k];
+ /* Compute S^T * D1 * alpha = S^T * da */
+ for(i=0; i<nd; i++) {
+ xd[nc+i] = ZERO;
+ for(k=0; k<nc; k++) xd[nc+i] += g_mat[k][nc+i]*da_data[k];
+ }
+
+ } else {
+
+ /* Compute S^T * alpha */
+ for(i=0; i<nd; i++) {
+ xd[nc+i] = ZERO;
+ for(k=0; k<nc; k++) xd[nc+i] += g_mat[k][nc+i]*bd[k];
+ }
+
+ }
+
+ /*
+ * Solve K*x2 = S^T*D1*alpha, using the Cholesky decomposition available in K.
+ * S^T*D1*alpha is stored in x2 = x[nc...ny-1].
+ */
+ DensePOTRS(K, &xd[nc]);
+
+ /*
+ * Compute x1 = alpha - S*x2
+ * alpha is stored in bd.
+ * x2 is stored in x[nc...ny-1].
+ * S^T is stored in g_mat[nc...ny-1][0...nc-1].
+ * Store result in x1 = x[0...nc-1].
+ */
+ for (i=0; i<nc; i++) {
+ xd[i] = bd[i];
+ col_i = g_mat[i];
+ for (k=nc; k<ny; k++) xd[i] -= col_i[k]*xd[k];
+ }
+
+ /*
+ * Compute P^T * x, where P is encoded into pivotsP.
+ * Store result in x.
+ */
+ for (k=nc-1; k>=0; k--) {
+ pk = pivotsP[k];
+ if(pk != k) {
+ tmp = xd[k];
+ xd[k] = xd[pk];
+ xd[pk] = tmp;
+ }
+ }
+
+
+ break;
+
+ case CPDIRECT_QR:
+
+ /*
+ * Solve R^T*alpha = bd using fwd. subst. (row version)
+ * alpha overwrites bd.
+ */
+ bd[0] /= g_mat[0][0];
+ for (i=1; i<nc; i++) {
+ col_i = g_mat[i];
+ for (k=0; k<i; k++) bd[i] -= bd[k]*col_i[k];
+ bd[i] /= col_i[i];
+ }
+
+ /* If projecting in WRMS norm, solve K*beta = alpha */
+ if (pnorm == CP_PROJ_ERRNORM) DensePOTRS(K, bd);
+
+ /* Compute x = Q*alpha */
+ s_tmpd = N_VGetArrayPointer(s_tmp1);
+ DenseORMQR(G, beta, bd, xd, s_tmpd);
+
+ /* If projecting in WRMS norm, scale x by D^(-1) */
+ if (pnorm == CP_PROJ_ERRNORM) {
+ for (i=0; i<ny; i++)
+ xd[i] /= ewt_data[i]*ewt_data[i];
+ }
+
+ break;
+
+ case CPDIRECT_SC:
+
+ /*
+ * Solve K*xi = bd, using the Cholesky decomposition available in K.
+ * xi overwrites bd.
+ */
+ DensePOTRS(K, bd);
+
+ /* Compute x = G^T * xi
+ * G^T is stored in g_mat[0...ny-1][0...nc-1]
+ */
+ for(i=0; i<ny; i++) {
+ xd[i] = ZERO;
+ for(k=0; k<nc; k++) xd[i] += g_mat[k][i]*bd[k];
+ }
+
+ /* If projecting in WRMS norm, scale x by D^(-1) */
+ if (pnorm == CP_PROJ_ERRNORM) {
+ for (i=0; i<ny; i++)
+ xd[i] /= ewt_data[i]*ewt_data[i];
+ }
+
+ break;
+
+ }
+
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpDenseProjMult
+ * -----------------------------------------------------------------
+ * This routine computes the Jacobian-vector product used a saved
+ * Jacobian copy.
+ * -----------------------------------------------------------------
+ */
+
+static void cpDenseProjMult(CPodeMem cp_mem, N_Vector x, N_Vector Gx)
+{
+ CPDlsProjMem cpdlsP_mem;
+ realtype **g_mat, *col_j, *x_d, *Gx_d;
+ long int j, k;
+
+ cpdlsP_mem = (CPDlsProjMem) lmemP;
+
+ g_mat = savedG->cols;
+ x_d = N_VGetArrayPointer(x);
+ Gx_d = N_VGetArrayPointer(Gx);
+
+ for (j=0; j<nc; j++) {
+ col_j = g_mat[j];
+ Gx_d[j] = ZERO;
+ for (k=0; k<ny; k++) Gx_d[j] += col_j[k]*x_d[k];
+ }
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpDenseProjFree
+ * -----------------------------------------------------------------
+ * This routine frees memory specific to the dense linear solver.
+ * -----------------------------------------------------------------
+ */
+
+static void cpDenseProjFree(CPodeMem cp_mem)
+{
+ CPDlsProjMem cpdlsP_mem;
+
+ cpdlsP_mem = (CPDlsProjMem) lmemP;
+
+ DestroyMat(G);
+ DestroyMat(savedG);
+ switch (ftype) {
+ case CPDIRECT_LU:
+ DestroyArray(pivotsP);
+ DestroyMat(K);
+ break;
+ case CPDIRECT_QR:
+ DestroyArray(beta);
+ if (pnorm == CP_PROJ_ERRNORM) DestroyMat(K);
+ break;
+ case CPDIRECT_SC:
+ DestroyMat(K);
+ break;
+ }
+
+ free(cpdlsP_mem);
+ cpdlsP_mem = NULL;
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Private functions for LU-, QR-, and SC-based projection
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * Compute the lower triangle of K = I + S^T*S
+ */
+static void cpdLUcomputeKI(CPodeMem cp_mem)
+{
+ CPDlsProjMem cpdlsP_mem;
+ realtype **g_mat, **k_mat, *k_col_j;
+ long int nd, i, j, k;
+
+ cpdlsP_mem = (CPDlsProjMem) lmemP;
+
+ g_mat = G->cols;
+ k_mat = K->cols;
+
+ nd = ny-nc;
+
+ /* Load K column by column */
+ for (j=0; j<nd; j++) {
+
+ k_col_j = k_mat[j];
+
+ for (i=j; i<nd; i++) {
+ k_col_j[i] = ZERO;
+ for (k=0; k<nc; k++) k_col_j[i] += g_mat[k][nc+i]*g_mat[k][nc+j];
+ }
+
+ k_col_j[j] += ONE;
+
+ }
+
+}
+
+/*
+ * Compute the lower triangle of K = D1 + S^T*D2*S,
+ * D = diag(D1, D2) = P*W*P^T, W is a diagonal matrix
+ * containing the squared error weights, and P is the
+ * permutation matrix encoded into pivotsP.
+ * D1 has length nc and D2 has length (ny-nc).
+ */
+static void cpdLUcomputeKD(CPodeMem cp_mem, N_Vector d)
+{
+ CPDlsProjMem cpdlsP_mem;
+ realtype *d_data, *ewt_data, tmp;
+ realtype **g_mat, **k_mat, *k_col_j;
+ long int nd, i, j, k, pk;
+
+ cpdlsP_mem = (CPDlsProjMem) lmemP;
+
+ ewt_data = N_VGetArrayPointer(ewt);
+ d_data = N_VGetArrayPointer(d);
+
+ g_mat = G->cols;
+ k_mat = K->cols;
+
+ nd = ny-nc;
+
+ /* Load squared error weights into d */
+ for (k=0; k<ny; k++) d_data[k] = ewt_data[k] * ewt_data[k];
+
+ /* Permute elements of d, based on pivotsP. Swap d[k] and d[pivotsP[k]]. */
+ for (k=0; k<nc; k++) {
+ pk = pivotsP[k];
+ if (pk != k) {
+ tmp = d_data[k];
+ d_data[k] = d_data[pk];
+ d_data[pk] = tmp;
+ }
+ }
+
+ /* load K column by column */
+ for (j=0; j<nd; j++) {
+
+ k_col_j = k_mat[j];
+
+ for (i=j; i<nd; i++) {
+ k_col_j[i] = ZERO;
+ for(k=0; k<nc; k++)
+ k_col_j[i] += g_mat[k][nc+i] * d_data[k]*d_data[k] * g_mat[k][nc+j];
+ }
+
+ k_col_j[j] += d_data[j]*d_data[j];
+
+ }
+
+}
+
+static void cpdQRcomputeKD(CPodeMem cp_mem, N_Vector d)
+{
+ /* RADU:: implement this ... */
+}
+
+
+/*
+ * Compute the lower triangle of K = G * G^T
+ * G^T is available in g_mat[0...ny][0...nc].
+ */
+static void cpdSCcomputeKI(CPodeMem cp_mem)
+{
+ CPDlsProjMem cpdlsP_mem;
+ realtype **g_mat, **k_mat, *k_col_j;
+ long int i, j, k;
+
+ cpdlsP_mem = (CPDlsProjMem) lmemP;
+
+ g_mat = G->cols;
+ k_mat = K->cols;
+
+ /* Load K column by column */
+ for (j=0; j<nc; j++) {
+ k_col_j = k_mat[j];
+ for (i=0; i<nc; i++) {
+ k_col_j[i] = ZERO;
+ for (k=0; k<ny; k++) k_col_j[i] += g_mat[i][k]*g_mat[j][k];
+ }
+ }
+
+}
+
+static void cpdSCcomputeKD(CPodeMem cp_mem, N_Vector d)
+{
+ /* RADU:: implement this ... */
+}
+
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_direct.c b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_direct.c
new file mode 100644
index 0000000..844a7ae
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_direct.c
@@ -0,0 +1,850 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.1 $
+ * $Date: 2006/11/22 00:12:48 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for the CPDIRECT linear solvers
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * =================================================================
+ * IMPORTED HEADER FILES
+ * =================================================================
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "cpodes_direct_impl.h"
+#include "cpodes_private.h"
+
+#include <sundials/sundials_math.h>
+
+/*
+ * =================================================================
+ * FUNCTION SPECIFIC CONSTANTS
+ * =================================================================
+ */
+
+/* Constant for DQ Jacobian approximation */
+#define MIN_INC_MULT RCONST(1000.0)
+
+#define ZERO RCONST(0.0)
+#define ONE RCONST(1.0)
+#define TWO RCONST(2.0)
+
+/*
+ * =================================================================
+ * READIBILITY REPLACEMENTS
+ * =================================================================
+ */
+
+#define ode_type (cp_mem->cp_ode_type)
+#define fe (cp_mem->cp_fe)
+#define fi (cp_mem->cp_fi)
+#define f_data (cp_mem->cp_f_data)
+#define uround (cp_mem->cp_uround)
+#define nst (cp_mem->cp_nst)
+#define tn (cp_mem->cp_tn)
+#define h (cp_mem->cp_h)
+#define gamma (cp_mem->cp_gamma)
+#define gammap (cp_mem->cp_gammap)
+#define gamrat (cp_mem->cp_gamrat)
+#define ewt (cp_mem->cp_ewt)
+#define lmem (cp_mem->cp_lmem)
+
+#define cfun (cp_mem->cp_cfun)
+#define c_data (cp_mem->cp_c_data)
+#define lmemP (cp_mem->cp_lmemP)
+
+#define mtype (cpdls_mem->d_type)
+#define n (cpdls_mem->d_n)
+#define ml (cpdls_mem->d_ml)
+#define mu (cpdls_mem->d_mu)
+#define smu (cpdls_mem->d_smu)
+#define djacE (cpdls_mem->d_djacE)
+#define djacI (cpdls_mem->d_djacI)
+#define bjacE (cpdls_mem->d_bjacE)
+#define bjacI (cpdls_mem->d_bjacI)
+#define M (cpdls_mem->d_M)
+#define savedJ (cpdls_mem->d_savedJ)
+#define pivots (cpdls_mem->d_pivots)
+#define nstlj (cpdls_mem->d_nstlj)
+#define nje (cpdls_mem->d_nje)
+#define nfeDQ (cpdls_mem->d_nfeDQ)
+#define J_data (cpdls_mem->d_J_data)
+#define last_flag (cpdls_mem->d_last_flag)
+
+#define djacP (cpdlsP_mem->d_jacP)
+#define JP_data (cpdlsP_mem->d_JP_data)
+#define njeP (cpdlsP_mem->d_njeP)
+#define nceDQ (cpdlsP_mem->d_nceDQ)
+
+/*
+ * =================================================================
+ * EXPORTED FUNCTIONS FOR IMPLICIT INTEGRATION
+ * =================================================================
+ */
+
+/*
+ * CPDlsSetJacFn specifies the (dense or band) Jacobian function.
+ */
+int CPDlsSetJacFn(void *cpode_mem, void *jac, void *jac_data)
+{
+ CPodeMem cp_mem;
+ CPDlsMem cpdls_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPDIRECT_MEM_NULL, "CPDIRECT", "CPDlsSetJacFn", MSGD_CPMEM_NULL);
+ return(CPDIRECT_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmem == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_LMEM_NULL, "CPDIRECT", "CPDlsSetJacFn", MSGD_LMEM_NULL);
+ return(CPDIRECT_LMEM_NULL);
+ }
+ cpdls_mem = (CPDlsMem) lmem;
+
+ switch (ode_type) {
+
+ case CP_EXPL:
+ if (mtype == SUNDIALS_DENSE)
+ djacE = (CPDlsDenseJacExplFn) jac;
+ else if (mtype == SUNDIALS_BAND)
+ bjacE = (CPDlsBandJacExplFn) jac;
+ break;
+
+ case CP_IMPL:
+ if (mtype == SUNDIALS_DENSE)
+ djacI = (CPDlsDenseJacImplFn) jac;
+ else if (mtype == SUNDIALS_BAND)
+ bjacI = (CPDlsBandJacImplFn) jac;
+ break;
+
+ }
+
+ J_data = jac_data;
+
+ return(CPDIRECT_SUCCESS);
+}
+
+/*
+ * CPDlsGetWorkSpace returns the length of workspace allocated for the
+ * CPDIRECT linear solver.
+ */
+int CPDlsGetWorkSpace(void *cpode_mem, long int *lenrwLS, long int *leniwLS)
+{
+ CPodeMem cp_mem;
+ CPDlsMem cpdls_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPDIRECT_MEM_NULL, "CPDIRECT", "CPDlsGetWorkSpace", MSGD_CPMEM_NULL);
+ return(CPDIRECT_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmem == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_LMEM_NULL, "CPDIRECT", "CPDlsGetWorkSpace", MSGD_LMEM_NULL);
+ return(CPDIRECT_LMEM_NULL);
+ }
+ cpdls_mem = (CPDlsMem) lmem;
+
+ switch (ode_type) {
+
+ case CP_EXPL:
+
+ if (mtype == SUNDIALS_DENSE) {
+ *lenrwLS = 2*n*n;
+ *leniwLS = n;
+ } else if (mtype == SUNDIALS_BAND) {
+ *lenrwLS = n*(smu + mu + 2*ml + 2);
+ *leniwLS = n;
+ }
+
+ break;
+
+ case CP_IMPL:
+
+ if (mtype == SUNDIALS_DENSE) {
+ *lenrwLS = n*n;
+ *leniwLS = n;
+ } else if (mtype == SUNDIALS_BAND) {
+ *lenrwLS = n*(smu + ml + 2);
+ *leniwLS = n;
+ }
+
+ break;
+
+ }
+
+ return(CPDIRECT_SUCCESS);
+}
+
+/*
+ * CPDlsGetNumJacEvals returns the number of Jacobian evaluations.
+ */
+int CPDlsGetNumJacEvals(void *cpode_mem, long int *njevals)
+{
+ CPodeMem cp_mem;
+ CPDlsMem cpdls_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPDIRECT_MEM_NULL, "CPDIRECT", "CPDlsGetNumJacEvals", MSGD_CPMEM_NULL);
+ return(CPDIRECT_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmem == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_LMEM_NULL, "CPDIRECT", "CPDlsGetNumJacEvals", MSGD_LMEM_NULL);
+ return(CPDIRECT_LMEM_NULL);
+ }
+ cpdls_mem = (CPDlsMem) lmem;
+
+ *njevals = nje;
+
+ return(CPDIRECT_SUCCESS);
+}
+
+/*
+ * CPDlsGetNumFctEvals returns the number of calls to the ODE function
+ * needed for the DQ Jacobian approximation.
+ */
+int CPDlsGetNumFctEvals(void *cpode_mem, long int *nfevalsLS)
+{
+ CPodeMem cp_mem;
+ CPDlsMem cpdls_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPDIRECT_MEM_NULL, "CPDIRECT", "CPDlsGetNumRhsEvals", MSGD_CPMEM_NULL);
+ return(CPDIRECT_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmem == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_LMEM_NULL, "CPDIRECT", "CPDlsGetNumRhsEvals", MSGD_LMEM_NULL);
+ return(CPDIRECT_LMEM_NULL);
+ }
+ cpdls_mem = (CPDlsMem) lmem;
+
+ *nfevalsLS = nfeDQ;
+
+ return(CPDIRECT_SUCCESS);
+}
+
+/*
+ * CPDlsGetReturnFlagName returns the name associated with a CPDIRECT
+ * return value.
+ */
+char *CPDlsGetReturnFlagName(int flag)
+{
+ char *name;
+
+ name = (char *)malloc(30*sizeof(char));
+
+ switch(flag) {
+ case CPDIRECT_SUCCESS:
+ sprintf(name,"CPDIRECT_SUCCESS");
+ break;
+ case CPDIRECT_MEM_NULL:
+ sprintf(name,"CPDIRECT_MEM_NULL");
+ break;
+ case CPDIRECT_LMEM_NULL:
+ sprintf(name,"CPDIRECT_LMEM_NULL");
+ break;
+ case CPDIRECT_ILL_INPUT:
+ sprintf(name,"CPDIRECT_ILL_INPUT");
+ break;
+ case CPDIRECT_MEM_FAIL:
+ sprintf(name,"CPDIRECT_MEM_FAIL");
+ break;
+ case CPDIRECT_JACFUNC_UNRECVR:
+ sprintf(name,"CPDIRECT_JACFUNC_UNRECVR");
+ break;
+ case CPDIRECT_JACFUNC_RECVR:
+ sprintf(name,"CPDIRECT_JACFUNC_RECVR");
+ break;
+ default:
+ sprintf(name,"NONE");
+ }
+
+ return(name);
+}
+
+/*
+ * CPDlsGetLastFlag returns the last flag set in a CPDIRECT function.
+ */
+int CPDlsGetLastFlag(void *cpode_mem, int *flag)
+{
+ CPodeMem cp_mem;
+ CPDlsMem cpdls_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPDIRECT_MEM_NULL, "CPDIRECT", "CPDlsGetLastFlag", MSGD_CPMEM_NULL);
+ return(CPDIRECT_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmem == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_LMEM_NULL, "CPDIRECT", "CPDlsGetLastFlag", MSGD_LMEM_NULL);
+ return(CPDIRECT_LMEM_NULL);
+ }
+ cpdls_mem = (CPDlsMem) lmem;
+
+ *flag = last_flag;
+
+ return(CPDIRECT_SUCCESS);
+}
+
+/*
+ * =================================================================
+ * EXPORTED FUNCTIONS FOR PROJECTION
+ * =================================================================
+ */
+
+/*
+ * CPDlsProjSetJacFn specifies the constraint Jacobian function.
+ */
+int CPDlsProjSetJacFn(void *cpode_mem, void *jacP, void *jacP_data)
+{
+ CPodeMem cp_mem;
+ CPDlsProjMem cpdlsP_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPDIRECT_MEM_NULL, "CPDIRECT", "CPDlsProjSetJacFn", MSGD_CPMEM_NULL);
+ return(CPDIRECT_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmemP == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_LMEM_NULL, "CPDIRECT", "CPDlsProjSetJacFn", MSGD_LMEM_NULL);
+ return(CPDIRECT_LMEM_NULL);
+ }
+ cpdlsP_mem = (CPDlsProjMem) lmemP;
+
+ djacP = (CPDlsDenseProjJacFn) jacP;
+ JP_data = jacP_data;
+
+ return(CPDIRECT_SUCCESS);
+}
+
+/*
+ * CPDlsProjGetNumJacEvals returns the number of constraint Jacobian evaluations
+ */
+int CPDlsProjGetNumJacEvals(void *cpode_mem, long int *njPevals)
+{
+ CPodeMem cp_mem;
+ CPDlsProjMem cpdlsP_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPDIRECT_MEM_NULL, "CPDIRECT", "CPDlsProjGetNumJacEvals", MSGD_CPMEM_NULL);
+ return(CPDIRECT_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmemP == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_LMEM_NULL, "CPDIRECT", "CPDlsProjGetNumJacEvals", MSGD_LMEM_NULL);
+ return(CPDIRECT_LMEM_NULL);
+ }
+ cpdlsP_mem = (CPDlsProjMem) lmemP;
+
+ *njPevals = njeP;
+
+ return(CPDIRECT_SUCCESS);
+}
+
+/*
+ * CPDlsProjGetNumFctEvals returns the number of constraint function
+ * evaluations for computing the DQ constraint Jacobian.
+ */
+int CPDlsProjGetNumFctEvals(void *cpode_mem, long int *ncevalsLS)
+{
+ CPodeMem cp_mem;
+ CPDlsProjMem cpdlsP_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPDIRECT_MEM_NULL, "CPDIRECT", "CPDlsProjGetNumFctEvals", MSGD_CPMEM_NULL);
+ return(CPDIRECT_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmemP == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_LMEM_NULL, "CPDIRECT", "CPDlsProjGetNumFctEvals", MSGD_LMEM_NULL);
+ return(CPDIRECT_LMEM_NULL);
+ }
+ cpdlsP_mem = (CPDlsProjMem) lmemP;
+
+ *ncevalsLS = nceDQ;
+
+ return(CPDIRECT_SUCCESS);
+}
+
+/*
+ * =================================================================
+ * DENSE DQ JACOBIAN APPROXIMATIONS FOR IMPLICIT INTEGRATION
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * cpDlsDenseDQJacExpl
+ * -----------------------------------------------------------------
+ * This routine generates a dense difference quotient approximation to
+ * the Jacobian of f(t,y). It assumes that a dense matrix of type
+ * DlsMat is stored column-wise, and that elements within each column
+ * are contiguous. The address of the jth column of J is obtained via
+ * the macro DENSE_COL and this pointer is associated with an N_Vector
+ * using the N_VGetArrayPointer/N_VSetArrayPointer functions.
+ * Finally, the actual computation of the jth column of the Jacobian is
+ * done with a call to N_VLinearSum.
+ * -----------------------------------------------------------------
+ */
+
+int cpDlsDenseDQJacExpl(int N, realtype t,
+ N_Vector y, N_Vector fy,
+ DlsMat Jac, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ realtype fnorm, minInc, inc, inc_inv, yjsaved, srur;
+ realtype *tmp2_data, *y_data, *ewt_data;
+ N_Vector ftemp, jthCol;
+ int j;
+ int retval = 0;
+
+ CPodeMem cp_mem;
+ CPDlsMem cpdls_mem;
+
+ /* jac_data points to cpode_mem */
+ cp_mem = (CPodeMem) jac_data;
+ cpdls_mem = (CPDlsMem) lmem;
+
+ /* Save pointer to the array in tmp2 */
+ tmp2_data = N_VGetArrayPointer(tmp2);
+
+ /* Rename work vectors for readibility */
+ ftemp = tmp1;
+ jthCol = tmp2;
+
+ /* Obtain pointers to the data for ewt, y */
+ ewt_data = N_VGetArrayPointer(ewt);
+ y_data = N_VGetArrayPointer(y);
+
+ /* Set minimum increment based on uround and norm of f */
+ srur = RSqrt(uround);
+ fnorm = N_VWrmsNorm(fy, ewt);
+ minInc = (fnorm != ZERO) ?
+ (MIN_INC_MULT * ABS(h) * uround * N * fnorm) : ONE;
+
+ for (j = 0; j < N; j++) {
+
+ /* Generate the jth col of J(tn,y) */
+
+ N_VSetArrayPointer(DENSE_COL(Jac,j), jthCol);
+
+ yjsaved = y_data[j];
+ inc = MAX(srur*ABS(yjsaved), minInc/ewt_data[j]);
+ y_data[j] += inc;
+
+ retval = fe(t, y, ftemp, f_data);
+ nfeDQ++;
+ if (retval != 0) break;
+
+ y_data[j] = yjsaved;
+
+ inc_inv = ONE/inc;
+ N_VLinearSum(inc_inv, ftemp, -inc_inv, fy, jthCol);
+
+ DENSE_COL(Jac,j) = N_VGetArrayPointer(jthCol);
+ }
+
+ /* Restore original array pointer in tmp2 */
+ N_VSetArrayPointer(tmp2_data, tmp2);
+
+ return(retval);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpDlsDenseDQJacImpl
+ * -----------------------------------------------------------------
+ * This routine generates a dense difference quotient approximation to
+ * the Jacobian F_y' + gamma*F_y. It assumes that a dense matrix of type
+ * DlsMat is stored column-wise, and that elements within each column
+ * are contiguous. The address of the jth column of J is obtained via
+ * the macro DENSE_COL and this pointer is associated with an N_Vector
+ * using the N_VGetArrayPointer/N_VSetArrayPointer functions.
+ * Finally, the actual computation of the jth column of the Jacobian is
+ * done with a call to N_VLinearSum.
+ * -----------------------------------------------------------------
+ */
+int cpDlsDenseDQJacImpl(int N, realtype t, realtype gm,
+ N_Vector y, N_Vector yp, N_Vector r,
+ DlsMat Jac, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ realtype inc, inc_inv, yj, ypj, srur;
+ realtype *tmp2_data, *y_data, *yp_data, *ewt_data;
+ N_Vector ftemp, jthCol;
+ int j;
+ int retval = 0;
+
+ CPodeMem cp_mem;
+ CPDlsMem cpdls_mem;
+
+ /* jac_data points to cpode_mem */
+ cp_mem = (CPodeMem) jac_data;
+ cpdls_mem = (CPDlsMem) lmem;
+
+ /* Save pointer to the array in tmp2 */
+ tmp2_data = N_VGetArrayPointer(tmp2);
+
+ /* Rename work vectors for readibility */
+ ftemp = tmp1;
+ jthCol = tmp2;
+
+ /* Obtain pointers to the data for ewt, y, and yp */
+ ewt_data = N_VGetArrayPointer(ewt);
+ y_data = N_VGetArrayPointer(y);
+ yp_data = N_VGetArrayPointer(yp);
+
+ /* Set minimum increment based on uround and norm of f */
+ srur = RSqrt(uround);
+
+ /* Generate each column of the Jacobian M = F_y' + gamma * F_y
+ as delta(F)/delta(y_j). */
+ for (j = 0; j < N; j++) {
+
+ /* Set data address of jthCol, and save y_j and yp_j values. */
+ N_VSetArrayPointer(DENSE_COL(Jac,j), jthCol);
+ yj = y_data[j];
+ ypj = yp_data[j];
+
+ /* Set increment inc to y_j based on sqrt(uround)*abs(y_j), with
+ adjustments using yp_j and ewt_j if this is small, and a further
+ adjustment to give it the same sign as h*yp_j. */
+ inc = MAX( srur * MAX( ABS(yj), ABS(h*ypj) ) , ONE/ewt_data[j] );
+ if (h*ypj < ZERO) inc = -inc;
+ inc = (yj + inc) - yj;
+
+ /* Increment y_j and yp_j, call res, and break on error return. */
+ y_data[j] += gamma*inc;
+ yp_data[j] += inc;
+ retval = fi(t, y, yp, ftemp, f_data);
+ nfeDQ++;
+ if (retval != 0) break;
+
+ /* Generate the jth col of J(tn,y) */
+ inc_inv = ONE/inc;
+ N_VLinearSum(inc_inv, ftemp, -inc_inv, r, jthCol);
+
+ DENSE_COL(Jac,j) = N_VGetArrayPointer(jthCol);
+
+ /* Reset y_j, yp_j */
+ y_data[j] = yj;
+ yp_data[j] = ypj;
+ }
+
+ /* Restore original array pointer in tmp2 */
+ N_VSetArrayPointer(tmp2_data, tmp2);
+
+ return(retval);
+}
+
+/*
+ * =================================================================
+ * BAND DQ JACOBIAN APPROXIMATIONS FOR IMPLICIT INTEGRATION
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * cpDlsBandDQJacExpl
+ * -----------------------------------------------------------------
+ * This routine generates a banded difference quotient approximation to
+ * the Jacobian of f(t,y). It assumes that a band matrix of type
+ * DlsMat is stored column-wise, and that elements within each column
+ * are contiguous. This makes it possible to get the address of a column
+ * of J via the macro BAND_COL and to write a simple for loop to set
+ * each of the elements of a column in succession.
+ * -----------------------------------------------------------------
+ */
+
+int cpDlsBandDQJacExpl(int N, int mupper, int mlower,
+ realtype t, N_Vector y, N_Vector fy,
+ DlsMat Jac, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ N_Vector ftemp, ytemp;
+ realtype fnorm, minInc, inc, inc_inv, srur;
+ realtype *col_j, *ewt_data, *fy_data, *ftemp_data, *y_data, *ytemp_data;
+ int group, i, j, width, ngroups, i1, i2;
+ int retval = 0;
+
+ CPodeMem cp_mem;
+ CPDlsMem cpdls_mem;
+
+ /* jac_dat points to cpode_mem */
+ cp_mem = (CPodeMem) jac_data;
+ cpdls_mem = (CPDlsMem) lmem;
+
+ /* Rename work vectors for use as temporary values of y and f */
+ ftemp = tmp1;
+ ytemp = tmp2;
+
+ /* Obtain pointers to the data for ewt, fy, ftemp, y, ytemp */
+ ewt_data = N_VGetArrayPointer(ewt);
+ fy_data = N_VGetArrayPointer(fy);
+ ftemp_data = N_VGetArrayPointer(ftemp);
+ y_data = N_VGetArrayPointer(y);
+ ytemp_data = N_VGetArrayPointer(ytemp);
+
+ /* Load ytemp with y = predicted y vector */
+ N_VScale(ONE, y, ytemp);
+
+ /* Set minimum increment based on uround and norm of f */
+ srur = RSqrt(uround);
+ fnorm = N_VWrmsNorm(fy, ewt);
+ minInc = (fnorm != ZERO) ?
+ (MIN_INC_MULT * ABS(h) * uround * N * fnorm) : ONE;
+
+ /* Set bandwidth and number of column groups for band differencing */
+ width = mlower + mupper + 1;
+ ngroups = MIN(width, N);
+
+ /* Loop over column groups. */
+ for (group=1; group <= ngroups; group++) {
+
+ /* Increment all y_j in group */
+ for(j=group-1; j < N; j+=width) {
+ inc = MAX(srur*ABS(y_data[j]), minInc/ewt_data[j]);
+ ytemp_data[j] += inc;
+ }
+
+ /* Evaluate f with incremented y */
+
+ retval = fe(tn, ytemp, ftemp, f_data);
+ nfeDQ++;
+ if (retval != 0) break;
+
+ /* Restore ytemp, then form and load difference quotients */
+ for (j=group-1; j < N; j+=width) {
+ ytemp_data[j] = y_data[j];
+ col_j = BAND_COL(Jac,j);
+ inc = MAX(srur*ABS(y_data[j]), minInc/ewt_data[j]);
+ inc_inv = ONE/inc;
+ i1 = MAX(0, j-mupper);
+ i2 = MIN(j+mlower, N-1);
+ for (i=i1; i <= i2; i++)
+ BAND_COL_ELEM(col_j,i,j) = inc_inv * (ftemp_data[i] - fy_data[i]);
+ }
+ }
+
+ return(retval);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpDlsBandDQJacImpl
+ * -----------------------------------------------------------------
+ */
+
+int cpDlsBandDQJacImpl(int N, int mupper, int mlower,
+ realtype t, realtype gm,
+ N_Vector y, N_Vector yp, N_Vector r,
+ DlsMat Jac, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ N_Vector ftemp, ytemp, yptemp;
+ realtype inc, inc_inv, yj, ypj, srur, ewtj;
+ realtype *y_data, *yp_data, *ewt_data;
+ realtype *ytemp_data, *yptemp_data, *ftemp_data, *r_data, *col_j;
+ int i, j, i1, i2, width, group, ngroups;
+ int retval = 0;
+
+ CPodeMem cp_mem;
+ CPDlsMem cpdls_mem;
+
+ /* jac_data points to cpode_mem */
+ cp_mem = (CPodeMem) jac_data;
+ cpdls_mem = (CPDlsMem) lmem;
+
+ ftemp = tmp1; /* Rename work vector for use as the perturbed residual. */
+ ytemp = tmp2; /* Rename work vector for use as a temporary for yy. */
+ yptemp= tmp3; /* Rename work vector for use as a temporary for yp. */
+
+ /* Obtain pointers to the data for all vectors used. */
+ ewt_data = N_VGetArrayPointer(ewt);
+ r_data = N_VGetArrayPointer(r);
+ y_data = N_VGetArrayPointer(y);
+ yp_data = N_VGetArrayPointer(yp);
+ ftemp_data = N_VGetArrayPointer(ftemp);
+ ytemp_data = N_VGetArrayPointer(ytemp);
+ yptemp_data = N_VGetArrayPointer(yptemp);
+
+ /* Initialize ytemp and yptemp. */
+ N_VScale(ONE, y, ytemp);
+ N_VScale(ONE, yp, yptemp);
+
+ /* Compute miscellaneous values for the Jacobian computation. */
+ srur = RSqrt(uround);
+ width = mlower + mupper + 1;
+ ngroups = MIN(width, N);
+
+ /* Loop over column groups. */
+ for (group=1; group <= ngroups; group++) {
+
+ /* Increment all y[j] and yp[j] for j in this group. */
+ for (j=group-1; j<N; j+=width) {
+ yj = y_data[j];
+ ypj = yp_data[j];
+ ewtj = ewt_data[j];
+
+ /* Set increment inc to yj based on sqrt(uround)*abs(yj), with
+ adjustments using ypj and ewtj if this is small, and a further
+ adjustment to give it the same sign as h*ypj. */
+ inc = MAX( srur * MAX( ABS(yj), ABS(h*ypj) ) , ONE/ewtj );
+
+ if (h*ypj < ZERO) inc = -inc;
+ inc = (yj + inc) - yj;
+
+ /* Increment yj and ypj. */
+ ytemp_data[j] += gamma*inc;
+ yptemp_data[j] += inc;
+ }
+
+ /* Call ODE fct. with incremented arguments. */
+ retval = fi(tn, ytemp, yptemp, ftemp, f_data);
+ nfeDQ++;
+ if (retval != 0) break;
+
+ /* Loop over the indices j in this group again. */
+ for (j=group-1; j<N; j+=width) {
+
+ /* Reset ytemp and yptemp components that were perturbed. */
+ yj = ytemp_data[j] = y_data[j];
+ ypj = yptemp_data[j] = yp_data[j];
+ col_j = BAND_COL(Jac, j);
+ ewtj = ewt_data[j];
+
+ /* Set increment inc exactly as above. */
+ inc = MAX( srur * MAX( ABS(yj), ABS(h*ypj) ) , ONE/ewtj );
+ if (h*ypj < ZERO) inc = -inc;
+ inc = (yj + inc) - yj;
+
+ /* Load the difference quotient Jacobian elements for column j. */
+ inc_inv = ONE/inc;
+ i1 = MAX(0, j-mupper);
+ i2 = MIN(j+mlower,N-1);
+ for (i=i1; i<=i2; i++)
+ BAND_COL_ELEM(col_j,i,j) = inc_inv*(ftemp_data[i]-r_data[i]);
+ }
+
+ }
+
+ return(retval);
+}
+
+/*
+ * =================================================================
+ * DENSE DQ JACOBIAN APPROXIMATIONS FOR PROJECTION
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * cpDlsDenseProjDQJac
+ * -----------------------------------------------------------------
+ * This routine generates a dense difference quotient approximation
+ * to the transpose of the Jacobian of c(t,y). It loads it into a
+ * dense matrix of type DlsMat stored column-wise with elements
+ * within each column contiguous. The address of the jth column of
+ * J is obtained via the macro DENSE_COL and this pointer is
+ * associated with an N_Vector using the N_VGetArrayPointer and
+ * N_VSetArrayPointer functions.
+ * Finally, the actual computation of the jth column of the Jacobian
+ * transposed is done with a call to N_VLinearSum.
+ * -----------------------------------------------------------------
+ */
+int cpDlsDenseProjDQJac(int Nc, int Ny, realtype t,
+ N_Vector y, N_Vector cy,
+ DlsMat Jac, void *jac_data,
+ N_Vector c_tmp1, N_Vector c_tmp2)
+{
+ realtype inc, inc_inv, yj, srur;
+ realtype *y_data, *ewt_data, *jthCol_data;
+ N_Vector ctemp, jthCol;
+ int i, j;
+ int retval = 0;
+
+ CPodeMem cp_mem;
+ CPDlsProjMem cpdlsP_mem;
+
+ /* jac_data points to cpode_mem */
+ cp_mem = (CPodeMem) jac_data;
+ cpdlsP_mem = (CPDlsProjMem) lmemP;
+
+ /* Rename work vectors for readibility */
+ ctemp = c_tmp1;
+ jthCol = c_tmp2;
+
+ /* Obtain pointers to the data for ewt and y */
+ ewt_data = N_VGetArrayPointer(ewt);
+ y_data = N_VGetArrayPointer(y);
+
+ /* Obtain pointer to the data for jthCol */
+ jthCol_data = N_VGetArrayPointer(jthCol);
+
+ /* Set minimum increment based on uround and norm of f */
+ srur = RSqrt(uround);
+
+ /* Generate each column of the Jacobian G = dc/dy as delta(c)/delta(y_j). */
+ for (j = 0; j < Ny; j++) {
+
+ /* Save the y_j values. */
+ yj = y_data[j];
+
+ /* Set increment inc to y_j based on sqrt(uround)*abs(y_j),
+ with an adjustment using ewt_j if this is small */
+ inc = MAX( srur * ABS(yj) , ONE/ewt_data[j] );
+ inc = (yj + inc) - yj;
+
+ /* Increment y_j, call cfun, and break on error return. */
+ y_data[j] += inc;
+ retval = cfun(t, y, ctemp, c_data);
+ nceDQ++;
+ if (retval != 0) break;
+
+ /* Generate the jth col of G(tn,y) */
+ inc_inv = ONE/inc;
+ N_VLinearSum(inc_inv, ctemp, -inc_inv, cy, jthCol);
+
+ /* Copy the j-th column of G into the j-th row of Jac */
+ for (i = 0; i < Nc ; i++) {
+ DENSE_ELEM(Jac,j,i) = jthCol_data[i];
+ }
+
+ /* Reset y_j */
+ y_data[j] = yj;
+ }
+
+ return(retval);
+
+}
+
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_direct_impl.h b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_direct_impl.h
new file mode 100644
index 0000000..a92224a
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_direct_impl.h
@@ -0,0 +1,174 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/29 00:05:08 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * Common implementation header file for the CPDIRECT linear solvers.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _CPDIRECT_IMPL_H
+#define _CPDIRECT_IMPL_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <cpodes/cpodes_direct.h>
+
+/*
+ * -----------------------------------------------------------------
+ * CPDIRECT solver constants
+ * -----------------------------------------------------------------
+ * CPD_MSBJ maximum number of steps between Jacobian evaluations
+ * CPD_DGMAX maximum change in gamma between Jacobian evaluations
+ * -----------------------------------------------------------------
+ */
+
+#define CPD_MSBJ 50
+#define CPD_DGMAX RCONST(0.2)
+
+/*
+ * -----------------------------------------------------------------
+ * Types : CPDlsMemRec, CPDlsMem
+ * -----------------------------------------------------------------
+ * CPDlsMem is pointer to a CPDlsMemRec structure.
+ * -----------------------------------------------------------------
+ */
+
+typedef struct {
+
+ int d_type; /* SUNDIALS_DENSE or SUNDIALS_BAND */
+
+ int d_n; /* problem dimension */
+
+ int d_ml; /* lower bandwidth of Jacobian */
+ int d_mu; /* upper bandwidth of Jacobian */
+ int d_smu; /* upper bandwith of M = MIN(N-1,d_mu+d_ml) */
+
+ CPDlsDenseJacExplFn d_djacE; /* dense Jacobian routine (CP_EXPL) */
+ CPDlsDenseJacImplFn d_djacI; /* dense Jacobian routine (CP_IMPL) */
+
+ CPDlsBandJacExplFn d_bjacE; /* band Jacobian routine (CP_EXPL) */
+ CPDlsBandJacImplFn d_bjacI; /* band Jacobian routine (CP_IMPL) */
+
+ void *d_J_data; /* user data is passed to d_jac or d_jac */
+
+ DlsMat d_M; /* M = I - gamma * df/dy */
+ DlsMat d_savedJ; /* savedJ = old Jacobian */
+
+ int *d_pivots; /* pivots = pivot array for PM = LU */
+
+ long int d_nstlj; /* nstlj = nst at last Jacobian eval. */
+
+ long int d_nje; /* nje = no. of calls to jac */
+
+ long int d_nfeDQ; /* no. of calls to f due to DQ Jacobian approx. */
+
+ int d_last_flag; /* last error return flag */
+
+} CPDlsMemRec, *CPDlsMem;
+
+/*
+ * -----------------------------------------------------------------
+ * Types : CPDlsProjMemRec, CPDlsProjMem
+ * -----------------------------------------------------------------
+ * The type CPDlsProjMem is pointer to a CPDlsProjMemRec.
+ * This structure contains CPDlsProj solver-specific data.
+ * -----------------------------------------------------------------
+ */
+
+typedef struct {
+
+ int d_type; /* always SUNDIALS_DENSE */
+
+ int d_nc; /* number of constraints */
+ int d_ny; /* number of states */
+
+ CPDlsDenseProjJacFn d_jacP; /* Jacobian routine to be called */
+ void *d_JP_data; /* J_data is passed to jacP */
+
+ int d_ftype; /* factorization type (LU, QR, or SC) */
+ int d_pnorm; /* projection norm (L2 or WRMS) */
+
+ DlsMat d_G; /* G = (dc/dy)^T, transpose of cnstr. Jacobian */
+ DlsMat d_savedG; /* saved Jacobian (before factorization) */
+
+ int d_nr; /* no. of independent constraints (QRP) */
+
+ DlsMat d_K; /* K matrix (s.p.d., form depends on ftype) */
+ int *d_pivotsP; /* pivotsP = pivot array (for ftype LU) */
+
+ realtype *d_beta; /* beta array (for ftype QR) */
+
+ realtype *d_wrk; /* work array (for ftype QR or QRP) */
+ int d_len_wrk; /* length of work array */
+
+ long int d_nstljP; /* nstljP = nst at last Jacobian eval. */
+
+ long int d_njeP; /* njeP = no. of calls to jacP */
+
+ long int d_nceDQ; /* no. of calls to c due to DQ Jacobian approx.*/
+
+} CPDlsProjMemRec, *CPDlsProjMem;
+
+/*
+ * -----------------------------------------------------------------
+ * Prototypes of internal functions
+ * -----------------------------------------------------------------
+ */
+
+int cpDlsDenseDQJacExpl(int N, realtype t,
+ N_Vector y, N_Vector fy,
+ DlsMat Jac, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+
+int cpDlsDenseDQJacImpl(int N, realtype t, realtype gm,
+ N_Vector y, N_Vector yp, N_Vector r,
+ DlsMat Jac, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+
+int cpDlsBandDQJacExpl(int N, int mupper, int mlower,
+ realtype t, N_Vector y, N_Vector fy,
+ DlsMat Jac, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+
+int cpDlsBandDQJacImpl(int N, int mupper, int mlower,
+ realtype t, realtype gm,
+ N_Vector y, N_Vector yp, N_Vector r,
+ DlsMat Jac, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+
+int cpDlsDenseProjDQJac(int Nc, int Ny, realtype t,
+ N_Vector y, N_Vector cy,
+ DlsMat Jac, void *jac_data,
+ N_Vector c_tmp1, N_Vector c_tmp2);
+
+/*
+ * -----------------------------------------------------------------
+ * Error Messages
+ * -----------------------------------------------------------------
+ */
+
+#define MSGD_CPMEM_NULL "Integrator memory is NULL."
+#define MSGD_BAD_NVECTOR "A required vector operation is not implemented."
+#define MSGD_BAD_SIZES "Illegal bandwidth parameter(s). Must have 0 <= ml, mu <= N-1."
+#define MSGD_MEM_FAIL "A memory request failed."
+#define MSGD_LMEM_NULL "Linear solver memory is NULL."
+#define MSGD_JACFUNC_FAILED "The Jacobian routine failed in an unrecoverable manner."
+
+#define MSGD_BAD_FACT "fact_type has an illegal value."
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_ic.c b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_ic.c
new file mode 100644
index 0000000..21ae6ee
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_ic.c
@@ -0,0 +1,664 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/12/01 22:48:57 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * Implementation of cmputation of consistent initial conditions
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * TODO:
+ *
+ * cpicImplComputeYp
+ */
+
+/*
+ * =================================================================
+ * IMPORTED HEADER FILES
+ * =================================================================
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "cpodes_private.h"
+#include <sundials/sundials_math.h>
+
+/*
+ * =================================================================
+ * ALGORITHMIC CONSTANTS
+ * =================================================================
+ */
+
+/* Maximum number of attempts to correct a recoverable
+ * constraint function error */
+#define MAX_RECVR 5
+#define MAX_ITERS 10
+
+/*
+ * =================================================================
+ * PROTOTYPES FOR PRIVATE FUNCTIONS
+ * =================================================================
+ */
+
+/* Initial consistency checks */
+static int cpicInitialSetup(CPodeMem cp_mem);
+
+/* Projection onto invariant manifold */
+static int cpicDoProjection(CPodeMem cp_mem);
+
+/* Fucntions called by cpicDoProjection */
+static int cpicProjLinear(CPodeMem cp_mem);
+static int cpicProjNonlinear(CPodeMem cp_mem);
+
+/* Calculation of consistent y' */
+static int cpicImplComputeYp(CPodeMem cp_mem);
+
+static void cpicFailFlag(CPodeMem, int flag);
+
+/*
+ * =================================================================
+ * READIBILITY REPLACEMENTS
+ * =================================================================
+ */
+
+#define ode_type (cp_mem->cp_ode_type)
+#define tn (cp_mem->cp_tn)
+#define zn (cp_mem->cp_zn)
+#define tol_type (cp_mem->cp_tol_type)
+#define efun (cp_mem->cp_efun)
+#define e_data (cp_mem->cp_e_data)
+#define ewt (cp_mem->cp_ewt)
+#define gamma (cp_mem->cp_gamma)
+#define tempv (cp_mem->cp_tempv)
+#define uround (cp_mem->cp_uround)
+
+#define linit (cp_mem->cp_linit)
+#define lsetup (cp_mem->cp_lsetup)
+#define lsolve (cp_mem->cp_lsolve)
+#define lsetup_exists (cp_mem->cp_lsetup_exists)
+
+#define proj_enabled (cp_mem->cp_proj_enabled)
+#define proj_type (cp_mem->cp_proj_type)
+#define proj_norm (cp_mem->cp_proj_norm)
+#define cnstr_type (cp_mem->cp_cnstr_type)
+#define pfun (cp_mem->cp_pfun)
+#define p_data (cp_mem->cp_p_data)
+#define cfun (cp_mem->cp_cfun)
+#define c_data (cp_mem->cp_c_data)
+
+#define prjcoef (cp_mem->cp_prjcoef)
+#define yC (cp_mem->cp_yC)
+#define acorP (cp_mem->cp_acorP)
+#define ctemp (cp_mem->cp_ctemp)
+#define ctol (cp_mem->cp_ctol)
+#define tempvP1 (cp_mem->cp_tempvP1)
+#define tempvP2 (cp_mem->cp_tempvP2)
+
+#define linitP (cp_mem->cp_linitP)
+#define lsetupP (cp_mem->cp_lsetupP)
+#define lsolveP (cp_mem->cp_lsolveP)
+#define lsetupP_exists (cp_mem->cp_lsetupP_exists)
+
+#define icprj_convcoef (cp_mem->cp_icprj_convcoef)
+#define icprj_normtol (cp_mem->cp_icprj_normtol)
+#define icprj_maxrcvr (cp_mem->cp_icprj_maxrcvr)
+#define icprj_maxiter (cp_mem->cp_icprj_maxiter)
+#define yy0 (cp_mem->cp_yy0)
+#define yp0 (cp_mem->cp_yp0)
+
+/*
+ * =================================================================
+ * EXPORTED FUNCTIONS
+ * =================================================================
+ */
+
+/*
+ * CPodeCalcIC
+ *
+ * This calculates corrected initial conditions that are consistent
+ * with the invariant constraints and (for implicit-form ODEs) with
+ * the ODE system itself. It first projects the initial guess for
+ * the state vector (given by the user through CPodeInit or CPodeReInit)
+ * and then, if necessary, computes a state derivative vector as
+ * solution of F(t0, y0, y0') = 0.
+ *
+ */
+int CPodeCalcIC(void *cpode_mem)
+{
+ CPodeMem cp_mem;
+ int flag;
+
+ /* Check if cpode_mem exists */
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeCalcIC", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Check if cpode_mem was allocated */
+ if (cp_mem->cp_MallocDone == FALSE) {
+ cpProcessError(cp_mem, CP_NO_MALLOC, "CPODES", "CPodeCalcIC", MSGCP_NO_MALLOC);
+ return(CP_NO_MALLOC);
+ }
+
+ /* For explicit ODE, if projection is not enabled, there is nothing to do */
+ if ( (ode_type == CP_EXPL) && (!proj_enabled) ) {
+ cpProcessError(cp_mem, CP_WARNING, "CPODES", "CPodeCalcIC", MSGCP_IC_NO_WORK);
+ return(CP_SUCCESS);
+ }
+
+ /* Perform initial setup */
+ flag = cpicInitialSetup(cp_mem);
+ if (flag != CP_SUCCESS) return(flag);
+
+ /* Initialize various convergence test constants
+ * icprj_convcoef - constant used to test weighted norms.
+ * Here, we use a value 10 times smaller than the
+ * one used in the projectin during integration.
+ * icprj_normtol - stopping tolerance on max-norm of constraints
+ * icprj_maxrcvr - maximum number of reductin in full Newton step
+ * in attempting to correct recoverable constraint
+ * function errors.
+ * icprj_maxiter - maximum number of nonlinear iterations
+ */
+ icprj_convcoef = PT1 * prjcoef;
+ icprj_normtol = RPowerR(uround, HALF);
+ icprj_maxrcvr = MAX_RECVR;
+ icprj_maxiter = MAX_ITERS;
+
+ /* Allocate space and initialize temporary vectors */
+ yy0 = N_VClone(tempv);
+ N_VScale(ONE, zn[0], yy0);
+ if (ode_type == CP_IMPL) {
+ yp0 = N_VClone(tempv);
+ N_VScale(ONE, zn[1], yy0);
+ }
+
+ /* Compute y consistent with constraints */
+ if (proj_enabled)
+ flag = cpicDoProjection(cp_mem);
+
+ /* Compute yp consistent with DE */
+ if ( (flag == CP_SUCCESS) && (ode_type == CP_IMPL) )
+ flag = cpicImplComputeYp(cp_mem);
+
+ /* If successful, load yy0 and yp0 into Nordsieck array */
+ if (flag == CP_SUCCESS) {
+ N_VScale(ONE, yy0, zn[0]);
+ if (ode_type == CP_IMPL) N_VScale(ONE, yp0, zn[1]);
+ }
+
+ /* Free temporary space */
+ N_VDestroy(yy0);
+ if (ode_type == CP_IMPL) N_VDestroy(yp0);
+
+ /* On any type of failure, print message and return proper flag */
+ if (flag != CP_SUCCESS) {
+ cpicFailFlag(cp_mem, flag);
+ return(flag);
+ }
+
+ /* Otherwise return success flag */
+ return(CP_SUCCESS);
+
+}
+
+/*
+ * =================================================================
+ * PRIVATE FUNCTIONS
+ * =================================================================
+ */
+
+/*
+ * cpicInitialSetup
+ *
+ * This routine performs input consistency checks for IC calculation.
+ * If needed, it also checks the linear solver module and calls the
+ * linear solver initialization routine.
+ *
+ * This function performs many of the same tests that cpInitialSetup
+ * does at the first integration step.
+ */
+static int cpicInitialSetup(CPodeMem cp_mem)
+{
+ int flag;
+
+ /* If projection is enabled, check if appropriate projection functions are available */
+ if (proj_enabled) {
+
+ switch (proj_type) {
+
+ case CP_PROJ_USER:
+
+ /* Check if user provided the projection function */
+ if ( pfun == NULL ) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeCalcIC", MSGCP_NO_PFUN);
+ return(CP_ILL_INPUT);
+ }
+
+ break;
+
+ case CP_PROJ_INTERNAL:
+
+ /* Check if user provided the constraint function */
+ if (cfun == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeCalcIC", MSGCP_NO_CFUN);
+ return(CP_ILL_INPUT);
+ }
+ /* Check that lsolveP exists */
+ if ( lsolveP == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeCalcIC", MSGCP_PLSOLVE_NULL);
+ return(CP_ILL_INPUT);
+ }
+ /* Call linitP if it exists */
+ if ( linitP != NULL ) {
+ flag = linitP(cp_mem);
+ if (flag != 0) {
+ cpProcessError(cp_mem, CP_PLINIT_FAIL, "CPODES", "CPodeCalcIC", MSGCP_PLINIT_FAIL);
+ return(CP_PLINIT_FAIL);
+ }
+ }
+
+ break;
+
+ }
+
+ }
+
+ /* For implicit ODE, we will always need a linear solver */
+ if (ode_type == CP_IMPL) {
+ /* Check that lsolve exists */
+ if (lsolve == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeCalcIC", MSGCP_LSOLVE_NULL);
+ return(CP_ILL_INPUT);
+ }
+ /* Call linit if it exists */
+ if ( linit != NULL ) {
+ flag = linit(cp_mem);
+ if (flag != 0) {
+ cpProcessError(cp_mem, CP_LINIT_FAIL, "CPODES", "CPodeCalcIC", MSGCP_LINIT_FAIL);
+ return(CP_LINIT_FAIL);
+ }
+ }
+ }
+
+ /* Did the user provide efun? */
+ if (tol_type != CP_WF) {
+ efun = cpEwtSet;
+ e_data = (void *)cp_mem;
+ } else {
+ if (efun == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeCalcIC", MSGCP_NO_EFUN);
+ return(CP_ILL_INPUT);
+ }
+ }
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * cpicDoProjection
+ *
+ * This is the top-level function handling the projection onto the
+ * invariant manifold. It either calls the user-supplied projection
+ * function or, depending on the type of constraints, one of the
+ * internal projection functions (cpicProjLinear or cpicProjNonlinear).
+ *
+ * For user supplied projection function, use tempv to store the
+ * corection due to projection, acorP (tempv is not touched until it
+ * is potentially used in cpCompleteStep).
+ *
+ * For the internal projection function, space for acorP was allocated
+ * in CPodeProjInit.
+ */
+static int cpicDoProjection(CPodeMem cp_mem)
+{
+ int flag = CP_SUCCESS, retval;
+
+ switch (proj_type) {
+
+ case CP_PROJ_INTERNAL:
+
+ /* Evaluate constraints at initial time and with the provided yy0 */
+ retval = cfun(tn, yy0, ctemp, c_data);
+ if (retval < 0) return(CP_CNSTRFUNC_FAIL);
+ if (retval > 0) return(CP_FIRST_CNSTRFUNC_ERR);
+
+ /* Perform projection step
+ * On a successful return, yy0 was updated. */
+ if (cnstr_type == CP_CNSTR_NONLIN) flag = cpicProjNonlinear(cp_mem);
+ else flag = cpicProjLinear(cp_mem);
+
+ break;
+
+ case CP_PROJ_USER:
+
+ acorP = tempv;
+
+ /* Call the user projection function (with err=NULL) */
+ retval = pfun(tn, yy0, acorP, icprj_convcoef, NULL, p_data);
+ if (retval != 0) return(CP_PROJFUNC_FAIL);
+
+ /* Update yy0 */
+ N_VLinearSum(ONE, yy0, ONE, acorP, yy0);
+
+ break;
+
+ }
+
+ return(flag);
+
+}
+
+/*
+ * cpicProjLinear
+ *
+ * This function performs the projection onto a linear invariant manifold
+ */
+static int cpicProjLinear(CPodeMem cp_mem)
+{
+ int retval;
+
+ /* Call the lsetupP function to evaluate and factorize the
+ * Jacobian of constraints */
+ retval = lsetupP(cp_mem, yy0, ctemp, tempvP1, tempvP2, tempv);
+ if (retval != 0) return(CP_PLSETUP_FAIL);
+
+ /* Call lsolveP (rhs is ctemp; solution in acorP) */
+ retval = lsolveP(cp_mem, ctemp, acorP, yy0, ctemp, tempvP1, tempv);
+ if (retval != 0) return(CP_PLSOLVE_FAIL);
+
+ /* Update yy0 */
+ N_VLinearSum(ONE, yy0, -ONE, acorP, yy0);
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * cpicProjNonlinear
+ *
+ * This function performs the projection onto a nonlinear invariant manifold.
+ *
+ * The convergence tests are:
+ * ||c||_WL2 < icprj_convcoef AND ||c||_max < icprj_normtol
+ * OR
+ * ||p||_WRMS < icprj_convcoef
+ *
+ * Default values:
+ * icprj_convcoef = 0.1 * prjcoef = 0.1 * 0.1
+ * icprj_normtol = sqrt(uround)
+ *
+ * In other words, we stop when the constraints are within the user-specified
+ * tolerances (but, to cover the case where ctol was too large, we also impose
+ * that the maximum constraint violation is small enough). Otherwise, we declare
+ * convergence when the current correction becomes small enough (within the
+ * integration tolerances).
+ *
+ * Convergence failure is declared on any unrecoverable function failure, or
+ * if it is not possible to correct a recoverable function error, or if the
+ * maximum number of iterations was reached with up-to-date Jacobian.
+ *
+ */
+static int cpicProjNonlinear(CPodeMem cp_mem)
+{
+ N_Vector yy0_new;
+ realtype pnorm, pnorm_p;
+ realtype cnorm, cmax;
+ realtype ccon, pcon, crate, ratio;
+ booleantype callSetup, jacCurrent, cOK;
+ int retval, m, ircvr, flag;
+
+ /* Evaluate ewt at yy0 */
+ flag = efun(yy0, ewt, e_data);
+ if (flag != 0) return(CP_ILL_INPUT);
+
+ /* Rename yC to yy0_new */
+ yy0_new = yC;
+
+ /* Initializations */
+ m = 0;
+ callSetup = TRUE;
+ crate = ONE;
+ pnorm = ZERO;
+ pnorm_p = ZERO;
+
+ /* Looping point for iterations */
+ loop {
+
+ /*
+ * 1. Evaluate Jacobian (if requested)
+ */
+
+ jacCurrent = FALSE;
+ if (lsetupP_exists && callSetup) {
+ retval = lsetupP(cp_mem, yy0, ctemp, tempvP1, tempvP2, tempv);
+ if (retval != 0) return(CP_PLSETUP_FAIL);
+ jacCurrent = TRUE;
+ callSetup = FALSE;
+ crate = ONE;
+ m = 0;
+ }
+
+#ifdef CPODES_DEBUG
+ printf("Iteration %d\n",m+1);
+ printf(" Jacobian current: %d\n",jacCurrent);
+#endif
+
+ /*
+ * 2. Solve for Newton step
+ */
+
+ /* compute Newton step and load it into acorP */
+ retval = lsolveP(cp_mem, ctemp, acorP, yy0, ctemp, tempvP1, tempv);
+ /* If lsolveP failed unrecoverably, return */
+ if (retval < 0) return(CP_PLSOLVE_FAIL);
+ /* If lsolveP had a recoverable error, with up-to-date Jacobian, return */
+ if (retval > 0) {
+ if (!lsetupP_exists || jacCurrent) return(CP_NO_RECOVERY);
+ callSetup = TRUE;
+ continue;
+ }
+
+ /*
+ * 3. Apply Newton step
+ */
+
+ /* Compute step length = ||acorP|| */
+ pnorm = N_VWrmsNorm(acorP, ewt);
+ ratio = ONE;
+
+#ifdef CPODES_DEBUG
+ printf(" Norm of full Newton step: %lg\n", pnorm);
+#endif
+
+ /* Attempt (at most icprj_maxrcvr times) to evaluate
+ constraint function at the new iterate */
+ cOK = FALSE;
+ for (ircvr=0; ircvr<icprj_maxrcvr; ircvr++) {
+
+ /* compute new iterate */
+ N_VLinearSum(ONE, yy0, -ONE, acorP, yy0_new);
+
+ /* evaluate constraints at yy0_new */
+ retval = cfun(tn, yy0_new, ctemp, c_data);
+
+ /* if successful, accept current acorP */
+ if (retval == 0) {cOK = TRUE; break;}
+
+ /* if function failed unrecoverably, give up */
+ if (retval < 0) return(CP_CNSTRFUNC_FAIL);
+
+ /* function had a recoverable error; cut step in half */
+ pnorm *= HALF;
+ ratio *= HALF;
+ N_VScale(HALF, acorP, acorP);
+
+ }
+
+ /* If cfunc failed recoverably MAX_RECVR times, give up */
+ if (!cOK) return(CP_REPTD_CNSTRFUNC_ERR);
+
+#ifdef CPODES_DEBUG
+ printf(" Full Newton step cut by ratio: %lg\n", ratio);
+#endif
+
+ /*
+ * 4. Evaluate various stoping test quantities
+ */
+
+ /* Weighted L-2 norm of constraints */
+ cnorm = N_VWL2Norm(ctemp, ctol);
+ ccon = cnorm / icprj_convcoef;
+
+ /* Max-norm of constraints */
+ cmax = N_VMaxNorm(ctemp);
+
+ /* Estimated convergence rate */
+ if (m > 0) crate = MAX(PRJ_CRDOWN * crate, pnorm/pnorm_p);
+
+ /* Convergence test based on pnorm and crate */
+ pcon = pnorm * MIN(ONE, crate) / icprj_convcoef;
+
+#ifdef CPODES_DEBUG
+ printf(" Stop test quantities:\n");
+ printf(" crate: %lg\n", crate);
+ printf(" cnorm: %lg\n", cnorm);
+ printf(" ccon: %lg <? %lg\n", ccon, ONE);
+ printf(" cmax: %lg <? %lg\n", cmax, icprj_normtol);
+ printf(" pcon: %lg <? %lg\n", pcon, ONE);
+#endif
+
+ /*
+ * 4. Test for convergence
+ */
+
+ /* If converged, load new solution and return */
+ if ( (pcon <= ONE) || (ccon <= ONE && cmax <= icprj_normtol) ) {
+ N_VScale(ONE, yy0_new, yy0);
+ return(CP_SUCCESS);
+ }
+
+ /* Increment iteration counter */
+ m++;
+
+#ifdef CPODES_DEBUG
+ printf(" m = %d ==? %d = maxiter\n",m,icprj_maxiter);
+ if (m >=2)
+ printf(" pnorm = %lg >? %lg = PRJ_RDIV*pnorm_p\n", pnorm, PRJ_RDIV*pnorm_p);
+#endif
+
+ /* Check if we reached max. iters. or if iters. seem to be diverging */
+ if ((m == icprj_maxiter) || ((m >= 2) && (pnorm > PRJ_RDIV*pnorm_p))) {
+
+ /* If the Jacobian is up-to-date, give up */
+ if (!lsetupP_exists || jacCurrent) return(CP_PROJ_FAILURE);
+
+ /* Otherwise, attempt to recover by re-evaluating the Jacobian */
+ callSetup = TRUE;
+ retval = cfun(tn, yy0, ctemp, c_data);
+ continue;
+
+ }
+
+ /* Save norm of correction, update solution and ewt, and loop again */
+ pnorm_p = pnorm;
+ N_VScale(ONE, yy0_new, yy0);
+ flag = efun(yy0, ewt, e_data);
+ if (flag != 0) return(CP_ILL_INPUT);
+
+ }
+
+ return(CP_SUCCESS);
+}
+
+
+/*
+ * cpicImplComputeYp
+ *
+ * For implicit-form ODEs, this function computes y' values consistent
+ * with the ODE, given y values consistent with the constraints.
+ * In other words, it solves F(t0,y0,y0') = 0 for y0' using a Newton
+ * iteration.
+ */
+static int cpicImplComputeYp(CPodeMem cp_mem)
+{
+ /*
+ int retval;
+ */
+
+ /* Evaluate residual at initial time, using the (projected) yy0
+ and the initial guess yp0 */
+
+ /*
+ retval = fi(tn, yy0, yp0, ftemp, f_data);
+ if (retval < 0) return(CP_ODEFUNC_FAIL);
+ if (retval > 0) return(CP_FIRST_ODEFUNC_ERR);
+ */
+
+ /* Set gamma */
+
+ /*
+ gamma = ZERO;
+ */
+
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * cpicFailFlag
+ *
+ * This function is called if the calculation of consistent IC failed.
+ * Depending on the flag value, it calls the error handler and returns
+ * the proper flag.
+ */
+static void cpicFailFlag(CPodeMem cp_mem, int flag)
+{
+ switch(flag) {
+ case CP_FIRST_CNSTRFUNC_ERR:
+ cpProcessError(cp_mem, CP_FIRST_CNSTRFUNC_ERR, "CPODES", "CPodeCalcIC", MSGCP_IC_CNSTRFUNC_FIRST);
+ break;
+ case CP_CNSTRFUNC_FAIL:
+ cpProcessError(cp_mem, CP_CNSTRFUNC_FAIL, "CPODES", "CPodeCalcIC", MSGCP_IC_CNSTRFUNC_FAILED);
+ break;
+ case CP_REPTD_CNSTRFUNC_ERR:
+ cpProcessError(cp_mem, CP_REPTD_CNSTRFUNC_ERR, "CPODES", "CPodeCalcIC", MSGCP_IC_CNSTRFUNC_REPTD);
+ break;
+ case CP_PROJFUNC_FAIL:
+ cpProcessError(cp_mem, CP_PROJFUNC_FAIL, "CPODES", "CPodeCalcIC", MSGCP_IC_PROJFUNC_FAILED);
+ break;
+ case CP_PLSETUP_FAIL:
+ cpProcessError(cp_mem, CP_PLSETUP_FAIL, "CPODES", "CPodeCalcIC", MSGCP_IC_PLSETUP_FAILED);
+ break;
+ case CP_PLSOLVE_FAIL:
+ cpProcessError(cp_mem, CP_PLSOLVE_FAIL, "CPODES", "CPodeCalcIC", MSGCP_IC_PLSOLVE_FAILED);
+ break;
+ case CP_NO_RECOVERY:
+ cpProcessError(cp_mem, CP_NO_RECOVERY, "CPODES", "CPodeCalcIC", MSGCP_IC_NO_RECOVERY);
+ break;
+ case CP_PROJ_FAILURE:
+ cpProcessError(cp_mem, CP_PROJ_FAILURE, "CPODES", "CPodeCalcIC", MSGCP_IC_PROJ_FAILS);
+ break;
+ case CP_ILL_INPUT:
+ if (tol_type == CP_WF) cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeCalcIC", MSGCP_IC_EWT_FAIL);
+ else cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeCalcIC", MSGCP_IC_EWT_BAD);
+ break;
+ case CP_FIRST_ODEFUNC_ERR:
+ cpProcessError(cp_mem, CP_FIRST_ODEFUNC_ERR, "CPODES", "CPodeCalcIC", MSGCP_IC_ODEFUNC_FIRST);
+ break;
+ case CP_ODEFUNC_FAIL:
+ cpProcessError(cp_mem, CP_ODEFUNC_FAIL, "CPODES", "CPodeCalcIC", MSGCP_IC_ODEFUNC_FAILED);
+ break;
+
+ }
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_impl.h b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_impl.h
new file mode 100644
index 0000000..bf64071
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_impl.h
@@ -0,0 +1,688 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.5 $
+ * $Date: 2007/10/26 21:51:29 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * Implementation header file for the main CPODES integrator.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _CPODES_IMPL_H
+#define _CPODES_IMPL_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <cpodes/cpodes.h>
+
+/*
+ * =================================================================
+ * M A I N I N T E G R A T O R M E M O R Y B L O C K
+ * =================================================================
+ */
+
+/* Basic LMM constants */
+
+#define ADAMS_Q_MAX 12 /* max value of q for ADAMS */
+#define BDF_Q_MAX 5 /* max value of q for BDF */
+#define Q_MAX ADAMS_Q_MAX /* max value of q for either LMM */
+#define L_MAX (Q_MAX+1) /* max value of L for either LMM */
+#define NUM_TESTS 5 /* number of error test quantities */
+
+/*
+ * -----------------------------------------------------------------
+ * Types : struct CPodeMemRec, CPodeMem
+ * -----------------------------------------------------------------
+ * The type CPodeMem is type pointer to struct CPodeMemRec.
+ * This structure contains fields to keep track of problem state.
+ * -----------------------------------------------------------------
+ */
+
+typedef struct CPodeMemRec {
+
+ realtype cp_uround; /* machine unit roundoff */
+
+ /*--------------------------
+ Problem Specification Data
+ --------------------------*/
+
+ int cp_ode_type; /* ODE type: ode = CP_IMPL or CP_EXPL */
+ CPResFn cp_fi; /* F(t,y'(t),y(t)) = 0 */
+ CPRhsFn cp_fe; /* y' = f(t,y(t)) */
+ void *cp_f_data; /* user pointer passed to f */
+
+ int cp_lmm_type; /* lmm_type = CP_ADAMS or CP_BDF */
+ int cp_nls_type; /* nls_type = CP_FUNCTIONAL or CP_NEWTON */
+ int cp_tol_type; /* tol_type = CP_SS or CP_SV or CP_WF */
+
+ realtype cp_reltol; /* relative tolerance */
+ realtype cp_Sabstol; /* scalar absolute tolerance */
+ N_Vector cp_Vabstol; /* vector absolute tolerance */
+ CPEwtFn cp_efun; /* function to set ewt */
+ void *cp_e_data; /* user pointer passed to efun */
+
+ /*-----------------------
+ Nordsieck History Array
+ -----------------------*/
+
+ /*
+ * Nordsieck array, of size N x (q+1).
+ * zn[j] is a vector of length N (j=0,...,q)
+ * zn[j] = [1/factorial(j)] * h^j * (jth derivative of the interpolating polynomial)
+ */
+
+ N_Vector cp_zn[L_MAX];
+
+ /*------------------------------------------------
+ Other vectors of same length as the state vector
+ ------------------------------------------------*/
+
+ N_Vector cp_ewt; /* error weight vector. */
+ N_Vector cp_y; /* work space for y vector (= yout) */
+ N_Vector cp_yp; /* work space for yp vector (= ypout) */
+ N_Vector cp_acor; /* accumulated correction acor = y_n(m)-y_n(0) */
+ N_Vector cp_tempv; /* temporary storage vector */
+ N_Vector cp_ftemp; /* temporary storage vector */
+
+ /*---------
+ Step Data
+ ---------*/
+
+ int cp_q; /* current order */
+ int cp_qprime; /* order to be used on the next step */
+ int cp_next_q; /* order to be used on the next step */
+ int cp_qwait; /* no. of steps to wait before a change in q */
+ int cp_L; /* L = q + 1 */
+
+ realtype cp_hin; /* initial step size */
+ realtype cp_h; /* current step size */
+ realtype cp_hprime; /* step size to be used on the next step */
+ realtype cp_next_h; /* step size to be used on the next step */
+ realtype cp_eta; /* eta = hprime / h */
+ realtype cp_hscale; /* value of h used in zn */
+ realtype cp_tn; /* current internal value of t */
+ realtype cp_tretlast; /* value of tret last returned by CPode */
+
+ realtype cp_tau[L_MAX+1]; /* array of previous q+1 successful step sizes */
+ realtype cp_tq[NUM_TESTS+1]; /* array of test quantities */
+ realtype cp_l[L_MAX]; /* coefficients of Lambda(x) (degree q poly) */
+ realtype cp_p[L_MAX]; /* coefficients of Phi(x) (degree q poly) */
+
+ realtype cp_rl1; /* the scalar 1/l[1] */
+ realtype cp_gamma; /* gamma = h * rl1 */
+ realtype cp_gammap; /* gamma at the last setup call */
+ realtype cp_gamrat; /* gamma / gammap */
+
+ realtype cp_crate; /* estimated corrector convergence rate */
+ realtype cp_acnrm; /* ||acor||_wrms */
+ realtype cp_nlscoef; /* coeficient in nonlinear convergence test */
+ int cp_mnewt; /* Newton iteration counter */
+
+ /*---------------
+ Projection Data
+ ---------------*/
+
+ int cp_proj_type; /* user vs. internal projection algorithm */
+ booleantype cp_proj_enabled; /* is projection enabled? */
+ booleantype cp_applyProj; /* was projection performed at current step? */
+
+ long int cp_proj_freq; /* projection frequency */
+ long int cp_nstlprj; /* step number of last projection */
+
+ int cp_proj_norm; /* type of projection norm (L2 or WRMS) */
+ int cp_cnstr_type; /* type of constraints (lin. or nonlin.) */
+ CPCnstrFn cp_cfun; /* 0 = c(t,y(t)) */
+ void *cp_c_data; /* user pointer passed to cfun */
+
+ CPProjFn cp_pfun; /* function to perform projection */
+ void *cp_p_data; /* user pointer passed to pfun */
+
+ realtype cp_prjcoef; /* coefficient in projection convergence test */
+
+ N_Vector cp_acorP; /* projection correction (length N) */
+ N_Vector cp_errP; /* projected error estimate (length N) */
+ N_Vector cp_yC; /* saved corrected state (length N) */
+
+ long int cp_lsetupP_freq; /* frequency of cnstr. Jacobian evaluation */
+ long int cp_nstlsetP; /* step number of last lsetupP call */
+
+ realtype cp_crateP; /* estimated conv. rate in nonlin. projection */
+ int cp_maxcorP; /* maximum number of nonlin. proj. iterations */
+
+ booleantype cp_project_err; /* should we project the error estimate? */
+
+ booleantype cp_test_cnstr; /* test constraint norm before projection? */
+ N_Vector cp_ctol; /* vector of constraint "absolute tolerances" */
+
+ N_Vector cp_ctemp; /* temporary vectors (length M) */
+ N_Vector cp_tempvP1;
+ N_Vector cp_tempvP2;
+
+ booleantype cp_first_proj; /* is this the first time we project? */
+
+ long int cp_nproj; /* number of projection steps performed */
+ long int cp_nprf; /* number of projection failures */
+ long int cp_nce; /* number of calls to cfun */
+ long int cp_nsetupsP; /* number of calls to lsetupP */
+ int cp_maxnpf; /* maximum number of projection failures */
+
+ /*-----------------------
+ Quadrature Related Data
+ -----------------------*/
+
+ booleantype cp_quadr; /* are we integrating quadratures? */
+
+ CPQuadFn cp_qfun; /* function defining the integrand */
+ void *cp_q_data; /* user pointer passed to fQ */
+ int cp_tol_typeQ; /* tol_typeQ = CP_SS or CP_SV */
+ booleantype cp_errconQ; /* are quadratures included in error test? */
+
+ realtype cp_reltolQ; /* relative tolerance for quadratures */
+ realtype cp_SabstolQ; /* scalar absolute tolerance for quadratures */
+ N_Vector cp_VabstolQ; /* vector absolute tolerance for quadratures */
+
+ N_Vector cp_znQ[L_MAX]; /* Nordsieck arrays for sensitivities */
+ N_Vector cp_ewtQ; /* error weight vector for quadratures */
+ N_Vector cp_yQ; /* Unlike y, yQ is not allocated by the user */
+ N_Vector cp_acorQ; /* acorQ = yQ_n(m) - yQ_n(0) */
+ N_Vector cp_tempvQ; /* temporary storage vector (~ tempv) */
+
+ realtype cp_acnrmQ; /* acnrmQ = ||acorQ||_WRMS */
+
+ long int cp_lrw1Q; /* no. of realtype words in 1 N_Vector yQ */
+ long int cp_liw1Q; /* no. of integer words in 1 N_Vector yQ */
+
+ int cp_qmax_allocQ; /* qmax used when allocating quad. memory */
+
+ booleantype cp_VabstolQMallocDone; /* did we allocate memory for abstolQ? */
+ booleantype cp_quadMallocDone; /* was quadrature memory allocated? */
+
+ long int cp_nqe; /* number of calls to qfun */
+ long int cp_netfQ; /* number of quadr. error test failures */
+
+ /*-----------------
+ Tstop information
+ -----------------*/
+
+ booleantype cp_tstopset;
+ booleantype cp_istop;
+ realtype cp_tstop;
+
+ /*------
+ Limits
+ ------*/
+
+ int cp_qmax; /* q <= qmax */
+ long int cp_mxstep; /* maximum number of internal steps for one user call */
+ int cp_maxcor; /* maximum number of corrector iterations for the */
+ /* solution of the nonlinear equation */
+ int cp_mxhnil; /* maximum number of warning messages issued to the */
+ /* user that t + h == t for the next internal step */
+ int cp_maxnef; /* maximum number of error test failures */
+ int cp_maxncf; /* maximum number of nonlinear convergence failures */
+
+ realtype cp_hmin; /* |h| >= hmin */
+ realtype cp_hmax_inv; /* |h| <= 1/hmax_inv */
+ realtype cp_etamax; /* eta <= etamax */
+
+ /*--------
+ Counters
+ --------*/
+
+ long int cp_nst; /* number of internal steps taken */
+ long int cp_nfe; /* number of f calls */
+ long int cp_ncfn; /* number of corrector convergence failures */
+ long int cp_netf; /* number of error test failures */
+ long int cp_nni; /* number of nonlinear iterations performed */
+ long int cp_nsetups; /* number of calls to lsetup */
+ int cp_nhnil; /* number of messages saying that t+h==t */
+
+ realtype cp_etaqm1; /* ratio of new to old h for order q-1 */
+ realtype cp_etaq; /* ratio of new to old h for order q */
+ realtype cp_etaqp1; /* ratio of new to old h for order q+1 */
+
+ /*----------------------------
+ Space requirements for CPODES
+ ----------------------------*/
+
+ long int cp_lrw1; /* no. of realtype words in 1 state N_Vector */
+ long int cp_liw1; /* no. of integer words in 1 state N_Vector */
+ long int cp_lrw2; /* no. of realtype words in 1 cnstr. N_Vector */
+ long int cp_liw2; /* no. of integer words in 1 cnstr. N_Vector */
+ long int cp_lrw; /* no. of realtype words in CPODES work vectors*/
+ long int cp_liw; /* no. of integer words in CPODES work vectors */
+
+ /*---------------------------------------
+ Implicit Integration Linear Solver Data
+ ---------------------------------------*/
+
+ /* Linear Solver functions to be called */
+ int (*cp_linit)(struct CPodeMemRec *cp_mem);
+ int (*cp_lsetup)(struct CPodeMemRec *cp_mem, int convfail,
+ N_Vector yP, N_Vector ypP, N_Vector fctP,
+ booleantype *jcurPtr,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+ int (*cp_lsolve)(struct CPodeMemRec *cp_mem, N_Vector b, N_Vector weight,
+ N_Vector yC, N_Vector ypC, N_Vector fctC);
+ void (*cp_lfree)(struct CPodeMemRec *cp_mem);
+
+ /* Linear Solver specific memory */
+ void *cp_lmem;
+
+ /* Does lsetup do anything? */
+ booleantype cp_lsetup_exists;
+
+ /*-----------------------------
+ Projection Linear Solver Data
+ -----------------------------*/
+
+ /* Linear Solver functions to be called */
+ int (*cp_linitP)(struct CPodeMemRec *cp_mem);
+ int (*cp_lsetupP)(struct CPodeMemRec *cp_mem,
+ N_Vector y, N_Vector cy,
+ N_Vector c_tmp1, N_Vector c_tmp2, N_Vector s_tmp1);
+ int (*cp_lsolveP)(struct CPodeMemRec *cp_mem, N_Vector b, N_Vector x,
+ N_Vector y, N_Vector cy,
+ N_Vector c_tmp1, N_Vector s_tmp1);
+ void (*cp_lmultP)(struct CPodeMemRec *cp_mem, N_Vector x, N_Vector Gx);
+ void (*cp_lfreeP)(struct CPodeMemRec *cp_mem);
+
+ /* Linear Solver specific memory */
+ void *cp_lmemP;
+
+ /* Does lsetupP do anything? */
+ booleantype cp_lsetupP_exists;
+
+ /*------------
+ Saved Values
+ ------------*/
+
+ int cp_qu; /* last successful q value used */
+ long int cp_nstlset; /* step number of last lsetup call */
+ realtype cp_h0u; /* actual initial stepsize */
+ realtype cp_hu; /* last successful h value used */
+ realtype cp_saved_tq5; /* saved value of tq[5] */
+ booleantype cp_jcur; /* Is Jacobian info current? */
+ realtype cp_tolsf; /* tolerance scale factor */
+ int cp_qmax_alloc; /* value of qmax used when allocating memory */
+ int cp_indx_acor; /* index of the zn vector where acor is saved */
+
+ booleantype cp_MallocDone;
+ booleantype cp_VabstolMallocDone;
+ booleantype cp_projMallocDone;
+ booleantype cp_rootMallocDone;
+
+
+ /*-------------------------------------------
+ Error handler function and error ouput file
+ -------------------------------------------*/
+
+ CPErrHandlerFn cp_ehfun; /* Error messages are handled by ehfun */
+ void *cp_eh_data; /* user pointer passed to ehfun */
+ FILE *cp_errfp; /* CPODES error messages are sent to errfp */
+
+ /*-------------------------
+ Stability Limit Detection
+ -------------------------*/
+
+ booleantype cp_sldeton; /* Is Stability Limit Detection on? */
+ realtype cp_ssdat[6][4]; /* scaled data array for STALD */
+ int cp_nscon; /* counter for STALD method */
+ long int cp_nor; /* counter for number of order reductions */
+
+ /*----------------
+ Rootfinding Data
+ ----------------*/
+
+ booleantype cp_doRootfinding;/* Is rootfinding enabled? */
+ CPRootFn cp_gfun; /* Function g for roots sought */
+ int cp_nrtfn; /* number of components of g */
+ void *cp_g_data; /* pointer to user data for g */
+ booleantype *cp_gactive; /* flags for active/inactive g functions */
+ int *cp_iroots; /* array for root information */
+ int *cp_rootdir; /* array specifying direction of zero-crossing */
+
+ realtype cp_tlo; /* nearest endpoint of interval in root search */
+ realtype cp_thi; /* farthest endpoint of interval in root search*/
+ realtype cp_trout; /* t value returned by rootfinding routine */
+ realtype *cp_glo; /* saved array of g values at t = tlo */
+ realtype *cp_ghi; /* saved array of g values at t = thi */
+ realtype *cp_grout; /* array of g values at t = trout */
+ realtype cp_toutc; /* copy of tout (if NORMAL mode) */
+ int cp_taskc; /* copy of parameter task */
+ int cp_irfnd; /* flag showing whether last step had a root */
+ long int cp_nge; /* counter for g evaluations */
+
+ /*------------------------------
+ Consistent IC calculation data
+ ------------------------------*/
+
+ realtype cp_icprj_convcoef;
+ realtype cp_icprj_normtol;
+ int cp_icprj_maxrcvr;
+ int cp_icprj_maxiter;
+ N_Vector cp_yy0;
+ N_Vector cp_yp0;
+
+} *CPodeMem;
+
+/*
+ * =================================================================
+ * I N T E R F A C E T O L I N E A R S O L V E R S
+ * F O R I M P L I C I T I N T E G R A T I O N
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * int (*cp_linit)(CPodeMem cp_mem);
+ * -----------------------------------------------------------------
+ * The purpose of cp_linit is to complete initializations for a
+ * specific linear solver, such as counters and statistics.
+ * An LInitFn should return 0 if it has successfully initialized the
+ * CPODES linear solver and a negative value otherwise.
+ * If an error does occur, an appropriate message should be sent to
+ * the error handler function.
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * int (*cp_lsetup)(CPodeMem cp_mem, int convfail,
+ * N_Vector yP, N_Vector ypP, N_Vector fctP,
+ * booleantype *jcurPtr,
+ * N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+ * -----------------------------------------------------------------
+ * The job of cp_lsetup is to prepare the linear solver for subsequent
+ * calls to cp_lsolve. It may recompute Jacobian-related data is it
+ * deems necessary. Its parameters are as follows:
+ *
+ * cp_mem - problem memory pointer of type CPodeMem. See the
+ * typedef earlier in this file.
+ *
+ * convfail - a flag to indicate any problem that occurred during
+ * the solution of the nonlinear equation on the
+ * current time step for which the linear solver is
+ * being used. This flag can be used to help decide
+ * whether the Jacobian data kept by a CPODES linear
+ * solver needs to be updated or not.
+ * Its possible values are documented below.
+ *
+ * NOTE: convfail is UNDEFINED for implicit-form ODE
+ *
+ * yP - the predicted y vector for the current CPODES
+ * internal step (i.e. zn[0])
+ *
+ * ypP - the predicted y' vector for the current CPODES
+ * internal step (i.e. h*zn[1] for CP_IMPL)
+ *
+ * NOTE: ypP = NULL for explicit-form ODE
+ *
+ * fctP - f(yP) or F(tn, yP, ypP).
+ *
+ * jcurPtr - a pointer to a boolean to be filled in by cp_lsetup.
+ * The function should set *jcurPtr=TRUE if its Jacobian
+ * data is current after the call and should set
+ * *jcurPtr=FALSE if its Jacobian data is not current.
+ * Note: If cp_lsetup calls for re-evaluation of
+ * Jacobian data (based on convfail and CPODES state
+ * data), it should return *jcurPtr=TRUE always;
+ * otherwise an infinite loop can result.
+ *
+ * NOTE: jcurPtr is IGNORED for implicit-form ODE
+ *
+ * tmp1, tmp2, tmp3 - temporary N_Vectors provided for use by cp_lsetup.
+ *
+ * The cp_lsetup routine should return 0 if successful, a positive
+ * value for a recoverable error, and a negative value for an
+ * unrecoverable error.
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * int (*cp_lsolve)(CPodeMem cp_mem, N_Vector b, N_Vector weight,
+ * N_Vector yC, N_Vector ypC, N_Vector fctC);
+ * -----------------------------------------------------------------
+ * cp_lsolve must solve the linear equation P x = b, where
+ * Explicit-form ODE:
+ * P is some approximation to (I - gamma * df/dy)
+ * and the RHS vector b is input.
+ * Implicit-form ODE:
+ * P is some approximation to (dF/dy' + gamma * dF/dy)
+ * and b is the current residual.
+ * Its arguments are as follows:
+ *
+ * cp_mem - problem memory pointer of type CPodeMem. See the big
+ * typedef earlier in this file.
+ *
+ * b - on input the right-hand side of the linear system
+ * to be solved. On output, the solution.
+ *
+ * weight - te current weights in the WRMS norm.
+ *
+ * yC - the solver's current approximation to y(tn)
+ *
+ * ypC - the solver's current approximation to y'(tn)
+ *
+ * NOTE: ypC = NULL for explicit-form ODE
+ *
+ * fctC - f(tn, yC) or F(tn, yC, ypc).
+ *
+ * The solution is to be returned in the vector b.
+ * The cp_lsolve function should return a positive value for a
+ * recoverable error and a negative value for an unrecoverable error.
+ * Success is indicated by a 0 return value.
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * void (*cp_lfree)(CPodeMem cp_mem);
+ * -----------------------------------------------------------------
+ * cp_lfree should free up any memory allocated by the linear
+ * solver. This routine is called once a problem has been
+ * completed and the linear solver is no longer needed.
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Communication between CPODES and a CPODES Linear Solver
+ * -----------------------------------------------------------------
+ * convfail is passed as an input to cp_lsetup. When dealing with
+ * an ODE in explicit form, this can be used to test whether the Jacobian
+ * must be reevakluated or whether it is enough to use the up-to-date gamma
+ * value.
+ *
+ * convfail can be one of:
+ *
+ * CP_NO_FAILURES : Either this is the first cp_setup call for this
+ * step, or the local error test failed on the
+ * previous attempt at this step (but the Newton
+ * iteration converged).
+ *
+ * CP_FAIL_BAD_J : This value is passed to cp_lsetup if
+ *
+ * (a) The previous Newton corrector iteration
+ * did not converge and the linear solver's
+ * setup routine indicated that its Jacobian-
+ * related data is not current
+ * or
+ * (b) During the previous Newton corrector
+ * iteration, the linear solver's solve routine
+ * failed in a recoverable manner and the
+ * linear solver's setup routine indicated that
+ * its Jacobian-related data is not current.
+ *
+ * CP_FAIL_OTHER : During the current internal step try, the
+ * previous Newton iteration failed to converge
+ * even though the linear solver was using current
+ * Jacobian-related data.
+ * -----------------------------------------------------------------
+ */
+
+
+/*
+ * =================================================================
+ * I N T E R F A C E T O L I N E A R S O L V E R S
+ * F O R P R O J E C T I O N
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * int (*cp_linitP)(CPodeMem cp_mem);
+ * -----------------------------------------------------------------
+ * The purpose of cp_linitP is to complete initializations for a
+ * specific linear solver, such as counters and statistics.
+ * An linit function should return 0 if it has successfully
+ * initialized the CPODES linear solver and a negative value otherwise.
+ * If an error does occur, an appropriate message should be sent to
+ * the error handler function.
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * int (*cp_lsetupP)(CPodeMem cp_mem,
+ * N_Vector y, N_Vector cy,
+ * N_Vector c_tmp1, N_Vector c_tmp2, N_Vector s_tmp1);
+ * -----------------------------------------------------------------
+ * The job of cp_lsetupP is to prepare the linear solver for subsequent
+ * calls to cp_lsolveP. It may recompute Jacobian-related data if it
+ * deems necessary. Its parameters are as follows:
+ *
+ * cp_mem - problem memory pointer of type CPodeMem. See the
+ * typedef earlier in this file.
+ *
+ * y - the corrected y vector for the current CPODES
+ * internal step (i.e. zn[0])
+ *
+ * cy - c(tn, y)
+ *
+ * c_tmp1, c_tmp2 - temporary N_Vectors (of same length as c) provided
+ * for use by cp_lsetupP.
+ * s_tmp1 - temporary N_Vectors (of same length as y) provided
+ * for use by cp_lsetupP.
+ *
+ * The cp_lsetupP routine should return 0 if successful, a positive
+ * value for a recoverable error, and a negative value for an
+ * unrecoverable error.
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * int (*cp_lsolveP)(CPodeMem cp_mem, N_Vector b, N_Vector x,
+ * N_Vector y, N_Vector cy,
+ * N_Vector c_tmp1, N_Vector s_tmp1);
+ * -----------------------------------------------------------------
+ * cp_lsolveP must solve the linear equation:
+ *
+ *
+ * Explicit-form ODE:
+ * P is some approximation to (I - gamma * df/dy)
+ * and the RHS vector b is input.
+ * Implicit-form ODE:
+ * P is some approximation to (dF/dy' + gamma * dF/dy)
+ * and b is the current residual.
+ * Its arguments are as follows:
+ *
+ * cp_mem - problem memory pointer of type CPodeMem. See the big
+ * typedef earlier in this file.
+ *
+ * b - on input the right-hand side of the linear system
+ * to be solved. On output, the solution.
+ *
+ * weight - te current weights in the WRMS norm.
+ *
+ * yC - the solver's current approximation to y(tn)
+ *
+ * ypC - the solver's current approximation to y'(tn)
+ *
+ * NOTE: ypC = NULL for explicit-form ODE
+ *
+ * fctC - f(tn, yC) or F(tn, yC, ypc).
+ *
+ * The solution is to be returned in the vector b.
+ * The cp_lsolve function should return a positive value for a
+ * recoverable error and a negative value for an unrecoverable error.
+ * Success is indicated by a 0 return value.
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * void (*cp_lmultP)(CPodeMem cp_mem, N_Vector x, N_Vector Gx);
+ * -----------------------------------------------------------------
+ * cp_lmultP should compute the Jacobian - vector product, for a
+ * given vector x and return the result in Gx.
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * void (*cp_lfreeP)(CPodeMem cp_mem);
+ * -----------------------------------------------------------------
+ * cp_lfreeP should free up any memory allocated by the linear
+ * solver. This routine is called once a problem has been
+ * completed and the linear solver is no longer needed.
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Communication between CPODES and a CPODES Linear Solver
+ * -----------------------------------------------------------------
+ * convfail is passed as an input to cp_lsetup. When dealing with
+ * an ODE in explicit form, this can be used to test whether the Jacobian
+ * must be reevakluated or whether it is enough to use the up-to-date gamma
+ * value.
+ *
+ * convfail can be one of:
+ *
+ * CP_NO_FAILURES : Either this is the first cp_setup call for this
+ * step, or the local error test failed on the
+ * previous attempt at this step (but the Newton
+ * iteration converged).
+ *
+ * CP_FAIL_BAD_J : This value is passed to cp_lsetup if
+ *
+ * (a) The previous Newton corrector iteration
+ * did not converge and the linear solver's
+ * setup routine indicated that its Jacobian-
+ * related data is not current
+ * or
+ * (b) During the previous Newton corrector
+ * iteration, the linear solver's solve routine
+ * failed in a recoverable manner and the
+ * linear solver's setup routine indicated that
+ * its Jacobian-related data is not current.
+ *
+ * CP_FAIL_OTHER : During the current internal step try, the
+ * previous Newton iteration failed to converge
+ * even though the linear solver was using current
+ * Jacobian-related data.
+ * -----------------------------------------------------------------
+ */
+
+
+#define CP_NO_FAILURES 0
+#define CP_FAIL_BAD_J 1
+#define CP_FAIL_OTHER 2
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_io.c b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_io.c
new file mode 100644
index 0000000..763b7ae
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_io.c
@@ -0,0 +1,1485 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.4 $
+ * $Date: 2007/04/06 20:33:24 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for the optional input and output
+ * functions for the CPODES solver.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "cpodes_private.h"
+
+#define lrw (cp_mem->cp_lrw)
+#define liw (cp_mem->cp_liw)
+#define lrw1 (cp_mem->cp_lrw1)
+#define liw1 (cp_mem->cp_liw1)
+#define lrw1Q (cp_mem->cp_lrw1Q)
+#define liw1Q (cp_mem->cp_liw1Q)
+
+/*
+ * =================================================================
+ * CPODES optional input functions
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Optional inputs to the main solver
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * CPodeSetErrHandlerFn
+ *
+ * Specifies the error handler function
+ */
+
+int CPodeSetErrHandlerFn(void *cpode_mem, CPErrHandlerFn ehfun, void *eh_data)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetErrHandlerFn", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ cp_mem->cp_ehfun = ehfun;
+ cp_mem->cp_eh_data = eh_data;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeSetErrFile
+ *
+ * Specifies the FILE pointer for output (NULL means no messages)
+ */
+
+int CPodeSetErrFile(void *cpode_mem, FILE *errfp)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetErrFile", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ cp_mem->cp_errfp = errfp;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeSetMaxOrd
+ *
+ * Specifies the maximum method order
+ */
+
+int CPodeSetMaxOrd(void *cpode_mem, int maxord)
+{
+ CPodeMem cp_mem;
+ int qmax_alloc;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetMaxOrd", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (maxord <= 0) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeSetMaxOrd", MSGCP_NEG_MAXORD);
+ return(CP_ILL_INPUT);
+ }
+
+ /* Cannot increase maximum order beyond the value that
+ was used when allocating memory */
+ qmax_alloc = cp_mem->cp_qmax_alloc;
+
+ if (maxord > qmax_alloc) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeSetMaxOrd", MSGCP_BAD_MAXORD);
+ return(CP_ILL_INPUT);
+ }
+
+ cp_mem->cp_qmax = maxord;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeSetMaxNumSteps
+ *
+ * Specifies the maximum number of integration steps
+ */
+
+int CPodeSetMaxNumSteps(void *cpode_mem, long int mxsteps)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetMaxNumSteps", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (mxsteps < 0) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeSetMaxNumSteps", MSGCP_NEG_MXSTEPS);
+ return(CP_ILL_INPUT);
+ }
+
+ /* Passing 0 sets the default */
+ if (mxsteps == 0)
+ cp_mem->cp_mxstep = MXSTEP_DEFAULT;
+ else
+ cp_mem->cp_mxstep = mxsteps;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeSetMaxHnilWarns
+ *
+ * Specifies the maximum number of warnings for small h
+ */
+
+int CPodeSetMaxHnilWarns(void *cpode_mem, int mxhnil)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetMaxHnilWarns", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ cp_mem->cp_mxhnil = mxhnil;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ *CPodeSetStabLimDet
+ *
+ * Turns on/off the stability limit detection algorithm
+ */
+
+int CPodeSetStabLimDet(void *cpode_mem, booleantype sldet)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetStabLimDet", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if( sldet && (cp_mem->cp_lmm_type != CP_BDF) ) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeSetStabLimDet", MSGCP_SET_SLDET);
+ return(CP_ILL_INPUT);
+ }
+
+ cp_mem->cp_sldeton = sldet;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeSetInitStep
+ *
+ * Specifies the initial step size
+ */
+
+int CPodeSetInitStep(void *cpode_mem, realtype hin)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetInitStep", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ cp_mem->cp_hin = hin;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeSetMinStep
+ *
+ * Specifies the minimum step size
+ */
+
+int CPodeSetMinStep(void *cpode_mem, realtype hmin)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetMinStep", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (hmin<0) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeSetMinStep", MSGCP_NEG_HMIN);
+ return(CP_ILL_INPUT);
+ }
+
+ /* Passing 0 sets hmin = zero */
+ if (hmin == ZERO) {
+ cp_mem->cp_hmin = HMIN_DEFAULT;
+ return(CP_SUCCESS);
+ }
+
+ if (hmin * cp_mem->cp_hmax_inv > ONE) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeSetMinStep", MSGCP_BAD_HMIN_HMAX);
+ return(CP_ILL_INPUT);
+ }
+
+ cp_mem->cp_hmin = hmin;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeSetMaxStep
+ *
+ * Specifies the maximum step size
+ */
+
+int CPodeSetMaxStep(void *cpode_mem, realtype hmax)
+{
+ realtype hmax_inv;
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetMaxStep", MSGCP_NO_MEM);
+ return (CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (hmax < 0) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeSetMaxStep", MSGCP_NEG_HMAX);
+ return(CP_ILL_INPUT);
+ }
+
+ /* Passing 0 sets hmax = infinity */
+ if (hmax == ZERO) {
+ cp_mem->cp_hmax_inv = HMAX_INV_DEFAULT;
+ return(CP_SUCCESS);
+ }
+
+ hmax_inv = ONE/hmax;
+ if (hmax_inv * cp_mem->cp_hmin > ONE) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeSetMaxStep", MSGCP_BAD_HMIN_HMAX);
+ return(CP_ILL_INPUT);
+ }
+
+ cp_mem->cp_hmax_inv = hmax_inv;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeSetStopTime
+ *
+ * Specifies the time beyond which the integration is not to
+ * proceed
+ */
+
+int CPodeSetStopTime(void *cpode_mem, realtype tstop)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetStopTime", MSGCP_NO_MEM);
+ return (CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ cp_mem->cp_tstop = tstop;
+ cp_mem->cp_tstopset = TRUE;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeSetMaxErrTestFails
+ *
+ * Specifies the maximum number of error test failures during one
+ * step try.
+ */
+
+int CPodeSetMaxErrTestFails(void *cpode_mem, int maxnef)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetMaxErrTestFails", MSGCP_NO_MEM);
+ return (CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ cp_mem->cp_maxnef = maxnef;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeSetMaxConvFails
+ *
+ * Specifies the maximum number of nonlinear convergence failures
+ * during one step try.
+ */
+
+int CPodeSetMaxConvFails(void *cpode_mem, int maxncf)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetMaxConvFails", MSGCP_NO_MEM);
+ return (CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ cp_mem->cp_maxncf = maxncf;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeSetMaxNonlinIters
+ *
+ * Specifies the maximum number of nonlinear iterations during
+ * one solve.
+ */
+
+int CPodeSetMaxNonlinIters(void *cpode_mem, int maxcor)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetMaxNonlinIters", MSGCP_NO_MEM);
+ return (CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ cp_mem->cp_maxcor = maxcor;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeSetNonlinConvCoef
+ *
+ * Specifies the coeficient in the nonlinear solver convergence
+ * test
+ */
+
+int CPodeSetNonlinConvCoef(void *cpode_mem, realtype nlscoef)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetNonlinConvCoef", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ cp_mem->cp_nlscoef = nlscoef;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeSetTolerances
+ *
+ * Changes the integration tolerances between calls to CPode().
+ * Here, only CP_SS or CP_SV are allowed.
+ */
+
+int CPodeSetTolerances(void *cpode_mem,
+ int tol_type, realtype reltol, void *abstol)
+{
+ CPodeMem cp_mem;
+ booleantype neg_abstol;
+
+ /* Check CPODES memory */
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetTolerances", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Check if cpode_mem was allocated */
+ if (cp_mem->cp_MallocDone == FALSE) {
+ cpProcessError(cp_mem, CP_NO_MALLOC, "CPODES", "CPodeSetTolerances", MSGCP_NO_MALLOC);
+ return(CP_NO_MALLOC);
+ }
+
+ /* Check inputs for legal values */
+ if ( (tol_type != CP_SS) && (tol_type != CP_SV) ) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeSetTolerances", MSGCP_BAD_ITOL);
+ return(CP_ILL_INPUT);
+ }
+ if (abstol == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeSetTolerances", MSGCP_NULL_ABSTOL);
+ return(CP_ILL_INPUT);
+ }
+
+ /* Check positivity of tolerances */
+ if (reltol < ZERO) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeSetTolerances", MSGCP_BAD_RELTOL);
+ return(CP_ILL_INPUT);
+ }
+
+ if (tol_type == CP_SS)
+ neg_abstol = (*((realtype *)abstol) < ZERO);
+ else
+ neg_abstol = (N_VMin((N_Vector)abstol) < ZERO);
+
+ if (neg_abstol) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeSetTolerances", MSGCP_BAD_ABSTOL);
+ return(CP_ILL_INPUT);
+ }
+
+ /* Copy tolerances into memory */
+ cp_mem->cp_tol_type = tol_type;
+ cp_mem->cp_reltol = reltol;
+
+ if (tol_type == CP_SS) {
+ cp_mem->cp_Sabstol = *((realtype *)abstol);
+ } else {
+ if ( !(cp_mem->cp_VabstolMallocDone) ) {
+ cp_mem->cp_Vabstol = N_VClone(cp_mem->cp_ewt);
+ lrw += lrw1;
+ liw += liw1;
+ cp_mem->cp_VabstolMallocDone = TRUE;
+ }
+ N_VScale(ONE, (N_Vector)abstol, cp_mem->cp_Vabstol);
+ }
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeSetEwtFn
+ *
+ * Specifies the user-provided function efun of type EwtSet
+ * and a data pointer for efun.
+ */
+
+int CPodeSetEwtFn(void *cpode_mem, CPEwtFn efun, void *e_data)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetEwtFn", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ cp_mem->cp_efun = efun;
+ cp_mem->cp_e_data = e_data;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeSetRootDirection
+ *
+ * Specifies the direction of zero-crossings to be monitored.
+ * The default is to monitor both crossings.
+ */
+
+int CPodeSetRootDirection(void *cpode_mem, int *rootdir)
+{
+ CPodeMem cp_mem;
+ int i, nrt;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetRootDirection", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+
+ cp_mem = (CPodeMem) cpode_mem;
+
+ nrt = cp_mem->cp_nrtfn;
+ if (nrt==0) {
+ cpProcessError(NULL, CP_ILL_INPUT, "CPODES", "CPodeSetRootDirection", MSGCP_NO_ROOT);
+ return(CP_ILL_INPUT);
+ }
+
+ for(i=0; i<nrt; i++) cp_mem->cp_rootdir[i] = rootdir[i];
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Optional inputs for projection step
+ * -----------------------------------------------------------------
+ */
+
+/*
+ *
+ * CPodeSetProjUpdateErrEst
+ */
+
+int CPodeSetProjUpdateErrEst(void *cpode_mem, booleantype proj_err)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetProjUpdateErrEst", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ cp_mem->cp_project_err = proj_err;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeSetProjFrequency
+ */
+
+int CPodeSetProjFrequency(void *cpode_mem, long int proj_freq)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetProjFrequency", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (proj_freq < 0) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeSetProjFrequency", MSGCP_BAD_FREQ);
+ return(CP_ILL_INPUT);
+ }
+
+ cp_mem->cp_proj_freq = proj_freq;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeSetProjTestCnstr
+ */
+
+int CPodeSetProjTestCnstr(void *cpode_mem, booleantype test_cnstr)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetProjTestCnstr", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ cp_mem->cp_test_cnstr = test_cnstr;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeSetProjLsetupFreq
+ *
+ * Spcifies the frequency of constraint Jacobian evaluations
+ * (actually, the maximum number of steps allowed between calls
+ * to the linear solver's setup function).
+ */
+
+int CPodeSetProjLsetupFreq(void *cpode_mem, long int lset_freq)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetProjLsetupFreq", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lset_freq <= 0) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeSetProjLsetupFreq", MSGCP_BAD_LSFREQ);
+ return(CP_ILL_INPUT);
+ }
+
+ cp_mem->cp_lsetupP_freq = lset_freq;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeSetProjNonlinConvCoef
+ *
+ * Specifies the coeficient in the projection nonlinear solver convergence
+ * test
+ */
+
+int CPodeSetProjNonlinConvCoef(void *cpode_mem, realtype prjcoef)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetProjNonlinConvCoef", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ cp_mem->cp_prjcoef = prjcoef;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Optional inputs for quadrature integration
+ * -----------------------------------------------------------------
+ */
+
+int CPodeSetQuadErrCon(void *cpode_mem, booleantype errconQ,
+ int tol_typeQ, realtype reltolQ, void *abstolQ)
+{
+ CPodeMem cp_mem;
+ booleantype neg_abstol;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeSetQuadErrCon", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+
+ cp_mem = (CPodeMem) cpode_mem;
+
+ cp_mem->cp_errconQ = errconQ;
+
+ /* Ckeck if quadrature was initialized? */
+
+ if (cp_mem->cp_quadMallocDone == FALSE) {
+ cpProcessError(cp_mem, CP_NO_QUAD, "CPODES", "CPodeSetQuadErrCon", MSGCP_NO_QUAD);
+ return(CP_NO_QUAD);
+ }
+
+ /* Check inputs */
+
+ if(errconQ == FALSE) {
+ if (cp_mem->cp_VabstolQMallocDone) {
+ N_VDestroy(cp_mem->cp_VabstolQ);
+ lrw -= lrw1Q;
+ liw -= liw1Q;
+ cp_mem->cp_VabstolQMallocDone = FALSE;
+ }
+ return(CP_SUCCESS);
+ }
+
+ if ((tol_typeQ != CP_SS) && (tol_typeQ != CP_SV)) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeSetQuadErrCon", MSGCP_BAD_ITOLQ);
+ return(CP_ILL_INPUT);
+ }
+
+ if (abstolQ == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeSetQuadErrCon", MSGCP_NULL_ABSTOLQ);
+ return(CP_ILL_INPUT);
+ }
+
+ if (reltolQ < ZERO) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeSetQuadErrCon", MSGCP_BAD_RELTOLQ);
+ return(CP_ILL_INPUT);
+ }
+
+ if (tol_typeQ == CP_SS)
+ neg_abstol = (*((realtype *)abstolQ) < ZERO);
+ else
+ neg_abstol = (N_VMin((N_Vector)abstolQ) < ZERO);
+
+ if (neg_abstol) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeSetQuadErrCon", MSGCP_BAD_ABSTOLQ);
+ return(CP_ILL_INPUT);
+ }
+
+ /* See if we need to free or allocate memory */
+
+ if ( (tol_typeQ != CP_SV) && (cp_mem->cp_VabstolQMallocDone) ) {
+ N_VDestroy(cp_mem->cp_VabstolQ);
+ lrw -= lrw1Q;
+ liw -= liw1Q;
+ cp_mem->cp_VabstolQMallocDone = FALSE;
+ }
+
+ if ( (tol_typeQ == CP_SV) && !(cp_mem->cp_VabstolQMallocDone) ) {
+ cp_mem->cp_VabstolQ = N_VClone(cp_mem->cp_tempvQ);
+ lrw += lrw1Q;
+ liw += liw1Q;
+ cp_mem->cp_VabstolQMallocDone = TRUE;
+ }
+
+ /* Copy tolerances into memory */
+
+ cp_mem->cp_tol_typeQ = tol_typeQ;
+ cp_mem->cp_reltolQ = reltolQ;
+
+ if (tol_typeQ == CP_SS)
+ cp_mem->cp_SabstolQ = *((realtype *)abstolQ);
+ else
+ N_VScale(ONE, (N_Vector)abstolQ, cp_mem->cp_VabstolQ);
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * =================================================================
+ * CPODE optional output functions
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Optional outputs for IC calculation
+ * -----------------------------------------------------------------
+ */
+
+int CPodeGetConsistentIC(void *cpode_mem, N_Vector yy0, N_Vector yp0)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetConsistentIC", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (cp_mem->cp_next_q != 0) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeGetConsistentIC", MSGCP_TOO_LATE);
+ return(CP_ILL_INPUT);
+ }
+
+ if (yy0 != NULL) N_VScale(ONE, cp_mem->cp_zn[0], yy0);
+ if (cp_mem->cp_ode_type == CP_IMPL)
+ if (yp0 != NULL) N_VScale(ONE, cp_mem->cp_zn[1], yp0);
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Optional outputs for integration
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * CPodeGetNumSteps
+ *
+ * Returns the current number of integration steps
+ */
+
+int CPodeGetNumSteps(void *cpode_mem, long int *nsteps)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetNumSteps", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *nsteps = cp_mem->cp_nst;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetNumFctEvals
+ *
+ * Returns the current number of calls to f
+ */
+
+int CPodeGetNumFctEvals(void *cpode_mem, long int *nfevals)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetNumFctEvals", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *nfevals = cp_mem->cp_nfe;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetNumLinSolvSetups
+ *
+ * Returns the current number of calls to the linear solver setup routine
+ */
+
+int CPodeGetNumLinSolvSetups(void *cpode_mem, long int *nlinsetups)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetNumLinSolvSetups", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *nlinsetups = cp_mem->cp_nsetups;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetNumErrTestFails
+ *
+ * Returns the current number of error test failures
+ */
+
+int CPodeGetNumErrTestFails(void *cpode_mem, long int *netfails)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetNumErrTestFails", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *netfails = cp_mem->cp_netf;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetLastOrder
+ *
+ * Returns the order on the last succesful step
+ */
+
+int CPodeGetLastOrder(void *cpode_mem, int *qlast)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetLastOrder", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *qlast = cp_mem->cp_qu;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetCurrentOrder
+ *
+ * Returns the order to be attempted on the next step
+ */
+
+int CPodeGetCurrentOrder(void *cpode_mem, int *qcur)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetCurrentOrder", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *qcur = cp_mem->cp_next_q;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetNumStabLimOrderReds
+ *
+ * Returns the number of order reductions triggered by the stability
+ * limit detection algorithm
+ */
+
+int CPodeGetNumStabLimOrderReds(void *cpode_mem, long int *nslred)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetNumStabLimOrderReds", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (cp_mem->cp_sldeton==FALSE)
+ *nslred = 0;
+ else
+ *nslred = cp_mem->cp_nor;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetActualInitStep
+ *
+ * Returns the step size used on the first step
+ */
+
+int CPodeGetActualInitStep(void *cpode_mem, realtype *hinused)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetActualInitStep", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *hinused = cp_mem->cp_h0u;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetLastStep
+ *
+ * Returns the step size used on the last successful step
+ */
+
+int CPodeGetLastStep(void *cpode_mem, realtype *hlast)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetLastStep", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *hlast = cp_mem->cp_hu;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetCurrentStep
+ *
+ * Returns the step size to be attempted on the next step
+ */
+
+int CPodeGetCurrentStep(void *cpode_mem, realtype *hcur)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetCurrentStep", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *hcur = cp_mem->cp_next_h;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetCurrentTime
+ *
+ * Returns the current value of the independent variable
+ */
+
+int CPodeGetCurrentTime(void *cpode_mem, realtype *tcur)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetCurrentTime", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *tcur = cp_mem->cp_tn;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetTolScaleFactor
+ *
+ * Returns a suggested factor for scaling tolerances
+ */
+
+int CPodeGetTolScaleFactor(void *cpode_mem, realtype *tolsfact)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetTolScaleFactor", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *tolsfact = cp_mem->cp_tolsf;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetErrWeights
+ *
+ * This routine returns the current weight vector.
+ */
+
+int CPodeGetErrWeights(void *cpode_mem, N_Vector eweight)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetErrWeights", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ N_VScale(ONE, cp_mem->cp_ewt, eweight);
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetEstLocalErrors
+ *
+ * Returns an estimate of the local error
+ */
+
+int CPodeGetEstLocalErrors(void *cpode_mem, N_Vector ele)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetEstLocalErrors", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ N_VScale(ONE, cp_mem->cp_acor, ele);
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetWorkSpace
+ *
+ * Returns integrator work space requirements
+ */
+
+int CPodeGetWorkSpace(void *cpode_mem, long int *lenrw, long int *leniw)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetWorkSpace", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *leniw = liw;
+ *lenrw = lrw;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetIntegratorStats
+ *
+ * Returns integrator statistics
+ */
+
+int CPodeGetIntegratorStats(void *cpode_mem, long int *nsteps, long int *nfevals,
+ long int *nlinsetups, long int *netfails, int *qlast,
+ int *qcur, realtype *hinused, realtype *hlast,
+ realtype *hcur, realtype *tcur)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetIntegratorStats", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *nsteps = cp_mem->cp_nst;
+ *nfevals = cp_mem->cp_nfe;
+ *nlinsetups = cp_mem->cp_nsetups;
+ *netfails = cp_mem->cp_netf;
+ *qlast = cp_mem->cp_qu;
+ *qcur = cp_mem->cp_next_q;
+ *hinused = cp_mem->cp_h0u;
+ *hlast = cp_mem->cp_hu;
+ *hcur = cp_mem->cp_next_h;
+ *tcur = cp_mem->cp_tn;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetNumGEvals
+ *
+ * Returns the current number of calls to g (for rootfinding)
+ */
+
+int CPodeGetNumGEvals(void *cpode_mem, long int *ngevals)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetNumGEvals", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *ngevals = cp_mem->cp_nge;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetRootInfo
+ *
+ * Returns pointer to array rootsfound showing roots found
+ */
+
+int CPodeGetRootInfo(void *cpode_mem, int *rootsfound)
+{
+ CPodeMem cp_mem;
+ int i, nrt;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetRootInfo", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ nrt = cp_mem->cp_nrtfn;
+
+ for (i=0; i<nrt; i++) rootsfound[i] = cp_mem->cp_iroots[i];
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetRootWindow
+ *
+ * Returns the window (tLo,tHi] within which root(s) were found. The result
+ * is meaningless unless this is called right after a root-found return.
+ */
+
+int CPodeGetRootWindow(void *cpode_mem, realtype *tLo, realtype *tHi)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetRootWindow", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *tLo = cp_mem->cp_tlo;
+ *tHi = cp_mem->cp_thi;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetNumNonlinSolvIters
+ *
+ * Returns the current number of iterations in the nonlinear solver
+ */
+
+int CPodeGetNumNonlinSolvIters(void *cpode_mem, long int *nniters)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetNumNonlinSolvIters", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *nniters = cp_mem->cp_nni;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetNumNonlinSolvConvFails
+ *
+ * Returns the current number of convergence failures in the
+ * nonlinear solver
+ */
+
+int CPodeGetNumNonlinSolvConvFails(void *cpode_mem, long int *nncfails)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetNumNonlinSolvConvFails", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *nncfails = cp_mem->cp_ncfn;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * CPodeGetNonlinSolvStats
+ *
+ * Returns nonlinear solver statistics
+ */
+
+int CPodeGetNonlinSolvStats(void *cpode_mem, long int *nniters,
+ long int *nncfails)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetNonlinSolvStats", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *nniters = cp_mem->cp_nni;
+ *nncfails = cp_mem->cp_ncfn;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Optional outputs for projection step
+ * -----------------------------------------------------------------
+ */
+
+int CPodeGetProjNumProj(void *cpode_mem, long int *nproj)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetNonlinSolvStats", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *nproj = cp_mem->cp_nproj;
+
+ return(CP_SUCCESS);
+}
+
+int CPodeGetProjNumCnstrEvals(void *cpode_mem, long int *nce)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetNonlinSolvStats", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *nce = cp_mem->cp_nce;
+
+ return(CP_SUCCESS);
+}
+
+int CPodeGetProjNumLinSolvSetups(void *cpode_mem, long int *nsetupsP)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetNonlinSolvStats", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *nsetupsP = cp_mem->cp_nsetupsP;
+
+ return(CP_SUCCESS);
+}
+
+int CPodeGetProjNumFailures(void *cpode_mem, long int *nprf)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetNonlinSolvStats", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *nprf = cp_mem->cp_nprf;
+
+ return(CP_SUCCESS);
+}
+
+int CPodeGetProjStats(void *cpode_mem, long int *nproj, long int *nce,
+ long int *nsetupsP, long int *nprf)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetNonlinSolvStats", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ *nproj = cp_mem->cp_nproj;
+ *nce = cp_mem->cp_nce;
+ *nsetupsP = cp_mem->cp_nsetupsP;
+ *nprf = cp_mem->cp_nprf;
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Optional outputs for quadrature integration
+ * -----------------------------------------------------------------
+ */
+
+int CPodeGetQuadNumFunEvals(void *cpode_mem, long int *nqevals)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetQuadNumFunEvals", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (cp_mem->cp_quadr==FALSE) {
+ cpProcessError(cp_mem, CP_NO_QUAD, "CPODES", "CPodeGetQuadNumFunEvals", MSGCP_NO_QUAD);
+ return(CP_NO_QUAD);
+ }
+
+ *nqevals = cp_mem->cp_nqe;
+
+ return(CP_SUCCESS);
+}
+
+int CPodeGetQuadErrWeights(void *cpode_mem, N_Vector eQweight)
+{
+ CPodeMem cp_mem;
+
+ if (cpode_mem==NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeGetQuadErrWeights", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (cp_mem->cp_quadr==FALSE) {
+ cpProcessError(cp_mem, CP_NO_QUAD, "CPODES", "CPodeGetQuadErrWeights", MSGCP_NO_QUAD);
+ return(CP_NO_QUAD);
+ }
+
+ if(cp_mem->cp_errconQ) N_VScale(ONE, cp_mem->cp_ewtQ, eQweight);
+
+ return(CP_SUCCESS);
+}
+
+
+/*-----------------------------------------------------------------*/
+
+char *CPodeGetReturnFlagName(int flag)
+{
+ char *name;
+
+ name = (char *)malloc(24*sizeof(char));
+
+ switch(flag) {
+ case CP_SUCCESS:
+ sprintf(name,"CP_SUCCESS");
+ break;
+ case CP_TSTOP_RETURN:
+ sprintf(name,"CP_TSTOP_RETURN");
+ break;
+ case CP_ROOT_RETURN:
+ sprintf(name,"CP_ROOT_RETURN");
+ break;
+ case CP_TOO_MUCH_WORK:
+ sprintf(name,"CP_TOO_MUCH_WORK");
+ break;
+ case CP_TOO_MUCH_ACC:
+ sprintf(name,"CP_TOO_MUCH_ACC");
+ break;
+ case CP_ERR_FAILURE:
+ sprintf(name,"CP_ERR_FAILURE");
+ break;
+ case CP_CONV_FAILURE:
+ sprintf(name,"CP_CONV_FAILURE");
+ break;
+ case CP_LINIT_FAIL:
+ sprintf(name,"CP_LINIT_FAIL");
+ break;
+ case CP_LSETUP_FAIL:
+ sprintf(name,"CP_LSETUP_FAIL");
+ break;
+ case CP_LSOLVE_FAIL:
+ sprintf(name,"CP_LSOLVE_FAIL");
+ break;
+ case CP_ODEFUNC_FAIL:
+ sprintf(name,"CP_ODEFUNC_FAIL");
+ break;
+ case CP_FIRST_ODEFUNC_ERR:
+ sprintf(name,"CP_FIRST_ODEFUNC_ERR");
+ break;
+ case CP_REPTD_ODEFUNC_ERR:
+ sprintf(name,"CP_REPTD_ODEFUNC_ERR");
+ break;
+ case CP_UNREC_ODEFUNC_ERR:
+ sprintf(name,"CP_UNREC_ODEFUNC_ERR");
+ break;
+ case CP_RTFUNC_FAIL:
+ sprintf(name,"CP_RTFUNC_FAIL");
+ break;
+ case CP_MEM_FAIL:
+ sprintf(name,"CP_MEM_FAIL");
+ break;
+ case CP_MEM_NULL:
+ sprintf(name,"CP_MEM_NULL");
+ break;
+ case CP_ILL_INPUT:
+ sprintf(name,"CP_ILL_INPUT");
+ break;
+ case CP_NO_MALLOC:
+ sprintf(name,"CP_NO_MALLOC");
+ break;
+ case CP_BAD_K:
+ sprintf(name,"CP_BAD_K");
+ break;
+ case CP_BAD_T:
+ sprintf(name,"CP_BAD_T");
+ break;
+ case CP_BAD_DKY:
+ sprintf(name,"CP_BAD_DKY");
+ break;
+ default:
+ sprintf(name,"NONE");
+ }
+
+ return(name);
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_lapack.c b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_lapack.c
new file mode 100644
index 0000000..f469490
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_lapack.c
@@ -0,0 +1,1495 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.3 $
+ * $Date: 2006/12/01 22:48:57 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for a CPODES dense linear solver
+ * using BLAS and LAPACK functions.
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * NOTE: the only operations (all O(n)) that do not use Blas/Lapack
+ * functions are:
+ *
+ * - matrix plus identity (I-gamma*J)
+ * (in lsetup -> use functions from sundials_lapack)
+ * - diagonal matrix times vector (D^(-1)*x)
+ * (in lsolveP for QR, QRP, and SC -> hardcoded)
+ * - permutation of a diagonal matrix (P*D*P^T)
+ * (in lsolveP for LU; P uses pivots from dgetrv -> hardcoded)
+ * - permutation matrix times vector (P^T*x)
+ * (in lsolveP for LU; P uses pivots from dgetr -> hardcoded)
+ * - permutation matrix times vector (P^T*b)
+ * (in lsolveP for QRP; P uses pivots from dgeqp3 -> hardcoded)
+ */
+
+/*
+ * TODO:
+ *
+ * cplLUcomputeKD
+ * cplQRcomputeKD
+ * cplSCcomputeKD
+ */
+
+/*
+ * =================================================================
+ * IMPORTED HEADER FILES
+ * =================================================================
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <cpodes/cpodes_lapack.h>
+#include "cpodes_direct_impl.h"
+#include "cpodes_private.h"
+
+#include <sundials/sundials_math.h>
+
+/*
+ * =================================================================
+ * PROTOTYPES FOR PRIVATE FUNCTIONS
+ * =================================================================
+ */
+
+/* CPLAPACK DENSE linit, lsetup, lsolve, and lfree routines */
+static int cpLapackDenseInit(CPodeMem cp_mem);
+static int cpLapackDenseSetup(CPodeMem cp_mem, int convfail,
+ N_Vector yP, N_Vector ypP, N_Vector fctP,
+ booleantype *jcurPtr,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+static int cpLapackDenseSolve(CPodeMem cp_mem, N_Vector b, N_Vector weight,
+ N_Vector yC, N_Vector ypC, N_Vector fctC);
+static void cpLapackDenseFree(CPodeMem cp_mem);
+
+/* CPLAPACK BAND linit, lsetup, lsolve, and lfree routines */
+static int cpLapackBandInit(CPodeMem cp_mem);
+static int cpLapackBandSetup(CPodeMem cp_mem, int convfail,
+ N_Vector yP, N_Vector ypP, N_Vector fctP,
+ booleantype *jcurPtr,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+static int cpLapackBandSolve(CPodeMem cp_mem, N_Vector b, N_Vector weight,
+ N_Vector yC, N_Vector ypC, N_Vector fctC);
+static void cpLapackBandFree(CPodeMem cp_mem);
+
+
+/* CPLAPACK DENSE linitP, lsetupP, lsolveP, lmultP, and lfreeP routines */
+static int cpLapackDenseProjInit(CPodeMem cp_mem);
+static int cpLapackDenseProjSetup(CPodeMem cp_mem, N_Vector y, N_Vector cy,
+ N_Vector c_tmp1, N_Vector c_tmp2, N_Vector s_tmp1);
+static int cpLapackDenseProjSolve(CPodeMem cp_mem, N_Vector b, N_Vector x,
+ N_Vector y, N_Vector cy,
+ N_Vector c_tmp1, N_Vector s_tmp1);
+static void cpLapackDenseProjMult(CPodeMem cp_mem, N_Vector x, N_Vector Gx);
+static void cpLapackDenseProjFree(CPodeMem cp_mem);
+
+/* Private functions for LU, QR, and SC projection */
+static void cplLUcomputeKD(CPodeMem cp_mem, N_Vector d);
+static void cplQRcomputeKD(CPodeMem cp_mem, N_Vector d);
+static void cplSCcomputeKD(CPodeMem cp_mem, N_Vector d);
+
+/*
+ * =================================================================
+ * READIBILITY REPLACEMENTS
+ * =================================================================
+ */
+
+#define ode_type (cp_mem->cp_ode_type)
+#define lmm_type (cp_mem->cp_lmm_type)
+#define fe (cp_mem->cp_fe)
+#define fi (cp_mem->cp_fi)
+#define f_data (cp_mem->cp_f_data)
+#define uround (cp_mem->cp_uround)
+#define nst (cp_mem->cp_nst)
+#define tn (cp_mem->cp_tn)
+#define h (cp_mem->cp_h)
+#define gamma (cp_mem->cp_gamma)
+#define gammap (cp_mem->cp_gammap)
+#define gamrat (cp_mem->cp_gamrat)
+#define ewt (cp_mem->cp_ewt)
+
+#define linit (cp_mem->cp_linit)
+#define lsetup (cp_mem->cp_lsetup)
+#define lsolve (cp_mem->cp_lsolve)
+#define lfree (cp_mem->cp_lfree)
+#define lmem (cp_mem->cp_lmem)
+#define tempv (cp_mem->cp_tempv)
+#define lsetup_exists (cp_mem->cp_lsetup_exists)
+
+#define pnorm (cp_mem->cp_proj_norm)
+
+#define linitP (cp_mem->cp_linitP)
+#define lsetupP (cp_mem->cp_lsetupP)
+#define lsolveP (cp_mem->cp_lsolveP)
+#define lmultP (cp_mem->cp_lmultP)
+#define lfreeP (cp_mem->cp_lfreeP)
+#define lmemP (cp_mem->cp_lmemP)
+#define lsetupP_exists (cp_mem->cp_lsetupP_exists)
+
+#define mtype (cpdls_mem->d_type)
+#define n (cpdls_mem->d_n)
+#define ml (cpdls_mem->d_ml)
+#define mu (cpdls_mem->d_mu)
+#define smu (cpdls_mem->d_smu)
+#define djacE (cpdls_mem->d_djacE)
+#define djacI (cpdls_mem->d_djacI)
+#define bjacE (cpdls_mem->d_bjacE)
+#define bjacI (cpdls_mem->d_bjacI)
+#define M (cpdls_mem->d_M)
+#define savedJ (cpdls_mem->d_savedJ)
+#define pivots (cpdls_mem->d_pivots)
+#define nstlj (cpdls_mem->d_nstlj)
+#define nje (cpdls_mem->d_nje)
+#define nfeDQ (cpdls_mem->d_nfeDQ)
+#define J_data (cpdls_mem->d_J_data)
+#define last_flag (cpdls_mem->d_last_flag)
+
+#define ny (cpdlsP_mem->d_ny)
+#define nc (cpdlsP_mem->d_nc)
+#define nr (cpdlsP_mem->d_nr)
+#define djacP (cpdlsP_mem->d_jacP)
+#define JP_data (cpdlsP_mem->d_JP_data)
+#define ftype (cpdlsP_mem->d_ftype)
+#define G (cpdlsP_mem->d_G)
+#define savedG (cpdlsP_mem->d_savedG)
+#define K (cpdlsP_mem->d_K)
+#define pivotsP (cpdlsP_mem->d_pivotsP)
+#define beta (cpdlsP_mem->d_beta)
+#define wrk (cpdlsP_mem->d_wrk)
+#define len_wrk (cpdlsP_mem->d_len_wrk)
+#define nstljP (cpdlsP_mem->d_nstljP)
+#define njeP (cpdlsP_mem->d_njeP)
+#define nceDQ (cpdlsP_mem->d_nceDQ)
+
+/*
+ * =================================================================
+ * EXPORTED FUNCTIONS
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * CPLapackDense
+ * -----------------------------------------------------------------
+ * This routine initializes the memory record and sets various function
+ * fields specific to the linear solver module. CPLapackDense first
+ * calls the existing lfree routine if this is not NULL. Then it sets
+ * the cp_linit, cp_lsetup, cp_lsolve, cp_lfree fields in (*cpode_mem)
+ * to be cpLapackDenseInit, cpLapackDenseSetup, cpLapackDenseSolve,
+ * and cpLapackDenseFree, respectively. It allocates memory for a
+ * structure of type CPDlsMemRec and sets the cp_lmem field in
+ * (*cpode_mem) to the address of this structure. It sets lsetup_exists
+ * in (*cpode_mem) to TRUE, and the d_jac field to the default
+ * cpDlsDenseDQJac. Finally, it allocates memory for M, pivots, and
+ * (if needed) savedJ.
+ * The return value is SUCCESS = 0, or LMEM_FAIL = -1.
+ *
+ * NOTE: The dense linear solver assumes a serial implementation
+ * of the NVECTOR package. Therefore, CPLapackDense will first
+ * test for a compatible N_Vector internal representation
+ * by checking that N_VGetArrayPointer and N_VSetArrayPointer
+ * exist.
+ * -----------------------------------------------------------------
+ */
+int CPLapackDense(void *cpode_mem, int N)
+{
+ CPodeMem cp_mem;
+ CPDlsMem cpdls_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPDIRECT_MEM_NULL, "CPLAPACK", "CPLapackDense", MSGD_CPMEM_NULL);
+ return(CPDIRECT_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Test if the NVECTOR package is compatible with the LAPACK solver */
+ if (tempv->ops->nvgetarraypointer == NULL ||
+ tempv->ops->nvsetarraypointer == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_ILL_INPUT, "CPLAPACK", "CPLapackDense", MSGD_BAD_NVECTOR);
+ return(CPDIRECT_ILL_INPUT);
+ }
+
+ if (lfree !=NULL) lfree(cp_mem);
+
+ /* Set four main function fields in cp_mem */
+ linit = cpLapackDenseInit;
+ lsetup = cpLapackDenseSetup;
+ lsolve = cpLapackDenseSolve;
+ lfree = cpLapackDenseFree;
+
+ /* Get memory for CPDlsMemRec */
+ cpdls_mem = NULL;
+ cpdls_mem = (CPDlsMem) malloc(sizeof(CPDlsMemRec));
+ if (cpdls_mem == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackDense", MSGD_MEM_FAIL);
+ return(CPDIRECT_MEM_FAIL);
+ }
+
+ /* Set matrix type */
+ mtype = SUNDIALS_DENSE;
+
+ /* Set default Jacobian routine and Jacobian data */
+ djacE = NULL;
+ djacI = NULL;
+ J_data = NULL;
+
+ last_flag = CPDIRECT_SUCCESS;
+ lsetup_exists = TRUE;
+
+ /* Set problem dimension */
+ n = N;
+
+ /* Allocate memory for M, pivot array, and (if needed) savedJ */
+ M = NULL;
+ pivots = NULL;
+ savedJ = NULL;
+
+ M = NewDenseMat(N, N);
+ if (M == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackDense", MSGD_MEM_FAIL);
+ free(cpdls_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ pivots = NewIntArray(N);
+ if (pivots == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackDense", MSGD_MEM_FAIL);
+ DestroyMat(M);
+ free(cpdls_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ if (ode_type == CP_EXPL) {
+ savedJ = NewDenseMat(N, N);
+ if (savedJ == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackDense", MSGD_MEM_FAIL);
+ DestroyMat(M);
+ DestroyArray(pivots);
+ free(cpdls_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ }
+
+ /* Attach linear solver memory to integrator memory */
+ lmem = cpdls_mem;
+
+ return(CPDIRECT_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * CPLapackBand
+ * -----------------------------------------------------------------
+ * This routine initializes the memory record and sets various function
+ * fields specific to the band linear solver module. It first calls
+ * the existing lfree routine if this is not NULL. It then sets the
+ * cp_linit, cp_lsetup, cp_lsolve, and cp_lfree fields in (*cpode_mem)
+ * to be cpLapackBandInit, cpLapackBandSetup, cpLapackBandSolve,
+ * and cpLapackBandFree, respectively. It allocates memory for a
+ * structure of type CPLapackBandMemRec and sets the cp_lmem field in
+ * (*cpode_mem) to the address of this structure. It sets lsetup_exists
+ * in (*cpode_mem) to be TRUE, mu to be mupper, ml to be mlower, and
+ * the jacE and jacI field to NULL.
+ * Finally, it allocates memory for M, pivots, and (if needed) savedJ.
+ * The CPLapackBand return value is CPDIRECT_SUCCESS = 0,
+ * CPDIRECT_MEM_FAIL = -1, or CPDIRECT_ILL_INPUT = -2.
+ *
+ * NOTE: The CPLAPACK linear solver assumes a serial implementation
+ * of the NVECTOR package. Therefore, CPLapackBand will first
+ * test for compatible a compatible N_Vector internal
+ * representation by checking that the function
+ * N_VGetArrayPointer exists.
+ * -----------------------------------------------------------------
+ */
+int CPLapackBand(void *cpode_mem, int N, int mupper, int mlower)
+{
+ CPodeMem cp_mem;
+ CPDlsMem cpdls_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPDIRECT_MEM_NULL, "CPLAPACK", "CPLapackBand", MSGD_CPMEM_NULL);
+ return(CPDIRECT_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Test if the NVECTOR package is compatible with the BAND solver */
+ if (tempv->ops->nvgetarraypointer == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_ILL_INPUT, "CPLAPACK", "CPLapackBand", MSGD_BAD_NVECTOR);
+ return(CPDIRECT_ILL_INPUT);
+ }
+
+ if (lfree != NULL) lfree(cp_mem);
+
+ /* Set four main function fields in cp_mem */
+ linit = cpLapackBandInit;
+ lsetup = cpLapackBandSetup;
+ lsolve = cpLapackBandSolve;
+ lfree = cpLapackBandFree;
+
+ /* Get memory for CPDlsMemRec */
+ cpdls_mem = NULL;
+ cpdls_mem = (CPDlsMem) malloc(sizeof(CPDlsMemRec));
+ if (cpdls_mem == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackBand", MSGD_MEM_FAIL);
+ return(CPDIRECT_MEM_FAIL);
+ }
+
+ /* Set matrix type */
+ mtype = SUNDIALS_BAND;
+
+ /* Set default Jacobian routine and Jacobian data */
+ bjacE = NULL;
+ bjacI = NULL;
+ J_data = NULL;
+
+ last_flag = CPDIRECT_SUCCESS;
+ lsetup_exists = TRUE;
+
+ /* Load problem dimension */
+ n = N;
+
+ /* Load half-bandwiths in cpdls_mem */
+ ml = mlower;
+ mu = mupper;
+
+ /* Test ml and mu for legality */
+ if ((ml < 0) || (mu < 0) || (ml >= N) || (mu >= N)) {
+ cpProcessError(cp_mem, CPDIRECT_ILL_INPUT, "CPLAPACK", "CPLapackBand", MSGD_BAD_SIZES);
+ return(CPDIRECT_ILL_INPUT);
+ }
+
+ /* Set extended upper half-bandwith for M (required for pivoting) */
+ smu = MIN(N-1, mu + ml);
+
+ /* Allocate memory for M, savedJ, and pivot arrays */
+ M = NULL;
+ pivots = NULL;
+ savedJ = NULL;
+
+ M = NewBandMat(N, mu, ml, smu);
+ if (M == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackBand", MSGD_MEM_FAIL);
+ free(cpdls_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ pivots = NewIntArray(N);
+ if (pivots == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackBand", MSGD_MEM_FAIL);
+ DestroyMat(M);
+ free(cpdls_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ if (ode_type == CP_EXPL) {
+ savedJ = NewBandMat(N, mu, ml, smu);
+ if (savedJ == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackBand", MSGD_MEM_FAIL);
+ DestroyMat(M);
+ DestroyArray(pivots);
+ free(cpdls_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ }
+
+ /* Attach linear solver memory to integrator memory */
+ lmem = cpdls_mem;
+
+ return(CPDIRECT_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * CPLapackDenseProj
+ * -----------------------------------------------------------------
+ * This routine initializes the memory record and sets various function
+ * fields specific to the dense linear solver module for the projection.
+ *
+ * NOTE: The dense linear solver assumes a serial implementation
+ * of the NVECTOR package. Therefore, CPLapackDenseProj will
+ * first test for compatible a compatible N_Vector internal
+ * representation by checking that N_VGetArrayPointer and
+ * N_VSetArrayPointer exist.
+ * -----------------------------------------------------------------
+ */
+int CPLapackDenseProj(void *cpode_mem, int Nc, int Ny, int fact_type)
+{
+ CPodeMem cp_mem;
+ CPDlsProjMem cpdlsP_mem;
+ int ier, mone = -1;
+ realtype tmp;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPDIRECT_MEM_NULL, "CPLAPACK", "CPLapackDenseProj", MSGD_CPMEM_NULL);
+ return(CPDIRECT_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Test if the NVECTOR package is compatible with the LAPACK solver */
+ if (tempv->ops->nvgetarraypointer == NULL ||
+ tempv->ops->nvsetarraypointer == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_ILL_INPUT, "CPLAPACK", "CPLapackDenseProj", MSGD_BAD_NVECTOR);
+ return(CPDIRECT_ILL_INPUT);
+ }
+
+ /* Check if fact_type has a legal value */
+ if ( (fact_type != CPDIRECT_LU) &&
+ (fact_type != CPDIRECT_QR) &&
+ (fact_type != CPDIRECT_SC) &&
+ (fact_type != CPDIRECT_QRP) ) {
+ cpProcessError(cp_mem, CPDIRECT_ILL_INPUT, "CPLAPACK", "CPLapackDenseProj", MSGD_BAD_FACT);
+ return(CPDIRECT_ILL_INPUT);
+ }
+
+ if (lfreeP !=NULL) lfreeP(cp_mem);
+
+ /* Set the five function fields in cp_mem */
+ linitP = cpLapackDenseProjInit;
+ lsetupP = cpLapackDenseProjSetup;
+ lsolveP = cpLapackDenseProjSolve;
+ lmultP = cpLapackDenseProjMult;
+ lfreeP = cpLapackDenseProjFree;
+
+ /* Get memory for CPDlsProjMemRec */
+ cpdlsP_mem = NULL;
+ cpdlsP_mem = (CPDlsProjMem) malloc(sizeof(CPDlsProjMemRec));
+ if (cpdlsP_mem == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackDenseProj", MSGD_MEM_FAIL);
+ return(CPDIRECT_MEM_FAIL);
+ }
+
+ /* Set default Jacobian routine and Jacobian data */
+ djacP = NULL;
+ JP_data = NULL;
+
+ lsetupP_exists = TRUE;
+
+ /* Allocate memory */
+ G = NULL;
+ pivotsP = NULL;
+ K = NULL;
+ beta = NULL;
+ wrk = NULL;
+
+ /* Allocate memory for G */
+ G = NewDenseMat(Ny, Nc);
+ if (G == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackDenseProj", MSGD_MEM_FAIL);
+ free(cpdlsP_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ savedG = NewDenseMat(Ny, Nc);
+ if (savedG == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackDenseProj", MSGD_MEM_FAIL);
+ DestroyMat(G);
+ free(cpdlsP_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+
+ /* Allocate additional work space, depending on factorization */
+ switch(fact_type) {
+
+ case CPDIRECT_LU:
+ /* Allocate space for pivotsP and K */
+ pivotsP = NewIntArray(Nc);
+ if (pivotsP == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackDenseProj", MSGD_MEM_FAIL);
+ DestroyMat(savedG);
+ DestroyMat(G);
+ free(cpdlsP_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ K = NewDenseMat(Ny-Nc, Ny-Nc);
+ if (K == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackDenseProj", MSGD_MEM_FAIL);
+ DestroyArray(pivotsP);
+ DestroyMat(savedG);
+ DestroyMat(G);
+ free(cpdlsP_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ break;
+
+ case CPDIRECT_QR:
+ /* Allocate space for beta */
+ beta = NewRealArray(Nc);
+ if (beta == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackDenseProj", MSGD_MEM_FAIL);
+ DestroyMat(savedG);
+ DestroyMat(G);
+ free(cpdlsP_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ /* Find optimal length of work array */
+ dgeqrf_f77(&Ny, &Nc, G->data, &Ny, beta, &tmp, &mone, &ier);
+ /* Allocate space for wrk */
+ len_wrk = (int)tmp;
+ wrk = NewRealArray(len_wrk);
+ if (wrk == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackDenseProj", MSGD_MEM_FAIL);
+ DestroyArray(beta);
+ DestroyMat(savedG);
+ DestroyMat(G);
+ free(cpdlsP_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ /* If projecting in WRMS norm, allocate space for K=Q^T*D^(-1)*Q */
+ if (pnorm == CP_PROJ_ERRNORM) {
+ K = NewDenseMat(Nc, Nc);
+ if (K == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackDenseProj", MSGD_MEM_FAIL);
+ DestroyArray(wrk);
+ DestroyArray(beta);
+ DestroyMat(savedG);
+ DestroyMat(G);
+ free(cpdlsP_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ }
+ break;
+
+ case CPDIRECT_QRP:
+ /* Allocate space for pivotsP */
+ pivotsP = NewIntArray(Nc);
+ if (pivotsP == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackDenseProj", MSGD_MEM_FAIL);
+ DestroyMat(savedG);
+ DestroyMat(G);
+ free(cpdlsP_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ /* Allocate space for beta */
+ beta = NewRealArray(Nc);
+ if (beta == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackDenseProj", MSGD_MEM_FAIL);
+ DestroyArray(pivotsP);
+ DestroyMat(savedG);
+ DestroyMat(G);
+ free(cpdlsP_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ /* Find optimal length of work array */
+ dgeqp3_f77(&Ny, &Nc, G->data, &Ny, pivotsP, beta, &tmp, &mone, &ier);
+ /* Allocate space for wrk */
+ len_wrk = (int)tmp;
+ wrk = NewRealArray(len_wrk);
+ if (wrk == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackDenseProj", MSGD_MEM_FAIL);
+ DestroyArray(beta);
+ DestroyArray(pivotsP);
+ DestroyMat(savedG);
+ DestroyMat(G);
+ free(cpdlsP_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ /* If projecting in WRMS norm, allocate space for K=Q^T*D^(-1)*Q */
+ if (pnorm == CP_PROJ_ERRNORM) {
+ K = NewDenseMat(Nc, Nc);
+ if (K == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackDenseProj", MSGD_MEM_FAIL);
+ DestroyArray(wrk);
+ DestroyArray(beta);
+ DestroyArray(pivotsP);
+ DestroyMat(savedG);
+ DestroyMat(G);
+ free(cpdlsP_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+ }
+ break;
+
+ case CPDIRECT_SC:
+ /* Allocate space for K = G * D^(-1) * G^T */
+ K = NewDenseMat(Nc, Nc);
+ if (K == NULL) {
+ cpProcessError(cp_mem, CPDIRECT_MEM_FAIL, "CPLAPACK", "CPLapackDenseProj", MSGD_MEM_FAIL);
+ DestroyMat(savedG);
+ DestroyMat(G);
+ free(cpdlsP_mem);
+ return(CPDIRECT_MEM_FAIL);
+ }
+
+ break;
+
+ }
+
+ /* Copy inputs into memory */
+ nc = Nc; /* number of constraints */
+ ny = Ny; /* number of states */
+ ftype = fact_type; /* factorization type */
+
+ /* Attach linear solver memory to integrator memory */
+ lmemP = cpdlsP_mem;
+
+ return(CPDIRECT_SUCCESS);
+}
+
+/*
+ * =================================================================
+ * PRIVATE FUNCTIONS FOR IMPLICIT INTEGRATION WITH DENSE JACOBIANS
+ * =================================================================
+ */
+
+/*
+ * cpLapackDenseInit does remaining initializations specific to the dense
+ * linear solver.
+ */
+static int cpLapackDenseInit(CPodeMem cp_mem)
+{
+ CPDlsMem cpdls_mem;
+
+ cpdls_mem = (CPDlsMem) lmem;
+
+ nje = 0;
+ nfeDQ = 0;
+ nstlj = 0;
+
+ if (ode_type == CP_EXPL && djacE == NULL) {
+ djacE = cpDlsDenseDQJacExpl;
+ J_data = cp_mem;
+ }
+
+ if (ode_type == CP_IMPL && djacI == NULL) {
+ djacI = cpDlsDenseDQJacImpl;
+ J_data = cp_mem;
+ }
+
+ last_flag = CPDIRECT_SUCCESS;
+ return(0);
+}
+
+/*
+ * cpLapackDenseSetup does the setup operations for the dense linear solver.
+ * It makes a decision whether or not to call the Jacobian evaluation
+ * routine based on various state variables, and if not it uses the
+ * saved copy (for explicit ODE only). In any case, it constructs
+ * the Newton matrix M = I - gamma*J or M = F_y' - gamma*F_y, updates
+ * counters, and calls the dense LU factorization routine.
+ */
+static int cpLapackDenseSetup(CPodeMem cp_mem, int convfail,
+ N_Vector yP, N_Vector ypP, N_Vector fctP,
+ booleantype *jcurPtr,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ CPDlsMem cpdls_mem;
+ realtype dgamma, fact;
+ booleantype jbad, jok;
+ int ier, retval, one = 1;
+
+ cpdls_mem = (CPDlsMem) lmem;
+
+ switch (ode_type) {
+
+ case CP_EXPL:
+
+ /* Use nst, gamma/gammap, and convfail to set J eval. flag jok */
+ dgamma = ABS((gamma/gammap) - ONE);
+ jbad = (nst == 0) || (nst > nstlj + CPD_MSBJ) ||
+ ((convfail == CP_FAIL_BAD_J) && (dgamma < CPD_DGMAX)) ||
+ (convfail == CP_FAIL_OTHER);
+ jok = !jbad;
+
+ if (jok) {
+
+ /* If jok = TRUE, use saved copy of J */
+ *jcurPtr = FALSE;
+ dcopy_f77(&(savedJ->ldata), savedJ->data, &one, M->data, &one);
+
+ } else {
+
+ /* If jok = FALSE, call jac routine for new J value */
+ nje++;
+ nstlj = nst;
+ *jcurPtr = TRUE;
+
+ retval = djacE(n, tn, yP, fctP, M, J_data, tmp1, tmp2, tmp3);
+ if (retval == 0) {
+ dcopy_f77(&(M->ldata), M->data, &one, savedJ->data, &one);
+ } else if (retval < 0) {
+ cpProcessError(cp_mem, CPDIRECT_JACFUNC_UNRECVR, "CPLAPACK", "cpLapackDenseSetup", MSGD_JACFUNC_FAILED);
+ last_flag = CPDIRECT_JACFUNC_UNRECVR;
+ return(-1);
+ } else if (retval > 0) {
+ last_flag = CPDIRECT_JACFUNC_RECVR;
+ return(1);
+ }
+
+ }
+
+ /* Scale J by - gamma */
+ fact = -gamma;
+ dscal_f77(&(M->ldata), &fact, M->data, &one);
+
+ /* Add identity to get M = I - gamma*J*/
+ LapackDenseAddI(M);
+
+ break;
+
+ case CP_IMPL:
+
+ /* Call Jacobian function */
+ nje++;
+ retval = djacI(n, tn, gamma, yP, ypP, fctP, M, J_data, tmp1, tmp2, tmp3);
+ if (retval == 0) {
+ break;
+ } else if (retval < 0) {
+ cpProcessError(cp_mem, CPDIRECT_JACFUNC_UNRECVR, "CPLAPACK", "cpLapackDenseSetup", MSGD_JACFUNC_FAILED);
+ last_flag = CPDIRECT_JACFUNC_UNRECVR;
+ return(-1);
+ } else if (retval > 0) {
+ last_flag = CPDIRECT_JACFUNC_RECVR;
+ return(1);
+ }
+
+ break;
+
+ }
+
+ /* Do LU factorization of M */
+ dgetrf_f77(&n, &n, M->data, &(M->ldim), pivots, &ier);
+
+ /* Return 0 if the LU was complete; otherwise return 1 */
+ last_flag = ier;
+ if (ier > 0) return(1);
+ return(0);
+}
+
+/*
+ * cpLapackDenseSolve handles the solve operation for the dense linear solver
+ * by calling the dense backsolve routine.
+ */
+static int cpLapackDenseSolve(CPodeMem cp_mem, N_Vector b, N_Vector weight,
+ N_Vector yC, N_Vector ypC, N_Vector fctC)
+{
+ CPDlsMem cpdls_mem;
+ realtype *bd, fact;
+ int ier, one = 1;
+
+ cpdls_mem = (CPDlsMem) lmem;
+
+ bd = N_VGetArrayPointer(b);
+
+ dgetrs_f77("N", &n, &one, M->data, &(M->ldim), pivots, bd, &n, &ier, 1);
+ if (ier > 0) return(1);
+
+ /* For BDF, scale the correction to account for change in gamma */
+ if ((lmm_type == CP_BDF) && (gamrat != ONE)) {
+ fact = TWO/(ONE + gamrat);
+ dscal_f77(&n, &fact, bd, &one);
+ }
+
+ last_flag = CPDIRECT_SUCCESS;
+ return(0);
+}
+
+/*
+ * cpLapackDenseFree frees memory specific to the dense linear solver.
+ */
+static void cpLapackDenseFree(CPodeMem cp_mem)
+{
+ CPDlsMem cpdls_mem;
+
+ cpdls_mem = (CPDlsMem) lmem;
+
+ DestroyMat(M);
+ DestroyArray(pivots);
+ if (ode_type == CP_EXPL) DestroyMat(savedJ);
+ free(cpdls_mem);
+ cpdls_mem = NULL;
+}
+
+/*
+ * =================================================================
+ * PRIVATE FUNCTIONS FOR IMPLICIT INTEGRATION WITH BAND JACOBIANS
+ * =================================================================
+ */
+
+/*
+ * cpLapackBandInit does remaining initializations specific to the band
+ * linear solver.
+ */
+static int cpLapackBandInit(CPodeMem cp_mem)
+{
+ CPDlsMem cpdls_mem;
+
+ cpdls_mem = (CPDlsMem) lmem;
+
+ nje = 0;
+ nfeDQ = 0;
+ nstlj = 0;
+
+ if (ode_type == CP_EXPL && bjacE == NULL) {
+ bjacE = cpDlsBandDQJacExpl;
+ J_data = cp_mem;
+ }
+
+ if (ode_type == CP_IMPL && bjacI == NULL) {
+ bjacI = cpDlsBandDQJacImpl;
+ J_data = cp_mem;
+ }
+
+ last_flag = CPDIRECT_SUCCESS;
+ return(0);
+}
+
+/*
+ * cpLapackBandSetup does the setup operations for the band linear solver.
+ * It makes a decision whether or not to call the Jacobian evaluation
+ * routine based on various state variables, and if not it uses the
+ * saved copy (for explicit ODE only). In any case, it constructs
+ * the Newton matrix M = I - gamma*J or M = F_y' - gamma*F_y, updates
+ * counters, and calls the band LU factorization routine.
+ */
+static int cpLapackBandSetup(CPodeMem cp_mem, int convfail,
+ N_Vector yP, N_Vector ypP, N_Vector fctP,
+ booleantype *jcurPtr,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ CPDlsMem cpdls_mem;
+ realtype dgamma, fact;
+ booleantype jbad, jok;
+ int ier, retval, one = 1;
+
+ cpdls_mem = (CPDlsMem) lmem;
+
+ switch (ode_type) {
+
+ case CP_EXPL:
+
+ /* Use nst, gamma/gammap, and convfail to set J eval. flag jok */
+ dgamma = ABS((gamma/gammap) - ONE);
+ jbad = (nst == 0) || (nst > nstlj + CPD_MSBJ) ||
+ ((convfail == CP_FAIL_BAD_J) && (dgamma < CPD_DGMAX)) ||
+ (convfail == CP_FAIL_OTHER);
+ jok = !jbad;
+
+ if (jok) {
+
+ /* If jok = TRUE, use saved copy of J */
+ *jcurPtr = FALSE;
+ dcopy_f77(&(savedJ->ldata), savedJ->data, &one, M->data, &one);
+
+ } else {
+
+ /* If jok = FALSE, call jac routine for new J value */
+ nje++;
+ nstlj = nst;
+ *jcurPtr = TRUE;
+
+ retval = bjacE(n, mu, ml, tn, yP, fctP, M, J_data, tmp1, tmp2, tmp3);
+ if (retval == 0) {
+ dcopy_f77(&(M->ldata), M->data, &one, savedJ->data, &one);
+ } else if (retval < 0) {
+ cpProcessError(cp_mem, CPDIRECT_JACFUNC_UNRECVR, "CPLAPACK", "cpLapackBandSetup", MSGD_JACFUNC_FAILED);
+ last_flag = CPDIRECT_JACFUNC_UNRECVR;
+ return(-1);
+ } else if (retval > 0) {
+ last_flag = CPDIRECT_JACFUNC_RECVR;
+ return(1);
+ }
+
+ }
+
+ /* Scale J by - gamma */
+ fact = -gamma;
+ dscal_f77(&(M->ldata), &fact, M->data, &one);
+
+ /* Add identity to get M = I - gamma*J*/
+ LapackBandAddI(M);
+
+ break;
+
+ case CP_IMPL:
+
+ /* Call Jacobian function */
+ nje++;
+ retval = bjacI(n, mu, ml, tn, gamma, yP, ypP, fctP, M, J_data, tmp1, tmp2, tmp3);
+ if (retval == 0) {
+ break;
+ } else if (retval < 0) {
+ cpProcessError(cp_mem, CPDIRECT_JACFUNC_UNRECVR, "CPLAPACK", "cpLapackBandSetup", MSGD_JACFUNC_FAILED);
+ last_flag = CPDIRECT_JACFUNC_UNRECVR;
+ return(-1);
+ } else if (retval > 0) {
+ last_flag = CPDIRECT_JACFUNC_RECVR;
+ return(+1);
+ }
+
+ break;
+
+ }
+
+ /* Do LU factorization of M */
+ dgbtrf_f77(&n, &n, &ml, &mu, M->data, &(M->ldim), pivots, &ier);
+
+ /* Return 0 if the LU was complete; otherwise return 1 */
+ last_flag = ier;
+ if (ier > 0) return(1);
+ return(0);
+
+}
+
+/*
+ * cpLapackBandSolve handles the solve operation for the band linear solver
+ * by calling the band backsolve routine.
+ */
+static int cpLapackBandSolve(CPodeMem cp_mem, N_Vector b, N_Vector weight,
+ N_Vector yC, N_Vector ypC, N_Vector fctC)
+{
+ CPDlsMem cpdls_mem;
+ realtype *bd, fact;
+ int ier, one = 1;
+
+ cpdls_mem = (CPDlsMem) lmem;
+
+ bd = N_VGetArrayPointer(b);
+
+ dgbtrs_f77("N", &n, &ml, &mu, &one, M->data, &(M->ldim), pivots, bd, &n, &ier, 1);
+ if (ier > 0) return(1);
+
+ /* For BDF, scale the correction to account for change in gamma */
+ if ((lmm_type == CP_BDF) && (gamrat != ONE)) {
+ fact = TWO/(ONE + gamrat);
+ dscal_f77(&n, &fact, bd, &one);
+ }
+
+ last_flag = CPDIRECT_SUCCESS;
+ return(0);
+}
+
+/*
+ * cpLapackBandFree frees memory specific to the band linear solver.
+ */
+static void cpLapackBandFree(CPodeMem cp_mem)
+{
+ CPDlsMem cpdls_mem;
+
+ cpdls_mem = (CPDlsMem) lmem;
+
+ DestroyMat(M);
+ DestroyArray(pivots);
+ if (ode_type == CP_EXPL) DestroyMat(savedJ);
+ free(cpdls_mem);
+ cpdls_mem = NULL;
+}
+
+/*
+ * =================================================================
+ * PRIVATE FUNCTIONS FOR PROJECTION WITH DENSE JACOBIANS
+ * =================================================================
+ */
+
+/*
+ * cpLapackDenseProjInit does remaining initializations specific to
+ * the dense linear solver.
+ */
+static int cpLapackDenseProjInit(CPodeMem cp_mem)
+{
+ CPDlsProjMem cpdlsP_mem;
+
+ cpdlsP_mem = (CPDlsProjMem) lmemP;
+
+ njeP = 0;
+ nceDQ = 0;
+ nstljP = 0;
+
+ if (djacP == NULL) {
+ djacP = cpDlsDenseProjDQJac;
+ JP_data = cp_mem;
+ }
+
+ return(0);
+}
+
+/*
+ * cpLapackDenseProjSetup does the setup operations for the dense
+ * linear solver.
+ * It calls the Jacobian evaluation routine and, depending on ftype,
+ * it performs various factorizations.
+ */
+static int cpLapackDenseProjSetup(CPodeMem cp_mem, N_Vector y, N_Vector cy,
+ N_Vector c_tmp1, N_Vector c_tmp2, N_Vector s_tmp1)
+{
+ int ier;
+ CPDlsProjMem cpdlsP_mem;
+ realtype *col_i, rim1, ri;
+ int i, j, nd, one = 1;
+ int retval;
+
+ realtype coef_1 = ONE, coef_0 = ZERO;
+
+ cpdlsP_mem = (CPDlsProjMem) lmemP;
+
+ nd = ny-nc;
+
+ /* Call Jacobian function (G will contain the Jacobian transposed) */
+ retval = djacP(nc, ny, tn, y, cy, G, JP_data, c_tmp1, c_tmp2);
+ njeP++;
+ if (retval < 0) {
+ cpProcessError(cp_mem, CPDIRECT_JACFUNC_UNRECVR, "CPLAPACK", "cpLapackDenseProjSetup", MSGD_JACFUNC_FAILED);
+ return(-1);
+ } else if (retval > 0) {
+ return(1);
+ }
+
+ /* Save Jacobian before factorization for possible use by lmultP */
+ dcopy_f77(&(G->ldata), G->data, &one, savedG->data, &one);
+
+ /* Factorize G, depending on ftype */
+ switch (ftype) {
+
+ case CPDIRECT_LU:
+
+ /*
+ * LU factorization of G^T with partial pivoting
+ * P*G^T = | U1^T | * L^T
+ * | U2^T |
+ * After factorization, P is encoded in pivotsP and
+ * G^T is overwritten with U1 (nc by nc unit upper triangular),
+ * U2 ( nc by ny-nc rectangular), and L (nc by nc lower triangular).
+ * Return ier if factorization failed.
+ */
+ dgetrf_f77(&ny, &nc, G->data, &ny, pivotsP, &ier);
+ if (ier != 0) return(ier);
+
+ /*
+ * Build S = U1^{-1} * U2 (in place, S overwrites U2)
+ * For each row j of G, j = nc,...,ny-1, perform
+ * a backward substitution (row version).
+ * After this step, G^T contains U1, S, and L.
+ */
+ for (j=nc; j<ny; j++)
+ dtrsv_f77("L", "T", "U", &nc, G->data, &ny, (G->data + j), &ny, 1, 1, 1);
+
+ /*
+ * Build K = D1 + S^T * D2 * S
+ * S^T is stored in g_mat[nc...ny-1][0...nc]
+ * Compute and store only the lower triangular part of K.
+ */
+ if (pnorm == CP_PROJ_L2NORM) {
+ dsyrk_f77("L", "N", &nd, &nc, &coef_1, (G->data + nc), &ny, &coef_0, K->data, &nd, 1, 1);
+ LapackDenseAddI(K);
+ } else {
+ cplLUcomputeKD(cp_mem, s_tmp1);
+ }
+
+ /*
+ * Perform Cholesky decomposition of K: K = C*C^T
+ * After factorization, the lower triangular part of K contains C.
+ * Return ier if factorization failed.
+ */
+ dpotrf_f77("L", &nd, K->data, &nd, &ier, 1);
+ if (ier != 0) return(ier);
+
+ break;
+
+ case CPDIRECT_QR:
+
+ /*
+ * QR factorization of G^T: G^T = Q*R
+ * After factorization, the upper triangular part of G^T
+ * contains the matrix R. The lower trapezoidal part of
+ * G^T, together with the array beta, encodes the orthonormal
+ * columns of Q as elementary reflectors.
+ */
+ dgeqrf_f77(&ny, &nc, G->data, &ny, beta, wrk, &len_wrk, &ier);
+ if (ier != 0) return(ier);
+
+ /* If projecting in WRMS norm */
+ if (pnorm == CP_PROJ_ERRNORM) {
+ /* Build K = Q^T * D^(-1) * Q */
+ cplQRcomputeKD(cp_mem, s_tmp1);
+ /* Perform Cholesky decomposition of K */
+ dpotrf_f77("L", &nc, K->data, &nc, &ier, 1);
+ if (ier != 0) return(ier);
+ }
+
+ break;
+
+ case CPDIRECT_QRP:
+
+ /*
+ * QR with pivoting factorization of G^T: G^T * P = Q * R.
+ * After factorization, the upper triangular part of G^T
+ * contains the matrix R. The lower trapezoidal part of
+ * G^T, together with the array beta, encodes the orthonormal
+ * columns of Q as elementary reflectors.
+ * The pivots are stored in 'pivotsP'.
+ */
+ for (i=0; i<nc; i++) pivotsP[i] = 0;
+ dgeqp3_f77(&ny, &nc, G->data, &ny, pivotsP, beta, wrk, &len_wrk, &ier);
+ if (ier != 0) return(ier);
+
+ /*
+ * Determine the number of independent constraints.
+ * After the QR factorization, the diagonal elements of R should
+ * be in decreasing order of their absolute values.
+ */
+ rim1 = ABS(G->data[0]);
+ for (i=1, nr=1; i<nc; i++, nr++) {
+ col_i = G->cols[i];
+ ri = ABS(col_i[i]);
+ if (ri < 100*uround) break;
+ if (ri/rim1 < RPowerR(uround, THREE/FOUR)) break;
+ }
+
+ /* If projecting in WRMS norm */
+ if (pnorm == CP_PROJ_ERRNORM) {
+ /* Build K = Q^T * D^(-1) * Q */
+ cplQRcomputeKD(cp_mem, s_tmp1);
+ /* Perform Cholesky decomposition of K */
+ dpotrf_f77("L", &nc, K->data, &nc, &ier, 1);
+ if (ier != 0) return(ier);
+ }
+
+ break;
+
+ case CPDIRECT_SC:
+
+ /*
+ * Build K = G*D^(-1)*G^T
+ * G^T is stored in g_mat[0...ny-1][0...nc]
+ * Compute and store only the lower triangular part of K.
+ */
+ if (pnorm == CP_PROJ_L2NORM) {
+ dsyrk_f77("L", "T", &nc, &ny, &coef_1, G->data, &ny, &coef_0, K->data, &nc, 1, 1);
+ } else {
+ cplSCcomputeKD(cp_mem, s_tmp1);
+ }
+
+ /*
+ * Perform Cholesky decomposition of K: K = C*C^T
+ * After factorization, the lower triangular part of K contains C.
+ * Return 1 if factorization failed.
+ */
+ dpotrf_f77("L", &nc, K->data, &nc, &ier, 1);
+ if (ier != 0) return(ier);
+
+ break;
+
+ }
+
+ return(0);
+}
+
+/*
+ * cpLapackDenseProjSolve handles the solve operation for the dense linear
+ * linear solver.
+ */
+static int cpLapackDenseProjSolve(CPodeMem cp_mem, N_Vector b, N_Vector x,
+ N_Vector y, N_Vector cy,
+ N_Vector c_tmp1, N_Vector s_tmp1)
+{
+ CPDlsProjMem cpdlsP_mem;
+ realtype *bd, *xd;
+ realtype *ewt_data, *d_data, *da_data, tmp;
+ int nd, i, j, k, pk, ier, one = 1;
+ realtype coef_1 = ONE, coef_0 = ZERO, coef_m1 = -ONE;
+
+ cpdlsP_mem = (CPDlsProjMem) lmemP;
+
+ ewt_data = N_VGetArrayPointer(ewt);
+ bd = N_VGetArrayPointer(b);
+ xd = N_VGetArrayPointer(x);
+ d_data = N_VGetArrayPointer(s_tmp1);
+
+ nd = ny - nc;
+
+ /* Solve the linear system, depending on ftype */
+ switch (ftype) {
+
+ case CPDIRECT_LU:
+
+ /* Solve L*U1*alpha = bd
+ * (a) solve L*beta = bd using fwd. subst.
+ * (b) solve U1*alpha = beta using bckwd. subst
+ * where L^T and U1^T are stored in G[0...nc-1][0...nc-1].
+ * beta and then alpha overwrite bd.
+ */
+ dtrsv_f77("U", "T", "N", &nc, G->data, &ny, bd, &one, 1, 1, 1);
+ dtrsv_f77("L", "T", "U", &nc, G->data, &ny, bd, &one, 1, 1, 1);
+
+ /* Compute S^T * (D1 * alpha)
+ * alpha is stored in bd.
+ * S^T is stored in g_mat[nc...ny-1][0...nc-1].
+ * Store result in x2 = x[nc...ny-1].
+ */
+ if (pnorm == CP_PROJ_ERRNORM) {
+
+ /* Load squared error weights into d */
+ for (k=0; k<ny; k++) d_data[k] = ewt_data[k] * ewt_data[k];
+ /* Permute elements of d, based on pivotsP.
+ * Note that pivot information from dgetrf is 1-based.
+ * Swap d[k] and d[pivotsP[k]]. */
+ for (k=0; k<nc; k++) {
+ pk = pivotsP[k];
+ if (pk != k) {
+ tmp = d_data[k];
+ d_data[k] = d_data[pk];
+ d_data[pk] = tmp;
+ }
+ }
+ /* Compute D1*alpha and store it into da_data */
+ da_data = N_VGetArrayPointer(c_tmp1);
+ for(k=0; k<nc; k++) da_data[k] = d_data[k] * bd[k];
+ /* Compute S^T * D1 * alpha = S^T * da */
+ dgemv_f77("N", &ny, &nc, &coef_1, (G->data + nc), &ny, da_data, &one, &coef_0, (xd + nc), &one, 1);
+
+ } else {
+
+ /* Compute S^T * alpha */
+ dgemv_f77("N", &nd, &nc, &coef_1, (G->data + nc), &ny, bd, &one, &coef_0, (xd + nc), &one, 1);
+
+ }
+
+ /* Solve K*x2 = S^T*D1*alpha, using the Cholesky decomposition available in K.
+ * S^T*D1*alpha is stored in x2 = x[nc...ny-1].
+ */
+ dpotrs_f77("L", &nd, &one, K->data, &nd, (xd + nc), &nd, &ier, 1);
+ if (ier != 0) return(ier);
+
+ /* Compute x1 = alpha - S*x2
+ * alpha is stored in bd.
+ * x2 is stored in x[nc...ny-1].
+ * S^T is stored in g_mat[nc...ny-1][0...nc-1].
+ * Store result in x1 = x[0...nc-1].
+ */
+ dcopy_f77(&nc, bd, &one, xd, &one);
+ dgemv_f77("T", &nd, &nc, &coef_m1, (G->data + nc), &ny, (xd + nc), &one, &coef_1, xd, &one, 1);
+
+ /* Compute P^T * x, where P is encoded into pivotsP.
+ * Note that pivot information from dgetrf is 1-based.
+ * Store result in x.
+ */
+ for (k=nc-1; k>=0; k--) {
+ pk = pivotsP[k]-1;
+ if(pk != k) {
+ tmp = xd[k];
+ xd[k] = xd[pk];
+ xd[pk] = tmp;
+ }
+ }
+
+ break;
+
+ case CPDIRECT_QR:
+
+ /*
+ * Solve R^T*alpha = bd using fwd. subst. (row version)
+ * The upper triangular matrix R is stored in g_mat[0...nc-1][0...nc-1]
+ * alpha overwrites bd.
+ */
+ dtrsv_f77("U", "T", "N", &nc, G->data, &ny, bd, &one, 1, 1, 1);
+
+ /* If projecting in WRMS norm, solve K*beta = alpha */
+ if (pnorm == CP_PROJ_ERRNORM) {
+ dpotrs_f77("L", &nc, &one, K->data, &nc, bd, &nc, &ier, 1);
+ if (ier != 0) return(ier);
+ }
+
+ /*
+ * Compute x = Q1*alpha
+ *
+ * Since we cannot really use the "thin" QR decomposition, we
+ * first need to initialize xd = [alpha; 0].
+ */
+ for (k=0; k<nc; k++) xd[k] = bd[k];
+ for (k=nc; k<ny; k++) xd[k] = ZERO;
+ dormqr_f77("L", "N", &ny, &one, &nc, G->data, &ny, beta, xd, &ny, wrk, &len_wrk, &ier, 1, 1);
+ if (ier != 0) return(ier);
+
+ /* If projecting in WRMS norm, scale x by D^(-1) */
+ if (pnorm == CP_PROJ_ERRNORM) {
+ for (i=0; i<ny; i++)
+ xd[i] /= ewt_data[i]*ewt_data[i];
+ }
+
+ break;
+
+
+ case CPDIRECT_QRP:
+
+ /* Compute P^T * b, where P is encoded into pivotsP.
+ * If pivotsP[j] = k, then, for the factorization, the j-th column of G^T*P
+ * was the k-th column of G^T.
+ * Therefore, to compute P^T*b, we must move the k-th element of b to the
+ * j-th position, for j=1,2,... This is a forward permutation.
+ * Note that pivot information from dgeqp3 is 1-based.
+ * Store result in b.
+ */
+ for (i=1; i<=nc; i++) pivotsP[i-1] = -pivotsP[i-1];
+
+ for (i=1; i<=nc; i++) {
+
+ if (pivotsP[i-1] > 0) continue;
+
+ j = i;
+ pivotsP[j-1] = -pivotsP[j-1];
+ pk = pivotsP[j-1];
+
+ while (pivotsP[pk-1] < 0) {
+
+ tmp = bd[j-1];
+ bd[j-1] = bd[pk-1];
+ bd[pk-1] = tmp;
+
+ pivotsP[pk-1] = -pivotsP[pk-1];
+ j = pk;
+ pk = pivotsP[pk-1];
+ }
+
+ }
+
+ /*
+ * Solve R11^T * alpha = P^T * bd using fwd. subst. (row version)
+ * The upper triangular matrix R is stored in g_mat[0...nr-1][0...nr-1]
+ * P^T * bd is available in bd.
+ * We only consider the first nr components in P^T*bd.
+ * alpha overwrites bd.
+ */
+ dtrsv_f77("U", "T", "N", &nr, G->data, &ny, bd, &one, 1, 1, 1);
+
+ /* If projecting in WRMS norm, solve K*beta = alpha */
+ if (pnorm == CP_PROJ_ERRNORM) {
+ dpotrs_f77("L", &nc, &one, K->data, &nc, bd, &nc, &ier, 1);
+ if (ier != 0) return(ier);
+ }
+
+ /*
+ * Compute x = Q1*alpha
+ *
+ * Since we cannot really use the "thin" QR decomposition, we
+ * first need to initialize xd = [alpha; 0].
+ */
+ for (k=0; k<nr; k++) xd[k] = bd[k];
+ for (k=nr; k<ny; k++) xd[k] = ZERO;
+ dormqr_f77("L", "N", &ny, &one, &nc, G->data, &ny, beta, xd, &ny, wrk, &len_wrk, &ier, 1, 1);
+ if (ier != 0) return(ier);
+
+ /* If projecting in WRMS norm, scale x by D^(-1) */
+ if (pnorm == CP_PROJ_ERRNORM) {
+ for (i=0; i<ny; i++)
+ xd[i] /= ewt_data[i]*ewt_data[i];
+ }
+
+ break;
+
+
+ case CPDIRECT_SC:
+
+ /*
+ * Solve K*xi = bd, using the Cholesky decomposition available in K.
+ * xi overwrites bd.
+ */
+ dpotrs_f77("L", &nc, &one, K->data, &nc, bd, &nc, &ier, 1);
+ if (ier != 0) return(ier);
+
+ /* Compute x = G^T * xi
+ * G^T is stored in g_mat[0...ny-1][0...nc-1]
+ * xi is available in bd.
+ */
+ dgemv_f77("N", &ny, &nc, &coef_1, G->data, &ny, bd, &one, &coef_0, xd, &one, 1);
+
+ /* If projecting in WRMS norm, scale x by D^(-1) */
+ if (pnorm == CP_PROJ_ERRNORM) {
+ for (i=0; i<ny; i++)
+ xd[i] /= ewt_data[i]*ewt_data[i];
+ }
+
+ break;
+
+ }
+
+ return(0);
+}
+
+/*
+ * cpDenseProjMult computes the Jacobian-vector product used a saved
+ * Jacobian copy.
+ */
+static void cpLapackDenseProjMult(CPodeMem cp_mem, N_Vector x, N_Vector Gx)
+{
+ CPDlsProjMem cpdlsP_mem;
+ realtype coef_1 = ONE, coef_0 = ZERO;
+ realtype *xd, *Gxd;
+ int one = 1;
+
+ cpdlsP_mem = (CPDlsProjMem) lmemP;
+
+ xd = N_VGetArrayPointer(x);
+ Gxd = N_VGetArrayPointer(Gx);
+
+ dgemv_f77("T", &ny, &nc, &coef_1, savedG->data, &ny, xd, &one, &coef_0, Gxd, &one, 1);
+}
+
+/*
+ * cpLapackDenseProjFree frees memory specific to the dense linear solver.
+ */
+static void cpLapackDenseProjFree(CPodeMem cp_mem)
+{
+ CPDlsProjMem cpdlsP_mem;
+
+ cpdlsP_mem = (CPDlsProjMem) lmemP;
+
+ DestroyMat(G);
+ DestroyMat(savedG);
+ switch (ftype) {
+ case CPDIRECT_LU:
+ DestroyArray(pivotsP);
+ DestroyMat(K);
+ break;
+ case CPDIRECT_QR:
+ DestroyArray(wrk);
+ DestroyArray(beta);
+ if (pnorm == CP_PROJ_ERRNORM) DestroyMat(K);
+ break;
+ case CPDIRECT_QRP:
+ DestroyArray(wrk);
+ DestroyArray(beta);
+ DestroyArray(pivotsP);
+ if (pnorm == CP_PROJ_ERRNORM) DestroyMat(K);
+ break;
+ case CPDIRECT_SC:
+ DestroyMat(K);
+ break;
+ }
+
+ free(cpdlsP_mem);
+ cpdlsP_mem = NULL;
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Private functions for LU-, QR-, and SC-based projection
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * Compute the lower triangle of K = D1 + S^T*D2*S,
+ * D = diag(D1, D2) = P*W*P^T, W is a diagonal matrix
+ * containing the squared error weights, and P is the
+ * permutation matrix encoded into pivotsP.
+ * D1 has length nc and D2 has length (ny-nc).
+ */
+static void cplLUcomputeKD(CPodeMem cp_mem, N_Vector d)
+{
+ /* RADU:: implement this ... */
+}
+
+static void cplQRcomputeKD(CPodeMem cp_mem, N_Vector d)
+{
+ /* RADU:: implement this ... */
+}
+
+static void cplSCcomputeKD(CPodeMem cp_mem, N_Vector d)
+{
+ /* RADU:: implement this ... */
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_nls.c b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_nls.c
new file mode 100644
index 0000000..674dfdb
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_nls.c
@@ -0,0 +1,801 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/12/01 22:48:57 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementatino for the nonlinear solvers for CPODES.
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * NOTE: cpNlsFunctionalImpl is never used
+ * (the combination CP_IMPL/CP_FUNCTIONAL is disallowed in CPodeCreate)
+ */
+
+
+/*
+ * =================================================================
+ * Import Header Files
+ * =================================================================
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
+
+#include "cpodes_private.h"
+#include <sundials/sundials_math.h>
+
+/*
+ * =================================================================
+ * Private Function Prototypes
+ * =================================================================
+ */
+
+static int cpNlsFunctionalExpl(CPodeMem cp_mem);
+static int cpNlsNewtonExpl(CPodeMem cp_mem, int nflag);
+static int cpNewtonIterationExpl(CPodeMem cp_mem);
+
+static int cpNlsFunctionalImpl(CPodeMem cp_mem);
+static int cpNlsNewtonImpl(CPodeMem cp_mem, int nflag);
+static int cpNewtonIterationImpl(CPodeMem cp_mem);
+
+/*
+ * =================================================================
+ * Readibility Constants
+ * =================================================================
+ */
+
+#define ode_type (cp_mem->cp_ode_type)
+#define nls_type (cp_mem->cp_nls_type)
+
+#define fi (cp_mem->cp_fi)
+#define fe (cp_mem->cp_fe)
+#define f_data (cp_mem->cp_f_data)
+
+#define zn (cp_mem->cp_zn)
+#define ewt (cp_mem->cp_ewt)
+#define y (cp_mem->cp_y)
+#define yp (cp_mem->cp_yp)
+#define maxcor (cp_mem->cp_maxcor)
+#define acor (cp_mem->cp_acor)
+#define tempv (cp_mem->cp_tempv)
+#define ftemp (cp_mem->cp_ftemp)
+#define q (cp_mem->cp_q)
+#define h (cp_mem->cp_h)
+#define next_h (cp_mem->cp_next_h)
+#define hmin (cp_mem->cp_hmin)
+#define hscale (cp_mem->cp_hscale)
+#define eta (cp_mem->cp_eta)
+#define etamax (cp_mem->cp_etamax)
+#define tn (cp_mem->cp_tn)
+#define tq (cp_mem->cp_tq)
+#define rl1 (cp_mem->cp_rl1)
+#define gamma (cp_mem->cp_gamma)
+#define gammap (cp_mem->cp_gammap)
+#define gamrat (cp_mem->cp_gamrat)
+#define crate (cp_mem->cp_crate)
+#define acnrm (cp_mem->cp_acnrm)
+#define maxncf (cp_mem->cp_maxncf)
+#define mnewt (cp_mem->cp_mnewt)
+#define nst (cp_mem->cp_nst)
+#define nfe (cp_mem->cp_nfe)
+#define ncfn (cp_mem->cp_ncfn)
+#define nni (cp_mem->cp_nni)
+#define nsetups (cp_mem->cp_nsetups)
+#define nscon (cp_mem->cp_nscon)
+
+#define lsetup (cp_mem->cp_lsetup)
+#define lsolve (cp_mem->cp_lsolve)
+
+#define jcur (cp_mem->cp_jcur)
+#define nstlset (cp_mem->cp_nstlset)
+
+#define lsetup_exists (cp_mem->cp_lsetup_exists)
+
+/*
+ * =================================================================
+ * Main interface function
+ * =================================================================
+ */
+
+/*
+ * cpNls
+ *
+ * This routine attempts to solve the nonlinear system associated
+ * with a single implicit step of the linear multistep method.
+ * Depending on nls_type, it calls cpNlsFunctional or cpNlsNewton
+ * to do the work.
+ *
+ * The input arguments to cpNls are as follows:
+ * nflag is one of FIRST_CALL, PREV_CONV_FAIL, or PREV_ERR_FAIL.
+ * saved_t is the old value of tn (in case we need to restore)
+ * ncfPtr is a pointer accumulating nls failures at current step.
+ *
+ * If the nonlinear solver is successful, cpNls retuns CP_SUCCESS.
+ *
+ * If the nonlinear solver fails, the following actions are taken:
+ *
+ * (1) ncfn and ncf=*ncfPtr are incremented and Nordsieck array zn
+ * is restored.
+ *
+ * (2) If the solution of the nonlinear system failed due to an
+ * unrecoverable failure by lsetup, lsolve, or fun, we return
+ * an appropriate failure flag (CP_LSETUP_FAIL, CP_LSOLVE_FAIL,
+ * or CP_ODEFUNC_FAIL) which will tell cpStep to halt.
+ *
+ * (3) Otherwise, a recoverable failure occurred when solving the
+ * nonlinear system (flag == CONV_FAIL or ODEFUNC_RECVR).
+ * In this case, if ncf is now equal to maxncf or |h| = hmin,
+ * we return the value CP_CONV_FAILURE (if flag=CONV_FAIL) or
+ * CP_REPTD_ODEFUNC_ERR (if flag=ODEFUNC_RECVR).
+ * If not, we return the value PREDICT_AGAIN, telling cpStep to
+ * reattempt the step (with nflag = PREV_CONV_FAIL).
+ */
+
+int cpNls(CPodeMem cp_mem, int nflag, realtype saved_t, int *ncfPtr)
+{
+ int flag = CP_SUCCESS;
+
+ switch(nls_type) {
+
+ case CP_FUNCTIONAL:
+ switch(ode_type) {
+ case CP_EXPL:
+ flag = cpNlsFunctionalExpl(cp_mem);
+ break;
+ case CP_IMPL:
+ flag = cpNlsFunctionalImpl(cp_mem);
+ break;
+ }
+ break;
+
+ case CP_NEWTON:
+ switch(ode_type) {
+ case CP_EXPL:
+ flag = cpNlsNewtonExpl(cp_mem, nflag);
+ break;
+ case CP_IMPL:
+ flag = cpNlsNewtonImpl(cp_mem, nflag);
+ break;
+ }
+ break;
+
+ }
+
+#ifdef CPODES_DEBUG
+ printf(" acnrm = %lg\n",acnrm);
+#endif
+
+ /* Return now if the nonlinear solver was successful */
+ if (flag == CP_SUCCESS) return(CP_SUCCESS);
+
+ /* The nonlinear solver failed. Increment ncfn and restore zn */
+ ncfn++;
+ cpRestore(cp_mem, saved_t);
+
+ /* Return if lsetup, lsolve, or rhs failed unrecoverably */
+ if (flag == CP_LSETUP_FAIL) return(CP_LSETUP_FAIL);
+ if (flag == CP_LSOLVE_FAIL) return(CP_LSOLVE_FAIL);
+ if (flag == CP_ODEFUNC_FAIL) return(CP_ODEFUNC_FAIL);
+
+ /* At this point, flag = CONV_FAIL or ODEFUNC_RECVR; increment ncf */
+ (*ncfPtr)++;
+ etamax = ONE;
+
+ /* If we had maxncf failures or |h| = hmin,
+ return CP_CONV_FAILURE or CP_REPTD_ODEFUNC_ERR. */
+ if ((ABS(h) <= hmin*ONEPSM) || (*ncfPtr == maxncf)) {
+ if (flag == CONV_FAIL) return(CP_CONV_FAILURE);
+ if (flag == ODEFUNC_RECVR) return(CP_REPTD_ODEFUNC_ERR);
+ }
+
+ /* Reduce step size; return to reattempt the step */
+ eta = MAX(ETACF, hmin / ABS(h));
+ cpRescale(cp_mem);
+
+ return(PREDICT_AGAIN);
+}
+
+/*
+ * =================================================================
+ * Functions for explicit-form ODE
+ * =================================================================
+ */
+
+/*
+ * cpNlsFunctionalExpl
+ *
+ * This routine attempts to solve the nonlinear system for an ODE in
+ * explicit form using functional iteration (no matrices involved).
+ *
+ * Possible return values are:
+ *
+ * CV_SUCCESS ---> continue with error test
+ *
+ * CV_ODEFUNC_FAIL ---> halt the integration
+ *
+ * CONV_FAIL -+
+ * ODEFUNC_RECVR -+-> predict again or stop if too many
+ *
+ */
+
+static int cpNlsFunctionalExpl(CPodeMem cp_mem)
+{
+ int retval, m;
+ realtype del, delp, dcon;
+
+#ifdef CPODES_DEBUG
+ printf(" Functional, explicit ODE\n");
+#endif
+
+ /* Initialize counter */
+ crate = ONE;
+ m = 0;
+
+ /* Evaluate f at predicted y */
+ retval = fe(tn, zn[0], tempv, f_data);
+ nfe++;
+ if (retval < 0) return(CP_ODEFUNC_FAIL);
+ if (retval > 0) return(ODEFUNC_RECVR);
+
+ /* Initialize accumulated correction to 0 */
+ N_VConst(ZERO, acor);
+
+ /* Initialize delp to avoid compiler warning message */
+ del = delp = ZERO;
+
+ /* Loop until convergence; accumulate corrections in acor */
+ loop {
+
+ nni++;
+
+ /* Correct y directly from the last f value */
+ N_VLinearSum(h, tempv, -ONE, zn[1], tempv);
+ N_VScale(rl1, tempv, tempv);
+ N_VLinearSum(ONE, zn[0], ONE, tempv, y);
+
+ /* Get WRMS norm of current correction to use in convergence test */
+ N_VLinearSum(ONE, tempv, -ONE, acor, acor);
+ del = N_VWrmsNorm(acor, ewt);
+
+ /* Update accumulated correction */
+ N_VScale(ONE, tempv, acor);
+
+ /* Test for convergence. If m > 0, an estimate of the convergence
+ rate constant is stored in crate, and used in the test. */
+ if (m > 0) crate = MAX(NLS_CRDOWN * crate, del / delp);
+ dcon = del * MIN(ONE, crate) / tq[4];
+ if (dcon <= ONE) {
+ acnrm = (m == 0) ? del : N_VWrmsNorm(acor, ewt);
+ return(CP_SUCCESS); /* Convergence achieved */
+ }
+
+ /* Stop at maxcor iterations or if iter. seems to be diverging */
+ m++;
+ if ((m==maxcor) || ((m >= 2) && (del > NLS_RDIV * delp))) return(CONV_FAIL);
+
+ /* Save norm of correction, evaluate f, and loop again */
+ delp = del;
+
+ /* Evaluate f again */
+ retval = fe(tn, y, tempv, f_data);
+ nfe++;
+ if (retval < 0) return(CP_ODEFUNC_FAIL);
+ if (retval > 0) return(ODEFUNC_RECVR);
+
+ }
+}
+
+/*
+ * cpNlsNewtonExpl
+ *
+ * This routine handles the Newton iteration for an ODE in explicit form.
+ * It calls lsetup if indicated, calls cpNewtonIterationExpl to perform
+ * the actual Newton iteration, and retries a failed attempt at Newton
+ * iteration if that is indicated.
+ *
+ * Possible return values:
+ *
+ * CP_SUCCESS ---> continue with error test
+ *
+ * CP_ODEFUNC_FAIL -+
+ * CP_LSETUP_FAIL |-> halt the integration
+ * CP_LSOLVE_FAIL -+
+ *
+ * CONV_FAIL -+
+ * ODEFUNC_RECVR -+-> predict again or stop if too many
+ *
+ */
+
+static int cpNlsNewtonExpl(CPodeMem cp_mem, int nflag)
+{
+ N_Vector vtemp1, vtemp2, vtemp3;
+ int convfail, retval, ier;
+ booleantype callSetup;
+
+#ifdef CPODES_DEBUG
+ printf(" Newton, explicit ODE\n");
+#endif
+
+ vtemp1 = acor; /* rename acor as vtemp1 for readability */
+ vtemp2 = y; /* rename y as vtemp2 for readability */
+ vtemp3 = tempv; /* rename tempv as vtemp3 for readability */
+
+ /* Set flag convfail, input to lsetup for its evaluation decision */
+ convfail = ((nflag == FIRST_CALL) || (nflag == PREV_ERR_FAIL)) ?
+ CP_NO_FAILURES : CP_FAIL_OTHER;
+
+ /* Decide whether or not to call setup routine (if one exists) */
+ if (lsetup_exists) {
+ callSetup = (nflag == PREV_CONV_FAIL) || (nflag == PREV_ERR_FAIL) ||
+ (nst == 0) || (nst >= nstlset + NLS_MSBLS) || (ABS(gamrat-ONE) > DGMAX);
+ } else {
+ crate = ONE;
+ callSetup = FALSE;
+ }
+
+ /* Begin the main loop. This loop is traversed at most twice.
+ * The second pass only occurs when the first pass had a recoverable
+ * failure with old Jacobian data.
+ */
+ loop {
+
+ /* Evaluate function at predicted y */
+ retval = fe(tn, zn[0], ftemp, f_data);
+ nfe++;
+ if (retval < 0) return(CP_ODEFUNC_FAIL);
+ if (retval > 0) return(ODEFUNC_RECVR);
+
+ /* If needed, call setup function (ypP = NULL in this case) */
+ if (callSetup) {
+ ier = lsetup(cp_mem, convfail, zn[0], NULL, ftemp, &jcur,
+ vtemp1, vtemp2, vtemp3);
+
+#ifdef CPODES_DEBUG
+ printf(" Linear solver setup return value = %d\n",ier);
+#endif
+
+ nsetups++;
+ callSetup = FALSE;
+ gamrat = crate = ONE;
+ gammap = gamma;
+ nstlset = nst;
+ if (ier < 0) return(CP_LSETUP_FAIL);
+ if (ier > 0) return(CONV_FAIL);
+ }
+
+ /* Set acor to zero and load prediction into y vector */
+ N_VConst(ZERO, acor);
+ N_VScale(ONE, zn[0], y);
+
+ /* Do the Newton iteration */
+
+#ifdef CPODES_DEBUG
+ printf(" NonlinearIteration\n");
+#endif
+
+ ier = cpNewtonIterationExpl(cp_mem);
+
+#ifdef CPODES_DEBUG
+ printf(" NonlinearIteration return value = %d\n",ier);
+#endif
+
+ /* If there is a convergence failure and the Jacobian-related
+ data appears not to be current, loop again with a call to lsetup
+ in which convfail=CP_FAIL_BAD_J. Otherwise return. */
+
+ if ( (ier > 0) && (lsetup_exists) && (!jcur) ) {
+ callSetup = TRUE;
+ convfail = CP_FAIL_BAD_J;
+ continue;
+ }
+
+ return(ier);
+
+ }
+}
+
+/*
+ * cpNewtonIterationExpl
+ *
+ * This routine performs the actual Newton iteration for an ODE system
+ * in explicit form.
+ *
+ * Possible return values:
+ *
+ * CP_SUCCESS - the Newton iteration was successful.
+ *
+ * CP_LSOLVE_FAIL - the linear solver solution failed unrecoverably.
+ * CP_ODEFUNC_FAIL - the ODE function failed unrecoverably.
+ *
+ * ODEFUNC_RECVR - the ODE function failed recoverably.
+ * CONV_FAIL - a recoverable error occured.
+ */
+
+static int cpNewtonIterationExpl(CPodeMem cp_mem)
+{
+ int m, retval;
+ realtype del, delp, dcon;
+ N_Vector b;
+
+ mnewt = m = 0;
+
+ /* Initialize delp to avoid compiler warning message */
+ del = delp = ZERO;
+
+ /* Looping point for Newton iteration */
+ loop {
+
+#ifdef CPODES_DEBUG
+ printf(" Iteration # %d\n",m);
+#endif
+#ifdef CPODES_DEBUG_SERIAL
+ printf(" zn[0] = "); N_VPrint_Serial(zn[0]);
+ printf(" zn[1] = "); N_VPrint_Serial(zn[1]);
+ printf(" ewt = "); N_VPrint_Serial(ewt);
+ printf(" acor = "); N_VPrint_Serial(acor);
+ printf(" ftemp = "); N_VPrint_Serial(ftemp);
+#endif
+
+ /* Evaluate the residual of the nonlinear system*/
+ N_VLinearSum(rl1, zn[1], ONE, acor, tempv);
+ N_VLinearSum(gamma, ftemp, -ONE, tempv, tempv);
+
+ /* Call the lsolve function */
+ b = tempv;
+
+#ifdef CPODES_DEBUG
+ printf(" Linear solver solve\n");
+#endif
+#ifdef CPODES_DEBUG_SERIAL
+ printf(" rhs = "); N_VPrint_Serial(b);
+#endif
+
+ retval = lsolve(cp_mem, b, ewt, y, NULL, ftemp);
+
+#ifdef CPODES_DEBUG_SERIAL
+ printf(" sol = "); N_VPrint_Serial(b);
+#endif
+#ifdef CPODES_DEBUG
+ printf(" Linear solver solve return value = %d\n",retval);
+#endif
+
+ nni++;
+ if (retval < 0) return(CP_LSOLVE_FAIL);
+ if (retval > 0) return(CONV_FAIL);
+
+ /* Get WRMS norm of correction; add correction to acor and y */
+ del = N_VWrmsNorm(b, ewt);
+
+#ifdef CPODES_DEBUG
+ printf(" Norm of correction: del = %lg\n", del);
+#endif
+
+ N_VLinearSum(ONE, acor, ONE, b, acor);
+ N_VLinearSum(ONE, zn[0], ONE, acor, y);
+
+ /* Test for convergence. If m > 0, an estimate of the convergence
+ rate constant is stored in crate, and used in the test. */
+ if (m > 0) {
+ crate = MAX(NLS_CRDOWN * crate, del/delp);
+ }
+ dcon = del * MIN(ONE, crate) / tq[4];
+
+#ifdef CPODES_DEBUG
+ printf(" Convergence test dcon = %lg\n", dcon);
+#endif
+
+ if (dcon <= ONE) {
+ acnrm = (m==0) ? del : N_VWrmsNorm(acor, ewt);
+
+#ifdef CPODES_DEBUG_SERIAL
+ printf(" acor = "); N_VPrint_Serial(acor);
+#endif
+#ifdef CPODES_DEBUG
+ printf(" Accumulated correction norm = %lg\n", acnrm);
+#endif
+
+ jcur = FALSE;
+ return(CP_SUCCESS); /* Nonlinear system was solved successfully */
+ }
+
+ mnewt = ++m;
+
+ /* Stop at maxcor iterations or if iter. seems to be diverging. */
+ if ((m == maxcor) || ((m >= 2) && (del > NLS_RDIV*delp))) return(CONV_FAIL);
+
+ /* Save norm of correction, evaluate f, and loop again */
+ delp = del;
+ retval = fe(tn, y, ftemp, f_data);
+ nfe++;
+ if (retval < 0) return(CP_ODEFUNC_FAIL);
+ if (retval > 0) return(ODEFUNC_RECVR);
+
+ } /* end loop */
+}
+
+
+/*
+ * =================================================================
+ * Functions for implicit-form ODE
+ * =================================================================
+ */
+
+/*
+ * cpNlsFunctionalImpl
+ *
+ * This routine attempts to solve the nonlinear system for an ODE in
+ * implicit form using functional iteration (no matrices involved).
+ *
+ * The system to be solved for y is written as
+ *
+ * y = y - gamma * F( y, yP' + (y - yP)/gamma )
+ *
+ * At each iterate, y' is obtained as
+ *
+ * y' = yP' + 1/gamma * ( y - yP )
+ *
+ * where yP and yP' are the predicted values for y and y'.
+ *
+ * The iterate updates are thus coded as:
+ * y' <- yP' + 1/gamma * ( y - yP )
+ * y <- yP - gamma * F(y,y')
+ *
+ * Possible return values are:
+ *
+ * CP_SUCCESS ---> continue with error test
+ *
+ * CP_ODEFUNC_FAIL ---> halt the integration
+ *
+ * CONV_FAIL -+
+ * ODEFUNC_RECVR -+-> predict again or stop if too many
+ *
+ */
+
+static int cpNlsFunctionalImpl(CPodeMem cp_mem)
+{
+ int retval, m;
+ realtype del, delp, dcon;
+
+#ifdef CPODES_DEBUG
+ printf(" Functional, implicit ODE\n");
+#endif
+
+ /* Initialize counter */
+ crate = ONE;
+ m = 0;
+
+ /* Evaluate f at predicted y and y' */
+ N_VScale(ONE, zn[0], y);
+ N_VScale(ONE/h, zn[1], yp);
+ retval = fi(tn, y, yp, ftemp, f_data);
+ nfe++;
+ if (retval < 0) return(CP_ODEFUNC_FAIL);
+ if (retval > 0) return(ODEFUNC_RECVR);
+
+ /* Initialize accumulated correction to 0 */
+ N_VConst(ZERO, acor);
+
+ /* Initialize delp to avoid compiler warning message */
+ del = delp = ZERO;
+
+ /* Loop until convergence; accumulate corrections in acor */
+ loop {
+
+ nni++;
+
+ /* Correct y directly from the last f value ( y <- y - gamma*f ) */
+ N_VScale(-gamma, ftemp, tempv);
+ N_VLinearSum(ONE, y, ONE, tempv, y);
+
+ /* Get WRMS norm of current correction to use in convergence test */
+ del = N_VWrmsNorm(tempv, ewt);
+
+ /* Update accumulated correction */
+ N_VLinearSum(ONE, acor, ONE, tempv, acor);
+
+ /* Test for convergence. If m > 0, an estimate of the convergence
+ rate constant is stored in crate, and used in the test. */
+ if (m > 0) crate = MAX(NLS_CRDOWN * crate, del / delp);
+ dcon = del * MIN(ONE, crate) / tq[4];
+ if (dcon <= ONE) {
+ acnrm = (m == 0) ? del : N_VWrmsNorm(acor, ewt);
+ return(CP_SUCCESS); /* Convergence achieved */
+ }
+
+ /* Stop at maxcor iterations or if iter. seems to be diverging */
+ m++;
+ if ((m==maxcor) || ((m >= 2) && (del > NLS_RDIV * delp))) return(CONV_FAIL);
+
+ /* Save norm of correction */
+ delp = del;
+
+ /* Update y' and evaluate f again */
+ N_VLinearSum(ONE, y, -ONE, zn[0], yp);
+ N_VLinearSum(ONE/gamma, yp, ONE/h, zn[1], yp);
+ retval = fi(tn, y, yp, ftemp, f_data);
+ nfe++;
+ if (retval < 0) return(CP_ODEFUNC_FAIL);
+ if (retval > 0) return(ODEFUNC_RECVR);
+
+ }
+}
+
+/*
+ * cpNlsNewtonImpl
+ *
+ * This routine handles the Newton iteration for an ODE in implicit form.
+ * It calls lsetup if indicated, calls cpNewtonIterationImpl to perform
+ * the actual Newton iteration, and retries a failed attempt at Newton
+ * iteration if that is indicated.
+ *
+ * Possible return values:
+ *
+ * CP_SUCCESS ---> continue with error test
+ *
+ * CP_ODEFUNC_FAIL -+
+ * CP_LSETUP_FAIL |-> halt the integration
+ * CP_LSOLVE_FAIL -+
+ *
+ * CONV_FAIL -+
+ * ODEFUNC_RECVR -+-> predict again or stop if too many
+ *
+ */
+
+static int cpNlsNewtonImpl(CPodeMem cp_mem, int nflag)
+{
+ N_Vector vtemp1, vtemp2, vtemp3;
+ int convfail, retval;
+ booleantype callSetup;
+
+#ifdef CPODES_DEBUG
+ printf(" Newton, implicit ODE\n");
+#endif
+
+ /* In this situation we do not pass convfail information to
+ the linear sovler setup function */
+ convfail = 0;
+
+ /* Vectors used as temporary space in lsetup */
+ vtemp1 = acor;
+ vtemp2 = y;
+ vtemp3 = tempv;
+
+ /* If the linear solver provides a setup function, call it if:
+ * - we are at the first step (nst == 0), or
+ * - gamma changed significantly, or
+ * - enough steps passed from last evaluation
+ */
+ if (lsetup_exists) {
+ callSetup = (nst == 0) || (ABS(gamrat-ONE) > DGMAX) || (nst >= nstlset + NLS_MSBLS);
+ } else {
+ callSetup = FALSE;
+ crate = ONE;
+ }
+
+
+ /* Begin the main loop. This loop is traversed at most twice.
+ * The second pass only occurs when the first pass had a recoverable
+ * failure with old Jacobian data.
+ */
+ loop {
+
+ /* Evaluate predicted y' from Nordsieck array */
+ N_VScale(ONE/h, zn[1], yp);
+
+ /* Evaluate residual at predicted y and y' */
+ retval = fi(tn, zn[0], yp, ftemp, f_data);
+ nfe++;
+ if (retval < 0) return(CP_ODEFUNC_FAIL);
+ if (retval > 0) return(ODEFUNC_RECVR);
+
+ /* If needed, call setup function */
+ if (callSetup) {
+ retval = lsetup(cp_mem, 0, zn[0], yp, ftemp, &jcur,
+ vtemp1, vtemp2, vtemp3);
+ nsetups++;
+ nstlset = nst;
+ crate = ONE;
+ gammap = gamma;
+ gamrat = ONE;
+ if (retval < 0) return(CP_LSETUP_FAIL);
+ if (retval > 0) return(CONV_FAIL);
+ }
+
+ /* Set acor to zero and load prediction into y vector */
+ N_VConst(ZERO, acor);
+ N_VScale(ONE, zn[0], y);
+
+ /* Do the Newton iteration */
+ retval = cpNewtonIterationImpl(cp_mem);
+
+ /* On a recoverable failure, if setup was not called, reattempt loop */
+ if ( (retval > 0) && lsetup_exists && !callSetup ) {
+ callSetup = TRUE;
+ continue;
+ }
+
+ /* Return (success or unrecoverable failure) */
+ return(retval);
+
+ }
+}
+
+/*
+ * cpNewtonIterationImpl
+ *
+ * This routine performs actual the Newton iteration for an ODE system
+ * given in implicit form.
+ *
+ * Possible return values:
+ *
+ * CP_SUCCESS - the Newton iteration was successful.
+ *
+ * CP_LSOLVE_FAIL - the linear solver solution failed unrecoverably.
+ * CP_ODEFUNC_FAIL - the ODE function failed unrecoverably.
+ *
+ * ODEFUNC_RECVR - the ODE function failed recoverably.
+ * CONV_FAIL - a recoverable error occured.
+ */
+
+static int cpNewtonIterationImpl(CPodeMem cp_mem)
+{
+ int m, retval;
+ realtype del, delp, dcon;
+
+ /* Initialize counters */
+ mnewt = m = 0;
+
+ /* Initialize delp to avoid compiler warning message */
+ del = delp = ZERO;
+
+ /* Looping point for Newton iteration */
+ loop {
+
+ /* Evaluate the residual of the nonlinear system*/
+ N_VScale(-gamma, ftemp, tempv);
+
+ /* Call the lsolve function (solution in tempv) */
+ retval = lsolve(cp_mem, tempv, ewt, y, yp, ftemp);
+ nni++;
+ if (retval < 0) return(CP_LSOLVE_FAIL);
+ if (retval > 0) return(CONV_FAIL);
+
+ /* Get WRMS norm of correction and add correction to acor, y, and yp */
+ del = N_VWrmsNorm(tempv, ewt);
+ N_VLinearSum(ONE, acor, ONE, tempv, acor);
+ N_VLinearSum(ONE, y, ONE, tempv, y);
+ N_VLinearSum(ONE, yp, ONE/gamma, tempv, yp);
+
+ /* Test for convergence. If m > 0, an estimate of the convergence
+ rate constant is stored in crate, and used in the test. */
+ if (m > 0) crate = MAX(NLS_CRDOWN * crate, del/delp);
+ dcon = del * MIN(ONE, crate) / tq[4];
+ if (dcon <= ONE) {
+ acnrm = (m==0) ? del : N_VWrmsNorm(acor, ewt);
+ return(CP_SUCCESS);
+ }
+
+ mnewt = ++m;
+
+ /* Stop at maxcor iterations or if iter. seems to be diverging. */
+ if ((m == maxcor) || ((m >= 2) && (del > NLS_RDIV*delp))) return(CONV_FAIL);
+
+ /* Save norm of correction, evaluate f, and loop again */
+ delp = del;
+ retval = fi(tn, y, yp, ftemp, f_data);
+ nfe++;
+ if (retval < 0) return(CP_ODEFUNC_FAIL);
+ if (retval > 0) return(ODEFUNC_RECVR);
+
+ } /* end loop */
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_private.h b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_private.h
new file mode 100644
index 0000000..063b752
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_private.h
@@ -0,0 +1,371 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.7 $
+ * $Date: 2007/10/26 21:51:29 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * Private header file for CPODES.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _CPODES_PRIVATE_H
+#define _CPODES_PRIVATE_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <stdarg.h>
+#include "cpodes_impl.h"
+
+/*
+ * =================================================================
+ * C P O D E S I N T E R N A L C O N S T A N T S
+ * =================================================================
+ */
+
+/*
+ * Default values for algorithmic constants
+ * ---------------------------------------------------------------
+ *
+ * Step size and number of steps
+ *
+ * HMIN_DEFAULT default value for minimum step size
+ * HMAX_INV_DEFAULT default value for inverse of maximum step size
+ * MXHNIL_DEFAULT default value for mxhnil
+ * MXSTEP_DEFAULT default value for maximum number of steps
+ *
+ * Nonlinear solver
+ *
+ * MXNCF max no. of convergence failures during one step try
+ * NLS_MAXCOR maximum no. of corrector iterations for the nonlinear solver
+ * NLS_TEST_COEF constant in error test (used in the test quantity tq[4])
+ * NLS_CRDOWN constant used in the estimation of the convergence rate (crate)
+ * NLS_RDIV declare divergence if ratio del/delp > NLS_RDIV
+ * NLS_MSBLS max no. of steps between lsetup calls
+ * DGMAX when nls_type=NEWTON, |gamma/gammap-1| > DGMAX => call lsetup
+ * ETACF maximum step size decrease on convergence failure
+ *
+ * Error test
+ *
+ * MXNEF max no. of error test failures during one step try
+ * MXNEF1 max no. of error test failures before forcing a reduction of order
+ * SMALL_NEF if an error failure occurs and SMALL_NEF <= nef <= MXNEF1, then
+ * reset eta = MIN(eta, ETAMXF)
+ * LONG_WAIT number of steps to wait before considering an order change when
+ * q==1 and MXNEF1 error test failures have occurred
+ * SMALL_NST nst > SMALL_NST => use ETAMX3
+ *
+ * Projection
+ *
+ * MXNPF max no. of projection failures during one step try
+ * PRJ_MAXCOR maximum no. of iterations for the nonlinear projection
+ * PRJ_TEST_COEF constant used in projection tolerances
+ * PRJ_CRDOWN constant used in the estimation of the convergence rate (crateP)
+ * PRJ_RDIV declare divergence if ratio del/delp > PRJ_RDIV
+ * PRJ_MSBLS max no. of steps between lsetupP calls
+ *
+ *
+ * Other
+ *
+ * FUZZ_FACTOR factor used in defining an infinitesimal time interval
+ */
+
+#define HMIN_DEFAULT RCONST(0.0)
+#define HMAX_INV_DEFAULT RCONST(0.0)
+#define MXHNIL_DEFAULT 10
+#define MXSTEP_DEFAULT 500
+
+#define MXNCF 10
+#define NLS_MAXCOR 3
+#define NLS_TEST_COEF RCONST(0.1)
+#define NLS_CRDOWN RCONST(0.3)
+#define NLS_RDIV RCONST(2.0)
+#define NLS_MSBLS 20
+#define DGMAX RCONST(0.3)
+#define ETACF RCONST(0.25)
+
+#define MXNEF 7
+#define MXNEF1 3
+#define SMALL_NEF 2
+#define LONG_WAIT 10
+#define SMALL_NST 10
+
+#define MXNPF 10
+#define PRJ_MAXCOR 3
+#define PRJ_TEST_COEF RCONST(0.1)
+#define PRJ_CRDOWN RCONST(0.3)
+#define PRJ_RDIV RCONST(2.0)
+#define PRJ_MSBLS 1
+
+#define FUZZ_FACTOR RCONST(100.0)
+
+
+/*
+ * Control constants for communication between main integrator and
+ * lower level functions in cpStep
+ * ---------------------------------------------------------------
+ *
+ * cpNls input nflag values:
+ * FIRST_CALL
+ * PREV_CONV_FAIL
+ * PREV_PROJ_FAIL
+ * PREV_ERR_FAIL
+ *
+ * cpNls return values:
+ * CP_SUCCESS,
+ * CP_LSETUP_FAIL, CP_LSOLVE_FAIL, CP_ODEFUNC_FAIL,
+ * CP_CONV_FAILURE, CP_REPTD_ODEFUNC_ERR,
+ * PREDICT_AGAIN
+ *
+ * cpDoProjection return values:
+ * CP_SUCCESS,
+ * CP_PLSETUP_FAIL, CP_PLSOLVE_FAIL, CP_CNSTRFUNC_FAIL, CP_PROJFUNC_FAIL
+ * CP_PROJ_FAILURE, CP_REPTD_CNSTRFUNC_ERR, CP_REPTD_PROJFUNC_ERR,
+ * PREDICT_AGAIN
+ *
+ * cpDoErrorTest return values:
+ * CP_SUCCESS,
+ * CP_ERR_FAILURE,
+ * PREDICT_AGAIN
+ *
+ * cpRcheck* return values:
+ * CP_RTFUNC_FAIL,
+ * RTFOUND,
+ * CP_SUCCESS
+ */
+
+#define PREDICT_AGAIN +3
+
+#define FIRST_CALL +101
+#define PREV_CONV_FAIL +102
+#define PREV_PROJ_FAIL +103
+#define PREV_ERR_FAIL +104
+
+#define CONV_FAIL +110
+#define ODEFUNC_RECVR +111
+#define CNSTRFUNC_RECVR +112
+#define PROJFUNC_RECVR +113
+#define QUADFUNC_RECVR +114
+
+#define RTFOUND +1
+
+/*
+ * CPODES Private Constants
+ * ---------------------------------------------------------------
+ */
+
+#define ZERO RCONST(0.0)
+#define TINY RCONST(1.0e-10)
+#define PT001 RCONST(0.001)
+#define PT01 RCONST(0.01)
+#define PT1 RCONST(0.1)
+#define PT2 RCONST(0.2)
+#define PT25 RCONST(0.25)
+#define HALF RCONST(0.5)
+#define ONE RCONST(1.0)
+#define ONEPSM RCONST(1.000001)
+#define TWO RCONST(2.0)
+#define THREE RCONST(3.0)
+#define FOUR RCONST(4.0)
+#define FIVE RCONST(5.0)
+#define TWELVE RCONST(12.0)
+#define HUNDRED RCONST(100.0)
+
+/*
+ * =================================================================
+ * C P O D E S I N T E R N A L F U N C T I O N S
+ * S H A R E D A M O N G M O D U L E S
+ * =================================================================
+ */
+
+/* Nonlinear solver function */
+int cpNls(CPodeMem cp_mem, int nflag, realtype saved_t, int *ncfPtr);
+
+/* Projection step function */
+int cpDoProjection(CPodeMem cp_mem, realtype saved_t, int *npfPtr);
+
+/* Internal error weight computation */
+int cpEwtSet(N_Vector ycur, N_Vector weight, void *edata);
+
+/* Error return handler */
+void cpProcessError(CPodeMem cp_mem,
+ int error_code, const char *module, const char *fname,
+ const char *msgfmt, ...);
+
+/* Functions acting on the Nordsieck history array */
+void cpRestore(CPodeMem cp_mem, realtype saved_t);
+void cpRescale(CPodeMem cp_mem);
+
+/* Function evaluating solution at given time */
+int cpGetSolution(void *cpode_mem, realtype t, N_Vector yret, N_Vector ypret);
+
+/* Root finding functions */
+int cpRcheck1(CPodeMem cp_mem);
+int cpRcheck2(CPodeMem cp_mem);
+int cpRcheck3(CPodeMem cp_mem);
+void cpRootFree(CPodeMem cp_mem);
+
+/*
+ * =================================================================
+ * M A C R O
+ * =================================================================
+ */
+
+#define loop for(;;)
+
+/*
+ * =================================================================
+ * C V O D E E R R O R M E S S A G E S
+ * =================================================================
+ */
+
+#if defined(SUNDIALS_EXTENDED_PRECISION)
+
+#define MSG_TIME "t = %Lg"
+#define MSG_TIME_H "t = %Lg and h = %Lg"
+#define MSG_TIME_INT "t = %Lg is not between tcur - hu = %Lg and tcur = %Lg."
+#define MSG_TIME_TOUT "tout = %Lg"
+
+#elif defined(SUNDIALS_DOUBLE_PRECISION)
+
+#define MSG_TIME "t = %lg"
+#define MSG_TIME_H "t = %lg and h = %lg"
+#define MSG_TIME_INT "t = %lg is not between tcur - hu = %lg and tcur = %lg."
+#define MSG_TIME_TOUT "tout = %lg"
+
+#else
+
+#define MSG_TIME "t = %g"
+#define MSG_TIME_H "t = %g and h = %g"
+#define MSG_TIME_INT "t = %g is not between tcur - hu = %g and tcur = %g."
+#define MSG_TIME_TOUT "tout = %g"
+
+#endif
+
+/* Initialization and I/O error messages */
+
+#define MSGCP_NO_MEM "cpode_mem = NULL illegal."
+#define MSGCP_CPMEM_FAIL "Allocation of cpode_mem failed."
+#define MSGCP_MEM_FAIL "A memory request failed."
+#define MSGCP_BAD_ODE "Illegal value for ode. The legal values are CP_EXPL and CP_IMPL."
+#define MSGCP_BAD_LMM "Illegal value for lmm_type. The legal values are CP_ADAMS and CP_BDF."
+#define MSGCP_BAD_NLS "Illegal value for nls_type. The legal values are CP_FUNCTIONAL and CP_NEWTON."
+#define MSGCP_BAD_ODE_NLS "Illegal combination ode_type=CP_IMPL nls_type=CP_FUNCTIONAL."
+#define MSGCP_BAD_ITOL "Illegal value for tol_type. The legal values are CP_SS, CP_SV, and CP_WF."
+#define MSGCP_NO_MALLOC "Attempt to call before CPodeInit."
+#define MSGCP_BAD_PROJ "Illegal value for proj_type."
+#define MSGCP_BAD_NORM "Illegal value for proj_norm."
+#define MSGCP_BAD_CNSTR "Illegal value for cnstr_type."
+#define MSGCP_CTOL_NULL "ctol = NULL illegal."
+#define MSGCP_NEG_MAXORD "maxord <= 0 illegal."
+#define MSGCP_BAD_MAXORD "Illegal attempt to increase maximum method order."
+#define MSGCP_NEG_MXSTEPS "mxsteps < 0 illegal."
+#define MSGCP_SET_SLDET "Attempt to use stability limit detection with the CP_ADAMS method illegal."
+#define MSGCP_NEG_HMIN "hmin < 0 illegal."
+#define MSGCP_NEG_HMAX "hmax < 0 illegal."
+#define MSGCP_BAD_HMIN_HMAX "Inconsistent step size limits: hmin > hmax."
+#define MSGCP_BAD_RELTOL "reltol < 0 illegal."
+#define MSGCP_BAD_ABSTOL "abstol has negative component(s) (illegal)."
+#define MSGCP_NULL_ABSTOL "abstol = NULL illegal."
+#define MSGCP_NULL_Y0 "y0 = NULL illegal."
+#define MSGCP_NULL_YP0 "yp0 = NULL illegal for ode_type=CP_IMPL."
+#define MSGCP_NULL_F "fun = NULL illegal."
+#define MSGCP_NULL_G "gfun = NULL illegal."
+#define MSGCP_NULL_C "cfun = NULL illegal."
+#define MSGCP_BAD_NVECTOR "A required vector operation is not implemented."
+#define MSGCP_BAD_K "Illegal value for k."
+#define MSGCP_NULL_DKY "dky = NULL illegal."
+#define MSGCP_BAD_T "Illegal value for t." MSG_TIME_INT
+#define MSGCP_BAD_FREQ "proj_freq < 0 illegal."
+#define MSGCP_BAD_LSFREQ "lset_freq <= 0 illegal."
+#define MSGCP_TOO_LATE "CPodeGetConsistentIC can only be called before the first call to CPode."
+
+
+/* CPode error messages */
+
+#define MSGCP_LSOLVE_NULL "The linear solver's solve routine is NULL."
+#define MSGCP_YOUT_NULL "yout = NULL illegal."
+#define MSGCP_YPOUT_NULL "ypout = NULL illegal."
+#define MSGCP_TRET_NULL "tret = NULL illegal."
+#define MSGCP_BAD_EWT "Initial ewt has component(s) equal to zero (illegal)."
+#define MSGCP_EWT_NOW_BAD "At " MSG_TIME ", a component of ewt has become <= 0."
+#define MSGCP_BAD_MODE "Illegal value for mode."
+#define MSGCP_BAD_H0 "h0 and tout - t0 inconsistent."
+#define MSGCP_BAD_TOUT "Trouble interpolating at " MSG_TIME_TOUT ". tout too far back in direction of integration"
+#define MSGCP_NO_EFUN "tol_type = CP_WF but no error weight function was provided."
+#define MSGCP_NO_PFUN "proj_type = CP_PROJ_USER but no projection function was provided."
+#define MSGCP_NO_CFUN "proj_type = CP_PROJ_INTERNAL but no constraint function was provided."
+#define MSGCP_NO_TSTOP "mode = CP_NORMAL_TSTOP or mode = CP_ONE_STEP_TSTOP but tstop was not set."
+#define MSGCP_EWT_FAIL "The user-provided EwtSet function failed."
+#define MSGCP_EWT_NOW_FAIL "At " MSG_TIME ", the user-provides EwtSet function failed."
+#define MSGCP_LINIT_FAIL "The linear solver's init routine failed."
+#define MSGCP_HNIL_DONE "The above warning has been issued mxhnil times and will not be issued again for this problem."
+#define MSGCP_TOO_CLOSE "tout too close to t0 to start integration."
+#define MSGCP_MAX_STEPS "At " MSG_TIME ", mxstep steps taken before reaching tout."
+#define MSGCP_TOO_MUCH_ACC "At " MSG_TIME ", too much accuracy requested."
+#define MSGCP_HNIL "Internal " MSG_TIME_H " are such that t + h = t on the next step. The solver will continue anyway."
+#define MSGCP_ERR_FAILS "At " MSG_TIME_H ", the error test failed repeatedly or with |h| = hmin."
+#define MSGCP_CONV_FAILS "At " MSG_TIME_H ", the corrector convergence test failed repeatedly or with |h| = hmin."
+#define MSGCP_SETUP_FAILED "At " MSG_TIME ", the setup routine failed in an unrecoverable manner."
+#define MSGCP_SOLVE_FAILED "At " MSG_TIME ", the solve routine failed in an unrecoverable manner."
+#define MSGCP_ODEFUNC_FAILED "At " MSG_TIME ", the ODE funciton failed in an unrecoverable manner."
+#define MSGCP_ODEFUNC_UNREC "At " MSG_TIME ", the ODE function failed in a recoverable manner, but no recovery is possible."
+#define MSGCP_ODEFUNC_REPTD "At " MSG_TIME "repeated recoverable ODE function errors."
+#define MSGCP_ODEFUNC_FIRST "The ODE function failed at the first call."
+#define MSGCP_RTFUNC_FAILED "At " MSG_TIME ", the rootfinding routine failed in an unrecoverable manner."
+#define MSGCP_BAD_TSTOP "tstop is behind current " MSG_TIME "in the direction of integration."
+#define MSGCP_NO_ROOT "Rootfinding was not initialized."
+
+/* Projection error messages */
+
+#define MSGCP_PLSOLVE_NULL "The projection linear solver's solve function is NULL."
+#define MSGCP_PLINIT_FAIL "The projection linear solver's init function failed."
+#define MSGCP_PLSETUP_FAILED "At " MSG_TIME ", the projection linear solver's setup function failed in an unrecoverable manner."
+#define MSGCP_PLSOLVE_FAILED "At " MSG_TIME ", the projection linear solver's solve function failed in an unrecoverable manner."
+#define MSGCP_PROJ_FAILS "At " MSG_TIME_H ", the projection failed repeatedly or with |h| = hmin."
+#define MSGCP_CNSTRFUNC_FAILED "At " MSG_TIME ", the constraint function failed in an unrecoverable manner."
+#define MSGCP_CNSTRFUNC_REPTD "At " MSG_TIME "repeated recoverable constraint function errors."
+#define MSGCP_PROJFUNC_FAILED "At " MSG_TIME ", the projection function failed in an unrecoverable manner."
+#define MSGCP_PROJFUNC_REPTD "At " MSG_TIME "repeated recoverable projection function errors."
+
+/* Quadrature integration error messages */
+
+#define MSGCP_NO_QUAD "Illegal attempt to call before calling CPodeQuadInit."
+#define MSGCP_BAD_ITOLQ "Illegal value for tol_typeQ. The legal values are CP_SS and CP_SV."
+#define MSGCP_NULL_ABSTOLQ "abstolQ = NULL illegal."
+#define MSGCP_BAD_RELTOLQ "reltolQ < 0 illegal."
+#define MSGCP_BAD_ABSTOLQ "abstolQ has negative component(s) (illegal)."
+#define MSGCP_BAD_EWTQ "Initial ewtQ has component(s) equal to zero (illegal)."
+#define MSGCP_EWTQ_NOW_BAD "At " MSG_TIME ", a component of ewtQ has become <= 0."
+#define MSGCP_QUADFUNC_FAILED "At " MSG_TIME ", the quadrature function failed in an unrecoverable manner."
+#define MSGCP_QUADFUNC_UNREC "At " MSG_TIME ", the quadrature function failed in a recoverable manner, but no recovery is possible."
+#define MSGCP_QUADFUNC_REPTD "At " MSG_TIME "repeated recoverable quadrature function errors."
+#define MSGCP_QUADFUNC_FIRST "The quadrature function failed at the first call."
+
+/* IC calculation error messages */
+#define MSGCP_IC_NO_WORK "Nothing to do for an explicit-form ODE if no constraints are defined."
+#define MSGCP_IC_CNSTRFUNC_FIRST "The constraint function failed at the first call."
+#define MSGCP_IC_CNSTRFUNC_FAILED "The constraint function failed in an unrecoverable manner."
+#define MSGCP_IC_CNSTRFUNC_REPTD "Repeated recoverable constraint function errors."
+#define MSGCP_IC_PROJFUNC_FAILED "The projection function failed in an unrecoverable manner."
+#define MSGCP_IC_PLSETUP_FAILED "The projection linear solver's setup function failed in an unrecoverable manner."
+#define MSGCP_IC_PLSOLVE_FAILED "The projection linear solver's solve function failed in an unrecoverable manner."
+#define MSGCP_IC_NO_RECOVERY "The linear solver's solve function failed recoverably, but the Jacobian data is already current."
+#define MSGCP_IC_PROJ_FAILS "WTF?"
+#define MSGCP_IC_EWT_BAD "A component of ewt has become <= 0."
+#define MSGCP_IC_EWT_FAIL "The user-provided EwtSet function failed."
+#define MSGCP_IC_ODEFUNC_FIRST "The ODE function failed at the first call."
+#define MSGCP_IC_ODEFUNC_FAILED "The ODE funciton failed in an unrecoverable manner."
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_proj.c b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_proj.c
new file mode 100644
index 0000000..02215ad
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_proj.c
@@ -0,0 +1,511 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.3 $
+ * $Date: 2006/11/30 21:11:29 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementatino for the internal projection step.
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * =================================================================
+ * Import Header Files
+ * =================================================================
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "cpodes_private.h"
+#include <sundials/sundials_math.h>
+#include <sundials/sundials_types.h>
+
+/* Maximum step size decrease on projection failure */
+#define ETAPR RCONST(0.25)
+
+/*
+ * =================================================================
+ * Private Function Prototypes
+ * =================================================================
+ */
+
+static int cpProjLinear(CPodeMem);
+static int cpProjNonlinear(CPodeMem);
+static int cpProjNonlinearIteration(CPodeMem cp_mem);
+
+/*
+ * =================================================================
+ * Readibility Constants
+ * =================================================================
+ */
+
+#define tn (cp_mem->cp_tn)
+#define zn (cp_mem->cp_zn)
+#define y (cp_mem->cp_y)
+#define nst (cp_mem->cp_nst)
+#define ewt (cp_mem->cp_ewt)
+#define acor (cp_mem->cp_acor)
+#define acnrm (cp_mem->cp_acnrm)
+#define ftemp (cp_mem->cp_ftemp)
+#define tempv (cp_mem->cp_tempv)
+#define q (cp_mem->cp_q)
+#define h (cp_mem->cp_h)
+#define next_h (cp_mem->cp_next_h)
+#define hmin (cp_mem->cp_hmin)
+#define hscale (cp_mem->cp_hscale)
+#define eta (cp_mem->cp_eta)
+#define etamax (cp_mem->cp_etamax)
+#define nscon (cp_mem->cp_nscon)
+
+#define proj_type (cp_mem->cp_proj_type)
+#define proj_norm (cp_mem->cp_proj_norm)
+#define cnstr_type (cp_mem->cp_cnstr_type)
+#define pfun (cp_mem->cp_pfun)
+#define p_data (cp_mem->cp_p_data)
+#define cfun (cp_mem->cp_cfun)
+#define c_data (cp_mem->cp_c_data)
+
+#define yC (cp_mem->cp_yC)
+#define acorP (cp_mem->cp_acorP)
+#define errP (cp_mem->cp_errP)
+#define ctol (cp_mem->cp_ctol)
+#define ctemp (cp_mem->cp_ctemp)
+#define tempvP1 (cp_mem->cp_tempvP1)
+#define tempvP2 (cp_mem->cp_tempvP2)
+
+#define lsetupP (cp_mem->cp_lsetupP)
+#define lsolveP (cp_mem->cp_lsolveP)
+#define lmultP (cp_mem->cp_lmultP)
+#define lsetupP_exists (cp_mem->cp_lsetupP_exists)
+#define lsetupP_freq (cp_mem->cp_lsetupP_freq)
+
+#define prjcoef (cp_mem->cp_prjcoef)
+#define crateP (cp_mem->cp_crateP)
+#define nstlsetP (cp_mem->cp_nstlsetP)
+#define maxcorP (cp_mem->cp_maxcorP)
+
+#define project_err (cp_mem->cp_project_err)
+#define test_cnstr (cp_mem->cp_test_cnstr)
+#define first_proj (cp_mem->cp_first_proj)
+#define applyProj (cp_mem->cp_applyProj)
+
+#define nproj (cp_mem->cp_nproj)
+#define nprf (cp_mem->cp_nprf)
+#define nce (cp_mem->cp_nce)
+#define nsetupsP (cp_mem->cp_nsetupsP)
+#define maxnpf (cp_mem->cp_maxnpf)
+
+/*
+ * =================================================================
+ * Main interface function
+ * =================================================================
+ */
+
+/*
+ * cpDoProjection
+ *
+ * For user supplied projection function, use ftemp as temporary storage
+ * for the current error estimate (acor) and use tempv to store the
+ * accumulated corection due to projection, acorP (tempv is not touched
+ * until it is potentially used in cpCompleteStep).
+ *
+ * For the internal projection function, both ftemp and tempv are needed
+ * for temporary storage, so space for acorP was allocated.
+ */
+
+int cpDoProjection(CPodeMem cp_mem, realtype saved_t, int *npfPtr)
+{
+ int flag, retval;
+ realtype cnorm;
+
+ switch (proj_type) {
+
+ case CP_PROJ_INTERNAL:
+
+ /* Evaluate constraints at current time and with the corrected y */
+ retval = cfun(tn, y, ctemp, c_data);
+ nce++;
+ if (retval < 0) {flag = CP_CNSTRFUNC_FAIL; break;}
+ if (retval > 0) {flag = CNSTRFUNC_RECVR; break;}
+
+ /*
+ * If activated, evaluate WL2 norm of constraint violation.
+ * If the constraint violation is small enough, return.
+ */
+ if (test_cnstr) {
+ cnorm = N_VWL2Norm(ctemp, ctol);
+ cnorm /= prjcoef;
+
+#ifdef CPODES_DEBUG
+ printf(" Constraint violation norm = %lg\n",cnorm);
+#endif
+
+ if (cnorm <= ONE) {
+ applyProj = FALSE;
+ return(CP_SUCCESS);
+ }
+ }
+
+#ifdef CPODES_DEBUG
+ else {
+ printf(" No constraint testing\n");
+ }
+#endif
+
+ /* Perform projection step
+ * On a successful return, the projection correction is available in acorP.
+ * Also, if projection of the error estimate was enabled, the new error
+ * estimate is available in errP and acnrm contains ||errP||_WRMS.
+ */
+ nproj++;
+ if (cnstr_type == CP_CNSTR_NONLIN) flag = cpProjNonlinear(cp_mem);
+ else flag = cpProjLinear(cp_mem);
+
+ break;
+
+ case CP_PROJ_USER:
+
+#ifdef CPODES_DEBUG
+ printf(" User-defined projection\n");
+#endif
+
+ /* Use ftemp to store errP and tempv to store acorP
+ * (recall that in this case we did not allocate memory
+ * errP and acorP).
+ */
+ errP = ftemp;
+ acorP = tempv;
+
+ /* Copy acor into errP */
+ N_VScale(ONE, acor, errP);
+
+ /* Call the user projection function */
+ retval = pfun(tn, y, acorP, prjcoef, errP, p_data);
+ nproj++;
+ if (retval < 0) {flag = CP_PROJFUNC_FAIL; break;}
+ if (retval > 0) {flag = PROJFUNC_RECVR; break;}
+
+ /* Recompute acnrm to be used in error test */
+ acnrm = N_VWrmsNorm(errP, ewt);
+
+ flag = CP_SUCCESS;
+
+ break;
+
+ }
+
+#ifdef CPODES_DEBUG
+ printf(" acnrm = %lg\n",acnrm);
+#endif
+
+ /* This is not the first projection anymore */
+ first_proj = FALSE;
+
+ /* If the projection was successful, return now. */
+ if (flag == CP_SUCCESS) {
+ applyProj = TRUE;
+ return(CP_SUCCESS);
+ }
+
+ /* The projection failed. Increment nprf and restore zn */
+ nprf++;
+ cpRestore(cp_mem, saved_t);
+
+ /* Return if lsetupP, lsolveP, cfun, or pfun failed unrecoverably */
+ if (flag == CP_PLSETUP_FAIL) return(CP_PLSETUP_FAIL);
+ if (flag == CP_PLSOLVE_FAIL) return(CP_PLSOLVE_FAIL);
+ if (flag == CP_CNSTRFUNC_FAIL) return(CP_CNSTRFUNC_FAIL);
+ if (flag == CP_PROJFUNC_FAIL) return(CP_PROJFUNC_FAIL);
+
+ /* At this point, flag = CONV_FAIL or CNSTRFUNC_RECVR or PRJFUNC_RECVR; increment npf */
+ (*npfPtr)++;
+ etamax = ONE;
+
+ /* If we had maxnpf failures or |h| = hmin,
+ return CP_PROJ_FAILURE or CP_REPTD_CNSTRFUNC_ERR or CP_REPTD_PROJFUNC_ERR. */
+ if ((ABS(h) <= hmin*ONEPSM) || (*npfPtr == maxnpf)) {
+ if (flag == CONV_FAIL) return(CP_PROJ_FAILURE);
+ if (flag == CNSTRFUNC_RECVR) return(CP_REPTD_CNSTRFUNC_ERR);
+ if (flag == PROJFUNC_RECVR) return(CP_REPTD_PROJFUNC_ERR);
+ }
+
+ /* Reduce step size; return to reattempt the step */
+ eta = MAX(ETAPR, hmin / ABS(h));
+ cpRescale(cp_mem);
+
+ return(PREDICT_AGAIN);
+
+}
+
+/*
+ * cpProjLinear
+ *
+ * This is the internal CPODES implementation of the projection function
+ * for linear constraints. On entry, ctemp contains c(t,y).
+ *
+ * Possible return values:
+ *
+ * CP_SUCCESS ---> continue with error test
+ *
+ * CP_CNSTRFUNC_FAIL -+
+ * CP_PLSETUP_FAIL |-> halt the integration
+ * CP_PLSOLVE_FAIL -+
+ *
+ * CONV_FAIL -+
+ * CNSTRFUNC_RECVR -+-> predict again or stop if too many
+ */
+
+static int cpProjLinear(CPodeMem cp_mem)
+{
+ int retval;
+
+#ifdef CPODES_DEBUG
+ printf(" Internal linear projection\n");
+#endif
+
+ /* Call the lsetupP function to evaluate and factorize the
+ * Jacobian of constraints ONLY the first time we get here */
+ if (first_proj) {
+ retval = lsetupP(cp_mem, y, ctemp, tempvP1, tempvP2, tempv);
+ nsetupsP++;
+ if (retval < 0) return(CP_PLSETUP_FAIL);
+ if (retval > 0) return(CONV_FAIL);
+ }
+
+ /* Call lsolveP (rhs is ctemp; solution in acorP) */
+ retval = lsolveP(cp_mem, ctemp, acorP, y, ctemp, tempvP1, tempv);
+ if (retval < 0) return(CP_PLSOLVE_FAIL);
+ if (retval > 0) return(CONV_FAIL);
+
+ /* Fix the sign of acorP */
+ N_VScale(-ONE, acorP, acorP);
+
+ /* If activated, compute error estimate of the projected method. */
+ if (project_err) {
+
+ /* Find G*acor and load it in tempvP2 */
+ lmultP(cp_mem, acor, tempvP2);
+
+ /* Call lsolveP (rhs is tempvP2; solution in errP) */
+ retval = lsolveP(cp_mem, tempvP2, errP, y, ctemp, tempvP1, tempv);
+ if (retval < 0) return(CP_PLSOLVE_FAIL);
+ if (retval > 0) return(CONV_FAIL);
+
+ /* Update error estimate: errP now contains the new estimated errors. */
+ N_VLinearSum(ONE, acor, -ONE, errP, errP);
+
+ /* Recompute acnrm to be used in error test */
+ acnrm = N_VWrmsNorm(errP, ewt);
+
+ }
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * cpProjNonlinear
+ *
+ * This is the internal CPODES implementation of the projection function
+ * for nonlinear constraints. On entry, ctemp contains c(t,y).
+ *
+ * Possible return values:
+ *
+ * CP_SUCCESS ---> continue with error test
+ *
+ * CP_CNSTRFUNC_FAIL -+
+ * CP_PLSETUP_FAIL |-> halt the integration
+ * CP_PLSOLVE_FAIL -+
+ *
+ * CONV_FAIL -+
+ * CNSTRFUNC_RECVR -+-> predict again or stop if too many
+ */
+
+static int cpProjNonlinear(CPodeMem cp_mem)
+{
+ booleantype callSetup;
+ int retval;
+
+#ifdef CPODES_DEBUG
+ printf(" Internal nonlinear projection\n");
+#endif
+
+ /* If the linear solver provides a setup function, call it if:
+ * - it was never called before (first_proj = TRUE), or
+ * - enough steps passed from last evaluation */
+ if (lsetupP_exists) {
+ callSetup = (first_proj) || (nst >= nstlsetP + lsetupP_freq);
+ } else {
+ callSetup = FALSE;
+ crateP = ONE;
+ }
+
+ /* Save the corrected y, in case we need a second pass through the loop */
+ N_VScale(ONE, y, yC);
+
+ /* Begin the main loop. This loop is traversed at most twice.
+ * The second pass only occurs when the first pass had a recoverable
+ * failure with old Jacobian data. */
+ loop {
+
+ /* If needed, call setup function */
+ if (callSetup) {
+ retval = lsetupP(cp_mem, y, ctemp, tempvP1, tempvP2, tempv);
+
+#ifdef CPODES_DEBUG
+ printf(" Linear solver setup return value = %d\n",retval);
+#endif
+
+ nsetupsP++;
+ nstlsetP = nst;
+ crateP = ONE;
+ if (retval < 0) return(CP_PLSETUP_FAIL);
+ if (retval > 0) return(CONV_FAIL);
+ }
+
+ /* Do the iteration */
+
+#ifdef CPODES_DEBUG
+ printf(" NonlinearIteration\n");
+#endif
+
+ retval = cpProjNonlinearIteration(cp_mem);
+
+#ifdef CPODES_DEBUG
+ printf(" NonlinearIteration return value = %d\n",retval);
+#endif
+
+ /* On a recoverable failure, if setup was not called,
+ * reload the corrected y, recompute constraints, and reattempt loop */
+ if ( (retval > 0) && lsetupP_exists && !callSetup ) {
+ N_VScale(ONE, yC, y);
+ retval = cfun(tn, y, ctemp, c_data);
+ nce++;
+ if (retval < 0) return(CP_CNSTRFUNC_FAIL);
+ if (retval > 0) return(CNSTRFUNC_RECVR);
+ callSetup = TRUE;
+ continue;
+ }
+
+ /* Break from loop */
+ break;
+
+ }
+
+ return(retval);
+}
+
+/*
+ * cpProjNonlinearIteration
+ *
+ * This routine performs the actual nonlinear iteration for the
+ * projection step.
+ *
+ * Possible return values:
+ *
+ * CP_SUCCESS - the iteration was successful.
+ *
+ * CP_PLSOLVE_FAIL - the linear solver solution failed unrecoverably.
+ * CP_CNSTRFUNC_FAIL - the constraint function failed unrecoverably.
+ *
+ * CNSTRFUNC_RECVR - the constraint function failed recoverably.
+ * CONV_FAIL - a recoverable error occured.
+ */
+
+static int cpProjNonlinearIteration(CPodeMem cp_mem)
+{
+ int m, retval;
+ realtype del, delp, dcon;
+ N_Vector corP;
+
+ /* Initialize counter */
+ m = 0;
+
+ /* Initialize delp to avoid compiler warning message */
+ del = delp = ZERO;
+
+ /* Rename ftemp as corP */
+ corP = ftemp;
+
+ /* Set acorP to zero */
+ N_VConst(ZERO, acorP);
+
+ /* Set errP to acor */
+ if (project_err) N_VScale(ONE, acor, errP);
+
+ /* Looping point for iterations */
+ loop {
+
+#ifdef CPODES_DEBUG
+ printf(" Iteration # %d\n",m);
+#endif
+
+ /* Call lsolveP (rhs is ctemp; solution in corP) */
+ retval = lsolveP(cp_mem, ctemp, corP, y, ctemp, tempvP1, tempv);
+
+#ifdef CPODES_DEBUG
+ printf(" Linear solver solve return value = %d\n",retval);
+#endif
+
+ if (retval < 0) return(CP_PLSOLVE_FAIL);
+ if (retval > 0) return(CONV_FAIL);
+
+ /* Add correction to acorP and y and get its WRMS norm */
+ N_VLinearSum(ONE, acorP, -ONE, corP, acorP);
+ N_VLinearSum(ONE, y, -ONE, corP, y);
+ del = N_VWrmsNorm(corP, ewt);
+
+#ifdef CPODES_DEBUG
+ printf(" Norm of correction: del = %lg\n", del);
+#endif
+
+ /* If activated, find correction to the error estimate */
+ if (project_err) {
+ /* Compute rhs as G*errP and load it in tempvP2 */
+ lmultP(cp_mem, errP, tempvP2);
+
+ /* Call lsolveP (rhs is tempvP2; solution in corP) */
+ retval = lsolveP(cp_mem, tempvP2, corP, y, ctemp, tempvP1, tempv);
+ if (retval < 0) return(CP_PLSOLVE_FAIL);
+ if (retval > 0) return(CONV_FAIL);
+
+ /* Add correction to errP */
+ N_VLinearSum(ONE, errP, -ONE, corP, errP);
+ }
+
+ /* Test for convergence. If m > 0, an estimate of the convergence
+ rate constant is stored in crateP, and used in the test. */
+ if (m > 0) crateP = MAX(PRJ_CRDOWN * crateP, del/delp);
+ dcon = del * MIN(ONE, crateP) / prjcoef;
+
+#ifdef CPODES_DEBUG
+ printf(" Convergence test dcon = %lg\n", dcon);
+#endif
+
+ if (dcon <= ONE) {
+
+ if (project_err) acnrm = N_VWrmsNorm(errP, ewt);
+
+ return(CP_SUCCESS);
+ }
+ m++;
+
+ /* Stop at maxcorP iterations or if iter. seems to be diverging. */
+ if ((m == maxcorP) || ((m >= 2) && (del > PRJ_RDIV*delp))) return(CONV_FAIL);
+
+ /* Save norm of correction, evaluate cfun, and loop again */
+ delp = del;
+ retval = cfun(tn, y, ctemp, c_data);
+ nce++;
+ if (retval < 0) return(CP_CNSTRFUNC_FAIL);
+ if (retval > 0) return(CNSTRFUNC_RECVR);
+
+ }
+
+}
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_root.c b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_root.c
new file mode 100644
index 0000000..be9f417
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_root.c
@@ -0,0 +1,734 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision:$
+ * $Date:$
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation for the event detection for CPODES.
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * =================================================================
+ * Import Header Files
+ * =================================================================
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
+
+#include "cpodes_private.h"
+#include <sundials/sundials_math.h>
+
+/*
+ * =================================================================
+ * Private Function Prototypes
+ * =================================================================
+ */
+
+static booleantype cpRootAlloc(CPodeMem cp_mem, int nrt);
+static int cpRootfind(CPodeMem cp_mem, realtype ttol);
+
+/*
+ * =================================================================
+ * EXPORTED FUNCTIONS
+ * =================================================================
+ */
+
+/*
+ * CPodeRootInit
+ *
+ * CPodeRootInit initializes a rootfinding problem to be solved
+ * during the integration of the ODE system. It loads the root
+ * function pointer and the number of root functions, and allocates
+ * workspace memory. The return value is CP_SUCCESS = 0 if no errors
+ * occurred, or a negative value otherwise.
+ */
+
+int CPodeRootInit(void *cpode_mem, int nrtfn, CPRootFn gfun, void *g_data)
+{
+ CPodeMem cp_mem;
+ booleantype allocOK;
+ int i;
+
+ /* Check cpode_mem pointer */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CP_MEM_NULL, "CPODES", "CPodeRootInit", MSGCP_NO_MEM);
+ return(CP_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* If called with nrtfn <= 0, then disable rootfinding and return */
+ if (nrtfn <= 0) {
+ cp_mem->cp_doRootfinding = FALSE;
+ return(CP_SUCCESS);
+ }
+
+ /* Check for legal input parameters */
+ if (gfun == NULL) {
+ cpProcessError(cp_mem, CP_ILL_INPUT, "CPODES", "CPodeRootInit", MSGCP_NULL_G);
+ return(CP_ILL_INPUT);
+ }
+
+ /* If rerunning CPodeRootInit() with a different number of root
+ * functions (changing number of gfun components), then free
+ * currently held memory resources */
+ if ( (cp_mem->cp_rootMallocDone) && (nrtfn != cp_mem->cp_nrtfn) ) {
+ cpRootFree(cp_mem);
+ cp_mem->cp_rootMallocDone = FALSE;
+ }
+
+ /* Allocate necessary memory and return */
+ if (!cp_mem->cp_rootMallocDone) {
+ allocOK = cpRootAlloc(cp_mem, nrtfn);
+ if (!allocOK) {
+ cpProcessError(cp_mem, CP_MEM_FAIL, "CPODES", "CPodeRootInit", MSGCP_MEM_FAIL);
+ return(CP_MEM_FAIL);
+ }
+ cp_mem->cp_rootMallocDone = TRUE;
+ }
+
+ /* Set variable values in CPODES memory block */
+ cp_mem->cp_nrtfn = nrtfn;
+ cp_mem->cp_gfun = gfun;
+ cp_mem->cp_g_data = g_data;
+
+ /* Set default values for rootdir (both directions)
+ * and for gactive (all active) */
+ for(i=0; i<nrtfn; i++) {
+ cp_mem->cp_rootdir[i] = 0;
+ cp_mem->cp_gactive[i] = TRUE;
+ }
+
+ /* Rootfinding is now enabled */
+ cp_mem->cp_doRootfinding = TRUE;
+
+ return(CP_SUCCESS);
+}
+
+
+/*
+ * =================================================================
+ * Readibility Constants
+ * =================================================================
+ */
+
+#define uround (cp_mem->cp_uround)
+#define tn (cp_mem->cp_tn)
+#define h (cp_mem->cp_h)
+#define zn (cp_mem->cp_zn)
+#define y (cp_mem->cp_y)
+#define yp (cp_mem->cp_yp)
+
+#define lrw (cp_mem->cp_lrw)
+#define liw (cp_mem->cp_liw)
+
+#define taskc (cp_mem->cp_taskc)
+#define toutc (cp_mem->cp_toutc)
+
+#define nrtfn (cp_mem->cp_nrtfn)
+#define gfun (cp_mem->cp_gfun)
+#define g_data (cp_mem->cp_g_data)
+#define nge (cp_mem->cp_nge)
+
+#define gactive (cp_mem->cp_gactive)
+#define iroots (cp_mem->cp_iroots)
+#define rootdir (cp_mem->cp_rootdir)
+#define irfnd (cp_mem->cp_irfnd)
+#define tlo (cp_mem->cp_tlo)
+#define thi (cp_mem->cp_thi)
+#define glo (cp_mem->cp_glo)
+#define ghi (cp_mem->cp_ghi)
+#define trout (cp_mem->cp_trout)
+#define grout (cp_mem->cp_grout)
+
+/*
+ * =================================================================
+ * INTERNAL FUNCTIONS
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Root finding functions
+ * cpRcheck1 |
+ * cpRcheck2 |-> interface functions to CPode
+ * cpRcheck3 |
+ * cpRootfind
+ * -----------------------------------------------------------------
+ * Memory allocation/deallocation for rootfinding
+ * cpRootAlloc
+ * cpRootFree
+ * -----------------------------------------------------------------
+ */
+
+/*
+ * cpRcheck1
+ *
+ * This routine completes the initialization of rootfinding memory
+ * information (it is called only once, at the very first step),
+ * and checks whether g has any components that are zero BOTH at AND
+ * very near the initial time of the IVP. Those components of g are
+ * made inactive and will be later reactivated only when they move
+ * away from zero.
+ *
+ * sherm 111125:
+ * thi (output only) set to tn
+ * ghi (output only) set to g(thi)
+ * where we may have advanced time by smallh to see whether g's that were zero
+ * at tn and deactivated can be reactivated at tn+smallh.
+ *
+ * The return value will be
+ * CV_RTFUNC_FAIL < 0 if the g function failed
+ * CP_SUCCESS = 0 otherwise.
+ */
+
+int cpRcheck1(CPodeMem cp_mem)
+{
+ int i, retval;
+ realtype ttol, smallh, hratio;
+ booleantype zroot;
+
+ for (i = 0; i < nrtfn; i++) iroots[i] = 0;
+
+ thi = tlo = tn;
+ ttol = (ABS(tn) + ABS(h))*uround*FUZZ_FACTOR;
+
+ /*
+ * Evaluate g at initial t and check for zero values.
+ * Note that cpRcheck1 is called at the first step
+ * before scaling zn[1] and therefore, y'(t0)=zn[1].
+ */
+
+ retval = gfun(tlo, zn[0], zn[1], glo, g_data);
+ nge = 1;
+ if (retval != 0) return(CP_RTFUNC_FAIL);
+
+ /* Assume we won't find a root at the start. */
+ for (i = 0; i < nrtfn; i++) ghi[i] = glo[i];
+
+ zroot = FALSE;
+ for (i = 0; i < nrtfn; i++) {
+ if (ABS(glo[i]) == ZERO) {
+ zroot = TRUE;
+ gactive[i] = FALSE;
+ }
+ }
+ if (!zroot) return(CP_SUCCESS);
+
+ /*
+ * Some g_i is zero at t0; look at g at t0+(small increment).
+ * At the initial time and at order 1, we have:
+ * y(t0+smallh) = zn[0] + (smallh/h) * zn[1]
+ * y'(t0+smallh) = zn[1]
+ */
+
+ hratio = MAX(ttol/ABS(h), PT1);
+ smallh = hratio*h;
+ thi += smallh;
+ N_VLinearSum(ONE, zn[0], hratio, zn[1], y);
+ retval = gfun(thi, y, zn[1], ghi, g_data);
+ nge++;
+ if (retval != 0) return(CP_RTFUNC_FAIL);
+
+ /*
+ * We check now only the components of g which were exactly
+ * zero at t0 to see if we can 'activate' them.
+ */
+
+ for (i = 0; i < nrtfn; i++) {
+ if (!gactive[i] && ABS(ghi[i]) != ZERO) {
+ gactive[i] = TRUE;
+ }
+ }
+
+ return(CP_SUCCESS);
+}
+
+
+/*
+ * cpRcheck2
+ *
+ * This routine is called at the beginning of a step to find the beginning tlo
+ * of the next root search interval, which is usually the end (thi) of the
+ * previous search interval. But it first checks for exact zeros of any active
+ * g at thi. It then checks for a close pair of zeros (a condition that
+ * would trigger making inactive the corresponding components
+ * of g), and for a new root at a nearby point.
+ * The endpoint thi of the previous search interval is thus adjusted
+ * if necessary to assure that all active g_i are nonzero there,
+ * before returning to do a root search in the interval.
+ *
+ * On entry, thi = tretlast is the last value of tret returned by
+ * CPode. This may be the previous tn, the previous tout value, or
+ * the last root location.
+ *
+ * This routine returns an int equal to:
+ * CP_RTFUNC_FAIL < 0 if gfun failed
+ * RTFOUND > 0 if a new zero of g was found near tlo, or
+ * CP_SUCCESS = 0 otherwise.
+ */
+
+int cpRcheck2(CPodeMem cp_mem)
+{
+ int i, retval;
+ realtype ttol, smallh, hratio;
+ booleantype zroot;
+
+ /* Move tlo up to end of previous search interval in case we find a root
+ here. */
+ tlo = thi;
+ /* Evaluate g(tlo) */
+ (void) cpGetSolution(cp_mem, tlo, y, yp);
+ retval = gfun(tlo, y, yp, glo, g_data);
+ nge++;
+ if (retval != 0) return(CP_RTFUNC_FAIL);
+
+ /* Assume we won't find a root at the start. */
+ for (i = 0; i < nrtfn; i++) ghi[i] = glo[i];
+
+ /* Check if any active g function is exactly ZERO at tlo.
+ * If not, simply return CP_SUCCESS. */
+
+ zroot = FALSE;
+ for (i = 0; i < nrtfn; i++) iroots[i] = 0;
+ for (i = 0; i < nrtfn; i++) {
+ if (!gactive[i]) continue;
+ if (ABS(glo[i]) == ZERO) {
+ zroot = TRUE;
+ iroots[i] = 1;
+ }
+ }
+
+ /* If no root then tlo==thi, glo==ghi. */
+ if (!zroot) return(CP_SUCCESS);
+
+ /* One or more g_i has a zero at tlo.
+ * Evaluate g(thi=tlo+smallh). */
+
+ ttol = (ABS(tn) + ABS(h))*uround*FUZZ_FACTOR;
+ smallh = (h > ZERO) ? ttol : -ttol;
+ thi = tlo+smallh;
+ if ( (thi - tn)*h >= ZERO) {
+ hratio = smallh/h;
+ N_VLinearSum(ONE, y, hratio, zn[1], y);
+ } else {
+ (void) cpGetSolution(cp_mem, thi, y, yp);
+ }
+ retval = gfun(thi, y, yp, ghi, g_data);
+ nge++;
+ if (retval != 0) return(CP_RTFUNC_FAIL);
+
+ /* Check if any active function is ZERO at thi+smallh.
+ * Make inactive those that were also ZERO at thi.
+ * Report a root for those that only became ZERO at tlo+smallh. */
+
+ zroot = FALSE;
+ for (i = 0; i < nrtfn; i++) {
+ if (ABS(ghi[i]) == ZERO) {
+ if (!gactive[i]) continue;
+ if (iroots[i] == 1) { iroots[i] = 0; gactive[i] = FALSE; }
+ else { iroots[i] = 1; zroot = TRUE; }
+ }
+ }
+
+ if (zroot) return(RTFOUND);
+
+ return(CP_SUCCESS);
+}
+
+/*
+ * cpRcheck3
+ *
+ * This routine interfaces to cpRootfind to look for a root of g
+ * between tlo and either tn or tout, whichever comes first.
+ * Only roots beyond tlo in the direction of integration are sought.
+ *
+ * On entry, both thi and ghi=g(thi) should have been evaluated. We start by
+ * setting tlo=thi and glo=ghi, shiting the search interval to start at the
+ * end of the previous one.
+ * On return, if there is a root it is in (tlo,thi] which will have been
+ * adjusted to a very narrow bracket around the zero crossing. If there is no
+ * root then thi and ghi are at the end of the search interval, where they can
+ * serve as the start for the next one.
+ *
+ * This routine returns an int equal to:
+ * CP_RTFUNC_FAIL < 0 if the g function failed,
+ * RTFOUND > 0 if a root of g was found, or
+ * CP_SUCCESS = 0 otherwise.
+ */
+
+int cpRcheck3(CPodeMem cp_mem)
+{
+ int i, retval, ier;
+ realtype ttol;
+
+ /* Move start of search interval to end of previous one. */
+ tlo = thi;
+ for (i = 0; i < nrtfn; ++i) glo[i] = ghi[i];
+
+ /* Set thi = tn or tout, whichever comes first. */
+ switch (taskc) {
+ case CP_ONE_STEP:
+ thi = tn;
+ break;
+ case CP_NORMAL:
+ thi = ( (toutc - tn)*h >= ZERO) ? tn : toutc;
+ break;
+ }
+
+ /* Get y and y' at thi. */
+ (void) cpGetSolution(cp_mem, thi, y, yp);
+
+ /* Set ghi = g(thi) */
+ retval = gfun(thi, y, yp, ghi, g_data);
+ nge++;
+ if (retval != 0) return(CP_RTFUNC_FAIL);
+
+ /* Call cpRootfind to search (tlo,thi) for roots, and to modify tlo,thi
+ to create a very narrow bracket around the first root. */
+ ttol = (ABS(tn) + ABS(h))*uround*FUZZ_FACTOR;
+ ier = cpRootfind(cp_mem, ttol);
+
+ /* If the root function g failed, return now */
+ if (ier == CP_RTFUNC_FAIL) return(CP_RTFUNC_FAIL);
+
+ /* If any of the inactive components moved away from zero,
+ * activate them now. */
+ for(i=0; i<nrtfn; i++) {
+ if(!gactive[i] && grout[i] != ZERO) gactive[i] = TRUE;
+ }
+
+ /* If no root found, return CP_SUCCESS. */
+ if (ier == CP_SUCCESS) return(CP_SUCCESS);
+
+ /* If a root was found, interpolate to get y(trout) and return. */
+ (void) cpGetSolution(cp_mem, trout, y, yp);
+
+ return(RTFOUND);
+}
+
+/*
+ * cpRootFind
+ *
+ * This routine solves for a root of g(t) between tlo and thi, if
+ * one exists. Only roots of odd multiplicity (i.e. with a change
+ * of sign in one of the g_i), or exact zeros, are found.
+ * Here the sign of tlo - thi is arbitrary, but if multiple roots
+ * are found, the one closest to tlo is returned.
+ *
+ * The method used is the Illinois algorithm, a modified secant method.
+ * Reference: Kathie L. Hiebert and Lawrence F. Shampine, Implicitly
+ * Defined Output Points for Solutions of ODEs, Sandia National
+ * Laboratory Report SAND80-0180, February 1980.
+ *
+ * This routine uses the following parameters for communication:
+ *
+ * nrtfn = number of functions g_i, or number of components of
+ * the vector-valued function g(t). Input only.
+ *
+ * gfun = user-defined function for g(t). Its form is
+ * (void) gfun(t, y, gt, g_data)
+ *
+ * gactive = array specifying whether a component of g should
+ * or should not be monitored. gactive[i] is initially
+ * set to TRUE for all i=0,...,nrtfn-1, but it may be
+ * reset to FALSE if at the first step g[i] is 0.0
+ * both at the I.C. and at a small perturbation of them.
+ * gactive[i] is then set back on TRUE only after the
+ * corresponding g function moves away from 0.0.
+ *
+ * rootdir = array specifying the direction of zero-crossings.
+ * If rootdir[i] > 0, search for roots of g_i only if
+ * g_i is increasing; if rootdir[i] < 0, search for
+ * roots of g_i only if g_i is decreasing; otherwise
+ * always search for roots of g_i.
+ *
+ * nge = cumulative counter for gfun calls.
+ *
+ * ttol = a convergence tolerance for trout. Input only.
+ * When a root at trout is found, it is located in time only to
+ * within a tolerance of ttol. Typically, ttol should
+ * be set to a value on the order of
+ * 100 * UROUND * max (ABS(tlo), ABS(thi))
+ * where UROUND is the unit roundoff of the machine.
+ *
+ * tlo, thi = endpoints of the interval in which roots are sought.
+ * On input, they must be distinct, but tlo - thi may
+ * be of either sign. The direction of integration is
+ * assumed to be from tlo to thi. On return, tlo and thi
+ * are the endpoints of the final relevant interval (tlo,thi];
+ * that is, the root has not yet occurred at tlo but has
+ * definitely occurred by thi. The reported root time trout is
+ * always the same as thi.
+ *
+ * glo, ghi = arrays of length nrtfn containing the vectors g(tlo)
+ * and g(thi) respectively. Input and output. On input,
+ * none of the active glo[i] should be zero.
+ *
+ * trout = root location (same as thi), if a root was found, or the original
+ * value of thi if not. Output only. trout is the endpoint thi of
+ * the final interval (tlo,thi] bracketing the root, with |thi-tlo|
+ * at most ttol.
+ *
+ * grout = array of length nrtfn containing g(trout) (==ghi) on return.
+ *
+ * iroots = int array of length nrtfn with root information.
+ * Output only. If a root was found, iroots indicates
+ * which components g_i have a root at trout. For
+ * i = 0, ..., nrtfn-1, iroots[i] = 1 if g_i has a root
+ * and g_i is increasing, iroots[i] = -1 if g_i has a
+ * root and g_i is decreasing, and iroots[i] = 0 if g_i
+ * has no roots or g_i varies in the direction opposite
+ * to that indicated by rootdir[i].
+ *
+ * This routine returns an int equal to:
+ * CP_RTFUNC_FAIL < 0 if gfun faild
+ * RTFOUND > 0 if a root of g was found, or
+ * CP_SUCCESS = 0 otherwise.
+ */
+static int cpRootfind(CPodeMem cp_mem, realtype ttol)
+{
+ realtype alpha, tmid, my_tmid, tmid_saved, thi_saved, fracint, fracsub;
+ int i, retval, side, sideprev;
+ booleantype zroot;
+
+ /* alpha is a bias weight in the secant method.
+ * On the first two passes, set alpha = 1. Thereafter, reset alpha
+ * according to the side (low vs high) of the subinterval in which
+ * the sign change was found in the previous two passes.
+ * If the sides were opposite, set alpha = 1.
+ * If the sides were the same, then double alpha (if high side),
+ * or halve alpha (if low side).
+ * The next guess tmid is the secant method value if alpha = 1, but
+ * is closer to tlo if alpha < 1, and closer to thi if alpha > 1.
+ */
+ alpha = ONE;
+
+ /* First, for each active g function, check whether an event occured in
+ * (tlo,thi). Since glo != 0 for an active component, this means we check for
+ * a sign change or for ghi = 0 (taking into account rootdir). For each
+ * component that triggers an event, we estimate a "proposal" mid point (by
+ * bisection if ghi=0 or with secant method otherwise) and select the one
+ * closest to tlo. */
+ zroot = FALSE;
+ tmid = thi;
+ for (i = 0; i < nrtfn; i++) {
+ if(!gactive[i]) continue;
+
+ if ( (glo[i]*ghi[i] <= ZERO) && (rootdir[i]*glo[i] <= ZERO) ) {
+ zroot = TRUE;
+ if (ghi[i] == ZERO) {
+ my_tmid = thi - HALF * (thi-tlo);
+ } else {
+ my_tmid = thi - (thi - tlo)*ghi[i]/(ghi[i] - alpha*glo[i]);
+ }
+ /* Pick my_tmid if it is closer to tlo than the current tmid. */
+ if ( (my_tmid-tmid)*h < ZERO ) tmid = my_tmid;
+ }
+ }
+
+ /* If no event was detected, set trout to thi and return CP_SUCCESS */
+ if (!zroot) {
+ trout = thi;
+ for (i = 0; i < nrtfn; i++) grout[i] = ghi[i];
+ return(CP_SUCCESS);
+ }
+
+ /* An event was detected. Loop to locate nearest root. */
+ side = 0;
+
+ loop {
+ sideprev = side;
+
+ /* If tmid is too close to tlo or thi, adjust it inward,
+ * by a fractional distance that is between 0.1 and 0.5. */
+ if (ABS(tmid - tlo) < HALF*ttol) {
+ fracint = ABS(thi - tlo)/ttol;
+ fracsub = (fracint > FIVE) ? PT1 : HALF/fracint;
+ tmid = tlo + fracsub*(thi - tlo);
+ }
+ if (ABS(thi - tmid) < HALF*ttol) {
+ fracint = ABS(thi - tlo)/ttol;
+ fracsub = (fracint > FIVE) ? PT1 : HALF/fracint;
+ tmid = thi - fracsub*(thi - tlo);
+ }
+
+ /* Get solution at tmid and evaluate g(tmid). */
+ (void) cpGetSolution(cp_mem, tmid, y, yp);
+ retval = gfun(tmid, y, yp, grout, g_data);
+ nge++;
+ if (retval != 0) return(CP_RTFUNC_FAIL);
+
+ /* Check (tlo, tmid) to see if an event occured in the "low" side
+ * First make temporary copies of thi and tmid in case the event
+ * turns out to be on the "high" side. */
+ tmid_saved = tmid;
+ thi_saved = thi;
+ thi = tmid;
+ zroot = FALSE;
+ for (i = 0; i < nrtfn; i++) {
+ if(!gactive[i]) continue;
+
+ if ( (glo[i]*grout[i] <= ZERO) && (rootdir[i]*glo[i] <= ZERO) ) {
+ zroot = TRUE;
+ if (grout[i] == ZERO) {
+ my_tmid = thi - HALF * (thi-tlo);
+ } else {
+ my_tmid = thi - (thi - tlo)*grout[i]/(grout[i] - alpha*glo[i]);
+ }
+ if ( (my_tmid-tmid)*h < ZERO ) tmid = my_tmid;
+ }
+ }
+
+ /* If we detected an event in the "low" side:
+ - accept current value of thi
+ - set ghi <- grout
+ - test for convergence and break from loop if converged
+ - set side=1 (low); if previous side was also 1, scale alpha by 1/2
+ - continue looping to refine root location */
+ if (zroot) {
+ for (i = 0; i < nrtfn; i++) ghi[i] = grout[i];
+ if (ABS(thi - tlo) <= ttol) break;
+
+ side = 1;
+ if (sideprev == 1) alpha = alpha*HALF;
+ else alpha = ONE;
+
+ continue;
+ }
+
+ /* No event detected in "low" side; event must be in "high" side.
+ - restore previously saved values for thi and set tlo = tmid_saved
+ - set glo <- grout
+ - test for convergence and break from loop if converged
+ - set side=2 (high); if previous side was also 2, scale alpha by 2
+ - continue looping to refine root location */
+ thi = thi_saved;
+ tlo = tmid_saved;
+
+ for (i = 0; i < nrtfn; i++) glo[i] = grout[i];
+ if (ABS(thi - tlo) <= ttol) break;
+
+ tmid = thi;
+ for (i = 0; i < nrtfn; i++) {
+ if(!gactive[i]) continue;
+
+ if ( (glo[i]*ghi[i] <= ZERO) && (rootdir[i]*glo[i] <= ZERO) ) {
+ if (ghi[i] == ZERO) {
+ my_tmid = thi - HALF * (thi-tlo);
+ } else {
+ my_tmid = thi - (thi - tlo)*ghi[i]/(ghi[i] - alpha*glo[i]);
+ }
+ if ( (my_tmid-tmid)*h < ZERO ) tmid = my_tmid;
+ }
+ }
+
+ side = 2;
+ if (sideprev == 2) alpha = alpha*TWO;
+ else alpha = ONE;
+
+ } /* End of root-search loop */
+
+ /* Root has been isolated to (tlo,thi] and |thi-tlo| <= ttol. We'll declare
+ that the root was found at trout=thi.
+ - Reset trout and grout
+ - Set iroots
+ - Return RTFOUND. */
+ trout = thi;
+ for (i = 0; i < nrtfn; i++) {
+ grout[i] = ghi[i];
+ iroots[i] = 0;
+ if(!gactive[i]) continue;
+ if ( (glo[i]*ghi[i] <= ZERO) && (rootdir[i]*glo[i] <= ZERO) )
+ iroots[i] = glo[i] > 0 ? -1:1;
+ }
+
+ return(RTFOUND);
+}
+
+
+
+/*
+ * cpRootAlloc allocates memory for the rootfinding algorithm.
+ */
+
+static booleantype cpRootAlloc(CPodeMem cp_mem, int nrt)
+{
+ glo = NULL;
+ glo = (realtype *) malloc(nrt*sizeof(realtype));
+
+ ghi = NULL;
+ ghi = (realtype *) malloc(nrt*sizeof(realtype));
+ if (ghi == NULL) {
+ free(glo); glo = NULL;
+ return(FALSE);
+ }
+
+ grout = NULL;
+ grout = (realtype *) malloc(nrt*sizeof(realtype));
+ if (grout == NULL) {
+ free(glo); glo = NULL;
+ free(ghi); ghi = NULL;
+ return(FALSE);
+ }
+
+ iroots = NULL;
+ iroots = (int *) malloc(nrt*sizeof(int));
+ if (iroots == NULL) {
+ free(glo); glo = NULL;
+ free(ghi); ghi = NULL;
+ free(grout); grout = NULL;
+ return(FALSE);
+ }
+
+ rootdir = NULL;
+ rootdir = (int *) malloc(nrt*sizeof(int));
+ if (rootdir == NULL) {
+ free(glo); glo = NULL;
+ free(ghi); ghi = NULL;
+ free(grout); grout = NULL;
+ free(iroots); iroots = NULL;
+ }
+
+ gactive = NULL;
+ gactive = (booleantype *) malloc(nrt*sizeof(booleantype));
+ if (gactive == NULL) {
+ free(glo); glo = NULL;
+ free(ghi); ghi = NULL;
+ free(grout); grout = NULL;
+ free(iroots); iroots = NULL;
+ free(rootdir); rootdir = NULL;
+ }
+
+ lrw += 3*nrt;
+ liw += 3*nrt;
+
+ return(TRUE);
+}
+
+
+/*
+ * cpRootFree frees the memory allocated in cpRootAlloc.
+ */
+
+void cpRootFree(CPodeMem cp_mem)
+{
+ free(glo); glo = NULL;
+ free(ghi); ghi = NULL;
+ free(grout); grout = NULL;
+ free(gactive); gactive = NULL;
+ free(iroots); iroots = NULL;
+ free(rootdir); rootdir = NULL;
+
+ lrw -= 3 * nrtfn;
+ liw -= 3 * nrtfn;
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_spbcgs.c b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_spbcgs.c
new file mode 100644
index 0000000..1cf6622
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_spbcgs.c
@@ -0,0 +1,509 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.1 $
+ * $Date: 2006/11/08 01:07:06 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for the CPSPBCG linear solver.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <cpodes/cpodes_spbcgs.h>
+#include "cpodes_spils_impl.h"
+#include "cpodes_private.h"
+
+#include <sundials/sundials_spbcgs.h>
+#include <sundials/sundials_math.h>
+
+/* CPSPBCG linit, lsetup, lsolve, and lfree routines */
+
+static int cpSpbcgInit(CPodeMem cp_mem);
+static int cpSpbcgSetup(CPodeMem cp_mem, int convfail,
+ N_Vector yP, N_Vector ypP, N_Vector fctP,
+ booleantype *jcurPtr,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+static int cpSpbcgSolve(CPodeMem cp_mem, N_Vector b, N_Vector weight,
+ N_Vector yC, N_Vector ypC, N_Vector fctC);
+static void cpSpbcgFree(CPodeMem cp_mem);
+
+
+/* Readability Replacements */
+
+#define ode_type (cp_mem->cp_ode_type)
+#define tq (cp_mem->cp_tq)
+#define nst (cp_mem->cp_nst)
+#define tn (cp_mem->cp_tn)
+#define gamma (cp_mem->cp_gamma)
+#define gammap (cp_mem->cp_gammap)
+#define f (cp_mem->cp_f)
+#define f_data (cp_mem->cp_f_data)
+#define ewt (cp_mem->cp_ewt)
+#define errfp (cp_mem->cp_errfp)
+#define mnewt (cp_mem->cp_mnewt)
+#define linit (cp_mem->cp_linit)
+#define lsetup (cp_mem->cp_lsetup)
+#define lsolve (cp_mem->cp_lsolve)
+#define lfree (cp_mem->cp_lfree)
+#define lmem (cp_mem->cp_lmem)
+#define vec_tmpl (cp_mem->cp_tempv)
+#define lsetup_exists (cp_mem->cp_lsetup_exists)
+
+#define sqrtN (cpspils_mem->s_sqrtN)
+#define ytemp (cpspils_mem->s_ytemp)
+#define yptemp (cpspils_mem->s_yptemp)
+#define x (cpspils_mem->s_x)
+#define ycur (cpspils_mem->s_ycur)
+#define ypcur (cpspils_mem->s_ypcur)
+#define fcur (cpspils_mem->s_fcur)
+#define delta (cpspils_mem->s_delta)
+#define deltar (cpspils_mem->s_deltar)
+#define npe (cpspils_mem->s_npe)
+#define nli (cpspils_mem->s_nli)
+#define nps (cpspils_mem->s_nps)
+#define ncfl (cpspils_mem->s_ncfl)
+#define nstlpre (cpspils_mem->s_nstlpre)
+#define njtimes (cpspils_mem->s_njtimes)
+#define nfes (cpspils_mem->s_nfes)
+#define spils_mem (cpspils_mem->s_spils_mem)
+#define last_flag (cpspils_mem->s_last_flag)
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPSpbcg
+ * -----------------------------------------------------------------
+ * This routine initializes the memory record and sets various function
+ * fields specific to the Spbcg linear solver module. CPSpbcg first
+ * calls the existing lfree routine if this is not NULL. It then sets
+ * the cp_linit, cp_lsetup, cp_lsolve, cp_lfree fields in (*cpode_mem)
+ * to be CPSpbcgInit, CPSpbcgSetup, CPSpbcgSolve, and CPSpbcgFree,
+ * respectively. It allocates memory for a structure of type
+ * CPSpilsMemRec and sets the cp_lmem field in (*cpode_mem) to the
+ * address of this structure. It sets lsetup_exists in (*cpode_mem),
+ * and sets the following fields in the CPSpilsMemRec structure:
+ *
+ * s_pretype = pretype
+ * s_maxl = CPSPILS_MAXL if maxl <= 0
+ * = maxl if maxl > 0
+ * s_delt = CPSPILS_DELT
+ * s_psetE = NULL
+ * s_psetI = NULL
+ * s_pslvE = NULL
+ * s_pslvI = NULL
+ * s_jtvE = NULL
+ * s_jtvI = NULL
+ * s_P_data = NULL
+ * s_j_data = NULL
+ * Finally, CPSpbcg allocates memory for ytemp and x, and calls
+ * SpbcgMalloc to allocate memory for the Spbcg solver.
+ * -----------------------------------------------------------------
+ */
+
+int CPSpbcg(void *cpode_mem, int pretype, int maxl)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+ SpbcgMem spbcg_mem;
+ int mxl;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPSPILS_MEM_NULL, "CPSPBCG", "CPSpbcg", MSGS_CPMEM_NULL);
+ return(CPSPILS_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Check if N_VDotProd is present */
+ if (vec_tmpl->ops->nvdotprod == NULL) {
+ cpProcessError(cp_mem, CPSPILS_ILL_INPUT, "CPSPBCG", "CPSpbcg", MSGS_BAD_NVECTOR);
+ return(CPSPILS_ILL_INPUT);
+ }
+
+ if (lfree != NULL) lfree(cp_mem);
+
+ /* Set four main function fields in cp_mem */
+ linit = cpSpbcgInit;
+ lsetup = cpSpbcgSetup;
+ lsolve = cpSpbcgSolve;
+ lfree = cpSpbcgFree;
+
+ /* Get memory for CPSpilsMemRec */
+ cpspils_mem = NULL;
+ cpspils_mem = (CPSpilsMem) malloc(sizeof(CPSpilsMemRec));
+ if (cpspils_mem == NULL) {
+ cpProcessError(cp_mem, CPSPILS_MEM_FAIL, "CPSPBCG", "CPSpbcg", MSGS_MEM_FAIL);
+ return(CPSPILS_MEM_FAIL);
+ }
+
+ /* Set ILS type */
+ cpspils_mem->s_type = SPILS_SPBCG;
+
+ /* Set Spbcg parameters that have been passed in call sequence */
+ cpspils_mem->s_pretype = pretype;
+ mxl = cpspils_mem->s_maxl = (maxl <= 0) ? CPSPILS_MAXL : maxl;
+
+ /* Set default values for the rest of the Spbcg parameters */
+ cpspils_mem->s_delt = CPSPILS_DELT;
+
+ cpspils_mem->s_psetE = NULL;
+ cpspils_mem->s_psetI = NULL;
+ cpspils_mem->s_pslvE = NULL;
+ cpspils_mem->s_pslvI = NULL;
+ cpspils_mem->s_jtvE = NULL;
+ cpspils_mem->s_jtvI = NULL;
+ cpspils_mem->s_P_data = NULL;
+ cpspils_mem->s_j_data = NULL;
+
+ cpspils_mem->s_last_flag = CPSPILS_SUCCESS;
+
+ lsetup_exists = FALSE;
+
+ /* Check for legal pretype */
+ if ((pretype != PREC_NONE) && (pretype != PREC_LEFT) &&
+ (pretype != PREC_RIGHT) && (pretype != PREC_BOTH)) {
+ cpProcessError(cp_mem, CPSPILS_ILL_INPUT, "CPSPBCG", "CPSpbcg", MSGS_BAD_PRETYPE);
+ return(CPSPILS_ILL_INPUT);
+ }
+
+ /* Alocate memory */
+ spbcg_mem = NULL;
+ ytemp = NULL;
+ yptemp = NULL;
+ x = NULL;
+
+ /* Call SpbcgMalloc to allocate workspace for Spbcg */
+ spbcg_mem = SpbcgMalloc(mxl, vec_tmpl);
+ if (spbcg_mem == NULL) {
+ cpProcessError(cp_mem, CPSPILS_MEM_FAIL, "CPSPBCG", "CPSpbcg", MSGS_MEM_FAIL);
+ free(cpspils_mem);
+ return(CPSPILS_MEM_FAIL);
+ }
+
+ /* Allocate memory for x, ytemp and (if needed) yptemp */
+ x = N_VClone(vec_tmpl);
+ if (x == NULL) {
+ cpProcessError(cp_mem, CPSPILS_MEM_FAIL, "CPSPBCG", "CPSpbcg", MSGS_MEM_FAIL);
+ SpbcgFree(spbcg_mem);
+ free(cpspils_mem);
+ return(CPSPILS_MEM_FAIL);
+ }
+ ytemp = N_VClone(vec_tmpl);
+ if (ytemp == NULL) {
+ cpProcessError(cp_mem, CPSPILS_MEM_FAIL, "CPSPBCG", "CPSpbcg", MSGS_MEM_FAIL);
+ SpbcgFree(spbcg_mem);
+ N_VDestroy(x);
+ free(cpspils_mem);
+ return(CPSPILS_MEM_FAIL);
+ }
+ if (ode_type == CP_IMPL) {
+ yptemp = N_VClone(vec_tmpl);
+ if (yptemp == NULL) {
+ cpProcessError(cp_mem, CPSPILS_MEM_FAIL, "CPSPBCG", "CPSpbcg", MSGS_MEM_FAIL);
+ SpbcgFree(spbcg_mem);
+ N_VDestroy(x);
+ N_VDestroy(ytemp);
+ free(cpspils_mem);
+ return(CPSPILS_MEM_FAIL);
+ }
+ }
+
+ /* Compute sqrtN from a dot product */
+ N_VConst(ONE, ytemp);
+ sqrtN = RSqrt(N_VDotProd(ytemp, ytemp));
+
+ /* Attach SPBCG memory to spils memory structure */
+ spils_mem = (void *) spbcg_mem;
+
+ /* Attach linear solver memory to integrator memory */
+ lmem = cpspils_mem;
+
+ return(CPSPILS_SUCCESS);
+}
+
+/* Additional readability replacements */
+
+#define pretype (cpspils_mem->s_pretype)
+#define delt (cpspils_mem->s_delt)
+#define maxl (cpspils_mem->s_maxl)
+#define psetE (cpspils_mem->s_psetE)
+#define psetI (cpspils_mem->s_psetI)
+#define pslvE (cpspils_mem->s_pslvE)
+#define pslvI (cpspils_mem->s_pslvI)
+#define jtvE (cpspils_mem->s_jtvE)
+#define jtvI (cpspils_mem->s_jtvI)
+#define P_data (cpspils_mem->s_P_data)
+#define j_data (cpspils_mem->s_j_data)
+
+/*
+ * -----------------------------------------------------------------
+ * Function : cpSpbcgInit
+ * -----------------------------------------------------------------
+ * This routine does remaining initializations specific to the Spbcg
+ * linear solver.
+ * -----------------------------------------------------------------
+ */
+
+static int cpSpbcgInit(CPodeMem cp_mem)
+{
+ CPSpilsMem cpspils_mem;
+ SpbcgMem spbcg_mem;
+
+ cpspils_mem = (CPSpilsMem) lmem;
+ spbcg_mem = (SpbcgMem) spils_mem;
+
+ /* Initialize counters */
+ npe = nli = nps = ncfl = nstlpre = 0;
+ njtimes = nfes = 0;
+
+ /*
+ * Check for legal combination pretype - psolve
+ *
+ * Set lsetup_exists = TRUE iff there is preconditioning (pretype != PREC_NONE)
+ * and there is a preconditioning setup phase (pset != NULL)
+ *
+ * If jtimes is NULL at this time, set it to DQ
+ */
+
+ if (ode_type == CP_EXPL) {
+ if ((pretype != PREC_NONE) && (pslvE == NULL)) {
+ cpProcessError(cp_mem, -1, "CPSPBCG", "CPSpbcgInit", MSGS_PSOLVE_REQ);
+ last_flag = CPSPILS_ILL_INPUT;
+ return(-1);
+ }
+ lsetup_exists = (pretype != PREC_NONE) && (psetE != NULL);
+ if (jtvE == NULL) {
+ jtvE = cpSpilsDQjtvExpl;
+ j_data = cp_mem;
+ }
+ } else {
+ if ((pretype != PREC_NONE) && (pslvI == NULL)) {
+ cpProcessError(cp_mem, -1, "CPSPBCG", "CPSpbcgInit", MSGS_PSOLVE_REQ);
+ last_flag = CPSPILS_ILL_INPUT;
+ return(-1);
+ }
+ lsetup_exists = (pretype != PREC_NONE) && (psetI != NULL);
+ if (jtvI == NULL) {
+ jtvI = cpSpilsDQjtvImpl;
+ j_data = cp_mem;
+ }
+ }
+
+ /* Set maxl in the SPBCG memory in case it was changed by the user */
+ spbcg_mem->l_max = maxl;
+
+ last_flag = CPSPILS_SUCCESS;
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Function : cpSpbcgSetup
+ * -----------------------------------------------------------------
+ * This routine does the setup operations for the Spbcg linear solver.
+ * It makes a decision as to whether or not to signal for reevaluation
+ * of Jacobian data in the pset routine, based on various state
+ * variables, then it calls pset. If we signal for reevaluation,
+ * then we reset jcur = *jcurPtr to TRUE, regardless of the pset output.
+ * In any case, if jcur == TRUE, we increment npe and save nst in nstlpre.
+ * -----------------------------------------------------------------
+ */
+static int cpSpbcgSetup(CPodeMem cp_mem, int convfail,
+ N_Vector yP, N_Vector ypP, N_Vector fctP,
+ booleantype *jcurPtr,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ booleantype jbad, jok;
+ realtype dgamma;
+ int retval;
+ CPSpilsMem cpspils_mem;
+
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ switch (ode_type) {
+
+ case CP_EXPL:
+
+ /* Use nst, gamma/gammap, and convfail to set J eval. flag jok */
+ dgamma = ABS((gamma/gammap) - ONE);
+ jbad = (nst == 0) || (nst > nstlpre + CPSPILS_MSBPRE) ||
+ ((convfail == CP_FAIL_BAD_J) && (dgamma < CPSPILS_DGMAX)) ||
+ (convfail == CP_FAIL_OTHER);
+ *jcurPtr = jbad;
+ jok = !jbad;
+
+ /* Call pset routine and possibly reset jcur */
+ retval = psetE(tn, yP, fctP, jok, jcurPtr, gamma, P_data, tmp1, tmp2, tmp3);
+ if (retval == 0) {
+ if (jbad) *jcurPtr = TRUE;
+ /* If jcur = TRUE, increment npe and save nst value */
+ if (*jcurPtr) {
+ npe++;
+ nstlpre = nst;
+ }
+ last_flag = SPBCG_SUCCESS;
+ } else if (retval < 0) {
+ cpProcessError(cp_mem, SPBCG_PSET_FAIL_UNREC, "CPSPBCG", "CPSpbcgSetup", MSGS_PSET_FAILED);
+ last_flag = SPBCG_PSET_FAIL_UNREC;
+ } else if (retval > 0) {
+ last_flag = SPBCG_PSET_FAIL_REC;
+ }
+
+ break;
+
+ case CP_IMPL:
+
+ /* Call psetI routine */
+ retval = psetI(tn, yP, ypP, fctP, gamma, P_data, tmp1, tmp2, tmp3);
+ if (retval == 0) {
+ last_flag = SPBCG_SUCCESS;
+ } else if (retval < 0) {
+ cpProcessError(cp_mem, SPBCG_PSET_FAIL_UNREC, "CPSPBCG", "CPSpbcgSetup", MSGS_PSET_FAILED);
+ last_flag = SPBCG_PSET_FAIL_UNREC;
+ }else if (retval > 0) {
+ last_flag = SPBCG_PSET_FAIL_REC;
+ }
+
+ npe++;
+ nstlpre = nst;
+
+ break;
+
+ }
+
+ /* Return the same value that pset returned */
+ return(retval);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Function : cpSpbcgSolve
+ * -----------------------------------------------------------------
+ * This routine handles the call to the generic solver SpbcgSolve
+ * for the solution of the linear system Ax = b with the SPBCG method.
+ * The solution x is returned in the vector b.
+ *
+ * If the WRMS norm of b is small, we return x = b (if this is the first
+ * Newton iteration) or x = 0 (if a later Newton iteration).
+ *
+ * Otherwise, we set the tolerance parameter and initial guess (x = 0),
+ * call SpbcgSolve, and copy the solution x into b. The x-scaling and
+ * b-scaling arrays are both equal to weight.
+ *
+ * The counters nli, nps, and ncfl are incremented, and the return value
+ * is set according to the success of SpbcgSolve. The success flag is
+ * returned if SpbcgSolve converged, or if this is the first Newton
+ * iteration and the residual norm was reduced below its initial value.
+ * -----------------------------------------------------------------
+ */
+static int cpSpbcgSolve(CPodeMem cp_mem, N_Vector b, N_Vector weight,
+ N_Vector yC, N_Vector ypC, N_Vector fctC)
+{
+ realtype bnorm, res_norm;
+ CPSpilsMem cpspils_mem;
+ SpbcgMem spbcg_mem;
+ int nli_inc, nps_inc, retval;
+
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ spbcg_mem = (SpbcgMem) spils_mem;
+
+ /* Test norm(b); if small, return x = 0 or x = b */
+ deltar = delt * tq[4];
+
+ bnorm = N_VWrmsNorm(b, weight);
+ if (bnorm <= deltar) {
+ if (mnewt > 0) N_VConst(ZERO, b);
+ return(0);
+ }
+
+ /* Set vectors ycur and fcur for use by the Atimes and Psolve routines */
+ ycur = yC;
+ ypcur = ypC;
+ fcur = fctC;
+
+ /* Set inputs delta and initial guess x = 0 to SpbcgSolve */
+ delta = deltar * sqrtN;
+ N_VConst(ZERO, x);
+
+ /* Call SpbcgSolve and copy x to b */
+ retval = SpbcgSolve(spbcg_mem, cp_mem, x, b, pretype, delta,
+ cp_mem, weight, weight, cpSpilsAtimes, cpSpilsPSolve,
+ &res_norm, &nli_inc, &nps_inc);
+
+ N_VScale(ONE, x, b);
+
+ /* Increment counters nli, nps, and ncfl */
+ nli += nli_inc;
+ nps += nps_inc;
+ if (retval != SPBCG_SUCCESS) ncfl++;
+
+ /* Interpret return value from SpbcgSolve */
+
+ last_flag = retval;
+
+ switch(retval) {
+
+ case SPBCG_SUCCESS:
+ return(0);
+ break;
+ case SPBCG_RES_REDUCED:
+ if (mnewt == 0) return(0);
+ else return(1);
+ break;
+ case SPBCG_CONV_FAIL:
+ return(1);
+ break;
+ case SPBCG_PSOLVE_FAIL_REC:
+ return(1);
+ break;
+ case SPBCG_ATIMES_FAIL_REC:
+ return(1);
+ break;
+ case SPBCG_MEM_NULL:
+ return(-1);
+ break;
+ case SPBCG_ATIMES_FAIL_UNREC:
+ cpProcessError(cp_mem, SPBCG_ATIMES_FAIL_UNREC, "CPSPBCG", "CPSpbcgSolve", MSGS_JTIMES_FAILED);
+ return(-1);
+ break;
+ case SPBCG_PSOLVE_FAIL_UNREC:
+ cpProcessError(cp_mem, SPBCG_PSOLVE_FAIL_UNREC, "CPSPBCG", "CPSpbcgSolve", MSGS_PSOLVE_FAILED);
+ return(-1);
+ break;
+ }
+
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Function : cpSpbcgFree
+ * -----------------------------------------------------------------
+ * This routine frees memory specific to the Spbcg linear solver.
+ * -----------------------------------------------------------------
+ */
+
+static void cpSpbcgFree(CPodeMem cp_mem)
+{
+ CPSpilsMem cpspils_mem;
+ SpbcgMem spbcg_mem;
+
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ spbcg_mem = (SpbcgMem) spils_mem;
+
+ SpbcgFree(spbcg_mem);
+ N_VDestroy(x);
+ N_VDestroy(ytemp);
+ if (ode_type == CP_EXPL) N_VDestroy(yptemp);
+ free(cpspils_mem); cpspils_mem = NULL;
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_spgmr.c b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_spgmr.c
new file mode 100644
index 0000000..960b94a
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_spgmr.c
@@ -0,0 +1,516 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.1 $
+ * $Date: 2006/11/08 01:07:06 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for the CPSPGMR linear solver.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <cpodes/cpodes_spgmr.h>
+#include "cpodes_spils_impl.h"
+#include "cpodes_private.h"
+
+#include <sundials/sundials_spgmr.h>
+#include <sundials/sundials_math.h>
+
+/* CPSPGMR linit, lsetup, lsolve, and lfree routines */
+static int cpSpgmrInit(CPodeMem cp_mem);
+static int cpSpgmrSetup(CPodeMem cp_mem, int convfail,
+ N_Vector yP, N_Vector ypP, N_Vector fctP,
+ booleantype *jcurPtr,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+static int cpSpgmrSolve(CPodeMem cp_mem, N_Vector b, N_Vector weight,
+ N_Vector yC, N_Vector ypC, N_Vector fctC);
+static void cpSpgmrFree(CPodeMem cp_mem);
+
+/* Readability Replacements */
+
+#define ode_type (cp_mem->cp_ode_type)
+#define uround (cp_mem->cp_uround)
+#define tq (cp_mem->cp_tq)
+#define nst (cp_mem->cp_nst)
+#define tn (cp_mem->cp_tn)
+#define h (cp_mem->cp_h)
+#define gamma (cp_mem->cp_gamma)
+#define gammap (cp_mem->cp_gammap)
+#define f (cp_mem->cp_f)
+#define f_data (cp_mem->cp_f_data)
+#define ewt (cp_mem->cp_ewt)
+#define mnewt (cp_mem->cp_mnewt)
+#define ropt (cp_mem->cp_ropt)
+#define linit (cp_mem->cp_linit)
+#define lsetup (cp_mem->cp_lsetup)
+#define lsolve (cp_mem->cp_lsolve)
+#define lfree (cp_mem->cp_lfree)
+#define lmem (cp_mem->cp_lmem)
+#define vec_tmpl (cp_mem->cp_tempv)
+#define lsetup_exists (cp_mem->cp_lsetup_exists)
+
+#define sqrtN (cpspils_mem->s_sqrtN)
+#define ytemp (cpspils_mem->s_ytemp)
+#define yptemp (cpspils_mem->s_yptemp)
+#define x (cpspils_mem->s_x)
+#define ycur (cpspils_mem->s_ycur)
+#define ypcur (cpspils_mem->s_ypcur)
+#define fcur (cpspils_mem->s_fcur)
+#define delta (cpspils_mem->s_delta)
+#define deltar (cpspils_mem->s_deltar)
+#define npe (cpspils_mem->s_npe)
+#define nli (cpspils_mem->s_nli)
+#define nps (cpspils_mem->s_nps)
+#define ncfl (cpspils_mem->s_ncfl)
+#define nstlpre (cpspils_mem->s_nstlpre)
+#define njtimes (cpspils_mem->s_njtimes)
+#define nfes (cpspils_mem->s_nfes)
+#define spils_mem (cpspils_mem->s_spils_mem)
+#define last_flag (cpspils_mem->s_last_flag)
+
+/*
+ * -----------------------------------------------------------------
+ * CPSpgmr
+ * -----------------------------------------------------------------
+ * This routine initializes the memory record and sets various function
+ * fields specific to the Spgmr linear solver module. CPSpgmr first
+ * calls the existing lfree routine if this is not NULL. It then sets
+ * the cp_linit, cp_lsetup, cp_lsolve, cp_lfree fields in (*cpode_mem)
+ * to be CPSpgmrInit, CPSpgmrSetup, CPSpgmrSolve, and CPSpgmrFree,
+ * respectively. It allocates memory for a structure of type
+ * CPSpilsMemRec and sets the cp_lmem field in (*cpode_mem) to the
+ * address of this structure. It sets lsetup_exists in (*cpode_mem),
+ * and sets the following fields in the CPSpilsMemRec structure:
+ * s_pretype = pretype
+ * s_gstype = gstype
+ * s_maxl = MIN(N,CPSPILS_MAXL if maxl <= 0
+ * = maxl if maxl > 0
+ * s_delt = CPSPILS_DELT if delt == 0.0
+ * = delt if delt != 0.0
+ * s_psetE = NULL
+ * s_psetI = NULL
+ * s_pslvE = NULL
+ * s_pslvI = NULL
+ * s_jtvE = NULL
+ * s_jtvI = NULL
+ * s_P_data = NULL
+ * s_j_data = NULL
+ * Finally, CPSpgmr allocates memory for ytemp and x, and calls
+ * SpgmrMalloc to allocate memory for the Spgmr solver.
+ * -----------------------------------------------------------------
+ */
+
+int CPSpgmr(void *cpode_mem, int pretype, int maxl)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+ SpgmrMem spgmr_mem;
+ int mxl;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPSPILS_MEM_NULL, "CPSPGMR", "CPSpgmr", MSGS_CPMEM_NULL);
+ return(CPSPILS_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Check if N_VDotProd is present */
+ if(vec_tmpl->ops->nvdotprod == NULL) {
+ cpProcessError(cp_mem, CPSPILS_ILL_INPUT, "CPSPGMR", "CPSpgmr", MSGS_BAD_NVECTOR);
+ return(CPSPILS_ILL_INPUT);
+ }
+
+ if (lfree != NULL) lfree(cp_mem);
+
+ /* Set four main function fields in cp_mem */
+ linit = cpSpgmrInit;
+ lsetup = cpSpgmrSetup;
+ lsolve = cpSpgmrSolve;
+ lfree = cpSpgmrFree;
+
+ /* Get memory for CPSpilsMemRec */
+ cpspils_mem = NULL;
+ cpspils_mem = (CPSpilsMem) malloc(sizeof(CPSpilsMemRec));
+ if (cpspils_mem == NULL) {
+ cpProcessError(cp_mem, CPSPILS_MEM_FAIL, "CPSPGMR", "CPSpgmr", MSGS_MEM_FAIL);
+ return(CPSPILS_MEM_FAIL);
+ }
+
+ /* Set ILS type */
+ cpspils_mem->s_type = SPILS_SPGMR;
+
+ /* Set Spgmr parameters that have been passed in call sequence */
+ cpspils_mem->s_pretype = pretype;
+ mxl = cpspils_mem->s_maxl = (maxl <= 0) ? CPSPILS_MAXL : maxl;
+
+ /* Set default values for the rest of the Spgmr parameters */
+ cpspils_mem->s_gstype = MODIFIED_GS;
+ cpspils_mem->s_delt = CPSPILS_DELT;
+
+ cpspils_mem->s_psetE = NULL;
+ cpspils_mem->s_psetI = NULL;
+ cpspils_mem->s_pslvE = NULL;
+ cpspils_mem->s_pslvI = NULL;
+ cpspils_mem->s_jtvE = NULL;
+ cpspils_mem->s_jtvI = NULL;
+ cpspils_mem->s_P_data = NULL;
+ cpspils_mem->s_j_data = NULL;
+
+ cpspils_mem->s_last_flag = CPSPILS_SUCCESS;
+
+ lsetup_exists = FALSE;
+
+ /* Check for legal pretype */
+ if ((pretype != PREC_NONE) && (pretype != PREC_LEFT) &&
+ (pretype != PREC_RIGHT) && (pretype != PREC_BOTH)) {
+ cpProcessError(cp_mem, CPSPILS_ILL_INPUT, "CPSPGMR", "CPSpgmr", MSGS_BAD_PRETYPE);
+ return(CPSPILS_ILL_INPUT);
+ }
+
+ /* Alocate memory */
+ spgmr_mem = NULL;
+ ytemp = NULL;
+ yptemp = NULL;
+ x = NULL;
+
+ /* Call SpgmrMalloc to allocate workspace for Spgmr */
+ spgmr_mem = SpgmrMalloc(mxl, vec_tmpl);
+ if (spgmr_mem == NULL) {
+ cpProcessError(cp_mem, CPSPILS_MEM_FAIL, "CPSPGMR", "CPSpgmr", MSGS_MEM_FAIL);
+ free(cpspils_mem);
+ return(CPSPILS_MEM_FAIL);
+ }
+
+ /* Allocate memory for x, ytemp and (if needed) yptemp */
+ x = N_VClone(vec_tmpl);
+ if (x == NULL) {
+ cpProcessError(cp_mem, CPSPILS_MEM_FAIL, "CPSPGMR", "CPSpgmr", MSGS_MEM_FAIL);
+ SpgmrFree(spgmr_mem);
+ free(cpspils_mem);
+ return(CPSPILS_MEM_FAIL);
+ }
+ ytemp = N_VClone(vec_tmpl);
+ if (ytemp == NULL) {
+ cpProcessError(cp_mem, CPSPILS_MEM_FAIL, "CPSPGMR", "CPSpgmr", MSGS_MEM_FAIL);
+ SpgmrFree(spgmr_mem);
+ N_VDestroy(x);
+ free(cpspils_mem);
+ return(CPSPILS_MEM_FAIL);
+ }
+ if (ode_type == CP_IMPL) {
+ yptemp = N_VClone(vec_tmpl);
+ if (yptemp == NULL) {
+ cpProcessError(cp_mem, CPSPILS_MEM_FAIL, "CPSPGMR", "CPSpgmr", MSGS_MEM_FAIL);
+ SpgmrFree(spgmr_mem);
+ N_VDestroy(x);
+ N_VDestroy(ytemp);
+ free(cpspils_mem);
+ return(CPSPILS_MEM_FAIL);
+ }
+ }
+
+ /* Compute sqrtN from a dot product */
+ N_VConst(ONE, ytemp);
+ sqrtN = RSqrt( N_VDotProd(ytemp, ytemp) );
+
+ /* Attach SPGMR memory to spils memory structure */
+ spils_mem = (void *) spgmr_mem;
+
+ /* Attach linear solver memory to integrator memory */
+ lmem = cpspils_mem;
+
+ return(CPSPILS_SUCCESS);
+}
+
+/* Additional readability Replacements */
+
+#define pretype (cpspils_mem->s_pretype)
+#define gstype (cpspils_mem->s_gstype)
+#define delt (cpspils_mem->s_delt)
+#define maxl (cpspils_mem->s_maxl)
+#define psetE (cpspils_mem->s_psetE)
+#define psetI (cpspils_mem->s_psetI)
+#define pslvE (cpspils_mem->s_pslvE)
+#define pslvI (cpspils_mem->s_pslvI)
+#define jtvE (cpspils_mem->s_jtvE)
+#define jtvI (cpspils_mem->s_jtvI)
+#define P_data (cpspils_mem->s_P_data)
+#define j_data (cpspils_mem->s_j_data)
+
+/*
+ * -----------------------------------------------------------------
+ * cpSpgmrInit
+ * -----------------------------------------------------------------
+ * This routine does remaining initializations specific to the Spgmr
+ * linear solver.
+ * -----------------------------------------------------------------
+ */
+
+static int cpSpgmrInit(CPodeMem cp_mem)
+{
+ CPSpilsMem cpspils_mem;
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ /* Initialize counters */
+ npe = nli = nps = ncfl = nstlpre = 0;
+ njtimes = nfes = 0;
+
+ /*
+ * Check for legal combination pretype - psolve
+ *
+ * Set lsetup_exists = TRUE iff there is preconditioning (pretype != PREC_NONE)
+ * and there is a preconditioning setup phase (pset != NULL)
+ *
+ * If jtimes is NULL at this time, set it to DQ
+ */
+
+ if (ode_type == CP_EXPL) {
+ if ((pretype != PREC_NONE) && (pslvE == NULL)) {
+ cpProcessError(cp_mem, -1, "CPSPGMR", "CPSpgmrInit", MSGS_PSOLVE_REQ);
+ last_flag = CPSPILS_ILL_INPUT;
+ return(-1);
+ }
+ lsetup_exists = (pretype != PREC_NONE) && (psetE != NULL);
+ if (jtvE == NULL) {
+ jtvE = cpSpilsDQjtvExpl;
+ j_data = cp_mem;
+ }
+ } else {
+ if ((pretype != PREC_NONE) && (pslvI == NULL)) {
+ cpProcessError(cp_mem, -1, "CPSPGMR", "CPSpgmrInit", MSGS_PSOLVE_REQ);
+ last_flag = CPSPILS_ILL_INPUT;
+ return(-1);
+ }
+ lsetup_exists = (pretype != PREC_NONE) && (psetI != NULL);
+ if (jtvI == NULL) {
+ jtvI = cpSpilsDQjtvImpl;
+ j_data = cp_mem;
+ }
+ }
+
+ last_flag = CPSPILS_SUCCESS;
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpSpgmrSetup
+ * -----------------------------------------------------------------
+ * This routine does the setup operations for the Spgmr linear solver.
+ * It makes a decision as to whether or not to signal for re-evaluation
+ * of Jacobian data in the pset routine, based on various state
+ * variables, then it calls pset. If we signal for re-evaluation,
+ * then we reset jcur = *jcurPtr to TRUE, regardless of the pset output.
+ * In any case, if jcur == TRUE, we increment npe and save nst in nstlpre.
+ * -----------------------------------------------------------------
+ */
+static int cpSpgmrSetup(CPodeMem cp_mem, int convfail,
+ N_Vector yP, N_Vector ypP, N_Vector fctP,
+ booleantype *jcurPtr,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ booleantype jbad, jok;
+ realtype dgamma;
+ int retval;
+ CPSpilsMem cpspils_mem;
+
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ switch (ode_type) {
+
+ case CP_EXPL:
+
+ /* Use nst, gamma/gammap, and convfail to set J eval. flag jok */
+ dgamma = ABS((gamma/gammap) - ONE);
+ jbad = (nst == 0) || (nst > nstlpre + CPSPILS_MSBPRE) ||
+ ((convfail == CP_FAIL_BAD_J) && (dgamma < CPSPILS_DGMAX)) ||
+ (convfail == CP_FAIL_OTHER);
+ *jcurPtr = jbad;
+ jok = !jbad;
+
+ /* Call psetE routine and possibly reset jcur */
+ retval = psetE(tn, yP, fctP, jok, jcurPtr, gamma, P_data, tmp1, tmp2, tmp3);
+ if (retval == 0 ) {
+ if (jbad) *jcurPtr = TRUE;
+ /* If jcur = TRUE, increment npe and save nst value */
+ if (*jcurPtr) {
+ npe++;
+ nstlpre = nst;
+ }
+ last_flag = SPGMR_SUCCESS;
+ } else if (retval < 0) {
+ cpProcessError(cp_mem, SPGMR_PSET_FAIL_UNREC, "CPSPGMR", "CPSpgmrSetup", MSGS_PSET_FAILED);
+ last_flag = SPGMR_PSET_FAIL_UNREC;
+ } else if (retval > 0) {
+ last_flag = SPGMR_PSET_FAIL_REC;
+ }
+
+
+ break;
+
+ case CP_IMPL:
+
+ /* Call psetI routine */
+ retval = psetI(tn, yP, ypP, fctP, gamma, P_data, tmp1, tmp2, tmp3);
+ if (retval == 0) {
+ last_flag = SPGMR_SUCCESS;
+ } else if (retval < 0) {
+ cpProcessError(cp_mem, SPGMR_PSET_FAIL_UNREC, "CPSPGMR", "CPSpgmrSetup", MSGS_PSET_FAILED);
+ last_flag = SPGMR_PSET_FAIL_UNREC;
+ }else if (retval > 0) {
+ last_flag = SPGMR_PSET_FAIL_REC;
+ }
+
+ npe++;
+ nstlpre = nst;
+
+ break;
+
+ }
+
+ /* Return the same value that pset returned */
+ return(retval);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpSpgmrSolve
+ * -----------------------------------------------------------------
+ * This routine handles the call to the generic solver SpgmrSolve
+ * for the solution of the linear system Ax = b with the SPGMR method,
+ * without restarts. The solution x is returned in the vector b.
+ *
+ * If the WRMS norm of b is small, we return x = b (if this is the first
+ * Newton iteration) or x = 0 (if a later Newton iteration).
+ *
+ * Otherwise, we set the tolerance parameter and initial guess (x = 0),
+ * call SpgmrSolve, and copy the solution x into b. The x-scaling and
+ * b-scaling arrays are both equal to weight, and no restarts are allowed.
+ *
+ * The counters nli, nps, and ncfl are incremented, and the return value
+ * is set according to the success of SpgmrSolve. The success flag is
+ * returned if SpgmrSolve converged, or if this is the first Newton
+ * iteration and the residual norm was reduced below its initial value.
+ * -----------------------------------------------------------------
+ */
+static int cpSpgmrSolve(CPodeMem cp_mem, N_Vector b, N_Vector weight,
+ N_Vector yC, N_Vector ypC, N_Vector fctC)
+{
+ realtype bnorm, res_norm;
+ CPSpilsMem cpspils_mem;
+ SpgmrMem spgmr_mem;
+ int nli_inc, nps_inc, retval;
+
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ spgmr_mem = (SpgmrMem) spils_mem;
+
+ /* Test norm(b); if small, return x = 0 or x = b */
+ deltar = delt*tq[4];
+
+ bnorm = N_VWrmsNorm(b, weight);
+ if (bnorm <= deltar) {
+ if (mnewt > 0) N_VConst(ZERO, b);
+ return(0);
+ }
+
+ /* Set vectors ycur and fcur for use by the Atimes and Psolve routines */
+ ycur = yC;
+ ypcur = ypC;
+ fcur = fctC;
+
+ /* Set inputs delta and initial guess x = 0 to SpgmrSolve */
+ delta = deltar * sqrtN;
+ N_VConst(ZERO, x);
+
+ /* Call SpgmrSolve and copy x to b */
+ retval = SpgmrSolve(spgmr_mem, cp_mem, x, b, pretype, gstype, delta, 0,
+ cp_mem, weight, weight, cpSpilsAtimes, cpSpilsPSolve,
+ &res_norm, &nli_inc, &nps_inc);
+
+ N_VScale(ONE, x, b);
+
+ /* Increment counters nli, nps, and ncfl */
+ nli += nli_inc;
+ nps += nps_inc;
+ if (retval != SPGMR_SUCCESS) ncfl++;
+
+ /* Interpret return value from SpgmrSolve */
+
+ last_flag = retval;
+
+ switch(retval) {
+
+ case SPGMR_SUCCESS:
+ return(0);
+ break;
+ case SPGMR_RES_REDUCED:
+ if (mnewt == 0) return(0);
+ else return(1);
+ break;
+ case SPGMR_CONV_FAIL:
+ return(1);
+ break;
+ case SPGMR_QRFACT_FAIL:
+ return(1);
+ break;
+ case SPGMR_PSOLVE_FAIL_REC:
+ return(1);
+ break;
+ case SPGMR_ATIMES_FAIL_REC:
+ return(1);
+ break;
+ case SPGMR_MEM_NULL:
+ return(-1);
+ break;
+ case SPGMR_ATIMES_FAIL_UNREC:
+ cpProcessError(cp_mem, SPGMR_ATIMES_FAIL_UNREC, "CPSPGMR", "CPSpgmrSolve", MSGS_JTIMES_FAILED);
+ return(-1);
+ break;
+ case SPGMR_PSOLVE_FAIL_UNREC:
+ cpProcessError(cp_mem, SPGMR_PSOLVE_FAIL_UNREC, "CPSPGMR", "CPSpgmrSolve", MSGS_PSOLVE_FAILED);
+ return(-1);
+ break;
+ case SPGMR_GS_FAIL:
+ return(-1);
+ break;
+ case SPGMR_QRSOL_FAIL:
+ return(-1);
+ break;
+ }
+
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpSpgmrFree
+ * -----------------------------------------------------------------
+ * This routine frees memory specific to the Spgmr linear solver.
+ * -----------------------------------------------------------------
+ */
+
+static void cpSpgmrFree(CPodeMem cp_mem)
+{
+ CPSpilsMem cpspils_mem;
+ SpgmrMem spgmr_mem;
+
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ spgmr_mem = (SpgmrMem) spils_mem;
+
+ SpgmrFree(spgmr_mem);
+ N_VDestroy(x);
+ N_VDestroy(ytemp);
+ if (ode_type == CP_EXPL) N_VDestroy(yptemp);
+ free(cpspils_mem); cpspils_mem = NULL;
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_spils.c b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_spils.c
new file mode 100644
index 0000000..f686612
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_spils.c
@@ -0,0 +1,773 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/24 19:09:18 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for the CPSPILS linear solvers.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "cpodes_private.h"
+#include "cpodes_spils_impl.h"
+
+/* Private constants */
+
+#define ZERO RCONST(0.0)
+#define PT25 RCONST(0.25)
+#define ONE RCONST(1.0)
+
+/* Algorithmic constants */
+
+#define MAX_ITERS 3 /* max. number of attempts to recover in DQ J*v */
+
+/* Readability Replacements */
+
+#define ode_type (cp_mem->cp_ode_type)
+#define lrw1 (cp_mem->cp_lrw1)
+#define liw1 (cp_mem->cp_liw1)
+#define tq (cp_mem->cp_tq)
+#define tn (cp_mem->cp_tn)
+#define h (cp_mem->cp_h)
+#define gamma (cp_mem->cp_gamma)
+#define fe (cp_mem->cp_fe)
+#define fi (cp_mem->cp_fi)
+#define f_data (cp_mem->cp_f_data)
+#define ewt (cp_mem->cp_ewt)
+#define lmem (cp_mem->cp_lmem)
+
+#define ils_type (cpspils_mem->s_type)
+#define sqrtN (cpspils_mem->s_sqrtN)
+#define ytemp (cpspils_mem->s_ytemp)
+#define yptemp (cpspils_mem->s_yptemp)
+#define x (cpspils_mem->s_x)
+#define ycur (cpspils_mem->s_ycur)
+#define ypcur (cpspils_mem->s_ypcur)
+#define fcur (cpspils_mem->s_fcur)
+#define delta (cpspils_mem->s_delta)
+#define npe (cpspils_mem->s_npe)
+#define nli (cpspils_mem->s_nli)
+#define nps (cpspils_mem->s_nps)
+#define ncfl (cpspils_mem->s_ncfl)
+#define njtimes (cpspils_mem->s_njtimes)
+#define nfes (cpspils_mem->s_nfes)
+#define last_flag (cpspils_mem->s_last_flag)
+
+/*
+ * -----------------------------------------------------------------
+ * OPTIONAL INPUT and OUTPUT
+ * -----------------------------------------------------------------
+ */
+
+
+/*
+ * -----------------------------------------------------------------
+ * CPSpilsSetPrecType
+ * -----------------------------------------------------------------
+ */
+
+int CPSpilsSetPrecType(void *cpode_mem, int pretype)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPSPILS_MEM_NULL, "CPSPILS", "CPSpilsSetPrecType", MSGS_CPMEM_NULL);
+ return(CPSPILS_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmem == NULL) {
+ cpProcessError(cp_mem, CPSPILS_LMEM_NULL, "CPSPILS", "CPSpilsSetPrecType", MSGS_LMEM_NULL);
+ return(CPSPILS_LMEM_NULL);
+ }
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ /* Check for legal pretype */
+ if ((pretype != PREC_NONE) && (pretype != PREC_LEFT) &&
+ (pretype != PREC_RIGHT) && (pretype != PREC_BOTH)) {
+ cpProcessError(cp_mem, CPSPILS_ILL_INPUT, "CPSPILS", "CPSpilsSetPrecType", MSGS_BAD_PRETYPE);
+ return(CPSPILS_ILL_INPUT);
+ }
+
+ cpspils_mem->s_pretype = pretype;
+
+ return(CPSPILS_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * CPSpilsSetGSType
+ * -----------------------------------------------------------------
+ */
+
+int CPSpilsSetGSType(void *cpode_mem, int gstype)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPSPILS_MEM_NULL, "CPSPILS", "CPSpilsSetGSType", MSGS_CPMEM_NULL);
+ return(CPSPILS_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmem == NULL) {
+ cpProcessError(cp_mem, CPSPILS_LMEM_NULL, "CPSPILS", "CPSpilsSetGSType", MSGS_LMEM_NULL);
+ return(CPSPILS_LMEM_NULL);
+ }
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ if (ils_type != SPILS_SPGMR) {
+ cpProcessError(cp_mem, CPSPILS_ILL_INPUT, "CPSPILS", "CPSpilsSetGSType", MSGS_BAD_LSTYPE);
+ return(CPSPILS_ILL_INPUT);
+ }
+
+ /* Check for legal gstype */
+ if ((gstype != MODIFIED_GS) && (gstype != CLASSICAL_GS)) {
+ cpProcessError(cp_mem, CPSPILS_ILL_INPUT, "CPSPILS", "CPSpilsSetGSType", MSGS_BAD_GSTYPE);
+ return(CPSPILS_ILL_INPUT);
+ }
+
+ cpspils_mem->s_gstype = gstype;
+
+ return(CPSPILS_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPSpilsSetMaxl
+ * -----------------------------------------------------------------
+ */
+
+int CPSpilsSetMaxl(void *cpode_mem, int maxl)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+ int mxl;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPSPILS_MEM_NULL, "CPSPILS", "CPSpilsSetMaxl", MSGS_CPMEM_NULL);
+ return(CPSPILS_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmem == NULL) {
+ cpProcessError(NULL, CPSPILS_LMEM_NULL, "CPSPILS", "CPSpilsSetMaxl", MSGS_LMEM_NULL);
+ return(CPSPILS_LMEM_NULL);
+ }
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ if (ils_type == SPILS_SPGMR) {
+ cpProcessError(cp_mem, CPSPILS_ILL_INPUT, "CPSPILS", "CPSpilsSetMaxl", MSGS_BAD_LSTYPE);
+ return(CPSPILS_ILL_INPUT);
+ }
+
+ mxl = (maxl <= 0) ? CPSPILS_MAXL : maxl;
+ cpspils_mem->s_maxl = mxl;
+
+ return(CPSPILS_SUCCESS);
+}
+
+
+/*
+ * -----------------------------------------------------------------
+ * CPSpilsSetDelt
+ * -----------------------------------------------------------------
+ */
+
+int CPSpilsSetDelt(void *cpode_mem, realtype delt)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPSPILS_MEM_NULL, "CPSPILS", "CPSpilsSetDelt", MSGS_CPMEM_NULL);
+ return(CPSPILS_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmem == NULL) {
+ cpProcessError(cp_mem, CPSPILS_LMEM_NULL, "CPSPILS", "CPSpilsSetDelt", MSGS_LMEM_NULL);
+ return(CPSPILS_LMEM_NULL);
+ }
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ /* Check for legal delt */
+ if(delt < ZERO) {
+ cpProcessError(cp_mem, CPSPILS_ILL_INPUT, "CPSPILS", "CPSpilsSetDelt", MSGS_BAD_DELT);
+ return(CPSPILS_ILL_INPUT);
+ }
+
+ cpspils_mem->s_delt = (delt == ZERO) ? CPSPILS_DELT : delt;
+
+ return(CPSPILS_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * CPSpilsSetPrecSetupFn
+ * -----------------------------------------------------------------
+ */
+
+int CPSpilsSetPreconditioner(void *cpode_mem, void *pset, void *psolve, void *P_data)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPSPILS_MEM_NULL, "CPSPILS", "CPSpilsSetPreconditioner", MSGS_CPMEM_NULL);
+ return(CPSPILS_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmem == NULL) {
+ cpProcessError(cp_mem, CPSPILS_LMEM_NULL, "CPSPILS", "CPSpilsSetPreconditioner", MSGS_LMEM_NULL);
+ return(CPSPILS_LMEM_NULL);
+ }
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ if (ode_type == CP_EXPL) {
+ cpspils_mem->s_psetE = (CPSpilsPrecSetupExplFn) pset;
+ cpspils_mem->s_pslvE = (CPSpilsPrecSolveExplFn) psolve;
+ } else {
+ cpspils_mem->s_psetI = (CPSpilsPrecSetupImplFn) pset;
+ cpspils_mem->s_pslvI = (CPSpilsPrecSolveImplFn) psolve;
+ }
+ cpspils_mem->s_P_data = P_data;
+
+ return(CPSPILS_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * CPSpilsSetJacTimesVecFn
+ * -----------------------------------------------------------------
+ */
+
+int CPSpilsSetJacTimesVecFn(void *cpode_mem, void *jtimes, void *jac_data)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPSPILS_MEM_NULL, "CPSPILS", "CPSpilsSetJacTimesVecFn", MSGS_CPMEM_NULL);
+ return(CPSPILS_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmem == NULL) {
+ cpProcessError(cp_mem, CPSPILS_LMEM_NULL, "CPSPILS", "CPSpilsSetJacTimesVecFn", MSGS_LMEM_NULL);
+ return(CPSPILS_LMEM_NULL);
+ }
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ if (ode_type == CP_EXPL) cpspils_mem->s_jtvE = (CPSpilsJacTimesVecExplFn) jtimes;
+ else cpspils_mem->s_jtvI = (CPSpilsJacTimesVecImplFn) jtimes;
+ cpspils_mem->s_j_data = jac_data;
+
+ return(CPSPILS_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * CPSpilsGetWorkSpace
+ * -----------------------------------------------------------------
+ */
+
+int CPSpilsGetWorkSpace(void *cpode_mem, long int *lenrwLS, long int *leniwLS)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+ int maxl;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPSPILS_MEM_NULL, "CPSPILS", "CPSpilsGetWorkSpace", MSGS_CPMEM_NULL);
+ return(CPSPILS_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmem == NULL) {
+ cpProcessError(cp_mem, CPSPILS_LMEM_NULL, "CPSPILS", "CPSpilsGetWorkSpace", MSGS_LMEM_NULL);
+ return(CPSPILS_LMEM_NULL);
+ }
+ cpspils_mem = (CPSpilsMem) lmem;
+
+
+ switch(ils_type) {
+ case SPILS_SPGMR:
+ maxl = cpspils_mem->s_maxl;
+ *lenrwLS = lrw1*(maxl + 5) + maxl*(maxl + 4) + 1;
+ *leniwLS = liw1*(maxl + 5);
+ break;
+ case SPILS_SPBCG:
+ *lenrwLS = lrw1 * 9;
+ *leniwLS = liw1 * 9;
+ break;
+ case SPILS_SPTFQMR:
+ *lenrwLS = lrw1*11;
+ *leniwLS = liw1*11;
+ break;
+ }
+
+
+ return(CPSPILS_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * CPSpilsGetNumPrecEvals
+ * -----------------------------------------------------------------
+ */
+
+int CPSpilsGetNumPrecEvals(void *cpode_mem, long int *npevals)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPSPILS_MEM_NULL, "CPSPILS", "CPSpilsGetNumPrecEvals", MSGS_CPMEM_NULL);
+ return(CPSPILS_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmem == NULL) {
+ cpProcessError(cp_mem, CPSPILS_LMEM_NULL, "CPSPILS", "CPSpilsGetNumPrecEvals", MSGS_LMEM_NULL);
+ return(CPSPILS_LMEM_NULL);
+ }
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ *npevals = npe;
+
+ return(CPSPILS_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * CPSpilsGetNumPrecSolves
+ * -----------------------------------------------------------------
+ */
+
+int CPSpilsGetNumPrecSolves(void *cpode_mem, long int *npsolves)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPSPILS_MEM_NULL, "CPSPILS", "CPSpilsGetNumPrecSolves", MSGS_CPMEM_NULL);
+ return(CPSPILS_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmem == NULL) {
+ cpProcessError(cp_mem, CPSPILS_LMEM_NULL, "CPSPILS", "CPSpilsGetNumPrecSolves", MSGS_LMEM_NULL);
+ return(CPSPILS_LMEM_NULL);
+ }
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ *npsolves = nps;
+
+ return(CPSPILS_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * CPSpilsGetNumLinIters
+ * -----------------------------------------------------------------
+ */
+
+int CPSpilsGetNumLinIters(void *cpode_mem, long int *nliters)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPSPILS_MEM_NULL, "CPSPILS", "CPSpilsGetNumLinIters", MSGS_CPMEM_NULL);
+ return(CPSPILS_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmem == NULL) {
+ cpProcessError(cp_mem, CPSPILS_LMEM_NULL, "CPSPILS", "CPSpilsGetNumLinIters", MSGS_LMEM_NULL);
+ return(CPSPILS_LMEM_NULL);
+ }
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ *nliters = nli;
+
+ return(CPSPILS_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * CPSpilsGetNumConvFails
+ * -----------------------------------------------------------------
+ */
+
+int CPSpilsGetNumConvFails(void *cpode_mem, long int *nlcfails)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPSPILS_MEM_NULL, "CPSPILS", "CPSpilsGetNumConvFails", MSGS_CPMEM_NULL);
+ return(CPSPILS_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmem == NULL) {
+ cpProcessError(cp_mem, CPSPILS_LMEM_NULL, "CPSPILS", "CPSpilsGetNumConvFails", MSGS_LMEM_NULL);
+ return(CPSPILS_LMEM_NULL);
+ }
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ *nlcfails = ncfl;
+
+ return(CPSPILS_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * CPSpilsGetNumJtimesEvals
+ * -----------------------------------------------------------------
+ */
+
+int CPSpilsGetNumJtimesEvals(void *cpode_mem, long int *njvevals)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPSPILS_MEM_NULL, "CPSPILS", "CPSpilsGetNumJtimesEvals", MSGS_CPMEM_NULL);
+ return(CPSPILS_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmem == NULL) {
+ cpProcessError(cp_mem, CPSPILS_LMEM_NULL, "CPSPILS", "CPSpilsGetNumJtimesEvals", MSGS_LMEM_NULL);
+ return(CPSPILS_LMEM_NULL);
+ }
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ *njvevals = njtimes;
+
+ return(CPSPILS_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * CPSpilsGetNumFctEvals
+ * -----------------------------------------------------------------
+ */
+
+int CPSpilsGetNumFctEvals(void *cpode_mem, long int *nfevalsLS)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPSPILS_MEM_NULL, "CPSPILS", "CPSpilsGetNumRhsEvals", MSGS_CPMEM_NULL);
+ return(CPSPILS_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmem == NULL) {
+ cpProcessError(cp_mem, CPSPILS_LMEM_NULL, "CPSPILS", "CPSpilsGetNumRhsEvals", MSGS_LMEM_NULL);
+ return(CPSPILS_LMEM_NULL);
+ }
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ *nfevalsLS = nfes;
+
+ return(CPSPILS_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * CPSpilsGetLastFlag
+ * -----------------------------------------------------------------
+ */
+
+int CPSpilsGetLastFlag(void *cpode_mem, int *flag)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPSPILS_MEM_NULL, "CPSPILS", "CPSpilsGetLastFlag", MSGS_CPMEM_NULL);
+ return(CPSPILS_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ if (lmem == NULL) {
+ cpProcessError(cp_mem, CPSPILS_LMEM_NULL, "CPSPILS", "CPSpilsGetLastFlag", MSGS_LMEM_NULL);
+ return(CPSPILS_LMEM_NULL);
+ }
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ *flag = last_flag;
+
+ return(CPSPILS_SUCCESS);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * CPSpilsGetReturnFlagName
+ * -----------------------------------------------------------------
+ */
+
+char *CPSpilsGetReturnFlagName(int flag)
+{
+ char *name;
+
+ name = (char *)malloc(30*sizeof(char));
+
+ switch(flag) {
+ case CPSPILS_SUCCESS:
+ sprintf(name,"CPSPILS_SUCCESS");
+ break;
+ case CPSPILS_MEM_NULL:
+ sprintf(name,"CPSPILS_MEM_NULL");
+ break;
+ case CPSPILS_LMEM_NULL:
+ sprintf(name,"CPSPILS_LMEM_NULL");
+ break;
+ case CPSPILS_ILL_INPUT:
+ sprintf(name,"CPSPILS_ILL_INPUT");
+ break;
+ case CPSPILS_MEM_FAIL:
+ sprintf(name,"CPSPILS_MEM_FAIL");
+ break;
+ default:
+ sprintf(name,"NONE");
+ }
+
+ return(name);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * CPSPILS private functions
+ * -----------------------------------------------------------------
+ */
+
+
+/* Additional readability Replacements */
+
+#define pretype (cpspils_mem->s_pretype)
+#define delt (cpspils_mem->s_delt)
+#define maxl (cpspils_mem->s_maxl)
+#define pslvE (cpspils_mem->s_pslvE)
+#define pslvI (cpspils_mem->s_pslvI)
+#define P_data (cpspils_mem->s_P_data)
+#define jtvE (cpspils_mem->s_jtvE)
+#define jtvI (cpspils_mem->s_jtvI)
+#define j_data (cpspils_mem->s_j_data)
+
+/*
+ * -----------------------------------------------------------------
+ * cpSpilsAtimes
+ * -----------------------------------------------------------------
+ * This routine generates the matrix-vector product z = Mv, where
+ * M = I - gamma*J. The product J*v is obtained by calling the jtimes
+ * routine. It is then scaled by -gamma and added to v to obtain M*v.
+ * The return value is the same as the value returned by jtimes --
+ * 0 if successful, nonzero otherwise.
+ * -----------------------------------------------------------------
+ */
+
+int cpSpilsAtimes(void *cpode_mem, N_Vector v, N_Vector z)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+ int jtflag;
+
+ cp_mem = (CPodeMem) cpode_mem;
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ if (ode_type == CP_EXPL) {
+ jtflag = jtvE(tn, ycur, fcur, v, z, j_data, ytemp);
+ njtimes++;
+ if (jtflag != 0) return(jtflag);
+ N_VLinearSum(ONE, v, -gamma, z, z);
+ } else {
+ jtflag = jtvI(tn, gamma, ycur, ypcur, fcur, v, z, j_data, ytemp, yptemp);
+ njtimes++;
+ if (jtflag != 0) return(jtflag);
+ }
+
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpSpilsPSolve
+ * -----------------------------------------------------------------
+ * This routine interfaces between the generic SpgmrSolve routine and
+ * the user's psolve routine. It passes to psolve all required state
+ * information from cpode_mem. Its return value is the same as that
+ * returned by psolve. Note that the generic SPGMR solver guarantees
+ * that CPSpilsPSolve will not be called in the case in which
+ * preconditioning is not done. This is the only case in which the
+ * user's psolve routine is allowed to be NULL.
+ * -----------------------------------------------------------------
+ */
+
+int cpSpilsPSolve(void *cpode_mem, N_Vector r, N_Vector z, int lr)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+ int retval;
+
+ cp_mem = (CPodeMem) cpode_mem;
+ cpspils_mem = (CPSpilsMem)lmem;
+
+ /* This call is counted in nps within the CPSp***Solve routine */
+ if (ode_type == CP_EXPL) {
+ retval = pslvE(tn, ycur, fcur, r, z, gamma, delta, lr, P_data, ytemp);
+ } else {
+ retval = pslvI(tn, ycur, ypcur, fcur, r, z, gamma, delta, P_data, ytemp);
+ }
+
+ return(retval);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpSpilsDQjtvExpl
+ * -----------------------------------------------------------------
+ * This routine generates a difference quotient approximation to
+ * the Jacobian times vector for explicit ODE: Jv = f_y(t,y) * v.
+ * The approximation is Jv = vnrm[f(y + v/vnrm) - f(y)], where
+ * vnrm = (WRMS norm of v) is input, i.e. WRMS norm of v/vnrm is 1.
+ * -----------------------------------------------------------------
+ */
+
+int cpSpilsDQjtvExpl(realtype t, N_Vector y, N_Vector fy,
+ N_Vector v, N_Vector Jv, void *jac_data,
+ N_Vector tmp)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+ realtype sig, siginv;
+ int iter, retval;
+
+ /* jac_data is cpode_mem */
+ cp_mem = (CPodeMem) jac_data;
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ /* Initialize perturbation to 1/||v|| */
+ sig = ONE/N_VWrmsNorm(v, ewt);
+
+ for (iter=0; iter<MAX_ITERS; iter++) {
+
+ /* Set tmp = y + sig*v */
+ N_VLinearSum(sig, v, ONE, y, tmp);
+
+ /* Set Jv = f(tn, y+sig*v) */
+ retval = fe(t, tmp, Jv, f_data);
+ nfes++;
+ if (retval == 0) break;
+ if (retval < 0) return(-1);
+
+ sig *= PT25;
+ }
+
+ if (retval > 0) return(+1);
+
+ /* Replace Jv by (Jv - fy)/sig */
+ siginv = ONE/sig;
+ N_VLinearSum(siginv, Jv, -siginv, fy, Jv);
+
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * cpSpilsDQjtvImpl
+ * -----------------------------------------------------------------
+ */
+int cpSpilsDQjtvImpl(realtype t, realtype gm,
+ N_Vector y, N_Vector yp, N_Vector r,
+ N_Vector v, N_Vector Jv, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+ N_Vector y_tmp, yp_tmp;
+ realtype sig, siginv;
+ int iter, retval;
+
+ realtype dqincfac;
+
+ /* jac_data is cp_mem */
+ cp_mem = (CPodeMem) jac_data;
+ cpspils_mem = (CPSpilsMem) lmem;
+
+
+ dqincfac = ONE;
+
+
+ switch(ils_type) {
+ case SPILS_SPGMR:
+ sig = sqrtN*dqincfac;
+ break;
+ case SPILS_SPBCG:
+ sig = dqincfac/N_VWrmsNorm(v, ewt);
+ break;
+ case SPILS_SPTFQMR:
+ sig = dqincfac/N_VWrmsNorm(v, ewt);
+ break;
+ }
+
+ /* Rename tmp1 and tmp2 for readibility */
+ y_tmp = tmp1;
+ yp_tmp = tmp2;
+
+ for (iter=0; iter<MAX_ITERS; iter++) {
+
+ /* Set y_tmp = y + gm*sig*v, yp_tmp = yp + sig*v. */
+ N_VLinearSum(gm*sig, v, ONE, y, y_tmp);
+ N_VLinearSum(sig, v, ONE, yp, yp_tmp);
+
+ /* Call res for Jv = F(t, y_tmp, yp_tmp), and return if it failed. */
+ retval = fi(t, y_tmp, yp_tmp, Jv, f_data);
+ nfes++;
+ if (retval == 0) break;
+ if (retval < 0) return(-1);
+
+ sig *= PT25;
+ }
+
+ if (retval > 0) return(+1);
+
+ /* Set Jv to [Jv - r]/sig and return. */
+ siginv = ONE/sig;
+ N_VLinearSum(siginv, Jv, -siginv, r, Jv);
+
+ return(0);
+
+}
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_spils_impl.h b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_spils_impl.h
new file mode 100644
index 0000000..7f608aa
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_spils_impl.h
@@ -0,0 +1,165 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/29 00:05:08 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * Common implementation header file for the scaled, preconditioned
+ * linear solver modules.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _CPSPILS_IMPL_H
+#define _CPSPILS_IMPL_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <cpodes/cpodes_spils.h>
+
+/*
+ * =================================================================
+ * C P S P I L S I N T E R N A L C O N S T A N T S
+ * =================================================================
+ */
+
+/*
+ * -----------------------------------------------------------------
+ * Types of iterative linear solvers
+ * -----------------------------------------------------------------
+ */
+
+#define SPILS_SPGMR 1
+#define SPILS_SPBCG 2
+#define SPILS_SPTFQMR 3
+
+/*
+ * -----------------------------------------------------------------
+ * CPSPILS solver constants
+ * -----------------------------------------------------------------
+ * CPSPILS_MAXL : default value for the maximum Krylov
+ * dimension
+ *
+ * CPSPILS_MSBPRE : maximum number of steps between
+ * preconditioner evaluations
+ *
+ * CPSPILS_DGMAX : maximum change in gamma between
+ * preconditioner evaluations
+ *
+ * CPSPILS_DELT : default value for factor by which the
+ * tolerance on the nonlinear iteration is
+ * multiplied to get a tolerance on the linear
+ * iteration
+ * -----------------------------------------------------------------
+ */
+
+#define CPSPILS_MAXL 5
+#define CPSPILS_MSBPRE 50
+#define CPSPILS_DGMAX RCONST(0.2)
+#define CPSPILS_DELT RCONST(0.05)
+
+/*
+ * -----------------------------------------------------------------
+ * Types : CPSpilsMemRec, CPSpilsMem
+ * -----------------------------------------------------------------
+ * The type CPSpilsMem is pointer to a CPSpilsMemRec.
+ * -----------------------------------------------------------------
+ */
+
+typedef struct {
+
+ int s_type; /* type of scaled preconditioned iterative LS */
+
+ int s_pretype; /* type of preconditioning */
+ int s_gstype; /* type of Gram-Schmidt orthogonalization */
+ realtype s_sqrtN; /* sqrt(N) */
+ realtype s_delt; /* delt = user specified or DELT_DEFAULT */
+ realtype s_deltar; /* deltar = delt * tq4 */
+ realtype s_delta; /* delta = deltar * sqrtN */
+ int s_maxl; /* maxl = maximum dimension of the Krylov space */
+
+ long int s_nstlpre; /* value of nst at the last pset call */
+ long int s_npe; /* npe = total number of pset calls */
+ long int s_nli; /* nli = total number of linear iterations */
+ long int s_nps; /* nps = total number of psolve calls */
+ long int s_ncfl; /* ncfl = total number of convergence failures */
+ long int s_njtimes; /* njtimes = total number of calls to jtimes */
+ long int s_nfes; /* no. of calls to f due to DQ Jacobian approx. */
+
+ N_Vector s_ytemp; /* temp vector passed to jtv and pslv */
+ N_Vector s_yptemp; /* temp vector passed to jtv and pslv */
+ N_Vector s_x; /* temp vector used by CPSpilsSolve */
+ N_Vector s_ycur; /* CPODES current y vector in Newton Iteration */
+ N_Vector s_ypcur; /* CPODES current y' vector in Newton Iteration */
+ N_Vector s_fcur; /* fcur = f(tn, ycur) */
+
+ CPSpilsPrecSetupExplFn s_psetE; /* preconditioner setup (CP_EXPL case) */
+ CPSpilsPrecSetupImplFn s_psetI; /* preconditioner setup (CP_IMPL case) */
+
+ CPSpilsPrecSolveExplFn s_pslvE; /* preconditioner solve (CP_EXPL case) */
+ CPSpilsPrecSolveImplFn s_pslvI; /* preconditioner solve (CP_IMPL case) */
+
+ CPSpilsJacTimesVecExplFn s_jtvE; /* Jac. times vec. (CP_EXPL case) */
+ CPSpilsJacTimesVecImplFn s_jtvI; /* Jac. times vec. (CP_EXPL case) */
+
+ void *s_P_data; /* data passed to psolve and pset */
+ void *s_j_data; /* data passed to jtv */
+
+ void* s_spils_mem; /* memory used by the generic solver */
+
+ int s_last_flag; /* last error flag returned by any function */
+
+} CPSpilsMemRec, *CPSpilsMem;
+
+/*
+ * -----------------------------------------------------------------
+ * Prototypes of internal functions
+ * -----------------------------------------------------------------
+ */
+
+/* Atimes and PSolve routines called by generic solver */
+int cpSpilsAtimes(void *cp_mem, N_Vector v, N_Vector z);
+int cpSpilsPSolve(void *cp_mem, N_Vector r, N_Vector z, int lr);
+
+/* Difference quotient approximations for Jac times vector */
+int cpSpilsDQjtvExpl(realtype t, N_Vector y, N_Vector fy,
+ N_Vector v, N_Vector Jv, void *jac_data,
+ N_Vector tmp);
+int cpSpilsDQjtvImpl(realtype t, realtype gm,
+ N_Vector y, N_Vector yp, N_Vector r,
+ N_Vector v, N_Vector Jv, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2);
+
+/*
+ * -----------------------------------------------------------------
+ * Error Messages
+ * -----------------------------------------------------------------
+ */
+
+#define MSGS_CPMEM_NULL "Integrator memory is NULL."
+#define MSGS_MEM_FAIL "A memory request failed."
+#define MSGS_BAD_NVECTOR "A required vector operation is not implemented."
+#define MSGS_BAD_LSTYPE "Incompatible linear solver type."
+#define MSGS_BAD_PRETYPE "Illegal value for pretype. Legal values are PREC_NONE, PREC_LEFT, PREC_RIGHT, and PREC_BOTH."
+#define MSGS_PSOLVE_REQ "pretype != PREC_NONE, but PSOLVE = NULL is illegal."
+#define MSGS_LMEM_NULL "Linear solver memory is NULL."
+#define MSGS_BAD_GSTYPE "Illegal value for gstype. Legal values are MODIFIED_GS and CLASSICAL_GS."
+#define MSGS_BAD_DELT "delt < 0 illegal."
+
+#define MSGS_PSET_FAILED "The preconditioner setup routine failed in an unrecoverable manner."
+#define MSGS_PSOLVE_FAILED "The preconditioner solve routine failed in an unrecoverable manner."
+#define MSGS_JTIMES_FAILED "The Jacobian x vector routine failed in an unrecoverable manner."
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_sptfqmr.c b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_sptfqmr.c
new file mode 100644
index 0000000..7af2070
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/cpodes/cpodes_sptfqmr.c
@@ -0,0 +1,510 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.1 $
+ * $Date: 2006/11/08 01:07:06 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2006, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for the CPSPTFQMR linear solver.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <cpodes/cpodes_sptfqmr.h>
+#include "cpodes_spils_impl.h"
+#include "cpodes_private.h"
+
+#include <sundials/sundials_sptfqmr.h>
+#include <sundials/sundials_math.h>
+
+/* CPSPTFQMR linit, lsetup, lsolve, and lfree routines */
+static int cpSptfqmrInit(CPodeMem cp_mem);
+static int cpSptfqmrSetup(CPodeMem cp_mem, int convfail,
+ N_Vector yP, N_Vector ypP, N_Vector fctP,
+ booleantype *jcurPtr,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+static int cpSptfqmrSolve(CPodeMem cp_mem, N_Vector b, N_Vector weight,
+ N_Vector yC, N_Vector ypC, N_Vector fctC);
+static void cpSptfqmrFree(CPodeMem cp_mem);
+
+
+/* Readability Replacements */
+
+#define ode_type (cp_mem->cp_ode_type)
+#define tq (cp_mem->cp_tq)
+#define nst (cp_mem->cp_nst)
+#define tn (cp_mem->cp_tn)
+#define gamma (cp_mem->cp_gamma)
+#define gammap (cp_mem->cp_gammap)
+#define f (cp_mem->cp_f)
+#define f_data (cp_mem->cp_f_data)
+#define ewt (cp_mem->cp_ewt)
+#define errfp (cp_mem->cp_errfp)
+#define mnewt (cp_mem->cp_mnewt)
+#define linit (cp_mem->cp_linit)
+#define lsetup (cp_mem->cp_lsetup)
+#define lsolve (cp_mem->cp_lsolve)
+#define lfree (cp_mem->cp_lfree)
+#define lmem (cp_mem->cp_lmem)
+#define vec_tmpl (cp_mem->cp_tempv)
+#define lsetup_exists (cp_mem->cp_lsetup_exists)
+
+#define sqrtN (cpspils_mem->s_sqrtN)
+#define ytemp (cpspils_mem->s_ytemp)
+#define yptemp (cpspils_mem->s_yptemp)
+#define x (cpspils_mem->s_x)
+#define ycur (cpspils_mem->s_ycur)
+#define ypcur (cpspils_mem->s_ypcur)
+#define fcur (cpspils_mem->s_fcur)
+#define delta (cpspils_mem->s_delta)
+#define deltar (cpspils_mem->s_deltar)
+#define npe (cpspils_mem->s_npe)
+#define nli (cpspils_mem->s_nli)
+#define nps (cpspils_mem->s_nps)
+#define ncfl (cpspils_mem->s_ncfl)
+#define nstlpre (cpspils_mem->s_nstlpre)
+#define njtimes (cpspils_mem->s_njtimes)
+#define nfes (cpspils_mem->s_nfes)
+#define spils_mem (cpspils_mem->s_spils_mem)
+#define last_flag (cpspils_mem->s_last_flag)
+
+/*
+ * -----------------------------------------------------------------
+ * Function : CPSptfqmr
+ * -----------------------------------------------------------------
+ * This routine initializes the memory record and sets various function
+ * fields specific to the Sptfqmr linear solver module. CPSptfqmr first
+ * calls the existing lfree routine if this is not NULL. It then sets
+ * the cp_linit, cp_lsetup, cp_lsolve, cp_lfree fields in (*cpode_mem)
+ * to be CPSptfqmrInit, CPSptfqmrSetup, CPSptfqmrSolve, and CPSptfqmrFree,
+ * respectively. It allocates memory for a structure of type
+ * CPSpilsMemRec and sets the cp_lmem field in (*cpode_mem) to the
+ * address of this structure. It sets lsetup_exists in (*cpode_mem),
+ * and sets the following fields in the CPSpilsMemRec structure:
+ *
+ * s_pretype = pretype
+ * s_maxl = CPSPILS_MAXL if maxl <= 0
+ * = maxl if maxl > 0
+ * s_delt = CPSPILS_DELT
+ * s_psetE = NULL
+ * s_psetI = NULL
+ * s_pslvE = NULL
+ * s_pslvI = NULL
+ * s_jtvE = NULL
+ * s_jtvI = NULL
+ * s_P_data = NULL
+ * s_j_data = NULL
+ * Finally, CPSptfqmr allocates memory for ytemp and x, and calls
+ * SptfqmrMalloc to allocate memory for the Sptfqmr solver.
+ * -----------------------------------------------------------------
+ */
+
+int CPSptfqmr(void *cpode_mem, int pretype, int maxl)
+{
+ CPodeMem cp_mem;
+ CPSpilsMem cpspils_mem;
+ SptfqmrMem sptfqmr_mem;
+ int mxl;
+
+ /* Return immediately if cpode_mem is NULL */
+ if (cpode_mem == NULL) {
+ cpProcessError(NULL, CPSPILS_MEM_NULL, "CPSPTFQMR", "CPSptfqmr", MSGS_CPMEM_NULL);
+ return(CPSPILS_MEM_NULL);
+ }
+ cp_mem = (CPodeMem) cpode_mem;
+
+ /* Check if N_VDotProd is present */
+ if (vec_tmpl->ops->nvdotprod == NULL) {
+ cpProcessError(cp_mem, CPSPILS_ILL_INPUT, "CPSPTFQMR", "CPSptfqmr", MSGS_BAD_NVECTOR);
+ return(CPSPILS_ILL_INPUT);
+ }
+
+ if (lfree != NULL) lfree(cp_mem);
+
+ /* Set four main function fields in cp_mem */
+ linit = cpSptfqmrInit;
+ lsetup = cpSptfqmrSetup;
+ lsolve = cpSptfqmrSolve;
+ lfree = cpSptfqmrFree;
+
+ /* Get memory for CPSpilsMemRec */
+ cpspils_mem = NULL;
+ cpspils_mem = (CPSpilsMem) malloc(sizeof(CPSpilsMemRec));
+ if (cpspils_mem == NULL) {
+ cpProcessError(cp_mem, CPSPILS_MEM_FAIL, "CPSPTFQMR", "CPSptfqmr", MSGS_MEM_FAIL);
+ return(CPSPILS_MEM_FAIL);
+ }
+
+ /* Set ILS type */
+ cpspils_mem->s_type = SPILS_SPTFQMR;
+
+ /* Set Sptfqmr parameters that have been passed in call sequence */
+ cpspils_mem->s_pretype = pretype;
+ mxl = cpspils_mem->s_maxl = (maxl <= 0) ? CPSPILS_MAXL : maxl;
+
+ /* Set default values for the rest of the Sptfqmr parameters */
+ cpspils_mem->s_delt = CPSPILS_DELT;
+
+ cpspils_mem->s_psetE = NULL;
+ cpspils_mem->s_psetI = NULL;
+ cpspils_mem->s_pslvE = NULL;
+ cpspils_mem->s_pslvI = NULL;
+ cpspils_mem->s_jtvE = NULL;
+ cpspils_mem->s_jtvI = NULL;
+ cpspils_mem->s_P_data = NULL;
+ cpspils_mem->s_j_data = NULL;
+
+ cpspils_mem->s_last_flag = CPSPILS_SUCCESS;
+
+ lsetup_exists = FALSE;
+
+ /* Check for legal pretype */
+ if ((pretype != PREC_NONE) && (pretype != PREC_LEFT) &&
+ (pretype != PREC_RIGHT) && (pretype != PREC_BOTH)) {
+ cpProcessError(cp_mem, CPSPILS_ILL_INPUT, "CPSPTFQMR", "CPSptfqmr", MSGS_BAD_PRETYPE);
+ return(CPSPILS_ILL_INPUT);
+ }
+
+ /* Alocate memory */
+ sptfqmr_mem = NULL;
+ ytemp = NULL;
+ yptemp = NULL;
+ x = NULL;
+
+ /* Call SptfqmrMalloc to allocate workspace for Sptfqmr */
+ sptfqmr_mem = SptfqmrMalloc(mxl, vec_tmpl);
+ if (sptfqmr_mem == NULL) {
+ cpProcessError(cp_mem, CPSPILS_MEM_FAIL, "CPSPTFQMR", "CPSptfqmr", MSGS_MEM_FAIL);
+ free(cpspils_mem);
+ return(CPSPILS_MEM_FAIL);
+ }
+
+ /* Allocate memory for x, ytemp and (if needed) yptemp */
+ x = N_VClone(vec_tmpl);
+ if (x == NULL) {
+ cpProcessError(cp_mem, CPSPILS_MEM_FAIL, "CPSPTFQMR", "CPSptfqmr", MSGS_MEM_FAIL);
+ SptfqmrFree(sptfqmr_mem);
+ free(cpspils_mem);
+ return(CPSPILS_MEM_FAIL);
+ }
+ ytemp = N_VClone(vec_tmpl);
+ if (ytemp == NULL) {
+ cpProcessError(cp_mem, CPSPILS_MEM_FAIL, "CPSPTFQMR", "CPSptfqmr", MSGS_MEM_FAIL);
+ SptfqmrFree(sptfqmr_mem);
+ N_VDestroy(x);
+ free(cpspils_mem);
+ return(CPSPILS_MEM_FAIL);
+ }
+ if (ode_type == CP_IMPL) {
+ yptemp = N_VClone(vec_tmpl);
+ if (yptemp == NULL) {
+ cpProcessError(cp_mem, CPSPILS_MEM_FAIL, "CPSPTFQMR", "CPSptfqmr", MSGS_MEM_FAIL);
+ SptfqmrFree(sptfqmr_mem);
+ N_VDestroy(x);
+ N_VDestroy(ytemp);
+ free(cpspils_mem);
+ return(CPSPILS_MEM_FAIL);
+ }
+ }
+
+ /* Compute sqrtN from a dot product */
+ N_VConst(ONE, ytemp);
+ sqrtN = RSqrt(N_VDotProd(ytemp, ytemp));
+
+ /* Attach SPTFQMR memory to spils memory structure */
+ spils_mem = (void *) sptfqmr_mem;
+
+ /* Attach linear solver memory to integrator memory */
+ lmem = cpspils_mem;
+
+ return(CPSPILS_SUCCESS);
+}
+
+/* Additional readability replacements */
+
+#define pretype (cpspils_mem->s_pretype)
+#define delt (cpspils_mem->s_delt)
+#define maxl (cpspils_mem->s_maxl)
+#define psetE (cpspils_mem->s_psetE)
+#define psetI (cpspils_mem->s_psetI)
+#define pslvE (cpspils_mem->s_pslvE)
+#define pslvI (cpspils_mem->s_pslvI)
+#define jtvE (cpspils_mem->s_jtvE)
+#define jtvI (cpspils_mem->s_jtvI)
+#define P_data (cpspils_mem->s_P_data)
+#define j_data (cpspils_mem->s_j_data)
+
+/*
+ * -----------------------------------------------------------------
+ * Function : cpSptfqmrInit
+ * -----------------------------------------------------------------
+ * This routine does remaining initializations specific to the Sptfqmr
+ * linear solver.
+ * -----------------------------------------------------------------
+ */
+
+static int cpSptfqmrInit(CPodeMem cp_mem)
+{
+ CPSpilsMem cpspils_mem;
+ SptfqmrMem sptfqmr_mem;
+
+ cpspils_mem = (CPSpilsMem) lmem;
+ sptfqmr_mem = (SptfqmrMem) spils_mem;
+
+ /* Initialize counters */
+ npe = nli = nps = ncfl = nstlpre = 0;
+ njtimes = nfes = 0;
+
+ /*
+ * Check for legal combination pretype - psolve
+ *
+ * Set lsetup_exists = TRUE iff there is preconditioning (pretype != PREC_NONE)
+ * and there is a preconditioning setup phase (pset != NULL)
+ *
+ * If jtimes is NULL at this time, set it to DQ
+ */
+
+ if (ode_type == CP_EXPL) {
+ if ((pretype != PREC_NONE) && (pslvE == NULL)) {
+ cpProcessError(cp_mem, -1, "CPSPTFQMR", "CPSptfqmrInit", MSGS_PSOLVE_REQ);
+ last_flag = CPSPILS_ILL_INPUT;
+ return(-1);
+ }
+ lsetup_exists = (pretype != PREC_NONE) && (psetE != NULL);
+ if (jtvE == NULL) {
+ jtvE = cpSpilsDQjtvExpl;
+ j_data = cp_mem;
+ }
+ } else {
+ if ((pretype != PREC_NONE) && (pslvI == NULL)) {
+ cpProcessError(cp_mem, -1, "CPSPTFQMR", "CPSptfqmrInit", MSGS_PSOLVE_REQ);
+ last_flag = CPSPILS_ILL_INPUT;
+ return(-1);
+ }
+ lsetup_exists = (pretype != PREC_NONE) && (psetI != NULL);
+ if (jtvI == NULL) {
+ jtvI = cpSpilsDQjtvImpl;
+ j_data = cp_mem;
+ }
+ }
+
+ /* Set maxl in the SPTFQMR memory in case it was changed by the user */
+ sptfqmr_mem->l_max = maxl;
+
+ last_flag = CPSPILS_SUCCESS;
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Function : cpSptfqmrSetup
+ * -----------------------------------------------------------------
+ * This routine does the setup operations for the Sptfqmr linear solver.
+ * It makes a decision as to whether or not to signal for reevaluation
+ * of Jacobian data in the pset routine, based on various state
+ * variables, then it calls pset. If we signal for reevaluation,
+ * then we reset jcur = *jcurPtr to TRUE, regardless of the pset output.
+ * In any case, if jcur == TRUE, we increment npe and save nst in nstlpre.
+ * -----------------------------------------------------------------
+ */
+static int cpSptfqmrSetup(CPodeMem cp_mem, int convfail,
+ N_Vector yP, N_Vector ypP, N_Vector fctP,
+ booleantype *jcurPtr,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ booleantype jbad, jok;
+ realtype dgamma;
+ int retval;
+ CPSpilsMem cpspils_mem;
+
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ switch (ode_type) {
+
+ case CP_EXPL:
+
+ /* Use nst, gamma/gammap, and convfail to set J eval. flag jok */
+ dgamma = ABS((gamma/gammap) - ONE);
+ jbad = (nst == 0) || (nst > nstlpre + CPSPILS_MSBPRE) ||
+ ((convfail == CP_FAIL_BAD_J) && (dgamma < CPSPILS_DGMAX)) ||
+ (convfail == CP_FAIL_OTHER);
+ *jcurPtr = jbad;
+ jok = !jbad;
+
+ /* Call pset routine and possibly reset jcur */
+ retval = psetE(tn, yP, fctP, jok, jcurPtr, gamma, P_data, tmp1, tmp2, tmp3);
+ if (retval == 0) {
+ if (jbad) *jcurPtr = TRUE;
+ /* If jcur = TRUE, increment npe and save nst value */
+ if (*jcurPtr) {
+ npe++;
+ nstlpre = nst;
+ }
+ last_flag = SPTFQMR_SUCCESS;
+ } else if (retval < 0) {
+ cpProcessError(cp_mem, SPTFQMR_PSET_FAIL_UNREC, "CPSPTFQMR", "CPSptfqmrSetup", MSGS_PSET_FAILED);
+ last_flag = SPTFQMR_PSET_FAIL_UNREC;
+ } else if (retval > 0) {
+ last_flag = SPTFQMR_PSET_FAIL_REC;
+ }
+
+ break;
+
+ case CP_IMPL:
+
+ /* Call psetI routine */
+ retval = psetI(tn, yP, ypP, fctP, gamma, P_data, tmp1, tmp2, tmp3);
+ if (retval == 0) {
+ last_flag = SPTFQMR_SUCCESS;
+ } else if (retval < 0) {
+ cpProcessError(cp_mem, SPTFQMR_PSET_FAIL_UNREC, "CPSPTFQMR", "CPSptfqmrSetup", MSGS_PSET_FAILED);
+ last_flag = SPTFQMR_PSET_FAIL_UNREC;
+ }else if (retval > 0) {
+ last_flag = SPTFQMR_PSET_FAIL_REC;
+ }
+
+ npe++;
+ nstlpre = nst;
+
+ break;
+
+ }
+
+ /* Return the same value that pset returned */
+ return(retval);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Function : cpSptfqmrSolve
+ * -----------------------------------------------------------------
+ * This routine handles the call to the generic solver SptfqmrSolve
+ * for the solution of the linear system Ax = b with the SPTFQMR method.
+ * The solution x is returned in the vector b.
+ *
+ * If the WRMS norm of b is small, we return x = b (if this is the first
+ * Newton iteration) or x = 0 (if a later Newton iteration).
+ *
+ * Otherwise, we set the tolerance parameter and initial guess (x = 0),
+ * call SptfqmrSolve, and copy the solution x into b. The x-scaling and
+ * b-scaling arrays are both equal to weight.
+ *
+ * The counters nli, nps, and ncfl are incremented, and the return value
+ * is set according to the success of SptfqmrSolve. The success flag is
+ * returned if SptfqmrSolve converged, or if this is the first Newton
+ * iteration and the residual norm was reduced below its initial value.
+ * -----------------------------------------------------------------
+ */
+static int cpSptfqmrSolve(CPodeMem cp_mem, N_Vector b, N_Vector weight,
+ N_Vector yC, N_Vector ypC, N_Vector fctC)
+{
+ realtype bnorm, res_norm;
+ CPSpilsMem cpspils_mem;
+ SptfqmrMem sptfqmr_mem;
+ int nli_inc, nps_inc, retval;
+
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ sptfqmr_mem = (SptfqmrMem) spils_mem;
+
+ /* Test norm(b); if small, return x = 0 or x = b */
+ deltar = delt * tq[4];
+
+ bnorm = N_VWrmsNorm(b, weight);
+ if (bnorm <= deltar) {
+ if (mnewt > 0) N_VConst(ZERO, b);
+ return(0);
+ }
+
+ /* Set vectors ycur and fcur for use by the Atimes and Psolve routines */
+ ycur = yC;
+ ypcur = ypC;
+ fcur = fctC;
+
+ /* Set inputs delta and initial guess x = 0 to SptfqmrSolve */
+ delta = deltar * sqrtN;
+ N_VConst(ZERO, x);
+
+ /* Call SptfqmrSolve and copy x to b */
+ retval = SptfqmrSolve(sptfqmr_mem, cp_mem, x, b, pretype, delta,
+ cp_mem, weight, weight, cpSpilsAtimes, cpSpilsPSolve,
+ &res_norm, &nli_inc, &nps_inc);
+
+ N_VScale(ONE, x, b);
+
+ /* Increment counters nli, nps, and ncfl */
+ nli += nli_inc;
+ nps += nps_inc;
+ if (retval != SPTFQMR_SUCCESS) ncfl++;
+
+ /* Interpret return value from SpgmrSolve */
+
+ last_flag = retval;
+
+ switch(retval) {
+
+ case SPTFQMR_SUCCESS:
+ return(0);
+ break;
+ case SPTFQMR_RES_REDUCED:
+ if (mnewt == 0) return(0);
+ else return(1);
+ break;
+ case SPTFQMR_CONV_FAIL:
+ return(1);
+ break;
+ case SPTFQMR_PSOLVE_FAIL_REC:
+ return(1);
+ break;
+ case SPTFQMR_ATIMES_FAIL_REC:
+ return(1);
+ break;
+ case SPTFQMR_MEM_NULL:
+ return(-1);
+ break;
+ case SPTFQMR_ATIMES_FAIL_UNREC:
+ cpProcessError(cp_mem, SPTFQMR_ATIMES_FAIL_UNREC, "CPSPTFQMR", "CPSptfqmrSolve", MSGS_JTIMES_FAILED);
+ return(-1);
+ break;
+ case SPTFQMR_PSOLVE_FAIL_UNREC:
+ cpProcessError(cp_mem, SPTFQMR_PSOLVE_FAIL_UNREC, "CPSPTFQMR", "CPSptfqmrSolve", MSGS_PSOLVE_FAILED);
+ return(-1);
+ break;
+ }
+
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Function : cpSptfqmrFree
+ * -----------------------------------------------------------------
+ * This routine frees memory specific to the Sptfqmr linear solver.
+ * -----------------------------------------------------------------
+ */
+
+static void cpSptfqmrFree(CPodeMem cp_mem)
+{
+ CPSpilsMem cpspils_mem;
+ SptfqmrMem sptfqmr_mem;
+
+ cpspils_mem = (CPSpilsMem) lmem;
+
+ sptfqmr_mem = (SptfqmrMem) spils_mem;
+
+ SptfqmrFree(sptfqmr_mem);
+ N_VDestroy(x);
+ N_VDestroy(ytemp);
+ if (ode_type == CP_EXPL) N_VDestroy(yptemp);
+ free(cpspils_mem); cpspils_mem = NULL;
+
+ return;
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/nvec_ser/README.txt b/SimTKmath/Integrators/src/CPodes/sundials/src/nvec_ser/README.txt
new file mode 100644
index 0000000..cabcca9
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/nvec_ser/README.txt
@@ -0,0 +1,121 @@
+ NVECTOR_SERIAL
+ Release 2.3.0, November 2006
+
+Serial implementation of the NVECTOR module for SUNDIALS.
+
+NVECTOR_SERIAL defines the content field of N_Vector to be a structure
+containing the length of the vector, a pointer to the beginning of a
+contiguous data array, and a boolean flag indicating ownership of the
+data array.
+
+NVECTOR_SERIAL defines five macros to provide access to the content of
+a serial N_Vector, several constructors for variables of type N_Vector,
+a constructor for an array of variables of type N_Vector, and destructors
+for N_Vector and N_Vector array.
+
+NVECTOR_SERIAL provides implementations for all vector operations defined
+by the generic NVECTOR module in the table of operations.
+
+
+A. Documentation
+----------------
+
+The serial NVECTOR implementation is fully described in the user documentation
+for any of the SUNDIALS solvers [1,2,3,4]. PostScript and PDF files for the user
+guide for a particular solver are available in the solver's directory.
+
+
+B. Installation
+---------------
+
+For basic installation instructions see /sundials/INSTALL.
+For complete installation instructions see any of the user guides.
+
+
+C. References
+-------------
+
+[1] A. C. Hindmarsh and R. Serban, "User Documentation for CVODE v2.4.0,"
+ LLLNL technical report UCRL-MA-208108, November 2004.
+
+[2] A. C. Hindmarsh and R. Serban, "User Documentation for CVODES v2.4.0,"
+ LLNL technical report UCRL-MA-208111, November 2004.
+
+[3] A. C. Hindmarsh and R. Serban, "User Documentation for IDA v2.4.0,"
+ LLNL technical report UCRL-MA-208112, November 2004.
+
+[4] A. M. Collier, A. C. Hindmarsh, R. Serban,and C. S. Woodward, "User
+ Documentation for KINSOL v2.4.0," LLNL technical report UCRL-MA-208116,
+ November 2004.
+
+
+D. Releases
+-----------
+
+v. 2.3.0 - Nov. 2006
+v. 2.2.0 - Mar. 2006
+v. 2.1.1 - May. 2005
+v. 2.1.0 - Apr. 2005
+v. 2.0.2 - Mar. 2005
+v. 2.0.1 - Jan. 2005
+v. 2.0 - Dec. 2004
+v. 1.0 - Jul. 2002 (first SUNDIALS release)
+
+
+E. Revision History
+-------------------
+
+v. 2.2.0 (Mar. 2006) ---> v. 2.3.0 (Nov. 2006)
+----------------------------------------------
+
+- Changes related to the build system
+ - reorganized source tree. Header files in ${srcdir}/include/nvector;
+ sources in ${srcdir}/src/nvec_ser
+ - exported header files in ${includedir}/sundials
+
+
+v. 2.1.1 (May. 2005) ---> v. 2.2.0 (Mar. 2006)
+----------------------------------------------
+
+- none
+
+v. 2.1.0 (Apr. 2005) ---> v. 2.1.1 (May. 2005)
+----------------------------------------------
+
+- Changes to data structures
+ - added N_VCloneEmpty to global vector operations table
+
+v. 2.0.2 (Mar. 2005) ---> v. 2.1.0 (Apr. 2005)
+----------------------------------------------
+
+- none
+
+v. 2.0.1 (Jan. 2005) ---> v. 2.0.2 (Mar. 2005)
+----------------------------------------------
+
+- Changes related to the build system
+ - fixed autoconf-related bug to allow configuration with the PGI Fortran compiler
+ - modified to use customized detection of the Fortran name mangling scheme
+ (autoconf's AC_F77_WRAPPERS routine is problematic on some platforms)
+ - added --with-mpi-flags as a configure option to allow user to specify
+ MPI-specific flags
+ - updated Makefiles for Fortran examples to avoid C++ compiler errors (now use
+ CC and MPICC to link)
+
+v. 2.0 (Dec. 2004) ---> v. 2.0.1 (Jan. 2005)
+--------------------------------------------
+
+- Changes related to the build system
+ - changed order of compiler directives in header files to avoid compilation
+ errors when using a C++ compiler.
+
+v. 1.0 (Jul. 2002) ---> v. 2.0 (Dec. 2004)
+------------------------------------------
+
+- Revised to correspond to new generic NVECTOR module
+ (see sundials/shared/README).
+- Extended the list of user-callable functions provided by NVECTOR_SERIAL
+ outside the table of vector operations.
+- Revised the F/C interface to use underscore flags for name mapping
+ and to use precision flag from configure.
+- Revised F/C routine NVECTOR names for uniformity.
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/nvec_ser/fnvector_serial.c b/SimTKmath/Integrators/src/CPodes/sundials/src/nvec_ser/fnvector_serial.c
new file mode 100644
index 0000000..8f83c80
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/nvec_ser/fnvector_serial.c
@@ -0,0 +1,147 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.1 $
+ * $Date: 2006/07/05 15:32:37 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This file (companion of nvector_serial.h) contains the
+ * implementation needed for the Fortran initialization of serial
+ * vector operations.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "fnvector_serial.h"
+
+/* Define global vector variables */
+
+N_Vector F2C_CVODE_vec;
+N_Vector F2C_CVODE_vecQ;
+N_Vector *F2C_CVODE_vecS;
+N_Vector F2C_CVODE_vecB;
+N_Vector F2C_CVODE_vecQB;
+
+N_Vector F2C_IDA_vec;
+N_Vector F2C_IDA_vecQ;
+N_Vector *F2C_IDA_vecS;
+N_Vector F2C_IDA_vecB;
+N_Vector F2C_IDA_vecQB;
+
+N_Vector F2C_KINSOL_vec;
+
+/* Fortran callable interfaces */
+
+void FNV_INITS(int *code, long int *N, int *ier)
+{
+ *ier = 0;
+
+ switch(*code) {
+ case FCMIX_CVODE:
+ F2C_CVODE_vec = NULL;
+ F2C_CVODE_vec = N_VNewEmpty_Serial(*N);
+ if (F2C_CVODE_vec == NULL) *ier = -1;
+ break;
+ case FCMIX_IDA:
+ F2C_IDA_vec = NULL;
+ F2C_IDA_vec = N_VNewEmpty_Serial(*N);
+ if (F2C_IDA_vec == NULL) *ier = -1;
+ break;
+ case FCMIX_KINSOL:
+ F2C_KINSOL_vec = NULL;
+ F2C_KINSOL_vec = N_VNewEmpty_Serial(*N);
+ if (F2C_KINSOL_vec == NULL) *ier = -1;
+ break;
+ default:
+ *ier = -1;
+ }
+}
+
+void FNV_INITS_Q(int *code, long int *Nq, int *ier)
+{
+ *ier = 0;
+
+ switch(*code) {
+ case FCMIX_CVODE:
+ F2C_CVODE_vecQ = NULL;
+ F2C_CVODE_vecQ = N_VNewEmpty_Serial(*Nq);
+ if (F2C_CVODE_vecQ == NULL) *ier = -1;
+ break;
+ case FCMIX_IDA:
+ F2C_IDA_vecQ = NULL;
+ F2C_IDA_vecQ = N_VNewEmpty_Serial(*Nq);
+ if (F2C_IDA_vecQ == NULL) *ier = -1;
+ break;
+ default:
+ *ier = -1;
+ }
+}
+
+void FNV_INITS_B(int *code, long int *NB, int *ier)
+{
+ *ier = 0;
+
+ switch(*code) {
+ case FCMIX_CVODE:
+ F2C_CVODE_vecB = NULL;
+ F2C_CVODE_vecB = N_VNewEmpty_Serial(*NB);
+ if (F2C_CVODE_vecB == NULL) *ier = -1;
+ break;
+ case FCMIX_IDA:
+ F2C_IDA_vecB = NULL;
+ F2C_IDA_vecB = N_VNewEmpty_Serial(*NB);
+ if (F2C_IDA_vecB == NULL) *ier = -1;
+ break;
+ default:
+ *ier = -1;
+ }
+}
+
+void FNV_INITS_QB(int *code, long int *NqB, int *ier)
+{
+ *ier = 0;
+
+ switch(*code) {
+ case FCMIX_CVODE:
+ F2C_CVODE_vecQB = NULL;
+ F2C_CVODE_vecQB = N_VNewEmpty_Serial(*NqB);
+ if (F2C_CVODE_vecQB == NULL) *ier = -1;
+ break;
+ case FCMIX_IDA:
+ F2C_IDA_vecQB = NULL;
+ F2C_IDA_vecQB = N_VNewEmpty_Serial(*NqB);
+ if (F2C_IDA_vecQB == NULL) *ier = -1;
+ break;
+ default:
+ *ier = -1;
+ }
+}
+
+void FNV_INITS_S(int *code, int *Ns, int *ier)
+{
+ *ier = 0;
+
+ switch(*code) {
+ case FCMIX_CVODE:
+ F2C_CVODE_vecS = NULL;
+ F2C_CVODE_vecS = (N_Vector *) N_VCloneVectorArrayEmpty_Serial(*Ns, F2C_CVODE_vec);
+ if (F2C_CVODE_vecS == NULL) *ier = -1;
+ break;
+ case FCMIX_IDA:
+ F2C_IDA_vecS = NULL;
+ F2C_IDA_vecS = (N_Vector *) N_VCloneVectorArrayEmpty_Serial(*Ns, F2C_IDA_vec);
+ if (F2C_IDA_vecS == NULL) *ier = -1;
+ break;
+ default:
+ *ier = -1;
+ }
+}
+
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/nvec_ser/fnvector_serial.h b/SimTKmath/Integrators/src/CPodes/sundials/src/nvec_ser/fnvector_serial.h
new file mode 100644
index 0000000..2642337
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/nvec_ser/fnvector_serial.h
@@ -0,0 +1,84 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.3 $
+ * $Date: 2006/11/29 00:05:09 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Radu Serban and Aaron Collier @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This file (companion of nvector_serial.h) contains the
+ * definitions needed for the initialization of serial
+ * vector operations in Fortran.
+ * -----------------------------------------------------------------
+ */
+
+#ifndef _FNVECTOR_SERIAL_H
+#define _FNVECTOR_SERIAL_H
+
+#ifdef __cplusplus /* wrapper to enable C++ usage */
+extern "C" {
+#endif
+
+#include <nvector/nvector_serial.h>
+#include <sundials/sundials_fnvector.h>
+
+#if defined(F77_FUNC)
+
+#define FNV_INITS F77_FUNC(fnvinits, FNVINITS)
+#define FNV_INITS_Q F77_FUNC_(fnvinits_q, FNVINITS_Q)
+#define FNV_INITS_S F77_FUNC_(fnvinits_s, FNVINITS_S)
+#define FNV_INITS_B F77_FUNC_(fnvinits_b, FNVINITS_B)
+#define FNV_INITS_QB F77_FUNC_(fnvinits_qb, FNVINITS_QB)
+
+#else
+
+#define FNV_INITS fnvinits_
+#define FNV_INITS_Q fnvinits_q_
+#define FNV_INITS_S fnvinits_s_
+#define FNV_INITS_B fnvinits_b_
+#define FNV_INITS_QB fnvinits_qb_
+
+#endif
+
+/* Declarations of global variables */
+
+extern N_Vector F2C_CVODE_vec;
+extern N_Vector F2C_CVODE_vecQ;
+extern N_Vector *F2C_CVODE_vecS;
+extern N_Vector F2C_CVODE_vecB;
+extern N_Vector F2C_CVODE_vecQB;
+
+extern N_Vector F2C_IDA_vec;
+extern N_Vector F2C_IDA_vecQ;
+extern N_Vector *F2C_IDA_vecS;
+extern N_Vector F2C_IDA_vecB;
+extern N_Vector F2C_IDA_vecQB;
+
+extern N_Vector F2C_KINSOL_vec;
+
+/*
+ * Prototypes of exported functions
+ *
+ * FNV_INITS - initializes serial vector operations for main problem
+ * FNV_INITS_Q - initializes serial vector operations for quadratures
+ * FNV_INITS_S - initializes serial vector operations for sensitivities
+ * FNV_INITS_B - initializes serial vector operations for adjoint problem
+ * FNV_INITS_QB - initializes serial vector operations for adjoint quadratures
+ *
+ */
+
+void FNV_INITS(int *code, long int *neq, int *ier);
+void FNV_INITS_Q(int *code, long int *Nq, int *ier);
+void FNV_INITS_S(int *code, int *Ns, int *ier);
+void FNV_INITS_B(int *code, long int *NB, int *ier);
+void FNV_INITS_QB(int *code, long int *NqB, int *ier);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/nvec_ser/nvector_serial.c b/SimTKmath/Integrators/src/CPodes/sundials/src/nvec_ser/nvector_serial.c
new file mode 100644
index 0000000..c890253
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/nvec_ser/nvector_serial.c
@@ -0,0 +1,1034 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.1 $
+ * $Date: 2006/07/05 15:32:37 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Scott D. Cohen, Alan C. Hindmarsh, Radu Serban,
+ * and Aaron Collier @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for a serial implementation
+ * of the NVECTOR package.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <nvector/nvector_serial.h>
+#include <sundials/sundials_math.h>
+
+#define ZERO RCONST(0.0)
+#define HALF RCONST(0.5)
+#define ONE RCONST(1.0)
+#define ONEPT5 RCONST(1.5)
+
+/* Private function prototypes */
+/* z=x */
+static void VCopy_Serial(N_Vector x, N_Vector z);
+/* z=x+y */
+static void VSum_Serial(N_Vector x, N_Vector y, N_Vector z);
+/* z=x-y */
+static void VDiff_Serial(N_Vector x, N_Vector y, N_Vector z);
+/* z=-x */
+static void VNeg_Serial(N_Vector x, N_Vector z);
+/* z=c(x+y) */
+static void VScaleSum_Serial(realtype c, N_Vector x, N_Vector y, N_Vector z);
+/* z=c(x-y) */
+static void VScaleDiff_Serial(realtype c, N_Vector x, N_Vector y, N_Vector z);
+/* z=ax+y */
+static void VLin1_Serial(realtype a, N_Vector x, N_Vector y, N_Vector z);
+/* z=ax-y */
+static void VLin2_Serial(realtype a, N_Vector x, N_Vector y, N_Vector z);
+/* y <- ax+y */
+static void Vaxpy_Serial(realtype a, N_Vector x, N_Vector y);
+/* x <- ax */
+static void VScaleBy_Serial(realtype a, N_Vector x);
+
+/*
+ * -----------------------------------------------------------------
+ * exported functions
+ * -----------------------------------------------------------------
+ */
+
+/* ----------------------------------------------------------------------------
+ * Function to create a new empty serial vector
+ */
+
+N_Vector N_VNewEmpty_Serial(long int length)
+{
+ N_Vector v;
+ N_Vector_Ops ops;
+ N_VectorContent_Serial content;
+
+ /* Create vector */
+ v = NULL;
+ v = (N_Vector) malloc(sizeof *v);
+ if (v == NULL) return(NULL);
+
+ /* Create vector operation structure */
+ ops = NULL;
+ ops = (N_Vector_Ops) malloc(sizeof(struct _generic_N_Vector_Ops));
+ if (ops == NULL) { free(v); return(NULL); }
+
+ ops->nvclone = N_VClone_Serial;
+ ops->nvcloneempty = N_VCloneEmpty_Serial;
+ ops->nvdestroy = N_VDestroy_Serial;
+ ops->nvspace = N_VSpace_Serial;
+ ops->nvgetarraypointer = N_VGetArrayPointer_Serial;
+ ops->nvsetarraypointer = N_VSetArrayPointer_Serial;
+ ops->nvlinearsum = N_VLinearSum_Serial;
+ ops->nvconst = N_VConst_Serial;
+ ops->nvprod = N_VProd_Serial;
+ ops->nvdiv = N_VDiv_Serial;
+ ops->nvscale = N_VScale_Serial;
+ ops->nvabs = N_VAbs_Serial;
+ ops->nvinv = N_VInv_Serial;
+ ops->nvaddconst = N_VAddConst_Serial;
+ ops->nvdotprod = N_VDotProd_Serial;
+ ops->nvmaxnorm = N_VMaxNorm_Serial;
+ ops->nvwrmsnormmask = N_VWrmsNormMask_Serial;
+ ops->nvwrmsnorm = N_VWrmsNorm_Serial;
+ ops->nvmin = N_VMin_Serial;
+ ops->nvwl2norm = N_VWL2Norm_Serial;
+ ops->nvl1norm = N_VL1Norm_Serial;
+ ops->nvcompare = N_VCompare_Serial;
+ ops->nvinvtest = N_VInvTest_Serial;
+ ops->nvconstrmask = N_VConstrMask_Serial;
+ ops->nvminquotient = N_VMinQuotient_Serial;
+
+ /* Create content */
+ content = NULL;
+ content = (N_VectorContent_Serial) malloc(sizeof(struct _N_VectorContent_Serial));
+ if (content == NULL) { free(ops); free(v); return(NULL); }
+
+ content->length = length;
+ content->own_data = FALSE;
+ content->data = NULL;
+
+ /* Attach content and ops */
+ v->content = content;
+ v->ops = ops;
+
+ return(v);
+}
+
+/* ----------------------------------------------------------------------------
+ * Function to create a new serial vector
+ */
+
+N_Vector N_VNew_Serial(long int length)
+{
+ N_Vector v;
+ realtype *data;
+
+ v = NULL;
+ v = N_VNewEmpty_Serial(length);
+ if (v == NULL) return(NULL);
+
+ /* Create data */
+ if (length > 0) {
+
+ /* Allocate memory */
+ data = NULL;
+ data = (realtype *) malloc(length * sizeof(realtype));
+ if(data == NULL) { N_VDestroy_Serial(v); return(NULL); }
+
+ /* Attach data */
+ NV_OWN_DATA_S(v) = TRUE;
+ NV_DATA_S(v) = data;
+
+ }
+
+ return(v);
+}
+
+/* ----------------------------------------------------------------------------
+ * Function to create a serial N_Vector with user data component
+ */
+
+N_Vector N_VMake_Serial(long int length, realtype *v_data)
+{
+ N_Vector v;
+
+ v = NULL;
+ v = N_VNewEmpty_Serial(length);
+ if (v == NULL) return(NULL);
+
+ if (length > 0) {
+ /* Attach data */
+ NV_OWN_DATA_S(v) = FALSE;
+ NV_DATA_S(v) = v_data;
+ }
+
+ return(v);
+}
+
+/* ----------------------------------------------------------------------------
+ * Function to create an array of new serial vectors.
+ */
+
+N_Vector *N_VCloneVectorArray_Serial(int count, N_Vector w)
+{
+ N_Vector *vs;
+ int j;
+
+ if (count <= 0) return(NULL);
+
+ vs = NULL;
+ vs = (N_Vector *) malloc(count * sizeof(N_Vector));
+ if(vs == NULL) return(NULL);
+
+ for (j = 0; j < count; j++) {
+ vs[j] = NULL;
+ vs[j] = N_VClone_Serial(w);
+ if (vs[j] == NULL) {
+ N_VDestroyVectorArray_Serial(vs, j-1);
+ return(NULL);
+ }
+ }
+
+ return(vs);
+}
+
+/* ----------------------------------------------------------------------------
+ * Function to create an array of new serial vectors with NULL data array.
+ */
+
+N_Vector *N_VCloneVectorArrayEmpty_Serial(int count, N_Vector w)
+{
+ N_Vector *vs;
+ int j;
+
+ if (count <= 0) return(NULL);
+
+ vs = NULL;
+ vs = (N_Vector *) malloc(count * sizeof(N_Vector));
+ if(vs == NULL) return(NULL);
+
+ for (j = 0; j < count; j++) {
+ vs[j] = NULL;
+ vs[j] = N_VCloneEmpty_Serial(w);
+ if (vs[j] == NULL) {
+ N_VDestroyVectorArray_Serial(vs, j-1);
+ return(NULL);
+ }
+ }
+
+ return(vs);
+}
+
+/* ----------------------------------------------------------------------------
+ * Function to free an array created with N_VCloneVectorArray_Serial
+ */
+
+void N_VDestroyVectorArray_Serial(N_Vector *vs, int count)
+{
+ int j;
+
+ for (j = 0; j < count; j++) N_VDestroy_Serial(vs[j]);
+
+ free(vs); vs = NULL;
+
+ return;
+}
+
+/* ----------------------------------------------------------------------------
+ * Function to print the a serial vector
+ */
+
+void N_VPrint_Serial(N_Vector x)
+{
+ long int i, N;
+ realtype *xd;
+
+ xd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+
+ for (i = 0; i < N; i++) {
+#if defined(SUNDIALS_EXTENDED_PRECISION)
+ printf("%11.8Lg\n", xd[i]);
+#elif defined(SUNDIALS_DOUBLE_PRECISION)
+ printf("%11.8lg\n", xd[i]);
+#else
+ printf("%11.8g\n", xd[i]);
+#endif
+ }
+ printf("\n");
+
+ return;
+}
+
+/*
+ * -----------------------------------------------------------------
+ * implementation of vector operations
+ * -----------------------------------------------------------------
+ */
+
+N_Vector N_VCloneEmpty_Serial(N_Vector w)
+{
+ N_Vector v;
+ N_Vector_Ops ops;
+ N_VectorContent_Serial content;
+
+ if (w == NULL) return(NULL);
+
+ /* Create vector */
+ v = NULL;
+ v = (N_Vector) malloc(sizeof *v);
+ if (v == NULL) return(NULL);
+
+ /* Create vector operation structure */
+ ops = NULL;
+ ops = (N_Vector_Ops) malloc(sizeof(struct _generic_N_Vector_Ops));
+ if (ops == NULL) { free(v); return(NULL); }
+
+ ops->nvclone = w->ops->nvclone;
+ ops->nvcloneempty = w->ops->nvcloneempty;
+ ops->nvdestroy = w->ops->nvdestroy;
+ ops->nvspace = w->ops->nvspace;
+ ops->nvgetarraypointer = w->ops->nvgetarraypointer;
+ ops->nvsetarraypointer = w->ops->nvsetarraypointer;
+ ops->nvlinearsum = w->ops->nvlinearsum;
+ ops->nvconst = w->ops->nvconst;
+ ops->nvprod = w->ops->nvprod;
+ ops->nvdiv = w->ops->nvdiv;
+ ops->nvscale = w->ops->nvscale;
+ ops->nvabs = w->ops->nvabs;
+ ops->nvinv = w->ops->nvinv;
+ ops->nvaddconst = w->ops->nvaddconst;
+ ops->nvdotprod = w->ops->nvdotprod;
+ ops->nvmaxnorm = w->ops->nvmaxnorm;
+ ops->nvwrmsnormmask = w->ops->nvwrmsnormmask;
+ ops->nvwrmsnorm = w->ops->nvwrmsnorm;
+ ops->nvmin = w->ops->nvmin;
+ ops->nvwl2norm = w->ops->nvwl2norm;
+ ops->nvl1norm = w->ops->nvl1norm;
+ ops->nvcompare = w->ops->nvcompare;
+ ops->nvinvtest = w->ops->nvinvtest;
+ ops->nvconstrmask = w->ops->nvconstrmask;
+ ops->nvminquotient = w->ops->nvminquotient;
+
+ /* Create content */
+ content = NULL;
+ content = (N_VectorContent_Serial) malloc(sizeof(struct _N_VectorContent_Serial));
+ if (content == NULL) { free(ops); free(v); return(NULL); }
+
+ content->length = NV_LENGTH_S(w);
+ content->own_data = FALSE;
+ content->data = NULL;
+
+ /* Attach content and ops */
+ v->content = content;
+ v->ops = ops;
+
+ return(v);
+}
+
+N_Vector N_VClone_Serial(N_Vector w)
+{
+ N_Vector v;
+ realtype *data;
+ long int length;
+
+ v = NULL;
+ v = N_VCloneEmpty_Serial(w);
+ if (v == NULL) return(NULL);
+
+ length = NV_LENGTH_S(w);
+
+ /* Create data */
+ if (length > 0) {
+
+ /* Allocate memory */
+ data = NULL;
+ data = (realtype *) malloc(length * sizeof(realtype));
+ if(data == NULL) { N_VDestroy_Serial(v); return(NULL); }
+
+ /* Attach data */
+ NV_OWN_DATA_S(v) = TRUE;
+ NV_DATA_S(v) = data;
+
+ }
+
+ return(v);
+}
+
+void N_VDestroy_Serial(N_Vector v)
+{
+ if (NV_OWN_DATA_S(v) == TRUE) {
+ free(NV_DATA_S(v));
+ NV_DATA_S(v) = NULL;
+ }
+ free(v->content); v->content = NULL;
+ free(v->ops); v->ops = NULL;
+ free(v); v = NULL;
+
+ return;
+}
+
+void N_VSpace_Serial(N_Vector v, long int *lrw, long int *liw)
+{
+ *lrw = NV_LENGTH_S(v);
+ *liw = 1;
+
+ return;
+}
+
+realtype *N_VGetArrayPointer_Serial(N_Vector v)
+{
+ return((realtype *) NV_DATA_S(v));
+}
+
+void N_VSetArrayPointer_Serial(realtype *v_data, N_Vector v)
+{
+ if (NV_LENGTH_S(v) > 0) NV_DATA_S(v) = v_data;
+
+ return;
+}
+
+void N_VLinearSum_Serial(realtype a, N_Vector x, realtype b, N_Vector y, N_Vector z)
+{
+ long int i, N;
+ realtype c, *xd, *yd, *zd;
+ N_Vector v1, v2;
+ booleantype test;
+
+ xd = yd = zd = NULL;
+
+ if ((b == ONE) && (z == y)) { /* BLAS usage: axpy y <- ax+y */
+ Vaxpy_Serial(a,x,y);
+ return;
+ }
+
+ if ((a == ONE) && (z == x)) { /* BLAS usage: axpy x <- by+x */
+ Vaxpy_Serial(b,y,x);
+ return;
+ }
+
+ /* Case: a == b == 1.0 */
+
+ if ((a == ONE) && (b == ONE)) {
+ VSum_Serial(x, y, z);
+ return;
+ }
+
+ /* Cases: (1) a == 1.0, b = -1.0, (2) a == -1.0, b == 1.0 */
+
+ if ((test = ((a == ONE) && (b == -ONE))) || ((a == -ONE) && (b == ONE))) {
+ v1 = test ? y : x;
+ v2 = test ? x : y;
+ VDiff_Serial(v2, v1, z);
+ return;
+ }
+
+ /* Cases: (1) a == 1.0, b == other or 0.0, (2) a == other or 0.0, b == 1.0 */
+ /* if a or b is 0.0, then user should have called N_VScale */
+
+ if ((test = (a == ONE)) || (b == ONE)) {
+ c = test ? b : a;
+ v1 = test ? y : x;
+ v2 = test ? x : y;
+ VLin1_Serial(c, v1, v2, z);
+ return;
+ }
+
+ /* Cases: (1) a == -1.0, b != 1.0, (2) a != 1.0, b == -1.0 */
+
+ if ((test = (a == -ONE)) || (b == -ONE)) {
+ c = test ? b : a;
+ v1 = test ? y : x;
+ v2 = test ? x : y;
+ VLin2_Serial(c, v1, v2, z);
+ return;
+ }
+
+ /* Case: a == b */
+ /* catches case both a and b are 0.0 - user should have called N_VConst */
+
+ if (a == b) {
+ VScaleSum_Serial(a, x, y, z);
+ return;
+ }
+
+ /* Case: a == -b */
+
+ if (a == -b) {
+ VScaleDiff_Serial(a, x, y, z);
+ return;
+ }
+
+ /* Do all cases not handled above:
+ (1) a == other, b == 0.0 - user should have called N_VScale
+ (2) a == 0.0, b == other - user should have called N_VScale
+ (3) a,b == other, a !=b, a != -b */
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ yd = NV_DATA_S(y);
+ zd = NV_DATA_S(z);
+
+ for (i = 0; i < N; i++)
+ zd[i] = (a*xd[i])+(b*yd[i]);
+
+ return;
+}
+
+void N_VConst_Serial(realtype c, N_Vector z)
+{
+ long int i, N;
+ realtype *zd;
+
+ zd = NULL;
+
+ N = NV_LENGTH_S(z);
+ zd = NV_DATA_S(z);
+
+ for (i = 0; i < N; i++) zd[i] = c;
+
+ return;
+}
+
+void N_VProd_Serial(N_Vector x, N_Vector y, N_Vector z)
+{
+ long int i, N;
+ realtype *xd, *yd, *zd;
+
+ xd = yd = zd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ yd = NV_DATA_S(y);
+ zd = NV_DATA_S(z);
+
+ for (i = 0; i < N; i++)
+ zd[i] = xd[i]*yd[i];
+
+ return;
+}
+
+void N_VDiv_Serial(N_Vector x, N_Vector y, N_Vector z)
+{
+ long int i, N;
+ realtype *xd, *yd, *zd;
+
+ xd = yd = zd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ yd = NV_DATA_S(y);
+ zd = NV_DATA_S(z);
+
+ for (i = 0; i < N; i++)
+ zd[i] = xd[i]/yd[i];
+
+ return;
+}
+
+void N_VScale_Serial(realtype c, N_Vector x, N_Vector z)
+{
+ long int i, N;
+ realtype *xd, *zd;
+
+ xd = zd = NULL;
+
+ if (z == x) { /* BLAS usage: scale x <- cx */
+ VScaleBy_Serial(c, x);
+ return;
+ }
+
+ if (c == ONE) {
+ VCopy_Serial(x, z);
+ } else if (c == -ONE) {
+ VNeg_Serial(x, z);
+ } else {
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ zd = NV_DATA_S(z);
+ for (i = 0; i < N; i++)
+ zd[i] = c*xd[i];
+ }
+
+ return;
+}
+
+void N_VAbs_Serial(N_Vector x, N_Vector z)
+{
+ long int i, N;
+ realtype *xd, *zd;
+
+ xd = zd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ zd = NV_DATA_S(z);
+
+ for (i = 0; i < N; i++)
+ zd[i] = ABS(xd[i]);
+
+ return;
+}
+
+void N_VInv_Serial(N_Vector x, N_Vector z)
+{
+ long int i, N;
+ realtype *xd, *zd;
+
+ xd = zd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ zd = NV_DATA_S(z);
+
+ for (i = 0; i < N; i++)
+ zd[i] = ONE/xd[i];
+
+ return;
+}
+
+void N_VAddConst_Serial(N_Vector x, realtype b, N_Vector z)
+{
+ long int i, N;
+ realtype *xd, *zd;
+
+ xd = zd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ zd = NV_DATA_S(z);
+
+ for (i = 0; i < N; i++)
+ zd[i] = xd[i]+b;
+
+ return;
+}
+
+realtype N_VDotProd_Serial(N_Vector x, N_Vector y)
+{
+ long int i, N;
+ realtype sum, *xd, *yd;
+
+ sum = ZERO;
+ xd = yd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ yd = NV_DATA_S(y);
+
+ for (i = 0; i < N; i++)
+ sum += xd[i]*yd[i];
+
+ return(sum);
+}
+
+realtype N_VMaxNorm_Serial(N_Vector x)
+{
+ long int i, N;
+ realtype max, *xd;
+
+ max = ZERO;
+ xd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+
+ for (i = 0; i < N; i++) {
+ if (ABS(xd[i]) > max) max = ABS(xd[i]);
+ }
+
+ return(max);
+}
+
+realtype N_VWrmsNorm_Serial(N_Vector x, N_Vector w)
+{
+ long int i, N;
+ realtype sum, prodi, *xd, *wd;
+
+ sum = ZERO;
+ xd = wd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ wd = NV_DATA_S(w);
+
+ for (i = 0; i < N; i++) {
+ prodi = xd[i]*wd[i];
+ sum += SQR(prodi);
+ }
+
+ return(RSqrt(sum/N));
+}
+
+realtype N_VWrmsNormMask_Serial(N_Vector x, N_Vector w, N_Vector id)
+{
+ long int i, N;
+ realtype sum, prodi, *xd, *wd, *idd;
+
+ sum = ZERO;
+ xd = wd = idd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ wd = NV_DATA_S(w);
+ idd = NV_DATA_S(id);
+
+ for (i = 0; i < N; i++) {
+ if (idd[i] > ZERO) {
+ prodi = xd[i]*wd[i];
+ sum += SQR(prodi);
+ }
+ }
+
+ return(RSqrt(sum / N));
+}
+
+realtype N_VMin_Serial(N_Vector x)
+{
+ long int i, N;
+ realtype min, *xd;
+
+ xd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+
+ min = xd[0];
+
+ for (i = 1; i < N; i++) {
+ if (xd[i] < min) min = xd[i];
+ }
+
+ return(min);
+}
+
+realtype N_VWL2Norm_Serial(N_Vector x, N_Vector w)
+{
+ long int i, N;
+ realtype sum, prodi, *xd, *wd;
+
+ sum = ZERO;
+ xd = wd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ wd = NV_DATA_S(w);
+
+ for (i = 0; i < N; i++) {
+ prodi = xd[i]*wd[i];
+ sum += SQR(prodi);
+ }
+
+ return(RSqrt(sum));
+}
+
+realtype N_VL1Norm_Serial(N_Vector x)
+{
+ long int i, N;
+ realtype sum, *xd;
+
+ sum = ZERO;
+ xd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+
+ for (i = 0; i<N; i++)
+ sum += ABS(xd[i]);
+
+ return(sum);
+}
+
+void N_VCompare_Serial(realtype c, N_Vector x, N_Vector z)
+{
+ long int i, N;
+ realtype *xd, *zd;
+
+ xd = zd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ zd = NV_DATA_S(z);
+
+ for (i = 0; i < N; i++) {
+ zd[i] = (ABS(xd[i]) >= c) ? ONE : ZERO;
+ }
+
+ return;
+}
+
+booleantype N_VInvTest_Serial(N_Vector x, N_Vector z)
+{
+ long int i, N;
+ realtype *xd, *zd;
+
+ xd = zd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ zd = NV_DATA_S(z);
+
+ for (i = 0; i < N; i++) {
+ if (xd[i] == ZERO) return(FALSE);
+ zd[i] = ONE/xd[i];
+ }
+
+ return(TRUE);
+}
+
+booleantype N_VConstrMask_Serial(N_Vector c, N_Vector x, N_Vector m)
+{
+ long int i, N;
+ booleantype test;
+ realtype *cd, *xd, *md;
+
+ cd = xd = md = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ cd = NV_DATA_S(c);
+ md = NV_DATA_S(m);
+
+ test = TRUE;
+
+ for (i = 0; i < N; i++) {
+ md[i] = ZERO;
+ if (cd[i] == ZERO) continue;
+ if (cd[i] > ONEPT5 || cd[i] < -ONEPT5) {
+ if ( xd[i]*cd[i] <= ZERO) { test = FALSE; md[i] = ONE; }
+ continue;
+ }
+ if ( cd[i] > HALF || cd[i] < -HALF) {
+ if (xd[i]*cd[i] < ZERO ) { test = FALSE; md[i] = ONE; }
+ }
+ }
+
+ return(test);
+}
+
+realtype N_VMinQuotient_Serial(N_Vector num, N_Vector denom)
+{
+ booleantype notEvenOnce;
+ long int i, N;
+ realtype *nd, *dd, min;
+
+ nd = dd = NULL;
+
+ N = NV_LENGTH_S(num);
+ nd = NV_DATA_S(num);
+ dd = NV_DATA_S(denom);
+
+ notEvenOnce = TRUE;
+ min = BIG_REAL;
+
+ for (i = 0; i < N; i++) {
+ if (dd[i] == ZERO) continue;
+ else {
+ if (!notEvenOnce) min = MIN(min, nd[i]/dd[i]);
+ else {
+ min = nd[i]/dd[i];
+ notEvenOnce = FALSE;
+ }
+ }
+ }
+
+ return(min);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * private functions
+ * -----------------------------------------------------------------
+ */
+
+static void VCopy_Serial(N_Vector x, N_Vector z)
+{
+ long int i, N;
+ realtype *xd, *zd;
+
+ xd = zd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ zd = NV_DATA_S(z);
+
+ for (i = 0; i < N; i++)
+ zd[i] = xd[i];
+
+ return;
+}
+
+static void VSum_Serial(N_Vector x, N_Vector y, N_Vector z)
+{
+ long int i, N;
+ realtype *xd, *yd, *zd;
+
+ xd = yd = zd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ yd = NV_DATA_S(y);
+ zd = NV_DATA_S(z);
+
+ for (i = 0; i < N; i++)
+ zd[i] = xd[i]+yd[i];
+
+ return;
+}
+
+static void VDiff_Serial(N_Vector x, N_Vector y, N_Vector z)
+{
+ long int i, N;
+ realtype *xd, *yd, *zd;
+
+ xd = yd = zd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ yd = NV_DATA_S(y);
+ zd = NV_DATA_S(z);
+
+ for (i = 0; i < N; i++)
+ zd[i] = xd[i]-yd[i];
+
+ return;
+}
+
+static void VNeg_Serial(N_Vector x, N_Vector z)
+{
+ long int i, N;
+ realtype *xd, *zd;
+
+ xd = zd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ zd = NV_DATA_S(z);
+
+ for (i = 0; i < N; i++)
+ zd[i] = -xd[i];
+
+ return;
+}
+
+static void VScaleSum_Serial(realtype c, N_Vector x, N_Vector y, N_Vector z)
+{
+ long int i, N;
+ realtype *xd, *yd, *zd;
+
+ xd = yd = zd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ yd = NV_DATA_S(y);
+ zd = NV_DATA_S(z);
+
+ for (i = 0; i < N; i++)
+ zd[i] = c*(xd[i]+yd[i]);
+
+ return;
+}
+
+static void VScaleDiff_Serial(realtype c, N_Vector x, N_Vector y, N_Vector z)
+{
+ long int i, N;
+ realtype *xd, *yd, *zd;
+
+ xd = yd = zd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ yd = NV_DATA_S(y);
+ zd = NV_DATA_S(z);
+
+ for (i = 0; i < N; i++)
+ zd[i] = c*(xd[i]-yd[i]);
+
+ return;
+}
+
+static void VLin1_Serial(realtype a, N_Vector x, N_Vector y, N_Vector z)
+{
+ long int i, N;
+ realtype *xd, *yd, *zd;
+
+ xd = yd = zd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ yd = NV_DATA_S(y);
+ zd = NV_DATA_S(z);
+
+ for (i = 0; i < N; i++)
+ zd[i] = (a*xd[i])+yd[i];
+
+ return;
+}
+
+static void VLin2_Serial(realtype a, N_Vector x, N_Vector y, N_Vector z)
+{
+ long int i, N;
+ realtype *xd, *yd, *zd;
+
+ xd = yd = zd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ yd = NV_DATA_S(y);
+ zd = NV_DATA_S(z);
+
+ for (i = 0; i < N; i++)
+ zd[i] = (a*xd[i])-yd[i];
+
+ return;
+}
+
+static void Vaxpy_Serial(realtype a, N_Vector x, N_Vector y)
+{
+ long int i, N;
+ realtype *xd, *yd;
+
+ xd = yd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+ yd = NV_DATA_S(y);
+
+ if (a == ONE) {
+ for (i = 0; i < N; i++)
+ yd[i] += xd[i];
+ return;
+ }
+
+ if (a == -ONE) {
+ for (i = 0; i < N; i++)
+ yd[i] -= xd[i];
+ return;
+ }
+
+ for (i = 0; i < N; i++)
+ yd[i] += a*xd[i];
+
+ return;
+}
+
+static void VScaleBy_Serial(realtype a, N_Vector x)
+{
+ long int i, N;
+ realtype *xd;
+
+ xd = NULL;
+
+ N = NV_LENGTH_S(x);
+ xd = NV_DATA_S(x);
+
+ for (i = 0; i < N; i++)
+ xd[i] *= a;
+
+ return;
+}
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/README.txt b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/README.txt
new file mode 100644
index 0000000..390e158
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/README.txt
@@ -0,0 +1,198 @@
+ SUNDIALS
+ Shared Module
+ Release 2.4.0, *** 2007
+
+
+The family of solvers referred to as SUNDIALS consists of solvers CVODE
+(for ODE systems), CVODES (ODE with sensitivity analysis capabilities),
+IDA (for differential-algebraic systems), and KINSOL (for nonlinear
+algebraic systems),
+
+The various solvers of this family share many subordinate modules contained
+in this module:
+- generic NVECTOR module
+- generic linear solver modules (band, dense, smalldense, spgmr, bcg, tfqmr)
+- definitions of SUNDIALS types (realtype, booleantype)
+- common math functions (RpowerI, RPowerR, RSqrt, RAbs,...)
+
+
+A. Documentation
+----------------
+All shared submodules are fully described in the user documentation for any of
+the SUNDIALS solvers [1,2,3,4]. PostScript and PDF files for the uer guide for
+a particular solver are available in the solver's directory.
+
+
+B. Installation
+---------------
+
+For basic installation instructions see the file /sundials/INSTALL.
+For complete installation instructions see any of the user guides.
+
+
+C. References
+-------------
+
+[1] A. C. Hindmarsh and R. Serban, "User Documentation for CVODE v2.4.0,"
+ LLLNL technical report UCRL-MA-208108, November 2004.
+
+[2] A. C. Hindmarsh and R. Serban, "User Documentation for CVODES v2.4.0,"
+ LLNL technical report UCRL-MA-208111, November 2004.
+
+[3] A. C. Hindmarsh and R. Serban, "User Documentation for IDA v2.4.0,"
+ LLNL technical report UCRL-MA-208112, November 2004.
+
+[4] A. M. Collier, A. C. Hindmarsh, R. Serban,and C. S. Woodward, "User
+ Documentation for KINSOL v2.4.0," LLNL technical report UCRL-MA-208116,
+ November 2004.
+
+
+D. Releases
+-----------
+
+v. 2.4.0 - ***. 2007
+v. 2.3.0 - Nov. 2006
+v. 2.2.0 - Mar. 2006
+v. 2.1.1 - May. 2005
+v. 2.1.0 - Apr. 2005
+v. 2.0.2 - Mar. 2005
+v. 2.0.1 - Jan. 2005
+v. 2.0 - Dec. 2004
+v. 1.0 - Jul. 2002 (first SUNDIALS release)
+v. 0.0 - Mar. 2002
+
+
+E. Revision History
+-------------------
+
+v. 2.3.0 (Nov. 2006) ---> v. 2.4.0 (***. 2007)
+---------------------------------------------------------
+
+- New features
+ - added a new generic linear solver module based on Blas + Lapack
+ for both dense and banded matrices.
+
+- Changes to user interface
+ - common functionality for all direct linear solvers (dense, band, and
+ the new Lapack solver) has been collected into the DLS (Direct Linear
+ Solver) module, implemented in the files sundials_direct.h and
+ sundials_direct.c (similar to the SPILS module for the iterative linear
+ solvers).
+ - in order to include the new Lapack-based linear solver, all dimensions
+ for the above linear solvers (problem sizes, bandwidths,... including
+ the underlying matrix data types) are now of type 'int' (and not 'long int').
+
+
+v. 2.2.0 (Mar. 2006) ---> v. 2.3.0 (Nov. 2006)
+----------------------------------------------
+
+- Changes to the user interface
+ - modified sundials_dense and sundials_smalldense to work with
+ rectangular m by n matrices (m <= n).
+
+- Changes related to the build system
+ - reorganized source tree
+ - exported header files are installed in solver-specific subdirectories
+ of ${includedir}
+ - sundialsTB is distributed only as part of the SUNDIALS tarball
+
+v. 2.1.1 (May 2005) ---> v. 2.2.0 (Mar. 2006)
+----------------------------------------------
+
+- New features
+ - added SPBCG (scaled preconditioned Bi-CGStab) linear solver module
+ - added SPTFQMR (scaled preconditioned TFQMR) linear solver module
+
+- Changes related to the build system
+ - updated configure script and Makefiles for Fortran examples to avoid C++
+ compiler errors (now use CC and MPICC to link only if necessary)
+ - SUNDIALS shared header files are installed under a 'sundials' subdirectory
+ of the install include directory
+ - the shared object files are now linked into each SUNDIALS library rather
+ than into a separate libsundials_shared library
+
+- Changes to the user interface
+ - added prefix 'sundials_' to all shared header files
+
+v. 2.1.0 (Apr. 2005) ---> v. 2.1.1 (May.2005)
+----------------------------------------------
+
+- Changes to data structures
+ - added N_VCloneEmpty to global vector operations table
+
+v. 2.0.2 (Mar. 2005) ---> v. 2.1.0 (Apr. 2005)
+----------------------------------------------
+
+- none
+
+v. 2.0.1 (Jan. 2005) ---> v. 2.0.2 (Mar. 2005)
+----------------------------------------------
+
+- Changes related to the build system
+ - fixed autoconf-related bug to allow configuration with the PGI Fortran compiler
+ - modified to use customized detection of the Fortran name mangling scheme
+ (autoconf's AC_F77_WRAPPERS routine is problematic on some platforms)
+ - added --with-mpi-flags as a configure option to allow user to specify
+ MPI-specific flags
+ - updated Makefiles for Fortran examples to avoid C++ compiler errors (now use
+ CC and MPICC to link)
+
+v. 2.0 (Dec. 2004) ---> v. 2.0.1 (Jan. 2005)
+--------------------------------------------
+
+- Changes related to the build system
+ - changed order of compiler directives in header files to avoid compilation
+ errors when using a C++ compiler.
+
+v. 1.0 (Jul. 2002) ---> v. 2.0 (Dec. 2004)
+------------------------------------------
+
+- Changes to the generic NVECTOR module
+ - removed machEnv, redefined table of vector operations (now contained
+ in the N_Vector structure itself).
+ - all SUNDIALS functions create new N_Vector variables through cloning, using
+ an N_Vector passed by the user as a template.
+ - a particular NVECTOR implementation is supposed to provide user-callable
+ constructor and destructor functions.
+ - removed from structure of vector operations the following functions:
+ N_VNew, N_VNew_S, N_VFree, N_VFree_S, N_VMake, N_VDispose, N_VGetData,
+ N_VSetData, N_VConstrProdPos, and N_VOneMask.
+ - added in structure of vector operations the following functions:
+ N_VClone, N_VDestroy, N_VSpace, N_VGetArrayPointer, N_VSetArrayPointer,
+ and N_VWrmsNormMask.
+ - Note that nvec_ser and nvec_par are now separate modules outside the
+ shared SUNDIALS module.
+
+- Changes to the generic linear solvers
+ - in SPGMR, added a dummy N_Vector argument to be used as a template
+ for cloning.
+ - in SPGMR, removed N (problem dimension) from argument list of SpgmrMalloc.
+ - iterative.{c,h} replace iterativ.{c,h}
+ - modified constant names in iterative.h (preconditioner types are prefixed
+ with 'PREC_').
+ - changed numerical values for MODIFIED_GS (from 0 to 1) and CLASSICAL_GS
+ (from 1 to 2).
+
+- Changes to sundialsmath submodule
+ - replaced internal routine for estimation of unit roundoff with definition
+ of unit roundoff from float.h
+ - modified functions to call appropriate math routines given the precision
+ level specified by the user.
+
+- Changes to sundialstypes submodule
+ - removed type 'integertype'.
+ - added definitions for 'BIG_REAL', 'SMALL_REAL', and 'UNIT_ROUNDOFF' using
+ values from float.h based on the precision.
+ - changed definition of macro RCONST to depend on precision.
+
+v 0.0 (Mar. 2002) ---> v. 1.0 (Jul. 2002)
+-----------------------------------------
+
+20020321 Defined and implemented generic NVECTOR module, and separate serial/
+ parallel NVECTOR modules, including serial/parallel F/C interfaces.
+ Modified dense and band backsolve routines to take real* type for
+ RHS and solution vector.
+20020329 Named the DenseMat, BandMat, and SpgmrMemRec structures.
+20020626 Changed type names to realtype, integertype, booleantype.
+ Renamed llnltypes and llnlmath files.
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_band.c b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_band.c
new file mode 100644
index 0000000..bf417b0
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_band.c
@@ -0,0 +1,259 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.5 $
+ * $Date: 2006/11/22 00:12:51 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Alan C. Hindmarsh and Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for a generic BAND linear
+ * solver package.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <sundials/sundials_band.h>
+#include <sundials/sundials_math.h>
+
+#define ZERO RCONST(0.0)
+#define ONE RCONST(1.0)
+
+#define ROW(i,j,smu) (i-j+smu)
+
+/*
+ * -----------------------------------------------------
+ * Functions working on DlsMat
+ * -----------------------------------------------------
+ */
+
+int BandGBTRF(DlsMat A, int *p)
+{
+ return(bandGBTRF(A->cols, A->M, A->mu, A->ml, A->s_mu, p));
+}
+
+void BandGBTRS(DlsMat A, int *p, realtype *b)
+{
+ bandGBTRS(A->cols, A->M, A->s_mu, A->ml, p, b);
+}
+
+void BandZero(DlsMat A)
+{
+ bandZero(A->cols, A->M, A->mu, A->ml, A->s_mu);
+}
+
+void BandCopy(DlsMat A, DlsMat B, int copymu, int copyml)
+{
+ bandCopy(A->cols, B->cols, A->M, A->s_mu, B->s_mu, copymu, copyml);
+}
+
+void BandScale(realtype c, DlsMat A)
+{
+ bandScale(c, A->cols, A->M, A->mu, A->ml, A->s_mu);
+}
+
+void BandAddI(DlsMat A)
+{
+ bandAddI(A->cols, A->M, A->s_mu);
+}
+
+/*
+ * -----------------------------------------------------
+ * Functions working on realtype**
+ * -----------------------------------------------------
+ */
+
+int bandGBTRF(realtype **a, int n, int mu, int ml, int smu, int *p)
+{
+ int c, r, num_rows;
+ int i, j, k, l, storage_l, storage_k, last_col_k, last_row_k;
+ realtype *a_c, *col_k, *diag_k, *sub_diag_k, *col_j, *kptr, *jptr;
+ realtype max, temp, mult, a_kj;
+ booleantype swap;
+
+ /* zero out the first smu - mu rows of the rectangular array a */
+
+ num_rows = smu - mu;
+ if (num_rows > 0) {
+ for (c=0; c < n; c++) {
+ a_c = a[c];
+ for (r=0; r < num_rows; r++) {
+ a_c[r] = ZERO;
+ }
+ }
+ }
+
+ /* k = elimination step number */
+
+ for (k=0; k < n-1; k++, p++) {
+
+ col_k = a[k];
+ diag_k = col_k + smu;
+ sub_diag_k = diag_k + 1;
+ last_row_k = MIN(n-1,k+ml);
+
+ /* find l = pivot row number */
+
+ l=k;
+ max = ABS(*diag_k);
+ for (i=k+1, kptr=sub_diag_k; i <= last_row_k; i++, kptr++) {
+ if (ABS(*kptr) > max) {
+ l=i;
+ max = ABS(*kptr);
+ }
+ }
+ storage_l = ROW(l, k, smu);
+ *p = l;
+
+ /* check for zero pivot element */
+
+ if (col_k[storage_l] == ZERO) return(k+1);
+
+ /* swap a(l,k) and a(k,k) if necessary */
+
+ if ( (swap = (l != k) )) {
+ temp = col_k[storage_l];
+ col_k[storage_l] = *diag_k;
+ *diag_k = temp;
+ }
+
+ /* Scale the elements below the diagonal in */
+ /* column k by -1.0 / a(k,k). After the above swap, */
+ /* a(k,k) holds the pivot element. This scaling */
+ /* stores the pivot row multipliers -a(i,k)/a(k,k) */
+ /* in a(i,k), i=k+1, ..., MIN(n-1,k+ml). */
+
+ mult = -ONE / (*diag_k);
+ for (i=k+1, kptr = sub_diag_k; i <= last_row_k; i++, kptr++)
+ (*kptr) *= mult;
+
+ /* row_i = row_i - [a(i,k)/a(k,k)] row_k, i=k+1, ..., MIN(n-1,k+ml) */
+ /* row k is the pivot row after swapping with row l. */
+ /* The computation is done one column at a time, */
+ /* column j=k+1, ..., MIN(k+smu,n-1). */
+
+ last_col_k = MIN(k+smu,n-1);
+ for (j=k+1; j <= last_col_k; j++) {
+
+ col_j = a[j];
+ storage_l = ROW(l,j,smu);
+ storage_k = ROW(k,j,smu);
+ a_kj = col_j[storage_l];
+
+ /* Swap the elements a(k,j) and a(k,l) if l!=k. */
+
+ if (swap) {
+ col_j[storage_l] = col_j[storage_k];
+ col_j[storage_k] = a_kj;
+ }
+
+ /* a(i,j) = a(i,j) - [a(i,k)/a(k,k)]*a(k,j) */
+ /* a_kj = a(k,j), *kptr = - a(i,k)/a(k,k), *jptr = a(i,j) */
+
+ if (a_kj != ZERO) {
+ for (i=k+1, kptr=sub_diag_k, jptr=col_j+ROW(k+1,j,smu);
+ i <= last_row_k;
+ i++, kptr++, jptr++)
+ (*jptr) += a_kj * (*kptr);
+ }
+ }
+ }
+
+ /* set the last pivot row to be n-1 and check for a zero pivot */
+
+ *p = n-1;
+ if (a[n-1][smu] == ZERO) return(n);
+
+ /* return 0 to indicate success */
+
+ return(0);
+}
+
+void bandGBTRS(realtype **a, int n, int smu, int ml, int *p, realtype *b)
+{
+ int k, l, i, first_row_k, last_row_k;
+ realtype mult, *diag_k;
+
+ /* Solve Ly = Pb, store solution y in b */
+
+ for (k=0; k < n-1; k++) {
+ l = p[k];
+ mult = b[l];
+ if (l != k) {
+ b[l] = b[k];
+ b[k] = mult;
+ }
+ diag_k = a[k]+smu;
+ last_row_k = MIN(n-1,k+ml);
+ for (i=k+1; i <= last_row_k; i++)
+ b[i] += mult * diag_k[i-k];
+ }
+
+ /* Solve Ux = y, store solution x in b */
+
+ for (k=n-1; k >= 0; k--) {
+ diag_k = a[k]+smu;
+ first_row_k = MAX(0,k-smu);
+ b[k] /= (*diag_k);
+ mult = -b[k];
+ for (i=first_row_k; i <= k-1; i++)
+ b[i] += mult*diag_k[i-k];
+ }
+}
+
+void bandZero(realtype **a, int n, int mu, int ml, int smu)
+{
+ int i, j, colSize;
+ realtype *col_j;
+
+ colSize = mu + ml + 1;
+ for (j=0; j < n; j++) {
+ col_j = a[j]+smu-mu;
+ for (i=0; i < colSize; i++)
+ col_j[i] = ZERO;
+ }
+}
+
+void bandCopy(realtype **a, realtype **b, int n, int a_smu, int b_smu,
+ int copymu, int copyml)
+{
+ int i, j, copySize;
+ realtype *a_col_j, *b_col_j;
+
+ copySize = copymu + copyml + 1;
+
+ for (j=0; j < n; j++) {
+ a_col_j = a[j]+a_smu-copymu;
+ b_col_j = b[j]+b_smu-copymu;
+ for (i=0; i < copySize; i++)
+ b_col_j[i] = a_col_j[i];
+ }
+}
+
+void bandScale(realtype c, realtype **a, int n, int mu, int ml, int smu)
+{
+ int i, j, colSize;
+ realtype *col_j;
+
+ colSize = mu + ml + 1;
+
+ for(j=0; j < n; j++) {
+ col_j = a[j]+smu-mu;
+ for (i=0; i < colSize; i++)
+ col_j[i] *= c;
+ }
+}
+
+void bandAddI(realtype **a, int n, int smu)
+{
+ int j;
+
+ for(j=0; j < n; j++)
+ a[j][smu] += ONE;
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_dense.c b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_dense.c
new file mode 100644
index 0000000..b41bb36
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_dense.c
@@ -0,0 +1,396 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.5 $
+ * $Date: 2006/11/22 00:12:51 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Scott D. Cohen, Alan C. Hindmarsh and
+ * Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for a generic package of dense
+ * matrix operations.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <sundials/sundials_dense.h>
+#include <sundials/sundials_math.h>
+
+#define ZERO RCONST(0.0)
+#define ONE RCONST(1.0)
+#define TWO RCONST(2.0)
+
+/*
+ * -----------------------------------------------------
+ * Functions working on DlsMat
+ * -----------------------------------------------------
+ */
+
+int DenseGETRF(DlsMat A, int *p)
+{
+ return(denseGETRF(A->cols, A->M, A->N, p));
+}
+
+void DenseGETRS(DlsMat A, int *p, realtype *b)
+{
+ denseGETRS(A->cols, A->N, p, b);
+}
+
+int DensePOTRF(DlsMat A)
+{
+ return(densePOTRF(A->cols, A->M));
+}
+
+void DensePOTRS(DlsMat A, realtype *b)
+{
+ densePOTRS(A->cols, A->M, b);
+}
+
+int DenseGEQRF(DlsMat A, realtype *beta, realtype *wrk)
+{
+ return(denseGEQRF(A->cols, A->M, A->N, beta, wrk));
+}
+
+int DenseORMQR(DlsMat A, realtype *beta, realtype *vn, realtype *vm, realtype *wrk)
+{
+ return(denseORMQR(A->cols, A->M, A->N, beta, vn, vm, wrk));
+}
+
+void DenseZero(DlsMat A)
+{
+ denseZero(A->cols, A->M, A->N);
+}
+
+void DenseCopy(DlsMat A, DlsMat B)
+{
+ denseCopy(A->cols, B->cols, A->M, A->N);
+}
+
+void DenseScale(realtype c, DlsMat A)
+{
+ denseScale(c, A->cols, A->M, A->N);
+}
+
+void DenseAddI(DlsMat A)
+{
+ denseAddI(A->cols, A->N);
+}
+
+int denseGETRF(realtype **a, int m, int n, int *p)
+{
+ int i, j, k, l;
+ realtype *col_j, *col_k;
+ realtype temp, mult, a_kj;
+
+ /* k-th elimination step number */
+ for (k=0; k < n; k++) {
+
+ col_k = a[k];
+
+ /* find l = pivot row number */
+ l=k;
+ for (i=k+1; i < m; i++)
+ if (ABS(col_k[i]) > ABS(col_k[l])) l=i;
+ p[k] = l;
+
+ /* check for zero pivot element */
+ if (col_k[l] == ZERO) return(k+1);
+
+ /* swap a(k,1:n) and a(l,1:n) if necessary */
+ if ( l!= k ) {
+ for (i=0; i<n; i++) {
+ temp = a[i][l];
+ a[i][l] = a[i][k];
+ a[i][k] = temp;
+ }
+ }
+
+ /* Scale the elements below the diagonal in
+ * column k by 1.0/a(k,k). After the above swap
+ * a(k,k) holds the pivot element. This scaling
+ * stores the pivot row multipliers a(i,k)/a(k,k)
+ * in a(i,k), i=k+1, ..., m-1.
+ */
+ mult = ONE/col_k[k];
+ for(i=k+1; i < m; i++) col_k[i] *= mult;
+
+ /* row_i = row_i - [a(i,k)/a(k,k)] row_k, i=k+1, ..., m-1 */
+ /* row k is the pivot row after swapping with row l. */
+ /* The computation is done one column at a time, */
+ /* column j=k+1, ..., n-1. */
+
+ for (j=k+1; j < n; j++) {
+
+ col_j = a[j];
+ a_kj = col_j[k];
+
+ /* a(i,j) = a(i,j) - [a(i,k)/a(k,k)]*a(k,j) */
+ /* a_kj = a(k,j), col_k[i] = - a(i,k)/a(k,k) */
+
+ if (a_kj != ZERO) {
+ for (i=k+1; i < m; i++)
+ col_j[i] -= a_kj * col_k[i];
+ }
+ }
+ }
+
+ /* return 0 to indicate success */
+
+ return(0);
+}
+
+void denseGETRS(realtype **a, int n, int *p, realtype *b)
+{
+ int i, k, pk;
+ realtype *col_k, tmp;
+
+ /* Permute b, based on pivot information in p */
+ for (k=0; k<n; k++) {
+ pk = p[k];
+ if(pk != k) {
+ tmp = b[k];
+ b[k] = b[pk];
+ b[pk] = tmp;
+ }
+ }
+
+ /* Solve Ly = b, store solution y in b */
+ for (k=0; k<n-1; k++) {
+ col_k = a[k];
+ for (i=k+1; i<n; i++) b[i] -= col_k[i]*b[k];
+ }
+
+ /* Solve Ux = y, store solution x in b */
+ for (k = n-1; k > 0; k--) {
+ col_k = a[k];
+ b[k] /= col_k[k];
+ for (i=0; i<k; i++) b[i] -= col_k[i]*b[k];
+ }
+ b[0] /= a[0][0];
+
+}
+
+/*
+ * Cholesky decomposition of a symmetric positive-definite matrix
+ * A = C^T*C: gaxpy version.
+ * Only the lower triangle of A is accessed and it is overwritten with
+ * the lower triangle of C.
+ */
+int densePOTRF(realtype **a, int m)
+{
+ realtype *a_col_j, *a_col_k;
+ realtype a_diag;
+ int i, j, k;
+
+ for (j=0; j<m; j++) {
+
+ a_col_j = a[j];
+
+ if (j>0) {
+ for(i=j; i<m; i++) {
+ for(k=0;k<j;k++) {
+ a_col_k = a[k];
+ a_col_j[i] -= a_col_k[i]*a_col_k[j];
+ }
+ }
+ }
+
+ a_diag = a_col_j[j];
+ if (a_diag <= ZERO) return(j);
+ a_diag = RSqrt(a_diag);
+
+ for(i=j; i<m; i++) a_col_j[i] /= a_diag;
+
+ }
+
+ return(0);
+}
+
+/*
+ * Solution of Ax=b, with A s.p.d., based on the Cholesky decomposition
+ * obtained with denPOTRF.; A = C*C^T, C lower triangular
+ *
+ */
+void densePOTRS(realtype **a, int m, realtype *b)
+{
+ realtype *col_j, *col_i;
+ int i, j;
+
+ /* Solve C y = b, forward substitution - column version.
+ Store solution y in b */
+ for (j=0; j < m-1; j++) {
+ col_j = a[j];
+ b[j] /= col_j[j];
+ for (i=j+1; i < m; i++)
+ b[i] -= b[j]*col_j[i];
+ }
+ col_j = a[m-1];
+ b[m-1] /= col_j[m-1];
+
+ /* Solve C^T x = y, backward substitution - row version.
+ Store solution x in b */
+ col_j = a[m-1];
+ b[m-1] /= col_j[m-1];
+ for (i=m-2; i>=0; i--) {
+ col_i = a[i];
+ for (j=i+1; j<m; j++)
+ b[i] -= col_i[j]*b[j];
+ b[i] /= col_i[i];
+ }
+
+}
+
+/*
+ * QR factorization of a rectangular matrix A of size m by n (m >= n)
+ * using Householder reflections.
+ *
+ * On exit, the elements on and above the diagonal of A contain the n by n
+ * upper triangular matrix R; the elements below the diagonal, with the array beta,
+ * represent the orthogonal matrix Q as a product of elementary reflectors .
+ *
+ * v (of length m) must be provided as workspace.
+ *
+ */
+
+int denseGEQRF(realtype **a, int m, int n, realtype *beta, realtype *v)
+{
+ realtype ajj, s, mu, v1, v1_2;
+ realtype *col_j, *col_k;
+ int i, j, k;
+
+ /* For each column...*/
+ for(j=0; j<n; j++) {
+
+ col_j = a[j];
+
+ ajj = col_j[j];
+
+ /* Compute the j-th Householder vector (of length m-j) */
+ v[0] = ONE;
+ s = ZERO;
+ for(i=1; i<m-j; i++) {
+ v[i] = col_j[i+j];
+ s += v[i]*v[i];
+ }
+
+ if(s != ZERO) {
+ mu = RSqrt(ajj*ajj+s);
+ v1 = (ajj <= ZERO) ? ajj-mu : -s/(ajj+mu);
+ v1_2 = v1*v1;
+ beta[j] = TWO * v1_2 / (s + v1_2);
+ for(i=1; i<m-j; i++) v[i] /= v1;
+ } else {
+ beta[j] = ZERO;
+ }
+
+ /* Update upper triangle of A (load R) */
+ for(k=j; k<n; k++) {
+ col_k = a[k];
+ s = ZERO;
+ for(i=0; i<m-j; i++) s += col_k[i+j]*v[i];
+ s *= beta[j];
+ for(i=0; i<m-j; i++) col_k[i+j] -= s*v[i];
+ }
+
+ /* Update A (load Householder vector) */
+ if(j<m-1) {
+ for(i=1; i<m-j; i++) col_j[i+j] = v[i];
+ }
+
+ }
+
+
+ return(0);
+}
+
+/*
+ * Computes vm = Q * vn, where the orthogonal matrix Q is stored as
+ * elementary reflectors in the m by n matrix A and in the vector beta.
+ * (NOTE: It is assumed that an QR factorization has been previously
+ * computed with denGEQRF).
+ *
+ * vn (IN) has length n, vm (OUT) has length m, and it's assumed that m >= n.
+ *
+ * v (of length m) must be provided as workspace.
+ */
+int denseORMQR(realtype **a, int m, int n, realtype *beta,
+ realtype *vn, realtype *vm, realtype *v)
+{
+ realtype *col_j, s;
+ int i, j;
+
+ /* Initialize vm */
+ for(i=0; i<n; i++) vm[i] = vn[i];
+ for(i=n; i<m; i++) vm[i] = ZERO;
+
+ /* Accumulate (backwards) corrections into vm */
+ for(j=n-1; j>=0; j--) {
+
+ col_j = a[j];
+
+ v[0] = ONE;
+ s = vm[j];
+ for(i=1; i<m-j; i++) {
+ v[i] = col_j[i+j];
+ s += v[i]*vm[i+j];
+ }
+ s *= beta[j];
+
+ for(i=0; i<m-j; i++) vm[i+j] -= s * v[i];
+
+ }
+
+ return(0);
+}
+
+void denseZero(realtype **a, int m, int n)
+{
+ int i, j;
+ realtype *col_j;
+
+ for (j=0; j < n; j++) {
+ col_j = a[j];
+ for (i=0; i < m; i++)
+ col_j[i] = ZERO;
+ }
+}
+
+void denseCopy(realtype **a, realtype **b, int m, int n)
+{
+ int i, j;
+ realtype *a_col_j, *b_col_j;
+
+ for (j=0; j < n; j++) {
+ a_col_j = a[j];
+ b_col_j = b[j];
+ for (i=0; i < m; i++)
+ b_col_j[i] = a_col_j[i];
+ }
+
+}
+
+void denseScale(realtype c, realtype **a, int m, int n)
+{
+ int i, j;
+ realtype *col_j;
+
+ for (j=0; j < n; j++) {
+ col_j = a[j];
+ for (i=0; i < m; i++)
+ col_j[i] *= c;
+ }
+}
+
+void denseAddI(realtype **a, int n)
+{
+ int i;
+
+ for (i=0; i < n; i++) a[i][i] += ONE;
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_direct.c b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_direct.c
new file mode 100644
index 0000000..3b59a56
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_direct.c
@@ -0,0 +1,275 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2006/11/29 00:05:10 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for operations to be used by a
+ * generic direct linear solver.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <sundials/sundials_direct.h>
+#include <sundials/sundials_math.h>
+
+DlsMat NewDenseMat(int M, int N)
+{
+ DlsMat A;
+ int j;
+
+ if ( (M <= 0) || (N <= 0) ) return(NULL);
+
+ A = NULL;
+ A = (DlsMat) malloc(sizeof *A);
+ if (A==NULL) return (NULL);
+
+ A->data = (realtype *) malloc(M * N * sizeof(realtype));
+ if (A->data == NULL) {
+ free(A); A = NULL;
+ return(NULL);
+ }
+ A->cols = (realtype **) malloc(N * sizeof(realtype *));
+ if (A->cols == NULL) {
+ free(A->data); A->data = NULL;
+ free(A); A = NULL;
+ return(NULL);
+ }
+
+ for (j=0; j < N; j++) A->cols[j] = A->data + j * M;
+
+ A->M = M;
+ A->N = N;
+ A->ldim = M;
+ A->ldata = M*N;
+
+ A->type = SUNDIALS_DENSE;
+
+ return(A);
+}
+
+realtype **newDenseMat(int m, int n)
+{
+ int j;
+ realtype **a;
+
+ if ( (n <= 0) || (m <= 0) ) return(NULL);
+
+ a = NULL;
+ a = (realtype **) malloc(n * sizeof(realtype *));
+ if (a == NULL) return(NULL);
+
+ a[0] = NULL;
+ a[0] = (realtype *) malloc(m * n * sizeof(realtype));
+ if (a[0] == NULL) {
+ free(a); a = NULL;
+ return(NULL);
+ }
+
+ for (j=1; j < n; j++) a[j] = a[0] + j * m;
+
+ return(a);
+}
+
+
+DlsMat NewBandMat(int N, int mu, int ml, int smu)
+{
+ DlsMat A;
+ int j, colSize;
+
+ if (N <= 0) return(NULL);
+
+ A = NULL;
+ A = (DlsMat) malloc(sizeof *A);
+ if (A == NULL) return (NULL);
+
+ colSize = smu + ml + 1;
+ A->data = NULL;
+ A->data = (realtype *) malloc(N * colSize * sizeof(realtype));
+ if (A->data == NULL) {
+ free(A); A = NULL;
+ return(NULL);
+ }
+
+ A->cols = NULL;
+ A->cols = (realtype **) malloc(N * sizeof(realtype *));
+ if (A->cols == NULL) {
+ free(A->data);
+ free(A); A = NULL;
+ return(NULL);
+ }
+
+ for (j=0; j < N; j++) A->cols[j] = A->data + j * colSize;
+
+ A->M = N;
+ A->N = N;
+ A->mu = mu;
+ A->ml = ml;
+ A->s_mu = smu;
+ A->ldim = colSize;
+ A->ldata = N * colSize;
+
+ A->type = SUNDIALS_BAND;
+
+ return(A);
+}
+
+realtype **newBandMat(int n, int smu, int ml)
+{
+ realtype **a;
+ int j, colSize;
+
+ if (n <= 0) return(NULL);
+
+ a = NULL;
+ a = (realtype **) malloc(n * sizeof(realtype *));
+ if (a == NULL) return(NULL);
+
+ colSize = smu + ml + 1;
+ a[0] = NULL;
+ a[0] = (realtype *) malloc(n * colSize * sizeof(realtype));
+ if (a[0] == NULL) {
+ free(a); a = NULL;
+ return(NULL);
+ }
+
+ for (j=1; j < n; j++) a[j] = a[0] + j * colSize;
+
+ return(a);
+}
+
+void DestroyMat(DlsMat A)
+{
+ free(A->data); A->data = NULL;
+ free(A->cols);
+ free(A); A = NULL;
+}
+
+void destroyMat(realtype **a)
+{
+ free(a[0]); a[0] = NULL;
+ free(a); a = NULL;
+}
+
+int *NewIntArray(int N)
+{
+ int *vec;
+
+ if (N <= 0) return(NULL);
+
+ vec = NULL;
+ vec = (int *) malloc(N * sizeof(int));
+
+ return(vec);
+}
+
+int *newIntArray(int n)
+{
+ int *v;
+
+ if (n <= 0) return(NULL);
+
+ v = NULL;
+ v = (int *) malloc(n * sizeof(int));
+
+ return(v);
+}
+
+realtype *NewRealArray(int N)
+{
+ realtype *vec;
+
+ if (N <= 0) return(NULL);
+
+ vec = NULL;
+ vec = (realtype *) malloc(N * sizeof(realtype));
+
+ return(vec);
+}
+
+realtype *newRealArray(int m)
+{
+ realtype *v;
+
+ if (m <= 0) return(NULL);
+
+ v = NULL;
+ v = (realtype *) malloc(m * sizeof(realtype));
+
+ return(v);
+}
+
+void DestroyArray(void *V)
+{
+ free(V);
+ V = NULL;
+}
+
+void destroyArray(void *v)
+{
+ free(v);
+ v = NULL;
+}
+
+void PrintMat(DlsMat A)
+{
+ int i, j, start, finish;
+ realtype **a;
+
+ switch (A->type) {
+
+ case SUNDIALS_DENSE:
+
+ printf("\n");
+ for (i=0; i < A->M; i++) {
+ for (j=0; j < A->N; j++) {
+#if defined(SUNDIALS_EXTENDED_PRECISION)
+ printf("%12Lg ", DENSE_ELEM(A,i,j));
+#elif defined(SUNDIALS_DOUBLE_PRECISION)
+ printf("%12lg ", DENSE_ELEM(A,i,j));
+#else
+ printf("%12g ", DENSE_ELEM(A,i,j));
+#endif
+ }
+ printf("\n");
+ }
+ printf("\n");
+
+ break;
+
+ case SUNDIALS_BAND:
+
+ a = A->cols;
+ printf("\n");
+ for (i=0; i < A->N; i++) {
+ start = MAX(0,i-A->ml);
+ finish = MIN(A->N-1,i+A->mu);
+ for (j=0; j < start; j++) printf("%12s ","");
+ for (j=start; j <= finish; j++) {
+#if defined(SUNDIALS_EXTENDED_PRECISION)
+ printf("%12Lg ", a[j][i-j+A->s_mu]);
+#elif defined(SUNDIALS_DOUBLE_PRECISION)
+ printf("%12lg ", a[j][i-j+A->s_mu]);
+#else
+ printf("%12g ", a[j][i-j+A->s_mu]);
+#endif
+ }
+ printf("\n");
+ }
+ printf("\n");
+
+ break;
+
+ }
+
+}
+
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_iterative.c b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_iterative.c
new file mode 100644
index 0000000..41ccc17
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_iterative.c
@@ -0,0 +1,288 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.1 $
+ * $Date: 2006/07/05 15:32:38 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Scott D. Cohen, Alan C. Hindmarsh and
+ * Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for the iterative.h header
+ * file. It contains the implementation of functions that may be
+ * useful for many different iterative solvers of A x = b.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+
+#include <sundials/sundials_iterative.h>
+#include <sundials/sundials_math.h>
+
+#define FACTOR RCONST(1000.0)
+#define ZERO RCONST(0.0)
+#define ONE RCONST(1.0)
+
+/*
+ * -----------------------------------------------------------------
+ * Function : ModifiedGS
+ * -----------------------------------------------------------------
+ * This implementation of ModifiedGS is a slight modification of a
+ * previous modified Gram-Schmidt routine (called mgs) written by
+ * Milo Dorr.
+ * -----------------------------------------------------------------
+ */
+
+int ModifiedGS(N_Vector *v, realtype **h, int k, int p,
+ realtype *new_vk_norm)
+{
+ int i, k_minus_1, i0;
+ realtype new_norm_2, new_product, vk_norm, temp;
+
+ vk_norm = RSqrt(N_VDotProd(v[k],v[k]));
+ k_minus_1 = k - 1;
+ i0 = MAX(k-p, 0);
+
+ /* Perform modified Gram-Schmidt */
+
+ for (i=i0; i < k; i++) {
+ h[i][k_minus_1] = N_VDotProd(v[i], v[k]);
+ N_VLinearSum(ONE, v[k], -h[i][k_minus_1], v[i], v[k]);
+ }
+
+ /* Compute the norm of the new vector at v[k] */
+
+ *new_vk_norm = RSqrt(N_VDotProd(v[k], v[k]));
+
+ /* If the norm of the new vector at v[k] is less than
+ FACTOR (== 1000) times unit roundoff times the norm of the
+ input vector v[k], then the vector will be reorthogonalized
+ in order to ensure that nonorthogonality is not being masked
+ by a very small vector length. */
+
+ temp = FACTOR * vk_norm;
+ if ((temp + (*new_vk_norm)) != temp) return(0);
+
+ new_norm_2 = ZERO;
+
+ for (i=i0; i < k; i++) {
+ new_product = N_VDotProd(v[i], v[k]);
+ temp = FACTOR * h[i][k_minus_1];
+ if ((temp + new_product) == temp) continue;
+ h[i][k_minus_1] += new_product;
+ N_VLinearSum(ONE, v[k],-new_product, v[i], v[k]);
+ new_norm_2 += SQR(new_product);
+ }
+
+ if (new_norm_2 != ZERO) {
+ new_product = SQR(*new_vk_norm) - new_norm_2;
+ *new_vk_norm = (new_product > ZERO) ? RSqrt(new_product) : ZERO;
+ }
+
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Function : ClassicalGS
+ * -----------------------------------------------------------------
+ * This implementation of ClassicalGS was contributed by Homer Walker
+ * and Peter Brown.
+ * -----------------------------------------------------------------
+ */
+
+int ClassicalGS(N_Vector *v, realtype **h, int k, int p,
+ realtype *new_vk_norm, N_Vector temp, realtype *s)
+{
+ int i, k_minus_1, i0;
+ realtype vk_norm;
+
+ k_minus_1 = k - 1;
+
+ /* Perform Classical Gram-Schmidt */
+
+ vk_norm = RSqrt(N_VDotProd(v[k], v[k]));
+
+ i0 = MAX(k-p, 0);
+ for (i=i0; i < k; i++) {
+ h[i][k_minus_1] = N_VDotProd(v[i], v[k]);
+ }
+
+ for (i=i0; i < k; i++) {
+ N_VLinearSum(ONE, v[k], -h[i][k_minus_1], v[i], v[k]);
+ }
+
+ /* Compute the norm of the new vector at v[k] */
+
+ *new_vk_norm = RSqrt(N_VDotProd(v[k], v[k]));
+
+ /* Reorthogonalize if necessary */
+
+ if ((FACTOR * (*new_vk_norm)) < vk_norm) {
+
+ for (i=i0; i < k; i++) {
+ s[i] = N_VDotProd(v[i], v[k]);
+ }
+
+ if (i0 < k) {
+ N_VScale(s[i0], v[i0], temp);
+ h[i0][k_minus_1] += s[i0];
+ }
+ for (i=i0+1; i < k; i++) {
+ N_VLinearSum(s[i], v[i], ONE, temp, temp);
+ h[i][k_minus_1] += s[i];
+ }
+ N_VLinearSum(ONE, v[k], -ONE, temp, v[k]);
+
+ *new_vk_norm = RSqrt(N_VDotProd(v[k],v[k]));
+ }
+
+ return(0);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Function : QRfact
+ * -----------------------------------------------------------------
+ * This implementation of QRfact is a slight modification of a
+ * previous routine (called qrfact) written by Milo Dorr.
+ * -----------------------------------------------------------------
+ */
+
+int QRfact(int n, realtype **h, realtype *q, int job)
+{
+ realtype c, s, temp1, temp2, temp3;
+ int i, j, k, q_ptr, n_minus_1, code=0;
+
+ switch (job) {
+ case 0:
+
+ /* Compute a new factorization of H */
+
+ code = 0;
+ for (k=0; k < n; k++) {
+
+ /* Multiply column k by the previous k-1 Givens rotations */
+
+ for (j=0; j < k-1; j++) {
+ i = 2*j;
+ temp1 = h[j][k];
+ temp2 = h[j+1][k];
+ c = q[i];
+ s = q[i+1];
+ h[j][k] = c*temp1 - s*temp2;
+ h[j+1][k] = s*temp1 + c*temp2;
+ }
+
+ /* Compute the Givens rotation components c and s */
+
+ q_ptr = 2*k;
+ temp1 = h[k][k];
+ temp2 = h[k+1][k];
+ if( temp2 == ZERO) {
+ c = ONE;
+ s = ZERO;
+ } else if (ABS(temp2) >= ABS(temp1)) {
+ temp3 = temp1/temp2;
+ s = -ONE/RSqrt(ONE+SQR(temp3));
+ c = -s*temp3;
+ } else {
+ temp3 = temp2/temp1;
+ c = ONE/RSqrt(ONE+SQR(temp3));
+ s = -c*temp3;
+ }
+ q[q_ptr] = c;
+ q[q_ptr+1] = s;
+ if( (h[k][k] = c*temp1 - s*temp2) == ZERO) code = k+1;
+ }
+ break;
+
+ default:
+
+ /* Update the factored H to which a new column has been added */
+
+ n_minus_1 = n - 1;
+ code = 0;
+
+ /* Multiply the new column by the previous n-1 Givens rotations */
+
+ for (k=0; k < n_minus_1; k++) {
+ i = 2*k;
+ temp1 = h[k][n_minus_1];
+ temp2 = h[k+1][n_minus_1];
+ c = q[i];
+ s = q[i+1];
+ h[k][n_minus_1] = c*temp1 - s*temp2;
+ h[k+1][n_minus_1] = s*temp1 + c*temp2;
+ }
+
+ /* Compute new Givens rotation and multiply it times the last two
+ entries in the new column of H. Note that the second entry of
+ this product will be 0, so it is not necessary to compute it. */
+
+ temp1 = h[n_minus_1][n_minus_1];
+ temp2 = h[n][n_minus_1];
+ if (temp2 == ZERO) {
+ c = ONE;
+ s = ZERO;
+ } else if (ABS(temp2) >= ABS(temp1)) {
+ temp3 = temp1/temp2;
+ s = -ONE/RSqrt(ONE+SQR(temp3));
+ c = -s*temp3;
+ } else {
+ temp3 = temp2/temp1;
+ c = ONE/RSqrt(ONE+SQR(temp3));
+ s = -c*temp3;
+ }
+ q_ptr = 2*n_minus_1;
+ q[q_ptr] = c;
+ q[q_ptr+1] = s;
+ if ((h[n_minus_1][n_minus_1] = c*temp1 - s*temp2) == ZERO)
+ code = n;
+ }
+
+ return (code);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Function : QRsol
+ * -----------------------------------------------------------------
+ * This implementation of QRsol is a slight modification of a
+ * previous routine (called qrsol) written by Milo Dorr.
+ * -----------------------------------------------------------------
+ */
+
+int QRsol(int n, realtype **h, realtype *q, realtype *b)
+{
+ realtype c, s, temp1, temp2;
+ int i, k, q_ptr, code=0;
+
+ /* Compute Q*b */
+
+ for (k=0; k < n; k++) {
+ q_ptr = 2*k;
+ c = q[q_ptr];
+ s = q[q_ptr+1];
+ temp1 = b[k];
+ temp2 = b[k+1];
+ b[k] = c*temp1 - s*temp2;
+ b[k+1] = s*temp1 + c*temp2;
+ }
+
+ /* Solve R*x = Q*b */
+
+ for (k=n-1; k >= 0; k--) {
+ if (h[k][k] == ZERO) {
+ code = k + 1;
+ break;
+ }
+ b[k] /= h[k][k];
+ for (i=0; i < k; i++) b[i] -= b[k]*h[i][k];
+ }
+
+ return (code);
+}
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_lapack.c b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_lapack.c
new file mode 100644
index 0000000..eb5ca72
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_lapack.c
@@ -0,0 +1,69 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.3 $
+ * $Date: 2006/11/22 00:12:51 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for a generic package of dense
+ * matrix operations based on BLAS/LAPACK.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+
+#include <sundials/sundials_lapack.h>
+#include <sundials/sundials_math.h>
+
+#define ZERO RCONST(0.0)
+#define ONE RCONST(1.0)
+
+
+/*
+ * LapackDenseZero sets all elements of the dense matrix A to 0
+ */
+void LapackDenseZero(DlsMat A)
+{
+ int i;
+ for (i=0; i<A->ldata; i++) A->data[i] = ZERO;
+}
+
+/*
+ * LapackBandZero sets all elements of the band matrix A to 0
+ */
+void LapackBandZero(DlsMat A)
+{
+ int i;
+ for (i=0; i<A->ldata; i++) A->data[i] = ZERO;
+}
+
+/*
+ * LapackDenseAddI overwrites the dense matrix A with I+A
+ */
+void LapackDenseAddI(DlsMat A)
+{
+ int j;
+ realtype *col_j;
+ for (j=0; j<A->N; j++) {
+ col_j = A->cols[j];
+ col_j[j] += ONE;
+ }
+}
+
+/*
+ * LapackBandAddI overwrites the banded matrix A with I+A
+ */
+void LapackBandAddI(DlsMat A)
+{
+ int j;
+ realtype *col_j;
+ for (j=0; j<A->N; j++) {
+ col_j = (A->cols)[j] + A->s_mu;
+ col_j[0] += ONE;
+ }
+}
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_math.c b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_math.c
new file mode 100644
index 0000000..8bc9d59
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_math.c
@@ -0,0 +1,94 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.1 $
+ * $Date: 2006/07/05 15:32:38 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Scott D. Cohen, Alan C. Hindmarsh and
+ * Aaron Collier @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for a simple C-language math
+ * library.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <sundials/sundials_math.h>
+
+#define ZERO RCONST(0.0)
+#define ONE RCONST(1.0)
+
+realtype RPowerI(realtype base, int exponent)
+{
+ int i, expt;
+ realtype prod;
+
+ prod = ONE;
+ expt = abs(exponent);
+ for(i = 1; i <= expt; i++) prod *= base;
+ if (exponent < 0) prod = ONE/prod;
+ return(prod);
+}
+
+realtype RPowerR(realtype base, realtype exponent)
+{
+ if (base <= ZERO) return(ZERO);
+
+#if defined(SUNDIALS_USE_GENERIC_MATH)
+ return((realtype) pow((double) base, (double) exponent));
+#elif defined(SUNDIALS_DOUBLE_PRECISION)
+ return(pow(base, exponent));
+#elif defined(SUNDIALS_SINGLE_PRECISION)
+ return(powf(base, exponent));
+#elif defined(SUNDIALS_EXTENDED_PRECISION)
+ return(powl(base, exponent));
+#endif
+}
+
+realtype RSqrt(realtype x)
+{
+ if (x <= ZERO) return(ZERO);
+
+#if defined(SUNDIALS_USE_GENERIC_MATH)
+ return((realtype) sqrt((double) x));
+#elif defined(SUNDIALS_DOUBLE_PRECISION)
+ return(sqrt(x));
+#elif defined(SUNDIALS_SINGLE_PRECISION)
+ return(sqrtf(x));
+#elif defined(SUNDIALS_EXTENDED_PRECISION)
+ return(sqrtl(x));
+#endif
+}
+
+realtype RAbs(realtype x)
+{
+#if defined(SUNDIALS_USE_GENERIC_MATH)
+ return((realtype) fabs((double) x));
+#elif defined(SUNDIALS_DOUBLE_PRECISION)
+ return(fabs(x));
+#elif defined(SUNDIALS_SINGLE_PRECISION)
+ return(fabsf(x));
+#elif defined(SUNDIALS_EXTENDED_PRECISION)
+ return(fabsl(x));
+#endif
+}
+
+realtype RExp(realtype x)
+{
+#if defined(SUNDIALS_USE_GENERIC_MATH)
+ return((realtype) exp((double) x));
+#elif defined(SUNDIALS_DOUBLE_PRECISION)
+ return(exp(x));
+#elif defined(SUNDIALS_SINGLE_PRECISION)
+ return(expf(x));
+#elif defined(SUNDIALS_EXTENDED_PRECISION)
+ return(expl(x));
+#endif
+}
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_nvector.c b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_nvector.c
new file mode 100644
index 0000000..e8e1b83
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_nvector.c
@@ -0,0 +1,233 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.3 $
+ * $Date: 2007/04/06 20:33:30 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Radu Serban and Aaron Collier @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for a generic NVECTOR package.
+ * It contains the implementation of the N_Vector operations listed
+ * in nvector.h.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdlib.h>
+
+#include <sundials/sundials_nvector.h>
+
+/*
+ * -----------------------------------------------------------------
+ * Functions in the 'ops' structure
+ * -----------------------------------------------------------------
+ */
+
+N_Vector N_VClone(N_Vector w)
+{
+ N_Vector v = NULL;
+ v = w->ops->nvclone(w);
+ return(v);
+}
+
+N_Vector N_VCloneEmpty(N_Vector w)
+{
+ N_Vector v = NULL;
+ v = w->ops->nvcloneempty(w);
+ return(v);
+}
+
+void N_VDestroy(N_Vector v)
+{
+ if (v==NULL) return;
+ v->ops->nvdestroy(v);
+ return;
+}
+
+void N_VSpace(N_Vector v, long int *lrw, long int *liw)
+{
+ v->ops->nvspace(v, lrw, liw);
+ return;
+}
+
+realtype *N_VGetArrayPointer(N_Vector v)
+{
+ return((realtype *) v->ops->nvgetarraypointer(v));
+}
+
+void N_VSetArrayPointer(realtype *v_data, N_Vector v)
+{
+ v->ops->nvsetarraypointer(v_data, v);
+ return;
+}
+
+void N_VLinearSum(realtype a, N_Vector x, realtype b, N_Vector y, N_Vector z)
+{
+ z->ops->nvlinearsum(a, x, b, y, z);
+ return;
+}
+
+void N_VConst(realtype c, N_Vector z)
+{
+ z->ops->nvconst(c, z);
+ return;
+}
+
+void N_VProd(N_Vector x, N_Vector y, N_Vector z)
+{
+ z->ops->nvprod(x, y, z);
+ return;
+}
+
+void N_VDiv(N_Vector x, N_Vector y, N_Vector z)
+{
+ z->ops->nvdiv(x, y, z);
+ return;
+}
+
+void N_VScale(realtype c, N_Vector x, N_Vector z)
+{
+ z->ops->nvscale(c, x, z);
+ return;
+}
+
+void N_VAbs(N_Vector x, N_Vector z)
+{
+ z->ops->nvabs(x, z);
+ return;
+}
+
+void N_VInv(N_Vector x, N_Vector z)
+{
+ z->ops->nvinv(x, z);
+ return;
+}
+
+void N_VAddConst(N_Vector x, realtype b, N_Vector z)
+{
+ z->ops->nvaddconst(x, b, z);
+ return;
+}
+
+realtype N_VDotProd(N_Vector x, N_Vector y)
+{
+ return((realtype) y->ops->nvdotprod(x, y));
+}
+
+realtype N_VMaxNorm(N_Vector x)
+{
+ return((realtype) x->ops->nvmaxnorm(x));
+}
+
+realtype N_VWrmsNorm(N_Vector x, N_Vector w)
+{
+ return((realtype) x->ops->nvwrmsnorm(x, w));
+}
+
+realtype N_VWrmsNormMask(N_Vector x, N_Vector w, N_Vector id)
+{
+ return((realtype) x->ops->nvwrmsnormmask(x, w, id));
+}
+
+realtype N_VMin(N_Vector x)
+{
+ return((realtype) x->ops->nvmin(x));
+}
+
+realtype N_VWL2Norm(N_Vector x, N_Vector w)
+{
+ return((realtype) x->ops->nvwl2norm(x, w));
+}
+
+realtype N_VL1Norm(N_Vector x)
+{
+ return((realtype) x->ops->nvl1norm(x));
+}
+
+void N_VCompare(realtype c, N_Vector x, N_Vector z)
+{
+ z->ops->nvcompare(c, x, z);
+ return;
+}
+
+booleantype N_VInvTest(N_Vector x, N_Vector z)
+{
+ return((booleantype) z->ops->nvinvtest(x, z));
+}
+
+booleantype N_VConstrMask(N_Vector c, N_Vector x, N_Vector m)
+{
+ return((booleantype) x->ops->nvconstrmask(c, x, m));
+}
+
+realtype N_VMinQuotient(N_Vector num, N_Vector denom)
+{
+ return((realtype) num->ops->nvminquotient(num, denom));
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Additional functions exported by the generic NVECTOR:
+ * N_VCloneEmptyVectorArray
+ * N_VCloneVectorArray
+ * N_VDestroyVectorArray
+ * -----------------------------------------------------------------
+ */
+
+N_Vector *N_VCloneEmptyVectorArray(int count, N_Vector w)
+{
+ N_Vector *vs = NULL;
+ int j;
+
+ if (count <= 0) return(NULL);
+
+ vs = (N_Vector *) malloc(count * sizeof(N_Vector));
+ if(vs == NULL) return(NULL);
+
+ for (j = 0; j < count; j++) {
+ vs[j] = N_VCloneEmpty(w);
+ if (vs[j] == NULL) {
+ N_VDestroyVectorArray(vs, j-1);
+ return(NULL);
+ }
+ }
+
+ return(vs);
+}
+
+N_Vector *N_VCloneVectorArray(int count, N_Vector w)
+{
+ N_Vector *vs = NULL;
+ int j;
+
+ if (count <= 0) return(NULL);
+
+ vs = (N_Vector *) malloc(count * sizeof(N_Vector));
+ if(vs == NULL) return(NULL);
+
+ for (j = 0; j < count; j++) {
+ vs[j] = N_VClone(w);
+ if (vs[j] == NULL) {
+ N_VDestroyVectorArray(vs, j-1);
+ return(NULL);
+ }
+ }
+
+ return(vs);
+}
+
+void N_VDestroyVectorArray(N_Vector *vs, int count)
+{
+ int j;
+
+ if (vs==NULL) return;
+
+ for (j = 0; j < count; j++) N_VDestroy(vs[j]);
+
+ free(vs); vs = NULL;
+
+ return;
+}
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_spbcgs.c b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_spbcgs.c
new file mode 100644
index 0000000..b73bf26
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_spbcgs.c
@@ -0,0 +1,379 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2007/04/06 20:33:30 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Peter Brown and Aaron Collier @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2004, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for the scaled, preconditioned
+ * Bi-CGSTAB (SPBCG) iterative linear solver.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <sundials/sundials_spbcgs.h>
+#include <sundials/sundials_math.h>
+
+/*
+ * -----------------------------------------------------------------
+ * private constants
+ * -----------------------------------------------------------------
+ */
+
+#define ZERO RCONST(0.0)
+#define ONE RCONST(1.0)
+
+/*
+ * -----------------------------------------------------------------
+ * Function : SpbcgMalloc
+ * -----------------------------------------------------------------
+ */
+
+SpbcgMem SpbcgMalloc(int l_max, N_Vector vec_tmpl)
+{
+ SpbcgMem mem;
+ N_Vector r_star, r, p, q, u, Ap, vtemp;
+
+ /* Check the input parameters */
+
+ if (l_max <= 0) return(NULL);
+
+ /* Get arrays to hold temporary vectors */
+
+ r_star = N_VClone(vec_tmpl);
+ if (r_star == NULL) {
+ return(NULL);
+ }
+
+ r = N_VClone(vec_tmpl);
+ if (r == NULL) {
+ N_VDestroy(r_star);
+ return(NULL);
+ }
+
+ p = N_VClone(vec_tmpl);
+ if (p == NULL) {
+ N_VDestroy(r_star);
+ N_VDestroy(r);
+ return(NULL);
+ }
+
+ q = N_VClone(vec_tmpl);
+ if (q == NULL) {
+ N_VDestroy(r_star);
+ N_VDestroy(r);
+ N_VDestroy(p);
+ return(NULL);
+ }
+
+ u = N_VClone(vec_tmpl);
+ if (u == NULL) {
+ N_VDestroy(r_star);
+ N_VDestroy(r);
+ N_VDestroy(p);
+ N_VDestroy(q);
+ return(NULL);
+ }
+
+ Ap = N_VClone(vec_tmpl);
+ if (Ap == NULL) {
+ N_VDestroy(r_star);
+ N_VDestroy(r);
+ N_VDestroy(p);
+ N_VDestroy(q);
+ N_VDestroy(u);
+ return(NULL);
+ }
+
+ vtemp = N_VClone(vec_tmpl);
+ if (vtemp == NULL) {
+ N_VDestroy(r_star);
+ N_VDestroy(r);
+ N_VDestroy(p);
+ N_VDestroy(q);
+ N_VDestroy(u);
+ N_VDestroy(Ap);
+ return(NULL);
+ }
+
+ /* Get memory for an SpbcgMemRec containing SPBCG matrices and vectors */
+
+ mem = NULL;
+ mem = (SpbcgMem) malloc(sizeof(SpbcgMemRec));
+ if (mem == NULL) {
+ N_VDestroy(r_star);
+ N_VDestroy(r);
+ N_VDestroy(p);
+ N_VDestroy(q);
+ N_VDestroy(u);
+ N_VDestroy(Ap);
+ N_VDestroy(vtemp);
+ return(NULL);
+ }
+
+ /* Set the fields of mem */
+
+ mem->l_max = l_max;
+ mem->r_star = r_star;
+ mem->r = r;
+ mem->p = p;
+ mem->q = q;
+ mem->u = u;
+ mem->Ap = Ap;
+ mem->vtemp = vtemp;
+
+ /* Return the pointer to SPBCG memory */
+
+ return(mem);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Function : SpbcgSolve
+ * -----------------------------------------------------------------
+ */
+
+int SpbcgSolve(SpbcgMem mem, void *A_data, N_Vector x, N_Vector b,
+ int pretype, realtype delta, void *P_data, N_Vector sx,
+ N_Vector sb, ATimesFn atimes, PSolveFn psolve,
+ realtype *res_norm, int *nli, int *nps)
+{
+ realtype alpha, beta, omega, omega_denom, beta_num, beta_denom, r_norm, rho;
+ N_Vector r_star, r, p, q, u, Ap, vtemp;
+ booleantype preOnLeft, preOnRight, scale_x, scale_b, converged;
+ int l, l_max, ier;
+
+ if (mem == NULL) return(SPBCG_MEM_NULL);
+
+ /* Make local copies of mem variables */
+
+ l_max = mem->l_max;
+ r_star = mem->r_star;
+ r = mem->r;
+ p = mem->p;
+ q = mem->q;
+ u = mem->u;
+ Ap = mem->Ap;
+ vtemp = mem->vtemp;
+
+ *nli = *nps = 0; /* Initialize counters */
+ converged = FALSE; /* Initialize converged flag */
+
+ if ((pretype != PREC_LEFT) && (pretype != PREC_RIGHT) && (pretype != PREC_BOTH)) pretype = PREC_NONE;
+
+ preOnLeft = ((pretype == PREC_BOTH) || (pretype == PREC_LEFT));
+ preOnRight = ((pretype == PREC_BOTH) || (pretype == PREC_RIGHT));
+
+ scale_x = (sx != NULL);
+ scale_b = (sb != NULL);
+
+ /* Set r_star to initial (unscaled) residual r_0 = b - A*x_0 */
+
+ if (N_VDotProd(x, x) == ZERO) N_VScale(ONE, b, r_star);
+ else {
+ ier = atimes(A_data, x, r_star);
+ if (ier != 0)
+ return((ier < 0) ? SPBCG_ATIMES_FAIL_UNREC : SPBCG_ATIMES_FAIL_REC);
+ N_VLinearSum(ONE, b, -ONE, r_star, r_star);
+ }
+
+ /* Apply left preconditioner and b-scaling to r_star = r_0 */
+
+ if (preOnLeft) {
+ ier = psolve(P_data, r_star, r, PREC_LEFT);
+ (*nps)++;
+ if (ier != 0) return((ier < 0) ? SPBCG_PSOLVE_FAIL_UNREC : SPBCG_PSOLVE_FAIL_REC);
+ }
+ else N_VScale(ONE, r_star, r);
+
+ if (scale_b) N_VProd(sb, r, r_star);
+ else N_VScale(ONE, r, r_star);
+
+ /* Initialize beta_denom to the dot product of r0 with r0 */
+
+ beta_denom = N_VDotProd(r_star, r_star);
+
+ /* Set r_norm to L2 norm of r_star = sb P1_inv r_0, and
+ return if small */
+
+ *res_norm = r_norm = rho = RSqrt(beta_denom);
+ if (r_norm <= delta) return(SPBCG_SUCCESS);
+
+ /* Copy r_star to r and p */
+
+ N_VScale(ONE, r_star, r);
+ N_VScale(ONE, r_star, p);
+
+ /* Begin main iteration loop */
+
+ for(l = 0; l < l_max; l++) {
+
+ (*nli)++;
+
+ /* Generate Ap = A-tilde p, where A-tilde = sb P1_inv A P2_inv sx_inv */
+
+ /* Apply x-scaling: vtemp = sx_inv p */
+
+ if (scale_x) N_VDiv(p, sx, vtemp);
+ else N_VScale(ONE, p, vtemp);
+
+ /* Apply right preconditioner: vtemp = P2_inv sx_inv p */
+
+ if (preOnRight) {
+ N_VScale(ONE, vtemp, Ap);
+ ier = psolve(P_data, Ap, vtemp, PREC_RIGHT);
+ (*nps)++;
+ if (ier != 0) return((ier < 0) ? SPBCG_PSOLVE_FAIL_UNREC : SPBCG_PSOLVE_FAIL_REC);
+ }
+
+ /* Apply A: Ap = A P2_inv sx_inv p */
+
+ ier = atimes(A_data, vtemp, Ap );
+ if (ier != 0)
+ return((ier < 0) ? SPBCG_ATIMES_FAIL_UNREC : SPBCG_ATIMES_FAIL_REC);
+
+ /* Apply left preconditioner: vtemp = P1_inv A P2_inv sx_inv p */
+
+ if (preOnLeft) {
+ ier = psolve(P_data, Ap, vtemp, PREC_LEFT);
+ (*nps)++;
+ if (ier != 0) return((ier < 0) ? SPBCG_PSOLVE_FAIL_UNREC : SPBCG_PSOLVE_FAIL_REC);
+ }
+ else N_VScale(ONE, Ap, vtemp);
+
+ /* Apply b-scaling: Ap = sb P1_inv A P2_inv sx_inv p */
+
+ if (scale_b) N_VProd(sb, vtemp, Ap);
+ else N_VScale(ONE, vtemp, Ap);
+
+
+ /* Calculate alpha = <r,r_star>/<Ap,r_star> */
+
+ alpha = ((N_VDotProd(r, r_star) / N_VDotProd(Ap, r_star)));
+
+ /* Update q = r - alpha*Ap = r - alpha*(sb P1_inv A P2_inv sx_inv p) */
+
+ N_VLinearSum(ONE, r, -alpha, Ap, q);
+
+ /* Generate u = A-tilde q */
+
+ /* Apply x-scaling: vtemp = sx_inv q */
+
+ if (scale_x) N_VDiv(q, sx, vtemp);
+ else N_VScale(ONE, q, vtemp);
+
+ /* Apply right preconditioner: vtemp = P2_inv sx_inv q */
+
+ if (preOnRight) {
+ N_VScale(ONE, vtemp, u);
+ ier = psolve(P_data, u, vtemp, PREC_RIGHT);
+ (*nps)++;
+ if (ier != 0) return((ier < 0) ? SPBCG_PSOLVE_FAIL_UNREC : SPBCG_PSOLVE_FAIL_REC);
+ }
+
+ /* Apply A: u = A P2_inv sx_inv u */
+
+ ier = atimes(A_data, vtemp, u );
+ if (ier != 0)
+ return((ier < 0) ? SPBCG_ATIMES_FAIL_UNREC : SPBCG_ATIMES_FAIL_REC);
+
+ /* Apply left preconditioner: vtemp = P1_inv A P2_inv sx_inv p */
+
+ if (preOnLeft) {
+ ier = psolve(P_data, u, vtemp, PREC_LEFT);
+ (*nps)++;
+ if (ier != 0) return((ier < 0) ? SPBCG_PSOLVE_FAIL_UNREC : SPBCG_PSOLVE_FAIL_REC);
+ }
+ else N_VScale(ONE, u, vtemp);
+
+ /* Apply b-scaling: u = sb P1_inv A P2_inv sx_inv u */
+
+ if (scale_b) N_VProd(sb, vtemp, u);
+ else N_VScale(ONE, vtemp, u);
+
+
+ /* Calculate omega = <u,q>/<u,u> */
+
+ omega_denom = N_VDotProd(u, u);
+ if (omega_denom == ZERO) omega_denom = ONE;
+ omega = (N_VDotProd(u, q) / omega_denom);
+
+ /* Update x = x + alpha*p + omega*q */
+
+ N_VLinearSum(alpha, p, omega, q, vtemp);
+ N_VLinearSum(ONE, x, ONE, vtemp, x);
+
+ /* Update the residual r = q - omega*u */
+
+ N_VLinearSum(ONE, q, -omega, u, r);
+
+ /* Set rho = norm(r) and check convergence */
+
+ *res_norm = rho = RSqrt(N_VDotProd(r, r));
+ if (rho <= delta) {
+ converged = TRUE;
+ break;
+ }
+
+ /* Not yet converged, continue iteration */
+ /* Update beta = <rnew,r_star> / <rold,r_start> * alpha / omega */
+
+ beta_num = N_VDotProd(r, r_star);
+ beta = ((beta_num / beta_denom) * (alpha / omega));
+ beta_denom = beta_num;
+
+ /* Update p = r + beta*(p - omega*Ap) */
+
+ N_VLinearSum(ONE, p, -omega, Ap, vtemp);
+ N_VLinearSum(ONE, r, beta, vtemp, p);
+
+ }
+
+ /* Main loop finished */
+
+ if ((converged == TRUE) || (rho < r_norm)) {
+
+ /* Apply the x-scaling and right preconditioner: x = P2_inv sx_inv x */
+
+ if (scale_x) N_VDiv(x, sx, x);
+ if (preOnRight) {
+ ier = psolve(P_data, x, vtemp, PREC_RIGHT);
+ (*nps)++;
+ if (ier != 0) return((ier < 0) ? SPBCG_PSOLVE_FAIL_UNREC : SPBCG_PSOLVE_FAIL_REC);
+ N_VScale(ONE, vtemp, x);
+ }
+
+ if (converged == TRUE) return(SPBCG_SUCCESS);
+ else return(SPBCG_RES_REDUCED);
+ }
+ else return(SPBCG_CONV_FAIL);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Function : SpbcgFree
+ * -----------------------------------------------------------------
+ */
+
+void SpbcgFree(SpbcgMem mem)
+{
+
+ if (mem == NULL) return;
+
+ N_VDestroy(mem->r_star);
+ N_VDestroy(mem->r);
+ N_VDestroy(mem->p);
+ N_VDestroy(mem->q);
+ N_VDestroy(mem->u);
+ N_VDestroy(mem->Ap);
+ N_VDestroy(mem->vtemp);
+
+ free(mem); mem = NULL;
+}
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_spgmr.c b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_spgmr.c
new file mode 100644
index 0000000..7efd187
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_spgmr.c
@@ -0,0 +1,458 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2007/04/06 20:33:30 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Scott D. Cohen, Alan C. Hindmarsh and
+ * Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2002, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for the scaled preconditioned
+ * GMRES (SPGMR) iterative linear solver.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <sundials/sundials_spgmr.h>
+#include <sundials/sundials_math.h>
+
+/*
+ * -----------------------------------------------------------------
+ * private constants
+ * -----------------------------------------------------------------
+ */
+
+#define ZERO RCONST(0.0)
+#define ONE RCONST(1.0)
+
+/*
+ * -----------------------------------------------------------------
+ * Function : SpgmrMalloc
+ * -----------------------------------------------------------------
+ */
+
+SpgmrMem SpgmrMalloc(int l_max, N_Vector vec_tmpl)
+{
+ SpgmrMem mem;
+ N_Vector *V, xcor, vtemp;
+ realtype **Hes, *givens, *yg;
+ int k, i;
+
+ /* Check the input parameters. */
+
+ if (l_max <= 0) return(NULL);
+
+ /* Get memory for the Krylov basis vectors V[0], ..., V[l_max]. */
+
+ V = N_VCloneVectorArray(l_max+1, vec_tmpl);
+ if (V == NULL) return(NULL);
+
+ /* Get memory for the Hessenberg matrix Hes. */
+
+ Hes = NULL;
+ Hes = (realtype **) malloc((l_max+1)*sizeof(realtype *));
+ if (Hes == NULL) {
+ N_VDestroyVectorArray(V, l_max+1);
+ return(NULL);
+ }
+
+ for (k = 0; k <= l_max; k++) {
+ Hes[k] = NULL;
+ Hes[k] = (realtype *) malloc(l_max*sizeof(realtype));
+ if (Hes[k] == NULL) {
+ for (i = 0; i < k; i++) {free(Hes[i]); Hes[i] = NULL;}
+ free(Hes); Hes = NULL;
+ N_VDestroyVectorArray(V, l_max+1);
+ return(NULL);
+ }
+ }
+
+ /* Get memory for Givens rotation components. */
+
+ givens = NULL;
+ givens = (realtype *) malloc(2*l_max*sizeof(realtype));
+ if (givens == NULL) {
+ for (i = 0; i <= l_max; i++) {free(Hes[i]); Hes[i] = NULL;}
+ free(Hes); Hes = NULL;
+ N_VDestroyVectorArray(V, l_max+1);
+ return(NULL);
+ }
+
+ /* Get memory to hold the correction to z_tilde. */
+
+ xcor = N_VClone(vec_tmpl);
+ if (xcor == NULL) {
+ free(givens); givens = NULL;
+ for (i = 0; i <= l_max; i++) {free(Hes[i]); Hes[i] = NULL;}
+ free(Hes); Hes = NULL;
+ N_VDestroyVectorArray(V, l_max+1);
+ return(NULL);
+ }
+
+ /* Get memory to hold SPGMR y and g vectors. */
+
+ yg = NULL;
+ yg = (realtype *) malloc((l_max+1)*sizeof(realtype));
+ if (yg == NULL) {
+ N_VDestroy(xcor);
+ free(givens); givens = NULL;
+ for (i = 0; i <= l_max; i++) {free(Hes[i]); Hes[i] = NULL;}
+ free(Hes); Hes = NULL;
+ N_VDestroyVectorArray(V, l_max+1);
+ return(NULL);
+ }
+
+ /* Get an array to hold a temporary vector. */
+
+ vtemp = N_VClone(vec_tmpl);
+ if (vtemp == NULL) {
+ free(yg); yg = NULL;
+ N_VDestroy(xcor);
+ free(givens); givens = NULL;
+ for (i = 0; i <= l_max; i++) {free(Hes[i]); Hes[i] = NULL;}
+ free(Hes); Hes = NULL;
+ N_VDestroyVectorArray(V, l_max+1);
+ return(NULL);
+ }
+
+ /* Get memory for an SpgmrMemRec containing SPGMR matrices and vectors. */
+
+ mem = NULL;
+ mem = (SpgmrMem) malloc(sizeof(SpgmrMemRec));
+ if (mem == NULL) {
+ N_VDestroy(vtemp);
+ free(yg); yg = NULL;
+ N_VDestroy(xcor);
+ free(givens); givens = NULL;
+ for (i = 0; i <= l_max; i++) {free(Hes[i]); Hes[i] = NULL;}
+ free(Hes); Hes = NULL;
+ N_VDestroyVectorArray(V, l_max+1);
+ return(NULL);
+ }
+
+ /* Set the fields of mem. */
+
+ mem->l_max = l_max;
+ mem->V = V;
+ mem->Hes = Hes;
+ mem->givens = givens;
+ mem->xcor = xcor;
+ mem->yg = yg;
+ mem->vtemp = vtemp;
+
+ /* Return the pointer to SPGMR memory. */
+
+ return(mem);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Function : SpgmrSolve
+ * -----------------------------------------------------------------
+ */
+
+int SpgmrSolve(SpgmrMem mem, void *A_data, N_Vector x, N_Vector b,
+ int pretype, int gstype, realtype delta, int max_restarts,
+ void *P_data, N_Vector s1, N_Vector s2, ATimesFn atimes,
+ PSolveFn psolve, realtype *res_norm, int *nli, int *nps)
+{
+ N_Vector *V, xcor, vtemp;
+ realtype **Hes, *givens, *yg;
+ realtype beta, rotation_product, r_norm, s_product, rho;
+ booleantype preOnLeft, preOnRight, scale2, scale1, converged;
+ int i, j, k, l, l_plus_1, l_max, krydim, ier, ntries;
+
+ if (mem == NULL) return(SPGMR_MEM_NULL);
+
+ /* Initialize some variables */
+
+ l_plus_1 = 0;
+ krydim = 0;
+
+ /* Make local copies of mem variables. */
+
+ l_max = mem->l_max;
+ V = mem->V;
+ Hes = mem->Hes;
+ givens = mem->givens;
+ xcor = mem->xcor;
+ yg = mem->yg;
+ vtemp = mem->vtemp;
+
+ *nli = *nps = 0; /* Initialize counters */
+ converged = FALSE; /* Initialize converged flag */
+
+ if (max_restarts < 0) max_restarts = 0;
+
+ if ((pretype != PREC_LEFT) && (pretype != PREC_RIGHT) && (pretype != PREC_BOTH))
+ pretype = PREC_NONE;
+
+ preOnLeft = ((pretype == PREC_LEFT) || (pretype == PREC_BOTH));
+ preOnRight = ((pretype == PREC_RIGHT) || (pretype == PREC_BOTH));
+ scale1 = (s1 != NULL);
+ scale2 = (s2 != NULL);
+
+ /* Set vtemp and V[0] to initial (unscaled) residual r_0 = b - A*x_0. */
+
+ if (N_VDotProd(x, x) == ZERO) {
+ N_VScale(ONE, b, vtemp);
+ } else {
+ ier = atimes(A_data, x, vtemp);
+ if (ier != 0)
+ return((ier < 0) ? SPGMR_ATIMES_FAIL_UNREC : SPGMR_ATIMES_FAIL_REC);
+ N_VLinearSum(ONE, b, -ONE, vtemp, vtemp);
+ }
+ N_VScale(ONE, vtemp, V[0]);
+
+ /* Apply left preconditioner and left scaling to V[0] = r_0. */
+
+ if (preOnLeft) {
+ ier = psolve(P_data, V[0], vtemp, PREC_LEFT);
+ (*nps)++;
+ if (ier != 0)
+ return((ier < 0) ? SPGMR_PSOLVE_FAIL_UNREC : SPGMR_PSOLVE_FAIL_REC);
+ } else {
+ N_VScale(ONE, V[0], vtemp);
+ }
+
+ if (scale1) {
+ N_VProd(s1, vtemp, V[0]);
+ } else {
+ N_VScale(ONE, vtemp, V[0]);
+ }
+
+ /* Set r_norm = beta to L2 norm of V[0] = s1 P1_inv r_0, and
+ return if small. */
+
+ *res_norm = r_norm = beta = RSqrt(N_VDotProd(V[0], V[0]));
+ if (r_norm <= delta)
+ return(SPGMR_SUCCESS);
+
+ /* Initialize rho to avoid compiler warning message */
+
+ rho = beta;
+
+ /* Set xcor = 0. */
+
+ N_VConst(ZERO, xcor);
+
+
+ /* Begin outer iterations: up to (max_restarts + 1) attempts. */
+
+ for (ntries = 0; ntries <= max_restarts; ntries++) {
+
+ /* Initialize the Hessenberg matrix Hes and Givens rotation
+ product. Normalize the initial vector V[0]. */
+
+ for (i = 0; i <= l_max; i++)
+ for (j = 0; j < l_max; j++)
+ Hes[i][j] = ZERO;
+
+ rotation_product = ONE;
+
+ N_VScale(ONE/r_norm, V[0], V[0]);
+
+ /* Inner loop: generate Krylov sequence and Arnoldi basis. */
+
+ for (l = 0; l < l_max; l++) {
+
+ (*nli)++;
+
+ krydim = l_plus_1 = l + 1;
+
+ /* Generate A-tilde V[l], where A-tilde = s1 P1_inv A P2_inv s2_inv. */
+
+ /* Apply right scaling: vtemp = s2_inv V[l]. */
+
+ if (scale2) N_VDiv(V[l], s2, vtemp);
+ else N_VScale(ONE, V[l], vtemp);
+
+ /* Apply right preconditioner: vtemp = P2_inv s2_inv V[l]. */
+
+ if (preOnRight) {
+ N_VScale(ONE, vtemp, V[l_plus_1]);
+ ier = psolve(P_data, V[l_plus_1], vtemp, PREC_RIGHT);
+ (*nps)++;
+ if (ier != 0)
+ return((ier < 0) ? SPGMR_PSOLVE_FAIL_UNREC : SPGMR_PSOLVE_FAIL_REC);
+ }
+
+ /* Apply A: V[l+1] = A P2_inv s2_inv V[l]. */
+
+ ier = atimes(A_data, vtemp, V[l_plus_1] );
+ if (ier != 0)
+ return((ier < 0) ? SPGMR_ATIMES_FAIL_UNREC : SPGMR_ATIMES_FAIL_REC);
+
+ /* Apply left preconditioning: vtemp = P1_inv A P2_inv s2_inv V[l]. */
+
+ if (preOnLeft) {
+ ier = psolve(P_data, V[l_plus_1], vtemp, PREC_LEFT);
+ (*nps)++;
+ if (ier != 0)
+ return((ier < 0) ? SPGMR_PSOLVE_FAIL_UNREC : SPGMR_PSOLVE_FAIL_REC);
+ } else {
+ N_VScale(ONE, V[l_plus_1], vtemp);
+ }
+
+ /* Apply left scaling: V[l+1] = s1 P1_inv A P2_inv s2_inv V[l]. */
+
+ if (scale1) {
+ N_VProd(s1, vtemp, V[l_plus_1]);
+ } else {
+ N_VScale(ONE, vtemp, V[l_plus_1]);
+ }
+
+ /* Orthogonalize V[l+1] against previous V[i]: V[l+1] = w_tilde. */
+
+ if (gstype == CLASSICAL_GS) {
+ if (ClassicalGS(V, Hes, l_plus_1, l_max, &(Hes[l_plus_1][l]),
+ vtemp, yg) != 0)
+ return(SPGMR_GS_FAIL);
+ } else {
+ if (ModifiedGS(V, Hes, l_plus_1, l_max, &(Hes[l_plus_1][l])) != 0)
+ return(SPGMR_GS_FAIL);
+ }
+
+ /* Update the QR factorization of Hes. */
+
+ if(QRfact(krydim, Hes, givens, l) != 0 )
+ return(SPGMR_QRFACT_FAIL);
+
+ /* Update residual norm estimate; break if convergence test passes. */
+
+ rotation_product *= givens[2*l+1];
+ *res_norm = rho = ABS(rotation_product*r_norm);
+
+ if (rho <= delta) { converged = TRUE; break; }
+
+ /* Normalize V[l+1] with norm value from the Gram-Schmidt routine. */
+
+ N_VScale(ONE/Hes[l_plus_1][l], V[l_plus_1], V[l_plus_1]);
+ }
+
+ /* Inner loop is done. Compute the new correction vector xcor. */
+
+ /* Construct g, then solve for y. */
+
+ yg[0] = r_norm;
+ for (i = 1; i <= krydim; i++) yg[i]=ZERO;
+ if (QRsol(krydim, Hes, givens, yg) != 0)
+ return(SPGMR_QRSOL_FAIL);
+
+ /* Add correction vector V_l y to xcor. */
+
+ for (k = 0; k < krydim; k++)
+ N_VLinearSum(yg[k], V[k], ONE, xcor, xcor);
+
+ /* If converged, construct the final solution vector x and return. */
+
+ if (converged) {
+
+ /* Apply right scaling and right precond.: vtemp = P2_inv s2_inv xcor. */
+
+ if (scale2) N_VDiv(xcor, s2, xcor);
+ if (preOnRight) {
+ ier = psolve(P_data, xcor, vtemp, PREC_RIGHT);
+ (*nps)++;
+ if (ier != 0)
+ return((ier < 0) ? SPGMR_PSOLVE_FAIL_UNREC : SPGMR_PSOLVE_FAIL_REC);
+ } else {
+ N_VScale(ONE, xcor, vtemp);
+ }
+
+ /* Add vtemp to initial x to get final solution x, and return */
+
+ N_VLinearSum(ONE, x, ONE, vtemp, x);
+
+ return(SPGMR_SUCCESS);
+ }
+
+ /* Not yet converged; if allowed, prepare for restart. */
+
+ if (ntries == max_restarts) break;
+
+ /* Construct last column of Q in yg. */
+
+ s_product = ONE;
+ for (i = krydim; i > 0; i--) {
+ yg[i] = s_product*givens[2*i-2];
+ s_product *= givens[2*i-1];
+ }
+ yg[0] = s_product;
+
+ /* Scale r_norm and yg. */
+ r_norm *= s_product;
+ for (i = 0; i <= krydim; i++)
+ yg[i] *= r_norm;
+ r_norm = ABS(r_norm);
+
+ /* Multiply yg by V_(krydim+1) to get last residual vector; restart. */
+ N_VScale(yg[0], V[0], V[0]);
+ for (k = 1; k <= krydim; k++)
+ N_VLinearSum(yg[k], V[k], ONE, V[0], V[0]);
+
+ }
+
+ /* Failed to converge, even after allowed restarts.
+ If the residual norm was reduced below its initial value, compute
+ and return x anyway. Otherwise return failure flag. */
+
+ if (rho < beta) {
+
+ /* Apply right scaling and right precond.: vtemp = P2_inv s2_inv xcor. */
+
+ if (scale2) N_VDiv(xcor, s2, xcor);
+ if (preOnRight) {
+ ier = psolve(P_data, xcor, vtemp, PREC_RIGHT);
+ (*nps)++;
+ if (ier != 0)
+ return((ier < 0) ? SPGMR_PSOLVE_FAIL_UNREC : SPGMR_PSOLVE_FAIL_REC);
+ } else {
+ N_VScale(ONE, xcor, vtemp);
+ }
+
+ /* Add vtemp to initial x to get final solution x, and return. */
+
+ N_VLinearSum(ONE, x, ONE, vtemp, x);
+
+ return(SPGMR_RES_REDUCED);
+ }
+
+ return(SPGMR_CONV_FAIL);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Function : SpgmrFree
+ * -----------------------------------------------------------------
+ */
+
+void SpgmrFree(SpgmrMem mem)
+{
+ int i, l_max;
+ realtype **Hes, *givens, *yg;
+
+ if (mem == NULL) return;
+
+ l_max = mem->l_max;
+ Hes = mem->Hes;
+ givens = mem->givens;
+ yg = mem->yg;
+
+ for (i = 0; i <= l_max; i++) {free(Hes[i]); Hes[i] = NULL;}
+ free(Hes); Hes = NULL;
+ free(mem->givens); givens = NULL;
+ free(mem->yg); yg = NULL;
+
+ N_VDestroyVectorArray(mem->V, l_max+1);
+ N_VDestroy(mem->xcor);
+ N_VDestroy(mem->vtemp);
+
+ free(mem); mem = NULL;
+}
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_sptfqmr.c b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_sptfqmr.c
new file mode 100644
index 0000000..3798c29
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/src/sundials/sundials_sptfqmr.c
@@ -0,0 +1,516 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.2 $
+ * $Date: 2007/04/06 20:33:30 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Aaron Collier @ LLNL
+ * -----------------------------------------------------------------
+ * Copyright (c) 2005, The Regents of the University of California.
+ * Produced at the Lawrence Livermore National Laboratory.
+ * All rights reserved.
+ * For details, see the LICENSE file.
+ * -----------------------------------------------------------------
+ * This is the implementation file for the scaled preconditioned
+ * Transpose-Free Quasi-Minimal Residual (SPTFQMR) linear solver.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <sundials/sundials_sptfqmr.h>
+#include <sundials/sundials_math.h>
+
+/*
+ * -----------------------------------------------------------------
+ * private constants
+ * -----------------------------------------------------------------
+ */
+
+#define ZERO RCONST(0.0)
+#define ONE RCONST(1.0)
+
+/*
+ * -----------------------------------------------------------------
+ * Function : SptfqmrMalloc
+ * -----------------------------------------------------------------
+ */
+
+SptfqmrMem SptfqmrMalloc(int l_max, N_Vector vec_tmpl)
+{
+ SptfqmrMem mem;
+ N_Vector *r;
+ N_Vector q, d, v, p, u;
+ N_Vector r_star, vtemp1, vtemp2, vtemp3;
+
+ /* Check the input parameters */
+ if ((l_max <= 0) || (vec_tmpl == NULL)) return(NULL);
+
+ /* Allocate space for vectors */
+
+ r_star = N_VClone(vec_tmpl);
+ if (r_star == NULL) return(NULL);
+
+ q = N_VClone(vec_tmpl);
+ if (q == NULL) {
+ N_VDestroy(r_star);
+ return(NULL);
+ }
+
+ d = N_VClone(vec_tmpl);
+ if (d == NULL) {
+ N_VDestroy(r_star);
+ N_VDestroy(q);
+ return(NULL);
+ }
+
+ v = N_VClone(vec_tmpl);
+ if (v == NULL) {
+ N_VDestroy(r_star);
+ N_VDestroy(q);
+ N_VDestroy(d);
+ return(NULL);
+ }
+
+ p = N_VClone(vec_tmpl);
+ if (p == NULL) {
+ N_VDestroy(r_star);
+ N_VDestroy(q);
+ N_VDestroy(d);
+ N_VDestroy(v);
+ return(NULL);
+ }
+
+ r = N_VCloneVectorArray(2, vec_tmpl);
+ if (r == NULL) {
+ N_VDestroy(r_star);
+ N_VDestroy(q);
+ N_VDestroy(d);
+ N_VDestroy(v);
+ N_VDestroy(p);
+ return(NULL);
+ }
+
+ u = N_VClone(vec_tmpl);
+ if (u == NULL) {
+ N_VDestroy(r_star);
+ N_VDestroy(q);
+ N_VDestroy(d);
+ N_VDestroy(v);
+ N_VDestroy(p);
+ N_VDestroyVectorArray(r, 2);
+ return(NULL);
+ }
+
+ vtemp1 = N_VClone(vec_tmpl);
+ if (vtemp1 == NULL) {
+ N_VDestroy(r_star);
+ N_VDestroy(q);
+ N_VDestroy(d);
+ N_VDestroy(v);
+ N_VDestroy(p);
+ N_VDestroyVectorArray(r, 2);
+ N_VDestroy(u);
+ return(NULL);
+ }
+
+ vtemp2 = N_VClone(vec_tmpl);
+ if (vtemp2 == NULL) {
+ N_VDestroy(r_star);
+ N_VDestroy(q);
+ N_VDestroy(d);
+ N_VDestroy(v);
+ N_VDestroy(p);
+ N_VDestroyVectorArray(r, 2);
+ N_VDestroy(u);
+ N_VDestroy(vtemp1);
+ return(NULL);
+ }
+
+ vtemp3 = N_VClone(vec_tmpl);
+ if (vtemp3 == NULL) {
+ N_VDestroy(r_star);
+ N_VDestroy(q);
+ N_VDestroy(d);
+ N_VDestroy(v);
+ N_VDestroy(p);
+ N_VDestroyVectorArray(r, 2);
+ N_VDestroy(u);
+ N_VDestroy(vtemp1);
+ N_VDestroy(vtemp2);
+ return(NULL);
+ }
+
+ /* Allocate memory for SptfqmrMemRec */
+ mem = NULL;
+ mem = (SptfqmrMem) malloc(sizeof(SptfqmrMemRec));
+ if (mem == NULL) {
+ N_VDestroy(r_star);
+ N_VDestroy(q);
+ N_VDestroy(d);
+ N_VDestroy(v);
+ N_VDestroy(p);
+ N_VDestroyVectorArray(r, 2);
+ N_VDestroy(u);
+ N_VDestroy(vtemp1);
+ N_VDestroy(vtemp2);
+ N_VDestroy(vtemp3);
+ return(NULL);
+ }
+
+ /* Intialize SptfqmrMemRec data structure */
+ mem->l_max = l_max;
+ mem->r_star = r_star;
+ mem->q = q;
+ mem->d = d;
+ mem->v = v;
+ mem->p = p;
+ mem->r = r;
+ mem->u = u;
+ mem->vtemp1 = vtemp1;
+ mem->vtemp2 = vtemp2;
+ mem->vtemp3 = vtemp3;
+
+ /* Return pointer to SPTFQMR memory block */
+ return(mem);
+}
+
+#define l_max (mem->l_max)
+#define r_star (mem->r_star)
+#define q_ (mem->q)
+#define d_ (mem->d)
+#define v_ (mem->v)
+#define p_ (mem->p)
+#define r_ (mem->r)
+#define u_ (mem->u)
+#define vtemp1 (mem->vtemp1)
+#define vtemp2 (mem->vtemp2)
+#define vtemp3 (mem->vtemp3)
+
+/*
+ * -----------------------------------------------------------------
+ * Function : SptfqmrSolve
+ * -----------------------------------------------------------------
+ */
+
+int SptfqmrSolve(SptfqmrMem mem, void *A_data, N_Vector x, N_Vector b,
+ int pretype, realtype delta, void *P_data, N_Vector sx,
+ N_Vector sb, ATimesFn atimes, PSolveFn psolve,
+ realtype *res_norm, int *nli, int *nps)
+{
+ realtype alpha, tau, eta, beta, c, sigma, v_bar, omega;
+ realtype rho[2];
+ realtype r_init_norm, r_curr_norm;
+ realtype temp_val;
+ booleantype preOnLeft, preOnRight, scale_x, scale_b, converged;
+ booleantype b_ok;
+ int n, m, ier;
+
+ /* Exit immediately if memory pointer is NULL */
+ if (mem == NULL) return(SPTFQMR_MEM_NULL);
+
+ temp_val = r_curr_norm = -ONE; /* Initialize to avoid compiler warnings */
+
+ *nli = *nps = 0; /* Initialize counters */
+ converged = FALSE; /* Initialize convergence flag */
+ b_ok = FALSE;
+
+ if ((pretype != PREC_LEFT) &&
+ (pretype != PREC_RIGHT) &&
+ (pretype != PREC_BOTH)) pretype = PREC_NONE;
+
+ preOnLeft = ((pretype == PREC_BOTH) || (pretype == PREC_LEFT));
+ preOnRight = ((pretype == PREC_BOTH) || (pretype == PREC_RIGHT));
+
+ scale_x = (sx != NULL);
+ scale_b = (sb != NULL);
+
+ /* Set r_star to initial (unscaled) residual r_star = r_0 = b - A*x_0 */
+ /* NOTE: if x == 0 then just set residual to b and continue */
+ if (N_VDotProd(x, x) == ZERO) N_VScale(ONE, b, r_star);
+ else {
+ ier = atimes(A_data, x, r_star);
+ if (ier != 0)
+ return((ier < 0) ? SPTFQMR_ATIMES_FAIL_UNREC : SPTFQMR_ATIMES_FAIL_REC);
+ N_VLinearSum(ONE, b, -ONE, r_star, r_star);
+ }
+
+ /* Apply left preconditioner and b-scaling to r_star (or really just r_0) */
+ if (preOnLeft) {
+ ier = psolve(P_data, r_star, vtemp1, PREC_LEFT);
+ (*nps)++;
+ if (ier != 0)
+ return((ier < 0) ? SPTFQMR_PSOLVE_FAIL_UNREC : SPTFQMR_PSOLVE_FAIL_REC);
+ }
+ else N_VScale(ONE, r_star, vtemp1);
+ if (scale_b) N_VProd(sb, vtemp1, r_star);
+ else N_VScale(ONE, vtemp1, r_star);
+
+ /* Initialize rho[0] */
+ /* NOTE: initialized here to reduce number of computations - avoid need
+ to compute r_star^T*r_star twice, and avoid needlessly squaring
+ values */
+ rho[0] = N_VDotProd(r_star, r_star);
+
+ /* Compute norm of initial residual (r_0) to see if we really need
+ to do anything */
+ *res_norm = r_init_norm = RSqrt(rho[0]);
+ if (r_init_norm <= delta) return(SPTFQMR_SUCCESS);
+
+ /* Set v_ = A*r_0 (preconditioned and scaled) */
+ if (scale_x) N_VDiv(r_star, sx, vtemp1);
+ else N_VScale(ONE, r_star, vtemp1);
+ if (preOnRight) {
+ N_VScale(ONE, vtemp1, v_);
+ ier = psolve(P_data, v_, vtemp1, PREC_RIGHT);
+ (*nps)++;
+ if (ier != 0) return((ier < 0) ? SPTFQMR_PSOLVE_FAIL_UNREC : SPTFQMR_PSOLVE_FAIL_REC);
+ }
+ ier = atimes(A_data, vtemp1, v_);
+ if (ier != 0)
+ return((ier < 0) ? SPTFQMR_ATIMES_FAIL_UNREC : SPTFQMR_ATIMES_FAIL_REC);
+ if (preOnLeft) {
+ ier = psolve(P_data, v_, vtemp1, PREC_LEFT);
+ (*nps)++;
+ if (ier != 0) return((ier < 0) ? SPTFQMR_PSOLVE_FAIL_UNREC : SPTFQMR_PSOLVE_FAIL_REC);
+ }
+ else N_VScale(ONE, v_, vtemp1);
+ if (scale_b) N_VProd(sb, vtemp1, v_);
+ else N_VScale(ONE, vtemp1, v_);
+
+ /* Initialize remaining variables */
+ N_VScale(ONE, r_star, r_[0]);
+ N_VScale(ONE, r_star, u_);
+ N_VScale(ONE, r_star, p_);
+ N_VConst(ZERO, d_);
+
+ tau = r_init_norm;
+ v_bar = eta = ZERO;
+
+ /* START outer loop */
+ for (n = 0; n < l_max; ++n) {
+
+ /* Increment linear iteration counter */
+ (*nli)++;
+
+ /* sigma = r_star^T*v_ */
+ sigma = N_VDotProd(r_star, v_);
+
+ /* alpha = rho[0]/sigma */
+ alpha = rho[0]/sigma;
+
+ /* q_ = u_-alpha*v_ */
+ N_VLinearSum(ONE, u_, -alpha, v_, q_);
+
+ /* r_[1] = r_[0]-alpha*A*(u_+q_) */
+ N_VLinearSum(ONE, u_, ONE, q_, r_[1]);
+ if (scale_x) N_VDiv(r_[1], sx, r_[1]);
+ if (preOnRight) {
+ N_VScale(ONE, r_[1], vtemp1);
+ ier = psolve(P_data, vtemp1, r_[1], PREC_RIGHT);
+ (*nps)++;
+ if (ier != 0) return((ier < 0) ? SPTFQMR_PSOLVE_FAIL_UNREC : SPTFQMR_PSOLVE_FAIL_REC);
+ }
+ ier = atimes(A_data, r_[1], vtemp1);
+ if (ier != 0)
+ return((ier < 0) ? SPTFQMR_ATIMES_FAIL_UNREC : SPTFQMR_ATIMES_FAIL_REC);
+ if (preOnLeft) {
+ ier = psolve(P_data, vtemp1, r_[1], PREC_LEFT);
+ (*nps)++;
+ if (ier != 0) return((ier < 0) ? SPTFQMR_PSOLVE_FAIL_UNREC : SPTFQMR_PSOLVE_FAIL_REC);
+ }
+ else N_VScale(ONE, vtemp1, r_[1]);
+ if (scale_b) N_VProd(sb, r_[1], vtemp1);
+ else N_VScale(ONE, r_[1], vtemp1);
+ N_VLinearSum(ONE, r_[0], -alpha, vtemp1, r_[1]);
+
+ /* START inner loop */
+ for (m = 0; m < 2; ++m) {
+
+ /* d_ = [*]+(v_bar^2*eta/alpha)*d_ */
+ /* NOTES:
+ * (1) [*] = u_ if m == 0, and q_ if m == 1
+ * (2) using temp_val reduces the number of required computations
+ * if the inner loop is executed twice
+ */
+ if (m == 0) {
+ temp_val = RSqrt(N_VDotProd(r_[1], r_[1]));
+ omega = RSqrt(RSqrt(N_VDotProd(r_[0], r_[0]))*temp_val);
+ N_VLinearSum(ONE, u_, SQR(v_bar)*eta/alpha, d_, d_);
+ }
+ else {
+ omega = temp_val;
+ N_VLinearSum(ONE, q_, SQR(v_bar)*eta/alpha, d_, d_);
+ }
+
+ /* v_bar = omega/tau */
+ v_bar = omega/tau;
+
+ /* c = (1+v_bar^2)^(-1/2) */
+ c = ONE / RSqrt(ONE+SQR(v_bar));
+
+ /* tau = tau*v_bar*c */
+ tau = tau*v_bar*c;
+
+ /* eta = c^2*alpha */
+ eta = SQR(c)*alpha;
+
+ /* x = x+eta*d_ */
+ N_VLinearSum(ONE, x, eta, d_, x);
+
+ /* Check for convergence... */
+ /* NOTE: just use approximation to norm of residual, if possible */
+ *res_norm = r_curr_norm = tau*RSqrt((realtype)m+1);
+
+ /* Exit inner loop if iteration has converged based upon approximation
+ to norm of current residual */
+ if (r_curr_norm <= delta) {
+ converged = TRUE;
+ break;
+ }
+
+ /* Decide if actual norm of residual vector should be computed */
+ /* NOTES:
+ * (1) if r_curr_norm > delta, then check if actual residual norm
+ * is OK (recall we first compute an approximation)
+ * (2) if r_curr_norm >= r_init_norm and m == 1 and n == l_max, then
+ * compute actual residual norm to see if the iteration can be
+ * saved
+ * (3) the scaled and preconditioned right-hand side of the given
+ * linear system (denoted by b) is only computed once, and the
+ * result is stored in vtemp3 so it can be reused - reduces the
+ * number of psovles if using left preconditioning
+ */
+ if ((r_curr_norm > delta) ||
+ (r_curr_norm >= r_init_norm && m == 1 && n == l_max)) {
+
+ /* Compute norm of residual ||b-A*x||_2 (preconditioned and scaled) */
+ if (scale_x) N_VDiv(x, sx, vtemp1);
+ else N_VScale(ONE, x, vtemp1);
+ if (preOnRight) {
+ ier = psolve(P_data, vtemp1, vtemp2, PREC_RIGHT);
+ (*nps)++;
+ if (ier != 0) return((ier < 0) ? SPTFQMR_PSOLVE_FAIL_UNREC : SPTFQMR_PSOLVE_FAIL_UNREC);
+ N_VScale(ONE, vtemp2, vtemp1);
+ }
+ ier = atimes(A_data, vtemp1, vtemp2);
+ if (ier != 0)
+ return((ier < 0) ? SPTFQMR_ATIMES_FAIL_UNREC : SPTFQMR_ATIMES_FAIL_REC);
+ if (preOnLeft) {
+ ier = psolve(P_data, vtemp2, vtemp1, PREC_LEFT);
+ (*nps)++;
+ if (ier != 0) return((ier < 0) ? SPTFQMR_PSOLVE_FAIL_UNREC : SPTFQMR_PSOLVE_FAIL_REC);
+ }
+ else N_VScale(ONE, vtemp2, vtemp1);
+ if (scale_b) N_VProd(sb, vtemp1, vtemp2);
+ else N_VScale(ONE, vtemp1, vtemp2);
+ /* Only precondition and scale b once (result saved for reuse) */
+ if (!b_ok) {
+ b_ok = TRUE;
+ if (preOnLeft) {
+ ier = psolve(P_data, b, vtemp3, PREC_LEFT);
+ (*nps)++;
+ if (ier != 0) return((ier < 0) ? SPTFQMR_PSOLVE_FAIL_UNREC : SPTFQMR_PSOLVE_FAIL_REC);
+ }
+ else N_VScale(ONE, b, vtemp3);
+ if (scale_b) N_VProd(sb, vtemp3, vtemp3);
+ }
+ N_VLinearSum(ONE, vtemp3, -ONE, vtemp2, vtemp1);
+ *res_norm = r_curr_norm = RSqrt(N_VDotProd(vtemp1, vtemp1));
+
+ /* Exit inner loop if inequality condition is satisfied
+ (meaning exit if we have converged) */
+ if (r_curr_norm <= delta) {
+ converged = TRUE;
+ break;
+ }
+
+ }
+
+ } /* END inner loop */
+
+ /* If converged, then exit outer loop as well */
+ if (converged == TRUE) break;
+
+ /* rho[1] = r_star^T*r_[1] */
+ rho[1] = N_VDotProd(r_star, r_[1]);
+
+ /* beta = rho[1]/rho[0] */
+ beta = rho[1]/rho[0];
+
+ /* u_ = r_[1]+beta*q_ */
+ N_VLinearSum(ONE, r_[1], beta, q_, u_);
+
+ /* p_ = u_+beta*(q_+beta*p_) */
+ N_VLinearSum(beta, q_, SQR(beta), p_, p_);
+ N_VLinearSum(ONE, u_, ONE, p_, p_);
+
+ /* v_ = A*p_ */
+ if (scale_x) N_VDiv(p_, sx, vtemp1);
+ else N_VScale(ONE, p_, vtemp1);
+ if (preOnRight) {
+ N_VScale(ONE, vtemp1, v_);
+ ier = psolve(P_data, v_, vtemp1, PREC_RIGHT);
+ (*nps)++;
+ if (ier != 0) return((ier < 0) ? SPTFQMR_PSOLVE_FAIL_UNREC : SPTFQMR_PSOLVE_FAIL_REC);
+ }
+ ier = atimes(A_data, vtemp1, v_);
+ if (ier != 0)
+ return((ier < 0) ? SPTFQMR_ATIMES_FAIL_UNREC : SPTFQMR_ATIMES_FAIL_REC);
+ if (preOnLeft) {
+ ier = psolve(P_data, v_, vtemp1, PREC_LEFT);
+ (*nps)++;
+ if (ier != 0) return((ier < 0) ? SPTFQMR_PSOLVE_FAIL_UNREC : SPTFQMR_PSOLVE_FAIL_REC);
+ }
+ else N_VScale(ONE, v_, vtemp1);
+ if (scale_b) N_VProd(sb, vtemp1, v_);
+ else N_VScale(ONE, vtemp1, v_);
+
+ /* Shift variable values */
+ /* NOTE: reduces storage requirements */
+ N_VScale(ONE, r_[1], r_[0]);
+ rho[0] = rho[1];
+
+ } /* END outer loop */
+
+ /* Determine return value */
+ /* If iteration converged or residual was reduced, then return current iterate (x) */
+ if ((converged == TRUE) || (r_curr_norm < r_init_norm)) {
+ if (scale_x) N_VDiv(x, sx, x);
+ if (preOnRight) {
+ ier = psolve(P_data, x, vtemp1, PREC_RIGHT);
+ (*nps)++;
+ if (ier != 0) return((ier < 0) ? SPTFQMR_PSOLVE_FAIL_UNREC : SPTFQMR_PSOLVE_FAIL_UNREC);
+ N_VScale(ONE, vtemp1, x);
+ }
+ if (converged == TRUE) return(SPTFQMR_SUCCESS);
+ else return(SPTFQMR_RES_REDUCED);
+ }
+ /* Otherwise, return error code */
+ else return(SPTFQMR_CONV_FAIL);
+}
+
+/*
+ * -----------------------------------------------------------------
+ * Function : SptfqmrFree
+ * -----------------------------------------------------------------
+ */
+
+void SptfqmrFree(SptfqmrMem mem)
+{
+
+ if (mem == NULL) return;
+
+ N_VDestroy(r_star);
+ N_VDestroy(q_);
+ N_VDestroy(d_);
+ N_VDestroy(v_);
+ N_VDestroy(p_);
+ N_VDestroyVectorArray(r_, 2);
+ N_VDestroy(u_);
+ N_VDestroy(vtemp1);
+ N_VDestroy(vtemp2);
+ N_VDestroy(vtemp3);
+
+ free(mem); mem = NULL;
+}
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/CMakeLists.txt b/SimTKmath/Integrators/src/CPodes/sundials/tests/CMakeLists.txt
new file mode 100644
index 0000000..58f2a2c
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/CMakeLists.txt
@@ -0,0 +1,62 @@
+# This set of tests was written by Radu Serban to test CPodes within
+# the Sundials framework. They do not use the SimTK C++ interface at all.
+#
+# Only static linking can be done here because the dynamic Simmath library
+# does not export internal cpodes functions.
+
+include_directories(
+ ${PROJECT_SOURCE_DIR}/Integrators/src/CPodes/sundials/include
+ ${SimTK_SDK}/include)
+
+# Adhoc tests are those test or demo programs which are not intended,
+# or not ready, to be part of the regression suite.
+#ADD_SUBDIRECTORY(adhoc)
+
+# Generate regression tests.
+#
+# This is boilerplate code for generating a set of executables, one per
+# .cpp file in a "tests" directory. These are for test cases which
+# have been engineered to be suitable as part of the nightly regression
+# test suite. Ideally, they know their correct answers and return
+# a simple thumbs-up/thumbs-down result, although some regressions
+# may be present and useful just to determine if they compile and
+# run to completion.
+#
+# For IDEs that can deal with PROJECT_LABEL properties (at least
+# Visual Studio) the projects for building each of these adhoc
+# executables will be labeled "Regr - TheTestName" if a file
+# TheTestName.cpp is found in this directory.
+#
+# We check the BUILD_TESTS_AND_EXAMPLES_{SHARED,STATIC} variables to determine
+# whether to build dynamically linked, statically linked, or both
+# versions of the executable.
+
+FILE(GLOB REGR_TESTS "*.c")
+FOREACH(TEST_PROG ${REGR_TESTS})
+ GET_FILENAME_COMPONENT(TEST_ROOT ${TEST_PROG} NAME_WE)
+
+ IF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+ # Link with shared library
+ ADD_EXECUTABLE(${TEST_ROOT} ${TEST_PROG})
+ SET_TARGET_PROPERTIES(${TEST_ROOT}
+ PROPERTIES
+ PROJECT_LABEL "Test_Regr - ${TEST_ROOT}")
+ TARGET_LINK_LIBRARIES(${TEST_ROOT}
+ ${TEST_SHARED_TARGET})
+ ADD_TEST(${TEST_ROOT} ${EXECUTABLE_OUTPUT_PATH}/${TEST_ROOT})
+ ENDIF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+
+ IF (BUILD_STATIC_LIBRARIES AND BUILD_TESTS_AND_EXAMPLES_STATIC)
+ # Link with static library
+ SET(TEST_STATIC ${TEST_ROOT}Static)
+ ADD_EXECUTABLE(${TEST_STATIC} ${TEST_PROG})
+ SET_TARGET_PROPERTIES(${TEST_STATIC}
+ PROPERTIES
+ COMPILE_FLAGS "-DSimTK_USE_STATIC_LIBRARIES"
+ PROJECT_LABEL "Test_Regr - ${TEST_STATIC}")
+ TARGET_LINK_LIBRARIES(${TEST_STATIC}
+ ${TEST_STATIC_TARGET})
+ ADD_TEST(${TEST_STATIC} ${EXECUTABLE_OUTPUT_PATH}/${TEST_STATIC})
+ ENDIF()
+
+ENDFOREACH(TEST_PROG ${REGR_TESTS})
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/README.txt b/SimTKmath/Integrators/src/CPodes/sundials/tests/README.txt
new file mode 100644
index 0000000..4d4ca11
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/README.txt
@@ -0,0 +1,22 @@
+List of serial CPODES examples
+
+ cpdenx : dense example
+ cvbanx : banded example
+ cpsadamsx : demonstration program for non-stiff problems
+ pend_test : demonstration program with and without projection
+ cpsdenx_lap : dense example using Lapack linear algebra
+ cpsbanx_lap : banded example using Lapack linear algebra
+ pendLr : pendulum example using Lapack linear algebra
+
+Sample results:
+
+ SUNDIALS was built with the following options:
+
+ ./configure CC=gcc F77=g77 CFLAGS="-g3 -O0" FFLAGS="-g3 -O0" --with-blas-lapack-libs="-L/home/radu/apps/lib -lSimTKlapack" --enable-examples
+
+ System Architecture: IA-32
+ Processor Type: Intel Pentium 4 Xeon DP (i686)
+ Operating System: Red Hat Enterprise Linux WS 3 (Taroon Update 7)
+ C/Fortran Compilers: gcc/gfortran v4.1.0
+
+ The SimTKlapack library provides ATLAS-tunned Blas and Lapack functions
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsAdvDiff_bnd.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsAdvDiff_bnd.c
new file mode 100644
index 0000000..5d1d793
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsAdvDiff_bnd.c
@@ -0,0 +1,265 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <cpodes/cpodes.h>
+#include <cpodes/cpodes_band.h>
+#include <nvector/nvector_serial.h>
+#include <sundials/sundials_math.h>
+
+/* Problem Constants */
+
+#define XMAX RCONST(2.0) /* domain boundaries */
+#define YMAX RCONST(1.0)
+#define MX 10 /* mesh dimensions */
+#define MY 5
+#define NEQ MX*MY /* number of equations */
+#define ATOL RCONST(1.0e-5) /* scalar absolute tolerance */
+#define T0 RCONST(0.0) /* initial time */
+#define T1 RCONST(0.1) /* first output time */
+#define DTOUT RCONST(0.1) /* output time increment */
+#define NOUT 10 /* number of output times */
+
+#define ZERO RCONST(0.0)
+#define HALF RCONST(0.5)
+#define ONE RCONST(1.0)
+#define TWO RCONST(2.0)
+#define FIVE RCONST(5.0)
+
+#define IJth(vdata,i,j) (vdata[(j-1) + (i-1)*MY])
+
+typedef struct {
+ realtype dx, dy, hdcoef, hacoef, vdcoef;
+} *UserData;
+
+/* Private Helper Functions */
+static void SetIC(N_Vector u, UserData data);
+static void PrintHeader(realtype reltol, realtype abstol, realtype umax);
+static void PrintOutput(realtype t, realtype umax, long int nst);
+static void PrintFinalStats(void *cvode_mem);
+
+/* Functions Called by the Solver */
+static int f(realtype t, N_Vector u, N_Vector udot, void *f_data);
+static int Jac(int N, int mu, int ml,
+ realtype t, N_Vector u, N_Vector fu,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+/*
+ *-------------------------------
+ * Main Program
+ *-------------------------------
+ */
+
+int main(void)
+{
+ realtype dx, dy, reltol, abstol, t, tout, umax;
+ N_Vector u, up;
+ UserData data;
+ void *cvode_mem;
+ int iout, flag;
+ long int nst;
+
+ u = NULL;
+ data = NULL;
+ cvode_mem = NULL;
+
+ u = N_VNew_Serial(NEQ);
+ up = N_VNew_Serial(NEQ);
+
+ reltol = ZERO;
+ abstol = ATOL;
+
+ data = (UserData) malloc(sizeof *data);
+ dx = data->dx = XMAX/(MX+1);
+ dy = data->dy = YMAX/(MY+1);
+ data->hdcoef = ONE/(dx*dx);
+ data->hacoef = HALF/(TWO*dx);
+ data->vdcoef = ONE/(dy*dy);
+
+ SetIC(u, data);
+
+ cvode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
+ flag = CPodeInit(cvode_mem, (void *)f, data, T0, u, NULL, CP_SS, reltol, &abstol);
+
+ flag = CPBand(cvode_mem, NEQ, MY, MY);
+ flag = CPDlsSetJacFn(cvode_mem, (void *)Jac, data);
+
+ /* In loop over output points: call CPode, print results, test for errors */
+ umax = N_VMaxNorm(u);
+ PrintHeader(reltol, abstol, umax);
+ for(iout=1, tout=T1; iout <= NOUT; iout++, tout += DTOUT) {
+ flag = CPode(cvode_mem, tout, &t, u, up, CP_NORMAL);
+ umax = N_VMaxNorm(u);
+ flag = CPodeGetNumSteps(cvode_mem, &nst);
+ PrintOutput(t, umax, nst);
+ }
+
+ PrintFinalStats(cvode_mem);
+
+ N_VDestroy_Serial(u);
+ CPodeFree(&cvode_mem);
+ free(data);
+
+ return(0);
+}
+
+/* f routine. Compute f(t,u). */
+static int f(realtype t, N_Vector u,N_Vector udot, void *f_data)
+{
+ realtype uij, udn, uup, ult, urt, hordc, horac, verdc, hdiff, hadv, vdiff;
+ realtype *udata, *dudata;
+ int i, j;
+ UserData data;
+
+ udata = NV_DATA_S(u);
+ dudata = NV_DATA_S(udot);
+
+ /* Extract needed constants from data */
+
+ data = (UserData) f_data;
+ hordc = data->hdcoef;
+ horac = data->hacoef;
+ verdc = data->vdcoef;
+
+ /* Loop over all grid points. */
+
+ for (j=1; j <= MY; j++) {
+
+ for (i=1; i <= MX; i++) {
+
+ /* Extract u at x_i, y_j and four neighboring points */
+
+ uij = IJth(udata, i, j);
+ udn = (j == 1) ? ZERO : IJth(udata, i, j-1);
+ uup = (j == MY) ? ZERO : IJth(udata, i, j+1);
+ ult = (i == 1) ? ZERO : IJth(udata, i-1, j);
+ urt = (i == MX) ? ZERO : IJth(udata, i+1, j);
+
+ /* Set diffusion and advection terms and load into udot */
+
+ hdiff = hordc*(ult - TWO*uij + urt);
+ hadv = horac*(urt - ult);
+ vdiff = verdc*(uup - TWO*uij + udn);
+ IJth(dudata, i, j) = hdiff + hadv + vdiff;
+ }
+ }
+
+ return(0);
+}
+
+/* Jacobian routine. Compute J(t,u). */
+static int Jac(int N, int mu, int ml,
+ realtype t, N_Vector u, N_Vector fu,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ long int i, j, k;
+ realtype *kthCol, hordc, horac, verdc;
+ UserData data;
+
+ /*
+ The components of f = udot that depend on u(i,j) are
+ f(i,j), f(i-1,j), f(i+1,j), f(i,j-1), f(i,j+1), with
+ df(i,j)/du(i,j) = -2 (1/dx^2 + 1/dy^2)
+ df(i-1,j)/du(i,j) = 1/dx^2 + .25/dx (if i > 1)
+ df(i+1,j)/du(i,j) = 1/dx^2 - .25/dx (if i < MX)
+ df(i,j-1)/du(i,j) = 1/dy^2 (if j > 1)
+ df(i,j+1)/du(i,j) = 1/dy^2 (if j < MY)
+ */
+
+ data = (UserData) jac_data;
+ hordc = data->hdcoef;
+ horac = data->hacoef;
+ verdc = data->vdcoef;
+
+ for (j=1; j <= MY; j++) {
+ for (i=1; i <= MX; i++) {
+ k = j-1 + (i-1)*MY;
+ kthCol = BAND_COL(J,k);
+
+ /* set the kth column of J */
+
+ BAND_COL_ELEM(kthCol,k,k) = -TWO*(verdc+hordc);
+ if (i != 1) BAND_COL_ELEM(kthCol,k-MY,k) = hordc + horac;
+ if (i != MX) BAND_COL_ELEM(kthCol,k+MY,k) = hordc - horac;
+ if (j != 1) BAND_COL_ELEM(kthCol,k-1,k) = verdc;
+ if (j != MY) BAND_COL_ELEM(kthCol,k+1,k) = verdc;
+ }
+ }
+
+ return(0);
+}
+
+
+/* Set initial conditions in u vector */
+static void SetIC(N_Vector u, UserData data)
+{
+ int i, j;
+ realtype x, y, dx, dy;
+ realtype *udata;
+
+ /* Extract needed constants from data */
+
+ dx = data->dx;
+ dy = data->dy;
+
+ /* Set pointer to data array in vector u. */
+
+ udata = NV_DATA_S(u);
+
+ /* Load initial profile into u vector */
+
+ for (j=1; j <= MY; j++) {
+ y = j*dy;
+ for (i=1; i <= MX; i++) {
+ x = i*dx;
+ IJth(udata,i,j) = x*(XMAX - x)*y*(YMAX - y)*EXP(FIVE*x*y);
+ }
+ }
+}
+
+/* Print first lines of output (problem description) */
+static void PrintHeader(realtype reltol, realtype abstol, realtype umax)
+{
+ printf("\n2-D Advection-Diffusion Equation\n");
+ printf("Mesh dimensions = %d X %d\n", MX, MY);
+ printf("Total system size = %d\n", NEQ);
+ printf("Tolerance parameters: reltol = %lg abstol = %lg\n\n", reltol, abstol);
+ printf("At t = %lg max.norm(u) =%14.6le \n", T0, umax);
+
+ return;
+}
+
+/* Print current value */
+static void PrintOutput(realtype t, realtype umax, long int nst)
+{
+ printf("At t = %4.2f max.norm(u) =%14.6le nst = %4ld\n", t, umax, nst);
+ return;
+}
+
+/* Get and print some final statistics */
+
+static void PrintFinalStats(void *cvode_mem)
+{
+ int flag;
+ long int nst, nfe, nsetups, netf, nni, ncfn, nje, nfeLS;
+
+ flag = CPodeGetNumSteps(cvode_mem, &nst);
+ flag = CPodeGetNumFctEvals(cvode_mem, &nfe);
+ flag = CPodeGetNumLinSolvSetups(cvode_mem, &nsetups);
+ flag = CPodeGetNumErrTestFails(cvode_mem, &netf);
+ flag = CPodeGetNumNonlinSolvIters(cvode_mem, &nni);
+ flag = CPodeGetNumNonlinSolvConvFails(cvode_mem, &ncfn);
+
+ flag = CPDlsGetNumJacEvals(cvode_mem, &nje);
+ flag = CPDlsGetNumFctEvals(cvode_mem, &nfeLS);
+
+ printf("\nFinal Statistics:\n");
+ printf("nst = %-6ld nfe = %-6ld nsetups = %-6ld nfeLS = %-6ld nje = %ld\n",
+ nst, nfe, nsetups, nfeLS, nje);
+ printf("nni = %-6ld ncfn = %-6ld netf = %ld\n \n",
+ nni, ncfn, netf);
+
+ return;
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsAdvDiff_bndL.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsAdvDiff_bndL.c
new file mode 100644
index 0000000..b877b88
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsAdvDiff_bndL.c
@@ -0,0 +1,265 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <cpodes/cpodes.h>
+#include <cpodes/cpodes_lapack.h>
+#include <nvector/nvector_serial.h>
+#include <sundials/sundials_math.h>
+
+/* Problem Constants */
+
+#define XMAX RCONST(2.0) /* domain boundaries */
+#define YMAX RCONST(1.0)
+#define MX 10 /* mesh dimensions */
+#define MY 5
+#define NEQ MX*MY /* number of equations */
+#define ATOL RCONST(1.0e-5) /* scalar absolute tolerance */
+#define T0 RCONST(0.0) /* initial time */
+#define T1 RCONST(0.1) /* first output time */
+#define DTOUT RCONST(0.1) /* output time increment */
+#define NOUT 10 /* number of output times */
+
+#define ZERO RCONST(0.0)
+#define HALF RCONST(0.5)
+#define ONE RCONST(1.0)
+#define TWO RCONST(2.0)
+#define FIVE RCONST(5.0)
+
+#define IJth(vdata,i,j) (vdata[(j-1) + (i-1)*MY])
+
+typedef struct {
+ realtype dx, dy, hdcoef, hacoef, vdcoef;
+} *UserData;
+
+/* Private Helper Functions */
+static void SetIC(N_Vector u, UserData data);
+static void PrintHeader(realtype reltol, realtype abstol, realtype umax);
+static void PrintOutput(realtype t, realtype umax, long int nst);
+static void PrintFinalStats(void *cvode_mem);
+
+/* Functions Called by the Solver */
+static int f(realtype t, N_Vector u, N_Vector udot, void *f_data);
+static int Jac(int N, int mu, int ml,
+ realtype t, N_Vector u, N_Vector fu,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+/*
+ *-------------------------------
+ * Main Program
+ *-------------------------------
+ */
+
+int main(void)
+{
+ realtype dx, dy, reltol, abstol, t, tout, umax;
+ N_Vector u, up;
+ UserData data;
+ void *cvode_mem;
+ int iout, flag;
+ long int nst;
+
+ u = NULL;
+ data = NULL;
+ cvode_mem = NULL;
+
+ u = N_VNew_Serial(NEQ);
+ up = N_VNew_Serial(NEQ);
+
+ reltol = ZERO;
+ abstol = ATOL;
+
+ data = (UserData) malloc(sizeof *data);
+ dx = data->dx = XMAX/(MX+1);
+ dy = data->dy = YMAX/(MY+1);
+ data->hdcoef = ONE/(dx*dx);
+ data->hacoef = HALF/(TWO*dx);
+ data->vdcoef = ONE/(dy*dy);
+
+ SetIC(u, data);
+
+ cvode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
+ flag = CPodeInit(cvode_mem, (void *)f, data, T0, u, NULL, CP_SS, reltol, &abstol);
+
+ flag = CPLapackBand(cvode_mem, NEQ, MY, MY);
+ flag = CPDlsSetJacFn(cvode_mem, (void *)Jac, data);
+
+ /* In loop over output points: call CPode, print results, test for errors */
+ umax = N_VMaxNorm(u);
+ PrintHeader(reltol, abstol, umax);
+ for(iout=1, tout=T1; iout <= NOUT; iout++, tout += DTOUT) {
+ flag = CPode(cvode_mem, tout, &t, u, up, CP_NORMAL);
+ umax = N_VMaxNorm(u);
+ flag = CPodeGetNumSteps(cvode_mem, &nst);
+ PrintOutput(t, umax, nst);
+ }
+
+ PrintFinalStats(cvode_mem);
+
+ N_VDestroy_Serial(u);
+ CPodeFree(&cvode_mem);
+ free(data);
+
+ return(0);
+}
+
+/* f routine. Compute f(t,u). */
+static int f(realtype t, N_Vector u,N_Vector udot, void *f_data)
+{
+ realtype uij, udn, uup, ult, urt, hordc, horac, verdc, hdiff, hadv, vdiff;
+ realtype *udata, *dudata;
+ int i, j;
+ UserData data;
+
+ udata = NV_DATA_S(u);
+ dudata = NV_DATA_S(udot);
+
+ /* Extract needed constants from data */
+
+ data = (UserData) f_data;
+ hordc = data->hdcoef;
+ horac = data->hacoef;
+ verdc = data->vdcoef;
+
+ /* Loop over all grid points. */
+
+ for (j=1; j <= MY; j++) {
+
+ for (i=1; i <= MX; i++) {
+
+ /* Extract u at x_i, y_j and four neighboring points */
+
+ uij = IJth(udata, i, j);
+ udn = (j == 1) ? ZERO : IJth(udata, i, j-1);
+ uup = (j == MY) ? ZERO : IJth(udata, i, j+1);
+ ult = (i == 1) ? ZERO : IJth(udata, i-1, j);
+ urt = (i == MX) ? ZERO : IJth(udata, i+1, j);
+
+ /* Set diffusion and advection terms and load into udot */
+
+ hdiff = hordc*(ult - TWO*uij + urt);
+ hadv = horac*(urt - ult);
+ vdiff = verdc*(uup - TWO*uij + udn);
+ IJth(dudata, i, j) = hdiff + hadv + vdiff;
+ }
+ }
+
+ return(0);
+}
+
+/* Jacobian routine. Compute J(t,u). */
+static int Jac(int N, int mu, int ml,
+ realtype t, N_Vector u, N_Vector fu,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ int i, j, k;
+ realtype *kthCol, hordc, horac, verdc;
+ UserData data;
+
+ /*
+ The components of f = udot that depend on u(i,j) are
+ f(i,j), f(i-1,j), f(i+1,j), f(i,j-1), f(i,j+1), with
+ df(i,j)/du(i,j) = -2 (1/dx^2 + 1/dy^2)
+ df(i-1,j)/du(i,j) = 1/dx^2 + .25/dx (if i > 1)
+ df(i+1,j)/du(i,j) = 1/dx^2 - .25/dx (if i < MX)
+ df(i,j-1)/du(i,j) = 1/dy^2 (if j > 1)
+ df(i,j+1)/du(i,j) = 1/dy^2 (if j < MY)
+ */
+
+ data = (UserData) jac_data;
+ hordc = data->hdcoef;
+ horac = data->hacoef;
+ verdc = data->vdcoef;
+
+ for (j=1; j <= MY; j++) {
+ for (i=1; i <= MX; i++) {
+ k = j-1 + (i-1)*MY;
+ kthCol = BAND_COL(J,k);
+
+ /* set the kth column of J */
+
+ BAND_COL_ELEM(kthCol,k,k) = -TWO*(verdc+hordc);
+ if (i != 1) BAND_COL_ELEM(kthCol,k-MY,k) = hordc + horac;
+ if (i != MX) BAND_COL_ELEM(kthCol,k+MY,k) = hordc - horac;
+ if (j != 1) BAND_COL_ELEM(kthCol,k-1,k) = verdc;
+ if (j != MY) BAND_COL_ELEM(kthCol,k+1,k) = verdc;
+ }
+ }
+
+ return(0);
+}
+
+
+/* Set initial conditions in u vector */
+static void SetIC(N_Vector u, UserData data)
+{
+ int i, j;
+ realtype x, y, dx, dy;
+ realtype *udata;
+
+ /* Extract needed constants from data */
+
+ dx = data->dx;
+ dy = data->dy;
+
+ /* Set pointer to data array in vector u. */
+
+ udata = NV_DATA_S(u);
+
+ /* Load initial profile into u vector */
+
+ for (j=1; j <= MY; j++) {
+ y = j*dy;
+ for (i=1; i <= MX; i++) {
+ x = i*dx;
+ IJth(udata,i,j) = x*(XMAX - x)*y*(YMAX - y)*EXP(FIVE*x*y);
+ }
+ }
+}
+
+/* Print first lines of output (problem description) */
+static void PrintHeader(realtype reltol, realtype abstol, realtype umax)
+{
+ printf("\n2-D Advection-Diffusion Equation\n");
+ printf("Mesh dimensions = %d X %d\n", MX, MY);
+ printf("Total system size = %d\n", NEQ);
+ printf("Tolerance parameters: reltol = %lg abstol = %lg\n\n", reltol, abstol);
+ printf("At t = %lg max.norm(u) =%14.6le \n", T0, umax);
+
+ return;
+}
+
+/* Print current value */
+static void PrintOutput(realtype t, realtype umax, long int nst)
+{
+ printf("At t = %4.2f max.norm(u) =%14.6le nst = %4ld\n", t, umax, nst);
+ return;
+}
+
+/* Get and print some final statistics */
+
+static void PrintFinalStats(void *cvode_mem)
+{
+ int flag;
+ long int nst, nfe, nsetups, netf, nni, ncfn, nje, nfeLS;
+
+ flag = CPodeGetNumSteps(cvode_mem, &nst);
+ flag = CPodeGetNumFctEvals(cvode_mem, &nfe);
+ flag = CPodeGetNumLinSolvSetups(cvode_mem, &nsetups);
+ flag = CPodeGetNumErrTestFails(cvode_mem, &netf);
+ flag = CPodeGetNumNonlinSolvIters(cvode_mem, &nni);
+ flag = CPodeGetNumNonlinSolvConvFails(cvode_mem, &ncfn);
+
+ flag = CPDlsGetNumJacEvals(cvode_mem, &nje);
+ flag = CPDlsGetNumFctEvals(cvode_mem, &nfeLS);
+
+ printf("\nFinal Statistics:\n");
+ printf("nst = %-6ld nfe = %-6ld nsetups = %-6ld nfeLS = %-6ld nje = %ld\n",
+ nst, nfe, nsetups, nfeLS, nje);
+ printf("nni = %-6ld ncfn = %-6ld netf = %ld\n \n",
+ nni, ncfn, netf);
+
+ return;
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsAdvDiff_non.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsAdvDiff_non.c
new file mode 100644
index 0000000..18a3567
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsAdvDiff_non.c
@@ -0,0 +1,187 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.1 $
+ * $Date: 2007/10/25 20:03:25 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Example problem:
+ * ydot = A * y, where A is a banded lower triangular
+ * matrix derived from 2-D advection PDE.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <cpodes/cpodes.h>
+#include <nvector/nvector_serial.h>
+#include <sundials/sundials_math.h>
+
+
+#define ODE CP_EXPL
+
+/* Problem Constants */
+
+#define ATOL RCONST(1.0e-6) /* 1.0e-6 */
+#define RTOL RCONST(0.0) /* 0.0 */
+
+#define ZERO RCONST(0.0)
+#define ONE RCONST(1.0)
+#define TWO RCONST(2.0)
+#define THIRTY RCONST(30.0)
+
+#define P2_MESHX 5
+#define P2_MESHY 5
+#define P2_NEQ P2_MESHX*P2_MESHY
+#define P2_ALPH1 RCONST(1.0)
+#define P2_ALPH2 RCONST(1.0)
+#define P2_NOUT 5
+#define P2_ML 5
+#define P2_MU 0
+#define P2_T0 RCONST(0.0)
+#define P2_T1 RCONST(0.01)
+#define P2_TOUT_MULT RCONST(10.0)
+
+/* Private Helper Functions */
+static realtype MaxError(N_Vector y, realtype t);
+static void PrintFinalStats(void *cpode_mem);
+
+/* Functions Called by the Solver */
+static int res(realtype t, N_Vector y, N_Vector yp, N_Vector res, void *f_data);
+static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data);
+
+
+int main()
+{
+ void *fct;
+ void *cpode_mem;
+ N_Vector y, yp;
+ realtype reltol=RTOL, abstol=ATOL, t, tout, erm, hu;
+ int flag, iout, qu;
+
+ y = NULL;
+ yp = NULL;
+ cpode_mem = NULL;
+
+ y = N_VNew_Serial(P2_NEQ);
+ N_VConst(ZERO, y);
+ NV_Ith_S(y,0) = ONE;
+
+ yp = N_VNew_Serial(P2_NEQ);
+
+ if (ODE == CP_EXPL) {
+ fct = (void *)f;
+ } else {
+ fct = (void *)res;
+ f(P2_T0, y, yp, NULL);
+ }
+
+ cpode_mem = CPodeCreate(ODE, CP_ADAMS, CP_FUNCTIONAL);
+ /* flag = CPodeSetInitStep(cpode_mem, 2.0e-9);*/
+ flag = CPodeInit(cpode_mem, fct, NULL, P2_T0, y, yp, CP_SS, reltol, &abstol);
+
+ printf("\n t max.err qu hu \n");
+ for(iout=1, tout=P2_T1; iout <= P2_NOUT; iout++, tout*=P2_TOUT_MULT) {
+ flag = CPode(cpode_mem, tout, &t, y, yp, CP_NORMAL);
+ if (flag != CP_SUCCESS) break;
+ erm = MaxError(y, t);
+ flag = CPodeGetLastOrder(cpode_mem, &qu);
+ flag = CPodeGetLastStep(cpode_mem, &hu);
+ printf("%10.3f %12.4le %2d %12.4le\n", t, erm, qu, hu);
+ }
+
+ PrintFinalStats(cpode_mem);
+
+ CPodeFree(&cpode_mem);
+ N_VDestroy_Serial(y);
+ N_VDestroy_Serial(yp);
+
+ return 0;
+}
+
+static int res(realtype t, N_Vector y, N_Vector yp, N_Vector res, void *f_data)
+{
+ f(t,y,res,f_data);
+ N_VLinearSum(1.0, yp, -1.0, res, res);
+ return(0);
+}
+
+static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data)
+{
+ long int i, j, k;
+ realtype d, *ydata, *dydata;
+
+ ydata = NV_DATA_S(y);
+ dydata = NV_DATA_S(ydot);
+
+ /*
+ Excluding boundaries,
+
+ ydot = f = -2 y + alpha1 * y + alpha2 * y
+ i,j i,j i,j i-1,j i,j-1
+ */
+
+ for (j=0; j < P2_MESHY; j++) {
+ for (i=0; i < P2_MESHX; i++) {
+ k = i + j * P2_MESHX;
+ d = -TWO*ydata[k];
+ if (i != 0) d += P2_ALPH1 * ydata[k-1];
+ if (j != 0) d += P2_ALPH2 * ydata[k-P2_MESHX];
+ dydata[k] = d;
+ }
+ }
+
+ return(0);
+}
+
+static realtype MaxError(N_Vector y, realtype t)
+{
+ long int i, j, k;
+ realtype *ydata, er, ex=ZERO, yt, maxError=ZERO, ifact_inv, jfact_inv=ONE;
+
+ if (t == ZERO) return(ZERO);
+
+ ydata = NV_DATA_S(y);
+ if (t <= THIRTY) ex = EXP(-TWO*t);
+
+ for (j = 0; j < P2_MESHY; j++) {
+ ifact_inv = ONE;
+ for (i = 0; i < P2_MESHX; i++) {
+ k = i + j * P2_MESHX;
+ yt = RPowerI(t,i+j) * ex * ifact_inv * jfact_inv;
+ er = ABS(ydata[k] - yt);
+ if (er > maxError) maxError = er;
+ ifact_inv /= (i+1);
+ }
+ jfact_inv /= (j+1);
+ }
+ return(maxError);
+}
+
+
+
+static void PrintFinalStats(void *cpode_mem)
+{
+ long int nst, nfe, nni, ncfn, netf;
+ realtype h0u;
+ int flag;
+
+ flag = CPodeGetActualInitStep(cpode_mem, &h0u);
+ flag = CPodeGetNumSteps(cpode_mem, &nst);
+ flag = CPodeGetNumFctEvals(cpode_mem, &nfe);
+ flag = CPodeGetNumErrTestFails(cpode_mem, &netf);
+ flag = CPodeGetNumNonlinSolvIters(cpode_mem, &nni);
+ flag = CPodeGetNumNonlinSolvConvFails(cpode_mem, &ncfn);
+
+ printf("\n Final statistics:\n\n");
+ printf(" Number of steps = %4ld \n", nst);
+ printf(" Number of f-s = %4ld \n", nfe);
+ printf(" Number of nonlinear iterations = %4ld \n", nni);
+ printf(" Number of nonlinear convergence failures = %4ld \n", ncfn);
+ printf(" Number of error test failures = %4ld \n", netf);
+ printf(" Initial step size = %g \n\n", h0u);
+
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsNewtCrd_dns.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsNewtCrd_dns.c
new file mode 100644
index 0000000..4b0784a
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsNewtCrd_dns.c
@@ -0,0 +1,493 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.1 $
+ * $Date: 2007/10/25 20:03:25 $
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Example problem for the event detection in CPODES: a 2-pendulum
+ * Newton's craddle. Two identical pendulums of length L and
+ * mass M are suspended at points (+R,0) and (-R,0), where R is the
+ * radius of the balls.
+ * We consider elastic impact with a coefficient of restitution C=1.
+ *
+ * Case 1: The first pendulum has an initial horizontal
+ * velocity V0, while the second one is at rest.
+ * Case 2: Both pendulums are initially at rest. The first one is
+ * then moved from rest after a short interval, by applying
+ * an external force, F = m*frc(t), where
+ * frc(t) = 3(1-cos(5t)) on t =[0,2*pi/5] and 0 otherwise.
+ *
+ * Each pendulum is modeled with 2 coordinates (x and y) and one
+ * constraint (x^2 + y^2 = L^2), resulting in a 1st order system
+ * with 8 diferential equations and 4 constraints.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <cpodes/cpodes.h>
+#include <cpodes/cpodes_dense.h>
+#include <nvector/nvector_serial.h>
+
+/* User data structure */
+
+typedef struct {
+ realtype R;
+ realtype L;
+ realtype m;
+ realtype g;
+} *PbData;
+
+/* Functions Called by the Solver */
+
+static int ffun1(realtype t, N_Vector y, N_Vector ydot, void *f_data);
+static int ffun2(realtype t, N_Vector y, N_Vector ydot, void *f_data);
+
+static int gfun(realtype t, N_Vector y, N_Vector yp, realtype *gout, void *g_data);
+
+static int cfun(realtype t, N_Vector yy, N_Vector cout, void *c_data);
+
+static void contact(N_Vector yy, PbData data);
+
+static void PrintFinalStats(void *cpode_mem);
+
+/*
+ *-------------------------------
+ * Main Program
+ *-------------------------------
+ */
+
+int main()
+{
+ PbData data;
+ realtype R, L, m, g, V0;
+ void *cpode_mem;
+ N_Vector yy, yp, ctols;
+ realtype reltol, abstol;
+ realtype t0, tf, t;
+ realtype x1, y1, x2, y2;
+ realtype vx1, vy1, vx2, vy2;
+ int flag, Neq, Nc;
+ int rdir[1], iroots[1];
+
+ FILE *fout;
+
+ /* --------------------------------
+ * INITIALIZATIONS
+ * -------------------------------- */
+
+ R = 0.1; /* ball radius */
+ L = 1.0; /* pendulum length */
+ m = 1.0; /* pendulum mass */
+ g = 9.8; /* gravitational acc. */
+
+ V0 = 3.0; /* initial velocity for pendulum 1 */
+
+ /* Set-up user data structure */
+
+ data = (PbData)malloc(sizeof *data);
+ data->R = R;
+ data->L = L;
+ data->m = m;
+ data->g = g;
+
+ /* Problem dimensions */
+
+ Neq = 2*2*2;
+ Nc = 2*2;
+
+ /* Solution vectors */
+
+ yy = N_VNew_Serial(Neq);
+ yp = N_VNew_Serial(Neq);
+
+ /* Integration limits */
+
+ t0 = 0.0;
+ tf = 6.0;
+
+ /* Integration and projection tolerances */
+
+ reltol = 1.0e-8;
+ abstol = 1.0e-8;
+
+ ctols = N_VNew_Serial(Nc);
+ N_VConst(1.0e-8, ctols);
+
+ /* Direction of monitored events
+ * (only zero-crossing with decreasing even function) */
+
+ rdir[0] = -1;
+
+ /* --------------------------------
+ * CASE 1
+ * -------------------------------- */
+
+ fout = fopen("newton1.out","w");
+
+ /* Initial conditions */
+
+ NV_Ith_S(yy,0) = R; NV_Ith_S(yy,4) = -R;
+ NV_Ith_S(yy,1) = -L; NV_Ith_S(yy,5) = -L;
+ NV_Ith_S(yy,2) = V0; NV_Ith_S(yy,6) = 0.0;
+ NV_Ith_S(yy,3) = 0.0; NV_Ith_S(yy,7) = 0.0;
+
+ /* Initialize solver */
+
+ cpode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
+ flag = CPodeInit(cpode_mem, ffun1, data, t0, yy, yp, CP_SS, reltol, &abstol);
+ flag = CPDense(cpode_mem, Neq);
+
+ flag = CPodeRootInit(cpode_mem, 1, gfun, data);
+ flag = CPodeSetRootDirection(cpode_mem, rdir);
+
+ /* Set-up the internal projection */
+
+ flag = CPodeProjInit(cpode_mem, CP_PROJ_L2NORM, CP_CNSTR_NONLIN, cfun, data, ctols);
+ flag = CPodeSetProjTestCnstr(cpode_mem, TRUE);
+ flag = CPDenseProj(cpode_mem, Nc, Neq, CPDIRECT_LU);
+
+ /* Integrate in ONE_STEP mode, while monitoring events */
+
+ t = t0;
+ while(t<tf) {
+
+ flag = CPode(cpode_mem, tf, &t, yy, yp, CP_ONE_STEP);
+
+ if (flag < 0) break;
+
+ x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
+ y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
+ vx1 = NV_Ith_S(yy,2); vx2 = NV_Ith_S(yy,6);
+ vy1 = NV_Ith_S(yy,3); vy2 = NV_Ith_S(yy,7);
+
+ fprintf(fout, "%lf %14.10lf %14.10lf %14.10lf %14.10lf %14.10lf %14.10lf %14.10lf %14.10lf",
+ t, x1, y1, x2, y2, vx1, vy1, vx2, vy2);
+
+ if (flag == CP_ROOT_RETURN) {
+
+ CPodeGetRootInfo(cpode_mem, iroots);
+ fprintf(fout, " %d\n", iroots[0]);
+
+ /* Note: the test iroots[0]<0 is really needed ONLY if not using rdir */
+
+ if (iroots[0] < 0) {
+ /* Update velocities in yy */
+ contact(yy, data);
+ /* reinitialize CPODES solver */
+ flag = CPodeReInit(cpode_mem, ffun1, data, t, yy, yp, CP_SS, reltol, &abstol);
+ }
+
+ } else {
+
+ fprintf(fout, " 0\n");
+
+ }
+
+ }
+
+ PrintFinalStats(cpode_mem);
+
+ CPodeFree(&cpode_mem);
+
+ fclose(fout);
+
+ /* --------------------------------
+ * CASE 2
+ * -------------------------------- */
+
+ fout = fopen("newton2.out","w");
+
+ /* Initial conditions */
+
+ NV_Ith_S(yy,0) = R; NV_Ith_S(yy,4) = -R;
+ NV_Ith_S(yy,1) = -L; NV_Ith_S(yy,5) = -L;
+ NV_Ith_S(yy,2) = 0.0; NV_Ith_S(yy,6) = 0.0;
+ NV_Ith_S(yy,3) = 0.0; NV_Ith_S(yy,7) = 0.0;
+
+ /* Initialize solver */
+
+ cpode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
+ flag = CPodeInit(cpode_mem, ffun2, data, t0, yy, yp, CP_SS, reltol, &abstol);
+ flag = CPDense(cpode_mem, Neq);
+
+ flag = CPodeRootInit(cpode_mem, 1, gfun, data);
+ flag = CPodeSetRootDirection(cpode_mem, rdir);
+
+ /* Set-up the internal projection */
+
+ flag = CPodeProjInit(cpode_mem, CP_PROJ_L2NORM, CP_CNSTR_NONLIN, cfun, data, ctols);
+ flag = CPodeSetProjTestCnstr(cpode_mem, TRUE);
+ flag = CPDenseProj(cpode_mem, Nc, Neq, CPDIRECT_LU);
+
+ /* Integrate in ONE_STEP mode, while monitoring events */
+
+ t = t0;
+ while(t<tf) {
+
+ flag = CPode(cpode_mem, tf, &t, yy, yp, CP_ONE_STEP);
+
+ if (flag < 0) break;
+
+ x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
+ y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
+ vx1 = NV_Ith_S(yy,2); vx2 = NV_Ith_S(yy,6);
+ vy1 = NV_Ith_S(yy,3); vy2 = NV_Ith_S(yy,7);
+
+ fprintf(fout, "%lf %14.10lf %14.10lf %14.10lf %14.10lf %14.10lf %14.10lf %14.10lf %14.10lf",
+ t, x1, y1, x2, y2, vx1, vy1, vx2, vy2);
+
+ if (flag == CP_ROOT_RETURN) {
+
+ CPodeGetRootInfo(cpode_mem, iroots);
+ fprintf(fout, " %d\n", iroots[0]);
+
+ /* Note: the test iroots[0]<0 is really needed ONLY if not using rdir */
+
+ if (iroots[0] < 0) {
+ /* Update velocities in yy */
+ contact(yy, data);
+ /* reinitialize CPODES solver */
+ flag = CPodeReInit(cpode_mem, ffun2, data, t, yy, yp, CP_SS, reltol, &abstol);
+ }
+
+ } else {
+
+ fprintf(fout, " 0\n");
+
+ }
+
+ }
+
+ PrintFinalStats(cpode_mem);
+
+ CPodeFree(&cpode_mem);
+
+ fclose(fout);
+
+ /* --------------------------------
+ * CLEAN-UP
+ * -------------------------------- */
+
+ free(data);
+ N_VDestroy_Serial(yy);
+ N_VDestroy_Serial(yp);
+ N_VDestroy_Serial(ctols);
+
+ return(0);
+}
+
+
+static int ffun1(realtype t, N_Vector yy, N_Vector fy, void *f_data)
+{
+ PbData data;
+ realtype x1, y1, x2, y2;
+ realtype vx1, vy1, vx2, vy2;
+ realtype ax1, ay1, ax2, ay2;
+ realtype lam1, lam2;
+ realtype R, L, m, g;
+
+ data = (PbData) f_data;
+ R = data->R;
+ L = data->L;
+ m = data->m;
+ g = data->g;
+
+ x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
+ y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
+ vx1 = NV_Ith_S(yy,2); vx2 = NV_Ith_S(yy,6);
+ vy1 = NV_Ith_S(yy,3); vy2 = NV_Ith_S(yy,7);
+
+ lam1 = m/(2*L*L)*(vx1*vx1 + vy1*vy1 - g*y1);
+ lam2 = m/(2*L*L)*(vx2*vx2 + vy2*vy2 - g*y2);
+
+ ax1 = -2.0*(x1-R)*lam1/m;
+ ay1 = -2.0*y1*lam1/m - g;
+
+ ax2 = -2.0*(x2+R)*lam2/m;
+ ay2 = -2.0*y2*lam2/m - g;
+
+ NV_Ith_S(fy,0) = vx1; NV_Ith_S(fy, 4) = vx2;
+ NV_Ith_S(fy,1) = vy1; NV_Ith_S(fy, 5) = vy2;
+ NV_Ith_S(fy,2) = ax1; NV_Ith_S(fy, 6) = ax2;
+ NV_Ith_S(fy,3) = ay1; NV_Ith_S(fy, 7) = ay2;
+
+ return(0);
+}
+
+static int ffun2(realtype t, N_Vector yy, N_Vector fy, void *f_data)
+{
+ PbData data;
+ realtype x1, y1, x2, y2;
+ realtype vx1, vy1, vx2, vy2;
+ realtype ax1, ay1, ax2, ay2;
+ realtype lam1, lam2;
+ realtype R, L, m, g;
+ realtype pi, frc;
+
+
+ pi = 4.0*atan(1.0);
+ if ( (t <= 2*pi/5.0) ) {
+ frc = 3.0 * ( 1.0 - cos(5.0*t) );
+ } else {
+ frc = 0.0;
+ }
+
+ data = (PbData) f_data;
+ R = data->R;
+ L = data->L;
+ m = data->m;
+ g = data->g;
+
+ x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
+ y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
+ vx1 = NV_Ith_S(yy,2); vx2 = NV_Ith_S(yy,6);
+ vy1 = NV_Ith_S(yy,3); vy2 = NV_Ith_S(yy,7);
+
+ lam1 = m/(2*L*L)*(vx1*vx1 + vy1*vy1 + frc*(x1-R) - g*y1);
+ lam2 = m/(2*L*L)*(vx2*vx2 + vy2*vy2 - g*y2);
+
+ ax1 = -2.0*(x1-R)*lam1/m + frc;
+ ay1 = -2.0*y1*lam1/m - g;
+
+ ax2 = -2.0*(x2+R)*lam2/m;
+ ay2 = -2.0*y2*lam2/m - g;
+
+ NV_Ith_S(fy,0) = vx1; NV_Ith_S(fy, 4) = vx2;
+ NV_Ith_S(fy,1) = vy1; NV_Ith_S(fy, 5) = vy2;
+ NV_Ith_S(fy,2) = ax1; NV_Ith_S(fy, 6) = ax2;
+ NV_Ith_S(fy,3) = ay1; NV_Ith_S(fy, 7) = ay2;
+
+ return(0);
+}
+
+static int cfun(realtype t, N_Vector yy, N_Vector c, void *c_data)
+{
+ PbData data;
+ realtype x1, y1, x2, y2;
+ realtype vx1, vy1, vx2, vy2;
+ realtype R, L;
+
+ data = (PbData) c_data;
+ R = data->R;
+ L = data->L;
+
+ x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
+ y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
+ vx1 = NV_Ith_S(yy,2); vx2 = NV_Ith_S(yy,6);
+ vy1 = NV_Ith_S(yy,3); vy2 = NV_Ith_S(yy,7);
+
+ NV_Ith_S(c,0) = (x1-R)*(x1-R) + y1*y1 - L*L;
+ NV_Ith_S(c,1) = (x1-R)*vx1 + y1*vy1;
+
+ NV_Ith_S(c,2) = (x2+R)*(x2+R) + y2*y2 - L*L;
+ NV_Ith_S(c,3) = (x2+R)*vx2 + y2*vy2;
+
+ return(0);
+}
+
+static int gfun(realtype t, N_Vector yy, N_Vector yp, realtype *gval, void *g_data)
+{
+ PbData data;
+ realtype x1, y1, x2, y2;
+ realtype R, L, m, g;
+
+ data = (PbData) g_data;
+ R = data->R;
+ L = data->L;
+ m = data->m;
+ g = data->g;
+
+ x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
+ y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
+
+ gval[0] = (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2) - 4.0*R*R;
+
+ return(0);
+}
+
+
+static void contact(N_Vector yy, PbData data)
+{
+ realtype x1, y1, x2, y2;
+ realtype vx1, vy1, vx2, vy2;
+ realtype vt1, vn1, vn1_, vt2, vn2, vn2_;
+ realtype alpha, ca, sa;
+
+ x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
+ y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
+ vx1 = NV_Ith_S(yy,2); vx2 = NV_Ith_S(yy,6);
+ vy1 = NV_Ith_S(yy,3); vy2 = NV_Ith_S(yy,7);
+
+ /* Angle of contact line */
+
+ alpha = atan2(y2-y1, x2-x1);
+ ca = cos(alpha);
+ sa = sin(alpha);
+
+ /* Normal and tangential velocities before impact
+ * (rotate velocity vectors by +alpha) */
+
+ vn1 = ca*vx1 + sa*vy1;
+ vt1 = -sa*vx1 + ca*vy1;
+
+ vn2 = ca*vx2 + sa*vy2;
+ vt2 = -sa*vx2 + ca*vy2;
+
+ /* New normal velocities (M1=M2 and COR=1.0) */
+
+ vn1_ = vn2;
+ vn2_ = vn1;
+
+ vn1 = vn1_;
+ vn2 = vn2_;
+
+ /* Velocities after impact (rotate back by -alpha) */
+
+ vx1 = ca*vn1 - sa*vt1;
+ vy1 = sa*vn1 + ca*vt1;
+
+ vx2 = ca*vn2 - sa*vt2;
+ vy2 = sa*vn2 + ca*vt2;
+
+ NV_Ith_S(yy,2) = vx1; NV_Ith_S(yy,6) = vx2;
+ NV_Ith_S(yy,3) = vy1; NV_Ith_S(yy,7) = vy2;
+
+ return;
+}
+
+static void PrintFinalStats(void *cpode_mem)
+{
+ realtype h0u;
+ long int nst, nfe, nsetups, nje, nfeLS, nni, ncfn, netf;
+ long int nproj, nce, nsetupsP, nprf;
+ int flag;
+
+ flag = CPodeGetActualInitStep(cpode_mem, &h0u);
+ flag = CPodeGetNumSteps(cpode_mem, &nst);
+ flag = CPodeGetNumFctEvals(cpode_mem, &nfe);
+ flag = CPodeGetNumLinSolvSetups(cpode_mem, &nsetups);
+ flag = CPodeGetNumErrTestFails(cpode_mem, &netf);
+ flag = CPodeGetNumNonlinSolvIters(cpode_mem, &nni);
+ flag = CPodeGetNumNonlinSolvConvFails(cpode_mem, &ncfn);
+
+ flag = CPDlsGetNumJacEvals(cpode_mem, &nje);
+ flag = CPDlsGetNumFctEvals(cpode_mem, &nfeLS);
+
+ flag = CPodeGetProjStats(cpode_mem, &nproj, &nce, &nsetupsP, &nprf);
+
+ printf("\nFinal Statistics:\n");
+ printf("h0u = %g\n",h0u);
+ printf("nst = %-6ld nfe = %-6ld nsetups = %-6ld\n",
+ nst, nfe, nsetups);
+ printf("nfeLS = %-6ld nje = %ld\n",
+ nfeLS, nje);
+ printf("nni = %-6ld ncfn = %-6ld netf = %-6ld \n",
+ nni, ncfn, netf);
+ printf("nproj = %-6ld nce = %-6ld nsetupsP = %-6ld nprf = %-6ld\n",
+ nproj, nce, nsetupsP, nprf);
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsPend_dns.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsPend_dns.c
new file mode 100644
index 0000000..6483afe
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsPend_dns.c
@@ -0,0 +1,265 @@
+#include <stdio.h>
+#include <math.h>
+
+#include <cpodes/cpodes.h>
+#include <cpodes/cpodes_dense.h>
+#include <nvector/nvector_serial.h>
+#include <sundials/sundials_math.h>
+
+#define Ith(v,i) NV_Ith_S(v,i-1)
+
+#define TOL RCONST(1.0e-5)
+#define TOL_REF RCONST(1.0e-14)
+
+static int fref(realtype t, N_Vector yy, N_Vector fy, void *f_data);
+
+static int f(realtype t, N_Vector yy, N_Vector fy, void *f_data);
+static int proj(realtype t, N_Vector yy, N_Vector corr,
+ realtype epsProj, N_Vector err, void *pdata);
+
+void GetSol(void *cpode_mem, N_Vector yy0, realtype tol,
+ realtype tout, booleantype proj, N_Vector yref);
+
+void RefSol(realtype tout, N_Vector yref);
+
+/*
+ *-------------------------------
+ * Main Program
+ *-------------------------------
+ */
+
+int main()
+{
+ void *cpode_mem;
+ N_Vector yref, yy0;
+ realtype tol, tout;
+ int i, flag;
+
+
+ tout = 30.0;
+
+ /* Get reference solution */
+ yref = N_VNew_Serial(4);
+ RefSol(tout, yref);
+
+ /* Initialize solver */
+ tol = TOL;
+ cpode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
+ yy0 = N_VNew_Serial(4);
+ Ith(yy0,1) = 1.0; /* x */
+ Ith(yy0,2) = 0.0; /* y */
+ Ith(yy0,3) = 0.0; /* xd */
+ Ith(yy0,4) = 0.0; /* yd */
+ flag = CPodeInit(cpode_mem, (void *)f, NULL, 0.0, yy0, NULL, CP_SS, tol, &tol);
+ flag = CPodeSetMaxNumSteps(cpode_mem, 50000);
+ flag = CPodeSetStopTime(cpode_mem, tout);
+ flag = CPodeProjDefine(cpode_mem, proj, NULL);
+ flag = CPDense(cpode_mem, 4);
+
+ for (i=0;i<5;i++) {
+
+ printf("\n\n%.2e\n", tol);
+ GetSol(cpode_mem, yy0, tol, tout, TRUE, yref);
+ GetSol(cpode_mem, yy0, tol, tout, FALSE, yref);
+ tol /= 10.0;
+ }
+
+ N_VDestroy_Serial(yref);
+ CPodeFree(&cpode_mem);
+
+ return(0);
+}
+
+void GetSol(void *cpode_mem, N_Vector yy0, realtype tol,
+ realtype tout, booleantype proj, N_Vector yref)
+{
+ N_Vector yy, yp;
+ realtype t, x, y, xd, yd, g;
+ int flag;
+ long int nst, nfe, nsetups, nje, nfeLS, ncfn, netf;
+
+ if (proj) {
+ printf(" YES ");
+ CPodeSetProjFrequency(cpode_mem, 1);
+ } else {
+ CPodeSetProjFrequency(cpode_mem, 0);
+ printf(" NO ");
+ }
+
+ yy = N_VNew_Serial(4);
+ yp = N_VNew_Serial(4);
+
+ flag = CPodeReInit(cpode_mem, (void *)f, NULL, 0.0, yy0, NULL, CP_SS, tol, &tol);
+
+ flag = CPode(cpode_mem, tout, &t, yy, yp, CP_NORMAL_TSTOP);
+
+ x = Ith(yy,1);
+ y = Ith(yy,2);
+ g = ABS(x*x + y*y - 1.0);
+
+ N_VLinearSum(1.0, yy, -1.0, yref, yy);
+
+ N_VAbs(yy, yy);
+
+ x = Ith(yy,1);
+ y = Ith(yy,2);
+ xd = Ith(yy,3);
+ yd = Ith(yy,4);
+
+
+ printf("%9.2e %9.2e %9.2e %9.2e | %9.2e |",
+ Ith(yy,1),Ith(yy,2),Ith(yy,3),Ith(yy,4),g);
+
+ CPodeGetNumSteps(cpode_mem, &nst);
+ CPodeGetNumFctEvals(cpode_mem, &nfe);
+ CPodeGetNumLinSolvSetups(cpode_mem, &nsetups);
+ CPodeGetNumErrTestFails(cpode_mem, &netf);
+ CPodeGetNumNonlinSolvConvFails(cpode_mem, &ncfn);
+
+ CPDlsGetNumJacEvals(cpode_mem, &nje);
+ CPDlsGetNumFctEvals(cpode_mem, &nfeLS);
+
+
+ printf(" %6ld %6ld+%-4ld %4ld (%3ld) | %3ld %3ld\n",
+ nst, nfe, nfeLS, nsetups, nje, ncfn, netf);
+
+ N_VDestroy_Serial(yy);
+ N_VDestroy_Serial(yp);
+
+ return;
+}
+
+
+void RefSol(realtype tout, N_Vector yref)
+{
+ void *cpode_mem;
+ N_Vector yy, yp;
+ realtype tol, t, th, thd;
+ int flag;
+
+
+ yy = N_VNew_Serial(2);
+ yp = N_VNew_Serial(2);
+ Ith(yy,1) = 0.0; /* theta */
+ Ith(yy,2) = 0.0; /* thetad */
+ tol = TOL_REF;
+
+ cpode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
+ flag = CPodeSetMaxNumSteps(cpode_mem, 100000);
+ flag = CPodeInit(cpode_mem, (void *)fref, NULL, 0.0, yy, yp, CP_SS, tol, &tol);
+ flag = CPDense(cpode_mem, 2);
+
+ flag = CPodeSetStopTime(cpode_mem, tout);
+ flag = CPode(cpode_mem, tout, &t, yy, yp, CP_NORMAL_TSTOP);
+ th = Ith(yy,1);
+ thd = Ith(yy,2);
+ Ith(yref,1) = cos(th);
+ Ith(yref,2) = sin(th);
+ Ith(yref,3) = -thd*sin(th);
+ Ith(yref,4) = thd*cos(th);
+
+ N_VDestroy_Serial(yy);
+ N_VDestroy_Serial(yp);
+ CPodeFree(&cpode_mem);
+
+ return;
+}
+
+static int fref(realtype t, N_Vector yy, N_Vector fy, void *f_data)
+{
+ realtype th, thd, g;
+
+ g = 13.7503716373294544;
+
+ th = Ith(yy,1);
+ thd = Ith(yy,2);
+
+ Ith(fy,1) = thd;
+ Ith(fy,2) = -g*cos(th);
+
+ return(0);
+}
+
+
+
+static int f(realtype t, N_Vector yy, N_Vector fy, void *f_data)
+{
+ realtype x, y, xd, yd, g, tmp;
+
+ g = 13.7503716373294544;
+
+ x = Ith(yy,1);
+ y = Ith(yy,2);
+ xd = Ith(yy,3);
+ yd = Ith(yy,4);
+
+ tmp = xd*xd + yd*yd - g*y;
+
+ Ith(fy,1) = xd;
+ Ith(fy,2) = yd;
+ Ith(fy,3) = -x*tmp;
+ Ith(fy,4) = -y*tmp - g;
+
+ return(0);
+}
+
+static int proj(realtype t, N_Vector yy, N_Vector corr,
+ realtype epsProj, N_Vector err, void *pdata)
+{
+ realtype x, y, xd, yd;
+ realtype x_new, y_new, xd_new, yd_new;
+ realtype e1, e2, e3, e4;
+ realtype e1_new, e2_new, e3_new, e4_new;
+ realtype R;
+
+ /* Extract current solution */
+
+ x = Ith(yy,1);
+ y = Ith(yy,2);
+ xd = Ith(yy,3);
+ yd = Ith(yy,4);
+
+ /* Project onto manifold */
+
+ R = sqrt(x*x+y*y);
+
+ x_new = x/R;
+ y_new = y/R;
+
+ xd_new = xd*y_new*y_new - yd*x_new*y_new;
+ yd_new = - xd*x_new*y_new + yd*x_new*x_new;
+
+ /* Return corrections */
+
+ Ith(corr,1) = x_new - x;
+ Ith(corr,2) = y_new - y;
+ Ith(corr,3) = xd_new - xd;
+ Ith(corr,4) = yd_new - yd;
+
+ /* +- -+
+ * | y*y -x*y |
+ * P = | |
+ * | -x*y x*x |
+ * +- -+
+ */
+
+ /* Return err <- P * err */
+
+ e1 = Ith(err,1);
+ e2 = Ith(err,2);
+ e3 = Ith(err,3);
+ e4 = Ith(err,4);
+
+ e1_new = y_new*y_new * e1 - x_new*y_new * e2;
+ e2_new = -x_new*y_new * e1 + x_new*x_new * e2;
+
+ e3_new = y_new*y_new * e3 - x_new*y_new * e4;
+ e4_new = -x_new*y_new * e3 + x_new*x_new * e4;
+
+ Ith(err,1) = e1_new;
+ Ith(err,2) = e2_new;
+ Ith(err,3) = e3_new;
+ Ith(err,4) = e4_new;
+
+ return(0);
+}
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsPend_dnsL.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsPend_dnsL.c
new file mode 100644
index 0000000..54dc5a2
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsPend_dnsL.c
@@ -0,0 +1,168 @@
+#include <stdio.h>
+
+#include <cpodes/cpodes.h>
+#include <cpodes/cpodes_lapack.h>
+#include <cpodes/cpodes_dense.h>
+#include <nvector/nvector_serial.h>
+
+#define Ith(v,i) NV_Ith_S(v,i-1)
+
+/* Problem Constants */
+
+#define RTOL RCONST(1.0e-8)
+#define ATOL RCONST(1.0e-8)
+
+/* Functions Called by the Solver */
+static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data);
+static int cfun(realtype t, N_Vector yy, N_Vector cout, void *c_data);
+static void PrintFinalStats(void *cpode_mem);
+
+/*
+ *-------------------------------
+ * Main Program
+ *-------------------------------
+ */
+
+int main()
+{
+ void *cpode_mem;
+ N_Vector yy, yp, ctols;
+ realtype reltol, abstol, t, tout, Tout;
+ realtype x, y, xd, yd, g;
+ int iout, Nout, flag;
+
+ yy = N_VNew_Serial(4);
+ yp = N_VNew_Serial(4);
+
+ /* Initialize y */
+ Ith(yy,1) = 1.0; /* x */
+ Ith(yy,2) = 0.0; /* y */
+ Ith(yy,3) = 0.0; /* xd */
+ Ith(yy,4) = 0.0; /* yd */
+
+ /* Set tolerances */
+ reltol = RTOL;
+ abstol = ATOL;
+
+ Nout = 30;
+ Tout = Nout*1.0;
+
+ /* Initialize solver */
+ cpode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
+ flag = CPodeInit(cpode_mem, (void *)f, NULL, 0.0, yy, yp, CP_SS, reltol, &abstol);
+ flag = CPodeSetMaxNumSteps(cpode_mem, 50000);
+ flag = CPodeSetStopTime(cpode_mem, Tout);
+ flag = CPLapackDense(cpode_mem, 4);
+
+ /* INTERNAL PROJECTION FUNCTION */
+ ctols = N_VNew_Serial(3);
+ Ith(ctols,1) = 1.0e-8;
+ Ith(ctols,2) = 1.0e-8;
+ Ith(ctols,3) = 1.0e-8;
+ flag = CPodeProjInit(cpode_mem, CP_PROJ_L2NORM, CP_CNSTR_NONLIN, cfun, NULL, ctols);
+ flag = CPodeSetProjTestCnstr(cpode_mem, TRUE);
+ flag = CPLapackDenseProj(cpode_mem, 3, 4, CPDIRECT_QRP);
+
+ /* COMPUTE CONSISTENT INITIAL CONDITIONS */
+ flag = CPodeCalcIC(cpode_mem);
+ flag = CPodeGetConsistentIC(cpode_mem, yy, NULL);
+
+ /* INTEGRATE THROUGH A SEQUENCE OF TIMES */
+ t = 0.0;
+ for(iout=1; iout<=Nout; iout++) {
+ tout = iout*1.0;
+ flag = CPode(cpode_mem, tout, &t, yy, yp, CP_NORMAL_TSTOP);
+ if (flag < 0) break;
+
+ x = Ith(yy,1);
+ y = Ith(yy,2);
+ xd = Ith(yy,3);
+ yd = Ith(yy,4);
+ g = x*x + y*y - 1.0;
+ printf(" -------------- %lf %14.10lf %14.10lf %14.10lf %14.10lf %14.10lf\n", t, x,y,xd,yd,g);
+ }
+
+ PrintFinalStats(cpode_mem);
+
+ N_VDestroy_Serial(yy);
+ N_VDestroy_Serial(yp);
+ N_VDestroy_Serial(ctols);
+ CPodeFree(&cpode_mem);
+
+ return(0);
+}
+
+
+static int f(realtype t, N_Vector yy, N_Vector fy, void *f_data)
+{
+ realtype x, y, xd, yd, g, tmp;
+
+ g = 13.7503716373294544;
+
+ x = Ith(yy,1);
+ y = Ith(yy,2);
+ xd = Ith(yy,3);
+ yd = Ith(yy,4);
+
+ tmp = xd*xd + yd*yd - g*y;
+
+ Ith(fy,1) = xd;
+ Ith(fy,2) = yd;
+ Ith(fy,3) = -x*tmp;
+ Ith(fy,4) = -y*tmp - g;
+
+ return(0);
+}
+
+static int cfun(realtype t, N_Vector yy, N_Vector cout, void *c_data)
+{
+ realtype x, y, xd, yd;
+
+ x = Ith(yy,1);
+ y = Ith(yy,2);
+ xd = Ith(yy,3);
+ yd = Ith(yy,4);
+
+ Ith(cout,1) = x*x + y*y - 1.0;
+ Ith(cout,2) = x*xd + y*yd;
+ Ith(cout,3) = Ith(cout,1) + Ith(cout,2);
+
+ return(0);
+}
+
+
+static void PrintFinalStats(void *cpode_mem)
+{
+ realtype h0u;
+ long int nst, nfe, nsetups, nje, nfeLS, nni, ncfn, netf, nge;
+ long int nproj, nce, nsetupsP, nprf;
+ int flag;
+
+ flag = CPodeGetActualInitStep(cpode_mem, &h0u);
+ flag = CPodeGetNumSteps(cpode_mem, &nst);
+ flag = CPodeGetNumFctEvals(cpode_mem, &nfe);
+ flag = CPodeGetNumLinSolvSetups(cpode_mem, &nsetups);
+ flag = CPodeGetNumErrTestFails(cpode_mem, &netf);
+ flag = CPodeGetNumNonlinSolvIters(cpode_mem, &nni);
+ flag = CPodeGetNumNonlinSolvConvFails(cpode_mem, &ncfn);
+
+ flag = CPDlsGetNumJacEvals(cpode_mem, &nje);
+ flag = CPDlsGetNumFctEvals(cpode_mem, &nfeLS);
+
+ flag = CPodeGetProjStats(cpode_mem, &nproj, &nce, &nsetupsP, &nprf);
+
+ flag = CPodeGetNumGEvals(cpode_mem, &nge);
+
+ printf("\nFinal Statistics:\n");
+ printf("h0u = %g\n",h0u);
+ printf("nst = %-6ld nfe = %-6ld nsetups = %-6ld\n",
+ nst, nfe, nsetups);
+ printf("nfeLS = %-6ld nje = %ld\n",
+ nfeLS, nje);
+ printf("nni = %-6ld ncfn = %-6ld netf = %-6ld \n",
+ nni, ncfn, netf);
+ printf("nproj = %-6ld nce = %-6ld nsetupsP = %-6ld nprf = %-6ld\n",
+ nproj, nce, nsetupsP, nprf);
+ printf("nge = %ld\n", nge);
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsRoberts_dns.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsRoberts_dns.c
new file mode 100644
index 0000000..f0a9d8b
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsRoberts_dns.c
@@ -0,0 +1,287 @@
+#include <stdio.h>
+
+#include <cpodes/cpodes.h>
+#include <cpodes/cpodes_dense.h>
+#include <nvector/nvector_serial.h>
+
+#define Ith(v,i) NV_Ith_S(v,i-1)
+#define IJth(A,i,j) DENSE_ELEM(A,i-1,j-1)
+
+/* Problem Constants */
+
+#define ODE CP_EXPL
+
+#define NEQ 3
+#define Y1 RCONST(1.0)
+#define Y2 RCONST(0.0)
+#define Y3 RCONST(0.0)
+
+#define RTOL RCONST(1.0e-4) /* 1e-4 */
+#define ATOL1 RCONST(1.0e-8) /* 1e-8 */
+#define ATOL2 RCONST(1.0e-14) /* 1e-14 */
+#define ATOL3 RCONST(1.0e-6) /* 1e-6 */
+
+#define T0 RCONST(0.0)
+#define T1 RCONST(0.4)
+#define TMULT RCONST(10.0)
+#define NOUT 12
+
+#define ONE RCONST(1.0)
+#define ZERO RCONST(0.0)
+
+/* Functions Called by the Solver */
+static int res(realtype t, N_Vector y, N_Vector yp, N_Vector res, void *f_data);
+static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data);
+static int g(realtype t, N_Vector y, N_Vector yp, realtype *gout, void *g_data);
+static int jacI(int N, realtype t, realtype gm,
+ N_Vector y, N_Vector yp, N_Vector r,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+static int jacE(int N, realtype t,
+ N_Vector y, N_Vector yp,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+
+static void PrintFinalStats(void *cpode_mem);
+
+/*
+ *-------------------------------
+ * Main Program
+ *-------------------------------
+ */
+
+int main()
+{
+ void *fct, *jac;
+ void *cpode_mem;
+ N_Vector yy, yp, abstol;
+ realtype reltol, t, tout;
+ int flag, flagr, iout;
+ int rootsfound[2];
+
+ /* Create serial vectors of length NEQ for I.C. and abstol */
+ yy = N_VNew_Serial(NEQ);
+ yp = N_VNew_Serial(NEQ);
+ abstol = N_VNew_Serial(NEQ);
+
+ /* Initialize y */
+ Ith(yy,1) = Y1;
+ Ith(yy,2) = Y2;
+ Ith(yy,3) = Y3;
+
+ /* Set tolerances */
+ reltol = RTOL;
+ Ith(abstol,1) = ATOL1;
+ Ith(abstol,2) = ATOL2;
+ Ith(abstol,3) = ATOL3;
+
+
+ if (ODE == CP_EXPL) {
+ fct = (void *)f;
+ jac = (void *)jacE;
+ } else {
+ f(T0, yy, yp, NULL);
+ fct = (void *)res;
+ jac = (void *)jacI;
+ }
+
+ /* Initialize solver */
+ cpode_mem = CPodeCreate(ODE, CP_BDF, CP_NEWTON);
+ flag = CPodeInit(cpode_mem, fct, NULL, T0, yy, yp, CP_SV, reltol, abstol);
+
+ /* Set initial step size */
+ /*
+ {
+ realtype h0;
+ h0 = 8.5e-14;
+ flag = CPodeSetInitStep(cpode_mem, h0);
+ }
+ */
+
+ /* Call CPodeRootInit to specify the root function g with 2 components */
+ flag = CPodeRootInit(cpode_mem, 2, g, NULL);
+
+ /* Call CPDense to specify the CPDENSE dense linear solver */
+ flag = CPDense(cpode_mem, NEQ);
+
+ /* Set the Jacobian routine (comment out for internal DQ) */
+ flag = CPDlsSetJacFn(cpode_mem, jac, NULL);
+
+ /* In loop, call CPode, print results, and test for error.
+ Break out of loop when NOUT preset output times have been reached. */
+ iout = 0; tout = T1;
+ while(1) {
+ flag = CPode(cpode_mem, tout, &t, yy, yp, CP_NORMAL);
+ printf("At t = %0.4le y =%14.6le %14.6le %14.6le\n",
+ t, Ith(yy,1), Ith(yy,2), Ith(yy,3));
+
+ if (flag < 0) break;
+
+ if (flag == CP_ROOT_RETURN) {
+ flagr = CPodeGetRootInfo(cpode_mem, rootsfound);
+ printf(" rootsfound[] = %3d %3d\n", rootsfound[0],rootsfound[1]);
+ }
+
+ if (flag == CP_SUCCESS) {
+ iout++;
+ tout *= TMULT;
+ }
+
+ if (iout == NOUT) break;
+ }
+
+ /* Print some final statistics */
+ PrintFinalStats(cpode_mem);
+
+ /* Free memory */
+ N_VDestroy_Serial(yy);
+ N_VDestroy_Serial(yp);
+ N_VDestroy_Serial(abstol);
+ CPodeFree(&cpode_mem);
+
+ return(0);
+}
+
+
+/*
+ *-------------------------------
+ * Functions called by the solver
+ *-------------------------------
+ */
+
+/*
+ * residual function: F(y',y) = y' - f(y)
+ */
+
+static int res(realtype t, N_Vector y, N_Vector yp, N_Vector res, void *f_data)
+{
+ realtype y1, y2, y3, yp1, yp2, yp3;
+
+ y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
+ yp1 = Ith(yp,1); yp2 = Ith(yp,2); yp3 = Ith(yp,3);
+
+ Ith(res,1) = yp1 - (RCONST(-0.04)*y1 + RCONST(1.0e4)*y2*y3);
+ Ith(res,2) = yp2 + (RCONST(-0.04)*y1 + RCONST(1.0e4)*y2*y3 + RCONST(3.0e7)*y2*y2);
+ Ith(res,3) = yp3 - RCONST(3.0e7)*y2*y2;
+
+ return(0);
+}
+
+/*
+ * f routine. Compute function f(t,y).
+ */
+
+static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data)
+{
+ realtype y1, y2, y3, yd1, yd3;
+
+ y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
+
+ yd1 = Ith(ydot,1) = RCONST(-0.04)*y1 + RCONST(1.0e4)*y2*y3;
+ yd3 = Ith(ydot,3) = RCONST(3.0e7)*y2*y2;
+ Ith(ydot,2) = -yd1 - yd3;
+
+ return(0);
+}
+
+/*
+ * Jacobian routine. Compute J(t,y) = dF/dy' + gm * dF/dy = I - gm*df/dy.
+ */
+static int jacI(int N, realtype t, realtype gm,
+ N_Vector y, N_Vector yp, N_Vector r,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+
+ realtype y1, y2, y3;
+
+ y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
+
+ IJth(J,1,1) = ONE - gm*RCONST(-0.04);
+ IJth(J,1,2) = -gm*RCONST(1.0e4)*y3;
+ IJth(J,1,3) = -gm*RCONST(1.0e4)*y2;
+
+ IJth(J,2,1) = -gm*RCONST(0.04);
+ IJth(J,2,2) = ONE - gm * (RCONST(-1.0e4)*y3 - RCONST(6.0e7)*y2);
+ IJth(J,2,3) = -gm*RCONST(-1.0e4)*y2;
+
+ IJth(J,3,1) = ZERO;
+ IJth(J,3,2) = -gm*RCONST(6.0e7)*y2;
+ IJth(J,3,3) = ONE;
+
+ return(0);
+}
+
+/*
+ * Jacobian routine. Compute J(t,y) = df/dy. *
+ */
+
+static int jacE(int N, realtype t,
+ N_Vector y, N_Vector fy,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ realtype y1, y2, y3;
+
+ y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
+
+ IJth(J,1,1) = RCONST(-0.04);
+ IJth(J,1,2) = RCONST(1.0e4)*y3;
+ IJth(J,1,3) = RCONST(1.0e4)*y2;
+ IJth(J,2,1) = RCONST(0.04);
+ IJth(J,2,2) = RCONST(-1.0e4)*y3-RCONST(6.0e7)*y2;
+ IJth(J,2,3) = RCONST(-1.0e4)*y2;
+ IJth(J,3,1) = ZERO;
+ IJth(J,3,2) = RCONST(6.0e7)*y2;
+ IJth(J,3,3) = ZERO;
+
+ return(0);
+}
+
+
+/*
+ * g routine. Compute functions g_i(t,y) for i = 0,1.
+ */
+
+static int g(realtype t, N_Vector y, N_Vector yp, realtype *gout, void *g_data)
+{
+ realtype y1, y3;
+
+ y1 = Ith(y,1); y3 = Ith(y,3);
+ gout[0] = y1 - RCONST(0.0001);
+ gout[1] = y3 - RCONST(0.01);
+
+ return(0);
+}
+
+/*
+ * Get and print some final statistics
+ */
+
+static void PrintFinalStats(void *cpode_mem)
+{
+ realtype h0u;
+ long int nst, nfe, nsetups, nje, nfeLS, nni, ncfn, netf, nge;
+ int flag;
+
+ flag = CPodeGetActualInitStep(cpode_mem, &h0u);
+ flag = CPodeGetNumSteps(cpode_mem, &nst);
+ flag = CPodeGetNumFctEvals(cpode_mem, &nfe);
+ flag = CPodeGetNumLinSolvSetups(cpode_mem, &nsetups);
+ flag = CPodeGetNumErrTestFails(cpode_mem, &netf);
+ flag = CPodeGetNumNonlinSolvIters(cpode_mem, &nni);
+ flag = CPodeGetNumNonlinSolvConvFails(cpode_mem, &ncfn);
+
+ flag = CPDlsGetNumJacEvals(cpode_mem, &nje);
+ flag = CPDlsGetNumFctEvals(cpode_mem, &nfeLS);
+
+ flag = CPodeGetNumGEvals(cpode_mem, &nge);
+
+ printf("\nFinal Statistics:\n");
+ printf("h0u = %g\n",h0u);
+ printf("nst = %-6ld nfe = %-6ld nsetups = %-6ld nfeLS = %-6ld nje = %ld\n",
+ nst, nfe, nsetups, nfeLS, nje);
+ printf("nni = %-6ld ncfn = %-6ld netf = %-6ld nge = %ld\n \n",
+ nni, ncfn, netf, nge);
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsRoberts_dnsL.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsRoberts_dnsL.c
new file mode 100644
index 0000000..7fb0806
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsRoberts_dnsL.c
@@ -0,0 +1,290 @@
+#include <stdio.h>
+
+#include <cpodes/cpodes.h>
+#include <cpodes/cpodes_lapack.h>
+#include <nvector/nvector_serial.h>
+
+#define Ith(v,i) NV_Ith_S(v,i-1)
+#define IJth(A,i,j) DENSE_ELEM(A,i-1,j-1)
+
+/* Problem Constants */
+
+#define ODE CP_EXPL
+
+#define NEQ 3
+#define Y1 RCONST(1.0)
+#define Y2 RCONST(0.0)
+#define Y3 RCONST(0.0)
+
+#define RTOL RCONST(1.0e-4) /* 1e-4 */
+#define ATOL1 RCONST(1.0e-8) /* 1e-8 */
+#define ATOL2 RCONST(1.0e-14) /* 1e-14 */
+#define ATOL3 RCONST(1.0e-6) /* 1e-6 */
+
+#define T0 RCONST(0.0)
+#define T1 RCONST(0.4)
+#define TMULT RCONST(10.0)
+#define NOUT 12
+
+#define ONE RCONST(1.0)
+#define ZERO RCONST(0.0)
+
+/* Functions Called by the Solver */
+static int res(realtype t, N_Vector y, N_Vector yp, N_Vector res, void *f_data);
+static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data);
+static int g(realtype t, N_Vector y, N_Vector yp, realtype *gout, void *g_data);
+static int jacI(int N, realtype t, realtype gm,
+ N_Vector y, N_Vector yp, N_Vector r,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+static int jacE(int N, realtype t,
+ N_Vector y, N_Vector yp,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+
+
+static void PrintFinalStats(void *cpode_mem);
+
+/*
+ *-------------------------------
+ * Main Program
+ *-------------------------------
+ */
+
+int main()
+{
+ void *fct, *jac;
+ void *cpode_mem;
+ N_Vector yy, yp, abstol;
+ realtype reltol, t, tout;
+ int flag, flagr, iout;
+ int rootsfound[2];
+
+ /* Create serial vectors of length NEQ for I.C. and abstol */
+ yy = N_VNew_Serial(NEQ);
+ yp = N_VNew_Serial(NEQ);
+ abstol = N_VNew_Serial(NEQ);
+
+ /* Initialize y */
+ Ith(yy,1) = Y1;
+ Ith(yy,2) = Y2;
+ Ith(yy,3) = Y3;
+
+ /* Set tolerances */
+ reltol = RTOL;
+ Ith(abstol,1) = ATOL1;
+ Ith(abstol,2) = ATOL2;
+ Ith(abstol,3) = ATOL3;
+
+
+ if (ODE == CP_EXPL) {
+ fct = (void *)f;
+ jac = (void *)jacE;
+ } else {
+ f(T0, yy, yp, NULL);
+ fct = (void *)res;
+ jac = (void *)jacI;
+ }
+
+ /* Initialize solver */
+ cpode_mem = CPodeCreate(ODE, CP_BDF, CP_NEWTON);
+ flag = CPodeInit(cpode_mem, fct, NULL, T0, yy, yp, CP_SV, reltol, abstol);
+
+ /* Set initial step size */
+ /*
+ {
+ realtype h0;
+ h0 = 8.5e-14;
+ flag = CPodeSetInitStep(cpode_mem, h0);
+ }
+ */
+
+ /* Call CPodeRootInit to specify the root function g with 2 components */
+ flag = CPodeRootInit(cpode_mem, 2, g, NULL);
+
+ /* Call CPLapackDense to specify the CPLAPACK dense linear solver */
+ flag = CPLapackDense(cpode_mem, NEQ);
+
+ /* Set the Jacobian routine to jac (comment out for internal DQ) */
+ flag = CPDlsSetJacFn(cpode_mem, jac, NULL);
+
+ /* In loop, call CPode, print results, and test for error.
+ Break out of loop when NOUT preset output times have been reached. */
+ iout = 0; tout = T1;
+ while(1) {
+ flag = CPode(cpode_mem, tout, &t, yy, yp, CP_NORMAL);
+ printf("At t = %0.4le y =%14.6le %14.6le %14.6le\n",
+ t, Ith(yy,1), Ith(yy,2), Ith(yy,3));
+
+ if (flag < 0) break;
+
+ if (flag == CP_ROOT_RETURN) {
+ flagr = CPodeGetRootInfo(cpode_mem, rootsfound);
+ printf(" rootsfound[] = %3d %3d\n", rootsfound[0],rootsfound[1]);
+ }
+
+ if (flag == CP_SUCCESS) {
+ iout++;
+ tout *= TMULT;
+ }
+
+ if (iout == NOUT) break;
+ }
+
+ /* Print some final statistics */
+ PrintFinalStats(cpode_mem);
+
+ /* Free memory */
+ N_VDestroy_Serial(yy);
+ N_VDestroy_Serial(yp);
+ N_VDestroy_Serial(abstol);
+ CPodeFree(&cpode_mem);
+
+ return(0);
+}
+
+
+/*
+ *-------------------------------
+ * Functions called by the solver
+ *-------------------------------
+ */
+
+/*
+ * residual function: F(y',y) = y' - f(y)
+ */
+
+static int res(realtype t, N_Vector y, N_Vector yp, N_Vector res, void *f_data)
+{
+ realtype y1, y2, y3, yp1, yp2, yp3;
+
+ y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
+ yp1 = Ith(yp,1); yp2 = Ith(yp,2); yp3 = Ith(yp,3);
+
+ Ith(res,1) = yp1 - (RCONST(-0.04)*y1 + RCONST(1.0e4)*y2*y3);
+ Ith(res,2) = yp2 + (RCONST(-0.04)*y1 + RCONST(1.0e4)*y2*y3 + RCONST(3.0e7)*y2*y2);
+ Ith(res,3) = yp3 - RCONST(3.0e7)*y2*y2;
+
+ return(0);
+}
+
+/*
+ * f routine. Compute function f(t,y).
+ */
+
+static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data)
+{
+ realtype y1, y2, y3, yd1, yd3;
+
+ y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
+
+ yd1 = Ith(ydot,1) = RCONST(-0.04)*y1 + RCONST(1.0e4)*y2*y3;
+ yd3 = Ith(ydot,3) = RCONST(3.0e7)*y2*y2;
+ Ith(ydot,2) = -yd1 - yd3;
+
+ return(0);
+}
+
+/*
+ * Jacobian routine. Compute J(t,y) = dF/dy' + gm * dF/dy = I - gm*df/dy.
+ */
+static int jacI(int N, realtype t, realtype gm,
+ N_Vector y, N_Vector yp, N_Vector r,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+
+ realtype y1, y2, y3;
+
+ y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
+
+ IJth(J,1,1) = ONE - gm*RCONST(-0.04);
+ IJth(J,1,2) = -gm*RCONST(1.0e4)*y3;
+ IJth(J,1,3) = -gm*RCONST(1.0e4)*y2;
+
+ IJth(J,2,1) = -gm*RCONST(0.04);
+ IJth(J,2,2) = ONE - gm * (RCONST(-1.0e4)*y3 - RCONST(6.0e7)*y2);
+ IJth(J,2,3) = -gm*RCONST(-1.0e4)*y2;
+
+ IJth(J,3,1) = ZERO;
+ IJth(J,3,2) = -gm*RCONST(6.0e7)*y2;
+ IJth(J,3,3) = ONE;
+
+ return(0);
+}
+
+/*
+ * Jacobian routine. Compute J(t,y) = df/dy. *
+ */
+
+static int jacE(int N, realtype t,
+ N_Vector y, N_Vector fy,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ realtype y1, y2, y3;
+
+ y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
+
+ IJth(J,1,1) = RCONST(-0.04);
+ IJth(J,1,2) = RCONST(1.0e4)*y3;
+ IJth(J,1,3) = RCONST(1.0e4)*y2;
+
+ IJth(J,2,1) = RCONST(0.04);
+ IJth(J,2,2) = RCONST(-1.0e4)*y3-RCONST(6.0e7)*y2;
+ IJth(J,2,3) = RCONST(-1.0e4)*y2;
+
+ IJth(J,3,1) = ZERO;
+ IJth(J,3,2) = RCONST(6.0e7)*y2;
+ IJth(J,3,3) = ZERO;
+
+ return(0);
+}
+
+
+/*
+ * g routine. Compute functions g_i(t,y) for i = 0,1.
+ */
+
+static int g(realtype t, N_Vector y, N_Vector yp, realtype *gout, void *g_data)
+{
+ realtype y1, y3;
+
+ y1 = Ith(y,1); y3 = Ith(y,3);
+ gout[0] = y1 - RCONST(0.0001);
+ gout[1] = y3 - RCONST(0.01);
+
+ return(0);
+}
+
+/*
+ * Get and print some final statistics
+ */
+
+static void PrintFinalStats(void *cpode_mem)
+{
+ realtype h0u;
+ long int nst, nfe, nsetups, nje, nfeLS, nni, ncfn, netf, nge;
+ int flag;
+
+ flag = CPodeGetActualInitStep(cpode_mem, &h0u);
+ flag = CPodeGetNumSteps(cpode_mem, &nst);
+ flag = CPodeGetNumFctEvals(cpode_mem, &nfe);
+ flag = CPodeGetNumLinSolvSetups(cpode_mem, &nsetups);
+ flag = CPodeGetNumErrTestFails(cpode_mem, &netf);
+ flag = CPodeGetNumNonlinSolvIters(cpode_mem, &nni);
+ flag = CPodeGetNumNonlinSolvConvFails(cpode_mem, &ncfn);
+
+ flag = CPDlsGetNumJacEvals(cpode_mem, &nje);
+ flag = CPDlsGetNumFctEvals(cpode_mem, &nfeLS);
+
+ flag = CPodeGetNumGEvals(cpode_mem, &nge);
+
+ printf("\nFinal Statistics:\n");
+ printf("h0u = %g\n",h0u);
+ printf("nst = %-6ld nfe = %-6ld nsetups = %-6ld nfeLS = %-6ld nje = %ld\n",
+ nst, nfe, nsetups, nfeLS, nje);
+ printf("nni = %-6ld ncfn = %-6ld netf = %-6ld nge = %ld\n \n",
+ nni, ncfn, netf, nge);
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsVanDPol_non.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsVanDPol_non.c
new file mode 100644
index 0000000..a1117d8
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsVanDPol_non.c
@@ -0,0 +1,141 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision: 1.1 $
+ * $Date: 2007/10/25 20:03:26 $
+ * -----------------------------------------------------------------
+ * Programmer(s): Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Van der Pol oscillator:
+ * xdotdot - 3*(1 - x^2)*xdot + x = 0, x(0) = 2, xdot(0) = 0.
+ * This second-order ODE is converted to a first-order system by
+ * defining y0 = x and y1 = xdot.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <cpodes/cpodes.h>
+#include <nvector/nvector_serial.h>
+#include <sundials/sundials_math.h>
+
+
+#define ODE CP_EXPL
+
+/* Problem Constants */
+
+#define ATOL RCONST(1.0e-6) /* 1.0e-6 */
+#define RTOL RCONST(0.0) /* 0.0 */
+
+#define ZERO RCONST(0.0)
+#define ONE RCONST(1.0)
+#define TWO RCONST(2.0)
+#define THIRTY RCONST(30.0)
+
+#define P1_NEQ 2
+#define P1_ETA RCONST(3.0)
+#define P1_NOUT 4
+#define P1_T0 RCONST(0.0)
+#define P1_T1 RCONST(1.39283880203)
+#define P1_DTOUT RCONST(2.214773875)
+
+/* Private Helper Functions */
+static void PrintFinalStats(void *cpode_mem);
+
+/* Functions Called by the Solver */
+static int res(realtype t, N_Vector y, N_Vector yp, N_Vector res, void *f_data);
+static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data);
+
+int main()
+{
+ void *fct;
+ void *cpode_mem;
+ N_Vector y, yp;
+ realtype reltol=RTOL, abstol=ATOL, t, tout, hu;
+ int flag, iout, qu;
+
+ y = NULL;
+ yp = NULL;
+ cpode_mem = NULL;
+
+ y = N_VNew_Serial(P1_NEQ);
+ NV_Ith_S(y,0) = TWO;
+ NV_Ith_S(y,1) = ZERO;
+
+ yp = N_VNew_Serial(P1_NEQ);
+
+ if (ODE == CP_EXPL) {
+ fct = (void *)f;
+ } else {
+ fct = (void *)res;
+ f(P1_T0, y, yp, NULL);
+ }
+
+ cpode_mem = CPodeCreate(ODE, CP_ADAMS, CP_FUNCTIONAL);
+ /* flag = CPodeSetInitStep(cpode_mem, 4.0e-9);*/
+ flag = CPodeInit(cpode_mem, fct, NULL, P1_T0, y, yp, CP_SS, reltol, &abstol);
+
+ printf("\n t x xdot qu hu \n");
+ for(iout=1, tout=P1_T1; iout <= P1_NOUT; iout++, tout += P1_DTOUT) {
+ flag = CPode(cpode_mem, tout, &t, y, yp, CP_NORMAL);
+ if (flag != CP_SUCCESS) break;
+ flag = CPodeGetLastOrder(cpode_mem, &qu);
+ flag = CPodeGetLastStep(cpode_mem, &hu);
+ printf("%10.5f %12.5le %12.5le %2d %6.4le\n", t, NV_Ith_S(y,0), NV_Ith_S(y,1), qu, hu);
+ }
+
+ PrintFinalStats(cpode_mem);
+
+ CPodeFree(&cpode_mem);
+ N_VDestroy_Serial(y);
+ N_VDestroy_Serial(yp);
+
+ return 0;
+}
+
+static int res(realtype t, N_Vector y, N_Vector yp, N_Vector res, void *f_data)
+{
+ f(t,y,res,f_data);
+ N_VLinearSum(1.0, yp, -1.0, res, res);
+ return(0);
+}
+
+
+static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data)
+{
+ realtype y0, y1;
+
+ y0 = NV_Ith_S(y,0);
+ y1 = NV_Ith_S(y,1);
+
+ NV_Ith_S(ydot,0) = y1;
+ NV_Ith_S(ydot,1) = (ONE - SQR(y0))* P1_ETA * y1 - y0;
+
+ return(0);
+}
+
+
+static void PrintFinalStats(void *cpode_mem)
+{
+ long int nst, nfe, nni, ncfn, netf;
+ realtype h0u;
+ int flag;
+
+ flag = CPodeGetActualInitStep(cpode_mem, &h0u);
+ flag = CPodeGetNumSteps(cpode_mem, &nst);
+ flag = CPodeGetNumFctEvals(cpode_mem, &nfe);
+ flag = CPodeGetNumErrTestFails(cpode_mem, &netf);
+ flag = CPodeGetNumNonlinSolvIters(cpode_mem, &nni);
+ flag = CPodeGetNumNonlinSolvConvFails(cpode_mem, &ncfn);
+
+ printf("\n Final statistics:\n\n");
+ printf(" Number of steps = %4ld \n", nst);
+ printf(" Number of f-s = %4ld \n", nfe);
+ printf(" Number of nonlinear iterations = %4ld \n", nni);
+ printf(" Number of nonlinear convergence failures = %4ld \n", ncfn);
+ printf(" Number of error test failures = %4ld \n", netf);
+ printf(" Initial step size = %g \n\n", h0u);
+
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsadamsx.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsadamsx.c
new file mode 100644
index 0000000..e016b42
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsadamsx.c
@@ -0,0 +1,283 @@
+/*
+ * -----------------------------------------------------------------
+ * Problem 1: Van der Pol oscillator
+ * xdotdot - 3*(1 - x^2)*xdot + x = 0, x(0) = 2, xdot(0) = 0.
+ * This second-order ODE is converted to a first-order system by
+ * defining y0 = x and y1 = xdot.
+ *
+ * Problem 2: ydot = A * y, where A is a banded lower triangular
+ * matrix derived from 2-D advection PDE.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <cpodes/cpodes.h>
+#include <nvector/nvector_serial.h>
+#include <sundials/sundials_math.h>
+
+
+#define ODE CP_EXPL
+
+/* Shared Problem Constants */
+
+#define ATOL RCONST(1.0e-6) /* 1.0e-6 */
+#define RTOL RCONST(0.0) /* 0.0 */
+
+#define ZERO RCONST(0.0)
+#define ONE RCONST(1.0)
+#define TWO RCONST(2.0)
+#define THIRTY RCONST(30.0)
+
+/* Problem #1 Constants */
+
+#define P1_NEQ 2
+#define P1_ETA RCONST(3.0)
+#define P1_NOUT 4
+#define P1_T0 RCONST(0.0)
+#define P1_T1 RCONST(1.39283880203)
+#define P1_DTOUT RCONST(2.214773875)
+
+/* Problem #2 Constants */
+
+#define P2_MESHX 5
+#define P2_MESHY 5
+#define P2_NEQ P2_MESHX*P2_MESHY
+#define P2_ALPH1 RCONST(1.0)
+#define P2_ALPH2 RCONST(1.0)
+#define P2_NOUT 5
+#define P2_ML 5
+#define P2_MU 0
+#define P2_T0 RCONST(0.0)
+#define P2_T1 RCONST(0.01)
+#define P2_TOUT_MULT RCONST(10.0)
+
+/* Private Helper Functions */
+static void Problem1(void);
+static void Problem2(void);
+static realtype MaxError(N_Vector y, realtype t);
+static void PrintFinalStats(void *cpode_mem);
+
+/* Functions Called by the Solver */
+static int res1(realtype t, N_Vector y, N_Vector yp, N_Vector res, void *f_data);
+static int f1(realtype t, N_Vector y, N_Vector ydot, void *f_data);
+static int res2(realtype t, N_Vector y, N_Vector yp, N_Vector res, void *f_data);
+static int f2(realtype t, N_Vector y, N_Vector ydot, void *f_data);
+
+int main(void)
+{
+
+ printf("Van der Pol\n\n");
+ Problem1();
+ printf("\n\ny' = A*y\n\n");
+ Problem2();
+
+ return(0);
+}
+
+static void Problem1(void)
+{
+ void *fct;
+ void *cpode_mem;
+ N_Vector y, yp;
+ realtype reltol=RTOL, abstol=ATOL, t, tout, hu;
+ int flag, iout, qu;
+
+ y = NULL;
+ yp = NULL;
+ cpode_mem = NULL;
+
+ y = N_VNew_Serial(P1_NEQ);
+ NV_Ith_S(y,0) = TWO;
+ NV_Ith_S(y,1) = ZERO;
+
+ yp = N_VNew_Serial(P1_NEQ);
+
+ if (ODE == CP_EXPL) {
+ fct = (void *)f1;
+ } else {
+ fct = (void *)res1;
+ f1(P1_T0, y, yp, NULL);
+ }
+
+ cpode_mem = CPodeCreate(ODE, CP_ADAMS, CP_FUNCTIONAL);
+ /* flag = CPodeSetInitStep(cpode_mem, 4.0e-9);*/
+ flag = CPodeInit(cpode_mem, fct, NULL, P1_T0, y, yp, CP_SS, reltol, &abstol);
+
+ printf("\n t x xdot qu hu \n");
+ for(iout=1, tout=P1_T1; iout <= P1_NOUT; iout++, tout += P1_DTOUT) {
+ flag = CPode(cpode_mem, tout, &t, y, yp, CP_NORMAL);
+ if (flag != CP_SUCCESS) break;
+ flag = CPodeGetLastOrder(cpode_mem, &qu);
+ flag = CPodeGetLastStep(cpode_mem, &hu);
+ printf("%10.5f %12.5le %12.5le %2d %6.4le\n", t, NV_Ith_S(y,0), NV_Ith_S(y,1), qu, hu);
+ }
+
+ PrintFinalStats(cpode_mem);
+
+ CPodeFree(&cpode_mem);
+ N_VDestroy_Serial(y);
+ N_VDestroy_Serial(yp);
+
+ return;
+}
+
+static int res1(realtype t, N_Vector y, N_Vector yp, N_Vector res, void *f_data)
+{
+ f1(t,y,res,f_data);
+ N_VLinearSum(1.0, yp, -1.0, res, res);
+ return(0);
+}
+
+
+static int f1(realtype t, N_Vector y, N_Vector ydot, void *f_data)
+{
+ realtype y0, y1;
+
+ y0 = NV_Ith_S(y,0);
+ y1 = NV_Ith_S(y,1);
+
+ NV_Ith_S(ydot,0) = y1;
+ NV_Ith_S(ydot,1) = (ONE - SQR(y0))* P1_ETA * y1 - y0;
+
+ return(0);
+}
+
+
+
+
+
+static void Problem2(void)
+{
+ void *fct;
+ void *cpode_mem;
+ N_Vector y, yp;
+ realtype reltol=RTOL, abstol=ATOL, t, tout, erm, hu;
+ int flag, iout, qu;
+
+ y = NULL;
+ yp = NULL;
+ cpode_mem = NULL;
+
+ y = N_VNew_Serial(P2_NEQ);
+ N_VConst(ZERO, y);
+ NV_Ith_S(y,0) = ONE;
+
+ yp = N_VNew_Serial(P2_NEQ);
+
+ if (ODE == CP_EXPL) {
+ fct = (void *)f2;
+ } else {
+ fct = (void *)res2;
+ f2(P2_T0, y, yp, NULL);
+ }
+
+ cpode_mem = CPodeCreate(ODE, CP_ADAMS, CP_FUNCTIONAL);
+ /* flag = CPodeSetInitStep(cpode_mem, 2.0e-9);*/
+ flag = CPodeInit(cpode_mem, fct, NULL, P2_T0, y, yp, CP_SS, reltol, &abstol);
+
+ printf("\n t max.err qu hu \n");
+ for(iout=1, tout=P2_T1; iout <= P2_NOUT; iout++, tout*=P2_TOUT_MULT) {
+ flag = CPode(cpode_mem, tout, &t, y, yp, CP_NORMAL);
+ if (flag != CP_SUCCESS) break;
+ erm = MaxError(y, t);
+ flag = CPodeGetLastOrder(cpode_mem, &qu);
+ flag = CPodeGetLastStep(cpode_mem, &hu);
+ printf("%10.3f %12.4le %2d %12.4le\n", t, erm, qu, hu);
+ }
+
+ PrintFinalStats(cpode_mem);
+
+ CPodeFree(&cpode_mem);
+ N_VDestroy_Serial(y);
+ N_VDestroy_Serial(yp);
+
+ return;
+}
+
+static int res2(realtype t, N_Vector y, N_Vector yp, N_Vector res, void *f_data)
+{
+ f2(t,y,res,f_data);
+ N_VLinearSum(1.0, yp, -1.0, res, res);
+ return(0);
+}
+
+static int f2(realtype t, N_Vector y, N_Vector ydot, void *f_data)
+{
+ long int i, j, k;
+ realtype d, *ydata, *dydata;
+
+ ydata = NV_DATA_S(y);
+ dydata = NV_DATA_S(ydot);
+
+ /*
+ Excluding boundaries,
+
+ ydot = f = -2 y + alpha1 * y + alpha2 * y
+ i,j i,j i,j i-1,j i,j-1
+ */
+
+ for (j=0; j < P2_MESHY; j++) {
+ for (i=0; i < P2_MESHX; i++) {
+ k = i + j * P2_MESHX;
+ d = -TWO*ydata[k];
+ if (i != 0) d += P2_ALPH1 * ydata[k-1];
+ if (j != 0) d += P2_ALPH2 * ydata[k-P2_MESHX];
+ dydata[k] = d;
+ }
+ }
+
+ return(0);
+}
+
+static realtype MaxError(N_Vector y, realtype t)
+{
+ long int i, j, k;
+ realtype *ydata, er, ex=ZERO, yt, maxError=ZERO, ifact_inv, jfact_inv=ONE;
+
+ if (t == ZERO) return(ZERO);
+
+ ydata = NV_DATA_S(y);
+ if (t <= THIRTY) ex = EXP(-TWO*t);
+
+ for (j = 0; j < P2_MESHY; j++) {
+ ifact_inv = ONE;
+ for (i = 0; i < P2_MESHX; i++) {
+ k = i + j * P2_MESHX;
+ yt = RPowerI(t,i+j) * ex * ifact_inv * jfact_inv;
+ er = ABS(ydata[k] - yt);
+ if (er > maxError) maxError = er;
+ ifact_inv /= (i+1);
+ }
+ jfact_inv /= (j+1);
+ }
+ return(maxError);
+}
+
+
+
+static void PrintFinalStats(void *cpode_mem)
+{
+ long int nst, nfe, nni, ncfn, netf;
+ realtype h0u;
+ int flag;
+
+ flag = CPodeGetActualInitStep(cpode_mem, &h0u);
+ flag = CPodeGetNumSteps(cpode_mem, &nst);
+ flag = CPodeGetNumFctEvals(cpode_mem, &nfe);
+ flag = CPodeGetNumErrTestFails(cpode_mem, &netf);
+ flag = CPodeGetNumNonlinSolvIters(cpode_mem, &nni);
+ flag = CPodeGetNumNonlinSolvConvFails(cpode_mem, &ncfn);
+
+ printf("\n Final statistics:\n\n");
+ printf(" Number of steps = %4ld \n", nst);
+ printf(" Number of f-s = %4ld \n", nfe);
+ printf(" Number of nonlinear iterations = %4ld \n", nni);
+ printf(" Number of nonlinear convergence failures = %4ld \n", ncfn);
+ printf(" Number of error test failures = %4ld \n", netf);
+ printf(" Initial step size = %g \n\n", h0u);
+
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsbanx.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsbanx.c
new file mode 100644
index 0000000..5d1d793
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsbanx.c
@@ -0,0 +1,265 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <cpodes/cpodes.h>
+#include <cpodes/cpodes_band.h>
+#include <nvector/nvector_serial.h>
+#include <sundials/sundials_math.h>
+
+/* Problem Constants */
+
+#define XMAX RCONST(2.0) /* domain boundaries */
+#define YMAX RCONST(1.0)
+#define MX 10 /* mesh dimensions */
+#define MY 5
+#define NEQ MX*MY /* number of equations */
+#define ATOL RCONST(1.0e-5) /* scalar absolute tolerance */
+#define T0 RCONST(0.0) /* initial time */
+#define T1 RCONST(0.1) /* first output time */
+#define DTOUT RCONST(0.1) /* output time increment */
+#define NOUT 10 /* number of output times */
+
+#define ZERO RCONST(0.0)
+#define HALF RCONST(0.5)
+#define ONE RCONST(1.0)
+#define TWO RCONST(2.0)
+#define FIVE RCONST(5.0)
+
+#define IJth(vdata,i,j) (vdata[(j-1) + (i-1)*MY])
+
+typedef struct {
+ realtype dx, dy, hdcoef, hacoef, vdcoef;
+} *UserData;
+
+/* Private Helper Functions */
+static void SetIC(N_Vector u, UserData data);
+static void PrintHeader(realtype reltol, realtype abstol, realtype umax);
+static void PrintOutput(realtype t, realtype umax, long int nst);
+static void PrintFinalStats(void *cvode_mem);
+
+/* Functions Called by the Solver */
+static int f(realtype t, N_Vector u, N_Vector udot, void *f_data);
+static int Jac(int N, int mu, int ml,
+ realtype t, N_Vector u, N_Vector fu,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+/*
+ *-------------------------------
+ * Main Program
+ *-------------------------------
+ */
+
+int main(void)
+{
+ realtype dx, dy, reltol, abstol, t, tout, umax;
+ N_Vector u, up;
+ UserData data;
+ void *cvode_mem;
+ int iout, flag;
+ long int nst;
+
+ u = NULL;
+ data = NULL;
+ cvode_mem = NULL;
+
+ u = N_VNew_Serial(NEQ);
+ up = N_VNew_Serial(NEQ);
+
+ reltol = ZERO;
+ abstol = ATOL;
+
+ data = (UserData) malloc(sizeof *data);
+ dx = data->dx = XMAX/(MX+1);
+ dy = data->dy = YMAX/(MY+1);
+ data->hdcoef = ONE/(dx*dx);
+ data->hacoef = HALF/(TWO*dx);
+ data->vdcoef = ONE/(dy*dy);
+
+ SetIC(u, data);
+
+ cvode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
+ flag = CPodeInit(cvode_mem, (void *)f, data, T0, u, NULL, CP_SS, reltol, &abstol);
+
+ flag = CPBand(cvode_mem, NEQ, MY, MY);
+ flag = CPDlsSetJacFn(cvode_mem, (void *)Jac, data);
+
+ /* In loop over output points: call CPode, print results, test for errors */
+ umax = N_VMaxNorm(u);
+ PrintHeader(reltol, abstol, umax);
+ for(iout=1, tout=T1; iout <= NOUT; iout++, tout += DTOUT) {
+ flag = CPode(cvode_mem, tout, &t, u, up, CP_NORMAL);
+ umax = N_VMaxNorm(u);
+ flag = CPodeGetNumSteps(cvode_mem, &nst);
+ PrintOutput(t, umax, nst);
+ }
+
+ PrintFinalStats(cvode_mem);
+
+ N_VDestroy_Serial(u);
+ CPodeFree(&cvode_mem);
+ free(data);
+
+ return(0);
+}
+
+/* f routine. Compute f(t,u). */
+static int f(realtype t, N_Vector u,N_Vector udot, void *f_data)
+{
+ realtype uij, udn, uup, ult, urt, hordc, horac, verdc, hdiff, hadv, vdiff;
+ realtype *udata, *dudata;
+ int i, j;
+ UserData data;
+
+ udata = NV_DATA_S(u);
+ dudata = NV_DATA_S(udot);
+
+ /* Extract needed constants from data */
+
+ data = (UserData) f_data;
+ hordc = data->hdcoef;
+ horac = data->hacoef;
+ verdc = data->vdcoef;
+
+ /* Loop over all grid points. */
+
+ for (j=1; j <= MY; j++) {
+
+ for (i=1; i <= MX; i++) {
+
+ /* Extract u at x_i, y_j and four neighboring points */
+
+ uij = IJth(udata, i, j);
+ udn = (j == 1) ? ZERO : IJth(udata, i, j-1);
+ uup = (j == MY) ? ZERO : IJth(udata, i, j+1);
+ ult = (i == 1) ? ZERO : IJth(udata, i-1, j);
+ urt = (i == MX) ? ZERO : IJth(udata, i+1, j);
+
+ /* Set diffusion and advection terms and load into udot */
+
+ hdiff = hordc*(ult - TWO*uij + urt);
+ hadv = horac*(urt - ult);
+ vdiff = verdc*(uup - TWO*uij + udn);
+ IJth(dudata, i, j) = hdiff + hadv + vdiff;
+ }
+ }
+
+ return(0);
+}
+
+/* Jacobian routine. Compute J(t,u). */
+static int Jac(int N, int mu, int ml,
+ realtype t, N_Vector u, N_Vector fu,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ long int i, j, k;
+ realtype *kthCol, hordc, horac, verdc;
+ UserData data;
+
+ /*
+ The components of f = udot that depend on u(i,j) are
+ f(i,j), f(i-1,j), f(i+1,j), f(i,j-1), f(i,j+1), with
+ df(i,j)/du(i,j) = -2 (1/dx^2 + 1/dy^2)
+ df(i-1,j)/du(i,j) = 1/dx^2 + .25/dx (if i > 1)
+ df(i+1,j)/du(i,j) = 1/dx^2 - .25/dx (if i < MX)
+ df(i,j-1)/du(i,j) = 1/dy^2 (if j > 1)
+ df(i,j+1)/du(i,j) = 1/dy^2 (if j < MY)
+ */
+
+ data = (UserData) jac_data;
+ hordc = data->hdcoef;
+ horac = data->hacoef;
+ verdc = data->vdcoef;
+
+ for (j=1; j <= MY; j++) {
+ for (i=1; i <= MX; i++) {
+ k = j-1 + (i-1)*MY;
+ kthCol = BAND_COL(J,k);
+
+ /* set the kth column of J */
+
+ BAND_COL_ELEM(kthCol,k,k) = -TWO*(verdc+hordc);
+ if (i != 1) BAND_COL_ELEM(kthCol,k-MY,k) = hordc + horac;
+ if (i != MX) BAND_COL_ELEM(kthCol,k+MY,k) = hordc - horac;
+ if (j != 1) BAND_COL_ELEM(kthCol,k-1,k) = verdc;
+ if (j != MY) BAND_COL_ELEM(kthCol,k+1,k) = verdc;
+ }
+ }
+
+ return(0);
+}
+
+
+/* Set initial conditions in u vector */
+static void SetIC(N_Vector u, UserData data)
+{
+ int i, j;
+ realtype x, y, dx, dy;
+ realtype *udata;
+
+ /* Extract needed constants from data */
+
+ dx = data->dx;
+ dy = data->dy;
+
+ /* Set pointer to data array in vector u. */
+
+ udata = NV_DATA_S(u);
+
+ /* Load initial profile into u vector */
+
+ for (j=1; j <= MY; j++) {
+ y = j*dy;
+ for (i=1; i <= MX; i++) {
+ x = i*dx;
+ IJth(udata,i,j) = x*(XMAX - x)*y*(YMAX - y)*EXP(FIVE*x*y);
+ }
+ }
+}
+
+/* Print first lines of output (problem description) */
+static void PrintHeader(realtype reltol, realtype abstol, realtype umax)
+{
+ printf("\n2-D Advection-Diffusion Equation\n");
+ printf("Mesh dimensions = %d X %d\n", MX, MY);
+ printf("Total system size = %d\n", NEQ);
+ printf("Tolerance parameters: reltol = %lg abstol = %lg\n\n", reltol, abstol);
+ printf("At t = %lg max.norm(u) =%14.6le \n", T0, umax);
+
+ return;
+}
+
+/* Print current value */
+static void PrintOutput(realtype t, realtype umax, long int nst)
+{
+ printf("At t = %4.2f max.norm(u) =%14.6le nst = %4ld\n", t, umax, nst);
+ return;
+}
+
+/* Get and print some final statistics */
+
+static void PrintFinalStats(void *cvode_mem)
+{
+ int flag;
+ long int nst, nfe, nsetups, netf, nni, ncfn, nje, nfeLS;
+
+ flag = CPodeGetNumSteps(cvode_mem, &nst);
+ flag = CPodeGetNumFctEvals(cvode_mem, &nfe);
+ flag = CPodeGetNumLinSolvSetups(cvode_mem, &nsetups);
+ flag = CPodeGetNumErrTestFails(cvode_mem, &netf);
+ flag = CPodeGetNumNonlinSolvIters(cvode_mem, &nni);
+ flag = CPodeGetNumNonlinSolvConvFails(cvode_mem, &ncfn);
+
+ flag = CPDlsGetNumJacEvals(cvode_mem, &nje);
+ flag = CPDlsGetNumFctEvals(cvode_mem, &nfeLS);
+
+ printf("\nFinal Statistics:\n");
+ printf("nst = %-6ld nfe = %-6ld nsetups = %-6ld nfeLS = %-6ld nje = %ld\n",
+ nst, nfe, nsetups, nfeLS, nje);
+ printf("nni = %-6ld ncfn = %-6ld netf = %ld\n \n",
+ nni, ncfn, netf);
+
+ return;
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsbanx_lap.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsbanx_lap.c
new file mode 100644
index 0000000..b877b88
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsbanx_lap.c
@@ -0,0 +1,265 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <cpodes/cpodes.h>
+#include <cpodes/cpodes_lapack.h>
+#include <nvector/nvector_serial.h>
+#include <sundials/sundials_math.h>
+
+/* Problem Constants */
+
+#define XMAX RCONST(2.0) /* domain boundaries */
+#define YMAX RCONST(1.0)
+#define MX 10 /* mesh dimensions */
+#define MY 5
+#define NEQ MX*MY /* number of equations */
+#define ATOL RCONST(1.0e-5) /* scalar absolute tolerance */
+#define T0 RCONST(0.0) /* initial time */
+#define T1 RCONST(0.1) /* first output time */
+#define DTOUT RCONST(0.1) /* output time increment */
+#define NOUT 10 /* number of output times */
+
+#define ZERO RCONST(0.0)
+#define HALF RCONST(0.5)
+#define ONE RCONST(1.0)
+#define TWO RCONST(2.0)
+#define FIVE RCONST(5.0)
+
+#define IJth(vdata,i,j) (vdata[(j-1) + (i-1)*MY])
+
+typedef struct {
+ realtype dx, dy, hdcoef, hacoef, vdcoef;
+} *UserData;
+
+/* Private Helper Functions */
+static void SetIC(N_Vector u, UserData data);
+static void PrintHeader(realtype reltol, realtype abstol, realtype umax);
+static void PrintOutput(realtype t, realtype umax, long int nst);
+static void PrintFinalStats(void *cvode_mem);
+
+/* Functions Called by the Solver */
+static int f(realtype t, N_Vector u, N_Vector udot, void *f_data);
+static int Jac(int N, int mu, int ml,
+ realtype t, N_Vector u, N_Vector fu,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+/*
+ *-------------------------------
+ * Main Program
+ *-------------------------------
+ */
+
+int main(void)
+{
+ realtype dx, dy, reltol, abstol, t, tout, umax;
+ N_Vector u, up;
+ UserData data;
+ void *cvode_mem;
+ int iout, flag;
+ long int nst;
+
+ u = NULL;
+ data = NULL;
+ cvode_mem = NULL;
+
+ u = N_VNew_Serial(NEQ);
+ up = N_VNew_Serial(NEQ);
+
+ reltol = ZERO;
+ abstol = ATOL;
+
+ data = (UserData) malloc(sizeof *data);
+ dx = data->dx = XMAX/(MX+1);
+ dy = data->dy = YMAX/(MY+1);
+ data->hdcoef = ONE/(dx*dx);
+ data->hacoef = HALF/(TWO*dx);
+ data->vdcoef = ONE/(dy*dy);
+
+ SetIC(u, data);
+
+ cvode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
+ flag = CPodeInit(cvode_mem, (void *)f, data, T0, u, NULL, CP_SS, reltol, &abstol);
+
+ flag = CPLapackBand(cvode_mem, NEQ, MY, MY);
+ flag = CPDlsSetJacFn(cvode_mem, (void *)Jac, data);
+
+ /* In loop over output points: call CPode, print results, test for errors */
+ umax = N_VMaxNorm(u);
+ PrintHeader(reltol, abstol, umax);
+ for(iout=1, tout=T1; iout <= NOUT; iout++, tout += DTOUT) {
+ flag = CPode(cvode_mem, tout, &t, u, up, CP_NORMAL);
+ umax = N_VMaxNorm(u);
+ flag = CPodeGetNumSteps(cvode_mem, &nst);
+ PrintOutput(t, umax, nst);
+ }
+
+ PrintFinalStats(cvode_mem);
+
+ N_VDestroy_Serial(u);
+ CPodeFree(&cvode_mem);
+ free(data);
+
+ return(0);
+}
+
+/* f routine. Compute f(t,u). */
+static int f(realtype t, N_Vector u,N_Vector udot, void *f_data)
+{
+ realtype uij, udn, uup, ult, urt, hordc, horac, verdc, hdiff, hadv, vdiff;
+ realtype *udata, *dudata;
+ int i, j;
+ UserData data;
+
+ udata = NV_DATA_S(u);
+ dudata = NV_DATA_S(udot);
+
+ /* Extract needed constants from data */
+
+ data = (UserData) f_data;
+ hordc = data->hdcoef;
+ horac = data->hacoef;
+ verdc = data->vdcoef;
+
+ /* Loop over all grid points. */
+
+ for (j=1; j <= MY; j++) {
+
+ for (i=1; i <= MX; i++) {
+
+ /* Extract u at x_i, y_j and four neighboring points */
+
+ uij = IJth(udata, i, j);
+ udn = (j == 1) ? ZERO : IJth(udata, i, j-1);
+ uup = (j == MY) ? ZERO : IJth(udata, i, j+1);
+ ult = (i == 1) ? ZERO : IJth(udata, i-1, j);
+ urt = (i == MX) ? ZERO : IJth(udata, i+1, j);
+
+ /* Set diffusion and advection terms and load into udot */
+
+ hdiff = hordc*(ult - TWO*uij + urt);
+ hadv = horac*(urt - ult);
+ vdiff = verdc*(uup - TWO*uij + udn);
+ IJth(dudata, i, j) = hdiff + hadv + vdiff;
+ }
+ }
+
+ return(0);
+}
+
+/* Jacobian routine. Compute J(t,u). */
+static int Jac(int N, int mu, int ml,
+ realtype t, N_Vector u, N_Vector fu,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ int i, j, k;
+ realtype *kthCol, hordc, horac, verdc;
+ UserData data;
+
+ /*
+ The components of f = udot that depend on u(i,j) are
+ f(i,j), f(i-1,j), f(i+1,j), f(i,j-1), f(i,j+1), with
+ df(i,j)/du(i,j) = -2 (1/dx^2 + 1/dy^2)
+ df(i-1,j)/du(i,j) = 1/dx^2 + .25/dx (if i > 1)
+ df(i+1,j)/du(i,j) = 1/dx^2 - .25/dx (if i < MX)
+ df(i,j-1)/du(i,j) = 1/dy^2 (if j > 1)
+ df(i,j+1)/du(i,j) = 1/dy^2 (if j < MY)
+ */
+
+ data = (UserData) jac_data;
+ hordc = data->hdcoef;
+ horac = data->hacoef;
+ verdc = data->vdcoef;
+
+ for (j=1; j <= MY; j++) {
+ for (i=1; i <= MX; i++) {
+ k = j-1 + (i-1)*MY;
+ kthCol = BAND_COL(J,k);
+
+ /* set the kth column of J */
+
+ BAND_COL_ELEM(kthCol,k,k) = -TWO*(verdc+hordc);
+ if (i != 1) BAND_COL_ELEM(kthCol,k-MY,k) = hordc + horac;
+ if (i != MX) BAND_COL_ELEM(kthCol,k+MY,k) = hordc - horac;
+ if (j != 1) BAND_COL_ELEM(kthCol,k-1,k) = verdc;
+ if (j != MY) BAND_COL_ELEM(kthCol,k+1,k) = verdc;
+ }
+ }
+
+ return(0);
+}
+
+
+/* Set initial conditions in u vector */
+static void SetIC(N_Vector u, UserData data)
+{
+ int i, j;
+ realtype x, y, dx, dy;
+ realtype *udata;
+
+ /* Extract needed constants from data */
+
+ dx = data->dx;
+ dy = data->dy;
+
+ /* Set pointer to data array in vector u. */
+
+ udata = NV_DATA_S(u);
+
+ /* Load initial profile into u vector */
+
+ for (j=1; j <= MY; j++) {
+ y = j*dy;
+ for (i=1; i <= MX; i++) {
+ x = i*dx;
+ IJth(udata,i,j) = x*(XMAX - x)*y*(YMAX - y)*EXP(FIVE*x*y);
+ }
+ }
+}
+
+/* Print first lines of output (problem description) */
+static void PrintHeader(realtype reltol, realtype abstol, realtype umax)
+{
+ printf("\n2-D Advection-Diffusion Equation\n");
+ printf("Mesh dimensions = %d X %d\n", MX, MY);
+ printf("Total system size = %d\n", NEQ);
+ printf("Tolerance parameters: reltol = %lg abstol = %lg\n\n", reltol, abstol);
+ printf("At t = %lg max.norm(u) =%14.6le \n", T0, umax);
+
+ return;
+}
+
+/* Print current value */
+static void PrintOutput(realtype t, realtype umax, long int nst)
+{
+ printf("At t = %4.2f max.norm(u) =%14.6le nst = %4ld\n", t, umax, nst);
+ return;
+}
+
+/* Get and print some final statistics */
+
+static void PrintFinalStats(void *cvode_mem)
+{
+ int flag;
+ long int nst, nfe, nsetups, netf, nni, ncfn, nje, nfeLS;
+
+ flag = CPodeGetNumSteps(cvode_mem, &nst);
+ flag = CPodeGetNumFctEvals(cvode_mem, &nfe);
+ flag = CPodeGetNumLinSolvSetups(cvode_mem, &nsetups);
+ flag = CPodeGetNumErrTestFails(cvode_mem, &netf);
+ flag = CPodeGetNumNonlinSolvIters(cvode_mem, &nni);
+ flag = CPodeGetNumNonlinSolvConvFails(cvode_mem, &ncfn);
+
+ flag = CPDlsGetNumJacEvals(cvode_mem, &nje);
+ flag = CPDlsGetNumFctEvals(cvode_mem, &nfeLS);
+
+ printf("\nFinal Statistics:\n");
+ printf("nst = %-6ld nfe = %-6ld nsetups = %-6ld nfeLS = %-6ld nje = %ld\n",
+ nst, nfe, nsetups, nfeLS, nje);
+ printf("nni = %-6ld ncfn = %-6ld netf = %ld\n \n",
+ nni, ncfn, netf);
+
+ return;
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsdenx.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsdenx.c
new file mode 100644
index 0000000..f0a9d8b
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsdenx.c
@@ -0,0 +1,287 @@
+#include <stdio.h>
+
+#include <cpodes/cpodes.h>
+#include <cpodes/cpodes_dense.h>
+#include <nvector/nvector_serial.h>
+
+#define Ith(v,i) NV_Ith_S(v,i-1)
+#define IJth(A,i,j) DENSE_ELEM(A,i-1,j-1)
+
+/* Problem Constants */
+
+#define ODE CP_EXPL
+
+#define NEQ 3
+#define Y1 RCONST(1.0)
+#define Y2 RCONST(0.0)
+#define Y3 RCONST(0.0)
+
+#define RTOL RCONST(1.0e-4) /* 1e-4 */
+#define ATOL1 RCONST(1.0e-8) /* 1e-8 */
+#define ATOL2 RCONST(1.0e-14) /* 1e-14 */
+#define ATOL3 RCONST(1.0e-6) /* 1e-6 */
+
+#define T0 RCONST(0.0)
+#define T1 RCONST(0.4)
+#define TMULT RCONST(10.0)
+#define NOUT 12
+
+#define ONE RCONST(1.0)
+#define ZERO RCONST(0.0)
+
+/* Functions Called by the Solver */
+static int res(realtype t, N_Vector y, N_Vector yp, N_Vector res, void *f_data);
+static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data);
+static int g(realtype t, N_Vector y, N_Vector yp, realtype *gout, void *g_data);
+static int jacI(int N, realtype t, realtype gm,
+ N_Vector y, N_Vector yp, N_Vector r,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+static int jacE(int N, realtype t,
+ N_Vector y, N_Vector yp,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+
+static void PrintFinalStats(void *cpode_mem);
+
+/*
+ *-------------------------------
+ * Main Program
+ *-------------------------------
+ */
+
+int main()
+{
+ void *fct, *jac;
+ void *cpode_mem;
+ N_Vector yy, yp, abstol;
+ realtype reltol, t, tout;
+ int flag, flagr, iout;
+ int rootsfound[2];
+
+ /* Create serial vectors of length NEQ for I.C. and abstol */
+ yy = N_VNew_Serial(NEQ);
+ yp = N_VNew_Serial(NEQ);
+ abstol = N_VNew_Serial(NEQ);
+
+ /* Initialize y */
+ Ith(yy,1) = Y1;
+ Ith(yy,2) = Y2;
+ Ith(yy,3) = Y3;
+
+ /* Set tolerances */
+ reltol = RTOL;
+ Ith(abstol,1) = ATOL1;
+ Ith(abstol,2) = ATOL2;
+ Ith(abstol,3) = ATOL3;
+
+
+ if (ODE == CP_EXPL) {
+ fct = (void *)f;
+ jac = (void *)jacE;
+ } else {
+ f(T0, yy, yp, NULL);
+ fct = (void *)res;
+ jac = (void *)jacI;
+ }
+
+ /* Initialize solver */
+ cpode_mem = CPodeCreate(ODE, CP_BDF, CP_NEWTON);
+ flag = CPodeInit(cpode_mem, fct, NULL, T0, yy, yp, CP_SV, reltol, abstol);
+
+ /* Set initial step size */
+ /*
+ {
+ realtype h0;
+ h0 = 8.5e-14;
+ flag = CPodeSetInitStep(cpode_mem, h0);
+ }
+ */
+
+ /* Call CPodeRootInit to specify the root function g with 2 components */
+ flag = CPodeRootInit(cpode_mem, 2, g, NULL);
+
+ /* Call CPDense to specify the CPDENSE dense linear solver */
+ flag = CPDense(cpode_mem, NEQ);
+
+ /* Set the Jacobian routine (comment out for internal DQ) */
+ flag = CPDlsSetJacFn(cpode_mem, jac, NULL);
+
+ /* In loop, call CPode, print results, and test for error.
+ Break out of loop when NOUT preset output times have been reached. */
+ iout = 0; tout = T1;
+ while(1) {
+ flag = CPode(cpode_mem, tout, &t, yy, yp, CP_NORMAL);
+ printf("At t = %0.4le y =%14.6le %14.6le %14.6le\n",
+ t, Ith(yy,1), Ith(yy,2), Ith(yy,3));
+
+ if (flag < 0) break;
+
+ if (flag == CP_ROOT_RETURN) {
+ flagr = CPodeGetRootInfo(cpode_mem, rootsfound);
+ printf(" rootsfound[] = %3d %3d\n", rootsfound[0],rootsfound[1]);
+ }
+
+ if (flag == CP_SUCCESS) {
+ iout++;
+ tout *= TMULT;
+ }
+
+ if (iout == NOUT) break;
+ }
+
+ /* Print some final statistics */
+ PrintFinalStats(cpode_mem);
+
+ /* Free memory */
+ N_VDestroy_Serial(yy);
+ N_VDestroy_Serial(yp);
+ N_VDestroy_Serial(abstol);
+ CPodeFree(&cpode_mem);
+
+ return(0);
+}
+
+
+/*
+ *-------------------------------
+ * Functions called by the solver
+ *-------------------------------
+ */
+
+/*
+ * residual function: F(y',y) = y' - f(y)
+ */
+
+static int res(realtype t, N_Vector y, N_Vector yp, N_Vector res, void *f_data)
+{
+ realtype y1, y2, y3, yp1, yp2, yp3;
+
+ y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
+ yp1 = Ith(yp,1); yp2 = Ith(yp,2); yp3 = Ith(yp,3);
+
+ Ith(res,1) = yp1 - (RCONST(-0.04)*y1 + RCONST(1.0e4)*y2*y3);
+ Ith(res,2) = yp2 + (RCONST(-0.04)*y1 + RCONST(1.0e4)*y2*y3 + RCONST(3.0e7)*y2*y2);
+ Ith(res,3) = yp3 - RCONST(3.0e7)*y2*y2;
+
+ return(0);
+}
+
+/*
+ * f routine. Compute function f(t,y).
+ */
+
+static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data)
+{
+ realtype y1, y2, y3, yd1, yd3;
+
+ y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
+
+ yd1 = Ith(ydot,1) = RCONST(-0.04)*y1 + RCONST(1.0e4)*y2*y3;
+ yd3 = Ith(ydot,3) = RCONST(3.0e7)*y2*y2;
+ Ith(ydot,2) = -yd1 - yd3;
+
+ return(0);
+}
+
+/*
+ * Jacobian routine. Compute J(t,y) = dF/dy' + gm * dF/dy = I - gm*df/dy.
+ */
+static int jacI(int N, realtype t, realtype gm,
+ N_Vector y, N_Vector yp, N_Vector r,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+
+ realtype y1, y2, y3;
+
+ y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
+
+ IJth(J,1,1) = ONE - gm*RCONST(-0.04);
+ IJth(J,1,2) = -gm*RCONST(1.0e4)*y3;
+ IJth(J,1,3) = -gm*RCONST(1.0e4)*y2;
+
+ IJth(J,2,1) = -gm*RCONST(0.04);
+ IJth(J,2,2) = ONE - gm * (RCONST(-1.0e4)*y3 - RCONST(6.0e7)*y2);
+ IJth(J,2,3) = -gm*RCONST(-1.0e4)*y2;
+
+ IJth(J,3,1) = ZERO;
+ IJth(J,3,2) = -gm*RCONST(6.0e7)*y2;
+ IJth(J,3,3) = ONE;
+
+ return(0);
+}
+
+/*
+ * Jacobian routine. Compute J(t,y) = df/dy. *
+ */
+
+static int jacE(int N, realtype t,
+ N_Vector y, N_Vector fy,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ realtype y1, y2, y3;
+
+ y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
+
+ IJth(J,1,1) = RCONST(-0.04);
+ IJth(J,1,2) = RCONST(1.0e4)*y3;
+ IJth(J,1,3) = RCONST(1.0e4)*y2;
+ IJth(J,2,1) = RCONST(0.04);
+ IJth(J,2,2) = RCONST(-1.0e4)*y3-RCONST(6.0e7)*y2;
+ IJth(J,2,3) = RCONST(-1.0e4)*y2;
+ IJth(J,3,1) = ZERO;
+ IJth(J,3,2) = RCONST(6.0e7)*y2;
+ IJth(J,3,3) = ZERO;
+
+ return(0);
+}
+
+
+/*
+ * g routine. Compute functions g_i(t,y) for i = 0,1.
+ */
+
+static int g(realtype t, N_Vector y, N_Vector yp, realtype *gout, void *g_data)
+{
+ realtype y1, y3;
+
+ y1 = Ith(y,1); y3 = Ith(y,3);
+ gout[0] = y1 - RCONST(0.0001);
+ gout[1] = y3 - RCONST(0.01);
+
+ return(0);
+}
+
+/*
+ * Get and print some final statistics
+ */
+
+static void PrintFinalStats(void *cpode_mem)
+{
+ realtype h0u;
+ long int nst, nfe, nsetups, nje, nfeLS, nni, ncfn, netf, nge;
+ int flag;
+
+ flag = CPodeGetActualInitStep(cpode_mem, &h0u);
+ flag = CPodeGetNumSteps(cpode_mem, &nst);
+ flag = CPodeGetNumFctEvals(cpode_mem, &nfe);
+ flag = CPodeGetNumLinSolvSetups(cpode_mem, &nsetups);
+ flag = CPodeGetNumErrTestFails(cpode_mem, &netf);
+ flag = CPodeGetNumNonlinSolvIters(cpode_mem, &nni);
+ flag = CPodeGetNumNonlinSolvConvFails(cpode_mem, &ncfn);
+
+ flag = CPDlsGetNumJacEvals(cpode_mem, &nje);
+ flag = CPDlsGetNumFctEvals(cpode_mem, &nfeLS);
+
+ flag = CPodeGetNumGEvals(cpode_mem, &nge);
+
+ printf("\nFinal Statistics:\n");
+ printf("h0u = %g\n",h0u);
+ printf("nst = %-6ld nfe = %-6ld nsetups = %-6ld nfeLS = %-6ld nje = %ld\n",
+ nst, nfe, nsetups, nfeLS, nje);
+ printf("nni = %-6ld ncfn = %-6ld netf = %-6ld nge = %ld\n \n",
+ nni, ncfn, netf, nge);
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsdenx_lap.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsdenx_lap.c
new file mode 100644
index 0000000..7fb0806
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/cpsdenx_lap.c
@@ -0,0 +1,290 @@
+#include <stdio.h>
+
+#include <cpodes/cpodes.h>
+#include <cpodes/cpodes_lapack.h>
+#include <nvector/nvector_serial.h>
+
+#define Ith(v,i) NV_Ith_S(v,i-1)
+#define IJth(A,i,j) DENSE_ELEM(A,i-1,j-1)
+
+/* Problem Constants */
+
+#define ODE CP_EXPL
+
+#define NEQ 3
+#define Y1 RCONST(1.0)
+#define Y2 RCONST(0.0)
+#define Y3 RCONST(0.0)
+
+#define RTOL RCONST(1.0e-4) /* 1e-4 */
+#define ATOL1 RCONST(1.0e-8) /* 1e-8 */
+#define ATOL2 RCONST(1.0e-14) /* 1e-14 */
+#define ATOL3 RCONST(1.0e-6) /* 1e-6 */
+
+#define T0 RCONST(0.0)
+#define T1 RCONST(0.4)
+#define TMULT RCONST(10.0)
+#define NOUT 12
+
+#define ONE RCONST(1.0)
+#define ZERO RCONST(0.0)
+
+/* Functions Called by the Solver */
+static int res(realtype t, N_Vector y, N_Vector yp, N_Vector res, void *f_data);
+static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data);
+static int g(realtype t, N_Vector y, N_Vector yp, realtype *gout, void *g_data);
+static int jacI(int N, realtype t, realtype gm,
+ N_Vector y, N_Vector yp, N_Vector r,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+static int jacE(int N, realtype t,
+ N_Vector y, N_Vector yp,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3);
+
+
+static void PrintFinalStats(void *cpode_mem);
+
+/*
+ *-------------------------------
+ * Main Program
+ *-------------------------------
+ */
+
+int main()
+{
+ void *fct, *jac;
+ void *cpode_mem;
+ N_Vector yy, yp, abstol;
+ realtype reltol, t, tout;
+ int flag, flagr, iout;
+ int rootsfound[2];
+
+ /* Create serial vectors of length NEQ for I.C. and abstol */
+ yy = N_VNew_Serial(NEQ);
+ yp = N_VNew_Serial(NEQ);
+ abstol = N_VNew_Serial(NEQ);
+
+ /* Initialize y */
+ Ith(yy,1) = Y1;
+ Ith(yy,2) = Y2;
+ Ith(yy,3) = Y3;
+
+ /* Set tolerances */
+ reltol = RTOL;
+ Ith(abstol,1) = ATOL1;
+ Ith(abstol,2) = ATOL2;
+ Ith(abstol,3) = ATOL3;
+
+
+ if (ODE == CP_EXPL) {
+ fct = (void *)f;
+ jac = (void *)jacE;
+ } else {
+ f(T0, yy, yp, NULL);
+ fct = (void *)res;
+ jac = (void *)jacI;
+ }
+
+ /* Initialize solver */
+ cpode_mem = CPodeCreate(ODE, CP_BDF, CP_NEWTON);
+ flag = CPodeInit(cpode_mem, fct, NULL, T0, yy, yp, CP_SV, reltol, abstol);
+
+ /* Set initial step size */
+ /*
+ {
+ realtype h0;
+ h0 = 8.5e-14;
+ flag = CPodeSetInitStep(cpode_mem, h0);
+ }
+ */
+
+ /* Call CPodeRootInit to specify the root function g with 2 components */
+ flag = CPodeRootInit(cpode_mem, 2, g, NULL);
+
+ /* Call CPLapackDense to specify the CPLAPACK dense linear solver */
+ flag = CPLapackDense(cpode_mem, NEQ);
+
+ /* Set the Jacobian routine to jac (comment out for internal DQ) */
+ flag = CPDlsSetJacFn(cpode_mem, jac, NULL);
+
+ /* In loop, call CPode, print results, and test for error.
+ Break out of loop when NOUT preset output times have been reached. */
+ iout = 0; tout = T1;
+ while(1) {
+ flag = CPode(cpode_mem, tout, &t, yy, yp, CP_NORMAL);
+ printf("At t = %0.4le y =%14.6le %14.6le %14.6le\n",
+ t, Ith(yy,1), Ith(yy,2), Ith(yy,3));
+
+ if (flag < 0) break;
+
+ if (flag == CP_ROOT_RETURN) {
+ flagr = CPodeGetRootInfo(cpode_mem, rootsfound);
+ printf(" rootsfound[] = %3d %3d\n", rootsfound[0],rootsfound[1]);
+ }
+
+ if (flag == CP_SUCCESS) {
+ iout++;
+ tout *= TMULT;
+ }
+
+ if (iout == NOUT) break;
+ }
+
+ /* Print some final statistics */
+ PrintFinalStats(cpode_mem);
+
+ /* Free memory */
+ N_VDestroy_Serial(yy);
+ N_VDestroy_Serial(yp);
+ N_VDestroy_Serial(abstol);
+ CPodeFree(&cpode_mem);
+
+ return(0);
+}
+
+
+/*
+ *-------------------------------
+ * Functions called by the solver
+ *-------------------------------
+ */
+
+/*
+ * residual function: F(y',y) = y' - f(y)
+ */
+
+static int res(realtype t, N_Vector y, N_Vector yp, N_Vector res, void *f_data)
+{
+ realtype y1, y2, y3, yp1, yp2, yp3;
+
+ y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
+ yp1 = Ith(yp,1); yp2 = Ith(yp,2); yp3 = Ith(yp,3);
+
+ Ith(res,1) = yp1 - (RCONST(-0.04)*y1 + RCONST(1.0e4)*y2*y3);
+ Ith(res,2) = yp2 + (RCONST(-0.04)*y1 + RCONST(1.0e4)*y2*y3 + RCONST(3.0e7)*y2*y2);
+ Ith(res,3) = yp3 - RCONST(3.0e7)*y2*y2;
+
+ return(0);
+}
+
+/*
+ * f routine. Compute function f(t,y).
+ */
+
+static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data)
+{
+ realtype y1, y2, y3, yd1, yd3;
+
+ y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
+
+ yd1 = Ith(ydot,1) = RCONST(-0.04)*y1 + RCONST(1.0e4)*y2*y3;
+ yd3 = Ith(ydot,3) = RCONST(3.0e7)*y2*y2;
+ Ith(ydot,2) = -yd1 - yd3;
+
+ return(0);
+}
+
+/*
+ * Jacobian routine. Compute J(t,y) = dF/dy' + gm * dF/dy = I - gm*df/dy.
+ */
+static int jacI(int N, realtype t, realtype gm,
+ N_Vector y, N_Vector yp, N_Vector r,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+
+ realtype y1, y2, y3;
+
+ y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
+
+ IJth(J,1,1) = ONE - gm*RCONST(-0.04);
+ IJth(J,1,2) = -gm*RCONST(1.0e4)*y3;
+ IJth(J,1,3) = -gm*RCONST(1.0e4)*y2;
+
+ IJth(J,2,1) = -gm*RCONST(0.04);
+ IJth(J,2,2) = ONE - gm * (RCONST(-1.0e4)*y3 - RCONST(6.0e7)*y2);
+ IJth(J,2,3) = -gm*RCONST(-1.0e4)*y2;
+
+ IJth(J,3,1) = ZERO;
+ IJth(J,3,2) = -gm*RCONST(6.0e7)*y2;
+ IJth(J,3,3) = ONE;
+
+ return(0);
+}
+
+/*
+ * Jacobian routine. Compute J(t,y) = df/dy. *
+ */
+
+static int jacE(int N, realtype t,
+ N_Vector y, N_Vector fy,
+ DlsMat J, void *jac_data,
+ N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
+{
+ realtype y1, y2, y3;
+
+ y1 = Ith(y,1); y2 = Ith(y,2); y3 = Ith(y,3);
+
+ IJth(J,1,1) = RCONST(-0.04);
+ IJth(J,1,2) = RCONST(1.0e4)*y3;
+ IJth(J,1,3) = RCONST(1.0e4)*y2;
+
+ IJth(J,2,1) = RCONST(0.04);
+ IJth(J,2,2) = RCONST(-1.0e4)*y3-RCONST(6.0e7)*y2;
+ IJth(J,2,3) = RCONST(-1.0e4)*y2;
+
+ IJth(J,3,1) = ZERO;
+ IJth(J,3,2) = RCONST(6.0e7)*y2;
+ IJth(J,3,3) = ZERO;
+
+ return(0);
+}
+
+
+/*
+ * g routine. Compute functions g_i(t,y) for i = 0,1.
+ */
+
+static int g(realtype t, N_Vector y, N_Vector yp, realtype *gout, void *g_data)
+{
+ realtype y1, y3;
+
+ y1 = Ith(y,1); y3 = Ith(y,3);
+ gout[0] = y1 - RCONST(0.0001);
+ gout[1] = y3 - RCONST(0.01);
+
+ return(0);
+}
+
+/*
+ * Get and print some final statistics
+ */
+
+static void PrintFinalStats(void *cpode_mem)
+{
+ realtype h0u;
+ long int nst, nfe, nsetups, nje, nfeLS, nni, ncfn, netf, nge;
+ int flag;
+
+ flag = CPodeGetActualInitStep(cpode_mem, &h0u);
+ flag = CPodeGetNumSteps(cpode_mem, &nst);
+ flag = CPodeGetNumFctEvals(cpode_mem, &nfe);
+ flag = CPodeGetNumLinSolvSetups(cpode_mem, &nsetups);
+ flag = CPodeGetNumErrTestFails(cpode_mem, &netf);
+ flag = CPodeGetNumNonlinSolvIters(cpode_mem, &nni);
+ flag = CPodeGetNumNonlinSolvConvFails(cpode_mem, &ncfn);
+
+ flag = CPDlsGetNumJacEvals(cpode_mem, &nje);
+ flag = CPDlsGetNumFctEvals(cpode_mem, &nfeLS);
+
+ flag = CPodeGetNumGEvals(cpode_mem, &nge);
+
+ printf("\nFinal Statistics:\n");
+ printf("h0u = %g\n",h0u);
+ printf("nst = %-6ld nfe = %-6ld nsetups = %-6ld nfeLS = %-6ld nje = %ld\n",
+ nst, nfe, nsetups, nfeLS, nje);
+ printf("nni = %-6ld ncfn = %-6ld netf = %-6ld nge = %ld\n \n",
+ nni, ncfn, netf, nge);
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/initroot1.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/initroot1.c
new file mode 100644
index 0000000..25494aa
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/initroot1.c
@@ -0,0 +1,151 @@
+#include <stdio.h>
+#include <math.h>
+#include <cpodes/cpodes.h>
+#include <cpodes/cpodes_dense.h>
+#include <nvector/nvector_serial.h>
+
+#define Ith(v,i) NV_Ith_S(v,i-1)
+
+#define RTOL RCONST(1.0e-8)
+#define ATOL RCONST(1.0e-8)
+
+static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data);
+
+static int g(realtype t, N_Vector y, N_Vector yp, realtype *gout, void *g_data);
+
+static void PrintFinalStats(void *cpode_mem);
+
+
+/*
+ *-------------------------------
+ * Main Program
+ *-------------------------------
+ */
+
+int main()
+{
+ realtype reltol, abstol, t, t0, tout, delt;
+ N_Vector yy, yp;
+ void *cpode_mem;
+ int flag;
+ int rdir[3], iroots[3];
+ realtype gout[3];
+
+ t0 = 0.0;
+ tout = 10.0;
+ delt = 0.01;
+
+ yy = N_VNew_Serial(1);
+ yp = N_VNew_Serial(1);
+ Ith(yy,1) = 0.0;
+ Ith(yp,1) = 0.0;
+
+ reltol = RTOL;
+ abstol = ATOL;
+
+ cpode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
+ flag = CPodeInit(cpode_mem, (void *)f, NULL, t0, yy, NULL, CP_SS, reltol, &abstol);
+ flag = CPDense(cpode_mem, 1);
+ flag = CPodeSetStopTime(cpode_mem, tout);
+ flag = CPodeRootInit(cpode_mem, 3, g, NULL);
+
+ rdir[0] = -1;
+ rdir[1] = 0;
+ rdir[2] = 0;
+ flag = CPodeSetRootDirection(cpode_mem, rdir);
+
+ t = t0;
+ g(t, yy, yp, gout, NULL);
+ printf("%le %le %le %le %le 0 0 0\n", t, Ith(yy,1), gout[0], gout[1], gout[2]);
+
+ while(t<tout) {
+
+ flag = CPode(cpode_mem, tout, &t, yy, yp, CP_ONE_STEP_TSTOP);
+ if (flag < 0) break;
+
+ g(t, yy, yp, gout, NULL);
+ printf("%le %le %le %le %le ", t, Ith(yy,1), gout[0], gout[1], gout[2]);
+ if (flag == CP_ROOT_RETURN) {
+ CPodeGetRootInfo(cpode_mem, iroots);
+ printf(" %d %d %d\n", iroots[0], iroots[1], iroots[2]);
+ } else {
+ printf("0 0 0\n");
+ }
+
+ }
+
+ // PrintFinalStats(cpode_mem);
+
+ /* Clean-up */
+
+ N_VDestroy_Serial(yy);
+ N_VDestroy_Serial(yp);
+ CPodeFree(&cpode_mem);
+
+ return(0);
+}
+
+
+/*
+ *-------------------------------
+ * Functions called by the solver
+ *-------------------------------
+ */
+
+static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data)
+{
+ Ith(ydot,1) = t*cos(t) + sin(t);
+ Ith(ydot,1) = cos(t);
+ return(0);
+}
+
+static int g(realtype t, N_Vector y, N_Vector yp, realtype *gout, void *g_data)
+{
+ gout[0] = Ith(y,1);
+
+ if (Ith(y,1)+0.41 >= 0) gout[1] = +0.6;
+ else gout[1] = -0.2;
+
+ if (Ith(y,1)-0.4 >= 0) gout[2] = -0.4;
+ else if (Ith(y,1)+0.4 <= 0) gout[2] = +0.4;
+ else gout[2] = 0.0;
+
+ return(0);
+}
+
+
+static void PrintFinalStats(void *cpode_mem)
+{
+ realtype h0u;
+ long int nst, nfe, nsetups, nje, nfeLS, nni, ncfn, netf, nge;
+ long int nproj, nce, nsetupsP, nprf;
+ int flag;
+
+ flag = CPodeGetActualInitStep(cpode_mem, &h0u);
+ flag = CPodeGetNumSteps(cpode_mem, &nst);
+ flag = CPodeGetNumFctEvals(cpode_mem, &nfe);
+ flag = CPodeGetNumLinSolvSetups(cpode_mem, &nsetups);
+ flag = CPodeGetNumErrTestFails(cpode_mem, &netf);
+ flag = CPodeGetNumNonlinSolvIters(cpode_mem, &nni);
+ flag = CPodeGetNumNonlinSolvConvFails(cpode_mem, &ncfn);
+
+ flag = CPDlsGetNumJacEvals(cpode_mem, &nje);
+ flag = CPDlsGetNumFctEvals(cpode_mem, &nfeLS);
+
+ flag = CPodeGetProjStats(cpode_mem, &nproj, &nce, &nsetupsP, &nprf);
+
+ flag = CPodeGetNumGEvals(cpode_mem, &nge);
+
+ printf("\nFinal Statistics:\n");
+ printf("h0u = %g\n",h0u);
+ printf("nst = %-6ld nfe = %-6ld nsetups = %-6ld\n",
+ nst, nfe, nsetups);
+ printf("nfeLS = %-6ld nje = %ld\n",
+ nfeLS, nje);
+ printf("nni = %-6ld ncfn = %-6ld netf = %-6ld \n",
+ nni, ncfn, netf);
+ printf("nproj = %-6ld nce = %-6ld nsetupsP = %-6ld nprf = %-6ld\n",
+ nproj, nce, nsetupsP, nprf);
+ printf("nge = %ld\n", nge);
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/newton.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/newton.c
new file mode 100644
index 0000000..1c0a049
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/newton.c
@@ -0,0 +1,484 @@
+/*
+ * -----------------------------------------------------------------
+ * $Revision:$
+ * $Date:$
+ * -----------------------------------------------------------------
+ * Programmer: Radu Serban @ LLNL
+ * -----------------------------------------------------------------
+ * Example problem for the event detection in CPODES: a 2-pendulum
+ * Newton's craddle. Two identical pendulums of length L and
+ * mass M are suspended at points (+R,0) and (-R,0), where R is the
+ * radius of the balls.
+ * We consider elastic impact with a coefficient of restitution C=1.
+ *
+ * Case 1: The first pendulum has an initial horizontal
+ * velocity V0, while the second one is at rest.
+ * Case 2: Both pendulums are initially at rest. The first one is
+ * then moved from rest after a short interval, by applying
+ * an external force, F = m*frc(t), where
+ * frc(t) = 3(1-cos(5t)) on t =[0,2*pi/5] and 0 otherwise.
+ *
+ * Each pendulum is modeled with 2 coordinates (x and y) and one
+ * constraint (x^2 + y^2 = L^2), resulting in a 1st order system
+ * with 8 diferential equations and 4 constraints.
+ * -----------------------------------------------------------------
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <cpodes/cpodes.h>
+#include <cpodes/cpodes_dense.h>
+#include <nvector/nvector_serial.h>
+
+/* User data structure */
+
+typedef struct {
+ realtype R;
+ realtype L;
+ realtype m;
+ realtype g;
+} *PbData;
+
+/* Functions Called by the Solver */
+
+static int ffun1(realtype t, N_Vector y, N_Vector ydot, void *f_data);
+static int ffun2(realtype t, N_Vector y, N_Vector ydot, void *f_data);
+
+static int gfun(realtype t, N_Vector y, N_Vector yp, realtype *gout, void *g_data);
+
+static int cfun(realtype t, N_Vector yy, N_Vector cout, void *c_data);
+
+static void contact(N_Vector yy, PbData data);
+
+static void PrintFinalStats(void *cpode_mem);
+
+/*
+ *-------------------------------
+ * Main Program
+ *-------------------------------
+ */
+
+int main()
+{
+ PbData data;
+ realtype R, L, m, g, V0;
+ void *cpode_mem;
+ N_Vector yy, yp, ctols;
+ realtype reltol, abstol;
+ realtype t0, tf, t;
+ realtype x1, y1, x2, y2;
+ int flag, Neq, Nc;
+ int rdir[1], iroots[1];
+
+ FILE *fout;
+
+ /* --------------------------------
+ * INITIALIZATIONS
+ * -------------------------------- */
+
+ R = 0.1; /* ball radius */
+ L = 1.0; /* pendulum length */
+ m = 1.0; /* pendulum mass */
+ g = 9.8; /* gravitational acc. */
+
+ V0 = 3.0; /* initial velocity for pendulum 1 */
+
+ /* Set-up user data structure */
+
+ data = (PbData)malloc(sizeof *data);
+ data->R = R;
+ data->L = L;
+ data->m = m;
+ data->g = g;
+
+ /* Problem dimensions */
+
+ Neq = 2*2*2;
+ Nc = 2*2;
+
+ /* Solution vectors */
+
+ yy = N_VNew_Serial(Neq);
+ yp = N_VNew_Serial(Neq);
+
+ /* Integration limits */
+
+ t0 = 0.0;
+ tf = 6.0;
+
+ /* Integration and projection tolerances */
+
+ reltol = 1.0e-8;
+ abstol = 1.0e-8;
+
+ ctols = N_VNew_Serial(Nc);
+ N_VConst(1.0e-8, ctols);
+
+ /* Direction of monitored events
+ * (only zero-crossing with decreasing even function) */
+
+ rdir[0] = -1;
+
+ /* --------------------------------
+ * CASE 1
+ * -------------------------------- */
+
+ fout = fopen("newton1.out","w");
+
+ /* Initial conditions */
+
+ NV_Ith_S(yy,0) = R; NV_Ith_S(yy,4) = -R;
+ NV_Ith_S(yy,1) = -L; NV_Ith_S(yy,5) = -L;
+ NV_Ith_S(yy,2) = V0; NV_Ith_S(yy,6) = 0.0;
+ NV_Ith_S(yy,3) = 0.0; NV_Ith_S(yy,7) = 0.0;
+
+ /* Initialize solver */
+
+ cpode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
+ flag = CPodeInit(cpode_mem, ffun1, data, t0, yy, yp, CP_SS, reltol, &abstol);
+ flag = CPDense(cpode_mem, Neq);
+
+ flag = CPodeRootInit(cpode_mem, 1, gfun, data);
+ flag = CPodeSetRootDirection(cpode_mem, rdir);
+
+ /* Set-up the internal projection */
+
+ flag = CPodeProjInit(cpode_mem, CP_PROJ_L2NORM, CP_CNSTR_NONLIN, cfun, data, ctols);
+ flag = CPodeSetProjTestCnstr(cpode_mem, TRUE);
+ flag = CPDenseProj(cpode_mem, Nc, Neq, CPDIRECT_LU);
+
+ /* Integrate in ONE_STEP mode, while monitoring events */
+
+ t = t0;
+ while(t<tf) {
+
+ flag = CPode(cpode_mem, tf, &t, yy, yp, CP_ONE_STEP);
+
+ if (flag < 0) break;
+
+ x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
+ y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
+ fprintf(fout, "%lf %14.10lf %14.10lf %14.10lf %14.10lf", t, x1, y1, x2, y2);
+
+ if (flag == CP_ROOT_RETURN) {
+
+ CPodeGetRootInfo(cpode_mem, iroots);
+ fprintf(fout, " %d\n", iroots[0]);
+
+ /* Note: the test iroots[0]<0 is really needed ONLY if not using rdir */
+
+ if (iroots[0] < 0) {
+ /* Update velocities in yy */
+ contact(yy, data);
+ /* reinitialize CPODES solver */
+ flag = CPodeReInit(cpode_mem, ffun1, data, t, yy, yp, CP_SS, reltol, &abstol);
+ }
+
+ } else {
+
+ fprintf(fout, " 0\n");
+
+ }
+
+ }
+
+ PrintFinalStats(cpode_mem);
+
+ CPodeFree(&cpode_mem);
+
+ fclose(fout);
+
+ /* --------------------------------
+ * CASE 2
+ * -------------------------------- */
+
+ fout = fopen("newton2.out","w");
+
+ /* Initial conditions */
+
+ NV_Ith_S(yy,0) = R; NV_Ith_S(yy,4) = -R;
+ NV_Ith_S(yy,1) = -L; NV_Ith_S(yy,5) = -L;
+ NV_Ith_S(yy,2) = 0.0; NV_Ith_S(yy,6) = 0.0;
+ NV_Ith_S(yy,3) = 0.0; NV_Ith_S(yy,7) = 0.0;
+
+ /* Initialize solver */
+
+ cpode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
+ flag = CPodeInit(cpode_mem, ffun2, data, t0, yy, yp, CP_SS, reltol, &abstol);
+ flag = CPDense(cpode_mem, Neq);
+
+ flag = CPodeRootInit(cpode_mem, 1, gfun, data);
+ flag = CPodeSetRootDirection(cpode_mem, rdir);
+
+ /* Set-up the internal projection */
+
+ flag = CPodeProjInit(cpode_mem, CP_PROJ_L2NORM, CP_CNSTR_NONLIN, cfun, data, ctols);
+ flag = CPodeSetProjTestCnstr(cpode_mem, TRUE);
+ flag = CPDenseProj(cpode_mem, Nc, Neq, CPDIRECT_LU);
+
+ /* Integrate in ONE_STEP mode, while monitoring events */
+
+ t = t0;
+ while(t<tf) {
+
+ flag = CPode(cpode_mem, tf, &t, yy, yp, CP_ONE_STEP);
+
+ if (flag < 0) break;
+
+ x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
+ y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
+ fprintf(fout, "%lf %14.10lf %14.10lf %14.10lf %14.10lf", t, x1, y1, x2, y2);
+
+ if (flag == CP_ROOT_RETURN) {
+
+ CPodeGetRootInfo(cpode_mem, iroots);
+ fprintf(fout, " %d\n", iroots[0]);
+
+ /* Note: the test iroots[0]<0 is really needed ONLY if not using rdir */
+
+ if (iroots[0] < 0) {
+ /* Update velocities in yy */
+ contact(yy, data);
+ /* reinitialize CPODES solver */
+ flag = CPodeReInit(cpode_mem, ffun2, data, t, yy, yp, CP_SS, reltol, &abstol);
+ }
+
+ } else {
+
+ fprintf(fout, " 0\n");
+
+ }
+
+ }
+
+ PrintFinalStats(cpode_mem);
+
+ CPodeFree(&cpode_mem);
+
+ fclose(fout);
+
+ /* --------------------------------
+ * CLEAN-UP
+ * -------------------------------- */
+
+ free(data);
+ N_VDestroy_Serial(yy);
+ N_VDestroy_Serial(yp);
+ N_VDestroy_Serial(ctols);
+
+ return(0);
+}
+
+
+static int ffun1(realtype t, N_Vector yy, N_Vector fy, void *f_data)
+{
+ PbData data;
+ realtype x1, y1, x2, y2;
+ realtype vx1, vy1, vx2, vy2;
+ realtype ax1, ay1, ax2, ay2;
+ realtype lam1, lam2;
+ realtype R, L, m, g;
+
+ data = (PbData) f_data;
+ R = data->R;
+ L = data->L;
+ m = data->m;
+ g = data->g;
+
+ x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
+ y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
+ vx1 = NV_Ith_S(yy,2); vx2 = NV_Ith_S(yy,6);
+ vy1 = NV_Ith_S(yy,3); vy2 = NV_Ith_S(yy,7);
+
+ lam1 = m/(2*L*L)*(vx1*vx1 + vy1*vy1 - g*y1);
+ lam2 = m/(2*L*L)*(vx2*vx2 + vy2*vy2 - g*y2);
+
+ ax1 = -2.0*(x1-R)*lam1/m;
+ ay1 = -2.0*y1*lam1/m - g;
+
+ ax2 = -2.0*(x2+R)*lam2/m;
+ ay2 = -2.0*y2*lam2/m - g;
+
+ NV_Ith_S(fy,0) = vx1; NV_Ith_S(fy, 4) = vx2;
+ NV_Ith_S(fy,1) = vy1; NV_Ith_S(fy, 5) = vy2;
+ NV_Ith_S(fy,2) = ax1; NV_Ith_S(fy, 6) = ax2;
+ NV_Ith_S(fy,3) = ay1; NV_Ith_S(fy, 7) = ay2;
+
+ return(0);
+}
+
+static int ffun2(realtype t, N_Vector yy, N_Vector fy, void *f_data)
+{
+ PbData data;
+ realtype x1, y1, x2, y2;
+ realtype vx1, vy1, vx2, vy2;
+ realtype ax1, ay1, ax2, ay2;
+ realtype lam1, lam2;
+ realtype R, L, m, g;
+ realtype pi, frc;
+
+
+ pi = 4.0*atan(1.0);
+ if ( (t <= 2*pi/5.0) ) {
+ frc = 3.0 * ( 1.0 - cos(5.0*t) );
+ } else {
+ frc = 0.0;
+ }
+
+ data = (PbData) f_data;
+ R = data->R;
+ L = data->L;
+ m = data->m;
+ g = data->g;
+
+ x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
+ y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
+ vx1 = NV_Ith_S(yy,2); vx2 = NV_Ith_S(yy,6);
+ vy1 = NV_Ith_S(yy,3); vy2 = NV_Ith_S(yy,7);
+
+ lam1 = m/(2*L*L)*(vx1*vx1 + vy1*vy1 + frc*(x1-R) - g*y1);
+ lam2 = m/(2*L*L)*(vx2*vx2 + vy2*vy2 - g*y2);
+
+ ax1 = -2.0*(x1-R)*lam1/m + frc;
+ ay1 = -2.0*y1*lam1/m - g;
+
+ ax2 = -2.0*(x2+R)*lam2/m;
+ ay2 = -2.0*y2*lam2/m - g;
+
+ NV_Ith_S(fy,0) = vx1; NV_Ith_S(fy, 4) = vx2;
+ NV_Ith_S(fy,1) = vy1; NV_Ith_S(fy, 5) = vy2;
+ NV_Ith_S(fy,2) = ax1; NV_Ith_S(fy, 6) = ax2;
+ NV_Ith_S(fy,3) = ay1; NV_Ith_S(fy, 7) = ay2;
+
+ return(0);
+}
+
+static int cfun(realtype t, N_Vector yy, N_Vector c, void *c_data)
+{
+ PbData data;
+ realtype x1, y1, x2, y2;
+ realtype vx1, vy1, vx2, vy2;
+ realtype R, L;
+
+ data = (PbData) c_data;
+ R = data->R;
+ L = data->L;
+
+ x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
+ y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
+ vx1 = NV_Ith_S(yy,2); vx2 = NV_Ith_S(yy,6);
+ vy1 = NV_Ith_S(yy,3); vy2 = NV_Ith_S(yy,7);
+
+ NV_Ith_S(c,0) = (x1-R)*(x1-R) + y1*y1 - L*L;
+ NV_Ith_S(c,1) = (x1-R)*vx1 + y1*vy1;
+
+ NV_Ith_S(c,2) = (x2+R)*(x2+R) + y2*y2 - L*L;
+ NV_Ith_S(c,3) = (x2+R)*vx2 + y2*vy2;
+
+ return(0);
+}
+
+static int gfun(realtype t, N_Vector yy, N_Vector yp, realtype *gval, void *g_data)
+{
+ PbData data;
+ realtype x1, y1, x2, y2;
+ realtype R, L, m, g;
+
+ data = (PbData) g_data;
+ R = data->R;
+ L = data->L;
+ m = data->m;
+ g = data->g;
+
+ x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
+ y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
+
+ gval[0] = (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2) - 4.0*R*R;
+
+ return(0);
+}
+
+
+static void contact(N_Vector yy, PbData data)
+{
+ realtype x1, y1, x2, y2;
+ realtype vx1, vy1, vx2, vy2;
+ realtype vt1, vn1, vn1_, vt2, vn2, vn2_;
+ realtype alpha, ca, sa;
+
+ x1 = NV_Ith_S(yy,0); x2 = NV_Ith_S(yy,4);
+ y1 = NV_Ith_S(yy,1); y2 = NV_Ith_S(yy,5);
+ vx1 = NV_Ith_S(yy,2); vx2 = NV_Ith_S(yy,6);
+ vy1 = NV_Ith_S(yy,3); vy2 = NV_Ith_S(yy,7);
+
+ /* Angle of contact line */
+
+ alpha = atan2(y2-y1, x2-x1);
+ ca = cos(alpha);
+ sa = sin(alpha);
+
+ /* Normal and tangential velocities before impact
+ * (rotate velocity vectors by +alpha) */
+
+ vn1 = ca*vx1 + sa*vy1;
+ vt1 = -sa*vx1 + ca*vy1;
+
+ vn2 = ca*vx2 + sa*vy2;
+ vt2 = -sa*vx2 + ca*vy2;
+
+ /* New normal velocities (M1=M2 and COR=1.0) */
+
+ vn1_ = vn2;
+ vn2_ = vn1;
+
+ vn1 = vn1_;
+ vn2 = vn2_;
+
+ /* Velocities after impact (rotate back by -alpha) */
+
+ vx1 = ca*vn1 - sa*vt1;
+ vy1 = sa*vn1 + ca*vt1;
+
+ vx2 = ca*vn2 - sa*vt2;
+ vy2 = sa*vn2 + ca*vt2;
+
+ NV_Ith_S(yy,2) = vx1; NV_Ith_S(yy,6) = vx2;
+ NV_Ith_S(yy,3) = vy1; NV_Ith_S(yy,7) = vy2;
+
+ return;
+}
+
+static void PrintFinalStats(void *cpode_mem)
+{
+ realtype h0u;
+ long int nst, nfe, nsetups, nje, nfeLS, nni, ncfn, netf;
+ long int nproj, nce, nsetupsP, nprf;
+ int flag;
+
+ flag = CPodeGetActualInitStep(cpode_mem, &h0u);
+ flag = CPodeGetNumSteps(cpode_mem, &nst);
+ flag = CPodeGetNumFctEvals(cpode_mem, &nfe);
+ flag = CPodeGetNumLinSolvSetups(cpode_mem, &nsetups);
+ flag = CPodeGetNumErrTestFails(cpode_mem, &netf);
+ flag = CPodeGetNumNonlinSolvIters(cpode_mem, &nni);
+ flag = CPodeGetNumNonlinSolvConvFails(cpode_mem, &ncfn);
+
+ flag = CPDlsGetNumJacEvals(cpode_mem, &nje);
+ flag = CPDlsGetNumFctEvals(cpode_mem, &nfeLS);
+
+ flag = CPodeGetProjStats(cpode_mem, &nproj, &nce, &nsetupsP, &nprf);
+
+ printf("\nFinal Statistics:\n");
+ printf("h0u = %g\n",h0u);
+ printf("nst = %-6ld nfe = %-6ld nsetups = %-6ld\n",
+ nst, nfe, nsetups);
+ printf("nfeLS = %-6ld nje = %ld\n",
+ nfeLS, nje);
+ printf("nni = %-6ld ncfn = %-6ld netf = %-6ld \n",
+ nni, ncfn, netf);
+ printf("nproj = %-6ld nce = %-6ld nsetupsP = %-6ld nprf = %-6ld\n",
+ nproj, nce, nsetupsP, nprf);
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/pend.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/pend.c
new file mode 100644
index 0000000..e51b8e4
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/pend.c
@@ -0,0 +1,258 @@
+#include <stdio.h>
+#include <math.h>
+
+#include <cpodes/cpodes.h>
+#include <cpodes/cpodes_dense.h>
+#include <nvector/nvector_serial.h>
+
+#define Ith(v,i) NV_Ith_S(v,i-1)
+#define IJth(A,i,j) DENSE_ELEM(A,i-1,j-1)
+
+/* Problem Constants */
+
+#define RTOL RCONST(1.0e-8)
+#define ATOL RCONST(1.0e-8)
+
+/* Functions Called by the Solver */
+static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data);
+static int proj(realtype t, N_Vector yy, N_Vector corr,
+ realtype epsProj, N_Vector err, void *pdata);
+static int cfun(realtype t, N_Vector yy, N_Vector cout, void *c_data);
+static void PrintFinalStats(void *cpode_mem);
+
+/*
+ *-------------------------------
+ * Main Program
+ *-------------------------------
+ */
+
+int main()
+{
+ void *cpode_mem;
+ N_Vector yy, yp, ctols;
+ realtype reltol, abstol, t, tout, Tout;
+ realtype x, y, xd, yd, g;
+ int iout, Nout, flag;
+
+
+ printf("double %d\n",(int)sizeof(double));
+ printf("long double %d\n",(int)sizeof(long double));
+ printf("float %d\n",(int)sizeof(float));
+ printf("realtype %d\n",(int)sizeof(realtype));
+
+
+ yy = N_VNew_Serial(4);
+ yp = N_VNew_Serial(4);
+
+ /* Initialize y */
+ Ith(yy,1) = 1.0; /* x */
+ Ith(yy,2) = 0.0; /* y */
+ Ith(yy,3) = 0.0; /* xd */
+ Ith(yy,4) = 0.0; /* yd */
+
+ /* Set tolerances */
+ reltol = RTOL;
+ abstol = ATOL;
+
+ Nout = 30;
+ Tout = Nout*1.0;
+
+ /* Initialize solver */
+ cpode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
+ flag = CPodeInit(cpode_mem, f, NULL, 0.0, yy, yp, CP_SS, reltol, &abstol);
+ flag = CPodeSetMaxNumSteps(cpode_mem, 5000);
+ flag = CPodeSetStopTime(cpode_mem, Tout);
+ flag = CPDense(cpode_mem, 4);
+
+ /* USER-PROVIDED PROJECTION FUNCTION */
+ /*
+ flag = CPodeProjDefine(cpode_mem, proj, NULL);
+ */
+
+ /* INTERNAL PROJECTION FUNCTION */
+
+ ctols = N_VNew_Serial(2);
+ Ith(ctols,1) = 1.0e-8;
+ Ith(ctols,2) = 1.0e-8;
+ flag = CPodeProjInit(cpode_mem, CP_PROJ_L2NORM, CP_CNSTR_NONLIN, cfun, NULL, ctols);
+ flag = CPodeSetProjTestCnstr(cpode_mem, TRUE);
+ flag = CPDenseProj(cpode_mem, 2, 4, CPDIRECT_LU);
+
+ flag = CPodeSetProjUpdateErrEst(cpode_mem, FALSE);
+
+ /* DISABLE PROJECTION */
+ /*
+ CPodeSetProjFrequency(cpode_mem, 0);
+ */
+
+ /* INTEGRATE TO FINAL TIME */
+ /*
+ flag = CPode(cpode_mem, Tout, &t, yy, yp, CP_NORMAL_TSTOP);
+ x = Ith(yy,1);
+ y = Ith(yy,2);
+ xd = Ith(yy,3);
+ yd = Ith(yy,4);
+ g = x*x + y*y - 1.0;
+ Ith(yy,1) = x - 1.0;
+ Ith(yy,2) = y - 0.0;
+ Ith(yy,3) = xd - 0.0;
+ Ith(yy,4) = yd - 0.0;
+ printf("%14.10e %14.10e %14.10e %14.10e | %14.10e\n",
+ Ith(yy,1),Ith(yy,2),Ith(yy,3),Ith(yy,4),g);
+ */
+
+ /* INTEGRATE THROUGH A SEQUENCE OF TIMES */
+ t = 0.0;
+ for(iout=1; iout<=Nout; iout++) {
+ tout = iout*1.0;
+ flag = CPode(cpode_mem, tout, &t, yy, yp, CP_NORMAL_TSTOP);
+ if (flag < 0) break;
+
+ x = Ith(yy,1);
+ y = Ith(yy,2);
+ xd = Ith(yy,3);
+ yd = Ith(yy,4);
+ g = x*x + y*y - 1.0;
+ printf(" -------------- %lf %14.10lf %14.10lf %14.10lf %14.10lf %14.10lf\n", t, x,y,xd,yd,g);
+ }
+
+
+ PrintFinalStats(cpode_mem);
+
+ N_VDestroy_Serial(yy);
+ N_VDestroy_Serial(yp);
+ N_VDestroy_Serial(ctols);
+ CPodeFree(&cpode_mem);
+
+ return(0);
+}
+
+
+static int f(realtype t, N_Vector yy, N_Vector fy, void *f_data)
+{
+ realtype x, y, xd, yd, g, tmp;
+
+ g = 13.7503716373294544;
+
+ x = Ith(yy,1);
+ y = Ith(yy,2);
+ xd = Ith(yy,3);
+ yd = Ith(yy,4);
+
+ tmp = xd*xd + yd*yd - g*y;
+
+ Ith(fy,1) = xd;
+ Ith(fy,2) = yd;
+ Ith(fy,3) = -x*tmp;
+ Ith(fy,4) = -y*tmp - g;
+
+ return(0);
+}
+
+static int proj(realtype t, N_Vector yy, N_Vector corr,
+ realtype epsProj, N_Vector err, void *pdata)
+{
+ realtype x, y, xd, yd;
+ realtype x_new, y_new, xd_new, yd_new;
+ realtype e1, e2, e3, e4;
+ realtype e1_new, e2_new, e3_new, e4_new;
+ realtype R;
+
+ x = Ith(yy,1);
+ y = Ith(yy,2);
+ xd = Ith(yy,3);
+ yd = Ith(yy,4);
+
+ R = sqrt(x*x+y*y);
+
+ x_new = x/R;
+ y_new = y/R;
+
+ xd_new = xd*y_new*y_new - yd*x_new*y_new;
+ yd_new = - xd*x_new*y_new + yd*x_new*x_new;
+
+ Ith(corr,1) = x_new - x;
+ Ith(corr,2) = y_new - y;
+ Ith(corr,3) = xd_new - xd;
+ Ith(corr,4) = yd_new - yd;
+
+ /* +- -+
+ * | y*y -x*y |
+ * P = | |
+ * | -x*y x*x |
+ * +- -+
+ */
+
+ /* Return err <- P * err */
+
+ e1 = Ith(err,1);
+ e2 = Ith(err,2);
+ e3 = Ith(err,3);
+ e4 = Ith(err,4);
+
+ e1_new = y_new*y_new * e1 - x_new*y_new * e2;
+ e2_new = -x_new*y_new * e1 + x_new*x_new * e2;
+
+ e3_new = y_new*y_new * e3 - x_new*y_new * e4;
+ e4_new = -x_new*y_new * e3 + x_new*x_new * e4;
+
+
+ Ith(err,1) = e1_new;
+ Ith(err,2) = e2_new;
+ Ith(err,3) = e3_new;
+ Ith(err,4) = e4_new;
+
+ // printf(":: %g %g %g %g\n",e1,e2,e3,e4);
+ // printf(":: %g %g %g %g\n",e1_new,e2_new,e3_new,e4_new);
+
+ return(0);
+}
+
+static int cfun(realtype t, N_Vector yy, N_Vector cout, void *c_data)
+{
+ realtype x, y, xd, yd;
+
+ x = Ith(yy,1);
+ y = Ith(yy,2);
+ xd = Ith(yy,3);
+ yd = Ith(yy,4);
+
+ Ith(cout,1) = x*x + y*y - 1.0;
+ Ith(cout,2) = x*xd + y*yd;
+
+ return(0);
+}
+
+
+static void PrintFinalStats(void *cpode_mem)
+{
+ realtype h0u;
+ long int nst, nfe, nsetups, nje, nfeLS, nni, ncfn, netf;
+ long int nproj, nce, nsetupsP, nprf;
+ int flag;
+
+ flag = CPodeGetActualInitStep(cpode_mem, &h0u);
+ flag = CPodeGetNumSteps(cpode_mem, &nst);
+ flag = CPodeGetNumFctEvals(cpode_mem, &nfe);
+ flag = CPodeGetNumLinSolvSetups(cpode_mem, &nsetups);
+ flag = CPodeGetNumErrTestFails(cpode_mem, &netf);
+ flag = CPodeGetNumNonlinSolvIters(cpode_mem, &nni);
+ flag = CPodeGetNumNonlinSolvConvFails(cpode_mem, &ncfn);
+
+ flag = CPDlsGetNumJacEvals(cpode_mem, &nje);
+ flag = CPDlsGetNumFctEvals(cpode_mem, &nfeLS);
+
+ flag = CPodeGetProjStats(cpode_mem, &nproj, &nce, &nsetupsP, &nprf);
+
+ printf("\nFinal Statistics:\n");
+ printf("h0u = %g\n",h0u);
+ printf("nst = %-6ld nfe = %-6ld nsetups = %-6ld\n",
+ nst, nfe, nsetups);
+ printf("nfeLS = %-6ld nje = %ld\n",
+ nfeLS, nje);
+ printf("nni = %-6ld ncfn = %-6ld netf = %-6ld \n",
+ nni, ncfn, netf);
+ printf("nproj = %-6ld nce = %-6ld nsetupsP = %-6ld nprf = %-6ld\n",
+ nproj, nce, nsetupsP, nprf);
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/pendEs.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/pendEs.c
new file mode 100644
index 0000000..a0aab56
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/pendEs.c
@@ -0,0 +1,203 @@
+#include <stdio.h>
+#include <math.h>
+
+#include <cpodes/cpodes.h>
+#include <cpodes/cpodes_dense.h>
+#include <nvector/nvector_serial.h>
+
+/* Problem Constants */
+#define g RCONST(13.7503716373294544)
+#define RTOL RCONST(1.0e-10)
+#define ATOL RCONST(1.0e-10)
+
+/* Functions Called by the Solver */
+static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data);
+static int cfun(realtype t, N_Vector yy, N_Vector cout, void *c_data);
+static void PrintFinalStats(void *cpode_mem, booleantype proj);
+
+/*
+ *-------------------------------
+ * Main Program
+ *-------------------------------
+ */
+
+int main()
+{
+ void *cpode_mem;
+ N_Vector yy, yp, ctols;
+ realtype reltol, abstol, t, /*tout,*/ Tout;
+ realtype half_pi, a, ad, E;
+ int /*iout,*/ Nout, flag;
+
+ half_pi = 2.0*atan(1.0);
+
+ yy = N_VNew_Serial(2);
+ yp = N_VNew_Serial(2);
+
+ /* Initialize y */
+ NV_Ith_S(yy,0) = half_pi;
+ NV_Ith_S(yy,1) = 0.0;
+
+ /* Set tolerances */
+ reltol = RTOL;
+ abstol = ATOL;
+
+ Nout = 1;
+ Tout = Nout*1.0;
+
+ /* CREATE CPODES OBJECT */
+ cpode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
+ flag = CPodeSetMaxNumSteps(cpode_mem, 50000);
+ flag = CPodeSetStopTime(cpode_mem, Tout);
+
+ /* INITIALIZE SOLVER */
+ flag = CPodeInit(cpode_mem, f, NULL, 0.0, yy, yp, CP_SS, reltol, &abstol);
+ flag = CPDense(cpode_mem, 2);
+
+ /* INTEGRATE IN ONE STEP MODE */
+ t = 0.0;
+ while(t<Tout) {
+ flag = CPode(cpode_mem, Tout, &t, yy, yp, CP_ONE_STEP_TSTOP);
+ if (flag < 0) break;
+ a = NV_Ith_S(yy,0);
+ ad = NV_Ith_S(yy,1);
+ E = ad*ad - 2*g*cos(a);
+ printf("%le %14.10le %14.10le %14.10le\n", t, a, ad, E);
+ }
+
+ /* INTEGRATE THROUGH A SEQUENCE OF TIMES */
+ /*
+ t = 0.0;
+ for(iout=1; iout<=Nout; iout++) {
+ tout = iout*1.0;
+ flag = CPode(cpode_mem, tout, &t, yy, yp, CP_NORMAL_TSTOP);
+ if (flag < 0) break;
+
+ a = NV_Ith_S(yy,0);
+ ad = NV_Ith_S(yy,1);
+ E = ad*ad - 2*g*cos(a);
+ printf("%le %14.10le %14.10le %14.10le\n", t, a, ad, E);
+ }
+ */
+
+ PrintFinalStats(cpode_mem, FALSE);
+
+ /* RE-INITIALIZE SOLVER */
+ NV_Ith_S(yy,0) = half_pi;
+ NV_Ith_S(yy,1) = 0.0;
+
+ printf("REINITIALIZE SOLVER\n\n");
+ a = NV_Ith_S(yy,0);
+ ad = NV_Ith_S(yy,1);
+ E = ad*ad - 2*g*cos(a);
+ printf("%14.10le %14.10le %14.10le\n\n", a, ad, E);
+
+
+ flag = CPodeReInit(cpode_mem, f, NULL, 0.0, yy, yp, CP_SS, reltol, &abstol);
+ flag = CPDense(cpode_mem, 2);
+
+ /* INTERNAL PROJECTION FUNCTION */
+ ctols = N_VNew_Serial(1);
+ NV_Ith_S(ctols,0) = 1.0e-2;
+ flag = CPodeProjInit(cpode_mem, CP_PROJ_L2NORM, CP_CNSTR_NONLIN, cfun, NULL, ctols);
+ flag = CPodeSetProjNonlinConvCoef(cpode_mem, 1.0e-3);
+ // flag = CPodeSetProjTestCnstr(cpode_mem, TRUE);
+ flag = CPDenseProj(cpode_mem, 1, 2, CPDIRECT_LU);
+
+ /* INTEGRATE IN ONE STEP MODE */
+ t = 0.0;
+ while(t<Tout) {
+ flag = CPode(cpode_mem, Tout, &t, yy, yp, CP_ONE_STEP_TSTOP);
+ if (flag < 0) break;
+ a = NV_Ith_S(yy,0);
+ ad = NV_Ith_S(yy,1);
+ E = ad*ad - 2*g*cos(a);
+ printf("%le %14.10le %14.10le %14.10le\n", t, a, ad, E);
+ }
+
+
+ /* INTEGRATE THROUGH A SEQUENCE OF TIMES */
+ /*
+ t = 0.0;
+ for(iout=1; iout<=Nout; iout++) {
+ tout = iout*1.0;
+ flag = CPode(cpode_mem, tout, &t, yy, yp, CP_NORMAL_TSTOP);
+ if (flag < 0) break;
+
+ a = NV_Ith_S(yy,0);
+ ad = NV_Ith_S(yy,1);
+ E = ad*ad - 2*g*cos(a);
+ printf(" -------------- %le %14.10le %14.10le %14.10le\n", t, a, ad, E);
+ }
+ */
+
+ PrintFinalStats(cpode_mem, TRUE);
+
+ N_VDestroy_Serial(yy);
+ N_VDestroy_Serial(yp);
+ N_VDestroy_Serial(ctols);
+ CPodeFree(&cpode_mem);
+
+ return(0);
+}
+
+
+static int f(realtype t, N_Vector yy, N_Vector fy, void *f_data)
+{
+ realtype a, ad;
+
+ a = NV_Ith_S(yy,0);
+ ad = NV_Ith_S(yy,1);
+
+ NV_Ith_S(fy,0) = ad;
+ NV_Ith_S(fy,1) = -g*sin(a);
+
+ return(0);
+}
+
+static int cfun(realtype t, N_Vector yy, N_Vector cout, void *c_data)
+{
+ realtype a, ad;
+
+ a = NV_Ith_S(yy,0);
+ ad = NV_Ith_S(yy,1);
+
+ NV_Ith_S(cout,0) = ad*ad - 2*g*cos(a);
+
+ return(0);
+}
+
+static void PrintFinalStats(void *cpode_mem, booleantype proj)
+{
+ realtype h0u;
+ long int nst, nfe, nsetups, nje, nfeLS, nni, ncfn, netf;
+ long int nproj, nce, nsetupsP, nprf;
+ int flag;
+
+ flag = CPodeGetActualInitStep(cpode_mem, &h0u);
+ flag = CPodeGetNumSteps(cpode_mem, &nst);
+ flag = CPodeGetNumFctEvals(cpode_mem, &nfe);
+ flag = CPodeGetNumLinSolvSetups(cpode_mem, &nsetups);
+ flag = CPodeGetNumErrTestFails(cpode_mem, &netf);
+ flag = CPodeGetNumNonlinSolvIters(cpode_mem, &nni);
+ flag = CPodeGetNumNonlinSolvConvFails(cpode_mem, &ncfn);
+
+ flag = CPDlsGetNumJacEvals(cpode_mem, &nje);
+ flag = CPDlsGetNumFctEvals(cpode_mem, &nfeLS);
+
+ printf("\nFinal Statistics:\n");
+ printf("h0u = %g\n",h0u);
+ printf("nst = %-6ld nfe = %-6ld nsetups = %-6ld\n",
+ nst, nfe, nsetups);
+ printf("nfeLS = %-6ld nje = %ld\n",
+ nfeLS, nje);
+ printf("nni = %-6ld ncfn = %-6ld netf = %-6ld \n",
+ nni, ncfn, netf);
+
+ if (proj) {
+ flag = CPodeGetProjStats(cpode_mem, &nproj, &nce, &nsetupsP, &nprf);
+ printf("nproj = %-6ld nce = %-6ld nsetupsP = %-6ld nprf = %-6ld\n",
+ nproj, nce, nsetupsP, nprf);
+ }
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/pendLr.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/pendLr.c
new file mode 100644
index 0000000..54dc5a2
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/pendLr.c
@@ -0,0 +1,168 @@
+#include <stdio.h>
+
+#include <cpodes/cpodes.h>
+#include <cpodes/cpodes_lapack.h>
+#include <cpodes/cpodes_dense.h>
+#include <nvector/nvector_serial.h>
+
+#define Ith(v,i) NV_Ith_S(v,i-1)
+
+/* Problem Constants */
+
+#define RTOL RCONST(1.0e-8)
+#define ATOL RCONST(1.0e-8)
+
+/* Functions Called by the Solver */
+static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data);
+static int cfun(realtype t, N_Vector yy, N_Vector cout, void *c_data);
+static void PrintFinalStats(void *cpode_mem);
+
+/*
+ *-------------------------------
+ * Main Program
+ *-------------------------------
+ */
+
+int main()
+{
+ void *cpode_mem;
+ N_Vector yy, yp, ctols;
+ realtype reltol, abstol, t, tout, Tout;
+ realtype x, y, xd, yd, g;
+ int iout, Nout, flag;
+
+ yy = N_VNew_Serial(4);
+ yp = N_VNew_Serial(4);
+
+ /* Initialize y */
+ Ith(yy,1) = 1.0; /* x */
+ Ith(yy,2) = 0.0; /* y */
+ Ith(yy,3) = 0.0; /* xd */
+ Ith(yy,4) = 0.0; /* yd */
+
+ /* Set tolerances */
+ reltol = RTOL;
+ abstol = ATOL;
+
+ Nout = 30;
+ Tout = Nout*1.0;
+
+ /* Initialize solver */
+ cpode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
+ flag = CPodeInit(cpode_mem, (void *)f, NULL, 0.0, yy, yp, CP_SS, reltol, &abstol);
+ flag = CPodeSetMaxNumSteps(cpode_mem, 50000);
+ flag = CPodeSetStopTime(cpode_mem, Tout);
+ flag = CPLapackDense(cpode_mem, 4);
+
+ /* INTERNAL PROJECTION FUNCTION */
+ ctols = N_VNew_Serial(3);
+ Ith(ctols,1) = 1.0e-8;
+ Ith(ctols,2) = 1.0e-8;
+ Ith(ctols,3) = 1.0e-8;
+ flag = CPodeProjInit(cpode_mem, CP_PROJ_L2NORM, CP_CNSTR_NONLIN, cfun, NULL, ctols);
+ flag = CPodeSetProjTestCnstr(cpode_mem, TRUE);
+ flag = CPLapackDenseProj(cpode_mem, 3, 4, CPDIRECT_QRP);
+
+ /* COMPUTE CONSISTENT INITIAL CONDITIONS */
+ flag = CPodeCalcIC(cpode_mem);
+ flag = CPodeGetConsistentIC(cpode_mem, yy, NULL);
+
+ /* INTEGRATE THROUGH A SEQUENCE OF TIMES */
+ t = 0.0;
+ for(iout=1; iout<=Nout; iout++) {
+ tout = iout*1.0;
+ flag = CPode(cpode_mem, tout, &t, yy, yp, CP_NORMAL_TSTOP);
+ if (flag < 0) break;
+
+ x = Ith(yy,1);
+ y = Ith(yy,2);
+ xd = Ith(yy,3);
+ yd = Ith(yy,4);
+ g = x*x + y*y - 1.0;
+ printf(" -------------- %lf %14.10lf %14.10lf %14.10lf %14.10lf %14.10lf\n", t, x,y,xd,yd,g);
+ }
+
+ PrintFinalStats(cpode_mem);
+
+ N_VDestroy_Serial(yy);
+ N_VDestroy_Serial(yp);
+ N_VDestroy_Serial(ctols);
+ CPodeFree(&cpode_mem);
+
+ return(0);
+}
+
+
+static int f(realtype t, N_Vector yy, N_Vector fy, void *f_data)
+{
+ realtype x, y, xd, yd, g, tmp;
+
+ g = 13.7503716373294544;
+
+ x = Ith(yy,1);
+ y = Ith(yy,2);
+ xd = Ith(yy,3);
+ yd = Ith(yy,4);
+
+ tmp = xd*xd + yd*yd - g*y;
+
+ Ith(fy,1) = xd;
+ Ith(fy,2) = yd;
+ Ith(fy,3) = -x*tmp;
+ Ith(fy,4) = -y*tmp - g;
+
+ return(0);
+}
+
+static int cfun(realtype t, N_Vector yy, N_Vector cout, void *c_data)
+{
+ realtype x, y, xd, yd;
+
+ x = Ith(yy,1);
+ y = Ith(yy,2);
+ xd = Ith(yy,3);
+ yd = Ith(yy,4);
+
+ Ith(cout,1) = x*x + y*y - 1.0;
+ Ith(cout,2) = x*xd + y*yd;
+ Ith(cout,3) = Ith(cout,1) + Ith(cout,2);
+
+ return(0);
+}
+
+
+static void PrintFinalStats(void *cpode_mem)
+{
+ realtype h0u;
+ long int nst, nfe, nsetups, nje, nfeLS, nni, ncfn, netf, nge;
+ long int nproj, nce, nsetupsP, nprf;
+ int flag;
+
+ flag = CPodeGetActualInitStep(cpode_mem, &h0u);
+ flag = CPodeGetNumSteps(cpode_mem, &nst);
+ flag = CPodeGetNumFctEvals(cpode_mem, &nfe);
+ flag = CPodeGetNumLinSolvSetups(cpode_mem, &nsetups);
+ flag = CPodeGetNumErrTestFails(cpode_mem, &netf);
+ flag = CPodeGetNumNonlinSolvIters(cpode_mem, &nni);
+ flag = CPodeGetNumNonlinSolvConvFails(cpode_mem, &ncfn);
+
+ flag = CPDlsGetNumJacEvals(cpode_mem, &nje);
+ flag = CPDlsGetNumFctEvals(cpode_mem, &nfeLS);
+
+ flag = CPodeGetProjStats(cpode_mem, &nproj, &nce, &nsetupsP, &nprf);
+
+ flag = CPodeGetNumGEvals(cpode_mem, &nge);
+
+ printf("\nFinal Statistics:\n");
+ printf("h0u = %g\n",h0u);
+ printf("nst = %-6ld nfe = %-6ld nsetups = %-6ld\n",
+ nst, nfe, nsetups);
+ printf("nfeLS = %-6ld nje = %ld\n",
+ nfeLS, nje);
+ printf("nni = %-6ld ncfn = %-6ld netf = %-6ld \n",
+ nni, ncfn, netf);
+ printf("nproj = %-6ld nce = %-6ld nsetupsP = %-6ld nprf = %-6ld\n",
+ nproj, nce, nsetupsP, nprf);
+ printf("nge = %ld\n", nge);
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/sundials/tests/pend_test.c b/SimTKmath/Integrators/src/CPodes/sundials/tests/pend_test.c
new file mode 100644
index 0000000..6483afe
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/sundials/tests/pend_test.c
@@ -0,0 +1,265 @@
+#include <stdio.h>
+#include <math.h>
+
+#include <cpodes/cpodes.h>
+#include <cpodes/cpodes_dense.h>
+#include <nvector/nvector_serial.h>
+#include <sundials/sundials_math.h>
+
+#define Ith(v,i) NV_Ith_S(v,i-1)
+
+#define TOL RCONST(1.0e-5)
+#define TOL_REF RCONST(1.0e-14)
+
+static int fref(realtype t, N_Vector yy, N_Vector fy, void *f_data);
+
+static int f(realtype t, N_Vector yy, N_Vector fy, void *f_data);
+static int proj(realtype t, N_Vector yy, N_Vector corr,
+ realtype epsProj, N_Vector err, void *pdata);
+
+void GetSol(void *cpode_mem, N_Vector yy0, realtype tol,
+ realtype tout, booleantype proj, N_Vector yref);
+
+void RefSol(realtype tout, N_Vector yref);
+
+/*
+ *-------------------------------
+ * Main Program
+ *-------------------------------
+ */
+
+int main()
+{
+ void *cpode_mem;
+ N_Vector yref, yy0;
+ realtype tol, tout;
+ int i, flag;
+
+
+ tout = 30.0;
+
+ /* Get reference solution */
+ yref = N_VNew_Serial(4);
+ RefSol(tout, yref);
+
+ /* Initialize solver */
+ tol = TOL;
+ cpode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
+ yy0 = N_VNew_Serial(4);
+ Ith(yy0,1) = 1.0; /* x */
+ Ith(yy0,2) = 0.0; /* y */
+ Ith(yy0,3) = 0.0; /* xd */
+ Ith(yy0,4) = 0.0; /* yd */
+ flag = CPodeInit(cpode_mem, (void *)f, NULL, 0.0, yy0, NULL, CP_SS, tol, &tol);
+ flag = CPodeSetMaxNumSteps(cpode_mem, 50000);
+ flag = CPodeSetStopTime(cpode_mem, tout);
+ flag = CPodeProjDefine(cpode_mem, proj, NULL);
+ flag = CPDense(cpode_mem, 4);
+
+ for (i=0;i<5;i++) {
+
+ printf("\n\n%.2e\n", tol);
+ GetSol(cpode_mem, yy0, tol, tout, TRUE, yref);
+ GetSol(cpode_mem, yy0, tol, tout, FALSE, yref);
+ tol /= 10.0;
+ }
+
+ N_VDestroy_Serial(yref);
+ CPodeFree(&cpode_mem);
+
+ return(0);
+}
+
+void GetSol(void *cpode_mem, N_Vector yy0, realtype tol,
+ realtype tout, booleantype proj, N_Vector yref)
+{
+ N_Vector yy, yp;
+ realtype t, x, y, xd, yd, g;
+ int flag;
+ long int nst, nfe, nsetups, nje, nfeLS, ncfn, netf;
+
+ if (proj) {
+ printf(" YES ");
+ CPodeSetProjFrequency(cpode_mem, 1);
+ } else {
+ CPodeSetProjFrequency(cpode_mem, 0);
+ printf(" NO ");
+ }
+
+ yy = N_VNew_Serial(4);
+ yp = N_VNew_Serial(4);
+
+ flag = CPodeReInit(cpode_mem, (void *)f, NULL, 0.0, yy0, NULL, CP_SS, tol, &tol);
+
+ flag = CPode(cpode_mem, tout, &t, yy, yp, CP_NORMAL_TSTOP);
+
+ x = Ith(yy,1);
+ y = Ith(yy,2);
+ g = ABS(x*x + y*y - 1.0);
+
+ N_VLinearSum(1.0, yy, -1.0, yref, yy);
+
+ N_VAbs(yy, yy);
+
+ x = Ith(yy,1);
+ y = Ith(yy,2);
+ xd = Ith(yy,3);
+ yd = Ith(yy,4);
+
+
+ printf("%9.2e %9.2e %9.2e %9.2e | %9.2e |",
+ Ith(yy,1),Ith(yy,2),Ith(yy,3),Ith(yy,4),g);
+
+ CPodeGetNumSteps(cpode_mem, &nst);
+ CPodeGetNumFctEvals(cpode_mem, &nfe);
+ CPodeGetNumLinSolvSetups(cpode_mem, &nsetups);
+ CPodeGetNumErrTestFails(cpode_mem, &netf);
+ CPodeGetNumNonlinSolvConvFails(cpode_mem, &ncfn);
+
+ CPDlsGetNumJacEvals(cpode_mem, &nje);
+ CPDlsGetNumFctEvals(cpode_mem, &nfeLS);
+
+
+ printf(" %6ld %6ld+%-4ld %4ld (%3ld) | %3ld %3ld\n",
+ nst, nfe, nfeLS, nsetups, nje, ncfn, netf);
+
+ N_VDestroy_Serial(yy);
+ N_VDestroy_Serial(yp);
+
+ return;
+}
+
+
+void RefSol(realtype tout, N_Vector yref)
+{
+ void *cpode_mem;
+ N_Vector yy, yp;
+ realtype tol, t, th, thd;
+ int flag;
+
+
+ yy = N_VNew_Serial(2);
+ yp = N_VNew_Serial(2);
+ Ith(yy,1) = 0.0; /* theta */
+ Ith(yy,2) = 0.0; /* thetad */
+ tol = TOL_REF;
+
+ cpode_mem = CPodeCreate(CP_EXPL, CP_BDF, CP_NEWTON);
+ flag = CPodeSetMaxNumSteps(cpode_mem, 100000);
+ flag = CPodeInit(cpode_mem, (void *)fref, NULL, 0.0, yy, yp, CP_SS, tol, &tol);
+ flag = CPDense(cpode_mem, 2);
+
+ flag = CPodeSetStopTime(cpode_mem, tout);
+ flag = CPode(cpode_mem, tout, &t, yy, yp, CP_NORMAL_TSTOP);
+ th = Ith(yy,1);
+ thd = Ith(yy,2);
+ Ith(yref,1) = cos(th);
+ Ith(yref,2) = sin(th);
+ Ith(yref,3) = -thd*sin(th);
+ Ith(yref,4) = thd*cos(th);
+
+ N_VDestroy_Serial(yy);
+ N_VDestroy_Serial(yp);
+ CPodeFree(&cpode_mem);
+
+ return;
+}
+
+static int fref(realtype t, N_Vector yy, N_Vector fy, void *f_data)
+{
+ realtype th, thd, g;
+
+ g = 13.7503716373294544;
+
+ th = Ith(yy,1);
+ thd = Ith(yy,2);
+
+ Ith(fy,1) = thd;
+ Ith(fy,2) = -g*cos(th);
+
+ return(0);
+}
+
+
+
+static int f(realtype t, N_Vector yy, N_Vector fy, void *f_data)
+{
+ realtype x, y, xd, yd, g, tmp;
+
+ g = 13.7503716373294544;
+
+ x = Ith(yy,1);
+ y = Ith(yy,2);
+ xd = Ith(yy,3);
+ yd = Ith(yy,4);
+
+ tmp = xd*xd + yd*yd - g*y;
+
+ Ith(fy,1) = xd;
+ Ith(fy,2) = yd;
+ Ith(fy,3) = -x*tmp;
+ Ith(fy,4) = -y*tmp - g;
+
+ return(0);
+}
+
+static int proj(realtype t, N_Vector yy, N_Vector corr,
+ realtype epsProj, N_Vector err, void *pdata)
+{
+ realtype x, y, xd, yd;
+ realtype x_new, y_new, xd_new, yd_new;
+ realtype e1, e2, e3, e4;
+ realtype e1_new, e2_new, e3_new, e4_new;
+ realtype R;
+
+ /* Extract current solution */
+
+ x = Ith(yy,1);
+ y = Ith(yy,2);
+ xd = Ith(yy,3);
+ yd = Ith(yy,4);
+
+ /* Project onto manifold */
+
+ R = sqrt(x*x+y*y);
+
+ x_new = x/R;
+ y_new = y/R;
+
+ xd_new = xd*y_new*y_new - yd*x_new*y_new;
+ yd_new = - xd*x_new*y_new + yd*x_new*x_new;
+
+ /* Return corrections */
+
+ Ith(corr,1) = x_new - x;
+ Ith(corr,2) = y_new - y;
+ Ith(corr,3) = xd_new - xd;
+ Ith(corr,4) = yd_new - yd;
+
+ /* +- -+
+ * | y*y -x*y |
+ * P = | |
+ * | -x*y x*x |
+ * +- -+
+ */
+
+ /* Return err <- P * err */
+
+ e1 = Ith(err,1);
+ e2 = Ith(err,2);
+ e3 = Ith(err,3);
+ e4 = Ith(err,4);
+
+ e1_new = y_new*y_new * e1 - x_new*y_new * e2;
+ e2_new = -x_new*y_new * e1 + x_new*x_new * e2;
+
+ e3_new = y_new*y_new * e3 - x_new*y_new * e4;
+ e4_new = -x_new*y_new * e3 + x_new*x_new * e4;
+
+ Ith(err,1) = e1_new;
+ Ith(err,2) = e2_new;
+ Ith(err,3) = e3_new;
+ Ith(err,4) = e4_new;
+
+ return(0);
+}
diff --git a/SimTKmath/Integrators/src/CPodes/tests/CMakeLists.txt b/SimTKmath/Integrators/src/CPodes/tests/CMakeLists.txt
new file mode 100644
index 0000000..fd3f799
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/tests/CMakeLists.txt
@@ -0,0 +1,54 @@
+INCLUDE_DIRECTORIES(${SimTK_SDK}/include)
+
+# Adhoc tests are those test or demo programs which are not intended,
+# or not ready, to be part of the regression suite.
+#ADD_SUBDIRECTORY(adhoc)
+
+# Generate regression tests.
+#
+# This is boilerplate code for generating a set of executables, one per
+# .cpp file in a "tests" directory. These are for test cases which
+# have been engineered to be suitable as part of the nightly regression
+# test suite. Ideally, they know their correct answers and return
+# a simple thumbs-up/thumbs-down result, although some regressions
+# may be present and useful just to determine if they compile and
+# run to completion.
+#
+# For IDEs that can deal with PROJECT_LABEL properties (at least
+# Visual Studio) the projects for building each of these adhoc
+# executables will be labeled "Regr - TheTestName" if a file
+# TheTestName.cpp is found in this directory.
+#
+# We check the BUILD_TESTS_AND_EXAMPLES_{SHARED,STATIC} variables to determine
+# whether to build dynamically linked, statically linked, or both
+# versions of the executable.
+
+FILE(GLOB REGR_TESTS "*.cpp")
+FOREACH(TEST_PROG ${REGR_TESTS})
+ GET_FILENAME_COMPONENT(TEST_ROOT ${TEST_PROG} NAME_WE)
+
+ IF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+ # Link with shared library
+ ADD_EXECUTABLE(${TEST_ROOT} ${TEST_PROG})
+ SET_TARGET_PROPERTIES(${TEST_ROOT}
+ PROPERTIES
+ PROJECT_LABEL "Test_Regr - ${TEST_ROOT}")
+ TARGET_LINK_LIBRARIES(${TEST_ROOT}
+ ${TEST_SHARED_TARGET})
+ ADD_TEST(${TEST_ROOT} ${EXECUTABLE_OUTPUT_PATH}/${TEST_ROOT})
+ ENDIF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+
+ IF (BUILD_STATIC_LIBRARIES AND BUILD_TESTS_AND_EXAMPLES_STATIC)
+ # Link with static library
+ SET(TEST_STATIC ${TEST_ROOT}Static)
+ ADD_EXECUTABLE(${TEST_STATIC} ${TEST_PROG})
+ SET_TARGET_PROPERTIES(${TEST_STATIC}
+ PROPERTIES
+ COMPILE_FLAGS "-DSimTK_USE_STATIC_LIBRARIES"
+ PROJECT_LABEL "Test_Regr - ${TEST_STATIC}")
+ TARGET_LINK_LIBRARIES(${TEST_STATIC}
+ ${TEST_STATIC_TARGET})
+ ADD_TEST(${TEST_STATIC} ${EXECUTABLE_OUTPUT_PATH}/${TEST_STATIC})
+ ENDIF()
+
+ENDFOREACH(TEST_PROG ${REGR_TESTS})
diff --git a/SimTKmath/Integrators/src/CPodes/tests/CpsAdamsXCpp.cpp b/SimTKmath/Integrators/src/CPodes/tests/CpsAdamsXCpp.cpp
new file mode 100644
index 0000000..bd20e33
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/tests/CpsAdamsXCpp.cpp
@@ -0,0 +1,281 @@
+/* -------------------------------------------------------------------------- *
+ * SimTK Simbody: SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This is a duplicate of Radu's cpsadamsx.c example converted to C++ and using
+ * the SimTKcpodes interface instead of the original C one.
+ */
+
+//#define SimTK_USE_STATIC_LIBRARIES
+
+#include "simmath/internal/SimTKcpodes.h"
+
+#include <cmath>
+#include <cstdio>
+
+using SimTK::Real;
+using SimTK::Vector;
+using SimTK::CPodes;
+using std::printf;
+
+
+class Problem1System : public SimTK::CPodesSystem {
+public:
+ // Override default implementations of these virtual functions.
+ int explicitODE(Real t, const Vector& y, Vector& fout) const;
+ int implicitODE(Real t, const Vector& y, const Vector& yp,
+ Vector& fout) const;
+};
+
+
+class Problem2System : public SimTK::CPodesSystem {
+public:
+ // Override default implementations of these virtual functions.
+ int explicitODE(Real t, const Vector& y, Vector& fout) const;
+ int implicitODE(Real t, const Vector& y, const Vector& yp,
+ Vector& fout) const;
+};
+
+static const CPodes::ODEType odeType = CPodes::ExplicitODE;
+
+/* Shared Problem Constants */
+
+#define ATOL Real(1.0e-6) /* 1.0e-6 */
+#define RTOL Real(0.0) /* 0.0 */
+
+#define ZERO Real(0.0)
+#define ONE Real(1.0)
+#define TWO Real(2.0)
+#define THIRTY Real(30.0)
+
+/* Problem #1 Constants */
+
+#define P1_NEQ 2
+#define P1_ETA Real(3.0)
+#define P1_NOUT 4
+#define P1_T0 Real(0.0)
+#define P1_T1 Real(1.39283880203)
+#define P1_DTOUT Real(2.214773875)
+
+/* Problem #2 Constants */
+
+#define P2_MESHX 5
+#define P2_MESHY 5
+#define P2_NEQ P2_MESHX*P2_MESHY
+#define P2_ALPH1 Real(1.0)
+#define P2_ALPH2 Real(1.0)
+#define P2_NOUT 5
+#define P2_ML 5
+#define P2_MU 0
+#define P2_T0 Real(0.0)
+#define P2_T1 Real(0.01)
+#define P2_TOUT_MULT Real(10.0)
+
+/* Private Helper Functions */
+static void Problem1(void);
+static void Problem2(void);
+static Real MaxError(const Vector& y, Real t);
+static void PrintFinalStats(CPodes&);
+
+int main(void)
+{
+
+ printf("Van der Pol\n\n");
+ Problem1();
+ printf("\n\ny' = A*y\n\n");
+ Problem2();
+
+ return(0);
+}
+
+static void Problem1(void)
+{
+ Vector y(P1_NEQ), yp(P1_NEQ);
+ Real reltol=RTOL, abstol=ATOL, t, tout, hu;
+ int flag, iout, qu;
+
+ y[0] = TWO;
+ y[1] = ZERO;
+
+ Problem1System sys1;
+
+ // Use the explicit function to initialize derivatives for the implicit one.
+ if (odeType == CPodes::ImplicitODE)
+ sys1.explicitODE(P1_T0, y, yp);
+
+ CPodes cpode(odeType, CPodes::Adams, CPodes::Functional);
+ /* flag = cpode.setInitStep(4.0e-9);*/
+ flag = cpode.init(sys1, P1_T0, y, yp, CPodes::ScalarScalar, reltol, &abstol);
+
+ printf("\n t x xdot qu hu \n");
+ for(iout=1, tout=P1_T1; iout <= P1_NOUT; iout++, tout += P1_DTOUT) {
+ flag = cpode.step(tout, &t, y, yp, CPodes::Normal);
+ if (flag != CPodes::Success) break;
+ flag = cpode.getLastOrder(&qu);
+ flag = cpode.getLastStep(&hu);
+ printf("%10.5f %12.5le %12.5le %2d %6.4le\n", t, y[0], y[1], qu, hu);
+ }
+
+ PrintFinalStats(cpode);
+
+ return;
+}
+
+int Problem1System::implicitODE(Real t, const Vector& y, const Vector& yp,
+ Vector& res) const
+{
+ const int ret = Problem1System::explicitODE(t,y,res);
+ res = yp - res;
+ return ret;
+}
+
+
+int Problem1System::explicitODE(Real t, const Vector& y, Vector& ydot) const
+{
+ Real y0, y1;
+
+ y0 = y[0];
+ y1 = y[1];
+
+ ydot[0] = y1;
+ ydot[1] = (ONE - y0*y0)* P1_ETA * y1 - y0;
+
+ return(0);
+}
+
+
+
+static void Problem2(void)
+{
+ Vector y(P2_NEQ), yp(P2_NEQ);
+ Real reltol=RTOL, abstol=ATOL, t, tout, erm, hu;
+ int flag, iout, qu;
+
+ y = 0;
+ y[0] = ONE;
+
+ Problem2System sys2;
+
+ // Use the explicit function to initialize derivatives for the implicit one.
+ if (odeType == CPodes::ImplicitODE)
+ sys2.explicitODE(P2_T0, y, yp);
+
+ CPodes cpode(odeType, CPodes::Adams, CPodes::Functional);
+ /* flag = cpode.setInitStep(2.0e-9);*/
+ flag = cpode.init(sys2, P2_T0, y, yp, CPodes::ScalarScalar, reltol, &abstol);
+
+ printf("\n t max.err qu hu \n");
+ for(iout=1, tout=P2_T1; iout <= P2_NOUT; iout++, tout*=P2_TOUT_MULT) {
+ flag = cpode.step(tout, &t, y, yp, CPodes::Normal);
+ if (flag != CPodes::Success) break;
+ erm = MaxError(y, t);
+ flag = cpode.getLastOrder(&qu);
+ flag = cpode.getLastStep(&hu);
+ printf("%10.3f %12.4le %2d %12.4le\n", t, erm, qu, hu);
+ }
+
+ PrintFinalStats(cpode);
+
+ return;
+}
+
+int Problem2System::implicitODE(Real t, const Vector& y, const Vector& yp,
+ Vector& res) const
+{
+ const int ret = Problem2System::explicitODE(t, y, res);
+ res = yp - res;
+ return ret;
+}
+
+int Problem2System::explicitODE(Real t, const Vector& y, Vector& ydot) const
+{
+ long int i, j, k;
+ Real d;
+
+ /*
+ Excluding boundaries,
+
+ ydot = f = -2 y + alpha1 * y + alpha2 * y
+ i,j i,j i,j i-1,j i,j-1
+ */
+
+ for (j=0; j < P2_MESHY; j++) {
+ for (i=0; i < P2_MESHX; i++) {
+ k = i + j * P2_MESHX;
+ d = -TWO*y[k];
+ if (i != 0) d += P2_ALPH1 * y[k-1];
+ if (j != 0) d += P2_ALPH2 * y[k-P2_MESHX];
+ ydot[k] = d;
+ }
+ }
+
+ return(0);
+}
+
+static Real MaxError(const Vector& y, Real t)
+{
+ Real er, ex=ZERO, yt, maxError=ZERO, ifact_inv, jfact_inv=ONE;
+
+ if (t == ZERO) return(ZERO);
+
+ if (t <= THIRTY) ex = std::exp(-TWO*t);
+
+ for (int j = 0; j < P2_MESHY; j++) {
+ ifact_inv = ONE;
+ for (int i = 0; i < P2_MESHX; i++) {
+ const int k = i + j * P2_MESHX;
+ yt = std::pow(t,i+j) * ex * ifact_inv * jfact_inv;
+ er = std::abs(y[k] - yt);
+ if (er > maxError) maxError = er;
+ ifact_inv /= (i+1);
+ }
+ jfact_inv /= (j+1);
+ }
+ return(maxError);
+}
+
+
+
+static void PrintFinalStats(CPodes& cpode)
+{
+ int nst, nfe, nni, ncfn, netf;
+ Real h0u;
+ int flag;
+
+ flag = cpode.getActualInitStep(&h0u);
+ flag = cpode.getNumSteps(&nst);
+ flag = cpode.getNumFctEvals(&nfe);
+ flag = cpode.getNumErrTestFails(&netf);
+ flag = cpode.getNumNonlinSolvIters(&nni);
+ flag = cpode.getNumNonlinSolvConvFails(&ncfn);
+
+ printf("\n Final statistics:\n\n");
+ printf(" Number of steps = %4d \n", nst);
+ printf(" Number of f-s = %4d \n", nfe);
+ printf(" Number of nonlinear iterations = %4d \n", nni);
+ printf(" Number of nonlinear convergence failures = %4d \n", ncfn);
+ printf(" Number of error test failures = %4d \n", netf);
+ printf(" Initial step size = %g \n\n", h0u);
+
+}
+
diff --git a/SimTKmath/Integrators/src/CPodes/tests/PendLrCpp.cpp b/SimTKmath/Integrators/src/CPodes/tests/PendLrCpp.cpp
new file mode 100644
index 0000000..061f2d8
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodes/tests/PendLrCpp.cpp
@@ -0,0 +1,250 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This is a duplicate of Radu's pendLr.c example converted to C++ and using
+ * the SimTKcpodes interface instead of the original C one.
+ */
+
+//#define SimTK_USE_STATIC_LIBRARIES
+
+#include "simmath/internal/SimTKcpodes.h"
+
+// Just so we can get the version number:
+#include "SimTKlapack.h"
+
+#include <cstdio>
+#include <iostream>
+
+using SimTK::Real;
+using SimTK::Vector;
+using SimTK::Matrix;
+using SimTK::CPodes;
+using std::printf;
+using std::cout;
+using std::endl;
+
+extern "C" {
+ void SimTK_version_SimTKlapack(int* major, int* minor, int* build);
+ void SimTK_about_SimTKlapack(const char* key, int maxlen, char* value);
+}
+
+
+static const Real RTOL = (Real)1e-8;
+static const Real ATOL = (Real)1e-8;
+static const Real CTOL = (Real)1e-8;
+
+class PendLrSystem : public SimTK::CPodesSystem {
+public:
+ // Override default implementations of these virtual functions.
+ int explicitODE(Real t, const Vector& y, Vector& fout) const;
+ int constraint(Real t, const Vector& y, Vector& cout) const;
+};
+
+// Functions Called by the Solver
+//static int f(Real t, N_Vector y, N_Vector ydot, void* f_data);
+//static int cfun(Real t, N_Vector yy, N_Vector cout, void* c_data);
+
+// Local static functions.
+static void PrintFinalStats(CPodes&);
+
+static void showSimTKAboutInfo() {
+
+ int major,minor,build;
+ char out[100];
+ const char* keylist[] = { "version", "library", "type", "debug", "authors", "copyright", "svn_revision", 0 };
+
+#ifdef TEST_LAPACK_VERSION
+ SimTK_version_SimTKlapack(&major,&minor,&build);
+ std::printf("==> SimTKlapack library version: %d.%d.%d\n", major, minor, build);
+ std::printf(" SimTK_about_SimTKlapack():\n");
+ for (const char** p = keylist; *p; ++p) {
+ SimTK_about_SimTKlapack(*p, 100, out);
+ std::printf(" about(%s)='%s'\n", *p, out);
+ }
+#endif
+
+ SimTK_version_SimTKcommon(&major,&minor,&build);
+ std::printf("==> SimTKcommon library version: %d.%d.%d\n", major, minor, build);
+ std::printf(" SimTK_about_SimTKcommon():\n");
+ for (const char** p = keylist; *p; ++p) {
+ SimTK_about_SimTKcommon(*p, 100, out);
+ std::printf(" about(%s)='%s'\n", *p, out);
+ }
+
+}
+
+int main() {
+ showSimTKAboutInfo();
+
+try {
+ Vector yy, yp, ctols;
+ Real reltol, abstol, t, tout, Tout;
+ Real x, y, xd, yd, g;
+ int iout, Nout, flag;
+
+ yy.resize(4);
+ yp.resize(4);
+
+ /* Initialize y */
+ yy[0] = 1.0; /* x */
+ yy[1] = 0.0; /* y */
+ yy[2] = 0.0; /* xd */
+ yy[3] = 0.0; /* yd */
+
+ /* Set tolerances */
+ reltol = RTOL;
+ abstol = ATOL;
+
+ Nout = 30;
+ Tout = Nout*1.0;
+
+ /* Initialize solver */
+ PendLrSystem sys;
+ CPodes cpode(CPodes::ExplicitODE, CPodes::BDF, CPodes::Newton);
+
+ flag = cpode.init(sys, 0.0, yy, yp, CPodes::ScalarScalar, reltol, &abstol);
+ flag = cpode.setMaxNumSteps(50000);
+ flag = cpode.setStopTime(Tout);
+ flag = cpode.lapackDense(4);
+
+ /* INTERNAL PROJECTION FUNCTION */
+ ctols.resize(/*3*/2);
+ ctols[0] = CTOL;
+ ctols[1] = CTOL;
+ //ctols[2] = CTOL;
+ flag = cpode.projInit(CPodes::L2Norm, CPodes::Nonlinear, ctols);
+ flag = cpode.setProjTestCnstr(true);
+ flag = cpode.lapackDenseProj(/*3*/2, 4, CPodes::ProjectWithQRPivot);
+
+ /* INTEGRATE THROUGH A SEQUENCE OF TIMES */
+ t = 0.0;
+ for(iout=1; iout<=Nout; iout++) {
+ tout = iout*1.0;
+ flag = cpode.step(tout, &t, yy, yp, CPodes::NormalTstop); // CPode() in C
+ if (flag < 0) break;
+
+ x = yy[0];
+ y = yy[1];
+ xd = yy[2];
+ yd = yy[3];
+ g = (x*x + y*y - 1.0)/2;
+ printf(" -------------- %lf %14.10lf %14.10lf %14.10lf %14.10lf %14.10lf\n", t, x,y,xd,yd,g);
+ }
+
+ PrintFinalStats(cpode);
+
+ return 0;
+}
+catch (const std::exception& e) {
+ std::cout << e.what() << std::endl;
+}
+ return 1;
+}
+
+static int pendODE(Real t, const Vector& yy, Vector& fy)
+{
+ Real x, y, xd, yd, g, tmp;
+
+ g = 13.7503716373294544;
+
+ x = yy[0];
+ y = yy[1];
+ xd = yy[2];
+ yd = yy[3];
+
+
+
+ // Radu's version:
+ //tmp = xd*xd + yd*yd - g*y;
+ tmp = (xd*xd + yd*yd - g*y)/(x*x + y*y);
+
+ fy[0] = xd;
+ fy[1] = yd;
+ fy[2] = - x*tmp;
+ fy[3] = - y*tmp - g;
+
+
+ return(0);
+}
+
+int PendLrSystem::explicitODE(Real t, const Vector& yy,
+ Vector& fy) const
+{
+ return pendODE(t, yy, fy);
+}
+
+int PendLrSystem::constraint(Real t, const Vector& yy,
+ Vector& cout) const
+{
+ Real x, y, xd, yd;
+
+ x = yy[0];
+ y = yy[1];
+ xd = yy[2];
+ yd = yy[3];
+
+ cout[0] = (x*x + y*y - 1.0)/2;
+ cout[1] = x*xd + y*yd;
+ //cout[2] = cout[0] + cout[1];
+
+ return(0);
+}
+
+
+static void PrintFinalStats(CPodes& cpode)
+{
+ Real h0u;
+ int nst, nfe, nsetups, nje, nfeLS, nni, ncfn, netf, nge;
+ int nproj, nce, nsetupsP, nprf;
+ int flag;
+
+ flag = cpode.getActualInitStep(&h0u);
+ flag = cpode.getNumSteps(&nst);
+ flag = cpode.getNumFctEvals(&nfe);
+ flag = cpode.getNumLinSolvSetups(&nsetups);
+ flag = cpode.getNumErrTestFails(&netf);
+ flag = cpode.getNumNonlinSolvIters(&nni);
+ flag = cpode.getNumNonlinSolvConvFails(&ncfn);
+
+ flag = cpode.dlsGetNumJacEvals(&nje);
+ flag = cpode.dlsGetNumFctEvals(&nfeLS);
+
+ flag = cpode.getProjStats(&nproj, &nce, &nsetupsP, &nprf);
+
+ flag = cpode.getNumGEvals(&nge);
+
+ printf("\nFinal Statistics:\n");
+ printf("h0u = %g\n",h0u);
+ printf("nst = %-6d nfe = %-6d nsetups = %-6d\n",
+ nst, nfe, nsetups);
+ printf("nfeLS = %-6d nje = %d\n",
+ nfeLS, nje);
+ printf("nni = %-6d ncfn = %-6d netf = %-6d \n",
+ nni, ncfn, netf);
+ printf("nproj = %-6d nce = %-6d nsetupsP = %-6d nprf = %-6d\n",
+ nproj, nce, nsetupsP, nprf);
+ printf("nge = %d\n", nge);
+
+}
+
diff --git a/SimTKmath/Integrators/src/CPodesIntegrator.cpp b/SimTKmath/Integrators/src/CPodesIntegrator.cpp
new file mode 100644
index 0000000..e6bb676
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodesIntegrator.cpp
@@ -0,0 +1,646 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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 "simmath/CPodesIntegrator.h"
+
+#include "IntegratorRep.h"
+#include "CPodesIntegratorRep.h"
+
+using namespace SimTK;
+
+
+//------------------------------------------------------------------------------
+// CPODES INTEGRATOR
+//------------------------------------------------------------------------------
+
+CPodesIntegrator::CPodesIntegrator(const System& sys, CPodes::LinearMultistepMethod method) {
+ rep = new CPodesIntegratorRep(this, sys, method);
+}
+
+CPodesIntegrator::CPodesIntegrator(const System& sys, CPodes::LinearMultistepMethod method, CPodes::NonlinearSystemIterationType iterationType) {
+ rep = new CPodesIntegratorRep(this, sys, method, iterationType);
+}
+
+void CPodesIntegrator::setUseCPodesProjection() {
+ CPodesIntegratorRep& cprep = dynamic_cast<CPodesIntegratorRep&>(*rep);
+ cprep.setUseCPodesProjection();
+}
+
+void CPodesIntegrator::setOrderLimit(int order) {
+ CPodesIntegratorRep& cprep = dynamic_cast<CPodesIntegratorRep&>(*rep);
+ cprep.setOrderLimit(order);
+}
+
+
+
+//------------------------------------------------------------------------------
+// CPODES INTEGRATOR REP
+//------------------------------------------------------------------------------
+// This class implements the abstract CPodesSystem interface understood by our
+// C++ interface to CPodes.
+
+class CPodesIntegratorRep::CPodesSystemImpl : public CPodesSystem {
+public:
+ CPodesSystemImpl(CPodesIntegratorRep& integ, const System& system)
+ : integ(integ), system(system) {}
+
+ // Calculate ydot = f(t,y).
+ int explicitODE(Real t, const Vector& y, Vector& ydot) const {
+ try {
+ integ.setAdvancedStateAndRealizeDerivatives(t,y);
+ }
+ catch(...) { return CPodes::RecoverableError; } // assume recoverable
+ ydot = integ.getAdvancedState().getYDot();
+ return CPodes::Success;
+ }
+
+ // Calculate yerr = c(t,y).
+ int constraint(Real t, const Vector& y, Vector& yerr) const {
+ try {
+ integ.setAdvancedStateAndRealizeKinematics(t,y);
+ }
+ catch(...) { return CPodes::RecoverableError; } // assume recoverable
+ yerr = integ.getAdvancedState().getYErr();
+ return CPodes::Success;
+ }
+
+ // Given a state (t,y) not on the constraint manifold, return ycorr
+ // such that (t,y+ycorr+eps) is on the manifold, with
+ // ||eps||_wrms <= epsProj. 'err' passed in as the integrator's current
+ // error estimate for state y; optionally project it to eliminate the
+ // portion normal to the manifold.
+ int project(Real t, const Vector& y, Vector& ycorr, Real epsProj, Vector& err) const {
+ integ.setAdvancedState(t,y);
+ State& advanced = integ.updAdvancedState();
+
+ try {
+ system.realize(advanced, Stage::Time);
+ system.prescribeQ(advanced); // set q_p
+ system.realize(advanced, Stage::Position);
+ bool anyChanges;
+ if (!integ.localProjectQAndQErrEstNoThrow
+ (advanced, err, anyChanges, Infinity)) //TODO: proj limit?
+ return CPodes::RecoverableError;
+
+ system.prescribeU(advanced); // set u_p
+ system.realize(advanced, Stage::Velocity);
+ if (!integ.localProjectUAndUErrEstNoThrow
+ (advanced, err, anyChanges, Infinity)) //TODO: proj limit?
+ return CPodes::RecoverableError;
+ }
+ catch (...) { return CPodes::RecoverableError; } // assume recoverable
+ ycorr = advanced.getY()-y;
+ return CPodes::Success;
+ }
+
+ /**
+ * Calculate the event trigger functions.
+ */
+ int root(Real t, const Vector& y, const Vector& yp, Vector& gout) const {
+ try {
+ integ.setAdvancedStateAndRealizeDerivatives(t,y);
+ }
+ catch(...) { return CPodes::RecoverableError; } // assume recoverable
+ gout = integ.getAdvancedState().getEventTriggers();
+ return CPodes::Success;
+ }
+private:
+ CPodesIntegratorRep& integ;
+ const System& system;
+};
+
+void CPodesIntegratorRep::init
+ (CPodes::LinearMultistepMethod method,
+ CPodes::NonlinearSystemIterationType iterationType)
+{
+ cpodes = new CPodes(CPodes::ExplicitODE, method, iterationType);
+ cps = new CPodesSystemImpl(*this, getSystem());
+ initialized = false;
+ useCpodesProjection = false;
+}
+
+CPodesIntegratorRep::CPodesIntegratorRep
+ (Integrator* handle, const System& sys,
+ CPodes::LinearMultistepMethod method)
+: IntegratorRep(handle, sys), method(method) {
+ init(method, method == CPodes::Adams ? CPodes::Functional : CPodes::Newton);
+}
+
+CPodesIntegratorRep::CPodesIntegratorRep
+ (Integrator* handle, const System& sys,
+ CPodes::LinearMultistepMethod method,
+ CPodes::NonlinearSystemIterationType iterationType)
+: IntegratorRep(handle, sys), method(method) {
+ init(method, iterationType);
+}
+
+CPodesIntegratorRep::~CPodesIntegratorRep() {
+ delete cpodes;
+ delete cps;
+}
+
+void CPodesIntegratorRep::methodInitialize(const State& state) {
+ if (state.getSystemStage() < Stage::Model)
+ reconstructForNewModel();
+ initializeIntegrationParameters();
+ initialized = true;
+ pendingReturnCode = -1;
+ previousStartTime = 0.0;
+ resetMethodStatistics();
+ getSystem().realize(state, Stage::Velocity);
+ const int ny = state.getY().size();
+ const int nc = state.getNYErr();
+ Vector ydot(ny);
+ if (cps->explicitODE(state.getTime(), Vector(state.getY()), ydot)
+ != CPodes::Success)
+ {
+ SimTK_THROW1(Integrator::InitializationFailed,
+ "Failed to calculate ydot");
+ }
+ int retval;
+ //TODO: change this to do abstol only for q, reltol for u&z
+ Real relTol = getAccuracyInUse();
+ Real absTol = relTol/10; //TODO: base on weights
+ if ((retval=cpodes->init(*cps, state.getTime(),
+ Vector(state.getY()), ydot,
+ CPodes::ScalarScalar, relTol, &absTol))
+ != CPodes::Success)
+ {
+ printf("init() returned %d\n", retval);
+ SimTK_THROW1(Integrator::InitializationFailed, "init() failed");
+ }
+ cpodes->lapackDense(ny);
+ cpodes->setNonlinConvCoef(Real(0.01)); // TODO (default is 0.1)
+ if (useCpodesProjection) {
+ const int nqerr = state.getNQErr(), nuerr = state.getNUErr();
+ const Real tol = getConstraintToleranceInUse();
+ Vector constraintTols(nqerr+nuerr);
+ constraintTols(0,nqerr) = tol*state.getQErrWeights();
+ constraintTols(nqerr,nuerr) = tol*state.getUErrWeights();
+ cpodes->projInit(CPodes::L2Norm, CPodes::Nonlinear,
+ constraintTols);
+ cpodes->lapackDenseProj(nc, ny, CPodes::ProjectWithQRPivot);
+ }
+ else {
+ cpodes->projDefine();
+ }
+ cpodes->rootInit(state.getNEventTriggers());
+ if (state.getNEventTriggers() > 0) {
+ Array_<EventTriggerInfo> triggerInfo;
+ getSystem().calcEventTriggerInfo(state, triggerInfo);
+ Array_<int> rootDir(triggerInfo.size());
+ for (int i = 0; i < (int)triggerInfo.size(); ++i) {
+ if (triggerInfo[i].shouldTriggerOnFallingSignTransition()) {
+ if (triggerInfo[i].shouldTriggerOnRisingSignTransition())
+ rootDir[i] = 0; // All transitions
+ else
+ rootDir[i] = -1; // Falling transitions only
+ }
+ else
+ rootDir[i] = 1; // Rising transitions only
+ }
+ cpodes->setRootDirection(rootDir);
+ }
+}
+
+void CPodesIntegratorRep::methodReinitialize
+ (Stage stage, bool shouldTerminate) {
+ if (stage < Stage::Report) {
+ pendingReturnCode = -1;
+ State state = getAdvancedState();
+ getSystem().realize(state, Stage::Acceleration);
+ //TODO: change this to do abstol only for q, reltol for u&z
+ Real relTol = getAccuracyInUse();
+ Real absTol = relTol/10; //TODO: base on weights
+ cpodes->reInit(*cps, state.getTime(),
+ Vector(state.getY()), Vector(state.getYDot()),
+ CPodes::ScalarScalar, relTol, &absTol);
+ }
+}
+
+void CPodesIntegratorRep::initializeIntegrationParameters() {
+ if (userInitStepSize != -1)
+ cpodes->setInitStep(userInitStepSize);
+ if (userMinStepSize != -1)
+ cpodes->setMinStep(userMinStepSize);
+ if (userMaxStepSize != -1)
+ cpodes->setMaxStep(userMaxStepSize);
+ if (userFinalTime != -1.)
+ cpodes->setStopTime(userFinalTime);
+ if (userInternalStepLimit != -1)
+ cpodes->setMaxNumSteps(userInternalStepLimit);
+ if (userProjectEveryStep != -1)
+ if (userProjectEveryStep==1)
+ cpodes->setProjFrequency(1); // every step
+}
+
+void CPodesIntegratorRep::reconstructForNewModel() {
+ initialized = false;
+ delete cpodes;
+ cpodes = new CPodes(CPodes::ExplicitODE, CPodes::BDF, CPodes::Newton);
+}
+
+// Create an interpolated state at time t, which is between tPrev and tCurrent.
+// If we haven't yet delivered an interpolated state in this interval, we have
+// to initialize its discrete part from the advanced state.
+void CPodesIntegratorRep::createInterpolatedState(Real t) {
+ const System& system = getSystem();
+ const State& advanced = getAdvancedState();
+ State& interp = updInterpolatedState();
+ interp = advanced; // pick up discrete stuff.
+ Vector yout(advanced.getY().size());
+ cpodes->getDky(t, 0, yout);
+ interp.updY() = yout;
+ interp.updTime() = t;
+
+ if (userProjectInterpolatedStates == 0) {
+ system.realize(interp, Stage::Time);
+ system.prescribeQ(interp);
+ system.realize(interp, Stage::Position);
+ system.prescribeU(interp);
+ system.realize(interp, Stage::Velocity);
+ return;
+ }
+
+ // We may need to project onto constraint manifold. Allow project()
+ // to throw an exception if it fails since there is no way to recover here.
+ realizeAndProjectKinematicsWithThrow(interp, ProjectOptions::LocalOnly);
+}
+
+// Take a step. See AbstractIntegratorRep::stepTo() for how this is supposed
+// to behave. We have to go through some contortions to squeeze CPodes into
+// that mold.
+Integrator::SuccessfulStepStatus CPodesIntegratorRep::
+stepTo(Real reportTime, Real scheduledEventTime) {
+ assert(initialized);
+ assert(reportTime >= getState().getTime());
+ assert(scheduledEventTime >= getState().getTime());
+
+ if (getStepCommunicationStatus() == FinalTimeHasBeenReturned) {
+ SimTK_ERRCHK2_ALWAYS(!"EndOfSimulation already returned",
+ "Integrator::stepTo()",
+ "Attempted stepTo(t=%g) but final time %g had already been "
+ "reached and returned."
+ "\nCheck for Integrator::EndOfSimulation status, or use the "
+ "Integrator::initialize() method to restart.",
+ reportTime, userFinalTime);
+ }
+
+ // If this is the start of a continuous interval, return immediately so
+ // the current state will be seen as part of the trajectory.
+ if (startOfContinuousInterval) {
+ // The set of event triggers might have changed.
+ getSystem().calcEventTriggerInfo(getAdvancedState(),
+ updEventTriggerInfo());
+ pendingReturnCode = -1; // forget post-event state
+ startOfContinuousInterval = false;
+ return Integrator::StartOfContinuousInterval;
+ }
+
+ CPodes::StepMode mode;
+ if (userFinalTime != -1 || userAllowInterpolation==0)
+ mode = (userReturnEveryInternalStep == 1) ? CPodes::OneStepTstop
+ : CPodes::NormalTstop;
+ else
+ mode = (userReturnEveryInternalStep == 1) ? CPodes::OneStep
+ : CPodes::Normal;
+
+ // Keep taking steps until something interesting happens at tMax or
+ // earlier.
+ Real tMax = std::min(reportTime, scheduledEventTime);
+
+ // We might have to create a fake tstop to prevent interpolation.
+ bool isFakeTstop = false;
+ if (userAllowInterpolation==0) {
+ if (userFinalTime == -1 || userFinalTime > tMax) {
+ isFakeTstop = true;
+ cpodes->setStopTime(tMax);
+ }
+ }
+
+ // Assume we'll return at the advanced state; we'll change this below
+ // if necessary.
+ setUseInterpolatedState(false);
+ while (true) {
+ Real tret;
+ int res;
+ const bool usePendingReturnCode = (pendingReturnCode != -1);
+ if (usePendingReturnCode) {
+ // The last time returned was an event or report time. The
+ // integrator has already gone beyond that time, so reset
+ // everything to how it was after the last call to cpodes->step()
+ // and then process the step.
+
+ res = pendingReturnCode;
+ tret = previousTimeReturned;
+ if (savedY.size() > 0) {
+ setAdvancedStateAndRealizeKinematics(tret, savedY);
+ } else {
+ updAdvancedState().updTime() = tret;
+ }
+ pendingReturnCode = -1;
+ }
+ else if (tMax == getState().getTime()) {
+
+ // A report or event is scheduled for the current time, so return
+ // immediately.
+
+ res = CPodes::Success;
+ tret = tMax;
+ previousTimeReturned = tret;
+ updAdvancedState().updTime() = tret;
+ }
+ else {
+ // We're going to advance time now.
+
+ // Auto-update discrete variables. This update is not allowed to
+ // affect any computations performed at the current state value so
+ // does not invalidate any stage. Swap the discrete state update
+ // cache entries with the state variables.
+ updAdvancedState().autoUpdateDiscreteVariables();
+
+ previousStartTime = getAdvancedTime();
+ Vector yout(getAdvancedState().getY().size());
+ Vector ypout(getAdvancedState().getY().size()); // ignored
+ int oldSteps=0, oldTestFailures=0, oldNonlinIterations=0,
+ oldNonlinConvFailures=0;
+ cpodes->getNumSteps(&oldSteps);
+ cpodes->getNumErrTestFails(&oldTestFailures);
+ cpodes->getNumNonlinSolvIters(&oldNonlinIterations);
+ cpodes->getNumNonlinSolvConvFails(&oldNonlinConvFailures);
+
+ //---------------------step------------------------
+ res = cpodes->step(tMax, &tret, yout, ypout, mode);
+ //-------------------------------------------------
+ if (res == CPodes::TstopReturn && isFakeTstop)
+ res = CPodes::Success;
+
+ if (res == CPodes::TooClose) {
+ // This happens when the user asked the integrator to advance
+ // time by a tiny amount, comparable to numerical precision.
+ // Since CPODES cannot advance time by such small increments,
+ // and the state would not change significantly in that time
+ // anyway, just set the time while leaving the rest of the
+ // state unchanged.
+ tret = tMax;
+ yout = getAdvancedState().getY();
+ res = CPodes::Success;
+ }
+
+ int newSteps=0, newTestFailures=0, newNonlinIterations=0,
+ newNonlinConvFailures=0;
+ cpodes->getNumSteps(&newSteps);
+ cpodes->getNumErrTestFails(&newTestFailures);
+ cpodes->getNumNonlinSolvIters(&newNonlinIterations);
+ cpodes->getNumNonlinSolvConvFails(&newNonlinConvFailures);
+ statsStepsTaken += newSteps-oldSteps;
+ statsErrorTestFailures += newTestFailures-oldTestFailures;
+ // Project stats were already updated in project() above.
+ statsIterations += newNonlinIterations-oldNonlinIterations;
+ statsConvergenceTestFailures += newNonlinConvFailures-oldNonlinConvFailures;
+
+ // This takes care of prescribed motion.
+ setAdvancedStateAndRealizeKinematics(tret, yout);
+ previousTimeReturned = tret;
+ }
+
+ realizeStateDerivatives(getAdvancedState());
+
+ // Check for integration errors.
+ if (res == CPodes::TooMuchWork) {
+ // The maximum number of steps was reached.
+ setStepCommunicationStatus(IntegratorRep::StepHasBeenReturnedNoEvent);
+ return Integrator::ReachedStepLimit;
+ }
+ if (res < 0) {
+ // An error of some sort occurred.
+ SimTK_THROW2(Integrator::StepFailed, getAdvancedState().getTime(),
+ "CPodes::step() returned an error");
+ }
+
+ // No error occurred.
+
+ // If a triggered event was isolated to the window (tLo,tHi],
+ // CPodes will have returned with tret==tHi, which is where the
+ // advancedState is now. We need to return an interpolated, "last good"
+ // state at tLo, with return status ReachedEventTrigger. The calling
+ // TimeStepper will then invoke the event
+ // handler to fix up the advanced state at tHi without reporting it.
+ // (If the event handler modified the state, startOfContinuousInterval
+ // will be set.) Then we will get called here again and need to report
+ // the now-fixed advanced state at tHi as part of the trajectory,
+ // either as an ordinary time advanced state or as a
+ // StartOfContinuousInterval state if something happened.
+ // But there might be some reports due prior to tLo first.
+
+ if (res == CPodes::RootReturn) {
+ Real tLo, tHi;
+ cpodes->getRootWindow(&tLo, &tHi);
+ tret = tLo;
+ }
+
+ // Determine the correct return code.
+
+ if (tret >= reportTime && reportTime <= scheduledEventTime) {
+ // We reached the scheduled report time.
+ // If necessary, generate an interpolated state.
+ if (tret > tMax) {
+ setUseInterpolatedState(true);
+ createInterpolatedState(tMax);
+ realizeStateDerivatives(getInterpolatedState());
+ }
+ savedY.resize(0);
+ pendingReturnCode = res;
+ setStepCommunicationStatus(IntegratorRep::StepHasBeenReturnedNoEvent);
+ return Integrator::ReachedReportTime;
+ }
+
+ if (tret >= scheduledEventTime) {
+ // We reached a scheduled event time.
+ savedY.resize(0);
+ if (tret > scheduledEventTime) {
+ // Back up the advanced state to the event time.
+ savedY = getAdvancedState().getY();
+ createInterpolatedState(scheduledEventTime);
+ setAdvancedStateAndRealizeDerivatives(scheduledEventTime,
+ getInterpolatedState().getY());
+ }
+ pendingReturnCode = res;
+ setStepCommunicationStatus(IntegratorRep::StepHasBeenReturnedWithEvent);
+ return Integrator::ReachedScheduledEvent;
+ }
+
+
+ if (res == CPodes::RootReturn) {
+ // An event was triggered in the interval (tLo,tHi]. We're
+ // going to return with an interpolated state at tLo; CPodes has
+ // already capped the advanced state at tHi.
+
+ Array_<SystemEventTriggerIndex> eventIndices;
+ Array_<Real> eventTimes;
+ Array_<Event::Trigger> eventTransitions;
+ int nevents = getAdvancedState().getNEventTriggers();
+ int* eventFlags = new int[nevents];
+ cpodes->getRootInfo(eventFlags);
+ for (SystemEventTriggerIndex i(0); i < nevents; ++i)
+ if (eventFlags[i] != 0) {
+ eventIndices.push_back(i);
+ eventTimes.push_back(previousTimeReturned);
+ eventTransitions.push_back(eventFlags[i] == 1
+ ? Event::Rising : Event::Falling);
+ }
+ delete[] eventFlags;
+ Array_<EventId> ids;
+ findEventIds(eventIndices, ids);
+
+ // Generate an interpolated state at tLo.
+ setUseInterpolatedState(true);
+ createInterpolatedState(tret);
+ realizeStateDerivatives(getInterpolatedState());
+
+ setTriggeredEvents(tret, previousTimeReturned,
+ ids, eventTimes, eventTransitions);
+
+ // For next time, we'll treat the state at tHi as an ordinary
+ // trajectory step, but this will only get used if the event
+ // handler makes no changes. Otherwise, we'll be called with
+ // startOfContinuousInterval==true and the handler-modified
+ // state will get reported above instead.
+ pendingReturnCode = CPodes::Success;
+ setStepCommunicationStatus(IntegratorRep::StepHasBeenReturnedWithEvent);
+ return Integrator::ReachedEventTrigger;
+ }
+
+ if (res == CPodes::TstopReturn) {
+ // The specified final time was reached.
+ if (usePendingReturnCode) {
+ // The final step result was already reported; now just return
+ // the same state but with an "end of simulation" status. No
+ // further calls should be made to stepTo().
+ setStepCommunicationStatus(IntegratorRep::FinalTimeHasBeenReturned);
+ terminationReason = Integrator::ReachedFinalTime;
+ return Integrator::EndOfSimulation;
+ }
+ // This is our first encounter with the final time. Report this as
+ // a TimeHasAdvanced state if the user has asked for those,
+ // otherwise pretend there was a report scheduled there. This should
+ // match the behavior of AbstractIntegratorRep.
+ savedY.resize(0);
+ pendingReturnCode = res;
+ setStepCommunicationStatus(IntegratorRep::StepHasBeenReturnedNoEvent);
+ return (userReturnEveryInternalStep == 1)
+ ? Integrator::TimeHasAdvanced
+ : Integrator::ReachedReportTime;
+ }
+
+ if (userReturnEveryInternalStep == 1) {
+ // The user asked to be notified of every internal step.
+ setStepCommunicationStatus(IntegratorRep::StepHasBeenReturnedNoEvent);
+ return Integrator::TimeHasAdvanced;
+ }
+
+ // Otherwise keep going -- no one wants to see this step.
+ }
+}
+
+Real CPodesIntegratorRep::getActualInitialStepSizeTaken() const {
+ assert(initialized);
+ Real size;
+ cpodes->getActualInitStep(&size);
+ return size;
+}
+
+Real CPodesIntegratorRep::getPreviousStepSizeTaken() const {
+ assert(initialized);
+ Real size;
+ cpodes->getLastStep(&size);
+ return size;
+}
+
+Real CPodesIntegratorRep::getPredictedNextStepSize() const {
+ assert(initialized);
+ Real size;
+ cpodes->getCurrentStep(&size);
+ return size;
+}
+
+int CPodesIntegratorRep::getNumStepsAttempted() const {
+ assert(initialized);
+ return statsStepsTaken+statsErrorTestFailures+statsConvergenceTestFailures;
+}
+
+int CPodesIntegratorRep::getNumStepsTaken() const {
+ assert(initialized);
+ return statsStepsTaken;
+}
+
+int CPodesIntegratorRep::getNumErrorTestFailures() const {
+ assert(initialized);
+ return statsErrorTestFailures;
+}
+
+int CPodesIntegratorRep::getNumConvergenceTestFailures() const {
+ assert(initialized);
+ return statsConvergenceTestFailures;
+}
+
+int CPodesIntegratorRep::getNumIterations() const {
+ assert(initialized);
+ return statsIterations;
+}
+
+void CPodesIntegratorRep::resetMethodStatistics() {
+ statsStepsTaken = 0;
+ statsErrorTestFailures = 0;
+ statsConvergenceTestFailures = 0;
+ statsIterations = 0;
+}
+
+const char* CPodesIntegratorRep::getMethodName() const {
+ return (method == CPodes::BDF ? "CPodesBDF" : "CPodesAdams");
+}
+
+int CPodesIntegratorRep::getMethodMinOrder() const {
+ return 1;
+}
+
+int CPodesIntegratorRep::getMethodMaxOrder() const {
+ return (method == CPodes::BDF ? 5 : 12);
+}
+
+bool CPodesIntegratorRep::methodHasErrorControl() const {
+ return true;
+}
+
+void CPodesIntegratorRep::setUseCPodesProjection() {
+ SimTK_APIARGCHECK_ALWAYS(!initialized, "CPodesIntegrator",
+ "setUseCPodesProjection",
+ "This method may not be invoked after the integrator has been initialized.");
+ useCpodesProjection = true;
+}
+
+void CPodesIntegratorRep::setOrderLimit(int order) {
+ cpodes->setMaxOrd(order);
+}
+
+
diff --git a/SimTKmath/Integrators/src/CPodesIntegratorRep.h b/SimTKmath/Integrators/src/CPodesIntegratorRep.h
new file mode 100644
index 0000000..9e1b49b
--- /dev/null
+++ b/SimTKmath/Integrators/src/CPodesIntegratorRep.h
@@ -0,0 +1,84 @@
+#ifndef SimTK_SIMMATH_CPODES_INTEGRATOR_REP_H_
+#define SimTK_SIMMATH_CPODES_INTEGRATOR_REP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/Integrator.h"
+#include "simmath/internal/SimTKcpodes.h"
+
+#include "IntegratorRep.h"
+
+
+namespace SimTK {
+
+class CPodesIntegratorRep : public IntegratorRep {
+public:
+ CPodesIntegratorRep(Integrator* handle, const System& sys, CPodes::LinearMultistepMethod method);
+ CPodesIntegratorRep(Integrator* handle, const System& sys, CPodes::LinearMultistepMethod method, CPodes::NonlinearSystemIterationType iterationType);
+ ~CPodesIntegratorRep();
+ void methodInitialize(const State&);
+ void methodReinitialize(Stage stage, bool shouldTerminate);
+ Integrator::SuccessfulStepStatus stepTo(Real reportTime, Real scheduledEventTime);
+ Real getActualInitialStepSizeTaken() const;
+ Real getPreviousStepSizeTaken() const;
+ Real getPredictedNextStepSize() const;
+ int getNumStepsAttempted() const;
+ int getNumStepsTaken() const;
+ int getNumErrorTestFailures() const;
+ int getNumConvergenceTestFailures() const;
+ int getNumConvergentIterations() const
+ {SimTK_ASSERT_ALWAYS(false, "CPodesIntegratorRep::getNumConvergentIterations(): not implemented");}
+ int getNumDivergentIterations() const
+ {SimTK_ASSERT_ALWAYS(false, "CPodesIntegratorRep::getNumDivergentIterations(): not implemented");}
+ int getNumIterations() const;
+ void resetMethodStatistics();
+ void createInterpolatedState(Real t);
+ void initializeIntegrationParameters();
+ void reconstructForNewModel();
+ const char* getMethodName() const;
+ int getMethodMinOrder() const;
+ int getMethodMaxOrder() const;
+ bool methodHasErrorControl() const;
+ void setUseCPodesProjection();
+ void setOrderLimit(int order);
+ class CPodesSystemImpl;
+ friend class CPodesSystemImpl;
+private:
+ CPodes* cpodes;
+ CPodesSystemImpl* cps;
+ bool initialized, useCpodesProjection;
+ int statsStepsTaken, statsErrorTestFailures, statsConvergenceTestFailures;
+ int statsIterations;
+ int pendingReturnCode;
+ Real previousStartTime, previousTimeReturned;
+ Vector savedY;
+ CPodes::LinearMultistepMethod method;
+ void init(CPodes::LinearMultistepMethod method, CPodes::NonlinearSystemIterationType iterationType);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_CPODES_INTEGRATOR_REP_H_
diff --git a/SimTKmath/Integrators/src/ExplicitEulerIntegrator.cpp b/SimTKmath/Integrators/src/ExplicitEulerIntegrator.cpp
new file mode 100644
index 0000000..f593a37
--- /dev/null
+++ b/SimTKmath/Integrators/src/ExplicitEulerIntegrator.cpp
@@ -0,0 +1,182 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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 "simmath/ExplicitEulerIntegrator.h"
+
+#include "IntegratorRep.h"
+#include "ExplicitEulerIntegratorRep.h"
+
+using namespace SimTK;
+
+
+//------------------------------------------------------------------------------
+// EXPLICIT EULER INTEGRATOR
+//------------------------------------------------------------------------------
+
+ExplicitEulerIntegrator::ExplicitEulerIntegrator(const System& sys) {
+ rep = new ExplicitEulerIntegratorRep(this, sys);
+}
+
+ExplicitEulerIntegrator::ExplicitEulerIntegrator(const System& sys, Real stepSize) {
+ rep = new ExplicitEulerIntegratorRep(this, sys);
+ setInitialStepSize(stepSize);
+}
+
+
+//------------------------------------------------------------------------------
+// EXPLICIT EULER INTEGRATOR REP
+//------------------------------------------------------------------------------
+ExplicitEulerIntegratorRep::ExplicitEulerIntegratorRep
+ (Integrator* handle, const System& sys)
+: AbstractIntegratorRep(handle, sys, 1, 1, "ExplicitEuler", true)
+{
+}
+
+
+
+//==============================================================================
+// CREATE INTERPOLATED STATE
+//==============================================================================
+// Create an interpolated state at time t, which is between tPrev and tCurrent.
+// If we haven't yet delivered an interpolated state in this interval, we have
+// to initialize its discrete part from the advanced state.
+void ExplicitEulerIntegratorRep::createInterpolatedState(Real t) {
+ const System& system = getSystem();
+ const State& advanced = getAdvancedState();
+ State& interp = updInterpolatedState();
+ interp = advanced; // pick up discrete stuff.
+ const Real weight1 = (getAdvancedTime()-t) /
+ (getAdvancedTime()-getPreviousTime());
+ const Real weight2 = 1-weight1;
+ interp.updY() = weight1*getPreviousY()+weight2*getAdvancedState().getY();
+ interp.updTime() = t;
+
+ if (userProjectInterpolatedStates == 0) {
+ system.realize(interp, Stage::Time);
+ system.prescribeQ(interp);
+ system.realize(interp, Stage::Position);
+ system.prescribeU(interp);
+ system.realize(interp, Stage::Velocity);
+ return;
+ }
+
+ // We may need to project onto constraint manifold. Allow project()
+ // to throw an exception if it fails since there is no way to recover here.
+ realizeAndProjectKinematicsWithThrow(interp, ProjectOptions::LocalOnly);
+}
+
+
+//==============================================================================
+// BACK UP ADVANCED STATE BY INTERPOLATION
+//==============================================================================
+// Interpolate the advanced state back to an earlier part of the interval,
+// forgetting about the rest of the interval. This is necessary, for
+// example after we have localized an event trigger to an interval tLow:tHigh
+// where tHigh < tAdvanced.
+void ExplicitEulerIntegratorRep::backUpAdvancedStateByInterpolation(Real t) {
+ const System& system = getSystem();
+ State& advanced = updAdvancedState();
+
+ assert(getPreviousTime() <= t && t <= advanced.getTime());
+ advanced.updY() = getPreviousY() + (t-getPreviousTime())*getPreviousYDot();
+ advanced.updTime() = t;
+
+ // Ignore any user request not to project interpolated states here -- this
+ // is the actual advanced state which will be propagated through the
+ // rest of the trajectory so we can't allow it not to satisfy the
+ // constraints!
+ // But it is OK if it just *barely* satisfies the constraints so we
+ // won't get carried away if the user isn't being finicky about it.
+
+ // Project position constraints if warranted. Allow project()
+ // to throw an exception if it fails since there is no way to recover here.
+
+ realizeAndProjectKinematicsWithThrow(advanced, ProjectOptions::LocalOnly);
+}
+
+
+
+//==============================================================================
+// ATTEMPT DAE STEP
+//==============================================================================
+// Note that ExplicitEuler overrides the entire DAE step because it can't use
+// the default ODE-then-DAE structure. Instead the constraint projections are
+// interwoven here. The reason is that we need the end-of-step derivative
+// value in order to be able to get an error estimate, and those derivatives
+// must not be calculated until projection is done or we would do more than
+// one function evaluation per step.
+bool ExplicitEulerIntegratorRep::attemptDAEStep
+ (Real t1, Vector& yErrEst, int& errOrder, int& numIterations)
+{
+ const System& system = getSystem();
+ State& advanced = updAdvancedState();
+
+ statsStepsAttempted++;
+ const Real h = t1 - getPreviousTime();
+
+ // Take the step.
+ advanced.updTime() = t1;
+ advanced.updY() = getPreviousY() + h*getPreviousYDot();
+ yErrEst = advanced.getY(); // save unprojected Y for error estimate
+
+ system.realize(advanced, Stage::Time);
+ system.prescribeQ(advanced);
+ system.realize(advanced, Stage::Position);
+
+ // Consider position constraint projection. Note that we have to do this
+ // projection prior to calculating prescribed u's since the prescription
+ // can depend on q's. Prevent project() from throwing an exception since
+ // failure here may be recoverable.
+ Vector dummy; // no error estimate to project
+ bool anyChanges;
+ if (!localProjectQAndQErrEstNoThrow(advanced, dummy, anyChanges))
+ return false; // convergence failure for this step
+
+ // q's satisfy the position constraint manifold. Now work on u's.
+
+ system.prescribeU(advanced);
+ system.realize(advanced, Stage::Velocity);
+
+ // Now try velocity constraint projection. Nothing will happen if
+ // velocity constraints are already satisfied unless user has set the
+ // ForceProjection option.
+
+ if (!localProjectUAndUErrEstNoThrow(advanced, dummy, anyChanges))
+ return false; // convergence failure for this step
+
+ // Now calculate derivatives at the end of this interval/start of next
+ // interval. TODO: prevent this from throwing
+ try {realizeStateDerivatives(advanced);}
+ catch (...) {return false;} // evaluation failed
+
+ // Calculate a reference state with the explicit trapezoidal rule, and use
+ // it to estimate error.
+ //TODO: this is an odd mix of the unprojected Y and the projected YDot;
+ //probably not right!
+ yErrEst -= getPreviousY() + (h/2)*(getPreviousYDot()+advanced.getYDot());
+ errOrder = 2;
+ numIterations = 1;
+ return true;
+}
+
+
diff --git a/SimTKmath/Integrators/src/ExplicitEulerIntegratorRep.h b/SimTKmath/Integrators/src/ExplicitEulerIntegratorRep.h
new file mode 100644
index 0000000..22da281
--- /dev/null
+++ b/SimTKmath/Integrators/src/ExplicitEulerIntegratorRep.h
@@ -0,0 +1,43 @@
+#ifndef SimTK_SIMMATH_EXPLICIT_EULER_INTEGRATOR_REP_H_
+#define SimTK_SIMMATH_EXPLICIT_EULER_INTEGRATOR_REP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "AbstractIntegratorRep.h"
+
+namespace SimTK {
+
+class ExplicitEulerIntegratorRep : public AbstractIntegratorRep {
+public:
+ ExplicitEulerIntegratorRep(Integrator* handle, const System& sys);
+protected:
+ bool attemptDAEStep
+ (Real t1, Vector& yErrEst, int& errOrder, int& numIterations);
+ void createInterpolatedState(Real t);
+ void backUpAdvancedStateByInterpolation(Real t);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_EXPLICIT_EULER_INTEGRATOR_REP_H_
diff --git a/SimTKmath/Integrators/src/Integrator.cpp b/SimTKmath/Integrators/src/Integrator.cpp
new file mode 100644
index 0000000..a474549
--- /dev/null
+++ b/SimTKmath/Integrators/src/Integrator.cpp
@@ -0,0 +1,451 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This is the private (library side) implementation of the Simmath
+ * Integrator family of classes.
+ */
+
+#include "SimTKcommon.h"
+#include "simmath/Integrator.h"
+
+#include "IntegratorRep.h"
+
+#include <exception>
+#include <limits>
+#include <iostream>
+
+using std::cout; using std::endl;
+
+namespace SimTK {
+
+ //////////////////////////////////
+ // IMPLEMENTATION OF INTEGRATOR //
+ //////////////////////////////////
+
+Integrator::~Integrator() {
+ delete rep; rep=0;
+}
+
+void Integrator::resetAllStatistics() {
+ updRep().resetIntegratorStatistics();
+ updRep().resetMethodStatistics();
+}
+
+void Integrator::initialize(const State& initState) {
+ updRep().initialize(initState);
+}
+
+void Integrator::reinitialize(Stage g, bool shouldTerminate) {
+ updRep().reinitialize(g,shouldTerminate);
+}
+
+Integrator::SuccessfulStepStatus
+Integrator::stepTo(Real reportTime, Real advanceLimit) {
+ return updRep().stepTo(reportTime, advanceLimit);
+}
+
+Integrator::SuccessfulStepStatus
+Integrator::stepBy(Real interval, Real advanceIntervalLimit) {
+ const Real t = getRep().getState().getTime();
+ return updRep().stepTo(t + interval, t + advanceIntervalLimit);
+}
+
+bool Integrator::isSimulationOver() const {
+ return getRep().isSimulationOver();
+}
+
+Integrator::TerminationReason Integrator::getTerminationReason() const {
+ return getRep().getTerminationReason();
+}
+
+/*static*/ String Integrator::
+getSuccessfulStepStatusString(SuccessfulStepStatus stat) {
+ switch(stat) {
+ case ReachedReportTime: return "ReachedReportTime";
+ case ReachedEventTrigger: return "ReachedEventTrigger";
+ case ReachedScheduledEvent: return "ReachedScheduledEvent";
+ case TimeHasAdvanced: return "TimeHasAdvanced";
+ case ReachedStepLimit: return "ReachedStepLimit";
+ case EndOfSimulation: return "EndOfSimulation";
+ case StartOfContinuousInterval: return "StartOfContinuousInterval";
+ case InvalidSuccessfulStepStatus: return "InvalidSuccessfulStepStatus";
+ default: return String("UNKNOWN SUCCESSFUL STEP STATUS ")
+ + String((int)stat);
+ }
+}
+
+/*static*/ String Integrator::
+getTerminationReasonString(TerminationReason reason) {
+ switch(reason) {
+ case ReachedFinalTime: return "ReachedFinalTime";
+ case AnUnrecoverableErrorOccurred: return "AnUnrecoverableErrorOccurred";
+ case EventHandlerRequestedTermination: return "EventHandlerRequestedTermination";
+ case InvalidTerminationReason: return "InvalidTerminationReason";
+ default: return String("UNKNOWN TERMINATION REASON ")
+ + String((int)reason);
+ }
+}
+
+// The following methods are callable only when the Integrator has just
+// returned a step that ended with an event triggering.
+
+Vec2 Integrator::getEventWindow() const {
+ if (getRep().getStepCommunicationStatus() != IntegratorRep::StepHasBeenReturnedWithEvent) {
+ SimTK_THROW2(CantAskForEventInfoWhenNoEventTriggered, "getEventWindow",
+ getRep().getState().getTime());
+ //NOTREACHED
+ }
+ assert(getRep().getEventWindowLow()==getRep().getState().getTime());
+ assert(getRep().getEventWindowHigh()==getRep().getAdvancedTime());
+ return Vec2(getRep().getEventWindowLow(), getRep().getEventWindowHigh());
+}
+
+const Array_<EventId>&
+Integrator::getTriggeredEvents() const {
+ if (getRep().getStepCommunicationStatus() != IntegratorRep::StepHasBeenReturnedWithEvent) {
+ SimTK_THROW2(CantAskForEventInfoWhenNoEventTriggered, "getTriggeredEvents",
+ getRep().getState().getTime());
+ //NOTREACHED
+ }
+ return getRep().getTriggeredEvents();
+}
+
+const Array_<Real>&
+Integrator::getEstimatedEventTimes() const {
+ if (getRep().getStepCommunicationStatus() != IntegratorRep::StepHasBeenReturnedWithEvent) {
+ SimTK_THROW2(CantAskForEventInfoWhenNoEventTriggered, "getEstimatedEventTimes",
+ getRep().getState().getTime());
+ //NOTREACHED
+ }
+ return getRep().getEstimatedEventTimes();
+}
+
+const Array_<Event::Trigger>&
+Integrator::getEventTransitionsSeen() const {
+ if (getRep().getStepCommunicationStatus() != IntegratorRep::StepHasBeenReturnedWithEvent) {
+ SimTK_THROW2(CantAskForEventInfoWhenNoEventTriggered, "getEventTransitionsSeen",
+ getRep().getState().getTime());
+ //NOTREACHED
+ }
+ return getRep().getEventTransitionsSeen();
+}
+
+const State& Integrator::getState() const {
+ return getRep().getState();
+}
+
+bool Integrator::isStateInterpolated() const {
+ return getRep().isStateInterpolated();
+}
+
+const State& Integrator::getAdvancedState() const {
+ return getRep().getAdvancedState();
+}
+
+State& Integrator::updAdvancedState() {
+ return updRep().updAdvancedState();
+}
+
+
+Real Integrator::getAccuracyInUse() const {
+ return getRep().getAccuracyInUse();
+}
+
+Real Integrator::getConstraintToleranceInUse() const {
+ return getRep().getConstraintToleranceInUse();
+}
+
+Real Integrator::getActualInitialStepSizeTaken() const {
+ return getRep().getActualInitialStepSizeTaken();
+}
+Real Integrator::getPreviousStepSizeTaken() const {
+ return getRep().getPreviousStepSizeTaken();
+}
+Real Integrator::getPredictedNextStepSize() const {
+ return getRep().getPredictedNextStepSize();
+}
+int Integrator::getNumStepsAttempted() const {
+ return getRep().getNumStepsAttempted();
+}
+int Integrator::getNumStepsTaken() const {
+ return getRep().getNumStepsTaken();
+}
+int Integrator::getNumRealizations() const {
+ return getRep().getNumRealizations();
+}
+int Integrator::getNumQProjections() const {
+ return getRep().getNumQProjections();
+}
+int Integrator::getNumUProjections() const {
+ return getRep().getNumUProjections();
+}
+int Integrator::getNumProjections() const {
+ return getRep().getNumQProjections()
+ + getRep().getNumUProjections();
+}
+int Integrator::getNumErrorTestFailures() const {
+ return getRep().getNumErrorTestFailures();
+}
+int Integrator::getNumConvergenceTestFailures() const {
+ return getRep().getNumConvergenceTestFailures();
+}
+int Integrator::getNumRealizationFailures() const {
+ return getRep().getNumRealizationFailures();
+}
+int Integrator::getNumQProjectionFailures() const {
+ return getRep().getNumQProjectionFailures();
+}
+int Integrator::getNumUProjectionFailures() const {
+ return getRep().getNumUProjectionFailures();
+}
+int Integrator::getNumProjectionFailures() const {
+ return getRep().getNumQProjectionFailures()
+ + getRep().getNumUProjectionFailures();
+}
+int Integrator::getNumConvergentIterations() const {
+ return getRep().getNumConvergentIterations();
+}
+int Integrator::getNumDivergentIterations() const {
+ return getRep().getNumDivergentIterations();
+}
+int Integrator::getNumIterations() const {
+ return getRep().getNumIterations();
+}
+
+void Integrator::setFinalTime(Real tFinal) {
+ assert(tFinal == -1. || (0. <= tFinal));
+ updRep().userFinalTime = tFinal;
+}
+
+void Integrator::setInternalStepLimit(int nSteps) {
+ updRep().userInternalStepLimit = nSteps > 0 ? nSteps : -1;
+}
+
+void Integrator::setInitialStepSize(Real z) {
+ assert(z == -1. || z > 0.);
+ assert(getRep().userMinStepSize==-1. || z >= getRep().userMinStepSize);
+ assert(getRep().userMaxStepSize==-1. || z <= getRep().userMaxStepSize);
+ updRep().userInitStepSize = z;
+}
+void Integrator::setMinimumStepSize(Real z) {
+ assert(z == -1. || z > 0.);
+ assert(getRep().userInitStepSize==-1. || z <= getRep().userInitStepSize);
+ assert(getRep().userMaxStepSize ==-1. || z <= getRep().userMaxStepSize);
+ updRep().userMinStepSize = z;
+}
+void Integrator::setMaximumStepSize(Real z) {
+ assert(z == -1. || z > 0.);
+ assert(getRep().userInitStepSize==-1. || z >= getRep().userInitStepSize);
+ assert(getRep().userMinStepSize ==-1. || z >= getRep().userMinStepSize);
+ updRep().userMaxStepSize = z;
+}
+void Integrator::setFixedStepSize(Real stepSize) {
+ updRep().userInitStepSize = stepSize;
+ updRep().userMinStepSize = stepSize;
+ updRep().userMaxStepSize = stepSize;
+}
+void Integrator::setAccuracy(Real accuracy) {
+ assert(accuracy == -1. || (0. < accuracy && accuracy < 1.));
+ updRep().userAccuracy = accuracy;
+}
+void Integrator::setConstraintTolerance(Real consTol) {
+ assert(consTol == -1. || (0. < consTol && consTol <= 1.));
+ updRep().userConsTol=consTol;
+}
+void Integrator::setUseInfinityNorm(bool useInfinityNorm) {
+ updRep().userUseInfinityNorm = useInfinityNorm ? 1 : 0;
+}
+bool Integrator::isInfinityNormInUse() const
+{ return getRep().userUseInfinityNorm == 1; }
+
+void Integrator::setForceFullNewton(bool forceFullNewton) {
+ updRep().userForceFullNewton = forceFullNewton ? 1 : 0;
+}
+void Integrator::setReturnEveryInternalStep(bool shouldReturn) {
+ updRep().userReturnEveryInternalStep = shouldReturn ? 1 : 0;
+}
+void Integrator::setProjectEveryStep(bool forceProject) {
+ updRep().userProjectEveryStep = forceProject ? 1 : 0;
+}
+void Integrator::setAllowInterpolation(bool shouldInterpolate) {
+ updRep().userAllowInterpolation = shouldInterpolate ? 1 : 0;
+}
+void Integrator::setProjectInterpolatedStates(bool shouldProject) {
+ updRep().userProjectInterpolatedStates = shouldProject ? 1 : 0;
+}
+
+bool Integrator::methodHasErrorControl() const {
+ return getRep().methodHasErrorControl();
+}
+
+const char* Integrator::getMethodName() const {
+ return getRep().getMethodName();
+}
+
+int Integrator::getMethodMinOrder() const {
+ return getRep().getMethodMinOrder();
+}
+
+int Integrator::getMethodMaxOrder() const {
+ return getRep().getMethodMaxOrder();
+}
+
+ //////////////////////////////////////
+ // IMPLEMENTATION OF INTEGRATOR REP //
+ //////////////////////////////////////
+
+IntegratorRep::IntegratorRep
+ (Integrator* handle,
+ const System& system)
+ : myHandle(handle), sys(system)
+{
+ invalidateIntegratorInternalState();
+ initializeUserStuff();
+ resetIntegratorStatistics();
+ resetMethodStatistics();
+}
+
+//------------------------------------------------------------------------------
+// INITIALIZE
+//------------------------------------------------------------------------------
+
+void IntegratorRep::initialize(const State& initState) {
+ try
+ { invalidateIntegratorInternalState();
+
+ // Copy the supplied initial state into the integrator.
+ updAdvancedState() = initState;
+
+ // Freeze the number and kinds of state variables.
+ getSystem().realizeModel(updAdvancedState());
+
+ // Freeze problem dimensions; at this point the state represents an
+ // instance of a physically realizable system.
+ getSystem().realize(getAdvancedState(), Stage::Instance);
+
+ // Some Systems need to get control whenever time is advanced
+ // successfully (and irreversibly) so that they can do discrete updates.
+ systemHasTimeAdvancedEvents = getSystem().hasTimeAdvancedEvents();
+
+ // Allocate integrator-local data structures now that we know the sizes.
+ const int ny = getAdvancedState().getNY();
+ const int nc = getAdvancedState().getNYErr();
+ const int ne = getAdvancedState().getNEventTriggers();
+ timeScaleInUse = getSystem().getDefaultTimeScale();
+
+ // Set accuracy and consTol to their user-requested values or
+ // to the appropriate defaults.
+ setAccuracyAndTolerancesFromUserRequests();
+
+ // Now we can ask the System for information about its event triggers.
+ // (Event trigger info is Instance stage information.) Note that the event
+ // localization windows here are expressed in terms of the system timescale
+ // -- be sure to multiply by timescale before using.
+ getSystem().calcEventTriggerInfo(getAdvancedState(), eventTriggerInfo);
+
+ // Realize Time stage, apply prescribed motions, and attempt to project q's
+ // and u's onto the
+ // position and velocity constraint manifolds to drive constraint errors
+ // below accuracy*unitTolerance for each constraint. We'll allow project()
+ // to throw an exception if it fails since we can't recover from here.
+ // However, we won't set the LocalOnly option which means project() is
+ // allowed to thrash around wildly to attempt to find *some* solution.
+ // Also force repeated updates of iteration matrix to maximize chances of
+ // finding a solution; we're not in a hurry here.
+ realizeAndProjectKinematicsWithThrow(updAdvancedState(),
+ ProjectOptions::ForceProjection, ProjectOptions::ForceFullNewton);
+
+ // Inform any state-using System elements (especially Measures) that we
+ // are starting a simulation and give them a chance to initialize their own
+ // continuous (z) variables and discrete variables.
+
+ // Handler is allowed to throw an exception if it fails since we don't
+ // have a way to recover.
+ HandleEventsOptions handleOpts(getConstraintToleranceInUse());
+ HandleEventsResults results;
+ getSystem().handleEvents(updAdvancedState(),
+ Event::Cause::Initialization,
+ Array_<EventId>(),
+ handleOpts, results);
+ SimTK_ERRCHK_ALWAYS(
+ results.getExitStatus()!=HandleEventsResults::ShouldTerminate,
+ "Integrator::initialize()",
+ "An initialization event handler requested termination.");
+
+ // Now evaluate the state through the Acceleration stage to calculate
+ // the initial state derivatives.
+ realizeStateDerivatives(getAdvancedState());
+
+ // Now that we have valid update values for auto-update discrete variables,
+ // use them to reinitialize the discrete state variables. This one time
+ // only, the value swapped in from the discrete variable here is not yet
+ // suitable for use as an update value, during time integration since it
+ // has never been realized to Instance stage. So we'll force a
+ // re-evaluation.
+ updAdvancedState().autoUpdateDiscreteVariables();
+ getAdvancedState().invalidateAllCacheAtOrAbove(Stage::Instance);
+ // Re-realize to fill in the swapped-in update values.
+ realizeStateDerivatives(getAdvancedState());
+
+ // Record the continuous parts of this now-realized initial state as the
+ // previous state as well (previous state is used when we have to back up
+ // from a failed step attempt).
+ saveStateAndDerivsAsPrevious(getAdvancedState());
+
+ // The initial state is set so it looks like we just *completed* a step to
+ // get here. That way if the first reportTime is zero, this will get
+ // reported.
+ setStepCommunicationStatus(CompletedInternalStepNoEvent);
+ startOfContinuousInterval = true;
+
+ // This will be set to something meaningful when the simulation ends.
+ terminationReason = Integrator::InvalidTerminationReason;
+
+ // Call this virtual method in case the concrete integrator class has
+ // additional initialization needs. Note that it gets the State as we have
+ // adjusted it, NOT the one originally passed to initialize().
+ methodInitialize(getAdvancedState());
+
+ } catch (const std::exception& e) {
+ SimTK_THROW1(Integrator::InitializationFailed, e.what());
+ } /* catch (...) {
+ SimTK_THROW1(Integrator::InitializationFailed, "UNKNOWN EXCEPTION");
+ } */
+}
+
+void IntegratorRep::reinitialize(Stage stage, bool shouldTerminate) {
+ if (stage < Stage::Report) {
+ startOfContinuousInterval = true;
+ setUseInterpolatedState(false);
+ }
+ if (shouldTerminate) {
+ setStepCommunicationStatus(FinalTimeHasBeenReturned);
+ terminationReason = Integrator::EventHandlerRequestedTermination;
+ }
+ methodReinitialize(stage,shouldTerminate);
+}
+
+} // namespace SimTK
+
+
diff --git a/SimTKmath/Integrators/src/IntegratorRep.h b/SimTKmath/Integrators/src/IntegratorRep.h
new file mode 100644
index 0000000..e41ef2c
--- /dev/null
+++ b/SimTKmath/Integrators/src/IntegratorRep.h
@@ -0,0 +1,1133 @@
+#ifndef SimTK_SIMMATH_INTEGRATOR_REP_H_
+#define SimTK_SIMMATH_INTEGRATOR_REP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This is the declaration of the abstract IntegratorRep class which
+ * represents the implementation of the Integrator class, and DAESystemRep
+ * which implements the Integrator::DAESystem class.
+ */
+
+#include "SimTKcommon.h"
+#include "simmath/Integrator.h"
+
+#include <exception>
+#include <limits>
+#include <algorithm>
+#include <vector>
+
+namespace SimTK {
+
+ ///////////////////////////
+ // INTEGRATOR EXCEPTIONS //
+ ///////////////////////////
+
+class Integrator::InitializationFailed : public Exception::Base {
+public:
+ InitializationFailed(const char* fn, int ln, const char* msg) : Base(fn,ln) {
+ setMessage("Integrator initialization failed apparently because:\n"
+ + String(msg));
+ }
+};
+
+
+class Integrator::StepSizeTooSmall : public Exception::Base {
+public:
+ StepSizeTooSmall(const char* fn, int ln, Real t, Real h, Real hmin) : Base(fn,ln) {
+ setMessage("At time=" + String(t) +
+ " the integrator failed to take a step with step size "
+ + String(h) + " which was already at or below the minimum allowed size "
+ + String(hmin));
+ }
+};
+
+class Integrator::StepFailed : public Exception::Base {
+public:
+ StepFailed(const char* fn, int ln, Real t, const char* msg) : Base(fn,ln) {
+ setMessage("Integrator step failed at time " + String(t) + " apparently because:\n"
+ + String(msg));
+ }
+};
+
+class Integrator::CantAskForEventInfoWhenNoEventTriggered : public Exception::Base {
+public:
+ CantAskForEventInfoWhenNoEventTriggered
+ (const char* fn, int ln, const char* methodname, Real t) : Base(fn,ln)
+ {
+ setMessage("Method Integrator::" + String(methodname)
+ + "() was called at time " + String(t)
+ + " but no event had triggered.");
+ }
+};
+
+//
+// Specification of the stepTo() method below:
+//
+// On entry we expect advancedState to be valid. Time may have already
+// been advanced past the indicated reportTime in which case we'll
+// be able to return an interpolated result immediately. We will never
+// advance past scheduledEventTime or finalTime.
+//
+// The Integrator has a simple state machine to deal with the fact that the
+// computation is interruptable due to the need for returning control
+// to the caller (usually the time stepper) for various reasons. We enter
+// this computation either due to a call to the routine stepTo(), or from
+// within stepTo() after taking an internal step that didn't require a return.
+// Here's a sketch:
+//
+// advance (or report interpolated value)
+// ------
+// | ^
+// v |no
+// (TimeAdvanced)--return?->(StepReturned)--final?->(ReturnedFinalTime)
+// ^ |no
+// <-------advance-------------------<
+//
+// In words, we take a step, we report that step if necessary (returning
+// control), then take another step unless we just reported one at the
+// final time. The state transition table below gives more detail about
+// what exactly gets reported. "->Xyz" below means return control
+// to caller of stepTo() with status Xyz, "INTERP t" means "interpolate
+// back to time t".
+//
+// There are additional states below used to distinguish between triggered
+// event conditions (which take some special handling) and the others.
+// The only difference between state StepReturned and StepReturnedEvent
+// is that some event-info query methods are allowed in the latter state.
+//
+// Current State Condition Action Next State
+// ---------------- ---------------- -------------- ----------------
+// Uninitialized any throw error X
+// ReturnedFinalTime any throw error X
+//
+// StepReturned or t_adv>=t_final ->Done ReturnedFinalTime
+// StepReturnedEvent OTHERWISE ADVANCE TimeAdvanced[Event]
+//
+// TimeAdvancedEvent t_report<t_low INTERP t_report (unchanged)
+// ->Report
+//
+// OTHERWISE INTERP t_low
+// (event triggered) ->Triggered StepReturnedEvent
+//
+// TimeAdvanced t_report<t_adv INTERP t_report
+// ->Report (unchanged)
+//
+// else
+// t_adv==t_sched ->Scheduled StepReturned
+//
+// else
+// user wants control
+// at end of step ->EndOfStep StepReturned
+//
+// else
+// t_adv==t_report ->Report StepReturned
+//
+// else
+// t_adv>=t_final ->Final StepReturned
+//
+// else
+// hit step limit ->StepLimit StepReturned
+//
+// OTHERWISE ADVANCE TimeAdvanced[Event]
+// ---------------- ---------------- -------------- ----------------
+//
+// Notes:
+// * for the above conditions to be sufficient, the integrator must
+// ensure that the interior of the interval (t_low,t_high) to
+// which an event trigger has been localized DOES NOT contain
+// t_report, t_sched, or t_final. It is OK if t_report is exactly
+// t_low, and it is OK if t_report, t_sched, or t_final is exactly
+// t_high.
+// * the intent of this state diagram is to make sure that the
+// most significant end-of-step reason is reported, but that each
+// step is reported at most once. Note that when the return status
+// is "Done", the final trajectory point has *already* been reported.
+// Simultaneous occurrences must be anticipated and handled by the caller
+// (time stepper).
+// * note that an interpolated report doesn't count as reporting the
+// step since we haven't reached t_advanced. But the interpolation
+// to t_low does since t_advanced==t_high in that case and there
+// is nothing in between t_low and t_high. Also, a report that
+// coincides with the end of an internal step counts as a step report.
+
+class IntegratorRep {
+public:
+ IntegratorRep(Integrator* handle,
+ const System&);
+ virtual ~IntegratorRep() { }
+ // no default constructor, no copy or copy assign
+
+ // The System must be successfully realized to Stage::Instance before this
+ // call. At this point the integrator can query the System about the
+ // problem size and allocate appropriately-sized internal data structures.
+ void initialize(const State&);
+ // This is for use after return from an event handler.
+ void reinitialize(Stage, bool shouldTerminate);
+ // We also give the concrete integration method a chance to initialize
+ // itself after the generic initialization is done.
+ virtual void methodInitialize(const State&) { }
+ // This is for use after return from an event handler.
+ virtual void methodReinitialize(Stage stage, bool shouldTerminate) { }
+
+ // The integrator has already been initialized. Take as many internal steps
+ // as needed to get up to or past reportTime, but don't advance past the
+ // next scheduled event time or the simulation final time.
+ virtual Integrator::SuccessfulStepStatus
+ stepTo(Real reportTime, Real scheduledEventTime) = 0;
+
+ bool isSimulationOver() const {
+ return stepCommunicationStatus == FinalTimeHasBeenReturned;
+ }
+
+ // Get the reason the simulation ended. This should only be invoked if
+ // isSimulationOver() returns true.
+ Integrator::TerminationReason getTerminationReason() const {
+ assert (isSimulationOver());
+ return terminationReason;
+ }
+
+ // This represents the Integrator's finite state machine for dealing with
+ // communication of internal steps to the caller. After initialization, the
+ // computation will be in state CompletedInternalStepNoEvent, but with
+ // t_prev=t_advanced=t_initial. We will process this as though we had just
+ // taken a step so that various conditions will result in the first
+ // stepTo() call returning immediately. Note: event handling and subsequent
+ // partial reinitialization of the integrator MUST NOT change the
+ // integrator's communication state.
+ enum StepCommunicationStatus {
+ // Time has advanced from t_prev to t_advanced; no event triggered. We
+ // have not yet returned to the caller with time t_advanced, although
+ // we may have returned at interpolated report times
+ // t_prev < t_report < t_advanced.
+ CompletedInternalStepNoEvent,
+
+ // Time has advanced from t_prev to t_advanced with an event trigger
+ // localized to the interval (t_low,t_high] where t_high==t_advanced.
+ // We have not yet returned to the caller with time t_low, although we
+ // may have returned at interpolated report times
+ // t_prev < t_report < t_low.
+ CompletedInternalStepWithEvent,
+
+ // We have already returned control to the caller at t_advanced with
+ // the strongest stopping reason given; no more returns are allowed for
+ // this step.
+ StepHasBeenReturnedNoEvent,
+
+ // We have already returned control to the caller at t_low with
+ // "EventTriggered" as the stopping reason; no more returns are allowed
+ // for this step.
+ StepHasBeenReturnedWithEvent,
+
+ // Any call to stepTo() when the integrator is already in this state is
+ // a fatal error.
+ FinalTimeHasBeenReturned,
+
+ InvalidStepCommunicationStatus = -1
+ };
+
+ const State& getAdvancedState() const {return advancedState;}
+ Real getAdvancedTime() const {return advancedState.getTime();}
+
+ const State& getState() const
+ { return useInterpolatedState ? interpolatedState : advancedState; }
+ bool isStateInterpolated() const {return useInterpolatedState;}
+
+ Real getAccuracyInUse() const {return accuracyInUse;}
+ Real getConstraintToleranceInUse() const {return consTol;}
+ Real getTimeScaleInUse() const {return timeScaleInUse;}
+
+ // What was the size of the first successful step after the last initialize() call?
+ virtual Real getActualInitialStepSizeTaken() const = 0;
+
+ // What was the size of the most recent successful step?
+ virtual Real getPreviousStepSizeTaken() const = 0;
+
+ // What step size will be attempted first on the next step() call?
+ virtual Real getPredictedNextStepSize() const = 0;
+
+ virtual int getNumStepsAttempted() const = 0;
+ virtual int getNumStepsTaken() const = 0;
+ virtual int getNumErrorTestFailures() const = 0;
+ virtual int getNumConvergenceTestFailures() const = 0;
+ virtual int getNumConvergentIterations() const = 0;
+ virtual int getNumDivergentIterations() const = 0;
+ virtual int getNumIterations() const = 0;
+
+ virtual void resetMethodStatistics() {
+ }
+
+ // Cubic Hermite interpolation. See Hairer, et al. Solving ODEs I, 2nd rev.
+ // ed., pg 190. Given (t0,y0,y0'),(t1,y1,y1') with y0 and y1 at least 3rd
+ // order accurate, we can obtain a 3rd order accurate interpolation yt for
+ // a time t0 < t < t1 using Hermite interpolation. Let
+ // f0=y0', f1=y1', h=t1-t0, d=(t-t0)/h (0<=d<=1). Then the interpolating
+ // function is:
+ // u(d)=y(t0+dh)=(1-d)y0 + dy1 + d(d-1)[(1-2d)(y1-y0) + (d-1)hf0 + dhf1]
+ // Rearrange so we only have to through each array once:
+ // u(d)=cy0*y0 + cy1*y1 + cf0*f0 + cf1*f1
+ // cy0= 1 - d^2(3 - 2*d)
+ // cy1= d^2(3 - 2*d) = (1-cy0)
+ // cf0=d*(d-1)^2*h = h*d*(d-1) * (d-1)
+ // cf1=d^2*(d-1)*h = h*d*(d-1) * d
+ //
+ // Note: you can't get a 3rd order estimate of the derivative ft, only
+ // the state yt.
+ //
+ // It is OK if yt is the same object as y0 or y1.
+ static void interpolateOrder3
+ (const Real& t0, const Vector& y0, const Vector& f0,
+ const Real& t1, const Vector& y1, const Vector& f1,
+ const Real& t, Vector& yt) {
+ assert(t0 < t1);
+ assert(t0 <= t && t <= t1);
+ assert(f0.size()==y0.size() && y1.size()==y0.size()
+ && f1.size()==y0.size());
+
+ const Real h = t1-t0, d=(t-t0)/h;
+ const Real cy1 = d*d*(3-2*d), cy0 = 1-cy1;
+ const Real hdd1 = h*d*(d-1), cf1=hdd1*d, cf0=cf1-hdd1;
+
+ yt = cy0*y0 + cy1*y1 + cf0*f0 + cf1*f1; // + O(h^4)
+ }
+
+ // We have bracketed a zero crossing for some function f(t)
+ // between (tLow,fLow) and (tHigh,fHigh), not including
+ // the end points. That means tHigh > tLow, and
+ // sign(fLow) != sign(fHigh) (sign(x) returns -1, 0, 1).
+ // We want to estimate time tRoot with
+ // tLow<tRoot<tHigh such that f(tRoot) is zero.
+ // For a nicely behaved continuous function this is just
+ // the secant method:
+ // x = fHigh/(fHigh-fLow) (0 <= x <= 1)
+ // tRoot = tHigh - x*(tHigh-tLow)
+ // However, if the function appears to be discontinuous we'll
+ // simply bisect the interval (x==0.5 above). We decide it is
+ // discontinuous if either end point is exactly zero, which
+ // would occur with a boolean function, for example. Also, if
+ // the time interval is already at or below the smallest allowable
+ // localization window, we'll bisect.
+ //
+ // One further twist, taken from CVODES, is to allow the caller
+ // to provide a bias (> 0) which will bias our returned tRoot
+ // further into the lower (bias<1) or upper (bias>1) half-interval.
+ // If bias==1 this is the pure secant method. Bias has no effect
+ // on functions deemed to be discontinuous; we'll always bisect
+ // the interval in that case.
+ //
+ // Finally, we won't return tRoot very close to either end of
+ // the interval. Instead we define a buffer zone at either end
+ // of width 10% of the interval, and push tRoot away from the
+ // edges if it gets any closer. And in any case we'll require
+ // at least 1/2 minWindow from either end, even if 10% of the
+ // interval is smaller than that.
+ //
+ // Note that "minWindow" here is *not* the desired localization window
+ // based on user accuracy requirements, but the (typically much smaller)
+ // smallest allowable localization window based on numerical roundoff
+ // considerations.
+
+ static Real estimateRootTime(Real tLow, Real fLow, Real tHigh, Real fHigh,
+ Real bias, Real minWindow)
+ {
+ assert(tLow < tHigh);
+ assert(sign(fLow) != sign(fHigh));
+ assert(bias > 0);
+ assert(minWindow > 0);
+
+ const Real h = tHigh-tLow;
+
+ if (fLow==0 || fHigh==0 || h <= minWindow) {
+ // bisecting
+ return tLow + h/2;
+ }
+
+ // Use secant method.
+
+ const Real x = fHigh/(fHigh-bias*fLow);
+ Real tRoot = tHigh - x*h;
+
+ // If tRoot is too close to either end point we'll assume bad behavior
+ // and guess a value 10% of the interval away from the end.
+
+ const Real BufferZone = std::max(Real(0.1*h), minWindow/2);
+ tRoot = std::max(tRoot, tLow + BufferZone);
+ tRoot = std::min(tRoot, tHigh - BufferZone);
+
+ return tRoot;
+ }
+
+ // Here we look at a pair of event trigger function values across an
+ // interval and decide if there are any events triggering. If so we return
+ // that event as an "event candidate". Optionally, pass in the current list
+ // of event candidates and we'll only look at those (that is, the list can
+ // only be narrowed). Don't use the same array for the current list and new
+ // list.
+ //
+ // For purposes of this method, events are specified by their indices in
+ // the array of trigger functions, NOT by their event IDs.
+ void findEventCandidates
+ (int nEvents,
+ const Array_<SystemEventTriggerIndex>* viableCandidates,
+ const Array_<Event::Trigger>* viableCandidateTransitions,
+ Real tLow, const Vector& eLow,
+ Real tHigh, const Vector& eHigh,
+ Real bias, Real minWindow,
+ Array_<SystemEventTriggerIndex>& candidates,
+ Array_<Real>& timeEstimates,
+ Array_<Event::Trigger>& transitions,
+ Real& earliestTimeEst,
+ Real& narrowestWindow) const
+ {
+ int nCandidates;
+ if (viableCandidates) {
+ nCandidates = (int)viableCandidates->size();
+ assert(viableCandidateTransitions && (int)viableCandidateTransitions->size()==nCandidates);
+ } else {
+ assert(!viableCandidateTransitions);
+ nCandidates = nEvents;
+ }
+
+ candidates.clear();
+ timeEstimates.clear();
+ transitions.clear();
+ earliestTimeEst = narrowestWindow = Infinity;
+ for (int i=0; i<nCandidates; ++i) {
+ const SystemEventTriggerIndex e = viableCandidates ? (*viableCandidates)[i]
+ : SystemEventTriggerIndex(i);
+ Event::Trigger transitionSeen =
+ Event::maskTransition(
+ Event::classifyTransition(sign(eLow[e]), sign(eHigh[e])),
+ eventTriggerInfo[e].calcTransitionMask());
+
+ if (transitionSeen != Event::NoEventTrigger) {
+ // Replace the transition we just saw with the appropriate one for
+ // reporting purposes. For example, if the event trigger only wants
+ // negative-to-positive transitions but we just saw negative-to-zero
+ // we'll report that as negative-to-positive.
+ transitionSeen = eventTriggerInfo[e].calcTransitionToReport(transitionSeen);
+ candidates.push_back(e);
+ narrowestWindow = std::max(
+ std::min(narrowestWindow,
+ accuracyInUse*timeScaleInUse*eventTriggerInfo[e].getRequiredLocalizationTimeWindow()),
+ minWindow);
+
+ // Set estimated event trigger time for the viable candidates.
+ timeEstimates.push_back(estimateRootTime(tLow, eLow[e], tHigh, eHigh[e],
+ bias, minWindow));
+ transitions.push_back(transitionSeen);
+ earliestTimeEst = std::min(earliestTimeEst, timeEstimates.back());
+ }
+ }
+ }
+
+ /// Given a list of events, specified by their indices in the list of trigger functions,
+ /// convert them to the corresponding event IDs.
+ void findEventIds(const Array_<SystemEventTriggerIndex>& indices, Array_<EventId>& ids) {
+ for (int i = 0; i < (int)indices.size(); ++i)
+ ids.push_back(eventTriggerInfo[indices[i]].getEventId());
+ }
+
+ // Calculate the error norm using RMS or Inf norm, and report which y
+ // was dominant.
+ Real calcErrorNorm(const State& s, const Vector& yErrEst,
+ int& worstY) const {
+ const int nq=s.getNQ(), nu=s.getNU(), nz=s.getNZ();
+ int worstQ, worstU, worstZ;
+ Real qNorm, uNorm, zNorm, maxNorm;
+ if (userUseInfinityNorm == 1) {
+ qNorm = calcWeightedInfNormQ(s, s.getUWeights(), yErrEst(0,nq),
+ worstQ);
+ uNorm = calcWeightedInfNorm(getPreviousUScale(), yErrEst(nq,nu),
+ worstU);
+ zNorm = calcWeightedInfNorm(getPreviousZScale(), yErrEst(nq+nu,nz),
+ worstZ);
+ } else {
+ qNorm = calcWeightedRMSNormQ(s, s.getUWeights(), yErrEst(0,nq),
+ worstQ);
+ uNorm = calcWeightedRMSNorm(getPreviousUScale(), yErrEst(nq,nu),
+ worstU);
+ zNorm = calcWeightedRMSNorm(getPreviousZScale(), yErrEst(nq+nu,nz),
+ worstZ);
+ }
+
+ // Find the largest of the three norms and report the corresponding
+ // worst offender within q, u, or z.
+ if (qNorm >= uNorm) {
+ if (qNorm >= zNorm)
+ {maxNorm = qNorm; worstY = worstQ;} // q>=u && q>=z
+ else {maxNorm = zNorm; worstY = nq+nu+worstZ;} // z>q>=u
+ } else { // qNorm < uNorm
+ if (uNorm >= zNorm)
+ {maxNorm = uNorm; worstY = nq + worstU;} // u>q && u>=z
+ else {maxNorm = zNorm; worstY = nq+nu+worstZ;} // z>u>q
+ }
+
+ return maxNorm;
+ }
+
+
+ // Given a proposed absolute change dq to generalized coordinates q, scale
+ // it to produce fq, such that fq_i is the fraction of q_i's "unit change"
+ // represented by dq_i. A suitable norm of fq can then be compared directly
+ // with the relative accuracy requirement.
+ //
+ // Because q's are not independent of u's (qdot=N(q)*u), the "unit change"
+ // of q is related to the unit change of u. We want fq=Wq*dq, but we
+ // determine Wq from Wu via Wq = N*Wu*pinv(N). Wq is block diagonal while
+ // Wu is diagonal. State must be realized to Position stage.
+ void scaleDQ(const State& state, const Vector& Wu,
+ const Vector& dq, Vector& dqw) const // in/out
+ {
+ const System& system = getSystem();
+ const int nq = state.getNQ();
+ const int nu = state.getNU();
+ assert(dq.size() == nq);
+ assert(Wu.size() == nu);
+ dqw.resize(nq);
+ if (nq==0) return;
+ Vector du(nu);
+ system.multiplyByNPInv(state, dq, du);
+ du.rowScaleInPlace(Wu);
+ system.multiplyByN(state, du, dqw);
+ }
+ // Calculate |Wq*dq|_RMS=|N*Wu*pinv(N)*dq|_RMS
+ Real calcWeightedRMSNormQ(const State& state, const Vector& Wu,
+ const Vector& dq, int& worstQ) const
+ {
+ const int nq = state.getNQ();
+ Vector dqw(nq);
+ scaleDQ(state, Wu, dq, dqw);
+ return dqw.normRMS(&worstQ);
+ }
+ // Calculate |Wq*dq|_Inf=|N*Wu*pinv(N)*dq|_Inf
+ Real calcWeightedInfNormQ(const State& state, const Vector& Wu,
+ const Vector& dq, int& worstQ) const
+ {
+ const int nq = state.getNQ();
+ Vector dqw(nq);
+ scaleDQ(state, Wu, dq, dqw);
+ return dqw.normInf(&worstQ);
+ }
+
+ // TODO: these utilities don't really belong here
+ static Real calcWeightedRMSNorm(const Vector& weights, const Vector& values,
+ int& worstOne) {
+ return values.weightedNormRMS(weights, &worstOne);
+ }
+
+ static Real calcWeightedInfNorm(const Vector& weights, const Vector& values,
+ int& worstOne) {
+ return values.weightedNormInf(weights, &worstOne);
+ }
+
+ virtual const char* getMethodName() const = 0;
+ virtual int getMethodMinOrder() const = 0;
+ virtual int getMethodMaxOrder() const = 0;
+ virtual bool methodHasErrorControl() const = 0;
+
+protected:
+ const System& getSystem() const {return sys;}
+
+ // This is information we extract from the System during initialization or
+ // reinitialization and save here.
+ bool getDynamicSystemHasTimeAdvancedEvents() const {return systemHasTimeAdvancedEvents;}
+ Real getDynamicSystemTimescale() const {return timeScaleInUse;}
+ const Array_<EventTriggerInfo>&
+ getDynamicSystemEventTriggerInfo() const {return eventTriggerInfo;}
+
+ const State& getInterpolatedState() const {return interpolatedState;}
+ State& updInterpolatedState() {return interpolatedState;}
+
+ State& updAdvancedState() {return advancedState;}
+
+ void setAdvancedState(const Real& t, const Vector& y) {
+ advancedState.updY() = y;
+ advancedState.updTime() = t;
+ }
+
+ void setTriggeredEvents(Real tlo, Real thi,
+ const Array_<EventId>& eventIds,
+ const Array_<Real>& estEventTimes,
+ const Array_<Event::Trigger>& transitionsSeen)
+ {
+ assert(tPrev <= tlo && tlo < thi && thi <= advancedState.getTime());
+ tLow = tlo;
+ tHigh = thi;
+
+ const int n = eventIds.size();
+ assert(n > 0 && estEventTimes.size()==n && transitionsSeen.size()==n);
+ triggeredEvents.resize(n); estimatedEventTimes.resize(n); eventTransitionsSeen.resize(n);
+ Array_<int> eventOrder; // will be a permutation of 0:n-1
+ calcEventOrder(eventIds, estEventTimes, eventOrder);
+ for (int i=0; i<(int)eventOrder.size(); ++i) {
+ const int ipos = eventOrder[i];
+ triggeredEvents[i] = eventIds[ipos];
+
+ assert(tlo < estEventTimes[ipos] && estEventTimes[ipos] <= thi);
+ estimatedEventTimes[i] = estEventTimes[ipos];
+
+ // TODO: assert that the transition is one of the allowed ones for this event
+ eventTransitionsSeen[i] = transitionsSeen[ipos];
+ }
+ }
+
+ Real getEventWindowLow() const {return tLow;}
+ Real getEventWindowHigh() const {return tHigh;}
+
+ const Array_<EventId>& getTriggeredEvents() const {return triggeredEvents;}
+ const Array_<Real>& getEstimatedEventTimes() const {return estimatedEventTimes;}
+ const Array_<Event::Trigger>&
+ getEventTransitionsSeen() const {return eventTransitionsSeen;}
+
+ // This determines which state will be returned by getState().
+ void setUseInterpolatedState(bool shouldUse) {
+ useInterpolatedState = shouldUse;
+ }
+
+ void setStepCommunicationStatus(StepCommunicationStatus scs) {
+ stepCommunicationStatus = scs;
+ }
+
+ StepCommunicationStatus getStepCommunicationStatus() const {
+ return stepCommunicationStatus;
+ }
+
+ const Real& getPreviousTime() const {return tPrev;}
+
+ // q,u,z are views into y.
+ const Vector& getPreviousY() const {return yPrev;}
+ const Vector& getPreviousQ() const {return qPrev;}
+ const Vector& getPreviousU() const {return uPrev;}
+ const Vector& getPreviousZ() const {return zPrev;}
+
+ const Vector& getPreviousUScale() const {return uScalePrev;}
+ const Vector& getPreviousZScale() const {return zScalePrev;}
+
+ // qdot,udot,zdot are views into ydot.
+ const Vector& getPreviousYDot() const {return ydotPrev;}
+ const Vector& getPreviousQDot() const {return qdotPrev;}
+ const Vector& getPreviousUDot() const {return udotPrev;}
+ const Vector& getPreviousZDot() const {return zdotPrev;}
+
+ const Vector& getPreviousQDotDot() const {return qdotdotPrev;}
+
+ const Vector& getPreviousEventTriggers() const {return triggersPrev;}
+
+ Array_<EventTriggerInfo>& updEventTriggerInfo() {return eventTriggerInfo;}
+
+ // Given an array of state variables v (either u or z) and corresponding
+ // weights w (1/absolute scale), return relative scale min(wi,1/|vi|) for
+ // each variable i. That is, we choose the current value of vi as its
+ // scale when it is large enough, otherwise use absolute scale.
+ static void calcRelativeScaling(const Vector& v, const Vector& w,
+ Vector& vScale)
+ {
+ const int nv = v.size();
+ assert(w.size() == nv);
+ vScale.resize(nv);
+ for (int i=0; i<nv; ++i) {
+ const Real vi = std::abs(v[i]);
+ const Real wi = w[i];
+ vScale[i] = vi*wi > 1 ? 1/vi : wi;
+ }
+ }
+
+
+ // We're about to take a step. Record the current time and state as
+ // the previous ones. We calculate the scaling for
+ // u and z here which may include relative scaling based on their current
+ // values. This scaling is frozen during a step attempt.
+ void saveTimeAndStateAsPrevious(const State& s) {
+ const int nq = s.getNQ(), nu = s.getNU(), nz = s.getNZ();
+
+ tPrev = s.getTime();
+
+ yPrev = s.getY();
+ qPrev.viewAssign(yPrev(0, nq));
+ uPrev.viewAssign(yPrev(nq, nu));
+ zPrev.viewAssign(yPrev(nq+nu, nz));
+
+ calcRelativeScaling(s.getU(), s.getUWeights(), uScalePrev);
+ calcRelativeScaling(s.getZ(), s.getZWeights(), zScalePrev);
+ }
+
+ // We have evaluated state derivatives and event witness function values
+ // at the start of a new step; record
+ // as previous values to make restarts easy. Must have called
+ // saveTimeAndStateAsPrevious() on this same state already. State must
+ // be realized through Acceleration stage.
+ void saveStateDerivsAsPrevious(const State& s) {
+ const int nq = s.getNQ(), nu = s.getNU(), nz = s.getNZ();
+
+ ydotPrev = s.getYDot();
+ qdotPrev.viewAssign(ydotPrev(0, nq));
+ udotPrev.viewAssign(ydotPrev(nq, nu));
+ zdotPrev.viewAssign(ydotPrev(nq+nu, nz));
+
+ qdotdotPrev = s.getQDotDot();
+ triggersPrev = s.getEventTriggers();
+ }
+
+ // State must already have been evaluated through Stage::Acceleration
+ // or this will throw a stage violation.
+ void saveStateAndDerivsAsPrevious(const State& s) {
+ saveTimeAndStateAsPrevious(s);
+ saveStateDerivsAsPrevious(s);
+ }
+
+ // collect user requests
+ Real userInitStepSize, userMinStepSize, userMaxStepSize;
+ Real userAccuracy; // also use for constraintTol
+ Real userConsTol; // for fussy people
+ Real userFinalTime; // never go past this
+ int userInternalStepLimit; // that is, in a single call to step(); 0=no limit
+
+ // three-state booleans
+ int userUseInfinityNorm; // -1 (not supplied), 0(false), 1(true)
+ int userReturnEveryInternalStep; // "
+ int userProjectEveryStep; // "
+ int userAllowInterpolation; // "
+ int userProjectInterpolatedStates; // "
+ int userForceFullNewton; // "
+
+ // Mark all user-supplied options "not supplied by user".
+ void initializeUserStuff() {
+ userInitStepSize = userMinStepSize = userMaxStepSize = -1.;
+ userAccuracy = userConsTol = -1.;
+ userFinalTime = -1.;
+ userInternalStepLimit = -1;
+
+ // booleans
+ userUseInfinityNorm = userReturnEveryInternalStep =
+ userProjectEveryStep = userAllowInterpolation =
+ userProjectInterpolatedStates = userForceFullNewton = -1;
+
+ accuracyInUse = NaN;
+ consTol = NaN;
+ }
+
+ // Required accuracy and constraint tolerance.
+ // These are obtained during initialization from the user requests above, and
+ // given default values if the user was agnostic.
+
+ Real consTol; // fraction of the constraint unit tolerance to be applied to each constraint
+
+ void setAccuracyAndTolerancesFromUserRequests() {
+ accuracyInUse = (userAccuracy != -1 ? userAccuracy : Real(1e-3));
+ consTol = (userConsTol != -1 ? userConsTol : accuracyInUse/10);
+ }
+
+ // If this is set to true, the next call to stepTo() will return immediately
+ // with result code StartOfContinuousInterval. This is set by initialize()
+ // and by reinitialize() after an event handler call that modified the
+ // state.
+ bool startOfContinuousInterval;
+
+ // The reason the simulation ended. If isSimulationOver() returns false,
+ // the value of this field is meaningless.
+ Integrator::TerminationReason terminationReason;
+
+ // Realize the supplied state through Stage::Acceleration
+ // and bump statistics appropriately. Throws
+ // an exception if it fails, with failure statistics bumped.
+ void realizeStateDerivatives(const State& s) const {
+ if (s.getSystemStage() < Stage::Acceleration) {
+ ++statsRealizations; ++statsRealizationFailures;
+ getSystem().realize(s, Stage::Acceleration);
+ --statsRealizationFailures;
+ }
+ }
+
+ // State should have had its q's prescribed and realized through Position
+ // stage. This will attempt to project q's and the q part of the yErrEst
+ // (if yErrEst is not length zero). Returns false if we fail which you
+ // can consider a convergence failure for the step. This is intended for
+ // use during integration.
+ // Stats are properly updated. State is realized through Position stage
+ // on successful return.
+ bool localProjectQAndQErrEstNoThrow(State& s, Vector& yErrEst,
+ bool& anyChanges, Real projectionLimit=Infinity)
+ {
+ ProjectOptions options;
+ options.setRequiredAccuracy(getConstraintToleranceInUse());
+ options.setProjectionLimit(projectionLimit);
+ options.setOption(ProjectOptions::LocalOnly);
+ options.setOption(ProjectOptions::DontThrow);
+ if (userProjectEveryStep==1)
+ options.setOption(ProjectOptions::ForceProjection);
+ if (userUseInfinityNorm==1)
+ options.setOption(ProjectOptions::UseInfinityNorm);
+ if (userForceFullNewton==1)
+ options.setOption(ProjectOptions::ForceFullNewton);
+
+ anyChanges = false;
+ ProjectResults results;
+ // Nothing happens here if position constraints were already satisfied
+ // unless we set the ForceProjection option above.
+ if (yErrEst.size()) {
+ VectorView qErrEst = yErrEst(0, s.getNQ());
+ getSystem().projectQ(s, qErrEst, options, results);
+ } else {
+ getSystem().projectQ(s, yErrEst, options, results);
+ }
+ if (results.getExitStatus() != ProjectResults::Succeeded) {
+ ++statsQProjectionFailures;
+ return false;
+ }
+ anyChanges = results.getAnyChangeMade();
+ if (anyChanges)
+ ++statsQProjections;
+
+ return true;
+ }
+
+ // State should have had its q's and u's prescribed and realized through
+ // Velocity stage. This will attempt to project u's and the u part of the
+ // yErrEst (if yErrEst is not length zero). Returns false if we fail which
+ // you can consider a convergence failure for the step. This is intended for
+ // use during integration.
+ // Stats are properly updated. State is realized through Velocity stage
+ // on successful return.
+ bool localProjectUAndUErrEstNoThrow(State& s, Vector& yErrEst,
+ bool& anyChanges, Real projectionLimit=Infinity)
+ {
+ ProjectOptions options;
+ options.setRequiredAccuracy(getConstraintToleranceInUse());
+ options.setProjectionLimit(projectionLimit);
+ options.setOption(ProjectOptions::LocalOnly);
+ options.setOption(ProjectOptions::DontThrow);
+ if (userProjectEveryStep==1)
+ options.setOption(ProjectOptions::ForceProjection);
+ if (userUseInfinityNorm==1)
+ options.setOption(ProjectOptions::UseInfinityNorm);
+ if (userForceFullNewton==1)
+ options.setOption(ProjectOptions::ForceFullNewton);
+
+ anyChanges = false;
+ ProjectResults results;
+ // Nothing happens here if velocity constraints were already satisfied
+ // unless we set the ForceProjection option above.
+ if (yErrEst.size()) {
+ VectorView uErrEst = yErrEst(s.getNQ(), s.getNU());
+ getSystem().projectU(s, uErrEst, options, results);
+ } else {
+ getSystem().projectU(s, yErrEst, options, results);
+ }
+ if (results.getExitStatus() != ProjectResults::Succeeded) {
+ ++statsUProjectionFailures;
+ return false;
+ }
+ anyChanges = results.getAnyChangeMade();
+ if (anyChanges)
+ ++statsUProjections;
+
+ return true;
+ }
+
+ // Given a state which has just had t and y updated, realize it through
+ // velocity stage taking care of prescribed motion, projection, and
+ // throwing an exception if anything goes wrong. This is not for use
+ // during normal integration but good for initialization and generation
+ // of interpolated states.
+ // Extra options to consider are whether to restrict projection to the
+ // local neighborhood, and whether to unconditionally force projection.
+ // Stats are properly updated. State is realized through Velocity stage
+ // on return.
+ void realizeAndProjectKinematicsWithThrow(State& s,
+ ProjectOptions::Option xtraOption1=ProjectOptions::None,
+ ProjectOptions::Option xtraOption2=ProjectOptions::None,
+ ProjectOptions::Option xtraOption3=ProjectOptions::None)
+ {
+ const System& system = getSystem();
+ ProjectOptions options;
+ options.setRequiredAccuracy(getConstraintToleranceInUse());
+ options.setOption(xtraOption1);
+ options.setOption(xtraOption2);
+ options.setOption(xtraOption3);
+ if (userProjectEveryStep==1)
+ options.setOption(ProjectOptions::ForceProjection);
+ if (userUseInfinityNorm==1)
+ options.setOption(ProjectOptions::UseInfinityNorm);
+ if (userForceFullNewton==1)
+ options.setOption(ProjectOptions::ForceFullNewton);
+
+ system.realize(s, Stage::Time);
+ system.prescribeQ(s);
+ system.realize(s, Stage::Position);
+
+ Vector dummy; // no error estimate to project
+ ProjectResults results;
+ ++statsQProjectionFailures; // assume failure, then fix if no throw
+ system.projectQ(s, dummy, options, results);
+ --statsQProjectionFailures; // false alarm -- it succeeded
+ if (results.getAnyChangeMade())
+ ++statsQProjections;
+
+ system.prescribeU(s);
+ system.realize(s, Stage::Velocity);
+
+ results.clear();
+ ++statsUProjectionFailures; // assume failure, then fix if no throw
+ system.projectU(s, dummy, options, results);
+ --statsUProjectionFailures; // false alarm -- it succeeded
+ if (results.getAnyChangeMade())
+ ++statsUProjections;
+ }
+
+ // Set the advanced state and then evaluate state derivatives. Throws an
+ // exception if it fails. Updates stats.
+ void setAdvancedStateAndRealizeDerivatives(const Real& t, const Vector& y)
+ {
+ const System& system = getSystem();
+ State& advanced = updAdvancedState();
+
+ setAdvancedState(t,y);
+
+ system.realize(advanced, Stage::Time);
+ system.prescribeQ(advanced); // set q_p
+ system.realize(advanced, Stage::Position);
+ system.prescribeU(advanced); // set u_p
+
+ // Now realize Velocity, Dynamics, and Acceleration stages.
+ realizeStateDerivatives(getAdvancedState());
+ }
+
+ // Set the advanced state and then evaluate constraint errors. Throws an
+ // exception if it fails. Never counts as a realization because
+ // we only need to realize kinematics.
+ void setAdvancedStateAndRealizeKinematics(const Real& t, const Vector& y)
+ {
+ const System& system = getSystem();
+ State& advanced = updAdvancedState();
+
+ setAdvancedState(t,y);
+
+ system.realize(advanced, Stage::Time);
+ system.prescribeQ(advanced); // set q_p
+ system.realize(advanced, Stage::Position);
+ system.prescribeU(advanced); // set u_p
+
+ // Now realize remaining kinematics.
+ system.realize(advanced, Stage::Velocity);
+ }
+
+
+ void resetIntegratorStatistics() {
+ statsRealizations = 0;
+ statsQProjections = statsUProjections = 0;
+ statsRealizationFailures = 0;
+ statsQProjectionFailures = statsUProjectionFailures = 0;
+ }
+
+ int getNumRealizations() const {return statsRealizations;}
+ int getNumQProjections() const {return statsQProjections;}
+ int getNumUProjections() const {return statsUProjections;}
+
+ int getNumRealizationFailures() const {return statsRealizationFailures;}
+ int getNumQProjectionFailures() const {return statsQProjectionFailures;}
+ int getNumUProjectionFailures() const {return statsUProjectionFailures;}
+
+private:
+ class EventSorter {
+ public:
+ EventSorter() : ordinal(-1), id(-1), estTime(NaN) { }
+ EventSorter(int ord, int eventId, Real estEventTime)
+ : ordinal(ord), id(eventId), estTime(estEventTime) { }
+ bool operator<(const EventSorter& r) const {
+ if (estTime < r.estTime) return true;
+ if (estTime > r.estTime) return false;
+ return id < r.id;
+ }
+
+ int ordinal; // original position in the eventIds array
+ int id;
+ Real estTime;
+ };
+
+ void calcEventOrder(const Array_<EventId>& eventIds,
+ const Array_<Real>& estEventTimes,
+ Array_<int>& eventOrder)
+ {
+ const unsigned n = eventIds.size();
+ assert(estEventTimes.size()==n);
+ eventOrder.resize(n);
+ if (n==0)
+ return;
+
+ if (n==1) {
+ eventOrder[0] = 0;
+ return;
+ }
+
+ // otherwise sort
+ Array_<EventSorter> events(n);
+ for (unsigned i=0; i<n; ++i)
+ events[i] = EventSorter(i, eventIds[i], estEventTimes[i]);
+ std::sort(events.begin(), events.end());
+ for (unsigned i=0; i<n; ++i)
+ eventOrder[i] = events[i].ordinal;
+ }
+
+private:
+ Integrator* myHandle;
+ friend class Integrator;
+
+ const System& sys;
+
+
+protected:
+ // realization and projection stats are shared by all integrators;
+ // others are left to the individual integration methods
+ mutable int statsQProjectionFailures, statsUProjectionFailures;
+ mutable int statsQProjections, statsUProjections;
+ mutable int statsRealizations;
+ mutable int statsRealizationFailures;
+private:
+
+ // SYSTEM INFORMATION
+ // Information extracted from the System describing properties we need
+ // to know about BEFORE taking a time step. These properties are always
+ // frozen across an integration step, but may be updated as discrete
+ // updates during time stepping.
+
+ // Some Systems need to get control whenever time is advanced
+ // successfully (and irreversibly) so that they can do discrete updates.
+ // This is Stage::Model information.
+ bool systemHasTimeAdvancedEvents;
+
+
+ // Event trigger functions specify which sign transitions should be
+ // monitored: rising, falling, to/from zero, or combinations of those.
+ // They also provide a localization window width.
+ // This is Stage::Instance information.
+
+ Array_<EventTriggerInfo> eventTriggerInfo;
+
+ // A unitless fraction.
+ Real accuracyInUse;
+
+ // The time scale is an estimate as to what is the smallest length of time
+ // on which we can expect any significant changes to occur. This can be
+ // used to estimate an initial step size, and it also establishes the units
+ // in which event trigger localization windows are defined (that is, the
+ // window is given as some fraction of the time scale).
+ // This is Stage::Instance information.
+ Real timeScaleInUse;
+
+ // INTEGRATOR INTERNAL STATE
+ // Persists between stepTo() calls.
+ // A concrete integrator is free to have more state.
+
+ StepCommunicationStatus stepCommunicationStatus;
+
+ // We save both the step size we have to take next and the one
+ // we wish we could take. Once we're done with whatever's causing
+ // us to come up short (like event isolation), we may be able to
+ // jump right back up to the ideal one.
+ Real nextStepSizeToTry; // This is what we'll actually try first.
+ Real idealNextStepSize; // But if accuracy were the only concern
+ // we would try this (>=nextStepSizeToTry).
+
+ // This is how far the integrator has advanced the system
+ // trajectory. We might allow an interpolated state a
+ // little earlier than this, but otherwise there is no
+ // going back from this point.
+ State advancedState;
+
+ // When stepCommunicationStatus indicates that an event has
+ // triggered, the following arrays are valid. Events are sorted
+ // by estimated occurrence time within the window; identical-time
+ // events are sorted in ascending order of event Id.
+
+ // These are the events that the integrator has algorithmically
+ // determined are now triggered. You may not get the same result
+ // simply comparing the trigger function values at tLow and tHigh.
+ Array_<EventId> triggeredEvents;
+
+ // These are the estimated times corresponding to the triggeredEvents.
+ // They are in ascending order although there may be duplicates.
+ Array_<Real> estimatedEventTimes;
+
+ // Which transition was seen for each triggered event (this is
+ // only a single transition, not an OR-ed together set). This is
+ // the integrator's algorithmic determination of the transition to
+ // be reported -- you might not get the same answer just looking
+ // at the event trigger functions at tLow and tHigh.
+ Array_<Event::Trigger> eventTransitionsSeen;
+
+ // When we have successfully localized a triggering event into
+ // the time interval (tLow,tHigh] we record the bounds here.
+ Real tLow, tHigh;
+
+ State interpolatedState; // might be unused
+ bool useInterpolatedState;
+
+ // Use these to record the continuous part of the previous
+ // accepted state. We use these in combination with the
+ // continuous contents of advancedState to fill in the
+ // interpolated state.
+ Real tPrev;
+ Vector yPrev;
+
+ // These combine weightings from the Prev state with the possible
+ // requirement of relative accuracy using the current values of u and z.
+ // The result is min( wxi, 1/|xi|) for the relative ones where wxi is
+ // the weight (1/absolute scale) and xi is either ui or zi.
+ Vector uScalePrev;
+ Vector zScalePrev;
+
+ // Save continuous derivatives and event function values
+ // from previous accepted state also so we don't have to
+ // recalculate for restarts.
+ Vector ydotPrev;
+ Vector qdotdotPrev;
+ Vector triggersPrev;
+
+ // These are views into yPrev and ydotPrev.
+ Vector qPrev, uPrev, zPrev;
+ Vector qdotPrev, udotPrev, zdotPrev;
+
+ // We'll leave the various arrays above sized as they are and full
+ // of garbage. They'll be resized when first assigned to something
+ // meaningful.
+ void invalidateIntegratorInternalState() {
+ stepCommunicationStatus = InvalidStepCommunicationStatus;
+ nextStepSizeToTry = NaN;
+ idealNextStepSize = NaN;
+ tLow = tHigh = NaN;
+ useInterpolatedState = false;
+ tPrev = NaN;
+ }
+
+ // suppress
+ IntegratorRep(const IntegratorRep&);
+ IntegratorRep& operator=(const IntegratorRep&);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_INTEGRATOR_REP_H_
+
+
diff --git a/SimTKmath/Integrators/src/RungeKutta2Integrator.cpp b/SimTKmath/Integrators/src/RungeKutta2Integrator.cpp
new file mode 100644
index 0000000..0e50504
--- /dev/null
+++ b/SimTKmath/Integrators/src/RungeKutta2Integrator.cpp
@@ -0,0 +1,120 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This is the private (library side) implementation of the
+ * RungeKutta2Integrator and RungeKutta2IntegratorRep classes.
+ */
+
+#include "SimTKcommon.h"
+#include "simmath/Integrator.h"
+#include "simmath/RungeKutta2Integrator.h"
+
+#include "IntegratorRep.h"
+#include "RungeKutta2IntegratorRep.h"
+
+#include <exception>
+#include <limits>
+
+using namespace SimTK;
+
+//------------------------------------------------------------------------------
+// RUNGE KUTTA 2 INTEGRATOR
+//------------------------------------------------------------------------------
+
+RungeKutta2Integrator::RungeKutta2Integrator(const System& sys)
+{
+ rep = new RungeKutta2IntegratorRep(this, sys);
+}
+
+
+//------------------------------------------------------------------------------
+// RUNGE KUTTA 2 INTEGRATOR REP
+//------------------------------------------------------------------------------
+
+
+RungeKutta2IntegratorRep::RungeKutta2IntegratorRep
+ (Integrator* handle, const System& sys)
+: AbstractIntegratorRep(handle, sys, 2, 2, "RungeKutta2", true) {
+}
+
+// This is the explicit trapezoid rule, a Runge-Kutta 2(1) method. Here is
+// the Butcher diagram:
+//
+// 0|
+// 1| 1
+// --|--------------
+// 1| 1/2 1/2 2nd order propagated solution
+// --|--------------
+// | 0 1 0 1st order Euler for error estimate
+//
+//
+// This is a 2-stage, first-same-as-last (FSAL) 2nd order method which
+// gives us an embedded 1st order method as well, so we can extract
+// a 2nd-order error estimate for the 1st-order result, which error
+// estimate can then be used for step size control, since it will
+// behave as h^2. We then propagate the 2nd order result (whose error
+// is unknown), which Hairer calls "local extrapolation".
+// We call the initial state (t0,y0) and want (t0+h,y1). We are
+// given the initial derivative f0=f(t0,y0), which most likely
+// is left over from an evaluation at the end of the last step.
+
+bool RungeKutta2IntegratorRep::attemptODEStep
+ (Real t1, Vector& y1err, int& errOrder, int& numIterations)
+{
+ const Real t0 = getPreviousTime();
+ assert(t1 > t0);
+
+ statsStepsAttempted++;
+ errOrder = 2;
+ const Vector& y0 = getPreviousY();
+ const Vector& f0 = getPreviousYDot();
+ if (ytmp[0].size() != y0.size())
+ for (int i=0; i<NTemps; ++i)
+ ytmp[i].resize(y0.size());
+ Vector& f1 = ytmp[0]; // rename temp
+
+ const Real h = t1-t0;
+
+ // First stage f1 = f(t1, y0+h*f0)
+ setAdvancedStateAndRealizeDerivatives(t1, y0 + h*f0);
+ f1 = getAdvancedState().getYDot();
+
+ // Final value. This is the 2nd order accurate estimate for
+ // y1=y(t0+h)+O(h^3): y1 = y0 + (h/2)*(f0 + f1).
+ // Evaluate through kinematics only; it is a waste of a stage to
+ // evaluate derivatives here since the caller will muck with this before
+ // the end of the step.
+ setAdvancedStateAndRealizeKinematics(t1, y0 + (h/2)*(f0 + f1));
+ // YErr is valid now
+
+ // This is an embedded 1st-order estimate y1hat=y(t1)+O(h^2), with
+ // y1hat = y0 + h*f1 (note that that is the the end-of-step
+ // derivative). We just need the error y1-y1hat.
+
+ const Vector& y1 = getAdvancedState().getY();
+ for (int i=0; i<y1.size(); ++i)
+ y1err[i] = std::abs(y1[i]-(y0[i] + h*f1[i]));
+
+ return true;
+}
diff --git a/SimTKmath/Integrators/src/RungeKutta2IntegratorRep.h b/SimTKmath/Integrators/src/RungeKutta2IntegratorRep.h
new file mode 100644
index 0000000..5365588
--- /dev/null
+++ b/SimTKmath/Integrators/src/RungeKutta2IntegratorRep.h
@@ -0,0 +1,52 @@
+#ifndef SimTK_SIMMATH_RUNGE_KUTTA_2_INTEGRATOR_REP_H_
+#define SimTK_SIMMATH_RUNGE_KUTTA_2_INTEGRATOR_REP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "AbstractIntegratorRep.h"
+
+namespace SimTK {
+
+/**
+ * This is the private (library side) implementation of the
+ * RungeKutta2IntegratorRep class which is a concrete class
+ * implementing the abstract IntegratorRep.
+ */
+
+class RungeKutta2IntegratorRep : public AbstractIntegratorRep {
+public:
+ RungeKutta2IntegratorRep(Integrator* handle, const System& sys);
+protected:
+ bool attemptODEStep
+ (Real t1, Vector& yErrEst, int& errOrder, int& numIterations);
+private:
+ static const int NTemps = 1;
+ Vector ytmp[NTemps];
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_RUNGE_KUTTA_2_INTEGRATOR_REP_H_
+
+
diff --git a/SimTKmath/Integrators/src/RungeKutta3Integrator.cpp b/SimTKmath/Integrators/src/RungeKutta3Integrator.cpp
new file mode 100644
index 0000000..507d670
--- /dev/null
+++ b/SimTKmath/Integrators/src/RungeKutta3Integrator.cpp
@@ -0,0 +1,125 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This is the private (library side) implementation of the
+ * RungeKutta3Integrator and RungeKutta3IntegratorRep classes.
+ */
+
+#include "SimTKcommon.h"
+#include "simmath/Integrator.h"
+#include "simmath/RungeKutta3Integrator.h"
+
+#include "IntegratorRep.h"
+#include "RungeKutta3IntegratorRep.h"
+
+#include <exception>
+#include <limits>
+
+using namespace SimTK;
+
+//------------------------------------------------------------------------------
+// RUNGE KUTTA 3 INTEGRATOR
+//------------------------------------------------------------------------------
+
+RungeKutta3Integrator::RungeKutta3Integrator(const System& sys)
+{
+ rep = new RungeKutta3IntegratorRep(this, sys);
+}
+
+//------------------------------------------------------------------------------
+// RUNGE KUTTA 3 INTEGRATOR REP
+//------------------------------------------------------------------------------
+
+
+RungeKutta3IntegratorRep::RungeKutta3IntegratorRep
+ (Integrator* handle, const System& sys)
+: AbstractIntegratorRep(handle, sys, 3, 3, "RungeKutta3", true) {
+}
+
+// For a discussion of this Runge-Kutta 3(2) method, see J.C. Butcher, "The
+// Numerical Analysis of Ordinary Differential Equations", John Wiley & Sons,
+// 1987, page 325. The embedded error estimate was derived using the method
+// mentioned in Hairer, Norsett & Wanner, Solving ODEs I, 2nd rev. ed. on
+// page 166. This is the Butcher diagram:
+//
+// 0|
+// 1/2| 1/2
+// 1| -1 2
+// --|-------------------
+// 1| 1/6 2/3 1/6 3rd order propagated solution
+// --|-------------------
+// 1| 0 1 0 0 2nd order midpoint for error estimate
+//
+// This is a 3-stage, first-same-as-last (FSAL) 3rd order method which
+// gives us an embedded 2nd order method as well, so we can extract
+// a 3rd-order error estimate for the 2nd-order result, which error
+// estimate can then be used for step size control, since it will
+// behave as h^3. We then propagate the 3rd order result (whose error
+// is unknown), which Hairer calls "local extrapolation".
+// We call the initial state (t0,y0) and want (t0+h,y1). We are
+// given the initial derivative f0=f(t0,y0), which most likely
+// is left over from an evaluation at the end of the last step.
+
+bool RungeKutta3IntegratorRep::attemptODEStep
+ (Real t1, Vector& y1err, int& errOrder, int& numIterations)
+{
+ const Real t0 = getPreviousTime();
+ assert(t1 > t0);
+
+ statsStepsAttempted++;
+ errOrder = 3;
+ const Vector& y0 = getPreviousY();
+ const Vector& f0 = getPreviousYDot();
+ if (ytmp[0].size() != y0.size())
+ for (int i=0; i<NTemps; ++i)
+ ytmp[i].resize(y0.size());
+ Vector& f1 = ytmp[0]; // rename temps
+ Vector& f2 = ytmp[1];
+
+ const Real h = t1-t0;
+
+ setAdvancedStateAndRealizeDerivatives(t0+h/2, y0 + (h/2)*f0);
+ f1 = getAdvancedState().getYDot();
+
+ setAdvancedStateAndRealizeDerivatives(t1, y0 + h*(2*f1-f0));
+ f2 = getAdvancedState().getYDot();
+
+ // Final value. This is the 3rd order accurate estimate for
+ // y1=y(t0+h)+O(h^4): y1 = y0 + (h/6)*(f0 + 4 f1 + f2).
+ // Evaluate through kinematics only; it is a waste of a stage to
+ // evaluate derivatives here since the caller will muck with this before
+ // the end of the step.
+ setAdvancedStateAndRealizeKinematics(t1, y0 + (h/6)*(f0 + 4*f1 + f2));
+ // YErr is valid now
+
+ // This is an embedded 2nd-order estimate y1hat=y(t1)+O(h^3), with
+ // y1hat = y0 + h*f1 (explicit midpoint method). We just need the
+ // error y1-y1hat.
+
+ const Vector& y1 = getAdvancedState().getY();
+ for (int i=0; i<y1.size(); ++i)
+ y1err[i] = std::abs(y1[i]-(y0[i] + h*f1[i]));
+
+ return true;
+}
diff --git a/SimTKmath/Integrators/src/RungeKutta3IntegratorRep.h b/SimTKmath/Integrators/src/RungeKutta3IntegratorRep.h
new file mode 100644
index 0000000..1b54553
--- /dev/null
+++ b/SimTKmath/Integrators/src/RungeKutta3IntegratorRep.h
@@ -0,0 +1,52 @@
+#ifndef SimTK_SIMMATH_RUNGE_KUTTA_3_INTEGRATOR_REP_H_
+#define SimTK_SIMMATH_RUNGE_KUTTA_3_INTEGRATOR_REP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "AbstractIntegratorRep.h"
+
+namespace SimTK {
+
+/**
+ * This is the private (library side) implementation of the
+ * RungeKutta3IntegratorRep class which is a concrete class
+ * implementing the abstract IntegratorRep.
+ */
+
+class RungeKutta3IntegratorRep : public AbstractIntegratorRep {
+public:
+ RungeKutta3IntegratorRep(Integrator* handle, const System& sys);
+protected:
+ bool attemptODEStep
+ (Real t1, Vector& yErrEst, int& errOrder, int& numIterations);
+private:
+ static const int NTemps = 2;
+ Vector ytmp[NTemps];
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_RUNGE_KUTTA_3_INTEGRATOR_REP_H_
+
+
diff --git a/SimTKmath/Integrators/src/RungeKuttaFeldbergIntegrator.cpp b/SimTKmath/Integrators/src/RungeKuttaFeldbergIntegrator.cpp
new file mode 100644
index 0000000..815240f
--- /dev/null
+++ b/SimTKmath/Integrators/src/RungeKuttaFeldbergIntegrator.cpp
@@ -0,0 +1,155 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This is the private (library side) implementation of the
+ * RungeKuttaFeldbergIntegrator and RungeKuttaFeldbergIntegratorRep classes.
+ */
+
+#include "SimTKcommon.h"
+#include "simmath/Integrator.h"
+#include "simmath/RungeKuttaFeldbergIntegrator.h"
+
+#include "IntegratorRep.h"
+#include "RungeKuttaFeldbergIntegratorRep.h"
+
+#include <exception>
+#include <limits>
+
+using namespace SimTK;
+
+//------------------------------------------------------------------------------
+// RUNGE KUTTA FELDBERG INTEGRATOR
+//------------------------------------------------------------------------------
+
+RungeKuttaFeldbergIntegrator::RungeKuttaFeldbergIntegrator(const System& sys)
+{
+ rep = new RungeKuttaFeldbergIntegratorRep(this, sys);
+}
+
+
+//------------------------------------------------------------------------------
+// RUNGE KUTTA FELDBERG INTEGRATOR REP
+//------------------------------------------------------------------------------
+
+RungeKuttaFeldbergIntegratorRep::RungeKuttaFeldbergIntegratorRep
+ (Integrator* handle, const System& sys)
+: AbstractIntegratorRep(handle, sys, 5, 5, "RungeKuttaFeldberg", true) {}
+
+bool RungeKuttaFeldbergIntegratorRep::attemptODEStep
+ (Real t1, Vector& y1err, int& errOrder, int& numIterations)
+{
+ const Real C21 = Real( 1.0/4.0);
+ const Real C22 = Real( 1.0/4.0);
+
+ const Real C31 = Real( 3.0/8.0);
+ const Real C32 = Real( 3.0/32.0);
+ const Real C33 = Real( 9.0/32.0);
+
+ const Real C41 = Real( 12.0/13.0);
+ const Real C42 = Real( 1932.0/2197.0);
+ const Real C43 = Real(-7200.0/2197.0);
+ const Real C44 = Real( 7296.0/2197.0);
+
+ const Real C51 = Real( 1.0);
+ const Real C52 = Real( 439.0/216.0);
+ const Real C53 = Real(-8.0);
+ const Real C54 = Real( 3680.0/513.0);
+ const Real C55 = Real(-845.0/4104.0);
+
+ const Real C61 = Real( 1.0/2.0);
+ const Real C62 = Real(-8.0/27.0);
+ const Real C63 = Real( 2.0);
+ const Real C64 = Real(-3544.0/2565.0);
+ const Real C65 = Real( 1859.0/4104.0);
+ const Real C66 = Real(-11.0/40.0);
+
+ const double dCY1 = 25.0/216.0;
+ const double dCY2 = 1408.0/2565.0;
+ const double dCY3 = 2197.0/4104.0;
+ const double dCY4 = -1.0/5.0;
+
+ const Real CY1 = Real(dCY1);
+ const Real CY2 = Real(dCY2);
+ const Real CY3 = Real(dCY3);
+ const Real CY4 = Real(dCY4);
+
+ const Real CE1 = Real( 16.0/135.0-dCY1);
+ const Real CE2 = Real( 6656.0/12825.0-dCY2);
+ const Real CE3 = Real( 28561.0/56430.0-dCY3);
+ const Real CE4 = Real(-9.0/50.0-dCY4);
+ const Real CE5 = Real( 2.0/55.0);
+
+ const Real t0 = getPreviousTime();
+ assert(t1 > t0);
+
+ statsStepsAttempted++;
+ errOrder = 4;
+ const Vector& y0 = getPreviousY();
+ const Vector& f0 = getPreviousYDot();
+ if (ytmp[0].size() != y0.size())
+ for (int i=0; i<NTemps; ++i)
+ ytmp[i].resize(y0.size());
+ Vector& ysave = ytmp[0]; // rename temps
+ Vector& fa = ytmp[1];
+ Vector& fb = ytmp[2];
+
+ const Real h = t1-t0;
+
+ // Calculate the intermediate states.
+
+ setAdvancedStateAndRealizeDerivatives(t0 + h*C21,
+ y0 + h*C22*f0);
+ ytmp[0] = getAdvancedState().getYDot();
+
+ setAdvancedStateAndRealizeDerivatives(t0 + h*C31,
+ y0 + h*C32*f0 + h*C33*ytmp[0]);
+ ytmp[1] = getAdvancedState().getYDot();
+
+ setAdvancedStateAndRealizeDerivatives(t0 + h*C41,
+ y0 + h*C42*f0 + h*C43*ytmp[0] + h*C44*ytmp[1]);
+ ytmp[2] = getAdvancedState().getYDot();
+
+ setAdvancedStateAndRealizeDerivatives(t0 + h*C51,
+ y0 + h*C52*f0 + h*C53*ytmp[0] + h*C54*ytmp[1] + h*C55*ytmp[2]);
+ ytmp[3] = getAdvancedState().getYDot();
+
+ setAdvancedStateAndRealizeDerivatives(t0 + h*C61,
+ y0 + h*C62*f0 + h*C63*ytmp[0] + h*C64*ytmp[1] + h*C65*ytmp[2]
+ + h*C66*ytmp[3]);
+ ytmp[4] = getAdvancedState().getYDot();
+
+ // Calculate the final state but don't evaluate the derivatives. That
+ // would be a wasted stage since the caller will muck with the state before
+ // the end of the step.
+ setAdvancedStateAndRealizeKinematics(t1,
+ y0 + h*CY1*f0 + h*CY2*ytmp[1] + h*CY3*ytmp[2] + h*CY4*ytmp[3]);
+ // YErr is valid now, but not YDot.
+
+ // Calculate the error estimate.
+ y1err = h*CE1*f0 + h*CE2*ytmp[1] + h*CE3*ytmp[2] + h*CE4*ytmp[3]
+ + h*CE5*ytmp[4];
+
+ return true;
+}
+
diff --git a/SimTKmath/Integrators/src/RungeKuttaFeldbergIntegratorRep.h b/SimTKmath/Integrators/src/RungeKuttaFeldbergIntegratorRep.h
new file mode 100644
index 0000000..84b2d06
--- /dev/null
+++ b/SimTKmath/Integrators/src/RungeKuttaFeldbergIntegratorRep.h
@@ -0,0 +1,52 @@
+#ifndef SimTK_SIMMATH_RUNGE_KUTTA_FELDBERG_INTEGRATOR_REP_H_
+#define SimTK_SIMMATH_RUNGE_KUTTA_FELDBERG_INTEGRATOR_REP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "AbstractIntegratorRep.h"
+
+namespace SimTK {
+
+/**
+ * This is the private (library side) implementation of the
+ * RungeKuttaFeldbergIntegratorRep class which is a concrete class
+ * implementing the abstract IntegratorRep.
+ */
+
+class RungeKuttaFeldbergIntegratorRep : public AbstractIntegratorRep {
+public:
+ RungeKuttaFeldbergIntegratorRep(Integrator* handle, const System& sys);
+protected:
+ bool attemptODEStep
+ (Real t1, Vector& yErrEst, int& errOrder, int& numIterations);
+private:
+ static const int NTemps = 5;
+ Vector ytmp[NTemps];
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_RUNGE_KUTTA_FELDBERG_INTEGRATOR_REP_H_
+
+
diff --git a/SimTKmath/Integrators/src/RungeKuttaMersonIntegrator.cpp b/SimTKmath/Integrators/src/RungeKuttaMersonIntegrator.cpp
new file mode 100644
index 0000000..2aa5f83
--- /dev/null
+++ b/SimTKmath/Integrators/src/RungeKuttaMersonIntegrator.cpp
@@ -0,0 +1,141 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This is the private (library side) implementation of the
+ * RungeKuttaMersonIntegrator and RungeKuttaMersonIntegratorRep class which
+ * is a concrete class implementing the abstract IntegratorRep.
+ */
+
+#include "SimTKcommon.h"
+#include "simmath/Integrator.h"
+#include "simmath/RungeKuttaMersonIntegrator.h"
+
+#include "IntegratorRep.h"
+#include "RungeKuttaMersonIntegratorRep.h"
+
+#include <exception>
+#include <limits>
+
+using namespace SimTK;
+
+//------------------------------------------------------------------------------
+// RUNGE KUTTA MERSON INTEGRATOR
+//------------------------------------------------------------------------------
+
+RungeKuttaMersonIntegrator::RungeKuttaMersonIntegrator(const System& sys)
+{
+ rep = new RungeKuttaMersonIntegratorRep(this, sys);
+}
+
+
+//------------------------------------------------------------------------------
+// RUNGE KUTTA MERSON INTEGRATOR REP
+//------------------------------------------------------------------------------
+
+RungeKuttaMersonIntegratorRep::RungeKuttaMersonIntegratorRep
+ (Integrator* handle, const System& sys)
+: AbstractIntegratorRep(handle, sys, 4, 4, "RungeKuttaMerson", true) {
+}
+
+// For a discussion of the Runge-Kutta-Merson method, see Hairer,
+// Norsett & Wanner, Solving ODEs I, 2nd rev. ed. pp. 166-8, and table 4.1
+// on page 167. This is the Butcher diagram:
+//
+// 0|
+// 1/3| 1/3
+// 1/3| 1/6 1/6
+// 1/2| 1/8 0 3/8
+// 1| 1/2 0 -3/2 2
+// --|----------------------------
+// 1| 1/6 0 0 2/3 1/6 propagated 4th order solution
+// --|----------------------------
+// 1| 1/10 0 3/10 2/5 1/5 0 embedded 3rd order solution
+//
+// This is a 5-stage, first-same-as-last (FSAL) 4th order method which gives
+// us an embedded 3rd order method as well, so we can extract a 4th-order
+// error estimate for the 3rd-order result, which error estimate can then be
+// used for step size control, since it will behave as h^4. We then propagate
+// the 4th order result (whose error is unknown), which Hairer calls "local
+// extrapolation". We call the initial state (t0,y0) and want (t0+h,y1). We
+// are given the initial derivative f0=f(t0,y0), which most likely is left
+// over from an evaluation at the end of the last step.
+//
+// We will call the derivatives at stage f1,f2,f3,f4 but these are done with
+// only two temporaries fa and fb. (What we're calling "f" Hairer calls "k".)
+bool RungeKuttaMersonIntegratorRep::attemptODEStep
+ (Real t1, Vector& y1err, int& errOrder, int& numIterations)
+{
+ const Real t0 = getPreviousTime();
+ assert(t1 > t0);
+
+ statsStepsAttempted++;
+ errOrder = 4;
+ const Vector& y0 = getPreviousY();
+ const Vector& f0 = getPreviousYDot();
+ if (ytmp[0].size() != y0.size())
+ for (int i=0; i<NTemps; ++i)
+ ytmp[i].resize(y0.size());
+ Vector& ysave = ytmp[0]; // rename temps
+ Vector& fa = ytmp[1];
+ Vector& fb = ytmp[2];
+
+ const Real h = t1-t0;
+
+ setAdvancedStateAndRealizeDerivatives(t0+h/3, y0 + (h/3)*f0);
+ fa = getAdvancedState().getYDot(); // fa=f1
+
+ setAdvancedStateAndRealizeDerivatives(t0+h/3, y0 + (h/6)*(f0+fa)); // f0+f1
+ fa = getAdvancedState().getYDot(); // fa=f2
+
+ setAdvancedStateAndRealizeDerivatives(t0+h/2, y0 + (h/8)*(f0 + 3*fa)); // f0+3f2
+ fb = getAdvancedState().getYDot(); // fb=f3
+
+ // We'll need this for error estimation.
+ ysave = y0 + (h/2)*(f0 - 3*fa + 4*fb); // f0-3f2+4f3
+ setAdvancedStateAndRealizeDerivatives(t1, ysave);
+ fa = getAdvancedState().getYDot(); // fa=f4
+
+ // Final value. This is the 4th order accurate estimate for
+ // y1=y(t0+h)+O(h^5): y1 = y0 + (h/6)*(f0 + 4 f3 + f4).
+ // Evaluate through kinematics only; it is a waste of a stage to
+ // evaluate derivatives here since the caller will muck with this before
+ // the end of the step.
+ setAdvancedStateAndRealizeKinematics(t1, y0 + (h/6)*(f0 + 4*fb + fa));
+ // YErr is valid now
+
+ // This is an embedded 3rd-order estimate y1hat=y(t0+h)+O(h^4). (Apparently
+ // Merson thought it was 5th order, but that is only true if
+ // the function is linear w/constant coefficients; not bloody likely!)
+ // y1hat = y0 + (h/10)*(f0 + 3 f2 + 4 f3 + 2 f4)
+ //
+ // We don't actually have any need for y1hat, just its 4th-order
+ // error estimate y1hat-y1=(1/5)(y1-ysave) (easily verified from the above).
+
+ const Vector& y1 = getAdvancedState().getY();
+ for (int i=0; i<y1.size(); ++i)
+ y1err[i] = Real(.2)*std::abs(y1[i]-ysave[i]);
+
+ return true;
+}
+
diff --git a/SimTKmath/Integrators/src/RungeKuttaMersonIntegratorRep.h b/SimTKmath/Integrators/src/RungeKuttaMersonIntegratorRep.h
new file mode 100644
index 0000000..33aaff6
--- /dev/null
+++ b/SimTKmath/Integrators/src/RungeKuttaMersonIntegratorRep.h
@@ -0,0 +1,52 @@
+#ifndef SimTK_SIMMATH_RUNGE_KUTTA_MERSON_INTEGRATOR_REP_H_
+#define SimTK_SIMMATH_RUNGE_KUTTA_MERSON_INTEGRATOR_REP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "AbstractIntegratorRep.h"
+
+namespace SimTK {
+
+/**
+ * This is the private (library side) implementation of the
+ * RungeKuttaMersonIntegratorRep class which is a concrete class
+ * implementing the abstract IntegratorRep.
+ */
+
+class RungeKuttaMersonIntegratorRep : public AbstractIntegratorRep {
+public:
+ RungeKuttaMersonIntegratorRep(Integrator* handle, const System& sys);
+protected:
+ bool attemptODEStep
+ (Real t1, Vector& yErrEst, int& errOrder, int& numIterations);
+private:
+ static const int NTemps = 3;
+ Vector ytmp[NTemps];
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_RUNGE_KUTTA_MERSON_INTEGRATOR_REP_H_
+
+
diff --git a/SimTKmath/Integrators/src/SemiExplicitEuler2Integrator.cpp b/SimTKmath/Integrators/src/SemiExplicitEuler2Integrator.cpp
new file mode 100644
index 0000000..77f44fb
--- /dev/null
+++ b/SimTKmath/Integrators/src/SemiExplicitEuler2Integrator.cpp
@@ -0,0 +1,263 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "simmath/SemiExplicitEuler2Integrator.h"
+
+#include "IntegratorRep.h"
+#include "SemiExplicitEuler2IntegratorRep.h"
+
+using namespace SimTK;
+
+//==============================================================================
+// SEMI EXPLICIT EULER 2 INTEGRATOR
+//==============================================================================
+
+SemiExplicitEuler2Integrator::SemiExplicitEuler2Integrator
+ (const System& sys) {
+ rep = new SemiExplicitEuler2IntegratorRep(this, sys);
+}
+
+
+//==============================================================================
+// SEMI EXPLICIT EULER 2 INTEGRATOR REP
+//==============================================================================
+SemiExplicitEuler2IntegratorRep::SemiExplicitEuler2IntegratorRep
+ (Integrator* handle, const System& sys)
+: AbstractIntegratorRep(handle, sys, 1, 1, "SemiExplicitEuler2", true)
+{
+}
+
+//==============================================================================
+// CREATE INTERPOLATED STATE
+//==============================================================================
+// Create an interpolated state at time t, which is between tPrev and tCurrent.
+// If we haven't yet delivered an interpolated state in this interval, we have
+// to initialize its discrete part from the advanced state.
+//
+// TODO: Note that this is a first-order interpolation across the *whole* step,
+// even though this integrator takes two smaller first-order substeps. It would
+// be better to record the midstep values and perform a piecewise-linear
+// interpolation across the half steps so that the interpolated values match
+// the underlying steps. Alternately, the midpoint value could be used to
+// perform a second-order interpolation here; I'm not sure whether that would
+// be better.
+void SemiExplicitEuler2IntegratorRep::createInterpolatedState(Real t) {
+ const System& system = getSystem();
+ const State& advanced = getAdvancedState();
+ State& interp = updInterpolatedState();
+ interp = advanced; // pick up discrete stuff.
+ const Real weight1 = (getAdvancedTime()-t) /
+ (getAdvancedTime()-getPreviousTime());
+ const Real weight2 = 1-weight1;
+ interp.updY() = weight1*getPreviousY()+weight2*getAdvancedState().getY();
+ interp.updTime() = t;
+
+ if (userProjectInterpolatedStates == 0) {
+ system.realize(interp, Stage::Time);
+ system.prescribeQ(interp);
+ system.realize(interp, Stage::Position);
+ system.prescribeU(interp);
+ system.realize(interp, Stage::Velocity);
+ return;
+ }
+
+ // We may need to project onto constraint manifold. Allow project()
+ // to throw an exception if it fails since there is no way to recover here.
+ realizeAndProjectKinematicsWithThrow(interp, ProjectOptions::LocalOnly);
+}
+
+
+//==============================================================================
+// BACK UP ADVANCED STATE BY INTERPOLATION
+//==============================================================================
+// Interpolate the advanced state back to an earlier part of the interval,
+// forgetting about the rest of the interval. This is necessary, for
+// example after we have localized an event trigger to an interval tLow:tHigh
+// where tHigh < tAdvanced.
+//
+// See comment above re this being a first-order *whole step* interpolation.
+void SemiExplicitEuler2IntegratorRep::
+backUpAdvancedStateByInterpolation(Real t) {
+ const System& system = getSystem();
+ State& advanced = updAdvancedState();
+ const Real t0 = getPreviousTime(), t1 = advanced.getTime();
+ const Vector& y0 = getPreviousY();
+ const Vector& y1 = advanced.getY();
+
+ assert(t0 <= t && t <= t1);
+ assert(t1 > t0);
+
+ const Real weight0 = (t1-t) / (t1-t0);
+ const Real weight1 = 1-weight0;
+ advanced.updY() = weight0*y0 + weight1*y1;
+ advanced.updTime() = t;
+
+ // Ignore any user request not to project interpolated states here -- this
+ // is the actual advanced state which will be propagated through the
+ // rest of the trajectory so we can't allow it not to satisfy the
+ // constraints!
+ // But it is OK if it just *barely* satisfies the constraints so we
+ // won't get carried away if the user isn't being finicky about it.
+
+ // Project position constraints if warranted. Allow project()
+ // to throw an exception if it fails since there is no way to recover here.
+
+ realizeAndProjectKinematicsWithThrow(advanced, ProjectOptions::LocalOnly);
+}
+
+
+
+//==============================================================================
+// ATTEMPT DAE STEP
+//==============================================================================
+// Note that SemiExplicitEuler overrides the entire DAE step because it can't
+// use the default ODE-then-DAE structure. Instead the constraint projections
+// are interwoven here.
+bool SemiExplicitEuler2IntegratorRep::attemptDAEStep
+ (Real t1, Vector& yErrEst, int& errOrder, int& numIterations)
+{
+ const System& system = getSystem();
+ State& advanced = updAdvancedState();
+ Vector dummyErrEst; // for when we don't want the error estimate projected
+
+ const int nq = advanced.getNQ();
+ const int nu = advanced.getNU();
+ const int nz = advanced.getNZ();
+
+ // We'll need to work with these variable separately.
+ VectorView qErrEst = yErrEst( 0, nq);
+ VectorView uErrEst = yErrEst( nq, nu);
+ VectorView zErrEst = yErrEst(nq+nu, nz);
+
+ statsStepsAttempted++;
+ errOrder = 2;
+
+ const Real t0 = getPreviousTime(); // nicer names
+ const Vector& q0 = getPreviousQ();
+ const Vector& u0 = getPreviousU();
+ const Vector& z0 = getPreviousZ();
+ const Vector& qdot0 = getPreviousQDot();
+ const Vector& udot0 = getPreviousUDot();
+ const Vector& zdot0 = getPreviousZDot();
+ const Vector& qdotdot0 = getPreviousQDotDot();
+
+ const Real h = t1-t0, hHalf = h/2, tHalf = t0 + hHalf;
+
+ // -------------------------------------------------------------------------
+ // First calculate the big step, borrowing advanced for the calculations.
+ advanced.updZ() = m_zBig = getPreviousZ() + h * getPreviousZDot();
+ advanced.updU() = getPreviousU() + h * getPreviousUDot();
+
+ //TODO: need to be able to do this without invalidating q's.
+ // Should be able to calculate u_prescribed(newTime, oldQ)
+ advanced.updTime() = t1;
+ system.realize(advanced, Stage::Position); // old q, new t=t1
+ system.prescribeU(advanced);
+
+ // Update qdotBig = N(q_t0)*u_t1 from now-advanced u.
+ system.multiplyByN(advanced, advanced.getU(), m_qdotTmp);
+ advanced.updQ() = getPreviousQ() + h * m_qdotTmp;
+ system.prescribeQ(advanced); // at t1
+ m_qBig = advanced.getQ();
+ system.realize(advanced, Stage::Position); // new q, new t=t1
+ system.prescribeU(advanced); // update prescribed u in case q-dependent
+ m_uBig = advanced.getU();
+ // -------------------------------------------------------------------------
+
+ // At this point yBig=(qBig,uBig,zBig) has been calculated.
+
+ // -------------------------------------------------------------------------
+ // Now take two half steps, working directly in advanced.
+ advanced.updZ() = getPreviousZ() + hHalf * getPreviousZDot();
+ advanced.updU() = getPreviousU() + hHalf * getPreviousUDot();
+ advanced.updQ() = getPreviousQ(); // back to old q
+ advanced.updTime() = tHalf;
+ system.realize(advanced, Stage::Position); // old q, new t=tHalf
+ system.prescribeU(advanced);
+
+ // Update qdot_tHalf = N(q_t0)*u_tHalf from now-advanced u.
+ system.multiplyByN(advanced, advanced.getU(), m_qdotTmp);
+ advanced.updQ() += hHalf * m_qdotTmp;
+ system.prescribeQ(advanced);
+ system.realize(advanced, Stage::Position); // new q, new t
+ system.prescribeU(advanced); // update prescribed u if q-dependent
+ system.realize(advanced, Stage::Velocity);
+
+ realizeStateDerivatives(advanced); // get udot,zdot at tHalf
+ // Get these references now -- as soon as we change u or z they
+ // will be invalid.
+ const Vector& zdotHalf = advanced.getZDot();
+ const Vector& udotHalf = advanced.getUDot();
+
+ // Second half-step.
+ advanced.updZ() += hHalf * zdotHalf;
+ advanced.updU() += hHalf * udotHalf;
+
+ advanced.updTime() = t1;
+ system.realize(advanced, Stage::Position); // old q=qHalf, new t=t1
+ system.prescribeU(advanced);
+
+ // Update qdot_t1 = N(q_tHalf)*u_t1 from now-advanced u.
+ system.multiplyByN(advanced, advanced.getU(), m_qdotTmp);
+ advanced.updQ() += hHalf * m_qdotTmp;
+ system.prescribeQ(advanced);
+ system.realize(advanced, Stage::Position); // new q=q1, new t=t1
+ system.prescribeU(advanced); // update prescribed u in case q-dependent
+ // -------------------------------------------------------------------------
+ // Now estimate the error and use local extrapolation to improve the
+ // final solution.
+ qErrEst = advanced.getQ() - m_qBig;
+ uErrEst = advanced.getU() - m_uBig;
+ zErrEst = advanced.getZ() - m_zBig;
+
+ // Local extrapolation. CAUSES STABILITY PROBLEMS! Don't do it!
+ //advanced.updZ() += zErrEst; // Solution is now second-order.
+ //advanced.updU() += uErrEst;
+ //advanced.updQ() += qErrEst;
+ //system.realize(advanced, Stage::Position);
+ //system.prescribeU(advanced); // update prescribed u in case q-dependent
+
+ // Consider position constraint projection. Note that we have to do this
+ // projection prior to calculating prescribed u's since the prescription
+ // can depend on q's. Prevent project() from throwing an exception since
+ // failure here may be recoverable.
+ bool anyChanges;
+ if (!localProjectQAndQErrEstNoThrow(advanced, dummyErrEst, anyChanges))
+ return false; // convergence failure for this step
+
+ // q's satisfy the position constraint manifold. Now work on u's.
+
+ system.prescribeU(advanced);
+ system.realize(advanced, Stage::Velocity);
+
+ // Now try velocity constraint projection. Nothing will happen if
+ // velocity constraints are already satisfied unless user has set the
+ // ForceProjection option.
+
+ if (!localProjectUAndUErrEstNoThrow(advanced, dummyErrEst, anyChanges))
+ return false; // convergence failure for this step
+
+ numIterations = 1;
+ return true;
+}
+
diff --git a/SimTKmath/Integrators/src/SemiExplicitEuler2IntegratorRep.h b/SimTKmath/Integrators/src/SemiExplicitEuler2IntegratorRep.h
new file mode 100644
index 0000000..653f59b
--- /dev/null
+++ b/SimTKmath/Integrators/src/SemiExplicitEuler2IntegratorRep.h
@@ -0,0 +1,45 @@
+#ifndef SimTK_SIMMATH_SEMI_EXPLICIT_EULER_2_INTEGRATOR_REP_H_
+#define SimTK_SIMMATH_SEMI_EXPLICIT_EULER_2_INTEGRATOR_REP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "AbstractIntegratorRep.h"
+
+namespace SimTK {
+
+class SemiExplicitEuler2IntegratorRep : public AbstractIntegratorRep {
+public:
+ SemiExplicitEuler2IntegratorRep(Integrator* handle, const System& sys);
+protected:
+ bool attemptDAEStep
+ (Real t1, Vector& yErrEst, int& errOrder, int& numIterations);
+ void createInterpolatedState(Real t);
+ void backUpAdvancedStateByInterpolation(Real t);
+private:
+ Vector m_qdotTmp, m_qBig, m_uBig, m_zBig;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_SEMI_EXPLICIT_EULER_2_INTEGRATOR_REP_H_
diff --git a/SimTKmath/Integrators/src/SemiExplicitEulerIntegrator.cpp b/SimTKmath/Integrators/src/SemiExplicitEulerIntegrator.cpp
new file mode 100644
index 0000000..de0309b
--- /dev/null
+++ b/SimTKmath/Integrators/src/SemiExplicitEulerIntegrator.cpp
@@ -0,0 +1,191 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "simmath/SemiExplicitEulerIntegrator.h"
+
+#include "IntegratorRep.h"
+#include "SemiExplicitEulerIntegratorRep.h"
+
+using namespace SimTK;
+
+//==============================================================================
+// SEMI EXPLICIT EULER INTEGRATOR
+//==============================================================================
+
+SemiExplicitEulerIntegrator::SemiExplicitEulerIntegrator
+ (const System& sys, Real stepSize) {
+ rep = new SemiExplicitEulerIntegratorRep(this, sys);
+ setFixedStepSize(stepSize);
+}
+
+
+//==============================================================================
+// SEMI EXPLICIT EULER INTEGRATOR REP
+//==============================================================================
+SemiExplicitEulerIntegratorRep::SemiExplicitEulerIntegratorRep
+ (Integrator* handle, const System& sys)
+: AbstractIntegratorRep(handle, sys, 1, 1, "SemiExplicitEuler", false)
+{
+}
+
+//==============================================================================
+// CREATE INTERPOLATED STATE
+//==============================================================================
+// Create an interpolated state at time t, which is between tPrev and tCurrent.
+// If we haven't yet delivered an interpolated state in this interval, we have
+// to initialize its discrete part from the advanced state.
+void SemiExplicitEulerIntegratorRep::createInterpolatedState(Real t) {
+ const System& system = getSystem();
+ const State& advanced = getAdvancedState();
+ State& interp = updInterpolatedState();
+ interp = advanced; // pick up discrete stuff.
+ const Real weight1 = (getAdvancedTime()-t) /
+ (getAdvancedTime()-getPreviousTime());
+ const Real weight2 = 1-weight1;
+ interp.updY() = weight1*getPreviousY()+weight2*getAdvancedState().getY();
+ interp.updTime() = t;
+
+ if (userProjectInterpolatedStates == 0) {
+ system.realize(interp, Stage::Time);
+ system.prescribeQ(interp);
+ system.realize(interp, Stage::Position);
+ system.prescribeU(interp);
+ system.realize(interp, Stage::Velocity);
+ return;
+ }
+
+ // We may need to project onto constraint manifold. Allow project()
+ // to throw an exception if it fails since there is no way to recover here.
+ realizeAndProjectKinematicsWithThrow(interp, ProjectOptions::LocalOnly);
+}
+
+
+//==============================================================================
+// BACK UP ADVANCED STATE BY INTERPOLATION
+//==============================================================================
+// Interpolate the advanced state back to an earlier part of the interval,
+// forgetting about the rest of the interval. This is necessary, for
+// example after we have localized an event trigger to an interval tLow:tHigh
+// where tHigh < tAdvanced.
+void SemiExplicitEulerIntegratorRep::
+backUpAdvancedStateByInterpolation(Real t) {
+ const System& system = getSystem();
+ State& advanced = updAdvancedState();
+ const Real t0 = getPreviousTime(), t1 = advanced.getTime();
+ const Vector& y0 = getPreviousY();
+ const Vector& y1 = advanced.getY();
+
+ assert(t0 <= t && t <= t1);
+ assert(t1 > t0);
+
+ const Real weight0 = (t1-t) / (t1-t0);
+ const Real weight1 = 1-weight0;
+ advanced.updY() = weight0*y0 + weight1*y1;
+ advanced.updTime() = t;
+
+ // Ignore any user request not to project interpolated states here -- this
+ // is the actual advanced state which will be propagated through the
+ // rest of the trajectory so we can't allow it not to satisfy the
+ // constraints!
+ // But it is OK if it just *barely* satisfies the constraints so we
+ // won't get carried away if the user isn't being finicky about it.
+
+ // Project position constraints if warranted. Allow project()
+ // to throw an exception if it fails since there is no way to recover here.
+
+ realizeAndProjectKinematicsWithThrow(advanced, ProjectOptions::LocalOnly);
+}
+
+
+
+//==============================================================================
+// ATTEMPT DAE STEP
+//==============================================================================
+// Note that SemiExplicitEuler overrides the entire DAE step because it can't
+// use the default ODE-then-DAE structure. Instead the constraint projections
+// are interwoven here.
+bool SemiExplicitEulerIntegratorRep::attemptDAEStep
+ (Real t1, Vector& yErrEst, int& errOrder, int& numIterations)
+{
+ const System& system = getSystem();
+ State& advanced = updAdvancedState();
+ Vector dummyErrEst; // for when we don't want the error estimate projected
+
+ statsStepsAttempted++;
+
+ const Real t0 = getPreviousTime(); // nicer names
+ const Vector& q0 = getPreviousQ();
+ const Vector& u0 = getPreviousU();
+ const Vector& z0 = getPreviousZ();
+ const Vector& qdot0 = getPreviousQDot();
+ const Vector& udot0 = getPreviousUDot();
+ const Vector& zdot0 = getPreviousZDot();
+ const Vector& qdotdot0 = getPreviousQDotDot();
+
+ const Real h = t1-t0;
+
+ // Advance the first order variables.
+ // TODO: this part should be implicit in u and z to make this symplectic
+ // Euler.
+ advanced.updZ() = getPreviousZ() + h * getPreviousZDot();
+ advanced.updU() = getPreviousU() + h * getPreviousUDot();
+
+ //TODO: need to be able to do this without invalidating q's.
+ // Should be able to calculate u_prescribed(newTime, oldQ)
+ advanced.updTime() = t1;
+ system.realize(advanced, Stage::Position); // old q, new t
+ system.prescribeU(advanced);
+
+ // Update qdot_t1 = N(q_t0)*u_t1 from now-advanced u.
+ const int nq = advanced.getNQ();
+ Vector qdot_t1(nq);
+ system.multiplyByN(advanced, advanced.getU(), qdot_t1);
+ advanced.updQ() = getPreviousQ() + h * qdot_t1;
+ system.prescribeQ(advanced);
+ system.realize(advanced, Stage::Position); // new q, new t
+ system.prescribeU(advanced); // update prescribed u in case q-dependent
+
+ // Consider position constraint projection. Note that we have to do this
+ // projection prior to calculating prescribed u's since the prescription
+ // can depend on q's. Prevent project() from throwing an exception since
+ // failure here may be recoverable.
+ bool anyChanges;
+ if (!localProjectQAndQErrEstNoThrow(advanced, dummyErrEst, anyChanges))
+ return false; // convergence failure for this step
+
+ // q's satisfy the position constraint manifold. Now work on u's.
+
+ system.prescribeU(advanced);
+ system.realize(advanced, Stage::Velocity);
+
+ // Now try velocity constraint projection. Nothing will happen if
+ // velocity constraints are already satisfied unless user has set the
+ // ForceProjection option.
+
+ if (!localProjectUAndUErrEstNoThrow(advanced, dummyErrEst, anyChanges))
+ return false; // convergence failure for this step
+
+ numIterations = 1;
+ return true;
+}
+
diff --git a/SimTKmath/Integrators/src/SemiExplicitEulerIntegratorRep.h b/SimTKmath/Integrators/src/SemiExplicitEulerIntegratorRep.h
new file mode 100644
index 0000000..b8b53c0
--- /dev/null
+++ b/SimTKmath/Integrators/src/SemiExplicitEulerIntegratorRep.h
@@ -0,0 +1,43 @@
+#ifndef SimTK_SIMMATH_SEMI_EXPLICIT_EULER_INTEGRATOR_REP_H_
+#define SimTK_SIMMATH_SEMI_EXPLICIT_EULER_INTEGRATOR_REP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "AbstractIntegratorRep.h"
+
+namespace SimTK {
+
+class SemiExplicitEulerIntegratorRep : public AbstractIntegratorRep {
+public:
+ SemiExplicitEulerIntegratorRep(Integrator* handle, const System& sys);
+protected:
+ bool attemptDAEStep
+ (Real t1, Vector& yErrEst, int& errOrder, int& numIterations);
+ void createInterpolatedState(Real t);
+ void backUpAdvancedStateByInterpolation(Real t);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_SEMI_EXPLICIT_EULER_INTEGRATOR_REP_H_
diff --git a/SimTKmath/Integrators/src/TimeStepper.cpp b/SimTKmath/Integrators/src/TimeStepper.cpp
new file mode 100644
index 0000000..08a1706
--- /dev/null
+++ b/SimTKmath/Integrators/src/TimeStepper.cpp
@@ -0,0 +1,212 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This is the private (library side) implementation of the Simmath
+ * TimeStepper family of classes.
+ */
+
+#include "SimTKcommon.h"
+#include "simmath/TimeStepper.h"
+
+#include "TimeStepperRep.h"
+
+#include <exception>
+#include <limits>
+
+namespace SimTK {
+
+ ////////////////////////////////////
+ // IMPLEMENTATION OF TIME STEPPER //
+ ////////////////////////////////////
+
+TimeStepper::TimeStepper(const System& sys) {
+ rep = new TimeStepperRep(this, sys);
+}
+
+TimeStepper::TimeStepper(const System& sys, Integrator& integrator) {
+ rep = new TimeStepperRep(this, sys);
+ setIntegrator(integrator);
+}
+
+TimeStepper::~TimeStepper() {
+ if (rep && rep->myHandle==this)
+ delete rep;
+ rep = 0;
+}
+
+void TimeStepper::setIntegrator(Integrator& integrator) {
+ rep->setIntegrator(integrator);
+}
+
+const Integrator& TimeStepper::getIntegrator() const {
+ return rep->getIntegrator();
+}
+
+Integrator& TimeStepper::updIntegrator() {
+ return rep->updIntegrator();
+}
+
+const State& TimeStepper::getState() const {
+ return rep->getState();
+}
+
+void TimeStepper::initialize(const State& initState) {
+ updIntegrator().initialize(initState);
+ rep->lastEventTime = -Infinity;
+ rep->lastReportTime = -Infinity;
+}
+
+Integrator::SuccessfulStepStatus TimeStepper::stepTo(Real reportTime) {
+ return( rep->stepTo(reportTime) );
+}
+
+bool TimeStepper::getReportAllSignificantStates() const {
+ return rep->getReportAllSignificantStates();
+}
+
+void TimeStepper::setReportAllSignificantStates(bool b) {
+ rep->setReportAllSignificantStates(b);
+}
+
+
+ ////////////////////////////////////////
+ // IMPLEMENTATION OF TIME STEPPER REP //
+ ////////////////////////////////////////
+
+TimeStepperRep::TimeStepperRep(TimeStepper* handle, const System& system)
+: myHandle(handle), system(system), integ(0),
+ reportAllSignificantStates(false) {}
+
+Integrator::SuccessfulStepStatus TimeStepperRep::stepTo(Real time) {
+ // Handler is allowed to throw an exception if it fails since we don't
+ // have a way to recover.
+ HandleEventsOptions handleOpts(integ->getConstraintToleranceInUse());
+
+ if (integ->isInfinityNormInUse())
+ handleOpts.setOption(HandleEventsOptions::UseInfinityNorm);
+
+ Array_<EventId> scheduledEventIds, scheduledReportIds;
+ while (!integ->isSimulationOver()) {
+ Real nextScheduledEvent = Infinity;
+ Real nextScheduledReport = Infinity;
+ Real currentTime = integ->getTime();
+ system.realize(integ->getState(), Stage::Time);
+ system.realize(integ->getAdvancedState(), Stage::Time);
+ system.calcTimeOfNextScheduledEvent
+ (integ->getState(), nextScheduledEvent, scheduledEventIds,
+ lastEventTime != currentTime); // whether to allow now as an answer
+ system.calcTimeOfNextScheduledReport
+ (integ->getState(), nextScheduledReport, scheduledReportIds,
+ lastReportTime != currentTime); // whether to allow now as an answer
+
+ Real reportTime = std::min(nextScheduledReport, time);
+ Real eventTime = std::min(nextScheduledEvent, time);
+
+ //---------------- take continuous step ----------------
+ Integrator::SuccessfulStepStatus status =
+ integ->stepTo(reportTime, eventTime);
+ //------------------------------------------------------
+
+ Stage lowestModified = Stage::Report;
+ bool shouldTerminate;
+ switch (status) {
+ case Integrator::ReachedStepLimit: {
+ if (reportAllSignificantStates)
+ return status;
+ continue;
+ }
+ case Integrator::StartOfContinuousInterval: {
+ if (reportAllSignificantStates)
+ return status;
+ continue;
+ }
+ case Integrator::ReachedReportTime: {
+ if (integ->getTime() >= nextScheduledReport) {
+ system.reportEvents(integ->getState(),
+ Event::Cause::Scheduled,
+ scheduledReportIds);
+ lastReportTime = integ->getTime();
+ }
+ if (integ->getTime() >= time || reportAllSignificantStates)
+ return status;
+ continue;
+ }
+ case Integrator::ReachedScheduledEvent: {
+ HandleEventsResults results;
+ system.handleEvents(integ->updAdvancedState(),
+ Event::Cause::Scheduled,
+ scheduledEventIds,
+ handleOpts, results);
+ lowestModified = results.getLowestModifiedStage();
+ shouldTerminate =
+ results.getExitStatus()==HandleEventsResults::ShouldTerminate;
+ lastEventTime = integ->getTime();
+ break;
+ }
+ case Integrator::TimeHasAdvanced: {
+ HandleEventsResults results;
+ system.handleEvents(integ->updAdvancedState(),
+ Event::Cause::TimeAdvanced,
+ Array_<EventId>(),
+ handleOpts, results);
+ lowestModified = results.getLowestModifiedStage();
+ shouldTerminate =
+ results.getExitStatus()==HandleEventsResults::ShouldTerminate;
+ break;
+ }
+ case Integrator::ReachedEventTrigger: {
+ HandleEventsResults results;
+ system.handleEvents(integ->updAdvancedState(),
+ Event::Cause::Triggered,
+ integ->getTriggeredEvents(),
+ handleOpts, results);
+ lowestModified = results.getLowestModifiedStage();
+ shouldTerminate =
+ results.getExitStatus()==HandleEventsResults::ShouldTerminate;
+ break;
+ }
+ case Integrator::EndOfSimulation: {
+ HandleEventsResults results;
+ system.handleEvents(integ->updAdvancedState(),
+ Event::Cause::Termination,
+ Array_<EventId>(),
+ handleOpts, results);
+ lowestModified = results.getLowestModifiedStage();
+ shouldTerminate =
+ results.getExitStatus()==HandleEventsResults::ShouldTerminate;
+ break;
+ }
+ default: assert(!"Unrecognized return from stepTo()");
+ }
+ integ->reinitialize(lowestModified, shouldTerminate);
+ if (reportAllSignificantStates)
+ return status;
+ }
+ return Integrator::EndOfSimulation;
+}
+
+} // namespace SimTK
+
+
+
diff --git a/SimTKmath/Integrators/src/TimeStepperRep.h b/SimTKmath/Integrators/src/TimeStepperRep.h
new file mode 100644
index 0000000..60f858f
--- /dev/null
+++ b/SimTKmath/Integrators/src/TimeStepperRep.h
@@ -0,0 +1,102 @@
+#ifndef SimTK_SIMMATH_TIMESTEPPER_REP_H_
+#define SimTK_SIMMATH_TIMESTEPPER_REP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This is the declaration of the TimeStepperRep class which
+ * represents the implementation of the TimeStepper class.
+ */
+
+#include "SimTKcommon.h"
+#include "SimTKcommon/internal/SystemGuts.h"
+
+#include "simmath/Integrator.h"
+#include "simmath/TimeStepper.h"
+
+#include <exception>
+
+namespace SimTK {
+
+ ////////////////////////////
+ // CLASS TIME STEPPER REP //
+ ////////////////////////////
+
+// This is a concrete class -- there is only one kind of SimTK TimeStepper.
+// The IntegratorRep on the other hand is abstract.
+class TimeStepperRep {
+public:
+ TimeStepperRep(TimeStepper* handle, const System& system);
+ // default destructor, no default constructor, no copy or copy assign
+
+ Integrator::SuccessfulStepStatus stepTo(Real time);
+
+ const State& getState() const {return integ->getState();}
+
+ const System getSystem() const {return system;}
+ void setIntegrator(Integrator& integrator) {
+ integ = &integrator;
+ }
+ const Integrator& getIntegrator() const {
+ assert(integ);
+ return *integ;
+ }
+ Integrator& updIntegrator() {
+ assert(integ);
+ return *integ;
+ }
+ bool getReportAllSignificantStates() const {
+ return reportAllSignificantStates;
+ }
+ void setReportAllSignificantStates(bool b) {
+ reportAllSignificantStates = b;
+ }
+
+private:
+ TimeStepper* myHandle;
+ friend class TimeStepper;
+
+ // This is the System to be advanced through time.
+ const System& system;
+
+ // This is the Integrator used to advance through continuous intervals. The Integrator
+ // maintains the current State for the System.
+ Integrator* integ;
+
+ // Whether to report every significant state.
+ bool reportAllSignificantStates;
+
+ // The last time at events and reports were processed.
+ Real lastEventTime, lastReportTime;
+
+ // suppress
+ TimeStepperRep(const TimeStepperRep&);
+ TimeStepperRep& operator=(const TimeStepperRep&);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_TIMESTEPPER_REP_H_
+
+
diff --git a/SimTKmath/Integrators/src/VerletIntegrator.cpp b/SimTKmath/Integrators/src/VerletIntegrator.cpp
new file mode 100644
index 0000000..9fe5591
--- /dev/null
+++ b/SimTKmath/Integrators/src/VerletIntegrator.cpp
@@ -0,0 +1,234 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "simmath/VerletIntegrator.h"
+
+#include "IntegratorRep.h"
+#include "VerletIntegratorRep.h"
+
+using namespace SimTK;
+
+//==============================================================================
+// VERLET INTEGRATOR
+//==============================================================================
+
+VerletIntegrator::VerletIntegrator(const System& sys) {
+ rep = new VerletIntegratorRep(this, sys);
+}
+
+VerletIntegrator::VerletIntegrator(const System& sys, Real stepSize) {
+ rep = new VerletIntegratorRep(this, sys);
+ setFixedStepSize(stepSize);
+}
+
+
+//==============================================================================
+// VERLET INTEGRATOR REP
+//==============================================================================
+VerletIntegratorRep::VerletIntegratorRep(Integrator* handle, const System& sys)
+: AbstractIntegratorRep(handle, sys, 2, 3, "Verlet", true)
+{
+}
+
+
+
+//==============================================================================
+// ATTEMPT DAE STEP
+//==============================================================================
+// Note that Verlet overrides the entire DAE step because it can't use the
+// default ODE-then-DAE structure. Instead the constraint projections are
+// interwoven here.
+bool VerletIntegratorRep::attemptDAEStep
+ (Real t1, Vector& yErrEst, int& errOrder, int& numIterations)
+{
+ const System& system = getSystem();
+ State& advanced = updAdvancedState();
+ Vector dummyErrEst; // for when we don't want the error estimate projected
+
+ statsStepsAttempted++;
+
+ const Real t0 = getPreviousTime(); // nicer names
+ const Vector& q0 = getPreviousQ();
+ const Vector& u0 = getPreviousU();
+ const Vector& z0 = getPreviousZ();
+ const Vector& qdot0 = getPreviousQDot();
+ const Vector& udot0 = getPreviousUDot();
+ const Vector& zdot0 = getPreviousZDot();
+ const Vector& qdotdot0 = getPreviousQDotDot();
+
+ const Real h = t1-t0;
+
+ const int nq = advanced.getNQ();
+ const int nu = advanced.getNU();
+ const int nz = advanced.getNZ();
+
+ // We will catch any exceptions thrown by realize() or project() and simply
+ // treat that as a failure to take a step due to the step size being too
+ // big. The idea is that the caller should reduce the step size and try
+ // again, giving up only when the step size goes below the allowed minimum.
+
+ try
+ {
+ numIterations = 0;
+
+ VectorView qErrEst = yErrEst( 0, nq); // all 3rd order estimates
+ VectorView uErrEst = yErrEst( nq, nu);
+ VectorView zErrEst = yErrEst(nq+nu, nz);
+ VectorView uzErrEst = yErrEst( nq, nu+nz); // all 2nd order estimates
+
+ // Calculate the new positions q (3rd order) and initial (1st order)
+ // estimate for the velocities u and auxiliary variables z.
+
+ // These are final values (the q's will get projected, though).
+ advanced.updTime() = t1;
+ advanced.updQ() = q0 + h*qdot0 + (h*h/2)*qdotdot0;
+
+ // Now make an initial estimate of first-order variable u and z.
+ const Vector u1_est = u0 + h*udot0;
+ const Vector z1_est = z0 + h*zdot0;
+
+ advanced.updU() = u1_est; // u's and z's will change in advanced below
+ advanced.updZ() = z1_est;
+
+ system.realize(advanced, Stage::Time);
+ system.prescribeQ(advanced);
+ system.realize(advanced, Stage::Position);
+
+ // Consider position constraint projection. (See AbstractIntegratorRep
+ // for how we decide not to project.)
+ const Real projectionLimit =
+ std::max(2*getConstraintToleranceInUse(),
+ std::sqrt(getConstraintToleranceInUse()));
+
+ bool anyChangesQ;
+ if (!localProjectQAndQErrEstNoThrow(advanced, dummyErrEst, anyChangesQ,
+ projectionLimit))
+ return false; // convergence failure for this step
+
+ // q is now at its final integrated, prescribed, and projected value.
+ // u and z still need refinement.
+
+ system.prescribeU(advanced);
+ system.realize(advanced, Stage::Velocity);
+
+ // No u projection yet.
+
+ // Get new values for the derivatives.
+ realizeStateDerivatives(advanced);
+
+ // We're going to integrate the u's and z's with the 2nd order implicit
+ // trapezoid rule: u(t+h) = u(t) + h*(f(u(t))+f(u(t+h)))/2. Unfortunately
+ // this is an implicit method so we have to iterate to refine u(t+h) until
+ // this equation is acceptably satisfied. We're using functional iteration
+ // here which has a very limited radius of convergence.
+
+ const Real tol = std::min(Real(1e-4), Real(0.1)*getAccuracyInUse());
+ Vector usave(nu), zsave(nz); // temporaries
+ bool converged = false;
+ Real prevChange = Infinity; // use this to quit early
+ for (int i = 0; !converged && i < 10; ++i) {
+ ++numIterations;
+ // At this point we know that the advanced state has been realized
+ // through the Acceleration level, so its uDot and zDot reflect
+ // the u and z state values it contains.
+ usave = advanced.getU();
+ zsave = advanced.getZ();
+
+ // Get these references now -- as soon as we change u or z they
+ // will be invalid.
+ const Vector& udot1 = advanced.getUDot();
+ const Vector& zdot1 = advanced.getZDot();
+
+ // Refine u and z estimates.
+ advanced.setU(u0 + (h/2)*(udot0 + udot1));
+ advanced.setZ(z0 + (h/2)*(zdot0 + zdot1));
+
+ // Fix prescribed u's which may have been changed here.
+ system.prescribeU(advanced);
+ system.realize(advanced, Stage::Velocity);
+
+ // No projection yet.
+
+ // Calculate fresh derivatives UDot and ZDot.
+ realizeStateDerivatives(advanced);
+
+ // Calculate convergence as the ratio of the norm of the last delta to
+ // the norm of the values prior to the last change. We're using the
+ // 2-norm but this ratio would be the same if we used the RMS norm.
+ // TinyReal is there to keep us out of trouble if we started at zero.
+
+ const Real convergenceU = (advanced.getU()-usave).norm()
+ / (usave.norm()+TinyReal);
+ const Real convergenceZ = (advanced.getZ()-zsave).norm()
+ / (zsave.norm()+TinyReal);
+ const Real change = std::max(convergenceU,convergenceZ);
+ converged = (change <= tol);
+ if (i > 1 && (change > prevChange))
+ break; // we're headed the wrong way after two iterations -- give up
+ prevChange = change;
+ }
+
+ // Now that we have achieved 2nd order estimates of u and z, we can use
+ // them to calculate a 3rd order error estimate for q and 2nd order error
+ // estimates for u and z. Note that we have already realized the state with
+ // the new values, so QDot reflects the new u's.
+
+ qErrEst = q0 + (h/2)*(qdot0+advanced.getQDot()) // implicit trapezoid rule integral
+ - advanced.getQ(); // Verlet integral
+
+ uErrEst = u1_est // explicit Euler integral
+ - advanced.getU(); // implicit trapezoid rule integral
+ zErrEst = z1_est - advanced.getZ(); // ditto for z's
+
+ // TODO: because we're only projecting velocities here, we aren't going to
+ // get our position errors reduced here, which is a shame. Should be able
+ // to do this even though we had to project q's earlier, because the
+ // projection matrix should still be around.
+ bool anyChangesU;
+ if (!localProjectUAndUErrEstNoThrow(advanced, yErrEst, anyChangesU,
+ projectionLimit))
+ return false; // convergence failure for this step
+
+ // Two different integrators were used to estimate errors: trapezoidal for
+ // Q, and explicit Euler for U and Z. This means that the U and Z errors
+ // are of a different order than the Q errors. We therefore multiply them
+ // by h so everything will be of the same order.
+
+ // TODO: (sherm) I don't think this is valid. Although it does fix the
+ // order, it also changes the absolute errors (typically by reducing
+ // them since h is probably < 1) which will affect whether the caller
+ // decides to accept the step. Instead, a different error order should
+ // be used when one of these is driving the step size.
+
+ uErrEst *= h; zErrEst *= h; // everything is 3rd order in h now
+ errOrder = 3;
+
+ //errOrder = qErrRMS > uzErrRMS ? 3 : 2;
+
+ return converged;
+ }
+ catch (std::exception ex) {
+ return false;
+ }
+}
+
diff --git a/SimTKmath/Integrators/src/VerletIntegratorRep.h b/SimTKmath/Integrators/src/VerletIntegratorRep.h
new file mode 100644
index 0000000..f50a89b
--- /dev/null
+++ b/SimTKmath/Integrators/src/VerletIntegratorRep.h
@@ -0,0 +1,41 @@
+#ifndef SimTK_SIMMATH_VERLET_INTEGRATOR_REP_H_
+#define SimTK_SIMMATH_VERLET_INTEGRATOR_REP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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 "AbstractIntegratorRep.h"
+
+namespace SimTK {
+
+class VerletIntegratorRep : public AbstractIntegratorRep {
+public:
+ VerletIntegratorRep(Integrator* handle, const System& sys);
+protected:
+ bool attemptDAEStep
+ (Real t1, Vector& yErrEst, int& errOrder, int& numIterations);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_VERLET_INTEGRATOR_REP_H_
diff --git a/SimTKmath/LinearAlgebra/CMakeLists.txt b/SimTKmath/LinearAlgebra/CMakeLists.txt
new file mode 100644
index 0000000..29e7cd6
--- /dev/null
+++ b/SimTKmath/LinearAlgebra/CMakeLists.txt
@@ -0,0 +1,10 @@
+# add in local source and headers here
+FILE(GLOB src_files ./src/*.cpp)
+FILE(GLOB incl_files ./src/*.h)
+
+# append to the local scope copy, and then copy up to parent scope
+list(APPEND SOURCE_FILES ${src_files})
+set(SOURCE_FILES ${SOURCE_FILES} PARENT_SCOPE)
+
+list(APPEND SOURCE_INCLUDE_FILES ${incl_files})
+set(SOURCE_INCLUDE_FILES ${SOURCE_INCLUDE_FILES} PARENT_SCOPE)
diff --git a/SimTKmath/LinearAlgebra/src/Eigen.cpp b/SimTKmath/LinearAlgebra/src/Eigen.cpp
new file mode 100644
index 0000000..d58a75f
--- /dev/null
+++ b/SimTKmath/LinearAlgebra/src/Eigen.cpp
@@ -0,0 +1,782 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ *
+ * Solves for eigen values and eigen vectors
+ */
+
+
+
+#include "SimTKcommon.h"
+
+#include "simmath/internal/common.h"
+#include "simmath/LinearAlgebra.h"
+
+#include "LapackInterface.h"
+#include "EigenRep.h"
+#include "WorkSpace.h"
+#include "LATraits.h"
+#include "LapackConvert.h"
+
+#include <iostream>
+#include <cstdio>
+#include <cmath>
+#include <complex>
+
+namespace SimTK {
+
+ //////////////////
+ // EigenDefault //
+ //////////////////
+EigenDefault::EigenDefault() {
+ isFactored = false;
+}
+EigenRepBase* EigenDefault::clone() const {
+ return( new EigenDefault(*this));
+}
+
+
+ ///////////
+ // Eigen //
+ ///////////
+Eigen::~Eigen() {
+ delete rep;
+}
+Eigen::Eigen() {
+ rep = new EigenDefault();
+}
+
+template < class ELT >
+Eigen::Eigen( const Matrix_<ELT>& m ) {
+ rep = new EigenRep<typename CNT<ELT>::StdNumber>(m);
+}
+// copy constructor
+Eigen::Eigen( const Eigen& c ) {
+ rep = c.rep->clone();
+}
+// copy assignment operator
+Eigen& Eigen::operator=(const Eigen& rhs) {
+ rep = rhs.rep->clone();
+ return *this;
+}
+
+template < class ELT >
+void Eigen::factor( const Matrix_<ELT>& m ) {
+ delete rep;
+ rep = new EigenRep<typename CNT<ELT>::StdNumber>(m);
+}
+
+template <class VAL, class VEC>
+void Eigen::getAllEigenValuesAndVectors( Vector_<VAL>& values, Matrix_<VEC>& vectors) {
+ rep->getAllEigenValuesAndVectors( values, vectors );
+ return;
+}
+template <class VAL>
+void Eigen::getAllEigenValues( Vector_<VAL>& values) {
+ rep->getAllEigenValues( values );
+ return;
+}
+template <class VAL, class VEC>
+void Eigen::getFewEigenValuesAndVectors( Vector_<VAL>& values, Matrix_<VEC>& vectors, int ilow, int ihi) {
+ rep->getFewEigenValuesAndVectors( values, vectors, ilow, ihi );
+ return;
+}
+template <class T>
+void Eigen::getFewEigenVectors( Matrix_<T>& vectors, int ilow, int ihi ) {
+ rep->getFewEigenVectors( vectors, ilow, ihi );
+ return;
+}
+
+template <class T>
+void Eigen::getFewEigenValues( Vector_<T>& values, int ilow, int ihi ) {
+ rep->getFewEigenValues( values, ilow, ihi );
+ return;
+}
+
+
+template <class VAL, class VEC>
+void Eigen::getFewEigenValuesAndVectors( Vector_<VAL>& values, Matrix_<VEC>& vectors, typename CNT<VAL>::TReal rlow, typename CNT<VAL>::TReal rhi) {
+ rep->getFewEigenValuesAndVectors( values, vectors, rlow, rhi );
+ return;
+}
+template <class T>
+void Eigen::getFewEigenVectors( Matrix_<T>& vectors, typename CNT<T>::TReal rlow, typename CNT<T>::TReal rhi) {
+ rep->getFewEigenVectors( vectors, rlow, rhi );
+ return;
+}
+
+template <class T>
+void Eigen::getFewEigenValues( Vector_<T>& values, typename CNT<T>::TReal rlow, typename CNT<T>::TReal rhi) {
+ rep->getFewEigenValues( values, rlow, rhi );
+ return;
+}
+
+ /////////////////
+ // EigenRep //
+ /////////////////
+template <typename T > // constructor
+ template < typename ELT >
+EigenRep<T>::EigenRep( const Matrix_<ELT>& mat):
+ n(mat.ncol()),
+ inputMatrix(n*n),
+ structure(mat.getMatrixCharacter().getStructure()),
+ range(AllValues),
+ valuesFound(0),
+ vectorsInMatrix(false),
+ needValues(true),
+ needVectors(true) {
+
+ LapackInterface::getMachineUnderflow( abstol );
+ abstol *= 0.5;
+
+ LapackConvert::convertMatrixToLapack( inputMatrix.data, mat );
+ isFactored = true;
+
+}
+template <typename T >
+EigenRepBase* EigenRep<T>::clone() const {
+ return( new EigenRep<T>(*this) );
+}
+
+
+
+template <typename T >
+EigenRep<T>::~EigenRep() {}
+
+template <typename T >
+void EigenRep<T>::copyValues(Vector_<float>& values) {
+
+ values.resize(valuesFound);
+ for(int j = 0;j<valuesFound;j++ ) {
+ values(j) = (float)realEigenValues.data[j];
+ }
+ return;
+}
+template <typename T >
+void EigenRep<T>::copyValues(Vector_<double>& values) {
+
+ values.resize(valuesFound);
+ for(int j = 0;j<valuesFound;j++ ) {
+ values(j) = realEigenValues.data[j];
+ }
+ return;
+}
+template <typename T >
+void EigenRep<T>::copyValues(Vector_<std::complex<float> >& values) {
+
+ values.resize(valuesFound);
+ for(int j = 0;j<valuesFound;j++ ) {
+ values(j) = complexEigenValues.data[j];
+ }
+}
+template <typename T >
+void EigenRep<T>::copyValues(Vector_<std::complex<double> >& values) {
+
+ values.resize(valuesFound);
+ for(int j = 0;j<valuesFound;j++ ) {
+ values(j) = complexEigenValues.data[j];
+ }
+}
+template <>
+ template <>
+void EigenRep<float>::copyVectors(Matrix_<float>& vectors) { // symmetric
+ int i,j;
+
+ vectors.resize(n,valuesFound);
+ if( vectorsInMatrix ) {
+ for(j=0;j<valuesFound;j++) for(i=0;i<n;i++) vectors(i,j) = inputMatrix.data[j*n+i];
+ } else {
+ for(j=0;j<valuesFound;j++) for(i=0;i<n;i++) vectors(i,j) = symmetricEigenVectors.data[j*n+i];
+ }
+
+ return;
+}
+template <>
+ template <>
+void EigenRep<double>::copyVectors(Matrix_<double>& vectors) { // symmetric
+ int i,j;
+
+ vectors.resize(n,valuesFound);
+ if( vectorsInMatrix ) {
+ for(j=0;j<valuesFound;j++) for(i=0;i<n;i++) vectors(i,j) = inputMatrix.data[j*n+i];
+ } else {
+ for(j=0;j<valuesFound;j++) for(i=0;i<n;i++) vectors(i,j) = symmetricEigenVectors.data[j*n+i];
+ }
+ return;
+}
+template <>
+ template <>
+void EigenRep<float>::copyVectors(Matrix_<std::complex<float> >& vectors) { // non-symmetric
+ int i,j;
+
+ vectors.resize(n,valuesFound);
+ for(j=0;j<valuesFound;j++) for(i=0;i<n;i++) vectors(i,j) = complexEigenVectors.data[j*n+i];
+
+ return;
+}
+template <>
+ template <>
+void EigenRep<double>::copyVectors(Matrix_<std::complex<double> >& vectors) { // non-symmetric
+ int i,j;
+
+ vectors.resize(n,valuesFound);
+ for(j=0;j<valuesFound;j++) for(i=0;i<n;i++) vectors(i,j) = complexEigenVectors.data[j*n+i];
+
+ return;
+}
+template <>
+ template <>
+void EigenRep<std::complex<float> >::copyVectors(Matrix_<std::complex<float> >& vectors) {
+ int i,j;
+
+ vectors.resize(n,valuesFound);
+/*
+ if( structure == MatrixStructure::Symmetric ) {
+ if( vectorsInMatrix ) {
+ for(j=0;j<valuesFound;j++) for(i=0;i<n;i++) vectors(i,j) = inputMatrix.data[j*n+i];
+ } else {
+ for(j=0;j<valuesFound;j++) for(i=0;i<n;i++) vectors(i,j) = symmetricEigenVectors.data[j*n+i];
+ }
+ } else {
+*/
+ for(j=0;j<valuesFound;j++) for(i=0;i<n;i++) vectors(i,j) = complexEigenVectors.data[j*n+i];
+// }
+ return;
+}
+template <>
+ template <>
+void EigenRep<std::complex<double> >::copyVectors(Matrix_<std::complex<double> >& vectors) {
+ int i,j;
+/*
+ vectors.resize(n,valuesFound);
+ if( structure == MatrixStructure::Symmetric ) {
+ if( vectorsInMatrix ) {
+ for(j=0;j<valuesFound;j++) for(i=0;i<n;i++) vectors(i,j) = inputMatrix.data[j*n+i];
+ } else {
+ for(j=0;j<valuesFound;j++) for(i=0;i<n;i++) vectors(i,j) = symmetricEigenVectors.data[j*n+i];
+ }
+ } else {
+*/
+ for(j=0;j<valuesFound;j++) for(i=0;i<n;i++) vectors(i,j) = complexEigenVectors.data[j*n+i];
+ // }
+ return;
+}
+template <>
+ template <>
+void EigenRep<std::complex<double> >::copyVectors(Matrix_<double>& vectors) { assert(false);} // should never get called
+
+template <>
+ template <>
+void EigenRep<std::complex<float> >::copyVectors(Matrix_<float>& vectors) { assert(false);} // should never get called
+
+template < class T >
+void EigenRep<T>::computeValues(bool computeVectors) {
+
+ int info;
+
+ T *leftVectors = NULL; // no left vectors are calculated
+ char calcLeftEigenVectors = 'N'; // don't compute left eigen vectors
+ char calcRightEigenVectors;
+ if( computeVectors ) {
+ calcRightEigenVectors = 'V'; // compute right eigen vectors
+ } else {
+ calcRightEigenVectors = 'N'; // don't compute right eigen vectors
+ }
+ int computeLwork = -1;
+ //T size[1];
+/*
+ if( structure == MatrixStructure::Symmetric ) {
+ char useUpper = 'U'; // matrix is stored in upper triangle
+ realEigenValues.resize(n);
+ if( range == AllValues ) {
+
+ LapackInterface::syev<T>( calcRightEigenVectors, useUpper, n,
+ inputMatrix.data, n, realEigenValues.data, info );
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::ConvergedFailed,
+ "LapackInterface::syev",
+ "off-diagonal elements of intermediate tridiagonal" );
+ }
+ vectorsInMatrix = true;
+ valuesFound = n;
+
+ } else {
+ char rangeChar;
+ if( range == IndexRange ) {
+ rangeChar = 'I';
+ } else {
+ rangeChar = 'V';
+ }
+ LapackInterface::getMachineUnderflow( abstol );
+ abstol *= 0.5;
+ ifail.resize((long)n);
+ symmetricEigenVectors.resize(n*n);
+ LapackInterface::syevx<T>( calcRightEigenVectors, rangeChar,
+ useUpper, n, inputMatrix.data, n, lowValue, hiValue,
+ lowIndex, hiIndex, abstol, valuesFound, realEigenValues.data,
+ symmetricEigenVectors.data, n, ifail.data, info );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::ConvergedFailed,
+ "LapackInterface::syev",
+ "off-diagonal elements of intermediate tridiagonal" );
+ }
+ }
+
+ } else {
+*/
+ complexEigenVectors.resize(n*n);
+ complexEigenValues.resize(n);
+/*
+ LapackInterface::geev<T>( calcLeftEigenVectors, calcRightEigenVectors,
+ n, inputMatrix.data, n, complexEigenValues.data, leftVectors,
+ n, complexEigenVectors.data, n, size, computeLwork, info);
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::ConvergedFailed,
+ "LapackInterface::geev",
+ "QR algorithm" );
+ }
+
+
+ int lwork = LapackInterface::getLWork( size );
+*/
+ int lwork = 2000;
+ valuesFound = n;
+ TypedWorkSpace<T> workSpace(lwork);
+
+ // compute all eigen values and eigen vectors of a general matrix
+
+ LapackInterface::geev<T>( calcLeftEigenVectors, calcRightEigenVectors,
+ n, inputMatrix.data, n, complexEigenValues.data,
+ leftVectors, n, complexEigenVectors.data, n,
+ workSpace.data, lwork, info);
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::ConvergedFailed,
+ "LapackInterface::geev",
+ "QR algorithm" );
+ }
+
+ // }
+ // copy computed eigen values and eigen vectors into caller's arguements
+ if( info != 0 ) {
+ // TODO THROW EXCEPTION
+ }
+ needVectors = false;
+ needValues = false;
+ return;
+}
+template < class T >
+void EigenRep<T>::getAllEigenValuesAndVectors( Vector_<std::complex<RType> >& values, Matrix_<std::complex<RType> >& vectors ) {
+
+ range = AllValues;
+
+ if( needValues ) computeValues( true );
+ copyValues( values );
+ copyVectors( vectors );
+
+ return;
+}
+// only for symmetric real matrix
+template < class T >
+void EigenRep<T>::getAllEigenValuesAndVectors( Vector_<RType>& values, Matrix_<RType>& vectors ) {
+
+ // symmtric matrices return real eigen values, nonsymmetric matrices return complex eigen values
+ if( !CNT<T>::IsPrecision ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getAllEigenValues",
+ "getAllEigenValuesAndVectors(Real, Real) called with for a complex matrix \n");
+ }
+ if( structure.getStructure() != MatrixStructure::Symmetric ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getAllEigenValues",
+ "getAllEigenValuesAndVectors(Real, Real) called with for a non symmetric matrix \n");
+ }
+
+ range = AllValues;
+
+ if( needValues ) computeValues( true );
+ copyValues( values );
+ copyVectors( vectors );
+
+ return;
+}
+// only for symmetric complex matrix
+template < class T >
+void EigenRep<T>::getAllEigenValuesAndVectors( Vector_<RType>& values, Matrix_<std::complex<RType> >& vectors ) {
+
+ // symmtric matrices return real eigen values, nonsymmetric matrices return complex eigen values
+ if( CNT<T>::IsPrecision ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getAllEigenValues",
+ "getAllEigenValuesAndVectors(Real, complex) called for a real matrix \n");
+ }
+ if( structure.getStructure() != MatrixStructure::Symmetric ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getAllEigenValues",
+ "getAllEigenValuesAndVectors(Real, complex) called for a non symmetric matrix \n");
+ }
+
+ range = AllValues;
+
+ if( needValues ) computeValues( true );
+ copyValues( values );
+ copyVectors( vectors );
+
+ return;
+}
+
+template < class T >
+void EigenRep<T>::getAllEigenValues( Vector_<RType>& values ) {
+
+ // symmtric matrices return real eigen values, nonsymmetric matrices return complex eigen values
+ if( structure.getStructure() != MatrixStructure::Symmetric ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getAllEigenValues",
+ "getAllEigenValues called with value of type real for a non symmetric matrix \n");
+ }
+ range = AllValues;
+ if( needValues ) computeValues( false );
+ copyValues( values );
+
+ return;
+}
+template < class T >
+void EigenRep<T>::getAllEigenValues( Vector_<std::complex<RType> >& values ) {
+
+ range = AllValues;
+ if( needValues ) computeValues( false );
+ copyValues( values );
+
+ return;
+}
+template < class T >
+void EigenRep<T>::getFewEigenValuesAndVectors( Vector_<RType>& values, Matrix_<RType>& vectors, typename CNT<T>::TReal rlow, typename CNT<T>::TReal rhi ) {
+
+ // symmtric matrices return real eigen values, nonsymmetric matrices return complex eigen values
+ if( structure.getStructure() != MatrixStructure::Symmetric ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValuesAndVectors",
+ "getFewEigenValuesAndVectors(real, real ) called for a non symmetric matrix \n");
+ }
+ range = ValueRange;
+ lowValue = rlow;
+ hiValue = rhi;
+ if( needValues ) computeValues(true);
+
+ copyValues(values);
+ copyVectors( vectors );
+
+
+ return;
+}
+template < class T >
+void EigenRep<T>::getFewEigenValuesAndVectors( Vector_<RType>& values, Matrix_<std::complex<RType> >& vectors, typename CNT<T>::TReal rlow, typename CNT<T>::TReal rhi ) {
+
+ // symmtric matrices return real eigen values, nonsymmetric matrices return complex eigen values
+ if( structure.getStructure() != MatrixStructure::Symmetric ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValuesAndVectors",
+ "getFewEigenValuesAndVectors(real, complex ) called for a non symmetric matrix \n");
+ }
+ if( !CNT<T>::IsPrecision ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValuesAndVectors",
+ "getFewEigenValuesAndVectors(real, complex ) called for a real matrix \n");
+ }
+ range = ValueRange;
+ lowValue = rlow;
+ hiValue = rhi;
+ if( needValues ) computeValues(true);
+
+ copyValues(values);
+ copyVectors( vectors );
+
+
+ return;
+}
+template < class T >
+void EigenRep<T>::getFewEigenValuesAndVectors( Vector_<std::complex<RType> >& values, Matrix_<std::complex<RType> >& vectors, typename CNT<T>::TReal rlow, typename CNT<T>::TReal rhi ) {
+
+ range = ValueRange;
+ lowValue = rlow;
+ hiValue = rhi;
+ if( needValues ) computeValues(true);
+
+ copyValues(values);
+ copyVectors( vectors );
+
+
+ return;
+}
+
+template < class T >
+void EigenRep<T>::getFewEigenValuesAndVectors( Vector_<RType>& values, Matrix_<RType>& vectors, int ilow, int ihi ) {
+
+ // symmtric matrices return real eigen values, nonsymmetric matrices return complex eigen values
+ if( structure.getStructure() != MatrixStructure::Symmetric ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValuesAndVectors",
+ "getFewEigenValuesAndVectors(real, complex ) called for a non symmetric matrix \n");
+ }
+ if( !CNT<T>::IsPrecision ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValuesAndVectors",
+ "getFewEigenValuesAndVectors(real, real ) called for a complex matrix \n");
+ }
+ range = IndexRange;
+ lowIndex = ilow;
+ hiIndex = ihi;
+ if( needValues ) computeValues( true );
+
+ copyValues(values);
+ copyVectors( vectors );
+
+ return;
+}
+template < class T >
+void EigenRep<T>::getFewEigenValuesAndVectors( Vector_<RType>& values, Matrix_<std::complex<RType> >& vectors, int ilow, int ihi ) {
+
+ // symmtric matrices return real eigen values, nonsymmetric matrices return complex eigen values
+ if( structure.getStructure() != MatrixStructure::Symmetric ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValuesAndVectors",
+ "getFewEigenValuesAndVectors(real, complex ) called for a non symmetric matrix \n");
+ }
+ if( CNT<T>::IsPrecision ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValuesAndVectors",
+ "getFewEigenValuesAndVectors(real, complex ) called for a real matrix \n");
+ }
+ range = IndexRange;
+ lowIndex = ilow;
+ hiIndex = ihi;
+ if( needValues ) computeValues( true );
+
+ copyValues(values);
+ copyVectors( vectors );
+
+ return;
+}
+template < class T >
+void EigenRep<T>::getFewEigenValuesAndVectors( Vector_<std::complex<RType> >& values, Matrix_<std::complex<RType> >& vectors, int ilow, int ihi ) {
+
+ range = IndexRange;
+ lowIndex = ilow;
+ hiIndex = ihi;
+ if( needValues ) computeValues( true );
+
+ copyValues(values);
+ copyVectors( vectors );
+
+ return;
+}
+template < class T >
+void EigenRep<T>::getFewEigenValues( Vector_<RType>& values, typename CNT<T>::TReal rlow, typename CNT<T>::TReal rhi ) {
+
+ // symmetric matrices return real eigen values, nonsymmetric matrices return complex eigen values
+ if( structure.getStructure() != MatrixStructure::Symmetric ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValues",
+ "getFewEigenValues(real) called for a non symmetric matrix \n");
+ }
+ range = ValueRange;
+ lowValue = rlow;
+ hiValue = rhi;
+ if( needValues ) computeValues(false);
+
+ copyValues(values);
+
+ return;
+}
+template < class T >
+void EigenRep<T>::getFewEigenValues( Vector_<std::complex<RType> >& values, typename CNT<T>::TReal rlow, typename CNT<T>::TReal rhi ) {
+
+ range = ValueRange;
+ lowValue = rlow;
+ hiValue = rhi;
+ if( needValues ) computeValues(false);
+
+ copyValues(values);
+
+ return;
+}
+template < class T >
+void EigenRep<T>::getFewEigenValues( Vector_<RType>& values, int ilow, int ihi ) {
+
+ // symmtric matrices return real eigen values, nonsymmetric matrices return complex eigen values
+ if( structure.getStructure() != MatrixStructure::Symmetric ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValues",
+ "getFewEigenValues(real) called for a non symmetric matrix \n");
+ }
+ range = IndexRange;
+ lowIndex = ilow;
+ hiIndex = ihi;
+ if( needValues ) computeValues(false);
+
+ copyValues(values);
+
+ return;
+}
+template < class T >
+void EigenRep<T>::getFewEigenValues( Vector_<std::complex<RType> >& values, int ilow, int ihi ) {
+
+ range = IndexRange;
+ lowIndex = ilow;
+ hiIndex = ihi;
+ if( needValues ) computeValues(false);
+
+ copyValues(values);
+
+ return;
+}
+template < class T >
+void EigenRep<T>::getFewEigenVectors( Matrix_<std::complex<RType> >& vectors, int ilow, int ihi ) {
+
+ range = IndexRange;
+ lowIndex = ilow;
+ hiIndex = ihi;
+ if( needVectors ) computeValues( true );
+ copyVectors( vectors );
+
+ return;
+}
+template < class T >
+void EigenRep<T>::getFewEigenVectors( Matrix_<std::complex<RType> >& vectors, typename CNT<T>::TReal rlow, typename CNT<T>::TReal rhi ) {
+
+ range = ValueRange;
+ lowValue = rlow;
+ hiValue = rhi;
+ if( needVectors ) computeValues( true );
+ copyVectors( vectors );
+
+ return;
+}
+template < class T >
+void EigenRep<T>::getFewEigenVectors( Matrix_<RType>& vectors, int ilow, int ihi ) {
+
+ // symmtric matrices return real eigen values, nonsymmetric matrices return complex eigen values
+ if( structure.getStructure() != MatrixStructure::Symmetric ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValues",
+ "getFewEigenVectors(real) called for a non symmetric matrix \n");
+ }
+ if( !CNT<T>::IsPrecision ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValues",
+ "getFewEigenVectors(real) called for a complex matrix \n");
+ }
+ range = IndexRange;
+ lowIndex = ilow;
+ hiIndex = ihi;
+ if( needVectors ) computeValues( true );
+ copyVectors( vectors );
+
+ return;
+}
+template < class T >
+void EigenRep<T>::getFewEigenVectors( Matrix_<RType>& vectors, RType rlow, RType rhi ) {
+
+ // symmtric matrices return real eigen values, nonsymmetric matrices return complex eigen values
+ if( structure.getStructure() != MatrixStructure::Symmetric ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValues",
+ "getFewEigenVectors(real) called for a non symmetric matrix \n");
+ }
+ if( !CNT<T>::IsPrecision ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValues",
+ "getFewEigenVectors(real) called for a complex matrix \n");
+ }
+ range = ValueRange;
+ lowValue = rlow;
+ hiValue = rhi;
+ if( needVectors ) computeValues( true );
+ copyVectors( vectors );
+
+ return;
+}
+
+// instantiate
+template SimTK_SIMMATH_EXPORT Eigen::Eigen( const Matrix_<double>& m );
+template SimTK_SIMMATH_EXPORT Eigen::Eigen( const Matrix_<float>& m );
+template SimTK_SIMMATH_EXPORT Eigen::Eigen( const Matrix_<std::complex<float> >& m );
+template SimTK_SIMMATH_EXPORT Eigen::Eigen( const Matrix_<std::complex<double> >& m );
+template SimTK_SIMMATH_EXPORT Eigen::Eigen( const Matrix_<conjugate<float> >& m );
+template SimTK_SIMMATH_EXPORT Eigen::Eigen( const Matrix_<conjugate<double> >& m );
+template SimTK_SIMMATH_EXPORT Eigen::Eigen( const Matrix_<negator< double> >& m );
+template SimTK_SIMMATH_EXPORT Eigen::Eigen( const Matrix_<negator< float> >& m );
+template SimTK_SIMMATH_EXPORT Eigen::Eigen( const Matrix_<negator< std::complex<float> > >& m );
+template SimTK_SIMMATH_EXPORT Eigen::Eigen( const Matrix_<negator< std::complex<double> > >& m );
+template SimTK_SIMMATH_EXPORT Eigen::Eigen( const Matrix_<negator< conjugate<float> > >& m );
+template SimTK_SIMMATH_EXPORT Eigen::Eigen( const Matrix_<negator< conjugate<double> > >& m );
+
+//template SimTK_SIMMATH_EXPORT void Eigen::getAllEigenValuesAndVectors<float, float >(Vector_<float>&, Matrix_<float>& );
+//template SimTK_SIMMATH_EXPORT void Eigen::getAllEigenValuesAndVectors<double, double >(Vector_<double>&, Matrix_<double>& );
+//template SimTK_SIMMATH_EXPORT void Eigen::getAllEigenValuesAndVectors<float, std::complex<float> >(Vector_<float>&, Matrix_<std::complex<float> >& );
+//template SimTK_SIMMATH_EXPORT void Eigen::getAllEigenValuesAndVectors<double, std::complex<double> >(Vector_<double>&, Matrix_<std::complex<double> >& );
+template SimTK_SIMMATH_EXPORT void Eigen::getAllEigenValuesAndVectors<std::complex<float>, std::complex<float> >(Vector_<std::complex<float> >&, Matrix_<std::complex<float> >& );
+template SimTK_SIMMATH_EXPORT void Eigen::getAllEigenValuesAndVectors<std::complex<double>, std::complex<double> >(Vector_<std::complex<double> >&, Matrix_<std::complex<double> >& );
+/*
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValuesAndVectors<float, float >(Vector_<float>&, Matrix_<float>&, int ilow, int ihi );
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValuesAndVectors<double, double >(Vector_<double>&, Matrix_<double>&, int ilow, int ihi );
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValuesAndVectors<float, std::complex<float> >(Vector_<float>&, Matrix_<std::complex<float> >&, int ilow, int ihi );
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValuesAndVectors<double, std::complex<double> >(Vector_<double>&, Matrix_<std::complex<double> >&, int ilow, int ihi );
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValuesAndVectors<std::complex<float>, std::complex<float> >(Vector_<std::complex<float> >&, Matrix_<std::complex<float> >&, int ilow, int ihi );
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValuesAndVectors<std::complex<double>, std::complex<double> >(Vector_<std::complex<double> >&, Matrix_<std::complex<double> >&, int ilow, int ihi );
+
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValuesAndVectors<float, float >(Vector_<float>&, Matrix_<float>&, float rlow, float rhi );
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValuesAndVectors<double, double >(Vector_<double>&, Matrix_<double>&, double rlow, double rhi );
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValuesAndVectors<float, std::complex<float> >(Vector_<float>&, Matrix_<std::complex<float> >&, float rlow, float rhi );
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValuesAndVectors<double, std::complex<double> >(Vector_<double>&, Matrix_<std::complex<double> >&, double rlow, double rhi );
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValuesAndVectors<std::complex<float>, std::complex<float> >(Vector_<std::complex<float> >&, Matrix_<std::complex<float> >&, float rlow, float rhi );
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValuesAndVectors<std::complex<double>, std::complex<double> >(Vector_<std::complex<double> >&, Matrix_<std::complex<double> >&, double rlow, double rhi );
+*/
+
+//template SimTK_SIMMATH_EXPORT void Eigen::getAllEigenValues<double>(Vector_<double>& );
+//template SimTK_SIMMATH_EXPORT void Eigen::getAllEigenValues<float>(Vector_<float>& );
+template SimTK_SIMMATH_EXPORT void Eigen::getAllEigenValues<std::complex<double> >(Vector_<std::complex<double> >& );
+template SimTK_SIMMATH_EXPORT void Eigen::getAllEigenValues<std::complex<float> >(Vector_<std::complex<float> >& );
+/*
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValues<double>(Vector_<double>&, int ilow, int ihi);
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValues<float>(Vector_<float>&, int ilow, int ihi );
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValues<std::complex<double> >(Vector_<std::complex<double> >&, int ilow, int ihi );
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValues<std::complex<float> >(Vector_<std::complex<float> >&, int ilow, int ihi );
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValues<double>(Vector_<double>&, double rlow, double rhi);
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValues<float>(Vector_<float>&, float rlow, float rhi );
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValues<std::complex<double> >(Vector_<std::complex<double> >&, double rlow, double rhi );
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenValues<std::complex<float> >(Vector_<std::complex<float> >&, float rlow, float rhi );
+
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenVectors<double>(Matrix_<double>&, int ilow, int ihi);
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenVectors<float>(Matrix_<float>&, int ilow, int ihi );
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenVectors<std::complex<double> >(Matrix_<std::complex<double> >&, int ilow, int ihi );
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenVectors<std::complex<float> >(Matrix_<std::complex<float> >&, int ilow, int ihi );
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenVectors<double>(Matrix_<double>&, double rlow, double rhi);
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenVectors<float>(Matrix_<float>&, float rlow, float rhi );
+template SimTK_SIMMATH_EXPORT void Eigen::getFewEigenVectors<std::complex<double> >(Matrix_<std::complex<double> >&, double rlow, double rhi );
+*/
+
+template class EigenRep<double>;
+template EigenRep<double>::EigenRep( const Matrix_<double>& m);
+template EigenRep<double>::EigenRep( const Matrix_<negator<double> >& m);
+
+template class EigenRep<float>;
+template EigenRep<float>::EigenRep( const Matrix_<float>& m );
+template EigenRep<float>::EigenRep( const Matrix_<negator<float> >& m );
+
+template class EigenRep<std::complex<double> >;
+template EigenRep<std::complex<double> >::EigenRep( const Matrix_<std::complex<double> >& m );
+template EigenRep<std::complex<double> >::EigenRep( const Matrix_<negator<std::complex<double> > >& m );
+template EigenRep<std::complex<double> >::EigenRep( const Matrix_<conjugate<double> >& m );
+template EigenRep<std::complex<double> >::EigenRep( const Matrix_<negator<conjugate<double> > >& m );
+
+template class EigenRep<std::complex<float> >;
+template EigenRep<std::complex<float> >::EigenRep( const Matrix_<std::complex<float> >& m );
+template EigenRep<std::complex<float> >::EigenRep( const Matrix_<negator<std::complex<float> > >& m );
+template EigenRep<std::complex<float> >::EigenRep( const Matrix_<conjugate<float> >& m );
+template EigenRep<std::complex<float> >::EigenRep( const Matrix_<negator<conjugate<float> > >& m );
+
+} // namespace SimTK
diff --git a/SimTKmath/LinearAlgebra/src/EigenRep.h b/SimTKmath/LinearAlgebra/src/EigenRep.h
new file mode 100644
index 0000000..f6ec5da
--- /dev/null
+++ b/SimTKmath/LinearAlgebra/src/EigenRep.h
@@ -0,0 +1,318 @@
+#ifndef SimTK_SIMMATH_EIGEN_REP_H_
+#define SimTK_SIMMATH_EIGEN_REP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 "WorkSpace.h"
+
+namespace SimTK {
+
+class EigenRepBase {
+ public:
+
+ virtual ~EigenRepBase(){};
+
+ virtual EigenRepBase* clone() const { return 0; };
+
+ virtual void getAllEigenValuesAndVectors( Vector_<float>& values, Matrix_<float>& vectors ){
+ checkIfFactored( "getAllEigenValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getAllEigenValuesAndVectors",
+ "getAllEigenValuesAndVectors called with vector of type <float, float> \n");
+ }
+ virtual void getAllEigenValuesAndVectors( Vector_<double>& values, Matrix_<double>& vectors ){
+ checkIfFactored( "getAllEigenValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getAllEigenValuesAndVectors",
+ "getAllEigenValuesAndVectors called with vector of type <double, double> \n");
+ }
+ virtual void getAllEigenValuesAndVectors( Vector_<double>& values, Matrix_<std::complex<double> >& vectors ){
+ checkIfFactored( "getAllEigenValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getAllEigenValuesAndVectors",
+ "getAllEigenValuesAndVectors called with vector of types <double, std::complex<double> \n");
+ }
+ virtual void getAllEigenValuesAndVectors( Vector_<std::complex<double> >& values, Matrix_<std::complex<double> >& vectors ){
+ checkIfFactored( "getAllEigenValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getAllEigenValuesAndVectors",
+ "getAllEigenValuesAndVectors called with vector of type <std::complex<double>, std::complex<double> > \n");
+ }
+ virtual void getAllEigenValuesAndVectors( Vector_<float>& values, Matrix_<std::complex<float> >& vectors ){
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getAllEigenValuesAndVectors",
+ "getAllEigenValuesAndVectors called with vector of type <float, std::complex<float> > \n");
+ }
+ virtual void getAllEigenValuesAndVectors( Vector_<std::complex<float> >& values, Matrix_<std::complex<float> >& vectors ){
+ checkIfFactored( "getAllEigenValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getAllEigenValuesAndVectors",
+ "getAllEigenValuesAndVectors called with vector of type <std::complex<float, std::complex<float> > \n");
+ }
+
+ virtual void getAllEigenValues( Vector_<float>& values ){
+ checkIfFactored( "getAllEigenValues" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getAllEigenValues",
+ "getAllEigenValues called with vector of type <float> \n");
+ }
+ virtual void getAllEigenValues( Vector_<double>& values ){
+ checkIfFactored( "getAllEigenValues" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getAllEigenValues",
+ "getAllEigenValues called with vector of type <double> \n");
+ }
+ virtual void getAllEigenValues( Vector_<std::complex<double> >& values){
+ checkIfFactored( "getAllEigenValues" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getAllEigenValues",
+ "getAllEigenValues called with vector of type <std::complex<double> \n");
+ }
+ virtual void getAllEigenValues( Vector_<std::complex<float> >& values ){
+ checkIfFactored( "getAllEigenValues" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getAllEigenValues",
+ "getAllEigenValues called with vector of type <std::complex<float> \n");
+ }
+ virtual void getFewEigenValuesAndVectors( Vector_<float>& values, Matrix_<float>& vectors, int ilow, int ihi ){
+ checkIfFactored( "getFewEigenValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValuesAndVectors",
+ "getFewEigenValuesAndVectors called with vector of type <float,float> \n");
+ }
+ virtual void getFewEigenValuesAndVectors( Vector_<double>& values, Matrix_<double>& vectors, int ilow, int ihi ){
+ checkIfFactored( "getFewEigenValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValuesAndVectors",
+ "getFewEigenValuesAndVectors called with vector of type <double,double> \n");
+ }
+ virtual void getFewEigenValuesAndVectors( Vector_<std::complex<float> >& values, Matrix_<std::complex<float> >& vectors, int ilow, int ihi ){
+ checkIfFactored( "getFewEigenValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValuesAndVectors",
+ "getFewEigenValuesAndVectors called with vector of type <std::complex<float>, std::complex<float> > \n");
+ }
+ virtual void getFewEigenValuesAndVectors( Vector_<float>& values, Matrix_<std::complex<float> >& vectors, int ilow, int ihi ){
+ checkIfFactored( "getFewEigenValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValuesAndVectors",
+ "getFewEigenValuesAndVectors called with vector of type <float, std::complex<float> > \n");
+ }
+ virtual void getFewEigenValuesAndVectors( Vector_<std::complex<double> >& values, Matrix_<std::complex<double> >& vectors, int ilow, int ihi ){
+ checkIfFactored( "getFewEigenValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValuesAndVectors",
+ "getFewEigenValuesAndVectors called with vector of type <std::complex<double>, std::complex<double> > \n");
+ }
+ virtual void getFewEigenValuesAndVectors( Vector_<double>& values, Matrix_<std::complex<double> >& vectors, int ilow, int ihi ){
+ checkIfFactored( "getFewEigenValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValuesAndVectors",
+ "getFewEigenValuesAndVectors called with vector of type < double, std::complex<double> > \n");
+ }
+ virtual void getFewEigenValuesAndVectors( Vector_<float>& values, Matrix_<float>& vectors, float rlow, float rhi ){
+ checkIfFactored( "getFewEigenValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValuesAndVectors",
+ "getFewEigenValuesAndVectors called with vector of type <float, float> \n");
+ }
+ virtual void getFewEigenValuesAndVectors( Vector_<double>& values, Matrix_<double>& vectors, double rlow, double rhi ){
+ checkIfFactored( "getFewEigenValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValuesAndVectors",
+ "getFewEigenValuesAndVectors called with vector of type <double, double> \n");
+ }
+ virtual void getFewEigenValuesAndVectors( Vector_<std::complex<float> >& values, Matrix_<std::complex<float> >& vectors, float rlow, float rhi ){
+ checkIfFactored( "getFewEigenValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValuesAndVectors",
+ "getFewEigenValuesAndVectors called with vector of type <std::complex<float>, std::complex<float> > \n");
+ }
+ virtual void getFewEigenValuesAndVectors( Vector_<float>& values, Matrix_<std::complex<float> >& vectors, float rlow, float rhi ){
+ checkIfFactored( "getFewEigenValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValuesAndVectors",
+ "getFewEigenValuesAndVectors called with vector of type <float, std::complex<float> > \n");
+ }
+ virtual void getFewEigenValuesAndVectors( Vector_<std::complex<double> >& values, Matrix_<std::complex<double> >& vectors, double rlow, double rhi ){
+ checkIfFactored( "getFewEigenValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValuesAndVectors",
+ "getFewEigenValuesAndVectors called with vector of type <std::complex<double>, std::complex<double> > \n");
+ }
+ virtual void getFewEigenValuesAndVectors( Vector_<double>& values, Matrix_<std::complex<double> >& vectors, double rlow, double rhi ){
+ checkIfFactored( "getFewEigenValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValuesAndVectors",
+ "getFewEigenValuesAndVectors called with vector of type <double, std::complex<double> > \n");
+ }
+ virtual void getFewEigenValues( Vector_<float>& values, int ilow, int ihi ){
+ checkIfFactored( "getFewEigenValues" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValues",
+ "getFewEigenValues called with vector of type <float> \n");
+ }
+ virtual void getFewEigenValues( Vector_<double>& values, int ilow, int ihi ){
+ checkIfFactored( "getFewEigenValues" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValues",
+ "getFewEigenValues called with vector of type <double> \n");
+ }
+ virtual void getFewEigenValues( Vector_<std::complex<float> >& values, int ilow, int ihi ){
+ checkIfFactored( "getFewEigenValues" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValues",
+ "getFewEigenValues called with vector of type <std::complex<float> > \n");
+ }
+ virtual void getFewEigenValues( Vector_<std::complex<double> >& values, int ilow, int ihi ){
+ checkIfFactored( "getFewEigenValues" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValues",
+ "getFewEigenValues called with vector of type <std::complex<double> > \n");
+ }
+ virtual void getFewEigenValues( Vector_<float>& values, float rlow, float rhi ){
+ checkIfFactored( "getFewEigenValues" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValues",
+ "getFewEigenValues called with vector of type <float> \n");
+ }
+ virtual void getFewEigenValues( Vector_<double>& values, double rlow, double rhi ){
+ checkIfFactored( "getFewEigenValues" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValues",
+ "getFewEigenValues called with vector of type <double> \n");
+ }
+ virtual void getFewEigenValues( Vector_<std::complex<float> >& values, float rlow, float rhi ){
+ checkIfFactored( "getFewEigenValues" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValues",
+ "getFewEigenValues called with vector of type <std::complex<float> > \n");
+ }
+ virtual void getFewEigenValues( Vector_<std::complex<double> >& values, double rlow, double rhi ){
+ checkIfFactored( "getFewEigenValues" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenValues",
+ "getFewEigenValues called with vector of type <std::complex<double> > \n");
+ }
+ virtual void getFewEigenVectors( Matrix_<float>& vectors, int ilow, int ihi ){
+ checkIfFactored( "getFewEigenVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenVectors",
+ "getFewEigenVectors called with vector of type <float> \n");
+ }
+ virtual void getFewEigenVectors( Matrix_<double>& vectors, int ilow, int ihi ){
+ checkIfFactored( "getFewEigenVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenVectors",
+ "getFewEigenVectors called with vector of type <double> \n");
+ }
+ virtual void getFewEigenVectors( Matrix_<std::complex<float> >& vectors, int ilow, int ihi ){
+ checkIfFactored( "getFewEigenVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenVectors",
+ "getFewEigenVectors called with vector of type <std::complex<float> > \n");
+ }
+ virtual void getFewEigenVectors( Matrix_<std::complex<double> >& vectors, int ilow, int ihi ){
+ checkIfFactored( "getFewEigenVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenVectors",
+ "getFewEigenVectors called with vector of type <std::complex<double> > \n");
+ }
+ virtual void getFewEigenVectors( Matrix_<float>& vectors, float rlow, float rhi ){
+ checkIfFactored( "getFewEigenVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenVectors",
+ "getFewEigenVectors called with vector of type <float> \n");
+ }
+ virtual void getFewEigenVectors( Matrix_<double>& vectors, double rlow, double rhi ){
+ checkIfFactored( "getFewEigenVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenVectors",
+ "getFewEigenVectors called with vector of type <double> \n");
+ }
+ virtual void getFewEigenVectors( Matrix_<std::complex<float> >& vectors, float rlow, float rhi ){
+ checkIfFactored( "getFewEigenVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenVectors",
+ "getFewEigenVectors called with vector of type <std::complex<float> > \n");
+ }
+ virtual void getFewEigenVectors( Matrix_<std::complex<double> >& vectors, double rlow, double rhi ){
+ checkIfFactored( "getFewEigenVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen","getFewEigenVectors",
+ "getFewEigenVectors called with vector of type <std::complex<double> > \n");
+ }
+ bool isFactored;
+
+ private:
+ void checkIfFactored(const char* function_name) const {
+ if( !isFactored ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Eigen",function_name,
+ "matrix has not been factored\n");
+ }
+
+ return;
+ }
+
+
+}; // class EigenRepBase
+
+class EigenDefault : public EigenRepBase {
+ public:
+ EigenDefault();
+ EigenRepBase* clone() const;
+};
+
+
+template <typename T>
+class EigenRep : public EigenRepBase {
+ public:
+ template <class ELT> EigenRep( const Matrix_<ELT>& );
+
+ enum EigenRange {
+ AllValues = 0,
+ IndexRange = 1,
+ ValueRange = 2
+ };
+
+ ~EigenRep();
+ EigenRepBase* clone() const;
+
+ typedef typename CNT<T>::TReal RType;
+// template <class VAL, class VEC> void getAllEigenValuesAndVectors( Vector_<VAL>& values, Matrix_<VEC>& vectors);
+ void getAllEigenValuesAndVectors( Vector_<std::complex<RType> >& values, Matrix_<std::complex<RType> >& vectors);
+ void getAllEigenValuesAndVectors( Vector_<RType>& values, Matrix_<RType>& vectors);
+ void getAllEigenValuesAndVectors( Vector_<RType>& values, Matrix_<std::complex<RType> >& vectors);
+ void getAllEigenValues( Vector_<RType>& values);
+ void getAllEigenValues( Vector_<std::complex<RType> >& values);
+
+ void getFewEigenValuesAndVectors( Vector_<RType>& values, Matrix_<RType>& vectors, int ilow, int ihi);
+ void getFewEigenValuesAndVectors( Vector_<std::complex<RType> >& values, Matrix_<std::complex<RType> >& vectors, int ilow, int ihi);
+ void getFewEigenValuesAndVectors( Vector_<RType>& values, Matrix_<std::complex<RType> >& vectors, int ilow, int ihi);
+ void getFewEigenVectors( Matrix_<RType>& vectors, int ilow, int ihi );
+ void getFewEigenVectors( Matrix_<std::complex<RType> >& vectors, int ilow, int ihi );
+ void getFewEigenValues( Vector_<RType>& values, int ilow, int ihi );
+ void getFewEigenValues( Vector_<std::complex<RType> >& values, int ilow, int ihi );
+
+ void getFewEigenValuesAndVectors( Vector_<RType>& values, Matrix_<RType>& vectors, RType rlow, RType ihi);
+ void getFewEigenValuesAndVectors( Vector_<std::complex<RType> >& values, Matrix_<std::complex<RType> >& vectors, RType rlow, RType ihi);
+ void getFewEigenValuesAndVectors( Vector_<RType>& values, Matrix_<std::complex<RType> >& vectors, RType rlow, RType ihi);
+ void getFewEigenVectors( Matrix_<RType>& vectors, RType rlow, RType ihi );
+ void getFewEigenVectors( Matrix_<std::complex<RType> >& vectors, RType rlow, RType ihi );
+ void getFewEigenValues( Vector_<RType>& values, RType rlow, RType ihi );
+ void getFewEigenValues( Vector_<std::complex<RType> >& values, RType rlow, RType ihi );
+
+ private:
+
+ void computeValues(bool);
+
+ template <typename P> void copyVectors( Matrix_<P>& vectors );
+ void copyValues( Vector_<float>& values );
+ void copyValues( Vector_<double>& values );
+ void copyValues( Vector_<std::complex<float> >& values );
+ void copyValues( Vector_<std::complex<double> >& values );
+
+ int n;
+ int lowIndex, hiIndex; // min and max indexes for computing a few eigenvalues
+ RType lowValue, hiValue; // min and max values for computing a few eigenvalues
+ RType abstol; // convergence tolerance used by interative eigen routines
+ bool needValues; // true if eigen values need to be computed
+ bool needVectors; // true if eigen vectors need to be computed
+ bool vectorsInMatrix; // true if eigen vectors stored in inputMatrix
+ int valuesFound; // number of eigen values found when computing few values
+ EigenRange range;
+ MatrixStructure structure;
+ TypedWorkSpace<T> inputMatrix;
+ TypedWorkSpace< RType> realEigenValues;
+ TypedWorkSpace< std::complex<RType> > complexEigenValues;
+ TypedWorkSpace< std::complex<RType> > complexEigenVectors;
+ TypedWorkSpace<int> ifail; // indexes of eigenvalues that did not converge
+ TypedWorkSpace<T> symmetricEigenVectors;
+
+}; // end class EigenRep
+} // namespace SimTK
+#endif // SimTK_SIMMATH_EIGEN_REP_H_
diff --git a/SimTKmath/LinearAlgebra/src/Factor.cpp b/SimTKmath/LinearAlgebra/src/Factor.cpp
new file mode 100644
index 0000000..69f0d39
--- /dev/null
+++ b/SimTKmath/LinearAlgebra/src/Factor.cpp
@@ -0,0 +1,399 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ *
+ * Factors systems of linear algebra equations.
+ */
+
+
+#include "SimTKcommon.h"
+
+#include "simmath/internal/common.h"
+#include "simmath/LinearAlgebra.h"
+
+#include "LapackInterface.h"
+#include "LATraits.h"
+#include "FactorRep.h"
+#include "WorkSpace.h"
+#include "LapackConvert.h"
+
+#include <iostream>
+#include <cmath>
+#include <complex>
+
+
+namespace SimTK {
+
+ //////////////////////
+ // FactorLUDefault //
+ //////////////////////
+FactorLUDefault::FactorLUDefault() {
+ isFactored = false;
+}
+FactorLURepBase* FactorLUDefault::clone() const {
+ return( new FactorLUDefault(*this));
+}
+
+
+ ///////////////
+ // FactorLU //
+ ///////////////
+FactorLU::~FactorLU() {
+ delete rep;
+}
+// default constructor
+FactorLU::FactorLU() {
+ rep = new FactorLUDefault();
+}
+
+// copy constructor
+FactorLU::FactorLU( const FactorLU& c ) {
+ rep = c.rep->clone();
+}
+// copy assignment operator
+FactorLU& FactorLU::operator=(const FactorLU& rhs) {
+ rep = rhs.rep->clone();
+ return *this;
+}
+
+template <typename ELT>
+void FactorLU::inverse( Matrix_<ELT>& inverse ) const {
+ rep->inverse( inverse );
+}
+
+
+template < class ELT >
+FactorLU::FactorLU( const Matrix_<ELT>& m ) {
+ rep = new FactorLURep<typename CNT<ELT>::StdNumber>(m);
+}
+
+template < class ELT >
+void FactorLU::factor( const Matrix_<ELT>& m ) {
+ delete rep;
+ rep = new FactorLURep<typename CNT<ELT>::StdNumber>(m);
+}
+
+template < typename ELT >
+void FactorLU::solve( const Vector_<ELT>& b, Vector_<ELT>& x ) const {
+ rep->solve( b, x );
+ return;
+}
+template < class ELT >
+void FactorLU::solve( const Matrix_<ELT>& b, Matrix_<ELT>& x ) const {
+ rep->solve( b, x );
+ return;
+}
+
+template < class ELT >
+void FactorLU::getL( Matrix_<ELT>& m) const {
+ rep->getL( m );
+ return;
+}
+template < class ELT >
+void FactorLU::getU( Matrix_<ELT>& m) const {
+ rep->getU( m );
+ return;
+}
+/*
+template < class ELT >
+void FactorLU::getD( Matrix_<ELT>& m) const {
+ rep->getD( m );
+ return;
+}
+*/
+
+/* implement in future release ?
+Real FactorLU::getConditionNumber() const {
+ return( rep->getConditionNumber() );
+}
+template < class ELT >
+void FactorLU::getErrorBounds (Vector_<ELT>& err, Vector_<ELT>& berr) const {
+ rep->getErrorBounds( err, berr );
+ return;
+}
+*/
+
+bool FactorLU::isSingular () const {
+ return( rep->isSingular() );
+}
+
+int FactorLU::getSingularIndex () const {
+ return( rep->getSingularIndex() );
+}
+ /////////////////
+ // FactorLURep //
+ /////////////////
+template <typename T >
+ template < typename ELT >
+FactorLURep<T>::FactorLURep( const Matrix_<ELT>& mat )
+ : nRow( mat.nrow() ),
+ nCol( mat.ncol() ),
+ mn( (mat.nrow() < mat.ncol()) ? mat.nrow() : mat.ncol() ),
+ singularIndex(0),
+ pivots(mat.ncol()),
+ lu( mat.nrow()*mat.ncol() )
+{
+ FactorLURep<T>::factor( mat );
+}
+template <typename T >
+FactorLURep<T>::FactorLURep()
+ : nRow(0),
+ nCol(0),
+ mn(0),
+ singularIndex(0),
+ pivots(0),
+ lu(0)
+{
+}
+template <typename T >
+FactorLURep<T>::~FactorLURep() {}
+
+template <typename T >
+FactorLURepBase* FactorLURep<T>::clone() const {
+ return( new FactorLURep<T>(*this) );
+}
+
+template < class T >
+void FactorLURep<T>::inverse( Matrix_<T>& inverse ) const {
+ Matrix_<T> iden(mn,mn);
+ iden.resize(mn,mn);
+ iden = 1.0;
+ solve( iden, inverse );
+}
+
+template < class T >
+void FactorLURep<T>::solve( const Vector_<T>& b, Vector_<T> &x ) const {
+
+ SimTK_APIARGCHECK2_ALWAYS(b.size()==nRow,"FactorLU","solve",
+ "number of rows in right hand side=%d does not match number of rows in original matrix=%d \n",
+ b.size(), nRow );
+
+ x.copyAssign(b);
+/* TODO after 1.0
+ if( structure == MatrixStructures::Symmetric ) {
+ if( condition == MatrixConditions::PositiveDefinite ) {
+ LapackInterface::potrs<T>( 'L', nCol, 1, lu.data, &x(0) );
+ } else {
+ LapackInterface::sytrs<T>( 'L', nCol, 1, lu.data, pivots.data, &x(0) );
+ }
+ } else {
+*/
+ LapackInterface::getrs<T>( 'N', nCol, 1, lu.data, pivots.data, &x(0) );
+// }
+
+ return;
+}
+template <typename T >
+void FactorLURep<T>::solve( const Matrix_<T>& b, Matrix_<T>& x ) const {
+
+ SimTK_APIARGCHECK2_ALWAYS(b.nrow()==nRow,"FactorLU","solve",
+ "number of rows in right hand side=%d does not match number of rows in original matrix=%d \n",
+ b.nrow(), nRow );
+
+ x.copyAssign(b);
+
+/* TODO after 1.0
+ if( structure == MatrixStructures::Symmetric ) {
+ const char uplo = 'L';
+ if( condition == MatrixConditions::PositiveDefinite ) {
+ LapackInterface::potrs<T>( uplo, nCol, b.ncol(), lu.data, &x(0,0) );
+ } else {
+ LapackInterface::sytrs<T>( uplo, nCol, b.ncol(), lu.data, pivots.data, &x(0,0) );
+ }
+ } else {
+*/
+ const char trans = 'N';
+ LapackInterface::getrs<T>( trans, nCol, b.ncol(), lu.data, pivots.data, &x(0,0) );
+// }
+
+ return;
+}
+template <typename T >
+void FactorLURep<T>::getL( Matrix_<T>& m) const {
+ int i,j;
+
+ m.resize( nRow, nCol );
+
+ for(i=0;i<nRow;i++) {
+ for(j=0;j<i;j++) m(j,i) = 0.0;
+ for(j=0;j<nCol;j++) m(j,i) = lu.data[j*nRow+i];
+ }
+
+ return;
+}
+template <typename T >
+void FactorLURep<T>::getU( Matrix_<T>& m) const {
+ int i,j;
+ m.resize( nRow, nCol );
+
+ for(i = 0;i<nRow;i++) {
+ for(j=0;j<i+1;j++) m(j,i) = lu.data[j*nRow+i];
+ for(;j<nCol;j++) m(j,i) = 0.0;
+ }
+ return;
+}
+template <typename T >
+void FactorLURep<T>::getD( Matrix_<T>& m) const {
+ int i,j;
+
+
+ m.resize( nRow, nCol );
+ for(i = 0;i<nRow;i++) {
+ for(j=0;j<nCol;j++) m(j,i) = 0.0;
+ }
+ for(i = 0;i<nRow;i++) m(i,i) = lu.data[i*nRow+i];
+
+ return;
+}
+template <typename T >
+Real FactorLURep<T>::getConditionNumber() const {
+ return 0.0;
+}
+template <typename T >
+void FactorLURep<T>::getErrorBounds ( Vector_<T>& err, Vector_<T>& berr) const {
+ return;
+}
+template <typename T >
+bool FactorLURep<T>::isSingular () const {
+
+ if( singularIndex > 0)
+ return( true );
+ else
+ return( false );
+}
+template <typename T >
+int FactorLURep<T>::getSingularIndex () const {
+ return( singularIndex );
+}
+
+template <class T>
+ template<typename ELT>
+void FactorLURep<T>::factor(const Matrix_<ELT>&mat ) {
+
+ SimTK_APIARGCHECK2_ALWAYS(mat.nelt() > 0,"FactorLU","factor",
+ "Can't factor a matrix that has a zero dimension -- got %d X %d.",
+ (int)mat.nrow(), (int)mat.ncol());
+
+ // initialize the matrix we pass to LAPACK
+ // converts (negated,conjugated etc.) to LAPACK format
+ LapackConvert::convertMatrixToLapack( lu.data, mat );
+
+
+ int lda = nRow;
+ int info;
+
+ LapackInterface::getrf<T>(nRow, nCol, lu.data, lda, pivots.data, info);
+ if( info > 0 )
+ singularIndex = info; // matrix is singular info = i when U(i,i) is exactly zero
+ else
+ singularIndex = 0;
+
+}
+
+// instantiate
+template SimTK_SIMMATH_EXPORT FactorLU::FactorLU( const Matrix_<double>& m );
+template SimTK_SIMMATH_EXPORT FactorLU::FactorLU( const Matrix_<float>& m );
+template SimTK_SIMMATH_EXPORT FactorLU::FactorLU( const Matrix_<std::complex<float> >& m );
+template SimTK_SIMMATH_EXPORT FactorLU::FactorLU( const Matrix_<std::complex<double> >& m );
+template SimTK_SIMMATH_EXPORT FactorLU::FactorLU( const Matrix_<conjugate<float> >& m );
+template SimTK_SIMMATH_EXPORT FactorLU::FactorLU( const Matrix_<conjugate<double> >& m );
+template SimTK_SIMMATH_EXPORT FactorLU::FactorLU( const Matrix_<negator< double> >& m );
+template SimTK_SIMMATH_EXPORT FactorLU::FactorLU( const Matrix_<negator< float> >& m );
+template SimTK_SIMMATH_EXPORT FactorLU::FactorLU( const Matrix_<negator< std::complex<float> > >& m );
+template SimTK_SIMMATH_EXPORT FactorLU::FactorLU( const Matrix_<negator< std::complex<double> > >& m );
+template SimTK_SIMMATH_EXPORT FactorLU::FactorLU( const Matrix_<negator< conjugate<float> > >& m );
+template SimTK_SIMMATH_EXPORT FactorLU::FactorLU( const Matrix_<negator< conjugate<double> > >& m );
+
+template SimTK_SIMMATH_EXPORT void FactorLU::factor( const Matrix_<double>& m );
+template SimTK_SIMMATH_EXPORT void FactorLU::factor( const Matrix_<float>& m );
+template SimTK_SIMMATH_EXPORT void FactorLU::factor( const Matrix_<std::complex<float> >& m );
+template SimTK_SIMMATH_EXPORT void FactorLU::factor( const Matrix_<std::complex<double> >& m );
+template SimTK_SIMMATH_EXPORT void FactorLU::factor( const Matrix_<conjugate<float> >& m );
+template SimTK_SIMMATH_EXPORT void FactorLU::factor( const Matrix_<conjugate<double> >& m );
+template SimTK_SIMMATH_EXPORT void FactorLU::factor( const Matrix_<negator< double> >& m );
+template SimTK_SIMMATH_EXPORT void FactorLU::factor( const Matrix_<negator< float> >& m );
+template SimTK_SIMMATH_EXPORT void FactorLU::factor( const Matrix_<negator< std::complex<float> > >& m );
+template SimTK_SIMMATH_EXPORT void FactorLU::factor( const Matrix_<negator< std::complex<double> > >& m );
+template SimTK_SIMMATH_EXPORT void FactorLU::factor( const Matrix_<negator< conjugate<float> > >& m );
+template SimTK_SIMMATH_EXPORT void FactorLU::factor( const Matrix_<negator< conjugate<double> > >& m );
+
+template class FactorLURep<double>;
+template FactorLURep<double>::FactorLURep( const Matrix_<double>& m);
+template FactorLURep<double>::FactorLURep( const Matrix_<negator<double> >& m);
+template void FactorLURep<double>::factor( const Matrix_<double>& m);
+template void FactorLURep<double>::factor( const Matrix_<negator<double> >& m);
+
+template class FactorLURep<float>;
+template FactorLURep<float>::FactorLURep( const Matrix_<float>& m);
+template FactorLURep<float>::FactorLURep( const Matrix_<negator<float> >& m);
+template void FactorLURep<float>::factor( const Matrix_<float>& m);
+template void FactorLURep<float>::factor( const Matrix_<negator<float> >& m);
+
+template class FactorLURep<std::complex<double> >;
+template FactorLURep<std::complex<double> >::FactorLURep( const Matrix_<std::complex<double> >& m);
+template FactorLURep<std::complex<double> >::FactorLURep( const Matrix_<negator<std::complex<double> > >& m);
+template FactorLURep<std::complex<double> >::FactorLURep( const Matrix_<conjugate<double> >& m);
+template FactorLURep<std::complex<double> >::FactorLURep( const Matrix_<negator<conjugate<double> > >& m);
+template void FactorLURep<std::complex<double> >::factor( const Matrix_<std::complex<double> >& m);
+template void FactorLURep<std::complex<double> >::factor( const Matrix_<negator<std::complex<double> > >& m);
+template void FactorLURep<std::complex<double> >::factor( const Matrix_<conjugate<double> >& m);
+template void FactorLURep<std::complex<double> >::factor( const Matrix_<negator<conjugate<double> > >& m);
+
+template class FactorLURep<std::complex<float> >;
+template FactorLURep<std::complex<float> >::FactorLURep( const Matrix_<std::complex<float> >& m);
+template FactorLURep<std::complex<float> >::FactorLURep( const Matrix_<negator<std::complex<float> > >& m);
+template FactorLURep<std::complex<float> >::FactorLURep( const Matrix_<conjugate<float> >& m);
+template FactorLURep<std::complex<float> >::FactorLURep( const Matrix_<negator<conjugate<float> > >& m);
+template void FactorLURep<std::complex<float> >::factor( const Matrix_<std::complex<float> >& m);
+template void FactorLURep<std::complex<float> >::factor( const Matrix_<negator<std::complex<float> > >& m);
+template void FactorLURep<std::complex<float> >::factor( const Matrix_<conjugate<float> >& m);
+template void FactorLURep<std::complex<float> >::factor( const Matrix_<negator<conjugate<float> > >& m);
+
+template SimTK_SIMMATH_EXPORT void FactorLU::getL<float>(Matrix_<float>&) const;
+template SimTK_SIMMATH_EXPORT void FactorLU::getL<double>(Matrix_<double>&) const;
+template SimTK_SIMMATH_EXPORT void FactorLU::getL<std::complex<float> >(Matrix_<std::complex<float> >&) const;
+template SimTK_SIMMATH_EXPORT void FactorLU::getL<std::complex<double> >(Matrix_<std::complex<double> >&) const;
+//template SimTK_SIMMATH_EXPORT void FactorLU::getD<float>(Matrix_<float>&) const;
+//template SimTK_SIMMATH_EXPORT void FactorLU::getD<double>(Matrix_<double>&) const;
+//template SimTK_SIMMATH_EXPORT void FactorLU::getD<std::complex<float> >(Matrix_<std::complex<float> >&) const;
+//template SimTK_SIMMATH_EXPORT void FactorLU::getD<std::complex<double> >(Matrix_<std::complex<double> >&) const;
+template SimTK_SIMMATH_EXPORT void FactorLU::getU<float>(Matrix_<float>&) const;
+template SimTK_SIMMATH_EXPORT void FactorLU::getU<double>(Matrix_<double>&) const;
+template SimTK_SIMMATH_EXPORT void FactorLU::getU<std::complex<float> >(Matrix_<std::complex<float> >&) const;
+template SimTK_SIMMATH_EXPORT void FactorLU::getU<std::complex<double> >(Matrix_<std::complex<double> >&) const;
+template SimTK_SIMMATH_EXPORT void FactorLU::solve<float>(const Vector_<float>&, Vector_<float>&) const;
+template SimTK_SIMMATH_EXPORT void FactorLU::solve<double>(const Vector_<double>&, Vector_<double>&) const;
+template SimTK_SIMMATH_EXPORT void FactorLU::solve<std::complex<float> >(const Vector_<std::complex<float> >&, Vector_<std::complex<float> >&) const;
+template SimTK_SIMMATH_EXPORT void FactorLU::solve<std::complex<double> >(const Vector_<std::complex<double> >&, Vector_<std::complex<double> >&) const;
+template SimTK_SIMMATH_EXPORT void FactorLU::solve<float>(const Matrix_<float>&, Matrix_<float>&) const;
+template SimTK_SIMMATH_EXPORT void FactorLU::solve<double>(const Matrix_<double>&, Matrix_<double>&) const;
+template SimTK_SIMMATH_EXPORT void FactorLU::solve<std::complex<float> >(const Matrix_<std::complex<float> >&, Matrix_<std::complex<float> >&) const;
+template SimTK_SIMMATH_EXPORT void FactorLU::solve<std::complex<double> >(const Matrix_<std::complex<double> >&, Matrix_<std::complex<double> >&) const;
+template SimTK_SIMMATH_EXPORT void FactorLU::inverse<float>(Matrix_<float>&) const;
+template SimTK_SIMMATH_EXPORT void FactorLU::inverse<double>(Matrix_<double>&) const;
+template SimTK_SIMMATH_EXPORT void FactorLU::inverse<std::complex<float> >(Matrix_<std::complex<float> >&) const;
+template SimTK_SIMMATH_EXPORT void FactorLU::inverse<std::complex<double> >(Matrix_<std::complex<double> >&) const;
+
+
+} // namespace SimTK
diff --git a/SimTKmath/LinearAlgebra/src/FactorQTZ.cpp b/SimTKmath/LinearAlgebra/src/FactorQTZ.cpp
new file mode 100644
index 0000000..8a41824
--- /dev/null
+++ b/SimTKmath/LinearAlgebra/src/FactorQTZ.cpp
@@ -0,0 +1,530 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: Michael Sherman *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ *
+ * Factors systems of linear algebra equations.
+ */
+
+#include "SimTKcommon.h"
+
+#include "simmath/internal/common.h"
+#include "simmath/LinearAlgebra.h"
+
+#include "LapackInterface.h"
+#include "FactorQTZRep.h"
+#include "WorkSpace.h"
+#include "LATraits.h"
+#include "LapackConvert.h"
+#include "SimTKcommon.h"
+
+#include <iostream>
+#include <cmath>
+#include <complex>
+
+
+namespace SimTK {
+
+ //////////////////////
+ // FactorQTZDefault //
+ //////////////////////
+FactorQTZDefault::FactorQTZDefault() {
+ isFactored = false;
+}
+FactorQTZRepBase* FactorQTZDefault::clone() const {
+ return( new FactorQTZDefault(*this));
+}
+
+ ///////////////
+ // FactorQTZ //
+ ///////////////
+FactorQTZ::~FactorQTZ() {
+ delete rep;
+}
+// default constructor
+FactorQTZ::FactorQTZ() {
+ rep = new FactorQTZDefault();
+}
+// copy constructor
+FactorQTZ::FactorQTZ( const FactorQTZ& c ) {
+ rep = c.rep->clone();
+}
+// copy assignment operator
+FactorQTZ& FactorQTZ::operator=(const FactorQTZ& rhs) {
+ rep = rhs.rep->clone();
+ return *this;
+}
+
+template <typename ELT>
+void FactorQTZ::inverse( Matrix_<ELT>& inverse ) const {
+ rep->inverse( inverse );
+}
+template < class ELT >
+void FactorQTZ::factor( const Matrix_<ELT>& m ){
+ delete rep;
+
+ // if user does not supply rcond set it to max(nRow,nCol)*(eps)^7/8 (similar to matlab)
+ int mnmax = (m.nrow() > m.ncol()) ? m.nrow() : m.ncol();
+ rep = new FactorQTZRep<typename CNT<ELT>::StdNumber>(m, mnmax*NTraits<typename CNT<ELT>::Precision>::getSignificant());
+}
+template < class ELT >
+void FactorQTZ::factor( const Matrix_<ELT>& m, double rcond ){
+ delete rep;
+ rep = new FactorQTZRep<typename CNT<ELT>::StdNumber>(m, rcond );
+}
+template < class ELT >
+void FactorQTZ::factor( const Matrix_<ELT>& m, float rcond ){
+ delete rep;
+ rep = new FactorQTZRep<typename CNT<ELT>::StdNumber>(m, rcond );
+}
+template < class ELT >
+FactorQTZ::FactorQTZ( const Matrix_<ELT>& m ) {
+
+ // if user does not supply rcond set it to max(nRow,nCol)*(eps)^7/8 (similar to matlab)
+ int mnmax = (m.nrow() > m.ncol()) ? m.nrow() : m.ncol();
+ rep = new FactorQTZRep<typename CNT<ELT>::StdNumber>(m, mnmax*NTraits<typename CNT<ELT>::Precision>::getSignificant());
+}
+template < class ELT >
+FactorQTZ::FactorQTZ( const Matrix_<ELT>& m, double rcond ) {
+ rep = new FactorQTZRep<typename CNT<ELT>::StdNumber>(m, rcond);
+}
+template < class ELT >
+FactorQTZ::FactorQTZ( const Matrix_<ELT>& m, float rcond ) {
+ rep = new FactorQTZRep<typename CNT<ELT>::StdNumber>(m, rcond);
+}
+
+int FactorQTZ::getRank() const {
+ return(rep->rank);
+}
+double FactorQTZ::getRCondEstimate() const {
+ return (rep->actualRCond);
+}
+template < typename ELT >
+void FactorQTZ::solve( const Vector_<ELT>& b, Vector_<ELT>& x ) const {
+ rep->solve( b, x );
+ return;
+}
+template < class ELT >
+void FactorQTZ::solve( const Matrix_<ELT>& b, Matrix_<ELT>& x ) const {
+ rep->solve( b, x );
+ return;
+}
+
+ /////////////////
+ // FactorQTZRep //
+ /////////////////
+template <typename T >
+FactorQTZRep<T>::FactorQTZRep()
+: mn(0),
+ maxmn(0),
+ nRow(0),
+ nCol(0),
+ scaleLinSys(false),
+ linSysScaleF(NTraits<typename CNT<T>::Precision>::getNaN()),
+ anrm(NTraits<typename CNT<T>::Precision>::getNaN()),
+ rcond(NTraits<typename CNT<T>::Precision>::getSignificant()),
+ pivots(0),
+ qtz(0),
+ tauGEQP3(0),
+ tauORMQR(0)
+{
+}
+
+template <typename T >
+ template < typename ELT >
+FactorQTZRep<T>::FactorQTZRep( const Matrix_<ELT>& mat, typename CNT<T>::TReal rc)
+: mn( (mat.nrow() < mat.ncol()) ? mat.nrow() : mat.ncol() ),
+ maxmn( (mat.nrow() > mat.ncol()) ? mat.nrow() : mat.ncol() ),
+ nRow( mat.nrow() ),
+ nCol( mat.ncol() ),
+ scaleLinSys(false),
+ linSysScaleF(NTraits<typename CNT<T>::Precision>::getNaN()),
+ anrm(NTraits<typename CNT<T>::Precision>::getNaN()),
+ rcond(rc),
+ pivots(mat.ncol()),
+ qtz( mat.nrow()*mat.ncol() ),
+ tauGEQP3(mn),
+ tauORMQR(mn)
+{
+ for(int i=0; i<mat.ncol(); ++i)
+ pivots.data[i] = 0;
+ FactorQTZRep<T>::factor( mat );
+ isFactored = true;
+}
+
+template <typename T >
+FactorQTZRepBase* FactorQTZRep<T>::clone() const {
+ return( new FactorQTZRep<T>(*this) );
+}
+
+template <typename T >
+FactorQTZRep<T>::~FactorQTZRep() {}
+
+template < class T >
+void FactorQTZRep<T>::solve( const Vector_<T>& b, Vector_<T> &x ) const {
+
+ SimTK_APIARGCHECK_ALWAYS(isFactored ,"FactorQTZ","solve",
+ "No matrix was passed to FactorQTZ. \n" );
+
+ SimTK_APIARGCHECK2_ALWAYS(b.size()==nRow,"FactorQTZ","solve",
+ "number of rows in right hand side=%d does not match number of rows in original matrix=%d \n",
+ b.size(), nRow );
+
+
+ Matrix_<T> m(maxmn,1);
+
+ for(int i=0;i<b.size();i++) {
+ m(i,0) = b(i);
+ }
+ Matrix_<T> r(nCol, 1 );
+ doSolve( m, r );
+ x.copyAssign(r);
+}
+
+template <typename T >
+void FactorQTZRep<T>::solve( const Matrix_<T>& b, Matrix_<T>& x ) const {
+ SimTK_APIARGCHECK_ALWAYS(isFactored ,"FactorQTZ","solve",
+ "No matrix was passed to FactorQTZ. \n" );
+
+ SimTK_APIARGCHECK2_ALWAYS(b.nrow()==nRow,"FactorQTZ","solve",
+ "number of rows in right hand side=%d does not match number of rows in original matrix=%d \n",
+ b.nrow(), nRow );
+
+ x.resize(nCol, b.ncol() );
+ Matrix_<T> tb;
+ tb.resize(maxmn, b.ncol() );
+ for(int j=0;j<b.ncol();j++) for(int i=0;i<b.nrow();i++) tb(i,j) = b(i,j);
+ doSolve(tb, x);
+}
+
+template <typename T >
+void FactorQTZRep<T>::doSolve(Matrix_<T>& b, Matrix_<T>& x) const {
+ int info;
+ typedef typename CNT<T>::TReal RealType;
+ RealType bnrm, smlnum, bignum;
+ int nrhs = b.ncol();
+ int n = nCol;
+ int m = nRow;
+ typename CNT<T>::TReal rhsScaleF; // scale factor applied to right hand side
+ bool scaleRHS = false; // true if right hand side should be scaled
+
+ if (rank == 0) return;
+
+ // Ask the experts for their optimal workspace sizes. The size parameters
+ // here must match the calls below.
+ T workSz;
+ LapackInterface::ormqr<T>('L', 'T', nRow, b.ncol(), mn, 0, nRow,
+ 0, 0, b.nrow(), &workSz, -1, info );
+ const int lwork1 = (int)NTraits<T>::real(workSz);
+
+ LapackInterface::ormrz<T>('L', 'T', nCol, b.ncol(), rank, nCol-rank,
+ 0, nRow, 0, 0,
+ b.nrow(), &workSz, -1, info );
+ const int lwork2 = (int)NTraits<T>::real(workSz);
+
+ TypedWorkSpace<T> work(std::max(lwork1, lwork2));
+
+ // compute norm of RHS
+ bnrm = (RealType)LapackInterface::lange<T>('M', m, nrhs, &b(0,0), b.nrow());
+
+ LapackInterface::getMachinePrecision<RealType>(smlnum, bignum);
+
+ // compute scale for RHS
+ if (bnrm > Zero && bnrm < smlnum) {
+ scaleRHS = true;
+ rhsScaleF = smlnum;
+ } else if (bnrm > bignum) {
+ scaleRHS = true;
+ rhsScaleF = bignum;
+ }
+
+
+ if (scaleRHS) { // apply scale factor to RHS
+ LapackInterface::lascl<T>('G', 0, 0, bnrm, rhsScaleF, b.nrow(), nrhs,
+ &b(0,0), b.nrow(), info );
+ }
+ // b1 = Q'*b0
+ LapackInterface::ormqr<T>('L', 'T', nRow, b.ncol(), mn, qtz.data,
+ nRow, tauGEQP3.data, &b(0,0), b.nrow(),
+ work.data, work.size, info );
+ // b2 = T^-1*b1 = T^-1 * Q' * b0
+ LapackInterface::trsm<T>('L', 'U', 'N', 'N', rank, b.ncol(), 1.0,
+ qtz.data, nRow, &b(0,0), b.nrow() );
+
+ // zero out elements of RHS for rank deficient systems
+ for(int j = 0; j<nrhs; ++j) {
+ for(int i = rank; i<n; ++i)
+ b(i,j) = 0;
+ }
+
+ if (rank < nCol) {
+ // b3 = Z'*b2 = Z'*T^-1*Q'*b0
+ LapackInterface::ormrz<T>('L', 'T', nCol, b.ncol(), rank, nCol-rank,
+ qtz.data, nRow, tauORMQR.data, &b(0,0),
+ b.nrow(), work.data, work.size, info );
+ }
+
+ // adjust for pivoting
+ Array_<T> b_pivot(n);
+ for(int j = 0; j<nrhs; ++j) {
+ for(int i = 0; i<n; ++i)
+ b_pivot[pivots.data[i]-1] = b(i,j);
+
+ LapackInterface::copy<T>(n, b_pivot.begin(), 1, &x(0,j), 1 );
+ }
+
+ // compensate for scaling of linear system
+ if (scaleLinSys) {
+ LapackInterface::lascl<T>('g', 0, 0, anrm, linSysScaleF, nCol, x.ncol(),
+ &x(0,0), nCol, info );
+ }
+
+ // compensate for scaling of RHS
+ if (scaleRHS) {
+ LapackInterface::lascl<T>('g', 0, 0, bnrm, rhsScaleF, nCol, x.ncol(),
+ &x(0,0), nCol, info);
+ }
+}
+
+
+template < class T >
+void FactorQTZRep<T>::inverse( Matrix_<T>& inverse ) const {
+ Matrix_<T> iden(mn,mn);
+ inverse.resize(mn,mn);
+ iden = 1.0;
+ doSolve( iden, inverse );
+
+ return;
+}
+
+template <class T>
+ template<typename ELT>
+void FactorQTZRep<T>::factor(const Matrix_<ELT>&mat ) {
+ SimTK_APIARGCHECK2_ALWAYS(mat.nelt() > 0,"FactorQTZ","factor",
+ "Can't factor a matrix that has a zero dimension -- got %d X %d.",
+ (int)mat.nrow(), (int)mat.ncol());
+
+
+ // allocate and initialize the matrix we pass to LAPACK
+ // converts (negated,conjugated etc.) to LAPACK format
+ LapackConvert::convertMatrixToLapack( qtz.data, mat );
+
+ int info;
+ const int smallestSingularValue = 2;
+ const int largestSingularValue = 1;
+ typedef typename CNT<T>::TReal RealType;
+ RealType smlnum, bignum, smin, smax;
+
+ if (mat.nelt() == 0) return;
+
+
+ // Compute optimal size for work space for dtzrzf and dgepq3. The
+ // arguments here should match the calls below, although we'll use maxRank
+ // rather than rank since we don't know the rank yet.
+ T workSz;
+ const int maxRank = std::min(nRow, nCol);
+ LapackInterface::tzrzf<T>(maxRank, nCol, 0, nRow, 0, &workSz, -1, info);
+ const int lwork1 = (int)NTraits<T>::real(workSz);
+
+ LapackInterface::geqp3<T>(nRow, nCol, 0, nRow, 0, 0, &workSz, -1, info);
+ const int lwork2 = (int)NTraits<T>::real(workSz);
+
+ TypedWorkSpace<T> work(std::max(lwork1, lwork2));
+
+ LapackInterface::getMachinePrecision<RealType>( smlnum, bignum);
+
+ // scale the input system of equations
+ anrm = (RealType)LapackInterface::lange<T>('M', nRow, nCol, qtz.data, nRow);
+
+ if (anrm > 0 && anrm < smlnum) {
+ scaleLinSys = true;
+ linSysScaleF = smlnum;
+ } else if( anrm > bignum ) {
+ scaleLinSys = true;
+ linSysScaleF = bignum;
+ }
+ if (anrm == 0) { // matrix all zeros
+ rank = 0;
+ } else {
+ if (scaleLinSys) {
+ LapackInterface::lascl<T>('G', 0, 0, anrm, linSysScaleF, nRow, nCol,
+ qtz.data, nRow, info );
+ }
+
+ // compute QR factorization with column pivoting: A = Q * R
+ // Q * R is returned in qtz.data
+ LapackInterface::geqp3<T>(nRow, nCol, qtz.data, nRow, pivots.data,
+ tauGEQP3.data, work.data, work.size, info );
+
+ // compute Rank
+
+ smax = CNT<T>::abs( qtz.data[0] );
+ smin = smax;
+ if( CNT<T>::abs(qtz.data[0]) == 0 ) {
+ rank = 0;
+ actualRCond = 0;
+ } else {
+ T s1,s2,c1,c2;
+ RealType smaxpr,sminpr;
+
+ // Determine rank using incremental condition estimate
+ Array_<T> xSmall(mn), xLarge(mn); // temporaries
+ xSmall[0] = xLarge[0] = 1;
+ for (rank=1,smaxpr=0.0,sminpr=1.0;
+ rank<mn && smaxpr*rcond < sminpr; )
+ {
+ LapackInterface::laic1<T>(smallestSingularValue, rank,
+ xSmall.begin(), smin, &qtz.data[rank*nRow],
+ qtz.data[(rank*nRow)+rank], sminpr, s1, c1);
+
+ LapackInterface::laic1<T>(largestSingularValue, rank,
+ xLarge.begin(), smax, &qtz.data[rank*nRow],
+ qtz.data[(rank*nRow)+rank], smaxpr, s2, c2);
+
+ if (smaxpr*rcond < sminpr) {
+ for(int i=0; i<rank; i++) {
+ xSmall[i] *= s1;
+ xLarge[i] *= s2;
+ }
+ xSmall[rank] = c1;
+ xLarge[rank] = c2;
+ smin = sminpr;
+ smax = smaxpr;
+ actualRCond = (double)(smin/smax);
+ rank++;
+ }
+ }
+
+ // R => T * Z
+ // Matrix is rank deficient so complete the orthogonalization by
+ // applying orthogonal transformations from the right to
+ // factor R from an upper trapezoidal to an upper triangular matrix
+ // T is returned in qtz.data and Z is returned in tauORMQR.data
+ if (rank < nCol) {
+ LapackInterface::tzrzf<T>(rank, nCol, qtz.data, nRow,
+ tauORMQR.data, work.data,
+ work.size, info);
+ }
+ }
+ }
+}
+
+// instantiate
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<double>& m );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<float>& m );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<std::complex<float> >& m );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<std::complex<double> >& m );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<conjugate<float> >& m );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<conjugate<double> >& m );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<negator< double> >& m );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<negator< float> >& m );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<negator< std::complex<float> > >& m );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<negator< std::complex<double> > >& m );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<negator< conjugate<float> > >& m );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<negator< conjugate<double> > >& m );
+
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<double>& m, double rcond );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<float>& m, float rcond );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<std::complex<float> >& m, float rcond );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<std::complex<double> >& m, double rcond );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<conjugate<float> >& m, float rcond );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<conjugate<double> >& m, double rcond );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<negator< double> >& m, double rcond );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<negator< float> >& m, float rcond );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<negator< std::complex<float> > >& m, float rcond );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<negator< std::complex<double> > >& m, double rcond );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<negator< conjugate<float> > >& m, float rcond );
+template SimTK_SIMMATH_EXPORT FactorQTZ::FactorQTZ( const Matrix_<negator< conjugate<double> > >& m, double rcond );
+
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<double>& m );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<float>& m );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<std::complex<float> >& m );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<std::complex<double> >& m );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<conjugate<float> >& m );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<conjugate<double> >& m );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<negator< double> >& m );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<negator< float> >& m );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<negator< std::complex<float> > >& m );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<negator< std::complex<double> > >& m );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<negator< conjugate<float> > >& m );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<negator< conjugate<double> > >& m );
+
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<double>& m, double rcond );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<float>& m, float rcond );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<std::complex<float> >& m, float rcond );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<std::complex<double> >& m, double rcond );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<conjugate<float> >& m, float rcond );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<conjugate<double> >& m, double rcond );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<negator< double> >& m, double rcond );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<negator< float> >& m, float rcond );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<negator< std::complex<float> > >& m, float rcond );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<negator< std::complex<double> > >& m, double rcond );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<negator< conjugate<float> > >& m, float rcond );
+template SimTK_SIMMATH_EXPORT void FactorQTZ::factor( const Matrix_<negator< conjugate<double> > >& m, double rcond );
+
+template class FactorQTZRep<double>;
+template FactorQTZRep<double>::FactorQTZRep( const Matrix_<double>& m, double rcond);
+template FactorQTZRep<double>::FactorQTZRep( const Matrix_<negator<double> >& m, double rcond);
+template void FactorQTZRep<double>::factor( const Matrix_<double>& m);
+template void FactorQTZRep<double>::factor( const Matrix_<negator<double> >& m);
+
+template class FactorQTZRep<float>;
+template FactorQTZRep<float>::FactorQTZRep( const Matrix_<float>& m, float rcond );
+template FactorQTZRep<float>::FactorQTZRep( const Matrix_<negator<float> >& m, float rcond );
+template void FactorQTZRep<float>::factor( const Matrix_<float>& m);
+template void FactorQTZRep<float>::factor( const Matrix_<negator<float> >& m);
+
+template class FactorQTZRep<std::complex<double> >;
+template FactorQTZRep<std::complex<double> >::FactorQTZRep( const Matrix_<std::complex<double> >& m, double rcond);
+template FactorQTZRep<std::complex<double> >::FactorQTZRep( const Matrix_<negator<std::complex<double> > >& m, double rcond);
+template FactorQTZRep<std::complex<double> >::FactorQTZRep( const Matrix_<conjugate<double> >& m, double rcond);
+template FactorQTZRep<std::complex<double> >::FactorQTZRep( const Matrix_<negator<conjugate<double> > >& m, double rcond);
+template void FactorQTZRep<std::complex<double> >::factor( const Matrix_<std::complex<double> >& m);
+template void FactorQTZRep<std::complex<double> >::factor( const Matrix_<negator<std::complex<double> > >& m);
+template void FactorQTZRep<std::complex<double> >::factor( const Matrix_<conjugate<double> >& m);
+template void FactorQTZRep<std::complex<double> >::factor( const Matrix_<negator<conjugate<double> > >& m);
+
+template class FactorQTZRep<std::complex<float> >;
+template FactorQTZRep<std::complex<float> >::FactorQTZRep( const Matrix_<std::complex<float> >& m, float rcond);
+template FactorQTZRep<std::complex<float> >::FactorQTZRep( const Matrix_<negator<std::complex<float> > >& m, float rcond);
+template FactorQTZRep<std::complex<float> >::FactorQTZRep( const Matrix_<conjugate<float> >& m, float rcond);
+template FactorQTZRep<std::complex<float> >::FactorQTZRep( const Matrix_<negator<conjugate<float> > >& m, float rcond);
+template void FactorQTZRep<std::complex<float> >::factor( const Matrix_<std::complex<float> >& m);
+template void FactorQTZRep<std::complex<float> >::factor( const Matrix_<negator<std::complex<float> > >& m);
+template void FactorQTZRep<std::complex<float> >::factor( const Matrix_<conjugate<float> >& m);
+template void FactorQTZRep<std::complex<float> >::factor( const Matrix_<negator<conjugate<float> > >& m);
+
+template SimTK_SIMMATH_EXPORT void FactorQTZ::solve<float>(const Vector_<float>&, Vector_<float>&) const;
+template SimTK_SIMMATH_EXPORT void FactorQTZ::solve<double>(const Vector_<double>&, Vector_<double>&) const;
+template SimTK_SIMMATH_EXPORT void FactorQTZ::solve<std::complex<float> >(const Vector_<std::complex<float> >&, Vector_<std::complex<float> >&) const;
+template SimTK_SIMMATH_EXPORT void FactorQTZ::solve<std::complex<double> >(const Vector_<std::complex<double> >&, Vector_<std::complex<double> >&) const;
+template SimTK_SIMMATH_EXPORT void FactorQTZ::solve<float>(const Matrix_<float>&, Matrix_<float>&) const;
+template SimTK_SIMMATH_EXPORT void FactorQTZ::solve<double>(const Matrix_<double>&, Matrix_<double>&) const;
+template SimTK_SIMMATH_EXPORT void FactorQTZ::solve<std::complex<float> >(const Matrix_<std::complex<float> >&, Matrix_<std::complex<float> >&) const;
+template SimTK_SIMMATH_EXPORT void FactorQTZ::solve<std::complex<double> >(const Matrix_<std::complex<double> >&, Matrix_<std::complex<double> >&) const;
+template SimTK_SIMMATH_EXPORT void FactorQTZ::inverse<float>(Matrix_<float>&) const;
+template SimTK_SIMMATH_EXPORT void FactorQTZ::inverse<double>(Matrix_<double>&) const;
+template SimTK_SIMMATH_EXPORT void FactorQTZ::inverse<std::complex<float> >(Matrix_<std::complex<float> >&) const;
+template SimTK_SIMMATH_EXPORT void FactorQTZ::inverse<std::complex<double> >(Matrix_<std::complex<double> >&) const;
+
+} // namespace SimTK
diff --git a/SimTKmath/LinearAlgebra/src/FactorQTZRep.h b/SimTKmath/LinearAlgebra/src/FactorQTZRep.h
new file mode 100644
index 0000000..64a80a8
--- /dev/null
+++ b/SimTKmath/LinearAlgebra/src/FactorQTZRep.h
@@ -0,0 +1,152 @@
+#ifndef SimTK_SIMMATH_FACTOR_QTZ_REP_H_
+#define SimTK_SIMMATH_FACTOR_QTZ_REP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 "WorkSpace.h"
+
+namespace SimTK {
+
+class FactorQTZRepBase {
+public:
+ FactorQTZRepBase() : isFactored(false), rank(0), actualRCond(0) {}
+
+ virtual ~FactorQTZRepBase(){};
+
+ virtual FactorQTZRepBase* clone() const { return 0; };
+ virtual void solve( const Vector_<float>& b, Vector_<float>& x ) const {
+ checkIfFactored();
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorQTZ","solve",
+ "solve called with rhs of type <float> which does not match type of original linear system \n");
+ }
+ virtual void solve( const Vector_<double>& b, Vector_<double>& x ) const {
+ checkIfFactored();
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorQTZ","solve",
+ "solve called with rhs of type <double> which does not match type of original linear system \n");
+ }
+ virtual void solve( const Vector_<std::complex<float> >& b, Vector_<std::complex<float> >& x ) const {
+ checkIfFactored();
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorQTZ","solve",
+ "solve called with rhs of type complex<float> which does not match type of original linear system \n");
+ }
+ virtual void solve( const Vector_<std::complex<double> >& b, Vector_<std::complex<double> >& x ) const {
+ checkIfFactored();
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorQTZ","solve",
+ "solve called with rhs of type complex<double> which does not match type of original linear system \n");
+ }
+ virtual void solve( const Matrix_<float>& b, Matrix_<float>& x ) const {
+ checkIfFactored();
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorQTZ","solve",
+ "solve called with rhs of type <float> which does not match type of original linear system \n");
+ }
+ virtual void solve( const Matrix_<double>& b, Matrix_<double>& x ) const {
+ checkIfFactored();
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorQTZ","solve",
+ "solve called with rhs of type <double> which does not match type of original linear system \n");
+ }
+ virtual void solve( const Matrix_<std::complex<float> >& b, Matrix_<std::complex<float> >& x ) const {
+ checkIfFactored();
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorQTZ","solve",
+ "solve called with rhs of type complex<float> which does not match type of original linear system \n");
+ }
+ virtual void solve ( const Matrix_<std::complex<double> >& b, Matrix_<std::complex<double> >& x ) const {
+ checkIfFactored();
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorQTZ","solve",
+ "solve called with rhs of type complex<double> which does not match type of original linear system \n");
+ }
+ virtual void inverse( Matrix_<double>& inverse ) const {
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorQTZ","inverse",
+ "inverse( <double> ) called with type that is inconsistant with the original matrix \n");
+ }
+ virtual void inverse( Matrix_<float>& inverse ) const {
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorQTZ","inverse",
+ "inverse( <float> ) called with type that is inconsistant with the original matrix \n");
+ }
+ virtual void inverse( Matrix_<std::complex<float> >& inverse ) const {
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorQTZ","inverse",
+ "inverse( std::complex<float> ) called with type that is inconsistant with the original matrix \n");
+ }
+ virtual void inverse( Matrix_<std::complex<double> >& inverse ) const {
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorQTZ","inverse",
+ "inverse( std::complex<double> ) called with type that is inconsistant with the original matrix \n");
+ }
+
+
+ bool isFactored;
+ int rank; // esitmated rank computed during factorization
+ double actualRCond; // 1 / condition number we actually got (est.)
+
+
+ void checkIfFactored() const {
+ if( !isFactored ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorQTZ","solve",
+ "solve called before the matrix was factored \n");
+ }
+ }
+
+}; // class FactorQTZRepBase
+
+class FactorQTZDefault : public FactorQTZRepBase {
+ public:
+ FactorQTZDefault();
+ FactorQTZRepBase* clone() const;
+};
+
+template <typename T>
+class FactorQTZRep : public FactorQTZRepBase {
+public:
+ template <class ELT> FactorQTZRep( const Matrix_<ELT>&, typename CNT<T>::TReal );
+ FactorQTZRep();
+
+ ~FactorQTZRep();
+
+ template < class ELT > void factor(const Matrix_<ELT>& );
+ void inverse( Matrix_<T>& ) const;
+ void solve( const Vector_<T>& b, Vector_<T>& x ) const;
+ void solve( const Matrix_<T>& b, Matrix_<T>& x ) const;
+
+ FactorQTZRepBase* clone() const;
+
+private:
+ void doSolve( Matrix_<T>& b, Matrix_<T>& x ) const;
+
+ int mn; // min of number of rows or columns
+ int maxmn; // max of number of rows or columns
+ int nRow; // number of rows in original matrix
+ int nCol; // number of columns in original matrix
+ bool scaleLinSys; // true if matrix was scaled during factorization
+ typename CNT<T>::TReal linSysScaleF; // scale factor applied to matrix
+ typename CNT<T>::TReal anrm;
+ typename CNT<T>::TReal rcond; // 1 / (largest acceptable condition number)
+ TypedWorkSpace<int> pivots;
+ TypedWorkSpace<T> qtz; // factored matrix
+ TypedWorkSpace<T> tauGEQP3;
+ TypedWorkSpace<T> tauORMQR;
+
+}; // end class FactorQTZRep
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_FACTOR_QTZ_REP_H_
diff --git a/SimTKmath/LinearAlgebra/src/FactorRep.h b/SimTKmath/LinearAlgebra/src/FactorRep.h
new file mode 100644
index 0000000..81bb107
--- /dev/null
+++ b/SimTKmath/LinearAlgebra/src/FactorRep.h
@@ -0,0 +1,234 @@
+#ifndef SimTK_SIMMATH_FACTOR_REP_H_
+#define SimTK_SIMMATH_FACTOR_REP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 "WorkSpace.h"
+
+namespace SimTK {
+
+class FactorLURepBase {
+ public:
+
+ virtual ~FactorLURepBase(){};
+ virtual FactorLURepBase* clone() const { return 0; };
+
+ virtual void solve( const Vector_<float>& b, Vector_<float>& x ) const {
+ checkIfFactored("solve");
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","solve",
+ "solve called with rhs of type <float> which does not match type of original linear system \n");
+ }
+ virtual void solve( const Vector_<double>& b, Vector_<double>& x ) const {
+ checkIfFactored("solve");
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","solve",
+ " solve called with rhs of type <double> which does not match type of original linear system \n");
+ }
+ virtual void solve( const Vector_<std::complex<float> >& b, Vector_<std::complex<float> >& x ) const {
+ checkIfFactored("solve");
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","solve",
+ " solve called with rhs of type complex<float> which does not match type of original linear system \n");
+ }
+ virtual void solve( const Vector_<std::complex<double> >& b, Vector_<std::complex<double> >& x ) const {
+ checkIfFactored("solve");
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","solve",
+ " solve called with rhs of type complex<double> which does not match type of original linear system \n");
+ }
+ virtual void solve( const Matrix_<float>& b, Matrix_<float>& x ) const {
+ checkIfFactored("solve");
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","solve",
+ " solve called with rhs of type <float> which does not match type of original linear system \n");
+ }
+ virtual void solve( const Matrix_<double>& b, Matrix_<double>& x ) const {
+ checkIfFactored("solve");
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","solve",
+ " solve called with rhs of type <double> which does not match type of original linear system \n");
+ }
+ virtual void solve( const Matrix_<std::complex<float> >& b, Matrix_<std::complex<float> >& x ) const {
+ checkIfFactored("solve");
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","solve",
+ " solve called with rhs of type complex<float> which does not match type of original linear system \n");
+ }
+ virtual void solve( const Matrix_<std::complex<double> >& b, Matrix_<std::complex<double> >& x ) const {
+ checkIfFactored("solve");
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","solve",
+ " solve called with rhs of type complex<double> which does not match type of original linear system \n");
+ }
+ virtual void getL( Matrix_<float>& l) const{
+ checkIfFactored( "getL" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","getL",
+ " getL called with L of type <float> which does not match type of original linear system \n");
+ }
+ virtual void getL( Matrix_<double>& l ) const{
+ checkIfFactored( "getL" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","getL",
+ " getL called with L of type <double> which does not match type of original linear system \n");
+ }
+ virtual void getL( Matrix_<std::complex<float> >& l) const{
+ checkIfFactored( "getL" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","getL",
+ " getL called with L of type complex<float> which does not match type of original linear system \n");
+ }
+ virtual void getL( Matrix_<std::complex<double> >& l ) const{
+ checkIfFactored( "getL" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","getL",
+ " getL called with L of type complex<double> which does not match type of original linear system \n");
+ }
+ virtual void getU( Matrix_<float>& u) const{
+ checkIfFactored( "getU" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","getU",
+ " getU called with U of type <float> which does not match type of original linear system \n");
+ }
+ virtual void getU( Matrix_<double>& u ) const{
+ checkIfFactored( "getU" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","getU",
+ " getU called with U of type <double> which does not match type of original linear system \n");
+ }
+ virtual void getU( Matrix_<std::complex<float> >& u) const{
+ checkIfFactored( "getU" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","getU",
+ " getU called with U of type complex<float> which does not match type of original linear system \n");
+ }
+ virtual void getU( Matrix_<std::complex<double> >& u ) const{
+ checkIfFactored( "getU" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","getU",
+ " getU called with U of type complex<double> which does not match type of original linear system \n");
+ }
+ virtual void getD( Matrix_<float>& d) const{
+ checkIfFactored( "getD" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","getD",
+ " getD called with D of type <float> which does not match type of original linear system \n");
+ }
+ virtual void getD( Matrix_<double>& d ) const{
+ checkIfFactored( "getD" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","getD",
+ " getD called with D of type <double> which does not match type of original linear system \n");
+ }
+ virtual void getD( Matrix_<std::complex<float> >& d) const{
+ checkIfFactored( "getD" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","getD",
+ " getD called with D of type complex<float> which does not match type of original linear system \n");
+ }
+ virtual void getD( Matrix_<std::complex<double> >& d ) const{
+ checkIfFactored( "getD" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","getD",
+ " getD called with D of type complex<double> which does not match type of original linear system \n");
+ }
+ virtual void inverse( Matrix_<double>& inverse ) const{
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","inverse", "inverse( <double> ) called with type that is inconsistant with the original matrix \n");
+ }
+ virtual void inverse( Matrix_<float>& inverse ) const{
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","inverse", "inverse( <float> ) called with type that is inconsistant with the original matrix \n");
+ }
+ virtual void inverse( Matrix_<std::complex<float> >& inverse ) const{
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","inverse", "inverse( std::complex<float> ) called with type that is inconsistant with the original matrix \n");
+ }
+ virtual void inverse( Matrix_<std::complex<double> >& inverse ) const{
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","inverse",
+ "inverse( std::complex<double> ) called with type that is inconsistant with the original matrix \n");
+ }
+
+ virtual bool isSingular() const{ return false;};
+ virtual int getSingularIndex() const{ return 1; };
+ virtual Real getConditionNumber() const{ return 0.0;};
+
+ virtual void getErrorBounds( const Vector_<float>& err, Vector_<float>& berr ){
+ checkIfFactored( "getErrorBounds" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","getErrorBounds",
+ " getErrorBounds called with rhs arguments type <float> which does not match type of original linear system \n");
+ }
+ virtual void getErrorBounds( const Vector_<double>& err, Vector_<float>& berr ){
+ checkIfFactored( "getErrorBounds" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","getErrorBounds",
+ " getErrorBounds called with arguments of type <double> which does not match type of original linear system \n");
+ }
+ virtual void getErrorBounds( const Vector_<std::complex<float> >& err, Vector_<float>& berr ){
+ checkIfFactored( "getErrorBounds" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","getErrorBounds",
+ " getErrorBounds called with arguments of type complex<float> which does not match type of original linear system \n");
+ }
+ virtual void getErrorBounds( const Vector_<std::complex<double> >& err, Vector_<float>& berr ){
+ checkIfFactored( "getErrorBounds" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU","getErrorBounds",
+ " getErrorBounds called with arguments of type complex<double> which does not match type of original linear system \n");
+ }
+ bool isFactored;
+
+ private:
+ void checkIfFactored( const char* function_name) const {
+ if( !isFactored ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorLU",function_name,
+ "matrix has not been factored \n");
+ }
+
+ return;
+ }
+
+
+
+}; // class FactorLURepBase
+
+class FactorLUDefault : public FactorLURepBase {
+ public:
+ FactorLUDefault();
+ FactorLURepBase* clone() const;
+
+};
+
+template <typename T>
+class FactorLURep : public FactorLURepBase {
+ public:
+ template <class ELT> FactorLURep( const Matrix_<ELT>& );
+ FactorLURep();
+
+ ~FactorLURep();
+ FactorLURepBase* clone() const;
+
+ template < class ELT > void factor(const Matrix_<ELT>& );
+ void solve( const Vector_<T>& b, Vector_<T>& x ) const;
+ void solve( const Matrix_<T>& b, Matrix_<T>& x ) const;
+ void inverse( Matrix_<T>& m ) const;
+
+ void getL( Matrix_<T>& l ) const;
+ void getU( Matrix_<T>& u ) const;
+ void getD( Matrix_<T>& d ) const;
+ void getErrorBounds( Vector_<T>& err, Vector_<T>& berr) const;
+ Real getConditionNumber() const;
+ bool isSingular() const;
+ int getSingularIndex() const;
+
+ private:
+
+// factored matrix stored in LAPACK LU format
+ template < class ELT> int getType(ELT*);
+ int nRow;
+ int nCol;
+ int mn; // min(m,n)
+ int singularIndex;
+
+ TypedWorkSpace<int> pivots;
+ TypedWorkSpace<T> lu;
+}; // end class FactorLURep
+} // namespace SimTK
+#endif // SimTK_SIMMATH_FACTOR_REP_H_
diff --git a/SimTKmath/LinearAlgebra/src/FactorSVD.cpp b/SimTKmath/LinearAlgebra/src/FactorSVD.cpp
new file mode 100644
index 0000000..046244c
--- /dev/null
+++ b/SimTKmath/LinearAlgebra/src/FactorSVD.cpp
@@ -0,0 +1,431 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ *
+ * Solves for singular values and singular vectors
+ */
+
+
+#include "SimTKcommon.h"
+
+#include "simmath/internal/common.h"
+#include "simmath/LinearAlgebra.h"
+
+#include "LapackInterface.h"
+#include "FactorSVDRep.h"
+#include "WorkSpace.h"
+#include "LATraits.h"
+#include "LapackConvert.h"
+
+#include <iostream>
+#include <cstdio>
+#include <cmath>
+#include <complex>
+
+
+namespace SimTK {
+
+ //////////////////
+ // FactorSVDDefault //
+ //////////////////
+FactorSVDDefault::FactorSVDDefault() {
+ isFactored = false;
+}
+FactorSVDRepBase* FactorSVDDefault::clone() const {
+ return( new FactorSVDDefault(*this));
+}
+
+
+ ///////////
+ // FactorSVD //
+ ///////////
+FactorSVD::~FactorSVD() {
+ delete rep;
+}
+FactorSVD::FactorSVD() {
+ rep = new FactorSVDDefault();
+}
+// copy constructor
+FactorSVD::FactorSVD( const FactorSVD& c ) {
+ rep = c.rep->clone();
+}
+// copy assignment operator
+FactorSVD& FactorSVD::operator=(const FactorSVD& rhs) {
+ rep = rhs.rep->clone();
+ return *this;
+}
+int FactorSVD::getRank() {
+ return(rep->getRank() );
+}
+template < typename ELT >
+void FactorSVD::solve( const Vector_<ELT>& b, Vector_<ELT>& x ) {
+ rep->solve( b, x );
+ return;
+}
+template < class ELT >
+void FactorSVD::solve( const Matrix_<ELT>& b, Matrix_<ELT>& x ) {
+ rep->solve( b, x );
+ return;
+}
+
+template <typename ELT>
+void FactorSVD::inverse( Matrix_<ELT>& inverse ) {
+ rep->inverse( inverse );
+}
+template < class ELT >
+FactorSVD::FactorSVD( const Matrix_<ELT>& m ) {
+
+ // if user does not supply rcond set it to max(nRow,nCol)*(eps)^7/8 (similar to matlab)
+ int mnmax = (m.nrow() > m.ncol()) ? m.nrow() : m.ncol();
+ rep = new FactorSVDRep<typename CNT<ELT>::StdNumber>(m, mnmax*NTraits<typename CNT<ELT>::Precision>::getSignificant());
+}
+template < class ELT >
+FactorSVD::FactorSVD( const Matrix_<ELT>& m, double rcond ) {
+ rep = new FactorSVDRep<typename CNT<ELT>::StdNumber>(m, rcond);
+}
+template < class ELT >
+FactorSVD::FactorSVD( const Matrix_<ELT>& m, float rcond ) {
+ rep = new FactorSVDRep<typename CNT<ELT>::StdNumber>(m, rcond);
+}
+
+template < class ELT >
+void FactorSVD::factor( const Matrix_<ELT>& m ) {
+ delete rep;
+
+ // if user does not supply rcond set it to max(nRow,nCol)*(eps)^7/8 (similar to matlab)
+ int mnmax = (m.nrow() > m.ncol()) ? m.nrow() : m.ncol();
+ rep = new FactorSVDRep<typename CNT<ELT>::StdNumber>(m, mnmax*NTraits<typename CNT<ELT>::Precision>::getSignificant());
+}
+
+template < class ELT >
+void FactorSVD::factor( const Matrix_<ELT>& m, double rcond ){
+ delete rep;
+ rep = new FactorSVDRep<typename CNT<ELT>::StdNumber>(m, rcond );
+}
+template < class ELT >
+void FactorSVD::factor( const Matrix_<ELT>& m, float rcond ){
+ delete rep;
+ rep = new FactorSVDRep<typename CNT<ELT>::StdNumber>(m, rcond );
+}
+
+template <class T>
+void FactorSVD::getSingularValuesAndVectors( Vector_<typename CNT<T>::TReal >& values, Matrix_<T>& leftVectors, Matrix_<T>& rightVectors) {
+
+ rep->getSingularValuesAndVectors( values, leftVectors, rightVectors );
+
+ return;
+}
+
+template <class T>
+void FactorSVD::getSingularValues( Vector_<T>& values ) {
+ rep->getSingularValues( values );
+ return;
+}
+//////////////////
+ // FactorSVDRep //
+ //////////////////
+template <typename T > // constructor
+ template < typename ELT >
+FactorSVDRep<T>::FactorSVDRep( const Matrix_<ELT>& mat, typename CNT<T>::TReal rc):
+ nCol(mat.ncol()),
+ nRow(mat.nrow()),
+ mn( (mat.nrow() < mat.ncol()) ? mat.nrow() : mat.ncol() ),
+ maxmn( (mat.nrow() > mat.ncol()) ? mat.nrow() : mat.ncol() ),
+ singularValues(mn),
+ inputMatrix(nCol*nRow),
+ rank(0),
+ structure(mat.getMatrixCharacter().getStructure()) {
+
+ LapackInterface::getMachineUnderflow( abstol );
+ abstol *= 0.5;
+ rcond = rc;
+
+ LapackConvert::convertMatrixToLapack( inputMatrix.data, mat );
+ isFactored = true;
+
+}
+template <typename T >
+int FactorSVDRep<T>::getRank() {
+
+ if( rank == 0 ) { // check if SVD has been done
+ Vector_<RType> v;
+ getSingularValues( v );
+ }
+ return( rank );
+}
+template < class T >
+void FactorSVDRep<T>::inverse( Matrix_<T>& inverse ) {
+
+ Matrix_<T> iden(mn,mn);
+ inverse.resize(mn,mn);
+ iden = 1.0;
+ solve( iden, inverse );
+
+}
+
+template <typename T >
+FactorSVDRep<T>::~FactorSVDRep() {}
+
+template <typename T >
+FactorSVDRepBase* FactorSVDRep<T>::clone() const {
+ return( new FactorSVDRep<T>(*this) );
+}
+
+template < class T >
+void FactorSVDRep<T>::solve( const Vector_<T>& b, Vector_<T> &x ) {
+
+ SimTK_APIARGCHECK_ALWAYS(isFactored ,"FactorSVD","solve",
+ "No matrix was passed to FactorSVD. \n" );
+
+ SimTK_APIARGCHECK2_ALWAYS(b.size()==nRow,"FactorSVD","solve",
+ "number of rows in right hand side=%d does not match number of rows in original matrix=%d \n",
+ b.size(), nRow );
+
+ if( inputMatrix.size == 0 || b.size() == 0 ) {
+ x.resize(0);
+ } else {
+
+ Matrix_<T> m(maxmn,1);
+
+ for(int i=0;i<b.size();i++) {
+ m(i,0) = b(i);
+ }
+ Matrix_<T> r(nCol, 1 );
+ doSolve( m, r );
+ x.copyAssign(r);
+ }
+ return;
+
+}
+
+template < class T >
+void FactorSVDRep<T>::solve( const Matrix_<T>& b, Matrix_<T> &x ) {
+
+ SimTK_APIARGCHECK_ALWAYS(isFactored ,"FactorSVD","solve",
+ "No matrix was passed to FactorSVD. \n" );
+
+ SimTK_APIARGCHECK2_ALWAYS(b.nrow()==nRow,"FactorSVD","solve",
+ "number of rows in right hand side=%d does not match number of rows in original matrix=%d \n",
+ b.nrow(), nRow );
+
+ if( inputMatrix.size == 0 || b.nelt() == 0 ) {
+ x.resize(0,0);
+ } else {
+ x.resize( nCol, b.ncol() );
+ Matrix_<T> tb;
+ tb.resize(maxmn, b.ncol() );
+ for(int j=0;j<b.ncol();j++) for(int i=0;i<b.nrow();i++) tb(i,j) = b(i,j);
+ doSolve(tb, x);
+ }
+
+
+}
+
+template <typename T >
+void FactorSVDRep<T>::doSolve( Matrix_<T>& b, Matrix_<T>& x) {
+ int i,j;
+ int info;
+ typedef typename CNT<T>::TReal RealType;
+
+ if( b.nelt() == 0 || inputMatrix.size == 0) return;
+
+ TypedWorkSpace<T> tempMatrix = inputMatrix;
+
+ x.resize(nCol, b.ncol() );
+ Matrix_<T> tb;
+ tb.resize(maxmn, b.ncol() );
+ for(j=0;j<b.ncol();j++) for(i=0;i<b.nrow();i++) tb(i,j) = b(i,j);
+
+ LapackInterface::gelss<T>( nRow, nCol, mn, b.ncol(), tempMatrix.data, nRow, &tb(0,0),
+ tb.nrow(), singularValues.data, rcond, rank, info );
+
+ if( info > 0 ) {
+ SimTK_THROW2( SimTK::Exception::ConvergedFailed,
+ "FactorSVD::solve",
+ "divide and conquer singular value decomposition" );
+ }
+
+ for(j=0;j<b.ncol();j++) for(i=0;i<nCol;i++) x(i,j) = tb(i,j);
+
+}
+
+template < class T >
+void FactorSVDRep<T>::getSingularValuesAndVectors( Vector_<RType>& values, Matrix_<T>& leftVectors, Matrix_<T>& rightVectors ) {
+
+
+ if( inputMatrix.size == 0 ) {
+ leftVectors.resize(0,0);
+ rightVectors.resize(0,0);
+ values.resize(0);
+ return;
+ } else {
+ leftVectors.resize(nRow,nRow);
+ rightVectors.resize(nCol,nCol);
+ values.resize(mn);
+ }
+
+ computeSVD( true, &values(0), &leftVectors(0,0), &rightVectors(0,0) );
+
+ return;
+}
+template < class T >
+void FactorSVDRep<T>::getSingularValues( Vector_<RType>& values ) {
+
+ if( inputMatrix.size == 0 ) {
+ values.resize(0);
+ return;
+ } else {
+ values.resize(mn);
+ }
+
+ computeSVD( false, &values(0), NULL, NULL );
+
+ return;
+}
+template < class T >
+void FactorSVDRep<T>::computeSVD( bool computeVectors, RType* values, T* leftVectors, T* rightVectors ) {
+
+ int info;
+ char jobz;
+
+
+ if( computeVectors ) {
+ jobz = 'A';
+ } else {
+ jobz = 'N';
+ }
+
+ TypedWorkSpace<T> tempMatrix = inputMatrix;
+ LapackInterface::gesdd<T>(jobz, nRow,nCol,tempMatrix.data, nRow, values,
+ leftVectors, nRow, rightVectors, nCol, info);
+
+ for(int i=0, rank=0;i<mn;i++) {
+ if( values[i] > rcond*values[0] ) rank++;
+ }
+
+ if( info > 0 ) {
+ SimTK_THROW2( SimTK::Exception::ConvergedFailed,
+ "FactorSVD::getSingularValuesAndVectors",
+ "divide and conquer singular value decomposition" );
+ }
+
+ return;
+}
+
+// instantiate
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<double>& m );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<float>& m );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<std::complex<float> >& m );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<std::complex<double> >& m );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<conjugate<float> >& m );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<conjugate<double> >& m );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<negator< double> >& m );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<negator< float> >& m );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<negator< std::complex<float> > >& m );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<negator< std::complex<double> > >& m );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<negator< conjugate<float> > >& m );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<negator< conjugate<double> > >& m );
+
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<double>& m, double rcond );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<float>& m, float rcond );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<std::complex<float> >& m, float rcond );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<std::complex<double> >& m, double rcond );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<conjugate<float> >& m, float rcond );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<conjugate<double> >& m, double rcond );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<negator< double> >& m, double rcond );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<negator< float> >& m, float rcond );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<negator< std::complex<float> > >& m, float rcond );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<negator< std::complex<double> > >& m, double rcond );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<negator< conjugate<float> > >& m, float rcond );
+template SimTK_SIMMATH_EXPORT FactorSVD::FactorSVD( const Matrix_<negator< conjugate<double> > >& m, double rcond );
+
+template SimTK_SIMMATH_EXPORT void FactorSVD::getSingularValues<float >(Vector_<float>& );
+template SimTK_SIMMATH_EXPORT void FactorSVD::getSingularValues<double >(Vector_<double>& );
+
+template SimTK_SIMMATH_EXPORT void FactorSVD::getSingularValuesAndVectors<float >(Vector_<float>&, Matrix_<float>&, Matrix_<float>& );
+template SimTK_SIMMATH_EXPORT void FactorSVD::getSingularValuesAndVectors<double >(Vector_<double>&, Matrix_<double>&, Matrix_<double>& );
+template SimTK_SIMMATH_EXPORT void FactorSVD::getSingularValuesAndVectors<std::complex<float> >(Vector_<float>&, Matrix_<std::complex<float> >&, Matrix_<std::complex<float> >& );
+template SimTK_SIMMATH_EXPORT void FactorSVD::getSingularValuesAndVectors<std::complex<double> >(Vector_<double>&, Matrix_<std::complex<double> >&, Matrix_<std::complex<double> >& );
+
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<double>& m );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<float>& m );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<std::complex<float> >& m );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<std::complex<double> >& m );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<conjugate<float> >& m );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<conjugate<double> >& m );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<negator< double> >& m );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<negator< float> >& m );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<negator< std::complex<float> > >& m );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<negator< std::complex<double> > >& m );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<negator< conjugate<float> > >& m );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<negator< conjugate<double> > >& m );
+
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<double>& m, double rcond );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<float>& m, float rcond );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<std::complex<float> >& m, float rcond );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<std::complex<double> >& m, double rcond );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<conjugate<float> >& m, float rcond );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<conjugate<double> >& m, double rcond );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<negator< double> >& m, double rcond );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<negator< float> >& m, float rcond );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<negator< std::complex<float> > >& m, float rcond );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<negator< std::complex<double> > >& m, double rcond );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<negator< conjugate<float> > >& m, float rcond );
+template SimTK_SIMMATH_EXPORT void FactorSVD::factor( const Matrix_<negator< conjugate<double> > >& m, double rcond );
+
+template SimTK_SIMMATH_EXPORT void FactorSVD::inverse<float>(Matrix_<float>&);
+template SimTK_SIMMATH_EXPORT void FactorSVD::inverse<double>(Matrix_<double>&);
+template SimTK_SIMMATH_EXPORT void FactorSVD::inverse<std::complex<float> >(Matrix_<std::complex<float> >&);
+template SimTK_SIMMATH_EXPORT void FactorSVD::inverse<std::complex<double> >(Matrix_<std::complex<double> >&);
+
+template SimTK_SIMMATH_EXPORT void FactorSVD::solve<float>(const Vector_<float>&, Vector_<float>&);
+template SimTK_SIMMATH_EXPORT void FactorSVD::solve<double>(const Vector_<double>&, Vector_<double>&);
+template SimTK_SIMMATH_EXPORT void FactorSVD::solve<std::complex<float> >(const Vector_<std::complex<float> >&, Vector_<std::complex<float> >&);
+template SimTK_SIMMATH_EXPORT void FactorSVD::solve<std::complex<double> >(const Vector_<std::complex<double> >&, Vector_<std::complex<double> >&);
+template SimTK_SIMMATH_EXPORT void FactorSVD::solve<float>(const Matrix_<float>&, Matrix_<float>&);
+template SimTK_SIMMATH_EXPORT void FactorSVD::solve<double>(const Matrix_<double>&, Matrix_<double>&);
+template SimTK_SIMMATH_EXPORT void FactorSVD::solve<std::complex<float> >(const Matrix_<std::complex<float> >&, Matrix_<std::complex<float> >&);
+template SimTK_SIMMATH_EXPORT void FactorSVD::solve<std::complex<double> >(const Matrix_<std::complex<double> >&, Matrix_<std::complex<double> >&);
+
+template class FactorSVDRep<double>;
+template FactorSVDRep<double>::FactorSVDRep( const Matrix_<double>& m, double rcond);
+template FactorSVDRep<double>::FactorSVDRep( const Matrix_<negator<double> >& m, double rcond);
+
+template class FactorSVDRep<float>;
+template FactorSVDRep<float>::FactorSVDRep( const Matrix_<float>& m, float rcond );
+template FactorSVDRep<float>::FactorSVDRep( const Matrix_<negator<float> >& m, float rcond );
+
+template class FactorSVDRep<std::complex<double> >;
+template FactorSVDRep<std::complex<double> >::FactorSVDRep( const Matrix_<std::complex<double> >& m, double rcond );
+template FactorSVDRep<std::complex<double> >::FactorSVDRep( const Matrix_<negator<std::complex<double> > >& m, double rcond );
+template FactorSVDRep<std::complex<double> >::FactorSVDRep( const Matrix_<conjugate<double> >& m, double rcond );
+template FactorSVDRep<std::complex<double> >::FactorSVDRep( const Matrix_<negator<conjugate<double> > >& m, double rcond );
+
+template class FactorSVDRep<std::complex<float> >;
+template FactorSVDRep<std::complex<float> >::FactorSVDRep( const Matrix_<std::complex<float> >& m, float rcond );
+template FactorSVDRep<std::complex<float> >::FactorSVDRep( const Matrix_<negator<std::complex<float> > >& m, float rcond );
+template FactorSVDRep<std::complex<float> >::FactorSVDRep( const Matrix_<conjugate<float> >& m, float rcond );
+template FactorSVDRep<std::complex<float> >::FactorSVDRep( const Matrix_<negator<conjugate<float> > >& m, float rcond );
+
+} // namespace SimTK
diff --git a/SimTKmath/LinearAlgebra/src/FactorSVDRep.h b/SimTKmath/LinearAlgebra/src/FactorSVDRep.h
new file mode 100644
index 0000000..85a8c55
--- /dev/null
+++ b/SimTKmath/LinearAlgebra/src/FactorSVDRep.h
@@ -0,0 +1,194 @@
+#ifndef SimTK_SIMMATH_FACTORSVD_REP_H_
+#define SimTK_SIMMATH_FACTORSVD_REP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 "WorkSpace.h"
+
+namespace SimTK {
+
+class FactorSVDRepBase {
+ public:
+
+ virtual ~FactorSVDRepBase(){};
+
+ virtual FactorSVDRepBase* clone() const { return 0; };
+
+
+ virtual void getSingularValues( Vector_<float>& values ){
+ checkIfFactored( "getSingularValues" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorSVD","getSingularValues",
+ "getSingularValues( float) called with types that are inconsistant with the original matrix \n");
+ }
+ virtual void getSingularValues( Vector_<double>& values ){
+ checkIfFactored( "getSingularValues" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorSVD","getSingularValues",
+ "getSingularValues( double) called with types that are inconsistant with the original matrix \n");
+ }
+ virtual void getSingularValuesAndVectors( Vector_<float>& values, Matrix_<float>& leftVectors, Matrix_<float>& rightVectors ){
+ checkIfFactored( "getSingularValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorSVD","getSingularValuesAndVectors",
+ "getSingularValuesAndVectors( float, float, float) called with types that are inconsistant with the original matrix \n");
+ }
+ virtual void getSingularValuesAndVectors( Vector_<double>& values, Matrix_<double>& leftVectors, Matrix_<double>& rightVectors ){
+ checkIfFactored( "getSingularValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorSVD","getSingularValuesAndVectors",
+ "getSingularValuesAndVectors( double, double, double) called with types that are inconsistant with the original matrix \n");
+ }
+
+ virtual void getSingularValuesAndVectors( Vector_<float>& values, Matrix_<std::complex<float> >& leftVectors, Matrix_<std::complex<float> >& rightVectors ){
+ checkIfFactored( "getSingularValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorSVD","getSingularValuesAndVectors",
+ "getSingularValuesAndVectors( float, std::complex<float> , std::complex<float> ) called with types that are inconsistant with the original matrix \n");
+ }
+
+ virtual void getSingularValuesAndVectors( Vector_<double>& values, Matrix_<std::complex<double> >& leftVectors, Matrix_<std::complex<double> >& rightVectors ){
+ checkIfFactored( "getSingularValuesAndVectors" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorSVD","getSingularValuesAndVectors",
+ "getSingularValuesAndVectors( double, std::complex<double> , std::complex<double> ) called with types that are inconsistant with the original matrix \n");
+ }
+ virtual void solve( const Vector_<float>& b, Vector_<float>& x ) {
+ checkIfFactored("solve");
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorSVD","solve",
+ "solve called with rhs of type <float> which does not match type of original linear system \n");
+ }
+ virtual void solve( const Vector_<double>& b, Vector_<double>& x ){
+ checkIfFactored("solve");
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorSVD","solve",
+ "solve called with rhs of type <double> which does not match type of original linear system \n");
+ }
+ virtual void solve( const Vector_<std::complex<float> >& b, Vector_<std::complex<float> >& x ) {
+ checkIfFactored("solve");
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorSVD","solve",
+ "solve called with rhs of type complex<float> which does not match type of original linear system \n");
+ }
+ virtual void solve( const Vector_<std::complex<double> >& b, Vector_<std::complex<double> >& x ) {
+ checkIfFactored("solve");
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorSVD","solve",
+ "solve called with rhs of type complex<double> which does not match type of original linear system \n");
+ }
+ virtual void solve( const Matrix_<float>& b, Matrix_<float>& x ) {
+ checkIfFactored("solve");
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorSVD","solve",
+ "solve called with rhs of type <float> which does not match type of original linear system \n");
+ }
+ virtual void solve( const Matrix_<double>& b, Matrix_<double>& x ) {
+ checkIfFactored("solve");
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorSVD","solve",
+ "solve called with rhs of type <double> which does not match type of original linear system \n");
+ }
+ virtual void solve( const Matrix_<std::complex<float> >& b, Matrix_<std::complex<float> >& x ) {
+ checkIfFactored("solve");
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorSVD","solve",
+ "solve called with rhs of type complex<float> which does not match type of original linear system \n");
+ }
+ virtual void solve ( const Matrix_<std::complex<double> >& b, Matrix_<std::complex<double> >& x ) {
+ checkIfFactored("solve");
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorSVD","solve",
+ "solve called with rhs of type complex<double> which does not match type of original linear system \n");
+ }
+ virtual void inverse( Matrix_<double>& inverse ){
+ checkIfFactored( "inverse" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorSVD","inverse",
+ "inverse( <double> ) called with type that is inconsistant with the original matrix \n");
+ }
+ virtual void inverse( Matrix_<float>& inverse ){
+ checkIfFactored( "inverse" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorSVD","inverse",
+ "inverse( <float> ) called with type that is inconsistant with the original matrix \n");
+ }
+ virtual void inverse( Matrix_<std::complex<float> >& inverse ){
+ checkIfFactored( "inverse" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorSVD","inverse",
+ "inverse( std::complex<float> ) called with type that is inconsistant with the original matrix \n");
+ }
+ virtual void inverse( Matrix_<std::complex<double> >& inverse ){
+ checkIfFactored( "inverse" );
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorSVD","inverse",
+ "inverse( std::complex<double> ) called with type that is inconsistant with the original matrix \n");
+ }
+ virtual int getRank() const {
+ checkIfFactored( "getRank" );
+ return(0);
+ }
+
+ void checkIfFactored(const char* function_name) const {
+ if( !isFactored ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"FactorSVD",function_name,
+ "matrix has not been specified\n");
+ }
+ return;
+ }
+ bool isFactored;
+
+
+
+
+}; // class FactorSVDRepBase
+
+class FactorSVDDefault : public FactorSVDRepBase {
+ public:
+ FactorSVDDefault();
+ FactorSVDRepBase* clone() const;
+};
+
+
+template <typename T>
+class FactorSVDRep : public FactorSVDRepBase {
+ public:
+ template <class ELT> FactorSVDRep( const Matrix_<ELT>&, typename CNT<T>::TReal );
+
+ ~FactorSVDRep();
+ FactorSVDRepBase* clone() const;
+
+ typedef typename CNT<T>::TReal RType;
+
+ void getSingularValuesAndVectors( Vector_<RType>& values, Matrix_<T>& leftVectors, Matrix_<T>& rightVectors );
+ void getSingularValues( Vector_<RType>& values );
+ int getRank();
+ void solve( const Vector_<T>& b, Vector_<T>& x );
+ void solve( const Matrix_<T>& b, Matrix_<T>& x );
+
+
+ private:
+
+ void computeSVD( bool, RType*, T*, T* );
+ void doSolve( Matrix_<T>& b, Matrix_<T>& x );
+ void inverse( Matrix_<T>& b );
+
+ int nCol; // number of columns in original matrix
+ int nRow; // number of rows in original matrix
+ int mn; // min(m,n)
+ int maxmn; // max(m,n)
+ int rank;
+ RType rcond; // reciprocol condition number
+ RType abstol;
+ MatrixStructure structure;
+ TypedWorkSpace<T> inputMatrix;
+ TypedWorkSpace<RType> singularValues;
+
+}; // end class FactorSVDRep
+} // namespace SimTK
+#endif // SimTK_SIMMATH_FACTORSVD_REP_H_
diff --git a/SimTKmath/LinearAlgebra/src/LATraits.h b/SimTKmath/LinearAlgebra/src/LATraits.h
new file mode 100644
index 0000000..7e1cb93
--- /dev/null
+++ b/SimTKmath/LinearAlgebra/src/LATraits.h
@@ -0,0 +1,84 @@
+#ifndef SimTK_SIMMATH_LA_TRAITS_H_
+#define SimTK_SIMMATH_LA_TRAITS_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 <complex>
+#include "SimTKcommon.h"
+
+namespace SimTK {
+
+template <typename T> struct LANT;
+
+template <> struct LANT<float>{
+ static const int sign = 1;
+ static const bool conjugate = false;
+};
+template <> struct LANT<double>{
+ static const int sign = 1;
+ static const bool conjugate = false;
+};
+template <> struct LANT<std::complex<float> >{
+ static const int sign = 1;
+ static const bool conjugate = false;
+};
+template <> struct LANT<std::complex<double> >{
+ static const int sign = 1;
+ static const bool conjgate = false;
+};
+template <> struct LANT<negator<float> >{
+ static const int sign = -1;
+ static const bool conjugate = false;
+};
+template <> struct LANT<negator<double> >{
+ static const int sign = -1;
+ static const bool conjugate = false;
+};
+template <> struct LANT<negator<std::complex<float> > >{
+ static const int sign = -1;
+ static const bool conjugate = false;
+};
+template <> struct LANT<negator<std::complex<double> > >{
+ static const int sign = -1;
+ static const bool conjugate = false;
+};
+template <> struct LANT<conjugate<float> >{
+ static const int sign = 1;
+ static const bool conjugate = true;
+};
+template <> struct LANT<conjugate<double> >{
+ static const int sign = 1;
+ static const bool conjugate = true;
+};
+template <> struct LANT<negator<conjugate<float> > >{
+ static const int sign = -1;
+ static const bool conjugate = true;
+};
+template <> struct LANT<negator<conjugate<double> > >{
+ static const int sign = -1;
+ static const bool conjugate = true;
+};
+
+} // namespace SimTK
+#endif // SimTK_SIMMATH_LA_TRAITS_H_
diff --git a/SimTKmath/LinearAlgebra/src/LapackConvert.cpp b/SimTKmath/LinearAlgebra/src/LapackConvert.cpp
new file mode 100644
index 0000000..1b8fc9f
--- /dev/null
+++ b/SimTKmath/LinearAlgebra/src/LapackConvert.cpp
@@ -0,0 +1,78 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 <complex>
+#include "SimTKcommon.h"
+#include "LapackConvert.h"
+
+namespace SimTK {
+
+
+template <>
+void LapackConvert::convertMatrixToLapack( std::complex<double>* lapackArray, const Matrix_<negator<conjugate<double> > >& mat ) {
+ int m = mat.nrow();
+ int n = mat.ncol();
+ for(int i=0;i<n;i++) {
+ for(int j=0;j<m;j++) {
+ lapackArray[i*m+j] = std::complex<double>(mat(j,i).real(),mat(j,i).imag());
+ }
+ }
+ return;
+}
+
+template <>
+void LapackConvert::convertMatrixToLapack( std::complex<float>* lapackArray, const Matrix_<negator<conjugate<float> > >& mat ) {
+ int m = mat.nrow();
+ int n = mat.ncol();
+ for(int i=0;i<n;i++) {
+ for(int j=0;j<m;j++) {
+ lapackArray[i*m+j] = std::complex<float>(mat(j,i).real(),mat(j,i).imag());
+ }
+ }
+ return;
+}
+template < typename T, typename ELT>
+void LapackConvert::convertMatrixToLapack ( T* lapackArray, const Matrix_<ELT>& mat ) {
+ int m = mat.nrow();
+ int n = mat.ncol();
+ for(int c=0;c<n;c++) {
+ for(int r=0;r<m;r++) {
+ lapackArray[c*m+r] = mat(r,c);
+ }
+ }
+ return;
+}
+
+
+template void LapackConvert::convertMatrixToLapack( float* lapackArray, const Matrix_<float>& mat );
+template void LapackConvert::convertMatrixToLapack( double* lapackArray, const Matrix_<double>& mat );
+template void LapackConvert::convertMatrixToLapack( float* lapackArray, const Matrix_<negator<float> >& mat );
+template void LapackConvert::convertMatrixToLapack( double* lapackArray, const Matrix_<negator<double> >& mat );
+template void LapackConvert::convertMatrixToLapack( std::complex<float>* lapackArray, const Matrix_<std::complex<float> >& mat );
+template void LapackConvert::convertMatrixToLapack( std::complex<double>* lapackArray, const Matrix_<std::complex<double> >& mat );
+template void LapackConvert::convertMatrixToLapack( std::complex<float>* lapackArray, const Matrix_<negator<std::complex<float> > >& mat );
+template void LapackConvert::convertMatrixToLapack( std::complex<double>* lapackArray, const Matrix_<negator<std::complex<double> > >& mat );
+template void LapackConvert::convertMatrixToLapack( std::complex<float>* lapackArray, const Matrix_<conjugate<float> >& mat );
+template void LapackConvert::convertMatrixToLapack( std::complex<double>* lapackArray, const Matrix_<conjugate<double> >& mat );
+
+} // namespace SimTK
diff --git a/SimTKmath/LinearAlgebra/src/LapackConvert.h b/SimTKmath/LinearAlgebra/src/LapackConvert.h
new file mode 100644
index 0000000..bc5343c
--- /dev/null
+++ b/SimTKmath/LinearAlgebra/src/LapackConvert.h
@@ -0,0 +1,40 @@
+
+#ifndef SimTK_SIMMATH_LAPACK_CONVERT_H_
+#define SimTK_SIMMATH_LAPACK_CONVERT_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 <complex>
+#include "SimTKcommon.h"
+
+namespace SimTK {
+class LapackConvert {
+ public:
+ template <typename T, typename ELT> static void convertMatrixToLapack( T* lapackArray, const Matrix_<ELT>& mat );
+};
+
+
+
+} // namespace SimTK
+#endif // SimTK_SIMMATH_LAPACK_CONVERT_H_
diff --git a/SimTKmath/LinearAlgebra/src/LapackInterface.cpp b/SimTKmath/LinearAlgebra/src/LapackInterface.cpp
new file mode 100644
index 0000000..9ae29cf
--- /dev/null
+++ b/SimTKmath/LinearAlgebra/src/LapackInterface.cpp
@@ -0,0 +1,1213 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * These is a templatized, C++ callable interface to LAPACK and BLAS.
+ * Each method must be explicitly specialized for the supported precisions.
+ */
+
+
+#include "SimTKcommon.h"
+#include "SimTKlapack.h"
+#include "SimTKmath.h"
+#include "LapackInterface.h"
+#include "WorkSpace.h"
+#include <cstring>
+
+static const double EPS = .000001;
+namespace SimTK {
+
+int LapackInterface::getLWork( float* work) { return( (int)work[0] ); }
+int LapackInterface::getLWork( double* work) { return( (int)work[0] ); }
+int LapackInterface::getLWork( std::complex<float>* work) { return( (int)work[0].real() ); }
+int LapackInterface::getLWork( std::complex<double>* work) { return( (int)work[0].real() ); }
+
+template <typename T> void LapackInterface::gelss( int m, int n, int mn, int nrhs,
+ T* a, int lda, T* b, int ldb, typename CNT<T>::TReal* s,
+ typename CNT<T>::TReal rcond, int& rank, int& info){ assert(false); }
+
+template <> void LapackInterface::gelss<double>( int m, int n, int mn, int nrhs,
+ double* a, int lda, double* b, int ldb, double* s,
+ double rcond, int& rank, int& info){
+
+ double wsize[1];
+ dgelss_(m, n, nrhs, a, lda, b, ldb, s, rcond, rank, wsize, -1, info );
+
+ int lwork = (int)wsize[0];
+ TypedWorkSpace<double> work(lwork);
+
+ dgelss_(m, n, nrhs, a, lda, b, ldb, s, rcond, rank, work.data, lwork, info );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "dgelss", info );
+ }
+}
+
+template <> void LapackInterface::gelss<float>( int m, int n, int mn, int nrhs,
+ float* a, int lda, float* b, int ldb, float* s,
+ float rcond, int& rank, int& info){
+
+ float wsize[1];
+ sgelss_(m, n, nrhs, a, lda, b, ldb, s, rcond, rank, wsize, -1, info );
+
+ int lwork = (int)wsize[0];
+ TypedWorkSpace<float> work(lwork);
+
+ sgelss_(m, n, nrhs, a, lda, b, ldb, s, rcond, rank, work.data, lwork, info );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "sgelss", info );
+ }
+}
+
+template <> void LapackInterface::gelss<std::complex<float> >( int m, int n, int mn, int nrhs,
+ std::complex<float>* a, int lda, std::complex<float>* b, int ldb, float* s,
+ float rcond, int& rank, int& info){
+
+ std::complex<float> wsize[1];
+ TypedWorkSpace<float> rwork(5*mn);
+ cgelss_(m, n, nrhs, a, lda, b, ldb, s, rcond, rank, wsize, -1, rwork.data, info );
+
+ int lwork = (int)wsize[0].real();
+ TypedWorkSpace<std::complex<float> > work(lwork);
+
+ cgelss_(m, n, nrhs, a, lda, b, ldb, s, rcond, rank, work.data, lwork, rwork.data, info );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "cgelss", info );
+ }
+}
+
+template <> void LapackInterface::gelss<std::complex<double> >( int m, int n, int mn, int nrhs,
+ std::complex<double>* a, int lda, std::complex<double>* b, int ldb, double* s,
+ double rcond, int& rank, int& info){
+
+ TypedWorkSpace<double> rwork(5*mn);
+ std::complex<double> wsize[1];
+ zgelss_(m, n, nrhs, a, lda, b, ldb, s, rcond, rank, wsize, -1, rwork.data, info );
+
+ int lwork = (int)wsize[0].real();
+ TypedWorkSpace<std::complex<double> > work(lwork);
+
+ zgelss_(m, n, nrhs, a, lda, b, ldb, s, rcond, rank, work.data, lwork, rwork.data, info );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "zgelss", info );
+ }
+}
+
+
+template <> void LapackInterface::potrs<double>
+ ( char uplo, const int ncol, const int nrhs, const double *lu, double *b ) {
+
+ int info;
+
+ dpotrs_(uplo, ncol, nrhs, lu, ncol, b, ncol, info, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "dpotrs", info );
+ }
+
+ return;
+}
+
+template <> void LapackInterface::potrs<float>
+ ( char uplo, const int ncol, const int nrhs, const float *lu, float *b ) {
+
+ int info;
+
+ spotrs_(uplo, ncol, nrhs, lu, ncol, b, ncol, info, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "spotrs", info );
+ }
+
+ return;
+}
+
+template <> void LapackInterface::potrs<std::complex<float> >
+ ( char uplo, const int ncol, const int nrhs, const std::complex<float>* lu, std::complex<float>* b ) {
+
+ int info;
+
+ cpotrs_(uplo, ncol, nrhs, lu, ncol, b, ncol, info, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "cpotrs", info );
+ }
+
+ return;
+}
+
+template <> void LapackInterface::potrs<std::complex<double> >
+ ( char uplo, const int ncol, const int nrhs, const std::complex<double>* lu, std::complex<double>* b ) {
+
+ int info;
+
+ zpotrs_(uplo, ncol, nrhs, lu, ncol, b, ncol, info, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "zpotrs", info );
+ }
+
+ return;
+}
+template <> void LapackInterface::sytrs<double>
+// TODO fix SimTKlapack.h for const int* pivots ( char trans, const int ncol, const int nrhs, const double *lu, const int *pivots, double *b ) {
+( char trans, const int ncol, const int nrhs, double *lu, int *pivots, double *b ) {
+
+ int info;
+
+ dsytrs_(trans, ncol, nrhs, lu, ncol, pivots, b, ncol, info, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "dsytrs", info );
+ }
+
+ return;
+}
+
+template <> void LapackInterface::sytrs<float>
+// TODO ( char trans, const int ncol, const int nrhs, const float *lu, const int *pivots, float *b ) {
+ ( char trans, const int ncol, const int nrhs, float *lu, int *pivots, float *b ) {
+
+ int info;
+
+ ssytrs_(trans, ncol, nrhs, lu, ncol, pivots, b, ncol, info, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "ssytrs", info );
+ }
+
+ return;
+}
+
+template <> void LapackInterface::sytrs<std::complex<float> >
+// TODO ( char trans, const int ncol, const int nrhs, const std::complex<float>* lu, const int *pivots, std::complex<float>* b ) {
+ ( char trans, const int ncol, const int nrhs, std::complex<float>* lu, int *pivots, std::complex<float>* b ) {
+
+ int info;
+
+ chetrs_(trans, ncol, nrhs, lu, ncol, pivots, b, ncol, info, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "chetrs", info );
+ }
+ return;
+}
+
+
+template <> void LapackInterface::sytrs<std::complex<double> >
+// TODO ( char trans, const int ncol, const int nrhs, const std::complex<double>* lu, const int *pivots, std::complex<double>* b ) {
+ ( char trans, const int ncol, const int nrhs, std::complex<double>* lu, int *pivots, std::complex<double>* b ) {
+
+ int info;
+
+ zhetrs_(trans, ncol, nrhs, lu, ncol, pivots, b, ncol, info, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "zhetrs", info );
+ }
+ return;
+}
+
+template <> void LapackInterface::getrs<double>
+ ( char trans, const int ncol, const int nrhs, const double *lu, const int *pivots, double *b ) {
+
+ int info;
+
+ dgetrs_(trans, ncol, nrhs, lu, ncol, pivots, b, ncol, info, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "dgetrs", info );
+ }
+
+ return;
+}
+
+
+template <> void LapackInterface::getrs<float>
+ ( char trans , const int ncol, const int nrhs, const float *lu, const int *pivots, float *b ) {
+
+ int info;
+
+ sgetrs_(trans, ncol, nrhs, lu, ncol, pivots, b, ncol, info, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "sgetrs", info );
+ }
+
+ return;
+}
+
+template <> void LapackInterface::getrs<complex<float> >
+ ( char trans, const int ncol, const int nrhs, const std::complex<float> *lu, const int *pivots, complex<float> *b ) {
+
+ int info;
+
+ cgetrs_(trans, ncol, nrhs, lu, ncol, pivots, b, ncol, info, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "cgetrs", info );
+ }
+
+ return;
+}
+template <> void LapackInterface::getrs<complex<double> >
+ ( char trans, const int ncol, const int nrhs, const complex<double> *lu, const int *pivots, complex<double> *b ) {
+
+ int info;
+
+ zgetrs_(trans, ncol, nrhs, lu, ncol, pivots, b, ncol, info, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "zgetrs", info );
+ }
+
+ return;
+}
+// selected eigenvalues for symmetric matrices
+template <class T>
+void LapackInterface::syevx( char jobz, char range, char uplo, int n, T* a, int lda,
+ typename CNT<T>::TReal vl, typename CNT<T>::TReal vu, int il, int iu,
+ typename CNT<T>::TReal abstol, int& nFound, typename CNT<T>::TReal* values,
+ T* vectors, int LDVectors, int* ifail, int& info ) {
+ assert(false);
+}
+template <> void LapackInterface::syevx<float>( char jobz, char range,
+ char uplo, int n, float* a, int lda, float vl, float vu, int il,
+ int iu, float abstol, int& nFound, float *values, float* vectors,
+ int LDVectors, int* ifail, int& info ) {
+
+ TypedWorkSpace<int> iwork(5*n);
+ float wsize[1];
+ ssyevx_( jobz, range, uplo, n, a, lda, vl, vu, il, iu, abstol, nFound,
+ values, vectors, LDVectors, wsize, -1, iwork.data, ifail, info,
+ 1, 1, 1);
+
+ int lwork = (int)wsize[0];
+ TypedWorkSpace<float> work(lwork);
+ ssyevx_( jobz, range, uplo, n, a, lda, vl, vu, il, iu, abstol, nFound,
+ values, vectors, LDVectors, work.data, lwork, iwork.data, ifail,
+ info, 1, 1, 1);
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "ssyevx", info );
+ }
+
+
+ return;
+}
+
+template <> void LapackInterface::syevx<double>( char jobz, char range,
+ char uplo, int n, double* a, int lda, double vl, double vu, int il,
+ int iu, double abstol, int& nFound, double *values, double* vectors,
+ int LDVectors, int* ifail, int& info ) {
+
+ TypedWorkSpace<int> iwork(5*n);
+ double wsize[1];
+ dsyevx_( jobz, range, uplo, n, a, lda, vl, vu, il, iu, abstol, nFound,
+ values, vectors, LDVectors, wsize, -1, iwork.data, ifail, info,
+ 1, 1, 1);
+
+ int lwork = (int)wsize[0];
+ TypedWorkSpace<double> work(lwork);
+ dsyevx_( jobz, range, uplo, n, a, lda, vl, vu, il, iu, abstol, nFound,
+ values, vectors, LDVectors, work.data, lwork, iwork.data, ifail,
+ info, 1, 1, 1);
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "dsyevx", info );
+ }
+ return;
+}
+
+template <> void LapackInterface::syevx<std::complex<double> >( char jobz,
+ char range, char uplo, int n, std::complex<double>* a, int lda, double vl,
+ double vu, int il, int iu, double abstol, int& nFound, double *values,
+ std::complex<double>* vectors, int LDVectors, int* ifail, int& info ) {
+
+ TypedWorkSpace<int> iwork(5*n);
+ TypedWorkSpace<double> rwork( 7*n );
+ std::complex<double> wsize[1];
+ zheevx_( jobz, range, uplo, n, a, lda, vl, vu, il, iu, abstol, nFound,
+ values, vectors, LDVectors, wsize, -1, rwork.data, iwork.data,
+ ifail, info, 1, 1, 1);
+
+ int lwork = (int)wsize[0].real();
+ TypedWorkSpace<std::complex<double> > work(lwork);
+ zheevx_( jobz, range, uplo, n, a, lda, vl, vu, il, iu, abstol, nFound,
+ values, vectors, LDVectors, work.data, lwork, rwork.data,
+ iwork.data, ifail, info, 1, 1, 1);
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "zheevx", info );
+ }
+ return;
+}
+
+template <> void LapackInterface::syevx<std::complex<float> >( char jobz,
+ char range, char uplo, int n, std::complex<float>* a, int lda, float vl,
+ float vu, int il, int iu, float abstol, int& nFound, float *values,
+ std::complex<float>* vectors, int LDVectors, int* ifail, int& info ) {
+
+ TypedWorkSpace<int> iwork(5*n);
+ TypedWorkSpace<float> rwork( 7*n );
+ std::complex<float> wsize[1];
+ cheevx_( jobz, range, uplo, n, a, lda, vl, vu, il, iu, abstol, nFound,
+ values, vectors, LDVectors, wsize, -1, rwork.data, iwork.data, ifail,
+ info, 1, 1, 1);
+
+ int lwork = (int)wsize[0].real();
+ TypedWorkSpace<std::complex<float> > work(lwork);
+ cheevx_( jobz, range, uplo, n, a, lda, vl, vu, il, iu, abstol, nFound,
+ values, vectors, LDVectors, work.data, lwork, rwork.data, iwork.data,
+ ifail, info, 1, 1, 1);
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "cheevx", info );
+ }
+ return;
+}
+
+// all eigenvalues for symmetric matrices eigen vectors returned in a
+template <class T>
+void LapackInterface::syev( char jobz, char uplo, int n,
+ T* a, int lda, typename CNT<T>::TReal * eigenValues, int& info ) {
+ assert(false);
+}
+template <> void LapackInterface::syev<float>( char jobz, char uplo, int n,
+ float* a, int lda, float* eigenValues, int& info ) {
+
+ float wsize[1];
+ ssyev_( jobz, uplo, n, a, lda, eigenValues, wsize, -1, info, 1, 1 );
+ int lwork = (int)wsize[0];
+ TypedWorkSpace<float> work(lwork);
+ ssyev_( jobz, uplo, n, a, lda, eigenValues, work.data, lwork, info, 1, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "ssyev", info );
+ }
+}
+
+template <> void LapackInterface::syev<double>( char jobz, char uplo, int n,
+ double* a, int lda, double* eigenValues, int& info ) {
+
+ double wsize[1];
+ dsyev_( jobz, uplo, n, a, lda, eigenValues, wsize, -1, info, 1, 1 );
+ int lwork = (int)wsize[0];
+ TypedWorkSpace<double> work(lwork);
+ dsyev_( jobz, uplo, n, a, lda, eigenValues, work.data, lwork, info, 1, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "dsyev", info );
+ }
+}
+
+template <> void LapackInterface::syev<std::complex<float> >( char jobz, char uplo, int n,
+ std::complex<float>* a, int lda, float* eigenValues, int& info ) {
+
+ std::complex<float> wsize[1];
+ int l = 3*n -2;
+ if( l < 1 ) l = 1;
+
+ TypedWorkSpace<float> rwork( l );
+
+ cheev_( jobz, uplo, n, a, lda, eigenValues, wsize, -1, rwork.data, info, 1, 1 );
+
+ int lwork = (int)wsize[0].real();
+ TypedWorkSpace<std::complex<float> > work(lwork);
+ cheev_( jobz, uplo, n, a, lda, eigenValues, work.data, lwork, rwork.data, info, 1, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "cheev", info );
+ }
+}
+
+template <> void LapackInterface::syev<std::complex<double> >( char jobz, char uplo, int n,
+ std::complex<double>* a, int lda, double* eigenValues, int& info ) {
+
+ std::complex<double> wsize[1];
+ int l = 3*n -2;
+ if( l < 1 ) l = 1;
+
+ TypedWorkSpace<double> rwork( l );
+
+ zheev_( jobz, uplo, n, a, lda, eigenValues, wsize, -1, rwork.data, info, 1, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "zheev", info );
+ }
+
+ int lwork = (int)wsize[0].real();
+ TypedWorkSpace<std::complex<double> > work(lwork);
+ zheev_( jobz, uplo, n, a, lda, eigenValues, work.data, lwork, rwork.data, info, 1, 1 );
+}
+template <class T>
+void LapackInterface::gesdd( char jobz, int m, int n, T* a, int lda,
+ typename CNT<T>::TReal* s, T* u, int ldu, T* vt,
+ int ldvt, int& info) {
+ assert(false);
+}
+template <>
+void LapackInterface::gesdd<float>( char jobz, int m, int n, float* a, int lda,
+ float* s, float* u, int ldu, float* vt,
+ int ldvt, int& info ){
+
+ int lwork;
+ int mn = (m < n ) ? m : n; // min(m,n)
+ TypedWorkSpace<float> work(1);
+ TypedWorkSpace<int> iwork(8*mn);
+
+ sgesdd_( jobz, m, n, a, lda, s, u, ldu, vt, ldvt, work.data, -1, iwork.data, info, 1);
+ lwork = (int)work.data[0];
+ work.resize(lwork);
+ sgesdd_( jobz, m, n, a, lda, s, u, ldu, vt, ldvt, work.data, lwork, iwork.data, info, 1);
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "sgesdd", info );
+ }
+}
+template <>
+void LapackInterface::gesdd<double>( char jobz, int m, int n, double* a, int lda,
+ double* s, double* u, int ldu, double* vt,
+ int ldvt, int& info ){
+
+ int lwork;
+ int mn = (m < n ) ? m : n; // min(m,n)
+ TypedWorkSpace<double> work(1);
+ TypedWorkSpace<int> iwork(8*mn);
+
+ dgesdd_( jobz, m, n, a, lda, s, u, ldu, vt, ldvt, work.data, -1, iwork.data, info, 1);
+ lwork = (int)work.data[0];
+ work.resize(lwork);
+ dgesdd_( jobz, m, n, a, lda, s, u, ldu, vt, ldvt, work.data, lwork, iwork.data, info, 1);
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "dgesdd", info );
+ }
+}
+// TODO REMOVE when added to SimTKlapack.
+extern "C" {
+ extern void cgesdd_(const char& jobz, const int& m, const int& n, std::complex<float> *a, const int& lda, float *s, std::complex<float> *u, const int& ldu, std::complex<float> *vt, const int& ldvt, std::complex<float> *work, const int& lwork, float *rwork, int *iwork, int& info, int jobz_len=1 );
+ extern void zgesdd_(const char& jobz, const int& m, const int& n, std::complex<double> *a, const int& lda, double *s, std::complex<double> *u, const int& ldu, std::complex<double> *vt, const int& ldvt, std::complex<double> *work, const int& lwork, double *rwork, int *iwork, int& info, int jobz_len=1 );
+}
+template <>
+void LapackInterface::gesdd<std::complex<float> >( char jobz, int m, int n,
+ std::complex<float>* a, int lda, float* s, std::complex<float>* u,
+ int ldu, std::complex<float>* vt, int ldvt, int& info ){
+
+ int mn = (m < n ) ? m : n; // min(m,n)
+ TypedWorkSpace<float> rwork;
+ if( jobz == 'N' ) {
+ rwork.resize(5*mn);
+ } else {
+ rwork.resize(5*mn*mn + 7*mn);
+ }
+ TypedWorkSpace<std::complex<float> > work(1);
+ TypedWorkSpace<int> iwork(8*mn);
+
+ cgesdd_( jobz, m, n, a, lda, s, u, ldu, vt, ldvt, work.data, -1, rwork.data, iwork.data, info, 1);
+ int lwork = (int)work.data[0].real();
+ work.resize(lwork);
+ cgesdd_( jobz, m, n, a, lda, s, u, ldu, vt, ldvt, work.data, lwork, rwork.data, iwork.data, info, 1);
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "cgesdd", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::gesdd<std::complex<double> >( char jobz, int m, int n,
+ std::complex<double>* a, int lda, double* s, std::complex<double>* u,
+ int ldu, std::complex<double>* vt, int ldvt, int& info ){
+
+ int mn = (m < n ) ? m : n; // min(m,n)
+ TypedWorkSpace<double> rwork;
+ if( jobz == 'N' ) {
+ rwork.resize(5*mn);
+ } else {
+ rwork.resize(5*mn*mn + 7*mn);
+ }
+ TypedWorkSpace<std::complex<double> > work(1);
+ TypedWorkSpace<int> iwork(8*mn);
+
+ zgesdd_( jobz, m, n, a, lda, s, u, ldu, vt, ldvt, work.data, -1, rwork.data, iwork.data, info, 1);
+ int lwork = (int)work.data[0].real();
+ work.resize(lwork);
+ zgesdd_( jobz, m, n, a, lda, s, u, ldu, vt, ldvt, work.data, lwork, rwork.data, iwork.data, info, 1);
+
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "zgesdd", info );
+ }
+ return;
+}
+// eigenvlaues for nonsymmetric matrices
+template <class T>
+void LapackInterface::geev (char jobvl, char jobvr,
+ int n, T* a, int lda, std::complex<typename CNT<T>::TReal>* values,
+ T* vl, int ldvl, std::complex<typename CNT<T>::TReal>* vr,
+ int ldvr, T* work, int lwork, int& info ) {
+ assert(false);
+}
+
+template <> void LapackInterface::geev<double>
+ (char jobvl, char jobvr,
+ int n, double* a, int lda, std::complex<double>* values,
+ double* vl, int ldvl, std::complex<double>* rightVectors, int ldvr, double* work,
+ int lwork, int& info )
+{
+ TypedWorkSpace<double> wr(n);
+ TypedWorkSpace<double> wi(n);
+ TypedWorkSpace<double> vr(n*n);
+
+ // avoid valgrind unintialized warnings
+ for(int i=0;i<n;i++) wi.data[i] = 0;
+ dgeev_( jobvl, jobvr,
+// n, a, lda, wr.data, wi.data, vl, ldvl, vr.data, ldvr,
+ n, a, lda, wr.data, wi.data, vl, ldvl, vr.data, ldvr,
+ work, lwork, info, 1, 1);
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "dgeev", info );
+ }
+
+ for(int i=0;i<n;i++) {
+ values[i] = std::complex<double>(wr.data[i], wi.data[i] );
+ }
+
+ /*
+ ** LAPACK returns the eigen vectors as complex conjuate pairs
+ ** if the eigen value is real ( imaginary part == 0 ) then the eigen vector is real
+ ** else the vectors are returned with the real part in the jth column and the
+ ** imaginary part in the j+1 column
+ */
+ for(int j=0;j<n;j++) {
+ if( fabs(wi.data[j]) < EPS ) {
+ for(int i=0;i<n;i++) {
+ rightVectors[j*n+i] = std::complex<double>(vr.data[j*n+i], 0.0 );
+ }
+ } else {
+ for(int i=0;i<n;i++) {
+ rightVectors[j*n+i] = std::complex<double>(vr.data[j*n+i], vr.data[(j+1)*n+i]);
+ rightVectors[(j+1)*n+i] = std::complex<double>(vr.data[j*n+i], -vr.data[(j+1)*n+i]);
+ }
+ j++;
+ }
+ }
+/*
+ for(int j=0;j<n;j++) {
+ for(int i=0;i<n;i++) printf("%f %f ", rightVectors(i,j).real(), rightVectors(i,j).imag() );
+ printf("\n");
+ }
+*/
+}
+
+template <> void LapackInterface::geev<float>
+ (char jobvl, char jobvr,
+ int n, float* a, int lda, std::complex<float>* values,
+ float* vl, int ldvl, std::complex<float>* rightVectors, int ldvr, float* work,
+ int lwork, int& info )
+{
+ TypedWorkSpace<float> wr(n);
+ TypedWorkSpace<float> wi(n);
+ TypedWorkSpace<float> vr(n*n);
+
+ // avoid valgrind unintialized warnings
+ for(int i=0;i<n;i++) wi.data[i] = 0;
+
+ sgeev_( jobvl, jobvr,
+ n, a, lda, wr.data, wi.data, vl, ldvl, vr.data, ldvr,
+ work, lwork, info,
+ 1, 1);
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "sgeev", info );
+ }
+
+ for(int i=0;i<n;i++) {
+ values[i] = std::complex<float>(wr.data[i], wi.data[i] );
+ }
+ /*
+ ** LAPACK returns the eigen vectors as complex conjuate pairs
+ ** if the eigen value is real ( imaginary part == 0 ) then the eigen vector is real
+ ** else the vectors are returned with the real part in the jth column and the
+ ** imaginary part in the j+1 column
+ */
+ for(int j=0;j<n;j++) {
+ if( fabs(wi.data[j]) < (float)EPS ) {
+ for(int i=0;i<n;i++) {
+ rightVectors[j*n+i] = std::complex<float>(vr.data[j*n+i], 0.0 );
+//printf(" %f ",vr.data[j*n+i] );
+ }
+//printf("\n");
+ } else {
+ for(int i=0;i<n;i++) {
+ rightVectors[j*n+i] = std::complex<float>(vr.data[j*n+i], vr.data[(j+1)*n+i]);
+ rightVectors[(j+1)*n+i] = std::complex<float>(vr.data[j*n+i], -vr.data[(j+1)*n+i]);
+ }
+ j++;
+ }
+ }
+}
+template <> void LapackInterface::geev<std::complex<float> >
+ (char jobvl, char jobvr,
+ int n, std::complex<float>* a, int lda, std::complex<float>* values,
+ std::complex<float>* vl, int ldvl, std::complex<float>* rightVectors, int ldvr, std::complex<float>* work,
+ int lwork, int& info )
+{
+
+ TypedWorkSpace<float> Rwork(2*n);
+ cgeev_( jobvl, jobvr,
+ n, a, lda, values, vl, ldvl, rightVectors, ldvr,
+ work, lwork, Rwork.data, info,
+ 1, 1);
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "cgeev", info );
+ }
+
+}
+
+template <> void LapackInterface::geev<std::complex<double> >
+ (char jobvl, char jobvr,
+ int n, std::complex<double>* a, int lda, std::complex<double>* values,
+ std::complex<double>* vl, int ldvl, std::complex<double>* rightVectors, int ldvr, std::complex<double>* work,
+ int lwork, int& info )
+{
+
+ TypedWorkSpace<double> Rwork(2*n);
+ zgeev_( jobvl, jobvr,
+ n, a, lda, values, vl, ldvl, rightVectors, ldvr,
+ work, lwork, Rwork.data, info,
+ 1, 1);
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "zgeev", info );
+ }
+
+}
+template <>
+void LapackInterface::getrf<double>( const int m, const int n, double *lu, const int lda, int *pivots, int& info ) {
+ dgetrf_(m, n, lu, lda, pivots, info );
+ return;
+}
+template <>
+void LapackInterface::getrf<float>( const int m, const int n, float *lu, const int lda, int *pivots, int& info ) {
+ sgetrf_(m, n, lu, lda, pivots, info );
+ return;
+}
+template <>
+void LapackInterface::getrf<std::complex<double> >( const int m, const int n, std::complex<double> *lu, const int lda, int *pivots, int& info ) {
+ zgetrf_(m, n, lu, lda, pivots, info );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "zgetrf", info );
+ }
+
+ return;
+}
+template <>
+void LapackInterface::getrf<std::complex<float> >( const int m, const int n, std::complex<float> *lu, const int lda, int *pivots, int& info ) {
+ cgetrf_(m, n, lu, lda, pivots, info );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "cgetrf", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::tzrzf<double>( const int& m, const int& n, double* a, const int& lda, double* tau, double* work, const int& lwork, int& info ) {
+ dtzrzf_(m, n, a, lda, tau, work, lwork, info );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "dtzrzf", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::tzrzf<float>( const int& m, const int& n, float* a, const int& lda, float* tau, float* work, const int& lwork, int& info ) {
+ stzrzf_(m, n, a, lda, tau, work, lwork, info );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "stzrzf", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::tzrzf<std::complex<double> >( const int& m, const int& n, std::complex<double>* a, const int& lda, std::complex<double>* tau, std::complex<double>* work, const int& lwork, int& info ) {
+ ztzrzf_(m, n, a, lda, tau, work, lwork, info );
+ return;
+}
+
+template <>
+void LapackInterface::tzrzf<std::complex<float> >( const int& m, const int& n, std::complex<float>* a, const int& lda, std::complex<float>* tau, std::complex<float>* work, const int& lwork, int& info ) {
+ ctzrzf_(m, n, a, lda, tau, work, lwork, info );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "ctzrzf", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::geqp3<double>( const int& m, const int& n, double* a, const int& lda, int *pivots, double* tau, double* work, const int& lwork, int& info ) {
+ dgeqp3_( m, n, a, lda, pivots, tau, work, lwork, info );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "dgeqp3", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::geqp3<float>( const int& m, const int& n, float* a, const int& lda, int *pivots, float* tau, float* work, const int& lwork, int& info ) {
+ sgeqp3_( m, n, a, lda, pivots, tau, work, lwork, info );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "sgeqp3", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::geqp3<std::complex<float> >( const int& m, const int& n, std::complex<float>* a, const int& lda, int *pivots, std::complex<float>* tau, std::complex<float>* work, const int& lwork, int& info ) {
+ TypedWorkSpace<float> rwork(2*n);
+ cgeqp3_( m, n, a, lda, pivots, tau, work, lwork, rwork.data, info );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "cgeqp3", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::geqp3<std::complex<double> >( const int& m, const int& n, std::complex<double>* a, const int& lda, int *pivots, std::complex<double>* tau, std::complex<double>* work, const int& lwork, int& info ) {
+ TypedWorkSpace<double> rwork(2*n);
+ zgeqp3_( m, n, a, lda, pivots, tau, work, lwork, rwork.data, info );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "zgeqp3", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::lascl<double>( const char& type, const int& kl, const int& ku, const double& cfrom, const double& cto, const int& m, const int& n, double* a, const int& lda, int& info ) {
+//TODO dlascl_( type, kl, ku, cfrom, cto, m, n, a, lda, info, 1 );
+ dlascl_( type, kl, ku, &cfrom, &cto, m, n, a, lda, info, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "dlascl", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::lascl<float>( const char& type, const int& kl, const int& ku, const float& cfrom, const float& cto, const int& m, const int& n, float* a, const int& lda, int& info ) {
+// TODO slascl_( type, kl, ku, cfrom, cto, m, n, a, lda, info, 1 );
+ slascl_( type, kl, ku, &cfrom, &cto, m, n, a, lda, info, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "slascl", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::lascl<std::complex<float> >( const char& type, const int& kl, const int& ku, const float& cfrom, const float& cto, const int& m, const int& n, std::complex<float>* a, const int& lda, int& info) {
+// TODO clascl_( type, kl, ku, cfrom, cto, m, n, a, lda, info, 1 );
+ clascl_( type, kl, ku, &cfrom, &cto, m, n, a, lda, info, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "clascl", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::lascl<std::complex<double> >( const char& type, const int& kl, const int& ku, const double& cfrom, const double& cto, const int& m, const int& n, std::complex<double>* a, const int& lda, int& info) {
+// TODO zlascl_( type, kl, ku, cfrom, cto, m, n, a, lda, info, 1 );
+ zlascl_( type, kl, ku, &cfrom, &cto, m, n, a, lda, info, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "zlascl", info );
+ }
+ return;
+}
+
+
+template <>
+double LapackInterface::lange<float>( const char& norm, const int& m, const int& n, const float* a, const int& lda){
+/*
+ TODO JACKM because g77 returns FORTRAN REAL's as doubles and gfortran returns them as floats
+ changes this once everyone has changed to new libraries and SimTKlapack.h has been updated
+ TypedWorkSpace<float> work(m);
+ return( slange_( norm, m, n, a, lda, work.data, 1 ) );
+*/
+
+ TypedWorkSpace<double> work(m);
+ TypedWorkSpace<double> da(m*n);
+ // Copy float matrix in Lapack full storage format into temporary
+ // dense double matrix.
+ for (int j=0; j<n; j++)
+ for (int i=0; i<m; i++)
+ da.data[j*m + i] = a[j*lda + i];
+ // leading dimension of da.data is m now, not lda
+ return( dlange_( norm, m, n, da.data, m, work.data, 1 ) );
+}
+
+template <>
+double LapackInterface::lange<double>( const char& norm, const int& m, const int& n, const double* a, const int& lda ){
+ TypedWorkSpace<double> work(m);
+ return( dlange_( norm, m, n, a, lda, work.data, 1 ) );
+}
+
+template <>
+double LapackInterface::lange<std::complex<float> >( const char& norm, const int& m, const int& n, const std::complex<float>* a, const int& lda ){
+/*
+ TODO JACKM because g77 returns FORTRAN REAL's as doubles and gfortran returns them as floats
+ switch to correct LAPACK call when everyone uses SimTKlapack.h has been updated
+ TypedWorkSpace<float> work(m);
+ return( clange_( norm, m, n, a, lda, work.data, 1 ) );
+*/
+ TypedWorkSpace<double> work(m);
+ TypedWorkSpace<std::complex<double> > za(m*n);
+
+ // Copy float matrix in Lapack full storage format into temporary
+ // dense double matrix.
+ for (int j=0; j<n; j++)
+ for (int i=0; i<m; i++)
+ za.data[j*m + i] = a[j*lda + i];
+ // leading dimension of za.data is m now, not lda
+ return zlange_( norm, m, n, za.data, m, work.data, 1 );
+}
+
+template <>
+double LapackInterface::lange<std::complex<double> >( const char& norm, const int& m, const int& n, const std::complex<double>* a, const int& lda) {
+ TypedWorkSpace<double> work(m);
+ return( zlange_( norm, m, n, a, lda, work.data, 1 ) );
+}
+
+template <>
+void LapackInterface::ormqr<float>(const char& side, const char& trans, const int& m, const int& n, const int& k, float* a, const int& lda, float *tau, float *c__, const int& ldc, float* work, const int& lwork, int& info) {
+
+ sormqr_( side, trans, m, n, k, a, lda, tau, c__, ldc, work, lwork, info, 1, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "sormqr", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::ormqr<double>(const char& side, const char& trans, const int& m, const int& n, const int& k, double* a, const int& lda, double *tau, double *c__, const int& ldc, double* work, const int& lwork, int& info) {
+
+ dormqr_( side, trans, m, n, k, a, lda, tau, c__, ldc, work, lwork, info, 1, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "dormqr", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::ormqr<std::complex<double> >(const char& side, const char& trans, const int& m, const int& n, const int& k, std::complex<double>* a, const int& lda, std::complex<double> *tau, std::complex<double> *c__, const int& ldc, std::complex<double>* work, const int& lwork, int& info) {
+
+ zunmqr_( side, trans, m, n, k, a, lda, tau, c__, ldc, work, lwork, info, 1, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "zunmqr", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::ormqr<std::complex<float> >(const char& side, const char& trans, const int& m, const int& n, const int& k, std::complex<float>* a, const int& lda, std::complex<float> *tau, std::complex<float> *c__, const int& ldc, std::complex<float>* work, const int& lwork, int& info) {
+
+ cunmqr_( side, trans, m, n, k, a, lda, tau, c__, ldc, work, lwork, info, 1, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "cunmqr", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::trsm<float>(const char& side, const char& uplo, const char& transA, const char& diag, const int& m, const int& n, const float& alpha, const float* a, const int& lda, float* b, const int& ldb ) {
+ strsm_( side, uplo, transA, diag, m, n, alpha, a, lda, b, ldb, 1, 1, 1 );
+ return;
+}
+
+template <>
+void LapackInterface::trsm<double>(const char& side, const char& uplo, const char& transA, const char& diag, const int& m, const int& n, const double& alpha, const double* a, const int& lda, double* b, const int& ldb ) {
+ dtrsm_( side, uplo, transA, diag, m, n, alpha, a, lda, b, ldb, 1, 1, 1 );
+ return;
+}
+
+template <>
+void LapackInterface::trsm<std::complex<double> >(const char& side, const char& uplo, const char& transA, const char& diag, const int& m, const int& n, const std::complex<double>& alpha, const std::complex<double>* a, const int& lda, std::complex<double>* b, const int& ldb ) {
+ ztrsm_( side, uplo, transA, diag, m, n, alpha, a, lda, b, ldb, 1, 1, 1 );
+ return;
+}
+
+template <>
+void LapackInterface::trsm<std::complex<float> >(const char& side, const char& uplo, const char& transA, const char& diag, const int& m, const int& n, const std::complex<float>& alpha, const std::complex<float>* a, const int& lda, std::complex<float>* b, const int& ldb ) {
+ ctrsm_( side, uplo, transA, diag, m, n, alpha, a, lda, b, ldb, 1, 1, 1 );
+ return;
+}
+template <>
+void LapackInterface::ormrz<float>(const char& side, const char& trans, const int& m, const int& n, const int& k, const int& l, float* a, const int& lda, float* tau, float* c__, const int& ldc, float* work, const int& lwork, int& info) {
+ sormrz_( side, trans, m, n, k, l, a, lda, tau, c__, ldc, work, lwork, info, 1, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "sormrz", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::ormrz<double>(const char& side, const char& trans, const int& m, const int& n, const int& k, const int& l, double* a, const int& lda, double* tau, double* c__, const int& ldc, double* work, const int& lwork, int& info) {
+ dormrz_( side, trans, m, n, k, l, a, lda, tau, c__, ldc, work, lwork, info, 1, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "dormrz", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::ormrz<std::complex<float> >(const char& side, const char& trans, const int& m, const int& n, const int& k, const int& l, std::complex<float>* a, const int& lda, std::complex<float>* tau, std::complex<float>* c__, const int& ldc, std::complex<float>* work, const int& lwork, int& info) {
+ cunmrz_( side, trans, m, n, k, l, a, lda, tau, c__, ldc, work, lwork, info, 1, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "cunmrz", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::ormrz<std::complex<double> >(const char& side, const char& trans, const int& m, const int& n, const int& k, const int& l, std::complex<double>* a, const int& lda, std::complex<double>* tau, std::complex<double>* c__, const int& ldc, std::complex<double>* work, const int& lwork, int& info) {
+ zunmrz_( side, trans, m, n, k, l, a, lda, tau, c__, ldc, work, lwork, info, 1, 1 );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "zunmrz", info );
+ }
+ return;
+}
+
+template <>
+void LapackInterface::copy<float>( const int& n, const float* x, const int& incx, float* y, const int& incy) {
+ scopy_(n, x, incx, y, incy );
+ return;
+}
+
+template <>
+void LapackInterface::copy<double>( const int& n, const double* x, const int& incx, double* y, const int& incy) {
+ dcopy_(n, x, incx, y, incy );
+ return;
+}
+
+template <>
+void LapackInterface::copy<std::complex<float> >( const int& n, const std::complex<float>* x, const int& incx, std::complex<float>* y, const int& incy) {
+ ccopy_(n, x, incx, y, incy );
+ return;
+}
+
+template <>
+void LapackInterface::copy<std::complex<double> >( const int& n, const std::complex<double>* x, const int& incx, std::complex<double>* y, const int& incy) {
+ zcopy_(n, x, incx, y, incy );
+ return;
+}
+template <>
+void LapackInterface::getMachineUnderflow<float>( float& underFlow ) {
+ underFlow = slamch_('S');
+ return;
+}
+template <>
+void LapackInterface::getMachineUnderflow<double>( double& underFlow ) {
+ underFlow = dlamch_('S');
+ return;
+}
+template <>
+void LapackInterface::getMachinePrecision<float>( float& smallNumber, float& bigNumber ) {
+
+ smallNumber = slamch_( 'S' )/slamch_( 'P' );
+ bigNumber = 1.f/smallNumber;
+ slabad_(smallNumber, bigNumber );
+}
+
+template <>
+void LapackInterface::getMachinePrecision<double>( double& smallNumber, double& bigNumber ) {
+
+ smallNumber = dlamch_( 'S' )/dlamch_( 'P' );
+ bigNumber = 1.0/smallNumber;
+ dlabad_(smallNumber, bigNumber );
+}
+
+template <>
+void LapackInterface::laic1<float>(const int& job, const int& j, const float* x, const float& sest, const float* w, const float& gamma, float& sestpr, float& s, float& c__ ) {
+ slaic1_( job, j, x, sest, w, gamma, sestpr, s, c__ );
+ return;
+}
+
+template <>
+void LapackInterface::laic1<double>(const int& job, const int& j, const double* x, const double& sest, const double* w, const double& gamma, double& sestpr, double& s, double& c__ ) {
+ dlaic1_( job, j, x, sest, w, gamma, sestpr, s, c__ );
+ return;
+}
+
+template <>
+void LapackInterface::laic1<std::complex<float> >(const int& job, const int& j, const std::complex<float>* x, const float& sest, const std::complex<float>* w, const std::complex<float>& gamma, float& sestpr, std::complex<float>& s, std::complex<float>& c__ ) {
+ claic1_( job, j, x, sest, w, gamma, sestpr, s, c__ );
+ return;
+}
+
+
+template <>
+void LapackInterface::laic1<std::complex<double> >(const int& job, const int& j, const std::complex<double>* x, const double& sest, const std::complex<double>* w, const std::complex<double>& gamma, double& sestpr, std::complex<double>& s, std::complex<double>& c__ ) {
+ zlaic1_( job, j, x, sest, w, gamma, sestpr, s, c__ );
+ return;
+}
+
+template <>
+void LapackInterface::potrf<double>( const char& uplo, const int n, double* a, const int lda, int& info ) {
+
+ dpotrf_(uplo, n, a, lda, info);
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "dpotrf", info );
+ }
+
+ return;
+ }
+template <>
+void LapackInterface::potrf<float>( const char& uplo, const int n, float* a, const int lda, int& info ) {
+
+ spotrf_(uplo, n, a, lda, info);
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "spotrf", info );
+ }
+
+ return;
+ }
+template <>
+void LapackInterface::potrf<std::complex<double> >( const char& uplo, const int n, std::complex<double>* a, const int lda, int& info ) {
+
+ zpotrf_(uplo, n, a, lda, info);
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "zpotrf", info );
+ }
+
+ return;
+ }
+template <>
+void LapackInterface::potrf<std::complex<float> >( const char& uplo, const int n, std::complex<float>* a, const int lda, int& info ) {
+
+ cpotrf_(uplo, n, a, lda, info);
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "cpotrf", info );
+ }
+
+ return;
+ }
+template <>
+void LapackInterface::sytrf<float>( const char& uplo, const int n, float* a, const int lda, int* pivots, float* work, const int lwork, int& info){
+
+ ssytrf_( uplo, n, a, lda, pivots, work, lwork, info );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "ssytrf", info );
+ }
+ return;
+}
+template <>
+void LapackInterface::sytrf<double>( const char& uplo, const int n, double* a, const int lda, int* pivots, double* work, const int lwork, int& info){
+
+ dsytrf_( uplo, n, a, lda, pivots, work, lwork, info );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "dsytrf", info );
+ }
+ return;
+}
+template <>
+void LapackInterface::sytrf<std::complex<double> >( const char& uplo, const int n, std::complex<double>* a, const int lda, int* pivots, std::complex<double>* work, const int lwork, int& info){
+
+ zsytrf_( uplo, n, a, lda, pivots, work, lwork, info );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "zsytrf", info );
+ }
+ return;
+}
+template <>
+void LapackInterface::sytrf<std::complex<float> >( const char& uplo, const int n, std::complex<float>* a, const int lda, int* pivots, std::complex<float>* work, const int lwork, int& info){
+
+ csytrf_( uplo, n, a, lda, pivots, work, lwork, info );
+
+ if( info < 0 ) {
+ SimTK_THROW2( SimTK::Exception::IllegalLapackArg, "csytrf", info );
+ }
+ return;
+}
+template <typename T>
+void LapackInterface::sytrf( const char& uplo, const int n, T* a, const int lda, int *pivots, T* work, const int lwork, int& info ) { assert(false); }
+
+template <>
+int LapackInterface::ilaenv<double>( const int& ispec, const char* name, const char *opts, const int& n1, const int& n2, const int& n3, const int& n4 ) {
+ char d[10];
+ d[0] = 'd';
+ d[1] = '\0';
+ return (ilaenv_( ispec, strcat( d, name), opts, n1, n2, n3, n3, 6, (int)strlen(opts)) );
+}
+template <>
+int LapackInterface::ilaenv<float>( const int& ispec, const char* name, const char *opts, const int& n1, const int& n2, const int& n3, const int& n4 ) {
+ char s[10];
+ s[0] = 's';
+ s[1] = '\0';
+ return (ilaenv_( ispec, strcat( s, name), opts, n1, n2, n3, n3, 6, (int)strlen(opts)) );
+}
+template <>
+int LapackInterface::ilaenv<std::complex<double> >( const int& ispec, const char* name, const char *opts, const int& n1, const int& n2, const int& n3, const int& n4 ) {
+ char z[10];
+ z[0] = 'z';
+ z[1] = '\0';
+ return (ilaenv_( ispec, strcat( z, name), opts, n1, n2, n3, n3, 6, (int)strlen(opts)) );
+}
+template <>
+int LapackInterface::ilaenv<std::complex<float> >( const int& ispec, const char* name, const char *opts, const int& n1, const int& n2, const int& n3, const int& n4 ) {
+ char c[10];
+ c[0] = 'c';
+ c[1] = '\0';
+ return (ilaenv_( ispec, strcat( c, name), opts, n1, n2, n3, n3, 6, (int)strlen(opts)) );
+}
+
+
+} // namespace SimTK
+
diff --git a/SimTKmath/LinearAlgebra/src/LapackInterface.h b/SimTKmath/LinearAlgebra/src/LapackInterface.h
new file mode 100644
index 0000000..edd31b4
--- /dev/null
+++ b/SimTKmath/LinearAlgebra/src/LapackInterface.h
@@ -0,0 +1,143 @@
+
+#ifndef SimTK_SIMMATH_LAPACK_INTERFACE_H_
+#define SimTK_SIMMATH_LAPACK_INTERFACE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * These is a templatized, C++ callable interface to LAPACK and BLAS.
+ * Each method must be explicitly specialized for the supported precisions.
+ */
+
+
+#include "SimTKcommon.h"
+#include "SimTKlapack.h"
+
+namespace SimTK {
+
+class LapackInterface {
+
+public:
+
+static int getLWork( float* work);
+static int getLWork( double* work);
+static int getLWork( std::complex<float>* work);
+static int getLWork( std::complex<double>* work);
+
+template <class T> static
+void gelss( int m, int n, int mn, int nrhs,
+ T* a, int lda, T* b, int ldb, typename CNT<T>::TReal* s,
+ typename CNT<T>::TReal rcond, int& rank, int& info);
+
+template <class T> static
+void gesdd( char jobz, int m, int n, T* a, int lda,
+ typename CNT<T>::TReal* s, T* u, int ldu,
+ T* vt, int ldvt, int& info);
+
+template <class T> static
+void geev(char jobvl, char jobvr, int n, T* a, int lda,
+ std::complex<typename CNT<T>::TReal>* values,
+ T* vl, int ldvl, std::complex<typename CNT<T>::TReal>* vr,
+ int ldvr, T* work, int lwork, int& info );
+
+template <class T> static
+void syevx( char jobz, char range, char uplo, int n, T* a, int lda,
+ typename CNT<T>::TReal vl, typename CNT<T>::TReal vu, int il, int iu,
+ typename CNT<T>::TReal abstol, int& nFound, typename CNT<T>::TReal* values,
+ T* vectors, int LDVectors, int* ifail, int& info );
+
+
+template <class T> static
+void syev( char jobz, char uplo, int n, T* a_eigenVectors, int lda,
+ typename CNT<T>::TReal* eigenValues, int& info );
+
+
+/* solve system of linear equations using the LU factorization */
+template <class T> static
+void potrs( char uplo, const int ncol, const int nrhs, const T *lu, T *b );
+
+template <class T> static
+// TODO void sytrs( char uplo, const int ncol, const int nrhs, const T *lu, const int* pivots, T *b );
+void sytrs( char uplo, const int ncol, const int nrhs, T *lu, int* pivots, T *b );
+
+
+template <class T> static
+void getrs( char trans, const int ncol, const int nrhs, const T *lu, const int* pivots, T *b );
+
+template <class T> static
+void getrf( const int m, const int n, T *a, const int lda, int* pivots, int& info );
+
+template <class T> static
+void gttrf( const int m, const int n, T* dl, T* d, T* du, T* du2, int* pivots, int& info );
+
+template <class T> static
+void gbtrf( const int m, const int n, const int kl, const int ku, T* lu, const int lda, int* pivots, int& info );
+
+template <class T> static
+void potrf( const char& uplo, const int n, T* lu, const int lda, int& info );
+
+template <class T> static
+void sytrf( const char& uplo, const int n, T* a, const int lda, int* pivots, T* work, const int lwork, int& info );
+
+template <class T> static
+int ilaenv( const int& ispec, const char* name, const char* opts, const int& n1, const int& n2, const int& n3, const int& n4 );
+
+template <class T> static
+void getMachinePrecision( T& smallNumber, T& bigNumber );
+
+template <class T> static
+void getMachineUnderflow( T& underFlow );
+
+template <class T> static
+void tzrzf( const int& m, const int& n, T* a, const int& lda, T* tau, T* work, const int& lwork, int& info );
+
+template <class T> static
+void geqp3( const int& m, const int& n, T* a, const int& lda, int *pivots, T* tau, T* work, const int& lwork, int& info );
+
+template <class T> static
+void lascl( const char& type, const int& kl, const int& ku, const typename CNT<T>::TReal& cfrom, const typename CNT<T>::TReal& cto, const int& m, const int& n, T* a, const int& lda, int& info );
+
+template <class T> static
+double lange( const char& norm, const int& m, const int& n, const T* a, const int& lda );
+
+template <class T> static
+void ormqr(const char& side, const char& trans, const int& m, const int& n, const int& k, T* a, const int& lda, T *tau, T *c__, const int& ldc, T* work, const int& lwork, int& info);
+
+template <class T> static
+void trsm(const char& side, const char& uplo, const char& transA, const char& diag, const int& m, const int& n, const T& alpha, const T* A, const int& lda, T* B, const int& ldb );
+
+template <class T> static
+void ormrz(const char& side, const char& trans, const int& m, const int& n, const int& k, const int& l, T* a, const int& lda, T* tau, T* c__, const int& ldc, T* work, const int& lwork, int& info);
+
+template <class T> static
+void copy( const int& n, const T* x, const int& incx, T* y, const int& incy);
+
+template <class T> static
+void laic1(const int& job, const int& j, const T* x, const typename CNT<T>::TReal& sest, const T* w, const T& gamma, typename CNT<T>::TReal& sestpr, T& s, T& c__ );
+
+}; // class LapackInterface
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_LAPACK_INTERFACE_H_
diff --git a/SimTKmath/LinearAlgebra/src/WorkSpace.h b/SimTKmath/LinearAlgebra/src/WorkSpace.h
new file mode 100644
index 0000000..1314136
--- /dev/null
+++ b/SimTKmath/LinearAlgebra/src/WorkSpace.h
@@ -0,0 +1,90 @@
+#ifndef SimTK_SIMMATH_WORK_SPACE_H_
+#define SimTK_SIMMATH_WORK_SPACE_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 <cstdio>
+#include <complex>
+#include <cassert>
+#include <iostream>
+
+
+namespace SimTK {
+
+template <typename T>
+class TypedWorkSpace {
+ public:
+
+ // copy constructor
+ TypedWorkSpace( const TypedWorkSpace& c ) {
+ size = c.size;
+
+ if( size == 0 ) {
+ data = 0;
+ } else {
+ data = new T[size];
+ for(int i=0;i<size;i++) data[i] = c.data[i];
+ }
+
+ }
+ TypedWorkSpace& operator=(const TypedWorkSpace& rhs) {
+ if (&rhs == this)
+ return *this;
+
+ delete [] data;
+ data = 0;
+ size = rhs.size;
+
+ if( size > 0) {
+ data = new T[size];
+ for(int i=0;i<size;i++) data[i] = rhs.data[i];
+ }
+ return *this;
+ }
+
+ explicit TypedWorkSpace( int n ) {
+ size = n;
+ data = (n==0 ? 0 : new T[n]);
+ }
+
+ TypedWorkSpace() : size(0), data(0) { }
+
+ ~TypedWorkSpace() {
+ delete [] data;
+ }
+
+ void resize( int n ) {
+ delete [] data;
+ size = n;
+ data = (n==0 ? 0 : new T[n]);
+ }
+
+ int size;
+ T* data;
+};
+
+} // namespace SimTK
+#endif // SimTK_SIMMATH_WORK_SPACE_H_
+
diff --git a/SimTKmath/Optimizers/CMakeLists.txt b/SimTKmath/Optimizers/CMakeLists.txt
new file mode 100644
index 0000000..80496f8
--- /dev/null
+++ b/SimTKmath/Optimizers/CMakeLists.txt
@@ -0,0 +1,18 @@
+# visit the IpOpt directory to gather up source
+add_subdirectory(src/IpOpt)
+
+# add in local source and headers here
+FILE(GLOB src_files ./src/*.cpp)
+FILE(GLOB incl_files ./src/*.h)
+
+# append to the local scope copy, and then copy up to parent scope
+list(APPEND SOURCE_FILES ${src_files})
+set(SOURCE_FILES ${SOURCE_FILES} PARENT_SCOPE)
+
+list(APPEND SOURCE_INCLUDE_FILES ${incl_files})
+set(SOURCE_INCLUDE_FILES ${SOURCE_INCLUDE_FILES} PARENT_SCOPE)
+
+# pass up the chain
+set(SOURCE_INCLUDE_DIRS ${SOURCE_INCLUDE_DIRS} PARENT_SCOPE)
+set(SOURCE_GROUPS ${SOURCE_GROUPS} PARENT_SCOPE)
+set(SOURCE_GROUP_FILES ${SOURCE_GROUP_FILES} PARENT_SCOPE)
diff --git a/SimTKmath/Optimizers/src/CFSQPOptimizer.cpp b/SimTKmath/Optimizers/src/CFSQPOptimizer.cpp
new file mode 100755
index 0000000..7d38178
--- /dev/null
+++ b/SimTKmath/Optimizers/src/CFSQPOptimizer.cpp
@@ -0,0 +1,437 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: Eran Guendelman, Frank C. Anderson *
+ * *
+ * 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 "CFSQPOptimizer.h"
+#include <string>
+
+//TODO only works in double precision without some mods
+#if SimTK_DEFAULT_PRECISION == 2
+
+// ----------------------------------------------------------------------
+// Code to manage the library loading
+// ----------------------------------------------------------------------
+
+#ifndef _WIN32
+#define SIMTK_PORTABLE_HMODULE void *
+#define SIMTK_PORTABLE_HINSTANCE void *
+#define WINAPI
+#include <dlfcn.h>
+#define GetProcAddress(handle, proc) dlsym(handle, proc)
+#define LoadLibrary(name) dlopen(name, RTLD_LAZY | RTLD_GLOBAL)
+const char *LoadLibraryErrorMessage() { return dlerror(); }
+#else
+#include <windows.h>
+#define SIMTK_PORTABLE_HMODULE HMODULE
+#define SIMTK_PORTABLE_HINSTANCE HINSTANCE
+const char *LoadLibraryErrorMessage() { return 0; }
+#endif
+
+
+#if defined(_WIN32)
+ static const char *CFSQP_LIBRARY_NAME = "osimCFSQP.dll";
+#elif defined(__APPLE__)
+ static const char *CFSQP_LIBRARY_NAME = "libosimCFSQP.dylib";
+#else
+ static const char *CFSQP_LIBRARY_NAME = "libosimCFSQP.so";
+#endif
+
+typedef void* (*CFSQP_FUNCTION)(int,int,int,int,int,int,int,int,int,int *,int,int,
+ int,int *,double,double,double,double,double *,
+ double *,double *,double *,double *,double *,
+ void (*)(int,int,double *,double *,void *),
+ void (*)(int,int,double *,double *,void *),
+ void (*)(int,int,double *,double *,
+ void (*)(int,int,double *,double *,void *),void *),
+ void (*)(int,int,double *,double *,
+ void (*)(int,int,double *,double *,void *),void *),
+ void *);
+CFSQP_FUNCTION cfsqp_fptr = 0;
+
+// ----------------------------------------------------------------------
+
+#define USE_CONSTRAINT_CACHE
+
+using namespace SimTK;
+
+//=============================================================================
+// CONSTRUCTION
+//=============================================================================
+//_____________________________________________________________________________
+/**
+ * Destructor.
+ */
+CFSQPOptimizer::
+~CFSQPOptimizer()
+{
+ delete[] _mesh;
+ delete[] _p;
+ delete[] _c;
+ delete[] _lambda;
+}
+
+Optimizer::OptimizerRep* CFSQPOptimizer::clone() const {
+ return( new CFSQPOptimizer(*this) );
+}
+
+//_____________________________________________________________________________
+CFSQPOptimizer::CFSQPOptimizer(const OptimizerSystem& sys)
+ : OptimizerRep(sys)
+{
+ bindToCFSQPLibrary();
+
+ _infinity = 1.0e10;
+ _mode = 100;
+ _epseqn = 1.0e-8;
+ _inform = 0;
+ _udelta = 0.0;
+
+ int nx = sys.getNumParameters();
+
+ // NEW MAX ITERATIONS
+ maxIterations = 4 * nx;
+
+ // SQP STUFF- NOT CURRENTLY SUPPORTED
+ int i;
+ _ncsrl = 0;
+ _ncsrn = 0;
+ _nfsr = 0;
+ int lenmesh = _nfsr+_ncsrn+_ncsrl; if(lenmesh<1) lenmesh=1;
+ _mesh = new int[lenmesh];
+ for(i=0;i<lenmesh;i++) _mesh[i] = 0;
+
+ // PERFORMANCE
+ int lenp = 1 - _nfsr; // _target->getNumContacts() - _nfsr;
+ for(int i=0;i<_nfsr;i++) lenp += _mesh[i];
+ if(lenp<1) lenp = 1;
+ _p = new double[lenp];
+
+ // CONSTRAINTS
+ int lenc = sys.getNumConstraints() - _ncsrl - _ncsrn;
+ for(int i=0;i<_ncsrn;i++) lenc += _mesh[i+_nfsr];
+ for(int i=0;i<_ncsrl;i++) lenc += _mesh[i+_nfsr+_ncsrn];
+ if(lenc<1) lenc = 1;
+ _c = new double[lenc];
+
+ // LAGRANGE MULTIPLIERS
+ _lambda = new double[nx+lenp+lenc];
+}
+
+bool CFSQPOptimizer::isAvailable()
+{
+ try {
+ bindToCFSQPLibrary();
+ } catch (const SimTK::Exception::Base &ex) {
+ std::cout << ex.getMessageText() << std::endl;
+ return false;
+ }
+ return true;
+}
+
+void CFSQPOptimizer::bindToCFSQPLibrary()
+{
+ if(cfsqp_fptr) return;
+
+ SIMTK_PORTABLE_HINSTANCE libraryHandle = LoadLibrary(CFSQP_LIBRARY_NAME);
+ if(!libraryHandle) {
+ const char *msg=LoadLibraryErrorMessage();
+ if(msg)
+ SimTK_THROW1(SimTK::Exception::Cant, std::string("Could not load CFSQP library '") + CFSQP_LIBRARY_NAME + "': " + msg);
+ else
+ SimTK_THROW1(SimTK::Exception::Cant, std::string("Could not load CFSQP library '") + CFSQP_LIBRARY_NAME + "'");
+ }
+
+ cfsqp_fptr = (CFSQP_FUNCTION)GetProcAddress(libraryHandle, "cfsqp");
+ if(!cfsqp_fptr) {
+ SimTK_THROW1(SimTK::Exception::Cant, std::string("CFSQP library '") + CFSQP_LIBRARY_NAME + "' not valid (could not find 'cfsqp' symbol)");
+ }
+
+ std::cout << "Successfully linked to CFSQP library '" << CFSQP_LIBRARY_NAME << "'" << std::endl;
+}
+
+//=============================================================================
+// COMPUTE OPTIMAL CONTROLS
+//=============================================================================
+//_____________________________________________________________________________
+/**
+ * Compute a set of optimal controls, given the input controls (xin) and
+ * the current state of the optimization target.
+ *
+ * Whether or not the optimization terminates normally, the latest value of
+ * the controls are copied to xout. It is safe to use the same pointer
+ * for xin and xout. However, in all cases the calling routine must
+ * allocate enough space for xin and xout.
+ *
+ * @param xin Values of the controls.
+ * @param xout Optimal values of the controls.
+ *
+ * @return Parameter inform of cfsqp. -1 means a fatal error.
+ */
+Real CFSQPOptimizer::
+optimize(Vector &results)
+{
+ const OptimizerSystem &sys = getOptimizerSystem();
+
+ int nx = sys.getNumParameters();
+
+ double *bl, *bu;
+ if(sys.getHasLimits()) {
+ sys.getParameterLimits(&bl,&bu);
+ } else {
+ bl = new double[nx];
+ bu = new double[nx];
+ for(int i=0; i<nx; i++) {
+ bl[i] = -_infinity;
+ bu[i] = _infinity;
+ }
+ }
+
+ double *x = &results[0];
+ int numObjectiveFunctions = 1;
+
+ // Clear cache before starting optimization (used to speed up constraint computations)
+ clearCache();
+
+ // OPTIMIZE
+ cfsqp_fptr(getOptimizerSystem().getNumParameters(),numObjectiveFunctions,_nfsr,
+ sys.getNumNonlinearInequalityConstraints(), sys.getNumInequalityConstraints(),
+ sys.getNumNonlinearEqualityConstraints(), sys.getNumEqualityConstraints(),
+ _ncsrl,_ncsrn,_mesh,
+ _mode,diagnosticsLevel,maxIterations,&_inform,_infinity,convergenceTolerance,_epseqn,_udelta,
+ bl,bu,x,_p,_c,_lambda,pFunc,cFunc,dpdxFunc,dcdxFunc,(void *)this);
+
+ if(!getOptimizerSystem().getHasLimits()) {
+ delete[] bl;
+ delete[] bu;
+ }
+
+ if(diagnosticsLevel > 0) PrintInform(_inform,std::cout);
+
+ if (_inform != 0 && _inform != 3 && _inform != 4 && _inform != 8) {
+ char buf[1024];
+ sprintf(buf, "CFSQP failed with status = %d",_inform);
+ SimTK_THROW1(SimTK::Exception::OptimizerFailed, SimTK::String(buf));
+ }
+
+ return *_p;
+}
+
+//=============================================================================
+// STATIC PERFORMANCE AND CONSTRAINT EVALUATIONS
+//=============================================================================
+//______________________________________________________________________________
+/**
+ * Compute the performance criterion.
+ */
+void CFSQPOptimizer::
+pFunc(int nparam,int j,double *x,double *p,void *cd)
+{
+ CFSQPOptimizer *cfsqp = (CFSQPOptimizer *)cd;
+ int nx=cfsqp->getOptimizerSystem().getNumParameters();
+ cfsqp->getOptimizerSystem().objectiveFunc(Vector(nx,x,true),true,*p);
+}
+//______________________________________________________________________________
+/**
+ * Compute the derivatives of the performance criterion.
+ */
+void CFSQPOptimizer::
+dpdxFunc(int nparam,int j,double *x,double *dpdx,
+ void (*dummy)(int,int,double *,double *,void *),void *cd)
+{
+ // TODO; support numerical gradients
+ CFSQPOptimizer *cfsqp = (CFSQPOptimizer *)cd;
+ int nx=cfsqp->getOptimizerSystem().getNumParameters();
+ Vector temp(nx,dpdx,true);
+ cfsqp->getOptimizerSystem().gradientFunc(Vector(nx,x,true),true,temp);
+}
+
+//______________________________________________________________________________
+/**
+ * Compute the constraints.
+ */
+void CFSQPOptimizer::
+cFunc(int nparam,int j,double *x,double *c,void *cd)
+{
+ CFSQPOptimizer *cfsqp = (CFSQPOptimizer *)cd;
+ int nx=cfsqp->getOptimizerSystem().getNumParameters();
+ // special wrapper to deal with caching
+ cfsqp->computeConstraint(Vector(nx,x,true),true,*c,j-1);
+}
+//______________________________________________________________________________
+/**
+ * Compute the gradient of the constraints.
+ */
+void CFSQPOptimizer::
+dcdxFunc(int nparam,int j,double *x,double *dcdx,
+ void (*dummy)(int,int,double *,double *,void *),
+ void *cd)
+{
+ // TODO: support numerical gradients
+ CFSQPOptimizer *cfsqp = (CFSQPOptimizer *)cd;
+ int nx=cfsqp->getOptimizerSystem().getNumParameters();
+ int nc=cfsqp->getOptimizerSystem().getNumConstraints();
+ // special wrapper to deal with caching
+ Vector temp(nx,dcdx,true);
+ cfsqp->computeConstraintGradient(Vector(nx,x,true),true,temp,j-1);
+}
+
+//=============================================================================
+// PRINT
+//=============================================================================
+//______________________________________________________________________________
+/**
+ * Print the meaning of the value returned by computeOptimalControls().
+ */
+void CFSQPOptimizer::
+PrintInform(int aInform,std::ostream &aOStream)
+{
+ switch(aInform) {
+ case(0):
+ aOStream<<"CFSQP(0): Normal termination.\n";
+ break;
+ case(1):
+ aOStream<<"CFSQP(1): User-provided initial guess is infeasible ";
+ aOStream<<"for linear constraints\n";
+ aOStream<<"and CFSQP is unable to generate a point satisfying these ";
+ aOStream<<"conditions.\n";
+ break;
+ case(2):
+ aOStream<<"CFSQP(2): The user-provided initial guess is infeasible ";
+ aOStream<<"for non-linear inequality constraints\n";
+ aOStream<<"and linear constraints, and CFSQP is unable to generate ";
+ aOStream<<"a point satisfying these constraints.\n";
+ aOStream<<"This may be due to insucient accuracy of the QP solver.\n";
+ break;
+ case(3):
+ aOStream<<"CFSQP(3): The maximum number of iterations has been ";
+ aOStream<<"reached before a solution was obtained.\n";
+ break;
+ case(4):
+ aOStream<<"CFSQP(4): The line search failed to find a new iterate.";
+ aOStream<<" The step size was smaller than\n";
+ aOStream<<"the machine precision.\n";
+ break;
+ case(5):
+ aOStream<<"CFSQP(5): Failure of the QP solver in attempting to construct d0.\n";
+ break;
+ case(6):
+ aOStream<<"CFSQP(6): Failure of the QP solver in attempting to construct d1.\n";
+ break;
+ case(7):
+ aOStream<<"CFSQP(7): Input data are not consistent. Set the print level";
+ aOStream<<" greater than 0 for more information.\n";
+ break;
+ case(8):
+ aOStream<<"CFSQP(8): The new iterate is numerically equivalent to ";
+ aOStream<<"the previous iterate,\n";
+ aOStream<<"though the stopping criterion is not yet satisfied. ";
+ aOStream<<"Relaxing the stopping criterion\n";
+ aOStream<<"shouldsolve this problem.\n";
+ break;
+ case(9):
+ aOStream<<"CFSQP(9): One of the penalty parameters exceeded ";
+ aOStream<<"the largest allowed bound.\n";
+ aOStream<<"The algorithm is having trouble satisfying a non-linear ";
+ aOStream<<"equality constraint.\n";
+ break;
+ default:
+ aOStream<<"CFSQP("<<aInform<<"): Unrecognized inform value.\n";
+ }
+}
+//=============================================================================
+// CACHING
+//=============================================================================
+void CFSQPOptimizer::
+clearCache()
+{
+#ifdef USE_CONSTRAINT_CACHE
+ int nx=getOptimizerSystem().getNumParameters();
+ int nc=getOptimizerSystem().getNumConstraints();
+ _cachedConstraintJacobian.resize(nc,nx);
+ _cachedConstraint.resize(nc);
+ _cachedConstraintJacobianParameters.resize(0);
+ _cachedConstraintParameters.resize(0);
+#endif
+}
+int CFSQPOptimizer::
+computeConstraint(const SimTK::Vector &x, const bool new_coefficients, double &c, int ic) const
+{
+ int nx=getOptimizerSystem().getNumParameters();
+ int nc=getOptimizerSystem().getNumConstraints();
+ int status = 0;
+#ifdef USE_CONSTRAINT_CACHE
+ bool cached_value_available = false;
+ if(_cachedConstraintParameters.size()) {
+ cached_value_available = true;
+ for(int i=0; i<nx; i++)
+ if(x[i] != _cachedConstraintParameters[i]) {
+ cached_value_available = false;
+ break;
+ }
+ }
+
+ if(!cached_value_available) {
+ status = getOptimizerSystem().constraintFunc(x,new_coefficients,_cachedConstraint);
+ _cachedConstraintParameters.resize(nx);
+ _cachedConstraintParameters = x;
+ }
+ c = _cachedConstraint[ic];
+#else
+ SimTK::Vector allc(nc);
+ status = constraintFunc(getOptimizerSystem(),x,new_coefficients,allc);
+ c=allc[ic];
+#endif
+ return status;
+}
+int CFSQPOptimizer::
+computeConstraintGradient(const SimTK::Vector &x, const bool new_coefficients, SimTK::Vector &dcdx, int ic) const
+{
+ int nx=getOptimizerSystem().getNumParameters();
+ int nc=getOptimizerSystem().getNumConstraints();
+ int status = 0;
+#ifdef USE_CONSTRAINT_CACHE
+ bool cached_value_available = false;
+ if(_cachedConstraintJacobianParameters.size()) {
+ cached_value_available = true;
+ for(int i=0; i<nx; i++)
+ if(x[i] != _cachedConstraintJacobianParameters[i]) {
+ cached_value_available = false;
+ break;
+ }
+ }
+
+ if(!cached_value_available) {
+ status = getOptimizerSystem().constraintJacobian(x,new_coefficients,_cachedConstraintJacobian);
+ _cachedConstraintJacobianParameters.resize(nx);
+ _cachedConstraintJacobianParameters = x;
+ }
+ for(int col=0;col<nx;col++) dcdx[col]=_cachedConstraintJacobian(ic,col);
+#else
+ SimTK::Matrix jacobian(nc,nx);
+ status = constraintJacobian(getOptimizerSystem(),x,new_coefficients,jacobian);
+ for(int col=0;col<nx;col++) dcdx[col]=jacobian(ic,col);
+#endif
+ return status;
+}
+
+#endif
+
diff --git a/SimTKmath/Optimizers/src/CFSQPOptimizer.h b/SimTKmath/Optimizers/src/CFSQPOptimizer.h
new file mode 100755
index 0000000..20df02f
--- /dev/null
+++ b/SimTKmath/Optimizers/src/CFSQPOptimizer.h
@@ -0,0 +1,136 @@
+#ifndef SimTK_CFSQP_OPTIMIZER_H_
+#define SimTK_CFSQP_OPTIMIZER_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: Eran Guendelman, Frank C. Anderson *
+ * *
+ * 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 "simmath/internal/common.h"
+
+#include "simmath/internal/OptimizerRep.h"
+
+//=============================================================================
+//=============================================================================
+/**
+ * This class provides methods for finding the optimal controls of a redundant
+ * system by applying sequential quadratic programming techniques. The core
+ * algorithm is called fsqp, "Fast Sequential Quadratic Programming".
+ */
+namespace SimTK {
+
+class CFSQPOptimizer: public Optimizer::OptimizerRep {
+//=============================================================================
+// DATA
+//=============================================================================
+private:
+ // PARAMETETERS
+ /** Mode. */
+ int _mode;
+ /** Variable that contains information about the status of an
+ optimization.*/
+ int _inform;
+ /** Value used for infinity or a very large number. */
+ double _infinity;
+ /** Convergence criterion for nonlinear equality constraints. */
+ double _epseqn;
+ /** I don't know. */
+ double _udelta;
+
+ // SEQUENTIAL INFO
+ int _ncsrl;
+ int _ncsrn;
+ int _nfsr;
+ int *_mesh;
+
+ // ALLOCATIONS
+ /** Array of performance criteria. */
+ double *_p;
+ /** Array of constraints. */
+ double *_c;
+ /** Lagrange multipliers for constraints. */
+ double *_lambda;
+
+//=============================================================================
+// METHODS
+//=============================================================================
+public:
+ //--------------------------------------------------------------------------
+ // CONSTRUCTION
+ //--------------------------------------------------------------------------
+ virtual ~CFSQPOptimizer();
+ CFSQPOptimizer(const OptimizerSystem& sys);
+
+ static bool isAvailable();
+
+ Real optimize(Vector &results);
+ OptimizerRep* clone() const;
+
+ OptimizerAlgorithm getAlgorithm() const
+ { return CFSQP; }
+
+private:
+ static void bindToCFSQPLibrary();
+
+ //--------------------------------------------------------------------------
+ // STATIC FUNCTIONS USED AS INPUT TO cfsqp()
+ //--------------------------------------------------------------------------
+ static void
+ pFunc(int nparam,int j,double *x,double *pj,void *cd);
+ static void
+ cFunc(int nparam,int j,double *x,double *cj,void *cd);
+ static void
+ dpdxFunc(int nparam,int j,double *x,double *dpdx,
+ void (*dummy)(int,int,double *,double *,void *),void *cd);
+ static void
+ dcdxFunc(int nparam,int j,double *x,double *dcdx,
+ void (*dummy)(int,int,double *,double *,void *),void *cd);
+
+ //--------------------------------------------------------------------------
+ // PRINT
+ //--------------------------------------------------------------------------
+ static void
+ PrintInform(int aInform,std::ostream &aOStream);
+
+ //--------------------------------------------------------------------------
+ // CACHING
+ //--------------------------------------------------------------------------
+ mutable SimTK::Vector _cachedConstraintJacobianParameters;
+ mutable SimTK::Matrix _cachedConstraintJacobian;
+ mutable SimTK::Vector _cachedConstraintParameters;
+ mutable SimTK::Vector _cachedConstraint;
+
+ // Caching support for FSQP (since it likes to query constraints one at a time)
+ void clearCache();
+public:
+ int computeConstraint(const SimTK::Vector &x, const bool new_coefficients, double &c, int ic) const;
+ int computeConstraintGradient(const SimTK::Vector &x, const bool new_coefficients, SimTK::Vector &dcdx, int ic) const;
+
+//=============================================================================
+};
+
+} // namespace SimTK
+//=============================================================================
+//=============================================================================
+
+#endif // SimTK_CFSQP_OPTIMIZER_H_
diff --git a/SimTKmath/Optimizers/src/InteriorPointOptimizer.cpp b/SimTKmath/Optimizers/src/InteriorPointOptimizer.cpp
new file mode 100644
index 0000000..34285e3
--- /dev/null
+++ b/SimTKmath/Optimizers/src/InteriorPointOptimizer.cpp
@@ -0,0 +1,271 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 "InteriorPointOptimizer.h"
+
+using std::cout;
+using std::endl;
+
+
+namespace SimTK {
+
+static std::string applicationReturnStatusToString(int status) {
+ switch(status) {
+ case Solve_Succeeded: return "Solve succeeded";
+ case Solved_To_Acceptable_Level: return "Solved to acceptable level";
+ case Infeasible_Problem_Detected: return "Infeasible problem detected";
+ case Search_Direction_Becomes_Too_Small: return "Search direction becomes too small";
+ case Diverging_Iterates: return "Diverging iterates";
+ case User_Requested_Stop: return "User requested stop";
+ case Maximum_Iterations_Exceeded: return "Maximum iterations exceeded";
+ case Restoration_Failed: return "Restoration failed";
+ case Error_In_Step_Computation: return "Error in step computation";
+ case Not_Enough_Degrees_Of_Freedom: return "Not enough degrees of freedom";
+ case Invalid_Problem_Definition: return "Invalid problem definition";
+ case Invalid_Option: return "Invalid option";
+ case Invalid_Number_Detected: return "Invalid number detected";
+ case Unrecoverable_Exception: return "Unrecoverable exception";
+ case NonIpopt_Exception_Thrown: return "Non-Ipopt exception thrown";
+ case Insufficient_Memory: return "Insufficient memory";
+ case Internal_Error: return "Internal error";
+ default: return "Unknown Ipopt return status";
+ }
+}
+Optimizer::OptimizerRep* InteriorPointOptimizer::clone() const {
+ return( new InteriorPointOptimizer(*this) );
+}
+
+// Assume by the time this constructor is called, the number of parameters and constraints has been finalized
+InteriorPointOptimizer::InteriorPointOptimizer( const OptimizerSystem& sys )
+ : OptimizerRep( sys ) {
+
+ int n = sys.getNumParameters();
+ int m = sys.getNumConstraints();
+
+ if( n < 1 ) {
+ const char* where = " InteriorPointOptimizer Initialization";
+ const char* szName = "dimension";
+ SimTK_THROW5(SimTK::Exception::ValueOutOfRange, szName, 1, n, INT_MAX, where);
+ }
+
+ // Initialize arrays to store multipliers -- will be used for warm starts
+ mult_x_L = new Number[n];
+ mult_x_U = new Number[n];
+ mult_g = new Number[m];
+ for(int i=0;i<n;i++) mult_x_L[i] = mult_x_U[i] = 0;
+ for(int i=0;i<m;i++) mult_g[i] = 0;
+
+ g_L = new Number[m];
+ g_U = new Number[m];
+ /* set the bounds on the equality constraint functions */
+ for(int i=0;i<sys.getNumEqualityConstraints();i++){
+ g_U[i] = g_L[i] = 0.0;
+ }
+ /* set the bounds on the inequality constraint functions */
+ for(int i=sys.getNumEqualityConstraints();i<m;i++){
+ g_U[i] = SimTK::Real(POSITIVE_INF);
+ g_L[i] = 0.0;
+ }
+
+ firstOptimization = true;
+ }
+
+
+ SimTK::Real InteriorPointOptimizer::optimize( Vector &results ) {
+
+ int n = getOptimizerSystem().getNumParameters();
+ int m = getOptimizerSystem().getNumConstraints();
+
+ Index index_style = 0; /* C-style; start counting of rows and column indices at 0 */
+ Index nele_hess = 0;
+ Index nele_jac = n*m; /* always assume dense */
+
+ // Parameter limits
+ Number *x_L = NULL, *x_U = NULL;
+ if( getOptimizerSystem().getHasLimits() ) {
+ getOptimizerSystem().getParameterLimits( &x_L, &x_U);
+ } else {
+ x_U = new Number[n];
+ x_L = new Number[n];
+ for(int i=0;i<n;i++) {
+ x_U[i] = SimTK::Real(POSITIVE_INF);
+ x_L[i] = SimTK::Real(NEGATIVE_INF);
+ }
+ }
+
+ SimTK::Real *x = &results[0];
+
+ IpoptProblem nlp = CreateIpoptProblem(n, x_L, x_U, m, g_L, g_U, nele_jac,
+ nele_hess, index_style, objectiveFuncWrapper, constraintFuncWrapper,
+ gradientFuncWrapper, constraintJacobianWrapper, hessianWrapper);
+
+ // If you want to verify which options are getting set in the optimizer, you can create a file ipopt.opt
+ // with "print_user_options yes", and set print_level to (at least 1). It will then print the options to the screen.
+
+ // sherm 100302: you have to set all of these tolerances to get IpOpt to change
+ // its convergence criteria; see OptimalityErrorConvergenceCheck::CheckConvergence().
+ // We'll set acceptable tolerances to the same value to disable them.
+ AddIpoptNumOption(nlp, "tol", convergenceTolerance);
+ AddIpoptNumOption(nlp, "dual_inf_tol", convergenceTolerance);
+ AddIpoptNumOption(nlp, "constr_viol_tol", constraintTolerance);
+ AddIpoptNumOption(nlp, "compl_inf_tol", convergenceTolerance);
+ AddIpoptNumOption(nlp, "acceptable_tol", convergenceTolerance);
+ AddIpoptNumOption(nlp, "acceptable_dual_inf_tol", convergenceTolerance);
+ AddIpoptNumOption(nlp, "acceptable_constr_viol_tol", constraintTolerance);
+ AddIpoptNumOption(nlp, "acceptable_compl_inf_tol", convergenceTolerance);
+
+
+ AddIpoptIntOption(nlp, "max_iter", maxIterations);
+ AddIpoptStrOption(nlp, "mu_strategy", "adaptive");
+ AddIpoptStrOption(nlp, "hessian_approximation", "limited-memory"); // needs to be limited-memory unless you have explicit hessians
+ AddIpoptIntOption(nlp, "limited_memory_max_history", limitedMemoryHistory);
+ AddIpoptIntOption(nlp, "print_level", diagnosticsLevel); // default is 4
+
+ int i;
+ static const char *advancedRealOptions[] = {
+ "compl_inf_tol",
+ "dual_inf_tol",
+ "constr_viol_tol",
+ "acceptable_tol",
+ "acceptable_compl_inf_tol",
+ "acceptable_constr_viol_tol",
+ "acceptable_dual_inf_tol",
+ "diverging_iterates_tol",
+ "barrier_tol_factor",
+ "obj_scaling_factor",
+ "nlp_scaling_max_gradient",
+ "bounds_relax_factor",
+ "recalc_y_feas_tol",
+ "mu_init",
+ "mu_max_fact",
+ "mu_max",
+ "mu_min",
+ "mu_linear_decrease_factor",
+ "mu_superlinear_decrease_factor",
+ "bound_frac",
+ "bound_push",
+ "bound_mult_init_val",
+ "constr_mult_init_max",
+ "constr_mult_init_val",
+ "warm_start_bound_push",
+ "warm_start_bound_frac",
+ "warm_start_mult_bound_push",
+ "warm_start_mult_init_max",
+ "recalc_y_feas_tol",
+ "expect_infeasible_problem_ctol",
+ "soft_resto_pderror_reduction_factor",
+ "required_infeasibility_reduction",
+ "bound_mult_reset_threshold",
+ "constr_mult_reset_threshold",
+ "max_hessian_perturbation",
+ "min_hessian_perturbation",
+ "first_hessian_perturbation",
+ "perturb_inc_fact_first",
+ "perturb_inc_fact",
+ "perturb_dec_fact",
+ "jacobian_reqularization_value",
+ "derivative_test_perturbation",
+ "derivative_test_tol",
+ 0};
+ Real value;
+ for(i=0;advancedRealOptions[i];i++) {
+ if(getAdvancedRealOption(advancedRealOptions[i],value))
+ AddIpoptNumOption(nlp, advancedRealOptions[i], value);
+ }
+
+ static const std::string advancedStrOptions[] = {"nlp_scaling_method",
+ "honor_original_bounds",
+ "check_derivatives_for_naninf",
+ "mu_strategy",
+ "mu_oracle",
+ "fixed_mu_oracle",
+ "alpha_for_y",
+ "recalc_y",
+ "expect_infeasible_problem",
+ "print_options_documentation",
+ "print_user_options",
+ "start_with_resto",
+ "evaluate_orig_obj_at_resto_trial",
+ "hessian_approximation",
+ "derivative_test",
+ ""};
+ std::string svalue;
+ for(i=0;!advancedStrOptions[i].empty();i++) {
+ if(getAdvancedStrOption(advancedStrOptions[i], svalue))
+ AddIpoptStrOption(nlp, advancedStrOptions[i].c_str(), svalue.c_str());
+ }
+
+ static const char* advancedIntOptions[] = {"quality_function_max_section_steps",
+ "max_soc",
+ "watchdog_shorted_iter_trigger",
+ "watchdog_trial_iter_max",
+ "max_refinement_steps",
+ "min_refinement_steps",
+ "limited_memory_max_history",
+ "limited_memory_max_skipping",
+ "derivative_test_print_all",
+ 0};
+ int ivalue;
+ for(i=0;advancedIntOptions[i];i++) {
+ if(getAdvancedIntOption(advancedIntOptions[i], ivalue))
+ AddIpoptIntOption(nlp, advancedIntOptions[i], ivalue);
+ }
+
+ // Only makes sense to do a warm start if this is not the first call to optimize() (since we need
+ // reasonable starting multiplier values)
+ bool use_warm_start=false;
+ if(getAdvancedBoolOption("warm_start", use_warm_start) && use_warm_start && !firstOptimization) {
+ AddIpoptStrOption(nlp, "warm_start_init_point", "yes");
+ AddIpoptStrOption(nlp, "warm_start_entire_iterate", "yes");
+ //AddIpoptStrOption(nlp, "warm_start_same_structure", "yes"); // couldn't get this one to work
+ }
+
+ SimTK::Real obj;
+
+ int status = IpoptSolve(nlp, x, NULL, &obj, mult_g, mult_x_L, mult_x_U, (void *)this );
+
+ FreeIpoptProblem(nlp);
+
+ // Only delete these if they aren't pointing to existing parameter limits
+ if( !getOptimizerSystem().getHasLimits() ) {
+ delete [] x_U;
+ delete [] x_L;
+ }
+
+ if(status == Solved_To_Acceptable_Level) {
+ std::cout << "Ipopt: Solved to acceptable level" << std::endl;
+ } else if (status != Solve_Succeeded) {
+ if( status != NonIpopt_Exception_Thrown) {
+ char buf[1024];
+ sprintf(buf, "Ipopt: %s (status %d)",applicationReturnStatusToString(status).c_str(),status);
+ SimTK_THROW1(SimTK::Exception::OptimizerFailed, SimTK::String(buf));
+ }
+ }
+
+ firstOptimization = false;
+
+ return(obj);
+ }
+
+
+} // namespace SimTK
diff --git a/SimTKmath/Optimizers/src/InteriorPointOptimizer.h b/SimTKmath/Optimizers/src/InteriorPointOptimizer.h
new file mode 100644
index 0000000..d291235
--- /dev/null
+++ b/SimTKmath/Optimizers/src/InteriorPointOptimizer.h
@@ -0,0 +1,69 @@
+#ifndef SimTK_SIMMATH_INTERIOR_POINT_OPTIMIZER_H_
+#define SimTK_SIMMATH_INTERIOR_POINT_OPTIMIZER_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "IpOpt/IpStdCInterface.h"
+#include "IpOpt/IpReturnCodes.h"
+
+#include "simmath/Optimizer.h"
+#include "simmath/internal/OptimizerRep.h"
+
+namespace SimTK {
+
+
+class InteriorPointOptimizer: public Optimizer::OptimizerRep {
+public:
+ ~InteriorPointOptimizer() {
+ delete [] g_U;
+ delete [] g_L;
+ delete [] mult_x_L;
+ delete [] mult_x_U;
+ delete [] mult_g;
+ }
+
+ InteriorPointOptimizer(const OptimizerSystem& sys);
+
+ Real optimize( Vector &results );
+ OptimizerRep* clone() const;
+
+ OptimizerAlgorithm getAlgorithm() const
+ { return InteriorPoint; }
+
+private:
+ Real *mult_x_L;
+ Real *mult_x_U;
+ Real *mult_g;
+ Real *g_L;
+ Real *g_U;
+ bool firstOptimization;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_INTERIOR_POINT_OPTIMIZER_H_
+
diff --git a/SimTKmath/Optimizers/src/IpOpt/CMakeLists.txt b/SimTKmath/Optimizers/src/IpOpt/CMakeLists.txt
new file mode 100644
index 0000000..0bc4e2f
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/CMakeLists.txt
@@ -0,0 +1,23 @@
+# add in local source and headers here
+FILE(GLOB src_files ./*.cpp ./*.c)
+FILE(GLOB incl_files ./*.hpp ./*.h)
+
+foreach( src ${src_files} )
+ list(APPEND SOURCE_GROUPS "Source Files\\IpOpt")
+ list(APPEND SOURCE_GROUP_FILES ${src})
+endforeach()
+
+foreach( incl ${incl_files} )
+ list(APPEND SOURCE_GROUPS "Header Files\\IpOpt")
+ list(APPEND SOURCE_GROUP_FILES ${incl})
+endforeach()
+
+set(SOURCE_GROUPS ${SOURCE_GROUPS} PARENT_SCOPE)
+set(SOURCE_GROUP_FILES ${SOURCE_GROUP_FILES} PARENT_SCOPE)
+
+# append to the local scope copy, and then copy up to parent scope
+list(APPEND SOURCE_FILES ${src_files})
+set(SOURCE_FILES ${SOURCE_FILES} PARENT_SCOPE)
+
+list(APPEND SOURCE_INCLUDE_FILES ${incl_files})
+set(SOURCE_INCLUDE_FILES ${SOURCE_INCLUDE_FILES} PARENT_SCOPE)
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpAdaptiveMuUpdate.cpp b/SimTKmath/Optimizers/src/IpOpt/IpAdaptiveMuUpdate.cpp
new file mode 100644
index 0000000..ec89de6
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpAdaptiveMuUpdate.cpp
@@ -0,0 +1,764 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpAdaptiveMuUpdate.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpAdaptiveMuUpdate.hpp"
+#include "IpJournalist.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ AdaptiveMuUpdate::AdaptiveMuUpdate
+ (const SmartPtr<LineSearch>& line_search,
+ const SmartPtr<MuOracle>& free_mu_oracle,
+ const SmartPtr<MuOracle>& fix_mu_oracle)
+ :
+ MuUpdate(),
+ linesearch_(line_search),
+ free_mu_oracle_(free_mu_oracle),
+ fix_mu_oracle_(fix_mu_oracle),
+ filter_(2)
+ {
+ DBG_ASSERT(IsValid(linesearch_));
+ DBG_ASSERT(IsValid(free_mu_oracle_));
+ // fix_mu_oracle may be NULL
+ }
+
+ AdaptiveMuUpdate::~AdaptiveMuUpdate()
+ {}
+
+ void AdaptiveMuUpdate::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->AddLowerBoundedNumberOption(
+ "mu_max_fact",
+ "Factor for initialization of maximum value for barrier parameter.",
+ 0.0, true, 1e3,
+ "This option determines the upper bound on the barrier parameter. This "
+ "upper bound is computed as the average complementarity at the initial "
+ "point times the value of this option. (Only used if option "
+ "\"mu_strategy\" is chosen as \"adaptive\".)");
+ roptions->AddLowerBoundedNumberOption(
+ "mu_max",
+ "Maximum value for barrier parameter.",
+ 0.0, true, 1e5,
+ "This option specifies an upper bound on the barrier parameter in the "
+ "adaptive mu selection mode. If this option is set, it overwrites the "
+ "effect of mu_max_fact. (Only used if option "
+ "\"mu_strategy\" is chosen as \"adaptive\".)");
+ roptions->AddLowerBoundedNumberOption(
+ "mu_min",
+ "Minimum value for barrier parameter.",
+ 0.0, true, 1e-9,
+ "This option specifies the lower bound on the barrier parameter in the "
+ "adaptive mu selection mode. By default, it is set to "
+ "min(\"tol\",\"compl_inf_tol\")/(\"barrier_tol_factor\"+1), which "
+ "should be a reasonable value. (Only used if option "
+ "\"mu_strategy\" is chosen as \"adaptive\".)");
+ std::string prev_cat = roptions->RegisteringCategory();
+ roptions->SetRegisteringCategory("Undocumented");
+ roptions->AddLowerBoundedNumberOption(
+ "adaptive_mu_safeguard_factor",
+ "",
+ 0.0, false, 0.0);
+ roptions->SetRegisteringCategory(prev_cat);
+
+ roptions->AddStringOption3(
+ "adaptive_mu_globalization",
+ "Globalization strategy for the adaptive mu selection mode.",
+ "obj-constr-filter",
+ "kkt-error", "nonmonotone decrease of kkt-error",
+ "obj-constr-filter", "2-dim filter for objective and constraint violation",
+ "never-monotone-mode", "disables globalization",
+ "To achieve global convergence of the adaptive version, the algorithm "
+ "has to switch to the monotone mode (Fiacco-McCormick approach) when "
+ "convergence does not seem to appear. This option sets the "
+ "criterion used to decide when to do this switch. (Only used if option "
+ "\"mu_strategy\" is chosen as \"adaptive\".)");
+
+ roptions->AddLowerBoundedIntegerOption(
+ "adaptive_mu_kkterror_red_iters",
+ "Maximum number of iterations requiring sufficient progress.",
+ 0, 4,
+ "For the \"kkt-error\" based globalization strategy, sufficient "
+ "progress must be made for \"adaptive_mu_kkterror_red_iters\" "
+ "iterations. If this number of iterations is exceeded, the "
+ "globalization strategy switches to the monotone mode.");
+
+ roptions->AddBoundedNumberOption(
+ "adaptive_mu_kkterror_red_fact",
+ "Sufficient decrease factor for \"kkt-error\" globalization strategy.",
+ 0.0, true, 1.0, true,
+ 0.9999,
+ "For the \"kkt-error\" based globalization strategy, the error "
+ "must decrease by this factor to be deemed sufficient decrease.");
+
+ roptions->AddBoundedNumberOption(
+ "filter_margin_fact",
+ "Factor determining width of margin for obj-constr-filter adaptive globalization strategy.",
+ 0.0, true, 1.0, true,
+ 1e-5,
+ "When using the adaptive globalization strategy, \"obj-constr-filter\", "
+ "sufficient progress for a filter entry is defined as "
+ "follows: (new obj) < (filter obj) - filter_margin_fact*(new "
+ "constr-voil) OR (new constr-viol) < (filter constr-viol) - "
+ "filter_margin_fact*(new constr-voil). For the description of "
+ "the \"kkt-error-filter\" option see \"filter_max_margin\".");
+ roptions->AddLowerBoundedNumberOption(
+ "filter_max_margin",
+ "Maximum width of margin in obj-constr-filter adaptive globalization strategy.",
+ 0.0, true,
+ 1.0,
+ // ToDo Detailed description later
+ "");
+ roptions->AddStringOption2(
+ "adaptive_mu_restore_previous_iterate",
+ "Indicates if the previous iterate should be restored if the monotone mode is entered.",
+ "no",
+ "no", "don't restore accepted iterate",
+ "yes", "restore accepted iterate",
+ "When the globalization strategy for the adaptive barrier algorithm "
+ "switches to the monotone mode, it can either start "
+ "from the most recent iterate (no), or from the last "
+ "iterate that was accepted (yes).");
+
+ roptions->AddLowerBoundedNumberOption(
+ "adaptive_mu_monotone_init_factor",
+ "Determines the initial value of the barrier parameter when switching to the monotone mode.",
+ 0.0, true, 0.8,
+ "When the globalization strategy for the adaptive barrier algorithm "
+ "switches to the monotone mode and fixed_mu_oracle is chosen as "
+ "\"average_compl\", the barrier parameter is set to the "
+ "current average complementarity times the value of "
+ "\"adaptive_mu_monotone_init_factor\".");
+
+ roptions->AddStringOption4(
+ "adaptive_mu_kkt_norm_type",
+ "Norm used for the KKT error in the adaptive mu globalization strategies.",
+ "2-norm-squared",
+ "1-norm", "use the 1-norm (abs sum)",
+ "2-norm-squared", "use the 2-norm squared (sum of squares)",
+ "max-norm", "use the infinity norm (max)",
+ "2-norm", "use 2-norm",
+ "When computing the KKT error for the globalization strategies, the "
+ "norm to be used is specified with this option. Note, this options is also used "
+ "in the QualityFunctionMuOracle.");
+
+ }
+
+ bool AdaptiveMuUpdate::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ options.GetNumericValue("mu_max_fact", mu_max_fact_, prefix);
+ if (!options.GetNumericValue("mu_max", mu_max_, prefix)) {
+ // Set to a negative value as a hint that this value still has
+ // to be computed
+ mu_max_ = -1.;
+ }
+ options.GetNumericValue("tau_min", tau_min_, prefix);
+ options.GetNumericValue("adaptive_mu_safeguard_factor", adaptive_mu_safeguard_factor_, prefix);
+ options.GetNumericValue("adaptive_mu_kkterror_red_fact", refs_red_fact_, prefix);
+ options.GetIntegerValue("adaptive_mu_kkterror_red_iters", num_refs_max_, prefix);
+ Index enum_int;
+ options.GetEnumValue("adaptive_mu_globalization", enum_int, prefix);
+ adaptive_mu_globalization_ = AdaptiveMuGlobalizationEnum(enum_int);
+ options.GetNumericValue("filter_max_margin", filter_max_margin_, prefix);
+ options.GetNumericValue("filter_margin_fact", filter_margin_fact_, prefix);
+ options.GetBoolValue("adaptive_mu_restore_previous_iterate", restore_accepted_iterate_, prefix);
+
+ bool retvalue = free_mu_oracle_->Initialize(Jnlst(), IpNLP(), IpData(),
+ IpCq(), options, prefix);
+ if (!retvalue) {
+ return retvalue;
+ }
+
+ if (IsValid(fix_mu_oracle_)) {
+ retvalue = fix_mu_oracle_->Initialize(Jnlst(), IpNLP(), IpData(),
+ IpCq(), options, prefix);
+ if (!retvalue) {
+ return retvalue;
+ }
+ }
+
+ options.GetNumericValue("adaptive_mu_monotone_init_factor", adaptive_mu_monotone_init_factor_, prefix);
+ options.GetNumericValue("barrier_tol_factor", barrier_tol_factor_, prefix);
+ options.GetNumericValue("mu_linear_decrease_factor", mu_linear_decrease_factor_, prefix);
+ options.GetNumericValue("mu_superlinear_decrease_power", mu_superlinear_decrease_power_, prefix);
+
+ options.GetEnumValue("quality_function_norm_type", enum_int, prefix);
+ adaptive_mu_kkt_norm_ = QualityFunctionMuOracle::NormEnum(enum_int);
+ options.GetEnumValue("quality_function_centrality", enum_int, prefix);
+ adaptive_mu_kkt_centrality_ = QualityFunctionMuOracle::CentralityEnum(enum_int);
+ options.GetEnumValue("quality_function_balancing_term", enum_int, prefix);
+ adaptive_mu_kkt_balancing_term_ = QualityFunctionMuOracle::BalancingTermEnum(enum_int);
+ options.GetNumericValue("compl_inf_tol", compl_inf_tol_, prefix);
+ if (!options.GetNumericValue("mu_min", mu_min_, prefix)) {
+ // Compute mu_min based on tolerance (once the NLP scaling is known)
+ mu_min_default_ = true;
+ }
+ else {
+ mu_min_default_ = false;
+ }
+
+ init_dual_inf_ = -1.;
+ init_primal_inf_ = -1.;
+
+ refs_vals_.clear();
+ check_if_no_bounds_ = false;
+ no_bounds_ = false;
+ filter_.Clear();
+ IpData().SetFreeMuMode(true);
+
+ accepted_point_ = NULL;
+
+ // The following lines are only here so that
+ // IpoptCalculatedQuantities::CalculateSafeSlack and the first
+ // output line have something to work with
+ IpData().Set_mu(1.);
+ IpData().Set_tau(0.);
+
+ return retvalue;
+ }
+
+ bool AdaptiveMuUpdate::UpdateBarrierParameter()
+ {
+ DBG_START_METH("AdaptiveMuUpdate::UpdateBarrierParameter",
+ dbg_verbosity);
+
+ // if min_mu_ has not been given, we now set the default (can't do
+ // that earlier, because during call of InitializeImpl, the
+ // scaling in the NLP is not yet determined). We compute this
+ // here in every iteration, since the tolerance might be changed
+ // (e.g. in the restoration phase)
+ if (mu_min_default_) {
+ mu_min_ = Min(IpData().tol(),
+ IpNLP().NLP_scaling()->apply_obj_scaling(compl_inf_tol_))/
+ (barrier_tol_factor_+1);
+ }
+
+ // if mu_max has not yet been computed, do so now, based on the
+ // current average complementarity
+ if (mu_max_<0.) {
+ mu_max_ = mu_max_fact_*IpCq().curr_avrg_compl();
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "Setting mu_max to %e.\n", mu_max_);
+ }
+
+ // if there are not bounds, we always return the minimum MU value
+ if (!check_if_no_bounds_) {
+ Index n_bounds = IpData().curr()->z_L()->Dim() + IpData().curr()->z_U()->Dim()
+ + IpData().curr()->v_L()->Dim() + IpData().curr()->v_U()->Dim();
+
+ if (n_bounds==0) {
+ no_bounds_ = true;
+ IpData().Set_mu(mu_min_);
+ IpData().Set_tau(tau_min_);
+ }
+
+ check_if_no_bounds_ = true;
+ }
+
+ if (no_bounds_)
+ return true;
+
+ bool tiny_step_flag = IpData().tiny_step_flag();
+ if (!IpData().FreeMuMode()) {
+ // if we are in the fixed mu mode, we need to check if the
+ // current iterate is good enough to continue with the free mode
+ bool sufficient_progress = CheckSufficientProgress();
+ if (sufficient_progress && !tiny_step_flag) {
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "Switching back to free mu mode.\n");
+ IpData().SetFreeMuMode(true);
+ // Skipping Restoration phase?
+ RememberCurrentPointAsAccepted();
+ }
+ else {
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "Remaining in fixed mu mode.\n");
+
+ // ToDo decide whether we want this for all options
+ Number sub_problem_error = IpCq().curr_barrier_error();
+ Number mu = IpData().curr_mu();
+ if (sub_problem_error <= barrier_tol_factor_ * mu ||
+ tiny_step_flag) {
+ // If the current barrier problem has been solved sufficiently
+ // well, decrease mu
+ // ToDo combine this code with MonotoneMuUpdate
+ Number tol = IpData().tol();
+ Number compl_inf_tol =
+ IpNLP().NLP_scaling()->apply_obj_scaling(compl_inf_tol_);
+
+ Number new_mu = Min( mu_linear_decrease_factor_*mu,
+ pow(mu, mu_superlinear_decrease_power_) );
+ DBG_PRINT((1,"new_mu = %e, compl_inf_tol = %e tol = %e\n", new_mu, compl_inf_tol, tol));
+ new_mu = Max(new_mu,
+ Min(compl_inf_tol, tol)/(barrier_tol_factor_+1));
+ if (tiny_step_flag && new_mu == mu) {
+ THROW_EXCEPTION(TINY_STEP_DETECTED,
+ "Problem solved to best possible numerical accuracy");
+ }
+ Number new_tau = Compute_tau_monotone(mu);
+ IpData().Set_mu(new_mu);
+ IpData().Set_tau(new_tau);
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "Reducing mu to %24.16e in fixed mu mode. Tau becomes %24.16e\n", new_mu, new_tau);
+ linesearch_->Reset();
+ }
+ }
+ }
+ else {
+ // Here we are in the free mu mode.
+ bool sufficient_progress = CheckSufficientProgress();
+ if (linesearch_->CheckSkippedLineSearch() || tiny_step_flag ) {
+ sufficient_progress = false;
+ }
+ if (sufficient_progress) {
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "Staying in free mu mode.\n");
+ RememberCurrentPointAsAccepted();
+ }
+ else {
+ IpData().SetFreeMuMode(false);
+
+ if (restore_accepted_iterate_) {
+ // Restore most recent accepted iterate to start fixed mode from
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "Restoring most recent accepted point.\n");
+ SmartPtr<IteratesVector> prev_iter = accepted_point_->MakeNewContainer();
+ IpData().set_trial(prev_iter);
+ IpData().AcceptTrialPoint();
+ }
+
+ // Set the new values for mu and tau and tell the linesearch
+ // to reset its memory
+ Number mu = NewFixedMu();
+ Number tau = Compute_tau_monotone(mu);
+
+ if (tiny_step_flag && mu==IpData().curr_mu()) {
+ THROW_EXCEPTION(TINY_STEP_DETECTED,
+ "Problem solved to best possible numerical accuracy");
+ }
+
+ IpData().Set_mu(mu);
+ IpData().Set_tau(tau);
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "Switching to fixed mu mode with mu = %24.16e and tau = %24.16e.\n", mu, tau);
+ linesearch_->Reset();
+ // Skipping Restoration phase?
+ }
+ }
+
+ if (IpData().FreeMuMode()) {
+
+ // Choose the fraction-to-the-boundary parameter for the current
+ // iteration
+ // ToDo: Is curr_nlp_error really what we should use here?
+ Number tau = Max(tau_min_, 1-IpCq().curr_nlp_error());
+ IpData().Set_tau(tau);
+
+ // Compute the new barrier parameter via the oracle
+ Number mu;
+ bool retval = free_mu_oracle_->CalculateMu(mu_min_, mu_max_, mu);
+ if (!retval) {
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "The mu oracle could not compute a new value of the barrier parameter.\n");
+ return false;
+ }
+
+ mu = Max(mu, mu_min_);
+ Number mu_lower_safe = lower_mu_safeguard();
+ if (mu < mu_lower_safe) {
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "mu = %e smaller than safeguard = %e. Increasing mu.\n",
+ mu, mu_lower_safe);
+ mu = mu_lower_safe;
+ IpData().Append_info_string("m");
+ }
+
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "Barrier parameter mu computed by oracle is %e\n",
+ mu);
+
+ // Apply safeguards if appropriate
+ mu = Min(mu, mu_max_);
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "Barrier parameter mu after safeguards is %e\n",
+ mu);
+
+ // Set the new values
+ IpData().Set_mu(mu);
+
+ linesearch_->Reset();
+ // Uncomment the next line if the line search should not switch to
+ // the restoration phase in the free mode
+
+ // linesearch_->SetRigorousLineSearch(false);
+ }
+ else {
+ IpData().Append_info_string("F");
+ linesearch_->SetRigorousLineSearch(true);
+ }
+
+ return true;
+ }
+
+ bool
+ AdaptiveMuUpdate::CheckSufficientProgress()
+ {
+ bool retval = true;
+
+ switch (adaptive_mu_globalization_) {
+ case KKT_ERROR : {
+ Index num_refs = (Index)refs_vals_.size();
+ if (num_refs >= num_refs_max_) {
+ retval = false;
+ Number curr_error = quality_function_pd_system();
+ std::list<Number>::iterator iter;
+ for (iter = refs_vals_.begin(); iter != refs_vals_.end();
+ iter++) {
+ if ( curr_error <= refs_red_fact_*(*iter) ) {
+ retval = true;
+ }
+ }
+ }
+ }
+ break;
+ case FILTER_OBJ_CONSTR : {
+ /*
+ retval = filter_.Acceptable(IpCq().curr_f(),
+ IpCq().curr_constraint_violation());
+ */
+ // ToDo: Is curr_nlp_error really what we should use here?
+ Number curr_error = IpCq().curr_nlp_error();
+ Number margin = filter_margin_fact_*Min(filter_max_margin_, curr_error);
+ retval = filter_.Acceptable(IpCq().curr_f() + margin,
+ IpCq().curr_constraint_violation() + margin);
+ }
+ break;
+ case FILTER_KKT_ERROR : {
+ DBG_ASSERT(false && "Unknown adaptive_mu_globalization value.");
+ }
+ break;
+ case NEVER_MONOTONE_MODE :
+ retval = true;
+ break;
+ default:
+ DBG_ASSERT(false && "Unknown adaptive_mu_globalization value.");
+ }
+
+ return retval;
+ }
+
+ void
+ AdaptiveMuUpdate::RememberCurrentPointAsAccepted()
+ {
+ switch (adaptive_mu_globalization_) {
+ case KKT_ERROR : {
+ Number curr_error = quality_function_pd_system();
+ Index num_refs = (Index)refs_vals_.size();
+ if (num_refs >= num_refs_max_) {
+ refs_vals_.pop_front();
+ }
+ refs_vals_.push_back(curr_error);
+
+ if (Jnlst().ProduceOutput(J_MOREDETAILED, J_BARRIER_UPDATE)) {
+ Index num_refs = 0;
+ std::list<Number>::iterator iter;
+ for (iter = refs_vals_.begin(); iter != refs_vals_.end();
+ iter++) {
+ num_refs++;
+ Jnlst().Printf(J_MOREDETAILED, J_BARRIER_UPDATE,
+ "pd system reference[%2d] = %.6e\n", num_refs, *iter);
+ }
+ }
+ }
+ break;
+ case FILTER_OBJ_CONSTR : {
+ /*
+ Number theta = IpCq().curr_constraint_violation();
+ filter_.AddEntry(IpCq().curr_f() - filter_margin_fact_*theta,
+ IpCq().curr_constraint_violation() - filter_margin_fact_*theta,
+ IpData().iter_count());
+ filter_.Print(Jnlst());
+ */
+ filter_.AddEntry(IpCq().curr_f(),
+ IpCq().curr_constraint_violation(),
+ IpData().iter_count());
+ filter_.Print(Jnlst());
+ }
+ break;
+ case FILTER_KKT_ERROR : {
+ DBG_ASSERT(false && "Unknown corrector_type value.");
+ }
+ break;
+ default:
+ DBG_ASSERT(false && "Unknown corrector_type value.");
+ }
+
+ if (restore_accepted_iterate_) {
+ // Keep pointers to this iterate so that it could be restored
+ accepted_point_ = IpData().curr();
+ }
+ }
+
+ Number
+ AdaptiveMuUpdate::Compute_tau_monotone(Number mu)
+ {
+ return Max(tau_min_, 1-mu);
+ }
+
+ Number
+ AdaptiveMuUpdate::min_ref_val()
+ {
+ DBG_ASSERT(adaptive_mu_globalization_==KKT_ERROR);
+ Number min_ref;
+ DBG_ASSERT(refs_vals_.size()>0);
+ std::list<Number>::iterator iter = refs_vals_.begin();
+ min_ref = *iter;
+ iter++;
+ while (iter != refs_vals_.end()) {
+ min_ref = Min(min_ref, *iter);
+ iter++;
+ }
+ return min_ref;
+ }
+
+ Number
+ AdaptiveMuUpdate::max_ref_val()
+ {
+ DBG_ASSERT(adaptive_mu_globalization_==KKT_ERROR);
+ Number max_ref;
+ DBG_ASSERT(refs_vals_.size()>0);
+ std::list<Number>::iterator iter = refs_vals_.begin();
+ max_ref = *iter;
+ iter++;
+ while (iter != refs_vals_.end()) {
+ max_ref = Max(max_ref, *iter);
+ iter++;
+ }
+ return max_ref;
+ }
+
+ Number
+ AdaptiveMuUpdate::NewFixedMu()
+ {
+ Number max_ref;
+ // ToDo: Decide whether we should impose an upper bound on
+ // mu based on the smallest reference value. For now, don't
+ // impose one.
+ max_ref = 1e20;
+ /*
+ switch (adaptive_mu_globalization_) {
+ case 1 :
+ max_ref = max_ref_val();
+ break;
+ case 2 : {
+ max_ref = 1e20;
+ }
+ break;
+ default:
+ DBG_ASSERT("Unknown corrector_type value.");
+ }
+ */
+
+ Number new_mu;
+ bool have_mu = false;
+ ;
+ if (IsValid(fix_mu_oracle_)) {
+ have_mu = fix_mu_oracle_->CalculateMu(mu_min_, mu_max_, new_mu);
+ if (!have_mu) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "New fixed value for mu could not be computed from the mu_oracle.\n");
+ }
+ }
+ if (!have_mu) {
+ new_mu = adaptive_mu_monotone_init_factor_*IpCq().curr_avrg_compl();
+ }
+ new_mu = Max(new_mu, lower_mu_safeguard());
+ new_mu = Min(new_mu, Number(0.1) * max_ref);
+
+ new_mu = Max(new_mu, mu_min_);
+ new_mu = Min(new_mu, mu_max_);
+
+ return new_mu;
+ }
+
+ Number
+ AdaptiveMuUpdate::quality_function_pd_system()
+ {
+ Index n_dual = IpData().curr()->x()->Dim() + IpData().curr()->s()->Dim();
+ Index n_pri = IpData().curr()->y_c()->Dim() + IpData().curr()->y_d()->Dim();
+ Index n_comp = IpData().curr()->z_L()->Dim() + IpData().curr()->z_U()->Dim() +
+ IpData().curr()->v_L()->Dim() + IpData().curr()->v_U()->Dim();
+
+ Number dual_inf=0.;
+ Number primal_inf=0.;
+ Number complty=0.;
+ switch (adaptive_mu_kkt_norm_) {
+ case QualityFunctionMuOracle::NM_NORM_1:
+ dual_inf =
+ IpCq().curr_dual_infeasibility(NORM_1);
+ primal_inf =
+ IpCq().curr_primal_infeasibility(NORM_1);
+ complty =
+ IpCq().curr_complementarity(0., NORM_1);
+ dual_inf /= (Number)n_dual;
+ DBG_ASSERT(n_pri>0 || primal_inf==0.);
+ if (n_pri>0) {
+ primal_inf /= (Number)n_pri;
+ }
+ DBG_ASSERT(n_comp>0 || complty==0.);
+ if (n_comp>0) {
+ complty /= (Number)n_comp;
+ }
+ break;
+ case QualityFunctionMuOracle::NM_NORM_2_SQUARED:
+ dual_inf =
+ IpCq().curr_dual_infeasibility(NORM_2);
+ dual_inf *= dual_inf;
+ primal_inf =
+ IpCq().curr_primal_infeasibility(NORM_2);
+ primal_inf *= primal_inf;
+ complty =
+ IpCq().curr_complementarity(0., NORM_2);
+ complty *= complty;
+ dual_inf /= (Number)n_dual;
+ DBG_ASSERT(n_pri>0 || primal_inf==0.);
+ if (n_pri>0) {
+ primal_inf /= (Number)n_pri;
+ }
+ DBG_ASSERT(n_comp>0 || complty==0.);
+ if (n_comp>0) {
+ complty /= (Number)n_comp;
+ }
+ break;
+ case QualityFunctionMuOracle::NM_NORM_MAX:
+ dual_inf =
+ IpCq().curr_dual_infeasibility(NORM_MAX);
+ primal_inf =
+ IpCq().curr_primal_infeasibility(NORM_MAX);
+ complty =
+ IpCq().curr_complementarity(0., NORM_MAX);
+ break;
+ case QualityFunctionMuOracle::NM_NORM_2:
+ dual_inf =
+ IpCq().curr_dual_infeasibility(NORM_2);
+ primal_inf =
+ IpCq().curr_primal_infeasibility(NORM_2);
+ complty =
+ IpCq().curr_complementarity(0., NORM_2);
+ dual_inf /= sqrt((Number)n_dual);
+ DBG_ASSERT(n_pri>0 || primal_inf==0.);
+ if (n_pri>0) {
+ primal_inf /= sqrt((Number)n_pri);
+ }
+ DBG_ASSERT(n_comp>0 || complty==0.);
+ if (n_comp>0) {
+ complty /= sqrt((Number)n_comp);
+ }
+ break;
+ }
+
+ Number centrality = 0.;
+ if (adaptive_mu_kkt_centrality_!=0) {
+ Number xi = IpCq().curr_centrality_measure();
+ switch (adaptive_mu_kkt_centrality_) {
+ case 1:
+ centrality = -complty*log(xi);
+ break;
+ case 2:
+ centrality = complty/xi;
+ case 3:
+ centrality = complty/pow(xi,3);
+ break;
+ default:
+ DBG_ASSERT(false && "Unknown value for adaptive_mu_kkt_centrality_");
+ }
+ }
+
+ Number balancing_term=0.;
+ switch (adaptive_mu_kkt_balancing_term_) {
+ case 0:
+ //Nothing
+ break;
+ case 1:
+ balancing_term = pow(Max(0., Max(dual_inf,primal_inf)-complty),3);
+ break;
+ default:
+ DBG_ASSERT(false && "Unknown value for adaptive_mu_kkt_balancing_term");
+ }
+
+ DBG_ASSERT(centrality>=0.);
+ DBG_ASSERT(balancing_term>=0);
+ Number kkt_error = primal_inf + dual_inf + complty +
+ centrality + balancing_term;
+
+ Jnlst().Printf(J_MOREDETAILED, J_BARRIER_UPDATE,
+ "KKT error in barrier update check:\n"
+ " primal infeasibility: %15.6e\n"
+ " dual infeasibility: %15.6e\n"
+ " complementarity: %15.6e\n"
+ " centrality: %15.6e\n"
+ " kkt error: %15.6e\n",
+ primal_inf, dual_inf, complty, centrality, kkt_error);
+
+ return kkt_error;
+ }
+
+ Number
+ AdaptiveMuUpdate::lower_mu_safeguard()
+ {
+ DBG_START_METH("AdaptiveMuUpdate::lower_mu_safeguard",
+ dbg_verbosity);
+ if (adaptive_mu_safeguard_factor_ == 0.)
+ return 0.;
+
+ Number dual_inf =
+ IpCq().curr_dual_infeasibility(NORM_1);
+ Number primal_inf =
+ IpCq().curr_primal_infeasibility(NORM_1);
+ Index n_dual = IpData().curr()->x()->Dim() + IpData().curr()->s()->Dim();
+ dual_inf /= (Number)n_dual;
+ Index n_pri = IpData().curr()->y_c()->Dim() + IpData().curr()->y_d()->Dim();
+ DBG_ASSERT(n_pri>0 || primal_inf==0.);
+ if (n_pri>0) {
+ primal_inf /= (Number)n_pri;
+ }
+
+ if (init_dual_inf_ < 0.) {
+ init_dual_inf_ = Max(1., dual_inf);
+ }
+ if (init_primal_inf_ < 0.) {
+ init_primal_inf_ = Max(1., primal_inf);
+ }
+
+ Number lower_mu_safeguard =
+ Max(adaptive_mu_safeguard_factor_ * (dual_inf/init_dual_inf_),
+ adaptive_mu_safeguard_factor_ * (primal_inf/init_primal_inf_));
+ DBG_PRINT((1,"dual_inf=%e init_dual_inf_=%e primal_inf=%e init_primal_inf_=%e\n", dual_inf, init_dual_inf_, primal_inf, init_primal_inf_));
+
+ if (adaptive_mu_globalization_==KKT_ERROR) {
+ lower_mu_safeguard = Min(lower_mu_safeguard, min_ref_val());
+ }
+
+ return lower_mu_safeguard;
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpAdaptiveMuUpdate.hpp b/SimTKmath/Optimizers/src/IpOpt/IpAdaptiveMuUpdate.hpp
new file mode 100644
index 0000000..c1216f8
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpAdaptiveMuUpdate.hpp
@@ -0,0 +1,195 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpAdaptiveMuUpdate.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPADAPTIVEMUUPDATE_HPP__
+#define __IPADAPTIVEMUUPDATE_HPP__
+
+#include "IpMuUpdate.hpp"
+#include "IpLineSearch.hpp"
+#include "IpMuOracle.hpp"
+#include "IpFilter.hpp"
+#include "IpQualityFunctionMuOracle.hpp"
+
+namespace Ipopt
+{
+
+ /** Non-monotone mu update.
+ */
+ class AdaptiveMuUpdate : public MuUpdate
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor */
+ AdaptiveMuUpdate(const SmartPtr<LineSearch>& linesearch,
+ const SmartPtr<MuOracle>& free_mu_oracle,
+ const SmartPtr<MuOracle>& fix_mu_oracle=NULL);
+ /** Default destructor */
+ virtual ~AdaptiveMuUpdate();
+ //@}
+
+ /** Initialize method - overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Method for determining the barrier parameter for the next
+ * iteration. When the optimality error for the current barrier
+ * parameter is less than a tolerance, the barrier parameter is
+ * reduced, and the Reset method of the LineSearch object
+ * linesearch is called. */
+ virtual bool UpdateBarrierParameter();
+
+ /** Methods for IpoptType */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+
+ /** Default Constructor */
+ AdaptiveMuUpdate();
+
+ /** Copy Constructor */
+ AdaptiveMuUpdate(const AdaptiveMuUpdate&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const AdaptiveMuUpdate&);
+ //@}
+
+ /** @name Algorithmic parameters */
+ //@{
+ Number mu_max_fact_;
+ Number mu_max_;
+ Number mu_min_;
+ bool mu_min_default_;
+ Number tau_min_;
+ Number adaptive_mu_safeguard_factor_; //ToDo don't need that?
+ Number adaptive_mu_monotone_init_factor_;
+ Number barrier_tol_factor_;
+ Number mu_linear_decrease_factor_;
+ Number mu_superlinear_decrease_power_;
+ QualityFunctionMuOracle::NormEnum adaptive_mu_kkt_norm_;
+ QualityFunctionMuOracle::CentralityEnum adaptive_mu_kkt_centrality_;
+ QualityFunctionMuOracle::BalancingTermEnum adaptive_mu_kkt_balancing_term_;
+ /** enumeration for adaptive globalization */
+ enum AdaptiveMuGlobalizationEnum
+ {
+ KKT_ERROR=0,
+ FILTER_OBJ_CONSTR,
+ FILTER_KKT_ERROR,
+ NEVER_MONOTONE_MODE
+ };
+ /** Flag indicating which globalization strategy should be used. */
+ AdaptiveMuGlobalizationEnum adaptive_mu_globalization_;
+ /** Maximal margin in filter */
+ Number filter_max_margin_;
+ /** Factor for filter margin */
+ Number filter_margin_fact_;
+ /** Unscaled tolerance for complementarity */
+ Number compl_inf_tol_;
+ //@}
+
+ /** @name Strategy objects */
+ //@{
+ /** Line search object of the Ipopt algorithm. */
+ SmartPtr<LineSearch> linesearch_;
+ /** Pointer to strategy object that is to be used for computing a
+ * suggested value of the barrier parameter in the free mu mode.
+ */
+ SmartPtr<MuOracle> free_mu_oracle_;
+ /** Pointer to strategy object that is to be used for computing a
+ * suggested value for the fixed mu mode. If NULL, the current
+ * average complementarity is used.
+ */
+ SmartPtr<MuOracle> fix_mu_oracle_;
+ //@}
+
+ /** Dual infeasibility at initial point. A negative value means
+ * that this quantity has not yet been initialized. */
+ Number init_dual_inf_;
+ /** Primal infeasibility at initial point. A negative value means
+ * that this quantity has not yet been initialized. */
+ Number init_primal_inf_;
+
+ /** @name Methods and data defining the outer globalization
+ * strategy (might be a strategy object later). */
+ //@{
+ void InitializeFixedMuGlobalization();
+ /** Check whether the point in the "current" fields offers
+ * sufficient reduction in order to remain in or switch to the
+ * free mu mode. */
+ bool CheckSufficientProgress();
+ /** Include the current point in internal memory to as accepted
+ * point */
+ void RememberCurrentPointAsAccepted();
+ /** Compute the value of the fixed mu that should be used in a new
+ * fixed mu phase. This method is called at the beginning of a
+ * new fixed mu phase. */
+ Number NewFixedMu();
+ /** Compute value for the fraction-to-the-boundary parameter given
+ * mu in the monotone phase */
+ Number Compute_tau_monotone(Number mu);
+
+ /** Method for computing the norm of the primal dual system at the
+ * current point. For consistency, this is computed in the same
+ * way as the quality function is computed. This is the
+ * quantities used in the nonmonontone KKT reduction
+ * globalization. */
+ Number quality_function_pd_system();
+
+ /** Method for computing a lower safeguard bound for the barrier
+ * parameter. For now, this is related to primal and dual
+ * infeasibility. */
+ Number lower_mu_safeguard();
+
+ /** Computer the currently largest reference value. */
+ Number max_ref_val();
+
+ /** Computer the currently smallest reference value. */
+ Number min_ref_val();
+
+ /** Maximal number of reference values (algorithmic parameter) */
+ Index num_refs_max_;
+ /** Values of the currently stored reference values (norm of pd
+ * equations) */
+ std::list<Number> refs_vals_;
+ /** Factor requested to reduce the reference values */
+ Number refs_red_fact_;
+
+ /** Alternatively, we might also want to use a filter */
+ Filter filter_;
+ /** Flag indicating whether the most recent accepted step should
+ * be restored, when switching to the fixed mode. */
+ bool restore_accepted_iterate_;
+ //@}
+
+ /** Flag indicating whether the problem has any inequality constraints */
+ bool no_bounds_;
+ /** Flag indicating whether no_bounds_ has been initialized */
+ bool check_if_no_bounds_;
+
+ /** @name Most recent accepted point in free mode, from which
+ * fixed mode should be started.
+ */
+ //@{
+ SmartPtr<const IteratesVector> accepted_point_;
+ //@}
+
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpAlgBuilder.cpp b/SimTKmath/Optimizers/src/IpOpt/IpAlgBuilder.cpp
new file mode 100644
index 0000000..2a42e68
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpAlgBuilder.cpp
@@ -0,0 +1,475 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpAlgBuilder.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-09-29
+
+#include "IpoptConfig.h"
+#include "IpAlgBuilder.hpp"
+
+#include "IpOrigIpoptNLP.hpp"
+#include "IpIpoptData.hpp"
+#include "IpIpoptCalculatedQuantities.hpp"
+
+#include "IpStdAugSystemSolver.hpp"
+#include "IpAugRestoSystemSolver.hpp"
+#include "IpPDFullSpaceSolver.hpp"
+#include "IpPDPerturbationHandler.hpp"
+#include "IpOptErrorConvCheck.hpp"
+#include "IpBacktrackingLineSearch.hpp"
+#include "IpFilterLSAcceptor.hpp"
+#include "IpMonotoneMuUpdate.hpp"
+#include "IpAdaptiveMuUpdate.hpp"
+#include "IpLoqoMuOracle.hpp"
+#include "IpProbingMuOracle.hpp"
+#include "IpQualityFunctionMuOracle.hpp"
+#include "IpRestoMinC_1Nrm.hpp"
+#include "IpLeastSquareMults.hpp"
+#include "IpDefaultIterateInitializer.hpp"
+#include "IpWarmStartIterateInitializer.hpp"
+#include "IpOrigIterationOutput.hpp"
+#include "IpLimMemQuasiNewtonUpdater.hpp"
+#include "IpOrigIpoptNLP.hpp"
+#include "IpLowRankAugSystemSolver.hpp"
+#include "IpRestoIterationOutput.hpp"
+#include "IpRestoFilterConvCheck.hpp"
+#include "IpRestoIterateInitializer.hpp"
+#include "IpRestoRestoPhase.hpp"
+#include "IpTSymLinearSolver.hpp"
+#include "IpUserScaling.hpp"
+#include "IpGradientScaling.hpp"
+#include "IpExactHessianUpdater.hpp"
+
+# include "IpLapackSolverInterface.hpp"
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ void AlgorithmBuilder::BuildIpoptObjects(const Journalist& jnlst,
+ const OptionsList& options,
+ const std::string& prefix,
+ const SmartPtr<NLP>& nlp,
+ SmartPtr<IpoptNLP>& ip_nlp,
+ SmartPtr<IpoptData>& ip_data,
+ SmartPtr<IpoptCalculatedQuantities>& ip_cq)
+ {
+ DBG_ASSERT(prefix == "");
+
+ SmartPtr<NLPScalingObject> nlp_scaling ;
+ std::string nlp_scaling_method;
+ options.GetStringValue("nlp_scaling_method", nlp_scaling_method, "");
+ if (nlp_scaling_method == "user-scaling") {
+ nlp_scaling = new UserScaling(ConstPtr(nlp));
+ }
+ else if (nlp_scaling_method == "gradient-based") {
+ nlp_scaling = new GradientScaling(nlp);
+ }
+ else {
+ nlp_scaling = new NoNLPScalingObject();
+ }
+
+ ip_nlp = new OrigIpoptNLP(&jnlst, GetRawPtr(nlp), nlp_scaling);
+
+ // Create the IpoptData
+ ip_data = new IpoptData();
+
+ // Create the IpoptCalculators
+ ip_cq = new IpoptCalculatedQuantities(ip_nlp, ip_data);
+ }
+
+ void AlgorithmBuilder::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->SetRegisteringCategory("Linear Solver");
+ roptions->AddStringOption6(
+ "linear_solver",
+ "Linear solver used for step computations.",
+ "lapack",
+ "ma27", "use the Harwell routine MA27",
+ "ma57", "use the Harwell routine MA57",
+ "pardiso", "use the Pardiso package",
+ "taucs", "use TAUCS package (not yet working)",
+ "mumps", "use MUMPS package (not yet working)",
+ "lapack", "use LAPACK package",
+ "Determines which linear algebra package is to be used for the "
+ "solution of the augmented linear system (for obtaining the search "
+ "directions). "
+ "Note, the code must have been compiled with the linear solver you want "
+ "to choose. Depending on your Ipopt installation, not all options are "
+ "available.");
+ roptions->SetRegisteringCategory("Linear Solver");
+ roptions->AddStringOption2(
+ "linear_system_scaling",
+ "Method for scaling the linear system.",
+#ifdef HAVE_MC19
+ "mc19",
+#else
+ "none",
+#endif
+ "none", "no scaling will be performed",
+ "mc19", "use the Harwell routine MC19",
+ "Determines the method used to compute symmetric scaling "
+ "factors for the augmented system (see also the "
+ "\"linear_scaling_on_demand\" option). This scaling is independent"
+ "of the NLP problem scaling. By default, MC19 is only used if MA27 or "
+ "MA57 are selected as linear solvers. This option is only available if "
+ "Ipopt has been compiled with MC19.");
+
+ roptions->SetRegisteringCategory("NLP Scaling");
+ roptions->AddStringOption3(
+ "nlp_scaling_method",
+ "Select the technique used for scaling the NLP.",
+ "gradient-based",
+ "none", "no problem scaling will be performed",
+ "user-scaling", "scaling parameters will come from the user",
+ "gradient-based", "scale the problem so the maximum gradient at the starting point is scaling_max_gradient",
+ "Selects the technique used for scaling the problem internally before it is solved."
+ " For user-scaling, the parameters come from the NLP. If you are using "
+ "AMPL, they can be specified through suffixes (\"scaling_factor\")");
+
+ roptions->SetRegisteringCategory("Barrier Parameter Update");
+ roptions->AddStringOption2(
+ "mu_strategy",
+ "Update strategy for barrier parameter.",
+ "monotone",
+ "monotone", "use the monotone (Fiacco-McCormick) strategy",
+ "adaptive", "use the adaptive update strategy",
+ "Determines which barrier parameter update strategy is to be used.");
+ roptions->AddStringOption3(
+ "mu_oracle",
+ "Oracle for a new barrier parameter in the adaptive strategy.",
+ "quality-function",
+ "probing", "Mehrotra's probing heuristic",
+ "loqo", "LOQO's centrality rule",
+ "quality-function", "minimize a quality function",
+ "Determines how a new barrier parameter is computed in each "
+ "\"free-mode\" iteration of the adaptive barrier parameter "
+ "strategy. (Only considered if \"adaptive\" is selected for "
+ "option \"mu_strategy\").");
+ roptions->AddStringOption4(
+ "fixed_mu_oracle",
+ "Oracle for the barrier parameter when switching to fixed mode.",
+ "average_compl",
+ "probing", "Mehrotra's probing heuristic",
+ "loqo", "LOQO's centrality rule",
+ "quality-function", "minimize a quality function",
+ "average_compl", "base on current average complementarity",
+ "Determines how the first value of the barrier parameter should be "
+ "computed when switching to the \"monotone mode\" in the adaptive "
+ "strategy. (Only considered if \"adaptive\" is selected for option "
+ "\"mu_strategy\".)");
+ }
+
+ SmartPtr<IpoptAlgorithm>
+ AlgorithmBuilder::BuildBasicAlgorithm(const Journalist& jnlst,
+ const OptionsList& options,
+ const std::string& prefix)
+ {
+ DBG_START_FUN("AlgorithmBuilder::BuildBasicAlgorithm",
+ dbg_verbosity);
+ // Create the convergence check
+ SmartPtr<ConvergenceCheck> convCheck =
+ new OptimalityErrorConvergenceCheck();
+
+ // Create the solvers that will be used by the main algorithm
+
+ SmartPtr<SparseSymLinearSolverInterface> SolverInterface;
+ std::string linear_solver;
+ options.GetStringValue("linear_solver", linear_solver, prefix);
+ if (linear_solver=="ma27") {
+
+ THROW_EXCEPTION(OPTION_INVALID,
+ "Selected linear solver MA27 not available.");
+
+ }
+ else if (linear_solver=="ma57") {
+
+ THROW_EXCEPTION(OPTION_INVALID,
+ "Selected linear solver MA57 not available.");
+
+ }
+ else if (linear_solver=="pardiso") {
+ THROW_EXCEPTION(OPTION_INVALID,
+ "Selected linear solver Pardiso not available.");
+
+ }
+ else if (linear_solver=="taucs") {
+ THROW_EXCEPTION(OPTION_INVALID,
+ "Selected linear solver TAUCS not available.");
+
+ }
+ else if (linear_solver=="wsmp") {
+
+ THROW_EXCEPTION(OPTION_INVALID,
+ "Selected linear solver WSMP not available.");
+
+ }
+ else if (linear_solver=="mumps") {
+
+ THROW_EXCEPTION(OPTION_INVALID,
+ "Selected linear solver MUMPS not available.");
+
+ }
+ else if (linear_solver=="lapack") {
+ SolverInterface = new LapackSolverInterface();
+
+ }
+
+ SmartPtr<TSymScalingMethod> ScalingMethod;
+ std::string linear_system_scaling;
+ if (!options.GetStringValue("linear_system_scaling",
+ linear_system_scaling, prefix)) {
+ // By default, don't use mc19 for non-HSL solvers
+ if (linear_solver!="ma27" && linear_solver!="ma57") {
+ linear_system_scaling="none";
+ }
+ }
+ if (linear_system_scaling=="mc19") {
+
+ THROW_EXCEPTION(OPTION_INVALID,
+ "Selected linear system scaling method MC19 not available.");
+
+ }
+
+ SmartPtr<SymLinearSolver> ScaledSolver =
+ new TSymLinearSolver(SolverInterface, ScalingMethod);
+
+ SmartPtr<AugSystemSolver> AugSolver =
+ new StdAugSystemSolver(*ScaledSolver);
+ Index enum_int;
+ options.GetEnumValue("hessian_approximation", enum_int, prefix);
+ HessianApproximationType hessian_approximation =
+ HessianApproximationType(enum_int);
+ if (hessian_approximation==LIMITED_MEMORY) {
+ SmartPtr<AugSystemSolver> tmp =
+ new LowRankAugSystemSolver(*AugSolver);
+ AugSolver = tmp;
+ }
+
+ SmartPtr<PDPerturbationHandler> pertHandler =
+ new PDPerturbationHandler();
+ SmartPtr<PDSystemSolver> PDSolver =
+ new PDFullSpaceSolver(*AugSolver, *pertHandler);
+
+ // Create the object for initializing the iterates Initialization
+ // object. We include both the warm start and the defaut
+ // initializer, so that the warm start options can be activated
+ // without having to rebuild the algorithm
+ SmartPtr<EqMultiplierCalculator> EqMultCalculator =
+ new LeastSquareMultipliers(*AugSolver);
+ SmartPtr<IterateInitializer> WarmStartInitializer =
+ new WarmStartIterateInitializer();
+ SmartPtr<IterateInitializer> IterInitializer =
+ new DefaultIterateInitializer(EqMultCalculator, WarmStartInitializer);
+
+ // Solver for the restoration phase
+ SmartPtr<AugSystemSolver> resto_AugSolver =
+ new AugRestoSystemSolver(*AugSolver);
+ SmartPtr<PDPerturbationHandler> resto_pertHandler =
+ new PDPerturbationHandler();
+ SmartPtr<PDSystemSolver> resto_PDSolver =
+ new PDFullSpaceSolver(*resto_AugSolver, *resto_pertHandler);
+
+ // Convergence check in the restoration phase
+ SmartPtr<RestoFilterConvergenceCheck> resto_convCheck =
+ new RestoFilterConvergenceCheck();
+
+ // Line search method for the restoration phase
+ SmartPtr<RestoRestorationPhase> resto_resto =
+ new RestoRestorationPhase();
+ SmartPtr<FilterLSAcceptor> resto_filterLSacceptor =
+ new FilterLSAcceptor(GetRawPtr(resto_PDSolver));
+ SmartPtr<LineSearch> resto_LineSearch =
+ new BacktrackingLineSearch(GetRawPtr(resto_filterLSacceptor),
+ GetRawPtr(resto_resto), GetRawPtr(resto_convCheck));
+
+ // Create the mu update that will be used by the restoration phase
+ // algorithm
+ SmartPtr<MuUpdate> resto_MuUpdate;
+ std::string resto_smuupdate;
+ if (!options.GetStringValue("mu_strategy", resto_smuupdate, "resto."+prefix)) {
+ // Change default for quasi-Newton option (then we use adaptive)
+ Index enum_int;
+ if (options.GetEnumValue("hessian_approximation", enum_int, prefix)) {
+ HessianApproximationType hessian_approximation =
+ HessianApproximationType(enum_int);
+ if (hessian_approximation==LIMITED_MEMORY) {
+ resto_smuupdate = "adaptive";
+ }
+ }
+ }
+
+ std::string resto_smuoracle;
+ std::string resto_sfixmuoracle;
+ if (resto_smuupdate=="adaptive" ) {
+ options.GetStringValue("mu_oracle", resto_smuoracle, "resto."+prefix);
+ options.GetStringValue("fixed_mu_oracle", resto_sfixmuoracle, "resto."+prefix);
+ }
+
+ if (resto_smuupdate=="monotone" ) {
+ resto_MuUpdate = new MonotoneMuUpdate(GetRawPtr(resto_LineSearch));
+ }
+ else if (resto_smuupdate=="adaptive") {
+ SmartPtr<MuOracle> resto_MuOracle;
+ if (resto_smuoracle=="loqo") {
+ resto_MuOracle = new LoqoMuOracle();
+ }
+ else if (resto_smuoracle=="probing") {
+ resto_MuOracle = new ProbingMuOracle(resto_PDSolver);
+ }
+ else if (resto_smuoracle=="quality-function") {
+ resto_MuOracle = new QualityFunctionMuOracle(resto_PDSolver);
+ }
+ SmartPtr<MuOracle> resto_FixMuOracle;
+ if (resto_sfixmuoracle=="loqo") {
+ resto_FixMuOracle = new LoqoMuOracle();
+ }
+ else if (resto_sfixmuoracle=="probing") {
+ resto_FixMuOracle = new ProbingMuOracle(resto_PDSolver);
+ }
+ else if (resto_sfixmuoracle=="quality-function") {
+ resto_FixMuOracle = new QualityFunctionMuOracle(resto_PDSolver);
+ }
+ else {
+ resto_FixMuOracle = NULL;
+ }
+ resto_MuUpdate =
+ new AdaptiveMuUpdate(GetRawPtr(resto_LineSearch),
+ resto_MuOracle, resto_FixMuOracle);
+ }
+
+ // Initialization of the iterates for the restoration phase
+ SmartPtr<EqMultiplierCalculator> resto_EqMultCalculator =
+ new LeastSquareMultipliers(*resto_AugSolver);
+ SmartPtr<IterateInitializer> resto_IterInitializer =
+ new RestoIterateInitializer(resto_EqMultCalculator);
+
+ // Create the object for the iteration output during restoration
+ SmartPtr<OrigIterationOutput> resto_OrigIterOutput = NULL;
+ // new OrigIterationOutput();
+ SmartPtr<IterationOutput> resto_IterOutput =
+ new RestoIterationOutput(resto_OrigIterOutput);
+
+ // Get the Hessian updater for the restoration phase
+ SmartPtr<HessianUpdater> resto_HessUpdater;
+ switch(hessian_approximation) {
+ case EXACT:
+ resto_HessUpdater = new ExactHessianUpdater();
+ break;
+ case LIMITED_MEMORY:
+ // ToDo This needs to be replaced!
+ resto_HessUpdater = new LimMemQuasiNewtonUpdater(true);
+ break;
+ }
+
+ // Put together the overall restoration phase IP algorithm
+ SmartPtr<IpoptAlgorithm> resto_alg =
+ new IpoptAlgorithm(resto_PDSolver,
+ GetRawPtr(resto_LineSearch),
+ GetRawPtr(resto_MuUpdate),
+ GetRawPtr(resto_convCheck),
+ resto_IterInitializer,
+ resto_IterOutput,
+ resto_HessUpdater,
+ resto_EqMultCalculator);
+
+ // Set the restoration phase
+ SmartPtr<RestorationPhase> resto_phase =
+ new MinC_1NrmRestorationPhase(*resto_alg, EqMultCalculator);
+
+ // Create the line search to be used by the main algorithm
+ SmartPtr<FilterLSAcceptor> filterLSacceptor =
+ new FilterLSAcceptor(GetRawPtr(PDSolver));
+ SmartPtr<LineSearch> lineSearch =
+ new BacktrackingLineSearch(GetRawPtr(filterLSacceptor),
+ GetRawPtr(resto_phase), convCheck);
+
+ // The following cross reference is not good: We have to store a
+ // pointer to the lineSearch object in resto_convCheck as a
+ // non-SmartPtr to make sure that things are properly deleted when
+ // the IpoptAlgorithm return by the Builder is destructed.
+ resto_convCheck->SetOrigFilterLSAcceptor(*filterLSacceptor);
+
+ // Create the mu update that will be used by the main algorithm
+ SmartPtr<MuUpdate> MuUpdate;
+ std::string smuupdate;
+ if (!options.GetStringValue("mu_strategy", smuupdate, prefix)) {
+ // Change default for quasi-Newton option (then we use adaptive)
+ Index enum_int;
+ if (options.GetEnumValue("hessian_approximation", enum_int, prefix)) {
+ HessianApproximationType hessian_approximation =
+ HessianApproximationType(enum_int);
+ if (hessian_approximation==LIMITED_MEMORY) {
+ smuupdate = "adaptive";
+ }
+ }
+ }
+ std::string smuoracle;
+ std::string sfixmuoracle;
+ if (smuupdate=="adaptive" ) {
+ options.GetStringValue("mu_oracle", smuoracle, prefix);
+ options.GetStringValue("fixed_mu_oracle", sfixmuoracle, prefix);
+ }
+
+ if (smuupdate=="monotone" ) {
+ MuUpdate = new MonotoneMuUpdate(GetRawPtr(lineSearch));
+ }
+ else if (smuupdate=="adaptive") {
+ SmartPtr<MuOracle> muOracle;
+ if (smuoracle=="loqo") {
+ muOracle = new LoqoMuOracle();
+ }
+ else if (smuoracle=="probing") {
+ muOracle = new ProbingMuOracle(PDSolver);
+ }
+ else if (smuoracle=="quality-function") {
+ muOracle = new QualityFunctionMuOracle(PDSolver);
+ }
+ SmartPtr<MuOracle> FixMuOracle;
+ if (sfixmuoracle=="loqo") {
+ FixMuOracle = new LoqoMuOracle();
+ }
+ else if (sfixmuoracle=="probing") {
+ FixMuOracle = new ProbingMuOracle(PDSolver);
+ }
+ else if (sfixmuoracle=="quality-function") {
+ FixMuOracle = new QualityFunctionMuOracle(PDSolver);
+ }
+ else {
+ FixMuOracle = NULL;
+ }
+ MuUpdate = new AdaptiveMuUpdate(GetRawPtr(lineSearch),
+ muOracle, FixMuOracle);
+ }
+
+ // Create the object for the iteration output
+ SmartPtr<IterationOutput> IterOutput =
+ new OrigIterationOutput();
+
+ // Get the Hessian updater for the main algorithm
+ SmartPtr<HessianUpdater> HessUpdater;
+ switch(hessian_approximation) {
+ case EXACT:
+ HessUpdater = new ExactHessianUpdater();
+ break;
+ case LIMITED_MEMORY:
+ // ToDo This needs to be replaced!
+ HessUpdater = new LimMemQuasiNewtonUpdater(false);
+ break;
+ }
+
+ // Create the main algorithm
+ SmartPtr<IpoptAlgorithm> alg =
+ new IpoptAlgorithm(PDSolver,
+ GetRawPtr(lineSearch), MuUpdate,
+ convCheck, IterInitializer, IterOutput,
+ HessUpdater, EqMultCalculator);
+
+ return alg;
+ }
+
+} // namespace
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpAlgBuilder.hpp b/SimTKmath/Optimizers/src/IpOpt/IpAlgBuilder.hpp
new file mode 100644
index 0000000..cc380e4
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpAlgBuilder.hpp
@@ -0,0 +1,84 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpAlgBuilder.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-09-29
+
+#ifndef __IPALGBUILDER_HPP__
+#define __IPALGBUILDER_HPP__
+
+#include "IpIpoptAlg.hpp"
+#include "IpReferenced.hpp"
+
+namespace Ipopt
+{
+
+ /** Builder to create a complete IpoptAlg object. This object
+ * contains all subelements (such as line search objects etc). How
+ * the resulting IpoptAlg object is built can be influenced by the
+ * options.
+ * TODO: Currently, this is a basic implementation with everything
+ * in one method that can be overloaded. This will need to be expanded
+ * to allow customization of different parts without recoding everything.
+ */
+ class AlgorithmBuilder : public ReferencedObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor */
+ AlgorithmBuilder()
+ {}
+
+ /** Destructor */
+ virtual ~AlgorithmBuilder()
+ {}
+
+ //@}
+
+ /** @name Methods to build parts of the algorithm */
+ //@{
+ virtual void BuildIpoptObjects(const Journalist& jnlst,
+ const OptionsList& options,
+ const std::string& prefix,
+ const SmartPtr<NLP>& nlp,
+ SmartPtr<IpoptNLP>& ip_nlp,
+ SmartPtr<IpoptData>& ip_data,
+ SmartPtr<IpoptCalculatedQuantities>& ip_cq);
+
+ virtual SmartPtr<IpoptAlgorithm> BuildBasicAlgorithm(const Journalist& jnlst,
+ const OptionsList& options,
+ const std::string& prefix);
+ //@}
+
+ /** Methods for IpoptTypeInfo */
+ //@{
+ /** register the options used by the algorithm builder */
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ // AlgorithmBuilder();
+
+ /** Copy Constructor */
+ AlgorithmBuilder(const AlgorithmBuilder&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const AlgorithmBuilder&);
+ //@}
+
+ };
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpAlgStrategy.hpp b/SimTKmath/Optimizers/src/IpOpt/IpAlgStrategy.hpp
new file mode 100644
index 0000000..1ae0eb8
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpAlgStrategy.hpp
@@ -0,0 +1,153 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpAlgStrategy.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPALGSTRATEGY_HPP__
+#define __IPALGSTRATEGY_HPP__
+
+#include "IpOptionsList.hpp"
+#include "IpJournalist.hpp"
+#include "IpIpoptCalculatedQuantities.hpp"
+
+namespace Ipopt
+{
+
+ /** This is the base class for all algorithm strategy objects. The
+ * AlgorithmStrategyObject base class implements a common interface
+ * for all algorithm strategy objects. A strategy object is a
+ * component of the algorithm for which different alternatives or
+ * implementations exists. It allows to compose the algorithm
+ * before execution for a particular configuration, without the
+ * need to call alternatives based on enums. For example, the
+ * LineSearch object is a strategy object, since different line
+ * search options might be used for different runs.
+ *
+ * This interface is used for
+ * things that are done to all strategy objects, like
+ * initialization and setting options.
+ */
+ class AlgorithmStrategyObject : public ReferencedObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor */
+ AlgorithmStrategyObject()
+ :
+ initialize_called_(false)
+ {}
+
+ /** Default Destructor */
+ virtual ~AlgorithmStrategyObject()
+ {}
+ //@}
+
+ /** This method is called every time the algorithm starts again -
+ * it is used to reset any internal state. The pointers to the
+ * Journalist, as well as to the IpoptNLP, IpoptData, and
+ * IpoptCalculatedQuantities objects should be stored in the
+ * instanciation of this base class. This method is also used to
+ * get all required user options from the OptionsList. Here, if
+ * prefix is given, each tag (identifying the options) is first
+ * looked for with the prefix in front, and if not found, without
+ * the prefix. Note: you should not cue off of the iteration
+ * count to indicate the "start" of an algorithm!
+ *
+ * Do not overload this method, since it does some general
+ * initialization that is common for all strategy objects.
+ * Overload the protected InitializeImpl method instead.
+ */
+ bool Initialize(const Journalist& jnlst,
+ IpoptNLP& ip_nlp,
+ IpoptData& ip_data,
+ IpoptCalculatedQuantities& ip_cq,
+ const OptionsList& options,
+ const std::string& prefix)
+ {
+ initialize_called_ = true;
+ // Copy the pointers for the problem defining objects
+ jnlst_ = &jnlst;
+ ip_nlp_ = &ip_nlp;
+ ip_data_ = &ip_data;
+ ip_cq_ = &ip_cq;
+
+ bool retval = InitializeImpl(options, prefix);
+ if (!retval) {
+ initialize_called_ = false;
+ }
+
+ return retval;
+ }
+
+ protected:
+ /** Implementation of the initialization method that has to be
+ * overloaded by for each derived class. */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix)=0;
+
+ /** @name Accessor methods for the problem defining objects.
+ * Those should be used by the derived classes. */
+ //@{
+ const Journalist& Jnlst() const
+ {
+ DBG_ASSERT(initialize_called_);
+ return *jnlst_;
+ }
+ IpoptNLP& IpNLP() const
+ {
+ DBG_ASSERT(initialize_called_);
+ return *ip_nlp_;
+ }
+ IpoptData& IpData() const
+ {
+ DBG_ASSERT(initialize_called_);
+ return *ip_data_;
+ }
+ IpoptCalculatedQuantities& IpCq() const
+ {
+ DBG_ASSERT(initialize_called_);
+ return *ip_cq_;
+ }
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ //AlgorithmStrategyObject();
+
+
+ /** Copy Constructor */
+ AlgorithmStrategyObject(const AlgorithmStrategyObject&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const AlgorithmStrategyObject&);
+ //@}
+
+ /** @name Pointers to objects defining a particular optimization
+ * problem */
+ //@{
+ SmartPtr<const Journalist> jnlst_;
+ SmartPtr<IpoptNLP> ip_nlp_;
+ SmartPtr<IpoptData> ip_data_;
+ SmartPtr<IpoptCalculatedQuantities> ip_cq_;
+ //@}
+
+ /** flag indicating if Initialize method has been called (for
+ * debugging) */
+ bool initialize_called_;
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpAlgTypes.hpp b/SimTKmath/Optimizers/src/IpOpt/IpAlgTypes.hpp
new file mode 100644
index 0000000..480dd0e
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpAlgTypes.hpp
@@ -0,0 +1,58 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpAlgTypes.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-07-19
+
+#ifndef __IPALGTYPES_HPP__
+#define __IPALGTYPES_HPP__
+
+#include "IpTypes.hpp"
+#include "IpException.hpp"
+
+namespace Ipopt
+{
+
+ /**@name Enumerations */
+ //@{
+ /** enum for the return from the optimize algorithm
+ * (obviously we need to add more) */
+ enum SolverReturn {
+ SUCCESS,
+ MAXITER_EXCEEDED,
+ STOP_AT_TINY_STEP,
+ STOP_AT_ACCEPTABLE_POINT,
+ LOCAL_INFEASIBILITY,
+ USER_REQUESTED_STOP,
+ DIVERGING_ITERATES,
+ RESTORATION_FAILURE,
+ ERROR_IN_STEP_COMPUTATION,
+ INVALID_NUMBER_DETECTED,
+ TOO_FEW_DEGREES_OF_FREEDOM,
+ INTERNAL_ERROR
+ };
+ //@}
+
+ /** @name Some exceptions used in multiple places */
+ //@{
+ DECLARE_STD_EXCEPTION(LOCALLY_INFEASIBLE);
+ DECLARE_STD_EXCEPTION(TOO_FEW_DOF);
+ DECLARE_STD_EXCEPTION(TINY_STEP_DETECTED);
+ DECLARE_STD_EXCEPTION(ACCEPTABLE_POINT_REACHED);
+ DECLARE_STD_EXCEPTION(FEASIBILITY_PROBLEM_SOLVED);
+ DECLARE_STD_EXCEPTION(INVALID_WARMSTART);
+ DECLARE_STD_EXCEPTION(INTERNAL_ABORT);
+ /** Exception FAILED_INITIALIZATION for problem during
+ * initialization of a strategy object (or other problems). This
+ * is thrown by a strategy object, if a problem arises during
+ * initialization, such as a value out of a feasible range.
+ */
+ DECLARE_STD_EXCEPTION(FAILED_INITIALIZATION);
+ //@}
+
+
+}
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpAlgorithmRegOp.cpp b/SimTKmath/Optimizers/src/IpOpt/IpAlgorithmRegOp.cpp
new file mode 100644
index 0000000..8551f53
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpAlgorithmRegOp.cpp
@@ -0,0 +1,90 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpAlgorithmRegOp.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-08-16
+
+#include "IpAlgorithmRegOp.hpp"
+#include "IpRegOptions.hpp"
+
+#include "IpAdaptiveMuUpdate.hpp"
+#include "IpAlgBuilder.hpp"
+#include "IpDefaultIterateInitializer.hpp"
+#include "IpBacktrackingLineSearch.hpp"
+#include "IpFilterLSAcceptor.hpp"
+#include "IpGradientScaling.hpp"
+#include "IpIpoptAlg.hpp"
+#include "IpIpoptCalculatedQuantities.hpp"
+#include "IpIpoptData.hpp"
+#include "IpMonotoneMuUpdate.hpp"
+#include "IpNLPScaling.hpp"
+#include "IpOptErrorConvCheck.hpp"
+#include "IpOrigIpoptNLP.hpp"
+#include "IpOrigIterationOutput.hpp"
+#include "IpLimMemQuasiNewtonUpdater.hpp"
+#include "IpPDFullSpaceSolver.hpp"
+#include "IpPDPerturbationHandler.hpp"
+#include "IpProbingMuOracle.hpp"
+#include "IpQualityFunctionMuOracle.hpp"
+#include "IpRestoFilterConvCheck.hpp"
+#include "IpRestoIpoptNLP.hpp"
+#include "IpRestoMinC_1Nrm.hpp"
+#include "IpWarmStartIterateInitializer.hpp"
+
+
+namespace Ipopt
+{
+
+ void RegisterOptions_Algorithm(const SmartPtr<RegisteredOptions>& roptions)
+ {
+ roptions->SetRegisteringCategory("Barrier Parameter Update");
+ AdaptiveMuUpdate::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Initialization");
+ DefaultIterateInitializer::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Main Algorithm");
+ AlgorithmBuilder::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Line Search");
+ BacktrackingLineSearch::RegisterOptions(roptions);
+ FilterLSAcceptor::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("NLP Scaling");
+ StandardScalingBase::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("NLP Scaling");
+ GradientScaling::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Uncategorized");
+ IpoptAlgorithm::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Uncategorized");
+ IpoptData::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Uncategorized");
+ IpoptCalculatedQuantities::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Hessian Approximation");
+ LimMemQuasiNewtonUpdater::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Barrier Parameter Update");
+ MonotoneMuUpdate::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Convergence");
+ OptimalityErrorConvergenceCheck::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("NLP");
+ OrigIpoptNLP::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Output");
+ OrigIterationOutput::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Step Calculation");
+ PDFullSpaceSolver::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Step Calculation");
+ PDPerturbationHandler::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Barrier Parameter Update");
+ ProbingMuOracle::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Barrier Parameter Update");
+ QualityFunctionMuOracle::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Restoration Phase");
+ RestoFilterConvergenceCheck::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Restoration Phase");
+ RestoIpoptNLP::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Uncategorized");
+ roptions->SetRegisteringCategory("Restoration Phase");
+ MinC_1NrmRestorationPhase::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Warm Start");
+ WarmStartIterateInitializer::RegisterOptions(roptions);
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpAlgorithmRegOp.hpp b/SimTKmath/Optimizers/src/IpOpt/IpAlgorithmRegOp.hpp
new file mode 100644
index 0000000..3a95422
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpAlgorithmRegOp.hpp
@@ -0,0 +1,22 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpAlgorithmRegOp.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-08-16
+
+#ifndef __IPALGORITHMREGOP_HPP__
+#define __IPALGORITHMREGOP_HPP__
+
+#include "IpSmartPtr.hpp"
+
+namespace Ipopt
+{
+ class RegisteredOptions;
+
+ void RegisterOptions_Algorithm(const SmartPtr<RegisteredOptions>& roptions);
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpAugRestoSystemSolver.cpp b/SimTKmath/Optimizers/src/IpOpt/IpAugRestoSystemSolver.cpp
new file mode 100644
index 0000000..db6f9ed
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpAugRestoSystemSolver.cpp
@@ -0,0 +1,618 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpAugRestoSystemSolver.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpAugRestoSystemSolver.hpp"
+#include "IpCompoundMatrix.hpp"
+#include "IpCompoundSymMatrix.hpp"
+#include "IpCompoundVector.hpp"
+#include "IpSumSymMatrix.hpp"
+#include "IpDiagMatrix.hpp"
+#include "IpLowRankUpdateSymMatrix.hpp"
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ AugRestoSystemSolver::AugRestoSystemSolver(AugSystemSolver& orig_aug_solver,
+ bool skip_orig_aug_solver_init)
+ :
+ AugSystemSolver(),
+ neg_omega_c_plus_D_c_cache_(1),
+ neg_omega_d_plus_D_d_cache_(1),
+ sigma_tilde_n_c_inv_cache_(1),
+ sigma_tilde_p_c_inv_cache_(1),
+ sigma_tilde_n_d_inv_cache_(1),
+ sigma_tilde_p_d_inv_cache_(1),
+ d_x_plus_wr_d_cache_(1),
+ rhs_cR_cache_(1),
+ rhs_dR_cache_(1),
+ orig_aug_solver_(&orig_aug_solver),
+ skip_orig_aug_solver_init_(skip_orig_aug_solver_init)
+ {
+ DBG_START_METH("AugRestoSystemSolver::AugRestoSystemSolver()",dbg_verbosity);
+ }
+
+
+ AugRestoSystemSolver::~AugRestoSystemSolver()
+ {
+ DBG_START_METH("AugRestoSystemSolver::~AugRestoSystemSolver()",dbg_verbosity);
+ }
+
+
+ bool AugRestoSystemSolver::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ bool retval = true;
+ if (!skip_orig_aug_solver_init_) {
+ retval = orig_aug_solver_->Initialize(Jnlst(), IpNLP(), IpData(), IpCq(),
+ options, prefix);
+ }
+
+ return retval;
+ }
+
+ ESymSolverStatus AugRestoSystemSolver::Solve(const SymMatrix* W,
+ Number W_factor,
+ const Vector* D_x,
+ Number delta_x,
+ const Vector* D_s,
+ Number delta_s,
+ const Matrix* J_c,
+ const Vector* D_c,
+ Number delta_c,
+ const Matrix* J_d,
+ const Vector* D_d,
+ Number delta_d,
+ const Vector& rhs_x,
+ const Vector& rhs_s,
+ const Vector& rhs_c,
+ const Vector& rhs_d,
+ Vector& sol_x,
+ Vector& sol_s,
+ Vector& sol_c,
+ Vector& sol_d,
+ bool check_NegEVals,
+ Index numberOfNegEVals)
+ {
+ DBG_START_METH("AugRestoSystemSolver::Solve",dbg_verbosity);
+ DBG_ASSERT(J_c && J_d); // should pass these by ref
+
+ // I think the comment below is incorrect
+ // Remember, W and the D's may be NULL!
+ // ToDo: I don't think the W's can ever be NULL (we always need the structure)
+ DBG_ASSERT(W);
+
+ SmartPtr<const CompoundSymMatrix> CW =
+ dynamic_cast<const CompoundSymMatrix*>(W);
+
+ SmartPtr<const CompoundVector> CD_x =
+ dynamic_cast<const CompoundVector*>(D_x);
+
+ SmartPtr<const CompoundMatrix> CJ_c =
+ dynamic_cast<const CompoundMatrix*>(J_c);
+ DBG_ASSERT(IsValid(CJ_c));
+
+ SmartPtr<const CompoundMatrix> CJ_d =
+ dynamic_cast<const CompoundMatrix*>(J_d);
+ DBG_ASSERT(IsValid(CJ_d));
+
+ SmartPtr<const CompoundVector> Crhs_x =
+ dynamic_cast<const CompoundVector*>(&rhs_x);
+ DBG_ASSERT(IsValid(Crhs_x));
+
+ SmartPtr<CompoundVector> Csol_x = dynamic_cast<CompoundVector*>(&sol_x);
+ DBG_ASSERT(IsValid(Csol_x));
+
+ // Get the Sigma inverses
+ SmartPtr<const Vector> sigma_n_c;
+ SmartPtr<const Vector> sigma_p_c;
+ SmartPtr<const Vector> sigma_n_d;
+ SmartPtr<const Vector> sigma_p_d;
+
+ if (IsValid(CD_x)) {
+ sigma_n_c = CD_x->GetComp(1);
+ sigma_p_c = CD_x->GetComp(2);
+ sigma_n_d = CD_x->GetComp(3);
+ sigma_p_d = CD_x->GetComp(4);
+ }
+
+ SmartPtr<const Vector> sigma_tilde_n_c_inv =
+ Sigma_tilde_n_c_inv(sigma_n_c, delta_x, *Crhs_x->GetComp(1));
+ SmartPtr<const Vector> sigma_tilde_p_c_inv =
+ Sigma_tilde_p_c_inv(sigma_p_c, delta_x, *Crhs_x->GetComp(2));
+ SmartPtr<const Vector> sigma_tilde_n_d_inv =
+ Sigma_tilde_n_d_inv(sigma_n_d, delta_x, *Crhs_x->GetComp(3));
+ SmartPtr<const Vector> sigma_tilde_p_d_inv =
+ Sigma_tilde_p_d_inv(sigma_p_d, delta_x, *Crhs_x->GetComp(4));
+
+ // Pull out the expansion matrices for d
+ SmartPtr<const Matrix> pd_l = CJ_d->GetComp(0,3);
+ SmartPtr<const Matrix> neg_pd_u = CJ_d->GetComp(0,4);
+
+ // Now map the correct entries into the Solve method
+ // pull out the parts of the hessian h_orig + diag
+ DBG_PRINT_MATRIX(2, "CW", *CW);
+ SmartPtr<const SymMatrix> h_orig;
+ SmartPtr<const Vector> D_xR;
+ SmartPtr<const SumSymMatrix> WR_sum =
+ dynamic_cast<const SumSymMatrix*>(GetRawPtr(CW->GetComp(0,0)));
+ Number orig_W_factor = W_factor;
+ if (IsValid(WR_sum)) {
+ // We seem to be in the regular situation with exact second
+ // derivatives
+ Number temp_factor;
+ WR_sum->GetTerm(0, temp_factor, h_orig);
+ DBG_ASSERT(temp_factor == 1. || temp_factor == 0.);
+ orig_W_factor = temp_factor * W_factor;
+ SmartPtr<const SymMatrix> eta_DR;
+ Number factor;
+ WR_sum->GetTerm(1, factor, eta_DR);
+ SmartPtr<const Vector> wr_d =
+ dynamic_cast<const DiagMatrix*>(GetRawPtr(eta_DR))->GetDiag();
+ DBG_ASSERT(IsValid(wr_d));
+
+ if (IsValid(CD_x)) {
+ D_xR = D_x_plus_wr_d(CD_x->GetComp(0), factor, *wr_d);
+ }
+ else {
+ D_xR = D_x_plus_wr_d(NULL, factor, *wr_d);
+ }
+ }
+ else {
+ // Looks like limited memory quasi-Newton stuff
+ const LowRankUpdateSymMatrix* LR_W =
+ dynamic_cast<const LowRankUpdateSymMatrix*>(GetRawPtr(CW->GetComp(0,0)));
+ DBG_ASSERT(LR_W);
+ h_orig = LR_W;
+ if (IsValid(CD_x)) {
+ D_xR = CD_x->GetComp(0);
+ }
+ else {
+ D_xR = NULL;
+ }
+ }
+
+ Number delta_xR = delta_x;
+ SmartPtr<const Vector> D_sR = D_s;
+ Number delta_sR = delta_s;
+ SmartPtr<const Matrix> J_cR = CJ_c->GetComp(0,0);
+ SmartPtr<const Vector> D_cR =
+ Neg_Omega_c_plus_D_c(sigma_tilde_n_c_inv, sigma_tilde_p_c_inv,
+ D_c, rhs_c);
+ DBG_PRINT((1,"D_cR tag = %d\n",D_cR->GetTag()));
+ Number delta_cR = delta_c;
+ SmartPtr<const Matrix> J_dR = CJ_d->GetComp(0,0);
+ SmartPtr<const Vector> D_dR =
+ Neg_Omega_d_plus_D_d(*pd_l, sigma_tilde_n_d_inv, *neg_pd_u,
+ sigma_tilde_p_d_inv, D_d, rhs_d);
+ Number delta_dR = delta_d;
+ SmartPtr<const Vector> rhs_xR = Crhs_x->GetComp(0);
+ SmartPtr<const Vector> rhs_sR = &rhs_s;
+ SmartPtr<const Vector> rhs_cR = Rhs_cR(rhs_c, sigma_tilde_n_c_inv,
+ *Crhs_x->GetComp(1),
+ sigma_tilde_p_c_inv,
+ *Crhs_x->GetComp(2));
+ SmartPtr<const Vector> rhs_dR = Rhs_dR(rhs_d, sigma_tilde_n_d_inv,
+ *Crhs_x->GetComp(3), *pd_l,
+ sigma_tilde_p_d_inv,
+ *Crhs_x->GetComp(4), *neg_pd_u);
+ SmartPtr<Vector> sol_xR = Csol_x->GetCompNonConst(0);
+ Vector& sol_sR = sol_s;
+ Vector& sol_cR = sol_c;
+ Vector& sol_dR = sol_d;
+
+ ESymSolverStatus status = orig_aug_solver_->Solve(GetRawPtr(h_orig),
+ orig_W_factor,
+ GetRawPtr(D_xR), delta_xR,
+ GetRawPtr(D_sR), delta_sR,
+ GetRawPtr(J_cR), GetRawPtr(D_cR),
+ delta_cR,
+ GetRawPtr(J_dR), GetRawPtr(D_dR),
+ delta_dR,
+ *rhs_xR, *rhs_sR, *rhs_cR, *rhs_dR,
+ *sol_xR, sol_sR, sol_cR, sol_dR,
+ check_NegEVals,
+ numberOfNegEVals);
+
+ if (status == SYMSOLVER_SUCCESS) {
+ // Now back out the solutions for the n and p variables
+ SmartPtr<Vector> sol_n_c = Csol_x->GetCompNonConst(1);
+ sol_n_c->Set(0.0);
+ if (IsValid(sigma_tilde_n_c_inv)) {
+ sol_n_c->AddTwoVectors(1., *Crhs_x->GetComp(1), -1.0, sol_cR, 0.);
+ sol_n_c->ElementWiseMultiply(*sigma_tilde_n_c_inv);
+ }
+
+ SmartPtr<Vector> sol_p_c = Csol_x->GetCompNonConst(2);
+ sol_p_c->Set(0.0);
+ if (IsValid(sigma_tilde_p_c_inv)) {
+ DBG_PRINT_VECTOR(2, "rhs_pc", *Crhs_x->GetComp(2));
+ DBG_PRINT_VECTOR(2, "delta_y_c", sol_cR);
+ DBG_PRINT_VECTOR(2, "Sig~_{p_c}^{-1}", *sigma_tilde_p_c_inv);
+ sol_p_c->AddTwoVectors(1., *Crhs_x->GetComp(2), 1.0, sol_cR, 0.);
+ sol_p_c->ElementWiseMultiply(*sigma_tilde_p_c_inv);
+ }
+
+ SmartPtr<Vector> sol_n_d = Csol_x->GetCompNonConst(3);
+ sol_n_d->Set(0.0);
+ if (IsValid(sigma_tilde_n_d_inv)) {
+ pd_l->TransMultVector(-1.0, sol_dR, 0.0, *sol_n_d);
+ sol_n_d->Axpy(1.0, *Crhs_x->GetComp(3));
+ sol_n_d->ElementWiseMultiply(*sigma_tilde_n_d_inv);
+ }
+
+ SmartPtr<Vector> sol_p_d = Csol_x->GetCompNonConst(4);
+ sol_p_d->Set(0.0);
+ if (IsValid(sigma_tilde_p_d_inv)) {
+ neg_pd_u->TransMultVector(-1.0, sol_dR, 0.0, *sol_p_d);
+ sol_p_d->Axpy(1.0, *Crhs_x->GetComp(4));
+ sol_p_d->ElementWiseMultiply(*sigma_tilde_p_d_inv);
+ }
+ }
+
+ return status;
+
+ }
+
+ SmartPtr<const Vector>
+ AugRestoSystemSolver::Neg_Omega_c_plus_D_c(
+ const SmartPtr<const Vector>& sigma_tilde_n_c_inv,
+ const SmartPtr<const Vector>& sigma_tilde_p_c_inv,
+ const Vector* D_c,
+ const Vector& any_vec_in_c)
+ {
+ DBG_START_METH("AugRestoSystemSolver::Neg_Omega_c_plus_D_c",dbg_verbosity);
+ SmartPtr<Vector> retVec;
+ if (IsValid(sigma_tilde_n_c_inv) || IsValid(sigma_tilde_p_c_inv) || D_c) {
+ if (!neg_omega_c_plus_D_c_cache_.
+ GetCachedResult3Dep(retVec, GetRawPtr(sigma_tilde_n_c_inv), GetRawPtr(sigma_tilde_p_c_inv), D_c)) {
+ DBG_PRINT((1,"Not found in cache\n"));
+ retVec = any_vec_in_c.MakeNew();
+
+ Number fact1, fact2;
+ SmartPtr<const Vector> v1;
+ SmartPtr<const Vector> v2;
+
+ if (IsValid(sigma_tilde_n_c_inv)) {
+ v1 = sigma_tilde_n_c_inv;
+ fact1 = -1.;
+ }
+ else {
+ v1 = &any_vec_in_c;
+ fact1 = 0.;
+ }
+ if (IsValid(sigma_tilde_p_c_inv)) {
+ v2 = sigma_tilde_p_c_inv;
+ fact2 = -1.;
+ }
+ else {
+ v2 = &any_vec_in_c;
+ fact2 = 0.;
+ }
+ retVec->AddTwoVectors(fact1, *v1, fact2, *v2, 0.);
+
+ if (D_c) {
+ retVec->Axpy(1.0, *D_c);
+ }
+
+ neg_omega_c_plus_D_c_cache_.
+ AddCachedResult3Dep(retVec, GetRawPtr(sigma_tilde_n_c_inv), GetRawPtr(sigma_tilde_p_c_inv), D_c);
+ }
+ }
+ return ConstPtr(retVec);
+ }
+
+ SmartPtr<const Vector>
+ AugRestoSystemSolver::Neg_Omega_d_plus_D_d(
+ const Matrix& Pd_L,
+ const SmartPtr<const Vector>& sigma_tilde_n_d_inv,
+ const Matrix& neg_Pd_U,
+ const SmartPtr<const Vector>& sigma_tilde_p_d_inv,
+ const Vector* D_d,
+ const Vector& any_vec_in_d)
+ {
+ DBG_START_METH("AugRestoSystemSolver::Neg_Omega_d_plus_D_d",dbg_verbosity);
+ SmartPtr<Vector> retVec;
+ if (IsValid(sigma_tilde_n_d_inv) || IsValid(sigma_tilde_p_d_inv) || D_d) {
+ std::vector<const TaggedObject*> deps(5);
+ std::vector<Number> scalar_deps;
+ deps[0] = &Pd_L;
+ deps[1] = GetRawPtr(sigma_tilde_n_d_inv);
+ deps[2] = &neg_Pd_U;
+ deps[3] = GetRawPtr(sigma_tilde_p_d_inv);
+ deps[4] = D_d;
+ if (!neg_omega_d_plus_D_d_cache_.
+ GetCachedResult(retVec, deps, scalar_deps)) {
+ DBG_PRINT((1,"Not found in cache\n"));
+ retVec = any_vec_in_d.MakeNew();
+ retVec->Set(0.0);
+ if (IsValid(sigma_tilde_n_d_inv)) {
+ Pd_L.MultVector(-1.0, *sigma_tilde_n_d_inv, 1.0, *retVec);
+ }
+ if (IsValid(sigma_tilde_p_d_inv)) {
+ neg_Pd_U.MultVector(1.0, *sigma_tilde_p_d_inv, 1.0, *retVec);
+ }
+ if (D_d) {
+ retVec->Copy(*D_d);
+ }
+ neg_omega_d_plus_D_d_cache_.
+ AddCachedResult(retVec, deps, scalar_deps);
+ }
+ }
+ return ConstPtr(retVec);
+ }
+
+ SmartPtr<const Vector> AugRestoSystemSolver::Sigma_tilde_n_c_inv(
+ const SmartPtr<const Vector>& sigma_n_c,
+ Number delta_x,
+ const Vector& any_vec_in_c)
+ {
+ DBG_START_METH("AugRestoSystemSolver::Sigma_tilde_n_c_inv",dbg_verbosity);
+ SmartPtr<Vector> retVec;
+ if (IsValid(sigma_n_c) || delta_x != 0.0) {
+ std::vector<const TaggedObject*> deps(1);
+ std::vector<Number> scalar_deps(1);
+ deps[0] = GetRawPtr(sigma_n_c);
+ scalar_deps[0] = delta_x;
+ if (!sigma_tilde_n_c_inv_cache_.GetCachedResult(retVec, deps, scalar_deps)) {
+ DBG_PRINT((1,"Not found in cache\n"));
+ retVec = any_vec_in_c.MakeNew();
+ if (IsValid(sigma_n_c)) {
+ if (delta_x != 0.) {
+ retVec->Copy(*sigma_n_c);
+ retVec->AddScalar(delta_x);
+ retVec->ElementWiseReciprocal();
+ }
+ else {
+ // Given a "homogenous vector" implementation (such as in
+ // DenseVector) the following should be more efficient
+ retVec->Set(1.);
+ retVec->ElementWiseDivide(*sigma_n_c);
+ }
+ }
+ else {
+ retVec->Set(1/delta_x);
+ }
+
+ sigma_tilde_n_c_inv_cache_.AddCachedResult(retVec, deps, scalar_deps);
+ }
+ }
+
+ return ConstPtr(retVec);
+ }
+
+ SmartPtr<const Vector> AugRestoSystemSolver::Sigma_tilde_p_c_inv(
+ const SmartPtr<const Vector>& sigma_p_c,
+ Number delta_x,
+ const Vector& any_vec_in_c)
+ {
+ DBG_START_METH("AugRestoSystemSolver::Sigma_tilde_p_c_inv",dbg_verbosity);
+ SmartPtr<Vector> retVec;
+ if (IsValid(sigma_p_c) || delta_x != 0.0) {
+ std::vector<const TaggedObject*> deps(1);
+ std::vector<Number> scalar_deps(1);
+ deps[0] = GetRawPtr(sigma_p_c);
+ scalar_deps[0] = delta_x;
+ if (!sigma_tilde_p_c_inv_cache_.GetCachedResult(retVec, deps, scalar_deps)) {
+ DBG_PRINT((1,"Not found in cache\n"));
+ retVec = any_vec_in_c.MakeNew();
+ if (IsValid(sigma_p_c)) {
+ if (delta_x != 0.) {
+ retVec->Copy(*sigma_p_c);
+ retVec->AddScalar(delta_x);
+ retVec->ElementWiseReciprocal();
+ }
+ else {
+ retVec->Set(1.);
+ retVec->ElementWiseDivide(*sigma_p_c);
+ }
+ }
+ else {
+ retVec->Set(1/delta_x);
+ }
+
+ sigma_tilde_p_c_inv_cache_.AddCachedResult(retVec, deps, scalar_deps);
+ }
+ }
+
+ return ConstPtr(retVec);
+ }
+
+ SmartPtr<const Vector> AugRestoSystemSolver::Sigma_tilde_n_d_inv(
+ const SmartPtr<const Vector>& sigma_n_d,
+ Number delta_x,
+ const Vector& any_vec_in_n_d)
+ {
+ DBG_START_METH("AugRestoSystemSolver::Sigma_tilde_n_d_inv",dbg_verbosity);
+ SmartPtr<Vector> retVec;
+ if (IsValid(sigma_n_d) || delta_x != 0) {
+ std::vector<const TaggedObject*> deps(1);
+ std::vector<Number> scalar_deps(1);
+ deps[0] = GetRawPtr(sigma_n_d);
+ scalar_deps[0] = delta_x;
+ if (!sigma_tilde_n_d_inv_cache_.GetCachedResult(retVec, deps, scalar_deps)) {
+ DBG_PRINT((1,"Not found in cache\n"));
+ retVec = any_vec_in_n_d.MakeNew();
+ if (IsValid(sigma_n_d)) {
+ if (delta_x != 0.) {
+ retVec->Copy(*sigma_n_d);
+ retVec->AddScalar(delta_x);
+ retVec->ElementWiseReciprocal();
+ }
+ else {
+ retVec->Set(1.);
+ retVec->ElementWiseDivide(*sigma_n_d);
+ }
+ }
+ else {
+ retVec->Set(1/delta_x);
+ }
+
+ sigma_tilde_n_d_inv_cache_.AddCachedResult(retVec, deps, scalar_deps);
+ }
+ }
+
+ return ConstPtr(retVec);
+ }
+
+ SmartPtr<const Vector> AugRestoSystemSolver::Sigma_tilde_p_d_inv(
+ const SmartPtr<const Vector>& sigma_p_d,
+ Number delta_x,
+ const Vector& any_vec_in_p_d)
+ {
+ DBG_START_METH("AugRestoSystemSolver::Sigma_tilde_p_d_inv",dbg_verbosity);
+ SmartPtr<Vector> retVec;
+ if (IsValid(sigma_p_d) || delta_x != 0) {
+ std::vector<const TaggedObject*> deps(1);
+ std::vector<Number> scalar_deps(1);
+ deps[0] = GetRawPtr(sigma_p_d);
+ scalar_deps[0] = delta_x;
+ if (!sigma_tilde_p_d_inv_cache_.GetCachedResult(retVec, deps, scalar_deps)) {
+ DBG_PRINT((1,"Not found in cache\n"));
+ retVec = any_vec_in_p_d.MakeNew();
+
+ if (IsValid(sigma_p_d)) {
+ if (delta_x != 0.) {
+ retVec->Copy(*sigma_p_d);
+ retVec->AddScalar(delta_x);
+ retVec->ElementWiseReciprocal();
+ }
+ else {
+ retVec->Set(1.);
+ retVec->ElementWiseDivide(*sigma_p_d);
+ }
+ }
+ else {
+ retVec->Set(1/delta_x);
+ }
+
+ sigma_tilde_p_d_inv_cache_.AddCachedResult(retVec, deps, scalar_deps);
+ }
+ }
+
+ return ConstPtr(retVec);
+ }
+
+ SmartPtr<const Vector> AugRestoSystemSolver::D_x_plus_wr_d(
+ const SmartPtr<const Vector>& CD_x0,
+ Number factor,
+ const Vector& wr_d)
+ {
+ DBG_START_METH("AugRestoSystemSolver::D_x_plus_wr_d",dbg_verbosity);
+ SmartPtr<Vector> retVec;
+
+ std::vector<const TaggedObject*> deps(2);
+ deps[0] = &wr_d;
+ if (IsValid(CD_x0)) {
+ deps[1] = GetRawPtr(CD_x0);
+ }
+ else {
+ deps[1] = NULL;
+ }
+ std::vector<Number> scalar_deps(1);
+ scalar_deps[0] = factor;
+
+ if (!d_x_plus_wr_d_cache_.GetCachedResult(retVec, deps, scalar_deps)) {
+ DBG_PRINT((1,"Not found in cache\n"));
+ retVec = wr_d.MakeNew();
+
+ Number fact;
+ SmartPtr<const Vector> v;
+ if (IsValid(CD_x0)) {
+ fact = 1.;
+ v = CD_x0;
+ }
+ else {
+ fact = 0.;
+ v = &wr_d;
+ }
+ retVec->AddTwoVectors(factor, wr_d, fact, *v, 0.);
+
+ d_x_plus_wr_d_cache_.AddCachedResult(retVec, deps, scalar_deps);
+ }
+ DBG_PRINT_VECTOR(2, "retVec", *retVec);
+ return ConstPtr(retVec);
+ }
+
+ SmartPtr<const Vector> AugRestoSystemSolver::Rhs_cR(const Vector& rhs_c,
+ const SmartPtr<const Vector>& sigma_tilde_n_c_inv, const Vector& rhs_n_c,
+ const SmartPtr<const Vector>& sigma_tilde_p_c_inv, const Vector& rhs_p_c)
+ {
+ DBG_START_METH("AugRestoSystemSolver::Rhs_cR",dbg_verbosity);
+ SmartPtr<Vector> retVec;
+ std::vector<const TaggedObject*> deps(5);
+ std::vector<Number> scalar_deps;
+ deps[0] = &rhs_c;
+ deps[1] = GetRawPtr(sigma_tilde_n_c_inv);
+ deps[2] = &rhs_n_c;
+ deps[3] = GetRawPtr(sigma_tilde_p_c_inv);
+ deps[4] = &rhs_p_c;
+ if (!rhs_cR_cache_.GetCachedResult(retVec, deps, scalar_deps)) {
+ DBG_PRINT((1,"Not found in cache\n"));
+ retVec = rhs_c.MakeNew();
+ retVec->Copy(rhs_c);
+
+ SmartPtr<Vector> tmp = retVec->MakeNew();
+ if (IsValid(sigma_tilde_n_c_inv)) {
+ tmp->Copy(*sigma_tilde_n_c_inv);
+ tmp->ElementWiseMultiply(rhs_n_c);
+ retVec->Axpy(-1.0, *tmp);
+ }
+
+ if (IsValid(sigma_tilde_p_c_inv)) {
+ tmp->Copy(*sigma_tilde_p_c_inv);
+ tmp->ElementWiseMultiply(rhs_p_c);
+ retVec->Axpy(1.0, *tmp);
+ }
+ rhs_cR_cache_.AddCachedResult(retVec, deps, scalar_deps);
+ }
+ return ConstPtr(retVec);
+ }
+
+ SmartPtr<const Vector> AugRestoSystemSolver::Rhs_dR(const Vector& rhs_d,
+ const SmartPtr<const Vector>& sigma_tilde_n_d_inv, const Vector& rhs_n_d, const Matrix& pd_L,
+ const SmartPtr<const Vector>& sigma_tilde_p_d_inv, const Vector& rhs_p_d, const Matrix& neg_pd_U)
+ {
+ DBG_START_METH("AugRestoSystemSolver::Rhs_dR",dbg_verbosity);
+ SmartPtr<Vector> retVec;
+ std::vector<const TaggedObject*> deps(7);
+ std::vector<Number> scalar_deps;
+ deps[0] = &rhs_d;
+ deps[1] = GetRawPtr(sigma_tilde_n_d_inv);
+ deps[2] = &rhs_n_d;
+ deps[3] = &pd_L;
+ deps[4] = GetRawPtr(sigma_tilde_p_d_inv);
+ deps[5] = &rhs_p_d;
+ deps[6] = &neg_pd_U;
+ if (!rhs_dR_cache_.GetCachedResult(retVec, deps, scalar_deps)) {
+ DBG_PRINT((1,"Not found in cache\n"));
+ retVec = rhs_d.MakeNew();
+ retVec->Copy(rhs_d);
+
+ if (IsValid(sigma_tilde_n_d_inv)) {
+ SmartPtr<Vector> tmpn = sigma_tilde_n_d_inv->MakeNew();
+ tmpn->Copy(*sigma_tilde_n_d_inv);
+ tmpn->ElementWiseMultiply(rhs_n_d);
+ pd_L.MultVector(-1.0, *tmpn, 1.0, *retVec);
+ }
+
+ if (IsValid(sigma_tilde_p_d_inv)) {
+ SmartPtr<Vector> tmpp = sigma_tilde_p_d_inv->MakeNew();
+ tmpp->Copy(*sigma_tilde_p_d_inv);
+ tmpp->ElementWiseMultiply(rhs_p_d);
+ neg_pd_U.MultVector(-1.0, *tmpp, 1.0, *retVec);
+ }
+
+ rhs_dR_cache_.AddCachedResult(retVec, deps, scalar_deps);
+ }
+ return ConstPtr(retVec);
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpAugRestoSystemSolver.hpp b/SimTKmath/Optimizers/src/IpOpt/IpAugRestoSystemSolver.hpp
new file mode 100644
index 0000000..eb62950
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpAugRestoSystemSolver.hpp
@@ -0,0 +1,197 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpAugRestoSystemSolver.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IP_AUGRESTOSYSTEMSOLVER_HPP__
+#define __IP_AUGRESTOSYSTEMSOLVER_HPP__
+
+#include "IpAugSystemSolver.hpp"
+
+namespace Ipopt
+{
+
+ /** Class that converts the an augmented system with compound restoration
+ * pieces into a smaller "pivoted" system to be solved with an
+ * existing AugSystemSolver. This is really a decorator that changes
+ * the behavior of the AugSystemSolver to account for the known structure
+ * of the restoration phase.
+ */
+ class AugRestoSystemSolver: public AugSystemSolver
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor. Here, orig_aug_solver is the object for solving
+ * the original augmented system. The flag
+ * skip_orig_aug_solver_init indicates, if the initialization
+ * call (to Initialize) should be skipped; this flag will usually
+ * be true, so that the symbolic factorization of the main
+ * algorithm will be used. */
+ AugRestoSystemSolver(AugSystemSolver& orig_aug_solver,
+ bool skip_orig_aug_solver_init=true);
+
+ /** Default destructor */
+ virtual ~AugRestoSystemSolver();
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Translate the augmented system (in the full space of the
+ * restoration variables) into the smaller space of the original
+ * variables
+ */
+ virtual ESymSolverStatus Solve(
+ const SymMatrix* W,
+ Number W_factor,
+ const Vector* D_x,
+ Number delta_x,
+ const Vector* D_s,
+ Number delta_s,
+ const Matrix* J_c,
+ const Vector* D_c,
+ Number delta_c,
+ const Matrix* J_d,
+ const Vector* D_d,
+ Number delta_d,
+ const Vector& rhs_x,
+ const Vector& rhs_s,
+ const Vector& rhs_c,
+ const Vector& rhs_d,
+ Vector& sol_x,
+ Vector& sol_s,
+ Vector& sol_c,
+ Vector& sol_d,
+ bool check_NegEVals,
+ Index numberOfNegEVals);
+
+ /** Returns the number of negative eigenvalues from the original
+ * augmented system call
+ */
+ virtual Index NumberOfNegEVals() const
+ {
+ return orig_aug_solver_->NumberOfNegEVals();
+ }
+
+ /** Query whether inertia is computed by linear solver.
+ * Returns true, if linear solver provides inertia.
+ */
+ virtual bool ProvidesInertia() const
+ {
+ return orig_aug_solver_->ProvidesInertia();
+ }
+
+ /** Request to increase quality of solution for next solve. Ask
+ * underlying linear solver to increase quality of solution for
+ * the next solve (e.g. increase pivot tolerance). Returns
+ * false, if this is not possible (e.g. maximal pivot tolerance
+ * already used.)
+ */
+ virtual bool IncreaseQuality()
+ {
+ return orig_aug_solver_->IncreaseQuality();
+ }
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ AugRestoSystemSolver();
+
+ /** Copy Constructor */
+ AugRestoSystemSolver(const AugRestoSystemSolver&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const AugRestoSystemSolver&);
+ //@}
+
+ /**@name Caches for some of the necessary calculated quantities */
+ //@{
+ CachedResults< SmartPtr<Vector> > neg_omega_c_plus_D_c_cache_;
+ CachedResults< SmartPtr<Vector> > neg_omega_d_plus_D_d_cache_;
+ CachedResults< SmartPtr<Vector> > sigma_tilde_n_c_inv_cache_;
+ CachedResults< SmartPtr<Vector> > sigma_tilde_p_c_inv_cache_;
+ CachedResults< SmartPtr<Vector> > sigma_tilde_n_d_inv_cache_;
+ CachedResults< SmartPtr<Vector> > sigma_tilde_p_d_inv_cache_;
+ CachedResults< SmartPtr<Vector> > d_x_plus_wr_d_cache_;
+ CachedResults< SmartPtr<Vector> > rhs_cR_cache_;
+ CachedResults< SmartPtr<Vector> > rhs_dR_cache_;
+ //@}
+
+ /**@name Methods to calculate the cached quantities */
+ //@{
+ SmartPtr<const Vector> Neg_Omega_c_plus_D_c(
+ const SmartPtr<const Vector>& sigma_tilde_n_c_inv,
+ const SmartPtr<const Vector>& sigma_tilde_p_c_inv,
+ const Vector* D_c,
+ const Vector& any_vec_in_c);
+
+ SmartPtr<const Vector> Neg_Omega_d_plus_D_d(
+ const Matrix& Pd_L,
+ const SmartPtr<const Vector>& sigma_tilde_n_d_inv,
+ const Matrix& neg_Pd_U,
+ const SmartPtr<const Vector>& sigma_tilde_p_d_inv,
+ const Vector* D_d,
+ const Vector& any_vec_in_d);
+
+ /** Sigma tilde is the sum of Sigma and delta_x times the identity */
+ SmartPtr<const Vector> Sigma_tilde_n_c_inv(
+ const SmartPtr<const Vector>& sigma_tilde_n_c,
+ Number delta_x,
+ const Vector& any_vec_in_n_c);
+
+ SmartPtr<const Vector> Sigma_tilde_p_c_inv(
+ const SmartPtr<const Vector>& sigma_tilde_p_c,
+ Number delta_x,
+ const Vector& any_vec_in_p_c);
+
+ SmartPtr<const Vector> Sigma_tilde_n_d_inv(
+ const SmartPtr<const Vector>& sigma_tilde_n_d,
+ Number delta_x,
+ const Vector& any_vec_in_n_d);
+
+ SmartPtr<const Vector> Sigma_tilde_p_d_inv(
+ const SmartPtr<const Vector>& sigma_tilde_p_d,
+ Number delta_x,
+ const Vector& any_vec_in_p_d);
+
+ SmartPtr<const Vector> D_x_plus_wr_d(
+ const SmartPtr<const Vector>& CD_x0,
+ Number factor,
+ const Vector& wr_d);
+
+ SmartPtr<const Vector> Rhs_cR(
+ const Vector& rhs_c,
+ const SmartPtr<const Vector>& sigma_tilde_n_c_inv,
+ const Vector& rhs_n_c,
+ const SmartPtr<const Vector>& sigma_tilde_p_c_inv,
+ const Vector& rhs_p_c);
+
+ SmartPtr<const Vector> Rhs_dR(
+ const Vector& rhs_d,
+ const SmartPtr<const Vector>& sigma_tilde_n_d_inv,
+ const Vector& rhs_n_d,
+ const Matrix& pd_L,
+ const SmartPtr<const Vector>& sigma_tilde_p_d_inv,
+ const Vector& rhs_p_d,
+ const Matrix& pd_U);
+ //@}
+
+ SmartPtr<AugSystemSolver> orig_aug_solver_;
+ bool skip_orig_aug_solver_init_;
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpAugSystemSolver.hpp b/SimTKmath/Optimizers/src/IpOpt/IpAugSystemSolver.hpp
new file mode 100644
index 0000000..b5d06bc
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpAugSystemSolver.hpp
@@ -0,0 +1,199 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpAugSystemSolver.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IP_AUGSYSTEMSOLVER_HPP__
+#define __IP_AUGSYSTEMSOLVER_HPP__
+
+#include "IpSymMatrix.hpp"
+#include "IpSymLinearSolver.hpp"
+#include "IpAlgStrategy.hpp"
+
+namespace Ipopt
+{
+
+ /** Base class for Solver for the augmented system. This is the
+ * base class for linear solvers that solve the augmented system,
+ * which is defined as
+ *
+ * \f$\left[\begin{array}{cccc}
+ * W + D_x + \delta_xI & 0 & J_c^T & J_d^T\\
+ * 0 & D_s + \delta_sI & 0 & -I \\
+ * J_c & 0 & D_c - \delta_cI & 0\\
+ * J_d & -I & 0 & D_d - \delta_dI
+ * \end{array}\right]
+ * \left(\begin{array}{c}sol_x\\sol_s\\sol_c\\sol_d\end{array}\right)=
+ * \left(\begin{array}{c}rhs_x\\rhs_s\\rhs_c\\rhs_d\end{array}\right)\f$
+ *
+ * Since this system might be solved repeatedly for different right
+ * hand sides, it is desirable to step the factorization of a
+ * direct linear solver if possible.
+ */
+ class AugSystemSolver: public AlgorithmStrategyObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default constructor. */
+ AugSystemSolver()
+ {}
+ /** Default destructor */
+ virtual ~AugSystemSolver()
+ {}
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix) = 0;
+
+ /** Set up the augmented system and solve it for a given right hand
+ * side. If desired (i.e. if check_NegEVals is true), then the
+ * solution is only computed if the number of negative eigenvalues
+ * matches numberOfNegEVals.
+ *
+ * The return value is the return value of the linear solver object.
+ */
+ virtual ESymSolverStatus Solve(
+ const SymMatrix* W,
+ Number W_factor,
+ const Vector* D_x,
+ Number delta_x,
+ const Vector* D_s,
+ Number delta_s,
+ const Matrix* J_c,
+ const Vector* D_c,
+ Number delta_c,
+ const Matrix* J_d,
+ const Vector* D_d,
+ Number delta_d,
+ const Vector& rhs_x,
+ const Vector& rhs_s,
+ const Vector& rhs_c,
+ const Vector& rhs_d,
+ Vector& sol_x,
+ Vector& sol_s,
+ Vector& sol_c,
+ Vector& sol_d,
+ bool check_NegEVals,
+ Index numberOfNegEVals)
+ {
+ std::vector<SmartPtr<const Vector> > rhs_xV(1);
+ rhs_xV[0] = &rhs_x;
+ std::vector<SmartPtr<const Vector> > rhs_sV(1);
+ rhs_sV[0] = &rhs_s;
+ std::vector<SmartPtr<const Vector> > rhs_cV(1);
+ rhs_cV[0] = &rhs_c;
+ std::vector<SmartPtr<const Vector> > rhs_dV(1);
+ rhs_dV[0] = &rhs_d;
+ std::vector<SmartPtr<Vector> > sol_xV(1);
+ sol_xV[0] = &sol_x;
+ std::vector<SmartPtr<Vector> > sol_sV(1);
+ sol_sV[0] = &sol_s;
+ std::vector<SmartPtr<Vector> > sol_cV(1);
+ sol_cV[0] = &sol_c;
+ std::vector<SmartPtr<Vector> > sol_dV(1);
+ sol_dV[0] = &sol_d;
+ return MultiSolve(W, W_factor, D_x, delta_x, D_s, delta_s, J_c, D_c, delta_c,
+ J_d, D_d, delta_d, rhs_xV, rhs_sV, rhs_cV, rhs_dV,
+ sol_xV, sol_sV, sol_cV, sol_dV, check_NegEVals,
+ numberOfNegEVals);
+ }
+
+ /** Like Solve, but for multiple right hand sides. The inheriting
+ * class has to be overload at least one of Solve and
+ * MultiSolve. */
+ virtual ESymSolverStatus MultiSolve(
+ const SymMatrix* W,
+ Number W_factor,
+ const Vector* D_x,
+ Number delta_x,
+ const Vector* D_s,
+ Number delta_s,
+ const Matrix* J_c,
+ const Vector* D_c,
+ Number delta_c,
+ const Matrix* J_d,
+ const Vector* D_d,
+ Number delta_d,
+ std::vector<SmartPtr<const Vector> >& rhs_xV,
+ std::vector<SmartPtr<const Vector> >& rhs_sV,
+ std::vector<SmartPtr<const Vector> >& rhs_cV,
+ std::vector<SmartPtr<const Vector> >& rhs_dV,
+ std::vector<SmartPtr<Vector> >& sol_xV,
+ std::vector<SmartPtr<Vector> >& sol_sV,
+ std::vector<SmartPtr<Vector> >& sol_cV,
+ std::vector<SmartPtr<Vector> >& sol_dV,
+ bool check_NegEVals,
+ Index numberOfNegEVals)
+ {
+ // Solve for one right hand side after the other
+ Index nrhs = (Index)rhs_xV.size();
+ DBG_ASSERT(nrhs>0);
+ DBG_ASSERT(nrhs==(Index)rhs_sV.size());
+ DBG_ASSERT(nrhs==(Index)rhs_cV.size());
+ DBG_ASSERT(nrhs==(Index)rhs_dV.size());
+ DBG_ASSERT(nrhs==(Index)sol_xV.size());
+ DBG_ASSERT(nrhs==(Index)sol_sV.size());
+ DBG_ASSERT(nrhs==(Index)sol_cV.size());
+ DBG_ASSERT(nrhs==(Index)sol_dV.size());
+
+ ESymSolverStatus retval=SYMSOLVER_SUCCESS;
+ for (Index i=0; i<nrhs; i++) {
+ retval = Solve(W, W_factor, D_x, delta_x, D_s, delta_s, J_c, D_c, delta_c,
+ J_d, D_d, delta_d,
+ *rhs_xV[i], *rhs_sV[i], *rhs_cV[i], *rhs_dV[i],
+ *sol_xV[i], *sol_sV[i], *sol_cV[i], *sol_dV[i],
+ check_NegEVals, numberOfNegEVals);
+ if (retval!=SYMSOLVER_SUCCESS) {
+ break;
+ }
+ }
+ return retval;
+ }
+
+ /** Number of negative eigenvalues detected during last
+ * solve. Returns the number of negative eigenvalues of
+ * the most recent factorized matrix. This must not be called if
+ * the linear solver does not compute this quantities (see
+ * ProvidesInertia).
+ */
+ virtual Index NumberOfNegEVals() const =0;
+
+ /** Query whether inertia is computed by linear solver.
+ * Returns true, if linear solver provides inertia.
+ */
+ virtual bool ProvidesInertia() const =0;
+
+ /** Request to increase quality of solution for next solve. Ask
+ * underlying linear solver to increase quality of solution for
+ * the next solve (e.g. increase pivot tolerance). Returns
+ * false, if this is not possible (e.g. maximal pivot tolerance
+ * already used.)
+ */
+ virtual bool IncreaseQuality() =0;
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ AugSystemSolver(const AugSystemSolver&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const AugSystemSolver&);
+ //@}
+
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpBacktrackingLSAcceptor.hpp b/SimTKmath/Optimizers/src/IpOpt/IpBacktrackingLSAcceptor.hpp
new file mode 100644
index 0000000..24a7be8
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpBacktrackingLSAcceptor.hpp
@@ -0,0 +1,137 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpBacktrackingLSAcceptor.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+// Andreas Waechter IBM 2005-10-13
+// derived file from IpFilterLineSearch.hpp
+
+#ifndef __IPBACKTRACKINGLSACCEPTOR_HPP__
+#define __IPBACKTRACKINGLSACCEPTOR_HPP__
+
+#include "IpAlgStrategy.hpp"
+
+namespace Ipopt
+{
+
+ /** Base class for backtracking line search acceptors.
+ */
+ class BacktrackingLSAcceptor : public AlgorithmStrategyObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor. */
+ BacktrackingLSAcceptor()
+ {}
+
+ /** Default destructor */
+ virtual ~BacktrackingLSAcceptor()
+ {}
+ //@}
+
+ /** InitializeImpl - overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix)=0;
+
+ /** Reset the acceptor.
+ * This function should be called if all previous information
+ * should be discarded when the line search is performed the
+ * next time. For example, this method should be called if
+ * the barrier parameter is changed.
+ */
+ virtual void Reset()=0;
+
+ /** Initialization for the next line search. The flag in_watchdog
+ * indicates if we are currently in an active watchdog
+ * procedure. */
+ virtual void InitThisLineSearch(bool in_watchdog)=0;
+
+ /** Method that is called before the restoration phase is called.
+ * Here, we can set up things that are required in the
+ * termination test for the restoration phase, such as augmenting
+ * a filter. */
+ virtual void PrepareRestoPhaseStart()=0;
+
+ /** Method returning the lower bound on the trial step sizes. If
+ * the backtracking procedure encounters a trial step size below
+ * this value after the first trial set, it swtiches to the
+ * (soft) restoration phase. */
+ virtual Number CalculateAlphaMin()=0;
+
+ /** Method for checking if current trial point is acceptable. It
+ * is assumed that the delta information in ip_data is the search
+ * direction used in criteria. The primal trial point has to be
+ * set before the call. alpha_primal is the step size which is
+ * to be used for the test; if it is zero, then this method is
+ * called during the soft restoration phase.
+ */
+ virtual bool CheckAcceptabilityOfTrialPoint(Number alpha_primal)=0;
+
+ /** Try a second order correction for the constraints. If the
+ * first trial step (with incoming alpha_primal) has been reject,
+ * this tries second order corrections, e.g., for the
+ * constraints. Here, alpha_primal_test is the step size that
+ * has to be used in the filter acceptance tests. On output
+ * actual_delta_ has been set to the step including the
+ * second order correction if it has been accepted, otherwise it
+ * is unchanged. If the SOC step has been accepted, alpha_primal
+ * has the fraction-to-the-boundary value for the SOC step on output.
+ * The return value is true, if a SOC step has been accepted.
+ */
+ virtual bool TrySecondOrderCorrection(Number alpha_primal_test,
+ Number& alpha_primal,
+ SmartPtr<IteratesVector>& actual_delta)=0;
+
+ /** Try higher order corrector (for fast local convergence). In
+ * contrast to a second order correction step, which tries to
+ * make an unacceptable point acceptable by improving constraint
+ * violation, this corrector step is tried even if the regular
+ * primal-dual step is acceptable.
+ */
+ virtual bool TryCorrector(Number alpha_primal_test,
+ Number& alpha_primal,
+ SmartPtr<IteratesVector>& actual_delta)=0;
+
+ /** Method for ending the current line search. When it is called,
+ * the internal data should be updates, e.g., the filter might be
+ * augmented. alpha_primal_test is the value of alpha that has
+ * been used for in the acceptence test ealier. Return value is
+ * a character for the info_alpha_primal_char field in IpData. */
+ virtual char UpdateForNextIteration(Number alpha_primal_test)=0;
+
+ /** Method for setting internal data if the watchdog procedure is
+ * started. */
+ virtual void StartWatchDog()=0;
+
+ /** Method for setting internal data if the watchdog procedure is
+ * stopped. */
+ virtual void StopWatchDog()=0;
+
+ /** Methods for OptionsList */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ BacktrackingLSAcceptor(const BacktrackingLSAcceptor&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const BacktrackingLSAcceptor&);
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpBacktrackingLineSearch.cpp b/SimTKmath/Optimizers/src/IpOpt/IpBacktrackingLineSearch.cpp
new file mode 100644
index 0000000..d1a1bf4
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpBacktrackingLineSearch.cpp
@@ -0,0 +1,1167 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpBacktrackingLineSearch.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+// Andreas Waechter IBM 2005-10-13
+// derived file from IpFilterLineSearch.cpp
+
+#include "IpBacktrackingLineSearch.hpp"
+#include "IpJournalist.hpp"
+#include "IpRestoPhase.hpp"
+#include "IpAlgTypes.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+namespace Ipopt
+{
+
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ BacktrackingLineSearch::BacktrackingLineSearch(
+ const SmartPtr<BacktrackingLSAcceptor>& acceptor,
+ const SmartPtr<RestorationPhase>& resto_phase,
+ const SmartPtr<ConvergenceCheck>& conv_check)
+ :
+ LineSearch(),
+ acceptor_(acceptor),
+ resto_phase_(resto_phase),
+ conv_check_(conv_check)
+ {
+ DBG_START_FUN("BacktrackingLineSearch::BacktrackingLineSearch",
+ dbg_verbosity);
+ DBG_ASSERT(IsValid(acceptor_));
+ }
+
+ BacktrackingLineSearch::~BacktrackingLineSearch()
+ {
+ DBG_START_FUN("BacktrackingLineSearch::~BacktrackingLineSearch()",
+ dbg_verbosity);
+ }
+
+ void BacktrackingLineSearch::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->AddBoundedNumberOption(
+ "alpha_red_factor",
+ "Fractional reduction of the trial step size in the backtracking line search.",
+ 0.0, true, 1.0, true, 0.5,
+ "At every step of the backtracking line search, the trial step size is "
+ "reduced by this factor.");
+
+ std::string prev_category = roptions->RegisteringCategory();
+ roptions->SetRegisteringCategory("Undocumented");
+ roptions->AddStringOption2(
+ "magic_steps",
+ "Enables magic steps.",
+ "no",
+ "no", "don't take magic steps",
+ "yes", "take magic steps",
+ "DOESN'T REALLY WORK YET!");
+ roptions->SetRegisteringCategory(prev_category);
+
+ roptions->AddStringOption2(
+ "accept_every_trial_step",
+ "Always accept the frist trial step.",
+ "no",
+ "no", "don't arbitrarily accept the full step",
+ "yes", "always accept the full step",
+ "Setting this option to \"yes\" essentially disables the line search "
+ "and makes the algorithm take aggressive steps, without global "
+ "convergence guarantees.");
+
+ roptions->AddStringOption7(
+ "alpha_for_y",
+ "Method to determine the step size for constraint multipliers.",
+ "primal",
+ "primal", "use primal step size",
+ "bound_mult", "use step size for the bound multipliers (good for LPs)",
+ "min", "use the min of primal and bound multipliers",
+ "max", "use the max of primal and bound multipliers",
+ "full", "take a full step of size one",
+ "min_dual_infeas", "choose step size minimizing new dual infeasibility",
+ "safe_min_dual_infeas", "like \"min_dual_infeas\", but safeguarded by \"min\" and \"max\"",
+ "This option determines how the step size (alpha_y) will be calculated when updating the "
+ "constraint multipliers.");
+
+ roptions->AddLowerBoundedNumberOption(
+ "tiny_step_tol",
+ "Tolerance for detecting numerically insignificant steps.",
+ Number(0), false, 10*std::numeric_limits<Number>::epsilon(),
+ "If the search direction in the primal variables (x and s) is, in "
+ "relative terms for each component, less than this value, the "
+ "algorithm accepts the full step without line search. If this happens "
+ "repeatedly, the algorithm will terminate with a corresponding exit "
+ "message. The default value is 10 times machine precision.");
+ roptions->AddLowerBoundedNumberOption(
+ "tiny_step_y_tol",
+ "Tolerance for quitting because of numerically insignificant steps.",
+ Number(0), false, Number(1e-2),
+ "If the search direction in the primal variables (x and s) is, in "
+ "relative terms for each component, repeatedly less than tiny_step_tol, "
+ "and the step in the y variables is smaller than this threshold, the "
+ "algorithm will terminate.");
+ roptions->AddLowerBoundedIntegerOption(
+ "watchdog_shortened_iter_trigger",
+ "Number of shortened iterations that trigger the watchdog.",
+ 0, 10,
+ "If the number of successive iterations in which the backtracking line search "
+ "did not accept the first trial point exceeds this number, the "
+ "watchdog procedure is activated. Choosing \"0\" here disables the "
+ "watchdog procedure.");
+ roptions->AddLowerBoundedIntegerOption(
+ "watchdog_trial_iter_max",
+ "Maximum number of watchdog iterations.",
+ 1, 3,
+ "This option determines the number of trial iterations "
+ "allowed before the watchdog "
+ "procedure is aborted and the algorithm returns to the stored point.");
+
+ roptions->SetRegisteringCategory("Restoration Phase");
+ roptions->AddStringOption2(
+ "expect_infeasible_problem",
+ "Enable heuristics to quickly detect an infeasible problem.",
+ "no",
+ "no", "the problem probably be feasible",
+ "yes", "the problem has a good chance to be infeasible",
+ "This options is meant to activate heuristics that may speed up the "
+ "infeasibility determination if you expect that there is a good chance for the problem to be "
+ "infeasible. In the filter line search procedure, the restoration "
+ "phase is called more quickly than usually, and more reduction in "
+ "the constraint violation is enforced before the restoration phase is "
+ "left. If the problem is square, this option is enabled automatically.");
+ roptions->AddLowerBoundedNumberOption(
+ "expect_infeasible_problem_ctol",
+ "Threshold for disabling \"expect_infeasible_problem\" option.",
+ Number(0), false, Number(1e-3),
+ "If the constraint violation becomes smaller than this threshold, "
+ "the \"expect_infeasible_problem\" heuristics in the filter line "
+ "search are disabled. If the problem is square, this options is set to "
+ "0.");
+ roptions->AddStringOption2(
+ "start_with_resto",
+ "Tells algorithm to switch to restoration phase in first iteration.",
+ "no",
+ "no", "don't force start in restoration phase",
+ "yes", "force start in restoration phase",
+ "Setting this option to \"yes\" forces the algorithm to switch to the "
+ "feasibility restoration phase in the first iteration. If the initial "
+ "point is feasible, the algorithm will abort with a failure.");
+ roptions->AddLowerBoundedNumberOption(
+ "soft_resto_pderror_reduction_factor",
+ "Required reduction in primal-dual error in the soft restoration phase.",
+ Number(0), false, Number(1.0 - 1e-4),
+ "The soft restoration phase attempts to reduce the "
+ "primal-dual error with regular steps. If the damped "
+ "primal-dual step (damped only to satisfy the "
+ "fraction-to-the-boundary rule) is not decreasing the primal-dual error "
+ "by at least this factor, then the regular restoration phase is called. "
+ "Choosing \"0\" here disables the soft "
+ "restoration phase.");
+ roptions->AddLowerBoundedIntegerOption(
+ "max_soft_resto_iters",
+ "Maximum number of iterations performed successively in soft restoration phase.",
+ 0, 10,
+ "If the soft restoration phase is performed for more than so many "
+ "iteratins in a row, the regular restoration phase is called.");
+ }
+
+ bool BacktrackingLineSearch::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ options.GetNumericValue("alpha_red_factor", alpha_red_factor_, prefix);
+ options.GetBoolValue("magic_steps", magic_steps_, prefix);
+ options.GetBoolValue("accept_every_trial_step", accept_every_trial_step_, prefix);
+ Index enum_int;
+ options.GetEnumValue("alpha_for_y", enum_int, prefix);
+ alpha_for_y_ = AlphaForYEnum(enum_int);
+ options.GetNumericValue("expect_infeasible_problem_ctol", expect_infeasible_problem_ctol_, prefix);
+ options.GetBoolValue("expect_infeasible_problem", expect_infeasible_problem_, prefix);
+
+ options.GetBoolValue("start_with_resto", start_with_resto_, prefix);
+
+ options.GetNumericValue("tiny_step_tol", tiny_step_tol_, prefix);
+ options.GetNumericValue("tiny_step_y_tol", tiny_step_y_tol_, prefix);
+ options.GetIntegerValue("watchdog_trial_iter_max", watchdog_trial_iter_max_, prefix);
+ options.GetIntegerValue("watchdog_shortened_iter_trigger", watchdog_shortened_iter_trigger_, prefix);
+ options.GetNumericValue("soft_resto_pderror_reduction_factor",
+ soft_resto_pderror_reduction_factor_, prefix);
+ options.GetIntegerValue("max_soft_resto_iters", max_soft_resto_iters_,
+ prefix);
+
+ bool retvalue = true;
+ if (IsValid(resto_phase_)) {
+ if (!resto_phase_->Initialize(Jnlst(), IpNLP(), IpData(), IpCq(),
+ options, prefix)) {
+ return false;
+ }
+ if (!acceptor_->Initialize(Jnlst(), IpNLP(), IpData(), IpCq(),
+ options, prefix)) {
+ return false;
+ }
+ }
+
+ rigorous_ = true;
+ skipped_line_search_ = false;
+ tiny_step_last_iteration_ = false;
+ fallback_activated_ = false;
+
+ Reset();
+
+ count_successive_shortened_steps_ = 0;
+
+ acceptable_iterate_ = NULL;
+ acceptable_iteration_number_ = -1;
+
+ last_mu_ = -1.;
+
+ return retvalue;
+ }
+
+ void BacktrackingLineSearch::FindAcceptableTrialPoint()
+ {
+ DBG_START_METH("BacktrackingLineSearch::FindAcceptableTrialPoint",
+ dbg_verbosity);
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "--> Starting filter line search in iteration %d <--\n",
+ IpData().iter_count());
+
+ Number curr_mu = IpData().curr_mu();
+ if (last_mu_!=curr_mu) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Mu has changed in line search - resetting watchdog counters.\n");
+ // Inactivate the watchdog and release all stored data
+ in_watchdog_ = false;
+ watchdog_iterate_ = NULL;
+ watchdog_delta_ = NULL;
+ watchdog_shortened_iter_ = 0;
+ last_mu_ = curr_mu;
+ }
+
+ // If the problem is square, we want to enable the
+ // expect_infeasible_problem option automatically so that the
+ // restoration phase is entered soon
+ if (IpCq().IsSquareProblem()) {
+ expect_infeasible_problem_ = true;
+ expect_infeasible_problem_ctol_ = 0.;
+ }
+
+ // Store current iterate if the optimality error is on acceptable
+ // level to restored if things fail later
+ if (CurrentIsAcceptable()) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Storing current iterate as backup acceptable point.\n");
+ StoreAcceptablePoint();
+ }
+
+ // First assume that line search will find an acceptable trial point
+ skipped_line_search_ = false;
+
+ // Get the search directions (this will store the actual search
+ // direction, possibly including higher order corrections)
+ SmartPtr<IteratesVector> actual_delta;
+
+ bool goto_resto = false;
+ if (fallback_activated_) {
+ // In this case, the algorithm had trouble to continue and wants
+ // to call the restoration phase immediately
+ goto_resto = true;
+ fallback_activated_ = false; // reset the flag
+ }
+ else {
+ // Initialize the acceptor for this backtracking line search
+ acceptor_->InitThisLineSearch(in_watchdog_);
+ actual_delta = IpData().delta()->MakeNewContainer();
+ }
+
+ if (start_with_resto_) {
+ // If the user requested to start with the restoration phase,
+ // skip the line search and do exactly that. Reset the flag so
+ // that this happens only once.
+ goto_resto = true;
+ start_with_resto_= false;
+ }
+
+ if (expect_infeasible_problem_ &&
+ Max(IpData().curr()->y_c()->Amax(),
+ IpData().curr()->y_d()->Amax()) > 1e8) {
+ goto_resto = true;
+ }
+
+ bool accept = false;
+ bool corr_taken = false;
+ bool soc_taken = false;
+ Index n_steps = 0;
+ Number alpha_primal = 0.;
+
+ // Check if search direction becomes too small
+ bool tiny_step = (!goto_resto && DetectTinyStep());
+
+ if (in_watchdog_ && (goto_resto || tiny_step)) {
+ // If the step could not be computed or is too small and the
+ // watchdog is active, stop the watch dog and resume everything
+ // from reference point
+ StopWatchDog(actual_delta);
+ goto_resto = false;
+ tiny_step = false;
+ }
+
+ // Check if we want to wake up the watchdog
+ if (watchdog_shortened_iter_trigger_ > 0 &&
+ !in_watchdog_ && !goto_resto && !tiny_step &&
+ !in_soft_resto_phase_ && !expect_infeasible_problem_ &&
+ watchdog_shortened_iter_ >= watchdog_shortened_iter_trigger_) {
+ StartWatchDog();
+ }
+
+ // Handle the situation of a tiny step
+ if (tiny_step) {
+ alpha_primal =
+ IpCq().curr_primal_frac_to_the_bound(IpData().curr_tau());
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Tiny step detected. Use step size alpha = %e unchecked\n",
+ alpha_primal);
+ IpData().SetTrialPrimalVariablesFromStep(alpha_primal, *IpData().delta()->x(), *IpData().delta()->s());
+
+ // Evaluate functions at trial point - if that fails, don't use
+ // the tiny step and continue with regular line search
+ try {
+ IpCq().trial_barrier_obj();
+ IpCq().trial_constraint_violation();
+ }
+ catch(IpoptNLP::Eval_Error& e) {
+ e.ReportException(Jnlst(), J_DETAILED);
+ tiny_step = false;
+ }
+
+ if (tiny_step) {
+ IpData().Set_info_ls_count(0);
+
+ if (tiny_step_last_iteration_) {
+ IpData().Set_info_alpha_primal_char('T');
+ IpData().Set_tiny_step_flag(true);
+ }
+ }
+ else {
+ IpData().Set_info_alpha_primal_char('t');
+ }
+
+ // If the step in the dual variables is also small, we remember
+ // that we just did a tiny step so that next time we might
+ // decide to quit
+ Number delta_y_norm = Max(IpData().curr()->y_c()->Amax(),
+ IpData().curr()->y_d()->Amax());
+ if (delta_y_norm < tiny_step_y_tol_) {
+ tiny_step_last_iteration_ = true;
+ }
+ else {
+ tiny_step_last_iteration_ = false;
+ }
+ accept = true;
+ }
+ else {
+ tiny_step_last_iteration_ = false;
+ }
+
+ if (!goto_resto && !tiny_step) {
+
+ if (in_soft_resto_phase_) {
+ soft_resto_counter_++;
+ if (soft_resto_counter_ > max_soft_resto_iters_) {
+ accept = false;
+ }
+ else {
+ // If we are currently in the soft restoration phase, continue
+ // that way, and switch back if enough progress is made to the
+ // original criterion (e.g., the filter)
+ bool satisfies_original_criterion = false;
+ // ToDo use tiny_step in TrySoftRestoStep?
+ accept = TrySoftRestoStep(actual_delta,
+ satisfies_original_criterion);
+ if (accept) {
+ IpData().Set_info_alpha_primal_char('s');
+ if (satisfies_original_criterion) {
+ in_soft_resto_phase_ = false;
+ soft_resto_counter_ = 0;
+ IpData().Set_info_alpha_primal_char('S');
+ }
+ }
+ }
+ }
+ else {
+ // Start the backtracking line search
+ bool done = false;
+ bool skip_first_trial_point = false;
+ bool evaluation_error;
+ while (!done) {
+ accept = DoBacktrackingLineSearch(skip_first_trial_point,
+ alpha_primal,
+ corr_taken,
+ soc_taken,
+ n_steps,
+ evaluation_error,
+ actual_delta);
+ DBG_PRINT((1, "evaluation_error = %d\n", evaluation_error));
+ if (in_watchdog_) {
+ if (accept) {
+ in_watchdog_ = false;
+ IpData().Append_info_string("W");
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Watch dog procedure successful!\n");
+ done = true;
+ }
+ else {
+ watchdog_trial_iter_++;
+ if (evaluation_error ||
+ watchdog_trial_iter_ > watchdog_trial_iter_max_) {
+ StopWatchDog(actual_delta);
+ skip_first_trial_point = true;
+ }
+ else {
+ done = true;
+ accept = true;
+ }
+ }
+ }
+ else {
+ done = true;
+ }
+ }
+ } /* else: if (in_soft_resto_phase_) { */
+ } /* if (!goto_resto && !tiny_step) { */
+
+ // If line search has been aborted because the step size becomes
+ // too small, go to the restoration phase or continue with soft
+ // restoration phase
+ if (!accept) {
+ // If we are not asked to do a rigorous line search, do no call
+ // the restoration phase.
+ if (!rigorous_) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH, "Skipping call of restoration phase...\n");
+ skipped_line_search_=true;
+ }
+ else {
+ // Check if we should start the soft restoration phase
+ if (!in_soft_resto_phase_
+ && !goto_resto && !expect_infeasible_problem_) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "--> Starting soft restoration phase <--\n");
+ // Prepare the restoration phase, e.g., augment the filter
+ // with the current point.
+ acceptor_->PrepareRestoPhaseStart();
+
+ // Try the current search direction for the soft restoration phase
+ bool satisfies_original_criterion;
+ accept = TrySoftRestoStep(actual_delta,
+ satisfies_original_criterion);
+ // If it has been accepted: If the original criterion is also
+ // satisfied, we can just take that step and continue with
+ // the regular algorithm, otherwise we stay in the soft
+ // restoration phase
+ if (accept) {
+ if (satisfies_original_criterion) {
+ IpData().Set_info_alpha_primal_char('S');
+ }
+ else {
+ in_soft_resto_phase_ = true;
+ IpData().Set_info_alpha_primal_char('s');
+ }
+ }
+ }
+
+ if (!accept) {
+ // Go to the restoration phase
+ if (!in_soft_resto_phase_) {
+ // Prepare the restoration phase, e.g., augment the filter
+ // with the current point. If we are already in the soft
+ // restoration phase, this has been done earlier
+ acceptor_->PrepareRestoPhaseStart();
+ }
+ if (CurrentIsAcceptable()) {
+ THROW_EXCEPTION(ACCEPTABLE_POINT_REACHED,
+ "Restoration phase called at acceptable point.");
+ }
+
+ if (!IsValid(resto_phase_)) {
+ THROW_EXCEPTION(IpoptException, "No Restoration Phase given to this Filter Line Search Object!");
+ }
+ // ToDo make the 1e-2 below a parameter?
+ if (IpCq().curr_constraint_violation()<=
+ 1e-2*IpData().tol()) {
+ bool found_acceptable = RestoreAcceptablePoint();
+ if (found_acceptable) {
+ Jnlst().Printf(J_WARNING, J_LINE_SEARCH,
+ "Restoration phase is called at almost feasible point,\n but acceptable point from iteration %d could be restored.\n", acceptable_iteration_number_);
+ THROW_EXCEPTION(ACCEPTABLE_POINT_REACHED,
+ "Restoration phase called at almost feasible point, but acceptable point could be restored.\n");
+ }
+ else {
+ // ToDo does that happen too often?
+ Jnlst().Printf(J_ERROR, J_LINE_SEARCH,
+ "Restoration phase is called at point that is almost feasible,\n with constraint violation %e. Abort.\n", IpCq().curr_constraint_violation());
+ THROW_EXCEPTION(RESTORATION_FAILED,
+ "Restoration phase called, but point is almost feasible.");
+ }
+ }
+
+ // Set the info fields for the first output line in the
+ // restoration phase which reflects why the restoration phase
+ // was called
+ IpData().Set_info_alpha_primal(alpha_primal);
+ IpData().Set_info_alpha_dual(0.);
+ IpData().Set_info_alpha_primal_char('R');
+ IpData().Set_info_ls_count(n_steps+1);
+
+ accept = resto_phase_->PerformRestoration();
+ if (!accept) {
+ bool found_acceptable = RestoreAcceptablePoint();
+ if (found_acceptable) {
+ THROW_EXCEPTION(ACCEPTABLE_POINT_REACHED,
+ "Restoration phase failed, but acceptable point could be restore.\n");
+ }
+ else {
+ THROW_EXCEPTION(RESTORATION_FAILED,
+ "Failed restoration phase!!!");
+ }
+ }
+ count_successive_shortened_steps_ = 0;
+ if (expect_infeasible_problem_) {
+ expect_infeasible_problem_ = false;
+ }
+ in_soft_resto_phase_ = false;
+ soft_resto_counter_ = 0;
+ watchdog_shortened_iter_ = 0;
+ }
+ }
+ }
+ else if (!in_soft_resto_phase_ || tiny_step) {
+ // we didn't do the restoration phase and are now updating the
+ // dual variables of the trial point
+ Number alpha_dual_max =
+ IpCq().dual_frac_to_the_bound(IpData().curr_tau(),
+ *actual_delta->z_L(), *actual_delta->z_U(),
+ *actual_delta->v_L(), *actual_delta->v_U());
+
+ PerformDualStep(alpha_primal, alpha_dual_max, actual_delta);
+
+ if (n_steps==0) {
+ // accepted this if a full step was
+ // taken
+ count_successive_shortened_steps_ = 0;
+ watchdog_shortened_iter_ = 0;
+ }
+ else {
+ count_successive_shortened_steps_++;
+ watchdog_shortened_iter_++;
+ }
+
+ if (expect_infeasible_problem_ &&
+ IpCq().curr_constraint_violation() <= expect_infeasible_problem_ctol_) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Constraint violation is with %e less than expect_infeasible_problem_ctol.\nDisable expect_infeasible_problem_heuristic.\n", IpCq().curr_constraint_violation());
+ expect_infeasible_problem_ = false;
+ }
+ }
+ }
+
+ bool BacktrackingLineSearch::DoBacktrackingLineSearch(bool skip_first_trial_point,
+ Number& alpha_primal,
+ bool& corr_taken,
+ bool& soc_taken,
+ Index& n_steps,
+ bool& evaluation_error,
+ SmartPtr<IteratesVector>& actual_delta)
+ {
+ evaluation_error = false;
+ bool accept = false;
+
+ DBG_START_METH("BacktrackingLineSearch::DoBacktrackingLineSearch",
+ dbg_verbosity);
+
+ // Compute primal fraction-to-the-boundary value
+ Number alpha_primal_max =
+ IpCq().primal_frac_to_the_bound(IpData().curr_tau(),
+ *actual_delta->x(),
+ *actual_delta->s());
+
+ // Compute smallest step size allowed
+ Number alpha_min = alpha_primal_max;
+ if (!in_watchdog_) {
+ alpha_min = acceptor_->CalculateAlphaMin();
+ }
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "minimal step size ALPHA_MIN = %E\n", alpha_min);
+
+ // Start line search from maximal step size
+ alpha_primal = alpha_primal_max;
+
+ // Step size used in ftype and armijo tests
+ Number alpha_primal_test = alpha_primal;
+ if (in_watchdog_) {
+ alpha_primal_test = watchdog_alpha_primal_test_;
+ }
+
+ if (skip_first_trial_point) {
+ alpha_primal *= alpha_red_factor_;
+ }
+
+ if (!skip_first_trial_point) {
+ // Before we do the actual backtracking line search for the
+ // regular primal-dual search direction, let's see if a step
+ // including a higher-order correctior is already acceptable
+ accept = acceptor_->TryCorrector(alpha_primal_test,
+ alpha_primal,
+ actual_delta);
+ }
+ if (accept) {
+ corr_taken = true;
+ }
+
+ if (!accept) {
+ // Loop over decreaseing step sizes until acceptable point is
+ // found or until step size becomes too small
+
+ while (alpha_primal>alpha_min ||
+ n_steps == 0) { // always allow the "full" step if it is
+ // acceptable (even if alpha_primal<=alpha_min)
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Starting checks for alpha (primal) = %8.2e\n",
+ alpha_primal);
+
+ try {
+ // Compute the primal trial point
+ IpData().SetTrialPrimalVariablesFromStep(alpha_primal, *actual_delta->x(), *actual_delta->s());
+
+ if (magic_steps_) {
+ PerformMagicStep();
+ }
+
+ // If it is acceptable, stop the search
+ alpha_primal_test = alpha_primal;
+ if (accept_every_trial_step_) {
+ // We call the evaluation at the trial point here, so that an
+ // exception will the thrown if there are problem during the
+ // evaluation of the functions (in that case, we want to further
+ // reduce the step size
+ IpCq().trial_barrier_obj();
+ IpCq().trial_constraint_violation();
+ accept = true;
+ }
+ else {
+ accept = acceptor_->CheckAcceptabilityOfTrialPoint(alpha_primal_test);
+ }
+ }
+ catch(IpoptNLP::Eval_Error& e) {
+ e.ReportException(Jnlst(), J_DETAILED);
+ Jnlst().Printf(J_WARNING, J_LINE_SEARCH,
+ "Warning: Cutting back alpha due to evaluation error\n");
+ IpData().Append_info_string("e");
+ accept = false;
+ evaluation_error = true;
+ }
+
+ if (accept) {
+ break;
+ }
+
+ if (in_watchdog_) {
+ break;
+ }
+
+ // Decide if we want to go to the restoration phase in a
+ // short cut to check if the problem is infeasible
+ if (expect_infeasible_problem_) {
+ if (count_successive_shortened_steps_>=5) {
+ break;
+ }
+ }
+
+ // try second order correction step if the function could
+ // be evaluated
+ // DoTo: check if we want to do SOC when watchdog is active
+ if (!evaluation_error) {
+ Number theta_curr = IpCq().curr_constraint_violation();
+ Number theta_trial = IpCq().trial_constraint_violation();
+ if (alpha_primal==alpha_primal_max && // i.e. first trial point
+ theta_curr<=theta_trial) {
+ // Try second order correction
+ accept = acceptor_->TrySecondOrderCorrection(alpha_primal_test,
+ alpha_primal,
+ actual_delta);
+ }
+ if (accept) {
+ soc_taken = true;
+ break;
+ }
+ }
+
+ // Point is not yet acceptable, try a shorter one
+ alpha_primal *= alpha_red_factor_;
+ n_steps++;
+ }
+ } /* if (!accept) */
+
+ char info_alpha_primal_char='?';
+ if (!accept && in_watchdog_) {
+ info_alpha_primal_char = 'w';
+ }
+ else if (accept) {
+ info_alpha_primal_char =
+ acceptor_->UpdateForNextIteration(alpha_primal_test);
+ }
+ if (soc_taken) {
+ info_alpha_primal_char = toupper(info_alpha_primal_char);
+ }
+ IpData().Set_info_alpha_primal_char(info_alpha_primal_char);
+ IpData().Set_info_ls_count(n_steps+1);
+ if (corr_taken) {
+ IpData().Append_info_string("C");
+ }
+
+ return accept;
+ }
+
+ void BacktrackingLineSearch::StartWatchDog()
+ {
+ DBG_START_FUN("BacktrackingLineSearch::StartWatchDog", dbg_verbosity);
+
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Starting Watch Dog\n");
+
+ in_watchdog_ = true;
+ watchdog_iterate_ = IpData().curr();
+ watchdog_delta_ = IpData().delta();
+ watchdog_trial_iter_ = 0;
+ watchdog_alpha_primal_test_ =
+ IpCq().curr_primal_frac_to_the_bound(IpData().curr_tau());
+
+ acceptor_->StartWatchDog();
+ }
+
+ void BacktrackingLineSearch::StopWatchDog(SmartPtr<IteratesVector>& actual_delta)
+ {
+ DBG_START_FUN("BacktrackingLineSearch::StopWatchDog", dbg_verbosity);
+
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Stopping Watch Dog\n");
+
+ IpData().Append_info_string("w");
+
+ in_watchdog_ = false;
+
+ // Reset all fields in IpData to reference point
+ SmartPtr<IteratesVector> old_trial = watchdog_iterate_->MakeNewContainer();
+ IpData().set_trial(old_trial);
+ IpData().AcceptTrialPoint();
+ actual_delta = watchdog_delta_->MakeNewContainer();
+ IpData().SetHaveAffineDeltas(false);
+
+ // reset the stored watchdog iterates
+ watchdog_iterate_ = NULL;
+ watchdog_delta_ = NULL;
+
+ watchdog_shortened_iter_ = 0;
+
+ acceptor_->StopWatchDog();
+ }
+
+ void BacktrackingLineSearch::Reset()
+ {
+ DBG_START_FUN("BacktrackingLineSearch::Reset", dbg_verbosity);
+ in_soft_resto_phase_ = false;
+ soft_resto_counter_ = 0;
+
+ acceptor_->Reset();
+ }
+
+ void BacktrackingLineSearch::PerformDualStep(Number alpha_primal,
+ Number alpha_dual,
+ SmartPtr<IteratesVector>& delta)
+ {
+ DBG_START_FUN("BacktrackingLineSearch::PerformDualStep", dbg_verbosity);
+
+ // set the bound multipliers from the step
+ IpData().SetTrialBoundMultipliersFromStep(alpha_dual, *delta->z_L(), *delta->z_U(), *delta->v_L(), *delta->v_U());
+
+ Number alpha_y=-1.;
+ switch (alpha_for_y_) {
+ case PRIMAL_ALPHA_FOR_Y:
+ alpha_y = alpha_primal;
+ break;
+ case DUAL_ALPHA_FOR_Y:
+ alpha_y = alpha_dual;
+ break;
+ case MIN_ALPHA_FOR_Y:
+ alpha_y = Min(alpha_dual, alpha_primal);
+ break;
+ case MAX_ALPHA_FOR_Y:
+ alpha_y = Max(alpha_dual, alpha_primal);
+ break;
+ case FULL_STEP_FOR_Y:
+ alpha_y = 1;
+ break;
+ case MIN_DUAL_INFEAS_ALPHA_FOR_Y:
+ case SAFE_MIN_DUAL_INFEAS_ALPHA_FOR_Y:
+ // Here we compute the step size for y so that the dual
+ // infeasibility is minimized along delta_y
+
+ // compute the dual infeasibility at new point with old y
+ SmartPtr<IteratesVector> temp_trial
+ = IpData().trial()->MakeNewContainer();
+ temp_trial->Set_y_c(*IpData().curr()->y_c());
+ temp_trial->Set_y_d(*IpData().curr()->y_d());
+ IpData().set_trial(temp_trial);
+ SmartPtr<const Vector> dual_inf_x = IpCq().trial_grad_lag_x();
+ SmartPtr<const Vector> dual_inf_s = IpCq().trial_grad_lag_s();
+
+ SmartPtr<Vector> new_jac_times_delta_y =
+ IpData().curr()->x()->MakeNew();
+ new_jac_times_delta_y->AddTwoVectors(1., *IpCq().trial_jac_cT_times_vec(*delta->y_c()),
+ 1., *IpCq().trial_jac_dT_times_vec(*delta->y_d()),
+ 0.);
+
+ Number a = std::pow(new_jac_times_delta_y->Nrm2(), Number(2)) +
+ std::pow(delta->y_d()->Nrm2(), Number(2));
+ Number b = dual_inf_x->Dot(*new_jac_times_delta_y) -
+ dual_inf_s->Dot(*delta->y_d());
+
+ Number alpha = - b/a;
+
+ if (alpha_for_y_==SAFE_MIN_DUAL_INFEAS_ALPHA_FOR_Y) {
+ alpha_y = Min(Max(alpha_primal, alpha_dual), Max(alpha, Min(alpha_primal, alpha_dual)));
+ }
+ else {
+ alpha_y = Min(1., Max(0., alpha));
+ }
+ break;
+ }
+
+ // Set the eq multipliers from the step now that alpha_y
+ // has been calculated.
+ DBG_PRINT((1, "alpha_y = %e\n", alpha_y));
+ DBG_PRINT_VECTOR(2, "delta_y_c", *delta->y_c());
+ DBG_PRINT_VECTOR(2, "delta_y_d", *delta->y_d());
+ IpData().SetTrialEqMultipliersFromStep(alpha_y, *delta->y_c(), *delta->y_d());
+
+ // Set some information for iteration summary output
+ IpData().Set_info_alpha_primal(alpha_primal);
+ IpData().Set_info_alpha_dual(alpha_dual);
+ }
+
+ void
+ BacktrackingLineSearch::PerformMagicStep()
+ {
+ DBG_START_METH("BacktrackingLineSearch::PerformMagicStep",
+ 2);//dbg_verbosity);
+
+ DBG_PRINT((1,"Incoming barr = %e and constrviol %e\n",
+ IpCq().trial_barrier_obj(),
+ IpCq().trial_constraint_violation()));
+ DBG_PRINT_VECTOR(2, "s in", *IpData().trial()->s());
+ DBG_PRINT_VECTOR(2, "d minus s in", *IpCq().trial_d_minus_s());
+ DBG_PRINT_VECTOR(2, "slack_s_L in", *IpCq().trial_slack_s_L());
+ DBG_PRINT_VECTOR(2, "slack_s_U in", *IpCq().trial_slack_s_U());
+
+ SmartPtr<const Vector> d_L = IpNLP().d_L();
+ SmartPtr<const Matrix> Pd_L = IpNLP().Pd_L();
+ SmartPtr<Vector> delta_s_magic_L = d_L->MakeNew();
+ delta_s_magic_L->Set(0.);
+ SmartPtr<Vector> tmp = d_L->MakeNew();
+ Pd_L->TransMultVector(1., *IpCq().trial_d_minus_s(), 0., *tmp);
+ delta_s_magic_L->ElementWiseMax(*tmp);
+
+ SmartPtr<const Vector> d_U = IpNLP().d_U();
+ SmartPtr<const Matrix> Pd_U = IpNLP().Pd_U();
+ SmartPtr<Vector> delta_s_magic_U = d_U->MakeNew();
+ delta_s_magic_U->Set(0.);
+ tmp = d_U->MakeNew();
+ Pd_U->TransMultVector(1., *IpCq().trial_d_minus_s(), 0., *tmp);
+ delta_s_magic_U->ElementWiseMin(*tmp);
+
+ SmartPtr<Vector> delta_s_magic = IpData().trial()->s()->MakeNew();
+ Pd_L->MultVector(1., *delta_s_magic_L, 0., *delta_s_magic);
+ Pd_U->MultVector(1., *delta_s_magic_U, 1., *delta_s_magic);
+ delta_s_magic_L = NULL; // free memory
+ delta_s_magic_U = NULL; // free memory
+
+ // Now find those entries with both lower and upper bounds, there
+ // the step is too large
+ // ToDo this should only be done if there are inequality
+ // constraints with two bounds
+ // also this can be done in a smaller space (d_L or d_U whichever
+ // is smaller)
+ tmp = delta_s_magic->MakeNew();
+ tmp->Copy(*IpData().trial()->s());
+ Pd_L->MultVector(1., *d_L, -2., *tmp);
+ Pd_U->MultVector(1., *d_U, 1., *tmp);
+ SmartPtr<Vector> tmp2 = tmp->MakeNew();
+ tmp2->Copy(*tmp);
+ tmp2->ElementWiseAbs();
+ tmp->Axpy(-2., *delta_s_magic);
+ tmp->ElementWiseAbs();
+ // now, tmp2 = |d_L + d_u - 2*s| and tmp = |d_L + d_u - 2*(s+Delta s)|
+ // we want to throw out those for which tmp2 > tmp
+ tmp->Axpy(-1., *tmp2);
+ tmp->ElementWiseSgn();
+ tmp2->Set(0.);
+ tmp2->ElementWiseMax(*tmp);
+ tmp = d_L->MakeNew();
+ Pd_L->TransMultVector(1., *tmp2, 0., *tmp);
+ Pd_L->MultVector(1., *tmp, 0., *tmp2);
+ tmp = d_U->MakeNew();
+ Pd_U->TransMultVector(1., *tmp2, 0., *tmp);
+ Pd_U->MultVector(1., *tmp, 0., *tmp2);
+ DBG_PRINT_VECTOR(2, "tmp indicator", *tmp2)
+ // tmp2 now is one for those entries with both bounds, for which
+ // no step should be taken
+
+ tmp = delta_s_magic->MakeNew();
+ tmp->Copy(*delta_s_magic);
+ tmp->ElementWiseMultiply(*tmp2);
+ delta_s_magic->Axpy(-1., *tmp);
+
+ Number delta_s_magic_max = delta_s_magic->Amax();
+ Number mach_eps = std::numeric_limits<Number>::epsilon();
+ if (delta_s_magic_max>0.) {
+ if (delta_s_magic_max > 10*mach_eps*IpData().trial()->s()->Amax()) {
+ IpData().Append_info_string("M");
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH, "Magic step with max-norm %.6e taken.\n", delta_s_magic->Amax());
+ delta_s_magic->Print(Jnlst(), J_MOREVECTOR, J_LINE_SEARCH,
+ "delta_s_magic");
+ }
+
+ // now finally compute the new overall slacks
+ delta_s_magic->Axpy(1., *IpData().trial()->s());
+ SmartPtr<IteratesVector> trial = IpData().trial()->MakeNewContainer();
+ trial->Set_s(*delta_s_magic);
+
+ // also update the set in the dual variables
+
+
+ IpData().set_trial(trial);
+ }
+
+ DBG_PRINT((1,"Outgoing barr = %e and constrviol %e\n", IpCq().trial_barrier_obj(), IpCq().trial_constraint_violation()));
+ DBG_PRINT_VECTOR(2, "s out", *IpData().trial()->s());
+ DBG_PRINT_VECTOR(2, "d minus s out", *IpCq().trial_d_minus_s());
+ DBG_PRINT_VECTOR(2, "slack_s_L out", *IpCq().trial_slack_s_L());
+ DBG_PRINT_VECTOR(2, "slack_s_U out", *IpCq().trial_slack_s_U());
+ }
+
+ bool BacktrackingLineSearch::TrySoftRestoStep(SmartPtr<IteratesVector>& actual_delta,
+ bool &satisfies_original_criterion)
+ {
+ DBG_START_FUN("FilterLSAcceptor::TrySoftRestoStep", dbg_verbosity);
+
+ if (soft_resto_pderror_reduction_factor_==0.) {
+ return false;
+ }
+
+ satisfies_original_criterion = false;
+
+ // ToDo: Need to decide if we want to try a corrector step first
+
+ // Compute the maximal step sizes (we use identical step sizes for
+ // primal and dual variables
+ Number alpha_primal_max =
+ IpCq().primal_frac_to_the_bound(IpData().curr_tau(),
+ *actual_delta->x(),
+ *actual_delta->s());
+ Number alpha_dual_max =
+ IpCq().dual_frac_to_the_bound(IpData().curr_tau(),
+ *actual_delta->z_L(),
+ *actual_delta->z_U(),
+ *actual_delta->v_L(),
+ *actual_delta->v_U());
+ Number alpha = Min(alpha_primal_max, alpha_dual_max);
+
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Trying soft restoration phase step with step length %13.6e\n",
+ alpha);
+
+ // We allow up to three trials in case there is an evaluation
+ // error for the functions
+ bool done=false;
+ Index count=3;
+ while (!done && count>0) {
+ // Set the trial point
+ IpData().SetTrialPrimalVariablesFromStep(alpha, *actual_delta->x(), *actual_delta->s());
+ PerformDualStep(alpha, alpha, actual_delta);
+
+ // Check if that point is acceptable with respect to the current
+ // original filter
+ try {
+ IpCq().trial_barrier_obj();
+ IpCq().trial_constraint_violation();
+ done=true;
+ }
+ catch(IpoptNLP::Eval_Error& e) {
+ e.ReportException(Jnlst(), J_DETAILED);
+ Jnlst().Printf(J_WARNING, J_LINE_SEARCH,
+ "Warning: Evaluation error during soft restoration phase step.\n");
+ IpData().Append_info_string("e");
+ count--;
+ }
+ }
+ if (!done) {
+ return false;
+ }
+
+ if (acceptor_->CheckAcceptabilityOfTrialPoint(0.)) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ " Trial step acceptable with respect to original backtracking globalization.\n");
+ satisfies_original_criterion = true;
+ return true;
+ }
+
+ // Evaluate the optimality error at the new point
+ Number mu = .0;
+ if (!IpData().FreeMuMode()) {
+ mu = IpData().curr_mu();
+ }
+ Number trial_pderror;
+ Number curr_pderror;
+ try {
+ trial_pderror = IpCq().trial_primal_dual_system_error(mu);
+ curr_pderror = IpCq().curr_primal_dual_system_error(mu);
+ }
+ catch(IpoptNLP::Eval_Error& e) {
+ e.ReportException(Jnlst(), J_DETAILED);
+ Jnlst().Printf(J_WARNING, J_LINE_SEARCH,
+ "Warning: Evaluation error during soft restoration phase step.\n");
+ IpData().Append_info_string("e");
+ return false;
+ }
+
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ " Primal-dual error at current point: %23.16e\n", curr_pderror);
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ " Primal-dual error at trial point : %23.16e\n", trial_pderror);
+ // Check if there is sufficient reduction in the optimality error
+ if (trial_pderror <= soft_resto_pderror_reduction_factor_*curr_pderror) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ " Trial step accepted.\n");
+ return true;
+ }
+
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ " Trial step rejected.\n");
+ return false;
+ }
+
+ bool
+ BacktrackingLineSearch::DetectTinyStep()
+ {
+ DBG_START_METH("BacktrackingLineSearch::DetectTinyStep",
+ dbg_verbosity);
+
+ Number max_step_x;
+ Number max_step_s;
+
+ if (tiny_step_tol_==0.)
+ return false;
+
+ // ToDo try to find more efficient implementation
+ DBG_PRINT_VECTOR(2, "curr_x", *IpData().curr()->x());
+ DBG_PRINT_VECTOR(2, "delta_x", *IpData().delta()->x());
+
+ SmartPtr<Vector> tmp = IpData().curr()->x()->MakeNewCopy();
+ tmp->ElementWiseAbs();
+ tmp->AddScalar(1.);
+
+ SmartPtr<Vector> tmp2 = IpData().delta()->x()->MakeNewCopy();
+ tmp2->ElementWiseDivide(*tmp);
+ max_step_x = tmp2->Amax();
+ Jnlst().Printf(J_MOREDETAILED, J_LINE_SEARCH,
+ "Relative step size for delta_x = %e\n",
+ max_step_x);
+ if (max_step_x > tiny_step_tol_)
+ return false;
+
+ tmp = IpData().curr()->s()->MakeNew();
+ tmp->Copy(*IpData().curr()->s());
+ tmp->ElementWiseAbs();
+ tmp->AddScalar(1.);
+
+ tmp2 = IpData().curr()->s()->MakeNew();
+ tmp2->Copy(*IpData().delta()->s());
+ tmp2->ElementWiseDivide(*tmp);
+ max_step_s = tmp2->Amax();
+ Jnlst().Printf(J_MOREDETAILED, J_LINE_SEARCH,
+ "Relative step size for delta_s = %e\n",
+ max_step_s);
+ if (max_step_s > tiny_step_tol_)
+ return false;
+
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Tiny step of relative size %e detected.\n",
+ Max(max_step_x, max_step_s));
+
+ return true;
+ }
+
+ bool BacktrackingLineSearch::CurrentIsAcceptable()
+ {
+ return (IsValid(conv_check_) &&
+ conv_check_->CurrentIsAcceptable());
+ }
+
+ void BacktrackingLineSearch::StoreAcceptablePoint()
+ {
+ DBG_START_METH("BacktrackingLineSearch::StoreAcceptablePoint",
+ dbg_verbosity);
+
+ acceptable_iterate_ = IpData().curr();
+ acceptable_iteration_number_ = IpData().iter_count();
+ }
+
+ bool BacktrackingLineSearch::RestoreAcceptablePoint()
+ {
+ DBG_START_METH("BacktrackingLineSearch::RestoreAcceptablePoint",
+ dbg_verbosity);
+
+ if (!IsValid(acceptable_iterate_)) {
+ return false;
+ }
+
+ SmartPtr<IteratesVector> prev_iterate = acceptable_iterate_->MakeNewContainer();
+ IpData().set_trial(prev_iterate);
+ IpData().AcceptTrialPoint();
+
+ return true;
+ }
+
+ bool BacktrackingLineSearch::ActivateFallbackMechanism()
+ {
+ // If we don't have a restoration phase, we don't know what to do
+ if (IsNull(resto_phase_)) {
+ return false;
+ }
+
+ // Reverting to the restoration phase only makes sense if there
+ // are constraints
+ if (IpData().curr()->y_c()->Dim()+IpData().curr()->y_d()->Dim()==0) {
+ return false;
+ }
+
+ fallback_activated_ = true;
+ rigorous_ = true;
+
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Fallback option activated in BacktrackingLineSearch!\n");
+
+ return true;
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpBacktrackingLineSearch.hpp b/SimTKmath/Optimizers/src/IpOpt/IpBacktrackingLineSearch.hpp
new file mode 100644
index 0000000..79bfdd1
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpBacktrackingLineSearch.hpp
@@ -0,0 +1,361 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpBacktrackingLineSearch.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+// Andreas Waechter IBM 2005-10-13
+// derived file from IpFilterLineSearch.hpp
+
+#ifndef __IPBACKTRACKINGLINESEARCH_HPP__
+#define __IPBACKTRACKINGLINESEARCH_HPP__
+
+#include "IpLineSearch.hpp"
+#include "IpBacktrackingLSAcceptor.hpp"
+#include "IpRestoPhase.hpp"
+#include "IpConvCheck.hpp"
+
+namespace Ipopt
+{
+
+ /** General implementation of a backtracking line search. This
+ * class can be used to perform the filter line search procedure or
+ * other procedures. The BacktrackingLSAcceptor is used to
+ * determine whether trial points are acceptable (e.g., based on a
+ * filter or other methods).
+ *
+ * This backtracking line search knows of a restoration phase
+ * (which is called when the trial step size becomes too small or
+ * no search direction could be computed). It also has the notion
+ * of a "soft restoration phase," which uses the regular steps but
+ * decides on the acceptability based on other measures than the
+ * regular ones (e.g., reduction of the PD error instead of
+ * acceptability to a filter mechanism).
+ */
+ class BacktrackingLineSearch : public LineSearch
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor. The acceptor implements the acceptance test for
+ * the line search. The PDSystemSolver object only needs to be
+ * provided (i.e. not NULL) if second order correction is to be
+ * used. The ConvergenceCheck object is used to determine
+ * whether the current iterate is acceptable (for example, the
+ * restoration phase is not started if the acceptability level
+ * has been reached). If conv_check is NULL, we assume that the
+ * current iterate is not acceptable (in the sense of the
+ * acceptable_tol option). */
+ BacktrackingLineSearch(const SmartPtr<BacktrackingLSAcceptor>& acceptor,
+ const SmartPtr<RestorationPhase>& resto_phase,
+ const SmartPtr<ConvergenceCheck>& conv_check
+ );
+
+ /** Default destructor */
+ virtual ~BacktrackingLineSearch();
+ //@}
+
+ /** InitializeImpl - overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Perform the line search. It is assumed that the search
+ * direction is computed in the data object.
+ */
+ virtual void FindAcceptableTrialPoint();
+
+ /** Reset the line search.
+ * This function should be called if all previous information
+ * should be discarded when the line search is performed the
+ * next time. For example, this method should be called if
+ * the barrier parameter is changed.
+ */
+ virtual void Reset();
+
+ /** Set flag indicating whether a very rigorous line search should
+ * be performed. If this flag is set to true, the line search
+ * algorithm might decide to abort the line search and not to
+ * accept a new iterate. If the line search decided not to
+ * accept a new iterate, the return value of
+ * CheckSkippedLineSearch() is true at the next call. For
+ * example, in the non-monotone barrier parameter update
+ * procedure, the filter algorithm should not switch to the
+ * restoration phase in the free mode; instead, the algorithm
+ * should swtich to the fixed mode.
+ */
+ virtual void SetRigorousLineSearch(bool rigorous)
+ {
+ rigorous_ = rigorous;
+ }
+
+ /** Check if the line search procedure didn't accept a new iterate
+ * during the last call of FindAcceptableTrialPoint().
+ *
+ */
+ virtual bool CheckSkippedLineSearch()
+ {
+ return skipped_line_search_;
+ }
+
+ /** Activate fallback mechanism. Return false, if that is not
+ * possible. */
+ virtual bool ActivateFallbackMechanism();
+
+ /** Methods for OptionsList */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ BacktrackingLineSearch(const BacktrackingLineSearch&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const BacktrackingLineSearch&);
+ //@}
+
+ /** Method performing the backtracking line search. The return
+ * value indicates if the step acceptance criteria are met. If
+ * the watchdog is active, only one trial step is performed (and
+ * the trial values are set accordingly). */
+ bool DoBacktrackingLineSearch(bool skip_first_trial_point,
+ Number& alpha_primal,
+ bool& corr_taken,
+ bool& soc_taken,
+ Index& n_steps,
+ bool& evaluation_error,
+ SmartPtr<IteratesVector>& actual_delta);
+
+ /** Method for starting the watch dog. Set all appropriate fields
+ * accordingly */
+ void StartWatchDog();
+
+ /** Method for stopping the watch dog. Set all appropriate fields
+ * accordingly. */
+ void StopWatchDog(SmartPtr<IteratesVector>& actual_delta);
+
+ /** Method for checking if current trial point is acceptable.
+ * It is assumed that the delta information in ip_data is the
+ * search direction used in criteria. The primal trial point has
+ * to be set before the call.
+ */
+ bool CheckAcceptabilityOfTrialPoint(Number alpha_primal);
+
+ /** Check comparison "lhs <= rhs", using machine precision based on BasVal */
+ //ToDo This should probably not be a static member function if we want to
+ // allow for different relaxation parameters values
+ static bool Compare_le(Number lhs, Number rhs, Number BasVal);
+
+ /** Method for setting the dual variables in the trial fields in
+ * IpData, given the search direction. The step size for the
+ * bound multipliers is alpha_dual (the fraction-to-the-boundary
+ * step size), and the step size for the equality constraint
+ * multipliers depends on the choice of alpha_for_y. */
+ void PerformDualStep(Number alpha_primal,
+ Number alpha_dual,
+ SmartPtr<IteratesVector>& delta);
+
+ /** Try a step for the soft restoration phase and check if it is
+ * acceptable. The step size is identical for all variables. A
+ * point is accepted if it is acceptable for the original
+ * acceptability criterion (in which case
+ * satisfies_original_criterion = true on return), or if the
+ * primal-dual system error was decrease by at least the factor
+ * soft_resto_pderror_reduction_factor_. The return value is
+ * true, if the trial point was acceptable for the soft
+ * restoration phase. */
+ bool TrySoftRestoStep(SmartPtr<IteratesVector>& actual_delta,
+ bool &satisfies_original_criterion);
+
+ /** Try a second order correction for the constraints. If the
+ * first trial step (with incoming alpha_primal) has been reject,
+ * this tries up to max_soc_ second order corrections for the
+ * constraints. Here, alpha_primal_test is the step size that
+ * has to be used in the filter acceptance tests. On output
+ * actual_delta_... has been set to the steps including the
+ * second order correction if it has been accepted, otherwise it
+ * is unchanged. If the SOC step has been accepted, alpha_primal
+ * has the fraction-to-the-boundary value for the SOC step on output.
+ * The return value is true, if an SOC step has been accepted.
+ */
+ bool TrySecondOrderCorrection(Number alpha_primal_test,
+ Number& alpha_primal,
+ SmartPtr<IteratesVector>& actual_delta);
+
+ /** Try higher order corrector (for fast local convergence). In
+ * contrast to a second order correction step, which tries to
+ * make an unacceptable point acceptable by improving constraint
+ * violation, this corrector step is tried even if the regular
+ * primal-dual step is acceptable.
+ */
+ bool TryCorrector(Number alpha_primal_test,
+ Number& alpha_primal,
+ SmartPtr<IteratesVector>& actual_delta);
+
+ /** Perform magic steps. Take the current values of the slacks in
+ * trial and replace them by better ones that lead to smaller
+ * values of the barrier function and less constraint
+ * violation. */
+ void PerformMagicStep();
+
+ /** Detect if the search direction is too small. This should be
+ * true if the search direction is so small that if makes
+ * numerically no difference. */
+ bool DetectTinyStep();
+
+ /** Store current iterate as acceptable point */
+ void StoreAcceptablePoint();
+
+ /** Restore acceptable point into the current fields of IpData if
+ * found. Returns true if such as point is available. */
+ bool RestoreAcceptablePoint();
+
+ /** Method for determining if the current iterate is acceptable
+ * (in the sense of the acceptable_tol options). This is a
+ * wrapper for same method from ConvergenceCheck, but returns
+ * false, if no ConvergenceCheck object is provided. */
+ bool CurrentIsAcceptable();
+
+ /** @name Parameters for the filter algorithm. Names as in the paper */
+ //@{
+ /** factor by which search direction is to be shortened if trial
+ * point is rejected. */
+ Number alpha_red_factor_;
+
+ /** enumeration for the different alpha_for_y_ settings */
+ enum AlphaForYEnum {
+ PRIMAL_ALPHA_FOR_Y=0,
+ DUAL_ALPHA_FOR_Y,
+ MIN_ALPHA_FOR_Y,
+ MAX_ALPHA_FOR_Y,
+ FULL_STEP_FOR_Y,
+ MIN_DUAL_INFEAS_ALPHA_FOR_Y,
+ SAFE_MIN_DUAL_INFEAS_ALPHA_FOR_Y
+ };
+ /** Flag indicating whether the dual step size is to be used for
+ * the equality constraint multipliers. If 0, the primal step
+ * size is used, if 1 the dual step size, and if 2, the minimum
+ * of both. */
+ AlphaForYEnum alpha_for_y_;
+
+ /** Reduction factor for the restoration phase that accepts steps
+ * reducing the optimality error ("soft restoration phase"). If
+ * 0., then this restoration phase is not enabled. */
+ Number soft_resto_pderror_reduction_factor_;
+ /** Maximal number of iterations that can be done in the soft
+ * iteration phase before the algorithm reverts to the regular
+ * restoration phase. */
+ Index max_soft_resto_iters_;
+
+ /** Flag indicating whether magic steps should be used. */
+ bool magic_steps_;
+ /** Flag indicating whether the line search should always accept
+ * the full (fraction-to-the-boundary) step. */
+ bool accept_every_trial_step_;
+ /** Indicates whether problem can be expected to be infeasible.
+ * This will trigger requesting a tighter reduction in
+ * infeasibility the first time the restoration phase is
+ * called. */
+ bool expect_infeasible_problem_;
+ /** Tolerance on constraint violation for
+ * expect_infeasible_problem heuristic. If the constraint
+ * violation becomes that than this value, the heuristic is
+ * disabled for the rest of the optimization run. */
+ Number expect_infeasible_problem_ctol_;
+
+ /** Tolerance for detecting tiny steps. */
+ Number tiny_step_tol_;
+
+ /** Tolerance for y variables for the tiny step stopping
+ * heuristic. If repeatedly a tiny step is detected and the step
+ * in the y_c and y_d variables is less than this threshold, we
+ * algorithm will stop. */
+ Number tiny_step_y_tol_;
+
+ /** Number of watch dog trial steps. */
+ Index watchdog_trial_iter_max_;
+ /** Number of shortened iterations that trigger the watchdog. */
+ Index watchdog_shortened_iter_trigger_;
+
+ /** Indicates whether the algorithm should start directly with the
+ * restoratin phase */
+ bool start_with_resto_;
+ //@}
+
+ /** @name Information related to watchdog procedure */
+ //@{
+ /** Flag indicating if the watchdog is active */
+ bool in_watchdog_;
+ /** Counter for shortened iterations. */
+ Index watchdog_shortened_iter_;
+ /** Counter for watch dog iterations */
+ Index watchdog_trial_iter_;
+ /** Step size for Armijo test in watch dog */
+ Number watchdog_alpha_primal_test_;
+ /** Watchdog reference iterate */
+ SmartPtr<const IteratesVector> watchdog_iterate_;
+ /** Watchdog search direction at reference point */
+ SmartPtr<const IteratesVector> watchdog_delta_;
+ /** Barrier parameter value during last line search */
+ Number last_mu_;
+ //@}
+
+ /** @name Storage for last iterate that satisfies the acceptable
+ * level of optimality error. */
+ //@{
+ SmartPtr<const IteratesVector> acceptable_iterate_;
+ Index acceptable_iteration_number_;
+ //@}
+
+ /** Flag indicating whether the algorithm has asked to immediately
+ * switch to the fallback mechanism (restoration phase) */
+ bool fallback_activated_;
+
+ /** Flag indicating whether the line search is to be performed
+ * robust (usually this is true, unless SetRigorousLineSearch is
+ * called with false).
+ */
+ bool rigorous_;
+
+ /** Flag indicating whether no acceptable trial point was found
+ * during last line search. */
+ bool skipped_line_search_;
+
+ /** Flag indicating whether we are currently in the "soft"
+ * restoration phase mode, in which steps are accepted if they
+ * reduce the optimality error (see
+ * soft_resto_pderror_reduction_factor) */
+ bool in_soft_resto_phase_;
+
+ /** Counter for iteration performed in soft restoration phase in a
+ * row */
+ Index soft_resto_counter_;
+
+ /** Counter for the number of successive iterations in which the
+ * full step was not accepted. */
+ Index count_successive_shortened_steps_;
+
+ /** Flag indicating if a tiny step was detected in previous
+ * iteration */
+ bool tiny_step_last_iteration_;
+
+ /** @name Strategy objective that are used */
+ //@{
+ SmartPtr<BacktrackingLSAcceptor> acceptor_;
+ SmartPtr<RestorationPhase> resto_phase_;
+ SmartPtr<ConvergenceCheck> conv_check_;
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpBlas.cpp b/SimTKmath/Optimizers/src/IpOpt/IpBlas.cpp
new file mode 100644
index 0000000..e80a296
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpBlas.cpp
@@ -0,0 +1,334 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpBlas.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpoptConfig.h"
+#include "IpBlas.hpp"
+
+#if SimTK_DEFAULT_PRECISION==1 // float
+#define DCOPY scopy_
+#define DSCAL sscal_
+#define DAXPY saxpy_
+#define DDOT sdot_
+#define DNRM2 snrm2_
+#define DASUM sasum_
+#define DGEMV sgemv_
+#define DSYMV ssymv_
+#define DGEMM sgemm_
+#define DSYRK ssyrk_
+#define DTRSM strsm_
+#define IDAMAX isamax_
+#else // double
+#define DCOPY dcopy_
+#define DSCAL dscal_
+#define DAXPY daxpy_
+#define DDOT ddot_
+#define DNRM2 dnrm2_
+#define DASUM dasum_
+#define DGEMV dgemv_
+#define DSYMV dsymv_
+#define DGEMM dgemm_
+#define DSYRK dsyrk_
+#define DTRSM dtrsm_
+#define IDAMAX idamax_
+#endif
+
+// Prototypes for the BLAS routines
+extern "C"
+{
+ /** BLAS Fortran function DDOT */
+ double ddot_(ipfint *n, const double *x, ipfint *incX,
+ const double *y, ipfint *incY);
+ /** BLAS Fortran function DNRM2 */
+ double dnrm2_(ipfint *n, const double *x, ipfint *incX);
+ /** BLAS Fortran function DASUM */
+ double dasum_(ipfint *n, const double *x, ipfint *incX);
+ /** BLAS Fortran function IDAMAX */
+ ipfint idamax_(ipfint *n, const double *x, ipfint *incX);
+ /** BLAS Fortran subroutine DCOPY */
+ void dcopy_(ipfint *n, const double *x, ipfint *incX,
+ double *y, ipfint *incY);
+ /** BLAS Fortran subroutine DAXPY */
+ void daxpy_(ipfint *n, const double *alpha, const double *x,
+ ipfint *incX, double *y, ipfint *incY);
+ /** BLAS Fortran subroutine DSCAL */
+ void dscal_(ipfint *n, const double *alpha, const double *x,
+ ipfint *incX);
+
+ void dgemv_(char* trans, ipfint *m, ipfint *n,
+ const double *alpha, const double *a, ipfint *lda,
+ const double *x, ipfint *incX, const double *beta,
+ double *y, ipfint *incY, int trans_len);
+ void dsymv_(char* uplo, ipfint *n,
+ const double *alpha, const double *a, ipfint *lda,
+ const double *x, ipfint *incX, const double *beta,
+ double *y, ipfint *incY, int uplo_len);
+ void dgemm_(char* transa, char* transb,
+ ipfint *m, ipfint *n, ipfint *k,
+ const double *alpha, const double *a, ipfint *lda,
+ const double *b, ipfint *ldb, const double *beta,
+ double *c, ipfint *ldc,
+ int transa_len, int transb_len);
+ void dsyrk_(char* uplo, char* trans, ipfint *n, ipfint *k,
+ const double *alpha, const double *a, ipfint *lda,
+ const double *beta, double *c, ipfint *ldc,
+ int uplo_len, int trans_len);
+ void dtrsm_(char* side, char* uplo, char* transa, char* diag,
+ ipfint *m, ipfint *n,
+ const double *alpha, const double *a, ipfint *lda,
+ const double *b, ipfint *ldb,
+ int side_len, int uplo_len,
+ int transa_len, int diag_len);
+
+ /** BLAS Fortran function DDOT */
+ float sdot_(ipfint *n, const float *x, ipfint *incX,
+ const float *y, ipfint *incY);
+ /** BLAS Fortran function DNRM2 */
+ float snrm2_(ipfint *n, const float *x, ipfint *incX);
+ /** BLAS Fortran function DASUM */
+ float sasum_(ipfint *n, const float *x, ipfint *incX);
+ /** BLAS Fortran function IDAMAX */
+ ipfint isamax_(ipfint *n, const float *x, ipfint *incX);
+ /** BLAS Fortran subroutine DCOPY */
+ void scopy_(ipfint *n, const float *x, ipfint *incX,
+ float *y, ipfint *incY);
+ /** BLAS Fortran subroutine DAXPY */
+ void saxpy_(ipfint *n, const float *alpha, const float *x,
+ ipfint *incX, float *y, ipfint *incY);
+ /** BLAS Fortran subroutine DSCAL */
+ void sscal_(ipfint *n, const float *alpha, const float *x,
+ ipfint *incX);
+
+ void sgemv_(char* trans, ipfint *m, ipfint *n,
+ const float *alpha, const float *a, ipfint *lda,
+ const float *x, ipfint *incX, const float *beta,
+ float *y, ipfint *incY, int trans_len);
+ void ssymv_(char* uplo, ipfint *n,
+ const float *alpha, const float *a, ipfint *lda,
+ const float *x, ipfint *incX, const float *beta,
+ float *y, ipfint *incY, int uplo_len);
+ void sgemm_(char* transa, char* transb,
+ ipfint *m, ipfint *n, ipfint *k,
+ const float *alpha, const float *a, ipfint *lda,
+ const float *b, ipfint *ldb, const float *beta,
+ float *c, ipfint *ldc,
+ int transa_len, int transb_len);
+ void ssyrk_(char* uplo, char* trans, ipfint *n, ipfint *k,
+ const float *alpha, const float *a, ipfint *lda,
+ const float *beta, float *c, ipfint *ldc,
+ int uplo_len, int trans_len);
+ void strsm_(char* side, char* uplo, char* transa, char* diag,
+ ipfint *m, ipfint *n,
+ const float *alpha, const float *a, ipfint *lda,
+ const float *b, ipfint *ldb,
+ int side_len, int uplo_len,
+ int transa_len, int diag_len);
+}
+
+namespace Ipopt
+{
+#ifndef HAVE_CBLAS
+ /* Interface to FORTRAN routine DDOT. */
+ Number IpBlasDdot(Index size, const Number *x, Index incX, const Number *y,
+ Index incY)
+ {
+ ipfint n=size, INCX=incX, INCY=incY;
+
+ return DDOT(&n, x, &INCX, y, &INCY);
+ }
+
+ /* Interface to FORTRAN routine DNRM2. */
+ Number IpBlasDnrm2(Index size, const Number *x, Index incX)
+ {
+ ipfint n=size, INCX=incX;
+
+ return DNRM2(&n, x, &INCX);
+ }
+
+ /* Interface to FORTRAN routine DASUM. */
+ Number IpBlasDasum(Index size, const Number *x, Index incX)
+ {
+ ipfint n=size, INCX=incX;
+
+ return DASUM(&n, x, &INCX);
+ }
+
+ /* Interface to FORTRAN routine DASUM. */
+ Index IpBlasIdamax(Index size, const Number *x, Index incX)
+ {
+ ipfint n=size, INCX=incX;
+
+ return (Index) IDAMAX(&n, x, &INCX);
+ }
+
+ /* Interface to FORTRAN routine DCOPY. */
+ void IpBlasDcopy(Index size, const Number *x, Index incX, Number *y, Index incY)
+ {
+ ipfint N=size, INCX=incX, INCY=incY;
+
+ DCOPY(&N, x, &INCX, y, &INCY);
+ }
+
+ /* Interface to FORTRAN routine DAXPY. */
+ void IpBlasDaxpy(Index size, Number alpha, const Number *x, Index incX, Number *y,
+ Index incY)
+ {
+ ipfint N=size, INCX=incX, INCY=incY;
+
+ DAXPY(&N, &alpha, x, &INCX, y, &INCY);
+ }
+
+ /* Interface to FORTRAN routine DSCAL. */
+ void IpBlasDscal(Index size, Number alpha, Number *x, Index incX)
+ {
+ ipfint N=size, INCX=incX;
+
+ DSCAL(&N, &alpha, x, &INCX);
+ }
+
+ void IpBlasDgemv(bool trans, Index nRows, Index nCols, Number alpha,
+ const Number* A, Index ldA, const Number* x,
+ Index incX, Number beta, Number* y, Index incY)
+ {
+ ipfint M=nCols, N=nRows, LDA=ldA, INCX=incX, INCY=incY;
+
+ char TRANS;
+ if (trans) {
+ TRANS = 'T';
+ }
+ else {
+ TRANS = 'N';
+ }
+
+ DGEMV(&TRANS, &M, &N, &alpha, A, &LDA, x,
+ &INCX, &beta, y, &INCY, 1);
+ }
+
+ void IpBlasDsymv(Index n, Number alpha, const Number* A, Index ldA,
+ const Number* x, Index incX, Number beta, Number* y,
+ Index incY)
+ {
+ ipfint N=n, LDA=ldA, INCX=incX, INCY=incY;
+
+ char UPLO='L';
+
+ DSYMV(&UPLO, &N, &alpha, A, &LDA, x,
+ &INCX, &beta, y, &INCY, 1);
+ }
+
+ void IpBlasDgemm(bool transa, bool transb, Index m, Index n, Index k,
+ Number alpha, const Number* A, Index ldA, const Number* B,
+ Index ldB, Number beta, Number* C, Index ldC)
+ {
+ ipfint M=m, N=n, K=k, LDA=ldA, LDB=ldB, LDC=ldC;
+
+ char TRANSA;
+ if (transa) {
+ TRANSA = 'T';
+ }
+ else {
+ TRANSA = 'N';
+ }
+ char TRANSB;
+ if (transb) {
+ TRANSB = 'T';
+ }
+ else {
+ TRANSB = 'N';
+ }
+
+ DGEMM(&TRANSA, &TRANSB, &M, &N, &K, &alpha, A, &LDA,
+ B, &LDB, &beta, C, &LDC, 1, 1);
+ }
+
+ void IpBlasDsyrk(bool trans, Index ndim, Index nrank,
+ Number alpha, const Number* A, Index ldA,
+ Number beta, Number* C, Index ldC)
+ {
+ ipfint N=ndim, K=nrank, LDA=ldA, LDC=ldC;
+
+ char UPLO='L';
+ char TRANS;
+ if (trans) {
+ TRANS = 'T';
+ }
+ else {
+ TRANS = 'N';
+ }
+
+ DSYRK(&UPLO, &TRANS, &N, &K, &alpha, A, &LDA,
+ &beta, C, &LDC, 1, 1);
+ }
+
+ void IpBlasDtrsm(bool trans, Index ndim, Index nrhs, Number alpha,
+ const Number* A, Index ldA, Number* B, Index ldB)
+ {
+ ipfint M=ndim, N=nrhs, LDA=ldA, LDB=ldB;
+
+ char SIDE = 'L';
+ char UPLO = 'L';
+ char TRANSA;
+ if (trans) {
+ TRANSA = 'T';
+ }
+ else {
+ TRANSA = 'N';
+ }
+ char DIAG = 'N';
+
+ DTRSM(&SIDE, &UPLO, &TRANSA, &DIAG, &M, &N,
+ &alpha, A, &LDA, B, &LDB, 1, 1, 1, 1);
+ }
+
+#else
+ /* Interface to CBLAS routine DDOT. */
+ Number IpBlasDdot(Index size, const Number *x, Index incX, const Number *y,
+ Index incY)
+ {
+ Not Implemented Yet!
+ }
+
+ /* Interface to CBLAS routine DNRM2. */
+ Number IpBlasDnrm2(Index size, const Number *x, Index incX)
+ {
+ Not Implemented Yet!
+ }
+
+ /* Interface to CBLAS routine DASUM. */
+ Number IpBlasDasum(Index size, const Number *x, Index incX)
+ {
+ Not Implemented Yet!
+ }
+
+ /* Interface to CBLAS routine DASUM. */
+ Index IpBlasIdamax(Index size, const Number *x, Index incX)
+ {
+ Not Implemented Yet!
+ }
+
+ /* Interface to CBLAS routine DCOPY. */
+ void IpBlasDcopy(Index size, const Number *x, Index incX, Number *y, Index incY)
+ {
+ Not Implemented Yet!
+ }
+
+ /* Interface to CBLAS routine DAXPY. */
+ void IpBlasDaxpy(Index size, Number alpha, const Number *x, Index incX, Number *y,
+ Index incY)
+ {
+ Not Implemented Yet!
+ }
+
+ /* Interface to CBLAS routine DSCAL. */
+ void IpBlasDscal(Index size, Number alpha, Number *x, Index incX)
+ {
+ Not Implemented Yet!
+ }
+
+#endif
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpBlas.hpp b/SimTKmath/Optimizers/src/IpOpt/IpBlas.hpp
new file mode 100644
index 0000000..a3885e0
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpBlas.hpp
@@ -0,0 +1,78 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpBlas.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPBLAS_HPP__
+#define __IPBLAS_HPP__
+
+#include "IpUtils.hpp"
+
+namespace Ipopt
+{
+ // If CBLAS is not available, this is our own interface to the Fortran
+ // implementation
+
+ /** Wrapper for BLAS function DDOT. Compute dot product of vector x
+ and vector y */
+ Number IpBlasDdot(Index size, const Number *x, Index incX, const Number *y,
+ Index incY);
+
+ /** Wrapper for BLAS function DNRM2. Compute 2-norm of vector x*/
+ Number IpBlasDnrm2(Index size, const Number *x, Index incX);
+
+ /** Wrapper for BLAS function DASUM. Compute 1-norm of vector x*/
+ Number IpBlasDasum(Index size, const Number *x, Index incX);
+
+ /** Wrapper for BLAS function DASUM. Compute index for largest
+ absolute element of vector x */
+ Index IpBlasIdamax(Index size, const Number *x, Index incX);
+
+ /** Wrapper for BLAS subroutine DCOPY. Copying vector x into vector
+ y */
+ void IpBlasDcopy(Index size, const Number *x, Index incX, Number *y,
+ Index incY);
+
+ /** Wrapper for BLAS subroutine DAXPY. Adding the alpha multiple of
+ vector x to vector y */
+ void IpBlasDaxpy(Index size, Number alpha, const Number *x, Index incX,
+ Number *y, Index incY);
+
+ /** Wrapper for BLAS subroutine DSCAL. Scaling vector x by scalar
+ alpha */
+ void IpBlasDscal(Index size, Number alpha, Number *x, Index incX);
+
+ /** Wrapper for BLAS subroutine DGEMV. Multiplying a matrix with a
+ vector. */
+ void IpBlasDgemv(bool trans, Index nRows, Index nCols, Number alpha,
+ const Number* A, Index ldA, const Number* x,
+ Index incX, Number beta, Number* y, Index incY);
+
+ /** Wrapper for BLAS subroutine DSYMV. Multiplying a symmetric
+ matrix with a vector. */
+ void IpBlasDsymv(Index n, Number alpha, const Number* A, Index ldA,
+ const Number* x, Index incX, Number beta, Number* y,
+ Index incY);
+
+ /** Wrapper for BLAS subroutine DGEMM. Multiplying two matrices */
+ void IpBlasDgemm(bool transa, bool transb, Index m, Index n, Index k,
+ Number alpha, const Number* A, Index ldA, const Number* B,
+ Index ldB, Number beta, Number* C, Index ldC);
+
+ /** Wrapper for BLAS subroutine DSYRK. Adding a high-rank update to
+ * a matrix */
+ void IpBlasDsyrk(bool trans, Index ndim, Index nrank,
+ Number alpha, const Number* A, Index ldA,
+ Number beta, Number* C, Index ldC);
+
+ /** Wrapper for BLAS subroutine DTRSM. Backsolve for a lower triangular
+ * matrix. */
+ void IpBlasDtrsm(bool trans, Index ndim, Index nrhs, Number alpha,
+ const Number* A, Index ldA, Number* B, Index ldB);
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpCachedResults.hpp b/SimTKmath/Optimizers/src/IpOpt/IpCachedResults.hpp
new file mode 100644
index 0000000..9dfd6a4
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpCachedResults.hpp
@@ -0,0 +1,747 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpCachedResults.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPCACHEDRESULTS_HPP__
+#define __IPCACHEDRESULTS_HPP__
+
+#include "IpTaggedObject.hpp"
+#include "IpObserver.hpp"
+#include "IpDebug.hpp"
+#include <algorithm>
+#include <vector>
+#include <list>
+
+namespace Ipopt
+{
+
+ // Forward Declarations
+
+ template <class T>
+ class DependentResult;
+
+ // AW: I'm taking this out, since this is by far the most used
+ // class. We should keep it as simple as possible.
+ // /** Cache Priority Enum */
+ // enum CachePriority
+ // {
+ // CP_Lowest,
+ // CP_Standard,
+ // CP_Trial,
+ // CP_Iterate
+ // };
+
+ /** Templated class for Chached Results. This class stores up to a
+ * given number of "results", entities that are stored here
+ * together with identifies, that can be used to later retrieve the
+ * information again.
+ *
+ * Typically, T is a SmartPtr for some calculated quantity that
+ * should be stored (such as a Vector). The identifiers (or
+ * dependencies) are a (possibly varying) number of Tags from
+ * TaggedObjects, and a number of Numbers. Results are added to
+ * the cache using the AddCachedResults methods, and the can be
+ * retrieved with the GetCachedResults methods. The second set of
+ * methods checks whether a result has been cached for the given
+ * identifiers. If a corresponding results is found, a copy of it
+ * is returned and the method evaluates to true, otherwise it
+ * evaluates to false.
+ *
+ * Note that cached results can become "stale", namely when a
+ * TaggedObject that is used to identify this CachedResult is
+ * changed. When this happens, the cached result can never be
+ * asked for again, so that there is no point in storing it any
+ * longer. For this purpose, a cached result, which is stored as a
+ * DependentResult, inherits off an Observer. This Observer
+ * retrieves notification whenever a TaggedObject dependency has
+ * changed. Stale results are later removed from the cache.
+ */
+ template <class T>
+ class CachedResults
+ {
+ public:
+#ifdef IP_DEBUG_CACHE
+ /** (Only if compiled in DEBUG mode): debug verbosity level */
+ static const Index dbg_verbosity;
+#endif
+
+ /** @name Constructors and Destructors. */
+ //@{
+ /** Constructor, where max_cache_size is the maximal number of
+ * results that should be cached. If max_cache_size is negative,
+ * we allow an infinite abount of cache.
+ */
+ CachedResults(Int max_cache_size);
+
+ /** Destructor */
+ virtual ~CachedResults();
+ //@}
+
+ /** @name Generic methods for adding and retrieving cached results. */
+ //@{
+ /** Generic method for adding a result to the cache, given a
+ * std::vector of TaggesObjects and a std::vector of Numbers.
+ */
+ void AddCachedResult(const T& result,
+ const std::vector<const TaggedObject*>& dependents,
+ const std::vector<Number>& scalar_dependents);
+
+ /** Generic method for retrieving a cached results, given the
+ * dependencies as a std::vector of TaggesObjects and a
+ * std::vector of Numbers.
+ */
+ bool GetCachedResult(T& retResult,
+ const std::vector<const TaggedObject*>& dependents,
+ const std::vector<Number>& scalar_dependents) const;
+
+ /** Method for adding a result, providing only a std::vector of
+ * TaggedObjects.
+ */
+ void AddCachedResult(const T& result,
+ const std::vector<const TaggedObject*>& dependents);
+
+ /** Method for retrieving a cached result, providing only a
+ * std::vector of TaggedObjects.
+ */
+ bool GetCachedResult(T& retResult,
+ const std::vector<const TaggedObject*>& dependents) const;
+ //@}
+
+ /** @name Pointer-based methods for adding and retrieving cached
+ * results, providing dependencies explicitly.
+ */
+ //@{
+ /** Method for adding a result to the cache, proving one
+ * dependency as a TaggedObject explicitly.
+ */
+ void AddCachedResult1Dep(const T& result,
+ const TaggedObject* dependent1);
+
+ /** Method for retrieving a cached result, proving one dependency
+ * as a TaggedObject explicitly.
+ */
+ bool GetCachedResult1Dep(T& retResult, const TaggedObject* dependent1);
+
+ /** Method for adding a result to the cache, proving two
+ * dependencies as a TaggedObject explicitly.
+ */
+ void AddCachedResult2Dep(const T& result,
+ const TaggedObject* dependent1,
+ const TaggedObject* dependent2);
+
+ /** Method for retrieving a cached result, proving two
+ * dependencies as a TaggedObject explicitly.
+ */
+ bool GetCachedResult2Dep(T& retResult,
+ const TaggedObject* dependent1,
+ const TaggedObject* dependent2);
+
+ /** Method for adding a result to the cache, proving three
+ * dependencies as a TaggedObject explicitly.
+ */
+ void AddCachedResult3Dep(const T& result,
+ const TaggedObject* dependent1,
+ const TaggedObject* dependent2,
+ const TaggedObject* dependent3);
+
+ /** Method for retrieving a cached result, proving three
+ * dependencies as a TaggedObject explicitly.
+ */
+ bool GetCachedResult3Dep(T& retResult,
+ const TaggedObject* dependent1,
+ const TaggedObject* dependent2,
+ const TaggedObject* dependent3);
+
+ /** @name Pointer-free version of the Add and Get methods */
+ //@{
+ bool GetCachedResult1Dep(T& retResult, const TaggedObject& dependent1)
+ {
+ return GetCachedResult1Dep(retResult, &dependent1);
+ }
+ bool GetCachedResult2Dep(T& retResult,
+ const TaggedObject& dependent1,
+ const TaggedObject& dependent2)
+ {
+ return GetCachedResult2Dep(retResult, &dependent1, &dependent2);
+ }
+ bool GetCachedResult3Dep(T& retResult,
+ const TaggedObject& dependent1,
+ const TaggedObject& dependent2,
+ const TaggedObject& dependent3)
+ {
+ return GetCachedResult3Dep(retResult, &dependent1, &dependent2, &dependent3);
+ }
+ void AddCachedResult1Dep(const T& result,
+ const TaggedObject& dependent1)
+ {
+ AddCachedResult1Dep(result, &dependent1);
+ }
+ void AddCachedResult2Dep(const T& result,
+ const TaggedObject& dependent1,
+ const TaggedObject& dependent2)
+ {
+ AddCachedResult2Dep(result, &dependent1, &dependent2);
+ }
+ void AddCachedResult3Dep(const T& result,
+ const TaggedObject& dependent1,
+ const TaggedObject& dependent2,
+ const TaggedObject& dependent3)
+ {
+ AddCachedResult3Dep(result, &dependent1, &dependent2, &dependent3);
+ }
+ //@}
+
+ /** Invalidates the result for given dependecies. Sets the stale
+ * flag for the corresponding cached result to true if it is
+ * found. Returns true, if the result was found. */
+ bool InvalidateResult(const std::vector<const TaggedObject*>& dependents,
+ const std::vector<Number>& scalar_dependents);
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ CachedResults();
+
+ /** Copy Constructor */
+ CachedResults(const CachedResults&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const CachedResults&);
+ //@}
+
+ /** maximum number of cached results */
+ Int max_cache_size_;
+
+ /** list of currently cached results. */
+ mutable std::list<DependentResult<T>*>* cached_results_;
+
+ /** internal method for removing stale DependentResults from the
+ * list. It is called at the beginning of every
+ * GetDependentResult method.
+ */
+ void CleanupInvalidatedResults() const;
+
+ /** Print list of currently cached results */
+ void DebugPrintCachedResults() const;
+ };
+
+ /** Templated class which stores one entry for the CachedResult
+ * class. It stores the result (of type T), together with its
+ * dependencies (vector of TaggedObjects and vector of Numbers).
+ * It also stores a priority.
+ */
+ template <class T>
+ class DependentResult : public Observer
+ {
+ public:
+
+#ifdef IP_DEBUG_CACHE
+
+ static const Index dbg_verbosity;
+#endif
+
+ /** @name Constructor, Destructors */
+ //@{
+ /** Constructor, given all information about the result. */
+ DependentResult(const T& result, const std::vector<const TaggedObject*>& dependents,
+ const std::vector<Number>& scalar_dependents);
+
+ /** Destructor. */
+ ~DependentResult();
+ //@}
+
+ /** @name Accessor method. */
+ //@{
+ /** This returns true, if the DependentResult is no longer valid. */
+ bool IsStale() const;
+
+ /** Invalidates the cached result. */
+ void Invalidate();
+
+ /** Returns the cached result. */
+ const T& GetResult() const;
+ //@}
+
+ /** This method returns true if the dependencies provided to this
+ * function are identical to the ones stored with the
+ * DependentResult.
+ */
+ bool DependentsIdentical(const std::vector<const TaggedObject*>& dependents,
+ const std::vector<Number>& scalar_dependents) const;
+
+ /** Print information about this DependentResults. */
+ void DebugPrint() const;
+
+ protected:
+ /** This method is overloading the pure virtual method from the
+ * Observer base class. This method is called when a Subject
+ * registered for this Observer sends a notification. In this
+ * particular case, if this method is called with
+ * notify_type==NT_Changed or NT_BeingDeleted, then this results
+ * is marked as stale.
+ */
+ virtual void RecieveNotification(NotifyType notify_type, const Subject* subject);
+
+ private:
+
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ DependentResult();
+
+ /** Copy Constructor */
+ DependentResult(const DependentResult&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const DependentResult&);
+ //@}
+
+ /** Flag indicating, if the cached result is still valid. A
+ result becomes invalid, if the RecieveNotification method is
+ called with NT_Changed */
+ bool stale_;
+ /** The value of the dependent results */
+ const T result_;
+ /** Dependencies in form of TaggedObjects */
+ std::vector<TaggedObject::Tag> dependent_tags_;
+ /** Dependencies in form a Numbers */
+ std::vector<Number> scalar_dependents_;
+ };
+
+#ifdef IP_DEBUG_CACHE
+
+ template <class T>
+ const Index CachedResults<T>::dbg_verbosity = 0;
+
+ template <class T>
+ const Index DependentResult<T>::dbg_verbosity = 0;
+#endif
+
+ template <class T>
+ DependentResult<T>::DependentResult(
+ const T& result,
+ const std::vector<const TaggedObject*>& dependents,
+ const std::vector<Number>& scalar_dependents)
+ :
+ stale_(false),
+ result_(result),
+ dependent_tags_(dependents.size()),
+ scalar_dependents_(scalar_dependents)
+ {
+#ifdef IP_DEBUG_CACHE
+ DBG_START_METH("DependentResult<T>::DependentResult()", dbg_verbosity);
+#endif
+
+ for (Index i=0; i<(Index)dependents.size(); i++) {
+ if (dependents[i]) {
+ // Call the RequestAttach method of the Observer base class.
+ // This will add this dependent result in the Observer list
+ // for the Subject dependents[i]. As a consequence, the
+ // RecieveNotification method of this DependentResult will be
+ // called with notify_type=NT_Changed, whenever the
+ // TaggedResult dependents[i] is changed (i.e. its HasChanged
+ // method is called).
+ RequestAttach(NT_Changed, dependents[i]);
+ dependent_tags_[i] = dependents[i]->GetTag();
+ }
+ else {
+ dependent_tags_[i] = 0;
+ }
+ }
+ }
+
+ template <class T>
+ DependentResult<T>::~DependentResult()
+ {
+#ifdef IP_DEBUG_CACHE
+ DBG_START_METH("DependentResult<T>::~DependentResult()", dbg_verbosity);
+#endif
+ //DBG_ASSERT(stale_ == true);
+ // Nothing to be done here, destructor
+ // of T should sufficiently remove
+ // any memory, etc.
+ }
+
+ template <class T>
+ bool DependentResult<T>::IsStale() const
+ {
+ return stale_;
+ }
+
+ template <class T>
+ void DependentResult<T>::Invalidate()
+ {
+ stale_ = true;
+ }
+
+ template <class T>
+ void DependentResult<T>::RecieveNotification(NotifyType notify_type, const Subject* subject)
+ {
+#ifdef IP_DEBUG_CACHE
+ DBG_START_METH("DependentResult<T>::RecieveNotification", dbg_verbosity);
+#endif
+
+ if (notify_type == NT_Changed || notify_type==NT_BeingDestroyed) {
+ stale_ = true;
+ // technically, I could unregister the notifications here, but they
+ // aren't really hurting anything
+ }
+ }
+
+ template <class T>
+ bool DependentResult<T>::DependentsIdentical(const std::vector<const TaggedObject*>& dependents,
+ const std::vector<Number>& scalar_dependents) const
+ {
+#ifdef IP_DEBUG_CACHE
+ DBG_START_METH("DependentResult<T>::DependentsIdentical", dbg_verbosity);
+#endif
+
+ DBG_ASSERT(stale_ == false);
+ DBG_ASSERT(dependents.size() == dependent_tags_.size());
+
+ bool retVal = true;
+
+ if (dependents.size() != dependent_tags_.size()
+ || scalar_dependents.size() != scalar_dependents_.size()) {
+ retVal = false;
+ }
+ else {
+ for (Index i=0; i<(Index)dependents.size(); i++) {
+ if ( (dependents[i] && dependents[i]->GetTag() != dependent_tags_[i])
+ || (!dependents[i] && dependent_tags_[i] != 0) ) {
+ retVal = false;
+ break;
+ }
+ }
+ if (retVal) {
+ for (Index i=0; i<(Index)scalar_dependents.size(); i++) {
+ if (scalar_dependents[i] != scalar_dependents_[i]) {
+ retVal = false;
+ break;
+ }
+ }
+ }
+ }
+
+ return retVal;
+ }
+
+ template <class T>
+ const T& DependentResult<T>::GetResult() const
+ {
+#ifdef IP_DEBUG_CACHE
+ DBG_START_METH("DependentResult<T>::GetResult()", dbg_verbosity);
+#endif
+
+ DBG_ASSERT(stale_ == false);
+ return result_;
+ }
+
+ template <class T>
+ void DependentResult<T>::DebugPrint() const
+ {
+#ifdef IP_DEBUG_CACHE
+ DBG_START_METH("DependentResult<T>::DebugPrint", dbg_verbosity);
+#endif
+
+ }
+
+ template <class T>
+ CachedResults<T>::CachedResults(Int max_cache_size)
+ :
+ max_cache_size_(max_cache_size),
+ cached_results_(NULL)
+ {
+#ifdef IP_DEBUG_CACHE
+ DBG_START_METH("CachedResults<T>::CachedResults", dbg_verbosity);
+#endif
+
+ }
+
+ template <class T>
+ CachedResults<T>::~CachedResults()
+ {
+#ifdef IP_DEBUG_CACHE
+ DBG_START_METH("CachedResults<T>::!CachedResults()", dbg_verbosity);
+#endif
+
+ if (cached_results_) {
+ for (typename std::list< DependentResult<T>* >::iterator iter = cached_results_->
+ begin();
+ iter != cached_results_->end();
+ iter++) {
+ delete *iter;
+ }
+ delete cached_results_;
+ }
+ /*
+ while (!cached_results_.empty()) {
+ DependentResult<T>* result = cached_results_.back();
+ cached_results_.pop_back();
+ delete result;
+ }
+ */
+ }
+
+ template <class T>
+ void CachedResults<T>::AddCachedResult(const T& result,
+ const std::vector<const TaggedObject*>& dependents,
+ const std::vector<Number>& scalar_dependents)
+ {
+#ifdef IP_DEBUG_CACHE
+ DBG_START_METH("CachedResults<T>::AddCachedResult", dbg_verbosity);
+#endif
+
+ CleanupInvalidatedResults();
+
+ // insert the new one here
+ DependentResult<T>* newResult = new DependentResult<T>(result, dependents, scalar_dependents);
+ if (!cached_results_) {
+ cached_results_ = new std::list<DependentResult<T>*>;
+ }
+ cached_results_->push_front(newResult);
+
+ // keep the list small enough
+ if (max_cache_size_ >= 0) { // if negative, allow infinite cache
+ // non-negative - limit size of list to max_cache_size
+ DBG_ASSERT((Int)cached_results_->size()<=max_cache_size_+1);
+ if ((Int)cached_results_->size() > max_cache_size_) {
+ delete cached_results_->back();
+ cached_results_->pop_back();
+ }
+ }
+
+#ifdef IP_DEBUG_CACHE
+ DBG_EXEC(2, DebugPrintCachedResults());
+#endif
+
+ }
+
+ template <class T>
+ void CachedResults<T>::AddCachedResult(const T& result,
+ const std::vector<const TaggedObject*>& dependents)
+ {
+ std::vector<Number> scalar_dependents;
+ AddCachedResult(result, dependents, scalar_dependents);
+ }
+
+ template <class T>
+ bool CachedResults<T>::GetCachedResult(T& retResult, const std::vector<const TaggedObject*>& dependents,
+ const std::vector<Number>& scalar_dependents) const
+ {
+#ifdef IP_DEBUG_CACHE
+ DBG_START_METH("CachedResults<T>::GetCachedResult", dbg_verbosity);
+#endif
+
+ if (!cached_results_)
+ return false;
+
+ CleanupInvalidatedResults();
+
+ bool retValue = false;
+ typename std::list< DependentResult<T>* >::const_iterator iter;
+ for (iter = cached_results_->begin(); iter != cached_results_->end(); iter++) {
+ if ((*iter)->DependentsIdentical(dependents, scalar_dependents)) {
+ retResult = (*iter)->GetResult();
+ retValue = true;
+ break;
+ }
+ }
+
+#ifdef IP_DEBUG_CACHE
+ DBG_EXEC(2, DebugPrintCachedResults());
+#endif
+
+ return retValue;
+ }
+
+ template <class T>
+ bool CachedResults<T>::GetCachedResult(
+ T& retResult, const std::vector<const TaggedObject*>& dependents) const
+ {
+ std::vector<Number> scalar_dependents;
+ return GetCachedResult(retResult, dependents, scalar_dependents);
+ }
+
+ template <class T>
+ void CachedResults<T>::AddCachedResult1Dep(const T& result,
+ const TaggedObject* dependent1)
+ {
+#ifdef IP_DEBUG_CACHE
+ DBG_START_METH("CachedResults<T>::AddCachedResult1Dep", dbg_verbosity);
+#endif
+
+ std::vector<const TaggedObject*> dependents(1);
+ dependents[0] = dependent1;
+
+ AddCachedResult(result, dependents);
+ }
+
+ template <class T>
+ bool CachedResults<T>::GetCachedResult1Dep(T& retResult, const TaggedObject* dependent1)
+ {
+#ifdef IP_DEBUG_CACHE
+ DBG_START_METH("CachedResults<T>::GetCachedResult1Dep", dbg_verbosity);
+#endif
+
+ std::vector<const TaggedObject*> dependents(1);
+ dependents[0] = dependent1;
+
+ return GetCachedResult(retResult, dependents);
+ }
+
+ template <class T>
+ void CachedResults<T>::AddCachedResult2Dep(const T& result, const TaggedObject* dependent1,
+ const TaggedObject* dependent2)
+
+ {
+#ifdef IP_DEBUG_CACHE
+ DBG_START_METH("CachedResults<T>::AddCachedResult2dDep", dbg_verbosity);
+#endif
+
+ std::vector<const TaggedObject*> dependents(2);
+ dependents[0] = dependent1;
+ dependents[1] = dependent2;
+
+ AddCachedResult(result, dependents);
+ }
+
+ template <class T>
+ bool CachedResults<T>::GetCachedResult2Dep(T& retResult, const TaggedObject* dependent1, const TaggedObject* dependent2)
+ {
+#ifdef IP_DEBUG_CACHE
+ DBG_START_METH("CachedResults<T>::GetCachedResult2Dep", dbg_verbosity);
+#endif
+
+ std::vector<const TaggedObject*> dependents(2);
+ dependents[0] = dependent1;
+ dependents[1] = dependent2;
+
+ return GetCachedResult(retResult, dependents);
+ }
+
+ template <class T>
+ void CachedResults<T>::AddCachedResult3Dep(const T& result, const TaggedObject* dependent1,
+ const TaggedObject* dependent2,
+ const TaggedObject* dependent3)
+
+ {
+#ifdef IP_DEBUG_CACHE
+ DBG_START_METH("CachedResults<T>::AddCachedResult2dDep", dbg_verbosity);
+#endif
+
+ std::vector<const TaggedObject*> dependents(3);
+ dependents[0] = dependent1;
+ dependents[1] = dependent2;
+ dependents[2] = dependent3;
+
+ AddCachedResult(result, dependents);
+ }
+
+ template <class T>
+ bool CachedResults<T>::GetCachedResult3Dep(T& retResult, const TaggedObject* dependent1,
+ const TaggedObject* dependent2,
+ const TaggedObject* dependent3)
+ {
+#ifdef IP_DEBUG_CACHE
+ DBG_START_METH("CachedResults<T>::GetCachedResult2Dep", dbg_verbosity);
+#endif
+
+ std::vector<const TaggedObject*> dependents(3);
+ dependents[0] = dependent1;
+ dependents[1] = dependent2;
+ dependents[2] = dependent3;
+
+ return GetCachedResult(retResult, dependents);
+ }
+
+ template <class T>
+ bool CachedResults<T>::InvalidateResult(const std::vector<const TaggedObject*>& dependents,
+ const std::vector<Number>& scalar_dependents)
+ {
+ if (!cached_results_)
+ return false;
+
+ CleanupInvalidatedResults();
+
+ bool retValue = false;
+ typename std::list< DependentResult<T>* >::const_iterator iter;
+ for (iter = cached_results_->begin(); iter != cached_results_->end(); iter++) {
+ if ((*iter)->DependentsIdentical(dependents, scalar_dependents)) {
+ (*iter)->Invalidate();
+ retValue = true;
+ break;
+ }
+ }
+
+ return retValue;
+ }
+
+ template <class T>
+ void CachedResults<T>::CleanupInvalidatedResults() const
+ {
+#ifdef IP_DEBUG_CACHE
+ DBG_START_METH("CachedResults<T>::CleanupInvalidatedResults", dbg_verbosity);
+#endif
+
+ if (!cached_results_)
+ return;
+
+ typename std::list< DependentResult<T>* >::iterator iter;
+ iter = cached_results_->begin();
+ while (iter != cached_results_->end()) {
+ if ((*iter)->IsStale()) {
+ typename std::list< DependentResult<T>* >::iterator
+ iter_to_remove = iter;
+ iter++;
+ DependentResult<T>* result_to_delete = (*iter_to_remove);
+ cached_results_->erase(iter_to_remove);
+ delete result_to_delete;
+ }
+ else {
+ iter++;
+ }
+ }
+ }
+
+ template <class T>
+ void CachedResults<T>::DebugPrintCachedResults() const
+ {
+#ifdef IP_DEBUG_CACHE
+ DBG_START_METH("CachedResults<T>::DebugPrintCachedResults", dbg_verbosity);
+ if (DBG_VERBOSITY()>=2 ) {
+ if (!chached_results_) {
+ DBG_PRINT((2," DependentResult:0x%x\n", (*iter)));
+ }
+ else {
+ typename std::list< DependentResult<T>* >::const_iterator iter;
+ DBG_PRINT((2,"Current set of cached results:\n"));
+ for (iter = cached_results_->begin(); iter != cached_results_->end(); iter++) {
+ DBG_PRINT((2," DependentResult:0x%x\n", (*iter)));
+ }
+ }
+ }
+#endif
+
+ }
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpCompoundMatrix.cpp b/SimTKmath/Optimizers/src/IpOpt/IpCompoundMatrix.cpp
new file mode 100644
index 0000000..ee261f6
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpCompoundMatrix.cpp
@@ -0,0 +1,582 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpCompoundMatrix.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpoptConfig.h"
+#include "IpCompoundMatrix.hpp"
+#include "IpCompoundVector.hpp"
+
+#ifdef HAVE_CSTDIO
+# include <cstdio>
+#else
+# ifdef HAVE_STDIO_H
+# include <stdio.h>
+# else
+# error "don't have header file for stdio"
+# endif
+#endif
+
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+
+
+namespace Ipopt
+{
+
+ CompoundMatrix::CompoundMatrix(const CompoundMatrixSpace* owner_space)
+ :
+ Matrix(owner_space),
+ owner_space_(owner_space),
+ matrices_valid_(false)
+ {
+ std::vector<SmartPtr<Matrix> > row(NComps_Cols());
+ std::vector<SmartPtr<const Matrix> > const_row(NComps_Cols());
+ for (Index irow=0; irow<NComps_Rows(); irow++) {
+ const_comps_.push_back(const_row);
+ comps_.push_back(row);
+ }
+ }
+
+
+ CompoundMatrix::~CompoundMatrix()
+ {}
+
+ void CompoundMatrix::SetComp(Index irow, Index jcol, const Matrix& matrix)
+ {
+ DBG_ASSERT(irow<NComps_Rows());
+ DBG_ASSERT(jcol<NComps_Cols());
+ DBG_ASSERT(owner_space_->GetCompSpace(irow, jcol)->IsMatrixFromSpace(matrix));
+
+ comps_[irow][jcol] = NULL;
+ const_comps_[irow][jcol] = &matrix;
+ ObjectChanged();
+ }
+
+ void CompoundMatrix::SetCompNonConst(Index irow, Index jcol, Matrix& matrix)
+ {
+ DBG_ASSERT(irow < NComps_Rows());
+ DBG_ASSERT(jcol < NComps_Cols());
+ DBG_ASSERT(owner_space_->GetCompSpace(irow, jcol)->IsMatrixFromSpace(matrix));
+
+ const_comps_[irow][jcol] = NULL;
+ comps_[irow][jcol] = &matrix;
+ ObjectChanged();
+ }
+
+ void CompoundMatrix::CreateBlockFromSpace(Index irow, Index jcol)
+ {
+ DBG_ASSERT(irow < NComps_Rows());
+ DBG_ASSERT(jcol < NComps_Cols());
+ DBG_ASSERT(IsValid(owner_space_->GetCompSpace(irow, jcol)));
+ SetCompNonConst(irow, jcol, *owner_space_->GetCompSpace(irow,jcol)->MakeNew());
+ }
+
+ void CompoundMatrix::MultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ if (!matrices_valid_) {
+ matrices_valid_ = MatricesValid();
+ }
+ DBG_ASSERT(matrices_valid_);
+
+ // The vectors are assumed to be compound Vectors as well
+ const CompoundVector* comp_x = dynamic_cast<const CompoundVector*>(&x);
+ CompoundVector* comp_y = dynamic_cast<CompoundVector*>(&y);
+
+ // A few sanity checks
+ if (comp_x) {
+ DBG_ASSERT(NComps_Cols()==comp_x->NComps());
+ }
+ else {
+ DBG_ASSERT(NComps_Cols() == 1);
+ }
+
+ if (comp_y) {
+ DBG_ASSERT(NComps_Rows()==comp_y->NComps());
+ }
+ else {
+ DBG_ASSERT(NComps_Rows() == 1);
+ }
+
+ // Take care of the y part of the addition
+ if( beta!=0.0 ) {
+ y.Scal(beta);
+ }
+ else {
+ y.Set(0.0); // In case y hasn't been initialized yet
+ }
+
+ for( Index irow = 0; irow < NComps_Rows(); irow++ ) {
+ SmartPtr<Vector> y_i;
+ if (comp_y) {
+ y_i = comp_y->GetCompNonConst(irow);
+ }
+ else {
+ y_i = &y;
+ }
+ DBG_ASSERT(IsValid(y_i));
+
+ for( Index jcol = 0; jcol < NComps_Cols(); jcol++ ) {
+ if ( (owner_space_->Diagonal() && irow == jcol)
+ || (!owner_space_->Diagonal() && ConstComp(irow,jcol)) ) {
+ SmartPtr<const Vector> x_j;
+ if (comp_x) {
+ x_j = comp_x->GetComp(jcol);
+ }
+ else if (NComps_Cols() == 1) {
+ x_j = &x;
+ }
+ DBG_ASSERT(IsValid(x_j));
+
+ ConstComp(irow, jcol)->MultVector(alpha, *x_j,
+ 1., *y_i);
+ }
+ }
+ }
+ }
+
+ void CompoundMatrix::TransMultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ if (!matrices_valid_) {
+ matrices_valid_ = MatricesValid();
+ }
+ DBG_ASSERT(matrices_valid_);
+
+ // The vectors are assumed to be compound Vectors as well
+ const CompoundVector* comp_x = dynamic_cast<const CompoundVector*>(&x);
+ CompoundVector* comp_y = dynamic_cast<CompoundVector*>(&y);
+
+ // A few sanity checks
+ if (comp_y) {
+ DBG_ASSERT(NComps_Cols()==comp_y->NComps());
+ }
+ else {
+ DBG_ASSERT(NComps_Cols() == 1);
+ }
+
+ if (comp_x) {
+ DBG_ASSERT(NComps_Rows()==comp_x->NComps());
+ }
+ else {
+ DBG_ASSERT(NComps_Rows() == 1);
+ }
+
+ // Take care of the y part of the addition
+ if( beta!=0.0 ) {
+ y.Scal(beta);
+ }
+ else {
+ y.Set(0.0); // In case y hasn't been initialized yet
+ }
+
+ for( Index irow = 0; irow < NComps_Cols(); irow++ ) {
+ SmartPtr<Vector> y_i;
+ if (comp_y) {
+ y_i = comp_y->GetCompNonConst(irow);
+ }
+ else {
+ y_i = &y;
+ }
+ DBG_ASSERT(IsValid(y_i));
+
+ for( Index jcol = 0; jcol < NComps_Rows(); jcol++ ) {
+ if ( (owner_space_->Diagonal() && irow == jcol)
+ || (!owner_space_->Diagonal() && ConstComp(jcol, irow)) ) {
+ SmartPtr<const Vector> x_j;
+ if (comp_x) {
+ x_j = comp_x->GetComp(jcol);
+ }
+ else {
+ x_j = &x;
+ }
+ DBG_ASSERT(IsValid(x_j));
+
+ ConstComp(jcol, irow)->TransMultVector(alpha, *x_j,
+ 1., *y_i);
+ }
+ }
+ }
+ }
+
+ // Specialized method (overloaded from IpMatrix)
+ void CompoundMatrix::AddMSinvZImpl(Number alpha, const Vector& S,
+ const Vector& Z, Vector& X) const
+ {
+ // The vectors are assumed to be compound Vectors as well (unless they
+ // are assumed to consist of only one component
+ const CompoundVector* comp_S = dynamic_cast<const CompoundVector*>(&S);
+ const CompoundVector* comp_Z = dynamic_cast<const CompoundVector*>(&Z);
+ CompoundVector* comp_X = dynamic_cast<CompoundVector*>(&X);
+
+ // A few sanity checks for sizes
+ if (comp_S) {
+ DBG_ASSERT(NComps_Cols()==comp_S->NComps());
+ }
+ else {
+ DBG_ASSERT(NComps_Cols() == 1);
+ }
+ if (comp_Z) {
+ DBG_ASSERT(NComps_Cols()==comp_Z->NComps());
+ }
+ else {
+ DBG_ASSERT(NComps_Cols() == 1);
+ }
+ if (comp_X) {
+ DBG_ASSERT(NComps_Rows()==comp_X->NComps());
+ }
+ else {
+ DBG_ASSERT(NComps_Rows() == 1);
+ }
+
+ for( Index irow = 0; irow < NComps_Cols(); irow++ ) {
+ SmartPtr<Vector> X_i;
+ if (comp_X) {
+ X_i = comp_X->GetCompNonConst(irow);
+ }
+ else {
+ X_i = &X;
+ }
+ DBG_ASSERT(IsValid(X_i));
+
+ for( Index jcol = 0; jcol < NComps_Cols(); jcol++ ) {
+ if ( (owner_space_->Diagonal() && irow == jcol)
+ || (!owner_space_->Diagonal() && ConstComp(jcol, irow)) ) {
+ SmartPtr<const Vector> S_j;
+ if (comp_S) {
+ S_j = comp_S->GetComp(jcol);
+ }
+ else {
+ S_j = &S;
+ }
+ DBG_ASSERT(IsValid(S_j));
+ SmartPtr<const Vector> Z_j;
+ if (comp_Z) {
+ Z_j = comp_Z->GetComp(jcol);
+ }
+ else {
+ Z_j = &Z;
+ }
+ DBG_ASSERT(IsValid(Z_j));
+
+ ConstComp(jcol, irow)->AddMSinvZ(alpha, *S_j, *Z_j, *X_i);
+ }
+ }
+ }
+ }
+
+ // Specialized method (overloaded from IpMatrix)
+ void CompoundMatrix::SinvBlrmZMTdBrImpl(Number alpha, const Vector& S,
+ const Vector& R, const Vector& Z,
+ const Vector& D, Vector& X) const
+ {
+ // First check if the matrix is indeed such that we can use the
+ // special methods from the component spaces (this only works if
+ // we have exactly one submatrix per column)
+ // CDL: in every case this was true, the matrix blocks were on the
+ // diagonal so I made this more efficient.
+
+ if (!owner_space_->Diagonal()) {
+ // Use the standard replacement implementation
+ Matrix::SinvBlrmZMTdBrImpl(alpha, S, R, Z, D, X);
+ DBG_ASSERT(false && "Found a matrix where we can't use the fast SinvBlrmZMTdBr implementation in CompoundMatrix");
+ }
+ else {
+ // The vectors are assumed to be compound Vectors as well (unless they
+ // are assumed to consist of only one component
+ const CompoundVector* comp_S = dynamic_cast<const CompoundVector*>(&S);
+ const CompoundVector* comp_R = dynamic_cast<const CompoundVector*>(&R);
+ const CompoundVector* comp_Z = dynamic_cast<const CompoundVector*>(&Z);
+ const CompoundVector* comp_D = dynamic_cast<const CompoundVector*>(&D);
+ CompoundVector* comp_X = dynamic_cast<CompoundVector*>(&X);
+
+ // A few sanity checks for sizes
+ if (comp_S) {
+ DBG_ASSERT(NComps_Cols()==comp_S->NComps());
+ }
+ else {
+ DBG_ASSERT(NComps_Cols() == 1);
+ }
+ if (comp_Z) {
+ DBG_ASSERT(NComps_Cols()==comp_Z->NComps());
+ }
+ else {
+ DBG_ASSERT(NComps_Cols() == 1);
+ }
+ if (comp_R) {
+ DBG_ASSERT(NComps_Cols()==comp_R->NComps());
+ }
+ else {
+ DBG_ASSERT(NComps_Cols() == 1);
+ }
+ if (comp_D) {
+ DBG_ASSERT(NComps_Rows()==comp_D->NComps());
+ }
+ else {
+ DBG_ASSERT(NComps_Rows() == 1);
+ }
+ if (comp_X) {
+ DBG_ASSERT(NComps_Cols()==comp_X->NComps());
+ }
+ else {
+ DBG_ASSERT(NComps_Cols() == 1);
+ }
+
+ for (Index irow=0; irow<NComps_Cols(); irow++ ) {
+ Index jcol = irow; // diagonal, remember
+ SmartPtr<const Vector> S_i;
+ if (comp_S) {
+ S_i = comp_S->GetComp(irow);
+ }
+ else {
+ S_i = &S;
+ }
+ DBG_ASSERT(IsValid(S_i));
+ SmartPtr<const Vector> Z_i;
+ if (comp_Z) {
+ Z_i = comp_Z->GetComp(irow);
+ }
+ else {
+ Z_i = &Z;
+ }
+ DBG_ASSERT(IsValid(Z_i));
+ SmartPtr<const Vector> R_i;
+ if (comp_R) {
+ R_i = comp_R->GetComp(irow);
+ }
+ else {
+ R_i = &R;
+ }
+ DBG_ASSERT(IsValid(R_i));
+ SmartPtr<const Vector> D_i;
+ if (comp_D) {
+ D_i = comp_D->GetComp(jcol);
+ }
+ else {
+ D_i = &D;
+ }
+ DBG_ASSERT(IsValid(D_i));
+ SmartPtr<Vector> X_i;
+ if (comp_X) {
+ X_i = comp_X->GetCompNonConst(irow);
+ }
+ else {
+ X_i = &X;
+ }
+ DBG_ASSERT(IsValid(X_i));
+
+ ConstComp(jcol, irow)->SinvBlrmZMTdBr(alpha, *S_i, *R_i, *Z_i,
+ *D_i, *X_i);
+ }
+ }
+ }
+
+ bool CompoundMatrix::HasValidNumbersImpl() const
+ {
+ if (!matrices_valid_) {
+ matrices_valid_ = MatricesValid();
+ }
+ DBG_ASSERT(matrices_valid_);
+
+ for( Index irow = 0; irow < NComps_Rows(); irow++ ) {
+ for( Index jcol = 0; jcol < NComps_Cols(); jcol++ ) {
+ if ( (owner_space_->Diagonal() && irow == jcol)
+ || (!owner_space_->Diagonal() && ConstComp(irow,jcol)) ) {
+ if (!ConstComp(irow, jcol)->HasValidNumbers()) {
+ return false;
+ }
+ }
+ }
+ }
+ return true;
+ }
+
+ void CompoundMatrix::PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ jnlst.Printf(level, category, "\n");
+ jnlst.PrintfIndented(level, category, indent,
+ "%sCompoundMatrix \"%s\" with %d row and %d columns components:\n",
+ prefix.c_str(), name.c_str(),
+ NComps_Rows(), NComps_Cols());
+ for (Index irow = 0; irow < NComps_Rows(); irow++ ) {
+ for (Index jcol = 0; jcol < NComps_Cols(); jcol++ ) {
+ jnlst.PrintfIndented(level, category, indent,
+ "%sComponent for row %d and column %d:\n",
+ prefix.c_str(), irow, jcol);
+ if (ConstComp(irow, jcol)) {
+ DBG_ASSERT(name.size()<200);
+ char buffer[256];
+ sprintf(buffer, "%s[%2d][%2d]", name.c_str(), irow, jcol);
+ std::string term_name = buffer;
+ ConstComp(irow, jcol)->Print(&jnlst, level, category, term_name,
+ indent+1, prefix);
+ }
+ else {
+ jnlst.PrintfIndented(level, category, indent,
+ "%sComponent has not been set.\n",
+ prefix.c_str());
+ }
+ }
+ }
+ }
+
+ bool CompoundMatrix::MatricesValid() const
+ {
+ // Check to make sure we have matrices everywhere the space has matrices
+ // We already check that the matrix agrees with the block space
+ // in the SetComp methods
+ bool retValue = true;
+ for (Index i=0; i<NComps_Rows(); i++) {
+ for (Index j=0; j<NComps_Cols(); j++) {
+ if ( (!ConstComp(i,j) && IsValid(owner_space_->GetCompSpace(i,j)))
+ || (ConstComp(i,j) && IsNull(owner_space_->GetCompSpace(i,j))) ) {
+ retValue = false;
+ break;
+ }
+ }
+ }
+ return retValue;
+ }
+
+
+ CompoundMatrixSpace::CompoundMatrixSpace(Index ncomps_rows, Index ncomps_cols, Index total_nRows, Index total_nCols)
+ :
+ MatrixSpace(total_nRows, total_nCols),
+ ncomps_rows_(ncomps_rows),
+ ncomps_cols_(ncomps_cols),
+ dimensions_set_(false),
+ block_rows_(ncomps_rows, -1),
+ block_cols_(ncomps_cols, -1),
+ diagonal_(false)
+ {
+ DBG_START_METH("CompoundMatrixSpace::CompoundMatrixSpace", 0);
+ std::vector<SmartPtr<const MatrixSpace> > row(ncomps_cols_);
+ std::vector< bool > allocate_row(ncomps_cols_, false);
+ for (Index i=0; i<ncomps_rows_; i++) {
+ DBG_PRINT((1, "block_rows_[%d] = %d\n", i, block_rows_[i]));
+ comp_spaces_.push_back(row);
+ allocate_block_.push_back(allocate_row);
+ }
+ }
+
+ void CompoundMatrixSpace::SetBlockCols(Index jcol, Index ncols)
+ {
+ DBG_ASSERT(!dimensions_set_ && "for now, if the dimensions have all been set, they cannot be changed");
+ DBG_ASSERT(jcol < ncomps_cols_);
+ DBG_ASSERT(block_cols_[jcol] == -1 && "This dimension has already been set - sanity check");
+ block_cols_[jcol] = ncols;
+ }
+
+ void CompoundMatrixSpace::SetBlockRows(Index irow, Index nrows)
+ {
+ DBG_ASSERT(!dimensions_set_ && "for now, if the dimensions have all been set, they cannot be changed");
+ DBG_ASSERT(irow < ncomps_rows_);
+ DBG_ASSERT(block_rows_[irow] == -1 && "This dimension has already been set - sanity check");
+ block_rows_[irow] = nrows;
+ }
+
+ Index CompoundMatrixSpace::GetBlockRows(Index irow) const
+ {
+ DBG_ASSERT(dimensions_set_);
+ DBG_ASSERT(irow < ncomps_rows_);
+ return block_rows_[irow];
+ }
+
+ Index CompoundMatrixSpace::GetBlockCols(Index jcol) const
+ {
+ DBG_ASSERT(dimensions_set_);
+ DBG_ASSERT(jcol < ncomps_cols_);
+ return block_cols_[jcol];
+ }
+
+ void CompoundMatrixSpace::SetCompSpace(Index irow, Index jcol,
+ const MatrixSpace& mat_space,
+ bool auto_allocate /*=false*/)
+ {
+ if (!dimensions_set_) {
+ dimensions_set_ = DimensionsSet();
+ }
+ DBG_ASSERT(dimensions_set_);
+ DBG_ASSERT(irow<ncomps_rows_);
+ DBG_ASSERT(jcol<ncomps_cols_);
+ DBG_ASSERT(IsNull(comp_spaces_[irow][jcol]));
+ DBG_ASSERT(block_cols_[jcol] != -1 && block_cols_[jcol] == mat_space.NCols());
+ DBG_ASSERT(block_rows_[irow] != -1 && block_rows_[irow] == mat_space.NRows());
+
+ comp_spaces_[irow][jcol] = &mat_space;
+ allocate_block_[irow][jcol] = auto_allocate;
+
+ diagonal_ = true;
+ for (Index i=0; i < NComps_Rows(); i++) {
+ for (Index j=0; j < NComps_Cols(); j++) {
+ if ( (i == j && IsNull(GetCompSpace(i,j)))
+ || (i != j && IsValid(GetCompSpace(i,j)))) {
+ diagonal_ = false;
+ break;
+ }
+ }
+ }
+ }
+
+ CompoundMatrix* CompoundMatrixSpace::MakeNewCompoundMatrix() const
+ {
+ if (!dimensions_set_) {
+ dimensions_set_ = DimensionsSet();
+ }
+ DBG_ASSERT(dimensions_set_);
+
+ CompoundMatrix* mat = new CompoundMatrix(this);
+ for(Index i=0; i<ncomps_rows_; i++) {
+ for (Index j=0; j<ncomps_cols_; j++) {
+ if (allocate_block_[i][j]) {
+ mat->SetCompNonConst(i, j, *GetCompSpace(i, j)->MakeNew());
+ }
+ }
+ }
+
+ return mat;
+ }
+
+ bool CompoundMatrixSpace::DimensionsSet() const
+ {
+ DBG_START_METH("CompoundMatrixSpace::DimensionsSet", 0);
+ Index total_nrows = 0;
+ Index total_ncols = 0;
+ bool valid = true;
+ for (Index i=0; i<ncomps_rows_; i++) {
+ if (block_rows_[i] == -1) {
+ valid = false;
+ break;
+ }
+ total_nrows += block_rows_[i];
+ }
+ if (valid) {
+ for (Index j=0; j<ncomps_cols_; j++) {
+ if (block_cols_[j] == -1) {
+ valid = false;
+ break;
+ }
+ total_ncols += block_cols_[j];
+ }
+ }
+
+ if (valid) {
+ DBG_ASSERT(total_nrows == NRows() && total_ncols == NCols());
+ }
+
+ return valid;
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpCompoundMatrix.hpp b/SimTKmath/Optimizers/src/IpOpt/IpCompoundMatrix.hpp
new file mode 100644
index 0000000..f9761ce
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpCompoundMatrix.hpp
@@ -0,0 +1,337 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpCompoundMatrix.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPCOMPOUNDMATRIX_HPP__
+#define __IPCOMPOUNDMATRIX_HPP__
+
+#include "IpUtils.hpp"
+#include "IpMatrix.hpp"
+
+namespace Ipopt
+{
+
+ /* forward declarations */
+ class CompoundMatrixSpace;
+
+ /** Class for Matrices consisting of other matrices. This matrix is
+ * a matrix that consists of zero, one or more Matrices's which are
+ * arranged like this: \f$ M_{\rm compound} =
+ * \left(\begin{array}{cccc}M_{00} & M_{01} & \ldots & M_{0,{\rm
+ * ncomp\_cols}-1} \\ \dots &&&\dots \\ M_{{\rm ncomp\_rows}-1,0} &
+ * M_{{\rm ncomp\_rows}-1,1} & \dots & M_{{\rm ncomp\_rows}-1,{\rm
+ * ncomp\_cols}-1}\end{array}\right)\f$. The individual components
+ * can be associated to different MatrixSpaces. The individual
+ * components can also be const and non-const Matrices. If a
+ * component is not set (i.e., it's pointer is NULL), then this
+ * components is treated like a zero-matrix of appropriate
+ * dimensions.
+ */
+ class CompoundMatrix : public Matrix
+ {
+ public:
+
+ /**@name Constructors / Destructors */
+ //@{
+
+ /** Constructor, taking the owner_space. The owner_space has to
+ * be defined, so that at each block row and column contain at
+ * least one non-NULL component. The individual components can
+ * be set afterwards with the SeteComp and SetCompNonConst
+ * methods.
+ */
+ CompoundMatrix(const CompoundMatrixSpace* owner_space);
+
+ /** Destructor */
+ ~CompoundMatrix();
+ //@}
+
+ /** Method for setting an individual component at position (irow,
+ * icol) in the compound matrix. The counting of indices starts
+ * at 0. */
+ void SetComp(Index irow, Index jcol, const Matrix& matrix);
+
+ /** Method to set a non-const Matrix entry */
+ void SetCompNonConst(Index irow, Index jcol, Matrix& matrix);
+
+ /** Method to create a new matrix from the space for this block */
+ void CreateBlockFromSpace(Index irow, Index jcol);
+
+ /** Method for retrieving one block from the compound matrix as a
+ * const Matrix.
+ */
+ SmartPtr<const Matrix> GetComp(Index irow, Index jcol) const
+ {
+ return ConstComp(irow, jcol);
+ }
+
+ /** Method for retrieving one block from the compound matrix as a
+ * non-const Matrix. Note that calling this method with mark the
+ * CompoundMatrix as changed. Therefore, only use this method if
+ * you are intending to change the Matrix that you receive. */
+ SmartPtr<Matrix> GetCompNonConst(Index irow, Index jcol)
+ {
+ ObjectChanged();
+ return Comp(irow, jcol);
+ }
+
+ /** Number of block rows of this compound matrix. */
+ inline Index NComps_Rows() const;
+ /** Number of block colmuns of this compound matrix. */
+ inline Index NComps_Cols() const;
+
+ protected:
+ /**@name Methods overloaded from Matrix */
+ //@{
+ virtual void MultVectorImpl(Number alpha, const Vector& x,
+ Number beta, Vector& y) const;
+
+ virtual void TransMultVectorImpl(Number alpha, const Vector& x,
+ Number beta, Vector& y) const;
+
+ /** X = beta*X + alpha*(Matrix S^{-1} Z). Specialized implementation.
+ */
+ virtual void AddMSinvZImpl(Number alpha, const Vector& S, const Vector& Z,
+ Vector& X) const;
+
+ /** X = S^{-1} (r + alpha*Z*M^Td). Specialized implementation.
+ */
+ virtual void SinvBlrmZMTdBrImpl(Number alpha, const Vector& S,
+ const Vector& R, const Vector& Z,
+ const Vector& D, Vector& X) const;
+
+ /** Method for determining if all stored numbers are valid (i.e.,
+ * no Inf or Nan). */
+ virtual bool HasValidNumbersImpl() const;
+
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const;
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ CompoundMatrix();
+
+ /** Copy Constructor */
+ CompoundMatrix(const CompoundMatrix&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const CompoundMatrix&);
+ //@}
+
+ /** Matrix of matrix's containing the components */
+ std::vector<std::vector<SmartPtr<Matrix> > > comps_;
+
+ /** Matrix of const matrix's containing the components */
+ std::vector<std::vector<SmartPtr<const Matrix> > > const_comps_;
+
+ /** Copy of the owner_space ptr as a CompoundMatrixSpace instead
+ * of MatrixSpace */
+ const CompoundMatrixSpace* owner_space_;
+
+ /** boolean indicating if the compound matrix is in a "valid" state */
+ mutable bool matrices_valid_;
+
+ /** Method to check whether or not the matrices are valid */
+ bool MatricesValid() const;
+
+ inline const Matrix* ConstComp(Index irow, Index jcol) const;
+
+ inline Matrix* Comp(Index irow, Index jcol);
+ };
+
+ /** This is the matrix space for CompoundMatrix. Before a CompoundMatrix
+ * can be created, at least one MatrixSpace has to be set per block
+ * row and column. Individual component MatrixSpace's can be set
+ * with the SetComp method.
+ */
+ class CompoundMatrixSpace : public MatrixSpace
+ {
+ public:
+ /** @name Constructors / Destructors */
+ //@{
+ /** Constructor, given the number of row and columns blocks, as
+ * well as the totel number of rows and columns.
+ */
+ CompoundMatrixSpace(Index ncomps_rows,
+ Index ncomps_cols,
+ Index total_nRows,
+ Index total_nCols);
+
+ /** Destructor */
+ ~CompoundMatrixSpace()
+ {}
+ ;
+ //@}
+
+ /** @name Methods for setting information about the components. */
+ //@{
+ /** Set the number nrows of rows in row-block number irow. */
+ void SetBlockRows(Index irow, Index nrows);
+
+ /** Set the number ncols of columns in column-block number jcol. */
+ void SetBlockCols(Index jcol, Index ncols);
+
+ /** Get the number nrows of rows in row-block number irow. */
+ Index GetBlockRows(Index irow) const;
+
+ /** Set the number ncols of columns in column-block number jcol. */
+ Index GetBlockCols(Index jcol) const;
+
+ /** Set the component MatrixSpace. If auto_allocate is true, then
+ * a new CompoundMatrix created later with MakeNew will have this
+ * component automatically created with the Matrix's MakeNew.
+ * Otherwise, the corresponding component will be NULL and has to
+ * be set with the SetComp methods of the CompoundMatrix.
+ */
+ void SetCompSpace(Index irow, Index jcol,
+ const MatrixSpace& mat_space,
+ bool auto_allocate = false);
+ //@}
+
+ /** Obtain the component MatrixSpace in block row irow and block
+ * column jcol.
+ */
+ SmartPtr<const MatrixSpace> GetCompSpace(Index irow, Index jcol) const
+ {
+ DBG_ASSERT(irow<NComps_Rows());
+ DBG_ASSERT(jcol<NComps_Cols());
+ return comp_spaces_[irow][jcol];
+ }
+
+ /** @name Accessor methods */
+ //@{
+ /** Number of block rows */
+ Index NComps_Rows() const
+ {
+ return ncomps_rows_;
+ }
+ /** Number of block columns */
+ Index NComps_Cols() const
+ {
+ return ncomps_cols_;
+ }
+
+ /** True if the blocks lie on the diagonal - can make some operations faster */
+ bool Diagonal() const
+ {
+ return diagonal_;
+ }
+ //@}
+
+ /** Method for creating a new matrix of this specific type. */
+ CompoundMatrix* MakeNewCompoundMatrix() const;
+
+ /** Overloaded MakeNew method for the MatrixSpace base class.
+ */
+ virtual Matrix* MakeNew() const
+ {
+ return MakeNewCompoundMatrix();
+ }
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default constructor */
+ CompoundMatrixSpace();
+
+ /** Copy Constructor */
+ CompoundMatrixSpace(const CompoundMatrixSpace&);
+
+ /** Overloaded Equals Operator */
+ CompoundMatrixSpace& operator=(const CompoundMatrixSpace&);
+ //@}
+
+ /** Number of block rows */
+ Index ncomps_rows_;
+
+ /** Number of block columns */
+ Index ncomps_cols_;
+
+ /** Store whether or not the dimensions are valid */
+ mutable bool dimensions_set_;
+
+ /** 2-dim std::vector of matrix spaces for the components */
+ std::vector<std::vector<SmartPtr<const MatrixSpace> > > comp_spaces_;
+
+ /** 2-dim std::vector of booleans deciding whether to
+ * allocate a new matrix for the blocks automagically */
+ std::vector<std::vector< bool > > allocate_block_;
+
+ /** Vector of the number of rows in each comp column */
+ std::vector<Index> block_rows_;
+
+ /** Vector of the number of cols in each comp row */
+ std::vector<Index> block_cols_;
+
+ /** true if the CompoundMatrixSpace only has Matrix spaces along the diagonal.
+ * this means that the CompoundMatrix will only have matrices along the
+ * diagonal and it could make some operations more efficient
+ */
+ bool diagonal_;
+
+ /** Auxilliary function for debugging to set if all block
+ * dimensions have been set. */
+ bool DimensionsSet() const;
+ };
+
+ /* inline methods */
+ inline
+ Index CompoundMatrix::NComps_Rows() const
+ {
+ return owner_space_->NComps_Rows();
+ }
+
+ inline
+ Index CompoundMatrix::NComps_Cols() const
+ {
+ return owner_space_->NComps_Cols();
+ }
+
+ inline
+ const Matrix* CompoundMatrix::ConstComp(Index irow, Index jcol) const
+ {
+ DBG_ASSERT(irow < NComps_Rows());
+ DBG_ASSERT(jcol < NComps_Cols());
+ if (IsValid(comps_[irow][jcol])) {
+ return GetRawPtr(comps_[irow][jcol]);
+ }
+ else if (IsValid(const_comps_[irow][jcol])) {
+ return GetRawPtr(const_comps_[irow][jcol]);
+ }
+
+ return NULL;
+ }
+
+ inline
+ Matrix* CompoundMatrix::Comp(Index irow, Index jcol)
+ {
+ DBG_ASSERT(irow < NComps_Rows());
+ DBG_ASSERT(jcol < NComps_Cols());
+ return GetRawPtr(comps_[irow][jcol]);
+ }
+
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpCompoundSymMatrix.cpp b/SimTKmath/Optimizers/src/IpOpt/IpCompoundSymMatrix.cpp
new file mode 100644
index 0000000..93f1212
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpCompoundSymMatrix.cpp
@@ -0,0 +1,314 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpCompoundSymMatrix.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpoptConfig.h"
+#include "IpCompoundSymMatrix.hpp"
+#include "IpCompoundVector.hpp"
+
+#ifdef HAVE_CSTDIO
+# include <cstdio>
+#else
+# ifdef HAVE_STDIO_H
+# include <stdio.h>
+# else
+# error "don't have header file for stdio"
+# endif
+#endif
+
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+
+
+
+
+namespace Ipopt
+{
+
+ CompoundSymMatrix::CompoundSymMatrix(const CompoundSymMatrixSpace* owner_space)
+ :
+ SymMatrix(owner_space),
+ owner_space_(owner_space),
+ matrices_valid_(false)
+ {
+ for (Index irow=0; irow<NComps_Dim(); irow++) {
+ std::vector< SmartPtr<Matrix> > row(irow+1);
+ std::vector< SmartPtr<const Matrix> > const_row(irow+1);
+ comps_.push_back(row);
+ const_comps_.push_back(const_row);
+ }
+ }
+
+ CompoundSymMatrix::~CompoundSymMatrix()
+ {}
+
+ void CompoundSymMatrix::SetComp(Index irow, Index jcol,
+ const Matrix& matrix)
+ {
+ DBG_ASSERT(!matrices_valid_);
+ DBG_ASSERT(irow<NComps_Dim());
+ DBG_ASSERT(jcol<=irow);
+ // Matrices on the diagonal must be symmetric
+ DBG_ASSERT(irow!=jcol || dynamic_cast<const SymMatrix*>(&matrix));
+ DBG_ASSERT(owner_space_->GetCompSpace(irow, jcol)->IsMatrixFromSpace(matrix));
+
+ comps_[irow][jcol] = NULL;
+ const_comps_[irow][jcol] = &matrix;
+ ObjectChanged();
+ }
+
+ void CompoundSymMatrix::SetCompNonConst(Index irow, Index jcol,
+ Matrix& matrix)
+ {
+ DBG_ASSERT(!matrices_valid_);
+ DBG_ASSERT(irow < NComps_Dim());
+ DBG_ASSERT(jcol <= irow);
+ // Matrices on the diagonal must be symmetric
+ DBG_ASSERT( irow != jcol || dynamic_cast<SymMatrix*>(&matrix));
+ DBG_ASSERT(owner_space_->GetCompSpace(irow, jcol)->IsMatrixFromSpace(matrix));
+
+ const_comps_[irow][jcol] = NULL;
+ comps_[irow][jcol] = &matrix;
+ ObjectChanged();
+ }
+
+ Index CompoundSymMatrix::NComps_Dim() const
+ {
+ return owner_space_->NComps_Dim();
+ }
+
+ void CompoundSymMatrix::MultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ if (!matrices_valid_) {
+ matrices_valid_ = MatricesValid();
+ }
+ DBG_ASSERT(matrices_valid_);
+
+ // The vectors are assumed to be compound Vectors as well
+ const CompoundVector* comp_x = dynamic_cast<const CompoundVector*>(&x);
+ CompoundVector* comp_y = dynamic_cast<CompoundVector*>(&y);
+
+ // A few sanity checks
+ if (comp_x) {
+ DBG_ASSERT(NComps_Dim()==comp_x->NComps());
+ }
+ else {
+ DBG_ASSERT(NComps_Dim() == 1);
+ }
+ if (comp_y) {
+ DBG_ASSERT(NComps_Dim()==comp_y->NComps());
+ }
+ else {
+ DBG_ASSERT(NComps_Dim() == 1);
+ }
+
+ // Take care of the y part of the addition
+ if( beta!=0.0 ) {
+ y.Scal(beta);
+ }
+ else {
+ y.Set(0.0); // In case y hasn't been initialized yet
+ }
+
+ for (Index irow=0; irow<NComps_Dim(); irow++) {
+ SmartPtr<Vector> y_i;
+ if (comp_y) {
+ y_i = comp_y->GetCompNonConst(irow);
+ }
+ else {
+ y_i = &y;
+ }
+ DBG_ASSERT(IsValid(y_i));
+
+ for (Index jcol=0; jcol<=irow; jcol++) {
+ SmartPtr<const Vector> x_j;
+ if (comp_x) {
+ x_j = comp_x->GetComp(irow);
+ }
+ else {
+ x_j = &x;
+ }
+ DBG_ASSERT(IsValid(x_j));
+
+ if (ConstComp(irow,jcol)) {
+ ConstComp(irow,jcol)->MultVector(alpha, *comp_x->GetComp(jcol),
+ 1., *comp_y->GetCompNonConst(irow));
+ }
+ }
+
+ for (Index jcol = irow+1; jcol < NComps_Dim(); jcol++) {
+ if (ConstComp(jcol,irow)) {
+ ConstComp(jcol,irow)->TransMultVector(alpha, *comp_x->GetComp(jcol),
+ 1., *comp_y->GetCompNonConst(irow));
+ }
+ }
+ }
+ }
+
+ bool CompoundSymMatrix::HasValidNumbersImpl() const
+ {
+ if (!matrices_valid_) {
+ matrices_valid_ = MatricesValid();
+ }
+ DBG_ASSERT(matrices_valid_);
+
+ for (Index irow=0; irow<NComps_Dim(); irow++) {
+ for (Index jcol=0; jcol<=irow; jcol++) {
+ if (ConstComp(irow,jcol)) {
+ if (!ConstComp(irow,jcol)->HasValidNumbers()) {
+ return false;
+ }
+ }
+ }
+ }
+ return true;
+ }
+
+ void CompoundSymMatrix::PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ jnlst.Printf(level, category, "\n");
+ jnlst.PrintfIndented(level, category, indent,
+ "%sCompoundSymMatrix \"%s\" with %d rows and columns components:\n",
+ prefix.c_str(), name.c_str(), NComps_Dim());
+ for (Index irow = 0; irow < NComps_Dim(); irow++ ) {
+ for (Index jcol = 0; jcol <= irow; jcol++ ) {
+ jnlst.PrintfIndented(level, category, indent,
+ "%sComponent for row %d and column %d:\n",
+ prefix.c_str(), irow, jcol);
+ if (ConstComp(irow,jcol)) {
+ DBG_ASSERT(name.size()<200);
+ char buffer[256];
+ sprintf(buffer, "%s[%d][%d]", name.c_str(), irow, jcol);
+ std::string term_name = buffer;
+ ConstComp(irow,jcol)->Print(&jnlst, level, category, term_name,
+ indent+1, prefix);
+ }
+ else {
+ jnlst.PrintfIndented(level, category, indent,
+ "%sThis component has not been set.\n",
+ prefix.c_str());
+ }
+ }
+ }
+ }
+
+ bool CompoundSymMatrix::MatricesValid() const
+ {
+ // Check to make sure we have matrices everywhere the space has matrices
+ // We already check that the matrix agrees with the block space
+ // in the SetComp methods
+ bool retValue = true;
+ for (Index i=0; i<NComps_Dim(); i++) {
+ for (Index j=0; j<=i; j++) {
+ if ( (!ConstComp(i, j) && IsValid(owner_space_->GetCompSpace(i,j)))
+ || (ConstComp(i, j) && IsNull(owner_space_->GetCompSpace(i,j))) ) {
+ retValue = false;
+ break;
+ }
+ }
+ }
+
+ return retValue;
+ }
+
+ CompoundSymMatrixSpace::CompoundSymMatrixSpace(Index ncomp_spaces, Index total_dim)
+ :
+ SymMatrixSpace(total_dim),
+ ncomp_spaces_(ncomp_spaces),
+ block_dim_(ncomp_spaces, -1),
+ dimensions_set_(false)
+ {
+ for (Index irow=0; irow<ncomp_spaces_; irow++) {
+ std::vector<SmartPtr<const MatrixSpace> > row(irow+1);
+ std::vector< bool > allocate_row(irow+1, false);
+ comp_spaces_.push_back(row);
+ allocate_block_.push_back(allocate_row);
+ }
+ }
+
+ void CompoundSymMatrixSpace::SetBlockDim(Index irow_jcol, Index dim)
+ {
+ DBG_ASSERT(!dimensions_set_ && "for now, if dimensions are set, they cannot be changed");
+ DBG_ASSERT(block_dim_[irow_jcol] == -1 && "This dimension has already been set - sanity check");
+ DBG_ASSERT(irow_jcol < ncomp_spaces_);
+ block_dim_[irow_jcol] = dim;
+ }
+
+ Index CompoundSymMatrixSpace::GetBlockDim(Index irow_jcol) const
+ {
+ DBG_ASSERT(dimensions_set_ && "Cannot get block dimensions before all dimensions are set.");
+ DBG_ASSERT(irow_jcol < ncomp_spaces_);
+ return block_dim_[irow_jcol];
+ }
+
+ void CompoundSymMatrixSpace::SetCompSpace(Index irow, Index jcol,
+ const MatrixSpace& mat_space,
+ bool auto_allocate /*=false*/)
+ {
+ if (!dimensions_set_) {
+ dimensions_set_ = DimensionsSet();
+ }
+ DBG_ASSERT(dimensions_set_);
+ DBG_ASSERT(irow<ncomp_spaces_);
+ DBG_ASSERT(jcol<=irow);
+ DBG_ASSERT(IsNull(comp_spaces_[irow][jcol]));
+ DBG_ASSERT(irow!=jcol || dynamic_cast<const SymMatrixSpace*> (&mat_space));
+ DBG_ASSERT(block_dim_[jcol] != -1 && block_dim_[jcol] == mat_space.NCols());
+ DBG_ASSERT(block_dim_[irow] != -1 && block_dim_[irow] == mat_space.NRows());
+
+ comp_spaces_[irow][jcol] = &mat_space;
+ allocate_block_[irow][jcol] = auto_allocate;
+ }
+
+ CompoundSymMatrix* CompoundSymMatrixSpace::MakeNewCompoundSymMatrix() const
+ {
+ if (!dimensions_set_) {
+ dimensions_set_ = DimensionsSet();
+ }
+ DBG_ASSERT(dimensions_set_);
+
+ CompoundSymMatrix* mat = new CompoundSymMatrix(this);
+ for(Index i=0; i<NComps_Dim(); i++) {
+ for (Index j=0; j<=i; j++) {
+ if (allocate_block_[i][j]) {
+ mat->SetCompNonConst(i, j, *GetCompSpace(i, j)->MakeNew());
+ }
+ }
+ }
+
+ return mat;
+ }
+
+ bool CompoundSymMatrixSpace::DimensionsSet() const
+ {
+ Index total_dim= 0;
+ bool valid = true;
+ for (Index i=0; i<ncomp_spaces_; i++) {
+ if (block_dim_[i] == -1) {
+ valid = false;
+ break;
+ }
+ total_dim += block_dim_[i];
+ }
+
+ if (valid) {
+ DBG_ASSERT(total_dim == Dim());
+ }
+
+ return valid;
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpCompoundSymMatrix.hpp b/SimTKmath/Optimizers/src/IpOpt/IpCompoundSymMatrix.hpp
new file mode 100644
index 0000000..2601529
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpCompoundSymMatrix.hpp
@@ -0,0 +1,281 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpCompoundSymMatrix.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPCOMPOUNDSYMMATRIX_HPP__
+#define __IPCOMPOUNDSYMMATRIX_HPP__
+
+#include "IpUtils.hpp"
+#include "IpSymMatrix.hpp"
+
+namespace Ipopt
+{
+
+ /* forward declarations */
+ class CompoundSymMatrixSpace;
+
+ /** Class for symmetric matrices consisting of other matrices.
+ * Here, the lower left block of the matrix is stored.
+ */
+ class CompoundSymMatrix : public SymMatrix
+ {
+ public:
+
+ /**@name Constructors / Destructors */
+ //@{
+
+ /** Constructor, taking only the number for block components into the
+ * row and column direction. The owner_space has to be defined, so
+ * that at each block row and column contain at least one non-NULL
+ * component.
+ */
+ CompoundSymMatrix(const CompoundSymMatrixSpace* owner_space);
+
+ /** Destructor */
+ ~CompoundSymMatrix();
+ //@}
+
+ /** Method for setting an individual component at position (irow,
+ * icol) in the compound matrix. The counting of indices starts
+ * at 0. Since this only the lower left components are stored, we need
+ * to have jcol<=irow, and if irow==jcol, the matrix must be a SymMatrix */
+ void SetComp(Index irow, Index jcol, const Matrix& matrix);
+
+ /** Non const version of the same method */
+ void SetCompNonConst(Index irow, Index jcol, Matrix& matrix);
+
+ /** Method for retrieving one block from the compound matrix.
+ * Since this only the lower left components are stored, we need
+ * to have jcol<=irow */
+ SmartPtr<const Matrix> GetComp(Index irow, Index jcol) const
+ {
+ return ConstComp(irow,jcol);
+ }
+
+ /** Non const version of GetComp. You should only use this method
+ * if you are intending to change the matrix you receive, since
+ * this CompoundSymMatrix will be marked as changed. */
+ SmartPtr<Matrix> GetCompNonConst(Index irow, Index jcol)
+ {
+ ObjectChanged();
+ return Comp(irow,jcol);
+ }
+
+ /** Method for creating a new matrix of this specific type. */
+ SmartPtr<CompoundSymMatrix> MakeNewCompoundSymMatrix() const;
+
+ // The following don't seem to be necessary
+ /* Number of block rows of this compound matrix. */
+ // Index NComps_NRows() const { return NComps_Dim(); }
+
+ /* Number of block colmuns of this compound matrix. */
+ // Index NComps_NCols() const { return NComps_Dim(); }
+
+ /** Number of block rows and columns */
+ Index NComps_Dim() const;
+
+ protected:
+ /**@name Methods overloaded from matrix */
+ //@{
+ virtual void MultVectorImpl(Number alpha, const Vector& x,
+ Number beta, Vector& y) const;
+
+ /** Method for determining if all stored numbers are valid (i.e.,
+ * no Inf or Nan). */
+ virtual bool HasValidNumbersImpl() const;
+
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const;
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ CompoundSymMatrix();
+
+ /** Copy Constructor */
+ CompoundSymMatrix(const CompoundSymMatrix&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const CompoundSymMatrix&);
+ //@}
+
+ /** Vector of vectors containing the components */
+ std::vector<std::vector<SmartPtr<Matrix> > > comps_;
+
+ /** Vector of vectors containing the const components */
+ std::vector<std::vector<SmartPtr<const Matrix> > > const_comps_;
+
+ /** Copy of the owner_space ptr as a CompoundSymMatrixSpace */
+ const CompoundSymMatrixSpace* owner_space_;
+
+ /** boolean indicating if the compound matrix is in a "valid" state */
+ mutable bool matrices_valid_;
+
+ /** method to check wether or not the matrices are valid */
+ bool MatricesValid() const;
+
+ /** Internal method to return a const pointer to one of the comps */
+ const Matrix* ConstComp(Index irow, Index jcol) const
+ {
+ DBG_ASSERT(irow < NComps_Dim());
+ DBG_ASSERT(jcol <= irow);
+ if (IsValid(comps_[irow][jcol])) {
+ return GetRawPtr(comps_[irow][jcol]);
+ }
+ else if (IsValid(const_comps_[irow][jcol])) {
+ return GetRawPtr(const_comps_[irow][jcol]);
+ }
+
+ return NULL;
+ }
+
+ /** Internal method to return a non-const pointer to one of the comps */
+ Matrix* Comp(Index irow, Index jcol)
+ {
+ DBG_ASSERT(irow < NComps_Dim());
+ DBG_ASSERT(jcol <= irow);
+ // We shouldn't be asking for a non-const if this entry holds a
+ // const one...
+ DBG_ASSERT(IsNull(const_comps_[irow][jcol]));
+ if (IsValid(comps_[irow][jcol])) {
+ return GetRawPtr(comps_[irow][jcol]);
+ }
+
+ return NULL;
+ }
+ };
+
+ /** This is the matrix space for CompoundSymMatrix. Before a
+ * CompoundSymMatrix can be created, at least one SymMatrixSpace has
+ * to be set per block row and column. Individual component
+ * SymMatrixSpace's can be set with the SetComp method.
+ */
+ class CompoundSymMatrixSpace : public SymMatrixSpace
+ {
+ public:
+ /** @name Constructors / Destructors */
+ //@{
+ /** Constructor, given the number of blocks (same for rows and
+ * columns), as well as the total dimension of the matrix.
+ */
+ CompoundSymMatrixSpace(Index ncomp_spaces, Index total_dim);
+
+ /** Destructor */
+ ~CompoundSymMatrixSpace()
+ {}
+ //@}
+
+ /** @name Methods for setting information about the components. */
+ //@{
+ /** Set the dimension dim for block row (or column) irow_jcol */
+ void SetBlockDim(Index irow_jcol, Index dim);
+
+ /** Get the dimension dim for block row (or column) irow_jcol */
+ Index GetBlockDim(Index irow_jcol) const;
+
+ /** Set the component SymMatrixSpace. If auto_allocate is true, then
+ * a new CompoundSymMatrix created later with MakeNew will have this
+ * component automatically created with the SymMatrix's MakeNew.
+ * Otherwise, the corresponding component will be NULL and has to
+ * be set with the SetComp methods of the CompoundSymMatrix.
+ */
+ void SetCompSpace(Index irow, Index jcol,
+ const MatrixSpace& mat_space,
+ bool auto_allocate = false);
+ //@}
+
+ /** Obtain the component MatrixSpace in block row irow and block
+ * column jcol.
+ */
+ SmartPtr<const MatrixSpace> GetCompSpace(Index irow, Index jcol) const
+ {
+ DBG_ASSERT(irow<ncomp_spaces_);
+ DBG_ASSERT(jcol<=irow);
+ return comp_spaces_[irow][jcol];
+ }
+
+ /** @name Accessor methods */
+ //@{
+ Index NComps_Dim() const
+ {
+ return ncomp_spaces_;
+ }
+ //@}
+
+ /** Method for creating a new matrix of this specific type. */
+ CompoundSymMatrix* MakeNewCompoundSymMatrix() const;
+
+ /** Overloaded MakeNew method for the SymMatrixSpace base class.
+ */
+ virtual SymMatrix* MakeNewSymMatrix() const
+ {
+ return MakeNewCompoundSymMatrix();
+ }
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default constructor */
+ CompoundSymMatrixSpace();
+
+ /** Copy Constructor */
+ CompoundSymMatrixSpace(const CompoundSymMatrix&);
+
+ /** Overloaded Equals Operator */
+ CompoundSymMatrixSpace& operator=(const CompoundSymMatrixSpace&);
+ //@}
+
+ /** Number of components per row and column */
+ Index ncomp_spaces_;
+
+ /** Vector of the number of rows in each comp column,
+ * Since this is symmetric, this is also the number
+ * of columns in each row, hence, it is the dimension
+ * each of the diagonals */
+ std::vector<Index> block_dim_;
+
+ /** 2-dim std::vector of matrix spaces for the components. Only
+ * the lower right part is stored. */
+ std::vector<std::vector<SmartPtr<const MatrixSpace> > > comp_spaces_;
+
+ /** 2-dim std::vector of booleans deciding whether to
+ * allocate a new matrix for the blocks automagically */
+ std::vector<std::vector< bool > > allocate_block_;
+
+ /** boolean indicating if the compound matrix space is in a "valid" state */
+ mutable bool dimensions_set_;
+
+ /** Method to check whether or not the spaces are valid */
+ bool DimensionsSet() const;
+ };
+
+ inline
+ SmartPtr<CompoundSymMatrix> CompoundSymMatrix::MakeNewCompoundSymMatrix() const
+ {
+ return owner_space_->MakeNewCompoundSymMatrix();
+ }
+
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpCompoundVector.cpp b/SimTKmath/Optimizers/src/IpOpt/IpCompoundVector.cpp
new file mode 100644
index 0000000..1853722
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpCompoundVector.cpp
@@ -0,0 +1,467 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpCompoundVector.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpoptConfig.h"
+#include "IpCompoundVector.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+
+
+
+
+#ifdef HAVE_CSTDIO
+# include <cstdio>
+#else
+# ifdef HAVE_STDIO_H
+# include <stdio.h>
+# else
+# error "don't have header file for stdio"
+# endif
+#endif
+
+#include <limits>
+
+namespace Ipopt
+{
+
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ CompoundVector::CompoundVector(const CompoundVectorSpace* owner_space, bool create_new)
+ :
+ Vector(owner_space),
+ comps_(owner_space->NCompSpaces()),
+ const_comps_(owner_space->NCompSpaces()),
+ owner_space_(owner_space),
+ vectors_valid_(false)
+ {
+ Index dim_check = 0;
+ for (Index i=0; i<NComps(); i++) {
+ SmartPtr<const VectorSpace> space = owner_space_->GetCompSpace(i);
+ DBG_ASSERT(IsValid(space));
+ dim_check += space->Dim();
+
+ if (create_new) {
+ comps_[i] = space->MakeNew();
+ }
+ }
+
+ DBG_ASSERT(dim_check == Dim());
+
+ if (create_new) {
+ vectors_valid_ = VectorsValid();
+ }
+ }
+
+ CompoundVector::~CompoundVector()
+ {
+ // ToDo: Do we need an empty here?
+ }
+
+ void CompoundVector::SetComp(Index icomp, const Vector& vec)
+ {
+ DBG_ASSERT(icomp<NComps());
+ comps_[icomp] = NULL;
+ const_comps_[icomp] = &vec;
+
+ vectors_valid_ = VectorsValid();
+ ObjectChanged();
+ }
+
+ void CompoundVector::SetCompNonConst(Index icomp, Vector& vec)
+ {
+ DBG_ASSERT(icomp < NComps());
+ comps_[icomp] = &vec;
+ const_comps_[icomp] = NULL;
+
+ vectors_valid_ = VectorsValid();
+ ObjectChanged();
+ }
+
+ void CompoundVector::CopyImpl(const Vector& x)
+ {
+ DBG_START_METH("CompoundVector::CopyImpl(const Vector& x)", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ const CompoundVector* comp_x = dynamic_cast<const CompoundVector*>(&x);
+ DBG_ASSERT(comp_x);
+ DBG_ASSERT(NComps() == comp_x->NComps());
+ for(Index i=0; i<NComps(); i++) {
+ Comp(i)->Copy(*comp_x->GetComp(i));
+ }
+ }
+
+ void CompoundVector::ScalImpl(Number alpha)
+ {
+ DBG_START_METH("CompoundVector::ScalImpl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ for(Index i=0; i<NComps(); i++) {
+ DBG_ASSERT(Comp(i));
+ Comp(i)->Scal(alpha);
+ }
+ }
+
+ void CompoundVector::AxpyImpl(Number alpha, const Vector &x)
+ {
+ DBG_START_METH("CompoundVector::AxpyImpl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ const CompoundVector* comp_x = dynamic_cast<const CompoundVector*>(&x);
+ DBG_ASSERT(comp_x);
+ DBG_ASSERT(NComps() == comp_x->NComps());
+ for(Index i=0; i<NComps(); i++) {
+ DBG_ASSERT(Comp(i));
+ Comp(i)->Axpy(alpha, *comp_x->GetComp(i));
+ }
+ }
+
+ Number CompoundVector::DotImpl(const Vector &x) const
+ {
+ DBG_START_METH("CompoundVector::DotImpl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ const CompoundVector* comp_x = dynamic_cast<const CompoundVector*>(&x);
+ DBG_ASSERT(comp_x);
+ DBG_ASSERT(NComps() == comp_x->NComps());
+ Number dot = 0.;
+ for(Index i=0; i<NComps(); i++) {
+ DBG_ASSERT(ConstComp(i));
+ dot += ConstComp(i)->Dot(*comp_x->GetComp(i));
+ }
+ return dot;
+ }
+
+ Number CompoundVector::Nrm2Impl() const
+ {
+ DBG_START_METH("CompoundVector::Nrm2Impl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ Number sum=0.;
+ for(Index i=0; i<NComps(); i++) {
+ Number nrm2 = ConstComp(i)->Nrm2();
+ sum += nrm2*nrm2;
+ }
+ return sqrt(sum);
+ }
+
+ Number CompoundVector::AsumImpl() const
+ {
+ DBG_START_METH("CompoundVector::AsumImpl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ Number sum=0.;
+ for(Index i=0; i<NComps(); i++) {
+ sum += ConstComp(i)->Asum();
+ }
+ return sum;
+ }
+
+ Number CompoundVector::AmaxImpl() const
+ {
+ DBG_START_METH("CompoundVector::AmaxImpl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ Number max=0.;
+ for(Index i=0; i<NComps(); i++) {
+ max = Ipopt::Max(max, ConstComp(i)->Amax());
+ }
+ return max;
+ }
+
+ void CompoundVector::SetImpl(Number value)
+ {
+ DBG_START_METH("CompoundVector::SetImpl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ for(Index i=0; i<NComps(); i++) {
+ Comp(i)->Set(value);
+ }
+ }
+
+ void CompoundVector::ElementWiseDivideImpl(const Vector& x)
+ {
+ DBG_START_METH("CompoundVector::ElementWiseDivideImpl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ const CompoundVector* comp_x = dynamic_cast<const CompoundVector*>(&x);
+ DBG_ASSERT(comp_x);
+ DBG_ASSERT(NComps() == comp_x->NComps());
+ for(Index i=0; i<NComps(); i++) {
+ Comp(i)->ElementWiseDivide(*comp_x->GetComp(i));
+ }
+ }
+
+ void CompoundVector::ElementWiseMultiplyImpl(const Vector& x)
+ {
+ DBG_START_METH("CompoundVector::ElementWiseMultiplyImpl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ const CompoundVector* comp_x = dynamic_cast<const CompoundVector*>(&x);
+ DBG_ASSERT(comp_x);
+ DBG_ASSERT(NComps() == comp_x->NComps());
+ for(Index i=0; i<NComps(); i++) {
+ Comp(i)->ElementWiseMultiply(*comp_x->GetComp(i));
+ }
+ }
+
+ void CompoundVector::ElementWiseMaxImpl(const Vector& x)
+ {
+ DBG_START_METH("CompoundVector::ElementWiseMaxImpl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ const CompoundVector* comp_x = dynamic_cast<const CompoundVector*>(&x);
+ DBG_ASSERT(comp_x);
+ DBG_ASSERT(NComps() == comp_x->NComps());
+ for(Index i=0; i<NComps(); i++) {
+ Comp(i)->ElementWiseMax(*comp_x->GetComp(i));
+ }
+ }
+
+ void CompoundVector::ElementWiseMinImpl(const Vector& x)
+ {
+ DBG_START_METH("CompoundVector::ElementWiseMinImpl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ const CompoundVector* comp_x = dynamic_cast<const CompoundVector*>(&x);
+ DBG_ASSERT(comp_x);
+ DBG_ASSERT(NComps() == comp_x->NComps());
+ for(Index i=0; i<NComps(); i++) {
+ Comp(i)->ElementWiseMin(*comp_x->GetComp(i));
+ }
+ }
+
+ void CompoundVector::ElementWiseReciprocalImpl()
+ {
+ DBG_START_METH("CompoundVector::ElementWiseReciprocalImpl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ for(Index i=0; i<NComps(); i++) {
+ Comp(i)->ElementWiseReciprocal();
+ }
+ }
+
+ void CompoundVector::ElementWiseAbsImpl()
+ {
+ DBG_START_METH("CompoundVector::ElementWiseAbsImpl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ for(Index i=0; i<NComps(); i++) {
+ Comp(i)->ElementWiseAbs();
+ }
+ }
+
+ void CompoundVector::ElementWiseSqrtImpl()
+ {
+ DBG_START_METH("CompoundVector::ElementWiseSqrtImpl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ for(Index i=0; i<NComps(); i++) {
+ Comp(i)->ElementWiseSqrt();
+ }
+ }
+
+ void CompoundVector::AddScalarImpl(Number scalar)
+ {
+ DBG_START_METH("CompoundVector::AddScalarImpl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ for(Index i=0; i<NComps(); i++) {
+ Comp(i)->AddScalar(scalar);
+ }
+ }
+
+ Number CompoundVector::MaxImpl() const
+ {
+ DBG_START_METH("CompoundVector::MaxImpl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ DBG_ASSERT(NComps() > 0 && Dim() > 0 && "There is no Max of a zero length vector (no reasonable default can be returned)");
+ Number max = -std::numeric_limits<Number>::max();
+ for(Index i=0; i<NComps(); i++) {
+ if (ConstComp(i)->Dim() != 0) {
+ max = Ipopt::Max(max, ConstComp(i)->Max());
+ }
+ }
+ return max;
+ }
+
+ Number CompoundVector::MinImpl() const
+ {
+ DBG_START_METH("CompoundVector::MinImpl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ DBG_ASSERT(NComps() > 0 && Dim() > 0 && "There is no Min of a zero length vector (no reasonable default can be returned)");
+ Number min = std::numeric_limits<Number>::max();
+ for (Index i=0; i<NComps(); i++) {
+ if (ConstComp(i)->Dim() != 0) {
+ min = Ipopt::Min(min, ConstComp(i)->Min());
+ }
+ }
+ return min;
+ }
+
+ Number CompoundVector::SumImpl() const
+ {
+ DBG_START_METH("CompoundVector::SumImpl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ Number sum=0.;
+ for(Index i=0; i<NComps(); i++) {
+ sum += ConstComp(i)->Sum();
+ }
+ return sum;
+ }
+
+ Number CompoundVector::SumLogsImpl() const
+ {
+ DBG_START_METH("CompoundVector::SumLogsImpl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ Number sum=0.;
+ for(Index i=0; i<NComps(); i++) {
+ sum += ConstComp(i)->SumLogs();
+ }
+ return sum;
+ }
+
+ void CompoundVector::ElementWiseSgnImpl()
+ {
+ DBG_START_METH("CompoundVector::ElementWiseSgnImpl", dbg_verbosity);
+ DBG_ASSERT(vectors_valid_);
+ for(Index i=0; i<NComps(); i++) {
+ Comp(i)->ElementWiseSgn();
+ }
+ }
+
+ // Specialized Functions
+ void CompoundVector::AddTwoVectorsImpl(Number a, const Vector& v1,
+ Number b, const Vector& v2, Number c)
+ {
+ DBG_ASSERT(vectors_valid_);
+ const CompoundVector* comp_v1 = dynamic_cast<const CompoundVector*>(&v1);
+ DBG_ASSERT(comp_v1);
+ DBG_ASSERT(NComps() == comp_v1->NComps());
+ const CompoundVector* comp_v2 = dynamic_cast<const CompoundVector*>(&v2);
+ DBG_ASSERT(comp_v2);
+ DBG_ASSERT(NComps() == comp_v2->NComps());
+
+ for(Index i=0; i<NComps(); i++) {
+ Comp(i)->AddTwoVectors(a, *comp_v1->GetComp(i), b, *comp_v2->GetComp(i), c);
+ }
+ }
+
+ Number
+ CompoundVector::FracToBoundImpl(const Vector& delta, Number tau) const
+ {
+ DBG_ASSERT(vectors_valid_);
+ const CompoundVector* comp_delta =
+ dynamic_cast<const CompoundVector*>(&delta);
+ DBG_ASSERT(comp_delta);
+ DBG_ASSERT(NComps() == comp_delta->NComps());
+
+ Number alpha = 1.;
+ for(Index i=0; i<NComps(); i++) {
+ alpha = Ipopt::Min(alpha,
+ ConstComp(i)->FracToBound(*comp_delta->GetComp(i), tau));
+ }
+ return alpha;
+ }
+
+ void CompoundVector::AddVectorQuotientImpl(Number a, const Vector& z,
+ const Vector& s, Number c)
+ {
+ DBG_ASSERT(vectors_valid_);
+ const CompoundVector* comp_z =
+ dynamic_cast<const CompoundVector*>(&z);
+ DBG_ASSERT(comp_z);
+ DBG_ASSERT(NComps() == comp_z->NComps());
+ const CompoundVector* comp_s =
+ dynamic_cast<const CompoundVector*>(&s);
+ DBG_ASSERT(comp_s);
+ DBG_ASSERT(NComps() == comp_s->NComps());
+
+ for(Index i=0; i<NComps(); i++) {
+ Comp(i)->AddVectorQuotient(a, *comp_z->GetComp(i),
+ *comp_s->GetComp(i), c);
+ }
+ }
+
+ bool CompoundVector::HasValidNumbersImpl() const
+ {
+ DBG_ASSERT(vectors_valid_);
+ for(Index i=0; i<NComps(); i++) {
+ if (!ConstComp(i)->HasValidNumbers()) {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ void CompoundVector::PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ DBG_START_METH("CompoundVector::PrintImpl", dbg_verbosity);
+ jnlst.Printf(level, category, "\n");
+ jnlst.PrintfIndented(level, category, indent,
+ "%sCompoundVector \"%s\" with %d components:\n",
+ prefix.c_str(), name.c_str(), NComps());
+ for(Index i=0; i<NComps(); i++) {
+ jnlst.Printf(level, category, "\n");
+ jnlst.PrintfIndented(level, category, indent,
+ "%sComponent %d:\n", prefix.c_str(), i+1);
+ if (ConstComp(i)) {
+ DBG_ASSERT(name.size()<200);
+ char buffer[256];
+ sprintf(buffer, "%s[%2d]", name.c_str(), i);
+ std::string term_name = buffer;
+ ConstComp(i)->Print(&jnlst, level, category, term_name,
+ indent+1, prefix);
+ }
+ else {
+ jnlst.PrintfIndented(level, category, indent,
+ "%sComponent %d is not yet set!\n",
+ prefix.c_str(), i+1);
+ }
+ }
+ }
+
+ bool CompoundVector::VectorsValid()
+ {
+ bool retVal = true;
+ for (Index i=0; i<NComps(); i++) {
+ // Better not have an entry in both (sanity check)
+ DBG_ASSERT(IsNull(comps_[i]) || IsNull(const_comps_[i]));
+ if (IsNull(comps_[i]) && IsNull(const_comps_[i])) {
+ retVal = false;
+ break;
+ }
+ }
+ return retVal;
+ }
+
+ CompoundVectorSpace::CompoundVectorSpace(Index ncomp_spaces, Index total_dim)
+ :
+ VectorSpace(total_dim),
+ ncomp_spaces_(ncomp_spaces),
+ comp_spaces_(ncomp_spaces)
+ {}
+
+ void CompoundVectorSpace::SetCompSpace(Index icomp, const VectorSpace& vec_space)
+ {
+ DBG_ASSERT(icomp<ncomp_spaces_);
+ DBG_ASSERT(IsNull(comp_spaces_[icomp]));
+ comp_spaces_[icomp] = &vec_space;
+ }
+
+ SmartPtr<const VectorSpace> CompoundVectorSpace::GetCompSpace(Index icomp) const
+ {
+ DBG_ASSERT(icomp<ncomp_spaces_);
+ return comp_spaces_[icomp];
+ }
+
+}
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpCompoundVector.hpp b/SimTKmath/Optimizers/src/IpOpt/IpCompoundVector.hpp
new file mode 100644
index 0000000..2c17f5b
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpCompoundVector.hpp
@@ -0,0 +1,339 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpCompoundVector.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPCOMPOUNDVECTOR_HPP__
+#define __IPCOMPOUNDVECTOR_HPP__
+
+#include "IpUtils.hpp"
+#include "IpVector.hpp"
+#include <vector>
+
+namespace Ipopt
+{
+
+ /* forward declarations */
+ class CompoundVectorSpace;
+
+ /** Class of Vectors consisting of other vectors. This vector is a
+ * vector that consists of zero, one or more Vector's which are
+ * stacked on each others: \f$ x_{\rm compound} =
+ * \left(\begin{array}{c}x_0\\\dots\\x_{{\rm
+ * ncomps} - 1}\end{array}\right)\f$. The individual components can be
+ * associated to different VectorSpaces. The individual components
+ * can also be const and non-const Vectors.
+ */
+ class CompoundVector : public Vector
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor, given the corresponding CompoundVectorSpace.
+ * Before this constructor can be called, all components of the
+ * CompoundVectorSpace have to be set, so that the constructors
+ * for the individual components can be called. If the flag
+ * create_new is true, then the individual components of the new
+ * CompoundVector are initialized with the MakeNew methods of
+ * each VectorSpace (and are non-const). Otherwise, the
+ * individual components can later be set using the SetComp and
+ * SetCompNonConst method.
+ */
+ CompoundVector(const CompoundVectorSpace* owner_space, bool create_new);
+
+ /** Default destructor */
+ virtual ~CompoundVector();
+ //@}
+
+ /** Method for setting the pointer for a component that is a const
+ * Vector
+ */
+ void SetComp(Index icomp, const Vector& vec);
+
+ /** Method for setting the pointer for a component that is a
+ * non-const Vector
+ */
+ void SetCompNonConst(Index icomp, Vector& vec);
+
+ /** Number of components of this compound vector */
+ inline Index NComps() const;
+
+ /** Check if a particular component is const or not */
+ bool IsCompConst(Index i) const
+ {
+ DBG_ASSERT(i > 0 && i < NComps());
+ DBG_ASSERT(IsValid(comps_[i]) || IsValid(const_comps_[i]));
+ if (IsValid(const_comps_[i])) {
+ return true;
+ }
+ return false;
+ }
+
+ /** Check if a particular component is null or not */
+ bool IsCompNull(Index i) const
+ {
+ DBG_ASSERT(i >= 0 && i < NComps());
+ if (IsValid(comps_[i]) || IsValid(const_comps_[i])) {
+ return false;
+ }
+ return true;
+ }
+
+ /** Return a particular component (const version) */
+ SmartPtr<const Vector> GetComp(Index i) const
+ {
+ return ConstComp(i);
+ }
+
+ /** Return a particular component (non-const version). Note that
+ * calling this method with mark the CompoundVector as changed.
+ * Therefore, only use this method if you are intending to change
+ * the Vector that you receive.
+ */
+ SmartPtr<Vector> GetCompNonConst(Index i)
+ {
+ ObjectChanged();
+ return Comp(i);
+ }
+
+ protected:
+ /** @name Overloaded methods from Vector base class */
+ //@{
+ /** Copy the data of the vector x into this vector (DCOPY). */
+ virtual void CopyImpl(const Vector& x);
+
+ /** Scales the vector by scalar alpha (DSCAL) */
+ virtual void ScalImpl(Number alpha);
+
+ /** Add the multiple alpha of vector x to this vector (DAXPY) */
+ virtual void AxpyImpl(Number alpha, const Vector &x);
+
+ /** Computes inner product of vector x with this (DDOT) */
+ virtual Number DotImpl(const Vector &x) const;
+
+ /** Computes the 2-norm of this vector (DNRM2) */
+ virtual Number Nrm2Impl() const;
+
+ /** Computes the 1-norm of this vector (DASUM) */
+ virtual Number AsumImpl() const;
+
+ /** Computes the max-norm of this vector (based on IDAMAX) */
+ virtual Number AmaxImpl() const;
+
+ /** Set each element in the vector to the scalar alpha. */
+ virtual void SetImpl(Number value);
+
+ /** Element-wise division \f$y_i \gets y_i/x_i\f$.*/
+ virtual void ElementWiseDivideImpl(const Vector& x);
+
+ /** Element-wise multiplication \f$y_i \gets y_i*x_i\f$.*/
+ virtual void ElementWiseMultiplyImpl(const Vector& x);
+
+ /** Element-wise max against entries in x */
+ virtual void ElementWiseMaxImpl(const Vector& x);
+
+ /** Element-wise min against entries in x */
+ virtual void ElementWiseMinImpl(const Vector& x);
+
+ /** Element-wise reciprocal */
+ virtual void ElementWiseReciprocalImpl();
+
+ /** Element-wise absolute values */
+ virtual void ElementWiseAbsImpl();
+
+ /** Element-wise square-root */
+ virtual void ElementWiseSqrtImpl();
+
+ /** Replaces entries with sgn of the entry */
+ virtual void ElementWiseSgnImpl();
+
+ /** Add scalar to every component of the vector.*/
+ virtual void AddScalarImpl(Number scalar);
+
+ /** Max value in the vector */
+ virtual Number MaxImpl() const;
+
+ /** Min value in the vector */
+ virtual Number MinImpl() const;
+
+ /** Computes the sum of the lements of vector */
+ virtual Number SumImpl() const;
+
+ /** Computes the sum of the logs of the elements of vector */
+ virtual Number SumLogsImpl() const;
+
+ /** @name Implemented specialized functions */
+ //@{
+ /** Add two vectors (a * v1 + b * v2). Result is stored in this
+ vector. */
+ void AddTwoVectorsImpl(Number a, const Vector& v1,
+ Number b, const Vector& v2, Number c);
+ /** Fraction to the boundary parameter. */
+ Number FracToBoundImpl(const Vector& delta, Number tau) const;
+ /** Add the quotient of two vectors, y = a * z/s + c * y. */
+ void AddVectorQuotientImpl(Number a, const Vector& z, const Vector& s,
+ Number c);
+ //@}
+
+ /** Method for determining if all stored numbers are valid (i.e.,
+ * no Inf or Nan). */
+ virtual bool HasValidNumbersImpl() const;
+
+ /** @name Output methods */
+ //@{
+ /* Print the entire vector with padding */
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const;
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called.
+ */
+ //@{
+ /** Default Constructor */
+ CompoundVector();
+
+ /** Copy Constructor */
+ CompoundVector(const CompoundVector&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const CompoundVector&);
+ //@}
+
+ /** Components of the compound vector. The components
+ * are stored by SmartPtrs in a std::vector
+ */
+ std::vector< SmartPtr<Vector> > comps_;
+ std::vector< SmartPtr<const Vector> > const_comps_;
+
+ const CompoundVectorSpace* owner_space_;
+
+ bool vectors_valid_;
+
+ bool VectorsValid();
+
+ inline const Vector* ConstComp(Index i) const;
+
+ inline Vector* Comp(Index i);
+ };
+
+ /** This vectors space is the vector space for CompoundVector.
+ * Before a CompoundVector can be created, all components of this
+ * CompoundVectorSpace have to be set. When calling the constructor,
+ * the number of component has to be specified. The individual
+ * VectorSpaces can be set with the SetComp method.
+ */
+ class CompoundVectorSpace : public VectorSpace
+ {
+ public:
+ /** @name Constructors/Destructors. */
+ //@{
+ /** Constructor, has to be given the number of components and the
+ * total dimension of all components combined. */
+ CompoundVectorSpace(Index ncomp_spaces, Index total_dim);
+
+ /** Destructor */
+ ~CompoundVectorSpace()
+ {}
+ //@}
+
+ /** Method for setting the individual component VectorSpaces */
+ virtual void SetCompSpace(Index icomp /** Number of the component to be set */ ,
+ const VectorSpace& vec_space /** VectorSpace for component icomp */
+ );
+
+ /** Method for obtaining an individual component VectorSpace */
+ SmartPtr<const VectorSpace> GetCompSpace(Index icomp) const;
+
+ /** Accessor method to obtain the number of components */
+ Index NCompSpaces() const
+ {
+ return ncomp_spaces_;
+ }
+
+ /** Method for creating a new vector of this specific type. */
+ virtual CompoundVector* MakeNewCompoundVector(bool create_new = true) const
+ {
+ return new CompoundVector(this, create_new);
+ }
+
+ /** Overloaded MakeNew method for the VectorSpace base class.
+ */
+ virtual Vector* MakeNew() const
+ {
+ return MakeNewCompoundVector();
+ }
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default constructor */
+ CompoundVectorSpace();
+
+ /** Copy Constructor */
+ CompoundVectorSpace(const CompoundVectorSpace&);
+
+ /** Overloaded Equals Operator */
+ CompoundVectorSpace& operator=(const CompoundVectorSpace&);
+ //@}
+
+ /** Number of components */
+ const Index ncomp_spaces_;
+
+ /** std::vector of vector spaces for the components */
+ std::vector< SmartPtr<const VectorSpace> > comp_spaces_;
+ };
+
+ /* inline methods */
+ inline
+ Index CompoundVector::NComps() const
+ {
+ return owner_space_->NCompSpaces();
+ }
+
+ inline
+ const Vector* CompoundVector::ConstComp(Index i) const
+ {
+ DBG_ASSERT(i < NComps());
+ DBG_ASSERT(IsValid(comps_[i]) || IsValid(const_comps_[i]));
+ if (IsValid(comps_[i])) {
+ return GetRawPtr(comps_[i]);
+ }
+ else if (IsValid(const_comps_[i])) {
+ return GetRawPtr(const_comps_[i]);
+ }
+
+ DBG_ASSERT(false && "shouldn't be here");
+ return NULL;
+ }
+
+ inline
+ Vector* CompoundVector::Comp(Index i)
+ {
+ DBG_ASSERT(i < NComps());
+ DBG_ASSERT(IsValid(comps_[i]));
+ return GetRawPtr(comps_[i]);
+ }
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpConvCheck.hpp b/SimTKmath/Optimizers/src/IpOpt/IpConvCheck.hpp
new file mode 100644
index 0000000..683e94a
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpConvCheck.hpp
@@ -0,0 +1,86 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpConvCheck.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPCONVCHECK_HPP__
+#define __IPCONVCHECK_HPP__
+
+#include "IpAlgStrategy.hpp"
+
+namespace Ipopt
+{
+
+ /** Base class for checking the algorithm
+ * termination criteria.
+ */
+ class ConvergenceCheck : public AlgorithmStrategyObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor */
+ ConvergenceCheck()
+ {}
+
+ /** Default destructor */
+ virtual ~ConvergenceCheck()
+ {}
+ //@}
+
+ /** Convergence return enum */
+ enum ConvergenceStatus {
+ CONTINUE,
+ CONVERGED,
+ CONVERGED_TO_ACCEPTABLE_POINT,
+ MAXITER_EXCEEDED,
+ DIVERGING,
+ USER_STOP,
+ FAILED
+ };
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix) = 0;
+
+ /** Pure virtual method for performing the convergence test. If
+ * call_intermediate_callback is true, the user callback method
+ * in the NLP should be called in order to see if the user
+ * requests an early termination. */
+ virtual ConvergenceStatus
+ CheckConvergence(bool call_intermediate_callback = true) = 0;
+
+ /** Method for testing if the current iterate is considered to
+ * satisfy the "accptable level" of accuracy. The idea is that
+ * if the desired convergence tolerance cannot be achieved, the
+ * algorithm might stop after a number of acceptable points have
+ * been encountered. */
+ virtual bool CurrentIsAcceptable()=0;
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ // ConvergenceCheck();
+
+ /** Copy Constructor */
+ ConvergenceCheck(const ConvergenceCheck&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const ConvergenceCheck&);
+ //@}
+
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpDebug.cpp b/SimTKmath/Optimizers/src/IpOpt/IpDebug.cpp
new file mode 100644
index 0000000..4c9bdf3
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpDebug.cpp
@@ -0,0 +1,107 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpDebug.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpoptConfig.h"
+
+#include "IpDebug.hpp"
+#include "IpJournalist.hpp"
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ Index DebugJournalistWrapper::indentation_level_ = 0;
+ Journalist* DebugJournalistWrapper::jrnl_ = NULL;
+
+ DebugJournalistWrapper::DebugJournalistWrapper(
+ std::string func_name,
+ Index verbose_level)
+ :
+ func_name_(func_name),
+ verbose_level_(verbose_level),
+ method_owner_(NULL)
+ {
+ if (jrnl_==NULL) {
+ verbose_level_ = 0;
+ return;
+ }
+ DebugPrintf(1, "-> Calling to: %s\n", func_name_.c_str());
+ if (verbose_level_>0) {
+ indentation_level_++;
+ }
+ }
+
+ DebugJournalistWrapper::DebugJournalistWrapper(
+ std::string func_name, Index verbose_level,
+ const void* const method_owner)
+ :
+ func_name_(func_name),
+ verbose_level_(verbose_level),
+ method_owner_(method_owner)
+ {
+ if (jrnl_==NULL) {
+ verbose_level_ = 0;
+ return;
+ }
+ DebugPrintf(1, "-> Calling to: %s in obj: 0x%x\n", func_name_.c_str(),
+ method_owner_);
+ if (verbose_level_>0) {
+ indentation_level_++;
+ }
+ }
+
+ DebugJournalistWrapper::~DebugJournalistWrapper()
+ {
+ if (verbose_level_>0) {
+ indentation_level_--;
+ }
+ if (jrnl_) {
+ if (method_owner_ == NULL) {
+ DebugPrintf(1, "<- Returning from : %s\n", func_name_.c_str());
+ }
+ else {
+ DebugPrintf(1, "<- Returning from : %s in obj: 0x%x\n",
+ func_name_.c_str(), method_owner_);
+ }
+ }
+ }
+
+
+ void DebugJournalistWrapper::SetJournalist(Journalist* jrnl)
+ {
+
+ if (jrnl == NULL) {
+ jrnl_->Printf(J_ERROR, J_DBG,
+ "# Setting Journalist to NULL in DebugJournalistWrapper.\n");
+ jrnl_ = NULL;
+ }
+ else if (!jrnl_) {
+ jrnl_ = jrnl;
+ }
+ }
+
+
+ void DebugJournalistWrapper::DebugPrintf(Index verbosity, const char* pformat, ...)
+ {
+
+ if (Verbosity() >= verbosity) {
+ va_list(ap);
+
+ va_start(ap, pformat);
+
+ DBG_ASSERT(jrnl_);
+ jrnl_->PrintfIndented(J_ERROR, J_DBG, indentation_level_, "# ");
+ jrnl_->VPrintf(J_ERROR, J_DBG, pformat, ap);
+
+ va_end(ap);
+ }
+ }
+#endif // #ifdef IP_DEBUG
+
+void preventNoSymbolsWarningInIpDebug() {}
+} // namespace Ipopt
+
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpDebug.hpp b/SimTKmath/Optimizers/src/IpOpt/IpDebug.hpp
new file mode 100644
index 0000000..df9af8d
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpDebug.hpp
@@ -0,0 +1,139 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpDebug.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPDEBUG_HPP__
+#define __IPDEBUG_HPP__
+
+#include "IpoptConfig.h"
+#include "IpTypes.hpp"
+
+#ifdef HAVE_CASSERT
+# include <cassert>
+#else
+# ifdef HAVE_ASSERT_H
+# include <assert.h>
+# else
+# error "don't have header file for assert"
+# endif
+#endif
+
+#ifdef IP_DEBUG
+# define DBG_ASSERT(test) assert(test)
+# define DBG_ASSERT_EXCEPTION(__condition, __except_type, __msg) \
+ ASSERT_EXCEPTION( (__condition), __except_type, __msg);
+# define DBG_DO(__cmd) __cmd
+#else
+# define DBG_ASSERT(test)
+# define DBG_ASSERT_EXCEPTION(__condition, __except_type, __msg)
+# define DBG_DO(__cmd)
+#endif
+
+#ifndef IP_DEBUG
+# define DBG_START_FUN(__func_name, __verbose_level)
+# define DBG_START_METH(__func_name, __verbose_level)
+# define DBG_PRINT(__printf_args)
+# define DBG_PRINT_VECTOR(__verbose_level, __vec_name, __vec)
+# define DBG_PRINT_MATRIX(__verbose_level, __mat_name, __mat)
+# define DBG_EXEC(__verbosity, __cmd)
+# define DBG_VERBOSITY() 0
+#else
+#include <string>
+
+namespace Ipopt
+{
+ // forward definition
+ class Journalist;
+
+ /** Class that lives throughout the execution of a method or
+ * function for which debug output is to be generated. The output
+ * is sent to the unique debug journalist that is set with
+ * SetJournalist at the beginning of program execution. */
+ class DebugJournalistWrapper
+ {
+ public:
+ /** @name Constructors/Destructors. */
+ //@{
+ DebugJournalistWrapper(std::string func_name, Index verbose_level);
+ DebugJournalistWrapper(std::string func_name, Index verbose_level,
+ const void* const method_owner);
+ ~DebugJournalistWrapper();
+ //@}
+
+ /** @name accessor methods */
+ //@{
+ Index Verbosity()
+ {
+ return verbose_level_;
+ }
+ const Journalist* Jnlst()
+ {
+ return jrnl_;
+ }
+ Index IndentationLevel()
+ {
+ return indentation_level_;
+ }
+ //@}
+
+ /** Printing */
+ void DebugPrintf(Index verbosity, const char* pformat, ...);
+
+ /* Method for initialization of the static GLOBAL journalist,
+ * through with all debug printout is to be written. This needs
+ * to be set before any debug printout can be done. */
+ static void SetJournalist(Journalist* jrnl);
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** default constructor */
+ DebugJournalistWrapper();
+
+ /** copy contructor */
+ DebugJournalistWrapper(const DebugJournalistWrapper&);
+
+ /** Overloaded Equals Operator */
+ DebugJournalistWrapper& operator=(const DebugJournalistWrapper&);
+ //@}
+
+ static Index indentation_level_;
+ std::string func_name_;
+ Index verbose_level_;
+ const void* method_owner_;
+
+ static Journalist* jrnl_;
+ };
+}
+
+# define DBG_START_FUN(__func_name, __verbose_level) \
+ DebugJournalistWrapper dbg_jrnl((__func_name), (__verbose_level)); \
+
+# define DBG_START_METH(__func_name, __verbose_level) \
+ DebugJournalistWrapper dbg_jrnl((__func_name), (__verbose_level), this);
+
+# define DBG_PRINT(__args) \
+ dbg_jrnl.DebugPrintf __args;
+
+# define DBG_EXEC(__verbose_level, __cmd) \
+ if (dbg_jrnl.Verbosity() >= (__verbose_level)) { \
+ (__cmd); \
+ }
+
+# define DBG_VERBOSITY() \
+ dbg_jrnl.Verbosity()
+
+#endif
+
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpDefaultIterateInitializer.cpp b/SimTKmath/Optimizers/src/IpOpt/IpDefaultIterateInitializer.cpp
new file mode 100644
index 0000000..b8f00c2
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpDefaultIterateInitializer.cpp
@@ -0,0 +1,398 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpDefaultIterateInitializer.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-09-23
+
+#include "IpDefaultIterateInitializer.hpp"
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ DefaultIterateInitializer::DefaultIterateInitializer
+ (const SmartPtr<EqMultiplierCalculator>& eq_mult_calculator,
+ const SmartPtr<IterateInitializer>& warm_start_initializer)
+ :
+ IterateInitializer(),
+ eq_mult_calculator_(eq_mult_calculator),
+ warm_start_initializer_(warm_start_initializer)
+ {}
+
+ void DefaultIterateInitializer::RegisterOptions(SmartPtr<RegisteredOptions> reg_options)
+ {
+ reg_options->AddLowerBoundedNumberOption(
+ "bound_push",
+ "Desired minimum absolute distance from the initial point to bound.",
+ 0.0, true, 0.01,
+ "Determines how much the initial point might have to "
+ "be modified in order to be sufficiently inside "
+ "the bounds (together with \"bound_frac\"). (This is kappa_1 in "
+ "Section 3.6 of implementation paper.)");
+ reg_options->AddBoundedNumberOption(
+ "bound_frac",
+ "Desired minimum relative distance from the initial point to bound.",
+ 0, true, 0.5, false, 0.01,
+ "Determines how much the initial point might have to "
+ "be modified in order to be sufficiently inside "
+ "the bounds (together with \"bound_push\"). (This is kappa_2 in "
+ "Section 3.6 of implementation paper.)");
+ reg_options->AddLowerBoundedNumberOption(
+ "constr_mult_init_max",
+ "Maximum allowed least-square guess of constraint multipliers.",
+ 0, false, 1e3,
+ "Determines how large the initial least-square guesses of the constraint "
+ "multipliers are allowed to be (in max-norm). If the guess is larger "
+ "than this value, it is discarded and all constraint multipliers are "
+ "set to zero. This options is also used when initializing the "
+ "restoration phase. By default, \"resto.constr_mult_init_max\" (the one "
+ "used in RestoIterateInitializer) is set to zero.");
+ reg_options->AddLowerBoundedNumberOption(
+ "bound_mult_init_val",
+ "Initial value for the bound multipliers.",
+ 0, true, 1.0,
+ "All dual variables corresponding to bound constraints are "
+ "initialized to this value.");
+ reg_options->SetRegisteringCategory("Warm Start");
+ reg_options->AddStringOption2(
+ "warm_start_init_point",
+ "Warm-start for initial point", "no",
+ "no", "do not use the warm start initialization",
+ "yes", "use the warm start initialization",
+ "Indicates whether this optimization should use a warm start "
+ "initialization, where values of primal and dual variables are "
+ "given (e.g., from a previous optimization of a related problem.)");
+ }
+
+ bool DefaultIterateInitializer::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ // Check for the algorithm options
+ options.GetNumericValue("bound_push", bound_push_, prefix);
+ options.GetNumericValue("bound_frac", bound_frac_, prefix);
+ options.GetNumericValue("constr_mult_init_max",
+ constr_mult_init_max_, prefix);
+ options.GetNumericValue("bound_mult_init_val",
+ bound_mult_init_val_, prefix);
+ options.GetBoolValue("warm_start_init_point",
+ warm_start_init_point_, prefix);
+
+ bool retvalue = true;
+ if (IsValid(eq_mult_calculator_)) {
+ retvalue = eq_mult_calculator_->Initialize(Jnlst(), IpNLP(), IpData(),
+ IpCq(), options, prefix);
+ if (!retvalue) {
+ return retvalue;
+ }
+ }
+ if (IsValid(warm_start_initializer_)) {
+ retvalue =
+ warm_start_initializer_->Initialize(Jnlst(), IpNLP(), IpData(),
+ IpCq(), options, prefix);
+ }
+ return retvalue;
+ }
+
+ bool DefaultIterateInitializer::SetInitialIterates()
+ {
+ DBG_START_METH("DefaultIterateInitializer::SetInitialIterates",
+ dbg_verbosity);
+
+ if (warm_start_init_point_) {
+ DBG_ASSERT(IsValid(warm_start_initializer_));
+ return warm_start_initializer_->SetInitialIterates();
+ }
+
+ // Get the starting values provided by the NLP and store them
+ // in the ip_data current fields. The following line only requests
+ // intial values for the primal variables x, but later we might
+ // make this more flexible based on user options.
+
+ /////////////////////////////////////////////////////////////////////
+ // Initialize primal variables //
+ /////////////////////////////////////////////////////////////////////
+
+ IpData().InitializeDataStructures(IpNLP(), true, false, false,
+ false, false);
+
+ // get a container of the current point. We will modify parts of this
+ // IteratesVector to set the trial point.
+ SmartPtr<IteratesVector> iterates = IpData().curr()->MakeNewContainer();
+
+ DBG_PRINT_VECTOR(2, "curr_x", *iterates->x());
+
+ // Now we compute the initial values that the algorithm is going to
+ // actually use. We first store them in the trial fields in ip_data.
+
+ // Push the x iterates sufficiently inside the bounds
+ // Calculate any required shift in x0 and s0
+
+ SmartPtr<const Vector> new_x;
+ push_variables(Jnlst(), bound_push_, bound_frac_,
+ "x", *iterates->x(), new_x, *IpNLP().x_L(),
+ *IpNLP().x_U(), *IpNLP().Px_L(), *IpNLP().Px_U());
+
+ iterates->Set_x(*new_x);
+ IpData().set_trial(iterates);
+
+ // Calculate the shift in s...
+ SmartPtr<const Vector> s = IpCq().trial_d();
+ DBG_PRINT_VECTOR(2, "s", *s);
+
+ SmartPtr<const Vector> new_s;
+ push_variables(Jnlst(), bound_push_, bound_frac_,
+ "s", *s, new_s, *IpNLP().d_L(),
+ *IpNLP().d_U(), *IpNLP().Pd_L(), *IpNLP().Pd_U());
+
+ iterates = IpData().trial()->MakeNewContainer();
+ iterates->Set_s(*new_s);
+
+ /////////////////////////////////////////////////////////////////////
+ // Initialize bound multipliers //
+ /////////////////////////////////////////////////////////////////////
+
+ // Initialize the bound multipliers to bound_mult_init_val.
+ iterates->create_new_z_L();
+ iterates->create_new_z_U();
+ iterates->create_new_v_L();
+ iterates->create_new_v_U();
+ iterates->z_L_NonConst()->Set(bound_mult_init_val_);
+ iterates->z_U_NonConst()->Set(bound_mult_init_val_);
+ iterates->v_L_NonConst()->Set(bound_mult_init_val_);
+ iterates->v_U_NonConst()->Set(bound_mult_init_val_);
+
+ IpData().set_trial(iterates);
+
+ /////////////////////////////////////////////////////////////////////
+ // Initialize equality constraint multipliers //
+ /////////////////////////////////////////////////////////////////////
+
+ least_square_mults(Jnlst(), IpNLP(), IpData(), IpCq(),
+ eq_mult_calculator_, constr_mult_init_max_);
+
+ // upgrade the trial to the current point
+ IpData().AcceptTrialPoint();
+
+ return true;
+ }
+
+ void DefaultIterateInitializer::push_variables(
+ const Journalist& jnlst,
+ Number bound_push,
+ Number bound_frac,
+ std::string name,
+ const Vector& orig_x,
+ SmartPtr<const Vector>& new_x,
+ const Vector& x_L,
+ const Vector& x_U,
+ const Matrix& Px_L,
+ const Matrix& Px_U)
+ {
+ DBG_START_FUN("DefaultIterateInitializer::push_variables",
+ dbg_verbosity);
+ // Calculate any required shift in x0 and s0
+ const double dbl_min = std::numeric_limits<double>::min();
+ const double tiny_double = 100.0*dbl_min;
+
+ SmartPtr<Vector> tmp = orig_x.MakeNew();
+ SmartPtr<Vector> tmp_l = x_L.MakeNew();
+ SmartPtr<Vector> tmp_u = x_U.MakeNew();
+ SmartPtr<Vector> tiny_l = x_L.MakeNew();
+ tiny_l->Set(tiny_double);
+
+ // Calculate p_l
+ SmartPtr<Vector> q_l = x_L.MakeNew();
+ SmartPtr<Vector> p_l = x_L.MakeNew();
+
+ DBG_PRINT_VECTOR(2,"orig_x", orig_x);
+ DBG_PRINT_MATRIX(2,"Px_L", Px_L);
+ DBG_PRINT_VECTOR(2, "x_L", x_L);
+ DBG_PRINT_MATRIX(2,"Px_U", Px_U);
+ DBG_PRINT_VECTOR(2, "x_U", x_U);
+
+ Px_L.MultVector(1.0, x_L, 0.0, *tmp);
+ Px_U.TransMultVector(1.0, *tmp, 0.0, *tmp_u);
+ tmp_u->AddOneVector(1., x_U, -1.);
+ Px_U.MultVector(1.0, *tmp_u, 0.0, *tmp);
+ Px_L.TransMultVector(1.0, *tmp, 0.0, *q_l);
+ q_l->AddOneVector(-1.0, *tiny_l, bound_frac);
+ DBG_PRINT_VECTOR(2, "q_l", *q_l);
+ // At this point, q_l is
+ // bound_frac * Px_L^T Px_U(x_U - Px_U^T Px_L x_L) - tiny_double
+ // i.e., it is bound_frac*(x_U - x_L) for those components that have
+ // two bounds
+ // and - tiny_double for those that have only one or no bounds
+
+ tmp_l->Set(bound_push);
+ p_l->AddOneVector(bound_push, x_L, 0.);
+ p_l->ElementWiseAbs();
+ p_l->ElementWiseMax(*tmp_l);
+ // now p_l is bound_push * max(|x_L|,1)
+
+ q_l->ElementWiseReciprocal();
+ p_l->ElementWiseReciprocal();
+
+ p_l->ElementWiseMax(*q_l);
+ p_l->ElementWiseReciprocal();
+ // p_l->Axpy(1.0, *tiny_l); we shouldn't need this
+
+ // At this point, p_l is
+ // min(bound_push * max(|x_L|,1), bound_frac*(x_U-x_L) for components
+ // with two bounds
+ // bound_push * max(|x_L|,1) otherwise
+ // This is the margin we want to the lower bound
+ DBG_PRINT_VECTOR(1, "p_l", *p_l);
+
+ // Calculate p_u
+ SmartPtr<Vector> q_u = x_U.MakeNew();
+ SmartPtr<Vector> p_u = x_U.MakeNew();
+ SmartPtr<Vector> tiny_u = x_U.MakeNew();
+ tiny_u->Set(tiny_double);
+
+ Px_U.MultVector(1.0, x_U, 0.0, *tmp);
+ Px_L.TransMultVector(1.0, *tmp, 0.0, *tmp_l);
+ tmp_l->Axpy(-1.0, x_L);
+ Px_L.MultVector(1.0, *tmp_l, 0.0, *tmp);
+ Px_U.TransMultVector(1.0, *tmp, 0.0, *q_u);
+ q_u->AddOneVector(-1.0, *tiny_u, bound_frac);
+ DBG_PRINT_VECTOR(2,"q_u",*q_u);
+ // q_u is now the same as q_l above, but of the same dimension as x_L
+
+ tmp_u->Set(bound_push);
+ p_u->Copy(x_U);
+ p_u->AddOneVector(bound_push, x_U, 0.);
+ p_u->ElementWiseAbs();
+ p_u->ElementWiseMax(*tmp_u);
+ DBG_PRINT_VECTOR(2,"p_u",*p_u);
+
+ q_u->ElementWiseReciprocal();
+ p_u->ElementWiseReciprocal();
+
+ p_u->ElementWiseMax(*q_u);
+ p_u->ElementWiseReciprocal();
+ p_u->Axpy(1.0, *tiny_u);
+ // At this point, p_l is
+ // min(bound_push * max(|x_U|,1), bound_frac*(x_U-x_L) for components
+ // with two bounds
+ // bound_push * max(|x_U|,1) otherwise
+ // This is the margin we want to the upper bound
+ DBG_PRINT_VECTOR(2,"actual_p_u",*p_u);
+
+ // Calculate the new x
+ SmartPtr<Vector> delta_x = orig_x.MakeNew();
+
+ SmartPtr<Vector> zero_l = x_L.MakeNew();
+ zero_l->Set(0.0);
+ SmartPtr<Vector> zero_u = x_U.MakeNew();
+ zero_u->Set(0.0);
+
+ Px_L.TransMultVector(-1.0, orig_x, 0.0, *tmp_l);
+ tmp_l->AddTwoVectors(1.0, x_L, 1.0, *p_l, 1.);
+ tmp_l->ElementWiseMax(*zero_l);
+ // tmp_l is now max(x_L + p_l - x, 0), i.e., the amount by how
+ // much need to correct the variable
+ Number nrm_l = tmp_l->Amax();
+ if (nrm_l>0.) {
+ Px_L.MultVector(1.0, *tmp_l, 0.0, *delta_x);
+ }
+ else {
+ delta_x->Set(0.);
+ }
+
+ Px_U.TransMultVector(1.0, orig_x, 0.0, *tmp_u);
+ tmp_u->AddTwoVectors(-1.0, x_U, 1.0, *p_u, 1.);
+ tmp_u->ElementWiseMax(*zero_u);
+ // tmp_u is now max(x - (x_U-p_u), 0), i.e., the amount by how
+ // much need to correct the variable
+ Number nrm_u = tmp_u->Amax();
+ if (nrm_u>0.) {
+ Px_U.MultVector(-1.0, *tmp_u, 1.0, *delta_x);
+ }
+
+ if (nrm_l > 0 || nrm_u > 0) {
+ delta_x->Axpy(1.0, orig_x);
+ new_x = ConstPtr(delta_x);
+ jnlst.Printf(J_DETAILED, J_INITIALIZATION, "Moved initial values of %s sufficiently inside the bounds.\n", name.c_str());
+ orig_x.Print(jnlst, J_VECTOR, J_INITIALIZATION, "original vars");
+ new_x->Print(jnlst, J_VECTOR, J_INITIALIZATION, "new vars");
+ }
+ else {
+ new_x = &orig_x;
+ jnlst.Printf(J_DETAILED, J_INITIALIZATION, "Initial values of %s sufficiently inside the bounds.\n", name.c_str());
+ }
+ }
+
+ void DefaultIterateInitializer::least_square_mults(
+ const Journalist& jnlst,
+ IpoptNLP& ip_nlp,
+ IpoptData& ip_data,
+ IpoptCalculatedQuantities& ip_cq,
+ const SmartPtr<EqMultiplierCalculator>& eq_mult_calculator,
+ Number constr_mult_init_max)
+ {
+ DBG_START_FUN("DefaultIterateInitializer::least_square_mults",
+ dbg_verbosity);
+
+ SmartPtr<IteratesVector> iterates = ip_data.trial()->MakeNewContainer();
+ iterates->create_new_y_c();
+ iterates->create_new_y_d();
+
+ if (iterates->y_c_NonConst()->Dim()==iterates->x()->Dim()) {
+ // This problem is square, we just set the multipliers to zero
+ iterates->y_c_NonConst()->Set(0.0);
+ iterates->y_d_NonConst()->Set(0.0);
+ ip_data.Append_info_string("s");
+ }
+ else if (IsValid(eq_mult_calculator) && constr_mult_init_max>0. &&
+ iterates->y_c_NonConst()->Dim()+iterates->y_d_NonConst()->Dim()>0) {
+ // First move all the trial data into the current fields, since
+ // those values are needed to compute the initial values for
+ // the multipliers
+ ip_data.CopyTrialToCurrent();
+ SmartPtr<Vector> y_c = iterates->y_c_NonConst();
+ SmartPtr<Vector> y_d = iterates->y_d_NonConst();
+ bool retval = eq_mult_calculator->CalculateMultipliers(*y_c, *y_d);
+ if (!retval) {
+ y_c->Set(0.0);
+ y_d->Set(0.0);
+ }
+ else {
+ /*
+ {
+ ip_data.set_trial(iterates);
+ printf("grad_x = %e grad_s = %e y_c = %e y_d = %e\n",
+ ip_cq.trial_grad_lag_x()->Amax(),
+ ip_cq.trial_grad_lag_s()->Amax(),
+ y_c->Amax(),
+ y_d->Amax());
+ iterates = ip_data.trial()->MakeNewContainer();
+ }
+ */
+ jnlst.Printf(J_DETAILED, J_INITIALIZATION,
+ "Least square estimates max(y_c) = %e, max(y_d) = %e\n",
+ y_c->Amax(), y_d->Amax());
+ Number yinitnrm = Max(y_c->Amax(), y_d->Amax());
+ if (yinitnrm > constr_mult_init_max) {
+ y_c->Set(0.0);
+ y_d->Set(0.0);
+ }
+ else {
+ ip_data.Append_info_string("y");
+ }
+ }
+ }
+ else {
+ iterates->y_c_NonConst()->Set(0.0);
+ iterates->y_d_NonConst()->Set(0.0);
+ }
+ ip_data.set_trial(iterates);
+
+ DBG_PRINT_VECTOR(2, "y_c", *ip_data.trial()->y_c());
+ DBG_PRINT_VECTOR(2, "y_d", *ip_data.trial()->y_d());
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpDefaultIterateInitializer.hpp b/SimTKmath/Optimizers/src/IpOpt/IpDefaultIterateInitializer.hpp
new file mode 100644
index 0000000..7eabfcd
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpDefaultIterateInitializer.hpp
@@ -0,0 +1,131 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpDefaultIterateInitializer.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-09-24
+
+#ifndef __IPDEFAULTITERATEINITIALIZER_HPP__
+#define __IPDEFAULTITERATEINITIALIZER_HPP__
+
+#include "IpIterateInitializer.hpp"
+#include "IpEqMultCalculator.hpp"
+
+namespace Ipopt
+{
+
+ /** Class implementing the default initialization procedure (based
+ * on user options) for the iterates. It is used at the very
+ * beginning of the optimization for determine the starting point
+ * for all variables.
+ */
+ class DefaultIterateInitializer: public IterateInitializer
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor. If eq_mult_calculator is not NULL, it will be
+ * used to compute the initial values for equality constraint
+ * multipliers. If warm_start_initializer is not NULL, it will
+ * be used to compute the initial values if the option
+ * warm_start_init_point is chosen. */
+ DefaultIterateInitializer
+ (const SmartPtr<EqMultiplierCalculator>& eq_mult_calculator,
+ const SmartPtr<IterateInitializer>& warm_start_initializer);
+
+ /** Default destructor */
+ virtual ~DefaultIterateInitializer()
+ {}
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Compute the initial iterates and set the into the curr field
+ * of the ip_data object. */
+ virtual bool SetInitialIterates();
+
+ /** Auxilliary function for moving the initial point. This is
+ * declared static so that it can also be used from
+ * WarmStartIterateInitializer. */
+ static void push_variables(const Journalist& jnlst,
+ Number bound_push,
+ Number bound_frac,
+ std::string name,
+ const Vector& orig_x,
+ SmartPtr<const Vector>& new_x,
+ const Vector& x_L,
+ const Vector& x_U,
+ const Matrix& Px_L,
+ const Matrix& Px_U);
+
+ /** Auxilliary function for computing least_square multipliers.
+ * The multipliers are computed based on the values in the trial
+ * fields (current is overwritten). On return, the multipliers
+ * are in the trial fields as well. The value of
+ * constr_mult_init_max determines if the computed least square
+ * estimate should be used, or if the initial multipliers are set
+ * to zero. */
+ static void least_square_mults(const Journalist& jnlst,
+ IpoptNLP& ip_nlp,
+ IpoptData& ip_data,
+ IpoptCalculatedQuantities& ip_cq,
+ const SmartPtr<EqMultiplierCalculator>& eq_mult_calculator,
+ Number constr_mult_init_max);
+
+
+ /** Methods for IpoptType */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> reg_options);
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ DefaultIterateInitializer();
+
+ /** Copy Constructor */
+ DefaultIterateInitializer(const DefaultIterateInitializer&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const DefaultIterateInitializer&);
+ //@}
+
+ /**@name Algorithmic Parameters */
+ //@{
+ /** Parameters for bumping x0 */
+ Number bound_push_;
+ /** Parameters for bumping x0 */
+ Number bound_frac_;
+
+ /** If max-norm of the initial equality constraint multiplier
+ * estimate is larger than this, the initial y_* variables are
+ * set to zero. */
+ Number constr_mult_init_max_;
+ /** Initial value for all bound mulitpliers. */
+ Number bound_mult_init_val_;
+ /** Flag indicating whether warm_start_initializer should be used
+ * instead of the default initialization */
+ bool warm_start_init_point_;
+ //@}
+
+ /** object to be used for the initialization of the equality
+ * constraint multipliers. */
+ SmartPtr<EqMultiplierCalculator> eq_mult_calculator_;
+
+ /** object to be used for a warm start initialization */
+ SmartPtr<IterateInitializer> warm_start_initializer_;
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpDenseGenMatrix.cpp b/SimTKmath/Optimizers/src/IpOpt/IpDenseGenMatrix.cpp
new file mode 100644
index 0000000..6b4f610
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpDenseGenMatrix.cpp
@@ -0,0 +1,300 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpDenseGenMatrix.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-12-24
+
+#include "IpDenseGenMatrix.hpp"
+#include "IpBlas.hpp"
+#include "IpLapack.hpp"
+
+namespace Ipopt
+{
+
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ DenseGenMatrix::DenseGenMatrix(const DenseGenMatrixSpace* owner_space)
+ :
+ Matrix(owner_space),
+ owner_space_(owner_space),
+ values_(new Number[NCols()*NRows()]),
+ initialized_(false)
+ {}
+
+ DenseGenMatrix::~DenseGenMatrix()
+ {
+ delete [] values_;
+ }
+
+ void DenseGenMatrix::ScaleColumns(const DenseVector& scal_vec)
+ {
+ DBG_ASSERT(scal_vec.Dim() == NCols());
+ DBG_ASSERT(initialized_);
+
+ const Number* scal_values = scal_vec.Values();
+
+ for (Index j=0; j<NCols(); j++) {
+ IpBlasDscal(NRows(), scal_values[j], &values_[j*NRows()], 1);
+ }
+ ObjectChanged();
+ }
+
+ void DenseGenMatrix::Copy(const DenseGenMatrix& M)
+ {
+ DBG_ASSERT(NCols()==M.NCols());
+ DBG_ASSERT(NRows()==M.NRows());
+
+ IpBlasDcopy(NCols()*NRows(), M.Values(), 1, values_, 1);
+ initialized_ = true;
+ ObjectChanged();
+ }
+
+ void DenseGenMatrix::FillIdentity(Number factor /*=1.*/)
+ {
+ DBG_ASSERT(NCols()==NRows());
+
+ const Number zero = 0.;
+ IpBlasDcopy(NCols()*NRows(), &zero, 0, values_, 1);
+
+ if (factor!=0.) {
+ for (Index i=0; i<NRows(); i++) {
+ values_[i + i*NRows()] = factor;
+ }
+ }
+ ObjectChanged();
+ initialized_ = true;
+ }
+
+ void DenseGenMatrix::AddMatrixProduct(Number alpha, const DenseGenMatrix& A,
+ bool transA, const DenseGenMatrix& B,
+ bool transB, Number beta)
+ {
+ Index m = NRows();
+ DBG_ASSERT((transA && A.NCols()==m) || (!transA && A.NRows()==m));
+ Index n = NCols();
+ DBG_ASSERT((transB && B.NRows()==n) || (!transB && B.NCols()==n));
+ Index k;
+ if (transA) {
+ k = A.NRows();
+ }
+ else {
+ k = A.NCols();
+ }
+ DBG_ASSERT((transB && B.NCols()==k) || (!transB && B.NRows()==k));
+ DBG_ASSERT(beta==0. || initialized_);
+
+ IpBlasDgemm(transA, transB, m, n, k, alpha, A.Values(), A.NRows(),
+ B.Values(), B.NRows(), beta, values_, NRows());
+ initialized_ = true;
+ ObjectChanged();
+ }
+
+ void DenseGenMatrix::HighRankUpdateTranspose(Number alpha,
+ const MultiVectorMatrix& V1,
+ const MultiVectorMatrix& V2,
+ Number beta)
+ {
+ DBG_ASSERT(NRows()==V1.NCols());
+ DBG_ASSERT(NCols()==V2.NCols());
+ DBG_ASSERT(beta==0. || initialized_);
+
+ if (beta==0.) {
+ for (Index j=0; j<NCols(); j++) {
+ for (Index i=0; i<NRows(); i++) {
+ values_[i+j*NRows()] = alpha*V1.GetVector(i)->Dot(*V2.GetVector(j));
+ }
+ }
+ }
+ else {
+ for (Index j=0; j<NCols(); j++) {
+ for (Index i=0; i<NRows(); i++) {
+ values_[i+j*NRows()] = alpha*V1.GetVector(i)->Dot(*V2.GetVector(j))
+ + beta*values_[i+j*NRows()];
+ }
+ }
+ }
+ initialized_ = true;
+ ObjectChanged();
+ }
+
+ bool DenseGenMatrix::ComputeCholeskyFactor(const DenseSymMatrix& M)
+ {
+ Index dim = M.Dim();
+ DBG_ASSERT(dim==NCols());
+ DBG_ASSERT(dim==NRows());
+
+ ObjectChanged();
+
+ // First we copy the content of the symmetric matrix into J
+ const Number* Mvalues = M.Values();
+ for (Index j=0; j<dim; j++) {
+ for (Index i=j; i<dim; i++) {
+ values_[i+j*dim] = Mvalues[i+j*dim];
+ }
+ }
+
+ // Now call the lapack subroutine to perform the factorization
+ Index info;
+ IpLapackDpotrf(dim, values_, dim, info);
+
+ DBG_ASSERT(info>=0);
+ if (info!=0) {
+ initialized_ = false;
+ return false;
+ }
+
+ // We set all strictly upper values to zero
+ // ToDo: This might not be necessary?!?
+ for (Index j=1; j<dim; j++) {
+ for (Index i=0; i<j; i++) {
+ values_[i+j*dim] = 0.;
+ }
+ }
+ initialized_ = true;
+ return true;
+ }
+
+ bool DenseGenMatrix::ComputeEigenVectors(const DenseSymMatrix& M,
+ DenseVector& Evalues)
+ {
+ Index dim = M.Dim();
+ DBG_ASSERT(Evalues.Dim()==dim);
+ DBG_ASSERT(NRows()==dim);
+ DBG_ASSERT(NCols()==dim);
+
+ // First we copy the content of the matrix into Q
+ const Number* Mvalues = M.Values();
+ for (Index j=0; j<dim; j++) {
+ for (Index i=j; i<dim; i++) {
+ values_[i+j*dim] = Mvalues[i+j*dim];
+ }
+ }
+
+ bool compute_eigenvectors = true;
+ Number* Evals = Evalues.Values();
+ Index info;
+ IpLapackDsyev(compute_eigenvectors, dim, values_,
+ dim, Evals, info);
+
+ initialized_ = (info==0);
+ ObjectChanged();
+ return (info==0);
+ }
+
+ void DenseGenMatrix::CholeskyBackSolveMatrix(bool trans, Number alpha,
+ DenseGenMatrix& B) const
+ {
+ DBG_ASSERT(NRows()==NCols());
+ DBG_ASSERT(B.NRows()==NRows());
+ DBG_ASSERT(initialized_);
+
+ Number* Bvalues = B.Values();
+
+ IpBlasDtrsm(trans, NRows(), B.NCols(), alpha,
+ values_, NRows(), Bvalues, B.NRows());
+ }
+
+ void DenseGenMatrix::CholeskySolveVector(DenseVector& b) const
+ {
+ DBG_ASSERT(NRows()==NCols());
+ DBG_ASSERT(b.Dim()==NRows());
+ DBG_ASSERT(initialized_);
+
+ Number* bvalues = b.Values();
+
+ IpLapackDpotrs(NRows(), 1, values_, NRows(), bvalues, b.Dim());
+ }
+
+ void DenseGenMatrix::CholeskySolveMatrix(DenseGenMatrix& B) const
+ {
+ DBG_ASSERT(NRows()==NCols());
+ DBG_ASSERT(B.NRows()==NRows());
+ DBG_ASSERT(initialized_);
+
+ Number* Bvalues = B.Values();
+
+ IpLapackDpotrs(NRows(), B.NCols(), values_, NRows(), Bvalues, B.NRows());
+ }
+
+ void DenseGenMatrix::MultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ // A few sanity checks
+ DBG_ASSERT(NCols()==x.Dim());
+ DBG_ASSERT(NRows()==y.Dim());
+ DBG_ASSERT(initialized_);
+
+ // See if we can understand the data
+ const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x);
+ DBG_ASSERT(dense_x); /* ToDo: Implement others */
+ DenseVector* dense_y = dynamic_cast<DenseVector*>(&y);
+ DBG_ASSERT(dense_y); /* ToDo: Implement others */
+
+ bool trans = false;
+ IpBlasDgemv(trans, NRows(), NCols(), alpha, values_, NRows(),
+ dense_x->Values(), 1, beta, dense_y->Values(), 1);
+ }
+
+ void DenseGenMatrix::TransMultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ // A few sanity checks
+ DBG_ASSERT(NCols()==y.Dim());
+ DBG_ASSERT(NRows()==x.Dim());
+ DBG_ASSERT(initialized_);
+
+ // See if we can understand the data
+ const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x);
+ DBG_ASSERT(dense_x); /* ToDo: Implement others */
+ DenseVector* dense_y = dynamic_cast<DenseVector*>(&y);
+ DBG_ASSERT(dense_y); /* ToDo: Implement others */
+
+ bool trans = true;
+ IpBlasDgemv(trans, NRows(), NCols(), alpha, values_, NRows(),
+ dense_x->Values(), 1, beta, dense_y->Values(), 1);
+ }
+
+ bool DenseGenMatrix::HasValidNumbersImpl() const
+ {
+ DBG_ASSERT(initialized_);
+ Number sum = IpBlasDasum(NRows()*NCols(), values_, 1);
+ return IsFiniteNumber(sum);
+ }
+
+ void DenseGenMatrix::PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ jnlst.Printf(level, category, "\n");
+ jnlst.PrintfIndented(level, category, indent,
+ "%sDenseGenMatrix \"%s\" with %d rows and %d columns:\n",
+ prefix.c_str(), name.c_str(), NRows(), NCols());
+
+ if (initialized_) {
+ for (Index j=0; j<NCols(); j++) {
+ for (Index i=0; i<NRows(); i++) {
+ jnlst.PrintfIndented(level, category, indent,
+ "%s%s[%5d,%5d]=%23.16e\n",
+ prefix.c_str(), name.c_str(), i, j, values_[i+NRows()*j]);
+ }
+ }
+ }
+ else {
+ jnlst.PrintfIndented(level, category, indent,
+ "The matrix has not yet been initialized!\n");
+ }
+ }
+
+ DenseGenMatrixSpace::DenseGenMatrixSpace(Index nRows, Index nCols)
+ :
+ MatrixSpace(nRows, nCols)
+ {}
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpDenseGenMatrix.hpp b/SimTKmath/Optimizers/src/IpOpt/IpDenseGenMatrix.hpp
new file mode 100644
index 0000000..3d77d75
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpDenseGenMatrix.hpp
@@ -0,0 +1,215 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpDenseGenMatrix.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-12-24
+
+#ifndef __IPDENSEGENMATRIX_HPP__
+#define __IPDENSEGENMATRIX_HPP__
+
+#include "IpUtils.hpp"
+#include "IpMatrix.hpp"
+#include "IpDenseVector.hpp"
+#include "IpDenseSymMatrix.hpp"
+
+namespace Ipopt
+{
+
+ /** forward declarations */
+ class DenseGenMatrixSpace;
+
+ /** Class for dense general matrices. Matrix elements are stored in
+ * one array in "Fortran" format.
+ */
+ class DenseGenMatrix : public Matrix
+ {
+ public:
+
+ /**@name Constructors / Destructors */
+ //@{
+
+ /** Constructor, taking the owner_space.
+ */
+ DenseGenMatrix(const DenseGenMatrixSpace* owner_space);
+
+ /** Destructor */
+ ~DenseGenMatrix();
+ //@}
+
+ /** Create a new DenseGenMatrix from same MatrixSpace */
+ SmartPtr<DenseGenMatrix> MakeNewDenseGenMatrix() const;
+
+ /** Retrieve the array for storing the matrix elements. This is
+ * the non-const version, and it is assume that afterwards the
+ * calling method will set all matrix elements. The matrix
+ * elements are stored one column after each other. */
+ Number* Values()
+ {
+ initialized_ = true;
+ ObjectChanged();
+ return values_;
+ }
+
+ /** Retrieve the array that stores the matrix elements. This is
+ * the const version, i.e., read-only. The matrix elements are
+ * stored one column after each other. */
+ const Number* Values() const
+ {
+ DBG_ASSERT(initialized_);
+ return values_;
+ }
+
+ /** Method for copying the content of another matrix into this
+ * matrix */
+ void Copy(const DenseGenMatrix& M);
+
+ /** Set this matrix to be a multiple of the identity matrix .
+ * This assumes that this matrix is square. */
+ void FillIdentity(Number factor=1.);
+
+ /** Method for scaling the columns of the matrix. The scaling
+ * factors are given in form of a DenseVector */
+ void ScaleColumns(const DenseVector& scal_vec);
+
+ /** Method for adding the product of two matrices to this matrix. */
+ void AddMatrixProduct(Number alpha, const DenseGenMatrix& A,
+ bool transA, const DenseGenMatrix& B,
+ bool transB, Number beta);
+
+ /** Method for adding a high-rank update to this matrix. It
+ * computes M = alpha*V1^T V2 + beta*M, where V1 and V2 are
+ * MultiVectorMatrices. */
+ void HighRankUpdateTranspose(Number alpha,
+ const MultiVectorMatrix& V1,
+ const MultiVectorMatrix& V2,
+ Number beta);
+
+ /** Method for computing the Cholesky factorization of a positive
+ * definite matrix. The factor is stored in this matrix, as
+ * lower-triangular matrix, i.e., M = J * J^T. The return values
+ * is false if the factorization could not be done, e.g., when
+ * the matrix is not positive definite. */
+ bool ComputeCholeskyFactor(const DenseSymMatrix& M);
+
+ /** Method for computing an eigenvalue decomposition of the given
+ * symmetrix matrix M. On return, this matrix contains the
+ * eigenvalues in its columns, and Evalues contains the
+ * eigenvalues. The return value is false, if there problems
+ * during the computation. */
+ bool ComputeEigenVectors(const DenseSymMatrix& M,
+ DenseVector& Evalues);
+
+ /** Method for performing one backsolve with an entire matrix on
+ * the right hand side, assuming that the this matrix is square
+ * and contains a lower triangular matrix. The incoming right
+ * hand side B is overwritten with the solution X of op(A)*X =
+ * alpha*B. op(A) = A or op(A) = A^T. */
+ void CholeskyBackSolveMatrix(bool trans, Number alpha,
+ DenseGenMatrix& B) const;
+
+ /** Method for performing a solve of a linear system for one
+ * vector, assuming that this matrix contains the Cholesky factor
+ * for the linear system. The vector b contains the right hand
+ * side on input, and contains the solution on output. */
+ void CholeskySolveVector(DenseVector& b) const;
+
+ /** Method for performing a solve of a linear system for one
+ * right-hand-side matrix, assuming that this matrix contains the
+ * Cholesky factor for the linear system. The matrix B contains
+ * the right hand sides on input, and contains the solution on
+ * output. */
+ void CholeskySolveMatrix(DenseGenMatrix& B) const;
+
+ protected:
+ /**@name Overloaded methods from Matrix base class*/
+ //@{
+ virtual void MultVectorImpl(Number alpha, const Vector &x, Number beta,
+ Vector &y) const;
+
+ virtual void TransMultVectorImpl(Number alpha, const Vector& x,
+ Number beta, Vector& y) const;
+
+ /** Method for determining if all stored numbers are valid (i.e.,
+ * no Inf or Nan). */
+ virtual bool HasValidNumbersImpl() const;
+
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const;
+ //@}
+
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ DenseGenMatrix();
+
+ /** Copy Constructor */
+ DenseGenMatrix(const DenseGenMatrix&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const DenseGenMatrix&);
+ //@}
+
+ const DenseGenMatrixSpace* owner_space_;
+
+ /** Array for storing the matrix elements (one columns after each
+ * other) */
+ Number* values_;
+
+ /** Flag indicating whether the values_ array has been initialized */
+ bool initialized_;
+ };
+
+ /** This is the matrix space for DenseGenMatrix.
+ */
+ class DenseGenMatrixSpace : public MatrixSpace
+ {
+ public:
+ /** @name Constructors / Destructors */
+ //@{
+ /** Constructor for matrix space for DenseGenMatrices. Takes in
+ * dimension of the matrices.
+ */
+ DenseGenMatrixSpace(Index nRows, Index nCols);
+
+ /** Destructor */
+ ~DenseGenMatrixSpace()
+ {}
+ //@}
+
+ /** Method for creating a new matrix of this specific type. */
+ DenseGenMatrix* MakeNewDenseGenMatrix() const
+ {
+ return new DenseGenMatrix(this);
+ }
+
+ /** Overloaded MakeNew method for the MatrixSpace base class.
+ */
+ virtual Matrix* MakeNew() const
+ {
+ return MakeNewDenseGenMatrix();
+ }
+
+ };
+
+ inline
+ SmartPtr<DenseGenMatrix> DenseGenMatrix::MakeNewDenseGenMatrix() const
+ {
+ return owner_space_->MakeNewDenseGenMatrix();
+ }
+
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpDenseSymMatrix.cpp b/SimTKmath/Optimizers/src/IpOpt/IpDenseSymMatrix.cpp
new file mode 100644
index 0000000..2000a48
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpDenseSymMatrix.cpp
@@ -0,0 +1,223 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpDenseSymMatrix.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-12-25
+
+#include "IpDenseSymMatrix.hpp"
+#include "IpDenseVector.hpp"
+#include "IpDenseGenMatrix.hpp"
+#include "IpBlas.hpp"
+
+namespace Ipopt
+{
+
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ DenseSymMatrix::DenseSymMatrix(const DenseSymMatrixSpace* owner_space)
+ :
+ SymMatrix(owner_space),
+ owner_space_(owner_space),
+ values_(new Number[NCols()*NRows()]),
+ initialized_(false)
+ {}
+
+ DenseSymMatrix::~DenseSymMatrix()
+ {
+ delete [] values_;
+ }
+
+ void DenseSymMatrix::MultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ // A few sanity checks
+ DBG_ASSERT(NCols()==x.Dim());
+ DBG_ASSERT(NRows()==y.Dim());
+ DBG_ASSERT(initialized_);
+
+ // See if we can understand the data
+ const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x);
+ DBG_ASSERT(dense_x); /* ToDo: Implement others */
+ DenseVector* dense_y = dynamic_cast<DenseVector*>(&y);
+ DBG_ASSERT(dense_y); /* ToDo: Implement others */
+
+ IpBlasDsymv(Dim(), alpha, values_, NRows(),
+ dense_x->Values(), 1, beta, dense_y->Values(), 1);
+ }
+
+ void DenseSymMatrix::FillIdentity(Number factor /*=1.*/)
+ {
+ const Index dim = Dim();
+ for (Index j=0; j<dim; j++) {
+ values_[j + j*dim] = factor;
+ for (Index i=j+1; i<dim; i++) {
+ values_[i + j*dim] = 0.;
+ }
+ }
+ ObjectChanged();
+ initialized_ = true;
+ }
+
+ void DenseSymMatrix::AddMatrix(Number alpha, const DenseSymMatrix& A,
+ Number beta)
+ {
+ DBG_ASSERT(beta==0. || initialized_);
+ DBG_ASSERT(Dim()==A.Dim());
+
+ if (alpha==0.)
+ return;
+
+ const Number* Avalues = A.Values();
+ const Index dim = Dim();
+ if (beta==0.) {
+ for (Index j=0; j<dim; j++) {
+ for (Index i=j; i<dim; i++) {
+ values_[i+j*dim] = alpha*Avalues[i+j*dim];
+ }
+ }
+ }
+ else if (beta==1.) {
+ for (Index j=0; j<dim; j++) {
+ for (Index i=j; i<dim; i++) {
+ values_[i+j*dim] += alpha*Avalues[i+j*dim];
+ }
+ }
+ }
+ else {
+ for (Index j=0; j<dim; j++) {
+ for (Index i=j; i<dim; i++) {
+ values_[i+j*dim] = alpha*Avalues[i+j*dim] + beta*values_[i+j*dim];
+ }
+ }
+ }
+ ObjectChanged();
+ initialized_ = true;
+ }
+
+ void DenseSymMatrix::HighRankUpdateTranspose(Number alpha,
+ const MultiVectorMatrix& V1,
+ const MultiVectorMatrix& V2,
+ Number beta)
+ {
+ DBG_ASSERT(Dim()==V1.NCols());
+ DBG_ASSERT(Dim()==V2.NCols());
+ DBG_ASSERT(beta==0. || initialized_);
+
+ const Index dim = Dim();
+ if (beta==0.) {
+ for (Index j=0; j<dim; j++) {
+ for (Index i=j; i<dim; i++) {
+ values_[i+j*dim] = alpha*V1.GetVector(i)->Dot(*V2.GetVector(j));
+ }
+ }
+ }
+ else {
+ for (Index j=0; j<dim; j++) {
+ for (Index i=j; i<dim; i++) {
+ values_[i+j*dim] = alpha*V1.GetVector(i)->Dot(*V2.GetVector(j))
+ + beta*values_[i+j*dim];
+ }
+ }
+ }
+ initialized_ = true;
+ ObjectChanged();
+ }
+
+ void DenseSymMatrix::HighRankUpdate(bool trans, Number alpha,
+ const DenseGenMatrix& V,
+ Number beta)
+ {
+ DBG_ASSERT((!trans && Dim()==V.NRows()) || (trans && Dim()==V.NCols()));
+ DBG_ASSERT(beta==0. || initialized_);
+
+ Index nrank;
+ if (trans) {
+ nrank = V.NRows();
+ }
+ else {
+ nrank = V.NCols();
+ }
+
+ IpBlasDsyrk(trans, Dim(), nrank, alpha, V.Values(), V.NRows(),
+ beta, values_, NRows());
+
+ initialized_ = true;
+ ObjectChanged();
+ }
+
+ void DenseSymMatrix::SpecialAddForLMSR1(const DenseVector& D,
+ const DenseGenMatrix& L)
+ {
+ const Index dim = Dim();
+ DBG_ASSERT(initialized_);
+ DBG_ASSERT(dim==D.Dim());
+ DBG_ASSERT(dim==L.NRows());
+ DBG_ASSERT(dim==L.NCols());
+
+ // First add the diagonal matrix
+ const Number* Dvalues = D.Values();
+ for (Index i=0; i<dim; i++) {
+ values_[i+i*dim] += Dvalues[i];
+ }
+
+ // Now add the strictly-lower triagular matrix L and its transpose
+ const Number* Lvalues = L.Values();
+ for (Index j=0; j<dim; j++) {
+ for (Index i=j+1; i<dim; i++) {
+ values_[i+j*dim] += Lvalues[i+j*dim];
+ }
+ }
+ ObjectChanged();
+ }
+
+ bool DenseSymMatrix::HasValidNumbersImpl() const
+ {
+ DBG_ASSERT(initialized_);
+ Number sum = 0.;
+ const Index dim = Dim();
+ for (Index j=0; j<dim; j++) {
+ sum += values_[j + j*dim];
+ for (Index i=j+1; i<dim; i++) {
+ sum += values_[i + j*dim];
+ }
+ }
+ return IsFiniteNumber(sum);
+ }
+
+ void DenseSymMatrix::PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ jnlst.Printf(level, category, "\n");
+ jnlst.PrintfIndented(level, category, indent,
+ "%sDenseSymMatrix \"%s\" of dimension %d (only lower triangular part printed):\n",
+ prefix.c_str(), name.c_str(), Dim());
+
+ if (initialized_) {
+ for (Index j=0; j<NCols(); j++) {
+ for (Index i=j; i<NRows(); i++) {
+ jnlst.PrintfIndented(level, category, indent,
+ "%s%s[%5d,%5d]=%23.16e\n",
+ prefix.c_str(), name.c_str(), i, j, values_[i+NRows()*j]);
+ }
+ }
+ }
+ else {
+ jnlst.PrintfIndented(level, category, indent,
+ "The matrix has not yet been initialized!\n");
+ }
+ }
+
+ DenseSymMatrixSpace::DenseSymMatrixSpace(Index nDim)
+ :
+ SymMatrixSpace(nDim)
+ {}
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpDenseSymMatrix.hpp b/SimTKmath/Optimizers/src/IpOpt/IpDenseSymMatrix.hpp
new file mode 100644
index 0000000..2a9f2d7
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpDenseSymMatrix.hpp
@@ -0,0 +1,184 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpDenseSymMatrix.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-12-25
+
+#ifndef __IPDENSESYMMATRIX_HPP__
+#define __IPDENSESYMMATRIX_HPP__
+
+#include "IpUtils.hpp"
+#include "IpSymMatrix.hpp"
+#include "IpMultiVectorMatrix.hpp"
+#include "IpDenseVector.hpp"
+
+namespace Ipopt
+{
+
+ /** forward declarations */
+ class DenseSymMatrixSpace;
+
+ /** forward declaration so that this include file can be included
+ * from DenseGenMatrix */
+ class DenseGenMatrix;
+
+ /** Class for dense symetrix matrices. Matrix elements are stored
+ * in one array in "Fortran" format, using BLAS "lower triangular"
+ * storage (not packed).
+ */
+ class DenseSymMatrix : public SymMatrix
+ {
+ public:
+
+ /**@name Constructors / Destructors */
+ //@{
+
+ /** Constructor, taking the owner_space.
+ */
+ DenseSymMatrix(const DenseSymMatrixSpace* owner_space);
+
+ /** Destructor */
+ ~DenseSymMatrix();
+ //@}
+
+ /** Create a new DenseSymMatrix from same MatrixSpace */
+ SmartPtr<DenseSymMatrix> MakeNewDenseSymMatrix() const;
+
+ /** Retrieve the array for storing the matrix elements. This is
+ * the non-const version, and it is assume that afterwards the
+ * calling method will set all matrix elements. The matrix
+ * elements are stored one column after each other. */
+ Number* Values()
+ {
+ ObjectChanged();
+ initialized_ = true;
+ return values_;
+ }
+
+ /** Retrieve the array that stores the matrix elements. This is
+ * the const version, i.e., read-only. The matrix elements are
+ * stored one column after each other. */
+ const Number* Values() const
+ {
+ DBG_ASSERT(initialized_);
+ return values_;
+ }
+
+ /** Set this matrix to be a multiple of the identity matrix. */
+ void FillIdentity(Number factor=1.);
+
+ /** Method for adding another matrix to this one. If B is this
+ * matrix, it becomes B = alpha * A + beta * B after this call. */
+ void AddMatrix(Number alpha, const DenseSymMatrix& A, Number beta);
+
+ /** Method for adding a high-rank update to this matrix. It
+ * computes M = alpha*op(V) op(V)^T + beta*M, where V is a
+ * DenseGenMatrix, where op(V) is V^T trans is true. */
+ void HighRankUpdate(bool trans, Number alpha, const DenseGenMatrix& V,
+ Number beta);
+
+ /** Method for adding a high-rank update to this matrix. It
+ * computes M = alpha*V1^T V2 + beta*M, where V1 and V2 are
+ * MultiVectorMatrices, so that V1^T V2 is symmetric. */
+ void HighRankUpdateTranspose(Number alpha,
+ const MultiVectorMatrix& V1,
+ const MultiVectorMatrix& V2,
+ Number beta);
+
+ /** Method for doing a specialized Add operation, required in the
+ * limited memory SR1 update. if M is this matrix, it computes M
+ * = M + D + L + L^T, where D is a diagonal matrix (given as a
+ * DenseVector), and L is a matrix that is assumed to be strictly
+ * lower triangular. */
+ void SpecialAddForLMSR1(const DenseVector& D, const DenseGenMatrix& L);
+
+ protected:
+ /**@name Overloaded methods from Matrix base class*/
+ //@{
+ virtual void MultVectorImpl(Number alpha, const Vector &x, Number beta,
+ Vector &y) const;
+
+ /** Method for determining if all stored numbers are valid (i.e.,
+ * no Inf or Nan). */
+ virtual bool HasValidNumbersImpl() const;
+
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const;
+ //@}
+
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ DenseSymMatrix();
+
+ /** Copy Constructor */
+ DenseSymMatrix(const DenseSymMatrix&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const DenseSymMatrix&);
+ //@}
+
+ const DenseSymMatrixSpace* owner_space_;
+
+ /** Array for storing the matrix elements (one columns after each
+ * other) */
+ Number* values_;
+
+ /** Flag indicating whether the values_ array has been initialized */
+ bool initialized_;
+ };
+
+ /** This is the matrix space for DenseSymMatrix.
+ */
+ class DenseSymMatrixSpace : public SymMatrixSpace
+ {
+ public:
+ /** @name Constructors / Destructors */
+ //@{
+ /** Constructor for matrix space for DenseSymMatrices. Takes in
+ * dimension of the matrices.
+ */
+ DenseSymMatrixSpace(Index nDim);
+
+ /** Destructor */
+ ~DenseSymMatrixSpace()
+ {}
+ //@}
+
+ /** Method for creating a new matrix of this specific type. */
+ DenseSymMatrix* MakeNewDenseSymMatrix() const
+ {
+ return new DenseSymMatrix(this);
+ }
+
+ /** Overloaded MakeNew method for the MatrixSpace base class.
+ */
+ virtual SymMatrix* MakeNewSymMatrix() const
+ {
+ return MakeNewDenseSymMatrix();
+ }
+
+ };
+
+ inline
+ SmartPtr<DenseSymMatrix> DenseSymMatrix::MakeNewDenseSymMatrix() const
+ {
+ return owner_space_->MakeNewDenseSymMatrix();
+ }
+
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpDenseVector.cpp b/SimTKmath/Optimizers/src/IpOpt/IpDenseVector.cpp
new file mode 100644
index 0000000..7e713fd
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpDenseVector.cpp
@@ -0,0 +1,1148 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpDenseVector.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpDenseVector.hpp"
+#include "IpBlas.hpp"
+#include "IpUtils.hpp"
+#include "IpDebug.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+namespace Ipopt
+{
+
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ DenseVector::DenseVector(const DenseVectorSpace* owner_space)
+ :
+ Vector(owner_space),
+ owner_space_(owner_space),
+ values_(NULL),
+ initialized_(false)
+ {
+ DBG_START_METH("DenseVector::DenseVector(Index dim)", dbg_verbosity);
+ if (Dim() == 0) {
+ initialized_ = true;
+ homogeneous_ = false;
+ }
+ }
+
+ DenseVector::~DenseVector()
+ {
+ DBG_START_METH("DenseVector::~DenseVector()", dbg_verbosity);
+ if (values_) {
+ owner_space_->FreeInternalStorage(values_);
+ }
+ }
+
+ void DenseVector::SetValues(const Number* x)
+ {
+ initialized_ = true;
+ IpBlasDcopy(Dim(), x, 1, values_allocated(), 1);
+ homogeneous_ = false;
+ // This is not an overloaded method from
+ // Vector. Here, we must call ObjectChanged()
+ // manually.
+ ObjectChanged();
+ }
+
+ void DenseVector::set_values_from_scalar()
+ {
+ DBG_ASSERT(homogeneous_);
+ initialized_ = true;
+ homogeneous_ = false;
+ Number* vals = values_allocated();
+ IpBlasDcopy(Dim(), &scalar_, 0, vals, 1);
+ }
+
+ void DenseVector::CopyImpl(const Vector& x)
+ {
+ DBG_START_METH("DenseVector::CopyImpl(const Vector& x)", dbg_verbosity);
+ const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x);
+ DBG_ASSERT(dense_x);
+ if (dense_x) {
+ DBG_ASSERT(dense_x->initialized_);
+ DBG_ASSERT(Dim() == dense_x->Dim());
+ homogeneous_ = dense_x->homogeneous_;
+ if (homogeneous_) {
+ scalar_ = dense_x->scalar_;
+ }
+ else {
+ IpBlasDcopy(Dim(), dense_x->values_, 1, values_allocated(), 1);
+ }
+ }
+ initialized_=true;
+ }
+
+ void DenseVector::ScalImpl(Number alpha)
+ {
+ DBG_ASSERT(initialized_);
+ if (homogeneous_) {
+ scalar_ *= alpha;
+ }
+ else {
+ IpBlasDscal(Dim(), alpha, values_, 1);
+ }
+ }
+
+ void DenseVector::AxpyImpl(Number alpha, const Vector &x)
+ {
+ DBG_ASSERT(initialized_);
+ const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x);
+ DBG_ASSERT(dense_x);
+ if (dense_x) {
+ DBG_ASSERT(dense_x->initialized_);
+ DBG_ASSERT(Dim() == dense_x->Dim());
+ if (homogeneous_) {
+ if (dense_x->homogeneous_) {
+ scalar_ += alpha * dense_x->scalar_;
+ }
+ else {
+ homogeneous_ = false;
+ Number* vals = values_allocated();
+ for (Index i=0; i<Dim(); i++) {
+ vals[i] = scalar_ + alpha*dense_x->values_[i];
+ }
+ }
+ }
+ else {
+ if (dense_x->homogeneous_) {
+ if (dense_x->scalar_!=0.) {
+ IpBlasDaxpy(Dim(), alpha, &dense_x->scalar_, 0, values_, 1);
+ }
+ }
+ else {
+ IpBlasDaxpy(Dim(), alpha, dense_x->values_, 1, values_, 1);
+ }
+ }
+ }
+ }
+
+ Number DenseVector::DotImpl(const Vector &x) const
+ {
+ DBG_ASSERT(initialized_);
+ Number retValue;
+ const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x);
+ DBG_ASSERT(dense_x);
+ DBG_ASSERT(dense_x->initialized_);
+ DBG_ASSERT(Dim() == dense_x->Dim());
+ if (homogeneous_) {
+ if (dense_x->homogeneous_) {
+ retValue = Dim() * scalar_ * dense_x->scalar_;
+ }
+ else {
+ retValue = IpBlasDdot(Dim(), dense_x->values_, 1, &scalar_, 0);
+ }
+ }
+ else {
+ if (dense_x->homogeneous_) {
+ retValue = IpBlasDdot(Dim(), &dense_x->scalar_, 0, values_, 1);
+ }
+ else {
+ retValue = IpBlasDdot(Dim(), dense_x->values_, 1, values_, 1);
+ }
+ }
+ return retValue;
+ }
+
+ Number DenseVector::Nrm2Impl() const
+ {
+ DBG_ASSERT(initialized_);
+ if (homogeneous_) {
+ return sqrt((double)Dim()) * fabs(scalar_);
+ }
+ else {
+ return IpBlasDnrm2(Dim(), values_, 1);
+ }
+ }
+
+ Number DenseVector::AsumImpl() const
+ {
+ DBG_ASSERT(initialized_);
+ if (homogeneous_) {
+ return Dim() * fabs(scalar_);
+ }
+ else {
+ return IpBlasDasum(Dim(), values_, 1);
+ }
+ }
+
+ Number DenseVector::AmaxImpl() const
+ {
+ DBG_ASSERT(initialized_);
+ if (Dim()==0) {
+ return 0.;
+ }
+ else {
+ if (homogeneous_) {
+ return fabs(scalar_);
+ }
+ else {
+ return fabs(values_[IpBlasIdamax(Dim(), values_, 1)-1]);
+ }
+ }
+ }
+
+ void DenseVector::SetImpl(Number value)
+ {
+ initialized_ = true;
+ homogeneous_ = true;
+ scalar_ = value;
+ // ToDo decide if we want this here:
+ if (values_) {
+ owner_space_->FreeInternalStorage(values_);
+ values_ = NULL;
+ }
+ }
+
+ void DenseVector::ElementWiseDivideImpl(const Vector& x)
+ {
+ DBG_ASSERT(initialized_);
+ const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x);
+ DBG_ASSERT(dense_x);
+ if (dense_x) {
+ DBG_ASSERT(dense_x->initialized_);
+ const Number* values_x = dense_x->values_;
+ DBG_ASSERT(Dim() == dense_x->Dim());
+ if (homogeneous_) {
+ if (dense_x->homogeneous_) {
+ scalar_ /= dense_x->scalar_;
+ }
+ else {
+ homogeneous_ = false;
+ Number* vals = values_allocated();
+ for (Index i=0; i<Dim(); i++) {
+ vals[i] = scalar_/values_x[i];
+ }
+ }
+ }
+ else {
+ if (dense_x->homogeneous_) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] /= dense_x->scalar_;
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] /= values_x[i];
+ }
+ }
+ }
+ }
+ }
+
+ void DenseVector::ElementWiseMultiplyImpl(const Vector& x)
+ {
+ DBG_ASSERT(initialized_);
+ const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x);
+ DBG_ASSERT(dense_x);
+ if (dense_x) {
+ DBG_ASSERT(dense_x->initialized_);
+ const Number* values_x = dense_x->values_;
+ DBG_ASSERT(Dim() == dense_x->Dim());
+ if (homogeneous_) {
+ if (dense_x->homogeneous_) {
+ scalar_ *= dense_x->scalar_;
+ }
+ else {
+ homogeneous_ = false;
+ Number* vals = values_allocated();
+ for (Index i=0; i<Dim(); i++) {
+ vals[i] = scalar_*values_x[i];
+ }
+ }
+ }
+ else {
+ if (dense_x->homogeneous_) {
+ if (dense_x->scalar_ != 1.0) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] *= dense_x->scalar_;
+ }
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] *= values_x[i];
+ }
+ }
+ }
+ }
+ }
+
+ void DenseVector::ElementWiseMaxImpl(const Vector& x)
+ {
+ DBG_ASSERT(initialized_);
+ const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x);
+ assert(dense_x); // ToDo: Implement Others
+ if (dense_x) {
+ DBG_ASSERT(dense_x->initialized_);
+ const Number* values_x = dense_x->values_;
+ DBG_ASSERT(Dim() == dense_x->Dim());
+ if (homogeneous_) {
+ if (dense_x->homogeneous_) {
+ scalar_ = Ipopt::Max(scalar_, dense_x->scalar_);
+ }
+ else {
+ homogeneous_ = false;
+ Number* vals = values_allocated();
+ for (Index i=0; i<Dim(); i++) {
+ vals[i] = Ipopt::Max(scalar_, values_x[i]);
+ }
+ }
+ }
+ else {
+ if (dense_x->homogeneous_) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = Ipopt::Max(values_[i], dense_x->scalar_);
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = Ipopt::Max(values_[i], values_x[i]);
+ }
+ }
+ }
+ }
+ }
+
+ void DenseVector::ElementWiseMinImpl(const Vector& x)
+ {
+ DBG_ASSERT(initialized_);
+ const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x);
+ DBG_ASSERT(dense_x);
+ if (dense_x) {
+ DBG_ASSERT(dense_x->initialized_);
+ const Number* values_x = dense_x->values_;
+ DBG_ASSERT(Dim() == dense_x->Dim());
+ if (homogeneous_) {
+ if (dense_x->homogeneous_) {
+ scalar_ = Ipopt::Min(scalar_, dense_x->scalar_);
+ }
+ else {
+ homogeneous_ = false;
+ Number* vals = values_allocated();
+ for (Index i=0; i<Dim(); i++) {
+ vals[i] = Ipopt::Min(scalar_, values_x[i]);
+ }
+ }
+ }
+ else {
+ if (dense_x->homogeneous_) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = Ipopt::Min(values_[i], dense_x->scalar_);
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = Ipopt::Min(values_[i], values_x[i]);
+ }
+ }
+ }
+ }
+ }
+
+ void DenseVector::ElementWiseReciprocalImpl()
+ {
+ DBG_ASSERT(initialized_);
+ if (homogeneous_) {
+ scalar_ = 1.0/scalar_;
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = 1.0/values_[i];
+ }
+ }
+ }
+
+ void DenseVector::ElementWiseAbsImpl()
+ {
+ DBG_ASSERT(initialized_);
+ if (homogeneous_) {
+ scalar_ = fabs(scalar_);
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = fabs(values_[i]);
+ }
+ }
+ }
+
+ void DenseVector::ElementWiseSqrtImpl()
+ {
+ DBG_ASSERT(initialized_);
+ if (homogeneous_) {
+ scalar_ = sqrt(scalar_);
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = sqrt(values_[i]);
+ }
+ }
+ }
+
+ void DenseVector::AddScalarImpl(Number scalar)
+ {
+ DBG_ASSERT(initialized_);
+ if (homogeneous_) {
+ scalar_ += scalar;
+ }
+ else {
+ IpBlasDaxpy(Dim(), 1., &scalar, 0, values_, 1);
+ }
+ }
+
+ Number DenseVector::MaxImpl() const
+ {
+ DBG_ASSERT(initialized_);
+ DBG_ASSERT(Dim() > 0 && "There is no Max of a zero length vector (no reasonable default can be returned)");
+ Number max;
+ if (homogeneous_) {
+ max = scalar_;
+ }
+ else {
+ max = values_[0];
+ for (Index i=1; i<Dim(); i++) {
+ max = Ipopt::Max(values_[i], max);
+ }
+ }
+ return max;
+ }
+
+ Number DenseVector::MinImpl() const
+ {
+ DBG_ASSERT(initialized_);
+ DBG_ASSERT(Dim() > 0 && "There is no Min of a zero length vector"
+ "(no reasonable default can be returned) - "
+ "Check for zero length vector before calling");
+ Number min;
+ if (homogeneous_) {
+ min = scalar_;
+ }
+ else {
+ min = values_[0];
+ for (Index i=1; i<Dim(); i++) {
+ min = Ipopt::Min(values_[i], min);
+ }
+ }
+ return min;
+ }
+
+ Number DenseVector::SumImpl() const
+ {
+ DBG_ASSERT(initialized_);
+ Number sum;
+ if (homogeneous_) {
+ sum = Dim()*scalar_;
+ }
+ else {
+ sum = 0.;
+ for (Index i=0; i<Dim(); i++) {
+ sum += values_[i];
+ }
+ }
+ return sum;
+ }
+
+ Number DenseVector::SumLogsImpl() const
+ {
+ DBG_ASSERT(initialized_);
+ Number sum;
+ if (homogeneous_) {
+ sum = Dim() * log(scalar_);
+ }
+ else {
+ sum = 0.0;
+ for (Index i=0; i<Dim(); i++) {
+ sum += log(values_[i]);
+ }
+ }
+ return sum;
+ }
+
+ void DenseVector::ElementWiseSgnImpl()
+ {
+ DBG_ASSERT(initialized_);
+ if (homogeneous_) {
+ if (scalar_ > 0.) {
+ scalar_ = 1.;
+ }
+ else if (scalar_ < 0.) {
+ scalar_ = -1.;
+ }
+ else {
+ scalar_ = 0.;
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ if (values_[i] > 0.) {
+ values_[i] = 1.;
+ }
+ else if (values_[i] < 0.) {
+ values_[i] = -1.;
+ }
+ else {
+ values_[i] = 0;
+ }
+ }
+ }
+ }
+
+ // Specialized Functions
+ void DenseVector::AddTwoVectorsImpl(Number a, const Vector& v1,
+ Number b, const Vector& v2, Number c)
+ {
+ Number* values_v1=NULL;
+ bool homogeneous_v1=false;
+ Number scalar_v1 = 0;
+ if (a!=0.) {
+ const DenseVector* dense_v1 = dynamic_cast<const DenseVector*>(&v1);
+ DBG_ASSERT(dense_v1);
+ DBG_ASSERT(dense_v1->initialized_);
+ DBG_ASSERT(Dim() == dense_v1->Dim());
+ values_v1=dense_v1->values_;
+ homogeneous_v1=dense_v1->homogeneous_;
+ if (homogeneous_v1)
+ scalar_v1 = dense_v1->scalar_;
+ }
+ Number* values_v2=NULL;
+ bool homogeneous_v2=false;
+ Number scalar_v2 = 0;
+ if (b!=0.) {
+ const DenseVector* dense_v2 = dynamic_cast<const DenseVector*>(&v2);
+ DBG_ASSERT(dense_v2);
+ DBG_ASSERT(dense_v2->initialized_);
+ DBG_ASSERT(Dim() == dense_v2->Dim());
+ values_v2=dense_v2->values_;
+ homogeneous_v2=dense_v2->homogeneous_;
+ if (homogeneous_v2)
+ scalar_v2 = dense_v2->scalar_;
+ }
+ DBG_ASSERT(c==0. || initialized_);
+ if ((c==0. || homogeneous_) && homogeneous_v1 && homogeneous_v2 ) {
+ homogeneous_ = true;
+ Number val = 0;
+ if (c!=0.) {
+ val = c*scalar_;
+ }
+ scalar_ = val + a*scalar_v1 + b*scalar_v2;
+ initialized_ = true;
+ return;
+ }
+ if (c==0.) {
+ // make sure we have memory allocated for this vector
+ values_allocated();
+ homogeneous_ = false;
+ }
+
+ // If any of the vectors is homogeneous, call the default implementation
+ if ( homogeneous_ || homogeneous_v1 || homogeneous_v2) {
+ // ToDo:Should we implement specialized methods here too?
+ Vector::AddTwoVectorsImpl(a, v1, b, v2, c);
+ return;
+ }
+
+ // I guess I'm going over board here, but it might be best to
+ // capture all cases for a, b, and c separately...
+ if (c==0 ) {
+ if (a==1.) {
+ if (b==0.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = values_v1[i];
+ }
+ }
+ else if (b==1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = values_v1[i] + values_v2[i];
+ }
+ }
+ else if (b==-1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = values_v1[i] - values_v2[i];
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = values_v1[i] + b*values_v2[i];
+ }
+ }
+ }
+ else if (a==-1.) {
+ if (b==0.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = -values_v1[i];
+ }
+ }
+ else if (b==1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = -values_v1[i] + values_v2[i];
+ }
+ }
+ else if (b==-1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = -values_v1[i] - values_v2[i];
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = -values_v1[i] + b*values_v2[i];
+ }
+ }
+ }
+ else if (a==0.) {
+ if (b==0.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = 0.;
+ }
+ }
+ else if (b==1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = values_v2[i];
+ }
+ }
+ else if (b==-1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = -values_v2[i];
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = b*values_v2[i];
+ }
+ }
+ }
+ else {
+ if (b==0.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = a*values_v1[i];
+ }
+ }
+ else if (b==1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = a*values_v1[i] + values_v2[i];
+ }
+ }
+ else if (b==-1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = a*values_v1[i] - values_v2[i];
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = a*values_v1[i] + b*values_v2[i];
+ }
+ }
+ }
+ }
+ else if (c==1.) {
+ if (a==1.) {
+ if (b==0.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] += values_v1[i];
+ }
+ }
+ else if (b==1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] += values_v1[i] + values_v2[i];
+ }
+ }
+ else if (b==-1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] += values_v1[i] - values_v2[i];
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] += values_v1[i] + b*values_v2[i];
+ }
+ }
+ }
+ else if (a==-1.) {
+ if (b==0.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] -= values_v1[i];
+ }
+ }
+ else if (b==1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] += -values_v1[i] + values_v2[i];
+ }
+ }
+ else if (b==-1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] += -values_v1[i] - values_v2[i];
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] += -values_v1[i] + b*values_v2[i];
+ }
+ }
+ }
+ else if (a==0.) {
+ if (b==0.) {
+ /* Nothing */
+ }
+ else if (b==1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] += values_v2[i];
+ }
+ }
+ else if (b==-1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] -= values_v2[i];
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] += b*values_v2[i];
+ }
+ }
+ }
+ else {
+ if (b==0.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] += a*values_v1[i];
+ }
+ }
+ else if (b==1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] += a*values_v1[i] + values_v2[i];
+ }
+ }
+ else if (b==-1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] += a*values_v1[i] - values_v2[i];
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] += a*values_v1[i] + b*values_v2[i];
+ }
+ }
+ }
+ }
+ else if (c==-1.) {
+ if (a==1.) {
+ if (b==0.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = values_v1[i] - values_[i];
+ }
+ }
+ else if (b==1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = values_v1[i] + values_v2[i] - values_[i];
+ }
+ }
+ else if (b==-1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = values_v1[i] - values_v2[i] - values_[i];
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = values_v1[i] + b*values_v2[i] - values_[i];
+ }
+ }
+ }
+ else if (a==-1.) {
+ if (b==0.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = -values_v1[i] - values_[i];
+ }
+ }
+ else if (b==1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = -values_v1[i] + values_v2[i] - values_[i];
+ }
+ }
+ else if (b==-1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = -values_v1[i] - values_v2[i] - values_[i];
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = -values_v1[i] + b*values_v2[i] - values_[i];
+ }
+ }
+ }
+ else if (a==0.) {
+ if (b==0.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] *= -1.;
+ }
+ }
+ else if (b==1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = values_v2[i] - values_[i];
+ }
+ }
+ else if (b==-1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = -values_v2[i] - values_[i];
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = b*values_v2[i] - values_[i];
+ }
+ }
+ }
+ else {
+ if (b==0.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = a*values_v1[i] - values_[i];
+ }
+ }
+ else if (b==1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = a*values_v1[i] + values_v2[i] - values_[i];
+ }
+ }
+ else if (b==-1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = a*values_v1[i] - values_v2[i] - values_[i];
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = a*values_v1[i] + b*values_v2[i] - values_[i];
+ }
+ }
+ }
+ }
+ else {
+ if (a==1.) {
+ if (b==0.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = values_v1[i] + c*values_[i];
+ }
+ }
+ else if (b==1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = values_v1[i] + values_v2[i] + c*values_[i];
+ }
+ }
+ else if (b==-1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = values_v1[i] - values_v2[i] + c*values_[i];
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = values_v1[i] + b*values_v2[i] + c*values_[i];
+ }
+ }
+ }
+ else if (a==-1.) {
+ if (b==0.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = -values_v1[i] + c*values_[i];
+ }
+ }
+ else if (b==1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = -values_v1[i] + values_v2[i] + c*values_[i];
+ }
+ }
+ else if (b==-1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = -values_v1[i] - values_v2[i] + c*values_[i];
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = -values_v1[i] + b*values_v2[i] + c*values_[i];
+ }
+ }
+ }
+ else if (a==0.) {
+ if (b==0.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] *= c;
+ }
+ }
+ else if (b==1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = values_v2[i] + c*values_[i];
+ }
+ }
+ else if (b==-1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = -values_v2[i] + c*values_[i];
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = b*values_v2[i] + c*values_[i];
+ }
+ }
+ }
+ else {
+ if (b==0.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = a*values_v1[i] + c*values_[i];
+ }
+ }
+ else if (b==1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = a*values_v1[i] + values_v2[i] + c*values_[i];
+ }
+ }
+ else if (b==-1.) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = a*values_v1[i] - values_v2[i] + c*values_[i];
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = a*values_v1[i] + b*values_v2[i] + c*values_[i];
+ }
+ }
+ }
+ }
+ initialized_=true;
+ }
+
+ Number
+ DenseVector::FracToBoundImpl(const Vector& delta, Number tau) const
+ {
+ DBG_ASSERT(Dim()==delta.Dim());
+ DBG_ASSERT(tau>=0.);
+ const DenseVector* dense_delta = dynamic_cast<const DenseVector*>(&delta);
+ DBG_ASSERT(dense_delta);
+
+ Number alpha = 1.;
+ Number* values_x = values_;
+ Number* values_delta = dense_delta->values_;
+ if (homogeneous_) {
+ if (dense_delta->homogeneous_) {
+ if (dense_delta->scalar_<0.) {
+ alpha = Ipopt::Min(alpha, -tau/dense_delta->scalar_ * scalar_);
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ if (values_delta[i]<0.) {
+ alpha = Ipopt::Min(alpha, -tau/values_delta[i] * scalar_);
+ }
+ }
+ }
+ }
+ else {
+ if (dense_delta->homogeneous_) {
+ if (dense_delta->scalar_<0.) {
+ for (Index i=0; i<Dim(); i++) {
+ alpha = Ipopt::Min(alpha, -tau/dense_delta->scalar_ * values_x[i]);
+ }
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ if (values_delta[i]<0.) {
+ alpha = Ipopt::Min(alpha, -tau/values_delta[i] * values_x[i]);
+ }
+ }
+ }
+ }
+
+ DBG_ASSERT(alpha>=0.);
+ return alpha;
+ }
+
+ void DenseVector::AddVectorQuotientImpl(Number a, const Vector& z,
+ const Vector& s, Number c)
+ {
+ DBG_ASSERT(Dim()==z.Dim());
+ DBG_ASSERT(Dim()==s.Dim());
+ const DenseVector* dense_z = dynamic_cast<const DenseVector*>(&z);
+ DBG_ASSERT(dense_z);
+ DBG_ASSERT(dense_z->initialized_);
+ const DenseVector* dense_s = dynamic_cast<const DenseVector*>(&s);
+ DBG_ASSERT(dense_s);
+ DBG_ASSERT(dense_s->initialized_);
+
+ DBG_ASSERT(c==0. || initialized_);
+ bool homogeneous_z = dense_z->homogeneous_;
+ bool homogeneous_s = dense_s->homogeneous_;
+
+ if ((c==0. || homogeneous_) && homogeneous_z && homogeneous_s) {
+ if (c==0.) {
+ scalar_ = a * dense_z->scalar_ / dense_s->scalar_;
+ }
+ else {
+ scalar_ = c * scalar_ + a * dense_z->scalar_ / dense_s->scalar_;
+ }
+ initialized_ = true;
+ homogeneous_ = true;
+ if (values_) {
+ owner_space_->FreeInternalStorage(values_);
+ values_ = NULL;
+ }
+ return;
+ }
+
+ // At least one is not homogeneous
+ // Make sure we have memory to store a non-homogeneous vector
+ values_allocated();
+
+ Number* values_z = dense_z->values_;
+ Number* values_s = dense_s->values_;
+
+ if (c==0.) {
+ if (homogeneous_z) {
+ // then s is not homogeneous
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = a * dense_z->scalar_ / values_s[i];
+ }
+ }
+ else if (homogeneous_s) {
+ // then z is not homogeneous
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = values_z[i] * a / dense_s->scalar_;
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = a * values_z[i] / values_s[i];
+ }
+ }
+ }
+ else if (homogeneous_) {
+ Number val = c*scalar_;
+ if (homogeneous_z) {
+ // then s is not homogeneous
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = val + a * dense_z->scalar_ / values_s[i];
+ }
+ }
+ else if (homogeneous_s) {
+ // then z is not homogeneous
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = val + values_z[i] * a / dense_s->scalar_;
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = val + a * values_z[i] / values_s[i];
+ }
+ }
+ }
+ else {
+ // ToDo could distinguish c = 1
+ if (homogeneous_z) {
+ if (homogeneous_s) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = c*values_[i] + a * dense_z->scalar_/dense_s->scalar_;
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = c*values_[i] + a * dense_z->scalar_/values_s[i];
+ }
+ }
+ }
+ else {
+ if (homogeneous_s) {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = c*values_[i] + values_z[i] * a /dense_s->scalar_;
+ }
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ values_[i] = c*values_[i] + a * values_z[i]/values_s[i];
+ }
+ }
+ }
+ }
+
+ initialized_ = true;
+ homogeneous_ = false;
+ }
+
+ void DenseVector::CopyToPos(Index Pos, const Vector& x)
+ {
+ Index dim_x = x.Dim();
+ DBG_ASSERT(dim_x+Pos<=Dim());
+ const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x);
+ DBG_ASSERT(dense_x);
+
+ Number* vals = values_allocated();
+ homogeneous_ = false;
+
+ if (dense_x->homogeneous_) {
+ IpBlasDcopy(dim_x, &scalar_, 1, vals+Pos, 1);
+ }
+ else {
+ IpBlasDcopy(dim_x, dense_x->values_, 1, vals+Pos, 1);
+ }
+ initialized_ = true;
+ ObjectChanged();
+ }
+
+ void DenseVector::CopyFromPos(Index Pos, Vector& x) const
+ {
+ Index dim_x = x.Dim();
+ DBG_ASSERT(dim_x+Pos<=Dim());
+ DenseVector* dense_x = dynamic_cast<DenseVector*>(&x);
+ DBG_ASSERT(dense_x);
+ DBG_ASSERT(dense_x->homogeneous_); // This might have to made more general
+
+ if (homogeneous_) {
+ IpBlasDcopy(dim_x, &scalar_, 1, dense_x->values_, 1);
+ }
+ else {
+ IpBlasDcopy(dim_x, values_+Pos, 1, dense_x->values_, 1);
+ }
+ // We need to tell X that it has changed!
+ dense_x->ObjectChanged();
+ dense_x->initialized_=true;
+ }
+
+ void DenseVector::PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ jnlst.PrintfIndented(level, category, indent,
+ "%sDenseVector \"%s\" with %d elements:\n",
+ prefix.c_str(), name.c_str(), Dim());
+ if (initialized_) {
+ if (homogeneous_) {
+ jnlst.PrintfIndented(level, category, indent,
+ "%sHomogeneous vector, all elements have value %23.16e\n",
+ prefix.c_str(), scalar_);
+ }
+ else {
+ for (Index i=0; i<Dim(); i++) {
+ jnlst.PrintfIndented(level, category, indent,
+ "%s%s[%5d]=%23.16e\n",
+ prefix.c_str(), name.c_str(), i+1, values_[i]);
+ }
+ }
+ }
+ else {
+ jnlst.PrintfIndented(level, category, indent,
+ "%sUninitialized!\n",
+ prefix.c_str());
+ }
+ }
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpDenseVector.hpp b/SimTKmath/Optimizers/src/IpOpt/IpDenseVector.hpp
new file mode 100644
index 0000000..acba466
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpDenseVector.hpp
@@ -0,0 +1,330 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpDenseVector.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPDENSEVECTOR_HPP__
+#define __IPDENSEVECTOR_HPP__
+
+#include "IpUtils.hpp"
+#include "IpVector.hpp"
+
+namespace Ipopt
+{
+
+ /* forward declarations */
+ class DenseVectorSpace;
+
+ /** Dense Vector Implementation.
+ */
+ class DenseVector : public Vector
+ {
+ public:
+
+ /**@name Constructors / Destructors */
+ //@{
+ /** Default Constructor
+ */
+ DenseVector(const DenseVectorSpace* owner_space);
+
+ /** Destructor
+ */
+ virtual ~DenseVector();
+ //@}
+
+ /** @name Additional public methods not in Vector base class. */
+ //@{
+ /** Create a new DenseVector from same VectorSpace */
+ SmartPtr<DenseVector> MakeNewDenseVector() const;
+
+ /** Set elements in the vector to the Number array x. */
+ void SetValues(const Number *x);
+
+ /** Obtain pointer to the internal Number array with vector
+ * elements with the indention to change the vector data (USE
+ * WITH CARE!). This does not produce a copy, and lifetime is not
+ * guaranteed!
+ */
+ inline Number* Values();
+
+ /** Obtain pointer to the internal Number array with vector
+ * elements without the intention to change the vector data (USE
+ * WITH CARE!). This does not produce a copy, and lifetime is not
+ * guaranteed!
+ */
+ inline const Number* Values() const;
+
+ /** Indicates if the vector is homogeneous (i.e., all entries have
+ * the value Scalar() */
+ bool IsHomogeneous() const
+ {
+ return homogeneous_;
+ }
+
+ /** Scalar value of all entries in a homogeneous vector */
+ Number Scalar() const
+ {
+ DBG_ASSERT(homogeneous_);
+ return scalar_;
+ }
+ //@}
+
+ /** @name Modifying subranges of the vector. */
+ //@{
+ /** Copy the data in x into the subrange of this vector starting
+ * at position Pos in this vector. Position count starts at 0.
+ */
+ void CopyToPos(Index Pos, const Vector& x);
+ /** Copy the data in this vector's subrange starting
+ * at position Pos to Vector x. Position count starts at 0.
+ */
+ void CopyFromPos(Index Pos, Vector& x) const;
+ //@}
+
+ protected:
+ /** @name Overloaded methods from Vector base class */
+ //@{
+ /** Copy the data of the vector x into this vector (DCOPY). */
+ virtual void CopyImpl(const Vector& x);
+
+ /** Scales the vector by scalar alpha (DSCAL) */
+ virtual void ScalImpl(Number alpha);
+
+ /** Add the multiple alpha of vector x to this vector (DAXPY) */
+ virtual void AxpyImpl(Number alpha, const Vector &x);
+
+ /** Computes inner product of vector x with this (DDOT) */
+ virtual Number DotImpl(const Vector &x) const;
+
+ /** Computes the 2-norm of this vector (DNRM2) */
+ virtual Number Nrm2Impl() const;
+
+ /** Computes the 1-norm of this vector (DASUM) */
+ virtual Number AsumImpl() const;
+
+ /** Computes the max-norm of this vector (based on IDAMAX) */
+ virtual Number AmaxImpl() const;
+
+ /** Set each element in the vector to the scalar alpha. */
+ virtual void SetImpl(Number value);
+
+ /** Element-wise division \f$y_i \gets y_i/x_i\f$.*/
+ virtual void ElementWiseDivideImpl(const Vector& x);
+
+ /** Element-wise multiplication \f$y_i \gets y_i*x_i\f$.*/
+ virtual void ElementWiseMultiplyImpl(const Vector& x);
+
+ /** Set entry to max of itself and the corresponding element in x */
+ virtual void ElementWiseMaxImpl(const Vector& x);
+
+ /** Set entry to min of itself and the corresponding element in x */
+ virtual void ElementWiseMinImpl(const Vector& x);
+
+ /** reciprocates the elements of the vector */
+ virtual void ElementWiseReciprocalImpl();
+
+ /** take abs of the elements of the vector */
+ virtual void ElementWiseAbsImpl();
+
+ /** take square-root of the elements of the vector */
+ virtual void ElementWiseSqrtImpl();
+
+ /** Changes each entry in the vector to its sgn value */
+ virtual void ElementWiseSgnImpl();
+
+ /** Add scalar to every component of the vector.*/
+ virtual void AddScalarImpl(Number scalar);
+
+ /** Max value in the vector */
+ virtual Number MaxImpl() const;
+
+ /** Min value in the vector */
+ virtual Number MinImpl() const;
+
+ /** Computes the sum of the lements of vector */
+ virtual Number SumImpl() const;
+
+ /** Computes the sum of the logs of the elements of vector */
+ virtual Number SumLogsImpl() const;
+
+ /** @name Implemented specialized functions */
+ //@{
+ /** Add two vectors (a * v1 + b * v2). Result is stored in this
+ vector. */
+ void AddTwoVectorsImpl(Number a, const Vector& v1,
+ Number b, const Vector& v2, Number c);
+ /** Fraction to the boundary parameter. */
+ Number FracToBoundImpl(const Vector& delta, Number tau) const;
+ /** Add the quotient of two vectors, y = a * z/s + c * y. */
+ void AddVectorQuotientImpl(Number a, const Vector& z, const Vector& s,
+ Number c);
+ //@}
+
+ /** @name Output methods */
+ //@{
+ /* Print the entire vector with padding */
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const;
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ DenseVector();
+
+ /** Copy Constructor */
+ DenseVector(const DenseVector&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const DenseVector&);
+ //@}
+
+ /** Copy of the owner_space ptr as a DenseVectorSpace instead
+ * of a VectorSpace
+ */
+ const DenseVectorSpace* owner_space_;
+
+ /** Dense Number array of vector values. */
+ Number* values_;
+
+ /** Method of getting the internal values array, making sure that
+ * memory has been allocated */
+ Number* values_allocated();
+
+ /** Flag for Initialization. This flag is false, if the data has
+ not yet been initialized. */
+ bool initialized_;
+
+ /** Flag indicating whether the vector is currently homogeneous
+ * (that is, all elements have the same value). This flag is used
+ * to determine whether the elements of the vector are stored in
+ * values_ or in scalar_ */
+ bool homogeneous_;
+
+ /** Homogeneous value of all elements if the vector is currently
+ * homogenous */
+ Number scalar_;
+
+ /** Auxilliary method for setting explicitly all elements in
+ * values_ to the current scalar value. */
+ void set_values_from_scalar();
+ };
+
+ /** This vectors space is the vector space for DenseVector.
+ */
+ class DenseVectorSpace : public VectorSpace
+ {
+ public:
+ /** @name Constructors/Destructors. */
+ //@{
+ /** Constructor, requires dimension of all vector for this
+ * VectorSpace
+ */
+ DenseVectorSpace(Index dim)
+ :
+ VectorSpace(dim)
+ {}
+
+ /** Destructor */
+ ~DenseVectorSpace()
+ {}
+ //@}
+
+ /** Method for creating a new vector of this specific type. */
+ DenseVector* MakeNewDenseVector() const
+ {
+ return new DenseVector(this);
+ }
+
+ /** Instantiation of the generate MakeNew method for the
+ * VectorSpace base class.
+ */
+ virtual Vector* MakeNew() const
+ {
+ return MakeNewDenseVector();
+ }
+
+ /**@name Methods called by DenseVector for memory management.
+ * This could allow to have sophisticated memory management in the
+ * VectorSpace.
+ */
+ //@{
+ /** Allocate internal storage for the DenseVector */
+ Number* AllocateInternalStorage() const;
+
+ /** Deallocate internal storage for the DenseVector */
+ void FreeInternalStorage(Number* values) const;
+ //@}
+ };
+
+ // inline functions
+ inline Number* DenseVector::Values()
+ {
+ // Here we assume that every time someone requests this direct raw
+ // pointer, the data is going to change and the Tag for this
+ // vector has to be updated.
+
+ if (initialized_ && homogeneous_) {
+ // If currently the vector is a homogeneous vector, set all elements
+ // explicitly to this value
+ set_values_from_scalar();
+ }
+ ObjectChanged();
+ initialized_= true;
+ homogeneous_ = false;
+ return values_allocated();
+ }
+
+ inline const Number* DenseVector::Values() const
+ {
+ DBG_ASSERT(initialized_ && (Dim()==0 || values_));
+ return values_;
+ }
+
+ inline Number* DenseVector::values_allocated()
+ {
+ if (values_==NULL) {
+ values_ = owner_space_->AllocateInternalStorage();
+ }
+ return values_;
+ }
+
+ inline
+ Number* DenseVectorSpace::AllocateInternalStorage() const
+ {
+ if (Dim()>0) {
+ return new Number[Dim()];
+ }
+ else {
+ return NULL;
+ }
+ }
+
+ inline
+ void DenseVectorSpace::FreeInternalStorage(Number* values) const
+ {
+ delete [] values;
+ }
+
+ inline
+ SmartPtr<DenseVector> DenseVector::MakeNewDenseVector() const
+ {
+ return owner_space_->MakeNewDenseVector();
+ }
+
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpDiagMatrix.cpp b/SimTKmath/Optimizers/src/IpOpt/IpDiagMatrix.cpp
new file mode 100644
index 0000000..eded176
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpDiagMatrix.cpp
@@ -0,0 +1,69 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpDiagMatrix.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpDiagMatrix.hpp"
+
+namespace Ipopt
+{
+
+ DiagMatrix::DiagMatrix(const SymMatrixSpace* owner_space)
+ :
+ SymMatrix(owner_space)
+ {}
+
+ DiagMatrix::~DiagMatrix()
+ {}
+
+ void DiagMatrix::MultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ // A few sanity checks
+ DBG_ASSERT(Dim()==x.Dim());
+ DBG_ASSERT(Dim()==y.Dim());
+ DBG_ASSERT(IsValid(diag_));
+
+ // Take care of the y part of the addition
+ if( beta!=0.0 ) {
+ y.Scal(beta);
+ }
+ else {
+ y.Set(0.0); // In case y hasn't been initialized yet
+ }
+
+ SmartPtr<Vector> tmp_vec = y.MakeNew();
+ tmp_vec->Copy(x);
+ tmp_vec->ElementWiseMultiply(*diag_);
+ y.Axpy(alpha, *tmp_vec);
+ }
+
+ bool DiagMatrix::HasValidNumbersImpl() const
+ {
+ DBG_ASSERT(IsValid(diag_));
+ return diag_->HasValidNumbers();
+ }
+
+ void DiagMatrix::PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ jnlst.Printf(level, category, "\n");
+ jnlst.PrintfIndented(level, category, indent,
+ "%sDiagMatrix \"%s\" with %d rows and columns, and with diagonal elements:\n",
+ prefix.c_str(), name.c_str(), Dim());
+ if (IsValid(diag_)) {
+ diag_->Print(&jnlst, level, category, name, indent+1, prefix);
+ }
+ else {
+ jnlst.PrintfIndented(level, category, indent,
+ "%sDiagonal elements not set!\n", prefix.c_str());
+ }
+ }
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpDiagMatrix.hpp b/SimTKmath/Optimizers/src/IpOpt/IpDiagMatrix.hpp
new file mode 100644
index 0000000..8525a5d
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpDiagMatrix.hpp
@@ -0,0 +1,139 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpDiagMatrix.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPDIAGMATRIX_HPP__
+#define __IPDIAGMATRIX_HPP__
+
+#include "IpUtils.hpp"
+#include "IpSymMatrix.hpp"
+
+namespace Ipopt
+{
+
+ /** Class for diagonal matrices. The diagonal is stored as a
+ * Vector. */
+ class DiagMatrix : public SymMatrix
+ {
+ public:
+
+ /**@name Constructors / Destructors */
+ //@{
+
+ /** Constructor, given the corresponding matrix space. */
+ DiagMatrix(const SymMatrixSpace* owner_space);
+
+ /** Destructor */
+ ~DiagMatrix();
+ //@}
+
+ /** Method for setting the diagonal elements (as a Vector). */
+ void SetDiag(const Vector& diag)
+ {
+ diag_ = &diag;
+ }
+
+ /** Method for setting the diagonal elements. */
+ SmartPtr<const Vector> GetDiag() const
+ {
+ return diag_;
+ }
+
+ protected:
+ /**@name Methods overloaded from matrix */
+ //@{
+ virtual void MultVectorImpl(Number alpha, const Vector& x,
+ Number beta, Vector& y) const;
+
+ /** Method for determining if all stored numbers are valid (i.e.,
+ * no Inf or Nan). */
+ virtual bool HasValidNumbersImpl() const;
+
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const;
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ DiagMatrix();
+
+ /** Copy Constructor */
+ DiagMatrix(const DiagMatrix&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const DiagMatrix&);
+ //@}
+
+ /** Vector storing the diagonal elements */
+ SmartPtr<const Vector> diag_;
+ };
+
+ /** This is the matrix space for DiagMatrix. */
+ class DiagMatrixSpace : public SymMatrixSpace
+ {
+ public:
+ /** @name Constructors / Destructors */
+ //@{
+ /** Constructor, given the dimension of the matrix. */
+ DiagMatrixSpace(Index dim)
+ :
+ SymMatrixSpace(dim)
+ {}
+
+ /** Destructor */
+ virtual ~DiagMatrixSpace()
+ {}
+ //@}
+
+ /** Overloaded MakeNew method for the SymMatrixSpace base class.
+ */
+ virtual SymMatrix* MakeNewSymMatrix() const
+ {
+ return MakeNewDiagMatrix();
+ }
+
+ /** Method for creating a new matrix of this specific type. */
+ DiagMatrix* MakeNewDiagMatrix() const
+ {
+ return new DiagMatrix(this);
+ }
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ DiagMatrixSpace();
+
+ /** Copy Constructor */
+ DiagMatrixSpace(const DiagMatrixSpace&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const DiagMatrixSpace&);
+ //@}
+
+ };
+
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpEqMultCalculator.hpp b/SimTKmath/Optimizers/src/IpOpt/IpEqMultCalculator.hpp
new file mode 100644
index 0000000..ac464c9
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpEqMultCalculator.hpp
@@ -0,0 +1,64 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpEqMultCalculator.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-09-23
+
+#ifndef __IPEQMULTCALCULATOR_HPP__
+#define __IPEQMULTCALCULATOR_HPP__
+
+#include "IpUtils.hpp"
+#include "IpAlgStrategy.hpp"
+
+namespace Ipopt
+{
+ /** Base Class for objects that compute estimates for the equality
+ * constraint multipliers y_c and y_d. For example, this is the
+ * base class for objects for computing least square multipliers or
+ * coordinate multipliers. */
+ class EqMultiplierCalculator: public AlgorithmStrategyObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor. */
+ EqMultiplierCalculator()
+ {}
+ /** Default destructor */
+ virtual ~EqMultiplierCalculator()
+ {}
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix) = 0;
+
+ /** This method computes the estimates for y_c and y_d at the
+ * current point. If the estimates cannot be computed (e.g. some
+ * linear system is singular), the return value of this method is
+ * false. */
+ virtual bool CalculateMultipliers(Vector& y_c,
+ Vector& y_d) = 0;
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ EqMultiplierCalculator(const EqMultiplierCalculator&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const EqMultiplierCalculator&);
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpExactHessianUpdater.cpp b/SimTKmath/Optimizers/src/IpOpt/IpExactHessianUpdater.cpp
new file mode 100644
index 0000000..50c947e
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpExactHessianUpdater.cpp
@@ -0,0 +1,35 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpExactHessianUpdater.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+// Andreas Waechter IBM 2005-10-13
+// derived file from IpFilterLineSearch.cpp
+
+#include "IpExactHessianUpdater.hpp"
+
+namespace Ipopt
+{
+
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ bool ExactHessianUpdater::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ return true;
+ }
+
+ void ExactHessianUpdater::UpdateHessian()
+ {
+ DBG_START_METH("ExactHessianUpdater::UpdateHessian",
+ dbg_verbosity);
+
+ IpData().Set_W(IpCq().curr_exact_hessian());
+ }
+
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpExactHessianUpdater.hpp b/SimTKmath/Optimizers/src/IpOpt/IpExactHessianUpdater.hpp
new file mode 100644
index 0000000..da78f6e
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpExactHessianUpdater.hpp
@@ -0,0 +1,62 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpExactHessianUpdater.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-12-26
+
+#ifndef __IPEXACTHESSIANUPDATER_HPP__
+#define __IPEXACTHESSIANUPDATER_HPP__
+
+#include "IpHessianUpdater.hpp"
+
+namespace Ipopt
+{
+
+ /** Implementation of the HessianUpdater for the use of exact second
+ * derivatives.
+ */
+ class ExactHessianUpdater : public HessianUpdater
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor */
+ ExactHessianUpdater()
+ {}
+
+ /** Default destructor */
+ virtual ~ExactHessianUpdater()
+ {}
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Update the Hessian based on the current information in IpData.
+ */
+ virtual void UpdateHessian();
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ ExactHessianUpdater(const ExactHessianUpdater&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const ExactHessianUpdater&);
+ //@}
+
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpException.hpp b/SimTKmath/Optimizers/src/IpOpt/IpException.hpp
new file mode 100644
index 0000000..5ee8c1f
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpException.hpp
@@ -0,0 +1,147 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpException.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPEXCEPTION_HPP__
+#define __IPEXCEPTION_HPP__
+
+#include "IpUtils.hpp"
+#include "IpJournalist.hpp"
+
+/* This file contains a base class for all exceptions
+ * and a set of macros to help with exceptions
+ */
+
+namespace Ipopt
+{
+
+ /** This is the base class for all exceptions. The easiest way to
+ * use this class is by means of the following macros:
+ *
+ * \verbatim
+
+ DECLARE_STD_EXCEPTION(ExceptionType);
+ \endverbatim
+ *
+ * This macro defines a new class with the name ExceptionType,
+ * inherited from the base class IpoptException. After this,
+ * exceptions of this type can be thrown using
+ *
+ * \verbatim
+
+ THROW_EXCEPTION(ExceptionType, Message);
+ \endverbatim
+ *
+ * where Message is a std::string with a message that gives an
+ * indication of what caused the exception. Exceptions can also be
+ * thrown using the macro
+ *
+ * \verbatim
+
+ ASSERT_EXCEPTION(Condition, ExceptionType, Message);
+ \endverbatim
+ *
+ * where Conditions is an expression. If Condition evaluates to
+ * false, then the exception of the type ExceptionType is thrown
+ * with Message.
+ *
+ * When an exception is caught, the method ReportException can be
+ * used to write the information about the exception to the
+ * Journalist, using the level J_ERROR and the category J_MAIN.
+ *
+ */
+ class IpoptException
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor */
+ IpoptException(std::string msg, std::string file_name, Index line_number, std::string type="IpoptException")
+ :
+ msg_(msg),
+ file_name_(file_name),
+ line_number_(line_number),
+ type_(type)
+ {}
+
+ /** Copy Constructor */
+ IpoptException(const IpoptException& copy)
+ :
+ msg_(copy.msg_),
+ file_name_(copy.file_name_),
+ line_number_(copy.line_number_),
+ type_(copy.type_)
+ {}
+
+ /** Default destructor */
+ virtual ~IpoptException()
+ {}
+ //@}
+
+ /** Method to report the exception to a journalist */
+ void ReportException(const Journalist& jnlst,
+ EJournalLevel level = J_ERROR) const
+ {
+ jnlst.Printf(level, J_MAIN,
+ "Exception of type: %s in file \"%s\" at line %d:\n Exception message: %s\n",
+ type_.c_str(), file_name_.c_str(), line_number_, msg_.c_str());
+ }
+
+ const std::string& Message() const
+ {
+ return msg_;
+ }
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ IpoptException();
+
+ /** Overloaded Equals Operator */
+ void operator=(const IpoptException&);
+ //@}
+
+ std::string msg_;
+ std::string file_name_;
+ Index line_number_;
+ std::string type_;
+ };
+
+#define THROW_EXCEPTION(__except_type, __msg) \
+ throw __except_type( (__msg), (__FILE__), (__LINE__) );
+
+#define ASSERT_EXCEPTION(__condition, __except_type, __msg) \
+ if (! (__condition) ) { \
+ std::string newmsg = #__condition; \
+ newmsg += " evaluated false: "; \
+ newmsg += __msg; \
+ throw __except_type( (newmsg), (__FILE__), (__LINE__) ); \
+ }
+
+#define DECLARE_STD_EXCEPTION(__except_type) \
+ class __except_type : public IpoptException \
+ { \
+ public: \
+ __except_type(std::string msg, std::string fname, Index line) \
+ : IpoptException(msg,fname,line, #__except_type) {} \
+ __except_type(const __except_type& copy) \
+ : IpoptException(copy) {} \
+ private: \
+ __except_type(); \
+ void operator=(const __except_type&); \
+ }
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpExpansionMatrix.cpp b/SimTKmath/Optimizers/src/IpOpt/IpExpansionMatrix.cpp
new file mode 100644
index 0000000..c3325d7
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpExpansionMatrix.cpp
@@ -0,0 +1,318 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpExpansionMatrix.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpExpansionMatrix.hpp"
+#include "IpDenseVector.hpp"
+
+namespace Ipopt
+{
+
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ ExpansionMatrix::ExpansionMatrix(const ExpansionMatrixSpace* owner_space)
+ :
+ Matrix(owner_space),
+ owner_space_(owner_space)
+ {}
+
+ ExpansionMatrix::~ExpansionMatrix()
+ {}
+
+ void ExpansionMatrix::MultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ // A few sanity checks
+ DBG_ASSERT(NCols()==x.Dim());
+ DBG_ASSERT(NRows()==y.Dim());
+
+ // Take care of the y part of the addition
+ if( beta!=0.0 ) {
+ y.Scal(beta);
+ }
+ else {
+ y.Set(0.0); // In case y hasn't been initialized yet
+ }
+
+ // See if we can understand the data
+ const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x);
+ DBG_ASSERT(dense_x); /* ToDo: Implement others */
+ DenseVector* dense_y = dynamic_cast<DenseVector*>(&y);
+ DBG_ASSERT(dense_y); /* ToDo: Implement others */
+
+ const Index* exp_pos = ExpandedPosIndices();
+
+ if (dense_x && dense_y) {
+ Number* yvals=dense_y->Values();
+ if (dense_x->IsHomogeneous()) {
+ Number val = alpha * dense_x->Scalar();
+ for(Index i=0; i<NCols(); i++) {
+ yvals[exp_pos[i]] += val;
+ }
+ }
+ else {
+ const Number* xvals=dense_x->Values();
+ for(Index i=0; i<NCols(); i++) {
+ yvals[exp_pos[i]] += alpha * xvals[i];
+ }
+ }
+ }
+ }
+
+ void ExpansionMatrix::TransMultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ // A few sanity checks
+ DBG_ASSERT(NCols()==y.Dim());
+ DBG_ASSERT(NRows()==x.Dim());
+
+ // Take care of the y part of the addition
+ if( beta!=0.0 ) {
+ y.Scal(beta);
+ }
+ else {
+ y.Set(0.0); // In case y hasn't been initialized yet
+ }
+
+ // See if we can understand the data
+ const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x);
+ DBG_ASSERT(dense_x); /* ToDo: Implement others */
+ DenseVector* dense_y = dynamic_cast<DenseVector*>(&y);
+ DBG_ASSERT(dense_y); /* ToDo: Implement others */
+
+ const Index* exp_pos = ExpandedPosIndices();
+
+ if (dense_x && dense_y) {
+ Number* yvals=dense_y->Values();
+ if (dense_x->IsHomogeneous()) {
+ Number val = alpha * dense_x->Scalar();
+ for(Index i=0; i<NCols(); i++) {
+ yvals[i] += val;
+ }
+ }
+ else {
+ const Number* xvals=dense_x->Values();
+ for(Index i=0; i<NCols(); i++) {
+ yvals[i] += alpha * xvals[exp_pos[i]];
+ }
+ }
+ }
+ }
+
+ // Specialized method (overloaded from IpMatrix)
+ void ExpansionMatrix::AddMSinvZImpl(Number alpha, const Vector& S,
+ const Vector& Z, Vector& X) const
+ {
+ DBG_ASSERT(NCols()==S.Dim());
+ DBG_ASSERT(NCols()==Z.Dim());
+ DBG_ASSERT(NRows()==X.Dim());
+
+ const DenseVector* dense_S = dynamic_cast<const DenseVector*>(&S);
+ DBG_ASSERT(dense_S);
+ const DenseVector* dense_Z = dynamic_cast<const DenseVector*>(&Z);
+ DBG_ASSERT(dense_Z);
+ DenseVector* dense_X = dynamic_cast<DenseVector*>(&X);
+ DBG_ASSERT(dense_X);
+
+ // if vector S is homogeneous type, call the default implementation
+ // ToDo: find out how often the default implementation is called and
+ // if we should implement specialized method for the homogenenous
+ // case
+ if (dense_S->IsHomogeneous()) {
+ DBG_ASSERT(false && "dense_S is homogeneous - implement specialized option?");
+ Matrix::AddMSinvZImpl(alpha, S, Z, X);
+ return;
+ }
+
+ const Index* exp_pos = ExpandedPosIndices();
+ const Number* vals_S = dense_S->Values();
+ Number* vals_X = dense_X->Values();
+
+ if (dense_Z->IsHomogeneous()) {
+ Number val = alpha*dense_Z->Scalar();
+ for(Index i=0; i<NCols(); i++) {
+ vals_X[exp_pos[i]] += val/vals_S[i];
+ }
+ }
+ else {
+ const Number* vals_Z = dense_Z->Values();
+ if (alpha==1.) {
+ for(Index i=0; i<NCols(); i++) {
+ vals_X[exp_pos[i]] += vals_Z[i]/vals_S[i];
+ }
+ }
+ else if (alpha==-1.) {
+ for(Index i=0; i<NCols(); i++) {
+ vals_X[exp_pos[i]] -= vals_Z[i]/vals_S[i];
+ }
+ }
+ else {
+ for(Index i=0; i<NCols(); i++) {
+ vals_X[exp_pos[i]] += alpha*vals_Z[i]/vals_S[i];
+ }
+ }
+ }
+ }
+
+ void ExpansionMatrix::SinvBlrmZMTdBrImpl(Number alpha, const Vector& S,
+ const Vector& R, const Vector& Z,
+ const Vector& D, Vector& X) const
+ {
+ DBG_START_METH("ExpansionMatrix::SinvBlrmZMTdBrImpl",
+ dbg_verbosity);
+
+ DBG_ASSERT(NCols()==S.Dim());
+ DBG_ASSERT(NCols()==R.Dim());
+ DBG_ASSERT(NCols()==Z.Dim());
+ DBG_ASSERT(NRows()==D.Dim());
+ DBG_ASSERT(NCols()==X.Dim());
+
+ const DenseVector* dense_S = dynamic_cast<const DenseVector*>(&S);
+ DBG_ASSERT(dense_S);
+ const DenseVector* dense_R = dynamic_cast<const DenseVector*>(&R);
+ DBG_ASSERT(dense_R);
+ const DenseVector* dense_Z = dynamic_cast<const DenseVector*>(&Z);
+ DBG_ASSERT(dense_Z);
+ const DenseVector* dense_D = dynamic_cast<const DenseVector*>(&D);
+ DBG_ASSERT(dense_D);
+ DenseVector* dense_X = dynamic_cast<DenseVector*>(&X);
+ DBG_ASSERT(dense_X);
+
+ // if the vectors S or D are of the homogeneous type, revert to the
+ // default implementation
+ // ToDo: find out how often the default implementation is called and
+ // if we should implement specialized method for the homogenenous
+ // case
+
+ if (dense_S->IsHomogeneous() ||
+ dense_D->IsHomogeneous()) {
+ DBG_ASSERT(false && "dense_S or dense_D is homogeneous - implement specialized option?");
+ Matrix::SinvBlrmZMTdBrImpl(alpha, S, R, Z, D, X);
+ return;
+ }
+
+ const Index* exp_pos = ExpandedPosIndices();
+ const Number* vals_S = dense_S->Values();
+ const Number* vals_D = dense_D->Values();
+ Number* vals_X = dense_X->Values();
+
+ if (dense_R->IsHomogeneous()) {
+ Number scalar_R = dense_R->Scalar();
+ if (dense_Z->IsHomogeneous()) {
+ Number val = alpha*dense_Z->Scalar();
+ if (val == 0.) {
+ for (Index i=0; i<NCols(); i++) {
+ // ToDo could treat val == 0 extra
+ vals_X[i] = (scalar_R)/vals_S[i];
+ }
+ }
+ else {
+ for (Index i=0; i<NCols(); i++) {
+ // ToDo could treat val == 0 extra
+ vals_X[i] = (scalar_R + val*vals_D[exp_pos[i]])/vals_S[i];
+ }
+ }
+ }
+ else {
+ const Number* vals_Z = dense_Z->Values();
+ if (alpha==1.) {
+ for (Index i=0; i<NCols(); i++) {
+ vals_X[i] = (scalar_R + vals_Z[i]*vals_D[exp_pos[i]])/vals_S[i];
+ }
+ }
+ else if (alpha==-1.) {
+ for (Index i=0; i<NCols(); i++) {
+ vals_X[i] = (scalar_R - vals_Z[i]*vals_D[exp_pos[i]])/vals_S[i];
+ }
+ }
+ else {
+ for (Index i=0; i<NCols(); i++) {
+ vals_X[i] = (scalar_R + alpha*vals_Z[i]*vals_D[exp_pos[i]])/vals_S[i];
+ }
+ }
+ }
+ }
+ else {
+ const Number* vals_R = dense_R->Values();
+ if (dense_Z->IsHomogeneous()) {
+ Number val = alpha*dense_Z->Scalar();
+ for (Index i=0; i<NCols(); i++) {
+ vals_X[i] = (vals_R[i] + val*vals_D[exp_pos[i]])/vals_S[i];
+ }
+ }
+ else {
+ const Number* vals_Z = dense_Z->Values();
+ if (alpha==1.) {
+ for (Index i=0; i<NCols(); i++) {
+ vals_X[i] = (vals_R[i] + vals_Z[i]*vals_D[exp_pos[i]])/vals_S[i];
+ }
+ }
+ else if (alpha==-1.) {
+ for (Index i=0; i<NCols(); i++) {
+ vals_X[i] = (vals_R[i] - vals_Z[i]*vals_D[exp_pos[i]])/vals_S[i];
+ }
+ }
+ else {
+ for (Index i=0; i<NCols(); i++) {
+ vals_X[i] = (vals_R[i] + alpha*vals_Z[i]*vals_D[exp_pos[i]])/vals_S[i];
+ }
+ }
+ }
+ }
+ }
+
+ void ExpansionMatrix::PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ jnlst.Printf(level, category, "\n");
+ jnlst.PrintfIndented(level, category, indent,
+ "%sExpansionMatrix \"%s\" with %d nonzero elements:\n",
+ prefix.c_str(), name.c_str(), NCols());
+
+ const Index* exp_pos = ExpandedPosIndices();
+
+ for (Index i=0; i<NCols(); i++) {
+ jnlst.PrintfIndented(level, category, indent,
+ "%s%s[%5d,%5d]=%23.16e (%d)\n",
+ prefix.c_str(), name.c_str(), exp_pos[i]+1,
+ i+1, 1., i);
+ }
+ }
+
+ ExpansionMatrixSpace::ExpansionMatrixSpace(Index NLargeVec,
+ Index NSmallVec,
+ const Index *ExpPos,
+ const int offset /*= 0*/)
+ :
+ MatrixSpace(NLargeVec, NSmallVec),
+ expanded_pos_(NULL),
+ compressed_pos_(NULL)
+ {
+ if(NCols()>0) {
+ expanded_pos_ = new Index[NCols()];
+ }
+ if(NRows()>0) {
+ compressed_pos_ = new Index[NRows()];
+ }
+ for (Index j=0; j<NRows(); j++) {
+ compressed_pos_[j] = -1;
+ }
+ for(Index i=0; i<NCols(); i++) {
+ //ToDo decide for offset
+ DBG_ASSERT(ExpPos[i]-offset<NRows() && ExpPos[i]-offset>=0);
+ expanded_pos_[i]=ExpPos[i]-offset;
+ compressed_pos_[ExpPos[i]-offset] = i;
+ }
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpExpansionMatrix.hpp b/SimTKmath/Optimizers/src/IpOpt/IpExpansionMatrix.hpp
new file mode 100644
index 0000000..22a6b79
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpExpansionMatrix.hpp
@@ -0,0 +1,195 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpExpansionMatrix.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPEXPANSIONMATRIX_HPP__
+#define __IPEXPANSIONMATRIX_HPP__
+
+#include "IpUtils.hpp"
+#include "IpMatrix.hpp"
+
+namespace Ipopt
+{
+
+ /** forward declarations */
+ class ExpansionMatrixSpace;
+
+ /** Class for expansion/projection matrices. These matrices allow
+ * to lift a vector to a vector with larger dimension, keeping
+ * some elements of the larger vector zero. This operation is achieved
+ * by the MultVector operation. The transpose operation then
+ * filters some elements from a large vector into a smaller vector.
+ */
+ class ExpansionMatrix : public Matrix
+ {
+ public:
+
+ /**@name Constructors / Destructors */
+ //@{
+
+ /** Constructor, taking the owner_space.
+ */
+ ExpansionMatrix(const ExpansionMatrixSpace* owner_space);
+
+ /** Destructor */
+ ~ExpansionMatrix();
+ //@}
+
+ /** Return the vector of indices marking the expanded position.
+ * The result is the Index array (of length NSmallVec=NCols())
+ * that stores the mapping from the small vector to the large
+ * vector. For each element i=0,..,NSmallVec in the small
+ * vector, ExpandedPosIndices()[i] give the corresponding index
+ * in the large vector.
+ */
+ const Index* ExpandedPosIndices() const;
+
+ /** Return the vector of indices marking the compressed position.
+ * The result is the Index array (of length NLargeVec=NRows())
+ * that stores the mapping from the large vector to the small
+ * vector. For each element i=0,..,NLargeVec in the large
+ * vector, CompressedPosIndices()[i] gives the corresponding
+ * index in the small vector, unless CompressedPosIndices()[i] is
+ * negative.
+ */
+ const Index* CompressedPosIndices() const;
+
+ protected:
+ /**@name Overloaded methods from Matrix base class*/
+ //@{
+ virtual void MultVectorImpl(Number alpha, const Vector &x, Number beta,
+ Vector &y) const;
+
+ virtual void TransMultVectorImpl(Number alpha, const Vector& x,
+ Number beta, Vector& y) const;
+
+ /** X = beta*X + alpha*(Matrix S^{-1} Z). Specialized implementation.
+ */
+ virtual void AddMSinvZImpl(Number alpha, const Vector& S, const Vector& Z,
+ Vector& X) const;
+
+ /** X = S^{-1} (r + alpha*Z*M^Td). Specialized implementation.
+ */
+ virtual void SinvBlrmZMTdBrImpl(Number alpha, const Vector& S,
+ const Vector& R, const Vector& Z,
+ const Vector& D, Vector& X) const;
+
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const;
+ //@}
+
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ ExpansionMatrix();
+
+ /** Copy Constructor */
+ ExpansionMatrix(const ExpansionMatrix&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const ExpansionMatrix&);
+ //@}
+
+ const ExpansionMatrixSpace* owner_space_;
+
+ };
+
+ /** This is the matrix space for ExpansionMatrix.
+ */
+ class ExpansionMatrixSpace : public MatrixSpace
+ {
+ public:
+ /** @name Constructors / Destructors */
+ //@{
+ /** Constructor, given the list of elements of the large vector
+ * (of size NLargeVec) to be filtered into the small vector (of
+ * size NSmallVec). For each i=0..NSmallVec-1 the i-th element
+ * of the small vector will be put into the ExpPos[i] position of
+ * the large vector. The position counting in the vector is
+ * assumed to start at 0 (C-like array notation).
+ */
+ ExpansionMatrixSpace(Index NLargeVec,
+ Index NSmallVec,
+ const Index *ExpPos,
+ const int offset = 0);
+
+ /** Destructor */
+ ~ExpansionMatrixSpace()
+ {
+ delete [] compressed_pos_;
+ delete [] expanded_pos_;
+ }
+ //@}
+
+ /** Method for creating a new matrix of this specific type. */
+ ExpansionMatrix* MakeNewExpansionMatrix() const
+ {
+ return new ExpansionMatrix(this);
+ }
+
+ /** Overloaded MakeNew method for the MatrixSpace base class.
+ */
+ virtual Matrix* MakeNew() const
+ {
+ return MakeNewExpansionMatrix();
+ }
+
+ /** Accessor Method to obtain the Index array (of length
+ * NSmallVec=NCols()) that stores the mapping from the small
+ * vector to the large vector. For each element i=0,..,NSmallVec
+ * in the small vector, ExpandedPosIndices()[i] give the
+ * corresponding index in the large vector.
+ */
+ const Index* ExpandedPosIndices() const
+ {
+ return expanded_pos_;
+ }
+
+ /** Accessor Method to obtain the Index array (of length
+ * NLargeVec=NRows()) that stores the mapping from the large
+ * vector to the small vector. For each element i=0,..,NLargeVec
+ * in the large vector, CompressedPosIndices()[i] gives the
+ * corresponding index in the small vector, unless
+ * CompressedPosIndices()[i] is negative.
+ */
+ const Index* CompressedPosIndices() const
+ {
+ return compressed_pos_;
+ }
+
+ private:
+ Index *expanded_pos_;
+ Index *compressed_pos_;
+ };
+
+ /* inline methods */
+ inline
+ const Index* ExpansionMatrix::ExpandedPosIndices() const
+ {
+ return owner_space_->ExpandedPosIndices();
+ }
+
+ inline
+ const Index* ExpansionMatrix::CompressedPosIndices() const
+ {
+ return owner_space_->CompressedPosIndices();
+ }
+
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpFilter.cpp b/SimTKmath/Optimizers/src/IpOpt/IpFilter.cpp
new file mode 100644
index 0000000..130b067
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpFilter.cpp
@@ -0,0 +1,114 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpFilter.cpp 765 2006-07-14 18:03:23Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpFilter.hpp"
+#include "IpJournalist.hpp"
+
+namespace Ipopt
+{
+
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Filter entries //
+ ///////////////////////////////////////////////////////////////////////////
+
+ FilterEntry::FilterEntry(std::vector<Number> vals, Index iter)
+ :
+ vals_(vals),
+ iter_(iter)
+ {}
+
+ FilterEntry::~FilterEntry()
+ {}
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Filter //
+ ///////////////////////////////////////////////////////////////////////////
+
+ Filter::Filter(Index dim)
+ :
+ dim_(dim)
+ {}
+
+ bool Filter::Acceptable(std::vector<Number> vals) const
+ {
+ DBG_START_METH("FilterLineSearch::Filter::Acceptable", dbg_verbosity);
+ DBG_ASSERT((Index)vals.size()==dim_);
+ bool acceptable = true;
+ std::list<FilterEntry*>::iterator iter;
+ for (iter = filter_list_.begin(); iter != filter_list_.end();
+ iter++) {
+ if (!(*iter)->Acceptable(vals)) {
+ acceptable = false;
+ break;
+ }
+ }
+ return acceptable;
+ }
+
+ void Filter::AddEntry(std::vector<Number> vals, Index iteration)
+ {
+ DBG_START_METH("FilterLineSearch::Filter::AddEntry", dbg_verbosity);
+ DBG_ASSERT((Index)vals.size()==dim_);
+ std::list<FilterEntry*>::iterator iter;
+ iter = filter_list_.begin();
+ while (iter != filter_list_.end()) {
+ if ((*iter)->Dominated(vals)) {
+ std::list<FilterEntry*>::iterator iter_to_remove = iter;
+ iter++;
+ FilterEntry* entry_to_remove = *iter_to_remove;
+ filter_list_.erase(iter_to_remove);
+ delete entry_to_remove;
+ }
+ else {
+ iter++;
+ }
+ }
+ FilterEntry* new_entry = new FilterEntry(vals, iteration);
+ filter_list_.push_back(new_entry);
+ }
+
+ void Filter::Clear()
+ {
+ DBG_START_METH("FilterLineSearch::Filter::Clear", dbg_verbosity);
+ while (!filter_list_.empty()) {
+ FilterEntry* entry = filter_list_.back();
+ filter_list_.pop_back();
+ delete entry;
+ }
+ }
+
+ void Filter::Print(const Journalist& jnlst)
+ {
+ DBG_START_METH("FilterLineSearch::Filter::Print", dbg_verbosity);
+ jnlst.Printf(J_DETAILED, J_LINE_SEARCH,
+ "The current filter has %d entries.\n", filter_list_.size());
+ if (!jnlst.ProduceOutput(J_VECTOR, J_LINE_SEARCH)) {
+ return;
+ }
+ std::list<FilterEntry*>::iterator iter;
+ Index count = 0;
+ for (iter = filter_list_.begin(); iter != filter_list_.end();
+ iter++) {
+ if (count % 10 == 0) {
+ jnlst.Printf(J_VECTOR, J_LINE_SEARCH,
+ " phi theta iter\n");
+ }
+ count++;
+ jnlst.Printf(J_VECTOR, J_LINE_SEARCH, "%5d ", count);
+ for (Index i=0; i<dim_; i++) {
+ jnlst.Printf(J_VECTOR, J_LINE_SEARCH, "%23.16e ", (*iter)->val(i));
+ }
+ jnlst.Printf(J_VECTOR, J_LINE_SEARCH, "%5d\n",(*iter)->iter());
+ }
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpFilter.hpp b/SimTKmath/Optimizers/src/IpOpt/IpFilter.hpp
new file mode 100644
index 0000000..4aa21f8
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpFilter.hpp
@@ -0,0 +1,189 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpFilter.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPFILTER_HPP__
+#define __IPFILTER_HPP__
+
+#include "IpJournalist.hpp"
+#include "IpDebug.hpp"
+#include <list>
+#include <vector>
+
+namespace Ipopt
+{
+
+ /** Class for one filter entry. */
+ class FilterEntry
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor with the two components and the current iteration count */
+ FilterEntry(std::vector<Number> vals, Index iter);
+
+ /** Default Destructor */
+ ~FilterEntry();
+ //@}
+
+ /** Check acceptability of pair (phi,theta) with respect
+ * to this filter entry. Returns true, if pair is acceptable.
+ */
+ bool Acceptable(std::vector<Number> vals) const
+ {
+ Index ncoor = (Index)vals_.size();
+ DBG_ASSERT((Index)vals.size() == ncoor);
+
+ // ToDo decide if we need Compare_le
+ bool retval = false;
+ for (Index i=0; i<ncoor; i++) {
+ if (vals[i] <= vals_[i]) {
+ retval = true;
+ break;
+ }
+ }
+
+ return retval;
+ }
+
+ /** Check if this entry is dominated by given coordinates.
+ * Returns true, if this entry is dominated.
+ */
+ bool Dominated(std::vector<Number> vals) const
+ {
+ Index ncoor = (Index)vals_.size();
+ DBG_ASSERT((Index)vals.size() == ncoor);
+
+ bool retval = true;
+ for (Index i=0; i<ncoor; i++) {
+ if (vals[i] > vals_[i]) {
+ retval = false;
+ break;
+ }
+ }
+
+ return retval;
+ }
+
+ /** @name Accessor functions */
+ //@{
+ Number val(Index i) const
+ {
+ return vals_[i];
+ }
+ Index iter() const
+ {
+ return iter_;
+ }
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ FilterEntry();
+ /** Copy Constructor */
+ FilterEntry(const FilterEntry&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const FilterEntry&);
+ //@}
+
+ /** values defining the coordinates of the entry */
+ std::vector<Number> vals_;
+ /** iteration number in which this entry was added to filter */
+ const Index iter_;
+ };
+
+ /** Class for the filter. This class contains all filter entries.
+ * The entries are stored as the corner point, including the
+ * margin. */
+ class Filter
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor */
+ Filter(Index dim);
+ /** Default Destructor */
+ ~Filter()
+ {
+ //ToDo figure out if that here is necessary
+ Clear();
+ }
+ //@}
+
+ /** Check acceptability of given coordinates with respect
+ * to the filter. Returns true, if pair is acceptable
+ */
+ bool Acceptable(std::vector<Number> vals) const;
+
+ /** Add filter entry for given coordinates. This will also
+ * delete all dominated entries in the current filter. */
+ void AddEntry(std::vector<Number> vals, Index iteration);
+
+ /** @name Wrappers for 2-dimensional filter. */
+ //@{
+ bool Acceptable(Number val1, Number val2) const
+ {
+ std::vector<Number> vals(2);
+ vals[0] = val1;
+ vals[1] = val2;
+
+ return Acceptable(vals);
+ }
+
+ void AddEntry(Number val1, Number val2, Index iteration)
+ {
+ std::vector<Number> vals(2);
+ vals[0] = val1;
+ vals[1] = val2;
+
+ AddEntry(vals, iteration);
+ }
+ //@}
+
+ /** Delete all filter entries */
+ void Clear();
+
+ /** Print current filter entries */
+ void Print(const Journalist& jnlst);
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ Filter();
+ /** Copy Constructor */
+ Filter(const Filter&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const Filter&);
+ //@}
+
+ /** Dimension of the filter (number of coordinates per entry) */
+ Index dim_;
+
+ /** List storing the filter entries */
+ mutable std::list<FilterEntry*> filter_list_;
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpFilterLSAcceptor.cpp b/SimTKmath/Optimizers/src/IpOpt/IpFilterLSAcceptor.cpp
new file mode 100644
index 0000000..01d1b15
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpFilterLSAcceptor.cpp
@@ -0,0 +1,800 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpFilterLSAcceptor.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+// Andreas Waechter IBM 2005-10-13
+// derived file from IpFilterLineSearch.cpp
+
+#include "IpFilterLSAcceptor.hpp"
+#include "IpJournalist.hpp"
+#include "IpRestoPhase.hpp"
+#include "IpAlgTypes.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+namespace Ipopt
+{
+
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ FilterLSAcceptor::FilterLSAcceptor(const SmartPtr<PDSystemSolver>& pd_solver)
+ :
+ filter_(2),
+ pd_solver_(pd_solver)
+ {
+ DBG_START_FUN("FilterLSAcceptor::FilterLSAcceptor",
+ dbg_verbosity);
+ }
+
+ FilterLSAcceptor::~FilterLSAcceptor()
+ {
+ DBG_START_FUN("FilterLSAcceptor::~FilterLSAcceptor()",
+ dbg_verbosity);
+ }
+
+ void FilterLSAcceptor::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->AddLowerBoundedNumberOption(
+ "theta_max_fact",
+ "Determines upper bound for constraint violation in the filter.",
+ 0.0, true, 1e4,
+ "The algorithmic parameter theta_max is determined as theta_max_fact "
+ "times the maximum of 1 and the constraint violation at initial point. "
+ "Any point with a constraint violation larger than theta_max is "
+ "unacceptable to the filter (see Eqn. (21) in implementation paper).");
+ roptions->AddLowerBoundedNumberOption(
+ "theta_min_fact",
+ "Determines constraint violation threshold in the switching rule.",
+ 0.0, true, 1e-4,
+ "The algorithmic parameter theta_min is determined as theta_min_fact "
+ "times the maximum of 1 and the constraint violation at initial point. "
+ "The switching rules treats an iteration as an h-type iteration whenever "
+ "the current constraint violation is larger than theta_min (see "
+ "paragraph before Eqn. (19) in implementation paper).");
+ roptions->AddBoundedNumberOption(
+ "eta_phi",
+ "Relaxation factor in the Armijo condition.",
+ 0.0, true, 0.5, true, 1e-8,
+ "(See Eqn. (20) in implementation paper)");
+ roptions->AddLowerBoundedNumberOption(
+ "delta", "Multiplier for constraint violation in the switching rule.",
+ 0.0, true, 1.0,
+ "(See Eqn. (19) in implementation paper.)");
+ roptions->AddLowerBoundedNumberOption(
+ "s_phi",
+ "Exponent for linear barrier function model in the switching rule.",
+ 1.0, true, 2.3,
+ "(See Eqn. (19) in implementation paper.)");
+ roptions->AddLowerBoundedNumberOption(
+ "s_theta",
+ "Exponent for current constraint violation in the switching rule.",
+ 1.0, true, 1.1,
+ "(See Eqn. (19) in implementation paper.)");
+ roptions->AddBoundedNumberOption(
+ "gamma_phi",
+ "Relaxation factor in the filter margin for the barrier function.",
+ 0.0, true, 1.0, true, 1e-8,
+ "(See Eqn. (18a) in implementation paper.)");
+ roptions->AddBoundedNumberOption(
+ "gamma_theta",
+ "Relaxation factor in the filter margin for the constraint violation.",
+ 0.0, true, 1.0, true, 1e-5,
+ "(See Eqn. (18b) in implementation paper.)");
+ roptions->AddBoundedNumberOption(
+ "alpha_min_frac",
+ "Safety factor for the minimal step size (before switching to restoration phase).",
+ 0.0, true, 1.0, true, 0.05,
+ "(This is gamma_alpha in Eqn. (20) in implementation paper.)");
+ roptions->AddLowerBoundedIntegerOption(
+ "max_soc",
+ "Maximum number of second order correction trial steps at each iteration.",
+ 0, 4,
+ "Choosing 0 disables the second order "
+ "corrections. (This is p^{max} of Step A-5.9 of "
+ "Algorithm A in implementation paper.)");
+ roptions->AddLowerBoundedNumberOption(
+ "kappa_soc",
+ "Factor in the sufficient reduction rule for second order correction.",
+ 0.0, true, 0.99,
+ "This option determines how much a second order correction step must reduce the "
+ "constraint violation so that further correction steps are attempted. "
+ "(See Step A-5.9 of Algorithm A in implementation paper.)");
+ roptions->AddLowerBoundedNumberOption(
+ "obj_max_inc",
+ "Determines the upper bound on the acceptable increase of barrier objective function.",
+ 1.0, true, 5.0,
+ "Trial points are rejected if they lead to an increase in the "
+ "barrier objective function by more than obj_max_inc orders "
+ "of magnitude.");
+
+ roptions->AddLowerBoundedIntegerOption(
+ "max_filter_resets",
+ "Maximal allowed number of filter resets",
+ 0, 5,
+ "A positive number enables a heuristic that resets the filter, whenever "
+ "in more than \"filter_reset_trigger\" successive iterations the last "
+ "rejected trial steps size was rejected because of the filter. This "
+ "option determine the maximal number of resets that are allowed to take "
+ "place.");
+ roptions->AddLowerBoundedIntegerOption(
+ "filter_reset_trigger",
+ "Number of iterations that trigger the filter reset.",
+ 1, 5,
+ "If the filter reset heuristic is active and the number of successive "
+ "iterations in which the last rejected trial step size was rejected "
+ "because of the filter, the filter is reset.");
+
+ roptions->AddStringOption3(
+ "corrector_type",
+ "The type of corrector steps that should be taken (unsupported!).",
+ "none",
+ "none", "no corrector",
+ "affine", "corrector step towards mu=0",
+ "primal-dual", "corrector step towards current mu",
+ "If \"mu_strategy\" is \"adaptive\", this option determines "
+ "what kind of corrector steps should be tried.");
+
+ roptions->AddStringOption2(
+ "skip_corr_if_neg_curv",
+ "Skip the corrector step in negative curvature iteration (unsupported!).",
+ "yes",
+ "no", "don't skip",
+ "yes", "skip",
+ "The corrector step is not tried if negative curvature has been "
+ "encountered during the computation of the search direction in "
+ "the current iteration. This option is only used if \"mu_strategy\" is "
+ "\"adaptive\".");
+
+ roptions->AddStringOption2(
+ "skip_corr_in_monotone_mode",
+ "Skip the corrector step during monotone barrier parameter mode (unsupported!).",
+ "yes",
+ "no", "don't skip",
+ "yes", "skip",
+ "The corrector step is not tried if the algorithm is currently in the "
+ "monotone mode (see also option \"barrier_strategy\")."
+ "This option is only used if \"mu_strategy\" is \"adaptive\".");
+
+ roptions->AddLowerBoundedNumberOption(
+ "corrector_compl_avrg_red_fact",
+ "Complementarity tolerance factor for accepting corrector step (unsupported!).",
+ 0.0, true, 1.0,
+ "This option determines the factor by which complementarity is allowed to increase "
+ "for a corrector step to be accepted.");
+ }
+
+ bool FilterLSAcceptor::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ options.GetNumericValue("theta_max_fact", theta_max_fact_, prefix);
+ options.GetNumericValue("theta_min_fact", theta_min_fact_, prefix);
+ ASSERT_EXCEPTION(theta_min_fact_ < theta_max_fact_, OPTION_INVALID,
+ "Option \"theta_min_fact\": This value must be larger than 0 and less than theta_max_fact.");
+ options.GetNumericValue("eta_phi", eta_phi_, prefix);
+ options.GetNumericValue("delta", delta_, prefix);
+ options.GetNumericValue("s_phi", s_phi_, prefix);
+ options.GetNumericValue("s_theta", s_theta_, prefix);
+ options.GetNumericValue("gamma_phi", gamma_phi_, prefix);
+ options.GetNumericValue("gamma_theta", gamma_theta_, prefix);
+ options.GetNumericValue("alpha_min_frac", alpha_min_frac_, prefix);
+ options.GetIntegerValue("max_soc", max_soc_, prefix);
+ if (max_soc_>0) {
+ ASSERT_EXCEPTION(IsValid(pd_solver_), OPTION_INVALID,
+ "Option \"max_soc\": This option is non-negative, but no linear solver for computing the SOC given to FilterLSAcceptor object.");
+ }
+ options.GetNumericValue("kappa_soc", kappa_soc_, prefix);
+ options.GetIntegerValue("max_filter_resets", max_filter_resets_, prefix);
+ options.GetIntegerValue("filter_reset_trigger", filter_reset_trigger_,
+ prefix);
+ options.GetNumericValue("obj_max_inc", obj_max_inc_, prefix);
+ Index enum_int;
+ options.GetEnumValue("corrector_type", enum_int, prefix);
+ corrector_type_ = CorrectorTypeEnum(enum_int);
+ options.GetBoolValue("skip_corr_if_neg_curv", skip_corr_if_neg_curv_, prefix);
+ options.GetBoolValue("skip_corr_in_monotone_mode", skip_corr_in_monotone_mode_, prefix);
+ options.GetNumericValue("corrector_compl_avrg_red_fact", corrector_compl_avrg_red_fact_, prefix);
+
+ theta_min_ = -1.;
+ theta_max_ = -1.;
+
+ n_filter_resets_ = 0;
+
+ Reset();
+
+ return true;
+ }
+
+ void FilterLSAcceptor::InitThisLineSearch(bool in_watchdog)
+ {
+ DBG_START_METH("FilterLSAcceptor::InitThisLineSearch",
+ dbg_verbosity);
+
+ // Set the values for the reference point
+ if (!in_watchdog) {
+ reference_theta_ = IpCq().curr_constraint_violation();
+ reference_barr_ = IpCq().curr_barrier_obj();
+ reference_gradBarrTDelta_ = IpCq().curr_gradBarrTDelta();
+ }
+ else {
+ reference_theta_ = watchdog_theta_;
+ reference_barr_ = watchdog_barr_;
+ reference_gradBarrTDelta_ = watchdog_gradBarrTDelta_;
+ }
+ filter_.Print(Jnlst());
+ }
+
+ bool FilterLSAcceptor::IsFtype(Number alpha_primal_test)
+ {
+ DBG_START_METH("FilterLSAcceptor::IsFtype",
+ dbg_verbosity);
+ Jnlst().Printf(J_MOREDETAILED, J_LINE_SEARCH,
+ "reference_theta = %e reference_gradBarrTDelta = %e\n",
+ reference_theta_, reference_gradBarrTDelta_);
+ Number mach_eps = std::numeric_limits<Number>::epsilon();
+ // ToDo find good value
+ if (reference_theta_==0. && reference_gradBarrTDelta_ > 0. &&
+ reference_gradBarrTDelta_ < 100.*mach_eps) {
+ reference_gradBarrTDelta_ = -mach_eps;
+ Jnlst().Printf(J_WARNING, J_LINE_SEARCH,
+ "reference_theta is slightly positive at feasible point. Setting it to %e\n",
+ reference_gradBarrTDelta_);
+ }
+ DBG_ASSERT(reference_theta_>0. || reference_gradBarrTDelta_ < 0.0);
+ return (reference_gradBarrTDelta_ < 0.0 &&
+ alpha_primal_test*pow(-reference_gradBarrTDelta_,s_phi_) >
+ delta_*pow(reference_theta_,s_theta_));
+ }
+
+ void FilterLSAcceptor::AugmentFilter()
+ {
+ DBG_START_METH("FilterLSAcceptor::AugmentFilter",
+ dbg_verbosity);
+
+ Number phi_add = reference_barr_ - gamma_phi_*reference_theta_;
+ Number theta_add = (1.-gamma_theta_)*reference_theta_;
+
+ filter_.AddEntry(phi_add, theta_add, IpData().iter_count());
+ }
+
+ bool
+ FilterLSAcceptor::CheckAcceptabilityOfTrialPoint(Number alpha_primal_test)
+ {
+ DBG_START_METH("FilterLSAcceptor::CheckAcceptabilityOfTrialPoint",
+ dbg_verbosity);
+
+ bool accept;
+
+ // First compute the barrier function and constraint violation at the
+ // current iterate and the trial point
+
+ Number trial_theta = IpCq().trial_constraint_violation();
+ // Check if constraint violation is becoming too large
+ if (theta_max_ < 0.0) {
+ // ToDo should 1.0 be based on dimension? (theta is in 1 norm!!!)
+ theta_max_ = theta_max_fact_*Max(1.0, reference_theta_);
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "trial_max is initialized to %e\n",
+ theta_max_);
+ }
+ if (theta_min_ < 0.0) {
+ theta_min_ = theta_min_fact_*Max(1.0, reference_theta_);
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "trial_min is initialized to %e\n",
+ theta_min_);
+ }
+
+ if (theta_max_>0 && trial_theta>theta_max_) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "trial_theta = %e is larger than theta_max = %e\n",
+ trial_theta, theta_max_);
+ IpData().Append_info_string("Tmax");
+ return false;
+ }
+
+ Number trial_barr = IpCq().trial_barrier_obj();
+ DBG_ASSERT(IsFiniteNumber(trial_barr));
+
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Checking acceptability for trial step size alpha_primal_test=%13.6e:\n", alpha_primal_test);
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ " New values of barrier function = %23.16e (reference %23.16e):\n", trial_barr, reference_barr_);
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ " New values of constraint violation = %23.16e (reference %23.16e):\n", trial_theta, reference_theta_);
+
+ // Check if point is acceptable w.r.t current iterate
+ if (alpha_primal_test>0. && IsFtype(alpha_primal_test) &&
+ reference_theta_ <= theta_min_) {
+ // Armijo condition for the barrier function has to be satisfied
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH, "Checking Armijo Condition...\n");
+ accept = ArmijoHolds(alpha_primal_test);
+ }
+ else {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH, "Checking sufficient reduction...\n");
+ accept = IsAcceptableToCurrentIterate(trial_barr, trial_theta);
+ }
+
+ if (!accept) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH, "Failed...\n");
+ last_rejection_due_to_filter_ = false;
+ return accept;
+ }
+ else {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH, "Succeeded...\n");
+ }
+
+ // Now check if that pair is acceptable to the filter
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH, "Checking filter acceptability...\n");
+ accept = IsAcceptableToCurrentFilter(trial_barr, trial_theta);
+ if (!accept) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH, "Failed...\n");
+ last_rejection_due_to_filter_ = true;
+ return accept;
+ }
+ else {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH, "Succeeded...\n");
+ }
+
+ // Filter reset heuristic
+ if (max_filter_resets_>0) {
+ if (n_filter_resets_<max_filter_resets_) {
+ if (last_rejection_due_to_filter_) {
+ count_successive_filter_rejections_++;
+ if (count_successive_filter_rejections_>=filter_reset_trigger_) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Resetting filter because in %d iterations last rejection was due to filter", count_successive_filter_rejections_);
+ IpData().Append_info_string("F+");
+ Reset();
+ }
+ }
+ else {
+ count_successive_filter_rejections_ = 0;
+ }
+ }
+ else {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Filter should be reset, but maximal number of resets already exceeded.\n");
+ IpData().Append_info_string("F-");
+ }
+ }
+ last_rejection_due_to_filter_= false;
+
+ return accept;
+ }
+
+ bool FilterLSAcceptor::ArmijoHolds(Number alpha_primal_test)
+ {
+ /*
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "ArmijoHolds test with trial_barr = %25.16e reference_barr = %25.16e\n alpha_primal_test = %25.16e reference_gradBarrTDelta = %25.16e\n", IpCq().trial_barrier_obj(), reference_barr_,alpha_primal_test,reference_gradBarrTDelta_);
+ */
+ return Compare_le(IpCq().trial_barrier_obj()-reference_barr_,
+ eta_phi_*alpha_primal_test*reference_gradBarrTDelta_,
+ reference_barr_);
+ }
+
+ Number FilterLSAcceptor::CalculateAlphaMin()
+ {
+ Number gBD = IpCq().curr_gradBarrTDelta();
+ Number curr_theta = IpCq().curr_constraint_violation();
+ Number alpha_min = gamma_theta_;
+
+ if (gBD < 0) {
+ alpha_min = Min( gamma_theta_,
+ gamma_phi_*curr_theta/(-gBD));
+ if (curr_theta <= theta_min_) {
+ alpha_min = Min( alpha_min,
+ delta_*pow(curr_theta,s_theta_)/pow(-gBD,s_phi_)
+ );
+ }
+ }
+
+ return alpha_min_frac_*alpha_min;
+ }
+
+ bool FilterLSAcceptor::IsAcceptableToCurrentIterate(Number trial_barr,
+ Number trial_theta,
+ bool called_from_restoration /*=false*/) const
+ {
+ DBG_START_METH("FilterLSAcceptor::IsAcceptableToCurrentIterate",
+ dbg_verbosity);
+
+ // Check if the barrier objective function is increasing to
+ // rapidly (according to option obj_max_inc)
+ if (!called_from_restoration && trial_barr > reference_barr_) {
+ Number basval = 1.;
+ if (fabs(reference_barr_)>10.) {
+ basval = log10(fabs(reference_barr_));
+ }
+ if (log10(trial_barr-reference_barr_)>obj_max_inc_+basval) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH, "Rejecting trial point because barrier objective function increasing too rapidly (from %27.15e to %27.15e)\n",reference_barr_,trial_barr);
+ return false;
+ }
+ }
+
+ DBG_PRINT((1,"trial_barr = %e reference_barr = %e\n", trial_barr, reference_barr_));
+ DBG_PRINT((1,"trial_theta = %e reference_theta = %e\n", trial_theta, reference_theta_));
+ return (Compare_le(trial_theta, (1.-gamma_theta_)*reference_theta_, reference_theta_)
+ || Compare_le(trial_barr-reference_barr_, -gamma_phi_*reference_theta_, reference_barr_));
+ }
+
+ bool FilterLSAcceptor::IsAcceptableToCurrentFilter(Number trial_barr, Number trial_theta) const
+ {
+ return filter_.Acceptable(trial_barr, trial_theta);
+ }
+
+ bool FilterLSAcceptor::Compare_le(Number lhs, Number rhs, Number BasVal)
+ {
+ DBG_START_FUN("FilterLSAcceptor::Compare_le",
+ dbg_verbosity);
+ DBG_PRINT((1,"lhs = %27.16e rhs = %27.16e BasVal = %27.16e\n",lhs,rhs,BasVal));
+
+ Number mach_eps = std::numeric_limits<Number>::epsilon();
+ return (lhs - rhs <= 10.*mach_eps*fabs(BasVal));
+ }
+
+ void FilterLSAcceptor::StartWatchDog()
+ {
+ DBG_START_FUN("FilterLSAcceptor::StartWatchDog", dbg_verbosity);
+
+ watchdog_theta_ = IpCq().curr_constraint_violation();
+ watchdog_barr_ = IpCq().curr_barrier_obj();
+ watchdog_gradBarrTDelta_ = IpCq().curr_gradBarrTDelta();
+ }
+
+ void FilterLSAcceptor::StopWatchDog()
+ {
+ DBG_START_FUN("FilterLSAcceptor::StopWatchDog", dbg_verbosity);
+
+ reference_theta_ = watchdog_theta_;
+ reference_barr_ = watchdog_barr_;
+ reference_gradBarrTDelta_ = watchdog_gradBarrTDelta_;
+ }
+
+ void FilterLSAcceptor::Reset()
+ {
+ DBG_START_FUN("FilterLSAcceptor::Reset", dbg_verbosity);
+
+ last_rejection_due_to_filter_ = false;
+ count_successive_filter_rejections_ = 0;
+
+ filter_.Clear();
+ }
+
+ bool
+ FilterLSAcceptor::TrySecondOrderCorrection(
+ Number alpha_primal_test,
+ Number& alpha_primal,
+ SmartPtr<IteratesVector>& actual_delta)
+ {
+ DBG_START_METH("FilterLSAcceptor::TrySecondOrderCorrection",
+ dbg_verbosity);
+
+ if (max_soc_==0) {
+ return false;
+ }
+
+ bool accept = false;
+ Index count_soc = 0;
+
+ Number theta_soc_old = 0.;
+ Number theta_trial = IpCq().trial_constraint_violation();
+ Number alpha_primal_soc = alpha_primal;
+
+ SmartPtr<Vector> c_soc = IpCq().curr_c()->MakeNew();
+ SmartPtr<Vector> dms_soc = IpCq().curr_d_minus_s()->MakeNew();
+ c_soc->Copy(*IpCq().curr_c());
+ dms_soc->Copy(*IpCq().curr_d_minus_s());
+ while (count_soc<max_soc_ && !accept &&
+ (count_soc==0 || theta_trial<=kappa_soc_*theta_soc_old) ) {
+ theta_soc_old = theta_trial;
+
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Trying second order correction number %d\n",
+ count_soc+1);
+
+ // Compute SOC constraint violation
+ c_soc->AddOneVector(1.0, *IpCq().trial_c(), alpha_primal_soc);
+ dms_soc->AddOneVector(1.0, *IpCq().trial_d_minus_s(), alpha_primal_soc);
+
+ // Compute the SOC search direction
+ SmartPtr<IteratesVector> delta_soc = actual_delta->MakeNewIteratesVector(true);
+ SmartPtr<IteratesVector> rhs = actual_delta->MakeNewContainer();
+ rhs->Set_x(*IpCq().curr_grad_lag_with_damping_x());
+ rhs->Set_s(*IpCq().curr_grad_lag_with_damping_s());
+ rhs->Set_y_c(*c_soc);
+ rhs->Set_y_d(*dms_soc);
+ rhs->Set_z_L(*IpCq().curr_relaxed_compl_x_L());
+ rhs->Set_z_U(*IpCq().curr_relaxed_compl_x_U());
+ rhs->Set_v_L(*IpCq().curr_relaxed_compl_s_L());
+ rhs->Set_v_U(*IpCq().curr_relaxed_compl_s_U());
+
+ bool retval = pd_solver_->Solve(-1.0, 0.0, *rhs, *delta_soc, true);
+ if (!retval) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "The linear system could not be solved for the corrector step.\n");
+ return false;
+ }
+
+ // Compute step size
+ alpha_primal_soc =
+ IpCq().primal_frac_to_the_bound(IpData().curr_tau(),
+ *delta_soc->x(),
+ *delta_soc->s());
+
+ // Check if trial point is acceptable
+ try {
+ // Compute the primal trial point
+ IpData().SetTrialPrimalVariablesFromStep(alpha_primal_soc, *delta_soc->x(), *delta_soc->s());
+
+ // in acceptance tests, use original step size!
+ accept = CheckAcceptabilityOfTrialPoint(alpha_primal_test);
+ }
+ catch(IpoptNLP::Eval_Error& e) {
+ e.ReportException(Jnlst(), J_DETAILED);
+ Jnlst().Printf(J_WARNING, J_MAIN, "Warning: SOC step rejected due to evaluation error\n");
+ IpData().Append_info_string("e");
+ accept = false;
+ // There is no point in continuing SOC procedure
+ break;
+ }
+
+ if (accept) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH, "Second order correction step accepted with %d corrections.\n", count_soc+1);
+ // Accept all SOC quantities
+ alpha_primal = alpha_primal_soc;
+ actual_delta = delta_soc;
+ }
+ else {
+ count_soc++;
+ theta_trial = IpCq().trial_constraint_violation();
+ }
+ }
+ return accept;
+ }
+
+ bool
+ FilterLSAcceptor::TryCorrector(
+ Number alpha_primal_test,
+ Number& alpha_primal,
+ SmartPtr<IteratesVector>& actual_delta)
+ {
+ if (corrector_type_==NO_CORRECTOR ||
+ (skip_corr_if_neg_curv_ && IpData().info_regu_x()!=0.) ||
+ (skip_corr_in_monotone_mode_ && !IpData().FreeMuMode())) {
+ return false;
+ }
+
+ DBG_START_METH("FilterLSAcceptor::TryCorrector",
+ dbg_verbosity);
+
+ Index n_bounds = IpData().curr()->z_L()->Dim() + IpData().curr()->z_U()->Dim()
+ + IpData().curr()->v_L()->Dim() + IpData().curr()->v_U()->Dim();
+ if (n_bounds==0) {
+ // Nothing to be done
+ return false;
+ }
+
+ IpData().TimingStats().TryCorrector().Start();
+
+ bool accept = false;
+
+ // Compute the corrector step based on corrector_type parameter
+ // create a new iterates vector and allocate space for all the entries
+ SmartPtr<IteratesVector> delta_corr = actual_delta->MakeNewIteratesVector(true);
+
+ switch (corrector_type_) {
+ case AFFINE_CORRECTOR : {
+ // 1: Standard MPC corrector
+
+ if (!IpData().HaveAffineDeltas()) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Solving the Primal Dual System for the affine step\n");
+ // First get the right hand side
+ SmartPtr<IteratesVector> rhs_aff = delta_corr->MakeNewContainer();
+
+ rhs_aff->Set_x(*IpCq().curr_grad_lag_x());
+ rhs_aff->Set_s(*IpCq().curr_grad_lag_s());
+ rhs_aff->Set_y_c(*IpCq().curr_c());
+ rhs_aff->Set_y_d(*IpCq().curr_d_minus_s());
+ rhs_aff->Set_z_L(*IpCq().curr_compl_x_L());
+ rhs_aff->Set_z_U(*IpCq().curr_compl_x_U());
+ rhs_aff->Set_v_L(*IpCq().curr_compl_s_L());
+ rhs_aff->Set_v_U(*IpCq().curr_compl_s_U());
+
+ // create a new iterates vector (with allocated space)
+ // for the affine scaling step
+ SmartPtr<IteratesVector> step_aff = delta_corr->MakeNewIteratesVector(true);
+
+ // Now solve the primal-dual system to get the step
+ pd_solver_->Solve(-1.0, 0.0, *rhs_aff, *step_aff, false);
+
+ DBG_PRINT_VECTOR(2, "step_aff", *step_aff);
+
+ IpData().set_delta_aff(step_aff);
+ IpData().SetHaveAffineDeltas(true);
+ }
+
+ DBG_ASSERT(IpData().HaveAffineDeltas());
+
+ const SmartPtr<const IteratesVector> delta_aff = IpData().delta_aff();
+
+ delta_corr->Copy(*actual_delta);
+
+ // create a rhs vector and allocate entries
+ SmartPtr<IteratesVector> rhs = actual_delta->MakeNewIteratesVector(true);
+
+ rhs->x_NonConst()->Set(0.);
+ rhs->s_NonConst()->Set(0.);
+ rhs->y_c_NonConst()->Set(0.);
+ rhs->y_d_NonConst()->Set(0.);
+ IpNLP().Px_L()->TransMultVector(-1., *delta_aff->x(), 0., *rhs->z_L_NonConst());
+ rhs->z_L_NonConst()->ElementWiseMultiply(*delta_aff->z_L());
+ IpNLP().Px_U()->TransMultVector(1., *delta_aff->x(), 0., *rhs->z_U_NonConst());
+ rhs->z_U_NonConst()->ElementWiseMultiply(*delta_aff->z_U());
+ IpNLP().Pd_L()->TransMultVector(-1., *delta_aff->s(), 0., *rhs->v_L_NonConst());
+ rhs->v_L_NonConst()->ElementWiseMultiply(*delta_aff->v_L());
+ IpNLP().Pd_U()->TransMultVector(1., *delta_aff->s(), 0., *rhs->v_U_NonConst());
+ rhs->v_U_NonConst()->ElementWiseMultiply(*delta_aff->v_U());
+
+ pd_solver_->Solve(1.0, 1.0, *rhs, *delta_corr, true);
+
+ DBG_PRINT_VECTOR(2, "delta_corr", *delta_corr);
+ }
+ break;
+ case PRIMAL_DUAL_CORRECTOR : {
+ // 2: Second order correction for primal-dual step to
+ // primal-dual mu
+
+ delta_corr->Copy(*actual_delta);
+
+ // allocate space for the rhs
+ SmartPtr<IteratesVector> rhs = actual_delta->MakeNewIteratesVector(true);
+
+ rhs->x_NonConst()->Set(0.);
+ rhs->s_NonConst()->Set(0.);
+ rhs->y_c_NonConst()->Set(0.);
+ rhs->y_d_NonConst()->Set(0.);
+
+ Number mu = IpData().curr_mu();
+ SmartPtr<Vector> tmp;
+
+ rhs->z_L_NonConst()->Copy(*IpCq().curr_slack_x_L());
+ IpNLP().Px_L()->TransMultVector(-1., *actual_delta->x(),
+ -1., *rhs->z_L_NonConst());
+ tmp = actual_delta->z_L()->MakeNew();
+ tmp->AddTwoVectors(1., *IpData().curr()->z_L(), 1., *actual_delta->z_L(), 0.);
+ rhs->z_L_NonConst()->ElementWiseMultiply(*tmp);
+ rhs->z_L_NonConst()->AddScalar(mu);
+
+ rhs->z_U_NonConst()->Copy(*IpCq().curr_slack_x_U());
+ IpNLP().Px_U()->TransMultVector(1., *actual_delta->x(),
+ -1., *rhs->z_U_NonConst());
+ tmp = actual_delta->z_U()->MakeNew();
+ tmp->AddTwoVectors(1., *IpData().curr()->z_U(), 1., *actual_delta->z_U(), 0.);
+ rhs->z_U_NonConst()->ElementWiseMultiply(*tmp);
+ rhs->z_U_NonConst()->AddScalar(mu);
+
+ rhs->v_L_NonConst()->Copy(*IpCq().curr_slack_s_L());
+ IpNLP().Pd_L()->TransMultVector(-1., *actual_delta->s(),
+ -1., *rhs->v_L_NonConst());
+ tmp = actual_delta->v_L()->MakeNew();
+ tmp->AddTwoVectors(1., *IpData().curr()->v_L(), 1., *actual_delta->v_L(), 0.);
+ rhs->v_L_NonConst()->ElementWiseMultiply(*tmp);
+ rhs->v_L_NonConst()->AddScalar(mu);
+
+ rhs->v_U_NonConst()->Copy(*IpCq().curr_slack_s_U());
+ IpNLP().Pd_U()->TransMultVector(1., *actual_delta->s(),
+ -1., *rhs->v_U_NonConst());
+ tmp = actual_delta->v_U()->MakeNew();
+ tmp->AddTwoVectors(1., *IpData().curr()->v_U(), 1., *actual_delta->v_U(), 0.);
+ rhs->v_U_NonConst()->ElementWiseMultiply(*tmp);
+ rhs->v_U_NonConst()->AddScalar(mu);
+
+ DBG_PRINT_VECTOR(2, "rhs", *rhs);
+
+ pd_solver_->Solve(1.0, 1.0, *rhs, *delta_corr, true);
+
+ DBG_PRINT_VECTOR(2, "delta_corr", *delta_corr);
+ }
+ break;
+ default:
+ DBG_ASSERT(false && "Unknown corrector_type value.");
+ }
+
+ // Compute step size
+ Number alpha_primal_corr =
+ IpCq().primal_frac_to_the_bound(IpData().curr_tau(),
+ *delta_corr->x(),
+ *delta_corr->s());
+ // Set the primal trial point
+ IpData().SetTrialPrimalVariablesFromStep(alpha_primal_corr, *delta_corr->x(), *delta_corr->s());
+
+ // Check if we want to not even try the filter criterion
+ Number alpha_dual_max =
+ IpCq().dual_frac_to_the_bound(IpData().curr_tau(),
+ *delta_corr->z_L(), *delta_corr->z_U(),
+ *delta_corr->v_L(), *delta_corr->v_U());
+
+ IpData().SetTrialBoundMultipliersFromStep(alpha_dual_max, *delta_corr->z_L(), *delta_corr->z_U(), *delta_corr->v_L(), *delta_corr->v_U());
+
+ Number trial_avrg_compl = IpCq().trial_avrg_compl();
+ Number curr_avrg_compl = IpCq().curr_avrg_compl();
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "avrg_compl(curr) = %e, avrg_compl(trial) = %e\n",
+ curr_avrg_compl, trial_avrg_compl);
+ if (corrector_type_==AFFINE_CORRECTOR &&
+ trial_avrg_compl>=corrector_compl_avrg_red_fact_*curr_avrg_compl) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Rejecting corrector step, because trial complementarity is too large.\n" );
+ IpData().TimingStats().TryCorrector().End();
+ return false;
+ }
+
+ // Check if trial point is acceptable
+ try {
+ // in acceptance tests, use original step size!
+ accept = CheckAcceptabilityOfTrialPoint(alpha_primal_test);
+ }
+ catch(IpoptNLP::Eval_Error& e) {
+ e.ReportException(Jnlst(), J_DETAILED);
+ Jnlst().Printf(J_WARNING, J_MAIN,
+ "Warning: Corrector step rejected due to evaluation error\n");
+ IpData().Append_info_string("e");
+ accept = false;
+ }
+
+ if (accept) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Corrector step accepted with alpha_primal = %e\n",
+ alpha_primal_corr);
+ // Accept all SOC quantities
+ alpha_primal = alpha_primal_corr;
+ actual_delta = delta_corr;
+
+ if (Jnlst().ProduceOutput(J_MOREVECTOR, J_MAIN)) {
+ Jnlst().Printf(J_MOREVECTOR, J_MAIN,
+ "*** Accepted corrector for Iteration: %d\n",
+ IpData().iter_count());
+ delta_corr->Print(Jnlst(), J_MOREVECTOR, J_MAIN, "delta_corr");
+ }
+ }
+
+ IpData().TimingStats().TryCorrector().End();
+ return accept;
+ }
+
+ char FilterLSAcceptor::UpdateForNextIteration(Number alpha_primal_test)
+ {
+ char info_alpha_primal_char;
+ // Augment the filter if required
+ if (!IsFtype(alpha_primal_test) ||
+ !ArmijoHolds(alpha_primal_test)) {
+ AugmentFilter();
+ info_alpha_primal_char = 'h';
+ }
+ else {
+ info_alpha_primal_char = 'f';
+ }
+ return info_alpha_primal_char;
+ }
+
+ void FilterLSAcceptor::PrepareRestoPhaseStart()
+ {
+ AugmentFilter();
+ }
+
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpFilterLSAcceptor.hpp b/SimTKmath/Optimizers/src/IpOpt/IpFilterLSAcceptor.hpp
new file mode 100644
index 0000000..3f1da70
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpFilterLSAcceptor.hpp
@@ -0,0 +1,273 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpFilterLSAcceptor.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-10-13
+// derived file from IpFilterLineSearch.hpp
+
+#ifndef __IPFILTERLSACCEPTOR_HPP__
+#define __IPFILTERLSACCEPTOR_HPP__
+
+#include "IpFilter.hpp"
+#include "IpBacktrackingLSAcceptor.hpp"
+#include "IpPDSystemSolver.hpp"
+
+namespace Ipopt
+{
+
+ /** Filter line search. This class implements the filter line
+ * search procedure.
+ */
+ class FilterLSAcceptor : public BacktrackingLSAcceptor
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor. The PDSystemSolver object only needs to be
+ * provided (i.e. not NULL) if second order correction or
+ * corrector steps are to be used. */
+ FilterLSAcceptor(const SmartPtr<PDSystemSolver>& pd_solver);
+
+ /** Default destructor */
+ virtual ~FilterLSAcceptor();
+ //@}
+
+ /** InitializeImpl - overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Reset the acceptor.
+ * This function should be called if all previous information
+ * should be discarded when the line search is performed the
+ * next time. For example, this method should be called if
+ * the barrier parameter is changed.
+ */
+ virtual void Reset();
+
+ /** Initialization for the next line search. The flag in_watchdog
+ * indicates if we are currently in an active watchdog
+ * procedure. */
+ virtual void InitThisLineSearch(bool in_watchdog);
+
+ /** Method that is called before the restoration phase is called.
+ * Here, we can set up things that are required in the
+ * termination test for the restoration phase, such as augmenting
+ * a filter. */
+ virtual void PrepareRestoPhaseStart();
+
+ /** Method returning the lower bound on the trial step sizes. If
+ * the backtracking procedure encounters a trial step size below
+ * this value after the first trial set, it swtiches to the
+ * (soft) restoration phase. */
+ virtual Number CalculateAlphaMin();
+
+ /** Method for checking if current trial point is acceptable.
+ * It is assumed that the delta information in ip_data is the
+ * search direction used in criteria. The primal trial point has
+ * to be set before the call.
+ */
+ virtual bool CheckAcceptabilityOfTrialPoint(Number alpha_primal);
+
+ /** Try a second order correction for the constraints. If the
+ * first trial step (with incoming alpha_primal) has been reject,
+ * this tries up to max_soc_ second order corrections for the
+ * constraints. Here, alpha_primal_test is the step size that
+ * has to be used in the filter acceptance tests. On output
+ * actual_delta_ has been set to the step including the
+ * second order correction if it has been accepted, otherwise it
+ * is unchanged. If the SOC step has been accepted, alpha_primal
+ * has the fraction-to-the-boundary value for the SOC step on output.
+ * The return value is true, if a SOC step has been accepted.
+ */
+ virtual bool TrySecondOrderCorrection(Number alpha_primal_test,
+ Number& alpha_primal,
+ SmartPtr<IteratesVector>& actual_delta);
+
+ /** Try higher order corrector (for fast local convergence). In
+ * contrast to a second order correction step, which tries to
+ * make an unacceptable point acceptable by improving constraint
+ * violation, this corrector step is tried even if the regular
+ * primal-dual step is acceptable.
+ */
+ virtual bool TryCorrector(Number alpha_primal_test,
+ Number& alpha_primal,
+ SmartPtr<IteratesVector>& actual_delta);
+
+ /** Method for ending the current line search. When it is called,
+ * the internal data should be updates, e.g., the filter might be
+ * augmented. alpha_primal_test is the value of alpha that has
+ * been used for in the acceptence test ealier. */
+ virtual char UpdateForNextIteration(Number alpha_primal_test);
+
+ /** Method for setting internal data if the watchdog procedure is
+ * started. */
+ virtual void StartWatchDog();
+
+ /** Method for setting internal data if the watchdog procedure is
+ * stopped. */
+ virtual void StopWatchDog();
+
+ /**@name Trial Point Accepting Methods. Used internally to check certain
+ * acceptability criteria and used externally (by the restoration phase
+ * convergence check object, for instance)
+ */
+ //@{
+ /** Checks if a trial point is acceptable to the current iterate */
+ bool IsAcceptableToCurrentIterate(Number trial_barr, Number trial_theta,
+ bool called_from_restoration=false) const;
+
+ /** Checks if a trial point is acceptable to the current filter */
+ bool IsAcceptableToCurrentFilter(Number trial_barr, Number trial_theta) const;
+ //@}
+
+ /** Methods for OptionsList */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ FilterLSAcceptor(const FilterLSAcceptor&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const FilterLSAcceptor&);
+ //@}
+
+ /** @name Filter information */
+ //@{
+ /** Upper bound on infeasibility */
+ Number theta_max_;
+ Number theta_max_fact_;
+
+ /** Infeasibility switching bound */
+ Number theta_min_;
+ Number theta_min_fact_;
+ //@}
+
+ /** Method for checking if the current step size satisfies the
+ * f-type switching condition. Here, we use the search direction
+ * stored in ip_data
+ */
+ bool IsFtype(Number alpha_primal_test);
+
+ /** Method for checking the Armijo condition, given a trial step
+ * size. The test uses the search direction stored in ip_data,
+ * and the values of the functions at the trial point in ip_data.
+ */
+ bool ArmijoHolds(Number alpha_primal_test);
+
+ /** Augment the filter used on the current values of the barrier
+ * objective function and the contraint violation */
+ void AugmentFilter();
+
+ /** Check comparison "lhs <= rhs", using machine precision based on BasVal */
+ //ToDo This should probably not be a static member function if we want to
+ // allow for different relaxation parameters values
+ static bool Compare_le(Number lhs, Number rhs, Number BasVal);
+
+ /** @name Parameters for the filter algorithm. Names as in the paper */
+ //@{
+ /** \f$ \eta_{\varphi} \f$ */
+ Number eta_phi_;
+ /** \f$ \delta \f$ */
+ Number delta_;
+ /** \f$ s_{\varphi} \f$ */
+ Number s_phi_;
+ /** \f$ s_{\Theta} \f$ */
+ Number s_theta_;
+ /** \f$ \gamma_{\varphi} \f$ */
+ Number gamma_phi_;
+ /** \f$ \gamma_{\Theta} \f$ */
+ Number gamma_theta_;
+ /** \f$ \gamma_{\alpha} \f$ */
+ Number alpha_min_frac_;
+ /** Maximal number of second order correction steps */
+ Index max_soc_;
+ /** Required reduction in constraint violation before trying
+ * multiple second order correction steps \f$ \kappa_{soc}\f$.
+ */
+ Number kappa_soc_;
+ /** Maximal increase in objective function in orders of magnitute
+ * (log10). If the log10(barrier objective function) is
+ * increased more than this compared to the current point, the
+ * trial point is rejected. */
+ Number obj_max_inc_;
+
+ /** enumeration for the corrector type */
+ enum CorrectorTypeEnum {
+ NO_CORRECTOR=0,
+ AFFINE_CORRECTOR,
+ PRIMAL_DUAL_CORRECTOR
+ };
+ /** Type of corrector steps that should be tried. */
+ CorrectorTypeEnum corrector_type_;
+ /** parameter in heurstic that determines whether corrector step
+ should be tried. */
+ Number corrector_compl_avrg_red_fact_;
+ /** Flag indicating whether the corrector should be skipped in an
+ * iteration in which negative curvature is detected */
+ bool skip_corr_if_neg_curv_;
+ /** Flag indicating whether the corrector should be skipped during
+ * the monotone mu mode. */
+ bool skip_corr_in_monotone_mode_;
+ /** maximal allowed number of filter resets. */
+ Index max_filter_resets_;
+ /** interation counter trigger for filter reset. If the
+ * successive number of iterations in which the last rejected
+ * step was due to the filter, and max_filter_resets is non-zero,
+ * then the filter is reset. */
+ Index filter_reset_trigger_;
+ //@}
+
+ /** @name Information related to watchdog procedure */
+ //@{
+ /** Constraint violation at the point with respect to which
+ * progress is to be made */
+ Number reference_theta_;
+ /** Barrier objective function at the point with respect to which
+ * progress is to be made */
+ Number reference_barr_;
+ /** Barrier gradient transpose search direction at the point with
+ * respect to which progress is to be made */
+ Number reference_gradBarrTDelta_;
+ /** Constraint violation at reference point */
+ Number watchdog_theta_;
+ /** Barrier objective function at reference point */
+ Number watchdog_barr_;
+ /** Barrier gradient transpose search direction at reference point */
+ Number watchdog_gradBarrTDelta_;
+ //@}
+
+ /** Filter with entries */
+ Filter filter_;
+
+ /** @name Filter reset stuff */
+ //@{
+ /** True, if last rejected was due to the filter. */
+ Number last_rejection_due_to_filter_;
+ /** Counter of successive iterations in which filter was reason
+ * for last rejection. */
+ Index count_successive_filter_rejections_;
+ /** Counter for the filter resets done so far. */
+ Index n_filter_resets_;
+ //@}
+
+ /** @name Strategy objective that are used */
+ //@{
+ SmartPtr<PDSystemSolver> pd_solver_;
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpGenTMatrix.cpp b/SimTKmath/Optimizers/src/IpOpt/IpGenTMatrix.cpp
new file mode 100644
index 0000000..3957b2f
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpGenTMatrix.cpp
@@ -0,0 +1,197 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpGenTMatrix.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpGenTMatrix.hpp"
+#include "IpDenseVector.hpp"
+#include "IpBlas.hpp"
+
+namespace Ipopt
+{
+
+ GenTMatrix::GenTMatrix(const GenTMatrixSpace* owner_space)
+ :
+ Matrix(owner_space),
+ owner_space_(owner_space),
+ values_(NULL),
+ initialized_(false)
+ {
+ values_ = owner_space_->AllocateInternalStorage();
+
+ if (Nonzeros() == 0) {
+ initialized_ = true; // I guess ?!? what does this mean ?!?
+ }
+ }
+
+ GenTMatrix::~GenTMatrix()
+ {
+ owner_space_->FreeInternalStorage(values_);
+ }
+
+ void GenTMatrix::SetValues(const Number* Values)
+ {
+ IpBlasDcopy(Nonzeros(), Values, 1, values_, 1);
+ initialized_ = true;
+ ObjectChanged();
+ }
+
+ void GenTMatrix::MultVectorImpl(Number alpha, const Vector &x, Number beta,
+ Vector &y) const
+ {
+ // A few sanity checks
+ DBG_ASSERT(NCols()==x.Dim());
+ DBG_ASSERT(NRows()==y.Dim());
+
+ // Take care of the y part of the addition
+ DBG_ASSERT(initialized_);
+ if( beta!=0.0 ) {
+ y.Scal(beta);
+ }
+ else {
+ y.Set(0.0); // In case y hasn't been initialized yet
+ }
+
+ // See if we can understand the data
+ const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x);
+ DBG_ASSERT(dense_x); /* ToDo: Implement others */
+ DenseVector* dense_y = dynamic_cast<DenseVector*>(&y);
+ DBG_ASSERT(dense_y); /* ToDo: Implement others */
+
+ if (dense_x && dense_y) {
+ const Index* irows=Irows();
+ const Index* jcols=Jcols();
+ const Number* val=values_;
+ Number* yvals=dense_y->Values();
+ if (dense_x->IsHomogeneous()) {
+ Number as = alpha * dense_x->Scalar();
+ for(Index i=0; i<Nonzeros(); i++) {
+ yvals[*irows-1] += as * (*val);
+ val++;
+ irows++;
+ }
+ }
+ else {
+ const Number* xvals=dense_x->Values();
+ for(Index i=0; i<Nonzeros(); i++) {
+ yvals[*irows-1] += alpha* (*val) * xvals[*jcols-1];
+ val++;
+ irows++;
+ jcols++;
+ }
+ }
+ }
+ }
+
+ void GenTMatrix::TransMultVectorImpl(Number alpha, const Vector &x, Number beta,
+ Vector &y) const
+ {
+ // A few sanity checks
+ DBG_ASSERT(NCols()==y.Dim());
+ DBG_ASSERT(NRows()==x.Dim());
+
+ // Take care of the y part of the addition
+ DBG_ASSERT(initialized_);
+ if( beta!=0.0 ) {
+ y.Scal(beta);
+ }
+ else {
+ y.Set(0.0); // In case y hasn't been initialized yet
+ }
+
+ // See if we can understand the data
+ const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x);
+ DBG_ASSERT(dense_x); /* ToDo: Implement others */
+ DenseVector* dense_y = dynamic_cast<DenseVector*>(&y);
+ DBG_ASSERT(dense_y); /* ToDo: Implement others */
+
+ if (dense_x && dense_y) {
+ const Index* irows=Irows();
+ const Index* jcols=Jcols();
+ const Number* val=values_;
+ Number* yvals=dense_y->Values();
+
+ if (dense_x->IsHomogeneous()) {
+ Number as = alpha * dense_x->Scalar();
+ for(Index i=0; i<Nonzeros(); i++) {
+ yvals[*jcols-1] += as * (*val);
+ val++;
+ jcols++;
+ }
+ }
+ else {
+ const Number* xvals=dense_x->Values();
+ for(Index i=0; i<Nonzeros(); i++) {
+ yvals[*jcols-1] += alpha* (*val) * xvals[*irows-1];
+ val++;
+ irows++;
+ jcols++;
+ }
+ }
+ }
+ }
+
+ bool GenTMatrix::HasValidNumbersImpl() const
+ {
+ DBG_ASSERT(initialized_);
+ Number sum = IpBlasDasum(Nonzeros(), values_, 1);
+ return IsFiniteNumber(sum);
+ }
+
+ void GenTMatrix::PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ jnlst.Printf(level, category, "\n");
+ jnlst.PrintfIndented(level, category, indent,
+ "%sGenTMatrix \"%s\" with %d nonzero elements:\n",
+ prefix.c_str(), name.c_str(), Nonzeros());
+ if (initialized_) {
+ for (Index i=0; i<Nonzeros(); i++) {
+ jnlst.PrintfIndented(level, category, indent,
+ "%s%s[%5d,%5d]=%23.16e (%d)\n",
+ prefix.c_str(), name.c_str(), Irows()[i],
+ Jcols()[i], values_[i], i);
+ }
+ }
+ else {
+ jnlst.PrintfIndented(level, category, indent,
+ "%sUninitialized!\n", prefix.c_str());
+ }
+ }
+
+ GenTMatrixSpace::GenTMatrixSpace(Index nRows, Index nCols,
+ Index nonZeros,
+ const Index* iRows, const Index* jCols)
+ :
+ MatrixSpace(nRows, nCols),
+ nonZeros_(nonZeros),
+ jCols_(NULL),
+ iRows_(NULL)
+ {
+ iRows_ = new Index[nonZeros];
+ jCols_ = new Index[nonZeros];
+ for (Index i=0; i<nonZeros; i++) {
+ iRows_[i] = iRows[i];
+ jCols_[i] = jCols[i];
+ }
+ }
+
+ Number* GenTMatrixSpace::AllocateInternalStorage() const
+ {
+ return new Number[Nonzeros()];
+ }
+
+ void GenTMatrixSpace::FreeInternalStorage(Number* values) const
+ {
+ delete [] values;
+ }
+
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpGenTMatrix.hpp b/SimTKmath/Optimizers/src/IpOpt/IpGenTMatrix.hpp
new file mode 100644
index 0000000..9696415
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpGenTMatrix.hpp
@@ -0,0 +1,248 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpGenTMatrix.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPGENTMATRIX_HPP__
+#define __IPGENTMATRIX_HPP__
+
+#include "IpUtils.hpp"
+#include "IpMatrix.hpp"
+
+namespace Ipopt
+{
+
+ /* forward declarations */
+ class GenTMatrixSpace;
+
+ /** Class for general matrices stored in triplet format. In the
+ * triplet format, the nonzeros elements of a general matrix is
+ * stored in three arrays, Irow, Jcol, and Values, all of length
+ * Nonzeros. The first two arrays indicate the location of a
+ * non-zero element (row and column indices), and the last array
+ * stores the value at that location. If nonzero elements are
+ * listed more than once, their values are added.
+ *
+ * The structure of the nonzeros (i.e. the arrays Irow and Jcol)
+ * cannot be changed after the matrix can been initialized. Only
+ * the values of the nonzero elements can be modified.
+ *
+ * Note that the first row and column of a matrix has index 1, not
+ * 0.
+ */
+ class GenTMatrix : public Matrix
+ {
+ public:
+
+ /**@name Constructors / Destructors */
+ //@{
+
+ /** Constructor, taking the owner_space.
+ */
+ GenTMatrix(const GenTMatrixSpace* owner_space);
+
+ /** Destructor */
+ ~GenTMatrix();
+ //@}
+
+ /**@name Changing the Values.*/
+ //@{
+ /** Set values of nonzero elements. The values of the nonzero
+ * elements is copied from the incoming Number array. Important:
+ * It is assume that the order of the values in Values
+ * corresponds to the one of Irn and Jcn given to one of the
+ * constructors above. */
+ void SetValues(const Number* Values);
+ //@}
+
+ /** @name Accessor Methods */
+ //@{
+ /** Number of nonzero entries */
+ Index Nonzeros() const;
+
+ /** Array with Row indices (counting starts at 1) */
+ const Index* Irows() const;
+
+ /** Array with Column indices (counting starts at 1) */
+ const Index* Jcols() const;
+
+ /** Array with nonzero values (const version). */
+ const Number* Values() const
+ {
+ return values_;
+ }
+
+ /** Array with the nonzero values of this matrix (non-const
+ * version). Use this method only if you are intending to change
+ * the values, because the GenTMatrix will be marked as changed.
+ */
+ Number* Values()
+ {
+ ObjectChanged();
+ initialized_ = true;
+ return values_;
+ }
+ //@}
+
+ protected:
+ /**@name Overloaded methods from Matrix base class*/
+ //@{
+ virtual void MultVectorImpl(Number alpha, const Vector &x, Number beta,
+ Vector &y) const;
+
+ virtual void TransMultVectorImpl(Number alpha, const Vector& x, Number beta,
+ Vector& y) const;
+
+ /** Method for determining if all stored numbers are valid (i.e.,
+ * no Inf or Nan). */
+ virtual bool HasValidNumbersImpl() const;
+
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const;
+ //@}
+
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ GenTMatrix();
+
+ /** Copy Constructor */
+ GenTMatrix(const GenTMatrix&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const GenTMatrix&);
+ //@}
+
+ /** Copy of the owner space as a GenTMatrixSpace instead of
+ * a MatrixSpace
+ */
+ const GenTMatrixSpace* owner_space_;
+
+ /** Values of nonzeros */
+ Number* values_;
+
+ /** Flag for Initialization */
+ bool initialized_;
+
+ };
+
+ /** This is the matrix space for a GenTMatrix with fixed sparsity
+ * structure. The sparsity structure is stored here in the matrix
+ * space.
+ */
+ class GenTMatrixSpace : public MatrixSpace
+ {
+ public:
+ /** @name Constructors / Destructors */
+ //@{
+ /** Constructor, given the number of rows and columns, as well as
+ * the number of nonzeros and the position of the nonzero
+ * elements. Note that the counting of the nonzeros starts a 1,
+ * i.e., iRows[i]==1 and jCols[i]==1 refers to the first element
+ * in the first row. This is in accordance with the HSL data
+ * structure.
+ */
+ GenTMatrixSpace(Index nRows, Index nCols,
+ Index nonZeros,
+ const Index* iRows, const Index* jCols);
+
+ /** Destructor */
+ ~GenTMatrixSpace()
+ {
+ delete [] iRows_;
+ delete [] jCols_;
+ }
+ //@}
+
+ /** Method for creating a new matrix of this specific type. */
+ GenTMatrix* MakeNewGenTMatrix() const
+ {
+ return new GenTMatrix(this);
+ }
+
+ /** Overloaded MakeNew method for the MatrixSpace base class.
+ */
+ virtual Matrix* MakeNew() const
+ {
+ return MakeNewGenTMatrix();
+ }
+
+ /**@name Methods describing Matrix structure */
+ //@{
+ /** Number of non-zeros in the sparse matrix */
+ Index Nonzeros() const
+ {
+ return nonZeros_;
+ }
+
+ /** Row index of each non-zero element (counting starts at 1) */
+ const Index* Irows() const
+ {
+ return iRows_;
+ }
+
+ /** Column index of each non-zero element (counting starts at 1) */
+ const Index* Jcols() const
+ {
+ return jCols_;
+ }
+ //@}
+
+ private:
+ /** @name Sparsity structure of matrices generated by this matrix
+ * space.
+ */
+ //@{
+ const Index nonZeros_;
+ Index* jCols_;
+ Index* iRows_;
+ //@}
+
+ /** This method is only for the GenTMatrix to call in order
+ * to allocate internal storage */
+ Number* AllocateInternalStorage() const;
+
+ /** This method is only for the GenTMatrix to call in order
+ * to de-allocate internal storage */
+ void FreeInternalStorage(Number* values) const;
+
+ friend class GenTMatrix;
+ };
+
+ /* inline methods */
+ inline
+ Index GenTMatrix::Nonzeros() const
+ {
+ return owner_space_->Nonzeros();
+ }
+
+ inline
+ const Index* GenTMatrix::Irows() const
+ {
+ return owner_space_->Irows();
+ }
+
+ inline
+ const Index* GenTMatrix::Jcols() const
+ {
+ return owner_space_->Jcols();
+ }
+
+
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpGradientScaling.cpp b/SimTKmath/Optimizers/src/IpOpt/IpGradientScaling.cpp
new file mode 100644
index 0000000..1c91f69
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpGradientScaling.cpp
@@ -0,0 +1,224 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpGradientScaling.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-07-13
+
+#include "IpGradientScaling.hpp"
+#include "IpTripletHelper.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+namespace Ipopt
+{
+
+ void GradientScaling::RegisterOptions(const SmartPtr<RegisteredOptions>& roptions)
+ {
+ roptions->AddLowerBoundedNumberOption(
+ "nlp_scaling_max_gradient", "Maximum gradient after NLP scaling.",
+ 0, true, 100.0,
+ "This is the gradient scaling cut-off. If the maximum"
+ " gradient is above this value, then gradient based scaling"
+ " will be performed. Scaling parameters are calculated to"
+ " scale the maximum gradient back to this value. (This is g_max in "
+ "Section 3.8 of the implementation paper.) Note: This"
+ " option is only used if \"nlp_scaling_method\" is chosen as"
+ " \"gradient-based\".");
+ }
+
+ bool GradientScaling::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ options.GetNumericValue("nlp_scaling_max_gradient", scaling_max_gradient_, prefix);
+ return StandardScalingBase::InitializeImpl(options, prefix);
+ }
+
+
+ void GradientScaling::DetermineScalingParametersImpl(
+ const SmartPtr<const VectorSpace> x_space,
+ const SmartPtr<const VectorSpace> c_space,
+ const SmartPtr<const VectorSpace> d_space,
+ const SmartPtr<const MatrixSpace> jac_c_space,
+ const SmartPtr<const MatrixSpace> jac_d_space,
+ const SmartPtr<const SymMatrixSpace> h_space,
+ Number& df,
+ SmartPtr<Vector>& dx,
+ SmartPtr<Vector>& dc,
+ SmartPtr<Vector>& dd)
+ {
+ DBG_ASSERT(IsValid(nlp_));
+
+ SmartPtr<Vector> x = x_space->MakeNew();
+ if (!nlp_->GetStartingPoint(GetRawPtr(x), true,
+ NULL, false,
+ NULL, false,
+ NULL, false,
+ NULL, false)) {
+ THROW_EXCEPTION(FAILED_INITIALIZATION,
+ "Error getting initial point from NLP in GradientScaling.\n");
+ }
+
+ //
+ // Calculate grad_f scaling
+ //
+ SmartPtr<Vector> grad_f = x_space->MakeNew();
+ if (nlp_->Eval_grad_f(*x, *grad_f)) {
+ Number max_grad_f = grad_f->Amax();
+ df = 1;
+ if (max_grad_f > scaling_max_gradient_) {
+ df = scaling_max_gradient_ / max_grad_f;
+ }
+ Jnlst().Printf(J_DETAILED, J_INITIALIZATION,
+ "Scaling parameter for objective function = %e\n", df);
+ }
+ else {
+ Jnlst().Printf(J_WARNING, J_INITIALIZATION,
+ "Error evaluating objective gradient at user provided starting point.\n No scaling factor for objective function computed!\n");
+ df = 1;
+ }
+ //
+ // No x scaling
+ //
+ dx = NULL;
+
+ //
+ // Calculate c scaling
+ //
+ SmartPtr<Matrix> jac_c = jac_c_space->MakeNew();
+ if (nlp_->Eval_jac_c(*x, *jac_c)) {
+ // ToDo: Don't use TripletHelper, have special methods on matrices instead
+ Index nnz = TripletHelper::GetNumberEntries(*jac_c);
+ Index* irow = new Index[nnz];
+ Index* jcol = new Index[nnz];
+ Number* values = new Number[nnz];
+ TripletHelper::FillRowCol(nnz, *jac_c, irow, jcol);
+ TripletHelper::FillValues(nnz, *jac_c, values);
+ Number* c_scaling = new Number[jac_c->NRows()];
+
+ for (Index r=0; r<jac_c->NRows(); r++) {
+ c_scaling[r] = 0;
+ }
+
+ // put the max of each row into c_scaling...
+ bool need_c_scale = false;
+ for (Index i=0; i<nnz; i++) {
+ if (fabs(values[i]) > scaling_max_gradient_) {
+ c_scaling[irow[i]-1] = Max(c_scaling[irow[i]-1], fabs(values[i]));
+ need_c_scale = true;
+ }
+ }
+
+ if (need_c_scale) {
+ // now compute the scaling factors for each row
+ for (Index r=0; r<jac_c->NRows(); r++) {
+ Number scaling = 1.0;
+ if (c_scaling[r] > scaling_max_gradient_) {
+ scaling = scaling_max_gradient_/c_scaling[r];
+ }
+ c_scaling[r] = scaling;
+ }
+
+ dc = c_space->MakeNew();
+ TripletHelper::PutValuesInVector(jac_c->NRows(), c_scaling, *dc);
+ if (Jnlst().ProduceOutput(J_DETAILED, J_INITIALIZATION)) {
+ Jnlst().Printf(J_DETAILED, J_INITIALIZATION,
+ "Equality constraints are scaled with smallest scaling parameter is %e\n", dc->Min());
+ }
+ }
+ else {
+ Jnlst().Printf(J_DETAILED, J_INITIALIZATION,
+ "Equality constraints are not scaled.\n");
+ dc = NULL;
+ }
+
+ delete [] irow;
+ irow = NULL;
+ delete [] jcol;
+ jcol = NULL;
+ delete [] values;
+ values = NULL;
+ delete [] c_scaling;
+ c_scaling = NULL;
+ }
+ else {
+ Jnlst().Printf(J_WARNING, J_INITIALIZATION,
+ "Error evaluating Jacobian of equality constraints at user provided starting point.\n No scaling factors for equality constraints computed!\n");
+ dc = NULL;
+ }
+
+ //
+ // Calculate d scaling
+ //
+ SmartPtr<Matrix> jac_d = jac_d_space->MakeNew();
+ if (nlp_->Eval_jac_d(*x, *jac_d)) {
+ Index nnz = TripletHelper::GetNumberEntries(*jac_d);
+ Index* irow = new Index[nnz];
+ Index* jcol = new Index[nnz];
+ Number* values = new Number[nnz];
+ TripletHelper::FillRowCol(nnz, *jac_d, irow, jcol);
+ TripletHelper::FillValues(nnz, *jac_d, values);
+ Number* d_scaling = new Number[jac_d->NRows()];
+
+ for (Index r=0; r<jac_d->NRows(); r++) {
+ d_scaling[r] = 0;
+ }
+
+ // put the max of each row into c_scaling...
+ bool need_d_scale = false;
+ for (Index i=0; i<nnz; i++) {
+ if (fabs(values[i]) > scaling_max_gradient_) {
+ d_scaling[irow[i]-1] = Max(d_scaling[irow[i]-1], fabs(values[i]));
+ need_d_scale = true;
+ }
+ }
+
+ if (need_d_scale) {
+ // now compute the scaling factors for each row
+ for (Index r=0; r<jac_d->NRows(); r++) {
+ Number scaling = 1.0;
+ if (d_scaling[r] > scaling_max_gradient_) {
+ scaling = scaling_max_gradient_/d_scaling[r];
+ }
+ d_scaling[r] = scaling;
+ }
+
+ dd = d_space->MakeNew();
+ TripletHelper::PutValuesInVector(jac_d->NRows(), d_scaling, *dd);
+ if (Jnlst().ProduceOutput(J_DETAILED, J_INITIALIZATION)) {
+ Jnlst().Printf(J_DETAILED, J_INITIALIZATION,
+ "Inequality constraints are scaled with smallest scaling parameter is %e\n", dd->Min());
+ }
+ }
+ else {
+ dd = NULL;
+ Jnlst().Printf(J_DETAILED, J_INITIALIZATION,
+ "Inequality constraints are not scaled.\n");
+ }
+
+ delete [] irow;
+ irow = NULL;
+ delete [] jcol;
+ jcol = NULL;
+ delete [] values;
+ values = NULL;
+ delete [] d_scaling;
+ d_scaling = NULL;
+ }
+ else {
+ Jnlst().Printf(J_WARNING, J_INITIALIZATION,
+ "Error evaluating Jacobian of inequality constraints at user provided starting point.\n No scaling factors for inequality constraints computed!\n");
+ dd = NULL;
+ }
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpGradientScaling.hpp b/SimTKmath/Optimizers/src/IpOpt/IpGradientScaling.hpp
new file mode 100644
index 0000000..6cba16c
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpGradientScaling.hpp
@@ -0,0 +1,85 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpGradientScaling.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-07-13
+
+#ifndef __IPGRADIENTSCALING_HPP__
+#define __IPGRADIENTSCALING_HPP__
+
+#include "IpNLPScaling.hpp"
+#include "IpNLP.hpp"
+
+namespace Ipopt
+{
+ /** This class does problem scaling by setting the
+ * scaling parameters based on the maximum of the
+ * gradient at the user provided initial point.
+ */
+ class GradientScaling : public StandardScalingBase
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ GradientScaling(const SmartPtr<NLP>& nlp)
+ :
+ StandardScalingBase(),
+ nlp_(nlp)
+ {}
+
+ /** Default destructor */
+ virtual ~GradientScaling()
+ {}
+ //@}
+
+ /** Methods for IpoptType */
+ //@{
+ /** Register the options for this class */
+ static void RegisterOptions(const SmartPtr<RegisteredOptions>& roptions);
+ //@}
+
+ protected:
+ /** Initialize the object from the options */
+ bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ virtual void DetermineScalingParametersImpl(
+ const SmartPtr<const VectorSpace> x_space,
+ const SmartPtr<const VectorSpace> c_space,
+ const SmartPtr<const VectorSpace> d_space,
+ const SmartPtr<const MatrixSpace> jac_c_space,
+ const SmartPtr<const MatrixSpace> jac_d_space,
+ const SmartPtr<const SymMatrixSpace> h_space,
+ Number& df,
+ SmartPtr<Vector>& dx,
+ SmartPtr<Vector>& dc,
+ SmartPtr<Vector>& dd);
+
+ private:
+
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+
+ /** Copy Constructor */
+ GradientScaling(const GradientScaling&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const GradientScaling&);
+ //@}
+
+ /** pointer to the NLP to get scaling parameters */
+ SmartPtr<NLP> nlp_;
+
+ /** maximum allowed gradient before scaling is performed */
+ Number scaling_max_gradient_;
+ };
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpHessianUpdater.hpp b/SimTKmath/Optimizers/src/IpOpt/IpHessianUpdater.hpp
new file mode 100644
index 0000000..e5aa5cb
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpHessianUpdater.hpp
@@ -0,0 +1,65 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpHessianUpdater.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-12-26
+
+#ifndef __IPHESSIANUPDATER_HPP__
+#define __IPHESSIANUPDATER_HPP__
+
+#include "IpAlgStrategy.hpp"
+
+namespace Ipopt
+{
+
+ /** Abstract base class for objects responsible for updating the
+ * Hessian information. This can be done using exact second
+ * derivatives from the NLP, or by a quasi-Newton Option. The
+ * result is put into the W field in IpData.
+ */
+ class HessianUpdater : public AlgorithmStrategyObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor */
+ HessianUpdater()
+ {}
+
+ /** Default destructor */
+ virtual ~HessianUpdater()
+ {}
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix) = 0;
+
+ /** Update the Hessian based on the current information in IpData,
+ * and possibly on information from previous calls.
+ */
+ virtual void UpdateHessian() = 0;
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ HessianUpdater(const HessianUpdater&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const HessianUpdater&);
+ //@}
+
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpIdentityMatrix.cpp b/SimTKmath/Optimizers/src/IpOpt/IpIdentityMatrix.cpp
new file mode 100644
index 0000000..f97b288
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpIdentityMatrix.cpp
@@ -0,0 +1,69 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpIdentityMatrix.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpIdentityMatrix.hpp"
+
+namespace Ipopt
+{
+
+ IdentityMatrix::IdentityMatrix(const SymMatrixSpace* owner_space)
+ :
+ SymMatrix(owner_space),
+ factor_(1.0)
+ {}
+
+ IdentityMatrix::~IdentityMatrix()
+ {}
+
+ Index IdentityMatrix::Dim() const
+ {
+ DBG_ASSERT(NRows() == NCols());
+ return NRows();
+ }
+
+ void IdentityMatrix::MultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ // A few sanity checks
+ DBG_ASSERT(NRows() == NCols());
+ DBG_ASSERT(NRows() == x.Dim());
+ DBG_ASSERT(NCols() == y.Dim());
+
+ y.AddOneVector(alpha*factor_, x, beta);
+ }
+
+ bool IdentityMatrix::HasValidNumbersImpl() const
+ {
+ return IsFiniteNumber(factor_);
+ }
+
+ void IdentityMatrix::PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ DBG_ASSERT(NRows() == NCols());
+ jnlst.Printf(level, category, "\n");
+ jnlst.PrintfIndented(level, category, indent,
+ "%sIdentityMatrix \"%s\" with %d rows and columns and the factor %23.16e.\n",
+ prefix.c_str(), name.c_str(), NRows(), factor_);
+ }
+
+ void IdentityMatrix::AddMSinvZImpl(Number alpha, const Vector& S,
+ const Vector& Z, Vector& X) const
+ {
+ DBG_ASSERT(NRows() == NCols());
+ DBG_ASSERT(NRows() == S.Dim());
+ DBG_ASSERT(NCols() == Z.Dim());
+ DBG_ASSERT(NCols() == X.Dim());
+
+ X.AddVectorQuotient(alpha, Z, S, 1.);
+ }
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpIdentityMatrix.hpp b/SimTKmath/Optimizers/src/IpOpt/IpIdentityMatrix.hpp
new file mode 100644
index 0000000..32a39fb
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpIdentityMatrix.hpp
@@ -0,0 +1,147 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpIdentityMatrix.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPIDENTITYMATRIX_HPP__
+#define __IPIDENTITYMATRIX_HPP__
+
+#include "IpUtils.hpp"
+#include "IpSymMatrix.hpp"
+
+namespace Ipopt
+{
+
+ /** Class for Matrices which are multiples of the identity matrix.
+ *
+ */
+ class IdentityMatrix : public SymMatrix
+ {
+ public:
+
+ /**@name Constructors / Destructors */
+ //@{
+
+ /** Constructor, initializing with dimensions of the matrix
+ * (true identity matrix).
+ */
+ IdentityMatrix(const SymMatrixSpace* owner_space);
+
+ /** Destructor */
+ ~IdentityMatrix();
+ //@}
+
+ /** Method for setting the factor for the identity matrix. */
+ void SetFactor(Number factor)
+ {
+ factor_ = factor;
+ }
+
+ /** Method for getting the factor for the identity matrix. */
+ Number GetFactor() const
+ {
+ return factor_;
+ }
+
+ /** Method for obtaining the dimention of the matrix. */
+ Index Dim() const;
+
+ protected:
+ /**@name Methods overloaded from matrix */
+ //@{
+ virtual void MultVectorImpl(Number alpha, const Vector& x,
+ Number beta, Vector& y) const;
+
+ virtual void AddMSinvZImpl(Number alpha, const Vector& S,
+ const Vector& Z, Vector& X) const;
+
+ /** Method for determining if all stored numbers are valid (i.e.,
+ * no Inf or Nan). */
+ virtual bool HasValidNumbersImpl() const;
+
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const;
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ IdentityMatrix();
+
+ /** Copy Constructor */
+ IdentityMatrix(const IdentityMatrix&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const IdentityMatrix&);
+ //@}
+
+ /** Scaling factor for this identity matrix */
+ Number factor_;
+ };
+
+ /** This is the matrix space for IdentityMatrix. */
+ class IdentityMatrixSpace : public SymMatrixSpace
+ {
+ public:
+ /** @name Constructors / Destructors */
+ //@{
+ /** Constructor, given the dimension of the matrix. */
+ IdentityMatrixSpace(Index dim)
+ :
+ SymMatrixSpace(dim)
+ {}
+
+ /** Destructor */
+ virtual ~IdentityMatrixSpace()
+ {}
+ //@}
+
+ /** Overloaded MakeNew method for the SymMatrixSpace base class.
+ */
+ virtual SymMatrix* MakeNewSymMatrix() const
+ {
+ return MakeNewIdentityMatrix();
+ }
+
+ /** Method for creating a new matrix of this specific type. */
+ IdentityMatrix* MakeNewIdentityMatrix() const
+ {
+ return new IdentityMatrix(this);
+ }
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ IdentityMatrixSpace();
+
+ /** Copy Constructor */
+ IdentityMatrixSpace(const IdentityMatrixSpace&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const IdentityMatrixSpace&);
+ //@}
+ };
+
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpInterfacesRegOp.cpp b/SimTKmath/Optimizers/src/IpOpt/IpInterfacesRegOp.cpp
new file mode 100644
index 0000000..c57f2e4
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpInterfacesRegOp.cpp
@@ -0,0 +1,26 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpInterfacesRegOp.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-08-16
+
+#include "IpInterfacesRegOp.hpp"
+#include "IpRegOptions.hpp"
+#include "IpIpoptApplication.hpp"
+#include "IpTNLPAdapter.hpp"
+
+namespace Ipopt
+{
+
+ void RegisterOptions_Interfaces(const SmartPtr<RegisteredOptions>& roptions)
+ {
+ roptions->SetRegisteringCategory("Uncategorized");
+ IpoptApplication::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Uncategorized");
+ TNLPAdapter::RegisterOptions(roptions);
+ roptions->SetRegisteringCategory("Uncategorized");
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpInterfacesRegOp.hpp b/SimTKmath/Optimizers/src/IpOpt/IpInterfacesRegOp.hpp
new file mode 100644
index 0000000..ec2b5ad
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpInterfacesRegOp.hpp
@@ -0,0 +1,22 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpInterfacesRegOp.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-08-16
+
+#ifndef __IPINTERFACESREGOP_HPP__
+#define __IPINTERFACESREGOP_HPP__
+
+#include "IpSmartPtr.hpp"
+
+namespace Ipopt
+{
+ class RegisteredOptions;
+
+ void RegisterOptions_Interfaces(const SmartPtr<RegisteredOptions>& roptions);
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpIpoptAlg.cpp b/SimTKmath/Optimizers/src/IpOpt/IpIpoptAlg.cpp
new file mode 100644
index 0000000..0b47f64
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpIpoptAlg.cpp
@@ -0,0 +1,838 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpIpoptAlg.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpIpoptAlg.hpp"
+#include "IpJournalist.hpp"
+#include "IpRestoPhase.hpp"
+#include "IpOrigIpoptNLP.hpp"
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ static bool message_printed = false;
+
+ static void print_message(const Journalist& jnlst)
+ {
+// jnlst.Printf(J_INSUPPRESSIBLE, J_MAIN,
+ jnlst.Printf(J_DETAILED, J_MAIN,
+ "\n******************************************************************************\n"
+ "This program contains Ipopt, a library for large-scale nonlinear optimization.\n"
+ " Ipopt is released as open source code under the Common Public License (CPL).\n"
+ " For more information visit http://projects.coin-or.org/Ipopt\n"
+ "******************************************************************************\n\n");
+ message_printed = true;
+ }
+
+ IpoptAlgorithm::IpoptAlgorithm(const SmartPtr<PDSystemSolver>& pd_solver,
+ const SmartPtr<LineSearch>& line_search,
+ const SmartPtr<MuUpdate>& mu_update,
+ const SmartPtr<ConvergenceCheck>& conv_check,
+ const SmartPtr<IterateInitializer>& iterate_initializer,
+ const SmartPtr<IterationOutput>& iter_output,
+ const SmartPtr<HessianUpdater>& hessian_updater,
+ const SmartPtr<EqMultiplierCalculator>& eq_multiplier_calculator /* = NULL*/)
+ :
+ pd_solver_(pd_solver),
+ line_search_(line_search),
+ mu_update_(mu_update),
+ conv_check_(conv_check),
+ iterate_initializer_(iterate_initializer),
+ iter_output_(iter_output),
+ hessian_updater_(hessian_updater),
+ eq_multiplier_calculator_(eq_multiplier_calculator)
+ {
+ DBG_START_METH("IpoptAlgorithm::IpoptAlgorithm",
+ dbg_verbosity);
+ DBG_ASSERT(IsValid(pd_solver_));
+ DBG_ASSERT(IsValid(line_search_));
+ DBG_ASSERT(IsValid(mu_update_));
+ DBG_ASSERT(IsValid(conv_check_));
+ DBG_ASSERT(IsValid(iterate_initializer_));
+ DBG_ASSERT(IsValid(iter_output_));
+ DBG_ASSERT(IsValid(hessian_updater_));
+ }
+
+ IpoptAlgorithm::~IpoptAlgorithm()
+ {
+ DBG_START_METH("IpoptAlgorithm::~IpoptAlgorithm()",
+ dbg_verbosity);
+ }
+
+ void IpoptAlgorithm::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->SetRegisteringCategory("Line Search");
+ roptions->AddLowerBoundedNumberOption(
+ "kappa_sigma",
+ "Factor limiting the deviation of dual variables from primal estimates.",
+ 0, true, 1e10,
+ "If the dual variables deviate from their primal estimates, a correction "
+ "is performed. (See Eqn. (16) in the implementation paper.) "
+ "Setting the value to less than 1 disables the correction.");
+ roptions->AddStringOption2(
+ "recalc_y",
+ "Tells the algorithm to recalculate the equality and inequality multipliers as least square estimates.",
+ "no",
+ "no", "use the Newton step to update the multipliers",
+ "yes", "use least-square multiplier estimates",
+ "This asks the algorithm to recompute the multipliers, whenever the "
+ "current infeasibility is less than recalc_y_feas_tol. "
+ "Choosing yes might be helpful in the quasi-Newton option. However, "
+ "each recalculation requires an extra factorization of the linear "
+ "system. If a limited memory quasi-Newton option is chosen, this is "
+ "used by default.");
+ roptions->AddLowerBoundedNumberOption(
+ "recalc_y_feas_tol",
+ "Feasibility threshold for recomputation of multipliers.",
+ 0, true, 1e-6,
+ "If recalc_y is chosen and the current infeasibility is less than this "
+ "value, then the multipliers are recomputed.");
+ }
+
+ bool IpoptAlgorithm::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ DBG_START_METH("IpoptAlgorithm::InitializeImpl",
+ dbg_verbosity);
+
+ // Read the IpoptAlgorithm options
+ // Initialize the Data object
+ bool retvalue = IpData().Initialize(Jnlst(),
+ options, prefix);
+ ASSERT_EXCEPTION(retvalue, FAILED_INITIALIZATION,
+ "the IpIpoptData object failed to initialize.");
+
+ // Initialize the CQ object
+ retvalue = IpCq().Initialize(Jnlst(),
+ options, prefix);
+ ASSERT_EXCEPTION(retvalue, FAILED_INITIALIZATION,
+ "the IpIpoptCalculatedQuantities object failed to initialize.");
+
+ // Initialize the CQ object
+ retvalue = IpNLP().Initialize(Jnlst(),
+ options, prefix);
+ ASSERT_EXCEPTION(retvalue, FAILED_INITIALIZATION,
+ "the IpIpoptNLP object failed to initialize.");
+
+ // Initialize all the strategies
+ retvalue = iterate_initializer_->Initialize(Jnlst(), IpNLP(), IpData(),
+ IpCq(), options, prefix);
+ ASSERT_EXCEPTION(retvalue, FAILED_INITIALIZATION,
+ "the iterate_initializer strategy failed to initialize.");
+
+ retvalue = mu_update_->Initialize(Jnlst(), IpNLP(), IpData(), IpCq(),
+ options, prefix);
+ ASSERT_EXCEPTION(retvalue, FAILED_INITIALIZATION,
+ "the mu_update strategy failed to initialize.");
+
+ retvalue = pd_solver_->Initialize(Jnlst(), IpNLP(), IpData(), IpCq(),
+ options, prefix);
+ ASSERT_EXCEPTION(retvalue, FAILED_INITIALIZATION,
+ "the pd_solver strategy failed to initialize.");
+
+ retvalue = line_search_->Initialize(Jnlst(), IpNLP(), IpData(), IpCq(),
+ options,prefix);
+ ASSERT_EXCEPTION(retvalue, FAILED_INITIALIZATION,
+ "the line_search strategy failed to initialize.");
+
+ retvalue = conv_check_->Initialize(Jnlst(), IpNLP(), IpData(), IpCq(),
+ options, prefix);
+ ASSERT_EXCEPTION(retvalue, FAILED_INITIALIZATION,
+ "the conv_check strategy failed to initialize.");
+
+ retvalue = iter_output_->Initialize(Jnlst(), IpNLP(), IpData(), IpCq(),
+ options, prefix);
+ ASSERT_EXCEPTION(retvalue, FAILED_INITIALIZATION,
+ "the iter_output strategy failed to initialize.");
+
+ retvalue = hessian_updater_->Initialize(Jnlst(), IpNLP(), IpData(), IpCq(),
+ options, prefix);
+ ASSERT_EXCEPTION(retvalue, FAILED_INITIALIZATION,
+ "the hessian_updater strategy failed to initialize.");
+
+ options.GetNumericValue("kappa_sigma", kappa_sigma_, prefix);
+ if (!options.GetBoolValue("recalc_y", recalc_y_, prefix)) {
+ Index enum_int;
+ if (options.GetEnumValue("hessian_approximation", enum_int, prefix)) {
+ HessianApproximationType hessian_approximation =
+ HessianApproximationType(enum_int);
+ if (hessian_approximation==LIMITED_MEMORY) {
+ recalc_y_ = true;
+ }
+ }
+ }
+ if (recalc_y_) {
+ options.GetNumericValue("recalc_y_feas_tol", recalc_y_feas_tol_, prefix);
+ }
+
+ if (prefix=="resto.") {
+ skip_print_problem_stats_ = true;
+ }
+ else {
+ skip_print_problem_stats_ = false;
+ }
+
+ return true;
+ }
+
+ SolverReturn IpoptAlgorithm::Optimize()
+ {
+ DBG_START_METH("IpoptAlgorithm::Optimize", dbg_verbosity);
+
+ // Start measuring CPU time
+ IpData().TimingStats().OverallAlgorithm().Start();
+
+ if (!message_printed) {
+ print_message(Jnlst());
+ }
+
+ try {
+ IpData().TimingStats().InitializeIterates().Start();
+ // Initialize the iterates
+ InitializeIterates();
+ IpData().TimingStats().InitializeIterates().End();
+
+ if (!skip_print_problem_stats_) {
+ IpData().TimingStats().PrintProblemStatistics().Start();
+ PrintProblemStatistics();
+ IpData().TimingStats().PrintProblemStatistics().End();
+ }
+
+ IpData().TimingStats().CheckConvergence().Start();
+ ConvergenceCheck::ConvergenceStatus conv_status
+ = conv_check_->CheckConvergence();
+ IpData().TimingStats().CheckConvergence().End();
+
+ // main loop
+ while (conv_status == ConvergenceCheck::CONTINUE) {
+ // Set the Hessian Matrix
+ IpData().TimingStats().UpdateHessian().Start();
+ UpdateHessian();
+ IpData().TimingStats().UpdateHessian().End();
+
+ // do all the output for this iteration
+ IpData().TimingStats().OutputIteration().Start();
+ OutputIteration();
+ IpData().ResetInfo();
+ IpData().TimingStats().OutputIteration().End();
+
+ // initialize the flag that is set to true if the algorithm
+ // has to continue with an emergency fallback mode. For
+ // example, when no search direction can be computed, continue
+ // with the restoration phase
+ bool emergency_mode = false;
+
+ // update the barrier parameter
+ IpData().TimingStats().UpdateBarrierParameter().Start();
+ emergency_mode = !UpdateBarrierParameter();
+ IpData().TimingStats().UpdateBarrierParameter().End();
+
+ if (!emergency_mode) {
+ // solve the primal-dual system to get the full step
+ IpData().TimingStats().ComputeSearchDirection().Start();
+ emergency_mode = !ComputeSearchDirection();
+ IpData().TimingStats().ComputeSearchDirection().End();
+ }
+
+ // If we are in the emergency mode, ask to line search object
+ // to go to the fallback options. If that isn't possible,
+ // issue error message
+ if (emergency_mode) {
+ bool retval = line_search_->ActivateFallbackMechanism();
+ if (retval) {
+ Jnlst().Printf(J_WARNING, J_MAIN,
+ "WARNING: Problem in step computation; switching to emergency mode.\n");
+ }
+ else {
+ Jnlst().Printf(J_ERROR, J_MAIN,
+ "ERROR: Problem in step computation, but emergency mode cannot be activated.\n");
+ THROW_EXCEPTION(STEP_COMPUTATION_FAILED,
+ "Step computation failed.");
+ }
+ }
+
+ // Compute the new iterate
+ IpData().TimingStats().ComputeAcceptableTrialPoint().Start();
+ ComputeAcceptableTrialPoint();
+ IpData().TimingStats().ComputeAcceptableTrialPoint().End();
+
+ // Accept the new iterate
+ IpData().TimingStats().AcceptTrialPoint().Start();
+ AcceptTrialPoint();
+ IpData().TimingStats().AcceptTrialPoint().End();
+
+ IpData().Set_iter_count(IpData().iter_count()+1);
+
+ IpData().TimingStats().CheckConvergence().Start();
+ conv_status = conv_check_->CheckConvergence();
+ IpData().TimingStats().CheckConvergence().End();
+ }
+
+ IpData().TimingStats().OutputIteration().Start();
+ OutputIteration();
+ IpData().TimingStats().OutputIteration().End();
+
+ IpData().TimingStats().OverallAlgorithm().End();
+
+ if (conv_status == ConvergenceCheck::CONVERGED) {
+ return SUCCESS;
+ }
+ else if (conv_status == ConvergenceCheck::CONVERGED_TO_ACCEPTABLE_POINT) {
+ return STOP_AT_ACCEPTABLE_POINT;
+ }
+ else if (conv_status == ConvergenceCheck::MAXITER_EXCEEDED) {
+ return MAXITER_EXCEEDED;
+ }
+ else if (conv_status == ConvergenceCheck::DIVERGING) {
+ return DIVERGING_ITERATES;
+ }
+ else if (conv_status == ConvergenceCheck::USER_STOP) {
+ return USER_REQUESTED_STOP;
+ }
+ }
+ catch(TINY_STEP_DETECTED& exc) {
+ exc.ReportException(Jnlst(), J_MOREDETAILED);
+ IpData().TimingStats().UpdateBarrierParameter().EndIfStarted();
+ IpData().TimingStats().OverallAlgorithm().End();
+ return STOP_AT_TINY_STEP;
+ }
+ catch(ACCEPTABLE_POINT_REACHED& exc) {
+ exc.ReportException(Jnlst(), J_MOREDETAILED);
+ IpData().TimingStats().ComputeAcceptableTrialPoint().EndIfStarted();
+ IpData().TimingStats().OverallAlgorithm().End();
+ return STOP_AT_ACCEPTABLE_POINT;
+ }
+ catch(LOCALLY_INFEASIBLE& exc) {
+ exc.ReportException(Jnlst(), J_MOREDETAILED);
+ IpData().TimingStats().ComputeAcceptableTrialPoint().EndIfStarted();
+ IpData().TimingStats().CheckConvergence().EndIfStarted();
+ IpData().TimingStats().OverallAlgorithm().End();
+ return LOCAL_INFEASIBILITY;
+ }
+ catch(RESTORATION_CONVERGED_TO_FEASIBLE_POINT& exc) {
+ exc.ReportException(Jnlst(), J_MOREDETAILED);
+ IpData().TimingStats().ComputeAcceptableTrialPoint().EndIfStarted();
+ IpData().TimingStats().OverallAlgorithm().End();
+ return RESTORATION_FAILURE;
+ }
+ catch(RESTORATION_FAILED& exc) {
+ exc.ReportException(Jnlst(), J_MOREDETAILED);
+ IpData().TimingStats().ComputeAcceptableTrialPoint().EndIfStarted();
+ IpData().TimingStats().OverallAlgorithm().End();
+ return RESTORATION_FAILURE;
+ }
+ catch(RESTORATION_MAXITER_EXCEEDED& exc) {
+ exc.ReportException(Jnlst(), J_MOREDETAILED);
+ IpData().TimingStats().ComputeAcceptableTrialPoint().EndIfStarted();
+ IpData().TimingStats().OverallAlgorithm().End();
+ return MAXITER_EXCEEDED;
+ }
+ catch(RESTORATION_USER_STOP& exc) {
+ exc.ReportException(Jnlst(), J_MOREDETAILED);
+ IpData().TimingStats().ComputeAcceptableTrialPoint().EndIfStarted();
+ IpData().TimingStats().OverallAlgorithm().End();
+ return USER_REQUESTED_STOP;
+ }
+ catch(STEP_COMPUTATION_FAILED& exc) {
+ exc.ReportException(Jnlst(), J_MOREDETAILED);
+ IpData().TimingStats().ComputeAcceptableTrialPoint().EndIfStarted();
+ IpData().TimingStats().OverallAlgorithm().End();
+ return ERROR_IN_STEP_COMPUTATION;
+ }
+ catch(IpoptNLP::Eval_Error& exc) {
+ exc.ReportException(Jnlst(), J_MOREDETAILED);
+ IpData().TimingStats().ComputeAcceptableTrialPoint().EndIfStarted();
+ IpData().TimingStats().OverallAlgorithm().End();
+ return INVALID_NUMBER_DETECTED;
+ }
+ catch(FEASIBILITY_PROBLEM_SOLVED& exc) {
+ exc.ReportException(Jnlst(), J_MOREDETAILED);
+ IpData().TimingStats().ComputeAcceptableTrialPoint().EndIfStarted();
+ IpData().TimingStats().OverallAlgorithm().End();
+ return SUCCESS;
+ }
+ catch(TOO_FEW_DOF& exc) {
+ exc.ReportException(Jnlst(), J_MOREDETAILED);
+ IpData().TimingStats().ComputeAcceptableTrialPoint().EndIfStarted();
+ IpData().TimingStats().OverallAlgorithm().End();
+ return TOO_FEW_DEGREES_OF_FREEDOM;
+ }
+ catch(INTERNAL_ABORT& exc) {
+ exc.ReportException(Jnlst());
+ IpData().TimingStats().OverallAlgorithm().End();
+ return INTERNAL_ERROR;
+ }
+
+ DBG_ASSERT(false && "Unknown return code in the algorithm");
+
+ IpData().TimingStats().OverallAlgorithm().End();
+ return INTERNAL_ERROR;
+ }
+
+ void IpoptAlgorithm::UpdateHessian()
+ {
+ Jnlst().Printf(J_DETAILED, J_MAIN, "\n**************************************************\n");
+ Jnlst().Printf(J_DETAILED, J_MAIN, "*** Update HessianMatrix for Iteration %d:", IpData().iter_count());
+ Jnlst().Printf(J_DETAILED, J_MAIN, "\n**************************************************\n\n");
+ hessian_updater_->UpdateHessian();
+ }
+
+ bool IpoptAlgorithm::UpdateBarrierParameter()
+ {
+ Jnlst().Printf(J_DETAILED, J_MAIN, "\n**************************************************\n");
+ Jnlst().Printf(J_DETAILED, J_MAIN, "*** Update Barrier Parameter for Iteration %d:", IpData().iter_count());
+ Jnlst().Printf(J_DETAILED, J_MAIN, "\n**************************************************\n\n");
+ bool retval = mu_update_->UpdateBarrierParameter();
+
+ if (retval) {
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "Barrier Parameter: %e\n", IpData().curr_mu());
+ }
+ else {
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "Barrier parameter could not be updated!\n");
+ }
+
+ return retval;
+ }
+
+ bool IpoptAlgorithm::ComputeSearchDirection()
+ {
+ DBG_START_METH("IpoptAlgorithm::ComputeSearchDirection", dbg_verbosity);
+
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "\n**************************************************\n");
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "*** Solving the Primal Dual System for Iteration %d:",
+ IpData().iter_count());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "\n**************************************************\n\n");
+
+ bool improve_solution = false;
+ if (IpData().HaveDeltas()) {
+ improve_solution = true;
+ }
+
+ SmartPtr<IteratesVector> rhs = IpData().curr()->MakeNewContainer();
+ rhs->Set_x(*IpCq().curr_grad_lag_with_damping_x());
+ rhs->Set_s(*IpCq().curr_grad_lag_with_damping_s());
+ rhs->Set_y_c(*IpCq().curr_c());
+ rhs->Set_y_d(*IpCq().curr_d_minus_s());
+ rhs->Set_z_L(*IpCq().curr_relaxed_compl_x_L());
+ rhs->Set_z_U(*IpCq().curr_relaxed_compl_x_U());
+ rhs->Set_v_L(*IpCq().curr_relaxed_compl_s_L());
+ rhs->Set_v_U(*IpCq().curr_relaxed_compl_s_U());
+
+ DBG_PRINT_VECTOR(2, "rhs", *rhs);
+
+ // Get space for the search direction
+ SmartPtr<IteratesVector> delta =
+ IpData().curr()->MakeNewIteratesVector(true);
+
+ if (improve_solution) {
+ // We can probably avoid copying and scaling...
+ delta->AddOneVector(-1., *IpData().delta(), 0.);
+ }
+
+ bool allow_inexact = false;
+ bool retval = pd_solver_->Solve(-1.0, 0.0, *rhs, *delta, allow_inexact,
+ improve_solution);
+
+ if (retval) {
+ // Store the search directions in the IpData object
+ IpData().set_delta(delta);
+
+ Jnlst().Printf(J_MOREVECTOR, J_MAIN,
+ "*** Step Calculated for Iteration: %d\n",
+ IpData().iter_count());
+ IpData().delta()->Print(Jnlst(), J_MOREVECTOR, J_MAIN, "delta");
+ }
+ else {
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "*** Step could not be computed in iteration %d!\n",
+ IpData().iter_count());
+ }
+
+ return retval;
+ }
+
+ void IpoptAlgorithm::ComputeAcceptableTrialPoint()
+ {
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "\n**************************************************\n");
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "*** Finding Acceptable Trial Point for Iteration %d:",
+ IpData().iter_count());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "\n**************************************************\n\n");
+ line_search_->FindAcceptableTrialPoint();
+ }
+
+ void IpoptAlgorithm::OutputIteration()
+ {
+ iter_output_->WriteOutput();
+ }
+
+ void IpoptAlgorithm::InitializeIterates()
+ {
+ DBG_START_METH("IpoptAlgorithm::InitializeIterates", dbg_verbosity);
+
+ iterate_initializer_->SetInitialIterates();
+ }
+
+ void IpoptAlgorithm::AcceptTrialPoint()
+ {
+ DBG_START_METH("IpoptAlgorithm::AcceptTrialPoint", dbg_verbosity);
+ // If the line search didn't determine a new acceptable trial
+ // point, do not accept a new iterate
+ if (line_search_->CheckSkippedLineSearch()) {
+ Jnlst().Printf(J_SUMMARY, J_MAIN,
+ "Line search didn't find acceptable trial point.\n");
+ return;
+ }
+
+ // Adjust the bounds if necessary
+ Index adjusted_slacks = IpCq().AdjustedTrialSlacks();
+ DBG_PRINT((1, "adjusted_slacks = %d\n", adjusted_slacks));
+ if (adjusted_slacks>0) {
+ IpCq().ResetAdjustedTrialSlacks();
+ if (adjusted_slacks==1) {
+ Jnlst().Printf(J_WARNING, J_MAIN,
+ "In iteration %d, %d Slack too small, adjusting variable bound\n",
+ IpData().iter_count(), adjusted_slacks);
+ }
+ else {
+ Jnlst().Printf(J_WARNING, J_MAIN,
+ "In iteration %d, %d Slacks too small, adjusting variable bounds\n",
+ IpData().iter_count(), adjusted_slacks);
+ }
+ if (Jnlst().ProduceOutput(J_VECTOR, J_MAIN)) {
+ IpNLP().x_L()->Print(Jnlst(), J_VECTOR, J_MAIN, "old_x_L");
+ IpNLP().x_U()->Print(Jnlst(), J_VECTOR, J_MAIN, "old_x_U");
+ IpNLP().d_L()->Print(Jnlst(), J_VECTOR, J_MAIN, "old_d_L");
+ IpNLP().d_U()->Print(Jnlst(), J_VECTOR, J_MAIN, "old_d_U");
+ }
+
+ SmartPtr<Vector> new_x_l = IpNLP().x_L()->MakeNew();
+ IpNLP().Px_L()->TransMultVector(1.0, *IpData().trial()->x(),
+ 0.0, *new_x_l);
+ new_x_l->Axpy(-1.0, *IpCq().trial_slack_x_L());
+
+ SmartPtr<Vector> new_x_u = IpNLP().x_U()->MakeNew();
+ IpNLP().Px_U()->TransMultVector(1.0, *IpData().trial()->x(),
+ 0.0, *new_x_u);
+ new_x_u->Axpy(1.0, *IpCq().trial_slack_x_U());
+
+ SmartPtr<Vector> new_d_l = IpNLP().d_L()->MakeNew();
+ IpNLP().Pd_L()->TransMultVector(1.0, *IpData().trial()->s(),
+ 0.0, *new_d_l);
+ new_d_l->Axpy(-1.0, *IpCq().trial_slack_s_L());
+
+ SmartPtr<Vector> new_d_u = IpNLP().d_U()->MakeNew();
+ IpNLP().Pd_U()->TransMultVector(1.0, *IpData().trial()->s(),
+ 0.0, *new_d_u);
+ new_d_u->Axpy(1.0, *IpCq().trial_slack_s_U());
+
+ IpNLP().AdjustVariableBounds(*new_x_l, *new_x_u, *new_d_l, *new_d_u);
+
+ if (Jnlst().ProduceOutput(J_VECTOR, J_MAIN)) {
+ IpNLP().x_L()->Print(Jnlst(), J_VECTOR, J_MAIN, "new_x_L");
+ IpNLP().x_U()->Print(Jnlst(), J_VECTOR, J_MAIN, "new_x_U");
+ IpNLP().d_L()->Print(Jnlst(), J_VECTOR, J_MAIN, "new_d_L");
+ IpNLP().d_U()->Print(Jnlst(), J_VECTOR, J_MAIN, "new_d_U");
+ }
+
+ }
+
+ // Make sure that bound multipliers are not too far from \mu * S^{-1}
+ // (see kappa_sigma in paper)
+ bool corrected = false;
+ Number max_correction;
+ SmartPtr<const Vector> new_z_L;
+ max_correction = correct_bound_multiplier(
+ *IpData().trial()->z_L(),
+ *IpCq().trial_slack_x_L(),
+ *IpCq().trial_compl_x_L(),
+ new_z_L);
+ if (max_correction>0.) {
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "Some value in z_L becomes too large - maximal correction = %8.2e\n",
+ max_correction);
+ corrected = true;
+ }
+ SmartPtr<const Vector> new_z_U;
+ max_correction = correct_bound_multiplier(
+ *IpData().trial()->z_U(),
+ *IpCq().trial_slack_x_U(),
+ *IpCq().trial_compl_x_U(),
+ new_z_U);
+ if (max_correction>0.) {
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "Some value in z_U becomes too large - maximal correction = %8.2e\n",
+ max_correction);
+ corrected = true;
+ }
+ SmartPtr<const Vector> new_v_L;
+ max_correction = correct_bound_multiplier(
+ *IpData().trial()->v_L(),
+ *IpCq().trial_slack_s_L(),
+ *IpCq().trial_compl_s_L(),
+ new_v_L);
+ if (max_correction>0.) {
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "Some value in v_L becomes too large - maximal correction = %8.2e\n",
+ max_correction);
+ corrected = true;
+ }
+ SmartPtr<const Vector> new_v_U;
+ max_correction = correct_bound_multiplier(
+ *IpData().trial()->v_U(),
+ *IpCq().trial_slack_s_U(),
+ *IpCq().trial_compl_s_U(),
+ new_v_U);
+ if (max_correction>0.) {
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "Some value in v_U becomes too large - maximal correction = %8.2e\n",
+ max_correction);
+ corrected = true;
+ }
+ SmartPtr<IteratesVector> trial = IpData().trial()->MakeNewContainer();
+ trial->Set_bound_mult(*new_z_L, *new_z_U, *new_v_L, *new_v_U);
+ IpData().set_trial(trial);
+
+ if (corrected) {
+ IpData().Append_info_string("z");
+ }
+
+ // Accept the step
+ IpData().AcceptTrialPoint();
+
+ // If we want to recalculate the multipliers (e.g., as least
+ // square estimates), call the calculator for that
+ if (recalc_y_) {
+ // There is no point in doing this if there are no constraints
+ if (IpData().curr()->y_c()->Dim()+IpData().curr()->y_d()->Dim()==0) {
+ recalc_y_ = false;
+ }
+ }
+ if (recalc_y_ && IpCq().curr_constraint_violation()<recalc_y_feas_tol_) {
+ if (Jnlst().ProduceOutput(J_MOREDETAILED, J_MAIN)) {
+ Jnlst().Printf(J_MOREDETAILED, J_MAIN,
+ "dual infeasisibility before least square multiplier update = %e\n",
+ IpCq().curr_dual_infeasibility(NORM_MAX));
+ }
+ IpData().Append_info_string("y ");
+ DBG_ASSERT(IsValid(eq_multiplier_calculator_));
+ if (IpData().curr()->y_c()->Dim()+IpData().curr()->y_d()->Dim()>0) {
+ SmartPtr<Vector> y_c = IpData().curr()->y_c()->MakeNew();
+ SmartPtr<Vector> y_d = IpData().curr()->y_d()->MakeNew();
+ bool retval =
+ eq_multiplier_calculator_->CalculateMultipliers(*y_c, *y_d);
+ if (retval) {
+ SmartPtr<const IteratesVector> curr = IpData().curr();
+ SmartPtr<IteratesVector> iterates = curr->MakeNewContainer();
+ iterates->Set_x(*curr->x());
+ iterates->Set_s(*curr->s());
+ iterates->Set_z_L(*curr->z_L());
+ iterates->Set_z_U(*curr->z_U());
+ iterates->Set_v_L(*curr->v_L());
+ iterates->Set_v_U(*curr->v_U());
+ iterates->Set_y_c(*y_c);
+ iterates->Set_y_d(*y_d);
+ IpData().set_trial(iterates);
+ IpData().AcceptTrialPoint();
+ }
+ else {
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "Recalculation of y multipliers skipped because eq_mult_calc returned false.\n");
+ }
+ }
+ }
+ }
+
+ void IpoptAlgorithm::PrintProblemStatistics()
+ {
+ if (!Jnlst().ProduceOutput(J_SUMMARY, J_STATISTICS)) {
+ // nothing to print
+ return;
+ }
+
+ SmartPtr<const Vector> x = IpData().curr()->x();
+ SmartPtr<const Vector> x_L = IpNLP().x_L();
+ SmartPtr<const Vector> x_U = IpNLP().x_U();
+ SmartPtr<const Matrix> Px_L = IpNLP().Px_L();
+ SmartPtr<const Matrix> Px_U = IpNLP().Px_U();
+
+ Index nx_tot, nx_only_lower, nx_both, nx_only_upper;
+ calc_number_of_bounds(*IpData().curr()->x(), *IpNLP().x_L(), *IpNLP().x_U(),
+ *IpNLP().Px_L(), *IpNLP().Px_U(),
+ nx_tot, nx_only_lower, nx_both, nx_only_upper);
+
+ Index ns_tot, ns_only_lower, ns_both, ns_only_upper;
+ calc_number_of_bounds(*IpData().curr()->s(), *IpNLP().d_L(), *IpNLP().d_U(),
+ *IpNLP().Pd_L(), *IpNLP().Pd_U(),
+ ns_tot, ns_only_lower, ns_both, ns_only_upper);
+
+ Jnlst().Printf(J_SUMMARY, J_STATISTICS,
+ "Total number of variables............................: %8d\n",nx_tot);
+ Jnlst().Printf(J_SUMMARY, J_STATISTICS,
+ " variables with only lower bounds: %8d\n",
+ nx_only_lower);
+ Jnlst().Printf(J_SUMMARY, J_STATISTICS,
+ " variables with lower and upper bounds: %8d\n",nx_both);
+ Jnlst().Printf(J_SUMMARY, J_STATISTICS,
+ " variables with only upper bounds: %8d\n",
+ nx_only_upper);
+ Jnlst().Printf(J_SUMMARY, J_STATISTICS,
+ "Total number of equality constraints.................: %8d\n",
+ IpData().curr()->y_c()->Dim());
+ Jnlst().Printf(J_SUMMARY, J_STATISTICS,
+ "Total number of inequality constraints...............: %8d\n",ns_tot);
+ Jnlst().Printf(J_SUMMARY, J_STATISTICS,
+ " inequality constraints with only lower bounds: %8d\n",
+ ns_only_lower);
+ Jnlst().Printf(J_SUMMARY, J_STATISTICS,
+ " inequality constraints with lower and upper bounds: %8d\n",ns_both);
+ Jnlst().Printf(J_SUMMARY, J_STATISTICS,
+ " inequality constraints with only upper bounds: %8d\n\n",
+ ns_only_upper);
+ }
+
+ void IpoptAlgorithm::calc_number_of_bounds(
+ const Vector& x,
+ const Vector& x_L,
+ const Vector& x_U,
+ const Matrix& Px_L,
+ const Matrix& Px_U,
+ Index& n_tot,
+ Index& n_only_lower,
+ Index& n_both,
+ Index& n_only_upper)
+ {
+ DBG_START_METH("IpoptAlgorithm::calc_number_of_bounds",
+ dbg_verbosity);
+
+ n_tot = x.Dim();
+
+ SmartPtr<Vector> tmpx = x.MakeNew();
+ SmartPtr<Vector> tmpxL = x_L.MakeNew();
+ SmartPtr<Vector> tmpxU = x_U.MakeNew();
+
+ tmpxL->Set(-1.);
+ tmpxU->Set(2.);
+ Px_L.MultVector(1.0, *tmpxL, 0.0, *tmpx);
+ Px_U.MultVector(1.0, *tmpxU, 1.0, *tmpx);
+ // Now, x has elements
+ // -1 : if component has only lower bound
+ // 0 : if component has no bound
+ // 1 : if component has both lower and upper bound
+ // 2 : if component has only upper bound
+ DBG_PRINT_VECTOR(2, "x-indicator", *tmpx);
+
+ SmartPtr<Vector> tmpx0 = x.MakeNew();
+ tmpx0->Set(0.);
+
+ SmartPtr<Vector> tmpx2 = x.MakeNew();
+ tmpx2->Set(-1.0);
+ tmpx2->Axpy(1.0, *tmpx);
+ tmpx2->ElementWiseMax(*tmpx0); // tmpx2 is now 1 in those
+ // components with only upper bounds
+ n_only_upper = (Index)tmpx2->Asum();
+
+ tmpx->Axpy(-2., *tmpx2); // now make all those entries for
+ // only upper bounds zero in tmpx
+
+ tmpx2->Copy(*tmpx);
+ tmpx2->ElementWiseMax(*tmpx0); // tmpx2 is now 1 in those
+ // components with both bounds
+ n_both = (Index)tmpx2->Asum();
+
+ tmpx->Axpy(-1., *tmpx2);
+ tmpx->ElementWiseMin(*tmpx); // tmpx is now -1 in those with only
+ // lower bounds
+ n_only_lower = (Index)tmpx->Asum();
+
+ }
+
+ Number IpoptAlgorithm::correct_bound_multiplier(
+ const Vector& trial_z,
+ const Vector& trial_slack,
+ const Vector& trial_compl,
+ SmartPtr<const Vector>& new_trial_z)
+ {
+ DBG_START_METH("IpoptAlgorithm::CorrectBoundMultiplier",
+ dbg_verbosity);
+
+ if (kappa_sigma_<1. || trial_z.Dim()==0) {
+ new_trial_z = &trial_z;
+ return 0.;
+ }
+
+ // We choose as barrier parameter to be used either the current
+ // algorithmic barrier parameter (if we are not in the free mode),
+ // or the average complementarity (at the trial point)
+ Number mu;
+ if (IpData().FreeMuMode()) {
+ mu = IpCq().trial_avrg_compl();
+ mu = Min(mu, 1e3);
+ }
+ else {
+ mu = IpData().curr_mu();
+ }
+ DBG_PRINT((1,"mu = %8.2e\n", mu));
+ DBG_PRINT_VECTOR(2, "trial_z", trial_z);
+
+ // First check quickly if anything need to be corrected, using the
+ // trial complementarity directly. Here, Amax is the same as Max
+ // (and we use Amax because that can be used later)
+ if (trial_compl.Amax() <= kappa_sigma_*mu &&
+ trial_compl.Min() >= 1./kappa_sigma_*mu) {
+ new_trial_z = &trial_z;
+ return 0.;
+ }
+
+ SmartPtr<Vector> one_over_s = trial_z.MakeNew();
+ one_over_s->Copy(trial_slack);
+ one_over_s->ElementWiseReciprocal();
+
+ SmartPtr<Vector> step_z = trial_z.MakeNew();
+ step_z->AddTwoVectors(kappa_sigma_*mu, *one_over_s, -1., trial_z, 0.);
+
+ DBG_PRINT_VECTOR(2, "step_z", *step_z);
+
+ Number max_correction_up = Max(0., -step_z->Min());
+ if (max_correction_up>0.) {
+ SmartPtr<Vector> tmp = trial_z.MakeNew();
+ tmp->Set(0.);
+ step_z->ElementWiseMin(*tmp);
+ tmp->AddTwoVectors(1., trial_z, 1., *step_z, 0.);
+ new_trial_z = GetRawPtr(tmp);
+ }
+ else {
+ new_trial_z = &trial_z;
+ }
+
+ step_z->AddTwoVectors(1./kappa_sigma_*mu, *one_over_s, -1., *new_trial_z, 0.);
+
+ Number max_correction_low = Max(0., step_z->Max());
+ if (max_correction_low>0.) {
+ SmartPtr<Vector> tmp = trial_z.MakeNew();
+ tmp->Set(0.);
+ step_z->ElementWiseMax(*tmp);
+ tmp->AddTwoVectors(1., *new_trial_z, 1., *step_z, 0.);
+ new_trial_z = GetRawPtr(tmp);
+ }
+
+ DBG_PRINT_VECTOR(2, "new_trial_z", *new_trial_z);
+
+ return Max(max_correction_up, max_correction_low);
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpIpoptAlg.hpp b/SimTKmath/Optimizers/src/IpOpt/IpIpoptAlg.hpp
new file mode 100644
index 0000000..a1d894c
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpIpoptAlg.hpp
@@ -0,0 +1,205 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpIpoptAlg.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPIPOPTALG_HPP__
+#define __IPIPOPTALG_HPP__
+
+#include "IpIpoptNLP.hpp"
+#include "IpAlgStrategy.hpp"
+#include "IpPDSystemSolver.hpp"
+#include "IpLineSearch.hpp"
+#include "IpMuUpdate.hpp"
+#include "IpConvCheck.hpp"
+#include "IpOptionsList.hpp"
+#include "IpIterateInitializer.hpp"
+#include "IpIterationOutput.hpp"
+#include "IpAlgTypes.hpp"
+#include "IpHessianUpdater.hpp"
+#include "IpEqMultCalculator.hpp"
+
+namespace Ipopt
+{
+
+ /** @name Exceptions */
+ //@{
+ DECLARE_STD_EXCEPTION(STEP_COMPUTATION_FAILED);
+ //@}
+
+ /** The main ipopt algorithm class.
+ * Main Ipopt algorithm class, contains the main optimize method,
+ * handles the execution of the optimization.
+ * The constructor initializes the data structures through the nlp,
+ * and the Optimize method then assumes that everything is
+ * initialized and ready to go.
+ * After an optimization is complete, the user can access the
+ * solution through the passed in ip_data structure.
+ * Multiple calls to the Optimize method are allowed as long as the
+ * structure of the problem remains the same (i.e. starting point
+ * or nlp parameter changes only).
+ */
+ class IpoptAlgorithm : public AlgorithmStrategyObject
+ {
+ public:
+
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor. (The IpoptAlgorithm uses smart pointers for these
+ * passed-in pieces to make sure that a user of IpoptAlgoroithm
+ * cannot pass in an object created on the stack!)
+ */
+ IpoptAlgorithm(const SmartPtr<PDSystemSolver>& pd_solver,
+ const SmartPtr<LineSearch>& line_search,
+ const SmartPtr<MuUpdate>& mu_update,
+ const SmartPtr<ConvergenceCheck>& conv_check,
+ const SmartPtr<IterateInitializer>& iterate_initializer,
+ const SmartPtr<IterationOutput>& iter_output,
+ const SmartPtr<HessianUpdater>& hessian_updater,
+ const SmartPtr<EqMultiplierCalculator>& eq_multiplier_calculator = NULL);
+
+ /** Default destructor */
+ virtual ~IpoptAlgorithm();
+ //@}
+
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Main solve method. */
+ SolverReturn Optimize();
+
+ /** Methods for IpoptType */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ IpoptAlgorithm();
+
+ /** Copy Constructor */
+ IpoptAlgorithm(const IpoptAlgorithm&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const IpoptAlgorithm&);
+ //@}
+
+ /** @name Strategy objects */
+ //@{
+ SmartPtr<PDSystemSolver> pd_solver_;
+ SmartPtr<LineSearch> line_search_;
+ SmartPtr<MuUpdate> mu_update_;
+ SmartPtr<ConvergenceCheck> conv_check_;
+ SmartPtr<IterateInitializer> iterate_initializer_;
+ SmartPtr<IterationOutput> iter_output_;
+ SmartPtr<HessianUpdater> hessian_updater_;
+ /** The multipler calculator (for y_c and y_d) has to be set only
+ * if option recalc_y is set to true */
+ SmartPtr<EqMultiplierCalculator> eq_multiplier_calculator_;
+ //@}
+
+ /** @name Main steps of the algorthim */
+ //@{
+ /** Method for updating the current Hessian. This can either just
+ * evaluate the exact Hessian (based on the current iterate), or
+ * perform a quasi-Newton update.
+ */
+ void UpdateHessian();
+
+ /** Method to update the barrier parameter. Returns false, if the
+ * algorithm can't continue with the regular procedure and needs
+ * to revert to a fallback mechanism in the line search (such as
+ * restoration phase) */
+ bool UpdateBarrierParameter();
+
+ /** Method to setup the call to the PDSystemSolver. Returns
+ * false, if the algorithm can't continue with the regular
+ * procedure and needs to revert to a fallback mechanism in the
+ * line search (such as restoration phase) */
+ bool ComputeSearchDirection();
+
+ /** Method computing the new iterate (usually vialine search).
+ * The acceptable point is the one in trial after return.
+ */
+ void ComputeAcceptableTrialPoint();
+
+ /** Method for accepting the trial point as the new iteration,
+ * possibly after adjusting the variable bounds in the NLP. */
+ void AcceptTrialPoint();
+
+ /** Do all the output for one iteration */
+ void OutputIteration();
+
+ /** Sets up initial values for the iterates,
+ * Corrects the initial values for x and s (force in bounds)
+ */
+ void InitializeIterates();
+
+ /** Print the problem size statistics */
+ void PrintProblemStatistics();
+ //@}
+
+ /** @name internal flags */
+ //@{
+ /** Flag indicating if the statistic should not be printed */
+ bool skip_print_problem_stats_;
+ //@}
+
+ /** @name Algorithmic parameters */
+ //@{
+ /** safeguard factor for bound multipliers. If value >= 1, then
+ * the dual variables will never deviate from the primal estimate
+ * by more than the factors kappa_sigma and 1./kappa_sigma.
+ */
+ Number kappa_sigma_;
+ /** Flag indicating whether the y multipliers should be
+ * recalculated with the eq_mutliplier_calculator object for each
+ * new point. */
+ bool recalc_y_;
+ /** Feasibility threshold for recalc_y */
+ Number recalc_y_feas_tol_;
+ //@}
+
+ /** @name auxilliary functions */
+ //@{
+ void calc_number_of_bounds(
+ const Vector& x,
+ const Vector& x_L,
+ const Vector& x_U,
+ const Matrix& Px_L,
+ const Matrix& Px_U,
+ Index& n_tot,
+ Index& n_only_lower,
+ Index& n_both,
+ Index& n_only_upper);
+
+ /** Method for ensuring that the trial multipliers are not too far
+ * from the primal estime. If a correction is made, new_trial_z
+ * is a pointer to the corrected multiplier, and the return value
+ * of this method give the magnitutde of the largest correction
+ * that we done. If no correction was made, new_trial_z is just
+ * a pointer to trial_z, and the return value is zero.
+ */
+ Number correct_bound_multiplier(const Vector& trial_z,
+ const Vector& trial_slack,
+ const Vector& trial_compl,
+ SmartPtr<const Vector>& new_trial_z);
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpIpoptApplication.cpp b/SimTKmath/Optimizers/src/IpOpt/IpIpoptApplication.cpp
new file mode 100644
index 0000000..3c47f0b
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpIpoptApplication.cpp
@@ -0,0 +1,783 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpIpoptApplication.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-09-02
+
+#include "IpoptConfig.h"
+#include "IpIpoptApplication.hpp"
+#include "IpTNLPAdapter.hpp"
+#include "IpIpoptAlg.hpp"
+#include "IpOrigIpoptNLP.hpp"
+#include "IpIpoptData.hpp"
+#include "IpIpoptCalculatedQuantities.hpp"
+#include "IpAlgBuilder.hpp"
+#include "IpSolveStatistics.hpp"
+#include "IpLinearSolversRegOp.hpp"
+#include "IpInterfacesRegOp.hpp"
+#include "IpAlgorithmRegOp.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+#include <fstream>
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ IpoptApplication::IpoptApplication(bool create_console_out)
+ :
+ jnlst_(new Journalist()),
+ options_(new OptionsList()),
+ statistics_(NULL),
+ alg_(NULL),
+ nlp_adapter_(NULL)
+ {
+ try {
+# ifdef IP_DEBUG
+ DebugJournalistWrapper::SetJournalist(GetRawPtr(jnlst_));
+ SmartPtr<Journal> debug_jrnl = jnlst_->AddFileJournal("Debug", "debug.out", J_ITERSUMMARY);
+ debug_jrnl->SetPrintLevel(J_DBG, J_ALL);
+# endif
+
+ DBG_START_METH("IpoptApplication::IpoptApplication()",
+ dbg_verbosity);
+
+ if (create_console_out) {
+ SmartPtr<Journal> stdout_jrnl =
+ jnlst_->AddFileJournal("console", "stdout", J_ITERSUMMARY);
+ stdout_jrnl->SetPrintLevel(J_DBG, J_NONE);
+ }
+
+ // Register the valid options
+ reg_options_ = new RegisteredOptions();
+ RegisterAllOptions(reg_options_);
+
+ options_->SetJournalist(jnlst_);
+ options_->SetRegisteredOptions(reg_options_);
+ }
+ catch(IpoptException& exc) {
+ exc.ReportException(*jnlst_);
+ THROW_EXCEPTION(IPOPT_APPLICATION_ERROR,
+ "Caught unknown Ipopt exception");
+ }
+ catch(std::bad_alloc) {
+ jnlst_->Printf(J_ERROR, J_MAIN, "\nEXIT: Not enough memory.\n");
+ THROW_EXCEPTION(IPOPT_APPLICATION_ERROR,
+ "Not enough memory");
+ }
+ catch(...) {
+ IpoptException exc("Unknown Exception caught in ipopt", "Unknown File", -1);
+ exc.ReportException(*jnlst_);
+ THROW_EXCEPTION(IPOPT_APPLICATION_ERROR,
+ "Caught unknown exception");
+ }
+ }
+
+ void IpoptApplication::Initialize(std::string params_file /*= "ipopt.opt"*/)
+ {
+ std::ifstream is;
+ if (params_file != "") {
+ try {
+ is.open(params_file.c_str());
+ }
+ catch(std::bad_alloc) {
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: Not enough memory.\n");
+ exit(-1);
+ }
+ catch(...) {
+ IpoptException exc("Unknown Exception caught in ipopt", "Unknown File", -1);
+ exc.ReportException(*jnlst_);
+ exit(-1);
+ }
+ }
+ Initialize(is);
+ if (is) {
+ is.close();
+ }
+ }
+
+ void IpoptApplication::Initialize(std::istream& is)
+ {
+ try {
+ // Get the options
+ if (is.good()) {
+ // stream exists, read the content
+ options_->ReadFromStream(*jnlst_, is);
+ }
+
+ Index ivalue;
+ options_->GetIntegerValue("print_level", ivalue, "");
+ EJournalLevel print_level = (EJournalLevel)ivalue;
+ SmartPtr<Journal> stdout_jrnl = jnlst_->GetJournal("console");
+ if (IsValid(stdout_jrnl)) {
+ // Set printlevel for stdout
+ stdout_jrnl->SetAllPrintLevels(print_level);
+ stdout_jrnl->SetPrintLevel(J_DBG, J_NONE);
+ }
+
+ bool option_set;
+
+#ifdef IP_DEBUG
+ // Set printlevel for debug
+ option_set = options_->GetIntegerValue("debug_print_level",
+ ivalue, "");
+ EJournalLevel debug_print_level;
+ if (option_set) {
+ debug_print_level = (EJournalLevel)ivalue;
+ }
+ else {
+ debug_print_level = print_level;
+ }
+ SmartPtr<Journal> debug_jrnl = jnlst_->GetJournal("Debug");
+ debug_jrnl->SetAllPrintLevels(debug_print_level);
+ debug_jrnl->SetPrintLevel(J_DBG, J_ALL);
+#endif
+
+ // Open an output file if required
+ std::string output_filename;
+ options_->GetStringValue("output_file", output_filename, "");
+ if (output_filename != "") {
+ EJournalLevel file_print_level;
+ option_set = options_->GetIntegerValue("file_print_level", ivalue, "");
+ if (option_set) {
+ file_print_level = (EJournalLevel)ivalue;
+ }
+ else {
+ file_print_level = print_level;
+ }
+ OpenOutputFile(output_filename, file_print_level);
+ }
+
+ // output a description of all the options
+ bool print_options_documentation;
+ options_->GetBoolValue("print_options_documentation",
+ print_options_documentation, "");
+ if (print_options_documentation) {
+ bool latex;
+ options_->GetBoolValue("print_options_latex_mode", latex, "");
+ if (latex) {
+ std::list<std::string> options_to_print;
+ // Output
+ options_to_print.push_back("print_level");
+ options_to_print.push_back("print_user_options");
+ options_to_print.push_back("print_options_documentation");
+ options_to_print.push_back("output_file");
+ options_to_print.push_back("file_print_level");
+ // Termination
+ options_to_print.push_back("tol");
+ options_to_print.push_back("max_iter");
+ options_to_print.push_back("compl_inf_tol");
+ options_to_print.push_back("dual_inf_tol");
+ options_to_print.push_back("constr_viol_tol");
+ options_to_print.push_back("acceptable_tol");
+ options_to_print.push_back("acceptable_compl_inf_tol");
+ options_to_print.push_back("acceptable_constr_viol_tol");
+ options_to_print.push_back("acceptable_dual_inf_tol");
+ options_to_print.push_back("diverging_iterates_tol");
+ options_to_print.push_back("barrier_tol_factor");
+ // NLP scaling
+ options_to_print.push_back("obj_scaling_factor");
+ options_to_print.push_back("nlp_scaling_method");
+ options_to_print.push_back("nlp_scaling_max_gradient");
+ // NLP corrections
+ options_to_print.push_back("bound_relax_factor");
+ options_to_print.push_back("honor_original_bounds");
+ options_to_print.push_back("check_derivatives_for_naninf");
+ // Barrier parameter
+ options_to_print.push_back("mu_strategy");
+ options_to_print.push_back("mu_oracle");
+ options_to_print.push_back("quality_function_max_section_steps");
+ options_to_print.push_back("fixed_mu_oracle");
+ options_to_print.push_back("mu_init");
+ options_to_print.push_back("mu_max_fact");
+ options_to_print.push_back("mu_max");
+ options_to_print.push_back("mu_min");
+ options_to_print.push_back("mu_linear_decrease_factor");
+ options_to_print.push_back("mu_superlinear_decrease_power");
+ //options_to_print.push_back("corrector_type");
+ // Initialization
+ options_to_print.push_back("bound_frac");
+ options_to_print.push_back("bound_push");
+ options_to_print.push_back("bound_mult_init_val");
+ options_to_print.push_back("constr_mult_init_max");
+ options_to_print.push_back("bound_mult_init_val");
+ // Warm start
+ options_to_print.push_back("warm_start_init_point");
+ options_to_print.push_back("warm_start_bound_push");
+ options_to_print.push_back("warm_start_bound_frac");
+ options_to_print.push_back("warm_start_mult_bound_push");
+ options_to_print.push_back("warm_start_mult_init_max");
+ // Multiplier updates
+ options_to_print.push_back("alpha_for_y");
+ options_to_print.push_back("recalc_y");
+ options_to_print.push_back("recalc_y_feas_tol");
+ // Line search
+ options_to_print.push_back("max_soc");
+ options_to_print.push_back("watchdog_shortened_iter_trigger");
+ options_to_print.push_back("watchdog_trial_iter_max");
+ // Restoration phase
+ options_to_print.push_back("expect_infeasible_problem");
+ options_to_print.push_back("expect_infeasible_problem_ctol");
+ options_to_print.push_back("start_with_resto");
+ options_to_print.push_back("soft_resto_pderror_reduction_factor");
+ options_to_print.push_back("required_infeasibility_reduction");
+ options_to_print.push_back("bound_mult_reset_threshold");
+ options_to_print.push_back("constr_mult_reset_threshold");
+ options_to_print.push_back("evaluate_orig_obj_at_resto_trial");
+ // Linear solver
+ options_to_print.push_back("linear_solver");
+ options_to_print.push_back("linear_system_scaling");
+ options_to_print.push_back("linear_scaling_on_demand");
+ options_to_print.push_back("max_refinement_steps");
+ options_to_print.push_back("min_refinement_steps");
+ // Hessian perturbation
+ options_to_print.push_back("max_hessian_perturbation");
+ options_to_print.push_back("min_hessian_perturbation");
+ options_to_print.push_back("first_hessian_perturbation");
+ options_to_print.push_back("perturb_inc_fact_first");
+ options_to_print.push_back("perturb_inc_fact");
+ options_to_print.push_back("perturb_dec_fact");
+ options_to_print.push_back("jacobian_regularization_value");
+ // Quasi-Newton
+ options_to_print.push_back("hessian_approximation");
+ options_to_print.push_back("limited_memory_max_history");
+ options_to_print.push_back("limited_memory_max_skipping");
+ // Derivative test
+ options_to_print.push_back("derivative_test");
+ options_to_print.push_back("derivative_test_perturbation");
+ options_to_print.push_back("derivative_test_tol");
+ options_to_print.push_back("derivative_test_print_all");
+
+ // Special linear solver
+#ifdef HAVE_MA27
+
+ options_to_print.push_back("ma27_pivtol");
+ options_to_print.push_back("ma27_pivtolmax");
+ options_to_print.push_back("ma27_liw_init_factor");
+ options_to_print.push_back("ma27_la_init_factor");
+ options_to_print.push_back("ma27_meminc_factor");
+#endif
+
+#ifdef HAVE_MA57
+
+ options_to_print.push_back("ma57_pivtol");
+ options_to_print.push_back("ma57_pivtolmax");
+ options_to_print.push_back("ma57_pre_alloc");
+#endif
+
+#ifdef HAVE_PARDISO
+
+ options_to_print.push_back("pardiso_matching_strategy");
+ options_to_print.push_back("pardiso_out_of_core_power");
+#endif
+
+#ifdef HAVE_WSMP
+
+ options_to_print.push_back("wsmp_num_threads");
+ options_to_print.push_back("wsmp_ordering_option");
+ options_to_print.push_back("wsmp_pivtol");
+ options_to_print.push_back("wsmp_pivtolmax");
+ options_to_print.push_back("wsmp_scaling");
+#endif
+
+ reg_options_->OutputLatexOptionDocumentation(*jnlst_, options_to_print);
+ }
+ else {
+ std::list<std::string> categories;
+ categories.push_back("Output");
+ /*categories.push_back("Main Algorithm");*/
+ categories.push_back("Convergence");
+ categories.push_back("NLP Scaling");
+ categories.push_back("Barrier Parameter Update");
+ categories.push_back("Line Search");
+ categories.push_back("Initialization");
+ categories.push_back("Warm Start");
+ categories.push_back("Linear Solver");
+ categories.push_back("Step Calculation");
+ categories.push_back("Restoration Phase");
+ categories.push_back("NLP");
+ categories.push_back("Derivative Checker");
+ categories.push_back("Hessian Approximation");
+#ifdef HAVE_MA27
+
+ categories.push_back("MA27 Linear Solver");
+#endif
+#ifdef HAVE_MA57
+
+ categories.push_back("MA57 Linear Solver");
+#endif
+#ifdef HAVE_PARDISO
+
+ categories.push_back("Pardiso Linear Solver");
+#endif
+#ifdef HAVE_WSMP
+
+ categories.push_back("WSMP Linear Solver");
+#endif
+
+ categories.push_back("Uncategorized");
+ //categories.push_back("Undocumented Options");
+ reg_options_->OutputOptionDocumentation(*jnlst_, categories);
+ }
+ }
+
+ }
+ catch(OPTION_INVALID& exc) {
+ exc.ReportException(*jnlst_, J_ERROR);
+ exit(-1);
+ }
+ catch(IpoptException& exc) {
+ exc.ReportException(*jnlst_, J_ERROR);
+ exit(-1);
+ }
+ catch(std::bad_alloc) {
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: Not enough memory.\n");
+ exit(-1);
+ }
+ }
+
+ IpoptApplication::~IpoptApplication()
+ {
+ DBG_START_METH("IpoptApplication::~IpoptApplication()",
+ dbg_verbosity);
+ }
+
+ void IpoptApplication::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->SetRegisteringCategory("Output");
+ roptions->AddBoundedIntegerOption(
+ "print_level",
+ "Output verbosity level.",
+ 0, J_LAST_LEVEL-1, J_ITERSUMMARY,
+ "Sets the default verbosity level for console output. The "
+ "larger this value the more detailed is the output.");
+
+ roptions->AddStringOption1(
+ "output_file",
+ "File name of desired output file (leave unset for no file output).",
+ "",
+ "*", "Any acceptable standard file name",
+ "NOTE: This option only works when read from the ipopt.opt options file! "
+ "An output file with this name will be written (leave unset for no "
+ "file output). The verbosity level is by default set to \"print_level\", "
+ "but can be overridden with \"file_print_level\". The file name is "
+ "changed to use only small letters.");
+ roptions->AddBoundedIntegerOption(
+ "file_print_level",
+ "Verbosity level for output file.",
+ 0, J_LAST_LEVEL-1, J_ITERSUMMARY,
+ "NOTE: This option only works when read from the ipopt.opt options file! "
+ "Determines the verbosity level for the file specified by "
+ "\"output_file\". By default it is the same as \"print_level\".");
+ roptions->AddStringOption2(
+ "print_user_options",
+ "Print all options set by the user.",
+ "no",
+ "no", "don't print options",
+ "yes", "print options",
+ "If selected, the algorithm will print the list of all options set by "
+ "the user including their values and whether they have been used.");
+ roptions->AddStringOption2(
+ "print_options_documentation",
+ "Switch to print all algorithmic options.",
+ "no",
+ "no", "don't print list",
+ "yes", "print list",
+ "If selected, the algorithm will print the list of all available "
+ "algorithmic options with some documentation before solving the "
+ "optimization problem.");
+
+#ifdef IP_DEBUG
+
+ roptions->AddBoundedIntegerOption(
+ "debug_print_level",
+ "Verbosity level for debug file.",
+ 0, J_LAST_LEVEL-1, J_ITERSUMMARY,
+ "This Ipopt library has been compiled in debug mode, and a file "
+ "\"debug.out\" is produced for every run. This option determines "
+ "the verbosity level for this file. By default it is the same as "
+ "\"print_level\".");
+
+#endif
+
+ roptions->AddStringOption2(
+ "print_timing_statistics",
+ "Switch to print timing statistics.",
+ "no",
+ "no", "don't print statistics",
+ "yes", "print all timing statistics",
+ "If selected, the program will print the CPU usage (user time) for "
+ "selected tasks.");
+
+ roptions->SetRegisteringCategory("Undocumented");
+ roptions->AddStringOption2(
+ "print_options_latex_mode",
+ "Undocumented", "no",
+ "no", "Undocumented",
+ "yes", "Undocumented",
+ "Undocumented"
+ );
+ }
+
+ ApplicationReturnStatus
+ IpoptApplication::OptimizeTNLP(const SmartPtr<TNLP>& tnlp)
+ {
+ nlp_adapter_ = new TNLPAdapter(GetRawPtr(tnlp), ConstPtr(jnlst_));
+
+ return OptimizeNLP(nlp_adapter_);
+ }
+
+ ApplicationReturnStatus
+ IpoptApplication::ReOptimizeTNLP(const SmartPtr<TNLP>& tnlp)
+ {
+ ASSERT_EXCEPTION(IsValid(nlp_adapter_), INVALID_WARMSTART,
+ "ReOptimizeTNLP called before OptimizeTNLP.");
+ TNLPAdapter* adapter =
+ dynamic_cast<TNLPAdapter*> (GetRawPtr(nlp_adapter_));
+ DBG_ASSERT(adapter);
+ ASSERT_EXCEPTION(adapter->tnlp()==tnlp, INVALID_WARMSTART,
+ "ReOptimizeTNLP called for different TNLP.")
+
+ return ReOptimizeNLP(nlp_adapter_);
+ }
+
+ ApplicationReturnStatus
+ IpoptApplication::OptimizeNLP(const SmartPtr<NLP>& nlp, SmartPtr<AlgorithmBuilder> alg_builder)
+ {
+ ApplicationReturnStatus retValue = Internal_Error;
+
+ // Prepare internal data structures of the algorithm
+ try {
+
+ if (IsNull(alg_builder)) {
+ alg_builder = new AlgorithmBuilder();
+ }
+
+ alg_builder->BuildIpoptObjects(*jnlst_, *options_, "", nlp,
+ ip_nlp_, ip_data_, ip_cq_);
+
+ alg_ = GetRawPtr(alg_builder->BuildBasicAlgorithm(*jnlst_, *options_, ""));
+
+ // finally call the optimization
+ retValue = call_optimize();
+ }
+ catch(OPTION_INVALID& exc) {
+ exc.ReportException(*jnlst_, J_ERROR);
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: Invalid option encountered.\n");
+ retValue = Invalid_Option;
+ }
+ catch(IpoptException& exc) {
+ exc.ReportException(*jnlst_, J_ERROR);
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: Some uncaught Ipopt exception encountered.\n");
+ retValue = Unrecoverable_Exception;
+ }
+ catch(std::bad_alloc) {
+ retValue = Insufficient_Memory;
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: Not enough memory.\n");
+ }
+
+ jnlst_->FlushBuffer();
+
+ return retValue;
+ }
+
+ ApplicationReturnStatus
+ IpoptApplication::ReOptimizeNLP(const SmartPtr<NLP>& nlp)
+ {
+ ASSERT_EXCEPTION(IsValid(alg_), INVALID_WARMSTART,
+ "ReOptimizeNLP called before OptimizeNLP.");
+ OrigIpoptNLP* orig_nlp =
+ dynamic_cast<OrigIpoptNLP*> (GetRawPtr(ip_nlp_));
+ DBG_ASSERT(orig_nlp);
+ ASSERT_EXCEPTION(orig_nlp->nlp()==nlp, INVALID_WARMSTART,
+ "ReOptimizeTNLP called for different NLP.")
+
+ return call_optimize();
+ }
+
+
+ ApplicationReturnStatus IpoptApplication::call_optimize()
+ {
+ // Reset the print-level for the screen output
+ Index ivalue;
+ options_->GetIntegerValue("print_level", ivalue, "");
+ EJournalLevel print_level = (EJournalLevel)ivalue;
+ SmartPtr<Journal> stdout_jrnl = jnlst_->GetJournal("console");
+ if (IsValid(stdout_jrnl)) {
+ // Set printlevel for stdout
+ stdout_jrnl->SetAllPrintLevels(print_level);
+ stdout_jrnl->SetPrintLevel(J_DBG, J_NONE);
+ }
+
+ statistics_ = NULL; /* delete old statistics */
+ // Reset Timing statistics
+ ip_data_->TimingStats().ResetTimes();
+
+ ApplicationReturnStatus retValue = Internal_Error;
+ try {
+ // Get the pointers to the real objects (need to do it that
+ // awkwardly since otherwise we would have to include so many
+ // things in IpoptApplication, which a user would have to
+ // include, too (SmartPtr doesn't work otherwise on AIX)
+ IpoptAlgorithm* p2alg = dynamic_cast<IpoptAlgorithm*> (GetRawPtr(alg_));
+ IpoptData* p2ip_data = dynamic_cast<IpoptData*> (GetRawPtr(ip_data_));
+ OrigIpoptNLP* p2ip_nlp = dynamic_cast<OrigIpoptNLP*> (GetRawPtr(ip_nlp_));
+ IpoptCalculatedQuantities* p2ip_cq = dynamic_cast<IpoptCalculatedQuantities*> (GetRawPtr(ip_cq_));
+ // Set up the algorithm
+ p2alg->Initialize(*jnlst_, *p2ip_nlp, *p2ip_data, *p2ip_cq,
+ *options_, "");
+
+ // Process the options used below
+ bool print_timing_statistics;
+ options_->GetBoolValue("print_timing_statistics",
+ print_timing_statistics, "");
+
+ // If selected, print the user options
+ bool print_user_options;
+ options_->GetBoolValue("print_user_options", print_user_options, "");
+ if (print_user_options) {
+ std::string liststr;
+ options_->PrintUserOptions(liststr);
+ jnlst_->Printf(J_ERROR, J_MAIN, "\nList of user-set options:\n\n%s", liststr.c_str());
+ }
+
+ if( jnlst_->ProduceOutput(J_DETAILED, J_MAIN) ) {
+ // Print out the options (including the number of times they were used
+ std::string liststr;
+ options_->PrintList(liststr);
+// TODO UNCOMMENT WHEN DONE TESTING
+/*
+ vatest( stdout, "\nList of user-set options:\n\n%s", liststr.c_str());
+ void vatest( FILE *, const char *, ... );
+
+printf("strlen user-set options= %d\n",strlen(liststr.c_str()) );
+ printf("\nList of user-set options:\n\n%s", liststr.c_str());
+*/
+ jnlst_->Printf(J_DETAILED, J_MAIN, "\nList of options:\n\n%s", liststr.c_str());
+ }
+
+
+ // Run the algorithm
+ SolverReturn status = p2alg->Optimize();
+
+ // Since all the output below doesn't make any sense in this
+ // case, we rethrow the TOO_FEW_DOF exception here
+ ASSERT_EXCEPTION(status != TOO_FEW_DEGREES_OF_FREEDOM, TOO_FEW_DOF,
+ "Too few degrees of freedom (rethrown)!");
+
+ jnlst_->Printf(J_SUMMARY, J_SOLUTION,
+ "\nNumber of Iterations....: %d\n",
+ p2ip_data->iter_count());
+
+ if (status!=INVALID_NUMBER_DETECTED) {
+ jnlst_->Printf(J_SUMMARY, J_SOLUTION,
+ "\n (scaled) (unscaled)\n");
+ jnlst_->Printf(J_SUMMARY, J_SOLUTION,
+ "Objective...............: %24.16e %24.16e\n",
+ p2ip_cq->curr_f(),
+ p2ip_cq->unscaled_curr_f());
+ jnlst_->Printf(J_SUMMARY, J_SOLUTION,
+ "Dual infeasibility......: %24.16e %24.16e\n",
+ p2ip_cq->curr_dual_infeasibility(NORM_MAX),
+ p2ip_cq->unscaled_curr_dual_infeasibility(NORM_MAX));
+ jnlst_->Printf(J_SUMMARY, J_SOLUTION,
+ "Constraint violation....: %24.16e %24.16e\n",
+ p2ip_cq->curr_nlp_constraint_violation(NORM_MAX),
+ p2ip_cq->unscaled_curr_nlp_constraint_violation(NORM_MAX));
+ jnlst_->Printf(J_SUMMARY, J_SOLUTION,
+ "Complementarity.........: %24.16e %24.16e\n",
+ p2ip_cq->curr_complementarity(0., NORM_MAX),
+ p2ip_cq->unscaled_curr_complementarity(0., NORM_MAX));
+ jnlst_->Printf(J_SUMMARY, J_SOLUTION,
+ "Overall NLP error.......: %24.16e %24.16e\n\n",
+ p2ip_cq->curr_nlp_error(),
+ p2ip_cq->unscaled_curr_nlp_error());
+ }
+
+ p2ip_data->curr()->x()->Print(*jnlst_, J_VECTOR, J_SOLUTION, "x");
+ p2ip_data->curr()->y_c()->Print(*jnlst_, J_VECTOR, J_SOLUTION, "y_c");
+ p2ip_data->curr()->y_d()->Print(*jnlst_, J_VECTOR, J_SOLUTION, "y_d");
+ p2ip_data->curr()->z_L()->Print(*jnlst_, J_VECTOR, J_SOLUTION, "z_L");
+ p2ip_data->curr()->z_U()->Print(*jnlst_, J_VECTOR, J_SOLUTION, "z_U");
+ p2ip_data->curr()->v_L()->Print(*jnlst_, J_VECTOR, J_SOLUTION, "v_L");
+ p2ip_data->curr()->v_U()->Print(*jnlst_, J_VECTOR, J_SOLUTION, "v_U");
+
+ if (status==LOCAL_INFEASIBILITY) {
+ p2ip_cq->curr_c()->Print(*jnlst_, J_VECTOR, J_SOLUTION, "curr_c");
+ p2ip_cq->curr_d_minus_s()->Print(*jnlst_, J_VECTOR, J_SOLUTION,
+ "curr_d_minus_s");
+ }
+
+ jnlst_->Printf(J_SUMMARY, J_STATISTICS,
+ "\nNumber of objective function evaluations = %d\n",
+ p2ip_nlp->f_evals());
+ jnlst_->Printf(J_SUMMARY, J_STATISTICS,
+ "Number of objective gradient evaluations = %d\n",
+ p2ip_nlp->grad_f_evals());
+ jnlst_->Printf(J_SUMMARY, J_STATISTICS,
+ "Number of equality constraint evaluations = %d\n",
+ p2ip_nlp->c_evals());
+ jnlst_->Printf(J_SUMMARY, J_STATISTICS,
+ "Number of inequality constraint evaluations = %d\n",
+ p2ip_nlp->d_evals());
+ jnlst_->Printf(J_SUMMARY, J_STATISTICS,
+ "Number of equality constraint Jacobian evaluations = %d\n",
+ p2ip_nlp->jac_c_evals());
+ jnlst_->Printf(J_SUMMARY, J_STATISTICS,
+ "Number of inequality constraint Jacobian evaluations = %d\n",
+ p2ip_nlp->jac_d_evals());
+ jnlst_->Printf(J_SUMMARY, J_STATISTICS,
+ "Number of Lagrangian Hessian evaluations = %d\n",
+ p2ip_nlp->h_evals());
+ Number time_overall_alg = p2ip_data->TimingStats().OverallAlgorithm().TotalTime();
+ Number time_funcs = p2ip_nlp->TotalFunctionEvaluationCPUTime();
+ jnlst_->Printf(J_SUMMARY, J_STATISTICS,
+ "Total CPU secs in IPOPT (w/o function evaluations) = %10.3f\n",
+ time_overall_alg-time_funcs);
+ jnlst_->Printf(J_SUMMARY, J_STATISTICS,
+ "Total CPU secs in NLP function evaluations = %10.3f\n",
+ time_funcs);
+
+ // Write timing statistics information
+ if (print_timing_statistics) {
+ jnlst_->Printf(J_SUMMARY, J_TIMING_STATISTICS,
+ "\n\nTiming Statistics:\n\n");
+ p2ip_data->TimingStats().PrintAllTimingStatistics(*jnlst_, J_SUMMARY,
+ J_TIMING_STATISTICS);
+ p2ip_nlp->PrintTimingStatistics(*jnlst_, J_SUMMARY,
+ J_TIMING_STATISTICS);
+ }
+
+ // Write EXIT message
+ if (status == SUCCESS) {
+ retValue = Solve_Succeeded;
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: Optimal Solution Found.\n");
+ }
+ else if (status == MAXITER_EXCEEDED) {
+ retValue = Maximum_Iterations_Exceeded;
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: Maximum Number of Iterations Exceeded.\n");
+ }
+ else if (status == STOP_AT_TINY_STEP) {
+ retValue = Search_Direction_Becomes_Too_Small;
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: Search Direction is becoming Too Small.\n");
+ }
+ else if (status == STOP_AT_ACCEPTABLE_POINT) {
+ retValue = Solved_To_Acceptable_Level;
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: Solved To Acceptable Level.\n");
+ }
+ else if (status == DIVERGING_ITERATES) {
+ retValue = Diverging_Iterates;
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: Iterates divering; problem might be unbounded.\n");
+ }
+ else if (status == RESTORATION_FAILURE) {
+ retValue = Restoration_Failed;
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: Restoration Failed!\n");
+ }
+ else if (status == ERROR_IN_STEP_COMPUTATION) {
+ retValue = Error_In_Step_Computation;
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: Error in step computation (regularization becomes too large?)!\n");
+ }
+ else if (status == LOCAL_INFEASIBILITY) {
+ retValue = Infeasible_Problem_Detected;
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: Converged to a point of local infeasibility. Problem may be infeasible.\n");
+ }
+ else if (status == USER_REQUESTED_STOP) {
+ retValue = User_Requested_Stop;
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: Stopping optimization at current point as requested by user.\n");
+ }
+ else if (status == INVALID_NUMBER_DETECTED) {
+ retValue = Invalid_Number_Detected;
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: Invalid number in NLP function or derivative detected.\n");
+ }
+ else {
+ retValue = Internal_Error;
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: INTERNAL ERROR: Unknown SolverReturn value - Notify IPOPT Authors.\n");
+ return retValue;
+ }
+ p2ip_nlp->FinalizeSolution(status,
+ *p2ip_data->curr()->x(),
+ *p2ip_data->curr()->z_L(),
+ *p2ip_data->curr()->z_U(),
+ *p2ip_cq->curr_c(), *p2ip_cq->curr_d(),
+ *p2ip_data->curr()->y_c(),
+ *p2ip_data->curr()->y_d(),
+ p2ip_cq->curr_f());
+
+ if (status!=INVALID_NUMBER_DETECTED) {
+ // Create a SolveStatistics object
+ statistics_ = new SolveStatistics(p2ip_nlp, p2ip_data, p2ip_cq);
+ }
+ }
+ catch(TOO_FEW_DOF& exc) {
+ exc.ReportException(*jnlst_, J_MOREDETAILED);
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: Problem has too few degrees of freedom.\n");
+ retValue = Not_Enough_Degrees_Of_Freedom;
+ }
+ catch(OPTION_INVALID& exc) {
+ exc.ReportException(*jnlst_, J_MOREDETAILED);
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: Invalid option encountered.\n");
+ retValue = Invalid_Option;
+ }
+ catch(IpoptException& exc) {
+ exc.ReportException(*jnlst_, J_ERROR);
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: Some uncaught Ipopt exception encountered.\n");
+ retValue = Unrecoverable_Exception;
+ }
+ catch(std::bad_alloc) {
+ retValue = Insufficient_Memory;
+ jnlst_->Printf(J_SUMMARY, J_MAIN, "\nEXIT: Not enough memory.\n");
+ }
+
+ jnlst_->FlushBuffer();
+
+ return retValue;
+ }
+static void vatest( FILE *fp, const char *format, ... ) {
+ va_list ap;
+ va_start( ap, format );
+ vfprintf( fp, format, ap );
+ va_end(ap);
+}
+
+ bool IpoptApplication::OpenOutputFile(std::string file_name,
+ EJournalLevel print_level)
+ {
+ SmartPtr<Journal> file_jrnl = jnlst_->GetJournal("OutputFile:"+file_name);
+
+ if (IsNull(file_jrnl)) {
+ file_jrnl = jnlst_->AddFileJournal("OutputFile:"+file_name,
+ file_name.c_str(),
+ print_level);
+ }
+
+ file_jrnl->SetPrintLevel(J_DBG, J_NONE);
+
+ return true;
+ }
+
+ void IpoptApplication::RegisterAllOptions(const SmartPtr<RegisteredOptions>& roptions)
+ {
+ RegisterOptions_Interfaces(roptions);
+ RegisterOptions_Algorithm(roptions);
+ RegisterOptions_LinearSolvers(roptions);
+ }
+
+ SmartPtr<SolveStatistics> IpoptApplication::Statistics()
+ {
+ return statistics_;
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpIpoptApplication.hpp b/SimTKmath/Optimizers/src/IpOpt/IpIpoptApplication.hpp
new file mode 100644
index 0000000..536eaee
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpIpoptApplication.hpp
@@ -0,0 +1,184 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpIpoptApplication.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPIPOPTAPPLICATION_HPP__
+#define __IPIPOPTAPPLICATION_HPP__
+
+#include <iostream>
+
+#include "IpJournalist.hpp"
+#include "IpTNLP.hpp"
+#include "IpNLP.hpp"
+/* Return codes for the Optimize call for an application */
+#include "IpReturnCodes.hpp"
+
+namespace Ipopt
+{
+ DECLARE_STD_EXCEPTION(IPOPT_APPLICATION_ERROR);
+
+ /* forward declarations */
+ class IpoptAlgorithm;
+ class IpoptNLP;
+ class IpoptData;
+ class IpoptCalculatedQuantities;
+ class AlgorithmBuilder;
+ class RegisteredOptions;
+ class OptionsList;
+ class SolveStatistics;
+
+ /** This is the main application class for making calls to Ipopt. */
+ class IpoptApplication : public ReferencedObject
+ {
+ public:
+ IpoptApplication(bool create_console_out = true);
+
+ virtual ~IpoptApplication();
+
+ /** Initialize method. This method reads the params file and initializes
+ * the journalists. You should call this method at some point before the
+ * first optimize call. Note: you can skip the processing of a params
+ * file by setting params_file to ""
+ */
+ void Initialize(std::string params_file = "ipopt.opt");
+ void Initialize(std::istream& is);
+
+ /**@name Solve methods */
+ //@{
+ /** Solve a problem that inherits from TNLP */
+ ApplicationReturnStatus OptimizeTNLP(const SmartPtr<TNLP>& tnlp);
+
+ /** Solve a problem that inherits from NLP */
+ ApplicationReturnStatus OptimizeNLP(const SmartPtr<NLP>& nlp, SmartPtr<AlgorithmBuilder> alg_builder=NULL);
+
+ /** Solve a problem (that inherits from TNLP) for a repeated time.
+ * The OptimizeTNLP method must have been called before. The
+ * TNLP must be the same object, and the structure (number of
+ * variables and constraints and position of nonzeros in Jacobian
+ * and Hessian must be the same). */
+ ApplicationReturnStatus ReOptimizeTNLP(const SmartPtr<TNLP>& tnlp);
+
+ /** Solve a problem (that inherits from NLP) for a repeated time.
+ * The OptimizeNLP method must have been called before. The
+ * NLP must be the same object, and the structure (number of
+ * variables and constraints and position of nonzeros in Jacobian
+ * and Hessian must be the same). */
+ ApplicationReturnStatus ReOptimizeNLP(const SmartPtr<NLP>& nlp);
+ //@}
+
+ /** Method for opening an output file with given print_level.
+ * Returns false if there was a problem. */
+ bool OpenOutputFile(std::string file_name, EJournalLevel print_level);
+
+ /**@name Accessor methods */
+ //@{
+ /** Get the Journalist for printing output */
+ SmartPtr<Journalist> Jnlst()
+ {
+ return jnlst_;
+ }
+
+ /** Get a pointer to RegisteredOptions object to
+ * add new options */
+ SmartPtr<RegisteredOptions> RegOptions()
+ {
+ return reg_options_;
+ }
+
+ /** Get the options list for setting options */
+ SmartPtr<OptionsList> Options()
+ {
+ return options_;
+ }
+
+ /** Get the options list for setting options (const version) */
+ SmartPtr<const OptionsList> Options() const
+ {
+ return ConstPtr(options_);
+ }
+
+ /** Get the object with the statistics about the most recent
+ * optimization run. */
+ SmartPtr<SolveStatistics> Statistics();
+ //@}
+
+ /** @name Methods for IpoptTypeInfo */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ // IpoptApplication();
+
+ /** Copy Constructor */
+ IpoptApplication(const IpoptApplication&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const IpoptApplication&);
+ //@}
+
+ /** Method to register all the options */
+ void RegisterAllOptions(const SmartPtr<RegisteredOptions>& roptions);
+
+ /** Method for the actual optimize call of the Ipopt algorithm.
+ * This is used both for Optimize and ReOptimize */
+ ApplicationReturnStatus call_optimize();
+
+ /**@name Variables that customize the application behavior */
+ //@{
+ /** Decide whether or not the ipopt.opt file should be read */
+ bool read_params_dat_;
+ //@}
+
+ /** Journalist for reporting output */
+ SmartPtr<Journalist> jnlst_;
+
+ /** RegisteredOptions */
+ SmartPtr<RegisteredOptions> reg_options_;
+
+ /** OptionsList used for the application */
+ SmartPtr<OptionsList> options_;
+
+ /** Object for storing statistics about the most recent
+ * optimization run. */
+ SmartPtr<SolveStatistics> statistics_;
+
+ /** Object with the algorithm sceleton.
+ */
+ SmartPtr<IpoptAlgorithm> alg_;
+
+ /** IpoptNLP Object for the NLP. We keep this around for a
+ * ReOptimize warm start. */
+ SmartPtr<IpoptNLP> ip_nlp_;
+
+ /** IpoptData Object for the NLP. We keep this around for a
+ * ReOptimize warm start.
+ */
+ SmartPtr<IpoptData> ip_data_;
+
+ /** IpoptCalculatedQuantities Object for the NLP. We keep this
+ * around for a ReOptimize warm start.
+ */
+ SmartPtr<IpoptCalculatedQuantities> ip_cq_;
+
+ /** Pointer to the TNLPAdapter used to convert the TNLP to an NLP.
+ * We keep this around for the ReOptimizerTNLP call. */
+ SmartPtr<NLP> nlp_adapter_;
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpIpoptCalculatedQuantities.cpp b/SimTKmath/Optimizers/src/IpOpt/IpIpoptCalculatedQuantities.cpp
new file mode 100644
index 0000000..bd2d4a8
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpIpoptCalculatedQuantities.cpp
@@ -0,0 +1,3312 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpIpoptCalculatedQuantities.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpIpoptCalculatedQuantities.hpp"
+#include "IpSumSymMatrix.hpp"
+#include "IpLowRankUpdateSymMatrix.hpp"
+#include "IpRestoIpoptNLP.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+#include <limits>
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ IpoptCalculatedQuantities::IpoptCalculatedQuantities
+ (const SmartPtr<IpoptNLP>& ip_nlp,
+ const SmartPtr<IpoptData>& ip_data)
+ :
+ ip_nlp_(ip_nlp),
+ ip_data_(ip_data),
+
+ curr_slack_x_L_cache_(1),
+ curr_slack_x_U_cache_(1),
+ curr_slack_s_L_cache_(1),
+ curr_slack_s_U_cache_(1),
+ trial_slack_x_L_cache_(1),
+ trial_slack_x_U_cache_(1),
+ trial_slack_s_L_cache_(1),
+ trial_slack_s_U_cache_(1),
+ num_adjusted_slack_x_L_(0),
+ num_adjusted_slack_x_U_(0),
+ num_adjusted_slack_s_L_(0),
+ num_adjusted_slack_s_U_(0),
+
+ curr_f_cache_(2),
+ trial_f_cache_(5),
+ curr_grad_f_cache_(2),
+ trial_grad_f_cache_(1),
+
+ curr_barrier_obj_cache_(2),
+ trial_barrier_obj_cache_(5),
+ curr_grad_barrier_obj_x_cache_(1),
+ curr_grad_barrier_obj_s_cache_(1),
+ grad_kappa_times_damping_x_cache_(1),
+ grad_kappa_times_damping_s_cache_(1),
+
+ curr_c_cache_(1),
+ trial_c_cache_(2),
+ curr_d_cache_(1),
+ trial_d_cache_(2),
+ curr_d_minus_s_cache_(1),
+ trial_d_minus_s_cache_(1),
+ curr_jac_c_cache_(1),
+ trial_jac_c_cache_(1),
+ curr_jac_d_cache_(1),
+ trial_jac_d_cache_(1),
+ curr_jac_cT_times_vec_cache_(2),
+ trial_jac_cT_times_vec_cache_(1),
+ curr_jac_dT_times_vec_cache_(2),
+ trial_jac_dT_times_vec_cache_(1),
+ curr_jac_c_times_vec_cache_(1),
+ curr_jac_d_times_vec_cache_(1),
+ curr_constraint_violation_cache_(2),
+ trial_constraint_violation_cache_(5),
+ curr_nlp_constraint_violation_cache_(3),
+ unscaled_curr_nlp_constraint_violation_cache_(3),
+
+ curr_exact_hessian_cache_(1),
+
+ curr_grad_lag_x_cache_(1),
+ trial_grad_lag_x_cache_(1),
+ curr_grad_lag_s_cache_(1),
+ trial_grad_lag_s_cache_(1),
+ curr_grad_lag_with_damping_x_cache_(0),
+ curr_grad_lag_with_damping_s_cache_(0),
+ curr_compl_x_L_cache_(1),
+ curr_compl_x_U_cache_(1),
+ curr_compl_s_L_cache_(1),
+ curr_compl_s_U_cache_(1),
+ trial_compl_x_L_cache_(1),
+ trial_compl_x_U_cache_(1),
+ trial_compl_s_L_cache_(1),
+ trial_compl_s_U_cache_(1),
+ curr_relaxed_compl_x_L_cache_(1),
+ curr_relaxed_compl_x_U_cache_(1),
+ curr_relaxed_compl_s_L_cache_(1),
+ curr_relaxed_compl_s_U_cache_(1),
+ curr_primal_infeasibility_cache_(3),
+ trial_primal_infeasibility_cache_(3),
+ curr_dual_infeasibility_cache_(3),
+ trial_dual_infeasibility_cache_(3),
+ unscaled_curr_dual_infeasibility_cache_(3),
+ curr_complementarity_cache_(6),
+ trial_complementarity_cache_(6),
+ curr_centrality_measure_cache_(1),
+ curr_nlp_error_cache_(1),
+ unscaled_curr_nlp_error_cache_(1),
+ curr_barrier_error_cache_(1),
+ curr_primal_dual_system_error_cache_(1),
+ trial_primal_dual_system_error_cache_(3),
+
+ primal_frac_to_the_bound_cache_(5),
+ dual_frac_to_the_bound_cache_(5),
+
+ curr_sigma_x_cache_(1),
+ curr_sigma_s_cache_(1),
+
+ curr_avrg_compl_cache_(1),
+ trial_avrg_compl_cache_(1),
+ curr_gradBarrTDelta_cache_(1),
+
+ dampind_x_L_(NULL),
+ dampind_x_U_(NULL),
+ dampind_s_L_(NULL),
+ dampind_s_U_(NULL),
+
+ initialize_called_(false)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::IpoptCalculatedQuantities",
+ dbg_verbosity);
+ DBG_ASSERT(IsValid(ip_nlp_) && IsValid(ip_data_));
+ }
+
+ IpoptCalculatedQuantities::~IpoptCalculatedQuantities()
+ {}
+
+ void IpoptCalculatedQuantities::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->SetRegisteringCategory("Convergence");
+ roptions->AddLowerBoundedNumberOption(
+ "s_max",
+ "Scaling threshold for the NLP error.",
+ 0.0, true, 100.0,
+ "(See paragraph after Eqn. (6) in the implementation paper.)");
+
+ roptions->SetRegisteringCategory("NLP");
+ roptions->AddLowerBoundedNumberOption(
+ "kappa_d",
+ "Weight for linear damping term (to handle one-sided bounds).",
+ 0.0, false, 1e-5,
+ "(see Section 3.7 in implementation paper.)");
+
+ roptions->SetRegisteringCategory("Line Search");
+ roptions->AddLowerBoundedNumberOption(
+ "slack_move",
+ "Correction size for very small slacks.",
+ 0.0, false,
+ pow(std::numeric_limits<double>::epsilon(), 0.75),
+ "Due to numerical issues or the lack of an interior, the slack variables might "
+ "become very small. If a slack becomes very small compared to machine "
+ "precision, the corresponding bound is moved slightly. This parameter "
+ "determines how large the move should be. Its default value is "
+ "mach_eps^{3/4}. (See also end of Section 3.5 in implementation paper "
+ "- but actual implementation might be somewhat different.)");
+ roptions->SetRegisteringCategory("Line search");
+ roptions->AddStringOption3(
+ "constraint_violation_norm_type",
+ "Norm to be used for the constraint violation in the line search.",
+ "1-norm",
+ "1-norm", "use the 1-norm",
+ "2-norm", "use the 2-norm",
+ "max-norm", "use the infinity norm",
+ "Determines which norm should be used when the algorithm computes the "
+ "constraint violation in the line search.");
+ }
+
+ bool IpoptCalculatedQuantities::Initialize(const Journalist& jnlst,
+ const OptionsList& options,
+ const std::string& prefix)
+ {
+ std::string svalue;
+ Index enum_int;
+
+ options.GetNumericValue("s_max", s_max_, prefix);
+ options.GetNumericValue("kappa_d", kappa_d_, prefix);
+ options.GetNumericValue("slack_move", slack_move_, prefix);
+ options.GetEnumValue("constraint_violation_norm_type", enum_int, prefix);
+ constr_viol_normtype_ = ENormType(enum_int);
+ // The following option is registered by OrigIpoptNLP
+ options.GetBoolValue("warm_start_same_structure",
+ warm_start_same_structure_, prefix);
+
+ if (!warm_start_same_structure_) {
+ dampind_x_L_ = NULL;
+ dampind_x_U_ = NULL;
+ dampind_s_L_ = NULL;
+ dampind_s_U_ = NULL;
+
+ tmp_x_ = NULL;
+ tmp_s_ = NULL;
+ tmp_c_ = NULL;
+ tmp_d_ = NULL;
+ tmp_x_L_ = NULL;
+ tmp_x_U_ = NULL;
+ tmp_s_L_ = NULL;
+ tmp_s_U_ = NULL;
+ }
+
+ num_adjusted_slack_x_L_ = 0;
+ num_adjusted_slack_x_U_ = 0;
+ num_adjusted_slack_s_L_ = 0;
+ num_adjusted_slack_s_U_ = 0;
+
+ initialize_called_ = true;
+ return true;
+ }
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Slack Calculations //
+ ///////////////////////////////////////////////////////////////////////////
+
+ SmartPtr<Vector>
+ IpoptCalculatedQuantities::CalcSlack_L(const Matrix& P,
+ const Vector& x,
+ const Vector& x_bound)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::CalcSlack_L",
+ dbg_verbosity);
+ SmartPtr<Vector> result;
+ result = x_bound.MakeNew();
+ result->Copy(x_bound);
+ P.TransMultVector(1.0, x, -1.0, *result);
+ return result;
+ }
+
+ SmartPtr<Vector>
+ IpoptCalculatedQuantities::CalcSlack_U(const Matrix& P,
+ const Vector& x,
+ const Vector& x_bound)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::CalcSlack_U",
+ dbg_verbosity);
+ SmartPtr<Vector> result;
+ result = x_bound.MakeNew();
+ result->Copy(x_bound);
+ P.TransMultVector(-1.0, x, 1.0, *result);
+ return result;
+ }
+
+ SmartPtr<const Vector> IpoptCalculatedQuantities::curr_slack_x_L()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_slack_x_L()",
+ dbg_verbosity);
+ SmartPtr<Vector> result;
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> x_bound = ip_nlp_->x_L();
+ if (!curr_slack_x_L_cache_.GetCachedResult1Dep(result, *x)) {
+ if (!trial_slack_x_L_cache_.GetCachedResult1Dep(result, *x)) {
+ SmartPtr<const Matrix> P = ip_nlp_->Px_L();
+ DBG_PRINT_VECTOR(2,"x_L", *x_bound);
+ result = CalcSlack_L(*P, *x, *x_bound);
+ DBG_ASSERT(num_adjusted_slack_x_L_==0);
+ num_adjusted_slack_x_L_ =
+ CalculateSafeSlack(result, x_bound, x, ip_data_->curr()->z_L());
+ }
+ curr_slack_x_L_cache_.AddCachedResult1Dep(result, *x);
+ }
+ return ConstPtr(result);
+ }
+
+ SmartPtr<const Vector> IpoptCalculatedQuantities::curr_slack_x_U()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_slack_x_U()",
+ dbg_verbosity);
+
+ SmartPtr<Vector> result;
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> x_bound = ip_nlp_->x_U();
+ if (!curr_slack_x_U_cache_.GetCachedResult1Dep(result, *x)) {
+ if (!trial_slack_x_U_cache_.GetCachedResult1Dep(result, *x)) {
+ SmartPtr<const Matrix> P = ip_nlp_->Px_U();
+ result = CalcSlack_U(*P, *x, *x_bound);
+ DBG_ASSERT(num_adjusted_slack_x_U_==0);
+ num_adjusted_slack_x_U_ =
+ CalculateSafeSlack(result, x_bound, x, ip_data_->curr()->z_U());
+ }
+ curr_slack_x_U_cache_.AddCachedResult1Dep(result, *x);
+ }
+ return ConstPtr(result);
+ }
+
+ SmartPtr<const Vector> IpoptCalculatedQuantities::curr_slack_s_L()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_slack_s_L()",
+ dbg_verbosity);
+
+ SmartPtr<Vector> result;
+ SmartPtr<const Vector> s = ip_data_->curr()->s();
+ SmartPtr<const Vector> s_bound = ip_nlp_->d_L();
+ if (!curr_slack_s_L_cache_.GetCachedResult1Dep(result, *s)) {
+ if (!trial_slack_s_L_cache_.GetCachedResult1Dep(result, *s)) {
+ SmartPtr<const Matrix> P = ip_nlp_->Pd_L();
+ result = CalcSlack_L(*P, *s, *s_bound);
+ DBG_ASSERT(num_adjusted_slack_s_L_==0);
+ num_adjusted_slack_s_L_ =
+ CalculateSafeSlack(result, s_bound, s, ip_data_->curr()->v_L());
+ }
+ curr_slack_s_L_cache_.AddCachedResult1Dep(result, *s);
+ }
+ return ConstPtr(result);
+ }
+
+ SmartPtr<const Vector> IpoptCalculatedQuantities::curr_slack_s_U()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_slack_s_U()",
+ dbg_verbosity);
+
+ SmartPtr<Vector> result;
+ SmartPtr<const Vector> s = ip_data_->curr()->s();
+ SmartPtr<const Vector> s_bound = ip_nlp_->d_U();
+ if (!curr_slack_s_U_cache_.GetCachedResult1Dep(result, *s)) {
+ if (!trial_slack_s_U_cache_.GetCachedResult1Dep(result, *s)) {
+ SmartPtr<const Matrix> P = ip_nlp_->Pd_U();
+ result = CalcSlack_U(*P, *s, *s_bound);
+ DBG_ASSERT(num_adjusted_slack_s_U_==0);
+ num_adjusted_slack_s_U_ =
+ CalculateSafeSlack(result, s_bound, s, ip_data_->curr()->v_U());
+ DBG_PRINT_VECTOR(2, "result", *result);
+ DBG_PRINT((1, "num_adjusted_slack_s_U = %d\n", num_adjusted_slack_s_U_));
+ }
+ curr_slack_s_U_cache_.AddCachedResult1Dep(result, *s);
+ }
+ return ConstPtr(result);
+ }
+
+ SmartPtr<const Vector> IpoptCalculatedQuantities::trial_slack_x_L()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_slack_x_L()",
+ dbg_verbosity);
+
+ num_adjusted_slack_x_L_ = 0;
+ SmartPtr<Vector> result;
+ SmartPtr<const Vector> x = ip_data_->trial()->x();
+ SmartPtr<const Vector> x_bound = ip_nlp_->x_L();
+ if (!trial_slack_x_L_cache_.GetCachedResult1Dep(result, *x)) {
+ if (!curr_slack_x_L_cache_.GetCachedResult1Dep(result, *x)) {
+ SmartPtr<const Matrix> P = ip_nlp_->Px_L();
+ result = CalcSlack_L(*P, *x, *x_bound);
+ DBG_ASSERT(num_adjusted_slack_x_L_==0);
+ num_adjusted_slack_x_L_ =
+ CalculateSafeSlack(result, x_bound, x, ip_data_->curr()->z_L());
+ }
+ trial_slack_x_L_cache_.AddCachedResult1Dep(result, *x);
+ }
+ return ConstPtr(result);
+ }
+
+ SmartPtr<const Vector> IpoptCalculatedQuantities::trial_slack_x_U()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_slack_x_U()",
+ dbg_verbosity);
+
+ num_adjusted_slack_x_U_ = 0;
+ SmartPtr<Vector> result;
+ SmartPtr<const Vector> x = ip_data_->trial()->x();
+ SmartPtr<const Vector> x_bound = ip_nlp_->x_U();
+ if (!trial_slack_x_U_cache_.GetCachedResult1Dep(result, *x)) {
+ if (!curr_slack_x_U_cache_.GetCachedResult1Dep(result, *x)) {
+ SmartPtr<const Matrix> P = ip_nlp_->Px_U();
+ result = CalcSlack_U(*P, *x, *x_bound);
+ DBG_ASSERT(num_adjusted_slack_x_U_==0);
+ num_adjusted_slack_x_U_ =
+ CalculateSafeSlack(result, x_bound, x, ip_data_->curr()->z_U());
+ }
+ trial_slack_x_U_cache_.AddCachedResult1Dep(result, *x);
+ }
+ return ConstPtr(result);
+ }
+
+ SmartPtr<const Vector> IpoptCalculatedQuantities::trial_slack_s_L()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_slack_s_L()",
+ dbg_verbosity);
+
+ num_adjusted_slack_s_L_ = 0;
+ SmartPtr<Vector> result;
+ SmartPtr<const Vector> s = ip_data_->trial()->s();
+ SmartPtr<const Vector> s_bound = ip_nlp_->d_L();
+ if (!trial_slack_s_L_cache_.GetCachedResult1Dep(result, *s)) {
+ if (!curr_slack_s_L_cache_.GetCachedResult1Dep(result, *s)) {
+ SmartPtr<const Matrix> P = ip_nlp_->Pd_L();
+ result = CalcSlack_L(*P, *s, *s_bound);
+ DBG_ASSERT(num_adjusted_slack_s_L_==0);
+ num_adjusted_slack_s_L_ =
+ CalculateSafeSlack(result, s_bound, s, ip_data_->curr()->v_L());
+ }
+ trial_slack_s_L_cache_.AddCachedResult1Dep(result, *s);
+ }
+ return ConstPtr(result);
+ }
+
+ SmartPtr<const Vector> IpoptCalculatedQuantities::trial_slack_s_U()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_slack_s_U()",
+ dbg_verbosity);
+
+ num_adjusted_slack_s_U_ = 0;
+ SmartPtr<Vector> result;
+ SmartPtr<const Vector> s = ip_data_->trial()->s();
+ SmartPtr<const Vector> s_bound = ip_nlp_->d_U();
+ if (!trial_slack_s_U_cache_.GetCachedResult1Dep(result, *s)) {
+ if (!curr_slack_s_U_cache_.GetCachedResult1Dep(result, *s)) {
+ SmartPtr<const Matrix> P = ip_nlp_->Pd_U();
+ result = CalcSlack_U(*P, *s, *s_bound);
+ DBG_ASSERT(num_adjusted_slack_s_U_==0);
+ num_adjusted_slack_s_U_ =
+ CalculateSafeSlack(result, s_bound, s, ip_data_->curr()->v_U());
+ DBG_PRINT((1, "num_adjusted_slack_s_U = %d\n", num_adjusted_slack_s_U_));
+ DBG_PRINT_VECTOR(2, "trial_slack_s_U", *result);
+ }
+ trial_slack_s_U_cache_.AddCachedResult1Dep(result, *s);
+ }
+ return ConstPtr(result);
+ }
+
+ Index IpoptCalculatedQuantities::
+ CalculateSafeSlack(SmartPtr<Vector>& slack,
+ const SmartPtr<const Vector>& bound,
+ const SmartPtr<const Vector>& curr_point,
+ const SmartPtr<const Vector>& multiplier)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::CalculateSafeSlack", dbg_verbosity);
+ DBG_ASSERT(initialize_called_);
+ Index retval = 0;
+ if (slack->Dim() > 0) {
+ Number min_slack = slack->Min();
+ // TODO we need to make sure that this also works for non-monotone MUs
+ Number s_min = std::numeric_limits<Number>::epsilon()
+ * Min(1., ip_data_->curr_mu());
+ DBG_PRINT((1,"s_min = %g, min_slack=%g\n", s_min, min_slack));
+ if (min_slack < s_min) {
+ // Need to correct the slacks and calculate new bounds...
+ SmartPtr<Vector> t = slack->MakeNew();
+ t->Copy(*slack);
+ t->AddScalar(-s_min);
+ t->ElementWiseSgn();
+
+ SmartPtr<Vector> zero_vec = t->MakeNew();
+ zero_vec->Set(0.0);
+ t->ElementWiseMin(*zero_vec);
+ t->Scal(-1.0);
+ retval = (Index)t->Asum();
+ DBG_PRINT((1,"Number of slack corrections = %d\n", retval));
+ DBG_PRINT_VECTOR(2, "t(sgn)", *t);
+
+ // ToDo AW: I added the follwing line b/c I found a case where
+ // slack was negative and this correction produced 0
+ slack->ElementWiseMax(*zero_vec);
+
+ SmartPtr<Vector> t2 = t->MakeNew();
+ t2->Set(ip_data_->curr_mu());
+ t2->ElementWiseDivide(*multiplier);
+
+ SmartPtr<Vector> s_min_vec = t2->MakeNew();
+ s_min_vec->Set(s_min);
+
+ t2->ElementWiseMax(*s_min_vec);
+ t2->Axpy(-1.0, *slack);
+ DBG_PRINT_VECTOR(2, "tw(smin,mu/mult)", *t2);
+
+ t->ElementWiseMultiply(*t2);
+ t->Axpy(1.0, *slack);
+
+ SmartPtr<Vector> t_max = t2;
+ t_max->Set(1.0);
+ SmartPtr<Vector> abs_bound = bound->MakeNew();
+ abs_bound->Copy(*bound);
+ abs_bound->ElementWiseAbs();
+ t_max->ElementWiseMax(*abs_bound);
+ DBG_PRINT_VECTOR(2, "t_max1", *t_max);
+ DBG_PRINT_VECTOR(2, "slack", *slack);
+ t_max->AddOneVector(1.0, *slack, slack_move_);
+ DBG_PRINT_VECTOR(2, "t_max2", *t_max);
+
+ t->ElementWiseMin(*t_max);
+ DBG_PRINT_VECTOR(2, "new_slack", *t);
+
+ slack = t;
+ return retval;
+ }
+ }
+
+ return retval;
+ }
+
+ Index
+ IpoptCalculatedQuantities::AdjustedTrialSlacks()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::AdjustedTrialSlacks()",
+ dbg_verbosity);
+ Index result = (num_adjusted_slack_x_L_ +
+ num_adjusted_slack_x_U_ +
+ num_adjusted_slack_s_L_ +
+ num_adjusted_slack_s_U_);
+ DBG_PRINT((1,"result = %d\n", result));
+ return result;
+ }
+
+ void
+ IpoptCalculatedQuantities::ResetAdjustedTrialSlacks()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::ResetAdjustedTrialSlacks()",
+ dbg_verbosity);
+ num_adjusted_slack_x_L_
+ = num_adjusted_slack_x_U_
+ = num_adjusted_slack_s_L_
+ = num_adjusted_slack_s_U_ = 0;
+ }
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Objective Function //
+ ///////////////////////////////////////////////////////////////////////////
+
+ Number
+ IpoptCalculatedQuantities::curr_f()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_f()",
+ dbg_verbosity);
+ Number result;
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ DBG_PRINT_VECTOR(2,"curr_x",*x);
+ DBG_PRINT((1, "curr_x tag = %d\n", x->GetTag()));
+
+ bool objective_depends_on_mu = ip_nlp_->objective_depends_on_mu();
+ std::vector<const TaggedObject*> tdeps(1);
+ tdeps[0] = GetRawPtr(x);
+ std::vector<Number> sdeps(1);
+ if (objective_depends_on_mu) {
+ sdeps[0] = ip_data_->curr_mu();
+ }
+ else {
+ sdeps[0] = -1.;
+ }
+
+ if (!curr_f_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ if (!trial_f_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ DBG_PRINT((2,"evaluate curr f\n"));
+ if (objective_depends_on_mu) {
+ result = ip_nlp_->f(*x, ip_data_->curr_mu());
+ }
+ else {
+ result = ip_nlp_->f(*x);
+ }
+ }
+ curr_f_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+ DBG_PRINT((1,"result (curr_f) = %e\n", result));
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::unscaled_curr_f()
+ {
+ return ip_nlp_->NLP_scaling()->unapply_obj_scaling(curr_f());
+ }
+
+ Number
+ IpoptCalculatedQuantities::trial_f()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_f()",
+ dbg_verbosity);
+ Number result;
+ SmartPtr<const Vector> x = ip_data_->trial()->x();
+ DBG_PRINT_VECTOR(2,"trial_x",*x);
+ DBG_PRINT((1, "trial_x tag = %d\n", x->GetTag()));
+
+ bool objective_depends_on_mu = ip_nlp_->objective_depends_on_mu();
+ std::vector<const TaggedObject*> tdeps(1);
+ tdeps[0] = GetRawPtr(x);
+ std::vector<Number> sdeps(1);
+ if (objective_depends_on_mu) {
+ sdeps[0] = ip_data_->curr_mu();
+ }
+ else {
+ sdeps[0] = -1.;
+ }
+
+ if (!trial_f_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ if (!curr_f_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ DBG_PRINT((2,"evaluate trial f\n"));
+ if (objective_depends_on_mu) {
+ result = ip_nlp_->f(*x, ip_data_->curr_mu());
+ }
+ else {
+ result = ip_nlp_->f(*x);
+ }
+ }
+ trial_f_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+ DBG_PRINT((1,"result (trial_f) = %e\n", result));
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::unscaled_trial_f()
+ {
+ return ip_nlp_->NLP_scaling()->unapply_obj_scaling(trial_f());
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_grad_f()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_grad_f()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+
+ bool objective_depends_on_mu = ip_nlp_->objective_depends_on_mu();
+ std::vector<const TaggedObject*> tdeps(1);
+ tdeps[0] = GetRawPtr(x);
+ std::vector<Number> sdeps(1);
+ if (objective_depends_on_mu) {
+ sdeps[0] = ip_data_->curr_mu();
+ }
+ else {
+ sdeps[0] = -1.;
+ }
+
+ if (!curr_grad_f_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ if (!trial_grad_f_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ if (objective_depends_on_mu) {
+ result = ip_nlp_->grad_f(*x, ip_data_->curr_mu());
+ }
+ else {
+ result = ip_nlp_->grad_f(*x);
+ }
+ }
+ curr_grad_f_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::trial_grad_f()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_grad_f()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+ SmartPtr<const Vector> x = ip_data_->trial()->x();
+
+ bool objective_depends_on_mu = ip_nlp_->objective_depends_on_mu();
+ std::vector<const TaggedObject*> tdeps(1);
+ tdeps[0] = GetRawPtr(x);
+ std::vector<Number> sdeps(1);
+ if (objective_depends_on_mu) {
+ sdeps[0] = ip_data_->curr_mu();
+ }
+ else {
+ sdeps[0] = -1.;
+ }
+
+ if (!trial_grad_f_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ if (!curr_grad_f_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ if (objective_depends_on_mu) {
+ result = ip_nlp_->grad_f(*x, ip_data_->curr_mu());
+ }
+ else {
+ result = ip_nlp_->grad_f(*x);
+ }
+ }
+ trial_grad_f_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+ return result;
+ }
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Barrier Objective Function //
+ ///////////////////////////////////////////////////////////////////////////
+ Number
+ IpoptCalculatedQuantities::CalcBarrierTerm(Number mu,
+ const Vector& slack_x_L,
+ const Vector& slack_x_U,
+ const Vector& slack_s_L,
+ const Vector& slack_s_U)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::CalcBarrierTerm",
+ dbg_verbosity);
+ DBG_ASSERT(initialize_called_);
+
+ DBG_PRINT_VECTOR(2, "slack_x_L", slack_x_L);
+ DBG_PRINT_VECTOR(2, "slack_x_U", slack_x_U);
+ DBG_PRINT_VECTOR(2, "slack_s_L", slack_s_L);
+ DBG_PRINT_VECTOR(2, "slack_s_U", slack_s_U);
+
+ Number retval=0.;
+ retval += slack_x_L.SumLogs();
+ DBG_PRINT((1, "BarrierTerm after x_L = %25.16e\n", retval));
+ retval += slack_x_U.SumLogs();
+ DBG_PRINT((1, "BarrierTerm after x_U = %25.16e\n", retval));
+ retval += slack_s_L.SumLogs();
+ DBG_PRINT((1, "BarrierTerm after s_L = %25.16e\n", retval));
+ retval += slack_s_U.SumLogs();
+ DBG_PRINT((1, "BarrierTerm after s_U = %25.16e\n", retval));
+ retval *= -mu;
+
+ DBG_PRINT((1, "BarrierTerm without damping = %25.16e\n", retval));
+
+ // Include the linear damping term if kappa_d is nonzero.
+ if (kappa_d_>0) {
+ SmartPtr<const Vector> dampind_x_L;
+ SmartPtr<const Vector> dampind_x_U;
+ SmartPtr<const Vector> dampind_s_L;
+ SmartPtr<const Vector> dampind_s_U;
+ ComputeDampingIndicators(dampind_x_L, dampind_x_U, dampind_s_L, dampind_s_U);
+
+ Tmp_x_L().Copy(slack_x_L);
+ Tmp_x_L().ElementWiseMultiply(*dampind_x_L);
+ retval += kappa_d_ * mu * Tmp_x_L().Asum();
+ Tmp_x_U().Copy(slack_x_U);
+ Tmp_x_U().ElementWiseMultiply(*dampind_x_U);
+ retval += kappa_d_ * mu * Tmp_x_U().Asum();
+ Tmp_s_L().Copy(slack_s_L);
+ Tmp_s_L().ElementWiseMultiply(*dampind_s_L);
+ retval += kappa_d_ * mu * Tmp_s_L().Asum();
+ Tmp_s_U().Copy(slack_s_U);
+ Tmp_s_U().ElementWiseMultiply(*dampind_s_U);
+ retval += kappa_d_ * mu * Tmp_s_U().Asum();
+ }
+
+ DBG_PRINT((1, "BarrierTerm with damping = %25.16e\n", retval));
+
+ DBG_ASSERT(IsFiniteNumber(retval));
+ return retval;
+ }
+
+ Number
+ IpoptCalculatedQuantities::curr_barrier_obj()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_barrier_obj()",
+ dbg_verbosity);
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> s = ip_data_->curr()->s();
+ DBG_PRINT_VECTOR(2,"curr_x",*x);
+ DBG_PRINT_VECTOR(2,"curr_s",*s);
+ std::vector<const TaggedObject*> tdeps(2);
+ tdeps[0] = GetRawPtr(x);
+ tdeps[1] = GetRawPtr(s);
+
+ Number mu = ip_data_->curr_mu();
+ DBG_PRINT((1,"curr_mu=%e\n",mu));
+ std::vector<Number> sdeps(1);
+ sdeps[0] = mu;
+
+ if (!curr_barrier_obj_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ if (!trial_barrier_obj_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ result = curr_f();
+ DBG_PRINT((1,"curr_F=%e\n",result));
+ result += CalcBarrierTerm(mu,
+ *curr_slack_x_L(),
+ *curr_slack_x_U(),
+ *curr_slack_s_L(),
+ *curr_slack_s_U());
+ }
+ curr_barrier_obj_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+ DBG_ASSERT(IsFiniteNumber(result));
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::trial_barrier_obj()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_barrier_obj()",
+ dbg_verbosity);
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->trial()->x();
+ SmartPtr<const Vector> s = ip_data_->trial()->s();
+ DBG_PRINT_VECTOR(2,"trial_x",*x);
+ DBG_PRINT_VECTOR(2,"trial_s",*s);
+ std::vector<const TaggedObject*> tdeps(2);
+ tdeps[0] = GetRawPtr(x);
+ tdeps[1] = GetRawPtr(s);
+
+ Number mu = ip_data_->curr_mu();
+ DBG_PRINT((1,"trial_mu=%e\n",mu));
+ std::vector<Number> sdeps(1);
+ sdeps[0] = mu;
+
+ if (!trial_barrier_obj_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ if (!curr_barrier_obj_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ result = trial_f();
+ DBG_PRINT((1,"trial_F=%e\n",result));
+ DBG_PRINT_VECTOR(2, "trial_slack_s_U", *trial_slack_s_U());
+ result += CalcBarrierTerm(ip_data_->curr_mu(),
+ *trial_slack_x_L(),
+ *trial_slack_x_U(),
+ *trial_slack_s_L(),
+ *trial_slack_s_U());
+ }
+ trial_barrier_obj_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+ DBG_ASSERT(IsFiniteNumber(result));
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_grad_barrier_obj_x()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_grad_barrier_obj_x()",
+ dbg_verbosity);
+ DBG_ASSERT(initialize_called_);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ std::vector<const TaggedObject*> tdeps(1);
+ tdeps[0] = GetRawPtr(x);
+ Number mu = ip_data_->curr_mu();
+ std::vector<Number> sdeps(1);
+ sdeps[0] = mu;
+ DBG_PRINT((1,"curr_mu=%e\n",mu));
+
+ if (!curr_grad_barrier_obj_x_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ SmartPtr<Vector> tmp1 = x->MakeNew();
+ tmp1->Copy(*curr_grad_f());
+
+ Tmp_x_L().Set(1.);
+ ip_nlp_->Px_L()->AddMSinvZ(-mu, *curr_slack_x_L(), Tmp_x_L(), *tmp1);
+
+ Tmp_x_U().Set(1.);
+ ip_nlp_->Px_U()->AddMSinvZ(mu, *curr_slack_x_U(), Tmp_x_U(), *tmp1);
+
+ DBG_PRINT_VECTOR(2, "Barrier_Grad_x without damping", *tmp1);
+
+ // Take care of linear damping terms
+ if (kappa_d_>0.) {
+ SmartPtr<const Vector> dampind_x_L;
+ SmartPtr<const Vector> dampind_x_U;
+ SmartPtr<const Vector> dampind_s_L;
+ SmartPtr<const Vector> dampind_s_U;
+ ComputeDampingIndicators(dampind_x_L, dampind_x_U, dampind_s_L, dampind_s_U);
+
+ DBG_PRINT((1, "kappa_d*mu = %e\n", kappa_d_*mu));
+ DBG_PRINT_VECTOR(2, "dampind_x_L", *dampind_x_L);
+ ip_nlp_->Px_L()->MultVector(kappa_d_*mu, *dampind_x_L, 1., *tmp1);
+ ip_nlp_->Px_U()->MultVector(-kappa_d_*mu, *dampind_x_U, 1., *tmp1);
+ }
+
+ DBG_PRINT_VECTOR(2, "Barrier_Grad_x with damping", *tmp1);
+
+ result = ConstPtr(tmp1);
+
+ curr_grad_barrier_obj_x_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::grad_kappa_times_damping_x()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::grad_kappa_times_damping_x()",
+ dbg_verbosity);
+ DBG_ASSERT(initialize_called_);
+ SmartPtr<const Vector> result;
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+
+ std::vector<const TaggedObject*> tdeps(2);
+ tdeps[0] = GetRawPtr(ip_nlp_->Px_L());
+ tdeps[1] = GetRawPtr(ip_nlp_->Px_U());
+ std::vector<Number> sdeps(1);
+ sdeps[0] = kappa_d_;
+ if (!grad_kappa_times_damping_x_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ SmartPtr<Vector> tmp1 = x->MakeNew();
+ if (kappa_d_>0.) {
+ SmartPtr<const Vector> dampind_x_L;
+ SmartPtr<const Vector> dampind_x_U;
+ SmartPtr<const Vector> dampind_s_L;
+ SmartPtr<const Vector> dampind_s_U;
+ ComputeDampingIndicators(dampind_x_L, dampind_x_U, dampind_s_L, dampind_s_U);
+
+ ip_nlp_->Px_L()->MultVector(kappa_d_, *dampind_x_L, 0., *tmp1);
+ ip_nlp_->Px_U()->MultVector(-kappa_d_, *dampind_x_U, 1., *tmp1);
+ }
+ else {
+ tmp1->Set(0.);
+ }
+ result = ConstPtr(tmp1);
+
+ grad_kappa_times_damping_x_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_grad_barrier_obj_s()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_grad_barrier_obj_s()",
+ dbg_verbosity);
+ DBG_ASSERT(initialize_called_);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> s = ip_data_->curr()->s();
+ std::vector<const TaggedObject*> tdeps(1);
+ tdeps[0] = GetRawPtr(s);
+ Number mu = ip_data_->curr_mu();
+ std::vector<Number> sdeps(1);
+ sdeps[0] = mu;
+ DBG_PRINT((1,"curr_mu=%e\n",mu));
+
+ if (!curr_grad_barrier_obj_s_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ SmartPtr<Vector> tmp1 = s->MakeNew();
+
+ Tmp_s_L().Set(-mu);
+ Tmp_s_L().ElementWiseDivide(*curr_slack_s_L());
+ ip_nlp_->Pd_L()->MultVector(1., Tmp_s_L(), 0., *tmp1);
+
+ Tmp_s_U().Set(1.);
+ ip_nlp_->Pd_U()->AddMSinvZ(mu, *curr_slack_s_U(), Tmp_s_U(), *tmp1);
+
+ DBG_PRINT_VECTOR(2, "Barrier_Grad_s without damping", *tmp1);
+
+ // Take care of linear damping terms
+ if (kappa_d_>0.) {
+ SmartPtr<const Vector> dampind_x_L;
+ SmartPtr<const Vector> dampind_x_U;
+ SmartPtr<const Vector> dampind_s_L;
+ SmartPtr<const Vector> dampind_s_U;
+ ComputeDampingIndicators(dampind_x_L, dampind_x_U, dampind_s_L, dampind_s_U);
+
+ DBG_PRINT((1, "kappa_d*mu = %e\n", kappa_d_*mu));
+ DBG_PRINT_VECTOR(2, "dampind_s_L", *dampind_s_L);
+ DBG_PRINT_VECTOR(2, "dampind_s_U", *dampind_s_U);
+ ip_nlp_->Pd_L()->MultVector(kappa_d_*mu, *dampind_s_L, 1., *tmp1);
+ ip_nlp_->Pd_U()->MultVector(-kappa_d_*mu, *dampind_s_U, 1., *tmp1);
+ }
+
+ DBG_PRINT_VECTOR(2, "Barrier_Grad_s with damping", *tmp1);
+
+ result = ConstPtr(tmp1);
+
+ curr_grad_barrier_obj_s_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::grad_kappa_times_damping_s()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::grad_kappa_times_damping_s()",
+ dbg_verbosity);
+ DBG_ASSERT(initialize_called_);
+ SmartPtr<const Vector> result;
+ SmartPtr<const Vector> s = ip_data_->curr()->s();
+
+ std::vector<const TaggedObject*> tdeps(2);
+ tdeps[0] = GetRawPtr(ip_nlp_->Pd_L());
+ tdeps[1] = GetRawPtr(ip_nlp_->Pd_U());
+ std::vector<Number> sdeps(1);
+ sdeps[0] = kappa_d_;
+ if (!grad_kappa_times_damping_s_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ SmartPtr<Vector> tmp1 = s->MakeNew();
+ if (kappa_d_>0.) {
+ SmartPtr<const Vector> dampind_x_L;
+ SmartPtr<const Vector> dampind_x_U;
+ SmartPtr<const Vector> dampind_s_L;
+ SmartPtr<const Vector> dampind_s_U;
+ ComputeDampingIndicators(dampind_x_L, dampind_x_U, dampind_s_L, dampind_s_U);
+
+ ip_nlp_->Pd_L()->MultVector(kappa_d_, *dampind_s_L, 0., *tmp1);
+ ip_nlp_->Pd_U()->MultVector(-kappa_d_, *dampind_s_U, 1., *tmp1);
+ }
+ else {
+ tmp1->Set(0.);
+ }
+ result = ConstPtr(tmp1);
+
+ grad_kappa_times_damping_s_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+
+ return result;
+ }
+
+ void
+ IpoptCalculatedQuantities::ComputeDampingIndicators(
+ SmartPtr<const Vector>& dampind_x_L,
+ SmartPtr<const Vector>& dampind_x_U,
+ SmartPtr<const Vector>& dampind_s_L,
+ SmartPtr<const Vector>& dampind_s_U)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::ComputeDampingFilters()",
+ dbg_verbosity);
+
+ // Assume that all indicators have to be computed if one of the
+ // SmartPtrs is still zero.
+ if (IsNull(dampind_x_L_)) {
+ // First for x
+ Tmp_x_L().Set(1.0);
+ ip_nlp_->Px_L()->MultVector(1.0, Tmp_x_L(), 0.0, Tmp_x());
+ Tmp_x_U().Set(1.0);
+ ip_nlp_->Px_U()->MultVector(-1.0, Tmp_x_U(), 1.0, Tmp_x());
+
+ dampind_x_L_ = ip_nlp_->x_L()->MakeNew();
+ ip_nlp_->Px_L()->TransMultVector(1.0, Tmp_x(), 0.0, *dampind_x_L_);
+
+ dampind_x_U_ = ip_nlp_->x_U()->MakeNew();
+ ip_nlp_->Px_U()->TransMultVector(-1.0, Tmp_x(), 0.0, *dampind_x_U_);
+
+ // Now for s
+ Tmp_s_L().Set(1.0);
+ ip_nlp_->Pd_L()->MultVector(1.0, Tmp_s_L(), 0.0, Tmp_s());
+ Tmp_s_U().Set(1.0);
+ ip_nlp_->Pd_U()->MultVector(-1.0, Tmp_s_U(), 1.0, Tmp_s());
+
+ dampind_s_L_ = ip_nlp_->d_L()->MakeNew();
+ ip_nlp_->Pd_L()->TransMultVector(1.0, Tmp_s(), 0.0, *dampind_s_L_);
+
+ dampind_s_U_ = ip_nlp_->d_U()->MakeNew();
+ ip_nlp_->Pd_U()->TransMultVector(-1.0, Tmp_s(), 0.0, *dampind_s_U_);
+
+ DBG_PRINT_VECTOR(2, "dampind_x_L_", *dampind_x_L_);
+ DBG_PRINT_VECTOR(2, "dampind_x_U_", *dampind_x_U_);
+ DBG_PRINT_VECTOR(2, "dampind_s_L_", *dampind_s_L_);
+ DBG_PRINT_VECTOR(2, "dampind_s_U_", *dampind_s_U_);
+ }
+
+ dampind_x_L = ConstPtr(dampind_x_L_);
+ dampind_x_U = ConstPtr(dampind_x_U_);
+ dampind_s_L = ConstPtr(dampind_s_L_);
+ dampind_s_U = ConstPtr(dampind_s_U_);
+ }
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Constraints //
+ ///////////////////////////////////////////////////////////////////////////
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_c()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_c()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+
+ if (!curr_c_cache_.GetCachedResult1Dep(result, *x)) {
+ if (!trial_c_cache_.GetCachedResult1Dep(result, *x)) {
+ result = ip_nlp_->c(*x);
+ }
+ curr_c_cache_.AddCachedResult1Dep(result, *x);
+ }
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::unscaled_curr_c()
+ {
+ return ip_nlp_->NLP_scaling()->unapply_vector_scaling_c(curr_c());
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::trial_c()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_c()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+ SmartPtr<const Vector> x = ip_data_->trial()->x();
+
+ if (!trial_c_cache_.GetCachedResult1Dep(result, *x)) {
+ if (!curr_c_cache_.GetCachedResult1Dep(result, *x)) {
+ result = ip_nlp_->c(*x);
+ }
+ trial_c_cache_.AddCachedResult1Dep(result, *x);
+ }
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_d()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_d()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+
+ if (!curr_d_cache_.GetCachedResult1Dep(result, *x)) {
+ if (!trial_d_cache_.GetCachedResult1Dep(result, *x)) {
+ result = ip_nlp_->d(*x);
+ }
+ curr_d_cache_.AddCachedResult1Dep(result, *x);
+ }
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::unscaled_curr_d()
+ {
+ return ip_nlp_->NLP_scaling()->unapply_vector_scaling_d(curr_d());
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::trial_d()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_d()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+ SmartPtr<const Vector> x = ip_data_->trial()->x();
+
+ if (!trial_d_cache_.GetCachedResult1Dep(result, *x)) {
+ if (!curr_d_cache_.GetCachedResult1Dep(result, *x)) {
+ result = ip_nlp_->d(*x);
+ }
+ trial_d_cache_.AddCachedResult1Dep(result, *x);
+ }
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_d_minus_s()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_d_minus_s()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> s = ip_data_->curr()->s();
+
+ if (!curr_d_minus_s_cache_.GetCachedResult2Dep(result, *x, *s)) {
+ if (!trial_d_minus_s_cache_.GetCachedResult2Dep(result, *x, *s)) {
+ SmartPtr<Vector> tmp = s->MakeNew();
+ tmp->AddTwoVectors(1., *curr_d(), -1., *s, 0.);
+ result = ConstPtr(tmp);
+ }
+ curr_d_minus_s_cache_.AddCachedResult2Dep(result, *x, *s);
+ }
+
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::trial_d_minus_s()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_d_minus_s()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> x = ip_data_->trial()->x();
+ SmartPtr<const Vector> s = ip_data_->trial()->s();
+
+ if (!trial_d_minus_s_cache_.GetCachedResult2Dep(result, *x, *s)) {
+ if (!curr_d_minus_s_cache_.GetCachedResult2Dep(result, *x, *s)) {
+ SmartPtr<Vector> tmp = s->MakeNew();
+ tmp->AddTwoVectors(1., *trial_d(), -1., *s, 0.);
+ result = ConstPtr(tmp);
+ }
+ trial_d_minus_s_cache_.AddCachedResult2Dep(result, *x, *s);
+ }
+
+ return result;
+ }
+
+ SmartPtr<const Matrix>
+ IpoptCalculatedQuantities::curr_jac_c()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_jac_c()",
+ dbg_verbosity);
+ SmartPtr<const Matrix> result;
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+
+ if (!curr_jac_c_cache_.GetCachedResult1Dep(result, *x)) {
+ if (!trial_jac_c_cache_.GetCachedResult1Dep(result, *x)) {
+ result = ip_nlp_->jac_c(*x);
+ }
+ curr_jac_c_cache_.AddCachedResult1Dep(result, *x);
+ }
+ return result;
+ }
+
+ SmartPtr<const Matrix>
+ IpoptCalculatedQuantities::trial_jac_c()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_jac_c()",
+ dbg_verbosity);
+ SmartPtr<const Matrix> result;
+ SmartPtr<const Vector> x = ip_data_->trial()->x();
+
+ if (!trial_jac_c_cache_.GetCachedResult1Dep(result, *x)) {
+ if (!curr_jac_c_cache_.GetCachedResult1Dep(result, *x)) {
+ result = ip_nlp_->jac_c(*x);
+ }
+ trial_jac_c_cache_.AddCachedResult1Dep(result, *x);
+ }
+ return result;
+ }
+
+ SmartPtr<const Matrix>
+ IpoptCalculatedQuantities::curr_jac_d()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_jac_d()",
+ dbg_verbosity);
+ SmartPtr<const Matrix> result;
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+
+ if (!curr_jac_d_cache_.GetCachedResult1Dep(result, *x)) {
+ if (!trial_jac_d_cache_.GetCachedResult1Dep(result, *x)) {
+ result = ip_nlp_->jac_d(*x);
+ }
+ curr_jac_d_cache_.AddCachedResult1Dep(result, *x);
+ }
+ return result;
+ }
+
+ SmartPtr<const Matrix>
+ IpoptCalculatedQuantities::trial_jac_d()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_jac_d()",
+ dbg_verbosity);
+ SmartPtr<const Matrix> result;
+ SmartPtr<const Vector> x = ip_data_->trial()->x();
+
+ if (!trial_jac_d_cache_.GetCachedResult1Dep(result, *x)) {
+ if (!curr_jac_d_cache_.GetCachedResult1Dep(result, *x)) {
+ result = ip_nlp_->jac_d(*x);
+ }
+ trial_jac_d_cache_.AddCachedResult1Dep(result, *x);
+ }
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_jac_c_times_vec(const Vector& vec)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_jac_c_times_vec",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+
+ if (!curr_jac_c_times_vec_cache_.GetCachedResult2Dep(result, *x, vec)) {
+ SmartPtr<Vector> tmp = ip_data_->curr()->y_c()->MakeNew();
+ curr_jac_c()->MultVector(1.0, vec, 0., *tmp);
+ result = ConstPtr(tmp);
+ curr_jac_c_times_vec_cache_.AddCachedResult2Dep(result, *x, vec);
+ }
+
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_jac_d_times_vec(const Vector& vec)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_jac_d_times_vec()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+
+ if (!curr_jac_d_times_vec_cache_.GetCachedResult2Dep(result, *x, vec)) {
+ SmartPtr<Vector> tmp = ip_data_->curr()->s()->MakeNew();
+ curr_jac_d()->MultVector(1.0, vec, 0., *tmp);
+ result = ConstPtr(tmp);
+ curr_jac_d_times_vec_cache_.AddCachedResult2Dep(result, *x, vec);
+ }
+
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_jac_cT_times_curr_y_c()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_jac_cT_times_curr_y_c()",
+ dbg_verbosity);
+ return curr_jac_cT_times_vec(*ip_data_->curr()->y_c());
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::trial_jac_cT_times_trial_y_c()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_jac_cT_times_trial_y_c()",
+ dbg_verbosity);
+ return trial_jac_cT_times_vec(*ip_data_->trial()->y_c());
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_jac_dT_times_curr_y_d()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_jac_dT_times_curr_y_d()",
+ dbg_verbosity);
+ return curr_jac_dT_times_vec(*ip_data_->curr()->y_d());
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::trial_jac_dT_times_trial_y_d()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_jac_dT_times_trial_y_d()",
+ dbg_verbosity);
+ return trial_jac_dT_times_vec(*ip_data_->trial()->y_d());
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_jac_cT_times_vec(const Vector& vec)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_jac_cT_times_vec",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+
+ if (!curr_jac_cT_times_vec_cache_.GetCachedResult2Dep(result, *x, vec)) {
+ if (!trial_jac_cT_times_vec_cache_.GetCachedResult2Dep(result, *x, vec)) {
+ SmartPtr<Vector> tmp = x->MakeNew();
+ curr_jac_c()->TransMultVector(1.0, vec, 0., *tmp);
+ result = ConstPtr(tmp);
+ }
+ curr_jac_cT_times_vec_cache_.AddCachedResult2Dep(result, *x, vec);
+ }
+
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::trial_jac_cT_times_vec(const Vector& vec)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_jac_cT_times_vec",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+ SmartPtr<const Vector> x = ip_data_->trial()->x();
+
+ if (!trial_jac_cT_times_vec_cache_.GetCachedResult2Dep(result, *x, vec)) {
+ if (!curr_jac_cT_times_vec_cache_.GetCachedResult2Dep(result, *x, vec)) {
+ SmartPtr<Vector> tmp = x->MakeNew();
+ trial_jac_c()->TransMultVector(1.0, vec, 0., *tmp);
+ result = ConstPtr(tmp);
+ }
+ trial_jac_cT_times_vec_cache_.AddCachedResult2Dep(result, *x, vec);
+ }
+
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_jac_dT_times_vec(const Vector& vec)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_jac_dT_times_vec()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+
+ if (!curr_jac_dT_times_vec_cache_.GetCachedResult2Dep(result, *x, vec)) {
+ if (!trial_jac_dT_times_vec_cache_.GetCachedResult2Dep(result, *x, vec)) {
+ SmartPtr<Vector> tmp = x->MakeNew();
+ curr_jac_d()->TransMultVector(1.0, vec, 0., *tmp);
+ result = ConstPtr(tmp);
+ }
+ curr_jac_dT_times_vec_cache_.AddCachedResult2Dep(result, *x, vec);
+ }
+
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::trial_jac_dT_times_vec(const Vector& vec)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_jac_dT_times_vec()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+ SmartPtr<const Vector> x = ip_data_->trial()->x();
+
+ if (!trial_jac_dT_times_vec_cache_.GetCachedResult2Dep(result, *x, vec)) {
+ if (!curr_jac_dT_times_vec_cache_.GetCachedResult2Dep(result, *x, vec)) {
+ SmartPtr<Vector> tmp = x->MakeNew();
+ trial_jac_d()->TransMultVector(1.0, vec, 0., *tmp);
+ result = ConstPtr(tmp);
+ }
+ trial_jac_dT_times_vec_cache_.AddCachedResult2Dep(result, *x, vec);
+ }
+
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::curr_constraint_violation()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_constraint_violation()",
+ dbg_verbosity);
+ return curr_primal_infeasibility(constr_viol_normtype_);
+ }
+
+ Number
+ IpoptCalculatedQuantities::trial_constraint_violation()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_constraint_violation()",
+ dbg_verbosity);
+ return trial_primal_infeasibility(constr_viol_normtype_);
+ }
+
+ Number
+ IpoptCalculatedQuantities::curr_nlp_constraint_violation
+ (ENormType NormType)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_nlp_constraint_violation()",
+ dbg_verbosity);
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+
+ std::vector<const TaggedObject*> deps(1);
+ deps[0] = GetRawPtr(x);
+ std::vector<Number> sdeps(1);
+ sdeps[0] = (Number)NormType;
+
+ if (!curr_nlp_constraint_violation_cache_.GetCachedResult(result, deps, sdeps)) {
+ SmartPtr<const Vector> c = curr_c();
+ SmartPtr<const Vector> d = curr_d();
+
+ SmartPtr<Vector> d_viol_L = ip_nlp_->d_L()->MakeNewCopy();
+ ip_nlp_->Pd_L()->TransMultVector(-1., *d, 1., *d_viol_L);
+ SmartPtr<Vector> tmp = d_viol_L->MakeNew();
+ tmp->Set(0.);
+ d_viol_L->ElementWiseMax(*tmp);
+ DBG_PRINT_VECTOR(2, "d_viol_L", *d_viol_L);
+
+ SmartPtr<Vector> d_viol_U = ip_nlp_->d_U()->MakeNewCopy();
+ ip_nlp_->Pd_U()->TransMultVector(-1., *d, 1., *d_viol_U);
+ tmp = d_viol_U->MakeNew();
+ tmp->Set(0.);
+ d_viol_U->ElementWiseMin(*tmp);
+ DBG_PRINT_VECTOR(2, "d_viol_U", *d_viol_U);
+
+ std::vector<SmartPtr<const Vector> > vecs(3);
+ vecs[0] = c;
+ vecs[1] = GetRawPtr(d_viol_L);
+ vecs[2] = GetRawPtr(d_viol_U);
+ result = CalcNormOfType(NormType, vecs);
+ curr_nlp_constraint_violation_cache_.AddCachedResult(result, deps, sdeps);
+ }
+
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::unscaled_curr_nlp_constraint_violation
+ (ENormType NormType)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::unscaled_curr_nlp_constraint_violation()",
+ dbg_verbosity);
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+
+ std::vector<const TaggedObject*> deps(1);
+ deps[0] = GetRawPtr(x);
+ std::vector<Number> sdeps(1);
+ sdeps[0] = (Number)NormType;
+
+ if (!unscaled_curr_nlp_constraint_violation_cache_.GetCachedResult(result, deps, sdeps)) {
+ SmartPtr<const Vector> c = unscaled_curr_c();
+
+ SmartPtr<const Vector> d = curr_d();
+
+ SmartPtr<Vector> d_viol_L = ip_nlp_->d_L()->MakeNewCopy();
+ ip_nlp_->Pd_L()->TransMultVector(-1., *d, 1., *d_viol_L);
+ SmartPtr<Vector> tmp = d_viol_L->MakeNew();
+ tmp->Set(0.);
+ d_viol_L->ElementWiseMax(*tmp);
+ DBG_PRINT_VECTOR(2, "d_viol_L", *d_viol_L);
+
+ SmartPtr<Vector> d_viol_U = ip_nlp_->d_U()->MakeNewCopy();
+ ip_nlp_->Pd_U()->TransMultVector(-1., *d, 1., *d_viol_U);
+ tmp = d_viol_U->MakeNew();
+ tmp->Set(0.);
+ d_viol_U->ElementWiseMin(*tmp);
+ DBG_PRINT_VECTOR(2, "d_viol_U", *d_viol_U);
+
+ std::vector<SmartPtr<const Vector> > vecs(3);
+ vecs[0] = c;
+ vecs[1] = GetRawPtr(d_viol_L);
+ vecs[2] = GetRawPtr(d_viol_U);
+ result = CalcNormOfType(NormType, vecs);
+ unscaled_curr_nlp_constraint_violation_cache_.AddCachedResult(result, deps, sdeps);
+ }
+
+ return result;
+ }
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Exact Hessian using second derivatives //
+ ///////////////////////////////////////////////////////////////////////////
+
+ SmartPtr<const SymMatrix>
+ IpoptCalculatedQuantities::curr_exact_hessian()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_exact_hessian()",
+ dbg_verbosity);
+
+ SmartPtr<const SymMatrix> result;
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> y_c = ip_data_->curr()->y_c();
+ SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
+
+ bool objective_depends_on_mu = ip_nlp_->objective_depends_on_mu();
+ std::vector<const TaggedObject*> tdeps(3);
+ tdeps[0] = GetRawPtr(x);
+ tdeps[1] = GetRawPtr(y_c);
+ tdeps[2] = GetRawPtr(y_d);
+ std::vector<Number> sdeps(1);
+ if (objective_depends_on_mu) {
+ sdeps[0] = ip_data_->curr_mu();
+ }
+ else {
+ sdeps[0] = -1.;
+ }
+
+ if (!curr_exact_hessian_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ if (objective_depends_on_mu) {
+ result = ip_nlp_->h(*x, 1.0, *y_c, *y_d, ip_data_->curr_mu());
+ }
+ else {
+ result = ip_nlp_->h(*x, 1.0, *y_c, *y_d);
+ }
+ curr_exact_hessian_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+
+ return result;
+ }
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Optimality Error and its components //
+ ///////////////////////////////////////////////////////////////////////////
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_grad_lag_x()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_grad_lag_x()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> y_c = ip_data_->curr()->y_c();
+ SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
+ SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
+ SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
+
+ std::vector<const TaggedObject*> deps(5);
+ deps[0] = GetRawPtr(x);
+ deps[1] = GetRawPtr(y_c);
+ deps[2] = GetRawPtr(y_d);
+ deps[3] = GetRawPtr(z_L);
+ deps[4] = GetRawPtr(z_U);
+
+ if (!curr_grad_lag_x_cache_.GetCachedResult(result, deps)) {
+ if (!trial_grad_lag_x_cache_.GetCachedResult(result, deps)) {
+ SmartPtr<Vector> tmp = x->MakeNew();
+ DBG_PRINT_VECTOR(2,"curr_grad_f",*curr_grad_f());
+ tmp->Copy(*curr_grad_f());
+ tmp->AddTwoVectors(1., *curr_jac_cT_times_curr_y_c(),
+ 1., *curr_jac_dT_times_curr_y_d(), 1.);
+ DBG_PRINT_VECTOR(2,"jac_cT*y_c",*curr_jac_cT_times_curr_y_c());
+ DBG_PRINT_VECTOR(2,"jac_dT*y_d",*curr_jac_dT_times_curr_y_d());
+ ip_nlp_->Px_L()->MultVector(-1., *z_L, 1., *tmp);
+ ip_nlp_->Px_U()->MultVector(1., *z_U, 1., *tmp);
+ result = ConstPtr(tmp);
+ }
+ curr_grad_lag_x_cache_.AddCachedResult(result, deps);
+ }
+
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::trial_grad_lag_x()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_grad_lag_x()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> x = ip_data_->trial()->x();
+ SmartPtr<const Vector> y_c = ip_data_->trial()->y_c();
+ SmartPtr<const Vector> y_d = ip_data_->trial()->y_d();
+ SmartPtr<const Vector> z_L = ip_data_->trial()->z_L();
+ SmartPtr<const Vector> z_U = ip_data_->trial()->z_U();
+
+ std::vector<const TaggedObject*> deps(5);
+ deps[0] = GetRawPtr(x);
+ deps[1] = GetRawPtr(y_c);
+ deps[2] = GetRawPtr(y_d);
+ deps[3] = GetRawPtr(z_L);
+ deps[4] = GetRawPtr(z_U);
+
+ if (!trial_grad_lag_x_cache_.GetCachedResult(result, deps)) {
+ if (!curr_grad_lag_x_cache_.GetCachedResult(result, deps)) {
+ SmartPtr<Vector> tmp = x->MakeNew();
+ DBG_PRINT_VECTOR(2,"trial_grad_f",*trial_grad_f());
+ tmp->Copy(*trial_grad_f());
+ tmp->AddTwoVectors(1., *trial_jac_cT_times_trial_y_c(),
+ 1., *trial_jac_dT_times_trial_y_d(), 1.);
+ ip_nlp_->Px_L()->MultVector(-1., *z_L, 1., *tmp);
+ ip_nlp_->Px_U()->MultVector(1., *z_U, 1., *tmp);
+ result = ConstPtr(tmp);
+ }
+ trial_grad_lag_x_cache_.AddCachedResult(result, deps);
+ }
+
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_grad_lag_s()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_grad_lag_s()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
+ SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
+ SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();
+
+ std::vector<const TaggedObject*> deps(3);
+ deps[0] = GetRawPtr(y_d);
+ deps[1] = GetRawPtr(v_L);
+ deps[2] = GetRawPtr(v_U);
+
+ if (!curr_grad_lag_s_cache_.GetCachedResult(result, deps)) {
+ if (!trial_grad_lag_s_cache_.GetCachedResult(result, deps)) {
+ SmartPtr<Vector> tmp = y_d->MakeNew();
+ ip_nlp_->Pd_U()->MultVector(1., *v_U, 0., *tmp);
+ ip_nlp_->Pd_L()->MultVector(-1., *v_L, 1., *tmp);
+ tmp->Axpy(-1., *y_d);
+ result = ConstPtr(tmp);
+ }
+ curr_grad_lag_s_cache_.AddCachedResult(result, deps);
+ }
+
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::trial_grad_lag_s()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_grad_lag_s()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> y_d = ip_data_->trial()->y_d();
+ SmartPtr<const Vector> v_L = ip_data_->trial()->v_L();
+ SmartPtr<const Vector> v_U = ip_data_->trial()->v_U();
+
+ std::vector<const TaggedObject*> deps(3);
+ deps[0] = GetRawPtr(y_d);
+ deps[1] = GetRawPtr(v_L);
+ deps[2] = GetRawPtr(v_U);
+
+ if (!trial_grad_lag_s_cache_.GetCachedResult(result, deps)) {
+ if (!curr_grad_lag_s_cache_.GetCachedResult(result, deps)) {
+ SmartPtr<Vector> tmp = y_d->MakeNew();
+ ip_nlp_->Pd_U()->MultVector(1., *v_U, 0., *tmp);
+ ip_nlp_->Pd_L()->MultVector(-1., *v_L, 1., *tmp);
+ tmp->Axpy(-1., *y_d);
+ result = ConstPtr(tmp);
+ }
+ trial_grad_lag_s_cache_.AddCachedResult(result, deps);
+ }
+
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_grad_lag_with_damping_x()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_grad_lag_with_damping_x()",
+ dbg_verbosity);
+
+ /* If no damping is used, just return the gradient of the regular
+ Lagrangian function */
+ if (kappa_d_==0.) {
+ return curr_grad_lag_x();
+ }
+
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> y_c = ip_data_->curr()->y_c();
+ SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
+ SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
+ SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
+ Number mu = ip_data_->curr_mu();
+
+ std::vector<const TaggedObject*> deps(5);
+ deps[0] = GetRawPtr(x);
+ deps[1] = GetRawPtr(y_c);
+ deps[2] = GetRawPtr(y_d);
+ deps[3] = GetRawPtr(z_L);
+ deps[4] = GetRawPtr(z_U);
+ std::vector<Number> sdeps(1);
+ sdeps[0] = mu;
+
+ if (!curr_grad_lag_with_damping_x_cache_.GetCachedResult(result, deps, sdeps)) {
+ SmartPtr<Vector> tmp = x->MakeNew();
+ tmp->Copy(*curr_grad_lag_x());
+
+ SmartPtr<const Vector> dampind_x_L;
+ SmartPtr<const Vector> dampind_x_U;
+ SmartPtr<const Vector> dampind_s_L;
+ SmartPtr<const Vector> dampind_s_U;
+ ComputeDampingIndicators(dampind_x_L, dampind_x_U, dampind_s_L, dampind_s_U);
+
+ ip_nlp_->Px_L()->MultVector(kappa_d_*mu, *dampind_x_L, 1., *tmp);
+ ip_nlp_->Px_U()->MultVector(-kappa_d_*mu, *dampind_x_U, 1., *tmp);
+
+ result = ConstPtr(tmp);
+ curr_grad_lag_with_damping_x_cache_.AddCachedResult(result, deps, sdeps);
+ }
+
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_grad_lag_with_damping_s()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_grad_lag_with_damping_s()",
+ dbg_verbosity);
+
+ /* If no damping is used, just return the gradient of the regular
+ Lagrangian function */
+ if (kappa_d_==0.) {
+ return curr_grad_lag_s();
+ }
+
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
+ SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
+ SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();
+ Number mu = ip_data_->curr_mu();
+
+ std::vector<const TaggedObject*> deps(3);
+ deps[0] = GetRawPtr(y_d);
+ deps[1] = GetRawPtr(v_L);
+ deps[2] = GetRawPtr(v_U);
+ std::vector<Number> sdeps(1);
+ sdeps[0] = mu;
+
+ if (!curr_grad_lag_with_damping_s_cache_.GetCachedResult(result, deps, sdeps)) {
+ SmartPtr<Vector> tmp = y_d->MakeNew();
+ tmp->Copy(*curr_grad_lag_s());
+
+ SmartPtr<const Vector> dampind_x_L;
+ SmartPtr<const Vector> dampind_x_U;
+ SmartPtr<const Vector> dampind_s_L;
+ SmartPtr<const Vector> dampind_s_U;
+ ComputeDampingIndicators(dampind_x_L, dampind_x_U, dampind_s_L, dampind_s_U);
+
+ ip_nlp_->Pd_L()->MultVector(kappa_d_*mu, *dampind_s_L, 1., *tmp);
+ ip_nlp_->Pd_U()->MultVector(-kappa_d_*mu, *dampind_s_U, 1., *tmp);
+
+ result = ConstPtr(tmp);
+ curr_grad_lag_with_damping_s_cache_.AddCachedResult(result, deps, sdeps);
+ }
+
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::CalcCompl(const Vector& slack,
+ const Vector& mult)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::CalcCompl()",
+ dbg_verbosity);
+ SmartPtr<Vector> result = slack.MakeNew();
+ result->Copy(slack);
+ result->ElementWiseMultiply(mult);
+ return ConstPtr(result);
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_compl_x_L()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_compl_x_L()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> slack = curr_slack_x_L();
+ SmartPtr<const Vector> mult = ip_data_->curr()->z_L();
+ DBG_PRINT_VECTOR(2, "slack_x_L", *slack);
+ DBG_PRINT_VECTOR(2, "z_L", *mult);
+
+ if (!curr_compl_x_L_cache_.GetCachedResult2Dep(result, *slack, *mult)) {
+ if (!trial_compl_x_L_cache_.GetCachedResult2Dep(result, *slack, *mult)) {
+ result = CalcCompl(*slack, *mult);
+ }
+ curr_compl_x_L_cache_.AddCachedResult2Dep(result, *slack, *mult);
+ }
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::trial_compl_x_L()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_compl_x_L()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> slack = trial_slack_x_L();
+ SmartPtr<const Vector> mult = ip_data_->trial()->z_L();
+ DBG_PRINT_VECTOR(2, "slack_x_L", *slack);
+ DBG_PRINT_VECTOR(2, "z_L", *mult);
+
+ if (!trial_compl_x_L_cache_.GetCachedResult2Dep(result, *slack, *mult)) {
+ if (!curr_compl_x_L_cache_.GetCachedResult2Dep(result, *slack, *mult)) {
+ result = CalcCompl(*slack, *mult);
+ }
+ trial_compl_x_L_cache_.AddCachedResult2Dep(result, *slack, *mult);
+ }
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_compl_x_U()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_compl_x_U()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> slack = curr_slack_x_U();
+ SmartPtr<const Vector> mult = ip_data_->curr()->z_U();
+
+ if (!curr_compl_x_U_cache_.GetCachedResult2Dep(result, *slack, *mult)) {
+ if (!trial_compl_x_U_cache_.GetCachedResult2Dep(result, *slack, *mult)) {
+ result = CalcCompl(*slack, *mult);
+ }
+ curr_compl_x_U_cache_.AddCachedResult2Dep(result, *slack, *mult);
+ }
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::trial_compl_x_U()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_compl_x_U()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> slack = trial_slack_x_U();
+ SmartPtr<const Vector> mult = ip_data_->trial()->z_U();
+
+ if (!trial_compl_x_U_cache_.GetCachedResult2Dep(result, *slack, *mult)) {
+ if (!curr_compl_x_U_cache_.GetCachedResult2Dep(result, *slack, *mult)) {
+ result = CalcCompl(*slack, *mult);
+ }
+ trial_compl_x_U_cache_.AddCachedResult2Dep(result, *slack, *mult);
+ }
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_compl_s_L()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_compl_s_L()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> slack = curr_slack_s_L();
+ SmartPtr<const Vector> mult = ip_data_->curr()->v_L();
+
+ if (!curr_compl_s_L_cache_.GetCachedResult2Dep(result, *slack, *mult)) {
+ if (!trial_compl_s_L_cache_.GetCachedResult2Dep(result, *slack, *mult)) {
+ result = CalcCompl(*slack, *mult);
+ }
+ curr_compl_s_L_cache_.GetCachedResult2Dep(result, *slack, *mult);
+ }
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::trial_compl_s_L()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_compl_s_L()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> slack = trial_slack_s_L();
+ SmartPtr<const Vector> mult = ip_data_->trial()->v_L();
+
+ if (!trial_compl_s_L_cache_.GetCachedResult2Dep(result, *slack, *mult)) {
+ if (!curr_compl_s_L_cache_.GetCachedResult2Dep(result, *slack, *mult)) {
+ result = CalcCompl(*slack, *mult);
+ }
+ trial_compl_s_L_cache_.GetCachedResult2Dep(result, *slack, *mult);
+ }
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_compl_s_U()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_compl_s_U()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> slack = curr_slack_s_U();
+ SmartPtr<const Vector> mult = ip_data_->curr()->v_U();
+
+ if (!curr_compl_s_U_cache_.GetCachedResult2Dep(result, *slack, *mult)) {
+ if (!trial_compl_s_U_cache_.GetCachedResult2Dep(result, *slack, *mult)) {
+ result = CalcCompl(*slack, *mult);
+ }
+ curr_compl_s_U_cache_.GetCachedResult2Dep(result, *slack, *mult);
+ }
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::trial_compl_s_U()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_compl_s_U()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> slack = trial_slack_s_U();
+ SmartPtr<const Vector> mult = ip_data_->trial()->v_U();
+
+ if (!trial_compl_s_U_cache_.GetCachedResult2Dep(result, *slack, *mult)) {
+ if (!curr_compl_s_U_cache_.GetCachedResult2Dep(result, *slack, *mult)) {
+ result = CalcCompl(*slack, *mult);
+ }
+ trial_compl_s_U_cache_.GetCachedResult2Dep(result, *slack, *mult);
+ }
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_relaxed_compl_x_L()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_relaxed_compl_x_L()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> slack = curr_slack_x_L();
+ SmartPtr<const Vector> mult = ip_data_->curr()->z_L();
+ std::vector<const TaggedObject*> tdeps(2);
+ tdeps[0] = GetRawPtr(slack);
+ tdeps[1] = GetRawPtr(mult);
+
+ Number mu = ip_data_->curr_mu();
+ std::vector<Number> sdeps(1);
+ sdeps[0] = mu;
+
+ if (!curr_relaxed_compl_x_L_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ SmartPtr<Vector> tmp = slack->MakeNew();
+ tmp->Copy(*curr_compl_x_L());
+ tmp->AddScalar(-mu);
+ result = ConstPtr(tmp);
+ curr_relaxed_compl_x_L_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_relaxed_compl_x_U()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_relaxed_compl_x_U()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> slack = curr_slack_x_U();
+ SmartPtr<const Vector> mult = ip_data_->curr()->z_U();
+ std::vector<const TaggedObject*> tdeps(2);
+ tdeps[0] = GetRawPtr(slack);
+ tdeps[1] = GetRawPtr(mult);
+
+ Number mu = ip_data_->curr_mu();
+ std::vector<Number> sdeps(1);
+ sdeps[0] = mu;
+
+ if (!curr_relaxed_compl_x_U_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ SmartPtr<Vector> tmp = slack->MakeNew();
+ tmp->Copy(*curr_compl_x_U());
+ tmp->AddScalar(-mu);
+ result = ConstPtr(tmp);
+ curr_relaxed_compl_x_U_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_relaxed_compl_s_L()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_relaxed_compl_s_L()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> slack = curr_slack_s_L();
+ SmartPtr<const Vector> mult = ip_data_->curr()->v_L();
+ std::vector<const TaggedObject*> tdeps(2);
+ tdeps[0] = GetRawPtr(slack);
+ tdeps[1] = GetRawPtr(mult);
+
+ Number mu = ip_data_->curr_mu();
+ std::vector<Number> sdeps(1);
+ sdeps[0] = mu;
+
+ if (!curr_relaxed_compl_s_L_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ SmartPtr<Vector> tmp = slack->MakeNew();
+ tmp->Copy(*curr_compl_s_L());
+ tmp->AddScalar(-mu);
+ result = ConstPtr(tmp);
+ curr_relaxed_compl_s_L_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_relaxed_compl_s_U()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_relaxed_compl_s_U()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+
+ SmartPtr<const Vector> slack = curr_slack_s_U();
+ SmartPtr<const Vector> mult = ip_data_->curr()->v_U();
+ std::vector<const TaggedObject*> tdeps(2);
+ tdeps[0] = GetRawPtr(slack);
+ tdeps[1] = GetRawPtr(mult);
+
+ Number mu = ip_data_->curr_mu();
+ std::vector<Number> sdeps(1);
+ sdeps[0] = mu;
+
+ if (!curr_relaxed_compl_s_U_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ SmartPtr<Vector> tmp = slack->MakeNew();
+ tmp->Copy(*curr_compl_s_U());
+ tmp->AddScalar(-mu);
+ result = ConstPtr(tmp);
+ curr_relaxed_compl_s_U_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::CalcNormOfType
+ (ENormType NormType,
+ const Vector& vec1, const Vector& vec2)
+ {
+ switch (NormType) {
+ case NORM_1 :
+ return vec1.Asum() + vec2.Asum();
+ case NORM_2 :
+ return sqrt(pow(vec1.Nrm2(),2) + pow(vec2.Nrm2(),2));
+ case NORM_MAX :
+ return Max(vec1.Amax(), vec2.Amax());
+ default:
+ DBG_ASSERT(false && "Unknown NormType.");
+ return 0.0;
+ }
+ }
+
+ Number
+ IpoptCalculatedQuantities::CalcNormOfType
+ (ENormType NormType,
+ std::vector<SmartPtr<const Vector> > vecs)
+ {
+ Number result=0.;
+
+ switch (NormType) {
+ case NORM_1 :
+ for (Index i=0; i<(Index)vecs.size(); i++) {
+ result += vecs[i]->Asum();
+ }
+ break;
+ case NORM_2 :
+ for (Index i=0; i<(Index)vecs.size(); i++) {
+ Number nrm = vecs[i]->Nrm2();
+ result += nrm*nrm;
+ }
+ result = sqrt(result);
+ break;
+ case NORM_MAX :
+ for (Index i=0; i<(Index)vecs.size(); i++) {
+ result = Max(result, vecs[i]->Amax());
+ }
+ break;
+ default:
+ DBG_ASSERT(false && "Unknown NormType.");
+ }
+
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::curr_primal_infeasibility
+ (ENormType NormType)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_primal_infeasibility()",
+ dbg_verbosity);
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> s = ip_data_->curr()->s();
+
+ DBG_PRINT_VECTOR(2, "x to eval", *x);
+ DBG_PRINT_VECTOR(2, "s to eval", *s);
+ DBG_PRINT((1,"NormType = %d\n", NormType))
+
+ std::vector<const TaggedObject*> deps(2);
+ deps[0] = GetRawPtr(x);
+ deps[1] = GetRawPtr(s);
+ std::vector<Number> sdeps(1);
+ sdeps[0] = (Number)NormType;
+
+ if (!curr_primal_infeasibility_cache_.GetCachedResult(result, deps, sdeps)) {
+ if (!trial_primal_infeasibility_cache_.GetCachedResult(result, deps, sdeps)) {
+ DBG_PRINT((1,"Recomputing recomputing infeasibility.\n"));
+ SmartPtr<const Vector> c = curr_c();
+ SmartPtr<const Vector> d_minus_s = curr_d_minus_s();
+
+ DBG_PRINT_VECTOR(2,"c", *c);
+ DBG_PRINT_VECTOR(2,"d_minus_s", *d_minus_s);
+
+ result = CalcNormOfType(NormType, *c, *d_minus_s);
+
+ }
+ curr_primal_infeasibility_cache_.AddCachedResult(result, deps, sdeps);
+ }
+
+ DBG_PRINT((1,"result = %e\n",result));
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::trial_primal_infeasibility
+ (ENormType NormType)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_primal_infeasibility()",
+ dbg_verbosity);
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->trial()->x();
+ SmartPtr<const Vector> s = ip_data_->trial()->s();
+
+ DBG_PRINT_VECTOR(2, "x to eval", *x);
+ DBG_PRINT_VECTOR(2, "s to eval", *s);
+ DBG_PRINT((1,"NormType = %d\n", NormType))
+
+ std::vector<const TaggedObject*> deps(2);
+ deps[0] = GetRawPtr(x);
+ deps[1] = GetRawPtr(s);
+ std::vector<Number> sdeps(1);
+ sdeps[0] = (Number)NormType;
+
+ if (!trial_primal_infeasibility_cache_.GetCachedResult(result, deps, sdeps)) {
+ if (!curr_primal_infeasibility_cache_.GetCachedResult(result, deps, sdeps)) {
+ DBG_PRINT((1,"Recomputing recomputing infeasibility.\n"));
+ SmartPtr<const Vector> c = trial_c();
+ SmartPtr<const Vector> d_minus_s = trial_d_minus_s();
+
+ DBG_PRINT_VECTOR(2,"c", *c);
+ DBG_PRINT_VECTOR(2,"d_minus_s", *d_minus_s);
+
+ result = CalcNormOfType(NormType, *c, *d_minus_s);
+ }
+ trial_primal_infeasibility_cache_.AddCachedResult(result, deps, sdeps);
+ }
+
+ DBG_PRINT((1,"result = %e\n",result));
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::curr_dual_infeasibility
+ (ENormType NormType)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_dual_infeasibility()",
+ dbg_verbosity);
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> s = ip_data_->curr()->s();
+ SmartPtr<const Vector> y_c = ip_data_->curr()->y_c();
+ SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
+ SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
+ SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
+ SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
+ SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();
+
+ std::vector<const TaggedObject*> deps(8);
+ deps[0] = GetRawPtr(x);
+ deps[1] = GetRawPtr(s);
+ deps[2] = GetRawPtr(y_c);
+ deps[3] = GetRawPtr(y_d);
+ deps[4] = GetRawPtr(z_L);
+ deps[5] = GetRawPtr(z_U);
+ deps[6] = GetRawPtr(v_L);
+ deps[7] = GetRawPtr(v_U);
+ std::vector<Number> sdeps(1);
+ sdeps[0] = (Number)NormType;
+
+ if (!curr_dual_infeasibility_cache_.GetCachedResult(result, deps, sdeps)) {
+ if (!trial_dual_infeasibility_cache_.GetCachedResult(result, deps, sdeps)) {
+ SmartPtr<const Vector> grad_lag_x = curr_grad_lag_x();
+ SmartPtr<const Vector> grad_lag_s = curr_grad_lag_s();
+ DBG_PRINT_VECTOR(2,"grad_lag_x", *grad_lag_x);
+ DBG_PRINT_VECTOR(2,"grad_lag_s", *grad_lag_s);
+
+ result = CalcNormOfType(NormType, *grad_lag_x, *grad_lag_s);
+ }
+ curr_dual_infeasibility_cache_.AddCachedResult(result, deps, sdeps);
+ }
+
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::trial_dual_infeasibility
+ (ENormType NormType)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_dual_infeasibility()",
+ dbg_verbosity);
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->trial()->x();
+ SmartPtr<const Vector> s = ip_data_->trial()->s();
+ SmartPtr<const Vector> y_c = ip_data_->trial()->y_c();
+ SmartPtr<const Vector> y_d = ip_data_->trial()->y_d();
+ SmartPtr<const Vector> z_L = ip_data_->trial()->z_L();
+ SmartPtr<const Vector> z_U = ip_data_->trial()->z_U();
+ SmartPtr<const Vector> v_L = ip_data_->trial()->v_L();
+ SmartPtr<const Vector> v_U = ip_data_->trial()->v_U();
+
+ std::vector<const TaggedObject*> deps(8);
+ deps[0] = GetRawPtr(x);
+ deps[1] = GetRawPtr(s);
+ deps[2] = GetRawPtr(y_c);
+ deps[3] = GetRawPtr(y_d);
+ deps[4] = GetRawPtr(z_L);
+ deps[5] = GetRawPtr(z_U);
+ deps[6] = GetRawPtr(v_L);
+ deps[7] = GetRawPtr(v_U);
+ std::vector<Number> sdeps(1);
+ sdeps[0] = (Number)NormType;
+
+ if (!trial_dual_infeasibility_cache_.GetCachedResult(result, deps, sdeps)) {
+ if (!curr_dual_infeasibility_cache_.GetCachedResult(result, deps, sdeps)) {
+ SmartPtr<const Vector> grad_lag_x = trial_grad_lag_x();
+ SmartPtr<const Vector> grad_lag_s = trial_grad_lag_s();
+ DBG_PRINT_VECTOR(2,"grad_lag_x", *grad_lag_x);
+ DBG_PRINT_VECTOR(2,"grad_lag_s", *grad_lag_s);
+
+ result = CalcNormOfType(NormType, *grad_lag_x, *grad_lag_s);
+
+ }
+ trial_dual_infeasibility_cache_.AddCachedResult(result, deps, sdeps);
+ }
+
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::unscaled_curr_dual_infeasibility
+ (ENormType NormType)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::unscaled_curr_dual_infeasibility()",
+ dbg_verbosity);
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> s = ip_data_->curr()->s();
+ SmartPtr<const Vector> y_c = ip_data_->curr()->y_c();
+ SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
+ SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
+ SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
+ SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
+ SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();
+
+ std::vector<const TaggedObject*> deps(8);
+ deps[0] = GetRawPtr(x);
+ deps[1] = GetRawPtr(s);
+ deps[2] = GetRawPtr(y_c);
+ deps[3] = GetRawPtr(y_d);
+ deps[4] = GetRawPtr(z_L);
+ deps[5] = GetRawPtr(z_U);
+ deps[6] = GetRawPtr(v_L);
+ deps[7] = GetRawPtr(v_U);
+ std::vector<Number> sdeps(1);
+ sdeps[0] = (Number)NormType;
+
+ if (!unscaled_curr_dual_infeasibility_cache_.GetCachedResult(result, deps, sdeps)) {
+ SmartPtr<const Vector> grad_lag_x =
+ ip_nlp_->NLP_scaling()->unapply_grad_obj_scaling(curr_grad_lag_x());
+
+ Number obj_unscal = ip_nlp_->NLP_scaling()->unapply_obj_scaling(1.);
+ SmartPtr<const Vector> grad_lag_s;
+ if (obj_unscal != 1.) {
+ SmartPtr<Vector> tmp =
+ ip_nlp_->NLP_scaling()->apply_vector_scaling_d_NonConst(ConstPtr(curr_grad_lag_s()));
+ tmp->Scal(obj_unscal);
+ grad_lag_s = ConstPtr(tmp);
+ }
+ else {
+ grad_lag_s = ip_nlp_->NLP_scaling()->apply_vector_scaling_d(curr_grad_lag_s());
+ }
+
+ result = CalcNormOfType(NormType, *grad_lag_x, *grad_lag_s);
+ unscaled_curr_dual_infeasibility_cache_.AddCachedResult(result, deps, sdeps);
+ }
+
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::curr_complementarity
+ (Number mu, ENormType NormType)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_complementarity()",
+ dbg_verbosity);
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> s = ip_data_->curr()->s();
+ SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
+ SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
+ SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
+ SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();
+
+ std::vector<const TaggedObject*> deps(6);
+ deps[0] = GetRawPtr(x);
+ deps[1] = GetRawPtr(s);
+ deps[2] = GetRawPtr(z_L);
+ deps[3] = GetRawPtr(z_U);
+ deps[4] = GetRawPtr(v_L);
+ deps[5] = GetRawPtr(v_U);
+ std::vector<Number> sdeps(2);
+ sdeps[0] = (Number)NormType;
+ sdeps[1] = mu;
+
+ if (!curr_complementarity_cache_.GetCachedResult(result, deps, sdeps)) {
+ if (!trial_complementarity_cache_.GetCachedResult(result, deps, sdeps)) {
+
+ std::vector<SmartPtr<const Vector> > vecs(4);
+ SmartPtr<const Vector> compl_x_L = curr_compl_x_L();
+ SmartPtr<const Vector> compl_x_U = curr_compl_x_U();
+ SmartPtr<const Vector> compl_s_L = curr_compl_s_L();
+ SmartPtr<const Vector> compl_s_U = curr_compl_s_U();
+
+ if (mu==.0) {
+ vecs[0] = GetRawPtr(compl_x_L);
+ vecs[1] = GetRawPtr(compl_x_U);
+ vecs[2] = GetRawPtr(compl_s_L);
+ vecs[3] = GetRawPtr(compl_s_U);
+ }
+ else {
+ SmartPtr<Vector> tmp = compl_x_L->MakeNew();
+ tmp->Copy(*compl_x_L);
+ tmp->AddScalar(-mu);
+ vecs[0] = GetRawPtr(tmp);
+ tmp = compl_x_U->MakeNew();
+ tmp->Copy(*compl_x_U);
+ tmp->AddScalar(-mu);
+ vecs[1] = GetRawPtr(tmp);
+ tmp = compl_s_L->MakeNew();
+ tmp->Copy(*compl_s_L);
+ tmp->AddScalar(-mu);
+ vecs[2] = GetRawPtr(tmp);
+ tmp = compl_s_U->MakeNew();
+ tmp->Copy(*compl_s_U);
+ tmp->AddScalar(-mu);
+ vecs[3] = GetRawPtr(tmp);
+ }
+
+ result = CalcNormOfType(NormType, vecs);
+ }
+
+ curr_complementarity_cache_.AddCachedResult(result, deps, sdeps);
+ }
+
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::trial_complementarity
+ (Number mu, ENormType NormType)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_complementarity()",
+ dbg_verbosity);
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->trial()->x();
+ SmartPtr<const Vector> s = ip_data_->trial()->s();
+ SmartPtr<const Vector> z_L = ip_data_->trial()->z_L();
+ SmartPtr<const Vector> z_U = ip_data_->trial()->z_U();
+ SmartPtr<const Vector> v_L = ip_data_->trial()->v_L();
+ SmartPtr<const Vector> v_U = ip_data_->trial()->v_U();
+
+ std::vector<const TaggedObject*> deps(6);
+ deps[0] = GetRawPtr(x);
+ deps[1] = GetRawPtr(s);
+ deps[2] = GetRawPtr(z_L);
+ deps[3] = GetRawPtr(z_U);
+ deps[4] = GetRawPtr(v_L);
+ deps[5] = GetRawPtr(v_U);
+ std::vector<Number> sdeps(2);
+ sdeps[0] = (Number)NormType;
+ sdeps[1] = mu;
+
+ if (!trial_complementarity_cache_.GetCachedResult(result, deps, sdeps)) {
+ if (!curr_complementarity_cache_.GetCachedResult(result, deps, sdeps)) {
+
+ std::vector<SmartPtr<const Vector> > vecs(4);
+ SmartPtr<const Vector> compl_x_L = trial_compl_x_L();
+ SmartPtr<const Vector> compl_x_U = trial_compl_x_U();
+ SmartPtr<const Vector> compl_s_L = trial_compl_s_L();
+ SmartPtr<const Vector> compl_s_U = trial_compl_s_U();
+
+ if (mu==.0) {
+ vecs[0] = GetRawPtr(compl_x_L);
+ vecs[1] = GetRawPtr(compl_x_U);
+ vecs[2] = GetRawPtr(compl_s_L);
+ vecs[3] = GetRawPtr(compl_s_U);
+ }
+ else {
+ SmartPtr<Vector> tmp = compl_x_L->MakeNew();
+ tmp->Copy(*compl_x_L);
+ tmp->AddScalar(-mu);
+ vecs[0] = GetRawPtr(tmp);
+ tmp = compl_x_U->MakeNew();
+ tmp->Copy(*compl_x_U);
+ tmp->AddScalar(-mu);
+ vecs[1] = GetRawPtr(tmp);
+ tmp = compl_s_L->MakeNew();
+ tmp->Copy(*compl_s_L);
+ tmp->AddScalar(-mu);
+ vecs[2] = GetRawPtr(tmp);
+ tmp = compl_s_U->MakeNew();
+ tmp->Copy(*compl_s_U);
+ tmp->AddScalar(-mu);
+ vecs[3] = GetRawPtr(tmp);
+ }
+
+ result = CalcNormOfType(NormType, vecs);
+ }
+
+ trial_complementarity_cache_.AddCachedResult(result, deps, sdeps);
+ }
+
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::unscaled_curr_complementarity
+ (Number mu, ENormType NormType)
+ {
+ return ip_nlp_->NLP_scaling()->unapply_obj_scaling(curr_complementarity(mu, NormType));
+ }
+
+ Number
+ IpoptCalculatedQuantities::CalcCentralityMeasure(const Vector& compl_x_L,
+ const Vector& compl_x_U,
+ const Vector& compl_s_L,
+ const Vector& compl_s_U)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::CalcCentralityMeasure()",
+ dbg_verbosity);
+
+ Number MinCompl = std::numeric_limits<Number>::max();
+ bool have_bounds = false;
+
+ Index n_compl_x_L = compl_x_L.Dim();
+ Index n_compl_x_U = compl_x_U.Dim();
+ Index n_compl_s_L = compl_s_L.Dim();
+ Index n_compl_s_U = compl_s_U.Dim();
+
+ // Compute the Minimum of all complementarities
+ if( n_compl_x_L>0 ) {
+ if( have_bounds ) {
+ MinCompl = Min(MinCompl, compl_x_L.Min());
+ }
+ else {
+ MinCompl = compl_x_L.Min();
+ }
+ have_bounds = true;
+ }
+ if( n_compl_x_U>0 ) {
+ if( have_bounds ) {
+ MinCompl = Min(MinCompl, compl_x_U.Min());
+ }
+ else {
+ MinCompl = compl_x_U.Min();
+ }
+ have_bounds = true;
+ }
+ if( n_compl_s_L>0 ) {
+ if( have_bounds ) {
+ MinCompl = Min(MinCompl, compl_s_L.Min());
+ }
+ else {
+ MinCompl = compl_s_L.Min();
+ }
+ have_bounds = true;
+ }
+ if( n_compl_s_U>0 ) {
+ if( have_bounds ) {
+ MinCompl = Min(MinCompl, compl_s_U.Min());
+ }
+ else {
+ MinCompl = compl_s_U.Min();
+ }
+ have_bounds = true;
+ }
+
+ // If there are no bounds, just return 0.;
+ if (!have_bounds) {
+ return 0.;
+ }
+
+ DBG_PRINT_VECTOR(2, "compl_x_L", compl_x_L);
+ DBG_PRINT_VECTOR(2, "compl_x_U", compl_x_U);
+ DBG_PRINT_VECTOR(2, "compl_s_L", compl_s_L);
+ DBG_PRINT_VECTOR(2, "compl_s_U", compl_s_U);
+
+ DBG_ASSERT(MinCompl>0. && "There is a zero complementarity entry");
+
+ Number avrg_compl = (compl_x_L.Asum() + compl_x_U.Asum() +
+ compl_s_L.Asum() + compl_s_U.Asum());
+ DBG_PRINT((1,"sum_compl = %25.16e\n", avrg_compl));
+ avrg_compl /= (n_compl_x_L + n_compl_x_U + n_compl_s_L + n_compl_s_U);
+ DBG_PRINT((1,"avrg_compl = %25.16e\n", avrg_compl));
+ DBG_PRINT((1,"MinCompl = %25.16e\n", MinCompl));
+
+ Number xi = MinCompl/avrg_compl;
+ // The folloking line added for the case that avrg_compl is
+ // slighly smaller than MinCompl, due to numerical roundoff
+ xi = Min(1., xi);
+
+ return xi;
+ }
+
+ Number
+ IpoptCalculatedQuantities::curr_centrality_measure()
+ {
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> s = ip_data_->curr()->s();
+ SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
+ SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
+ SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
+ SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();
+
+ std::vector<const TaggedObject*> tdeps(6);
+ tdeps[0] = GetRawPtr(x);
+ tdeps[1] = GetRawPtr(s);
+ tdeps[2] = GetRawPtr(z_L);
+ tdeps[3] = GetRawPtr(z_U);
+ tdeps[4] = GetRawPtr(v_L);
+ tdeps[5] = GetRawPtr(v_U);
+
+ if (!curr_centrality_measure_cache_.GetCachedResult(result, tdeps)) {
+ SmartPtr<const Vector> compl_x_L = curr_compl_x_L();
+ SmartPtr<const Vector> compl_x_U = curr_compl_x_U();
+ SmartPtr<const Vector> compl_s_L = curr_compl_s_L();
+ SmartPtr<const Vector> compl_s_U = curr_compl_s_U();
+
+ result = CalcCentralityMeasure(*compl_x_L, *compl_x_U,
+ *compl_s_L, *compl_s_U);
+
+ curr_centrality_measure_cache_.AddCachedResult(result, tdeps);
+ }
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::curr_nlp_error()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_nlp_error()",
+ dbg_verbosity);
+ DBG_ASSERT(initialize_called_);
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> s = ip_data_->curr()->s();
+ SmartPtr<const Vector> y_c = ip_data_->curr()->y_c();
+ SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
+ SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
+ SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
+ SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
+ SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();
+
+ std::vector<const TaggedObject*> tdeps(8);
+ tdeps[0] = GetRawPtr(x);
+ tdeps[1] = GetRawPtr(s);
+ tdeps[2] = GetRawPtr(y_c);
+ tdeps[3] = GetRawPtr(y_d);
+ tdeps[4] = GetRawPtr(z_L);
+ tdeps[5] = GetRawPtr(z_U);
+ tdeps[6] = GetRawPtr(v_L);
+ tdeps[7] = GetRawPtr(v_U);
+
+ if (!curr_nlp_error_cache_.GetCachedResult(result, tdeps)) {
+ if (ip_data_->curr()->x()->Dim()==ip_data_->curr()->y_c()->Dim()) {
+ // This is a square problem, we only need to consider the
+ // infeasibility
+ result = curr_nlp_constraint_violation(NORM_MAX);
+ }
+ else {
+ Number s_d = 0;
+ Number s_c = 0;
+ ComputeOptimalityErrorScaling(*ip_data_->curr()->y_c(), *ip_data_->curr()->y_d(),
+ *ip_data_->curr()->z_L(), *ip_data_->curr()->z_U(),
+ *ip_data_->curr()->v_L(), *ip_data_->curr()->v_U(),
+ s_max_,
+ s_d, s_c);
+ DBG_PRINT((1, "s_d = %lf, s_c = %lf\n", s_d, s_c));
+
+ // Dual infeasibility
+ DBG_PRINT((1, "curr_dual_infeasibility(NORM_MAX) = %8.2e\n",
+ curr_dual_infeasibility(NORM_MAX)));
+ result = curr_dual_infeasibility(NORM_MAX)/s_d;
+ /*
+ // Primal infeasibility
+ DBG_PRINT((1, "curr_primal_infeasibility(NORM_MAX) = %8.2e\n",
+ curr_primal_infeasibility(NORM_MAX)));
+ result = Max(result, curr_primal_infeasibility(NORM_MAX));
+ */
+ result = Max(result, curr_nlp_constraint_violation(NORM_MAX));
+ // Complementarity
+ DBG_PRINT((1, "curr_complementarity(0., NORM_MAX) = %8.2e\n",
+ curr_complementarity(0., NORM_MAX)));
+ result = Max(result, curr_complementarity(0., NORM_MAX)/s_c);
+ }
+
+ curr_nlp_error_cache_.AddCachedResult(result, tdeps);
+ }
+
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::unscaled_curr_nlp_error()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::unscaled_curr_nlp_error()",
+ dbg_verbosity);
+ DBG_ASSERT(initialize_called_);
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> s = ip_data_->curr()->s();
+ SmartPtr<const Vector> y_c = ip_data_->curr()->y_c();
+ SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
+ SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
+ SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
+ SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
+ SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();
+
+ std::vector<const TaggedObject*> tdeps(8);
+ tdeps[0] = GetRawPtr(x);
+ tdeps[1] = GetRawPtr(s);
+ tdeps[2] = GetRawPtr(y_c);
+ tdeps[3] = GetRawPtr(y_d);
+ tdeps[4] = GetRawPtr(z_L);
+ tdeps[5] = GetRawPtr(z_U);
+ tdeps[6] = GetRawPtr(v_L);
+ tdeps[7] = GetRawPtr(v_U);
+
+ if (!unscaled_curr_nlp_error_cache_.GetCachedResult(result, tdeps)) {
+
+ // Dual infeasibility
+ result = unscaled_curr_dual_infeasibility(NORM_MAX);
+ // Constraint violation
+ result = Max(result, unscaled_curr_nlp_constraint_violation(NORM_MAX));
+ // Complementarity (ToDo use unscaled?)
+ DBG_PRINT((1, "curr_complementarity(0., NORM_MAX) = %8.2e\n",
+ curr_complementarity(0., NORM_MAX)));
+ result = Max(result, unscaled_curr_complementarity(0., NORM_MAX));
+
+ unscaled_curr_nlp_error_cache_.AddCachedResult(result, tdeps);
+ }
+
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::curr_barrier_error()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_barrier_error()",
+ dbg_verbosity);
+ DBG_ASSERT(initialize_called_);
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> s = ip_data_->curr()->s();
+ SmartPtr<const Vector> y_c = ip_data_->curr()->y_c();
+ SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
+ SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
+ SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
+ SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
+ SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();
+ Number mu = ip_data_->curr_mu();
+
+ std::vector<const TaggedObject*> tdeps(8);
+ tdeps[0] = GetRawPtr(x);
+ tdeps[1] = GetRawPtr(s);
+ tdeps[2] = GetRawPtr(y_c);
+ tdeps[3] = GetRawPtr(y_d);
+ tdeps[4] = GetRawPtr(z_L);
+ tdeps[5] = GetRawPtr(z_U);
+ tdeps[6] = GetRawPtr(v_L);
+ tdeps[7] = GetRawPtr(v_U);
+ std::vector<Number> sdeps(1);
+ sdeps[0] = mu;
+
+ if (!curr_barrier_error_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ Number s_d = 0;
+ Number s_c = 0;
+ ComputeOptimalityErrorScaling(*ip_data_->curr()->y_c(), *ip_data_->curr()->y_d(),
+ *ip_data_->curr()->z_L(), *ip_data_->curr()->z_U(),
+ *ip_data_->curr()->v_L(), *ip_data_->curr()->v_U(),
+ s_max_,
+ s_d, s_c);
+ DBG_PRINT((1, "s_d = %lf, s_c = %lf\n", s_d, s_c));
+
+ // Primal infeasibility
+ result = curr_dual_infeasibility(NORM_MAX)/s_d;
+ // Dual infeasibility
+ result = Max(result, curr_primal_infeasibility(NORM_MAX));
+ // Complementarity
+ result = Max(result, curr_complementarity(mu, NORM_MAX)/s_c);
+
+ curr_barrier_error_cache_.AddCachedResult(result, tdeps);
+ }
+
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::curr_primal_dual_system_error(Number mu)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_primal_dual_system_error()",
+ dbg_verbosity);
+ DBG_ASSERT(initialize_called_);
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> s = ip_data_->curr()->s();
+ SmartPtr<const Vector> y_c = ip_data_->curr()->y_c();
+ SmartPtr<const Vector> y_d = ip_data_->curr()->y_d();
+ SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
+ SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
+ SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
+ SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();
+
+ std::vector<const TaggedObject*> tdeps(8);
+ tdeps[0] = GetRawPtr(x);
+ tdeps[1] = GetRawPtr(s);
+ tdeps[2] = GetRawPtr(y_c);
+ tdeps[3] = GetRawPtr(y_d);
+ tdeps[4] = GetRawPtr(z_L);
+ tdeps[5] = GetRawPtr(z_U);
+ tdeps[6] = GetRawPtr(v_L);
+ tdeps[7] = GetRawPtr(v_U);
+ std::vector<Number> sdeps(1);
+ sdeps[0] = mu;
+
+ if (!curr_primal_dual_system_error_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ if (!trial_primal_dual_system_error_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ // For now we use the 1 norm, and scale each component by the number of entries...
+ Index n_dual = x->Dim() + s->Dim();
+ Number dual_inf = curr_dual_infeasibility(NORM_1)/((Number)n_dual);
+
+ Index n_primal = y_c->Dim() + y_d->Dim();
+ Number primal_inf = 0.;
+ if (n_primal>0) {
+ primal_inf = curr_primal_infeasibility(NORM_1)/((Number)n_primal);
+ }
+
+ Index n_cmpl = z_L->Dim() + z_U->Dim() + v_L->Dim() + v_U->Dim();
+ Number cmpl = 0.;
+ if (n_cmpl>0) {
+ cmpl = curr_complementarity(mu, NORM_1)/((Number)n_cmpl);
+ }
+
+ result = dual_inf + primal_inf + cmpl;
+ }
+ curr_primal_dual_system_error_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::trial_primal_dual_system_error(Number mu)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_primal_dual_system_error()",
+ dbg_verbosity);
+ DBG_ASSERT(initialize_called_);
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->trial()->x();
+ SmartPtr<const Vector> s = ip_data_->trial()->s();
+ SmartPtr<const Vector> y_c = ip_data_->trial()->y_c();
+ SmartPtr<const Vector> y_d = ip_data_->trial()->y_d();
+ SmartPtr<const Vector> z_L = ip_data_->trial()->z_L();
+ SmartPtr<const Vector> z_U = ip_data_->trial()->z_U();
+ SmartPtr<const Vector> v_L = ip_data_->trial()->v_L();
+ SmartPtr<const Vector> v_U = ip_data_->trial()->v_U();
+
+ std::vector<const TaggedObject*> tdeps(8);
+ tdeps[0] = GetRawPtr(x);
+ tdeps[1] = GetRawPtr(s);
+ tdeps[2] = GetRawPtr(y_c);
+ tdeps[3] = GetRawPtr(y_d);
+ tdeps[4] = GetRawPtr(z_L);
+ tdeps[5] = GetRawPtr(z_U);
+ tdeps[6] = GetRawPtr(v_L);
+ tdeps[7] = GetRawPtr(v_U);
+ std::vector<Number> sdeps(1);
+ sdeps[0] = mu;
+
+ if (!trial_primal_dual_system_error_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ if (!curr_primal_dual_system_error_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ // For now we use the 1 norm, and scale each component by the number of entries...
+ Index n_dual = x->Dim() + s->Dim();
+ Number dual_inf = trial_dual_infeasibility(NORM_1)/((Number)n_dual);
+
+ Index n_primal = y_c->Dim() + y_d->Dim();
+ Number primal_inf = 0.;
+ if (n_primal>0) {
+ primal_inf = trial_primal_infeasibility(NORM_1)/((Number)n_primal);
+ }
+
+ Index n_cmpl = z_L->Dim() + z_U->Dim() + v_L->Dim() + v_U->Dim();
+ Number cmpl = 0.;
+ if (n_cmpl>0) {
+ cmpl = trial_complementarity(mu, NORM_1)/((Number)n_cmpl);
+ }
+
+ result = dual_inf + primal_inf + cmpl;
+ }
+ trial_primal_dual_system_error_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+
+ return result;
+ }
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Fraction-to-the-boundary step sizes //
+ ///////////////////////////////////////////////////////////////////////////
+
+ Number
+ IpoptCalculatedQuantities::CalcFracToBound(const Vector& slack_L,
+ Vector& tmp_L,
+ const Matrix& P_L,
+ const Vector& slack_U,
+ Vector& tmp_U,
+ const Matrix& P_U,
+ const Vector& delta,
+ Number tau)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::CalcFracToBound",
+ dbg_verbosity);
+
+ Number alpha_L = 1.0;
+ Number alpha_U = 1.0;
+ if (slack_L.Dim() > 0) {
+ P_L.TransMultVector(1.0, delta, 0.0, tmp_L);
+ alpha_L = slack_L.FracToBound(tmp_L, tau);
+ }
+
+ if (slack_U.Dim() > 0) {
+ P_U.TransMultVector(-1.0, delta, 0.0, tmp_U);
+ alpha_U = slack_U.FracToBound(tmp_U, tau);
+ }
+
+ DBG_PRINT((1,"alpha_L = %lf, alpha_U = %lf\n", alpha_L, alpha_U));
+ DBG_ASSERT(alpha_L >= 0.0 && alpha_L <= 1.0
+ && alpha_U >=0.0 && alpha_U <= 1.0);
+
+ return Min(alpha_L, alpha_U);
+ }
+
+ Number
+ IpoptCalculatedQuantities::primal_frac_to_the_bound(Number tau,
+ const Vector& delta_x,
+ const Vector& delta_s)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::primal_frac_to_the_bound",
+ dbg_verbosity);
+ Number result;
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> s = ip_data_->curr()->s();
+ std::vector<const TaggedObject*> tdeps(4);
+ tdeps[0] = GetRawPtr(x);
+ tdeps[1] = GetRawPtr(s);
+ tdeps[2] = &delta_x;
+ tdeps[3] = &delta_s;
+
+ std::vector<Number> sdeps(1);
+ sdeps[0] = tau;
+
+ if (!primal_frac_to_the_bound_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ result = Min(CalcFracToBound(*curr_slack_x_L(), Tmp_x_L(), *ip_nlp_->Px_L(),
+ *curr_slack_x_U(), Tmp_x_U(), *ip_nlp_->Px_U(),
+ delta_x, tau),
+ CalcFracToBound(*curr_slack_s_L(), Tmp_s_L(), *ip_nlp_->Pd_L(),
+ *curr_slack_s_U(), Tmp_s_U(), *ip_nlp_->Pd_U(),
+ delta_s, tau));
+
+ primal_frac_to_the_bound_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::curr_primal_frac_to_the_bound(Number tau)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_primal_frac_to_the_bound()",
+ dbg_verbosity);
+ return primal_frac_to_the_bound(tau, *ip_data_->delta()->x(),
+ *ip_data_->delta()->s());
+ }
+
+ Number
+ IpoptCalculatedQuantities::uncached_dual_frac_to_the_bound(
+ Number tau,
+ const Vector& delta_z_L,
+ const Vector& delta_z_U,
+ const Vector& delta_v_L,
+ const Vector& delta_v_U)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::uncached_dual_frac_to_the_bound",
+ dbg_verbosity);
+ Number result;
+
+ result = ip_data_->curr()->z_L()->FracToBound(delta_z_L, tau);
+ result = Min(result, ip_data_->curr()->z_U()->FracToBound(delta_z_U, tau));
+ result = Min(result, ip_data_->curr()->v_L()->FracToBound(delta_v_L, tau));
+ result = Min(result, ip_data_->curr()->v_U()->FracToBound(delta_v_U, tau));
+
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::dual_frac_to_the_bound(
+ Number tau,
+ const Vector& delta_z_L,
+ const Vector& delta_z_U,
+ const Vector& delta_v_L,
+ const Vector& delta_v_U)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::dual_frac_to_the_bound",
+ dbg_verbosity);
+ Number result;
+
+ SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
+ SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
+ SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
+ SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();
+ std::vector<const TaggedObject*> tdeps(8);
+ tdeps[0] = GetRawPtr(z_L);
+ tdeps[1] = GetRawPtr(z_U);
+ tdeps[2] = GetRawPtr(v_L);
+ tdeps[3] = GetRawPtr(v_U);
+ tdeps[4] = &delta_z_L;
+ tdeps[5] = &delta_z_U;
+ tdeps[6] = &delta_v_L;
+ tdeps[7] = &delta_v_U;
+
+ std::vector<Number> sdeps(1);
+ sdeps[0] = tau;
+
+ if (!dual_frac_to_the_bound_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ result = z_L->FracToBound(delta_z_L, tau);
+ result = Min(result, z_U->FracToBound(delta_z_U, tau));
+ result = Min(result, v_L->FracToBound(delta_v_L, tau));
+ result = Min(result, v_U->FracToBound(delta_v_U, tau));
+
+ dual_frac_to_the_bound_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::curr_dual_frac_to_the_bound(Number tau)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_dual_frac_to_the_bound()",
+ dbg_verbosity);
+ return dual_frac_to_the_bound(tau, *ip_data_->delta()->z_L(),
+ *ip_data_->delta()->z_U(),
+ *ip_data_->delta()->v_L(),
+ *ip_data_->delta()->v_U());
+ }
+
+ Number
+ IpoptCalculatedQuantities::uncached_slack_frac_to_the_bound(
+ Number tau,
+ const Vector& delta_x_L,
+ const Vector& delta_x_U,
+ const Vector& delta_s_L,
+ const Vector& delta_s_U)
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::slack_frac_to_the_bound",
+ dbg_verbosity);
+ Number result;
+
+ SmartPtr<const Vector> x_L = curr_slack_x_L();
+ SmartPtr<const Vector> x_U = curr_slack_x_U();
+ SmartPtr<const Vector> s_L = curr_slack_s_L();
+ SmartPtr<const Vector> s_U = curr_slack_s_U();
+
+ result = x_L->FracToBound(delta_x_L, tau);
+ result = Min(result, x_U->FracToBound(delta_x_U, tau));
+ result = Min(result, s_L->FracToBound(delta_s_L, tau));
+ result = Min(result, s_U->FracToBound(delta_s_U, tau));
+
+ return result;
+ }
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Sigma Matrices //
+ ///////////////////////////////////////////////////////////////////////////
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_sigma_x()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_sigma_x()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
+ SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
+
+ if (!curr_sigma_x_cache_.GetCachedResult3Dep(result, *x, *z_L, *z_U)) {
+ SmartPtr<Vector> sigma = x->MakeNew();
+
+ sigma->Set(0.);
+ ip_nlp_->Px_L()->AddMSinvZ(1., *curr_slack_x_L(), *z_L, *sigma);
+ ip_nlp_->Px_U()->AddMSinvZ(1., *curr_slack_x_U(), *z_U, *sigma);
+
+ DBG_PRINT_VECTOR(2,"sigma_x", *sigma);
+
+ result = ConstPtr(sigma);
+ curr_sigma_x_cache_.AddCachedResult3Dep(result, *x, *z_L, *z_U);
+ }
+
+ return result;
+ }
+
+ SmartPtr<const Vector>
+ IpoptCalculatedQuantities::curr_sigma_s()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_sigma_s()",
+ dbg_verbosity);
+ SmartPtr<const Vector> result;
+ SmartPtr<const Vector> s = ip_data_->curr()->s();
+ SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
+ SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();
+
+ if (!curr_sigma_s_cache_.GetCachedResult3Dep(result, *s, *v_L, *v_U)) {
+ SmartPtr<Vector> sigma = s->MakeNew();
+
+ sigma->Set(0.);
+ ip_nlp_->Pd_L()->AddMSinvZ(1., *curr_slack_s_L(), *v_L, *sigma);
+ ip_nlp_->Pd_U()->AddMSinvZ(1., *curr_slack_s_U(), *v_U, *sigma);
+
+ result = ConstPtr(sigma);
+ curr_sigma_s_cache_.AddCachedResult3Dep(result, *s, *v_L, *v_U);
+ }
+
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::curr_avrg_compl()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_avrg_compl()",
+ dbg_verbosity);
+
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> s = ip_data_->curr()->s();
+ SmartPtr<const Vector> z_L = ip_data_->curr()->z_L();
+ SmartPtr<const Vector> z_U = ip_data_->curr()->z_U();
+ SmartPtr<const Vector> v_L = ip_data_->curr()->v_L();
+ SmartPtr<const Vector> v_U = ip_data_->curr()->v_U();
+
+ std::vector<const TaggedObject*> tdeps(6);
+ tdeps[0] = GetRawPtr(x);
+ tdeps[1] = GetRawPtr(s);
+ tdeps[2] = GetRawPtr(z_L);
+ tdeps[3] = GetRawPtr(z_U);
+ tdeps[4] = GetRawPtr(v_L);
+ tdeps[5] = GetRawPtr(v_U);
+
+ if (!curr_avrg_compl_cache_.GetCachedResult(result, tdeps)) {
+ if (!trial_avrg_compl_cache_.GetCachedResult(result, tdeps)) {
+
+ SmartPtr<const Vector> slack_x_L = curr_slack_x_L();
+ SmartPtr<const Vector> slack_x_U = curr_slack_x_U();
+ SmartPtr<const Vector> slack_s_L = curr_slack_s_L();
+ SmartPtr<const Vector> slack_s_U = curr_slack_s_U();
+
+ Index ncomps = z_L->Dim() + z_U->Dim() + v_L->Dim() + v_U->Dim();
+
+ if (ncomps>0) {
+ result = z_L->Dot(*slack_x_L);
+ result += z_U->Dot(*slack_x_U);
+ result += v_L->Dot(*slack_s_L);
+ result += v_U->Dot(*slack_s_U);
+
+ result /= (Number)ncomps;
+ }
+ else {
+ result = 0.;
+ }
+ }
+
+ curr_avrg_compl_cache_.AddCachedResult(result, tdeps);
+ }
+
+ return result;
+ }
+
+ Number
+ IpoptCalculatedQuantities::trial_avrg_compl()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::trial_avrg_compl()",
+ dbg_verbosity);
+
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->trial()->x();
+ SmartPtr<const Vector> s = ip_data_->trial()->s();
+ SmartPtr<const Vector> z_L = ip_data_->trial()->z_L();
+ SmartPtr<const Vector> z_U = ip_data_->trial()->z_U();
+ SmartPtr<const Vector> v_L = ip_data_->trial()->v_L();
+ SmartPtr<const Vector> v_U = ip_data_->trial()->v_U();
+
+ std::vector<const TaggedObject*> tdeps(6);
+ tdeps[0] = GetRawPtr(x);
+ tdeps[1] = GetRawPtr(s);
+ tdeps[2] = GetRawPtr(z_L);
+ tdeps[3] = GetRawPtr(z_U);
+ tdeps[4] = GetRawPtr(v_L);
+ tdeps[5] = GetRawPtr(v_U);
+
+ if (!trial_avrg_compl_cache_.GetCachedResult(result, tdeps)) {
+ if (!curr_avrg_compl_cache_.GetCachedResult(result, tdeps)) {
+
+ SmartPtr<const Vector> slack_x_L = trial_slack_x_L();
+ SmartPtr<const Vector> slack_x_U = trial_slack_x_U();
+ SmartPtr<const Vector> slack_s_L = trial_slack_s_L();
+ SmartPtr<const Vector> slack_s_U = trial_slack_s_U();
+
+ Index ncomps = z_L->Dim() + z_U->Dim() + v_L->Dim() + v_U->Dim();
+
+ if (ncomps>0) {
+ result = z_L->Dot(*slack_x_L);
+ result += z_U->Dot(*slack_x_U);
+ result += v_L->Dot(*slack_s_L);
+ result += v_U->Dot(*slack_s_U);
+
+ result /= (Number)ncomps;
+ }
+ else {
+ result = 0.;
+ }
+ }
+
+ trial_avrg_compl_cache_.AddCachedResult(result, tdeps);
+ }
+
+ return result;
+ }
+
+ void IpoptCalculatedQuantities::ComputeOptimalityErrorScaling(const Vector& y_c, const Vector& y_d,
+ const Vector& z_L, const Vector& z_U,
+ const Vector& v_L, const Vector& v_U,
+ Number s_max,
+ Number& s_d, Number& s_c)
+ {
+ DBG_ASSERT(initialize_called_);
+
+ s_c = z_L.Asum() + z_U.Asum() + v_L.Asum() + v_U.Asum();
+ Number n = (z_L.Dim() + z_U.Dim() + v_L.Dim() + v_U.Dim());
+ if (n == 0) {
+ s_c = 1.0;
+ }
+ else {
+ s_c = s_c / n;
+ s_c = Max(s_max, s_c)/s_max;
+ }
+
+ s_d = y_c.Asum() + y_d.Asum() + z_L.Asum() + z_U.Asum() + v_L.Asum() + v_U.Asum();
+ n = (y_c.Dim() + y_d.Dim() + z_L.Dim() + z_U.Dim() + v_L.Dim() + v_U.Dim());
+ if ( n == 0 ) {
+ s_d = 1.0;
+ }
+ else {
+ s_d = s_d / n;
+ s_d = Max(s_max, s_d)/s_max;
+ }
+ }
+
+ Number IpoptCalculatedQuantities::curr_gradBarrTDelta()
+ {
+ DBG_START_METH("IpoptCalculatedQuantities::curr_gradBarrTDelta()",
+ dbg_verbosity);
+
+ Number result;
+
+ SmartPtr<const Vector> x = ip_data_->curr()->x();
+ SmartPtr<const Vector> s = ip_data_->curr()->s();
+ SmartPtr<const Vector> delta_x = ip_data_->delta()->x();
+ SmartPtr<const Vector> delta_s = ip_data_->delta()->s();
+ std::vector<const TaggedObject*> tdeps(4);
+ tdeps[0] = GetRawPtr(x);
+ tdeps[1] = GetRawPtr(s);
+ tdeps[2] = GetRawPtr(delta_x);
+ tdeps[3] = GetRawPtr(delta_s);
+ Number mu = ip_data_->curr_mu();
+ std::vector<Number> sdeps(1);
+ sdeps[0] = mu;
+ DBG_PRINT((1,"curr_mu=%e\n",mu));
+
+ if (!curr_gradBarrTDelta_cache_.GetCachedResult(result, tdeps, sdeps)) {
+ result = curr_grad_barrier_obj_x()->Dot(*delta_x) +
+ curr_grad_barrier_obj_s()->Dot(*delta_s);
+
+ curr_gradBarrTDelta_cache_.AddCachedResult(result, tdeps, sdeps);
+ }
+ return result;
+ }
+
+ bool IpoptCalculatedQuantities::IsSquareProblem() const
+ {
+ return (ip_data_->curr()->x()->Dim() == ip_data_->curr()->y_c()->Dim());
+ }
+
+ Vector& IpoptCalculatedQuantities::Tmp_x()
+ {
+ if (!IsValid(tmp_x_)) {
+ tmp_x_ = ip_data_->curr()->x()->MakeNew();
+ }
+ return *tmp_x_;
+ }
+
+ Vector& IpoptCalculatedQuantities::Tmp_s()
+ {
+ if (!IsValid(tmp_s_)) {
+ tmp_s_ = ip_data_->curr()->s()->MakeNew();
+ }
+ return *tmp_s_;
+ }
+
+ Vector& IpoptCalculatedQuantities::Tmp_c()
+ {
+ if (!IsValid(tmp_c_)) {
+ tmp_c_ = ip_data_->curr()->y_c()->MakeNew();
+ }
+ return *tmp_c_;
+ }
+
+ Vector& IpoptCalculatedQuantities::Tmp_d()
+ {
+ if (!IsValid(tmp_d_)) {
+ tmp_d_ = ip_data_->curr()->y_d()->MakeNew();
+ }
+ return *tmp_d_;
+ }
+
+ Vector& IpoptCalculatedQuantities::Tmp_x_L()
+ {
+ if (!IsValid(tmp_x_L_)) {
+ tmp_x_L_ = ip_nlp_->x_L()->MakeNew();
+ }
+ return *tmp_x_L_;
+ }
+
+ Vector& IpoptCalculatedQuantities::Tmp_x_U()
+ {
+ if (!IsValid(tmp_x_U_)) {
+ tmp_x_U_ = ip_nlp_->x_U()->MakeNew();
+ }
+ return *tmp_x_U_;
+ }
+
+ Vector& IpoptCalculatedQuantities::Tmp_s_L()
+ {
+ if (!IsValid(tmp_s_L_)) {
+ tmp_s_L_ = ip_nlp_->d_L()->MakeNew();
+ }
+ return *tmp_s_L_;
+ }
+
+ Vector& IpoptCalculatedQuantities::Tmp_s_U()
+ {
+ if (!IsValid(tmp_s_U_)) {
+ tmp_s_U_ = ip_nlp_->d_U()->MakeNew();
+ }
+ return *tmp_s_U_;
+ }
+
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpIpoptCalculatedQuantities.hpp b/SimTKmath/Optimizers/src/IpOpt/IpIpoptCalculatedQuantities.hpp
new file mode 100644
index 0000000..325fbe8
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpIpoptCalculatedQuantities.hpp
@@ -0,0 +1,659 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpIpoptCalculatedQuantities.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPIPOPTCALCULATEDQUANTITIES_HPP__
+#define __IPIPOPTCALCULATEDQUANTITIES_HPP__
+
+#include "IpIpoptNLP.hpp"
+#include "IpIpoptData.hpp"
+
+namespace Ipopt
+{
+
+ /** Norm types */
+ enum ENormType {
+ NORM_1=0,
+ NORM_2,
+ NORM_MAX
+ };
+
+ /** Class for all IPOPT specific calculated quantities.
+ *
+ */
+ class IpoptCalculatedQuantities : public ReferencedObject
+ {
+ public:
+
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor */
+ IpoptCalculatedQuantities(const SmartPtr<IpoptNLP>& ip_nlp,
+ const SmartPtr<IpoptData>& ip_data);
+ /** Default destructor */
+ virtual ~IpoptCalculatedQuantities();
+ //@}
+
+ /** This method must be called to initialize the global
+ * algorithmic parameters. The parameters are taken from the
+ * OptionsList object. */
+ bool Initialize(const Journalist& jnlst,
+ const OptionsList& options,
+ const std::string& prefix);
+
+ /** @name Slacks */
+ //@{
+ /** Slacks for x_L (at current iterate) */
+ SmartPtr<const Vector> curr_slack_x_L();
+ /** Slacks for x_U (at current iterate) */
+ SmartPtr<const Vector> curr_slack_x_U();
+ /** Slacks for s_L (at current iterate) */
+ SmartPtr<const Vector> curr_slack_s_L();
+ /** Slacks for s_U (at current iterate) */
+ SmartPtr<const Vector> curr_slack_s_U();
+ /** Slacks for x_L (at trial point) */
+ SmartPtr<const Vector> trial_slack_x_L();
+ /** Slacks for x_U (at trial point) */
+ SmartPtr<const Vector> trial_slack_x_U();
+ /** Slacks for s_L (at trial point) */
+ SmartPtr<const Vector> trial_slack_s_L();
+ /** Slacks for s_U (at trial point) */
+ SmartPtr<const Vector> trial_slack_s_U();
+ /** Indicating whether or not we "fudged" the slacks */
+ Index AdjustedTrialSlacks();
+ /** Reset the flags for "fudged" slacks */
+ void ResetAdjustedTrialSlacks();
+ //@}
+
+ /** @name Objective function */
+ //@{
+ /** Value of objective function (at current point) */
+ Number curr_f();
+ /** Unscaled value of the objective function (at the current point) */
+ Number unscaled_curr_f();
+ /** Value of objective function (at trial point) */
+ Number trial_f();
+ /** Unscaled value of the objective function (at the trial point) */
+ Number unscaled_trial_f();
+ /** Gradient of objective function (at current point) */
+ SmartPtr<const Vector> curr_grad_f();
+ /** Gradient of objective function (at trial point) */
+ SmartPtr<const Vector> trial_grad_f();
+ //@}
+
+ /** @name Barrier Objective Function */
+ //@{
+ /** Barrier Objective Function Value
+ * (at current iterate with current mu)
+ */
+ Number curr_barrier_obj();
+ /** Barrier Objective Function Value
+ * (at trial point with current mu)
+ */
+ Number trial_barrier_obj();
+
+ /** Gradient of barrier objective function with respect to x
+ * (at current point with current mu) */
+ SmartPtr<const Vector> curr_grad_barrier_obj_x();
+ /** Gradient of barrier objective function with respect to s
+ * (at current point with current mu) */
+ SmartPtr<const Vector> curr_grad_barrier_obj_s();
+
+ /** Gradient of the damping term with respect to x (times
+ * kappa_d) */
+ SmartPtr<const Vector> grad_kappa_times_damping_x();
+ /** Gradient of the damping term with respect to s (times
+ * kappa_d) */
+ SmartPtr<const Vector> grad_kappa_times_damping_s();
+ //@}
+
+ /** @name Constraints */
+ //@{
+ /** c(x) (at current point) */
+ SmartPtr<const Vector> curr_c();
+ /** unscaled c(x) (at current point) */
+ SmartPtr<const Vector> unscaled_curr_c();
+ /** c(x) (at trial point) */
+ SmartPtr<const Vector> trial_c();
+ /** d(x) (at current point) */
+ SmartPtr<const Vector> curr_d();
+ /** unscaled d(x) (at current point) */
+ SmartPtr<const Vector> unscaled_curr_d();
+ /** d(x) (at trial point) */
+ SmartPtr<const Vector> trial_d();
+ /** d(x) - s (at current point) */
+ SmartPtr<const Vector> curr_d_minus_s();
+ /** d(x) - s (at trial point) */
+ SmartPtr<const Vector> trial_d_minus_s();
+ /** Jacobian of c (at current point) */
+ SmartPtr<const Matrix> curr_jac_c();
+ /** Jacobian of c (at trial point) */
+ SmartPtr<const Matrix> trial_jac_c();
+ /** Jacobian of d (at current point) */
+ SmartPtr<const Matrix> curr_jac_d();
+ /** Jacobian of d (at trial point) */
+ SmartPtr<const Matrix> trial_jac_d();
+ /** Product of Jacobian (evaluated at current point) of C
+ * transpose with general vector */
+ SmartPtr<const Vector> curr_jac_cT_times_vec(const Vector& vec);
+ /** Product of Jacobian (evaluated at trial point) of C
+ * transpose with general vector */
+ SmartPtr<const Vector> trial_jac_cT_times_vec(const Vector& vec);
+ /** Product of Jacobian (evaluated at current point) of D
+ * transpose with general vector */
+ SmartPtr<const Vector> curr_jac_dT_times_vec(const Vector& vec);
+ /** Product of Jacobian (evaluated at trial point) of D
+ * transpose with general vector */
+ SmartPtr<const Vector> trial_jac_dT_times_vec(const Vector& vec);
+ /** Product of Jacobian (evaluated at current point) of C
+ * transpose with current y_c */
+ SmartPtr<const Vector> curr_jac_cT_times_curr_y_c();
+ /** Product of Jacobian (evaluated at trial point) of C
+ * transpose with trial y_c */
+ SmartPtr<const Vector> trial_jac_cT_times_trial_y_c();
+ /** Product of Jacobian (evaluated at current point) of D
+ * transpose with current y_d */
+ SmartPtr<const Vector> curr_jac_dT_times_curr_y_d();
+ /** Product of Jacobian (evaluated at trial point) of D
+ * transpose with trial y_d */
+ SmartPtr<const Vector> trial_jac_dT_times_trial_y_d();
+ /** Product of Jacobian (evaluated at current point) of C
+ * with general vector */
+ SmartPtr<const Vector> curr_jac_c_times_vec(const Vector& vec);
+ /** Product of Jacobian (evaluated at current point) of D
+ * with general vector */
+ SmartPtr<const Vector> curr_jac_d_times_vec(const Vector& vec);
+ /** Constraint Violation (at current iterate). This value should
+ * be used in the line search, and not curr_primal_infeasibility().
+ * What type of norm is used depends on constr_viol_normtype */
+ Number curr_constraint_violation();
+ /** Constraint Violation (at trial point). This value should
+ * be used in the line search, and not curr_primal_infeasibility().
+ * What type of norm is used depends on constr_viol_normtype */
+ Number trial_constraint_violation();
+ /** Real constraint violation in a given norm (at current
+ * iterate). This considers the inequality constraints without
+ * slacks. */
+ Number curr_nlp_constraint_violation(ENormType NormType);
+ /** Unscaled real constraint violation in a given norm (at current
+ * iterate). This considers the inequality constraints without
+ * slacks. */
+ Number unscaled_curr_nlp_constraint_violation(ENormType NormType);
+ //@}
+
+ /** @name Hessian matrices */
+ //@{
+ /** exact Hessian at current iterate (uncached) */
+ SmartPtr<const SymMatrix> curr_exact_hessian();
+ //@}
+
+ /** @name primal-dual error and its components */
+ //@{
+ /** x-part of gradient of Lagrangian function (at current point) */
+ SmartPtr<const Vector> curr_grad_lag_x();
+ /** x-part of gradient of Lagrangian function (at trial point) */
+ SmartPtr<const Vector> trial_grad_lag_x();
+ /** s-part of gradient of Lagrangian function (at current point) */
+ SmartPtr<const Vector> curr_grad_lag_s();
+ /** s-part of gradient of Lagrangian function (at trial point) */
+ SmartPtr<const Vector> trial_grad_lag_s();
+ /** x-part of gradient of Lagrangian function (at current point)
+ including linear damping term */
+ SmartPtr<const Vector> curr_grad_lag_with_damping_x();
+ /** s-part of gradient of Lagrangian function (at current point)
+ including linear damping term */
+ SmartPtr<const Vector> curr_grad_lag_with_damping_s();
+ /** Complementarity for x_L (for current iterate) */
+ SmartPtr<const Vector> curr_compl_x_L();
+ /** Complementarity for x_U (for current iterate) */
+ SmartPtr<const Vector> curr_compl_x_U();
+ /** Complementarity for s_L (for current iterate) */
+ SmartPtr<const Vector> curr_compl_s_L();
+ /** Complementarity for s_U (for current iterate) */
+ SmartPtr<const Vector> curr_compl_s_U();
+ /** Complementarity for x_L (for trial iterate) */
+ SmartPtr<const Vector> trial_compl_x_L();
+ /** Complementarity for x_U (for trial iterate) */
+ SmartPtr<const Vector> trial_compl_x_U();
+ /** Complementarity for s_L (for trial iterate) */
+ SmartPtr<const Vector> trial_compl_s_L();
+ /** Complementarity for s_U (for trial iterate) */
+ SmartPtr<const Vector> trial_compl_s_U();
+ /** Relaxed complementarity for x_L (for current iterate and current mu) */
+ SmartPtr<const Vector> curr_relaxed_compl_x_L();
+ /** Relaxed complementarity for x_U (for current iterate and current mu) */
+ SmartPtr<const Vector> curr_relaxed_compl_x_U();
+ /** Relaxed complementarity for s_L (for current iterate and current mu) */
+ SmartPtr<const Vector> curr_relaxed_compl_s_L();
+ /** Relaxed complementarity for s_U (for current iterate and current mu) */
+ SmartPtr<const Vector> curr_relaxed_compl_s_U();
+
+ /** Primal infeasibility in a given norm (at current iterate). */
+ Number curr_primal_infeasibility(ENormType NormType);
+ /** Primal infeasibility in a given norm (at trial point) */
+ Number trial_primal_infeasibility(ENormType NormType);
+
+ /** Dual infeasibility in a given norm (at current iterate) */
+ Number curr_dual_infeasibility(ENormType NormType);
+ /** Dual infeasibility in a given norm (at trial iterate) */
+ Number trial_dual_infeasibility(ENormType NormType);
+ /** Unscaled dual infeasibility in a given norm (at current iterate) */
+ Number unscaled_curr_dual_infeasibility(ENormType NormType);
+
+ /** Complementarity (for all complementarity conditions together)
+ * in a given norm (at current iterate) */
+ Number curr_complementarity(Number mu, ENormType NormType);
+ /** Complementarity (for all complementarity conditions together)
+ * in a given norm (at trial iterate) */
+ Number trial_complementarity(Number mu, ENormType NormType);
+ /** Complementarity (for all complementarity conditions together)
+ * in a given norm (at current iterate) without NLP scaling. */
+ Number unscaled_curr_complementarity(Number mu, ENormType NormType);
+
+ /** Centrality measure (in spirit of the -infinity-neighborhood. */
+ Number CalcCentralityMeasure(const Vector& compl_x_L,
+ const Vector& compl_x_U,
+ const Vector& compl_s_L,
+ const Vector& compl_s_U);
+ /** Centrality measure at current point */
+ Number curr_centrality_measure();
+
+ /** Total optimality error for the original NLP at the current
+ * iterate, using scaling factors based on multipliers. Note
+ * that here the constraint violation is measured without slacks
+ * (nlp_constraint_violation) */
+ Number curr_nlp_error();
+ /** Total optimality error for the original NLP at the current
+ * iterate, but using no scaling based on multipliers, and no
+ * scaling for the NLP. Note that here the constraint violation
+ * is measured without slacks (nlp_constraint_violation) */
+ Number unscaled_curr_nlp_error();
+
+ /** Total optimality error for the barrier problem at the
+ * current iterate, using scaling factors based on multipliers. */
+ Number curr_barrier_error();
+
+ /** Norm of the primal-dual system for a given mu (at current
+ * iterate). The norm is defined as the sum of the 1-norms of
+ * dual infeasibiliy, primal infeasibility, and complementarity,
+ * all divided by the number of elements of the vectors of which
+ * the norm is taken.
+ */
+ Number curr_primal_dual_system_error(Number mu);
+ /** Norm of the primal-dual system for a given mu (at trial
+ * iterate). The norm is defined as the sum of the 1-norms of
+ * dual infeasibiliy, primal infeasibility, and complementarity,
+ * all divided by the number of elements of the vectors of which
+ * the norm is taken.
+ */
+ Number trial_primal_dual_system_error(Number mu);
+ //@}
+
+ /** @name Computing fraction-to-the-boundary step sizes */
+ //@{
+ /** Fraction to the boundary from (current) primal variables x and s
+ * for a given step */
+ Number primal_frac_to_the_bound(Number tau,
+ const Vector& delta_x,
+ const Vector& delta_s);
+ /** Fraction to the boundary from (current) primal variables x and s
+ * for internal (current) step */
+ Number curr_primal_frac_to_the_bound(Number tau);
+ /** Fraction to the boundary from (current) dual variables z and v
+ * for a given step */
+ Number dual_frac_to_the_bound(Number tau,
+ const Vector& delta_z_L,
+ const Vector& delta_z_U,
+ const Vector& delta_v_L,
+ const Vector& delta_v_U);
+ /** Fraction to the boundary from (current) dual variables z and v
+ * for a given step, without caching */
+ Number uncached_dual_frac_to_the_bound(Number tau,
+ const Vector& delta_z_L,
+ const Vector& delta_z_U,
+ const Vector& delta_v_L,
+ const Vector& delta_v_U);
+ /** Fraction to the boundary from (current) dual variables z and v
+ * for internal (current) step */
+ Number curr_dual_frac_to_the_bound(Number tau);
+ /** Fraction to the boundary from (current) slacks for a given
+ * step in the slacks. Usually, one will use the
+ * primal_frac_to_the_bound method to compute the primal fraction
+ * to the boundary step size, but if it is cheaper to provide the
+ * steps in the slacks directly (e.g. when the primal step sizes
+ * are only temporary), the this method is more efficient. This
+ * method does not cache computations. */
+ Number uncached_slack_frac_to_the_bound(Number tau,
+ const Vector& delta_x_L,
+ const Vector& delta_x_U,
+ const Vector& delta_s_L,
+ const Vector& delta_s_U);
+ //@}
+
+ /** @name Sigma matrices */
+ //@{
+ SmartPtr<const Vector> curr_sigma_x();
+ SmartPtr<const Vector> curr_sigma_s();
+ //@}
+
+ /** average of current values of the complementarities */
+ Number curr_avrg_compl();
+ /** average of trial values of the complementarities */
+ Number trial_avrg_compl();
+
+ /** inner_product of current barrier obj. fn. gradient with
+ * current search direction */
+ Number curr_gradBarrTDelta();
+
+ /** Compute the norm of a specific type of a set of vectors (uncached) */
+ Number
+ CalcNormOfType(ENormType NormType,
+ std::vector<SmartPtr<const Vector> > vecs);
+
+ /** Compute the norm of a specific type of two vectors (uncached) */
+ Number
+ CalcNormOfType(ENormType NormType,
+ const Vector& vec1, const Vector& vec2);
+
+ /** Norm type used for calculating constraint violation */
+ ENormType constr_viol_normtype() const
+ {
+ return constr_viol_normtype_;
+ }
+
+ /** Method returning true if this is a square problem */
+ bool IsSquareProblem() const;
+
+ /** Methods for IpoptType */
+ //@{
+ /** Called by IpoptType to register the options */
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ IpoptCalculatedQuantities();
+
+ /** Copy Constructor */
+ IpoptCalculatedQuantities(const IpoptCalculatedQuantities&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const IpoptCalculatedQuantities&);
+ //@}
+
+ /** @name Pointers for easy access to data and NLP information */
+ //@{
+ /** Ipopt NLP object */
+ SmartPtr<IpoptNLP> ip_nlp_;
+ /** Ipopt Data object */
+ SmartPtr<IpoptData> ip_data_;
+ //@}
+
+ /** @name Algorithmic Parameters that can be set throught the
+ * options list. Those parameters are initialize by calling the
+ * Initialize method.*/
+ //@{
+ /** Parameter in formula for computing overall primal-dual
+ * optimality error */
+ Number s_max_;
+ /** Weighting factor for the linear damping term added to the
+ * barrier objective funciton. */
+ Number kappa_d_;
+ /** fractional movement allowed in bounds */
+ Number slack_move_;
+ /** Norm type to be used when calculating the constraint violation */
+ ENormType constr_viol_normtype_;
+ /** Flag indicating whether the TNLP with identical structure has
+ * already been solved before. */
+ bool warm_start_same_structure_;
+ //@}
+
+ /** @name Caches for slacks */
+ //@{
+ CachedResults< SmartPtr<Vector> > curr_slack_x_L_cache_;
+ CachedResults< SmartPtr<Vector> > curr_slack_x_U_cache_;
+ CachedResults< SmartPtr<Vector> > curr_slack_s_L_cache_;
+ CachedResults< SmartPtr<Vector> > curr_slack_s_U_cache_;
+ CachedResults< SmartPtr<Vector> > trial_slack_x_L_cache_;
+ CachedResults< SmartPtr<Vector> > trial_slack_x_U_cache_;
+ CachedResults< SmartPtr<Vector> > trial_slack_s_L_cache_;
+ CachedResults< SmartPtr<Vector> > trial_slack_s_U_cache_;
+ Index num_adjusted_slack_x_L_;
+ Index num_adjusted_slack_x_U_;
+ Index num_adjusted_slack_s_L_;
+ Index num_adjusted_slack_s_U_;
+ //@}
+
+ /** @name Cached for objective function stuff */
+ //@{
+ CachedResults<Number> curr_f_cache_;
+ CachedResults<Number> trial_f_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_grad_f_cache_;
+ CachedResults< SmartPtr<const Vector> > trial_grad_f_cache_;
+ //@}
+
+ /** @name Caches for barrier function stuff */
+ //@{
+ CachedResults<Number> curr_barrier_obj_cache_;
+ CachedResults<Number> trial_barrier_obj_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_grad_barrier_obj_x_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_grad_barrier_obj_s_cache_;
+ CachedResults< SmartPtr<const Vector> > grad_kappa_times_damping_x_cache_;
+ CachedResults< SmartPtr<const Vector> > grad_kappa_times_damping_s_cache_;
+ //@}
+
+ /** @name Caches for constraint stuff */
+ //@{
+ CachedResults< SmartPtr<const Vector> > curr_c_cache_;
+ CachedResults< SmartPtr<const Vector> > trial_c_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_d_cache_;
+ CachedResults< SmartPtr<const Vector> > trial_d_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_d_minus_s_cache_;
+ CachedResults< SmartPtr<const Vector> > trial_d_minus_s_cache_;
+ CachedResults< SmartPtr<const Matrix> > curr_jac_c_cache_;
+ CachedResults< SmartPtr<const Matrix> > trial_jac_c_cache_;
+ CachedResults< SmartPtr<const Matrix> > curr_jac_d_cache_;
+ CachedResults< SmartPtr<const Matrix> > trial_jac_d_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_jac_cT_times_vec_cache_;
+ CachedResults< SmartPtr<const Vector> > trial_jac_cT_times_vec_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_jac_dT_times_vec_cache_;
+ CachedResults< SmartPtr<const Vector> > trial_jac_dT_times_vec_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_jac_c_times_vec_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_jac_d_times_vec_cache_;
+ CachedResults<Number> curr_constraint_violation_cache_;
+ CachedResults<Number> trial_constraint_violation_cache_;
+ CachedResults<Number> curr_nlp_constraint_violation_cache_;
+ CachedResults<Number> unscaled_curr_nlp_constraint_violation_cache_;
+ //@}
+
+ /** Cache for the exact Hessian */
+ CachedResults< SmartPtr<const SymMatrix> > curr_exact_hessian_cache_;
+
+ /** @name Components of primal-dual error */
+ //@{
+ CachedResults< SmartPtr<const Vector> > curr_grad_lag_x_cache_;
+ CachedResults< SmartPtr<const Vector> > trial_grad_lag_x_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_grad_lag_s_cache_;
+ CachedResults< SmartPtr<const Vector> > trial_grad_lag_s_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_grad_lag_with_damping_x_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_grad_lag_with_damping_s_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_compl_x_L_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_compl_x_U_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_compl_s_L_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_compl_s_U_cache_;
+ CachedResults< SmartPtr<const Vector> > trial_compl_x_L_cache_;
+ CachedResults< SmartPtr<const Vector> > trial_compl_x_U_cache_;
+ CachedResults< SmartPtr<const Vector> > trial_compl_s_L_cache_;
+ CachedResults< SmartPtr<const Vector> > trial_compl_s_U_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_relaxed_compl_x_L_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_relaxed_compl_x_U_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_relaxed_compl_s_L_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_relaxed_compl_s_U_cache_;
+ CachedResults<Number> curr_primal_infeasibility_cache_;
+ CachedResults<Number> trial_primal_infeasibility_cache_;
+ CachedResults<Number> curr_dual_infeasibility_cache_;
+ CachedResults<Number> trial_dual_infeasibility_cache_;
+ CachedResults<Number> unscaled_curr_dual_infeasibility_cache_;
+ CachedResults<Number> curr_complementarity_cache_;
+ CachedResults<Number> trial_complementarity_cache_;
+ CachedResults<Number> curr_centrality_measure_cache_;
+ CachedResults<Number> curr_nlp_error_cache_;
+ CachedResults<Number> unscaled_curr_nlp_error_cache_;
+ CachedResults<Number> curr_barrier_error_cache_;
+ CachedResults<Number> curr_primal_dual_system_error_cache_;
+ CachedResults<Number> trial_primal_dual_system_error_cache_;
+ //@}
+
+ /** @name Caches for fraction to the boundary step sizes */
+ //@{
+ CachedResults<Number> primal_frac_to_the_bound_cache_;
+ CachedResults<Number> dual_frac_to_the_bound_cache_;
+ //@}
+
+ /** @name Caches for sigma matrices */
+ //@{
+ CachedResults< SmartPtr<const Vector> > curr_sigma_x_cache_;
+ CachedResults< SmartPtr<const Vector> > curr_sigma_s_cache_;
+ //@}
+
+ /** Cache for average of current complementarity */
+ CachedResults<Number> curr_avrg_compl_cache_;
+ /** Cache for average of trial complementarity */
+ CachedResults<Number> trial_avrg_compl_cache_;
+
+ /** Cache for grad barrier obj. fn inner product with step */
+ CachedResults<Number> curr_gradBarrTDelta_cache_;
+
+ /** @name Indicator vectors required for the linear damping terms
+ * to handle unbounded solution sets. */
+ //@{
+ /** Indicator vector for selecting the elements in x that have
+ * only lower bounds. */
+ SmartPtr<Vector> dampind_x_L_;
+ /** Indicator vector for selecting the elements in x that have
+ * only upper bounds. */
+ SmartPtr<Vector> dampind_x_U_;
+ /** Indicator vector for selecting the elements in s that have
+ * only lower bounds. */
+ SmartPtr<Vector> dampind_s_L_;
+ /** Indicator vector for selecting the elements in s that have
+ * only upper bounds. */
+ SmartPtr<Vector> dampind_s_U_;
+ //@}
+
+ /** @name Temporary vectors for intermediate calcuations. We keep
+ * these around to avoid unnecessarily many new allocations of
+ * Vectors. */
+ //@{
+ SmartPtr<Vector> tmp_x_;
+ SmartPtr<Vector> tmp_s_;
+ SmartPtr<Vector> tmp_c_;
+ SmartPtr<Vector> tmp_d_;
+ SmartPtr<Vector> tmp_x_L_;
+ SmartPtr<Vector> tmp_x_U_;
+ SmartPtr<Vector> tmp_s_L_;
+ SmartPtr<Vector> tmp_s_U_;
+
+ /** Accessor methods for the temporary vectors */
+ Vector& Tmp_x();
+ Vector& Tmp_s();
+ Vector& Tmp_c();
+ Vector& Tmp_d();
+ Vector& Tmp_x_L();
+ Vector& Tmp_x_U();
+ Vector& Tmp_s_L();
+ Vector& Tmp_s_U();
+ //@}
+
+ /** flag indicating if Initialize method has been called (for
+ * debugging) */
+ bool initialize_called_;
+
+ /** @name Auxiliary functions */
+ //@{
+ /** Compute new vector containing the slack to a lower bound
+ * (uncached)
+ */
+ SmartPtr<Vector> CalcSlack_L(const Matrix& P,
+ const Vector& x,
+ const Vector& x_bound);
+ /** Compute new vector containing the slack to a upper bound
+ * (uncached)
+ */
+ SmartPtr<Vector> CalcSlack_U(const Matrix& P,
+ const Vector& x,
+ const Vector& x_bound);
+ /** Compute barrier term at given point
+ * (uncached)
+ */
+ Number CalcBarrierTerm(Number mu,
+ const Vector& slack_x_L,
+ const Vector& slack_x_U,
+ const Vector& slack_s_L,
+ const Vector& slack_s_U);
+
+ /** Compute complementarity for slack / multiplier pair */
+ SmartPtr<const Vector> CalcCompl(const Vector& slack,
+ const Vector& mult);
+
+ /** Compute fraction to the boundary parameter for lower and upper bounds */
+ Number CalcFracToBound(const Vector& slack_L,
+ Vector& tmp_L,
+ const Matrix& P_L,
+ const Vector& slack_U,
+ Vector& tmp_U,
+ const Matrix& P_U,
+ const Vector& delta,
+ Number tau);
+
+ /** Compute the scaling factors for the optimality error. */
+ void ComputeOptimalityErrorScaling(const Vector& y_c, const Vector& y_d,
+ const Vector& z_L, const Vector& z_U,
+ const Vector& v_L, const Vector& v_U,
+ Number s_max,
+ Number& s_d, Number& s_c);
+
+ /** Check if slacks are becoming too small. If slacks are
+ * becoming too small, they are change. The return value is the
+ * number of corrected slacks. */
+ Index CalculateSafeSlack(SmartPtr<Vector>& slack,
+ const SmartPtr<const Vector>& bound,
+ const SmartPtr<const Vector>& curr_point,
+ const SmartPtr<const Vector>& multiplier);
+
+ /** Computes the indicator vectors that can be used to filter out
+ * those entries in the slack_... variables, that correspond to
+ * variables with only lower and upper bounds. This is required
+ * for the linear damping term in the barrier objective function
+ * to handle unbounded solution sets. */
+ void ComputeDampingIndicators(SmartPtr<const Vector>& dampind_x_L,
+ SmartPtr<const Vector>& dampind_x_U,
+ SmartPtr<const Vector>& dampind_s_L,
+ SmartPtr<const Vector>& dampind_s_U);
+
+ /** Check if we are in the restoration phase. Returns true, if the
+ * ip_nlp is of the type RestoIpoptNLP. ToDo: We probably want to
+ * handle this more elegant and don't have an explicit dependency
+ * here. Now I added this because otherwise the caching doesn't
+ * work properly since the restoration phase objective function
+ * depends on the current barrier parameter. */
+ bool in_restoration_phase();
+
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpIpoptData.cpp b/SimTKmath/Optimizers/src/IpOpt/IpIpoptData.cpp
new file mode 100644
index 0000000..a66577c
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpIpoptData.cpp
@@ -0,0 +1,236 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpIpoptData.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpIpoptData.hpp"
+#include "IpIpoptNLP.hpp"
+
+namespace Ipopt
+{
+
+ IpoptData::IpoptData()
+ {}
+
+ IpoptData::~IpoptData()
+ {}
+
+ void IpoptData::RegisterOptions(const SmartPtr<RegisteredOptions>& roptions)
+ {
+ roptions->SetRegisteringCategory("Convergence");
+ roptions->AddLowerBoundedNumberOption(
+ "tol",
+ "Desired convergence tolerance (relative).",
+ 0.0, true, 1e-8,
+ "Determines the convergence tolerance for the algorithm. The "
+ "algorithm terminates successfully, if the (scaled) NLP error "
+ "becomes smaller than this value, and if the (absolute) criteria "
+ "according to \"dual_inf_tol\", \"primal_inf_tol\", and "
+ "\"cmpl_inf_tol\" are met. (This is epsilon_tol in Eqn. (6) in "
+ "implementation paper). See also \"acceptable_tol\" as a second "
+ "termination criterion. Note, some other algorithmic features also use "
+ "this quantity to determine thresholds etc.");
+ }
+
+ bool IpoptData::Initialize(const Journalist& jnlst,
+ const OptionsList& options,
+ const std::string& prefix)
+ {
+ if (prefix=="resto.") {
+ // The default for the restoration phase is 1e-2 time the value
+ // for the regular algorithm
+ if (!options.GetNumericValue("resto.tol", tol_, "")) {
+ options.GetNumericValue("tol", tol_, prefix);
+ tol_ *= 1e-2;
+ }
+ }
+ else {
+ options.GetNumericValue("tol", tol_, prefix);
+ }
+
+ iter_count_ = 0;
+ curr_mu_ = -1.;
+ mu_initialized_ = false;
+ curr_tau_ = -1.;
+ tau_initialized_ = false;
+ initialize_called_ = false;
+ have_prototypes_ = false;
+ have_deltas_ = false;
+ have_affine_deltas_ = false;
+
+ free_mu_mode_ = false;
+ tiny_step_flag_ = false;
+
+ ResetInfo();
+
+ initialize_called_ = true;
+
+ return true;
+ }
+
+ bool IpoptData::InitializeDataStructures(IpoptNLP& ip_nlp,
+ bool want_x,
+ bool want_y_c,
+ bool want_y_d,
+ bool want_z_L,
+ bool want_z_U)
+ {
+ DBG_ASSERT(initialize_called_);
+ /*
+ * Allocate space for all the required linear algebra
+ * structures
+ */
+
+ SmartPtr<Vector> new_x;
+ SmartPtr<Vector> new_s;
+ SmartPtr<Vector> new_y_c;
+ SmartPtr<Vector> new_y_d;
+ SmartPtr<Vector> new_z_L;
+ SmartPtr<Vector> new_z_U;
+ SmartPtr<Vector> new_v_L;
+ SmartPtr<Vector> new_v_U;
+
+ // Get the required linear algebra structures from the model
+ bool retValue
+ = ip_nlp.InitializeStructures(new_x, want_x,
+ new_y_c, want_y_c,
+ new_y_d, want_y_d,
+ new_z_L, want_z_L,
+ new_z_U, want_z_U,
+ new_v_L, new_v_U);
+ if (!retValue) {
+ return false;
+ }
+
+ new_s = new_y_d->MakeNew(); // same dimension as d
+
+ iterates_space_ = new IteratesVectorSpace(*(new_x->OwnerSpace()), *(new_s->OwnerSpace()),
+ *(new_y_c->OwnerSpace()), *(new_y_d->OwnerSpace()),
+ *(new_z_L->OwnerSpace()), *(new_z_U->OwnerSpace()),
+ *(new_v_L->OwnerSpace()), *(new_v_U->OwnerSpace())
+ );
+
+ curr_ = iterates_space_->MakeNewIteratesVector(*new_x,
+ *new_s,
+ *new_y_c,
+ *new_y_d,
+ *new_z_L,
+ *new_z_U,
+ *new_v_L,
+ *new_v_U);
+#ifdef IP_DEBUG
+
+ debug_curr_tag_ = curr_->GetTag();
+ debug_curr_tag_sum_ = curr_->GetTagSum();
+ debug_trial_tag_ = 0;
+ debug_trial_tag_sum_ = 0;
+ debug_delta_tag_ = 0;
+ debug_delta_tag_sum_ = 0;
+ debug_delta_aff_tag_ = 0;
+ debug_delta_aff_tag_sum_ = 0;
+#endif
+
+ trial_ = NULL;
+
+ // Set the pointers for storing steps to NULL
+ delta_ = NULL;
+
+ // Set the pointers for storing steps to NULL
+ delta_aff_ = NULL;
+
+ have_prototypes_ = true;
+ have_deltas_ = false;
+
+ return true;
+ }
+
+ void IpoptData::SetTrialPrimalVariablesFromStep(Number alpha,
+ const Vector& delta_x,
+ const Vector& delta_s)
+ {
+ DBG_ASSERT(have_prototypes_);
+
+ if (IsNull(trial_)) {
+ trial_ = iterates_space_->MakeNewIteratesVector(false);
+ }
+
+ SmartPtr<IteratesVector> newvec = trial_->MakeNewContainer();
+ newvec->create_new_x();
+ newvec->x_NonConst()->AddTwoVectors(1., *curr_->x(), alpha, delta_x, 0.);
+
+ newvec->create_new_s();
+ newvec->s_NonConst()->AddTwoVectors(1., *curr_->s(), alpha, delta_s, 0.);
+
+ set_trial(newvec);
+ }
+
+ void IpoptData::SetTrialEqMultipliersFromStep(Number alpha,
+ const Vector& delta_y_c,
+ const Vector& delta_y_d)
+ {
+ DBG_ASSERT(have_prototypes_);
+
+ SmartPtr<IteratesVector> newvec = trial()->MakeNewContainer();
+ newvec->create_new_y_c();
+ newvec->y_c_NonConst()->AddTwoVectors(1., *curr()->y_c(), alpha, delta_y_c, 0.);
+
+ newvec->create_new_y_d();
+ newvec->y_d_NonConst()->AddTwoVectors(1., *curr()->y_d(), alpha, delta_y_d, 0.);
+
+ set_trial(newvec);
+ }
+
+ void IpoptData::SetTrialBoundMultipliersFromStep(Number alpha,
+ const Vector& delta_z_L,
+ const Vector& delta_z_U,
+ const Vector& delta_v_L,
+ const Vector& delta_v_U)
+ {
+ DBG_ASSERT(have_prototypes_);
+
+ SmartPtr<IteratesVector> newvec = trial()->MakeNewContainer();
+ newvec->create_new_z_L();
+ newvec->z_L_NonConst()->AddTwoVectors(1., *curr()->z_L(), alpha, delta_z_L, 0.);
+
+ newvec->create_new_z_U();
+ newvec->z_U_NonConst()->AddTwoVectors(1., *curr()->z_U(), alpha, delta_z_U, 0.);
+
+ newvec->create_new_v_L();
+ newvec->v_L_NonConst()->AddTwoVectors(1., *curr()->v_L(), alpha, delta_v_L, 0.);
+
+ newvec->create_new_v_U();
+ newvec->v_U_NonConst()->AddTwoVectors(1., *curr()->v_U(), alpha, delta_v_U, 0.);
+
+ set_trial(newvec);
+ }
+
+ void IpoptData::AcceptTrialPoint()
+ {
+ DBG_ASSERT(IsValid(trial_));
+ DBG_ASSERT(IsValid(trial_->x()));
+ DBG_ASSERT(IsValid(trial_->s()));
+ DBG_ASSERT(IsValid(trial_->y_c()));
+ DBG_ASSERT(IsValid(trial_->y_d()));
+ DBG_ASSERT(IsValid(trial_->z_L()));
+ DBG_ASSERT(IsValid(trial_->z_U()));
+ DBG_ASSERT(IsValid(trial_->v_L()));
+ DBG_ASSERT(IsValid(trial_->v_U()));
+
+ CopyTrialToCurrent();
+
+ // Set trial pointers to Null (frees memory unless someone else is
+ // still referring to it, and it makes sure that indeed all trial
+ // values are set before a new trial point is accepted)
+ trial_ = NULL;
+
+ // Free the memory for the affine-scaling step
+ delta_aff_ = NULL;
+
+ have_deltas_ = false;
+ have_affine_deltas_ = false;
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpIpoptData.hpp b/SimTKmath/Optimizers/src/IpOpt/IpIpoptData.hpp
new file mode 100644
index 0000000..e320733
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpIpoptData.hpp
@@ -0,0 +1,645 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpIpoptData.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPIPOPTDATA_HPP__
+#define __IPIPOPTDATA_HPP__
+
+#include "IpSymMatrix.hpp"
+#include "IpOptionsList.hpp"
+#include "IpIteratesVector.hpp"
+#include "IpRegOptions.hpp"
+#include "IpTimingStatistics.hpp"
+
+namespace Ipopt
+{
+
+ /* Forward declaration */
+ class IpoptNLP;
+
+ /** Class to organize all the data required by the algorithm.
+ * Internally, once this Data object has been initialized, all
+ * internal curr_ vectors must always be set (so that prototyes are
+ * available). The current values can only be set from the trial
+ * values. The trial values can be set by copying from a vector or
+ * by adding some fraction of a step to the current values. This
+ * object also stores steps, which allows to easily communicate the
+ * step from the step computation object to the line search object.
+ */
+ class IpoptData : public ReferencedObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor */
+ IpoptData();
+
+ /** Default destructor */
+ ~IpoptData();
+ //@}
+
+ /** Initialize Data Structures */
+ bool InitializeDataStructures(IpoptNLP& ip_nlp,
+ bool want_x,
+ bool want_y_c,
+ bool want_y_d,
+ bool want_z_L,
+ bool want_z_U);
+
+ /** This method must be called to initialize the global
+ * algorithmic parameters. The parameters are taken from the
+ * OptionsList object. */
+ bool Initialize(const Journalist& jnlst,
+ const OptionsList& options,
+ const std::string& prefix);
+
+ /** @name Get Methods for Iterates */
+ //@{
+ /** Current point */
+ SmartPtr<const IteratesVector> curr() const;
+
+ /** Get the current point in a copied container that is non-const.
+ The entries in the container cannot be modified, but
+ the container can be modified to point to new entries.
+ */
+ // SmartPtr<IteratesVector> curr_container() const;
+
+ /** Get Trial point */
+ SmartPtr<const IteratesVector> trial() const;
+
+ /** Get Trial point in a copied container that is non-const.
+ * The entries in the container can not be modified, but
+ * the container can be modified to point to new entries.
+ */
+ //SmartPtr<IteratesVector> trial_container() const;
+
+ /** Set the trial point - this method copies the pointer for
+ * efficiency (no copy and to keep cache tags the same) so
+ * after you call set you cannot modify the data again
+ */
+ void set_trial(SmartPtr<IteratesVector>& trial);
+
+ /** Set the values of the primal trial variables (x and s) from
+ * provided Step with step length alpha.
+ */
+ void SetTrialPrimalVariablesFromStep(Number alpha,
+ const Vector& delta_x,
+ const Vector& delta_s);
+ /** Set the values of the trial values for the equality constraint
+ * multipliers (y_c and y_d) from provided step with step length
+ * alpha.
+ */
+ void SetTrialEqMultipliersFromStep(Number alpha,
+ const Vector& delta_y_c,
+ const Vector& delta_y_d);
+ /** Set the value of the trial values for the bound multipliers
+ * (z_L, z_U, v_L, v_U) from provided step with step length
+ * alpha.
+ */
+ void SetTrialBoundMultipliersFromStep(Number alpha,
+ const Vector& delta_z_L,
+ const Vector& delta_z_U,
+ const Vector& delta_v_L,
+ const Vector& delta_v_U);
+
+ /** ToDo: I may need to add versions of set_trial like the
+ * following, but I am not sure
+ */
+ // void set_trial(const SmartPtr<IteratesVector>& trial_iterates);
+ // void set_trial(SmartPtr<const IteratesVector>& trial_iterates);
+
+ /** get the current delta */
+ SmartPtr<const IteratesVector> delta() const;
+
+ /** Set the current delta - like the trial point, this method copies
+ * the pointer for efficiency (no copy and to keep cache tags the
+ * same) so after you call set, you cannot modify the data
+ */
+ void set_delta(SmartPtr<IteratesVector>& delta);
+
+ /** Affine Delta */
+ SmartPtr<const IteratesVector> delta_aff() const;
+
+ /** Set the affine delta - like the trial point, this method copies
+ * the pointer for efficiency (no copy and to keep cache tags the
+ * same) so after you call set, you cannot modify the data
+ */
+ void set_delta_aff(SmartPtr<IteratesVector>& delta_aff);
+
+
+ /** Hessian or Hessian approximation (do not hold on to it, it might be changed) */
+ SmartPtr<const SymMatrix> W()
+ {
+ DBG_ASSERT(IsValid(W_));
+ return W_;
+ }
+
+ /** Set Hessian approximation */
+ void Set_W(SmartPtr<const SymMatrix> W)
+ {
+ W_ = W;
+ }
+
+ /** @name ("Main") Primal-dual search direction. Those fields are
+ * used to store the search directions computed from solving the
+ * primal-dual system, and can be used in the line search. They
+ * are overwritten in every iteration, so do not hold on to the
+ * pointers (make copies instead) */
+ //@{
+
+ /** Returns true, if the primal-dual step have been already
+ * computed for the current iteration. This flag is reset after
+ * every call of AcceptTrialPoint(). If the search direction is
+ * computed during the computation of the barrier parameter, the
+ * method computing the barrier parameter should call
+ * SetHaveDeltas(true) to tell the IpoptAlgorithm object that it
+ * doesn't need to recompute the primal-dual step. */
+ bool HaveDeltas() const
+ {
+ return have_deltas_;
+ }
+
+ /** Method for setting the HaveDeltas flag. This method should be
+ * called if some method computes the primal-dual step (and
+ * stores it in the delta_ fields of IpoptData) at an early part
+ * of the iteration. If that flag is set to true, the
+ * IpoptAlgorithm object will not recompute the step. */
+ void SetHaveDeltas(bool have_deltas)
+ {
+ have_deltas_ = have_deltas;
+ }
+ //@}
+
+ /** @name Affine-scaling step. Those fields can be used to store
+ * the affine scaling step. For example, if the method for
+ * computing the current barrier parameter computes the affine
+ * scaling steps, then the corrector step in the line search does
+ * not have to recompute those solutions of the linear system. */
+ //@{
+
+ /** Returns true, if the affine-scaling step have been already
+ * computed for the current iteration. This flag is reset after
+ * every call of AcceptTrialPoint(). If the search direction is
+ * computed during the computation of the barrier parameter, the
+ * method computing the barrier parameter should call
+ * SetHaveDeltas(true) to tell the line search does not have to
+ * recompute them in case it wants to do a corrector step. */
+ bool HaveAffineDeltas() const
+ {
+ return have_affine_deltas_;
+ }
+
+ /** Method for setting the HaveDeltas flag. This method should be
+ * called if some method computes the primal-dual step (and
+ * stores it in the delta_ fields of IpoptData) at an early part
+ * of the iteration. If that flag is set to true, the
+ * IpoptAlgorithm object will not recompute the step. */
+ void SetHaveAffineDeltas(bool have_affine_deltas)
+ {
+ have_affine_deltas_ = have_affine_deltas;
+ }
+ //@}
+
+
+ /** @name Public Methods for updating iterates */
+ //@{
+ /** Copy the trial values to the current values */
+ void CopyTrialToCurrent();
+
+ /** Set the current iterate values from the
+ * trial values. */
+ void AcceptTrialPoint();
+ //@}
+
+ /** @name General algorithmic data */
+ //@{
+ Index iter_count() const
+ {
+ return iter_count_;
+ }
+ void Set_iter_count(Index iter_count)
+ {
+ iter_count_ = iter_count;
+ }
+
+ Number curr_mu() const
+ {
+ DBG_ASSERT(mu_initialized_);
+ return curr_mu_;
+ }
+ void Set_mu(Number mu)
+ {
+ curr_mu_ = mu;
+ mu_initialized_ = true;
+ }
+ bool MuInitialized() const
+ {
+ return mu_initialized_;
+ }
+
+ Number curr_tau() const
+ {
+ DBG_ASSERT(tau_initialized_);
+ return curr_tau_;
+ }
+ void Set_tau(Number tau)
+ {
+ curr_tau_ = tau;
+ tau_initialized_ = true;
+ }
+ bool TauInitialized() const
+ {
+ return tau_initialized_;
+ }
+
+ void SetFreeMuMode(bool free_mu_mode)
+ {
+ free_mu_mode_ = free_mu_mode;
+ }
+ bool FreeMuMode() const
+ {
+ return free_mu_mode_;
+ }
+
+ /** Setting the flag that indicates if a tiny step (below machine
+ * precision) has been detected */
+ void Set_tiny_step_flag(bool flag)
+ {
+ tiny_step_flag_ = flag;
+ }
+ bool tiny_step_flag()
+ {
+ return tiny_step_flag_;
+ }
+ //@}
+
+ /** Overall convergence tolerance. It is used in the convergence
+ * test, but also in some other parts of the algorithm that
+ * depend on the specified tolerance, such as the minimum value
+ * for the barrier parameter. */
+ //@{
+ /** Obtain the tolerance. */
+ Number tol() const
+ {
+ DBG_ASSERT(initialize_called_);
+ return tol_;
+ }
+ /** Set a new value for the tolerance. One should be very careful
+ * when using this, since changing the predefined tolerance might
+ * have unexpected consequences. This method is for example used
+ * in the restoration convergence checker to tighten the
+ * restoration phase convergence tolerance, if the restoration
+ * phase converged to a point that has not a large value for the
+ * constraint violation. */
+ void Set_tol(Number tol)
+ {
+ tol_ = tol;
+ }
+ //@}
+
+ /** @name Information gathered for iteration output */
+ //@{
+ Number info_regu_x() const
+ {
+ return info_regu_x_;
+ }
+ void Set_info_regu_x(Number regu_x)
+ {
+ info_regu_x_ = regu_x;
+ }
+ Number info_alpha_primal() const
+ {
+ return info_alpha_primal_;
+ }
+ void Set_info_alpha_primal(Number alpha_primal)
+ {
+ info_alpha_primal_ = alpha_primal;
+ }
+ char info_alpha_primal_char() const
+ {
+ return info_alpha_primal_char_;
+ }
+ void Set_info_alpha_primal_char(char info_alpha_primal_char)
+ {
+ info_alpha_primal_char_ = info_alpha_primal_char;
+ }
+ Number info_alpha_dual() const
+ {
+ return info_alpha_dual_;
+ }
+ void Set_info_alpha_dual(Number alpha_dual)
+ {
+ info_alpha_dual_ = alpha_dual;
+ }
+ Index info_ls_count() const
+ {
+ return info_ls_count_;
+ }
+ void Set_info_ls_count(Index ls_count)
+ {
+ info_ls_count_ = ls_count;
+ }
+ bool info_skip_output() const
+ {
+ return info_skip_output_;
+ }
+ void Append_info_string(const std::string& add_str)
+ {
+ info_string_ += add_str;
+ }
+ const std::string& info_string() const
+ {
+ return info_string_;
+ }
+ /** Set this to true, if the next time when output is written, the
+ * summary line should not be printed. */
+ void Set_info_skip_output(bool info_skip_output)
+ {
+ info_skip_output_ = info_skip_output;
+ }
+
+ /** Reset all info fields */
+ void ResetInfo()
+ {
+ info_regu_x_ = 0;
+ info_alpha_primal_ = 0;
+ info_alpha_dual_ = 0.;
+ info_alpha_primal_char_ = ' ';
+ info_ls_count_ = 0;
+ info_skip_output_ = false;
+ info_string_.clear();
+ }
+ //@}
+
+ /** Return Timing Statistics Object */
+ TimingStatistics& TimingStats()
+ {
+ return timing_statistics_;
+ }
+
+ /** Methods for IpoptType */
+ //@{
+ static void RegisterOptions(const SmartPtr<RegisteredOptions>& roptions);
+ //@}
+
+ private:
+ /** @name Iterates */
+ //@{
+ /** Main iteration variables
+ * (current iteration) */
+ SmartPtr<const IteratesVector> curr_;
+
+ /** Main iteration variables
+ * (trial calculations) */
+ SmartPtr<const IteratesVector> trial_;
+
+ /** Hessian (approximation) - might be changed elsewhere! */
+ SmartPtr<const SymMatrix> W_;
+
+ /** @name Primal-dual Step */
+ //@{
+ SmartPtr<const IteratesVector> delta_;
+ /** The following flag is set to true, if some other part of the
+ * algorithm (like the method for computing the barrier
+ * parameter) has already computed the primal-dual search
+ * direction. This flag is reset when the AcceptTrialPoint
+ * method is called.
+ * ToDo: we could cue off of a null delta_;
+ */
+ bool have_deltas_;
+ //@}
+
+ /** @name Affine-scaling step. This used to transfer the
+ * information about the affine-scaling step from the computation
+ * of the barrier parameter to the corrector (in the line
+ * search). */
+ //@{
+ SmartPtr<const IteratesVector> delta_aff_;
+ /** The following flag is set to true, if some other part of the
+ * algorithm (like the method for computing the barrier
+ * parameter) has already computed the affine-scaling step. This
+ * flag is reset when the AcceptTrialPoint method is called.
+ * ToDo: we could cue off of a null delta_aff_;
+ */
+ bool have_affine_deltas_;
+ //@}
+
+ /** iteration count */
+ Index iter_count_;
+
+ /** current barrier parameter */
+ Number curr_mu_;
+ bool mu_initialized_;
+
+ /** current fraction to the boundary parameter */
+ Number curr_tau_;
+ bool tau_initialized_;
+
+ /** flag indicating if Initialize method has been called (for
+ * debugging) */
+ bool initialize_called_;
+
+ /** flag for debugging whether we have already curr_ values
+ * available (from which new Vectors can be generated */
+ bool have_prototypes_;
+
+ /** @name Global algorithm parameters. Those are options that can
+ * be modified by the user and appear at different places in the
+ * algorithm. They are set using an OptionsList object in the
+ * Initialize method. */
+ //@{
+ /** Overall convergence tolerance */
+ Number tol_;
+ //@}
+
+ /** @name Status data **/
+ //@{
+ /** flag indicating whether the algorithm is in the free mu mode */
+ bool free_mu_mode_;
+ /** flag indicating if a tiny step has been detected */
+ bool tiny_step_flag_;
+ //@}
+
+ /** @name Gathered information for iteration output */
+ //@{
+ /** Size of regularization for the Hessian */
+ Number info_regu_x_;
+ /** Primal step size */
+ Number info_alpha_primal_;
+ /** Info character for primal step size */
+ char info_alpha_primal_char_;
+ /** Dual step size */
+ Number info_alpha_dual_;
+ /** Number of backtracking trial steps */
+ Index info_ls_count_;
+ /** true, if next summary output line should not be printed (eg
+ * after restoration phase. */
+ bool info_skip_output_;
+ /** any string of characters for the end of the output line */
+ std::string info_string_;
+ //@}
+
+ /** VectorSpace for all the iterates */
+ SmartPtr<IteratesVectorSpace> iterates_space_;
+
+ /** TimingStatistics object collecting all Ipopt timing
+ * statistics */
+ TimingStatistics timing_statistics_;
+
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ IpoptData(const IpoptData&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const IpoptData&);
+ //@}
+
+#ifdef IP_DEBUG
+ /** Some debug flags to make sure vectors are not changed
+ * behind the IpoptData's back
+ */
+ //@{
+ TaggedObject::Tag debug_curr_tag_;
+ TaggedObject::Tag debug_trial_tag_;
+ TaggedObject::Tag debug_delta_tag_;
+ TaggedObject::Tag debug_delta_aff_tag_;
+ TaggedObject::Tag debug_curr_tag_sum_;
+ TaggedObject::Tag debug_trial_tag_sum_;
+ TaggedObject::Tag debug_delta_tag_sum_;
+ TaggedObject::Tag debug_delta_aff_tag_sum_;
+ //@}
+#endif
+
+ };
+
+ inline
+ SmartPtr<const IteratesVector> IpoptData::curr() const
+ {
+#ifdef IP_DEBUG
+ DBG_ASSERT(IsNull(curr_) || (curr_->GetTag() == debug_curr_tag_ && curr_->GetTagSum() == debug_curr_tag_sum_) );
+#endif
+
+ return curr_;
+ }
+
+ inline
+ SmartPtr<const IteratesVector> IpoptData::trial() const
+ {
+#ifdef IP_DEBUG
+ DBG_ASSERT(IsNull(trial_) || (trial_->GetTag() == debug_trial_tag_ && trial_->GetTagSum() == debug_trial_tag_sum_) );
+#endif
+
+ return trial_;
+ }
+
+ inline
+ SmartPtr<const IteratesVector> IpoptData::delta() const
+ {
+# ifdef IP_DEBUG
+ DBG_ASSERT(IsNull(delta_) || (delta_->GetTag() == debug_delta_tag_ && delta_->GetTagSum() == debug_delta_tag_sum_) );
+# endif
+
+ return delta_;
+ }
+
+ inline
+ SmartPtr<const IteratesVector> IpoptData::delta_aff() const
+ {
+# ifdef IP_DEBUG
+ DBG_ASSERT(IsNull(delta_aff_) || (delta_aff_->GetTag() == debug_delta_aff_tag_ && delta_aff_->GetTagSum() == debug_delta_aff_tag_sum_) );
+# endif
+
+ return delta_aff_;
+ }
+
+ inline
+ void IpoptData::CopyTrialToCurrent()
+ {
+ curr_ = trial_;
+#ifdef IP_DEBUG
+
+ if (IsValid(curr_)) {
+ debug_curr_tag_ = curr_->GetTag();
+ debug_curr_tag_sum_ = curr_->GetTagSum();
+ }
+ else {
+ debug_curr_tag_ = 0;
+ debug_curr_tag_sum_ = 0;
+ }
+#endif
+
+ }
+
+ inline
+ void IpoptData::set_trial(SmartPtr<IteratesVector>& trial)
+ {
+ trial_ = ConstPtr(trial);
+
+#ifdef IP_DEBUG
+ // verify the correct space
+ DBG_ASSERT(trial_->OwnerSpace() == (VectorSpace*)GetRawPtr(iterates_space_));
+ if (IsValid(trial)) {
+ debug_trial_tag_ = trial->GetTag();
+ debug_trial_tag_sum_ = trial->GetTagSum();
+ }
+ else {
+ debug_trial_tag_ = 0;
+ debug_trial_tag_sum_ = 0;
+ }
+#endif
+
+ trial = NULL;
+ }
+
+ inline
+ void IpoptData::set_delta(SmartPtr<IteratesVector>& delta)
+ {
+ delta_ = ConstPtr(delta);
+#ifdef IP_DEBUG
+
+ if (IsValid(delta)) {
+ debug_delta_tag_ = delta->GetTag();
+ debug_delta_tag_sum_ = delta->GetTagSum();
+ }
+ else {
+ debug_delta_tag_ = 0;
+ debug_delta_tag_sum_ = 0;
+ }
+#endif
+
+ delta = NULL;
+ }
+
+ inline
+ void IpoptData::set_delta_aff(SmartPtr<IteratesVector>& delta_aff)
+ {
+ delta_aff_ = ConstPtr(delta_aff);
+#ifdef IP_DEBUG
+
+ if (IsValid(delta_aff)) {
+ debug_delta_aff_tag_ = delta_aff->GetTag();
+ debug_delta_aff_tag_sum_ = delta_aff->GetTagSum();
+ }
+ else {
+ debug_delta_aff_tag_ = 0;
+ debug_delta_aff_tag_sum_ = delta_aff->GetTagSum();
+ }
+#endif
+
+ delta_aff = NULL;
+ }
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpIpoptNLP.hpp b/SimTKmath/Optimizers/src/IpOpt/IpIpoptNLP.hpp
new file mode 100644
index 0000000..0ec13fa
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpIpoptNLP.hpp
@@ -0,0 +1,256 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpIpoptNLP.hpp 765 2006-07-14 18:03:23Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPIPOPTNLP_HPP__
+#define __IPIPOPTNLP_HPP__
+
+#include "IpNLP.hpp"
+#include "IpJournalist.hpp"
+#include "IpNLPScaling.hpp"
+
+namespace Ipopt
+{
+ // forward declarations
+ class IteratesVector;
+
+ /** This is the abstract base class for classes that map
+ * the traditional NLP into
+ * something that is more useful by Ipopt.
+ * This class takes care of storing the
+ * calculated model results, handles cacheing,
+ * and (some day) takes care of addition of slacks.
+ */
+ class IpoptNLP : public ReferencedObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ IpoptNLP(const SmartPtr<NLPScalingObject> nlp_scaling)
+ :
+ nlp_scaling_(nlp_scaling)
+ {}
+
+ /** Default destructor */
+ virtual ~IpoptNLP()
+ {}
+ //@}
+
+ /** Initialization method. Set the internal options and
+ * initialize internal data structures. */
+ virtual bool Initialize(const Journalist& jnlst,
+ const OptionsList& options,
+ const std::string& prefix)
+ {
+ bool ret = true;
+ if (IsValid(nlp_scaling_)) {
+ ret = nlp_scaling_->Initialize(jnlst, options, prefix);
+ }
+ return ret;
+ }
+
+ /**@name Possible Exceptions */
+ //@{
+ /** thrown if there is any error evaluating values from the nlp */
+ DECLARE_STD_EXCEPTION(Eval_Error);
+ //@}
+ /** Initialize (create) structures for
+ * the iteration data */
+ virtual bool InitializeStructures(SmartPtr<Vector>& x,
+ bool init_x,
+ SmartPtr<Vector>& y_c,
+ bool init_y_c,
+ SmartPtr<Vector>& y_d,
+ bool init_y_d,
+ SmartPtr<Vector>& z_L,
+ bool init_z_L,
+ SmartPtr<Vector>& z_U,
+ bool init_z_U,
+ SmartPtr<Vector>& v_L,
+ SmartPtr<Vector>& v_U
+ ) = 0;
+
+ /** Method accessing the GetWarmStartIterate of the NLP */
+ virtual bool GetWarmStartIterate(IteratesVector& warm_start_iterate)=0;
+
+ /** Accessor methods for model data */
+ //@{
+ /** Objective value */
+ virtual Number f(const Vector& x) = 0;
+
+ /** Gradient of the objective */
+ virtual SmartPtr<const Vector> grad_f(const Vector& x) = 0;
+
+ /** Equality constraint residual */
+ virtual SmartPtr<const Vector> c(const Vector& x) = 0;
+
+ /** Jacobian Matrix for equality constraints */
+ virtual SmartPtr<const Matrix> jac_c(const Vector& x) = 0;
+
+ /** Inequality constraint residual (reformulated
+ * as equalities with slacks */
+ virtual SmartPtr<const Vector> d(const Vector& x) = 0;
+
+ /** Jacobian Matrix for inequality constraints */
+ virtual SmartPtr<const Matrix> jac_d(const Vector& x) = 0;
+
+ /** Hessian of the Lagrangian */
+ virtual SmartPtr<const SymMatrix> h(const Vector& x,
+ Number obj_factor,
+ const Vector& yc,
+ const Vector& yd
+ ) = 0;
+
+ /** Lower bounds on x */
+ virtual SmartPtr<const Vector> x_L() = 0;
+
+ /** Permutation matrix (x_L_ -> x) */
+ virtual SmartPtr<const Matrix> Px_L() = 0;
+
+ /** Upper bounds on x */
+ virtual SmartPtr<const Vector> x_U() = 0;
+
+ /** Permutation matrix (x_U_ -> x */
+ virtual SmartPtr<const Matrix> Px_U() = 0;
+
+ /** Lower bounds on d */
+ virtual SmartPtr<const Vector> d_L() = 0;
+
+ /** Permutation matrix (d_L_ -> d) */
+ virtual SmartPtr<const Matrix> Pd_L() = 0;
+
+ /** Upper bounds on d */
+ virtual SmartPtr<const Vector> d_U() = 0;
+
+ /** Permutation matrix (d_U_ -> d */
+ virtual SmartPtr<const Matrix> Pd_U() = 0;
+
+ /** Accessor method to obtain the MatrixSpace for the Hessian
+ * matrix (or it's approximation) */
+ virtual SmartPtr<const SymMatrixSpace> HessianMatrixSpace() const = 0;
+ //@}
+
+ /** Accessor method for vector/matrix spaces pointers. */
+ virtual void GetSpaces(SmartPtr<const VectorSpace>& x_space,
+ SmartPtr<const VectorSpace>& c_space,
+ SmartPtr<const VectorSpace>& d_space,
+ SmartPtr<const VectorSpace>& x_l_space,
+ SmartPtr<const MatrixSpace>& px_l_space,
+ SmartPtr<const VectorSpace>& x_u_space,
+ SmartPtr<const MatrixSpace>& px_u_space,
+ SmartPtr<const VectorSpace>& d_l_space,
+ SmartPtr<const MatrixSpace>& pd_l_space,
+ SmartPtr<const VectorSpace>& d_u_space,
+ SmartPtr<const MatrixSpace>& pd_u_space,
+ SmartPtr<const MatrixSpace>& Jac_c_space,
+ SmartPtr<const MatrixSpace>& Jac_d_space,
+ SmartPtr<const SymMatrixSpace>& Hess_lagrangian_space) = 0;
+
+ /** Method for adapting the variable bounds. This is called if
+ * slacks are becoming too small */
+ virtual void AdjustVariableBounds(const Vector& new_x_L,
+ const Vector& new_x_U,
+ const Vector& new_d_L,
+ const Vector& new_d_U)=0;
+
+ /** @name Counters for the number of function evaluations. */
+ //@{
+ virtual Index f_evals() const = 0;
+ virtual Index grad_f_evals() const = 0;
+ virtual Index c_evals() const = 0;
+ virtual Index jac_c_evals() const = 0;
+ virtual Index d_evals() const = 0;
+ virtual Index jac_d_evals() const = 0;
+ virtual Index h_evals() const = 0;
+ //@}
+
+ /** @name Special method for dealing with the fact that the
+ * restoration phase objective function depends on the barrier
+ * parameter */
+ //@{
+ /** Method for telling the IpoptCalculatedQuantities class whether
+ * the objective function depends on the barrier function. This
+ * is only used for the restoration phase NLP
+ * formulation. Probably only RestoIpoptNLP should overwrite
+ * this. */
+ virtual bool objective_depends_on_mu() const
+ {
+ return false;
+ }
+
+ /** Replacement for the default objective function method which
+ * knows about the barrier parameter */
+ virtual Number f(const Vector& x, Number mu) = 0;
+
+ /** Replacement for the default objective gradient method which
+ * knows about the barrier parameter */
+ virtual SmartPtr<const Vector> grad_f(const Vector& x, Number mu) = 0;
+
+ /** Replacement for the default Lagrangian Hessian method which
+ * knows about the barrier parameter */
+ virtual SmartPtr<const SymMatrix> h(const Vector& x,
+ Number obj_factor,
+ const Vector& yc,
+ const Vector& yd,
+ Number mu) = 0;
+
+ /** Provides a Hessian matrix from the correct matrix space with
+ * uninitialized values. This can be used in LeastSquareMults to
+ * obtain a "zero Hessian". */
+ virtual SmartPtr<const SymMatrix> uninitialized_h() = 0;
+ //@}
+
+ /**@name solution routines */
+ //@{
+ virtual void FinalizeSolution(SolverReturn status,
+ const Vector& x, const Vector& z_L, const Vector& z_U,
+ const Vector& c, const Vector& d,
+ const Vector& y_c, const Vector& y_d,
+ Number obj_value)=0;
+
+ virtual bool IntermediateCallBack(AlgorithmMode mode,
+ Index iter, Number obj_value,
+ Number inf_pr, Number inf_du,
+ Number mu, Number d_norm,
+ Number regularization_size,
+ Number alpha_du, Number alpha_pr,
+ Index ls_trials,
+ SmartPtr<const IpoptData> ip_data,
+ SmartPtr<IpoptCalculatedQuantities> ip_cq)=0;
+ //@}
+
+ /** Returns the scaling strategy object */
+ SmartPtr<NLPScalingObject> NLP_scaling() const
+ {
+ DBG_ASSERT(IsValid(nlp_scaling_));
+ return nlp_scaling_;
+ }
+
+ private:
+
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+
+ /** Copy Constructor */
+ IpoptNLP(const IpoptNLP&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const IpoptNLP&);
+ //@}
+
+ SmartPtr<NLPScalingObject> nlp_scaling_;
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpIterateInitializer.hpp b/SimTKmath/Optimizers/src/IpOpt/IpIterateInitializer.hpp
new file mode 100644
index 0000000..fb4f8bb
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpIterateInitializer.hpp
@@ -0,0 +1,64 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpIterateInitializer.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-09-24
+
+#ifndef __IPITERATEINITIALIZER_HPP__
+#define __IPITERATEINITIALIZER_HPP__
+
+#include "IpAlgStrategy.hpp"
+#include "IpIpoptNLP.hpp"
+#include "IpIpoptData.hpp"
+#include "IpIpoptCalculatedQuantities.hpp"
+
+namespace Ipopt
+{
+
+ /** Base class for all methods for initializing the iterates.
+ */
+ class IterateInitializer: public AlgorithmStrategyObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor */
+ IterateInitializer()
+ {}
+
+ /** Default destructor */
+ virtual ~IterateInitializer()
+ {}
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix) = 0;
+
+ /** Compute the initial iterates and set the into the curr field
+ * of the ip_data object. */
+ virtual bool SetInitialIterates() = 0;
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ IterateInitializer(const IterateInitializer&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const IterateInitializer&);
+ //@}
+
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpIteratesVector.cpp b/SimTKmath/Optimizers/src/IpOpt/IpIteratesVector.cpp
new file mode 100644
index 0000000..3bb7d0e
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpIteratesVector.cpp
@@ -0,0 +1,155 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpIteratesVector.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-06-06
+
+#include "IpIteratesVector.hpp"
+
+namespace Ipopt
+{
+
+ IteratesVector::IteratesVector(const IteratesVectorSpace* owner_space, bool create_new)
+ :
+ CompoundVector(owner_space, create_new),
+ owner_space_(owner_space)
+ {
+ DBG_ASSERT(owner_space_);
+ }
+
+ IteratesVector::~IteratesVector()
+ {}
+
+ SmartPtr<IteratesVector> IteratesVector::MakeNewIteratesVector(bool create_new) const
+ {
+ return owner_space_->MakeNewIteratesVector(create_new);
+ }
+
+ SmartPtr<IteratesVector> IteratesVector::MakeNewContainer() const
+ {
+ SmartPtr<IteratesVector> ret = MakeNewIteratesVector(false);
+
+ if (IsValid(x())) {
+ ret->Set_x(*x());
+ }
+ if (IsValid(s())) {
+ ret->Set_s(*s());
+ }
+ if (IsValid(y_c())) {
+ ret->Set_y_c(*y_c());
+ }
+ if (IsValid(y_d())) {
+ ret->Set_y_d(*y_d());
+ }
+ if (IsValid(z_L())) {
+ ret->Set_z_L(*z_L());
+ }
+ if (IsValid(z_U())) {
+ ret->Set_z_U(*z_U());
+ }
+ if (IsValid(v_L())) {
+ ret->Set_v_L(*v_L());
+ }
+ if (IsValid(v_U())) {
+ ret->Set_v_U(*v_U());
+ }
+
+ return ret;
+
+ // We may need a non const version
+ // if (IsCompConst(0)) {
+ // ret->Set_x(*x());
+ // }
+ // else {
+ // ret->Set_x_NonConst(*x_NonConst());
+ // }
+
+ // if (IsCompConst(1)) {
+ // ret->Set_s(*s());
+ // }
+ // else {
+ // ret->Set_s_NonConst(*s_NonConst());
+ // }
+
+ // if (IsCompConst(2)) {
+ // ret->Set_y_c(*y_c());
+ // }
+ // else {
+ // ret->Set_y_c_NonConst(*y_c_NonConst());
+ // }
+
+ // if (IsCompConst(3)) {
+ // ret->Set_y_d(*y_d());
+ // }
+ // else {
+ // ret->Set_y_d_NonConst(*y_d_NonConst());
+ // }
+
+ // if (IsCompConst(4)) {
+ // ret->Set_z_L(*z_L());
+ // }
+ // else {
+ // ret->Set_z_L_NonConst(*z_L_NonConst());
+ // }
+
+ // if (IsCompConst(5)) {
+ // ret->Set_z_U(*z_U());
+ // }
+ // else {
+ // ret->Set_z_U_NonConst(*z_U_NonConst());
+ // }
+
+ // if (IsCompConst(6)) {
+ // ret->Set_v_L(*v_L());
+ // }
+ // else {
+ // ret->Set_v_L_NonConst(*v_L_NonConst());
+ // }
+
+ // if (IsCompConst(7)) {
+ // ret->Set_v_U(*v_U());
+ // }
+ // else {
+ // ret->Set_v_U_NonConst(*v_U_NonConst());
+ // }
+
+ // return ret;
+ }
+
+ IteratesVectorSpace::IteratesVectorSpace(const VectorSpace& x_space, const VectorSpace& s_space,
+ const VectorSpace& y_c_space, const VectorSpace& y_d_space,
+ const VectorSpace& z_L_space, const VectorSpace& z_U_space,
+ const VectorSpace& v_L_space, const VectorSpace& v_U_space
+ )
+ :
+ CompoundVectorSpace(8, x_space.Dim() + s_space.Dim()
+ + y_c_space.Dim() + y_d_space.Dim()
+ + z_L_space.Dim() + z_U_space.Dim()
+ + v_L_space.Dim() + v_U_space.Dim()
+ )
+ {
+ x_space_ = &x_space;
+ s_space_ = &s_space;
+ y_c_space_ = &y_c_space;
+ y_d_space_ = &y_d_space;
+ z_L_space_ = &z_L_space;
+ z_U_space_ = &z_U_space;
+ v_L_space_ = &v_L_space;
+ v_U_space_ = &v_U_space;
+
+ this->CompoundVectorSpace::SetCompSpace(0, *x_space_);
+ this->CompoundVectorSpace::SetCompSpace(1, *s_space_);
+ this->CompoundVectorSpace::SetCompSpace(2, *y_c_space_);
+ this->CompoundVectorSpace::SetCompSpace(3, *y_d_space_);
+ this->CompoundVectorSpace::SetCompSpace(4, *z_L_space_);
+ this->CompoundVectorSpace::SetCompSpace(5, *z_U_space_);
+ this->CompoundVectorSpace::SetCompSpace(6, *v_L_space_);
+ this->CompoundVectorSpace::SetCompSpace(7, *v_U_space_);
+ }
+
+ IteratesVectorSpace::~IteratesVectorSpace()
+ {}
+
+} // namespae Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpIteratesVector.hpp b/SimTKmath/Optimizers/src/IpOpt/IpIteratesVector.hpp
new file mode 100644
index 0000000..3107fc0
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpIteratesVector.hpp
@@ -0,0 +1,681 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpIteratesVector.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-06-06
+
+#ifndef __IPITERATESVECTOR_HPP__
+#define __IPITERATESVECTOR_HPP__
+
+#include "IpCompoundVector.hpp"
+
+namespace Ipopt
+{
+ /* forward declarations */
+ class IteratesVectorSpace;
+
+ /** Specialized CompoundVector class specifically for the algorithm
+ * iterates. This class inherits from CompoundVector and is a
+ * specialized class for handling the iterates of the Ipopt
+ * Algorithm, that is, x, s, y_c, y_d, z_L, z_U, v_L, and v_U. It
+ * inherits from CompoundVector so it can behave like a CV in most
+ * calculations, but it has fixed dimensions and cannot be
+ * customized
+ */
+ class IteratesVector : public CompoundVector
+ {
+ public:
+ /** Constructors / Destructors */
+ //@{
+ IteratesVector(const IteratesVectorSpace* owner_space, bool create_new);
+
+ virtual ~IteratesVector();
+ //@}
+
+ /** Make New methods */
+ //@{
+ /** Use this method to create a new iterates vector. The MakeNew
+ * method on the Vector class also works, but it does not give
+ * the create_new option.
+ */
+ SmartPtr<IteratesVector> MakeNewIteratesVector(bool create_new = true) const;
+
+ /** Use this method to create a new iterates vector with a copy of
+ * all the data.
+ */
+ SmartPtr<IteratesVector> MakeNewIteratesVectorCopy() const
+ {
+ SmartPtr<IteratesVector> ret = MakeNewIteratesVector(true);
+ ret->Copy(*this);
+ return ret;
+ }
+
+ /** Use this method to create a new iterates vector
+ * container. This creates a new NonConst container, but the
+ * elements inside the iterates vector may be const. Therefore,
+ * the container can be modified to point to new entries, but the
+ * existing entries may or may not be modifiable.
+ */
+ SmartPtr<IteratesVector> MakeNewContainer() const;
+ //@}
+
+ /** Iterates Set/Get Methods */
+ //@{
+ /** Get the x iterate (const) */
+ SmartPtr<const Vector> x() const
+ {
+ return GetIterateFromComp(0);
+ }
+
+ /** Get the x iterate (non-const) - this can only be called if the
+ * vector was created intenally, or the Set_x_NonConst method was
+ * used. */
+ SmartPtr<Vector> x_NonConst()
+ {
+ return GetNonConstIterateFromComp(0);
+ }
+
+ /** Create a new vector in the x entry */
+ SmartPtr<Vector> create_new_x();
+
+ /** Create a new vector in the x entry and copy the current values
+ * into it. */
+ SmartPtr<Vector> create_new_x_copy()
+ {
+ SmartPtr<const Vector> curr_x = GetComp(0);
+ Set_x_NonConst(*curr_x->MakeNew());
+ x_NonConst()->Copy(*curr_x);
+ return x_NonConst();
+ }
+
+ /** Set the x iterate (const). Sets the pointer, does NOT copy
+ * data. */
+ void Set_x(const Vector& vec)
+ {
+ SetComp(0, vec);
+ }
+
+ /** Set the x iterate (non-const). Sets the pointer, does NOT copy
+ * data. */
+ void Set_x_NonConst(Vector& vec)
+ {
+ SetCompNonConst(0, vec);
+ }
+
+ /** Get the s iterate (const) */
+ SmartPtr<const Vector> s() const
+ {
+ return GetIterateFromComp(1);
+ }
+
+ /** Get the s iterate (non-const) - this can only be called if the
+ * vector was created intenally, or the Set_s_NonConst method was
+ * used. */
+ SmartPtr<Vector> s_NonConst()
+ {
+ return GetNonConstIterateFromComp(1);
+ }
+
+ /** Create a new vector in the s entry */
+ SmartPtr<Vector> create_new_s();
+
+ /** Create a new vector in the s entry and copy the current values
+ * into it. */
+ SmartPtr<Vector> create_new_s_copy()
+ {
+ SmartPtr<const Vector> curr_s = GetComp(1);
+ Set_s_NonConst(*curr_s->MakeNew());
+ s_NonConst()->Copy(*curr_s);
+ return s_NonConst();
+ }
+
+ /** Set the s iterate (const). Sets the pointer, does NOT copy
+ * data. */
+ void Set_s(const Vector& vec)
+ {
+ SetComp(1, vec);
+ }
+
+ /** Set the s iterate (non-const). Sets the pointer, does NOT copy
+ * data. */
+ void Set_s_NonConst(Vector& vec)
+ {
+ SetCompNonConst(1, vec);
+ }
+
+ /** Get the y_c iterate (const) */
+ SmartPtr<const Vector> y_c() const
+ {
+ return GetIterateFromComp(2);
+ }
+
+ /** Get the y_c iterate (non-const) - this can only be called if
+ * the vector was created intenally, or the Set_y_c_NonConst
+ * method was used. */
+ SmartPtr<Vector> y_c_NonConst()
+ {
+ return GetNonConstIterateFromComp(2);
+ }
+
+ /** Create a new vector in the y_c entry */
+ SmartPtr<Vector> create_new_y_c();
+
+ /** Create a new vector in the y_c entry and copy the current
+ * values into it. */
+ SmartPtr<Vector> create_new_y_c_copy()
+ {
+ SmartPtr<const Vector> curr_y_c = GetComp(2);
+ Set_y_c_NonConst(*curr_y_c->MakeNew());
+ y_c_NonConst()->Copy(*curr_y_c);
+ return y_c_NonConst();
+ }
+
+ /** Set the y_c iterate (const). Sets the pointer, does NOT copy
+ * data. */
+ void Set_y_c(const Vector& vec)
+ {
+ SetComp(2, vec);
+ }
+
+ /** Set the y_c iterate (non-const). Sets the pointer, does NOT
+ * copy data. */
+ void Set_y_c_NonConst(Vector& vec)
+ {
+ SetCompNonConst(2, vec);
+ }
+
+ /** Get the y_d iterate (const) */
+ SmartPtr<const Vector> y_d() const
+ {
+ return GetIterateFromComp(3);
+ }
+
+ /** Get the y_d iterate (non-const) - this can only be called if
+ * the vector was created intenally, or the Set_y_d_NonConst
+ * method was used. */
+ SmartPtr<Vector> y_d_NonConst()
+ {
+ return GetNonConstIterateFromComp(3);
+ }
+
+ /** Create a new vector in the y_d entry */
+ SmartPtr<Vector> create_new_y_d();
+
+ /** Create a new vector in the y_d entry and copy the current
+ * values into it. */
+ SmartPtr<Vector> create_new_y_d_copy()
+ {
+ SmartPtr<const Vector> curr_y_d = GetComp(3);
+ Set_y_d_NonConst(*curr_y_d->MakeNew());
+ y_d_NonConst()->Copy(*curr_y_d);
+ return y_d_NonConst();
+ }
+
+ /** Set the y_d iterate (const). Sets the pointer, does NOT copy
+ * data. */
+ void Set_y_d(const Vector& vec)
+ {
+ SetComp(3, vec);
+ }
+
+ /** Set the y_d iterate (non-const). Sets the pointer, does NOT
+ * copy data. */
+ void Set_y_d_NonConst(Vector& vec)
+ {
+ SetCompNonConst(3, vec);
+ }
+
+ /** Get the z_L iterate (const) */
+ SmartPtr<const Vector> z_L() const
+ {
+ return GetIterateFromComp(4);
+ }
+
+ /** Get the z_L iterate (non-const) - this can only be called if
+ * the vector was created intenally, or the Set_z_L_NonConst
+ * method was used. */
+ SmartPtr<Vector> z_L_NonConst()
+ {
+ return GetNonConstIterateFromComp(4);
+ }
+
+ /** Create a new vector in the z_L entry */
+ SmartPtr<Vector> create_new_z_L() ;
+
+ /** Create a new vector in the z_L entry and copy the current
+ * values into it. */
+ SmartPtr<Vector> create_new_z_L_copy()
+ {
+ SmartPtr<const Vector> curr_z_L = GetComp(4);
+ Set_z_L_NonConst(*curr_z_L->MakeNew());
+ z_L_NonConst()->Copy(*curr_z_L);
+ return z_L_NonConst();
+ }
+
+ /** Set the z_L iterate (const). Sets the pointer, does NOT copy
+ * data. */
+ void Set_z_L(const Vector& vec)
+ {
+ SetComp(4, vec);
+ }
+
+ /** Set the z_L iterate (non-const). Sets the pointer, does NOT
+ * copy data. */
+ void Set_z_L_NonConst(Vector& vec)
+ {
+ SetCompNonConst(4, vec);
+ }
+
+ /** Get the z_U iterate (const) */
+ SmartPtr<const Vector> z_U() const
+ {
+ return GetIterateFromComp(5);
+ }
+
+ /** Get the z_U iterate (non-const) - this can only be called if
+ * the vector was created intenally, or the Set_z_U_NonConst
+ * method was used. */
+ SmartPtr<Vector> z_U_NonConst()
+ {
+ return GetNonConstIterateFromComp(5);
+ }
+
+ /** Create a new vector in the z_U entry */
+ SmartPtr<Vector> create_new_z_U();
+
+ /** Create a new vector in the z_U entry and copy the current
+ * values into it. */
+ SmartPtr<Vector> create_new_z_U_copy()
+ {
+ SmartPtr<const Vector> curr_z_U = GetComp(5);
+ Set_z_U_NonConst(*curr_z_U->MakeNew());
+ z_U_NonConst()->Copy(*curr_z_U);
+ return z_U_NonConst();
+ }
+
+ /** Set the z_U iterate (const). Sets the pointer, does NOT copy
+ * data. */
+ void Set_z_U(const Vector& vec)
+ {
+ SetComp(5, vec);
+ }
+
+ /** Set the z_U iterate (non-const). Sets the pointer, does NOT
+ * copy data. */
+ void Set_z_U_NonConst(Vector& vec)
+ {
+ SetCompNonConst(5, vec);
+ }
+
+ /** Get the v_L iterate (const) */
+ SmartPtr<const Vector> v_L() const
+ {
+ return GetIterateFromComp(6);
+ }
+
+ /** Get the v_L iterate (non-const) - this can only be called if
+ * the vector was created intenally, or the Set_v_L_NonConst
+ * method was used. */
+ SmartPtr<Vector> v_L_NonConst()
+ {
+ return GetNonConstIterateFromComp(6);
+ }
+
+ /** Create a new vector in the v_L entry */
+ SmartPtr<Vector> create_new_v_L();
+
+ /** Create a new vector in the v_L entry and copy the current
+ * values into it. */
+ SmartPtr<Vector> create_new_v_L_copy()
+ {
+ SmartPtr<const Vector> curr_v_L = GetComp(6);
+ Set_v_L_NonConst(*curr_v_L->MakeNew());
+ v_L_NonConst()->Copy(*curr_v_L);
+ return v_L_NonConst();
+ }
+
+ /** Set the v_L iterate (const). Sets the pointer, does NOT copy
+ * data. */
+ void Set_v_L(const Vector& vec)
+ {
+ SetComp(6, vec);
+ }
+
+ /** Set the v_L iterate (non-const). Sets the pointer, does NOT
+ * copy data. */
+ void Set_v_L_NonConst(Vector& vec)
+ {
+ SetCompNonConst(6, vec);
+ }
+
+ /** Get the v_U iterate (const) */
+ SmartPtr<const Vector> v_U() const
+ {
+ return GetIterateFromComp(7);
+ }
+
+ /** Get the v_U iterate (non-const) - this can only be called if
+ * the vector was created intenally, or the Set_v_U_NonConst
+ * method was used. */
+ SmartPtr<Vector> v_U_NonConst()
+ {
+ return GetNonConstIterateFromComp(7);
+ }
+
+ /** Create a new vector in the v_U entry */
+ SmartPtr<Vector> create_new_v_U();
+
+ /** Create a new vector in the v_U entry and copy the current
+ * values into it. */
+ SmartPtr<Vector> create_new_v_U_copy()
+ {
+ SmartPtr<const Vector> curr_v_U = GetComp(7);
+ Set_v_U_NonConst(*curr_v_U->MakeNew());
+ v_U_NonConst()->Copy(*curr_v_U);
+ return v_U_NonConst();
+ }
+
+ /** Set the v_U iterate (const). Sets the pointer, does NOT copy
+ * data. */
+ void Set_v_U(const Vector& vec)
+ {
+ SetComp(7, vec);
+ }
+
+ /** Set the v_U iterate (non-const). Sets the pointer, does NOT
+ * copy data. */
+ void Set_v_U_NonConst(Vector& vec)
+ {
+ SetCompNonConst(7, vec);
+ }
+
+ /** Set the primal variables all in one shot. Sets the pointers,
+ * does NOT copy data */
+ void Set_primal(const Vector& x, const Vector& s)
+ {
+ SetComp(0, x);
+ SetComp(1, s);
+ }
+ void Set_primal_NonConst(Vector& x, Vector& s)
+ {
+ SetCompNonConst(0, x);
+ SetCompNonConst(1, s);
+ }
+
+ /** Set the eq multipliers all in one shot. Sets the pointers,
+ * does not copy data. */
+ void Set_eq_mult(const Vector& y_c, const Vector& y_d)
+ {
+ SetComp(2, y_c);
+ SetComp(3, y_d);
+ }
+ void Set_eq_mult_NonConst(Vector& y_c, Vector& y_d)
+ {
+ SetCompNonConst(2, y_c);
+ SetCompNonConst(3, y_d);
+ }
+
+ /** Set the bound multipliers all in one shot. Sets the pointers,
+ * does not copy data. */
+ void Set_bound_mult(const Vector& z_L, const Vector& z_U, const Vector& v_L, const Vector& v_U)
+ {
+ SetComp(4, z_L);
+ SetComp(5, z_U);
+ SetComp(6, v_L);
+ SetComp(7, v_U);
+ }
+ void Set_bound_mult_NonConst(Vector& z_L, Vector& z_U, Vector& v_L, Vector& v_U)
+ {
+ SetCompNonConst(4, z_L);
+ SetCompNonConst(5, z_U);
+ SetCompNonConst(6, v_L);
+ SetCompNonConst(7, v_U);
+ }
+
+ /** Get a sum of the tags of the contained items. There is no
+ * guarantee that this is unique, but there is a high chance it
+ * is unique and it can be used for debug checks relatively
+ * reliably.
+ */
+ TaggedObject::Tag GetTagSum() const
+ {
+ TaggedObject::Tag tag = 0;
+
+ if (IsValid(x())) {
+ tag += x()->GetTag();
+ }
+ if (IsValid(s())) {
+ tag += s()->GetTag();
+ }
+ if (IsValid(y_c())) {
+ tag += y_c()->GetTag();
+ }
+ if (IsValid(y_d())) {
+ tag += y_d()->GetTag();
+ }
+ if (IsValid(z_L())) {
+ tag += z_L()->GetTag();
+ }
+ if (IsValid(z_U())) {
+ tag += z_U()->GetTag();
+ }
+ if (IsValid(v_L())) {
+ tag += v_L()->GetTag();
+ }
+ if (IsValid(v_U())) {
+ tag += v_U()->GetTag();
+ }
+
+ return tag;
+ }
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods (Hidden to avoid
+ * implicit creation/calling). These methods are not implemented
+ * and we do not want the compiler to implement them for us, so we
+ * declare them private and do not define them. This ensures that
+ * they will not be implicitly created/called.
+ */
+ //@{
+ /** Default Constructor */
+ IteratesVector();
+
+ /** Copy Constructor */
+ IteratesVector(const IteratesVector&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const IteratesVector&);
+ //@}
+
+ const IteratesVectorSpace* owner_space_;
+
+ /** private method to return the const element from the compound
+ * vector. This method will return NULL if none is currently
+ * set.
+ */
+ SmartPtr<const Vector> GetIterateFromComp(Index i) const
+ {
+ if (IsCompNull(i)) {
+ return NULL;
+ }
+ return GetComp(i);
+ }
+
+ /** private method to return the non-const element from the
+ * compound vector. This method will return NULL if none is
+ * currently set.
+ */
+ SmartPtr<Vector> GetNonConstIterateFromComp(Index i)
+ {
+ if (IsCompNull(i)) {
+ return NULL;
+ }
+ return GetCompNonConst(i);
+ }
+
+ };
+
+ /** Vector Space for the IteratesVector class. This is a
+ * specialized vector space for the IteratesVector class.
+ */
+ class IteratesVectorSpace : public CompoundVectorSpace
+ {
+ public:
+ /** @name Constructors/Destructors. */
+ //@{
+ /** Constructor that takes the spaces for each of the iterates.
+ * Warning! None of these can be NULL !
+ */
+ IteratesVectorSpace(const VectorSpace& x_space, const VectorSpace& s_space,
+ const VectorSpace& y_c_space, const VectorSpace& y_d_space,
+ const VectorSpace& z_L_space, const VectorSpace& z_U_space,
+ const VectorSpace& v_L_space, const VectorSpace& v_U_space
+ );
+
+ virtual ~IteratesVectorSpace();
+ //@}
+
+ /** Method for creating vectors . */
+ //@{
+ /** Use this to create a new IteratesVector. You can pass-in
+ * create_new = false if you only want a container and do not
+ * want vectors allocated.
+ */
+ virtual IteratesVector* MakeNewIteratesVector(bool create_new = true) const
+ {
+ return new IteratesVector(this, create_new);
+ }
+
+ /** Use this method to create a new const IteratesVector. You must pass in
+ * valid pointers for all of the entries.
+ */
+ const SmartPtr<const IteratesVector> MakeNewIteratesVector(const Vector& x, const Vector& s,
+ const Vector& y_c, const Vector& y_d,
+ const Vector& z_L, const Vector& z_U,
+ const Vector& v_L, const Vector& v_U)
+ {
+ SmartPtr<IteratesVector> newvec = MakeNewIteratesVector(false);
+ newvec->Set_x(x);
+ newvec->Set_s(s);
+ newvec->Set_y_c(y_c);
+ newvec->Set_y_d(y_d);
+ newvec->Set_z_L(z_L);
+ newvec->Set_z_U(z_U);
+ newvec->Set_v_L(v_L);
+ newvec->Set_v_U(v_U);
+ return ConstPtr(newvec);
+ }
+
+
+ /** This method overloads
+ * ComooundVectorSpace::MakeNewCompoundVector to make sure that
+ * we get a vector of the correct type
+ */
+ virtual CompoundVector* MakeNewCompoundVector(bool create_new = true) const
+ {
+ return MakeNewIteratesVector(create_new);
+ }
+
+ /** This method creates a new vector (and allocates space in all
+ * the contained vectors. This is really only used for code that
+ * does not know what type of vector it is dealing with - for
+ * example, this method is called from Vector::MakeNew()
+ */
+ virtual Vector* MakeNew() const
+ {
+ return MakeNewIteratesVector();
+ }
+ //@}
+
+ /** This method hides the CompoundVectorSpace::SetCompSpace method
+ * since the components of the Iterates are fixed at
+ * construction.
+ */
+ virtual void SetCompSpace(Index icomp, const VectorSpace& vec_space)
+ {
+ DBG_ASSERT(false && "This is an IteratesVectorSpace - a special compound vector for Ipopt iterates. The contained spaces should not be modified.");
+ }
+
+ private:
+ /**@name Default Compiler Generated Methods (Hidden to avoid
+ * implicit creation/calling). These methods are not implemented
+ * and we do not want the compiler to implement them for us, so we
+ * declare them private and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default constructor */
+ IteratesVectorSpace();
+
+ /** Copy Constructor */
+ IteratesVectorSpace(const IteratesVectorSpace&);
+
+ /** Overloaded Equals Operator */
+ IteratesVectorSpace& operator=(const IteratesVectorSpace&);
+ //@}
+
+ /** Contained Spaces */
+ SmartPtr<const VectorSpace> x_space_;
+ SmartPtr<const VectorSpace> s_space_;
+ SmartPtr<const VectorSpace> y_c_space_;
+ SmartPtr<const VectorSpace> y_d_space_;
+ SmartPtr<const VectorSpace> z_L_space_;
+ SmartPtr<const VectorSpace> z_U_space_;
+ SmartPtr<const VectorSpace> v_L_space_;
+ SmartPtr<const VectorSpace> v_U_space_;
+ };
+
+
+ inline
+ SmartPtr<Vector> IteratesVector::create_new_x()
+ {
+ Set_x_NonConst(*owner_space_->GetCompSpace(0)->MakeNew());
+ return x_NonConst();
+ }
+ inline
+ SmartPtr<Vector> IteratesVector::create_new_s()
+ {
+ Set_s_NonConst(*owner_space_->GetCompSpace(1)->MakeNew());
+ return s_NonConst();
+ }
+ inline
+ SmartPtr<Vector> IteratesVector::create_new_y_c()
+ {
+ Set_y_c_NonConst(*owner_space_->GetCompSpace(2)->MakeNew());
+ return y_c_NonConst();
+ }
+ inline
+ SmartPtr<Vector> IteratesVector::create_new_y_d()
+ {
+ Set_y_d_NonConst(*owner_space_->GetCompSpace(3)->MakeNew());
+ return y_d_NonConst();
+ }
+ inline
+ SmartPtr<Vector> IteratesVector::create_new_z_L()
+ {
+ Set_z_L_NonConst(*owner_space_->GetCompSpace(4)->MakeNew());
+ return z_L_NonConst();
+ }
+ inline
+ SmartPtr<Vector> IteratesVector::create_new_z_U()
+ {
+ Set_z_U_NonConst(*owner_space_->GetCompSpace(5)->MakeNew());
+ return z_U_NonConst();
+ }
+ inline
+ SmartPtr<Vector> IteratesVector::create_new_v_L()
+ {
+ Set_v_L_NonConst(*owner_space_->GetCompSpace(6)->MakeNew());
+ return v_L_NonConst();
+ }
+ inline
+ SmartPtr<Vector> IteratesVector::create_new_v_U()
+ {
+ Set_v_U_NonConst(*owner_space_->GetCompSpace(7)->MakeNew());
+ return v_U_NonConst();
+ }
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpIterationOutput.hpp b/SimTKmath/Optimizers/src/IpOpt/IpIterationOutput.hpp
new file mode 100644
index 0000000..c546dfe
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpIterationOutput.hpp
@@ -0,0 +1,63 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpIterationOutput.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter, Carl Laird IBM 2004-09-27
+
+#ifndef __IPITERATIONOUTPUT_HPP__
+#define __IPITERATIONOUTPUT_HPP__
+
+#include "IpAlgStrategy.hpp"
+#include "IpIpoptNLP.hpp"
+#include "IpIpoptData.hpp"
+#include "IpIpoptCalculatedQuantities.hpp"
+
+namespace Ipopt
+{
+
+ /** Base class for objects that do the output summary per iteration.
+ */
+ class IterationOutput: public AlgorithmStrategyObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor */
+ IterationOutput()
+ {}
+
+ /** Default destructor */
+ virtual ~IterationOutput()
+ {}
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix) = 0;
+
+ /** Method to do all the summary output per iteration. This
+ * include the one-line summary output as well as writing the
+ * details about the iterates if desired */
+ virtual void WriteOutput() = 0;
+
+ private:
+ /**@name Default Compiler Generated Methods (Hidden to avoid
+ * implicit creation/calling). These methods are not implemented
+ * and we do not want the compiler to implement them for us, so we
+ * declare them private and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ IterationOutput(const IterationOutput&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const IterationOutput&);
+ //@}
+
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpJournalist.cpp b/SimTKmath/Optimizers/src/IpOpt/IpJournalist.cpp
new file mode 100644
index 0000000..563793f
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpJournalist.cpp
@@ -0,0 +1,434 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpJournalist.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpoptConfig.h"
+#include "IpJournalist.hpp"
+#include "IpDebug.hpp"
+#include <cstring>
+
+#ifdef HAVE_CSTDIO
+# include <cstdio>
+#else
+# ifdef HAVE_STDIO_H
+# include <stdio.h>
+# else
+# error "don't have header file for stdio"
+# endif
+#endif
+
+
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, fopen etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+namespace Ipopt
+{
+
+ Journalist::Journalist()
+ {}
+
+ Journalist::~Journalist()
+ {
+ journals_.clear();
+ }
+
+ void Journalist::Printf( EJournalLevel level, EJournalCategory category,
+ const char* pformat, ... ) const
+ {
+ // wrap the arguments and pass to VPrintf
+ va_list ap;
+ va_start(ap, pformat);
+ VPrintf(level, category, pformat, ap);
+// TODO uncomment when done testing
+// vfprintf( stdout, pformat, ap );
+
+ va_end(ap);
+ }
+
+ void Journalist::PrintStringOverLines(EJournalLevel level,
+ EJournalCategory category,
+ Index indent_spaces, Index max_length,
+ const std::string& line) const
+ {
+ DBG_ASSERT(indent_spaces + max_length + 1 < 1024);
+ char buffer[1024];
+ std::string::size_type last_line_pos = 0;
+ std::string::size_type last_word_pos = 0;
+ bool first_line = true;
+ Index buffer_pos = 0;
+
+ while (last_line_pos < line.length()) {
+ std::string::size_type line_pos = last_line_pos;
+ Index curr_length = 0;
+ while (curr_length < max_length && line_pos < line.length()) {
+ buffer[buffer_pos] = line[line_pos];
+ if (line[line_pos] == ' ') {
+ last_word_pos = line_pos+1;
+ }
+ curr_length++;
+ buffer_pos++;
+ line_pos++;
+ }
+ if (line_pos == line.length()) {
+ // This is the last line to be printed.
+ buffer[buffer_pos] = '\0';
+ Printf(level, category, "%s", buffer);
+ break;
+ }
+ if (last_word_pos == last_line_pos) {
+ if (line[line_pos]==' ') {
+ buffer[buffer_pos] = '\0';
+ last_word_pos = line_pos+1;
+ last_line_pos = line_pos+1;
+ }
+ else {
+ // The current word is too long to fit into one line
+ // split word over two lines
+ buffer[buffer_pos-1] = '-';
+ buffer[buffer_pos] = '\0';
+ last_word_pos = line_pos-1;
+ last_line_pos = last_word_pos;
+ }
+ }
+ else {
+ // insert '\0' character after last complete word
+ buffer[buffer_pos-(line_pos-last_word_pos)-1] = '\0';
+ last_line_pos = last_word_pos;
+ }
+
+ Printf(level, category, "%s\n", buffer);
+ if (first_line) {
+ for(Index i=0; i<indent_spaces; i++) {
+ buffer[i] = ' ';
+ }
+ first_line = false;
+ }
+ buffer_pos = indent_spaces;
+ }
+ }
+
+ void Journalist::PrintfIndented( EJournalLevel level,
+ EJournalCategory category, Index indent_level,
+ const char* pformat, ... ) const
+ {
+ // wrap the arguments and pass to VPrintfIndented
+ va_list ap;
+ va_start(ap, pformat);
+
+ VPrintfIndented(level, category, indent_level, pformat, ap);
+// TODO uncomment when done testing
+// vfprintf( stdout, pformat, ap );
+
+ va_end(ap);
+ }
+
+ // void Journalist::PrintVector(EJournalLevel level,
+ // EJournalCategory category,
+ // const std::string& name,
+ // const Vector& vector,
+ // Index indent,
+ // const std::string prefix) const
+ // {
+ // // print the msg on every journal that accepts
+ // // the category and output level
+ // for (Index i=0; i<(Index)journals_.size(); i++) {
+ // if (journals_[i]->IsAccepted(category, level)) {
+ // // print the message
+ // journals_[i]->PrintVector(name, vector, indent, prefix);
+ // }
+ // }
+ // }
+
+ // void Journalist::PrintMatrix(EJournalLevel level,
+ // EJournalCategory category,
+ // const std::string& name,
+ // const Matrix& matrix,
+ // Index indent /*=0*/,
+ // std::string prefix /*=""*/) const
+ // {
+ // // print the msg on every journal that accepts
+ // // the category and output level
+ // for (Index i=0; i<(Index)journals_.size(); i++) {
+ // if (journals_[i]->IsAccepted(category, level)) {
+ // // print the message
+ // journals_[i]->PrintMatrix(name, matrix, indent, prefix);
+ // }
+ // }
+ // }
+
+ void Journalist::VPrintf(
+ EJournalLevel level,
+ EJournalCategory category,
+ const char* pformat, va_list ap) const
+ {
+ // print the msg on every journal that accepts
+ // the category and output level
+ for (Index i=0; i<(Index)journals_.size(); i++) {
+ if (journals_[i]->IsAccepted(category, level)) {
+ // print the message
+#ifdef va_copy
+ va_list apcopy;
+ va_copy(apcopy, ap);
+ journals_[i]->Printf(pformat, apcopy);
+ va_end(apcopy);
+#else
+
+ journals_[i]->Printf(pformat, ap);
+#endif
+
+ }
+ }
+ }
+
+ void Journalist::VPrintfIndented(
+ EJournalLevel level,
+ EJournalCategory category,
+ Index indent_level,
+ const char* pformat, va_list ap) const
+ {
+ // print the msg on every journal that accepts
+ // the category and output level
+ for (Index i=0; i<(Index)journals_.size(); i++) {
+ if (journals_[i]->IsAccepted(category, level)) {
+
+ // indent the appropriate amount
+ for (Index s=0; s<indent_level; s++) {
+ journals_[i]->Print(" ");
+ }
+
+ // print the message
+#ifdef va_copy
+ va_list apcopy;
+ va_copy(apcopy, ap);
+ journals_[i]->Printf(pformat, apcopy);
+ va_end(apcopy);
+#else
+
+ journals_[i]->Printf(pformat, ap);
+#endif
+
+ }
+ }
+ }
+
+ bool Journalist::ProduceOutput(EJournalLevel level,
+ EJournalCategory category) const
+ {
+ for (Index i=0; i<(Index)journals_.size(); i++) {
+ if (journals_[i]->IsAccepted(category, level)) {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ bool Journalist::AddJournal(const SmartPtr<Journal> jrnl)
+ {
+ DBG_ASSERT(IsValid(jrnl));
+ std::string name = jrnl->Name();
+
+ SmartPtr<Journal> temp = GetJournal(name);
+ DBG_ASSERT(IsNull(temp));
+ if (IsValid(temp)) {
+ return false;
+ }
+
+ journals_.push_back(jrnl);
+ return true;
+ }
+
+ SmartPtr<Journal> Journalist::AddFileJournal(
+ const std::string& journal_name,
+ const std::string& fname,
+ EJournalLevel default_level
+ )
+ {
+ SmartPtr<FileJournal> temp = new FileJournal(journal_name, default_level);
+
+ // Open the file (Note:, a fname of "stdout" is handled by the
+ // Journal class to mean stdout, etc.
+ if (temp->Open(fname.c_str()) && AddJournal(GetRawPtr(temp))) {
+ return GetRawPtr(temp);
+ }
+ return NULL;
+ }
+
+ void Journalist::FlushBuffer() const
+ {
+ for (Index i=0; i<(Index)journals_.size(); i++) {
+ journals_[i]->FlushBuffer();
+ }
+ }
+
+ SmartPtr<Journal> Journalist::GetJournal(
+ const std::string& journal_name
+ )
+ {
+ SmartPtr<Journal> retValue = NULL;
+
+ // try to find the journal
+ for (Index i=0; i<(Index)journals_.size(); i++) {
+ SmartPtr<Journal> tmp = journals_[i];
+ if (tmp->Name() == journal_name) {
+ retValue = tmp;
+ break;
+ }
+ }
+
+ return retValue;
+ }
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Implementation of the Journal class //
+ ///////////////////////////////////////////////////////////////////////////
+
+ Journal::Journal(
+ const std::string& name,
+ EJournalLevel default_level
+ )
+ :
+ name_(name)
+ {
+ for (Index i=0; i<J_LAST_CATEGORY; i++) {
+ print_levels_[i] = default_level;
+ }
+ }
+
+ Journal::~Journal()
+ {}
+
+ std::string Journal::Name()
+ {
+ return name_;
+ }
+
+ bool Journal::IsAccepted(
+ EJournalCategory category,
+ EJournalLevel level
+ ) const
+ {
+ if (print_levels_[(Index)category] >= (Index) level) {
+ return true;
+ }
+
+ return false;
+ }
+
+ void Journal::SetPrintLevel(
+ EJournalCategory category,
+ EJournalLevel level)
+ {
+ print_levels_[(Index)category] = (Index) level;
+ }
+
+ void Journal::SetAllPrintLevels(
+ EJournalLevel level)
+ {
+ for (Index category=(Index)J_DBG;
+ category<(Index)J_LAST_CATEGORY;
+ category++) {
+ print_levels_[category] = (Index) level;
+ }
+ }
+
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Implementation of the FileJournal class //
+ ///////////////////////////////////////////////////////////////////////////
+
+ FileJournal::FileJournal(
+ const std::string& name,
+ EJournalLevel default_level
+ )
+ :
+ Journal(name, default_level),
+ file_(NULL)
+ {}
+
+ FileJournal::~FileJournal()
+ {
+ if (file_ && file_ != stdout && file_ != stderr) {
+ // close the file
+ fclose(file_);
+ }
+ file_ = NULL;
+ }
+
+
+ bool FileJournal::Open(const char* fname)
+ {
+ if (file_ && file_ != stdout && file_ != stderr) {
+ // file already opened, close it
+ fclose(file_);
+ }
+ file_ = NULL;
+
+ if (strcmp("stdout", fname)==0) {
+ file_=stdout;
+ return true;
+ }
+ else if (strcmp("stderr", fname)==0) {
+ file_=stderr;
+ return true;
+ }
+ else {
+ // open the file on disk
+ file_ = fopen(fname, "w+");
+ if (file_) {
+ return true;
+ }
+ }
+
+ return false;
+ }
+
+
+ void FileJournal::PrintImpl(const char* str)
+ {
+ DBG_START_METH("Journal::Print", 0);
+ if (file_) {
+ fprintf(file_, str, 0); // dummy argument avoids gcc warning
+ DBG_EXEC(0, fflush(file_));
+ }
+ }
+
+ void FileJournal::PrintfImpl(const char* pformat, va_list ap)
+ {
+ DBG_START_METH("Journal::Printf", 0);
+ if (file_) {
+ vfprintf(file_, pformat, ap);
+ DBG_EXEC(0, fflush(file_));
+ }
+ }
+
+ // void FileJournal::PrintVectorImpl(const std::string name, const Vector& vector, Index indent, std::string prefix)
+ // {
+ // DBG_START_METH("Journal::PrintVector", 0);
+ // if (file_) {
+ // vector.Print(file_, name, indent, prefix);
+ // DBG_EXEC(0, fflush(file_));
+ // }
+ // }
+
+ // void FileJournal::PrintMatrixImpl(const std::string name, const Matrix& matrix, Index indent, std::string prefix)
+ // {
+ // DBG_START_METH("Journal::PrintMatrix", 0);
+ // if (file_) {
+ // matrix.Print(file_, name, indent, prefix);
+ // DBG_EXEC(0, fflush(file_));
+ // }
+ // }
+
+ void FileJournal::FlushBufferImpl()
+ {
+ if (file_) {
+ fflush(file_);
+ }
+ }
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpJournalist.hpp b/SimTKmath/Optimizers/src/IpOpt/IpJournalist.hpp
new file mode 100644
index 0000000..1712808
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpJournalist.hpp
@@ -0,0 +1,400 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpJournalist.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPJOURNALIST_HPP__
+#define __IPJOURNALIST_HPP__
+
+#include "IpoptConfig.h"
+#include "IpTypes.hpp"
+#include "IpReferenced.hpp"
+#include "IpSmartPtr.hpp"
+
+#ifdef HAVE_CSTDARG
+# include <cstdarg>
+#else
+# ifdef HAVE_STDARG_H
+# include <stdarg.h>
+# else
+# error "don't have header file for stdarg"
+# endif
+#endif
+
+#include <string>
+#include <vector>
+
+namespace Ipopt
+{
+
+ // forward declarations
+ class Journal;
+ class FileJournal;
+
+ /**@name Journalist Enumerations. */
+ //@{
+ /** Print Level Enum. */
+ enum EJournalLevel {
+ J_INSUPPRESSIBLE=-1,
+ J_NONE=0,
+ J_ERROR,
+ J_WARNING,
+ J_SUMMARY,
+ J_ITERSUMMARY,
+ J_DETAILED,
+ J_MOREDETAILED,
+ J_VECTOR,
+ J_MOREVECTOR,
+ J_MATRIX,
+ J_MOREMATRIX,
+ J_ALL,
+ J_LAST_LEVEL
+ };
+
+ /** Category Selection Enum. */
+ enum EJournalCategory {
+ J_DBG=0,
+ J_STATISTICS,
+ J_MAIN,
+ J_INITIALIZATION,
+ J_BARRIER_UPDATE,
+ J_SOLVE_PD_SYSTEM,
+ J_FRAC_TO_BOUND,
+ J_LINEAR_ALGEBRA,
+ J_LINE_SEARCH,
+ J_HESSIAN_APPROXIMATION,
+ J_SOLUTION,
+ J_DOCUMENTATION,
+ J_NLP,
+ J_TIMING_STATISTICS,
+ J_USER_APPLICATION /** This can be used by the user's application*/ ,
+ J_LAST_CATEGORY
+ };
+ //@}
+
+ /** Class responsible for all message output.
+ * This class is responsible for all messaging and output.
+ * The "printing" code or "author" should send ALL messages to the
+ * Journalist, indicating an appropriate category and print level.
+ * The journalist then decides, based on reader specified
+ * acceptance criteria, which message is actually printed in which
+ * journals.
+ * This allows the printing code to send everything, while the
+ * "reader" can decide what they really want to see.
+ *
+ * Authors:
+ * Authors use the
+ * Journals: You can add as many Journals as you like to the
+ * Journalist with the AddJournal or the AddFileJournal methods.
+ * Each one represents a different printing location (or file).
+ * Then, you can call the "print" methods of the Journalist to output
+ * information to each of the journals.
+ *
+ * Acceptance Criteria: Each print message should be flagged
+ * appropriately with an EJournalCategory and EJournalLevel.
+ *
+ * The AddFileJournal
+ * method returns a pointer to the newly created Journal object
+ * (if successful) so you can set Acceptance criteria for that
+ * particular location.
+ *
+ */
+ class Journalist : public ReferencedObject
+ {
+ public:
+ /**@name Constructor / Desructor. */
+ //@{
+ /** Constructor. */
+ Journalist();
+
+ /** Destructor... */
+ virtual ~Journalist();
+ //@}
+
+ /**@name Author Methods.
+ * These methods are used by authoring code, or code that wants
+ * to report some information.
+ */
+ //@{
+ /** Method to print a formatted string */
+ void Printf(EJournalLevel level, EJournalCategory category,
+ const char* format, ...) const;
+
+ /** Method to print a long string including indentation. The
+ * string is printed starting at the current position. If the
+ * position (counting started at the current position) exceeds
+ * max_length, a new line is inserted, and indent_spaces many
+ * spaces are printed before the string is continued. This is
+ * for example used during the printing of the option
+ * documentation. */
+ void PrintStringOverLines(EJournalLevel level, EJournalCategory category,
+ Index indent_spaces, Index max_length,
+ const std::string& line) const;
+
+ /** Method to print a formatted string with indentation */
+ void PrintfIndented(EJournalLevel level,
+ EJournalCategory category,
+ Index indent_level,
+ const char* format, ...) const;
+
+ /** Method to print a formatted string
+ * using the va_list argument. */
+ void VPrintf(EJournalLevel level,
+ EJournalCategory category,
+ const char* pformat,
+ va_list ap) const;
+
+ /** Method to print a formatted string with indentation,
+ * using the va_list argument. */
+ void VPrintfIndented(EJournalLevel level,
+ EJournalCategory category,
+ Index indent_level,
+ const char* pformat,
+ va_list ap) const;
+
+ /** Method that returns true if there is a Journal that would
+ * write output for the given JournalLevel and JournalCategory.
+ * This is useful if expensive computation would be required for
+ * a particular output. The author code can check with this
+ * method if the computations are indeed required.
+ */
+ bool ProduceOutput(EJournalLevel level,
+ EJournalCategory category) const;
+
+
+ /** Method that flushes the current buffer for all Journalists.
+ Calling this method after one optimization run helps to avoid
+ cluttering output with that produced by other parts of the
+ program (e.g. written in Fortran) */
+ void FlushBuffer() const;
+ //@}
+
+ /**@name Reader Methods.
+ * These methods are used by the reader. The reader will setup the
+ * journalist with each output file and the acceptance
+ * criteria for that file.
+ *
+ * Use these methods to setup the journals (files or other output).
+ * These are the internal objects that keep track of the print levels
+ * for each category. Then use the internal Journal objects to
+ * set specific print levels for each category (or keep defaults).
+ *
+ */
+ //@{
+ /** Add a new journal. The location_name is a string identifier,
+ * which can be used to obtain the pointer to the new Journal at
+ * a later point using the GetJournal method.
+ * The default_level is
+ * used to initialize the * printing level for all categories.
+ */
+ bool AddJournal(const SmartPtr<Journal> jrnl);
+
+ /** Add a new FileJournal. fname is the name
+ * of the * file to which this Journal corresponds. Use
+ * fname="stdout" * for stdout, and use fname="stderr" for
+ * stderr. This method * returns the Journal pointer so you can
+ * set specific acceptance criteria. It returns NULL if there
+ * was a problem creating a new Journal.
+ */
+ SmartPtr<Journal> AddFileJournal(
+ const std::string& location_name, /** identifier */
+ const std::string& fname,
+ EJournalLevel default_level = J_WARNING
+ );
+
+ /** Get an existing journal. You can use this method to change
+ * the acceptance criteria at runtime.
+ */
+ SmartPtr<Journal> GetJournal(const std::string& location_name);
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ Journalist(const Journalist&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const Journalist&);
+ //@}
+
+ //** Private Data Members. */
+ //@{
+ std::vector< SmartPtr<Journal> > journals_;
+ //@}
+ };
+
+ /** Journal class (part of the Journalist implementation.). This
+ * class is the base class for all Journals. It controls the
+ * acceptance criteria for print statements etc. Derived classes
+ * like the FileJournal - output those messages to specific locations
+ */
+ class Journal : public ReferencedObject
+ {
+ public:
+ /** Constructor. */
+ Journal(const std::string& name, EJournalLevel default_level);
+
+ /** Destructor. */
+ virtual ~Journal();
+
+ /** Get the name of the Journal */
+ std::string Name();
+
+ /** Set the print level for a particular category. */
+ void SetPrintLevel(
+ EJournalCategory category, EJournalLevel level
+ );
+
+ /** Set the print level for all category. */
+ void SetAllPrintLevels(
+ EJournalLevel level
+ );
+
+ /**@name Journal Output Methods. These methods are called by the
+ * Journalist who first checks if the output print level and category
+ * are acceptable.
+ * Calling the Print methods explicitly (instead of through the
+ * Journalist will output the message regardless of print level
+ * and category. You should use the Journalist to print & flush instead
+ */
+ //@{
+ /** Ask if a particular print level/category is accepted by the
+ * journal.
+ */
+ bool IsAccepted(
+ EJournalCategory category, EJournalLevel level
+ ) const;
+
+ /** Print to the designated output location */
+ void Print(const char* str)
+ {
+ PrintImpl(str);
+ }
+
+ /** Printf to the designated output location */
+ void Printf(const char* pformat, va_list ap)
+ {
+ PrintfImpl(pformat, ap);
+ }
+
+ /** Flush output buffer.*/
+ void FlushBuffer()
+ {
+ FlushBufferImpl();
+ }
+ //@}
+
+ protected:
+ /**@name Implementation version of Print methods. Derived classes
+ * should overload the Impl methods.
+ */
+ //@{
+ /** Print to the designated output location */
+ virtual void PrintImpl(const char* str)=0;
+
+ /** Printf to the designated output location */
+ virtual void PrintfImpl(const char* pformat, va_list ap)=0;
+
+ /** Flush output buffer.*/
+ virtual void FlushBufferImpl()=0;
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ Journal();
+
+ /** Copy Constructor */
+ Journal(const Journal&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const Journal&);
+ //@}
+
+ /** Name of the output location */
+ std::string name_;
+
+ /** vector of integers indicating the level for each category */
+ Index print_levels_[J_LAST_CATEGORY];
+ };
+
+
+ /** FileJournal class. This is a particular Journal implementation that
+ * writes to a file for output. It can write to (stdout, stderr, or disk)
+ * by using "stdout" and "stderr" as filenames.
+ */
+ class FileJournal : public Journal
+ {
+ public:
+ /** Constructor. */
+ FileJournal(const std::string& name, EJournalLevel default_level);
+
+ /** Destructor. */
+ virtual ~FileJournal();
+
+ /** Open a new file for the output location.
+ * Special Names: stdout means stdout,
+ * : stderr means stderr.
+ *
+ * Return code is false only if the file with the given name
+ * could not be opened.
+ */
+ bool Open(const char* fname);
+
+ protected:
+ /**@name Implementation version of Print methods - Overloaded from
+ * Journal base class.
+ */
+ //@{
+ /** Print to the designated output location */
+ virtual void PrintImpl(const char* str);
+
+ /** Printf to the designated output location */
+ virtual void PrintfImpl(const char* pformat, va_list ap);
+
+ /** Flush output buffer.*/
+ virtual void FlushBufferImpl();
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ FileJournal();
+
+ /** Copy Constructor */
+ FileJournal(const FileJournal&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const FileJournal&);
+ //@}
+
+ /** FILE pointer for the output destination */
+ FILE* file_;
+ };
+}
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpLapack.cpp b/SimTKmath/Optimizers/src/IpOpt/IpLapack.cpp
new file mode 100644
index 0000000..6a54a12
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpLapack.cpp
@@ -0,0 +1,139 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpLapack.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-12-25
+
+#include "IpoptConfig.h"
+#include "IpLapack.hpp"
+
+#if SimTK_DEFAULT_PRECISION==1 // float
+#define DPOTRS spotrs_
+#define DPOTRF spotrf_
+#define DSYEV ssyev_
+#else // double
+#define DPOTRS dpotrs_
+#define DPOTRF dpotrf_
+#define DSYEV dsyev_
+#endif
+
+// Prototypes for the LAPACK routines
+extern "C"
+{
+ /** LAPACK Fortran subroutine DPOTRS. */
+ void dpotrs_(char *uplo, ipfint *n,
+ ipfint *nrhs, const double *A, ipfint *ldA,
+ double *B, ipfint *ldB, ipfint *info,
+ int uplo_len);
+ /** LAPACK Fortran subroutine DPOTRF. */
+ void dpotrf_(char *uplo, ipfint *n,
+ double *A, ipfint *ldA,
+ ipfint *info, int uplo_len);
+
+ /** LAPACK Fortran subroutine DSYEV */
+ void dsyev_(char *jobz, char *uplo, ipfint *n,
+ double *A, ipfint *ldA, double *W,
+ double *WORK, ipfint *LWORK, ipfint *info,
+ int jobz_len, int uplo_len);
+
+ /** LAPACK Fortran subroutine DPOTRS. */
+ void spotrs_(char *uplo, ipfint *n,
+ ipfint *nrhs, const float *A, ipfint *ldA,
+ float *B, ipfint *ldB, ipfint *info,
+ int uplo_len);
+ /** LAPACK Fortran subroutine DPOTRF. */
+ void spotrf_(char *uplo, ipfint *n,
+ float *A, ipfint *ldA,
+ ipfint *info, int uplo_len);
+
+ /** LAPACK Fortran subroutine DSYEV */
+ void ssyev_(char *jobz, char *uplo, ipfint *n,
+ float *A, ipfint *ldA, float *W,
+ float *WORK, ipfint *LWORK, ipfint *info,
+ int jobz_len, int uplo_len);
+}
+
+namespace Ipopt
+{
+ /* Interface to FORTRAN routine DPOTRS. */
+ void IpLapackDpotrs(Index ndim, Index nrhs, const Number *a, Index lda,
+ Number *b, Index ldb)
+ {
+#ifdef COIN_HAS_LAPACK
+ ipfint N=ndim, NRHS=nrhs, LDA=lda, LDB=ldb, INFO;
+ char uplo = 'L';
+
+ DPOTRS(&uplo, &N, &NRHS, a, &LDA, b, &LDB, &INFO, 1);
+ DBG_ASSERT(INFO==0);
+#else
+
+ std::string msg = "Ipopt has been compiled without LAPACK routine DPOTRS, but options are chosen that require this dependency. Abort.";
+ THROW_EXCEPTION(LAPACK_NOT_INCLUDED, msg);
+#endif
+
+ }
+
+ void IpLapackDpotrf(Index ndim, Number *a, Index lda, Index& info)
+ {
+#ifdef COIN_HAS_LAPACK
+ ipfint N=ndim, LDA=lda, INFO;
+
+ char UPLO = 'L';
+
+ DPOTRF(&UPLO, &N, a, &LDA, &INFO, 1);
+
+ info = INFO;
+#else
+
+ std::string msg = "Ipopt has been compiled without LAPACK routine DPOTRF, but options are chosen that require this dependency. Abort.";
+ THROW_EXCEPTION(LAPACK_NOT_INCLUDED, msg);
+#endif
+
+ }
+
+ void IpLapackDsyev(bool compute_eigenvectors, Index ndim, Number *a,
+ Index lda, Number *w, Index& info)
+ {
+#ifdef COIN_HAS_LAPACK
+ ipfint N=ndim, LDA=lda, INFO;
+
+ char JOBZ;
+ if (compute_eigenvectors) {
+ JOBZ = 'V';
+ }
+ else {
+ JOBZ = 'N';
+ }
+ char UPLO = 'L';
+
+ // First we find out how large LWORK should be
+ ipfint LWORK = -1;
+ Number WORK_PROBE;
+ DSYEV(&JOBZ, &UPLO, &N, a, &LDA, w,
+ &WORK_PROBE, &LWORK, &INFO, 1, 1);
+ DBG_ASSERT(INFO==0);
+
+ LWORK = (ipfint) WORK_PROBE;
+ DBG_ASSERT(LWORK>0);
+
+ Number* WORK = new Number[LWORK];
+ for (Index i=0; i<LWORK; i++) {
+ WORK[i] = Number(i);
+ }
+ DSYEV(&JOBZ, &UPLO, &N, a, &LDA, w,
+ WORK, &LWORK, &INFO, 1, 1);
+
+ DBG_ASSERT(INFO>=0);
+ info = INFO;
+
+ delete [] WORK;
+#else
+
+ std::string msg = "Ipopt has been compiled without LAPACK routine DSYEV, but options are chosen that require this dependency. Abort.";
+ THROW_EXCEPTION(LAPACK_NOT_INCLUDED, msg);
+#endif
+
+ }
+}
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpLapack.hpp b/SimTKmath/Optimizers/src/IpOpt/IpLapack.hpp
new file mode 100644
index 0000000..d72f3fb
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpLapack.hpp
@@ -0,0 +1,39 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpLapack.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-12-25
+
+#ifndef __IPLAPACK_HPP__
+#define __IPLAPACK_HPP__
+
+#include "IpUtils.hpp"
+#include "IpException.hpp"
+
+namespace Ipopt
+{
+ DECLARE_STD_EXCEPTION(LAPACK_NOT_INCLUDED);
+
+ /** Wrapper for LAPACK subroutine DPOTRS. Solving a linear system
+ * given a Cholesky factorization. We assume that the Cholesky
+ * factor is lower traiangular. */
+ void IpLapackDpotrs(Index ndim, Index nrhs, const Number *a, Index lda,
+ Number *b, Index ldb);
+
+ /** Wrapper for LAPACK subroutine DPOTRF. Compute Cholesky
+ * factorization (lower triangular factor). info is the return
+ * value from the LAPACK routine. */
+ void IpLapackDpotrf(Index ndim, Number *a, Index lda, Index& info);
+
+ /** Wrapper for LAPACK subroutine DSYEV. Compute the Eigenvalue
+ * decomposition for a given matrix. If compute_eigenvectors is
+ * true, a will contain the eigenvectors in its columns on
+ * return. */
+ void IpLapackDsyev(bool compute_eigenvectors, Index ndim, Number *a,
+ Index lda, Number *w, Index& info);
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpLapackSolverInterface.cpp b/SimTKmath/Optimizers/src/IpOpt/IpLapackSolverInterface.cpp
new file mode 100644
index 0000000..4294ce5
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpLapackSolverInterface.cpp
@@ -0,0 +1,248 @@
+
+#include "IpLapackSolverInterface.hpp"
+#include "SimTKlapack.h"
+
+#if SimTK_DEFAULT_PRECISION==1 // float
+#define DGELSD sgelsd_
+#define DSYEV ssyev_
+#define DGETRF sgetrf_
+#define DGETRS sgetrs_
+#else // double
+#define DGELSD dgelsd_
+#define DSYEV dsyev_
+#define DGETRF dgetrf_
+#define DGETRS dgetrs_
+#endif
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+int *ipiv=0;
+double *afact;
+ LapackSolverInterface::LapackSolverInterface()
+ :
+ n(0),
+ nz(0),
+ a(NULL),
+ irn_(NULL),
+ jcn_(NULL),
+ negevals_(-1)
+ {
+ DBG_START_METH("LapackSolverInterface::LapackSolverInterface()", dbg_verbosity);
+ }
+
+
+ LapackSolverInterface::~LapackSolverInterface()
+ {
+ DBG_START_METH("LapackSolverInterface::~LapackSolverInterface()", dbg_verbosity);
+
+ delete [] a;
+ delete [] irn_;
+ delete [] jcn_;
+ }
+
+ void LapackSolverInterface::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {}
+
+ bool LapackSolverInterface::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ return true;
+ }
+
+ ESymSolverStatus LapackSolverInterface::MultiSolve(bool new_matrix, const Index* ia, const Index* ja,
+ Index nrhs, Number* rhs_vals, bool check_NegEVals,
+ Index numberOfNegEVals)
+ {
+ int i;
+ Number *atmp;
+ DBG_START_METH("LapackSolverInterface::MultiSolve", dbg_verbosity);
+ DBG_ASSERT(!check_NegEVals || ProvidesInertia());
+ //DBG_ASSERT(initialized_);
+
+
+ ESymSolverStatus retval = SYMSOLVER_SUCCESS;
+
+ // check if a factorization has to be done
+ // perform the factorization
+
+ atmp = new Number[n*n];
+ for(i=0;i<n*n;i++) atmp[i] = a[i];
+ if (new_matrix) {
+ retval = Factorization(ia, ja, check_NegEVals, numberOfNegEVals);
+ if (retval == SYMSOLVER_SUCCESS) {
+ isFactored = 1;
+ } else {
+// DBG_PRINT((1, "FACTORIZATION FAILED!\n"));
+ isFactored = 0;
+ }
+
+ }
+ if (isFactored) {
+ retval = Solve(ia, ja, nrhs, rhs_vals);
+ } else {
+ Number rcond = -1.0;
+ Number *s,*work,workSize[2];
+ int ispec = 1;
+ int info;
+ int *iwork,rank,nlvl,smlsiz,lwork,liwork,nosmlsiz;
+ const char *name = "DGELSD";
+ const char opts = ' ';
+ s = new Number[n];
+
+ smlsiz = 25;
+ nosmlsiz = n/(smlsiz+1);
+/*
+** increased size of nlvl by adding 1 due to 64bit failures
+*/
+ nlvl = (int)(log10((double)nosmlsiz)/log10(2.)) + 2;
+ if( nlvl < 0 ) nlvl = 0;
+ liwork = 3*n*nlvl + 11*n;
+ iwork = new int[liwork];
+/*
+** compute optimal size of workspace
+*/
+ DGELSD( n, n, nrhs, atmp, n, rhs_vals, n, s, rcond, rank, workSize,
+ -1, iwork, info );
+ lwork = (int)workSize[0];
+ work = new Number[lwork];
+ DGELSD( n, n, nrhs, atmp, n, rhs_vals, n, s, rcond, rank, work,
+ lwork, iwork, info );
+
+ delete [] work;
+ delete [] s;
+ delete [] iwork;
+ if( info > 0 ) {
+// printf( "dgelsd %d elements failed to converge to zero \n",info );
+ } else if( info < 0 ) {
+// printf( "dgelsd illegal arg #%d \n",info );
+ } else {
+ retval = SYMSOLVER_SUCCESS;
+ }
+ }
+ delete [] atmp;
+ return retval;
+
+ }
+
+
+ Number* LapackSolverInterface::GetValuesArrayPtr()
+ {
+ return a;
+ }
+
+ /** Initialize the local copy of the positions of the nonzero
+ elements */
+ ESymSolverStatus LapackSolverInterface::InitializeStructure(Index dim, Index nonzeros,
+ const Index* ia, const Index* ja)
+ {
+ ESymSolverStatus retval = SYMSOLVER_SUCCESS;
+ DBG_START_METH("LapackSolverInterface::InitializeStructure", dbg_verbosity);
+ n = dim;
+ nz = nonzeros;
+ delete [] a;
+ delete [] irn_;
+ delete [] jcn_;
+ a = new Number[dim*dim];
+ irn_ = new int[nz];
+ jcn_ = new int[nz];
+ for (Index i=0; i<nz; i++) {
+ irn_[i] = ia[i];
+ jcn_[i] = ja[i];
+ }
+
+ return retval;
+ }
+
+
+ ESymSolverStatus LapackSolverInterface::Factorization(const Index* ia, const Index* ja,
+ bool check_NegEVals, Index numberOfNegEVals)
+ {
+ DBG_START_METH("LapackSolverInterface::Factorization", dbg_verbosity);
+ ESymSolverStatus retval = SYMSOLVER_SUCCESS;
+ int info,lwork;
+ Number *w, *work,*atmp;
+ char jobz = 'N';
+ char uplo = 'L';
+ int i;
+
+ delete [] ipiv;
+ ipiv = new int[n];
+
+ /* compute negative eigenvalues */
+
+ negevals_ = 0;
+ w = new Number[n];
+ lwork = 3*n; // TODO get optimial value
+ work = new Number[lwork];
+ atmp = new Number[n*n];
+ for(i=0;i<n*n;i++) atmp[i] = a[i];
+ DSYEV(jobz, uplo, n, atmp, n, w, work, lwork, info, 1, 1);
+ if( info != 0 ) {
+ delete [] w;
+ delete [] work;
+ delete [] atmp;
+ return(SYMSOLVER_FATAL_ERROR);
+ }
+ for(i=0;i<n;i++){
+ if( w[i] < 0.0 ) negevals_++;
+ }
+ delete [] w;
+ delete [] work;
+ if (check_NegEVals && (numberOfNegEVals!=negevals_)) {
+ delete [] atmp;
+ return SYMSOLVER_WRONG_INERTIA;
+ }
+
+
+ DGETRF( n, n, a, n, ipiv, info);
+ delete [] atmp;
+
+ if( info > 0 ) {
+ retval = SYMSOLVER_SINGULAR;
+ }
+
+ return retval;
+ }
+
+ ESymSolverStatus LapackSolverInterface::Solve(const Index* ia, const Index* ja, Index nrhs, Number *b)
+ {
+ DBG_START_METH("LapackSolverInterface::Solve", dbg_verbosity);
+ ESymSolverStatus retval = SYMSOLVER_SUCCESS;
+ int info;
+ char transpose = 'N';
+
+ DGETRS( transpose, n, nrhs, a, n, ipiv, b, n, info, 1);
+ if( info != 0 ) {
+ if( info > 0 && info <= n ) {
+ retval = SYMSOLVER_SINGULAR;
+ } else {
+ retval = SYMSOLVER_FATAL_ERROR;
+ }
+ } else {
+ retval = SYMSOLVER_SUCCESS;
+ }
+
+
+ return retval;
+ }
+
+ Index LapackSolverInterface::NumberOfNegEVals() const
+ {
+// DBG_START_METH("LapackSolverInterface::NumberOfNegEVals", dbg_verbosity);
+ DBG_ASSERT(negevals >= 0);
+ return negevals_;
+ }
+
+ bool LapackSolverInterface::IncreaseQuality()
+ {
+ return false;
+ }
+
+}//end Ipopt namespace
+
+
+
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpLapackSolverInterface.hpp b/SimTKmath/Optimizers/src/IpOpt/IpLapackSolverInterface.hpp
new file mode 100644
index 0000000..c560a1f
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpLapackSolverInterface.hpp
@@ -0,0 +1,166 @@
+
+#ifndef __IPLAPACKSOLVERINTERFACE_HPP__
+#define __IPLAPACKSOLVERINTERFACE_HPP__
+
+#include "IpSparseSymLinearSolverInterface.hpp"
+
+namespace Ipopt
+{
+
+ /** Interface to the linear solver Lapack, derived from
+ * SparseSymLinearSolverInterface. For details, see description of
+ * SparseSymLinearSolverInterface base class.
+ */
+ class LapackSolverInterface: public SparseSymLinearSolverInterface
+ {
+ public:
+ /** @name Constructor/Destructor */
+ //@{
+ /** Constructor */
+ LapackSolverInterface();
+
+ /** Destructor */
+ virtual ~LapackSolverInterface();
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+
+ /** @name Methods for requesting solution of the linear system. */
+ //@{
+ /** Method for initializing internal stuctures. */
+ virtual ESymSolverStatus InitializeStructure(Index dim, Index nonzeros, const Index *ia, const Index *ja);
+
+ /** Method returing an internal array into which the nonzero
+ * elements are to be stored. */
+ virtual Number* GetValuesArrayPtr();
+
+ /** Solve operation for multiple right hand sides. */
+ virtual ESymSolverStatus MultiSolve(bool new_matrix,
+ const Index* ia,
+ const Index* ja,
+ Index nrhs,
+ Number* rhs_vals,
+ bool check_NegEVals,
+ Index numberOfNegEVals);
+
+ /** Number of negative eigenvalues detected during last
+ * factorization.
+ */
+ virtual Index NumberOfNegEVals() const;
+ //@}
+
+ //* @name Options of Linear solver */
+ //@{
+ /** Request to increase quality of solution for next solve.
+ */
+ virtual bool IncreaseQuality();
+
+ /** Query whether inertia is computed by linear solver.
+ * Returns true, if linear solver provides inertia.
+ */
+ virtual bool ProvidesInertia() const
+ {
+ return true;
+ }
+ /** Query of requested matrix type that the linear solver
+ * understands.
+ */
+ EMatrixFormat MatrixFormat() const
+ {
+ return Dense_Format;
+ }
+ //@}
+
+ /** Methods for IpoptType */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ LapackSolverInterface(const LapackSolverInterface&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const LapackSolverInterface&);
+ //@}
+
+ /** @name Information about the matrix */
+ //@{
+ /** @name Information about the matrix */
+ //@{
+ /** Number of rows and columns of the matrix */
+ Index n;
+
+ /** Number of nonzeros of the matrix in triplet representation. */
+ Index nz;
+
+ /** Array for storing the values of the matrix. */
+ Number* a;
+
+ /** Array for storing the values of the factored matrix. */
+ Number* afact;
+
+ /** Array for storing the row indices of the matrix */
+ int* irn_;
+ /** Array for storing the column indices of the matrix */
+ int* jcn_;
+ //@}
+ /** @name Information about most recent factorization/solve */
+ //@{
+ /** Number of negative eigenvalues */
+ Index negevals_;
+
+ bool isFactored;
+
+ //@}
+
+ /** @name Solver specific options */
+ //@{
+ //@}
+
+ /** @name Initialization flags */
+ //@{
+ //@}
+
+ /** @name Solver specific information */
+ //@{
+ /**@name Some counters for debugging */
+ //@{
+ //@}
+
+ /** @name Internal functions */
+ //@{
+ /** Call Lapack to do the analysis phase.
+ */
+ /** Call Lapack to factorize the Matrix.
+ */
+ ESymSolverStatus Factorization(const Index* ia,
+ const Index* ja,
+ bool check_NegEVals,
+ Index numberOfNegEVals);
+
+ /** Call Lapack to do the Solve.
+ */
+ ESymSolverStatus Solve(const Index* ia,
+ const Index* ja,
+ Index nrhs,
+ Number *rhs_vals);
+ //@}
+ //MUMPS data structure
+// DMUMPS_STRUC_C mumps_data;
+
+ };
+
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpLeastSquareMults.cpp b/SimTKmath/Optimizers/src/IpOpt/IpLeastSquareMults.cpp
new file mode 100644
index 0000000..19a7374
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpLeastSquareMults.cpp
@@ -0,0 +1,94 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpLeastSquareMults.cpp 765 2006-07-14 18:03:23Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-09-23
+
+#include "IpLeastSquareMults.hpp"
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ LeastSquareMultipliers::LeastSquareMultipliers(AugSystemSolver& augSysSolver)
+ :
+ EqMultiplierCalculator(),
+ augsyssolver_(&augSysSolver)
+ {}
+
+ bool LeastSquareMultipliers::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ return augsyssolver_->Initialize(Jnlst(), IpNLP(), IpData(), IpCq(),
+ options, prefix);
+ }
+
+ bool LeastSquareMultipliers::CalculateMultipliers
+ (Vector& y_c,
+ Vector& y_d)
+ {
+ DBG_START_METH("LeastSquareMultipliers::CalculateMultipliers",
+ dbg_verbosity);
+
+ SmartPtr<const SymMatrix> zeroW = IpNLP().uninitialized_h();
+ DBG_PRINT_MATRIX(2, "zeroW", *zeroW);
+ SmartPtr<const Matrix> J_c = IpCq().curr_jac_c();
+ SmartPtr<const Matrix> J_d = IpCq().curr_jac_d();
+ SmartPtr<const Vector> grad_f = IpCq().curr_grad_f();
+ SmartPtr<const Matrix> Px_L = IpNLP().Px_L();
+ SmartPtr<const Matrix> Px_U = IpNLP().Px_U();
+ SmartPtr<const Matrix> Pd_L = IpNLP().Pd_L();
+ SmartPtr<const Matrix> Pd_U = IpNLP().Pd_U();
+ SmartPtr<const Vector> z_L = IpData().curr()->z_L();
+ SmartPtr<const Vector> z_U = IpData().curr()->z_U();
+ SmartPtr<const Vector> v_L = IpData().curr()->v_L();
+ SmartPtr<const Vector> v_U = IpData().curr()->v_U();
+
+ // Compute the right hand side
+ SmartPtr<Vector> rhs_x = grad_f->MakeNew();
+ rhs_x->Copy(*grad_f);
+ DBG_PRINT_VECTOR(2, "rhs_x grad_f", *rhs_x);
+ Px_L->MultVector(1., *z_L, -1., *rhs_x);
+ Px_U->MultVector(-1., *z_U, 1., *rhs_x);
+
+ SmartPtr<Vector> rhs_s = IpData().curr()->s()->MakeNew();
+ Pd_L->MultVector(1., *v_L, 0., *rhs_s);
+ Pd_U->MultVector(-1., *v_U, 1., *rhs_s);
+
+ SmartPtr<Vector> rhs_c = y_c.MakeNew();
+ rhs_c->Set(0.);
+ SmartPtr<Vector> rhs_d = y_d.MakeNew();
+ rhs_d->Set(0.);
+
+ SmartPtr<Vector> sol_x = rhs_x->MakeNew();
+ SmartPtr<Vector> sol_s = rhs_s->MakeNew();
+
+ DBG_PRINT_VECTOR(2, "rhs_x", *rhs_x);
+ DBG_PRINT_VECTOR(2, "rhs_s", *rhs_s);
+ DBG_PRINT_VECTOR(2, "rhs_c", *rhs_c);
+ DBG_PRINT_VECTOR(2, "rhs_d", *rhs_d);
+
+ enum ESymSolverStatus retval;
+ Index numberOfEVals=rhs_c->Dim()+rhs_d->Dim();
+ retval = augsyssolver_->Solve(GetRawPtr(zeroW), 0.0, NULL, 1.0, NULL,
+ 1.0, GetRawPtr(J_c), NULL, 0.,
+ GetRawPtr(J_d), NULL, 0., *rhs_x, *rhs_s,
+ *rhs_c, *rhs_d, *sol_x, *sol_s,
+ y_c, y_d, true, numberOfEVals);
+ if (retval!=SYMSOLVER_SUCCESS) {
+ return false;
+ }
+
+ DBG_PRINT_VECTOR(2, "sol_x", *sol_x);
+ DBG_PRINT_VECTOR(2, "sol_s", *sol_s);
+ DBG_PRINT_VECTOR(2, "sol_c", y_c);
+ DBG_PRINT_VECTOR(2, "sol_d", y_d);
+
+ return true;
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpLeastSquareMults.hpp b/SimTKmath/Optimizers/src/IpOpt/IpLeastSquareMults.hpp
new file mode 100644
index 0000000..feca505
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpLeastSquareMults.hpp
@@ -0,0 +1,73 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpLeastSquareMults.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-09-23
+
+#ifndef __IPLEASTSQUAREMULTS_HPP__
+#define __IPLEASTSQUAREMULTS_HPP__
+
+#include "IpAugSystemSolver.hpp"
+#include "IpEqMultCalculator.hpp"
+
+namespace Ipopt
+{
+
+ /** Class for calculator for the least-square equality constraint
+ * multipliers. The Calculate method of this class computes the
+ * least-square estimate for the y_c and y_d multiplers, based on
+ * the current values of the gradient of the Lagrangian.
+ */
+ class LeastSquareMultipliers: public EqMultiplierCalculator
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor. It needs to be given the strategy object for
+ * solving the augmented system. */
+ LeastSquareMultipliers(AugSystemSolver& augSysSolver);
+ /** Default destructor */
+ virtual ~LeastSquareMultipliers()
+ {}
+ //@}
+
+ /* overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** This method computes the least-square estimates for y_c and
+ * y_d at the current point. The return value is false, if the
+ * least square system could not be solved (the linear system is
+ * singular). */
+ virtual bool CalculateMultipliers(Vector& y_c,
+ Vector& y_d);
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ LeastSquareMultipliers();
+
+ /** Copy Constructor */
+ LeastSquareMultipliers(const LeastSquareMultipliers&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const LeastSquareMultipliers&);
+ //@}
+
+ /** Pointer for the augmented system solver to be used for solving
+ * the linear system */
+ SmartPtr<AugSystemSolver> augsyssolver_;
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpLimMemQuasiNewtonUpdater.cpp b/SimTKmath/Optimizers/src/IpOpt/IpLimMemQuasiNewtonUpdater.cpp
new file mode 100644
index 0000000..f6c7ff7
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpLimMemQuasiNewtonUpdater.cpp
@@ -0,0 +1,1289 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpLimMemQuasiNewtonUpdater.cpp 795 2006-10-11 19:01:37Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-12-26
+
+#include "IpLimMemQuasiNewtonUpdater.hpp"
+#include "IpRestoIpoptNLP.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+#include <limits>
+
+namespace Ipopt
+{
+
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ LimMemQuasiNewtonUpdater::LimMemQuasiNewtonUpdater(
+ bool update_for_resto)
+ :
+ sigma_safe_min_(1e-8),
+ sigma_safe_max_(1e+8),
+ update_for_resto_(update_for_resto)
+ {}
+
+ void LimMemQuasiNewtonUpdater::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->AddLowerBoundedIntegerOption(
+ "limited_memory_max_history",
+ "Maximum size of the history for the limited quasi-Newton Hessian approximation.",
+ 0, 6,
+ "This option determines the number of most recent iterations that are "
+ "taken into account for the limited-memory quasi-Newton approximation.");
+
+ roptions->AddStringOption2(
+ "limited_memory_update_type",
+ "Quasi-Newton update formula for the limited memory approximation.",
+ "bfgs",
+ "bfgs", "BFGS update (with skipping)",
+ "sr1", "SR1 (not working well)",
+ "Determines which update formula is to be used for the limited-memory "
+ "quasi-Newton approximation.");
+
+ roptions->AddStringOption3(
+ "limited_memory_initialization",
+ "Initialization strategy for the limited memory quasi-Newton approximation.",
+ "scalar1",
+ "scalar1", "sigma = s^Ty/s^Ts",
+ "scalar2", "sigma = y^Ty/s^Ty",
+ "constant", "sigma = limited_memory_init_val",
+ "Determines how the diagonal Matrix B_0 as the first term in the "
+ "limited memory approximation should be computed.");
+
+ roptions->AddLowerBoundedNumberOption(
+ "limited_memory_init_val",
+ "Value for B0 in low-rank update.",
+ 0, true, 1.,
+ "The starting matrix in the low rank update, B0, is chosen to be this "
+ "multiple of the identity in the first iteration (when no updates have "
+ "been performed yet), and is constantly chosen as this value, if "
+ "\"limited_memory_initialization\" is \"constant\".");
+
+ roptions->AddLowerBoundedIntegerOption(
+ "limited_memory_max_skipping",
+ "Threshold for successive iterations where update is skipped.",
+ 1, 2,
+ "If the update is skipped more than this number of successive "
+ "iterations, we quasi-Newton approximation is reset.");
+ }
+
+ bool LimMemQuasiNewtonUpdater::InitializeImpl(
+ const OptionsList& options,
+ const std::string& prefix)
+ {
+ options.GetIntegerValue("limited_memory_max_history",
+ limited_memory_max_history_, prefix);
+ Index enum_int;
+ options.GetEnumValue("limited_memory_update_type", enum_int, prefix);
+ limited_memory_update_type_ = LMUpdateType(enum_int);
+ options.GetEnumValue("limited_memory_initialization", enum_int, prefix);
+ limited_memory_initialization_ = LMInitialization(enum_int);
+ options.GetNumericValue("limited_memory_init_val",
+ limited_memory_init_val_, prefix);
+ options.GetIntegerValue("limited_memory_max_skipping",
+ limited_memory_max_skipping_, prefix);
+
+ h_space_ = NULL;
+ curr_lm_memory_ = 0;
+ S_ = NULL;
+ Y_ = NULL;
+ Ypart_ = NULL;
+ D_ = NULL;
+ L_ = NULL;
+ sigma_ = -1;
+ V_ = NULL;
+ U_ = NULL;
+ SdotS_ = NULL;
+ SdotS_uptodate_ = false;
+ STDRS_ = NULL;
+ DRS_ = NULL;
+ curr_DR_x_tag_ = 0;
+
+ last_x_ = NULL;
+ last_grad_f_ = NULL;
+ last_jac_c_ = NULL;
+ last_jac_d_ = NULL;
+ lm_skipped_iter_ = 0;
+
+ last_eta_ = -1.;
+
+ return true;
+ }
+
+ void LimMemQuasiNewtonUpdater::UpdateHessian()
+ {
+ DBG_START_METH("LimMemQuasiNewtonUpdater::UpdateHessian",
+ dbg_verbosity);
+
+ // First check if this is the first call - it is if the h_space_
+ // has not been set yet.
+ if (IsNull(h_space_)) {
+ if (update_for_resto_) {
+ SmartPtr<const SymMatrixSpace> sp = IpNLP().HessianMatrixSpace();
+ const CompoundSymMatrixSpace* csp =
+ dynamic_cast<const CompoundSymMatrixSpace*> (GetRawPtr(sp));
+ DBG_ASSERT(csp);
+ h_space_ = dynamic_cast<const LowRankUpdateSymMatrixSpace*>
+ (GetRawPtr(csp->GetCompSpace(0,0)));
+ }
+ else {
+ // ToDo don't need that?!? Can always get the space from the NLP?
+ SmartPtr<const SymMatrixSpace> sp = IpNLP().HessianMatrixSpace();
+ h_space_ = dynamic_cast<const LowRankUpdateSymMatrixSpace*>(GetRawPtr(sp));
+ ASSERT_EXCEPTION(IsValid(h_space_), OPTION_INVALID,
+ "Limited-memory quasi-Newton option chosen, but NLP doesn't provide LowRankUpdateSymMatrixSpace.");
+ }
+ DBG_ASSERT((h_space_->ReducedDiag() && !update_for_resto_) ||
+ (!h_space_->ReducedDiag() && update_for_resto_));
+ }
+
+ SmartPtr<const Matrix> P_LM = h_space_->P_LowRank();
+ SmartPtr<const VectorSpace> LM_vecspace =
+ h_space_->LowRankVectorSpace();
+ DBG_ASSERT(IsValid(LM_vecspace));
+
+ // If we are in the restoration phase, get some additional data
+ // for the structured update
+ if (update_for_resto_) {
+ DBG_ASSERT(IpNLP().objective_depends_on_mu());
+ RestoIpoptNLP* resto_nlp = dynamic_cast<RestoIpoptNLP*>
+ (&IpNLP());
+ DBG_ASSERT(resto_nlp);
+ curr_DR_x_ = resto_nlp->DR_x();
+ DBG_ASSERT(IsValid(curr_DR_x_));
+ DBG_ASSERT(curr_DR_x_tag_==0 || curr_DR_x_tag_==curr_DR_x_->GetTag());
+ curr_DR_x_tag_ = curr_DR_x_->GetTag();
+ if (IsNull(P_LM)) {
+ curr_red_DR_x_ = curr_DR_x_;
+ }
+ else {
+ SmartPtr<Vector> tmp = LM_vecspace->MakeNew();
+ P_LM->TransMultVector(1, *curr_DR_x_, 0., *tmp);
+ curr_red_DR_x_ = ConstPtr(tmp);
+ }
+ curr_eta_ = resto_nlp->Eta(IpData().curr_mu());
+ eta_changed_ = (curr_eta_!=last_eta_);
+ Jnlst().Printf(J_DETAILED, J_HESSIAN_APPROXIMATION,
+ "curr_eta (for B0) is %e\n", curr_eta_);
+ }
+
+ // Get the current values
+ SmartPtr<const Vector> curr_x;
+ SmartPtr<const Vector> curr_grad_f;
+ SmartPtr<const Matrix> curr_jac_c;
+ SmartPtr<const Matrix> curr_jac_d;
+ if (update_for_resto_) {
+ const CompoundVector* cv =
+ dynamic_cast<const CompoundVector*> (GetRawPtr(IpData().curr()->x()));
+ DBG_ASSERT(cv);
+ //DBG_PRINT_VECTOR(2, "cv", *cv);
+ curr_x = cv->GetComp(0);
+ const CompoundMatrix* cm =
+ dynamic_cast<const CompoundMatrix*> (GetRawPtr(IpCq().curr_jac_c()));
+ curr_jac_c = cm->GetComp(0,0);
+ cm = dynamic_cast<const CompoundMatrix*> (GetRawPtr(IpCq().curr_jac_d()));
+ curr_jac_d = cm->GetComp(0,0);
+ }
+ else {
+ curr_x = IpData().curr()->x();
+ curr_grad_f = IpCq().curr_grad_f();
+ curr_jac_c = IpCq().curr_jac_c();
+ curr_jac_d = IpCq().curr_jac_d();
+ }
+ //DBG_PRINT_VECTOR(2, "curr_x", *curr_x);
+
+ // If this is the first iteration, we just gather information and
+ // set W to be the identity matrix.
+ if (IsNull(last_x_) ||
+ lm_skipped_iter_ >= limited_memory_max_skipping_) {
+ if (IsNull(last_x_)) {
+ DBG_ASSERT(IsNull(last_grad_f_));
+ DBG_ASSERT(IsNull(last_jac_c_));
+ DBG_ASSERT(IsNull(last_jac_d_));
+ Jnlst().Printf(J_DETAILED, J_HESSIAN_APPROXIMATION,
+ "Limited-Memory approximation started; store data at current iterate.\n");
+ }
+ else {
+ Jnlst().Printf(J_DETAILED, J_HESSIAN_APPROXIMATION,
+ "Resetting Limited-Memory Update.\n");
+ IpData().Append_info_string("Wr");
+ }
+
+ last_x_ = curr_x;
+ last_grad_f_ = curr_grad_f;
+ last_jac_c_ = curr_jac_c;
+ last_jac_d_ = curr_jac_d;
+
+ curr_lm_memory_ = 0;
+ lm_skipped_iter_ = 0;
+ S_ = NULL;
+ Y_ = NULL;
+ Ypart_ = NULL;
+ D_ = NULL;
+ L_ = NULL;
+ V_ = NULL;
+ U_ = NULL;
+ SdotS_ = NULL;
+ SdotS_uptodate_ = false;
+ STDRS_ = NULL;
+ DRS_ = NULL;
+
+ if (update_for_resto_) {
+ sigma_ = -1;
+ last_eta_ = -1.;
+ }
+ else {
+ // Set up W to be multiple of I
+ sigma_ = limited_memory_init_val_; // not for resto
+ }
+ SetW();
+ return;
+ }
+
+ // Compute s and y for the current iteration
+
+ // s = x_k - x_{k-1}
+ SmartPtr<Vector> s_full_new = curr_x->MakeNew();
+ DBG_PRINT_VECTOR(2, "last_x", *last_x_);
+ DBG_PRINT_VECTOR(2, "curr_x", *curr_x);
+ s_full_new->AddTwoVectors(1, *curr_x, -1, *last_x_, 0.);
+
+ // y = grad_lag(x_k,y_k) - grad_lag(x_{k-1},y_k)
+ SmartPtr<Vector> y_full_new = curr_x->MakeNew();
+ if (update_for_resto_) {
+ curr_jac_c->TransMultVector(1., *IpData().curr()->y_c(),
+ 0., *y_full_new);
+ curr_jac_d->TransMultVector(1., *IpData().curr()->y_d(),
+ 1., *y_full_new);
+ }
+ else {
+ y_full_new->AddTwoVectors(1., *IpCq().curr_grad_f(),
+ -1, *last_grad_f_, 0.);
+ y_full_new->AddTwoVectors(1., *IpCq().curr_jac_cT_times_curr_y_c(),
+ 1., *IpCq().curr_jac_dT_times_curr_y_d(), 1.);
+ }
+ last_jac_c_->TransMultVector(-1., *IpData().curr()->y_c(),
+ 1., *y_full_new);
+ last_jac_d_->TransMultVector(-1., *IpData().curr()->y_d(),
+ 1., *y_full_new);
+
+ SmartPtr<Vector> s_new;
+ SmartPtr<Vector> y_new;
+
+ if (IsNull(P_LM)) {
+ // Then the approximation is in the same space as x
+ s_new = s_full_new;
+ y_new = y_full_new;
+ }
+ else {
+ s_new = LM_vecspace->MakeNew();
+ y_new = LM_vecspace->MakeNew();
+ P_LM->TransMultVector(1., *s_full_new, 0., *s_new);
+ P_LM->TransMultVector(1., *y_full_new, 0., *y_new);
+ }
+ s_full_new = NULL;
+ y_full_new = NULL;
+ DBG_PRINT_VECTOR(2, "s_new", *s_new);
+ DBG_PRINT_VECTOR(2, "ypart_new", *y_new);
+
+ // In the restoration phase case, y_new is only the y without the
+ // objective part, so now we add the explicitly known objective
+ // part
+ SmartPtr<Vector> ypart_new;
+ if (update_for_resto_) {
+ ypart_new = y_new;
+ y_new = ypart_new->MakeNew();
+ y_new->AddOneVector(curr_eta_, *s_new, 0.);
+ y_new->ElementWiseMultiply(*curr_red_DR_x_);
+ y_new->AddOneVector(1., *ypart_new, 1.);
+ }
+
+ bool skipping = false;
+ bool retroactive_skip = false;
+
+ // Sometimes the change in the primal variables is very small, and
+ // we should then skip the update...
+ // ToDo: Find good number or do relative test?
+ Number s_new_max = s_new->Amax();
+ Jnlst().Printf(J_DETAILED, J_HESSIAN_APPROXIMATION,
+ "In limited-memory update, s_new_max is %e\n", s_new_max);
+ if (s_new_max<100.*std::numeric_limits<Number>::epsilon()) {
+ skipping = true;
+ IpData().Append_info_string("WS");
+ }
+
+ if (!skipping) {
+ switch (limited_memory_update_type_) {
+ case BFGS: {
+ skipping = CheckSkippingBFGS(*s_new, *y_new);
+ DBG_PRINT_VECTOR(2, "y_new", *y_new);
+ if (skipping) {
+ break;
+ }
+
+ if (update_for_resto_) {
+ // In the restoration phase we don't know if the update has
+ // to be skipped yet, since a change in curr_eta_ might
+ // cause some of the s^Ty pairs to become negative. For
+ // now, we then just skip the update and use what we had
+ // before.
+ //
+ // ToDo: It would probably be better just to reset the
+ // entire update.
+ StoreInternalDataBackup();
+ }
+
+ bool augment_memory = UpdateInternalData(*s_new, *y_new, ypart_new);
+
+ if (update_for_resto_) {
+ Number dmin = D_->Min();
+ if (dmin <= 0.) {
+ Jnlst().Printf(J_DETAILED, J_HESSIAN_APPROXIMATION,
+ "Skipping BFGS update in restoration phase because Dmin is %e\n", dmin);
+ // Check if any of the s^Ty pairs are non-positive. If so, skip
+ IpData().Append_info_string("We");
+ skipping = true;
+ RestoreInternalDataBackup();
+ break;
+ }
+ }
+
+ Number sTy_new = s_new->Dot(*y_new);
+ if (!update_for_resto_) {
+ // Compute the initial matrix B_0
+ switch (limited_memory_initialization_) {
+ case SCALAR1:
+ sigma_ = sTy_new/pow(s_new->Nrm2(),2);
+ break;
+ case SCALAR2:
+ sigma_ = pow(y_new->Nrm2(),2)/sTy_new;
+ break;
+ case CONSTANT:
+ sigma_ = limited_memory_init_val_;
+ break;
+ }
+ Jnlst().Printf(J_DETAILED, J_HESSIAN_APPROXIMATION,
+ "sigma (for B0) is %e\n", sigma_);
+ if (sigma_ < sigma_safe_min_ ||
+ sigma_ > sigma_safe_max_) {
+ sigma_ = Max(Min(sigma_safe_max_, sigma_), sigma_safe_min_);
+ IpData().Append_info_string("Wp");
+ Jnlst().Printf(J_DETAILED, J_HESSIAN_APPROXIMATION,
+ "Projecting sigma into safeguards to be %e!\n", sigma_);
+ }
+ }
+
+ if (limited_memory_max_history_ == 0 ) {
+ break;
+ }
+
+ // First update V - here only the last column is updated
+ DBG_ASSERT(sTy_new > 0.);
+ SmartPtr<Vector> v_new = y_new->MakeNewCopy();
+ v_new->Scal(1./sqrt(sTy_new));
+ if (augment_memory) {
+ AugmentMultiVector(V_, *v_new);
+ }
+ else {
+ ShiftMultiVector(V_, *v_new);
+ }
+
+ // Compute Ltilde = L * diag(D^{-1/2});
+ SmartPtr<DenseVector> Dtilde = D_->MakeNewDenseVector();
+ Dtilde->Copy(*D_);
+ Dtilde->ElementWiseSqrt();
+ Dtilde->ElementWiseReciprocal();
+ SmartPtr<DenseGenMatrix> Ltilde = L_->MakeNewDenseGenMatrix();
+ DBG_PRINT_MATRIX(3, "D", *D_);
+ DBG_PRINT_MATRIX(3, "L", *L_);
+ Ltilde->Copy(*L_);
+ Ltilde->ScaleColumns(*Dtilde);
+ DBG_PRINT_MATRIX(3, "Ltilde", *Ltilde);
+
+ // M = Ltilde * Ltilde^T
+ SmartPtr<DenseSymMatrixSpace> Mspace =
+ new DenseSymMatrixSpace(curr_lm_memory_);
+ SmartPtr<DenseSymMatrix> M = Mspace->MakeNewDenseSymMatrix();
+ M->HighRankUpdate(false, 1., *Ltilde, 0.);
+
+ // M += S^T B_0 S
+ if (!update_for_resto_) {
+ // For now, we assume that B_0 is sigma*I
+ DBG_ASSERT(SdotS_uptodate_);
+ DBG_PRINT_MATRIX(3, "SdotS", *SdotS_);
+ M->AddMatrix(sigma_, *SdotS_, 1.);
+ }
+ else {
+ DBG_PRINT_MATRIX(3, "STDRS", *STDRS_);
+ M->AddMatrix(curr_eta_, *STDRS_, 1.);
+ }
+
+ // Compute Cholesky factor J with M = J J^T
+ DBG_PRINT_MATRIX(3, "M", *M);
+ SmartPtr<DenseGenMatrix> J = L_->MakeNewDenseGenMatrix();
+ bool cholesky_retval = J->ComputeCholeskyFactor(*M);
+ DBG_PRINT_MATRIX(3, "J", *J);
+ if (!cholesky_retval) {
+ Jnlst().Printf(J_WARNING, J_HESSIAN_APPROXIMATION,
+ "Cholesky factorization failed for LBFGS update! Skipping update.\n");
+ skipping = true;
+ break;
+ }
+
+ // Compute C = J^{-T}
+ SmartPtr<DenseGenMatrix> C = J->MakeNewDenseGenMatrix();
+ C->FillIdentity();
+ J->CholeskyBackSolveMatrix(true, 1., *C);
+
+ // Compute U = B_0 * S * C
+ U_ = S_->MakeNewMultiVectorMatrix();
+ if (!update_for_resto_) {
+ DBG_ASSERT(sigma_>0.);
+ U_->AddRightMultMatrix(sigma_, *S_, *C, 0.);
+ }
+ else {
+ DBG_ASSERT(sigma_<0.);
+ U_->AddRightMultMatrix(curr_eta_, *DRS_, *C, 0.);
+ }
+
+ // Compute Lbar = Ltilde^T * C
+ SmartPtr<DenseGenMatrix> Lbar = Ltilde->MakeNewDenseGenMatrix();
+ Lbar->AddMatrixProduct(1., *Ltilde, true, *C, false, 0.);
+
+ // Compute U += V * Lbar;
+ U_->AddRightMultMatrix(1., *V_, *Lbar, 1.);
+ break;
+ }
+ case SR1:
+ // TODO IMPLEMENT WELL!
+ if (IpData().info_regu_x()>0.) {
+ RestoreInternalDataBackup();
+ Jnlst().Printf(J_DETAILED, J_HESSIAN_APPROXIMATION,
+ "Undoing most recent SR1 update.\n");
+ IpData().Append_info_string("Wb");
+ retroactive_skip = true;
+ }
+
+ // We don't know if the update has to be skipped, so we store a
+ // backup of all internal data that can be restored in case we
+ // have to skip the update.
+ StoreInternalDataBackup();
+
+ // Update the internal stuff
+ UpdateInternalData(*s_new, *y_new, ypart_new);
+
+ if (!update_for_resto_) {
+ // Set B0 for now as we do for BFGS - except that we take the
+ // abs value?
+ //
+ // It seems that it is not a good idea to use that update if
+ // this is the first contribution to the limited-memory history,
+ // since otherwise the update will be skipped (and then all
+ // updates will be skipped)
+ if (curr_lm_memory_==1) {
+ sigma_ = limited_memory_init_val_;
+ }
+ else {
+ // ToDo: What lower bound to use?
+ Number sTy_new = Max(1e-8, fabs(s_new->Dot(*y_new)));
+ DBG_ASSERT(sTy_new!=0.);
+ switch (limited_memory_initialization_) {
+ case SCALAR1:
+ sigma_ = sTy_new/pow(s_new->Nrm2(),2);
+ break;
+ case SCALAR2:
+ sigma_ = pow(y_new->Nrm2(),2)/sTy_new;
+ break;
+ case CONSTANT:
+ sigma_ = limited_memory_init_val_;
+ break;
+ }
+ }
+ Jnlst().Printf(J_DETAILED, J_HESSIAN_APPROXIMATION,
+ "sigma (for B0) is %e\n", sigma_);
+ // ToDo Decide what to use for SR1 - or at least use different
+ // skipping rule
+ }
+
+ if (limited_memory_max_history_ == 0 ) {
+ break;
+ }
+
+ DBG_PRINT_VECTOR(2,"S",*S_);
+ DBG_PRINT_VECTOR(2,"Y",*Y_);
+
+ // Compute Z as D + L + L^T - S^TB_0S
+ SmartPtr<DenseSymMatrix> Z;
+ if (!update_for_resto_) {
+ Z = SdotS_->MakeNewDenseSymMatrix();
+ DBG_PRINT_MATRIX(3, "SdotS", *SdotS_);
+ DBG_PRINT((1, "sigma_ = %e\n", sigma_));
+ Z->AddMatrix(-sigma_, *SdotS_, 0.);
+ }
+ else {
+ Z = STDRS_->MakeNewDenseSymMatrix();
+ Z->AddMatrix(-curr_eta_, *STDRS_, 0.);
+ }
+
+ DBG_PRINT_MATRIX(3, "L", *L_);
+ DBG_PRINT_VECTOR(3, "D", *D_);
+ Z->SpecialAddForLMSR1(*D_, *L_);
+ // Compute the eigenvectors Q and eignevalues E for Z
+ SmartPtr<DenseGenMatrix> Q = L_->MakeNewDenseGenMatrix();
+ SmartPtr<DenseVector> E = D_->MakeNewDenseVector();
+ DBG_PRINT_MATRIX(3, "Z", *Z);
+ bool retval = Q->ComputeEigenVectors(*Z, *E);
+ ASSERT_EXCEPTION(retval, INTERNAL_ABORT,
+ "Eigenvalue decomposition failed for limited-memory SR1 update.");
+ DBG_PRINT_VECTOR(2, "E", *E);
+ DBG_PRINT_MATRIX(3, "Q", *Q);
+ SmartPtr<DenseGenMatrix> Qminus;
+ SmartPtr<DenseGenMatrix> Qplus;
+ // Split the eigenvectors and scale them
+ skipping = SplitEigenvalues(*Q, *E, Qminus, Qplus);
+ if (skipping) {
+ RestoreInternalDataBackup();
+ break;
+ }
+
+ // Compute Vtilde = Y - B_0*S
+ SmartPtr<MultiVectorMatrix> Vtilde = Y_->MakeNewMultiVectorMatrix();
+ Vtilde->AddOneMultiVectorMatrix(1., *Y_, 0.);
+ if (!update_for_resto_) {
+ Vtilde->AddOneMultiVectorMatrix(-sigma_, *S_, 1.);
+ }
+ else {
+ DBG_ASSERT(sigma_<0.);
+ Vtilde->AddOneMultiVectorMatrix(-curr_eta_, *DRS_, 1.);
+ }
+
+ // Now get U as the Vtilde * Qminus
+ if (IsValid(Qminus)) {
+ SmartPtr<MultiVectorMatrixSpace> U_space =
+ new MultiVectorMatrixSpace(Qminus->NCols(), *s_new->OwnerSpace());
+ U_ = U_space->MakeNewMultiVectorMatrix();
+ U_->AddRightMultMatrix(1., *Vtilde, *Qminus, 0.);
+ DBG_PRINT_MATRIX(3, "U", *U_);
+ }
+ else {
+ U_ = NULL;
+ }
+
+ // Now get V as the Vtilde * Qplus
+ if (IsValid(Qplus)) {
+ SmartPtr<MultiVectorMatrixSpace> V_space =
+ new MultiVectorMatrixSpace(Qplus->NCols(), *s_new->OwnerSpace());
+ V_ = V_space->MakeNewMultiVectorMatrix();
+ V_->AddRightMultMatrix(1., *Vtilde, *Qplus, 0.);
+ DBG_PRINT_MATRIX(3, "V", *V_);
+ }
+ else {
+ V_ = NULL;
+ }
+ break;
+ }
+ }
+
+ if (!skipping) {
+ // Put together W
+ SetW();
+ if (retroactive_skip) {
+ lm_skipped_iter_++;
+ }
+ else {
+ lm_skipped_iter_ = 0;
+ }
+ }
+ else { // if (!skipping) {
+ IpData().Append_info_string("Ws");
+ lm_skipped_iter_++;
+ }
+ Jnlst().Printf(J_DETAILED, J_HESSIAN_APPROXIMATION,
+ "Number of successive iterations with skipping: %d\n",
+ lm_skipped_iter_);
+
+ // Keep stuff around in case we want to skip SR1 retroactively
+ // because of negative curvature!
+ //
+ //ReleaseInternalDataBackup();
+
+ last_x_ = curr_x;
+ last_grad_f_ = curr_grad_f;
+ last_jac_c_ = curr_jac_c;
+ last_jac_d_ = curr_jac_d;
+
+ if (update_for_resto_) {
+ last_eta_ = curr_eta_;
+ curr_DR_x_ = NULL;
+ }
+ }
+
+ void LimMemQuasiNewtonUpdater::
+ StoreInternalDataBackup()
+ {
+ DBG_START_METH("LimMemQuasiNewtonUpdater::StoreInternalDataBackup",
+ dbg_verbosity);
+ curr_lm_memory_old_ = curr_lm_memory_;
+ S_old_ = S_;
+ Y_old_ = Y_;
+ Ypart_old_ = Ypart_;
+ D_old_ = D_;
+ L_old_ = L_;
+ SdotS_old_ = SdotS_;
+ SdotS_uptodate_old_ = SdotS_uptodate_;
+ STDRS_old_ = STDRS_;
+ DRS_old_ = DRS_;
+ sigma_old_ = sigma_;
+ V_old_ = V_;
+ U_old_ = U_;
+ }
+
+ void LimMemQuasiNewtonUpdater::
+ RestoreInternalDataBackup()
+ {
+ DBG_START_METH("LimMemQuasiNewtonUpdater::RestoreInternalDataBackup",
+ dbg_verbosity);
+ curr_lm_memory_ = curr_lm_memory_old_;
+ S_ = S_old_;
+ Y_ = Y_old_;
+ Ypart_ = Ypart_old_;
+ D_ = D_old_;
+ L_ = L_old_;
+ SdotS_ = SdotS_old_;
+ SdotS_uptodate_ = SdotS_uptodate_old_;
+ STDRS_ = STDRS_old_;
+ DRS_ = DRS_old_;
+ sigma_ = sigma_old_;
+ V_ = V_old_;
+ U_ = U_old_;
+ }
+
+ void LimMemQuasiNewtonUpdater::
+ ReleaseInternalDataBackup()
+ {
+ DBG_START_METH("LimMemQuasiNewtonUpdater::ReleaseInternalDataBackup",
+ dbg_verbosity);
+ S_old_ = NULL;
+ Y_old_ = NULL;
+ Ypart_old_ = NULL;
+ D_old_ = NULL;
+ L_old_ = NULL;
+ SdotS_old_ = NULL;
+ SdotS_uptodate_old_ = false;
+ STDRS_old_ = NULL;
+ DRS_old_ = NULL;
+ V_old_ = NULL;
+ U_old_ = NULL;
+ }
+
+ bool LimMemQuasiNewtonUpdater::
+ UpdateInternalData(const Vector& s_new, const Vector& y_new,
+ SmartPtr<Vector> ypart_new)
+ {
+ DBG_START_METH("LimMemQuasiNewtonUpdater::UpdateInternalData",
+ dbg_verbosity);
+
+ if (limited_memory_max_history_==0) {
+ return false;
+ }
+
+ bool augment_memory;
+ if (curr_lm_memory_ < limited_memory_max_history_) {
+ augment_memory = true;
+ curr_lm_memory_++;
+ }
+ else {
+ augment_memory = false;
+ }
+
+ if (!update_for_resto_) {
+ // Update the internal information
+ if (augment_memory) {
+ // If the memory is still
+ // growing, increase the vector spaces etc
+ AugmentMultiVector(S_, s_new);
+ AugmentMultiVector(Y_, y_new);
+ AugmentDenseVector(D_, s_new.Dot(y_new));
+ AugmentLMatrix(L_, *S_, *Y_);
+ DBG_ASSERT(SdotS_uptodate_ || S_->NCols()==1);
+ AugmentSdotSMatrix(SdotS_, *S_);
+ SdotS_uptodate_ = true;
+ }
+ else {
+ // Otherwise, we shift the internal data
+ ShiftMultiVector(S_, s_new);
+ ShiftMultiVector(Y_, y_new);
+ ShiftDenseVector(D_, s_new.Dot(y_new));
+ ShiftLMatrix(L_, *S_, *Y_);
+ DBG_ASSERT(SdotS_uptodate_);
+ ShiftSdotSMatrix(SdotS_, *S_);
+ }
+ }
+ else {
+ // Compute DR*s_new;
+ SmartPtr<Vector> DRs_new = s_new.MakeNewCopy();
+ DRs_new->ElementWiseMultiply(*curr_red_DR_x_);
+ if (augment_memory) {
+ AugmentMultiVector(S_, s_new);
+ AugmentMultiVector(DRS_, *DRs_new);
+ AugmentMultiVector(Ypart_, *ypart_new);
+ AugmentSTDRSMatrix(STDRS_, *S_, *DRS_);
+ }
+ else {
+ ShiftMultiVector(S_, s_new);
+ ShiftMultiVector(DRS_, *DRs_new);
+ ShiftMultiVector(Ypart_, *ypart_new);
+ ShiftSTDRSMatrix(STDRS_, *S_, *DRS_);
+ }
+ DBG_PRINT((1,"curr_eta = %e\n", curr_eta_));
+ DBG_PRINT_VECTOR(2,"curr_red_DR_x", *curr_red_DR_x_);
+ DBG_PRINT_VECTOR(2,"S", *S_);
+ DBG_PRINT_VECTOR(2,"Ypart", *Ypart_);
+ RecalcY(curr_eta_, *curr_red_DR_x_, *S_, *Ypart_, Y_);
+ DBG_PRINT_VECTOR(2,"Y", *Y_);
+ RecalcD(*S_, *Y_, D_);
+ RecalcL(*S_, *Y_, L_);
+ }
+
+ return augment_memory;
+ }
+
+ bool LimMemQuasiNewtonUpdater::
+ SplitEigenvalues(DenseGenMatrix& Q, const DenseVector& E,
+ SmartPtr<DenseGenMatrix>& Qminus,
+ SmartPtr<DenseGenMatrix>& Qplus)
+ {
+ DBG_START_METH("LimMemQuasiNewtonUpdater::SplitEigenvalues",
+ dbg_verbosity);
+
+ Index dim = E.Dim();
+ DBG_ASSERT(dim==Q.NCols());
+ DBG_ASSERT(dim==Q.NRows());
+
+ const Number* Evals = E.Values();
+ const Number* Qvals = Q.Values();
+
+ // Determine number of negative eigenvalues
+ Index nneg=0;
+ for (Index i=0; i<dim; i++) {
+ if (Evals[i]<0.) {
+ nneg++;
+ }
+ }
+
+ // Determine the ratio of smallest over the largest eigenvalue
+ Number emax = Max(fabs(Evals[0]), fabs(Evals[dim-1]));
+ if (emax==0.) {
+ return true;
+ }
+ Number emin;
+ if (nneg==0) {
+ emin = Evals[0];
+ }
+ else if (nneg==dim) {
+ emin = -Evals[dim-1];
+ }
+ else {
+ emin = Min(-Evals[nneg-1],Evals[nneg]);
+ }
+ Number ratio = emin/emax;
+ Jnlst().Printf(J_DETAILED, J_HESSIAN_APPROXIMATION,
+ "Eigenvalues in SR1 update: emin=%e emax=%e ratio=%e\n",
+ emin, emax, ratio);
+ DBG_ASSERT(ratio>=0.);
+ // ToDo make the following an option?
+ const Number tol = 1e-12;
+ if (ratio<tol) {
+ return true;
+ }
+
+ // Consider the special cases where there are only positive or
+ // only negative eigenvalues
+ if (nneg==0) {
+ SmartPtr<DenseVector> tmp = E.MakeNewDenseVector();
+ tmp->Copy(E);
+ tmp->ElementWiseSqrt();
+ tmp->ElementWiseReciprocal();
+ Q.ScaleColumns(*tmp);
+ Qplus = &Q;
+ Qminus = NULL;
+ return false;
+ }
+ else if (nneg==E.Dim()) {
+ SmartPtr<DenseVector> tmp = E.MakeNewDenseVector();
+ tmp->AddOneVector(-1., E, 0.);
+ tmp->ElementWiseSqrt();
+ tmp->ElementWiseReciprocal();
+ Q.ScaleColumns(*tmp);
+ Qminus = &Q;
+ Qplus = NULL;
+ return false;
+ }
+
+ // Create Qminus
+ SmartPtr<DenseGenMatrixSpace> Qminus_space =
+ new DenseGenMatrixSpace(dim, nneg);
+ Qminus = Qminus_space->MakeNewDenseGenMatrix();
+ Number* Qminus_vals = Qminus->Values();
+ for (Index j=0; j<nneg; j++) {
+ Number esqrt = sqrt(-Evals[j]);
+ for (Index i=0; i<dim; i++) {
+ Qminus_vals[i+j*dim] = Qvals[i+j*dim]/esqrt;
+ }
+ }
+
+ // Create Qplus
+ SmartPtr<DenseGenMatrixSpace> Qplus_space =
+ new DenseGenMatrixSpace(dim, dim-nneg);
+ Qplus = Qplus_space->MakeNewDenseGenMatrix();
+ Number* Qplus_vals = Qplus->Values();
+ for (Index j=0; j<dim-nneg; j++) {
+ DBG_ASSERT(Evals[j+nneg]>0.);
+ Number esqrt = sqrt(Evals[j+nneg]);
+ for (Index i=0; i<dim; i++) {
+ Qplus_vals[i+j*dim] = Qvals[i+(j+nneg)*dim]/esqrt;
+ }
+ }
+
+ return false;
+ }
+
+ bool LimMemQuasiNewtonUpdater::
+ CheckSkippingBFGS(Vector& s_new, Vector& y_new)
+ {
+ Number sTy = s_new.Dot(y_new);
+ Number snrm = s_new.Nrm2();
+ Number ynrm = y_new.Nrm2();
+
+ // ToDo make a parameter?
+ Number tol = sqrt(std::numeric_limits<Number>::epsilon());
+
+ Jnlst().Printf(J_DETAILED, J_HESSIAN_APPROXIMATION,
+ "Limited-Memory test for skipping:\n");
+ Jnlst().Printf(J_DETAILED, J_HESSIAN_APPROXIMATION,
+ " s^Ty = %e snrm = %e ynrm = %e\n",
+ sTy, snrm, ynrm);
+
+ bool skipping;
+
+ DBG_ASSERT(limited_memory_update_type_==BFGS);
+ skipping = (sTy <= tol*snrm*ynrm);
+
+ if (skipping) {
+ Jnlst().Printf(J_DETAILED, J_HESSIAN_APPROXIMATION,
+ " Skip the update.\n");
+ }
+ else {
+ Jnlst().Printf(J_DETAILED, J_HESSIAN_APPROXIMATION,
+ " Perform the update.\n");
+ }
+
+ return skipping;
+ }
+
+ void LimMemQuasiNewtonUpdater::
+ AugmentMultiVector(SmartPtr<MultiVectorMatrix>& V,
+ const Vector& v_new)
+ {
+ Index ncols;
+ if (IsValid(V)) {
+ ncols = V->NCols();
+ }
+ else {
+ ncols = 0;
+ }
+
+ SmartPtr<const VectorSpace> vec_space = v_new.OwnerSpace();
+ SmartPtr<MultiVectorMatrixSpace> new_Vspace =
+ new MultiVectorMatrixSpace(ncols+1, *vec_space);
+ SmartPtr<MultiVectorMatrix> new_V =
+ new_Vspace->MakeNewMultiVectorMatrix();
+ for (Index i=0; i<ncols; i++) {
+ new_V->SetVector(i, *V->GetVector(i));
+ }
+ new_V->SetVector(ncols, v_new);
+
+ V = new_V;
+ }
+
+ void LimMemQuasiNewtonUpdater::
+ AugmentDenseVector(SmartPtr<DenseVector>& V,
+ Number v_new)
+ {
+ Index ndim;
+ if (IsValid(V)) {
+ ndim = V->Dim();
+ }
+ else {
+ ndim = 0;
+ }
+
+ SmartPtr<DenseVectorSpace> new_Vspace =
+ new DenseVectorSpace(ndim+1);
+ SmartPtr<DenseVector> new_V =
+ new_Vspace->MakeNewDenseVector();
+ Number* newVvalues = new_V->Values();
+ if (IsValid(V)) {
+ DBG_ASSERT(!V->IsHomogeneous());
+ const Number* Vvalues = V->Values();
+ for (Index i=0; i<ndim; i++) {
+ newVvalues[i] = Vvalues[i];
+ }
+ }
+ newVvalues[ndim] = v_new;
+
+ V = new_V;
+ }
+
+ void LimMemQuasiNewtonUpdater::
+ AugmentLMatrix(SmartPtr<DenseGenMatrix>& V,
+ const MultiVectorMatrix& S,
+ const MultiVectorMatrix& Y)
+ {
+ Index ndim;
+ if (IsValid(V)) {
+ ndim = V->NCols();
+ DBG_ASSERT(ndim==V->NRows());
+ }
+ else {
+ ndim = 0;
+ }
+ DBG_ASSERT(S.NCols()==ndim+1);
+ DBG_ASSERT(Y.NCols()==ndim+1);
+
+ SmartPtr<DenseGenMatrixSpace> new_Vspace =
+ new DenseGenMatrixSpace(ndim+1, ndim+1);
+ SmartPtr<DenseGenMatrix> new_V =
+ new_Vspace->MakeNewDenseGenMatrix();
+ Number* newVvalues = new_V->Values();
+ if (IsValid(V)) {
+ const Number* Vvalues = V->Values();
+ for (Index j=0; j<ndim; j++) {
+ for (Index i=0; i<ndim; i++) {
+ newVvalues[i+j*(ndim+1)] = Vvalues[i+j*ndim];
+ }
+ }
+ }
+
+ for (Index j=0; j<ndim; j++) {
+ newVvalues[ndim + j*(ndim+1)] =
+ S.GetVector(ndim)->Dot(*Y.GetVector(j));
+ }
+
+ for (Index i=0; i<ndim+1; i++) {
+ newVvalues[i + ndim*(ndim+1)] = 0.;
+ }
+
+ V = new_V;
+ }
+
+ void LimMemQuasiNewtonUpdater::
+ AugmentSdotSMatrix(SmartPtr<DenseSymMatrix>& V,
+ const MultiVectorMatrix& S)
+ {
+ Index ndim;
+ if (IsValid(V)) {
+ ndim = V->Dim();
+ }
+ else {
+ ndim = 0;
+ }
+ DBG_ASSERT(S.NCols()==ndim+1);
+
+ SmartPtr<DenseSymMatrixSpace> new_Vspace =
+ new DenseSymMatrixSpace(ndim+1);
+ SmartPtr<DenseSymMatrix> new_V =
+ new_Vspace->MakeNewDenseSymMatrix();
+ Number* newVvalues = new_V->Values();
+ if (IsValid(V)) {
+ const Number* Vvalues = V->Values();
+ for (Index j=0; j<ndim; j++) {
+ for (Index i=j; i<ndim; i++) {
+ newVvalues[i+j*(ndim+1)] = Vvalues[i+j*ndim];
+ }
+ }
+ }
+
+ for (Index j=0; j<ndim+1; j++) {
+ newVvalues[ndim + j*(ndim+1)] =
+ S.GetVector(ndim)->Dot(*S.GetVector(j));
+ }
+
+ V = new_V;
+ }
+
+ void LimMemQuasiNewtonUpdater::
+ AugmentSTDRSMatrix(SmartPtr<DenseSymMatrix>& V,
+ const MultiVectorMatrix& S,
+ const MultiVectorMatrix& DRS)
+ {
+ Index ndim;
+ if (IsValid(V)) {
+ ndim = V->Dim();
+ }
+ else {
+ ndim = 0;
+ }
+ DBG_ASSERT(S.NCols()==ndim+1);
+
+ SmartPtr<DenseSymMatrixSpace> new_Vspace =
+ new DenseSymMatrixSpace(ndim+1);
+ SmartPtr<DenseSymMatrix> new_V =
+ new_Vspace->MakeNewDenseSymMatrix();
+ Number* newVvalues = new_V->Values();
+ if (IsValid(V)) {
+ const Number* Vvalues = V->Values();
+ for (Index j=0; j<ndim; j++) {
+ for (Index i=j; i<ndim; i++) {
+ newVvalues[i+j*(ndim+1)] = Vvalues[i+j*ndim];
+ }
+ }
+ }
+
+ for (Index j=0; j<ndim+1; j++) {
+ newVvalues[ndim + j*(ndim+1)] =
+ S.GetVector(ndim)->Dot(*DRS.GetVector(j));
+ }
+
+ V = new_V;
+ }
+
+ void LimMemQuasiNewtonUpdater::
+ ShiftMultiVector(SmartPtr<MultiVectorMatrix>& V, const Vector& v_new)
+ {
+ Index ncols = V->NCols();
+
+ SmartPtr<MultiVectorMatrix> new_V = V->MakeNewMultiVectorMatrix();
+
+ for (Index i=0; i<ncols-1; i++) {
+ new_V->SetVector(i, *V->GetVector(i+1));
+ }
+ new_V->SetVector(ncols-1, v_new);
+
+ V = new_V;
+ }
+
+ void LimMemQuasiNewtonUpdater::
+ ShiftDenseVector(SmartPtr<DenseVector>& V, Number v_new)
+ {
+ Index ndim = V->Dim();
+
+ SmartPtr<DenseVector> new_V = V->MakeNewDenseVector();
+
+ DBG_ASSERT(!V->IsHomogeneous());
+ Number* Vvalues = V->Values();
+ Number* new_Vvalues = new_V->Values();
+ for (Index i=0; i<ndim-1; i++) {
+ new_Vvalues[i] = Vvalues[i+1];
+ }
+ new_Vvalues[ndim-1] = v_new;
+
+ V = new_V;
+ }
+
+ void LimMemQuasiNewtonUpdater::
+ ShiftLMatrix(SmartPtr<DenseGenMatrix>& V,
+ const MultiVectorMatrix& S,
+ const MultiVectorMatrix& Y)
+ {
+ Index ndim = V->NCols();
+ DBG_ASSERT(ndim==V->NRows());
+ DBG_ASSERT(S.NCols()==ndim);
+ DBG_ASSERT(Y.NCols()==ndim);
+
+ SmartPtr<DenseGenMatrix> new_V = V->MakeNewDenseGenMatrix();
+
+ Number* Vvalues = V->Values();
+ Number* new_Vvalues = new_V->Values();
+ for (Index j=0; j<ndim-1; j++) {
+ for (Index i=0; i<ndim-1; i++) {
+ new_Vvalues[i+j*ndim] = Vvalues[i+1+(j+1)*ndim];
+ }
+ }
+
+ for (Index j=0; j<ndim-1; j++) {
+ new_Vvalues[ndim-1 + j*ndim] =
+ S.GetVector(ndim-1)->Dot(*Y.GetVector(j));
+ }
+
+ for (Index i=0; i<ndim; i++) {
+ new_Vvalues[i + ndim*(ndim-1)] = 0.;
+ }
+
+ V = new_V;
+ }
+
+ void LimMemQuasiNewtonUpdater::
+ ShiftSdotSMatrix(SmartPtr<DenseSymMatrix>& V,
+ const MultiVectorMatrix& S)
+ {
+ Index ndim = V->Dim();
+ DBG_ASSERT(S.NCols()==ndim);
+
+ SmartPtr<DenseSymMatrix> new_V = V->MakeNewDenseSymMatrix();
+
+ Number* Vvalues = V->Values();
+ Number* new_Vvalues = new_V->Values();
+ for (Index j=0; j<ndim-1; j++) {
+ for (Index i=j; i<ndim-1; i++) {
+ new_Vvalues[i+j*ndim] = Vvalues[i+1+(j+1)*ndim];
+ }
+ }
+
+ for (Index j=0; j<ndim; j++) {
+ new_Vvalues[ndim-1 + j*ndim] =
+ S.GetVector(ndim-1)->Dot(*S.GetVector(j));
+ }
+
+ V = new_V;
+ }
+
+ void LimMemQuasiNewtonUpdater::
+ ShiftSTDRSMatrix(SmartPtr<DenseSymMatrix>& V,
+ const MultiVectorMatrix& S,
+ const MultiVectorMatrix& DRS)
+ {
+ Index ndim = V->Dim();
+ DBG_ASSERT(S.NCols()==ndim);
+
+ SmartPtr<DenseSymMatrix> new_V = V->MakeNewDenseSymMatrix();
+
+ Number* Vvalues = V->Values();
+ Number* new_Vvalues = new_V->Values();
+ for (Index j=0; j<ndim-1; j++) {
+ for (Index i=j; i<ndim-1; i++) {
+ new_Vvalues[i+j*ndim] = Vvalues[i+1+(j+1)*ndim];
+ }
+ }
+
+ for (Index j=0; j<ndim; j++) {
+ new_Vvalues[ndim-1 + j*ndim] =
+ S.GetVector(ndim-1)->Dot(*DRS.GetVector(j));
+ }
+
+ V = new_V;
+ }
+
+ void LimMemQuasiNewtonUpdater::SetW()
+ {
+ DBG_START_METH("LimMemQuasiNewtonUpdater::SetW",
+ dbg_verbosity);
+
+ SmartPtr<Vector> B0;
+ if (update_for_resto_) {
+ B0 = curr_DR_x_->MakeNew();
+ B0->AddOneVector(curr_eta_, *curr_DR_x_, 0.);
+ }
+ else {
+ SmartPtr<const VectorSpace> LM_vecspace =
+ h_space_->LowRankVectorSpace();
+ DBG_ASSERT(IsValid(LM_vecspace));
+ B0 = LM_vecspace->MakeNew();
+ B0->Set(sigma_);
+ }
+ DBG_PRINT_VECTOR(2, "B0", *B0);
+
+ SmartPtr<LowRankUpdateSymMatrix> W =
+ h_space_->MakeNewLowRankUpdateSymMatrix();
+ W->SetDiag(*B0);
+ if (IsValid(V_)) {
+ W->SetV(*V_);
+ }
+ if (IsValid(U_)) {
+ W->SetU(*U_);
+ }
+ if (update_for_resto_) {
+ SmartPtr<const SymMatrixSpace> sp = IpNLP().HessianMatrixSpace();
+ const CompoundSymMatrixSpace* csp =
+ dynamic_cast<const CompoundSymMatrixSpace*> (GetRawPtr(sp));
+ SmartPtr<CompoundSymMatrix> CW =
+ csp->MakeNewCompoundSymMatrix();
+ CW->SetComp(0,0,*W);
+ IpData().Set_W(GetRawPtr(CW));
+ }
+ else {
+ IpData().Set_W(GetRawPtr(W));
+ }
+
+#ifdef PRINT_W
+ // DELETEME
+ const DenseVector* dx = dynamic_cast<const DenseVector*>
+ (GetRawPtr(IpData().curr()->x()));
+ DBG_ASSERT(dx);
+ SmartPtr<DenseVector> tmpx = dx->MakeNewDenseVector();
+ SmartPtr<DenseVector> tmpy = dx->MakeNewDenseVector();
+ for (Index i=0; i<dx->Dim(); i++) {
+ Number* tmpx_vals = tmpx->Values();
+ for (Index j=0; j<dx->Dim(); j++) {
+ tmpx_vals[j] = 0.;
+ }
+ tmpx_vals[i] = 1.;
+ W->MultVector(1., *tmpx, 0., *tmpy);
+ tmpx->Print(Jnlst(), J_DETAILED, J_MAIN, "tmpx");
+ tmpy->Print(Jnlst(), J_DETAILED, J_MAIN, "tmpy");
+ }
+ // ENDDELETEME
+#endif
+
+ }
+
+ void LimMemQuasiNewtonUpdater::RecalcY(Number eta, const Vector& DR_x,
+ MultiVectorMatrix& DRS,
+ MultiVectorMatrix& Ypart,
+ SmartPtr<MultiVectorMatrix>& Y)
+ {
+ SmartPtr<const MultiVectorMatrixSpace> mvspace =
+ Ypart.MultiVectorMatrixOwnerSpace();
+ Y = mvspace->MakeNewMultiVectorMatrix();
+ Y->AddOneMultiVectorMatrix(eta, DRS, 0.);
+ Y->AddOneMultiVectorMatrix(1., Ypart, 1.);
+ }
+
+ void LimMemQuasiNewtonUpdater::RecalcD(MultiVectorMatrix& S,
+ MultiVectorMatrix& Y,
+ SmartPtr<DenseVector>& D)
+ {
+ SmartPtr<DenseVectorSpace> space =
+ new DenseVectorSpace(S.NCols());
+ D = space->MakeNewDenseVector();
+ Number* Dvalues = D->Values();
+ for (Index i=0; i<S.NCols(); i++) {
+ Dvalues[i] = S.GetVector(i)->Dot(*Y.GetVector(i));
+ }
+ }
+
+ void LimMemQuasiNewtonUpdater::RecalcL(MultiVectorMatrix& S,
+ MultiVectorMatrix& Y,
+ SmartPtr<DenseGenMatrix>& L)
+ {
+ Index dim = S.NCols();
+ SmartPtr<DenseGenMatrixSpace> space =
+ new DenseGenMatrixSpace(dim, dim);
+ L = space->MakeNewDenseGenMatrix();
+ Number* Lvalues = L->Values();
+ for (Index j=0; j<dim; j++) {
+ for (Index i=0; i<=j; i++) {
+ Lvalues[i+j*dim] = 0.;
+ }
+ for (Index i=j+1; i<dim; i++) {
+ Lvalues[i+j*dim] = S.GetVector(i)->Dot(*Y.GetVector(j));
+ ;
+ }
+ }
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpLimMemQuasiNewtonUpdater.hpp b/SimTKmath/Optimizers/src/IpOpt/IpLimMemQuasiNewtonUpdater.hpp
new file mode 100644
index 0000000..762cb4e
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpLimMemQuasiNewtonUpdater.hpp
@@ -0,0 +1,354 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpLimMemQuasiNewtonUpdater.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-12-26
+
+#ifndef __IPLIMMEMQUASINEWTONUPDATER_HPP__
+#define __IPLIMMEMQUASINEWTONUPDATER_HPP__
+
+#include "IpHessianUpdater.hpp"
+#include "IpLowRankUpdateSymMatrix.hpp"
+#include "IpMultiVectorMatrix.hpp"
+#include "IpDenseVector.hpp"
+#include "IpDenseGenMatrix.hpp"
+#include "IpDenseSymMatrix.hpp"
+
+namespace Ipopt
+{
+
+ /** Implementation of the HessianUpdater for limit-memory
+ * quasi-Newton approximation of the Lagrangian Hessian.
+ */
+ class LimMemQuasiNewtonUpdater : public HessianUpdater
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor */
+ LimMemQuasiNewtonUpdater(bool update_for_resto);
+
+ /** Default destructor */
+ virtual ~LimMemQuasiNewtonUpdater()
+ {}
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Update the Hessian based on the current information in IpData.
+ */
+ virtual void UpdateHessian();
+
+ /** Methods for OptionsList */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ LimMemQuasiNewtonUpdater(const LimMemQuasiNewtonUpdater&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const LimMemQuasiNewtonUpdater&);
+ //@}
+
+ /** Matrix space for the low-rank Hessian approximation. */
+ SmartPtr<const LowRankUpdateSymMatrixSpace> h_space_;
+
+ /** @name Algorithmic parameters */
+ //@{
+ /** Size of memory for limited memory update. */
+ Index limited_memory_max_history_;
+ /** enumeration for the Hessian update type. */
+ enum LMUpdateType {
+ BFGS=0,
+ SR1
+ };
+ /** Type of Hessian update. */
+ LMUpdateType limited_memory_update_type_;
+ /** enumeration for the Hessian initialization. */
+ enum LMInitialization {
+ SCALAR1=0,
+ SCALAR2,
+ CONSTANT
+ };
+ /** How to choose B0 in the low-rank update. */
+ LMInitialization limited_memory_initialization_;
+ /** Value of B0 (as this multiple of the identity in certain
+ * situations.) */
+ Number limited_memory_init_val_;
+ /** Number of successive iterations of skipped updates after which
+ * the approximation is reset. */
+ Index limited_memory_max_skipping_;
+ /** Minimal safeguard value for sigma */
+ Number sigma_safe_min_;
+ /** Maximal safeguard value for sigma */
+ Number sigma_safe_max_;
+ //@}
+
+ /** Flag indicating if the update is to be done for the original
+ * NLP or for the restoration phase NLP. In the latter case, we
+ * are performing a "structured" update, taking into account the
+ * first explicit term in the objective function of the form
+ * eta*D_r*x_k */
+ const bool update_for_resto_;
+ /** Most recent value for eta in the restoration phase objective
+ * function (only for update_for_resto_ = true) */
+ Number last_eta_;
+ /** Current DR_x scaling factors in the restoration phase
+ * objective function (only for update_for_resto_ = true). This
+ * should not change throughout one restoration phase. */
+ SmartPtr<const Vector> curr_DR_x_;
+ /** Tag for curr_DR_x_ */
+ TaggedObject::Tag curr_DR_x_tag_;
+ /** Current DR_x scaling factors in the restoration phase
+ * objective function in the smaller space for the approximation -
+ * this is only computed if the space is indeed smaller than the
+ * x space (only for update_for_resto_ = true) */
+ SmartPtr<const Vector> curr_red_DR_x_;
+ /** Current value of weighing factor eta in the restoration phase
+ * objective function (only for update_for_resto_ = true) */
+ Number curr_eta_;
+ /** Flag inidicating whether DR_x or eta have changed since the
+ * last update */
+ bool eta_changed_;
+
+ /** Counter for successive iterations in which the update was
+ * skipped */
+ Index lm_skipped_iter_;
+
+ /** @name Information for the limited memory update */
+ //@{
+ /** current size of limited memory */
+ Index curr_lm_memory_;
+ /** s pairs for the recent iterations */
+ SmartPtr<MultiVectorMatrix> S_;
+ /** y pairs for the recent iterations. If update_for_resto is
+ * true, then this includes only the information for the
+ * constraints. */
+ SmartPtr<MultiVectorMatrix> Y_;
+ /** For restoration phase update: Y without the quadratic
+ * objective function part */
+ SmartPtr<MultiVectorMatrix> Ypart_;
+ /** Diagonal elements D_k for compact formulation from last
+ * update. */
+ SmartPtr<DenseVector> D_;
+ /** Matrix L_k for compact formulation from last update.*/
+ SmartPtr<DenseGenMatrix> L_;
+ /** First term (starting matrix) for the approximation. */
+ SmartPtr<Vector> B0_;
+ /** First term (starting matrix) for the approximation. If that
+ * first terms is a multiple of the identy, sigma give that
+ * factor. Otherwise sigma = -1. */
+ Number sigma_;
+ /** V in LowRankUpdateMatrix from last update */
+ SmartPtr<MultiVectorMatrix> V_;
+ /** U in LowRankUpdateMatrix from last update */
+ SmartPtr<MultiVectorMatrix> U_;
+ /** For efficient implementation, we store the pairwise products
+ * for s's. */
+ SmartPtr<DenseSymMatrix> SdotS_;
+ /** Flag indicating whether SdotS_ is update to date from most
+ * recent update. */
+ bool SdotS_uptodate_;
+ /** DR * S (only for restoration phase) */
+ SmartPtr<MultiVectorMatrix> DRS_;
+ /** For efficient implementation, we store the S^T S DR * S. Only
+ * for restoration phase. */
+ SmartPtr<DenseSymMatrix> STDRS_;
+ /** Primal variables x from most recent update */
+ SmartPtr<const Vector> last_x_;
+ /** Gradient of objective function w.r.t. x at x_last_ */
+ SmartPtr<const Vector> last_grad_f_;
+ /** Jacobian for equality constraints w.r.t x at x_last */
+ SmartPtr<const Matrix> last_jac_c_;
+ /** Jacobian for inequality constraints w.r.t x at x_last */
+ SmartPtr<const Matrix> last_jac_d_;
+ /** current size of limited memory */
+ Index curr_lm_memory_old_;
+ /** s pairs for the recent iterations (backup) */
+ SmartPtr<MultiVectorMatrix> S_old_;
+ /** y pairs for the recent iterations. If update_for_resto is
+ * true, then this includes only the information for the
+ * constraints. (backup) */
+ SmartPtr<MultiVectorMatrix> Y_old_;
+ /** For restoration phase update: Y without the quadratic
+ * objective function part (backup) */
+ SmartPtr<MultiVectorMatrix> Ypart_old_;
+ /** Diagonal elements D_k for compact formulation from last
+ * update (backup). */
+ SmartPtr<DenseVector> D_old_;
+ /** Matrix L_k for compact formulation from last update (backup).*/
+ SmartPtr<DenseGenMatrix> L_old_;
+ /** First term (starting matrix) for the approximation (backup). */
+ SmartPtr<Vector> B0_old_;
+ /** First term (starting matrix) for the approximation. If that
+ * first terms is a multiple of the identy, sigma give that
+ * factor. Otherwise sigma = -1. (backup) */
+ Number sigma_old_;
+ /** V in LowRankUpdateMatrix from last update (backup) */
+ SmartPtr<MultiVectorMatrix> V_old_;
+ /** U in LowRankUpdateMatrix from last update (backup) */
+ SmartPtr<MultiVectorMatrix> U_old_;
+ /** For efficient implementation, we store the pairwise products
+ * for s's (backup). */
+ SmartPtr<DenseSymMatrix> SdotS_old_;
+ /** Flag indicating whether SdotS_ is update to date from most
+ * recent update (backup). */
+ bool SdotS_uptodate_old_;
+ /** DR * S (only for restoration phase) (backup) */
+ SmartPtr<MultiVectorMatrix> DRS_old_;
+ /** For efficient implementation, we store the S^T S DR * S. Only
+ * for restoration phase. (backup) */
+ SmartPtr<DenseSymMatrix> STDRS_old_;
+ //@}
+
+ /** @name Auxilliary function */
+ //@{
+ /** Method deciding whether the BFGS update should be skipped. It
+ * returns true, if no update is to be performed this time. If
+ * Powell-damping is performed, the Vectors s_new and y_new,
+ * might be adapted. */
+ bool CheckSkippingBFGS(Vector& s_new, Vector& y_new);
+ /** Update the internal data, such as the S, Y, L, D etc matrices
+ * and vectors that are required for computing the compact
+ * representation. The method returns true if the limited memory
+ * history grew (i.e., curr_lm_memory_ was increased). */
+ bool UpdateInternalData(const Vector& s_new, const Vector& y_new,
+ SmartPtr<Vector> ypart_new);
+ /** Given a MutliVector V, create a new MultiVectorSpace with one
+ * more column, and return V as a member of that space,
+ * consisting of all previous vectors, and in addition v_new in
+ * the last column. If V is NULL, then a new MatrixSpace with
+ * one column is created. */
+ void AugmentMultiVector(SmartPtr<MultiVectorMatrix>& V,
+ const Vector& v_new);
+ /** Given a DenseVector V, create a new DenseVectorSpace with one
+ * more row, and return V as a member of that space,
+ * consisting of all previous elements, and in addition v_new in
+ * the last row. If V is NULL, then a new DenseVectorSpace with
+ * dimension one is created. */
+ void AugmentDenseVector(SmartPtr<DenseVector>& V,
+ Number v_new);
+ /** Given a strictly-lower triangular square DenseGenMatrix V,
+ * create a new DenseGenMatrixSpace with one more dimension, and
+ * return V as a member of that space, consisting of all previous
+ * elements, and in addition elements s_i^Ty_j for (i<j), where s
+ * and y are the vectors in the MultiVectors S and Y. If V is
+ * NULL, then a new DenseGenMatrixSpace with dimension one is
+ * created. */
+ void AugmentLMatrix(SmartPtr<DenseGenMatrix>& V,
+ const MultiVectorMatrix& S,
+ const MultiVectorMatrix& Y);
+ /** Given a DenseSymMatrix V, create a new DenseGenMatrixSpace
+ * with one more dimension, and return V as a member of that
+ * space, consisting of all previous elements, and in addition
+ * elements s_i^Ts_j for the new entries, where s are the vectors
+ * in the MultiVector S. If V is NULL, then a new
+ * DenseGenMatrixSpace with dimension one is created. */
+ void AugmentSdotSMatrix(SmartPtr<DenseSymMatrix>& V,
+ const MultiVectorMatrix& S);
+ /** Given a DenseSymMatrix V, create a new DenseGenMatrixSpace
+ * with one more dimension, and return V as a member of that
+ * space, consisting of all previous elements, and in addition
+ * elements s_i^TDRs_j for the new entries, where s are the
+ * vectors in the MultiVector S, and DRs are the vectors in DRS.
+ * If V is NULL, then a new DenseGenMatrixSpace with dimension
+ * one is created. */
+ void AugmentSTDRSMatrix(SmartPtr<DenseSymMatrix>& V,
+ const MultiVectorMatrix& S,
+ const MultiVectorMatrix& DRS);
+
+ /** Given a MutliVector V, get rid of the first column, shift all
+ * other columns to the left, and make v_new the last column.
+ * The entity that V points to at the call, is not changed - a
+ * new entity is created in the method and returned as V. */
+ void ShiftMultiVector(SmartPtr<MultiVectorMatrix>& V, const Vector& v_new);
+ /** Given a DenseVector V, get rid of the first element, shift all
+ * other elements one position to the top, and make v_new the
+ * last entry. The entity that V points to at the call, is not
+ * changed - a new entity is created in the method and returned
+ * as V. */
+ void ShiftDenseVector(SmartPtr<DenseVector>& V, Number v_new);
+ /** Given a strictly-lower triangular square DenseGenMatrix V,
+ * shift everything one row and column up, and fill the new
+ * strictly lower triangular entries as s_i^Ty_j for (i<j), where
+ * s and y are the vectors in the MultiVectors S and Y. The
+ * entity that V points to at the call, is not changed - a new
+ * entity is created in the method and returned as V. */
+ void ShiftLMatrix(SmartPtr<DenseGenMatrix>& V,
+ const MultiVectorMatrix& S,
+ const MultiVectorMatrix& Y);
+ /** Given a DenseSymMatrix V, shift everything up one row and
+ * column, and fill the new entries as s_i^Ts_j, where s are the
+ * vectors in the MultiVector S. The entity that V points to at
+ * the call, is not changed - a new entity is created in the
+ * method and returned as V. */
+ void ShiftSdotSMatrix(SmartPtr<DenseSymMatrix>& V,
+ const MultiVectorMatrix& S);
+ /** Given a DenseSymMatrix V, shift everything up one row and
+ * column, and fill the new entries as s_i^TDRs_j, where s are
+ * the vectors in the MultiVector S, and DRs are the vectors in
+ * DRS. The entity that V points to at the call, is not changed -
+ * a new entity is created in the method and returned as V. */
+ void ShiftSTDRSMatrix(SmartPtr<DenseSymMatrix>& V,
+ const MultiVectorMatrix& S,
+ const MultiVectorMatrix& DRS);
+ /** Method for recomputing Y from scratch, using Ypart (only for
+ * restoration phase) */
+ void RecalcY(Number eta, const Vector& DR_x,
+ MultiVectorMatrix& S,
+ MultiVectorMatrix& Ypart,
+ SmartPtr<MultiVectorMatrix>& Y);
+ /** Method for recomputing D from S and Y */
+ void RecalcD(MultiVectorMatrix& S,
+ MultiVectorMatrix& Y,
+ SmartPtr<DenseVector>& D);
+ /** Method for recomputing L from S and Y */
+ void RecalcL(MultiVectorMatrix& S,
+ MultiVectorMatrix& Y,
+ SmartPtr<DenseGenMatrix>& L);
+ /** Split the eigenvectors into negative and positive ones. Given
+ * the eigenvectors in Q and the eigenvalues (in ascending order)
+ * in, this returns Qminus as the negative eigenvectors times
+ * sqrt(-eval), and Qplus as the positive eigenvectors times
+ * sqrt(eval). If Qminus or Qplus is NULL, it means that there
+ * are not negetive or positive eigenvalues. Q might be changed
+ * during this call. The return value is true, if the ratio of
+ * the smallest over the largest eigenvalue (in absolute values)
+ * is too small; in that case, the update should be skipped. */
+ bool SplitEigenvalues(DenseGenMatrix& Q, const DenseVector& E,
+ SmartPtr<DenseGenMatrix>& Qminus,
+ SmartPtr<DenseGenMatrix>& Qplus);
+ /** Store a copy of the pointers to the internal data (S, Y, D, L,
+ * SdotS, curr_lm_memory) This is called in case the update is
+ * started but skipped during the process. */
+ void StoreInternalDataBackup();
+ /** Restore the copy of the pointers to the internal data most
+ * recently stored with StoreInternalDataBackup(). */
+ void RestoreInternalDataBackup();
+ /** Release anything that we allocated for
+ * StoreInternalDataBackup and is no longer needed. */
+ void ReleaseInternalDataBackup();
+ /** Set the W field in IpData based on the current values of
+ * B0_, V_, and U_ */
+ void SetW();
+ //@}
+
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpLineSearch.hpp b/SimTKmath/Optimizers/src/IpOpt/IpLineSearch.hpp
new file mode 100644
index 0000000..0909cc7
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpLineSearch.hpp
@@ -0,0 +1,96 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpLineSearch.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPLINESEARCH_HPP__
+#define __IPLINESEARCH_HPP__
+
+#include "IpAlgStrategy.hpp"
+#include "IpIpoptCalculatedQuantities.hpp"
+
+namespace Ipopt
+{
+
+ /** Base class for line search objects.
+ */
+ class LineSearch : public AlgorithmStrategyObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor */
+ LineSearch()
+ {}
+
+ /** Default destructor */
+ virtual ~LineSearch()
+ {}
+ //@}
+
+ /** Perform the line search. As search direction the delta
+ * in the data object is used
+ */
+ virtual void FindAcceptableTrialPoint() = 0;
+
+ /** Reset the line search.
+ * This function should be called if all previous information
+ * should be discarded when the line search is performed the
+ * next time. For example, this method should be called after
+ * the barrier parameter is changed.
+ */
+ virtual void Reset() = 0;
+
+ /** Set flag indicating whether a very rigorous line search should
+ * be performed. If this flag is set to true, the line search
+ * algorithm might decide to abort the line search and not to
+ * accept a new iterate. If the line search decided not to
+ * accept a new iterate, the return value of
+ * CheckSkippedLineSearch() is true at the next call. For
+ * example, in the non-monotone barrier parameter update
+ * procedure, the filter algorithm should not switch to the
+ * restoration phase in the free mode; instead, the algorithm
+ * should swtich to the fixed mode.
+ */
+ virtual void SetRigorousLineSearch(bool rigorous) = 0;
+
+ /** Check if the line search procedure didn't accept a new iterate
+ * during the last call of FindAcceptableTrialPoint().
+ *
+ */
+ virtual bool CheckSkippedLineSearch() = 0;
+
+ /** This method should be called if the optimization process
+ * requires the line search object to switch to some fallback
+ * mechanism (like the restoration phase), when the regular
+ * optimization procedure cannot be continued (for example,
+ * because the search direction could not be computed). This
+ * will cause the line search object to immediately proceed with
+ * this mechanism when FindAcceptableTrialPoint() is call. This
+ * method returns false if no fallback mechanism is available. */
+ virtual bool ActivateFallbackMechanism() = 0;
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ LineSearch(const LineSearch&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const LineSearch&);
+ //@}
+
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpLinearSolversRegOp.cpp b/SimTKmath/Optimizers/src/IpOpt/IpLinearSolversRegOp.cpp
new file mode 100644
index 0000000..12dd8b0
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpLinearSolversRegOp.cpp
@@ -0,0 +1,26 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpLinearSolversRegOp.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-08-16
+
+#include "IpoptConfig.h"
+#include "IpLinearSolversRegOp.hpp"
+#include "IpRegOptions.hpp"
+#include "IpTSymLinearSolver.hpp"
+
+
+namespace Ipopt
+{
+
+ void RegisterOptions_LinearSolvers(const SmartPtr<RegisteredOptions>& roptions)
+ {
+ roptions->SetRegisteringCategory("Linear Solver");
+ TSymLinearSolver::RegisterOptions(roptions);
+
+ roptions->SetRegisteringCategory("Uncategorized");
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpLinearSolversRegOp.hpp b/SimTKmath/Optimizers/src/IpOpt/IpLinearSolversRegOp.hpp
new file mode 100644
index 0000000..536317e
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpLinearSolversRegOp.hpp
@@ -0,0 +1,22 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpLinearSolversRegOp.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-08-16
+
+#ifndef __IPLINEARSOLVERSREGOP_HPP__
+#define __IPLINEARSOLVERSREGOP_HPP__
+
+#include "IpSmartPtr.hpp"
+
+namespace Ipopt
+{
+ class RegisteredOptions;
+
+ void RegisterOptions_LinearSolvers(const SmartPtr<RegisteredOptions>& roptions);
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpLoqoMuOracle.cpp b/SimTKmath/Optimizers/src/IpOpt/IpLoqoMuOracle.cpp
new file mode 100644
index 0000000..3c4035b
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpLoqoMuOracle.cpp
@@ -0,0 +1,85 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpLoqoMuOracle.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpLoqoMuOracle.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+#include <limits>
+#include <cstdio>
+
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+
+
+namespace Ipopt
+{
+
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ LoqoMuOracle::LoqoMuOracle()
+ :
+ MuOracle()
+ {}
+
+ LoqoMuOracle::~LoqoMuOracle()
+ {}
+
+ bool LoqoMuOracle::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ return true;
+ }
+
+ bool LoqoMuOracle::CalculateMu(Number mu_min, Number mu_max,
+ Number& new_mu)
+ {
+ DBG_START_METH("LoqoMuOracle::CalculateMu",
+ dbg_verbosity);
+
+ Number avrg_compl = IpCq().curr_avrg_compl();
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ " Average complemantarity is %lf\n", avrg_compl);
+
+ Number xi = IpCq().curr_centrality_measure();
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ " Xi (distance from uniformity) is %lf\n", xi);
+
+ //Number factor = 1.-tau_min_; //This is the original values
+ Number factor = Number(0.05); //This is the value I used otherwise
+ Number sigma = Number(0.1)*std::pow(Min(factor*(1-xi)/xi,Number(2)),Number(3));
+
+ Number mu = sigma*avrg_compl;
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ " Barrier parameter proposed by LOQO rule is %lf\n", mu);
+
+ // DELETEME
+ char ssigma[40];
+ sprintf(ssigma, " sigma=%8.2e", sigma);
+ IpData().Append_info_string(ssigma);
+ sprintf(ssigma, " xi=%8.2e ", IpCq().curr_centrality_measure());
+ IpData().Append_info_string(ssigma);
+
+ new_mu = Max(Min(mu_max, mu), mu_min);
+ return true;
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpLoqoMuOracle.hpp b/SimTKmath/Optimizers/src/IpOpt/IpLoqoMuOracle.hpp
new file mode 100644
index 0000000..500234b
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpLoqoMuOracle.hpp
@@ -0,0 +1,61 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpLoqoMuOracle.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPLOQOMUORACLE_HPP__
+#define __IPLOQOMUORACLE_HPP__
+
+#include "IpMuOracle.hpp"
+
+namespace Ipopt
+{
+
+ /** Implementation of the LOQO formula for computing the
+ * barrier parameter.
+ */
+ class LoqoMuOracle : public MuOracle
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor */
+ LoqoMuOracle();
+ /** Default destructor */
+ virtual ~LoqoMuOracle();
+ //@}
+
+ /** Initialize method - overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Method for computing the value of the barrier parameter that
+ * could be used in the current iteration (using the LOQO formula).
+ */
+ virtual bool CalculateMu(Number mu_min, Number mu_max, Number& new_mu);
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+
+ /** Copy Constructor */
+ LoqoMuOracle(const LoqoMuOracle&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const LoqoMuOracle&);
+ //@}
+
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpLowRankAugSystemSolver.cpp b/SimTKmath/Optimizers/src/IpOpt/IpLowRankAugSystemSolver.cpp
new file mode 100644
index 0000000..1ad52ac
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpLowRankAugSystemSolver.cpp
@@ -0,0 +1,599 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpLowRankAugSystemSolver.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-12-27
+
+#include "IpLowRankAugSystemSolver.hpp"
+#include "IpLowRankUpdateSymMatrix.hpp"
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ LowRankAugSystemSolver::LowRankAugSystemSolver(
+ AugSystemSolver& aug_system_solver)
+ :
+ AugSystemSolver(),
+ aug_system_solver_(&aug_system_solver),
+ w_tag_(0),
+ w_factor_(0.),
+ d_x_tag_(0),
+ delta_x_(0.),
+ d_s_tag_(0),
+ delta_s_(0.),
+ j_c_tag_(0),
+ d_c_tag_(0),
+ delta_c_(0.),
+ j_d_tag_(0),
+ d_d_tag_(0),
+ delta_d_(0.)
+ {
+ DBG_START_METH("LowRankAugSystemSolver::LowRankAugSystemSolver()",dbg_verbosity);
+ DBG_ASSERT(IsValid(aug_system_solver_));
+ }
+
+ LowRankAugSystemSolver::~LowRankAugSystemSolver()
+ {
+ DBG_START_METH("LowRankAugSystemSolver::~LowRankAugSystemSolver()",dbg_verbosity);
+ }
+
+ bool LowRankAugSystemSolver::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ first_call_ = true;
+ J1_ = NULL;
+ J2_ = NULL;
+ Vtilde1_ = NULL;
+ Utilde2_ = NULL;
+ Wdiag_ = NULL;
+ compound_sol_vecspace_ = NULL;
+
+ return aug_system_solver_->Initialize(Jnlst(), IpNLP(), IpData(), IpCq(),
+ options, prefix);
+ }
+
+ ESymSolverStatus LowRankAugSystemSolver::Solve(
+ const SymMatrix* W,
+ Number W_factor,
+ const Vector* D_x,
+ Number delta_x,
+ const Vector* D_s,
+ Number delta_s,
+ const Matrix* J_c,
+ const Vector* D_c,
+ Number delta_c,
+ const Matrix* J_d,
+ const Vector* D_d,
+ Number delta_d,
+ const Vector& rhs_x,
+ const Vector& rhs_s,
+ const Vector& rhs_c,
+ const Vector& rhs_d,
+ Vector& sol_x,
+ Vector& sol_s,
+ Vector& sol_c,
+ Vector& sol_d,
+ bool check_NegEVals,
+ Index numberOfNegEVals)
+ {
+ DBG_START_METH("LowRankAugSystemSolver::Solve",dbg_verbosity);
+
+ ESymSolverStatus retval;
+
+ if (first_call_) {
+ DBG_ASSERT(IsNull(Wdiag_));
+ // Set up the diagonal matrix Wdiag_
+ Index dimx = rhs_x.Dim();
+ SmartPtr<DiagMatrixSpace> Wdiag_space = new DiagMatrixSpace(dimx);
+ Wdiag_ = Wdiag_space->MakeNewDiagMatrix();
+ }
+
+ if (first_call_ ||
+ AugmentedSystemRequiresChange(W, W_factor, D_x, delta_x, D_s, delta_s,
+ *J_c, D_c, delta_c, *J_d, D_d,
+ delta_d) ) {
+ retval = UpdateFactorization(W, W_factor, D_x, delta_x, D_s, delta_s,
+ *J_c, D_c, delta_c, *J_d, D_d, delta_d,
+ rhs_x, rhs_s, rhs_c, rhs_d,
+ check_NegEVals, numberOfNegEVals);
+ if (retval != SYMSOLVER_SUCCESS) {
+ return retval;
+ }
+
+ // Store the tags
+ w_tag_ = W->GetTag();
+ w_factor_ = W_factor;
+ if (D_x) {
+ d_x_tag_ = D_x->GetTag();
+ }
+ else {
+ d_x_tag_ = 0;
+ }
+ delta_x_ = delta_x;
+ if (D_s) {
+ d_s_tag_ = D_s->GetTag();
+ }
+ else {
+ d_s_tag_ = 0;
+ }
+ delta_s_ = delta_s;
+ if (J_c) {
+ j_c_tag_ = J_c->GetTag();
+ }
+ else {
+ j_c_tag_ = 0;
+ }
+ if (D_c) {
+ d_c_tag_ = D_c->GetTag();
+ }
+ else {
+ d_c_tag_ = 0;
+ }
+ delta_c_ = delta_c;
+ if (J_d) {
+ j_d_tag_ = J_d->GetTag();
+ }
+ else {
+ j_d_tag_ = 0;
+ }
+ if (D_d) {
+ d_d_tag_ = D_d->GetTag();
+ }
+ else {
+ d_d_tag_ = 0;
+ }
+ delta_d_ = delta_d;
+
+ first_call_ = false;
+ }
+
+ // Now solve the system for the given right hand side, using the
+ // Sherman-Morrison formula with factorization information already
+ // computed.
+ retval = aug_system_solver_->Solve(GetRawPtr(Wdiag_), W_factor,
+ D_x, delta_x, D_s, delta_s,
+ J_c, D_c, delta_c, J_d, D_d, delta_d,
+ rhs_x, rhs_s, rhs_c, rhs_d,
+ sol_x, sol_s, sol_c, sol_d,
+ check_NegEVals, numberOfNegEVals);
+ if (aug_system_solver_->ProvidesInertia()) {
+ num_neg_evals_ = aug_system_solver_->NumberOfNegEVals();
+ }
+ if (retval != SYMSOLVER_SUCCESS) {
+ Jnlst().Printf(J_DETAILED, J_SOLVE_PD_SYSTEM,
+ "LowRankAugSystemSolver: AugSystemSolver returned retval = %d for right hand side.\n", retval);
+ return retval;
+ }
+
+ if (IsValid(Vtilde1_) || IsValid(Utilde2_)) {
+ // Create a CompoundVectors to store the right hand side and
+ // solutions
+ SmartPtr<CompoundVector> crhs =
+ compound_sol_vecspace_->MakeNewCompoundVector(false);
+ crhs->SetComp(0, rhs_x);
+ crhs->SetComp(1, rhs_s);
+ crhs->SetComp(2, rhs_c);
+ crhs->SetComp(3, rhs_d);
+ SmartPtr<CompoundVector> csol =
+ compound_sol_vecspace_->MakeNewCompoundVector(false);
+ csol->SetCompNonConst(0, sol_x);
+ csol->SetCompNonConst(1, sol_s);
+ csol->SetCompNonConst(2, sol_c);
+ csol->SetCompNonConst(3, sol_d);
+
+ if (IsValid(Utilde2_)) {
+ Index nU = Utilde2_->NCols();
+ SmartPtr<DenseVectorSpace> bUspace =
+ new DenseVectorSpace(nU);
+ SmartPtr<DenseVector> bU = bUspace->MakeNewDenseVector();
+ Utilde2_->TransMultVector(1., *crhs, 0., *bU);
+ J2_->CholeskySolveVector(*bU);
+ Utilde2_->MultVector(1., *bU, 1., *csol);
+ }
+ if (IsValid(Vtilde1_)) {
+ Index nV = Vtilde1_->NCols();
+ SmartPtr<DenseVectorSpace> bVspace =
+ new DenseVectorSpace(nV);
+ SmartPtr<DenseVector> bV = bVspace->MakeNewDenseVector();
+ Vtilde1_->TransMultVector(1., *crhs, 0., *bV);
+ J1_->CholeskySolveVector(*bV);
+ Vtilde1_->MultVector(-1., *bV, 1., *csol);
+ }
+ }
+
+ return retval;
+ }
+
+ ESymSolverStatus LowRankAugSystemSolver::UpdateFactorization(
+ const SymMatrix* W,
+ Number W_factor,
+ const Vector* D_x,
+ Number delta_x,
+ const Vector* D_s,
+ Number delta_s,
+ const Matrix& J_c,
+ const Vector* D_c,
+ Number delta_c,
+ const Matrix& J_d,
+ const Vector* D_d,
+ Number delta_d,
+ const Vector& proto_rhs_x,
+ const Vector& proto_rhs_s,
+ const Vector& proto_rhs_c,
+ const Vector& proto_rhs_d,
+ bool check_NegEVals,
+ Index numberOfNegEVals)
+ {
+ DBG_START_METH("LowRankAugSystemSolver::UpdateFactorization",
+ dbg_verbosity);
+
+ DBG_ASSERT(W_factor == 0.0 || W_factor == 1.0);
+ ESymSolverStatus retval = SYMSOLVER_SUCCESS;
+
+ // Get the low update information out of W
+ const LowRankUpdateSymMatrix* LR_W =
+ dynamic_cast<const LowRankUpdateSymMatrix*> (W);
+ DBG_ASSERT(LR_W);
+ DBG_PRINT_MATRIX(2, "LR_W", *LR_W);
+
+ SmartPtr<const Vector> B0;
+ SmartPtr<const MultiVectorMatrix> V;
+ SmartPtr<const MultiVectorMatrix> U;
+ if (W_factor == 1.0) {
+ V = LR_W->GetV();
+ U = LR_W->GetU();
+ B0 = LR_W->GetDiag();
+ }
+ SmartPtr<const Matrix> P_LM = LR_W->P_LowRank();
+ SmartPtr<const VectorSpace> LR_VecSpace = LR_W->LowRankVectorSpace();
+
+ if (IsNull(B0)) {
+ SmartPtr<Vector> zero_B0 = (IsValid(P_LM)) ? LR_VecSpace->MakeNew() : proto_rhs_x.MakeNew();
+ zero_B0->Set(0.0);
+ B0 = GetRawPtr(zero_B0);
+ }
+
+ // set up the Hessian for the underlying augmented system solver
+ // without the low-rank update
+ if (IsValid(P_LM) && LR_W->ReducedDiag()) {
+ DBG_ASSERT(IsValid(B0));
+ SmartPtr<Vector> fullx = proto_rhs_x.MakeNew();
+ P_LM->MultVector(1., *B0, 0., *fullx);
+ Wdiag_->SetDiag(*fullx);
+ }
+ else {
+ Wdiag_->SetDiag(*B0);
+ }
+
+ SmartPtr<MultiVectorMatrix> Vtilde1_x;
+ if (IsValid(V)) {
+ SmartPtr<MultiVectorMatrix> V_x;
+ Index nV = V->NCols();
+ //DBG_PRINT((1, "delta_x = %e\n", delta_x));
+ //DBG_PRINT_MATRIX(2, "V", *V);
+ retval = SolveMultiVector(D_x, delta_x, D_s, delta_s, J_c,
+ D_c, delta_c, J_d, D_d, delta_d,
+ proto_rhs_x, proto_rhs_s, proto_rhs_c,
+ proto_rhs_d, *V, P_LM, V_x, Vtilde1_,
+ Vtilde1_x, check_NegEVals, numberOfNegEVals);
+ if (retval != SYMSOLVER_SUCCESS) {
+ Jnlst().Printf(J_DETAILED, J_SOLVE_PD_SYSTEM,
+ "LowRankAugSystemSolver: SolveMultiVector returned retval = %d for V.\n", retval);
+ return retval;
+ }
+ //DBG_PRINT_MATRIX(2, "Vtilde1_x", *Vtilde1_x);
+
+ SmartPtr<DenseSymMatrixSpace> M1space =
+ new DenseSymMatrixSpace(nV);
+ SmartPtr<DenseSymMatrix> M1 = M1space->MakeNewDenseSymMatrix();
+ M1->FillIdentity();
+ M1->HighRankUpdateTranspose(1., *Vtilde1_x, *V_x, 1.);
+ //DBG_PRINT_MATRIX(2, "M1", *M1);
+ SmartPtr<DenseGenMatrixSpace> J1space =
+ new DenseGenMatrixSpace(nV, nV);
+ J1_ = J1space->MakeNewDenseGenMatrix();
+ bool retchol = J1_->ComputeCholeskyFactor(*M1);
+ // M1 must be positive definite!
+ //DBG_ASSERT(retchol);
+ if (!retchol) {
+ Jnlst().Printf(J_DETAILED, J_SOLVE_PD_SYSTEM,
+ "LowRankAugSystemSolver: Cholesky for M1 returned error!\n");
+ retval = SYMSOLVER_WRONG_INERTIA;
+ num_neg_evals_++;
+ return retval;
+ }
+ }
+ else {
+ Vtilde1_ = NULL;
+ J1_ = NULL;
+ }
+
+ if (IsValid(U)) {
+ Index nU = U->NCols();
+ SmartPtr<MultiVectorMatrix> U_x;
+ SmartPtr<MultiVectorMatrix> Utilde1;
+ SmartPtr<MultiVectorMatrix> Utilde1_x;
+ SmartPtr<MultiVectorMatrix> Utilde2_x;
+ retval = SolveMultiVector(D_x, delta_x, D_s, delta_s, J_c,
+ D_c, delta_c, J_d, D_d, delta_d,
+ proto_rhs_x, proto_rhs_s, proto_rhs_c,
+ proto_rhs_d, *U, P_LM, U_x, Utilde1,
+ Utilde1_x, check_NegEVals, numberOfNegEVals);
+ if (retval != SYMSOLVER_SUCCESS) {
+ Jnlst().Printf(J_DETAILED, J_SOLVE_PD_SYSTEM,
+ "LowRankAugSystemSolver: SolveMultiVector returned retval = %d for U.\n", retval);
+ return retval;
+ }
+
+ if (IsNull(Vtilde1_)) {
+ Utilde2_ = Utilde1;
+ Utilde2_x = Utilde1_x;
+ }
+ else {
+ Index nV = Vtilde1_->NCols();
+ SmartPtr<DenseGenMatrixSpace> Cspace =
+ new DenseGenMatrixSpace(nV, nU);
+ SmartPtr<DenseGenMatrix> C = Cspace->MakeNewDenseGenMatrix();
+ C->HighRankUpdateTranspose(1., *Vtilde1_x, *U_x, 0.);
+ J1_->CholeskySolveMatrix(*C);
+ Utilde2_ = Utilde1;
+ Utilde2_->AddRightMultMatrix(-1, *Vtilde1_, *C, 1.);
+ Utilde2_x = Utilde1_x->MakeNewMultiVectorMatrix();
+ for (Index i=0; i<Utilde1_x->NCols(); i++) {
+ const CompoundVector* cvec =
+ dynamic_cast<const CompoundVector*> (GetRawPtr(Utilde2_->GetVector(i)));
+ DBG_ASSERT(cvec);
+ Utilde2_x->SetVector(i, *cvec->GetComp(0));
+ }
+ }
+
+ SmartPtr<DenseSymMatrixSpace> M2space =
+ new DenseSymMatrixSpace(nU);
+ SmartPtr<DenseSymMatrix> M2 = M2space->MakeNewDenseSymMatrix();
+ M2->FillIdentity();
+ M2->HighRankUpdateTranspose(-1., *Utilde2_x, *U_x, 1.);
+ SmartPtr<DenseGenMatrixSpace> J2space =
+ new DenseGenMatrixSpace(nU, nU);
+ J2_ = J2space->MakeNewDenseGenMatrix();
+ //DBG_PRINT_MATRIX(2, "M2", *M2);
+ bool retchol = J2_->ComputeCholeskyFactor(*M2);
+ if (!retchol) {
+ Jnlst().Printf(J_DETAILED, J_SOLVE_PD_SYSTEM,
+ "LowRankAugSystemSolver: Cholesky for M2 returned error.\n");
+ retval = SYMSOLVER_WRONG_INERTIA;
+ num_neg_evals_++;
+ return retval;
+ }
+ }
+ else {
+ J2_ = NULL;
+ Utilde2_ = NULL;
+ }
+
+ return retval;
+ }
+
+ ESymSolverStatus LowRankAugSystemSolver::SolveMultiVector(
+ const Vector* D_x,
+ Number delta_x,
+ const Vector* D_s,
+ Number delta_s,
+ const Matrix& J_c,
+ const Vector* D_c,
+ Number delta_c,
+ const Matrix& J_d,
+ const Vector* D_d,
+ Number delta_d,
+ const Vector& proto_rhs_x,
+ const Vector& proto_rhs_s,
+ const Vector& proto_rhs_c,
+ const Vector& proto_rhs_d,
+ const MultiVectorMatrix& V,
+ const SmartPtr<const Matrix>& P_LM,
+ SmartPtr<MultiVectorMatrix>& V_x,
+ SmartPtr<MultiVectorMatrix>& Vtilde,
+ SmartPtr<MultiVectorMatrix>& Vtilde_x,
+ bool check_NegEVals,
+ Index numberOfNegEVals)
+ {
+ DBG_START_METH("LowRankAugSystemSolver::SolveMultiVector",
+ dbg_verbosity);
+
+ ESymSolverStatus retval;
+
+ Index nrhs = V.NCols();
+ DBG_ASSERT(nrhs>0);
+
+ SmartPtr<MultiVectorMatrixSpace> V_xspace =
+ new MultiVectorMatrixSpace(nrhs, *proto_rhs_x.OwnerSpace());
+ V_x = V_xspace->MakeNewMultiVectorMatrix();
+
+ // Create the right hand sides
+ std::vector<SmartPtr<const Vector> > rhs_xV(nrhs);
+ std::vector<SmartPtr<const Vector> > rhs_sV(nrhs);
+ std::vector<SmartPtr<const Vector> > rhs_cV(nrhs);
+ std::vector<SmartPtr<const Vector> > rhs_dV(nrhs);
+
+ for (Index i=0; i<nrhs; i++) {
+ if (IsNull(P_LM)) {
+ rhs_xV[i] = V.GetVector(i);
+ DBG_ASSERT(rhs_xV[i]->Dim() == proto_rhs_x.Dim());
+ }
+ else {
+ SmartPtr<Vector> fullx = proto_rhs_x.MakeNew();
+ P_LM->MultVector(1., *V.GetVector(i), 0., *fullx);
+ rhs_xV[i] = ConstPtr(fullx);
+ }
+ V_x->SetVector(i, *rhs_xV[i]);
+ SmartPtr<Vector> tmp;
+ tmp = proto_rhs_s.MakeNew();
+ tmp->Set(0.);
+ rhs_sV[i] = ConstPtr(tmp);
+ tmp = proto_rhs_c.MakeNew();
+ tmp->Set(0.);
+ rhs_cV[i] = ConstPtr(tmp);
+ tmp = proto_rhs_d.MakeNew();
+ tmp->Set(0.);
+ rhs_dV[i] = ConstPtr(tmp);
+ }
+
+ // now get space for the solution
+ std::vector<SmartPtr<Vector> > sol_xV(nrhs);
+ std::vector<SmartPtr<Vector> > sol_sV(nrhs);
+ std::vector<SmartPtr<Vector> > sol_cV(nrhs);
+ std::vector<SmartPtr<Vector> > sol_dV(nrhs);
+ for (Index i=0; i<nrhs; i++) {
+ sol_xV[i] = proto_rhs_x.MakeNew();
+ sol_sV[i] = proto_rhs_s.MakeNew();
+ sol_cV[i] = proto_rhs_c.MakeNew();
+ sol_dV[i] = proto_rhs_d.MakeNew();
+ }
+
+ // Call the actual augmented system solver to obtain Vtilde
+ retval = aug_system_solver_->MultiSolve(GetRawPtr(Wdiag_), Number(1), D_x, delta_x, D_s, delta_s,
+ &J_c, D_c, delta_c, &J_d, D_d, delta_d,
+ rhs_xV, rhs_sV, rhs_cV, rhs_dV,
+ sol_xV, sol_sV, sol_cV, sol_dV,
+ check_NegEVals, numberOfNegEVals);
+
+ if (aug_system_solver_->ProvidesInertia()) {
+ num_neg_evals_ = aug_system_solver_->NumberOfNegEVals();
+ }
+ if (retval != SYMSOLVER_SUCCESS) {
+ return retval;
+ }
+
+ // Pack the results into Vtilde
+ if (IsNull(compound_sol_vecspace_)) {
+ Index dimx = proto_rhs_x.Dim();
+ Index dims = proto_rhs_s.Dim();
+ Index dimc = proto_rhs_c.Dim();
+ Index dimd = proto_rhs_d.Dim();
+ Index dimtot = dimx+dims+dimc+dimd;
+ SmartPtr<CompoundVectorSpace> vecspace =
+ new CompoundVectorSpace(4, dimtot);
+ vecspace->SetCompSpace(0, *proto_rhs_x.OwnerSpace());
+ vecspace->SetCompSpace(1, *proto_rhs_s.OwnerSpace());
+ vecspace->SetCompSpace(2, *proto_rhs_c.OwnerSpace());
+ vecspace->SetCompSpace(3, *proto_rhs_d.OwnerSpace());
+ compound_sol_vecspace_ = ConstPtr(vecspace);
+ }
+ SmartPtr<MultiVectorMatrixSpace> V1space =
+ new MultiVectorMatrixSpace(nrhs, *compound_sol_vecspace_);
+ Vtilde = V1space->MakeNewMultiVectorMatrix();
+ Vtilde_x = V_xspace->MakeNewMultiVectorMatrix();
+ for (Index i=0; i<nrhs; i++) {
+ Vtilde_x->SetVector(i, *sol_xV[i]);
+ SmartPtr<CompoundVector> cvec =
+ compound_sol_vecspace_->MakeNewCompoundVector(false);
+ cvec->SetCompNonConst(0, *sol_xV[i]);
+ cvec->SetCompNonConst(1, *sol_sV[i]);
+ cvec->SetCompNonConst(2, *sol_cV[i]);
+ cvec->SetCompNonConst(3, *sol_dV[i]);
+ Vtilde->SetVectorNonConst(i, *cvec);
+ }
+
+ return retval;
+ }
+
+ bool LowRankAugSystemSolver::AugmentedSystemRequiresChange(
+ const SymMatrix* W,
+ Number W_factor,
+ const Vector* D_x,
+ Number delta_x,
+ const Vector* D_s,
+ Number delta_s,
+ const Matrix& J_c,
+ const Vector* D_c,
+ Number delta_c,
+ const Matrix& J_d,
+ const Vector* D_d,
+ Number delta_d)
+ {
+ DBG_START_METH("LowRankAugSystemSolver::AugmentedSystemRequiresChange",
+ dbg_verbosity);
+
+#ifdef IP_DEBUG
+
+ bool Wtest = (W && W->GetTag() != w_tag_);
+ bool iWtest = (!W && w_tag_ != 0);
+ bool wfactor_test = (W_factor != w_factor_);
+ bool D_xtest = (D_x && D_x->GetTag() != d_x_tag_);
+ bool iD_xtest = (!D_x && d_x_tag_ != 0);
+ bool delta_xtest = (delta_x != delta_x_);
+ bool D_stest = (D_s && D_s->GetTag() != d_s_tag_);
+ bool iD_stest = (!D_s && d_s_tag_ != 0);
+ bool delta_stest = (delta_s != delta_s_);
+ bool J_ctest = (J_c.GetTag() != j_c_tag_);
+ bool D_ctest = (D_c && D_c->GetTag() != d_c_tag_);
+ bool iD_ctest = (!D_c && d_c_tag_ != 0);
+ bool delta_ctest = (delta_c != delta_c_);
+ bool J_dtest = (J_d.GetTag() != j_d_tag_);
+ bool D_dtest = (D_d && D_d->GetTag() != d_d_tag_);
+ bool iD_dtest = (!D_d && d_d_tag_ != 0);
+ bool delta_dtest = (delta_d != delta_d_);
+#endif
+
+ DBG_PRINT((2,"Wtest = %d\n", Wtest));
+ DBG_PRINT((2,"iWtest = %d\n", iWtest));
+ DBG_PRINT((2,"wfactor_test = %d\n", wfactor_test));
+ DBG_PRINT((2,"D_xtest = %d\n", D_xtest));
+ DBG_PRINT((2,"iD_xtest = %d\n", iD_xtest));
+ DBG_PRINT((2,"delta_xtest = %d\n", delta_xtest));
+ DBG_PRINT((2,"D_stest = %d\n", D_stest));
+ DBG_PRINT((2,"iD_stest = %d\n", iD_stest));
+ DBG_PRINT((2,"delta_stest = %d\n", delta_stest));
+ DBG_PRINT((2,"J_ctest = %d\n", J_ctest));
+ DBG_PRINT((2,"D_ctest = %d\n", D_ctest));
+ DBG_PRINT((2,"iD_ctest = %d\n", iD_ctest));
+ DBG_PRINT((2,"delta_ctest = %d\n", delta_ctest));
+ DBG_PRINT((2,"J_dtest = %d\n", J_dtest));
+ DBG_PRINT((2,"D_dtest = %d\n", D_dtest));
+ DBG_PRINT((2,"iD_dtest = %d\n", iD_dtest));
+ DBG_PRINT((2,"delta_dtest = %d\n", delta_dtest));
+
+ if ( (W && W->GetTag() != w_tag_)
+ || (!W && w_tag_ != 0)
+ || (W_factor != w_factor_)
+ || (D_x && D_x->GetTag() != d_x_tag_)
+ || (!D_x && d_x_tag_ != 0)
+ || (delta_x != delta_x_)
+ || (D_s && D_s->GetTag() != d_s_tag_)
+ || (!D_s && d_s_tag_ != 0)
+ || (delta_s != delta_s_)
+ || (J_c.GetTag() != j_c_tag_)
+ || (D_c && D_c->GetTag() != d_c_tag_)
+ || (!D_c && d_c_tag_ != 0)
+ || (delta_c != delta_c_)
+ || (J_d.GetTag() != j_d_tag_)
+ || (D_d && D_d->GetTag() != d_d_tag_)
+ || (!D_d && d_d_tag_ != 0)
+ || (delta_d != delta_d_) ) {
+ return true;
+ }
+
+ return false;
+ }
+
+ Index LowRankAugSystemSolver::NumberOfNegEVals() const
+ {
+ DBG_ASSERT(!first_call_);
+ return num_neg_evals_;
+ }
+
+ bool LowRankAugSystemSolver::ProvidesInertia() const
+ {
+ return aug_system_solver_->ProvidesInertia();
+ }
+
+ bool LowRankAugSystemSolver::IncreaseQuality()
+ {
+ return aug_system_solver_->IncreaseQuality();
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpLowRankAugSystemSolver.hpp b/SimTKmath/Optimizers/src/IpOpt/IpLowRankAugSystemSolver.hpp
new file mode 100644
index 0000000..f37ebf6
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpLowRankAugSystemSolver.hpp
@@ -0,0 +1,255 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpLowRankAugSystemSolver.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-12-27
+
+#ifndef __IP_LOWRANKAUGSYSTEMSOLVER_HPP__
+#define __IP_LOWRANKAUGSYSTEMSOLVER_HPP__
+
+#include "IpAugSystemSolver.hpp"
+#include "IpDenseGenMatrix.hpp"
+#include "IpMultiVectorMatrix.hpp"
+#include "IpDiagMatrix.hpp"
+
+namespace Ipopt
+{
+
+ /** Solver for the augmented system with LowRankUpdateSymMatrix
+ * Hessian matrices.
+ */
+ class LowRankAugSystemSolver : public AugSystemSolver
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor using only a linear solver object */
+ LowRankAugSystemSolver(AugSystemSolver& aug_system_solver);
+
+ /** Default destructor */
+ virtual ~LowRankAugSystemSolver();
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Set up the augmented system and solve it for a given right hand
+ * side.
+ */
+ virtual ESymSolverStatus Solve(
+ const SymMatrix* W,
+ Number W_factor,
+ const Vector* D_x,
+ Number delta_x,
+ const Vector* D_s,
+ Number delta_s,
+ const Matrix* J_c,
+ const Vector* D_c,
+ Number delta_c,
+ const Matrix* J_d,
+ const Vector* D_d,
+ Number delta_d,
+ const Vector& rhs_x,
+ const Vector& rhs_s,
+ const Vector& rhs_c,
+ const Vector& rhs_d,
+ Vector& sol_x,
+ Vector& sol_s,
+ Vector& sol_c,
+ Vector& sol_d,
+ bool check_NegEVals,
+ Index numberOfNegEVals);
+
+ /** Number of negative eigenvalues detected during last
+ * solve. Returns the number of negative eigenvalues of
+ * the most recent factorized matrix. This must not be called if
+ * the linear solver does not compute this quantities (see
+ * ProvidesInertia).
+ */
+ virtual Index NumberOfNegEVals() const;
+
+ /** Query whether inertia is computed by linear solver.
+ * Returns true, if linear solver provides inertia.
+ */
+ virtual bool ProvidesInertia() const;
+
+ /** Request to increase quality of solution for next solve. Ask
+ * underlying linear solver to increase quality of solution for
+ * the next solve (e.g. increase pivot tolerance). Returns
+ * false, if this is not possible (e.g. maximal pivot tolerance
+ * already used.)
+ */
+ virtual bool IncreaseQuality();
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default constructor. */
+ LowRankAugSystemSolver();
+ /** Copy Constructor */
+ LowRankAugSystemSolver(const LowRankAugSystemSolver&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const LowRankAugSystemSolver&);
+ //@}
+
+ /** The augmented system solver object that should be used for the
+ * factorization of the augmented system without the low-rank
+ * update.
+ */
+ SmartPtr<AugSystemSolver> aug_system_solver_;
+
+ /**@name Tags and values to track in order to decide whether the
+ matrix has to be updated compared to the most recent call of
+ the Set method.
+ */
+ //@{
+ /** Tag for W matrix. If W has been given to Set as NULL, then
+ * this tag is set to 0
+ */
+ TaggedObject::Tag w_tag_;
+ /** Most recent value of W_factor */
+ double w_factor_;
+ /** Tag for D_x vector, representing the diagonal matrix D_x. If
+ * D_x has been given to Set as NULL, then this tag is set to 0
+ */
+ TaggedObject::Tag d_x_tag_;
+ /** Most recent value of delta_x from Set method */
+ double delta_x_;
+ /** Tag for D_s vector, representing the diagonal matrix D_s. If
+ * D_s has been given to Set as NULL, then this tag is set to 0
+ */
+ TaggedObject::Tag d_s_tag_;
+ /** Most recent value of delta_s from Set method */
+ double delta_s_;
+ /** Tag for J_c matrix. If J_c has been given to Set as NULL, then
+ * this tag is set to 0
+ */
+ TaggedObject::Tag j_c_tag_;
+ /** Tag for D_c vector, representing the diagonal matrix D_c. If
+ * D_c has been given to Set as NULL, then this tag is set to 0
+ */
+ TaggedObject::Tag d_c_tag_;
+ /** Most recent value of delta_c from Set method */
+ double delta_c_;
+ /** Tag for J_d matrix. If J_d has been given to Set as NULL, then
+ * this tag is set to 0
+ */
+ TaggedObject::Tag j_d_tag_;
+ /** Tag for D_d vector, representing the diagonal matrix D_d. If
+ * D_d has been given to Set as NULL, then this tag is set to 0
+ */
+ TaggedObject::Tag d_d_tag_;
+ /** Most recent value of delta_d from Set method */
+ double delta_d_;
+ //@}
+
+ /** @name Information to be stored in order to resolve for the
+ * same matrix with a different right hand side. */
+ //@{
+ bool first_call_;
+ SmartPtr<DenseGenMatrix> J1_;
+ SmartPtr<DenseGenMatrix> J2_;
+ SmartPtr<MultiVectorMatrix> Vtilde1_;
+ SmartPtr<MultiVectorMatrix> Utilde2_;
+ /** Hessian Matrix passed to the augmented system solver solving
+ * the matrix without the low-rank update. */
+ SmartPtr<DiagMatrix> Wdiag_;
+ /** Vector space for Compound vectors that capture the entire
+ * right hand side and solution vectors .*/
+ SmartPtr<const CompoundVectorSpace> compound_sol_vecspace_;
+ //@}
+
+ /** Stores the number of negative eigenvalues detected during most
+ * recent factorization. This is what is returned by
+ * NumberOfNegEVals() of this class. It usually is the number of
+ * negative eigenvalues retured from the aug_system_solver solve,
+ * but if a Cholesky factorization could not be performed, the
+ * returned value is one more than this what the
+ * aug_system_solver returned. */
+ Index num_neg_evals_;
+
+ /** @name Internal functions */
+ //@{
+ /** Method for updating the factorization, including J1_, J2_,
+ * Vtilde1_, Utilde2, Wdiag_, compound_sol_vecspace_ */
+ ESymSolverStatus UpdateFactorization(
+ const SymMatrix* W,
+ Number W_factor,
+ const Vector* D_x,
+ Number delta_x,
+ const Vector* D_s,
+ Number delta_s,
+ const Matrix& J_c,
+ const Vector* D_c,
+ Number delta_c,
+ const Matrix& J_d,
+ const Vector* D_d,
+ Number delta_d,
+ const Vector& proto_rhs_x,
+ const Vector& proto_rhs_s,
+ const Vector& proto_rhs_c,
+ const Vector& proto_rhs_d,
+ bool check_NegEVals,
+ Index numberOfNegEVals);
+
+ /** Method for solving the augmented system without low-rank
+ * update for multiple right hand sides that are provided as
+ * MultiVectorMatrix. The result is returned as a
+ * MultiVectorMatrix in Vtilde1. V_x and Vtilde1_x are V and
+ * Vtilde1 in the x-space. */
+ ESymSolverStatus SolveMultiVector(
+ const Vector* D_x,
+ Number delta_x,
+ const Vector* D_s,
+ Number delta_s,
+ const Matrix& J_c,
+ const Vector* D_c,
+ Number delta_c,
+ const Matrix& J_d,
+ const Vector* D_d,
+ Number delta_d,
+ const Vector& proto_rhs_x,
+ const Vector& proto_rhs_s,
+ const Vector& proto_rhs_c,
+ const Vector& proto_rhs_d,
+ const MultiVectorMatrix& V,
+ const SmartPtr<const Matrix>& P_LM,
+ SmartPtr<MultiVectorMatrix>& V_x,
+ SmartPtr<MultiVectorMatrix>& Vtilde1,
+ SmartPtr<MultiVectorMatrix>& Vtilde1_x,
+ bool check_NegEVals,
+ Index numberOfNegEVals);
+
+ /** Method that compares the tags of the data for the matrix with
+ * those from the previous call. Returns true, if there was a
+ * change and the factorization has to be updated. */
+ bool AugmentedSystemRequiresChange(
+ const SymMatrix* W,
+ Number W_factor,
+ const Vector* D_x,
+ Number delta_x,
+ const Vector* D_s,
+ Number delta_s,
+ const Matrix& J_c,
+ const Vector* D_c,
+ Number delta_c,
+ const Matrix& J_d,
+ const Vector* D_d,
+ Number delta_d);
+ //@}
+
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpLowRankUpdateSymMatrix.cpp b/SimTKmath/Optimizers/src/IpOpt/IpLowRankUpdateSymMatrix.cpp
new file mode 100644
index 0000000..2f6ef44
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpLowRankUpdateSymMatrix.cpp
@@ -0,0 +1,187 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpLowRankUpdateSymMatrix.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-12-25
+
+#include "IpLowRankUpdateSymMatrix.hpp"
+
+namespace Ipopt
+{
+
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ LowRankUpdateSymMatrix::LowRankUpdateSymMatrix(const LowRankUpdateSymMatrixSpace* owner_space)
+ :
+ SymMatrix(owner_space),
+ owner_space_(owner_space)
+ {}
+
+ LowRankUpdateSymMatrix::~LowRankUpdateSymMatrix()
+ {}
+
+ void LowRankUpdateSymMatrix::MultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ DBG_START_METH("LowRankUpdateSymMatrix::MultVectorImpl",
+ dbg_verbosity);
+ // A few sanity checks
+ DBG_ASSERT(Dim()==x.Dim());
+ DBG_ASSERT(Dim()==y.Dim());
+ DBG_ASSERT(IsValid(D_));
+
+ SmartPtr<const Matrix> P_LR = P_LowRank();
+ if (IsNull(P_LR)) {
+
+ // Diagonal part
+ if( beta!=0.0 ) {
+ SmartPtr<Vector> tmp_vec = x.MakeNewCopy();
+ tmp_vec->ElementWiseMultiply(*D_);
+ y.AddOneVector(alpha, *tmp_vec, beta);
+ }
+ else {
+ y.AddOneVector(alpha, x, 0.);
+ y.ElementWiseMultiply(*D_);
+ }
+ DBG_PRINT_VECTOR(2, "y = D*alpha*x", y);
+
+ if (IsValid(V_)) {
+ // Positive update
+ V_->LRMultVector(alpha, x, 1., y);
+ }
+
+ if (IsValid(U_)) {
+ // Negative update
+ U_->LRMultVector(-alpha, x, 1., y);
+ }
+ }
+ else {
+ if (ReducedDiag()) {
+ // Get everything into the smaller space
+ SmartPtr<const VectorSpace> LR_vec_space = LowRankVectorSpace();
+ SmartPtr<Vector> small_x = LR_vec_space->MakeNew();
+ P_LR->TransMultVector(1., x, 0., *small_x);
+
+ // Diagonal part in small space
+ SmartPtr<Vector> small_y = LR_vec_space->MakeNew();
+ small_y->Copy(*small_x);
+ small_y->ElementWiseMultiply(*D_);
+
+ if (IsValid(V_)) {
+ // Positive update
+ V_->LRMultVector(1., *small_x, 1., *small_y);
+ }
+
+ if (IsValid(U_)) {
+ // Negative update
+ U_->LRMultVector(-1., *small_x, 1., *small_y);
+ }
+
+ // Get the result back into the large space
+ P_LR->MultVector(alpha, *small_y, beta, y);
+ }
+ else {
+ // Diagonal part
+ SmartPtr<Vector> tmp = x.MakeNewCopy();
+ tmp->ElementWiseMultiply(*D_);
+ y.AddOneVector(alpha, *tmp, beta);
+
+ // Get x into the smaller space
+ SmartPtr<const VectorSpace> LR_vec_space = LowRankVectorSpace();
+ SmartPtr<Vector> small_x = LR_vec_space->MakeNew();
+ P_LR->TransMultVector(1., x, 0., *small_x);
+
+ SmartPtr<Vector> small_y = LR_vec_space->MakeNew();
+ if (IsValid(V_)) {
+ // Positive update
+ V_->LRMultVector(1., *small_x, 0., *small_y);
+ }
+ else {
+ small_y->Set(0.);
+ }
+
+ if (IsValid(U_)) {
+ // Negative update
+ U_->LRMultVector(-1., *small_x, 1., *small_y);
+ }
+
+ // Get the result back into the large space
+ P_LR->MultVector(alpha, *small_y, 1., y);
+ }
+ }
+ }
+
+ bool LowRankUpdateSymMatrix::HasValidNumbersImpl() const
+ {
+ DBG_ASSERT(IsValid(D_));
+ if (!D_->HasValidNumbers()) {
+ return false;
+ }
+ if (IsValid(V_)) {
+ if (!V_->HasValidNumbers()) {
+ return false;
+ }
+ }
+ if (IsValid(U_)) {
+ if (!U_->HasValidNumbers()) {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ void LowRankUpdateSymMatrix::PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ jnlst.Printf(level, category, "\n");
+ jnlst.PrintfIndented(level, category, indent,
+ "%sLowRankUpdateSymMatrix \"%s\" with %d rows and columns:\n",
+ prefix.c_str(), name.c_str(), Dim());
+
+ if (ReducedDiag()) {
+ jnlst.PrintfIndented(level, category, indent+1,
+ "%sThis matrix has reduced diagonal.\n", prefix.c_str());
+ }
+ else {
+ jnlst.PrintfIndented(level, category, indent+1,
+ "%sThis matrix has full diagonal.\n", prefix.c_str());
+ }
+ jnlst.PrintfIndented(level, category, indent+1,
+ "%sDiagonal matrix:\n", prefix.c_str());
+ if (IsValid(D_)) {
+ D_->Print(&jnlst, level, category, name+"-D", indent+1, prefix);
+ }
+ else {
+ jnlst.PrintfIndented(level, category, indent,
+ "%sDiagonal matrix not set!\n", prefix.c_str());
+ }
+
+ jnlst.PrintfIndented(level, category, indent+1,
+ "%sMultiVectorMatrix V for positive update:\n", prefix.c_str());
+ if (IsValid(V_)) {
+ V_->Print(&jnlst, level, category, name+"-V", indent+1, prefix);
+ }
+ else {
+ jnlst.PrintfIndented(level, category, indent,
+ "%sV matrix not set!\n", prefix.c_str());
+ }
+
+ jnlst.PrintfIndented(level, category, indent+1,
+ "%sMultiVectorMatrix U for positive update:\n", prefix.c_str());
+ if (IsValid(U_)) {
+ U_->Print(&jnlst, level, category, name+"-U", indent+1, prefix);
+ }
+ else {
+ jnlst.PrintfIndented(level, category, indent,
+ "%sU matrix not set!\n", prefix.c_str());
+ }
+ }
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpLowRankUpdateSymMatrix.hpp b/SimTKmath/Optimizers/src/IpOpt/IpLowRankUpdateSymMatrix.hpp
new file mode 100644
index 0000000..6f49cf9
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpLowRankUpdateSymMatrix.hpp
@@ -0,0 +1,251 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpLowRankUpdateSymMatrix.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-12-25
+
+#ifndef __IPLOWRANKUPDATESYMMATRIX_HPP__
+#define __IPLOWRANKUPDATESYMMATRIX_HPP__
+
+#include "IpUtils.hpp"
+#include "IpSymMatrix.hpp"
+#include "IpMultiVectorMatrix.hpp"
+
+namespace Ipopt
+{
+
+ /* forward declarations */
+ class LowRankUpdateSymMatrixSpace;
+
+ /** Class for symmetric matrices, represented as low-rank updates.
+ * The matrix M is represented as M = P_LR(D + V V^T - U U^T)P_LR^T
+ * (if reduced_diag is true), or M = D + P_LR(V V^T - U U^T)P_LR^T
+ * (if reduced_diag is false). D is a diagonal matrix, and V and U
+ * are MultiVectorMatrices, and P_LR is an ExpansionMatrix. The
+ * vectors in the low-rank update (before expansion) live in the
+ * LowRankVectorSpace. If P_LR is NULL, P_LR is assumed to be the
+ * identity matrix. If V or U is NULL, it is assume to be a matrix
+ * of zero columns. */
+ class LowRankUpdateSymMatrix : public SymMatrix
+ {
+ public:
+
+ /**@name Constructors / Destructors */
+ //@{
+
+ /** Constructor, given the corresponding matrix space. */
+ LowRankUpdateSymMatrix(const LowRankUpdateSymMatrixSpace* owner_space);
+
+ /** Destructor */
+ ~LowRankUpdateSymMatrix();
+ //@}
+
+ /** Method for setting the diagonal elements (as a Vector). */
+ void SetDiag(const Vector& D)
+ {
+ D_ = &D;
+ ObjectChanged();
+ }
+
+ /** Method for getting the diagonal elements. */
+ SmartPtr<const Vector> GetDiag() const
+ {
+ return D_;
+ }
+
+ /** Method for setting the positive low-rank update part. */
+ void SetV(const MultiVectorMatrix& V)
+ {
+ V_ = &V;
+ ObjectChanged();
+ }
+
+ /** Method for getting the positive low-rank update part. */
+ SmartPtr<const MultiVectorMatrix> GetV() const
+ {
+ return V_;
+ }
+
+ /** Method for setting the negative low-rank update part. */
+ void SetU(const MultiVectorMatrix& U)
+ {
+ U_ = &U;
+ ObjectChanged();
+ }
+
+ /** Method for getting the negative low-rank update part. */
+ SmartPtr<const MultiVectorMatrix> GetU() const
+ {
+ return U_;
+ }
+
+ /** Return the expansion matrix to lift the low-rank update to the
+ * higher-dimensional space. */
+ SmartPtr<const Matrix> P_LowRank() const;
+
+ /** Return the vector space in with the low-rank update vectors
+ * live. */
+ SmartPtr<const VectorSpace> LowRankVectorSpace() const;
+
+ /** Flag indicating whether the diagonal term lives in the smaller
+ * space (from P_LowRank) or in the full space. */
+ bool ReducedDiag() const;
+
+ protected:
+ /**@name Methods overloaded from matrix */
+ //@{
+ virtual void MultVectorImpl(Number alpha, const Vector& x,
+ Number beta, Vector& y) const;
+
+ /** Method for determining if all stored numbers are valid (i.e.,
+ * no Inf or Nan). */
+ virtual bool HasValidNumbersImpl() const;
+
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const;
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ LowRankUpdateSymMatrix();
+
+ /** Copy Constructor */
+ LowRankUpdateSymMatrix(const LowRankUpdateSymMatrix&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const LowRankUpdateSymMatrix&);
+ //@}
+
+ /** corresponding matrix space */
+ SmartPtr<const LowRankUpdateSymMatrixSpace> owner_space_;
+
+ /** Vector storing the diagonal matrix D. */
+ SmartPtr<const Vector> D_;
+
+ /** Vector storing the positive low-rank update. */
+ SmartPtr<const MultiVectorMatrix> V_;
+
+ /** Vector storing the negative low-rank update. */
+ SmartPtr<const MultiVectorMatrix> U_;
+ };
+
+ /** This is the matrix space for LowRankUpdateSymMatrix. */
+ class LowRankUpdateSymMatrixSpace : public SymMatrixSpace
+ {
+ public:
+ /** @name Constructors / Destructors */
+ //@{
+ /** Constructor, given the dimension of the matrix. */
+ LowRankUpdateSymMatrixSpace(Index dim,
+ SmartPtr<const Matrix> P_LowRank,
+ SmartPtr<const VectorSpace> LowRankVectorSpace,
+ bool reduced_diag)
+ :
+ SymMatrixSpace(dim),
+ P_LowRank_(P_LowRank),
+ lowrank_vector_space_(LowRankVectorSpace),
+ reduced_diag_(reduced_diag)
+ {
+ DBG_ASSERT(IsValid(lowrank_vector_space_));
+ }
+
+ /** Destructor */
+ virtual ~LowRankUpdateSymMatrixSpace()
+ {}
+ //@}
+
+ /** Overloaded MakeNew method for the SymMatrixSpace base class.
+ */
+ virtual SymMatrix* MakeNewSymMatrix() const
+ {
+ return MakeNewLowRankUpdateSymMatrix();
+ }
+
+ /** Method for creating a new matrix of this specific type. */
+ LowRankUpdateSymMatrix* MakeNewLowRankUpdateSymMatrix() const
+ {
+ return new LowRankUpdateSymMatrix(this);
+ }
+
+ SmartPtr<const Matrix> P_LowRank() const
+ {
+ return P_LowRank_;
+ }
+
+ SmartPtr<const VectorSpace> LowRankVectorSpace() const
+ {
+ return lowrank_vector_space_;
+ }
+
+ bool ReducedDiag() const
+ {
+ return reduced_diag_;
+ }
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ LowRankUpdateSymMatrixSpace();
+
+ /** Copy Constructor */
+ LowRankUpdateSymMatrixSpace(const LowRankUpdateSymMatrixSpace&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const LowRankUpdateSymMatrixSpace&);
+ //@}
+
+ /** Expansion matrix to lift the low-rank approximation into a
+ * possibly higher-dimensional space. If it is NULL, it is
+ * assume that no lift is performed. */
+ SmartPtr<const Matrix> P_LowRank_;
+
+ /** Vector space for the space in which the low-rank approximation
+ * lives. */
+ SmartPtr<const VectorSpace> lowrank_vector_space_;
+
+ /** Flag indicating whether the diagonal matrix is nonzero only in
+ * the space of V or in the full space. */
+ bool reduced_diag_;
+ };
+
+ inline
+ SmartPtr<const Matrix> LowRankUpdateSymMatrix::P_LowRank() const
+ {
+ return owner_space_->P_LowRank();
+ }
+
+ inline
+ SmartPtr<const VectorSpace> LowRankUpdateSymMatrix::LowRankVectorSpace() const
+ {
+ return owner_space_->LowRankVectorSpace();
+ }
+
+ inline
+ bool LowRankUpdateSymMatrix::ReducedDiag() const
+ {
+ return owner_space_->ReducedDiag();
+ }
+
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpMatrix.cpp b/SimTKmath/Optimizers/src/IpOpt/IpMatrix.cpp
new file mode 100644
index 0000000..0de69c8
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpMatrix.cpp
@@ -0,0 +1,104 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpMatrix.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpMatrix.hpp"
+
+namespace Ipopt
+{
+ void Matrix::MultVector(Number alpha, const Vector& x, Number beta,
+ Vector& y) const
+ {
+ MultVectorImpl(alpha, x, beta, y);
+ }
+
+ void Matrix::TransMultVector(Number alpha, const Vector& x, Number beta,
+ Vector& y) const
+ {
+ TransMultVectorImpl(alpha, x, beta, y);
+ }
+
+ void Matrix::AddMSinvZ(Number alpha, const Vector& S, const Vector& Z,
+ Vector& X) const
+ {
+ AddMSinvZImpl(alpha, S, Z, X);
+ }
+
+ void Matrix::SinvBlrmZMTdBr(Number alpha, const Vector& S,
+ const Vector& R, const Vector& Z,
+ const Vector& D, Vector& X) const
+ {
+ SinvBlrmZMTdBrImpl(alpha, S, R, Z, D, X);
+ }
+
+ // Prototype for specialize methods (can and should be overloaded)
+ void Matrix::AddMSinvZImpl(Number alpha, const Vector& S, const Vector& Z,
+ Vector& X) const
+ {
+ SmartPtr<Vector> tmp = S.MakeNew();
+ tmp->AddVectorQuotient(1., Z, S, 0.);
+ MultVector(alpha, *tmp, 1., X);
+ }
+
+ void Matrix::SinvBlrmZMTdBrImpl(Number alpha, const Vector& S,
+ const Vector& R, const Vector& Z,
+ const Vector& D, Vector& X) const
+ {
+ TransMultVector(alpha, D, 0., X);
+ X.ElementWiseMultiply(Z);
+ X.Axpy(1., R);
+ X.ElementWiseDivide(S);
+ }
+
+ bool Matrix::HasValidNumbers() const
+ {
+ if (valid_cache_tag_ != GetTag()) {
+ cached_valid_ = HasValidNumbersImpl();
+ valid_cache_tag_ = GetTag();
+ }
+ return cached_valid_;
+ }
+
+ void Matrix::Print(SmartPtr<const Journalist> jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ if (IsValid(jnlst) && jnlst->ProduceOutput(level, category)) {
+ PrintImpl(*jnlst, level, category, name, indent, prefix);
+ }
+ }
+
+ void Matrix::Print(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ if (jnlst.ProduceOutput(level, category)) {
+ PrintImpl(jnlst, level, category, name, indent, prefix);
+ }
+ }
+
+ MatrixSpace::MatrixSpace(Index nRows, Index nCols)
+ :
+ nRows_(nRows),
+ nCols_(nCols)
+ {}
+
+ MatrixSpace::~MatrixSpace()
+ {}
+
+ bool MatrixSpace::IsMatrixFromSpace(const Matrix& matrix) const
+ {
+ return (matrix.OwnerSpace() == this);
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpMatrix.hpp b/SimTKmath/Optimizers/src/IpOpt/IpMatrix.hpp
new file mode 100644
index 0000000..1117574
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpMatrix.hpp
@@ -0,0 +1,303 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpMatrix.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPMATRIX_HPP__
+#define __IPMATRIX_HPP__
+
+#include "IpVector.hpp"
+
+namespace Ipopt
+{
+
+ /* forward declarations */
+ class MatrixSpace;
+
+ /** Matrix Base Class. This is the base class for all derived matrix
+ * types. All Matrices, such as Jacobian and Hessian matrices, as
+ * well as possibly the iteration matrices needed for the step
+ * computation, are of this type.
+ *
+ * Deriving from Matrix: Overload the protected XXX_Impl method.
+ */
+ class Matrix : public TaggedObject
+ {
+ public:
+ /** @name Constructor/Destructor */
+ //@{
+ /** Constructor. It has to be given a pointer to the
+ * corresponding MatrixSpace.
+ */
+ Matrix(const MatrixSpace* owner_space);
+
+ /** Destructor */
+ virtual ~Matrix();
+ //@}
+
+ /**@name Operations of the Matrix on a Vector */
+ //@{
+ /** Matrix-vector multiply. Computes y = alpha * Matrix * x +
+ * beta * y. Do not overload. Overload MultVectorImpl instead.
+ */
+ void MultVector(Number alpha, const Vector& x, Number beta,
+ Vector& y) const;
+
+ /** Matrix(transpose) vector multiply. Computes y = alpha *
+ * Matrix^T * x + beta * y. Do not overload. Overload
+ * TransMultVectorImpl instead.
+ */
+ void TransMultVector(Number alpha, const Vector& x, Number beta,
+ Vector& y) const;
+ //@}
+
+ /** @name Methods for specialized operations. A prototype
+ * implementation is provided, but for efficient implementation
+ * those should be specially implemented.
+ */
+ //@{
+ /** X = X + alpha*(Matrix S^{-1} Z). Should be implemented
+ * efficiently for the ExansionMatrix
+ */
+ void AddMSinvZ(Number alpha, const Vector& S, const Vector& Z,
+ Vector& X) const;
+
+ /** X = S^{-1} (r + alpha*Z*M^Td). Should be implemented
+ * efficiently for the ExansionMatrix
+ */
+ void SinvBlrmZMTdBr(Number alpha, const Vector& S,
+ const Vector& R, const Vector& Z,
+ const Vector& D, Vector& X) const;
+ //@}
+
+ /** Method for determining if all stored numbers are valid (i.e.,
+ * no Inf or Nan). */
+ bool HasValidNumbers() const;
+
+ //* @name Information about the size of the matrix */
+ //@{
+ /** Number of rows */
+ Index NRows() const;
+
+ /** Number of columns */
+ Index NCols() const;
+ //@}
+
+ /** Print detailed information about the matrix. Do not overload.
+ * Overload PrintImpl instead.
+ */
+ //@{
+ virtual void Print(SmartPtr<const Journalist> jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent=0,
+ const std::string& prefix="") const;
+ virtual void Print(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent=0,
+ const std::string& prefix="") const;
+ //@}
+
+ /** Return the owner MatrixSpace*/
+ SmartPtr<const MatrixSpace> OwnerSpace() const;
+
+ protected:
+ /** @name implementation methods (derived classes MUST
+ * overload these pure virtual protected methods.
+ */
+ //@{
+ /** Matrix-vector multiply. Computes y = alpha * Matrix * x +
+ * beta * y
+ */
+ virtual void MultVectorImpl(Number alpha, const Vector& x, Number beta, Vector& y) const =0;
+
+ /** Matrix(transpose) vector multiply.
+ * Computes y = alpha * Matrix^T * x + beta * y
+ */
+ virtual void TransMultVectorImpl(Number alpha, const Vector& x, Number beta, Vector& y) const =0;
+
+ /** X = X + alpha*(Matrix S^{-1} Z). Prototype for this
+ * specialize method is provided, but for efficient
+ * implementation it should be overloaded for the expansion matrix.
+ */
+ virtual void AddMSinvZImpl(Number alpha, const Vector& S, const Vector& Z,
+ Vector& X) const;
+
+ /** X = S^{-1} (r + alpha*Z*M^Td). Should be implemented
+ * efficiently for the ExpansionMatrix.
+ */
+ virtual void SinvBlrmZMTdBrImpl(Number alpha, const Vector& S,
+ const Vector& R, const Vector& Z,
+ const Vector& D, Vector& X) const;
+
+ /** Method for determining if all stored numbers are valid (i.e.,
+ * no Inf or Nan). A default implementation always returning true
+ * is provided, but if possible it should be implemented. */
+ virtual bool HasValidNumbersImpl() const
+ {
+ return true;
+ }
+
+ /** Print detailed information about the matrix. */
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const =0;
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** default constructor */
+ Matrix();
+
+ /** Copy constructor */
+ Matrix(const Matrix&);
+
+ /** Overloaded Equals Operator */
+ Matrix& operator=(const Matrix&);
+ //@}
+
+ const SmartPtr<const MatrixSpace> owner_space_;
+
+ /**@name CachedResults data members */
+ //@{
+ mutable TaggedObject::Tag valid_cache_tag_;
+ mutable bool cached_valid_;
+ //@}
+ };
+
+
+ /** MatrixSpace base class, corresponding to the Matrix base class.
+ * For each Matrix implementation, a corresponding MatrixSpace has
+ * to be implemented. A MatrixSpace is able to create new Matrices
+ * of a specific type. The MatrixSpace should also store
+ * information that is common to all Matrices of that type. For
+ * example, the dimensions of a Matrix is stored in the MatrixSpace
+ * base class.
+ */
+ class MatrixSpace : public ReferencedObject
+ {
+ public:
+ /** @name Constructors/Destructors */
+ //@{
+ /** Constructor, given the number rows and columns of all matrices
+ * generated by this MatrixSpace.
+ */
+ MatrixSpace(Index nRows, Index nCols);
+
+ /** Destructor */
+ virtual ~MatrixSpace();
+ //@}
+
+ /** Pure virtual method for creating a new Matrix of the
+ * corresponding type.
+ */
+ virtual Matrix* MakeNew() const=0;
+
+ /** Accessor function for the number of rows. */
+ Index NRows() const
+ {
+ return nRows_;
+ }
+ /** Accessor function for the number of columns. */
+ Index NCols() const
+ {
+ return nCols_;
+ }
+
+ /** Method to test if a given matrix belongs to a particular
+ * matrix space.
+ */
+ bool IsMatrixFromSpace(const Matrix& matrix) const;
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** default constructor */
+ MatrixSpace();
+
+ /** Copy constructor */
+ MatrixSpace(const MatrixSpace&);
+
+ /** Overloaded Equals Operator */
+ MatrixSpace& operator=(const MatrixSpace&);
+ //@}
+
+ /** Number of rows for all matrices of this type. */
+ const Index nRows_;
+ /** Number of columns for all matrices of this type. */
+ const Index nCols_;
+ };
+
+
+ /* Inline Methods */
+ inline
+ Matrix::Matrix(const MatrixSpace* owner_space)
+ :
+ TaggedObject(),
+ owner_space_(owner_space),
+ valid_cache_tag_(0)
+ {}
+
+ inline
+ Matrix::~Matrix()
+ {}
+
+ inline
+ Index Matrix::NRows() const
+ {
+ return owner_space_->NRows();
+ }
+
+ inline
+ Index Matrix::NCols() const
+ {
+ return owner_space_->NCols();
+ }
+
+ inline
+ SmartPtr<const MatrixSpace> Matrix::OwnerSpace() const
+ {
+ return owner_space_;
+ }
+
+} // namespace Ipopt
+
+// Macro definitions for debugging matrices
+#ifndef IP_DEBUG
+# define DBG_PRINT_MATRIX(__verbose_level, __mat_name, __mat)
+#else
+# define DBG_PRINT_MATRIX(__verbose_level, __mat_name, __mat) \
+ if (dbg_jrnl.Verbosity() >= (__verbose_level)) { \
+ if (dbg_jrnl.Jnlst()!=NULL) { \
+ (__mat).Print(dbg_jrnl.Jnlst(), \
+ J_ERROR, J_DBG, \
+ __mat_name, \
+ dbg_jrnl.IndentationLevel()*2, \
+ "# "); \
+ } \
+ }
+#endif // IP_DEBUG
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpMonotoneMuUpdate.cpp b/SimTKmath/Optimizers/src/IpOpt/IpMonotoneMuUpdate.cpp
new file mode 100644
index 0000000..4f1dd1a
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpMonotoneMuUpdate.cpp
@@ -0,0 +1,219 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpMonotoneMuUpdate.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpMonotoneMuUpdate.hpp"
+#include "IpJournalist.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+namespace Ipopt
+{
+
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ MonotoneMuUpdate::MonotoneMuUpdate(const SmartPtr<LineSearch>& linesearch)
+ :
+ MuUpdate(),
+ linesearch_(linesearch),
+ initialized_(false)
+ {
+ DBG_START_METH("MonotoneMuUpdate::MonotoneMuUpdate",
+ dbg_verbosity);
+ DBG_ASSERT(IsValid(linesearch_));
+ }
+
+ MonotoneMuUpdate::~MonotoneMuUpdate()
+ {
+ DBG_START_METH("MonotoneMuUpdate::~MonotoneMuUpdate",
+ dbg_verbosity);
+ }
+
+ void MonotoneMuUpdate::RegisterOptions(const SmartPtr<RegisteredOptions>& roptions)
+ {
+ roptions->AddLowerBoundedNumberOption(
+ "mu_init", "Initial value for the barrier parameter.",
+ 0.0, true,
+ 0.1,
+ "This option determines the initial value for the barrier parameter "
+ "(mu). It is only relevant in the monotone, Fiacco-McCormick "
+ "version of the algorithm. (i.e., if \"mu_strategy\" is chosen "
+ "as \"monotone\")");
+ roptions->AddLowerBoundedNumberOption(
+ "barrier_tol_factor",
+ "Factor for mu in barrier stop test.",
+ 0.0, true,
+ 10.0,
+ "The convergence tolerance for each barrier problem in the monotone mode "
+ "is the value of the barrier parameter times \"barrier_tol_factor\". "
+ "This option is also used in the adaptive mu strategy during the "
+ "monotone mode. (This is kappa_epsilon in implementation paper).");
+ roptions->AddBoundedNumberOption(
+ "mu_linear_decrease_factor",
+ "Determines linear decrease rate of barrier parameter.",
+ 0.0, true, 1.0, true,
+ 0.2,
+ "For the Fiacco-McCormick update procedure the new barrier parameter mu "
+ "is obtained by taking the minimum of mu*\"mu_linear_decrease_factor\" "
+ "and mu^\"superlinear_decrease_power\". (This is kappa_mu in "
+ "implementation paper.) This option is also used in the adaptive mu "
+ "strategy during the monotone mode.");
+ roptions->AddBoundedNumberOption(
+ "mu_superlinear_decrease_power",
+ "Determines superlinear decrease rate of barrier parameter.",
+ 1.0, true, 2.0, true,
+ 1.5,
+ "For the Fiacco-McCormick update procedure the new barrier parameter mu "
+ "is obtained by taking the minimum of mu*\"mu_linear_decrease_factor\" "
+ "and mu^\"superlinear_decrease_power\". (This is theta_mu in "
+ "implementation paper.) This option is also used in the adaptive mu "
+ "strategy during the monotone mode.");
+ roptions->AddStringOption2(
+ "mu_allow_fast_monotone_decrease",
+ "Allow skipping of barrier problem if barrier test is already met.",
+ "yes",
+ "no", "Take at least one iteration per barrier problem",
+ "yes", "Allow fast decrease of mu if barrier test it met",
+ "If set to \"no\", the algorithm enforces at least one iteration per "
+ "barrier problem, even if the barrier test is already met for the "
+ "updated barrier parameter.");
+ roptions->AddBoundedNumberOption(
+ "tau_min",
+ "Lower bound on fraction-to-the-boundary parameter tau.",
+ 0.0, true, 1.0, true,
+ 0.99,
+ "(This is tau_min in implementation paper.) This option is also used "
+ "in the adaptive mu strategy during the monotone mode.");
+ }
+
+ bool MonotoneMuUpdate::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ options.GetNumericValue("mu_init", mu_init_, prefix);
+ options.GetNumericValue("barrier_tol_factor", barrier_tol_factor_, prefix);
+ options.GetNumericValue("mu_linear_decrease_factor", mu_linear_decrease_factor_, prefix);
+ options.GetNumericValue("mu_superlinear_decrease_power", mu_superlinear_decrease_power_, prefix);
+ options.GetBoolValue("mu_allow_fast_monotone_decrease", mu_allow_fast_monotone_decrease_, prefix);
+ options.GetNumericValue("tau_min", tau_min_, prefix);
+ options.GetNumericValue("compl_inf_tol", compl_inf_tol_, prefix);
+
+ IpData().Set_mu(mu_init_);
+ Number tau = Max(tau_min_, 1.0 - mu_init_);
+ IpData().Set_tau(tau);
+
+ initialized_ = false;
+
+ //TODO we need to clean up the mu-update for the restoration phase
+ if (prefix=="resto.") {
+ first_iter_resto_ = true;
+ }
+ else {
+ first_iter_resto_ = false;
+ }
+
+ return true;
+ }
+
+ bool MonotoneMuUpdate::UpdateBarrierParameter()
+ {
+ Number mu = IpData().curr_mu();
+ Number tau = IpData().curr_tau();
+
+ Number sub_problem_error = IpCq().curr_barrier_error();
+
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "Optimality Error for Barrier Sub-problem = %e\n",
+ sub_problem_error);
+ Number kappa_eps_mu = barrier_tol_factor_ * mu;
+
+ bool done = false;
+ bool tiny_step_flag = IpData().tiny_step_flag();
+ while ((sub_problem_error <= kappa_eps_mu || tiny_step_flag)
+ && !done && !first_iter_resto_) {
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ " sub_problem_error < kappa_eps * mu (%e)\n", kappa_eps_mu);
+
+ // Compute the new values for mu and tau
+ Number new_mu;
+ Number new_tau;
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "Updating mu=%25.16e and tau=%25.16e to ", mu, tau);
+ CalcNewMuAndTau(new_mu, new_tau);
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "new_mu=%25.16e and new_tau=%25.16e\n", new_mu, new_tau);
+ bool mu_changed = (mu != new_mu);
+ if (!mu_changed && tiny_step_flag) {
+ THROW_EXCEPTION(TINY_STEP_DETECTED,
+ "Problem solved to best possible numerical accuracy");
+ }
+
+ // Set the new values for mu and tau
+ IpData().Set_mu(new_mu);
+ IpData().Set_tau(new_tau);
+ mu = new_mu;
+ tau = new_tau;
+
+ // If this is the first iteration or if
+ // mu_allow_fast_monotone_decrease_ is true, we want to check if
+ // we can decrease mu even more
+ if (initialized_ && !mu_allow_fast_monotone_decrease_) {
+ done = true;
+ }
+ else if (!mu_changed) {
+ done = true;
+ }
+ else {
+ sub_problem_error = IpCq().curr_barrier_error();
+ kappa_eps_mu = barrier_tol_factor_ * mu;
+ done = (sub_problem_error > kappa_eps_mu);
+ }
+
+ // Reset the line search
+ if (done && mu_changed) {
+ linesearch_->Reset();
+ }
+
+ tiny_step_flag = false;
+ }
+
+ first_iter_resto_ = false;
+ initialized_ = true;
+
+ return true;
+ }
+
+ void MonotoneMuUpdate::CalcNewMuAndTau(Number &new_mu,
+ Number &new_tau)
+ {
+ // update the barrier parameter
+ Number mu = IpData().curr_mu();
+ Number tol = IpData().tol();
+
+ // Here we need the complementarity tolerance that is posed to the
+ // scaled problem
+ Number compl_inf_tol =
+ IpNLP().NLP_scaling()->apply_obj_scaling(compl_inf_tol_);
+
+ new_mu = Min( mu_linear_decrease_factor_*mu,
+ pow(mu, mu_superlinear_decrease_power_) );
+ new_mu = Max(new_mu, Min(tol, compl_inf_tol)/(barrier_tol_factor_+1.));
+
+ // update the fraction to the boundary parameter
+ new_tau = Max(tau_min_, 1.-new_mu);
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpMonotoneMuUpdate.hpp b/SimTKmath/Optimizers/src/IpOpt/IpMonotoneMuUpdate.hpp
new file mode 100644
index 0000000..8cab884
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpMonotoneMuUpdate.hpp
@@ -0,0 +1,99 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpMonotoneMuUpdate.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPMONOTONEMUUPDATE_HPP__
+#define __IPMONOTONEMUUPDATE_HPP__
+
+#include "IpMuUpdate.hpp"
+#include "IpLineSearch.hpp"
+#include "IpRegOptions.hpp"
+
+namespace Ipopt
+{
+
+ /** Monotone Mu Update. This class implements the standard monotone mu update
+ * approach.
+ */
+ class MonotoneMuUpdate : public MuUpdate
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor */
+ MonotoneMuUpdate(const SmartPtr<LineSearch>& linesearch);
+
+ /** Default destructor */
+ virtual ~MonotoneMuUpdate();
+ //@}
+
+ /** Initialize method - overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Method for determining the barrier parameter for the next
+ * iteration. When the optimality error for the current barrier
+ * parameter is less than a tolerance, the barrier parameter is
+ * reduced, and the Reset method of the LineSearch object
+ * linesearch is called. */
+ virtual bool UpdateBarrierParameter();
+
+ /** Methods for IpoptType */
+ //@{
+ static void RegisterOptions(const SmartPtr<RegisteredOptions>& roptions);
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ MonotoneMuUpdate();
+
+ /** Copy Constructor */
+ MonotoneMuUpdate(const MonotoneMuUpdate&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const MonotoneMuUpdate&);
+ //@}
+
+ /** Internal method for computing the new values for mu and tau */
+ void CalcNewMuAndTau(Number &new_mu,
+ Number &new_tau);
+
+ /** @name Algorithmic parameters */
+ //@{
+ /** Initial value of the barrier parameter */
+ Number mu_init_;
+ Number barrier_tol_factor_;
+ Number mu_linear_decrease_factor_;
+ Number mu_superlinear_decrease_power_;
+ bool mu_allow_fast_monotone_decrease_;
+ /** Tau_min for fraction to boundary rule */
+ Number tau_min_;
+ Number compl_inf_tol_;
+ //@}
+
+ SmartPtr<LineSearch> linesearch_;
+
+ /** Flag indicating whether the method has been called at least once so
+ * far */
+ bool initialized_;
+
+ /** If true, no modification of the barrier parameter will be done
+ * at the first call of Update (fix for the restoration phase -
+ * we should clean that up!) */
+ bool first_iter_resto_;
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpMuOracle.hpp b/SimTKmath/Optimizers/src/IpOpt/IpMuOracle.hpp
new file mode 100644
index 0000000..e384aae
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpMuOracle.hpp
@@ -0,0 +1,71 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpMuOracle.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPMUORACLE_HPP__
+#define __IPMUORACLE_HPP__
+
+#include "IpAlgStrategy.hpp"
+
+namespace Ipopt
+{
+
+ /** Abstract Base Class for classes that are able to compute a
+ * suggested value of the barrier parameter that can be used
+ * as an oracle in the NonmontoneMuUpdate class.
+ */
+ class MuOracle : public AlgorithmStrategyObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor */
+ MuOracle()
+ {}
+ ;
+ /** Default destructor */
+ virtual ~MuOracle()
+ {}
+ ;
+ //@}
+
+ /** Initialize method - overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix) = 0;
+
+ /** Method for computing the value of the barrier parameter that
+ * could be used in the current iteration. Here, mu_min and
+ * mu_max are the lower and upper bounds on acceptable values for
+ * the barrier parameter. The new value of mu is returned in
+ * new_mu, and the method returns false if a new value could not
+ * be determined (e.g., because the linear system could not be
+ * solved for a predictor step).
+ */
+ virtual bool CalculateMu(Number mu_min, Number mu_max, Number& new_mu) = 0;
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+
+ /** Copy Constructor */
+ MuOracle(const MuOracle&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const MuOracle&);
+ //@}
+
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpMuUpdate.hpp b/SimTKmath/Optimizers/src/IpOpt/IpMuUpdate.hpp
new file mode 100644
index 0000000..84e5f30
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpMuUpdate.hpp
@@ -0,0 +1,69 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpMuUpdate.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPMUUPDATE_HPP__
+#define __IPMUUPDATE_HPP__
+
+#include "IpAlgStrategy.hpp"
+
+namespace Ipopt
+{
+ /** Abstract Base Class for classes that implement methods for computing
+ * the barrier and fraction-to-the-boundary rule parameter for the
+ * current iteration.
+ */
+ class MuUpdate : public AlgorithmStrategyObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor */
+ MuUpdate()
+ {}
+
+ /** Default destructor */
+ virtual ~MuUpdate()
+ {}
+ //@}
+
+ /** Initialize method - overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix) = 0;
+
+ /** Method for determining the barrier parameter for the next
+ * iteration. A LineSearch object is passed, so that this method
+ * can call the Reset method in the LineSearch object, for
+ * example when then barrier parameter is changed. This method is
+ * also responsible for setting the fraction-to-the-boundary
+ * parameter tau. This method returns false if the update could
+ * not be performed and the algorithm should revert to an
+ * emergency fallback mechanism. */
+ virtual bool UpdateBarrierParameter() = 0;
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+
+ /** Copy Constructor */
+ MuUpdate(const MuUpdate&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const MuUpdate&);
+ //@}
+
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpMultiVectorMatrix.cpp b/SimTKmath/Optimizers/src/IpOpt/IpMultiVectorMatrix.cpp
new file mode 100644
index 0000000..a4dc44b
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpMultiVectorMatrix.cpp
@@ -0,0 +1,283 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpMultiVectorMatrix.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-12-24
+
+#include "IpMultiVectorMatrix.hpp"
+#include "IpDenseVector.hpp"
+#include "IpDenseGenMatrix.hpp"
+
+#include <cstdio>
+
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+
+
+namespace Ipopt
+{
+
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ MultiVectorMatrix::MultiVectorMatrix(const MultiVectorMatrixSpace* owner_space)
+ :
+ Matrix(owner_space),
+ owner_space_(owner_space),
+ const_vecs_(owner_space->NCols()),
+ non_const_vecs_(owner_space->NCols())
+ {}
+
+ void MultiVectorMatrix::SetVector(Index i, const Vector& vec)
+ {
+ DBG_ASSERT(i<NCols());
+ non_const_vecs_[i] = NULL;
+ const_vecs_[i] = &vec;
+ ObjectChanged();
+ }
+
+ void MultiVectorMatrix::SetVectorNonConst(Index i, Vector& vec)
+ {
+ DBG_ASSERT(i<NCols());
+ const_vecs_[i] = NULL;
+ non_const_vecs_[i] = &vec;
+ ObjectChanged();
+ }
+
+ void MultiVectorMatrix::MultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ // A few sanity checks
+ DBG_ASSERT(NCols()==x.Dim());
+ DBG_ASSERT(NRows()==y.Dim());
+
+ // Take care of the y part of the addition
+ if( beta!=0.0 ) {
+ y.Scal(beta);
+ }
+ else {
+ y.Set(0.0); // In case y hasn't been initialized yet
+ }
+
+ // See if we can understand the data
+ const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x);
+ DBG_ASSERT(dense_x);
+
+ // We simply add all the Vectors one after the other
+ if (dense_x->IsHomogeneous()) {
+ Number val = dense_x->Scalar();
+ for (Index i=0; i<NCols(); i++) {
+ y.AddOneVector(alpha*val, *ConstVec(i), 1.);
+ }
+ }
+ else {
+ const Number* values = dense_x->Values();
+ for (Index i=0; i<NCols(); i++) {
+ y.AddOneVector(alpha*values[i], *ConstVec(i), 1.);
+ }
+ }
+ }
+
+ void MultiVectorMatrix::TransMultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ // A few sanity checks
+ DBG_ASSERT(NCols()==y.Dim());
+ DBG_ASSERT(NRows()==x.Dim());
+
+ // See if we can understand the data
+ DenseVector* dense_y = dynamic_cast<DenseVector*>(&y);
+ DBG_ASSERT(dense_y);
+
+ // Use the individual dot products to get the matrix (transpose)
+ // vector product
+ Number *yvals=dense_y->Values();
+ if( beta!=0.0 ) {
+ for (Index i=0; i<NCols(); i++) {
+ yvals[i] = alpha*ConstVec(i)->Dot(x) + beta*yvals[i];
+ }
+ }
+ else {
+ for (Index i=0; i<NCols(); i++) {
+ yvals[i] = alpha*ConstVec(i)->Dot(x);
+ }
+ }
+ }
+
+ void MultiVectorMatrix::LRMultVector(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ DBG_START_METH("MultiVectorMatrix::LRMultVector(",
+ dbg_verbosity);
+
+ DBG_ASSERT(NRows()==x.Dim());
+ DBG_ASSERT(NRows()==y.Dim());
+
+ DBG_PRINT((1, "alpha = %e beta = %e\n", alpha, beta));
+ DBG_PRINT_VECTOR(2, "x", x);
+
+ if( beta!=0.0 ) {
+ y.Scal(beta);
+ }
+ else {
+ y.Set(0.0);
+ }
+
+ DBG_PRINT_VECTOR(2, "beta*y", y);
+ for (Index i=0; i<NCols(); i++) {
+ DBG_PRINT_VECTOR(2, "ConstVec(i)", *ConstVec(i));
+ y.AddOneVector(alpha*ConstVec(i)->Dot(x), *ConstVec(i), 1.);
+ DBG_PRINT_VECTOR(2, "y mid", y);
+ }
+ }
+
+ void MultiVectorMatrix::FillWithNewVectors()
+ {
+ SmartPtr<const VectorSpace> vec_space = owner_space_->ColVectorSpace();
+ for (Index i=0; i<NCols(); i++) {
+ non_const_vecs_[i] = vec_space->MakeNew();
+ const_vecs_[i] = NULL;
+ }
+ ObjectChanged();
+ }
+
+ void MultiVectorMatrix::ScaleRows(const Vector& scal_vec)
+ {
+ // Santiy checks
+ DBG_ASSERT(scal_vec.Dim() == NRows());
+
+ for (Index i=0; i<NCols(); i++) {
+ Vec(i)->ElementWiseMultiply(scal_vec);
+ }
+ ObjectChanged();
+ }
+
+ void MultiVectorMatrix::ScaleColumns(const Vector& scal_vec)
+ {
+ // Santiy checks
+ DBG_ASSERT(scal_vec.Dim() == NCols());
+
+ // See if we can understand the data
+ const DenseVector* dense_scal_vec =
+ dynamic_cast<const DenseVector*>(&scal_vec);
+ DBG_ASSERT(dense_scal_vec);
+
+ if (dense_scal_vec->IsHomogeneous()) {
+ Number val = dense_scal_vec->Scalar();
+ for (Index i=0; i<NCols(); i++) {
+ Vec(i)->Scal(val);
+ }
+ }
+ else {
+ const Number* values = dense_scal_vec->Values();
+ for (Index i=0; i<NCols(); i++) {
+ Vec(i)->Scal(values[i]);
+ }
+ }
+ ObjectChanged();
+ }
+
+ void
+ MultiVectorMatrix::AddOneMultiVectorMatrix(Number a,
+ const MultiVectorMatrix& mv1,
+ Number c)
+ {
+ DBG_ASSERT(NRows()==mv1.NRows());
+ DBG_ASSERT(NCols()==mv1.NCols());
+
+ if (c==0.) {
+ FillWithNewVectors();
+ }
+
+ for (Index i=0; i<NCols(); i++) {
+ Vec(i)->AddOneVector(a, *mv1.GetVector(i), c);
+ }
+ ObjectChanged();
+ }
+
+ void
+ MultiVectorMatrix::AddRightMultMatrix(Number a,
+ const MultiVectorMatrix& U,
+ const Matrix& C,
+ Number b)
+ {
+ DBG_ASSERT(NRows()==U.NRows());
+ DBG_ASSERT(U.NCols()==C.NRows());
+ DBG_ASSERT(NCols()==C.NCols());
+
+ if (b==0.) {
+ FillWithNewVectors();
+ }
+
+ // ToDo: For now, we simply use MatrixVector multiplications, but
+ // we might be more efficient (at least in the non-parallel case)
+ // if we used Level 3 Blas
+ SmartPtr<const DenseVectorSpace> mydspace = new DenseVectorSpace(C.NRows());
+ SmartPtr<DenseVector> mydvec = mydspace->MakeNewDenseVector();
+
+ const DenseGenMatrix* dgm_C = dynamic_cast<const DenseGenMatrix*>(&C);
+ DBG_ASSERT(dgm_C);
+ for (Index i=0; i<NCols(); i++) {
+ const Number* CValues = dgm_C->Values();
+ Number* myvalues = mydvec->Values();
+ for (Index j=0; j<U.NCols(); j++) {
+ myvalues[j] = CValues[i*C.NRows() + j];
+ }
+ U.MultVector(a, *mydvec, b, *Vec(i));
+ }
+ ObjectChanged();
+ }
+
+ bool MultiVectorMatrix::HasValidNumbersImpl() const
+ {
+ for (Index i=0; i<NCols(); i++) {
+ if (!ConstVec(i)->HasValidNumbers()) {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ void MultiVectorMatrix::PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ jnlst.Printf(level, category, "\n");
+ jnlst.PrintfIndented(level, category, indent,
+ "%sMultiVectorMatrix \"%s\" with %d columns:\n",
+ prefix.c_str(), name.c_str(), NCols());
+
+ for (Index i=0; i<NCols(); i++) {
+ if (ConstVec(i)) {
+ DBG_ASSERT(name.size()<200);
+ char buffer[256];
+ sprintf(buffer, "%s[%2d]", name.c_str(), i);
+ std::string term_name = buffer;
+ ConstVec(i)->Print(&jnlst, level, category, term_name,
+ indent+1, prefix);
+ }
+ else {
+ jnlst.PrintfIndented(level, category, indent,
+ "%sVector in column %d is not yet set!\n",
+ prefix.c_str(), i);
+ }
+ }
+ }
+
+ MultiVectorMatrixSpace::MultiVectorMatrixSpace(Index ncols,
+ const VectorSpace& vec_space)
+ :
+ MatrixSpace(vec_space.Dim(), ncols),
+ vec_space_(&vec_space)
+ {}
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpMultiVectorMatrix.hpp b/SimTKmath/Optimizers/src/IpOpt/IpMultiVectorMatrix.hpp
new file mode 100644
index 0000000..885f35b
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpMultiVectorMatrix.hpp
@@ -0,0 +1,244 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpMultiVectorMatrix.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-12-24
+
+#ifndef __IPMULTIVECTORMATRIX_HPP__
+#define __IPMULTIVECTORMATRIX_HPP__
+
+#include "IpUtils.hpp"
+#include "IpMatrix.hpp"
+
+namespace Ipopt
+{
+
+ /** forward declarations */
+ class MultiVectorMatrixSpace;
+
+ /** Class for Matrices with few columns that consists of Vectors.
+ * Those matrices are for example useful in the implementation of
+ * limited memory quasi-Newton methods.
+ */
+ class MultiVectorMatrix : public Matrix
+ {
+ public:
+
+ /**@name Constructors / Destructors */
+ //@{
+
+ /** Constructor, taking the owner_space.
+ */
+ MultiVectorMatrix(const MultiVectorMatrixSpace* owner_space);
+
+ /** Destructor */
+ ~MultiVectorMatrix();
+ //@}
+
+ /** Create a new MultiVectorMatrix from same MatrixSpace */
+ SmartPtr<MultiVectorMatrix> MakeNewMultiVectorMatrix() const;
+
+ /** Set a particular Vector at a given column position, replacing
+ * another vector if there has been one. Depending on whether
+ * the Vector is const or not, it is stored in the const or
+ * non-const internal column. */
+ //@{
+ void SetVector(Index i, const Vector& vec);
+ /* For the non-const version, keep in mind that operations that
+ * change this matrix also change the Vector that has been given
+ * here. */
+ void SetVectorNonConst(Index i, Vector& vec);
+ //@}
+
+ /** Get a Vector in a particular column as a const Vector */
+ inline SmartPtr<const Vector> GetVector(Index i) const
+ {
+ return ConstVec(i);
+ }
+
+ /** Get a Vector in a particular column as a non-const
+ * Vector. This is fail if the column has currently only a
+ * non-const Vector stored. */
+ inline SmartPtr<Vector> GetVectorNonConst(Index i)
+ {
+ ObjectChanged();
+ return Vec(i);
+ }
+
+ /** Method for scaling the rows of the matrix, using the
+ * ElementWiseMultiply method for each column vector. */
+ void ScaleRows(const Vector& scal_vec);
+
+ /** Method for scaling the columns of the matrix, using the Scal
+ * method for each column vector. */
+ void ScaleColumns(const Vector& scal_vec);
+
+ /** Adding another MultiVectorMatrix, using the AddOneVector
+ * methods for the individual column vectors */
+ void AddOneMultiVectorMatrix(Number a, const MultiVectorMatrix& mv1,
+ Number c);
+
+ /** Multiplying a Matrix C (for now assumed to be a
+ * DenseGenMatrix) from the right to a MultiVectorMatrix U and
+ * adding the result to this MultiVectorMatrix V. V = a * U * C +
+ * b * V. */
+ void AddRightMultMatrix(Number a, const MultiVectorMatrix& U,
+ const Matrix& C, Number b);
+
+ /** Method for initializing all Vectors with new (uninitialized)
+ * Vectors. */
+ void FillWithNewVectors();
+
+ /** Method for adding the low-rank update matrix corresponding to
+ * this matrix to a vector. If V is this MultiVectorMatrix, the
+ * operation is y = beta*y + alpha*V*V^T*x. */
+ void LRMultVector(Number alpha, const Vector &x,
+ Number beta, Vector &y) const;
+
+ /** Vector space for the columns */
+ SmartPtr<const VectorSpace> ColVectorSpace() const;
+
+ /** Return the MultiVectorMatrixSpace */
+ SmartPtr<const MultiVectorMatrixSpace> MultiVectorMatrixOwnerSpace() const;
+
+ protected:
+ /**@name Overloaded methods from Matrix base class */
+ //@{
+ virtual void MultVectorImpl(Number alpha, const Vector &x, Number beta,
+ Vector &y) const;
+
+ virtual void TransMultVectorImpl(Number alpha, const Vector& x,
+ Number beta, Vector& y) const;
+
+ /** Method for determining if all stored numbers are valid (i.e.,
+ * no Inf or Nan). */
+ virtual bool HasValidNumbersImpl() const;
+
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const;
+ //@}
+
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ MultiVectorMatrix();
+
+ /** Copy Constructor */
+ MultiVectorMatrix(const MultiVectorMatrix&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const MultiVectorMatrix&);
+ //@}
+
+ const MultiVectorMatrixSpace* owner_space_;
+
+ /** space for storing the const Vector's */
+ std::vector<SmartPtr<const Vector> > const_vecs_;
+
+ /** space for storing the non-const Vector's */
+ std::vector<SmartPtr<Vector> > non_const_vecs_;
+
+ /** Method for accessing the internal Vectors internally */
+ //@{
+ inline const Vector* ConstVec(Index i) const
+ {
+ DBG_ASSERT(i < NCols());
+ DBG_ASSERT(IsValid(const_vecs_[i]) || IsValid(non_const_vecs_[i]));
+ if (IsValid(non_const_vecs_[i])) {
+ return GetRawPtr(non_const_vecs_[i]);
+ }
+ else {
+ return GetRawPtr(const_vecs_[i]);
+ }
+ }
+
+ inline Vector* Vec(Index i)
+ {
+ DBG_ASSERT(i < NCols());
+ DBG_ASSERT(IsValid(non_const_vecs_[i]));
+ return GetRawPtr(non_const_vecs_[i]);
+ }
+ //@}
+ };
+
+ /** This is the matrix space for MultiVectorMatrix.
+ */
+ class MultiVectorMatrixSpace : public MatrixSpace
+ {
+ public:
+ /** @name Constructors / Destructors */
+ //@{
+ /** Constructor, given the number of columns (i.e., Vectors to be
+ * stored) and given the VectorSpace for the Vectors.
+ */
+ MultiVectorMatrixSpace(Index ncols,
+ const VectorSpace& vec_space);
+
+ /** Destructor */
+ ~MultiVectorMatrixSpace()
+ {}
+ //@}
+
+ /** Method for creating a new matrix of this specific type. */
+ MultiVectorMatrix* MakeNewMultiVectorMatrix() const
+ {
+ return new MultiVectorMatrix(this);
+ }
+
+ /** Overloaded MakeNew method for the MatrixSpace base class.
+ */
+ virtual Matrix* MakeNew() const
+ {
+ return MakeNewMultiVectorMatrix();
+ }
+
+ /** Accessor method for the VectorSpace for the columns */
+ SmartPtr<const VectorSpace> ColVectorSpace() const
+ {
+ return vec_space_;
+ }
+
+ private:
+ SmartPtr<const VectorSpace> vec_space_;
+
+ };
+
+ inline
+ MultiVectorMatrix::~MultiVectorMatrix()
+ {}
+
+ inline
+ SmartPtr<MultiVectorMatrix> MultiVectorMatrix::MakeNewMultiVectorMatrix() const
+ {
+ return owner_space_->MakeNewMultiVectorMatrix();
+ }
+
+ inline
+ SmartPtr<const VectorSpace> MultiVectorMatrix::ColVectorSpace() const
+ {
+ return owner_space_->ColVectorSpace();
+ }
+
+ inline
+ SmartPtr<const MultiVectorMatrixSpace>
+ MultiVectorMatrix::MultiVectorMatrixOwnerSpace() const
+ {
+ return owner_space_;
+ }
+
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpNLP.hpp b/SimTKmath/Optimizers/src/IpOpt/IpNLP.hpp
new file mode 100644
index 0000000..c98aabf
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpNLP.hpp
@@ -0,0 +1,240 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpNLP.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPNLP_HPP__
+#define __IPNLP_HPP__
+
+#include "IpUtils.hpp"
+#include "IpVector.hpp"
+#include "IpSmartPtr.hpp"
+#include "IpMatrix.hpp"
+#include "IpSymMatrix.hpp"
+#include "IpOptionsList.hpp"
+#include "IpAlgTypes.hpp"
+#include "IpReturnCodes.hpp"
+
+namespace Ipopt
+{
+ // forward declarations
+ class IpoptData;
+ class IpoptCalculatedQuantities;
+ class IteratesVector;
+
+ /** Brief Class Description.
+ * Detailed Class Description.
+ */
+ class NLP : public ReferencedObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default constructor */
+ NLP()
+ {}
+
+ /** Default destructor */
+ virtual ~NLP()
+ {}
+ //@}
+
+ /** Exceptions */
+ //@{
+ DECLARE_STD_EXCEPTION(USER_SCALING_NOT_IMPLEMENTED);
+ //@}
+
+ /** @name NLP Initialization (overload in
+ * derived classes).*/
+ //@{
+ /** Overload if you want the chance to process options or parameters that
+ * may be specific to the NLP */
+ virtual bool ProcessOptions(const OptionsList& options,
+ const std::string& prefix)
+ {
+ return true;
+ }
+
+ /** Method for creating the derived vector / matrix types. The
+ * Hess_lagrangian_space pointer can be NULL if a quasi-Newton
+ * options is chosen. */
+ virtual bool GetSpaces(SmartPtr<const VectorSpace>& x_space,
+ SmartPtr<const VectorSpace>& c_space,
+ SmartPtr<const VectorSpace>& d_space,
+ SmartPtr<const VectorSpace>& x_l_space,
+ SmartPtr<const MatrixSpace>& px_l_space,
+ SmartPtr<const VectorSpace>& x_u_space,
+ SmartPtr<const MatrixSpace>& px_u_space,
+ SmartPtr<const VectorSpace>& d_l_space,
+ SmartPtr<const MatrixSpace>& pd_l_space,
+ SmartPtr<const VectorSpace>& d_u_space,
+ SmartPtr<const MatrixSpace>& pd_u_space,
+ SmartPtr<const MatrixSpace>& Jac_c_space,
+ SmartPtr<const MatrixSpace>& Jac_d_space,
+ SmartPtr<const SymMatrixSpace>& Hess_lagrangian_space)=0;
+
+ /** Method for obtaining the bounds information */
+ virtual bool GetBoundsInformation(const Matrix& Px_L,
+ Vector& x_L,
+ const Matrix& Px_U,
+ Vector& x_U,
+ const Matrix& Pd_L,
+ Vector& d_L,
+ const Matrix& Pd_U,
+ Vector& d_U)=0;
+
+ /** Method for obtaining the starting point for all the
+ * iterates. ToDo it might not make sense to ask for initial
+ * values for v_L and v_U? */
+ virtual bool GetStartingPoint(
+ SmartPtr<Vector> x,
+ bool need_x,
+ SmartPtr<Vector> y_c,
+ bool need_y_c,
+ SmartPtr<Vector> y_d,
+ bool need_y_d,
+ SmartPtr<Vector> z_L,
+ bool need_z_L,
+ SmartPtr<Vector> z_U,
+ bool need_z_U
+ )=0;
+
+ /** Method for obtaining an entire iterate as a warmstart point.
+ * The incoming IteratesVector has to be filled. The default
+ * dummy implementation returns false. */
+ virtual bool GetWarmStartIterate(IteratesVector& warm_start_iterate)
+ {
+ return false;
+ }
+ //@}
+
+ /** @name NLP evaluation routines (overload
+ * in derived classes. */
+ //@{
+ virtual bool Eval_f(const Vector& x, Number& f) = 0;
+
+ virtual bool Eval_grad_f(const Vector& x, Vector& g_f) = 0;
+
+ virtual bool Eval_c(const Vector& x, Vector& c) = 0;
+
+ virtual bool Eval_jac_c(const Vector& x, Matrix& jac_c) = 0;
+
+ virtual bool Eval_d(const Vector& x, Vector& d) = 0;
+
+ virtual bool Eval_jac_d(const Vector& x, Matrix& jac_d) = 0;
+
+ virtual bool Eval_h(const Vector& x,
+ Number obj_factor,
+ const Vector& yc,
+ const Vector& yd,
+ SymMatrix& h) = 0;
+ //@}
+
+ /** @name NLP solution routines. Have default dummy
+ * implementations that can be overloaded. */
+ //@{
+ /** This method is called at the very end of the optimization. It
+ * provides the final iterate to the user, so that it can be
+ * stored as the solution. The status flag indicates the outcome
+ * of the optimization, where SolverReturn is defined in
+ * IpAlgTypes.hpp. */
+ virtual void FinalizeSolution(SolverReturn status,
+ const Vector& x, const Vector& z_L,
+ const Vector& z_U,
+ const Vector& c, const Vector& d,
+ const Vector& y_c, const Vector& y_d,
+ Number obj_value)
+ {}
+
+ /** This method is called once per iteration, after the iteration
+ * summary output has been printed. It provides the current
+ * information to the user to do with it anything she wants. It
+ * also allows the user to ask for a premature termination of the
+ * optimization by returning false, in which case Ipopt will
+ * terminate with a corresponding return status. The basic
+ * information provided in the argument list has the quantities
+ * values printed in the iteration summary line. If more
+ * information is required, a user can obtain it from the IpData
+ * and IpCalculatedQuantities objects. However, note that the
+ * provided quantities are all for the problem that Ipopt sees,
+ * i.e., the quantities might be scaled, fixed variables might be
+ * sorted out, etc. The status indicates things like whether the
+ * algorithm is in the restoration phase... In the restoration
+ * phase, the dual variables are probably not not changing. */
+ virtual bool IntermediateCallBack(AlgorithmMode mode,
+ Index iter, Number obj_value,
+ Number inf_pr, Number inf_du,
+ Number mu, Number d_norm,
+ Number regularization_size,
+ Number alpha_du, Number alpha_pr,
+ Index ls_trials,
+ const IpoptData* ip_data,
+ IpoptCalculatedQuantities* ip_cq)
+ {
+ return true;
+ }
+ //@}
+
+ /** Routines to get the scaling parameters. These do not need to
+ * be overloaded unless the options are set for User scaling
+ */
+ //@{
+ virtual void GetScalingParameters(
+ const SmartPtr<const VectorSpace> x_space,
+ const SmartPtr<const VectorSpace> c_space,
+ const SmartPtr<const VectorSpace> d_space,
+ Number& obj_scaling,
+ SmartPtr<Vector>& x_scaling,
+ SmartPtr<Vector>& c_scaling,
+ SmartPtr<Vector>& d_scaling) const
+ {
+ THROW_EXCEPTION(USER_SCALING_NOT_IMPLEMENTED,
+ "You have set options for user provided scaling, but have"
+ " not implemented GetScalingParameters in the NLP interface");
+ }
+ //@}
+
+ /** Method for obtaining the subspace in which the limited-memory
+ * Hessian approximation should be done. This is only called if
+ * the limited-memory Hessian approximation is chosen. Since the
+ * Hessian is zero in the space of all variables that appear in
+ * the problem functions only linearly, this allows the user to
+ * provide a VectorSpace for all nonlinear variables, and an
+ * ExpansionMatrix to lift from this VectorSpace to the
+ * VectorSpace of the primal variables x. If the returned values
+ * are NULL, it is assumed that the Hessian is to be approximated
+ * in the space of all x variables. The default instantiation of
+ * this method returns NULL, and a user only has to overwrite
+ * this method if the approximation is to be done only in a
+ * subspace. */
+ virtual void
+ GetQuasiNewtonApproximationSpaces(SmartPtr<VectorSpace>& approx_space,
+ SmartPtr<Matrix>& P_approx)
+ {
+ approx_space = NULL;
+ P_approx = NULL;
+ }
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ NLP(const NLP&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const NLP&);
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpNLPScaling.cpp b/SimTKmath/Optimizers/src/IpOpt/IpNLPScaling.cpp
new file mode 100644
index 0000000..40f60a2
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpNLPScaling.cpp
@@ -0,0 +1,539 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpNLPScaling.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-06-25
+
+#include "IpNLPScaling.hpp"
+
+namespace Ipopt
+{
+
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ SmartPtr<Vector> NLPScalingObject::apply_vector_scaling_x_LU_NonConst(
+ const Matrix& Px_LU,
+ const SmartPtr<const Vector>& lu,
+ const VectorSpace& x_space)
+ {
+ SmartPtr<Vector> scaled_x_LU = lu->MakeNew();
+ if (have_x_scaling()) {
+ SmartPtr<Vector> tmp_x = x_space.MakeNew();
+
+ // move to full x space
+ Px_LU.MultVector(1.0, *lu, 0.0, *tmp_x);
+
+ // scale in full x space
+ tmp_x = apply_vector_scaling_x_NonConst(ConstPtr(tmp_x));
+
+ // move back to x_L space
+ Px_LU.TransMultVector(1.0, *tmp_x, 0.0, *scaled_x_LU);
+ }
+ else {
+ scaled_x_LU->Copy(*lu);
+ }
+
+ return scaled_x_LU;
+ }
+
+ SmartPtr<const Vector> NLPScalingObject::apply_vector_scaling_x_LU(
+ const Matrix& Px_LU,
+ const SmartPtr<const Vector>& lu,
+ const VectorSpace& x_space)
+ {
+ if (have_x_scaling()) {
+ return ConstPtr(apply_vector_scaling_x_LU_NonConst(Px_LU, lu, x_space));
+ }
+ else {
+ return lu;
+ }
+ }
+
+ SmartPtr<Vector> NLPScalingObject::apply_vector_scaling_d_LU_NonConst(
+ const Matrix& Pd_LU,
+ const SmartPtr<const Vector>& lu,
+ const VectorSpace& d_space)
+ {
+ SmartPtr<Vector> scaled_d_LU = lu->MakeNew();
+ if (have_d_scaling()) {
+ SmartPtr<Vector> tmp_d = d_space.MakeNew();
+
+ // move to full d space
+ Pd_LU.MultVector(1.0, *lu, 0.0, *tmp_d);
+
+ // scale in full x space
+ tmp_d = apply_vector_scaling_d_NonConst(ConstPtr(tmp_d));
+
+ // move back to x_L space
+ Pd_LU.TransMultVector(1.0, *tmp_d, 0.0, *scaled_d_LU);
+ }
+ else {
+ scaled_d_LU->Copy(*lu);
+ }
+
+ return scaled_d_LU;
+ }
+
+ SmartPtr<const Vector> NLPScalingObject::apply_vector_scaling_d_LU(
+ const Matrix& Pd_LU,
+ const SmartPtr<const Vector>& lu,
+ const VectorSpace& d_space)
+ {
+ if (have_d_scaling()) {
+ return ConstPtr(apply_vector_scaling_d_LU_NonConst(Pd_LU, lu, d_space));
+ }
+ else {
+ return lu;
+ }
+ }
+
+ SmartPtr<Vector> NLPScalingObject::unapply_vector_scaling_d_LU_NonConst(
+ const Matrix& Pd_LU,
+ const SmartPtr<const Vector>& lu,
+ const VectorSpace& d_space)
+ {
+ SmartPtr<Vector> unscaled_d_LU = lu->MakeNew();
+ if (have_d_scaling()) {
+ SmartPtr<Vector> tmp_d = d_space.MakeNew();
+
+ // move to full d space
+ Pd_LU.MultVector(1.0, *lu, 0.0, *tmp_d);
+
+ // scale in full x space
+ tmp_d = unapply_vector_scaling_d_NonConst(ConstPtr(tmp_d));
+
+ // move back to x_L space
+ Pd_LU.TransMultVector(1.0, *tmp_d, 0.0, *unscaled_d_LU);
+ }
+ else {
+ unscaled_d_LU->Copy(*lu);
+ }
+
+ return unscaled_d_LU;
+ }
+
+ SmartPtr<const Vector> NLPScalingObject::unapply_vector_scaling_d_LU(
+ const Matrix& Pd_LU,
+ const SmartPtr<const Vector>& lu,
+ const VectorSpace& d_space)
+ {
+ if (have_d_scaling()) {
+ return ConstPtr(unapply_vector_scaling_d_LU_NonConst(Pd_LU, lu, d_space));
+ }
+ else {
+ return lu;
+ }
+ }
+
+ SmartPtr<Vector> NLPScalingObject::apply_grad_obj_scaling_NonConst(
+ const SmartPtr<const Vector>& v)
+ {
+ SmartPtr<Vector> scaled_v = unapply_vector_scaling_x_NonConst(v);
+ Number df = apply_obj_scaling(1.0);
+ if (df != 1.) {
+ scaled_v->Scal(df);
+ }
+ return scaled_v;
+ }
+
+ SmartPtr<const Vector> NLPScalingObject::apply_grad_obj_scaling(
+ const SmartPtr<const Vector>& v)
+ {
+ Number df = apply_obj_scaling(1.);
+ if (df != 1.) {
+ SmartPtr<Vector> scaled_v = apply_grad_obj_scaling_NonConst(v);
+ return ConstPtr(scaled_v);
+ }
+ else {
+ SmartPtr<const Vector> scaled_v = unapply_vector_scaling_x(v);
+ return scaled_v;
+ }
+ }
+
+ SmartPtr<Vector> NLPScalingObject::unapply_grad_obj_scaling_NonConst(
+ const SmartPtr<const Vector>& v)
+ {
+ SmartPtr<Vector> unscaled_v = apply_vector_scaling_x_NonConst(v);
+ Number df = unapply_obj_scaling(1.);
+ if (df != 1.) {
+ unscaled_v->Scal(df);
+ }
+ return unscaled_v;
+ }
+
+ SmartPtr<const Vector> NLPScalingObject::unapply_grad_obj_scaling(
+ const SmartPtr<const Vector>& v)
+ {
+ Number df = unapply_obj_scaling(1.);
+ if (df != 1.) {
+ SmartPtr<Vector> unscaled_v = unapply_grad_obj_scaling_NonConst(v);
+ return ConstPtr(unscaled_v);
+ }
+ else {
+ SmartPtr<const Vector> scaled_v = apply_vector_scaling_x(v);
+ return scaled_v;
+ }
+ }
+
+ void
+ StandardScalingBase::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->AddNumberOption(
+ "obj_scaling_factor",
+ "Scaling factor for the objective function.",
+ 1.,
+ "This option sets a scaling factor for the objective function. "
+ "The scaling is seen internally by Ipopt but the unscaled objective is "
+ "reported in the console output. "
+ "If additional scaling parameters are computed "
+ "(e.g. user-scaling or gradient-based), both factors are multiplied. "
+ "If this value is chosen to be negative, Ipopt will "
+ "maximize the objective function instead of minimizing it.");
+ }
+
+ bool StandardScalingBase::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ options.GetNumericValue("obj_scaling_factor", obj_scaling_factor_, prefix);
+ return true;
+ }
+
+ void StandardScalingBase::DetermineScaling(
+ const SmartPtr<const VectorSpace> x_space,
+ const SmartPtr<const VectorSpace> c_space,
+ const SmartPtr<const VectorSpace> d_space,
+ const SmartPtr<const MatrixSpace> jac_c_space,
+ const SmartPtr<const MatrixSpace> jac_d_space,
+ const SmartPtr<const SymMatrixSpace> h_space,
+ SmartPtr<const MatrixSpace>& new_jac_c_space,
+ SmartPtr<const MatrixSpace>& new_jac_d_space,
+ SmartPtr<const SymMatrixSpace>& new_h_space)
+ {
+ SmartPtr<Vector> dc;
+ SmartPtr<Vector> dd;
+ DetermineScalingParametersImpl(x_space, c_space, d_space,
+ jac_c_space, jac_d_space,
+ h_space, df_, dx_, dc, dd);
+
+ df_ *= obj_scaling_factor_;
+
+ if (Jnlst().ProduceOutput(J_VECTOR, J_MAIN)) {
+ Jnlst().Printf(J_VECTOR, J_MAIN, "objective scaling factor = %g\n", df_);
+ if (IsValid(dx_)) {
+ dx_->Print(Jnlst(), J_VECTOR, J_MAIN, "x scaling vector");
+ }
+ else {
+ Jnlst().Printf(J_VECTOR, J_MAIN, "No x scaling provided\n");
+ }
+ if (IsValid(dc)) {
+ dc->Print(Jnlst(), J_VECTOR, J_MAIN, "c scaling vector");
+ }
+ else {
+ Jnlst().Printf(J_VECTOR, J_MAIN, "No c scaling provided\n");
+ }
+ if (IsValid(dd)) {
+ dd->Print(Jnlst(), J_VECTOR, J_MAIN, "d scaling vector");
+ }
+ else {
+ Jnlst().Printf(J_VECTOR, J_MAIN, "No d scaling provided\n");
+ }
+ }
+
+ // create the scaling matrix spaces
+ if (IsValid(dx_) || IsValid(dc)) {
+ scaled_jac_c_space_ =
+ new ScaledMatrixSpace(ConstPtr(dc), false, jac_c_space,
+ ConstPtr(dx_), true);
+ new_jac_c_space = GetRawPtr(scaled_jac_c_space_);
+ }
+ else {
+ scaled_jac_c_space_ = NULL;
+ new_jac_c_space = jac_c_space;
+ }
+
+ if (IsValid(dx_) || IsValid(dc)) {
+ scaled_jac_d_space_ =
+ new ScaledMatrixSpace(ConstPtr(dd), false, jac_d_space,
+ ConstPtr(dx_), true);
+ new_jac_d_space = GetRawPtr(scaled_jac_d_space_);
+ }
+ else {
+ scaled_jac_d_space_ = NULL;
+ new_jac_d_space =jac_d_space ;
+ }
+
+ if (IsValid(h_space)) {
+ if (IsValid(dx_)) {
+ scaled_h_space_ = new SymScaledMatrixSpace(ConstPtr(dx_), true, h_space);
+ new_h_space = GetRawPtr(scaled_h_space_);
+ }
+ else {
+ scaled_h_space_ = NULL;
+ new_h_space = h_space;
+ }
+ }
+ else {
+ new_h_space = NULL;
+ }
+ }
+
+ Number StandardScalingBase::apply_obj_scaling(const Number& f)
+ {
+ return df_*f;
+ }
+
+ Number StandardScalingBase::unapply_obj_scaling(const Number& f)
+ {
+ return f/df_;
+ }
+
+ SmartPtr<Vector> StandardScalingBase::apply_vector_scaling_x_NonConst(
+ const SmartPtr<const Vector>& v)
+ {
+ DBG_START_METH("StandardScalingBase::apply_vector_scaling_x_NonConst",
+ dbg_verbosity);
+ SmartPtr<Vector> scaled_x = v->MakeNewCopy();
+ if (IsValid(dx_)) {
+ scaled_x->ElementWiseMultiply(*dx_);
+ }
+ else {
+ DBG_PRINT((1, "Creating copy in apply_vector_scaling_x_NonConst!"));
+ }
+ return scaled_x;
+ }
+
+ SmartPtr<const Vector> StandardScalingBase::apply_vector_scaling_x(
+ const SmartPtr<const Vector>& v)
+ {
+ if (IsValid(dx_)) {
+ return ConstPtr(apply_vector_scaling_x_NonConst(v));
+ }
+ else {
+ return v;
+ }
+ }
+
+ SmartPtr<Vector> StandardScalingBase::unapply_vector_scaling_x_NonConst(
+ const SmartPtr<const Vector>& v)
+ {
+ DBG_START_METH("StandardScalingBase::unapply_vector_scaling_x_NonConst",
+ dbg_verbosity);
+ SmartPtr<Vector> unscaled_x = v->MakeNewCopy();
+ if (IsValid(dx_)) {
+ unscaled_x->ElementWiseDivide(*dx_);
+ }
+ else {
+ DBG_PRINT((1, "Creating copy in unapply_vector_scaling_x_NonConst!"));
+ }
+ return unscaled_x;
+ }
+
+ SmartPtr<const Vector> StandardScalingBase::unapply_vector_scaling_x(
+ const SmartPtr<const Vector>& v)
+ {
+ if (IsValid(dx_)) {
+ return ConstPtr(unapply_vector_scaling_x_NonConst(v));
+ }
+ else {
+ return v;
+ }
+ }
+
+ SmartPtr<Vector> StandardScalingBase::apply_vector_scaling_c_NonConst(
+ const SmartPtr<const Vector>& v)
+ {
+ DBG_START_METH("StandardScalingBase::apply_vector_scaling_c_NonConst",
+ dbg_verbosity);
+ SmartPtr<Vector> scaled_c = v->MakeNewCopy();
+ if (IsValid(scaled_jac_c_space_) &&
+ IsValid(scaled_jac_c_space_->RowScaling())) {
+ scaled_c->ElementWiseMultiply(*scaled_jac_c_space_->RowScaling());
+ }
+ else {
+ DBG_PRINT((1,"Creating copy in apply_vector_scaling_c_NonConst!"));
+ }
+ return scaled_c;
+ }
+
+ SmartPtr<const Vector> StandardScalingBase::apply_vector_scaling_c(
+ const SmartPtr<const Vector>& v)
+ {
+ if (IsValid(scaled_jac_c_space_) &&
+ IsValid(scaled_jac_c_space_->RowScaling())) {
+ return ConstPtr(apply_vector_scaling_c_NonConst(v));
+ }
+ else {
+ return v;
+ }
+ }
+
+ SmartPtr<Vector> StandardScalingBase::unapply_vector_scaling_c_NonConst(
+ const SmartPtr<const Vector>& v)
+ {
+ DBG_START_METH("StandardScalingBase::unapply_vector_scaling_c_NonConst",
+ dbg_verbosity);
+ SmartPtr<Vector> scaled_c = v->MakeNewCopy();
+ if (IsValid(scaled_jac_c_space_) &&
+ IsValid(scaled_jac_c_space_->RowScaling())) {
+ scaled_c->ElementWiseDivide(*scaled_jac_c_space_->RowScaling());
+ }
+ else {
+ DBG_PRINT((1,"Creating copy in unapply_vector_scaling_c_NonConst!"));
+ }
+ return scaled_c;
+ }
+
+ SmartPtr<const Vector> StandardScalingBase::unapply_vector_scaling_c(
+ const SmartPtr<const Vector>& v)
+ {
+ if (IsValid(scaled_jac_c_space_) &&
+ IsValid(scaled_jac_c_space_->RowScaling())) {
+ return ConstPtr(unapply_vector_scaling_c_NonConst(v));
+ }
+ else {
+ return v;
+ }
+ }
+
+ SmartPtr<Vector> StandardScalingBase::apply_vector_scaling_d_NonConst(
+ const SmartPtr<const Vector>& v)
+ {
+ DBG_START_METH("StandardScalingBase::apply_vector_scaling_d_NonConst",
+ dbg_verbosity);
+ SmartPtr<Vector> scaled_d = v->MakeNewCopy();
+ if (IsValid(scaled_jac_d_space_) &&
+ IsValid(scaled_jac_d_space_->RowScaling())) {
+ scaled_d->ElementWiseMultiply(*scaled_jac_d_space_->RowScaling());
+ }
+ else {
+ DBG_PRINT((1,"Creating copy in apply_vector_scaling_d_NonConst!"));
+ }
+ return scaled_d;
+ }
+
+ SmartPtr<const Vector> StandardScalingBase::apply_vector_scaling_d(
+ const SmartPtr<const Vector>& v)
+ {
+ if (IsValid(scaled_jac_d_space_) &&
+ IsValid(scaled_jac_d_space_->RowScaling())) {
+ return ConstPtr(apply_vector_scaling_d_NonConst(v));
+ }
+ else {
+ return v;
+ }
+ }
+
+ SmartPtr<Vector> StandardScalingBase::unapply_vector_scaling_d_NonConst(
+ const SmartPtr<const Vector>& v)
+ {
+ DBG_START_METH("StandardScalingBase::unapply_vector_scaling_d_NonConst",
+ dbg_verbosity);
+ SmartPtr<Vector> scaled_d = v->MakeNewCopy();
+ if (IsValid(scaled_jac_d_space_) &&
+ IsValid(scaled_jac_d_space_->RowScaling())) {
+ scaled_d->ElementWiseDivide(*scaled_jac_d_space_->RowScaling());
+ }
+ else {
+ DBG_PRINT((1,"Creating copy in unapply_vector_scaling_d_NonConst!"));
+ }
+ return scaled_d;
+ }
+
+ SmartPtr<const Vector> StandardScalingBase::unapply_vector_scaling_d(
+ const SmartPtr<const Vector>& v)
+ {
+ if (IsValid(scaled_jac_d_space_) &&
+ IsValid(scaled_jac_d_space_->RowScaling())) {
+ return ConstPtr(unapply_vector_scaling_d_NonConst(v));
+ }
+ else {
+ return v;
+ }
+ }
+
+ // ToDo: matrix not passed by reference, so setting to NULL doesn't make difference
+ SmartPtr<const Matrix> StandardScalingBase::apply_jac_c_scaling(
+ SmartPtr<const Matrix> matrix)
+ {
+ if (IsValid(scaled_jac_c_space_)) {
+ SmartPtr<ScaledMatrix> ret = scaled_jac_c_space_->MakeNewScaledMatrix(false);
+ ret->SetUnscaledMatrix(matrix);
+ return GetRawPtr(ret);
+ }
+ else {
+ SmartPtr<const Matrix> ret = matrix;
+ matrix = NULL;
+ return ret;
+ }
+ }
+
+ SmartPtr<const Matrix> StandardScalingBase::apply_jac_d_scaling(
+ SmartPtr<const Matrix> matrix)
+ {
+ if (IsValid(scaled_jac_d_space_)) {
+ SmartPtr<ScaledMatrix> ret = scaled_jac_d_space_->MakeNewScaledMatrix(false);
+ ret->SetUnscaledMatrix(matrix);
+ return GetRawPtr(ret);
+ }
+ else {
+ SmartPtr<const Matrix> ret = matrix;
+ matrix = NULL;
+ return ret;
+ }
+ }
+
+ SmartPtr<const SymMatrix> StandardScalingBase::apply_hessian_scaling(
+ SmartPtr<const SymMatrix> matrix)
+ {
+ if (IsValid(scaled_h_space_)) {
+ SmartPtr<SymScaledMatrix> ret = scaled_h_space_->MakeNewSymScaledMatrix(false);
+ ret->SetUnscaledMatrix(matrix);
+ return GetRawPtr(ret);
+ }
+ else {
+ SmartPtr<const SymMatrix> ret = matrix;
+ matrix = NULL;
+ return ret;
+ }
+ }
+
+ bool StandardScalingBase::have_x_scaling()
+ {
+ return IsValid(dx_);
+ }
+
+ bool StandardScalingBase::have_c_scaling()
+ {
+ return (IsValid(scaled_jac_c_space_) &&
+ IsValid(scaled_jac_c_space_->RowScaling()));
+ }
+
+ bool StandardScalingBase::have_d_scaling()
+ {
+ return (IsValid(scaled_jac_d_space_) &&
+ IsValid(scaled_jac_d_space_->RowScaling()));
+ }
+
+ void NoNLPScalingObject::DetermineScalingParametersImpl(
+ const SmartPtr<const VectorSpace> x_space,
+ const SmartPtr<const VectorSpace> c_space,
+ const SmartPtr<const VectorSpace> d_space,
+ const SmartPtr<const MatrixSpace> jac_c_space,
+ const SmartPtr<const MatrixSpace> jac_d_space,
+ const SmartPtr<const SymMatrixSpace> h_space,
+ Number& df,
+ SmartPtr<Vector>& dx,
+ SmartPtr<Vector>& dc,
+ SmartPtr<Vector>& dd)
+ {
+ df = 1.;
+ dx = NULL;
+ dc = NULL;
+ dd = NULL;
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpNLPScaling.hpp b/SimTKmath/Optimizers/src/IpOpt/IpNLPScaling.hpp
new file mode 100644
index 0000000..c3a905e
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpNLPScaling.hpp
@@ -0,0 +1,440 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpNLPScaling.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPNLPSCALING_HPP__
+#define __IPNLPSCALING_HPP__
+
+#include "IpSymMatrix.hpp"
+#include "IpScaledMatrix.hpp"
+#include "IpSymScaledMatrix.hpp"
+#include "IpOptionsList.hpp"
+#include "IpRegOptions.hpp"
+
+namespace Ipopt
+{
+ /** This is the abstract base class for problem scaling.
+ * It is repsonsible for determining the scaling factors
+ * and mapping quantities in and out of scaled and unscaled
+ * versions
+ */
+ class NLPScalingObject : public ReferencedObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ NLPScalingObject()
+ {}
+
+ /** Default destructor */
+ virtual ~NLPScalingObject()
+ {}
+ //@}
+
+ /** Method to initialize the options */
+ bool Initialize(const Journalist& jnlst,
+ const OptionsList& options,
+ const std::string& prefix)
+ {
+ jnlst_ = &jnlst;
+ return InitializeImpl(options, prefix);
+ }
+
+ /** Methods to map scaled and unscaled matrices */
+ //@{
+ /** Returns an obj-scaled version of the given scalar */
+ virtual Number apply_obj_scaling(const Number& f)=0;
+ /** Returns an obj-unscaled version of the given scalar */
+ virtual Number unapply_obj_scaling(const Number& f)=0;
+ /** Returns an x-scaled version of the given vector */
+ virtual SmartPtr<Vector>
+ apply_vector_scaling_x_NonConst(const SmartPtr<const Vector>& v)=0;
+ /** Returns an x-scaled version of the given vector */
+ virtual SmartPtr<const Vector>
+ apply_vector_scaling_x(const SmartPtr<const Vector>& v)=0;
+ /** Returns an x-unscaled version of the given vector */
+ virtual SmartPtr<Vector>
+ unapply_vector_scaling_x_NonConst(const SmartPtr<const Vector>& v)=0;
+ /** Returns an x-unscaled version of the given vector */
+ virtual SmartPtr<const Vector>
+ unapply_vector_scaling_x(const SmartPtr<const Vector>& v)=0;
+ /** Returns an c-scaled version of the given vector */
+ virtual SmartPtr<const Vector>
+ apply_vector_scaling_c(const SmartPtr<const Vector>& v)=0;
+ /** Returns an c-unscaled version of the given vector */
+ virtual SmartPtr<const Vector>
+ unapply_vector_scaling_c(const SmartPtr<const Vector>& v)=0;
+ /** Returns an c-scaled version of the given vector */
+ virtual SmartPtr<Vector>
+ apply_vector_scaling_c_NonConst(const SmartPtr<const Vector>& v)=0;
+ /** Returns an c-unscaled version of the given vector */
+ virtual SmartPtr<Vector>
+ unapply_vector_scaling_c_NonConst(const SmartPtr<const Vector>& v)=0;
+ /** Returns an d-scaled version of the given vector */
+ virtual SmartPtr<const Vector>
+ apply_vector_scaling_d(const SmartPtr<const Vector>& v)=0;
+ /** Returns an d-unscaled version of the given vector */
+ virtual SmartPtr<const Vector>
+ unapply_vector_scaling_d(const SmartPtr<const Vector>& v)=0;
+ /** Returns an d-scaled version of the given vector */
+ virtual SmartPtr<Vector>
+ apply_vector_scaling_d_NonConst(const SmartPtr<const Vector>& v)=0;
+ /** Returns an d-unscaled version of the given vector */
+ virtual SmartPtr<Vector>
+ unapply_vector_scaling_d_NonConst(const SmartPtr<const Vector>& v)=0;
+ /** Returns a scaled version of the jacobian for c. If the
+ * overloaded method does not make a new matrix, make sure to set
+ * the matrix ptr passed in to NULL.
+ */
+ virtual SmartPtr<const Matrix>
+ apply_jac_c_scaling(SmartPtr<const Matrix> matrix)=0;
+ /** Returns a scaled version of the jacobian for d If the
+ * overloaded method does not create a new matrix, make sure to
+ * set the matrix ptr passed in to NULL.
+ */
+ virtual SmartPtr<const Matrix>
+ apply_jac_d_scaling(SmartPtr<const Matrix> matrix)=0;
+ /** Returns a scaled version of the hessian of the lagrangian If
+ * the overloaded method does not create a new matrix, make sure
+ * to set the matrix ptr passed in to NULL.
+ */
+ virtual SmartPtr<const SymMatrix>
+ apply_hessian_scaling(SmartPtr<const SymMatrix> matrix)=0;
+ //@}
+
+ /** Methods for scaling bounds - these wrap those above */
+ //@{
+ /** Returns an x-scaled vector in the x_L or x_U space */
+ SmartPtr<Vector> apply_vector_scaling_x_LU_NonConst(
+ const Matrix& Px_LU,
+ const SmartPtr<const Vector>& lu,
+ const VectorSpace& x_space);
+ /** Returns an x-scaled vector in the x_L or x_U space */
+ SmartPtr<const Vector> apply_vector_scaling_x_LU(
+ const Matrix& Px_LU,
+ const SmartPtr<const Vector>& lu,
+ const VectorSpace& x_space);
+ /** Returns an d-scaled vector in the d_L or d_U space */
+ SmartPtr<Vector> apply_vector_scaling_d_LU_NonConst(
+ const Matrix& Pd_LU,
+ const SmartPtr<const Vector>& lu,
+ const VectorSpace& d_space);
+ /** Returns an d-scaled vector in the d_L or d_U space */
+ SmartPtr<const Vector> apply_vector_scaling_d_LU(
+ const Matrix& Pd_LU,
+ const SmartPtr<const Vector>& lu,
+ const VectorSpace& d_space);
+ /** Returns an d-unscaled vector in the d_L or d_U space */
+ SmartPtr<Vector> unapply_vector_scaling_d_LU_NonConst(
+ const Matrix& Pd_LU,
+ const SmartPtr<const Vector>& lu,
+ const VectorSpace& d_space);
+ /** Returns an d-unscaled vector in the d_L or d_U space */
+ SmartPtr<const Vector> unapply_vector_scaling_d_LU(
+ const Matrix& Pd_LU,
+ const SmartPtr<const Vector>& lu,
+ const VectorSpace& d_space);
+ //@}
+
+ /** Methods for scaling the gradient of the objective - wraps the
+ * virtual methods above
+ */
+ //@{
+ /** Returns a grad_f scaled version (d_f * D_x^{-1}) of the given vector */
+ virtual SmartPtr<Vector>
+ apply_grad_obj_scaling_NonConst(const SmartPtr<const Vector>& v);
+ /** Returns a grad_f scaled version (d_f * D_x^{-1}) of the given vector */
+ virtual SmartPtr<const Vector>
+ apply_grad_obj_scaling(const SmartPtr<const Vector>& v);
+ /** Returns a grad_f unscaled version (d_f * D_x^{-1}) of the
+ * given vector */
+ virtual SmartPtr<Vector>
+ unapply_grad_obj_scaling_NonConst(const SmartPtr<const Vector>& v);
+ /** Returns a grad_f unscaled version (d_f * D_x^{-1}) of the
+ * given vector */
+ virtual SmartPtr<const Vector>
+ unapply_grad_obj_scaling(const SmartPtr<const Vector>& v);
+ //@}
+
+ /** @name Methods for determining whether scaling for entities is
+ * done */
+ //@{
+ /** Returns true if the primal x variables are scaled. */
+ virtual bool have_x_scaling()=0;
+ /** Returns true if the equality constraints are scaled. */
+ virtual bool have_c_scaling()=0;
+ /** Returns true if the inequality constraints are scaled. */
+ virtual bool have_d_scaling()=0;
+ //@}
+
+ /** This method is called by the IpoptNLP's at a convenient time to
+ * compute and/or read scaling factors
+ */
+ virtual void DetermineScaling(const SmartPtr<const VectorSpace> x_space,
+ const SmartPtr<const VectorSpace> c_space,
+ const SmartPtr<const VectorSpace> d_space,
+ const SmartPtr<const MatrixSpace> jac_c_space,
+ const SmartPtr<const MatrixSpace> jac_d_space,
+ const SmartPtr<const SymMatrixSpace> h_space,
+ SmartPtr<const MatrixSpace>& new_jac_c_space,
+ SmartPtr<const MatrixSpace>& new_jac_d_space,
+ SmartPtr<const SymMatrixSpace>& new_h_space)=0;
+ protected:
+ /** Implementation of the initialization method that has to be
+ * overloaded by for each derived class. */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix)=0;
+
+ /** Accessor method for the journalist */
+ const Journalist& Jnlst() const
+ {
+ return *jnlst_;
+ }
+ private:
+
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+
+ /** Copy Constructor */
+ NLPScalingObject(const NLPScalingObject&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const NLPScalingObject&);
+ //@}
+
+ SmartPtr<const Journalist> jnlst_;
+ };
+
+ /** This is a base class for many standard scaling
+ * techniques. The overloaded classes only need to
+ * provide the scaling parameters
+ */
+ class StandardScalingBase : public NLPScalingObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ StandardScalingBase()
+ {}
+
+ /** Default destructor */
+ virtual ~StandardScalingBase()
+ {}
+ //@}
+
+ /** Methods to map scaled and unscaled matrices */
+ //@{
+ /** Returns an obj-scaled version of the given scalar */
+ virtual Number apply_obj_scaling(const Number& f);
+ /** Returns an obj-unscaled version of the given scalar */
+ virtual Number unapply_obj_scaling(const Number& f);
+ /** Returns an x-scaled version of the given vector */
+ virtual SmartPtr<Vector>
+ apply_vector_scaling_x_NonConst(const SmartPtr<const Vector>& v);
+ /** Returns an x-scaled version of the given vector */
+ virtual SmartPtr<const Vector>
+ apply_vector_scaling_x(const SmartPtr<const Vector>& v);
+ /** Returns an x-unscaled version of the given vector */
+ virtual SmartPtr<Vector>
+ unapply_vector_scaling_x_NonConst(const SmartPtr<const Vector>& v);
+ /** Returns an x-unscaled version of the given vector */
+ virtual SmartPtr<const Vector>
+ unapply_vector_scaling_x(const SmartPtr<const Vector>& v);
+ /** Returns an c-scaled version of the given vector */
+ virtual SmartPtr<const Vector>
+ apply_vector_scaling_c(const SmartPtr<const Vector>& v);
+ /** Returns an c-unscaled version of the given vector */
+ virtual SmartPtr<const Vector>
+ unapply_vector_scaling_c(const SmartPtr<const Vector>& v);
+ /** Returns an c-scaled version of the given vector */
+ virtual SmartPtr<Vector>
+ apply_vector_scaling_c_NonConst(const SmartPtr<const Vector>& v);
+ /** Returns an c-unscaled version of the given vector */
+ virtual SmartPtr<Vector>
+ unapply_vector_scaling_c_NonConst(const SmartPtr<const Vector>& v);
+ /** Returns an d-scaled version of the given vector */
+ virtual SmartPtr<const Vector>
+ apply_vector_scaling_d(const SmartPtr<const Vector>& v);
+ /** Returns an d-unscaled version of the given vector */
+ virtual SmartPtr<const Vector>
+ unapply_vector_scaling_d(const SmartPtr<const Vector>& v);
+ /** Returns an d-scaled version of the given vector */
+ virtual SmartPtr<Vector>
+ apply_vector_scaling_d_NonConst(const SmartPtr<const Vector>& v);
+ /** Returns an d-unscaled version of the given vector */
+ virtual SmartPtr<Vector>
+ unapply_vector_scaling_d_NonConst(const SmartPtr<const Vector>& v);
+ /** Returns a scaled version of the jacobian for c. If the
+ * overloaded method does not make a new matrix, make sure to set
+ * the matrix ptr passed in to NULL.
+ */
+ virtual SmartPtr<const Matrix>
+ apply_jac_c_scaling(SmartPtr<const Matrix> matrix);
+ /** Returns a scaled version of the jacobian for d If the
+ * overloaded method does not create a new matrix, make sure to
+ * set the matrix ptr passed in to NULL.
+ */
+ virtual SmartPtr<const Matrix>
+ apply_jac_d_scaling(SmartPtr<const Matrix> matrix);
+ /** Returns a scaled version of the hessian of the lagrangian If
+ * the overloaded method does not create a new matrix, make sure
+ * to set the matrix ptr passed in to NULL.
+ */
+ virtual SmartPtr<const SymMatrix>
+ apply_hessian_scaling(SmartPtr<const SymMatrix> matrix);
+ //@}
+
+ /** @name Methods for determining whether scaling for entities is
+ * done */
+ //@{
+ virtual bool have_x_scaling();
+ virtual bool have_c_scaling();
+ virtual bool have_d_scaling();
+ //@}
+
+ /** This method is called by the IpoptNLP's at a convenient time to
+ * compute and/or read scaling factors
+ */
+ virtual void DetermineScaling(const SmartPtr<const VectorSpace> x_space,
+ const SmartPtr<const VectorSpace> c_space,
+ const SmartPtr<const VectorSpace> d_space,
+ const SmartPtr<const MatrixSpace> jac_c_space,
+ const SmartPtr<const MatrixSpace> jac_d_space,
+ const SmartPtr<const SymMatrixSpace> h_space,
+ SmartPtr<const MatrixSpace>& new_jac_c_space,
+ SmartPtr<const MatrixSpace>& new_jac_d_space,
+ SmartPtr<const SymMatrixSpace>& new_h_space);
+
+ /** Methods for IpoptType */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ protected:
+ /** Overloaded initialization method */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** This is the method that has to be overloaded by a particular
+ * scaling method that somehow computes the scaling vectors dx,
+ * dc, and dd. The pointers to those vectors can be NULL, in
+ * which case no scaling for that item will be done later. */
+ virtual void DetermineScalingParametersImpl(
+ const SmartPtr<const VectorSpace> x_space,
+ const SmartPtr<const VectorSpace> c_space,
+ const SmartPtr<const VectorSpace> d_space,
+ const SmartPtr<const MatrixSpace> jac_c_space,
+ const SmartPtr<const MatrixSpace> jac_d_space,
+ const SmartPtr<const SymMatrixSpace> h_space,
+ Number& df,
+ SmartPtr<Vector>& dx,
+ SmartPtr<Vector>& dc,
+ SmartPtr<Vector>& dd)=0;
+
+ private:
+
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+
+ /** Copy Constructor */
+ StandardScalingBase(const StandardScalingBase&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const StandardScalingBase&);
+ //@}
+
+ /** Scaling parameters - we only need to keep copies of
+ * the objective scaling and the x scaling - the others we can
+ * get from the scaled matrix spaces.
+ */
+ //@{
+ /** objective scaling parameter */
+ Number df_;
+ /** x scaling */
+ SmartPtr<Vector> dx_;
+ //@}
+
+ /** Scaled Matrix Spaces */
+ //@{
+ /** Scaled jacobian of c space */
+ SmartPtr<ScaledMatrixSpace> scaled_jac_c_space_;
+ /** Scaled jacobian of d space */
+ SmartPtr<ScaledMatrixSpace> scaled_jac_d_space_;
+ /** Scaled hessian of lagrangian spacea */
+ SmartPtr<SymScaledMatrixSpace> scaled_h_space_;
+ //@}
+
+ /** @name Algorithmic parameters */
+ //@{
+ /** Additional scaling value for the objective function */
+ Number obj_scaling_factor_;
+ //@}
+ };
+
+ /** Class implementing the scaling object that doesn't to any scaling */
+ class NoNLPScalingObject : public StandardScalingBase
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ NoNLPScalingObject()
+ {}
+
+ /** Default destructor */
+ virtual ~NoNLPScalingObject()
+ {}
+ //@}
+
+
+ protected:
+ /** Overloaded from StandardScalingBase */
+ virtual void DetermineScalingParametersImpl(
+ const SmartPtr<const VectorSpace> x_space,
+ const SmartPtr<const VectorSpace> c_space,
+ const SmartPtr<const VectorSpace> d_space,
+ const SmartPtr<const MatrixSpace> jac_c_space,
+ const SmartPtr<const MatrixSpace> jac_d_space,
+ const SmartPtr<const SymMatrixSpace> h_space,
+ Number& df,
+ SmartPtr<Vector>& dx,
+ SmartPtr<Vector>& dc,
+ SmartPtr<Vector>& dd);
+
+ private:
+
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+
+ /** Copy Constructor */
+ NoNLPScalingObject(const NoNLPScalingObject&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const NoNLPScalingObject&);
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpObserver.cpp b/SimTKmath/Optimizers/src/IpOpt/IpObserver.cpp
new file mode 100644
index 0000000..340b409
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpObserver.cpp
@@ -0,0 +1,19 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpObserver.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpObserver.hpp"
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG_OBSERVER
+ const Index Observer::dbg_verbosity = 0;
+ const Index Subject::dbg_verbosity = 0;
+#endif
+
+void preventNoSymbolsWarningInIpObserver() {}
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpObserver.hpp b/SimTKmath/Optimizers/src/IpOpt/IpObserver.hpp
new file mode 100644
index 0000000..d8e6ee1
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpObserver.hpp
@@ -0,0 +1,357 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpObserver.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPOBSERVER_HPP__
+#define __IPOBSERVER_HPP__
+
+#include "IpUtils.hpp"
+#include "IpDebug.hpp"
+#include <vector>
+#include <algorithm>
+
+namespace Ipopt
+{
+ /** Forward declarations */
+ class Subject;
+
+ /** Slight Variation of the Observer Design Pattern.
+ * This class implements the Observer class of the
+ * Observer Design Pattern. An Observer "Attach"es
+ * to a Subject, indicating that it would like to
+ * be notified of changes in the Subject.
+ * Any derived class wishing to recieve notifications
+ * from a Subject should inherit off of
+ * Observer and overload the protected method,
+ * RecieveNotification_(...).
+ */
+ class Observer
+ {
+ public:
+#ifdef IP_DEBUG_OBSERVER
+
+ static const Index dbg_verbosity;
+#endif
+
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor */
+ Observer()
+ {}
+
+ /** Default destructor */
+ virtual ~Observer();
+ //@}
+
+ /** Enumeration specifying the type of notification */
+ enum NotifyType {
+ NT_All,
+ NT_BeingDestroyed,
+ NT_Changed
+ };
+
+ protected:
+ /** Derived classes should call this method
+ * to request an "Attach" to a Subject. Do
+ * not call "Attach" explicitly on the Subject
+ * since further processing is done here
+ */
+ void RequestAttach(NotifyType notify_type, const Subject* subject);
+
+ /** Derived classes should call this method
+ * to request a "Detach" to a Subject. Do
+ * not call "Detach" explicitly on the Subject
+ * since further processing is done here
+ */
+ void RequestDetach(NotifyType notify_type, const Subject* subject);
+
+ /** Derived classes should overload this method to
+ * recieve the requested notification from
+ * attached Subjects
+ */
+ virtual void RecieveNotification(NotifyType notify_type, const Subject* subject)=0;
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ Observer(const Observer&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const Observer&);
+ //@}
+
+ /** A list of the subjects currently being
+ * observed. */
+ std::vector<const Subject*> subjects_;
+
+ /** Private Method for Recieving Notification
+ * should only be called by the friend class
+ * Subject. This method will, in turn, call
+ * the overloaded RecieveNotification method
+ * for the derived class to process.
+ */
+ void ProcessNotification(NotifyType notify_type, const Subject* subject);
+
+ friend class Subject;
+ };
+
+ /** Slight Variation of the Observer Design Pattern (Subject part).
+ * This class implements the Subject class of the Observer Design
+ * Pattern. An Observer "Attach"es to a Subject, indicating that it
+ * would like to be notified of changes in the Subject. Any
+ * derived class that is to be observed has to inherit off the
+ * Subject base class. If the subject needs to notify the
+ * Observer, it calls the Notify method.
+ */
+ class Subject
+ {
+ public:
+#ifdef IP_DEBUG_OBSERVER
+
+ static const Index dbg_verbosity;
+#endif
+
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor */
+ Subject()
+ {}
+ ;
+
+ /** Default destructor */
+ virtual ~Subject();
+ //@}
+
+ /**@name Methods to Add and Remove Observers.
+ * Currently, the notify_type flags are not used,
+ * and Observers are attached in general and will
+ * recieve all notifications (of the type requested
+ * and possibly of types not requested). It is
+ * up to the observer to ignore the types they
+ * are not interested in. The NotifyType in the
+ * parameter list is so a more efficient mechanism
+ * depending on type could be implemented later if
+ * necessary.*/
+ //@{
+
+ /** Attach the specified observer
+ * (i.e., begin recieving notifications). */
+ void AttachObserver(Observer::NotifyType notify_type, Observer* observer) const;
+
+ /** Detach the specified observer
+ * (i.e., no longer recieve notifications). */
+ void DetachObserver(Observer::NotifyType notify_type, Observer* observer) const;
+ //@}
+
+ protected:
+
+ void Notify(Observer::NotifyType notify_type) const;
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ Subject(const Subject&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const Subject&);
+ //@}
+
+ mutable std::vector<Observer*> observers_;
+
+ };
+
+ /* inline methods */
+ inline
+ Observer::~Observer()
+ {
+#ifdef IP_DEBUG_OBSERVER
+ DBG_START_METH("Observer::~Observer", dbg_verbosity);
+ if (DBG_VERBOSITY()>=1) {
+ for (Index i=0; i<(Index)subjects_.size(); i++) {
+ DBG_PRINT((1,"subjects_[%d] = 0x%x\n", i, subjects_[i]));
+ }
+ }
+#endif
+ // Detach all subjects
+ for (Int i=(Int)(subjects_.size()-1); i>=0; i--) {
+#ifdef IP_DEBUG_OBSERVER
+ DBG_PRINT((1,"About to detach subjects_[%d] = 0x%x\n", i, subjects_[i]));
+#endif
+
+ RequestDetach(NT_All, subjects_[i]);
+ }
+ }
+
+ inline
+ void Observer::RequestAttach(NotifyType notify_type, const Subject* subject)
+ {
+#ifdef IP_DEBUG_OBSERVER
+ DBG_START_METH("Observer::RequestAttach", dbg_verbosity);
+#endif
+
+#ifdef IP_DEBUG
+ // Add the subject to the list if it does not already exist
+ std::vector<const Subject*>::iterator attached_subject;
+ attached_subject = std::find(subjects_.begin(), subjects_.end(), subject);
+ DBG_ASSERT(attached_subject == subjects_.end());
+#endif
+
+ DBG_ASSERT(subject);
+
+ // add the subject to the list
+ subjects_.push_back(subject);
+ // Attach the observer to the subject
+ subject->AttachObserver(notify_type, this);
+ }
+
+ inline
+ void Observer::RequestDetach(NotifyType notify_type, const Subject* subject)
+ {
+#ifdef IP_DEBUG_OBSERVER
+ DBG_START_METH("Observer::RequestDetach", dbg_verbosity);
+ DBG_PRINT((1, "Requesting detach of subject: 0x%x\n", subject));
+ DBG_ASSERT(subject);
+#endif
+
+ if (subject) {
+ std::vector<const Subject*>::iterator attached_subject;
+ attached_subject = std::find(subjects_.begin(), subjects_.end(), subject);
+#ifdef IP_DEBUG_OBSERVER
+
+ DBG_ASSERT(attached_subject != subjects_.end());
+#endif
+
+ if (attached_subject != subjects_.end()) {
+#ifdef IP_DEBUG_OBSERVER
+ DBG_PRINT((1, "Removing subject: 0x%x from the list\n", subject));
+#endif
+
+ subjects_.erase(attached_subject);
+ }
+
+ // Detach the observer from the subject
+ subject->DetachObserver(notify_type, this);
+ }
+ }
+
+ inline
+ void Observer::ProcessNotification(NotifyType notify_type, const Subject* subject)
+ {
+#ifdef IP_DEBUG_OBSERVER
+ DBG_START_METH("Observer::ProcessNotification", dbg_verbosity);
+ DBG_ASSERT(subject);
+#endif
+
+ if (subject) {
+ std::vector<const Subject*>::iterator attached_subject;
+ attached_subject = std::find(subjects_.begin(), subjects_.end(), subject);
+
+ // We must be processing a notification for a
+ // subject that was previously attached.
+#ifdef IP_DEBUG_OBSERVER
+
+ DBG_ASSERT(attached_subject != subjects_.end());
+#endif
+
+ this->RecieveNotification(notify_type, subject);
+
+ if (notify_type == NT_BeingDestroyed) {
+ // the subject is going away, remove it from our list
+ subjects_.erase(attached_subject);
+ }
+ }
+ }
+
+ inline
+ Subject::~Subject()
+ {
+#ifdef IP_DEBUG_OBSERVER
+ DBG_START_METH("Subject::~Subject", dbg_verbosity);
+#endif
+
+ std::vector<Observer*>::iterator iter;
+ for (iter = observers_.begin(); iter != observers_.end(); iter++) {
+ (*iter)->ProcessNotification(Observer::NT_BeingDestroyed, this);
+ }
+ }
+
+ inline
+ void Subject::AttachObserver(Observer::NotifyType notify_type, Observer* observer) const
+ {
+#ifdef IP_DEBUG_OBSERVER
+ DBG_START_METH("Subject::AttachObserver", dbg_verbosity);
+ // current implementation notifies all observers of everything
+ // they must filter the notifications that they are not interested
+ // in (i.e. a hub, not a router)
+ DBG_ASSERT(observer);
+#endif
+
+
+#ifdef IP_DEBUG
+
+ std::vector<Observer*>::iterator attached_observer;
+ attached_observer = std::find(observers_.begin(), observers_.end(), observer);
+ DBG_ASSERT(attached_observer == observers_.end());
+#endif
+
+ DBG_ASSERT(observer);
+ observers_.push_back(observer);
+ }
+
+ inline
+ void Subject::DetachObserver(Observer::NotifyType notify_type, Observer* observer) const
+ {
+#ifdef IP_DEBUG_OBSERVER
+ DBG_START_METH("Subject::DetachObserver", dbg_verbosity);
+ DBG_ASSERT(observer);
+#endif
+
+ if (observer) {
+ std::vector<Observer*>::iterator attached_observer;
+ attached_observer = std::find(observers_.begin(), observers_.end(), observer);
+#ifdef IP_DEBUG_OBSERVER
+
+ DBG_ASSERT(attached_observer != observers_.end());
+#endif
+
+ if (attached_observer != observers_.end()) {
+ observers_.erase(attached_observer);
+ }
+ }
+ }
+
+ inline
+ void Subject::Notify(Observer::NotifyType notify_type) const
+ {
+#ifdef IP_DEBUG_OBSERVER
+ DBG_START_METH("Subject::Notify", dbg_verbosity);
+#endif
+
+ std::vector<Observer*>::iterator iter;
+ for (iter = observers_.begin(); iter != observers_.end(); iter++) {
+ (*iter)->ProcessNotification(notify_type, this);
+ }
+ }
+
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpOptErrorConvCheck.cpp b/SimTKmath/Optimizers/src/IpOpt/IpOptErrorConvCheck.cpp
new file mode 100644
index 0000000..8869426
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpOptErrorConvCheck.cpp
@@ -0,0 +1,236 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpOptErrorConvCheck.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpOptErrorConvCheck.hpp"
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ OptimalityErrorConvergenceCheck::OptimalityErrorConvergenceCheck()
+ {}
+
+ OptimalityErrorConvergenceCheck::~OptimalityErrorConvergenceCheck()
+ {}
+
+ void OptimalityErrorConvergenceCheck::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->AddLowerBoundedIntegerOption(
+ "max_iter",
+ "Maximum number of iterations.",
+ 0, 3000,
+ "The algorithm terminates with an error message if the number of "
+ "iterations exceeded this number.");
+ roptions->AddLowerBoundedNumberOption(
+ "dual_inf_tol",
+ "Desired threshold for the dual infeasibility.",
+ 0.0, true, 1e-4,
+ "Absolute tolerance on the dual infeasibility. Successful termination "
+ "requires that the max-norm of the (unscaled) dual infeasibility is less than this "
+ "threshold.");
+ roptions->AddLowerBoundedNumberOption(
+ "constr_viol_tol",
+ "Desired threshold for the constraint violation.",
+ 0.0, true, 1e-4,
+ "Absolute tolerance on the constraint violation. Successful termination "
+ "requires that the max-norm of the (unscaled) constraint violation is less than this "
+ "threshold.");
+ roptions->AddLowerBoundedNumberOption(
+ "compl_inf_tol",
+ "Desired threshold for the complementarity conditions.",
+ 0.0, true, 1e-4,
+ "Absolute tolerance on the complementarity. Successful termination "
+ "requires that the max-norm of the (unscaled) complementarity is less than this "
+ "threshold.");
+ roptions->AddLowerBoundedNumberOption(
+ "acceptable_tol",
+ "\"Acceptable\" convergence tolerance (relative).",
+ 0.0, true, 1e-6,
+ "Determines which (scaled) overall optimality error is considered to be"
+ " \"acceptable.\" There are two levels of termination criteria. If the "
+ "usual \"desired\" tolerances (see tol, dual_inf_tol etc) are satisfied "
+ "at an iteration, the algorithm immediately terminates with a success "
+ "message. On the other hand, if the algorithm encounters "
+ "\"acceptable_iter\" many iterations in a row that are considered "
+ "\"acceptable\", it will terminate before the desired convergence "
+ "tolerance is met. This is useful in cases where the algorithm might "
+ "not be able to achieve the \"desired\" level of accuracy.");
+ roptions->AddLowerBoundedIntegerOption(
+ "acceptable_iter",
+ "Number of \"acceptable\" iterates before triggering termination.",
+ 0, 15,
+ "If the algorithm encounters this many successive \"acceptable\" iterates "
+ "(see \"acceptable_tol\"), it terminates, assuming that the problem "
+ "has been solved to best possible accuracy given round-off. If it is "
+ "set to zero, this heuristic is disabled.");
+ roptions->AddLowerBoundedNumberOption(
+ "acceptable_dual_inf_tol",
+ "\"Acceptance\" threshold for the dual infeasibility.",
+ 0.0, true, 1e-2,
+ "Absolute tolerance on the dual infeasibility. \"Acceptable\" termination "
+ "requires that the (max-norm of the unscaled) dual infeasibility is less than this "
+ "threshold; see also acceptable_tol.");
+ roptions->AddLowerBoundedNumberOption(
+ "acceptable_constr_viol_tol",
+ "\"Acceptance\" threshold for the constraint violation.",
+ 0.0, true, 1e-2,
+ "Absolute tolerance on the constraint violation. \"Acceptable\" termination "
+ "requires that the max-norm of the (unscaled) constraint violation is less than this "
+ "threshold; see also acceptable_tol.");
+ roptions->AddLowerBoundedNumberOption(
+ "acceptable_compl_inf_tol",
+ "\"Acceptance\" threshold for the complementarity conditions.",
+ 0.0, true, 1e-2,
+ "Absolute tolerance on the complementarity. \"Acceptable\" termination "
+ "requires that the max-norm of the (unscaled) complementarity is less than this "
+ "threshold; see also acceptable_tol.");
+ roptions->AddLowerBoundedNumberOption(
+ "diverging_iterates_tol",
+ "Threshold for maximal value of primal iterates.",
+ 0.0, true, 1e20,
+ "If any component of the primal iterates exceeded this value (in "
+ "absolute terms), the optimization is aborted with the exit message "
+ "that the iterates seem to be diverging.");
+ }
+
+ bool
+ OptimalityErrorConvergenceCheck::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ options.GetIntegerValue("max_iter", max_iterations_, prefix);
+ options.GetNumericValue("dual_inf_tol", dual_inf_tol_, prefix);
+ options.GetNumericValue("constr_viol_tol", constr_viol_tol_, prefix);
+ options.GetNumericValue("compl_inf_tol", compl_inf_tol_, prefix);
+ options.GetIntegerValue("acceptable_iter", acceptable_iter_, prefix);
+ options.GetNumericValue("acceptable_tol", acceptable_tol_, prefix);
+ options.GetNumericValue("acceptable_dual_inf_tol", acceptable_dual_inf_tol_, prefix);
+ options.GetNumericValue("acceptable_constr_viol_tol", acceptable_constr_viol_tol_, prefix);
+ options.GetNumericValue("acceptable_compl_inf_tol", acceptable_compl_inf_tol_, prefix);
+ options.GetNumericValue("diverging_iterates_tol", diverging_iterates_tol_, prefix);
+ acceptable_counter_ = 0;
+
+ return true;
+ }
+
+ ConvergenceCheck::ConvergenceStatus
+ OptimalityErrorConvergenceCheck::CheckConvergence(bool call_intermediate_callback /*= true*/)
+ {
+ DBG_START_METH("OptimalityErrorConvergenceCheck::CheckConvergence", dbg_verbosity);
+
+ if (call_intermediate_callback) {
+ // Check if user requested termination by calling the intermediate
+ // user callback function
+ AlgorithmMode mode = RegularMode;
+ // Gather the information also used in the iteration output
+ Index iter = IpData().iter_count();
+ Number inf_pr = IpCq().curr_primal_infeasibility(NORM_MAX);
+ Number inf_du = IpCq().curr_dual_infeasibility(NORM_MAX);
+ Number mu = IpData().curr_mu();
+ Number dnrm;
+ if (IsValid(IpData().delta()) && IsValid(IpData().delta()->x()) && IsValid(IpData().delta()->s())) {
+ dnrm = Max(IpData().delta()->x()->Amax(), IpData().delta()->s()->Amax());
+ }
+ else {
+ // This is the first iteration - no search direction has been
+ // computed yet.
+ dnrm = 0.;
+ }
+ Number alpha_primal = IpData().info_alpha_primal();
+ Number alpha_dual = IpData().info_alpha_dual();
+ Number regu_x = IpData().info_regu_x();
+ Number unscaled_f = IpCq().unscaled_curr_f();
+ Index ls_count = IpData().info_ls_count();
+ bool request_stop =
+ !IpNLP().IntermediateCallBack(mode, iter, unscaled_f, inf_pr, inf_du,
+ mu, dnrm, regu_x, alpha_dual,
+ alpha_primal, ls_count,
+ &IpData(), &IpCq());
+
+ if (request_stop) {
+ return ConvergenceCheck::USER_STOP;
+ }
+ }
+
+ if (IpData().iter_count() >= max_iterations_) {
+ return ConvergenceCheck::MAXITER_EXCEEDED;
+ }
+
+ Number overall_error = IpCq().curr_nlp_error();
+ Number dual_inf = IpCq().unscaled_curr_dual_infeasibility(NORM_MAX);
+ Number constr_viol = IpCq().unscaled_curr_nlp_constraint_violation(NORM_MAX);
+ Number compl_inf = IpCq().unscaled_curr_complementarity(0., NORM_MAX);
+
+ if (IpData().curr()->x()->Dim()==IpData().curr()->y_c()->Dim()) {
+ // the problem is square, there is no point in looking at dual
+ // infeasibility and complementarity as termination criterion
+ dual_inf_tol_ = 1e300;
+ compl_inf_tol_ = 1e300;
+ }
+
+ if (overall_error <= IpData().tol() &&
+ dual_inf <= dual_inf_tol_ &&
+ constr_viol <= constr_viol_tol_ &&
+ compl_inf <= compl_inf_tol_) {
+ return ConvergenceCheck::CONVERGED;
+ }
+
+ if (acceptable_iter_>0 && CurrentIsAcceptable()) {
+ IpData().Append_info_string("A");
+ acceptable_counter_++;
+ if (acceptable_counter_ >= acceptable_iter_) {
+ return ConvergenceCheck::CONVERGED_TO_ACCEPTABLE_POINT;
+ }
+ }
+ else {
+ acceptable_counter_ = 0;
+ }
+
+ if (IpData().curr()->x()->Amax() > diverging_iterates_tol_) {
+ return ConvergenceCheck::DIVERGING;
+ }
+
+ return ConvergenceCheck::CONTINUE;
+ }
+
+ bool OptimalityErrorConvergenceCheck::CurrentIsAcceptable()
+ {
+ DBG_START_METH("OptimalityErrorConvergenceCheck::CurrentIsAcceptable",
+ dbg_verbosity);
+
+ Number overall_error = IpCq().curr_nlp_error();
+ Number dual_inf = IpCq().unscaled_curr_dual_infeasibility(NORM_MAX);
+ Number constr_viol = IpCq().unscaled_curr_nlp_constraint_violation(NORM_MAX);
+ Number compl_inf = IpCq().unscaled_curr_complementarity(0., NORM_MAX);
+
+ DBG_PRINT((1, "overall_error = %e\n", overall_error));
+ DBG_PRINT((1, "dual_inf = %e\n", dual_inf));
+ DBG_PRINT((1, "constr_viol = %e\n", constr_viol));
+ DBG_PRINT((1, "compl_inf = %e\n", compl_inf));
+
+ DBG_PRINT((1, "acceptable_tol_ = %e\n", acceptable_tol_));
+ DBG_PRINT((1, "acceptable_dual_inf_tol_ = %e\n", acceptable_dual_inf_tol_));
+ DBG_PRINT((1, "acceptable_constr_viol_tol_ = %e\n", acceptable_constr_viol_tol_));
+ DBG_PRINT((1, "acceptable_compl_inf_tol_ = %e\n", acceptable_compl_inf_tol_));
+
+ if (IpData().curr()->x()->Dim()==IpData().curr()->y_c()->Dim()) {
+ // the problem is square, there is no point in looking at dual
+ // infeasibility and complementarity as termination criterion
+ acceptable_dual_inf_tol_ = 1e300;
+ acceptable_compl_inf_tol_ = 1e300;
+ }
+
+ return (overall_error <= acceptable_tol_ &&
+ dual_inf <= acceptable_dual_inf_tol_ &&
+ constr_viol <= acceptable_constr_viol_tol_ &&
+ compl_inf <= acceptable_compl_inf_tol_);
+ }
+
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpOptErrorConvCheck.hpp b/SimTKmath/Optimizers/src/IpOpt/IpOptErrorConvCheck.hpp
new file mode 100644
index 0000000..94e5a83
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpOptErrorConvCheck.hpp
@@ -0,0 +1,98 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpOptErrorConvCheck.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPOPTERRORCONVCHECK_HPP__
+#define __IPOPTERRORCONVCHECK_HPP__
+
+#include "IpConvCheck.hpp"
+
+namespace Ipopt
+{
+
+ /** Brief Class Description.
+ * Detailed Class Description.
+ */
+ class OptimalityErrorConvergenceCheck : public ConvergenceCheck
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor */
+ OptimalityErrorConvergenceCheck();
+
+ /** Default destructor */
+ virtual ~OptimalityErrorConvergenceCheck();
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Overloaded convergence check */
+ virtual ConvergenceStatus
+ CheckConvergence(bool call_intermediate_callback = true);
+
+ /** Auxilliary function for testing whether current iterate
+ * satisfies the acceptable level of optimality */
+ virtual bool CurrentIsAcceptable();
+
+ /** Methods for IpoptType */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ protected:
+ /** @name Algorithmic parameters */
+ //@{
+ /** Maximal number of iterations */
+ Index max_iterations_;
+ /** Tolerance on unscaled dual infeasibility */
+ Number dual_inf_tol_;
+ /** Tolerance on unscaled constraint violation */
+ Number constr_viol_tol_;
+ /** Tolerance on unscaled complementarity */
+ Number compl_inf_tol_;
+ /** Number of iterations with acceptable level of accuracy, after
+ * which the algorithm terminates. If 0, this heuristic is
+ * disabled. */
+ Index acceptable_iter_;
+ /** Acceptable tolerance for the problem to terminate earlier if
+ * algorithm seems stuck or cycling */
+ Number acceptable_tol_;
+ /** Acceptable tolerance on unscaled dual infeasibility */
+ Number acceptable_dual_inf_tol_;
+ /** Acceptable tolerance on unscaled constraint violation */
+ Number acceptable_constr_viol_tol_;
+ /** Acceptable tolerance on unscaled complementarity */
+ Number acceptable_compl_inf_tol_;
+ /** Threshold for primal iterates for divergence test */
+ Number diverging_iterates_tol_;
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods (Hidden to avoid
+ * implicit creation/calling). These methods are not implemented
+ * and we do not want the compiler to implement them for us, so we
+ * declare them private and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ OptimalityErrorConvergenceCheck(const OptimalityErrorConvergenceCheck&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const OptimalityErrorConvergenceCheck&);
+ //@}
+
+ /** Counter for successive iterations in which acceptability
+ * criteria are met. */
+ Index acceptable_counter_;
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpOptionsList.cpp b/SimTKmath/Optimizers/src/IpOpt/IpOptionsList.cpp
new file mode 100644
index 0000000..e2d7557
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpOptionsList.cpp
@@ -0,0 +1,690 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpOptionsList.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpoptConfig.h"
+#include "IpOptionsList.hpp"
+#include <cstdlib>
+
+#ifdef HAVE_CCTYPE
+# include <cctype>
+#else
+# ifdef HAVE_CTYPE_H
+# include <ctype.h>
+# else
+# error "don't have header file for ctype"
+# endif
+#endif
+
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+
+
+#ifdef HAVE_CSTDIO
+# include <cstdio>
+#else
+# ifdef HAVE_STDIO_H
+# include <stdio.h>
+# else
+# error "don't have header file for stdio"
+# endif
+#endif
+
+namespace Ipopt
+{
+
+ bool OptionsList::SetStringValue(const std::string& tag,
+ const std::string& value,
+ bool allow_clobber, /* = true */
+ bool dont_print /* = false */)
+ {
+ if (IsValid(reg_options_)) {
+ SmartPtr<const RegisteredOption> option = reg_options_->GetOption(tag);
+
+ if (IsNull(option)) {
+ if (IsValid(jnlst_)) {
+ std::string msg = "Tried to set Option: " + tag;
+ msg += ". It is not a valid option. Please check the list of available options.\n";
+ jnlst_->Printf(J_ERROR, J_MAIN, msg.c_str());
+ }
+ //THROW_EXCEPTION(OPTION_INVALID, msg);
+ return false;
+ }
+
+ if (option->Type() != OT_String) {
+ if (IsValid(jnlst_)) {
+ std::string msg = "Tried to set Option: " + tag;
+ msg += ". It is a valid option, but it is of type ";
+ if (option->Type() == OT_Number) {
+ msg += " Number";
+ }
+ else if (option->Type() == OT_Integer) {
+ msg += " Integer";
+ }
+ else {
+ msg += " Unknown";
+ }
+ msg += ", not of type String. Please check the documentation for options.\n";
+ jnlst_->Printf(J_ERROR, J_MAIN, msg.c_str());
+ option->OutputDescription(*jnlst_);
+ }
+ //THROW_EXCEPTION(OPTION_INVALID, msg);
+ return false;
+ }
+
+ if (!option->IsValidStringSetting(value)) {
+ if (IsValid(jnlst_)) {
+ std::string msg = "Setting: " + value;
+ msg += " is not a valid setting for Option: ";
+ msg += tag;
+ msg += ". Check the option documentation.\n";
+ jnlst_->Printf(J_ERROR, J_MAIN, msg.c_str());
+ option->OutputDescription(*jnlst_);
+ }
+ //THROW_EXCEPTION(OPTION_INVALID, msg);
+ return false;
+ }
+ }
+
+ if (!will_allow_clobber(tag)) {
+ if (IsValid(jnlst_)) {
+ std::string msg = "WARNING: Tried to set option \"" + tag;
+ msg += "\" to a value of \"" + value;
+ msg += "\",\n but the previous value is set to disallow clobbering.\n";
+ msg += " The setting will remain as: \"" + tag;
+ msg += " " + options_[lowercase(tag)].GetValue();
+ msg += "\"\n";
+ jnlst_->Printf(J_WARNING, J_MAIN, msg.c_str());
+ }
+ }
+ else {
+ // if (will_allow_clobber(tag)) {
+ OptionsList::OptionValue optval(lowercase(value), allow_clobber,
+ dont_print);
+ options_[lowercase(tag)] = optval;
+ }
+ return true;
+
+ // std::string msg = "Option: \"" + tag;
+ // msg += " ";
+ // msg += value;
+ // msg += "\" not taken because a value of \n\"" ;
+ // msg += options_[lowercase(tag)].GetValue();
+ // msg += "\" already exists and is set to disallow clobbering.\n\n";
+ // jnlst_->Printf(J_ERROR, J_MAIN, msg.c_str());
+ // return false;
+ }
+
+ bool OptionsList::SetNumericValue(const std::string& tag, Number value,
+ bool allow_clobber, /* = true */
+ bool dont_print /* = false */)
+ {
+ char buffer[256];
+ sprintf(buffer, "%g", value);
+
+ if (IsValid(reg_options_)) {
+ SmartPtr<const RegisteredOption> option = reg_options_->GetOption(tag);
+
+ if (IsNull(option)) {
+ if (IsValid(jnlst_)) {
+ std::string msg = "Tried to set Option: " + tag;
+ msg += ". It is not a valid option. Please check the list of available options.\n";
+ jnlst_->Printf(J_ERROR, J_MAIN, msg.c_str());
+ }
+ //THROW_EXCEPTION(OPTION_INVALID, msg);
+ return false;
+ }
+
+ if (option->Type() != OT_Number) {
+ if (IsValid(jnlst_)) {
+ std::string msg = "Tried to set Option: " + tag;
+ msg += ". It is a valid option, but it is of type ";
+ if (option->Type() == OT_String) {
+ msg += " String";
+ }
+ else if (option->Type() == OT_Integer) {
+ msg += " Integer";
+ }
+ else {
+ msg += " Unknown";
+ }
+ msg += ", not of type Number. Please check the documentation for options.\n";
+ jnlst_->Printf(J_ERROR, J_MAIN, msg.c_str());
+ option->OutputDescription(*jnlst_);
+ }
+ //THROW_EXCEPTION(OPTION_INVALID, msg);
+ return false;
+ }
+
+ if (!option->IsValidNumberSetting(value)) {
+ if (IsValid(jnlst_)) {
+ std::string msg = "Setting: ";
+ msg += buffer;
+ msg += " is not a valid setting for Option: ";
+ msg += tag;
+ msg += ". Check the option documentation.\n";
+ jnlst_->Printf(J_ERROR, J_MAIN, msg.c_str());
+ option->OutputDescription(*jnlst_);
+ }
+ //THROW_EXCEPTION(OPTION_INVALID, msg);
+ return false;
+ }
+ }
+
+ if (!will_allow_clobber(tag)) {
+ if (IsValid(jnlst_)) {
+ std::string msg = "WARNING: Tried to set option \"" + tag;
+ msg += "\" to a value of \"";
+ msg += buffer;
+ msg += "\",\n but the previous value is set to disallow clobbering.\n";
+ msg += " The setting will remain as: \"" + tag;
+ msg += " " + options_[lowercase(tag)].GetValue();
+ msg += "\"\n";
+ jnlst_->Printf(J_WARNING, J_MAIN, msg.c_str());
+ }
+ }
+ else {
+ OptionsList::OptionValue optval(buffer, allow_clobber, dont_print);
+ options_[lowercase(tag)] = optval;
+ }
+ return true;
+ }
+
+ bool OptionsList::SetIntegerValue(const std::string& tag, Index value,
+ bool allow_clobber, /* = true */
+ bool dont_print /* = false */)
+ {
+ char buffer[256];
+ sprintf(buffer, "%d", value);
+
+ if (IsValid(reg_options_)) {
+ SmartPtr<const RegisteredOption> option = reg_options_->GetOption(tag);
+
+ if (IsNull(option)) {
+ std::string msg = "Tried to set Option: " + tag;
+ msg += ". It is not a valid option. Please check the list of available options.\n";
+ if (IsValid(jnlst_)) {
+ jnlst_->Printf(J_ERROR, J_MAIN, msg.c_str());
+ }
+ //THROW_EXCEPTION(OPTION_INVALID, msg);
+ return false;
+ }
+
+ if (option->Type() != OT_Integer) {
+ if (IsValid(jnlst_)) {
+ std::string msg = "Tried to set Option: " + tag;
+ msg += ". It is a valid option, but it is of type ";
+ if (option->Type() == OT_String) {
+ msg += " String";
+ }
+ else if (option->Type() == OT_Number) {
+ msg += " Number";
+ }
+ else {
+ msg += " Unknown";
+ }
+ msg += ", not of type Integer. Please check the documentation for options.\n";
+ jnlst_->Printf(J_ERROR, J_MAIN, msg.c_str());
+ option->OutputDescription(*jnlst_);
+ }
+ //THROW_EXCEPTION(OPTION_INVALID, msg);
+ return false;
+ }
+
+ if (!option->IsValidIntegerSetting(value)) {
+ if (IsValid(jnlst_)) {
+ std::string msg = "Setting: ";
+ msg += buffer;
+ msg += " is not a valid setting for Option: ";
+ msg += tag;
+ msg += ". Check the option documentation.\n";
+ jnlst_->Printf(J_ERROR, J_MAIN, msg.c_str());
+ option->OutputDescription(*jnlst_);
+ }
+ //THROW_EXCEPTION(OPTION_INVALID, msg);
+ return false;
+ }
+ }
+
+ if (!will_allow_clobber(tag)) {
+ if (IsValid(jnlst_)) {
+ std::string msg = "WARNING: Tried to set option \"" + tag;
+ msg += "\" to a value of \"";
+ msg += buffer;
+ msg += "\",\n but the previous value is set to disallow clobbering.\n";
+ msg += " The setting will remain as: \"" + tag;
+ msg += " " + options_[lowercase(tag)].GetValue();
+ msg += "\"\n";
+ jnlst_->Printf(J_WARNING, J_MAIN, msg.c_str());
+ }
+ }
+ else {
+ // if (will_allow_clobber(tag)) {
+ OptionsList::OptionValue optval(buffer, allow_clobber, dont_print);
+ options_[lowercase(tag)] = optval;
+ }
+ return true;
+ }
+
+ bool OptionsList::GetStringValue(const std::string& tag, std::string& value,
+ const std::string& prefix) const
+ {
+ SmartPtr<const RegisteredOption> option = NULL;
+
+ bool found = find_tag(tag, prefix, value);
+
+ if (IsValid(reg_options_)) {
+ option = reg_options_->GetOption(tag);
+ if (IsNull(option)) {
+ std::string msg = "IPOPT tried to get the value of Option: " + tag;
+ msg += ". It is not a valid registered option.";
+ THROW_EXCEPTION(OPTION_INVALID, msg);
+ }
+
+ if (option->Type() != OT_String) {
+ std::string msg = "IPOPT tried to get the value of Option: " + tag;
+ msg += ". It is a valid option, but it is of type ";
+ if (option->Type() == OT_Integer) {
+ msg += " Integer";
+ }
+ else if (option->Type() == OT_Number) {
+ msg += " Number";
+ }
+ else {
+ msg += " Unknown";
+ }
+ msg += ", not of type String. Please check the documentation for options.";
+ if (IsValid(jnlst_)) {
+ option->OutputDescription(*jnlst_);
+ }
+ THROW_EXCEPTION(OPTION_INVALID, msg);
+ }
+
+ if (found) {
+ value = option->MapStringSetting(value);
+ }
+ else {
+ value = option->DefaultString();
+ }
+ }
+
+ return found;
+ }
+
+ bool OptionsList::GetEnumValue(const std::string& tag, Index& value,
+ const std::string& prefix) const
+ {
+ std::string str;
+ SmartPtr<const RegisteredOption> option = NULL;
+
+ bool found = find_tag(tag, prefix, str);
+
+ if (IsValid(reg_options_)) {
+ option = reg_options_->GetOption(tag);
+ if (IsNull(option)) {
+ std::string msg = "IPOPT tried to get the value of Option: " + tag;
+ msg += ". It is not a valid registered option.";
+ THROW_EXCEPTION(OPTION_INVALID, msg);
+ }
+
+ if (option->Type() != OT_String) {
+ std::string msg = "IPOPT tried to get the value of Option: " + tag;
+ msg += ". It is a valid option, but it is of type ";
+ if (option->Type() == OT_Integer) {
+ msg += " Integer";
+ }
+ else if (option->Type() == OT_Number) {
+ msg += " Number";
+ }
+ else {
+ msg += " Unknown";
+ }
+ msg += ", not of type String. Please check the documentation for options.";
+ if (IsValid(jnlst_)) {
+ option->OutputDescription(*jnlst_);
+ }
+ THROW_EXCEPTION(OPTION_INVALID, msg);
+ }
+
+ if (found) {
+ value = option->MapStringSettingToEnum(str);
+ }
+ else {
+ value = option->DefaultStringAsEnum();
+ }
+ }
+
+ return found;
+ }
+
+ bool OptionsList::GetBoolValue(const std::string& tag, bool& value,
+ const std::string& prefix) const
+ {
+ std::string str;
+ bool ret = GetStringValue(tag, str, prefix);
+ if (str == "no" || str == "false" || str == "off") {
+ value = false;
+ }
+ else if (str == "yes" || str == "true" || str == "on") {
+ value = true;
+ }
+ else {
+ THROW_EXCEPTION(OPTION_INVALID, "Tried to get a boolean from an option and failed.");
+ }
+
+ return ret;
+ }
+
+ bool OptionsList::GetNumericValue(const std::string& tag, Number& value,
+ const std::string& prefix) const
+ {
+ SmartPtr<const RegisteredOption> option = NULL;
+
+ if (IsValid(reg_options_)) {
+ option = reg_options_->GetOption(tag);
+ if (IsNull(option)) {
+ std::string msg = "IPOPT tried to get the value of Option: " + tag;
+ msg += ". It is not a valid registered option.";
+ THROW_EXCEPTION(OPTION_INVALID, msg);
+ }
+
+ if (option->Type() != OT_Number) {
+ std::string msg = "IPOPT tried to get the value of Option: " + tag;
+ msg += ". It is a valid option, but it is of type ";
+ if (option->Type() == OT_Integer) {
+ msg += " Integer";
+ }
+ else if (option->Type() == OT_String) {
+ msg += " String";
+ }
+ else {
+ msg += " Unknown";
+ }
+ msg += ", not of type Number. Please check the documentation for options.";
+ if (IsValid(jnlst_)) {
+ option->OutputDescription(*jnlst_);
+ }
+ THROW_EXCEPTION(OPTION_INVALID, msg);
+ }
+ }
+
+ std::string strvalue;
+ if (find_tag(tag, prefix, strvalue)) {
+ char* p_end;
+ Number retval = strtod(strvalue.c_str(), &p_end);
+ if (*p_end!='\0' && !isspace(*p_end)) {
+ std::string msg = "Option \"" + tag +
+ "\": Double value expected, but non-numeric value \"" +
+ strvalue+"\" found.\n";
+ THROW_EXCEPTION(OPTION_INVALID, msg);
+ }
+ value = retval;
+ return true;
+ }
+ else if (IsValid(option)) {
+ value = option->DefaultNumber();
+ return false;
+ }
+ return false;
+ }
+
+ bool OptionsList::GetIntegerValue(const std::string& tag, Index& value,
+ const std::string& prefix) const
+ {
+ SmartPtr<const RegisteredOption> option = NULL;
+
+ if (IsValid(reg_options_)) {
+ option = reg_options_->GetOption(tag);
+ if (IsNull(option)) {
+ std::string msg = "IPOPT tried to get the value of Option: " + tag;
+ msg += ". It is not a valid registered option.";
+ THROW_EXCEPTION(OPTION_INVALID, msg);
+ }
+
+ if (option->Type() != OT_Integer) {
+ std::string msg = "IPOPT tried to get the value of Option: " + tag;
+ msg += ". It is a valid option, but it is of type ";
+ if (option->Type() == OT_Number) {
+ msg += " Number";
+ }
+ else if (option->Type() == OT_String) {
+ msg += " String";
+ }
+ else {
+ msg += " Unknown";
+ }
+ msg += ", not of type Integer. Please check the documentation for options.";
+ if (IsValid(jnlst_)) {
+ option->OutputDescription(*jnlst_);
+ }
+ THROW_EXCEPTION(OPTION_INVALID, msg);
+ }
+ }
+
+ std::string strvalue;
+ if (find_tag(tag, prefix, strvalue)) {
+ char* p_end;
+ Index retval = strtol(strvalue.c_str(), &p_end, 10);
+ if (*p_end!='\0' && !isspace(*p_end)) {
+ std::string msg = "Option \"" + tag +
+ "\": Integer value expected, but non-integer value \"" +
+ strvalue+"\" found.\n";
+ THROW_EXCEPTION(OPTION_INVALID, msg);
+ }
+ value = retval;
+ return true;
+ }
+ else if (IsValid(option)) {
+ value = option->DefaultInteger();
+ return false;
+ }
+
+ return false;
+ }
+
+ const std::string& OptionsList::lowercase(const std::string tag) const
+ {
+ lowercase_buffer_ = tag;
+ for(Index i=0; i<(Index)tag.length(); i++) {
+ lowercase_buffer_[i] = tolower(tag[i]);
+ }
+ return lowercase_buffer_;
+ }
+
+ void OptionsList::PrintList(std::string& list) const
+ {
+ list.clear();
+ char buffer[256];
+ sprintf(buffer, "%40s %-20s %s\n", "Name", "Value", "# times used");
+ list += buffer;
+ for(std::map< std::string, OptionValue >::const_iterator p = options_.begin();
+ p != options_.end();
+ p++ ) {
+ sprintf(buffer, "%40s = %-20s %6d\n", p->first.c_str(),
+ p->second.Value().c_str(), p->second.Counter());
+ list += buffer;
+ }
+ }
+
+ void OptionsList::PrintUserOptions(std::string& list) const
+ {
+ list.clear();
+ char buffer[256];
+ sprintf(buffer, "%40s %-20s %s\n", "Name", "Value", "used");
+ list += buffer;
+ for(std::map< std::string, OptionValue >::const_iterator p = options_.begin();
+ p != options_.end();
+ p++ ) {
+ if (!p->second.DontPrint()) {
+ const char yes[] = "yes";
+ const char no[] = "no";
+ const char* used;
+ if (p->second.Counter()>0) {
+ used = yes;
+ }
+ else {
+ used = no;
+ }
+ sprintf(buffer, "%40s = %-20s %4s\n", p->first.c_str(),
+ p->second.Value().c_str(), used);
+ list += buffer;
+ }
+ }
+ }
+
+ bool OptionsList::ReadFromStream(const Journalist& jnlst,
+ std::istream& is)
+ {
+ jnlst.Printf(J_DETAILED, J_MAIN, "Start reading options from stream.\n");
+
+ while (true) {
+ std::string tag;
+ std::string value;
+
+ if (!readnexttoken(is, tag)) {
+ // That's it - end of file reached.
+ jnlst.Printf(J_DETAILED, J_MAIN,
+ "Finished reading options from file.\n");
+ return true;
+ }
+
+ if (!readnexttoken(is, value)) {
+ // Can't read value for a given tag
+ jnlst.Printf(J_ERROR, J_MAIN,
+ "Error reading value for tag %s from file.\n",
+ tag.c_str());
+ return false;
+ }
+
+ // Now add the value for the options list
+ jnlst.Printf(J_DETAILED, J_MAIN,
+ "Adding option \"%s\" with value \"%s\" to OptionsList.\n",
+ tag.c_str(), value.c_str());
+
+ if (IsValid(reg_options_)) {
+ SmartPtr<const RegisteredOption> option = reg_options_->GetOption(tag);
+ if (IsNull(option)) {
+ std::string msg = "Read Option: ";
+ msg += tag;
+ msg += ". It is not a valid option. Check the list of available options.";
+ THROW_EXCEPTION(OPTION_INVALID, msg);
+ }
+
+ if (option->Type() == OT_String) {
+ bool result = SetStringValue(tag, value, false);
+ ASSERT_EXCEPTION(result, OPTION_INVALID,
+ "Error setting string value read from option file.");
+ }
+ else if (option->Type() == OT_Number) {
+ char* p_end;
+ Number retval = strtod(value.c_str(), &p_end);
+ if (*p_end!='\0' && !isspace(*p_end)) {
+ std::string msg = "Option \"" + tag +
+ "\": Double value expected, but non-numeric option value \"" +
+ value + "\" found.\n";
+ THROW_EXCEPTION(OPTION_INVALID, msg);
+ }
+ bool result = SetNumericValue(tag, retval, false);
+ ASSERT_EXCEPTION(result, OPTION_INVALID,
+ "Error setting numeric value read from file.");
+ }
+ else if (option->Type() == OT_Integer) {
+ char* p_end;
+ Index retval = strtol(value.c_str(), &p_end, 10);
+ if (*p_end!='\0' && !isspace(*p_end)) {
+ std::string msg = "Option \"" + tag +
+ "\": Integer value expected, but non-integer option value \"" +
+ value + "\" found.\n";
+ if (IsValid(jnlst_)) {
+ option->OutputDescription(*jnlst_);
+ }
+ THROW_EXCEPTION(OPTION_INVALID, msg);
+ }
+ bool result = SetIntegerValue(tag, retval, false);
+ ASSERT_EXCEPTION(result, OPTION_INVALID,
+ "Error setting integer value read from option file.");
+ }
+ else {
+ DBG_ASSERT(false && "Option Type: Unknown");
+ }
+ }
+ else {
+ bool result = SetStringValue(tag, value, false);
+ ASSERT_EXCEPTION(result, OPTION_INVALID,
+ "Error setting value read from option file.");
+ }
+ }
+ }
+
+ bool OptionsList::find_tag(const std::string& tag,
+ const std::string& prefix,
+ std::string& value) const
+ {
+ bool found=false;
+ std::map< std::string, OptionValue >::const_iterator p;
+
+ if (prefix != "") {
+ p = options_.find(lowercase(prefix+tag));
+ if (p != options_.end()) {
+ found = true;
+ }
+ }
+
+ if (!found) {
+ p = options_.find(lowercase(tag));
+ if (p != options_.end()) {
+ found = true;
+ }
+ }
+
+ if (found) {
+ value = p->second.GetValue();
+ }
+
+ return found;
+ }
+
+ bool OptionsList::will_allow_clobber(const std::string& tag) const
+ {
+ bool allow_clobber=true;
+ std::map< std::string, OptionValue >::const_iterator p;
+
+ p = options_.find(lowercase(tag));
+ if (p != options_.end()) {
+ allow_clobber = p->second.AllowClobber();
+ }
+
+ return allow_clobber;
+ }
+
+ bool OptionsList::readnexttoken(std::istream& is, std::string& token)
+ {
+ token.clear();
+ int c = is.get();
+
+ // First get rid of all comments and white spaces
+ while (!is.eof() && (isspace(c) || c=='#') ) {
+ if (c=='#') {
+ is.ignore(10000000, '\n');
+ }
+ c=is.get();
+ }
+
+ // Now read the token
+ while (!is.eof() && !isspace(c)) {
+ token += c;
+ c = is.get();
+ }
+
+ return (!is.eof());
+ }
+
+} // namespace Ipopt
+
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpOptionsList.hpp b/SimTKmath/Optimizers/src/IpOpt/IpOptionsList.hpp
new file mode 100644
index 0000000..45711ec
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpOptionsList.hpp
@@ -0,0 +1,270 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpOptionsList.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPOPTLIST_HPP__
+#define __IPOPTLIST_HPP__
+
+#include "IpUtils.hpp"
+#include "IpReferenced.hpp"
+#include "IpException.hpp"
+#include "IpRegOptions.hpp"
+
+#include <iostream>
+#include <map>
+
+namespace Ipopt
+{
+ /** Exception that can be used to indicate errors with options */
+ DECLARE_STD_EXCEPTION(OPTION_INVALID);
+
+ /** This class stores a list of user set options. Each options is
+ * identified by a case-insensitive keyword (tag). Its value is
+ * stored internally as a string (always lower case), but for
+ * convenience set and get methods are provided to obtain Index and
+ * Number type values. For each keyword we also keep track of how
+ * often the value of an option has been requested by a get method.
+ */
+ class OptionsList : public ReferencedObject
+ {
+ /** Class for storing the value and counter for each option in
+ * OptionsList. */
+ class OptionValue
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default constructor (needed for the map) */
+ OptionValue()
+ :
+ initialized_(false)
+ {}
+
+ /** Constructor given the value */
+ OptionValue(std::string value, bool allow_clobber, bool dont_print)
+ :
+ value_(value),
+ counter_(0),
+ initialized_(true),
+ allow_clobber_(allow_clobber),
+ dont_print_(dont_print)
+ {}
+
+ /** Copy Constructor */
+ OptionValue(const OptionValue& copy)
+ :
+ value_(copy.value_),
+ counter_(copy.counter_),
+ initialized_(copy.initialized_),
+ allow_clobber_(copy.allow_clobber_),
+ dont_print_(copy.dont_print_)
+ {}
+
+ /** Equals operator */
+ void operator=(const OptionValue& copy)
+ {
+ value_=copy.value_;
+ counter_=copy.counter_;
+ initialized_=copy.initialized_;
+ allow_clobber_=copy.allow_clobber_;
+ dont_print_=copy.dont_print_;
+ }
+
+ /** Default Destructor */
+ ~OptionValue()
+ {}
+ //@}
+
+ /** Method for retrieving the value of an option. Calling this
+ * method will increase the counter by one. */
+ std::string GetValue() const
+ {
+ DBG_ASSERT(initialized_);
+ counter_++;
+ return value_;
+ }
+
+ /** Method for retrieving the value without increasing the
+ * counter */
+ std::string Value() const
+ {
+ DBG_ASSERT(initialized_);
+ return value_;
+ }
+
+ /** Method for accessing current value of the request counter */
+ Index Counter() const
+ {
+ DBG_ASSERT(initialized_);
+ return counter_;
+ }
+
+ /** True if the option can be overwritten */
+ bool AllowClobber() const
+ {
+ DBG_ASSERT(initialized_);
+ return allow_clobber_;
+ }
+
+ /** True if this option is not to show up in the
+ * print_user_options output */
+ bool DontPrint() const
+ {
+ DBG_ASSERT(initialized_);
+ return dont_print_;
+ }
+
+ private:
+ /** Value for this option */
+ std::string value_;
+
+ /** Counter for requests */
+ mutable Index counter_;
+
+ /** for debugging */
+ bool initialized_;
+
+ /** True if the option can be overwritten */
+ bool allow_clobber_;
+
+ /** True if this option is not to show up in the
+ * print_user_options output */
+ bool dont_print_;
+ };
+
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ OptionsList(SmartPtr<RegisteredOptions> reg_options, SmartPtr<Journalist> jnlst)
+ : reg_options_(reg_options), jnlst_(jnlst)
+ {}
+
+ OptionsList()
+ {}
+
+ /** Copy Constructor */
+ OptionsList(const OptionsList& copy)
+ {
+ // copy all the option strings and values
+ options_ = copy.options_;
+ // copy the registered options pointer
+ reg_options_ = copy.reg_options_;
+ }
+
+ /** Default destructor */
+ virtual ~OptionsList()
+ {}
+
+ /** Overloaded Equals Operator */
+ void operator=(const OptionsList& source)
+ {
+ options_ = source.options_;
+ }
+ //@}
+
+ /** @name Get / Set Methods */
+ //@{
+ void SetRegisteredOptions(const SmartPtr<RegisteredOptions> reg_options)
+ {
+ reg_options_ = reg_options;
+ }
+ void SetJournalist(const SmartPtr<Journalist> jnlst)
+ {
+ jnlst_ = jnlst;
+ }
+ //@}
+ /** @name Methods for setting options */
+ //@{
+ bool SetStringValue(const std::string& tag, const std::string& value,
+ bool allow_clobber = true, bool dont_print = false);
+ bool SetNumericValue(const std::string& tag, Number value,
+ bool allow_clobber = true, bool dont_print = false);
+ bool SetIntegerValue(const std::string& tag, Index value,
+ bool allow_clobber = true, bool dont_print = false);
+ //@}
+
+ /** @name Methods for retrieving values from the options list. If
+ * a tag is not found, the methods return false, and value is set
+ * to the default value defined in the registered options. */
+ //@{
+ bool GetStringValue(const std::string& tag, std::string& value,
+ const std::string& prefix) const;
+ bool GetEnumValue(const std::string& tag, Index& value,
+ const std::string& prefix) const;
+ bool GetBoolValue(const std::string& tag, bool& value,
+ const std::string& prefix) const;
+ bool GetNumericValue(const std::string& tag, Number& value,
+ const std::string& prefix) const;
+ bool GetIntegerValue(const std::string& tag, Index& value,
+ const std::string& prefix) const;
+ //@}
+
+ /** Get a string with the list of all options (tag, value, counter) */
+ void PrintList(std::string& list) const;
+
+ /** Get a string with the list of all options set by the user
+ * (tag, value, use/notused). Here, options with dont_print flag
+ * set to true are not printed. */
+ void PrintUserOptions(std::string& list) const;
+
+ /** Read options from the stream is. Returns false if
+ * an error was encountered. */
+ bool ReadFromStream(const Journalist& jnlst, std::istream& is);
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ // OptionsList();
+
+ //@}
+
+ /** map for storing the options */
+ std::map< std::string, OptionValue > options_;
+
+ /** list of all the registered options to validate against */
+ SmartPtr<RegisteredOptions> reg_options_;
+
+ /** Journalist for writing error messages, etc. */
+ SmartPtr<Journalist> jnlst_;
+
+ /** auxilliary method for converting sting to all lower-case
+ * letters */
+ const std::string& lowercase(const std::string tag) const;
+
+ /** auxilliary method for finding the value for a tag in the
+ * options list. This method first looks for the concatenated
+ * string prefix+tag (if prefix is not ""), and if this is not
+ * found, it looks for tag. The return value is true iff
+ * prefix+tag or tag is found. In that case, the corresponding
+ * string value is copied into value. */
+ bool find_tag(const std::string& tag, const std::string& prefix,
+ std::string& value) const;
+
+ /** tells whether or not we can clobber a particular option.
+ * returns true if the option does not already exist, or if
+ * the option exists but is set to allow_clobber
+ */
+ bool will_allow_clobber(const std::string& tag) const;
+
+ /** read the next token from stream is. Returns false, if EOF was
+ * reached before a tokens was ecountered. */
+ bool readnexttoken(std::istream& is, std::string& token);
+
+ /** auxilliary string set by lowercase method */
+ mutable std::string lowercase_buffer_;
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpOrigIpoptNLP.cpp b/SimTKmath/Optimizers/src/IpOpt/IpOrigIpoptNLP.cpp
new file mode 100644
index 0000000..dc73a09
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpOrigIpoptNLP.cpp
@@ -0,0 +1,829 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpOrigIpoptNLP.cpp 765 2006-07-14 18:03:23Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpOrigIpoptNLP.hpp"
+#include "IpLowRankUpdateSymMatrix.hpp"
+#include "IpIpoptData.hpp"
+#include "IpIpoptCalculatedQuantities.hpp"
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ OrigIpoptNLP::OrigIpoptNLP(const SmartPtr<const Journalist>& jnlst,
+ const SmartPtr<NLP>& nlp,
+ const SmartPtr<NLPScalingObject>& nlp_scaling)
+ :
+ IpoptNLP(nlp_scaling),
+ jnlst_(jnlst),
+ nlp_(nlp),
+ x_space_(NULL),
+ f_cache_(1),
+ grad_f_cache_(1),
+ c_cache_(1),
+ jac_c_cache_(1),
+ d_cache_(1),
+ jac_d_cache_(1),
+ h_cache_(1),
+ f_evals_(0),
+ grad_f_evals_(0),
+ c_evals_(0),
+ jac_c_evals_(0),
+ d_evals_(0),
+ jac_d_evals_(0),
+ h_evals_(0),
+ initialized_(false)
+ {}
+
+ OrigIpoptNLP::~OrigIpoptNLP()
+ {}
+
+ void OrigIpoptNLP::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->AddLowerBoundedNumberOption(
+ "bound_relax_factor",
+ "Factor for initial relaxation of the bounds.",
+ 0, false,
+ 1e-8,
+ "Before start of the optimization, the bounds given by the user are "
+ "relaxed. This option sets the factor for this relaxation. If it "
+ "is set to zero, then then bounds relaxation is disabled. "
+ "(See Eqn.(35) in implementation paper.)");
+ roptions->AddStringOption2(
+ "honor_original_bounds",
+ "Indicates whether final points should be projected into original bounds.",
+ "yes",
+ "no", "Leave final point unchanged",
+ "yes", "Project final point back into original bounds",
+ "Ipopt might relax the bounds during the optimization (see, e.g., option "
+ "\"bound_relax_factor\"). This option determines whether the final "
+ "point should be projected back into the user-provide original bounds "
+ "after the optimization.");
+ roptions->SetRegisteringCategory("Warm Start");
+ roptions->AddStringOption2(
+ "warm_start_same_structure",
+ "Indicates whether a problem with a structure identical to the previous one is to be solved.",
+ "no",
+ "no", "Assume this is a new problem.",
+ "yes", "Assume this is problem has known structure",
+ "If \"yes\" is chosen, then the algorithm assumes that an NLP is now to "
+ "be solved, whose strcture is identical to one that already was "
+ "considered (with the same NLP object).");
+ roptions->SetRegisteringCategory("NLP");
+ roptions->AddStringOption2(
+ "check_derivatives_for_naninf",
+ "Indicates whether it is desired to check for Nan/Inf in derivative matrices",
+ "no",
+ "no", "Don't check (faster).",
+ "yes", "Check Jacobians and Hessian for Nan and Inf.",
+ "Activating this option will cause an error if an invalid number is "
+ "detected in the constraint Jacobians or the Lagrangian Hessian. If "
+ "this is not activated, the test is skipped, and the algorithm might "
+ "proceed with invalid numbers and fail.");
+ roptions->SetRegisteringCategory("Hessian Approximation");
+ roptions->AddStringOption2(
+ "hessian_approximation",
+ "Indicates what Hessian information is to be used.",
+ "exact",
+ "exact", "Use second derivatives provided by the NLP.",
+ "limited-memory", "Perform a limited-memory quasi-Newton approximation",
+ "This determines which kind of information for the Hessian of the "
+ "Lagrangian function is used by the algorithm.");
+ }
+
+ bool OrigIpoptNLP::Initialize(const Journalist& jnlst,
+ const OptionsList& options,
+ const std::string& prefix)
+ {
+ options.GetNumericValue("bound_relax_factor", bound_relax_factor_, prefix);
+ options.GetBoolValue("honor_original_bounds",
+ honor_original_bounds_, prefix);
+ options.GetBoolValue("warm_start_same_structure",
+ warm_start_same_structure_, prefix);
+ options.GetBoolValue("check_derivatives_for_naninf",
+ check_derivatives_for_naninf_, prefix);
+ Index enum_int;
+ options.GetEnumValue("hessian_approximation", enum_int, prefix);
+ hessian_approximation_ = HessianApproximationType(enum_int);
+
+ // Reset the function evaluation counters (for warm start)
+ f_evals_=0;
+ grad_f_evals_=0;
+ c_evals_=0;
+ jac_c_evals_=0;
+ d_evals_=0;
+ jac_d_evals_=0;
+ h_evals_=0;
+
+ // Reset the cache entries belonging to a dummy dependency. This
+ // is required for repeated solve, since the cache is not updated
+ // if a dimension is zero
+ std::vector<const TaggedObject*> deps(1);
+ deps[0] = NULL;
+ std::vector<Number> sdeps(0);
+ c_cache_.InvalidateResult(deps, sdeps);
+ d_cache_.InvalidateResult(deps, sdeps);
+ jac_c_cache_.InvalidateResult(deps, sdeps);
+ jac_d_cache_.InvalidateResult(deps, sdeps);
+
+ if (!nlp_->ProcessOptions(options, prefix)) {
+ return false;
+ }
+
+ initialized_ = true;
+ return IpoptNLP::Initialize(jnlst, options, prefix);
+ }
+
+ bool OrigIpoptNLP::InitializeStructures(SmartPtr<Vector>& x,
+ bool init_x,
+ SmartPtr<Vector>& y_c,
+ bool init_y_c,
+ SmartPtr<Vector>& y_d,
+ bool init_y_d,
+ SmartPtr<Vector>& z_L,
+ bool init_z_L,
+ SmartPtr<Vector>& z_U,
+ bool init_z_U,
+ SmartPtr<Vector>& v_L,
+ SmartPtr<Vector>& v_U
+ )
+ {
+ DBG_START_METH("OrigIpoptNLP::InitializeStructures", dbg_verbosity);
+ DBG_ASSERT(initialized_);
+ bool retValue;
+
+ if (!warm_start_same_structure_) {
+
+ retValue = nlp_->GetSpaces(x_space_, c_space_, d_space_,
+ x_l_space_, px_l_space_,
+ x_u_space_, px_u_space_,
+ d_l_space_, pd_l_space_,
+ d_u_space_, pd_u_space_,
+ jac_c_space_, jac_d_space_,
+ h_space_);
+
+ if (!retValue) {
+ return false;
+ }
+
+ // Check if the Hessian space is actually a limited-memory
+ // approximation. If so, get the required information from the
+ // NLP and create an appropreate h_space
+ if (hessian_approximation_==LIMITED_MEMORY) {
+ SmartPtr<VectorSpace> approx_vecspace;
+ SmartPtr<Matrix> P_approx;
+ nlp_->GetQuasiNewtonApproximationSpaces(approx_vecspace,
+ P_approx);
+ if (IsValid(approx_vecspace)) {
+ DBG_ASSERT(IsValid(P_approx));
+ h_space_ = new LowRankUpdateSymMatrixSpace(x_space_->Dim(),
+ ConstPtr(P_approx),
+ ConstPtr(approx_vecspace),
+ true);
+ jnlst_->Printf(J_DETAILED, J_INITIALIZATION,
+ "Hessian approximation will be done in smaller space of dimension %d (instead of %d)\n\n",
+ P_approx->NCols(), P_approx->NRows());
+ }
+ else {
+ DBG_ASSERT(IsNull(P_approx));
+ h_space_ = new LowRankUpdateSymMatrixSpace(x_space_->Dim(),
+ ConstPtr(P_approx),
+ ConstPtr(x_space_),
+ true);
+ jnlst_->Printf(J_DETAILED, J_INITIALIZATION,
+ "Hessian approximation will be done in the space of all %d x variables.\n\n",
+ x_space_->Dim());
+ }
+ }
+
+ NLP_scaling()->DetermineScaling(x_space_,
+ c_space_, d_space_,
+ jac_c_space_, jac_d_space_,
+ h_space_,
+ scaled_jac_c_space_, scaled_jac_d_space_,
+ scaled_h_space_);
+
+ ASSERT_EXCEPTION(x_space_->Dim() >= c_space_->Dim(), TOO_FEW_DOF,
+ "Too few degrees of freedom!");
+ ASSERT_EXCEPTION(x_space_->Dim() > 0, TOO_FEW_DOF,
+ "Too few degrees of freedom (no free variables)!");
+
+ // cannot have any null pointers, want zero length vectors
+ // instead of null - this will later need to be changed for _h;
+ retValue = (IsValid(x_space_) && IsValid(c_space_) && IsValid(d_space_)
+ && IsValid(x_l_space_) && IsValid(px_l_space_)
+ && IsValid(x_u_space_) && IsValid(px_u_space_)
+ && IsValid(d_u_space_) && IsValid(pd_u_space_)
+ && IsValid(d_l_space_) && IsValid(pd_l_space_)
+ && IsValid(jac_c_space_) && IsValid(jac_d_space_)
+ && IsValid(h_space_)
+ && IsValid(scaled_jac_c_space_)
+ && IsValid(scaled_jac_d_space_)
+ && IsValid(scaled_h_space_));
+
+ DBG_ASSERT(retValue && "Model cannot return null vector or matrix prototypes or spaces,"
+ " please return zero length vectors instead");
+ }
+ else {
+ ASSERT_EXCEPTION(IsValid(x_space_), INVALID_WARMSTART,
+ "OrigIpoptNLP called with warm_start_same_structure, but the problem is solved for the first time.");
+ }
+
+ // Create the bounds structures
+ SmartPtr<Vector> x_L = x_l_space_->MakeNew();
+ SmartPtr<Matrix> Px_L = px_l_space_->MakeNew();
+ SmartPtr<Vector> x_U = x_u_space_->MakeNew();
+ SmartPtr<Matrix> Px_U = px_u_space_->MakeNew();
+ SmartPtr<Vector> d_L = d_l_space_->MakeNew();
+ SmartPtr<Matrix> Pd_L = pd_l_space_->MakeNew();
+ SmartPtr<Vector> d_U = d_u_space_->MakeNew();
+ SmartPtr<Matrix> Pd_U = pd_u_space_->MakeNew();
+
+ retValue = nlp_->GetBoundsInformation(*Px_L, *x_L, *Px_U, *x_U,
+ *Pd_L, *d_L, *Pd_U, *d_U);
+
+ if (!retValue) {
+ return false;
+ }
+
+ x_L->Print(*jnlst_, J_MOREVECTOR, J_INITIALIZATION,
+ "original x_L unscaled");
+ x_U->Print(*jnlst_, J_MOREVECTOR, J_INITIALIZATION,
+ "original x_U unscaled");
+ d_L->Print(*jnlst_, J_MOREVECTOR, J_INITIALIZATION,
+ "original d_L unscaled");
+ d_U->Print(*jnlst_, J_MOREVECTOR, J_INITIALIZATION,
+ "original d_U unscaled");
+
+ if (honor_original_bounds_) {
+ SmartPtr<Vector> tmp;
+ tmp = x_L->MakeNewCopy();
+ orig_x_L_ = ConstPtr(tmp);
+ tmp = x_U->MakeNewCopy();
+ orig_x_U_ = ConstPtr(tmp);
+ }
+
+ relax_bounds(-bound_relax_factor_, *x_L);
+ relax_bounds( bound_relax_factor_, *x_U);
+ relax_bounds(-bound_relax_factor_, *d_L);
+ relax_bounds( bound_relax_factor_, *d_U);
+
+ x_L_ = ConstPtr(x_L);
+ Px_L_ = ConstPtr(Px_L);
+ x_U_ = ConstPtr(x_U);
+ Px_U_ = ConstPtr(Px_U);
+ d_L_ = ConstPtr(d_L);
+ Pd_L_ = ConstPtr(Pd_L);
+ d_U_ = ConstPtr(d_U);
+ Pd_U_ = ConstPtr(Pd_U);
+
+ // now create and store the scaled bounds
+ x_L_ = NLP_scaling()->apply_vector_scaling_x_LU(*Px_L_, x_L_, *x_space_);
+ x_U_ = NLP_scaling()->apply_vector_scaling_x_LU(*Px_U_, x_U_, *x_space_);
+ d_L_ = NLP_scaling()->apply_vector_scaling_d_LU(*Pd_L_, d_L_, *d_space_);
+ d_U_ = NLP_scaling()->apply_vector_scaling_d_LU(*Pd_U_, d_U_, *d_space_);
+
+ x_L->Print(*jnlst_, J_VECTOR, J_INITIALIZATION,
+ "modified x_L scaled");
+ x_U->Print(*jnlst_, J_VECTOR, J_INITIALIZATION,
+ "modified x_U scaled");
+ d_L->Print(*jnlst_, J_VECTOR, J_INITIALIZATION,
+ "modified d_L scaled");
+ d_U->Print(*jnlst_, J_VECTOR, J_INITIALIZATION,
+ "modified d_U scaled");
+
+ // Create the iterates structures
+ x = x_space_->MakeNew();
+ y_c = c_space_->MakeNew();
+ y_d = d_space_->MakeNew();
+ z_L = x_l_space_->MakeNew();
+ z_U = x_u_space_->MakeNew();
+ v_L = d_l_space_->MakeNew();
+ v_U = d_u_space_->MakeNew();
+
+ retValue = nlp_->GetStartingPoint(GetRawPtr(x), init_x,
+ GetRawPtr(y_c), init_y_c,
+ GetRawPtr(y_d), init_y_d,
+ GetRawPtr(z_L), init_z_L,
+ GetRawPtr(z_U), init_z_U);
+
+ if (!retValue) {
+ return false;
+ }
+
+
+ Number obj_scal = NLP_scaling()->apply_obj_scaling(1.);
+ if (init_x) {
+ x->Print(*jnlst_, J_VECTOR, J_INITIALIZATION, "initial x unscaled");
+ if (NLP_scaling()->have_x_scaling()) {
+ x = NLP_scaling()->apply_vector_scaling_x_NonConst(ConstPtr(x));
+ }
+ }
+ if (init_y_c) {
+ y_c->Print(*jnlst_, J_VECTOR, J_INITIALIZATION, "initial y_c unscaled");
+ if (NLP_scaling()->have_c_scaling()) {
+ y_c = NLP_scaling()->unapply_vector_scaling_c_NonConst(ConstPtr(y_c));
+ }
+ if (obj_scal!=1.) {
+ y_c->Scal(obj_scal);
+ }
+ }
+ if (init_y_d) {
+ y_d->Print(*jnlst_, J_VECTOR, J_INITIALIZATION, "initial y_d unscaled");
+ if (NLP_scaling()->have_d_scaling()) {
+ y_d = NLP_scaling()->unapply_vector_scaling_d_NonConst(ConstPtr(y_d));
+ }
+ if (obj_scal!=1.) {
+ y_d->Scal(obj_scal);
+ }
+ }
+ if (init_z_L) {
+ z_L->Print(*jnlst_, J_VECTOR, J_INITIALIZATION, "initial z_L unscaled");
+ if (NLP_scaling()->have_x_scaling()) {
+ z_L = NLP_scaling()->apply_vector_scaling_x_LU_NonConst(*Px_L_, ConstPtr(z_L), *x_space_);
+ }
+ if (obj_scal!=1.) {
+ z_L->Scal(obj_scal);
+ }
+ }
+ if (init_z_U) {
+ z_U->Print(*jnlst_, J_VECTOR, J_INITIALIZATION, "initial z_U unscaled");
+ if (NLP_scaling()->have_x_scaling()) {
+ z_U = NLP_scaling()->apply_vector_scaling_x_LU_NonConst(*Px_U_, ConstPtr(z_U), *x_space_);
+ }
+ if (obj_scal!=1.) {
+ z_U->Scal(obj_scal);
+ }
+ }
+
+ return true;
+ }
+
+ void
+ OrigIpoptNLP::relax_bounds(Number bound_relax_factor, Vector& bounds)
+ {
+ DBG_START_METH("OrigIpoptNLP::relax_bounds", dbg_verbosity);
+ if (bound_relax_factor!=0.) {
+ SmartPtr<Vector> tmp = bounds.MakeNew();
+ tmp->Copy(bounds);
+ tmp->ElementWiseAbs();
+ SmartPtr<Vector> ones = bounds.MakeNew();
+ ones->Set(1.);
+ tmp->ElementWiseMax(*ones);
+ DBG_PRINT((1, "bound_relax_factor = %e", bound_relax_factor));
+ DBG_PRINT_VECTOR(2, "tmp", *tmp);
+ DBG_PRINT_VECTOR(2, "bounds before", bounds);
+ bounds.Axpy(bound_relax_factor, *tmp);
+ DBG_PRINT_VECTOR(2, "bounds after", bounds);
+ }
+ }
+
+ Number OrigIpoptNLP::f(const Vector& x)
+ {
+ x.Dot(x);
+ x.Dot(x);
+ DBG_START_METH("OrigIpoptNLP::f", dbg_verbosity);
+ Number ret = 0.0;
+ DBG_PRINT((2, "x.Tag = %d\n", x.GetTag()));
+ if (!f_cache_.GetCachedResult1Dep(ret, &x)) {
+ f_evals_++;
+ SmartPtr<const Vector> unscaled_x = NLP_scaling()->unapply_vector_scaling_x(&x);
+ f_eval_time_.Start();
+ bool success = nlp_->Eval_f(*unscaled_x, ret);
+ f_eval_time_.End();
+ DBG_PRINT((1, "success = %d ret = %e\n", success, ret));
+ ASSERT_EXCEPTION(success && IsFiniteNumber(ret), Eval_Error,
+ "Error evaluating the objective function");
+ ret = NLP_scaling()->apply_obj_scaling(ret);
+ f_cache_.AddCachedResult1Dep(ret, &x);
+ }
+
+ return ret;
+ }
+
+ Number OrigIpoptNLP::f(const Vector& x, Number mu)
+ {
+ assert(false && "ERROR: This method is only a placeholder for f(mu) and should not be called");
+ return 0.;
+ }
+
+ SmartPtr<const Vector> OrigIpoptNLP::grad_f(const Vector& x)
+ {
+ SmartPtr<Vector> unscaled_grad_f;
+ SmartPtr<const Vector> retValue;
+ if (!grad_f_cache_.GetCachedResult1Dep(retValue, &x)) {
+ grad_f_evals_++;
+ unscaled_grad_f = x_space_->MakeNew();
+
+ SmartPtr<const Vector> unscaled_x = NLP_scaling()->unapply_vector_scaling_x(&x);
+ grad_f_eval_time_.Start();
+ bool success = nlp_->Eval_grad_f(*unscaled_x, *unscaled_grad_f);
+ grad_f_eval_time_.End();
+ ASSERT_EXCEPTION(success && IsFiniteNumber(unscaled_grad_f->Nrm2()),
+ Eval_Error, "Error evaluating the gradient of the objective function");
+ retValue = NLP_scaling()->apply_grad_obj_scaling(ConstPtr(unscaled_grad_f));
+ grad_f_cache_.AddCachedResult1Dep(retValue, &x);
+ }
+
+ return retValue;
+ }
+
+ SmartPtr<const Vector> OrigIpoptNLP::grad_f(const Vector& x, Number mu)
+ {
+ THROW_EXCEPTION(INTERNAL_ABORT,
+ "ERROR: This method is only a placeholder for grad_f(mu) and should not be called");
+ return NULL;
+ }
+
+ /** Equality constraint residual */
+ SmartPtr<const Vector> OrigIpoptNLP::c(const Vector& x)
+ {
+ SmartPtr<const Vector> retValue;
+ if (c_space_->Dim()==0) {
+ // We do this caching of an empty vector so that the returned
+ // Vector has always the same tag (this might make a difference
+ // in cases where only the constraints are supposed to change...
+ SmartPtr<const Vector> dep = NULL;
+ if (!c_cache_.GetCachedResult1Dep(retValue, GetRawPtr(dep))) {
+ retValue = c_space_->MakeNew();
+ c_cache_.AddCachedResult1Dep(retValue, GetRawPtr(dep));
+ }
+ }
+ else {
+ if (!c_cache_.GetCachedResult1Dep(retValue, x)) {
+ SmartPtr<Vector> unscaled_c = c_space_->MakeNew();
+ c_evals_++;
+ SmartPtr<const Vector> unscaled_x = NLP_scaling()->unapply_vector_scaling_x(&x);
+ c_eval_time_.Start();
+ bool success = nlp_->Eval_c(*unscaled_x, *unscaled_c);
+ c_eval_time_.End();
+ ASSERT_EXCEPTION(success && IsFiniteNumber(unscaled_c->Nrm2()),
+ Eval_Error, "Error evaluating the equality constraints");
+ retValue = NLP_scaling()->apply_vector_scaling_c(ConstPtr(unscaled_c));
+ c_cache_.AddCachedResult1Dep(retValue, x);
+ }
+ }
+
+ return retValue;
+ }
+
+ SmartPtr<const Vector> OrigIpoptNLP::d(const Vector& x)
+ {
+ DBG_START_METH("OrigIpoptNLP::d", dbg_verbosity);
+ SmartPtr<const Vector> retValue;
+ if (d_space_->Dim()==0) {
+ // We do this caching of an empty vector so that the returned
+ // Vector has always the same tag (this might make a difference
+ // in cases where only the constraints are supposed to change...
+ SmartPtr<const Vector> dep = NULL;
+ if (!d_cache_.GetCachedResult1Dep(retValue, GetRawPtr(dep))) {
+ retValue = d_space_->MakeNew();
+ d_cache_.AddCachedResult1Dep(retValue, GetRawPtr(dep));
+ }
+ }
+ else {
+ if (!d_cache_.GetCachedResult1Dep(retValue, x)) {
+ d_evals_++;
+ SmartPtr<Vector> unscaled_d = d_space_->MakeNew();
+
+ DBG_PRINT_VECTOR(2, "scaled_x", x);
+ SmartPtr<const Vector> unscaled_x = NLP_scaling()->unapply_vector_scaling_x(&x);
+ d_eval_time_.Start();
+ bool success = nlp_->Eval_d(*unscaled_x, *unscaled_d);
+ d_eval_time_.End();
+ DBG_PRINT_VECTOR(2, "unscaled_d", *unscaled_d);
+ ASSERT_EXCEPTION(success && IsFiniteNumber(unscaled_d->Nrm2()),
+ Eval_Error, "Error evaluating the inequality constraints");
+ retValue = NLP_scaling()->apply_vector_scaling_d(ConstPtr(unscaled_d));
+ d_cache_.AddCachedResult1Dep(retValue, x);
+ }
+ }
+
+ return retValue;
+ }
+
+ SmartPtr<const Matrix> OrigIpoptNLP::jac_c(const Vector& x)
+ {
+ SmartPtr<const Matrix> retValue;
+ if (c_space_->Dim()==0) {
+ // We do this caching of an empty vector so that the returned
+ // Matrix has always the same tag
+ SmartPtr<const Vector> dep = NULL;
+ if (!jac_c_cache_.GetCachedResult1Dep(retValue, GetRawPtr(dep))) {
+ SmartPtr<Matrix> unscaled_jac_c = jac_c_space_->MakeNew();
+ retValue = NLP_scaling()->apply_jac_c_scaling(ConstPtr(unscaled_jac_c));
+ jac_c_cache_.AddCachedResult1Dep(retValue, GetRawPtr(dep));
+ }
+ }
+ else {
+ if (!jac_c_cache_.GetCachedResult1Dep(retValue, x)) {
+ jac_c_evals_++;
+ SmartPtr<Matrix> unscaled_jac_c = jac_c_space_->MakeNew();
+
+ SmartPtr<const Vector> unscaled_x = NLP_scaling()->unapply_vector_scaling_x(&x);
+ jac_c_eval_time_.Start();
+ bool success = nlp_->Eval_jac_c(*unscaled_x, *unscaled_jac_c);
+ jac_c_eval_time_.End();
+ ASSERT_EXCEPTION(success, Eval_Error, "Error evaluating the jacobian of the equality constraints");
+ if (check_derivatives_for_naninf_) {
+ if (!unscaled_jac_c->HasValidNumbers()) {
+ jnlst_->Printf(J_WARNING, J_NLP,
+ "The Jacobian for the equality constraints contains an invalid number\n");
+ THROW_EXCEPTION(Eval_Error, "The Jacobian for the equality constraints contains an invalid number");
+ }
+ }
+ retValue = NLP_scaling()->apply_jac_c_scaling(ConstPtr(unscaled_jac_c));
+ jac_c_cache_.AddCachedResult1Dep(retValue, x);
+ }
+ }
+
+ return retValue;
+ }
+
+ SmartPtr<const Matrix> OrigIpoptNLP::jac_d(const Vector& x)
+ {
+ SmartPtr<const Matrix> retValue;
+ if (d_space_->Dim()==0) {
+ // We do this caching of an empty vector so that the returned
+ // Matrix has always the same tag
+ SmartPtr<const Vector> dep = NULL;
+ if (!jac_d_cache_.GetCachedResult1Dep(retValue, GetRawPtr(dep))) {
+ SmartPtr<Matrix> unscaled_jac_d = jac_d_space_->MakeNew();
+ retValue = NLP_scaling()->apply_jac_d_scaling(ConstPtr(unscaled_jac_d));
+ jac_d_cache_.AddCachedResult1Dep(retValue, GetRawPtr(dep));
+ }
+ }
+ else {
+ if (!jac_d_cache_.GetCachedResult1Dep(retValue, x)) {
+ jac_d_evals_++;
+ SmartPtr<Matrix> unscaled_jac_d = jac_d_space_->MakeNew();
+
+ SmartPtr<const Vector> unscaled_x = NLP_scaling()->unapply_vector_scaling_x(&x);
+ jac_d_eval_time_.Start();
+ bool success = nlp_->Eval_jac_d(*unscaled_x, *unscaled_jac_d);
+ jac_d_eval_time_.End();
+ ASSERT_EXCEPTION(success, Eval_Error, "Error evaluating the jacobian of the inequality constraints");
+ retValue = NLP_scaling()->apply_jac_d_scaling(ConstPtr(unscaled_jac_d));
+ jac_d_cache_.AddCachedResult1Dep(retValue, x);
+ }
+ }
+
+ return retValue;
+ }
+
+ SmartPtr<const SymMatrix> OrigIpoptNLP::uninitialized_h()
+ {
+ return h_space_->MakeNewSymMatrix();
+ }
+
+ SmartPtr<const SymMatrix> OrigIpoptNLP::h(const Vector& x,
+ Number obj_factor,
+ const Vector& yc,
+ const Vector& yd)
+ {
+ std::vector<const TaggedObject*> deps(3);
+ deps[0] = &x;
+ deps[1] = &yc;
+ deps[2] = &yd;
+ std::vector<Number> scalar_deps(1);
+ scalar_deps[0] = obj_factor;
+
+ SmartPtr<SymMatrix> unscaled_h;
+ SmartPtr<const SymMatrix> retValue;
+ if (!h_cache_.GetCachedResult(retValue, deps, scalar_deps)) {
+ h_evals_++;
+ unscaled_h = h_space_->MakeNewSymMatrix();
+
+ SmartPtr<const Vector> unscaled_x = NLP_scaling()->unapply_vector_scaling_x(&x);
+ SmartPtr<const Vector> unscaled_yc = NLP_scaling()->apply_vector_scaling_c(&yc);
+ SmartPtr<const Vector> unscaled_yd = NLP_scaling()->apply_vector_scaling_d(&yd);
+ Number scaled_obj_factor = NLP_scaling()->apply_obj_scaling(obj_factor);
+ h_eval_time_.Start();
+ bool success = nlp_->Eval_h(*unscaled_x, scaled_obj_factor, *unscaled_yc, *unscaled_yd, *unscaled_h);
+ h_eval_time_.End();
+ ASSERT_EXCEPTION(success, Eval_Error, "Error evaluating the hessian of the lagrangian");
+ retValue = NLP_scaling()->apply_hessian_scaling(ConstPtr(unscaled_h));
+ h_cache_.AddCachedResult(retValue, deps, scalar_deps);
+ }
+
+ return retValue;
+ }
+
+ SmartPtr<const SymMatrix> OrigIpoptNLP::h(const Vector& x,
+ Number obj_factor,
+ const Vector& yc,
+ const Vector& yd,
+ Number mu)
+ {
+ THROW_EXCEPTION(INTERNAL_ABORT,
+ "ERROR: This method is only a for h(mu) and should not be called");
+ return NULL;
+ }
+
+
+ void OrigIpoptNLP::GetSpaces(SmartPtr<const VectorSpace>& x_space,
+ SmartPtr<const VectorSpace>& c_space,
+ SmartPtr<const VectorSpace>& d_space,
+ SmartPtr<const VectorSpace>& x_l_space,
+ SmartPtr<const MatrixSpace>& px_l_space,
+ SmartPtr<const VectorSpace>& x_u_space,
+ SmartPtr<const MatrixSpace>& px_u_space,
+ SmartPtr<const VectorSpace>& d_l_space,
+ SmartPtr<const MatrixSpace>& pd_l_space,
+ SmartPtr<const VectorSpace>& d_u_space,
+ SmartPtr<const MatrixSpace>& pd_u_space,
+ SmartPtr<const MatrixSpace>& Jac_c_space,
+ SmartPtr<const MatrixSpace>& Jac_d_space,
+ SmartPtr<const SymMatrixSpace>& Hess_lagrangian_space)
+ {
+ // Make sure that we already have all the pointers
+ DBG_ASSERT(IsValid(x_space_) &&
+ IsValid(c_space_) &&
+ IsValid(d_space_) &&
+ IsValid(x_l_space_) &&
+ IsValid(px_l_space_) &&
+ IsValid(x_u_space_) &&
+ IsValid(px_u_space_) &&
+ IsValid(d_l_space_) &&
+ IsValid(pd_l_space_) &&
+ IsValid(d_u_space_) &&
+ IsValid(pd_u_space_) &&
+ IsValid(scaled_jac_c_space_) &&
+ IsValid(scaled_jac_d_space_) &&
+ IsValid(scaled_h_space_));
+
+ DBG_ASSERT(IsValid(NLP_scaling()));
+
+ x_space = x_space_;
+ c_space = c_space_;
+ d_space = d_space_;
+ x_l_space = x_l_space_;
+ px_l_space = px_l_space_;
+ x_u_space = x_u_space_;
+ px_u_space = px_u_space_;
+ d_l_space = d_l_space_;
+ pd_l_space = pd_l_space_;
+ d_u_space = d_u_space_;
+ pd_u_space = pd_u_space_;
+ Jac_c_space = scaled_jac_c_space_;
+ Jac_d_space = scaled_jac_d_space_;
+ Hess_lagrangian_space = scaled_h_space_;
+ }
+
+ void OrigIpoptNLP::FinalizeSolution(SolverReturn status,
+ const Vector& x, const Vector& z_L, const Vector& z_U,
+ const Vector& c, const Vector& d,
+ const Vector& y_c, const Vector& y_d,
+ Number obj_value)
+ {
+ DBG_START_METH("OrigIpoptNLP::FinalizeSolution", dbg_verbosity);
+ // need to submit the unscaled solution back to the nlp
+ SmartPtr<const Vector> unscaled_x =
+ NLP_scaling()->unapply_vector_scaling_x(&x);
+ SmartPtr<const Vector> unscaled_c =
+ NLP_scaling()->unapply_vector_scaling_c(&c);
+ SmartPtr<const Vector> unscaled_d =
+ NLP_scaling()->unapply_vector_scaling_d(&d);
+ const Number unscaled_obj = NLP_scaling()->unapply_obj_scaling(obj_value);
+
+ SmartPtr<const Vector> unscaled_z_L;
+ SmartPtr<const Vector> unscaled_z_U;
+ SmartPtr<const Vector> unscaled_y_c;
+ SmartPtr<const Vector> unscaled_y_d;
+
+ // The objective function scaling factor also appears in the constraints
+ Number obj_unscale_factor = NLP_scaling()->unapply_obj_scaling(1.);
+ if (obj_unscale_factor!=1.) {
+ SmartPtr<Vector> tmp = NLP_scaling()->apply_vector_scaling_x_LU_NonConst(*Px_L_, &z_L, *x_space_);
+ tmp->Scal(obj_unscale_factor);
+ unscaled_z_L = ConstPtr(tmp);
+
+ tmp = NLP_scaling()->apply_vector_scaling_x_LU_NonConst(*Px_U_, &z_U, *x_space_);
+ tmp->Scal(obj_unscale_factor);
+ unscaled_z_U = ConstPtr(tmp);
+
+ tmp = NLP_scaling()->apply_vector_scaling_c_NonConst(&y_c);
+ tmp->Scal(obj_unscale_factor);
+ unscaled_y_c = ConstPtr(tmp);
+
+ tmp = NLP_scaling()->apply_vector_scaling_d_NonConst(&y_d);
+ tmp->Scal(obj_unscale_factor);
+ unscaled_y_d = ConstPtr(tmp);
+ }
+ else {
+ unscaled_z_L = NLP_scaling()->apply_vector_scaling_x_LU(*Px_L_, &z_L, *x_space_);
+ unscaled_z_U = NLP_scaling()->apply_vector_scaling_x_LU(*Px_U_, &z_U, *x_space_);
+ unscaled_y_c = NLP_scaling()->apply_vector_scaling_c(&y_c);
+ unscaled_y_d = NLP_scaling()->apply_vector_scaling_d(&y_d);
+ }
+
+ if (honor_original_bounds_ && (Px_L_->NCols()>0 || Px_U_->NCols()>0)) {
+ // Make sure the user specified bounds are satisfied
+ SmartPtr<Vector> tmp;
+ SmartPtr<Vector> un_x = unscaled_x->MakeNewCopy();
+ if (Px_L_->NCols()>0) {
+ tmp = orig_x_L_->MakeNewCopy();
+ Px_L_->TransMultVector(1., *un_x, 0., *tmp);
+ Px_L_->MultVector(-1., *tmp, 1., *un_x);
+ tmp->ElementWiseMax(*orig_x_L_);
+ Px_L_->MultVector(1., *tmp, 1., *un_x);
+ }
+ if (Px_U_->NCols()>0) {
+ tmp = orig_x_U_->MakeNewCopy();
+ Px_U_->TransMultVector(1., *un_x, 0., *tmp);
+ Px_U_->MultVector(-1., *tmp, 1., *un_x);
+ tmp->ElementWiseMin(*orig_x_U_);
+ Px_U_->MultVector(1., *tmp, 1., *un_x);
+ }
+ unscaled_x = ConstPtr(un_x);
+ }
+
+ unscaled_x->Print(*jnlst_, J_VECTOR, J_SOLUTION, "final x unscaled");
+ unscaled_y_c->Print(*jnlst_, J_VECTOR, J_SOLUTION, "final y_c unscaled");
+ unscaled_y_d->Print(*jnlst_, J_VECTOR, J_SOLUTION, "final y_d unscaled");
+ unscaled_z_L->Print(*jnlst_, J_VECTOR, J_SOLUTION, "final z_L unscaled");
+ unscaled_z_U->Print(*jnlst_, J_VECTOR, J_SOLUTION, "final z_U unscaled");
+
+ nlp_->FinalizeSolution(status, *unscaled_x,
+ *unscaled_z_L, *unscaled_z_U,
+ *unscaled_c, *unscaled_d,
+ *unscaled_y_c, *unscaled_y_d,
+ unscaled_obj);
+ }
+
+ bool OrigIpoptNLP::IntermediateCallBack(AlgorithmMode mode,
+ Index iter, Number obj_value,
+ Number inf_pr, Number inf_du,
+ Number mu, Number d_norm,
+ Number regularization_size,
+ Number alpha_du, Number alpha_pr,
+ Index ls_trials,
+ SmartPtr<const IpoptData> ip_data,
+ SmartPtr<IpoptCalculatedQuantities> ip_cq)
+ {
+ return nlp_->IntermediateCallBack(mode, iter, obj_value, inf_pr, inf_du,
+ mu, d_norm, regularization_size,
+ alpha_du, alpha_pr, ls_trials,
+ GetRawPtr(ip_data), GetRawPtr(ip_cq));
+ }
+
+ void OrigIpoptNLP::AdjustVariableBounds(const Vector& new_x_L, const Vector& new_x_U,
+ const Vector& new_d_L, const Vector& new_d_U)
+ {
+ x_L_ = new_x_L.MakeNewCopy();
+ x_U_ = new_x_U.MakeNewCopy();
+ d_L_ = new_d_L.MakeNewCopy();
+ d_U_ = new_d_U.MakeNewCopy();
+ }
+
+ void
+ OrigIpoptNLP::PrintTimingStatistics(
+ Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category) const
+ {
+ if (!jnlst.ProduceOutput(level, category))
+ return;
+
+ jnlst.Printf(level, category,
+ "Function Evaluations................: %10.3f\n",
+ TotalFunctionEvaluationCPUTime());
+ jnlst.Printf(level, category,
+ " Objective function.................: %10.3f\n",
+ f_eval_time_.TotalTime());
+ jnlst.Printf(level, category,
+ " Equality constraints...............: %10.3f\n",
+ c_eval_time_.TotalTime());
+ jnlst.Printf(level, category,
+ " Inequality constraints.............: %10.3f\n",
+ d_eval_time_.TotalTime());
+ jnlst.Printf(level, category,
+ " Equality constraint Jacobian.......: %10.3f\n",
+ jac_c_eval_time_.TotalTime());
+ jnlst.Printf(level, category,
+ " Inequality constraint Jacobian.....: %10.3f\n",
+ jac_d_eval_time_.TotalTime());
+ jnlst.Printf(level, category,
+ " Lagrangian Hessian.................: %10.3f\n",
+ h_eval_time_.TotalTime());
+ }
+
+ Number
+ OrigIpoptNLP::TotalFunctionEvaluationCPUTime() const
+ {
+ return f_eval_time_.TotalTime()+
+ c_eval_time_.TotalTime()+
+ d_eval_time_.TotalTime()+
+ jac_c_eval_time_.TotalTime()+
+ jac_d_eval_time_.TotalTime()+
+ h_eval_time_.TotalTime();
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpOrigIpoptNLP.hpp b/SimTKmath/Optimizers/src/IpOpt/IpOrigIpoptNLP.hpp
new file mode 100644
index 0000000..ff278af
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpOrigIpoptNLP.hpp
@@ -0,0 +1,422 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpOrigIpoptNLP.hpp 765 2006-07-14 18:03:23Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPORIGIPOPTNLP_HPP__
+#define __IPORIGIPOPTNLP_HPP__
+
+#include "IpIpoptNLP.hpp"
+#include "IpException.hpp"
+#include "IpTimingStatistics.hpp"
+
+namespace Ipopt
+{
+
+ /** enumeration for the Hessian information type. */
+ enum HessianApproximationType {
+ EXACT=0,
+ LIMITED_MEMORY
+ };
+
+ /** This class maps the traditional NLP into
+ * something that is more useful by Ipopt.
+ * This class takes care of storing the
+ * calculated model results, handles cacheing,
+ * and (some day) takes care of addition of slacks.
+ */
+ class OrigIpoptNLP : public IpoptNLP
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ OrigIpoptNLP(const SmartPtr<const Journalist>& jnlst,
+ const SmartPtr<NLP>& nlp,
+ const SmartPtr<NLPScalingObject>& nlp_scaling);
+
+ /** Default destructor */
+ virtual ~OrigIpoptNLP();
+ //@}
+
+ /** Initialize - overloaded from IpoptNLP */
+ virtual bool Initialize(const Journalist& jnlst,
+ const OptionsList& options,
+ const std::string& prefix);
+
+ /** Initialize (create) structures for
+ * the iteration data */
+ virtual bool InitializeStructures(SmartPtr<Vector>& x,
+ bool init_x,
+ SmartPtr<Vector>& y_c,
+ bool init_y_c,
+ SmartPtr<Vector>& y_d,
+ bool init_y_d,
+ SmartPtr<Vector>& z_L,
+ bool init_z_L,
+ SmartPtr<Vector>& z_U,
+ bool init_z_U,
+ SmartPtr<Vector>& v_L,
+ SmartPtr<Vector>& v_U
+ );
+
+ /** Method accessing the GetWarmStartIterate of the NLP */
+ virtual bool GetWarmStartIterate(IteratesVector& warm_start_iterate)
+ {
+ return nlp_->GetWarmStartIterate(warm_start_iterate);
+ }
+ /** Accessor methods for model data */
+ //@{
+ /** Objective value */
+ virtual Number f(const Vector& x);
+
+ /** Objective value (depending in mu) - incorrect version for
+ * OrigIpoptNLP */
+ virtual Number f(const Vector& x, Number mu);
+
+ /** Gradient of the objective */
+ virtual SmartPtr<const Vector> grad_f(const Vector& x);
+
+ /** Gradient of the objective (depending in mu) - incorrect
+ * version for OrigIpoptNLP */
+ virtual SmartPtr<const Vector> grad_f(const Vector& x, Number mu);
+
+ /** Equality constraint residual */
+ virtual SmartPtr<const Vector> c(const Vector& x);
+
+ /** Jacobian Matrix for equality constraints */
+ virtual SmartPtr<const Matrix> jac_c(const Vector& x);
+
+ /** Inequality constraint residual (reformulated
+ * as equalities with slacks */
+ virtual SmartPtr<const Vector> d(const Vector& x);
+
+ /** Jacobian Matrix for inequality constraints*/
+ virtual SmartPtr<const Matrix> jac_d(const Vector& x);
+
+ /** Hessian of the Lagrangian */
+ virtual SmartPtr<const SymMatrix> h(const Vector& x,
+ Number obj_factor,
+ const Vector& yc,
+ const Vector& yd
+ );
+
+ /** Hessian of the Lagrangian (depending in mu) - incorrect
+ * version for OrigIpoptNLP */
+ virtual SmartPtr<const SymMatrix> h(const Vector& x,
+ Number obj_factor,
+ const Vector& yc,
+ const Vector& yd,
+ Number mu);
+
+ /** Provides a Hessian matrix from the correct matrix space with
+ * uninitialized values. This can be used in LeastSquareMults to
+ * obtain a "zero Hessian". */
+ virtual SmartPtr<const SymMatrix> uninitialized_h();
+
+ /** Lower bounds on x */
+ virtual SmartPtr<const Vector> x_L()
+ {
+ return x_L_;
+ }
+
+ /** Permutation matrix (x_L_ -> x) */
+ virtual SmartPtr<const Matrix> Px_L()
+ {
+ return Px_L_;
+ }
+
+ /** Upper bounds on x */
+ virtual SmartPtr<const Vector> x_U()
+ {
+ return x_U_;
+ }
+
+ /** Permutation matrix (x_U_ -> x */
+ virtual SmartPtr<const Matrix> Px_U()
+ {
+ return Px_U_;
+ }
+
+ /** Lower bounds on d */
+ virtual SmartPtr<const Vector> d_L()
+ {
+ return d_L_;
+ }
+
+ /** Permutation matrix (d_L_ -> d) */
+ virtual SmartPtr<const Matrix> Pd_L()
+ {
+ return Pd_L_;
+ }
+
+ /** Upper bounds on d */
+ virtual SmartPtr<const Vector> d_U()
+ {
+ return d_U_;
+ }
+
+ /** Permutation matrix (d_U_ -> d */
+ virtual SmartPtr<const Matrix> Pd_U()
+ {
+ return Pd_U_;
+ }
+
+ virtual SmartPtr<const SymMatrixSpace> HessianMatrixSpace() const
+ {
+ return h_space_;
+ }
+ //@}
+
+ /** Accessor method for vector/matrix spaces pointers */
+ virtual void GetSpaces(SmartPtr<const VectorSpace>& x_space,
+ SmartPtr<const VectorSpace>& c_space,
+ SmartPtr<const VectorSpace>& d_space,
+ SmartPtr<const VectorSpace>& x_l_space,
+ SmartPtr<const MatrixSpace>& px_l_space,
+ SmartPtr<const VectorSpace>& x_u_space,
+ SmartPtr<const MatrixSpace>& px_u_space,
+ SmartPtr<const VectorSpace>& d_l_space,
+ SmartPtr<const MatrixSpace>& pd_l_space,
+ SmartPtr<const VectorSpace>& d_u_space,
+ SmartPtr<const MatrixSpace>& pd_u_space,
+ SmartPtr<const MatrixSpace>& Jac_c_space,
+ SmartPtr<const MatrixSpace>& Jac_d_space,
+ SmartPtr<const SymMatrixSpace>& Hess_lagrangian_space);
+
+ /** Method for adapting the variable bounds. This is called if
+ * slacks are becoming too small */
+ virtual void AdjustVariableBounds(const Vector& new_x_L,
+ const Vector& new_x_U,
+ const Vector& new_d_L,
+ const Vector& new_d_U);
+
+ /** @name Counters for the number of function evaluations. */
+ //@{
+ virtual Index f_evals() const
+ {
+ return f_evals_;
+ }
+ virtual Index grad_f_evals() const
+ {
+ return grad_f_evals_;
+ }
+ virtual Index c_evals() const
+ {
+ return c_evals_;
+ }
+ virtual Index jac_c_evals() const
+ {
+ return jac_c_evals_;
+ }
+ virtual Index d_evals() const
+ {
+ return d_evals_;
+ }
+ virtual Index jac_d_evals() const
+ {
+ return jac_d_evals_;
+ }
+ virtual Index h_evals() const
+ {
+ return h_evals_;
+ }
+ //@}
+
+ /** Solution Routines - overloaded from IpoptNLP*/
+ //@{
+ void FinalizeSolution(SolverReturn status,
+ const Vector& x, const Vector& z_L, const Vector& z_U,
+ const Vector& c, const Vector& d,
+ const Vector& y_c, const Vector& y_d,
+ Number obj_value);
+ bool IntermediateCallBack(AlgorithmMode mode,
+ Index iter, Number obj_value,
+ Number inf_pr, Number inf_du,
+ Number mu, Number d_norm,
+ Number regularization_size,
+ Number alpha_du, Number alpha_pr,
+ Index ls_trials,
+ SmartPtr<const IpoptData> ip_data,
+ SmartPtr<IpoptCalculatedQuantities> ip_cq);
+ //@}
+
+ /** @name Methods for IpoptType */
+ //@{
+ /** Called by IpoptType to register the options */
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ /** Accessor method to the underlying NLP */
+ SmartPtr<NLP> nlp()
+ {
+ return nlp_;
+ }
+
+ void PrintTimingStatistics(Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category) const;
+
+ Number TotalFunctionEvaluationCPUTime() const;
+
+ private:
+ /** journalist */
+ SmartPtr<const Journalist> jnlst_;
+
+ /** Pointer to the NLP */
+ SmartPtr<NLP> nlp_;
+
+ /** Necessary Vector/Matrix spaces */
+ //@{
+ SmartPtr<const VectorSpace> x_space_;
+ SmartPtr<const VectorSpace> c_space_;
+ SmartPtr<const VectorSpace> d_space_;
+ SmartPtr<const VectorSpace> x_l_space_;
+ SmartPtr<const MatrixSpace> px_l_space_;
+ SmartPtr<const VectorSpace> x_u_space_;
+ SmartPtr<const MatrixSpace> px_u_space_;
+ SmartPtr<const VectorSpace> d_l_space_;
+ SmartPtr<const MatrixSpace> pd_l_space_;
+ SmartPtr<const VectorSpace> d_u_space_;
+ SmartPtr<const MatrixSpace> pd_u_space_;
+ SmartPtr<const MatrixSpace> jac_c_space_;
+ SmartPtr<const MatrixSpace> jac_d_space_;
+ SmartPtr<const SymMatrixSpace> h_space_;
+
+ SmartPtr<const MatrixSpace> scaled_jac_c_space_;
+ SmartPtr<const MatrixSpace> scaled_jac_d_space_;
+ SmartPtr<const SymMatrixSpace> scaled_h_space_;
+ //@}
+ /**@name Storage for Model Quantities */
+ //@{
+ /** Objective function */
+ CachedResults<Number> f_cache_;
+
+ /** Gradient of the objective function */
+ CachedResults<SmartPtr<const Vector> > grad_f_cache_;
+
+ /** Equality constraint residuals */
+ CachedResults<SmartPtr<const Vector> > c_cache_;
+
+ /** Jacobian Matrix for equality constraints
+ * (current iteration) */
+ CachedResults<SmartPtr<const Matrix> > jac_c_cache_;
+
+ /** Inequality constraint residual (reformulated
+ * as equalities with slacks */
+ CachedResults<SmartPtr<const Vector> > d_cache_;
+
+ /** Jacobian Matrix for inequality constraints
+ * (current iteration) */
+ CachedResults<SmartPtr<const Matrix> > jac_d_cache_;
+
+ /** Hessian of the lagrangian
+ * (current iteration) */
+ CachedResults<SmartPtr<const SymMatrix> > h_cache_;
+
+ /** Lower bounds on x */
+ SmartPtr<const Vector> x_L_;
+
+ /** Permutation matrix (x_L_ -> x) */
+ SmartPtr<const Matrix> Px_L_;
+
+ /** Upper bounds on x */
+ SmartPtr<const Vector> x_U_;
+
+ /** Permutation matrix (x_U_ -> x */
+ SmartPtr<const Matrix> Px_U_;
+
+ /** Lower bounds on d */
+ SmartPtr<const Vector> d_L_;
+
+ /** Permutation matrix (d_L_ -> d) */
+ SmartPtr<const Matrix> Pd_L_;
+
+ /** Upper bounds on d */
+ SmartPtr<const Vector> d_U_;
+
+ /** Permutation matrix (d_U_ -> d */
+ SmartPtr<const Matrix> Pd_U_;
+
+ /** Original unmodified lower bounds on x */
+ SmartPtr<const Vector> orig_x_L_;
+
+ /** Original unmodified upper bounds on x */
+ SmartPtr<const Vector> orig_x_U_;
+ //@}
+
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ OrigIpoptNLP();
+
+ /** Copy Constructor */
+ OrigIpoptNLP(const OrigIpoptNLP&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const OrigIpoptNLP&);
+ //@}
+
+ /** @name auxilliary functions */
+ //@{
+ /** relax the bounds by a relative move of relax_bound_factor.
+ * Here, relax_bound_factor should be negative (or zero) for
+ * lower bounds, and positive (or zero) for upper bounds.
+ */
+ void relax_bounds(Number bound_relax_factor, Vector& bounds);
+ //@}
+
+ /** @name Algorithmic parameters */
+ //@{
+ /** relaxation factor for the bounds */
+ Number bound_relax_factor_;
+ /** Flag indicating whether the primal variables should be
+ * projected back into original bounds are optimization. */
+ bool honor_original_bounds_;
+ /** Flag indicating whether the TNLP with identical structure has
+ * already been solved before. */
+ bool warm_start_same_structure_;
+ /** Flag indicating what Hessian information is to be used. */
+ HessianApproximationType hessian_approximation_;
+ /** Flag indicating whether it is desired to check if there are
+ * Nan or Inf entries in first and second derivative matrices. */
+ bool check_derivatives_for_naninf_;
+ //@}
+
+ /** @name Counters for the function evaluations */
+ //@{
+ Index f_evals_;
+ Index grad_f_evals_;
+ Index c_evals_;
+ Index jac_c_evals_;
+ Index d_evals_;
+ Index jac_d_evals_;
+ Index h_evals_;
+ //@}
+
+ /** Flag indicating if initialization method has been called */
+ bool initialized_;
+
+ /**@name Timing statistics for the function evaluations. */
+ //@{
+ TimedTask f_eval_time_;
+ TimedTask grad_f_eval_time_;
+ TimedTask c_eval_time_;
+ TimedTask jac_c_eval_time_;
+ TimedTask d_eval_time_;
+ TimedTask jac_d_eval_time_;
+ TimedTask h_eval_time_;
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpOrigIterationOutput.cpp b/SimTKmath/Optimizers/src/IpOpt/IpOrigIterationOutput.cpp
new file mode 100644
index 0000000..c9cbfce
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpOrigIterationOutput.cpp
@@ -0,0 +1,247 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpOrigIterationOutput.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-09-23
+
+#include "IpOrigIterationOutput.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+#include <cstdio>
+
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+
+
+
+namespace Ipopt
+{
+ OrigIterationOutput::OrigIterationOutput()
+ {}
+
+ OrigIterationOutput::~OrigIterationOutput()
+ {}
+
+ void
+ OrigIterationOutput::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ std::string prev_cat = roptions->RegisteringCategory();
+ roptions->SetRegisteringCategory("Output");
+ roptions->AddStringOption2(
+ "print_info_string",
+ "Enables printing of additional info string at end of iteration output.",
+ "no",
+ "no", "don't print string",
+ "yes", "print string at end of each iteration output",
+ "This string contains some insider information about the current iteration.");
+ roptions->SetRegisteringCategory(prev_cat);
+ }
+
+ bool OrigIterationOutput::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ options.GetBoolValue("print_info_string", print_info_string_, prefix);
+
+ return true;
+ }
+
+ void OrigIterationOutput::WriteOutput()
+ {
+ //////////////////////////////////////////////////////////////////////
+ // First print the summary line for the iteration //
+ //////////////////////////////////////////////////////////////////////
+
+ Index iter = IpData().iter_count();
+ std::string header =
+ "iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls\n";
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "\n\n**************************************************\n");
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "*** Summary of Iteration: %d:", IpData().iter_count());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "\n**************************************************\n\n");
+ if (iter%10 == 0 && !IpData().info_skip_output()) {
+ // output the header
+ Jnlst().Printf(J_ITERSUMMARY, J_MAIN, header.c_str());
+ }
+ else {
+ Jnlst().Printf(J_DETAILED, J_MAIN, header.c_str());
+ }
+ Number inf_pr = IpCq().curr_primal_infeasibility(NORM_MAX);
+ Number inf_du = IpCq().curr_dual_infeasibility(NORM_MAX);
+ Number mu = IpData().curr_mu();
+ Number dnrm;
+ if (IsValid(IpData().delta()) && IsValid(IpData().delta()->x()) && IsValid(IpData().delta()->s())) {
+ dnrm = Max(IpData().delta()->x()->Amax(), IpData().delta()->s()->Amax());
+ }
+ else {
+ // This is the first iteration - no search direction has been
+ // computed yet.
+ dnrm = 0.;
+ }
+ Number unscaled_f = IpCq().unscaled_curr_f();
+
+ // Retrieve some information set in the different parts of the algorithm
+ char info_iter=' ';
+ Number alpha_primal = IpData().info_alpha_primal();
+ char alpha_primal_char = IpData().info_alpha_primal_char();
+ Number alpha_dual = IpData().info_alpha_dual();
+ Number regu_x = IpData().info_regu_x();
+ char regu_x_buf[8];
+ char dashes[]=" - ";
+ char *regu_x_ptr;
+ if (regu_x==.0) {
+ regu_x_ptr = dashes;
+ }
+ else {
+ sprintf(regu_x_buf, "%5.1f", log10(regu_x));
+ regu_x_ptr = regu_x_buf;
+ }
+ Index ls_count = IpData().info_ls_count();
+ const std::string info_string = IpData().info_string();
+
+ if (!IpData().info_skip_output()) {
+ Jnlst().Printf(J_ITERSUMMARY, J_MAIN,
+ "%4d%c%14.7e %7.2e %7.2e %5.1f %7.2e %5s %7.2e %7.2e%c%3d",
+ iter, info_iter, unscaled_f, inf_pr, inf_du, log10(mu), dnrm, regu_x_ptr,
+ alpha_dual, alpha_primal, alpha_primal_char,
+ ls_count);
+ if (print_info_string_) {
+ Jnlst().Printf(J_ITERSUMMARY, J_MAIN, " %s", info_string.c_str());
+ }
+ else {
+// TODO UNCOMMENT WHEN DONE TESTING
+ Jnlst().Printf(J_DETAILED, J_MAIN, " %s", info_string.c_str());
+//printf(" %s",info_string.c_str());
+ }
+ Jnlst().Printf(J_ITERSUMMARY, J_MAIN, "\n");
+ }
+
+
+ //////////////////////////////////////////////////////////////////////
+ // Now if desired more detail on the iterates //
+ //////////////////////////////////////////////////////////////////////
+
+ if (Jnlst().ProduceOutput(J_DETAILED, J_MAIN)) {
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "\n**************************************************\n");
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "*** Beginning Iteration %d from the following point:",
+ IpData().iter_count());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "\n**************************************************\n\n");
+
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "Current barrier parameter mu = %21.16e\n", IpData().curr_mu());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "Current fraction-to-the-boundary parameter tau = %21.16e\n\n",
+ IpData().curr_tau());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "||curr_x||_inf = %.16e\n", IpData().curr()->x()->Amax());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "||curr_s||_inf = %.16e\n", IpData().curr()->s()->Amax());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "||curr_y_c||_inf = %.16e\n", IpData().curr()->y_c()->Amax());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "||curr_y_d||_inf = %.16e\n", IpData().curr()->y_d()->Amax());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "||curr_z_L||_inf = %.16e\n", IpData().curr()->z_L()->Amax());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "||curr_z_U||_inf = %.16e\n", IpData().curr()->z_U()->Amax());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "||curr_v_L||_inf = %.16e\n", IpData().curr()->v_L()->Amax());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "||curr_v_U||_inf = %.16e\n", IpData().curr()->v_U()->Amax());
+ }
+ if (Jnlst().ProduceOutput(J_MOREDETAILED, J_MAIN)) {
+ if (IsValid(IpData().delta())) {
+ Jnlst().Printf(J_MOREDETAILED, J_MAIN,
+ "\n||delta_x||_inf = %.16e\n", IpData().delta()->x()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_MAIN,
+ "||delta_s||_inf = %.16e\n", IpData().delta()->s()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_MAIN,
+ "||delta_y_c||_inf = %.16e\n", IpData().delta()->y_c()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_MAIN,
+ "||delta_y_d||_inf = %.16e\n", IpData().delta()->y_d()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_MAIN,
+ "||delta_z_L||_inf = %.16e\n", IpData().delta()->z_L()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_MAIN,
+ "||delta_z_U||_inf = %.16e\n", IpData().delta()->z_U()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_MAIN,
+ "||delta_v_L||_inf = %.16e\n", IpData().delta()->v_L()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_MAIN,
+ "||delta_v_U||_inf = %.16e\n", IpData().delta()->v_U()->Amax());
+ }
+ else {
+ Jnlst().Printf(J_MOREDETAILED, J_MAIN,
+ "\nNo search direction has been computed yet.\n");
+ }
+ }
+ if (Jnlst().ProduceOutput(J_VECTOR, J_MAIN)) {
+ IpData().curr()->x()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_x");
+ IpData().curr()->s()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_s");
+
+ IpData().curr()->y_c()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_y_c");
+ IpData().curr()->y_d()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_y_d");
+
+ IpCq().curr_slack_x_L()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_slack_x_L");
+ IpCq().curr_slack_x_U()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_slack_x_U");
+ IpData().curr()->z_L()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_z_L");
+ IpData().curr()->z_U()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_z_U");
+
+ IpCq().curr_slack_s_L()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_slack_s_L");
+ IpCq().curr_slack_s_U()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_slack_s_U");
+ IpData().curr()->v_L()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_v_L");
+ IpData().curr()->v_U()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_v_U");
+ }
+ if (Jnlst().ProduceOutput(J_MOREVECTOR, J_MAIN)) {
+ IpCq().curr_grad_lag_x()->Print(Jnlst(), J_MOREVECTOR, J_MAIN, "curr_grad_lag_x");
+ IpCq().curr_grad_lag_s()->Print(Jnlst(), J_MOREVECTOR, J_MAIN, "curr_grad_lag_s");
+ if (IsValid(IpData().delta())) {
+ IpData().delta()->Print(Jnlst(), J_MOREVECTOR, J_MAIN, "delta");
+ }
+ }
+
+ if (Jnlst().ProduceOutput(J_DETAILED, J_MAIN)) {
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "\n\n***Current NLP Values for Iteration %d:\n",
+ IpData().iter_count());
+ Jnlst().Printf(J_DETAILED, J_MAIN, "\n (scaled) (unscaled)\n");
+ Jnlst().Printf(J_DETAILED, J_MAIN, "Objective...............: %24.16e %24.16e\n", IpCq().curr_f(), IpCq().unscaled_curr_f());
+ Jnlst().Printf(J_DETAILED, J_MAIN, "Dual infeasibility......: %24.16e %24.16e\n", IpCq().curr_dual_infeasibility(NORM_MAX), IpCq().unscaled_curr_dual_infeasibility(NORM_MAX));
+ Jnlst().Printf(J_DETAILED, J_MAIN, "Constraint violation....: %24.16e %24.16e\n", IpCq().curr_nlp_constraint_violation(NORM_MAX), IpCq().unscaled_curr_nlp_constraint_violation(NORM_MAX));
+ Jnlst().Printf(J_DETAILED, J_MAIN, "Complementarity.........: %24.16e %24.16e\n", IpCq().curr_complementarity(0., NORM_MAX), IpCq().unscaled_curr_complementarity(0., NORM_MAX));
+ Jnlst().Printf(J_DETAILED, J_MAIN, "Overall NLP error.......: %24.16e %24.16e\n\n", IpCq().curr_nlp_error(), IpCq().unscaled_curr_nlp_error());
+ }
+ if (Jnlst().ProduceOutput(J_VECTOR, J_MAIN)) {
+ IpCq().curr_grad_f()->Print(Jnlst(), J_VECTOR, J_MAIN, "grad_f");
+ IpCq().curr_c()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_c");
+ IpCq().curr_d()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_d");
+ IpCq().curr_d_minus_s()->Print(Jnlst(), J_VECTOR, J_MAIN,
+ "curr_d - curr_s");
+ }
+
+ if (Jnlst().ProduceOutput(J_MATRIX, J_MAIN)) {
+ IpCq().curr_jac_c()->Print(Jnlst(), J_MATRIX, J_MAIN, "jac_c");
+ IpCq().curr_jac_d()->Print(Jnlst(), J_MATRIX, J_MAIN, "jac_d");
+ IpData().W()->Print(Jnlst(), J_MATRIX, J_MAIN, "W");
+ }
+
+ Jnlst().Printf(J_DETAILED, J_MAIN, "\n\n");
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpOrigIterationOutput.hpp b/SimTKmath/Optimizers/src/IpOpt/IpOrigIterationOutput.hpp
new file mode 100644
index 0000000..880f53e
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpOrigIterationOutput.hpp
@@ -0,0 +1,66 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpOrigIterationOutput.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter, Carl Laird IBM 2004-09-27
+
+#ifndef __IPORIGITERATIONOUTPUT_HPP__
+#define __IPORIGITERATIONOUTPUT_HPP__
+
+#include "IpIterationOutput.hpp"
+
+namespace Ipopt
+{
+
+ /** Class for the iteration summary output for the original NLP.
+ */
+ class OrigIterationOutput: public IterationOutput
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor */
+ OrigIterationOutput();
+
+ /** Default destructor */
+ virtual ~OrigIterationOutput();
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Method to do all the summary output per iteration. This
+ * include the one-line summary output as well as writing the
+ * details about the iterates if desired */
+ virtual void WriteOutput();
+
+ /** Methods for OptionsList */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods (Hidden to avoid
+ * implicit creation/calling). These methods are not implemented
+ * and we do not want the compiler to implement them for us, so we
+ * declare them private and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ OrigIterationOutput(const OrigIterationOutput&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const OrigIterationOutput&);
+ //@}
+
+ /** Flag indicating weather info string should be printed at end
+ * of iteration summary line. */
+ bool print_info_string_;
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpPDFullSpaceSolver.cpp b/SimTKmath/Optimizers/src/IpOpt/IpPDFullSpaceSolver.cpp
new file mode 100644
index 0000000..fbe5eb7
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpPDFullSpaceSolver.cpp
@@ -0,0 +1,720 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpPDFullSpaceSolver.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpPDFullSpaceSolver.hpp"
+#include "IpDebug.hpp"
+
+namespace Ipopt
+{
+
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ PDFullSpaceSolver::PDFullSpaceSolver(AugSystemSolver& augSysSolver,
+ PDPerturbationHandler& perturbHandler)
+ :
+ PDSystemSolver(),
+ augSysSolver_(&augSysSolver),
+ perturbHandler_(&perturbHandler),
+ dummy_cache_(1)
+ {
+ DBG_START_METH("PDFullSpaceSolver::PDFullSpaceSolver",dbg_verbosity);
+ }
+
+ PDFullSpaceSolver::~PDFullSpaceSolver()
+ {
+ DBG_START_METH("PDFullSpaceSolver::~PDFullSpaceSolver()",dbg_verbosity);
+ }
+
+ void PDFullSpaceSolver::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->AddLowerBoundedIntegerOption(
+ "min_refinement_steps",
+ "Minimum number of iterative refinement steps per linear system solve.",
+ 0, 1,
+ "Iterative refinement (on the full unsymmetric system) is performed for "
+ "each right hand side. This option determines the minimum number "
+ "of iterative refinements (i.e. at least \"min_refinement_steps\" "
+ "iterative refinement steps are enforced per right hand side.)");
+ roptions->AddLowerBoundedIntegerOption(
+ "max_refinement_steps",
+ "Maximum number of iterative refinement steps per linear system solve.",
+ 0, 10,
+ "Iterative refinement (on the full unsymmetric system) is performed for "
+ "each right hand side. This option determines the maximum number "
+ "of iterative refinement steps.");
+ roptions->AddLowerBoundedNumberOption(
+ "residual_ratio_max",
+ "Iterative refinement tolerance",
+ 0.0, true, 1e-10,
+ "Iterative refinement is performed until the residual test ratio is "
+ "less than this tolerance (or until \"max_refinement_steps\" refinement "
+ "steps are performed).");
+ roptions->AddLowerBoundedNumberOption(
+ "residual_ratio_singular",
+ "Threshold for declaring linear system singular after failed iterative refinement.",
+ 0.0, true, 1e-5,
+ "If the residual test ratio is larger than this value after failed "
+ "iterative refinement, the algorithm pretends that the linear system is "
+ "singular.");
+ // ToDo Think about following option - are the correct norms used?
+ roptions->AddLowerBoundedNumberOption(
+ "residual_improvement_factor",
+ "Minimal required reduction of residual test ratio in iterative refinement.",
+ 0.0, true, 0.999999999,
+ "If the improvement of the residual test ratio made by one iterative "
+ "refinement step is not better than this factor, iterative refinement "
+ "is aborted.");
+ }
+
+
+ bool PDFullSpaceSolver::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ // Check for the algorithm options
+ options.GetIntegerValue("min_refinement_steps", min_refinement_steps_, prefix);
+ options.GetIntegerValue("max_refinement_steps", max_refinement_steps_, prefix);
+ ASSERT_EXCEPTION(max_refinement_steps_ >= min_refinement_steps_, OPTION_INVALID,
+ "Option \"max_refinement_steps\": This value must be larger than or equal to min_refinement_steps (default 1)");
+
+ options.GetNumericValue("residual_ratio_max", residual_ratio_max_, prefix);
+ options.GetNumericValue("residual_ratio_singular", residual_ratio_singular_, prefix);
+ ASSERT_EXCEPTION(residual_ratio_singular_ >= residual_ratio_max_, OPTION_INVALID,
+ "Option \"residual_ratio_singular\": This value must be not smaller than residual_ratio_max.");
+ options.GetNumericValue("residual_improvement_factor", residual_improvement_factor_, prefix);
+
+ // Reset internal flags and data
+ augsys_improved_ = false;
+
+ if (!augSysSolver_->Initialize(Jnlst(), IpNLP(), IpData(), IpCq(),
+ options, prefix)) {
+ return false;
+ }
+
+ return perturbHandler_->Initialize(Jnlst(), IpNLP(), IpData(), IpCq(),
+ options, prefix);
+ }
+
+ bool PDFullSpaceSolver::Solve(Number alpha,
+ Number beta,
+ const IteratesVector& rhs,
+ IteratesVector& res,
+ bool allow_inexact,
+ bool improve_solution /* = false */)
+ {
+ DBG_START_METH("PDFullSpaceSolver::Solve",dbg_verbosity);
+ DBG_ASSERT(!allow_inexact || !improve_solution);
+ DBG_ASSERT(!improve_solution || beta==0.);
+
+ // Timing of PDSystem solver starts here
+ IpData().TimingStats().PDSystemSolverTotal().Start();
+
+ DBG_PRINT_VECTOR(2, "rhs_x", *rhs.x());
+ DBG_PRINT_VECTOR(2, "rhs_s", *rhs.s());
+ DBG_PRINT_VECTOR(2, "rhs_c", *rhs.y_c());
+ DBG_PRINT_VECTOR(2, "rhs_d", *rhs.y_d());
+ DBG_PRINT_VECTOR(2, "rhs_zL", *rhs.z_L());
+ DBG_PRINT_VECTOR(2, "rhs_zU", *rhs.z_U());
+ DBG_PRINT_VECTOR(2, "rhs_vL", *rhs.v_L());
+ DBG_PRINT_VECTOR(2, "rhs_vU", *rhs.v_U());
+ DBG_PRINT_VECTOR(2, "res_x in", *res.x());
+ DBG_PRINT_VECTOR(2, "res_s in", *res.s());
+ DBG_PRINT_VECTOR(2, "res_c in", *res.y_c());
+ DBG_PRINT_VECTOR(2, "res_d in", *res.y_d());
+ DBG_PRINT_VECTOR(2, "res_zL in", *res.z_L());
+ DBG_PRINT_VECTOR(2, "res_zU in", *res.z_U());
+ DBG_PRINT_VECTOR(2, "res_vL in", *res.v_L());
+ DBG_PRINT_VECTOR(2, "res_vU in", *res.v_U());
+
+ // if beta is nonzero, keep a copy of the incoming values in res_ */
+ SmartPtr<IteratesVector> copy_res;
+ if (beta != 0.) {
+ copy_res = res.MakeNewIteratesVectorCopy();
+ }
+
+ // Receive data about matrix
+ SmartPtr<const Vector> x = IpData().curr()->x();
+ SmartPtr<const Vector> s = IpData().curr()->s();
+ SmartPtr<const SymMatrix> W = IpData().W();
+ SmartPtr<const Matrix> J_c = IpCq().curr_jac_c();
+ SmartPtr<const Matrix> J_d = IpCq().curr_jac_d();
+ SmartPtr<const Matrix> Px_L = IpNLP().Px_L();
+ SmartPtr<const Matrix> Px_U = IpNLP().Px_U();
+ SmartPtr<const Matrix> Pd_L = IpNLP().Pd_L();
+ SmartPtr<const Matrix> Pd_U = IpNLP().Pd_U();
+ SmartPtr<const Vector> z_L = IpData().curr()->z_L();
+ SmartPtr<const Vector> z_U = IpData().curr()->z_U();
+ SmartPtr<const Vector> v_L = IpData().curr()->v_L();
+ SmartPtr<const Vector> v_U = IpData().curr()->v_U();
+ SmartPtr<const Vector> slack_x_L = IpCq().curr_slack_x_L();
+ SmartPtr<const Vector> slack_x_U = IpCq().curr_slack_x_U();
+ SmartPtr<const Vector> slack_s_L = IpCq().curr_slack_s_L();
+ SmartPtr<const Vector> slack_s_U = IpCq().curr_slack_s_U();
+ SmartPtr<const Vector> sigma_x = IpCq().curr_sigma_x();
+ SmartPtr<const Vector> sigma_s = IpCq().curr_sigma_s();
+ DBG_PRINT_VECTOR(2, "Sigma_x", *sigma_x);
+ DBG_PRINT_VECTOR(2, "Sigma_s", *sigma_s);
+
+ bool done = false;
+ // The following flag is set to true, if we asked the linear
+ // solver to improve the quality of the solution in
+ // the next solve
+ bool resolve_with_better_quality = false;
+ // the following flag is set to true, if iterative refinement
+ // failed and we want to try if a modified system is able to
+ // remedy that problem by pretending the matrix is singular
+ bool pretend_singular = false;
+ bool pretend_singular_last_time = false;
+
+ // Beginning of loop for solving the system (including all
+ // modifications for the linear system to ensure good solution
+ // quality)
+ while (!done) {
+
+ // if improve_solution is true, we are given already a solution
+ // from the calling function, so we can skip the first solve
+ bool solve_retval = true;
+ if (!improve_solution) {
+ solve_retval =
+ SolveOnce(resolve_with_better_quality, pretend_singular,
+ *W, *J_c, *J_d, *Px_L, *Px_U, *Pd_L, *Pd_U, *z_L, *z_U,
+ *v_L, *v_U, *slack_x_L, *slack_x_U, *slack_s_L, *slack_s_U,
+ *sigma_x, *sigma_s, 1., 0., rhs, res);
+ resolve_with_better_quality = false;
+ pretend_singular = false;
+ }
+ improve_solution = false;
+
+ if (!solve_retval) {
+ // If system seems not to be solvable, we return with false
+ // and let the calling routine deal with it.
+ IpData().TimingStats().PDSystemSolverTotal().End();
+ return false;
+ }
+
+ if (allow_inexact) {
+ // no safety checks required
+ break;
+ }
+
+ // Get space for the residual
+ SmartPtr<IteratesVector> resid = res.MakeNewIteratesVector(true);
+
+ // ToDo don't to that after max refinement?
+ ComputeResiduals(*W, *J_c, *J_d, *Px_L, *Px_U, *Pd_L, *Pd_U,
+ *z_L, *z_U, *v_L, *v_U, *slack_x_L, *slack_x_U,
+ *slack_s_L, *slack_s_U, *sigma_x, *sigma_s,
+ alpha, beta, rhs, res, *resid);
+
+ Number residual_ratio =
+ ComputeResidualRatio(rhs, res, *resid);
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
+ "residual_ratio = %e\n", residual_ratio);
+ Number residual_ratio_old = residual_ratio;
+
+ // Beginning of loop for iterative refinement
+ Index num_iter_ref = 0;
+ bool quit_refinement = false;
+ while (!allow_inexact && !quit_refinement &&
+ (num_iter_ref < min_refinement_steps_ ||
+ residual_ratio > residual_ratio_max_) ) {
+
+ // To the next back solve
+ solve_retval =
+ SolveOnce(resolve_with_better_quality, false,
+ *W, *J_c, *J_d, *Px_L, *Px_U, *Pd_L, *Pd_U, *z_L, *z_U,
+ *v_L, *v_U, *slack_x_L, *slack_x_U, *slack_s_L, *slack_s_U,
+ *sigma_x, *sigma_s, -1., 1., *resid, res);
+ ASSERT_EXCEPTION(solve_retval, INTERNAL_ABORT,
+ "SolveOnce returns false during iterative refinement.");
+
+ ComputeResiduals(*W, *J_c, *J_d, *Px_L, *Px_U, *Pd_L, *Pd_U,
+ *z_L, *z_U, *v_L, *v_U, *slack_x_L, *slack_x_U,
+ *slack_s_L, *slack_s_U, *sigma_x, *sigma_s,
+ alpha, beta, rhs, res, *resid);
+
+ residual_ratio =
+ ComputeResidualRatio(rhs, res, *resid);
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
+ "residual_ratio = %e\n", residual_ratio);
+
+ num_iter_ref++;
+ // Check if we have to give up on iterative refinement
+ if (num_iter_ref>min_refinement_steps_ &&
+ (num_iter_ref>max_refinement_steps_ ||
+ residual_ratio>residual_improvement_factor_*residual_ratio_old)) {
+
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
+ "Iterative refinement failed with residual_ratio = %e\n", residual_ratio);
+ quit_refinement = true;
+
+ // Pretend singularity only once - if it didn't help, we
+ // have to live with what we got so far
+ resolve_with_better_quality = false;
+ DBG_PRINT((1, "pretend_singular = %d\n", pretend_singular));
+ if (!pretend_singular_last_time) {
+ // First try if we can ask the augmented system solver to
+ // improve the quality of the solution (only if that hasn't
+ // been done before for this linear system)
+ if (!augsys_improved_) {
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
+ "Asking augmented system solver to improve quality of its solutions.\n");
+ augsys_improved_ = augSysSolver_->IncreaseQuality();
+ if (augsys_improved_) {
+ IpData().Append_info_string("q");
+ resolve_with_better_quality = true;
+ }
+ else {
+ // solver said it cannot improve quality, so let
+ // possibly conclude that the current modification is
+ // singular
+ pretend_singular = true;
+ }
+ }
+ else {
+ // we had already asked the solver before to improve the
+ // quality of the solution, so let's now pretend that the
+ // modification is possibly singular
+ pretend_singular = true;
+ }
+ pretend_singular_last_time = pretend_singular;
+ if (pretend_singular) {
+ // let's only conclude that the current linear system
+ // including modifications is singular, if the residual is
+ // quite bad
+ if (residual_ratio < residual_ratio_singular_) {
+ pretend_singular = false;
+ IpData().Append_info_string("S");
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
+ "Just accept current solution.\n");
+ }
+ else {
+ IpData().Append_info_string("s");
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
+ "Pretend that the current system (including modifications) is singular.\n");
+ }
+ }
+ }
+ else {
+ pretend_singular = false;
+ DBG_PRINT((1,"Resetting pretend_singular to false.\n"));
+ }
+ }
+
+ residual_ratio_old = residual_ratio;
+ } // End of loop for iterative refinement
+
+ done = !(resolve_with_better_quality) && !(pretend_singular);
+
+ } // End of loop for solving the linear system (incl. modifications)
+
+ // Finally let's assemble the res result vectors
+ if (alpha != 0.) {
+ res.Scal(alpha);
+ }
+
+ if (beta != 0.) {
+ res.Axpy(beta, *copy_res);
+ }
+
+ DBG_PRINT_VECTOR(2, "res_x", *res.x());
+ DBG_PRINT_VECTOR(2, "res_s", *res.s());
+ DBG_PRINT_VECTOR(2, "res_c", *res.y_c());
+ DBG_PRINT_VECTOR(2, "res_d", *res.y_d());
+ DBG_PRINT_VECTOR(2, "res_zL", *res.z_L());
+ DBG_PRINT_VECTOR(2, "res_zU", *res.z_U());
+ DBG_PRINT_VECTOR(2, "res_vL", *res.v_L());
+ DBG_PRINT_VECTOR(2, "res_vU", *res.v_U());
+
+ IpData().TimingStats().PDSystemSolverTotal().End();
+
+ return true;
+ }
+
+ bool PDFullSpaceSolver::SolveOnce(bool resolve_with_better_quality,
+ bool pretend_singular,
+ const SymMatrix& W,
+ const Matrix& J_c,
+ const Matrix& J_d,
+ const Matrix& Px_L,
+ const Matrix& Px_U,
+ const Matrix& Pd_L,
+ const Matrix& Pd_U,
+ const Vector& z_L,
+ const Vector& z_U,
+ const Vector& v_L,
+ const Vector& v_U,
+ const Vector& slack_x_L,
+ const Vector& slack_x_U,
+ const Vector& slack_s_L,
+ const Vector& slack_s_U,
+ const Vector& sigma_x,
+ const Vector& sigma_s,
+ Number alpha,
+ Number beta,
+ const IteratesVector& rhs,
+ IteratesVector& res)
+ {
+ // TO DO LIST:
+ //
+ // 1. decide for reasonable return codes (e.g. fatal error, too
+ // ill-conditioned...)
+ // 2. Make constants parameters that can be set from the outside
+ // 3. Get Information out of Ipopt structures
+ // 4. add heuristic for structurally singular problems
+ // 5. see if it makes sense to distinguish delta_x and delta_s,
+ // or delta_c and delta_d
+ // 6. increase pivot tolerance if number of get evals so too small
+ DBG_START_METH("PDFullSpaceSolver::SolveOnce",dbg_verbosity);
+
+ IpData().TimingStats().PDSystemSolverSolveOnce().Start();
+
+ // Compute the right hand side for the augmented system formulation
+ SmartPtr<Vector> augRhs_x = rhs.x()->MakeNewCopy();
+ Px_L.AddMSinvZ(1.0, slack_x_L, *rhs.z_L(), *augRhs_x);
+ Px_U.AddMSinvZ(-1.0, slack_x_U, *rhs.z_U(), *augRhs_x);
+
+ SmartPtr<Vector> augRhs_s = rhs.s()->MakeNewCopy();
+ Pd_L.AddMSinvZ(1.0, slack_s_L, *rhs.v_L(), *augRhs_s);
+ Pd_U.AddMSinvZ(-1.0, slack_s_U, *rhs.v_U(), *augRhs_s);
+
+ // Get space into which we can put the solution of the augmented system
+ SmartPtr<IteratesVector> sol = res.MakeNewIteratesVector(true);
+
+ // Now check whether any data has changed
+ std::vector<const TaggedObject*> deps(13);
+ deps[0] = &W;
+ deps[1] = &J_c;
+ deps[2] = &J_d;
+ deps[3] = &z_L;
+ deps[4] = &z_U;
+ deps[5] = &v_L;
+ deps[6] = &v_U;
+ deps[7] = &slack_x_L;
+ deps[8] = &slack_x_U;
+ deps[9] = &slack_s_L;
+ deps[10] = &slack_s_U;
+ deps[11] = &sigma_x;
+ deps[12] = &sigma_s;
+ void* dummy;
+ bool uptodate = dummy_cache_.GetCachedResult(dummy, deps);
+ if (!uptodate) {
+ dummy_cache_.AddCachedResult(dummy, deps);
+ augsys_improved_ = false;
+ }
+ // improve_current_solution can only be true, if that system has
+ // been solved before
+ DBG_ASSERT((!resolve_with_better_quality && !pretend_singular) || uptodate);
+
+ ESymSolverStatus retval;
+ if (uptodate && !pretend_singular) {
+
+ // Get the perturbation values
+ Number delta_x;
+ Number delta_s;
+ Number delta_c;
+ Number delta_d;
+ perturbHandler_->CurrentPerturbation(delta_x, delta_s, delta_c, delta_d);
+
+ // No need to go throught the pain of finding the appropriate
+ // values for the deltas, because the matrix hasn't changed since
+ // the last call. So, just call the Solve Method
+ //
+ // Note: resolve_with_better_quality is true, then the Solve
+ // method has already asked the augSysSolver to increase the
+ // quality at the end solve, and we are now getting the solution
+ // with that better quality
+ retval = augSysSolver_->Solve(&W, 1.0, &sigma_x, delta_x,
+ &sigma_s, delta_s, &J_c, NULL,
+ delta_c, &J_d, NULL, delta_d,
+ *augRhs_x, *augRhs_s, *rhs.y_c(), *rhs.y_d(),
+ *sol->x_NonConst(), *sol->s_NonConst(),
+ *sol->y_c_NonConst(), *sol->y_d_NonConst(),
+ false, 0);
+ if (retval!=SYMSOLVER_SUCCESS) {
+ IpData().TimingStats().PDSystemSolverSolveOnce().End();
+ return false;
+ }
+ }
+ else {
+ Index numberOfEVals=rhs.y_c()->Dim()+rhs.y_d()->Dim();
+ // counter for the number of trial evaluations
+ // (ToDo is not at the correct place)
+ Index count = 0;
+
+ // Get the very first perturbation values from the perturbation
+ // Handler
+ Number delta_x;
+ Number delta_s;
+ Number delta_c;
+ Number delta_d;
+ perturbHandler_->ConsiderNewSystem(delta_x, delta_s, delta_c, delta_d);
+
+ retval = SYMSOLVER_SINGULAR;
+ bool fail = false;
+
+ while (retval!= SYMSOLVER_SUCCESS && !fail) {
+
+ if (pretend_singular) {
+ retval = SYMSOLVER_SINGULAR;
+ pretend_singular = false;
+ }
+ else {
+ count++;
+ Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
+ "Solving system with delta_x=%e delta_s=%e\n delta_c=%e delta_d=%e\n",
+ delta_x, delta_s, delta_c, delta_d);
+ retval = augSysSolver_->Solve(&W, 1.0, &sigma_x, delta_x,
+ &sigma_s, delta_s, &J_c, NULL,
+ delta_c, &J_d, NULL, delta_d,
+ *augRhs_x, *augRhs_s, *rhs.y_c(), *rhs.y_d(),
+ *sol->x_NonConst(), *sol->s_NonConst(),
+ *sol->y_c_NonConst(), *sol->y_d_NonConst(),
+ true, numberOfEVals);
+ }
+ assert(retval!=SYMSOLVER_FATAL_ERROR); //TODO make return code
+ if (retval==SYMSOLVER_SINGULAR &&
+ (rhs.y_c()->Dim()+rhs.y_d()->Dim() > 0) ) {
+
+ // Get new perturbation factors from the perturbation
+ // handlers for the singular case
+ bool pert_return =
+ perturbHandler_->PerturbForSingularity(delta_x, delta_s,
+ delta_c, delta_d);
+ if (!pert_return) {
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
+ "PerturbForSingularity can't be done\n");
+ IpData().TimingStats().PDSystemSolverSolveOnce().End();
+ return false;
+ }
+ }
+ else if (retval==SYMSOLVER_WRONG_INERTIA &&
+ augSysSolver_->NumberOfNegEVals() < numberOfEVals) {
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
+ "Number of negative eigenvalues too small!\n");
+ // If the number of negative eigenvalues is too small, then
+ // we first try to remedy this by asking for better quality
+ // solution (e.g. increasing pivot tolerance), and if that
+ // doesn't help, we assume that the system is singular
+ bool assume_singular = true;
+ if (!augsys_improved_) {
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
+ "Asking augmented system solver to improve quality of its solutions.\n");
+ augsys_improved_ = augSysSolver_->IncreaseQuality();
+ if (augsys_improved_) {
+ IpData().Append_info_string("q");
+ resolve_with_better_quality = true;
+ assume_singular = false;
+ }
+ else {
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
+ "Quality could not be improved\n");
+ }
+ }
+ if (assume_singular) {
+ bool pert_return =
+ perturbHandler_->PerturbForSingularity(delta_x, delta_s,
+ delta_c, delta_d);
+ if (!pert_return) {
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
+ "PerturbForSingularity can't be done for assume singular.\n");
+ IpData().TimingStats().PDSystemSolverSolveOnce().End();
+ return false;
+ }
+ IpData().Append_info_string("a");
+ }
+ }
+ else if (retval==SYMSOLVER_WRONG_INERTIA ||
+ retval==SYMSOLVER_SINGULAR) {
+ // Get new perturbation factors from the perturbation
+ // handlers for the case of wrong inertia
+ bool pert_return =
+ perturbHandler_->PerturbForWrongInertia(delta_x, delta_s,
+ delta_c, delta_d);
+ if (!pert_return) {
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
+ "PerturbForWrongInertia can't be done for wrong interia or singular.\n");
+ IpData().TimingStats().PDSystemSolverSolveOnce().End();
+ return false;
+ }
+ }
+ } // while (retval!=SYMSOLVER_SUCCESS && !fail) {
+
+ // Some output
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
+ "Number of trial factorizations performed: %d\n",
+ count);
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
+ "Perturbation parameters: delta_x=%e delta_s=%e\n delta_c=%e delta_d=%e\n",
+ delta_x, delta_s, delta_c, delta_d);
+ }
+
+ // Compute the remaining sol Vectors
+ Px_L.SinvBlrmZMTdBr(-1., slack_x_L, *rhs.z_L(), z_L, *sol->x_NonConst(), *sol->z_L_NonConst());
+ Px_U.SinvBlrmZMTdBr(1., slack_x_U, *rhs.z_U(), z_U, *sol->x_NonConst(), *sol->z_U_NonConst());
+ Pd_L.SinvBlrmZMTdBr(-1., slack_s_L, *rhs.v_L(), v_L, *sol->s_NonConst(), *sol->v_L_NonConst());
+ Pd_U.SinvBlrmZMTdBr(1., slack_s_U, *rhs.v_U(), v_U, *sol->s_NonConst(), *sol->v_U_NonConst());
+
+ // Finally let's assemble the res result vectors
+ res.AddOneVector(alpha, *sol, beta);
+
+ IpData().TimingStats().PDSystemSolverSolveOnce().End();
+
+ return true;
+ }
+
+ void PDFullSpaceSolver::ComputeResiduals(
+ const SymMatrix& W,
+ const Matrix& J_c,
+ const Matrix& J_d,
+ const Matrix& Px_L,
+ const Matrix& Px_U,
+ const Matrix& Pd_L,
+ const Matrix& Pd_U,
+ const Vector& z_L,
+ const Vector& z_U,
+ const Vector& v_L,
+ const Vector& v_U,
+ const Vector& slack_x_L,
+ const Vector& slack_x_U,
+ const Vector& slack_s_L,
+ const Vector& slack_s_U,
+ const Vector& sigma_x,
+ const Vector& sigma_s,
+ Number alpha,
+ Number beta,
+ const IteratesVector& rhs,
+ const IteratesVector& res,
+ IteratesVector& resid)
+ {
+ DBG_START_METH("PDFullSpaceSolver::ComputeResiduals", dbg_verbosity);
+
+ DBG_PRINT_VECTOR(2, "res", res);
+ IpData().TimingStats().ComputeResiduals().Start();
+
+ // Get the current sizes of the perturbation factors
+ Number delta_x;
+ Number delta_s;
+ Number delta_c;
+ Number delta_d;
+ perturbHandler_->CurrentPerturbation(delta_x, delta_s, delta_c, delta_d);
+
+ SmartPtr<Vector> tmp;
+
+ // x
+ W.MultVector(1., *res.x(), 0., *resid.x_NonConst());
+ J_c.TransMultVector(1., *res.y_c(), 1., *resid.x_NonConst());
+ J_d.TransMultVector(1., *res.y_d(), 1., *resid.x_NonConst());
+ Px_L.MultVector(-1., *res.z_L(), 1., *resid.x_NonConst());
+ Px_U.MultVector(1., *res.z_U(), 1., *resid.x_NonConst());
+ resid.x_NonConst()->AddTwoVectors(delta_x, *res.x(), -1., *rhs.x(), 1.);
+
+ // s
+ Pd_U.MultVector(1., *res.v_U(), 0., *resid.s_NonConst());
+ Pd_L.MultVector(-1., *res.v_L(), 1., *resid.s_NonConst());
+ resid.s_NonConst()->AddTwoVectors(-1., *res.y_d(), -1., *rhs.s(), 1.);
+ if (delta_s!=0.) {
+ resid.s_NonConst()->Axpy(delta_s, *res.s());
+ }
+
+ // c
+ J_c.MultVector(1., *res.x(), 0., *resid.y_c_NonConst());
+ resid.y_c_NonConst()->AddTwoVectors(-delta_c, *res.y_c(), -1., *rhs.y_c(), 1.);
+
+ // d
+ J_d.MultVector(1., *res.x(), 0., *resid.y_d_NonConst());
+ resid.y_d_NonConst()->AddTwoVectors(-1., *res.s(), -1., *rhs.y_d(), 1.);
+ if (delta_d!=0.) {
+ resid.y_d_NonConst()->Axpy(-delta_d, *res.y_d());
+ }
+
+ // zL
+ resid.z_L_NonConst()->Copy(*res.z_L());
+ resid.z_L_NonConst()->ElementWiseMultiply(slack_x_L);
+ tmp = z_L.MakeNew();
+ Px_L.TransMultVector(1., *res.x(), 0., *tmp);
+ tmp->ElementWiseMultiply(z_L);
+ resid.z_L_NonConst()->AddTwoVectors(1., *tmp, -1., *rhs.z_L(), 1.);
+
+ // zU
+ resid.z_U_NonConst()->Copy(*res.z_U());
+ resid.z_U_NonConst()->ElementWiseMultiply(slack_x_U);
+ tmp = z_U.MakeNew();
+ Px_U.TransMultVector(1., *res.x(), 0., *tmp);
+ tmp->ElementWiseMultiply(z_U);
+ resid.z_U_NonConst()->AddTwoVectors(-1., *tmp, -1., *rhs.z_U(), 1.);
+
+ // vL
+ resid.v_L_NonConst()->Copy(*res.v_L());
+ resid.v_L_NonConst()->ElementWiseMultiply(slack_s_L);
+ tmp = v_L.MakeNew();
+ Pd_L.TransMultVector(1., *res.s(), 0., *tmp);
+ tmp->ElementWiseMultiply(v_L);
+ resid.v_L_NonConst()->AddTwoVectors(1., *tmp, -1., *rhs.v_L(), 1.);
+
+ // vU
+ resid.v_U_NonConst()->Copy(*res.v_U());
+ resid.v_U_NonConst()->ElementWiseMultiply(slack_s_U);
+ tmp = v_U.MakeNew();
+ Pd_U.TransMultVector(1., *res.s(), 0., *tmp);
+ tmp->ElementWiseMultiply(v_U);
+ resid.v_U_NonConst()->AddTwoVectors(-1., *tmp, -1., *rhs.v_U(), 1.);
+
+ DBG_PRINT_VECTOR(2, "resid", resid);
+
+ if (Jnlst().ProduceOutput(J_MOREVECTOR, J_LINEAR_ALGEBRA)) {
+ resid.Print(Jnlst(), J_MOREVECTOR, J_LINEAR_ALGEBRA, "resid");
+ }
+
+ if (Jnlst().ProduceOutput(J_MOREDETAILED, J_LINEAR_ALGEBRA)) {
+ Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
+ "max-norm resid_x %e\n", resid.x()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
+ "max-norm resid_s %e\n", resid.s()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
+ "max-norm resid_c %e\n", resid.y_c()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
+ "max-norm resid_d %e\n", resid.y_d()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
+ "max-norm resid_zL %e\n", resid.z_L()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
+ "max-norm resid_zU %e\n", resid.z_U()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
+ "max-norm resid_vL %e\n", resid.v_L()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
+ "max-norm resid_vU %e\n", resid.v_U()->Amax());
+ }
+ IpData().TimingStats().ComputeResiduals().End();
+ }
+
+ Number PDFullSpaceSolver::ComputeResidualRatio(const IteratesVector& rhs,
+ const IteratesVector& res,
+ const IteratesVector& resid)
+ {
+ DBG_START_METH("PDFullSpaceSolver::ComputeResidualRatio", dbg_verbosity);
+
+ Number nrm_rhs = rhs.Amax();
+ Number nrm_res = res.Amax();
+ Number nrm_resid = resid.Amax();
+ Jnlst().Printf(J_MOREDETAILED, J_LINEAR_ALGEBRA,
+ "nrm_rhs = %8.2e nrm_sol = %8.2e nrm_resid = %8.2e\n",
+ nrm_rhs, nrm_res, nrm_resid);
+
+ if (nrm_rhs+nrm_res == 0.) {
+ return nrm_resid; // this should be zero
+ }
+ else {
+ // ToDo: determine how to include norm of matrix, and what
+ // safeguard to use against incredibly large solution vectors
+ Number max_cond = 1e6;
+ return nrm_resid/(Min(nrm_res, max_cond*nrm_rhs)+nrm_rhs);
+ }
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpPDFullSpaceSolver.hpp b/SimTKmath/Optimizers/src/IpOpt/IpPDFullSpaceSolver.hpp
new file mode 100644
index 0000000..87c236d
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpPDFullSpaceSolver.hpp
@@ -0,0 +1,189 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpPDFullSpaceSolver.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPPDFULLSPACESOLVER_HPP__
+#define __IPPDFULLSPACESOLVER_HPP__
+
+#include "IpPDSystemSolver.hpp"
+#include "IpAugSystemSolver.hpp"
+#include "IpPDPerturbationHandler.hpp"
+
+namespace Ipopt
+{
+
+ /** This is the implemetation of the Primal-Dual System, using the
+ * full space approach with a direct linear solver.
+ *
+ * A note on the iterative refinement: We perform at least
+ * min_refinement_steps number of iterative refinement steps. If after
+ * one iterative refinement the quality of the solution (defined in
+ * ResidualRatio) does not improve or the maximal number of
+ * iterative refinement steps is exceeded before the tolerance
+ * residual_ratio_max_ is satisfied, we first ask the linear solver
+ * to solve the system more accurately (e.g. by increasing the
+ * pivot tolerance). If that doesn't help or is not possible, we
+ * treat the system, as if it is singular (i.e. increase delta's).
+ */
+ class PDFullSpaceSolver: public PDSystemSolver
+ {
+ public:
+ /** @name /Destructor */
+ //@{
+ /** Constructor that takes in the Augmented System solver that
+ * is to be used inside
+ */
+ PDFullSpaceSolver(AugSystemSolver& augSysSolver,
+ PDPerturbationHandler& perturbHandler);
+
+ /** Default destructor */
+ virtual ~PDFullSpaceSolver();
+ //@}
+
+ /* overloaded from AlgorithmStrategyObject */
+ bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Solve the primal dual system, given one right hand side.
+ */
+ virtual bool Solve(Number alpha,
+ Number beta,
+ const IteratesVector& rhs,
+ IteratesVector& res,
+ bool allow_inexact=false,
+ bool improve_solution=false);
+
+ /** Methods for IpoptType */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ PDFullSpaceSolver();
+ /** Overloaded Equals Operator */
+ PDFullSpaceSolver& operator=(const PDFullSpaceSolver&);
+ //@}
+
+ /** @name Strategy objects to hold on to. */
+ //@{
+ /** Pointer to the Solver for the augmented system */
+ SmartPtr<AugSystemSolver> augSysSolver_;
+ /** Pointer to the Perturbation Handler. */
+ SmartPtr<PDPerturbationHandler> perturbHandler_;
+ //@}
+
+ /**@name Data about the correction made to the system */
+ //@{
+ /** A dummy cache to figure out if the deltas are still up to date*/
+ CachedResults<void*> dummy_cache_;
+ /** Flag indicating if for the current matrix the solution quality
+ * of the augmented system solver has already been increased. */
+ bool augsys_improved_;
+ //@}
+
+ /** @name Parameters */
+ //@{
+ /** Minimal number of iterative refinement performed per backsolve */
+ Index min_refinement_steps_;
+ /** Maximal number of iterative refinement performed per backsolve */
+ Index max_refinement_steps_;
+ /** Maximal allowed ratio of the norm of the residual over the
+ * norm of the right hand side and solution. */
+ Number residual_ratio_max_;
+ /** If the residual_ratio is larger than this value after trying
+ * to improve the solution, the linear system is assumed to be
+ * singular and modified. */
+ Number residual_ratio_singular_;
+ /** Factor defining require improvement to consider iterative
+ * refinement successful. */
+ Number residual_improvement_factor_;
+ //@}
+
+ /** Internal function for a single backsolve (which will be used
+ * for iterative refinement on the outside). This method returns
+ * false, if for some reason the linear system could not be
+ * solved (e.g. when the regularization parameter becomes too
+ * large.)
+ */
+ bool SolveOnce(bool resolve_unmodified,
+ bool pretend_singular,
+ const SymMatrix& W,
+ const Matrix& J_c,
+ const Matrix& J_d,
+ const Matrix& Px_L,
+ const Matrix& Px_U,
+ const Matrix& Pd_L,
+ const Matrix& Pd_U,
+ const Vector& z_L,
+ const Vector& z_U,
+ const Vector& v_L,
+ const Vector& v_U,
+ const Vector& slack_x_L,
+ const Vector& slack_x_U,
+ const Vector& slack_s_L,
+ const Vector& slack_s_U,
+ const Vector& sigma_x,
+ const Vector& sigma_s,
+ Number alpha,
+ Number beta,
+ const IteratesVector& rhs,
+ IteratesVector& res);
+
+ /** Internal function for computing the residual (resid) given the
+ * right hand side (rhs) and the solution of the system (res).
+ */
+ void ComputeResiduals(const SymMatrix& W,
+ const Matrix& J_c,
+ const Matrix& J_d,
+ const Matrix& Px_L,
+ const Matrix& Px_U,
+ const Matrix& Pd_L,
+ const Matrix& Pd_U,
+ const Vector& z_L,
+ const Vector& z_U,
+ const Vector& v_L,
+ const Vector& v_U,
+ const Vector& slack_x_L,
+ const Vector& slack_x_U,
+ const Vector& slack_s_L,
+ const Vector& slack_s_U,
+ const Vector& sigma_x,
+ const Vector& sigma_s,
+ Number alpha,
+ Number beta,
+ const IteratesVector& rhs,
+ const IteratesVector& res,
+ IteratesVector& resid);
+
+ /** Internal function for computing the ratio of the residual
+ * compared to the right hand side and solution. The smaller
+ * this value, the better the solution. */
+ Number ComputeResidualRatio(const IteratesVector& rhs,
+ const IteratesVector& res,
+ const IteratesVector& resid);
+
+ /** @name Auxilliary functions */
+ //@{
+ /** Compute \f$ x = S^{-1}(r + \alpha Z P^T d)\f$ */
+ void SinvBlrmZPTdBr(Number alpha, const Vector& S,
+ const Vector& R, const Vector& Z,
+ const Matrix& P, const Vector&g, Vector& X);
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpPDPerturbationHandler.cpp b/SimTKmath/Optimizers/src/IpOpt/IpPDPerturbationHandler.cpp
new file mode 100644
index 0000000..47e7c8b
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpPDPerturbationHandler.cpp
@@ -0,0 +1,466 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpPDPerturbationHandler.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-08-04
+
+#include "IpPDPerturbationHandler.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ PDPerturbationHandler::PDPerturbationHandler()
+ :
+ reset_last_(false),
+ degen_iters_max_(3)
+ {}
+
+ void
+ PDPerturbationHandler::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->AddLowerBoundedNumberOption(
+ "max_hessian_perturbation",
+ "Maximum value of regularization parameter for handling negative curvature.",
+ 0, true,
+ 1e20,
+ "In order to guarantee that the search directions are indeed proper "
+ "descent directions, Ipopt requires that the inertia of the "
+ "(augmented) linear system for the step computation has the "
+ "correct number of negative and positive eigenvalues. The idea "
+ "is that this guides the algorithm away from maximizers and makes "
+ "Ipopt more likely converge to first order optimal points that "
+ "are minimizers. If the inertia is not correct, a multiple of the "
+ "identity matrix is added to the Hessian of the Lagrangian in the "
+ "augmented system. This parameter gives the maximum value of the "
+ "regularization parameter. If a regularization of that size is "
+ "not enough, the algorithm skips this iteration and goes to the "
+ "restoration phase. (This is delta_w^max in the implementation paper.)");
+ roptions->AddLowerBoundedNumberOption(
+ "min_hessian_perturbation",
+ "Smallest perturbation of the Hessian block.",
+ 0., false, 1e-20,
+ "The size of the perturbation of the Hessian block is never selected "
+ "smaller than this value, unless no perturbation is necessary. (This "
+ "is delta_w^min in implementation paper.)");
+ roptions->AddLowerBoundedNumberOption(
+ "perturb_inc_fact_first",
+ "Increase factor for x-s perturbation for very first perturbation.",
+ 1., true, 100.,
+ "The factor by which the perturbation is increased when a trial value "
+ "was not sufficient - this value is used for the computation of the "
+ "very first perturbation and allows a different value for for the first "
+ "perturbation than that used for the remaining perturbations. "
+ "(This is bar_kappa_w^+ in the implementation paper.)");
+ roptions->AddLowerBoundedNumberOption(
+ "perturb_inc_fact",
+ "Increase factor for x-s perturbation.",
+ 1., true, 8.,
+ "The factor by which the perturbation is increased when a trial value "
+ "was not sufficient - this value is used for the computation of "
+ "all perturbations except for the first. "
+ "(This is kappa_w^+ in the implementation paper.)");
+ roptions->AddBoundedNumberOption(
+ "perturb_dec_fact",
+ "Decrease factor for x-s perturbation.",
+ 0., true, 1., true, 1./3.,
+ "The factor by which the perturbation is decreased when a trial value "
+ "is deduced from the size of the most recent successful perturbation. "
+ "(This is kappa_w^- in the implementation paper.)");
+ roptions->AddLowerBoundedNumberOption(
+ "first_hessian_perturbation",
+ "Size of first x-s perturbation tried.",
+ 0., true, 1e-4,
+ "The first value tried for the x-s perturbation in the inertia "
+ "correction scheme."
+ "(This is delta_0 in the implementation paper.)");
+ roptions->AddLowerBoundedNumberOption(
+ "jacobian_regularization_value",
+ "Size of the regularization for rank-deficient constraint Jacobians.",
+ 0., false, 1e-8,
+ "(This is bar delta_c in the implementation paper.)");
+ roptions->AddLowerBoundedNumberOption(
+ "jacobian_regularization_exponent",
+ "Exponent for mu in the regularization for rank-deficient constraint Jacobians.",
+ 0., false, 0.25,
+ "(This is kappa_c in the implementation paper.)");
+ }
+
+ bool PDPerturbationHandler::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ options.GetNumericValue("max_hessian_perturbation", delta_xs_max_, prefix);
+ options.GetNumericValue("min_hessian_perturbation", delta_xs_min_, prefix);
+ options.GetNumericValue("perturb_inc_fact_first", delta_xs_first_inc_fact_, prefix);
+ options.GetNumericValue("perturb_inc_fact", delta_xs_inc_fact_, prefix);
+ options.GetNumericValue("perturb_dec_fact", delta_xs_dec_fact_, prefix);
+ options.GetNumericValue("first_hessian_perturbation", delta_xs_init_, prefix);
+ options.GetNumericValue("jacobian_regularization_value", delta_cd_val_, prefix);
+ options.GetNumericValue("jacobian_regularization_exponent", delta_cd_exp_, prefix);
+
+ hess_degenerate_ = NOT_YET_DETERMINED;
+ jac_degenerate_ = NOT_YET_DETERMINED;
+ degen_iters_ = 0;
+
+ delta_x_curr_ = 0.;
+ delta_s_curr_ = 0.;
+ delta_c_curr_ = 0.;
+ delta_d_curr_ = 0.;
+ delta_x_last_ = 0.;
+ delta_s_last_ = 0.;
+ delta_c_last_ = 0.;
+ delta_d_last_ = 0.;
+
+ test_status_ = NO_TEST;
+
+ return true;
+ }
+
+ bool
+ PDPerturbationHandler::ConsiderNewSystem(Number& delta_x, Number& delta_s,
+ Number& delta_c, Number& delta_d)
+ {
+ DBG_START_METH("PDPerturbationHandler::ConsiderNewSystem",dbg_verbosity);
+
+ // Check if we can conclude that some components of the system are
+ // structurally degenerate
+ finalize_test();
+
+ // Store the perturbation from the previous matrix
+ if (reset_last_) {
+ delta_x_last_ = delta_x_curr_;
+ delta_s_last_ = delta_s_curr_;
+ delta_c_last_ = delta_c_curr_;
+ delta_d_last_ = delta_d_curr_;
+ }
+ else {
+ if (delta_x_curr_ > 0.) {
+ delta_x_last_ = delta_x_curr_;
+ }
+ if (delta_s_curr_ > 0.) {
+ delta_s_last_ = delta_s_curr_;
+ }
+ if (delta_c_curr_ > 0.) {
+ delta_c_last_ = delta_c_curr_;
+ }
+ if (delta_d_curr_ > 0.) {
+ delta_d_last_ = delta_d_curr_;
+ }
+ }
+
+ DBG_ASSERT((hess_degenerate_ != NOT_YET_DETERMINED ||
+ jac_degenerate_ != DEGENERATE) &&
+ (jac_degenerate_ != NOT_YET_DETERMINED ||
+ hess_degenerate_ != DEGENERATE));
+
+ if (hess_degenerate_ == NOT_YET_DETERMINED ||
+ jac_degenerate_ == NOT_YET_DETERMINED) {
+ test_status_ = TEST_DELTA_C_EQ_0_DELTA_X_EQ_0;
+ }
+ else {
+ test_status_ = NO_TEST;
+ }
+
+ if (jac_degenerate_ == DEGENERATE) {
+ delta_c = delta_c_curr_ = delta_cd();
+ IpData().Append_info_string("l");
+ }
+ else {
+ delta_c = delta_c_curr_ = 0.;
+ }
+ delta_d = delta_d_curr_ = delta_c;
+
+ if (hess_degenerate_ == DEGENERATE) {
+ delta_x_curr_ = 0.;
+ delta_s_curr_ = 0.;
+ bool retval = get_deltas_for_wrong_inertia(delta_x, delta_s,
+ delta_c, delta_d);
+ if (!retval) {
+ return false;
+ }
+ }
+ else {
+ delta_x = 0.;
+ delta_s = delta_x;
+ }
+
+ delta_x_curr_ = delta_x;
+ delta_s_curr_ = delta_s;
+ delta_c_curr_ = delta_c;
+ delta_d_curr_ = delta_d;
+
+ IpData().Set_info_regu_x(delta_x);
+
+ get_deltas_for_wrong_inertia_called_ = false;
+
+ return true;
+ }
+
+ bool
+ PDPerturbationHandler::PerturbForSingularity(
+ Number& delta_x, Number& delta_s,
+ Number& delta_c, Number& delta_d)
+ {
+ DBG_START_METH("PDPerturbationHandler::PerturbForSingularity",
+ dbg_verbosity);
+
+ bool retval;
+
+ // Check for structural degeneracy
+ if (hess_degenerate_ == NOT_YET_DETERMINED ||
+ jac_degenerate_ == NOT_YET_DETERMINED) {
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
+ "Degeneracy test for hess_degenerate_ = %d and jac_degenerate_ = %d\n test_status_ = %d\n",
+ hess_degenerate_, jac_degenerate_, test_status_);
+ switch (test_status_) {
+ case TEST_DELTA_C_EQ_0_DELTA_X_EQ_0:
+ DBG_ASSERT(delta_x_curr_ == 0. && delta_c_curr_ == 0.);
+ // in this case we haven't tried anything for this matrix yet
+ if (jac_degenerate_ == NOT_YET_DETERMINED) {
+ delta_d_curr_ = delta_c_curr_ = delta_cd();
+ test_status_ = TEST_DELTA_C_GT_0_DELTA_X_EQ_0;
+ }
+ else {
+ DBG_ASSERT(hess_degenerate_ == NOT_YET_DETERMINED);
+ retval = get_deltas_for_wrong_inertia(delta_x, delta_s,
+ delta_c, delta_d);
+ if (!retval) {
+ return false;
+ }
+ DBG_ASSERT(delta_c == 0. && delta_d == 0.);
+ test_status_ = TEST_DELTA_C_EQ_0_DELTA_X_GT_0;
+ }
+ break;
+ case TEST_DELTA_C_GT_0_DELTA_X_EQ_0:
+ DBG_ASSERT(delta_x_curr_ == 0. && delta_c_curr_ > 0.);
+ DBG_ASSERT(jac_degenerate_ == NOT_YET_DETERMINED);
+ delta_d_curr_ = delta_c_curr_ = 0.;
+ retval = get_deltas_for_wrong_inertia(delta_x, delta_s,
+ delta_c, delta_d);
+ if (!retval) {
+ return false;
+ }
+ DBG_ASSERT(delta_c == 0. && delta_d == 0.);
+ test_status_ = TEST_DELTA_C_EQ_0_DELTA_X_GT_0;
+ break;
+ case TEST_DELTA_C_EQ_0_DELTA_X_GT_0:
+ DBG_ASSERT(delta_x_curr_ > 0. && delta_c_curr_ == 0.);
+ delta_d_curr_ = delta_c_curr_ = delta_cd();
+ retval = get_deltas_for_wrong_inertia(delta_x, delta_s,
+ delta_c, delta_d);
+ if (!retval) {
+ return false;
+ }
+ test_status_ = TEST_DELTA_C_GT_0_DELTA_X_GT_0;
+ break;
+ case TEST_DELTA_C_GT_0_DELTA_X_GT_0:
+ retval = get_deltas_for_wrong_inertia(delta_x, delta_s,
+ delta_c, delta_d);
+ if (!retval) {
+ return false;
+ }
+ break;
+ case NO_TEST:
+ DBG_ASSERT(false && "we should not get here.");
+ }
+ }
+ else {
+ if (delta_c_curr_ > 0. || get_deltas_for_wrong_inertia_called_) {
+ // If we already used a perturbation for the constraints, we do
+ // the same thing as if we were encountering negative curvature
+ retval = get_deltas_for_wrong_inertia(delta_x, delta_s,
+ delta_c, delta_d);
+ if (!retval) {
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
+ "Can't get_deltas_for_wrong_inertia for delta_x_curr_ = %e and delta_c_curr_ = %e\n",
+ delta_x_curr_, delta_c_curr_);
+ return false;
+ }
+ }
+ else {
+ // Otherwise we now perturb the lower right corner
+ delta_d_curr_ = delta_c_curr_ = delta_cd();
+
+ // ToDo - also perturb Hessian?
+ IpData().Append_info_string("L");
+ }
+ }
+
+ delta_x = delta_x_curr_;
+ delta_s = delta_s_curr_;
+ delta_c = delta_c_curr_;
+ delta_d = delta_d_curr_;
+
+ IpData().Set_info_regu_x(delta_x);
+
+ return true;
+ }
+
+ bool
+ PDPerturbationHandler::get_deltas_for_wrong_inertia(
+ Number& delta_x, Number& delta_s,
+ Number& delta_c, Number& delta_d)
+ {
+ if (delta_x_curr_ == 0.) {
+ if (delta_x_last_ == 0.) {
+ delta_x_curr_ = delta_xs_init_;
+ }
+ else {
+ delta_x_curr_ = Max(delta_xs_min_,
+ delta_x_last_*delta_xs_dec_fact_);
+ }
+ }
+ else {
+ if (delta_x_last_ == 0. || 1e5*delta_x_last_<delta_x_curr_) {
+ delta_x_curr_ = delta_xs_first_inc_fact_*delta_x_curr_;
+ }
+ else {
+ delta_x_curr_ = delta_xs_inc_fact_*delta_x_curr_;
+ }
+ }
+ if (delta_x_curr_ > delta_xs_max_) {
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
+ "delta_x perturbation is becoming too large: %e\n",
+ delta_x_curr_);
+ delta_x_last_ = 0.;
+ delta_s_last_ = 0.;
+ IpData().Append_info_string("dx");
+ return false;
+ }
+
+ delta_s_curr_ = delta_x_curr_;
+
+ delta_x = delta_x_curr_;
+ delta_s = delta_s_curr_;
+ delta_c = delta_c_curr_;
+ delta_d = delta_d_curr_;
+
+ IpData().Set_info_regu_x(delta_x);
+
+ get_deltas_for_wrong_inertia_called_ = true;
+
+ return true;
+ }
+
+ bool
+ PDPerturbationHandler::PerturbForWrongInertia(
+ Number& delta_x, Number& delta_s,
+ Number& delta_c, Number& delta_d)
+ {
+ DBG_START_METH("PDPerturbationHandler::PerturbForWrongInertia",
+ dbg_verbosity);
+
+ // Check if we can conclude that components of the system are
+ // structurally degenerate (we only get here if the most recent
+ // perturbation for a test did not result in a singular system)
+ finalize_test();
+
+ bool retval = get_deltas_for_wrong_inertia(delta_x, delta_s, delta_c, delta_d);
+ if (!retval && delta_c==0.) {
+ DBG_ASSERT(delta_d == 0.);
+ delta_c_curr_ = delta_cd();
+ delta_d_curr_ = delta_c_curr_;
+ delta_x_curr_ = 0.;
+ delta_s_curr_ = 0.;
+ test_status_ = NO_TEST;
+ if (hess_degenerate_ == DEGENERATE) {
+ hess_degenerate_ = NOT_YET_DETERMINED;
+ }
+ retval = get_deltas_for_wrong_inertia(delta_x, delta_s, delta_c, delta_d);
+ }
+ return retval;
+ }
+
+ void
+ PDPerturbationHandler::CurrentPerturbation(
+ Number& delta_x, Number& delta_s,
+ Number& delta_c, Number& delta_d)
+ {
+ delta_x = delta_x_curr_;
+ delta_s = delta_s_curr_;
+ delta_c = delta_c_curr_;
+ delta_d = delta_d_curr_;
+ }
+
+ Number
+ PDPerturbationHandler::delta_cd()
+ {
+ return delta_cd_val_ * pow(IpData().curr_mu(), delta_cd_exp_);
+ }
+
+ void
+ PDPerturbationHandler::finalize_test()
+ {
+ switch (test_status_) {
+ case NO_TEST:
+ return;
+ case TEST_DELTA_C_EQ_0_DELTA_X_EQ_0:
+ if (hess_degenerate_ == NOT_YET_DETERMINED &&
+ jac_degenerate_ == NOT_YET_DETERMINED) {
+ hess_degenerate_ = NOT_DEGENERATE;
+ jac_degenerate_ = NOT_DEGENERATE;
+ IpData().Append_info_string("Nhj ");
+ }
+ else if (hess_degenerate_ == NOT_YET_DETERMINED) {
+ hess_degenerate_ = NOT_DEGENERATE;
+ IpData().Append_info_string("Nh ");
+ }
+ else if (jac_degenerate_ == NOT_YET_DETERMINED) {
+ jac_degenerate_ = NOT_DEGENERATE;
+ IpData().Append_info_string("Nj ");
+ }
+ break;
+ case TEST_DELTA_C_GT_0_DELTA_X_EQ_0:
+ if (hess_degenerate_ == NOT_YET_DETERMINED) {
+ hess_degenerate_ = NOT_DEGENERATE;
+ IpData().Append_info_string("Nh ");
+ }
+ if (jac_degenerate_ == NOT_YET_DETERMINED) {
+ degen_iters_++;
+ if (degen_iters_ >= degen_iters_max_) {
+ jac_degenerate_ = DEGENERATE;
+ IpData().Append_info_string("Dj ");
+ }
+ IpData().Append_info_string("L");
+ }
+ break;
+ case TEST_DELTA_C_EQ_0_DELTA_X_GT_0:
+ if (jac_degenerate_ == NOT_YET_DETERMINED) {
+ jac_degenerate_ = NOT_DEGENERATE;
+ IpData().Append_info_string("Nj ");
+ }
+ if (hess_degenerate_ == NOT_YET_DETERMINED) {
+ degen_iters_++;
+ if (degen_iters_ >= degen_iters_max_) {
+ hess_degenerate_ = DEGENERATE;
+ IpData().Append_info_string("Dh ");
+ }
+ }
+ break;
+ case TEST_DELTA_C_GT_0_DELTA_X_GT_0:
+ degen_iters_++;
+ if (degen_iters_ >= degen_iters_max_) {
+ hess_degenerate_ = DEGENERATE;
+ jac_degenerate_ = DEGENERATE;
+ IpData().Append_info_string("Dhj ");
+ }
+ IpData().Append_info_string("L");
+ break;
+ }
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpPDPerturbationHandler.hpp b/SimTKmath/Optimizers/src/IpOpt/IpPDPerturbationHandler.hpp
new file mode 100644
index 0000000..fcbca2a
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpPDPerturbationHandler.hpp
@@ -0,0 +1,199 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpPDPerturbationHandler.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-08-04
+
+#ifndef __IPPDPERTURBATIONHANDLER_HPP__
+#define __IPPDPERTURBATIONHANDLER_HPP__
+
+#include "IpAlgStrategy.hpp"
+
+namespace Ipopt
+{
+
+ /** Class for handling the perturbation factors delta_x, delta_s,
+ * delta_c, and delta_d in the primal dual system. This class is
+ * used by the PDFullSpaceSolver to handle the cases where the
+ * primal-dual system is singular or has the wrong inertia. The
+ * perturbation factors are obtained based on simple heuristics,
+ * taking into account the size of previous perturbations.
+ */
+ class PDPerturbationHandler: public AlgorithmStrategyObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor */
+ PDPerturbationHandler();
+ /** Default destructor */
+ virtual ~PDPerturbationHandler()
+ {}
+ //@}
+
+ /* overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** This method must be called for each new matrix, and before any
+ * other method for generating perturbation factors. Usually,
+ * the returned perturbation factors are zero, but if the system
+ * is thought to be structurally singular, they might be
+ * positive. If the return value is false, no suitable
+ * perturbation could be found. */
+ bool ConsiderNewSystem(Number& delta_x, Number& delta_s,
+ Number& delta_c, Number& delta_d);
+
+ /** This method returns pertubation factors for the case when the
+ * most recent factorization resulted in a singular matrix. If
+ * the return value is false, no suitable perturbation could be
+ * found. */
+ bool PerturbForSingularity(Number& delta_x, Number& delta_s,
+ Number& delta_c, Number& delta_d);
+
+ /** This method returns pertubation factors for the case when the
+ * most recent factorization resulted in a matrix with an
+ * incorrect number of negative eigenvalues. If the return value
+ * is false, no suitable perturbation could be found. */
+ bool PerturbForWrongInertia(Number& delta_x, Number& delta_s,
+ Number& delta_c, Number& delta_d);
+
+ /** Just return the perturbation values that have been determined
+ * most recently */
+ void CurrentPerturbation(Number& delta_x, Number& delta_s,
+ Number& delta_c, Number& delta_d);
+
+ /** Methods for IpoptType */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ PDPerturbationHandler(const PDPerturbationHandler&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const PDPerturbationHandler&);
+ //@}
+
+ /** @name Size of the most recent non-zero perturbation. */
+ //@{
+ /** The last nonzero value for delta_x */
+ Number delta_x_last_;
+ /** The last nonzero value for delta_s */
+ Number delta_s_last_;
+ /** The last nonzero value for delta_c */
+ Number delta_c_last_;
+ /** The last nonzero value for delta_d */
+ Number delta_d_last_;
+ //@}
+
+ /** @name Size of the most recently suggested perturbation for the
+ * current matrix. */
+ //@{
+ /** The current value for delta_x */
+ Number delta_x_curr_;
+ /** The current value for delta_s */
+ Number delta_s_curr_;
+ /** The current value for delta_c */
+ Number delta_c_curr_;
+ /** The current value for delta_d */
+ Number delta_d_curr_;
+ //@}
+
+ /** Flag indicating if for the given matrix the perturb for wrong
+ * inertia method has already been called. */
+ bool get_deltas_for_wrong_inertia_called_;
+
+ /** @name Handling structural degeneracy */
+ //@{
+ /** Type for degeneracy flags */
+ enum DegenType {
+ NOT_YET_DETERMINED,
+ NOT_DEGENERATE,
+ DEGENERATE
+ };
+
+ /** Flag indicating whether the reduced Hessian matrix is thought
+ * to be structurally singular. */
+ DegenType hess_degenerate_;
+
+ /** Flag indicating whether the Jacobian of the constraints is
+ * thought to be structurally rank-deficient. */
+ DegenType jac_degenerate_;
+
+ /** Flag counting matrices in which degeneracy was observed in the
+ * first successive iterations. -1 means that there was a
+ * non-degenerate (unperturbed) matrix at some point. */
+ Index degen_iters_;
+
+ /** Status of current trial configuration */
+ enum TrialStatus {
+ NO_TEST,
+ TEST_DELTA_C_EQ_0_DELTA_X_EQ_0,
+ TEST_DELTA_C_GT_0_DELTA_X_EQ_0,
+ TEST_DELTA_C_EQ_0_DELTA_X_GT_0,
+ TEST_DELTA_C_GT_0_DELTA_X_GT_0
+ };
+
+ /** Current status */
+ TrialStatus test_status_;
+ //@}
+
+ /** @name Algorithmic parameters. */
+ //@{
+ /** Maximal perturbation for x and s. */
+ Number delta_xs_max_;
+ /** Smallest possible perturbation for x and s. */
+ Number delta_xs_min_;
+ /** Increase factor for delta_xs for first required perturbation. */
+ Number delta_xs_first_inc_fact_;
+ /** Increase factor for delta_xs for later perturbations. */
+ Number delta_xs_inc_fact_;
+ /** Decrease factor for delta_xs for later perturbations. */
+ Number delta_xs_dec_fact_;
+ /** Very first trial value for delta_xs perturbation. */
+ Number delta_xs_init_;
+ /** Size of perturbation for c and d blocks. */
+ Number delta_cd_val_;
+ /** Exponent on mu in formula for of perturbation for c and d blocks. */
+ Number delta_cd_exp_;
+ /** Flag indicating whether the new values are based on the
+ * perturbations in the last iteration or in the more recent
+ * iteration in which a perturbation was done. */
+ bool reset_last_;
+ /** Required number of iterations for degeneracy conclusions. */
+ Index degen_iters_max_;
+ //@}
+
+ /** @name Auxilliary methods */
+ //@{
+ /** Internal version of PerturbForWrongInertia with the
+ * difference, that finalize_test is not called. Returns false
+ * if the delta_x and delta_s parameters become too large. */
+ bool get_deltas_for_wrong_inertia(Number& delta_x, Number& delta_s,
+ Number& delta_c, Number& delta_d);
+
+ /** This method is call whenever a matrix had been factorization
+ * and is not singular. In here, we can evaluate the outcome of
+ * the deneracy test heuristics. */
+ void finalize_test();
+ /** Compute perturbation value for constraints */
+ Number delta_cd();
+ //@}
+
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpPDSystemSolver.hpp b/SimTKmath/Optimizers/src/IpOpt/IpPDSystemSolver.hpp
new file mode 100644
index 0000000..a6eec24
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpPDSystemSolver.hpp
@@ -0,0 +1,130 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpPDSystemSolver.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPPDSYSTEMSOLVER_HPP__
+#define __IPPDSYSTEMSOLVER_HPP__
+
+#include "IpUtils.hpp"
+#include "IpSymMatrix.hpp"
+#include "IpAlgStrategy.hpp"
+#include "IpIteratesVector.hpp"
+
+namespace Ipopt
+{
+
+ /** Pure Primal Dual System Solver Base Class.
+ * This is the base class for all derived Primal-Dual System Solver Types.
+ *
+ * Here, we understand the primal-dual system as the following linear
+ * system:
+ *
+ * \f$
+ * \left[\begin{array}{cccccccc}
+ * W & 0 & J_c^T & J_d^T & -P^x_L & P^x_U & 0 & 0 \\
+ * 0 & 0 & 0 & -I & 0 & 0 & -P_L^d & P_U^d \\
+ * J_c & 0 & 0 & 0 & 0 & 0 & 0 & 0\\
+ * J_d & -I & 0 & 0 & 0 & 0 & 0 & 0\\
+ * Z_L(P_L^x)^T & 0 & 0 & 0 & Sl^x_L & 0 & 0 & 0\\
+ * -Z_U(P_U^x)^T & 0 & 0 & 0 & 0 & Sl^x_U & 0 & 0\\
+ * 0 & V_L(P_L^d)^T & 0 & 0 & 0 & 0 & Sl^s_L & 0 \\
+ * 0 & -V_U(P_U^d)^T & 0 & 0 & 0 & 0 & 0 & Sl^s_U \\
+ * \end{array}\right]
+ * \left(\begin{array}{c}
+ * sol_x\\ sol_s\\ sol_c\\ sol_d\\ sol^z_L\\ sol^z_U\\ sol^v_L\\
+ * sol^v_U
+ * \end{array}\right) =
+ * \left(\begin{array}{c}
+ * rhs_x\\ rhs_s\\ rhs_c\\ rhs_d\\ rhs^z_L\\ rhs^z_U\\ rhs^v_L\\
+ * rhs^v_U
+ * \end{array}\right)
+ * \f$
+ *
+ * Here, \f$Sl^x_L = (P^x_L)^T x - x_L\f$,
+ * \f$Sl^x_U = x_U - (P^x_U)^T x\f$, \f$Sl^d_L = (P^d_L)^T d(x) - d_L\f$,
+ * \f$Sl^d_U = d_U - (P^d_U)^T d(x)\f$. The results returned to the
+ * caller is \f$res = \alpha * sol + \beta * res\f$.
+ *
+ * The solution of this linear system (in order to compute the search
+ * direction of the algorthim) usually requires a considerable amount of
+ * computation time. Therefore, it is important to tailor the solution
+ * of this system to the characteristics of the problem. The purpose of
+ * this base class is to provide a generic interface to the algorithm
+ * that it can use whenever it requires a solution of the above system.
+ * Particular implementation can then be written to provide the methods
+ * defined here.
+ *
+ * It is implicitly assumed here, that the upper left 2 by 2 block
+ * is possibly modified (implicitly or explicitly) so that its
+ * projection onto the null space of the overall constraint
+ * Jacobian \f$\left[\begin{array}{cc}J_c & 0\\J_d &
+ * -I\end{array}\right]\f$ is positive definite. This is necessary
+ * to guarantee certain descent properties of the resulting search
+ * direction. For example, in the full space implementation, a
+ * multiple of the identity might be added to the upper left 2 by 2
+ * block.
+ *
+ * Note that the Solve method might be called several times for different
+ * right hand sides, but with identical data. Therefore, if possible,
+ * an implemetation of PDSystem should check whether the incoming data has
+ * changed, and not redo factorization etc. unless necessary.
+ */
+ class PDSystemSolver: public AlgorithmStrategyObject
+ {
+ public:
+ /** @name /Destructor */
+ //@{
+ /** Default Constructor */
+ PDSystemSolver()
+ {}
+
+ /** Default destructor */
+ virtual ~PDSystemSolver()
+ {}
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix) = 0;
+
+ /** Solve the primal dual system, given one right hand side. If
+ * the flag allow_inexact is set to true, it is not necessary to
+ * solve the system to best accuracy; for example, we don't want
+ * iterative refinement during the computation of the second
+ * order correction. On the other hand, if improve_solution is
+ * true, the solution given in res should be improved (here beta
+ * has to be zero, and res is assume to be the solution for the
+ * system using rhs, without the factor alpha...). THe return
+ * value is false, if a solution could not be computed (for
+ * example, when the Hessian regularization parameter becomes too
+ * large.)
+ */
+ virtual bool Solve(Number alpha,
+ Number beta,
+ const IteratesVector& rhs,
+ IteratesVector& res,
+ bool allow_inexact=false,
+ bool improve_solution=false) =0;
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Overloaded Equals Operator */
+ PDSystemSolver& operator=(const PDSystemSolver&);
+ //@}
+ };
+
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpProbingMuOracle.cpp b/SimTKmath/Optimizers/src/IpOpt/IpProbingMuOracle.cpp
new file mode 100644
index 0000000..8443ce4
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpProbingMuOracle.cpp
@@ -0,0 +1,257 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpProbingMuOracle.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpProbingMuOracle.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+#include <cstdio>
+
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+
+
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ ProbingMuOracle::ProbingMuOracle(const SmartPtr<PDSystemSolver>& pd_solver)
+ :
+ MuOracle(),
+ pd_solver_(pd_solver)
+ {
+ DBG_ASSERT(IsValid(pd_solver_));
+ }
+
+ ProbingMuOracle::~ProbingMuOracle()
+ {}
+
+ void ProbingMuOracle::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ // None to register...
+ }
+
+ bool ProbingMuOracle::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ options.GetNumericValue("sigma_max", sigma_max_, prefix);
+
+ return true;
+ }
+
+ bool ProbingMuOracle::CalculateMu(Number mu_min, Number mu_max,
+ Number& new_mu)
+ {
+ DBG_START_METH("ProbingMuOracle::CalculateMu",
+ dbg_verbosity);
+
+ /////////////////////////////////////
+ // Compute the affine scaling step //
+ /////////////////////////////////////
+
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "Solving the Primal Dual System for the affine step\n");
+ // First get the right hand side
+ SmartPtr<IteratesVector> rhs = IpData().curr()->MakeNewContainer();
+
+ rhs->Set_x(*IpCq().curr_grad_lag_x());
+ rhs->Set_s(*IpCq().curr_grad_lag_s());
+ rhs->Set_y_c(*IpCq().curr_c());
+ rhs->Set_y_d(*IpCq().curr_d_minus_s());
+ rhs->Set_z_L(*IpCq().curr_compl_x_L());
+ rhs->Set_z_U(*IpCq().curr_compl_x_U());
+ rhs->Set_v_L(*IpCq().curr_compl_s_L());
+ rhs->Set_v_U(*IpCq().curr_compl_s_U());
+
+ // Get space for the affine scaling step
+ SmartPtr<IteratesVector> step = rhs->MakeNewIteratesVector(true);
+
+ // Now solve the primal-dual system to get the affine step. We
+ // allow a somewhat inexact solution here
+ bool allow_inexact = true;
+ bool retval = pd_solver_->Solve(-1.0, 0.0,
+ *rhs,
+ *step,
+ allow_inexact
+ );
+ if (!retval) {
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "The linear system could not be solved for the affine step!\n");
+ return false;
+ }
+
+ DBG_PRINT_VECTOR(2, "step", *step);
+
+ /////////////////////////////////////////////////////////////
+ // Use Mehrotra's formula to compute the barrier parameter //
+ /////////////////////////////////////////////////////////////
+
+ // First compute the fraction-to-the-boundary step sizes
+ Number alpha_primal_aff = IpCq().primal_frac_to_the_bound(1.0,
+ *step->x(),
+ *step->s());
+
+ Number alpha_dual_aff = IpCq().dual_frac_to_the_bound(1.0,
+ *step->z_L(),
+ *step->z_U(),
+ *step->v_L(),
+ *step->v_U());
+
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ " The affine maximal step sizes are\n"
+ " alpha_primal_aff = %23.16e\n"
+ " alpha_dual_aff = %23.16e\n",
+ alpha_primal_aff,
+ alpha_dual_aff);
+
+ // now compute the average complementarity at the affine step
+ // ToDo shoot for mu_min instead of 0?
+ Number mu_aff = CalculateAffineMu(alpha_primal_aff, alpha_dual_aff, *step);
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ " The average complementariy at the affine step is %23.16e\n",
+ mu_aff);
+
+ // get the current average complementarity
+ Number mu_curr = IpCq().curr_avrg_compl();
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ " The average complementariy at the current point is %23.16e\n",
+ mu_curr);
+ DBG_ASSERT(mu_curr>0.);
+
+ // Apply Mehrotra's rule
+ Number sigma = pow((mu_aff/mu_curr),3);
+ // Make sure, sigma is not too large
+ sigma = Min(sigma, sigma_max_);
+
+ Number mu = sigma*mu_curr;
+
+ // Store the affine search direction (in case it is needed in the
+ // line search for a corrector step)
+ IpData().set_delta_aff(step);
+ IpData().SetHaveAffineDeltas(true);
+
+ char ssigma[40];
+ sprintf(ssigma, " sigma=%8.2e", sigma);
+ IpData().Append_info_string(ssigma);
+ //sprintf(ssigma, " xi=%8.2e ", IpCq().curr_centrality_measure());
+ //IpData().Append_info_string(ssigma);
+
+ new_mu = Max(Min(mu, mu_max), mu_min);
+ return true;
+ }
+
+ Number ProbingMuOracle::CalculateAffineMu
+ (
+ Number alpha_primal,
+ Number alpha_dual,
+ const IteratesVector& step)
+ {
+ // Get the current values of the slack variables and bound multipliers
+ SmartPtr<const Vector> slack_x_L = IpCq().curr_slack_x_L();
+ SmartPtr<const Vector> slack_x_U = IpCq().curr_slack_x_U();
+ SmartPtr<const Vector> slack_s_L = IpCq().curr_slack_s_L();
+ SmartPtr<const Vector> slack_s_U = IpCq().curr_slack_s_U();
+
+ SmartPtr<const Vector> z_L = IpData().curr()->z_L();
+ SmartPtr<const Vector> z_U = IpData().curr()->z_U();
+ SmartPtr<const Vector> v_L = IpData().curr()->v_L();
+ SmartPtr<const Vector> v_U = IpData().curr()->v_U();
+
+ SmartPtr<Vector> tmp_slack;
+ SmartPtr<Vector> tmp_mult;
+ SmartPtr<const Matrix> P;
+ Index ncomp = 0;
+ Number sum =0.;
+
+ // For each combination of slack and multiplier, compute the new
+ // values and their dot products.
+
+ // slack_x_L
+ if (slack_x_L->Dim()>0) {
+ ncomp += slack_x_L->Dim();
+
+ P = IpNLP().Px_L();
+ tmp_slack = slack_x_L->MakeNew();
+ tmp_slack->Copy(*slack_x_L);
+ P->TransMultVector(alpha_primal, *step.x(), 1.0, *tmp_slack);
+
+ tmp_mult = z_L->MakeNew();
+ tmp_mult->Copy(*z_L);
+ tmp_mult->Axpy(alpha_dual, *step.z_L());
+
+ sum += tmp_slack->Dot(*tmp_mult);
+ }
+
+ // slack_x_U
+ if (slack_x_U->Dim()>0) {
+ ncomp += slack_x_U->Dim();
+
+ P = IpNLP().Px_U();
+ tmp_slack = slack_x_U->MakeNew();
+ tmp_slack->Copy(*slack_x_U);
+ P->TransMultVector(-alpha_primal, *step.x(), 1.0, *tmp_slack);
+
+ tmp_mult = z_U->MakeNew();
+ tmp_mult->Copy(*z_U);
+ tmp_mult->Axpy(alpha_dual, *step.z_U());
+
+ sum += tmp_slack->Dot(*tmp_mult);
+ }
+
+ // slack_s_L
+ if (slack_s_L->Dim()>0) {
+ ncomp += slack_s_L->Dim();
+
+ P = IpNLP().Pd_L();
+ tmp_slack = slack_s_L->MakeNew();
+ tmp_slack->Copy(*slack_s_L);
+ P->TransMultVector(alpha_primal, *step.s(), 1.0, *tmp_slack);
+
+ tmp_mult = v_L->MakeNew();
+ tmp_mult->Copy(*v_L);
+ tmp_mult->Axpy(alpha_dual, *step.v_L());
+
+ sum += tmp_slack->Dot(*tmp_mult);
+ }
+
+ // slack_s_U
+ if (slack_s_U->Dim()>0) {
+ ncomp += slack_s_U->Dim();
+
+ P = IpNLP().Pd_U();
+ tmp_slack = slack_s_U->MakeNew();
+ tmp_slack->Copy(*slack_s_U);
+ P->TransMultVector(-alpha_primal, *step.s(), 1.0, *tmp_slack);
+
+ tmp_mult = v_U->MakeNew();
+ tmp_mult->Copy(*v_U);
+ tmp_mult->Axpy(alpha_dual, *step.v_U());
+
+ sum += tmp_slack->Dot(*tmp_mult);
+ }
+
+ DBG_ASSERT(ncomp>0);
+
+ return sum/((Number)ncomp);
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpProbingMuOracle.hpp b/SimTKmath/Optimizers/src/IpOpt/IpProbingMuOracle.hpp
new file mode 100644
index 0000000..1ce7703
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpProbingMuOracle.hpp
@@ -0,0 +1,87 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpProbingMuOracle.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPPROBINGMUORACLE_HPP__
+#define __IPPROBINGMUORACLE_HPP__
+
+#include "IpMuOracle.hpp"
+#include "IpPDSystemSolver.hpp"
+
+namespace Ipopt
+{
+
+ /** Implementation of the probing strategy for computing the
+ * barrier parameter.
+ */
+ class ProbingMuOracle : public MuOracle
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor */
+ ProbingMuOracle(const SmartPtr<PDSystemSolver>& pd_solver);
+ /** Default destructor */
+ virtual ~ProbingMuOracle();
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Method for computing the value of the barrier parameter that
+ * could be used in the current iteration (using Mehrotra's
+ * probing heuristic).
+ */
+ virtual bool CalculateMu(Number mu_min, Number mu_max, Number& new_mu);
+
+ /** Methods for IpoptType */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+
+ /** Default Constructor */
+ ProbingMuOracle();
+ /** Copy Constructor */
+ ProbingMuOracle(const ProbingMuOracle&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const ProbingMuOracle&);
+ //@}
+
+ /** Pointer to the object that should be used to solve the
+ * primal-dual system.
+ */
+ SmartPtr<PDSystemSolver> pd_solver_;
+
+ /** Auxilliary function for computing the average complementarity
+ * at a point, given step sizes and step
+ */
+ Number CalculateAffineMu(Number alpha_primal,
+ Number alpha_dual,
+ const IteratesVector& step);
+
+ /** @name Algorithmic parameters */
+ //@{
+ /** safeguarding upper bound on centering parameter sigma */
+ Number sigma_max_;
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpQualityFunctionMuOracle.cpp b/SimTKmath/Optimizers/src/IpOpt/IpQualityFunctionMuOracle.cpp
new file mode 100644
index 0000000..6266280
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpQualityFunctionMuOracle.cpp
@@ -0,0 +1,1300 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpQualityFunctionMuOracle.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-11-12
+
+#include "IpQualityFunctionMuOracle.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+#include <cstdio>
+
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ QualityFunctionMuOracle::QualityFunctionMuOracle(const SmartPtr<PDSystemSolver>& pd_solver)
+ :
+ MuOracle(),
+ pd_solver_(pd_solver),
+
+ tmp_step_x_L_(NULL),
+ tmp_step_x_U_(NULL),
+ tmp_step_s_L_(NULL),
+ tmp_step_s_U_(NULL),
+ tmp_step_z_L_(NULL),
+ tmp_step_z_U_(NULL),
+ tmp_step_v_L_(NULL),
+ tmp_step_v_U_(NULL),
+
+ tmp_slack_x_L_(NULL),
+ tmp_slack_x_U_(NULL),
+ tmp_slack_s_L_(NULL),
+ tmp_slack_s_U_(NULL),
+ tmp_z_L_(NULL),
+ tmp_z_U_(NULL),
+ tmp_v_L_(NULL),
+ tmp_v_U_(NULL),
+
+ count_qf_evals_(0)
+ {
+ DBG_ASSERT(IsValid(pd_solver_));
+ }
+
+ QualityFunctionMuOracle::~QualityFunctionMuOracle()
+ {}
+
+ void QualityFunctionMuOracle::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->AddLowerBoundedNumberOption(
+ "sigma_max",
+ "Maximum value of the centering parameter.",
+ 0.0, true, 1e2,
+ "This is the upper bound for the centering parameter chosen by the "
+ "quality function based barrier parameter update. (Only used if option "
+ "\"mu_oracle\" is set to \"quality-function\".)");
+ roptions->AddLowerBoundedNumberOption(
+ "sigma_min",
+ "Minimum value of the centering parameter.",
+ 0.0, false, 1e-6,
+ "This is the lower bound for the centering parameter chosen by the "
+ "quality function based barrier parameter update. (Only used if option "
+ "\"mu_oracle\" is set to \"quality-function\".)");
+ roptions->AddStringOption4(
+ "quality_function_norm_type",
+ "Norm used for components of the quality function.",
+ "2-norm-squared",
+ "1-norm", "use the 1-norm (abs sum)",
+ "2-norm-squared", "use the 2-norm squared (sum of squares)",
+ "max-norm", "use the infinity norm (max)",
+ "2-norm", "use 2-norm",
+ "(Only used if option \"mu_oracle\" is set to \"quality-function\".)");
+ roptions->AddStringOption4(
+ "quality_function_centrality",
+ "The penalty term for centrality that is included in quality function.",
+ "none",
+ "none", "no penalty term is added",
+ "log", "complementarity * the log of the centrality measure",
+ "reciprocal", "complementarity * the reciprocal of the centrality measure",
+ "cubed-reciprocal", "complementarity * the reciprocal of the centrality measure cubed",
+ "This determines whether a term is added to the quality function to "
+ "penalize deviation from centrality with respect to complementarity. The "
+ "complementarity measure here is the xi in the Loqo update rule. (Only used if option "
+ "\"mu_oracle\" is set to \"quality-function\".)");
+ roptions->AddStringOption2(
+ "quality_function_balancing_term",
+ "The balancing term included in the quality function for centrality.",
+ "none",
+ "none", "no balancing term is added",
+ "cubic", "Max(0,Max(dual_inf,primal_inf)-compl)^3",
+ "This determines whether a term is added to the quality function that "
+ "penalizes situations where the complementarity is much smaller "
+ "than dual and primal infeasibilities. (Only used if option "
+ "\"mu_oracle\" is set to \"quality-function\".)");
+ roptions->AddLowerBoundedIntegerOption(
+ "quality_function_max_section_steps",
+ "Maximum number of search steps during direct search procedure "
+ "determining the optimal centering parameter.",
+ 0, 8,
+ "The golden section search is performed for the quality function based "
+ "mu oracle. (Only used if option "
+ "\"mu_oracle\" is set to \"quality-function\".)");
+ roptions->AddBoundedNumberOption(
+ "quality_function_section_sigma_tol",
+ "Tolerance for the section search procedure determining "
+ "the optimal centering parameter (in sigma space).",
+ 0.0, false, 1.0, true,
+ 1e-2,
+ "The golden section search is performed for the quality function based "
+ "mu oractle. (Only used if option "
+ "\"mu_oracle\" is set to \"quality-function\".)");
+ roptions->AddBoundedNumberOption(
+ "quality_function_section_qf_tol",
+ "Tolerance for the golden section search procedure determining "
+ "the optimal centering parameter (in the function value space).",
+ 0.0, false, 1.0, true,
+ 0e-2,
+ "The golden section search is performed for the quality function based mu "
+ "oractle. (Only used if option "
+ "\"mu_oracle\" is set to \"quality-function\".)");
+ }
+
+
+ bool QualityFunctionMuOracle::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ Index enum_int;
+
+ options.GetNumericValue("sigma_max", sigma_max_, prefix);
+ options.GetNumericValue("sigma_min", sigma_min_, prefix);
+
+ options.GetEnumValue("quality_function_norm_type", enum_int, prefix);
+ quality_function_norm_ = NormEnum(enum_int);
+ options.GetEnumValue("quality_function_centrality", enum_int, prefix);
+ quality_function_centrality_ = CentralityEnum(enum_int);
+ options.GetEnumValue("quality_function_balancing_term", enum_int, prefix);
+ quality_function_balancing_term_ = BalancingTermEnum(enum_int);
+ options.GetIntegerValue("quality_function_max_section_steps",
+ quality_function_max_section_steps_, prefix);
+ options.GetNumericValue("quality_function_section_sigma_tol",
+ quality_function_section_sigma_tol_, prefix);
+ options.GetNumericValue("quality_function_section_qf_tol",
+ quality_function_section_qf_tol_, prefix);
+
+ initialized_ = false;
+
+ return true;
+ }
+
+ bool QualityFunctionMuOracle::CalculateMu(Number mu_min, Number mu_max,
+ Number& new_mu)
+ {
+ DBG_START_METH("QualityFunctionMuOracle::CalculateMu",
+ dbg_verbosity);
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Reserve memory for temporary vectors used in CalculateQualityFunction //
+ ///////////////////////////////////////////////////////////////////////////
+
+ tmp_step_x_L_ = IpNLP().x_L()->MakeNew();
+ tmp_step_x_U_ = IpNLP().x_U()->MakeNew();
+ tmp_step_s_L_ = IpNLP().d_L()->MakeNew();
+ tmp_step_s_U_ = IpNLP().d_U()->MakeNew();
+ tmp_step_z_L_ = IpNLP().x_L()->MakeNew();
+ tmp_step_z_U_ = IpNLP().x_U()->MakeNew();
+ tmp_step_v_L_ = IpNLP().d_L()->MakeNew();
+ tmp_step_v_U_ = IpNLP().d_U()->MakeNew();
+
+ tmp_slack_x_L_ = IpNLP().x_L()->MakeNew();
+ tmp_slack_x_U_ = IpNLP().x_U()->MakeNew();
+ tmp_slack_s_L_ = IpNLP().d_L()->MakeNew();
+ tmp_slack_s_U_ = IpNLP().d_U()->MakeNew();
+ tmp_z_L_ = IpNLP().x_L()->MakeNew();
+ tmp_z_U_ = IpNLP().x_U()->MakeNew();
+ tmp_v_L_ = IpNLP().d_L()->MakeNew();
+ tmp_v_U_ = IpNLP().d_U()->MakeNew();
+
+ /////////////////////////////////////
+ // Compute the affine scaling step //
+ /////////////////////////////////////
+
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "Solving the Primal Dual System for the affine step\n");
+ // First get the right hand side
+ SmartPtr<IteratesVector> rhs_aff = IpData().curr()->MakeNewIteratesVector(false);
+ rhs_aff->Set_x(*IpCq().curr_grad_lag_x());
+ rhs_aff->Set_s(*IpCq().curr_grad_lag_s());
+ rhs_aff->Set_y_c(*IpCq().curr_c());
+ rhs_aff->Set_y_d(*IpCq().curr_d_minus_s());
+ rhs_aff->Set_z_L(*IpCq().curr_compl_x_L());
+ rhs_aff->Set_z_U(*IpCq().curr_compl_x_U());
+ rhs_aff->Set_v_L(*IpCq().curr_compl_s_L());
+ rhs_aff->Set_v_U(*IpCq().curr_compl_s_U());
+
+ // Get space for the affine scaling step
+ SmartPtr<IteratesVector> step_aff = IpData().curr()->MakeNewIteratesVector(true);
+
+ // Now solve the primal-dual system to get the step. We allow a
+ // somewhat inexact solution, iterative refinement will be done
+ // after mu is known
+ bool allow_inexact = true;
+ bool retval = pd_solver_->Solve(-1.0, 0.0,
+ *rhs_aff,
+ *step_aff,
+ allow_inexact
+ );
+ if (!retval) {
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "The linear system could not be solved for the affine step!\n");
+ return false;
+ }
+
+ DBG_PRINT_VECTOR(2, "step_aff", *step_aff);
+
+ /////////////////////////////////////
+ // Compute the pure centering step //
+ /////////////////////////////////////
+
+ Number avrg_compl = IpCq().curr_avrg_compl();
+
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "Solving the Primal Dual System for the centering step\n");
+ // First get the right hand side
+ SmartPtr<IteratesVector> rhs_cen = IpData().curr()->MakeNewIteratesVector(true);
+ rhs_cen->x_NonConst()->AddOneVector(-avrg_compl,
+ *IpCq().grad_kappa_times_damping_x(),
+ 0.);
+ rhs_cen->s_NonConst()->AddOneVector(-avrg_compl,
+ *IpCq().grad_kappa_times_damping_s(),
+ 0.);
+
+ rhs_cen->y_c_NonConst()->Set(0.);
+ rhs_cen->y_d_NonConst()->Set(0.);
+ rhs_cen->z_L_NonConst()->Set(avrg_compl);
+ rhs_cen->z_U_NonConst()->Set(avrg_compl);
+ rhs_cen->v_L_NonConst()->Set(avrg_compl);
+ rhs_cen->v_U_NonConst()->Set(avrg_compl);
+
+ // Get space for the centering step
+ SmartPtr<IteratesVector> step_cen = IpData().curr()->MakeNewIteratesVector(true);
+
+ // Now solve the primal-dual system to get the step
+ allow_inexact = true;
+ retval = pd_solver_->Solve(1.0, 0.0,
+ *rhs_cen,
+ *step_cen,
+ allow_inexact
+ );
+ if (!retval) {
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "The linear system could not be solved for the centering step!\n");
+ return false;
+ }
+
+ DBG_PRINT_VECTOR(2, "step_cen", *step_cen);
+
+ // Start the timing for the quality function search here
+ IpData().TimingStats().QualityFunctionSearch().Start();
+
+ // Some initializations
+ if (!initialized_) {
+ n_dual_ = IpData().curr()->x()->Dim() + IpData().curr()->s()->Dim();
+ n_pri_ = IpData().curr()->y_c()->Dim() + IpData().curr()->y_d()->Dim();
+ n_comp_ = IpData().curr()->z_L()->Dim() + IpData().curr()->z_U()->Dim() +
+ IpData().curr()->v_L()->Dim() + IpData().curr()->v_U()->Dim();
+ initialized_ = true;
+ }
+
+ count_qf_evals_ = 0;
+
+ // Compute some quantities used for the quality function evaluations
+ // (This way we try to avoid retrieving numbers from cache...
+
+ curr_slack_x_L_ = IpCq().curr_slack_x_L();
+ curr_slack_x_U_ = IpCq().curr_slack_x_U();
+ curr_slack_s_L_ = IpCq().curr_slack_s_L();
+ curr_slack_s_U_ = IpCq().curr_slack_s_U();
+
+ curr_z_L_ = IpData().curr()->z_L();
+ curr_z_U_ = IpData().curr()->z_U();
+ curr_v_L_ = IpData().curr()->v_L();
+ curr_v_U_ = IpData().curr()->v_U();
+
+ IpData().TimingStats().Task5().Start();
+ switch (quality_function_norm_) {
+ case NM_NORM_1:
+ curr_grad_lag_x_asum_ = IpCq().curr_grad_lag_x()->Asum();
+ curr_grad_lag_s_asum_ = IpCq().curr_grad_lag_s()->Asum();
+ curr_c_asum_ = IpCq().curr_c()->Asum();
+ curr_d_minus_s_asum_ = IpCq().curr_d_minus_s()->Asum();
+ break;
+ case NM_NORM_2_SQUARED:
+ case NM_NORM_2:
+ curr_grad_lag_x_nrm2_ = IpCq().curr_grad_lag_x()->Nrm2();
+ curr_grad_lag_s_nrm2_ = IpCq().curr_grad_lag_s()->Nrm2();
+ curr_c_nrm2_ = IpCq().curr_c()->Nrm2();
+ curr_d_minus_s_nrm2_ = IpCq().curr_d_minus_s()->Nrm2();
+ break;
+ case NM_NORM_MAX:
+ curr_grad_lag_x_amax_ = IpCq().curr_grad_lag_x()->Amax();
+ curr_grad_lag_s_amax_ = IpCq().curr_grad_lag_s()->Amax();
+ curr_c_amax_ = IpCq().curr_c()->Amax();
+ curr_d_minus_s_amax_ = IpCq().curr_d_minus_s()->Amax();
+ break;
+ default:
+ DBG_ASSERT(false && "Unknown value for quality_function_norm_");
+ }
+ IpData().TimingStats().Task5().End();
+
+ // Some initializations
+ if (!initialized_) {
+ n_dual_ = IpData().curr()->x()->Dim() + IpData().curr()->s()->Dim();
+ n_pri_ = IpData().curr()->y_c()->Dim() + IpData().curr()->y_d()->Dim();
+ n_comp_ = IpData().curr()->z_L()->Dim() + IpData().curr()->z_U()->Dim() +
+ IpData().curr()->v_L()->Dim() + IpData().curr()->v_U()->Dim();
+ initialized_ = true;
+ }
+
+ count_qf_evals_ = 0;
+
+ // Compute some quantities used for the quality function evaluations
+ // (This way we try to avoid retrieving numbers from cache...
+
+ curr_slack_x_L_ = IpCq().curr_slack_x_L();
+ curr_slack_x_U_ = IpCq().curr_slack_x_U();
+ curr_slack_s_L_ = IpCq().curr_slack_s_L();
+ curr_slack_s_U_ = IpCq().curr_slack_s_U();
+
+ curr_z_L_ = IpData().curr()->z_L();
+ curr_z_U_ = IpData().curr()->z_U();
+ curr_v_L_ = IpData().curr()->v_L();
+ curr_v_U_ = IpData().curr()->v_U();
+
+ IpData().TimingStats().Task5().Start();
+ switch (quality_function_norm_) {
+ case NM_NORM_1:
+ curr_grad_lag_x_asum_ = IpCq().curr_grad_lag_x()->Asum();
+ curr_grad_lag_s_asum_ = IpCq().curr_grad_lag_s()->Asum();
+ curr_c_asum_ = IpCq().curr_c()->Asum();
+ curr_d_minus_s_asum_ = IpCq().curr_d_minus_s()->Asum();
+ break;
+ case NM_NORM_2_SQUARED:
+ case NM_NORM_2:
+ curr_grad_lag_x_nrm2_ = IpCq().curr_grad_lag_x()->Nrm2();
+ curr_grad_lag_s_nrm2_ = IpCq().curr_grad_lag_s()->Nrm2();
+ curr_c_nrm2_ = IpCq().curr_c()->Nrm2();
+ curr_d_minus_s_nrm2_ = IpCq().curr_d_minus_s()->Nrm2();
+ break;
+ case NM_NORM_MAX:
+ curr_grad_lag_x_amax_ = IpCq().curr_grad_lag_x()->Amax();
+ curr_grad_lag_s_amax_ = IpCq().curr_grad_lag_s()->Amax();
+ curr_c_amax_ = IpCq().curr_c()->Amax();
+ curr_d_minus_s_amax_ = IpCq().curr_d_minus_s()->Amax();
+ break;
+ default:
+ DBG_ASSERT(false && "Unknown value for quality_function_norm_");
+ }
+ IpData().TimingStats().Task5().End();
+
+ // We now compute the step for the slack variables. This safes
+ // time, because we then don't have to do this any more for each
+ // evaluation of the quality function
+ SmartPtr<Vector> step_aff_x_L = step_aff->z_L()->MakeNew();
+ SmartPtr<Vector> step_aff_x_U = step_aff->z_U()->MakeNew();
+ SmartPtr<Vector> step_aff_s_L = step_aff->v_L()->MakeNew();
+ SmartPtr<Vector> step_aff_s_U = step_aff->v_U()->MakeNew();
+ IpNLP().Px_L()->TransMultVector(1., *step_aff->x(), 0., *step_aff_x_L);
+ IpNLP().Px_U()->TransMultVector(-1., *step_aff->x(), 0., *step_aff_x_U);
+ IpNLP().Pd_L()->TransMultVector(1., *step_aff->s(), 0., *step_aff_s_L);
+ IpNLP().Pd_U()->TransMultVector(-1., *step_aff->s(), 0., *step_aff_s_U);
+ SmartPtr<Vector> step_cen_x_L = step_cen->z_L()->MakeNew();
+ SmartPtr<Vector> step_cen_x_U = step_cen->z_U()->MakeNew();
+ SmartPtr<Vector> step_cen_s_L = step_cen->v_L()->MakeNew();
+ SmartPtr<Vector> step_cen_s_U = step_cen->v_U()->MakeNew();
+ IpNLP().Px_L()->TransMultVector(1., *step_cen->x(), 0., *step_cen_x_L);
+ IpNLP().Px_U()->TransMultVector(-1., *step_cen->x(), 0., *step_cen_x_U);
+ IpNLP().Pd_L()->TransMultVector(1., *step_cen->s(), 0., *step_cen_s_L);
+ IpNLP().Pd_U()->TransMultVector(-1., *step_cen->s(), 0., *step_cen_s_U);
+
+ Number sigma;
+
+ // First we determine whether we want to search for a value of
+ // sigma larger or smaller than 1. For this, we estimate the
+ // slope of the quality function at sigma=1.
+ Number qf_1 = CalculateQualityFunction(1.,
+ *step_aff_x_L,
+ *step_aff_x_U,
+ *step_aff_s_L,
+ *step_aff_s_U,
+ *step_aff->y_c(),
+ *step_aff->y_d(),
+ *step_aff->z_L(),
+ *step_aff->z_U(),
+ *step_aff->v_L(),
+ *step_aff->v_U(),
+ *step_cen_x_L,
+ *step_cen_x_U,
+ *step_cen_s_L,
+ *step_cen_s_U,
+ *step_cen->y_c(),
+ *step_cen->y_d(),
+ *step_cen->z_L(),
+ *step_cen->z_U(),
+ *step_cen->v_L(),
+ *step_cen->v_U());
+
+ Number sigma_1minus = 1-Max(Number(1e-4), quality_function_section_sigma_tol_);
+ Number qf_1minus = CalculateQualityFunction(sigma_1minus,
+ *step_aff_x_L,
+ *step_aff_x_U,
+ *step_aff_s_L,
+ *step_aff_s_U,
+ *step_aff->y_c(),
+ *step_aff->y_d(),
+ *step_aff->z_L(),
+ *step_aff->z_U(),
+ *step_aff->v_L(),
+ *step_aff->v_U(),
+ *step_cen_x_L,
+ *step_cen_x_U,
+ *step_cen_s_L,
+ *step_cen_s_U,
+ *step_cen->y_c(),
+ *step_cen->y_d(),
+ *step_cen->z_L(),
+ *step_cen->z_U(),
+ *step_cen->v_L(),
+ *step_cen->v_U());
+
+ if (qf_1minus > qf_1) {
+ // It seems that the quality function decreases for values
+ // larger than sigma, so perform golden section search for sigma
+ // > 1.
+ Number sigma_up = Min(sigma_max_, mu_max/avrg_compl);
+ Number sigma_lo = 1.;
+ if (sigma_lo >= sigma_up) {
+ sigma = sigma_up;
+ }
+ else {
+ // ToDo maybe we should use different tolerances for sigma>1
+ sigma = PerformGoldenSection(sigma_up, -100., sigma_lo, qf_1,
+ quality_function_section_sigma_tol_,
+ quality_function_section_qf_tol_,
+ *step_aff_x_L,
+ *step_aff_x_U,
+ *step_aff_s_L,
+ *step_aff_s_U,
+ *step_aff->y_c(),
+ *step_aff->y_d(),
+ *step_aff->z_L(),
+ *step_aff->z_U(),
+ *step_aff->v_L(),
+ *step_aff->v_U(),
+ *step_cen_x_L,
+ *step_cen_x_U,
+ *step_cen_s_L,
+ *step_cen_s_U,
+ *step_cen->y_c(),
+ *step_cen->y_d(),
+ *step_cen->z_L(),
+ *step_cen->z_U(),
+ *step_cen->v_L(),
+ *step_cen->v_U());
+ }
+ }
+ else {
+ // Search for sigma less than 1
+
+ Number sigma_lo = Max(sigma_min_, mu_min/avrg_compl);
+ Number sigma_up = Min(Max(sigma_lo, sigma_1minus), mu_max/avrg_compl);
+ if (sigma_lo >= sigma_up) {
+ // Skip the search, we are already at the minimum
+ sigma = sigma_lo;
+ }
+ else {
+ sigma = PerformGoldenSection(sigma_up, qf_1minus, sigma_lo, -100.,
+ quality_function_section_sigma_tol_,
+ quality_function_section_qf_tol_,
+ *step_aff_x_L,
+ *step_aff_x_U,
+ *step_aff_s_L,
+ *step_aff_s_U,
+ *step_aff->y_c(),
+ *step_aff->y_d(),
+ *step_aff->z_L(),
+ *step_aff->z_U(),
+ *step_aff->v_L(),
+ *step_aff->v_U(),
+ *step_cen_x_L,
+ *step_cen_x_U,
+ *step_cen_s_L,
+ *step_cen_s_U,
+ *step_cen->y_c(),
+ *step_cen->y_d(),
+ *step_cen->z_L(),
+ *step_cen->z_U(),
+ *step_cen->v_L(),
+ *step_cen->v_U());
+ }
+ }
+
+ //#define tracequalityfunction
+#ifdef tracequalityfunction
+ char fname[100];
+ sprintf(fname, "qf_values_%d.dat", IpData().iter_count());
+ FILE* fid = fopen(fname, "w");
+
+ Number sigma_1 = sigma_max_;
+ Number sigma_2 = 1e-9/avrg_compl;
+ Number sigma_trace = sigma_1;
+ while(sigma_trace > sigma_2) {
+ Number qf = CalculateQualityFunction(sigma_trace,
+ *step_aff_x_L,
+ *step_aff_x_U,
+ *step_aff_s_L,
+ *step_aff_s_U,
+ *step_aff->y_c(),
+ *step_aff->y_d(),
+ *step_aff->z_L(),
+ *step_aff->z_U(),
+ *step_aff->v_L(),
+ *step_aff->v_U(),
+ *step_cen_x_L,
+ *step_cen_x_U,
+ *step_cen_s_L,
+ *step_cen_s_U,
+ *step_cen->y_c(),
+ *step_cen->y_d(),
+ *step_cen->z_L(),
+ *step_cen->z_U(),
+ *step_cen->v_L(),
+ *step_cen->v_U());
+ fprintf(fid, "%9.2e %25.16e\n", sigma_trace, qf);
+ sigma_trace /= 1.1;
+ }
+ fclose(fid);
+#endif
+
+ // End timing of quality function search
+ IpData().TimingStats().QualityFunctionSearch().End();
+
+ Jnlst().Printf(J_DETAILED, J_BARRIER_UPDATE,
+ "Sigma = %e\n", sigma);
+ Number mu = sigma*avrg_compl;
+
+ // Store the affine search direction (in case it is needed in the
+ // line search for a corrector step)
+ IpData().set_delta_aff(step_aff);
+ IpData().SetHaveAffineDeltas(true);
+
+ // Now construct the overall search direction here
+ SmartPtr<IteratesVector> step = IpData().curr()->MakeNewIteratesVector(true);
+ step->AddTwoVectors(sigma, *step_cen, 1.0, *IpData().delta_aff(), 0.0);
+
+ DBG_PRINT_VECTOR(2, "step", *step);
+ IpData().set_delta(step);
+ IpData().SetHaveDeltas(true);
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Release memory for temporary vectors used in CalculateQualityFunction //
+ ///////////////////////////////////////////////////////////////////////////
+
+ tmp_step_x_L_ = NULL;
+ tmp_step_x_U_ = NULL;
+ tmp_step_s_L_ = NULL;
+ tmp_step_s_U_ = NULL;
+ tmp_step_z_L_ = NULL;
+ tmp_step_z_U_ = NULL;
+ tmp_step_v_L_ = NULL;
+ tmp_step_v_U_ = NULL;
+
+ tmp_slack_x_L_ = NULL;
+ tmp_slack_x_U_ = NULL;
+ tmp_slack_s_L_ = NULL;
+ tmp_slack_s_U_ = NULL;
+ tmp_z_L_ = NULL;
+ tmp_z_U_ = NULL;
+ tmp_v_L_ = NULL;
+ tmp_v_U_ = NULL;
+
+ curr_slack_x_L_ = NULL;
+ curr_slack_x_U_ = NULL;
+ curr_slack_s_L_ = NULL;
+ curr_slack_s_U_ = NULL;
+
+ // DELETEME
+ char ssigma[40];
+ sprintf(ssigma, " sigma=%8.2e", sigma);
+ IpData().Append_info_string(ssigma);
+ sprintf(ssigma, " qf=%d", count_qf_evals_);
+ IpData().Append_info_string(ssigma);
+ /*
+ sprintf(ssigma, " xi=%8.2e ", IpCq().curr_centrality_measure());
+ IpData().Append_info_string(ssigma);
+ if (sigma>1.) {
+ IpData().Append_info_string("LARGESIGMA");
+ }
+ */
+
+ new_mu = mu;
+ return true;
+ }
+
+ Number QualityFunctionMuOracle::CalculateQualityFunction
+ (Number sigma,
+ const Vector& step_aff_x_L,
+ const Vector& step_aff_x_U,
+ const Vector& step_aff_s_L,
+ const Vector& step_aff_s_U,
+ const Vector& step_aff_y_c,
+ const Vector& step_aff_y_d,
+ const Vector& step_aff_z_L,
+ const Vector& step_aff_z_U,
+ const Vector& step_aff_v_L,
+ const Vector& step_aff_v_U,
+ const Vector& step_cen_x_L,
+ const Vector& step_cen_x_U,
+ const Vector& step_cen_s_L,
+ const Vector& step_cen_s_U,
+ const Vector& step_cen_y_c,
+ const Vector& step_cen_y_d,
+ const Vector& step_cen_z_L,
+ const Vector& step_cen_z_U,
+ const Vector& step_cen_v_L,
+ const Vector& step_cen_v_U
+ )
+ {
+ DBG_START_METH("QualityFunctionMuOracle::CalculateQualityFunction",
+ dbg_verbosity);
+ count_qf_evals_++;
+
+ IpData().TimingStats().Task1().Start();
+ tmp_step_x_L_->AddTwoVectors(1., step_aff_x_L, sigma, step_cen_x_L, 0.);
+ tmp_step_x_U_->AddTwoVectors(1., step_aff_x_U, sigma, step_cen_x_U, 0.);
+ tmp_step_s_L_->AddTwoVectors(1., step_aff_s_L, sigma, step_cen_s_L, 0.);
+ tmp_step_s_U_->AddTwoVectors(1., step_aff_s_U, sigma, step_cen_s_U, 0.);
+ tmp_step_z_L_->AddTwoVectors(1., step_aff_z_L, sigma, step_cen_z_L, 0.);
+ tmp_step_z_U_->AddTwoVectors(1., step_aff_z_U, sigma, step_cen_z_U, 0.);
+ tmp_step_v_L_->AddTwoVectors(1., step_aff_v_L, sigma, step_cen_v_L, 0.);
+ tmp_step_v_U_->AddTwoVectors(1., step_aff_v_U, sigma, step_cen_v_U, 0.);
+ IpData().TimingStats().Task1().End();
+
+ // Compute the fraction-to-the-boundary step sizes
+ IpData().TimingStats().Task2().Start();
+ Number tau = IpData().curr_tau();
+ Number alpha_primal = IpCq().uncached_slack_frac_to_the_bound(tau,
+ *tmp_step_x_L_,
+ *tmp_step_x_U_,
+ *tmp_step_s_L_,
+ *tmp_step_s_U_);
+
+ Number alpha_dual = IpCq().uncached_dual_frac_to_the_bound(tau,
+ *tmp_step_z_L_,
+ *tmp_step_z_U_,
+ *tmp_step_v_L_,
+ *tmp_step_v_U_);
+ IpData().TimingStats().Task2().End();
+
+ Number xi = 0.; // centrality measure
+
+ IpData().TimingStats().Task1().Start();
+ tmp_slack_x_L_->AddTwoVectors(1., *curr_slack_x_L_,
+ alpha_primal, *tmp_step_x_L_, 0.);
+ tmp_slack_x_U_->AddTwoVectors(1., *curr_slack_x_U_,
+ alpha_primal, *tmp_step_x_U_, 0.);
+ tmp_slack_s_L_->AddTwoVectors(1., *curr_slack_s_L_,
+ alpha_primal, *tmp_step_s_L_, 0.);
+ tmp_slack_s_U_->AddTwoVectors(1., *curr_slack_s_U_,
+ alpha_primal, *tmp_step_s_U_, 0.);
+
+ tmp_z_L_->AddTwoVectors(1., *curr_z_L_,
+ alpha_dual, *tmp_step_z_L_, 0.);
+ tmp_z_U_->AddTwoVectors(1., *curr_z_U_,
+ alpha_dual, *tmp_step_z_U_, 0.);
+ tmp_v_L_->AddTwoVectors(1., *curr_v_L_,
+ alpha_dual, *tmp_step_v_L_, 0.);
+ tmp_v_U_->AddTwoVectors(1., *curr_v_U_,
+ alpha_dual, *tmp_step_v_U_, 0.);
+ IpData().TimingStats().Task1().End();
+
+ IpData().TimingStats().Task3().Start();
+ tmp_slack_x_L_->ElementWiseMultiply(*tmp_z_L_);
+ tmp_slack_x_U_->ElementWiseMultiply(*tmp_z_U_);
+ tmp_slack_s_L_->ElementWiseMultiply(*tmp_v_L_);
+ tmp_slack_s_U_->ElementWiseMultiply(*tmp_v_U_);
+ IpData().TimingStats().Task3().End();
+
+ DBG_PRINT_VECTOR(2, "compl_x_L", *tmp_slack_x_L_);
+ DBG_PRINT_VECTOR(2, "compl_x_U", *tmp_slack_x_U_);
+ DBG_PRINT_VECTOR(2, "compl_s_L", *tmp_slack_s_L_);
+ DBG_PRINT_VECTOR(2, "compl_s_U", *tmp_slack_s_U_);
+
+ Number dual_inf=-1.;
+ Number primal_inf=-1.;
+ Number compl_inf=-1.;
+
+ IpData().TimingStats().Task5().Start();
+ switch (quality_function_norm_) {
+ case NM_NORM_1:
+ dual_inf = (1-alpha_dual)*(curr_grad_lag_x_asum_ +
+ curr_grad_lag_s_asum_);
+
+ primal_inf = (1-alpha_primal)*(curr_c_asum_ +
+ curr_d_minus_s_asum_);
+
+ compl_inf = tmp_slack_x_L_->Asum() + tmp_slack_x_U_->Asum() +
+ tmp_slack_s_L_->Asum() + tmp_slack_s_U_->Asum();
+
+ dual_inf /= n_dual_;
+ if (n_pri_>0) {
+ primal_inf /= n_pri_;
+ }
+ DBG_ASSERT(n_comp_>0);
+ compl_inf /= n_comp_;
+ break;
+ case NM_NORM_2_SQUARED:
+ dual_inf =
+ pow(1-alpha_dual, 2)*(pow(curr_grad_lag_x_nrm2_, 2) +
+ pow(curr_grad_lag_s_nrm2_, 2));
+ primal_inf =
+ pow(1-alpha_primal, 2)*(pow(curr_c_nrm2_, 2) +
+ pow(curr_d_minus_s_nrm2_, 2));
+ compl_inf =
+ pow(tmp_slack_x_L_->Nrm2(), 2) + pow(tmp_slack_x_U_->Nrm2(), 2) +
+ pow(tmp_slack_s_L_->Nrm2(), 2) + pow(tmp_slack_s_U_->Nrm2(), 2);
+
+ dual_inf /= n_dual_;
+ if (n_pri_>0) {
+ primal_inf /= n_pri_;
+ }
+ DBG_ASSERT(n_comp_>0);
+ compl_inf /= n_comp_;
+ break;
+ case NM_NORM_MAX:
+ dual_inf =
+ (1-alpha_dual)*Max(curr_grad_lag_x_amax_,
+ curr_grad_lag_s_amax_);
+ primal_inf =
+ (1-alpha_primal)*Max(curr_c_amax_,
+ curr_d_minus_s_amax_);
+ compl_inf =
+ Max(tmp_slack_x_L_->Amax(), tmp_slack_x_U_->Amax(),
+ tmp_slack_s_L_->Amax(), tmp_slack_s_U_->Amax());
+ break;
+ case NM_NORM_2:
+ dual_inf =
+ (1-alpha_dual)*sqrt(pow(curr_grad_lag_x_nrm2_, 2) +
+ pow(curr_grad_lag_s_nrm2_, 2));
+ primal_inf =
+ (1-alpha_primal)*sqrt(pow(curr_c_nrm2_, 2) +
+ pow(curr_d_minus_s_nrm2_, 2));
+ compl_inf =
+ sqrt(pow(tmp_slack_x_L_->Nrm2(), 2) + pow(tmp_slack_x_U_->Nrm2(), 2) +
+ pow(tmp_slack_s_L_->Nrm2(), 2) + pow(tmp_slack_s_U_->Nrm2(), 2));
+
+ dual_inf /= sqrt((Number)n_dual_);
+ if (n_pri_>0) {
+ primal_inf /= sqrt((Number)n_pri_);
+ }
+ DBG_ASSERT(n_comp_>0);
+ compl_inf /= sqrt((Number)n_comp_);
+ break;
+ default:
+ DBG_ASSERT(false && "Unknown value for quality_function_norm_");
+ }
+ IpData().TimingStats().Task5().End();
+
+ Number quality_function = dual_inf + primal_inf + compl_inf;
+
+ if (quality_function_centrality_!=CEN_NONE) {
+ IpData().TimingStats().Task4().Start();
+ xi = IpCq().CalcCentralityMeasure(*tmp_slack_x_L_, *tmp_slack_x_U_,
+ *tmp_slack_s_L_, *tmp_slack_s_U_);
+ IpData().TimingStats().Task4().End();
+ }
+ switch (quality_function_centrality_) {
+ case CEN_NONE:
+ //Nothing
+ break;
+ case CEN_LOG:
+ quality_function -= compl_inf*log(xi);
+ break;
+ case CEN_RECIPROCAL:
+ quality_function += compl_inf/xi;
+ case CEN_CUBED_RECIPROCAL:
+ quality_function += compl_inf/pow(xi,3);
+ break;
+ default:
+ DBG_ASSERT(false && "Unknown value for quality_function_centrality_");
+ }
+
+ switch (quality_function_balancing_term_) {
+ case BT_NONE:
+ //Nothing
+ break;
+ case BT_CUBIC:
+ quality_function += pow(Max(0., Max(dual_inf,primal_inf)-compl_inf),3);
+ break;
+ default:
+ DBG_ASSERT(false && "Unknown value for quality_function_balancing term_");
+ }
+
+ Jnlst().Printf(J_MOREDETAILED, J_BARRIER_UPDATE,
+ "sigma = %8.2e d_inf = %18.12e p_inf = %18.12e cmpl = %18.12e q = %18.12e a_pri = %8.2e a_dual = %8.2e xi = %8.2e\n", sigma, dual_inf, primal_inf, compl_inf, quality_function, alpha_primal, alpha_dual, xi);
+
+
+ return quality_function;
+ //return compl_inf;
+ }
+
+ Number
+ QualityFunctionMuOracle::PerformGoldenSection
+ (Number sigma_up_in,
+ Number q_up,
+ Number sigma_lo_in,
+ Number q_lo,
+ Number sigma_tol,
+ Number qf_tol,
+ const Vector& step_aff_x_L,
+ const Vector& step_aff_x_U,
+ const Vector& step_aff_s_L,
+ const Vector& step_aff_s_U,
+ const Vector& step_aff_y_c,
+ const Vector& step_aff_y_d,
+ const Vector& step_aff_z_L,
+ const Vector& step_aff_z_U,
+ const Vector& step_aff_v_L,
+ const Vector& step_aff_v_U,
+ const Vector& step_cen_x_L,
+ const Vector& step_cen_x_U,
+ const Vector& step_cen_s_L,
+ const Vector& step_cen_s_U,
+ const Vector& step_cen_y_c,
+ const Vector& step_cen_y_d,
+ const Vector& step_cen_z_L,
+ const Vector& step_cen_z_U,
+ const Vector& step_cen_v_L,
+ const Vector& step_cen_v_U
+ )
+ {
+ Number sigma_up = ScaleSigma(sigma_up_in);
+ Number sigma_lo = ScaleSigma(sigma_lo_in);
+
+ Number sigma;
+ Number gfac = (3-std::sqrt(Number(5)))/2;
+ Number sigma_mid1 = sigma_lo + gfac*(sigma_up-sigma_lo);
+ Number sigma_mid2 = sigma_lo + (1-gfac)*(sigma_up-sigma_lo);
+
+ Number qmid1 = CalculateQualityFunction(UnscaleSigma(sigma_mid1),
+ step_aff_x_L,
+ step_aff_x_U,
+ step_aff_s_L,
+ step_aff_s_U,
+ step_aff_y_c,
+ step_aff_y_d,
+ step_aff_z_L,
+ step_aff_z_U,
+ step_aff_v_L,
+ step_aff_v_U,
+ step_cen_x_L,
+ step_cen_x_U,
+ step_cen_s_L,
+ step_cen_s_U,
+ step_cen_y_c,
+ step_cen_y_d,
+ step_cen_z_L,
+ step_cen_z_U,
+ step_cen_v_L,
+ step_cen_v_U);
+ Number qmid2 = CalculateQualityFunction(UnscaleSigma(sigma_mid2),
+ step_aff_x_L,
+ step_aff_x_U,
+ step_aff_s_L,
+ step_aff_s_U,
+ step_aff_y_c,
+ step_aff_y_d,
+ step_aff_z_L,
+ step_aff_z_U,
+ step_aff_v_L,
+ step_aff_v_U,
+ step_cen_x_L,
+ step_cen_x_U,
+ step_cen_s_L,
+ step_cen_s_U,
+ step_cen_y_c,
+ step_cen_y_d,
+ step_cen_z_L,
+ step_cen_z_U,
+ step_cen_v_L,
+ step_cen_v_U);
+
+ Index nsections = 0;
+ while ((sigma_up-sigma_lo)>=sigma_tol*sigma_up &&
+ //while ((sigma_up-sigma_lo)>=sigma_tol && // Note we are using the non-relative criterion here for sigma
+ (1.-Min(q_lo, q_up, qmid1, qmid2)/Max(q_lo, q_up, qmid1, qmid2))>=qf_tol &&
+ nsections<quality_function_max_section_steps_) {
+ nsections++;
+ //printf("sigma_lo=%e sigma_mid1=%e sigma_mid2=%e sigma_up=%e\n",sigma_lo, sigma_mid1, sigma_mid2, sigma_up);
+ if (qmid1 > qmid2) {
+ sigma_lo = sigma_mid1;
+ q_lo = qmid1;
+ sigma_mid1 = sigma_mid2;
+ qmid1 = qmid2;
+ sigma_mid2 = sigma_lo + (1-gfac)*(sigma_up-sigma_lo);
+ qmid2 = CalculateQualityFunction(UnscaleSigma(sigma_mid2),
+ step_aff_x_L,
+ step_aff_x_U,
+ step_aff_s_L,
+ step_aff_s_U,
+ step_aff_y_c,
+ step_aff_y_d,
+ step_aff_z_L,
+ step_aff_z_U,
+ step_aff_v_L,
+ step_aff_v_U,
+ step_cen_x_L,
+ step_cen_x_U,
+ step_cen_s_L,
+ step_cen_s_U,
+ step_cen_y_c,
+ step_cen_y_d,
+ step_cen_z_L,
+ step_cen_z_U,
+ step_cen_v_L,
+ step_cen_v_U);
+ }
+ else {
+ sigma_up = sigma_mid2;
+ q_up = qmid2;
+ sigma_mid2 = sigma_mid1;
+ qmid2 = qmid1;
+ sigma_mid1 = sigma_lo + gfac*(sigma_up-sigma_lo);
+ qmid1 = CalculateQualityFunction(UnscaleSigma(sigma_mid1),
+ step_aff_x_L,
+ step_aff_x_U,
+ step_aff_s_L,
+ step_aff_s_U,
+ step_aff_y_c,
+ step_aff_y_d,
+ step_aff_z_L,
+ step_aff_z_U,
+ step_aff_v_L,
+ step_aff_v_U,
+ step_cen_x_L,
+ step_cen_x_U,
+ step_cen_s_L,
+ step_cen_s_U,
+ step_cen_y_c,
+ step_cen_y_d,
+ step_cen_z_L,
+ step_cen_z_U,
+ step_cen_v_L,
+ step_cen_v_U);
+ }
+ }
+
+ if ((sigma_up-sigma_lo)>=sigma_tol*sigma_up &&
+ (1.-Min(q_lo, q_up, qmid1, qmid2)/Max(q_lo, q_up, qmid1, qmid2))<qf_tol) {
+ // The qf tolerance make it stop
+ IpData().Append_info_string("qf_tol ");
+ Number qf_min = Min(q_lo, q_up, qmid1, qmid2);
+ DBG_ASSERT(qf_min>-100.);
+ if (qf_min == q_lo) {
+ sigma = sigma_lo;
+ }
+ else if (qf_min == qmid1) {
+ sigma = sigma_mid1;
+ }
+ else if (qf_min == qmid2) {
+ sigma = sigma_mid2;
+ }
+ else {
+ sigma = sigma_up;
+ }
+ }
+ else {
+ Number q;
+ if (qmid1 < qmid2) {
+ sigma = sigma_mid1;
+ q = qmid1;
+ }
+ else {
+ sigma = sigma_mid2;
+ q = qmid2;
+ }
+ if (sigma_up == ScaleSigma(sigma_up_in)) {
+ Number qtmp;
+ if (q_up<0.) {
+ qtmp = CalculateQualityFunction(UnscaleSigma(sigma_up),
+ step_aff_x_L,
+ step_aff_x_U,
+ step_aff_s_L,
+ step_aff_s_U,
+ step_aff_y_c,
+ step_aff_y_d,
+ step_aff_z_L,
+ step_aff_z_U,
+ step_aff_v_L,
+ step_aff_v_U,
+ step_cen_x_L,
+ step_cen_x_U,
+ step_cen_s_L,
+ step_cen_s_U,
+ step_cen_y_c,
+ step_cen_y_d,
+ step_cen_z_L,
+ step_cen_z_U,
+ step_cen_v_L,
+ step_cen_v_U);
+ }
+ else {
+ qtmp = q_up;
+ }
+ if (qtmp < q) {
+ sigma = sigma_up;
+ q = qtmp;
+ }
+ }
+ else if (sigma_lo == ScaleSigma(sigma_lo_in)) {
+ Number qtmp;
+ if (q_lo<0.) {
+ qtmp = CalculateQualityFunction(UnscaleSigma(sigma_lo),
+ step_aff_x_L,
+ step_aff_x_U,
+ step_aff_s_L,
+ step_aff_s_U,
+ step_aff_y_c,
+ step_aff_y_d,
+ step_aff_z_L,
+ step_aff_z_U,
+ step_aff_v_L,
+ step_aff_v_U,
+ step_cen_x_L,
+ step_cen_x_U,
+ step_cen_s_L,
+ step_cen_s_U,
+ step_cen_y_c,
+ step_cen_y_d,
+ step_cen_z_L,
+ step_cen_z_U,
+ step_cen_v_L,
+ step_cen_v_U);
+ }
+ else {
+ qtmp = q_lo;
+ }
+ if (qtmp < q) {
+ sigma = sigma_lo;
+ q = qtmp;
+ }
+ }
+ }
+
+ return UnscaleSigma(sigma);
+ }
+
+ /*
+ Number QualityFunctionMuOracle::ScaleSigma(Number sigma) {return log(sigma);}
+ Number QualityFunctionMuOracle::UnscaleSigma(Number scaled_sigma) {return exp(scaled_sigma);}
+ */
+ Number QualityFunctionMuOracle::ScaleSigma(Number sigma)
+ {
+ return sigma;
+ }
+ Number QualityFunctionMuOracle::UnscaleSigma(Number scaled_sigma)
+ {
+ return scaled_sigma;
+ }
+
+ /* AW: Tried search in the log space, but that was even worse than
+ search in unscaled space */
+ /*
+ Number
+ QualityFunctionMuOracle::PerformGoldenSectionLog
+ (Number sigma_up,
+ Number sigma_lo,
+ Number tol,
+ const Vector& step_aff_x_L,
+ const Vector& step_aff_x_U,
+ const Vector& step_aff_s_L,
+ const Vector& step_aff_s_U,
+ const Vector& step_aff_y_c,
+ const Vector& step_aff_y_d,
+ const Vector& step_aff_z_L,
+ const Vector& step_aff_z_U,
+ const Vector& step_aff_v_L,
+ const Vector& step_aff_v_U,
+ const Vector& step_cen_x_L,
+ const Vector& step_cen_x_U,
+ const Vector& step_cen_s_L,
+ const Vector& step_cen_s_U,
+ const Vector& step_cen_y_c,
+ const Vector& step_cen_y_d,
+ const Vector& step_cen_z_L,
+ const Vector& step_cen_z_U,
+ const Vector& step_cen_v_L,
+ const Vector& step_cen_v_U
+ )
+ {
+ Number log_sigma;
+ Number log_sigma_up = log(sigma_up);
+ Number log_sigma_lo = log(sigma_lo);
+
+ Number log_sigma_up_in = log_sigma_up;
+ Number log_sigma_lo_in = log_sigma_lo;
+ Number gfac = (3.-sqrt(5.))/2.;
+ Number log_sigma_mid1 = log_sigma_lo + gfac*(log_sigma_up-log_sigma_lo);
+ Number log_sigma_mid2 = log_sigma_lo + (1.-gfac)*(log_sigma_up-log_sigma_lo);
+
+ Number qmid1 = CalculateQualityFunction(exp(log_sigma_mid1),
+ step_aff_x_L,
+ step_aff_x_U,
+ step_aff_s_L,
+ step_aff_s_U,
+ step_aff_y_c,
+ step_aff_y_d,
+ step_aff_z_L,
+ step_aff_z_U,
+ step_aff_v_L,
+ step_aff_v_U,
+ step_cen_x_L,
+ step_cen_x_U,
+ step_cen_s_L,
+ step_cen_s_U,
+ step_cen_y_c,
+ step_cen_y_d,
+ step_cen_z_L,
+ step_cen_z_U,
+ step_cen_v_L,
+ step_cen_v_U);
+ Number qmid2 = CalculateQualityFunction(exp(log_sigma_mid2),
+ step_aff_x_L,
+ step_aff_x_U,
+ step_aff_s_L,
+ step_aff_s_U,
+ step_aff_y_c,
+ step_aff_y_d,
+ step_aff_z_L,
+ step_aff_z_U,
+ step_aff_v_L,
+ step_aff_v_U,
+ step_cen_x_L,
+ step_cen_x_U,
+ step_cen_s_L,
+ step_cen_s_U,
+ step_cen_y_c,
+ step_cen_y_d,
+ step_cen_z_L,
+ step_cen_z_U,
+ step_cen_v_L,
+ step_cen_v_U);
+
+ Index nsections = 0;
+ while ((log_sigma_up-log_sigma_lo)>=tol*log_sigma_up && nsections<quality_function_max_section_steps_) {
+ nsections++;
+ if (qmid1 > qmid2) {
+ log_sigma_lo = log_sigma_mid1;
+ log_sigma_mid1 = log_sigma_mid2;
+ qmid1 = qmid2;
+ log_sigma_mid2 = log_sigma_lo + (1.-gfac)*(log_sigma_up-log_sigma_lo);
+ qmid2 = CalculateQualityFunction(exp(log_sigma_mid2),
+ step_aff_x_L,
+ step_aff_x_U,
+ step_aff_s_L,
+ step_aff_s_U,
+ step_aff_y_c,
+ step_aff_y_d,
+ step_aff_z_L,
+ step_aff_z_U,
+ step_aff_v_L,
+ step_aff_v_U,
+ step_cen_x_L,
+ step_cen_x_U,
+ step_cen_s_L,
+ step_cen_s_U,
+ step_cen_y_c,
+ step_cen_y_d,
+ step_cen_z_L,
+ step_cen_z_U,
+ step_cen_v_L,
+ step_cen_v_U);
+ }
+ else {
+ log_sigma_up = log_sigma_mid2;
+ log_sigma_mid2 = log_sigma_mid1;
+ qmid2 = qmid1;
+ log_sigma_mid1 = log_sigma_lo + gfac*(log_sigma_up-log_sigma_lo);
+ qmid1 = CalculateQualityFunction(exp(log_sigma_mid1),
+ step_aff_x_L,
+ step_aff_x_U,
+ step_aff_s_L,
+ step_aff_s_U,
+ step_aff_y_c,
+ step_aff_y_d,
+ step_aff_z_L,
+ step_aff_z_U,
+ step_aff_v_L,
+ step_aff_v_U,
+ step_cen_x_L,
+ step_cen_x_U,
+ step_cen_s_L,
+ step_cen_s_U,
+ step_cen_y_c,
+ step_cen_y_d,
+ step_cen_z_L,
+ step_cen_z_U,
+ step_cen_v_L,
+ step_cen_v_U);
+ }
+ }
+
+ Number q;
+ if (qmid1 < qmid2) {
+ log_sigma = log_sigma_mid1;
+ q = qmid1;
+ }
+ else {
+ log_sigma = log_sigma_mid2;
+ q = qmid2;
+ }
+ if (log_sigma_up == log_sigma_up_in) {
+ Number qtmp = CalculateQualityFunction(exp(log_sigma_up),
+ step_aff_x_L,
+ step_aff_x_U,
+ step_aff_s_L,
+ step_aff_s_U,
+ step_aff_y_c,
+ step_aff_y_d,
+ step_aff_z_L,
+ step_aff_z_U,
+ step_aff_v_L,
+ step_aff_v_U,
+ step_cen_x_L,
+ step_cen_x_U,
+ step_cen_s_L,
+ step_cen_s_U,
+ step_cen_y_c,
+ step_cen_y_d,
+ step_cen_z_L,
+ step_cen_z_U,
+ step_cen_v_L,
+ step_cen_v_U);
+ if (qtmp < q) {
+ log_sigma = log_sigma_up;
+ q = qtmp;
+ }
+ }
+ else if (log_sigma_lo == log_sigma_lo_in) {
+ Number qtmp = CalculateQualityFunction(exp(log_sigma_lo),
+ step_aff_x_L,
+ step_aff_x_U,
+ step_aff_s_L,
+ step_aff_s_U,
+ step_aff_y_c,
+ step_aff_y_d,
+ step_aff_z_L,
+ step_aff_z_U,
+ step_aff_v_L,
+ step_aff_v_U,
+ step_cen_x_L,
+ step_cen_x_U,
+ step_cen_s_L,
+ step_cen_s_U,
+ step_cen_y_c,
+ step_cen_y_d,
+ step_cen_z_L,
+ step_cen_z_U,
+ step_cen_v_L,
+ step_cen_v_U);
+ if (qtmp < q) {
+ log_sigma = log_sigma_lo;
+ q = qtmp;
+ }
+ }
+
+ return exp(log_sigma);
+ }
+ */
+
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpQualityFunctionMuOracle.hpp b/SimTKmath/Optimizers/src/IpOpt/IpQualityFunctionMuOracle.hpp
new file mode 100644
index 0000000..2aacc34
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpQualityFunctionMuOracle.hpp
@@ -0,0 +1,277 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpQualityFunctionMuOracle.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2004-11-12
+
+#ifndef __IPQUALITYFUNCTIONMUORACLE_HPP__
+#define __IPQUALITYFUNCTIONMUORACLE_HPP__
+
+#include "IpMuOracle.hpp"
+#include "IpPDSystemSolver.hpp"
+#include "IpIpoptCalculatedQuantities.hpp"
+
+namespace Ipopt
+{
+
+ /** Implementation of the probing strategy for computing the
+ * barrier parameter.
+ */
+ class QualityFunctionMuOracle : public MuOracle
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor */
+ QualityFunctionMuOracle(const SmartPtr<PDSystemSolver>& pd_solver);
+ /** Default destructor */
+ virtual ~QualityFunctionMuOracle();
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Method for computing the value of the barrier parameter that
+ * could be used in the current iteration (using the LOQO formula).
+ */
+ virtual bool CalculateMu(Number mu_min, Number mu_max, Number& new_mu);
+
+ /** Methods for IpoptType */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ /** @name Public enums. Some of those are also used for the
+ * quality function */
+ //@{
+ /** enum for norm type */
+ enum NormEnum
+ {
+ NM_NORM_1=0,
+ NM_NORM_2_SQUARED,
+ NM_NORM_MAX,
+ NM_NORM_2
+ };
+ /** enum for centrality type */
+ enum CentralityEnum
+ {
+ CEN_NONE=0,
+ CEN_LOG,
+ CEN_RECIPROCAL,
+ CEN_CUBED_RECIPROCAL
+ };
+ /** enum for the quality function balancing term type */
+ enum BalancingTermEnum
+ {
+ BT_NONE=0,
+ BT_CUBIC
+ };
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+
+ /** Default Constructor */
+ QualityFunctionMuOracle();
+ /** Copy Constructor */
+ QualityFunctionMuOracle(const QualityFunctionMuOracle&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const QualityFunctionMuOracle&);
+ //@}
+
+ /** Pointer to the object that should be used to solve the
+ * primal-dual system.
+ */
+ SmartPtr<PDSystemSolver> pd_solver_;
+
+ /** Auxilliary function for computing the average complementarity
+ * at a point, given step sizes and step
+ */
+ // ToDo Replace pointers by references
+ Number CalculateQualityFunction(Number sigma,
+ const Vector& step_aff_x_L,
+ const Vector& step_aff_x_U,
+ const Vector& step_aff_s_L,
+ const Vector& step_aff_s_U,
+ const Vector& step_aff_y_c,
+ const Vector& step_aff_y_d,
+ const Vector& step_aff_z_L,
+ const Vector& step_aff_z_U,
+ const Vector& step_aff_v_L,
+ const Vector& step_aff_v_U,
+ const Vector& step_cen_x_L,
+ const Vector& step_cen_x_U,
+ const Vector& step_cen_s_L,
+ const Vector& step_cen_s_U,
+ const Vector& step_cen_y_c,
+ const Vector& step_cen_y_d,
+ const Vector& step_cen_z_L,
+ const Vector& step_cen_z_U,
+ const Vector& step_cen_v_L,
+ const Vector& step_cen_v_U);
+
+ /** Auxilliary function performing the golden section */
+ Number PerformGoldenSection(Number sigma_up,
+ Number q_up,
+ Number sigma_lo,
+ Number q_lo,
+ Number sigma_tol,
+ Number qf_tol,
+ const Vector& step_aff_x_L,
+ const Vector& step_aff_x_U,
+ const Vector& step_aff_s_L,
+ const Vector& step_aff_s_U,
+ const Vector& step_aff_y_c,
+ const Vector& step_aff_y_d,
+ const Vector& step_aff_z_L,
+ const Vector& step_aff_z_U,
+ const Vector& step_aff_v_L,
+ const Vector& step_aff_v_U,
+ const Vector& step_cen_x_L,
+ const Vector& step_cen_x_U,
+ const Vector& step_cen_s_L,
+ const Vector& step_cen_s_U,
+ const Vector& step_cen_y_c,
+ const Vector& step_cen_y_d,
+ const Vector& step_cen_z_L,
+ const Vector& step_cen_z_U,
+ const Vector& step_cen_v_L,
+ const Vector& step_cen_v_U);
+
+ /** Auxilliary functions for scaling the sigma axis in the golden
+ * section procedure */
+ //@{
+ Number ScaleSigma(Number sigma);
+ Number UnscaleSigma(Number scaled_sigma);
+ //@}
+
+ /** Auxilliary function performing the golden section in the
+ * logarithmic scale */
+ /* This doesn't seem to work well, so I took it out for now (AW)
+ Number PerformGoldenSectionLog(Number sigma_up,
+ Number sigma_lo,
+ Number tol,
+ const Vector& step_aff_x_L,
+ const Vector& step_aff_x_U,
+ const Vector& step_aff_s_L,
+ const Vector& step_aff_s_U,
+ const Vector& step_aff_y_c,
+ const Vector& step_aff_y_d,
+ const Vector& step_aff_z_L,
+ const Vector& step_aff_z_U,
+ const Vector& step_aff_v_L,
+ const Vector& step_aff_v_U,
+ const Vector& step_cen_x_L,
+ const Vector& step_cen_x_U,
+ const Vector& step_cen_s_L,
+ const Vector& step_cen_s_U,
+ const Vector& step_cen_y_c,
+ const Vector& step_cen_y_d,
+ const Vector& step_cen_z_L,
+ const Vector& step_cen_z_U,
+ const Vector& step_cen_v_L,
+ const Vector& step_cen_v_U);
+ */
+
+ /** @name Algorithmic parameters */
+ //@{
+ /** Upper bound on centering parameter sigma */
+ Number sigma_max_;
+ /** Lower bound on centering parameter sigma */
+ Number sigma_min_;
+ /** Norm to be used for the quality function. */
+ NormEnum quality_function_norm_;
+ /** Flag indicating how centrality should be involved in the
+ * quality function */
+ CentralityEnum quality_function_centrality_;
+ /** Flag indicating whether we use a balancing term in the quality
+ * function.
+ */
+ BalancingTermEnum quality_function_balancing_term_;
+ /** Relative tolerance for golden bi-section algorithm in sigma
+ * space. */
+ Number quality_function_section_sigma_tol_;
+ /** Relative tolerance for golden bi-section algorithm in function
+ * value space. */
+ Number quality_function_section_qf_tol_;
+ /** Maximal number of bi-section steps in the golden section
+ * search for sigma. */
+ Index quality_function_max_section_steps_;
+ //@}
+
+ /** @name Temporary work space vectors. We use those to avoid
+ * repeated reallocation in CalculateQualityFunction. */
+ //@{
+ SmartPtr<Vector> tmp_step_x_L_;
+ SmartPtr<Vector> tmp_step_x_U_;
+ SmartPtr<Vector> tmp_step_s_L_;
+ SmartPtr<Vector> tmp_step_s_U_;
+ SmartPtr<Vector> tmp_step_z_L_;
+ SmartPtr<Vector> tmp_step_z_U_;
+ SmartPtr<Vector> tmp_step_v_L_;
+ SmartPtr<Vector> tmp_step_v_U_;
+
+ SmartPtr<Vector> tmp_slack_x_L_;
+ SmartPtr<Vector> tmp_slack_x_U_;
+ SmartPtr<Vector> tmp_slack_s_L_;
+ SmartPtr<Vector> tmp_slack_s_U_;
+ SmartPtr<Vector> tmp_z_L_;
+ SmartPtr<Vector> tmp_z_U_;
+ SmartPtr<Vector> tmp_v_L_;
+ SmartPtr<Vector> tmp_v_U_;
+ //@}
+
+ /* Counter for the qualify function evaluations */
+ Index count_qf_evals_;
+
+ /**@name Quantities used many times in CalculateQualityFunction,
+ * which we store here instead of retrieving them from cache every
+ * time. I (AW) don't know if that really makes a difference, but
+ * some of those things showed up in gprof. */
+ //@{
+ bool initialized_;
+ Index n_dual_;
+ Index n_pri_;
+ Index n_comp_;
+
+ SmartPtr<const Vector> curr_slack_x_L_;
+ SmartPtr<const Vector> curr_slack_x_U_;
+ SmartPtr<const Vector> curr_slack_s_L_;
+ SmartPtr<const Vector> curr_slack_s_U_;
+
+ SmartPtr<const Vector> curr_z_L_;
+ SmartPtr<const Vector> curr_z_U_;
+ SmartPtr<const Vector> curr_v_L_;
+ SmartPtr<const Vector> curr_v_U_;
+
+ Number curr_grad_lag_x_asum_;
+ Number curr_grad_lag_s_asum_;
+ Number curr_c_asum_;
+ Number curr_d_minus_s_asum_;
+
+ Number curr_grad_lag_x_nrm2_;
+ Number curr_grad_lag_s_nrm2_;
+ Number curr_c_nrm2_;
+ Number curr_d_minus_s_nrm2_;
+
+ Number curr_grad_lag_x_amax_;
+ Number curr_grad_lag_s_amax_;
+ Number curr_c_amax_;
+ Number curr_d_minus_s_amax_;
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpReferenced.hpp b/SimTKmath/Optimizers/src/IpOpt/IpReferenced.hpp
new file mode 100644
index 0000000..e031e32
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpReferenced.hpp
@@ -0,0 +1,249 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpReferenced.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPREFERENCED_HPP__
+#define __IPREFERENCED_HPP__
+
+#include "IpTypes.hpp"
+#include "IpDebug.hpp"
+
+#include <list>
+
+namespace Ipopt
+{
+
+ /** Psydo-class, from which everything has to inherit that wants to
+ * use be registered as a Referencer for a ReferencedObject.
+ */
+ class Referencer
+ {}
+ ;
+
+ /** ReferencedObject class.
+ * This is part of the implementation of an intrusive smart pointer
+ * design. This class stores the reference count of all the smart
+ * pointers that currently reference it. See the documentation for
+ * the SmartPtr class for more details.
+ *
+ * A SmartPtr behaves much like a raw pointer, but manages the lifetime
+ * of an object, deleting the object automatically. This class implements
+ * a reference-counting, intrusive smart pointer design, where all
+ * objects pointed to must inherit off of ReferencedObject, which
+ * stores the reference count. Although this is intrusive (native types
+ * and externally authored classes require wrappers to be referenced
+ * by smart pointers), it is a safer design. A more detailed discussion of
+ * these issues follows after the usage information.
+ *
+ * Usage Example:
+ * Note: to use the SmartPtr, all objects to which you point MUST
+ * inherit off of ReferencedObject.
+ *
+ * \verbatim
+ *
+ * In MyClass.hpp...
+ *
+ * #include "IpReferenced.hpp"
+
+ * namespace Ipopt {
+ *
+ * class MyClass : public ReferencedObject // must derive from ReferencedObject
+ * {
+ * ...
+ * }
+ * } // namespace Ipopt
+ *
+ *
+ * In my_usage.cpp...
+ *
+ * #include "IpSmartPtr.hpp"
+ * #include "MyClass.hpp"
+ *
+ * void func(AnyObject& obj)
+ * {
+ * SmartPtr<MyClass> ptr_to_myclass = new MyClass(...);
+ * // ptr_to_myclass now points to a new MyClass,
+ * // and the reference count is 1
+ *
+ * ...
+ *
+ * obj.SetMyClass(ptr_to_myclass);
+ * // Here, let's assume that AnyObject uses a
+ * // SmartPtr<MyClass> internally here.
+ * // Now, both ptr_to_myclass and the internal
+ * // SmartPtr in obj point to the same MyClass object
+ * // and its reference count is 2.
+ *
+ * ...
+ *
+ * // No need to delete ptr_to_myclass, this
+ * // will be done automatically when the
+ * // reference count drops to zero.
+ *
+ * }
+ *
+ * \endverbatim
+ *
+ * Other Notes:
+ * The SmartPtr implements both dereference operators -> & *.
+ * The SmartPtr does NOT implement a conversion operator to
+ * the raw pointer. Use the GetRawPtr() method when this
+ * is necessary. Make sure that the raw pointer is NOT
+ * deleted.
+ * The SmartPtr implements the comparison operators == & !=
+ * for a variety of types. Use these instead of
+ * \verbatim
+ * if (GetRawPtr(smrt_ptr) == ptr) // Don't use this
+ * \endverbatim
+ * SmartPtr's, as currently implemented, do NOT handle circular references.
+ * For example: consider a higher level object using SmartPtrs to point to
+ * A and B, but A and B also point to each other (i.e. A has a SmartPtr
+ * to B and B has a SmartPtr to A). In this scenario, when the higher
+ * level object is finished with A and B, their reference counts will
+ * never drop to zero (since they reference each other) and they
+ * will not be deleted. This can be detected by memory leak tools like
+ * valgrind. If the circular reference is necessary, the problem can be
+ * overcome by a number of techniques:
+ *
+ * 1) A and B can have a method that "releases" each other, that is
+ * they set their internal SmartPtrs to NULL.
+ * \verbatim
+ * void AClass::ReleaseCircularReferences()
+ * {
+ * smart_ptr_to_B = NULL;
+ * }
+ * \endverbatim
+ * Then, the higher level class can call these methods before
+ * it is done using A & B.
+ *
+ * 2) Raw pointers can be used in A and B to reference each other.
+ * Here, an implicit assumption is made that the lifetime is
+ * controlled by the higher level object and that A and B will
+ * both exist in a controlled manner. Although this seems
+ * dangerous, in many situations, this type of referencing
+ * is very controlled and this is reasonably safe.
+ *
+ * 3) This SmartPtr class could be redesigned with the Weak/Strong
+ * design concept. Here, the SmartPtr is identified as being
+ * Strong (controls lifetime of the object) or Weak (merely
+ * referencing the object). The Strong SmartPtr increments
+ * (and decrements) the reference count in ReferencedObject
+ * but the Weak SmartPtr does not. In the example above,
+ * the higher level object would have Strong SmartPtrs to
+ * A and B, but A and B would have Weak SmartPtrs to each
+ * other. Then, when the higher level object was done with
+ * A and B, they would be deleted. The Weak SmartPtrs in A
+ * and B would not decrement the reference count and would,
+ * of course, not delete the object. This idea is very similar
+ * to item (2), where it is implied that the sequence of events
+ * is controlled such that A and B will not call anything using
+ * their pointers following the higher level delete (i.e. in
+ * their destructors!). This is somehow safer, however, because
+ * code can be written (however expensive) to perform run-time
+ * detection of this situation. For example, the ReferencedObject
+ * could store pointers to all Weak SmartPtrs that are referencing
+ * it and, in its destructor, tell these pointers that it is
+ * dying. They could then set themselves to NULL, or set an
+ * internal flag to detect usage past this point.
+ *
+ * Comments on Non-Intrusive Design:
+ * In a non-intrusive design, the reference count is stored somewhere other
+ * than the object being referenced. This means, unless the reference
+ * counting pointer is the first referencer, it must get a pointer to the
+ * referenced object from another smart pointer (so it has access to the
+ * reference count location). In this non-intrusive design, if we are
+ * pointing to an object with a smart pointer (or a number of smart
+ * pointers), and we then give another smart pointer the address through
+ * a RAW pointer, we will have two independent, AND INCORRECT, reference
+ * counts. To avoid this pitfall, we use an intrusive reference counting
+ * technique where the reference count is stored in the object being
+ * referenced.
+ */
+ class ReferencedObject
+ {
+ public:
+ ReferencedObject()
+ :
+ reference_count_(0)
+ {}
+
+ virtual ~ReferencedObject()
+ {
+ DBG_ASSERT(reference_count_ == 0);
+ }
+
+ Index ReferenceCount() const;
+
+ void AddRef(const Referencer* referencer) const;
+
+ void ReleaseRef(const Referencer* referencer) const;
+
+ private:
+ mutable Index reference_count_;
+
+# ifdef REF_DEBUG
+
+ mutable std::list<const Referencer*> referencers_;
+# endif
+
+ };
+
+ /* inline methods */
+ inline
+ Index ReferencedObject::ReferenceCount() const
+ {
+ // DBG_START_METH("ReferencedObject::ReferenceCount()", 0);
+ // DBG_PRINT((1,"Returning reference_count_ = %d\n", reference_count_));
+ return reference_count_;
+ }
+
+ inline
+ void ReferencedObject::AddRef(const Referencer* referencer) const
+ {
+ // DBG_START_METH("ReferencedObject::AddRef(const Referencer* referencer)", 0);
+ reference_count_++;
+ // DBG_PRINT((1, "New reference_count_ = %d\n", reference_count_));
+# ifdef REF_DEBUG
+
+ referencers_.push_back(referencer);
+# endif
+
+ }
+
+ inline
+ void ReferencedObject::ReleaseRef(const Referencer* referencer) const
+ {
+ // DBG_START_METH("ReferencedObject::ReleaseRef(const Referencer* referencer)",
+ // 0);
+ reference_count_--;
+ // DBG_PRINT((1, "New reference_count_ = %d\n", reference_count_));
+
+# ifdef REF_DEBUG
+
+ bool found = false;
+ std::list<const Referencer*>::iterator iter;
+ for (iter = referencers_.begin(); iter != referencers_.end(); iter++) {
+ if ((*iter) == referencer) {
+ found = true;
+ break;
+ }
+ }
+
+ // cannot call release on a reference that was never added...
+ DBG_ASSERT(found);
+
+ if (found) {
+ referencers_.erase(iter);
+ }
+# endif
+
+ }
+
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpRegOptions.cpp b/SimTKmath/Optimizers/src/IpOpt/IpRegOptions.cpp
new file mode 100644
index 0000000..8b2fb87
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpRegOptions.cpp
@@ -0,0 +1,881 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpRegOptions.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-06-18
+
+#include "IpoptConfig.h"
+#include "IpRegOptions.hpp"
+
+#include <set>
+
+#ifdef HAVE_CSTDIO
+# include <cstdio>
+#else
+# ifdef HAVE_STDIO_H
+# include <stdio.h>
+# else
+# error "don't have header file for stdio"
+# endif
+#endif
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+
+
+
+#ifdef HAVE_CCTYPE
+# include <cctype>
+#else
+# ifdef HAVE_CTYPE_H
+# include <ctype.h>
+# else
+# error "don't have header file for ctype"
+# endif
+#endif
+
+namespace Ipopt
+{
+
+ Index RegisteredOption::next_counter_ = 1;
+
+ void RegisteredOption::OutputDescription(const Journalist& jnlst) const
+ {
+ std::string type_str = "Unknown";
+ if (type_ ==OT_Number) {
+ type_str = "Real Number";
+ }
+ else if (type_ ==OT_Integer) {
+ type_str = "Integer";
+ }
+ else if (type_ ==OT_String) {
+ type_str = "String";
+ }
+
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION,
+ "\n### %s (%s) ###\nCategory: %s\nDescription: %s\n",
+ name_.c_str(), type_str.c_str(),
+ registering_category_.c_str(), short_description_.c_str());
+
+ if (type_ ==OT_Number) {
+ if (has_lower_) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "%g", lower_);
+ }
+ else {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "-inf");
+ }
+
+ if (lower_strict_) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " < ");
+ }
+ else {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " <= ");
+ }
+
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "(%g)", default_number_);
+
+ if (has_upper_ && upper_strict_) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " < ");
+ }
+ else {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " <= ");
+ }
+
+ if (has_upper_) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "%g\n", upper_);
+ }
+ else {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "+inf\n");
+ }
+ }
+ else if (type_ ==OT_Integer) {
+ if (has_lower_) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "%d", (Index)lower_);
+ }
+ else {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "-inf");
+ }
+
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " <= (%d) <= ", (Index)default_number_);
+
+ if (has_upper_) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "%d\n", (Index)upper_);
+ }
+ else {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "+inf\n");
+ }
+ }
+ else if (type_ ==OT_String) {
+ std::vector<string_entry>::const_iterator i;
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "Valid Settings:\n");
+ for (i = valid_strings_.begin(); i != valid_strings_.end(); i++) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "\t%s (%s)\n",
+ (*i).value_.c_str(), (*i).description_.c_str());
+ }
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "Default: \"%s\"\n",
+ default_string_.c_str());
+ }
+ }
+
+ void RegisteredOption::OutputLatexDescription(const Journalist& jnlst) const
+ {
+ std::string latex_name;
+ MakeValidLatexString(name_, latex_name);
+ std::string latex_desc;
+ MakeValidLatexString(short_description_, latex_desc);
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION,
+ "\\paragraph{%s:} %s $\\;$ \\\\\n",
+ latex_name.c_str(),
+ latex_desc.c_str());
+
+ // Index length = name_.length() + short_description_.length();
+ // DBG_ASSERT(length <= 80);
+ // jnlst.PrintStringOverLines(J_SUMMARY, J_DOCUMENTATION, 0, 50,
+ // latex_desc.c_str());
+
+ if (long_description_ != "") {
+ latex_desc = "";
+ MakeValidLatexString(long_description_, latex_desc);
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " ");
+ jnlst.PrintStringOverLines(J_SUMMARY, J_DOCUMENTATION, 0, 50,
+ latex_desc.c_str());
+ }
+
+ if (type_ == OT_Number) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION,
+ " The valid range for this real option is \n$");
+ std::string buff;
+ if (has_lower_) {
+ buff = MakeValidLatexNumber(lower_);
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "%s", buff.c_str());
+ }
+ else {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "%s", "{\\tt -inf}");
+ }
+
+ if (has_lower_ && !lower_strict_) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " \\le ");
+ }
+ else {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " < ");
+ }
+
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "{\\tt %s }", latex_name.c_str());
+
+ if (has_upper_ && !upper_strict_) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " \\le ");
+ }
+ else {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " < ");
+ }
+
+ if (has_upper_) {
+ buff = MakeValidLatexNumber(upper_);
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "%s", buff.c_str());
+ }
+ else {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "%s", "{\\tt +inf}");
+ }
+
+ buff = MakeValidLatexNumber(default_number_);
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION,
+ "$\nand its default value is $%s$.\n\n", buff.c_str());
+
+ }
+ else if (type_ == OT_Integer) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION,
+ " The valid range for this integer option is\n$");
+ if (has_lower_) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "%d \\le ", (Index)lower_);
+ }
+ else {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "%s < ", "{\\tt -inf}");
+ }
+
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "{\\tt %s }", latex_name.c_str());
+
+ if (has_upper_) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " \\le %d", (Index)upper_);
+ }
+ else {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " < %s", "{\\tt +inf}");
+ }
+
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION,
+ "$\nand its default value is $%d$.\n\n",
+ (Index)default_number_);
+ }
+ else if (type_ == OT_String) {
+ std::string buff;
+ MakeValidLatexString(default_string_, buff);
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION,
+ "\nThe default value for this string option is \"%s\".\n",
+ buff.c_str());
+
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "\\\\ \nPossible values:\n");
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "\\begin{itemize}\n");
+ for (std::vector<string_entry>::const_iterator
+ i = valid_strings_.begin();
+ i != valid_strings_.end(); i++) {
+ std::string latex_value;
+ MakeValidLatexString((*i).value_, latex_value);
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " \\item %s: ",
+ latex_value.c_str());
+
+ std::string latex_desc;
+ MakeValidLatexString((*i).description_, latex_desc);
+ jnlst.PrintStringOverLines(J_SUMMARY, J_DOCUMENTATION, 0, 48,
+ latex_desc.c_str());
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "\n");
+ }
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "\\end{itemize}\n");
+ }
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "\n");
+ }
+
+ void RegisteredOption::MakeValidLatexString(std::string source, std::string& dest) const
+ {
+ std::string::iterator c;
+ for (c=source.begin(); c!=source.end(); c++) {
+ if (*c == '_') {
+ dest.append("\\_");
+ }
+ else if (*c == '^') {
+ dest.append("\\^");
+ }
+ else {
+ dest.push_back(*c);
+ }
+ }
+ }
+
+ std::string RegisteredOption::MakeValidLatexNumber(Number value) const
+ {
+ char buffer[256];
+ sprintf(buffer, "%g", value);
+ std::string source = buffer;
+ std::string dest;
+
+ std::string::iterator c;
+ bool found_e = false;
+ for (c=source.begin(); c!=source.end(); c++) {
+ if (*c == 'e') {
+ found_e = true;
+ dest.append(" \\cdot 10^{");
+ }
+ else {
+ dest.push_back(*c);
+ }
+ }
+ if (found_e) {
+ dest.append("}");
+ }
+
+ return dest;
+ }
+
+ void RegisteredOption::OutputShortDescription(const Journalist& jnlst) const
+ {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "%-30s", name_.c_str());
+
+
+ if (type_ == OT_Number) {
+ if (has_lower_) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "%10g", lower_);
+ }
+ else {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "%10s", "-inf");
+ }
+
+ if (has_lower_ && !lower_strict_) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " <= ");
+ }
+ else {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " < ");
+ }
+
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "(%11g)", default_number_);
+
+ if (has_upper_ && !upper_strict_) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " <= ");
+ }
+ else {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " < ");
+ }
+
+ if (has_upper_) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "%-10g\n", upper_);
+ }
+ else {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "%-10s\n", "+inf");
+ }
+ }
+ else if (type_ == OT_Integer) {
+ if (has_lower_) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "%10d <= ", (Index)lower_);
+ }
+ else {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "%10s < ", "-inf");
+ }
+
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "(%11d)",
+ (Index)default_number_);
+
+ if (has_upper_) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " <= %-10d\n", (Index)upper_);
+ }
+ else {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " < %-10s\n", "+inf");
+ }
+ }
+ else if (type_ == OT_String) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "(\"%s\")\n",
+ default_string_.c_str());
+ }
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " ");
+ jnlst.PrintStringOverLines(J_SUMMARY, J_DOCUMENTATION, 3, 76,
+ short_description_.c_str());
+ if (long_description_ != "") {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "\n ");
+ jnlst.PrintStringOverLines(J_SUMMARY, J_DOCUMENTATION, 5, 74,
+ long_description_.c_str());
+ }
+ if (type_ == OT_String) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "\n Possible values:\n");
+ for (std::vector<string_entry>::const_iterator
+ i = valid_strings_.begin();
+ i != valid_strings_.end(); i++) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, " - %-23s [",
+ (*i).value_.c_str());
+
+ jnlst.PrintStringOverLines(J_SUMMARY, J_DOCUMENTATION, 31, 48,
+ (*i).description_.c_str());
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "]\n");
+ }
+ }
+ else {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "\n");
+ }
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "\n");
+ }
+
+ bool RegisteredOption::IsValidStringSetting(const std::string& value) const
+ {
+ DBG_ASSERT(type_ == OT_String);
+
+ std::vector<string_entry>::const_iterator i;
+ for (i = valid_strings_.begin(); i != valid_strings_.end(); i++) {
+ if (i->value_ == "*" || string_equal_insensitive(i->value_, value)) {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ std::string
+ RegisteredOption::MapStringSetting(const std::string& value) const
+ {
+ DBG_ASSERT(type_ == OT_String);
+
+ std::string matched_setting = "";
+
+ std::vector<string_entry>::const_iterator i;
+ for (i = valid_strings_.begin(); i != valid_strings_.end(); i++) {
+ if (i->value_ == "*") {
+ matched_setting = value;
+ }
+ else if (string_equal_insensitive(i->value_, value)) {
+ matched_setting = i->value_;
+ }
+ }
+ return matched_setting;
+ }
+
+ Index
+ RegisteredOption::MapStringSettingToEnum(const std::string& value) const
+ {
+ DBG_ASSERT(type_ == OT_String);
+
+ Index matched_setting = -1;
+
+ Index cnt = 0;
+ std::vector<string_entry>::const_iterator i;
+ for (i = valid_strings_.begin(); i != valid_strings_.end(); i++) {
+ ASSERT_EXCEPTION(i->value_ != "*", IpoptException,
+ "Cannot map a wildcard setting to an enumeration");
+ if (string_equal_insensitive(i->value_, value)) {
+ matched_setting = cnt;
+ break;
+ }
+ cnt++;
+ }
+
+ ASSERT_EXCEPTION(matched_setting != -1, ERROR_CONVERTING_STRING_TO_ENUM,
+ std::string("Could not find a match for setting ") + value +
+ " in option: " + name_);
+ return matched_setting;
+ }
+
+ bool
+ RegisteredOption::string_equal_insensitive(const std::string& s1,
+ const std::string& s2) const
+ {
+ using namespace std;
+
+ if (s1.size()!=s2.size())
+ return false;
+
+ string::const_iterator i1 = s1.begin();
+ string::const_iterator i2 = s2.begin();
+
+ while(i1!=s1.end()) {
+ if (toupper(*i1)!=toupper(*i2))
+ return false;
+ i1++;
+ i2++;
+ }
+ return true;
+ }
+
+ void
+ RegisteredOptions::AddNumberOption(const std::string& name,
+ const std::string& short_description,
+ Number default_value,
+ const std::string& long_description)
+ {
+ SmartPtr<RegisteredOption> option =
+ new RegisteredOption(name, short_description, long_description,
+ current_registering_category_);
+ option->SetType(OT_Number);
+ option->SetDefaultNumber(default_value);
+ ASSERT_EXCEPTION(registered_options_.find(name) == registered_options_.end(),
+ OPTION_ALREADY_REGISTERED,
+ std::string("The option: ") + option->Name() + " has already been registered by someone else");
+ registered_options_[name] = option;
+ }
+
+ void
+ RegisteredOptions::AddLowerBoundedNumberOption(const std::string& name,
+ const std::string& short_description,
+ Number lower, bool strict,
+ Number default_value,
+ const std::string& long_description)
+ {
+ SmartPtr<RegisteredOption> option =
+ new RegisteredOption(name, short_description, long_description,
+ current_registering_category_);
+ option->SetType(OT_Number);
+ option->SetDefaultNumber(default_value);
+ option->SetLowerNumber(lower, strict);
+ ASSERT_EXCEPTION(registered_options_.find(name) == registered_options_.end(), OPTION_ALREADY_REGISTERED,
+ std::string("The option: ") + option->Name() + " has already been registered by someone else");
+ registered_options_[name] = option;
+ }
+
+ void
+ RegisteredOptions::AddUpperBoundedNumberOption(const std::string& name,
+ const std::string& short_description,
+ Number upper, bool strict,
+ Number default_value,
+ const std::string& long_description)
+ {
+ SmartPtr<RegisteredOption> option =
+ new RegisteredOption(name, short_description, long_description,
+ current_registering_category_);
+ option->SetType(OT_Number);
+ option->SetDefaultNumber(default_value);
+ option->SetUpperNumber(upper, strict);
+ ASSERT_EXCEPTION(registered_options_.find(name) == registered_options_.end(), OPTION_ALREADY_REGISTERED,
+ std::string("The option: ") + option->Name() + " has already been registered by someone else");
+ registered_options_[name] = option;
+ }
+
+ void
+ RegisteredOptions::AddBoundedNumberOption(const std::string& name,
+ const std::string& short_description,
+ Number lower, bool lower_strict,
+ Number upper, bool upper_strict,
+ Number default_value,
+ const std::string& long_description)
+ {
+ SmartPtr<RegisteredOption> option =
+ new RegisteredOption(name, short_description, long_description,
+ current_registering_category_);
+ option->SetType(OT_Number);
+ option->SetDefaultNumber(default_value);
+ option->SetLowerNumber(lower, lower_strict);
+ option->SetUpperNumber(upper, upper_strict);
+ ASSERT_EXCEPTION(registered_options_.find(name) == registered_options_.end(), OPTION_ALREADY_REGISTERED,
+ std::string("The option: ") + option->Name() + " has already been registered by someone else");
+ registered_options_[name] = option;
+ }
+
+ void
+ RegisteredOptions::AddIntegerOption(const std::string& name,
+ const std::string& short_description,
+ Index default_value,
+ const std::string& long_description)
+ {
+ SmartPtr<RegisteredOption> option =
+ new RegisteredOption(name, short_description, long_description,
+ current_registering_category_);
+ option->SetType(OT_Integer);
+ option->SetDefaultInteger(default_value);
+ ASSERT_EXCEPTION(registered_options_.find(name) == registered_options_.end(), OPTION_ALREADY_REGISTERED,
+ std::string("The option: ") + option->Name() + " has already been registered by someone else");
+ registered_options_[name] = option;
+ }
+
+ void
+ RegisteredOptions::AddLowerBoundedIntegerOption(const std::string& name,
+ const std::string& short_description,
+ Index lower, Index default_value,
+ const std::string& long_description)
+ {
+ SmartPtr<RegisteredOption> option =
+ new RegisteredOption(name, short_description, long_description,
+ current_registering_category_);
+ option->SetType(OT_Integer);
+ option->SetDefaultInteger(default_value);
+ option->SetLowerInteger(lower);
+ ASSERT_EXCEPTION(registered_options_.find(name) == registered_options_.end(), OPTION_ALREADY_REGISTERED,
+ std::string("The option: ") + option->Name() + " has already been registered by someone else");
+ registered_options_[name] = option;
+ }
+
+ void
+ RegisteredOptions::AddUpperBoundedIntegerOption(const std::string& name,
+ const std::string& short_description,
+ Index upper, Index default_value,
+ const std::string& long_description)
+ {
+ SmartPtr<RegisteredOption> option =
+ new RegisteredOption(name, short_description, long_description,
+ current_registering_category_);
+ option->SetType(OT_Integer);
+ option->SetDefaultInteger(default_value);
+ option->SetUpperInteger(upper);
+ ASSERT_EXCEPTION(registered_options_.find(name) == registered_options_.end(), OPTION_ALREADY_REGISTERED,
+ std::string("The option: ") + option->Name() + " has already been registered by someone else");
+ registered_options_[name] = option;
+ }
+
+ void
+ RegisteredOptions::AddBoundedIntegerOption(const std::string& name,
+ const std::string& short_description,
+ Index lower, Index upper,
+ Index default_value,
+ const std::string& long_description)
+ {
+ SmartPtr<RegisteredOption> option =
+ new RegisteredOption(name, short_description, long_description,
+ current_registering_category_);
+ option->SetType(OT_Integer);
+ option->SetDefaultInteger(default_value);
+ option->SetLowerInteger(lower);
+ option->SetUpperInteger(upper);
+ ASSERT_EXCEPTION(registered_options_.find(name) == registered_options_.end(), OPTION_ALREADY_REGISTERED,
+ std::string("The option: ") + option->Name() + " has already been registered by someone else");
+ registered_options_[name] = option;
+ }
+
+ void
+ RegisteredOptions::AddStringOption(const std::string& name,
+ const std::string& short_description,
+ const std::string& default_value,
+ const std::vector<std::string>& settings,
+ const std::vector<std::string>& descriptions,
+ const std::string& long_description)
+ {
+ SmartPtr<RegisteredOption> option =
+ new RegisteredOption(name, short_description, long_description,
+ current_registering_category_);
+ option->SetType(OT_String);
+ option->SetDefaultString(default_value);
+ DBG_ASSERT(settings.size() == descriptions.size());
+ for (int i=0; i<(int)settings.size(); i++) {
+ option->AddValidStringSetting(settings[i], descriptions[i]);
+ }
+ ASSERT_EXCEPTION(registered_options_.find(name) == registered_options_.end(), OPTION_ALREADY_REGISTERED,
+ std::string("The option: ") + option->Name() + " has already been registered by someone else");
+ registered_options_[name] = option;
+ }
+
+ void
+ RegisteredOptions::AddStringOption1(const std::string& name,
+ const std::string& short_description,
+ const std::string& default_value,
+ const std::string& setting1,
+ const std::string& description1,
+ const std::string& long_description)
+ {
+ SmartPtr<RegisteredOption> option =
+ new RegisteredOption(name, short_description, long_description,
+ current_registering_category_);
+ option->SetType(OT_String);
+ option->SetDefaultString(default_value);
+ option->AddValidStringSetting(setting1, description1);
+ ASSERT_EXCEPTION(registered_options_.find(name) == registered_options_.end(), OPTION_ALREADY_REGISTERED,
+ std::string("The option: ") + option->Name() + " has already been registered by someone else");
+ registered_options_[name] = option;
+ }
+
+ void
+ RegisteredOptions::AddStringOption2(const std::string& name,
+ const std::string& short_description,
+ const std::string& default_value,
+ const std::string& setting1,
+ const std::string& description1,
+ const std::string& setting2,
+ const std::string& description2,
+ const std::string& long_description)
+ {
+ SmartPtr<RegisteredOption> option =
+ new RegisteredOption(name, short_description, long_description,
+ current_registering_category_);
+ option->SetType(OT_String);
+ option->SetDefaultString(default_value);
+ option->AddValidStringSetting(setting1, description1);
+ option->AddValidStringSetting(setting2, description2);
+ ASSERT_EXCEPTION(registered_options_.find(name) == registered_options_.end(), OPTION_ALREADY_REGISTERED,
+ std::string("The option: ") + option->Name() + " has already been registered by someone else");
+ registered_options_[name] = option;
+ }
+
+ void
+ RegisteredOptions::AddStringOption3(const std::string& name,
+ const std::string& short_description,
+ const std::string& default_value,
+ const std::string& setting1,
+ const std::string& description1,
+ const std::string& setting2,
+ const std::string& description2,
+ const std::string& setting3,
+ const std::string& description3,
+ const std::string& long_description)
+ {
+ SmartPtr<RegisteredOption> option =
+ new RegisteredOption(name, short_description, long_description,
+ current_registering_category_);
+ option->SetType(OT_String);
+ option->SetDefaultString(default_value);
+ option->AddValidStringSetting(setting1, description1);
+ option->AddValidStringSetting(setting2, description2);
+ option->AddValidStringSetting(setting3, description3);
+ ASSERT_EXCEPTION(registered_options_.find(name) == registered_options_.end(), OPTION_ALREADY_REGISTERED,
+ std::string("The option: ") + option->Name() + " has already been registered by someone else");
+ registered_options_[name] = option;
+ }
+
+ void
+ RegisteredOptions::AddStringOption4(const std::string& name,
+ const std::string& short_description,
+ const std::string& default_value,
+ const std::string& setting1,
+ const std::string& description1,
+ const std::string& setting2,
+ const std::string& description2,
+ const std::string& setting3,
+ const std::string& description3,
+ const std::string& setting4,
+ const std::string& description4,
+ const std::string& long_description)
+ {
+ SmartPtr<RegisteredOption> option =
+ new RegisteredOption(name, short_description, long_description,
+ current_registering_category_);
+ option->SetType(OT_String);
+ option->SetDefaultString(default_value);
+ option->AddValidStringSetting(setting1, description1);
+ option->AddValidStringSetting(setting2, description2);
+ option->AddValidStringSetting(setting3, description3);
+ option->AddValidStringSetting(setting4, description4);
+ ASSERT_EXCEPTION(registered_options_.find(name) == registered_options_.end(), OPTION_ALREADY_REGISTERED,
+ std::string("The option: ") + option->Name() + " has already been registered by someone else");
+ registered_options_[name] = option;
+ }
+
+ void
+ RegisteredOptions::AddStringOption5(const std::string& name,
+ const std::string& short_description,
+ const std::string& default_value,
+ const std::string& setting1,
+ const std::string& description1,
+ const std::string& setting2,
+ const std::string& description2,
+ const std::string& setting3,
+ const std::string& description3,
+ const std::string& setting4,
+ const std::string& description4,
+ const std::string& setting5,
+ const std::string& description5,
+ const std::string& long_description)
+ {
+ SmartPtr<RegisteredOption> option =
+ new RegisteredOption(name, short_description, long_description,
+ current_registering_category_);
+ option->SetType(OT_String);
+ option->SetDefaultString(default_value);
+ option->AddValidStringSetting(setting1, description1);
+ option->AddValidStringSetting(setting2, description2);
+ option->AddValidStringSetting(setting3, description3);
+ option->AddValidStringSetting(setting4, description4);
+ option->AddValidStringSetting(setting5, description5);
+ ASSERT_EXCEPTION(registered_options_.find(name) == registered_options_.end(), OPTION_ALREADY_REGISTERED,
+ std::string("The option: ") + option->Name() + " has already been registered by someone else");
+ registered_options_[name] = option;
+ }
+
+ void
+ RegisteredOptions::AddStringOption6(const std::string& name,
+ const std::string& short_description,
+ const std::string& default_value,
+ const std::string& setting1,
+ const std::string& description1,
+ const std::string& setting2,
+ const std::string& description2,
+ const std::string& setting3,
+ const std::string& description3,
+ const std::string& setting4,
+ const std::string& description4,
+ const std::string& setting5,
+ const std::string& description5,
+ const std::string& setting6,
+ const std::string& description6,
+ const std::string& long_description)
+ {
+ SmartPtr<RegisteredOption> option =
+ new RegisteredOption(name, short_description, long_description,
+ current_registering_category_);
+ option->SetType(OT_String);
+ option->SetDefaultString(default_value);
+ option->AddValidStringSetting(setting1, description1);
+ option->AddValidStringSetting(setting2, description2);
+ option->AddValidStringSetting(setting3, description3);
+ option->AddValidStringSetting(setting4, description4);
+ option->AddValidStringSetting(setting5, description5);
+ option->AddValidStringSetting(setting6, description6);
+ ASSERT_EXCEPTION(registered_options_.find(name) == registered_options_.end(), OPTION_ALREADY_REGISTERED,
+ std::string("The option: ") + option->Name() + " has already been registered by someone else");
+ registered_options_[name] = option;
+ }
+
+ void
+ RegisteredOptions::AddStringOption7(const std::string& name,
+ const std::string& short_description,
+ const std::string& default_value,
+ const std::string& setting1,
+ const std::string& description1,
+ const std::string& setting2,
+ const std::string& description2,
+ const std::string& setting3,
+ const std::string& description3,
+ const std::string& setting4,
+ const std::string& description4,
+ const std::string& setting5,
+ const std::string& description5,
+ const std::string& setting6,
+ const std::string& description6,
+ const std::string& setting7,
+ const std::string& description7,
+ const std::string& long_description)
+ {
+ SmartPtr<RegisteredOption> option =
+ new RegisteredOption(name, short_description, long_description,
+ current_registering_category_);
+ option->SetType(OT_String);
+ option->SetDefaultString(default_value);
+ option->AddValidStringSetting(setting1, description1);
+ option->AddValidStringSetting(setting2, description2);
+ option->AddValidStringSetting(setting3, description3);
+ option->AddValidStringSetting(setting4, description4);
+ option->AddValidStringSetting(setting5, description5);
+ option->AddValidStringSetting(setting6, description6);
+ option->AddValidStringSetting(setting7, description7);
+ ASSERT_EXCEPTION(registered_options_.find(name) == registered_options_.end(), OPTION_ALREADY_REGISTERED,
+ std::string("The option: ") + option->Name() + " has already been registered by someone else");
+ registered_options_[name] = option;
+ }
+
+ SmartPtr<const RegisteredOption> RegisteredOptions::GetOption(const std::string& name)
+ {
+ std::string tag_only = name;
+ std::string::size_type pos = name.rfind(".", name.length());
+ if (pos != std::string::npos) {
+ tag_only = name.substr(pos+1, name.length()-pos);
+ }
+ SmartPtr<const RegisteredOption> option;
+ std::map< std::string, SmartPtr<RegisteredOption> >::iterator reg_option = registered_options_.find(tag_only);
+ if (reg_option == registered_options_.end()) {
+ option = NULL;
+ }
+ else {
+ option = ConstPtr(reg_option->second);
+ }
+
+ return option;
+ }
+
+ void RegisteredOptions::OutputOptionDocumentation(const Journalist& jnlst, std::list<std::string>& categories)
+ {
+ // create a set to print sorted output
+ // std::set
+ // <std::string> classes;
+ // std::map <std::string, SmartPtr<RegisteredOption> >::iterator option;
+ // for (option = registered_options_.begin(); option != registered_options_.end(); option++) {
+ // classes.insert(option->second->RegisteringCategory());
+ // }
+
+
+ std::list
+ <std::string>::iterator i;
+ for (i = categories.begin(); i != categories.end(); i++) {
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION,
+ "\n### %s ###\n\n", (*i).c_str());
+ std::map<Index, SmartPtr<RegisteredOption> > class_options;
+ std::map <std::string, SmartPtr<RegisteredOption> >::iterator option;
+ for (option = registered_options_.begin();
+ option != registered_options_.end(); option++) {
+ if (option->second->RegisteringCategory() == (*i)) {
+
+ class_options[option->second->Counter()] = option->second;
+ }
+ }
+ std::map<Index, SmartPtr<RegisteredOption> >::const_iterator co;
+ for (co = class_options.begin(); co != class_options.end(); co++) {
+ co->second->OutputShortDescription(jnlst);
+ }
+ jnlst.Printf(J_SUMMARY, J_DOCUMENTATION, "\n");
+ }
+ }
+
+ void RegisteredOptions::OutputLatexOptionDocumentation(
+ const Journalist& jnlst,
+ std::list<std::string>& options_to_print)
+ {
+
+ if (!options_to_print.empty()) {
+ std::list<std::string>::iterator coption;
+ for (coption = options_to_print.begin();
+ coption != options_to_print.end();
+ coption++) {
+ // std::map <std::string, SmartPtr<RegisteredOption> >::iterator option;
+ SmartPtr<RegisteredOption> option = registered_options_[*coption];
+ DBG_ASSERT(IsValid(option));
+ option->OutputLatexDescription(jnlst);
+ }
+ }
+ else {
+ std::map <std::string, SmartPtr<RegisteredOption> >::iterator option;
+ for (option = registered_options_.begin();
+ option != registered_options_.end();
+ option++) {
+ option->second->OutputLatexDescription(jnlst);
+ }
+ }
+ }
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpRegOptions.hpp b/SimTKmath/Optimizers/src/IpOpt/IpRegOptions.hpp
new file mode 100644
index 0000000..738721e
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpRegOptions.hpp
@@ -0,0 +1,576 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpRegOptions.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-06-18
+
+#ifndef __IPREGOPTIONS_HPP__
+#define __IPREGOPTIONS_HPP__
+
+#include "IpUtils.hpp"
+#include "IpReferenced.hpp"
+#include "IpException.hpp"
+#include "IpSmartPtr.hpp"
+
+#include <map>
+
+namespace Ipopt
+{
+
+ enum RegisteredOptionType
+ {
+ OT_Number,
+ OT_Integer,
+ OT_String,
+ OT_Unknown
+ };
+
+ /** Base class for registered options. The derived types are more
+ * specific to a string option or a Number (real) option, etc.
+ */
+ class RegisteredOption : public ReferencedObject
+ {
+ public:
+ /** Constructors / Destructors */
+ //@{
+ RegisteredOption()
+ :
+ type_(OT_Unknown),
+ has_lower_(false),
+ has_upper_(false),
+ counter_(0)
+ {}
+
+ RegisteredOption(const std::string& name,
+ const std::string& short_description,
+ const std::string& long_description,
+ const std::string& registering_category)
+ :
+ name_(name),
+ short_description_(short_description),
+ long_description_(long_description),
+ registering_category_(registering_category),
+ type_(OT_Unknown),
+ has_lower_(false),
+ has_upper_(false),
+ counter_(next_counter_++)
+ {}
+
+ RegisteredOption(const RegisteredOption& copy)
+ :
+ name_(copy.name_),
+ short_description_(copy.short_description_),
+ long_description_(copy.long_description_),
+ registering_category_(copy.registering_category_),
+ type_(copy.type_),
+ has_lower_(copy.has_lower_),
+ lower_(copy.lower_),
+ has_upper_(copy.has_upper_),
+ upper_(copy.upper_),
+ valid_strings_(copy.valid_strings_),
+ counter_(copy.counter_)
+ {}
+
+ virtual ~RegisteredOption()
+ {}
+ //@}
+
+ DECLARE_STD_EXCEPTION(ERROR_CONVERTING_STRING_TO_ENUM);
+
+ /** Standard Get / Set Methods */
+ //@{
+ /** Get the option's name (tag in the input file) */
+ const std::string& Name() const
+ {
+ return name_;
+ }
+ /** Set the option's name (tag in the input file) */
+ void SetName(const std::string& name)
+ {
+ name_ = name;
+ }
+ /** Get the short description */
+ const std::string& ShortDescription() const
+ {
+ return short_description_;
+ }
+ /** Get the long description */
+ const std::string& LongDescription() const
+ {
+ return long_description_;
+ }
+ /** Set the short description */
+ void SetShortDescription(const std::string& short_description)
+ {
+ short_description_ = short_description;
+ }
+ /** Set the long description */
+ void SetLongDescription(const std::string& long_description)
+ {
+ long_description_ = long_description;
+ }
+ /** Get the registering class */
+ const std::string& RegisteringCategory() const
+ {
+ return registering_category_;
+ }
+ /** Set the registering class */
+ void SetRegisteringCategory(const std::string& registering_category)
+ {
+ registering_category_ = registering_category;
+ }
+ /** Get the Option's type */
+ const RegisteredOptionType& Type() const
+ {
+ return type_;
+ }
+ /** Get the Option's type */
+ void SetType(const RegisteredOptionType& type)
+ {
+ type_ = type;
+ }
+ /** Counter */
+ Index Counter() const
+ {
+ return counter_;
+ }
+ //@}
+
+ /** @name Get / Set methods valid for specific types - NOTE: the Type
+ * must be set before calling these methods.
+ */
+ //@{
+ /** check if the option has a lower bound - can be called for
+ * OT_Number & OT_Integer*/
+ const bool& HasLower() const
+ {
+ DBG_ASSERT(type_ == OT_Number || type_ == OT_Integer);
+ return has_lower_;
+ }
+ /** check if the lower bound is strict - can be called for
+ OT_Number */
+ const bool& LowerStrict() const
+ {
+ DBG_ASSERT(type_ == OT_Number && has_lower_ == true);
+ return lower_strict_;
+ }
+ /** get the Number version of the lower bound - can be called for
+ * OT_Number */
+ Number LowerNumber() const
+ {
+ DBG_ASSERT(has_lower_ == true && type_ == OT_Number);
+ return lower_;
+ }
+ /** set the Number version of the lower bound - can be called for
+ * OT_Number */
+ void SetLowerNumber(const Number& lower, const bool& strict)
+ {
+ DBG_ASSERT(type_ == OT_Number);
+ lower_ = lower;
+ lower_strict_ = strict, has_lower_ = true;
+ }
+ /** get the Integer version of the lower bound can be called for
+ * OT_Integer*/
+ Index LowerInteger() const
+ {
+ DBG_ASSERT(has_lower_ == true && type_ == OT_Integer);
+ return (Index)lower_;
+ }
+ /** set the Integer version of the lower bound - can be called for
+ * OT_Integer */
+ void SetLowerInteger(const Index& lower)
+ {
+ DBG_ASSERT(type_ == OT_Integer);
+ lower_ = (Number)lower;
+ has_lower_ = true;
+ }
+ /** check if the option has an upper bound - can be called for
+ * OT_Number & OT_Integer*/
+ const bool& HasUpper() const
+ {
+ DBG_ASSERT(type_ == OT_Number || type_ == OT_Integer);
+ return has_upper_;
+ }
+ /** check if the upper bound is strict - can be called for
+ * OT_Number */
+ const bool& UpperStrict() const
+ {
+ DBG_ASSERT(type_ == OT_Number && has_upper_ == true);
+ return upper_strict_;
+ }
+ /** get the Number version of the upper bound - can be called for
+ * OT_Number */
+ Number UpperNumber()
+ {
+ DBG_ASSERT(has_upper_ == true && type_ == OT_Number);
+ return upper_;
+ }
+ /** set the Number version of the upper bound - can be called for
+ * OT_Number */
+ void SetUpperNumber(const Number& upper, const bool& strict)
+ {
+ DBG_ASSERT(type_ == OT_Number);
+ upper_ = upper;
+ upper_strict_ = strict;
+ has_upper_ = true;
+ }
+ /** get the Integer version of the upper bound - can be called for
+ * OT_Integer*/
+ Index UpperInteger() const
+ {
+ DBG_ASSERT(has_upper_ == true && type_ == OT_Integer);
+ return (Index)upper_;
+ }
+ /** set the Integer version of the upper bound - can be called for
+ * OT_Integer */
+ void SetUpperInteger(const Index& upper)
+ {
+ DBG_ASSERT(type_ == OT_Integer);
+ upper_ = (Number)upper;
+ has_upper_ = true;
+ }
+ /** method to add valid string entries - can be called for
+ * OT_String */
+ void AddValidStringSetting(const std::string value,
+ const std::string description)
+ {
+ DBG_ASSERT(type_ == OT_String);
+ valid_strings_.push_back(string_entry(value, description));
+ }
+ /** get the default as a Number - can be called for OT_Number */
+ Number DefaultNumber() const
+ {
+ DBG_ASSERT(type_ == OT_Number);
+ return default_number_;
+ }
+ /** Set the default as a Number - can be called for OT_Number */
+ void SetDefaultNumber(const Number& default_value)
+ {
+ DBG_ASSERT(type_ == OT_Number);
+ default_number_ = default_value;
+ }
+ /** get the default as an Integer - can be called for OT_Integer*/
+ Index DefaultInteger() const
+ {
+ DBG_ASSERT(type_ == OT_Integer);
+ return (Index)default_number_;
+ }
+ /** Set the default as an Integer - can be called for
+ OT_Integer */
+ void SetDefaultInteger(const Index& default_value)
+ {
+ DBG_ASSERT(type_ == OT_Integer);
+ default_number_ = (Number)default_value;
+ }
+ /** get the default as a string - can be called for OT_String */
+ std::string DefaultString() const
+ {
+ DBG_ASSERT(type_ == OT_String);
+ return default_string_;
+ }
+ /** get the default as a string, but as the index of the string in
+ * the list - helps map from a string to an enum- can be called
+ * for OT_String */
+ Index DefaultStringAsEnum() const
+ {
+ DBG_ASSERT(type_ == OT_String);
+ return MapStringSettingToEnum(default_string_);
+ }
+ /** Set the default as a string - can be called for OT_String */
+ void SetDefaultString(const std::string& default_value)
+ {
+ DBG_ASSERT(type_ == OT_String);
+ default_string_ = default_value;
+ }
+ /** Check if the Number value is a valid setting - can be called
+ * for OT_Number */
+ bool IsValidNumberSetting(const Number& value) const
+ {
+ DBG_ASSERT(type_ == OT_Number);
+ if (has_lower_ && ((lower_strict_ == true && value <= lower_) ||
+ (lower_strict_ == false && value < lower_))) {
+ return false;
+ }
+ if (has_upper_ && ((upper_strict_ == true && value >= upper_) ||
+ (upper_strict_ == false && value > upper_))) {
+ return false;
+ }
+ return true;
+ }
+ /** Check if the Integer value is a valid setting - can be called
+ * for OT_Integer */
+ bool IsValidIntegerSetting(const Index& value) const
+ {
+ DBG_ASSERT(type_ == OT_Integer);
+ if (has_lower_ && value < lower_) {
+ return false;
+ }
+ if (has_upper_ && value > upper_) {
+ return false;
+ }
+ return true;
+ }
+ /** Check if the String value is a valid setting - can be called
+ * for OT_String */
+ bool IsValidStringSetting(const std::string& value) const;
+
+ /** Map a user setting (allowing any case) to the case used when
+ * the setting was registered.
+ */
+ std::string MapStringSetting(const std::string& value) const;
+
+ /** Map a user setting (allowing any case) to the index of the
+ * matched setting in the list of string settings. Helps map a
+ * string setting to an enumeration.
+ */
+ Index MapStringSettingToEnum(const std::string& value) const;
+ //@}
+
+ /** output a description of the option */
+ void OutputDescription(const Journalist& jnlst) const;
+ /** output a more concise version */
+ void OutputShortDescription(const Journalist& jnlst) const;
+ /** output a latex version */
+ void OutputLatexDescription(const Journalist& jnlst) const;
+
+ private:
+ std::string name_;
+ std::string short_description_;
+ std::string long_description_;
+ std::string registering_category_;
+ RegisteredOptionType type_;
+
+ bool has_lower_;
+ bool lower_strict_;
+ Number lower_;
+ bool has_upper_;
+ bool upper_strict_;
+ Number upper_;
+ Number default_number_;
+
+ void MakeValidLatexString(std::string source, std::string& dest) const;
+ std::string MakeValidLatexNumber(Number value) const;
+
+ /** Compare two strings and return true if they are equal (case
+ insensitive comparison) */
+ bool string_equal_insensitive(const std::string& s1,
+ const std::string& s2) const;
+
+ /** class to hold the valid string settings for a string option */
+ class string_entry
+ {
+ public:
+ string_entry(const std::string& value, const std::string& description)
+ : value_(value), description_(description)
+ {}
+ std::string value_;
+ std::string description_;
+ };
+
+ std::vector<string_entry> valid_strings_;
+ std::string default_string_;
+
+ /** Has the information as how many-th option this one was
+ * registered. */
+ const Index counter_;
+
+ static Index next_counter_;
+ };
+
+ /** Class for storing registered options. Used for validation and
+ * documentation.
+ */
+ class RegisteredOptions : public ReferencedObject
+ {
+ public:
+ /** Constructors / Destructors */
+ //@{
+ /** Standard Constructor */
+ RegisteredOptions()
+ :
+ current_registering_category_("Uncategorized")
+ {}
+
+ /** Standard Destructor */
+ ~RegisteredOptions()
+ {}
+ //@}
+
+ DECLARE_STD_EXCEPTION(OPTION_ALREADY_REGISTERED);
+
+ /** Methods to interact with registered options */
+ //@{
+ /** set the registering class. All subsequent options will be
+ * added with the registered class */
+ void SetRegisteringCategory(const std::string& registering_category)
+ {
+ current_registering_category_ = registering_category;
+ }
+
+ /** retrieve the value of the current registering category */
+ std::string RegisteringCategory()
+ {
+ return current_registering_category_;
+ }
+
+ /** Add a Number option (with no restrictions) */
+ void AddNumberOption(const std::string& name,
+ const std::string& short_description,
+ Number default_value,
+ const std::string& long_description="");
+ /** Add a Number option (with a lower bound) */
+ void AddLowerBoundedNumberOption(const std::string& name,
+ const std::string& short_description,
+ Number lower, bool strict,
+ Number default_value,
+ const std::string& long_description="");
+ /** Add a Number option (with a upper bound) */
+ void AddUpperBoundedNumberOption(const std::string& name,
+ const std::string& short_description,
+ Number upper, bool strict,
+ Number default_value,
+ const std::string& long_description="");
+ /** Add a Number option (with a both bounds) */
+ void AddBoundedNumberOption(const std::string& name,
+ const std::string& short_description,
+ Number lower, bool lower_strict,
+ Number upper, bool upper_strict,
+ Number default_value,
+ const std::string& long_description="");
+ /** Add a Integer option (with no restrictions) */
+ void AddIntegerOption(const std::string& name,
+ const std::string& short_description,
+ Index default_value,
+ const std::string& long_description="");
+ /** Add a Integer option (with a lower bound) */
+ void AddLowerBoundedIntegerOption(const std::string& name,
+ const std::string& short_description,
+ Index lower, Index default_value,
+ const std::string& long_description="");
+ /** Add a Integer option (with a upper bound) */
+ void AddUpperBoundedIntegerOption(const std::string& name,
+ const std::string& short_description,
+ Index upper, Index default_value,
+ const std::string& long_description="");
+ /** Add a Integer option (with a both bounds) */
+ void AddBoundedIntegerOption(const std::string& name,
+ const std::string& short_description,
+ Index lower, Index upper,
+ Index default_value,
+ const std::string& long_description="");
+
+ /** Add a String option (with no restrictions) */
+ void AddStringOption(const std::string& name,
+ const std::string& short_description,
+ const std::string& default_value,
+ const std::vector<std::string>& settings,
+ const std::vector<std::string>& descriptions,
+ const std::string& long_description="");
+ /** Methods that make adding string options with only a few
+ * entries easier */
+ void AddStringOption1(const std::string& name,
+ const std::string& short_description,
+ const std::string& default_value,
+ const std::string& setting1,
+ const std::string& description1,
+ const std::string& long_description="");
+ void AddStringOption2(const std::string& name,
+ const std::string& short_description,
+ const std::string& default_value,
+ const std::string& setting1,
+ const std::string& description1,
+ const std::string& setting2,
+ const std::string& description2,
+ const std::string& long_description="");
+ void AddStringOption3(const std::string& name,
+ const std::string& short_description,
+ const std::string& default_value,
+ const std::string& setting1,
+ const std::string& description1,
+ const std::string& setting2,
+ const std::string& description2,
+ const std::string& setting3,
+ const std::string& description3,
+ const std::string& long_description="");
+ void AddStringOption4(const std::string& name,
+ const std::string& short_description,
+ const std::string& default_value,
+ const std::string& setting1,
+ const std::string& description1,
+ const std::string& setting2,
+ const std::string& description2,
+ const std::string& setting3,
+ const std::string& description3,
+ const std::string& setting4,
+ const std::string& description4,
+ const std::string& long_description="");
+ void AddStringOption5(const std::string& name,
+ const std::string& short_description,
+ const std::string& default_value,
+ const std::string& setting1,
+ const std::string& description1,
+ const std::string& setting2,
+ const std::string& description2,
+ const std::string& setting3,
+ const std::string& description3,
+ const std::string& setting4,
+ const std::string& description4,
+ const std::string& setting5,
+ const std::string& description5,
+ const std::string& long_description="");
+ void AddStringOption6(const std::string& name,
+ const std::string& short_description,
+ const std::string& default_value,
+ const std::string& setting1,
+ const std::string& description1,
+ const std::string& setting2,
+ const std::string& description2,
+ const std::string& setting3,
+ const std::string& description3,
+ const std::string& setting4,
+ const std::string& description4,
+ const std::string& setting5,
+ const std::string& description5,
+ const std::string& setting6,
+ const std::string& description6,
+ const std::string& long_description="");
+ void AddStringOption7(const std::string& name,
+ const std::string& short_description,
+ const std::string& default_value,
+ const std::string& setting1,
+ const std::string& description1,
+ const std::string& setting2,
+ const std::string& description2,
+ const std::string& setting3,
+ const std::string& description3,
+ const std::string& setting4,
+ const std::string& description4,
+ const std::string& setting5,
+ const std::string& description5,
+ const std::string& setting6,
+ const std::string& description6,
+ const std::string& setting7,
+ const std::string& description7,
+ const std::string& long_description="");
+
+ /** Get a registered option - this will return NULL if the option
+ * does not exist */
+ SmartPtr<const RegisteredOption> GetOption(const std::string& name);
+
+ /** Output documentation for the options - gives a description,
+ * etc. */
+ void OutputOptionDocumentation(const Journalist& jnlst, std::list<std::string>& categories);
+
+ /** Output documentation in Latex format to include in a latex file */
+ void OutputLatexOptionDocumentation(const Journalist& jnlst, std::list<std::string>& categories);
+ //@}
+
+ private:
+ std::string current_registering_category_;
+ std::map<std::string, SmartPtr<RegisteredOption> > registered_options_;
+ };
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpRestoFilterConvCheck.cpp b/SimTKmath/Optimizers/src/IpOpt/IpRestoFilterConvCheck.cpp
new file mode 100644
index 0000000..8323ef3
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpRestoFilterConvCheck.cpp
@@ -0,0 +1,228 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpRestoFilterConvCheck.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpRestoFilterConvCheck.hpp"
+#include "IpCompoundVector.hpp"
+#include "IpRestoIpoptNLP.hpp"
+#include "IpRestoPhase.hpp"
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ RestoFilterConvergenceCheck::RestoFilterConvergenceCheck()
+ :
+ orig_filter_ls_acceptor_(NULL)
+ {
+ DBG_START_FUN("RestoFilterConvergenceCheck::RestoFilterConvergenceCheck()",
+ dbg_verbosity);
+
+ }
+
+ RestoFilterConvergenceCheck::~RestoFilterConvergenceCheck()
+ {
+ DBG_START_FUN("RestoFilterConvergenceCheck::RestoFilterConvergenceCheck()",
+ dbg_verbosity);
+ }
+
+ void
+ RestoFilterConvergenceCheck::SetOrigFilterLSAcceptor
+ (const FilterLSAcceptor& orig_filter_ls_acceptor)
+ {
+ orig_filter_ls_acceptor_ = &orig_filter_ls_acceptor;
+ }
+
+ void RestoFilterConvergenceCheck::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->AddBoundedNumberOption(
+ "required_infeasibility_reduction",
+ "Required reduction of infeasibility before leaving restoration phase.",
+ 0.0, false, 1.0, true,
+ 0.9,
+ "The restoration phase algorithm is performed, until a point is found "
+ "that is acceptable to the filter and the infeasibility has been "
+ "reduced by at least the fraction given by this option.");
+ }
+
+ bool RestoFilterConvergenceCheck::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ DBG_ASSERT(orig_filter_ls_acceptor_ && "Need to call RestoFilterConvergenceCheck::SetOrigFilterLineSearch before Initialize");
+ options.GetNumericValue("required_infeasibility_reduction", kappa_resto_, prefix);
+ options.GetIntegerValue("max_iter", maximum_iters_, prefix);
+
+ first_resto_iter_ = true;
+
+ return OptimalityErrorConvergenceCheck::InitializeImpl(options, prefix);
+ }
+
+ ConvergenceCheck::ConvergenceStatus
+ RestoFilterConvergenceCheck::CheckConvergence(bool call_intermediate_callback /*= true*/)
+ {
+ // Get pointers to the Original NLP objects
+ const RestoIpoptNLP* resto_ipopt_nlp =
+ dynamic_cast<const RestoIpoptNLP*>(&IpNLP());
+ DBG_ASSERT(resto_ipopt_nlp);
+
+ SmartPtr<IpoptData> orig_ip_data = &resto_ipopt_nlp->OrigIpData();
+ SmartPtr<IpoptCalculatedQuantities> orig_ip_cq =
+ &resto_ipopt_nlp->OrigIpCq();
+
+ // set the trial point for the original problem
+ SmartPtr<const Vector> x = IpData().curr()->x();
+ const CompoundVector* cx =
+ dynamic_cast<const CompoundVector*>(GetRawPtr(x));
+ DBG_ASSERT(cx);
+
+ SmartPtr<IteratesVector> trial = orig_ip_data->curr()->MakeNewContainer();
+ trial->Set_x(*cx->GetComp(0));
+ trial->Set_s(*IpData().curr()->s());
+ orig_ip_data->set_trial(trial);
+
+ if (call_intermediate_callback) {
+ // Check if user requested termination by calling the intermediate
+ // user callback function
+ AlgorithmMode mode = RestorationPhaseMode;
+ // Gather the information also used in the iteration output
+ Index iter = IpData().iter_count();
+ Number inf_pr = orig_ip_cq->trial_primal_infeasibility(NORM_MAX);
+ Number inf_du = IpCq().curr_dual_infeasibility(NORM_MAX);
+ Number mu = IpData().curr_mu();
+ Number dnrm;
+ if (IsValid(IpData().delta()) && IsValid(IpData().delta()->x()) && IsValid(IpData().delta()->s())) {
+ dnrm = Max(IpData().delta()->x()->Amax(), IpData().delta()->s()->Amax());
+ }
+ else {
+ // This is the first iteration - no search direction has been
+ // computed yet.
+ dnrm = 0.;
+ }
+ Number alpha_primal = IpData().info_alpha_primal();
+ Number alpha_dual = IpData().info_alpha_dual();
+ Number regu_x = IpData().info_regu_x();
+ Number unscaled_f = orig_ip_cq->unscaled_trial_f();
+ Index ls_count = IpData().info_ls_count();
+ bool request_stop =
+ !IpNLP().IntermediateCallBack(mode, iter, unscaled_f, inf_pr, inf_du,
+ mu, dnrm, regu_x, alpha_dual,
+ alpha_primal, ls_count,
+ &IpData(), &IpCq());
+
+ if (request_stop) {
+ return ConvergenceCheck::USER_STOP;
+ }
+ }
+
+ if (IpData().iter_count() >= maximum_iters_) {
+ return ConvergenceCheck::MAXITER_EXCEEDED;
+ }
+
+ // First check if the point is now acceptable for the outer filter
+ ConvergenceStatus status;
+
+ // Calculate the f and theta for the original problem
+ Number orig_trial_theta = orig_ip_cq->trial_constraint_violation();
+ Number orig_curr_theta = orig_ip_cq->curr_constraint_violation();
+
+ // check acceptability to the filter
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "orig_curr_theta = %8.2e, orig_trial_theta = %8.2e\n",
+ orig_curr_theta, orig_trial_theta);
+
+ // ToDo: In the following we might want to be more careful with the lower bound
+ Number orig_theta_max = Max(kappa_resto_*orig_curr_theta,
+ 1.e2*Min(orig_ip_data->tol(),
+ constr_viol_tol_));
+
+ if (first_resto_iter_) {
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "This is the first iteration - continue to take at least one step.\n");
+ status = CONTINUE;
+ }
+ else if (orig_trial_theta > orig_theta_max) {
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "Point does not provide sufficient reduction w.r.t the original theta (orig_theta_max=%e).\n", orig_theta_max);
+ status = CONTINUE;
+ }
+ else if (orig_ip_cq->IsSquareProblem() &&
+ orig_trial_theta <= orig_ip_data->tol()) {
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "Restoration phase found points satisfying feasibility tolerance in square problem.\n");
+ status = CONVERGED;
+ }
+ else {
+ Number orig_trial_barr = orig_ip_cq->trial_barrier_obj();
+
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "orig_trial_barr = %8.2e\n", orig_trial_barr);
+
+ if (!orig_filter_ls_acceptor_->IsAcceptableToCurrentFilter(orig_trial_barr, orig_trial_theta)) {
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "Point is not acceptable to the original filter.\n");
+ status = CONTINUE;
+ }
+ else if (!orig_filter_ls_acceptor_->IsAcceptableToCurrentIterate(orig_trial_barr, orig_trial_theta, true) ) {
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "Point is not acceptable to the original current point.\n");
+ status = CONTINUE;
+ }
+ else {
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "Restoration found a point that provides sufficient reduction in"
+ " theta and is acceptable to the current filter.\n");
+ status = CONVERGED;
+ }
+ }
+
+ // If the point is not yet acceptable to the filter, check if the problem
+ // is maybe locally infeasible
+
+ if (status==CONTINUE) {
+
+ status = OptimalityErrorConvergenceCheck::CheckConvergence(false);
+ if (status == CONVERGED || status == CONVERGED_TO_ACCEPTABLE_POINT) {
+ Number orig_trial_primal_inf =
+ orig_ip_cq->trial_primal_infeasibility(NORM_MAX);
+ // ToDo make the factor in following line an option
+ if (orig_trial_primal_inf <= 1e2*IpData().tol()) {
+ // if (orig_trial_primal_inf <= 1e2*orig_ip_data->tol()) {
+ if (IpData().tol() > 1e-1*orig_ip_data->tol()) {
+ // For once, we tighten the convergence tolerance for the
+ // restoration phase problem in case the problem is only
+ // very slightly infeasible.
+ IpData().Set_tol(1e-2*IpData().tol());
+ status = CONTINUE;
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Tightening restoration phase tolerance to %e.\n",
+ IpData().tol());
+ IpData().Append_info_string("!");
+ }
+ else {
+ Jnlst().Printf(J_WARNING, J_LINE_SEARCH,
+ "Restoration phase converged to a feasible point that is\n"
+ "unacceptable to the filter for the original problem.\n");
+ THROW_EXCEPTION(RESTORATION_CONVERGED_TO_FEASIBLE_POINT,
+ "Restoration phase converged to a feasible point that is "
+ "unacceptable to the filter for the original problem.");
+ }
+ }
+ else {
+ THROW_EXCEPTION(LOCALLY_INFEASIBLE,
+ "Restoration phase converged to a point of local infeasibility");
+ }
+ }
+ }
+
+ first_resto_iter_ = false;
+
+ return status;
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpRestoFilterConvCheck.hpp b/SimTKmath/Optimizers/src/IpOpt/IpRestoFilterConvCheck.hpp
new file mode 100644
index 0000000..82c1f97
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpRestoFilterConvCheck.hpp
@@ -0,0 +1,100 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpRestoFilterConvCheck.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPRESTOFILTERCONVCHECK_HPP__
+#define __IPRESTOFILTERCONVCHECK_HPP__
+
+#include "IpOptErrorConvCheck.hpp"
+#include "IpFilterLSAcceptor.hpp"
+
+namespace Ipopt
+{
+
+ /** Convergence check for the restoration phase as called by the
+ * filter. This inherits from the OptimalityErrorConvergenceCheck
+ * so that the method for the regular optimality error convergence
+ * criterion can be checked as well. In addition, this convergence
+ * check returns the CONVERGED message, if the current iteration is
+ * acceptable to the original filter.
+ *
+ * Since this object needs to know about the original NLP, it also
+ * inherits from RestoProblemCoupler, so that the restoration phase
+ * object can call the SetObjs method to set the corresponding
+ * pointers before the Initilize for the restoration phase
+ * algorithm is called.
+ */
+ class RestoFilterConvergenceCheck :
+ public OptimalityErrorConvergenceCheck
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor */
+ RestoFilterConvergenceCheck();
+
+ /** Default destructor */
+ virtual ~RestoFilterConvergenceCheck();
+ //@}
+
+ /** Set the object for the original filter line search. Here,
+ * filter_line_search must be the same strategy object to which
+ * the restoration phase object with this object is given. This
+ * method must be called to finish the definition of the
+ * algorithm, before Initialize is called. */
+ void SetOrigFilterLSAcceptor(const FilterLSAcceptor& orig_filter_ls_acceptor);
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** overloaded from ConvergenceCheck */
+ virtual ConvergenceStatus CheckConvergence(bool call_intermediate_callback = true);
+
+ /** Methods used by IpoptType */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+ private:
+ /**@name Default Compiler Generated Methods (Hidden to avoid
+ * implicit creation/calling). These methods are not implemented
+ * and we do not want the compiler to implement them for us, so we
+ * declare them private and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ RestoFilterConvergenceCheck(const RestoFilterConvergenceCheck&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const RestoFilterConvergenceCheck&);
+ //@}
+
+ /** @name Algorithmic parameters */
+ //@{
+ /** Fraction of required reduction in infeasibility before problem
+ * is considered to be solved. */
+ Number kappa_resto_;
+ /** Maximum number of iterations in restoration phase */
+ Index maximum_iters_;
+ //@}
+
+ /** Flag indicating that this is the first call. We don't want to
+ * leave the restoration phase without taking at least one step,
+ * so this flag is used to ensure this. */
+ bool first_resto_iter_;
+
+ /** Strategy object for the filter line search method for the
+ * original NLP. CAREFUL: We must not hold on to this object
+ * with a SmartPtr, because have otherwise circular references
+ * that prevent the destructor of the line search object to be
+ * called! */
+ const FilterLSAcceptor* orig_filter_ls_acceptor_;
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpRestoIpoptNLP.cpp b/SimTKmath/Optimizers/src/IpOpt/IpRestoIpoptNLP.cpp
new file mode 100644
index 0000000..86124a6
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpRestoIpoptNLP.cpp
@@ -0,0 +1,734 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpRestoIpoptNLP.cpp 765 2006-07-14 18:03:23Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpRestoIpoptNLP.hpp"
+#include "IpIdentityMatrix.hpp"
+#include "IpSumSymMatrix.hpp"
+#include "IpSumMatrix.hpp"
+#include "IpNLPScaling.hpp"
+#include "IpLowRankUpdateSymMatrix.hpp"
+#include "IpIpoptData.hpp"
+#include "IpIpoptCalculatedQuantities.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ RestoIpoptNLP::RestoIpoptNLP(IpoptNLP& orig_ip_nlp,
+ IpoptData& orig_ip_data,
+ IpoptCalculatedQuantities& orig_ip_cq)
+ :
+ IpoptNLP(new NoNLPScalingObject()),
+ orig_ip_nlp_(&orig_ip_nlp),
+ orig_ip_data_(&orig_ip_data),
+ orig_ip_cq_(&orig_ip_cq),
+ rho_(1000.),
+ eta_factor_(1.0),
+ eta_mu_exponent_(0.5)
+ {}
+
+ RestoIpoptNLP::~RestoIpoptNLP()
+ {}
+
+ void RestoIpoptNLP::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->AddStringOption2(
+ "evaluate_orig_obj_at_resto_trial",
+ "Determines if the original objective function should be evaluated at restoration phase trial points.",
+ "yes",
+ "no", "skip evaluation",
+ "yes", "evaluate at every trial point",
+ "Setting this option to \"yes\" makes the restoration phase algorithm "
+ "evaluate the objective function of the original problem at every trial "
+ "point encountered during the restoration phase, even if this value is "
+ "not required. In this way, it is guaranteed that the original "
+ "objective function can be evaluated without error at all accepted "
+ "iterates; otherwise the algorithm might fail at a point where the "
+ "restoration phase accepts an iterate that is good for the restoration "
+ "phase problem, but not the original problem. On the other hand, if "
+ "the evaluation of the original objective is expensive, this might be "
+ "costly.");
+ }
+
+ bool RestoIpoptNLP::Initialize(const Journalist& jnlst,
+ const OptionsList& options,
+ const std::string& prefix)
+ {
+ options.GetBoolValue("evaluate_orig_obj_at_resto_trial",
+ evaluate_orig_obj_at_resto_trial_, prefix);
+ Index enum_int;
+ options.GetEnumValue("hessian_approximation", enum_int, prefix);
+ hessian_approximation_ = HessianApproximationType(enum_int);
+
+ initialized_ = true;
+ return IpoptNLP::Initialize(jnlst, options, prefix);
+ }
+
+ bool RestoIpoptNLP::InitializeStructures(SmartPtr<Vector>& x,
+ bool init_x,
+ SmartPtr<Vector>& y_c,
+ bool init_y_c,
+ SmartPtr<Vector>& y_d,
+ bool init_y_d,
+ SmartPtr<Vector>& z_L,
+ bool init_z_L,
+ SmartPtr<Vector>& z_U,
+ bool init_z_U,
+ SmartPtr<Vector>& v_L,
+ SmartPtr<Vector>& v_U
+ )
+ {
+ DBG_START_METH("RestoIpoptNLP::InitializeStructures", 0);
+ DBG_ASSERT(initialized_);
+ ///////////////////////////////////////////////////////////
+ // Get the vector/matrix spaces for the original problem //
+ ///////////////////////////////////////////////////////////
+
+ SmartPtr<const VectorSpace> orig_x_space;
+ SmartPtr<const VectorSpace> orig_c_space;
+ SmartPtr<const VectorSpace> orig_d_space;
+ SmartPtr<const VectorSpace> orig_x_l_space;
+ SmartPtr<const MatrixSpace> orig_px_l_space;
+ SmartPtr<const VectorSpace> orig_x_u_space;
+ SmartPtr<const MatrixSpace> orig_px_u_space;
+ SmartPtr<const VectorSpace> orig_d_l_space;
+ SmartPtr<const MatrixSpace> orig_pd_l_space;
+ SmartPtr<const VectorSpace> orig_d_u_space;
+ SmartPtr<const MatrixSpace> orig_pd_u_space;
+ SmartPtr<const MatrixSpace> orig_jac_c_space;
+ SmartPtr<const MatrixSpace> orig_jac_d_space;
+ SmartPtr<const SymMatrixSpace> orig_h_space;
+
+ orig_ip_nlp_->GetSpaces(orig_x_space, orig_c_space, orig_d_space,
+ orig_x_l_space, orig_px_l_space,
+ orig_x_u_space, orig_px_u_space,
+ orig_d_l_space, orig_pd_l_space,
+ orig_d_u_space, orig_pd_u_space,
+ orig_jac_c_space, orig_jac_d_space,
+ orig_h_space);
+
+ // Create the restoration phase problem vector/matrix spaces, based
+ // on the original spaces (pretty inconvenient with all the
+ // matrix spaces, isn't it?!?)
+ DBG_PRINT((1, "Creating the x_space_\n"));
+ // vector x
+ Index total_dim = orig_x_space->Dim() + 2*orig_c_space->Dim()
+ + 2*orig_d_space->Dim();
+ x_space_ = new CompoundVectorSpace(5, total_dim);
+ x_space_->SetCompSpace(0, *orig_x_space);
+ x_space_->SetCompSpace(1, *orig_c_space); // n_c
+ x_space_->SetCompSpace(2, *orig_c_space); // p_c
+ x_space_->SetCompSpace(3, *orig_d_space); // n_d
+ x_space_->SetCompSpace(4, *orig_d_space); // p_d
+
+ DBG_PRINT((1, "Setting the c_space_\n"));
+ // vector c
+ c_space_ = orig_c_space;
+
+ DBG_PRINT((1, "Setting the d_space_\n"));
+ // vector d
+ d_space_ = orig_d_space;
+
+ DBG_PRINT((1, "Creating the x_l_space_\n"));
+ // vector x_L
+ total_dim = orig_x_l_space->Dim() + 2*orig_c_space->Dim()
+ + 2*orig_d_space->Dim();
+ x_l_space_ = new CompoundVectorSpace(5, total_dim);
+ x_l_space_->SetCompSpace(0, *orig_x_l_space);
+ x_l_space_->SetCompSpace(1, *orig_c_space); // n_c >=0
+ x_l_space_->SetCompSpace(2, *orig_c_space); // p_c >=0
+ x_l_space_->SetCompSpace(3, *orig_d_space); // n_d >=0
+ x_l_space_->SetCompSpace(4, *orig_d_space); // p_d >=0
+
+ DBG_PRINT((1, "Setting the x_u_space_\n"));
+ // vector x_U
+ x_u_space_ = orig_x_u_space;
+
+ DBG_PRINT((1, "Creating the px_l_space_\n"));
+ // matrix px_l
+ Index total_rows = orig_x_space->Dim() + 2*orig_c_space->Dim()
+ + 2*orig_d_space->Dim();
+ Index total_cols = orig_x_l_space->Dim() + 2*orig_c_space->Dim()
+ + 2*orig_d_space->Dim();
+ px_l_space_ = new CompoundMatrixSpace(5, 5, total_rows, total_cols);
+ px_l_space_->SetBlockRows(0, orig_x_space->Dim());
+ px_l_space_->SetBlockRows(1, orig_c_space->Dim());
+ px_l_space_->SetBlockRows(2, orig_c_space->Dim());
+ px_l_space_->SetBlockRows(3, orig_d_space->Dim());
+ px_l_space_->SetBlockRows(4, orig_d_space->Dim());
+ px_l_space_->SetBlockCols(0, orig_x_l_space->Dim());
+ px_l_space_->SetBlockCols(1, orig_c_space->Dim());
+ px_l_space_->SetBlockCols(2, orig_c_space->Dim());
+ px_l_space_->SetBlockCols(3, orig_d_space->Dim());
+ px_l_space_->SetBlockCols(4, orig_d_space->Dim());
+
+ px_l_space_->SetCompSpace(0, 0, *orig_px_l_space);
+ // now setup the identity matrix
+ // This could be changed to be something like...
+ // px_l_space_->SetBlockToIdentity(1,1,1.0);
+ // px_l_space_->SetBlockToIdentity(2,2,other_factor);
+ // ... etc with some simple changes to the CompoundMatrixSpace
+ // to allow this (space should auto create the matrices)
+ //
+ // for now, we use the new feature and set the true flag for this block
+ // to say that the matrices should be auto_allocated
+ SmartPtr<const MatrixSpace> identity_mat_space_nc
+ = new IdentityMatrixSpace(orig_c_space->Dim());
+ px_l_space_->SetCompSpace(1, 1, *identity_mat_space_nc, true);
+ px_l_space_->SetCompSpace(2, 2, *identity_mat_space_nc, true);
+ SmartPtr<const MatrixSpace> identity_mat_space_nd
+ = new IdentityMatrixSpace(orig_d_space->Dim());
+ px_l_space_->SetCompSpace(3, 3, *identity_mat_space_nd, true);
+ px_l_space_->SetCompSpace(4, 4, *identity_mat_space_nd, true);
+
+ DBG_PRINT((1, "Creating the px_u_space_\n"));
+ // matrix px_u px_u_space_->SetBlockRows(0, orig_x_space->Dim());
+
+ total_rows = orig_x_space->Dim() + 2*orig_c_space->Dim()
+ + 2*orig_d_space->Dim();
+ total_cols = orig_x_u_space->Dim();
+ DBG_PRINT((1, "total_rows = %d, total_cols = %d\n",total_rows, total_cols));
+ px_u_space_ = new CompoundMatrixSpace(5, 1, total_rows, total_cols);
+ px_u_space_->SetBlockRows(0, orig_x_space->Dim());
+ px_u_space_->SetBlockRows(1, orig_c_space->Dim());
+ px_u_space_->SetBlockRows(2, orig_c_space->Dim());
+ px_u_space_->SetBlockRows(3, orig_d_space->Dim());
+ px_u_space_->SetBlockRows(4, orig_d_space->Dim());
+ px_u_space_->SetBlockCols(0, orig_x_u_space->Dim());
+
+ px_u_space_->SetCompSpace(0, 0, *orig_px_u_space);
+ // other matrices are zero'ed out
+
+ // vector d_L
+ d_l_space_ = orig_d_l_space;
+
+ // vector d_U
+ d_u_space_ = orig_d_u_space;
+
+ // matrix pd_L
+ pd_l_space_ = orig_pd_l_space;
+
+ // matrix pd_U
+ pd_u_space_ = orig_pd_u_space;
+
+ DBG_PRINT((1, "Creating the jac_c_space_\n"));
+ // matrix jac_c
+ total_rows = orig_c_space->Dim();
+ total_cols = orig_x_space->Dim() + 2*orig_c_space->Dim()
+ + 2*orig_d_space->Dim();
+ jac_c_space_ = new CompoundMatrixSpace(1, 5, total_rows, total_cols);
+ jac_c_space_->SetBlockRows(0, orig_c_space->Dim());
+ jac_c_space_->SetBlockCols(0, orig_x_space->Dim());
+ jac_c_space_->SetBlockCols(1, orig_c_space->Dim());
+ jac_c_space_->SetBlockCols(2, orig_c_space->Dim());
+ jac_c_space_->SetBlockCols(3, orig_d_space->Dim());
+ jac_c_space_->SetBlockCols(4, orig_d_space->Dim());
+
+ jac_c_space_->SetCompSpace(0, 0, *orig_jac_c_space);
+ jac_c_space_->SetCompSpace(0, 1, *identity_mat_space_nc, true);
+ jac_c_space_->SetCompSpace(0, 2, *identity_mat_space_nc, true);
+ // remaining blocks are zero'ed
+
+ DBG_PRINT((1, "Creating the jac_d_space_\n"));
+ // matrix jac_d
+ total_rows = orig_d_space->Dim();
+ total_cols = orig_x_space->Dim() + 2*orig_c_space->Dim()
+ + 2*orig_d_space->Dim();
+ jac_d_space_ = new CompoundMatrixSpace(1, 5, total_rows, total_cols);
+ jac_d_space_->SetBlockRows(0, orig_d_space->Dim());
+ jac_d_space_->SetBlockCols(0, orig_x_space->Dim());
+ jac_d_space_->SetBlockCols(1, orig_c_space->Dim());
+ jac_d_space_->SetBlockCols(2, orig_c_space->Dim());
+ jac_d_space_->SetBlockCols(3, orig_d_space->Dim());
+ jac_d_space_->SetBlockCols(4, orig_d_space->Dim());
+
+ jac_d_space_->SetCompSpace(0, 0, *orig_jac_d_space);
+ DBG_PRINT((1, "orig_jac_d_space = %x\n", GetRawPtr(orig_jac_d_space)))
+ // Blocks (0,1) and (0,2) are zero'ed out
+ jac_d_space_->SetCompSpace(0, 3, *identity_mat_space_nd, true);
+ jac_d_space_->SetCompSpace(0, 4, *identity_mat_space_nd, true);
+
+ DBG_PRINT((1, "Creating the h_space_\n"));
+ // matrix h
+ total_dim = orig_x_space->Dim() + 2*orig_c_space->Dim()
+ + 2*orig_d_space->Dim();
+ h_space_ = new CompoundSymMatrixSpace(5, total_dim);
+ h_space_->SetBlockDim(0, orig_x_space->Dim());
+ h_space_->SetBlockDim(1, orig_c_space->Dim());
+ h_space_->SetBlockDim(2, orig_c_space->Dim());
+ h_space_->SetBlockDim(3, orig_d_space->Dim());
+ h_space_->SetBlockDim(4, orig_d_space->Dim());
+
+ SmartPtr<DiagMatrixSpace> DR_x_space
+ = new DiagMatrixSpace(orig_x_space->Dim());
+ if (hessian_approximation_==LIMITED_MEMORY) {
+ const LowRankUpdateSymMatrixSpace* LR_h_space =
+ dynamic_cast<const LowRankUpdateSymMatrixSpace*> (GetRawPtr(orig_h_space));
+ DBG_ASSERT(LR_h_space);
+ SmartPtr<LowRankUpdateSymMatrixSpace> new_orig_h_space =
+ new LowRankUpdateSymMatrixSpace(LR_h_space->Dim(),
+ NULL,
+ orig_x_space,
+ false);
+ h_space_->SetCompSpace(0, 0, *new_orig_h_space, true);
+ }
+ else {
+ SmartPtr<SumSymMatrixSpace> sumsym_mat_space =
+ new SumSymMatrixSpace(orig_x_space->Dim(), 2);
+ sumsym_mat_space->SetTermSpace(0, *orig_h_space);
+ sumsym_mat_space->SetTermSpace(1, *DR_x_space);
+ h_space_->SetCompSpace(0, 0, *sumsym_mat_space, true);
+ // All remaining blocks are zero'ed out
+ }
+
+ SmartPtr<const MatrixSpace> scaled_jac_c_space;
+ SmartPtr<const MatrixSpace> scaled_jac_d_space;
+ SmartPtr<const SymMatrixSpace> scaled_h_space;
+ NLP_scaling()->DetermineScaling(GetRawPtr(x_space_),
+ c_space_, d_space_,
+ GetRawPtr(jac_c_space_),
+ GetRawPtr(jac_d_space_),
+ GetRawPtr(h_space_),
+ scaled_jac_c_space, scaled_jac_d_space,
+ scaled_h_space);
+ // For now we assume that no scaling is done inside the NLP_Scaling
+ DBG_ASSERT(scaled_jac_c_space == jac_c_space_);
+ DBG_ASSERT(scaled_jac_d_space == jac_d_space_);
+ DBG_ASSERT(scaled_h_space == h_space_);
+
+ ///////////////////////////
+ // Create the bound data //
+ ///////////////////////////
+
+ // x_L
+ x_L_ = x_l_space_->MakeNewCompoundVector();
+ x_L_->SetComp(0, *orig_ip_nlp_->x_L()); // x >= x_L
+ x_L_->GetCompNonConst(1)->Set(0.0); // n_c >= 0
+ x_L_->GetCompNonConst(2)->Set(0.0); // p_c >= 0
+ x_L_->GetCompNonConst(3)->Set(0.0); // n_d >= 0
+ x_L_->GetCompNonConst(4)->Set(0.0); // p_d >= 0
+ DBG_PRINT_VECTOR(2,"resto_x_L", *x_L_);
+
+ // x_U
+ x_U_ = orig_ip_nlp_->x_U();
+
+ // d_L
+ d_L_ = orig_ip_nlp_->d_L();
+
+ // d_U
+ d_U_ = orig_ip_nlp_->d_U();
+
+ // Px_L
+ Px_L_ = px_l_space_->MakeNewCompoundMatrix();
+ Px_L_->SetComp(0, 0, *orig_ip_nlp_->Px_L());
+ // Identities are auto-created (true flag passed into SetCompSpace)
+
+ // Px_U
+ Px_U_ = px_u_space_->MakeNewCompoundMatrix();
+ Px_U_->SetComp(0, 0, *orig_ip_nlp_->Px_U());
+ // Remaining matrices will be zero'ed out
+
+ // Pd_L
+ Pd_L_ = orig_ip_nlp_->Pd_L();
+
+ // Pd_U
+ Pd_U_ = orig_ip_nlp_->Pd_U();
+
+ /////////////////////////////////////////////////////////////////////////
+ // Create and initialize the vectors for the restoration phase problem //
+ /////////////////////////////////////////////////////////////////////////
+
+ // Vector x
+ SmartPtr<CompoundVector> comp_x = x_space_->MakeNewCompoundVector();
+ if (init_x) {
+ comp_x->GetCompNonConst(0)->Copy(*orig_ip_data_->curr()->x());
+ comp_x->GetCompNonConst(1)->Set(1.0);
+ comp_x->GetCompNonConst(2)->Set(1.0);
+ comp_x->GetCompNonConst(3)->Set(1.0);
+ comp_x->GetCompNonConst(4)->Set(1.0);
+ }
+ x = GetRawPtr(comp_x);
+
+ // Vector y_c
+ y_c = c_space_->MakeNew();
+ if (init_y_c) {
+ y_c->Set(0.0); // ToDo
+ }
+
+ // Vector y_d
+ y_d = d_space_->MakeNew();
+ if (init_y_d) {
+ y_d->Set(0.0);
+ }
+
+ // Vector z_L
+ z_L = x_l_space_->MakeNew();
+ if (init_z_L) {
+ z_L->Set(1.0);
+ }
+
+ // Vector z_U
+ z_U = x_u_space_->MakeNew();
+ if (init_z_U) {
+ z_U->Set(1.0);
+ }
+
+ // Vector v_L
+ v_L = d_l_space_->MakeNew();
+
+ // Vector v_U
+ v_U = d_u_space_->MakeNew();
+
+ // Initialize other data needed by the restoration nlp. x_ref is
+ // the point to reference to which we based the regularization
+ // term
+ x_ref_ = orig_x_space->MakeNew();
+ x_ref_->Copy(*orig_ip_data_->curr()->x());
+
+ dr_x_ = orig_x_space->MakeNew();
+ dr_x_->Set(1.0);
+ SmartPtr<Vector> tmp = dr_x_->MakeNew();
+ tmp->Copy(*x_ref_);
+ dr_x_->ElementWiseMax(*tmp);
+ tmp->Scal(-1.);
+ dr_x_->ElementWiseMax(*tmp);
+ dr_x_->ElementWiseReciprocal();
+ DBG_PRINT_VECTOR(2, "dr_x_", *dr_x_);
+ DR_x_ = DR_x_space->MakeNewDiagMatrix();
+ DR_x_->SetDiag(*dr_x_);
+
+ return true;
+ }
+
+ Number RestoIpoptNLP::f(const Vector& x)
+ {
+ THROW_EXCEPTION(INTERNAL_ABORT,
+ "ERROR: In RestoIpoptNLP f() is called without mu!");
+ return 0.;
+ }
+
+ Number RestoIpoptNLP::f(const Vector& x, Number mu)
+ {
+ DBG_START_METH("RestoIpoptNLP::f",
+ dbg_verbosity);
+ Number ret = 0.0;
+ // rho*(pcTe + ncTe + pdT*e + ndT*e) + eta/2*||Dr*(x-xr)||_2^2
+ const CompoundVector* c_vec = dynamic_cast<const CompoundVector*>(&x);
+ DBG_ASSERT(c_vec);
+ SmartPtr<const Vector> x_only = c_vec->GetComp(0);
+ ret = x.Sum() - x_only->Sum();
+ DBG_PRINT((1,"xdiff sum = %e\n",ret));
+ ret = rho_ * ret;
+ DBG_PRINT((1,"rho_ = %e\n",rho_));
+
+ SmartPtr<Vector> x_diff = x_only->MakeNew();
+ x_diff->Copy(*x_only);
+ x_diff->Axpy(-1.0, *x_ref_);
+ DBG_PRINT_VECTOR(2,"x_ref",*x_ref_);
+ x_diff->ElementWiseMultiply(*dr_x_);
+ Number ret2 = x_diff->Nrm2();
+ DBG_PRINT((1,"Eta = %e\n",Eta(mu)));
+ ret2 = Eta(mu)/2.0*ret2*ret2;
+
+ ret += ret2;
+
+ // We evaluate also the objective function for the original
+ // problem here. This might be wasteful, but it will detect if
+ // the original objective function cannot be evaluated at the
+ // trial point in the restoration phase
+ if (evaluate_orig_obj_at_resto_trial_) {
+ /* Number orig_f = */ orig_ip_nlp_->f(*x_only);
+ }
+
+ return ret;
+ }
+
+ SmartPtr<const Vector> RestoIpoptNLP::grad_f(const Vector& x, Number mu)
+ {
+ SmartPtr<Vector> retPtr = x.MakeNew();
+ // Scale the p's and n's by rho (Scale all, take out the x part later)
+ retPtr->Set(rho_);
+
+ const CompoundVector* c_vec_in = dynamic_cast<const CompoundVector*>(&x);
+ SmartPtr<const Vector> x_only_in = c_vec_in->GetComp(0);
+
+ CompoundVector* c_vec = dynamic_cast<CompoundVector*>(GetRawPtr(retPtr));
+ DBG_ASSERT(c_vec);
+ SmartPtr<Vector> x_only = c_vec->GetCompNonConst(0);
+ x_only->Copy(*x_only_in);
+ x_only->Axpy(-1.0, *x_ref_);
+ x_only->ElementWiseMultiply(*dr_x_);
+ x_only->Scal(Eta(mu));
+
+ return ConstPtr(retPtr);
+ }
+
+ SmartPtr<const Vector> RestoIpoptNLP::c(const Vector& x)
+ {
+ const CompoundVector* c_vec = dynamic_cast<const CompoundVector*>(&x);
+ SmartPtr<const Vector> x_only = c_vec->GetComp(0);
+ SmartPtr<const Vector> nc_only = c_vec->GetComp(1);
+ SmartPtr<const Vector> pc_only = c_vec->GetComp(2);
+
+ SmartPtr<const Vector> orig_c = orig_ip_nlp_->c(*x_only);
+ SmartPtr<Vector> retPtr = c_space_->MakeNew();
+ retPtr->Copy(*orig_c);
+ retPtr->Axpy(1.0, *nc_only);
+ retPtr->Axpy(-1.0, *pc_only);
+
+ return GetRawPtr(retPtr);
+ }
+
+ SmartPtr<const Vector> RestoIpoptNLP::grad_f(const Vector& x)
+ {
+ THROW_EXCEPTION(INTERNAL_ABORT,
+ "ERROR: In RestoIpoptNLP grad_f() is called without mu!");
+ }
+
+ SmartPtr<const Vector> RestoIpoptNLP::d(const Vector& x)
+ {
+ const CompoundVector* c_vec = dynamic_cast<const CompoundVector*>(&x);
+ SmartPtr<const Vector> x_only = c_vec->GetComp(0);
+ SmartPtr<const Vector> nd_only = c_vec->GetComp(3);
+ SmartPtr<const Vector> pd_only = c_vec->GetComp(4);
+
+ SmartPtr<const Vector> orig_d = orig_ip_nlp_->d(*x_only);
+ SmartPtr<Vector> retPtr = d_space_->MakeNew();
+ retPtr->Copy(*orig_d);
+ retPtr->Axpy(1., *nd_only);
+ retPtr->Axpy(-1., *pd_only);
+
+ return GetRawPtr(retPtr);
+ }
+
+ SmartPtr<const Matrix> RestoIpoptNLP::jac_c(const Vector& x)
+ {
+
+ // Here, we set the (0,0) block with the values from the
+ // original jac_c and set the factor for the -I (jac w.r.t. p_c)
+
+ // get out the x_only part
+ const CompoundVector* c_vec = dynamic_cast<const CompoundVector*>(&x);
+ DBG_ASSERT(c_vec);
+ SmartPtr<const Vector> x_only = c_vec->GetComp(0);
+
+ // calculate the jacobian for the original problem
+ SmartPtr<const Matrix> jac_c_only = orig_ip_nlp_->jac_c(*x_only);
+
+ // Create the new compound matrix
+ // The zero parts remain NULL, the identities are created from the matrix
+ // space (since auto_allocate was set to true in SetCompSpace)
+ SmartPtr<CompoundMatrix> retPtr = jac_c_space_->MakeNewCompoundMatrix();
+
+ // set the (0,0) block to the original jacobian
+ retPtr->SetComp(0,0,*jac_c_only);
+
+ // we currently do not have a default factor in the matrix spaces
+ // so we need to set the factor on the identity (jacobian of the
+ // restoration c w.r.t. p_c is -I)
+ // This could easily be changed to include special processing
+ // for identities in the CompoundMatrixSpace (and a factor)
+ SmartPtr<Matrix> jac_c_pc_mat = retPtr->GetCompNonConst(0,2);
+ IdentityMatrix* jac_c_pc = dynamic_cast<IdentityMatrix*>(GetRawPtr(jac_c_pc_mat));
+ DBG_ASSERT(jac_c_pc);
+ jac_c_pc->SetFactor(-1.0);
+
+ return GetRawPtr(retPtr);
+ }
+
+ SmartPtr<const Matrix> RestoIpoptNLP::jac_d(const Vector& x)
+ {
+ DBG_START_METH("RestoIpoptNLP::jac_d", dbg_verbosity);
+
+ // Here, we set the (0,0) block with the values from the
+ // original jac_d and set the factor for the -I (jac w.r.t. p_d)
+
+ // get out the x_only part
+ const CompoundVector* c_vec = dynamic_cast<const CompoundVector*>(&x);
+ DBG_ASSERT(c_vec);
+ SmartPtr<const Vector> x_only = c_vec->GetComp(0);
+
+ // calculate the jacobian for the original problem
+ SmartPtr<const Matrix> jac_d_only = orig_ip_nlp_->jac_d(*x_only);
+
+ // Create the new compound matrix
+ // The zero parts remain NULL, the identities are created from the matrix
+ // space (since auto_allocate was set to true in SetCompSpace)
+ SmartPtr<CompoundMatrix> retPtr = jac_d_space_->MakeNewCompoundMatrix();
+ DBG_PRINT((1, "jac_d_space_ = %x\n", GetRawPtr(jac_d_space_)))
+
+ // Set the block for the original Jacobian
+ retPtr->SetComp(0,0,*jac_d_only);
+
+ // (0,1) and (0,2) blocks are zero (NULL)
+
+ // set the factor for the identity matrix for the pd variables
+ // (likr in jac_c)
+ SmartPtr<Matrix> jac_d_pd_mat = retPtr->GetCompNonConst(0,4);
+ IdentityMatrix* jac_d_pd = dynamic_cast<IdentityMatrix*>(GetRawPtr(jac_d_pd_mat));
+ DBG_ASSERT(jac_d_pd);
+ jac_d_pd->SetFactor(-1.0);
+
+ return GetRawPtr(retPtr);
+ }
+
+ SmartPtr<const SymMatrix> RestoIpoptNLP::h(const Vector& x,
+ Number obj_factor,
+ const Vector& yc,
+ const Vector& yd
+ )
+ {
+ assert(false && "ERROR: In RestoIpoptNLP h() is called without mu!");
+ return NULL;
+ }
+
+ SmartPtr<const SymMatrix> RestoIpoptNLP::h(const Vector& x,
+ Number obj_factor,
+ const Vector& yc,
+ const Vector& yd,
+ Number mu)
+ {
+ // Here, we use a SumSymMatrix for the (0,0) block of the
+ // Hessian. We need to set this to the hessian of the restoration
+ // problem, which is the hessian of the objective from the restoration
+ // problem + the constraint only part of the hessian from the original
+ // problem
+ // All other blocks are zero'ed (NULL)
+
+ // get the x_only part
+ const CompoundVector* c_vec = dynamic_cast<const CompoundVector*>(&x);
+ DBG_ASSERT(c_vec);
+ SmartPtr<const Vector> x_only = c_vec->GetComp(0);
+
+ // yc and yd should not be compound vectors
+
+ // calculate the original hessian
+ SmartPtr<const SymMatrix> h_con_orig = orig_ip_nlp_->h(*x_only, 0.0, yc, yd);
+
+ // Create the new compound matrix
+ // The SumSymMatrix is auto_allocated
+ SmartPtr<CompoundSymMatrix> retPtr = h_space_->MakeNewCompoundSymMatrix();
+
+ // Set the entries in the SumSymMatrix
+ SmartPtr<Matrix> h_sum_mat = retPtr->GetCompNonConst(0,0);
+ SmartPtr<SumSymMatrix> h_sum = dynamic_cast<SumSymMatrix*>(GetRawPtr(h_sum_mat));
+ h_sum->SetTerm(0, 1.0, *h_con_orig);
+ h_sum->SetTerm(1, obj_factor*Eta(mu), *DR_x_);
+
+ return GetRawPtr(retPtr);
+ }
+
+ SmartPtr<const SymMatrix> RestoIpoptNLP::uninitialized_h()
+ {
+ SmartPtr<CompoundSymMatrix> retPtr;
+ if (hessian_approximation_==LIMITED_MEMORY) {
+ retPtr = h_space_->MakeNewCompoundSymMatrix();
+ }
+ else {
+ SmartPtr<const SymMatrix> h_con_orig = orig_ip_nlp_->uninitialized_h();
+ retPtr = h_space_->MakeNewCompoundSymMatrix();
+ SmartPtr<Matrix> h_sum_mat = retPtr->GetCompNonConst(0,0);
+ SmartPtr<SumSymMatrix> h_sum = dynamic_cast<SumSymMatrix*>(GetRawPtr(h_sum_mat));
+ h_sum->SetTerm(0, 1.0, *h_con_orig);
+ h_sum->SetTerm(1, 1.0, *DR_x_);
+ }
+
+ return GetRawPtr(retPtr);
+ }
+
+ void RestoIpoptNLP::GetSpaces(SmartPtr<const VectorSpace>& x_space,
+ SmartPtr<const VectorSpace>& c_space,
+ SmartPtr<const VectorSpace>& d_space,
+ SmartPtr<const VectorSpace>& x_l_space,
+ SmartPtr<const MatrixSpace>& px_l_space,
+ SmartPtr<const VectorSpace>& x_u_space,
+ SmartPtr<const MatrixSpace>& px_u_space,
+ SmartPtr<const VectorSpace>& d_l_space,
+ SmartPtr<const MatrixSpace>& pd_l_space,
+ SmartPtr<const VectorSpace>& d_u_space,
+ SmartPtr<const MatrixSpace>& pd_u_space,
+ SmartPtr<const MatrixSpace>& Jac_c_space,
+ SmartPtr<const MatrixSpace>& Jac_d_space,
+ SmartPtr<const SymMatrixSpace>& Hess_lagrangian_space)
+ {
+ x_space = GetRawPtr(x_space_);
+ c_space = GetRawPtr(c_space_);
+ d_space = GetRawPtr(d_space_);
+ x_l_space = GetRawPtr(x_l_space_);
+ px_l_space = GetRawPtr(px_l_space_);
+ x_u_space = GetRawPtr(x_u_space_);
+ px_u_space = GetRawPtr(px_u_space_);
+ d_l_space = GetRawPtr(d_l_space_);
+ pd_l_space = GetRawPtr(pd_l_space_);
+ d_u_space = GetRawPtr(d_u_space_);
+ pd_u_space = GetRawPtr(pd_u_space_);
+ Jac_c_space = GetRawPtr(jac_c_space_);
+ Jac_d_space = GetRawPtr(jac_d_space_);
+ Hess_lagrangian_space = GetRawPtr(h_space_);
+ }
+
+ Number RestoIpoptNLP::Eta(Number mu) const
+ {
+ return eta_factor_ * pow(mu, eta_mu_exponent_);
+ }
+
+ void RestoIpoptNLP::AdjustVariableBounds(const Vector& new_x_L, const Vector& new_x_U,
+ const Vector& new_d_L, const Vector& new_d_U)
+ {
+
+ const CompoundVector* comp_new_x_L =
+ dynamic_cast<const CompoundVector*>(&new_x_L);
+ DBG_ASSERT(comp_new_x_L);
+
+ SmartPtr<const Vector> new_orig_x_L = comp_new_x_L->GetComp(0);
+
+ // adapt bounds for the original NLP
+ orig_ip_nlp_->AdjustVariableBounds(*new_orig_x_L, new_x_U, new_d_L, new_d_U);
+
+ // adapt bounds for the p and n variables
+ SmartPtr<const Vector> new_nc_L = comp_new_x_L->GetComp(1);
+ SmartPtr<const Vector> new_pc_L = comp_new_x_L->GetComp(2);
+ SmartPtr<const Vector> new_nd_L = comp_new_x_L->GetComp(3);
+ SmartPtr<const Vector> new_pd_L = comp_new_x_L->GetComp(4);
+
+ x_L_->GetCompNonConst(1)->Copy(*new_nc_L);
+ x_L_->GetCompNonConst(2)->Copy(*new_pc_L);
+ x_L_->GetCompNonConst(3)->Copy(*new_nd_L);
+ x_L_->GetCompNonConst(4)->Copy(*new_pd_L);
+
+ }
+
+ bool RestoIpoptNLP::IntermediateCallBack(AlgorithmMode mode,
+ Index iter, Number obj_value,
+ Number inf_pr, Number inf_du,
+ Number mu, Number d_norm,
+ Number regularization_size,
+ Number alpha_du, Number alpha_pr,
+ Index ls_trials,
+ SmartPtr<const IpoptData> ip_data,
+ SmartPtr<IpoptCalculatedQuantities> ip_cq)
+ {
+ return orig_ip_nlp_->IntermediateCallBack(mode, iter, obj_value, inf_pr, inf_du,
+ mu, d_norm, regularization_size,
+ alpha_du, alpha_pr, ls_trials,
+ ip_data, ip_cq);
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpRestoIpoptNLP.hpp b/SimTKmath/Optimizers/src/IpOpt/IpRestoIpoptNLP.hpp
new file mode 100644
index 0000000..db3bade
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpRestoIpoptNLP.hpp
@@ -0,0 +1,429 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpRestoIpoptNLP.hpp 765 2006-07-14 18:03:23Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPRESTOIPOPTNLP_HPP__
+#define __IPRESTOIPOPTNLP_HPP__
+
+#include "IpIpoptNLP.hpp"
+#include "IpIpoptData.hpp"
+#include "IpIpoptCalculatedQuantities.hpp"
+#include "IpCompoundMatrix.hpp"
+#include "IpCompoundSymMatrix.hpp"
+#include "IpCompoundVector.hpp"
+#include "IpIdentityMatrix.hpp"
+#include "IpDiagMatrix.hpp"
+#include "IpZeroMatrix.hpp"
+#include "IpOrigIpoptNLP.hpp"
+
+namespace Ipopt
+{
+
+ /** This class maps the traditional NLP into
+ * something that is more useful by Ipopt.
+ * This class takes care of storing the
+ * calculated model results, handles cacheing,
+ * and (some day) takes care of addition of slacks.
+ */
+ class RestoIpoptNLP : public IpoptNLP
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ RestoIpoptNLP(IpoptNLP& orig_ip_nlp,
+ IpoptData& orig_ip_data,
+ IpoptCalculatedQuantities& orig_ip_cq);
+
+ /** Default destructor */
+ ~RestoIpoptNLP();
+ //@}
+
+ /** Initialize - overloaded from IpoptNLP */
+ virtual bool Initialize(const Journalist& jnlst,
+ const OptionsList& options,
+ const std::string& prefix);
+
+ /** Initialize (create) structures for
+ * the iteration data */
+ virtual bool InitializeStructures(SmartPtr<Vector>& x,
+ bool init_x,
+ SmartPtr<Vector>& y_c,
+ bool init_y_c,
+ SmartPtr<Vector>& y_d,
+ bool init_y_d,
+ SmartPtr<Vector>& z_L,
+ bool init_z_L,
+ SmartPtr<Vector>& z_U,
+ bool init_z_U,
+ SmartPtr<Vector>& v_L,
+ SmartPtr<Vector>& v_U
+ );
+
+ /** Method accessing the GetWarmStartIterate of the NLP */
+ virtual bool GetWarmStartIterate(IteratesVector& warm_start_iterate)
+ {
+ return false;
+ }
+
+ /** Solution Routines - overloaded from IpoptNLP*/
+ //@{
+ void FinalizeSolution(SolverReturn status,
+ const Vector& x, const Vector& z_L, const Vector& z_U,
+ const Vector& c, const Vector& d,
+ const Vector& y_c, const Vector& y_d,
+ Number obj_value)
+ {}
+ //@}
+
+ /** Accessor methods for model data */
+ //@{
+ /** Method for telling IpoptCalculatedQuantities that the
+ * restoration phase objective function depends on the barrier
+ * parameter */
+ virtual bool objective_depends_on_mu() const
+ {
+ return true;
+ }
+
+ /** Objective value (incorrect version for restoration phase) */
+ virtual Number f(const Vector& x);
+
+ /** Objective value */
+ virtual Number f(const Vector& x, Number mu);
+
+ /** Gradient of the objective (incorrect version for restoration phase) */
+ virtual SmartPtr<const Vector> grad_f(const Vector& x);
+
+ /** Gradient of the objective */
+ virtual SmartPtr<const Vector> grad_f(const Vector& x, Number mu);
+
+ /** Equality constraint residual */
+ virtual SmartPtr<const Vector> c(const Vector& x);
+
+ /** Jacobian Matrix for equality constraints */
+ virtual SmartPtr<const Matrix> jac_c(const Vector& x);
+
+ /** Inequality constraint residual (reformulated
+ * as equalities with slacks */
+ virtual SmartPtr<const Vector> d(const Vector& x);
+
+ /** Jacobian Matrix for inequality constraints */
+ virtual SmartPtr<const Matrix> jac_d(const Vector& x);
+
+ /** Hessian of the Lagrangian (incorrect version for restoration
+ * phase) */
+ virtual SmartPtr<const SymMatrix> h(const Vector& x,
+ Number obj_factor,
+ const Vector& yc,
+ const Vector& yd
+ );
+
+ /** Hessian of the Lagrangian */
+ virtual SmartPtr<const SymMatrix> h(const Vector& x,
+ Number obj_factor,
+ const Vector& yc,
+ const Vector& yd,
+ Number mu);
+
+ /** Provides a Hessian matrix from the correct matrix space with
+ * uninitialized values. This can be used in LeastSquareMults to
+ * obtain a "zero Hessian". */
+ virtual SmartPtr<const SymMatrix> uninitialized_h();
+
+ /** Lower bounds on x */
+ virtual SmartPtr<const Vector> x_L()
+ {
+ return GetRawPtr(x_L_);
+ }
+
+ /** Permutation matrix (x_L_ -> x) */
+ virtual SmartPtr<const Matrix> Px_L()
+ {
+ return GetRawPtr(Px_L_);
+ }
+
+ /** Upper bounds on x */
+ virtual SmartPtr<const Vector> x_U()
+ {
+ return GetRawPtr(x_U_);
+ }
+
+ /** Permutation matrix (x_U_ -> x */
+ virtual SmartPtr<const Matrix> Px_U()
+ {
+ return GetRawPtr(Px_U_);
+ }
+
+ /** Lower bounds on d */
+ virtual SmartPtr<const Vector> d_L()
+ {
+ return GetRawPtr(d_L_);
+ }
+
+ /** Permutation matrix (d_L_ -> d) */
+ virtual SmartPtr<const Matrix> Pd_L()
+ {
+ return GetRawPtr(Pd_L_);
+ }
+
+ /** Upper bounds on d */
+ virtual SmartPtr<const Vector> d_U()
+ {
+ return GetRawPtr(d_U_);
+ }
+
+ /** Permutation matrix (d_U_ -> d */
+ virtual SmartPtr<const Matrix> Pd_U()
+ {
+ return GetRawPtr(Pd_U_);
+ }
+
+ virtual SmartPtr<const SymMatrixSpace> HessianMatrixSpace() const
+ {
+ return GetRawPtr(h_space_);
+ }
+ //@}
+
+ /** Accessor method for vector/matrix spaces pointers */
+ virtual void GetSpaces(SmartPtr<const VectorSpace>& x_space,
+ SmartPtr<const VectorSpace>& c_space,
+ SmartPtr<const VectorSpace>& d_space,
+ SmartPtr<const VectorSpace>& x_l_space,
+ SmartPtr<const MatrixSpace>& px_l_space,
+ SmartPtr<const VectorSpace>& x_u_space,
+ SmartPtr<const MatrixSpace>& px_u_space,
+ SmartPtr<const VectorSpace>& d_l_space,
+ SmartPtr<const MatrixSpace>& pd_l_space,
+ SmartPtr<const VectorSpace>& d_u_space,
+ SmartPtr<const MatrixSpace>& pd_u_space,
+ SmartPtr<const MatrixSpace>& Jac_c_space,
+ SmartPtr<const MatrixSpace>& Jac_d_space,
+ SmartPtr<const SymMatrixSpace>& Hess_lagrangian_space);
+ /** Method for adapting the variable bounds. This is called if
+ * slacks are becoming too small */
+ virtual void AdjustVariableBounds(const Vector& new_x_L,
+ const Vector& new_x_U,
+ const Vector& new_d_L,
+ const Vector& new_d_U);
+
+ /** User callback method */
+ bool IntermediateCallBack(AlgorithmMode mode,
+ Index iter, Number obj_value,
+ Number inf_pr, Number inf_du,
+ Number mu, Number d_norm,
+ Number regularization_size,
+ Number alpha_du, Number alpha_pr,
+ Index ls_trials,
+ SmartPtr<const IpoptData> ip_data,
+ SmartPtr<IpoptCalculatedQuantities> ip_cq);
+
+ /** @name Accessor method for the information of the original NLP.
+ * These methods are not overloaded from IpoptNLP */
+ //@{
+ IpoptNLP& OrigIpNLP() const
+ {
+ return *orig_ip_nlp_;
+ }
+ IpoptData& OrigIpData() const
+ {
+ return *orig_ip_data_;
+ }
+ IpoptCalculatedQuantities& OrigIpCq() const
+ {
+ return *orig_ip_cq_;
+ }
+ //@}
+
+ /** Accessor Method for obtaining the Rho penalization factor for
+ * the ell_1 norm */
+ Number Rho() const
+ {
+ return rho_;
+ }
+
+ /** @name Counters for the number of function evaluations. */
+ //@{
+ virtual Index f_evals() const
+ {
+ return f_evals_;
+ }
+ virtual Index grad_f_evals() const
+ {
+ return grad_f_evals_;
+ }
+ virtual Index c_evals() const
+ {
+ return c_evals_;
+ }
+ virtual Index jac_c_evals() const
+ {
+ return jac_c_evals_;
+ }
+ virtual Index d_evals() const
+ {
+ return d_evals_;
+ }
+ virtual Index jac_d_evals() const
+ {
+ return jac_d_evals_;
+ }
+ virtual Index h_evals() const
+ {
+ return h_evals_;
+ }
+ //@}
+
+ /** Method to calculate eta, the factor for the regularization term */
+ Number Eta(Number mu) const;
+
+ /** Method returning the scaling factors for the 2-norm
+ * penalization term. */
+ SmartPtr<const Vector> DR_x() const
+ {
+ return ConstPtr(dr_x_);
+ }
+
+ /** Methods for IpoptType */
+ //@{
+ /** Called by IpoptType to register the options */
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ private:
+ /** @name Pointers for the original NLP information. */
+ //@{
+ /** Pointer to the original IpoptNLP */
+ SmartPtr<IpoptNLP> orig_ip_nlp_;
+
+ /** Pointer to the original IpoptData */
+ SmartPtr<IpoptData> orig_ip_data_;
+
+ /** Pointer to the original IpoptCalculatedQuantities */
+ SmartPtr<IpoptCalculatedQuantities> orig_ip_cq_;
+ //@}
+
+ /** Necessary Vector/Matrix spaces */
+ //@{
+ SmartPtr<CompoundVectorSpace> x_space_;
+
+ SmartPtr<const VectorSpace> c_space_;
+
+ SmartPtr<const VectorSpace> d_space_;
+
+ SmartPtr<CompoundVectorSpace> x_l_space_;
+
+ SmartPtr<CompoundMatrixSpace> px_l_space_;
+
+ SmartPtr<const VectorSpace> x_u_space_;
+
+ SmartPtr<CompoundMatrixSpace> px_u_space_;
+
+ SmartPtr<const VectorSpace> d_l_space_;
+
+ SmartPtr<const MatrixSpace> pd_l_space_;
+
+ SmartPtr<const VectorSpace> d_u_space_;
+
+ SmartPtr<const MatrixSpace> pd_u_space_;
+
+ SmartPtr<CompoundMatrixSpace> jac_c_space_;
+
+ SmartPtr<CompoundMatrixSpace> jac_d_space_;
+
+ SmartPtr<CompoundSymMatrixSpace> h_space_;
+ //@}
+
+ /**@name Storage for Model Quantities */
+ //@{
+ /** Lower bounds on x */
+ SmartPtr<CompoundVector> x_L_;
+
+ /** Permutation matrix (x_L_ -> x) */
+ SmartPtr<CompoundMatrix> Px_L_;
+
+ /** Upper bounds on x */
+ SmartPtr<const Vector> x_U_;
+
+ /** Permutation matrix (x_U_ -> x) */
+ SmartPtr<CompoundMatrix> Px_U_;
+
+ /** Lower bounds on d */
+ SmartPtr<const Vector> d_L_;
+
+ /** Permutation matrix (d_L_ -> d) */
+ SmartPtr<const Matrix> Pd_L_;
+
+ /** Upper bounds on d */
+ SmartPtr<const Vector> d_U_;
+
+ /** Permutation matrix (d_U_ -> d */
+ SmartPtr<const Matrix> Pd_U_;
+ //@}
+
+ /** @name Values particular to the restoration phase problem statement */
+ //@{
+ /** Penalty parameter for the \$l_1\$ norm */
+ /* ToDo make this parameter? */
+ Number rho_;
+ /** scaling factor for eta calculation */
+ Number eta_factor_;
+ /** exponent for mu in eta calculation */
+ Number eta_mu_exponent_;
+ // TODO in the following we should use pointers to CONST values
+ /** Scaling factors for the \$x\$ part of the regularization term */
+ SmartPtr<Vector> dr_x_;
+ SmartPtr<DiagMatrix> DR_x_; //TODO We can get rid of one of the dr DR
+ /** \$x\$ part of the reference point in the regularization term */
+ SmartPtr<Vector> x_ref_;
+ //@}
+
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ RestoIpoptNLP();
+
+ /** Copy Constructor */
+ RestoIpoptNLP(const RestoIpoptNLP&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const RestoIpoptNLP&);
+ //@}
+
+ /** @name Algorithmic parameter */
+ //@{
+ /** Flag indicating if evalution of the objective should be
+ * performed for every restoration phase objective function
+ * evaluation. */
+ bool evaluate_orig_obj_at_resto_trial_;
+ /** Flag indicating how hessian information is obtained */
+ HessianApproximationType hessian_approximation_;
+ //@}
+
+ /** Flag indicating if initialization method has been called */
+ bool initialized_;
+
+ /** @name Counters for the function evaluations */
+ //@{
+ Index f_evals_;
+ Index grad_f_evals_;
+ Index c_evals_;
+ Index jac_c_evals_;
+ Index d_evals_;
+ Index jac_d_evals_;
+ Index h_evals_;
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpRestoIterateInitializer.cpp b/SimTKmath/Optimizers/src/IpOpt/IpRestoIterateInitializer.cpp
new file mode 100644
index 0000000..97d01c0
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpRestoIterateInitializer.cpp
@@ -0,0 +1,225 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpRestoIterateInitializer.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-10-12
+
+#include "IpRestoIterateInitializer.hpp"
+#include "IpRestoIpoptNLP.hpp"
+#include "IpDefaultIterateInitializer.hpp"
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ RestoIterateInitializer::RestoIterateInitializer
+ (const SmartPtr<EqMultiplierCalculator>& resto_eq_mult_calculator)
+ :
+ IterateInitializer(),
+ resto_eq_mult_calculator_(resto_eq_mult_calculator)
+ {}
+
+ bool RestoIterateInitializer::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ if (!options.GetNumericValue("constr_mult_init_max", constr_mult_init_max_, prefix)) {
+ // By default, we want to set this to zero. Seems to work better
+ // as initialization for the restoration phase
+ constr_mult_init_max_ = 0.;
+ }
+
+ bool retvalue = true;
+ if (IsValid(resto_eq_mult_calculator_)) {
+ retvalue = resto_eq_mult_calculator_->Initialize(Jnlst(),
+ IpNLP(), IpData(),
+ IpCq(), options, prefix);
+ }
+ return retvalue;
+ }
+
+ bool RestoIterateInitializer::SetInitialIterates()
+ {
+ DBG_START_METH("RestoIterateInitializer::SetInitialIterates",
+ dbg_verbosity);
+
+ // Get a grip on the restoration phase NLP and obtain the pointers
+ // to the original NLP data
+ SmartPtr<RestoIpoptNLP> resto_ip_nlp =
+ dynamic_cast<RestoIpoptNLP*> (&IpNLP());
+ DBG_ASSERT(IsValid(resto_ip_nlp));
+ SmartPtr<IpoptNLP> orig_ip_nlp =
+ dynamic_cast<IpoptNLP*> (&resto_ip_nlp->OrigIpNLP());
+ DBG_ASSERT(IsValid(orig_ip_nlp));
+ SmartPtr<IpoptData> orig_ip_data =
+ dynamic_cast<IpoptData*> (&resto_ip_nlp->OrigIpData());
+ DBG_ASSERT(IsValid(orig_ip_data));
+ SmartPtr<IpoptCalculatedQuantities> orig_ip_cq =
+ dynamic_cast<IpoptCalculatedQuantities*> (&resto_ip_nlp->OrigIpCq());
+ DBG_ASSERT(IsValid(orig_ip_cq));
+
+ // Set the value of the barrier parameter
+ Number resto_mu;
+ resto_mu = Max(orig_ip_data->curr_mu(),
+ orig_ip_cq->curr_c()->Amax(),
+ orig_ip_cq->curr_d_minus_s()->Amax());
+ IpData().Set_mu(resto_mu);
+ Jnlst().Printf(J_DETAILED, J_INITIALIZATION,
+ "Initial barrier parameter resto_mu = %e\n", resto_mu);
+
+ /////////////////////////////////////////////////////////////////////
+ // Initialize primal varialbes //
+ /////////////////////////////////////////////////////////////////////
+
+ // initialize the data structures in the restoration phase NLP
+ IpData().InitializeDataStructures(IpNLP(), false, false, false,
+ false, false);
+
+ SmartPtr<Vector> new_x = IpData().curr()->x()->MakeNew();
+ SmartPtr<CompoundVector> Cnew_x =
+ dynamic_cast<CompoundVector*> (GetRawPtr(new_x));
+
+ // Set the trial x variables from the original NLP
+ Cnew_x->GetCompNonConst(0)->Copy(*orig_ip_data->curr()->x());
+
+ // Compute the initial values for the n and p variables for the
+ // equality constraints
+ Number rho = resto_ip_nlp->Rho();
+ DBG_PRINT((1,"rho = %e\n", rho));
+ SmartPtr<Vector> nc = Cnew_x->GetCompNonConst(1);
+ SmartPtr<Vector> pc = Cnew_x->GetCompNonConst(2);
+ SmartPtr<const Vector> cvec = orig_ip_cq->curr_c();
+ DBG_PRINT_VECTOR(2, "cvec", *cvec);
+ SmartPtr<Vector> a = nc->MakeNew();
+ SmartPtr<Vector> b = nc->MakeNew();
+ a->Set(resto_mu/(2.*rho));
+ a->Axpy(-0.5, *cvec);
+ b->Copy(*cvec);
+ b->Scal(resto_mu/(2.*rho));
+ DBG_PRINT_VECTOR(2, "a", *a);
+ DBG_PRINT_VECTOR(2, "b", *b);
+ solve_quadratic(*a, *b, *nc);
+ pc->Copy(*cvec);
+ pc->Axpy(1., *nc);
+ DBG_PRINT_VECTOR(2, "nc", *nc);
+ DBG_PRINT_VECTOR(2, "pc", *pc);
+
+ // initial values for the n and p variables for the inequality
+ // constraints
+ SmartPtr<Vector> nd = Cnew_x->GetCompNonConst(3);
+ SmartPtr<Vector> pd = Cnew_x->GetCompNonConst(4);
+ cvec = orig_ip_cq->curr_d_minus_s();
+ a = nd->MakeNew();
+ b = nd->MakeNew();
+ a->Set(resto_mu/(2.*rho));
+ a->Axpy(-0.5, *cvec);
+ b->Copy(*cvec);
+ b->Scal(resto_mu/(2.*rho));
+ solve_quadratic(*a, *b, *nd);
+ pd->Copy(*cvec);
+ pd->Axpy(1., *nd);
+ DBG_PRINT_VECTOR(2, "nd", *nd);
+ DBG_PRINT_VECTOR(2, "pd", *pd);
+
+ // Leave the slacks unchanged
+ SmartPtr<const Vector> new_s = orig_ip_data->curr()->s();
+
+ // Now set the primal trial variables
+ DBG_PRINT_VECTOR(2,"new_s",*new_s);
+ DBG_PRINT_VECTOR(2,"new_x",*new_x);
+ SmartPtr<IteratesVector> trial = IpData().curr()->MakeNewContainer();
+ trial->Set_primal(*new_x, *new_s);
+ IpData().set_trial(trial);
+
+ DBG_PRINT_VECTOR(2, "resto_c", *IpCq().trial_c());
+ DBG_PRINT_VECTOR(2, "resto_d_minus_s", *IpCq().trial_d_minus_s());
+
+ /////////////////////////////////////////////////////////////////////
+ // Initialize bound multipliers //
+ /////////////////////////////////////////////////////////////////////
+
+ SmartPtr<Vector> new_z_L = IpData().curr()->z_L()->MakeNew();
+ SmartPtr<CompoundVector> Cnew_z_L =
+ dynamic_cast<CompoundVector*> (GetRawPtr(new_z_L));
+ DBG_ASSERT(IsValid(Cnew_z_L));
+ SmartPtr<Vector> new_z_U = IpData().curr()->z_U()->MakeNew();
+ SmartPtr<Vector> new_v_L = IpData().curr()->v_L()->MakeNew();
+ SmartPtr<Vector> new_v_U = IpData().curr()->v_U()->MakeNew();
+
+ // multipliers for the original bounds are
+ SmartPtr<const Vector> orig_z_L = orig_ip_data->curr()->z_L();
+ SmartPtr<const Vector> orig_z_U = orig_ip_data->curr()->z_U();
+ SmartPtr<const Vector> orig_v_L = orig_ip_data->curr()->v_L();
+ SmartPtr<const Vector> orig_v_U = orig_ip_data->curr()->v_U();
+
+ // Set the new multipliers to the min of the penalty parameter Rho
+ // and their current value
+ SmartPtr<Vector> Cnew_z_L0 = Cnew_z_L->GetCompNonConst(0);
+ Cnew_z_L0->Set(rho);
+ Cnew_z_L0->ElementWiseMin(*orig_z_L);
+ new_z_U->Set(rho);
+ new_z_U->ElementWiseMin(*orig_z_U);
+ new_v_L->Set(rho);
+ new_v_L->ElementWiseMin(*orig_v_L);
+ new_v_U->Set(rho);
+ new_v_U->ElementWiseMin(*orig_v_U);
+
+ // Set the multipliers for the p and n bounds to the "primal" multipliers
+ SmartPtr<Vector> Cnew_z_L1 = Cnew_z_L->GetCompNonConst(1);
+ Cnew_z_L1->Set(resto_mu);
+ Cnew_z_L1->ElementWiseDivide(*nc);
+ SmartPtr<Vector> Cnew_z_L2 = Cnew_z_L->GetCompNonConst(2);
+ Cnew_z_L2->Set(resto_mu);
+ Cnew_z_L2->ElementWiseDivide(*pc);
+ SmartPtr<Vector> Cnew_z_L3 = Cnew_z_L->GetCompNonConst(3);
+ Cnew_z_L3->Set(resto_mu);
+ Cnew_z_L3->ElementWiseDivide(*nd);
+ SmartPtr<Vector> Cnew_z_L4 = Cnew_z_L->GetCompNonConst(4);
+ Cnew_z_L4->Set(resto_mu);
+ Cnew_z_L4->ElementWiseDivide(*pd);
+
+ // Set those initial values to be the trial values in Data
+ trial = IpData().trial()->MakeNewContainer();
+ trial->Set_bound_mult(*new_z_L, *new_z_U, *new_v_L, *new_v_U);
+ IpData().set_trial(trial);
+
+ /////////////////////////////////////////////////////////////////////
+ // Initialize equality constraint multipliers //
+ /////////////////////////////////////////////////////////////////////
+
+ DefaultIterateInitializer::least_square_mults(
+ Jnlst(), IpNLP(), IpData(), IpCq(),
+ resto_eq_mult_calculator_, constr_mult_init_max_);
+
+ // upgrade the trial to the current point
+ IpData().AcceptTrialPoint();
+
+ DBG_PRINT_VECTOR(2, "y_c", *IpData().curr()->y_c());
+ DBG_PRINT_VECTOR(2, "y_d", *IpData().curr()->y_d());
+
+ DBG_PRINT_VECTOR(2, "z_L", *IpData().curr()->z_L());
+ DBG_PRINT_VECTOR(2, "z_U", *IpData().curr()->z_U());
+ DBG_PRINT_VECTOR(2, "v_L", *IpData().curr()->v_L());
+ DBG_PRINT_VECTOR(2, "v_U", *IpData().curr()->v_U());
+
+ return true;
+ }
+
+ void
+ RestoIterateInitializer::solve_quadratic(const Vector& a,
+ const Vector& b,
+ Vector& v)
+ {
+ v.Copy(a);
+ v.ElementWiseMultiply(a);
+
+ v.Axpy(1., b);
+ v.ElementWiseSqrt();
+
+ v.Axpy(1., a);
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpRestoIterateInitializer.hpp b/SimTKmath/Optimizers/src/IpOpt/IpRestoIterateInitializer.hpp
new file mode 100644
index 0000000..13d2232
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpRestoIterateInitializer.hpp
@@ -0,0 +1,92 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpRestoIterateInitializer.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-09-24
+
+#ifndef __IPRESTOITERATEINITIALIZER_HPP__
+#define __IPRESTOITERATEINITIALIZER_HPP__
+
+#include "IpIterateInitializer.hpp"
+#include "IpEqMultCalculator.hpp"
+
+namespace Ipopt
+{
+ /** Class implementing the default initialization procedure (based
+ * on user options) for the iterates. It is used at the very
+ * beginning of the optimization for determine the starting point
+ * for all variables.
+ */
+ class RestoIterateInitializer: public IterateInitializer
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor. If eq_mult_calculator is not NULL, it will be
+ * used to compute the initial values for equality constraint
+ * multipliers. */
+ RestoIterateInitializer
+ (const SmartPtr<EqMultiplierCalculator>& eq_mult_calculator);
+
+ /** Default destructor */
+ virtual ~RestoIterateInitializer()
+ {}
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Compute the initial iterates and set the into the curr field
+ * of the ip_data object. */
+ virtual bool SetInitialIterates();
+
+ /** Methods for IpoptType */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ RestoIterateInitializer();
+
+ /** Copy Constructor */
+ RestoIterateInitializer(const RestoIterateInitializer&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const RestoIterateInitializer&);
+ //@}
+
+ /**@name Parameters for bumping x0 */
+ //@{
+ /** If max-norm of the initial equality constraint multiplier
+ * estimate is larger than this, the initial y_* variables are
+ * set to zero. */
+ Number constr_mult_init_max_;
+ //@}
+
+ /** object to be used for the initialization of the equality
+ * constraint multipliers. */
+ SmartPtr<EqMultiplierCalculator> resto_eq_mult_calculator_;
+
+ /** @name Auxilliary functions */
+ //@{
+ /** Method for solving the quadratic vector equation v^2 + 2a*v -
+ b = 0 */
+ void solve_quadratic(const Vector& a, const Vector& b, Vector& v);
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpRestoIterationOutput.cpp b/SimTKmath/Optimizers/src/IpOpt/IpRestoIterationOutput.cpp
new file mode 100644
index 0000000..cae5d69
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpRestoIterationOutput.cpp
@@ -0,0 +1,272 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpRestoIterationOutput.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-09-23
+
+#include "IpRestoIterationOutput.hpp"
+#include "IpRestoIpoptNLP.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+#include <cstdio>
+
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+
+
+
+namespace Ipopt
+{
+ RestoIterationOutput::RestoIterationOutput(const SmartPtr<OrigIterationOutput>& resto_orig_iteration_output)
+ :
+ resto_orig_iteration_output_(resto_orig_iteration_output)
+ {}
+
+ RestoIterationOutput::~RestoIterationOutput()
+ {}
+
+ bool RestoIterationOutput::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ options.GetBoolValue("print_info_string", print_info_string_, prefix);
+
+ bool retval = true;
+ if (IsValid(resto_orig_iteration_output_)) {
+ retval = resto_orig_iteration_output_->Initialize(Jnlst(), IpNLP(),
+ IpData(), IpCq(),
+ options, prefix);
+ }
+ return retval;
+ }
+
+ void RestoIterationOutput::WriteOutput()
+ {
+ // Get pointers to the Original NLP objects
+ const RestoIpoptNLP* resto_ipopt_nlp =
+ dynamic_cast<const RestoIpoptNLP*>(&IpNLP());
+ DBG_ASSERT(resto_ipopt_nlp);
+
+ SmartPtr<IpoptData> orig_ip_data = &resto_ipopt_nlp->OrigIpData();
+ SmartPtr<IpoptNLP> orig_ip_nlp = &resto_ipopt_nlp->OrigIpNLP();
+ SmartPtr<IpoptCalculatedQuantities> orig_ip_cq =
+ &resto_ipopt_nlp->OrigIpCq();
+
+ // Set the iteration counter for the original NLP to the current value
+ Index iter = IpData().iter_count();
+ orig_ip_data->Set_iter_count(iter);
+
+ // If a resto_orig_iteration_output object was given, first do the
+ // WriteOutput method with that one
+ if (IsValid(resto_orig_iteration_output_)) {
+ resto_orig_iteration_output_->WriteOutput();
+ }
+
+ //////////////////////////////////////////////////////////////////////
+ // First print the summary line for the iteration //
+ //////////////////////////////////////////////////////////////////////
+
+ std::string header =
+ "iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls\n";
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "\n\n**************************************************\n");
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "*** Summary of Iteration %d for original NLP:", IpData().iter_count());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "\n**************************************************\n\n");
+ if (iter%10 == 0 && !IsValid(resto_orig_iteration_output_)) {
+ // output the header
+ Jnlst().Printf(J_ITERSUMMARY, J_MAIN, header.c_str());
+ }
+ else {
+ Jnlst().Printf(J_DETAILED, J_MAIN, header.c_str());
+ }
+
+ // For now, just print the total NLP error for the restoration
+ // phase problem in the dual infeasibility column
+ Number inf_du =
+ IpCq().curr_dual_infeasibility(NORM_MAX);
+
+ Number mu = IpData().curr_mu();
+ Number dnrm = 0.;
+ if (IsValid(IpData().delta()) && IsValid(IpData().delta()->x()) && IsValid(IpData().delta()->s())) {
+ dnrm = Max(IpData().delta()->x()->Amax(), IpData().delta()->s()->Amax());
+ }
+
+ // Set the trial values for the original Data object to the
+ // current restoration phase values
+ SmartPtr<const Vector> x = IpData().curr()->x();
+ const CompoundVector* cx =
+ dynamic_cast<const CompoundVector*>(GetRawPtr(x));
+ DBG_ASSERT(cx);
+
+ SmartPtr<IteratesVector> trial = orig_ip_data->trial()->MakeNewContainer();
+ trial->Set_x(*cx->GetComp(0));
+ trial->Set_s(*IpData().curr()->s());
+ orig_ip_data->set_trial(trial);
+
+ // Compute primal infeasibility
+ Number inf_pr = orig_ip_cq->trial_primal_infeasibility(NORM_MAX);
+ Number f = orig_ip_cq->unscaled_trial_f();
+
+ // Retrieve some information set in the different parts of the algorithm
+ char info_iter='r';
+
+ Number alpha_primal = IpData().info_alpha_primal();
+ char alpha_primal_char = IpData().info_alpha_primal_char();
+ Number alpha_dual = IpData().info_alpha_dual();
+ Number regu_x = IpData().info_regu_x();
+ char regu_x_buf[8];
+ char dashes[]=" - ";
+ char *regu_x_ptr;
+ if (regu_x==.0) {
+ regu_x_ptr = dashes;
+ }
+ else {
+ sprintf(regu_x_buf, "%5.1f", log10(regu_x));
+ regu_x_ptr = regu_x_buf;
+ }
+ Index ls_count = IpData().info_ls_count();
+ const std::string info_string = IpData().info_string();
+
+ Jnlst().Printf(J_ITERSUMMARY, J_MAIN,
+ "%4d%c%14.7e %7.2e %7.2e %5.1f %7.2e %5s %7.2e %7.2e%c%3d",
+ iter, info_iter, f, inf_pr, inf_du, log10(mu), dnrm, regu_x_ptr,
+ alpha_dual, alpha_primal, alpha_primal_char,
+ ls_count);
+ if (print_info_string_) {
+ Jnlst().Printf(J_ITERSUMMARY, J_MAIN, " %s", info_string.c_str());
+ }
+ else {
+ Jnlst().Printf(J_DETAILED, J_MAIN, " %s", info_string.c_str());
+ }
+ Jnlst().Printf(J_ITERSUMMARY, J_MAIN, "\n");
+
+ //////////////////////////////////////////////////////////////////////
+ // Now if desired more detail on the iterates //
+ //////////////////////////////////////////////////////////////////////
+
+ if (Jnlst().ProduceOutput(J_DETAILED, J_MAIN)) {
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "\n**************************************************\n");
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "*** Beginning Iteration %d from the following point:",
+ IpData().iter_count());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "\n**************************************************\n\n");
+
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "Primal infeasibility for restoration phase problem = %.16e\n",
+ IpCq().curr_primal_infeasibility(NORM_MAX));
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "Dual infeasibility for restoration phase problem = %.16e\n",
+ IpCq().curr_dual_infeasibility(NORM_MAX));
+
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "||curr_x||_inf = %.16e\n", IpData().curr()->x()->Amax());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "||curr_s||_inf = %.16e\n", IpData().curr()->s()->Amax());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "||curr_y_c||_inf = %.16e\n", IpData().curr()->y_c()->Amax());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "||curr_y_d||_inf = %.16e\n", IpData().curr()->y_d()->Amax());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "||curr_z_L||_inf = %.16e\n", IpData().curr()->z_L()->Amax());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "||curr_z_U||_inf = %.16e\n", IpData().curr()->z_U()->Amax());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "||curr_v_L||_inf = %.16e\n", IpData().curr()->v_L()->Amax());
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "||curr_v_U||_inf = %.16e\n", IpData().curr()->v_U()->Amax());
+ }
+ if (Jnlst().ProduceOutput(J_MOREDETAILED, J_MAIN)) {
+ if (IsValid(IpData().delta())) {
+ Jnlst().Printf(J_MOREDETAILED, J_MAIN,
+ "\n||delta_x||_inf = %.16e\n", IpData().delta()->x()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_MAIN,
+ "||delta_s||_inf = %.16e\n", IpData().delta()->s()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_MAIN,
+ "||delta_y_c||_inf = %.16e\n", IpData().delta()->y_c()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_MAIN,
+ "||delta_y_d||_inf = %.16e\n", IpData().delta()->y_d()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_MAIN,
+ "||delta_z_L||_inf = %.16e\n", IpData().delta()->z_L()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_MAIN,
+ "||delta_z_U||_inf = %.16e\n", IpData().delta()->z_U()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_MAIN,
+ "||delta_v_L||_inf = %.16e\n", IpData().delta()->v_L()->Amax());
+ Jnlst().Printf(J_MOREDETAILED, J_MAIN,
+ "||delta_v_U||_inf = %.16e\n", IpData().delta()->v_U()->Amax());
+ }
+ else {
+ Jnlst().Printf(J_MOREDETAILED, J_MAIN,
+ "\nNo search direction has been computed yet.\n");
+ }
+ }
+ if (Jnlst().ProduceOutput(J_VECTOR, J_MAIN)) {
+ IpData().curr()->x()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_x");
+ IpData().curr()->s()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_s");
+
+ IpData().curr()->y_c()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_y_c");
+ IpData().curr()->y_d()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_y_d");
+
+ IpCq().curr_slack_x_L()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_slack_x_L");
+ IpCq().curr_slack_x_U()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_slack_x_U");
+ IpData().curr()->z_L()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_z_L");
+ IpData().curr()->z_U()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_z_U");
+
+ IpCq().curr_slack_s_L()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_slack_s_L");
+ IpCq().curr_slack_s_U()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_slack_s_U");
+ IpData().curr()->v_L()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_v_L");
+ IpData().curr()->v_U()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_v_U");
+ }
+ if (Jnlst().ProduceOutput(J_MOREVECTOR, J_MAIN)) {
+ IpCq().curr_grad_lag_x()->Print(Jnlst(), J_MOREVECTOR, J_MAIN, "curr_grad_lag_x");
+ IpCq().curr_grad_lag_s()->Print(Jnlst(), J_MOREVECTOR, J_MAIN, "curr_grad_lag_s");
+ if (IsValid(IpData().delta())) {
+ IpData().delta()->Print(Jnlst(), J_MOREVECTOR, J_MAIN, "delta");
+ }
+ }
+
+ if (Jnlst().ProduceOutput(J_DETAILED, J_MAIN)) {
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "\n\n***Current NLP Values for Iteration (Restoration phase problem) %d:\n",
+ IpData().iter_count());
+ Jnlst().Printf(J_DETAILED, J_MAIN, "\n (scaled) (unscaled)\n");
+ Jnlst().Printf(J_DETAILED, J_MAIN, "Objective...............: %24.16e %24.16e\n", IpCq().curr_f(), IpCq().unscaled_curr_f());
+ Jnlst().Printf(J_DETAILED, J_MAIN, "Dual infeasibility......: %24.16e %24.16e\n", IpCq().curr_dual_infeasibility(NORM_MAX), IpCq().unscaled_curr_dual_infeasibility(NORM_MAX));
+ Jnlst().Printf(J_DETAILED, J_MAIN, "Constraint violation....: %24.16e %24.16e\n", IpCq().curr_nlp_constraint_violation(NORM_MAX), IpCq().unscaled_curr_nlp_constraint_violation(NORM_MAX));
+ Jnlst().Printf(J_DETAILED, J_MAIN, "Complementarity.........: %24.16e %24.16e\n", IpCq().curr_complementarity(0., NORM_MAX), IpCq().unscaled_curr_complementarity(0., NORM_MAX));
+ Jnlst().Printf(J_DETAILED, J_MAIN, "Overall NLP error.......: %24.16e %24.16e\n\n", IpCq().curr_nlp_error(), IpCq().unscaled_curr_nlp_error());
+ }
+ if (Jnlst().ProduceOutput(J_VECTOR, J_MAIN)) {
+ IpCq().curr_grad_f()->Print(Jnlst(), J_VECTOR, J_MAIN, "grad_f");
+ IpCq().curr_c()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_c");
+ IpCq().curr_d()->Print(Jnlst(), J_VECTOR, J_MAIN, "curr_d");
+ IpCq().curr_d_minus_s()->Print(Jnlst(), J_VECTOR, J_MAIN,
+ "curr_d - curr_s");
+ }
+
+ if (Jnlst().ProduceOutput(J_MATRIX, J_MAIN)) {
+ IpCq().curr_jac_c()->Print(Jnlst(), J_MATRIX, J_MAIN, "jac_c");
+ IpCq().curr_jac_d()->Print(Jnlst(), J_MATRIX, J_MAIN, "jac_d");
+ IpData().W()->Print(Jnlst(), J_MATRIX, J_MAIN, "W");
+ }
+
+ Jnlst().Printf(J_DETAILED, J_MAIN, "\n\n");
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpRestoIterationOutput.hpp b/SimTKmath/Optimizers/src/IpOpt/IpRestoIterationOutput.hpp
new file mode 100644
index 0000000..464641c
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpRestoIterationOutput.hpp
@@ -0,0 +1,73 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpRestoIterationOutput.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-09-27
+
+#ifndef __IPRESTOITERATIONOUTPUT_HPP__
+#define __IPRESTOITERATIONOUTPUT_HPP__
+
+#include "IpIterationOutput.hpp"
+#include "IpOrigIterationOutput.hpp"
+
+namespace Ipopt
+{
+
+ /** Class for the iteration summary output for the restoration
+ * phase. This prints information for the ORIGINAL NLP (and
+ * possibly for the restoration phase NLP.
+ */
+ class RestoIterationOutput: public IterationOutput
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor. If resto_orig_iteration_output is not NULL, the
+ * output will be done twice per iteration, first for the
+ * restoration phase problem, and secondyl using the functions
+ * for the original NLP. */
+ RestoIterationOutput(const SmartPtr<OrigIterationOutput>& resto_orig_iteration_output);
+
+ /** Default destructor */
+ virtual ~RestoIterationOutput();
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Method to do all the summary output per iteration. This
+ * include the one-line summary output as well as writing the
+ * details about the iterates if desired */
+ virtual void WriteOutput();
+
+ private:
+ /**@name Default Compiler Generated Methods (Hidden to avoid
+ * implicit creation/calling). These methods are not implemented
+ * and we do not want the compiler to implement them for us, so we
+ * declare them private and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ RestoIterationOutput();
+
+ /** Copy Constructor */
+ RestoIterationOutput(const RestoIterationOutput&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const RestoIterationOutput&);
+ //@}
+
+ /** Pointer to output strategy object during regular iterations. */
+ SmartPtr<OrigIterationOutput> resto_orig_iteration_output_;
+
+ /** Flag indicating weather info string should be printed at end
+ * of iteration summary line. */
+ bool print_info_string_;
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpRestoMinC_1Nrm.cpp b/SimTKmath/Optimizers/src/IpOpt/IpRestoMinC_1Nrm.cpp
new file mode 100644
index 0000000..38c3278
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpRestoMinC_1Nrm.cpp
@@ -0,0 +1,320 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpRestoMinC_1Nrm.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpRestoMinC_1Nrm.hpp"
+#include "IpCompoundVector.hpp"
+#include "IpRestoIpoptNLP.hpp"
+#include "IpDefaultIterateInitializer.hpp"
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ MinC_1NrmRestorationPhase::MinC_1NrmRestorationPhase
+ (IpoptAlgorithm& resto_alg,
+ const SmartPtr<EqMultiplierCalculator>& eq_mult_calculator)
+ :
+ resto_alg_(&resto_alg),
+ eq_mult_calculator_(eq_mult_calculator),
+ resto_options_(NULL)
+ {
+ DBG_ASSERT(IsValid(resto_alg_));
+ }
+
+ MinC_1NrmRestorationPhase::~MinC_1NrmRestorationPhase()
+ {}
+
+ void MinC_1NrmRestorationPhase::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->AddLowerBoundedNumberOption(
+ "bound_mult_reset_threshold",
+ "Threshold for resetting bound multipliers after the restoration phase.",
+ 0.0, false,
+ 1e3,
+ "After returning from the restoration phase, the bound multipliers are "
+ "updated with a Newton step for complementarity. Here, the "
+ "change in the primal variables during the entire restoration "
+ "phase is taken to be the corresponding primal Newton step. "
+ "However, if after the update the largest bound multiplier "
+ "exceeds the threshold specified by this option, the multipliers "
+ "are all reset to 1.");
+ roptions->AddLowerBoundedNumberOption(
+ "constr_mult_reset_threshold",
+ "Threshold for resetting equality and inequality multipliers after restoration phase.",
+ 0.0, false,
+ 0e3,
+ "After returning from the restoration phase, the constraint multipliers "
+ "are recomputed by a least square estimate. This option triggers when "
+ "those least-square estimates should be ignored.");
+ }
+
+ bool MinC_1NrmRestorationPhase::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ // keep a copy of these options to use when setting up the
+ // restoration phase
+ resto_options_ = new OptionsList(options);
+
+ options.GetNumericValue("constr_mult_reset_threshold",
+ constr_mult_reset_threshold_,
+ prefix);
+ options.GetNumericValue("bound_mult_reset_threshold",
+ bound_mult_reset_threshold_,
+ prefix);
+ options.GetBoolValue("expect_infeasible_problem",
+ expect_infeasible_problem_,
+ prefix);
+
+ // Avoid that the restoration phase is trigged by user option in
+ // first iteration of the restoration phase
+ resto_options_->SetStringValue("resto.start_with_resto", "no");
+
+ // We want the default for the theta_max_fact in the restoration
+ // phase higher than for the regular phase
+ Number theta_max_fact;
+ if (!options.GetNumericValue("resto.theta_max_fact",
+ theta_max_fact, "")) {
+ resto_options_->SetNumericValue("resto.theta_max_fact", 1e8);
+ }
+
+ count_restorations_ = 0;
+
+ bool retvalue = true;
+ if (IsValid(eq_mult_calculator_)) {
+ retvalue = eq_mult_calculator_->Initialize(Jnlst(), IpNLP(), IpData(),
+ IpCq(), options, prefix);
+ }
+ return retvalue;
+ }
+
+ bool
+ MinC_1NrmRestorationPhase::PerformRestoration()
+ {
+ DBG_START_METH("MinC_1NrmRestorationPhase::PerformRestoration",
+ dbg_verbosity);
+
+ // Increase counter for restoration phase calls
+ count_restorations_++;
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "Starting Restoration Phase for the %d. time\n",
+ count_restorations_);
+
+ DBG_ASSERT(IpCq().curr_constraint_violation()>0.);
+
+ // ToDo set those up during initialize?
+ // Create the restoration phase NLP etc objects
+ SmartPtr<IpoptData> resto_ip_data = new IpoptData();
+ SmartPtr<IpoptNLP> resto_ip_nlp =
+ new RestoIpoptNLP(IpNLP(), IpData(), IpCq());
+ SmartPtr<IpoptCalculatedQuantities> resto_ip_cq =
+ new IpoptCalculatedQuantities(resto_ip_nlp, resto_ip_data);
+
+ // Determine if this is a square problem
+ bool square_problem = IpCq().IsSquareProblem();
+
+ // Decide if we want to use the original option or want to make
+ // some changes
+ SmartPtr<OptionsList> actual_resto_options = resto_options_;
+ if(square_problem) {
+ actual_resto_options = new OptionsList(*resto_options_);
+ // If this is a square problem, the want the restoration phase
+ // never to be left until the problem is converged
+ actual_resto_options->SetNumericValue("resto.required_infeasibility_reduction", 0.);
+ }
+ else if (expect_infeasible_problem_ && count_restorations_==1) {
+ if (IpCq().curr_constraint_violation()>1e-3) {
+ actual_resto_options = new OptionsList(*resto_options_);
+ // Ask for significant reduction of infeasibility, in the hope
+ // that we do not return from the restoration phase is the
+ // problem is infeasible
+ actual_resto_options->SetNumericValue("resto.required_infeasibility_reduction", 1e-3);
+ }
+ }
+
+ // Initialize the restoration phase algorithm
+ resto_alg_->Initialize(Jnlst(), *resto_ip_nlp, *resto_ip_data,
+ *resto_ip_cq, *actual_resto_options, "resto.");
+
+ // Set iteration counter and info field for the restoration phase
+ resto_ip_data->Set_iter_count(IpData().iter_count()+1);
+ resto_ip_data->Set_info_regu_x(IpData().info_regu_x());
+ resto_ip_data->Set_info_alpha_primal(IpData().info_alpha_primal());
+ resto_ip_data->Set_info_alpha_primal_char(IpData().info_alpha_primal_char());
+ resto_ip_data->Set_info_alpha_dual(IpData().info_alpha_dual());
+ resto_ip_data->Set_info_ls_count(IpData().info_ls_count());
+
+ // Call the optimization algorithm to solve the restoration phase
+ // problem
+ SolverReturn resto_status = resto_alg_->Optimize();
+
+ int retval=-1;
+
+ if (resto_status == SUCCESS) {
+ if (Jnlst().ProduceOutput(J_DETAILED, J_LINE_SEARCH)) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "\nRESTORATION PHASE RESULTS\n");
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "\n\nOptimal solution found! \n");
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Optimal Objective Value = %.16E\n", resto_ip_cq->curr_f());
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Number of Iterations = %d\n", resto_ip_data->iter_count());
+ }
+ if (Jnlst().ProduceOutput(J_VECTOR, J_LINE_SEARCH)) {
+ resto_ip_data->curr()->Print(Jnlst(), J_VECTOR, J_LINE_SEARCH, "curr");
+ }
+
+ retval = 0;
+ }
+ else if (resto_status == STOP_AT_TINY_STEP ||
+ resto_status == STOP_AT_ACCEPTABLE_POINT) {
+ Number orig_primal_inf =
+ IpCq().curr_primal_infeasibility(NORM_MAX);
+ // ToDo make the factor in following line an option
+ if (orig_primal_inf <= 1e2*IpData().tol()) {
+ Jnlst().Printf(J_WARNING, J_LINE_SEARCH,
+ "Restoration phase converged to a point with small primal infeasibility.\n");
+ THROW_EXCEPTION(RESTORATION_CONVERGED_TO_FEASIBLE_POINT,
+ "Restoration phase converged to a point with small primal infeasibility");
+ }
+ else {
+ THROW_EXCEPTION(LOCALLY_INFEASIBLE,
+ "Restoration phase converged to a point of local infeasibility");
+ }
+ }
+ else if (resto_status == MAXITER_EXCEEDED) {
+ THROW_EXCEPTION(RESTORATION_MAXITER_EXCEEDED,
+ "Maximal number of iterations exceeded in restoration phase.");
+ }
+ else if (resto_status == LOCAL_INFEASIBILITY) {
+ // converged to locally infeasible point - pass this on to the outer algorithm...
+ THROW_EXCEPTION(LOCALLY_INFEASIBLE, "Restoration phase converged to a point of local infeasibility");
+ }
+ else if (resto_status == RESTORATION_FAILURE) {
+ Jnlst().Printf(J_WARNING, J_LINE_SEARCH,
+ "Restoration phase in the restoration phase failed.\n");
+ THROW_EXCEPTION(RESTORATION_FAILED, "Restoration phase in the restoration phase failed.");
+ }
+ else if (resto_status == USER_REQUESTED_STOP) {
+ // Use requested stop during restoration phase - rethrow exception
+ THROW_EXCEPTION(RESTORATION_USER_STOP, "User requested stop during restoration phase");
+ }
+ else {
+ Jnlst().Printf(J_ERROR, J_MAIN, "Sorry, things failed ?!?!\n");
+ retval = 1;
+ }
+
+ if (retval == 0) {
+ // Copy the results into the trial fields;. They will be
+ // accepted later in the full algorithm
+ SmartPtr<const CompoundVector> cx =
+ dynamic_cast<const CompoundVector*>(GetRawPtr(resto_ip_data->curr()->x()));
+ DBG_ASSERT(IsValid(cx));
+ SmartPtr<IteratesVector> trial = IpData().trial()->MakeNewContainer();
+ trial->Set_primal(*cx->GetComp(0), *resto_ip_data->curr()->s());
+ IpData().set_trial(trial);
+
+ // If this is a square problem, we are done because a
+ // sufficiently feasible point has been found
+ if (square_problem) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Recursive restoration phase algorithm termined successfully for square problem.\n");
+ IpData().AcceptTrialPoint();
+ THROW_EXCEPTION(FEASIBILITY_PROBLEM_SOLVED, "Restoration phase converged to sufficiently feasible point of original square problem.");
+ }
+
+ // Update the bound multiplers, pretending that the entire
+ // progress in x and s in the restoration phase has been one
+ // [rimal-dual Newton step (and therefore the result of solving
+ // an augmented system)
+ SmartPtr<IteratesVector> delta = IpData().curr()->MakeNewIteratesVector(true);
+ delta->Set(0.0);
+ ComputeBoundMultiplierStep(*delta->z_L_NonConst(), *IpData().curr()->z_L(),
+ *IpCq().curr_slack_x_L(),
+ *IpCq().trial_slack_x_L());
+ ComputeBoundMultiplierStep(*delta->z_U_NonConst(), *IpData().curr()->z_U(),
+ *IpCq().curr_slack_x_U(),
+ *IpCq().trial_slack_x_U());
+ ComputeBoundMultiplierStep(*delta->v_L_NonConst(), *IpData().curr()->v_L(),
+ *IpCq().curr_slack_s_L(),
+ *IpCq().trial_slack_s_L());
+ ComputeBoundMultiplierStep(*delta->v_U_NonConst(), *IpData().curr()->v_U(),
+ *IpCq().curr_slack_s_U(),
+ *IpCq().trial_slack_s_U());
+
+ DBG_PRINT_VECTOR(1, "delta_z_L", *delta->z_L());
+ DBG_PRINT_VECTOR(1, "delta_z_U", *delta->z_U());
+ DBG_PRINT_VECTOR(1, "delta_v_L", *delta->v_L());
+ DBG_PRINT_VECTOR(1, "delta_v_U", *delta->v_U());
+
+ Number alpha_dual = IpCq().dual_frac_to_the_bound(IpData().curr_tau(),
+ *delta->z_L_NonConst(),
+ *delta->z_U_NonConst(),
+ *delta->v_L_NonConst(),
+ *delta->v_U_NonConst());
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Step size for bound multipliers: %8.2e\n", alpha_dual);
+
+ IpData().SetTrialBoundMultipliersFromStep(alpha_dual, *delta->z_L(), *delta->z_U(), *delta->v_L(), *delta->v_U() );
+
+ // ToDo: Check what to do here:
+ Number bound_mult_max = Max(IpData().trial()->z_L()->Amax(),
+ IpData().trial()->z_U()->Amax(),
+ IpData().trial()->v_L()->Amax(),
+ IpData().trial()->v_U()->Amax());
+ if (bound_mult_max > bound_mult_reset_threshold_) {
+ trial = IpData().trial()->MakeNewContainer();
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Bound multipliers after restoration phase too large (max=%8.2e). Set all to 1.\n",
+ bound_mult_max);
+ trial->create_new_z_L();
+ trial->create_new_z_U();
+ trial->create_new_v_L();
+ trial->create_new_v_U();
+ trial->z_L_NonConst()->Set(1.0);
+ trial->z_U_NonConst()->Set(1.0);
+ trial->v_L_NonConst()->Set(1.0);
+ trial->v_U_NonConst()->Set(1.0);
+ IpData().set_trial(trial);
+
+ }
+
+ DefaultIterateInitializer::least_square_mults(
+ Jnlst(), IpNLP(), IpData(), IpCq(),
+ eq_mult_calculator_, constr_mult_reset_threshold_);
+
+ DBG_PRINT_VECTOR(2, "y_c", *IpData().curr()->y_c());
+ DBG_PRINT_VECTOR(2, "y_d", *IpData().curr()->y_d());
+
+ IpData().Set_iter_count(resto_ip_data->iter_count()-1);
+ // Skip the next line, because it would just replicate the first
+ // on during the restoration phase.
+ IpData().Set_info_skip_output(true);
+ }
+
+ return (retval == 0);
+ }
+
+ void MinC_1NrmRestorationPhase::ComputeBoundMultiplierStep(Vector& delta_z,
+ const Vector& curr_z,
+ const Vector& curr_slack,
+ const Vector& trial_slack)
+ {
+ Number mu = IpData().curr_mu();
+
+ delta_z.Copy(curr_slack);
+ delta_z.Axpy(-1., trial_slack);
+ delta_z.ElementWiseMultiply(curr_z);
+ delta_z.AddScalar(mu);
+ delta_z.ElementWiseDivide(curr_slack);
+ delta_z.Axpy(-1., curr_z);
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpRestoMinC_1Nrm.hpp b/SimTKmath/Optimizers/src/IpOpt/IpRestoMinC_1Nrm.hpp
new file mode 100644
index 0000000..b938e74
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpRestoMinC_1Nrm.hpp
@@ -0,0 +1,112 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpRestoMinC_1Nrm.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPRESTOC_1NRM_HPP__
+#define __IPRESTOC_1NRM_HPP__
+
+#include "IpRestoPhase.hpp"
+#include "IpIpoptAlg.hpp"
+#include "IpEqMultCalculator.hpp"
+
+namespace Ipopt
+{
+
+ /** Restoration Phase that minimizes the 1-norm of the constraint
+ * violation - using the interior point method (Ipopt).
+ */
+ class MinC_1NrmRestorationPhase : public RestorationPhase
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor, taking strategy objects. The resto_alg strategy
+ * object is the restoration phase Ipopt algorithm. The
+ * eq_mult_calculator is used to reinitialize the equality
+ * constraint multipliers after the restoration phase algorithm
+ * has finished - unless it is NULL, in which case the
+ * multipliers are set to 0. */
+ MinC_1NrmRestorationPhase(IpoptAlgorithm& resto_alg,
+ const SmartPtr<EqMultiplierCalculator>& eq_mult_calculator);
+
+ /** Default destructor */
+ virtual ~MinC_1NrmRestorationPhase();
+ //@}
+
+ /** Overloaded from AlgorithmStrategy case class */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Methods for IpoptType */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ protected:
+ /** Overloaded method from RestorationPhase. */
+ virtual bool PerformRestoration();
+
+ private:
+ /**@name Default Compiler Generated Methods (Hidden to avoid
+ * implicit creation/calling). These methods are not implemented
+ * and we do not want the compiler to implement them for us, so we
+ * declare them private and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ MinC_1NrmRestorationPhase();
+
+ /** Copy Constructor */
+ MinC_1NrmRestorationPhase(const MinC_1NrmRestorationPhase&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const MinC_1NrmRestorationPhase&);
+ //@}
+
+ /** @name Strategy objects */
+ //@{
+ SmartPtr<IpoptAlgorithm> resto_alg_;
+ SmartPtr<EqMultiplierCalculator> eq_mult_calculator_;
+ //@}
+
+ /** Copy of original options, which is required to initialize the
+ * Ipopt algorithm strategy object before restoration phase is
+ * started. */
+ SmartPtr<OptionsList> resto_options_;
+
+ /** @name Algorithmic parameters */
+ //@{
+ Number constr_mult_reset_threshold_;
+ /** Maximal allowed value of a bound multiplier after restoration
+ * phase.
+ */
+ Number bound_mult_reset_threshold_;
+ /** Indicates whether problem can be expected to be infeasible.
+ * This will request the to set kappa_resto to a small value for
+ * the first time the restoration phase is called. (ToDo) */
+ bool expect_infeasible_problem_;
+ //@}
+
+ /** Counter for the number of time that PerformRestoration is
+ * called. */
+ Index count_restorations_;
+
+ /** @name Auxilliary methods */
+ //@{
+ /** Method for computing "primal-dual" step in bound multipliers,
+ * given step in slacks.
+ */
+ void ComputeBoundMultiplierStep(Vector& delta_z,
+ const Vector& curr_z,
+ const Vector& curr_slack,
+ const Vector& trial_slack);
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpRestoPhase.hpp b/SimTKmath/Optimizers/src/IpOpt/IpRestoPhase.hpp
new file mode 100644
index 0000000..33eaa75
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpRestoPhase.hpp
@@ -0,0 +1,71 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpRestoPhase.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPRESTOPHASE_HPP__
+#define __IPRESTOPHASE_HPP__
+
+#include "IpAlgStrategy.hpp"
+#include "IpIpoptNLP.hpp"
+#include "IpIpoptData.hpp"
+#include "IpIpoptCalculatedQuantities.hpp"
+
+namespace Ipopt
+{
+
+ /** @name Exceptions */
+ //@{
+ /** Exception RESTORATION_FAILED for all trouble with the
+ * restoration phase.
+ */
+ DECLARE_STD_EXCEPTION(RESTORATION_CONVERGED_TO_FEASIBLE_POINT);
+ DECLARE_STD_EXCEPTION(RESTORATION_FAILED);
+ DECLARE_STD_EXCEPTION(RESTORATION_MAXITER_EXCEEDED);
+ DECLARE_STD_EXCEPTION(RESTORATION_USER_STOP);
+ //@}
+
+ /** Base class for different restoration phases. The restoration
+ * phase is part of the FilterLineSearch. */
+ class RestorationPhase : public AlgorithmStrategyObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor */
+ RestorationPhase()
+ {}
+ /** Default Destructor */
+ virtual ~RestorationPhase()
+ {}
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix) = 0;
+
+ /** Method called to perform restoration for the filter line
+ * search method. */
+ virtual bool PerformRestoration() = 0;
+
+ private:
+ /**@name Default Compiler Generated Methods (Hidden to avoid
+ * implicit creation/calling). These methods are not implemented
+ * and we do not want the compiler to implement them for us, so we
+ * declare them private and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ RestorationPhase(const RestorationPhase&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const RestorationPhase&);
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpRestoRestoPhase.cpp b/SimTKmath/Optimizers/src/IpOpt/IpRestoRestoPhase.cpp
new file mode 100644
index 0000000..2a22f3d
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpRestoRestoPhase.cpp
@@ -0,0 +1,124 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpRestoRestoPhase.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-02-11
+
+#include "IpRestoRestoPhase.hpp"
+#include "IpRestoIpoptNLP.hpp"
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ RestoRestorationPhase::RestoRestorationPhase()
+ {}
+
+ RestoRestorationPhase::~RestoRestorationPhase()
+ {}
+
+ bool RestoRestorationPhase::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ return true;
+ }
+
+ bool
+ RestoRestorationPhase::PerformRestoration()
+ {
+ DBG_START_METH("RestoRestorationPhase::PerformRestoration",
+ dbg_verbosity);
+ Jnlst().Printf(J_DETAILED, J_MAIN,
+ "Performing second level restoration phase for current constriant violation %8.2e\n", IpCq().curr_constraint_violation());
+
+ DBG_ASSERT(IpCq().curr_constraint_violation()>0.);
+
+ // Get a grip on the restoration phase NLP and obtain the pointers
+ // to the original NLP data
+ SmartPtr<RestoIpoptNLP> resto_ip_nlp =
+ dynamic_cast<RestoIpoptNLP*> (&IpNLP());
+ DBG_ASSERT(IsValid(resto_ip_nlp));
+ SmartPtr<IpoptNLP> orig_ip_nlp =
+ dynamic_cast<IpoptNLP*> (&resto_ip_nlp->OrigIpNLP());
+ DBG_ASSERT(IsValid(orig_ip_nlp));
+
+ // Get the current point and create a new vector for the result
+ SmartPtr<const CompoundVector> Ccurr_x =
+ dynamic_cast<const CompoundVector*> (GetRawPtr(IpData().curr()->x()));
+ SmartPtr<Vector> new_x = IpData().curr()->x()->MakeNew();
+ SmartPtr<CompoundVector> Cnew_x =
+ dynamic_cast<CompoundVector*> (GetRawPtr(new_x));
+
+ // The x values remain unchanged
+ SmartPtr<Vector> x = Cnew_x->GetCompNonConst(0);
+ x->Copy(*Ccurr_x->GetComp(0));
+
+ // ToDo in free mu mode - what to do here?
+ Number mu = IpData().curr_mu();
+
+ // Compute the initial values for the n and p variables for the
+ // equality constraints
+ Number rho = resto_ip_nlp->Rho();
+ SmartPtr<Vector> nc = Cnew_x->GetCompNonConst(1);
+ SmartPtr<Vector> pc = Cnew_x->GetCompNonConst(2);
+ SmartPtr<const Vector> cvec = orig_ip_nlp->c(*Ccurr_x->GetComp(0));
+ SmartPtr<Vector> a = nc->MakeNew();
+ SmartPtr<Vector> b = nc->MakeNew();
+ a->Set(mu/(2.*rho));
+ a->Axpy(-0.5, *cvec);
+ b->Copy(*cvec);
+ b->Scal(mu/(2.*rho));
+ solve_quadratic(*a, *b, *nc);
+ pc->Copy(*cvec);
+ pc->Axpy(1., *nc);
+ DBG_PRINT_VECTOR(2, "nc", *nc);
+ DBG_PRINT_VECTOR(2, "pc", *pc);
+
+ // initial values for the n and p variables for the inequality
+ // constraints
+ SmartPtr<Vector> nd = Cnew_x->GetCompNonConst(3);
+ SmartPtr<Vector> pd = Cnew_x->GetCompNonConst(4);
+ SmartPtr<Vector> dvec = pd->MakeNew();
+ dvec->Copy(*orig_ip_nlp->d(*Ccurr_x->GetComp(0)));
+ dvec->Axpy(-1., *IpData().curr()->s());
+ a = nd->MakeNew();
+ b = nd->MakeNew();
+ a->Set(mu/(2.*rho));
+ a->Axpy(-0.5, *dvec);
+ b->Copy(*dvec);
+ b->Scal(mu/(2.*rho));
+ solve_quadratic(*a, *b, *nd);
+ pd->Copy(*dvec);
+ pd->Axpy(1., *nd);
+ DBG_PRINT_VECTOR(2, "nd", *nd);
+ DBG_PRINT_VECTOR(2, "pd", *pd);
+
+ // Now set the trial point to the solution of the restoration phase
+ // s and all multipliers remain unchanged
+ SmartPtr<IteratesVector> new_trial = IpData().curr()->MakeNewContainer();
+ new_trial->Set_x(*new_x);
+ IpData().set_trial(new_trial);
+
+ IpData().Append_info_string("R");
+
+ return true;
+ }
+
+ void RestoRestorationPhase::solve_quadratic(const Vector& a,
+ const Vector& b,
+ Vector& v)
+ {
+ v.Copy(a);
+ v.ElementWiseMultiply(a);
+
+ v.Axpy(1., b);
+ v.ElementWiseSqrt();
+
+ v.Axpy(1., a);
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpRestoRestoPhase.hpp b/SimTKmath/Optimizers/src/IpOpt/IpRestoRestoPhase.hpp
new file mode 100644
index 0000000..a0c1b9a
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpRestoRestoPhase.hpp
@@ -0,0 +1,69 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpRestoRestoPhase.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-02-11
+
+#ifndef __IPRESTORESTOPHASE_HPP__
+#define __IPRESTORESTOPHASE_HPP__
+
+#include "IpRestoPhase.hpp"
+#include "IpIpoptAlg.hpp"
+#include "IpEqMultCalculator.hpp"
+
+namespace Ipopt
+{
+
+ /** Recursive Restoration Phase for the.MinC_1NrmRestorationPhase.
+ * This procedure chooses the n and p variables in the
+ * MinC_1NrmRestorationPhase problem formulation by treating the
+ * problem as separable (assuming that the x and s variables are
+ * fixed).
+ */
+ class RestoRestorationPhase : public RestorationPhase
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default Constructor. */
+ RestoRestorationPhase();
+
+ /** Default destructor */
+ virtual ~RestoRestorationPhase();
+ //@}
+
+ /** Overloaded from AlgorithmStrategy case class */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ protected:
+ /** Overloaded method from RestorationPhase. */
+ virtual bool PerformRestoration();
+
+ private:
+ /**@name Default Compiler Generated Methods (Hidden to avoid
+ * implicit creation/calling). These methods are not implemented
+ * and we do not want the compiler to implement them for us, so we
+ * declare them private and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ RestoRestorationPhase(const RestoRestorationPhase&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const RestoRestorationPhase&);
+ //@}
+
+ /** @name Auxilliary methods */
+ //@{
+ /** Method for solving the quadratic vector equation v^2 + 2a*v -
+ b = 0 */
+ void solve_quadratic(const Vector& a, const Vector& b, Vector& v);
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpReturnCodes.h b/SimTKmath/Optimizers/src/IpOpt/IpReturnCodes.h
new file mode 100644
index 0000000..5a3c669
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpReturnCodes.h
@@ -0,0 +1,18 @@
+/***********************************************************************
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpReturnCodes.h 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+************************************************************************/
+
+#ifndef __IPRETURNCODES_H__
+#define __IPRETURNCODES_H__
+
+/* include from a common include file */
+
+#include "IpReturnCodes_inc.h"
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpReturnCodes.hpp b/SimTKmath/Optimizers/src/IpOpt/IpReturnCodes.hpp
new file mode 100644
index 0000000..329b284
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpReturnCodes.hpp
@@ -0,0 +1,21 @@
+/***********************************************************************
+// Copyright (C) 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpReturnCodes.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2006-03-01
+************************************************************************/
+
+#ifndef __IPRETURNCODES_HPP__
+#define __IPRETURNCODES_HPP__
+
+/* include from a common include file */
+
+namespace Ipopt
+{
+#include "IpReturnCodes_inc.h"
+}
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpReturnCodes.inc b/SimTKmath/Optimizers/src/IpOpt/IpReturnCodes.inc
new file mode 100644
index 0000000..b0ce593
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpReturnCodes.inc
@@ -0,0 +1,64 @@
+C Copyright (C) 2005, 2006 Carnegie Mellon University and others.
+C All Rights Reserved.
+C This code is published under the Common Public License.
+C
+C $Id: IpReturnCodes.inc 759 2006-07-07 03:07:08Z andreasw $
+C
+C Author: Andreas Waechter IBM 2005-08-11
+C
+ INTEGER IP_SOLVE_SUCCEEDED
+ PARAMETER( IP_SOLVE_SUCCEEDED = 0 )
+
+ INTEGER IP_ACCEPTABLE_LEVEL
+ PARAMETER( IP_ACCEPTABLE_LEVEL = 1 )
+
+ INTEGER IP_INFEASIBLE_PROBLEM
+ PARAMETER( IP_INFEASIBLE_PROBLEM = 2 )
+
+ INTEGER IP_SEARCH_DIRECTION_TOO_SMALL
+ PARAMETER( IP_SEARCH_DIRECTION_TOO_SMALL = 3 )
+
+ INTEGER IP_DIVERGING_ITERATES
+ PARAMETER( IP_DIVERGING_ITERATES = 4 )
+
+ INTEGER IP_USER_REQUESTED_STOP
+ PARAMETER( IP_USER_REQUESTED_STOP = 5 )
+
+ INTEGER IP_ITERATION_EXCEEDED
+ PARAMETER( IP_ITERATION_EXCEEDED = -1 )
+
+ INTEGER IP_RESTORATION_FAILED
+ PARAMETER( IP_RESTORATION_FAILED = -2 )
+
+ INTEGER IP_ERROR_IN_STEP_COMPUTATION
+ PARAMETER( IP_ERROR_IN_STEP_COMPUTATION = -3 )
+
+ INTEGER IP_NOT_ENOUGH_DEGREES_OF_FREEDOM
+ PARAMETER( IP_NOT_ENOUGH_DEGREES_OF_FREEDOM = -10 )
+
+ INTEGER IP_INVALID_PROBLEM_DEFINITION
+ PARAMETER( IP_INVALID_PROBLEM_DEFINITION = -11)
+
+ INTEGER IP_INVALID_OPTION
+ PARAMETER( IP_INVALID_OPTION = -12 )
+
+ INTEGER IP_INVALID_NUMBER_DETECTED
+ PARAMETER( IP_INVALID_NUMBER_DETECTED = -13 )
+
+ INTEGER IP_UNRECOVERABLE_EXCEPTION
+ PARAMETER( IP_UNRECOVERABLE_EXCEPTION = -100 )
+
+ INTEGER IP_NON_IPOPT_EXCEPTION
+ PARAMETER( IP_NON_IPOPT_EXCEPTION = -101 )
+
+ INTEGER IP_INSUFFICIENT_MEMORY
+ PARAMETER( IP_INSUFFICIENT_MEMORY = -102 )
+
+ INTEGER IP_INTERNAL_ERROR
+ PARAMETER( IP_INTERNAL_ERROR = -199 )
+
+ INTEGER IP_REGULAR_MODE
+ PARAMETER( IP_REGULAR_MODE = 0 )
+
+ INTEGER IP_RESTORATION_PHASE_MODE
+ PARAMETER( IP_RESTORATION_PHASE_MODE = 1 )
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpReturnCodes_inc.h b/SimTKmath/Optimizers/src/IpOpt/IpReturnCodes_inc.h
new file mode 100644
index 0000000..9697b7c
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpReturnCodes_inc.h
@@ -0,0 +1,44 @@
+/***********************************************************************
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpReturnCodes_inc.h 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+************************************************************************/
+
+/* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! */
+/* !!!!!!!!!!!!!!!! REMEMBER TO UPDATE IpReturnCodes.inc !!!!!!!!!!!!!!!! */
+/* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! */
+
+/** Return codes for the Optimize call for an application */
+enum ApplicationReturnStatus
+ {
+ Solve_Succeeded=0,
+ Solved_To_Acceptable_Level=1,
+ Infeasible_Problem_Detected=2,
+ Search_Direction_Becomes_Too_Small=3,
+ Diverging_Iterates=4,
+ User_Requested_Stop=5,
+
+ Maximum_Iterations_Exceeded=-1,
+ Restoration_Failed=-2,
+ Error_In_Step_Computation=-3,
+ Not_Enough_Degrees_Of_Freedom=-10,
+ Invalid_Problem_Definition=-11,
+ Invalid_Option=-12,
+ Invalid_Number_Detected=-13,
+
+ Unrecoverable_Exception=-100,
+ NonIpopt_Exception_Thrown=-101,
+ Insufficient_Memory=-102,
+ Internal_Error=-199
+ };
+
+/** enum to indicate the mode in which the algorithm is */
+enum AlgorithmMode
+ {
+ RegularMode=0,
+ RestorationPhaseMode=1
+ };
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpScaledMatrix.cpp b/SimTKmath/Optimizers/src/IpOpt/IpScaledMatrix.cpp
new file mode 100644
index 0000000..2df608f
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpScaledMatrix.cpp
@@ -0,0 +1,181 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpScaledMatrix.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpScaledMatrix.hpp"
+
+namespace Ipopt
+{
+
+ ScaledMatrix::ScaledMatrix(const ScaledMatrixSpace* owner_space)
+ :
+ Matrix(owner_space),
+ owner_space_(owner_space)
+ {}
+
+
+ ScaledMatrix::~ScaledMatrix()
+ {}
+
+ void ScaledMatrix::MultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ DBG_ASSERT(IsValid(matrix_));
+
+ // Take care of the y part of the addition
+ if( beta!=0.0 ) {
+ y.Scal(beta);
+ }
+ else {
+ y.Set(0.0); // In case y hasn't been initialized yet
+ }
+
+ // need some temporary vectors
+ SmartPtr<Vector> tmp_x = x.MakeNewCopy();
+ SmartPtr<Vector> tmp_y = y.MakeNew();
+
+ if (IsValid(owner_space_->ColumnScaling())) {
+ tmp_x->ElementWiseMultiply(*owner_space_->ColumnScaling());
+ }
+
+ matrix_->MultVector(1.0, *tmp_x, 0.0, *tmp_y);
+
+ if (IsValid(owner_space_->RowScaling())) {
+ tmp_y->ElementWiseMultiply(*owner_space_->RowScaling());
+ }
+
+ y.Axpy(1.0, *tmp_y);
+ }
+
+ void ScaledMatrix::TransMultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ DBG_ASSERT(IsValid(matrix_));
+
+ // Take care of the y part of the addition
+ if( beta!=0.0 ) {
+ y.Scal(beta);
+ }
+ else {
+ y.Set(0.0); // In case y hasn't been initialized yet
+ }
+
+ // need some temporary vectors
+ SmartPtr<Vector> tmp_x = x.MakeNewCopy();
+ SmartPtr<Vector> tmp_y = y.MakeNew();
+
+ if (IsValid(owner_space_->RowScaling())) {
+ tmp_x->ElementWiseMultiply(*owner_space_->RowScaling());
+ }
+
+ matrix_->TransMultVector(1.0, *tmp_x, 0.0, *tmp_y);
+
+ if (IsValid(owner_space_->ColumnScaling())) {
+ tmp_y->ElementWiseMultiply(*owner_space_->ColumnScaling());
+ }
+
+ y.Axpy(1.0, *tmp_y);
+ }
+
+ bool ScaledMatrix::HasValidNumbersImpl() const
+ {
+ DBG_ASSERT(IsValid(matrix_));
+ return matrix_->HasValidNumbers();
+ }
+
+ void ScaledMatrix::PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ jnlst.Printf(level, category, "\n");
+ jnlst.PrintfIndented(level, category, indent,
+ "%sScaledMatrix \"%s\" of dimension %d x %d:\n",
+ prefix.c_str(), name.c_str(), NRows(), NCols());
+ if (IsValid(owner_space_->RowScaling())) {
+ owner_space_->RowScaling()->Print(&jnlst, level, category,
+ name+"_row_scaling", indent+1, prefix);
+ }
+ else {
+ jnlst.PrintfIndented(level, category, indent+1, "RowScaling is NULL\n");
+ }
+ if (IsValid(matrix_)) {
+ matrix_->Print(&jnlst, level, category, name+"_unscaled_matrix",
+ indent+1, prefix);
+ }
+ else {
+ jnlst.PrintfIndented(level, category, indent+1,
+ "%sunscaled matrix is NULL\n", prefix.c_str());
+ }
+ if (IsValid(owner_space_->ColumnScaling())) {
+ owner_space_->ColumnScaling()->Print(&jnlst, level, category,
+ name+"_column_scaling",
+ indent+1, prefix);
+ }
+ else {
+ jnlst.PrintfIndented(level, category, indent+1,
+ "%sColumnScaling is NULL\n", prefix.c_str());
+ }
+ }
+
+ void ScaledMatrix::AddMSinvZImpl(Number alpha, const Vector& S,
+ const Vector& Z, Vector& X) const
+ {
+ DBG_ASSERT(false && "Got the ScaledMatrix::AddMSinvZImpl. Should implement specialized method!");
+
+ SmartPtr<Vector> tmp = S.MakeNew();
+ tmp->AddVectorQuotient(1., Z, S, 0.);
+ MultVector(alpha, *tmp, 1., X);
+ }
+
+ void ScaledMatrix::SinvBlrmZMTdBrImpl(Number alpha, const Vector& S,
+ const Vector& R, const Vector& Z,
+ const Vector& D, Vector& X) const
+ {
+ DBG_ASSERT(false && "Got the ScaledMatrix::SinvBlrmZMTdBrImpl. Should implement specialized method!");
+
+ TransMultVector(alpha, D, 0., X);
+ X.ElementWiseMultiply(Z);
+ X.Axpy(1., R);
+ X.ElementWiseDivide(S);
+ }
+
+
+ ScaledMatrixSpace::ScaledMatrixSpace(
+ const SmartPtr<const Vector>& row_scaling,
+ bool row_scaling_reciprocal,
+ const SmartPtr<const MatrixSpace>& unscaled_matrix_space,
+ const SmartPtr<const Vector>& column_scaling,
+ bool column_scaling_reciprocal)
+ :
+ MatrixSpace(unscaled_matrix_space->NRows(),
+ unscaled_matrix_space->NCols()),
+ unscaled_matrix_space_(unscaled_matrix_space)
+ {
+ if (IsValid(row_scaling)) {
+ row_scaling_ = row_scaling->MakeNewCopy();
+ if (row_scaling_reciprocal) {
+ row_scaling_->ElementWiseReciprocal();
+ }
+ }
+ else {
+ row_scaling_ = NULL;
+ }
+
+ if (IsValid(column_scaling)) {
+ column_scaling_ = column_scaling->MakeNewCopy();
+ if (column_scaling_reciprocal) {
+ column_scaling_->ElementWiseReciprocal();
+ }
+ }
+ else {
+ column_scaling_ = NULL;
+ }
+ }
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpScaledMatrix.hpp b/SimTKmath/Optimizers/src/IpOpt/IpScaledMatrix.hpp
new file mode 100644
index 0000000..86fe142
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpScaledMatrix.hpp
@@ -0,0 +1,251 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpScaledMatrix.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPSCALEDMATRIX_HPP__
+#define __IPSCALEDMATRIX_HPP__
+
+#include "IpUtils.hpp"
+#include "IpMatrix.hpp"
+
+namespace Ipopt
+{
+
+ /* forward declarations */
+ class ScaledMatrixSpace;
+
+ /** Class for a Matrix in conjunction with its scaling factors for
+ * row and column scaling. Operations on the matrix are performed using
+ * the scaled matrix. You can pull out the pointer to the
+ * unscaled matrix for unscaled calculations.
+ */
+ class ScaledMatrix : public Matrix
+ {
+ public:
+
+ /**@name Constructors / Destructors */
+ //@{
+
+ /** Constructor, taking the owner_space.
+ */
+ ScaledMatrix(const ScaledMatrixSpace* owner_space);
+
+ /** Destructor */
+ ~ScaledMatrix();
+ //@}
+
+ /** Set the unscaled matrix */
+ void SetUnscaledMatrix(const SmartPtr<const Matrix> unscaled_matrix);
+
+ /** Set the unscaled matrix in a non-const version */
+ void SetUnscaledMatrixNonConst(const SmartPtr<Matrix>& unscaled_matrix);
+
+ /** Return the unscaled matrix in const form */
+ SmartPtr<const Matrix> GetUnscaledMatrix() const;
+
+ /** Return the unscaled matrix in non-const form */
+ SmartPtr<Matrix> GetUnscaledMatrixNonConst();
+
+ /** return the vector for the row scaling */
+ SmartPtr<const Vector> RowScaling() const;
+
+ /** return the vector for the column scaling */
+ SmartPtr<const Vector> ColumnScaling() const;
+
+ protected:
+ /**@name Methods overloaded from Matrix */
+ //@{
+ virtual void MultVectorImpl(Number alpha, const Vector& x,
+ Number beta, Vector& y) const;
+
+ virtual void TransMultVectorImpl(Number alpha, const Vector& x,
+ Number beta, Vector& y) const;
+
+ /** Method for determining if all stored numbers are valid (i.e.,
+ * no Inf or Nan). It is assumed that the scaling factors are
+ * valid. */
+ virtual bool HasValidNumbersImpl() const;
+
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const;
+
+ /** X = beta*X + alpha*(Matrix S^{-1} Z). Specialized
+ * implementation missing so far!
+ */
+ virtual void AddMSinvZImpl(Number alpha, const Vector& S, const Vector& Z,
+ Vector& X) const;
+
+ /** X = S^{-1} (r + alpha*Z*M^Td). Specialized implementation
+ * missing so far!
+ */
+ virtual void SinvBlrmZMTdBrImpl(Number alpha, const Vector& S,
+ const Vector& R, const Vector& Z,
+ const Vector& D, Vector& X) const;
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ ScaledMatrix();
+
+ /** Copy Constructor */
+ ScaledMatrix(const ScaledMatrix&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const ScaledMatrix&);
+ //@}
+
+ /** const version of the unscaled matrix */
+ SmartPtr<const Matrix> matrix_;
+ /** non-const version of the unscaled matrix */
+ SmartPtr<Matrix> nonconst_matrix_;
+
+ /** Matrix space stored as a ScaledMatrixSpace */
+ SmartPtr<const ScaledMatrixSpace> owner_space_;
+ };
+
+ /** This is the matrix space for ScaledMatrix.
+ */
+ class ScaledMatrixSpace : public MatrixSpace
+ {
+ public:
+ /** @name Constructors / Destructors */
+ //@{
+ /** Constructor, given the number of row and columns blocks, as
+ * well as the totel number of rows and columns.
+ */
+ ScaledMatrixSpace(const SmartPtr<const Vector>& row_scaling,
+ bool row_scaling_reciprocal,
+ const SmartPtr<const MatrixSpace>& unscaled_matrix_space,
+ const SmartPtr<const Vector>& column_scaling,
+ bool column_scaling_reciprocal);
+
+ /** Destructor */
+ ~ScaledMatrixSpace()
+ {}
+ ;
+ //@}
+
+ /** Method for creating a new matrix of this specific type. */
+ ScaledMatrix* MakeNewScaledMatrix(bool allocate_unscaled_matrix = false) const
+ {
+ ScaledMatrix* ret = new ScaledMatrix(this);
+ if (allocate_unscaled_matrix) {
+ SmartPtr<Matrix> unscaled_matrix = unscaled_matrix_space_->MakeNew();
+ ret->SetUnscaledMatrixNonConst(unscaled_matrix);
+ }
+ return ret;
+ }
+
+ /** Overloaded MakeNew method for the MatrixSpace base class.
+ */
+ virtual Matrix* MakeNew() const
+ {
+ return MakeNewScaledMatrix();
+ }
+
+ /** return the vector for the row scaling */
+ SmartPtr<const Vector> RowScaling() const
+ {
+ return ConstPtr(row_scaling_);
+ }
+
+ /** return the matrix space for the unscaled matrix */
+ SmartPtr<const MatrixSpace> UnscaledMatrixSpace() const
+ {
+ return unscaled_matrix_space_;
+ }
+
+ /** return the vector for the column scaling */
+ SmartPtr<const Vector> ColumnScaling() const
+ {
+ return ConstPtr(column_scaling_);
+ }
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default constructor */
+ ScaledMatrixSpace();
+
+ /** Copy Constructor */
+ ScaledMatrixSpace(const ScaledMatrixSpace&);
+
+ /** Overloaded Equals Operator */
+ ScaledMatrixSpace& operator=(const ScaledMatrixSpace&);
+ //@}
+
+ /** Row scaling vector */
+ SmartPtr<Vector> row_scaling_;
+ /** unscaled matrix space */
+ SmartPtr<const MatrixSpace> unscaled_matrix_space_;
+ /** column scaling vector */
+ SmartPtr<Vector> column_scaling_;
+ };
+
+ inline
+ void ScaledMatrix::SetUnscaledMatrix(const SmartPtr<const Matrix> unscaled_matrix)
+ {
+ matrix_ = unscaled_matrix;
+ nonconst_matrix_ = NULL;
+ ObjectChanged();
+ }
+
+ inline
+ void ScaledMatrix::SetUnscaledMatrixNonConst(const SmartPtr<Matrix>& unscaled_matrix)
+ {
+ nonconst_matrix_ = unscaled_matrix;
+ matrix_ = GetRawPtr(unscaled_matrix);
+ ObjectChanged();
+ }
+
+ inline
+ SmartPtr<const Matrix> ScaledMatrix::GetUnscaledMatrix() const
+ {
+ return matrix_;
+ }
+
+ inline
+ SmartPtr<Matrix> ScaledMatrix::GetUnscaledMatrixNonConst()
+ {
+ DBG_ASSERT(IsValid(nonconst_matrix_));
+ ObjectChanged();
+ return nonconst_matrix_;
+ }
+
+ inline
+ SmartPtr<const Vector> ScaledMatrix::RowScaling() const
+ {
+ return ConstPtr(owner_space_->RowScaling());
+ }
+
+ inline
+ SmartPtr<const Vector> ScaledMatrix::ColumnScaling() const
+ {
+ return ConstPtr(owner_space_->ColumnScaling());
+ }
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpSmartPtr.hpp b/SimTKmath/Optimizers/src/IpOpt/IpSmartPtr.hpp
new file mode 100644
index 0000000..1aa2681
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpSmartPtr.hpp
@@ -0,0 +1,723 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpSmartPtr.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPSMARTPTR_HPP__
+#define __IPSMARTPTR_HPP__
+
+#include "IpReferenced.hpp"
+
+//#define IP_DEBUG_SMARTPTR
+#ifdef IP_DEBUG_SMARTPTR
+# include "IpDebug.hpp"
+#endif
+
+namespace Ipopt
+{
+
+ /** Template class for Smart Pointers.
+ * A SmartPtr behaves much like a raw pointer, but manages the lifetime
+ * of an object, deleting the object automatically. This class implements
+ * a reference-counting, intrusive smart pointer design, where all
+ * objects pointed to must inherit off of ReferencedObject, which
+ * stores the reference count. Although this is intrusive (native types
+ * and externally authored classes require wrappers to be referenced
+ * by smart pointers), it is a safer design. A more detailed discussion of
+ * these issues follows after the usage information.
+ *
+ * Usage Example:
+ * Note: to use the SmartPtr, all objects to which you point MUST
+ * inherit off of ReferencedObject.
+ *
+ * \verbatim
+ *
+ * In MyClass.hpp...
+ *
+ * #include "IpReferenced.hpp"
+
+ * namespace Ipopt {
+ *
+ * class MyClass : public ReferencedObject // must derive from ReferencedObject
+ * {
+ * ...
+ * }
+ * } // namespace Ipopt
+ *
+ *
+ * In my_usage.cpp...
+ *
+ * #include "IpSmartPtr.hpp"
+ * #include "MyClass.hpp"
+ *
+ * void func(AnyObject& obj)
+ * {
+ * SmartPtr<MyClass> ptr_to_myclass = new MyClass(...);
+ * // ptr_to_myclass now points to a new MyClass,
+ * // and the reference count is 1
+ *
+ * ...
+ *
+ * obj.SetMyClass(ptr_to_myclass);
+ * // Here, let's assume that AnyObject uses a
+ * // SmartPtr<MyClass> internally here.
+ * // Now, both ptr_to_myclass and the internal
+ * // SmartPtr in obj point to the same MyClass object
+ * // and its reference count is 2.
+ *
+ * ...
+ *
+ * // No need to delete ptr_to_myclass, this
+ * // will be done automatically when the
+ * // reference count drops to zero.
+ *
+ * }
+ *
+ * \endverbatim
+ *
+ * It is not necessary to use SmartPtr's in all cases where an
+ * object is used that has been allocated "into" a SmartPtr. It is
+ * possible to just pass objects by reference or regular pointers,
+ * even if lower down in the stack a SmartPtr is to be held on to.
+ * Everything should work fine as long as a pointer created by "new"
+ * is immediately passed into a SmartPtr, and if SmartPtr's are used
+ * to hold on to objects.
+ *
+ * Other Notes:
+ * The SmartPtr implements both dereference operators -> & *.
+ * The SmartPtr does NOT implement a conversion operator to
+ * the raw pointer. Use the GetRawPtr() method when this
+ * is necessary. Make sure that the raw pointer is NOT
+ * deleted.
+ * The SmartPtr implements the comparison operators == & !=
+ * for a variety of types. Use these instead of
+ * \verbatim
+ * if (GetRawPtr(smrt_ptr) == ptr) // Don't use this
+ * \endverbatim
+ * SmartPtr's, as currently implemented, do NOT handle circular references.
+ * For example: consider a higher level object using SmartPtrs to point to
+ * A and B, but A and B also point to each other (i.e. A has a SmartPtr
+ * to B and B has a SmartPtr to A). In this scenario, when the higher
+ * level object is finished with A and B, their reference counts will
+ * never drop to zero (since they reference each other) and they
+ * will not be deleted. This can be detected by memory leak tools like
+ * valgrind. If the circular reference is necessary, the problem can be
+ * overcome by a number of techniques:
+ *
+ * 1) A and B can have a method that "releases" each other, that is
+ * they set their internal SmartPtrs to NULL.
+ * \verbatim
+ * void AClass::ReleaseCircularReferences()
+ * {
+ * smart_ptr_to_B = NULL;
+ * }
+ * \endverbatim
+ * Then, the higher level class can call these methods before
+ * it is done using A & B.
+ *
+ * 2) Raw pointers can be used in A and B to reference each other.
+ * Here, an implicit assumption is made that the lifetime is
+ * controlled by the higher level object and that A and B will
+ * both exist in a controlled manner. Although this seems
+ * dangerous, in many situations, this type of referencing
+ * is very controlled and this is reasonably safe.
+ *
+ * 3) This SmartPtr class could be redesigned with the Weak/Strong
+ * design concept. Here, the SmartPtr is identified as being
+ * Strong (controls lifetime of the object) or Weak (merely
+ * referencing the object). The Strong SmartPtr increments
+ * (and decrements) the reference count in ReferencedObject
+ * but the Weak SmartPtr does not. In the example above,
+ * the higher level object would have Strong SmartPtrs to
+ * A and B, but A and B would have Weak SmartPtrs to each
+ * other. Then, when the higher level object was done with
+ * A and B, they would be deleted. The Weak SmartPtrs in A
+ * and B would not decrement the reference count and would,
+ * of course, not delete the object. This idea is very similar
+ * to item (2), where it is implied that the sequence of events
+ * is controlled such that A and B will not call anything using
+ * their pointers following the higher level delete (i.e. in
+ * their destructors!). This is somehow safer, however, because
+ * code can be written (however expensive) to perform run-time
+ * detection of this situation. For example, the ReferencedObject
+ * could store pointers to all Weak SmartPtrs that are referencing
+ * it and, in its destructor, tell these pointers that it is
+ * dying. They could then set themselves to NULL, or set an
+ * internal flag to detect usage past this point.
+ *
+ * Comments on Non-Intrusive Design:
+ * In a non-intrusive design, the reference count is stored somewhere other
+ * than the object being referenced. This means, unless the reference
+ * counting pointer is the first referencer, it must get a pointer to the
+ * referenced object from another smart pointer (so it has access to the
+ * reference count location). In this non-intrusive design, if we are
+ * pointing to an object with a smart pointer (or a number of smart
+ * pointers), and we then give another smart pointer the address through
+ * a RAW pointer, we will have two independent, AND INCORRECT, reference
+ * counts. To avoid this pitfall, we use an intrusive reference counting
+ * technique where the reference count is stored in the object being
+ * referenced.
+ */
+ template<class T>
+ class SmartPtr : public Referencer
+ {
+ public:
+#define dbg_smartptr_verbosity 0
+
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default constructor, initialized to NULL */
+ SmartPtr();
+
+ /** Copy constructor, initialized from copy */
+ SmartPtr(const SmartPtr<T>& copy);
+
+ /** Copy Constructor, initialized from copy of
+ * a different type, will cause run-time error
+ * if type is not valid.
+ */
+ /* Don't support this, use dynamic_cast(GetRawPtr(copy))
+ template <class U>
+ SmartPtr(const SmartPtr<U> ©);
+ */
+
+ /** Constructor, initialized from T* ptr */
+ SmartPtr(T* ptr);
+
+ /** Destructor, automatically decrements the
+ * reference count, deletes the object if
+ * necessary.*/
+ ~SmartPtr();
+ //@}
+
+ /**@name Overloaded operators. */
+ //@{
+ /** Overloaded arrow operator, allows the user to call
+ * methods using the contained pointer. */
+ T* operator->() const;
+
+ /** Overloaded dereference operator, allows the user
+ * to dereference the contained pointer. */
+ T& operator*() const;
+
+ /** Overloaded equals operator, allows the user to
+ * set the value of the SmartPtr from a raw pointer */
+ SmartPtr<T>& operator=(T* rhs);
+
+ /** Overloaded equals operator, allows the user to
+ * set the value of the SmartPtr from another
+ * SmartPtr */
+ SmartPtr<T>& operator=(const SmartPtr<T>& rhs);
+
+ /** Overloaded equality comparison operator, allows the
+ * user to compare the value of two SmartPtrs */
+ template <class U1, class U2>
+ friend
+ bool operator==(const SmartPtr<U1>& lhs, const SmartPtr<U2>& rhs);
+
+ /** Overloaded equality comparison operator, allows the
+ * user to compare the value of a SmartPtr with a raw pointer. */
+ template <class U1, class U2>
+ friend
+ bool operator==(const SmartPtr<U1>& lhs, U2* raw_rhs);
+
+ /** Overloaded equality comparison operator, allows the
+ * user to compare the value of a raw pointer with a SmartPtr. */
+ template <class U1, class U2>
+ friend
+ bool operator==(U1* lhs, const SmartPtr<U2>& raw_rhs);
+
+ /** Overloaded in-equality comparison operator, allows the
+ * user to compare the value of two SmartPtrs */
+ template <class U1, class U2>
+ friend
+ bool operator!=(const SmartPtr<U1>& lhs, const SmartPtr<U2>& rhs);
+
+ /** Overloaded in-equality comparison operator, allows the
+ * user to compare the value of a SmartPtr with a raw pointer. */
+ template <class U1, class U2>
+ friend
+ bool operator!=(const SmartPtr<U1>& lhs, U2* raw_rhs);
+
+ /** Overloaded in-equality comparison operator, allows the
+ * user to compare the value of a SmartPtr with a raw pointer. */
+ template <class U1, class U2>
+ friend
+ bool operator!=(U1* lhs, const SmartPtr<U2>& raw_rhs);
+ //@}
+
+ /**@name friend method declarations. */
+ //@{
+ /** Returns the raw pointer contained.
+ * Use to get the value of
+ * the raw ptr (i.e. to pass to other
+ * methods/functions, etc.)
+ * Note: This method does NOT copy,
+ * therefore, modifications using this
+ * value modify the underlying object
+ * contained by the SmartPtr,
+ * NEVER delete this returned value.
+ */
+ template <class U>
+ friend
+ U* GetRawPtr(const SmartPtr<U>& smart_ptr);
+
+ /** Returns a const pointer */
+ template <class U>
+ friend
+ SmartPtr<const U> ConstPtr(const SmartPtr<U>& smart_ptr);
+
+ /** Returns true if the SmartPtr is NOT NULL.
+ * Use this to check if the SmartPtr is not null
+ * This is preferred to if(GetRawPtr(sp) != NULL)
+ */
+ template <class U>
+ friend
+ bool IsValid(const SmartPtr<U>& smart_ptr);
+
+ /** Returns true if the SmartPtr is NULL.
+ * Use this to check if the SmartPtr IsNull.
+ * This is preferred to if(GetRawPtr(sp) == NULL)
+ */
+ template <class U>
+ friend
+ bool IsNull(const SmartPtr<U>& smart_ptr);
+ //@}
+
+ private:
+ /**@name Private Data/Methods */
+ //@{
+ /** Actual raw pointer to the object. */
+ T* ptr_;
+
+ /** Set the value of the internal raw pointer
+ * from another raw pointer, releasing the
+ * previously referenced object if necessary. */
+ SmartPtr<T>& SetFromRawPtr_(T* rhs);
+
+ /** Set the value of the internal raw pointer
+ * from a SmartPtr, releasing the previously referenced
+ * object if necessary. */
+ SmartPtr<T>& SetFromSmartPtr_(const SmartPtr<T>& rhs);
+
+ /** Release the currently referenced object. */
+ void ReleasePointer_();
+ //@}
+ };
+
+ /**@name SmartPtr friend function declarations.*/
+ //@{
+ template <class U>
+ U* GetRawPtr(const SmartPtr<U>& smart_ptr);
+
+ template <class U>
+ SmartPtr<const U> ConstPtr(const SmartPtr<U>& smart_ptr);
+
+ template <class U>
+ bool IsNull(const SmartPtr<U>& smart_ptr);
+
+ template <class U>
+ bool IsValid(const SmartPtr<U>& smart_ptr);
+
+ template <class U1, class U2>
+ bool operator==(const SmartPtr<U1>& lhs, const SmartPtr<U2>& rhs);
+
+ template <class U1, class U2>
+ bool operator==(const SmartPtr<U1>& lhs, U2* raw_rhs);
+
+ template <class U1, class U2>
+ bool operator==(U1* lhs, const SmartPtr<U2>& raw_rhs);
+
+ template <class U1, class U2>
+ bool operator!=(const SmartPtr<U1>& lhs, const SmartPtr<U2>& rhs);
+
+ template <class U1, class U2>
+ bool operator!=(const SmartPtr<U1>& lhs, U2* raw_rhs);
+
+ template <class U1, class U2>
+ bool operator!=(U1* lhs, const SmartPtr<U2>& raw_rhs);
+
+ //@}
+
+
+ template <class T>
+ SmartPtr<T>::SmartPtr()
+ :
+ ptr_(NULL)
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_METH("SmartPtr<T>::SmartPtr()", dbg_smartptr_verbosity);
+#endif
+
+#ifdef CHECK_SMARTPTR
+
+ const ReferencedObject* trying_to_use_SmartPtr_with_an_object_that_does_not_inherit_from_ReferencedObject_
+ = ptr_;
+ trying_to_use_SmartPtr_with_an_object_that_does_not_inherit_from_ReferencedObject_ = NULL;
+#endif
+
+ }
+
+
+ template <class T>
+ SmartPtr<T>::SmartPtr(const SmartPtr<T>& copy)
+ :
+ ptr_(NULL)
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_METH("SmartPtr<T>::SmartPtr(const SmartPtr<T>& copy)", dbg_smartptr_verbosity);
+#endif
+
+#ifdef CHECK_SMARTPTR
+
+ const ReferencedObject* trying_to_use_SmartPtr_with_an_object_that_does_not_inherit_from_ReferencedObject_
+ = ptr_;
+ trying_to_use_SmartPtr_with_an_object_that_does_not_inherit_from_ReferencedObject_ = NULL;
+#endif
+
+ (void) SetFromSmartPtr_(copy);
+ }
+
+
+ /* Don't support this, use dynamic_cast(GetRawPtr(copy))
+ template <class T>
+ template <class U> SmartPtr<T>::SmartPtr(const SmartPtr<U>& copy)
+ :
+ ptr_(NULL)
+ {
+ DBG_START_METH("SmartPtr<T>::SmartPtr(const SmartPtr<U>& copy)", dbg_smartptr_verbosity);
+
+ U* raw_U_ptr = GetRawPtr(copy);
+
+ // try to cast to type T
+ T* raw_T_ptr = dynamic_cast<T*>(raw_U_ptr);
+
+ // Don't try to point to type U with a
+ // type T SmartPtr if the types U & T
+ // are not related
+ DBG_ASSERT(raw_T_ptr);
+
+ (void) SetFromRawPtr_(raw_T_ptr);
+ }
+ */
+
+
+ template <class T>
+ SmartPtr<T>::SmartPtr(T* ptr)
+ :
+ ptr_(NULL)
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_METH("SmartPtr<T>::SmartPtr(T* ptr)", dbg_smartptr_verbosity);
+#endif
+
+#ifdef CHECK_SMARTPTR
+
+ const ReferencedObject* trying_to_use_SmartPtr_with_an_object_that_does_not_inherit_from_ReferencedObject_
+ = ptr_;
+ trying_to_use_SmartPtr_with_an_object_that_does_not_inherit_from_ReferencedObject_ = NULL;
+#endif
+
+ (void) SetFromRawPtr_(ptr);
+ }
+
+ template <class T>
+ SmartPtr<T>::~SmartPtr()
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_METH("SmartPtr<T>::~SmartPtr(T* ptr)", dbg_smartptr_verbosity);
+#endif
+
+ ReleasePointer_();
+ }
+
+
+ template <class T>
+ T* SmartPtr<T>::operator->() const
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_METH("T* SmartPtr<T>::operator->()", dbg_smartptr_verbosity);
+#endif
+
+ // cannot deref a null pointer
+#ifdef IP_DEBUG
+
+ assert(ptr_);
+#endif
+
+ return ptr_;
+ }
+
+
+ template <class T>
+ T& SmartPtr<T>::operator*() const
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_METH("T& SmartPtr<T>::operator*()", dbg_smartptr_verbosity);
+#endif
+
+ // cannot dereference a null pointer
+#ifdef IP_DEBUG
+
+ assert(ptr_);
+#endif
+
+ return *ptr_;
+ }
+
+
+ template <class T>
+ SmartPtr<T>& SmartPtr<T>::operator=(T* rhs)
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_METH("SmartPtr<T>& SmartPtr<T>::operator=(T* rhs)", dbg_smartptr_verbosity);
+#endif
+
+ return SetFromRawPtr_(rhs);
+ }
+
+
+ template <class T>
+ SmartPtr<T>& SmartPtr<T>::operator=(const SmartPtr<T>& rhs)
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_METH(
+ "SmartPtr<T>& SmartPtr<T>::operator=(const SmartPtr<T>& rhs)",
+ dbg_smartptr_verbosity);
+#endif
+
+ return SetFromSmartPtr_(rhs);
+ }
+
+
+ template <class T>
+ SmartPtr<T>& SmartPtr<T>::SetFromRawPtr_(T* rhs)
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_METH(
+ "SmartPtr<T>& SmartPtr<T>::SetFromRawPtr_(T* rhs)", dbg_smartptr_verbosity);
+#endif
+
+ // Release any old pointer
+ ReleasePointer_();
+
+ if (rhs != NULL) {
+ rhs->AddRef(this);
+ ptr_ = rhs;
+ // const ReferencedObject* r_ptr =
+ // dynamic_cast<const ReferencedObject*>(rhs);
+
+ // // All objects pointed to by a SmartPtr MUST
+ // // inherit off of ReferencedObject
+ // DBG_ASSERT(r_ptr && "This is not inherited from ReferencedObject");
+
+ // if (r_ptr) {
+ // r_ptr->AddRef(this);
+ // ptr_ = rhs;
+ // }
+ }
+
+ return *this;
+ }
+
+ template <class T>
+ SmartPtr<T>& SmartPtr<T>::SetFromSmartPtr_(const SmartPtr<T>& rhs)
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_METH(
+ "SmartPtr<T>& SmartPtr<T>::SetFromSmartPtr_(const SmartPtr<T>& rhs)",
+ dbg_smartptr_verbosity);
+#endif
+
+ T* ptr = GetRawPtr(rhs);
+ /* AW: I changed this so that NULL is correctly copied from the
+ right hand side */
+ // if (ptr != NULL) {
+ // SetFromRawPtr_(ptr);
+ // }
+ SetFromRawPtr_(ptr);
+
+ return (*this);
+ }
+
+
+ template <class T>
+ void SmartPtr<T>::ReleasePointer_()
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_METH(
+ "void SmartPtr<T>::ReleasePointer()",
+ dbg_smartptr_verbosity);
+#endif
+
+ if (ptr_) {
+ ptr_->ReleaseRef(this);
+ if (ptr_->ReferenceCount() == 0) {
+ delete ptr_;
+ }
+ ptr_ = NULL;
+ }
+ // if (ptr_ != NULL) {
+ // const ReferencedObject* r_ptr =
+ // dynamic_cast<const ReferencedObject*>(ptr_);
+
+ // r_ptr->ReleaseRef(this);
+ // if (r_ptr->ReferenceCount() == 0) {
+ // delete ptr_;
+ // }
+ // ptr_ = NULL;
+ // }
+ }
+
+
+ template <class U>
+ U* GetRawPtr(const SmartPtr<U>& smart_ptr)
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_FUN(
+ "T* GetRawPtr(const SmartPtr<T>& smart_ptr)",
+ 0);
+#endif
+
+ return smart_ptr.ptr_;
+ }
+
+ template <class U>
+ SmartPtr<const U> ConstPtr(const SmartPtr<U>& smart_ptr)
+ {
+ // compiler should implicitly cast
+ return GetRawPtr(smart_ptr);
+ }
+
+ template <class U>
+ bool IsValid(const SmartPtr<U>& smart_ptr)
+ {
+ return !IsNull(smart_ptr);
+ }
+
+ template <class U>
+ bool IsNull(const SmartPtr<U>& smart_ptr)
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_FUN(
+ "bool IsNull(const SmartPtr<T>& smart_ptr)",
+ 0);
+#endif
+
+ return (smart_ptr.ptr_ == NULL);
+ }
+
+
+ template <class U1, class U2>
+ bool ComparePointers(const U1* lhs, const U2* rhs)
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_FUN(
+ "bool ComparePtrs(const U1* lhs, const U2* rhs)",
+ dbg_smartptr_verbosity);
+#endif
+
+ if (lhs == rhs) {
+ return true;
+ }
+
+ // Even if lhs and rhs point to the same object
+ // with different interfaces U1 and U2, we cannot guarantee that
+ // the value of the pointers will be equivalent. We can
+ // guarantee this if we convert to void*
+ const void* v_lhs = dynamic_cast<const void*>(lhs);
+ const void* v_rhs = dynamic_cast<const void*>(rhs);
+ if (v_lhs == v_rhs) {
+ return true;
+ }
+
+ // They must not be the same
+ return false;
+ }
+
+ template <class U1, class U2>
+ bool operator==(const SmartPtr<U1>& lhs, const SmartPtr<U2>& rhs)
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_FUN(
+ "bool operator==(const SmartPtr<U1>& lhs, const SmartPtr<U2>& rhs)",
+ dbg_smartptr_verbosity);
+#endif
+
+ U1* raw_lhs = GetRawPtr(lhs);
+ U2* raw_rhs = GetRawPtr(rhs);
+ return ComparePointers(raw_lhs, raw_rhs);
+ }
+
+ template <class U1, class U2>
+ bool operator==(const SmartPtr<U1>& lhs, U2* raw_rhs)
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_FUN(
+ "bool operator==(SmartPtr<U1>& lhs, U2* rhs)",
+ dbg_smartptr_verbosity);
+#endif
+
+ U1* raw_lhs = GetRawPtr(lhs);
+ return ComparePointers(raw_lhs, raw_rhs);
+ }
+
+ template <class U1, class U2>
+ bool operator==(U1* raw_lhs, const SmartPtr<U2>& rhs)
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_FUN(
+ "bool operator==(U1* raw_lhs, SmartPtr<U2>& rhs)",
+ dbg_smartptr_verbosity);
+#endif
+
+ const U2* raw_rhs = GetRawPtr(rhs);
+ return ComparePointers(raw_lhs, raw_rhs);
+ }
+
+ template <class U1, class U2>
+ bool operator!=(const SmartPtr<U1>& lhs, const SmartPtr<U2>& rhs)
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_FUN(
+ "bool operator!=(const SmartPtr<U1>& lhs, const SmartPtr<U2>& rhs)",
+ dbg_smartptr_verbosity);
+#endif
+
+ bool retValue = operator==(lhs, rhs);
+ return !retValue;
+ }
+
+ template <class U1, class U2>
+ bool operator!=(const SmartPtr<U1>& lhs, U2* raw_rhs)
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_FUN(
+ "bool operator!=(SmartPtr<U1>& lhs, U2* rhs)",
+ dbg_smartptr_verbosity);
+#endif
+
+ bool retValue = operator==(lhs, raw_rhs);
+ return !retValue;
+ }
+
+ template <class U1, class U2>
+ bool operator!=(U1* raw_lhs, const SmartPtr<U2>& rhs)
+ {
+#ifdef IP_DEBUG_SMARTPTR
+ DBG_START_FUN(
+ "bool operator!=(U1* raw_lhs, SmartPtr<U2>& rhs)",
+ dbg_smartptr_verbosity);
+#endif
+
+ bool retValue = operator==(raw_lhs, rhs);
+ return !retValue;
+ }
+
+} // namespace Ipopt
+
+#endif
+
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpSolveStatistics.cpp b/SimTKmath/Optimizers/src/IpOpt/IpSolveStatistics.cpp
new file mode 100644
index 0000000..5ab265d
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpSolveStatistics.cpp
@@ -0,0 +1,96 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpSolveStatistics.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-08-15
+
+#include "IpSolveStatistics.hpp"
+#include "IpIpoptCalculatedQuantities.hpp"
+
+namespace Ipopt
+{
+
+ SolveStatistics::SolveStatistics(
+ const SmartPtr<IpoptNLP>& ip_nlp,
+ const SmartPtr<IpoptData>& ip_data,
+ const SmartPtr<IpoptCalculatedQuantities>& ip_cq)
+ :
+ num_iters_(ip_data->iter_count()),
+ total_cpu_time_(ip_data->TimingStats().OverallAlgorithm().TotalTime()),
+ num_obj_evals_(ip_nlp->f_evals()),
+ num_constr_evals_(Max(ip_nlp->c_evals(), ip_nlp->d_evals())),
+ num_obj_grad_evals_(ip_nlp->grad_f_evals()),
+ num_constr_jac_evals_(Max(ip_nlp->jac_c_evals(),ip_nlp->jac_c_evals())),
+ num_hess_evals_(ip_nlp->h_evals()),
+
+ scaled_obj_val_(ip_cq->curr_f()),
+ obj_val_(ip_cq->unscaled_curr_f()),
+ scaled_dual_inf_(ip_cq->curr_dual_infeasibility(NORM_MAX)),
+ dual_inf_(ip_cq->unscaled_curr_dual_infeasibility(NORM_MAX)),
+ scaled_constr_viol_(ip_cq->curr_nlp_constraint_violation(NORM_MAX)),
+ constr_viol_(ip_cq->unscaled_curr_nlp_constraint_violation(NORM_MAX)),
+ scaled_compl_(ip_cq->curr_complementarity(0., NORM_MAX)),
+ compl_(ip_cq->unscaled_curr_complementarity(0., NORM_MAX)),
+ scaled_kkt_error_(ip_cq->curr_nlp_error()),
+ kkt_error_(ip_cq->unscaled_curr_nlp_error())
+ {}
+
+ Index SolveStatistics::IterationCount() const
+ {
+ return num_iters_;
+ }
+
+ Number SolveStatistics::TotalCPUTime() const
+ {
+ return total_cpu_time_;
+ }
+
+ void SolveStatistics::NumberOfEvaluations(
+ Index& num_obj_evals,
+ Index& num_constr_evals,
+ Index& num_obj_grad_evals,
+ Index& num_constr_jac_evals,
+ Index& num_hess_evals) const
+ {
+ num_obj_evals = num_obj_evals_;
+ num_constr_evals = num_constr_evals_;
+ num_obj_grad_evals = num_obj_grad_evals_;
+ num_constr_jac_evals = num_constr_jac_evals_;
+ num_hess_evals = num_hess_evals_;
+ }
+
+ void SolveStatistics::Infeasibilities(Number& dual_inf,
+ Number& constr_viol,
+ Number& complementarity,
+ Number& kkt_error) const
+ {
+ dual_inf = dual_inf_;
+ constr_viol = constr_viol_;
+ complementarity = compl_;
+ kkt_error = kkt_error_;
+ }
+
+ void SolveStatistics::ScaledInfeasibilities(Number& scaled_dual_inf,
+ Number& scaled_constr_viol,
+ Number& scaled_complementarity,
+ Number& scaled_kkt_error) const
+ {
+ scaled_dual_inf = scaled_dual_inf_;
+ scaled_constr_viol = scaled_constr_viol_;
+ scaled_complementarity = scaled_compl_;
+ scaled_kkt_error = scaled_kkt_error_;
+ }
+
+ Number SolveStatistics::FinalObjective() const
+ {
+ return obj_val_;
+ }
+
+ Number SolveStatistics::FinalScaledObjective() const
+ {
+ return scaled_obj_val_;
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpSolveStatistics.hpp b/SimTKmath/Optimizers/src/IpOpt/IpSolveStatistics.hpp
new file mode 100644
index 0000000..1fc296a
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpSolveStatistics.hpp
@@ -0,0 +1,136 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpSolveStatistics.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-08-15
+
+#ifndef __IPSOLVESTATISTICS_HPP__
+#define __IPSOLVESTATISTICS_HPP__
+
+#include "IpReferenced.hpp"
+#include "IpSmartPtr.hpp"
+
+namespace Ipopt
+{
+ // forward declaration (to avoid inclusion of too many header files)
+ class IpoptNLP;
+ class IpoptData;
+ class IpoptCalculatedQuantities;
+
+ /** This class collects statistics about an optimziation run, such
+ * as iteration count, final infeasibilities etc. It is meant to
+ * provide such information to a user of Ipopt during the
+ * finalize_solution call.
+ */
+ class SolveStatistics : public ReferencedObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default constructor. It takes in those collecting Ipopt
+ * objects that can provide the statistics information. Those
+ * statistics are retrieved at the time of the constructor
+ * call. */
+ SolveStatistics(const SmartPtr<IpoptNLP>& ip_nlp,
+ const SmartPtr<IpoptData>& ip_data,
+ const SmartPtr<IpoptCalculatedQuantities>& ip_cq);
+
+ /** Default destructor */
+ virtual ~SolveStatistics()
+ {}
+ //@}
+
+ /** @name Accessor methods for retrieving different kind of solver
+ * statistics information */
+ //@{
+ /** Iteration counts. */
+ Index IterationCount() const;
+ /** Total CPU time, including function evaluations. */
+ Number TotalCPUTime() const;
+ /** Number of NLP function evaluations. */
+ void NumberOfEvaluations(Index& num_obj_evals,
+ Index& num_constr_evals,
+ Index& num_obj_grad_evals,
+ Index& num_constr_jac_evals,
+ Index& num_hess_evals) const;
+ /** Unscaled solution infeasibilities */
+ void Infeasibilities(Number& dual_inf,
+ Number& constr_viol,
+ Number& complementarity,
+ Number& kkt_error) const;
+ /** Scaled solution infeasibilities */
+ void ScaledInfeasibilities(Number& scaled_dual_inf,
+ Number& scaled_constr_viol,
+ Number& scaled_complementarity,
+ Number& scaled_kkt_error) const;
+ /** Final value of objective function */
+ Number FinalObjective() const;
+ /** Final scaled value of objective function */
+ Number FinalScaledObjective() const;
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ SolveStatistics();
+
+ /** Copy Constructor */
+ SolveStatistics(const SolveStatistics&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const SolveStatistics&);
+ //@}
+
+ /** @name Fields for storing the statistics data */
+ //@{
+ /** Number of iterations. */
+ Index num_iters_;
+ /* Total CPU time */
+ Number total_cpu_time_;
+ /** Number of objective function evaluations. */
+ Index num_obj_evals_;
+ /** Number of constraints evaluations (max of equality and
+ * inequality) */
+ Index num_constr_evals_;
+ /** Number of objective gradient evaluations. */
+ Index num_obj_grad_evals_;
+ /** Number of constraint Jacobian evaluations. */
+ Index num_constr_jac_evals_;
+ /** Number of Lagrangian Hessian evaluations. */
+ Index num_hess_evals_;
+
+ /** Final scaled value of objective function */
+ Number scaled_obj_val_;
+ /** Final unscaled value of objective function */
+ Number obj_val_;
+ /** Final scaled dual infeasibility (max-norm) */
+ Number scaled_dual_inf_;
+ /** Final unscaled dual infeasibility (max-norm) */
+ Number dual_inf_;
+ /** Final scaled constraint violation (max-norm) */
+ Number scaled_constr_viol_;
+ /** Final unscaled constraint violation (max-norm) */
+ Number constr_viol_;
+ /** Final scaled complementarity error (max-norm) */
+ Number scaled_compl_;
+ /** Final unscaled complementarity error (max-norm) */
+ Number compl_;
+ /** Final overall scaled KKT error (max-norm) */
+ Number scaled_kkt_error_;
+ /** Final overall unscaled KKT error (max-norm) */
+ Number kkt_error_;
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpSparseSymLinearSolverInterface.hpp b/SimTKmath/Optimizers/src/IpOpt/IpSparseSymLinearSolverInterface.hpp
new file mode 100644
index 0000000..a7f6e1f
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpSparseSymLinearSolverInterface.hpp
@@ -0,0 +1,222 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpSparseSymLinearSolverInterface.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-03-17
+
+#ifndef __IPSPARSESYMLINEARSOLVERINTERFACE_HPP__
+#define __IPSPARSESYMLINEARSOLVERINTERFACE_HPP__
+
+#include "IpUtils.hpp"
+#include "IpAlgStrategy.hpp"
+#include "IpSymLinearSolver.hpp"
+
+namespace Ipopt
+{
+
+ /** Base class for interfaces to symmetric indefinite linear solvers
+ * for sparse matrices.
+ *
+ * This defines the general interface to linear solvers for sparse
+ * symmetric indefinite matrices. The matrices can be provided
+ * either in "triplet format" (like for Harwell's MA27 solver), or
+ * in compressed sparse row (CSR) format for the lower triangular
+ * part of the symmetric matrix.
+ *
+ * The solver should be able to compute the interia of the matrix,
+ * or more specifically, the number of negative eigenvalues in the
+ * factorized matrix.
+ *
+ * This interface is used by the calling objective in the following
+ * way:
+ *
+ * 1. The InitializeImpl method is called at the very beginning
+ * (for every optimization run), which allows the linear solver
+ * object to retrieve options given in the OptionsList (such as
+ * pivot tolerances etc). At this point, some internal data can
+ * also be initialized.
+ *
+ * 2. The calling class calls MatrixFormat to find out which matrix
+ * representation the linear solver requires. The possible options
+ * are Triplet_Format, as well as CSR_Format_0_Offset and
+ * CSR_Format_1_Offset. The difference between the last two is
+ * that for CSR_Format_0_Offset the couning of the element position
+ * in the ia and ja arrays starts are 0 (C-style numbering),
+ * whereas for the other one it starts at 1 (Fortran-style
+ * numbering).
+ *
+ * 3. After this, the InitializeStructure method is called (once).
+ * Here, the structure of the matrix is provided. If the linear
+ * solver requires a symbolic preprocessing phase that can be done
+ * without knowledge of the matrix element values, it can be done
+ * here.
+ *
+ * 4. The calling class will request an array for storing the
+ * actual values for a matrix using the GetValuesArrayPtr method.
+ * This array must be at least as large as the number of nonzeros
+ * in the matrix (as given to this class by the InitializeStructure
+ * method call). After a call of this method, the calling class
+ * will fill this array with the actual values of the matrix.
+ *
+ * 5. Every time lateron, when actual solves of a linear system is
+ * requested, the calling class will call the MultiSolve to request
+ * the solve, possibly for mulitple right-hand sides. The flag
+ * new_matrix then indicates if the values of the matrix have
+ * changed and if a factorization is required, or if an old
+ * factorization can be used to do the solve.
+ *
+ * Note that the GetValuesArrayPtr method will be called before
+ * every call of MultiSolve with new_matrix=true, or before a
+ * renewed call of MultiSolve if the most previous return value was
+ * SYMSOLV_CALL_AGAIN.
+ *
+ * 6. The calling class might request with NumberOfNegEVals the
+ * number of the negative eigenvalues for the original matrix that
+ * were detected during the most recently performed factorization.
+ *
+ * 7. The calling class might ask the linear solver to increase the
+ * quality of the solution. For example, if the linear solver uses
+ * a pivot tolerance, a larger value should be used for the next
+ * solve (which might require a refactorization).
+ *
+ * 8. Finally, when the destructor is called, the internal storage,
+ * also in the linear solver, should be released.
+ *
+ * Note, if the matrix is given in triplet format, entries might be
+ * listed multiple times, in which case the corresponsing elements
+ * have to be added.
+ *
+ * A note for warm starts: If the option
+ * "warm_start_same_structure" is specified with "yes", the
+ * algorithm assumes that a problem with the same sparsity
+ * structure is solved for a repeated time. In that case, the
+ * linear solver might reuse information from the previous
+ * optimization. See Ma27TSolverInterface for an example.
+ */
+ class SparseSymLinearSolverInterface: public AlgorithmStrategyObject
+ {
+ public:
+ /** Enum to specify sparse matrix format. */
+ enum EMatrixFormat {
+ /** Triplet (MA27) format. */
+ Triplet_Format,
+ /** Compressed sparse row format for lower triangular part, with
+ * 0 offset. */
+ CSR_Format_0_Offset,
+ /** Compressed sparse row format for lower triangular part, with
+ * 1 offset. */
+ CSR_Format_1_Offset,
+ /** Dense format
+ * 1 offset. */
+ Dense_Format
+ };
+ /** @name Constructor/Destructor */
+ //@{
+ SparseSymLinearSolverInterface()
+ {}
+
+ virtual ~SparseSymLinearSolverInterface()
+ {}
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix) = 0;
+
+ /** @name Methods for requesting solution of the linear system. */
+ //@{
+ /** Method for initializing internal stuctures. Here, ndim gives
+ * the number of rows and columns of the matrix, nonzeros give
+ * the number of nonzero elements, and ia and ja give the
+ * positions of the nonzero elements, given in the matrix format
+ * determined by MatrixFormat.
+ */
+ virtual ESymSolverStatus InitializeStructure(Index dim, Index nonzeros,
+ const Index* ia,
+ const Index* ja) = 0;
+
+ /** Method returing an internal array into which the nonzero
+ * elements (in the same order as ja) will be stored by the
+ * calling routine before a call to MultiSolve with a
+ * new_matrix=true (or after a return of MultiSolve with
+ * SYMSOLV_CALL_AGAIN). The returned array must have space for at
+ * least nonzero elements. */
+ virtual Number* GetValuesArrayPtr() = 0;
+
+ /** Solve operation for multiple right hand sides. Solves the
+ * linear system A * x = b with multiple right hand sides, where
+ * A is the symmtric indefinite matrix. Here, ia and ja give the
+ * positions of the values (in the required matrix data format).
+ * The actual values of the matrix will have been given to this
+ * object by copying them into the array provided by
+ * GetValuesArrayPtr. ia and ja are identical to the ones given
+ * to InitializeStructure. The flag new_matrix is set to true,
+ * if the values of the matrix has changed, and a refactorzation
+ * is required.
+ *
+ * The return code is SYMSOLV_SUCCESS if the factorization and
+ * solves were successful, SYMSOLV_SINGULAR if the linear system
+ * is singular, and SYMSOLV_WRONG_INERTIA if check_NegEVals is
+ * true and the number of negative eigenvalues in the matrix does
+ * not match numberOfNegEVals. If SYMSOLV_CALL_AGAIN is
+ * returned, then the calling function will request the pointer
+ * for the array for storing a again (with GetValuesPtr), write
+ * the values of the nonzero elements into it, and call this
+ * MultiSolve method again with the same right-hand sides. (This
+ * can be done, for example, if the linear solver realized it
+ * does not have sufficient memory and needs to redo the
+ * factorization; e.g., for MA27.)
+ *
+ * The number of right-hand sides is given by nrhs, the values of
+ * the right-hand sides are given in rhs_vals (one full right-hand
+ * side stored immediately after the other), and solutions are
+ * to be returned in the same array.
+ *
+ * check_NegEVals will not be chosen true, if ProvidesInertia()
+ * returns false.
+ */
+ virtual ESymSolverStatus MultiSolve(bool new_matrix,
+ const Index* ia,
+ const Index* ja,
+ Index nrhs,
+ Number* rhs_vals,
+ bool check_NegEVals,
+ Index numberOfNegEVals)=0;
+
+ /** Number of negative eigenvalues detected during last
+ * factorization. Returns the number of negative eigenvalues of
+ * the most recent factorized matrix. This must not be called if
+ * the linear solver does not compute this quantities (see
+ * ProvidesInertia).
+ */
+ virtual Index NumberOfNegEVals() const =0;
+ //@}
+
+ //* @name Options of Linear solver */
+ //@{
+ /** Request to increase quality of solution for next solve. The
+ * calling class asks linear solver to increase quality of
+ * solution for the next solve (e.g. increase pivot tolerance).
+ * Returns false, if this is not possible (e.g. maximal pivot
+ * tolerance already used.)
+ */
+ virtual bool IncreaseQuality() =0;
+
+ /** Query whether inertia is computed by linear solver. Returns
+ * true, if linear solver provides inertia.
+ */
+ virtual bool ProvidesInertia() const =0;
+
+ /** Query of requested matrix type that the linear solver
+ * understands.
+ */
+ virtual EMatrixFormat MatrixFormat() const =0;
+ //@}
+ };
+
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpStdAugSystemSolver.cpp b/SimTKmath/Optimizers/src/IpOpt/IpStdAugSystemSolver.cpp
new file mode 100644
index 0000000..e687fb5
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpStdAugSystemSolver.cpp
@@ -0,0 +1,551 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpStdAugSystemSolver.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpStdAugSystemSolver.hpp"
+#include "IpDebug.hpp"
+
+#include "IpCompoundSymMatrix.hpp"
+#include "IpCompoundVector.hpp"
+#include "IpSumSymMatrix.hpp"
+#include "IpDiagMatrix.hpp"
+#include "IpIdentityMatrix.hpp"
+// ToDo: Remove below here - for debug only
+#include "IpTripletHelper.hpp"
+// ToDo: Remove above here
+
+#include <cstdio>
+
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ StdAugSystemSolver::StdAugSystemSolver(SymLinearSolver& linSolver)
+ :
+ AugSystemSolver(),
+ linsolver_(&linSolver),
+ augmented_system_space_(NULL),
+ sumsym_space_x_(NULL),
+ diag_space_x_(NULL),
+ diag_space_s_(NULL),
+ diag_space_c_(NULL),
+ ident_space_ds_(NULL),
+ diag_space_d_(NULL),
+ w_tag_(0),
+ d_x_tag_(0),
+ delta_x_(0.),
+ d_s_tag_(0),
+ delta_s_(0.),
+ j_c_tag_(0),
+ d_c_tag_(0),
+ delta_c_(0.),
+ j_d_tag_(0),
+ d_d_tag_(0),
+ delta_d_(0.),
+ old_w_(NULL)
+ {
+ DBG_START_METH("StdAugSystemSolver::StdAugSystemSolver()",dbg_verbosity);
+ DBG_ASSERT(IsValid(linsolver_));
+ }
+
+ StdAugSystemSolver::~StdAugSystemSolver()
+ {
+ DBG_START_METH("StdAugSystemSolver::~StdAugSystemSolver()",dbg_verbosity);
+ }
+
+
+ bool StdAugSystemSolver::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ // This option is registered by OrigIpoptNLP
+ options.GetBoolValue("warm_start_same_structure",
+ warm_start_same_structure_, prefix);
+
+ if (!warm_start_same_structure_) {
+ augsys_tag_ = 0;
+ augmented_system_ = NULL;
+ }
+ else {
+ ASSERT_EXCEPTION(IsValid(augmented_system_), INVALID_WARMSTART,
+ "StdAugSystemSolver called with warm_start_same_structure, but augmented system is not initialized.");
+ }
+
+ return linsolver_->Initialize(Jnlst(), IpNLP(), IpData(), IpCq(),
+ options, prefix);
+ }
+
+ ESymSolverStatus StdAugSystemSolver::MultiSolve(
+ const SymMatrix* W,
+ Number W_factor,
+ const Vector* D_x,
+ Number delta_x,
+ const Vector* D_s,
+ Number delta_s,
+ const Matrix* J_c,
+ const Vector* D_c,
+ Number delta_c,
+ const Matrix* J_d,
+ const Vector* D_d,
+ Number delta_d,
+ std::vector<SmartPtr<const Vector> >& rhs_xV,
+ std::vector<SmartPtr<const Vector> >& rhs_sV,
+ std::vector<SmartPtr<const Vector> >& rhs_cV,
+ std::vector<SmartPtr<const Vector> >& rhs_dV,
+ std::vector<SmartPtr<Vector> >& sol_xV,
+ std::vector<SmartPtr<Vector> >& sol_sV,
+ std::vector<SmartPtr<Vector> >& sol_cV,
+ std::vector<SmartPtr<Vector> >& sol_dV,
+ bool check_NegEVals,
+ Index numberOfNegEVals)
+ {
+ DBG_START_METH("StdAugSystemSolver::MultiSolve",dbg_verbosity);
+ DBG_ASSERT(J_c && J_d && "Currently, you MUST specify J_c and J_d in the augmented system");
+
+ DBG_ASSERT(W_factor == 0.0 || W_factor == 1.0);
+
+ Index nrhs = (Index)rhs_xV.size();
+ DBG_ASSERT(nrhs>0);
+ DBG_ASSERT(nrhs==(Index)rhs_sV.size());
+ DBG_ASSERT(nrhs==(Index)rhs_cV.size());
+ DBG_ASSERT(nrhs==(Index)rhs_dV.size());
+ DBG_ASSERT(nrhs==(Index)sol_xV.size());
+ DBG_ASSERT(nrhs==(Index)sol_sV.size());
+ DBG_ASSERT(nrhs==(Index)sol_cV.size());
+ DBG_ASSERT(nrhs==(Index)sol_dV.size());
+
+ // Create the compound matrix of the augmented system if it has not
+ // yet been created - It is assumed that the structure will not change
+ // after this call
+ DBG_DO(bool debug_first_time_through = false;)
+ if (!IsValid(augmented_system_)) {
+ // pass in the information to form the structure of the augmented system
+ // rhs_? are passed in to provide a prototype vector
+ // for D_? (since these may be NULL)
+ DBG_ASSERT(W && J_c && J_d); // W must exist during the first call to setup the structure!
+ CreateAugmentedSpace(*W, *J_c, *J_d, *rhs_xV[0], *rhs_sV[0],
+ *rhs_cV[0], *rhs_dV[0]);
+ CreateAugmentedSystem(W, W_factor, D_x, delta_x, D_s, delta_s,
+ *J_c, D_c, delta_c, *J_d, D_d, delta_d,
+ *rhs_xV[0], *rhs_sV[0], *rhs_cV[0], *rhs_dV[0]);
+ DBG_DO(debug_first_time_through = true;)
+ }
+
+
+ // Check if anything that was just passed in is different from
+ // what is currently in the compound matrix of the augmented
+ // system. If anything is different, then update the augmented
+ // system
+ if ( AugmentedSystemRequiresChange(W, W_factor, D_x, delta_x, D_s, delta_s,
+ *J_c, D_c, delta_c, *J_d, D_d, delta_d) ) {
+ DBG_ASSERT(!debug_first_time_through);
+ CreateAugmentedSystem(W, W_factor, D_x, delta_x, D_s, delta_s,
+ *J_c, D_c, delta_c, *J_d, D_d, delta_d,
+ *rhs_xV[0], *rhs_sV[0], *rhs_cV[0], *rhs_dV[0]);
+ }
+
+ // Sanity checks
+ DBG_ASSERT(rhs_xV[0]->Dim() == sol_xV[0]->Dim());
+ DBG_ASSERT(rhs_sV[0]->Dim() == sol_sV[0]->Dim());
+ DBG_ASSERT(rhs_cV[0]->Dim() == sol_cV[0]->Dim());
+ DBG_ASSERT(rhs_dV[0]->Dim() == sol_dV[0]->Dim());
+
+ // Now construct the overall right hand side vector that will be passed
+ // to the linear solver
+ std::vector<SmartPtr<const Vector> > augmented_rhsV(nrhs);
+ for (Index i=0; i<nrhs; i++) {
+ SmartPtr<CompoundVector> augrhs =
+ augmented_vector_space_->MakeNewCompoundVector();
+ augrhs->SetComp(0, *rhs_xV[i]);
+ augrhs->SetComp(1, *rhs_sV[i]);
+ augrhs->SetComp(2, *rhs_cV[i]);
+ augrhs->SetComp(3, *rhs_dV[i]);
+ char buffer[16];
+ sprintf(buffer, "RHS[%2d]", i);
+ augrhs->Print(Jnlst(), J_MOREVECTOR, J_LINEAR_ALGEBRA, buffer);
+ augmented_rhsV[i] = GetRawPtr(augrhs);
+ }
+
+ augmented_system_->Print(Jnlst(), J_MATRIX, J_LINEAR_ALGEBRA, "KKT");
+ if (Jnlst().ProduceOutput(J_MOREMATRIX, J_LINEAR_ALGEBRA)) {
+ // ToDo: remove below here - for debug only
+ Index dbg_nz = TripletHelper::GetNumberEntries(*augmented_system_);
+ Index* dbg_iRows = new Index[dbg_nz];
+ Index* dbg_jCols = new Index[dbg_nz];
+ Number* dbg_values = new Number[dbg_nz];
+ TripletHelper::FillRowCol(dbg_nz, *augmented_system_, dbg_iRows, dbg_jCols);
+ TripletHelper::FillValues(dbg_nz, *augmented_system_, dbg_values);
+ Jnlst().Printf(J_MOREMATRIX, J_LINEAR_ALGEBRA, "******* KKT SYSTEM *******\n");
+ for (Index dbg_i=0; dbg_i<dbg_nz; dbg_i++) {
+ Jnlst().Printf(J_MOREMATRIX, J_LINEAR_ALGEBRA, "(%d) KKT[%d][%d] = %23.15e\n", dbg_i, dbg_iRows[dbg_i], dbg_jCols[dbg_i], dbg_values[dbg_i]);
+ }
+ delete [] dbg_iRows;
+ dbg_iRows = NULL;
+ delete [] dbg_jCols;
+ dbg_jCols = NULL;
+ delete [] dbg_values;
+ dbg_values = NULL;
+ // ToDo: remove above here
+ }
+
+ // Call the linear solver
+ std::vector<SmartPtr<Vector> > augmented_solV(nrhs);
+ for (Index i=0; i<nrhs; i++) {
+ SmartPtr<CompoundVector> augsol =
+ augmented_vector_space_->MakeNewCompoundVector();
+ augsol->SetCompNonConst(0, *sol_xV[i]);
+ augsol->SetCompNonConst(1, *sol_sV[i]);
+ augsol->SetCompNonConst(2, *sol_cV[i]);
+ augsol->SetCompNonConst(3, *sol_dV[i]);
+ augmented_solV[i] = GetRawPtr(augsol);
+ }
+ ESymSolverStatus retval;
+ retval = linsolver_->MultiSolve(*augmented_system_, augmented_rhsV,
+ augmented_solV, check_NegEVals,
+ numberOfNegEVals);
+ if (retval==SYMSOLVER_SUCCESS) {
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA, "Factorization successful.\n");
+ for (Index i=0; i<nrhs; i++) {
+ char buffer[16];
+ sprintf(buffer, "SOL[%2d]", i);
+ augmented_solV[i]->Print(Jnlst(), J_MOREVECTOR, J_LINEAR_ALGEBRA,
+ buffer);
+ }
+ }
+ else if (retval==SYMSOLVER_FATAL_ERROR) {
+ THROW_EXCEPTION(FATAL_ERROR_IN_LINEAR_SOLVER,"A fatal error occured in the linear solver.");
+ }
+ else {
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA, "Factorization failed with retval = %d\n", retval);
+ }
+
+ return retval;
+ }
+
+ void StdAugSystemSolver::CreateAugmentedSpace(const SymMatrix& W,
+ const Matrix& J_c,
+ const Matrix& J_d,
+ const Vector& proto_x,
+ const Vector& proto_s,
+ const Vector& proto_c,
+ const Vector& proto_d)
+ {
+ DBG_ASSERT(!IsValid(augmented_system_));
+
+ old_w_ = &W;
+
+ //===
+ // Setup the augmented system matrix (described in IpAugSystemSolver.hpp")
+ //===
+
+ // created the compound symmetric matrix space
+ Index n_x = J_c.NCols();
+ Index n_s = J_d.NRows();
+ Index n_c = J_c.NRows();
+ Index n_d = n_s;
+
+ Index total_nRows = n_x + n_s + n_c + n_d;
+ augmented_system_space_ = new CompoundSymMatrixSpace(4, total_nRows);
+ augmented_system_space_->SetBlockDim(0, n_x);
+ augmented_system_space_->SetBlockDim(1, n_s);
+ augmented_system_space_->SetBlockDim(2, n_c);
+ augmented_system_space_->SetBlockDim(3, n_d);
+
+ // (1,1) block
+ // create the spaces and sum matrix for the upper left corner (W + D_x delta_x*I)
+ // of the hessian part for the 1,1 block
+ diag_space_x_ = new DiagMatrixSpace(n_x);
+
+ sumsym_space_x_ = new SumSymMatrixSpace(n_x, 2);
+ sumsym_space_x_->SetTermSpace(0, *W.OwnerSymMatrixSpace());
+ sumsym_space_x_->SetTermSpace(1, *diag_space_x_);
+ augmented_system_space_->SetCompSpace(0,0, *sumsym_space_x_);
+
+
+ // (2,2) block
+ // create the spaces and diag matrix for the lower right corner (D_s + delta_s*I)
+ // of the hessian part, the 2,2 block
+ diag_space_s_ = new DiagMatrixSpace(n_s);
+ augmented_system_space_->SetCompSpace(1,1, *diag_space_s_);
+
+ // (3,1) block
+ augmented_system_space_->SetCompSpace(2,0, *J_c.OwnerSpace());
+
+ // (3,3) block
+ // create the matrix space and matrix for the 3,3 block
+ diag_space_c_ = new DiagMatrixSpace(n_c);
+ augmented_system_space_->SetCompSpace(2,2, *diag_space_c_);
+
+ // (4,1) block
+ augmented_system_space_->SetCompSpace(3,0, *J_d.OwnerSpace());
+
+ // (4,2) block
+ // create the identity matrix space and matrix for the 4,2 block
+ ident_space_ds_ = new IdentityMatrixSpace(n_s);
+ augmented_system_space_->SetCompSpace(3,1, *ident_space_ds_);
+
+ // (4,4) block
+ // create the sum matrix space and matrix for the 4,4 block
+ diag_space_d_ = new DiagMatrixSpace(n_d);
+ augmented_system_space_->SetCompSpace(3,3, *diag_space_d_);
+
+ // Create the space for the vectors
+ augmented_vector_space_ = new CompoundVectorSpace(4, n_x + n_s + n_c + n_d);
+ augmented_vector_space_->SetCompSpace(0, *proto_x.OwnerSpace());
+ augmented_vector_space_->SetCompSpace(1, *proto_s.OwnerSpace());
+ augmented_vector_space_->SetCompSpace(2, *proto_c.OwnerSpace());
+ augmented_vector_space_->SetCompSpace(3, *proto_d.OwnerSpace());
+
+ }
+
+ void StdAugSystemSolver::CreateAugmentedSystem(
+ const SymMatrix* W,
+ Number W_factor,
+ const Vector* D_x,
+ Number delta_x,
+ const Vector* D_s,
+ Number delta_s,
+ const Matrix& J_c,
+ const Vector* D_c,
+ Number delta_c,
+ const Matrix& J_d,
+ const Vector* D_d,
+ Number delta_d,
+ const Vector& proto_x,
+ const Vector& proto_s,
+ const Vector& proto_c,
+ const Vector& proto_d)
+ {
+ augmented_system_ = augmented_system_space_->MakeNewCompoundSymMatrix();
+
+ // (1,1) block
+ SmartPtr<SumSymMatrix> sumsym_x = sumsym_space_x_->MakeNewSumSymMatrix();
+
+ if (W) {
+ sumsym_x->SetTerm(0, W_factor, *W);
+ old_w_ = W;
+ w_tag_ = W->GetTag();
+ }
+ else {
+ sumsym_x->SetTerm(0, 0.0, *old_w_);
+ w_tag_ = 0;
+ }
+ w_factor_ = W_factor;
+
+ SmartPtr<DiagMatrix> diag_x = diag_space_x_->MakeNewDiagMatrix();
+ if (D_x) {
+ if (delta_x==0.) {
+ diag_x->SetDiag(*D_x);
+ }
+ else {
+ SmartPtr<Vector> tmp = D_x->MakeNewCopy();
+ tmp->AddScalar(delta_x);
+ diag_x->SetDiag(*tmp);
+ }
+ d_x_tag_ = D_x->GetTag();
+ }
+ else {
+ SmartPtr<Vector> tmp = proto_x.MakeNew();
+ tmp->Set(delta_x);
+ diag_x->SetDiag(*tmp);
+ d_x_tag_ = 0;
+ }
+ sumsym_x->SetTerm(1, 1.0, *diag_x);
+ delta_x_ = delta_x;
+
+ augmented_system_->SetComp(0,0, *sumsym_x);
+
+ // (2,2) block
+ SmartPtr<DiagMatrix> diag_s = diag_space_s_->MakeNewDiagMatrix();
+ if (D_s) {
+ if (delta_s==0.) {
+ diag_s->SetDiag(*D_s);
+ }
+ else {
+ SmartPtr<Vector> tmp = D_s->MakeNewCopy();
+ tmp->AddScalar(delta_s);
+ diag_s->SetDiag(*tmp);
+ }
+ d_s_tag_ = D_s->GetTag();
+ }
+ else {
+ SmartPtr<Vector> tmp = proto_s.MakeNew();
+ tmp->Set(delta_s);
+ diag_s->SetDiag(*tmp);
+ d_s_tag_ = 0;
+ }
+ delta_s_ = delta_s;
+
+ augmented_system_->SetComp(1, 1, *diag_s);
+
+ // (3,1) block
+ augmented_system_->SetComp(2,0, J_c);
+ j_c_tag_ = J_c.GetTag();
+
+ // (3,3) block
+ SmartPtr<DiagMatrix> diag_c = diag_space_c_->MakeNewDiagMatrix();
+ if (D_c) {
+ if (delta_c==0.) {
+ diag_c->SetDiag(*D_c);
+ }
+ else {
+ SmartPtr<Vector> tmp = D_c->MakeNewCopy();
+ tmp->AddScalar(-delta_c);
+ diag_c->SetDiag(*tmp);
+ }
+ d_c_tag_ = D_c->GetTag();
+ }
+ else {
+ SmartPtr<Vector> tmp = proto_c.MakeNew();
+ tmp->Set(-delta_c);
+ diag_c->SetDiag(*tmp);
+ d_c_tag_ = 0;
+ }
+ delta_c_ = delta_c;
+
+ augmented_system_->SetComp(2,2, *diag_c);
+
+ // (4,1) block
+ augmented_system_->SetComp(3,0, J_d);
+ j_d_tag_ = J_d.GetTag();
+
+ // (4,2) block
+ SmartPtr<IdentityMatrix> ident_ds = ident_space_ds_->MakeNewIdentityMatrix();
+ ident_ds->SetFactor(-1.0);
+ augmented_system_->SetComp(3,1, *ident_ds);
+
+ // (4,4) block
+ SmartPtr<DiagMatrix> diag_d = diag_space_d_->MakeNewDiagMatrix();
+ if (D_d) {
+ if (delta_d==0.) {
+ diag_d->SetDiag(*D_d);
+ }
+ else {
+ SmartPtr<Vector> tmp = D_d->MakeNewCopy();
+ tmp->AddScalar(-delta_d);
+ diag_d->SetDiag(*tmp);
+ }
+ d_d_tag_ = D_d->GetTag();
+ }
+ else {
+ SmartPtr<Vector> tmp = proto_d.MakeNew();
+ tmp->Set(-delta_d);
+ diag_d->SetDiag(*tmp);
+ d_d_tag_ = 0;
+ }
+ delta_d_ = delta_d;
+
+ augmented_system_->SetComp(3,3, *diag_d);
+
+ augsys_tag_ = augmented_system_->GetTag();
+ }
+
+
+ bool StdAugSystemSolver::AugmentedSystemRequiresChange(
+ const SymMatrix* W,
+ Number W_factor,
+ const Vector* D_x,
+ Number delta_x,
+ const Vector* D_s,
+ Number delta_s,
+ const Matrix& J_c,
+ const Vector* D_c,
+ Number delta_c,
+ const Matrix& J_d,
+ const Vector* D_d,
+ Number delta_d)
+ {
+ DBG_START_METH("StdAugSystemSolver::AugmentedSystemRequiresChange",dbg_verbosity);
+ DBG_ASSERT(augsys_tag_ == augmented_system_->GetTag() && "Someone has changed the augmented system outside of the AugSystemSolver. This should NOT happen.");
+
+#ifdef IP_DEBUG
+
+ bool Wtest = (W && W->GetTag() != w_tag_);
+ bool iWtest = (!W && w_tag_ != 0);
+ bool wfactor_test = (W_factor != w_factor_);
+ bool D_xtest = (D_x && D_x->GetTag() != d_x_tag_);
+ bool iD_xtest = (!D_x && d_x_tag_ != 0);
+ bool delta_xtest = (delta_x != delta_x_);
+ bool D_stest = (D_s && D_s->GetTag() != d_s_tag_);
+ bool iD_stest = (!D_s && d_s_tag_ != 0);
+ bool delta_stest = (delta_s != delta_s_);
+ bool J_ctest = (J_c.GetTag() != j_c_tag_);
+ bool D_ctest = (D_c && D_c->GetTag() != d_c_tag_);
+ bool iD_ctest = (!D_c && d_c_tag_ != 0);
+ bool delta_ctest = (delta_c != delta_c_);
+ bool J_dtest = (J_d.GetTag() != j_d_tag_);
+ bool D_dtest = (D_d && D_d->GetTag() != d_d_tag_);
+ bool iD_dtest = (!D_d && d_d_tag_ != 0);
+ bool delta_dtest = (delta_d != delta_d_);
+#endif
+
+ DBG_PRINT((2,"Wtest = %d\n", Wtest));
+ DBG_PRINT((2,"iWtest = %d\n", iWtest));
+ DBG_PRINT((2,"wfactor_test = %d\n", wfactor_test));
+ DBG_PRINT((2,"D_xtest = %d\n", D_xtest));
+ DBG_PRINT((2,"iD_xtest = %d\n", iD_xtest));
+ DBG_PRINT((2,"delta_xtest = %d\n", delta_xtest));
+ DBG_PRINT((2,"D_stest = %d\n", D_stest));
+ DBG_PRINT((2,"iD_stest = %d\n", iD_stest));
+ DBG_PRINT((2,"delta_stest = %d\n", delta_stest));
+ DBG_PRINT((2,"J_ctest = %d\n", J_ctest));
+ DBG_PRINT((2,"D_ctest = %d\n", D_ctest));
+ DBG_PRINT((2,"iD_ctest = %d\n", iD_ctest));
+ DBG_PRINT((2,"delta_ctest = %d\n", delta_ctest));
+ DBG_PRINT((2,"J_dtest = %d\n", J_dtest));
+ DBG_PRINT((2,"D_dtest = %d\n", D_dtest));
+ DBG_PRINT((2,"iD_dtest = %d\n", iD_dtest));
+ DBG_PRINT((2,"delta_dtest = %d\n", delta_dtest));
+
+ if ( (W && W->GetTag() != w_tag_)
+ || (!W && w_tag_ != 0)
+ || (W_factor != w_factor_)
+ || (D_x && D_x->GetTag() != d_x_tag_)
+ || (!D_x && d_x_tag_ != 0)
+ || (delta_x != delta_x_)
+ || (D_s && D_s->GetTag() != d_s_tag_)
+ || (!D_s && d_s_tag_ != 0)
+ || (delta_s != delta_s_)
+ || (J_c.GetTag() != j_c_tag_)
+ || (D_c && D_c->GetTag() != d_c_tag_)
+ || (!D_c && d_c_tag_ != 0)
+ || (delta_c != delta_c_)
+ || (J_d.GetTag() != j_d_tag_)
+ || (D_d && D_d->GetTag() != d_d_tag_)
+ || (!D_d && d_d_tag_ != 0)
+ || (delta_d != delta_d_) ) {
+ return true;
+ }
+
+ return false;
+ }
+
+ Index StdAugSystemSolver::NumberOfNegEVals() const
+ {
+ DBG_ASSERT(IsValid(augmented_system_));
+ return linsolver_->NumberOfNegEVals();
+ }
+
+ bool StdAugSystemSolver::ProvidesInertia() const
+ {
+ return linsolver_->ProvidesInertia();
+ }
+
+ bool StdAugSystemSolver::IncreaseQuality()
+ {
+ return linsolver_->IncreaseQuality();
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpStdAugSystemSolver.hpp b/SimTKmath/Optimizers/src/IpOpt/IpStdAugSystemSolver.hpp
new file mode 100644
index 0000000..fb9f1ee
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpStdAugSystemSolver.hpp
@@ -0,0 +1,253 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpStdAugSystemSolver.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IP_STDAUGSYSTEMSOLVER_HPP__
+#define __IP_STDAUGSYSTEMSOLVER_HPP__
+
+#include "IpAugSystemSolver.hpp"
+#include "IpCompoundMatrix.hpp"
+#include "IpCompoundSymMatrix.hpp"
+#include "IpCompoundVector.hpp"
+#include "IpSumSymMatrix.hpp"
+#include "IpDiagMatrix.hpp"
+#include "IpIdentityMatrix.hpp"
+
+namespace Ipopt
+{
+
+ DECLARE_STD_EXCEPTION(FATAL_ERROR_IN_LINEAR_SOLVER);
+
+ /** Solver for the augmented system for triple type matrices.
+ *
+ * The current implemetation assumes that all matrices are of the
+ * type SymTMatrix, and all vectors are of the type DenseVector.
+ */
+ class StdAugSystemSolver : public AugSystemSolver
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor using only a linear solver object */
+ StdAugSystemSolver(SymLinearSolver& LinSolver);
+
+ /** Default destructor */
+ virtual ~StdAugSystemSolver();
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Set up the augmented system and solve it for a set of given
+ * right hand side - implementation for GenTMatrices and
+ * SymTMatrices.
+ */
+ virtual ESymSolverStatus MultiSolve(
+ const SymMatrix* W,
+ Number W_factor,
+ const Vector* D_x,
+ Number delta_x,
+ const Vector* D_s,
+ Number delta_s,
+ const Matrix* J_c,
+ const Vector* D_c,
+ Number delta_c,
+ const Matrix* J_d,
+ const Vector* D_d,
+ Number delta_d,
+ std::vector<SmartPtr<const Vector> >& rhs_xV,
+ std::vector<SmartPtr<const Vector> >& rhs_sV,
+ std::vector<SmartPtr<const Vector> >& rhs_cV,
+ std::vector<SmartPtr<const Vector> >& rhs_dV,
+ std::vector<SmartPtr<Vector> >& sol_xV,
+ std::vector<SmartPtr<Vector> >& sol_sV,
+ std::vector<SmartPtr<Vector> >& sol_cV,
+ std::vector<SmartPtr<Vector> >& sol_dV,
+ bool check_NegEVals,
+ Index numberOfNegEVals);
+
+ /** Number of negative eigenvalues detected during last
+ * solve. Returns the number of negative eigenvalues of
+ * the most recent factorized matrix. This must not be called if
+ * the linear solver does not compute this quantities (see
+ * ProvidesInertia).
+ */
+ virtual Index NumberOfNegEVals() const;
+
+ /** Query whether inertia is computed by linear solver.
+ * Returns true, if linear solver provides inertia.
+ */
+ virtual bool ProvidesInertia() const;
+
+ /** Request to increase quality of solution for next solve. Ask
+ * underlying linear solver to increase quality of solution for
+ * the next solve (e.g. increase pivot tolerance). Returns
+ * false, if this is not possible (e.g. maximal pivot tolerance
+ * already used.)
+ */
+ virtual bool IncreaseQuality();
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default constructor. */
+ StdAugSystemSolver();
+ /** Copy Constructor */
+ StdAugSystemSolver(const StdAugSystemSolver&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const StdAugSystemSolver&);
+ //@}
+
+
+ /** Create the matrix space for the Compound Sym Matrix that
+ * represents the augmented system. This signifies the "first"
+ * time through and requires all structural knowledge */
+ void CreateAugmentedSpace(const SymMatrix& W,
+ const Matrix& J_c,
+ const Matrix& J_d,
+ const Vector& proto_x,
+ const Vector& proto_s,
+ const Vector& proto_c,
+ const Vector& proto_d);
+
+
+ /** Create the new compound sym matrix that represents the
+ * augmented system. This is done EVERY time Solve is called
+ * with ANY different information */
+ void CreateAugmentedSystem(const SymMatrix* W,
+ Number W_factor,
+ const Vector* D_x,
+ Number delta_x,
+ const Vector* D_s,
+ Number delta_s,
+ const Matrix& J_c,
+ const Vector* D_c,
+ Number delta_c,
+ const Matrix& J_d,
+ const Vector* D_d,
+ Number delta_d,
+ const Vector& proto_x,
+ const Vector& proto_s,
+ const Vector& proto_c,
+ const Vector& proto_d);
+
+ /** Check the internal tags and decide if the passed variables are
+ * different from what is in the augmented_system_ */
+ bool AugmentedSystemRequiresChange(const SymMatrix* W,
+ Number W_factor,
+ const Vector* D_x,
+ Number delta_x,
+ const Vector* D_s,
+ Number delta_s,
+ const Matrix& J_c,
+ const Vector* D_c,
+ Number delta_c,
+ const Matrix& J_d,
+ const Vector* D_d,
+ Number delta_d);
+
+ /** The linear solver object that is to be used to solve the
+ * linear systems.
+ */
+ SmartPtr<SymLinearSolver> linsolver_;
+
+ /** Spaces for piecing together the augmented system */
+ SmartPtr<CompoundSymMatrixSpace> augmented_system_space_;
+ SmartPtr<SumSymMatrixSpace> sumsym_space_x_;
+ SmartPtr<DiagMatrixSpace> diag_space_x_;
+ SmartPtr<DiagMatrixSpace> diag_space_s_;
+ SmartPtr<DiagMatrixSpace> diag_space_c_;
+ SmartPtr<IdentityMatrixSpace> ident_space_ds_;
+ SmartPtr<DiagMatrixSpace> diag_space_d_;
+
+ SmartPtr<CompoundVectorSpace> augmented_vector_space_;
+
+ /**@name Tags and values to track in order to decide whether the
+ matrix has to be updated compared to the most recent call of
+ the Set method.
+ */
+ //@{
+ /** Tag for W matrix. If W has been given to Set as NULL, then
+ * this tag is set to 0
+ */
+ TaggedObject::Tag w_tag_;
+ /** Most recent value of W_factor */
+ double w_factor_;
+ /** Tag for D_x vector, representing the diagonal matrix D_x. If
+ * D_x has been given to Set as NULL, then this tag is set to 0
+ */
+ TaggedObject::Tag d_x_tag_;
+ /** Most recent value of delta_x from Set method */
+ double delta_x_;
+ /** Tag for D_s vector, representing the diagonal matrix D_s. If
+ * D_s has been given to Set as NULL, then this tag is set to 0
+ */
+ TaggedObject::Tag d_s_tag_;
+ /** Most recent value of delta_s from Set method */
+ double delta_s_;
+ /** Tag for J_c matrix. If J_c has been given to Set as NULL, then
+ * this tag is set to 0
+ */
+ TaggedObject::Tag j_c_tag_;
+ /** Tag for D_c vector, representing the diagonal matrix D_c. If
+ * D_c has been given to Set as NULL, then this tag is set to 0
+ */
+ TaggedObject::Tag d_c_tag_;
+ /** Most recent value of delta_c from Set method */
+ double delta_c_;
+ /** Tag for J_d matrix. If J_d has been given to Set as NULL, then
+ * this tag is set to 0
+ */
+ TaggedObject::Tag j_d_tag_;
+ /** Tag for D_d vector, representing the diagonal matrix D_d. If
+ * D_d has been given to Set as NULL, then this tag is set to 0
+ */
+ TaggedObject::Tag d_d_tag_;
+ /** Most recent value of delta_d from Set method */
+ double delta_d_;
+
+ /** This is the tag of the matrix storing the augmented system. Since
+ * this object owns this matrix, no changes should happen outside.
+ * However, since it is given away as a smart pointer, someone outside
+ * might change it. For debugging purposes, we now track its tag as
+ * well.
+ */
+ TaggedObject::Tag augsys_tag_;
+ //@}
+
+ /** The resulting augmented matrix.
+ * This matrix is stored as follows: First we have the diagonal elements
+ * for the upper left block (for D_W and delta_W), then the elements for
+ * the Hessian W, then the Jacobian A, and finally the diagonal elements
+ * for the lower right block (for D_C and delta_C).
+ */
+ SmartPtr<CompoundSymMatrix> augmented_system_;
+
+ /** A copy of a previous W used in the augmented_system_. Since Solve can
+ * be called with a NULL W, we keep a copy of the last W passed to keep
+ * the nonzero structure of the augmented_system_ consistent */
+ SmartPtr<const SymMatrix> old_w_;
+
+ /** @name Algorithmic parameters */
+ //@{
+ /** Flag indicating whether the TNLP with identical structure has
+ * already been solved before. */
+ bool warm_start_same_structure_;
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpStdCInterface.cpp b/SimTKmath/Optimizers/src/IpOpt/IpStdCInterface.cpp
new file mode 100644
index 0000000..fec38a5
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpStdCInterface.cpp
@@ -0,0 +1,221 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpStdCInterface.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpStdCInterface.h"
+#include "IpStdInterfaceTNLP.hpp"
+#include "IpOptionsList.hpp"
+#include "IpIpoptApplication.hpp"
+
+struct IpoptProblemInfo
+{
+ Index n;
+ Number* x_L;
+ Number* x_U;
+ Index m;
+ Number* g_L;
+ Number* g_U;
+ Index nele_jac;
+ Index nele_hess;
+ Index index_style;
+ Eval_F_CB eval_f;
+ Eval_G_CB eval_g;
+ Eval_Grad_F_CB eval_grad_f;
+ Eval_Jac_G_CB eval_jac_g;
+ Eval_H_CB eval_h;
+ Ipopt::SmartPtr<Ipopt::IpoptApplication> app;
+};
+
+IpoptProblem CreateIpoptProblem(
+ Index n,
+ Number* x_L,
+ Number* x_U,
+ Index m,
+ Number* g_L,
+ Number* g_U,
+ Index nele_jac,
+ Index nele_hess,
+ Index index_style,
+ Eval_F_CB eval_f,
+ Eval_G_CB eval_g,
+ Eval_Grad_F_CB eval_grad_f,
+ Eval_Jac_G_CB eval_jac_g,
+ Eval_H_CB eval_h)
+{
+ // make sure input is Ok
+ if (n<1 || m<0 || !x_L || !x_U || (m>0 && (!g_L || !g_U)) ||
+ (m==0 && nele_jac != 0) || (m>0 && nele_jac < 1) || nele_hess < 0 ||
+ !eval_f || !eval_grad_f || (m>0 && (!eval_g || !eval_jac_g))) {
+ return NULL;
+ }
+
+ IpoptProblem retval = new IpoptProblemInfo;
+
+ retval->n = n;
+ retval->x_L = new Number[n];
+ for (Index i=0; i<n; i++) {
+ retval->x_L[i] = x_L[i];
+ }
+ retval->x_U = new Number[n];
+ for (Index i=0; i<n; i++) {
+ retval->x_U[i] = x_U[i];
+ }
+
+ retval->m = m;
+ if (m>0) {
+ retval->g_L = new Number[m];
+ for (Index i=0; i<m; i++) {
+ retval->g_L[i] = g_L[i];
+ }
+ retval->g_U = new Number[m];
+ for (Index i=0; i<m; i++) {
+ retval->g_U[i] = g_U[i];
+ }
+ }
+ else {
+ retval->g_L = NULL;
+ retval->g_U = NULL;
+ }
+
+ retval->nele_jac = nele_jac;
+ retval->nele_hess = nele_hess;
+ retval->index_style = index_style;
+ retval->eval_f = eval_f;
+ retval->eval_g = eval_g;
+ retval->eval_grad_f = eval_grad_f;
+ retval->eval_jac_g = eval_jac_g;
+ retval->eval_h = eval_h;
+
+ retval->app = new Ipopt::IpoptApplication();
+
+ return retval;
+}
+
+void FreeIpoptProblem(IpoptProblem ipopt_problem)
+{
+ delete [] ipopt_problem->x_L;
+ delete [] ipopt_problem->x_U;
+ if (ipopt_problem->m>0) {
+ delete [] ipopt_problem->g_L;
+ delete [] ipopt_problem->g_U;
+ }
+
+ ipopt_problem->app = NULL;
+
+ delete ipopt_problem;
+}
+
+
+Bool AddIpoptStrOption(IpoptProblem ipopt_problem, const char* keyword, const char* val)
+{
+ std::string tag(keyword);
+ std::string value(val);
+ return (Bool) ipopt_problem->app->Options()->SetStringValue(tag, value);
+}
+
+Bool AddIpoptNumOption(IpoptProblem ipopt_problem, const char* keyword, Number val)
+{
+ std::string tag(keyword);
+ Ipopt::Number value=val;
+ return (Bool) ipopt_problem->app->Options()->SetNumericValue(tag, value);
+}
+
+Bool AddIpoptIntOption(IpoptProblem ipopt_problem, const char* keyword, Int val)
+{
+ std::string tag(keyword);
+ Ipopt::Index value=val;
+ return (Bool) ipopt_problem->app->Options()->SetIntegerValue(tag, value);
+}
+
+Bool OpenIpoptOutputFile(IpoptProblem ipopt_problem, char* file_name,
+ Int print_level)
+{
+ std::string name(file_name);
+ Ipopt::EJournalLevel level = Ipopt::EJournalLevel(print_level);
+ return (Bool) ipopt_problem->app->OpenOutputFile(name, level);
+}
+
+enum ApplicationReturnStatus IpoptSolve(
+ IpoptProblem ipopt_problem,
+ Number* x,
+ Number* g,
+ Number* obj_val,
+ Number* mult_g,
+ Number* mult_x_L,
+ Number* mult_x_U,
+ UserDataPtr user_data)
+{
+ using namespace Ipopt;
+
+ // Initialize and process options
+ ipopt_problem->app->Initialize();
+
+ // For now only copy the values of the x's. When we allow warm
+ // starts we also need to copy the values of the multipliers
+ ::Number* start_x = new ::Number[ipopt_problem->n];
+ for (::Index i=0; i<ipopt_problem->n; i++) {
+ start_x[i] = x[i];
+ }
+
+ // Copy values of multipliers to support warm starts - Eran, 05/07
+ ::Number* start_mult_g = NULL;
+ ::Number* start_mult_x_L = NULL;
+ ::Number* start_mult_x_U = NULL;
+ if(mult_g) {
+ start_mult_g = new ::Number[ipopt_problem->m];
+ for (::Index i=0; i<ipopt_problem->m; i++) start_mult_g[i] = mult_g[i];
+ }
+ if(mult_x_L) {
+ start_mult_x_L = new ::Number[ipopt_problem->n];
+ for (::Index i=0; i<ipopt_problem->n; i++) start_mult_x_L[i] = mult_x_L[i];
+ }
+ if(mult_x_U) {
+ start_mult_x_U = new ::Number[ipopt_problem->n];
+ for (::Index i=0; i<ipopt_problem->n; i++) start_mult_x_U[i] = mult_x_U[i];
+ }
+
+
+ // Create the original nlp
+ SmartPtr<TNLP> tnlp;
+
+ bool skip_optimize = false;
+ try {
+ tnlp = new StdInterfaceTNLP(ipopt_problem->n, ipopt_problem->x_L,
+ ipopt_problem->x_U, ipopt_problem->m,
+ ipopt_problem->g_L, ipopt_problem->g_U,
+ ipopt_problem->nele_jac,
+ ipopt_problem->nele_hess,
+ ipopt_problem->index_style,
+ start_x, start_mult_g, start_mult_x_L, start_mult_x_U,
+ ipopt_problem->eval_f, ipopt_problem->eval_g,
+ ipopt_problem->eval_grad_f,
+ ipopt_problem->eval_jac_g,
+ ipopt_problem->eval_h,
+ x, mult_x_L, mult_x_U, g, mult_g,
+ obj_val, user_data);
+ }
+ catch(INVALID_STDINTERFACE_NLP& exc) {
+ exc.ReportException(*ipopt_problem->app->Jnlst(), J_ERROR);
+ skip_optimize = true;
+ }
+
+ Ipopt::ApplicationReturnStatus status;
+ if (!skip_optimize) {
+ status = ipopt_problem->app->OptimizeTNLP(tnlp);
+ }
+ else {
+ status = Ipopt::Invalid_Problem_Definition;
+ }
+
+ delete [] start_x;
+ delete [] start_mult_g;
+ delete [] start_mult_x_L;
+ delete [] start_mult_x_U;
+
+ return (::ApplicationReturnStatus) status;
+}
+
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpStdCInterface.h b/SimTKmath/Optimizers/src/IpOpt/IpStdCInterface.h
new file mode 100644
index 0000000..ede7aa1
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpStdCInterface.h
@@ -0,0 +1,216 @@
+/*************************************************************************
+ Copyright (C) 2004, 2006 International Business Machines and others.
+ All Rights Reserved.
+ This code is published under the Common Public License.
+
+ $Id: IpStdCInterface.h 759 2006-07-07 03:07:08Z andreasw $
+
+ Authors: Carl Laird, Andreas Waechter IBM 2004-09-02
+ *************************************************************************/
+
+#ifndef __IPSTDCINTERFACE_H__
+#define __IPSTDCINTERFACE_H__
+
+#include "SimTKcommon/internal/common.h"
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+ /** Type for all number. We need to make sure that this is
+ identical with what is defined in Common/IpTypes.hpp */
+ typedef SimTK::Real Number;
+
+ /** Type for all incides. We need to make sure that this is
+ identical with what is defined in Common/IpTypes.hpp */
+ typedef int Index;
+
+ /** Type for all integers. We need to make sure that this is
+ identical with what is defined in Common/IpTypes.hpp */
+ typedef int Int;
+
+ /* This includes the SolverReturn enum type */
+#include "IpReturnCodes.h"
+
+ /** Structure collecting all information about the problem
+ * definition and solve statistics etc. This is defined in the
+ * source file. */
+ struct IpoptProblemInfo;
+
+ /** Pointer to a Ipopt Problem. */
+ typedef struct IpoptProblemInfo* IpoptProblem;
+
+ /** define a boolean type for C */
+ typedef int Bool;
+#ifndef TRUE
+# define TRUE (1)
+#endif
+#ifndef FALSE
+# define FALSE (0)
+#endif
+
+ /** A pointer for anything that is to be passed between the called
+ * and individual callback function */
+ typedef void* UserDataPtr;
+
+ /** Type defining the callback function for evaluating the value of
+ * the objective function. Return value should be set to false if
+ * there was a problem doing the evaluation. */
+ typedef Bool (*Eval_F_CB)(Index n, const Number* x, Bool new_x,
+ Number* obj_value, UserDataPtr user_data);
+
+ /** Type defining the callback function for evaluating the gradient of
+ * the objective function. Return value should be set to false if
+ * there was a problem doing the evaluation. */
+ typedef Bool (*Eval_Grad_F_CB)(Index n, const Number* x, Bool new_x,
+ Number* grad_f, UserDataPtr user_data);
+
+ /** Type defining the callback function for evaluating the value of
+ * the constraint functions. Return value should be set to false if
+ * there was a problem doing the evaluation. */
+ typedef Bool (*Eval_G_CB)(Index n, const Number* x, Bool new_x,
+ Index m, Number* g, UserDataPtr user_data);
+
+ /** Type defining the callback function for evaluating the Jacobian of
+ * the constrant functions. Return value should be set to false if
+ * there was a problem doing the evaluation. */
+ typedef Bool (*Eval_Jac_G_CB)(Index n, const Number *x, Bool new_x,
+ Index m, Index nele_jac,
+ Index *iRow, Index *jCol, Number *values,
+ UserDataPtr user_data);
+
+ /** Type defining the callback function for evaluating the Hessian of
+ * the Lagrangian function. Return value should be set to false if
+ * there was a problem doing the evaluation. */
+ typedef Bool (*Eval_H_CB)(Index n, const Number *x, Bool new_x, Number obj_factor,
+ Index m, Number *lambda, Bool new_lambda,
+ Index nele_hess, Index *iRow, Index *jCol,
+ Number *values, UserDataPtr user_data);
+
+ /** Function for creating a new Ipopt Problem object. This function
+ * returns an object that can be passed to the IpoptSolve call. It
+ * contains the basic definition of the optimization problem, such
+ * as number of variables and constraints, bounds on variables and
+ * constraints, information about the derivatives, and the callback
+ * function for the computation of the optimization problem
+ * functions and derivatives. During this call, the options file
+ * PARAMS.DAT is read as well.
+ *
+ * If NULL is returned, there was a problem with one of the inputs
+ * or reading the options file. */
+ IpoptProblem CreateIpoptProblem(
+ Index n /** Number of optimization variables */
+ , Number* x_L /** Lower bounds on variables. This array of
+ size n is copied internally, so that the
+ caller can change the incoming data after
+ return without that IpoptProblem is
+ modified. Any value less or equal than
+ the number specified by option
+ 'nlp_lower_bound_inf' is interpreted to
+ be minus infinity. */
+ , Number* x_U /** Upper bounds on variables. This array of
+ size n is copied internally, so that the
+ caller can change the incoming data after
+ return without that IpoptProblem is
+ modified. Any value greater or equal
+ than the number specified by option
+ 'nlp_upper_bound_inf' is interpreted to
+ be plus infinity. */
+ , Index m /** Number of constraints. */
+ , Number* g_L /** Lower bounds on constraints. This array of
+ size m is copied internally, so that the
+ caller can change the incoming data after
+ return without that IpoptProblem is
+ modified. Any value less or equal than
+ the number specified by option
+ 'nlp_lower_bound_inf' is interpreted to
+ be minus infinity. */
+ , Number* g_U /** Upper bounds on constraints. This array of
+ size m is copied internally, so that the
+ caller can change the incoming data after
+ return without that IpoptProblem is
+ modified. Any value greater or equal
+ than the number specified by option
+ 'nlp_upper_bound_inf' is interpreted to
+ be plus infinity. */
+ , Index nele_jac /** Number of non-zero elements in constraint
+ Jacobian. */
+ , Index nele_hess /** Number of non-zero elements in Hessian of
+ Lagrangian. */
+ , Index index_style /** indexing style for iRow & jCol,
+ 0 for C style, 1 for Fortran style */
+ , Eval_F_CB eval_f /** Callback function for evaluating
+ objective function */
+ , Eval_G_CB eval_g /** Callback function for evaluating
+ constraint functions */
+ , Eval_Grad_F_CB eval_grad_f
+ /** Callback function for evaluating gradient
+ of objective function */
+ , Eval_Jac_G_CB eval_jac_g
+ /** Callback function for evaluating Jacobian
+ of constraint functions */
+ , Eval_H_CB eval_h /** Callback function for evaluating Hessian
+ of Lagrangian function */
+ );
+
+ /** Method for freeing a previously created IpoptProblem. After
+ freeing an IpoptProblem, it cannot be used anymore. */
+ void FreeIpoptProblem(IpoptProblem ipopt_problem);
+
+
+ /** Function for adding a string option. Returns FALSE the option
+ * could not be set (e.g., if keyword is unknown) */
+ Bool AddIpoptStrOption(IpoptProblem ipopt_problem, const char* keyword, const char* val);
+
+ /** Function for adding a Number option. Returns FALSE the option
+ * could not be set (e.g., if keyword is unknown) */
+ Bool AddIpoptNumOption(IpoptProblem ipopt_problem, const char* keyword, Number val);
+
+ /** Function for adding an Int option. Returns FALSE the option
+ * could not be set (e.g., if keyword is unknown) */
+ Bool AddIpoptIntOption(IpoptProblem ipopt_problem, const char* keyword, Int val);
+
+ /** Function for opening an output file for a given name with given
+ * printlevel. Returns false, if there was a problem opening the
+ * file. */
+ Bool OpenIpoptOutputFile(IpoptProblem ipopt_problem, char* file_name,
+ Int print_level);
+
+ /** Function calling the Ipopt optimization algorithm for a problem
+ previously defined with CreateIpoptProblem. The return
+ specified outcome of the optimization procedure (e.g., success,
+ failure etc).
+ */
+ enum ApplicationReturnStatus IpoptSolve(
+ IpoptProblem ipopt_problem
+ /** Problem that is to be optimized. Ipopt
+ will use the options previously specified with
+ AddIpoptOption (etc) for this problem. */
+ , Number* x /** Input: Starting point
+ Output: Optimal solution */
+ , Number* g /** Values of constraint at final point
+ (output only - ignored if set to NULL) */
+ , Number* obj_val /** Final value of objective function
+ (output only - ignored if set to NULL) */
+ , Number* mult_g /** Final multipliers for constraints
+ (output only - ignored if set to NULL) */
+ , Number* mult_x_L /** Final multipliers for lower variable bounds
+ (output only - ignored if set to NULL) */
+ , Number* mult_x_U /** Final multipliers for upper variable bounds
+ (output only - ignored if set to NULL) */
+ , UserDataPtr user_data
+ /** Pointer to user data. This will be
+ passed unmodified to the callback
+ functions. */
+ );
+
+ /**
+ void IpoptStatisticsCounts;
+
+ void IpoptStatisticsInfeasibilities; */
+
+#ifdef __cplusplus
+} /* extern "C" { */
+#endif
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpStdInterfaceTNLP.cpp b/SimTKmath/Optimizers/src/IpOpt/IpStdInterfaceTNLP.cpp
new file mode 100644
index 0000000..d5ac118
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpStdInterfaceTNLP.cpp
@@ -0,0 +1,318 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpStdInterfaceTNLP.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-09-02
+
+#include "IpStdInterfaceTNLP.hpp"
+#include "IpBlas.hpp"
+
+namespace Ipopt
+{
+ StdInterfaceTNLP::StdInterfaceTNLP(Index n_var,
+ const Number* x_L, const Number* x_U,
+ Index n_con,
+ const Number* g_L, const Number* g_U,
+ Index nele_jac,
+ Index nele_hess,
+ Index index_style,
+ const Number* start_x,
+ const Number* start_lam,
+ const Number* start_z_L,
+ const Number* start_z_U,
+ Eval_F_CB eval_f,
+ Eval_G_CB eval_g,
+ Eval_Grad_F_CB eval_grad_f,
+ Eval_Jac_G_CB eval_jac_g,
+ Eval_H_CB eval_h,
+ Number* x_sol,
+ Number* z_L_sol,
+ Number* z_U_sol,
+ Number* g_sol,
+ Number* lam_sol,
+ Number* obj_sol,
+ UserDataPtr user_data)
+ :
+ TNLP(),
+ n_var_(n_var),
+ n_con_(n_con),
+ x_L_(x_L),
+ x_U_(x_U),
+ g_L_(g_L),
+ g_U_(g_U),
+ nele_jac_(nele_jac),
+ nele_hess_(nele_hess),
+ index_style_(index_style),
+ start_x_(start_x),
+ start_lam_(start_lam),
+ start_z_L_(start_z_L),
+ start_z_U_(start_z_U),
+ eval_f_(eval_f),
+ eval_g_(eval_g),
+ eval_grad_f_(eval_grad_f),
+ eval_jac_g_(eval_jac_g),
+ eval_h_(eval_h),
+ user_data_(user_data),
+ non_const_x_(NULL),
+ x_sol_(x_sol),
+ z_L_sol_(z_L_sol),
+ z_U_sol_(z_U_sol),
+ g_sol_(g_sol),
+ lambda_sol_(lam_sol),
+ obj_sol_(obj_sol)
+ {
+ ASSERT_EXCEPTION(n_var_>0, INVALID_STDINTERFACE_NLP,
+ "The number of variables must be at least 1.");
+ ASSERT_EXCEPTION(n_con_>=0, INVALID_STDINTERFACE_NLP,
+ "The number of constrains must be non-negative.");
+ ASSERT_EXCEPTION(x_L_, INVALID_STDINTERFACE_NLP,
+ "No lower bounds for variables provided.");
+ ASSERT_EXCEPTION(x_U_, INVALID_STDINTERFACE_NLP,
+ "No upper bounds for variables provided.");
+ ASSERT_EXCEPTION(g_L_ || n_con_==0, INVALID_STDINTERFACE_NLP,
+ "No lower bounds for constraints provided.");
+ ASSERT_EXCEPTION(g_U_ || n_con_==0, INVALID_STDINTERFACE_NLP,
+ "No upper bounds for constraints provided.");
+ ASSERT_EXCEPTION(nele_jac_>=0, INVALID_STDINTERFACE_NLP,
+ "Number of non-zero elements in constraint Jacobian must be non-negative.");
+ ASSERT_EXCEPTION(nele_hess_>=0, INVALID_STDINTERFACE_NLP,
+ "Number of non-zero elements in Hessian of Lagrangian must be non-negative.");
+ ASSERT_EXCEPTION(index_style_ == 0 || index_style_ == 1, INVALID_STDINTERFACE_NLP,
+ "Valid index styles are 0 (C style) or 1 (Fortran style)");
+ ASSERT_EXCEPTION(start_x_, INVALID_STDINTERFACE_NLP,
+ "No initial point for the variables provided.");
+ ASSERT_EXCEPTION(eval_f_, INVALID_STDINTERFACE_NLP,
+ "No callback function for evaluating the value of objective function provided.");
+ ASSERT_EXCEPTION(eval_g_, INVALID_STDINTERFACE_NLP,
+ "No callback function for evaluating the values of constraints provided.");
+ ASSERT_EXCEPTION(eval_grad_f_, INVALID_STDINTERFACE_NLP,
+ "No callback function for evaluating the gradient of objective function provided.");
+ ASSERT_EXCEPTION(eval_jac_g_, INVALID_STDINTERFACE_NLP,
+ "No callback function for evaluating the Jacobian of the constraints provided.");
+ ASSERT_EXCEPTION(eval_h_, INVALID_STDINTERFACE_NLP,
+ "No callback function for evaluating the Hessian of the constraints provided.");
+ }
+
+ StdInterfaceTNLP::~StdInterfaceTNLP()
+ {
+ delete [] non_const_x_;
+ }
+
+ bool StdInterfaceTNLP::get_nlp_info(Index& n, Index& m, Index& nnz_jac_g,
+ Index& nnz_h_lag, IndexStyleEnum& index_style)
+ {
+ n = n_var_; // # of variables (variable types have been asserted in the constructor
+ m = n_con_; // # of constraints
+ nnz_jac_g = nele_jac_; // # of non-zeros in the jacobian
+ nnz_h_lag = nele_hess_; // # of non-zeros in the hessian
+
+ index_style = (index_style_ == 0) ? C_STYLE : FORTRAN_STYLE;
+
+ return true;
+ }
+
+ bool StdInterfaceTNLP::get_bounds_info(Index n, Number* x_l, Number* x_u,
+ Index m, Number* g_l, Number* g_u)
+ {
+ DBG_ASSERT(n == n_var_);
+ DBG_ASSERT(m == n_con_);
+
+ for (Index i=0; i<n; i++) {
+ x_l[i] = x_L_[i];
+ x_u[i] = x_U_[i];
+ }
+
+ for (Index i=0; i<m; i++) {
+ g_l[i] = g_L_[i];
+ g_u[i] = g_U_[i];
+ }
+
+ return true;
+ }
+
+ bool StdInterfaceTNLP::get_starting_point(Index n, bool init_x,
+ Number* x, bool init_z,
+ Number* z_L, Number* z_U,
+ Index m, bool init_lambda,
+ Number* lambda)
+ {
+ bool retval=true;
+
+ DBG_ASSERT(n == n_var_);
+ DBG_ASSERT(m == n_con_);
+
+ if (init_x) {
+ for (Index i=0; i<n; i++) {
+ x[i] = start_x_[i];
+ }
+ }
+
+ if (init_z) {
+ if (start_z_L_==NULL) {
+ retval = false;
+ }
+ else {
+ for (Index i=0; i<n; i++) {
+ z_L[i] = start_z_L_[i];
+ }
+ }
+ if (start_z_U_==NULL) {
+ retval = false;
+ }
+ else {
+ for (Index i=0; i<n; i++) {
+ z_U[i] = start_z_U_[i];
+ }
+ }
+ }
+
+ if (init_lambda) {
+ if(start_lam_==NULL) {
+ retval = false;
+ }
+ else {
+ for (Index i=0; i<m; i++) {
+ lambda[i] = start_lam_[i];
+ }
+ }
+ }
+
+ return retval;
+ }
+
+ bool StdInterfaceTNLP::eval_f(Index n, const Number* x, bool new_x,
+ Number& obj_value)
+ {
+ DBG_ASSERT(n==n_var_);
+
+ apply_new_x(new_x, n, x);
+
+ Bool retval = (*eval_f_)(n, non_const_x_, (Bool)new_x,
+ &obj_value, user_data_);
+ return (retval!=0);
+ }
+
+ bool StdInterfaceTNLP::eval_grad_f(Index n, const Number* x, bool new_x,
+ Number* grad_f)
+ {
+ DBG_ASSERT(n==n_var_);
+
+ apply_new_x(new_x, n, x);
+
+ Bool retval = (*eval_grad_f_)(n, non_const_x_, (Bool)new_x, grad_f,
+ user_data_);
+ return (retval!=0);
+ }
+
+ bool StdInterfaceTNLP::eval_g(Index n, const Number* x, bool new_x,
+ Index m, Number* g)
+ {
+ DBG_ASSERT(n==n_var_);
+ DBG_ASSERT(m==n_con_);
+
+ apply_new_x(new_x, n, x);
+
+ Bool retval = (*eval_g_)(n, non_const_x_, (Bool)new_x, m, g, user_data_);
+
+ return (retval!=0);
+ }
+
+ bool StdInterfaceTNLP::eval_jac_g(Index n, const Number* x, bool new_x,
+ Index m, Index nele_jac, Index* iRow,
+ Index *jCol, Number* values)
+ {
+ DBG_ASSERT(n==n_var_);
+ DBG_ASSERT(nele_jac==nele_jac_);
+
+ Bool retval=1;
+
+ if ( (iRow && jCol && !values) || (!iRow && !jCol && values) ) {
+ apply_new_x(new_x, n, x);
+ retval = (*eval_jac_g_)(n, non_const_x_, (Bool)new_x, m, nele_jac,
+ iRow, jCol, values, user_data_);
+ }
+ else {
+ DBG_ASSERT(false && "Invalid combination of iRow, jCol, and values pointers");
+ }
+ return (retval!=0);
+ }
+
+ bool StdInterfaceTNLP::eval_h(Index n, const Number* x, bool new_x,
+ Number obj_factor, Index m,
+ const Number* lambda, bool new_lambda,
+ Index nele_hess, Index* iRow, Index* jCol,
+ Number* values)
+ {
+ DBG_ASSERT(n==n_var_);
+ DBG_ASSERT(m==n_con_);
+ DBG_ASSERT(nele_hess==nele_hess_);
+
+ Bool retval=1;
+
+ if ( (iRow && jCol && !values) || (!iRow && !jCol && values) ) {
+ apply_new_x(new_x, n, x);
+ Number* non_const_lambda = new Number[m];
+ if (lambda) {
+ for (Index i=0; i<m; i++) {
+ non_const_lambda[i] = lambda[i];
+ }
+ }
+
+ retval = (*eval_h_)(n, non_const_x_, (Bool)new_x, obj_factor, m,
+ non_const_lambda, (Bool)new_lambda, nele_hess,
+ iRow, jCol, values, user_data_);
+ delete [] non_const_lambda;
+ }
+ else {
+ DBG_ASSERT(false && "Invalid combination of iRow, jCol, and values pointers");
+ }
+ return (retval!=0);
+ }
+
+ void StdInterfaceTNLP::finalize_solution(SolverReturn status,
+ Index n, const Number* x, const Number* z_L, const Number* z_U,
+ Index m, const Number* g, const Number* lambda,
+ Number obj_value)
+ {
+ if (x_sol_) {
+ IpBlasDcopy(n, x, 1, x_sol_, 1);
+ }
+ if (z_L_sol_) {
+ IpBlasDcopy(n, z_L, 1, z_L_sol_, 1);
+ }
+ if (z_U_sol_) {
+ IpBlasDcopy(n, z_U, 1, z_U_sol_, 1);
+ }
+ if (g_sol_) {
+ IpBlasDcopy(m, g, 1, g_sol_, 1);
+ }
+ if (lambda_sol_) {
+ IpBlasDcopy(m, lambda, 1, lambda_sol_, 1);
+ }
+ if (obj_sol_) {
+ *obj_sol_ = obj_value;
+ }
+ // don't need to store the status, we get the status from the OptimizeTNLP method
+ }
+
+ void StdInterfaceTNLP::apply_new_x(bool new_x, Index n, const Number* x)
+ {
+ if (new_x) {
+ //copy the data to the non_const_x_
+ if (!non_const_x_) {
+ non_const_x_ = new Number[n];
+ }
+
+ DBG_ASSERT(x && "x is NULL");
+ for (Index i=0; i<n; i++) {
+ non_const_x_[i] = x[i];
+ }
+ }
+ }
+
+} // namespace Ipopt
+
+
+
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpStdInterfaceTNLP.hpp b/SimTKmath/Optimizers/src/IpOpt/IpStdInterfaceTNLP.hpp
new file mode 100644
index 0000000..4b8b351
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpStdInterfaceTNLP.hpp
@@ -0,0 +1,208 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpStdInterfaceTNLP.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPSTDINTERFACETNLP_HPP__
+#define __IPSTDINTERFACETNLP_HPP__
+
+#include "IpUtils.hpp"
+#include "IpTNLP.hpp"
+#include "IpJournalist.hpp"
+#include "IpException.hpp"
+#include "IpStdCInterface.h"
+#include "IpSmartPtr.hpp"
+
+namespace Ipopt
+{
+ /** Declare excpetion that is thrown when invalid NLP data
+ * is provided */
+ DECLARE_STD_EXCEPTION(INVALID_STDINTERFACE_NLP);
+
+ /** Implementation of a TNLP for the Standard C interface. The
+ * standard C interface is exposed to the user as a single C
+ * function that is given problem dimension, starting points, and
+ * pointers for functions that evaluate objective function etc.
+ */
+ class StdInterfaceTNLP : public TNLP
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor, given dimensions of problem, function pointers
+ * for evaluation callback functions, and starting points. Note
+ * that the constrctor does not make a copy of any of the Number
+ * arrays, i.e. it is up to the called to keep them around. */
+ StdInterfaceTNLP(Index n_var,
+ const Number* x_L, const Number* x_U,
+ Index n_con,
+ const Number* g_L, const Number* g_U,
+ Index nele_jac,
+ Index nele_hess,
+ Index index_style,
+ const Number* start_x,
+ const Number* start_lam,
+ const Number* start_z_L,
+ const Number* start_z_U,
+ Eval_F_CB eval_f,
+ Eval_G_CB eval_g,
+ Eval_Grad_F_CB eval_grad_f,
+ Eval_Jac_G_CB eval_jac_g,
+ Eval_H_CB eval_h,
+ Number* x_sol,
+ Number* z_L_sol,
+ Number* z_U_sol,
+ Number* g_sol,
+ Number* lam_sol,
+ Number* obj_sol,
+ UserDataPtr user_data);
+
+ /** Default destructor */
+ virtual ~StdInterfaceTNLP();
+ //@}
+
+ /**@name methods to gather information about the NLP. These methods are
+ * overloaded from TNLP. See TNLP for their more detailed documentation. */
+ //@{
+ /** returns dimensions of the nlp. Overloaded from TNLP */
+ virtual bool get_nlp_info(Index& n, Index& m, Index& nnz_jac_g,
+ Index& nnz_h_lag, IndexStyleEnum& index_style);
+
+ /** returns bounds of the nlp. Overloaded from TNLP */
+ virtual bool get_bounds_info(Index n, Number* x_l, Number* x_u,
+ Index m, Number* g_l, Number* g_u);
+
+ /** provides a starting point for the nlp variables. Overloaded from TNLP */
+ virtual bool get_starting_point(Index n, bool init_x, Number* x,
+ bool init_z, Number* z_L, Number* z_U,
+ Index m, bool init_lambda, Number* lambda);
+
+ /** evaluates the objective value for the nlp. Overloaded from TNLP */
+ virtual bool eval_f(Index n, const Number* x, bool new_x,
+ Number& obj_value);
+
+ /** evaluates the gradient of the objective for the
+ * nlp. Overloaded from TNLP */
+ virtual bool eval_grad_f(Index n, const Number* x, bool new_x,
+ Number* grad_f);
+
+ /** evaluates the constraint residuals for the nlp. Overloaded from TNLP */
+ virtual bool eval_g(Index n, const Number* x, bool new_x, Index m,
+ Number* g);
+
+ /** specifies the jacobian structure (if values is NULL) and
+ * evaluates the jacobian values (if values is not NULL) for the
+ * nlp. Overloaded from TNLP */
+ virtual bool eval_jac_g(Index n, const Number* x, bool new_x, Index m,
+ Index nele_jac, Index* iRow, Index *jCol,
+ Number* values);
+
+ /** specifies the structure of the hessian of the lagrangian (if values is NULL) and
+ * evaluates the values (if values is not NULL). Overloaded from TNLP */
+ virtual bool eval_h(Index n, const Number* x, bool new_x,
+ Number obj_factor, Index m, const Number* lambda,
+ bool new_lambda, Index nele_hess, Index* iRow,
+ Index* jCol, Number* values);
+ //@}
+
+ /** @name Solution Methods */
+ //@{
+ virtual void finalize_solution(SolverReturn status,
+ Index n, const Number* x, const Number* z_L, const Number* z_U,
+ Index m, const Number* g, const Number* lambda,
+ Number obj_value);
+ //@}
+
+ private:
+ /** Journlist */
+ SmartPtr<const Journalist> jnlst_;
+
+ /** @name Information about the problem */
+ //@{
+ /** Number of variables */
+ const Index n_var_;
+ /** Number of constraints */
+ const Index n_con_;
+ /** Pointer to Number array containing lower bounds for variables */
+ const Number* x_L_;
+ /** Pointer to Number array containing upper bounds for variables */
+ const Number* x_U_;
+ /** Pointer to Number array containing lower bounds for constraints */
+ const Number* g_L_;
+ /** Pointer to Number array containing upper bounds for constraints */
+ const Number* g_U_;
+ /** Number of non-zero elements in the constraint Jacobian */
+ const Index nele_jac_;
+ /** Number of non-zero elements in the Hessian */
+ const Index nele_hess_;
+ /** Starting value of the iRow and jCol parameters for matrices */
+ const Index index_style_;
+ /** Pointer to Number array containing starting point for variables */
+ const Number* start_x_;
+ /** Poitner to Number array containing starting values for
+ * constraint multipliers */
+ const Number* start_lam_;
+ /** Pointer to Number array containing starting values for lower
+ * bound multipliers */
+ const Number* start_z_L_;
+ /** Pointer to Number array containing starting values for upper
+ * bound multipliers */
+ const Number* start_z_U_;
+ /** Pointer to callback function evaluating value of objective function */
+ Eval_F_CB eval_f_;
+ /** Pointer to callback function evaluating value of constraints */
+ Eval_G_CB eval_g_;
+ /** Pointer to callback function evaluating gradient of objective
+ * function */
+ Eval_Grad_F_CB eval_grad_f_;
+ /** Pointer to callback function evaluating Jacobian of constraints */
+ Eval_Jac_G_CB eval_jac_g_;
+ /** Pointer to callback function evaluating Hessian of Lagrangian */
+ Eval_H_CB eval_h_;
+ /** Pointer to user data */
+ UserDataPtr user_data_;
+ //@}
+
+
+ /** A non-const copy of x - this is kept up-to-date in apply_new_x */
+ Number* non_const_x_;
+
+ /** Pointers to the user provided vectors for solution */
+ Number* x_sol_;
+ Number* z_L_sol_;
+ Number* z_U_sol_;
+ Number* g_sol_;
+ Number* lambda_sol_;
+ Number* obj_sol_;
+
+ /** Internal function to update the internal and ampl state if the
+ * x value changes */
+ void apply_new_x(bool new_x, Index n, const Number* x);
+
+
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ StdInterfaceTNLP();
+
+ /** Copy Constructor */
+ StdInterfaceTNLP(const StdInterfaceTNLP&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const StdInterfaceTNLP&);
+ //@}
+
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpSumMatrix.cpp b/SimTKmath/Optimizers/src/IpOpt/IpSumMatrix.cpp
new file mode 100644
index 0000000..ed56f77
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpSumMatrix.cpp
@@ -0,0 +1,165 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpSumMatrix.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpoptConfig.h"
+#include "IpSumMatrix.hpp"
+
+#ifdef HAVE_CSTDIO
+# include <cstdio>
+#else
+# ifdef HAVE_STDIO_H
+# include <stdio.h>
+# else
+# error "don't have header file for stdio"
+# endif
+#endif
+
+
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+namespace Ipopt
+{
+
+ SumMatrix::SumMatrix(const SumMatrixSpace* owner_space)
+ :
+ Matrix(owner_space),
+ factors_(owner_space->NTerms(), 1.0),
+ matrices_(owner_space->NTerms()),
+ owner_space_(owner_space)
+ {}
+
+ SumMatrix::~SumMatrix()
+ {}
+
+ void SumMatrix::SetTerm(Index iterm, Number factor,
+ const Matrix& matrix)
+ {
+ DBG_ASSERT(iterm<owner_space_->NTerms());
+ factors_[iterm] = factor;
+ matrices_[iterm] = &matrix;
+ }
+
+ void SumMatrix::GetTerm(Index iterm, Number& factor, SmartPtr<const Matrix>& matrix) const
+ {
+ DBG_ASSERT(iterm<owner_space_->NTerms());
+ factor = factors_[iterm];
+ matrix = matrices_[iterm];
+ }
+
+ Index SumMatrix::NTerms() const
+ {
+ return owner_space_->NTerms();
+ }
+
+ void SumMatrix::MultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ // A few sanity checks
+ DBG_ASSERT(NCols()==x.Dim());
+ DBG_ASSERT(NRows()==y.Dim());
+
+ // Take care of the y part of the addition
+ if( beta!=0.0 ) {
+ y.Scal(beta);
+ }
+ else {
+ y.Set(0.0); // In case y hasn't been initialized yet
+ }
+
+ for (Index iterm=0; iterm<NTerms(); iterm++) {
+ DBG_ASSERT(IsValid(matrices_[iterm]));
+ matrices_[iterm]->MultVector(alpha*factors_[iterm], x,
+ 1.0, y);
+ }
+ }
+
+ void SumMatrix::TransMultVectorImpl(Number alpha, const Vector& x,
+ Number beta, Vector& y) const
+ {
+ // A few sanity checks
+ DBG_ASSERT(NRows()==x.Dim());
+ DBG_ASSERT(NCols()==y.Dim());
+
+ // Take care of the y part of the addition
+ if( beta!=0.0 ) {
+ y.Scal(beta);
+ }
+ else {
+ y.Set(0.0); // In case y hasn't been initialized yet
+ }
+
+ for (Index iterm=0; iterm<NTerms(); iterm++) {
+ DBG_ASSERT(IsValid(matrices_[iterm]));
+ matrices_[iterm]->TransMultVector(alpha*factors_[iterm], x,
+ 1.0, y);
+ }
+ }
+
+ bool SumMatrix::HasValidNumbersImpl() const
+ {
+ for (Index iterm=0; iterm<NTerms(); iterm++) {
+ DBG_ASSERT(IsValid(matrices_[iterm]));
+ if (!matrices_[iterm]->HasValidNumbers()) {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ void SumMatrix::PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ jnlst.Printf(level, category, "\n");
+ jnlst.PrintfIndented(level, category, indent,
+ "%sSumMatrix \"%s\" of dimension %d x %d with %d terms:\n",
+ prefix.c_str(), name.c_str(), NRows(), NCols(), NTerms());
+ for (Index iterm=0; iterm<NTerms(); iterm++) {
+ jnlst.PrintfIndented(level, category, indent,
+ "%sTerm %d with factor %23.16e and the following matrix:\n",
+ prefix.c_str(), iterm, factors_[iterm]);
+ char buffer[256];
+ sprintf(buffer, "Term: %d", iterm);
+ std::string name = buffer;
+ matrices_[iterm]->Print(&jnlst, level, category, name, indent+1, prefix);
+ }
+ }
+
+ void SumMatrixSpace::SetTermSpace(Index term_idx, const MatrixSpace& mat_space)
+ {
+ while(term_idx >= (Index)term_spaces_.size()) {
+ term_spaces_.push_back(NULL);
+ }
+ term_spaces_[term_idx] = &mat_space;
+ }
+
+ SmartPtr<const MatrixSpace> SumMatrixSpace::GetTermSpace(Index term_idx) const
+ {
+ if (term_idx >= 0 && term_idx < (Index)term_spaces_.size()) {
+ return term_spaces_[term_idx];
+ }
+ return NULL;
+ }
+
+ SumMatrix* SumMatrixSpace::MakeNewSumMatrix() const
+ {
+ DBG_ASSERT(nterms_ == (Index)term_spaces_.size());
+ return new SumMatrix(this);
+ }
+
+ Matrix* SumMatrixSpace::MakeNew() const
+ {
+ return MakeNewSumMatrix();
+ }
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpSumMatrix.hpp b/SimTKmath/Optimizers/src/IpOpt/IpSumMatrix.hpp
new file mode 100644
index 0000000..329f451
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpSumMatrix.hpp
@@ -0,0 +1,163 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpSumMatrix.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPSUMMATRIX_HPP__
+#define __IPSUMMATRIX_HPP__
+
+#include "IpUtils.hpp"
+#include "IpMatrix.hpp"
+
+namespace Ipopt
+{
+
+ /* forward declarations */
+ class SumMatrixSpace;
+
+ /** Class for Matrices which are sum of matrices.
+ * For each term in the we store the matrix and a factor.
+ */
+ class SumMatrix : public Matrix
+ {
+ public:
+
+ /**@name Constructors / Destructors */
+ //@{
+ /** Constructor, taking the owner_space.
+ */
+ SumMatrix(const SumMatrixSpace* owner_space);
+
+ /** Destructor */
+ virtual ~SumMatrix();
+ //@}
+
+ /** Method for setting term iterm for the sum. */
+ void SetTerm(Index iterm, Number factor, const Matrix& matrix);
+
+ /** Method for getting term iterm for the sum. Note that counting
+ * of terms starts at 0. */
+ void GetTerm(Index iterm, Number& factor, SmartPtr<const Matrix>& matrix) const;
+
+ /** Return the number of terms */
+ Index NTerms() const;
+
+ protected:
+ /**@name Methods overloaded from matrix */
+ //@{
+ virtual void MultVectorImpl(Number alpha, const Vector& x,
+ Number beta, Vector& y) const;
+
+ virtual void TransMultVectorImpl(Number alpha, const Vector& x,
+ Number beta, Vector& y) const;
+
+ /** Method for determining if all stored numbers are valid (i.e.,
+ * no Inf or Nan). */
+ virtual bool HasValidNumbersImpl() const;
+
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const;
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ SumMatrix();
+
+ /** Copy Constructor */
+ SumMatrix(const SumMatrix&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const SumMatrix&);
+ //@}
+
+ /** std::vector storing the factors for each term. */
+ std::vector<Number> factors_;
+
+ /** std::vector storing the matrices for each term. */
+ std::vector<SmartPtr<const Matrix> > matrices_;
+
+ /** Copy of the owner_space as a SumMatrixSpace */
+ const SumMatrixSpace* owner_space_;
+ };
+
+ /** Class for matrix space for SumMatrix. */
+ class SumMatrixSpace : public MatrixSpace
+ {
+ public:
+ /** @name Constructors / Destructors */
+ //@{
+ /** Constructor, given the number of row and columns, as well as
+ * the number of terms in the sum.
+ */
+ SumMatrixSpace(Index nrows, Index ncols, Index nterms)
+ :
+ MatrixSpace(nrows, ncols),
+ nterms_(nterms)
+ {}
+
+ /** Destructor */
+ virtual ~SumMatrixSpace()
+ {}
+ //@}
+
+ /** Accessor functions to get the number of terms in the sum. */
+ Index NTerms() const
+ {
+ return nterms_;
+ }
+
+ /** Set the appropriate matrix space for each term. This must
+ * be called for each term or a runtime error will occur */
+ void SetTermSpace(Index term_idx, const MatrixSpace& mat_space);
+
+ /** Get the matrix space for a particular term */
+ SmartPtr<const MatrixSpace> GetTermSpace(Index term_idx) const;
+
+ /** Method for creating a new matrix of this specific type. */
+ SumMatrix* MakeNewSumMatrix() const;
+
+ /** Overloaded MakeNew method for the MatrixSpace base class.
+ */
+ virtual Matrix* MakeNew() const;
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default constructor */
+ SumMatrixSpace();
+
+ /** Copy Constructor */
+ SumMatrixSpace(const SumMatrixSpace&);
+
+ /** Overloaded Equals Operator */
+ SumMatrixSpace& operator=(const SumMatrixSpace&);
+ //@}
+
+ const Index nterms_;
+
+ std::vector< SmartPtr<const MatrixSpace> > term_spaces_;
+ };
+
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpSumSymMatrix.cpp b/SimTKmath/Optimizers/src/IpOpt/IpSumSymMatrix.cpp
new file mode 100644
index 0000000..228c601
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpSumSymMatrix.cpp
@@ -0,0 +1,145 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpSumSymMatrix.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpoptConfig.h"
+#include "IpSumSymMatrix.hpp"
+
+#ifdef HAVE_CSTDIO
+# include <cstdio>
+#else
+# ifdef HAVE_STDIO_H
+# include <stdio.h>
+# else
+# error "don't have header file for stdio"
+# endif
+#endif
+
+
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+namespace Ipopt
+{
+
+ SumSymMatrix::SumSymMatrix(const SumSymMatrixSpace* owner_space)
+ :
+ SymMatrix(owner_space),
+ factors_(owner_space->NTerms(), 1.0),
+ matrices_(owner_space->NTerms()),
+ owner_space_(owner_space)
+ {}
+
+ SumSymMatrix::~SumSymMatrix()
+ {}
+
+ void SumSymMatrix::SetTerm(Index iterm, Number factor,
+ const SymMatrix& matrix)
+ {
+ DBG_ASSERT(iterm<owner_space_->NTerms());
+ factors_[iterm] = factor;
+ matrices_[iterm] = &matrix;
+ }
+
+ void SumSymMatrix::GetTerm(Index iterm, Number& factor, SmartPtr<const SymMatrix>& matrix) const
+ {
+ DBG_ASSERT(iterm<owner_space_->NTerms());
+ factor = factors_[iterm];
+ matrix = matrices_[iterm];
+ }
+
+
+ Index SumSymMatrix::NTerms() const
+ {
+ return owner_space_->NTerms();
+ }
+
+ void SumSymMatrix::MultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ // A few sanity checks
+ DBG_ASSERT(Dim()==x.Dim());
+ DBG_ASSERT(Dim()==y.Dim());
+
+ // Take care of the y part of the addition
+ if( beta!=0.0 ) {
+ y.Scal(beta);
+ }
+ else {
+ y.Set(0.0); // In case y hasn't been initialized yet
+ }
+
+ for (Index iterm=0; iterm<NTerms(); iterm++) {
+ DBG_ASSERT(IsValid(matrices_[iterm]));
+ matrices_[iterm]->MultVector(alpha*factors_[iterm], x,
+ 1.0, y);
+ }
+ }
+
+ bool SumSymMatrix::HasValidNumbersImpl() const
+ {
+ for (Index iterm=0; iterm<NTerms(); iterm++) {
+ DBG_ASSERT(IsValid(matrices_[iterm]));
+ if (!matrices_[iterm]->HasValidNumbers()) {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ void SumSymMatrix::PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ jnlst.Printf(level, category, "\n");
+ jnlst.PrintfIndented(level, category, indent,
+ "%sSumSymMatrix \"%s\" of dimension %d with %d terms:\n",
+ prefix.c_str(), name.c_str(), Dim(), NTerms());
+ for (Index iterm=0; iterm<NTerms(); iterm++) {
+ jnlst.PrintfIndented(level, category, indent,
+ "%sTerm %d with factor %23.16e and the following matrix:\n",
+ prefix.c_str(), iterm, factors_[iterm]);
+ char buffer[256];
+ sprintf(buffer, "Term: %d", iterm);
+ std::string name = buffer;
+ matrices_[iterm]->Print(&jnlst, level, category, name, indent+1, prefix);
+ }
+ }
+
+ void SumSymMatrixSpace::SetTermSpace
+ (Index term_idx, const SymMatrixSpace& space)
+ {
+ while(term_idx >= (Index)term_spaces_.size()) {
+ term_spaces_.push_back(NULL);
+ }
+ term_spaces_[term_idx] = &space;
+ }
+
+ SmartPtr<const SymMatrixSpace> SumSymMatrixSpace::GetTermSpace(Index term_idx) const
+ {
+ if (term_idx >= 0 && term_idx < (Index)term_spaces_.size()) {
+ return term_spaces_[term_idx];
+ }
+ return NULL;
+ }
+
+ SumSymMatrix* SumSymMatrixSpace::MakeNewSumSymMatrix() const
+ {
+ DBG_ASSERT(nterms_ == (Index)term_spaces_.size());
+ return new SumSymMatrix(this);
+ }
+
+ SymMatrix* SumSymMatrixSpace::MakeNewSymMatrix() const
+ {
+ return MakeNewSumSymMatrix();
+ }
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpSumSymMatrix.hpp b/SimTKmath/Optimizers/src/IpOpt/IpSumSymMatrix.hpp
new file mode 100644
index 0000000..7cc1f6e
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpSumSymMatrix.hpp
@@ -0,0 +1,148 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpSumSymMatrix.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPSUMSYMMATRIX_HPP__
+#define __IPSUMSYMMATRIX_HPP__
+
+#include "IpUtils.hpp"
+#include "IpSymMatrix.hpp"
+
+namespace Ipopt
+{
+
+ /* forward declarations */
+ class SumSymMatrixSpace;
+
+ /** Class for Matrices which are sum of symmetric matrices.
+ * For each term in the we store the matrix and a factor.
+ */
+ class SumSymMatrix : public SymMatrix
+ {
+ public:
+
+ /**@name Constructors / Destructors */
+ //@{
+
+ /** Constructor, initializing with dimensions of the matrix and
+ * the number of terms in the sum.
+ */
+ SumSymMatrix(const SumSymMatrixSpace* owner_space);
+
+ /** Destructor */
+ ~SumSymMatrix();
+ //@}
+
+ /** Method for setting term iterm for the sum. Note that counting
+ * of terms starts at 0. */
+ void SetTerm(Index iterm, Number factor, const SymMatrix& matrix);
+
+ /** Method for getting term iterm for the sum. Note that counting
+ * of terms starts at 0. */
+ void GetTerm(Index iterm, Number& factor, SmartPtr<const SymMatrix>& matrix) const;
+
+ /** Return the number of terms */
+ Index NTerms() const;
+
+ protected:
+ /**@name Methods overloaded from matrix */
+ //@{
+ virtual void MultVectorImpl(Number alpha, const Vector& x,
+ Number beta, Vector& y) const;
+
+ /** Method for determining if all stored numbers are valid (i.e.,
+ * no Inf or Nan). */
+ virtual bool HasValidNumbersImpl() const;
+
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const;
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ SumSymMatrix();
+
+ /** Copy Constructor */
+ SumSymMatrix(const SumSymMatrix&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const SumSymMatrix&);
+ //@}
+
+ /** std::vector storing the factors for each term. */
+ std::vector<Number> factors_;
+
+ /** std::vector storing the matrices for each term. */
+ std::vector<SmartPtr<const SymMatrix> > matrices_;
+
+ /** Copy of the owner_space as a SumSymMatrixSpace */
+ const SumSymMatrixSpace* owner_space_;
+ };
+
+ /** Class for matrix space for SumSymMatrix */
+ class SumSymMatrixSpace : public SymMatrixSpace
+ {
+ public:
+ /** @name Constructors / Destructors */
+ //@{
+ /** Constructor, given the dimension of the matrix and the number
+ * of terms in the sum. */
+ SumSymMatrixSpace(Index ndim, Index nterms)
+ :
+ SymMatrixSpace(ndim),
+ nterms_(nterms)
+ {}
+
+ /** Destructor */
+ ~SumSymMatrixSpace()
+ {}
+ //@}
+
+ /** @name Accessor functions */
+ //@{
+ /** Number of terms in the sum. */
+ Index NTerms() const
+ {
+ return nterms_;
+ }
+ //@}
+
+ /** Use this method to set the matrix spaces for the various terms.
+ * You will not be able to create a matrix until all these spaces
+ * are set. */
+ void SetTermSpace(Index term_idx, const SymMatrixSpace& space);
+
+ /** Get the matix space for a particular term */
+ SmartPtr<const SymMatrixSpace> GetTermSpace(Index term_idx) const;
+
+ /** Method for creating a new matrix of this specific type. */
+ SumSymMatrix* MakeNewSumSymMatrix() const;
+
+ /** Overloaded MakeNew method for the SymMatrixSpace base class.
+ */
+ virtual SymMatrix* MakeNewSymMatrix() const;
+
+ private:
+ Index nterms_;
+
+ std::vector< SmartPtr<const SymMatrixSpace> > term_spaces_;
+ };
+
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpSymLinearSolver.hpp b/SimTKmath/Optimizers/src/IpOpt/IpSymLinearSolver.hpp
new file mode 100644
index 0000000..ac2bd76
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpSymLinearSolver.hpp
@@ -0,0 +1,130 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpSymLinearSolver.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPSYMLINEARSOLVER_HPP__
+#define __IPSYMLINEARSOLVER_HPP__
+
+#include "IpUtils.hpp"
+#include "IpSymMatrix.hpp"
+#include "IpAlgStrategy.hpp"
+#include <vector>
+
+namespace Ipopt
+{
+
+ /** Enum to report outcome of a linear solve */
+ enum ESymSolverStatus {
+ /** Successful solve */
+ SYMSOLVER_SUCCESS,
+ /** Matrix seems to be singular; solve was aborted */
+ SYMSOLVER_SINGULAR,
+ /** The number of negative eigenvalues is not correct */
+ SYMSOLVER_WRONG_INERTIA,
+ /** Call the solver interface again after the matrix values have
+ * been restored */
+ SYMSOLVER_CALL_AGAIN,
+ /** Unrecoverable error in linear solver occured. The
+ * optimization will be aborted. */
+ SYMSOLVER_FATAL_ERROR
+ };
+
+ /** Base class for all derived symmetric linear
+ * solvers. In the full space version of Ipopt a large linear
+ * system has to be solved for the augmented system. This case is
+ * meant to be the base class for all derived linear solvers for
+ * symmetric matrices (of type SymMatrix).
+ *
+ * A linear solver can be used repeatedly for matrices with
+ * identical structure of nonzero elements. The nonzero structure
+ * of those matrices must not be changed between calls.
+ *
+ * The called might ask the solver to only solve the linear system
+ * if the system is nonsingular, and if the number of negative
+ * eigenvalues matches a given number.
+ */
+ class SymLinearSolver: public AlgorithmStrategyObject
+ {
+ public:
+ /** @name Constructor/Destructor */
+ //@{
+ SymLinearSolver()
+ {}
+
+ virtual ~SymLinearSolver()
+ {}
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix) = 0;
+
+ /** @name Methods for requesting solution of the linear system. */
+ //@{
+ /** Solve operation for multiple right hand sides. Solves the
+ * linear system A * Sol = Rhs with multiple right hand sides. If
+ * necessary, A is factorized. Correct solutions are only
+ * guaranteed if the return values is SYMSOLVER_SUCCESS. The
+ * solver will return SYMSOLVER_SINGULAR if the linear system is
+ * singular, and it will return SYMSOLVER_WRONG_INERTIA if
+ * check_NegEVals is true and the number of negative eigenvalues
+ * in the matrix does not match numberOfNegEVals.
+ *
+ * check_NegEVals cannot be chosen true, if ProvidesInertia()
+ * returns false.
+ */
+ virtual ESymSolverStatus MultiSolve(const SymMatrix &A,
+ std::vector<SmartPtr<const Vector> >& rhsV,
+ std::vector<SmartPtr<Vector> >& solV,
+ bool check_NegEVals,
+ Index numberOfNegEVals)=0;
+
+ /** Solve operation for a single right hand side. Solves the
+ * linear system A * Sol = Rhs. See MultiSolve for more
+ * details. */
+ ESymSolverStatus Solve(const SymMatrix &A,
+ const Vector& rhs, Vector& sol,
+ bool check_NegEVals,
+ Index numberOfNegEVals)
+ {
+ std::vector<SmartPtr<const Vector> > rhsV(1);
+ rhsV[0] = &rhs;
+ std::vector<SmartPtr<Vector> > solV(1);
+ solV[0] = /
+ return MultiSolve(A, rhsV, solV, check_NegEVals,
+ numberOfNegEVals);
+ }
+
+ /** Number of negative eigenvalues detected during last
+ * factorization. Returns the number of negative eigenvalues of
+ * the most recent factorized matrix. This must not be called if
+ * the linear solver does not compute this quantities (see
+ * ProvidesInertia).
+ */
+ virtual Index NumberOfNegEVals() const =0;
+ //@}
+
+ //* @name Options of Linear solver */
+ //@{
+ /** Request to increase quality of solution for next solve.
+ * Ask linear solver to increase quality of solution for the next
+ * solve (e.g. increase pivot tolerance). Returns false, if this
+ * is not possible (e.g. maximal pivot tolerance already used.)
+ */
+ virtual bool IncreaseQuality() =0;
+
+ /** Query whether inertia is computed by linear solver.
+ * Returns true, if linear solver provides inertia.
+ */
+ virtual bool ProvidesInertia() const =0;
+ //@}
+ };
+
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpSymMatrix.cpp b/SimTKmath/Optimizers/src/IpOpt/IpSymMatrix.cpp
new file mode 100644
index 0000000..5c91483
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpSymMatrix.cpp
@@ -0,0 +1,50 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpSymMatrix.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpSymMatrix.hpp"
+
+namespace Ipopt
+{
+
+ SymMatrix::SymMatrix(const SymMatrixSpace* owner_space)
+ :
+ Matrix(owner_space),
+ owner_space_(owner_space)
+ {}
+
+ SymMatrix::~SymMatrix()
+ {}
+
+ SmartPtr<const SymMatrixSpace> SymMatrix::OwnerSymMatrixSpace() const
+ {
+ return owner_space_;
+ }
+
+ void SymMatrix::TransMultVectorImpl(Number alpha, const Vector& x, Number beta,
+ Vector& y) const
+ {
+ // Since this matrix is symetric, this is the same operation as
+ // MultVector
+ MultVector(alpha, x, beta, y);
+ }
+
+ SymMatrixSpace::SymMatrixSpace(Index dim)
+ :
+ MatrixSpace(dim,dim)
+ {}
+
+ SymMatrixSpace::~SymMatrixSpace()
+ {}
+
+
+ Matrix* SymMatrixSpace::MakeNew() const
+ {
+ return MakeNewSymMatrix();
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpSymMatrix.hpp b/SimTKmath/Optimizers/src/IpOpt/IpSymMatrix.hpp
new file mode 100644
index 0000000..0191563
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpSymMatrix.hpp
@@ -0,0 +1,130 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpSymMatrix.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPSYMMATRIX_HPP__
+#define __IPSYMMATRIX_HPP__
+
+#include "IpUtils.hpp"
+#include "IpMatrix.hpp"
+
+namespace Ipopt
+{
+
+ /* forward declarations */
+ class SymMatrixSpace;
+
+ /** This is the base class for all derived symmetric matrix types.
+ */
+ class SymMatrix : public Matrix
+ {
+ public:
+ /** @name Constructor/Destructor */
+ //@{
+ /** Constructor, taking the owner_space.
+ */
+ SymMatrix(const SymMatrixSpace* owner_space);
+
+ /** Destructor */
+ virtual ~SymMatrix();
+ //@}
+
+ /** @name Information about the size of the matrix */
+ //@{
+ /** Dimension of the matrix (number of rows and columns) */
+ Index Dim() const;
+ //@}
+
+ SmartPtr<const SymMatrixSpace> OwnerSymMatrixSpace() const;
+
+ protected:
+ /** @name Overloaded methods from Matrix. Since the matrix is
+ * symmetric, it is only necessary to implement the
+ * MultVectorImpl method in a class that inherits from this base
+ * class. If the TransMultVectorImpl is called, this base class
+ * automatically calls MultVectorImpl instead.
+ */
+ //@{
+ virtual void TransMultVectorImpl(Number alpha, const Vector& x, Number beta,
+ Vector& y) const;
+ //@}
+
+ private:
+ /** Copy of the owner space ptr as a SymMatrixSpace instead
+ * of a MatrixSpace
+ */
+ const SymMatrixSpace* owner_space_;
+ };
+
+
+ /** SymMatrixSpace base class, corresponding to the SymMatrix base
+ * class. */
+ class SymMatrixSpace : public MatrixSpace
+ {
+ public:
+ /** @name Constructors/Destructors */
+ //@{
+ /** Constructor, given the dimension (identical to the number of
+ * rows and columns).
+ */
+ SymMatrixSpace(Index dim);
+
+ /** Destructor */
+ virtual ~SymMatrixSpace();
+ //@}
+
+ /** Pure virtual method for creating a new matrix of this specific
+ * type. */
+ virtual SymMatrix* MakeNewSymMatrix() const=0;
+
+ /** Overloaded MakeNew method for the MatrixSpace base class.
+ */
+ virtual Matrix* MakeNew() const;
+
+ /** Accessor method for the dimension of the matrices in this
+ * matrix space.
+ */
+ Index Dim() const;
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** default constructor */
+ SymMatrixSpace();
+
+ /* Copy constructor */
+ SymMatrixSpace(const SymMatrixSpace&);
+
+ /** Overloaded Equals Operator */
+ SymMatrixSpace& operator=(const SymMatrixSpace&);
+ //@}
+
+ };
+
+ /* inline methods */
+ inline
+ Index SymMatrixSpace::Dim() const
+ {
+ DBG_ASSERT(NRows() == NCols());
+ return NRows();
+ }
+
+ inline
+ Index SymMatrix::Dim() const
+ {
+ return owner_space_->Dim();
+ }
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpSymScaledMatrix.cpp b/SimTKmath/Optimizers/src/IpOpt/IpSymScaledMatrix.cpp
new file mode 100644
index 0000000..e12913c
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpSymScaledMatrix.cpp
@@ -0,0 +1,83 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpSymScaledMatrix.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpSymScaledMatrix.hpp"
+
+namespace Ipopt
+{
+
+ SymScaledMatrix::SymScaledMatrix(const SymScaledMatrixSpace* owner_space)
+ :
+ SymMatrix(owner_space),
+ owner_space_(owner_space)
+ {}
+
+
+ SymScaledMatrix::~SymScaledMatrix()
+ {}
+
+ void SymScaledMatrix::MultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ DBG_ASSERT(IsValid(matrix_));
+
+ // Take care of the y part of the addition
+ if( beta!=0.0 ) {
+ y.Scal(beta);
+ }
+ else {
+ y.Set(0.0); // In case y hasn't been initialized yet
+ }
+
+ // need some temporary vectors
+ SmartPtr<Vector> tmp_x = x.MakeNewCopy();
+ SmartPtr<Vector> tmp_y = y.MakeNew();
+
+ if (IsValid(owner_space_->RowColScaling())) {
+ tmp_x->ElementWiseMultiply(*owner_space_->RowColScaling());
+ }
+
+ matrix_->MultVector(1.0, *tmp_x, 0.0, *tmp_y);
+
+ if (IsValid(owner_space_->RowColScaling())) {
+ tmp_y->ElementWiseMultiply(*owner_space_->RowColScaling());
+ }
+
+ y.Axpy(1.0, *tmp_y);
+ }
+
+ bool SymScaledMatrix::HasValidNumbersImpl() const
+ {
+ return matrix_->HasValidNumbers();
+ }
+
+ void SymScaledMatrix::PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ jnlst.Printf(level, category, "\n");
+ jnlst.PrintfIndented(level, category, indent,
+ "%sSymScaledMatrix \"%s\" of dimension %d x %d:\n",
+ prefix.c_str(), name.c_str(), NRows(), NCols());
+ owner_space_->RowColScaling()->Print(&jnlst, level, category,
+ name+"_row_col_scaling",
+ indent+1, prefix);
+ if (IsValid(matrix_)) {
+ matrix_->Print(&jnlst, level, category, name+"_unscaled_matrix",
+ indent+1, prefix);
+ }
+ else {
+ jnlst.PrintfIndented(level, category, indent,
+ "%sunscaled matrix is NULL\n", prefix.c_str());
+ }
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpSymScaledMatrix.hpp b/SimTKmath/Optimizers/src/IpOpt/IpSymScaledMatrix.hpp
new file mode 100644
index 0000000..7a5b4da
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpSymScaledMatrix.hpp
@@ -0,0 +1,229 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpSymScaledMatrix.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPSYMSCALEDMATRIX_HPP__
+#define __IPSYMSCALEDMATRIX_HPP__
+
+#include "IpUtils.hpp"
+#include "IpSymMatrix.hpp"
+
+namespace Ipopt
+{
+
+ /* forward declarations */
+ class SymScaledMatrixSpace;
+
+ /** Class for a Matrix in conjunction with its scaling factors for
+ * row and column scaling. Operations on the matrix are performed using
+ * the scaled matrix. You can pull out the pointer to the
+ * unscaled matrix for unscaled calculations.
+ */
+ class SymScaledMatrix : public SymMatrix
+ {
+ public:
+
+ /**@name Constructors / Destructors */
+ //@{
+
+ /** Constructor, taking the owner_space.
+ */
+ SymScaledMatrix(const SymScaledMatrixSpace* owner_space);
+
+ /** Destructor */
+ ~SymScaledMatrix();
+ //@}
+
+ /** Set the unscaled matrix */
+ void SetUnscaledMatrix(const SmartPtr<const SymMatrix> unscaled_matrix);
+
+ /** Set the unscaled matrix in a non-const version */
+ void SetUnscaledMatrixNonConst(const SmartPtr<SymMatrix>& unscaled_matrix);
+
+ /** Return the unscaled matrix in const form */
+ SmartPtr<const SymMatrix> GetUnscaledMatrix() const;
+
+ /** Return the unscaled matrix in non-const form */
+ SmartPtr<SymMatrix> GetUnscaledMatrixNonConst();
+
+ /** return the vector for the row and column scaling */
+ SmartPtr<const Vector> RowColScaling() const;
+
+ protected:
+ /**@name Methods overloaded from Matrix */
+ //@{
+ virtual void MultVectorImpl(Number alpha, const Vector& x,
+ Number beta, Vector& y) const;
+
+ /** Method for determining if all stored numbers are valid (i.e.,
+ * no Inf or Nan). It is assumed here that the scaling factors
+ * are always valid numbers. */
+ virtual bool HasValidNumbersImpl() const;
+
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const;
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ SymScaledMatrix();
+
+ /** Copy Constructor */
+ SymScaledMatrix(const SymScaledMatrix&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const SymScaledMatrix&);
+ //@}
+
+ /** const version of the unscaled matrix */
+ SmartPtr<const SymMatrix> matrix_;
+ /** non-const version of the unscaled matrix */
+ SmartPtr<SymMatrix> nonconst_matrix_;
+
+ /** Matrix space stored as a SymScaledMatrixSpace */
+ SmartPtr<const SymScaledMatrixSpace> owner_space_;
+ };
+
+ /** This is the matrix space for SymScaledMatrix.
+ */
+ class SymScaledMatrixSpace : public SymMatrixSpace
+ {
+ public:
+ /** @name Constructors / Destructors */
+ //@{
+ /** Constructor, given the number of row and columns blocks, as
+ * well as the totel number of rows and columns.
+ */
+ SymScaledMatrixSpace(const SmartPtr<const Vector>& row_col_scaling,
+ bool row_col_scaling_reciprocal,
+ const SmartPtr<const SymMatrixSpace>& unscaled_matrix_space)
+ :
+ SymMatrixSpace(unscaled_matrix_space->Dim()),
+ unscaled_matrix_space_(unscaled_matrix_space)
+ {
+ scaling_ = row_col_scaling->MakeNewCopy();
+ if (row_col_scaling_reciprocal) {
+ scaling_->ElementWiseReciprocal();
+ }
+ }
+
+ /** Destructor */
+ ~SymScaledMatrixSpace()
+ {}
+ ;
+ //@}
+
+ /** Method for creating a new matrix of this specific type. */
+ SymScaledMatrix* MakeNewSymScaledMatrix(bool allocate_unscaled_matrix = false) const
+ {
+ SymScaledMatrix* ret = new SymScaledMatrix(this);
+ if (allocate_unscaled_matrix) {
+ SmartPtr<SymMatrix> unscaled_matrix = unscaled_matrix_space_->MakeNewSymMatrix();
+ ret->SetUnscaledMatrixNonConst(unscaled_matrix);
+ }
+ return ret;
+ }
+
+ /** Overloaded method from SymMatrixSpace */
+ virtual SymMatrix* MakeNewSymMatrix() const
+ {
+ return MakeNewSymScaledMatrix();
+ }
+ /** Overloaded MakeNew method for the MatrixSpace base class.
+ */
+ virtual Matrix* MakeNew() const
+ {
+ return MakeNewSymScaledMatrix();
+ }
+
+ /** return the vector for the row and column scaling */
+ SmartPtr<const Vector> RowColScaling() const
+ {
+ return ConstPtr(scaling_);
+ }
+
+ /** return the matrix space for the unscaled matrix */
+ SmartPtr<const SymMatrixSpace> UnscaledMatrixSpace() const
+ {
+ return unscaled_matrix_space_;
+ }
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default constructor */
+ SymScaledMatrixSpace();
+
+ /** Copy Constructor */
+ SymScaledMatrixSpace(const SymScaledMatrixSpace&);
+
+ /** Overloaded Equals Operator */
+ SymScaledMatrixSpace& operator=(const SymScaledMatrixSpace&);
+ //@}
+
+ /** Row scaling vector */
+ SmartPtr<Vector> scaling_;
+ /** unscaled matrix space */
+ SmartPtr<const SymMatrixSpace> unscaled_matrix_space_;
+ };
+
+ inline
+ void SymScaledMatrix::SetUnscaledMatrix(const SmartPtr<const SymMatrix> unscaled_matrix)
+ {
+ matrix_ = unscaled_matrix;
+ nonconst_matrix_ = NULL;
+ ObjectChanged();
+ }
+
+ inline
+ void SymScaledMatrix::SetUnscaledMatrixNonConst(const SmartPtr<SymMatrix>& unscaled_matrix)
+ {
+ nonconst_matrix_ = unscaled_matrix;
+ matrix_ = GetRawPtr(unscaled_matrix);
+ ObjectChanged();
+ }
+
+ inline
+ SmartPtr<const SymMatrix> SymScaledMatrix::GetUnscaledMatrix() const
+ {
+ return matrix_;
+ }
+
+ inline
+ SmartPtr<SymMatrix> SymScaledMatrix::GetUnscaledMatrixNonConst()
+ {
+ DBG_ASSERT(IsValid(nonconst_matrix_));
+ ObjectChanged();
+ return nonconst_matrix_;
+ }
+
+ inline SmartPtr<const Vector> SymScaledMatrix::RowColScaling() const
+ {
+ return ConstPtr(owner_space_->RowColScaling());
+ }
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpSymTMatrix.cpp b/SimTKmath/Optimizers/src/IpOpt/IpSymTMatrix.cpp
new file mode 100644
index 0000000..69752b0
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpSymTMatrix.cpp
@@ -0,0 +1,207 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpSymTMatrix.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpSymTMatrix.hpp"
+#include "IpDenseVector.hpp"
+#include "IpBlas.hpp"
+
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+
+
+namespace Ipopt
+{
+
+ SymTMatrix::SymTMatrix(const SymTMatrixSpace* owner_space)
+ :
+ SymMatrix(owner_space),
+ owner_space_(owner_space),
+ values_(NULL),
+ initialized_(false)
+ {
+ values_ = owner_space_->AllocateInternalStorage();
+
+ if (Nonzeros() == 0) {
+ initialized_ = true; // I guess ?!? what does this mean ?!?
+ }
+ }
+
+ SymTMatrix::~SymTMatrix()
+ {
+
+ owner_space_->FreeInternalStorage(values_);
+ }
+
+ void SymTMatrix::SetValues(const Number* Values)
+ {
+ IpBlasDcopy(Nonzeros(), Values, 1, values_, 1);
+ initialized_ = true;
+ ObjectChanged();
+ }
+
+ void SymTMatrix::MultVectorImpl(Number alpha, const Vector &x, Number beta,
+ Vector &y) const
+ {
+ // A few sanity checks
+ DBG_ASSERT(Dim()==x.Dim());
+ DBG_ASSERT(Dim()==y.Dim());
+
+ // Take care of the y part of the addition
+ DBG_ASSERT(initialized_);
+ if( beta!=0.0 ) {
+ y.Scal(beta);
+ }
+ else {
+ y.Set(0.0); // In case y hasn't been initialized yet
+ }
+
+ // See if we can understand the data
+ const DenseVector* dense_x = dynamic_cast<const DenseVector*>(&x);
+ DBG_ASSERT(dense_x); /* ToDo: Implement others */
+ DenseVector* dense_y = dynamic_cast<DenseVector*>(&y);
+ DBG_ASSERT(dense_y); /* ToDo: Implement others */
+
+ if (dense_x && dense_y) {
+ const Index* irn=Irows();
+ const Index* jcn=Jcols();
+ const Number* val=values_;
+ Number* yvals=dense_y->Values();
+
+ if (dense_x->IsHomogeneous()) {
+ Number as = alpha * dense_x->Scalar();
+ for(Index i=0; i<Nonzeros(); i++) {
+ yvals[*irn-1] += as * (*val);
+ if (*irn!=*jcn) {
+ // this is not a diagonal element
+ yvals[*jcn-1] += as * (*val);
+ }
+ val++;
+ irn++;
+ jcn++;
+ }
+ }
+ else {
+ const Number* xvals=dense_x->Values();
+ for(Index i=0; i<Nonzeros(); i++) {
+ yvals[*irn-1] += alpha* (*val) * xvals[*jcn-1];
+ if (*irn!=*jcn) {
+ // this is not a diagonal element
+ yvals[*jcn-1] += alpha* (*val) * xvals[*irn-1];
+ }
+ val++;
+ irn++;
+ jcn++;
+ }
+ }
+ }
+ }
+
+ Number* SymTMatrix::Values()
+ {
+ // cannot check for initialized values here, in case this pointer is
+ // requested for setting the first values
+ //DBG_ASSERT(initialized_);
+
+ // Here we assume that every time someone requests this direct raw
+ // pointer, the data is going to change and the Tag for this
+ // vector has to be updated.
+ ObjectChanged();
+ initialized_ = true;
+ return values_;
+ }
+
+ const Number* SymTMatrix::Values() const
+ {
+ DBG_ASSERT(initialized_);
+ return values_;
+ }
+
+ void SymTMatrix::FillStruct(ipfint* Irn, ipfint* Jcn) const
+ {
+ DBG_ASSERT(initialized_);
+ for(Index i=0; i<Nonzeros(); i++) {
+ Irn[i] = Irows()[i];
+ Jcn[i] = Jcols()[i];
+ }
+ }
+
+ void SymTMatrix::FillValues(Number* Values) const
+ {
+ DBG_ASSERT(initialized_);
+ IpBlasDcopy(Nonzeros(), values_, 1, Values, 1);
+ }
+
+ bool SymTMatrix::HasValidNumbersImpl() const
+ {
+ DBG_ASSERT(initialized_);
+ Number sum = IpBlasDasum(Nonzeros(), values_, 1);
+ return IsFiniteNumber(sum);
+ }
+
+ void SymTMatrix::PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ jnlst.Printf(level, category, "\n");
+ jnlst.PrintfIndented(level, category, indent,
+ "%sSymTMatrix \"%s\" with %d nonzero elements:\n",
+ prefix.c_str(), name.c_str(), Nonzeros());
+ if (initialized_) {
+ for (Index i=0; i<Nonzeros(); i++) {
+ jnlst.PrintfIndented(level, category, indent,
+ "%s%s[%5d,%5d]=%23.16e (%d)\n",
+ prefix.c_str(), name.c_str(), Irows()[i],
+ Jcols()[i], values_[i], i);
+ }
+ }
+ else {
+ jnlst.PrintfIndented(level, category, indent,
+ "%sUninitialized!\n", prefix.c_str());
+ }
+ }
+
+ SymTMatrixSpace::SymTMatrixSpace(Index dim, Index nonZeros,
+ const Index* iRows,
+ const Index* jCols)
+ :
+ SymMatrixSpace(dim),
+ nonZeros_(nonZeros),
+ iRows_(NULL),
+ jCols_(NULL)
+ {
+ iRows_ = new Index[nonZeros];
+ jCols_ = new Index[nonZeros];
+ for (Index i=0; i<nonZeros; i++) {
+ iRows_[i] = iRows[i];
+ jCols_[i] = jCols[i];
+ }
+ }
+
+ SymTMatrixSpace::~SymTMatrixSpace()
+ {
+ delete [] iRows_;
+ delete [] jCols_;
+ }
+
+ Number* SymTMatrixSpace::AllocateInternalStorage() const
+ {
+ return new Number[Nonzeros()];
+ }
+
+ void SymTMatrixSpace::FreeInternalStorage(Number* values) const
+ {
+ delete [] values;
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpSymTMatrix.hpp b/SimTKmath/Optimizers/src/IpOpt/IpSymTMatrix.hpp
new file mode 100644
index 0000000..b87c21b
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpSymTMatrix.hpp
@@ -0,0 +1,251 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpSymTMatrix.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPSYMTMATRIX_HPP__
+#define __IPSYMTMATRIX_HPP__
+
+#include "IpUtils.hpp"
+#include "IpSymMatrix.hpp"
+
+namespace Ipopt
+{
+
+ /* forward declarations */
+ class SymTMatrixSpace;
+
+ /** Class for symmetric matrices stored in triplet format. In the
+ * triplet format, the nonzeros elements of a symmetric matrix is
+ * stored in three arrays, Irn, Jcn, and Values, all of length
+ * Nonzeros. The first two arrays indicate the location of a
+ * non-zero element (as the row and column indices), and the last
+ * array stores the value at that location. Off-diagonal elements
+ * need to be stored only once since the matrix is symmetric. For
+ * example, the element \f$a_{1,2}=a_{2,1}\f$ would be stored only
+ * once, either with Irn[i]=1 and Jcn[i]=2, or with Irn[i]=2 and
+ * Jcn[i]=1. Both representations are identical. If nonzero
+ * elements (or their symmetric counter part) are listed more than
+ * once, their values are added.
+ *
+ * The structure of the nonzeros (i.e. the arrays Irn and Jcn)
+ * cannot be changed after the matrix can been initialized. Only
+ * the values of the nonzero elements can be modified.
+ *
+ * Note that the first row and column of a matrix has index 1, not
+ * 0.
+ *
+ */
+ class SymTMatrix : public SymMatrix
+ {
+ public:
+
+ /**@name Constructors / Destructors */
+ //@{
+
+ /** Constructor, taking the corresponding matrix space.
+ */
+ SymTMatrix(const SymTMatrixSpace* owner_space);
+
+ /** Destructor */
+ ~SymTMatrix();
+ //@}
+
+ /**@name Changing the Values.*/
+ //@{
+ /** Set values of nonzero elements. The values of the nonzero
+ * elements is copied from the incoming Number array. Important:
+ * It is assume that the order of the values in Values
+ * corresponds to the one of Irn and Jcn given to the matrix
+ * space. */
+ void SetValues(const Number* Values);
+ //@}
+
+ /** @name Accessor Methods */
+ //@{
+ /** Number of nonzero entries */
+ Index Nonzeros() const;
+
+ /** Obtain pointer to the internal Index array irn_ without the
+ * intention to change the matrix data (USE WITH CARE!). This
+ * does not produce a copy, and lifetime is not guaranteed!
+ */
+ const Index* Irows() const;
+
+ /** Obtain pointer to the internal Index array jcn_ without the
+ * intention to change the matrix data (USE WITH CARE!). This
+ * does not produce a copy, and lifetime is not guaranteed!
+ */
+ const Index* Jcols() const;
+
+ /** Obtain pointer to the internal Number array values_ with the
+ * intention to change the matrix data (USE WITH CARE!). This
+ * does not produce a copy, and lifetime is not guaranteed!
+ */
+ Number* Values();
+ /** Obtain pointer to the internal Number array values_ without the
+ * intention to change the matrix data (USE WITH CARE!). This
+ * does not produce a copy, and lifetime is not guaranteed!
+ */
+ const Number* Values() const;
+ //@}
+
+ /**@name Methods for providing copy of the matrix data */
+ //@{
+ /** Copy the nonzero structure into provided space */
+ void FillStruct(ipfint* Irn, ipfint* Jcn) const;
+
+ /** Copy the value data into provided space */
+ void FillValues(Number* Values) const;
+ //@}
+
+ protected:
+ /**@name Methods overloaded from matrix */
+ //@{
+ virtual void MultVectorImpl(Number alpha, const Vector& x, Number beta,
+ Vector& y) const;
+
+ /** Method for determining if all stored numbers are valid (i.e.,
+ * no Inf or Nan). */
+ virtual bool HasValidNumbersImpl() const;
+
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const;
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ SymTMatrix();
+
+ /** Copy Constructor */
+ SymTMatrix(const SymTMatrix&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const SymTMatrix&);
+ //@}
+
+ /** Copy of the owner_space ptr as a SymTMatrixSpace insteaqd
+ * of a MatrixSpace
+ */
+ const SymTMatrixSpace* owner_space_;
+
+ /** Values of nonzeros */
+ Number* values_;
+
+ /** Flag for Initialization */
+ bool initialized_;
+
+ };
+
+ /** This is the matrix space for a SymTMatrix with fixed sparsity
+ * structure. The sparsity structure is stored here in the matrix
+ * space.
+ */
+ class SymTMatrixSpace : public SymMatrixSpace
+ {
+ public:
+ /** @name Constructors / Destructors */
+ //@{
+ /** Constructor, given the number of rows and columns (both as
+ * dim), as well as the number of nonzeros and the position of
+ * the nonzero elements. Note that the counting of the nonzeros
+ * starts a 1, i.e., iRows[i]==1 and jCols[i]==1 refers to the
+ * first element in the first row. This is in accordance with
+ * the HSL data structure. Off-diagonal elements are stored only
+ * once.
+ */
+ SymTMatrixSpace(Index dim, Index nonZeros, const Index* iRows,
+ const Index* jCols);
+
+ /** Destructor */
+ ~SymTMatrixSpace();
+ //@}
+
+ /** Overloaded MakeNew method for the sYMMatrixSpace base class.
+ */
+ virtual SymMatrix* MakeNewSymMatrix() const
+ {
+ return MakeNewSymTMatrix();
+ }
+
+ /** Method for creating a new matrix of this specific type. */
+ SymTMatrix* MakeNewSymTMatrix() const
+ {
+ return new SymTMatrix(this);
+ }
+
+ /**@name Methods describing Matrix structure */
+ //@{
+ /** Number of non-zeros in the sparse matrix */
+ Index Nonzeros() const
+ {
+ return nonZeros_;
+ }
+
+ /** Row index of each non-zero element */
+ const Index* Irows() const
+ {
+ return iRows_;
+ }
+
+ /** Column index of each non-zero element */
+ const Index* Jcols() const
+ {
+ return jCols_;
+ }
+ //@}
+
+ private:
+ /**@name Methods called by SymTMatrix for memory management */
+ //@{
+ /** Allocate internal storage for the SymTMatrix values */
+ Number* AllocateInternalStorage() const;
+
+ /** Deallocate internal storage for the SymTMatrix values */
+ void FreeInternalStorage(Number* values) const;
+ //@}
+
+ const Index nonZeros_;
+ Index* iRows_;
+ Index* jCols_;
+
+ friend class SymTMatrix;
+ };
+
+ /* Inline Methods */
+ inline
+ Index SymTMatrix::Nonzeros() const
+ {
+ return owner_space_->Nonzeros();
+ }
+
+ inline
+ const Index* SymTMatrix::Irows() const
+ {
+ return owner_space_->Irows();
+ }
+
+ inline
+ const Index* SymTMatrix::Jcols() const
+ {
+ return owner_space_->Jcols();
+ }
+
+
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpTNLP.hpp b/SimTKmath/Optimizers/src/IpOpt/IpTNLP.hpp
new file mode 100644
index 0000000..46aa463
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpTNLP.hpp
@@ -0,0 +1,234 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpTNLP.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPTNLP_HPP__
+#define __IPTNLP_HPP__
+
+#include "IpUtils.hpp"
+#include "IpReferenced.hpp"
+#include "IpException.hpp"
+#include "IpAlgTypes.hpp"
+#include "IpReturnCodes.hpp"
+
+namespace Ipopt
+{
+ // forward declarations
+ class IpoptData;
+ class IpoptCalculatedQuantities;
+ class IteratesVector;
+
+ /** Base class for all NLP's that use standard triplet matrix form
+ * and dense vectors. This is the standard base class for all
+ * NLP's that use the standard triplet matrix form (as for Harwell
+ * routines) and dense vectors. The class TNLPAdapter then converts
+ * this interface to an interface that can be used directly by
+ * ipopt.
+ *
+ * This interface presents the problem form:
+ *
+ * min f(x)
+ *
+ * s.t. gL <= g(x) <= gU
+ *
+ * xL <= x <= xU
+ *
+ * In order to specify an equality constraint, set gL_i = gU_i =
+ * rhs. The value that indicates "infinity" for the bounds
+ * (i.e. the variable or constraint has no lower bound (-infinity)
+ * or upper bound (+infinity)) is set through the option
+ * nlp_lower_bound_inf and nlp_upper_bound_inf. To indicate that a
+ * variable has no upper or lower bound, set the bound to
+ * -ipopt_inf or +ipopt_inf respectively
+ */
+ class TNLP : public ReferencedObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ TNLP()
+ {}
+ ;
+
+ /** Default destructor */
+ virtual ~TNLP()
+ {}
+ ;
+ //@}
+
+ DECLARE_STD_EXCEPTION(INVALID_TNLP);
+
+ /**@name methods to gather information about the NLP */
+ //@{
+ /** overload this method to return the number of variables
+ * and constraints, and the number of non-zeros in the jacobian and
+ * the hessian. The index_style parameter lets you specify C or Fortran
+ * style indexing for the sparse matrix iRow and jCol parameters.
+ * C_STYLE is 0-based, and FORTRAN_STYLE is 1-based.
+ */
+ enum IndexStyleEnum { C_STYLE=0, FORTRAN_STYLE=1 };
+ virtual bool get_nlp_info(Index& n, Index& m, Index& nnz_jac_g,
+ Index& nnz_h_lag, IndexStyleEnum& index_style)=0;
+
+ /** overload this method to return the information about the bound
+ * on the variables and constraints. The value that indicates
+ * that a bound does not exist is specified in the parameters
+ * nlp_lower_bound_inf and nlp_upper_bound_inf. By default,
+ * nlp_lower_bound_inf is -1e19 and nlp_upper_bound_inf is
+ * 1e19. (see TNLPAdapter) */
+ virtual bool get_bounds_info(Index n, Number* x_l, Number* x_u,
+ Index m, Number* g_l, Number* g_u)=0;
+
+ /** overload this method to return scaling parameters. This is
+ * only called if the options are set to retrieve user scaling.
+ * There, use_x_scaling (or use_g_scaling) should get set to true
+ * only if the variables (or constraints) are to be scaled. This
+ * method should return true only if the scaling parameters could
+ * be provided.
+ */
+ virtual bool get_scaling_parameters(Number& obj_scaling,
+ bool& use_x_scaling, Index n,
+ Number* x_scaling,
+ bool& use_g_scaling, Index m,
+ Number* g_scaling)
+ {
+ return false;
+ }
+
+ /** overload this method to return the starting point. The bools
+ * init_x and init_lambda are both inputs and outputs. As inputs,
+ * they indicate whether or not the algorithm wants you to
+ * initialize x and lambda respectively. If, for some reason, the
+ * algorithm wants you to initialize these and you cannot, set
+ * the respective bool to false.
+ */
+ virtual bool get_starting_point(Index n, bool init_x, Number* x,
+ bool init_z, Number* z_L, Number* z_U,
+ Index m, bool init_lambda,
+ Number* lambda)=0;
+
+ /** overload this method to provide an Ipopt iterate (already in
+ * the form Ipopt requires it internally) for a warm start.
+ * Since this is only for expert users, a default dummy
+ * implementation is provided and returns false. */
+ virtual bool get_warm_start_iterate(IteratesVector& warm_start_iterate)
+ {
+ return false;
+ }
+
+ /** overload this method to return the value of the objective function */
+ virtual bool eval_f(Index n, const Number* x, bool new_x,
+ Number& obj_value)=0;
+
+ /** overload this method to return the vector of the gradient of
+ * the objective w.r.t. x */
+ virtual bool eval_grad_f(Index n, const Number* x, bool new_x,
+ Number* grad_f)=0;
+
+ /** overload this method to return the vector of constraint values */
+ virtual bool eval_g(Index n, const Number* x, bool new_x,
+ Index m, Number* g)=0;
+ /** overload this method to return the jacobian of the
+ * constraints. The vectors iRow and jCol only need to be set
+ * once. The first call is used to set the structure only (iRow
+ * and jCol will be non-NULL, and values will be NULL) For
+ * subsequent calls, iRow and jCol will be NULL. */
+ virtual bool eval_jac_g(Index n, const Number* x, bool new_x,
+ Index m, Index nele_jac, Index* iRow,
+ Index *jCol, Number* values)=0;
+
+ /** overload this method to return the hessian of the
+ * lagrangian. The vectors iRow and jCol only need to be set once
+ * (during the first call). The first call is used to set the
+ * structure only (iRow and jCol will be non-NULL, and values
+ * will be NULL) For subsequent calls, iRow and jCol will be
+ * NULL. This matrix is symmetric - specify the lower diagonal
+ * only. A default implementation is provided, in case the user
+ * wants to se quasi-Newton approximations to estimate the second
+ * derivatives and doesn't not neet to implement this method. */
+ virtual bool eval_h(Index n, const Number* x, bool new_x,
+ Number obj_factor, Index m, const Number* lambda,
+ bool new_lambda, Index nele_hess,
+ Index* iRow, Index* jCol, Number* values)
+ {
+ return false;
+ }
+ //@}
+
+ /** @name Solution Methods */
+ //@{
+ /** This method is called when the algorithm is complete so the TNLP can store/write the solution */
+ virtual void finalize_solution(SolverReturn status,
+ Index n, const Number* x, const Number* z_L, const Number* z_U,
+ Index m, const Number* g, const Number* lambda,
+ Number obj_value)=0;
+
+ /** Intermediate Callback method for the user. Providing dummy
+ * default implementation. For details see IntermediateCallBack
+ * in IpNLP.hpp. */
+ virtual bool intermediate_callback(AlgorithmMode mode,
+ Index iter, Number obj_value,
+ Number inf_pr, Number inf_du,
+ Number mu, Number d_norm,
+ Number regularization_size,
+ Number alpha_du, Number alpha_pr,
+ Index ls_trials,
+ const IpoptData* ip_data,
+ IpoptCalculatedQuantities* ip_cq)
+ {
+ return true;
+ }
+ //@}
+
+ /** @name Methods for quasi-Newton approximation. If the second
+ * derivatives are approximated by Ipopt, it is better to do this
+ * only in the space of nonlinear variables. The following
+ * methods are call by Ipopt if the quasi-Newton approximation is
+ * selected. If -1 is returned as number of nonlinear variables,
+ * Ipopt assumes that all variables are nonlinear. Otherwise, it
+ * calls get_list_of_nonlinear_variables with an array into which
+ * the indices of the nonlinear variables should be written - the
+ * array has the lengths num_nonlin_vars, which is identical with
+ * the return value of get_number_of_nonlinear_variables(). It
+ * is assumed that the indices are counted starting with 1 in the
+ * FORTRAN_STYLE, and 0 for the C_STYLE. */
+ //@{
+ virtual Index get_number_of_nonlinear_variables()
+ {
+ return -1;
+ }
+
+ virtual bool get_list_of_nonlinear_variables(Index num_nonlin_vars,
+ Index* pos_nonlin_vars)
+ {
+ return false;
+ }
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ //TNLP();
+
+ /** Copy Constructor */
+ TNLP(const TNLP&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const TNLP&);
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpTNLPAdapter.cpp b/SimTKmath/Optimizers/src/IpOpt/IpTNLPAdapter.cpp
new file mode 100644
index 0000000..f166c97
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpTNLPAdapter.cpp
@@ -0,0 +1,1908 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpTNLPAdapter.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpTNLPAdapter.hpp"
+#include "IpBlas.hpp"
+#include "IpIpoptData.hpp"
+#include "IpIpoptCalculatedQuantities.hpp"
+#include "IpDenseVector.hpp"
+#include "IpExpansionMatrix.hpp"
+#include "IpGenTMatrix.hpp"
+#include "IpSymTMatrix.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+#include <cstdio>
+
+// Keeps MS VC++ 8 quiet about sprintf, strcpy, etc.
+#ifdef _MSC_VER
+#pragma warning(disable:4996)
+#endif
+
+
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ TNLPAdapter::TNLPAdapter(const SmartPtr<TNLP> tnlp,
+ const SmartPtr<const Journalist> jnlst /* = NULL */)
+ :
+ tnlp_(tnlp),
+ jnlst_(jnlst),
+ n_full_x_(-1),
+ n_full_g_(-1),
+ nz_jac_c_(-1),
+ nz_jac_c_no_fixed_(-1),
+ nz_jac_d_(-1),
+ nz_full_jac_g_(-1),
+ nz_full_h_(-1),
+ nz_h_(-1),
+ full_x_(NULL),
+ full_lambda_(NULL),
+ full_g_(NULL),
+ jac_g_(NULL),
+ c_rhs_(NULL),
+ x_tag_for_iterates_(0),
+ y_c_tag_for_iterates_(0),
+ y_d_tag_for_iterates_(0),
+ x_tag_for_g_(0),
+ x_tag_for_jac_g_(0),
+ jac_idx_map_(NULL),
+ h_idx_map_(NULL),
+ x_fixed_map_(NULL)
+ {
+ ASSERT_EXCEPTION(IsValid(tnlp_), INVALID_TNLP,
+ "The TNLP passed to TNLPAdapter is NULL. This MUST be a valid TNLP!");
+ }
+
+ TNLPAdapter::~TNLPAdapter()
+ {
+ delete [] full_x_;
+ delete [] full_lambda_;
+ delete [] full_g_;
+ delete [] jac_g_;
+ delete [] c_rhs_;
+ delete [] jac_idx_map_;
+ delete [] h_idx_map_;
+ delete [] x_fixed_map_;
+ }
+
+ void TNLPAdapter::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->SetRegisteringCategory("NLP");
+ roptions->AddNumberOption(
+ "nlp_lower_bound_inf",
+ "any bound less or equal this value will be considered -inf (i.e. not lower bounded).",
+ -1e19);
+ roptions->AddNumberOption(
+ "nlp_upper_bound_inf",
+ "any bound greater or this value will be considered +inf (i.e. not upper bounded).",
+ 1e19);
+ roptions->AddStringOption2(
+ "fixed_variable_treatment",
+ "Determines how fixed variables should be handled.",
+ "make_parameter",
+ "make_parameter", "Remove fixed variable from optimization variables",
+ "make_constraint", "Add equality constraints fixing variables",
+ "The main difference between those two options is that the starting "
+ "point in the \"make_constraint\" case still has the fixed variables at "
+ "their given values, whereas in the other case the functions are always "
+ "evaluated with the fixed values for those variables. Also, for "
+ "\"make_constraints\", bound multipliers are computed for the fixed "
+ "variables.");
+ roptions->SetRegisteringCategory("Derivative Checker");
+ roptions->AddStringOption3(
+ "derivative_test",
+ "Enable derivative checker",
+ "none",
+ "none", "do not perform derivative test",
+ "first-order", "perform test of first derivatives at starting point",
+ "second-order", "perform test of first and second derivatives at starting point",
+ "If this option is enabled, a (slow) derivative test will be performed "
+ "before the optimization. The test is performed at the user provided "
+ "starting point and marks derivative values that seem suspicious");
+ roptions->AddLowerBoundedNumberOption(
+ "derivative_test_perturbation",
+ "Size of the finite difference perturbation in derivative test.",
+ 0., true,
+ 1e-8,
+ "This determines the relative perturbation of the variable entries.");
+ roptions->AddLowerBoundedNumberOption(
+ "derivative_test_tol",
+ "Threshold for indicating wrong derivative.",
+ 0., true,
+ 1e-4,
+ "If the relative deviation of the estimated derivative from the given "
+ "one is larger than this value, the corresponding derivative is marked "
+ "as wrong.");
+ roptions->AddStringOption2(
+ "derivative_test_print_all",
+ "Indicates whether information for all estimated derivatives should be printed.",
+ "no",
+ "no", "Print only suspect derivatives",
+ "yes", "Print all derivatives",
+ "Determines verbosity of derivative checker.");
+ }
+
+ bool TNLPAdapter::ProcessOptions(const OptionsList& options,
+ const std::string& prefix)
+ {
+ DBG_START_METH("TNLPAdapter::ProcessOptions", dbg_verbosity);
+ options.GetNumericValue("nlp_lower_bound_inf", nlp_lower_bound_inf_, prefix);
+ options.GetNumericValue("nlp_upper_bound_inf", nlp_upper_bound_inf_, prefix);
+
+ ASSERT_EXCEPTION(nlp_lower_bound_inf_ < nlp_upper_bound_inf_,
+ OPTION_INVALID,
+ "Option \"nlp_lower_bound_inf\" must be smaller than \"nlp_upper_bound_inf\".");
+
+ Index enum_int;
+ options.GetEnumValue("fixed_variable_treatment", enum_int, prefix);
+ fixed_variable_treatment_ = FixedVariableTreatmentEnum(enum_int);
+ options.GetEnumValue("derivative_test", enum_int, prefix);
+ derivative_test_ = DerivativeTestEnum(enum_int);
+ options.GetNumericValue("derivative_test_perturbation",
+ derivative_test_perturbation_, prefix);
+ options.GetNumericValue("derivative_test_tol",
+ derivative_test_tol_, prefix);
+ options.GetBoolValue("derivative_test_print_all",
+ derivative_test_print_all_, prefix);
+
+ // The option warm_start_same_structure is registered by OrigIpoptNLP
+ options.GetBoolValue("warm_start_same_structure",
+ warm_start_same_structure_, prefix);
+ // The following is registered in OrigIpoptNLP
+ options.GetEnumValue("hessian_approximation", enum_int, prefix);
+ hessian_approximation_ = HessianApproximationType(enum_int);
+
+ return true;
+ }
+
+ bool TNLPAdapter::GetSpaces(SmartPtr<const VectorSpace>& x_space,
+ SmartPtr<const VectorSpace>& c_space,
+ SmartPtr<const VectorSpace>& d_space,
+ SmartPtr<const VectorSpace>& x_l_space,
+ SmartPtr<const MatrixSpace>& px_l_space,
+ SmartPtr<const VectorSpace>& x_u_space,
+ SmartPtr<const MatrixSpace>& px_u_space,
+ SmartPtr<const VectorSpace>& d_l_space,
+ SmartPtr<const MatrixSpace>& pd_l_space,
+ SmartPtr<const VectorSpace>& d_u_space,
+ SmartPtr<const MatrixSpace>& pd_u_space,
+ SmartPtr<const MatrixSpace>& Jac_c_space,
+ SmartPtr<const MatrixSpace>& Jac_d_space,
+ SmartPtr<const SymMatrixSpace>& Hess_lagrangian_space)
+ {
+ DBG_START_METH("TNLPAdapter::GetSpaces", dbg_verbosity);
+
+ // First, if required, perform derivative test
+ if (derivative_test_ != NO_TEST) {
+ bool retval = CheckDerivatives(derivative_test_);
+ if (!retval) {
+ return retval;
+ }
+ }
+
+ if (warm_start_same_structure_) {
+ ASSERT_EXCEPTION(full_x_, INVALID_WARMSTART,
+ "warm_start_same_structure chosen, but TNLPAdapter is called for the first time.");
+ if (IsValid(jnlst_)) {
+ jnlst_->Printf(J_DETAILED, J_INITIALIZATION,
+ "Reusing previous information for warm start in TNLPAdapter.\n");
+ }
+ }
+ else {
+ // In case the Adapter has been used before, but this is not a
+ // warm start, make sure we delete all previously allocated
+ // memory
+ delete [] full_x_;
+ delete [] full_lambda_;
+ delete [] full_g_;
+ delete [] jac_g_;
+ delete [] c_rhs_;
+ delete [] jac_idx_map_;
+ delete [] h_idx_map_;
+ delete [] x_fixed_map_;
+ }
+
+ // Get the full dimensions of the problem
+ Index n_full_x, n_full_g, nz_full_jac_g, nz_full_h;
+ tnlp_->get_nlp_info(n_full_x, n_full_g, nz_full_jac_g,
+ nz_full_h, index_style_);
+ ASSERT_EXCEPTION(!warm_start_same_structure_ ||
+ (n_full_x == n_full_x_ &&
+ n_full_g == n_full_g_ &&
+ nz_full_jac_g == nz_full_jac_g_ &&
+ nz_full_h == nz_full_h_),
+ INVALID_WARMSTART,
+ "warm_start_same_structure chosen, but problem dimensions are different.");
+ n_full_x_ = n_full_x;
+ n_full_g_ = n_full_g;
+ nz_full_jac_g_ = nz_full_jac_g;
+ nz_full_h_ = nz_full_h;
+
+ if (!warm_start_same_structure_) {
+ // create space to store vectors that are the full length of x
+ full_x_ = new Number[n_full_x_];
+
+ // create space to store vectors that area the full length of lambda
+ full_lambda_ = new Number[n_full_g_];
+
+ // create space to store vectors that are the full length of g
+ full_g_ = new Number[n_full_g_];
+
+ // allocate internal space to store the full jacobian
+ jac_g_ = new Number[nz_full_jac_g_];
+
+ //*********************************************************
+ // Create the spaces and permutation spaces
+ //*********************************************************
+ /* Spaces for x, x_L, and x_U. We need to remove the fixed variables
+ * and find out which bounds do not exist. */
+ Number* x_l = new Number[n_full_x_];
+ Number* x_u = new Number[n_full_x_];
+ Number* g_l = new Number[n_full_g_];
+ Number* g_u = new Number[n_full_g_];
+ tnlp_->get_bounds_info(n_full_x_, x_l, x_u, n_full_g_, g_l, g_u);
+
+ Index n_x_var = 0;
+ Index n_x_l = 0;
+ Index n_x_u = 0;
+ Index* x_not_fixed_map = new Index[n_full_x_];
+ Index* x_fixed_map_tmp = new Index[n_full_x_];
+ Index* x_l_map = new Index[n_full_x_];
+ Index* x_u_map = new Index[n_full_x_];
+ n_x_fixed_ = 0;
+
+ for (Index i=0; i<n_full_x_; i++) {
+ Number lower_bound = x_l[i];
+ Number upper_bound = x_u[i];
+ if (lower_bound == upper_bound) {
+ switch (fixed_variable_treatment_) {
+ case MAKE_PARAMETER:
+ // Variable is fixed, remove it from the problem
+ full_x_[i] = lower_bound;
+ x_fixed_map_tmp[n_x_fixed_] = i;
+ n_x_fixed_++;
+ break;
+ case MAKE_CONSTRAINT:
+ x_fixed_map_tmp[n_x_fixed_] = i; // don't really need this
+ // array then
+ n_x_fixed_++;
+ x_not_fixed_map[n_x_var] = i;
+ n_x_var++;
+ break;
+ default:
+ DBG_ASSERT(false && "invalid fixed_variable_treatment_");
+ }
+ }
+ else if (lower_bound > upper_bound) {
+ char string[128];
+ sprintf(string, "There are inconsistent bounds on variable %d: lower = %25.16e and upper = %25.16e.", i, lower_bound, upper_bound);
+ THROW_EXCEPTION(INVALID_TNLP, string);
+ }
+ else {
+ x_not_fixed_map[n_x_var] = i;
+ if (lower_bound > nlp_lower_bound_inf_) {
+ x_l_map[n_x_l] = n_x_var;
+ n_x_l++;
+ }
+
+ if (upper_bound < nlp_upper_bound_inf_) {
+ x_u_map[n_x_u] = n_x_var;
+ n_x_u++;
+ }
+ n_x_var++;
+ }
+ }
+
+ // If there are fixed variables, we keep their position around
+ // for a possible warm start later or if fixed variables are
+ // treated by added equality constraints
+ if (n_x_fixed_>0) {
+ x_fixed_map_ = new Index[n_x_fixed_];
+ for (Index i=0; i<n_x_fixed_; i++) {
+ x_fixed_map_[i] = x_fixed_map_tmp[i];
+ }
+ }
+ else {
+ x_fixed_map_ = NULL;
+ }
+ delete [] x_fixed_map_tmp;
+
+ x_space_ = new DenseVectorSpace(n_x_var);
+ x_l_space_ = new DenseVectorSpace(n_x_l);
+ x_u_space_ = new DenseVectorSpace(n_x_u);
+
+ if (n_x_fixed_>0 && fixed_variable_treatment_==MAKE_PARAMETER) {
+ P_x_full_x_space_ = new ExpansionMatrixSpace(n_full_x_, n_x_var, x_not_fixed_map);
+ P_x_full_x_ = P_x_full_x_space_->MakeNewExpansionMatrix();
+ }
+ else {
+ P_x_full_x_space_ = NULL;
+ P_x_full_x_ = NULL;
+ }
+
+ P_x_x_L_space_ = new ExpansionMatrixSpace(n_x_var, n_x_l, x_l_map);
+ px_l_space_ = GetRawPtr(P_x_x_L_space_);
+ P_x_x_L_ = P_x_x_L_space_->MakeNewExpansionMatrix();
+ P_x_x_U_space_ = new ExpansionMatrixSpace(n_x_var, n_x_u, x_u_map);
+ px_u_space_ = GetRawPtr(P_x_x_U_space_);
+ P_x_x_U_ = P_x_x_U_space_->MakeNewExpansionMatrix();
+
+ delete [] x_l;
+ x_l = NULL;
+ delete [] x_u;
+ x_u = NULL;
+ delete [] x_not_fixed_map;
+ x_not_fixed_map = NULL;
+ delete [] x_l_map;
+ x_l_map = NULL;
+ delete [] x_u_map;
+ x_u_map = NULL;
+
+ // Create the spaces for c and d
+ // - includes the internal permutation matrices for
+ // full_g to c and d
+ // - includes the permutation matrices for d_l and d_u
+ // c(x) = (P_c)T * g(x)
+ // d(x) = (P_d)T * g(x)
+ // d_L = (P_d_L)T * (P_d)T * g_l
+ // d_U = (P_d_U)T * (P_d)T * g_u
+ Index n_c = 0;
+ Index n_d = 0;
+ Index n_d_l = 0;
+ Index n_d_u = 0;
+
+ Index* c_map = new Index[n_full_g_]; // we do not know n_c yet!
+ Index* d_map = new Index[n_full_g_]; // we do not know n_d yet!
+ Index* d_l_map = new Index[n_full_g_]; // "
+ Index* d_u_map = new Index[n_full_g_]; // "
+ for (Index i=0; i<n_full_g_; i++) {
+ Number lower_bound = g_l[i];
+ Number upper_bound = g_u[i];
+ if (lower_bound == upper_bound) {
+ // equality constraint
+ c_map[n_c] = i;
+ n_c++;
+ }
+ else if (lower_bound > upper_bound) {
+ char string[128];
+ sprintf(string, "There are inconsistent bounds on constraint %d: lower = %25.16e and upper = %25.16e.", i, lower_bound, upper_bound);
+ THROW_EXCEPTION(INVALID_TNLP, string);
+ }
+ else {
+ // inequality constraint
+ d_map[n_d] = i;
+ if (lower_bound > nlp_lower_bound_inf_) {
+ d_l_map[n_d_l] = n_d;
+ n_d_l++;
+ }
+ if (upper_bound < nlp_upper_bound_inf_) {
+ d_u_map[n_d_u] = n_d;
+ n_d_u++;
+ }
+ n_d++;
+ }
+ }
+
+ // create the required c_space
+
+ if (n_x_fixed_==0 || fixed_variable_treatment_==MAKE_PARAMETER) {
+ SmartPtr<DenseVectorSpace> dc_space = new DenseVectorSpace(n_c);
+ c_space_ = GetRawPtr(dc_space);
+ c_rhs_ = new Number[n_c];
+ }
+ else {
+ SmartPtr<DenseVectorSpace> dc_space =
+ new DenseVectorSpace(n_c+n_x_fixed_);
+ c_space_ = GetRawPtr(dc_space);
+ c_rhs_ = new Number[n_c+n_x_fixed_];
+ }
+ // create the internal expansion matrix for c to g
+ P_c_g_space_ = new ExpansionMatrixSpace(n_full_g_, n_c, c_map);
+ P_c_g_ = P_c_g_space_->MakeNewExpansionMatrix();
+ delete [] c_map;
+ c_map = NULL;
+
+ // create the required d_space
+ d_space_ = new DenseVectorSpace(n_d);
+ // create the internal expansion matrix for d to g
+ P_d_g_space_ = new ExpansionMatrixSpace(n_full_g_, n_d, d_map);
+ P_d_g_ = P_d_g_space_->MakeNewExpansionMatrix();
+ delete [] d_map;
+ d_map = NULL;
+
+ // create the required d_l space
+ d_l_space_ = new DenseVectorSpace(n_d_l);
+ // create the required expansion matrix for d_L to d_L_exp
+ pd_l_space_ = new ExpansionMatrixSpace(n_d, n_d_l, d_l_map);
+ delete [] d_l_map;
+ d_l_map = NULL;
+
+ // create the required d_u space
+ d_u_space_ = new DenseVectorSpace(n_d_u);
+ // create the required expansion matrix for d_U to d_U_exp
+ pd_u_space_ = new ExpansionMatrixSpace(n_d, n_d_u, d_u_map);
+ delete [] d_u_map;
+ d_u_map = NULL;
+
+ delete [] g_l;
+ g_l = NULL;
+ delete [] g_u;
+ g_u = NULL;
+
+ /** Create the matrix space for the jacobians
+ */
+ // Get the non zero structure
+ Index* g_iRow = new Index[nz_full_jac_g_];
+ Index* g_jCol = new Index[nz_full_jac_g_];
+ tnlp_->eval_jac_g(n_full_x_, NULL, false, n_full_g_, nz_full_jac_g_,
+ g_iRow, g_jCol, NULL);
+
+ if (index_style_ != TNLP::FORTRAN_STYLE) {
+ for (Index i=0; i<nz_full_jac_g_; i++) {
+ g_iRow[i] += 1;
+ g_jCol[i] += 1;
+ }
+ }
+
+ // ... build the non-zero structure for jac_c
+ // ... (the permutation from rows in jac_g to jac_c is
+ // ... the same as P_c_g_)
+ Index nz_jac_all;
+ if (fixed_variable_treatment_==MAKE_PARAMETER) {
+ nz_jac_all = nz_full_jac_g_;
+ }
+ else {
+ nz_jac_all = nz_full_jac_g_ + n_x_fixed_;
+ }
+ jac_idx_map_ = new Index[nz_jac_all];
+ Index* jac_c_iRow = new Index[nz_jac_all];
+ Index* jac_c_jCol = new Index[nz_jac_all];
+ Index current_nz = 0;
+ const Index* c_row_pos = P_c_g_->CompressedPosIndices();
+ if (IsValid(P_x_full_x_)) {
+ // there are missing variables x
+ const Index* c_col_pos = P_x_full_x_->CompressedPosIndices();
+ for (Index i=0; i<nz_full_jac_g_; i++) {
+ const Index c_row = c_row_pos[g_iRow[i]-1];
+ const Index c_col = c_col_pos[g_jCol[i]-1];
+ if (c_col != -1 && c_row != -1) {
+ jac_idx_map_[current_nz] = i;
+ jac_c_iRow[current_nz] = c_row + 1;
+ jac_c_jCol[current_nz] = c_col + 1;
+ current_nz++;
+ }
+ }
+ }
+ else {
+ for (Index i=0; i<nz_full_jac_g_; i++) {
+ const Index c_row = c_row_pos[g_iRow[i]-1];
+ const Index c_col = g_jCol[i]-1;
+ if (c_row != -1) {
+ jac_idx_map_[current_nz] = i;
+ jac_c_iRow[current_nz] = c_row + 1;
+ jac_c_jCol[current_nz] = c_col + 1;
+ current_nz++;
+ }
+ }
+ }
+ nz_jac_c_no_fixed_ = current_nz;
+ Index n_added_constr;
+ if (fixed_variable_treatment_==MAKE_PARAMETER) {
+ nz_jac_c_ = nz_jac_c_no_fixed_;
+ n_added_constr = 0;
+ }
+ else {
+ nz_jac_c_ = nz_jac_c_no_fixed_ + n_x_fixed_;
+ for (Index i=0; i<n_x_fixed_; i++) {
+ jac_c_iRow[current_nz] = n_c + i + 1;
+ jac_c_jCol[current_nz] = x_fixed_map_[i]+1;
+ current_nz++;
+ }
+ n_added_constr = n_x_fixed_;
+ }
+
+ Jac_c_space_ = new GenTMatrixSpace(n_c+n_added_constr, n_x_var,
+ nz_jac_c_, jac_c_iRow, jac_c_jCol);
+ delete [] jac_c_iRow;
+ jac_c_iRow = NULL;
+ delete [] jac_c_jCol;
+ jac_c_jCol = NULL;
+
+ // ... build the nonzero structure for jac_d
+ // ... (the permuation from rows in jac_g to jac_c is the
+ // ... the same as P_d_g_)
+ Index* jac_d_iRow = new Index[nz_full_jac_g_];
+ Index* jac_d_jCol = new Index[nz_full_jac_g_];
+ current_nz = 0;
+ const Index* d_row_pos = P_d_g_->CompressedPosIndices();
+ if (IsValid(P_x_full_x_)) {
+ const Index* d_col_pos = P_x_full_x_->CompressedPosIndices();
+ for (Index i=0; i<nz_full_jac_g_; i++) {
+ const Index d_row = d_row_pos[g_iRow[i]-1];
+ const Index d_col = d_col_pos[g_jCol[i]-1];
+ if (d_col != -1 && d_row != -1) {
+ jac_idx_map_[current_nz + nz_jac_c_no_fixed_] = i;
+ jac_d_iRow[current_nz] = d_row + 1;
+ jac_d_jCol[current_nz] = d_col + 1;
+ current_nz++;
+ }
+ }
+ }
+ else {
+ for (Index i=0; i<nz_full_jac_g_; i++) {
+ const Index d_row = d_row_pos[g_iRow[i]-1];
+ const Index d_col = g_jCol[i]-1;
+ if (d_row != -1) {
+ jac_idx_map_[current_nz + nz_jac_c_no_fixed_] = i;
+ jac_d_iRow[current_nz] = d_row + 1;
+ jac_d_jCol[current_nz] = d_col + 1;
+ current_nz++;
+ }
+ }
+ }
+ nz_jac_d_ = current_nz;
+ Jac_d_space_ = new GenTMatrixSpace(n_d, n_x_var, nz_jac_d_, jac_d_iRow, jac_d_jCol);
+ delete [] jac_d_iRow;
+ jac_d_iRow = NULL;
+ delete [] jac_d_jCol;
+ jac_d_jCol = NULL;
+
+ delete [] g_iRow;
+ g_iRow = NULL;
+ delete [] g_jCol;
+ g_jCol = NULL;
+
+ if (hessian_approximation_==EXACT) {
+ /** Create the matrix space for the hessian of the lagrangian */
+ Index* full_h_iRow = new Index[nz_full_h_];
+ Index* full_h_jCol = new Index[nz_full_h_];
+ Index* h_iRow = new Index[nz_full_h_];
+ Index* h_jCol = new Index[nz_full_h_];
+ bool retval =tnlp_->eval_h(n_full_x_, NULL, false, 0, n_full_g_,
+ NULL, false,
+ nz_full_h_, full_h_iRow, full_h_jCol, NULL);
+ if (!retval) {
+ jnlst_->Printf(J_ERROR, J_INITIALIZATION,
+ "Option hessian_information is not chosen as limited_memory, but eval_h returns false.\n");
+ THROW_EXCEPTION(OPTION_INVALID, "eval_h is called but has not been implemented");
+ }
+
+ if (index_style_ != TNLP::FORTRAN_STYLE) {
+ for (Index i=0; i<nz_full_h_; i++) {
+ full_h_iRow[i] += 1;
+ full_h_jCol[i] += 1;
+ }
+ }
+
+ current_nz = 0;
+ if (IsValid(P_x_full_x_)) {
+ h_idx_map_ = new Index[nz_full_h_];
+ const Index* h_pos = P_x_full_x_->CompressedPosIndices();
+ for (Index i=0; i<nz_full_h_; i++) {
+ const Index h_row = h_pos[full_h_iRow[i]-1];
+ const Index h_col = h_pos[full_h_jCol[i]-1];
+ if (h_row != -1 && h_col != -1) {
+ h_idx_map_[current_nz] = i;
+ h_iRow[current_nz] = h_row + 1;
+ h_jCol[current_nz] = h_col + 1;
+ current_nz++;
+ }
+ }
+ }
+ else {
+ h_idx_map_ = NULL;
+ for (Index i=0; i<nz_full_h_; i++) {
+ const Index h_row = full_h_iRow[i]-1;
+ const Index h_col = full_h_jCol[i]-1;
+ h_iRow[i] = h_row + 1;
+ h_jCol[i] = h_col + 1;
+ current_nz++;
+ }
+ current_nz = nz_full_h_;
+ }
+ nz_h_ = current_nz;
+ Hess_lagrangian_space_ = new SymTMatrixSpace(n_x_var, nz_h_, h_iRow, h_jCol);
+ delete [] full_h_iRow;
+ full_h_iRow = NULL;
+ delete [] full_h_jCol;
+ full_h_jCol = NULL;
+ delete [] h_iRow;
+ h_iRow = NULL;
+ delete [] h_jCol;
+ h_jCol = NULL;
+ }
+ else {
+ nz_h_ = 0;
+ Hess_lagrangian_space_ = NULL;
+ }
+ } /* if (warm_start_same_structure_) { */
+
+ // Assign the spaces to the returned pointers
+ x_space = x_space_;
+ c_space = c_space_;
+ d_space = d_space_;
+ x_l_space = x_l_space_;
+ px_l_space = px_l_space_;
+ x_u_space = x_u_space_;
+ px_u_space = px_u_space_;
+ d_l_space = d_l_space_;
+ pd_l_space = pd_l_space_;
+ d_u_space = d_u_space_;
+ pd_u_space = pd_u_space_;
+ Jac_c_space = Jac_c_space_;
+ Jac_d_space = Jac_d_space_;
+ Hess_lagrangian_space = Hess_lagrangian_space_;
+
+ if (IsValid(jnlst_)) {
+ jnlst_->Printf(J_ITERSUMMARY, J_STATISTICS,
+ "Number of nonzeros in equality constraint Jacobian...:%9d\n", nz_jac_c_);
+ jnlst_->Printf(J_ITERSUMMARY, J_STATISTICS,
+ "Number of nonzeros in inequality constraint Jacobian.:%9d\n", nz_jac_d_);
+ jnlst_->Printf(J_ITERSUMMARY, J_STATISTICS,
+ "Number of nonzeros in Lagrangian Hessian.............:%9d\n\n", nz_h_);
+ }
+
+ return true;
+ }
+
+ bool TNLPAdapter::GetBoundsInformation(const Matrix& Px_L,
+ Vector& x_L,
+ const Matrix& Px_U,
+ Vector& x_U,
+ const Matrix& Pd_L,
+ Vector& d_L,
+ const Matrix& Pd_U,
+ Vector& d_U)
+ {
+ // This could be done more efficiently, I have already called this method
+ // once to setup the structure for the problem, I could store the values
+ // and use them here ?
+ // Actually, this is better for a warm start
+ Number* x_l = new Number[n_full_x_];
+ Number* x_u = new Number[n_full_x_];
+ Number* g_l = new Number[n_full_g_];
+ Number* g_u = new Number[n_full_g_];
+ tnlp_->get_bounds_info(n_full_x_, x_l, x_u, n_full_g_, g_l, g_u);
+
+ if (fixed_variable_treatment_==MAKE_PARAMETER) {
+ // Set the values of fixed variables
+ for (Index i=0; i<n_x_fixed_; i++) {
+ DBG_ASSERT(x_l[x_fixed_map_[i]] == x_u[x_fixed_map_[i]]);
+ full_x_[x_fixed_map_[i]] = x_l[x_fixed_map_[i]];
+ }
+ }
+
+ // Set the bounds values for x
+ DenseVector* dx_L = dynamic_cast<DenseVector*>(&x_L);
+ DBG_ASSERT(dx_L);
+ Number* values = dx_L->Values();
+ const ExpansionMatrix* em_Px_L =
+ dynamic_cast<const ExpansionMatrix*>(&Px_L);
+ DBG_ASSERT(em_Px_L);
+ if (IsValid(P_x_full_x_)) {
+ for (Index i=0; i<Px_L.NCols(); i++) {
+ const Index ipopt_idx = em_Px_L->ExpandedPosIndices()[i];
+ const Index full_idx = P_x_full_x_->ExpandedPosIndices()[ipopt_idx];
+ const Number lower_bound = x_l[full_idx];
+ values[i] = lower_bound;
+ }
+ }
+ else {
+ for (Index i=0; i<Px_L.NCols(); i++) {
+ const Index ipopt_idx = em_Px_L->ExpandedPosIndices()[i];
+ const Number lower_bound = x_l[ipopt_idx];
+ values[i] = lower_bound;
+ }
+ }
+
+ DenseVector* dx_U = dynamic_cast<DenseVector*>(&x_U);
+ DBG_ASSERT(dx_U);
+ values = dx_U->Values();
+ const ExpansionMatrix* em_Px_U =
+ dynamic_cast<const ExpansionMatrix*>(&Px_U);
+ DBG_ASSERT(em_Px_U);
+ if (IsValid(P_x_full_x_)) {
+ for (Index i=0; i<Px_U.NCols(); i++) {
+ const Index ipopt_idx = em_Px_U->ExpandedPosIndices()[i];
+ const Index full_idx = P_x_full_x_->ExpandedPosIndices()[ipopt_idx];
+ const Number upper_bound = x_u[full_idx];
+ values[i] = upper_bound;
+ }
+ }
+ else {
+ for (Index i=0; i<Px_U.NCols(); i++) {
+ const Index ipopt_idx = em_Px_U->ExpandedPosIndices()[i];
+ const Number upper_bound = x_u[ipopt_idx];
+ values[i] = upper_bound;
+ }
+ }
+
+ // get the bounds values (rhs values to subtract) for c
+ // i.e. if gL == gU, then we actually have g(x) = gL = gU,
+ // since we solve c(x) = 0, we actually need c(x) - gL = 0
+ for (Index i=0; i<P_c_g_->NCols(); i++) {
+ Index full_idx = P_c_g_->ExpandedPosIndices()[i];
+ Number rhs = g_l[full_idx];
+ c_rhs_[i] = rhs;
+ }
+ // similarly, if we have fixed variables, consider them here
+ if (fixed_variable_treatment_==MAKE_CONSTRAINT) {
+ Index n_c_no_fixed = P_c_g_->NCols();
+ for (Index i=0; i<n_x_fixed_; i++) {
+ DBG_ASSERT(x_l[x_fixed_map_[i]]==x_u[x_fixed_map_[i]]);
+ c_rhs_[n_c_no_fixed+i] = x_l[x_fixed_map_[i]];
+ }
+ }
+
+ // get the bounds values for d
+ DenseVector* dd_L = dynamic_cast<DenseVector*>(&d_L);
+ DBG_ASSERT(dd_L);
+ values = dd_L->Values();
+ const ExpansionMatrix* em_Pd_L =
+ dynamic_cast<const ExpansionMatrix*>(&Pd_L);
+ DBG_ASSERT(em_Pd_L);
+ for (Index i=0; i<Pd_L.NCols(); i++) {
+ Index d_exp_idx = em_Pd_L->ExpandedPosIndices()[i];
+ Index full_idx = P_d_g_->ExpandedPosIndices()[d_exp_idx];
+ Number lower_bound = g_l[full_idx];
+ values[i] = lower_bound;
+ }
+
+ DenseVector* dd_U = dynamic_cast<DenseVector*>(&d_U);
+ DBG_ASSERT(dd_U);
+ values = dd_U->Values();
+ const ExpansionMatrix* em_Pd_U =
+ dynamic_cast<const ExpansionMatrix*>(&Pd_U);
+ DBG_ASSERT(em_Pd_U);
+ for (Index i=0; i<Pd_U.NCols(); i++) {
+ Index d_exp_idx = em_Pd_U->ExpandedPosIndices()[i];
+ Index full_idx = P_d_g_->ExpandedPosIndices()[d_exp_idx];
+ Number upper_bound = g_u[full_idx];
+ values[i] = upper_bound;
+ }
+
+ delete [] x_l;
+ x_l = NULL;
+ delete [] x_u;
+ x_u = NULL;
+ delete [] g_l;
+ g_l = NULL;
+ delete [] g_u;
+ g_u = NULL;
+
+ return true;
+ }
+
+ bool TNLPAdapter::GetStartingPoint(SmartPtr<Vector> x,
+ bool need_x,
+ SmartPtr<Vector> y_c,
+ bool need_y_c,
+ SmartPtr<Vector> y_d,
+ bool need_y_d,
+ SmartPtr<Vector> z_L,
+ bool need_z_L,
+ SmartPtr<Vector> z_U,
+ bool need_z_U
+ )
+ {
+ Number* full_x = new Number[n_full_x_];
+ Number* full_z_l = new Number[n_full_x_];
+ Number* full_z_u = new Number[n_full_x_];
+ Number* full_lambda = new Number[n_full_g_];
+ bool init_x = need_x;
+ bool init_z = need_z_L && need_z_U;
+ bool init_lambda = need_y_c && need_y_d;
+
+ bool retvalue =
+ tnlp_->get_starting_point(n_full_x_, init_x, full_x, init_z, full_z_l, full_z_u, n_full_g_, init_lambda, full_lambda);
+
+ if (!retvalue) {
+ return false;
+ }
+
+ if (need_x) {
+ DenseVector* dx = dynamic_cast<DenseVector*>(GetRawPtr(x));
+ DBG_ASSERT(dx);
+ Number* values = dx->Values();
+ if (IsValid(P_x_full_x_)) {
+ const Index* x_pos = P_x_full_x_->ExpandedPosIndices();
+ for (Index i=0; i<x->Dim(); i++) {
+ values[i] = full_x[x_pos[i]];
+ }
+ }
+ else {
+ IpBlasDcopy(x->Dim(), full_x, 1, values, 1);
+ }
+ }
+
+ if (need_y_c) {
+ DenseVector* dy_c = dynamic_cast<DenseVector*>(GetRawPtr(y_c));
+ DBG_ASSERT(dy_c);
+ Number* values = dy_c->Values();
+ const Index* y_c_pos = P_c_g_->ExpandedPosIndices();
+ for (Index i=0; i<P_c_g_->NCols(); i++) {
+ values[i] = full_lambda[y_c_pos[i]];
+ }
+ if (fixed_variable_treatment_==MAKE_CONSTRAINT) {
+ // ToDo maybe use info from z_L and Z_U here?
+ const Number zero = 0.;
+ IpBlasDcopy(n_x_fixed_, &zero, 0, &values[P_c_g_->NCols()], 1);
+ }
+ }
+
+ if (need_y_d) {
+ DenseVector* dy_d = dynamic_cast<DenseVector*>(GetRawPtr(y_d));
+ DBG_ASSERT(dy_d);
+ Number* values = dy_d->Values();
+ const Index* y_d_pos = P_d_g_->ExpandedPosIndices();
+ for (Index i=0; i<y_d->Dim(); i++) {
+ values[i] = full_lambda[y_d_pos[i]];
+ }
+ }
+
+ if (need_z_L) {
+ DenseVector* dz_l = dynamic_cast<DenseVector*>(GetRawPtr(z_L));
+ DBG_ASSERT(dz_l);
+ Number* values = dz_l->Values();
+ const Index* z_l_pos = P_x_x_L_->ExpandedPosIndices();
+ if (IsValid(P_x_full_x_)) {
+ const Index* x_pos = P_x_full_x_->ExpandedPosIndices();
+ for (Index i=0; i<z_L->Dim(); i++) {
+ Index idx = z_l_pos[i]; // convert from x_L to x (ipopt)
+ idx = x_pos[idx]; // convert from x (ipopt) to x_full
+ values[i] = full_z_l[idx];
+ }
+ }
+ else {
+ for (Index i=0; i<z_L->Dim(); i++) {
+ Index idx = z_l_pos[i]; // convert from x_L to x (ipopt)
+ values[i] = full_z_l[idx];
+ }
+ }
+ }
+
+ if (need_z_U) {
+ DenseVector* dz_u = dynamic_cast<DenseVector*>(GetRawPtr(z_U));
+ DBG_ASSERT(dz_u);
+ Number* values = dz_u->Values();
+ const Index* z_u_pos = P_x_x_U_->ExpandedPosIndices();
+ if (IsValid(P_x_full_x_)) {
+ const Index* x_pos = P_x_full_x_->ExpandedPosIndices();
+ for (Index i=0; i<z_U->Dim(); i++) {
+ Index idx = z_u_pos[i]; // convert from x_u to x (ipopt)
+ idx = x_pos[idx]; // convert from x (ipopt) to x_full
+ values[i] = full_z_u[idx];
+ }
+ }
+ else {
+ for (Index i=0; i<z_U->Dim(); i++) {
+ Index idx = z_u_pos[i]; // convert from x_u to x (ipopt)
+ values[i] = full_z_u[idx];
+ }
+ }
+ }
+
+ delete [] full_x;
+ full_x = NULL;
+ delete [] full_z_l;
+ full_z_l = NULL;
+ delete [] full_z_u;
+ full_z_u = NULL;
+ delete [] full_lambda;
+ full_lambda = NULL;
+
+ return true;
+ }
+
+ bool TNLPAdapter::GetWarmStartIterate(IteratesVector& warm_start_iterate)
+ {
+ return tnlp_->get_warm_start_iterate(warm_start_iterate);
+ }
+
+ bool TNLPAdapter::Eval_f(const Vector& x, Number& f)
+ {
+ bool new_x = false;
+ if (update_local_x(x)) {
+ new_x = true;
+ }
+ return tnlp_->eval_f(n_full_x_, full_x_, new_x, f);
+ }
+
+ bool TNLPAdapter::Eval_grad_f(const Vector& x, Vector& g_f)
+ {
+ bool retvalue = false;
+ bool new_x = false;
+ if (update_local_x(x)) {
+ new_x = true;
+ }
+
+ if (IsValid(P_x_full_x_)) {
+ Number* full_grad_f = new Number[n_full_x_];
+ if (tnlp_->eval_grad_f(n_full_x_, full_x_, new_x, full_grad_f)) {
+ const Index* x_pos = P_x_full_x_->ExpandedPosIndices();
+ DenseVector* dg_f = dynamic_cast<DenseVector*>(&g_f);
+ DBG_ASSERT(dg_f);
+ Number* values = dg_f->Values();
+
+ for (Index i=0; i<g_f.Dim(); i++) {
+ values[i] = full_grad_f[x_pos[i]];
+ }
+ retvalue = true;
+ }
+ delete [] full_grad_f;
+ }
+ else {
+ DenseVector* dg_f = dynamic_cast<DenseVector*>(&g_f);
+ DBG_ASSERT(dg_f);
+ Number* values = dg_f->Values();
+ retvalue = tnlp_->eval_grad_f(n_full_x_, full_x_, new_x, values);
+ }
+
+ return retvalue;
+ }
+
+ bool TNLPAdapter::Eval_c(const Vector& x, Vector& c)
+ {
+ bool new_x = false;
+ if (update_local_x(x)) {
+ new_x = true;
+ }
+
+ DenseVector* dc = dynamic_cast<DenseVector*>(&c);
+ DBG_ASSERT(dc);
+ Number* values = dc->Values();
+ if (internal_eval_g(new_x)) {
+ const Index* c_pos = P_c_g_->ExpandedPosIndices();
+ Index n_c_no_fixed = P_c_g_->NCols();
+ for (Index i=0; i<n_c_no_fixed; i++) {
+ values[i] = full_g_[c_pos[i]];
+ values[i] -= c_rhs_[i];
+ }
+ if (fixed_variable_treatment_==MAKE_CONSTRAINT) {
+ for (Index i=0; i<n_x_fixed_; i++) {
+ values[n_c_no_fixed+i] =
+ full_x_[x_fixed_map_[i]] - c_rhs_[n_c_no_fixed+i];
+ }
+ }
+ return true;
+ }
+
+ return false;
+ }
+
+ bool TNLPAdapter::Eval_jac_c(const Vector& x, Matrix& jac_c)
+ {
+ bool new_x = false;
+ if (update_local_x(x)) {
+ new_x = true;
+ }
+
+ if (internal_eval_jac_g(new_x)) {
+ GenTMatrix* gt_jac_c = dynamic_cast<GenTMatrix*>(&jac_c);
+ DBG_ASSERT(gt_jac_c);
+ Number* values = gt_jac_c->Values();
+
+ for (Index i=0; i<nz_jac_c_no_fixed_; i++) {
+ // Assume the same structure as initially given
+ values[i] = jac_g_[jac_idx_map_[i]];
+ }
+ if (fixed_variable_treatment_==MAKE_CONSTRAINT) {
+ const Number one = 1.;
+ IpBlasDcopy(n_x_fixed_, &one, 0, &values[nz_jac_c_no_fixed_], 1);
+ }
+ return true;
+ }
+ return false;
+ }
+
+ bool TNLPAdapter::Eval_d(const Vector& x, Vector& d)
+ {
+ bool new_x = false;
+ if (update_local_x(x)) {
+ new_x = true;
+ }
+
+ DenseVector* dd = dynamic_cast<DenseVector*>(&d);
+ DBG_ASSERT(dd);
+ Number* values = dd->Values();
+ if (internal_eval_g(new_x)) {
+ const Index* d_pos = P_d_g_->ExpandedPosIndices();
+ for (Index i=0; i<d.Dim(); i++) {
+ values[i] = full_g_[d_pos[i]];
+ }
+ return true;
+ }
+
+ return false;
+ }
+
+ bool TNLPAdapter::Eval_jac_d(const Vector& x, Matrix& jac_d)
+ {
+ bool new_x = false;
+ if (update_local_x(x)) {
+ new_x = true;
+ }
+
+ if (internal_eval_jac_g(new_x)) {
+ GenTMatrix* gt_jac_d = dynamic_cast<GenTMatrix*>(&jac_d);
+ DBG_ASSERT(gt_jac_d);
+ Number* values = gt_jac_d->Values();
+
+ for (Index i=0; i<nz_jac_d_; i++) {
+ // Assume the same structure as initially given
+ values[i] = jac_g_[jac_idx_map_[nz_jac_c_no_fixed_ + i]];
+ }
+ return true;
+ }
+ return false;
+ }
+
+ bool TNLPAdapter::Eval_h(const Vector& x,
+ Number obj_factor,
+ const Vector& yc,
+ const Vector& yd,
+ SymMatrix& h)
+ {
+ // First see if all weights are set to zero (for example, when
+ // computing the least square multiplier estimates, this is what
+ // we do). In that case, there is no need to compute values, just
+ // set them to zero.
+ if (obj_factor==0. && yc.Asum()==0. && yd.Asum()==0.) {
+ SymTMatrix* st_h = dynamic_cast<SymTMatrix*>(&h);
+ DBG_ASSERT(st_h);
+ Number* values = st_h->Values();
+ for (Index i=0; i<nz_h_; i++) {
+ values[i] = 0.;
+ }
+ return true;
+ }
+
+ bool retval = false;
+ bool new_x = false;
+ if (update_local_x(x)) {
+ new_x = true;
+ }
+ bool new_y = false;
+ if (update_local_lambda(yc, yd)) {
+ new_y = true;
+ }
+
+ SymTMatrix* st_h = dynamic_cast<SymTMatrix*>(&h);
+ DBG_ASSERT(st_h);
+ Number* values = st_h->Values();
+
+ if (h_idx_map_) {
+ Number* full_h = new Number[nz_full_h_];
+
+ if (tnlp_->eval_h(n_full_x_, full_x_, new_x, obj_factor, n_full_g_,
+ full_lambda_, new_y, nz_full_h_, NULL, NULL, full_h)) {
+ for (Index i=0; i<nz_h_; i++) {
+ values[i] = full_h[h_idx_map_[i]];
+ }
+ retval = true;
+ }
+ delete [] full_h;
+ }
+ else {
+ retval = tnlp_->eval_h(n_full_x_, full_x_, new_x, obj_factor, n_full_g_,
+ full_lambda_, new_y, nz_full_h_, NULL, NULL,
+ values);
+ }
+
+ return retval;
+ }
+
+ void TNLPAdapter::GetScalingParameters(
+ const SmartPtr<const VectorSpace> x_space,
+ const SmartPtr<const VectorSpace> c_space,
+ const SmartPtr<const VectorSpace> d_space,
+ Number& obj_scaling,
+ SmartPtr<Vector>& x_scaling,
+ SmartPtr<Vector>& c_scaling,
+ SmartPtr<Vector>& d_scaling) const
+ {
+ x_scaling = x_space->MakeNew();
+ c_scaling = c_space->MakeNew();
+ d_scaling = d_space->MakeNew();
+ DBG_ASSERT((c_scaling->Dim()+d_scaling->Dim()) == n_full_g_);
+
+ DenseVector* dx = dynamic_cast<DenseVector*>(GetRawPtr(x_scaling));
+ DenseVector* dc = dynamic_cast<DenseVector*>(GetRawPtr(c_scaling));
+ DenseVector* dd = dynamic_cast<DenseVector*>(GetRawPtr(d_scaling));
+ DBG_ASSERT(dx && dc && dd);
+ Number* dx_values = dx->Values();
+ Number* dc_values = dc->Values();
+ Number* dd_values = dd->Values();
+
+ Number* full_g_scaling = new Number[n_full_g_];
+ bool use_x_scaling=true;
+ bool use_g_scaling=true;
+
+ if (IsValid(P_x_full_x_)) {
+ Number* full_x_scaling = new Number[n_full_x_];
+ bool retval = tnlp_->get_scaling_parameters(obj_scaling,
+ use_x_scaling, n_full_x_,
+ full_x_scaling,
+ use_g_scaling, n_full_g_,
+ full_g_scaling);
+ if (!retval) {
+ jnlst_->Printf(J_ERROR, J_INITIALIZATION,
+ "Option nlp_scaling_method selected as user-scaling, but no user-scaling available, or it cannot be computed.\n");
+ THROW_EXCEPTION(OPTION_INVALID,
+ "User scaling chosen, but get_scaling_parameters returned false.");
+ }
+
+ if (use_x_scaling) {
+ const Index* x_pos = P_x_full_x_->ExpandedPosIndices();
+ for (Index i=0; i<dx->Dim(); i++) {
+ dx_values[i] = full_x_scaling[x_pos[i]];
+ }
+ }
+ delete [] full_x_scaling;
+ }
+ else {
+ bool retval = tnlp_->get_scaling_parameters(obj_scaling,
+ use_x_scaling, n_full_x_,
+ dx_values,
+ use_g_scaling, n_full_g_,
+ full_g_scaling);
+ if (!retval) {
+ jnlst_->Printf(J_ERROR, J_INITIALIZATION,
+ "Option nlp_scaling_method selected as user-scaling, but no user-scaling available, or it cannot be computed.\n");
+ THROW_EXCEPTION(OPTION_INVALID,
+ "User scaling chosen, but get_scaling_parameters returned false.");
+ }
+ }
+
+ if (!use_x_scaling) {
+ x_scaling = NULL;
+ }
+
+ if (use_g_scaling) {
+ const Index* c_pos = P_c_g_->ExpandedPosIndices();
+ for (Index i=0; i<P_c_g_->NCols(); i++) {
+ dc_values[i] = full_g_scaling[c_pos[i]];
+ }
+ if (fixed_variable_treatment_==MAKE_CONSTRAINT) {
+ const Number one = 1.;
+ IpBlasDcopy(n_x_fixed_, &one, 0, &dc_values[P_c_g_->NCols()], 1);
+ }
+
+ const Index* d_pos = P_d_g_->ExpandedPosIndices();
+ for (Index i=0; i<dd->Dim(); i++) {
+ dd_values[i] = full_g_scaling[d_pos[i]];
+ }
+ }
+ else {
+ c_scaling = NULL;
+ d_scaling = NULL;
+ }
+
+ delete [] full_g_scaling;
+ }
+
+
+ void TNLPAdapter::FinalizeSolution(SolverReturn status,
+ const Vector& x, const Vector& z_L, const Vector& z_U,
+ const Vector& c, const Vector& d,
+ const Vector& y_c, const Vector& y_d,
+ Number obj_value)
+ {
+ DBG_START_METH("TNLPAdapter::FinalizeSolution", dbg_verbosity);
+
+ update_local_x(x);
+ update_local_lambda(y_c, y_d);
+
+ ResortX(x, full_x_);
+ ResortG(y_c, y_d, full_lambda_);
+
+ Number* full_g = new Number[n_full_g_];
+ ResortG(c, d, full_g);
+
+ Number* full_z_L = new Number[n_full_x_];
+ Number* full_z_U = new Number[n_full_x_];
+ for (int i=0; i<n_full_x_; i++) {
+ full_z_L[i] = nlp_lower_bound_inf_;
+ full_z_U[i] = nlp_upper_bound_inf_;
+ }
+ ResortBnds(z_L, full_z_L, z_U, full_z_U);
+
+ // Hopefully the following is correct to recover the bound
+ // multipliers for fixed variables (sign ok?)
+ if (fixed_variable_treatment_==MAKE_CONSTRAINT && n_x_fixed_>0) {
+ const DenseVector* dy_c = dynamic_cast<const DenseVector*>(&y_c);
+ DBG_ASSERT(dy_c);
+ DBG_ASSERT(!dy_c->IsHomogeneous());
+ const Number* values = dy_c->Values();
+ Index n_c_no_fixed = y_c.Dim() - n_x_fixed_;
+ for (Index i=0; i<n_x_fixed_; i++) {
+ full_z_L[x_fixed_map_[i]] = Max(0., -values[n_c_no_fixed+i]);
+ full_z_U[x_fixed_map_[i]] = Max(0., values[n_c_no_fixed+i]);
+ }
+ }
+
+ tnlp_->finalize_solution(status,
+ n_full_x_, full_x_, full_z_L, full_z_U,
+ n_full_g_, full_g, full_lambda_,
+ obj_value);
+
+ delete [] full_z_L;
+ full_z_L = NULL;
+ delete [] full_z_U;
+ full_z_U = NULL;
+ delete [] full_g;
+ full_g = NULL;
+ }
+
+ bool TNLPAdapter::
+ IntermediateCallBack(AlgorithmMode mode,
+ Index iter, Number obj_value,
+ Number inf_pr, Number inf_du,
+ Number mu, Number d_norm,
+ Number regularization_size,
+ Number alpha_du, Number alpha_pr,
+ Index ls_trials,
+ const IpoptData* ip_data,
+ IpoptCalculatedQuantities* ip_cq)
+ {
+ return tnlp_->intermediate_callback(mode, iter, obj_value, inf_pr, inf_du,
+ mu, d_norm, regularization_size,
+ alpha_du, alpha_pr, ls_trials,
+ ip_data, ip_cq);
+ }
+
+
+ void TNLPAdapter::
+ GetQuasiNewtonApproximationSpaces(SmartPtr<VectorSpace>& approx_space,
+ SmartPtr<Matrix>& P_approx)
+ {
+ Index num_nonlin_vars =
+ tnlp_->get_number_of_nonlinear_variables();
+
+ if (num_nonlin_vars<0) {
+ approx_space = NULL;
+ P_approx = NULL;
+ }
+ else {
+ Index* pos_nonlin_vars = new Index[num_nonlin_vars];
+ if (num_nonlin_vars>0) {
+ bool retval = tnlp_->get_list_of_nonlinear_variables(num_nonlin_vars,
+ pos_nonlin_vars);
+ if (!retval) {
+ jnlst_->Printf(J_ERROR, J_INITIALIZATION,
+ "TNLP's get_number_of_nonlinear_variables returns non-negative number, but get_list_of_nonlinear_variables returns false.\n");
+ THROW_EXCEPTION(INVALID_TNLP, "get_list_of_nonlinear_variables has not been overwritten");
+ }
+ }
+
+ // Correct indices in case user starts counting variables at 1
+ // and not 0
+ if (index_style_ == TNLP::FORTRAN_STYLE) {
+ for (Index i=0; i<num_nonlin_vars; i++) {
+ pos_nonlin_vars[i]--;
+ }
+ }
+
+ if (IsNull(P_x_full_x_)) {
+ if (num_nonlin_vars == n_full_x_) {
+ approx_space = NULL;
+ P_approx = NULL;
+ }
+ else {
+ SmartPtr<ExpansionMatrixSpace> ex_sp =
+ new ExpansionMatrixSpace(n_full_x_, num_nonlin_vars,
+ pos_nonlin_vars);
+ P_approx = ex_sp->MakeNew();
+ approx_space = new DenseVectorSpace(num_nonlin_vars);
+ }
+ }
+ else {
+ const Index* compr_pos = P_x_full_x_->CompressedPosIndices();
+ Index* nonfixed_pos_nonlin_vars = new Index[num_nonlin_vars];
+
+ Index nonfixed_nonlin_vars = 0;
+ for (Index i=0; i<num_nonlin_vars; i++) {
+ Index full_pos = pos_nonlin_vars[i];
+ Index nonfixed_pos = compr_pos[full_pos];
+ if (nonfixed_pos>=0) {
+ nonfixed_pos_nonlin_vars[nonfixed_nonlin_vars] = nonfixed_pos;
+ nonfixed_nonlin_vars++;
+ }
+ }
+
+ const Index n_x_free = n_full_x_ - n_x_fixed_;
+ if (nonfixed_nonlin_vars == n_x_free) {
+ approx_space = NULL;
+ P_approx = NULL;
+ }
+ else {
+ SmartPtr<ExpansionMatrixSpace> ex_sp =
+ new ExpansionMatrixSpace(n_x_free, nonfixed_nonlin_vars,
+ nonfixed_pos_nonlin_vars);
+ P_approx = ex_sp->MakeNew();
+ approx_space = new DenseVectorSpace(nonfixed_nonlin_vars);
+ }
+
+ delete [] nonfixed_pos_nonlin_vars;
+ }
+ delete [] pos_nonlin_vars;
+ }
+ }
+
+ void TNLPAdapter::ResortX(const Vector& x, Number* x_orig)
+ {
+ const DenseVector* dx = dynamic_cast<const DenseVector*>(&x);
+ DBG_ASSERT(dx);
+
+ if (IsValid(P_x_full_x_)) {
+ const Index* x_pos = P_x_full_x_->CompressedPosIndices();
+
+ if (dx->IsHomogeneous()) {
+ Number scalar = dx->Scalar();
+ for (Index i=0; i<n_full_x_; i++) {
+ Index idx = x_pos[i];
+ if (idx != -1) {
+ x_orig[i] = scalar;
+ }
+ else {
+ x_orig[i] = full_x_[i];
+ }
+ }
+ }
+ else {
+ const Number* x_values = dx->Values();
+ for (Index i=0; i<n_full_x_; i++) {
+ Index idx = x_pos[i];
+ if (idx != -1) {
+ x_orig[i] = x_values[idx];
+ }
+ else {
+ x_orig[i] = full_x_[i];
+ }
+ }
+ }
+ }
+ else {
+ if (dx->IsHomogeneous()) {
+ Number scalar = dx->Scalar();
+ IpBlasDcopy(n_full_x_, &scalar, 0, x_orig, 1);
+ }
+ else {
+ IpBlasDcopy(n_full_x_, dx->Values(), 1, x_orig, 1);
+ }
+ }
+ }
+
+ void TNLPAdapter::ResortG(const Vector& c, const Vector& d, Number* g_orig)
+ {
+ const DenseVector* dc = dynamic_cast<const DenseVector*>(&c);
+ DBG_ASSERT(dc);
+
+ const Index* c_pos = P_c_g_->ExpandedPosIndices();
+ if (dc->IsHomogeneous()) {
+ Number scalar = dc->Scalar();
+ for (Index i=0; i<P_c_g_->NCols(); i++) {
+ g_orig[c_pos[i]] = scalar;
+ }
+ }
+ else {
+ const Number* c_values = dc->Values();
+ for (Index i=0; i<P_c_g_->NCols(); i++) {
+ g_orig[c_pos[i]] = c_values[i];
+ }
+ }
+
+ const DenseVector* dd = dynamic_cast<const DenseVector*>(&d);
+ DBG_ASSERT(dd);
+
+ const Index* d_pos = P_d_g_->ExpandedPosIndices();
+ if (dd->IsHomogeneous()) {
+ Number scalar = dd->Scalar();
+ for (Index i=0; i<d.Dim(); i++) {
+ g_orig[d_pos[i]] = scalar;
+ }
+ }
+ else {
+ const Number* d_values = dd->Values();
+ for (Index i=0; i<d.Dim(); i++) {
+ g_orig[d_pos[i]] = d_values[i];
+ }
+ }
+ }
+
+ void TNLPAdapter::ResortBnds(const Vector& x_L, Number* x_L_orig,
+ const Vector& x_U, Number* x_U_orig)
+ {
+ if (x_L_orig) {
+ const DenseVector* dx_L = dynamic_cast<const DenseVector*>(&x_L);
+ DBG_ASSERT(dx_L);
+
+ const Index* bnds_pos_not_fixed = P_x_x_L_->ExpandedPosIndices();
+
+ if (IsValid(P_x_full_x_)) {
+ const Index* bnds_pos_full = P_x_full_x_->ExpandedPosIndices();
+ if (dx_L->IsHomogeneous()) {
+ Number scalar = dx_L->Scalar();
+ for (Index i=0; i<x_L.Dim(); i++) {
+ int idx = bnds_pos_not_fixed[i];
+ idx = bnds_pos_full[idx];
+ x_L_orig[idx] = scalar;
+ }
+ }
+ else {
+ const Number* x_L_values = dx_L->Values();
+ for (Index i=0; i<x_L.Dim(); i++) {
+ int idx = bnds_pos_not_fixed[i];
+ idx = bnds_pos_full[idx];
+ x_L_orig[idx] = x_L_values[i];
+ }
+ }
+ }
+ else {
+ if (dx_L->IsHomogeneous()) {
+ Number scalar = dx_L->Scalar();
+ for (Index i=0; i<x_L.Dim(); i++) {
+ int idx = bnds_pos_not_fixed[i];
+ x_L_orig[idx] = scalar;
+ }
+ }
+ else {
+ const Number* x_L_values = dx_L->Values();
+ for (Index i=0; i<x_L.Dim(); i++) {
+ int idx = bnds_pos_not_fixed[i];
+ x_L_orig[idx] = x_L_values[i];
+ }
+ }
+ }
+ }
+
+ if (x_U_orig) {
+ const DenseVector* dx_U = dynamic_cast<const DenseVector*>(&x_U);
+ DBG_ASSERT(dx_U);
+
+ const Index* bnds_pos_not_fixed = P_x_x_U_->ExpandedPosIndices();
+
+ if (IsValid(P_x_full_x_)) {
+ const Index* bnds_pos_full = P_x_full_x_->ExpandedPosIndices();
+ if (dx_U->IsHomogeneous()) {
+ Number scalar = dx_U->Scalar();
+ for (Index i=0; i<x_U.Dim(); i++) {
+ int idx = bnds_pos_not_fixed[i];
+ idx = bnds_pos_full[idx];
+ x_U_orig[idx] = scalar;
+ }
+ }
+ else {
+ const Number* x_U_values = dx_U->Values();
+ for (Index i=0; i<x_U.Dim(); i++) {
+ int idx = bnds_pos_not_fixed[i];
+ idx = bnds_pos_full[idx];
+ x_U_orig[idx] = x_U_values[i];
+ }
+ }
+ }
+ else {
+ if (dx_U->IsHomogeneous()) {
+ Number scalar = dx_U->Scalar();
+ for (Index i=0; i<x_U.Dim(); i++) {
+ int idx = bnds_pos_not_fixed[i];
+ x_U_orig[idx] = scalar;
+ }
+ }
+ else {
+ const Number* x_U_values = dx_U->Values();
+ for (Index i=0; i<x_U.Dim(); i++) {
+ int idx = bnds_pos_not_fixed[i];
+ x_U_orig[idx] = x_U_values[i];
+ }
+ }
+ }
+ }
+ }
+
+ bool TNLPAdapter::update_local_x(const Vector& x)
+ {
+ if (x.GetTag() == x_tag_for_iterates_) {
+ return false;
+ }
+
+ ResortX(x, full_x_);
+
+ x_tag_for_iterates_ = x.GetTag();
+
+ return true;
+ }
+
+ bool TNLPAdapter::update_local_lambda(const Vector& y_c, const Vector& y_d)
+ {
+ if (y_c.GetTag() == y_c_tag_for_iterates_
+ && y_d.GetTag() == y_d_tag_for_iterates_) {
+ return false;
+ }
+
+ ResortG(y_c, y_d, full_lambda_);
+
+ y_c_tag_for_iterates_ = y_c.GetTag();
+ y_d_tag_for_iterates_ = y_d.GetTag();
+
+ return true;
+ }
+
+ bool TNLPAdapter::internal_eval_g(bool new_x)
+ {
+ if (x_tag_for_g_ == x_tag_for_iterates_) {
+ // already calculated!
+ return true;
+ }
+
+ x_tag_for_g_ = x_tag_for_iterates_;
+
+ bool retval = tnlp_->eval_g(n_full_x_, full_x_, new_x, n_full_g_, full_g_);
+
+ if (!retval) {
+ x_tag_for_jac_g_ = 0;
+ }
+
+ return retval;
+ }
+
+ bool TNLPAdapter::internal_eval_jac_g(bool new_x)
+ {
+ if (x_tag_for_jac_g_ == x_tag_for_iterates_) {
+ // already calculated!
+ return true;
+ }
+
+ x_tag_for_jac_g_ = x_tag_for_iterates_;
+
+ bool retval = tnlp_->eval_jac_g(n_full_x_, full_x_, new_x, n_full_g_,
+ nz_full_jac_g_, NULL, NULL, jac_g_);
+
+ if (!retval) {
+ x_tag_for_jac_g_ = 0;
+ }
+
+ return retval;
+ }
+
+ bool TNLPAdapter::CheckDerivatives(TNLPAdapter::DerivativeTestEnum deriv_test)
+ {
+ if (deriv_test == NO_TEST) {
+ return true;
+ }
+
+ Index nerrors = 0;
+
+ ASSERT_EXCEPTION(IsValid(jnlst_), ERROR_IN_TNLP_DERIVATIVE_TEST,
+ "No Journalist given to TNLPAdapter. Need Journalist, otherwise can't produce any output in DerivativeChecker!");
+
+ jnlst_->Printf(J_SUMMARY, J_NLP,
+ "\nStarting derivative checker.\n\n");
+
+ bool retval = true;
+ // Since this method should be independent of all other internal
+ // data (so that it can be called indpenendent of GetSpace etc),
+ // we are not using any internal fields
+
+ // Obtain the problem size
+ Index nx; // number of variables
+ Index ng; // number of constriants
+ Index nz_jac_g; // number of nonzeros in constraint Jacobian
+ Index nz_hess_lag; // number of nonzeros in Lagrangian Hessian
+ TNLP::IndexStyleEnum index_style;
+ tnlp_->get_nlp_info(nx, ng, nz_jac_g, nz_hess_lag, index_style);
+
+ // Obtain starting point as reference point at which derivative
+ // test should be performed
+ Number* xref = new Number[nx];
+ tnlp_->get_starting_point(nx, true, xref, false, NULL, NULL, ng, false, NULL);
+
+ // Obtain value of objective and constraints at reference point
+ bool new_x = true;
+ Number fref;
+ Number* gref = NULL;
+ if (ng>0) {
+ gref = new Number[ng];
+ }
+ retval = tnlp_->eval_f(nx, xref, new_x, fref);
+ ASSERT_EXCEPTION(retval, ERROR_IN_TNLP_DERIVATIVE_TEST,
+ "In TNLP derivative test: f could not be evaluated at reference point.");
+ new_x = false;
+ if (ng>0) {
+ retval = tnlp_->eval_g(nx, xref, new_x, ng, gref);
+ ASSERT_EXCEPTION(retval, ERROR_IN_TNLP_DERIVATIVE_TEST,
+ "In TNLP derivative test: g could not be evaluated at reference point.");
+ }
+
+ // Obtain gradient of objective function at reference pont
+ Number* grad_f = new Number[nx];
+ retval = tnlp_->eval_grad_f(nx, xref, true, grad_f);
+ ASSERT_EXCEPTION(retval, ERROR_IN_TNLP_DERIVATIVE_TEST,
+ "In TNLP derivative test: grad_f could not be evaluated at reference point.");
+
+ Index* g_iRow = NULL;
+ Index* g_jCol = NULL;
+ Number* jac_g = NULL;
+ if (ng>0) {
+ // Obtain constraint Jacobian at reference point (including structure)
+ g_iRow = new Index[nz_jac_g];
+ g_jCol = new Index[nz_jac_g];
+ retval = tnlp_->eval_jac_g(nx, NULL, false, ng, nz_jac_g,
+ g_iRow, g_jCol, NULL);
+ ASSERT_EXCEPTION(retval, ERROR_IN_TNLP_DERIVATIVE_TEST,
+ "In TNLP derivative test: Jacobian structure could not be evaluated.");
+ // Correct counting if required to C-style
+ if (index_style == TNLP::FORTRAN_STYLE) {
+ for (Index i=0; i<nz_jac_g; i++) {
+ g_iRow[i] -= 1;
+ g_jCol[i] -= 1;
+ }
+ }
+ // Obtain values at reference pont
+ jac_g = new Number[nz_jac_g];
+ retval = tnlp_->eval_jac_g(nx, xref, new_x, ng,
+ nz_jac_g, NULL, NULL, jac_g);
+ ASSERT_EXCEPTION(retval, ERROR_IN_TNLP_DERIVATIVE_TEST,
+ "In TNLP derivative test: Jacobian values could not be evaluated at reference point.");
+ }
+
+ // Space for the perturbed point
+ Number* xpert = new Number[nx];
+ IpBlasDcopy(nx, xref, 1, xpert, 1);
+
+ // Space for constraints at perturbed point
+ Number* gpert = NULL;
+ if (ng>0) {
+ gpert = new Number[ng];
+ }
+
+ Index index_correction = 0;
+ if (index_style == TNLP::FORTRAN_STYLE) {
+ index_correction = 1;
+ }
+
+ // Now go through all variables and check the partial derivatives
+ for (Index ivar=0; ivar<nx; ivar++) {
+ Number this_perturbation =
+ derivative_test_perturbation_*Max(1.,fabs(xref[ivar]));
+ xpert[ivar] = xref[ivar] + this_perturbation;
+
+ Number fpert;
+ new_x = true;
+ retval = tnlp_->eval_f(nx, xpert, new_x, fpert);
+ ASSERT_EXCEPTION(retval, ERROR_IN_TNLP_DERIVATIVE_TEST,
+ "In TNLP derivative test: f could not be evaluated at perturbed point.");
+ new_x = false;
+
+ Number deriv_approx = (fpert - fref)/this_perturbation;
+ Number deriv_exact = grad_f[ivar];
+ Number rel_error =
+ fabs(deriv_approx-deriv_exact)/Max(fabs(deriv_approx),1.);
+ char cflag=' ';
+ if (rel_error >= derivative_test_tol_) {
+ cflag='*';
+ nerrors++;
+ }
+ if (cflag != ' ' || derivative_test_print_all_) {
+ jnlst_->Printf(J_WARNING, J_NLP,
+ "%c grad_f[ %5d] = %23.16e ~ %23.16e [%10.3e]\n",
+ cflag, ivar+index_correction,
+ deriv_exact, deriv_approx, rel_error);
+ }
+
+ if (ng>0) {
+ retval = tnlp_->eval_g(nx, xpert, new_x, ng, gpert);
+ ASSERT_EXCEPTION(retval, ERROR_IN_TNLP_DERIVATIVE_TEST,
+ "In TNLP derivative test: g could not be evaluated at reference point.");
+ for (Index icon=0; icon<ng; icon++) {
+ deriv_approx = (gpert[icon] - gref[icon])/this_perturbation;
+ deriv_exact = 0.;
+ bool found = false;
+ for (Index i=0; i<nz_jac_g; i++) {
+ if (g_iRow[i]==icon && g_jCol[i]==ivar) {
+ found = true;
+ deriv_exact += jac_g[i];
+ }
+ }
+
+ rel_error = fabs(deriv_approx-deriv_exact)/Max(fabs(deriv_approx),1.);
+ cflag=' ';
+ if (rel_error >= derivative_test_tol_) {
+ cflag='*';
+ nerrors++;
+ }
+ char sflag=' ';
+ if (found) {
+ sflag = 'v';
+ }
+ if (cflag != ' ' || derivative_test_print_all_) {
+ jnlst_->Printf(J_WARNING, J_NLP,
+ "%c jac_g [%5d,%5d] = %23.16e %c ~ %23.16e [%10.3e]\n",
+ cflag, icon+index_correction, ivar+index_correction,
+ deriv_exact, sflag, deriv_approx, rel_error);
+ }
+ }
+ }
+
+ xpert[ivar] = xref[ivar];
+ }
+
+ const Number zero = 0.;
+ if (deriv_test == SECOND_ORDER_TEST) {
+ // Get sparsity structure of Hessian
+ Index* h_iRow = new Index[nz_hess_lag];
+ Index* h_jCol = new Index[nz_hess_lag];
+ retval = tnlp_->eval_h(nx, NULL, false, 0., ng, NULL, false,
+ nz_hess_lag, h_iRow, h_jCol, NULL);
+ ASSERT_EXCEPTION(retval, ERROR_IN_TNLP_DERIVATIVE_TEST,
+ "In TNLP derivative test: Hessian structure could not be evaluated.");
+
+ if (index_style == TNLP::FORTRAN_STYLE) {
+ for (Index i=0; i<nz_hess_lag; i++) {
+ h_iRow[i] -= 1;
+ h_jCol[i] -= 1;
+ }
+ }
+ Number* h_values = new Number[nz_hess_lag];
+
+ Number* lambda = NULL;
+ if (ng>0) {
+ lambda = new Number[ng];
+ IpBlasDcopy(ng, &zero, 0, lambda, 1);
+ }
+ Number* gradref = new Number[nx]; // gradient of objective or constraint at reference point
+ Number* gradpert = new Number[nx]; // gradient of objective or constraint at perturbed point
+ Number* jacpert = new Number[nz_jac_g];
+
+ // Check all Hessians
+ for (Index icon=-1; icon<ng; icon++) {
+ Number objfact = 0.;
+ if (icon == -1) {
+ objfact = 1.;
+ IpBlasDcopy(nx, grad_f, 1, gradref, 1);
+ }
+ else {
+ lambda[icon] = 1.;
+ IpBlasDcopy(nx, &zero, 0, gradref, 1);
+ for (Index i=0; i<nz_jac_g; i++) {
+ if (g_iRow[i]==icon) {
+ gradref[g_jCol[i]] += jac_g[i];
+ }
+ }
+ }
+ // Hessian at reference point
+ new_x = true;
+ bool new_y = true;
+ retval = tnlp_->eval_h(nx, xref, new_x, objfact, ng, lambda, new_y,
+ nz_hess_lag, NULL, NULL, h_values);
+ new_x = false;
+ new_y = false;
+ ASSERT_EXCEPTION(retval, ERROR_IN_TNLP_DERIVATIVE_TEST,
+ "In TNLP derivative test: Hessian could not be evaluated at reference point.");
+
+ for (Index ivar=0; ivar<nx; ivar++) {
+ Number this_perturbation =
+ derivative_test_perturbation_*Max(1.,fabs(xref[ivar]));
+ xpert[ivar] = xref[ivar] + this_perturbation;
+
+ new_x = true;
+ if (icon==-1) {
+ // we are looking at the objective function
+ retval = tnlp_->eval_grad_f(nx, xpert, new_x, gradpert);
+ ASSERT_EXCEPTION(retval, ERROR_IN_TNLP_DERIVATIVE_TEST,
+ "In TNLP derivative test: grad_f could not be evaluated at perturbed point.");
+ }
+ else {
+ // this is the icon-th constraint
+ retval = tnlp_->eval_jac_g(nx, xpert, new_x, ng,
+ nz_jac_g, NULL, NULL, jacpert);
+ ASSERT_EXCEPTION(retval, ERROR_IN_TNLP_DERIVATIVE_TEST,
+ "In TNLP derivative test: Jacobian values could not be evaluated at reference point.");
+ // ok, now we need to filter the gradient of the icon-th constraint
+ IpBlasDcopy(nx, &zero, 0, gradpert, 1);
+ IpBlasDcopy(nx, &zero, 0, gradref, 1);
+ for (Index i=0; i<nz_jac_g; i++) {
+ if (g_iRow[i]==icon) {
+ gradpert[g_jCol[i]] += jacpert[i];
+ gradref[g_jCol[i]] += jac_g[i];
+ }
+ }
+ }
+ new_x = false;
+
+ for (Index ivar2=0; ivar2<nx; ivar2++) {
+ Number deriv_approx = (gradpert[ivar2] - gradref[ivar2])/this_perturbation;
+ Number deriv_exact = 0.;
+ bool found = false;
+ for (Index i=0; i<nz_hess_lag; i++) {
+ if ( (h_iRow[i]==ivar && h_jCol[i]==ivar2) ||
+ (h_jCol[i]==ivar && h_iRow[i]==ivar2) ) {
+ deriv_exact += h_values[i];
+ found = true;
+ }
+ }
+ Number rel_error =
+ fabs(deriv_approx-deriv_exact)/Max(fabs(deriv_approx),1.);
+ char cflag=' ';
+ if (rel_error >= derivative_test_tol_) {
+ cflag='*';
+ nerrors++;
+ }
+ char sflag = ' ';
+ if (found) {
+ sflag = 'v';
+ }
+ if (cflag != ' ' || derivative_test_print_all_) {
+ if (icon==-1) {
+ jnlst_->Printf(J_WARNING, J_NLP,
+ "%c obj_hess[%5d,%5d] = %23.16e %c ~ %23.16e [%10.3e]\n",
+ cflag, ivar+index_correction, ivar2+index_correction,
+ deriv_exact, sflag, deriv_approx, rel_error);
+ }
+ else {
+ jnlst_->Printf(J_WARNING, J_NLP,
+ "%c %5d-th constr_hess[%5d,%5d] = %23.16e %c ~ %23.16e [%10.3e]\n",
+ cflag, icon+index_correction, ivar+index_correction, ivar2+index_correction,
+ deriv_exact, sflag, deriv_approx, rel_error);
+ }
+ }
+
+ }
+
+ xpert[ivar] = xref[ivar];
+ }
+
+ if (icon>=0) {
+ lambda[icon] = 0.;
+ }
+ }
+
+ delete [] h_iRow;
+ delete [] h_jCol;
+ delete [] h_values;
+ delete [] lambda;
+ delete [] gradref;
+ delete [] gradpert;
+ delete [] jacpert;
+ }
+
+ delete [] xref;
+ delete [] gref;
+ delete [] grad_f;
+ delete [] xpert;
+ delete [] g_iRow;
+ delete [] g_jCol;
+ delete [] jac_g;
+ delete [] gpert;
+
+ if (nerrors==0) {
+ jnlst_->Printf(J_SUMMARY, J_NLP,
+ "\nNo errors detected by derivative checker.\n\n");
+ }
+ else {
+ jnlst_->Printf(J_WARNING, J_NLP,
+ "\nDerivative checker detected %d error(s).\n\n", nerrors);
+ }
+
+ return retval;
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpTNLPAdapter.hpp b/SimTKmath/Optimizers/src/IpOpt/IpTNLPAdapter.hpp
new file mode 100644
index 0000000..12d6687
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpTNLPAdapter.hpp
@@ -0,0 +1,353 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpTNLPAdapter.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPTNLPADAPTER_HPP__
+#define __IPTNLPADAPTER_HPP__
+
+#include "IpNLP.hpp"
+#include "IpTNLP.hpp"
+#include "IpOrigIpoptNLP.hpp"
+
+namespace Ipopt
+{
+
+ // forward declarations
+ class ExpansionMatrix;
+ class ExpansionMatrixSpace;
+ class IteratesVector;
+
+ /** This class Adapts the TNLP interface so it looks like an NLP interface.
+ * This is an Adapter class (Design Patterns) that converts a TNLP to an
+ * NLP. This allows users to write to the "more convenient" TNLP interface.
+ */
+ class TNLPAdapter : public NLP
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default constructor */
+ TNLPAdapter(const SmartPtr<TNLP> tnlp,
+ const SmartPtr<const Journalist> jnlst = NULL);
+
+ /** Default destructor */
+ virtual ~TNLPAdapter();
+ //@}
+
+ /**@name Exceptions */
+ //@{
+ DECLARE_STD_EXCEPTION(INVALID_TNLP);
+ DECLARE_STD_EXCEPTION(ERROR_IN_TNLP_DERIVATIVE_TEST);
+ //@}
+
+ /** @name TNLPAdapter Initialization. */
+ //@{
+ virtual bool ProcessOptions(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Method for creating the derived vector / matrix types
+ * (Do not delete these, the ). */
+ virtual bool GetSpaces(SmartPtr<const VectorSpace>& x_space,
+ SmartPtr<const VectorSpace>& c_space,
+ SmartPtr<const VectorSpace>& d_space,
+ SmartPtr<const VectorSpace>& x_l_space,
+ SmartPtr<const MatrixSpace>& px_l_space,
+ SmartPtr<const VectorSpace>& x_u_space,
+ SmartPtr<const MatrixSpace>& px_u_space,
+ SmartPtr<const VectorSpace>& d_l_space,
+ SmartPtr<const MatrixSpace>& pd_l_space,
+ SmartPtr<const VectorSpace>& d_u_space,
+ SmartPtr<const MatrixSpace>& pd_u_space,
+ SmartPtr<const MatrixSpace>& Jac_c_space,
+ SmartPtr<const MatrixSpace>& Jac_d_space,
+ SmartPtr<const SymMatrixSpace>& Hess_lagrangian_space);
+
+ /** Method for obtaining the bounds information */
+ virtual bool GetBoundsInformation(const Matrix& Px_L,
+ Vector& x_L,
+ const Matrix& Px_U,
+ Vector& x_U,
+ const Matrix& Pd_L,
+ Vector& d_L,
+ const Matrix& Pd_U,
+ Vector& d_U);
+
+ /** Method for obtaining the starting point
+ * for all the iterates. */
+ virtual bool GetStartingPoint(
+ SmartPtr<Vector> x,
+ bool need_x,
+ SmartPtr<Vector> y_c,
+ bool need_y_c,
+ SmartPtr<Vector> y_d,
+ bool need_y_d,
+ SmartPtr<Vector> z_L,
+ bool need_z_L,
+ SmartPtr<Vector> z_U,
+ bool need_z_U
+ );
+
+ /** Method for obtaining an entire iterate as a warmstart point.
+ * The incoming IteratesVector has to be filled. */
+ virtual bool GetWarmStartIterate(IteratesVector& warm_start_iterate);
+ //@}
+
+ /** @name TNLPAdapter evaluation routines. */
+ //@{
+ virtual bool Eval_f(const Vector& x, Number& f);
+
+ virtual bool Eval_grad_f(const Vector& x, Vector& g_f);
+
+ virtual bool Eval_c(const Vector& x, Vector& c);
+
+ virtual bool Eval_jac_c(const Vector& x, Matrix& jac_c);
+
+ virtual bool Eval_d(const Vector& x, Vector& d);
+
+ virtual bool Eval_jac_d(const Vector& x, Matrix& jac_d);
+
+ virtual bool Eval_h(const Vector& x,
+ Number obj_factor,
+ const Vector& yc,
+ const Vector& yd,
+ SymMatrix& h);
+
+ virtual void GetScalingParameters(
+ const SmartPtr<const VectorSpace> x_space,
+ const SmartPtr<const VectorSpace> c_space,
+ const SmartPtr<const VectorSpace> d_space,
+ Number& obj_scaling,
+ SmartPtr<Vector>& x_scaling,
+ SmartPtr<Vector>& c_scaling,
+ SmartPtr<Vector>& d_scaling) const;
+ //@}
+
+ /** @name Solution Reporting Methods */
+ //@{
+ virtual void FinalizeSolution(SolverReturn status,
+ const Vector& x, const Vector& z_L, const Vector& z_U,
+ const Vector& c, const Vector& d,
+ const Vector& y_c, const Vector& y_d,
+ Number obj_value);
+ virtual bool IntermediateCallBack(AlgorithmMode mode,
+ Index iter, Number obj_value,
+ Number inf_pr, Number inf_du,
+ Number mu, Number d_norm,
+ Number regularization_size,
+ Number alpha_du, Number alpha_pr,
+ Index ls_trials,
+ const IpoptData* ip_data,
+ IpoptCalculatedQuantities* ip_cq);
+ //@}
+
+ /** Method returning information on quasi-Newton approximation. */
+ virtual void
+ GetQuasiNewtonApproximationSpaces(SmartPtr<VectorSpace>& approx_space,
+ SmartPtr<Matrix>& P_approx);
+
+ /** Enum for treatment of fixed variables option */
+ enum FixedVariableTreatmentEnum {
+ MAKE_PARAMETER=0,
+ MAKE_CONSTRAINT
+ };
+
+ /** Enum for specifying which derivative test is to be performed. */
+ enum DerivativeTestEnum {
+ NO_TEST=0,
+ FIRST_ORDER_TEST,
+ SECOND_ORDER_TEST
+ };
+
+ /** Method for performing the derivative test */
+ bool CheckDerivatives(DerivativeTestEnum deriv_test);
+
+ /** @name Methods for IpoptType */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ /** Accessor method for the underlying TNLP. */
+ SmartPtr<TNLP> tnlp() const
+ {
+ return tnlp_;
+ }
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ TNLPAdapter(const TNLPAdapter&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const TNLPAdapter&);
+ //@}
+
+ /** @name Methods for translating data for IpoptNLP into the TNLP
+ * data. These methods are used to obtain the current (or
+ * final) data for the TNLP formulation from the IpoptNLP
+ * structure. */
+ //@{
+ /** Sort the primal variables, and add the fixed values in x */
+ void ResortX(const Vector& x, Number* x_orig);
+ void ResortG(const Vector& c, const Vector& d, Number *g_orig);
+ void ResortBnds(const Vector& x_L, Number* x_L_orig,
+ const Vector& x_U, Number* x_U_orig);
+ //@}
+
+ /** Pointer to the TNLP class (class specific to Number* vectors and
+ * harwell triplet matrices) */
+ SmartPtr<TNLP> tnlp_;
+
+ /** Journalist */
+ SmartPtr<const Journalist> jnlst_;
+
+ /**@name Algorithmic parameters */
+ //@{
+ /** Value for a lower bound that denotes -infinity */
+ Number nlp_lower_bound_inf_;
+ /** Value for a upper bound that denotes infinity */
+ Number nlp_upper_bound_inf_;
+ /** Flag indicating how fixed variables should be handled */
+ FixedVariableTreatmentEnum fixed_variable_treatment_;
+ /* Maximal slack for one-sidedly bounded variables. If a
+ * variable has only one bound, say a lower bound xL, then an
+ * upper bound xL + max_onesided_bound_slack_. If this value is
+ * zero, no upper bound is added. */
+ /* Took this out: Number max_onesided_bound_slack_; */
+ /** Enum indicating whether and which derivative test should be
+ * performed at starting point. */
+ DerivativeTestEnum derivative_test_;
+ /** Size of the perturbation for the derivative test */
+ Number derivative_test_perturbation_;
+ /** Relative threshold for marking deviation from finite
+ * difference test */
+ Number derivative_test_tol_;
+ /** Flag indicating if all test values should be printed, or only
+ * those violating the threshold. */
+ bool derivative_test_print_all_;
+ /** Flag indicating whether the TNLP with identical structure has
+ * already been solved before. */
+ bool warm_start_same_structure_;
+ /** Flag indicating what Hessian information is to be used. */
+ HessianApproximationType hessian_approximation_;
+ //@}
+
+ /**@name Problem Size Data */
+ //@{
+ /** full dimension of x (fixed + non-fixed) */
+ Index n_full_x_;
+ /** full dimension of g (c + d) */
+ Index n_full_g_;
+ /** non-zeros of the jacobian of c */
+ Index nz_jac_c_;
+ /** non-zeros of the jacobian of c without added constraints for
+ * fixed variables. */
+ Index nz_jac_c_no_fixed_;
+ /** non-zeros of the jacobian of d */
+ Index nz_jac_d_;
+ /** number of non-zeros in full-size Jacobian of g */
+ Index nz_full_jac_g_;
+ /** number of non-zeros in full-size Hessian */
+ Index nz_full_h_;
+ /** number of non-zeros in the non-fixed-size Hessian */
+ Index nz_h_;
+ /** Number of fixed variables */
+ Index n_x_fixed_;
+ //@}
+
+ /** Numbering style of variables and constraints */
+ TNLP::IndexStyleEnum index_style_;
+
+ /** @name Local copy of spaces (for warm start) */
+ //@{
+ SmartPtr<const VectorSpace> x_space_;
+ SmartPtr<const VectorSpace> c_space_;
+ SmartPtr<const VectorSpace> d_space_;
+ SmartPtr<const VectorSpace> x_l_space_;
+ SmartPtr<const MatrixSpace> px_l_space_;
+ SmartPtr<const VectorSpace> x_u_space_;
+ SmartPtr<const MatrixSpace> px_u_space_;
+ SmartPtr<const VectorSpace> d_l_space_;
+ SmartPtr<const MatrixSpace> pd_l_space_;
+ SmartPtr<const VectorSpace> d_u_space_;
+ SmartPtr<const MatrixSpace> pd_u_space_;
+ SmartPtr<const MatrixSpace> Jac_c_space_;
+ SmartPtr<const MatrixSpace> Jac_d_space_;
+ SmartPtr<const SymMatrixSpace> Hess_lagrangian_space_;
+ //@}
+
+ /**@name Local Copy of the Data */
+ //@{
+ Number* full_x_; /** copy of the full x vector (fixed & non-fixed) */
+ Number* full_lambda_; /** copy of lambda (yc & yd) */
+ Number* full_g_; /** copy of g (c & d) */
+ Number* jac_g_; /** the values for the full jacobian of g */
+ Number* c_rhs_; /** the rhs values of c */
+ //@}
+
+ /**@name Tags for deciding when to update internal copies of vectors */
+ //@{
+ TaggedObject::Tag x_tag_for_iterates_;
+ TaggedObject::Tag y_c_tag_for_iterates_;
+ TaggedObject::Tag y_d_tag_for_iterates_;
+ TaggedObject::Tag x_tag_for_g_;
+ TaggedObject::Tag x_tag_for_jac_g_;
+ //@}
+
+ /**@name Methods to update the values in the local copies of vectors */
+ //@{
+ bool update_local_x(const Vector& x);
+ bool update_local_lambda(const Vector& y_c, const Vector& y_d);
+ //@}
+
+ /**@name Internal routines for evaluating g and jac_g (values stored since
+ * they are used in both c and d routins */
+ //@{
+ bool internal_eval_g(bool new_x);
+ bool internal_eval_jac_g(bool new_x);
+ //@}
+
+ /**@name Internal Permutation Spaces and matrices
+ */
+ //@{
+ /** Expansion from fixed x (ipopt) to full x */
+ SmartPtr<ExpansionMatrix> P_x_full_x_;
+ SmartPtr<ExpansionMatrixSpace> P_x_full_x_space_;
+
+ /** Expansion from fixed x_L (ipopt) to full x */
+ SmartPtr<ExpansionMatrix> P_x_x_L_;
+ SmartPtr<ExpansionMatrixSpace> P_x_x_L_space_;
+
+ /** Expansion from fixed x_U (ipopt) to full x */
+ SmartPtr<ExpansionMatrix> P_x_x_U_;
+ SmartPtr<ExpansionMatrixSpace> P_x_x_U_space_;
+
+ /** Expansion from c only (ipopt) to full ampl c */
+ SmartPtr<ExpansionMatrixSpace> P_c_g_space_;
+ SmartPtr<ExpansionMatrix> P_c_g_;
+
+ /** Expansion from d only (ipopt) to full ampl d */
+ SmartPtr<ExpansionMatrixSpace> P_d_g_space_;
+ SmartPtr<ExpansionMatrix> P_d_g_;
+
+ Index* jac_idx_map_;
+ Index* h_idx_map_;
+
+ /** Position of fixed variables. This is required for a warm start */
+ Index* x_fixed_map_;
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpTSymLinearSolver.cpp b/SimTKmath/Optimizers/src/IpOpt/IpTSymLinearSolver.cpp
new file mode 100644
index 0000000..58542ab
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpTSymLinearSolver.cpp
@@ -0,0 +1,449 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpTSymLinearSolver.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-03-17
+
+#include "IpTSymLinearSolver.hpp"
+#include "IpTripletHelper.hpp"
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ TSymLinearSolver::TSymLinearSolver
+ (SmartPtr<SparseSymLinearSolverInterface> solver_interface,
+ SmartPtr<TSymScalingMethod> scaling_method)
+ :
+ SymLinearSolver(),
+ atag_(0),
+ dim_(0),
+ nonzeros_triplet_(0),
+ nonzeros_compressed_(0),
+ have_structure_(false),
+ initialized_(false),
+
+ solver_interface_(solver_interface),
+ scaling_method_(scaling_method),
+ scaling_factors_(NULL),
+ airn_(NULL),
+ ajcn_(NULL)
+ {
+ DBG_START_METH("TSymLinearSolver::TSymLinearSolver()",dbg_verbosity);
+ DBG_ASSERT(IsValid(solver_interface));
+ }
+
+ TSymLinearSolver::~TSymLinearSolver()
+ {
+ DBG_START_METH("TSymLinearSolver::~TSymLinearSolver()",
+ dbg_verbosity);
+ delete [] airn_;
+ delete [] ajcn_;
+ delete [] scaling_factors_;
+ }
+
+ void TSymLinearSolver::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->AddStringOption2(
+ "linear_scaling_on_demand",
+ "Flag indicating that linear scaling is only done if it seems required.",
+ "yes",
+ "no", "Always scale the linear system.",
+ "yes", "Start using linear system scaling if solutions seem not good.",
+ "This option is only important if a linear scaling method (e.g., mc19) "
+ "is used. If you choose \"no\", then the scaling factors are computed "
+ "for every linear system from the start. This can be quite expensive. "
+ "Choosing \"yes\" means that the algorithm will start the scaling "
+ "method only when the solutions to the linear system seem not good, and "
+ "then use it until the end.");
+ }
+
+ bool TSymLinearSolver::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ if (IsValid(scaling_method_)) {
+ options.GetBoolValue("linear_scaling_on_demand",
+ linear_scaling_on_demand_, prefix);
+ }
+ else {
+ linear_scaling_on_demand_ = false;
+ }
+ // This option is registered by OrigIpoptNLP
+ options.GetBoolValue("warm_start_same_structure",
+ warm_start_same_structure_, prefix);
+
+ if (!solver_interface_->Initialize(Jnlst(), IpNLP(), IpData(), IpCq(),
+ options, prefix)) {
+ return false;
+ }
+
+ if (!warm_start_same_structure_) {
+ // Reset all private data
+ atag_=0;
+ dim_=0;
+ nonzeros_triplet_=0;
+ nonzeros_compressed_=0;
+ have_structure_=false;
+
+ matrix_format_ = solver_interface_->MatrixFormat();
+ switch (matrix_format_) {
+ case SparseSymLinearSolverInterface::Dense_Format:
+ triplet_to_dense_converter_ = new TripletToDenseConverter(0);
+ break;
+ case SparseSymLinearSolverInterface::CSR_Format_0_Offset:
+ triplet_to_csr_converter_ = new TripletToCSRConverter(0);
+ break;
+ case SparseSymLinearSolverInterface::CSR_Format_1_Offset:
+ triplet_to_csr_converter_ = new TripletToCSRConverter(1);
+ break;
+ case SparseSymLinearSolverInterface::Triplet_Format:
+ triplet_to_csr_converter_ = NULL;
+ break;
+ default:
+ DBG_ASSERT(false && "Invalid MatrixFormat returned from solver interface.");
+ return false;
+ }
+ }
+ else {
+ ASSERT_EXCEPTION(have_structure_, INVALID_WARMSTART,
+ "TSymLinearSolver called with warm_start_same_structure, but the internal structures are not initialized.");
+ }
+
+ // reset the initialize flag to make sure that InitializeStructure
+ // is called for the linear solver
+ initialized_=false;
+
+ if (IsValid(scaling_method_) && !linear_scaling_on_demand_) {
+ use_scaling_ = true;
+ }
+ else {
+ use_scaling_ = false;
+ }
+ just_switched_on_scaling_ = false;
+
+ bool retval = true;
+ if (IsValid(scaling_method_)) {
+ IpData().TimingStats().LinearSystemScaling().Start();
+ retval = scaling_method_->Initialize(Jnlst(), IpNLP(), IpData(), IpCq(),
+ options, prefix);
+ IpData().TimingStats().LinearSystemScaling().End();
+ }
+ return retval;
+ }
+
+ ESymSolverStatus
+ TSymLinearSolver::MultiSolve(const SymMatrix& sym_A,
+ std::vector<SmartPtr<const Vector> >& rhsV,
+ std::vector<SmartPtr<Vector> >& solV,
+ bool check_NegEVals,
+ Index numberOfNegEVals)
+ {
+ DBG_START_METH("TSymLinearSolver::MultiSolve",dbg_verbosity);
+ DBG_ASSERT(!check_NegEVals || ProvidesInertia());
+
+ // Check if this object has ever seen a matrix If not,
+ // allocate memory of the matrix structure and copy the nonzeros
+ // structure (it is assumed that this will never change).
+ if (!initialized_) {
+ ESymSolverStatus retval = InitializeStructure(sym_A);
+ if (retval != SYMSOLVER_SUCCESS) {
+ return retval;
+ }
+ }
+
+ DBG_ASSERT(nonzeros_triplet_== TripletHelper::GetNumberEntries(sym_A));
+
+ // Check if the matrix has been changed
+ DBG_PRINT((1,"atag_=%d sym_A->GetTag()=%d\n",atag_,sym_A.GetTag()));
+ bool new_matrix = sym_A.HasChanged(atag_);
+ atag_ = sym_A.GetTag();
+
+ // If a new matrix is encountered, get the array for storing the
+ // entries from the linear solver interface, fill in the new
+ // values, compute the new scaling factors (if required), and
+ // scale the matrix
+ if (new_matrix || just_switched_on_scaling_) {
+ GiveMatrixToSolver(true, sym_A);
+ new_matrix = true;
+ }
+
+ // Retrieve the right hand sides and scale if required
+ Index nrhs = (Index)rhsV.size();
+ Number* rhs_vals = new Number[dim_*nrhs];
+ for (Index irhs=0; irhs<nrhs; irhs++) {
+ TripletHelper::FillValuesFromVector(dim_, *rhsV[irhs],
+ &rhs_vals[irhs*(dim_)]);
+ if (use_scaling_) {
+ IpData().TimingStats().LinearSystemScaling().Start();
+ for (Index i=0; i<dim_; i++) {
+ rhs_vals[irhs*(dim_)+i] *= scaling_factors_[i];
+ }
+ IpData().TimingStats().LinearSystemScaling().End();
+ }
+ }
+
+ bool done = false;
+ // Call the linear solver through the interface to solve the
+ // system. This is repeated, if the return values is S_CALL_AGAIN
+ // after the values have been restored (this might be necessary
+ // for MA27 if the size of the work space arrays was not large
+ // enough).
+ ESymSolverStatus retval;
+ while (!done) {
+ const Index* ia;
+ const Index* ja;
+ if (matrix_format_==SparseSymLinearSolverInterface::Triplet_Format) {
+ ia = airn_;
+ ja = ajcn_;
+ } else if (matrix_format_==SparseSymLinearSolverInterface::Dense_Format) {
+ ia = triplet_to_dense_converter_->IA();
+ ja = triplet_to_dense_converter_->JA();
+ }else {
+ IpData().TimingStats().LinearSystemStructureConverter().Start();
+ ia = triplet_to_csr_converter_->IA();
+ ja = triplet_to_csr_converter_->JA();
+ IpData().TimingStats().LinearSystemStructureConverter().End();
+ }
+
+ retval = solver_interface_->MultiSolve(new_matrix, ia, ja,
+ nrhs, rhs_vals, check_NegEVals,
+ numberOfNegEVals);
+ if (retval==SYMSOLVER_CALL_AGAIN) {
+ DBG_PRINT((1, "Solver interface asks to be called again.\n"));
+ GiveMatrixToSolver(false, sym_A);
+ }
+ else {
+ done = true;
+ }
+ }
+
+ // If the solve was successful, unscale the solution (if required)
+ // and transfer the result into the Vectors
+ if (retval==SYMSOLVER_SUCCESS) {
+ for (Index irhs=0; irhs<nrhs; irhs++) {
+ if (use_scaling_) {
+ IpData().TimingStats().LinearSystemScaling().Start();
+ for (Index i=0; i<dim_; i++) {
+ rhs_vals[irhs*(dim_)+i] *= scaling_factors_[i];
+ }
+ IpData().TimingStats().LinearSystemScaling().End();
+ }
+ TripletHelper::PutValuesInVector(dim_, &rhs_vals[irhs*(dim_)],
+ *solV[irhs]);
+ }
+ }
+
+ delete[] rhs_vals;
+
+ return retval;
+ }
+
+ // Initialize the local copy of the positions of the nonzero
+ // elements
+ ESymSolverStatus
+ TSymLinearSolver::InitializeStructure(const SymMatrix& sym_A)
+ {
+ DBG_START_METH("TSymLinearSolver::InitializeStructure",
+ dbg_verbosity);
+ DBG_ASSERT(!initialized_);
+
+ ESymSolverStatus retval;
+
+ // have_structure_ is already true if this is a warm start for a
+ // problem with identical structure
+ if (!have_structure_) {
+
+ dim_ = sym_A.Dim();
+ nonzeros_triplet_ = TripletHelper::GetNumberEntries(sym_A);
+
+ delete [] airn_;
+ delete [] ajcn_;
+ airn_ = new Index[nonzeros_triplet_];
+ ajcn_ = new Index[nonzeros_triplet_];
+
+ TripletHelper::FillRowCol(nonzeros_triplet_, sym_A, airn_, ajcn_);
+
+ // If the solver wants the compressed format, the converter has to
+ // be initialized
+ const Index *ia;
+ const Index *ja;
+ Index nonzeros;
+ if (matrix_format_ == SparseSymLinearSolverInterface::Triplet_Format) {
+ ia = airn_;
+ ja = ajcn_;
+ nonzeros = nonzeros_triplet_;
+ } else if(matrix_format_ == SparseSymLinearSolverInterface::Dense_Format) {
+ nonzeros_compressed_ =
+ triplet_to_dense_converter_->InitializeConverter(dim_, nonzeros_triplet_,
+ airn_, ajcn_);
+ ia = triplet_to_dense_converter_->IA();
+ ja = triplet_to_dense_converter_->JA();
+ nonzeros = nonzeros_compressed_;
+ } else {
+ IpData().TimingStats().LinearSystemStructureConverter().Start();
+ IpData().TimingStats().LinearSystemStructureConverterInit().Start();
+ nonzeros_compressed_ =
+ triplet_to_csr_converter_->InitializeConverter(dim_, nonzeros_triplet_,
+ airn_, ajcn_);
+ IpData().TimingStats().LinearSystemStructureConverterInit().End();
+ ia = triplet_to_csr_converter_->IA();
+ ja = triplet_to_csr_converter_->JA();
+ IpData().TimingStats().LinearSystemStructureConverter().End();
+ nonzeros = nonzeros_compressed_;
+ }
+
+ retval = solver_interface_->InitializeStructure(dim_, nonzeros, ia, ja);
+ if (retval != SYMSOLVER_SUCCESS) {
+ return retval;
+ }
+
+ // Get space for the scaling factors
+ delete [] scaling_factors_;
+ if (IsValid(scaling_method_)) {
+ IpData().TimingStats().LinearSystemScaling().Start();
+ scaling_factors_ = new Number[dim_];
+ IpData().TimingStats().LinearSystemScaling().End();
+ }
+
+ have_structure_ = true;
+ }
+ else {
+ ASSERT_EXCEPTION(dim_==sym_A.Dim(), INVALID_WARMSTART,
+ "TSymLinearSolver called with warm_start_same_structure, but the problem is solved for the first time.");
+ // This is a warm start for identical structure, so we don't need to
+ // recompute the nonzeros location arrays
+ const Index *ia;
+ const Index *ja;
+ Index nonzeros;
+ if (matrix_format_ == SparseSymLinearSolverInterface::Triplet_Format) {
+ ia = airn_;
+ ja = ajcn_;
+ nonzeros = nonzeros_triplet_;
+ } else if (matrix_format_ == SparseSymLinearSolverInterface::Dense_Format) {
+ ia = triplet_to_dense_converter_->IA();
+ ja = triplet_to_dense_converter_->JA();
+ nonzeros = nonzeros_compressed_;
+ } else {
+ IpData().TimingStats().LinearSystemStructureConverter().Start();
+ ia = triplet_to_csr_converter_->IA();
+ ja = triplet_to_csr_converter_->JA();
+ IpData().TimingStats().LinearSystemStructureConverter().End();
+ nonzeros = nonzeros_compressed_;
+ }
+ retval = solver_interface_->InitializeStructure(dim_, nonzeros, ia, ja);
+ }
+ initialized_=true;
+ return retval;
+ }
+
+ Index TSymLinearSolver::NumberOfNegEVals() const
+ {
+ DBG_START_METH("TSymLinearSolver::NumberOfNegEVals",dbg_verbosity);
+ return solver_interface_->NumberOfNegEVals();
+ }
+
+ bool TSymLinearSolver::IncreaseQuality()
+ {
+ DBG_START_METH("TSymLinearSolver::IncreaseQuality",dbg_verbosity);
+
+ if (IsValid(scaling_method_) && !use_scaling_ &&
+ linear_scaling_on_demand_) {
+ Jnlst().Printf(J_DETAILED, J_LINEAR_ALGEBRA,
+ "Switching on scaling of the linear system (on demand).\n");
+ IpData().Append_info_string("Mc");
+ use_scaling_ = true;
+ just_switched_on_scaling_ = true;
+ return true;
+ }
+
+ return solver_interface_->IncreaseQuality();
+ }
+
+ bool TSymLinearSolver::ProvidesInertia() const
+ {
+ DBG_START_METH("TSymLinearSolver::ProvidesInertia",dbg_verbosity);
+
+ return solver_interface_->ProvidesInertia();
+ }
+
+ void TSymLinearSolver::GiveMatrixToSolver(bool new_matrix,
+ const SymMatrix& sym_A)
+ {
+ DBG_START_METH("TSymLinearSolver::GiveMatrixToSolver",dbg_verbosity);
+ DBG_PRINT((1,"new_matrix = %d\n",new_matrix));
+
+ Number* pa = solver_interface_->GetValuesArrayPtr();
+ Number* atriplet;
+
+ if (matrix_format_!=SparseSymLinearSolverInterface::Triplet_Format) {
+ atriplet = new Number[nonzeros_triplet_];
+ }
+ else {
+ atriplet = pa;
+ }
+
+ //DBG_PRINT_MATRIX(3, "Aunscaled", sym_A);
+ TripletHelper::FillValues(nonzeros_triplet_, sym_A, atriplet);
+ if (DBG_VERBOSITY()>=3) {
+ for (Index i=0; i<nonzeros_triplet_; i++) {
+ DBG_PRINT((3, "KKTunscaled(%6d,%6d) = %24.16e\n", airn_[i], ajcn_[i], atriplet[i]));
+ }
+ }
+
+ if (use_scaling_) {
+ IpData().TimingStats().LinearSystemScaling().Start();
+ DBG_ASSERT(scaling_factors_);
+ if (new_matrix || just_switched_on_scaling_) {
+ // only compute scaling factors if the matrix has not been
+ // changed since the last call to this method
+ bool retval =
+ scaling_method_->ComputeSymTScalingFactors(dim_, nonzeros_triplet_,
+ airn_, ajcn_,
+ atriplet, scaling_factors_);
+ if (!retval) {
+ Jnlst().Printf(J_ERROR, J_LINEAR_ALGEBRA,
+ "Error during computation of scaling factors.\n");
+ THROW_EXCEPTION(ERROR_IN_LINEAR_SCALING_METHOD, "scaling_method_->ComputeSymTScalingFactors returned false.")
+ }
+ // complain if not in debug mode
+ if (Jnlst().ProduceOutput(J_MOREVECTOR, J_LINEAR_ALGEBRA)) {
+ for (Index i=0; i<dim_; i++) {
+ Jnlst().Printf(J_MOREVECTOR, J_LINEAR_ALGEBRA,
+ "scaling factor[%6d] = %22.17e\n",
+ i, scaling_factors_[i]);
+ }
+ }
+ just_switched_on_scaling_ = false;
+ }
+ for (Index i=0; i<nonzeros_triplet_; i++) {
+ atriplet[i] *=
+ scaling_factors_[airn_[i]-1] * scaling_factors_[ajcn_[i]-1];
+ }
+ if (DBG_VERBOSITY()>=3) {
+ for (Index i=0; i<nonzeros_triplet_; i++) {
+ DBG_PRINT((3, "KKTscaled(%6d,%6d) = %24.16e\n", airn_[i], ajcn_[i], atriplet[i]));
+ }
+ }
+ IpData().TimingStats().LinearSystemScaling().End();
+ }
+
+ if (matrix_format_!=SparseSymLinearSolverInterface::Triplet_Format) {
+ IpData().TimingStats().LinearSystemStructureConverter().Start();
+ if( matrix_format_ == SparseSymLinearSolverInterface::Dense_Format ){
+ triplet_to_dense_converter_->ConvertValues(nonzeros_triplet_, atriplet,
+ dim_, pa);
+ } else {
+ triplet_to_csr_converter_->ConvertValues(nonzeros_triplet_, atriplet,
+ nonzeros_compressed_, pa);
+ }
+ IpData().TimingStats().LinearSystemStructureConverter().End();
+ delete[] atriplet;
+ }
+
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpTSymLinearSolver.hpp b/SimTKmath/Optimizers/src/IpOpt/IpTSymLinearSolver.hpp
new file mode 100644
index 0000000..567148b
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpTSymLinearSolver.hpp
@@ -0,0 +1,195 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpTSymLinearSolver.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-03-17
+
+#ifndef __IPTSYMLINEARSOLVER_HPP__
+#define __IPTSYMLINEARSOLVER_HPP__
+
+#include "IpSymLinearSolver.hpp"
+#include "IpSparseSymLinearSolverInterface.hpp"
+#include "IpTSymScalingMethod.hpp"
+#include "IpSymMatrix.hpp"
+#include "IpTripletToCSRConverter.hpp"
+#include "IpTripletToDenseConverter.hpp"
+#include <vector>
+
+namespace Ipopt
+{
+
+ /** General driver for linear solvers for sparse indefinite
+ * symmetric matrices. This interface includes a call to a method
+ * for scaling of the matrix (if given). This class takes in the
+ * contructor a pointer to the interface to an actual linear
+ * solver, and possibly a pointer to a method for computing scaling
+ * factors. It translates the SymMatrix into the format required
+ * by the linear solver and calls the solver via the
+ * TSymLinearSolverInterface. If a scaling method has been given,
+ * the matrix, the right hand side, and the solution are scaled.
+ */
+ class TSymLinearSolver: public SymLinearSolver
+ {
+ public:
+ /** @name Constructor/Destructor */
+ //@{
+ /** Constructor. The solver_interface is a pointer to a linear
+ * solver for symmetric matrices in triplet format. If
+ * scaling_method not NULL, it must be a pointer to a class for
+ * computing scaling factors for the matrix. */
+ TSymLinearSolver(SmartPtr<SparseSymLinearSolverInterface> solver_interface,
+ SmartPtr<TSymScalingMethod> scaling_method);
+
+ /** Destructor */
+ virtual ~TSymLinearSolver();
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** @name Methods for requesting solution of the linear system. */
+ //@{
+ /** Solve operation for multiple right hand sides. For details
+ * see the description in the base class SymLinearSolver.
+ */
+ virtual ESymSolverStatus MultiSolve(const SymMatrix &A,
+ std::vector<SmartPtr<const Vector> >& rhsV,
+ std::vector<SmartPtr<Vector> >& solV,
+ bool check_NegEVals,
+ Index numberOfNegEVals);
+
+ /** Number of negative eigenvalues detected during last
+ * factorization. Returns the number of negative eigenvalues of
+ * the most recent factorized matrix.
+ */
+ virtual Index NumberOfNegEVals() const;
+ //@}
+
+ //* @name Options of Linear solver */
+ //@{
+ /** Request to increase quality of solution for next solve.
+ * Ask linear solver to increase quality of solution for the next
+ * solve (e.g. increase pivot tolerance). Returns false, if this
+ * is not possible (e.g. maximal pivot tolerance already used.)
+ */
+ virtual bool IncreaseQuality();
+
+ /** Query whether inertia is computed by linear solver.
+ * Returns true, if linear solver provides inertia.
+ */
+ virtual bool ProvidesInertia() const;
+ //@}
+
+ /** Methods for OptionsList */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ TSymLinearSolver();
+
+ /** Copy Constructor */
+ TSymLinearSolver(const TSymLinearSolver&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const TSymLinearSolver&);
+ //@}
+
+ /** @name Information about the matrix */
+ //@{
+ /** Tag for the incoming matrix */
+ TaggedObject::Tag atag_;
+
+ /** Number of rows and columns of the matrix */
+ Index dim_;
+
+ /** Number of nonzeros of the matrix in triplet format. Note that
+ * some elements might appear multiple times in which case the
+ * values are added. */
+ Index nonzeros_triplet_;
+ /** Number of nonzeros in compressed format. This is only
+ * computed if the sparse linear solver works with the CSR
+ * format. */
+ Index nonzeros_compressed_;
+ //@}
+
+ /** @name Initialization flags */
+ //@{
+ /** Flag indicating if the internal structures are initialized.
+ * For initialization, this object needs to have seen a matrix */
+ bool have_structure_;
+ /** Flag indicating whether the scaling objected is to be switched
+ * on when increased quality is requested */
+ bool linear_scaling_on_demand_;
+ /** Flag indicating if the InitializeStructure method has been
+ * called for the linear solver. */
+ bool initialized_;
+ //@}
+
+ /** Strategy Object for an interface to a linear solver. */
+ SmartPtr<SparseSymLinearSolverInterface> solver_interface_;
+ /** @name Stuff for scaling of the linear system. */
+ //@{
+ /** Strategy Object for a method that computes scaling factors for
+ * the matrices. If NULL, no scaling is performed. */
+ SmartPtr<TSymScalingMethod> scaling_method_;
+ /** Array storing the scaling factors */
+ Number* scaling_factors_;
+ /** Flag indicating whether scaling should be performed */
+ bool use_scaling_;
+ /** Flag indicating whether we just switched on the scaling */
+ bool just_switched_on_scaling_;
+ //@}
+
+ /** @name information about the matrix. */
+ //@{
+ /** row indices of matrix in triplet (MA27) format.
+ */
+ Index* airn_;
+ /** column indices of matrix in triplet (MA27) format.
+ */
+ Index* ajcn_;
+ /** Pointer to object for conversion from triplet to compressed
+ * format. This is only required if the linear solver works with
+ * the compressed representation. */
+ SmartPtr<TripletToCSRConverter> triplet_to_csr_converter_;
+ SmartPtr<TripletToDenseConverter> triplet_to_dense_converter_;
+ /** Flag indicating what matrix data format the solver requires. */
+ SparseSymLinearSolverInterface::EMatrixFormat matrix_format_;
+ //@}
+
+ /** @name Algorithmic parameters */
+ //@{
+ /** Flag indicating whether the TNLP with identical structure has
+ * already been solved before. */
+ bool warm_start_same_structure_;
+ //@}
+
+ /** @name Internal functions */
+ //@{
+ /** Initialize nonzero structure.
+ * Set dim_ and nonzeros_, and copy the nonzero structure of symT_A
+ * into airn_ and ajcn_
+ */
+ ESymSolverStatus InitializeStructure(const SymMatrix& symT_A);
+
+ /** Copy the elements of the matrix in the required format into
+ * the array that is provided by the solver interface. */
+ void GiveMatrixToSolver(bool new_matrix, const SymMatrix& sym_A);
+ //@}
+ };
+
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpTSymScalingMethod.hpp b/SimTKmath/Optimizers/src/IpOpt/IpTSymScalingMethod.hpp
new file mode 100644
index 0000000..b913957
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpTSymScalingMethod.hpp
@@ -0,0 +1,63 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpTSymScalingMethod.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-03-17
+
+#ifndef __IPTSYMSCALINGMETHOD_HPP__
+#define __IPTSYMSCALINGMETHOD_HPP__
+
+#include "IpUtils.hpp"
+#include "IpAlgStrategy.hpp"
+
+namespace Ipopt
+{
+
+ DECLARE_STD_EXCEPTION(ERROR_IN_LINEAR_SCALING_METHOD);
+
+ /** Base class for the method for computing scaling factors for symmetric
+ * matrices in triplet format.
+ */
+ class TSymScalingMethod: public AlgorithmStrategyObject
+ {
+ public:
+ /** @name Constructor/Destructor */
+ //@{
+ TSymScalingMethod()
+ {}
+
+ ~TSymScalingMethod()
+ {}
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix) = 0;
+
+ /** Method for computing the symmetric scaling factors, given the
+ * symmtric matrix in triplet (MA27) format. */
+ virtual bool ComputeSymTScalingFactors(Index n,
+ Index nnz,
+ const Index* airn,
+ const Index* ajcn,
+ const Number* a,
+ Number* scaling_factors) = 0;
+ private:
+ /**@name Default Compiler Generated Methods (Hidden to avoid
+ * implicit creation/calling). These methods are not implemented
+ * and we do not want the compiler to implement them for us, so we
+ * declare them private and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ TSymScalingMethod(const TSymScalingMethod&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const TSymScalingMethod&);
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpTaggedObject.cpp b/SimTKmath/Optimizers/src/IpOpt/IpTaggedObject.cpp
new file mode 100644
index 0000000..03e3d09
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpTaggedObject.cpp
@@ -0,0 +1,16 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpTaggedObject.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpTaggedObject.hpp"
+
+namespace Ipopt
+{
+
+ TaggedObject::Tag TaggedObject::unique_tag_ = 1;
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpTaggedObject.hpp b/SimTKmath/Optimizers/src/IpOpt/IpTaggedObject.hpp
new file mode 100644
index 0000000..bc5d6c5
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpTaggedObject.hpp
@@ -0,0 +1,149 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpTaggedObject.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPTAGGEDOBJECT_HPP__
+#define __IPTAGGEDOBJECT_HPP__
+
+#include "IpUtils.hpp"
+#include "IpDebug.hpp"
+#include "IpReferenced.hpp"
+#include "IpObserver.hpp"
+#include <limits>
+
+namespace Ipopt
+{
+
+ /** TaggedObject class.
+ * Often, certain calculations or operations are expensive,
+ * and it can be very inefficient to perform these calculations
+ * again if the input to the calculation has not changed
+ * since the result was last stored.
+ * This base class provides an efficient mechanism to update
+ * a tag, indicating that the object has changed.
+ * Users of a TaggedObject class, need their own Tag data
+ * member to keep track of the state of the TaggedObject, the
+ * last time they performed a calculation. A basic use case for
+ * users of a class inheriting off of TaggedObject follows like
+ * this:
+ *
+ * Initialize your own Tag to zero in constructor.
+ *
+ *
+ *
+ * Before an expensive calculation,
+ * check if the TaggedObject has changed, passing in
+ * your own Tag, indicating the last time you used
+ * the object for the calculation. If it has changed,
+ * perform the calculation again, and store the result.
+ * If it has not changed, simply return the stored result.
+ *
+ * Here is a simple example:
+ \verbatim
+ if (vector.HasChanged(my_vector_tag_)) {
+ my_vector_tag_ = vector.GetTag();
+ result = PerformExpensiveCalculation(vector);
+ return result;
+ }
+ else {
+ return result;
+ }
+ \endverbatim
+ *
+ * Objects derived from TaggedObject:
+ * Objects derived from TaggedObject must indicate that they have changed to
+ * the base class using the protected member function ObjectChanged(). For
+ * example, a Vector class, inside its own set method, MUST call
+ * ObjectChanged() to update the internally stored tag for comparison.
+ */
+ class TaggedObject : public ReferencedObject, public Subject
+ {
+ public:
+ /** Type for the Tag values */
+ typedef unsigned int Tag;
+
+ /** Constructor. */
+ TaggedObject()
+ :
+ Subject()
+ {
+ ObjectChanged();
+ }
+
+ /** Destructor. */
+ virtual ~TaggedObject()
+ {}
+
+ /** Users of TaggedObjects call this to
+ * update their own internal tags every time
+ * they perform the expensive operation.
+ */
+ Tag GetTag() const
+ {
+ return tag_;
+ }
+
+ /** Users of TaggedObjects call this to
+ * check if the object HasChanged since
+ * they last updated their own internal
+ * tag.
+ */
+ bool HasChanged(const Tag comparison_tag) const
+ {
+ return (comparison_tag == tag_) ? false : true;
+ }
+ protected:
+ /** Objects derived from TaggedObject MUST call this
+ * method every time their internal state changes to
+ * update the internal tag for comparison
+ */
+ void ObjectChanged()
+ {
+ DBG_START_METH("TaggedObject::ObjectChanged()", 0);
+ tag_ = unique_tag_;
+ unique_tag_++;
+ DBG_ASSERT(unique_tag_ < std::numeric_limits<Tag>::max());
+ // The Notify method from the Subject base class notifies all
+ // registed Observers that this subject has changed.
+ Notify(Observer::NT_Changed);
+ }
+ private:
+ /**@name Default Compiler Generated Methods (Hidden to avoid
+ * implicit creation/calling). These methods are not implemented
+ * and we do not want the compiler to implement them for us, so we
+ * declare them private and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ TaggedObject(const TaggedObject&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const TaggedObject&);
+ //@}
+
+ /** static data member that is incremented every
+ * time ANY TaggedObject changes. This allows us
+ * to obtain a unique Tag when the object changes
+ */
+ static Tag unique_tag_;
+
+ /** The tag indicating the current state of the object.
+ * We use this to compare against the comparison_tag
+ * in the HasChanged method. This member is updated
+ * from the unique_tag_ every time the object changes.
+ */
+ Tag tag_;
+
+ /** The index indicating the cache priority for this
+ * TaggedObject. If a result that depended on this
+ * TaggedObject is cached, it will be cached with this
+ * priority
+ */
+ Index cache_priority_;
+ };
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpTimedTask.hpp b/SimTKmath/Optimizers/src/IpOpt/IpTimedTask.hpp
new file mode 100644
index 0000000..239dbf8
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpTimedTask.hpp
@@ -0,0 +1,160 @@
+// Copyright (C) 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpTimedTask.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-09-19
+
+#ifndef __IPTIMEDTASK_HPP__
+#define __IPTIMEDTASK_HPP__
+
+#ifdef HAVE_CTIME
+# include <ctime>
+#else
+# ifdef HAVE_TIME_H
+# include <time.h>
+# else
+# error "don't have header file for time"
+# endif
+#endif
+
+// The following lines are copied from CoinTime.hpp
+// We should probably make some more tests here
+#if defined(_MSC_VER)
+// Turn off compiler warning about long names
+# pragma warning(disable:4786)
+#else
+// MacOS-X and FreeBSD needs sys/time.h
+# if defined(__MACH__) || defined (__FreeBSD__)
+# include <sys/time.h>
+# endif
+# if !defined(__MSVCRT__)
+# include <sys/resource.h>
+# endif
+#endif
+
+namespace Ipopt
+{
+ /** This class is used to collect timing information for a
+ * particular task. */
+ class TimedTask
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default constructor. */
+ TimedTask()
+ :
+ total_time_(0.),
+ start_called_(false),
+ end_called_(true)
+ {}
+
+ /** Default destructor */
+ ~TimedTask()
+ {}
+ //@}
+
+ /** Method for resetting time to zero. */
+ void Reset()
+ {
+ total_time_ = 0.;
+ start_called_ = false;
+ end_called_ = true;
+ }
+
+ /** Method that is called before execution of the task. */
+ void Start()
+ {
+ DBG_ASSERT(end_called_);
+ DBG_ASSERT(!start_called_);
+ end_called_ = false;
+ start_called_ = true;
+ start_time_ = CpuTime();
+ }
+
+ /** Method that is called after execution of the task. */
+ void End()
+ {
+ DBG_ASSERT(!end_called_);
+ DBG_ASSERT(start_called_);
+ end_called_ = true;
+ start_called_ = false;
+ total_time_ += CpuTime() - start_time_;
+ }
+
+ /** Method that is called after execution of the task for which
+ * timing might have been started. This only updates the timing
+ * if the timing has indeed been conducted. This is useful to
+ * stop timing after catching exceptions. */
+ void EndIfStarted()
+ {
+ if (start_called_) {
+ end_called_ = true;
+ start_called_ = false;
+ total_time_ += CpuTime() - start_time_;
+ }
+ DBG_ASSERT(end_called_);
+ }
+
+ /** Method returning total time spend for task so far. */
+ Number TotalTime() const
+ {
+ DBG_ASSERT(end_called_);
+ return total_time_;
+ }
+
+ // The following lines were taken from CoinTime.hpp in COIN/Coin
+ /** method determining CPU executed since start of program */
+ static inline Number CpuTime()
+ {
+ double cpu_temp;
+#if defined(_MSC_VER) || defined(__MSVCRT__)
+
+ unsigned int ticksnow; /* clock_t is same as int */
+
+ ticksnow = (unsigned int)clock();
+
+ cpu_temp = (double)((double)ticksnow/CLOCKS_PER_SEC);
+#else
+
+ struct rusage usage;
+ getrusage(RUSAGE_SELF,&usage);
+ cpu_temp = usage.ru_utime.tv_sec;
+ cpu_temp += 1.0e-6*((double) usage.ru_utime.tv_usec);
+#endif
+
+ return Number(cpu_temp);
+ }
+
+ private:
+ /**@name Default Compiler Generated Methods (Hidden to avoid
+ * implicit creation/calling). These methods are not
+ * implemented and we do not want the compiler to implement them
+ * for us, so we declare them private and do not define
+ * them. This ensures that they will not be implicitly
+ * created/called. */
+ //@{
+ /** Copy Constructor */
+ TimedTask(const TimedTask&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const TimedTask&);
+ //@}
+
+ /** Time at beginning of task. */
+ Number start_time_;
+ /** Total time for task measured so far. */
+ Number total_time_;
+
+ /** @name fields for debugging */
+ //@{
+ bool start_called_;
+ bool end_called_;
+ //@}
+
+ };
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpTimingStatistics.cpp b/SimTKmath/Optimizers/src/IpOpt/IpTimingStatistics.cpp
new file mode 100644
index 0000000..95da927
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpTimingStatistics.cpp
@@ -0,0 +1,134 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpTimingStatistics.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-09-19
+
+#include "IpTimingStatistics.hpp"
+
+namespace Ipopt
+{
+ void
+ TimingStatistics::ResetTimes()
+ {
+ OverallAlgorithm_.Reset();
+ PrintProblemStatistics_.Reset();
+ InitializeIterates_.Reset();
+ UpdateHessian_.Reset();
+ OutputIteration_.Reset();
+ UpdateBarrierParameter_.Reset();
+ ComputeSearchDirection_.Reset();
+ ComputeAcceptableTrialPoint_.Reset();
+ AcceptTrialPoint_.Reset();
+ CheckConvergence_.Reset();
+ PDSystemSolverTotal_.Reset();
+ PDSystemSolverSolveOnce_.Reset();
+ ComputeResiduals_.Reset();
+ LinearSystemScaling_.Reset();
+ LinearSystemSymbolicFactorization_.Reset();
+ LinearSystemFactorization_.Reset();
+ LinearSystemBackSolve_.Reset();
+ LinearSystemStructureConverter_.Reset();
+ LinearSystemStructureConverterInit_.Reset();
+ QualityFunctionSearch_.Reset();
+ TryCorrector_.Reset();
+ Task1_.Reset();
+ Task2_.Reset();
+ Task3_.Reset();
+ Task4_.Reset();
+ Task5_.Reset();
+ Task6_.Reset();
+ }
+
+ void
+ TimingStatistics::PrintAllTimingStatistics(
+ Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category) const
+ {
+ if (!jnlst.ProduceOutput(level, category))
+ return;
+
+ jnlst.Printf(level, category,
+ "OverallAlgorithm....................: %10.3f\n",
+ OverallAlgorithm_.TotalTime());
+ jnlst.Printf(level, category,
+ " PrintProblemStatistics.............: %10.3f\n",
+ PrintProblemStatistics_.TotalTime());
+ jnlst.Printf(level, category,
+ " InitializeIterates.................: %10.3f\n",
+ InitializeIterates_.TotalTime());
+ jnlst.Printf(level, category,
+ " UpdateHessian......................: %10.3f\n",
+ UpdateHessian_.TotalTime());
+ jnlst.Printf(level, category,
+ " OutputIteration....................: %10.3f\n",
+ OutputIteration_.TotalTime());
+ jnlst.Printf(level, category,
+ " UpdateBarrierParameter.............: %10.3f\n",
+ UpdateBarrierParameter_.TotalTime());
+ jnlst.Printf(level, category,
+ " ComputeSearchDirection.............: %10.3f\n",
+ ComputeSearchDirection_.TotalTime());
+ jnlst.Printf(level, category,
+ " ComputeAcceptableTrialPoint........: %10.3f\n",
+ ComputeAcceptableTrialPoint_.TotalTime());
+ jnlst.Printf(level, category,
+ " AcceptTrialPoint...................: %10.3f\n",
+ AcceptTrialPoint_.TotalTime());
+ jnlst.Printf(level, category,
+ " CheckConvergence...................: %10.3f\n",
+ CheckConvergence_.TotalTime());
+
+ jnlst.Printf(level, category,
+ "PDSystemSolverTotal.................: %10.3f\n",
+ PDSystemSolverTotal_.TotalTime());
+ jnlst.Printf(level, category,
+ " PDSystemSolverSolveOnce............: %10.3f\n",
+ PDSystemSolverSolveOnce_.TotalTime());
+ jnlst.Printf(level, category,
+ " ComputeResiduals...................: %10.3f\n",
+ ComputeResiduals_.TotalTime());
+ jnlst.Printf(level, category,
+ " LinearSystemScaling................: %10.3f\n",
+ LinearSystemScaling_.TotalTime());
+ jnlst.Printf(level, category,
+ " LinearSystemSymbolicFactorization..: %10.3f\n",
+ LinearSystemSymbolicFactorization_.TotalTime());
+ jnlst.Printf(level, category,
+ " LinearSystemFactorization..........: %10.3f\n",
+ LinearSystemFactorization_.TotalTime());
+ jnlst.Printf(level, category,
+ " LinearSystemBackSolve..............: %10.3f\n",
+ LinearSystemBackSolve_.TotalTime());
+ jnlst.Printf(level, category,
+ " LinearSystemStructureConverter.....: %10.3f\n",
+ LinearSystemStructureConverter_.TotalTime());
+ jnlst.Printf(level, category,
+ " LinearSystemStructureConverterInit: %10.3f\n",
+ LinearSystemStructureConverterInit_.TotalTime());
+ jnlst.Printf(level, category,
+ "QualityFunctionSearch...............: %10.3f\n",
+ QualityFunctionSearch_.TotalTime());
+ jnlst.Printf(level, category,
+ "TryCorrector........................: %10.3f\n",
+ TryCorrector_.TotalTime());
+ jnlst.Printf(level, category,
+ "Task1...............................: %10.3f\n",
+ Task1_.TotalTime());
+ jnlst.Printf(level, category,
+ "Task2...............................: %10.3f\n",
+ Task2_.TotalTime());
+ jnlst.Printf(level, category,
+ "Task3...............................: %10.3f\n",
+ Task3_.TotalTime());
+ jnlst.Printf(level, category,
+ "Task4...............................: %10.3f\n",
+ Task4_.TotalTime());
+ jnlst.Printf(level, category,
+ "Task5...............................: %10.3f\n",
+ Task5_.TotalTime());
+ }
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpTimingStatistics.hpp b/SimTKmath/Optimizers/src/IpOpt/IpTimingStatistics.hpp
new file mode 100644
index 0000000..1d1fc85
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpTimingStatistics.hpp
@@ -0,0 +1,233 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpTimingStatistics.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Andreas Waechter IBM 2005-09-19
+
+#ifndef __IPTIMINGSTATISTICS_HPP__
+#define __IPTIMINGSTATISTICS_HPP__
+
+#include "IpReferenced.hpp"
+#include "IpJournalist.hpp"
+#include "IpTimedTask.hpp"
+
+#ifdef HAVE_CTIME
+# include <ctime>
+#else
+# ifdef HAVE_TIME_H
+# include <time.h>
+# else
+# error "don't have header file for time"
+# endif
+#endif
+
+// The following lines are copied from CoinTime.hpp
+// We should probably make some more tests here
+#if defined(_MSC_VER)
+// Turn off compiler warning about long names
+# pragma warning(disable:4786)
+#else
+// MacOS-X and FreeBSD needs sys/time.h
+# if defined(__MACH__) || defined (__FreeBSD__)
+# include <sys/time.h>
+# endif
+# if !defined(__MSVCRT__)
+# include <sys/resource.h>
+# endif
+#endif
+
+namespace Ipopt
+{
+ /** This class collects all timing statistics for Ipopt.
+ */
+ class TimingStatistics : public ReferencedObject
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Default constructor. */
+ TimingStatistics()
+ {}
+
+ /** Default destructor */
+ virtual ~TimingStatistics()
+ {}
+ //@}
+
+ /** Method for resetting all times. */
+ void ResetTimes();
+
+ /** Method for printing all timing information */
+ void PrintAllTimingStatistics(Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category) const;
+
+ /**@name Accessor methods to all timed tasks. */
+ //@{
+ TimedTask& OverallAlgorithm()
+ {
+ return OverallAlgorithm_;
+ }
+ TimedTask& PrintProblemStatistics()
+ {
+ return PrintProblemStatistics_;
+ }
+ TimedTask& InitializeIterates()
+ {
+ return InitializeIterates_;
+ }
+ TimedTask& UpdateHessian()
+ {
+ return UpdateHessian_;
+ }
+ TimedTask& OutputIteration()
+ {
+ return OutputIteration_;
+ }
+ TimedTask& UpdateBarrierParameter()
+ {
+ return UpdateBarrierParameter_;
+ }
+ TimedTask& ComputeSearchDirection()
+ {
+ return ComputeSearchDirection_;
+ }
+ TimedTask& ComputeAcceptableTrialPoint()
+ {
+ return ComputeAcceptableTrialPoint_;
+ }
+ TimedTask& AcceptTrialPoint()
+ {
+ return AcceptTrialPoint_;
+ }
+ TimedTask& CheckConvergence()
+ {
+ return CheckConvergence_;
+ }
+
+ TimedTask& PDSystemSolverTotal()
+ {
+ return PDSystemSolverTotal_;
+ }
+ TimedTask& PDSystemSolverSolveOnce()
+ {
+ return PDSystemSolverSolveOnce_;
+ }
+ TimedTask& ComputeResiduals()
+ {
+ return ComputeResiduals_;
+ }
+ TimedTask& LinearSystemScaling()
+ {
+ return LinearSystemScaling_;
+ }
+ TimedTask& LinearSystemSymbolicFactorization()
+ {
+ return LinearSystemSymbolicFactorization_;
+ }
+ TimedTask& LinearSystemFactorization()
+ {
+ return LinearSystemFactorization_;
+ }
+ TimedTask& LinearSystemBackSolve()
+ {
+ return LinearSystemBackSolve_;
+ }
+ TimedTask& LinearSystemStructureConverter()
+ {
+ return LinearSystemStructureConverter_;
+ }
+ TimedTask& LinearSystemStructureConverterInit()
+ {
+ return LinearSystemStructureConverterInit_;
+ }
+ TimedTask& QualityFunctionSearch()
+ {
+ return QualityFunctionSearch_;
+ }
+ TimedTask& TryCorrector()
+ {
+ return TryCorrector_;
+ }
+
+ TimedTask& Task1()
+ {
+ return Task1_;
+ }
+ TimedTask& Task2()
+ {
+ return Task2_;
+ }
+ TimedTask& Task3()
+ {
+ return Task3_;
+ }
+ TimedTask& Task4()
+ {
+ return Task4_;
+ }
+ TimedTask& Task5()
+ {
+ return Task5_;
+ }
+ TimedTask& Task6()
+ {
+ return Task6_;
+ }
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ TimingStatistics(const TimingStatistics&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const TimingStatistics&);
+ //@}
+
+ /**@name All timed tasks. */
+ //@{
+ TimedTask OverallAlgorithm_;
+ TimedTask PrintProblemStatistics_;
+ TimedTask InitializeIterates_;
+ TimedTask UpdateHessian_;
+ TimedTask OutputIteration_;
+ TimedTask UpdateBarrierParameter_;
+ TimedTask ComputeSearchDirection_;
+ TimedTask ComputeAcceptableTrialPoint_;
+ TimedTask AcceptTrialPoint_;
+ TimedTask CheckConvergence_;
+
+ TimedTask PDSystemSolverTotal_;
+ TimedTask PDSystemSolverSolveOnce_;
+ TimedTask ComputeResiduals_;
+ TimedTask LinearSystemScaling_;
+ TimedTask LinearSystemSymbolicFactorization_;
+ TimedTask LinearSystemFactorization_;
+ TimedTask LinearSystemBackSolve_;
+ TimedTask LinearSystemStructureConverter_;
+ TimedTask LinearSystemStructureConverterInit_;
+ TimedTask QualityFunctionSearch_;
+ TimedTask TryCorrector_;
+
+ TimedTask Task1_;
+ TimedTask Task2_;
+ TimedTask Task3_;
+ TimedTask Task4_;
+ TimedTask Task5_;
+ TimedTask Task6_;
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpTripletHelper.cpp b/SimTKmath/Optimizers/src/IpOpt/IpTripletHelper.cpp
new file mode 100644
index 0000000..2e27ced
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpTripletHelper.cpp
@@ -0,0 +1,734 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpTripletHelper.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpTripletHelper.hpp"
+
+#include "IpGenTMatrix.hpp"
+#include "IpSymTMatrix.hpp"
+#include "IpDiagMatrix.hpp"
+#include "IpIdentityMatrix.hpp"
+#include "IpExpansionMatrix.hpp"
+#include "IpScaledMatrix.hpp"
+#include "IpSymScaledMatrix.hpp"
+#include "IpSumMatrix.hpp"
+#include "IpSumSymMatrix.hpp"
+#include "IpZeroMatrix.hpp"
+#include "IpCompoundMatrix.hpp"
+#include "IpCompoundSymMatrix.hpp"
+
+#include "IpDenseVector.hpp"
+#include "IpCompoundVector.hpp"
+
+#include "IpBlas.hpp"
+
+namespace Ipopt
+{
+
+ Index TripletHelper::GetNumberEntries(const Matrix& matrix)
+ {
+ const Matrix* mptr = &matrix;
+ const GenTMatrix* gent = dynamic_cast<const GenTMatrix*>(mptr);
+ if (gent) {
+ return gent->Nonzeros();
+ }
+
+ const SymTMatrix* symt = dynamic_cast<const SymTMatrix*>(mptr);
+ if (symt) {
+ return symt->Nonzeros();
+ }
+
+ const ScaledMatrix* scaled = dynamic_cast<const ScaledMatrix*>(mptr);
+ if (scaled) {
+ return GetNumberEntries(*GetRawPtr(scaled->GetUnscaledMatrix()));
+ }
+
+ const SymScaledMatrix* symscaled = dynamic_cast<const SymScaledMatrix*>(mptr);
+ if (symscaled) {
+ return GetNumberEntries(*GetRawPtr(symscaled->GetUnscaledMatrix()));
+ }
+
+ const DiagMatrix* diag = dynamic_cast<const DiagMatrix*>(mptr);
+ if (diag) {
+ return diag->Dim();
+ }
+
+ const IdentityMatrix* ident = dynamic_cast<const IdentityMatrix*>(mptr);
+ if (ident) {
+ return ident->Dim();
+ }
+
+ const ExpansionMatrix* exp = dynamic_cast<const ExpansionMatrix*>(mptr);
+ if (exp) {
+ return exp->NCols();
+ }
+
+ const SumMatrix* sum = dynamic_cast<const SumMatrix*>(mptr);
+ if (sum) {
+ return GetNumberEntries_(*sum);
+ }
+
+ const SumSymMatrix* sumsym = dynamic_cast<const SumSymMatrix*>(mptr);
+ if (sumsym) {
+ return GetNumberEntries_(*sumsym);
+ }
+
+ const ZeroMatrix* zero = dynamic_cast<const ZeroMatrix*>(mptr);
+ if (zero) {
+ return 0;
+ }
+
+ const CompoundMatrix* cmpd = dynamic_cast<const CompoundMatrix*>(mptr);
+ if (cmpd) {
+ return GetNumberEntries_(*cmpd);
+ }
+
+ const CompoundSymMatrix* cmpd_sym = dynamic_cast<const CompoundSymMatrix*>(mptr);
+ if (cmpd_sym) {
+ return GetNumberEntries_(*cmpd_sym);
+ }
+
+ THROW_EXCEPTION(UNKNOWN_MATRIX_TYPE,"Unknown matrix type passed to TripletHelper::GetNumberEntries");
+ }
+
+ void TripletHelper::FillRowCol(Index n_entries, const Matrix& matrix, Index* iRow, Index* jCol, Index row_offset/*=0*/, Index col_offset/*=0*/)
+ {
+ const Matrix* mptr = &matrix;
+ const GenTMatrix* gent = dynamic_cast<const GenTMatrix*>(mptr);
+ if (gent) {
+ FillRowCol_(n_entries, *gent, row_offset, col_offset, iRow, jCol);
+ return;
+ }
+
+ const SymTMatrix* symt = dynamic_cast<const SymTMatrix*>(mptr);
+ if (symt) {
+ FillRowCol_(n_entries, *symt, row_offset, col_offset, iRow, jCol);
+ return;
+ }
+
+ const ScaledMatrix* scaled = dynamic_cast<const ScaledMatrix*>(mptr);
+ if (scaled) {
+ FillRowCol_(n_entries, *scaled, row_offset, col_offset, iRow, jCol);
+ return;
+ }
+
+ const SymScaledMatrix* symscaled = dynamic_cast<const SymScaledMatrix*>(mptr);
+ if (symscaled) {
+ FillRowCol_(n_entries, *symscaled, row_offset, col_offset, iRow, jCol);
+ return;
+ }
+
+ const DiagMatrix* diag = dynamic_cast<const DiagMatrix*>(mptr);
+ if (diag) {
+ FillRowCol_(n_entries, *diag, row_offset, col_offset, iRow, jCol);
+ return;
+ }
+
+ const IdentityMatrix* ident = dynamic_cast<const IdentityMatrix*>(mptr);
+ if (ident) {
+ FillRowCol_(n_entries, *ident, row_offset, col_offset, iRow, jCol);
+ return;
+ }
+
+ const ExpansionMatrix* exp = dynamic_cast<const ExpansionMatrix*>(mptr);
+ if (exp) {
+ FillRowCol_(n_entries, *exp, row_offset, col_offset, iRow, jCol);
+ return;
+ }
+
+ const SumMatrix* sum = dynamic_cast<const SumMatrix*>(mptr);
+ if (sum) {
+ FillRowCol_(n_entries, *sum, row_offset, col_offset, iRow, jCol);
+ return;
+ }
+
+ const SumSymMatrix* sumsym = dynamic_cast<const SumSymMatrix*>(mptr);
+ if (sumsym) {
+ FillRowCol_(n_entries, *sumsym, row_offset, col_offset, iRow, jCol);
+ return;
+ }
+
+ const ZeroMatrix* zero = dynamic_cast<const ZeroMatrix*>(mptr);
+ if (zero) {
+ DBG_ASSERT(n_entries == 0);
+ return;
+ }
+
+ const CompoundMatrix* cmpd = dynamic_cast<const CompoundMatrix*>(mptr);
+ if (cmpd) {
+ FillRowCol_(n_entries, *cmpd, row_offset, col_offset, iRow, jCol);
+ return;
+ }
+
+ const CompoundSymMatrix* cmpd_sym = dynamic_cast<const CompoundSymMatrix*>(mptr);
+ if (cmpd_sym) {
+ FillRowCol_(n_entries, *cmpd_sym, row_offset, col_offset, iRow, jCol);
+ return;
+ }
+
+ THROW_EXCEPTION(UNKNOWN_MATRIX_TYPE,"Unknown matrix type passed to TripletHelper::FillRowCol");
+ }
+
+ void TripletHelper::FillValues(Index n_entries, const Matrix& matrix, Number* values)
+ {
+ const Matrix* mptr = &matrix;
+ const GenTMatrix* gent = dynamic_cast<const GenTMatrix*>(mptr);
+ if (gent) {
+ FillValues_(n_entries, *gent, values);
+ return;
+ }
+
+ const SymTMatrix* symt = dynamic_cast<const SymTMatrix*>(mptr);
+ if (symt) {
+ FillValues_(n_entries, *symt, values);
+ return;
+ }
+
+ const ScaledMatrix* scaled = dynamic_cast<const ScaledMatrix*>(mptr);
+ if (scaled) {
+ FillValues_(n_entries, *scaled, values);
+ return;
+ }
+
+ const SymScaledMatrix* symscaled = dynamic_cast<const SymScaledMatrix*>(mptr);
+ if (symscaled) {
+ FillValues_(n_entries, *symscaled, values);
+ return;
+ }
+
+ const DiagMatrix* diag = dynamic_cast<const DiagMatrix*>(mptr);
+ if (diag) {
+ FillValues_(n_entries, *diag, values);
+ return;
+ }
+
+ const IdentityMatrix* ident = dynamic_cast<const IdentityMatrix*>(mptr);
+ if (ident) {
+ FillValues_(n_entries, *ident, values);
+ return;
+ }
+
+ const ExpansionMatrix* exp = dynamic_cast<const ExpansionMatrix*>(mptr);
+ if (exp) {
+ FillValues_(n_entries, *exp, values);
+ return;
+ }
+
+ const SumMatrix* sum = dynamic_cast<const SumMatrix*>(mptr);
+ if (sum) {
+ FillValues_(n_entries, *sum, values);
+ return;
+ }
+
+ const SumSymMatrix* sumsym = dynamic_cast<const SumSymMatrix*>(mptr);
+ if (sumsym) {
+ FillValues_(n_entries, *sumsym, values);
+ return;
+ }
+
+ const ZeroMatrix* zero = dynamic_cast<const ZeroMatrix*>(mptr);
+ if (zero) {
+ DBG_ASSERT(n_entries == 0);
+ return;
+ }
+
+ const CompoundMatrix* cmpd = dynamic_cast<const CompoundMatrix*>(mptr);
+ if (cmpd) {
+ FillValues_(n_entries, *cmpd, values);
+ return;
+ }
+
+ const CompoundSymMatrix* cmpd_sym = dynamic_cast<const CompoundSymMatrix*>(mptr);
+ if (cmpd_sym) {
+ FillValues_(n_entries, *cmpd_sym, values);
+ return;
+ }
+
+ THROW_EXCEPTION(UNKNOWN_MATRIX_TYPE,"Unknown matrix type passed to TripletHelper::FillValues");
+ }
+
+ Index TripletHelper::GetNumberEntries_(const SumMatrix& matrix)
+ {
+ Index n_entries = 0;
+ Index nterms = matrix.NTerms();
+ for (Index i=0; i<nterms; i++) {
+ Number dummy;
+ SmartPtr<const Matrix> i_mat;
+ matrix.GetTerm(i, dummy, i_mat);
+ n_entries += GetNumberEntries(*i_mat);
+ }
+ return n_entries;
+ }
+
+ Index TripletHelper::GetNumberEntries_(const SumSymMatrix& matrix)
+ {
+ Index n_entries = 0;
+ Index nterms = matrix.NTerms();
+ for (Index i=0; i<nterms; i++) {
+ Number dummy;
+ SmartPtr<const SymMatrix> i_mat;
+ matrix.GetTerm(i, dummy, i_mat);
+ n_entries += GetNumberEntries(*i_mat);
+ }
+ return n_entries;
+ }
+
+ Index TripletHelper::GetNumberEntries_(const CompoundMatrix& matrix)
+ {
+ Index n_entries = 0;
+ Index nrows = matrix.NComps_Rows();
+ Index ncols = matrix.NComps_Cols();
+ for (Index i=0; i<nrows; i++) {
+ for (Index j=0; j<ncols; j++) {
+ SmartPtr<const Matrix> comp = matrix.GetComp(i,j);
+ if (IsValid(comp)) {
+ n_entries += GetNumberEntries(*comp);
+ }
+ }
+ }
+ return n_entries;
+ }
+
+ Index TripletHelper::GetNumberEntries_(const CompoundSymMatrix& matrix)
+ {
+ Index n_entries = 0;
+ Index dim = matrix.NComps_Dim();
+ for (Index i=0; i<dim; i++) {
+ for (Index j=0; j<=i; j++) {
+ SmartPtr<const Matrix> comp = matrix.GetComp(i,j);
+ if (IsValid(comp)) {
+ n_entries += GetNumberEntries(*comp);
+ }
+ }
+ }
+ return n_entries;
+ }
+
+ void TripletHelper::FillRowCol_(Index n_entries, const GenTMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol)
+ {
+ DBG_ASSERT(n_entries == matrix.Nonzeros());
+ const Index* irow = matrix.Irows();
+ const Index* jcol = matrix.Jcols();
+ for (Index i=0; i<n_entries; i++) {
+ iRow[i] = irow[i] + row_offset;
+ jCol[i] = jcol[i] + col_offset;
+ }
+ }
+
+ void TripletHelper::FillValues_(Index n_entries, const GenTMatrix& matrix, Number* values)
+ {
+ DBG_ASSERT(n_entries == matrix.Nonzeros());
+ const Number* vals = matrix.Values();
+ for (Index i=0; i<n_entries; i++) {
+ values[i] = vals[i];
+ }
+ }
+
+ void TripletHelper::FillRowCol_(Index n_entries, const SymTMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol)
+ {
+ DBG_ASSERT(n_entries == matrix.Nonzeros());
+ const Index* irow = matrix.Irows();
+ const Index* jcol = matrix.Jcols();
+ for (Index i=0; i<n_entries; i++) {
+ iRow[i] = irow[i] + row_offset;
+ jCol[i] = jcol[i] + col_offset;
+ }
+ }
+
+ void TripletHelper::FillValues_(Index n_entries, const SymTMatrix& matrix, Number* values)
+ {
+ DBG_ASSERT(n_entries == matrix.Nonzeros());
+ matrix.FillValues(values);
+ }
+
+ void TripletHelper::FillRowCol_(Index n_entries, const DiagMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol)
+ {
+ DBG_ASSERT(n_entries == matrix.Dim());
+ for (Index i=0; i<n_entries; i++) {
+ iRow[i] = i + row_offset + 1;
+ jCol[i] = i + col_offset + 1;
+ }
+ }
+
+ void TripletHelper::FillValues_(Index n_entries, const DiagMatrix& matrix, Number* values)
+ {
+ DBG_ASSERT(n_entries == matrix.Dim());
+ SmartPtr<const Vector> v = matrix.GetDiag();
+ FillValuesFromVector(n_entries, *v, values);
+ // const DenseVector* vec = dynamic_cast<const DenseVector*>(GetRawPtr(v));
+ // ASSERT_EXCEPTION(vec, UNKNOWN_VECTOR_TYPE, "Unkown Vector type found in FillValues for DiagMatrix");
+ // const Number* vals = vec->Values();
+ // IpBlasDcopy(n_entries, vals, 1, values, 1);
+ }
+
+ void TripletHelper::FillRowCol_(Index n_entries, const IdentityMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol)
+ {
+ DBG_ASSERT(n_entries == matrix.Dim());
+ for (Index i=0; i<n_entries; i++) {
+ iRow[i] = i + row_offset + 1;
+ jCol[i] = i + col_offset + 1;
+ }
+ }
+
+ void TripletHelper::FillValues_(Index n_entries, const IdentityMatrix& matrix, Number* values)
+ {
+ DBG_ASSERT(n_entries == matrix.Dim());
+ Number factor = matrix.GetFactor();
+ for (Index i=0; i<n_entries; i++) {
+ values[i] = factor;
+ }
+ }
+
+ void TripletHelper::FillRowCol_(Index n_entries, const ExpansionMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol)
+ {
+ DBG_ASSERT(n_entries == matrix.NCols());
+ const Index* exp_pos = matrix.ExpandedPosIndices();
+ for (Index i=0; i<n_entries; i++) {
+ iRow[i] = exp_pos[i] + row_offset + 1;
+ jCol[i] = i + col_offset + 1;
+ }
+ }
+
+ void TripletHelper::FillValues_(Index n_entries, const ExpansionMatrix& matrix, Number* values)
+ {
+ DBG_ASSERT(n_entries == matrix.NCols());
+ for (Index i=0; i<n_entries; i++) {
+ values[i] = 1.0;
+ }
+ }
+
+ void TripletHelper::FillRowCol_(Index n_entries, const SumMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol)
+ {
+ Index total_n_entries = 0;
+ for (Index i=0; i<matrix.NTerms(); i++) {
+ // Fill the indices for the individual term
+ Number retFactor = 0.0;
+ SmartPtr<const Matrix> retTerm;
+ matrix.GetTerm(i, retFactor, retTerm);
+ Index term_n_entries = GetNumberEntries(*retTerm);
+ total_n_entries += term_n_entries;
+ FillRowCol(term_n_entries, *retTerm, iRow, jCol, row_offset, col_offset);
+
+ // now shift the iRow, jCol pointers for the next term
+ iRow += term_n_entries;
+ jCol += term_n_entries;
+ }
+ DBG_ASSERT(total_n_entries == n_entries);
+ }
+
+ void TripletHelper::FillValues_(Index n_entries, const SumMatrix& matrix, Number* values)
+ {
+ Index total_n_entries = 0;
+ for (Index i=0; i<matrix.NTerms(); i++) {
+ // Fill the values for the individual term
+ Number retFactor = 0.0;
+ SmartPtr<const Matrix> retTerm;
+ matrix.GetTerm(i, retFactor, retTerm);
+ Index term_n_entries = GetNumberEntries(*retTerm);
+ total_n_entries += term_n_entries;
+ FillValues(term_n_entries, *retTerm, values);
+
+ // Now adjust the values based on the factor
+ IpBlasDscal(term_n_entries, retFactor, values, 1);
+
+ // now shift the values pointer for the next term
+ values += term_n_entries;
+ }
+ DBG_ASSERT(total_n_entries == n_entries);
+ }
+
+ void TripletHelper::FillRowCol_(Index n_entries, const SumSymMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol)
+ {
+ Index total_n_entries = 0;
+ for (Index i=0; i<matrix.NTerms(); i++) {
+ // Fill the indices for the individual term
+ Number retFactor = 0.0;
+ SmartPtr<const SymMatrix> retTerm;
+ matrix.GetTerm(i, retFactor, retTerm);
+ Index term_n_entries = GetNumberEntries(*retTerm);
+ total_n_entries += term_n_entries;
+ FillRowCol(term_n_entries, *retTerm, iRow, jCol, row_offset, col_offset);
+
+ // now shift the iRow, jCol pointers for the next term
+ iRow += term_n_entries;
+ jCol += term_n_entries;
+ }
+ DBG_ASSERT(total_n_entries == n_entries);
+ }
+
+ void TripletHelper::FillValues_(Index n_entries, const SumSymMatrix& matrix, Number* values)
+ {
+ Index total_n_entries = 0;
+ for (Index i=0; i<matrix.NTerms(); i++) {
+ // Fill the values for the individual term
+ Number retFactor = 0.0;
+ SmartPtr<const SymMatrix> retTerm;
+ matrix.GetTerm(i, retFactor, retTerm);
+ Index term_n_entries = GetNumberEntries(*retTerm);
+ total_n_entries += term_n_entries;
+ if (retFactor!=0.0) {
+ FillValues(term_n_entries, *retTerm, values);
+
+ if (retFactor!=1.) {
+ // Now adjust the values based on the factor
+ IpBlasDscal(term_n_entries, retFactor, values, 1);
+ }
+ }
+ else {
+ const Number zero = 0.;
+ IpBlasDcopy(term_n_entries, &zero, 0, values, 1);
+ }
+
+ // now shift the values pointer for the next term
+ values += term_n_entries;
+ }
+ DBG_ASSERT(total_n_entries == n_entries);
+ }
+
+ void TripletHelper::FillRowCol_(Index n_entries, const CompoundMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol)
+ {
+ Index total_n_entries = 0;
+
+ const CompoundMatrixSpace* owner_space = dynamic_cast<const CompoundMatrixSpace*>(GetRawPtr(matrix.OwnerSpace()));
+ DBG_ASSERT(owner_space);
+
+ Index c_row_offset = row_offset;
+ for (Index i=0; i<matrix.NComps_Rows(); i++) {
+ Index c_col_offset = col_offset;
+ for (Index j=0; j<matrix.NComps_Cols(); j++) {
+ // Fill the indices for the individual term
+ SmartPtr<const Matrix> blk_mat = matrix.GetComp(i, j);
+ if (IsValid(blk_mat)) {
+ Index blk_n_entries = GetNumberEntries(*blk_mat);
+ total_n_entries += blk_n_entries;
+ FillRowCol(blk_n_entries, *blk_mat, iRow, jCol, c_row_offset, c_col_offset);
+
+ // now shift the iRow, jCol pointers for the next term
+ iRow += blk_n_entries;
+ jCol += blk_n_entries;
+ }
+ c_col_offset += owner_space->GetBlockCols(j);
+ }
+ c_row_offset += owner_space->GetBlockRows(i);
+ }
+ DBG_ASSERT(total_n_entries == n_entries);
+ }
+
+ void TripletHelper::FillValues_(Index n_entries, const CompoundMatrix& matrix, Number* values)
+ {
+ Index total_n_entries = 0;
+
+ for (Index i=0; i<matrix.NComps_Rows(); i++) {
+ for (Index j=0; j<matrix.NComps_Cols(); j++) {
+ // Fill the indices for the individual term
+ SmartPtr<const Matrix> blk_mat = matrix.GetComp(i, j);
+ if (IsValid(blk_mat)) {
+ Index blk_n_entries = GetNumberEntries(*blk_mat);
+ total_n_entries += blk_n_entries;
+ FillValues(blk_n_entries, *blk_mat, values);
+
+ // now shift the values pointer for the next term
+ values += blk_n_entries;
+ }
+ }
+ }
+ DBG_ASSERT(total_n_entries == n_entries);
+ }
+
+ void TripletHelper::FillRowCol_(Index n_entries, const CompoundSymMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol)
+ {
+ Index total_n_entries = 0;
+
+ const CompoundSymMatrixSpace* owner_space = dynamic_cast<const CompoundSymMatrixSpace*>(GetRawPtr(matrix.OwnerSpace()));
+ DBG_ASSERT(owner_space);
+
+ Index c_row_offset = row_offset;
+ for (Index i=0; i<matrix.NComps_Dim(); i++) {
+ Index c_col_offset = col_offset;
+ for (Index j=0; j<=i; j++) {
+ // Fill the indices for the individual term
+ SmartPtr<const Matrix> blk_mat = matrix.GetComp(i, j);
+ if (IsValid(blk_mat)) {
+ Index blk_n_entries = GetNumberEntries(*blk_mat);
+ total_n_entries += blk_n_entries;
+ FillRowCol(blk_n_entries, *blk_mat, iRow, jCol, c_row_offset, c_col_offset);
+
+ // now shift the iRow, jCol pointers for the next term
+ iRow += blk_n_entries;
+ jCol += blk_n_entries;
+ }
+ c_col_offset += owner_space->GetBlockDim(j);
+ }
+ c_row_offset += owner_space->GetBlockDim(i);
+ }
+ DBG_ASSERT(total_n_entries == n_entries);
+ }
+
+ void TripletHelper::FillValues_(Index n_entries, const CompoundSymMatrix& matrix, Number* values)
+ {
+ Index total_n_entries = 0;
+
+ for (Index i=0; i<matrix.NComps_Dim(); i++) {
+ for (Index j=0; j<=i; j++) {
+ // Fill the indices for the individual term
+ SmartPtr<const Matrix> blk_mat = matrix.GetComp(i, j);
+ if (IsValid(blk_mat)) {
+ Index blk_n_entries = GetNumberEntries(*blk_mat);
+ total_n_entries += blk_n_entries;
+ FillValues(blk_n_entries, *blk_mat, values);
+
+ // now shift the iRow, jCol pointers for the next term
+ values += blk_n_entries;
+ }
+ }
+ }
+ DBG_ASSERT(total_n_entries == n_entries);
+ }
+
+ void TripletHelper::FillValuesFromVector(Index dim, const Vector& vector, Number* values)
+ {
+ DBG_ASSERT(dim == vector.Dim());
+ const DenseVector* dv = dynamic_cast<const DenseVector*>(&vector);
+ if (dv) {
+ if (dv->IsHomogeneous()) {
+ Number scalar = dv->Scalar();
+ IpBlasDcopy(dim, &scalar, 0, values, 1);
+ }
+ else {
+ const Number* dv_vals = dv->Values();
+ IpBlasDcopy(dim, dv_vals, 1, values, 1);
+ }
+ return;
+ }
+
+ const CompoundVector* cv = dynamic_cast<const CompoundVector*>(&vector);
+ if (cv) {
+ Index ncomps = cv->NComps();
+ Index total_dim = 0;
+ for (Index i=0; i<ncomps; i++) {
+ SmartPtr<const Vector> comp = cv->GetComp(i);
+ Index comp_dim = comp->Dim();
+ FillValuesFromVector(comp_dim, *comp, values);
+ values += comp_dim;
+ total_dim += comp_dim;
+ }
+ DBG_ASSERT(total_dim == dim);
+ return;
+ }
+
+ THROW_EXCEPTION(UNKNOWN_VECTOR_TYPE,"Unknown vector type passed to TripletHelper::FillValues");
+ }
+
+ void TripletHelper::FillRowCol_(Index n_entries, const ScaledMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol)
+ {
+ FillRowCol(n_entries, *GetRawPtr(matrix.GetUnscaledMatrix()), iRow, jCol, row_offset, col_offset);
+ }
+
+ void TripletHelper::FillValues_(Index n_entries, const ScaledMatrix& matrix, Number* values)
+ {
+ // ToDo:
+ // This method can be made much more efficient for ScaledMatrix with GenTMatrix
+ // contained
+
+ // Get the matrix values
+ FillValues(n_entries, *GetRawPtr(matrix.GetUnscaledMatrix()), values);
+
+ // Scale the values
+ // To Do : This assumes 1-base values (like the TMatrices)
+ Index* iRow = new Index[n_entries];
+ Index* jCol = new Index[n_entries];
+ FillRowCol(n_entries, *GetRawPtr(matrix.GetUnscaledMatrix()), iRow, jCol, 0, 0);
+
+ if (IsValid(matrix.RowScaling())) {
+ Index n_rows = matrix.NRows();
+ Number* row_scaling = new Number[n_rows];
+ FillValuesFromVector(n_rows, *matrix.RowScaling(), row_scaling);
+ for (Index i=0; i<n_entries; i++) {
+ values[i] *= row_scaling[iRow[i]-1];
+ }
+ delete [] row_scaling;
+ }
+
+ if (IsValid(matrix.ColumnScaling())) {
+ Index n_cols = matrix.NCols();
+ Number* col_scaling = new Number[n_cols];
+ FillValuesFromVector(n_cols, *matrix.ColumnScaling(), col_scaling);
+ for (Index i=0; i<n_entries; i++) {
+ values[i] *= col_scaling[jCol[i]-1];
+ }
+ delete [] col_scaling;
+ }
+
+ delete [] iRow;
+ delete [] jCol;
+ }
+
+ void TripletHelper::FillRowCol_(Index n_entries, const SymScaledMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol)
+ {
+ FillRowCol(n_entries, *GetRawPtr(matrix.GetUnscaledMatrix()), iRow, jCol, row_offset, col_offset);
+ }
+
+ void TripletHelper::FillValues_(Index n_entries, const SymScaledMatrix& matrix, Number* values)
+ {
+ // ToDo:
+ // This method can be made much more efficient for ScaledMatrix with SymTMatrix
+ // contained
+
+ // Get the matrix values
+ FillValues(n_entries, *GetRawPtr(matrix.GetUnscaledMatrix()), values);
+
+ // Scale the values
+ // To Do : This assumes 1-base values (like the TMatrices)
+ Index* iRow = new Index[n_entries];
+ Index* jCol = new Index[n_entries];
+ FillRowCol(n_entries, *GetRawPtr(matrix.GetUnscaledMatrix()), iRow, jCol, 0, 0);
+
+ if (IsValid(matrix.RowColScaling())) {
+ Index n_dim = matrix.NRows();
+ Number* scaling = new Number[n_dim];
+ FillValuesFromVector(n_dim, *matrix.RowColScaling(), scaling);
+ for (Index i=0; i<n_entries; i++) {
+ values[i] *= scaling[iRow[i]-1];
+ values[i] *= scaling[jCol[i]-1];
+ }
+ delete [] scaling;
+ }
+
+ delete [] iRow;
+ delete [] jCol;
+ }
+
+ void TripletHelper::PutValuesInVector(Index dim, const Number* values, Vector& vector)
+ {
+ DBG_ASSERT(dim == vector.Dim());
+ DenseVector* dv = dynamic_cast<DenseVector*>(&vector);
+ if (dv) {
+ Number* dv_vals = dv->Values();
+ IpBlasDcopy(dim, values, 1, dv_vals, 1);
+ return;
+ }
+
+ CompoundVector* cv = dynamic_cast<CompoundVector*>(&vector);
+ if (cv) {
+ Index ncomps = cv->NComps();
+ Index total_dim = 0;
+ for (Index i=0; i<ncomps; i++) {
+ SmartPtr<Vector> comp = cv->GetCompNonConst(i);
+ Index comp_dim = comp->Dim();
+ PutValuesInVector(comp_dim, values, *comp);
+ values += comp_dim;
+ total_dim += comp_dim;
+ }
+ DBG_ASSERT(total_dim == dim);
+ return;
+ }
+
+ THROW_EXCEPTION(UNKNOWN_VECTOR_TYPE,"Unknown vector type passed to TripletHelper::PutValuesInVector");
+ }
+
+} // namespace Ipopt
+
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpTripletHelper.hpp b/SimTKmath/Optimizers/src/IpOpt/IpTripletHelper.hpp
new file mode 100644
index 0000000..f4fcdcd
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpTripletHelper.hpp
@@ -0,0 +1,118 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpTripletHelper.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPTRIPLETHELPER_HPP__
+#define __IPTRIPLETHELPER_HPP__
+
+#include "IpTypes.hpp"
+#include "IpException.hpp"
+
+namespace Ipopt
+{
+
+ /** forward declarations */
+ class Matrix;
+ class GenTMatrix;
+ class SymTMatrix;
+ class DiagMatrix;
+ class IdentityMatrix;
+ class ExpansionMatrix;
+ class ScaledMatrix;
+ class SymScaledMatrix;
+ class SumMatrix;
+ class SumSymMatrix;
+ class ZeroMatrix;
+ class CompoundMatrix;
+ class CompoundSymMatrix;
+ class Vector;
+
+ class TripletHelper
+ {
+ public:
+ /**@name A set of recursive routines that help with the Triplet format. */
+ //@{
+ /** find the total number of triplet entries of a Matrix */
+ static Index GetNumberEntries(const Matrix& matrix);
+
+ /** fill the irows, jcols structure for the triplet format from the matrix */
+ static void FillRowCol(Index n_entries, const Matrix& matrix, Index* iRow, Index* jCol, Index row_offset=0, Index col_offset=0);
+
+ /** fill the values for the triplet format from the matrix */
+ static void FillValues(Index n_entries, const Matrix& matrix, Number* values);
+
+ /** fill the values from the vector into a dense double* structure */
+ static void FillValuesFromVector(Index dim, const Vector& vector, Number* values);
+
+ /** put the values from the double* back into the vector */
+ static void PutValuesInVector(Index dim, const Number* values, Vector& vector);
+ //@}
+
+ DECLARE_STD_EXCEPTION(UNKNOWN_MATRIX_TYPE);
+ DECLARE_STD_EXCEPTION(UNKNOWN_VECTOR_TYPE);
+
+ private:
+ /** find the total number of triplet entries for the SumMatrix */
+ static Index GetNumberEntries_(const SumMatrix& matrix);
+
+ /** find the total number of triplet entries for the SumSymMatrix */
+ static Index GetNumberEntries_(const SumSymMatrix& matrix);
+
+ /** find the total number of triplet entries for the CompoundMatrix */
+ static Index GetNumberEntries_(const CompoundMatrix& matrix);
+
+ /** find the total number of triplet entries for the CompoundSymMatrix */
+ static Index GetNumberEntries_(const CompoundSymMatrix& matrix);
+
+ static void FillRowCol_(Index n_entries, const GenTMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol);
+
+ static void FillValues_(Index n_entries, const GenTMatrix& matrix, Number* values);
+
+ static void FillRowCol_(Index n_entries, const SymTMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol);
+
+ static void FillValues_(Index n_entries, const SymTMatrix& matrix, Number* values);
+
+ static void FillRowCol_(Index n_entries, const DiagMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol);
+
+ static void FillValues_(Index n_entries, const DiagMatrix& matrix, Number* values);
+
+ static void FillRowCol_(Index n_entries, const IdentityMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol);
+
+ static void FillValues_(Index n_entries, const IdentityMatrix& matrix, Number* values);
+
+ static void FillRowCol_(Index n_entries, const ExpansionMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol);
+
+ static void FillValues_(Index n_entries, const ExpansionMatrix& matrix, Number* values);
+
+ static void FillRowCol_(Index n_entries, const SumMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol);
+
+ static void FillValues_(Index n_entries, const SumMatrix& matrix, Number* values);
+
+ static void FillRowCol_(Index n_entries, const SumSymMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol);
+
+ static void FillValues_(Index n_entries, const SumSymMatrix& matrix, Number* values);
+
+ static void FillRowCol_(Index n_entries, const CompoundMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol);
+
+ static void FillValues_(Index n_entries, const CompoundMatrix& matrix, Number* values);
+
+ static void FillRowCol_(Index n_entries, const CompoundSymMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol);
+
+ static void FillValues_(Index n_entries, const CompoundSymMatrix& matrix, Number* values);
+
+ static void FillRowCol_(Index n_entries, const ScaledMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol);
+
+ static void FillValues_(Index n_entries, const ScaledMatrix& matrix, Number* values);
+
+ static void FillRowCol_(Index n_entries, const SymScaledMatrix& matrix, Index row_offset, Index col_offset, Index* iRow, Index* jCol);
+
+ static void FillValues_(Index n_entries, const SymScaledMatrix& matrix, Number* values);
+
+ };
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpTripletToCSRConverter.cpp b/SimTKmath/Optimizers/src/IpOpt/IpTripletToCSRConverter.cpp
new file mode 100644
index 0000000..792955c
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpTripletToCSRConverter.cpp
@@ -0,0 +1,211 @@
+
+#include "IpTripletToCSRConverter.hpp"
+#include <list>
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ TripletToCSRConverter::TripletToCSRConverter(Index offset)
+ :
+ offset_(offset),
+ ia_(NULL),
+ ja_(NULL),
+ dim_(0),
+ nonzeros_triplet_(0),
+ nonzeros_compressed_(0),
+ initialized_(false),
+ ipos_first_(NULL),
+ ipos_double_triplet_(NULL),
+ ipos_double_compressed_(NULL)
+ {
+ DBG_ASSERT(offset==0|| offset==1);
+ }
+
+ TripletToCSRConverter::~TripletToCSRConverter()
+ {
+ delete[] ia_;
+ delete[] ja_;
+ delete[] ipos_first_;
+ delete[] ipos_double_triplet_;
+ delete[] ipos_double_compressed_;
+ }
+
+ Index TripletToCSRConverter::InitializeConverter(Index dim, Index nonzeros,
+ const Index* airn,
+ const Index* ajcn)
+ {
+ DBG_START_METH("TSymLinearSolver::InitializeStructure",
+ dbg_verbosity);
+
+ DBG_ASSERT(dim>0);
+ DBG_ASSERT(nonzeros>0);
+
+ delete[] ia_;
+ delete[] ja_;
+ delete[] ipos_first_;
+ delete[] ipos_double_triplet_;
+ delete[] ipos_double_compressed_;
+
+ dim_ = dim;
+ nonzeros_triplet_ = nonzeros;
+
+ // Create a list with all triplet entries
+ std::list<TripletEntry> entry_list(nonzeros);
+ std::list<TripletEntry>::iterator list_iterator = entry_list.begin();
+ for (Index i=0; i<nonzeros; i++) {
+ list_iterator->Set(airn[i], ajcn[i], i);
+ list_iterator++;
+ }
+ DBG_ASSERT(list_iterator == entry_list.end());
+
+ if (DBG_VERBOSITY()>=2) {
+ for (Index i=0; i<nonzeros; i++) {
+ DBG_PRINT((2, "airn[%5d] = %5d acjn[%5d] = %5d\n", i, airn[i], i, ajcn[i]));
+ }
+ }
+
+ // sort the list
+ entry_list.sort();
+
+ // Now got through the list and compute ipos_ arrays and the
+ // number of elements in the compressed format
+ Index* ja_tmp = new Index[nonzeros]; // overestimate memory requirement
+ ia_ = new Index[dim_+1];
+ Index* ipos_first_tmp = new Index[nonzeros]; // overestimate memory requirement
+ Index* ipos_double_triplet_tmp = new Index[nonzeros]; // overestimate memory requirement
+ Index* ipos_double_compressed_tmp = new Index[nonzeros]; // overestimate memory requirement
+
+ nonzeros_compressed_ = 0;
+ Index cur_row = 1;
+
+ // The first element must be the first diagonal element
+ list_iterator = entry_list.begin();
+ DBG_ASSERT(list_iterator->IRow()==1);
+ DBG_ASSERT(list_iterator->JCol()==1);
+ ia_[0] = 0;
+ ja_tmp[0] = 1;
+ ipos_first_tmp[0] = list_iterator->PosTriplet();
+
+ list_iterator++;
+ Index idouble = 0;
+ while (list_iterator != entry_list.end()) {
+ Index irow = list_iterator->IRow();
+ Index jcol = list_iterator->JCol();
+ if (cur_row == irow && ja_tmp[nonzeros_compressed_] == jcol) {
+ // This element appears repeatedly, add to the double list
+ ipos_double_triplet_tmp[idouble] = list_iterator->PosTriplet();
+ ipos_double_compressed_tmp[idouble] = nonzeros_compressed_;
+ idouble++;
+ }
+ else {
+ // This is a new element
+ nonzeros_compressed_++;
+ ja_tmp[nonzeros_compressed_] = jcol;
+ ipos_first_tmp[nonzeros_compressed_] = list_iterator->PosTriplet();
+ if (cur_row != irow) {
+ // this is in a new row
+
+ // make sure that the diagonal element is given and that no
+ // row is omitted
+ DBG_ASSERT(irow==jcol);
+ DBG_ASSERT(cur_row+1==irow);
+ ia_[cur_row] = nonzeros_compressed_;
+ cur_row++;
+ }
+ }
+
+ list_iterator++;
+ }
+ nonzeros_compressed_++;
+ ia_[dim_] = nonzeros_compressed_;
+
+ DBG_ASSERT(cur_row == dim_);
+ DBG_ASSERT(idouble == nonzeros_triplet_-nonzeros_compressed_);
+
+ // Now copy the ja_tmp array to the (shorter) final one and make
+ // sure that the correct offset is used
+ ja_ = new Index[nonzeros_compressed_];
+ if (offset_==0) {
+ for (Index i=0; i<nonzeros_compressed_; i++) {
+ ja_[i] = ja_tmp[i] - 1;
+ }
+ }
+ else {
+ for (Index i=0; i<nonzeros_compressed_; i++) {
+ ja_[i] = ja_tmp[i];
+ }
+ for (Index i=0; i<=dim_; i++) {
+ ia_[i] = ia_[i] + 1;
+ }
+ }
+ delete[] ja_tmp;
+
+ // Reallocate memory for the "first" array
+ ipos_first_ = new Index[nonzeros_compressed_];
+ for (Index i=0; i<nonzeros_compressed_; i++) {
+ ipos_first_[i] = ipos_first_tmp[i];
+ }
+ delete[] ipos_first_tmp;
+
+ // Reallocate memory for the "double" arrays
+ ipos_double_triplet_ = new Index[idouble];
+ ipos_double_compressed_ = new Index[idouble];
+ for (Index i=0; i<idouble; i++) {
+ ipos_double_triplet_[i] = ipos_double_triplet_tmp[i];
+ ipos_double_compressed_[i] = ipos_double_compressed_tmp[i];
+ }
+ delete[] ipos_double_triplet_tmp;
+ delete[] ipos_double_compressed_tmp;
+
+ initialized_ = true;
+
+ if (DBG_VERBOSITY()>=2) {
+ for (Index i=0; i<=dim_; i++) {
+ DBG_PRINT((2, "ia[%5d] = %5d\n", i, ia_[i]));
+ }
+ for (Index i=0; i<nonzeros_compressed_; i++) {
+ DBG_PRINT((2, "ja[%5d] = %5d ipos_first[%5d] = %5d\n", i, ja_[i], i, ipos_first_[i]));
+ }
+ for (Index i=0; i<nonzeros_triplet_-nonzeros_compressed_; i++) {
+ DBG_PRINT((2, "ipos_double_triplet[%5d] = %5d ipos_double_compressed[%5d] = %5d\n", i, ipos_double_triplet_[i], i, ipos_double_compressed_[i]));
+ }
+ }
+
+ return nonzeros_compressed_;
+ }
+
+ void TripletToCSRConverter::ConvertValues(Index nonzeros_triplet,
+ const Number* a_triplet,
+ Index nonzeros_compressed,
+ Number* a_compressed)
+ {
+ DBG_START_METH("TSymLinearSolver::ConvertValues",
+ dbg_verbosity);
+
+ DBG_ASSERT(initialized_);
+
+ DBG_ASSERT(nonzeros_triplet_==nonzeros_triplet);
+ DBG_ASSERT(nonzeros_compressed_==nonzeros_compressed);
+
+ for (Index i=0; i<nonzeros_compressed_; i++) {
+ a_compressed[i] = a_triplet[ipos_first_[i]];
+ }
+ for (Index i=0; i<nonzeros_triplet_-nonzeros_compressed_; i++) {
+ a_compressed[ipos_double_compressed_[i]] +=
+ a_triplet[ipos_double_triplet_[i]];
+ }
+
+ if (DBG_VERBOSITY()>=2) {
+ for (Index i=0; i<nonzeros_triplet; i++) {
+ DBG_PRINT((2, "atriplet[%5d] = %24.16e\n", i, a_triplet[i]));
+ }
+ for (Index i=0; i<nonzeros_compressed; i++) {
+ DBG_PRINT((2, "acompre[%5d] = %24.16e\n", i, a_compressed[i]));
+ }
+ }
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpTripletToCSRConverter.hpp b/SimTKmath/Optimizers/src/IpOpt/IpTripletToCSRConverter.hpp
new file mode 100644
index 0000000..d71d5c6
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpTripletToCSRConverter.hpp
@@ -0,0 +1,226 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpTripletToCSRConverter.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-03-13
+
+#ifndef __IPTRIPLETTOCSRCONVERTER_HPP__
+#define __IPTRIPLETTOCSRCONVERTER_HPP__
+
+#include "IpUtils.hpp"
+#include "IpReferenced.hpp"
+namespace Ipopt
+{
+
+ /** Class for converting symmetric matrices given in triplet format
+ * to matrices in compressed sparse row (CSR) format of the upper
+ * triangual part (or, equivalently, compressed sparse column (CSC)
+ * format for the lower triangular part). In the description for
+ * this class, we assume that we discuss the CSR format.
+ */
+ class TripletToCSRConverter: public ReferencedObject
+ {
+ /** Class for one triplet position entry. */
+ class TripletEntry
+ {
+ public:
+ /** @name Constructor/Destructor */
+ //@{
+ /** Constructor. */
+ TripletEntry()
+ {}
+
+ /** Destructor */
+ ~TripletEntry()
+ {}
+
+ /** Dummy copy constructor. Note that nothing is really copied!
+ * This is just implemented to that the std::list can be
+ * created with uninitialized entries. The values are
+ * afterwards set with the Set method. */
+ TripletEntry(const TripletEntry&)
+ {}
+ //@}
+
+ /** Set the values of an entry */
+ void Set(Index i_row, Index j_col, Index i_pos_triplet)
+ {
+ if (i_row>j_col) {
+ i_row_ = j_col;
+ j_col_ = i_row;
+ }
+ else {
+ i_row_ = i_row;
+ j_col_ = j_col;
+ }
+ i_pos_triplet_ = i_pos_triplet;
+ }
+
+ /** @name Accessor methods. */
+ //@{
+ /** Row position. */
+ Index IRow() const
+ {
+ return i_row_;
+ }
+ /** Column position. */
+ Index JCol() const
+ {
+ return j_col_;
+ }
+ /** Index in original triplet matrix. */
+ Index PosTriplet() const
+ {
+ return i_pos_triplet_;
+ }
+ //@}
+
+ /** Comparison operator. This is required for the sort function. */
+ bool operator< (const TripletEntry& Tentry) const
+ {
+ return ((i_row_ < Tentry.i_row_) ||
+ (i_row_==Tentry.i_row_ && j_col_<Tentry.j_col_));
+ }
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ //TripletEntry();
+
+ /** Copy Constructor */
+ /*
+ TripletEntry(const TripletEntry&);
+ */
+
+ /** Overloaded Equals Operator */
+ void operator=(const TripletEntry&);
+ //@}
+
+ /** @name Entry content. */
+ //@{
+ Index i_row_;
+ Index j_col_;
+ Index i_pos_triplet_;
+ //@}
+ };
+
+ public:
+ /** @name Constructor/Destructor */
+ //@{
+ /* Constructor. If offset is 0, then the counting of indices in
+ the compressed format starts a 0 (C-style numbering); if offset
+ is 1, then the counting starts at 1 (Fortran-type
+ numbering). */
+ TripletToCSRConverter(Index offset);
+
+ /** Destructor */
+ virtual ~TripletToCSRConverter();
+ //@}
+
+ /** Initialize the converter, given the fixed structure of the
+ * matrix. There, ndim gives the number of rows and columns of
+ * the matrix, nonzeros give the number of nonzero elements, and
+ * airn and acjn give the positions of the nonzero elements. The
+ * return value is the number of nonzeros in the condensed
+ * matrix. (Since nonzero elements can be listed several times
+ * in the triplet format, it is possible that this value is
+ * different from the input value nonzeros.) This method must be
+ * called before the GetIA, GetJA, GetValues methods are called.
+ */
+ Index InitializeConverter(Index dim, Index nonzeros,
+ const Index* airn,
+ const Index* ajcn);
+
+ /** Return the IA array for the condensed format. */
+ const Index* IA() const
+ {
+ DBG_ASSERT(initialized_);
+ return ia_;
+ }
+
+ /** Return the JA array for the condensed format. */
+ const Index* JA() const
+ {
+ DBG_ASSERT(initialized_);
+ return ja_;
+ }
+
+ /** Convert the values of the nonzero elements. Given the values
+ * a_triplet for the triplet format, return the array of values
+ * for the condensed format in a_condensed. nonzeros_condensed is
+ * the length of the array a_condensed and must be identical to
+ * the return value of InitializeConverter. */
+ void ConvertValues(Index nonzeros_triplet, const Number* a_triplet,
+ Index nonzeros_compressed, Number* a_compressed);
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ TripletToCSRConverter();
+
+ /** Copy Constructor */
+ TripletToCSRConverter(const TripletToCSRConverter&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const TripletToCSRConverter&);
+ //@}
+
+ /** Offset for CSR numbering. */
+ Index offset_;
+
+ /** Array storing the values for IA in the condensed format */
+ Index* ia_;
+
+ /** Array storing the values for JA in the condensed format */
+ Index* ja_;
+
+ /** Dimension of the matrix. */
+ Index dim_;
+
+ /** Number of nonzeros in the triplet format. */
+ Index nonzeros_triplet_;
+
+ /** Number of nonzeros in the compressed format. */
+ Index nonzeros_compressed_;
+
+ /** Flag indicating if initialize method had been called. */
+ bool initialized_;
+
+ /** @name Arrays for cross-positions for the conversion of values. */
+ //@{
+ /** First elements assignement. For i with 0 <= i <=
+ * nonzeros_compressed-1, the i-th element in the compressed
+ * format is obtained from copying the ipos_filter_[i]-th element
+ * from the triplet format. */
+ Index* ipos_first_;
+ /** Position of multiple elements in triplet matrix. For i =
+ * 0,..,nonzeros_triplet_-nonzeros_compressed_, the
+ * ipos_double_triplet_[i]-th element in the triplet matrix has
+ * to be added to the ipos_double_compressed_[i]-th element in
+ * the compressed matrix. */
+ Index* ipos_double_triplet_;
+ /** Position of multiple elements in compressed matrix. */
+ Index* ipos_double_compressed_;
+ //@}
+ };
+
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpTripletToDenseConverter.cpp b/SimTKmath/Optimizers/src/IpOpt/IpTripletToDenseConverter.cpp
new file mode 100644
index 0000000..fd9418e
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpTripletToDenseConverter.cpp
@@ -0,0 +1,110 @@
+
+#include "IpTripletToDenseConverter.hpp"
+#include <list>
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ TripletToDenseConverter::TripletToDenseConverter(Index offset)
+ :
+ offset_(offset),
+ ia_(NULL),
+ ja_(NULL),
+ dim_(0),
+ nonzeros_triplet_(0),
+ nonzeros_compressed_(0),
+ initialized_(false),
+ ipos_first_(NULL),
+ ipos_double_triplet_(NULL),
+ ipos_double_compressed_(NULL)
+ {
+ DBG_ASSERT(offset==0|| offset==1);
+ }
+
+ TripletToDenseConverter::~TripletToDenseConverter()
+ {
+ delete[] ia_;
+ delete[] ja_;
+ delete[] ipos_first_;
+ delete[] ipos_double_triplet_;
+ delete[] ipos_double_compressed_;
+ }
+
+ Index TripletToDenseConverter::InitializeConverter(Index dim, Index nonzeros,
+ const Index* airn,
+ const Index* ajcn)
+ {
+ int i;
+ DBG_START_METH("TripletToDenseConverter::InitializeConverter",
+ dbg_verbosity);
+
+ DBG_ASSERT(dim>0);
+ DBG_ASSERT(nonzeros>0);
+
+//printf("TripletToDenseConverter::InitializeConverter");
+ ia_ = new Index[nonzeros];
+ ja_ = new Index[nonzeros];
+
+//printf("TSymLinearSolver::InitializeStructure indexes =\n");
+ for(i=0;i<nonzeros;i++ ) {
+ ia_[i] = airn[i] - 1;
+ ja_[i] = ajcn[i] - 1;
+// printf("%d %d \n",ia_[i],ja_[i]);
+ }
+
+ return nonzeros;
+ }
+
+ void TripletToDenseConverter::ConvertValues(Index nonzeros_triplet,
+ const Number* a_triplet,
+ Index dim,
+ Number* a)
+ {
+ int i,j;
+ DBG_START_METH("TSymLinearSolver::ConvertValues",
+ dbg_verbosity);
+//printf(" TripletToDenseConverter:::ConvertValues\n");
+
+ DBG_ASSERT(initialized_);
+
+ DBG_ASSERT(nonzeros_triplet_==nonzeros_triplet);
+ DBG_ASSERT(nonzeros_compressed_==nonzeros_compressed);
+
+ /* initialize matrix to all zeros */
+ for (i=0;i<dim;i++ ) {
+ for (j=0;j<dim;j++ ) {
+ a[i*dim+j] = 0.0;
+ }
+ }
+/*
+printf("TripletToDenseConverter::ConvertValues input = \n");
+ for (Index i=0; i<nonzeros_triplet; i++) {
+ printf(" %d %d %f \n",ia_[i],ja_[i],a_triplet[i]);
+}
+*/
+
+ /* fill in non zeros assuming symmetric input matrix */
+ for (Index i=0; i<nonzeros_triplet; i++) {
+// printf("dim = %d value = %f index = %d %d \n",dim, a_triplet[i], ia_[i]*dim+ja_[i], ja_[i]*dim+ia_[i]);
+ a[ja_[i]*dim+ia_[i]] += a_triplet[i]; // lower triangle
+ if(ia_[i] != ja_[i] ) { // only fill in off diagonal elements
+ a[ia_[i]*dim+ja_[i]] += a_triplet[i]; // upper triangle
+ }
+ }
+/*
+printf("TripletToDenseConverter::ConvertValues a matrix : \n");
+ for (i=0;i<dim;i++ ) {
+ for (j=0;j<dim;j++ ) {
+ printf("%f ",a[i*dim+j] );
+ }
+ printf("\n");
+ }
+*/
+
+
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpTripletToDenseConverter.hpp b/SimTKmath/Optimizers/src/IpOpt/IpTripletToDenseConverter.hpp
new file mode 100644
index 0000000..2645628
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpTripletToDenseConverter.hpp
@@ -0,0 +1,216 @@
+
+#ifndef __IPTRIPLETTODENSECONVERTER_HPP__
+#define __IPTRIPLETTODENSECONVERTER_HPP__
+
+#include "IpUtils.hpp"
+#include "IpReferenced.hpp"
+namespace Ipopt
+{
+
+ /** Class for converting symmetric matrices given in triplet format
+ * to matrices in dense format
+ */
+ class TripletToDenseConverter: public ReferencedObject
+ {
+ /** Class for one triplet position entry. */
+ class TripletEntry
+ {
+ public:
+ /** @name Constructor/Destructor */
+ //@{
+ /** Constructor. */
+ TripletEntry()
+ {}
+
+ /** Destructor */
+ ~TripletEntry()
+ {}
+
+ /** Dummy copy constructor. Note that nothing is really copied!
+ * This is just implemented to that the std::list can be
+ * created with uninitialized entries. The values are
+ * afterwards set with the Set method. */
+ TripletEntry(const TripletEntry&)
+ {}
+ //@}
+
+ /** Set the values of an entry */
+ void Set(Index i_row, Index j_col, Index i_pos_triplet)
+ {
+ if (i_row>j_col) {
+ i_row_ = j_col;
+ j_col_ = i_row;
+ }
+ else {
+ i_row_ = i_row;
+ j_col_ = j_col;
+ }
+ i_pos_triplet_ = i_pos_triplet;
+ }
+
+ /** @name Accessor methods. */
+ //@{
+ /** Row position. */
+ Index IRow() const
+ {
+ return i_row_;
+ }
+ /** Column position. */
+ Index JCol() const
+ {
+ return j_col_;
+ }
+ /** Index in original triplet matrix. */
+ Index PosTriplet() const
+ {
+ return i_pos_triplet_;
+ }
+ //@}
+
+ /** Comparison operator. This is required for the sort function. */
+ bool operator< (const TripletEntry& Tentry) const
+ {
+ return ((i_row_ < Tentry.i_row_) ||
+ (i_row_==Tentry.i_row_ && j_col_<Tentry.j_col_));
+ }
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ //TripletEntry();
+
+ /** Copy Constructor */
+ /*
+ TripletEntry(const TripletEntry&);
+ */
+
+ /** Overloaded Equals Operator */
+ void operator=(const TripletEntry&);
+ //@}
+
+ /** @name Entry content. */
+ //@{
+ Index i_row_;
+ Index j_col_;
+ Index i_pos_triplet_;
+ //@}
+ };
+
+ public:
+ /** @name Constructor/Destructor */
+ //@{
+ /* Constructor. If offset is 0, then the counting of indices in
+ the compressed format starts a 0 (C-style numbering); if offset
+ is 1, then the counting starts at 1 (Fortran-type
+ numbering). */
+ TripletToDenseConverter(Index offset);
+
+ /** Destructor */
+ virtual ~TripletToDenseConverter();
+ //@}
+
+ /** Initialize the converter, given the fixed structure of the
+ * matrix. There, ndim gives the number of rows and columns of
+ * the matrix, nonzeros give the number of nonzero elements, and
+ * airn and acjn give the positions of the nonzero elements. The
+ * return value is the number of nonzeros in the condensed
+ * matrix. (Since nonzero elements can be listed several times
+ * in the triplet format, it is possible that this value is
+ * different from the input value nonzeros.) This method must be
+ * called before the GetIA, GetJA, GetValues methods are called.
+ */
+ Index InitializeConverter(Index dim, Index nonzeros,
+ const Index* airn,
+ const Index* ajcn);
+
+ /** Return the IA array for the condensed format. */
+ const Index* IA() const
+ {
+ DBG_ASSERT(initialized_);
+ return ia_;
+ }
+
+ /** Return the JA array for the condensed format. */
+ const Index* JA() const
+ {
+ DBG_ASSERT(initialized_);
+ return ja_;
+ }
+
+ /** Convert the values of the nonzero elements. Given the values
+ * a_triplet for the triplet format, return the array of values
+ * for the condensed format in a_condensed. nonzeros_condensed is
+ * the length of the array a_condensed and must be identical to
+ * the return value of InitializeConverter. */
+ void ConvertValues(Index nonzeros_triplet, const Number* a_triplet,
+ Index nonzeros_compressed, Number* a_compressed);
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ TripletToDenseConverter();
+
+ /** Copy Constructor */
+ TripletToDenseConverter(const TripletToDenseConverter&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const TripletToDenseConverter&);
+ //@}
+
+ /** Offset for Dense numbering. */
+ Index offset_;
+
+ /** Array storing the values for IA in the condensed format */
+ Index* ia_;
+
+ /** Array storing the values for JA in the condensed format */
+ Index* ja_;
+
+ /** Dimension of the matrix. */
+ Index dim_;
+
+ /** Number of nonzeros in the triplet format. */
+ Index nonzeros_triplet_;
+
+ /** Number of nonzeros in the compressed format. */
+ Index nonzeros_compressed_;
+
+ /** Flag indicating if initialize method had been called. */
+ bool initialized_;
+
+ /** @name Arrays for cross-positions for the conversion of values. */
+ //@{
+ /** First elements assignement. For i with 0 <= i <=
+ * nonzeros_compressed-1, the i-th element in the compressed
+ * format is obtained from copying the ipos_filter_[i]-th element
+ * from the triplet format. */
+ Index* ipos_first_;
+ /** Position of multiple elements in triplet matrix. For i =
+ * 0,..,nonzeros_triplet_-nonzeros_compressed_, the
+ * ipos_double_triplet_[i]-th element in the triplet matrix has
+ * to be added to the ipos_double_compressed_[i]-th element in
+ * the compressed matrix. */
+ Index* ipos_double_triplet_;
+ /** Position of multiple elements in compressed matrix. */
+ Index* ipos_double_compressed_;
+ //@}
+ };
+
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpTypes.hpp b/SimTKmath/Optimizers/src/IpOpt/IpTypes.hpp
new file mode 100644
index 0000000..d9cf61b
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpTypes.hpp
@@ -0,0 +1,25 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpTypes.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPTYPES_HPP__
+#define __IPTYPES_HPP__
+
+#include "SimTKcommon/internal/common.h"
+
+namespace Ipopt
+{
+ /** Type of all numbers */
+ typedef SimTK::Real Number;
+ /** Type of all indices of vectors, matrices etc */
+ typedef int Index;
+ /** Type of default integer */
+ typedef int Int;
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpUserScaling.cpp b/SimTKmath/Optimizers/src/IpOpt/IpUserScaling.cpp
new file mode 100644
index 0000000..e98288d
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpUserScaling.cpp
@@ -0,0 +1,30 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpUserScaling.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-06-25
+
+#include "IpUserScaling.hpp"
+
+namespace Ipopt
+{
+
+ void UserScaling::DetermineScalingParametersImpl(
+ const SmartPtr<const VectorSpace> x_space,
+ const SmartPtr<const VectorSpace> c_space,
+ const SmartPtr<const VectorSpace> d_space,
+ const SmartPtr<const MatrixSpace> jac_c_space,
+ const SmartPtr<const MatrixSpace> jac_d_space,
+ const SmartPtr<const SymMatrixSpace> h_space,
+ Number& df,
+ SmartPtr<Vector>& dx,
+ SmartPtr<Vector>& dc,
+ SmartPtr<Vector>& dd)
+ {
+ DBG_ASSERT(IsValid(nlp_));
+ nlp_->GetScalingParameters(x_space, c_space, d_space, df, dx, dc, dd);
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpUserScaling.hpp b/SimTKmath/Optimizers/src/IpOpt/IpUserScaling.hpp
new file mode 100644
index 0000000..5440233
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpUserScaling.hpp
@@ -0,0 +1,71 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpUserScaling.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-06-25
+
+#ifndef __IPUSERSCALING_HPP__
+#define __IPUSERSCALING_HPP__
+
+#include "IpNLPScaling.hpp"
+#include "IpNLP.hpp"
+
+namespace Ipopt
+{
+ /** This class does problem scaling by getting scaling parameters
+ * from the user (through the NLP interface).
+ */
+ class UserScaling : public StandardScalingBase
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ UserScaling(const SmartPtr<const NLP>& nlp)
+ :
+ StandardScalingBase(),
+ nlp_(nlp)
+ {}
+
+ /** Default destructor */
+ virtual ~UserScaling()
+ {}
+ //@}
+
+ protected:
+ virtual void DetermineScalingParametersImpl(
+ const SmartPtr<const VectorSpace> x_space,
+ const SmartPtr<const VectorSpace> c_space,
+ const SmartPtr<const VectorSpace> d_space,
+ const SmartPtr<const MatrixSpace> jac_c_space,
+ const SmartPtr<const MatrixSpace> jac_d_space,
+ const SmartPtr<const SymMatrixSpace> h_space,
+ Number& df,
+ SmartPtr<Vector>& dx,
+ SmartPtr<Vector>& dc,
+ SmartPtr<Vector>& dd);
+
+ private:
+
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+
+ /** Copy Constructor */
+ UserScaling(const UserScaling&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const UserScaling&);
+ //@}
+
+ /** pointer to the NLP to get scaling parameters */
+ SmartPtr<const NLP> nlp_;
+ };
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpUtils.cpp b/SimTKmath/Optimizers/src/IpOpt/IpUtils.cpp
new file mode 100644
index 0000000..277affc
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpUtils.cpp
@@ -0,0 +1,52 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpUtils.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-08-12
+
+#include "IpoptConfig.h"
+#include "IpUtils.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+#ifdef HAVE_CFLOAT
+# include <cfloat>
+#else
+# ifdef HAVE_FLOAT_H
+# include <float.h>
+# endif
+#endif
+
+#ifdef HAVE_CIEEEFP
+# include <cieeefp>
+#else
+# ifdef HAVE_IEEEFP_H
+# include <ieeefp.h>
+# endif
+#endif
+
+namespace Ipopt
+{
+
+ bool IsFiniteNumber(Number val)
+ {
+#ifdef MY_C_FINITE
+ return (bool)MY_C_FINITE(val);
+#else
+
+ return true;
+#endif
+
+ }
+
+} //namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpUtils.hpp b/SimTKmath/Optimizers/src/IpOpt/IpUtils.hpp
new file mode 100644
index 0000000..056a089
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpUtils.hpp
@@ -0,0 +1,73 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpUtils.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPUTILS_HPP__
+#define __IPUTILS_HPP__
+
+// Standard Ip Include Files
+#include "IpTypes.hpp"
+#include "IpDebug.hpp"
+
+namespace Ipopt
+{
+
+ inline ipfint Max(ipfint a, ipfint b)
+ {
+ return ((a) > (b) ? (a) : (b));
+ }
+
+ inline ipfint Min(ipfint a, ipfint b)
+ {
+ return ((a) < (b) ? (a) : (b));
+ }
+
+ inline Number Max(Number a, Number b)
+ {
+ return ((a) > (b) ? (a) : (b));
+ }
+
+ inline Number Max(Number a, Number b, Number c)
+ {
+ Number max = Max(a,b);
+ max = Max(max, c);
+ return max;
+ }
+
+ inline Number Max(Number a, Number b, Number c, Number d)
+ {
+ Number max = Max(a, b, c);
+ max = Max(max, d);
+ return max;
+ }
+
+ inline Number Min(Number a, Number b)
+ {
+ return ((a) < (b) ? (a) : (b));
+ }
+
+ inline Number Min(Number a, Number b, Number c)
+ {
+ Number min = Min(a,b);
+ min = Min(min, c);
+ return min;
+ }
+
+ inline Number Min(Number a, Number b, Number c, Number d)
+ {
+ Number min = Min(a, b, c);
+ min = Min(min, d);
+ return min;
+ }
+
+ /** Function returning true iff the argument is a valid double number
+ * (not NaN or Inf). */
+ bool IsFiniteNumber(Number val);
+
+} //namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpVector.cpp b/SimTKmath/Optimizers/src/IpOpt/IpVector.cpp
new file mode 100644
index 0000000..6dfbb1b
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpVector.cpp
@@ -0,0 +1,197 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpVector.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpVector.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+namespace Ipopt
+{
+
+ void Vector::Print(SmartPtr<const Journalist> jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ if (IsValid(jnlst) && jnlst->ProduceOutput(level, category)) {
+ PrintImpl(*jnlst, level, category, name, indent, prefix);
+ }
+ }
+
+ void Vector::Print(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ if (jnlst.ProduceOutput(level, category)) {
+ PrintImpl(jnlst, level, category, name, indent, prefix);
+ }
+ }
+
+ /* Prototype implementation for specialized functions */
+ void Vector::AddTwoVectorsImpl(Number a, const Vector& v1,
+ Number b, const Vector& v2, Number c)
+ {
+ if (c==0.) {
+ if (a==1.) {
+ Copy(v1);
+ if (b!=0.) {
+ Axpy(b, v2);
+ }
+ }
+ else if (a==0.) {
+ if (b==0.) {
+ Set(0.);
+ }
+ else {
+ Copy(v2);
+ if (b!=1.) {
+ Scal(b);
+ }
+ }
+ }
+ else {
+ if (b==1.) {
+ Copy(v2);
+ Axpy(a, v1);
+ }
+ else if (b==0.) {
+ Copy(v1);
+ Scal(a);
+ }
+ else {
+ Copy(v1);
+ Scal(a);
+ Axpy(b, v2);
+ }
+ }
+ }
+ else { /* c==0. */
+ if (c!=1.) {
+ Scal(c);
+ }
+ if (a!=0.) {
+ Axpy(a, v1);
+ }
+ if (b!=0.) {
+ Axpy(b, v2);
+ }
+ }
+ }
+
+ Number Vector::FracToBoundImpl(const Vector& delta, Number tau) const
+ {
+ DBG_ASSERT(tau>=0.);
+ DBG_ASSERT(Dim() == delta.Dim());
+ if (Dim() == 0 && delta.Dim() == 0) {
+ return 1;
+ }
+
+ SmartPtr<Vector> inv_alpha_bar = MakeNew();
+ inv_alpha_bar->AddOneVector(-1/tau, delta, Number(0));
+ inv_alpha_bar->ElementWiseDivide(*this);
+
+ Number alpha = inv_alpha_bar->Max();
+ if (alpha > 0) {
+ alpha = Ipopt::Min(1/alpha, Number(1));
+ }
+ else {
+ alpha = 1;
+ }
+
+ return alpha;
+ }
+
+ // Need to put this here so that we don't need to include math.h in
+ // the IpVector.hpp header file
+ void Vector::Scal(Number alpha)
+ {
+ if (alpha!=1.) {
+ TaggedObject::Tag old_tag = GetTag();
+ ScalImpl(alpha);
+ ObjectChanged();
+ if (old_tag == nrm2_cache_tag_) {
+ nrm2_cache_tag_ = GetTag();
+ cached_nrm2_ *= fabs(alpha);
+ }
+ if (old_tag == asum_cache_tag_) {
+ asum_cache_tag_ = GetTag();
+ cached_asum_ *= fabs(alpha);
+ }
+ if (old_tag == amax_cache_tag_) {
+ amax_cache_tag_ = GetTag();
+ cached_amax_ *= fabs(alpha);
+ }
+ if (old_tag == max_cache_tag_) {
+ if (alpha>=0.) {
+ max_cache_tag_ = GetTag();
+ cached_max_ *= alpha;
+ }
+ else if (alpha<0.) {
+ min_cache_tag_ = GetTag();
+ cached_min_ = cached_max_ * alpha;
+ }
+ }
+ if (old_tag == min_cache_tag_) {
+ if (alpha>=0.) {
+ min_cache_tag_ = GetTag();
+ cached_min_ *= alpha;
+ }
+ else if (alpha<0.) {
+ max_cache_tag_ = GetTag();
+ cached_max_ = cached_min_ * alpha;
+ }
+ }
+ if (old_tag == sum_cache_tag_) {
+ sum_cache_tag_ = GetTag();
+ cached_sum_ *= alpha;
+ }
+ if (old_tag == sumlogs_cache_tag_) {
+ sumlogs_cache_tag_ = GetTag();
+ cached_sumlogs_ += ((Number)Dim())*log(alpha);
+ }
+ }
+ }
+
+ void Vector::AddVectorQuotientImpl(Number a, const Vector& z,
+ const Vector& s, Number c)
+ {
+ DBG_ASSERT(Dim() == z.Dim());
+ DBG_ASSERT(Dim() == s.Dim());
+
+ if (c==0.) {
+ AddOneVector(a, z, 0.);
+ ElementWiseDivide(s);
+ }
+ else {
+ SmartPtr<Vector> tmp = MakeNew();
+ tmp->Copy(z);
+ tmp->ElementWiseDivide(s);
+ AddOneVector(a, *tmp, c);
+ }
+ }
+
+ bool Vector::HasValidNumbersImpl() const
+ {
+ Number sum = Asum();
+ return IsFiniteNumber(sum);
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpVector.hpp b/SimTKmath/Optimizers/src/IpOpt/IpVector.hpp
new file mode 100644
index 0000000..5e4dce7
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpVector.hpp
@@ -0,0 +1,739 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpVector.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPVECTOR_HPP__
+#define __IPVECTOR_HPP__
+
+#include "IpTypes.hpp"
+#include "IpTaggedObject.hpp"
+#include "IpCachedResults.hpp"
+#include "IpSmartPtr.hpp"
+#include "IpJournalist.hpp"
+
+#include <vector>
+
+namespace Ipopt
+{
+ /* forward declarations */
+ class VectorSpace;
+
+ /** Vector Base Class.
+ * This is the base class for all derived vector types. Those vectors
+ * are meant to store entities like iterates, Lagrangian multipliers,
+ * constraint values etc. The implementation of a vector type depends
+ * on the computational environment (e.g. just a double array on a shared
+ * memory machine, or distributed double arrays for a distributed
+ * memory machine.)
+ *
+ * Deriving from Vector: This class inherits from tagged object to
+ * implement an advanced caching scheme. Because of this, the
+ * TaggedObject method ObjectChanged() must be called each time the
+ * Vector changes. If you overload the XXXX_Impl protected methods,
+ * this taken care of (along with caching if possible) for you. If
+ * you have additional methods in your derived class that change the
+ * underlying data (vector values), you MUST remember to call
+ * ObjectChanged() AFTER making the change!
+ */
+ class Vector : public TaggedObject
+ {
+ public:
+ /** @name Constructor/Destructor */
+ //@{
+ /** Constructor. It has to be given a pointer to the
+ * corresponding VectorSpace.
+ */
+ Vector(const VectorSpace* owner_space);
+
+ /** Destructor */
+ virtual ~Vector();
+ //@}
+
+ /** Create new Vector of the same type with uninitialized data */
+ Vector* MakeNew() const;
+
+ /** Create new Vector of the same type and copy the data over */
+ Vector* MakeNewCopy() const;
+
+ /**@name Standard BLAS-1 Operations
+ * (derived classes do NOT overload these
+ * methods, instead, overload the
+ * protected versions of these methods). */
+ //@{
+ /** Copy the data of the vector x into this vector (DCOPY). */
+ void Copy(const Vector& x);
+
+ /** Scales the vector by scalar alpha (DSCAL) */
+ void Scal(Number alpha);
+
+ /** Add the multiple alpha of vector x to this vector (DAXPY) */
+ void Axpy(Number alpha, const Vector &x);
+
+ /** Computes inner product of vector x with this (DDOT) */
+ Number Dot(const Vector &x) const;
+
+ /** Computes the 2-norm of this vector (DNRM2) */
+ Number Nrm2() const;
+
+ /** Computes the 1-norm of this vector (DASUM) */
+ Number Asum() const;
+
+ /** Computes the max-norm of this vector (based on IDAMAX) */
+ Number Amax() const;
+ //@}
+
+ /** @name Additional (Non-BLAS) Vector Methods
+ * (derived classes do NOT overload these
+ * methods, instead, overload the
+ * protected versions of these methods). */
+ //@{
+ /** Set each element in the vector to the scalar alpha. */
+ void Set(Number alpha);
+
+ /** Element-wise division \f$y_i \gets y_i/x_i\f$*/
+ void ElementWiseDivide(const Vector& x);
+
+ /** Element-wise multiplication \f$y_i \gets y_i*x_i\f$ */
+ void ElementWiseMultiply(const Vector& x);
+
+ /** Element-wise max against entries in x */
+ void ElementWiseMax(const Vector& x);
+
+ /** Element-wise min against entries in x */
+ void ElementWiseMin(const Vector& x);
+
+ /** Reciprocates the entries in the vector */
+ void ElementWiseReciprocal();
+
+ /** Absolute values of the entries in the vector */
+ void ElementWiseAbs();
+
+ /** Element-wise square root of the entries in the vector */
+ void ElementWiseSqrt();
+
+ /** Replaces the vector values with their sgn values
+ ( -1 if x_i < 0, 0 if x_i == 0, and 1 if x_i > 0)
+ */
+ void ElementWiseSgn();
+
+ /** Add scalar to every vector component */
+ void AddScalar(Number scalar);
+
+ /** Returns the maximum value in the vector */
+ Number Max() const;
+
+ /** Returns the minimum value in the vector */
+ Number Min() const;
+
+ /** Returns the sum of the vector entries */
+ Number Sum() const;
+
+ /** Returns the sum of the logs of each vector entry */
+ Number SumLogs() const;
+ //@}
+
+ /** @name Methods for specialized operations. A prototype
+ * implementation is provided, but for efficient implementation
+ * those should be specially implemented.
+ */
+ //@{
+ /** Add one vector, y = a * v1 + c * y. This is automatically
+ * reduced to call AddTwoVectors. */
+ void AddOneVector(Number a, const Vector& v1, Number c);
+
+ /** Add two vectors, y = a * v1 + b * v2 + c * y. Here, this
+ * vector is y */
+ void AddTwoVectors(Number a, const Vector& v1,
+ Number b, const Vector& v2, Number c);
+ /** Fraction to the boundary parameter. Computes \f$\alpha =
+ * \max\{\bar\alpha\in(0,1] : x + \bar\alpha \Delta \geq (1-\tau)x\}\f$
+ */
+ Number FracToBound(const Vector& delta, Number tau) const;
+ /** Add the quotient of two vectors, y = a * z/s + c * y. */
+ void AddVectorQuotient(Number a, const Vector& z, const Vector& s,
+ Number c);
+ //@}
+
+ /** Method for determining if all stored numbers are valid (i.e.,
+ * no Inf or Nan). */
+ bool HasValidNumbers() const;
+
+ /** @name Accessor methods */
+ //@{
+ /** Dimension of the Vector */
+ Index Dim() const;
+
+ /** Return the owner VectorSpace*/
+ SmartPtr<const VectorSpace> OwnerSpace() const;
+ //@}
+
+ /** @name Output methods
+ * (derived classes do NOT overload these
+ * methods, instead, overload the
+ * protected versions of these methods). */
+ //@{
+ /** Print the entire vector */
+ void Print(SmartPtr<const Journalist> jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent=0,
+ const std::string& prefix="") const;
+ void Print(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent=0,
+ const std::string& prefix="") const;
+ //@}
+
+ protected:
+ /** @name implementation methods (derived classes MUST
+ * overload these pure virtual protected methods.)
+ */
+ //@{
+ /** Copy the data of the vector x into this vector (DCOPY). */
+ virtual void CopyImpl(const Vector& x)=0;
+
+ /** Scales the vector by scalar alpha (DSCAL) */
+ virtual void ScalImpl(Number alpha)=0;
+
+ /** Add the multiple alpha of vector x to this vector (DAXPY) */
+ virtual void AxpyImpl(Number alpha, const Vector &x)=0;
+
+ /** Computes inner product of vector x with this (DDOT) */
+ virtual Number DotImpl(const Vector &x) const =0;
+
+ /** Computes the 2-norm of this vector (DNRM2) */
+ virtual Number Nrm2Impl() const =0;
+
+ /** Computes the 1-norm of this vector (DASUM) */
+ virtual Number AsumImpl() const =0;
+
+ /** Computes the max-norm of this vector (based on IDAMAX) */
+ virtual Number AmaxImpl() const =0;
+
+ /** Set each element in the vector to the scalar alpha. */
+ virtual void SetImpl(Number alpha)=0;
+
+ /** Element-wise division \f$y_i \gets y_i/x_i\f$*/
+ virtual void ElementWiseDivideImpl(const Vector& x)=0;
+
+ /** Element-wise multiplication \f$y_i \gets y_i*x_i\f$ */
+ virtual void ElementWiseMultiplyImpl(const Vector& x)=0;
+
+ /** Element-wise max against entries in x */
+ virtual void ElementWiseMaxImpl(const Vector& x)=0;
+
+ /** Element-wise min against entries in x */
+ virtual void ElementWiseMinImpl(const Vector& x)=0;
+
+ /** Reciprocates the elements of the vector */
+ virtual void ElementWiseReciprocalImpl()=0;
+
+ /** Take elementwise absolute values of the elements of the vector */
+ virtual void ElementWiseAbsImpl()=0;
+
+ /** Take elementwise square-root of the elements of the vector */
+ virtual void ElementWiseSqrtImpl()=0;
+
+ /** Replaces entries with sgn of the entry */
+ virtual void ElementWiseSgnImpl()=0;
+
+ /** Add scalar to every component of vector */
+ virtual void AddScalarImpl(Number scalar)=0;
+
+ /** Max value in the vector */
+ virtual Number MaxImpl() const=0;
+
+ /** Min number in the vector */
+ virtual Number MinImpl() const=0;
+
+ /** Sum of entries in the vector */
+ virtual Number SumImpl() const=0;
+
+ /** Sum of logs of entries in the vector */
+ virtual Number SumLogsImpl() const=0;
+
+ /** Add two vectors (a * v1 + b * v2). Result is stored in this
+ vector. */
+ virtual void AddTwoVectorsImpl(Number a, const Vector& v1,
+ Number b, const Vector& v2, Number c);
+
+ /** Fraction to boundary parameter. */
+ virtual Number FracToBoundImpl(const Vector& delta, Number tau) const;
+
+ /** Add the quotient of two vectors */
+ virtual void AddVectorQuotientImpl(Number a, const Vector& z,
+ const Vector& s, Number c);
+
+ /** Method for determining if all stored numbers are valid (i.e.,
+ * no Inf or Nan). A default implementation using Asum is
+ * provided. */
+ virtual bool HasValidNumbersImpl() const;
+
+ /** Print the entire vector */
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const =0;
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default constructor */
+ Vector();
+
+ /** Copy constructor */
+ Vector(const Vector&);
+
+ /** Overloaded Equals Operator */
+ Vector& operator=(const Vector&);
+ //@}
+
+ /** Vector Space */
+ const SmartPtr<const VectorSpace> owner_space_;
+
+ /**@name CachedResults data members */
+ //@{
+ /** Cache for dot products */
+ mutable CachedResults<Number> dot_cache_;
+
+ mutable TaggedObject::Tag nrm2_cache_tag_;
+ mutable Number cached_nrm2_;
+
+ mutable TaggedObject::Tag asum_cache_tag_;
+ mutable Number cached_asum_;
+
+ mutable TaggedObject::Tag amax_cache_tag_;
+ mutable Number cached_amax_;
+
+ mutable TaggedObject::Tag max_cache_tag_;
+ mutable Number cached_max_;
+
+ mutable TaggedObject::Tag min_cache_tag_;
+ mutable Number cached_min_;
+
+ mutable TaggedObject::Tag sum_cache_tag_;
+ mutable Number cached_sum_;
+
+ mutable TaggedObject::Tag sumlogs_cache_tag_;
+ mutable Number cached_sumlogs_;
+
+ mutable TaggedObject::Tag valid_cache_tag_;
+ mutable bool cached_valid_;
+
+ // AW: I removed this cache since it gets in the way for the
+ // quality function search
+ // /** Cache for FracToBound */
+ // mutable CachedResults<Number> frac_to_bound_cache_;
+ //@}
+
+ };
+
+ /** VectorSpace base class, corresponding to the Vector base class.
+ * For each Vector implementation, a corresponding VectorSpace has
+ * to be implemented. A VectorSpace is able to create new Vectors
+ * of a specific type. The VectorSpace should also store
+ * information that is common to all Vectors of that type. For
+ * example, the dimension of a Vector is stored in the VectorSpace
+ * base class.
+ */
+ class VectorSpace : public ReferencedObject
+ {
+ public:
+ /** @name Constructors/Destructors */
+ //@{
+ /** Constructor, given the dimension of all vectors generated by
+ * this VectorSpace.
+ */
+ VectorSpace(Index dim);
+
+ /** Destructor */
+ virtual ~VectorSpace()
+ {}
+ //@}
+
+ /** Pure virtual method for creating a new Vector of the
+ * corresponding type.
+ */
+ virtual Vector* MakeNew() const=0;
+
+ /** Accessor function for the dimension of the vectors of this type.*/
+ Index Dim() const
+ {
+ return dim_;
+ }
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** default constructor */
+ VectorSpace();
+
+ /** Copy constructor */
+ VectorSpace(const VectorSpace&);
+
+ /** Overloaded Equals Operator */
+ VectorSpace& operator=(const VectorSpace&);
+ //@}
+
+ /** Dimension of the vectors in this vector space. */
+ const Index dim_;
+ };
+
+ /* inline methods */
+ inline
+ Vector::~Vector()
+ {}
+
+ inline
+ Vector::Vector(const VectorSpace* owner_space)
+ :
+ TaggedObject(),
+ owner_space_(owner_space),
+ dot_cache_(10),
+ nrm2_cache_tag_(0),
+ asum_cache_tag_(0),
+ amax_cache_tag_(0),
+ max_cache_tag_(0),
+ min_cache_tag_(0),
+ sum_cache_tag_(0),
+ sumlogs_cache_tag_(0),
+ cached_valid_(0)
+ {
+ DBG_ASSERT(IsValid(owner_space_));
+ }
+
+ inline
+ Vector* Vector::MakeNew() const
+ {
+ return owner_space_->MakeNew();
+ }
+
+ inline
+ Vector* Vector::MakeNewCopy() const
+ {
+ // ToDo: We can probably copy also the cached values for Norms etc here
+ Vector* copy = MakeNew();
+ copy->Copy(*this);
+ return copy;
+ }
+
+ inline
+ void Vector::Copy(const Vector& x)
+ {
+ CopyImpl(x);
+ ObjectChanged();
+ // Also copy any cached scalar values from the original vector
+ // ToDo: Check if that is too much overhead
+ TaggedObject::Tag x_tag = x.GetTag();
+ if (x_tag == x.nrm2_cache_tag_) {
+ nrm2_cache_tag_ = GetTag();
+ cached_nrm2_ = x.cached_nrm2_;
+ }
+ if (x_tag == x.asum_cache_tag_) {
+ asum_cache_tag_ = GetTag();
+ cached_asum_ = x.cached_asum_;
+ }
+ if (x_tag == x.amax_cache_tag_) {
+ amax_cache_tag_ = GetTag();
+ cached_amax_ = x.cached_amax_;
+ }
+ if (x_tag == x.max_cache_tag_) {
+ max_cache_tag_ = GetTag();
+ cached_max_ = x.cached_max_;
+ }
+ if (x_tag == x.min_cache_tag_) {
+ min_cache_tag_ = GetTag();
+ cached_min_ = x.cached_min_;
+ }
+ if (x_tag == x.sum_cache_tag_) {
+ sum_cache_tag_ = GetTag();
+ cached_sum_ = x.cached_sum_;
+ }
+ if (x_tag == x.sumlogs_cache_tag_) {
+ sumlogs_cache_tag_ = GetTag();
+ cached_sumlogs_ = x.cached_sumlogs_;
+ }
+ }
+
+ inline
+ void Vector::Axpy(Number alpha, const Vector &x)
+ {
+ AxpyImpl(alpha, x);
+ ObjectChanged();
+ }
+
+ inline
+ Number Vector::Dot(const Vector &x) const
+ {
+ // The current implementation of the caching doesn't allow to have
+ // a dependency of something with itself. Therefore, we use the
+ // Nrm2 method if the dot product is to be taken with the vector
+ // itself. Might be more efficient anyway.
+ if (this==&x) {
+ Number nrm2 = Nrm2();
+ return nrm2*nrm2;
+ }
+ Number retValue;
+ if (!dot_cache_.GetCachedResult2Dep(retValue, this, &x)) {
+ retValue = DotImpl(x);
+ dot_cache_.AddCachedResult2Dep(retValue, this, &x);
+ }
+ return retValue;
+ }
+
+ inline
+ Number Vector::Nrm2() const
+ {
+ if (nrm2_cache_tag_ != GetTag()) {
+ cached_nrm2_ = Nrm2Impl();
+ nrm2_cache_tag_ = GetTag();
+ }
+ return cached_nrm2_;
+ }
+
+ inline
+ Number Vector::Asum() const
+ {
+ if (asum_cache_tag_ != GetTag()) {
+ cached_asum_ = AsumImpl();
+ asum_cache_tag_ = GetTag();
+ }
+ return cached_asum_;
+ }
+
+ inline
+ Number Vector::Amax() const
+ {
+ if (amax_cache_tag_ != GetTag()) {
+ cached_amax_ = AmaxImpl();
+ amax_cache_tag_ = GetTag();
+ }
+ return cached_amax_;
+ }
+
+ inline
+ Number Vector::Sum() const
+ {
+ if (sum_cache_tag_ != GetTag()) {
+ cached_sum_ = SumImpl();
+ sum_cache_tag_ = GetTag();
+ }
+ return cached_sum_;
+ }
+
+ inline
+ Number Vector::SumLogs() const
+ {
+ if (sumlogs_cache_tag_ != GetTag()) {
+ cached_sumlogs_ = SumLogsImpl();
+ sumlogs_cache_tag_ = GetTag();
+ }
+ return cached_sumlogs_;
+ }
+
+ inline
+ void Vector::ElementWiseSgn()
+ {
+ ElementWiseSgnImpl();
+ ObjectChanged();
+ }
+
+ inline
+ void Vector::Set(Number alpha)
+ {
+ // Could initialize caches here
+ SetImpl(alpha);
+ ObjectChanged();
+ }
+
+ inline
+ void Vector::ElementWiseDivide(const Vector& x)
+ {
+ ElementWiseDivideImpl(x);
+ ObjectChanged();
+ }
+
+ inline
+ void Vector::ElementWiseMultiply(const Vector& x)
+ {
+ ElementWiseMultiplyImpl(x);
+ ObjectChanged();
+ }
+
+ inline
+ void Vector::ElementWiseReciprocal()
+ {
+ ElementWiseReciprocalImpl();
+ ObjectChanged();
+ }
+
+ inline
+ void Vector::ElementWiseMax(const Vector& x)
+ {
+ // Could initialize some caches here
+ ElementWiseMaxImpl(x);
+ ObjectChanged();
+ }
+
+ inline
+ void Vector::ElementWiseMin(const Vector& x)
+ {
+ // Could initialize some caches here
+ ElementWiseMinImpl(x);
+ ObjectChanged();
+ }
+
+ inline
+ void Vector::ElementWiseAbs()
+ {
+ // Could initialize some caches here
+ ElementWiseAbsImpl();
+ ObjectChanged();
+ }
+
+ inline
+ void Vector::ElementWiseSqrt()
+ {
+ ElementWiseSqrtImpl();
+ ObjectChanged();
+ }
+
+ inline
+ void Vector::AddScalar(Number scalar)
+ {
+ // Could initialize some caches here
+ AddScalarImpl(scalar);
+ ObjectChanged();
+ }
+
+ inline
+ Number Vector::Max() const
+ {
+ if (max_cache_tag_ != GetTag()) {
+ cached_max_ = MaxImpl();
+ max_cache_tag_ = GetTag();
+ }
+ return cached_max_;
+ }
+
+ inline
+ Number Vector::Min() const
+ {
+ if (min_cache_tag_ != GetTag()) {
+ cached_min_ = MinImpl();
+ min_cache_tag_ = GetTag();
+ }
+ return cached_min_;
+ }
+
+ inline
+ void Vector::AddOneVector(Number a, const Vector& v1, Number c)
+ {
+ AddTwoVectorsImpl(a, v1, 0., v1, c);
+ }
+
+ inline
+ void Vector::AddTwoVectors(Number a, const Vector& v1,
+ Number b, const Vector& v2, Number c)
+ {
+ AddTwoVectorsImpl(a, v1, b, v2, c);
+ ObjectChanged();
+ }
+
+ inline
+ Number Vector::FracToBound(const Vector& delta, Number tau) const
+ {
+ /* AW: I avoid the caching here, since it leads to overhead in the
+ quality function search. Caches for this are in
+ CalculatedQuantities.
+ Number retValue;
+ std::vector<const TaggedObject*> tdeps(1);
+ tdeps[0] = δ
+ std::vector<Number> sdeps(1);
+ sdeps[0] = tau;
+ if (!frac_to_bound_cache_.GetCachedResult(retValue, tdeps, sdeps)) {
+ retValue = FracToBoundImpl(delta, tau);
+ frac_to_bound_cache_.AddCachedResult(retValue, tdeps, sdeps);
+ }
+ return retValue;
+ */
+ return FracToBoundImpl(delta, tau);
+ }
+
+ inline
+ void Vector::AddVectorQuotient(Number a, const Vector& z,
+ const Vector& s, Number c)
+ {
+ AddVectorQuotientImpl(a, z, s, c);
+ ObjectChanged();
+ }
+
+ inline
+ bool Vector::HasValidNumbers() const
+ {
+ if (valid_cache_tag_ != GetTag()) {
+ cached_valid_ = HasValidNumbersImpl();
+ valid_cache_tag_ = GetTag();
+ }
+ return cached_valid_;
+ }
+
+ inline
+ Index Vector::Dim() const
+ {
+ return owner_space_->Dim();
+ }
+
+ inline
+ SmartPtr<const VectorSpace> Vector::OwnerSpace() const
+ {
+ return owner_space_;
+ }
+
+ inline
+ VectorSpace::VectorSpace(Index dim)
+ :
+ dim_(dim)
+ {}
+
+} // namespace Ipopt
+
+// Macro definitions for debugging vectors
+#ifndef IP_DEBUG
+# define DBG_PRINT_VECTOR(__verbose_level, __vec_name, __vec)
+#else
+# define DBG_PRINT_VECTOR(__verbose_level, __vec_name, __vec) \
+ if (dbg_jrnl.Verbosity() >= (__verbose_level)) { \
+ if (dbg_jrnl.Jnlst()!=NULL) { \
+ (__vec).Print(dbg_jrnl.Jnlst(), \
+ J_ERROR, J_DBG, \
+ __vec_name, \
+ dbg_jrnl.IndentationLevel()*2, \
+ "# "); \
+ } \
+ }
+#endif // IP_DEBUG
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpWarmStartIterateInitializer.cpp b/SimTKmath/Optimizers/src/IpOpt/IpWarmStartIterateInitializer.cpp
new file mode 100644
index 0000000..ae0828f
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpWarmStartIterateInitializer.cpp
@@ -0,0 +1,415 @@
+// Copyright (C) 2005, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpWarmStartIterateInitializer.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-04-01
+
+#include "IpWarmStartIterateInitializer.hpp"
+#include "IpDefaultIterateInitializer.hpp"
+
+// ToDo make independent of DenseVector
+#include "IpDenseVector.hpp"
+
+#ifdef HAVE_CMATH
+# include <cmath>
+#else
+# ifdef HAVE_MATH_H
+# include <math.h>
+# else
+# error "don't have header file for math"
+# endif
+#endif
+
+namespace Ipopt
+{
+#ifdef IP_DEBUG
+ static const Index dbg_verbosity = 0;
+#endif
+
+ WarmStartIterateInitializer::WarmStartIterateInitializer()
+ :
+ IterateInitializer()
+ {}
+
+ void WarmStartIterateInitializer::RegisterOptions(SmartPtr<RegisteredOptions> roptions)
+ {
+ roptions->AddLowerBoundedNumberOption(
+ "warm_start_bound_push",
+ "same as bound_push for the regular initializer.",
+ 0.0, true, 1e-3);
+ roptions->AddBoundedNumberOption(
+ "warm_start_bound_frac",
+ "same as bound_frac for the regular initializer.",
+ 0.0, true, 0.5, false, 1e-3);
+ roptions->AddLowerBoundedNumberOption(
+ "warm_start_mult_bound_push",
+ "same as mult_bound_push for the regular initializer.",
+ 0.0, true, 1e-3);
+ roptions->AddNumberOption(
+ "warm_start_mult_init_max",
+ "Maximum initial value for the equality multipliers.",
+ 1e6);
+ roptions->AddNumberOption(
+ "warm_start_target_mu",
+ "Unsupported!",
+ 0e-3);
+ roptions->AddStringOption2(
+ "warm_start_entire_iterate",
+ "Tells algorithm whether to use the GetWarmStartIterate method in the NLP.",
+ "no",
+ "no", "call GetStartingPoint in the NLP",
+ "yes", "call GetWarmStartIterate in the NLP",
+ "");
+ }
+
+ bool WarmStartIterateInitializer::InitializeImpl(const OptionsList& options,
+ const std::string& prefix)
+ {
+ options.GetNumericValue("warm_start_bound_push",
+ warm_start_bound_push_, prefix);
+ options.GetNumericValue("warm_start_bound_frac",
+ warm_start_bound_frac_, prefix);
+ options.GetNumericValue("warm_start_mult_bound_push",
+ warm_start_mult_bound_push_, prefix);
+ options.GetNumericValue("warm_start_mult_init_max",
+ warm_start_mult_init_max_, prefix);
+ options.GetNumericValue("warm_start_target_mu",
+ warm_start_target_mu_, prefix);
+ options.GetBoolValue("warm_start_entire_iterate",
+ warm_start_entire_iterate_, prefix);
+
+ return true;
+ }
+
+ bool WarmStartIterateInitializer::SetInitialIterates()
+ {
+ DBG_START_METH("WarmStartIterateInitializer::SetInitialIterates",
+ dbg_verbosity);
+
+ // Get the starting values provided by the NLP and store them
+ // in the ip_data current fields.
+
+ SmartPtr<IteratesVector> init_vec;
+ bool have_iterate = false;
+
+ if (warm_start_entire_iterate_) {
+ IpData().InitializeDataStructures(IpNLP(), false, false, false,
+ false, false);
+
+ init_vec = IpData().curr()->MakeNewIteratesVector(true);
+
+ have_iterate = IpNLP().GetWarmStartIterate(*init_vec);
+
+ if (!have_iterate) {
+ Jnlst().Printf(J_DETAILED, J_LINE_SEARCH,
+ "Tried to obtain entire warm start iterate from NLP, but it returned false.\n");
+ IpData().Append_info_string("NW");
+ }
+
+ // Make sure given bounds are respected
+ if (have_iterate && warm_start_mult_init_max_>0.) {
+ SmartPtr<Vector> y_c = init_vec->create_new_y_c_copy();
+ SmartPtr<Vector> tmp = y_c->MakeNew();
+ tmp->Set(warm_start_mult_init_max_);
+ y_c->ElementWiseMin(*tmp);
+ tmp->Set(-warm_start_mult_init_max_);
+ y_c->ElementWiseMax(*tmp);
+
+ SmartPtr<Vector> y_d = init_vec->create_new_y_d_copy();
+ tmp = y_d->MakeNew();
+ tmp->Set(warm_start_mult_init_max_);
+ y_d->ElementWiseMin(*tmp);
+ tmp->Set(-warm_start_mult_init_max_);
+ y_d->ElementWiseMax(*tmp);
+
+ SmartPtr<Vector> z_L = init_vec->create_new_z_L_copy();
+ tmp = z_L->MakeNew();
+ tmp->Set(warm_start_mult_init_max_);
+ z_L->ElementWiseMin(*tmp);
+
+ SmartPtr<Vector> z_U = init_vec->create_new_z_U_copy();
+ tmp = z_U->MakeNew();
+ tmp->Set(warm_start_mult_init_max_);
+ z_U->ElementWiseMin(*tmp);
+
+ SmartPtr<Vector> v_L = init_vec->create_new_v_L_copy();
+ tmp = v_L->MakeNew();
+ tmp->Set(warm_start_mult_init_max_);
+ v_L->ElementWiseMin(*tmp);
+
+ SmartPtr<Vector> v_U = init_vec->create_new_v_U_copy();
+ tmp = v_U->MakeNew();
+ tmp->Set(warm_start_mult_init_max_);
+ v_U->ElementWiseMin(*tmp);
+ }
+ }
+
+ if (!have_iterate) {
+
+ /////////////////////////////////////////////////////////////////////
+ // Initialize primal variables //
+ /////////////////////////////////////////////////////////////////////
+
+ // Get the intial values for x, y_c, y_d, z_L, z_U,
+ IpData().InitializeDataStructures(IpNLP(), true, true, true, true, true);
+
+ IpData().curr()->x()->Print(Jnlst(), J_VECTOR, J_INITIALIZATION,
+ "user-provided x");
+ IpData().curr()->y_c()->Print(Jnlst(), J_VECTOR, J_INITIALIZATION,
+ "user-provided y_c");
+ IpData().curr()->y_d()->Print(Jnlst(), J_VECTOR, J_INITIALIZATION,
+ "user-provided y_d");
+ IpData().curr()->z_L()->Print(Jnlst(), J_VECTOR, J_INITIALIZATION,
+ "user-provided z_L");
+ IpData().curr()->z_U()->Print(Jnlst(), J_VECTOR, J_INITIALIZATION,
+ "user-provided z_U");
+ if (Jnlst().ProduceOutput(J_MOREVECTOR, J_INITIALIZATION)) {
+ IpCq().curr_d()->Print(Jnlst(), J_MOREVECTOR, J_INITIALIZATION,
+ "d at user-provided x");
+ }
+
+ SmartPtr<Vector> tmp;
+
+ init_vec = IpData().curr()->MakeNewContainer();
+
+ // If requested, make sure that the multipliers are not too large
+ if (warm_start_mult_init_max_>0.) {
+ SmartPtr<Vector> y_c = init_vec->create_new_y_c_copy();
+ tmp = y_c->MakeNew();
+ tmp->Set(warm_start_mult_init_max_);
+ y_c->ElementWiseMin(*tmp);
+ tmp->Set(-warm_start_mult_init_max_);
+ y_c->ElementWiseMax(*tmp);
+
+ SmartPtr<Vector> y_d = init_vec->create_new_y_d_copy();
+ tmp = y_d->MakeNew();
+ tmp->Set(warm_start_mult_init_max_);
+ y_d->ElementWiseMin(*tmp);
+ tmp->Set(-warm_start_mult_init_max_);
+ y_d->ElementWiseMax(*tmp);
+
+ SmartPtr<Vector> z_L = init_vec->create_new_z_L_copy();
+ tmp = z_L->MakeNew();
+ tmp->Set(warm_start_mult_init_max_);
+ z_L->ElementWiseMin(*tmp);
+
+ SmartPtr<Vector> z_U = init_vec->create_new_z_U_copy();
+ tmp = z_U->MakeNew();
+ tmp->Set(warm_start_mult_init_max_);
+ z_U->ElementWiseMin(*tmp);
+ }
+
+ // Get the initial values for v_L and v_U out of y_d
+ SmartPtr<Vector> v_L = init_vec->create_new_v_L();
+ IpNLP().Pd_L()->TransMultVector(-1., *init_vec->y_d(), 0., *v_L);
+ tmp = v_L->MakeNew();
+ tmp->Set(warm_start_mult_bound_push_);
+ v_L->ElementWiseMax(*tmp);
+
+ SmartPtr<Vector> v_U = init_vec->create_new_v_U();
+ IpNLP().Pd_U()->TransMultVector(1., *init_vec->y_d(), 0., *v_U);
+ tmp = v_U->MakeNew();
+ tmp->Set(warm_start_mult_bound_push_);
+ v_U->ElementWiseMax(*tmp);
+
+ // Initialize slack variables
+ init_vec->Set_s(*IpCq().curr_d());
+ }
+
+ // Make the corrected values current (and initialize s)
+ IpData().set_trial(init_vec);
+ IpData().AcceptTrialPoint();
+
+ // Now apply the target mu heuristic if required
+ if (warm_start_target_mu_>0.) {
+ SmartPtr<const Vector> new_x;
+ SmartPtr<const Vector> new_z_L;
+
+ SmartPtr<const IteratesVector> curr = IpData().curr();
+ process_target_mu(1., *curr->x(), *IpCq().curr_slack_x_L(),
+ *curr->z_L(), *IpNLP().Px_L(),
+ new_x, new_z_L);
+ SmartPtr<const Vector> new_s;
+ SmartPtr<const Vector> new_v_L;
+ process_target_mu(1., *curr->s(), *IpCq().curr_slack_s_L(),
+ *curr->v_L(), *IpNLP().Pd_L(),
+ new_s, new_v_L);
+
+ // Set the trial pointers to new_x and new_s. The process_target_mu
+ // methods below create new vectors in new_x and new_s and do not alter
+ // the existing ones.
+ init_vec->Set_x(*new_x);
+ init_vec->Set_s(*new_s);
+ IpData().set_trial(init_vec);
+
+ SmartPtr<const Vector> new_z_U;
+ process_target_mu(-1., *IpData().trial()->x(), *IpCq().trial_slack_x_U(),
+ *IpData().curr()->z_U(), *IpNLP().Px_U(),
+ new_x, new_z_U);
+ SmartPtr<const Vector> new_v_U;
+ process_target_mu(-1., *IpData().trial()->s(), *IpCq().trial_slack_s_U(),
+ *IpData().curr()->v_U(), *IpNLP().Pd_U(),
+ new_s, new_v_U);
+
+ // Now submit the full modified point
+ init_vec->Set_x(*new_x);
+ init_vec->Set_s(*new_s);
+ // y_c and y_d currently contain a copy of curr()->y_c...
+ // we set them back to the actual pointer to reuse the tags
+ init_vec->Set_y_c(*IpData().curr()->y_c());
+ init_vec->Set_y_d(*IpData().curr()->y_d());
+ init_vec->Set_z_L(*new_z_L);
+ init_vec->Set_z_U(*new_z_U);
+ init_vec->Set_v_L(*new_v_L);
+ init_vec->Set_v_U(*new_v_U);
+ IpData().set_trial(init_vec);
+ IpData().AcceptTrialPoint();
+
+ // We need to call this to make sure that we don't get an error
+ // message because at some point a slack became too small
+ IpCq().ResetAdjustedTrialSlacks();
+ }
+
+ SmartPtr<const Vector> new_x;
+ SmartPtr<const Vector> new_s;
+ // Push the primal x variables
+ DefaultIterateInitializer::push_variables(Jnlst(),
+ warm_start_bound_push_,
+ warm_start_bound_frac_,
+ "x",
+ *IpData().curr()->x(),
+ new_x,
+ *IpNLP().x_L(),
+ *IpNLP().x_U(),
+ *IpNLP().Px_L(),
+ *IpNLP().Px_U());
+
+ // Push the primal s variables
+ DefaultIterateInitializer::push_variables(Jnlst(),
+ warm_start_bound_push_,
+ warm_start_bound_frac_,
+ "s",
+ *IpData().curr()->s(),
+ new_s,
+ *IpNLP().d_L(),
+ *IpNLP().d_U(),
+ *IpNLP().Pd_L(),
+ *IpNLP().Pd_U());
+
+ // Push the multipliers
+ SmartPtr<Vector> new_z_L = IpData().curr()->z_L()->MakeNewCopy();
+ SmartPtr<Vector> tmp = IpData().curr()->z_L()->MakeNew();
+ tmp->Set(warm_start_mult_bound_push_);
+ new_z_L->ElementWiseMax(*tmp);
+
+ SmartPtr<Vector> new_z_U = IpData().curr()->z_U()->MakeNewCopy();
+ tmp = IpData().curr()->z_U()->MakeNew();
+ tmp->Set(warm_start_mult_bound_push_);
+ new_z_U->ElementWiseMax(*tmp);
+
+ SmartPtr<Vector> new_v_L = IpData().curr()->v_L()->MakeNewCopy();
+ tmp = IpData().curr()->v_L()->MakeNew();
+ tmp->Set(warm_start_mult_bound_push_);
+ new_v_L->ElementWiseMax(*tmp);
+
+ SmartPtr<Vector> new_v_U = IpData().curr()->v_U()->MakeNewCopy();
+ tmp = IpData().curr()->v_U()->MakeNew();
+ tmp->Set(warm_start_mult_bound_push_);
+ new_v_U->ElementWiseMax(*tmp);
+
+ // Make sure the new variables are current
+ init_vec = IpData().curr()->MakeNewContainer();
+ init_vec->Set_x(*new_x);
+ init_vec->Set_s(*new_s);
+ init_vec->Set_z_L(*new_z_L);
+ init_vec->Set_z_U(*new_z_U);
+ init_vec->Set_v_L(*new_v_L);
+ init_vec->Set_v_U(*new_v_U);
+ IpData().set_trial(init_vec);
+ IpData().AcceptTrialPoint();
+
+ IpData().curr()->x()->Print(Jnlst(), J_VECTOR, J_INITIALIZATION,
+ "initial x");
+ IpData().curr()->s()->Print(Jnlst(), J_VECTOR, J_INITIALIZATION,
+ "initial s");
+ IpData().curr()->y_c()->Print(Jnlst(), J_VECTOR, J_INITIALIZATION,
+ "initial y_c");
+ IpData().curr()->y_d()->Print(Jnlst(), J_VECTOR, J_INITIALIZATION,
+ "initial y_d");
+ IpData().curr()->z_L()->Print(Jnlst(), J_VECTOR, J_INITIALIZATION,
+ "initial z_L");
+ IpData().curr()->z_U()->Print(Jnlst(), J_VECTOR, J_INITIALIZATION,
+ "initial z_U");
+ IpData().curr()->v_L()->Print(Jnlst(), J_VECTOR, J_INITIALIZATION,
+ "initial v_L");
+ IpData().curr()->v_U()->Print(Jnlst(), J_VECTOR, J_INITIALIZATION,
+ "initial v_U");
+ if (Jnlst().ProduceOutput(J_MOREVECTOR, J_INITIALIZATION)) {
+ IpCq().curr_slack_x_L()->Print(Jnlst(), J_MOREVECTOR, J_INITIALIZATION,
+ "initial slack_x_L");
+ IpCq().curr_slack_x_U()->Print(Jnlst(), J_MOREVECTOR, J_INITIALIZATION,
+ "initial slack_x_U");
+ IpCq().curr_slack_s_L()->Print(Jnlst(), J_MOREVECTOR, J_INITIALIZATION,
+ "initial slack_s_L");
+ IpCq().curr_slack_s_U()->Print(Jnlst(), J_MOREVECTOR, J_INITIALIZATION,
+ "initial slack_s_U");
+ }
+
+ return true;
+ }
+
+ void WarmStartIterateInitializer::process_target_mu(Number factor,
+ const Vector& curr_vars,
+ const Vector& curr_slacks,
+ const Vector& curr_mults,
+ const Matrix& P,
+ SmartPtr<const Vector>& ret_vars,
+ SmartPtr<const Vector>& ret_mults)
+ {
+ SmartPtr<Vector> new_slacks = curr_slacks.MakeNewCopy();
+ SmartPtr<Vector> new_mults = curr_mults.MakeNewCopy();
+ adapt_to_target_mu(*new_slacks, *new_mults, warm_start_target_mu_);
+ new_slacks->Axpy(-1, curr_slacks); // this is now correction step
+ SmartPtr<Vector> new_vars = curr_vars.MakeNew();
+ new_vars->Copy(curr_vars);
+ P.MultVector(factor, *new_slacks, 1., *new_vars);
+
+ ret_vars = ConstPtr(new_vars);
+ ret_mults = ConstPtr(new_mults);
+ }
+
+ void WarmStartIterateInitializer::adapt_to_target_mu(Vector& new_s,
+ Vector& new_z,
+ Number target_mu)
+ {
+ DBG_ASSERT(new_s.Dim() == new_z.Dim());
+
+ DenseVector* dnew_s = dynamic_cast<DenseVector*>(&new_s);
+ assert(dnew_s);
+ DenseVector* dnew_z = dynamic_cast<DenseVector*>(&new_z);
+ assert(dnew_z);
+ Number* values_s = dnew_s->Values();
+ Number* values_z = dnew_z->Values();
+
+ for (Index i=0; i<new_s.Dim(); i++) {
+ if (values_s[i] > 1e4*values_z[i]) {
+ values_z[i] = target_mu/values_s[i];
+ if (values_z[i]>values_s[i]) {
+ values_s[i] = values_z[i] = sqrt(target_mu);
+ }
+ }
+ else if (values_z[i] > 1e4*values_s[i]) {
+ values_s[i] = target_mu/values_z[i];
+ if (values_s[i]>values_z[i]) {
+ values_s[i] = values_z[i] = sqrt(target_mu);
+ }
+ }
+ else {
+ values_s[i] = values_z[i] = sqrt(target_mu);
+ }
+ }
+ }
+
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpWarmStartIterateInitializer.hpp b/SimTKmath/Optimizers/src/IpOpt/IpWarmStartIterateInitializer.hpp
new file mode 100644
index 0000000..23a0f9b
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpWarmStartIterateInitializer.hpp
@@ -0,0 +1,100 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpWarmStartIterateInitializer.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2005-04-01
+
+#ifndef __IPWARMSTARTITERATEINITIALIZER_HPP__
+#define __IPWARMSTARTITERATEINITIALIZER_HPP__
+
+#include "IpIterateInitializer.hpp"
+#include "IpEqMultCalculator.hpp"
+
+namespace Ipopt
+{
+
+ /** Class implementing an initialization procedure for warm starts.
+ */
+ class WarmStartIterateInitializer: public IterateInitializer
+ {
+ public:
+ /**@name Constructors/Destructors */
+ //@{
+ /** Constructor. */
+ WarmStartIterateInitializer();
+
+ /** Default destructor */
+ virtual ~WarmStartIterateInitializer()
+ {}
+ //@}
+
+ /** overloaded from AlgorithmStrategyObject */
+ virtual bool InitializeImpl(const OptionsList& options,
+ const std::string& prefix);
+
+ /** Compute the initial iterates and set the into the curr field
+ * of the ip_data object. */
+ virtual bool SetInitialIterates();
+
+ /** Methods used by IpoptType */
+ //@{
+ static void RegisterOptions(SmartPtr<RegisteredOptions> roptions);
+ //@}
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Copy Constructor */
+ WarmStartIterateInitializer(const WarmStartIterateInitializer&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const WarmStartIterateInitializer&);
+ //@}
+
+ /**@name Algorithmic Parameters */
+ //@{
+ /** Parameters for bumping x0 in warm start mode */
+ Number warm_start_bound_push_;
+ /** Parameters for bumping x0 in warm start mode */
+ Number warm_start_bound_frac_;
+ /** Parameters for bumping initial bound multipliers */
+ Number warm_start_mult_bound_push_;
+ /** Maximal size of entries in bound and equality constraint
+ * multipliers in magnitute. If chosen less of equal to zero, no
+ * upper limit is imposed. Otherwise, the entries exceeding the
+ * given limit are set to the value closest to the limit. */
+ Number warm_start_mult_init_max_;
+ /** Target values for the barrier parameter in warm start option.
+ */
+ Number warm_start_target_mu_;
+ /** Indicator for which method in the NLP should be used to get
+ * the warm start */
+ bool warm_start_entire_iterate_;
+ //@}
+
+ /** @name Auxilliary functions */
+ //@{
+ void process_target_mu(Number factor,
+ const Vector& curr_vars,
+ const Vector& curr_slacks,
+ const Vector& curr_mults,
+ const Matrix& P,
+ SmartPtr<const Vector>& ret_vars,
+ SmartPtr<const Vector>& ret_mults);
+
+ void adapt_to_target_mu(Vector& new_s,
+ Vector& new_z,
+ Number target_mu);
+ //@}
+ };
+
+} // namespace Ipopt
+
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpZeroMatrix.cpp b/SimTKmath/Optimizers/src/IpOpt/IpZeroMatrix.cpp
new file mode 100644
index 0000000..c4d9db6
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpZeroMatrix.cpp
@@ -0,0 +1,66 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpZeroMatrix.cpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#include "IpZeroMatrix.hpp"
+
+namespace Ipopt
+{
+
+ ZeroMatrix::ZeroMatrix(const MatrixSpace* owner_space)
+ :
+ Matrix(owner_space)
+ {}
+
+ ZeroMatrix::~ZeroMatrix()
+ {}
+
+ void ZeroMatrix::MultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ // A few sanity checks
+ DBG_ASSERT(NCols()==x.Dim());
+ DBG_ASSERT(NRows()==y.Dim());
+
+ // Take care of the y part of the addition
+ if( beta!=0.0 ) {
+ y.Scal(beta);
+ }
+ else {
+ y.Set(0.0); // In case y hasn't been initialized yet
+ }
+ }
+
+ void ZeroMatrix::TransMultVectorImpl(Number alpha, const Vector &x,
+ Number beta, Vector &y) const
+ {
+ // A few sanity checks
+ DBG_ASSERT(NCols()==y.Dim());
+ DBG_ASSERT(NRows()==x.Dim());
+
+ // Take care of the y part of the addition
+ if( beta!=0.0 ) {
+ y.Scal(beta);
+ }
+ else {
+ y.Set(0.0); // In case y hasn't been initialized yet
+ }
+ }
+
+ void ZeroMatrix::PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const
+ {
+ jnlst.Printf(level, category, "\n");
+ jnlst.PrintfIndented(level, category, indent,
+ "%sZeroMatrix \"%s\" with %d row and %d columns components:\n",
+ prefix.c_str(), name.c_str(), NRows(), NCols());
+ }
+} // namespace Ipopt
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpZeroMatrix.hpp b/SimTKmath/Optimizers/src/IpOpt/IpZeroMatrix.hpp
new file mode 100644
index 0000000..e92a0f9
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpZeroMatrix.hpp
@@ -0,0 +1,122 @@
+// Copyright (C) 2004, 2006 International Business Machines and others.
+// All Rights Reserved.
+// This code is published under the Common Public License.
+//
+// $Id: IpZeroMatrix.hpp 759 2006-07-07 03:07:08Z andreasw $
+//
+// Authors: Carl Laird, Andreas Waechter IBM 2004-08-13
+
+#ifndef __IPZEROMATRIX_HPP__
+#define __IPZEROMATRIX_HPP__
+
+#include "IpUtils.hpp"
+#include "IpMatrix.hpp"
+
+namespace Ipopt
+{
+
+ /** Class for Matrices with only zero entries.
+ */
+ class ZeroMatrix : public Matrix
+ {
+ public:
+
+ /**@name Constructors / Destructors */
+ //@{
+
+ /** Constructor, taking the corresponding matrix space.
+ */
+ ZeroMatrix(const MatrixSpace* owner_space);
+
+ /** Destructor */
+ ~ZeroMatrix();
+ //@}
+
+ protected:
+ /**@name Methods overloaded from matrix */
+ //@{
+ virtual void MultVectorImpl(Number alpha, const Vector& x,
+ Number beta, Vector& y) const;
+
+ virtual void TransMultVectorImpl(Number alpha, const Vector& x,
+ Number beta, Vector& y) const;
+
+ virtual void PrintImpl(const Journalist& jnlst,
+ EJournalLevel level,
+ EJournalCategory category,
+ const std::string& name,
+ Index indent,
+ const std::string& prefix) const;
+ //@}
+
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ ZeroMatrix();
+
+ /** Copy Constructor */
+ ZeroMatrix(const ZeroMatrix&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const ZeroMatrix&);
+ //@}
+ };
+
+ /** Class for matrix space for ZeroMatrix. */
+ class ZeroMatrixSpace : public MatrixSpace
+ {
+ public:
+ /** @name Constructors / Destructors */
+ //@{
+ /** Constructor, given the number of row and columns.
+ */
+ ZeroMatrixSpace(Index nrows, Index ncols)
+ :
+ MatrixSpace(nrows, ncols)
+ {}
+
+ /** Destructor */
+ virtual ~ZeroMatrixSpace()
+ {}
+ //@}
+
+ /** Overloaded MakeNew method for the MatrixSpace base class.
+ */
+ virtual Matrix* MakeNew() const
+ {
+ return MakeNewZeroMatrix();
+ }
+
+ /** Method for creating a new matrix of this specific type. */
+ ZeroMatrix* MakeNewZeroMatrix() const
+ {
+ return new ZeroMatrix(this);
+ }
+ private:
+ /**@name Default Compiler Generated Methods
+ * (Hidden to avoid implicit creation/calling).
+ * These methods are not implemented and
+ * we do not want the compiler to implement
+ * them for us, so we declare them private
+ * and do not define them. This ensures that
+ * they will not be implicitly created/called. */
+ //@{
+ /** Default Constructor */
+ ZeroMatrixSpace();
+
+ /** Copy Constructor */
+ ZeroMatrixSpace(const ZeroMatrixSpace&);
+
+ /** Overloaded Equals Operator */
+ void operator=(const ZeroMatrixSpace&);
+ //@}
+ };
+} // namespace Ipopt
+#endif
diff --git a/SimTKmath/Optimizers/src/IpOpt/IpoptConfig.h b/SimTKmath/Optimizers/src/IpOpt/IpoptConfig.h
new file mode 100644
index 0000000..0a8c7a6
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/IpoptConfig.h
@@ -0,0 +1,113 @@
+/*
+ * Include file for the configuration of Ipopt.
+ *
+ * On systems where the code is configured with the configure script
+ * (i.e., compilation is always done with HAVE_CONFIG_H defined), this
+ * header file includes the automatically generated header file, and
+ * undefines macros that might configure with other Config.h files.
+ *
+ * On systems that are compiled in other ways (e.g., with the
+ * Developer Studio), a header files is included to define those
+ * macros that depend on the operating system and the compiler. The
+ * macros that define the configuration of the particular user setting
+ * (e.g., presence of other COIN packages or third party code) are set
+ * here. The project maintainer needs to remember to update this file
+ * and choose reasonable defines. A user can modify the default
+ * setting by editing this file here.
+ *
+ */
+
+#ifndef __IPOPTCONFIG_H__
+#define __IPOPTCONFIG_H__
+
+// VC++ tries to be secure by leaving bounds checking on for STL containers
+// even in Release mode. This macro exists to disable that feature and can
+// result in a considerable speedup.
+// CAUTION: every linked-together compilation unit must have this set the same
+// way. Everyone who properly includes SimTKcommon/internal/common.h
+// first is fine but IpOpt doesn't do that. (TODO: it should)
+// (sherm 081204 disabling for now: doesn't work on VC++ 8 and is
+// tricky on VC++ 9 because all libraries, including 3rd party, must
+// be built the same way)
+//#ifdef _WIN32
+// #ifdef NDEBUG
+// #undef _SECURE_SCL
+// #define _SECURE_SCL 0
+// #endif
+//#endif
+
+#ifdef HAVE_CONFIG_H
+#include "config_ipopt.h"
+
+/* undefine macros that could conflict with those in other config.h
+ files */
+#undef PACKAGE
+#undef PACKAGE_BUGREPORT
+#undef PACKAGE_NAME
+#undef PACKAGE_STRING
+#undef PACKAGE_TARNAME
+#undef PACKAGE_VERSION
+#undef VERSION
+
+#else /* HAVE_CONFIG_H */
+
+/* include the COIN-wide system specific configure header */
+#include "configall_system.h"
+
+/***************************************************************************/
+/* HERE DEFINE THE CONFIGURATION SPECIFIC MACROS */
+/***************************************************************************/
+
+/* Define to the debug sanity check level (0 is no test) */
+#define COIN_IPOPT_CHECKLEVEL 0
+
+/* Define to the debug verbosity level (0 is no output) */
+#define COIN_IPOPT_VERBOSITY 0
+
+/* If defined, the Ampl Solver Library is available. */
+#define COIN_HAS_ASL 1
+
+/* If defined, the BLAS Library is available. */
+#define COIN_HAS_BLAS 1
+
+/* Define to 1 if the Ipopt package is used */
+#define COIN_HAS_IPOPT 1
+
+/* If defined, the LAPACK Library is available. */
+#define COIN_HAS_LAPACK 1
+
+/* Define to 1 if MA27 is available */
+#define HAVE_MA27 1
+
+/* Define to 1 if MA57 is available */
+/* #undef HAVE_MA57 */
+
+/* Define to 1 if MC19 is available */
+#define HAVE_MC19 1
+
+/* Define to 1 if MUMPS is available */
+/* #undef HAVE_MUMPS */
+
+/* Define to 1 if Pardiso is available */
+/* #undef HAVE_PARDISO */
+
+/* Define to 1 if you are using the parallel version of Pardiso */
+/* #undef HAVE_PARDISO_PARALLEL */
+
+/* Define to 1 if TAUCS is available */
+/* #undef HAVE_TAUCS */
+
+/* Define to 1 if WSMP is available */
+/* #undef HAVE_WSMP */
+
+#endif /* HAVE_CONFIG_H */
+
+/* In the Ipopt code, we use IP_DEBUG to indicate debug compile */
+#if COIN_IPOPT_DEBUG > 0
+#define IP_DEBUG 1
+#endif
+
+/* Type of Fortran integer translated into C */
+typedef FORTRAN_INTEGER_TYPE ipfint;
+
+#endif /*__IPOPTCONFIG_H__*/
diff --git a/SimTKmath/Optimizers/src/IpOpt/LICENSE.txt b/SimTKmath/Optimizers/src/IpOpt/LICENSE.txt
new file mode 100644
index 0000000..aa3d6f6
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/LICENSE.txt
@@ -0,0 +1,234 @@
+IPOPT LICENSE AGREEMENT
+-----------------------
+
+IPOPT is distributed from https://projects.coin-or.org/Ipopt. The version we
+downloaded and modified to produce the source code in this directory was
+covered by the Common Public License, but that has been superceded by the
+Eclipse Public License version 1.0 available here:
+http://www.eclipse.org/legal/epl-v10.html and included below;
+we have chosen to use that one.
+
+This license applies only to the IpOpt-derived source code in this directory;
+although this is a very permissive license, the rest of Simbody is covered by
+an even more permissive license (derived from the MIT license). If EPL 1.0
+is too restrictive for your purposes, just don't included IpOpt in your
+Simbody build.
+
+----- EPL 1.0 -----
+
+Eclipse Public License - v 1.0
+THE ACCOMPANYING PROGRAM IS PROVIDED UNDER THE TERMS OF THIS ECLIPSE PUBLIC
+LICENSE ("AGREEMENT"). ANY USE, REPRODUCTION OR DISTRIBUTION OF THE PROGRAM
+CONSTITUTES RECIPIENT'S ACCEPTANCE OF THIS AGREEMENT.
+
+1. DEFINITIONS
+
+"Contribution" means:
+
+a) in the case of the initial Contributor, the initial code and documentation
+distributed under this Agreement, and
+
+b) in the case of each subsequent Contributor:
+
+i) changes to the Program, and
+
+ii) additions to the Program;
+
+where such changes and/or additions to the Program originate from and are
+distributed by that particular Contributor. A Contribution 'originates' from
+a Contributor if it was added to the Program by such Contributor itself or
+anyone acting on such Contributor's behalf. Contributions do not include
+additions to the Program which: (i) are separate modules of software
+distributed in conjunction with the Program under their own license agreement,
+and (ii) are not derivative works of the Program.
+
+"Contributor" means any person or entity that distributes the Program.
+
+"Licensed Patents" mean patent claims licensable by a Contributor which are
+necessarily infringed by the use or sale of its Contribution alone or when
+combined with the Program.
+
+"Program" means the Contributions distributed in accordance with this
+Agreement.
+
+"Recipient" means anyone who receives the Program under this Agreement,
+including all Contributors.
+
+2. GRANT OF RIGHTS
+
+a) Subject to the terms of this Agreement, each Contributor hereby grants
+Recipient a non-exclusive, worldwide, royalty-free copyright license to
+reproduce, prepare derivative works of, publicly display, publicly perform,
+distribute and sublicense the Contribution of such Contributor, if any, and
+such derivative works, in source code and object code form.
+
+b) Subject to the terms of this Agreement, each Contributor hereby grants
+Recipient a non-exclusive, worldwide, royalty-free patent license under
+Licensed Patents to make, use, sell, offer to sell, import and otherwise
+transfer the Contribution of such Contributor, if any, in source code and
+object code form. This patent license shall apply to the combination of the
+Contribution and the Program if, at the time the Contribution is added by the
+Contributor, such addition of the Contribution causes such combination to be
+covered by the Licensed Patents. The patent license shall not apply to any
+other combinations which include the Contribution. No hardware per se is
+licensed hereunder.
+
+c) Recipient understands that although each Contributor grants the licenses
+to its Contributions set forth herein, no assurances are provided by any
+Contributor that the Program does not infringe the patent or other
+intellectual property rights of any other entity. Each Contributor
+disclaims any liability to Recipient for claims brought by any other
+entity based on infringement of intellectual property rights or otherwise.
+As a condition to exercising the rights and licenses granted hereunder, each
+Recipient hereby assumes sole responsibility to secure any other intellectual
+property rights needed, if any. For example, if a third party patent license
+is required to allow Recipient to distribute the Program, it is Recipient's
+responsibility to acquire that license before distributing the Program.
+
+d) Each Contributor represents that to its knowledge it has sufficient
+copyright rights in its Contribution, if any, to grant the copyright
+license set forth in this Agreement.
+
+3. REQUIREMENTS
+
+A Contributor may choose to distribute the Program in object code form
+under its own license agreement, provided that:
+
+a) it complies with the terms and conditions of this Agreement; and
+
+b) its license agreement:
+
+i) effectively disclaims on behalf of all Contributors all warranties and
+conditions, express and implied, including warranties or conditions of
+title and non-infringement, and implied warranties or conditions of
+merchantability and fitness for a particular purpose;
+
+ii) effectively excludes on behalf of all Contributors all liability for
+damages, including direct, indirect, special, incidental and consequential
+damages, such as lost profits;
+
+iii) states that any provisions which differ from this Agreement are
+offered by that Contributor alone and not by any other party; and
+
+iv) states that source code for the Program is available from such
+Contributor, and informs licensees how to obtain it in a reasonable
+manner on or through a medium customarily used for software exchange.
+
+When the Program is made available in source code form:
+
+a) it must be made available under this Agreement; and
+
+b) a copy of this Agreement must be included with each copy of the Program.
+
+Contributors may not remove or alter any copyright notices contained within
+the Program.
+
+Each Contributor must identify itself as the originator of its Contribution,
+if any, in a manner that reasonably allows subsequent Recipients to
+identify the originator of the Contribution.
+
+4. COMMERCIAL DISTRIBUTION
+
+Commercial distributors of software may accept certain responsibilities
+with respect to end users, business partners and the like. While this
+license is intended to facilitate the commercial use of the Program, the
+Contributor who includes the Program in a commercial product offering
+should do so in a manner which does not create potential liability for other
+Contributors. Therefore, if a Contributor includes the Program in a
+commercial product offering, such Contributor ("Commercial Contributor")
+hereby agrees to defend and indemnify every other Contributor ("Indemnified
+Contributor") against any losses, damages and costs (collectively "Losses")
+arising from claims, lawsuits and other legal actions brought by a third
+party against the Indemnified Contributor to the extent caused by the acts
+or omissions of such Commercial Contributor in connection with its
+distribution of the Program in a commercial product offering. The
+obligations in this section do not apply to any claims or Losses relating
+to any actual or alleged intellectual property infringement. In order to
+qualify, an Indemnified Contributor must: a) promptly notify the Commercial
+Contributor in writing of such claim, and b) allow the Commercial
+Contributor to control, and cooperate with the Commercial Contributor in,
+the defense and any related settlement negotiations. The Indemnified
+Contributor may participate in any such claim at its own expense.
+
+For example, a Contributor might include the Program in a commercial
+product offering, Product X. That Contributor is then a Commercial
+Contributor. If that Commercial Contributor then makes performance claims,
+or offers warranties related to Product X, those performance claims and
+warranties are such Commercial Contributor's responsibility alone. Under
+this section, the Commercial Contributor would have to defend claims
+against the other Contributors related to those performance claims and
+warranties, and if a court requires any other Contributor to pay any
+damages as a result, the Commercial Contributor must pay those damages.
+
+5. NO WARRANTY
+
+EXCEPT AS EXPRESSLY SET FORTH IN THIS AGREEMENT, THE PROGRAM IS PROVIDED ON
+AN "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, EITHER
+EXPRESS OR IMPLIED INCLUDING, WITHOUT LIMITATION, ANY WARRANTIES OR
+CONDITIONS OF TITLE, NON-INFRINGEMENT, MERCHANTABILITY OR FITNESS FOR A
+PARTICULAR PURPOSE. Each Recipient is solely responsible for determining the
+appropriateness of using and distributing the Program and assumes all risks
+associated with its exercise of rights under this Agreement , including but
+not limited to the risks and costs of program errors, compliance with
+applicable laws, damage to or loss of data, programs or equipment, and
+unavailability or interruption of operations.
+
+6. DISCLAIMER OF LIABILITY
+
+EXCEPT AS EXPRESSLY SET FORTH IN THIS AGREEMENT, NEITHER RECIPIENT NOR ANY
+CONTRIBUTORS SHALL HAVE ANY LIABILITY FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING WITHOUT LIMITATION
+LOST PROFITS), HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OR DISTRIBUTION OF THE PROGRAM OR THE
+EXERCISE OF ANY RIGHTS GRANTED HEREUNDER, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGES.
+
+7. GENERAL
+
+If any provision of this Agreement is invalid or unenforceable under
+applicable law, it shall not affect the validity or enforceability of the
+remainder of the terms of this Agreement, and without further action by the
+parties hereto, such provision shall be reformed to the minimum extent
+necessary to make such provision valid and enforceable.
+
+If Recipient institutes patent litigation against any entity (including a
+cross-claim or counterclaim in a lawsuit) alleging that the Program itself
+(excluding combinations of the Program with other software or hardware)
+infringes such Recipient's patent(s), then such Recipient's rights granted
+under Section 2(b) shall terminate as of the date such litigation is filed.
+
+All Recipient's rights under this Agreement shall terminate if it fails to
+comply with any of the material terms or conditions of this Agreement and
+does not cure such failure in a reasonable period of time after becoming
+aware of such noncompliance. If all Recipient's rights under this
+Agreement terminate, Recipient agrees to cease use and distribution of the
+Program as soon as reasonably practicable. However, Recipient's obligations
+under this Agreement and any licenses granted by Recipient relating to the
+Program shall continue and survive.
+
+Everyone is permitted to copy and distribute copies of this Agreement, but
+in order to avoid inconsistency the Agreement is copyrighted and may only
+be modified in the following manner. The Agreement Steward reserves the
+right to publish new versions (including revisions) of this Agreement from
+time to time. No one other than the Agreement Steward has the right to
+modify this Agreement. The Eclipse Foundation is the initial Agreement
+Steward. The Eclipse Foundation may assign the responsibility to serve as
+the Agreement Steward to a suitable separate entity. Each new version of
+the Agreement will be given a distinguishing version number. The Program
+(including Contributions) may always be distributed subject to the version
+of the Agreement under which it was received. In addition, after a new
+version of the Agreement is published, Contributor may elect to distribute
+the Program (including its Contributions) under the new version. Except
+as expressly stated in Sections 2(a) and 2(b) above, Recipient receives
+no rights or licenses to the intellectual property of any Contributor
+under this Agreement, whether expressly, by implication, estoppel or
+otherwise. All rights in the Program not expressly granted under this
+Agreement are reserved.
+
+This Agreement is governed by the laws of the State of New York and the
+intellectual property laws of the United States of America. No party to
+this Agreement will bring a legal action under this Agreement more than
+one year after the cause of action arose. Each party waives its rights
+to a jury trial in any resulting litigation.
+
diff --git a/SimTKmath/Optimizers/src/IpOpt/configall_system.h b/SimTKmath/Optimizers/src/IpOpt/configall_system.h
new file mode 100644
index 0000000..78243e3
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/configall_system.h
@@ -0,0 +1,9 @@
+/*
+ * This header file is included by the *Config.h in the individual
+ * COIN packages when the code is compiled in a setting that doesn't
+ * use the configure script (i.e., HAVE_CONFIG_H is not defined).
+ * This header file includes the system and compile dependent header
+ * file defining macros that depend on what compiler is used.
+ */
+
+# include "configall_system_msc.h"
diff --git a/SimTKmath/Optimizers/src/IpOpt/configall_system_msc.h b/SimTKmath/Optimizers/src/IpOpt/configall_system_msc.h
new file mode 100644
index 0000000..090d7fe
--- /dev/null
+++ b/SimTKmath/Optimizers/src/IpOpt/configall_system_msc.h
@@ -0,0 +1,130 @@
+/* This is the header file for the Microsoft compiler, defining all
+ * system and compiler dependent configuration macros */
+
+/* Define to dummy `main' function (if any) required to link to the Fortran
+ libraries. */
+/* #undef F77_DUMMY_MAIN */
+
+/* Define to a macro mangling the given C identifier (in lower and upper
+ case), which must not contain underscores, for linking with Fortran. */
+#define F77_FUNC(name,NAME) name##_
+
+/* As F77_FUNC, but for C identifiers containing underscores. */
+#define F77_FUNC_(name,NAME) name##_
+
+/* Define if F77 and FC dummy `main' functions are identical. */
+/* #undef FC_DUMMY_MAIN_EQ_F77 */
+
+/* Define to the C type corresponding to Fortran INTEGER */
+#define FORTRAN_INTEGER_TYPE int
+
+/* Define to 1 if you have the <assert.h> header file. */
+/* #undef HAVE_ASSERT_H */
+
+/* Define to 1 if you have the <cassert> header file. */
+#define HAVE_CASSERT 1
+
+/* Define to 1 if you have the <cctype> header file. */
+#define HAVE_CCTYPE 1
+
+/* Define to 1 if you have the <cfloat> header file. */
+#define HAVE_CFLOAT 1
+
+/* Define to 1 if you have the <cieeefp> header file. */
+/* #undef HAVE_CIEEEFP */
+
+/* Define to 1 if you have the <cmath> header file. */
+#define HAVE_CMATH 1
+
+/* Define to 1 if you have the <cstdarg> header file. */
+#define HAVE_CSTDARG 1
+
+/* Define to 1 if you have the <cstdio> header file. */
+#define HAVE_CSTDIO 1
+
+/* Define to 1 if you have the <cstdlib> header file. */
+/* #undef HAVE_CSTDLIB */
+
+/* Define to 1 if you have the <ctime> header file. */
+#define HAVE_CTIME 1
+
+/* Define to 1 if you have the <ctype.h> header file. */
+/* #undef HAVE_CTYPE_H */
+
+/* Define to 1 if you have the <dlfcn.h> header file. */
+/* #define HAVE_DLFCN_H */
+
+/* Define to 1 if you have the <float.h> header file. */
+/* #undef HAVE_FLOAT_H */
+
+/* Define to 1 if you have the <ieeefp.h> header file. */
+/* #undef HAVE_IEEEFP_H */
+
+/* Define to 1 if you have the <inttypes.h> header file. */
+/* #define HAVE_INTTYPES_H */
+
+/* Define to 1 if you have the <math.h> header file. */
+/* #undef HAVE_MATH_H */
+
+/* Define to 1 if you have the <memory.h> header file. */
+#define HAVE_MEMORY_H 1
+
+/* Define to 1 if you have the <stdarg.h> header file. */
+/* #undef HAVE_STDARG_H */
+
+/* Define to 1 if you have the <stdint.h> header file. */
+/* #define HAVE_STDINT_H */
+
+/* Define to 1 if you have the <stdio.h> header file. */
+/* #undef HAVE_STDIO_H */
+
+/* Define to 1 if you have the <stdlib.h> header file. */
+#define HAVE_STDLIB_H 1
+
+/* Define to 1 if you have the <strings.h> header file. */
+/* #define HAVE_STRINGS_H */
+
+/* Define to 1 if you have the <string.h> header file. */
+#define HAVE_STRING_H 1
+
+/* Define to 1 if you have the <sys/stat.h> header file. */
+#define HAVE_SYS_STAT_H 1
+
+/* Define to 1 if you have the <sys/types.h> header file. */
+#define HAVE_SYS_TYPES_H 1
+
+/* Define to 1 if you have the <time.h> header file. */
+/* #undef HAVE_TIME_H */
+
+/* Define to 1 if you have the <unistd.h> header file. */
+/* #define HAVE_UNISTD_H */
+
+// TODO temporary defined HAVE_VA_COPY FOR TESTING
+/* Define to 1 if va_copy is avaliable */
+// #undef HAVE_VA_COPY
+#define HAVE_VA_COPY 1
+
+/* The size of a `double', as computed by sizeof. */
+#define SIZEOF_DOUBLE 8
+
+/* The size of a `int', as computed by sizeof. */
+#define SIZEOF_INT 4
+
+/* The size of a `int *', as computed by sizeof. */
+//#define SIZEOF_INT_P 4
+// TODO do not hard code this, set in configuration
+#define SIZEOF_INT_P 8
+
+/* The size of a `long', as computed by sizeof. */
+// TODO do not hard code this, set in configuration
+//#define SIZEOF_LONG 4
+#define SIZEOF_LONG 8
+
+/* Define to 1 if you have the ANSI C header files. */
+#define STDC_HEADERS 1
+
+
+#define MY_C_ISNAN _isnan
+/*
+#define MY_C_FINITE _finite
+*/
diff --git a/SimTKmath/Optimizers/src/LBFGSBOptimizer.cpp b/SimTKmath/Optimizers/src/LBFGSBOptimizer.cpp
new file mode 100644
index 0000000..1d2b439
--- /dev/null
+++ b/SimTKmath/Optimizers/src/LBFGSBOptimizer.cpp
@@ -0,0 +1,131 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "LBFGSBOptimizer.h"
+#include <cstring>
+
+using std::cout;
+using std::endl;
+namespace SimTK {
+
+Optimizer::OptimizerRep* LBFGSBOptimizer::clone() const {
+ return( new LBFGSBOptimizer(*this) );
+}
+
+LBFGSBOptimizer::LBFGSBOptimizer( const OptimizerSystem& sys )
+: OptimizerRep( sys ),
+ factr( 1.0e7) {
+ int n,i;
+
+ n = sys.getNumParameters();
+
+ if( n < 1 ) {
+ const char* where = "Optimizer Initialization";
+ const char* szName = "dimension";
+ SimTK_THROW5(SimTK::Exception::ValueOutOfRange, szName, 1, n, INT_MAX, where);
+ }
+
+ /* We don't yet know what kinds of bounds we'll have so set the bounds
+ descriptor nbd to an illegal value. */
+ nbd = new int[n];
+ for(i=0;i<n;i++)
+ nbd[i] = -1;
+}
+
+Real LBFGSBOptimizer::optimize( Vector &results ) {
+ int run_optimizer = 1;
+ char task[61];
+ Real f;
+ int *iwa;
+ char csave[61];
+ bool lsave[4];
+ int isave[44];
+ Real dsave[29];
+ Real *wa;
+ Real *lowerLimits, *upperLimits;
+ const OptimizerSystem& sys = getOptimizerSystem();
+ int n = sys.getNumParameters();
+ int m = limitedMemoryHistory;
+ Real *gradient;
+ gradient = new Real[n];
+
+ iprint[0] = iprint[1] = iprint[2] = diagnosticsLevel;
+
+ if( sys.getHasLimits() ) {
+ sys.getParameterLimits( &lowerLimits, &upperLimits );
+ // Determine what kind of limits each parameter has.
+ // nbd = 0, unbounded; 1, only lower; 2, both; 3 only upper
+ for (int i=0; i < n; ++i) {
+ if (lowerLimits[i] == -Infinity)
+ nbd[i] = (upperLimits[i] == Infinity ? 0 : 3);
+ else nbd[i] = (upperLimits[i] == Infinity ? 1 : 2);
+ }
+ } else {
+ lowerLimits = 0;
+ upperLimits = 0;
+ for(int i=0;i<n;i++)
+ nbd[i] = 0; // unbounded
+ }
+
+ iwa = new int[3*n];
+ wa = new Real[((2*m + 4)*n + 12*m*m + 12*m)];
+
+ Real factor;
+ if( getAdvancedRealOption("factr", factor ) ) {
+ SimTK_APIARGCHECK_ALWAYS(factor > 0,"LBFGSBOptimizer","optimize",
+ "factr must be positive \n");
+ factr = factor;
+ }
+ strcpy( task, "START" );
+ while( run_optimizer ) {
+ setulb_(&n, &m, &results[0], lowerLimits,
+ upperLimits, nbd, &f, gradient,
+ &factr, &convergenceTolerance, wa, iwa,
+ task, iprint, csave, lsave, isave, dsave, 60, 60);
+
+ if( strncmp( task, "FG", 2) == 0 ) {
+ objectiveFuncWrapper( n, &results[0], true, &f, this);
+ gradientFuncWrapper( n, &results[0], false, gradient, this);
+ } else if( strncmp( task, "NEW_X", 5) == 0 ){
+ //objectiveFuncWrapper( n, &results[0], true, &f, (void*)this );
+ } else {
+ run_optimizer = 0;
+ if( strncmp( task, "CONV", 4) != 0 ){
+ delete[] gradient;
+ delete[] iwa;
+ delete[] wa;
+ SimTK_THROW1(SimTK::Exception::OptimizerFailed , SimTK::String(task) );
+ }
+ }
+ }
+ delete[] gradient;
+ delete[] iwa;
+ delete[] wa;
+
+ return f;
+}
+
+
+} // namespace SimTK
diff --git a/SimTKmath/Optimizers/src/LBFGSBOptimizer.h b/SimTKmath/Optimizers/src/LBFGSBOptimizer.h
new file mode 100644
index 0000000..26cd999
--- /dev/null
+++ b/SimTKmath/Optimizers/src/LBFGSBOptimizer.h
@@ -0,0 +1,60 @@
+#ifndef _SimTK_LBFGSB_OPTIMIZER_H_
+#define _SimTK_LBFGSB_OPTIMIZER_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: Michael Sherman *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/internal/OptimizerRep.h"
+
+namespace SimTK {
+
+class LBFGSBOptimizer: public Optimizer::OptimizerRep {
+public:
+ ~LBFGSBOptimizer() {
+ delete [] nbd;
+ }
+
+ LBFGSBOptimizer(const OptimizerSystem& sys);
+
+ Real optimize( Vector &results );
+ OptimizerRep* clone() const;
+
+ OptimizerAlgorithm getAlgorithm() const
+ { return LBFGSB; }
+
+ int setulb_(int *n, int *m, Real *x, Real *l,
+ Real *u, int *nbd, Real *f, Real *g,
+ Real *factr, Real *pgtol, Real *wa, int *iwa,
+ char *task, int *iprint, char *csave, bool *lsave,
+ int *isave, Real *dsave, long task_len, long csave_len);
+
+private:
+ Real factr;
+ int iprint[3];
+ int *nbd;
+};
+
+} // namespace SimTK
+#endif //_SimTK_LBFGSB_OPTIMIZER_H_
+
diff --git a/SimTKmath/Optimizers/src/LBFGSOptimizer.cpp b/SimTKmath/Optimizers/src/LBFGSOptimizer.cpp
new file mode 100644
index 0000000..f1306d5
--- /dev/null
+++ b/SimTKmath/Optimizers/src/LBFGSOptimizer.cpp
@@ -0,0 +1,74 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 "LBFGSOptimizer.h"
+
+using std::cout;
+using std::endl;
+
+namespace SimTK {
+
+Optimizer::OptimizerRep* LBFGSOptimizer::clone() const {
+ return( new LBFGSOptimizer(*this) );
+}
+
+
+LBFGSOptimizer::LBFGSOptimizer( const OptimizerSystem& sys )
+ : OptimizerRep( sys ) ,
+ xtol(SignificantReal)
+
+{
+ /* internal flags for LBFGS */
+ iprint[0] = iprint[1] = 0; // no output generated
+
+ if( sys.getNumParameters() < 1 ) {
+ const char* where = "Optimizer Initialization";
+ const char* szName = "dimension";
+ SimTK_THROW5(SimTK::Exception::ValueOutOfRange, szName, 1,
+ sys.getNumParameters(), INT_MAX, where);
+ }
+}
+
+Real LBFGSOptimizer::optimize( Vector &results ) {
+ int iflag[1] = {0};
+ Real f;
+ const OptimizerSystem& sys = getOptimizerSystem();
+ int n = sys.getNumParameters();
+ int m = limitedMemoryHistory;
+
+ iprint[0] = iprint[1] = iprint[2] = diagnosticsLevel;
+
+ Real tol;
+ if( getAdvancedRealOption("xtol", tol ) ) {
+ SimTK_APIARGCHECK_ALWAYS(tol > 0,"LBFGSOptimizer","optimize",
+ "xtol must be positive \n");
+ xtol = tol;
+ }
+
+ lbfgs_( n, m, &results[0], &f, iprint, &convergenceTolerance, &xtol );
+
+ objectiveFuncWrapper(n, &results[0], true, &f, this);
+ return f;
+}
+
+} // namespace SimTK
diff --git a/SimTKmath/Optimizers/src/LBFGSOptimizer.h b/SimTKmath/Optimizers/src/LBFGSOptimizer.h
new file mode 100644
index 0000000..4817d56
--- /dev/null
+++ b/SimTKmath/Optimizers/src/LBFGSOptimizer.h
@@ -0,0 +1,63 @@
+#ifndef SimTK_SIMMATH_LBFGS_OPTIMIZER_H_
+#define SimTK_SIMMATH_LBFGS_OPTIMIZER_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+
+#include "simmath/internal/OptimizerRep.h"
+
+#include <iostream>
+
+namespace SimTK {
+
+
+class LBFGSOptimizer: public Optimizer::OptimizerRep {
+public:
+ ~LBFGSOptimizer() { }
+
+ LBFGSOptimizer(const OptimizerSystem& sys);
+
+ Real optimize( SimTK::Vector &results );
+ OptimizerRep* clone() const;
+
+ OptimizerAlgorithm getAlgorithm() const
+ { return LBFGS; }
+
+ private:
+ int iprint[3];
+ Real xtol;
+ void lbfgs_( int n, int m, Real *x, Real *f, int *iprint, Real *eps, Real *xtol );
+ void mcsrch_(int *n, Real *x, Real *f, Real *g, Real *s, Real *stp,
+ Real *ftol, Real *xtol, int *maxfev, int *info, int *nfev, Real *wa);
+
+ void setXtol( double );
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_LBFGS_OPTIMIZER_H_
+
diff --git a/SimTKmath/Optimizers/src/Optimizer.cpp b/SimTKmath/Optimizers/src/Optimizer.cpp
new file mode 100644
index 0000000..4a2c861
--- /dev/null
+++ b/SimTKmath/Optimizers/src/Optimizer.cpp
@@ -0,0 +1,208 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-13 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: Michael Sherman *
+ * *
+ * 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 "LBFGSOptimizer.h"
+#include "LBFGSBOptimizer.h"
+#include "InteriorPointOptimizer.h"
+#include "CFSQPOptimizer.h"
+#include <string>
+
+namespace SimTK {
+
+Optimizer::~Optimizer() {
+ delete (OptimizerRep *)rep;
+}
+
+bool Optimizer::isAlgorithmAvailable(OptimizerAlgorithm algorithm) {
+ switch(algorithm) {
+ case InteriorPoint: return InteriorPointOptimizer::isAvailable();
+ case LBFGS: return LBFGSOptimizer::isAvailable();
+ case LBFGSB: return LBFGSBOptimizer::isAvailable();
+#if SimTK_DEFAULT_PRECISION==2 // double only
+ case CFSQP: return CFSQPOptimizer::isAvailable();
+#endif
+ default: return false;
+ }
+}
+
+Optimizer::Optimizer( const OptimizerSystem& sys) : rep(0) {
+ rep = constructOptimizerRep(sys, BestAvailable );
+}
+Optimizer::Optimizer( const OptimizerSystem& sys, OptimizerAlgorithm algorithm) : rep(0) {
+ rep = constructOptimizerRep(sys, algorithm);
+}
+
+Optimizer::Optimizer() : rep(0) {
+ rep = (OptimizerRep *) new DefaultOptimizer();
+}
+
+void Optimizer::setOptimizerSystem( const OptimizerSystem& sys ) {
+ delete rep;
+ rep = constructOptimizerRep( sys, BestAvailable );
+}
+void Optimizer::setOptimizerSystem( const OptimizerSystem& sys, OptimizerAlgorithm algorithm ) {
+ delete rep;
+ rep = constructOptimizerRep( sys, algorithm );
+}
+
+const OptimizerSystem&
+Optimizer::getOptimizerSystem() const {
+ assert(rep);
+ return rep->getOptimizerSystem();
+}
+
+// copy constructor
+Optimizer::Optimizer( const Optimizer& c ) : rep(0) {
+ if (c.rep)
+ rep = c.rep->clone();
+}
+
+// copy assignment operator
+Optimizer& Optimizer::operator=(const Optimizer& rhs) {
+ if (&rhs != this) {
+ delete rep; rep = 0;
+ if (rhs.rep)
+ rep = rhs.rep->clone();
+ }
+ return *this;
+}
+
+Optimizer::OptimizerRep*
+Optimizer::constructOptimizerRep( const OptimizerSystem& sys, OptimizerAlgorithm algorithm ) {
+ OptimizerRep* newRep = 0;
+
+ // if constructor specifies which algorithm, use it else select based on
+ // problem paramters
+ if ( algorithm == InteriorPoint ) {
+ newRep = (OptimizerRep *) new InteriorPointOptimizer( sys );
+ } else if( algorithm == LBFGSB ) {
+ newRep = (OptimizerRep *) new LBFGSBOptimizer( sys );
+ } else if( algorithm == LBFGS ) {
+ newRep = (OptimizerRep *) new LBFGSOptimizer( sys );
+ }
+#if SimTK_DEFAULT_PRECISION==2 // double only
+ else if( algorithm == CFSQP ) {
+ try {
+ newRep = (OptimizerRep *) new CFSQPOptimizer( sys );
+ } catch (const SimTK::Exception::Base &ex) {
+ std::cout << ex.getMessage() << std::endl;
+ std::cout << "Failed to load CFSQP library. "
+ "Will fall back to default optimizer choice." << std::endl;
+ newRep = 0;
+ }
+ }
+#endif
+
+ if(!newRep) {
+ if( sys.getNumConstraints() > 0) {
+ newRep = (OptimizerRep *) new InteriorPointOptimizer( sys );
+ } else if( sys.getHasLimits() ) {
+ newRep = (OptimizerRep *) new LBFGSBOptimizer( sys );
+ } else {
+ newRep = (OptimizerRep *) new LBFGSOptimizer( sys );
+ }
+ }
+ newRep->setMyHandle(*this);
+ newRep->sysp = &sys;
+
+ return newRep;
+}
+
+void Optimizer::useNumericalGradient(bool flag, Real objEstAccuracy) {
+ updRep().useNumericalGradient(flag, objEstAccuracy);
+}
+void Optimizer::useNumericalJacobian(bool flag, Real consEstAccuracy) {
+ updRep().useNumericalJacobian(flag, consEstAccuracy);
+}
+
+void Optimizer::setDifferentiatorMethod( Differentiator::Method method) {
+ updRep().setDifferentiatorMethod(method);
+}
+
+void Optimizer::setConvergenceTolerance( Real accuracy ) {
+ updRep().setConvergenceTolerance(accuracy);
+}
+
+void Optimizer::setConstraintTolerance( Real tolerance ) {
+ updRep().setConstraintTolerance(tolerance);
+}
+
+void Optimizer::setMaxIterations( int iter ) {
+ updRep().setMaxIterations(iter);
+}
+
+
+void Optimizer::setLimitedMemoryHistory( int history ) {
+ updRep().setLimitedMemoryHistory(history);
+}
+
+void Optimizer::setDiagnosticsLevel( int level ) {
+ updRep().setDiagnosticsLevel(level);
+}
+
+bool Optimizer::setAdvancedStrOption( const char *option, const char *value ) {
+ return updRep().setAdvancedStrOption( option, value);
+}
+
+bool Optimizer::setAdvancedRealOption( const char *option, const Real value ) {
+ return updRep().setAdvancedRealOption( option, value);
+}
+
+bool Optimizer::setAdvancedIntOption( const char *option, const int value ) {
+ return updRep().setAdvancedIntOption( option, value);
+}
+
+bool Optimizer::setAdvancedBoolOption( const char *option, const bool value ) {
+ return updRep().setAdvancedBoolOption( option, value);
+}
+
+Real Optimizer::optimize(SimTK::Vector &results) {
+ return updRep().optimize(results);
+}
+
+bool Optimizer::isUsingNumericalGradient() const {
+ return getRep().isUsingNumericalGradient();
+}
+bool Optimizer::isUsingNumericalJacobian() const {
+ return getRep().isUsingNumericalJacobian();
+}
+
+Differentiator::Method Optimizer::getDifferentiatorMethod() const {
+ return getRep().getDifferentiatorMethod();
+}
+
+OptimizerAlgorithm Optimizer::getAlgorithm() const {
+ return getRep().getAlgorithm();
+}
+
+Real Optimizer::getEstimatedAccuracyOfObjective() const {
+ return getRep().getEstimatedAccuracyOfObjective();
+}
+
+Real Optimizer::getEstimatedAccuracyOfConstraints() const {
+ return getRep().getEstimatedAccuracyOfConstraints();
+}
+
+} // namespace SimTK
diff --git a/SimTKmath/Optimizers/src/OptimizerRep.cpp b/SimTKmath/Optimizers/src/OptimizerRep.cpp
new file mode 100644
index 0000000..651f3b7
--- /dev/null
+++ b/SimTKmath/Optimizers/src/OptimizerRep.cpp
@@ -0,0 +1,267 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-13 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: Michael Sherman *
+ * *
+ * 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 "simmath/internal/OptimizerRep.h"
+
+namespace SimTK {
+
+//////////////////////
+// DefaultOptimizer //
+//////////////////////
+
+Optimizer::OptimizerRep* DefaultOptimizer::clone() const {
+ return( new DefaultOptimizer(*this));
+}
+Real DefaultOptimizer::optimize( Vector &results ) {
+ SimTK_APIARGCHECK_ALWAYS(false,"Optimizer","optimize",
+ "the OptimizerSystem has not been set \n");
+}
+OptimizerAlgorithm DefaultOptimizer::getAlgorithm() const {
+ SimTK_APIARGCHECK_ALWAYS(false,"Optimizer","getAlgorithm",
+ "the OptimizerSystem has not been set \n");
+}
+
+//////////////////
+// OptimizerRep //
+//////////////////
+
+Optimizer::OptimizerRep::~OptimizerRep() {
+ delete jacDiff;
+ delete gradDiff;
+ delete cf;
+ delete of;
+}
+
+void Optimizer::OptimizerRep::setConvergenceTolerance(Real accuracy ) {
+ convergenceTolerance = accuracy;
+}
+void Optimizer::OptimizerRep::setConstraintTolerance(Real tolerance ) {
+ constraintTolerance = tolerance;
+}
+void Optimizer::OptimizerRep::setMaxIterations( const int iter ) {
+ maxIterations = iter;
+}
+void Optimizer::OptimizerRep::setLimitedMemoryHistory( const int history ) {
+ limitedMemoryHistory = history;
+}
+
+void Optimizer::OptimizerRep::setDiagnosticsLevel( const int level ) {
+ diagnosticsLevel = level;
+}
+
+// These helper methods are private to this compilation unit.
+template<class T>
+static bool setAdvancedOptionHelper(std::map<std::string,T> &optionMap, const std::string &option, const T &value) {
+ optionMap[option] = value;
+ return true;
+}
+
+template<class T>
+static bool getAdvancedOptionHelper(const std::map<std::string,T> &optionMap, const std::string &option, T &value) {
+ typename std::map<std::string,T>::const_iterator iter = optionMap.find(option);
+ if(iter != optionMap.end()) {
+ value = iter->second;
+ return true;
+ } else
+ return false;
+}
+
+bool Optimizer::OptimizerRep::setAdvancedStrOption( const std::string &option, const std::string &value ) {
+ return setAdvancedOptionHelper(advancedStrOptions, option, value);
+}
+bool Optimizer::OptimizerRep::setAdvancedRealOption( const std::string &option, const Real value ) {
+ return setAdvancedOptionHelper(advancedRealOptions, option, value);
+}
+bool Optimizer::OptimizerRep::setAdvancedIntOption( const std::string &option, const int value ) {
+ return setAdvancedOptionHelper(advancedIntOptions, option, value);
+}
+bool Optimizer::OptimizerRep::setAdvancedBoolOption( const std::string &option, const bool value ) {
+ return setAdvancedOptionHelper(advancedBoolOptions, option, value);
+}
+
+bool Optimizer::OptimizerRep::getAdvancedStrOption( const std::string &option, std::string &value ) const {
+ return getAdvancedOptionHelper(advancedStrOptions, option, value);
+}
+bool Optimizer::OptimizerRep::getAdvancedRealOption( const std::string &option, Real &value ) const {
+ return getAdvancedOptionHelper(advancedRealOptions, option, value);
+}
+bool Optimizer::OptimizerRep::getAdvancedIntOption( const std::string &option, int &value ) const {
+ return getAdvancedOptionHelper(advancedIntOptions, option, value);
+}
+bool Optimizer::OptimizerRep::getAdvancedBoolOption( const std::string &option, bool &value ) const {
+ return getAdvancedOptionHelper(advancedBoolOptions, option, value);
+}
+
+// TODO: this only works if called *prior* to the routines below.
+void Optimizer::OptimizerRep::
+setDifferentiatorMethod(Differentiator::Method method) {
+ diffMethod = method;
+}
+
+void Optimizer::OptimizerRep::
+useNumericalGradient(bool flag, Real objEstAccuracy) {
+ objectiveEstimatedAccuracy =
+ objEstAccuracy > 0 ? objEstAccuracy : SignificantReal;
+ delete gradDiff; gradDiff=0; delete of; of=0;
+ if (flag) { // turn on numerical jacbobian
+ of = new SysObjectiveFunc(sysp->getNumParameters(), sysp);
+ of->setEstimatedAccuracy(objectiveEstimatedAccuracy);
+ gradDiff = new Differentiator(*of, diffMethod);
+ }
+ numericalGradient = flag;
+}
+void Optimizer::OptimizerRep::
+useNumericalJacobian(bool flag, Real consEstAccuracy) {
+ constraintsEstimatedAccuracy =
+ consEstAccuracy > 0 ? consEstAccuracy : SignificantReal;
+ delete jacDiff; jacDiff=0; delete cf; cf=0;
+ if (flag) { // turn on numerical gradients
+ cf = new SysConstraintFunc(sysp->getNumConstraints(),
+ sysp->getNumParameters(), sysp);
+ cf->setEstimatedAccuracy(constraintsEstimatedAccuracy);
+ jacDiff = new Differentiator(*cf, diffMethod);
+ }
+ numericalJacobian = flag;
+}
+
+
+int Optimizer::OptimizerRep::objectiveFuncWrapper
+ (int n, const Real* x, int newX, Real* f, void* vrep)
+{
+ assert(vrep);
+ const OptimizerRep* rep = reinterpret_cast<const OptimizerRep*>(vrep);
+
+ // This Vector is just a reference to existing space.
+ const Vector params(n, x, true);
+ const bool isNewParam = (newX==1);
+ Real& frep = *f;
+
+ return (rep->getOptimizerSystem().objectiveFunc(params, isNewParam, frep)==0)
+ ? 1 : 0;
+}
+
+int Optimizer::OptimizerRep::gradientFuncWrapper
+ (int n, const Real* x, int newX, Real* gradient, void* vrep)
+{
+ assert(vrep);
+ const OptimizerRep* rep = reinterpret_cast<const OptimizerRep*>(vrep);
+
+ // These Vectors are just references to existing space.
+ const Vector params(n, x, true);
+ Vector grad_vec(n,gradient,true);
+ Real fy0;
+ const bool isNewParam = (newX==1);
+
+ const OptimizerSystem& osys = rep->getOptimizerSystem();
+
+ if( rep->isUsingNumericalGradient() ) {
+ osys.objectiveFunc(params, true, fy0);
+ rep->getGradientDifferentiator().calcGradient(params, fy0, grad_vec);
+ return 1;
+ }
+
+ return (osys.gradientFunc(params, isNewParam, grad_vec)==0)
+ ? 1 : 0;
+}
+
+int Optimizer::OptimizerRep::constraintFuncWrapper
+ (int n, const Real* x, int newX, int m, Real* g, void* vrep)
+{
+ assert(vrep);
+ const OptimizerRep* rep = reinterpret_cast<const OptimizerRep*>(vrep);
+
+ // These Vectors are just references to existing space.
+ const Vector params( n, x, true);
+ Vector constraints(m, g, true);
+ const bool isNewParam = (newX==1);
+
+ return (rep->getOptimizerSystem().constraintFunc(params, isNewParam, constraints)==0)
+ ? 1 : 0;
+}
+
+int Optimizer::OptimizerRep::constraintJacobianWrapper
+ (int n, const Real* x, int newX, int m, int nele_jac,
+ int* iRow, int* jCol, Real* values, void* vrep)
+{
+ assert(vrep);
+ const OptimizerRep* rep = reinterpret_cast<const OptimizerRep*>(vrep);
+
+ if(m==0) return 1; // m==0 case occurs if you run IPOPT with no constraints
+
+ const bool isNewParam = (newX==1);
+
+ if (values == NULL) {
+ // always assume the jacobian is dense
+ int index = 0;
+ for(int j=0; j<m; ++j)
+ for(int i=0; i<n; ++i) {
+ iRow[index] = j;
+ jCol[index++] = i;
+ }
+ return 1; // success
+ }
+
+ // Calculate the Jacobian of the constraints.
+
+ const Vector params(n,x,true); // This Vector refers to existing space
+ Matrix jac(m,n); // This is a new local temporary. TODO: get rid of this
+
+ int status = -1;
+ if( rep->isUsingNumericalJacobian() ) {
+ Vector sfy0(m);
+ status = rep->getOptimizerSystem().constraintFunc(params, true, sfy0);
+ rep->getJacobianDifferentiator().calcJacobian( params, sfy0, jac);
+ } else {
+ status = rep->getOptimizerSystem().constraintJacobian(params, isNewParam, jac);
+ }
+
+ // Transpose the jacobian because Ipopt indexes in Row major format.
+ Real *ptr = values;
+ for(int j=0; j<m; ++j)
+ for(int i=0; i<n; ++i,++ptr)
+ *ptr = jac(j,i);
+
+ return (status==0) ? 1 : 0;
+}
+
+// TODO finish hessianWrapper
+int Optimizer::OptimizerRep::hessianWrapper
+ (int n, const Real* x, int newX, Real obj_factor,
+ int m, Real* lambda, int new_lambda,
+ int nele_hess, int* iRow, int* jCol,
+ Real* values, void* vrep)
+{
+ assert(vrep);
+ const OptimizerRep* rep = reinterpret_cast<const OptimizerRep*>(vrep);
+
+ // These Vectors refer to existing space.
+ const Vector coeff(n,x,true);
+ Vector hess(n*n,values,true);
+ const bool isNewParam = (newX==1);
+
+ return rep->getOptimizerSystem().hessian(coeff, isNewParam, hess)==0
+ ? 1 : 0;
+}
+
+} // namespace SimTK
diff --git a/SimTKmath/Optimizers/src/lbfgs.cpp b/SimTKmath/Optimizers/src/lbfgs.cpp
new file mode 100644
index 0000000..f03baa9
--- /dev/null
+++ b/SimTKmath/Optimizers/src/lbfgs.cpp
@@ -0,0 +1,1371 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+
+
+#include "LBFGSOptimizer.h"
+#include "../src/Simmath_f2c.h"
+
+#include <iostream>
+#include <cmath>
+
+#define NUMBER_OF_CORRECTIONS 5
+
+#if SimTK_DEFAULT_PRECISION==1 // float
+#define DAXPY saxpy_
+#define DDOT sdot_
+#else // double
+#define DAXPY daxpy_
+#define DDOT ddot_
+#endif
+
+using SimTK::Real;
+
+struct lb3_1_ {
+/*
+C GTOL is a DOUBLE PRECISION variable with default value 0.9, which
+C controls the accuracy of the line search routine MCSRCH. If the
+C function and gradient evaluations are inexpensive with respect
+C to the cost of the iteration (which is sometimes the case when
+C solving very large problems) it may be advantageous to set GTOL
+C to a small value. A typical small value is 0.1. Restriction:
+C GTOL should be greater than 1.D-04.
+C
+C STPMIN and STPMAX are non-negative DOUBLE PRECISION variables which
+C specify lower and upper bounds for the step in the line search.
+C Their default values are 1.D-20 and 1.D+20, respectively. These
+C values need not be modified unless the exponents are too large
+C for the machine being used, or unless the problem is extremely
+C badly scaled (in which case the exponents should be increased).
+*/
+ int mp, lp; /* Fortran i/o stuff. Unused here. */
+ Real gtol, stpmin, stpmax;
+ Real stpawf; /* line search default step length, added by awf */
+};
+
+/*#define lb3_1 (*(struct lb3_1_ *) &lb3_)*/
+#define lb3_1 lb3_
+
+
+using std::cout;
+using std::endl;
+
+/* Initialized data */
+const struct lb3_1_ lb3_1 = {0, 0, Real(.9), Real(1e-20), Real(1e20), Real(1)};
+
+/* Table of constant values */
+static const integer c__1 = 1;
+
+#include "SimTKlapack.h"
+
+
+static void mcstep_(Real *stx, Real *fx, Real *dx, Real *sty, Real *fy, Real *dy,
+ Real *stp, Real *fp, Real *dp, bool *brackt,
+ Real *stpmin, Real *stpmax, integer *info);
+void lb1_(int *iprint, int *iter, int *nfun, Real *gnorm, int *n, int *m,
+ Real *x, Real *f, Real *g, Real *stp, bool *finish);
+void lbptf_(char* msg);
+void lbp1d_(char* msg, int* i);
+
+
+/* ----------------------------------------------------------------------*/
+/* This file contains the LBFGS algorithm and supporting routines */
+
+/* **************** */
+/* LBFGS SUBROUTINE */
+/* **************** */
+
+
+
+void SimTK::LBFGSOptimizer::lbfgs_
+ (int n, int m, SimTK::Real *x, SimTK::Real *f,
+ int *iprint, SimTK::Real *eps, SimTK::Real *xtol)
+{
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ Real beta;
+ integer inmc;
+ integer iscn, nfev, iycn, iter;
+ Real ftol;
+ integer nfun, ispt, iypt;
+ integer bound;
+ Real gnorm;
+ integer point;
+ integer cp;
+ Real sq, yr, ys;
+ Real yy;
+ integer maxfev;
+ integer npt;
+ Real stp, stp1;
+
+/* LIMITED MEMORY BFGS METHOD FOR LARGE SCALE OPTIMIZATION */
+/* JORGE NOCEDAL */
+/* *** July 1990 *** */
+
+/* This subroutine solves the unconstrained minimization problem */
+
+/* min F(x), x= (x1,x2,...,xN), */
+
+/* using the limited memory BFGS method. The routine is especially */
+/* effective on problems involving a large number of variables. In */
+/* a typical iteration of this method an approximation Hk to the */
+/* inverse of the Hessian is obtained by applying M BFGS updates to */
+/* a diagonal matrix Hk0, using information from the previous M steps. */
+/* The user specifies the number M, which determines the amount of */
+/* storage required by the routine. The user may also provide the */
+/* diagonal matrices Hk0 if not satisfied with the default choice. */
+/* The algorithm is described in "On the limited memory BFGS method */
+/* for large scale optimization", by D. Liu and J. Nocedal, */
+/* Mathematical Programming B 45 (1989) 503-528. */
+
+/* The user is required to calculate the function value F and its */
+/* gradient G. In order to allow the user complete control over */
+/* these computations, reverse communication is used. The routine */
+/* must be called repeatedly under the control of the parameter */
+/* IFLAG. */
+
+/* The steplength is determined at each iteration by means of the */
+/* line search routine MCVSRCH, which is a slight modification of */
+/* the routine CSRCH written by More' and Thuente. */
+
+/* The calling statement is */
+
+/* CALL LBFGS(N,M,X,F,G,DIAGCO,DIAG,IPRINT,EPS,XTOL,W,IFLAG) */
+
+/* where */
+
+/* N is an INTEGER variable that must be set by the user to the */
+/* number of variables. It is not altered by the routine. */
+/* Restriction: N>0. */
+
+/* M is an INTEGER variable that must be set by the user to */
+/* the number of corrections used in the BFGS update. It */
+/* is not altered by the routine. Values of M less than 3 are */
+/* not recommended; large values of M will result in excessive */
+/* computing time. 3<= M <=7 is recommended. Restriction: M>0. */
+
+/* X is a DOUBLE PRECISION array of length N. On initial entry */
+/* it must be set by the user to the values of the initial */
+/* estimate of the solution vector. On exit with IFLAG=0, it */
+/* contains the values of the variables at the best point */
+/* found (usually a solution). */
+
+/* F is a DOUBLE PRECISION variable. Before initial entry and on */
+/* a re-entry with IFLAG=1, it must be set by the user to */
+/* contain the value of the function F at the point X. */
+
+/* IPRINT is an INTEGER array of length two which must be set by the */
+/* user. */
+
+/* IPRINT(1) specifies the frequency of the output: */
+/* IPRINT(1) < 0 : no output is generated, */
+/* IPRINT(1) = 0 : output only at first and last iteration, */
+/* IPRINT(1) > 0 : output every IPRINT(1) iterations. */
+
+/* IPRINT(2) specifies the type of output generated: */
+/* IPRINT(2) = 0 : iteration count, number of function */
+/* evaluations, function value, norm of the */
+/* gradient, and steplength, */
+/* IPRINT(2) = 1 : same as IPRINT(2)=0, plus vector of */
+/* variables and gradient vector at the */
+/* initial point, */
+/* IPRINT(2) = 2 : same as IPRINT(2)=1, plus vector of */
+/* variables, */
+/* IPRINT(2) = 3 : same as IPRINT(2)=2, plus gradient vector.*/
+
+
+/* EPS is a positive DOUBLE PRECISION variable that must be set by */
+/* the user, and determines the accuracy with which the solution*/
+/* is to be found. The subroutine terminates when */
+
+ //sherm 100303: this criteria makes no sense to me. It looks like the X
+ // is on the wrong side.
+/* ||G|| < EPS max(1,||X||), */
+
+/* where ||.|| denotes the Euclidean norm. */
+
+/* XTOL is a positive DOUBLE PRECISION variable that must be set by */
+/* the user to an estimate of the machine precision (e.g. */
+/* 10**(-16) on a SUN station 3/60). The line search routine will*/
+/* terminate if the relative width of the interval of uncertainty*/
+/* is less than XTOL. */
+
+
+/* ON THE DRIVER: */
+
+/* The program that calls LBFGS must contain the declaration: */
+
+/* EXTERNAL LB2 */
+
+/* LB2 is a BLOCK DATA that defines the default values of several */
+/* parameters described in the COMMON section. */
+
+/* COMMON: */
+
+/* The subroutine contains one common area, which the user may wish to */
+/* reference: */
+
+/* awf added stpawf */
+
+/* MP is an INTEGER variable with default value 6. It is used as the */
+/* unit number for the printing of the monitoring information */
+/* controlled by IPRINT. */
+
+/* LP is an INTEGER variable with default value 6. It is used as the */
+/* unit number for the printing of error messages. This printing */
+/* may be suppressed by setting LP to a non-positive value. */
+
+/* GTOL is a DOUBLE PRECISION variable with default value 0.9, which */
+/* controls the accuracy of the line search routine MCSRCH. If the */
+/* function and gradient evaluations are inexpensive with respect */
+/* to the cost of the iteration (which is sometimes the case when */
+/* solving very large problems) it may be advantageous to set GTOL */
+/* to a small value. A typical small value is 0.1. Restriction: */
+/* GTOL should be greater than 1.D-04. */
+
+/* STPMIN and STPMAX are non-negative DOUBLE PRECISION variables which */
+/* specify lower and uper bounds for the step in the line search. */
+/* Their default values are 1.D-20 and 1.D+20, respectively. These */
+/* values need not be modified unless the exponents are too large */
+/* for the machine being used, or unless the problem is extremely */
+/* badly scaled (in which case the exponents should be increased). */
+
+
+/* MACHINE DEPENDENCIES */
+
+/* The only variables that are machine-dependent are XTOL, */
+/* STPMIN and STPMAX. */
+
+
+/* GENERAL INFORMATION */
+
+/* Other routines called directly: DAXPY, DDOT, LB1, MCSRCH */
+
+/* Input/Output : No input; diagnostic messages on unit MP and */
+/* error messages on unit LP. */
+
+
+/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
+
+/* BLOCK DATA LB3 */
+
+/* INITIALIZE */
+/* ---------- */
+
+ char buf[256];
+ Real *diag, *gradient, *w;
+ int info;
+ bool converged = false;
+ ispt = n + (m << 1);
+ iypt = ispt + n * m;
+ ftol = Real(1e-4);
+ maxfev = 20;
+
+ iter = 0;
+ if (n <= 0 || m <= 0) {
+ SimTK_THROW1(SimTK::Exception::OptimizerFailed , "IMPROPER INPUT PARAMETERS N OR M ARE NOT POSITIVE");
+ }
+ diag = new Real[n];
+ w = new Real[n*(2*m+1) + 2*m];
+ gradient = new Real[n];
+ nfun = 1;
+ point = 0;
+
+ /* compute initial function and gradient values */
+ objectiveFuncWrapper( n, x, true, f, this);
+ gradientFuncWrapper( n, x, false, gradient, this);
+
+ // sherm 100303:
+ // Scale the absolute gradient df/dx by f and x to get a relative
+ // gradient (%chg f)/(%chg x). Then if 100% change of an x doesn't
+ // produce at least a change eps*f we can say that x has converged.
+ // This is the convergence criteria used for BFGS in Numerical Recipes
+ // in C++ 2nd ed. p433 and makes more sense to me than the one that
+ // came with this LBFGS implementation.
+ // This is a scaled infinity-norm.
+
+ Real fscale = 1 / std::max(Real(0.1), std::abs(*f));
+
+ // Make a quick attempt to quit if we're already converged. No need
+ // to calculate the whole norm here; we'll stop as soon as we find
+ // a non-converged element.
+ converged = true;
+ for (int i=0; i<n; ++i) {
+ const Real xscale = std::max(Real(1), std::abs(x[i]));
+ if( std::abs(gradient[i])*fscale*xscale > *eps ) {
+ converged=false;
+ break;
+ }
+ }
+ if( converged ) {
+ delete [] diag;
+ delete [] gradient;
+ delete [] w;
+ return; // check if starting at minimum
+ }
+
+ // This is the unscaled 2-norm of the gradient.
+ gnorm = std::sqrt(DDOT(n, gradient, c__1, gradient, c__1));
+
+ stp1 = Real(1) / gnorm;
+
+ for (int i = 0; i < n; ++i) {
+ diag[i] = 1.;
+ }
+
+/* THE WORK VECTOR W IS DIVIDED AS FOLLOWS: */
+/* --------------------------------------- */
+/* THE FIRST N LOCATIONS ARE USED TO STORE THE GRADIENT AND */
+/* OTHER TEMPORARY INFORMATION. */
+/* LOCATIONS (N+1)...(N+M) STORE THE SCALARS RHO. */
+/* LOCATIONS (N+M+1)...(N+2M) STORE THE NUMBERS ALPHA USED */
+/* IN THE FORMULA THAT COMPUTES H*G. */
+/* LOCATIONS (N+2M+1)...(N+2M+NM) STORE THE LAST M SEARCH */
+/* STEPS. */
+/* LOCATIONS (N+2M+NM+1)...(N+2M+2NM) STORE THE LAST M */
+/* GRADIENT DIFFERENCES. */
+
+/* THE SEARCH STEPS AND GRADIENT DIFFERENCES ARE STORED IN A */
+/* CIRCULAR ORDER CONTROLLED BY THE PARAMETER POINT. */
+
+ for (int i = 0; i < n; ++i) {
+ w[ispt + i] = -gradient[i] * diag[i];
+ }
+
+/* PARAMETERS FOR LINE SEARCH ROUTINE */
+
+
+ if (iprint[0] > 0) {
+ lb1_(iprint, &iter, &nfun, &gnorm, &n, &m, x, f, gradient, &stp, &converged);
+ }
+
+/* -------------------- */
+/* MAIN ITERATION LOOP */
+/* -------------------- */
+
+ while( !converged ) {
+ ++iter;
+ info = 0;
+ bound = iter - 1;
+ if (iter != 1) {
+ if (iter > m) {
+ bound = m;
+ }
+
+ ys = DDOT(n, &w[iypt + npt], c__1, &w[ispt + npt], c__1);
+ yy = DDOT(n, &w[iypt + npt], c__1, &w[iypt + npt], c__1);
+ for (int i = 0; i < n; ++i) {
+ diag[i] = ys / yy;
+ }
+
+/* COMPUTE -H*G USING THE FORMULA GIVEN IN: Nocedal, J. 1980, */
+/* "Updating quasi-Newton matrices with limited storage", */
+/* Mathematics of Computation, Vol.24, No.151, pp. 773-782. */
+/* --------------------------------------------------------- */
+
+ cp = point;
+ if (point == 0) {
+ cp = m;
+ }
+ w[n + cp-1] = Real(1) / ys;
+ for (int i = 0; i < n; ++i) {
+ w[i] = -gradient[i];
+ }
+ cp = point;
+ for (int i = 0; i < bound; ++i) {
+ --cp;
+ if (cp == -1) {
+ cp = m - 1;
+ }
+ sq = DDOT(n, &w[ispt + cp * n], c__1, w, c__1);
+ inmc = n + m + cp;
+ iycn = iypt + cp * n;
+ w[inmc] = w[n + cp] * sq;
+ d__1 = -w[inmc];
+ DAXPY(n, d__1, &w[iycn], c__1, w, c__1);
+ }
+
+ for (int i = 0; i < n; ++i) {
+ w[i] *= diag[i];
+ }
+
+ for (int i = 0; i < bound; ++i) {
+ yr = DDOT(n, &w[iypt + cp * n], c__1, w, c__1);
+ beta = w[n + cp] * yr;
+ inmc = n + m + cp;
+ beta = w[inmc] - beta;
+ iscn = ispt + cp * n;
+ DAXPY(n, beta, &w[iscn], c__1, w, c__1);
+ ++cp;
+ if (cp == m) {
+ cp = 0;
+ }
+ }
+
+/* STORE THE NEW SEARCH DIRECTION */
+/* ------------------------------ */
+
+ for (int i = 0; i < n; ++i) {
+ w[ispt + point * n + i] = w[i];
+ }
+
+/* OBTAIN THE ONE-DIMENSIONAL MINIMIZER OF THE FUNCTION */
+/* BY USING THE LINE SEARCH ROUTINE MCSRCH */
+/* ---------------------------------------------------- */
+ }
+ nfev = 0;
+/* awf changed initial step from ONE to be parametrized. */
+ stp = lb3_1.stpawf;
+ if (iter == 1) {
+ stp = stp1;
+ }
+ for (int i = 0; i < n; ++i) {
+ w[i] = gradient[i];
+ }
+
+ do {
+ SimTK::LBFGSOptimizer::mcsrch_(&n, x, f, gradient,
+ &w[ispt + point * n],
+ &stp, &ftol, xtol,
+ &maxfev, &info, &nfev, diag);
+ if (info == -1) {
+ objectiveFuncWrapper( n, x, true, f, this);
+ gradientFuncWrapper( n, x, false, gradient, this);
+ } else if (info != 1) {
+ delete [] diag;
+ delete [] gradient;
+ delete [] w;
+ if (lb3_1.lp > 0) {
+ SimTK_THROW1(SimTK::Exception::OptimizerFailed ,
+ "LBFGS LINE SEARCH FAILED POSSIBLE CAUSES: FUNCTION OR GRADIENT ARE INCORRECT OR INCORRECT TOLERANCES");
+ } else if( info == 0) {
+ SimTK_THROW1(SimTK::Exception::OptimizerFailed,
+ "LBFGS ERROR: IMPROPER INPUT PARAMETERS");
+ } else if( info == 2) {
+ SimTK_THROW1(SimTK::Exception::OptimizerFailed,
+ "LBFGS ERROR: RELATIVE WIDTH OF THE INTERVAL OF UNCERTAINTY IS AT MOST XTOL");
+ } else if( info == 3) {
+ SimTK_THROW1(SimTK::Exception::OptimizerFailed,
+ "LBFGS ERROR: MORE THAN 20 FUNCTION EVALUATIONS WERE REQUIRED AT THE PRESENT ITERATION");
+ } else if( info == 4) {
+ SimTK_THROW1(SimTK::Exception::OptimizerFailed,
+ "LBFGS ERROR: THE STEP IS TOO SMALL");
+ } else if( info == 5) {
+ SimTK_THROW1(SimTK::Exception::OptimizerFailed,
+ "LBFGS ERROR: THE STEP IS TOO LARGE");
+ } else if( info == 6){
+ SimTK_THROW1(SimTK::Exception::OptimizerFailed,
+ "LBFGS ERROR: ROUNDING ERRORS PREVENT FURTHER PROGRESS.\n THERE MAY NOT BE A STEP WHICH SATISFIES THE SUFFICIENT DECREASE AND CURVATURE\n CONDITIONS. TOLERANCES MAY BE TOO SMALL.");
+ } else if( info == 7){
+ SimTK_THROW1(SimTK::Exception::OptimizerFailed , "Error in input parameters to MCSRCH");
+ } else if( info == 8){
+ SimTK_THROW1(SimTK::Exception::OptimizerFailed , "THE SEARCH DIRECTION IS NOT A DESCENT DIRECTION");
+ } else if( info == 9){
+ SimTK_THROW1(SimTK::Exception::OptimizerFailed , "Error in input parameters to mcstep_");
+ } else {
+ sprintf(buf, "LBFGS ERROR: info = %d \n",info );
+ SimTK_THROW1(SimTK::Exception::OptimizerFailed, SimTK::String(buf) );
+ }
+ }
+ } while( info == -1 );
+
+ nfun += nfev;
+
+/* COMPUTE THE NEW STEP AND GRADIENT CHANGE */
+/* ----------------------------------------- */
+
+ npt = point * n;
+ for (int i = 0; i < n; ++i) {
+ w[ispt + npt + i] *= stp;
+ w[iypt + npt + i] = gradient[i] - w[i];
+ }
+ ++point;
+ if (point == m) {
+ point = 0;
+ }
+
+/* TERMINATION TEST */
+/* ---------------- */
+
+ //gnorm = std::sqrt(DDOT(n, gradient, c__1, gradient, c__1));
+ //xnorm = std::sqrt(DDOT(n, x, c__1, x, c__1));
+ // xnorm = std::max(1.,xnorm);
+ // if (gnorm / xnorm <= *eps) {
+ // converged = true;
+ // }
+
+
+ // sherm 100303: use scaled infinity norm instead
+ fscale = 1 / std::max(Real(0.1), std::abs(*f));
+ gnorm = 0;
+ for (int i=0; i<n; ++i) {
+ const Real xscale = std::max(Real(1), std::abs(x[i]));
+ gnorm = std::max(gnorm, std::abs(gradient[i])*fscale*xscale);
+ }
+ converged = (gnorm <= *eps);
+
+ if (iprint[0] > 0)
+ lb1_(iprint, &iter, &nfun, &gnorm, &n, &m,
+ x, f, gradient, &stp, &converged);
+
+ } // end while loop
+ delete [] diag;
+ delete [] gradient;
+ delete [] w;
+
+/* ------------------------------------------------------------ */
+/* END OF MAIN ITERATION LOOP. */
+/* ------------------------------------------------------------ */
+
+} /* LAST LINE OF SUBROUTINE lbfgs_ */
+
+
+/* SUBROUTINE LB1(IPRINT,ITER,NFUN,GNORM,N,M,X,F,G,STP,FINISH) */
+/* ** moved to c file */
+
+/* ---------------------------------------------------------- */
+
+/* These routines removed for insertion into TargetJr netlib */
+
+/* awf subroutine daxpy(n,da,dx,incx,dy,incy) */
+/* awf c */
+/* awf c constant times a vector plus a vector. */
+/* awf c uses unrolled loops for increments equal to one. */
+/* awf c jack dongarra, linpack, 3/11/78. */
+/* awf c */
+/* awf Real precision dx(1),dy(1),da */
+/* awf integer i,incx,incy,ix,iy,m,mp1,n */
+/* awf c */
+/* awf if(n.le.0)return */
+/* awf if (da .eq. 0.0d0) return */
+/* awf if(incx.eq.1.and.incy.eq.1)go to 20 */
+/* awf c */
+/* awf c code for unequal increments or equal increments */
+/* awf c not equal to 1 */
+/* awf c */
+/* awf ix = 1 */
+/* awf iy = 1 */
+/* awf if(incx.lt.0)ix = (-n+1)*incx + 1 */
+/* awf if(incy.lt.0)iy = (-n+1)*incy + 1 */
+/* awf do 10 i = 1,n */
+/* awf dy(iy) = dy(iy) + da*dx(ix) */
+/* awf ix = ix + incx */
+/* awf iy = iy + incy */
+/* awf 10 continue */
+/* awf return */
+/* awf c */
+/* awf c code for both increments equal to 1 */
+/* awf c */
+/* awf c */
+/* awf c clean-up loop */
+/* awf c */
+/* awf 20 m = mod(n,4) */
+/* awf if( m .eq. 0 ) go to 40 */
+/* awf do 30 i = 1,m */
+/* awf dy(i) = dy(i) + da*dx(i) */
+/* awf 30 continue */
+/* awf if( n .lt. 4 ) return */
+/* awf 40 mp1 = m + 1 */
+/* awf do 50 i = mp1,n,4 */
+/* awf dy(i) = dy(i) + da*dx(i) */
+/* awf dy(i + 1) = dy(i + 1) + da*dx(i + 1) */
+/* awf dy(i + 2) = dy(i + 2) + da*dx(i + 2) */
+/* awf dy(i + 3) = dy(i + 3) + da*dx(i + 3) */
+/* awf 50 continue */
+/* awf return */
+/* awf end */
+/* awf C */
+/* awf C */
+/* awf C ---------------------------------------------------------- */
+/* awf C */
+/* awf Real precision function ddot(n,dx,incx,dy,incy) */
+/* awf c */
+/* awf c forms the dot product of two vectors. */
+/* awf c uses unrolled loops for increments equal to one. */
+/* awf c jack dongarra, linpack, 3/11/78. */
+/* awf c */
+/* awf Real precision dx(1),dy(1),dtemp */
+/* awf integer i,incx,incy,ix,iy,m,mp1,n */
+/* awf c */
+/* awf ddot = 0.0d0 */
+/* awf dtemp = 0.0d0 */
+/* awf if(n.le.0)return */
+/* awf if(incx.eq.1.and.incy.eq.1)go to 20 */
+/* awf c */
+/* awf c code for unequal increments or equal increments */
+/* awf c not equal to 1 */
+/* awf c */
+/* awf ix = 1 */
+/* awf iy = 1 */
+/* awf if(incx.lt.0)ix = (-n+1)*incx + 1 */
+/* awf if(incy.lt.0)iy = (-n+1)*incy + 1 */
+/* awf do 10 i = 1,n */
+/* awf dtemp = dtemp + dx(ix)*dy(iy) */
+/* awf ix = ix + incx */
+/* awf iy = iy + incy */
+/* awf 10 continue */
+/* awf ddot = dtemp */
+/* awf return */
+/* awf c */
+/* awf c code for both increments equal to 1 */
+/* awf c */
+/* awf c */
+/* awf c clean-up loop */
+/* awf c */
+/* awf 20 m = mod(n,5) */
+/* awf if( m .eq. 0 ) go to 40 */
+/* awf do 30 i = 1,m */
+/* awf dtemp = dtemp + dx(i)*dy(i) */
+/* awf 30 continue */
+/* awf if( n .lt. 5 ) go to 60 */
+/* awf 40 mp1 = m + 1 */
+/* awf do 50 i = mp1,n,5 */
+/* awf dtemp = dtemp + dx(i)*dy(i) + dx(i + 1)*dy(i + 1) + */
+/* awf * dx(i + 2)*dy(i + 2) + dx(i + 3)*dy(i + 3) + dx(i + 4)*dy(i + 4) */
+/* awf 50 continue */
+/* awf 60 ddot = dtemp */
+/* awf return */
+/* awf end */
+/* ------------------------------------------------------------------ */
+
+/* ************************** */
+/* LINE SEARCH ROUTINE MCSRCH */
+/* ************************** */
+
+/* Subroutine */
+void SimTK::LBFGSOptimizer::mcsrch_
+ (integer *n, Real *x, Real *f, Real *g, Real *s, Real *stp,
+ Real *ftol, Real *xtol, integer *maxfev,
+ integer *info, integer *nfev, Real *wa)
+{
+ /* Initialized data */
+
+ const Real xtrapf = 4.;
+
+ /* Local variables */
+ Real dgxm, dgym;
+ integer j, infoc;
+ Real finit, width, stmin, stmax;
+ bool stage1;
+ Real width1, ftest1, dg, fm, fx, fy;
+ bool brackt;
+ Real dginit, dgtest;
+ Real dgm, dgx, dgy, fxm, fym, stx, sty;
+
+
+/* SUBROUTINE MCSRCH */
+
+/* A slight modification of the subroutine CSRCH of More' and Thuente. */
+/* The changes are to allow reverse communication, and do not affect */
+/* the performance of the routine. */
+
+/* THE PURPOSE OF MCSRCH IS TO FIND A STEP WHICH SATISFIES */
+/* A SUFFICIENT DECREASE CONDITION AND A CURVATURE CONDITION. */
+
+/* AT EACH STAGE THE SUBROUTINE UPDATES AN INTERVAL OF */
+/* UNCERTAINTY WITH ENDPOINTS STX AND STY. THE INTERVAL OF */
+/* UNCERTAINTY IS INITIALLY CHOSEN SO THAT IT CONTAINS A */
+/* MINIMIZER OF THE MODIFIED FUNCTION */
+
+/* F(X+STP*S) - F(X) - FTOL*STP*(GRADF(X)'S). */
+
+/* IF A STEP IS OBTAINED FOR WHICH THE MODIFIED FUNCTION */
+/* HAS A NONPOSITIVE FUNCTION VALUE AND NONNEGATIVE DERIVATIVE, */
+/* THEN THE INTERVAL OF UNCERTAINTY IS CHOSEN SO THAT IT */
+/* CONTAINS A MINIMIZER OF F(X+STP*S). */
+
+/* THE ALGORITHM IS DESIGNED TO FIND A STEP WHICH SATISFIES */
+/* THE SUFFICIENT DECREASE CONDITION */
+
+/* F(X+STP*S) .LE. F(X) + FTOL*STP*(GRADF(X)'S), */
+
+/* AND THE CURVATURE CONDITION */
+
+/* ABS(GRADF(X+STP*S)'S)) .LE. GTOL*ABS(GRADF(X)'S). */
+
+/* IF FTOL IS LESS THAN GTOL AND IF, FOR EXAMPLE, THE FUNCTION */
+/* IS BOUNDED BELOW, THEN THERE IS ALWAYS A STEP WHICH SATISFIES */
+/* BOTH CONDITIONS. IF NO STEP CAN BE FOUND WHICH SATISFIES BOTH */
+/* CONDITIONS, THEN THE ALGORITHM USUALLY STOPS WHEN ROUNDING */
+/* ERRORS PREVENT FURTHER PROGRESS. IN THIS CASE STP ONLY */
+/* SATISFIES THE SUFFICIENT DECREASE CONDITION. */
+
+/* THE SUBROUTINE STATEMENT IS */
+
+/* SUBROUTINE MCSRCH(N,X,F,G,S,STP,FTOL,XTOL, MAXFEV,INFO,NFEV,WA) */
+/* WHERE */
+
+/* N IS A POSITIVE INTEGER INPUT VARIABLE SET TO THE NUMBER */
+/* OF VARIABLES. */
+
+/* X IS AN ARRAY OF LENGTH N. ON INPUT IT MUST CONTAIN THE */
+/* BASE POINT FOR THE LINE SEARCH. ON OUTPUT IT CONTAINS */
+/* X + STP*S. */
+
+/* F IS A VARIABLE. ON INPUT IT MUST CONTAIN THE VALUE OF F */
+/* AT X. ON OUTPUT IT CONTAINS THE VALUE OF F AT X + STP*S. */
+
+/* G IS AN ARRAY OF LENGTH N. ON INPUT IT MUST CONTAIN THE */
+/* GRADIENT OF F AT X. ON OUTPUT IT CONTAINS THE GRADIENT */
+/* OF F AT X + STP*S. */
+
+/* S IS AN INPUT ARRAY OF LENGTH N WHICH SPECIFIES THE */
+/* SEARCH DIRECTION. */
+
+/* STP IS A NONNEGATIVE VARIABLE. ON INPUT STP CONTAINS AN */
+/* INITIAL ESTIMATE OF A SATISFACTORY STEP. ON OUTPUT */
+/* STP CONTAINS THE FINAL ESTIMATE. */
+
+/* FTOL AND GTOL ARE NONNEGATIVE INPUT VARIABLES. (In this reverse */
+/* communication implementation GTOL is defined in a COMMON */
+/* statement.) TERMINATION OCCURS WHEN THE SUFFICIENT DECREASE */
+/* CONDITION AND THE DIRECTIONAL DERIVATIVE CONDITION ARE */
+/* SATISFIED. */
+
+/* XTOL IS A NONNEGATIVE INPUT VARIABLE. TERMINATION OCCURS */
+/* WHEN THE RELATIVE WIDTH OF THE INTERVAL OF UNCERTAINTY */
+/* IS AT MOST XTOL. */
+
+/* STPMIN AND STPMAX ARE NONNEGATIVE INPUT VARIABLES WHICH */
+/* SPECIFY LOWER AND UPPER BOUNDS FOR THE STEP. (In this reverse */
+/* communication implementatin they are defined in a COMMON */
+/* statement). */
+
+/* MAXFEV IS A POSITIVE INTEGER INPUT VARIABLE. TERMINATION */
+/* OCCURS WHEN THE NUMBER OF CALLS TO FCN IS AT LEAST */
+/* MAXFEV BY THE END OF AN ITERATION. */
+
+/* INFO IS AN INTEGER OUTPUT VARIABLE SET AS FOLLOWS: */
+
+/* INFO = 0 IMPROPER INPUT PARAMETERS. */
+
+/* INFO =-1 A RETURN IS MADE TO COMPUTE THE FUNCTION AND GRADIENT. */
+/* NFEV IS AN INTEGER OUTPUT VARIABLE SET TO THE NUMBER OF */
+/* CALLS TO FCN. */
+
+/* WA IS A WORK ARRAY OF LENGTH N. */
+
+/* SUBPROGRAMS CALLED */
+
+/* MCSTEP */
+
+/* FORTRAN-SUPPLIED...ABS,MAX,MIN */
+
+/* ARGONNE NATIONAL LABORATORY. MINPACK PROJECT. JUNE 1983 */
+/* JORGE J. MORE', DAVID J. THUENTE */
+
+ infoc = 1;
+
+/* CHECK THE INPUT PARAMETERS FOR ERRORS. */
+
+ if (*n <= 0 || *stp <= 0. || *ftol < 0. || lb3_1.gtol < 0. || *xtol < 0. ||
+ lb3_1.stpmin < 0. || lb3_1.stpmax < lb3_1.stpmin || *maxfev <= 0) {
+ *info = 7;
+ return;
+ }
+
+/* COMPUTE THE INITIAL GRADIENT IN THE SEARCH DIRECTION */
+/* AND CHECK THAT S IS A DESCENT DIRECTION. */
+
+ dginit = 0.;
+ for (j = 0; j < *n; ++j) {
+ dginit += g[j] * s[j];
+ }
+ if (dginit >= 0.) {
+ *info = 8;
+ return;
+ }
+
+/* INITIALIZE LOCAL VARIABLES. */
+
+ brackt = FALSE_;
+ stage1 = TRUE_;
+ *nfev = 0;
+ finit = *f;
+ dgtest = *ftol * dginit;
+ width = lb3_1.stpmax - lb3_1.stpmin;
+ width1 = 2*width;
+ for (j = 0; j < *n; ++j) {
+ wa[j] = x[j];
+ }
+
+/* THE VARIABLES STX, FX, DGX CONTAIN THE VALUES OF THE STEP, */
+/* FUNCTION, AND DIRECTIONAL DERIVATIVE AT THE BEST STEP. */
+/* THE VARIABLES STY, FY, DGY CONTAIN THE VALUE OF THE STEP, */
+/* FUNCTION, AND DERIVATIVE AT THE OTHER ENDPOINT OF */
+/* THE INTERVAL OF UNCERTAINTY. */
+/* THE VARIABLES STP, F, DG CONTAIN THE VALUES OF THE STEP, */
+/* FUNCTION, AND DERIVATIVE AT THE CURRENT STEP. */
+
+ stx = 0.;
+ fx = finit;
+ dgx = dginit;
+ sty = 0.;
+ fy = finit;
+ dgy = dginit;
+
+/* START OF ITERATION. */
+
+L30:
+
+/* SET THE MINIMUM AND MAXIMUM STEPS TO CORRESPOND */
+/* TO THE PRESENT INTERVAL OF UNCERTAINTY. */
+
+ if (brackt) {
+ stmin = std::min(stx,sty);
+ stmax = std::max(stx,sty);
+ } else {
+ stmin = stx;
+ stmax = *stp + xtrapf * (*stp - stx);
+ }
+
+/* FORCE THE STEP TO BE WITHIN THE BOUNDS STPMAX AND STPMIN. */
+
+ *stp = std::max(*stp,lb3_1.stpmin);
+ *stp = std::min(*stp,lb3_1.stpmax);
+
+/* IF AN UNUSUAL TERMINATION IS TO OCCUR THEN LET */
+/* STP BE THE LOWEST POINT OBTAINED SO FAR. */
+
+ if ( (brackt && (*stp <= stmin || *stp >= stmax) ) || *nfev >= *maxfev - 1
+ || infoc == 0 || ( brackt && stmax - stmin <= *xtol * stmax) ) {
+ *stp = stx;
+ }
+
+/* EVALUATE THE FUNCTION AND GRADIENT AT STP */
+/* AND COMPUTE THE DIRECTIONAL DERIVATIVE. */
+
+ for (j = 0; j < *n; ++j) {
+ x[j] = wa[j] + *stp * s[j];
+ }
+
+ objectiveFuncWrapper( *n, x, true, f, this);
+ gradientFuncWrapper( *n, x, false, g, this);
+
+ *info = 0;
+ ++(*nfev);
+ dg = 0.;
+ for (j = 0; j < *n; ++j) {
+ dg += g[j] * s[j];
+ }
+ ftest1 = finit + *stp * dgtest;
+
+/* TEST FOR CONVERGENCE. */
+
+ if ( (brackt && (*stp <= stmin || *stp >= stmax) ) || infoc == 0) {
+ *info = 6;
+ }
+ if (*stp == lb3_1.stpmax && *f <= ftest1 && dg <= dgtest) {
+ *info = 5;
+ }
+ if (*stp == lb3_1.stpmin && (*f > ftest1 || dg >= dgtest)) {
+ *info = 4;
+ }
+ if (*nfev >= *maxfev) {
+ *info = 3;
+ }
+ if (brackt && stmax - stmin <= *xtol * stmax) {
+ *info = 2;
+ }
+ if (*f <= ftest1 && fabs(dg) <= lb3_1.gtol * (-dginit)) {
+ *info = 1;
+ }
+
+/* CHECK FOR TERMINATION. */
+
+ if (*info != 0) {
+ // Moved exception handling one level deeper Sept 2009 cmb
+ if (*info != 1) {
+ // Error condition - might want to breakpoint here
+ return;
+ }
+ return;
+ }
+
+/* IN THE FIRST STAGE WE SEEK A STEP FOR WHICH THE MODIFIED */
+/* FUNCTION HAS A NONPOSITIVE VALUE AND NONNEGATIVE DERIVATIVE. */
+
+ if (stage1 && *f <= ftest1 && dg >= std::min(*ftol,lb3_1.gtol) * dginit) {
+ stage1 = FALSE_;
+ }
+
+/* A MODIFIED FUNCTION IS USED TO PREDICT THE STEP ONLY IF */
+/* WE HAVE NOT OBTAINED A STEP FOR WHICH THE MODIFIED */
+/* FUNCTION HAS A NONPOSITIVE FUNCTION VALUE AND NONNEGATIVE */
+/* DERIVATIVE, AND IF A LOWER FUNCTION VALUE HAS BEEN */
+/* OBTAINED BUT THE DECREASE IS NOT SUFFICIENT. */
+
+ if (stage1 && *f <= fx && *f > ftest1) {
+
+/* DEFINE THE MODIFIED FUNCTION AND DERIVATIVE VALUES. */
+
+ fm = *f - *stp * dgtest;
+ fxm = fx - stx * dgtest;
+ fym = fy - sty * dgtest;
+ dgm = dg - dgtest;
+ dgxm = dgx - dgtest;
+ dgym = dgy - dgtest;
+
+/* CALL CSTEP TO UPDATE THE INTERVAL OF UNCERTAINTY */
+/* AND TO COMPUTE THE NEW STEP. */
+
+ mcstep_(&stx, &fxm, &dgxm, &sty, &fym, &dgym, stp, &fm, &dgm, &brackt, &stmin, &stmax, &infoc);
+
+/* RESET THE FUNCTION AND GRADIENT VALUES FOR F. */
+
+ fx = fxm + stx * dgtest;
+ fy = fym + sty * dgtest;
+ dgx = dgxm + dgtest;
+ dgy = dgym + dgtest;
+ } else {
+
+/* CALL MCSTEP TO UPDATE THE INTERVAL OF UNCERTAINTY */
+/* AND TO COMPUTE THE NEW STEP. */
+
+ mcstep_(&stx, &fx, &dgx, &sty, &fy, &dgy, stp, f, &dg, &brackt, &stmin, &stmax, &infoc);
+ }
+
+/* FORCE A SUFFICIENT DECREASE IN THE SIZE OF THE */
+/* INTERVAL OF UNCERTAINTY. */
+
+ if (brackt) {
+ if (std::abs(sty - stx) >= .66 * width1) {
+ *stp = stx + (sty - stx)/2;
+ }
+ width1 = width;
+ width = std::abs(sty - stx);
+ }
+
+/* END OF ITERATION. */
+
+ goto L30;
+
+/* LAST LINE OF SUBROUTINE MCSRCH. */
+
+} /* mcsrch_ */
+
+/* Subroutine */
+static void mcstep_(Real *stx, Real *fx, Real *dx, Real *sty, Real *fy, Real *dy,
+ Real *stp, Real *fp, Real *dp, bool *brackt,
+ Real *stpmin, Real *stpmax, integer *info)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ Real sgnd, stpc, stpf, stpq, p, q, gamma, r, s, theta;
+ bool bound;
+
+
+/* SUBROUTINE MCSTEP */
+
+/* THE PURPOSE OF MCSTEP IS TO COMPUTE A SAFEGUARDED STEP FOR */
+/* A LINESEARCH AND TO UPDATE AN INTERVAL OF UNCERTAINTY FOR */
+/* A MINIMIZER OF THE FUNCTION. */
+
+/* THE PARAMETER STX CONTAINS THE STEP WITH THE LEAST FUNCTION */
+/* VALUE. THE PARAMETER STP CONTAINS THE CURRENT STEP. IT IS */
+/* ASSUMED THAT THE DERIVATIVE AT STX IS NEGATIVE IN THE */
+/* DIRECTION OF THE STEP. IF BRACKT IS SET TRUE THEN A */
+/* MINIMIZER HAS BEEN BRACKETED IN AN INTERVAL OF UNCERTAINTY */
+/* WITH ENDPOINTS STX AND STY. */
+
+/* THE SUBROUTINE STATEMENT IS */
+
+/* SUBROUTINE MCSTEP(STX,FX,DX,STY,FY,DY,STP,FP,DP,BRACKT, */
+/* STPMIN,STPMAX,INFO) */
+
+/* WHERE */
+
+/* STX, FX, AND DX ARE VARIABLES WHICH SPECIFY THE STEP, */
+/* THE FUNCTION, AND THE DERIVATIVE AT THE BEST STEP OBTAINED */
+/* SO FAR. THE DERIVATIVE MUST BE NEGATIVE IN THE DIRECTION */
+/* OF THE STEP, THAT IS, DX AND STP-STX MUST HAVE OPPOSITE */
+/* SIGNS. ON OUTPUT THESE PARAMETERS ARE UPDATED APPROPRIATELY. */
+
+/* STY, FY, AND DY ARE VARIABLES WHICH SPECIFY THE STEP, */
+/* THE FUNCTION, AND THE DERIVATIVE AT THE OTHER ENDPOINT OF */
+/* THE INTERVAL OF UNCERTAINTY. ON OUTPUT THESE PARAMETERS ARE */
+/* UPDATED APPROPRIATELY. */
+
+/* STP, FP, AND DP ARE VARIABLES WHICH SPECIFY THE STEP, */
+/* THE FUNCTION, AND THE DERIVATIVE AT THE CURRENT STEP. */
+/* IF BRACKT IS SET TRUE THEN ON INPUT STP MUST BE */
+/* BETWEEN STX AND STY. ON OUTPUT STP IS SET TO THE NEW STEP. */
+
+/* BRACKT IS A LOGICAL VARIABLE WHICH SPECIFIES IF A MINIMIZER */
+/* HAS BEEN BRACKETED. IF THE MINIMIZER HAS NOT BEEN BRACKETED */
+/* THEN ON INPUT BRACKT MUST BE SET FALSE. IF THE MINIMIZER */
+/* IS BRACKETED THEN ON OUTPUT BRACKT IS SET TRUE. */
+
+/* STPMIN AND STPMAX ARE INPUT VARIABLES WHICH SPECIFY LOWER */
+/* AND UPPER BOUNDS FOR THE STEP. */
+
+/* INFO IS AN INTEGER OUTPUT VARIABLE SET AS FOLLOWS: */
+/* IF INFO = 1,2,3,4,5, THEN THE STEP HAS BEEN COMPUTED */
+/* ACCORDING TO ONE OF THE FIVE CASES BELOW. OTHERWISE */
+/* INFO = 0, AND THIS INDICATES IMPROPER INPUT PARAMETERS. */
+
+/* SUBPROGRAMS CALLED */
+
+/* FORTRAN-SUPPLIED ... ABS,MAX,MIN,SQRT */
+
+/* ARGONNE NATIONAL LABORATORY. MINPACK PROJECT. JUNE 1983 */
+/* JORGE J. MORE', DAVID J. THUENTE */
+
+ *info = 0;
+
+/* CHECK THE INPUT PARAMETERS FOR ERRORS. */
+
+ if ( ( *brackt && ( *stp <= std::min(*stx,*sty) || *stp >= std::max(*stx,*sty) ) )
+ || *dx * (*stp - *stx) >= 0. || *stpmax < *stpmin) {
+ *info = 9;
+ return;
+ }
+
+/* DETERMINE IF THE DERIVATIVES HAVE OPPOSITE SIGN. */
+
+ sgnd = *dp * (*dx / std::abs(*dx));
+
+/* FIRST CASE. A HIGHER FUNCTION VALUE. */
+/* THE MINIMUM IS BRACKETED. IF THE CUBIC STEP IS CLOSER */
+/* TO STX THAN THE QUADRATIC STEP, THE CUBIC STEP IS TAKEN, */
+/* ELSE THE AVERAGE OF THE CUBIC AND QUADRATIC STEPS IS TAKEN. */
+
+ if (*fp > *fx) {
+ *info = 1;
+ bound = TRUE_;
+ theta = (*fx - *fp) * 3 / (*stp - *stx) + *dx + *dp;
+ s = std::max(std::max(std::abs(theta),std::abs(*dx)),std::abs(*dp));
+ d__1 = theta / s;
+ gamma = s * std::sqrt(d__1 * d__1 - *dx / s * (*dp / s));
+ if (*stp < *stx) {
+ gamma = -gamma;
+ }
+ p = gamma - *dx + theta;
+ q = gamma - *dx + gamma + *dp;
+ r = p / q;
+ stpc = *stx + r * (*stp - *stx);
+ stpq = *stx + *dx / ((*fx - *fp) / (*stp - *stx) + *dx) / 2 * (*stp - *stx);
+ if (std::abs(stpc - *stx) < std::abs(stpq - *stx)) {
+ stpf = stpc;
+ } else {
+ stpf = stpc + (stpq - stpc) / 2;
+ }
+ *brackt = TRUE_;
+
+/* SECOND CASE. A LOWER FUNCTION VALUE AND DERIVATIVES OF */
+/* OPPOSITE SIGN. THE MINIMUM IS BRACKETED. IF THE CUBIC */
+/* STEP IS CLOSER TO STX THAN THE QUADRATIC (SECANT) STEP, */
+/* THE CUBIC STEP IS TAKEN, ELSE THE QUADRATIC STEP IS TAKEN. */
+
+ } else if (sgnd < 0.) {
+ *info = 2;
+ bound = FALSE_;
+ theta = (*fx - *fp) * 3 / (*stp - *stx) + *dx + *dp;
+ s = std::max(std::max(std::abs(theta),std::abs(*dx)),std::abs(*dp));
+ d__1 = theta / s;
+ gamma = s * std::sqrt(d__1 * d__1 - *dx / s * (*dp / s));
+ if (*stp > *stx) {
+ gamma = -gamma;
+ }
+ p = gamma - *dp + theta;
+ q = gamma - *dp + gamma + *dx;
+ r = p / q;
+ stpc = *stp + r * (*stx - *stp);
+ stpq = *stp + *dp / (*dp - *dx) * (*stx - *stp);
+ if (std::abs(stpc - *stp) > std::abs(stpq - *stp)) {
+ stpf = stpc;
+ } else {
+ stpf = stpq;
+ }
+ *brackt = TRUE_;
+
+/* THIRD CASE. A LOWER FUNCTION VALUE, DERIVATIVES OF THE */
+/* SAME SIGN, AND THE MAGNITUDE OF THE DERIVATIVE DECREASES. */
+/* THE CUBIC STEP IS ONLY USED IF THE CUBIC TENDS TO INFINITY */
+/* IN THE DIRECTION OF THE STEP OR IF THE MINIMUM OF THE CUBIC */
+/* IS BEYOND STP. OTHERWISE THE CUBIC STEP IS DEFINED TO BE */
+/* EITHER STPMIN OR STPMAX. THE QUADRATIC (SECANT) STEP IS ALSO */
+/* COMPUTED AND IF THE MINIMUM IS BRACKETED THEN THE THE STEP */
+/* CLOSEST TO STX IS TAKEN, ELSE THE STEP FARTHEST AWAY IS TAKEN. */
+
+ } else if (std::abs(*dp) < std::abs(*dx)) {
+ *info = 3;
+ bound = TRUE_;
+ theta = (*fx - *fp) * 3 / (*stp - *stx) + *dx + *dp;
+ s = std::max(std::max(std::abs(theta),std::abs(*dx)),std::abs(*dp));
+
+/* THE CASE GAMMA = 0 ONLY ARISES IF THE CUBIC DOES NOT TEND */
+/* TO INFINITY IN THE DIRECTION OF THE STEP. */
+
+ d__1 = theta / s;
+ d__1 = d__1 * d__1 - *dx / s * (*dp / s);
+ gamma = s * std::sqrt((std::max(Real(0),d__1)));
+ if (*stp > *stx) {
+ gamma = -gamma;
+ }
+ p = gamma - *dp + theta;
+ q = gamma + (*dx - *dp) + gamma;
+ r = p / q;
+ if (r < 0. && gamma != 0.) {
+ stpc = *stp + r * (*stx - *stp);
+ } else if (*stp > *stx) {
+ stpc = *stpmax;
+ } else {
+ stpc = *stpmin;
+ }
+ stpq = *stp + *dp / (*dp - *dx) * (*stx - *stp);
+ if (*brackt) {
+ if (std::abs(*stp - stpc) < std::abs(*stp - stpq)) {
+ stpf = stpc;
+ } else {
+ stpf = stpq;
+ }
+ } else {
+ if (std::abs(*stp - stpc) > std::abs(*stp - stpq)) {
+ stpf = stpc;
+ } else {
+ stpf = stpq;
+ }
+ }
+
+/* FOURTH CASE. A LOWER FUNCTION VALUE, DERIVATIVES OF THE */
+/* SAME SIGN, AND THE MAGNITUDE OF THE DERIVATIVE DOES */
+/* NOT DECREASE. IF THE MINIMUM IS NOT BRACKETED, THE STEP */
+/* IS EITHER STPMIN OR STPMAX, ELSE THE CUBIC STEP IS TAKEN. */
+
+ } else {
+ *info = 4;
+ bound = FALSE_;
+ if (*brackt) {
+ theta = (*fp - *fy) * 3 / (*sty - *stp) + *dy + *dp;
+ s = std::max(std::max(std::abs(theta),std::abs(*dy)),std::abs(*dp));
+ d__1 = theta / s;
+ gamma = s * std::sqrt(d__1 * d__1 - *dy / s * (*dp / s));
+ if (*stp > *sty) {
+ gamma = -gamma;
+ }
+ p = gamma - *dp + theta;
+ q = gamma - *dp + gamma + *dy;
+ r = p / q;
+ stpc = *stp + r * (*sty - *stp);
+ stpf = stpc;
+ } else if (*stp > *stx) {
+ stpf = *stpmax;
+ } else {
+ stpf = *stpmin;
+ }
+ }
+
+/* UPDATE THE INTERVAL OF UNCERTAINTY. THIS UPDATE DOES NOT */
+/* DEPEND ON THE NEW STEP OR THE CASE ANALYSIS ABOVE. */
+
+ if (*fp > *fx) {
+ *sty = *stp;
+ *fy = *fp;
+ *dy = *dp;
+ } else {
+ if (sgnd < 0.) {
+ *sty = *stx;
+ *fy = *fx;
+ *dy = *dx;
+ }
+ *stx = *stp;
+ *fx = *fp;
+ *dx = *dp;
+ }
+
+/* COMPUTE THE NEW STEP AND SAFEGUARD IT. */
+
+ stpf = std::min(*stpmax,stpf);
+ stpf = std::max(*stpmin,stpf);
+ *stp = stpf;
+ if (*brackt && bound) {
+ if (*sty > *stx) {
+ d__1 = *stx + (*sty - *stx) * .66f;
+ *stp = std::min(d__1,*stp);
+ } else {
+ d__1 = *stx + (*sty - *stx) * .66f;
+ *stp = std::max(d__1,*stp);
+ }
+ }
+} /* mcstep_ */
+
+
+
+void lbptf_(const char* msg)
+{
+ printf(msg, 0); // dummy argument avoids gcc warning
+}
+
+void lbp1d_(const char* msg, int* i)
+{
+ printf(msg, *i);
+}
+
+void lbp1f_(const char* msg, Real* i)
+{
+ printf(msg, *i);
+}
+
+static void write50(Real* v, int n)
+{
+ int cols = 15;
+ Real vmax = 0;
+ int i;
+ Real vmaxscale;
+ for (i = 0; i < n; ++i)
+ if (std::abs(v[i]) > vmax)
+ vmax = v[i];
+ vmaxscale = std::log(std::abs(vmax)) / std::log(Real(10));
+ vmaxscale = std::pow(Real(10), ceil(vmaxscale) - 1);
+ if (vmaxscale != 1.0)
+ printf(" %e x\n", vmaxscale);
+
+ for (i = 0; i < n; ++i) {
+ if (i > 0 && i%cols == 0)
+ printf("\n");
+ printf(" %10.5f", v[i] / vmaxscale);
+ }
+ printf("\n");
+}
+
+/*C
+//C -------------------------------------------------------------
+//C THIS ROUTINE PRINTS MONITORING INFORMATION. THE FREQUENCY AND
+//C AMOUNT OF OUTPUT ARE CONTROLLED BY IPRINT.
+//C -------------------------------------------------------------
+*/
+void lb1_( int *iprint, int *iter, int *nfun, Real *gnorm, int *n,
+ int *m, Real *x, Real *f, Real *g, Real *stp, bool *finish) /* bool*/
+{
+ (void)m;
+ --iprint;
+/* C*/
+/*IF (ITER.EQ.0)THEN*/
+ if (*iter == 0) {
+/* 30 FORMAT(' F= ',1PD10.3,' GNORM= ',1PD10.3)*/
+/* WRITE(MP,30)F,GNORM*/
+ printf(" F = %g, GNORM = %g\n", *f, *gnorm);
+/* IF (IPRINT(2).GE.1)THEN*/
+ if (iprint[2] >= 1) {
+/* 40 FORMAT(' VECTOR X= ')*/
+/* WRITE(MP,40)*/
+ printf(" VECTOR X=\n");
+/* WRITE(MP,50) (X(I),I=1,N)*/
+ write50(x, *n);
+/* 60 FORMAT(' GRADIENT VECTOR G= ')*/
+/* WRITE(MP,60)*/
+ printf(" GRADIENT VECTOR G=\n");
+/* WRITE(MP,50) (G(I),I=1,N)*/
+ write50(g, *n);
+/* ENDIF*/
+ }
+/* 10 FORMAT('*************************************************')*/
+ printf("*************************************************\n");
+/* 70 FORMAT(/' I NFN',4X,'FUNC',8X,'GNORM',7X,'STEPLENGTH'/)*/
+/* WRITE(MP,70)*/
+ printf(" I NFN FUNC GNORM STEPLENGTH\n");
+/*ELSE*/
+ } else {
+/* IF ((IPRINT(1).EQ.0).AND.(ITER.NE.1.AND..NOT.FINISH))RETURN*/
+ if ((iprint[1]==0) && (*iter != 1 && !*finish))
+ return;
+/* IF (IPRINT(1).NE.0)THEN*/
+ if (iprint[1] != 0) {
+/* IF(MOD(ITER-1,IPRINT(1)).EQ.0.OR.FINISH)THEN*/
+ if ((*iter - 1)%iprint[1] == 0 || *finish) {
+/* 70 FORMAT(/' I NFN',4X,'FUNC',8X,'GNORM',7X,'STEPLENGTH'/)*/
+/* IF(IPRINT(2).GT.1.AND.ITER.GT.1) WRITE(MP,70)*/
+ if (iprint[2] > 1 && *iter > 1)
+ printf(" I NFN FUNC GNORM STEPLENGTH\n");
+/* 80 FORMAT(2(I4,1X),3X,3(1PD10.3,2X))*/
+/* WRITE(MP,80)ITER,NFUN,F,GNORM,STP*/
+ printf("%4d %4d %10.3f %10.3f %10.3f\n", *iter, *nfun, *f, *gnorm, *stp);
+ }
+/* ELSE*/
+ else {
+/* RETURN*/
+ return;
+/* ENDIF*/
+ }
+ }
+/* ELSE*/
+ else {
+
+/* 70 FORMAT(/' I NFN',4X,'FUNC',8X,'GNORM',7X,'STEPLENGTH'/)*/
+/* IF( IPRINT(2).GT.1.AND.FINISH) WRITE(MP,70)*/
+ if (iprint[2] > 1 && *finish)
+ printf(" I NFN FUNC GNORM STEPLENGTH\n");
+
+/* 80 FORMAT(2(I4,1X),3X,3(1PD10.3,2X))*/
+/* WRITE(MP,80)ITER,NFUN,F,GNORM,STP*/
+ printf("%4d %4d %10.3f %10.3f %10.3f\n", *iter, *nfun, *f, *gnorm, *stp);
+/* ENDIF*/
+ }
+
+/* IF (IPRINT(2).EQ.2.OR.IPRINT(2).EQ.3)THEN*/
+ if (iprint[2] == 2 || iprint[2] == 3) {
+/* IF (FINISH)THEN*/
+ if (*finish)
+/* 90 FORMAT(' FINAL POINT X= ')*/
+/* WRITE(MP,90)*/
+ printf(" FINAL POINT X=\n");
+/* ELSE*/
+ else
+/* 40 FORMAT(' VECTOR X= ')*/
+/* WRITE(MP,40)*/
+ printf(" VECTOR X=\n");
+/* ENDIF*/
+
+/* 50 FORMAT(6(2X,1PD10.3))*/
+/* WRITE(MP,50)(X(I),I=1,N)*/
+ write50(x, *n);
+/* IF (IPRINT(2).EQ.3)THEN*/
+ if (iprint[2] == 3) {
+/* 60 FORMAT(' GRADIENT VECTOR G= ')*/
+/* WRITE(MP,60)*/
+ printf(" GRADIENT VECTOR G=\n");
+/* 50 FORMAT(6(2X,1PD10.3))*/
+/* WRITE(MP,50)(G(I),I=1,N)*/
+ write50(g, *n);
+/* ENDIF*/
+ }
+/* ENDIF*/
+ }
+/* 100 FORMAT(/' THE MINIMIZATION TERMINATED WITHOUT DETECTING ERRORS.',*/
+/* . /' IFLAG = 0')*/
+/* IF (FINISH) WRITE(MP,100)*/
+ if (*finish)
+ printf(" THE MINIMIZATION TERMINATED WITHOUT DETECTING ERRORS.\n");
+ }
+/* ENDIF*/
+/* C*/
+/* RETURN*/
+/* END*/
+}
diff --git a/SimTKmath/Optimizers/src/lbfgsb.cpp b/SimTKmath/Optimizers/src/lbfgsb.cpp
new file mode 100644
index 0000000..87bee54
--- /dev/null
+++ b/SimTKmath/Optimizers/src/lbfgsb.cpp
@@ -0,0 +1,5161 @@
+/*=========================================================================
+
+ Program: Insight Segmentation & Registration Toolkit
+ Module: $RCSfile: lbfgsb.c,v $
+ Language: C++
+ Date: $Date: 2005/02/21 20:16:24 $
+ Version: $Revision: 1.13 $
+
+ Copyright (c) Insight Software Consortium. All rights reserved.
+ See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.
+
+ This software is distributed WITHOUT ANY WARRANTY; without even
+ the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ PURPOSE. See the above copyright notices for more information.
+
+=========================================================================*/
+
+/* This file contains code for Large-scale Bound-constrained or
+ * Unconstrainted Optimization.
+ *
+ * The original fortan code was obtained from
+ * http://www.ece.northwestern.edu/~nocedal/lbfgsb.html
+ * and converted to c code using the f2c converted.
+ * The resulting c file was then heavily modified in order
+ * remove calls to output routines.
+ *
+ * Refer to the original webpage for condition of use.
+ *
+ * References:
+ *
+ * [1] R. H. Byrd, P. Lu and J. Nocedal.
+ * A Limited Memory Algorithm for Bound Constrained Optimization, (1995),
+ * SIAM Journal on Scientific and Statistical Computing ,
+ * 16, 5, pp. 1190-1208.
+ *
+ * [2] C. Zhu, R. H. Byrd and J. Nocedal.
+ * L-BFGS-B: Algorithm 778: L-BFGS-B, FORTRAN routines for large scale
+ * bound constrained optimization (1997),
+ * ACM Transactions on Mathematical Software,
+ * Vol 23, Num. 4, pp. 550 - 560.
+ *
+ *
+ */
+
+#include "simmath/internal/common.h"
+#include "SimTKlapack.h"
+
+#include "LBFGSBOptimizer.h"
+#include "../src/Simmath_f2c.h"
+
+#include <cmath>
+#include <ctime>
+#include <cstring>
+
+#define imin(X, Y) ((X) < (Y) ? (X) : (Y))
+
+#if SimTK_DEFAULT_PRECISION==1 // float
+#define DCOPY scopy_
+#define DSCAL sscal_
+#define DAXPY saxpy_
+#define DDOT sdot_
+#define DPOTRF spotrf_
+#else // double
+#define DCOPY dcopy_
+#define DSCAL dscal_
+#define DAXPY daxpy_
+#define DDOT ddot_
+#define DPOTRF dpotrf_
+#endif
+
+using SimTK::Real;
+
+/* Table of constant values */
+
+const static Real c_b9 = 0.;
+const static int c__1 = 1;
+/* static int c__9 = 9; */
+const static int c__11 = 11;
+/* static int c__3 = 3; */
+const static Real c_b275 = Real(.001);
+const static Real c_b276 = Real(.9);
+const static Real c_b277 = Real(.1);
+/* static int c__5 = 5; */
+//Reordering function calls could prevent the need for defining these statics before use.
+static int dtrsl_( Real *t, int *ldt, int *n, Real *b, const int *job, int *info);
+static int hpsolb_( int *n, Real *t, int *iorder, int *iheap);
+static int dcsrch_( Real *f, Real *g, Real *stp, const Real *ftol,
+ const Real *gtol, const Real *xtol, const Real *stpmin, Real *stpmax,
+ char *task, int *isave, Real *dsave, ftnlen task_len);
+static int dcstep_( Real *stx, Real *fx, Real *dx,
+ Real *sty, Real *fy, Real *dy, Real *stp, Real *fp, Real *dp,
+ bool *brackt, Real *stpmin, Real *stpmax);
+
+static Real timer() {
+ return( (Real)(std::clock())/CLOCKS_PER_SEC );
+}
+
+/* Subroutine */
+static int active_( int *n, Real *l, Real *u, int *nbd,
+Real *x, int *iwhere, int *iprint,
+bool *prjctd, bool *cnstnd, bool *boxed)
+{
+ /* Format strings */
+/*
+ static char fmt_1001[] = "(/,\002At X0 \002,i9,\002 variables are exactl\
+y at the bounds\002)";
+*/
+
+ /* System generated locals */
+ int i__1;
+
+ /* Local variables */
+ int nbdd, i__;
+
+ /* Fortran I/O blocks */
+/*
+ static cilist io___81 = { 0, 6, 0, 0, 0 };
+ static cilist io___82 = { 0, 6, 0, 0, 0 };
+ static cilist io___83 = { 0, 6, 0, fmt_1001, 0
+*/
+
+
+/* ************ */
+
+/* Subroutine active */
+
+/* This subroutine initializes iwhere and projects the initial x to */
+/* the feasible set if necessary. */
+
+/* iwhere is an int array of dimension n. */
+/* On entry iwhere is unspecified. */
+/* On exit iwhere(i)=-1 if x(i) has no bounds */
+/* 3 if l(i)=u(i) */
+/* 0 otherwise. */
+/* In cauchy, iwhere is given finer gradations. */
+
+
+/* * * * */
+
+/* NEOS, November 1994. (Latest revision June 1996.) */
+/* Optimization Technology Center. */
+/* Argonne National Laboratory and Northwestern University. */
+/* Written by */
+/* Ciyou Zhu */
+/* in collaboration with R.H. Byrd, P. Lu-Chen and J. Nocedal. */
+
+
+/* ************ */
+/* Initialize nbdd, prjctd, cnstnd and boxed. */
+ /* Parameter adjustments */
+ --iwhere;
+ --x;
+ --nbd;
+ --u;
+ --l;
+
+ /* Function Body */
+ nbdd = 0;
+ *prjctd = FALSE_;
+ *cnstnd = FALSE_;
+ *boxed = TRUE_;
+/* Project the initial x to the easible set if necessary. */
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ if (nbd[i__] > 0) {
+ if (nbd[i__] <= 2 && x[i__] <= l[i__]) {
+ if (x[i__] < l[i__]) {
+ *prjctd = TRUE_;
+ x[i__] = l[i__];
+ }
+ ++nbdd;
+ } else if (nbd[i__] >= 2 && x[i__] >= u[i__]) {
+ if (x[i__] > u[i__]) {
+ *prjctd = TRUE_;
+ x[i__] = u[i__];
+ }
+ ++nbdd;
+ }
+ }
+/* L10: */
+ }
+/* Initialize iwhere and assign values to cnstnd and boxed. */
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ if (nbd[i__] != 2) {
+ *boxed = FALSE_;
+ }
+ if (nbd[i__] == 0) {
+/* this variable is always free */
+ iwhere[i__] = -1;
+/* otherwise set x(i)=mid(x(i), u(i), l(i)). */
+ } else {
+ *cnstnd = TRUE_;
+ if (nbd[i__] == 2 && u[i__] - l[i__] <= 0.) {
+/* this variable is always fixed */
+ iwhere[i__] = 3;
+ } else {
+ iwhere[i__] = 0;
+ }
+ }
+/* L20: */
+ }
+ if (*iprint >= 0) {
+ if (*prjctd) {
+/*
+ s_wsle(&io___81);
+ do_lio(&c__9, &c__1, "The initial X is infeasible. Restart with\
+ its projection.", (ftnlen)58);
+ e_wsle();
+*/
+ }
+ if (! (*cnstnd)) {
+/*
+ s_wsle(&io___82);
+ do_lio(&c__9, &c__1, "This problem is unconstrained.", (ftnlen)30)
+ ;
+ e_wsle();
+*/
+ }
+ }
+ if (*iprint > 0) {
+/*
+ s_wsfe(&io___83);
+ do_fio(&c__1, (char *)&nbdd, (ftnlen)sizeof(int));
+ e_wsfe();
+*/
+ }
+ return 0;
+} /* active_ */
+
+/* ======================= The end of active ============================= */
+/* Subroutine */
+static int bmv_( int *m, Real *sy, Real *wt,
+int *col, Real *v, Real *p, int *info)
+{
+ /* System generated locals */
+ int sy_dim1, sy_offset, wt_dim1, wt_offset, i__1, i__2;
+
+ /* Local variables */
+ int i__, k;
+ int i2;
+ Real sum;
+
+/* ************ */
+
+/* Subroutine bmv */
+
+/* This subroutine computes the product of the 2m x 2m middle matrix */
+/* in the compact L-BFGS formula of B and a 2m vector v; */
+/* it returns the product in p. */
+
+/* m is an int variable. */
+/* On entry m is the maximum number of variable metric corrections */
+/* used to define the limited memory matrix. */
+/* On exit m is unchanged. */
+
+/* sy is a double precision array of dimension m x m. */
+/* On entry sy specifies the matrix S'Y. */
+/* On exit sy is unchanged. */
+
+/* wt is a double precision array of dimension m x m. */
+/* On entry wt specifies the upper triangular matrix J' which is */
+/* the Cholesky factor of (thetaS'S+LD^(-1)L'). */
+/* On exit wt is unchanged. */
+
+/* col is an int variable. */
+/* On entry col specifies the number of s-vectors (or y-vectors) */
+/* stored in the compact L-BFGS formula. */
+/* On exit col is unchanged. */
+
+/* v is a double precision array of dimension 2col. */
+/* On entry v specifies vector v. */
+/* On exit v is unchanged. */
+
+/* p is a double precision array of dimension 2col. */
+/* On entry p is unspecified. */
+/* On exit p is the product Mv. */
+
+/* info is an int variable. */
+/* On entry info is unspecified. */
+/* On exit info = 0 for normal return, */
+/* = nonzero for abnormal return when the system */
+/* to be solved by dtrsl is singular. */
+
+/* Subprograms called: */
+
+/* Linpack ... dtrsl. */
+
+
+/* * * * */
+
+/* NEOS, November 1994. (Latest revision June 1996.) */
+/* Optimization Technology Center. */
+/* Argonne National Laboratory and Northwestern University. */
+/* Written by */
+/* Ciyou Zhu */
+/* in collaboration with R.H. Byrd, P. Lu-Chen and J. Nocedal. */
+
+
+/* ************ */
+ /* Parameter adjustments */
+ wt_dim1 = *m;
+ wt_offset = 1 + wt_dim1 * 1;
+ wt -= wt_offset;
+ sy_dim1 = *m;
+ sy_offset = 1 + sy_dim1 * 1;
+ sy -= sy_offset;
+ --p;
+ --v;
+
+ /* Function Body */
+ if (*col == 0) {
+ return 0;
+ }
+/* PART I: solve [ D^(1/2) O ] [ p1 ] = [ v1 ] */
+/* [ -L*D^(-1/2) J ] [ p2 ] [ v2 ]. */
+/* solve Jp2=v2+LD^(-1)v1. */
+ p[*col + 1] = v[*col + 1];
+ i__1 = *col;
+ for (i__ = 2; i__ <= i__1; ++i__) {
+ i2 = *col + i__;
+ sum = 0.;
+ i__2 = i__ - 1;
+ for (k = 1; k <= i__2; ++k) {
+ sum += sy[i__ + k * sy_dim1] * v[k] / sy[k + k * sy_dim1];
+/* L10: */
+ }
+ p[i2] = v[i2] + sum;
+/* L20: */
+ }
+/* Solve the triangular system */
+ dtrsl_(&wt[wt_offset], m, col, &p[*col + 1], &c__11, info);
+ if (*info != 0) {
+ return 0;
+ }
+/* solve D^(1/2)p1=v1. */
+ i__1 = *col;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ p[i__] = v[i__] / std::sqrt(sy[i__ + i__ * sy_dim1]);
+/* L30: */
+ }
+/* PART II: solve [ -D^(1/2) D^(-1/2)*L' ] [ p1 ] = [ p1 ] */
+/* [ 0 J' ] [ p2 ] [ p2 ]. */
+/* solve J^Tp2=p2. */
+ dtrsl_(&wt[wt_offset], m, col, &p[*col + 1], &c__1, info);
+ if (*info != 0) {
+ return 0;
+ }
+/* compute p1=-D^(-1/2)(p1-D^(-1/2)L'p2) */
+/* =-D^(-1/2)p1+D^(-1)L'p2. */
+ i__1 = *col;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ p[i__] = -p[i__] / std::sqrt(sy[i__ + i__ * sy_dim1]);
+/* L40: */
+ }
+ i__1 = *col;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ sum = 0.;
+ i__2 = *col;
+ for (k = i__ + 1; k <= i__2; ++k) {
+ sum += sy[k + i__ * sy_dim1] * p[*col + k] / sy[i__ + i__ *
+ sy_dim1];
+/* L50: */
+ }
+ p[i__] += sum;
+/* L60: */
+ }
+ return 0;
+} /* bmv_ */
+
+/* ======================== The end of bmv =============================== */
+/* Subroutine */
+static int cauchy_(
+int *n, Real *x, Real *l, Real *u, int *nbd,
+Real *g, int *iorder, int *iwhere, Real *t,
+Real *d__, Real *xcp, int *m, Real *wy, Real *ws,
+Real *sy, Real *wt, Real *theta, int *col, int *head,
+Real *p, Real *c__, Real *wbp, Real *v, int *nint,
+Real *sg, Real *yg, int *iprint, Real *sbgnrm,
+int *info, Real *epsmch)
+{
+
+ /* Format strings */
+/*
+ static char fmt_3010[] = "(/,\002---------------- CAUCHY entered--------\
+-----------\002)";
+ static char fmt_1010[] = "(\002Cauchy X = \002,/,(4x,1p,6(1x,d11.4)))";
+ static char fmt_4011[] = "(/,\002Piece \002,i3,\002 --f1, f2 at start\
+ point \002,1p,2(1x,d11.4))";
+ static char fmt_5010[] = "(\002Distance to the next break point = \002,\
+1p,d11.4)";
+ static char fmt_6010[] = "(\002Distance to the stationary point = \002,\
+1p,d11.4)";
+ static char fmt_4010[] = "(\002Piece \002,i3,\002 --f1, f2 at start p\
+oint \002,1p,2(1x,d11.4))";
+ static char fmt_2010[] = "(/,\002---------------- exit CAUCHY-----------\
+-----------\002,/)";
+*/
+
+ /* System generated locals */
+ int wy_dim1, wy_offset, ws_dim1, ws_offset, sy_dim1, sy_offset,
+ wt_dim1, wt_offset, i__1, i__2;
+ Real d__1;
+
+ /* Local variables */
+ Real dibp;
+ int iter;
+ Real zibp, tsum, dibp2;
+ int i__, j;
+ bool bnded;
+ Real neggi;
+ int nfree;
+ Real bkmin;
+ int nleft;
+ Real f1, f2, f2_org__, dt, tj, tl;
+ int nbreak, ibkmin;
+ Real tu;
+ int pointr;
+ Real tj0;
+ bool xlower, xupper;
+ int ibp;
+ Real dtm;
+ Real wmc, wmp, wmw;
+ int col2;
+
+ /* Fortran I/O blocks */
+/*
+ static cilist io___88 = { 0, 6, 0, 0, 0 };
+ static cilist io___96 = { 0, 6, 0, fmt_3010, 0 };
+ static cilist io___105 = { 0, 6, 0, fmt_1010, 0 };
+ static cilist io___110 = { 0, 6, 0, 0, 0 };
+ static cilist io___117 = { 0, 6, 0, fmt_4011, 0 };
+ static cilist io___118 = { 0, 6, 0, fmt_5010, 0 };
+ static cilist io___119 = { 0, 6, 0, fmt_6010, 0 };
+ static cilist io___122 = { 0, 6, 0, 0, 0 };
+ static cilist io___127 = { 0, 6, 0, 0, 0 };
+ static cilist io___128 = { 0, 6, 0, 0, 0 };
+ static cilist io___129 = { 0, 6, 0, fmt_4010, 0 };
+ static cilist io___130 = { 0, 6, 0, fmt_6010, 0 };
+ static cilist io___131 = { 0, 6, 0, fmt_1010, 0 };
+ static cilist io___132 = { 0, 6, 0, fmt_2010, 0 };
+*/
+
+
+/* ************ */
+
+/* Subroutine cauchy */
+
+/* For given x, l, u, g (with sbgnrm > 0), and a limited memory */
+/* BFGS matrix B defined in terms of matrices WY, WS, WT, and */
+/* scalars head, col, and theta, this subroutine computes the */
+/* generalized Cauchy point (GCP), defined as the first local */
+/* minimizer of the quadratic */
+
+/* Q(x + s) = g's + 1/2 s'Bs */
+
+/* along the projected gradient direction P(x-tg,l,u). */
+/* The routine returns the GCP in xcp. */
+
+/* n is an int variable. */
+/* On entry n is the dimension of the problem. */
+/* On exit n is unchanged. */
+
+/* x is a double precision array of dimension n. */
+/* On entry x is the starting point for the GCP computation. */
+/* On exit x is unchanged. */
+
+/* l is a double precision array of dimension n. */
+/* On entry l is the lower bound of x. */
+/* On exit l is unchanged. */
+
+/* u is a double precision array of dimension n. */
+/* On entry u is the upper bound of x. */
+/* On exit u is unchanged. */
+
+/* nbd is an int array of dimension n. */
+/* On entry nbd represents the type of bounds imposed on the */
+/* variables, and must be specified as follows: */
+/* nbd(i)=0 if x(i) is unbounded, */
+/* 1 if x(i) has only a lower bound, */
+/* 2 if x(i) has both lower and upper bounds, and */
+/* 3 if x(i) has only an upper bound. */
+/* On exit nbd is unchanged. */
+
+/* g is a double precision array of dimension n. */
+/* On entry g is the gradient of f(x). g must be a nonzero vector. */
+/* On exit g is unchanged. */
+
+/* iorder is an int working array of dimension n. */
+/* iorder will be used to store the breakpoints in the piecewise */
+/* linear path and free variables encountered. On exit, */
+/* iorder(1),...,iorder(nleft) are indices of breakpoints */
+/* which have not been encountered; */
+/* iorder(nleft+1),...,iorder(nbreak) are indices of */
+/* encountered breakpoints; and */
+/* iorder(nfree),...,iorder(n) are indices of variables which */
+/* have no bound constraits along the search direction. */
+
+/* iwhere is an int array of dimension n. */
+/* On entry iwhere indicates only the permanently fixed (iwhere=3) */
+/* or free (iwhere= -1) components of x. */
+/* On exit iwhere records the status of the current x variables. */
+/* iwhere(i)=-3 if x(i) is free and has bounds, but is not moved */
+/* 0 if x(i) is free and has bounds, and is moved */
+/* 1 if x(i) is fixed at l(i), and l(i) .ne. u(i) */
+/* 2 if x(i) is fixed at u(i), and u(i) .ne. l(i) */
+/* 3 if x(i) is always fixed, i.e., u(i)=x(i)=l(i) */
+/* -1 if x(i) is always free, i.e., it has no bounds. */
+
+/* t is a double precision working array of dimension n. */
+/* t will be used to store the break points. */
+
+/* d is a double precision array of dimension n used to store */
+/* the Cauchy direction P(x-tg)-x. */
+
+/* xcp is a double precision array of dimension n used to return the */
+/* GCP on exit. */
+
+/* m is an int variable. */
+/* On entry m is the maximum number of variable metric corrections */
+/* used to define the limited memory matrix. */
+/* On exit m is unchanged. */
+
+/* ws, wy, sy, and wt are double precision arrays. */
+/* On entry they store information that defines the */
+/* limited memory BFGS matrix: */
+/* ws(n,m) stores S, a set of s-vectors; */
+/* wy(n,m) stores Y, a set of y-vectors; */
+/* sy(m,m) stores S'Y; */
+/* wt(m,m) stores the */
+/* Cholesky factorization of (theta*S'S+LD^(-1)L'). */
+/* On exit these arrays are unchanged. */
+
+/* theta is a double precision variable. */
+/* On entry theta is the scaling factor specifying B_0 = theta I. */
+/* On exit theta is unchanged. */
+
+/* col is an int variable. */
+/* On entry col is the actual number of variable metric */
+/* corrections stored so far. */
+/* On exit col is unchanged. */
+
+/* head is an int variable. */
+/* On entry head is the location of the first s-vector (or y-vector) */
+/* in S (or Y). */
+/* On exit col is unchanged. */
+
+/* p is a double precision working array of dimension 2m. */
+/* p will be used to store the vector p = W^(T)d. */
+
+/* c is a double precision working array of dimension 2m. */
+/* c will be used to store the vector c = W^(T)(xcp-x). */
+
+/* wbp is a double precision working array of dimension 2m. */
+/* wbp will be used to store the row of W corresponding */
+/* to a breakpoint. */
+
+/* v is a double precision working array of dimension 2m. */
+
+/* nint is an int variable. */
+/* On exit nint records the number of quadratic segments explored */
+/* in searching for the GCP. */
+
+/* sg and yg are double precision arrays of dimension m. */
+/* On entry sg and yg store S'g and Y'g correspondingly. */
+/* On exit they are unchanged. */
+
+/* iprint is an INTEGER variable that must be set by the user. */
+/* It controls the frequency and type of output generated: */
+/* iprint<0 no output is generated; */
+/* iprint=0 print only one line at the last iteration; */
+/* 0<iprint<99 print also f and |proj g| every iprint iterations; */
+/* iprint=99 print details of every iteration except n-vectors; */
+/* iprint=100 print also the changes of active set and final x; */
+/* iprint>100 print details of every iteration including x and g; */
+/* When iprint > 0, the file iterate.dat will be created to */
+/* summarize the iteration. */
+
+/* sbgnrm is a double precision variable. */
+/* On entry sbgnrm is the norm of the projected gradient at x. */
+/* On exit sbgnrm is unchanged. */
+
+/* info is an int variable. */
+/* On entry info is 0. */
+/* On exit info = 0 for normal return, */
+/* = nonzero for abnormal return when the the system */
+/* used in routine bmv is singular. */
+
+/* Subprograms called: */
+
+/* L-BFGS-B Library ... hpsolb, bmv. */
+
+/* Linpack ... dscal dcopy, daxpy. */
+
+
+/* References: */
+
+/* [1] R. H. Byrd, P. Lu, J. Nocedal and C. Zhu, ``A limited */
+/* memory algorithm for bound constrained optimization'', */
+/* SIAM J. Scientific Computing 16 (1995), no. 5, pp. 1190--1208. */
+
+/* [2] C. Zhu, R.H. Byrd, P. Lu, J. Nocedal, ``L-BFGS-B: FORTRAN */
+/* Subroutines for Large Scale Bound Constrained Optimization'' */
+/* Tech. Report, NAM-11, EECS Department, Northwestern University, */
+/* 1994. */
+
+/* (Postscript files of these papers are available via anonymous */
+/* ftp to eecs.nwu.edu in the directory pub/lbfgs/lbfgs_bcm.) */
+
+/* * * * */
+
+/* NEOS, November 1994. (Latest revision June 1996.) */
+/* Optimization Technology Center. */
+/* Argonne National Laboratory and Northwestern University. */
+/* Written by */
+/* Ciyou Zhu */
+/* in collaboration with R.H. Byrd, P. Lu-Chen and J. Nocedal. */
+
+
+/* ************ */
+/* Check the status of the variables, reset iwhere(i) if necessary; */
+/* compute the Cauchy direction d and the breakpoints t; initialize */
+/* the derivative f1 and the vector p = W'd (for theta = 1). */
+ /* Parameter adjustments */
+ --xcp;
+ --d__;
+ --t;
+ --iwhere;
+ --iorder;
+ --g;
+ --nbd;
+ --u;
+ --l;
+ --x;
+ --yg;
+ --sg;
+ --v;
+ --wbp;
+ --c__;
+ --p;
+ wt_dim1 = *m;
+ wt_offset = 1 + wt_dim1 * 1;
+ wt -= wt_offset;
+ sy_dim1 = *m;
+ sy_offset = 1 + sy_dim1 * 1;
+ sy -= sy_offset;
+ ws_dim1 = *n;
+ ws_offset = 1 + ws_dim1 * 1;
+ ws -= ws_offset;
+ wy_dim1 = *n;
+ wy_offset = 1 + wy_dim1 * 1;
+ wy -= wy_offset;
+
+ /* Function Body */
+ if (*sbgnrm <= 0.) {
+ if (*iprint >= 0) {
+/*
+ s_wsle(&io___88);
+ do_lio(&c__9, &c__1, "Subgnorm = 0. GCP = X.", (ftnlen)23);
+ e_wsle();
+*/
+ }
+ DCOPY(*n, &x[1], (const int&)c__1, &xcp[1], (const int&)c__1);
+ return 0;
+ }
+ bnded = TRUE_;
+ nfree = *n + 1;
+ nbreak = 0;
+ ibkmin = 0;
+ bkmin = 0.;
+ col2 = *col << 1;
+ f1 = 0.;
+ if (*iprint >= 99) {
+/*
+ s_wsfe(&io___96);
+ e_wsfe();
+*/
+ }
+/* We set p to zero and build it up as we determine d. */
+ i__1 = col2;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ p[i__] = 0.;
+/* L20: */
+ }
+/* In the following loop we determine for each variable its bound */
+/* status and its breakpoint, and update p accordingly. */
+/* Smallest breakpoint is identified. */
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ neggi = -g[i__];
+ if (iwhere[i__] != 3 && iwhere[i__] != -1) {
+/* if x(i) is not a constant and has bounds, */
+/* compute the difference between x(i) and its bounds. */
+ if (nbd[i__] <= 2) {
+ tl = x[i__] - l[i__];
+ }
+ if (nbd[i__] >= 2) {
+ tu = u[i__] - x[i__];
+ }
+/* If a variable is close enough to a bound */
+/* we treat it as at bound. */
+ xlower = nbd[i__] <= 2 && tl <= 0.;
+ xupper = nbd[i__] >= 2 && tu <= 0.;
+/* reset iwhere(i). */
+ iwhere[i__] = 0;
+ if (xlower) {
+ if (neggi <= 0.) {
+ iwhere[i__] = 1;
+ }
+ } else if (xupper) {
+ if (neggi >= 0.) {
+ iwhere[i__] = 2;
+ }
+ } else {
+ if (fabs(neggi) <= 0.) {
+ iwhere[i__] = -3;
+ }
+ }
+ }
+ pointr = *head;
+ if (iwhere[i__] != 0 && iwhere[i__] != -1) {
+ d__[i__] = 0.;
+ } else {
+ d__[i__] = neggi;
+ f1 -= neggi * neggi;
+/* calculate p := p - W'e_i* (g_i). */
+ i__2 = *col;
+ for (j = 1; j <= i__2; ++j) {
+ p[j] += wy[i__ + pointr * wy_dim1] * neggi;
+ p[*col + j] += ws[i__ + pointr * ws_dim1] * neggi;
+ pointr = pointr % *m + 1;
+/* L40: */
+ }
+ if (nbd[i__] <= 2 && nbd[i__] != 0 && neggi < 0.) {
+/* x(i) + d(i) is bounded; compute t(i). */
+ ++nbreak;
+ iorder[nbreak] = i__;
+ t[nbreak] = tl / (-neggi);
+ if (nbreak == 1 || t[nbreak] < bkmin) {
+ bkmin = t[nbreak];
+ ibkmin = nbreak;
+ }
+ } else if (nbd[i__] >= 2 && neggi > 0.) {
+/* x(i) + d(i) is bounded; compute t(i). */
+ ++nbreak;
+ iorder[nbreak] = i__;
+ t[nbreak] = tu / neggi;
+ if (nbreak == 1 || t[nbreak] < bkmin) {
+ bkmin = t[nbreak];
+ ibkmin = nbreak;
+ }
+ } else {
+/* x(i) + d(i) is not bounded. */
+ --nfree;
+ iorder[nfree] = i__;
+ if (fabs(neggi) > 0.) {
+ bnded = FALSE_;
+ }
+ }
+ }
+/* L50: */
+ }
+/* The indices of the nonzero components of d are now stored */
+/* in iorder(1),...,iorder(nbreak) and iorder(nfree),...,iorder(n). */
+/* The smallest of the nbreak breakpoints is in t(ibkmin)=bkmin. */
+ if (*theta != 1.) {
+/* complete the initialization of p for theta not= one. */
+ DSCAL(*col, *theta, &p[*col + 1], (const int&)c__1);
+ }
+/* Initialize GCP xcp = x. */
+ DCOPY(*n, &x[1], (const int&)c__1, &xcp[1], (const int&)c__1);
+ if (nbreak == 0 && nfree == *n + 1) {
+/* is a zero vector, return with the initial xcp as GCP. */
+ if (*iprint > 100) {
+/*
+ s_wsfe(&io___105);
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ do_fio(&c__1, (char *)&xcp[i__], (ftnlen)sizeof(Real));
+ }
+ e_wsfe();
+*/
+ }
+ return 0;
+ }
+/* Initialize c = W'(xcp - x) = 0. */
+ i__1 = col2;
+ for (j = 1; j <= i__1; ++j) {
+ c__[j] = 0.;
+/* L60: */
+ }
+/* Initialize derivative f2. */
+ f2 = -(*theta) * f1;
+ f2_org__ = f2;
+ if (*col > 0) {
+ bmv_(m, &sy[sy_offset], &wt[wt_offset], col, &p[1], &v[1], info);
+ if (*info != 0) {
+ return 0;
+ }
+ f2 -= DDOT((const int&)col2, &v[1], (const int&)c__1, &p[1], (const int&)c__1);
+ }
+ dtm = -f1 / f2;
+ tsum = 0.;
+ *nint = 1;
+ if (*iprint >= 99) {
+/*
+ s_wsle(&io___110);
+ do_lio(&c__9, &c__1, "There are ", (ftnlen)10);
+ do_lio(&c__3, &c__1, (char *)&nbreak, (ftnlen)sizeof(int));
+ do_lio(&c__9, &c__1, " breakpoints ", (ftnlen)14);
+ e_wsle();
+*/
+ }
+/* If there are no breakpoints, locate the GCP and return. */
+ if (nbreak == 0) {
+ goto L888;
+ }
+ nleft = nbreak;
+ iter = 1;
+ tj = 0.;
+/* ------------------- the beginning of the loop ------------------------- */
+L777:
+/* Find the next smallest breakpoint; */
+/* compute dt = t(nleft) - t(nleft + 1). */
+ tj0 = tj;
+ if (iter == 1) {
+/* Since we already have the smallest breakpoint we need not do */
+/* heapsort yet. Often only one breakpoint is used and the */
+/* cost of heapsort is avoided. */
+ tj = bkmin;
+ ibp = iorder[ibkmin];
+ } else {
+ if (iter == 2) {
+/* Replace the already used smallest breakpoint with the */
+/* breakpoint numbered nbreak > nlast, before heapsort call. */
+ if (ibkmin != nbreak) {
+ t[ibkmin] = t[nbreak];
+ iorder[ibkmin] = iorder[nbreak];
+ }
+/* Update heap structure of breakpoints */
+/* (if iter=2, initialize heap). */
+ }
+ i__1 = iter - 2;
+ hpsolb_(&nleft, &t[1], &iorder[1], &i__1);
+ tj = t[nleft];
+ ibp = iorder[nleft];
+ }
+ dt = tj - tj0;
+ if (dt != 0. && *iprint >= 100) {
+/*
+ s_wsfe(&io___117);
+ do_fio(&c__1, (char *)&(*nint), (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&f1, (ftnlen)sizeof(Real));
+ do_fio(&c__1, (char *)&f2, (ftnlen)sizeof(Real));
+ e_wsfe();
+ s_wsfe(&io___118);
+ do_fio(&c__1, (char *)&dt, (ftnlen)sizeof(Real));
+ e_wsfe();
+ s_wsfe(&io___119);
+ do_fio(&c__1, (char *)&dtm, (ftnlen)sizeof(Real));
+ e_wsfe();
+*/
+ }
+/* If a minimizer is within this interval, locate the GCP and return. */
+ if (dtm < dt) {
+ goto L888;
+ }
+/* Otherwise fix one variable and */
+/* reset the corresponding component of d to zero. */
+ tsum += dt;
+ --nleft;
+ ++iter;
+ dibp = d__[ibp];
+ d__[ibp] = 0.;
+ if (dibp > 0.) {
+ zibp = u[ibp] - x[ibp];
+ xcp[ibp] = u[ibp];
+ iwhere[ibp] = 2;
+ } else {
+ zibp = l[ibp] - x[ibp];
+ xcp[ibp] = l[ibp];
+ iwhere[ibp] = 1;
+ }
+ if (*iprint >= 100) {
+/*
+ s_wsle(&io___122);
+ do_lio(&c__9, &c__1, "Variable ", (ftnlen)10);
+ do_lio(&c__3, &c__1, (char *)&ibp, (ftnlen)sizeof(int));
+ do_lio(&c__9, &c__1, " is fixed.", (ftnlen)11);
+ e_wsle();
+*/
+ }
+ if (nleft == 0 && nbreak == *n) {
+/* all n variables are fixed, */
+/* return with xcp as GCP. */
+ dtm = dt;
+ goto L999;
+ }
+/* Update the derivative information. */
+ ++(*nint);
+/* Computing 2nd power */
+ d__1 = dibp;
+ dibp2 = d__1 * d__1;
+/* Update f1 and f2. */
+/* temporarily set f1 and f2 for col=0. */
+ f1 = f1 + dt * f2 + dibp2 - *theta * dibp * zibp;
+ f2 -= *theta * dibp2;
+ if (*col > 0) {
+/* update c = c + dt*p. */
+ DAXPY((const int&)col2, (const Real&)dt, &p[1], (const int&)c__1, &c__[1], (const int&)c__1);
+/* choose wbp, */
+/* the row of W corresponding to the breakpoint encountered. */
+ pointr = *head;
+ i__1 = *col;
+ for (j = 1; j <= i__1; ++j) {
+ wbp[j] = wy[ibp + pointr * wy_dim1];
+ wbp[*col + j] = *theta * ws[ibp + pointr * ws_dim1];
+ pointr = pointr % *m + 1;
+/* L70: */
+ }
+/* compute (wbp)Mc, (wbp)Mp, and (wbp)M(wbp)'. */
+ bmv_(m, &sy[sy_offset], &wt[wt_offset], col, &wbp[1], &v[1], info);
+ if (*info != 0) {
+ return 0;
+ }
+ wmc = DDOT((const int&)col2, &c__[1], (const int&)c__1, &v[1], (const int&)c__1);
+ wmp = DDOT((const int&)col2, &p[1], (const int&)c__1, &v[1], (const int&)c__1);
+ wmw = DDOT((const int&)col2, &wbp[1], (const int&)c__1, &v[1], (const int&)c__1);
+/* update p = p - dibp*wbp. */
+ d__1 = -dibp;
+ DAXPY((const int&)col2, (const Real&)d__1, &wbp[1], (const int&)c__1, &p[1], (const int&)c__1);
+/* complete updating f1 and f2 while col > 0. */
+ f1 += dibp * wmc;
+ f2 = f2 + dibp * Real(2) * wmp - dibp2 * wmw;
+ }
+/* Computing MAX */
+ d__1 = *epsmch * f2_org__;
+ f2 = std::max(d__1,f2);
+ if (nleft > 0) {
+ dtm = -f1 / f2;
+ goto L777;
+/* to repeat the loop for unsearched intervals. */
+ } else if (bnded) {
+ f1 = 0.;
+ f2 = 0.;
+ dtm = 0.;
+ } else {
+ dtm = -f1 / f2;
+ }
+/* ------------------- the end of the loop ------------------------------- */
+L888:
+ if (*iprint >= 99) {
+/*
+ s_wsle(&io___127);
+ e_wsle();
+ s_wsle(&io___128);
+ do_lio(&c__9, &c__1, "GCP found in this segment", (ftnlen)25);
+ e_wsle();
+ s_wsfe(&io___129);
+ do_fio(&c__1, (char *)&(*nint), (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&f1, (ftnlen)sizeof(Real));
+ do_fio(&c__1, (char *)&f2, (ftnlen)sizeof(Real));
+ e_wsfe();
+ s_wsfe(&io___130);
+ do_fio(&c__1, (char *)&dtm, (ftnlen)sizeof(Real));
+ e_wsfe();
+*/
+ }
+ if (dtm <= 0.) {
+ dtm = 0.;
+ }
+ tsum += dtm;
+/* Move free variables (i.e., the ones w/o breakpoints) and */
+/* the variables whose breakpoints haven't been reached. */
+ DAXPY(*n, (const Real&)tsum, &d__[1], (const int&)c__1, &xcp[1], (const int&)c__1);
+L999:
+/* Update c = c + dtm*p = W'(x^c - x) */
+/* which will be used in computing r = Z'(B(x^c - x) + g). */
+ if (*col > 0) {
+ DAXPY((const int&)col2, (const Real&)dtm, &p[1], (const int&)c__1, &c__[1], (const int&)c__1);
+ }
+ if (*iprint > 100) {
+/*
+ s_wsfe(&io___131);
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ do_fio(&c__1, (char *)&xcp[i__], (ftnlen)sizeof(Real));
+ }
+ e_wsfe();
+*/
+ }
+ if (*iprint >= 99) {
+/*
+ s_wsfe(&io___132);
+ e_wsfe();
+*/
+ }
+ return 0;
+} /* cauchy_ */
+
+/* ====================== The end of cauchy ============================== */
+/* Subroutine */
+static int cmprlb_( int *n, int *m, Real *x, Real *g,
+ Real *ws, Real *wy, Real *sy, Real *wt,
+ Real *z__, Real *r__, Real *wa, int *index,
+ Real *theta, int *col, int *head, int *nfree,
+ bool *cnstnd, int *info)
+{
+ /* System generated locals */
+ int ws_dim1, ws_offset, wy_dim1, wy_offset, sy_dim1, sy_offset,
+ wt_dim1, wt_offset, i__1, i__2;
+
+ /* Local variables */
+ int i__, j, k;
+ Real a1, a2;
+ int pointr;
+
+/* ************ */
+
+/* Subroutine cmprlb */
+
+/* This subroutine computes r=-Z'B(xcp-xk)-Z'g by using */
+/* wa(2m+1)=W'(xcp-x) from subroutine cauchy. */
+
+/* Subprograms called: */
+
+/* L-BFGS-B Library ... bmv. */
+
+
+/* * * * */
+
+/* NEOS, November 1994. (Latest revision June 1996.) */
+/* Optimization Technology Center. */
+/* Argonne National Laboratory and Northwestern University. */
+/* Written by */
+/* Ciyou Zhu */
+/* in collaboration with R.H. Byrd, P. Lu-Chen and J. Nocedal. */
+
+
+/* ************ */
+ /* Parameter adjustments */
+ --index;
+ --r__;
+ --z__;
+ --g;
+ --x;
+ --wa;
+ wt_dim1 = *m;
+ wt_offset = 1 + wt_dim1 * 1;
+ wt -= wt_offset;
+ sy_dim1 = *m;
+ sy_offset = 1 + sy_dim1 * 1;
+ sy -= sy_offset;
+ wy_dim1 = *n;
+ wy_offset = 1 + wy_dim1 * 1;
+ wy -= wy_offset;
+ ws_dim1 = *n;
+ ws_offset = 1 + ws_dim1 * 1;
+ ws -= ws_offset;
+
+ /* Function Body */
+ if (! (*cnstnd) && *col > 0) {
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ r__[i__] = -g[i__];
+/* L26: */
+ }
+ } else {
+ i__1 = *nfree;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ k = index[i__];
+ r__[i__] = -(*theta) * (z__[k] - x[k]) - g[k];
+/* L30: */
+ }
+ bmv_(m, &sy[sy_offset], &wt[wt_offset], col, &wa[(*m << 1) + 1], &wa[
+ 1], info);
+ if (*info != 0) {
+ *info = -8;
+ return 0;
+ }
+ pointr = *head;
+ i__1 = *col;
+ for (j = 1; j <= i__1; ++j) {
+ a1 = wa[j];
+ a2 = *theta * wa[*col + j];
+ i__2 = *nfree;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ k = index[i__];
+ r__[i__] = r__[i__] + wy[k + pointr * wy_dim1] * a1 + ws[k +
+ pointr * ws_dim1] * a2;
+/* L32: */
+ }
+ pointr = pointr % *m + 1;
+/* L34: */
+ }
+ }
+ return 0;
+} /* cmprlb_ */
+
+/* ======================= The end of cmprlb ============================= */
+/* Subroutine */
+static int errclb_( int *n, int *m, Real *factr, Real *l,
+ Real *u, int *nbd, char *task, int *info, int *k,
+ ftnlen task_len)
+{
+ /* System generated locals */
+ int i__1;
+
+ /* Local variables */
+ int i__;
+ (void)task_len;
+
+/* ************ */
+
+/* Subroutine errclb */
+
+/* This subroutine checks the validity of the input data. */
+
+
+/* * * * */
+
+/* NEOS, November 1994. (Latest revision June 1996.) */
+/* Optimization Technology Center. */
+/* Argonne National Laboratory and Northwestern University. */
+/* Written by */
+/* Ciyou Zhu */
+/* in collaboration with R.H. Byrd, P. Lu-Chen and J. Nocedal. */
+
+
+/* ************ */
+/* Check the input arguments for errors. */
+ /* Parameter adjustments */
+ --nbd;
+ --u;
+ --l;
+
+ /* Function Body */
+ if (*n <= 0) {
+ strcpy(task, "ERROR: N .LE. 0");
+ }
+ if (*m <= 0) {
+ strcpy(task, "ERROR: M .LE. 0");
+ }
+ if (*factr < 0.) {
+ strcpy(task, "ERROR: FACTR .LT. 0");
+ }
+/* Check the validity of the arrays nbd(i), u(i), and l(i). */
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ if (nbd[i__] < 0 || nbd[i__] > 3) {
+/* return */
+ strcpy(task, "ERROR: INVALID NBD");
+ *info = -6;
+ *k = i__;
+ }
+ if (nbd[i__] == 2) {
+ if (l[i__] > u[i__]) {
+/* return */
+ strcpy(task, "ERROR: NO FEASIBLE SOLUTION" );
+ *info = -7;
+ *k = i__;
+ }
+ }
+/* L10: */
+ }
+ return 0;
+} /* errclb_ */
+
+/* ======================= The end of errclb ============================= */
+/* Subroutine */
+static int formk_(
+ int *n, int *nsub, int *ind, int *nenter, int *ileave, int *indx2, int *iupdat,
+ bool *updatd, Real *wn, Real *wn1, int *m,
+ Real *ws, Real *wy, Real *sy, Real *theta,
+ int *col, int *head, int *info)
+{
+ /* System generated locals */
+ int wn_dim1, wn_offset, wn1_dim1, wn1_offset, ws_dim1, ws_offset,
+ wy_dim1, wy_offset, sy_dim1, sy_offset, i__1, i__2, i__3;
+
+ /* Local variables */
+ int dend, pend;
+ int upcl;
+ Real temp1, temp2, temp3, temp4;
+ int i__, k;
+ int ipntr, jpntr, k1, m2, dbegin, is, js, iy, jy, pbegin, is1,
+ js1, col2;
+
+/* ************ */
+
+/* Subroutine formk */
+
+/* This subroutine forms the LEL^T factorization of the indefinite */
+
+/* matrix K = [-D -Y'ZZ'Y/theta L_a'-R_z' ] */
+/* [L_a -R_z theta*S'AA'S ] */
+/* where E = [-I 0] */
+/* [ 0 I] */
+/* The matrix K can be shown to be equal to the matrix M^[-1]N */
+/* occurring in section 5.1 of [1], as well as to the matrix */
+/* Mbar^[-1] Nbar in section 5.3. */
+
+/* n is an int variable. */
+/* On entry n is the dimension of the problem. */
+/* On exit n is unchanged. */
+
+/* nsub is an int variable */
+/* On entry nsub is the number of subspace variables in free set. */
+/* On exit nsub is not changed. */
+
+/* ind is an int array of dimension nsub. */
+/* On entry ind specifies the indices of subspace variables. */
+/* On exit ind is unchanged. */
+
+/* nenter is an int variable. */
+/* On entry nenter is the number of variables entering the */
+/* free set. */
+/* On exit nenter is unchanged. */
+
+/* ileave is an int variable. */
+/* On entry indx2(ileave),...,indx2(n) are the variables leaving */
+/* the free set. */
+/* On exit ileave is unchanged. */
+
+/* indx2 is an int array of dimension n. */
+/* On entry indx2(1),...,indx2(nenter) are the variables entering */
+/* the free set, while indx2(ileave),...,indx2(n) are the */
+/* variables leaving the free set. */
+/* On exit indx2 is unchanged. */
+
+/* iupdat is an int variable. */
+/* On entry iupdat is the total number of BFGS updates made so far. */
+/* On exit iupdat is unchanged. */
+
+/* updatd is a bool variable. */
+/* On entry 'updatd' is true if the L-BFGS matrix is updatd. */
+/* On exit 'updatd' is unchanged. */
+
+/* wn is a double precision array of dimension 2m x 2m. */
+/* On entry wn is unspecified. */
+/* On exit the upper triangle of wn stores the LEL^T factorization */
+/* of the 2*col x 2*col indefinite matrix */
+/* [-D -Y'ZZ'Y/theta L_a'-R_z' ] */
+/* [L_a -R_z theta*S'AA'S ] */
+
+/* wn1 is a double precision array of dimension 2m x 2m. */
+/* On entry wn1 stores the lower triangular part of */
+/* [Y' ZZ'Y L_a'+R_z'] */
+/* [L_a+R_z S'AA'S ] */
+/* in the previous iteration. */
+/* On exit wn1 stores the corresponding updated matrices. */
+/* The purpose of wn1 is just to store these inner products */
+/* so they can be easily updated and inserted into wn. */
+
+/* m is an int variable. */
+/* On entry m is the maximum number of variable metric corrections */
+/* used to define the limited memory matrix. */
+/* On exit m is unchanged. */
+
+/* ws, wy, sy, and wtyy are double precision arrays; */
+/* theta is a double precision variable; */
+/* col is an int variable; */
+/* head is an int variable. */
+/* On entry they store the information defining the */
+/* limited memory BFGS matrix: */
+/* ws(n,m) stores S, a set of s-vectors; */
+/* wy(n,m) stores Y, a set of y-vectors; */
+/* sy(m,m) stores S'Y; */
+/* wtyy(m,m) stores the Cholesky factorization */
+/* of (theta*S'S+LD^(-1)L') */
+/* theta is the scaling factor specifying B_0 = theta I; */
+/* col is the number of variable metric corrections stored; */
+/* head is the location of the 1st s- (or y-) vector in S (or Y). */
+/* On exit they are unchanged. */
+
+/* info is an int variable. */
+/* On entry info is unspecified. */
+/* On exit info = 0 for normal return; */
+/* = -1 when the 1st Cholesky factorization failed; */
+/* = -2 when the 2st Cholesky factorization failed. */
+
+/* Subprograms called: */
+
+/* Linpack ... dcopy, dpofa, dtrsl. */
+
+
+/* References: */
+/* [1] R. H. Byrd, P. Lu, J. Nocedal and C. Zhu, ``A limited */
+/* memory algorithm for bound constrained optimization'', */
+/* SIAM J. Scientific Computing 16 (1995), no. 5, pp. 1190--1208. */
+
+/* [2] C. Zhu, R.H. Byrd, P. Lu, J. Nocedal, ``L-BFGS-B: a */
+/* limited memory FORTRAN code for solving bound constrained */
+/* optimization problems'', Tech. Report, NAM-11, EECS Department, */
+/* Northwestern University, 1994. */
+
+/* (Postscript files of these papers are available via anonymous */
+/* ftp to eecs.nwu.edu in the directory pub/lbfgs/lbfgs_bcm.) */
+
+/* * * * */
+
+/* NEOS, November 1994. (Latest revision June 1996.) */
+/* Optimization Technology Center. */
+/* Argonne National Laboratory and Northwestern University. */
+/* Written by */
+/* Ciyou Zhu */
+/* in collaboration with R.H. Byrd, P. Lu-Chen and J. Nocedal. */
+
+
+/* ************ */
+/* Form the lower triangular part of */
+/* WN1 = [Y' ZZ'Y L_a'+R_z'] */
+/* [L_a+R_z S'AA'S ] */
+/* where L_a is the strictly lower triangular part of S'AA'Y */
+/* R_z is the upper triangular part of S'ZZ'Y. */
+
+ /* Parameter adjustments */
+ --indx2;
+ --ind;
+ sy_dim1 = *m;
+ sy_offset = 1 + sy_dim1 * 1;
+ sy -= sy_offset;
+ wy_dim1 = *n;
+ wy_offset = 1 + wy_dim1 * 1;
+ wy -= wy_offset;
+ ws_dim1 = *n;
+ ws_offset = 1 + ws_dim1 * 1;
+ ws -= ws_offset;
+ wn1_dim1 = 2 * *m;
+ wn1_offset = 1 + wn1_dim1 * 1;
+ wn1 -= wn1_offset;
+ wn_dim1 = 2 * *m;
+ wn_offset = 1 + wn_dim1 * 1;
+ wn -= wn_offset;
+
+ /* Function Body */
+ if (*updatd) {
+ if (*iupdat > *m) {
+/* shift old part of WN1. */
+ i__1 = *m - 1;
+ for (jy = 1; jy <= i__1; ++jy) {
+ js = *m + jy;
+ i__2 = *m - jy;
+ DCOPY((const int&)i__2, &wn1[jy + 1 + (jy + 1) * wn1_dim1], (const int&)c__1, &wn1[
+ jy + jy * wn1_dim1], (const int&)c__1);
+ i__2 = *m - jy;
+ DCOPY((const int&)i__2, &wn1[js + 1 + (js + 1) * wn1_dim1], (const int&)c__1, &wn1[
+ js + js * wn1_dim1], (const int&)c__1);
+ i__2 = *m - 1;
+ DCOPY((const int&)i__2, &wn1[*m + 2 + (jy + 1) * wn1_dim1], (const int&)c__1, &wn1[
+ *m + 1 + jy * wn1_dim1], (const int&)c__1);
+/* L10: */
+ }
+ }
+/* put new rows in blocks (1,1), (2,1) and (2,2). */
+ pbegin = 1;
+ pend = *nsub;
+ dbegin = *nsub + 1;
+ dend = *n;
+ iy = *col;
+ is = *m + *col;
+ ipntr = *head + *col - 1;
+ if (ipntr > *m) {
+ ipntr -= *m;
+ }
+ jpntr = *head;
+ i__1 = *col;
+ for (jy = 1; jy <= i__1; ++jy) {
+ js = *m + jy;
+ temp1 = 0.;
+ temp2 = 0.;
+ temp3 = 0.;
+/* compute element jy of row 'col' of Y'ZZ'Y */
+ i__2 = pend;
+ for (k = pbegin; k <= i__2; ++k) {
+ k1 = ind[k];
+ temp1 += wy[k1 + ipntr * wy_dim1] * wy[k1 + jpntr * wy_dim1];
+/* L15: */
+ }
+/* compute elements jy of row 'col' of L_a and S'AA'S */
+ i__2 = dend;
+ for (k = dbegin; k <= i__2; ++k) {
+ k1 = ind[k];
+ temp2 += ws[k1 + ipntr * ws_dim1] * ws[k1 + jpntr * ws_dim1];
+ temp3 += ws[k1 + ipntr * ws_dim1] * wy[k1 + jpntr * wy_dim1];
+/* L16: */
+ }
+ wn1[iy + jy * wn1_dim1] = temp1;
+ wn1[is + js * wn1_dim1] = temp2;
+ wn1[is + jy * wn1_dim1] = temp3;
+ jpntr = jpntr % *m + 1;
+/* L20: */
+ }
+/* put new column in block (2,1). */
+ jy = *col;
+ jpntr = *head + *col - 1;
+ if (jpntr > *m) {
+ jpntr -= *m;
+ }
+ ipntr = *head;
+ i__1 = *col;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ is = *m + i__;
+ temp3 = 0.;
+/* compute element i of column 'col' of R_z */
+ i__2 = pend;
+ for (k = pbegin; k <= i__2; ++k) {
+ k1 = ind[k];
+ temp3 += ws[k1 + ipntr * ws_dim1] * wy[k1 + jpntr * wy_dim1];
+/* L25: */
+ }
+ ipntr = ipntr % *m + 1;
+ wn1[is + jy * wn1_dim1] = temp3;
+/* L30: */
+ }
+ upcl = *col - 1;
+ } else {
+ upcl = *col;
+ }
+/* modify the old parts in blocks (1,1) and (2,2) due to changes */
+/* in the set of free variables. */
+ ipntr = *head;
+ i__1 = upcl;
+ for (iy = 1; iy <= i__1; ++iy) {
+ is = *m + iy;
+ jpntr = *head;
+ i__2 = iy;
+ for (jy = 1; jy <= i__2; ++jy) {
+ js = *m + jy;
+ temp1 = 0.;
+ temp2 = 0.;
+ temp3 = 0.;
+ temp4 = 0.;
+ i__3 = *nenter;
+ for (k = 1; k <= i__3; ++k) {
+ k1 = indx2[k];
+ temp1 += wy[k1 + ipntr * wy_dim1] * wy[k1 + jpntr * wy_dim1];
+ temp2 += ws[k1 + ipntr * ws_dim1] * ws[k1 + jpntr * ws_dim1];
+/* L35: */
+ }
+ i__3 = *n;
+ for (k = *ileave; k <= i__3; ++k) {
+ k1 = indx2[k];
+ temp3 += wy[k1 + ipntr * wy_dim1] * wy[k1 + jpntr * wy_dim1];
+ temp4 += ws[k1 + ipntr * ws_dim1] * ws[k1 + jpntr * ws_dim1];
+/* L36: */
+ }
+ wn1[iy + jy * wn1_dim1] = wn1[iy + jy * wn1_dim1] + temp1 - temp3;
+ wn1[is + js * wn1_dim1] = wn1[is + js * wn1_dim1] - temp2 + temp4;
+ jpntr = jpntr % *m + 1;
+/* L40: */
+ }
+ ipntr = ipntr % *m + 1;
+/* L45: */
+ }
+/* modify the old parts in block (2,1). */
+ ipntr = *head;
+ i__1 = *m + upcl;
+ for (is = *m + 1; is <= i__1; ++is) {
+ jpntr = *head;
+ i__2 = upcl;
+ for (jy = 1; jy <= i__2; ++jy) {
+ temp1 = 0.;
+ temp3 = 0.;
+ i__3 = *nenter;
+ for (k = 1; k <= i__3; ++k) {
+ k1 = indx2[k];
+ temp1 += ws[k1 + ipntr * ws_dim1] * wy[k1 + jpntr * wy_dim1];
+/* L50: */
+ }
+ i__3 = *n;
+ for (k = *ileave; k <= i__3; ++k) {
+ k1 = indx2[k];
+ temp3 += ws[k1 + ipntr * ws_dim1] * wy[k1 + jpntr * wy_dim1];
+/* L51: */
+ }
+ if (is <= jy + *m) {
+ wn1[is + jy * wn1_dim1] = wn1[is + jy * wn1_dim1] + temp1 -
+ temp3;
+ } else {
+ wn1[is + jy * wn1_dim1] = wn1[is + jy * wn1_dim1] - temp1 +
+ temp3;
+ }
+ jpntr = jpntr % *m + 1;
+/* L55: */
+ }
+ ipntr = ipntr % *m + 1;
+/* L60: */
+ }
+/* Form the upper triangle of WN = [D+Y' ZZ'Y/theta -L_a'+R_z' ] */
+/* [-L_a +R_z S'AA'S*theta] */
+ m2 = *m << 1;
+ i__1 = *col;
+ for (iy = 1; iy <= i__1; ++iy) {
+ is = *col + iy;
+ is1 = *m + iy;
+ i__2 = iy;
+ for (jy = 1; jy <= i__2; ++jy) {
+ js = *col + jy;
+ js1 = *m + jy;
+ wn[jy + iy * wn_dim1] = wn1[iy + jy * wn1_dim1] / *theta;
+ wn[js + is * wn_dim1] = wn1[is1 + js1 * wn1_dim1] * *theta;
+/* L65: */
+ }
+ i__2 = iy - 1;
+ for (jy = 1; jy <= i__2; ++jy) {
+ wn[jy + is * wn_dim1] = -wn1[is1 + jy * wn1_dim1];
+/* L66: */
+ }
+ i__2 = *col;
+ for (jy = iy; jy <= i__2; ++jy) {
+ wn[jy + is * wn_dim1] = wn1[is1 + jy * wn1_dim1];
+/* L67: */
+ }
+ wn[iy + iy * wn_dim1] += sy[iy + iy * sy_dim1];
+/* L70: */
+ }
+/* Form the upper triangle of WN= [ LL' L^-1(-L_a'+R_z')] */
+/* [(-L_a +R_z)L'^-1 S'AA'S*theta ] */
+/* first Cholesky factor (1,1) block of wn to get LL' */
+/* with L' stored in the upper triangle of wn. */
+ DPOTRF((const char &)'U', *col, &wn[wn_offset], (const int &)m2, *info, 1);
+/* dpofa_(&wn[wn_offset], &m2, col, info); LINPACK
+ if (*info != 0) {
+ *info = -1;
+ return 0;
+ }
+*/
+/* then form L^-1(-L_a'+R_z') in the (1,2) block. */
+ col2 = *col << 1;
+ i__1 = col2;
+ for (js = *col + 1; js <= i__1; ++js) {
+ dtrsl_(&wn[wn_offset], &m2, col, &wn[js * wn_dim1 + 1], &c__11, info);
+/* L71: */
+ }
+/* Form S'AA'S*theta + (L^-1(-L_a'+R_z'))'L^-1(-L_a'+R_z') in the */
+/* upper triangle of (2,2) block of wn. */
+ i__1 = col2;
+ for (is = *col + 1; is <= i__1; ++is) {
+ i__2 = col2;
+ for (js = is; js <= i__2; ++js) {
+ wn[is + js * wn_dim1] += DDOT(*col, &wn[is * wn_dim1 + 1], (const int&)c__1,
+ &wn[js * wn_dim1 + 1], (const int&)c__1);
+/* L74: */
+ }
+/* L72: */
+ }
+/* Cholesky factorization of (2,2) block of wn. */
+ DPOTRF((const char&)'U', *col, &wn[*col + 1 + (*col + 1) * wn_dim1], (const int&)m2, *info, 1);
+/* dpofa_(&wn[*col + 1 + (*col + 1) * wn_dim1], &m2, col, info);
+ if (*info != 0) {
+ *info = -2;
+ return 0;
+ }
+*/
+ return 0;
+} /* formk_ */
+
+/* ======================= The end of formk ============================== */
+/* Subroutine */
+static int formt_( int *m, Real *wt, Real *sy, Real *ss,
+int *col, Real *theta, int *info)
+{
+
+ /* System generated locals */
+ int wt_dim1, wt_offset, sy_dim1, sy_offset, ss_dim1, ss_offset, i__1,
+ i__2, i__3;
+
+ /* Local variables */
+ Real ddum;
+ int i__, j, k;
+ int k1;
+
+/* ************ */
+
+/* Subroutine formt */
+
+/* This subroutine forms the upper half of the pos. def. and symm. */
+/* T = theta*SS + L*D^(-1)*L', stores T in the upper triangle */
+/* of the array wt, and performs the Cholesky factorization of T */
+/* to produce J*J', with J' stored in the upper triangle of wt. */
+
+/* Subprograms called: */
+
+/* Linpack ... dpofa. */
+
+
+/* * * * */
+
+/* NEOS, November 1994. (Latest revision June 1996.) */
+/* Optimization Technology Center. */
+/* Argonne National Laboratory and Northwestern University. */
+/* Written by */
+/* Ciyou Zhu */
+/* in collaboration with R.H. Byrd, P. Lu-Chen and J. Nocedal. */
+
+
+/* ************ */
+/* Form the upper half of T = theta*SS + L*D^(-1)*L', */
+/* store T in the upper triangle of the array wt. */
+ /* Parameter adjustments */
+ ss_dim1 = *m;
+ ss_offset = 1 + ss_dim1 * 1;
+ ss -= ss_offset;
+ sy_dim1 = *m;
+ sy_offset = 1 + sy_dim1 * 1;
+ sy -= sy_offset;
+ wt_dim1 = *m;
+ wt_offset = 1 + wt_dim1 * 1;
+ wt -= wt_offset;
+
+ /* Function Body */
+ i__1 = *col;
+ for (j = 1; j <= i__1; ++j) {
+ wt[j * wt_dim1 + 1] = *theta * ss[j * ss_dim1 + 1];
+/* L52: */
+ }
+ i__1 = *col;
+ for (i__ = 2; i__ <= i__1; ++i__) {
+ i__2 = *col;
+ for (j = i__; j <= i__2; ++j) {
+ k1 = imin(i__,j) - 1;
+ ddum = 0.;
+ i__3 = k1;
+ for (k = 1; k <= i__3; ++k) {
+ ddum += sy[i__ + k * sy_dim1] * sy[j + k * sy_dim1] / sy[k +
+ k * sy_dim1];
+/* L53: */
+ }
+ wt[i__ + j * wt_dim1] = ddum + *theta * ss[i__ + j * ss_dim1];
+/* L54: */
+ }
+/* L55: */
+ }
+/* Cholesky factorize T to J*J' with */
+/* J' stored in the upper triangle of wt. */
+ DPOTRF((const char&)'U', *col, &wt[wt_offset], *m, *info, 1);
+/* dpofa_(&wt[wt_offset], m, col, info); LINPACK
+ if (*info != 0) {
+ *info = -3;
+ }
+*/
+ return 0;
+} /* formt_ */
+
+/* ======================= The end of formt ============================== */
+/* Subroutine */
+static int freev_(
+int *n, int *nfree, int *index, int *nenter, int *ileave, int *indx2, int *iwhere,
+bool *wrk, bool *updatd, bool *cnstnd, int *iprint, int *iter)
+{
+ /* System generated locals */
+ int i__1;
+
+ /* Local variables */
+ int iact, i__, k;
+
+ /* Fortran I/O blocks */
+/*
+ static cilist io___169 = { 0, 6, 0, 0, 0 };
+ static cilist io___170 = { 0, 6, 0, 0, 0 };
+ static cilist io___171 = { 0, 6, 0, 0, 0 };
+ static cilist io___173 = { 0, 6, 0, 0, 0 };
+*/
+
+
+/* ************ */
+
+/* Subroutine freev */
+
+/* This subroutine counts the entering and leaving variables when */
+/* iter > 0, and finds the index set of free and active variables */
+/* at the GCP. */
+
+/* cnstnd is a bool variable indicating whether bounds are present */
+
+/* index is an int array of dimension n */
+/* for i=1,...,nfree, index(i) are the indices of free variables */
+/* for i=nfree+1,...,n, index(i) are the indices of bound variables */
+/* On entry after the first iteration, index gives */
+/* the free variables at the previous iteration. */
+/* On exit it gives the free variables based on the determination */
+/* in cauchy using the array iwhere. */
+
+/* indx2 is an int array of dimension n */
+/* On entry indx2 is unspecified. */
+/* On exit with iter>0, indx2 indicates which variables */
+/* have changed status since the previous iteration. */
+/* For i= 1,...,nenter, indx2(i) have changed from bound to free. */
+/* For i= ileave+1,...,n, indx2(i) have changed from free to bound. */
+
+
+/* * * * */
+
+/* NEOS, November 1994. (Latest revision June 1996.) */
+/* Optimization Technology Center. */
+/* Argonne National Laboratory and Northwestern University. */
+/* Written by */
+/* Ciyou Zhu */
+/* in collaboration with R.H. Byrd, P. Lu-Chen and J. Nocedal. */
+
+
+/* ************ */
+ /* Parameter adjustments */
+ --iwhere;
+ --indx2;
+ --index;
+
+ /* Function Body */
+ *nenter = 0;
+ *ileave = *n + 1;
+ if (*iter > 0 && *cnstnd) {
+/* count the entering and leaving variables. */
+ i__1 = *nfree;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ k = index[i__];
+ if (iwhere[k] > 0) {
+ --(*ileave);
+ indx2[*ileave] = k;
+ if (*iprint >= 100) {
+/*
+ s_wsle(&io___169);
+ do_lio(&c__9, &c__1, "Variable ", (ftnlen)9);
+ do_lio(&c__3, &c__1, (char *)&k, (ftnlen)sizeof(int));
+ do_lio(&c__9, &c__1, " leaves the set of free variables",
+ (ftnlen)33);
+ e_wsle();
+*/
+ }
+ }
+/* L20: */
+ }
+ i__1 = *n;
+ for (i__ = *nfree + 1; i__ <= i__1; ++i__) {
+ k = index[i__];
+ if (iwhere[k] <= 0) {
+ ++(*nenter);
+ indx2[*nenter] = k;
+ if (*iprint >= 100) {
+/*
+ s_wsle(&io___170);
+ do_lio(&c__9, &c__1, "Variable ", (ftnlen)9);
+ do_lio(&c__3, &c__1, (char *)&k, (ftnlen)sizeof(int));
+ do_lio(&c__9, &c__1, " enters the set of free variables",
+ (ftnlen)33);
+ e_wsle();
+*/
+ }
+ }
+/* L22: */
+ }
+ if (*iprint >= 99) {
+/*
+ s_wsle(&io___171);
+ i__1 = *n + 1 - *ileave;
+ do_lio(&c__3, &c__1, (char *)&i__1, (ftnlen)sizeof(int));
+ do_lio(&c__9, &c__1, " variables leave; ", (ftnlen)18);
+ do_lio(&c__3, &c__1, (char *)&(*nenter), (ftnlen)sizeof(int));
+ do_lio(&c__9, &c__1, " variables enter", (ftnlen)16);
+ e_wsle();
+*/
+ }
+ }
+ *wrk = *ileave < *n + 1 || *nenter > 0 || *updatd;
+/* Find the index set of free and active variables at the GCP. */
+ *nfree = 0;
+ iact = *n + 1;
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ if (iwhere[i__] <= 0) {
+ ++(*nfree);
+ index[*nfree] = i__;
+ } else {
+ --iact;
+ index[iact] = i__;
+ }
+/* L24: */
+ }
+ if (*iprint >= 99) {
+/*
+ s_wsle(&io___173);
+ do_lio(&c__3, &c__1, (char *)&(*nfree), (ftnlen)sizeof(int));
+ do_lio(&c__9, &c__1, " variables are free at GCP ", (ftnlen)27);
+ i__1 = *iter + 1;
+ do_lio(&c__3, &c__1, (char *)&i__1, (ftnlen)sizeof(int));
+ e_wsle();
+*/
+ }
+ return 0;
+} /* freev_ */
+
+/* ======================= The end of freev ============================== */
+/* Subroutine */
+static int hpsolb_( int *n, Real *t, int *iorder, int *iheap)
+{
+ /* System generated locals */
+ int i__1;
+
+ /* Local variables */
+ Real ddum;
+ int i__, j, k, indxin, indxou;
+ Real out;
+
+/* ************ */
+
+/* Subroutine hpsolb */
+
+/* This subroutine sorts out the least element of t, and puts the */
+/* remaining elements of t in a heap. */
+
+/* n is an int variable. */
+/* On entry n is the dimension of the arrays t and iorder. */
+/* On exit n is unchanged. */
+
+/* t is a double precision array of dimension n. */
+/* On entry t stores the elements to be sorted, */
+/* On exit t(n) stores the least elements of t, and t(1) to t(n-1) */
+/* stores the remaining elements in the form of a heap. */
+
+/* iorder is an int array of dimension n. */
+/* On entry iorder(i) is the index of t(i). */
+/* On exit iorder(i) is still the index of t(i), but iorder may be */
+/* permuted in accordance with t. */
+
+/* iheap is an int variable specifying the task. */
+/* On entry iheap should be set as follows: */
+/* iheap .eq. 0 if t(1) to t(n) is not in the form of a heap, */
+/* iheap .ne. 0 if otherwise. */
+/* On exit iheap is unchanged. */
+
+
+/* References: */
+/* Algorithm 232 of CACM (J. W. J. Williams): HEAPSORT. */
+
+/* * * * */
+
+/* NEOS, November 1994. (Latest revision June 1996.) */
+/* Optimization Technology Center. */
+/* Argonne National Laboratory and Northwestern University. */
+/* Written by */
+/* Ciyou Zhu */
+/* in collaboration with R.H. Byrd, P. Lu-Chen and J. Nocedal. */
+
+/* ************ */
+ /* Parameter adjustments */
+ --iorder;
+ --t;
+
+ /* Function Body */
+ if (*iheap == 0) {
+/* Rearrange the elements t(1) to t(n) to form a heap. */
+ i__1 = *n;
+ for (k = 2; k <= i__1; ++k) {
+ ddum = t[k];
+ indxin = iorder[k];
+/* Add ddum to the heap. */
+ i__ = k;
+L10:
+ if (i__ > 1) {
+ j = i__ / 2;
+ if (ddum < t[j]) {
+ t[i__] = t[j];
+ iorder[i__] = iorder[j];
+ i__ = j;
+ goto L10;
+ }
+ }
+ t[i__] = ddum;
+ iorder[i__] = indxin;
+/* L20: */
+ }
+ }
+/* Assign to 'out' the value of t(1), the least member of the heap, */
+/* and rearrange the remaining members to form a heap as */
+/* elements 1 to n-1 of t. */
+ if (*n > 1) {
+ i__ = 1;
+ out = t[1];
+ indxou = iorder[1];
+ ddum = t[*n];
+ indxin = iorder[*n];
+/* Restore the heap */
+L30:
+ j = i__ + i__;
+ if (j <= *n - 1) {
+ if (t[j + 1] < t[j]) {
+ ++j;
+ }
+ if (t[j] < ddum) {
+ t[i__] = t[j];
+ iorder[i__] = iorder[j];
+ i__ = j;
+ goto L30;
+ }
+ }
+ t[i__] = ddum;
+ iorder[i__] = indxin;
+/* Put the least member in t(n). */
+ t[*n] = out;
+ iorder[*n] = indxou;
+ }
+ return 0;
+} /* hpsolb_ */
+
+/* ====================== The end of hpsolb ============================== */
+/* Subroutine */
+static int lnsrlb_( int *n, Real *l, Real *u, int *nbd,
+Real *x, Real *f, Real *fold, Real *gd, Real *gdold,
+Real *g, Real *d__, Real *r__, Real *t, Real *z__,
+Real *stp, Real *dnorm, Real *dtd, Real *xstep, Real *stpmx,
+int *iter, int *ifun, int *iback, int *nfgv, int *info,
+char *task, bool *boxed, bool *cnstnd, char *csave, int *isave,
+Real *dsave, ftnlen task_len, ftnlen csave_len)
+{
+ const int& nr = *n;
+ /* System generated locals */
+ int i__1;
+ Real d__1;
+
+ /* Local variables */
+ int i__;
+ Real a1, a2;
+ (void)task_len;
+ (void)csave_len;
+
+/* ********** */
+
+/* Subroutine lnsrlb */
+
+/* This subroutine calls subroutine dcsrch from the Minpack2 library */
+/* to perform the line search. Subroutine dscrch is safeguarded so */
+/* that all trial points lie within the feasible region. */
+
+/* Subprograms called: */
+
+/* Minpack2 Library ... dcsrch. */
+
+/* Linpack ... dtrsl, ddot. */
+
+
+/* * * * */
+
+/* NEOS, November 1994. (Latest revision June 1996.) */
+/* Optimization Technology Center. */
+/* Argonne National Laboratory and Northwestern University. */
+/* Written by */
+/* Ciyou Zhu */
+/* in collaboration with R.H. Byrd, P. Lu-Chen and J. Nocedal. */
+
+
+/* ********** */
+ /* Parameter adjustments */
+ --z__;
+ --t;
+ --r__;
+ --d__;
+ --g;
+ --x;
+ --nbd;
+ --u;
+ --l;
+ --isave;
+ --dsave;
+
+ /* Function Body */
+ if (strncmp(task, "FG_LN", 5) == 0) {
+ goto L556;
+ }
+ *dtd = DDOT(nr, &d__[1], (const int&)c__1, &d__[1], (const int&)c__1);
+ *dnorm = std::sqrt(*dtd);
+/* Determine the maximum step length. */
+ *stpmx = 1e10;
+ if (*cnstnd) {
+ if (*iter == 0) {
+ *stpmx = 1.;
+ } else {
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ a1 = d__[i__];
+ if (nbd[i__] != 0) {
+ if (a1 < 0. && nbd[i__] <= 2) {
+ a2 = l[i__] - x[i__];
+ if (a2 >= 0.) {
+ *stpmx = 0.;
+ } else if (a1 * *stpmx < a2) {
+ *stpmx = a2 / a1;
+ }
+ } else if (a1 > 0. && nbd[i__] >= 2) {
+ a2 = u[i__] - x[i__];
+ if (a2 <= 0.) {
+ *stpmx = 0.;
+ } else if (a1 * *stpmx > a2) {
+ *stpmx = a2 / a1;
+ }
+ }
+ }
+/* L43: */
+ }
+ }
+ }
+ if (*iter == 0 && ! (*boxed)) {
+/* Computing MIN */
+ d__1 = Real(1) / *dnorm;
+ *stp = std::min(d__1,*stpmx);
+ } else {
+ *stp = 1.;
+ }
+ DCOPY(nr, &x[1], (const int&)c__1, &t[1], (const int&)c__1);
+ DCOPY(nr, &g[1], (const int&)c__1, &r__[1], (const int&)c__1);
+ *fold = *f;
+ *ifun = 0;
+ *iback = 0;
+ strcpy(csave, "START");
+L556:
+ *gd = DDOT(nr, &g[1], (const int&)c__1, &d__[1], (const int&)c__1);
+ if (*ifun == 0) {
+ *gdold = *gd;
+ if (*gd >= 0.) {
+/* the directional derivative >=0. */
+/* Line search is impossible. */
+ *info = -4;
+ return 0;
+ }
+ }
+ dcsrch_(f, gd, stp, &c_b275, &c_b276, &c_b277, &c_b9, stpmx, csave, &
+ isave[1], &dsave[1], (ftnlen)60);
+ *xstep = *stp * *dnorm;
+ if (strncmp(csave, "CONV", 4 ) != 0 && strncmp(csave, "WARN", 4) != 0) {
+ strcpy(task, "FG_LNSRCH");
+ ++(*ifun);
+ ++(*nfgv);
+ *iback = *ifun - 1;
+ if (*stp == 1.) {
+ DCOPY(nr, &z__[1], (const int&)c__1, &x[1], (const int&)c__1);
+ } else {
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ x[i__] = *stp * d__[i__] + t[i__];
+/* L41: */
+ }
+ }
+ } else {
+ strcpy(task, "NEW_X");
+ }
+ return 0;
+} /* lnsrlb_ */
+
+/* ======================= The end of lnsrlb ============================= */
+/* Subroutine */
+static int matupd_( int *n, int *m,
+Real *ws, Real *wy, Real *sy, Real *ss, Real *d__, Real *r__,
+int *itail, int *iupdat, int *col, int *head,
+Real *theta, Real *rr, Real *dr, Real *stp, Real *dtd)
+{
+ /* System generated locals */
+ int ws_dim1, ws_offset, wy_dim1, wy_offset, sy_dim1, sy_offset,
+ ss_dim1, ss_offset, i__1, i__2;
+
+
+ /* Local variables */
+ int j;
+ int pointr;
+ const int& jr = j;
+
+/* ************ */
+
+/* Subroutine matupd */
+
+/* This subroutine updates matrices WS and WY, and forms the */
+/* middle matrix in B. */
+
+/* Subprograms called: */
+
+/* Linpack ... dcopy, ddot. */
+
+
+/* * * * */
+
+/* NEOS, November 1994. (Latest revision June 1996.) */
+/* Optimization Technology Center. */
+/* Argonne National Laboratory and Northwestern University. */
+/* Written by */
+/* Ciyou Zhu */
+/* in collaboration with R.H. Byrd, P. Lu-Chen and J. Nocedal. */
+
+
+/* ************ */
+/* Set pointers for matrices WS and WY. */
+ /* Parameter adjustments */
+ --r__;
+ --d__;
+ ss_dim1 = *m;
+ ss_offset = 1 + ss_dim1 * 1;
+ ss -= ss_offset;
+ sy_dim1 = *m;
+ sy_offset = 1 + sy_dim1 * 1;
+ sy -= sy_offset;
+ wy_dim1 = *n;
+ wy_offset = 1 + wy_dim1 * 1;
+ wy -= wy_offset;
+ ws_dim1 = *n;
+ ws_offset = 1 + ws_dim1 * 1;
+ ws -= ws_offset;
+
+ /* Function Body */
+ if (*iupdat <= *m) {
+ *col = *iupdat;
+ *itail = (*head + *iupdat - 2) % *m + 1;
+ } else {
+ *itail = *itail % *m + 1;
+ *head = *head % *m + 1;
+ }
+/* Update matrices WS and WY. */
+ DCOPY(*n, &d__[1], (const int&)c__1, &ws[*itail * ws_dim1 + 1], (const int&)c__1);
+ DCOPY(*n, &r__[1], (const int&)c__1, &wy[*itail * wy_dim1 + 1], (const int&)c__1);
+/* Set theta=yy/ys. */
+ *theta = *rr / *dr;
+/* Form the middle matrix in B. */
+/* update the upper triangle of SS, */
+/* and the lower triangle of SY: */
+ if (*iupdat > *m) {
+/* move old information */
+ i__1 = *col - 1;
+ for (j = 1; j <= i__1; ++j) {
+ DCOPY((const int &)j, &ss[(j + 1) * ss_dim1 + 2], (const int&)c__1, &ss[j * ss_dim1 + 1]
+ , (const int&)c__1);
+ i__2 = *col - j;
+ DCOPY((const int&)i__2, &sy[j + 1 + (j + 1) * sy_dim1], (const int&)c__1, &sy[j + j *
+ sy_dim1], (const int&)c__1);
+/* L50: */
+ }
+ }
+/* add new information: the last row of SY */
+/* and the last column of SS: */
+ pointr = *head;
+ i__1 = *col - 1;
+ for (j = 1; j <= i__1; ++j) {
+ sy[*col + j * sy_dim1] = DDOT(*n, &d__[1], (const int&)c__1, &wy[pointr *
+ wy_dim1 + 1], (const int&)c__1);
+ ss[j + *col * ss_dim1] = DDOT(*n, &ws[pointr * ws_dim1 + 1], (const int&)c__1, &
+ d__[1], (const int&)c__1);
+ pointr = pointr % *m + 1;
+/* L51: */
+ }
+ if (*stp == 1.) {
+ ss[*col + *col * ss_dim1] = *dtd;
+ } else {
+ ss[*col + *col * ss_dim1] = *stp * *stp * *dtd;
+ }
+ sy[*col + *col * sy_dim1] = *dr;
+ return 0;
+} /* matupd_ */
+
+/* ======================= The end of matupd ============================= */
+/* Subroutine */
+static int prn1lb_( int *n, int *m, Real *l, Real *u, Real *x,
+int *iprint, int *itfile, Real *epsmch)
+{
+ (void)n;
+ (void)m;
+ (void)l;
+ (void)u;
+ (void)x;
+ (void)iprint;
+ (void)itfile;
+ (void)epsmch;
+#if 0
+ /* Format strings */
+/*
+ static char fmt_7001[] = "(\002RUNNING THE L-BFGS-B CODE\002,/,/,\002 \
+ * * *\002,/,/,\002Machine precision =\002,1p,d10.3)";
+ static char fmt_2001[] = "(\002RUNNING THE L-BFGS-B CODE\002,/,/,\002it \
+ = iteration number\002,/,\002nf = number of function evaluations\002,/,\
+\002nint = number of segments explored during the Cauchy search\002,/,\002n\
+act = number of active bounds at the generalized Cauchy point\002,/,\002sub\
+ = manner in which the subspace minimization terminated:\002,/,\002 \
+ con = converged, bnd = a bound was reached\002,/,\002itls = number of iter\
+ations performed in the line search\002,/,\002stepl = step length used\002,/,\
+\002tstep = norm of the displacement (total step)\002,/,\002projg = norm of \
+the projected gradient\002,/,\002f = function value\002,/,/,\002 \
+ * * *\002,/,/,\002Machine precision =\002,1p,d10.3)";
+ static char fmt_9001[] = "(/,3x,\002it\002,3x,\002nf\002,2x,\002nint\002\
+,2x,\002nact\002,2x,\002sub\002,2x,\002itls\002,2x,\002stepl\002,4x,\002tstep\
+\002,5x,\002projg\002,8x,\002f\002)";
+ static char fmt_1004[] = "(/,a4,1p,6(1x,d11.4),/,(4x,1p,6(1x,d11.4)))";
+*/
+ /* System generated locals */
+ int i__1;
+
+ /* Local variables */
+ static int i__;
+
+ /* Fortran I/O blocks */
+ static cilist io___186 = { 0, 6, 0, fmt_7001, 0 };
+ static cilist io___187 = { 0, 6, 0, 0, 0 };
+ static cilist io___188 = { 0, 0, 0, fmt_2001, 0 };
+ static cilist io___189 = { 0, 0, 0, 0, 0 };
+ static cilist io___190 = { 0, 0, 0, fmt_9001, 0 };
+ static cilist io___191 = { 0, 6, 0, fmt_1004, 0 };
+ static cilist io___193 = { 0, 6, 0, fmt_1004, 0 };
+ static cilist io___194 = { 0, 6, 0, fmt_1004, 0 };
+
+
+/* ************ */
+
+/* Subroutine prn1lb */
+
+/* This subroutine prints the input data, initial point, upper and */
+/* lower bounds of each variable, machine precision, as well as */
+/* the headings of the output. */
+
+
+/* * * * */
+
+/* NEOS, November 1994. (Latest revision June 1996.) */
+/* Optimization Technology Center. */
+/* Argonne National Laboratory and Northwestern University. */
+/* Written by */
+/* Ciyou Zhu */
+/* in collaboration with R.H. Byrd, P. Lu-Chen and J. Nocedal. */
+
+
+/* ************ */
+ /* Parameter adjustments */
+ --x;
+ --u;
+ --l;
+
+ /* Function Body */
+ if (*iprint >= 0) {
+
+ s_wsfe(&io___186);
+ do_fio(&c__1, (char *)&(*epsmch), (ftnlen)sizeof(Real));
+ e_wsfe();
+
+ s_wsle(&io___187);
+ do_lio(&c__9, &c__1, "N = ", (ftnlen)4);
+ do_lio(&c__3, &c__1, (char *)&(*n), (ftnlen)sizeof(int));
+ do_lio(&c__9, &c__1, " M = ", (ftnlen)8);
+ do_lio(&c__3, &c__1, (char *)&(*m), (ftnlen)sizeof(int));
+ e_wsle();
+ if (*iprint >= 1) {
+
+ io___188.ciunit = *itfile;
+ s_wsfe(&io___188);
+ do_fio(&c__1, (char *)&(*epsmch), (ftnlen)sizeof(Real));
+ e_wsfe();
+
+ io___189.ciunit = *itfile;
+ s_wsle(&io___189);
+ do_lio(&c__9, &c__1, "N = ", (ftnlen)4);
+ do_lio(&c__3, &c__1, (char *)&(*n), (ftnlen)sizeof(int));
+ do_lio(&c__9, &c__1, " M = ", (ftnlen)8);
+ do_lio(&c__3, &c__1, (char *)&(*m), (ftnlen)sizeof(int));
+ e_wsle();
+
+ io___190.ciunit = *itfile;
+ s_wsfe(&io___190);
+ e_wsfe();
+
+
+ if (*iprint > 100) {
+ s_wsfe(&io___191);
+ do_fio(&c__1, "L =", (ftnlen)3);
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ do_fio(&c__1, (char *)&l[i__], (ftnlen)sizeof(Real))
+ ;
+ }
+ e_wsfe();
+ s_wsfe(&io___193);
+ do_fio(&c__1, "X0 =", (ftnlen)4);
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ do_fio(&c__1, (char *)&x[i__], (ftnlen)sizeof(Real))
+ ;
+ }
+ e_wsfe();
+ s_wsfe(&io___194);
+ do_fio(&c__1, "U =", (ftnlen)3);
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ do_fio(&c__1, (char *)&u[i__], (ftnlen)sizeof(Real))
+ ;
+ }
+ e_wsfe();
+ }
+
+ }
+ }
+#endif
+ return 0;
+} /* prn1lb_ */
+
+/* ======================= The end of prn1lb ============================= */
+/* Subroutine */
+static int prn2lb_( int *n, Real *x, Real *f, Real *g,
+int *iprint, int *itfile, int *iter, int *nfgv, int *nact,
+Real *sbgnrm, int *nint, char *word, int *iword, int *iback,
+Real *stp, Real *xstep, ftnlen word_len)
+{
+ (void)n;
+ (void)x;
+ (void)f;
+ (void)g;
+ (void)iprint;
+ (void)itfile;
+ (void)iter;
+ (void)nfgv;
+ (void)nact;
+ (void)sbgnrm;
+ (void)nint;
+ (void)word;
+ (void)iword;
+ (void)iback;
+ (void)stp;
+ (void)xstep;
+ (void)word_len;
+#if 0
+ /* Format strings */
+ static char fmt_2001[] = "(/,\002At iterate\002,i5,4x,\002f= \002,1p,d12\
+.5,4x,\002|proj g|= \002,1p,d12.5)";
+ static char fmt_1004[] = "(/,a4,1p,6(1x,d11.4),/,(4x,1p,6(1x,d11.4)))";
+ static char fmt_3001[] = "(2(1x,i4),2(1x,i5),2x,a3,1x,i4,1p,2(2x,d7.1),1\
+p,2(1x,d10.3))";
+
+ /* System generated locals */
+ int i__1;
+
+ /* Local variables */
+ static int imod, i__;
+
+ /* Fortran I/O blocks */
+ static cilist io___195 = { 0, 6, 0, 0, 0 };
+ static cilist io___196 = { 0, 6, 0, fmt_2001, 0 };
+ static cilist io___197 = { 0, 6, 0, fmt_1004, 0 };
+ static cilist io___199 = { 0, 6, 0, fmt_1004, 0 };
+ static cilist io___201 = { 0, 6, 0, fmt_2001, 0 };
+ static cilist io___202 = { 0, 0, 0, fmt_3001, 0 };
+
+
+/* ************ */
+
+/* Subroutine prn2lb */
+
+/* This subroutine prints out new information after a successful */
+/* line search. */
+
+
+/* * * * */
+
+/* NEOS, November 1994. (Latest revision June 1996.) */
+/* Optimization Technology Center. */
+/* Argonne National Laboratory and Northwestern University. */
+/* Written by */
+/* Ciyou Zhu */
+/* in collaboration with R.H. Byrd, P. Lu-Chen and J. Nocedal. */
+
+
+/* ************ */
+/* 'word' records the status of subspace solutions. */
+ /* Parameter adjustments */
+ --g;
+ --x;
+
+ /* Function Body */
+ if (*iword == 0) {
+/* the subspace minimization converged. */
+ s_copy(word, "con", (ftnlen)3, (ftnlen)3);
+ } else if (*iword == 1) {
+/* the subspace minimization stopped at a bound. */
+ s_copy(word, "bnd", (ftnlen)3, (ftnlen)3);
+ } else if (*iword == 5) {
+/* the truncated Newton step has been used. */
+ s_copy(word, "TNT", (ftnlen)3, (ftnlen)3);
+ } else {
+ s_copy(word, "---", (ftnlen)3, (ftnlen)3);
+ }
+ if (*iprint >= 99) {
+ s_wsle(&io___195);
+ do_lio(&c__9, &c__1, "LINE SEARCH", (ftnlen)11);
+ do_lio(&c__3, &c__1, (char *)&(*iback), (ftnlen)sizeof(int));
+ do_lio(&c__9, &c__1, " times; norm of step = ", (ftnlen)23);
+ do_lio(&c__5, &c__1, (char *)&(*xstep), (ftnlen)sizeof(Real));
+ e_wsle();
+ s_wsfe(&io___196);
+ do_fio(&c__1, (char *)&(*iter), (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&(*f), (ftnlen)sizeof(Real));
+ do_fio(&c__1, (char *)&(*sbgnrm), (ftnlen)sizeof(Real));
+ e_wsfe();
+ if (*iprint > 100) {
+ s_wsfe(&io___197);
+ do_fio(&c__1, "X =", (ftnlen)3);
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ do_fio(&c__1, (char *)&x[i__], (ftnlen)sizeof(Real));
+ }
+ e_wsfe();
+ s_wsfe(&io___199);
+ do_fio(&c__1, "G =", (ftnlen)3);
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ do_fio(&c__1, (char *)&g[i__], (ftnlen)sizeof(Real));
+ }
+ e_wsfe();
+ }
+ } else if (*iprint > 0) {
+ imod = *iter % *iprint;
+ if (imod == 0) {
+ s_wsfe(&io___201);
+ do_fio(&c__1, (char *)&(*iter), (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&(*f), (ftnlen)sizeof(Real));
+ do_fio(&c__1, (char *)&(*sbgnrm), (ftnlen)sizeof(Real));
+ e_wsfe();
+ }
+ }
+ if (*iprint >= 1) {
+ io___202.ciunit = *itfile;
+ s_wsfe(&io___202);
+ do_fio(&c__1, (char *)&(*iter), (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&(*nfgv), (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&(*nint), (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&(*nact), (ftnlen)sizeof(int));
+ do_fio(&c__1, word, (ftnlen)3);
+ do_fio(&c__1, (char *)&(*iback), (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&(*stp), (ftnlen)sizeof(Real));
+ do_fio(&c__1, (char *)&(*xstep), (ftnlen)sizeof(Real));
+ do_fio(&c__1, (char *)&(*sbgnrm), (ftnlen)sizeof(Real));
+ do_fio(&c__1, (char *)&(*f), (ftnlen)sizeof(Real));
+ e_wsfe();
+ }
+#endif
+ return 0;
+} /* prn2lb_ */
+
+/* ======================= The end of prn2lb ============================= */
+/* Subroutine */
+static int prn3lb_( int *n, Real *x, Real *f, char *task,
+int *iprint, int *info, int *itfile, int *iter, int *nfgv, int *nintol, int *nskip, int *nact,
+Real *sbgnrm, const Real *time, int *nint, char *word, int *iback,
+Real *stp, Real *xstep, int *k,
+Real *cachyt, Real *sbtime, Real *lnscht, ftnlen task_len,
+ftnlen word_len)
+{
+ (void)n;
+ (void)x;
+ (void)f;
+ (void)task;
+ (void)iprint;
+ (void)info;
+ (void)itfile;
+ (void)iter;
+ (void)nfgv;
+ (void)nintol;
+ (void)nskip;
+ (void)nact;
+ (void)sbgnrm;
+ (void)time;
+ (void)nint;
+ (void)word;
+ (void)iback;
+ (void)stp;
+ (void)xstep;
+ (void)k;
+ (void)cachyt;
+ (void)sbtime;
+ (void)lnscht;
+ (void)task_len;
+ (void)word_len;
+#if 0
+ /* Format strings */
+ static char fmt_3003[] = "(/,\002 * * *\002,/,/,\002Tit = to\
+tal number of iterations\002,/,\002Tnf = total number of function evaluati\
+ons\002,/,\002Tnint = total number of segments explored during\002,\002 Cauc\
+hy searches\002,/,\002Skip = number of BFGS updates skipped\002,/,\002Nact \
+ = number of active bounds at final generalized\002,\002 Cauchy point\002,/\
+,\002Projg = norm of the final projected gradient\002,/,\002F = final fu\
+nction value\002,/,/,\002 * * *\002)";
+ static char fmt_3004[] = "(/,3x,\002N\002,3x,\002Tit\002,2x,\002Tnf\002,\
+2x,\002Tnint\002,2x,\002Skip\002,2x,\002Nact\002,5x,\002Projg\002,8x,\002\
+F\002)";
+ static char fmt_3005[] = "(i5,2(1x,i4),(1x,i6),(2x,i4),(1x,i5),1p,2(2x,d\
+10.3))";
+ static char fmt_1004[] = "(/,a4,1p,6(1x,d11.4),/,(4x,1p,6(1x,d11.4)))";
+ static char fmt_3009[] = "(/,a60)";
+ static char fmt_9011[] = "(/,\002 Matrix in 1st Cholesky factorization i\
+n formk is not Pos. Def.\002)";
+ static char fmt_9012[] = "(/,\002 Matrix in 2st Cholesky factorization i\
+n formk is not Pos. Def.\002)";
+ static char fmt_9013[] = "(/,\002 Matrix in the Cholesky factorization i\
+n formt is not Pos. Def.\002)";
+ static char fmt_9014[] = "(/,\002 Derivative >= 0, backtracking line sea\
+rch impossible.\002,/,\002 Previous x, f and g restored.\002,/,\002 Possib\
+le causes: 1 error in function or gradient evaluation;\002,/,\002 \
+ 2 rounding errors dominate computation.\002)";
+ static char fmt_9015[] = "(/,\002 Warning: more than 10 function and gr\
+adient\002,/,\002 evaluations in the last line search. Termination\002,/\
+,\002 may possibly be caused by a bad search direction.\002)";
+ static char fmt_9018[] = "(/,\002 The triangular system is singular.\002)"
+ ;
+ static char fmt_9019[] = "(/,\002 Line search cannot locate an adequate \
+point after 20 function\002,/,\002 and gradient evaluations. Previous x, f\
+ and g restored.\002,/,\002 Possible causes: 1 error in function or gradient\
+ evaluation;\002,/,\002 2 rounding error dominate computati\
+on.\002)";
+ static char fmt_3007[] = "(/,\002 Cauchy time\002,1p,e10.\
+3,\002 seconds.\002,/\002 Subspace minimization time\002,1p,e10.3,\002 secon\
+ds.\002,/\002 Line search time\002,1p,e10.3,\002 seconds.\002)";
+ static char fmt_3008[] = "(/,\002 Total User time\002,1p,e10.3,\002 seco\
+nds.\002,/)";
+ static char fmt_3002[] = "(2(1x,i4),2(1x,i5),2x,a3,1x,i4,1p,2(2x,d7.1),6\
+x,\002-\002,10x,\002-\002)";
+
+ /* System generated locals */
+ int i__1;
+
+ /* Local variables */
+ static int i__;
+
+ /* Fortran I/O blocks */
+ static cilist io___203 = { 0, 6, 0, fmt_3003, 0 };
+ static cilist io___204 = { 0, 6, 0, fmt_3004, 0 };
+ static cilist io___205 = { 0, 6, 0, fmt_3005, 0 };
+ static cilist io___206 = { 0, 6, 0, fmt_1004, 0 };
+ static cilist io___208 = { 0, 6, 0, 0, 0 };
+ static cilist io___209 = { 0, 6, 0, fmt_3009, 0 };
+ static cilist io___210 = { 0, 6, 0, fmt_9011, 0 };
+ static cilist io___211 = { 0, 6, 0, fmt_9012, 0 };
+ static cilist io___212 = { 0, 6, 0, fmt_9013, 0 };
+ static cilist io___213 = { 0, 6, 0, fmt_9014, 0 };
+ static cilist io___214 = { 0, 6, 0, fmt_9015, 0 };
+ static cilist io___215 = { 0, 6, 0, 0, 0 };
+ static cilist io___216 = { 0, 6, 0, 0, 0 };
+ static cilist io___217 = { 0, 6, 0, fmt_9018, 0 };
+ static cilist io___218 = { 0, 6, 0, fmt_9019, 0 };
+ static cilist io___219 = { 0, 6, 0, fmt_3007, 0 };
+ static cilist io___220 = { 0, 6, 0, fmt_3008, 0 };
+ static cilist io___221 = { 0, 0, 0, fmt_3002, 0 };
+ static cilist io___222 = { 0, 0, 0, fmt_3009, 0 };
+ static cilist io___223 = { 0, 0, 0, fmt_9011, 0 };
+ static cilist io___224 = { 0, 0, 0, fmt_9012, 0 };
+ static cilist io___225 = { 0, 0, 0, fmt_9013, 0 };
+ static cilist io___226 = { 0, 0, 0, fmt_9014, 0 };
+ static cilist io___227 = { 0, 0, 0, fmt_9015, 0 };
+ static cilist io___228 = { 0, 0, 0, fmt_9018, 0 };
+ static cilist io___229 = { 0, 0, 0, fmt_9019, 0 };
+ static cilist io___230 = { 0, 0, 0, fmt_3008, 0 };
+
+
+/* ************ */
+
+/* Subroutine prn3lb */
+
+/* This subroutine prints out information when either a built-in */
+/* convergence test is satisfied or when an error message is */
+/* generated. */
+
+
+/* * * * */
+
+/* NEOS, November 1994. (Latest revision June 1996.) */
+/* Optimization Technology Center. */
+/* Argonne National Laboratory and Northwestern University. */
+/* Written by */
+/* Ciyou Zhu */
+/* in collaboration with R.H. Byrd, P. Lu-Chen and J. Nocedal. */
+
+
+/* ************ */
+ /* Parameter adjustments */
+ --x;
+
+ /* Function Body */
+ if (strncmp(task, "ERROR", 5) == 0) {
+ goto L999;
+ }
+ if (*iprint >= 0) {
+ s_wsfe(&io___203);
+ e_wsfe();
+ s_wsfe(&io___204);
+ e_wsfe();
+ s_wsfe(&io___205);
+ do_fio(&c__1, (char *)&(*n), (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&(*iter), (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&(*nfgv), (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&(*nintol), (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&(*nskip), (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&(*nact), (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&(*sbgnrm), (ftnlen)sizeof(Real));
+ do_fio(&c__1, (char *)&(*f), (ftnlen)sizeof(Real));
+ e_wsfe();
+ if (*iprint >= 100) {
+ s_wsfe(&io___206);
+ do_fio(&c__1, "X =", (ftnlen)3);
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ do_fio(&c__1, (char *)&x[i__], (ftnlen)sizeof(Real));
+ }
+ e_wsfe();
+ }
+ if (*iprint >= 1) {
+ s_wsle(&io___208);
+ do_lio(&c__9, &c__1, " F =", (ftnlen)4);
+ do_lio(&c__5, &c__1, (char *)&(*f), (ftnlen)sizeof(Real));
+ e_wsle();
+ }
+ }
+L999:
+ if (*iprint >= 0) {
+ s_wsfe(&io___209);
+ do_fio(&c__1, task, (ftnlen)60);
+ e_wsfe();
+ if (*info != 0) {
+ if (*info == -1) {
+ s_wsfe(&io___210);
+ e_wsfe();
+ }
+ if (*info == -2) {
+ s_wsfe(&io___211);
+ e_wsfe();
+ }
+ if (*info == -3) {
+ s_wsfe(&io___212);
+ e_wsfe();
+ }
+ if (*info == -4) {
+ s_wsfe(&io___213);
+ e_wsfe();
+ }
+ if (*info == -5) {
+ s_wsfe(&io___214);
+ e_wsfe();
+ }
+ if (*info == -6) {
+ s_wsle(&io___215);
+ do_lio(&c__9, &c__1, " Input nbd(", (ftnlen)11);
+ do_lio(&c__3, &c__1, (char *)&(*k), (ftnlen)sizeof(int));
+ do_lio(&c__9, &c__1, ") is invalid.", (ftnlen)13);
+ e_wsle();
+ }
+ if (*info == -7) {
+ s_wsle(&io___216);
+ do_lio(&c__9, &c__1, " l(", (ftnlen)3);
+ do_lio(&c__3, &c__1, (char *)&(*k), (ftnlen)sizeof(int));
+ do_lio(&c__9, &c__1, ") > u(", (ftnlen)6);
+ do_lio(&c__3, &c__1, (char *)&(*k), (ftnlen)sizeof(int));
+ do_lio(&c__9, &c__1, "). No feasible solution.", (ftnlen)25);
+ e_wsle();
+ }
+ if (*info == -8) {
+ s_wsfe(&io___217);
+ e_wsfe();
+ }
+ if (*info == -9) {
+ s_wsfe(&io___218);
+ e_wsfe();
+ }
+ }
+ if (*iprint >= 1) {
+ s_wsfe(&io___219);
+ do_fio(&c__1, (char *)&(*cachyt), (ftnlen)sizeof(Real));
+ do_fio(&c__1, (char *)&(*sbtime), (ftnlen)sizeof(Real));
+ do_fio(&c__1, (char *)&(*lnscht), (ftnlen)sizeof(Real));
+ e_wsfe();
+ }
+ s_wsfe(&io___220);
+ do_fio(&c__1, (char *)&(*time), (ftnlen)sizeof(Real));
+ e_wsfe();
+ if (*iprint >= 1) {
+ if (*info == -4 || *info == -9) {
+ io___221.ciunit = *itfile;
+ s_wsfe(&io___221);
+ do_fio(&c__1, (char *)&(*iter), (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&(*nfgv), (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&(*nint), (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&(*nact), (ftnlen)sizeof(int));
+ do_fio(&c__1, word, (ftnlen)3);
+ do_fio(&c__1, (char *)&(*iback), (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&(*stp), (ftnlen)sizeof(Real));
+ do_fio(&c__1, (char *)&(*xstep), (ftnlen)sizeof(Real));
+ e_wsfe();
+ }
+ io___222.ciunit = *itfile;
+ s_wsfe(&io___222);
+ do_fio(&c__1, task, (ftnlen)60);
+ e_wsfe();
+ if (*info != 0) {
+ if (*info == -1) {
+ io___223.ciunit = *itfile;
+ s_wsfe(&io___223);
+ e_wsfe();
+ }
+ if (*info == -2) {
+ io___224.ciunit = *itfile;
+ s_wsfe(&io___224);
+ e_wsfe();
+ }
+ if (*info == -3) {
+ io___225.ciunit = *itfile;
+ s_wsfe(&io___225);
+ e_wsfe();
+ }
+ if (*info == -4) {
+ io___226.ciunit = *itfile;
+ s_wsfe(&io___226);
+ e_wsfe();
+ }
+ if (*info == -5) {
+ io___227.ciunit = *itfile;
+ s_wsfe(&io___227);
+ e_wsfe();
+ }
+ if (*info == -8) {
+ io___228.ciunit = *itfile;
+ s_wsfe(&io___228);
+ e_wsfe();
+ }
+ if (*info == -9) {
+ io___229.ciunit = *itfile;
+ s_wsfe(&io___229);
+ e_wsfe();
+ }
+ }
+ io___230.ciunit = *itfile;
+ s_wsfe(&io___230);
+ do_fio(&c__1, (char *)&(*time), (ftnlen)sizeof(Real));
+ e_wsfe();
+ }
+ }
+/* L3006: */
+#endif
+ return 0;
+} /* prn3lb_ */
+
+/* ======================= The end of prn3lb ============================= */
+/* Subroutine */
+static int projgr_( int *n, Real *l, Real *u, int *nbd,
+ Real *x, Real f, Real *g,
+ Real *sbgnrm)
+{
+/* ************ */
+
+/* Subroutine projgr */
+
+/* This subroutine computes the infinity norm of the projected */
+/* gradient. */
+
+/* NEOS, November 1994. (Latest revision June 1996.) */
+/* Optimization Technology Center. */
+/* Argonne National Laboratory and Northwestern University. */
+/* Written by */
+/* Ciyou Zhu */
+/* in collaboration with R.H. Byrd, P. Lu-Chen and J. Nocedal. */
+
+// sherm 100303: from Byrd, Lu, Nocedal, Zhu Northwestern U. Tech.
+// Report NAM08, 1994, Eqs. 2.2 and 6.1, this calculates the projected
+// gradient this way:
+// 2.2 P(x,l,u)_i = { l_i, x_i < l_i
+// { x_i, x_i in [l_i,u_i]
+// { u_i, x_i > u_i
+//
+// Now take a unit step in the gradient direction, but use the above
+// piecewise linear function to trim the final point to the bounds. Then
+// recalculate the direction that was actually taken; that is the projected
+// gradient pg:
+// 6.1 pg = P(x - g, l, u) - x
+//
+// To make this scale independent for both the function f and variables x,
+// we take pg = df/dx * (1/f) * x (with suitable worrying for f or x near
+// zero) to get pg' = (%chg f)/(%chg x), i.e. the fractional change of f
+// caused by a "unit" change in x. We return the infinity norm of pg'.
+//
+
+/* ************ */
+ /* Adjustments to allow 1-based indexing. */
+ --g;
+ --x;
+ --nbd;
+ --u;
+ --l;
+
+ /* Function Body */
+ const Real fscale = 1 / std::max(Real(0.1), std::abs(f));
+ *sbgnrm = 0.;
+ for (int i = 1; i <= *n; ++i) {
+ Real gi = g[i];
+ if (nbd[i] != 0) {
+ if (gi < 0.) {
+ if (nbd[i] >= 2) {
+ /* Computing MAX */
+ const Real d = x[i] - u[i]; // -distance to upper bound
+ gi = std::max(d,gi);
+ }
+ } else { // gi >= 0
+ if (nbd[i] <= 2) {
+ /* Computing MIN */
+ const Real d = x[i] - l[i]; // distance to lower bound
+ gi = std::min(d,gi);
+ }
+ }
+ }
+
+ // For large x, we want to know how the function changes with an
+ // "x-sized" change, not a "1-sized" change.
+ const Real xscale = std::max(Real(1), std::abs(x[i]));
+ // Retain the largest absolute value.
+ *sbgnrm = std::max(*sbgnrm, std::abs(gi)*fscale*xscale);
+ }
+ return 0;
+} /* projgr_ */
+
+/* ======================= The end of projgr ============================= */
+/* Subroutine */
+static int subsm_( int *n, int *m, int *nsub, int *ind,
+Real *l, Real *u, int *nbd,
+Real *x, Real *d__, Real *ws, Real *wy, Real *theta,
+int *col, int *head, int *iword,
+Real *wv, Real *wn, int *iprint, int *info)
+{
+ /* Format strings */
+/*
+ static char fmt_1001[] = "(/,\002----------------SUBSM entered----------\
+-------\002,/)";
+ static char fmt_1002[] = "(\002ALPHA = \002,f7.5,\002 backtrack to the B\
+OX\002)";
+ static char fmt_1003[] = "(\002Subspace solution X = \002,/,(4x,1p,6(1x\
+,d11.4)))";
+ static char fmt_1004[] = "(/,\002----------------exit SUBSM ------------\
+--------\002,/)";
+*/
+
+ /* System generated locals */
+ int ws_dim1, ws_offset, wy_dim1, wy_offset, wn_dim1, wn_offset, i__1,
+ i__2;
+
+ /* Local variables */
+ Real temp1, temp2;
+ int i__, j, k;
+ Real alpha;
+ int m2;
+ Real dk;
+ int js, jy, pointr, ibd, col2;
+
+ /* Fortran I/O blocks */
+/*
+ static cilist io___233 = { 0, 6, 0, fmt_1001, 0 };
+ static cilist io___247 = { 0, 6, 0, fmt_1002, 0 };
+ static cilist io___248 = { 0, 6, 0, 0, 0 };
+ static cilist io___249 = { 0, 6, 0, fmt_1003, 0 };
+ static cilist io___250 = { 0, 6, 0, fmt_1004, 0 };
+*/
+
+
+/* ************ */
+
+/* Subroutine subsm */
+
+/* Given xcp, l, u, r, an index set that specifies */
+/* the active set at xcp, and an l-BFGS matrix B */
+/* (in terms of WY, WS, SY, WT, head, col, and theta), */
+/* this subroutine computes an approximate solution */
+/* of the subspace problem */
+
+/* (P) min Q(x) = r'(x-xcp) + 1/2 (x-xcp)' B (x-xcp) */
+
+/* subject to l<=x<=u */
+/* x_i=xcp_i for all i in A(xcp) */
+
+/* along the subspace unconstrained Newton direction */
+
+/* d = -(Z'BZ)^(-1) r. */
+
+/* The formula for the Newton direction, given the L-BFGS matrix */
+/* and the Sherman-Morrison formula, is */
+
+/* d = (1/theta)r + (1/theta*2) Z'WK^(-1)W'Z r. */
+
+/* where */
+/* K = [-D -Y'ZZ'Y/theta L_a'-R_z' ] */
+/* [L_a -R_z theta*S'AA'S ] */
+
+/* Note that this procedure for computing d differs */
+/* from that described in [1]. One can show that the matrix K is */
+/* equal to the matrix M^[-1]N in that paper. */
+
+/* n is an int variable. */
+/* On entry n is the dimension of the problem. */
+/* On exit n is unchanged. */
+
+/* m is an int variable. */
+/* On entry m is the maximum number of variable metric corrections */
+/* used to define the limited memory matrix. */
+/* On exit m is unchanged. */
+
+/* nsub is an int variable. */
+/* On entry nsub is the number of free variables. */
+/* On exit nsub is unchanged. */
+
+/* ind is an int array of dimension nsub. */
+/* On entry ind specifies the coordinate indices of free variables. */
+/* On exit ind is unchanged. */
+
+/* l is a double precision array of dimension n. */
+/* On entry l is the lower bound of x. */
+/* On exit l is unchanged. */
+
+/* u is a double precision array of dimension n. */
+/* On entry u is the upper bound of x. */
+/* On exit u is unchanged. */
+
+/* nbd is a int array of dimension n. */
+/* On entry nbd represents the type of bounds imposed on the */
+/* variables, and must be specified as follows: */
+/* nbd(i)=0 if x(i) is unbounded, */
+/* 1 if x(i) has only a lower bound, */
+/* 2 if x(i) has both lower and upper bounds, and */
+/* 3 if x(i) has only an upper bound. */
+/* On exit nbd is unchanged. */
+
+/* x is a double precision array of dimension n. */
+/* On entry x specifies the Cauchy point xcp. */
+/* On exit x(i) is the minimizer of Q over the subspace of */
+/* free variables. */
+
+/* d is a double precision array of dimension n. */
+/* On entry d is the reduced gradient of Q at xcp. */
+/* On exit d is the Newton direction of Q. */
+
+/* ws and wy are double precision arrays; */
+/* theta is a double precision variable; */
+/* col is an int variable; */
+/* head is an int variable. */
+/* On entry they store the information defining the */
+/* limited memory BFGS matrix: */
+/* ws(n,m) stores S, a set of s-vectors; */
+/* wy(n,m) stores Y, a set of y-vectors; */
+/* theta is the scaling factor specifying B_0 = theta I; */
+/* col is the number of variable metric corrections stored; */
+/* head is the location of the 1st s- (or y-) vector in S (or Y). */
+/* On exit they are unchanged. */
+
+/* iword is an int variable. */
+/* On entry iword is unspecified. */
+/* On exit iword specifies the status of the subspace solution. */
+/* iword = 0 if the solution is in the box, */
+/* 1 if some bound is encountered. */
+
+/* wv is a double precision working array of dimension 2m. */
+
+/* wn is a double precision array of dimension 2m x 2m. */
+/* On entry the upper triangle of wn stores the LEL^T factorization */
+/* of the indefinite matrix */
+
+/* K = [-D -Y'ZZ'Y/theta L_a'-R_z' ] */
+/* [L_a -R_z theta*S'AA'S ] */
+/* where E = [-I 0] */
+/* [ 0 I] */
+/* On exit wn is unchanged. */
+
+/* iprint is an INTEGER variable that must be set by the user. */
+/* It controls the frequency and type of output generated: */
+/* iprint<0 no output is generated; */
+/* iprint=0 print only one line at the last iteration; */
+/* 0<iprint<99 print also f and |proj g| every iprint iterations; */
+/* iprint=99 print details of every iteration except n-vectors; */
+/* iprint=100 print also the changes of active set and final x; */
+/* iprint>100 print details of every iteration including x and g; */
+/* When iprint > 0, the file iterate.dat will be created to */
+/* summarize the iteration. */
+
+/* info is an int variable. */
+/* On entry info is unspecified. */
+/* On exit info = 0 for normal return, */
+/* = nonzero for abnormal return */
+/* when the matrix K is ill-conditioned. */
+
+/* Subprograms called: */
+
+/* Linpack dtrsl. */
+
+
+/* References: */
+
+/* [1] R. H. Byrd, P. Lu, J. Nocedal and C. Zhu, ``A limited */
+/* memory algorithm for bound constrained optimization'', */
+/* SIAM J. Scientific Computing 16 (1995), no. 5, pp. 1190--1208. */
+
+
+
+/* * * * */
+
+/* NEOS, November 1994. (Latest revision June 1996.) */
+/* Optimization Technology Center. */
+/* Argonne National Laboratory and Northwestern University. */
+/* Written by */
+/* Ciyou Zhu */
+/* in collaboration with R.H. Byrd, P. Lu-Chen and J. Nocedal. */
+
+
+/* ************ */
+ /* Parameter adjustments */
+ --d__;
+ --x;
+ --nbd;
+ --u;
+ --l;
+ wn_dim1 = 2 * *m;
+ wn_offset = 1 + wn_dim1 * 1;
+ wn -= wn_offset;
+ --wv;
+ wy_dim1 = *n;
+ wy_offset = 1 + wy_dim1 * 1;
+ wy -= wy_offset;
+ ws_dim1 = *n;
+ ws_offset = 1 + ws_dim1 * 1;
+ ws -= ws_offset;
+ --ind;
+
+ /* Function Body */
+ if (*nsub <= 0) {
+ return 0;
+ }
+ if (*iprint >= 99) {
+/*
+ s_wsfe(&io___233);
+ e_wsfe();
+*/
+ }
+/* Compute wv = W'Zd. */
+ pointr = *head;
+ i__1 = *col;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ temp1 = 0.;
+ temp2 = 0.;
+ i__2 = *nsub;
+ for (j = 1; j <= i__2; ++j) {
+ k = ind[j];
+ temp1 += wy[k + pointr * wy_dim1] * d__[j];
+ temp2 += ws[k + pointr * ws_dim1] * d__[j];
+/* L10: */
+ }
+ wv[i__] = temp1;
+ wv[*col + i__] = *theta * temp2;
+ pointr = pointr % *m + 1;
+/* L20: */
+ }
+/* Compute wv:=K^(-1)wv. */
+ m2 = *m << 1;
+ col2 = *col << 1;
+ dtrsl_(&wn[wn_offset], &m2, &col2, &wv[1], &c__11, info);
+ if (*info != 0) {
+ return 0;
+ }
+ i__1 = *col;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ wv[i__] = -wv[i__];
+/* L25: */
+ }
+ dtrsl_(&wn[wn_offset], &m2, &col2, &wv[1], &c__1, info);
+ if (*info != 0) {
+ return 0;
+ }
+/* Compute d = (1/theta)d + (1/theta**2)Z'W wv. */
+ pointr = *head;
+ i__1 = *col;
+ for (jy = 1; jy <= i__1; ++jy) {
+ js = *col + jy;
+ i__2 = *nsub;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ k = ind[i__];
+ d__[i__] = d__[i__] + wy[k + pointr * wy_dim1] * wv[jy] / *theta
+ + ws[k + pointr * ws_dim1] * wv[js];
+/* L30: */
+ }
+ pointr = pointr % *m + 1;
+/* L40: */
+ }
+ i__1 = *nsub;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ d__[i__] /= *theta;
+/* L50: */
+ }
+/* Backtrack to the feasible region. */
+ alpha = 1.;
+ temp1 = alpha;
+ i__1 = *nsub;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ k = ind[i__];
+ dk = d__[i__];
+ if (nbd[k] != 0) {
+ if (dk < 0. && nbd[k] <= 2) {
+ temp2 = l[k] - x[k];
+ if (temp2 >= 0.) {
+ temp1 = 0.;
+ } else if (dk * alpha < temp2) {
+ temp1 = temp2 / dk;
+ }
+ } else if (dk > 0. && nbd[k] >= 2) {
+ temp2 = u[k] - x[k];
+ if (temp2 <= 0.) {
+ temp1 = 0.;
+ } else if (dk * alpha > temp2) {
+ temp1 = temp2 / dk;
+ }
+ }
+ if (temp1 < alpha) {
+ alpha = temp1;
+ ibd = i__;
+ }
+ }
+/* L60: */
+ }
+ if (alpha < 1.) {
+ dk = d__[ibd];
+ k = ind[ibd];
+ if (dk > 0.) {
+ x[k] = u[k];
+ d__[ibd] = 0.;
+ } else if (dk < 0.) {
+ x[k] = l[k];
+ d__[ibd] = 0.;
+ }
+ }
+ i__1 = *nsub;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ k = ind[i__];
+ x[k] += alpha * d__[i__];
+/* L70: */
+ }
+ if (*iprint >= 99) {
+ if (alpha < 1.) {
+/*
+ s_wsfe(&io___247);
+ do_fio(&c__1, (char *)&alpha, (ftnlen)sizeof(Real));
+ e_wsfe();
+*/
+ } else {
+/*
+ s_wsle(&io___248);
+ do_lio(&c__9, &c__1, "SM solution inside the box", (ftnlen)26);
+ e_wsle();
+*/
+ }
+ if (*iprint > 100) {
+/*
+ s_wsfe(&io___249);
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ do_fio(&c__1, (char *)&x[i__], (ftnlen)sizeof(Real));
+ }
+ e_wsfe();
+*/
+ }
+ }
+ if (alpha < 1.) {
+ *iword = 1;
+ } else {
+ *iword = 0;
+ }
+ if (*iprint >= 99) {
+/*
+ s_wsfe(&io___250);
+ e_wsfe();
+*/
+ }
+ return 0;
+} /* subsm_ */
+
+/* ====================== The end of subsm =============================== */
+/* Subroutine */
+static int dcsrch_(
+Real *f, Real *g, Real *stp, const Real *ftol, const Real *gtol, const Real *xtol, const Real *stpmin, Real *stpmax,
+char *task, int *isave, Real *dsave, ftnlen task_len)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ int stage;
+ Real finit, ginit, width, ftest, gtest, stmin, stmax, width1,
+ fm, gm, fx, fy, gx, gy;
+ bool brackt;
+ Real fxm, fym, gxm, gym, stx, sty;
+
+/* ********** */
+
+/* Subroutine dcsrch */
+
+/* This subroutine finds a step that satisfies a sufficient */
+/* decrease condition and a curvature condition. */
+
+/* Each call of the subroutine updates an interval with */
+/* endpoints stx and sty. The interval is initially chosen */
+/* so that it contains a minimizer of the modified function */
+
+/* psi(stp) = f(stp) - f(0) - ftol*stp*f'(0). */
+
+/* If psi(stp) <= 0 and f'(stp) >= 0 for some step, then the */
+/* interval is chosen so that it contains a minimizer of f. */
+
+/* The algorithm is designed to find a step that satisfies */
+/* the sufficient decrease condition */
+
+/* f(stp) <= f(0) + ftol*stp*f'(0), */
+
+/* and the curvature condition */
+
+/* abs(f'(stp)) <= gtol*abs(f'(0)). */
+
+/* If ftol is less than gtol and if, for example, the function */
+/* is bounded below, then there is always a step which satisfies */
+/* both conditions. */
+
+/* If no step can be found that satisfies both conditions, then */
+/* the algorithm stops with a warning. In this case stp only */
+/* satisfies the sufficient decrease condition. */
+
+/* A typical invocation of dcsrch has the following outline: */
+
+/* task = 'START' */
+/* 10 continue */
+/* call dcsrch( ... ) */
+/* if (task .eq. 'FG') then */
+/* Evaluate the function and the gradient at stp */
+/* goto 10 */
+/* end if */
+
+/* NOTE: The user must no alter work arrays between calls. */
+
+/* The subroutine statement is */
+
+/* subroutine dcsrch(f,g,stp,ftol,gtol,xtol,stpmin,stpmax, */
+/* task,isave,dsave) */
+/* where */
+
+/* f is a double precision variable. */
+/* On initial entry f is the value of the function at 0. */
+/* On subsequent entries f is the value of the */
+/* function at stp. */
+/* On exit f is the value of the function at stp. */
+
+/* g is a double precision variable. */
+/* On initial entry g is the derivative of the function at 0. */
+/* On subsequent entries g is the derivative of the */
+/* function at stp. */
+/* On exit g is the derivative of the function at stp. */
+
+/* stp is a double precision variable. */
+/* On entry stp is the current estimate of a satisfactory */
+/* step. On initial entry, a positive initial estimate */
+/* must be provided. */
+/* On exit stp is the current estimate of a satisfactory step */
+/* if task = 'FG'. If task = 'CONV' then stp satisfies */
+/* the sufficient decrease and curvature condition. */
+
+/* ftol is a double precision variable. */
+/* On entry ftol specifies a nonnegative tolerance for the */
+/* sufficient decrease condition. */
+/* On exit ftol is unchanged. */
+
+/* gtol is a double precision variable. */
+/* On entry gtol specifies a nonnegative tolerance for the */
+/* curvature condition. */
+/* On exit gtol is unchanged. */
+
+/* xtol is a double precision variable. */
+/* On entry xtol specifies a nonnegative relative tolerance */
+/* for an acceptable step. The subroutine exits with a */
+/* warning if the relative difference between sty and stx */
+/* is less than xtol. */
+/* On exit xtol is unchanged. */
+
+/* stpmin is a double precision variable. */
+/* On entry stpmin is a nonnegative lower bound for the step. */
+/* On exit stpmin is unchanged. */
+
+/* stpmax is a double precision variable. */
+/* On entry stpmax is a nonnegative upper bound for the step. */
+/* On exit stpmax is unchanged. */
+
+/* task is a character variable of length at least 60. */
+/* On initial entry task must be set to 'START'. */
+/* On exit task indicates the required action: */
+
+/* If task(1:2) = 'FG' then evaluate the function and */
+/* derivative at stp and call dcsrch again. */
+
+/* If task(1:4) = 'CONV' then the search is successful. */
+
+/* If task(1:4) = 'WARN' then the subroutine is not able */
+/* to satisfy the convergence conditions. The exit value of */
+/* stp contains the best point found during the search. */
+
+/* If task(1:5) = 'ERROR' then there is an error in the */
+/* input arguments. */
+
+/* On exit with convergence, a warning or an error, the */
+/* variable task contains additional information. */
+
+/* isave is an int work array of dimension 2. */
+
+/* dsave is a double precision work array of dimension 13. */
+
+/* Subprograms called */
+
+/* MINPACK-2 ... dcstep */
+
+/* MINPACK-1 Project. June 1983. */
+/* Argonne National Laboratory. */
+/* Jorge J. More' and David J. Thuente. */
+
+/* MINPACK-2 Project. October 1993. */
+/* Argonne National Laboratory and University of Minnesota. */
+/* Brett M. Averick, Richard G. Carter, and Jorge J. More'. */
+
+/* ********** */
+/* Initialization block. */
+ /* Parameter adjustments */
+ --dsave;
+ --isave;
+
+ /* Function Body */
+ if (strncmp(task, "START", 5) == 0) {
+/* Check the input arguments for errors. */
+ if (*stp < *stpmin) {
+ strcpy(task, "ERROR: STP .LT. STPMIN");
+ }
+ if (*stp > *stpmax) {
+ strcpy(task, "ERROR: STP .GT. STPMAX");
+ }
+ if (*g >= 0.) {
+ strcpy(task, "ERROR: INITIAL G .GE. ZERO");
+ }
+ if (*ftol < 0.) {
+ strcpy(task, "ERROR: FTOL .LT. ZERO");
+ }
+ if (*gtol < 0.) {
+ strcpy(task, "ERROR: GTOL .LT. ZERO");
+ }
+ if (*xtol < 0.) {
+ strcpy(task, "ERROR: XTOL .LT. ZERO");
+ }
+ if (*stpmin < 0.) {
+ strcpy(task, "ERROR: STPMIN .LT. ZERO");
+ }
+ if (*stpmax < *stpmin) {
+ strcpy(task, "ERROR: STPMAX .LT. STPMIN");
+ }
+/* Exit if there are errors on input. */
+ if (strncmp(task, "ERROR", 5) == 0) {
+ return 0;
+ }
+/* Initialize local variables. */
+ brackt = FALSE_;
+ stage = 1;
+ finit = *f;
+ ginit = *g;
+ gtest = *ftol * ginit;
+ width = *stpmax - *stpmin;
+ width1 = 2*width;
+/* The variables stx, fx, gx contain the values of the step, */
+/* function, and derivative at the best step. */
+/* The variables sty, fy, gy contain the value of the step, */
+/* function, and derivative at sty. */
+/* The variables stp, f, g contain the values of the step, */
+/* function, and derivative at stp. */
+ stx = 0.;
+ fx = finit;
+ gx = ginit;
+ sty = 0.;
+ fy = finit;
+ gy = ginit;
+ stmin = 0.;
+ stmax = *stp + *stp * 4;
+ strcpy(task, "FG");
+ goto L1000;
+ } else {
+/* Restore local variables. */
+ if (isave[1] == 1) {
+ brackt = TRUE_;
+ } else {
+ brackt = FALSE_;
+ }
+ stage = isave[2];
+ ginit = dsave[1];
+ gtest = dsave[2];
+ gx = dsave[3];
+ gy = dsave[4];
+ finit = dsave[5];
+ fx = dsave[6];
+ fy = dsave[7];
+ stx = dsave[8];
+ sty = dsave[9];
+ stmin = dsave[10];
+ stmax = dsave[11];
+ width = dsave[12];
+ width1 = dsave[13];
+ }
+/* If psi(stp) <= 0 and f'(stp) >= 0 for some step, then the */
+/* algorithm enters the second stage. */
+ ftest = finit + *stp * gtest;
+ if (stage == 1 && *f <= ftest && *g >= 0.) {
+ stage = 2;
+ }
+/* Test for warnings. */
+ if (brackt && (*stp <= stmin || *stp >= stmax)) {
+ strcpy(task, "WARNING: ROUNDING ERRORS PREVENT PROGRESS" );
+ }
+ if (brackt && stmax - stmin <= *xtol * stmax) {
+ strcpy(task, "WARNING: XTOL TEST SATISFIED");
+ }
+ if (*stp == *stpmax && *f <= ftest && *g <= gtest) {
+ strcpy(task, "WARNING: STP = STPMAX");
+ }
+ if (*stp == *stpmin && (*f > ftest || *g >= gtest)) {
+ strcpy(task, "WARNING: STP = STPMIN");
+ }
+/* Test for convergence. */
+ if (*f <= ftest && fabs(*g) <= *gtol * (-ginit)) {
+ strcpy(task, "CONVERGENCE");
+ }
+/* Test for termination. */
+ if (strncmp(task, "WARN", 4) == 0 || strncmp(task, "CONV", 4) == 0) {
+ goto L1000;
+ }
+/* A modified function is used to predict the step during the */
+/* first stage if a lower function value has been obtained but */
+/* the decrease is not sufficient. */
+ if (stage == 1 && *f <= fx && *f > ftest) {
+/* Define the modified function and derivative values. */
+ fm = *f - *stp * gtest;
+ fxm = fx - stx * gtest;
+ fym = fy - sty * gtest;
+ gm = *g - gtest;
+ gxm = gx - gtest;
+ gym = gy - gtest;
+/* Call dcstep to update stx, sty, and to compute the new step. */
+ dcstep_(&stx, &fxm, &gxm, &sty, &fym, &gym, stp, &fm, &gm, &brackt, &
+ stmin, &stmax);
+/* Reset the function and derivative values for f. */
+ fx = fxm + stx * gtest;
+ fy = fym + sty * gtest;
+ gx = gxm + gtest;
+ gy = gym + gtest;
+ } else {
+/* Call dcstep to update stx, sty, and to compute the new step. */
+ dcstep_(&stx, &fx, &gx, &sty, &fy, &gy, stp, f, g, &brackt, &stmin, &
+ stmax);
+ }
+/* Decide if a bisection step is needed. */
+ if (brackt) {
+ if ((d__1 = sty - stx, fabs(d__1)) >= width1 * .66) {
+ *stp = stx + (sty - stx)/2;
+ }
+ width1 = width;
+ width = (d__1 = sty - stx, fabs(d__1));
+ }
+/* Set the minimum and maximum steps allowed for stp. */
+ if (brackt) {
+ stmin = std::min(stx,sty);
+ stmax = std::max(stx,sty);
+ } else {
+ stmin = *stp + (*stp - stx) * Real(1.1);
+ stmax = *stp + (*stp - stx) * 4;
+ }
+/* Force the step to be within the bounds stpmax and stpmin. */
+ *stp = std::max(*stp,*stpmin);
+ *stp = std::min(*stp,*stpmax);
+/* If further progress is not possible, let stp be the best */
+/* point obtained during the search. */
+ if ( ( brackt && (*stp <= stmin || *stp >= stmax) )
+ || ( brackt && (stmax - stmin <= *xtol * stmax) ) ) {
+ *stp = stx;
+ }
+/* Obtain another function and derivative. */
+ strcpy(task, "FG");
+L1000:
+/* Save local variables. */
+ if (brackt) {
+ isave[1] = 1;
+ } else {
+ isave[1] = 0;
+ }
+ isave[2] = stage;
+ dsave[1] = ginit;
+ dsave[2] = gtest;
+ dsave[3] = gx;
+ dsave[4] = gy;
+ dsave[5] = finit;
+ dsave[6] = fx;
+ dsave[7] = fy;
+ dsave[8] = stx;
+ dsave[9] = sty;
+ dsave[10] = stmin;
+ dsave[11] = stmax;
+ dsave[12] = width;
+ dsave[13] = width1;
+
+ return 0;
+
+} /* dcsrch_ */
+
+/* ====================== The end of dcsrch ============================== */
+/* Subroutine */
+static int dcstep_( Real *stx, Real *fx, Real *dx,
+ Real *sty, Real *fy, Real *dy, Real *stp, Real *fp, Real *dp,
+ bool *brackt, Real *stpmin, Real *stpmax)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ Real sgnd, stpc, stpf, stpq, p, q, gamma, r__, s, theta;
+
+/* ********** */
+
+/* Subroutine dcstep */
+
+/* This subroutine computes a safeguarded step for a search */
+/* procedure and updates an interval that contains a step that */
+/* satisfies a sufficient decrease and a curvature condition. */
+
+/* The parameter stx contains the step with the least function */
+/* value. If brackt is set to .true. then a minimizer has */
+/* been bracketed in an interval with endpoints stx and sty. */
+/* The parameter stp contains the current step. */
+/* The subroutine assumes that if brackt is set to .true. then */
+
+/* min(stx,sty) < stp < max(stx,sty), */
+
+/* and that the derivative at stx is negative in the direction */
+/* of the step. */
+
+/* The subroutine statement is */
+
+/* subroutine dcstep(stx,fx,dx,sty,fy,dy,stp,fp,dp,brackt, */
+/* stpmin,stpmax) */
+
+/* where */
+
+/* stx is a double precision variable. */
+/* On entry stx is the best step obtained so far and is an */
+/* endpoint of the interval that contains the minimizer. */
+/* On exit stx is the updated best step. */
+
+/* fx is a double precision variable. */
+/* On entry fx is the function at stx. */
+/* On exit fx is the function at stx. */
+
+/* dx is a double precision variable. */
+/* On entry dx is the derivative of the function at */
+/* stx. The derivative must be negative in the direction of */
+/* the step, that is, dx and stp - stx must have opposite */
+/* signs. */
+/* On exit dx is the derivative of the function at stx. */
+
+/* sty is a double precision variable. */
+/* On entry sty is the second endpoint of the interval that */
+/* contains the minimizer. */
+/* On exit sty is the updated endpoint of the interval that */
+/* contains the minimizer. */
+
+/* fy is a double precision variable. */
+/* On entry fy is the function at sty. */
+/* On exit fy is the function at sty. */
+
+/* dy is a double precision variable. */
+/* On entry dy is the derivative of the function at sty. */
+/* On exit dy is the derivative of the function at the exit sty. */
+
+/* stp is a double precision variable. */
+/* On entry stp is the current step. If brackt is set to .true. */
+/* then on input stp must be between stx and sty. */
+/* On exit stp is a new trial step. */
+
+/* fp is a double precision variable. */
+/* On entry fp is the function at stp */
+/* On exit fp is unchanged. */
+
+/* dp is a double precision variable. */
+/* On entry dp is the the derivative of the function at stp. */
+/* On exit dp is unchanged. */
+
+/* brackt is an bool variable. */
+/* On entry brackt specifies if a minimizer has been bracketed. */
+/* Initially brackt must be set to .false. */
+/* On exit brackt specifies if a minimizer has been bracketed. */
+/* When a minimizer is bracketed brackt is set to .true. */
+
+/* stpmin is a double precision variable. */
+/* On entry stpmin is a lower bound for the step. */
+/* On exit stpmin is unchanged. */
+
+/* stpmax is a double precision variable. */
+/* On entry stpmax is an upper bound for the step. */
+/* On exit stpmax is unchanged. */
+
+/* MINPACK-1 Project. June 1983 */
+/* Argonne National Laboratory. */
+/* Jorge J. More' and David J. Thuente. */
+
+/* MINPACK-2 Project. October 1993. */
+/* Argonne National Laboratory and University of Minnesota. */
+/* Brett M. Averick and Jorge J. More'. */
+
+/* ********** */
+ sgnd = *dp * (*dx / fabs(*dx));
+/* First case: A higher function value. The minimum is bracketed. */
+/* If the cubic step is closer to stx than the quadratic step, the */
+/* cubic step is taken, otherwise the average of the cubic and */
+/* quadratic steps is taken. */
+ if (*fp > *fx) {
+ theta = (*fx - *fp) * Real(3) / (*stp - *stx) + *dx + *dp;
+/* Computing MAX */
+ d__1 = fabs(theta), d__2 = fabs(*dx), d__1 = std::max(d__1,d__2), d__2 = fabs(
+ *dp);
+ s = std::max(d__1,d__2);
+/* Computing 2nd power */
+ d__1 = theta / s;
+ gamma = s * std::sqrt(d__1 * d__1 - *dx / s * (*dp / s));
+ if (*stp < *stx) {
+ gamma = -gamma;
+ }
+ p = gamma - *dx + theta;
+ q = gamma - *dx + gamma + *dp;
+ r__ = p / q;
+ stpc = *stx + r__ * (*stp - *stx);
+ stpq = *stx + *dx / ((*fx - *fp) / (*stp - *stx) + *dx) / Real(2) * (*stp
+ - *stx);
+ if ((d__1 = stpc - *stx, fabs(d__1)) < (d__2 = stpq - *stx, fabs(d__2)))
+ {
+ stpf = stpc;
+ } else {
+ stpf = stpc + (stpq - stpc) / 2;
+ }
+ *brackt = TRUE_;
+/* Second case: A lower function value and derivatives of opposite */
+/* sign. The minimum is bracketed. If the cubic step is farther from */
+/* stp than the secant step, the cubic step is taken, otherwise the */
+/* secant step is taken. */
+ } else if (sgnd < 0.) {
+ theta = (*fx - *fp) * Real(3) / (*stp - *stx) + *dx + *dp;
+/* Computing MAX */
+ d__1 = fabs(theta), d__2 = fabs(*dx), d__1 = std::max(d__1,d__2), d__2 = fabs(
+ *dp);
+ s = std::max(d__1,d__2);
+/* Computing 2nd power */
+ d__1 = theta / s;
+ gamma = s * std::sqrt(d__1 * d__1 - *dx / s * (*dp / s));
+ if (*stp > *stx) {
+ gamma = -gamma;
+ }
+ p = gamma - *dp + theta;
+ q = gamma - *dp + gamma + *dx;
+ r__ = p / q;
+ stpc = *stp + r__ * (*stx - *stp);
+ stpq = *stp + *dp / (*dp - *dx) * (*stx - *stp);
+ if ((d__1 = stpc - *stp, fabs(d__1)) > (d__2 = stpq - *stp, fabs(d__2)))
+ {
+ stpf = stpc;
+ } else {
+ stpf = stpq;
+ }
+ *brackt = TRUE_;
+/* Third case: A lower function value, derivatives of the same sign, */
+/* and the magnitude of the derivative decreases. */
+ } else if (fabs(*dp) < fabs(*dx)) {
+/* The cubic step is computed only if the cubic tends to infinity */
+/* in the direction of the step or if the minimum of the cubic */
+/* is beyond stp. Otherwise the cubic step is defined to be the */
+/* secant step. */
+ theta = (*fx - *fp) * Real(3) / (*stp - *stx) + *dx + *dp;
+/* Computing MAX */
+ d__1 = fabs(theta), d__2 = fabs(*dx), d__1 = std::max(d__1,d__2), d__2 = fabs(
+ *dp);
+ s = std::max(d__1,d__2);
+/* The case gamma = 0 only arises if the cubic does not tend */
+/* to infinity in the direction of the step. */
+/* Computing MAX */
+/* Computing 2nd power */
+ d__3 = theta / s;
+ d__1 = 0., d__2 = d__3 * d__3 - *dx / s * (*dp / s);
+ gamma = s * std::sqrt((std::max(d__1,d__2)));
+ if (*stp > *stx) {
+ gamma = -gamma;
+ }
+ p = gamma - *dp + theta;
+ q = gamma + (*dx - *dp) + gamma;
+ r__ = p / q;
+ if (r__ < 0. && gamma != 0.) {
+ stpc = *stp + r__ * (*stx - *stp);
+ } else if (*stp > *stx) {
+ stpc = *stpmax;
+ } else {
+ stpc = *stpmin;
+ }
+ stpq = *stp + *dp / (*dp - *dx) * (*stx - *stp);
+ if (*brackt) {
+/* A minimizer has been bracketed. If the cubic step is */
+/* closer to stp than the secant step, the cubic step is */
+/* taken, otherwise the secant step is taken. */
+ if ((d__1 = stpc - *stp, fabs(d__1)) < (d__2 = stpq - *stp, fabs(
+ d__2))) {
+ stpf = stpc;
+ } else {
+ stpf = stpq;
+ }
+ if (*stp > *stx) {
+/* Computing MIN */
+ d__1 = *stp + (*sty - *stp) * Real(.66);
+ stpf = std::min(d__1,stpf);
+ } else {
+/* Computing MAX */
+ d__1 = *stp + (*sty - *stp) * Real(.66);
+ stpf = std::max(d__1,stpf);
+ }
+ } else {
+/* A minimizer has not been bracketed. If the cubic step is */
+/* farther from stp than the secant step, the cubic step is */
+/* taken, otherwise the secant step is taken. */
+ if ((d__1 = stpc - *stp, fabs(d__1)) > (d__2 = stpq - *stp, fabs(
+ d__2))) {
+ stpf = stpc;
+ } else {
+ stpf = stpq;
+ }
+ stpf = std::min(*stpmax,stpf);
+ stpf = std::max(*stpmin,stpf);
+ }
+/* Fourth case: A lower function value, derivatives of the same sign, */
+/* and the magnitude of the derivative does not decrease. If the */
+/* minimum is not bracketed, the step is either stpmin or stpmax, */
+/* otherwise the cubic step is taken. */
+ } else {
+ if (*brackt) {
+ theta = (*fp - *fy) * Real(3) / (*sty - *stp) + *dy + *dp;
+/* Computing MAX */
+ d__1 = fabs(theta), d__2 = fabs(*dy), d__1 = std::max(d__1,d__2), d__2 =
+ fabs(*dp);
+ s = std::max(d__1,d__2);
+/* Computing 2nd power */
+ d__1 = theta / s;
+ gamma = s * std::sqrt(d__1 * d__1 - *dy / s * (*dp / s));
+ if (*stp > *sty) {
+ gamma = -gamma;
+ }
+ p = gamma - *dp + theta;
+ q = gamma - *dp + gamma + *dy;
+ r__ = p / q;
+ stpc = *stp + r__ * (*sty - *stp);
+ stpf = stpc;
+ } else if (*stp > *stx) {
+ stpf = *stpmax;
+ } else {
+ stpf = *stpmin;
+ }
+ }
+/* Update the interval which contains a minimizer. */
+ if (*fp > *fx) {
+ *sty = *stp;
+ *fy = *fp;
+ *dy = *dp;
+ } else {
+ if (sgnd < 0.) {
+ *sty = *stx;
+ *fy = *fx;
+ *dy = *dx;
+ }
+ *stx = *stp;
+ *fx = *fp;
+ *dx = *dp;
+ }
+/* Compute the new step. */
+ *stp = stpf;
+
+ return 0;
+
+} /* dcstep_ */
+
+/* ====================== The end of dcstep ============================== */
+
+/* ====================== The end of dnrm2 =============================== */
+static Real dpmeps_(void)
+{
+ /* Initialized data */
+
+ Real zero = 0.;
+ Real one = 1.;
+ Real two = 2.;
+
+ /* System generated locals */
+ int i__1;
+ Real ret_val;
+
+ /* Local variables */
+ Real beta;
+ int irnd;
+ Real temp, temp1, a, b;
+ int i__;
+ Real betah;
+ int ibeta, negep;
+ Real tempa;
+ int itemp, it;
+ Real betain;
+
+/* ********** */
+
+/* Subroutine dpeps */
+
+/* This subroutine computes the machine precision parameter */
+/* dpmeps as the smallest floating point number such that */
+/* 1 + dpmeps differs from 1. */
+
+/* This subroutine is based on the subroutine machar described in */
+
+/* W. J. Cody, */
+/* MACHAR: A subroutine to dynamically determine machine parameters, */
+/* ACM Transactions on Mathematical Software, 14, 1988, pages 303-311. */
+
+/* The subroutine statement is: */
+
+/* subroutine dpeps(dpmeps) */
+
+/* where */
+
+/* dpmeps is a double precision variable. */
+/* On entry dpmeps need not be specified. */
+/* On exit dpmeps is the machine precision. */
+
+/* MINPACK-2 Project. February 1991. */
+/* Argonne National Laboratory and University of Minnesota. */
+/* Brett M. Averick. */
+
+/* ******* */
+/* determine ibeta, beta ala malcolm. */
+ a = one;
+ b = one;
+L10:
+ a += a;
+ temp = a + one;
+ temp1 = temp - a;
+ if (temp1 - one == zero) {
+ goto L10;
+ }
+L20:
+ b += b;
+ temp = a + b;
+ itemp = (int) (temp - a);
+ if (itemp == 0) {
+ goto L20;
+ }
+ ibeta = itemp;
+ beta = (Real) ibeta;
+/* determine it, irnd. */
+ it = 0;
+ b = one;
+L30:
+ ++it;
+ b *= beta;
+ temp = b + one;
+ temp1 = temp - b;
+ if (temp1 - one == zero) {
+ goto L30;
+ }
+ irnd = 0;
+ betah = beta / two;
+ temp = a + betah;
+ if (temp - a != zero) {
+ irnd = 1;
+ }
+ tempa = a + beta;
+ temp = tempa + betah;
+ if (irnd == 0 && temp - tempa != zero) {
+ irnd = 2;
+ }
+/* determine dpmeps. */
+ negep = it + 3;
+ betain = one / beta;
+ a = one;
+ i__1 = negep;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ a *= betain;
+/* L40: */
+ }
+L50:
+ temp = one + a;
+ if (temp - one != zero) {
+ goto L60;
+ }
+ a *= beta;
+ goto L50;
+L60:
+ ret_val = a;
+ if (ibeta == 2 || irnd == 0) {
+ goto L70;
+ }
+ a = a * (one + a) / two;
+ temp = one + a;
+ if (temp - one != zero) {
+ ret_val = a;
+ }
+L70:
+ return ret_val;
+} /* dpmeps_ */
+
+/* ====================== The end of dpmeps ============================== */
+
+/* Subroutine */
+static int dtrsl_( Real *t, int *ldt, int *n,
+Real *b, const int *job, int *info)
+{
+ /* System generated locals */
+ int t_dim1, t_offset, i__1, i__2;
+
+ /* Local variables */
+ int case__;
+ Real temp;
+ int j;
+ int jj;
+
+
+
+/* dtrsl solves systems of the form */
+
+/* t * x = b */
+/* or */
+/* trans(t) * x = b */
+
+/* where t is a triangular matrix of order n. here trans(t) */
+/* denotes the transpose of the matrix t. */
+
+/* on entry */
+
+/* t double precision(ldt,n) */
+/* t contains the matrix of the system. the zero */
+/* elements of the matrix are not referenced, and */
+/* the corresponding elements of the array can be */
+/* used to store other information. */
+
+/* ldt int */
+/* ldt is the leading dimension of the array t. */
+
+/* n int */
+/* n is the order of the system. */
+
+/* b double precision(n). */
+/* b contains the right hand side of the system. */
+
+/* job int */
+/* job specifies what kind of system is to be solved. */
+/* if job is */
+
+/* 00 solve t*x=b, t lower triangular, */
+/* 01 solve t*x=b, t upper triangular, */
+/* 10 solve trans(t)*x=b, t lower triangular, */
+/* 11 solve trans(t)*x=b, t upper triangular. */
+
+/* on return */
+
+/* b b contains the solution, if info .eq. 0. */
+/* otherwise b is unaltered. */
+
+/* info int */
+/* info contains zero if the system is nonsingular. */
+/* otherwise info contains the index of */
+/* the first zero diagonal element of t. */
+
+/* linpack. this version dated 08/14/78 . */
+/* g. w. stewart, university of maryland, argonne national lab. */
+
+/* subroutines and functions */
+
+/* blas daxpy,ddot */
+/* fortran mod */
+
+/* internal variables */
+
+
+/* begin block permitting ...exits to 150 */
+
+/* check for zero diagonal elements. */
+
+ /* Parameter adjustments */
+ t_dim1 = *ldt;
+ t_offset = 1 + t_dim1 * 1;
+ t -= t_offset;
+ --b;
+
+ /* Function Body */
+ i__1 = *n;
+ for (*info = 1; *info <= i__1; ++(*info)) {
+/* ......exit */
+ if (t[*info + *info * t_dim1] == 0.) {
+ goto L150;
+ }
+/* L10: */
+ }
+ *info = 0;
+
+/* determine the task and go to it. */
+
+ case__ = 1;
+ if (*job % 10 != 0) {
+ case__ = 2;
+ }
+ if (*job % 100 / 10 != 0) {
+ case__ += 2;
+ }
+ switch ((int)case__) {
+ case 1: goto L20;
+ case 2: goto L50;
+ case 3: goto L80;
+ case 4: goto L110;
+ }
+
+/* solve t*x=b for t lower triangular */
+
+L20:
+ b[1] /= t[t_dim1 + 1];
+ if (*n < 2) {
+ goto L40;
+ }
+ i__1 = *n;
+ for (j = 2; j <= i__1; ++j) {
+ temp = -b[j - 1];
+ i__2 = *n - j + 1;
+ DAXPY((const int&)i__2, (const Real &)temp, &t[j + (j - 1) * t_dim1], (const int&)c__1, &b[j], (const int&)c__1);
+ b[j] /= t[j + j * t_dim1];
+/* L30: */
+ }
+L40:
+ goto L140;
+
+/* solve t*x=b for t upper triangular. */
+
+L50:
+ b[*n] /= t[*n + *n * t_dim1];
+ if (*n < 2) {
+ goto L70;
+ }
+ i__1 = *n;
+ for (jj = 2; jj <= i__1; ++jj) {
+ j = *n - jj + 1;
+ temp = -b[j + 1];
+ DAXPY((const int&)j, (const Real &)temp, &t[(j + 1) * t_dim1 + 1], (const int&)c__1, &b[1], (const int&)c__1);
+ b[j] /= t[j + j * t_dim1];
+/* L60: */
+ }
+L70:
+ goto L140;
+
+/* solve trans(t)*x=b for t lower triangular. */
+
+L80:
+ b[*n] /= t[*n + *n * t_dim1];
+ if (*n < 2) {
+ goto L100;
+ }
+ i__1 = *n;
+ for (jj = 2; jj <= i__1; ++jj) {
+ j = *n - jj + 1;
+ i__2 = jj - 1;
+ b[j] -= DDOT((const int&)i__2, &t[j + 1 + j * t_dim1], (const int&)c__1, &b[j + 1], (const int&)c__1);
+ b[j] /= t[j + j * t_dim1];
+/* L90: */
+ }
+L100:
+ goto L140;
+
+/* solve trans(t)*x=b for t upper triangular. */
+
+L110:
+ b[1] /= t[t_dim1 + 1];
+ if (*n < 2) {
+ goto L130;
+ }
+ i__1 = *n;
+ for (j = 2; j <= i__1; ++j) {
+ i__2 = j - 1;
+ b[j] -= DDOT((const int&)i__2, &t[j * t_dim1 + 1], (const int&)c__1, &b[1], (const int&)c__1);
+ b[j] /= t[j + j * t_dim1];
+/* L120: */
+ }
+L130:
+L140:
+L150:
+ return 0;
+} /* dtrsl_ */
+
+/* Subroutine */
+static int mainlb_ (int *n, int *m, Real *x, Real *l,
+ Real *u, int *nbd, Real *f, Real *g,
+ Real *factr, Real *pgtol, Real *ws, Real *wy,
+ Real *sy, Real *ss, Real *yy, Real *wt,
+ Real *wn, Real *snd, Real *z__, Real *r__,
+ Real *d__, Real *t, Real *wa, Real *sg,
+ Real *sgo, Real *yg, Real *ygo, int *index,
+ int *iwhere, int *indx2, char *task, int *iprint, char *csave,
+ bool *lsave, int *isave, Real *dsave, ftnlen task_len,
+ ftnlen csave_len)
+{
+ /* Format strings */
+/*
+ static char fmt_1002[] = "(/,\002At iterate\002,i5,4x,\002f= \002,1p,d12\
+.5,4x,\002|proj g|= \002,1p,d12.5)";
+ static char fmt_1003[] = "(2(1x,i4),5x,\002-\002,5x,\002-\002,3x,\002\
+-\002,5x,\002-\002,5x,\002-\002,8x,\002-\002,3x,1p,2(1x,d10.3))";
+ static char fmt_1001[] = "(//,\002ITERATION \002,i5)";
+ static char fmt_1005[] = "(/,\002 Singular triangular system detected\
+;\002,/,\002 refresh the lbfgs memory and restart the iteration.\002)";
+ static char fmt_1006[] = "(/,\002 Nonpositive definiteness in Cholesky f\
+actorization in formk;\002,/,\002 refresh the lbfgs memory and restart the\
+ iteration.\002)";
+ static char fmt_1008[] = "(/,\002 Bad direction in the line search;\002,\
+/,\002 refresh the lbfgs memory and restart the iteration.\002)";
+ static char fmt_1004[] = "(\002 ys=\002,1p,e10.3,\002 -gs=\002,1p,e10.\
+3,\002 BFGS update SKIPPED\002)";
+ static char fmt_1007[] = "(/,\002 Nonpositive definiteness in Cholesky f\
+actorization in formt;\002,/,\002 refresh the lbfgs memory and restart the\
+ iteration.\002)";
+*/
+
+ /* System generated locals */
+ int ws_dim1, ws_offset, wy_dim1, wy_offset, sy_dim1, sy_offset,
+ ss_dim1, ss_offset, yy_dim1, yy_offset, wt_dim1, wt_offset,
+ wn_dim1, wn_offset, snd_dim1, snd_offset, i__1;
+ Real d__1, d__2;
+/* olist o__1; */
+
+ /* 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;
+
+ /* Fortran I/O blocks */
+/*
+ static cilist io___62 = { 0, 6, 0, fmt_1002, 0 };
+ static cilist io___63 = { 0, 0, 0, fmt_1003, 0 };
+ static cilist io___64 = { 0, 6, 0, fmt_1001, 0 };
+ static cilist io___66 = { 0, 6, 0, fmt_1005, 0 };
+ static cilist io___68 = { 0, 6, 0, fmt_1006, 0 };
+ static cilist io___69 = { 0, 6, 0, fmt_1005, 0 };
+ static cilist io___71 = { 0, 6, 0, fmt_1008, 0 };
+ static cilist io___75 = { 0, 6, 0, fmt_1004, 0 };
+ static cilist io___76 = { 0, 6, 0, fmt_1007, 0 };
+*/
+ (void)task_len;
+ (void)csave_len;
+
+/* ************ */
+
+/* Subroutine mainlb */
+
+/* This subroutine solves bound constrained optimization problems by */
+/* using the compact formula of the limited memory BFGS updates. */
+
+/* n is an int variable. */
+/* On entry n is the number of variables. */
+/* On exit n is unchanged. */
+
+/* m is an int variable. */
+/* On entry m is the maximum number of variable metric */
+/* corrections allowed in the limited memory matrix. */
+/* On exit m is unchanged. */
+
+/* x is a double precision array of dimension n. */
+/* On entry x is an approximation to the solution. */
+/* On exit x is the current approximation. */
+
+/* l is a double precision array of dimension n. */
+/* On entry l is the lower bound of x. */
+/* On exit l is unchanged. */
+
+/* u is a double precision array of dimension n. */
+/* On entry u is the upper bound of x. */
+/* On exit u is unchanged. */
+
+/* nbd is an int array of dimension n. */
+/* On entry nbd represents the type of bounds imposed on the */
+/* variables, and must be specified as follows: */
+/* nbd(i)=0 if x(i) is unbounded, */
+/* 1 if x(i) has only a lower bound, */
+/* 2 if x(i) has both lower and upper bounds, */
+/* 3 if x(i) has only an upper bound. */
+/* On exit nbd is unchanged. */
+
+/* f is a double precision variable. */
+/* On first entry f is unspecified. */
+/* On final exit f is the value of the function at x. */
+
+/* g is a double precision array of dimension n. */
+/* On first entry g is unspecified. */
+/* On final exit g is the value of the gradient at x. */
+
+/* factr is a double precision variable. */
+/* On entry factr >= 0 is specified by the user. The iteration */
+/* will stop when */
+
+/* (f^k - f^{k+1})/max{|f^k|,|f^{k+1}|,1} <= factr*epsmch */
+
+/* where epsmch is the machine precision, which is automatically */
+/* generated by the code. */
+/* On exit factr is unchanged. */
+
+/* pgtol is a double precision variable. */
+/* On entry pgtol >= 0 is specified by the user. The iteration */
+/* will stop when */
+
+/* max{|proj g_i | i = 1, ..., n} <= pgtol */
+
+/* where pg_i is the ith component of the projected gradient. */
+/* On exit pgtol is unchanged. */
+
+/* ws, wy, sy, and wt are double precision working arrays used to */
+/* store the following information defining the limited memory */
+/* BFGS matrix: */
+/* ws, of dimension n x m, stores S, the matrix of s-vectors; */
+/* wy, of dimension n x m, stores Y, the matrix of y-vectors; */
+/* sy, of dimension m x m, stores S'Y; */
+/* ss, of dimension m x m, stores S'S; */
+/* yy, of dimension m x m, stores Y'Y; */
+/* wt, of dimension m x m, stores the Cholesky factorization */
+/* of (theta*S'S+LD^(-1)L'); see eq. */
+/* (2.26) in [3]. */
+
+/* wn is a double precision working array of dimension 2m x 2m */
+/* used to store the LEL^T factorization of the indefinite matrix */
+/* K = [-D -Y'ZZ'Y/theta L_a'-R_z' ] */
+/* [L_a -R_z theta*S'AA'S ] */
+
+/* where E = [-I 0] */
+/* [ 0 I] */
+
+/* snd is a double precision working array of dimension 2m x 2m */
+/* used to store the lower triangular part of */
+/* N = [Y' ZZ'Y L_a'+R_z'] */
+/* [L_a +R_z S'AA'S ] */
+
+/* z(n),r(n),d(n),t(n),wa(8*m) are double precision working arrays. */
+/* z is used at different times to store the Cauchy point and */
+/* the Newton point. */
+
+/* sg(m),sgo(m),yg(m),ygo(m) are double precision working arrays. */
+
+/* index is an int working array of dimension n. */
+/* In subroutine freev, index is used to store the free and fixed */
+/* variables at the Generalized Cauchy Point (GCP). */
+
+/* iwhere is an int working array of dimension n used to record */
+/* the status of the vector x for GCP computation. */
+/* iwhere(i)=0 or -3 if x(i) is free and has bounds, */
+/* 1 if x(i) is fixed at l(i), and l(i) .ne. u(i) */
+/* 2 if x(i) is fixed at u(i), and u(i) .ne. l(i) */
+/* 3 if x(i) is always fixed, i.e., u(i)=x(i)=l(i) */
+/* -1 if x(i) is always free, i.e., no bounds on it. */
+
+/* indx2 is an int working array of dimension n. */
+/* Within subroutine cauchy, indx2 corresponds to the array iorder. */
+/* In subroutine freev, a list of variables entering and leaving */
+/* the free set is stored in indx2, and it is passed on to */
+/* subroutine formk with this information. */
+
+/* task is a working string of characters of length 60 indicating */
+/* the current job when entering and leaving this subroutine. */
+
+/* iprint is an INTEGER variable that must be set by the user. */
+/* It controls the frequency and type of output generated: */
+/* iprint<0 no output is generated; */
+/* iprint=0 print only one line at the last iteration; */
+/* 0<iprint<99 print also f and |proj g| every iprint iterations; */
+/* iprint=99 print details of every iteration except n-vectors; */
+/* iprint=100 print also the changes of active set and final x; */
+/* iprint>100 print details of every iteration including x and g; */
+/* When iprint > 0, the file iterate.dat will be created to */
+/* summarize the iteration. */
+
+/* csave is a working string of characters of length 60. */
+
+/* lsave is a bool working array of dimension 4. */
+
+/* isave is an int working array of dimension 23. */
+
+/* dsave is a double precision working array of dimension 29. */
+
+
+/* Subprograms called */
+
+/* L-BFGS-B Library ... cauchy, subsm, lnsrlb, formk, */
+
+/* errclb, prn1lb, prn2lb, prn3lb, active, projgr, */
+
+/* freev, cmprlb, matupd, formt. */
+
+/* Minpack2 Library ... timer, dpmeps. */
+
+/* Linpack Library ... dcopy, ddot. */
+
+
+/* References: */
+
+/* [1] R. H. Byrd, P. Lu, J. Nocedal and C. Zhu, ``A limited */
+/* memory algorithm for bound constrained optimization'', */
+/* SIAM J. Scientific Computing 16 (1995), no. 5, pp. 1190--1208. */
+
+/* [2] C. Zhu, R.H. Byrd, P. Lu, J. Nocedal, ``L-BFGS-B: FORTRAN */
+/* Subroutines for Large Scale Bound Constrained Optimization'' */
+/* Tech. Report, NAM-11, EECS Department, Northwestern University, */
+/* 1994. */
+
+/* [3] R. Byrd, J. Nocedal and R. Schnabel "Representations of */
+/* Quasi-Newton Matrices and their use in Limited Memory Methods'', */
+/* Mathematical Programming 63 (1994), no. 4, pp. 129-156. */
+
+/* (Postscript files of these papers are available via anonymous */
+/* ftp to eecs.nwu.edu in the directory pub/lbfgs/lbfgs_bcm.) */
+
+/* * * * */
+
+/* NEOS, November 1994. (Latest revision June 1996.) */
+/* Optimization Technology Center. */
+/* Argonne National Laboratory and Northwestern University. */
+/* Written by */
+/* Ciyou Zhu */
+/* in collaboration with R.H. Byrd, P. Lu-Chen and J. Nocedal. */
+
+
+/* ************ */
+ /* Parameter adjustments */
+ --indx2;
+ --iwhere;
+ --index;
+ --t;
+ --d__;
+ --r__;
+ --z__;
+ --g;
+ --nbd;
+ --u;
+ --l;
+ --x;
+ --ygo;
+ --yg;
+ --sgo;
+ --sg;
+ --wa;
+ snd_dim1 = 2 * *m;
+ snd_offset = 1 + snd_dim1 * 1;
+ snd -= snd_offset;
+ wn_dim1 = 2 * *m;
+ wn_offset = 1 + wn_dim1 * 1;
+ wn -= wn_offset;
+ wt_dim1 = *m;
+ wt_offset = 1 + wt_dim1 * 1;
+ wt -= wt_offset;
+ yy_dim1 = *m;
+ yy_offset = 1 + yy_dim1 * 1;
+ yy -= yy_offset;
+ ss_dim1 = *m;
+ ss_offset = 1 + ss_dim1 * 1;
+ ss -= ss_offset;
+ sy_dim1 = *m;
+ sy_offset = 1 + sy_dim1 * 1;
+ sy -= sy_offset;
+ wy_dim1 = *n;
+ wy_offset = 1 + wy_dim1 * 1;
+ wy -= wy_offset;
+ ws_dim1 = *n;
+ ws_offset = 1 + ws_dim1 * 1;
+ ws -= ws_offset;
+ --lsave;
+ --isave;
+ --dsave;
+
+ /* Function Body */
+ if (strncmp(task, "START", 5) == 0) {
+/* Generate the current machine precision. */
+ epsmch = dpmeps_();
+/* Initialize counters and scalars when task='START'. */
+/* for the limited memory BFGS matrices: */
+ col = 0;
+ head = 1;
+ theta = 1.;
+ iupdat = 0;
+ updatd = FALSE_;
+/* for operation counts: */
+ iter = 0;
+ nfgv = 0;
+ nint = 0;
+ nintol = 0;
+ nskip = 0;
+ nfree = *n;
+/* for stopping tolerance: */
+ tol = *factr * epsmch;
+/* for measuring running time: */
+ cachyt = 0.;
+ sbtime = 0.;
+ lnscht = 0.;
+/* 'word' records the status of subspace solutions. */
+ strcpy(word, "---");
+/* 'info' records the termination information. */
+ info = 0;
+ if (*iprint >= 1) {
+/* open a summary file 'iterate.dat' */
+/*
+ o__1.oerr = 0;
+ o__1.ounit = 8;
+ o__1.ofnmlen = 11;
+ o__1.ofnm = "iterate.dat";
+ o__1.orl = 0;
+ o__1.osta = "unknown";
+ o__1.oacc = 0;
+ o__1.ofm = 0;
+ o__1.oblnk = 0;
+ f_open(&o__1);
+ itfile = 8;
+*/
+ }
+/* Check the input arguments for errors. */
+ errclb_(n, m, factr, &l[1], &u[1], &nbd[1], task, &info, &k, (ftnlen)
+ 60);
+ if (strncmp(task, "ERROR", 5 ) == 0) {
+ prn3lb_(n, &x[1], f, task, iprint, &info, &itfile, &iter, &nfgv, &
+ nintol, &nskip, &nact, &sbgnrm, &c_b9, &nint, word, &
+ iback, &stp, &xstep, &k, &cachyt, &sbtime, &lnscht, (
+ ftnlen)60, (ftnlen)3);
+ return 0;
+ }
+ prn1lb_(n, m, &l[1], &u[1], &x[1], iprint, &itfile, &epsmch);
+/* Initialize iwhere & project x onto the feasible set. */
+ active_(n, &l[1], &u[1], &nbd[1], &x[1], &iwhere[1], iprint, &prjctd,
+ &cnstnd, &boxed);
+/* The end of the initialization. */
+ } else {
+/* restore local variables. */
+ prjctd = lsave[1];
+ cnstnd = lsave[2];
+ boxed = lsave[3];
+ updatd = lsave[4];
+ nintol = isave[1];
+ itfile = isave[3];
+ iback = isave[4];
+ nskip = isave[5];
+ head = isave[6];
+ col = isave[7];
+ itail = isave[8];
+ iter = isave[9];
+ iupdat = isave[10];
+ nint = isave[12];
+ nfgv = isave[13];
+ info = isave[14];
+ ifun = isave[15];
+ iword = isave[16];
+ nfree = isave[17];
+ nact = isave[18];
+ ileave = isave[19];
+ nenter = isave[20];
+ theta = dsave[1];
+ fold = dsave[2];
+ tol = dsave[3];
+ dnorm = dsave[4];
+ epsmch = dsave[5];
+ cpu1 = dsave[6];
+ cachyt = dsave[7];
+ sbtime = dsave[8];
+ lnscht = dsave[9];
+ time1 = dsave[10];
+ gd = dsave[11];
+ stpmx = dsave[12];
+ sbgnrm = dsave[13];
+ stp = dsave[14];
+ gdold = dsave[15];
+ dtd = dsave[16];
+/* After returning from the driver go to the point where execution */
+/* is to resume. */
+ if (strncmp(task, "FG_LN", 5) == 0) {
+ goto L666;
+ }
+ if (strncmp(task, "NEW_X", 5) == 0) {
+ goto L777;
+ }
+ if (strncmp(task, "FG_ST", 5) == 0) {
+ goto L111;
+ }
+ if (strncmp(task, "STOP", 4) == 0) {
+ if (strncmp(task + 6, "CPU", 3) == 0) {
+/* restore the previous iterate. */
+ DCOPY(*n, &t[1], (const int&)c__1, &x[1], (const int&)c__1);
+ DCOPY(*n, &r__[1], (const int&)c__1, &g[1], (const int&)c__1);
+ *f = fold;
+ }
+ goto L999;
+ }
+ }
+/* Compute f0 and g0. */
+ strcpy(task, "FG_START");
+/* return to the driver to calculate f and g; reenter at 111. */
+ goto L1000;
+L111:
+ nfgv = 1;
+/* Compute the infinity norm of the (-) projected gradient. */
+ projgr_(n, &l[1], &u[1], &nbd[1], &x[1], *f, &g[1], &sbgnrm);
+ if (*iprint >= 1) {
+/*
+ s_wsfe(&io___62);
+ do_fio(&c__1, (char *)&iter, (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&(*f), (ftnlen)sizeof(Real));
+ do_fio(&c__1, (char *)&sbgnrm, (ftnlen)sizeof(Real));
+ e_wsfe();
+ io___63.ciunit = itfile;
+ s_wsfe(&io___63);
+ do_fio(&c__1, (char *)&iter, (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&nfgv, (ftnlen)sizeof(int));
+ do_fio(&c__1, (char *)&sbgnrm, (ftnlen)sizeof(Real));
+ do_fio(&c__1, (char *)&(*f), (ftnlen)sizeof(Real));
+ e_wsfe();
+*/
+ }
+ if (sbgnrm <= *pgtol) {
+/* terminate the algorithm. */
+ strcpy(task, "CONVERGENCE: NORM OF PROJECTED GRADIENT <= PGTOL");
+ goto L999;
+ }
+/* ----------------- the beginning of the loop -------------------------- */
+L222:
+ if (*iprint >= 99) {
+/*
+ s_wsfe(&io___64);
+ i__1 = iter + 1;
+ do_fio(&c__1, (char *)&i__1, (ftnlen)sizeof(int));
+ e_wsfe();
+*/
+ }
+ iword = -1;
+
+ if (! cnstnd && col > 0) {
+/* skip the search for GCP. */
+ DCOPY(*n, &x[1], (const int&)c__1, &z__[1], (const int&)c__1);
+ wrk = updatd;
+ nint = 0;
+ goto L333;
+ }
+/* ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc */
+
+/* Compute the Generalized Cauchy Point (GCP). */
+
+/* ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc */
+ cauchy_(n, &x[1], &l[1], &u[1], &nbd[1], &g[1], &indx2[1], &iwhere[1], &t[
+ 1], &d__[1], &z__[1], m, &wy[wy_offset], &ws[ws_offset], &sy[
+ sy_offset], &wt[wt_offset], &theta, &col, &head, &wa[1], &wa[(*m
+ << 1) + 1], &wa[(*m << 2) + 1], &wa[*m * 6 + 1], &nint, &sg[1], &
+ yg[1], iprint, &sbgnrm, &info, &epsmch);
+ if (info != 0) {
+/* singular triangular system detected; refresh the lbfgs memory. */
+ if (*iprint >= 1) {
+/*
+ s_wsfe(&io___66);
+ e_wsfe();
+*/
+ }
+ info = 0;
+ col = 0;
+ head = 1;
+ theta = 1.;
+ iupdat = 0;
+ updatd = FALSE_;
+ cpu2 = timer();
+ cachyt = cachyt + cpu2 - cpu1;
+ goto L222;
+ }
+ cpu2 = timer();
+ cachyt = cachyt + cpu2 - cpu1;
+ nintol += nint;
+/* Count the entering and leaving variables for iter > 0; */
+/* find the index set of free and active variables at the GCP. */
+ freev_(n, &nfree, &index[1], &nenter, &ileave, &indx2[1], &iwhere[1], &
+ wrk, &updatd, &cnstnd, iprint, &iter);
+ nact = *n - nfree;
+L333:
+/* If there are no free variables or B=theta*I, then */
+/* skip the subspace minimization. */
+ if (nfree == 0 || col == 0) {
+ goto L555;
+ }
+/* ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc */
+
+/* Subspace minimization. */
+
+/* ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc */
+/* Form the LEL^T factorization of the indefinite */
+/* matrix K = [-D -Y'ZZ'Y/theta L_a'-R_z' ] */
+/* [L_a -R_z theta*S'AA'S ] */
+/* where E = [-I 0] */
+/* [ 0 I] */
+ if (wrk) {
+ formk_(n, &nfree, &index[1], &nenter, &ileave, &indx2[1], &iupdat, &
+ updatd, &wn[wn_offset], &snd[snd_offset], m, &ws[ws_offset], &
+ wy[wy_offset], &sy[sy_offset], &theta, &col, &head, &info);
+ }
+ if (info != 0) {
+/* nonpositive definiteness in Cholesky factorization; */
+/* refresh the lbfgs memory and restart the iteration. */
+ if (*iprint >= 1) {
+/*
+ s_wsfe(&io___68);
+ e_wsfe();
+*/
+ }
+ info = 0;
+ col = 0;
+ head = 1;
+ theta = 1.;
+ iupdat = 0;
+ updatd = FALSE_;
+ cpu2 = timer();
+ sbtime = sbtime + cpu2 - cpu1;
+ goto L222;
+ }
+/* compute r=-Z'B(xcp-xk)-Z'g (using wa(2m+1)=W'(xcp-x) */
+/* from 'cauchy'). */
+ cmprlb_(n, m, &x[1], &g[1], &ws[ws_offset], &wy[wy_offset], &sy[sy_offset]
+ , &wt[wt_offset], &z__[1], &r__[1], &wa[1], &index[1], &theta, &
+ col, &head, &nfree, &cnstnd, &info);
+ if (info != 0) {
+ goto L444;
+ }
+/* call the direct method. */
+ subsm_(n, m, &nfree, &index[1], &l[1], &u[1], &nbd[1], &z__[1], &r__[1], &
+ ws[ws_offset], &wy[wy_offset], &theta, &col, &head, &iword, &wa[1]
+ , &wn[wn_offset], iprint, &info);
+L444:
+ if (info != 0) {
+/* singular triangular system detected; */
+/* refresh the lbfgs memory and restart the iteration. */
+ if (*iprint >= 1) {
+/*
+ s_wsfe(&io___69);
+ e_wsfe();
+*/
+ }
+ info = 0;
+ col = 0;
+ head = 1;
+ theta = 1.;
+ iupdat = 0;
+ updatd = FALSE_;
+ cpu2 = timer();
+ sbtime = sbtime + cpu2 - cpu1;
+ goto L222;
+ }
+ cpu2 = timer();
+ sbtime = sbtime + cpu2 - cpu1;
+L555:
+/* ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc */
+
+/* Line search and optimality tests. */
+
+/* ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc */
+/* Generate the search direction d:=z-x. */
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ d__[i__] = z__[i__] - x[i__];
+/* L40: */
+ }
+L666:
+ lnsrlb_(n, &l[1], &u[1], &nbd[1], &x[1], f, &fold, &gd, &gdold, &g[1], &
+ d__[1], &r__[1], &t[1], &z__[1], &stp, &dnorm, &dtd, &xstep, &
+ stpmx, &iter, &ifun, &iback, &nfgv, &info, task, &boxed, &cnstnd,
+ csave, &isave[22], &dsave[17], (ftnlen)60, (ftnlen)60);
+ if (info != 0 || iback >= 20) {
+/* restore the previous iterate. */
+ DCOPY(*n, &t[1], (const int&)c__1, &x[1], (const int&)c__1);
+ DCOPY(*n, &r__[1], (const int&)c__1, &g[1], (const int&)c__1);
+ *f = fold;
+ if (col == 0) {
+/* abnormal termination. */
+ if (info == 0) {
+ info = -9;
+/* restore the actual number of f and g evaluations etc. */
+ --nfgv;
+ --ifun;
+ --iback;
+ }
+ strcpy(task, "ABNORMAL_TERMINATION_IN_LNSRCH");
+ ++iter;
+ goto L999;
+ } else {
+/* refresh the lbfgs memory and restart the iteration. */
+ if (*iprint >= 1) {
+/*
+ s_wsfe(&io___71);
+ e_wsfe();
+*/
+ }
+ if (info == 0) {
+ --nfgv;
+ }
+ info = 0;
+ col = 0;
+ head = 1;
+ theta = 1.;
+ iupdat = 0;
+ updatd = FALSE_;
+ strcpy(task, "RESTART_FROM_LNSRCH");
+ cpu2 = timer();
+ lnscht = lnscht + cpu2 - cpu1;
+ goto L222;
+ }
+ } else if (strncmp(task, "FG_LN", 5) == 0) {
+/* return to the driver for calculating f and g; reenter at 666. */
+ goto L1000;
+ } else {
+/* calculate and print out the quantities related to the new X. */
+ cpu2 = timer();
+ lnscht = lnscht + cpu2 - cpu1;
+ ++iter;
+/* Compute the infinity norm of the projected (-)gradient. */
+ projgr_(n, &l[1], &u[1], &nbd[1], &x[1], *f, &g[1], &sbgnrm);
+/* Print iteration information. */
+ prn2lb_(n, &x[1], f, &g[1], iprint, &itfile, &iter, &nfgv, &nact, &
+ sbgnrm, &nint, word, &iword, &iback, &stp, &xstep, (ftnlen)3);
+ goto L1000;
+ }
+L777:
+/* Test for termination. */
+ if (sbgnrm <= *pgtol) {
+/* terminate the algorithm. */
+ strcpy(task, "CONVERGENCE: NORM OF PROJECTED GRADIENT <= PGTOL" );
+ goto L999;
+ }
+/* Computing MAX */
+ d__1 = fabs(fold), d__2 = fabs(*f), d__1 = std::max(d__1,d__2);
+ ddum = std::max(d__1,Real(1));
+ if (fold - *f <= tol * ddum) {
+/* terminate the algorithm. */
+ strcpy(task, "CONVERGENCE: REL_REDUCTION_OF_F <= FACTR*EPSMCH");
+ if (iback >= 10) {
+ info = -5;
+ }
+/* i.e., to issue a warning if iback>10 in the line search. */
+ goto L999;
+ }
+/* Compute d=newx-oldx, r=newg-oldg, rr=y'y and dr=y's. */
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ r__[i__] = g[i__] - r__[i__];
+/* L42: */
+ }
+ rr = DDOT(*n, &r__[1], (const int&)c__1, &r__[1], (const int&)c__1);
+ if (stp == 1.) {
+ dr = gd - gdold;
+ ddum = -gdold;
+ } else {
+ dr = (gd - gdold) * stp;
+ DSCAL(*n, (const Real &)stp, &d__[1], (const int&)c__1);
+ ddum = -gdold * stp;
+ }
+ if (dr <= epsmch * ddum) {
+/* skip the L-BFGS update. */
+ ++nskip;
+ updatd = FALSE_;
+ if (*iprint >= 1) {
+/*
+ s_wsfe(&io___75);
+ do_fio(&c__1, (char *)&dr, (ftnlen)sizeof(Real));
+ do_fio(&c__1, (char *)&ddum, (ftnlen)sizeof(Real));
+ e_wsfe();
+*/
+ }
+ goto L888;
+ }
+/* ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc */
+
+/* Update the L-BFGS matrix. */
+
+/* ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc */
+ updatd = TRUE_;
+ ++iupdat;
+/* Update matrices WS and WY and form the middle matrix in B. */
+ matupd_(n, m, &ws[ws_offset], &wy[wy_offset], &sy[sy_offset], &ss[
+ ss_offset], &d__[1], &r__[1], &itail, &iupdat, &col, &head, &
+ theta, &rr, &dr, &stp, &dtd);
+/* Form the upper half of the pds T = theta*SS + L*D^(-1)*L'; */
+/* Store T in the upper triangular of the array wt; */
+/* Cholesky factorize T to J*J' with */
+/* J' stored in the upper triangular of wt. */
+ formt_(m, &wt[wt_offset], &sy[sy_offset], &ss[ss_offset], &col, &theta, &
+ info);
+ if (info != 0) {
+/* nonpositive definiteness in Cholesky factorization; */
+/* refresh the lbfgs memory and restart the iteration. */
+ if (*iprint >= 1) {
+/*
+ s_wsfe(&io___76);
+ e_wsfe();
+*/
+ }
+ info = 0;
+ col = 0;
+ head = 1;
+ theta = 1.;
+ iupdat = 0;
+ updatd = FALSE_;
+ goto L222;
+ }
+/* Now the inverse of the middle matrix in B is */
+/* [ D^(1/2) O ] [ -D^(1/2) D^(-1/2)*L' ] */
+/* [ -L*D^(-1/2) J ] [ 0 J' ] */
+L888:
+/* -------------------- the end of the loop ----------------------------- */
+ goto L222;
+L999:
+ time2 = timer();
+ time = time2 - time1;
+ prn3lb_(n, &x[1], f, task, iprint, &info, &itfile, &iter, &nfgv, &nintol,
+ &nskip, &nact, &sbgnrm, &time, &nint, word, &iback, &stp, &xstep,
+ &k, &cachyt, &sbtime, &lnscht, (ftnlen)60, (ftnlen)3);
+L1000:
+/* Save local variables. */
+ lsave[1] = prjctd;
+ lsave[2] = cnstnd;
+ lsave[3] = boxed;
+ lsave[4] = updatd;
+ isave[1] = nintol;
+ isave[3] = itfile;
+ isave[4] = iback;
+ isave[5] = nskip;
+ isave[6] = head;
+ isave[7] = col;
+ isave[8] = itail;
+ isave[9] = iter;
+ isave[10] = iupdat;
+ isave[12] = nint;
+ isave[13] = nfgv;
+ isave[14] = info;
+ isave[15] = ifun;
+ isave[16] = iword;
+ isave[17] = nfree;
+ isave[18] = nact;
+ isave[19] = ileave;
+ isave[20] = nenter;
+ dsave[1] = theta;
+ dsave[2] = fold;
+ dsave[3] = tol;
+ dsave[4] = dnorm;
+ dsave[5] = epsmch;
+ dsave[6] = cpu1;
+ dsave[7] = cachyt;
+ dsave[8] = sbtime;
+ dsave[9] = lnscht;
+ dsave[10] = time1;
+ dsave[11] = gd;
+ dsave[12] = stpmx;
+ dsave[13] = sbgnrm;
+ dsave[14] = stp;
+ dsave[15] = gdold;
+ dsave[16] = dtd;
+ return 0;
+} /* mainlb_ */
+
+/* ======================= The end of mainlb ============================= */
+
+/* ================ L-BFGS-B (version 2.1) ========================== */
+/* Subroutine */
+int SimTK::LBFGSBOptimizer::setulb_(int *n, int *m, Real *x, Real *l,
+ Real *u, int *nbd, Real *f, Real *g,
+ Real *factr, Real *pgtol, Real *wa, int *iwa,
+ char *task, int *iprint, char *csave, bool *lsave,
+ int *isave, Real *dsave, long task_len, long csave_len)
+{
+ /* System generated locals */
+ int i__1;
+
+ /* Local variables */
+ int lsnd, lsgo, lygo, /* l1, l2, l3, */ ld, lr, lt;
+ int lz, lwa, lsg, lyg, lwn, lss, lws, lwt, lsy, lwy, lyy;
+ (void)task_len;
+ (void)csave_len;
+
+/* ************ */
+
+/* Subroutine setulb */
+
+/* This subroutine partitions the working arrays wa and iwa, and */
+/* then uses the limited memory BFGS method to solve the bound */
+/* constrained optimization problem by calling mainlb. */
+/* (The direct method will be used in the subspace minimization.) */
+
+/* n is an int variable. */
+/* On entry n is the dimension of the problem. */
+/* On exit n is unchanged. */
+
+/* m is an int variable. */
+/* On entry m is the maximum number of variable metric corrections */
+/* used to define the limited memory matrix. */
+/* On exit m is unchanged. */
+
+/* x is a double precision array of dimension n. */
+/* On entry x is an approximation to the solution. */
+/* On exit x is the current approximation. */
+
+/* l is a double precision array of dimension n. */
+/* On entry l is the lower bound on x. */
+/* On exit l is unchanged. */
+
+/* u is a double precision array of dimension n. */
+/* On entry u is the upper bound on x. */
+/* On exit u is unchanged. */
+
+/* nbd is an int array of dimension n. */
+/* On entry nbd represents the type of bounds imposed on the */
+/* variables, and must be specified as follows: */
+/* nbd(i)=0 if x(i) is unbounded, */
+/* 1 if x(i) has only a lower bound, */
+/* 2 if x(i) has both lower and upper bounds, and */
+/* 3 if x(i) has only an upper bound. */
+/* On exit nbd is unchanged. */
+
+/* f is a double precision variable. */
+/* On first entry f is unspecified. */
+/* On final exit f is the value of the function at x. */
+
+/* g is a double precision array of dimension n. */
+/* On first entry g is unspecified. */
+/* On final exit g is the value of the gradient at x. */
+
+/* factr is a double precision variable. */
+/* On entry factr >= 0 is specified by the user. The iteration */
+/* will stop when */
+
+/* (f^k - f^{k+1})/max{|f^k|,|f^{k+1}|,1} <= factr*epsmch */
+
+/* where epsmch is the machine precision, which is automatically */
+/* generated by the code. Typical values for factr: 1.d+12 for */
+/* low accuracy; 1.d+7 for moderate accuracy; 1.d+1 for extremely */
+/* high accuracy. */
+/* On exit factr is unchanged. */
+
+/* pgtol is a double precision variable. */
+/* On entry pgtol >= 0 is specified by the user. The iteration */
+/* will stop when */
+
+/* max{|proj g_i | i = 1, ..., n} <= pgtol */
+
+/* where pg_i is the ith component of the projected gradient. */
+/* On exit pgtol is unchanged. */
+
+/* wa is a double precision working array of length */
+/* (2mmax + 4)nmax + 12mmax^2 + 12mmax. */
+
+/* iwa is an int working array of length 3nmax. */
+
+/* task is a working string of characters of length 60 indicating */
+/* the current job when entering and quitting this subroutine. */
+
+/* iprint is an int variable that must be set by the user. */
+/* It controls the frequency and type of output generated: */
+/* iprint<0 no output is generated; */
+/* iprint=0 print only one line at the last iteration; */
+/* 0<iprint<99 print also f and |proj g| every iprint iterations; */
+/* iprint=99 print details of every iteration except n-vectors; */
+/* iprint=100 print also the changes of active set and final x; */
+/* iprint>100 print details of every iteration including x and g; */
+/* When iprint > 0, the file iterate.dat will be created to */
+/* summarize the iteration. */
+
+/* csave is a working string of characters of length 60. */
+
+/* lsave is a bool working array of dimension 4. */
+/* On exit with 'task' = NEW_X, the following information is */
+/* available: */
+/* If lsave(1) = .true. then the initial X has been replaced by */
+/* its projection in the feasible set; */
+/* If lsave(2) = .true. then the problem is constrained; */
+/* If lsave(3) = .true. then each variable has upper and lower */
+/* bounds; */
+
+/* isave is an int working array of dimension 44. */
+/* On exit with 'task' = NEW_X, the following information is */
+/* available: */
+/* isave(22) = the total number of intervals explored in the */
+/* search of Cauchy points; */
+/* isave(26) = the total number of skipped BFGS updates before */
+/* the current iteration; */
+/* isave(30) = the number of current iteration; */
+/* isave(31) = the total number of BFGS updates prior the current */
+/* iteration; */
+/* isave(33) = the number of intervals explored in the search of */
+/* Cauchy point in the current iteration; */
+/* isave(34) = the total number of function and gradient */
+/* evaluations; */
+/* isave(36) = the number of function value or gradient */
+/* evaluations in the current iteration; */
+/* if isave(37) = 0 then the subspace argmin is within the box; */
+/* if isave(37) = 1 then the subspace argmin is beyond the box; */
+/* isave(38) = the number of free variables in the current */
+/* iteration; */
+/* isave(39) = the number of active constraints in the current */
+/* iteration; */
+/* n + 1 - isave(40) = the number of variables leaving the set of */
+/* active constraints in the current iteration; */
+/* isave(41) = the number of variables entering the set of active */
+/* constraints in the current iteration. */
+
+/* dsave is a double precision working array of dimension 29. */
+/* On exit with 'task' = NEW_X, the following information is */
+/* available: */
+/* dsave(1) = current 'theta' in the BFGS matrix; */
+/* dsave(2) = f(x) in the previous iteration; */
+/* dsave(3) = factr*epsmch; */
+/* dsave(4) = 2-norm of the line search direction vector; */
+/* dsave(5) = the machine precision epsmch generated by the code; */
+/* dsave(7) = the accumulated time spent on searching for */
+/* Cauchy points; */
+/* dsave(8) = the accumulated time spent on */
+/* subspace minimization; */
+/* dsave(9) = the accumulated time spent on line search; */
+/* dsave(11) = the slope of the line search function at */
+/* the current point of line search; */
+/* dsave(12) = the maximum relative step length imposed in */
+/* line search; */
+/* dsave(13) = the infinity norm of the projected gradient; */
+/* dsave(14) = the relative step length in the line search; */
+/* dsave(15) = the slope of the line search function at */
+/* the starting point of the line search; */
+/* dsave(16) = the square of the 2-norm of the line search */
+/* direction vector. */
+
+/* Subprograms called: */
+
+/* L-BFGS-B Library ... mainlb. */
+
+
+/* References: */
+
+/* [1] R. H. Byrd, P. Lu, J. Nocedal and C. Zhu, ``A limited */
+/* memory algorithm for bound constrained optimization'', */
+/* SIAM J. Scientific Computing 16 (1995), no. 5, pp. 1190--1208. */
+
+/* [2] C. Zhu, R.H. Byrd, P. Lu, J. Nocedal, ``L-BFGS-B: a */
+/* limited memory FORTRAN code for solving bound constrained */
+/* optimization problems'', Tech. Report, NAM-11, EECS Department, */
+/* Northwestern University, 1994. */
+
+/* (Postscript files of these papers are available via anonymous */
+/* ftp to eecs.nwu.edu in the directory pub/lbfgs/lbfgs_bcm.) */
+
+/* * * * */
+
+/* NEOS, November 1994. (Latest revision June 1996.) */
+/* Optimization Technology Center. */
+/* Argonne National Laboratory and Northwestern University. */
+/* Written by */
+/* Ciyou Zhu */
+/* in collaboration with R.H. Byrd, P. Lu-Chen and J. Nocedal. */
+
+
+/* ************ */
+ /* Parameter adjustments */
+ --iwa;
+ --g;
+ --nbd;
+ --u;
+ --l;
+ --x;
+ --wa;
+ --lsave;
+ --isave;
+ --dsave;
+
+ /* Function Body */
+ if (strncmp(task, "START", 5) == 0) {
+ isave[1] = *m * *n;
+/* Computing 2nd power */
+ i__1 = *m;
+ isave[2] = i__1 * i__1;
+/* Computing 2nd power */
+ i__1 = *m;
+ isave[3] = i__1 * i__1 << 2;
+ isave[4] = 1;
+ isave[5] = isave[4] + isave[1];
+ isave[6] = isave[5] + isave[1];
+ isave[7] = isave[6] + isave[2];
+ isave[8] = isave[7] + isave[2];
+ isave[9] = isave[8] + isave[2];
+ isave[10] = isave[9] + isave[2];
+ isave[11] = isave[10] + isave[3];
+ isave[12] = isave[11] + isave[3];
+ isave[13] = isave[12] + *n;
+ isave[14] = isave[13] + *n;
+ isave[15] = isave[14] + *n;
+ isave[16] = isave[15] + *n;
+ isave[17] = isave[16] + (*m << 3);
+ isave[18] = isave[17] + *m;
+ isave[19] = isave[18] + *m;
+ isave[20] = isave[19] + *m;
+ }
+ /* l1 = isave[1];
+ l2 = isave[2];
+ l3 = isave[3]; */
+ lws = isave[4];
+ lwy = isave[5];
+ lsy = isave[6];
+ lss = isave[7];
+ lyy = isave[8];
+ lwt = isave[9];
+ lwn = isave[10];
+ lsnd = isave[11];
+ lz = isave[12];
+ lr = isave[13];
+ ld = isave[14];
+ lt = isave[15];
+ lwa = isave[16];
+ lsg = isave[17];
+ lsgo = isave[18];
+ lyg = isave[19];
+ lygo = isave[20];
+ mainlb_(n, m, &x[1], &l[1], &u[1], &nbd[1], f, &g[1], factr, pgtol, &wa[
+ lws], &wa[lwy], &wa[lsy], &wa[lss], &wa[lyy], &wa[lwt], &wa[lwn],
+ &wa[lsnd], &wa[lz], &wa[lr], &wa[ld], &wa[lt], &wa[lwa], &wa[lsg],
+ &wa[lsgo], &wa[lyg], &wa[lygo], &iwa[1], &iwa[*n + 1], &iwa[(*n
+ << 1) + 1], task, iprint, csave, &lsave[1], &isave[22], &dsave[1],
+ (ftnlen)60, (ftnlen)60);
+ return 0;
+} /* setulb_ */
+
+/* ======================= The end of setulb ============================= */
diff --git a/SimTKmath/doc/SimmathUserGuide.doc b/SimTKmath/doc/SimmathUserGuide.doc
new file mode 100644
index 0000000..00d0f26
Binary files /dev/null and b/SimTKmath/doc/SimmathUserGuide.doc differ
diff --git a/SimTKmath/doc/SimmathUserGuide.pdf b/SimTKmath/doc/SimmathUserGuide.pdf
new file mode 100644
index 0000000..756bf80
Binary files /dev/null and b/SimTKmath/doc/SimmathUserGuide.pdf differ
diff --git a/SimTKmath/doc/images/BicubicSurface1.png b/SimTKmath/doc/images/BicubicSurface1.png
new file mode 100644
index 0000000..b10c288
Binary files /dev/null and b/SimTKmath/doc/images/BicubicSurface1.png differ
diff --git a/SimTKmath/examples/CMakeLists.txt b/SimTKmath/examples/CMakeLists.txt
new file mode 100644
index 0000000..a6c7d28
--- /dev/null
+++ b/SimTKmath/examples/CMakeLists.txt
@@ -0,0 +1,63 @@
+
+#FILE( GLOB SOURCES ${PROJECT_SOURCE_DIR}/examples/*.cpp )
+
+#FOREACH (src_file ${SOURCES} )
+# GET_FILENAME_COMPONENT( file_name ${src_file} NAME )
+# INSTALL_FILES( "/core/examples/simmath/" FILES ${file_name} )
+#ENDFOREACH (src_file ${SOURCES})
+
+#INSTALL_FILES( "/core/examples/simmath/" FILES
+# ${PROJECT_SOURCE_DIR}/examples/README.txt)
+
+# Generate and install examples.
+#
+# This is boilerplate code for generating a set of executables, one per
+# .cpp file in an "examples" subdirectory. These are intended to
+# be installed with the library but we don't handle installation
+# here. Unlike the similar boilerplate code in the "tests"
+# directory (but like the "tests/adhoc" boilerplate), this does
+# not generate CMake ADD_TEST commands so these will never run
+# automatically.
+#
+# For IDEs that can deal with PROJECT_LABEL properties (at least
+# Visual Studio) the projects for building each of these adhoc
+# executables will be labeled "Example - TheExampleName" if a file
+# TheExampleName.cpp is found in this directory.
+#
+# We check the BUILD_TESTS_AND_EXAMPLES_{SHARED,STATIC} variables to determine
+# whether to build dynamically linked, statically linked, or both
+# versions of the executable.
+
+
+SET(EXAMPLES_INSTALL ${CMAKE_INSTALL_DOCDIR}/examples/simmath/)
+FILE(GLOB EXAMPLES "*.cpp")
+FOREACH(EX_PROG ${EXAMPLES})
+ GET_FILENAME_COMPONENT(EX_SRC ${EX_PROG} NAME)
+ GET_FILENAME_COMPONENT(EX_ROOT ${EX_PROG} NAME_WE)
+
+ IF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+ # Link with shared library
+ ADD_EXECUTABLE(${EX_ROOT} ${EX_PROG})
+ SET_TARGET_PROPERTIES(${EX_ROOT}
+ PROPERTIES
+ PROJECT_LABEL "Example - ${EX_ROOT}")
+ TARGET_LINK_LIBRARIES(${EX_ROOT} ${TEST_SHARED_TARGET})
+ ENDIF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+
+ IF (BUILD_STATIC_LIBRARIES AND BUILD_TESTS_AND_EXAMPLES_STATIC)
+ # Link with static library
+ SET(EX_STATIC ${EX_ROOT}Static)
+ ADD_EXECUTABLE(${EX_STATIC} ${EX_PROG})
+ SET_TARGET_PROPERTIES(${EX_STATIC}
+ PROPERTIES
+ COMPILE_FLAGS "-DSimTK_USE_STATIC_LIBRARIES"
+ PROJECT_LABEL "Example - ${EX_STATIC}")
+ TARGET_LINK_LIBRARIES(${EX_STATIC} ${TEST_STATIC_TARGET})
+ ENDIF()
+
+ INSTALL(FILES ${EX_SRC} DESTINATION ${EXAMPLES_INSTALL})
+
+ENDFOREACH(EX_PROG ${ADHOC_TESTS})
+
+INSTALL(FILES README.txt DESTINATION ${EXAMPLES_INSTALL})
+
diff --git a/SimTKmath/examples/ConstrainedNumericalDiffOptimization.cpp b/SimTKmath/examples/ConstrainedNumericalDiffOptimization.cpp
new file mode 100644
index 0000000..186fc15
--- /dev/null
+++ b/SimTKmath/examples/ConstrainedNumericalDiffOptimization.cpp
@@ -0,0 +1,123 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Constrained Optimization w/Numerical Gradient *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+
+using namespace SimTK;
+
+static int NumberOfParameters = 4;
+static int NumberOfEqualityConstraints = 1;
+static int NumberOfInequalityConstraints = 1;
+
+/*
+ * Problem hs071 looks like this
+ *
+ * min x1*x4*(x1 + x2 + x3) + x3
+ * s.t. x1*x2*x3*x4 >= 25
+ * x1**2 + x2**2 + x3**2 + x4**2 = 40
+ * 1 <= x1,x2,x3,x4 <= 5
+ *
+ * Starting point:
+ * x = (1, 5, 5, 1)
+ *
+ * Optimal solution:
+ * x = (1.00000000, 4.74299963, 3.82114998, 1.37940829)
+ *
+ */
+
+class ProblemSystem : public OptimizerSystem {
+public:
+
+
+ int objectiveFunc( const Vector &coefficients, bool new_coefficients, Real& f ) const {
+ const Real *x;
+
+ x = &coefficients[0];
+
+ f = x[0] * x[3] * (x[0] + x[1] + x[2]) + x[2];
+ return( 0 );
+ }
+
+ int constraintFunc( const Vector &coefficients, bool new_coefficients, Vector &constraints) const{
+ const Real *x;
+
+ x = &coefficients[0];
+ constraints[0] = x[0]*x[0] + x[1]*x[1] + x[2]*x[2] + x[3]*x[3] - 40.0;
+ constraints[1] = x[0] * x[1] * x[2] * x[3] - 25.0;
+
+ return(0);
+ }
+
+ ProblemSystem( const int numParams, const int numEqualityConstraints, const int numInequalityConstraints ) :
+ OptimizerSystem( numParams )
+ {
+ setNumEqualityConstraints( numEqualityConstraints );
+ setNumInequalityConstraints( numInequalityConstraints );
+ }
+
+};
+
+
+int main() {
+ /* create the system to be optimized */
+ ProblemSystem sys(NumberOfParameters, NumberOfEqualityConstraints, NumberOfInequalityConstraints);
+
+ Vector results(NumberOfParameters);
+ Vector lower_bounds(NumberOfParameters);
+ Vector upper_bounds(NumberOfParameters);
+
+ /* set initial conditions */
+ results[0] = 1.0;
+ results[1] = 5.0;
+ results[2] = 5.0;
+ results[3] = 1.0;
+
+ /* set bounds */
+ for(int i=0;i<NumberOfParameters;i++) {
+ lower_bounds[i] = 1.0;
+ upper_bounds[i] = 5.0;
+ }
+
+ sys.setParameterLimits( lower_bounds, upper_bounds );
+
+ Real f = NaN;
+ try {
+ Optimizer opt( sys );
+
+ opt.setConvergenceTolerance( .0001 );
+ opt.useNumericalGradient( true );
+ opt.useNumericalJacobian( true );
+
+ /* compute optimization */
+ f = opt.optimize( results );
+ }
+ catch (const std::exception& e) {
+ std::cout << "ConstrainedNumericalDiffOptimization.cpp: Caught exception" << std::endl;
+ std::cout << e.what() << std::endl;
+ }
+
+ printf("Optimal Solution: f = %f parameters = %f %f %f %f \n",f, results[0],results[1],results[2],results[3]);
+
+ return 0;
+}
diff --git a/SimTKmath/examples/ConstrainedOptimization.cpp b/SimTKmath/examples/ConstrainedOptimization.cpp
new file mode 100644
index 0000000..88b5fd9
--- /dev/null
+++ b/SimTKmath/examples/ConstrainedOptimization.cpp
@@ -0,0 +1,166 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Constrained Optimization w/Analytical Gradient *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+
+using namespace SimTK;
+
+static int NumberOfParameters = 4;
+static int NumberOfEqualityConstraints = 1;
+static int NumberOfInequalityConstraints = 1;
+
+/*
+ * This example was adapted from IPOPT's hs071 example
+ *
+ * Problem statement:
+ *
+ * minimize: x1*x4*(x1 + x2 + x3) + x3
+ *
+ * s.t. x1*x2*x3*x4 >= 25 inequality constraint
+ * x1**2 + x2**2 + x3**2 + x4**2 - 40.0 = 0.0 equality constraint
+ *
+ * 1 <= x1,x2,x3,x4 <= 5 each parameter has a lower limit of 1.0 and an upper limit of 5.0
+ *
+ * Starting point:
+ * x = (1, 5, 5, 1) will be used for the initial conditions
+ *
+ * Optimal solution:
+ * x = (1.00000000, 4.74299963, 3.82114998, 1.37940829)
+ *
+ */
+
+class ProblemSystem : public OptimizerSystem {
+public:
+
+
+ int objectiveFunc( const Vector &coefficients, bool new_coefficients, Real& f ) const {
+ const Real *x;
+
+ x = &coefficients[0];
+
+ f = x[0] * x[3] * (x[0] + x[1] + x[2]) + x[2];
+ return( 0 );
+ }
+
+ int gradientFunc( const Vector &coefficients, bool new_coefficients, Vector &gradient ) const{
+ const Real *x;
+
+ x = &coefficients[0];
+
+ gradient[0] = x[0] * x[3] + x[3] * (x[0] + x[1] + x[2]);
+ gradient[1] = x[0] * x[3];
+ gradient[2] = x[0] * x[3] + 1;
+ gradient[3] = x[0] * (x[0] + x[1] + x[2]);
+
+ return(0);
+
+ }
+
+ /*
+ ** Method to compute the value of the constraints.
+ ** Equality constraints are first followed by the any inequality constraints
+ */
+ int constraintFunc( const Vector &coefficients, bool new_coefficients, Vector &constraints) const{
+ const Real *x;
+
+ x = &coefficients[0];
+ constraints[0] = x[0]*x[0] + x[1]*x[1] + x[2]*x[2] + x[3]*x[3] - 40.0;
+ constraints[1] = x[0] * x[1] * x[2] * x[3] - 25.0;
+
+ return(0);
+ }
+
+
+ /*
+ ** Method to compute the jacobian of the constraints.
+ **
+ */
+ int constraintJacobian( const Vector& coefficients, bool new_coefficients, Matrix& jac) const{
+ const Real *x;
+
+ x = &coefficients[0];
+ jac(0,0) = 2*x[0];
+ jac(0,1) = 2*x[1];
+ jac(0,2) = 2*x[2];
+ jac(0,3) = 2*x[3];
+ jac(1,0) = x[1]*x[2]*x[3];
+ jac(1,1) = x[0]*x[2]*x[3];
+ jac(1,2) = x[0]*x[1]*x[3];
+ jac(1,3) = x[0]*x[1]*x[2];
+
+
+ return(0);
+ }
+
+ ProblemSystem( const int numParams, const int numEqualityConstraints, const int numInequalityConstraints ) :
+ OptimizerSystem( numParams )
+ {
+ setNumEqualityConstraints( numEqualityConstraints );
+ setNumInequalityConstraints( numInequalityConstraints );
+ }
+
+};
+
+
+int main() {
+ /* create the system to be optimized */
+ ProblemSystem sys(NumberOfParameters, NumberOfEqualityConstraints, NumberOfInequalityConstraints);
+
+ Vector results(NumberOfParameters);
+ Vector lower_bounds(NumberOfParameters);
+ Vector upper_bounds(NumberOfParameters);
+
+ /* set initial conditions */
+ results[0] = 1.0;
+ results[1] = 5.0;
+ results[2] = 5.0;
+ results[3] = 1.0;
+
+ /* set bounds */
+ for(int i=0;i<NumberOfParameters;i++) {
+ lower_bounds[i] = 1.0;
+ upper_bounds[i] = 5.0;
+ }
+
+ sys.setParameterLimits( lower_bounds, upper_bounds );
+
+ Real f = NaN;
+ try {
+ Optimizer opt( sys );
+
+ opt.setConvergenceTolerance( .0001 );
+
+ /* compute optimization */
+ f = opt.optimize( results );
+ }
+ catch (const std::exception& e) {
+ std::cout << "ConstrainedOptimization.cpp Caught exception:" << std::endl;
+ std::cout << e.what() << std::endl;
+ }
+
+
+ printf("Optimal Solution: f = %f parameters = %f %f %f %f \n",f,results[0],results[1],results[2],results[3]);
+
+ return 0;
+}
diff --git a/SimTKmath/examples/Differentiator.cpp b/SimTKmath/examples/Differentiator.cpp
new file mode 100644
index 0000000..6d5b563
--- /dev/null
+++ b/SimTKmath/examples/Differentiator.cpp
@@ -0,0 +1,384 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Differentiator *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This is a test program which uses the Differentiator class in various ways.
+ */
+
+//#define SimTK_USE_STATIC_LIBRARIES
+
+#include "SimTKmath.h"
+
+// Just so we can get the version number:
+#include "SimTKlapack.h"
+
+#include <cstdio>
+#include <iostream>
+
+using namespace SimTK;
+
+// This is a system of functions of a particular set of parameters (state).
+// The underlying function wants time also, so we provide that as data in
+// the concrete class. Time should be set prior to calculation of the Jacobian.
+class MyVectorFunc : public Differentiator::JacobianFunction {
+public:
+ MyVectorFunc(int nf, int ny)
+ : Differentiator::JacobianFunction(nf,ny), time(0) { }
+
+ void setTime(Real t) {time=t;}
+ Real getTime() const {return time;}
+
+ // Must provide this pure virtual function.
+ int f(const Vector& y, Vector& fy) const;
+private:
+ Real time;
+};
+
+// This is a single scalar function of a vector of parameters.
+class MyObjectiveFunc : public Differentiator::GradientFunction {
+public:
+ MyObjectiveFunc(int ny)
+ : Differentiator::GradientFunction(ny), time(0) { }
+
+ void setTime(Real t) {time=t;}
+ Real getTime() const {return time;}
+
+ // Must provide this pure virtual function.
+ int f(const Vector& y, Real& fy) const;
+private:
+ Real time;
+};
+
+// This represents a generic scalar function of a scalar parameter,
+// where the actual function has a simple C signature.
+class GenericScalarFunc : public Differentiator::ScalarFunction {
+ typedef Real (*CFunc)(Real);
+public:
+ GenericScalarFunc(CFunc cf)
+ : Differentiator::ScalarFunction(), cp(cf) { }
+
+ // Must provide this pure virtual function.
+ int f(Real x, Real& fx) const {
+ fx = cp(x);
+ return 0;
+ }
+
+ CFunc cp;
+};
+
+#define PREC double
+class SinOmegaX : public Differentiator::ScalarFunction {
+public:
+ SinOmegaX(Real omega, Real acc)
+ : ScalarFunction(acc), w(omega)
+ {
+ }
+
+ Real calc(Real x) const {
+ return std::sin(w*x);
+ }
+ Real calcD1(Real x) const {
+ return w*std::cos(w*x);
+ }
+ Real calcD2(Real x) const {
+ return -w*w*std::sin(w*x);
+ }
+ // Must provide this virtual function.
+ int f(Real x, Real& fx) const {
+ volatile PREC ffx = (PREC)calc(x);
+ fx = ffx;
+ return 0; // success
+ }
+private:
+ const Real w;
+};
+
+class Cubic : public Differentiator::ScalarFunction {
+public:
+ // ax^3+bx^2+cx+d
+ Cubic(Real aa, Real bb, Real cc, Real dd, Real acc)
+ : ScalarFunction(acc), a(aa),b(bb),c(cc),d(dd)
+ {
+ }
+
+ Real calc(Real x) const {
+ return (a*x*x*x + b*x*x + c*x + d)*std::exp(a*x);
+ }
+
+ Real calcD1(Real x) const {
+ return (3*a*x*x+2*b*x+c)*std::exp(a*x) + a*calc(x);
+ }
+
+ Real calcD2(Real x) const {
+ return (6*a*x+2*b)*std::exp(a*x) + (3*a*x*x+2*b*x+c)*a*std::exp(a*x)
+ + a*calcD1(x);
+ }
+
+ // Must provide this virtual function.
+ int f(Real x, Real& fx) const {
+ volatile PREC ffx = (PREC)calc(x);
+ fx = ffx;
+ return 0; // success
+ }
+private:
+ const Real a, b, c, d;
+};
+
+
+
+static void doSinOmegaExample() {
+ for (int digits=0; digits<=41; ++digits) {
+ Real acc;
+ if (digits < 40) acc = std::pow(10., -(digits/(1.5*sizeof(double)/sizeof(PREC))));
+ else if (digits==40) acc=SimTK::NTraits<PREC>::getSignificant();
+ else if (digits==41) acc=SimTK::NTraits<PREC>::getEps();
+
+ const Real w = .01;
+ SinOmegaX func(w, acc);
+ //Cubic func(-1,-2,3,4, acc);
+ Differentiator dsin3x(func);
+
+ const int NEntries = 1000;
+ const Real offs =0.1;
+ const Real increment = (Real)SimTK_PI/NEntries;
+ //printf("%8s %12s %12s %12s\n",
+ // "x", "3cos3x", "err1", "err2");
+
+ Real err1rms=0, err1max=0, err2rms=0, err2max=0;
+ for (int i=0; i < NEntries; ++i) {
+ const Real x = offs + i*increment;
+ if (digits==0) printf("%12g %12g %12g\n", func.calc(x), func.calcD1(x), func.calcD2(x));
+ const Real analytic = func.calcD1(x);
+ const Real approx1st = dsin3x.calcDerivative(x);
+ const Real approx2nd = dsin3x.calcDerivative(x, Differentiator::CentralDifference);
+ const Real err1 = std::abs((approx1st-analytic)/analytic);
+ const Real err2 = std::abs((approx2nd-analytic)/analytic);
+ err1rms += err1*err1; err2rms += err2*err2;
+ if (err1 > err1max) err1max=err1;
+ if (err2 > err2max) err2max=err2;
+ //printf("%8g %12g %12.3e %12.3e\n",
+ // x, analytic, err1, err2);
+ }
+ printf("%.3e: err1: max=%.3e, rms=%.3e err2: max=%.3e, rms=%.3e\n",
+ acc, err1max, std::sqrt(err1rms/NEntries), err2max, std::sqrt(err2rms/NEntries));
+ }
+ exit(0);
+};
+
+static void showSimTKAboutInfo() {
+
+ int major,minor,build;
+ char out[100];
+ const char* keylist[] = { "version", "library", "type", "debug", "authors", "copyright", "svn_revision", 0 };
+
+#ifdef TEST_LAPACK_VERSION
+ SimTK_version_SimTKlapack(&major,&minor,&build);
+ std::printf("==> SimTKlapack library version: %d.%d.%d\n", major, minor, build);
+ std::printf(" SimTK_about_SimTKlapack():\n");
+ for (const char** p = keylist; *p; ++p) {
+ SimTK_about_SimTKlapack(*p, 100, out);
+ std::printf(" about(%s)='%s'\n", *p, out);
+ }
+#endif
+
+ SimTK_version_SimTKcommon(&major,&minor,&build);
+ std::printf("==> SimTKcommon library version: %d.%d.%d\n", major, minor, build);
+ std::printf(" SimTK_about_SimTKcommon():\n");
+ for (const char** p = keylist; *p; ++p) {
+ SimTK_about_SimTKcommon(*p, 100, out);
+ std::printf(" about(%s)='%s'\n", *p, out);
+ }
+
+ SimTK_version_simmath(&major,&minor,&build);
+ std::printf("==> SimTKmath library version: %d.%d.%d\n", major, minor, build);
+ std::printf(" SimTK_about_simmath():\n");
+ for (const char** p = keylist; *p; ++p) {
+ SimTK_about_simmath(*p, 100, out);
+ std::printf(" about(%s)='%s'\n", *p, out);
+ }
+ std::printf("\n");
+
+}
+
+static Real mysin(Real x) {
+ return std::sin(x);
+}
+static Real mycos(Real x) {
+ return std::cos(x);
+}
+
+int main() {
+ showSimTKAboutInfo();
+ doSinOmegaExample();
+
+ Vector yy, yp;
+
+ yy.resize(4);
+ yp.resize(4);
+
+ /* Initialize y */
+ yy[0] = 1.0; /* x */
+ yy[1] = 0.0; /* y */
+ yy[2] = 0.0; /* xd */
+ yy[3] = 0.0; /* yd */
+
+ // Define a scalar, vector, and system of functions.
+ GenericScalarFunc gf(mysin);
+ MyObjectiveFunc sf(4);
+ MyVectorFunc vf(4,4);
+ vf.setEstimatedAccuracy(1e-6); // claim reduced accuracy (6 digits)
+
+ // Create differentiators for each of the functions.
+ Differentiator dsin(gf,Differentiator::CentralDifference); // use calcDerivative()
+ Differentiator gradf(sf); // use calcGradient()
+ Differentiator df(vf); // use calcJacobian()
+
+ int returnValue = 0; // assume success
+ try {
+ gradf.setDefaultMethod(Differentiator::ForwardDifference);
+ df.setDefaultMethod(Differentiator::UnspecifiedMethod);
+
+ printf("dsin default method: %s\n", Differentiator::getMethodName(dsin.getDefaultMethod()));
+ printf("gradf default method: %s\n", Differentiator::getMethodName(gradf.getDefaultMethod()));
+ printf("df default method: %s\n", Differentiator::getMethodName(df.getDefaultMethod()));
+
+ const Real ang = SimTK_PI/8;
+ std::cout << "sin(x)=" << mysin(ang) << std::endl;
+ std::cout << "d sin(x)/dx=" << dsin.calcDerivative(ang) << std::endl;
+ std::cout << "cos(x)=" << mycos(ang) << std::endl;
+
+ const Real rp[] = {.01,.02,.03,-.14};
+ Vector delta_y(4,rp);
+ Vector y0 = yy+delta_y; // don't start right at 0
+
+
+ printf("Func gf: nf=%d np=%d estacc=%g\n",
+ gf.getNumFunctions(), gf.getNumParameters(), gf.getEstimatedAccuracy());
+ printf("Func sf: nf=%d np=%d estacc=%g\n",
+ sf.getNumFunctions(), sf.getNumParameters(), sf.getEstimatedAccuracy());
+ printf("Func vf: nf=%d np=%d estacc=%g\n",
+ vf.getNumFunctions(), vf.getNumParameters(), vf.getEstimatedAccuracy());
+
+
+ Real sfy0, sfyd;
+ sf.f(y0, sfy0); // calculate unperturbed value
+ Vector grad1;
+ gradf.calcGradient(y0, sfy0, grad1);
+
+ Vector grad2;
+ gradf.calcGradient(y0, sfy0, grad2, Differentiator::CentralDifference);
+ std::cout << "sf(y0)=" << sfy0 << std::endl;
+ std::cout << "order 1 grad(sf)=" << grad1 << std::endl;
+ std::cout << "order 2 grad(sf)=" << grad2 << std::endl;
+
+ sf.f(y0+delta_y, sfyd);
+ std::cout << "sf(y0+dy)=" << sfyd << std::endl;
+ std::cout << "sf(y0)+~grad1*dy=" << sfy0 + ~grad1*delta_y << std::endl;
+ std::cout << "err @order1=" << std::abs(sfyd-(sfy0 + ~grad1*delta_y)) << std::endl;
+ std::cout << "err @order2=" << std::abs(sfyd-(sfy0 + ~grad2*delta_y)) << std::endl;
+
+ vf.f(y0, yp);
+ Matrix dfdy;
+ df.calcJacobian(y0,yp,dfdy);
+ const Matrix dfdy2 = df.calcJacobian(y0, Differentiator::CentralDifference);
+
+ std::cout << "vf(" << y0 << ")=" << yp << std::endl;
+ std::cout << "order " << df.getDefaultMethod()
+ << " df/dy=" << dfdy;
+ std::cout << "2nd order dfdy: " << dfdy2;
+ Vector yp2(4);
+ vf.f(y0+2*delta_y, yp2);
+ std::cout << "f(y0+dy)=" << yp2 << std::endl;
+ std::cout << "1 f(y0)+(df/dy)dy=" << yp+dfdy*2*delta_y << std::endl;
+ std::cout << "2 f(y0)+(f/dy)dy=" << yp+dfdy2*2*delta_y << std::endl;
+ std::cout << std::setprecision(16);
+ std::cout << "1 err=" << (yp2-(yp+dfdy*2*delta_y)).norm() << std::endl;
+ std::cout << "2 err=" << (yp2-(yp+dfdy2*2*delta_y)).norm() << std::endl;
+ }
+ catch (const std::exception& e) {
+ std::cout << "Differentiator.cpp caught exception:";
+ std::cout << e.what() << std::endl;
+ }
+
+ printf("dsin stats: ndiffs=%d nfail=%d nfcalls=%d\n",
+ dsin.getNumDifferentiations(), dsin.getNumDifferentiationFailures(),
+ dsin.getNumCallsToUserFunction());
+ printf("gradf stats: ndiffs=%d nfail=%d nfcalls=%d\n",
+ gradf.getNumDifferentiations(), gradf.getNumDifferentiationFailures(),
+ gradf.getNumCallsToUserFunction());
+ printf("df stats: ndiffs=%d nfail=%d nfcalls=%d\n",
+ df.getNumDifferentiations(), df.getNumDifferentiationFailures(),
+ df.getNumCallsToUserFunction());
+
+ printf("gf stats: nCalls=%d, nFailures=%d\n",
+ gf.getNumCalls(), gf.getNumFailures());
+ printf("sf stats: nCalls=%d, nFailures=%d\n",
+ sf.getNumCalls(), sf.getNumFailures());
+ printf("vf stats: nCalls=%d, nFailures=%d\n",
+ vf.getNumCalls(), vf.getNumFailures());
+
+ vf.resetAllStatistics();
+ printf("vf after reset: nCalls=%d, nFailures=%d\n",
+ vf.getNumCalls(), vf.getNumFailures());
+
+ return 0;
+}
+
+// Here is our system of equations, representing a pendulum. We'll
+// use the system as-is for calculating a Jacobian, and use its
+// norm for testing gradient calculation.
+static int pendODE(Real t, const Vector& yy, Vector& fy)
+{
+ Real x, y, xd, yd, g, tmp;
+
+ g = 13.7503716373294544;
+
+ x = yy[0];
+ y = yy[1];
+ xd = yy[2];
+ yd = yy[3];
+
+ tmp = xd*xd + yd*yd - g*y;
+
+ fy[0] = xd;
+ fy[1] = yd;
+ fy[2] = -x*tmp;
+ fy[3] = -y*tmp - g;
+
+ return 0;
+}
+
+// Implement the virtual method for a JacobianFunction.
+int MyVectorFunc::f(const Vector& yy, Vector& fy) const
+{
+ return pendODE(getTime(), yy, fy);
+}
+
+// Implement the virtual function for a GradientFunction.
+int MyObjectiveFunc::f(const Vector& yy, Real& fy) const
+{
+ Vector tmp(4);
+ const int res = pendODE(getTime(), yy, tmp);
+ fy = tmp.norm();
+ return res;
+}
diff --git a/SimTKmath/examples/ParameterConstrainedOptimization.cpp b/SimTKmath/examples/ParameterConstrainedOptimization.cpp
new file mode 100644
index 0000000..c76359b
--- /dev/null
+++ b/SimTKmath/examples/ParameterConstrainedOptimization.cpp
@@ -0,0 +1,119 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Parameter Constrained Optimization *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+
+using namespace SimTK;
+
+const static int NUMBER_OF_PARAMETERS = 25;
+
+class ProblemSystem : public OptimizerSystem {
+
+ public:
+
+ ProblemSystem( const int numParameters ) : OptimizerSystem( numParameters ) {}
+
+ int objectiveFunc( const Vector &coefficients, bool new_coefficients, Real& f ) const {
+ const Real *x = &coefficients[0];
+
+ f = .25 *(x[0]-1.0)*(x[0]-1.0);
+ for(int i=1;i<getNumParameters();i++) {
+ f = f + pow(x[i]-x[i-1]*x[i-1], 2.0);
+ }
+
+ f = 4.0* f;
+ return( 0 );
+ }
+
+ int gradientFunc( const Vector &coefficients, bool new_coefficients, Vector &gradient ) const {
+ const Real *x;
+ Real t1,t2;
+
+ x = &coefficients[0];
+
+ t1 = x[1]-(x[0]*x[0]);
+ gradient[0] = 2.0*(x[0]-1.0)-16.0*x[0]*t1;
+ for(int i=1;i<getNumParameters()-1;i++) {
+ t2=t1;
+ t1=x[i+1]-(x[i]*x[i]);
+ gradient[i]=8.0*t2-16.0*x[i]*t1;
+ }
+ gradient[getNumParameters()-1]=8.0*t1;
+
+ return(0);
+
+ }
+
+};
+
+/* adapted from driver1.f of Lbfgsb.2.1.tar.gz */
+int main() {
+ int n = NUMBER_OF_PARAMETERS;
+
+ Vector results(NUMBER_OF_PARAMETERS);
+ Vector lower_bounds(NUMBER_OF_PARAMETERS);
+ Vector upper_bounds(NUMBER_OF_PARAMETERS);
+
+ ProblemSystem sys(NUMBER_OF_PARAMETERS);
+
+
+ /* set initial conditions */
+ for(int i=0;i<n;i++) {
+ results[i] = 3.0;
+ }
+
+ /* set bounds */
+ for(int i=0;i<n;i=i+2) { // even numbered
+ lower_bounds[i] = 1.0;
+ upper_bounds[i] = 100.0;
+ }
+ for(int i=1;i<n;i=i+2) { // odd numbered
+ lower_bounds[i] = -100.0;
+ upper_bounds[i] = 100.0;
+ }
+
+ sys.setParameterLimits( lower_bounds, upper_bounds );
+
+ Real f = NaN;
+ try {
+ Optimizer opt( sys );
+
+ opt.setConvergenceTolerance( .0001 );
+
+ f = opt.optimize( results );
+ }
+
+ catch (SimTK::Exception::Base e) {
+ std::cout << "ParameterConstrainedOptimization.cpp Caught exception :" << std::endl;
+ std::cout << e.what() << std::endl;
+ }
+
+ printf("f = %f params = ",f);
+ for(int i=0; i<NUMBER_OF_PARAMETERS; i++ ) {
+ printf(" %f",results[i]);
+ }
+ printf("\n");
+
+ return 0;
+}
diff --git a/SimTKmath/examples/README.txt b/SimTKmath/examples/README.txt
new file mode 100644
index 0000000..82d3474
--- /dev/null
+++ b/SimTKmath/examples/README.txt
@@ -0,0 +1,15 @@
+This directory contains example programs for using the simmath library.
+
+Differentiator.cpp Example of using numerical differentiator.
+SimpleDifferentiator.cpp Simple example of using numerical differentiator.
+
+ConstrainedOptimization.cpp Optimization problem with nonlinear constriants
+ConstrainedNumericalDiffOptimization.cpp Optimization problem with nonlinear constriants and
+ uses numerical differentiator
+
+ParameterConstrainedOptimization.cpp Optimization problem with limits on the paramters
+
+UnconstrainedOptimization.cpp Optimization problem with no constraints
+UnconstrainedNumericalDiffOptimization.cpp Optimization problem with no constraints and uses
+ numerical differentiator
+
diff --git a/SimTKmath/examples/SimpleDifferentiator.cpp b/SimTKmath/examples/SimpleDifferentiator.cpp
new file mode 100644
index 0000000..c4f641f
--- /dev/null
+++ b/SimTKmath/examples/SimpleDifferentiator.cpp
@@ -0,0 +1,123 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Simple Differentiator *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This is a test program which uses the Differentiator class in various ways.
+ */
+
+//#define SimTK_USE_STATIC_LIBRARIES
+
+#include "SimTKmath.h"
+
+// Just so we can get the version number:
+#include "SimTKlapack.h"
+
+#include <cstdio>
+#include <iostream>
+
+using namespace SimTK;
+
+// This is a system of functions of a particular set of parameters (state).
+// The underlying function wants time also, so we provide that as data in
+// the concrete class. Time should be set prior to calculation of the Jacobian.
+class MyVectorFunc : public Differentiator::JacobianFunction {
+public:
+ MyVectorFunc(int nf, int ny)
+ : Differentiator::JacobianFunction(nf,ny), time(0) { }
+
+ void setTime(Real t) {time=t;}
+ Real getTime() const {return time;}
+
+ // Must provide this pure virtual function.
+ int f(const Vector& y, Vector& fy) const;
+private:
+ Real time;
+};
+
+// This is a single scalar function of a vector of parameters.
+class MyObjectiveFunc : public Differentiator::GradientFunction {
+public:
+ MyObjectiveFunc(int ny)
+ : Differentiator::GradientFunction(ny), time(0) { }
+
+ void setTime(Real t) {time=t;}
+ Real getTime() const {return time;}
+
+ // Must provide this pure virtual function.
+ int f(const Vector& y, Real& fy) const;
+private:
+ Real time;
+};
+
+// This represents a generic scalar function of a scalar parameter,
+// where the actual function has a simple C signature.
+class GenericScalarFunc : public Differentiator::ScalarFunction {
+ typedef Real (*CFunc)(Real);
+public:
+ GenericScalarFunc(CFunc cf)
+ : Differentiator::ScalarFunction(), cp(cf) { }
+
+ // Must provide this pure virtual function.
+ int f(Real x, Real& fx) const {
+ fx = cp(x);
+ return 0;
+ }
+
+ CFunc cp;
+};
+
+class SinOmegaX : public Differentiator::ScalarFunction {
+public:
+ SinOmegaX(Real omega) : w(omega) { }
+
+ // Must provide this virtual function.
+ int f(Real x, Real& fx) const {
+ fx = std::sin(w*x);
+ return 0; // success
+ }
+private:
+ const Real w;
+};
+
+
+int main () {
+ try {
+ const Real w=3;
+ SinOmegaX sinwx(w); // user-written class
+ Differentiator dsinwx(sinwx);
+
+ const Real x = 1.234;
+ Real exact = w*std::cos(w*x);
+ Real approx = dsinwx.calcDerivative(x);
+
+ std::printf("exact =%16.12f\n", exact);
+ std::printf("approx=%16.12f err=%.3e\n",
+ approx, std::abs((approx-exact)/exact));
+
+ return 0;
+ }
+ catch (std::exception& e) {
+ std::printf("FAILED: %s\n", e.what());
+ return 1;
+ }
+}
diff --git a/SimTKmath/examples/SimpleIntegrator.cpp b/SimTKmath/examples/SimpleIntegrator.cpp
new file mode 100644
index 0000000..ac7ffb1
--- /dev/null
+++ b/SimTKmath/examples/SimpleIntegrator.cpp
@@ -0,0 +1,188 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Simple Integrator *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* This example shows how to make use of the Simbody numerical integrators
+to integrate an ordinary ODE. These integrators are designed to integrate the
+complicated DAEs that arise in multibody dynamics so there is more overhead
+in setting up the problem than you would expect for just an ODE. However, you
+can use the framework supplied here for any simple ODE, and you can easily
+extend it to integrate DAEs (ODEs + constraints) if you need to do so. */
+
+// We need only the SimTKmath (and implicitly, SimTKcommon) declarations since
+// we're not doing any multibody dynamics here.
+#include "SimTKmath.h"
+
+#include <cstdio>
+#include <iostream>
+
+using namespace SimTK;
+
+// We'll use this class to communicate the desired period to the
+// function we're integrating. This is an example to show how you can
+// pass any constant stuff you want to your function.
+struct SomeStuff {
+ Real w;
+};
+
+// Fill in your function here. This one is
+// sin( w t )
+Real myFunction(const SomeStuff& stuff, Real t) {
+ const Real w = stuff.w; // get the period
+ return std::sin(w*t);
+}
+
+// For testing, put the analytic result here. Normally you wouldn't know
+// this -- it is the function we're going to calculate by numerical
+// integration. However, in the case of the above myFunction() the integral is:
+// (1-cos(w t))/w
+Real analyticIntegral(const SomeStuff& stuff, Real t) {
+ const Real w = stuff.w; // get the period
+ return (1-std::cos(w*t)) / w;
+}
+
+// This class has the form required by the Simbody integrators; that is, it
+// is a SimTK::System. It calls myFunction() to calculate its state derivative.
+// It is defined after the main program below.
+class MySystem : public System {
+public:
+ explicit MySystem(const SomeStuff&);
+};
+
+
+const Real startTime = 0;
+const Real finalTime = 1;
+const Real reportInterval = 0.1;
+int main () {
+ try {
+ SomeStuff myStuff; // constant data to pass to myFunction()
+ myStuff.w = Pi/2;
+ MySystem sys(myStuff);
+ State initState = sys.realizeTopology();
+ initState.setTime(startTime);
+
+ RungeKuttaMersonIntegrator integ(sys);
+ //RungeKutta3Integrator integ(sys);
+ //RungeKuttaFeldbergIntegrator integ(sys);
+ //ExplicitEulerIntegrator integ(sys);
+ //VerletIntegrator integ(sys);
+ //CPodesIntegrator integ(sys); // implicit integrator
+
+ integ.setAccuracy(1e-6);
+ integ.setFinalTime(finalTime);
+
+ // Use this to get output that is spaced according to the local
+ // complexity of the function being integrated. By default you will
+ // just get output at specified reporting times, with the integrator
+ // taking as many steps as necessary to get there with the specified
+ // accuracy.
+ //integ.setReturnEveryInternalStep(true);
+
+ initState.setTime(startTime);
+ integ.initialize(initState);
+
+ Integrator::SuccessfulStepStatus status;
+ std::printf("%6s %12s\n", "time", "error");
+ // Keep stepping until the integrator returns EndOfSimulation.
+ while (true) {
+ const Real prevT = integ.getTime();
+
+ // To get evenly-spaced reporting intervals, disable
+ // returnEveryInternalStep above and use this line.
+ status = integ.stepTo(prevT + reportInterval);
+
+ // Use this for variable step output, along with setting
+ // returnEveryInternalStep above.
+ // status = integ.stepTo(Infinity);
+
+ //std::printf("step to %g, status=%s\n", integ.getTime(),
+ // Integrator::getSuccessfulStepStatusString(status).c_str());
+
+ if (status == Integrator::EndOfSimulation)
+ break;
+
+ // Note that the integrator maintains its own internal State.
+ const State& state = integ.getState();
+ const Real t = state.getTime();
+ const Real numerical = state.getZ()[0];
+ const Real analytic = analyticIntegral(myStuff, state.getTime());
+
+ std::printf("%6g %12g\n", t, numerical-analytic);
+ }
+
+ std::printf("\nDone. Used %s with %d function calls.\n",
+ integ.getMethodName(), integ.getNumRealizations());
+ std::printf(" %d steps taken out of %d attempted.\n",
+ integ.getNumStepsTaken(), integ.getNumStepsAttempted());
+ }
+ catch (const std::exception& e) {
+ std::printf("FAILED: %s\n", e.what());
+ return 1;
+ }
+
+ return 0;
+}
+
+
+//--------------- SYSTEM CLASS NEEDED FOR INTEGRATOR BOOKKEEPING ---------------
+// Simbody integrators act on System objects to advance their States, so we'll
+// have to make a minimal System and give it a State that corresponds to the
+// function of interest.
+//
+// A System is actually two classes: System::Guts does the work while System
+// provides a pleasant veneer to make usage easier.
+class MySystemGuts : public System::Guts {
+ friend class MySystem;
+
+ MySystemGuts(const SomeStuff& stuff) : stuff(stuff) {}
+
+ // Implement required System::Guts virtuals.
+ MySystemGuts* cloneImpl() const {return new MySystemGuts(*this);}
+
+ // During realizeTopology() we allocate the needed State.
+ int realizeTopologyImpl(State& state) const {
+ // HERE'S WHERE THE IC GETS SET
+ const Vector zInit(1, 0.); // initial value for the one z we want
+ state.allocateZ(SubsystemIndex(0), zInit);
+ return 0;
+ }
+
+ // During realizeAcceleration() we calculate the State derivative.
+ int realizeAccelerationImpl(const State& state) const {
+ const Real t = state.getTime();
+ // HERE'S THE CALL TO YOUR FUNCTION
+ state.updZDot()[0] = myFunction(stuff, t);
+ return 0;
+ }
+
+private:
+ SomeStuff stuff;
+};
+
+// Define MySystem's constructor so that it creates a MySystemGuts object and
+// a default Subsystem.
+MySystem::MySystem(const SomeStuff& stuff) {
+ adoptSystemGuts(new MySystemGuts(stuff));
+ DefaultSystemSubsystem defsub(*this);
+}
+//------------------------ END OF OBSCURE BOOKKEEPING --------------------------
+
diff --git a/SimTKmath/examples/UnconstrainedNumericalDiffOptimization.cpp b/SimTKmath/examples/UnconstrainedNumericalDiffOptimization.cpp
new file mode 100644
index 0000000..a88ec91
--- /dev/null
+++ b/SimTKmath/examples/UnconstrainedNumericalDiffOptimization.cpp
@@ -0,0 +1,78 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Unconstrained Optimization w/Numerical Gradient *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+using std::cout;
+using std::endl;
+
+using namespace SimTK;
+
+/* adapted from itkLBFGSOptimizerTest.cxx */
+
+const static int NUMBER_OF_PARAMETERS = 2;
+
+class ProblemSystem : public OptimizerSystem {
+ public:
+
+ ProblemSystem( int numParameters) : OptimizerSystem( numParameters){}
+
+ int objectiveFunc( const Vector &coefficients, bool new_coefficients, Real& f ) const {
+
+ const Real x = coefficients[0];
+ const Real y = coefficients[1];
+
+ f = 0.5*(3*x*x+4*x*y+6*y*y) - 2*x + 8*y;
+
+ return(0);
+
+ }
+};
+
+int main() {
+ ProblemSystem sys(NUMBER_OF_PARAMETERS);
+
+ Vector results(NUMBER_OF_PARAMETERS);
+
+ Real f = NaN;
+ try {
+ Optimizer opt( sys );
+
+ opt.setConvergenceTolerance( .0001 );
+ opt.useNumericalGradient( true );
+
+ results[0] = 100;
+ results[1] = -100;
+
+ f = opt.optimize( results );
+ }
+ catch(const std::exception& e) {
+ cout << "ParameterConstrainedOptimization.cpp Caught exception :" << endl;
+ cout << e.what() << endl;
+ }
+
+ printf(" Optimal solution: f = %f parameters = %f %f \n",f,results[0],results[1]);
+
+ return 0;
+}
diff --git a/SimTKmath/examples/UnconstrainedOptimization.cpp b/SimTKmath/examples/UnconstrainedOptimization.cpp
new file mode 100644
index 0000000..83bf05e
--- /dev/null
+++ b/SimTKmath/examples/UnconstrainedOptimization.cpp
@@ -0,0 +1,86 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Unconstrained Optimization w/Analytical Gradient *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+
+using namespace SimTK;
+
+/* adapted from itkLBFGSOptimizerTest.cxx */
+
+const static int NUMBER_OF_PARAMETERS = 2;
+
+class ProblemSystem : public OptimizerSystem {
+ public:
+
+ ProblemSystem( int numParameters) : OptimizerSystem( numParameters){}
+
+ int objectiveFunc( const Vector &coefficients, bool new_coefficients, Real& f ) const {
+
+ const Real x = coefficients[0];
+ const Real y = coefficients[1];
+
+ f = 0.5*(3*x*x+4*x*y+6*y*y) - 2*x + 8*y;
+
+ return(0);
+
+ }
+
+ int gradientFunc( const Vector &coefficients, bool new_coefficients, Vector &gradient )const {
+
+ const Real x = coefficients[0];
+ const Real y = coefficients[1];
+
+ gradient[0] = 3*x + 2*y -2;
+ gradient[1] = 2*x + 6*y +8;
+ return(0);
+
+ }
+};
+
+int main() {
+ ProblemSystem sys(NUMBER_OF_PARAMETERS);
+
+ Vector results(NUMBER_OF_PARAMETERS);
+
+ Real f = NaN;
+ try {
+ Optimizer opt( sys );
+
+ opt.setConvergenceTolerance( .0001 );
+
+ results[0] = 100;
+ results[1] = -100;
+
+ f = opt.optimize( results );
+ }
+ catch (SimTK::Exception::Base e) {
+ std::cout << "UnconstrainedOptimization.cpp Caught exception :" << std::endl;
+ std::cout << e.what() << std::endl;
+ }
+
+ printf(" Optimal solution: f = %f parameters = %f %f \n",f,results[0],results[1]);
+
+ return 0;
+}
diff --git a/SimTKmath/examples/UserGuide.cpp b/SimTKmath/examples/UserGuide.cpp
new file mode 100644
index 0000000..2c4d3d5
--- /dev/null
+++ b/SimTKmath/examples/UserGuide.cpp
@@ -0,0 +1,147 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+
+using namespace SimTK;
+
+static int NUMBER_OF_PARAMETERS = 2;
+static int NUMBER_OF_EQUALITY_CONSTRAINTS = 0;
+static int NUMBER_OF_INEQUALITY_CONSTRAINTS = 2;
+
+/*
+ *
+ * Problem statement:
+ *
+ * minimize: (x1+5)^2 + (x2 - 4)^2
+ *
+ * s.t. x1 - x2^2 <= 0 // inequality constraint
+ x1 + x2 >= -2 // inequality constraint
+ *
+ * Starting point:
+ * x = ( 5, 5) will be used for the initial conditions
+ *
+ * Optimal solution:
+ * x = (1.00000000 )
+ *
+ */
+
+class ProblemSystem : public OptimizerSystem {
+public:
+
+
+ int objectiveFunc( const Vector &coefficients, bool new_coefficients, Real& f ) const {
+ const Real *x;
+
+ x = &coefficients[0];
+
+ f = (x[0] - 5.0)*(x[0] - 5.0) + (x[1] - 1.0)*(x[1] - 1.0);
+ return( 0 );
+ }
+
+ int gradientFunc( const Vector &coefficients, bool new_coefficients, Vector &gradient ) const{
+ const Real *x;
+
+ x = &coefficients[0];
+
+ gradient[0] = 2.0*(x[0] - 5.0);
+ gradient[1] = 2.0*(x[1] - 1.0);
+
+ return(0);
+
+ }
+
+ /*
+ ** Method to compute the value of the constraints.
+ ** Equality constraints are first followed by the any inequality constraints
+ */
+ int constraintFunc( const Vector &coefficients, bool new_coefficients, Vector &constraints) const{
+ const Real *x;
+
+ x = &coefficients[0];
+ constraints[0] = x[0] - x[1]*x[1];
+ constraints[1] = x[1] - x[0] + 2.0;
+
+ return(0);
+ }
+
+
+ /*
+ ** Method to compute the jacobian of the constraints.
+ **
+ */
+ int constraintJacobian( const Vector& coefficients, bool new_coefficients, Matrix& jac) const{
+ const Real *x;
+
+ x = &coefficients[0];
+ jac(0,0) = 1.0;
+ jac(0,1) = -2.0*x[1];
+ jac(1,0) = -1.0;
+ jac(1,1) = 1.0;
+
+
+ return(0);
+ }
+
+ ProblemSystem( const int numParams, const int numEqualityConstraints, const int numInequalityConstraints ) :
+ OptimizerSystem( numParams )
+ {
+ setNumEqualityConstraints( numEqualityConstraints );
+ setNumInequalityConstraints( numInequalityConstraints );
+ }
+
+};
+
+
+int main() {
+ /* create the system to be optimized */
+ ProblemSystem sys(NUMBER_OF_PARAMETERS, NUMBER_OF_EQUALITY_CONSTRAINTS, NUMBER_OF_INEQUALITY_CONSTRAINTS);
+
+ Vector results(NUMBER_OF_PARAMETERS);
+
+ /* set initial conditions */
+ results[0] = 5.0;
+ results[1] = 5.0;
+
+ Real f = NaN;
+
+ try {
+ Optimizer opt( sys );
+
+ opt.setConvergenceTolerance( .0000001 );
+
+ /* compute optimization */
+ f = opt.optimize( results );
+ }
+ catch (const std::exception& e) {
+ std::cout << "ConstrainedOptimization.cpp Caught exception:" << std::endl;
+ std::cout << e.what() << std::endl;
+ }
+
+
+ printf("Optimal Solution: f = %f parameters = %f %f \n",f,results[0],results[1]);
+
+ return 0;
+}
diff --git a/SimTKmath/examples/UserGuideLimits.cpp b/SimTKmath/examples/UserGuideLimits.cpp
new file mode 100644
index 0000000..0f13661
--- /dev/null
+++ b/SimTKmath/examples/UserGuideLimits.cpp
@@ -0,0 +1,155 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+
+using namespace SimTK;
+
+static int NUMBER_OF_PARAMETERS = 2;
+static int NUMBER_OF_EQUALITY_CONSTRAINTS = 0;
+static int NUMBER_OF_INEQUALITY_CONSTRAINTS = 2;
+
+/*
+ *
+ * Problem statement:
+ *
+ * minimize: (x1+5)^2 + (x2 - 4)^2
+ *
+ * s.t. x1 - x2^2 <= 0 // inequality constraint
+ x1 + x2 >= -2 // inequality constraint
+ *
+ * Starting point:
+ * x = ( 5, 5) will be used for the initial conditions
+ *
+ * Optimal solution:
+ * x = (1.00000000 )
+ *
+ */
+
+class ProblemSystem : public OptimizerSystem {
+public:
+
+
+ int objectiveFunc( const Vector &coefficients, bool new_coefficients, Real& f ) const {
+ const Real *x;
+
+ x = &coefficients[0];
+
+ f = (x[0] - 5.0)*(x[0] - 5.0) + (x[1] - 1.0)*(x[1] - 1.0);
+ return( 0 );
+ }
+
+ int gradientFunc( const Vector &coefficients, bool new_coefficients, Vector &gradient ) const{
+ const Real *x;
+
+ x = &coefficients[0];
+
+ gradient[0] = 2.0*(x[0] - 5.0);
+ gradient[1] = 2.0*(x[1] - 1.0);
+
+ return(0);
+
+ }
+
+ /*
+ ** Method to compute the value of the constraints.
+ ** Equality constraints are first followed by the any inequality constraints
+ */
+ int constraintFunc( const Vector &coefficients, bool new_coefficients, Vector &constraints) const{
+ const Real *x;
+
+ x = &coefficients[0];
+ constraints[0] = x[0] - x[1]*x[1];
+ constraints[1] = x[1] - x[0] + 2.0;
+
+ return(0);
+ }
+
+
+ /*
+ ** Method to compute the jacobian of the constraints.
+ **
+ */
+ int constraintJacobian( const Vector& coefficients, bool new_coefficients, Matrix& jac) const{
+ const Real *x;
+
+ x = &coefficients[0];
+ jac(0,0) = 1.0;
+ jac(0,1) = -2.0*x[1];
+ jac(1,0) = -1.0;
+ jac(1,1) = 1.0;
+
+
+ return(0);
+ }
+
+ ProblemSystem( const int numParams, const int numEqualityConstraints, const int numInequalityConstraints ) :
+ OptimizerSystem( numParams )
+ {
+ setNumEqualityConstraints( numEqualityConstraints );
+ setNumInequalityConstraints( numInequalityConstraints );
+ }
+
+};
+
+
+int main() {
+ Vector lower_limits(NUMBER_OF_PARAMETERS);
+ Vector upper_limits(NUMBER_OF_PARAMETERS);
+
+ /* create the system to be optimized */
+ ProblemSystem sys(NUMBER_OF_PARAMETERS, NUMBER_OF_EQUALITY_CONSTRAINTS, NUMBER_OF_INEQUALITY_CONSTRAINTS);
+
+ /* set bounds */
+ lower_limits[0] = 1.0;
+ upper_limits[0] = 3.0;
+ lower_limits[1] = -2e19;
+ upper_limits[1] = 2e19;
+
+ sys.setParameterLimits( lower_limits, upper_limits );
+
+ Vector results(NUMBER_OF_PARAMETERS);
+ /* set initial conditions */
+ results[0] = 5.0;
+ results[1] = 5.0;
+
+ Real f = NaN;
+ try {
+ Optimizer opt( sys );
+
+ opt.setConvergenceTolerance( .0000001 );
+
+ /* compute optimization */
+ f = opt.optimize( results );
+ }
+ catch (const std::exception& e) {
+ std::cout << "ConstrainedOptimization.cpp Caught exception:" << std::endl;
+ std::cout << e.what() << std::endl;
+ }
+
+ printf("Optimal Solution: f = %f parameters = %f %f \n",f,results[0],results[1]);
+
+ return 0;
+}
diff --git a/SimTKmath/examples/makefile b/SimTKmath/examples/makefile
new file mode 100644
index 0000000..b712ec3
--- /dev/null
+++ b/SimTKmath/examples/makefile
@@ -0,0 +1,40 @@
+all: UserGuideLimits UserGuide ConstrainedNumericalDiffOptimization ConstrainedOptimization ParameterConstrainedOptimization SimpleDifferentiator UnconstrainedNumericalDiffOptimization UnconstrainedOptimization IPOPTParameterConstrainedOptimization
+
+inc_path = -I../include -I/usr/local/SimTK/core/include -I../include/simmath -I../include/simmath/internal
+lib_path = -L/usr/local/SimTK/core/lib64
+libs = -lSimTKmath_d -lSimTKlapack
+
+
+IPOPTParameterConstrainedOptimization : IPOPTParameterConstrainedOptimization.cpp
+ g++ $(inc_path) $(lib_path) $(libs) IPOPTParameterConstrainedOptimization.cpp -o IPOPTParameterConstrainedOptimization
+UserGuideLimits : UserGuideLimits.cpp
+ g++ $(inc_path) $(lib_path) $(libs) UserGuideLimits.cpp -o UserGuideLimits
+
+UserGuide : UserGuide.cpp
+ g++ $(inc_path) $(lib_path) $(libs) UserGuide.cpp -o UserGuide
+
+ConstrainedNumericalDiffOptimization : ConstrainedNumericalDiffOptimization.cpp
+ g++ $(inc_path) $(lib_path) $(libs) ConstrainedNumericalDiffOptimization.cpp -o ConstrainedNumericalDiffOptimization
+
+ConstrainedOptimization : ConstrainedOptimization.cpp
+ g++ $(inc_path) $(lib_path) $(libs) ConstrainedOptimization.cpp -o ConstrainedOptimization
+
+ParameterConstrainedOptimization : ParameterConstrainedOptimization.cpp
+ g++ $(inc_path) $(lib_path) $(libs) ParameterConstrainedOptimization.cpp -o ParameterConstrainedOptimization
+
+SimpleDifferentiator : SimpleDifferentiator.cpp
+ g++ $(inc_path) $(lib_path) $(libs) SimpleDifferentiator.cpp -o SimpleDifferentiator
+
+UnconstrainedNumericalDiffOptimization : UnconstrainedNumericalDiffOptimization.cpp
+ g++ $(inc_path) $(lib_path) $(libs) UnconstrainedNumericalDiffOptimization.cpp -o UnconstrainedNumericalDiffOptimization
+
+UnconstrainedOptimization : UnconstrainedOptimization.cpp
+ g++ $(inc_path) $(lib_path) $(libs) UnconstrainedOptimization.cpp -o UnconstrainedOptimization
+
+clean:
+ rm ConstrainedNumericalDiffOptimization
+ rm ConstrainedOptimization
+ rm ParameterConstrainedOptimization
+ rm SimpleDifferentiator
+ rm UnconstrainedNumericalDiffOptimization
+ rm UnconstrainedOptimization
diff --git a/SimTKmath/include/SimTKmath.h b/SimTKmath/include/SimTKmath.h
new file mode 100644
index 0000000..e5d4fcb
--- /dev/null
+++ b/SimTKmath/include/SimTKmath.h
@@ -0,0 +1,66 @@
+#ifndef SimTK_SIMMATH_H_
+#define SimTK_SIMMATH_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton, Michael Sherman, Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/internal/Geo.h"
+#include "simmath/internal/Geo_Point.h"
+#include "simmath/internal/Geo_Sphere.h"
+#include "simmath/internal/Geo_LineSeg.h"
+#include "simmath/internal/Geo_Box.h"
+#include "simmath/internal/Geo_Triangle.h"
+#include "simmath/internal/Geo_CubicHermiteCurve.h"
+#include "simmath/internal/Geo_BicubicHermitePatch.h"
+#include "simmath/internal/Geo_CubicBezierCurve.h"
+#include "simmath/internal/Geo_BicubicBezierPatch.h"
+#include "simmath/internal/Spline.h"
+#include "simmath/internal/SplineFitter.h"
+#include "simmath/internal/BicubicSurface.h"
+#include "simmath/internal/Geodesic.h"
+#include "simmath/internal/GeodesicIntegrator.h"
+#include "simmath/internal/ContactGeometry.h"
+#include "simmath/internal/OrientedBoundingBox.h"
+#include "simmath/internal/Contact.h"
+#include "simmath/internal/ContactTracker.h"
+#include "simmath/internal/CollisionDetectionAlgorithm.h"
+
+#include "simmath/LinearAlgebra.h"
+#include "simmath/Differentiator.h"
+#include "simmath/Optimizer.h"
+#include "simmath/MultibodyGraphMaker.h"
+#include "simmath/Integrator.h"
+#include "simmath/TimeStepper.h"
+#include "simmath/CPodesIntegrator.h"
+#include "simmath/RungeKuttaMersonIntegrator.h"
+#include "simmath/RungeKuttaFeldbergIntegrator.h"
+#include "simmath/RungeKutta3Integrator.h"
+#include "simmath/RungeKutta2Integrator.h"
+#include "simmath/ExplicitEulerIntegrator.h"
+#include "simmath/VerletIntegrator.h"
+#include "simmath/SemiExplicitEulerIntegrator.h"
+#include "simmath/SemiExplicitEuler2Integrator.h"
+
+#endif // SimTK_SIMMATH_H_
diff --git a/SimTKmath/include/simmath/Differentiator.h b/SimTKmath/include/simmath/Differentiator.h
new file mode 100644
index 0000000..3579340
--- /dev/null
+++ b/SimTKmath/include/simmath/Differentiator.h
@@ -0,0 +1,243 @@
+#ifndef SimTK_DIFFERENTIATOR_H_
+#define SimTK_DIFFERENTIATOR_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This is the header file that user code should include to pick up the
+ * SimTK Simmath numerical differentiation tools.
+ */
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+
+namespace SimTK {
+
+
+/**
+ * Given a function f(y), where f, y or both can be vectors, calculate the
+ * derivative (gradient, Jacobian) df/dy.
+ *
+ * Calculation is done using numerical differencing, which should be considered
+ * a last resort for cases in which the analytic derivative is unavailable.
+ * (Note that you can obtain an analytic gradient automatically from the source
+ * code for f using automatic differentiation methods like complex step
+ * derivatives, ADIFOR, etc.).
+ *
+ * @par Theory and Implementation
+ *
+ * The SimTK::Differentiator class uses methods adapted from the book
+ * Practical Optimization by Gill, Murray, and Wright (1981), section 8.6
+ * (339ff) and Numerical Recipies in C++ 2nd ed. (2002) section 5.7 (192ff).
+ * Here is a summary:
+ * - We want to differentiate a function f(y) whose estimated relative
+ * accuracy eps is known (e.g. eps=1e-6). (We'll treat y as a scalar here
+ * but for vector y this is done for one element yi at a time.)
+ * - We need to know what perturbation h to use for calculating an estimate
+ * of df/dy that optimally balances roundoff error (h too small) with
+ * truncation error (h too big).
+ * - First guess at h depends on the order of the numerical method: either
+ * forward difference (1st order) or central difference (2nd order). For
+ * 1st order, h0=eps^(1/2); for 2nd order h0=eps^(1/3).
+ * - Now we have to make sure that we can compute y+h reliably. If y is very
+ * large, we can not allow h to be too small. We calculate a scaled
+ * perturbation h1=h0*max(y, 0.1). The 0.1 allows a small y to pull down
+ * the step size <em>a little</em>; but it is dangerous to go much lower
+ * because a very small y might just be zero plus noise.
+ * - Finally, the step size should be exactly representable as a power of 2.
+ * Conceptually, this is just h=(y+h1)-y although one must be careful to
+ * stop the compiler from cleverly "simplifying" this expression.
+ * Differentiator uses a C++ volatile variable for that purpose.
+ *
+ * Then the derivative, gradient element, or Jacobian column is computed
+ * as df/dy=[f(x+h)-f(x)]/h (1st order) or df/dy=[f(x+h)-f(x-h)]/(2h)
+ * (2nd order).
+ */
+class SimTK_SIMMATH_EXPORT Differentiator {
+public:
+ // This are local classes within Differentiator; defined below.
+ class ScalarFunction; // ordinary scalar function of a scalar
+ class GradientFunction; // scalar function of vector
+ class JacobianFunction; // vector function of vector
+ class Function; // abstraction of the above
+
+ // These are the exceptions that can be thrown by this class.
+ class OpNotAllowedForFunctionOfThisShape;
+ class UserFunctionThrewAnException;
+ class UserFunctionReturnedNonzeroStatus;
+ class UnknownMethodSpecified;
+
+
+ enum Method {
+ UnspecifiedMethod=0,
+ ForwardDifference=1,
+ CentralDifference=2
+ };
+ static bool isValidMethod(Method);
+ static const char* getMethodName(Method);
+ static int getMethodOrder(Method);
+
+ virtual ~Differentiator();
+ explicit Differentiator(const Function& f,
+ Method defaultMethod=UnspecifiedMethod);
+
+ // You can change the default method; normally it is ForwardDifference.
+ // If you set it to 'UnspecifiedMethod' it goes back to the original default.
+ Differentiator& setDefaultMethod(Method);
+ Method getDefaultMethod() const;
+
+ // These are the real routines, which are efficient and flexible
+ // but somewhat messy to use.
+ void calcDerivative(Real y0, Real fy0, Real& dfdy,
+ Method=UnspecifiedMethod) const;
+ void calcGradient (const Vector& y0, Real fy0, Vector& gf,
+ Method=UnspecifiedMethod) const;
+ void calcJacobian (const Vector& y0, const Vector& fy0, Matrix& dfdy,
+ Method=UnspecifiedMethod) const;
+
+ // These provide a simpler though less efficient interface. They will
+ // do some heap allocation, and will make an initial unperturbed call
+ // to the user function.
+ Real calcDerivative(Real y0, Method=UnspecifiedMethod) const;
+ Vector calcGradient (const Vector& y0, Method=UnspecifiedMethod) const;
+ Matrix calcJacobian (const Vector& y0, Method=UnspecifiedMethod) const;
+
+ // Statistics (mutable)
+ void resetAllStatistics(); // reset all stats to zero
+ int getNumDifferentiations() const; // total # calls of calcWhatever
+ int getNumDifferentiationFailures() const; // # of those that failed
+ int getNumCallsToUserFunction() const; // total # calls to user function
+
+ // This is a local class.
+ class DifferentiatorRep;
+private:
+ // opaque implementation for binary compatibility
+ DifferentiatorRep* rep;
+};
+
+/**
+ * This abstract class defines a function to be differentiated (repeatedly)
+ * by a Differentiator object. Users should not access this class directly;
+ * instead, use one of the specialized function classes ScalarFunction,
+ * GradientFunction, or JacobianFunction depending on the type of function
+ * you want to differentiate.
+ *
+ * The Differentiator class will assume the function is calculated to
+ * about machine accuracy unless told otherwise. But if f is the result of some
+ * approximate calculation (for example, it came from another Differentiator
+ * approximation, or from numerical integration), we will need to know that in
+ * order to have a reasonable crack at calculating df.
+ */
+class SimTK_SIMMATH_EXPORT Differentiator::Function {
+public:
+ Function& setNumFunctions(int);
+ Function& setNumParameters(int);
+ Function& setEstimatedAccuracy(Real);
+
+ // These values are fixed after construction.
+ int getNumFunctions() const;
+ int getNumParameters() const;
+ Real getEstimatedAccuracy() const; // approx. "roundoff" in f calculation
+
+ // Statistics (mutable)
+ void resetAllStatistics();
+ int getNumCalls() const; // # evaluations of this function since reset
+ int getNumFailures() const; // # of calls which failed
+
+ // This is the declaration of a local class name.
+ class FunctionRep;
+protected:
+ Function();
+ ~Function();
+
+ // opaque implementation for binary compatibility
+ FunctionRep* rep;
+
+private:
+ // suppress copy constructor and copy assignment
+ Function(const Function&);
+ Function& operator=(const Function&);
+
+friend class Differentiator;
+};
+
+/**
+ * Derive a concrete class from this one if you have a scalar function
+ * of a single scalar variable that you want to differentiate.
+ */
+class SimTK_SIMMATH_EXPORT Differentiator::ScalarFunction : public Differentiator::Function {
+public:
+ virtual int f(Real x, Real& fx) const=0;
+
+protected:
+ explicit ScalarFunction(Real acc=-1);
+ virtual ~ScalarFunction() { }
+
+private:
+ // suppress copy constructor and copy assignment
+ ScalarFunction(const Function&);
+ ScalarFunction& operator=(const Function&);
+};
+
+/**
+ * Derive a concrete class from this one if you have a scalar function
+ * of multiple variables that you want to differentiate. This is the typical
+ * form for an optimization objective function, for example.
+ */
+class SimTK_SIMMATH_EXPORT Differentiator::GradientFunction : public Differentiator::Function {
+public:
+ virtual int f(const Vector& y, Real& fy) const=0;
+
+protected:
+ explicit GradientFunction(int ny=-1, Real acc=-1);
+ virtual ~GradientFunction() { }
+
+private:
+ // suppress copy constructor and copy assignment
+ GradientFunction(const GradientFunction&);
+ GradientFunction& operator=(const GradientFunction&);
+};
+
+/**
+ * Derive a concrete class from this one if you have a set of functions
+ * (i.e., a vector-valued function) of multiple variables that you want
+ * to differentiate. This is the typical form for a multibody system, for example.
+ */
+class SimTK_SIMMATH_EXPORT Differentiator::JacobianFunction : public Differentiator::Function {
+public:
+ virtual int f(const Vector& y, Vector& fy) const=0;
+
+protected:
+ explicit JacobianFunction(int nf=-1, int ny=-1, Real acc=-1);
+ virtual ~JacobianFunction() { }
+
+private:
+ // suppress copy constructor and copy assignment
+ JacobianFunction(const JacobianFunction&);
+ JacobianFunction& operator=(const JacobianFunction&);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_DIFFERENTIATOR_H_
diff --git a/SimTKmath/include/simmath/LinearAlgebra.h b/SimTKmath/include/simmath/LinearAlgebra.h
new file mode 100644
index 0000000..4363ad2
--- /dev/null
+++ b/SimTKmath/include/simmath/LinearAlgebra.h
@@ -0,0 +1,237 @@
+#ifndef SimTK_LINEAR_ALGEBRA_H_
+#define SimTK_LINEAR_ALGEBRA_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: Michael Sherman *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This is the header file that user code should include to pick up the
+ * SimTK Simmath linear algebra tools.
+ */
+
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+
+
+namespace SimTK {
+
+// default for reciprocal of the condition number
+// TODO: sherm 080128 I changed this from 0.01 to a more reasonable
+// value but it is still wrong because the default should depend
+// on the matrix size, something like max(m,n)*eps^(7/8) where
+// eps is machine precision for float or double as appropriate.
+static const double DefaultRecpCondition = 1e-12;
+
+/**
+ * Base class for the various matrix factorizations.
+ */
+class SimTK_SIMMATH_EXPORT Factor {
+public:
+
+ Factor() {}
+ /// creates an factorization of a matrix
+ template <class ELT> Factor( Matrix_<ELT> m );
+ /// solves a single right hand side using a factorization
+ template <class ELT> void solve( const Vector_<ELT>& b, Vector_<ELT>& x ) const;
+ /// solves multiple right hand sides using a factorization
+ template <class ELT> void solve( const Matrix_<ELT>& b, Matrix_<ELT>& x ) const;
+
+}; // class Factor
+
+class FactorLURepBase;
+
+/**
+ * Class for performing LU matrix factorizations
+ */
+class SimTK_SIMMATH_EXPORT FactorLU: public Factor {
+ public:
+
+ ~FactorLU();
+
+ FactorLU();
+ FactorLU( const FactorLU& c );
+ FactorLU& operator=(const FactorLU& rhs);
+
+ template <class ELT> FactorLU( const Matrix_<ELT>& m );
+ /// factors a matrix
+ template <class ELT> void factor( const Matrix_<ELT>& m );
+ /// solves a single right hand side
+ template <class ELT> void solve( const Vector_<ELT>& b, Vector_<ELT>& x ) const;
+ /// solves multiple right hand sides
+ template <class ELT> void solve( const Matrix_<ELT>& b, Matrix_<ELT>& x ) const;
+
+ /// returns the lower triangle of an LU factorization
+ template <class ELT> void getL( Matrix_<ELT>& l ) const;
+ /// returns the upper triangle of an LU factorization
+ template <class ELT> void getU( Matrix_<ELT>& u ) const;
+ /// returns the inverse of a matrix using an LU factorization
+ template < class ELT > void inverse( Matrix_<ELT>& m ) const;
+
+ /// returns true if matrix was singular
+ bool isSingular() const;
+ /// returns the first diagonal which was found to be singular
+ int getSingularIndex() const;
+
+
+ protected:
+ class FactorLURepBase *rep;
+
+}; // class FactorLU
+
+
+class FactorQTZRepBase;
+/**
+ * Class to perform a QTZ (linear least squares) factorization
+ */
+class SimTK_SIMMATH_EXPORT FactorQTZ: public Factor {
+ public:
+
+ ~FactorQTZ();
+
+ FactorQTZ();
+ FactorQTZ( const FactorQTZ& c );
+ FactorQTZ& operator=(const FactorQTZ& rhs);
+ /// do QTZ factorization of a matrix
+ template <typename ELT> FactorQTZ( const Matrix_<ELT>& m);
+ /// do QTZ factorization of a matrix for a given reciprocal condition number
+ template <typename ELT> FactorQTZ( const Matrix_<ELT>& m, double rcond );
+ /// do QTZ factorization of a matrix for a given reciprocal condition number
+ template <typename ELT> FactorQTZ( const Matrix_<ELT>& m, float rcond );
+ /// do QTZ factorization of a matrix
+ template <typename ELT> void factor( const Matrix_<ELT>& m);
+ /// do QTZ factorization of a matrix for a given reciprocal condition number
+ template <typename ELT> void factor( const Matrix_<ELT>& m, float rcond );
+ /// do QTZ factorization of a matrix for a given reciprocal condition number
+ template <typename ELT> void factor( const Matrix_<ELT>& m, double rcond );
+ /// solve for a vector x given a right hand side vector b
+ template <typename ELT> void solve( const Vector_<ELT>& b, Vector_<ELT>& x ) const;
+ /// solve for an array of vectors given multiple right hand sides
+ template <typename ELT> void solve( const Matrix_<ELT>& b, Matrix_<ELT>& x ) const;
+
+ template < class ELT > void inverse( Matrix_<ELT>& m ) const;
+
+ /// returns the rank of the matrix
+ int getRank() const;
+ /// returns the actual reciprocal condition number at this rank
+ double getRCondEstimate() const;
+// void setRank(int rank); TBD
+
+ protected:
+ class FactorQTZRepBase *rep;
+}; // class FactorQTZ
+/**
+ * Class to compute Eigen values and Eigen vectors of a matrix
+ */
+class SimTK_SIMMATH_EXPORT Eigen {
+ public:
+
+ ~Eigen();
+
+ Eigen();
+ Eigen( const Eigen& c );
+ Eigen& operator=(const Eigen& rhs);
+
+ /// create a default eigen class
+ template <class ELT> Eigen( const Matrix_<ELT>& m );
+ /// supply matrix which eigen values will be computed for
+ template <class ELT> void factor( const Matrix_<ELT>& m );
+ /// get all the eigen values and eigen vectors of a matrix
+ template <class VAL, class VEC> void getAllEigenValuesAndVectors( Vector_<VAL>& values, Matrix_<VEC>& vectors);
+ /// get all the eigen values of a matrix
+ template <class T> void getAllEigenValues( Vector_<T>& values);
+
+ /// get a few eigen values and eigen vectors of a symmetric matrix which are within a range of indices
+ template <class VAL, class VEC> void getFewEigenValuesAndVectors( Vector_<VAL>& values, Matrix_<VEC>& vectors, int ilow, int ihi);
+ /// get a few eigen vectors of a symmetric matrix which are within a range of indices
+ template <class T> void getFewEigenVectors( Matrix_<T>& vectors, int ilow, int ihi );
+ /// get a few eigen values of a symmetric matrix which are within a range of indices
+ template <class T> void getFewEigenValues( Vector_<T>& values, int ilow, int ihi );
+
+ /// get a few eigen values and eigen vectors of a symmetric matrix which are within a range of eigen values
+ template <class VAL, class VEC> void getFewEigenValuesAndVectors( Vector_<VAL>& values, Matrix_<VEC>& vectors, typename CNT<VAL>::TReal rlow, typename CNT<VAL>::TReal rhi);
+ /// get a few eigen vectors of a symmetric matrix which are within a range of eigen values
+ template <class T> void getFewEigenVectors( Matrix_<T>& vectors, typename CNT<T>::TReal rlow, typename CNT<T>::TReal rhi );
+ /// get a few eigen values of a symmetric matrix which are within a range of eigen values
+ template <class T> void getFewEigenValues( Vector_<T>& values, typename CNT<T>::TReal rlow, typename CNT<T>::TReal rhi );
+
+
+ protected:
+ class EigenRepBase *rep;
+
+}; // class Eigen
+/**
+ * Class to compute a singular value decomposition of a matrix
+ */
+class SimTK_SIMMATH_EXPORT FactorSVD: public Factor {
+ public:
+
+ ~FactorSVD();
+ /// default constructor
+ FactorSVD();
+ /// copy constructor
+ FactorSVD( const FactorSVD& c );
+ /// copy assign
+ FactorSVD& operator=(const FactorSVD& rhs);
+
+ /// constructor
+ template < class ELT > FactorSVD( const Matrix_<ELT>& m );
+ /// singular value decomposition of a matrix using the specified reciprocal of the condition
+ /// number rcond
+ template < class ELT > FactorSVD( const Matrix_<ELT>& m, float rcond );
+ /// singular value decomposition of a matrix using the specified reciprocal of the condition
+ /// number rcond
+ template < class ELT > FactorSVD( const Matrix_<ELT>& m, double rcond );
+ /// supply the matrix to do a singular value decomposition
+ template < class ELT > void factor( const Matrix_<ELT>& m );
+ /// supply the matrix to do a singular value decomposition using the specified
+ /// reciprocal of the condition number rcond
+ template < class ELT > void factor( const Matrix_<ELT>& m, float rcond );
+ /// supply the matrix to do a singular value decomposition using the specified reciprocal of the condition
+ /// reciprocal of the condition number rcond
+ template < class ELT > void factor( const Matrix_<ELT>& m, double rcond );
+
+ /// get the singular values and singular vectors of the matrix
+ template < class T > void getSingularValuesAndVectors( Vector_<typename CNT<T>::TReal>& values,
+ Matrix_<T>& leftVectors, Matrix_<T>& rightVectors );
+ /// get just the singular values of the matrix
+ template < class T > void getSingularValues( Vector_<T>& values);
+
+ /// get rank of the matrix
+ int getRank();
+ /// get inverse of the matrix using singular value decomposition (sometimes called the pseudo inverse)
+ template < class ELT > void inverse( Matrix_<ELT>& m );
+ /// solve for x given a right hand side vector using the singular value decomposition
+ template <class ELT> void solve( const Vector_<ELT>& b, Vector_<ELT>& x );
+ /// solve for a set of x vectors given multiple right hand side vectors
+ /// using the singular value decomposition
+ template <class ELT> void solve( const Matrix_<ELT>& b, Matrix_<ELT>& x );
+
+ protected:
+ class FactorSVDRepBase *rep;
+
+}; // class FactorSVD
+
+} // namespace SimTK
+
+#endif //SimTK_LINEAR_ALGEBRA_H_
diff --git a/SimTKmath/include/simmath/MultibodyGraphMaker.h b/SimTKmath/include/simmath/MultibodyGraphMaker.h
new file mode 100644
index 0000000..64d0078
--- /dev/null
+++ b/SimTKmath/include/simmath/MultibodyGraphMaker.h
@@ -0,0 +1,625 @@
+#ifndef SimTK_SIMMATH_MULTIBODY_GRAPH_MAKER_H_
+#define SimTK_SIMMATH_MULTIBODY_GRAPH_MAKER_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): Multibody Graph Maker *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Kevin He *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the SimTK::MultibodyGraphMaker class for use in constructing a
+spanning-tree-plus-constraints representation of a multibody system from a
+list of its bodies and joints. **/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+
+#include <utility>
+#include <string>
+#include <vector>
+#include <map>
+#include <iosfwd>
+
+namespace SimTK {
+
+//==============================================================================
+// MULTIBODY GRAPH MAKER
+//==============================================================================
+/** Construct a reasonably good spanning-tree-plus-constraints structure for
+modeling a given set of bodies and joints with a generalized coordinate
+multibody system like Simbody. Each body has a unique name; each joint
+connects two distinct bodies with one designated as the "parent" body and
+the other the "child". We will output a spanning tree with Ground as the root,
+containing a mobilizer for every body. Each mobilizer corresponds to one of
+the joints from the input and has an "inboard" (topologically closer to Ground)
+and "outboard" (further from Ground) body, chosen from the parent and child
+bodies of the joint but possibly reordered in which case the mobilizer is
+marked as "reversed". Additional "free" mobilizers are added as needed between
+bodies and Ground so that there is a path from every body in the inboard
+direction all the way to Ground.
+
+<h3>Results</h3>
+The output is
+ -# A sequence of mobilizers (tree joints) in ascending order from
+Ground to a set of terminal bodies. Every input body will be mobilized by one
+of these, and there may also be some additional mobilized "slave" bodies.
+ -# A set of constraints, representing either welds to attach slave bodies
+to their masters, or loop joint constraints that correspond to particular input
+joints.
+ -# A correspondence between input bodies and mobilized bodies, with some
+mobilized bodies designated as slaves to particular master input bodies. That
+is, more than one mobilized body may correspond to the same input body.
+ -# A correspondence between mobilizers and input joints, with some extra
+mobilizers having been added to connect base bodies to Ground.
+ -# Statistics and diagnostics.
+
+Then to build a multibody model
+ -# Run through the mobilizers in order, adding
+one mobilized body to the tree each time, setting the "reversed" flag if
+appropriate (that just affects the interpretation of the generalized
+coordinates). Mass properties and joint information are obtained from the
+original bodies and joints; they are not stored here.
+ -# Run through the list of constraints adding master-slave welds or loop
+joint constraints as indicated.
+
+<h3>Inputs</h3>
+ - ground body: name (so we can recognize it in joints)
+ - bodies: name, mass, mustBeBaseBody flag
+ - joints: name, type, parent body, child body, mustBeLoopJoint flag
+ - joint type: name, # dofs, haveGoodLoopJoint flag
+
+<h3>Loop joints</h3>
+Normally every joint produces a corresponding mobilizer. A joint that would
+form a loop is marked as such, and then its child body is split to form a new
+"slave" body that can be mobilized by the joint. We expect that in the
+constructed multibody model, a master body and its slaves will be reconnected
+via weld constraints. This provides for uniform treatment of all joints, such
+as providing pin joint coordinates that can wrap. However, for some joint types
+that doesn't matter because the multibody system has equally good "loop joint"
+constraints. For example, Simbody's Ball mobilizer uses quaternions for
+orientation and hence has no wrapping; it can be replaced with a Ball constraint
+which removes the 3-dof mobilizer and 6 weld constraint equations, leaving just
+3 translational constraints and indistinguishable behavior. Similarly a loop-
+closing Weld joint can just be replaced by a Weld constraint, and a loop-
+closing Free joint can simply be ignored.
+
+The algorithm normally decides which joints are the loop-breakers, however you
+can specify in the input that certain joints must be loop joints.
+
+<h3>Massless bodies</h3>
+Massless bodies can be very useful in defining compound joints, by composing
+pin and slider joints, for example. This is fine in an internal coordinate code
+as long as no massless (or inertialess) body is a terminal (most outboard) body
+in the spanning tree. Bodies can have a mass provided; if that mass is zero
+the algorithm will try to avoid ending any branch of the spanning tree there.
+If it fails, the resulting spanning tree is no good and an exception will be
+thrown. If no mass is provided we assume it is 1.
+
+<h3>Base bodies</h3>
+A body in the spanning tree that is directly connected to Ground is called a
+"base" body. If its mobilizer is a free joint then it can be given an arbitrary
+pose with respect to Ground and everything outboard of it moves together. This
+is a nice property and you can influence this choice by providing an explicit
+joint to Ground or designating some bodies as base bodies. Note that although
+you will then have a set of generalized coordinates permitting you to place
+these bodies arbitrarily, there is no guarantee that such placement will satisfy
+constraints. If you don't designate base bodies, the algorithm will pick
+them heuristically, which often leads to a good choice. The heuristic is to
+pick the body that has the most children but does not itself appear as a child.
+Failing that, pick the body that has the most children. In case of tie, pick
+the first body among the tied ones.
+
+<h3>Ground body</h3>
+The first body you supply in the input will be interpreted as the Ground body.
+Its name will be used to recognize joints that connect other bodies to Ground.
+Typical names are "ground" or "world".
+
+<h3>Levels</h3>
+Each body in the spanning tree will be assigned a level, an integer that
+measures how many edges must be traversed to get from the body to Ground.
+Ground is at level 0, bodies that have a direct connection to Ground (base
+bodies) are at level 1, bodies whose mobilizer connects them to base bodies
+are at level 2, and so on. Multibody models need to be constructed in order
+of increasing level, so that the inboard body is already in the tree when the
+outboard body is added. We consider the level of a mobilizer to be
+the same as the level of its outboard body. Loop joints are not mobilizers and
+do not have a level.
+**/
+class SimTK_SIMMATH_EXPORT MultibodyGraphMaker {
+public:
+ // Local classes.
+ class Body;
+ class Joint;
+ class JointType;
+ class Mobilizer;
+ class LoopConstraint;
+
+ /** Construct an empty %MultibodyGraphMaker object and set the default
+ names for weld and free joints to "weld" and "free". **/
+ MultibodyGraphMaker();
+
+
+ /** Specify relevant properties of a joint type. Type name must be unique.
+ Weld and free types are predefined and their names are reserved (though you
+ can change the names to be used).
+ @returns Small integer joint type number used for referencing. **/
+ int addJointType(const std::string& name,
+ int numMobilities,
+ bool haveGoodLoopJointAvailable = false,
+ void* userRef = 0);
+
+
+ /** Add a new body (link) to the set of input bodies.
+ @param[in] name
+ A unique string identifying this body. There are no other restrictions
+ on the contents of \a name. Note that the Ground body is predefined and
+ its name is reserved.
+ @param[in] mass
+ The mass here is used as a graph-building hint. If the body is massless,
+ be sure to set mass=0 so that the algorithm will avoid making this a
+ terminal body. The algorithm might also use \a mass to preferentially
+ choose heavier bodies as terminal to improve conditioning.
+ @param[in] mustBeBaseBody
+ If you feel strongly that this body should be able to move freely with
+ respect to Ground, set this flag so that the algorithm will connect it
+ to Ground by a free joint before attempting to build the rest of the
+ tree. Alternatively, provide a joint that connects this body directly
+ to Ground, in which case you should \e not set this flag.
+ @param[in] userRef
+ This is a generic user reference pointer that is kept with the body
+ and can be used by the caller to map back to his or her own data
+ structure containing body information.
+ @see deleteBody() **/
+ void addBody(const std::string& name,
+ double mass,
+ bool mustBeBaseBody,
+ void* userRef = 0);
+
+ /** Delete a body (link) from the set of input bodies. All the joints that
+ reference this body will be deleted too.
+ @param[in] name
+ A unique string identifying this body. There are no other restrictions
+ on the contents of \a name. Note that the Ground body is predefined and
+ its name is reserved.
+ @returns \c true if the body is succesfully deleted, \c false if it
+ didn't exist. **/
+ bool deleteBody(const std::string& name);
+
+ /** Add a new joint to the set of input joints.
+ @param[in] name
+ A string uniquely identifying this joint. There are no other
+ restrictions on the contents of \a name.
+ @param[in] type
+ A string designating the type of this joint, such as "revolute" or
+ "ball". This must be chosen from the set of joint types previously
+ specified.
+ @param[in] parentBodyName
+ This must be the name of a body that was already specified in an earlier
+ addBody() call, or it must be the designated name for the Ground body.
+ If possible, this will be used as the inboard body for the corresponding
+ mobilizer.
+ @param[in] childBodyName
+ This must be the name of a body that was already specified in an earlier
+ addBody() call, or it must be the designated name for the Ground body.
+ It must be distinct from \a parentBodyName. If possible, this will be
+ used as the outboard body for the corresponding mobilizer.
+ @param[in] mustBeLoopJoint
+ If you feel strongly that this joint should be chosen as a loop joint,
+ set this flag. In that case the joint will not appear in the list of
+ joints that are candidates for mobilizers (tree joints). Only after the
+ tree has been successfully built will this joint be added, either using
+ a loop joint equivalent if one is available, or by splitting the child
+ body into master and slave otherwise. In the latter case this joint
+ \e will be made into a mobilizer but a loop weld joint will be added
+ to attach the child slave body to its master.
+ @param[in] userRef
+ This is a generic user reference pointer that is kept with the joint
+ and can be used by the caller to map back to his or her own data
+ structure containing joint information.
+ @see deleteJoint() **/
+ void addJoint(const std::string& name,
+ const std::string& type,
+ const std::string& parentBodyName,
+ const std::string& childBodyName,
+ bool mustBeLoopJoint,
+ void* userRef = 0);
+
+ /** Delete an existing joint from the set of input joints. The bodies(links)
+ referenced by the joint are expected to exist and their references to this
+ joint will be removed as well.
+ @param[in] name
+ A string uniquely identifying this joint. There are no other
+ restrictions on the contents of \a name.
+ @returns \c true if the joint is succesfully deleted, \c false if it
+ didn't exist. **/
+ bool deleteJoint(const std::string& name);
+
+ /** Generate a new multibody graph from the input data. Throws an std::exception
+ if it fails, with a message in the what() string. **/
+ void generateGraph();
+ void clearGraph();
+
+ /** Output a text representation of the multibody graph for debugging. **/
+ void dumpGraph(std::ostream& out) const;
+
+ /** Returns the number of mobilizers (tree joints) in the spanning tree.
+ These are numbered in order of level. The 0th mobilizer has level 0 and
+ is just a placeholder for Ground's immobile connection to the universe.
+ After that come the base mobilizers at level 1, then mobilizers that
+ connect children to base bodies at level 2, and so on. This is also the
+ number of mobilized bodies, including Ground and slave bodies. **/
+ int getNumMobilizers() const {return (int)mobilizers.size();}
+ /** Get a Mobilizer object by its mobilizer number, ordered outwards by
+ topological distance from Ground. **/
+ const Mobilizer& getMobilizer(int mobilizerNum) const
+ { return mobilizers[mobilizerNum]; }
+
+ /** Return the number of loop joint constraints that were used to close
+ loops in the graph topology. These do not include loops that were broken
+ by cutting a body to make a slave body, just those where the joint itself
+ was implemented using a constraint rather than a mobilizer plus a slave.
+ The latter occurs only if were told there is a perfectly good loop joint
+ constraint available; typically that applies for ball joints and not much
+ else. **/
+ int getNumLoopConstraints() const {return (int)constraints.size();}
+ /** Get a loop constraint by its assigned number. These are assigned in
+ an arbitrary order. **/
+ const LoopConstraint& getLoopConstraint(int loopConstraintNum) const
+ { return constraints[loopConstraintNum]; }
+
+ /** Return the number of bodies, including all input bodies, a ground body,
+ and any slave bodies. **/
+ int getNumBodies() const {return (int)bodies.size();}
+ /** Get a Body object by its assigned number. These are assigned first to
+ input bodies, then we add one for Ground, then we add slave bodies created
+ by body splitting after that. **/
+ const Body& getBody(int bodyNum) const {return bodies[bodyNum];}
+ /** Return the body number assigned to the input body with the given name.
+ Returns -1 if the body name is not recognized. You can't look up by name
+ slave bodies that were added by the graph-making algorithm. **/
+ int getBodyNum(const std::string& bodyName) const {
+ std::map<std::string,int>::const_iterator p =
+ bodyName2Num.find(bodyName);
+ return p==bodyName2Num.end() ? -1 : p->second;
+ }
+
+ /** Return the number of joints, including all input joints, and all joints
+ added to connect otherwise disconnected bodies to Ground. Don't confuse
+ these with mobilizers, which are an ordered subset of the joints that are
+ chosen to form a spanning tree connecting all the bodies. **/
+ int getNumJoints() const {return (int)joints.size();}
+ /** Get a Joint object by its assigned number. These are assigned first to
+ input joints, then we add additional joints to Ground as needed. **/
+ const Joint& getJoint(int jointNum) const {return joints[jointNum];}
+ /** Return the joint number assigned to the input joint with the given name.
+ Returns -1 if the joint name is not recognized. You can't look up by name
+ extra joints that were added by the graph-making algorithm. **/
+ int getJointNum(const std::string& jointName) const {
+ std::map<std::string,int>::const_iterator p =
+ jointName2Num.find(jointName);
+ return p==jointName2Num.end() ? -1 : p->second;
+ }
+
+ /** Return the number of registered joint types. **/
+ int getNumJointTypes() const {return (int)jointTypes.size();}
+ /** Get a JointType object by its assigned number. **/
+ const JointType& getJointType(int jointTypeNum) const
+ { return jointTypes[jointTypeNum]; }
+ /** Get the assigned number for a joint type from the type name. **/
+ int getJointTypeNum(const std::string& jointTypeName) const {
+ std::map<std::string,int>::const_iterator p =
+ jointTypeName2Num.find(jointTypeName);
+ return p==jointTypeName2Num.end() ? -1 : p->second;
+ }
+
+ /** Change the name to be used to identify the weld joint type (0 dof) and
+ weld loop constraint type (6 constraints). The default is "weld". Changing
+ this name clears and reinitializes this %MultibodyGraphMaker object. **/
+ void setWeldJointTypeName(const std::string& name)
+ { weldTypeName=name; initialize(); }
+ /** Return the name currently being used to identify the weld joint type
+ and weld loop constraint type. **/
+ const std::string& getWeldJointTypeName() const {return weldTypeName;}
+
+ /** Change the name to be used to identify the free (6 dof) joint type and
+ free (0 constraints) loop constraint type. The default is "free". Changing
+ this name clears and reinitializes this %MultibodyGraphMaker object. **/
+ void setFreeJointTypeName(const std::string& name)
+ { freeTypeName=name; initialize(); }
+ /** Return the name currently being used to identify the free joint type
+ and free loop constraint type. **/
+ const std::string& getFreeJointTypeName() const {return freeTypeName;}
+
+ /** Return the name we recognize as that Ground (or World) body. This is
+ the name that was provided in the first addBody() call. **/
+ const std::string& getGroundBodyName() const;
+private:
+ // Get writable access to bodies and joints.
+ Body& updBody(int bodyNum) {return bodies[bodyNum];}
+ Joint& updJoint(int jointNum) {return joints[jointNum];}
+ Joint& updJoint(const std::string& name) {return joints[jointName2Num[name]];}
+
+ void initialize();
+ int splitBody(int bodyNum);
+ int chooseNewBaseBody() const;
+ void connectBodyToGround(int bodyNum);
+ int addMobilizerForJoint(int jointNum);
+ int findHeaviestUnassignedForwardJoint(int inboardBody) const;
+ int findHeaviestUnassignedReverseJoint(int inboardBody) const;
+ void growTree();
+ void breakLoops();
+ bool bodiesAreConnected(int b1, int b2) const;
+
+ // Clear everything except for default names.
+ void clear() {
+ bodies.clear(); joints.clear(); jointTypes.clear();
+ bodyName2Num.clear(); jointName2Num.clear(); jointTypeName2Num.clear();
+ mobilizers.clear(); constraints.clear();
+ }
+
+ std::string weldTypeName, freeTypeName;
+ std::vector<Body> bodies; // ground + input bodies + slaves
+ std::vector<Joint> joints; // input joints + added joints
+ std::vector<JointType> jointTypes;
+ std::map<std::string,int> bodyName2Num;
+ std::map<std::string,int> jointName2Num;
+ std::map<std::string,int> jointTypeName2Num;
+
+ // Calculated by generateGraph()
+ std::vector<Mobilizer> mobilizers; // mobilized bodies
+ std::vector<LoopConstraint> constraints;
+};
+
+//------------------------------------------------------------------------------
+// MULTIBODY GRAPH MAKER :: BODY
+//------------------------------------------------------------------------------
+/** Local class that collects information about bodies. **/
+class MultibodyGraphMaker::Body {
+public:
+ explicit Body(const std::string& name,
+ double mass,
+ bool mustBeBaseBody,
+ void* userRef)
+ : name(name), mass(mass), mustBeBaseBody(mustBeBaseBody),
+ userRef(userRef), level(-1), mobilizer(-1), master(-1) {}
+
+ void forgetGraph(MultibodyGraphMaker& graph);
+ int getNumFragments() const {return 1 + getNumSlaves();}
+ int getNumSlaves() const {return (int)slaves.size();}
+ int getNumJoints() const
+ { return int(jointsAsChild.size() + jointsAsParent.size()); }
+ bool isSlave() const {return master >= 0;}
+ bool isMaster() const {return getNumSlaves()>0;}
+ bool isInTree() const {return level>=0;}
+
+ // Inputs
+ std::string name;
+ double mass;
+ bool mustBeBaseBody;
+ void* userRef;
+
+ // How this body appears in joints (input and added).
+ std::vector<int> jointsAsChild; // where this body is the child
+ std::vector<int> jointsAsParent; // where this body is the parent
+
+ // Disposition of this body in the spanning tree.
+
+ int level; // Ground=0, connected to Ground=1, contact to that=2, etc.
+ int mobilizer; // the unique mobilizer where this is the outboard body
+
+ int master; // >=0 if this is a slave
+ std::vector<int> slaves; // slave links, if this is a master
+};
+
+//------------------------------------------------------------------------------
+// MULTIBODY GRAPH MAKER :: JOINT
+//------------------------------------------------------------------------------
+/** Local class that collects information about joints. **/
+class MultibodyGraphMaker::Joint {
+public:
+ Joint(const std::string& name, int jointTypeNum,
+ int parentBodyNum, int childBodyNum,
+ bool mustBeLoopJoint, void* userRef)
+ : name(name), jointTypeNum(jointTypeNum),
+ parentBodyNum(parentBodyNum), childBodyNum(childBodyNum),
+ mustBeLoopJoint(mustBeLoopJoint), userRef(userRef),
+ isAddedBaseJoint(false), mobilizer(-1), loopConstraint(-1) {}
+
+ /** Return true if the joint is deleted as a result of restoring it
+ to the state prior to generateGraph(). **/
+ bool forgetGraph(MultibodyGraphMaker& graph);
+
+ // Only one of these will be true -- we don't consider it a LoopConstraint
+ // if we split a body and weld it back.
+ bool hasMobilizer() const {return mobilizer>=0;}
+ bool hasLoopConstraint() const {return loopConstraint>=0;}
+
+ // Inputs
+ std::string name;
+ bool mustBeLoopJoint;
+ void* userRef;
+
+ // Mapping of strings to indices for fast lookup.
+ int parentBodyNum, childBodyNum;
+ int jointTypeNum;
+
+ bool isAddedBaseJoint; // true if this wasn't one of the input joints
+
+ // Disposition of this joint in the multibody graph.
+ int mobilizer; // if this joint is part of the spanning tree, else -1
+ int loopConstraint; // if this joint used a loop constraint, else -1
+};
+
+//------------------------------------------------------------------------------
+// MULTIBODY GRAPH MAKER :: JOINT TYPE
+//------------------------------------------------------------------------------
+/** Local class that defines the properties of a known joint type. **/
+class MultibodyGraphMaker::JointType {
+public:
+ JointType(const std::string& name, int numMobilities,
+ bool haveGoodLoopJointAvailable, void* userRef)
+ : name(name), numMobilities(numMobilities),
+ haveGoodLoopJointAvailable(haveGoodLoopJointAvailable),
+ userRef(userRef) {}
+ std::string name;
+ int numMobilities;
+ bool haveGoodLoopJointAvailable;
+ void* userRef;
+};
+
+//------------------------------------------------------------------------------
+// MULTIBODY GRAPH MAKER :: MOBILIZER
+//------------------------------------------------------------------------------
+/** Local class that represents one of the mobilizers (tree joints) in the
+generated spanning tree. There is always a corresponding joint, although that
+joint might be a ground-to-body free joint that was added automatically. **/
+class MultibodyGraphMaker::Mobilizer {
+public:
+ Mobilizer()
+ : joint(-1), level(-1), inboardBody(-1), outboardBody(-1),
+ isReversed(false), mgm(0) {}
+ Mobilizer(int jointNum, int level, int inboardBodyNum, int outboardBodyNum,
+ bool isReversed, MultibodyGraphMaker* graphMaker)
+ : joint(jointNum), level(level), inboardBody(inboardBodyNum),
+ outboardBody(outboardBodyNum), isReversed(isReversed),
+ mgm(graphMaker) {}
+
+ /** Return true if this mobilizer does not represent one of the input
+ joints, but is instead a joint we added connecting a base body to ground.
+ If this returns true then there will be no user reference pointer returned
+ from getJointRef(). Also, the inboard body is always ground. When you
+ create this mobilizer, the joint frames should be identity, that is, the
+ joint should connect the ground frame to the outboard body frame. **/
+ bool isAddedBaseMobilizer() const
+ { return mgm->getJoint(joint).isAddedBaseJoint; }
+ /** Get the user reference pointer for the joint associated with this
+ mobilizer, if there is such a joint. If this mobilizer doesn't correspond
+ to one of the input joints then a null pointer is returned. **/
+ void* getJointRef() const
+ { return mgm->getJoint(joint).userRef; }
+ /** Get the user reference pointer for the inboard body of this mobilizer.
+ The inboard body is always one of the input bodies so this will not be
+ returned null unless no reference pointer was supplied in the addBody()
+ call that defined this body. **/
+ void* getInboardBodyRef() const
+ { return mgm->getBody(inboardBody).userRef; }
+ /** Get the user reference pointer for the outboard body of this mobilizer.
+ The outboard body may be one of the input bodies, but could also be a
+ slave body, in which case a null pointer will be returned. You can use
+ getOutboardMasterBodyRef() instead to ensure that you will get a reference
+ to one of the input bodies. **/
+ void* getOutboardBodyRef() const
+ { return mgm->getBody(outboardBody).userRef; }
+ /** Get the user reference pointer for the outboard body of this mobilizer,
+ if it is one of the input bodes, or to the master body for the outboard
+ body if the outboard body is a slave body. This ensures that you will get a
+ reference to one of the input bodies. **/
+ void* getOutboardMasterBodyRef() const
+ { return mgm->getBody(getOutboardMasterBodyNum()).userRef; }
+ /** Get the joint type name of the joint that this mobilizer represents. **/
+ const std::string& getJointTypeName() const
+ { return mgm->getJointType(mgm->getJoint(joint).jointTypeNum).name; }
+ /** Get the reference pointer (if any) that was provided when this
+ mobilizer's joint type was defined in an addJointType() call. **/
+ void* getJointTypeRef() const
+ { return mgm->getJointType(mgm->getJoint(joint).jointTypeNum).userRef; }
+ /** Return true if the outboard body of this mobilizer is a slave we
+ created in order to cut a loop, rather than one of the input bodies. **/
+ bool isSlaveMobilizer() const
+ { return mgm->getBody(outboardBody).isSlave(); }
+ /** Return the number of fragments into which we chopped the outboard body
+ of this mobilizer. There is one fragment for the master body plus however
+ many slaves of that body were created. Thus you should divide the master
+ body's mass by this number to obtain the mass to be assigned to each of
+ the body fragments. **/
+ int getNumFragments() const
+ { return mgm->getBody(getOutboardMasterBodyNum()).getNumFragments(); }
+ /** Return true if this mobilizer represents one of the input joints but
+ the sense of inboard->outboard is reversed from the parent->child sense
+ defined in the input joint. In that case you should use a reverse joint
+ when you build the system. **/
+ bool isReversedFromJoint() const {return isReversed;}
+
+private:
+friend class MultibodyGraphMaker;
+
+ int getOutboardMasterBodyNum() const
+ { const Body& outb = mgm->getBody(outboardBody);
+ return outb.isSlave() ? outb.master : outboardBody; }
+
+ int joint; ///< corresponding joint (not necessarily from input)
+ int level; ///< level of the outboard body; distance from ground
+ int inboardBody; ///< might be ground
+ int outboardBody; ///< might be a slave body; can't be ground
+ bool isReversed; ///< if so, inboard=child, outboard=parent
+
+ MultibodyGraphMaker* mgm; // just a reference to container
+};
+
+
+//------------------------------------------------------------------------------
+// MULTIBODY GRAPH MAKER :: LOOP CONSTRAINT
+//------------------------------------------------------------------------------
+/** Local class that represents one of the constraints that were added to close
+topological loops that were cut to form the spanning tree. **/
+class MultibodyGraphMaker::LoopConstraint {
+public:
+ LoopConstraint() : joint(-1), parentBody(-1), childBody(-1), mgm(0) {}
+ LoopConstraint(const std::string& type, int jointNum,
+ int parentBodyNum, int childBodyNum,
+ MultibodyGraphMaker* graphMaker)
+ : type(type), joint(jointNum),
+ parentBody(parentBodyNum), childBody(childBodyNum),
+ mgm(graphMaker) {}
+
+ /** Get the user reference pointer for the joint associated with this
+ loop constraint. **/
+ void* getJointRef() const
+ { return mgm->getJoint(joint).userRef; }
+ /** Get the loop constraint type name of the constraint that should be
+ used here. **/
+ const std::string& getJointTypeName() const
+ { return type; }
+ /** Get the user reference pointer for the parent body defined by the
+ joint associated with this loop constraint. **/
+ void* getParentBodyRef() const
+ { return mgm->getBody(parentBody).userRef; }
+ /** Get the user reference pointer for the child body defined by the
+ joint associated with this loop constraint. **/
+ void* getChildBodyRef() const
+ { return mgm->getBody(childBody).userRef; }
+
+private:
+friend class MultibodyGraphMaker;
+
+ std::string type; // e.g., ball
+ int joint; // always one of the input joints
+ int parentBody; // parent from the joint
+ int childBody; // child from the joint
+
+ MultibodyGraphMaker* mgm; // just a reference to container
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_MULTIBODY_GRAPH_MAKER_H_
+
diff --git a/SimTKmath/include/simmath/Optimizer.h b/SimTKmath/include/simmath/Optimizer.h
new file mode 100644
index 0000000..d1f96f2
--- /dev/null
+++ b/SimTKmath/include/simmath/Optimizer.h
@@ -0,0 +1,366 @@
+#ifndef SimTK_SIMMATH_OPTIMIZER_H_
+#define SimTK_SIMMATH_OPTIMIZER_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-13 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: Michael Sherman *
+ * *
+ * 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 "simmath/internal/common.h"
+#include "simmath/Differentiator.h"
+
+namespace SimTK {
+
+enum OptimizerAlgorithm {
+ BestAvailable = 0, // Simmath will select best Optimizer based on problem type
+ InteriorPoint = 1, // IPOPT interior point optimizer
+ LBFGS = 2, // LBFGS optimizer
+ LBFGSB = 3, // LBFGS optimizer with simple bounds
+ CFSQP = 4 // CFSQP sequential quadratic programming optimizer (requires external library)
+};
+
+/**
+ * Abstract class which defines an objective/cost function which is optimized by
+ * and Optimizer object. The OptimizerSystem also defines any constraints which
+ * must be satisfied.
+ */
+class SimTK_SIMMATH_EXPORT OptimizerSystem {
+public:
+ OptimizerSystem() : numParameters(0),
+ numEqualityConstraints(0),
+ numInequalityConstraints(0),
+ numLinearEqualityConstraints(0),
+ numLinearInequalityConstraints(0),
+ useLimits( false ),
+ lowerLimits(0),
+ upperLimits(0) {
+ }
+
+ explicit OptimizerSystem(int nParameters ) {
+ new (this) OptimizerSystem(); // call the above constructor
+ setNumParameters(nParameters);
+ }
+
+ virtual ~OptimizerSystem() {
+ if( useLimits ) {
+ delete lowerLimits;
+ delete upperLimits;
+ }
+ }
+
+ /// Objective/cost function which is to be optimized; return 0 when successful.
+ /// This method must be supplied by concrete class.
+ virtual int objectiveFunc ( const Vector& parameters,
+ bool new_parameters, Real& f ) const {
+ SimTK_THROW2(SimTK::Exception::UnimplementedVirtualMethod , "OptimizerSystem", "objectiveFunc" );
+ return -1; }
+
+ /// Computes the gradient of the objective function; return 0 when successful.
+ /// This method does not have to be supplied if a numerical gradient is used.
+ virtual int gradientFunc ( const Vector ¶meters,
+ bool new_parameters, Vector &gradient ) const {
+ SimTK_THROW2(SimTK::Exception::UnimplementedVirtualMethod , "OptimizerSystem", "gradientFunc" );
+ return -1; }
+ /// Computes the value of the constraints; return 0 when successful.
+ /// This method must be supplied if the objective function has constraints.
+ virtual int constraintFunc ( const Vector & parameters,
+ bool new_parameters, Vector & constraints ) const {
+ SimTK_THROW2(SimTK::Exception::UnimplementedVirtualMethod , "OptimizerSystem", "constraintFunc" );
+ return -1; }
+ /// Computes Jacobian of the constraints; return 0 when successful.
+ /// This method does not have to be supplied if a numerical jacobian is used.
+ virtual int constraintJacobian ( const Vector& parameters,
+ bool new_parameters, Matrix& jac ) const {
+ SimTK_THROW2(SimTK::Exception::UnimplementedVirtualMethod , "OptimizerSystem", "constraintJacobian" );
+ return -1; }
+ /// Computes Hessian of the objective function; return 0 when successful.
+ /// This method does not have to be supplied if limited memory is used.
+ virtual int hessian ( const Vector ¶meters,
+ bool new_parameters, Vector &gradient) const {
+ SimTK_THROW2(SimTK::Exception::UnimplementedVirtualMethod , "OptimizerSystem", "hessian" );
+ return -1; }
+
+ /// Sets the number of parameters in the objective function.
+ void setNumParameters( const int nParameters ) {
+ if( nParameters < 1 ) {
+ const char* where = " OptimizerSystem Constructor";
+ const char* szName = "number of parameters";
+ SimTK_THROW5(SimTK::Exception::ValueOutOfRange, szName, 1, nParameters, INT_MAX, where);
+ } else {
+ numParameters = nParameters;
+ }
+ }
+ /// Sets the number of equality constraints.
+ void setNumEqualityConstraints( const int n ) {
+ if( n < 0 ) {
+ const char* where = " OptimizerSystem setNumEqualityConstraints";
+ const char* szName = "number of equality constraints";
+ SimTK_THROW3(SimTK::Exception::SizeWasNegative, szName, n, where);
+ } else {
+ numEqualityConstraints = n;
+ }
+ }
+ /// Sets the number of inequality constraints.
+ void setNumInequalityConstraints( const int n ) {
+ if( n < 0 ) {
+ const char* where = " OptimizerSystem setNumInequalityConstraints";
+ const char* szName = "number of inequality constraints";
+ SimTK_THROW3(SimTK::Exception::SizeWasNegative, szName, n, where);
+ } else {
+ numInequalityConstraints = n;
+ }
+ }
+ /// Sets the number of lineaer equality constraints.
+ void setNumLinearEqualityConstraints( const int n ) {
+ if( n < 0 || n > numEqualityConstraints ) {
+ const char* where = " OptimizerSystem setNumLinearEqualityConstraints";
+ const char* szName = "number of linear equality constraints";
+ SimTK_THROW4(SimTK::Exception::SizeOutOfRange, szName, n, numEqualityConstraints, where);
+ } else {
+ numLinearEqualityConstraints = n;
+ }
+ }
+ /// Sets the number of lineaer inequality constraints.
+ void setNumLinearInequalityConstraints( const int n ) {
+ if( n < 0 || n > numInequalityConstraints ) {
+ const char* where = " OptimizerSystem setNumLinearInequalityConstraints";
+ const char* szName = "number of linear inequality constraints";
+ SimTK_THROW4(SimTK::Exception::SizeOutOfRange, szName, n, numInequalityConstraints, where);
+ } else {
+ numLinearInequalityConstraints = n;
+ }
+ }
+ /// Set the upper and lower bounds on the paramters.
+ void setParameterLimits( const Vector& lower, const Vector& upper ) {
+ if( upper.size() != numParameters && upper.size() != 0) {
+ const char* where = " OptimizerSystem setParamtersLimits";
+ const char* szName = "upper limits length";
+ SimTK_THROW5(Exception::IncorrectArrayLength, szName, upper.size(), "numParameters", numParameters, where);
+ }
+ if( lower.size() != numParameters && lower.size() != 0 ) {
+ const char* where = " OptimizerSystem setParamtersLimits";
+ const char* szName = "lower limits length";
+ SimTK_THROW5(Exception::IncorrectArrayLength, szName, lower.size(), "numParameters", numParameters, where);
+ }
+
+ // set the upper and lower limits
+ if( useLimits ) {
+ delete lowerLimits;
+ delete upperLimits;
+ }
+
+ if( upper.size() == 0 ) {
+ useLimits = false;
+ } else {
+ lowerLimits = new Vector( lower );
+ upperLimits = new Vector( upper );
+ useLimits = true;
+ }
+ }
+
+ /// Returns the number of parameters, that is, the number of variables that
+ /// the Optimizer may adjust while searching for a solution.
+ int getNumParameters() const {return numParameters;}
+ /// Returns the total number of constraints.
+ int getNumConstraints() const {return numEqualityConstraints+numInequalityConstraints;}
+ /// Returns the number of equality constraints.
+ int getNumEqualityConstraints() const {return numEqualityConstraints;}
+ /// Returns the number of inequality constraints.
+ int getNumInequalityConstraints() const {return numInequalityConstraints;}
+ /// Returns the number of linear equality constraints.
+ int getNumLinearEqualityConstraints() const {return numLinearEqualityConstraints;}
+ /// Returns the number of nonlinear equality constraints.
+ int getNumNonlinearEqualityConstraints() const {return numEqualityConstraints-numLinearEqualityConstraints;}
+ /// Returns the number of linear inequality constraints.
+ int getNumLinearInequalityConstraints() const {return numLinearInequalityConstraints;}
+ /// Returns the number of linear inequality constraints.
+ int getNumNonlinearInequalityConstraints() const {return numInequalityConstraints-numLinearInequalityConstraints;}
+
+ /// Returns true if there are limits on the parameters.
+ bool getHasLimits() const { return useLimits; }
+ /// Returns the limits on the allowed values of each parameter, as
+ /// an array of lower bounds and an array of upper bounds, with
+ /// assumed lengths matching the number of parameters.
+ void getParameterLimits( Real **lower, Real **upper ) const {
+ *lower = &(*lowerLimits)[0];
+ *upper = &(*upperLimits)[0];
+ }
+
+private:
+ int numParameters;
+ int numEqualityConstraints;
+ int numInequalityConstraints;
+ int numLinearEqualityConstraints;
+ int numLinearInequalityConstraints;
+ bool useLimits;
+ Vector* lowerLimits;
+ Vector* upperLimits;
+
+}; // class OptimizerSystem
+
+/**
+ * API for SimTK Simmath's optimizers.
+ * An optimizer finds a local minimum to an objective function. The
+ * optimizer can be constrained to search for a minimum within a feasible
+ * region. The feasible region can be defined by setting limits on the
+ * parameters of the objective function and/or supplying constraint
+ * functions that must be satisfied.
+ * The optimizer starts searching for a minimum beginning at a user supplied
+ * initial value for the set of parameters.
+ *
+ * The objective function and constraints are specified by supplying the
+ * Optimizer with a concrete implemenation of an OptimizerSystem class.
+ * The OptimizerSystem can be passed to the Optimizer either through the
+ * Optimizer constructor or by calling the setOptimizerSystem method.
+ * The Optimizer class will select the best optimization algorithm to solve the
+ * problem based on the constraints supplied by the OptimizerSystem.
+ * A user can also override the optimization algorithm selected by the
+ * Optimizer by specifying the optimization algorithm.
+ *
+ */
+class SimTK_SIMMATH_EXPORT Optimizer {
+public:
+ Optimizer();
+ Optimizer( const OptimizerSystem& sys);
+ Optimizer( const OptimizerSystem& sys, OptimizerAlgorithm algorithm);
+ ~Optimizer();
+
+ static bool isAlgorithmAvailable(OptimizerAlgorithm algorithm);
+
+ /// Sets the relative accuracy used determine if the problem has converged.
+ void setConvergenceTolerance(Real accuracy );
+ /// Sets the absolute tolerance used to determine whether constraint
+ /// violation is acceptable.
+ void setConstraintTolerance(Real tolerance);
+
+
+ /// Set the maximum number of iterations allowed of the optimization
+ /// method's outer / stepping loop. Most optimizers also have an inner loop
+ /// ("line search") which is / also iterative but is not affected by this
+ /// setting. Inner loop convergence is / typically prescribed by theory, and
+ /// failure there is often an indication of / an ill-formed problem.
+ void setMaxIterations( int iter );
+ /// Set the maximum number of previous hessians used in a limitied memory
+ /// hessian approximation.
+ void setLimitedMemoryHistory( int history );
+ /// Set the level of debugging info displayed.
+ void setDiagnosticsLevel( int level );
+
+ void setOptimizerSystem( const OptimizerSystem& sys );
+ void setOptimizerSystem( const OptimizerSystem& sys, OptimizerAlgorithm algorithm );
+
+ /// Set the value of an advanced option specified by a string.
+ bool setAdvancedStrOption( const char *option, const char *value );
+ /// Set the value of an advanced option specified by a real value.
+ bool setAdvancedRealOption( const char *option, const Real value );
+ /// Set the value of an advanced option specified by an integer value.
+ bool setAdvancedIntOption( const char *option, const int value );
+ /// Set the value of an advanced option specified by an boolean value.
+ bool setAdvancedBoolOption( const char *option, const bool value );
+
+
+ /// Set which numerical differentiation algorithm is to be used for the next
+ /// useNumericalGradient() or useNumericalJacobian() call. Choices are
+ /// Differentiator::ForwardDifference (first order) or
+ /// Differentiator::CentralDifference (second order) with central the
+ /// default.
+ /// @warning This has no effect if you have already called
+ /// useNumericalGradient() or useNumericalJacobian(). Those must be called
+ /// \e after setDifferentiatorMethod().
+ /// @see SimTK::Differentiator
+ void setDifferentiatorMethod(Differentiator::Method method);
+ /// Return the differentiation method last supplied in a call to
+ /// setDifferentiatorMethod(), \e not necessarily the method currently
+ /// in use. See setDifferentiatorMethod() for more information.
+ /// @see SimTK::Differentiator
+ Differentiator::Method getDifferentiatorMethod() const;
+
+ /// Return the algorithm used for the optimization. You may be interested
+ /// in this value if you didn't specify an algorithm, or specified for
+ /// Simbody to choose the BestAvailable algorithm. This method won't return
+ /// BestAvailable, even if it's the 'algorithm' that you chose.
+ OptimizerAlgorithm getAlgorithm() const;
+
+ /// Enable numerical calculation of gradient, with optional estimation of
+ /// the accuracy to which the objective function is calculated. For example,
+ /// if you are calculate about 6 significant digits, supply the estimated
+ /// accuracy as 1e-6. Providing the estimated accuracy improves the quality
+ /// of the calculated derivative. If no accuracy is provided we'll assume
+ /// the objective is calculated to near machine precision. The method used
+ /// for calculating the derivative will be whatever was \e previously
+ /// supplied in a call to setDifferentiatorMethod(), or the default which
+ /// is to use central differencing (two function evaluations per
+ /// gradient entry). See SimTK::Differentiator for more information.
+ /// @see setDifferentiatorMethod(), SimTK::Differentiator
+ void useNumericalGradient(bool flag,
+ Real estimatedAccuracyOfObjective = SignificantReal);
+ /// Enable numerical calculation of the constraint Jacobian, with optional
+ /// estimation of the accuracy to which the constraint functions are
+ /// calculated. For example, if you are calculating about 6 significant
+ /// digits, supply the estimated accuracy as 1e-6. Providing the estimated
+ /// accuracy improves the quality of the calculated derivative. If no
+ /// accuracy is provided we'll assume the constraints are calculated to near
+ /// machine precision. The method used for calculating the derivative will
+ /// be whatever was \e previously supplied in a call to
+ /// setDifferentiatorMethod(), or the default which is to use central
+ /// differencing (two function evaluations per Jacobian column. See
+ /// SimTK::Differentiator for more information.
+ /// @see setDifferentiatorMethod(), SimTK::Differentiator
+ void useNumericalJacobian(bool flag,
+ Real estimatedAccuracyOfConstraints = SignificantReal);
+
+ /// Compute optimization.
+ Real optimize(Vector&);
+
+ /// Return a reference to the OptimizerSystem currently associated with this Optimizer.
+ const OptimizerSystem& getOptimizerSystem() const;
+
+ /// Indicate whether the Optimizer is currently set to use a numerical gradient.
+ bool isUsingNumericalGradient() const;
+ /// Indicate whether the Optimizer is currently set to use a numerical Jacobian.
+ bool isUsingNumericalJacobian() const;
+ /// Return the estimated accuracy last specified in useNumericalGradient().
+ Real getEstimatedAccuracyOfObjective() const;
+ /// Return the estimated accuracy last specified in useNumericalJacobian().
+ Real getEstimatedAccuracyOfConstraints() const;
+
+ // This is a local class.
+ class OptimizerRep;
+private:
+ Optimizer( const Optimizer& c );
+ Optimizer& operator=(const Optimizer& rhs);
+
+ OptimizerRep* constructOptimizerRep(const OptimizerSystem&, OptimizerAlgorithm);
+ const OptimizerRep& getRep() const {assert(rep); return *rep;}
+ OptimizerRep& updRep() {assert(rep); return *rep;}
+
+ // Hidden implementation to preserve binary compatibility.
+ OptimizerRep* rep;
+
+friend class OptimizerRep;
+}; // class Optimizer
+
+} // namespace SimTK
+
+#endif //SimTK_SIMMATH_OPTIMIZER_H_
+
diff --git a/SimTKmath/include/simmath/internal/OptimizerRep.h b/SimTKmath/include/simmath/internal/OptimizerRep.h
new file mode 100644
index 0000000..d913f04
--- /dev/null
+++ b/SimTKmath/include/simmath/internal/OptimizerRep.h
@@ -0,0 +1,216 @@
+#ifndef SimTK_SIMMATH_OPTIMIZER_REP_H_
+#define SimTK_SIMMATH_OPTIMIZER_REP_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-13 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: Michael Sherman *
+ * *
+ * 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 "simmath/Optimizer.h"
+#include "simmath/Differentiator.h"
+#include <map>
+
+namespace SimTK {
+
+
+/* class for Diff jacobian */
+class SysObjectiveFunc : public Differentiator::GradientFunction {
+public:
+ SysObjectiveFunc(int ny, const OptimizerSystem* sysPtr )
+ : Differentiator::GradientFunction(ny) { sysp = sysPtr; }
+
+ // Must provide this pure virtual function.
+ int f(const Vector& y, Real& fy) const {
+ return(sysp->objectiveFunc(y, true, fy)); // class user's objectiveFunc
+ }
+ const OptimizerSystem* sysp;
+};
+
+
+/* class for Diff gradient */
+class SysConstraintFunc : public Differentiator::JacobianFunction {
+ public:
+ SysConstraintFunc(int nf, int ny, const OptimizerSystem* sysPtr)
+ : Differentiator::JacobianFunction(nf,ny) { sysp = sysPtr; }
+
+ // Must provide this pure virtual function.
+ int f(const Vector& y, Vector& fy) const {
+ return(sysp->constraintFunc(y, true, fy)); // calls user's contraintFunc
+ }
+ const OptimizerSystem* sysp;
+};
+
+
+class SimTK_SIMMATH_EXPORT Optimizer::OptimizerRep {
+public:
+ virtual ~OptimizerRep();
+ OptimizerRep(const OptimizerSystem& sys)
+ : sysp(&sys),
+ myHandle(0),
+ cf(0),
+ of(0),
+ jacDiff(0),
+ gradDiff(0),
+ convergenceTolerance(Real(1e-3)),
+ constraintTolerance(Real(1e-4)),
+ maxIterations(1000),
+ limitedMemoryHistory(50),
+ diagnosticsLevel(0),
+ diffMethod(Differentiator::CentralDifference),
+ objectiveEstimatedAccuracy(SignificantReal),
+ constraintsEstimatedAccuracy(SignificantReal),
+ numericalGradient(false),
+ numericalJacobian(false)
+
+ {
+ }
+ OptimizerRep()
+ : sysp(0),
+ myHandle(0),
+ cf(0),
+ of(0),
+ jacDiff(0),
+ gradDiff(0),
+ convergenceTolerance(Real(1e-3)),
+ constraintTolerance(Real(1e-4)),
+ maxIterations(1000),
+ limitedMemoryHistory(50),
+ diagnosticsLevel(0),
+ diffMethod(Differentiator::CentralDifference),
+ objectiveEstimatedAccuracy(SignificantReal),
+ constraintsEstimatedAccuracy(SignificantReal),
+ numericalGradient(false),
+ numericalJacobian(false)
+ {
+ }
+
+ virtual OptimizerRep* clone() const { return 0; };
+ static bool isAvailable() { return true; }
+
+ virtual Real optimize( Vector &results ) = 0;
+
+ const OptimizerSystem& getOptimizerSystem() const {return *sysp;}
+
+
+ void setDiagnosticsLevel( const int level );
+ void setConvergenceTolerance( Real accuracy );
+ void setConstraintTolerance( Real tolerance );
+ void setMaxIterations( const int iter );
+ void setLimitedMemoryHistory( const int history );
+
+ bool setAdvancedStrOption( const std::string &option, const std::string &value );
+ bool setAdvancedRealOption( const std::string &option, const Real value );
+ bool setAdvancedIntOption( const std::string &option, const int value );
+ bool setAdvancedBoolOption( const std::string &option, const bool value );
+
+ bool getAdvancedStrOption( const std::string &option, std::string &value ) const;
+ bool getAdvancedRealOption( const std::string &option, Real &value ) const;
+ bool getAdvancedIntOption( const std::string &option, int &value ) const;
+ bool getAdvancedBoolOption( const std::string &option, bool &value ) const;
+
+ void setMyHandle(Optimizer& cp) {myHandle = &cp;}
+ const Optimizer& getMyHandle() const {assert(myHandle); return *myHandle;}
+ void clearMyHandle() {myHandle=0;}
+
+ void useNumericalGradient(bool flag, Real objEstAccuracy);
+ void useNumericalJacobian(bool flag, Real consEstAccuracy);
+ void setDifferentiatorMethod( Differentiator::Method method);
+
+ bool isUsingNumericalGradient() const { return numericalGradient; }
+ bool isUsingNumericalJacobian() const { return numericalJacobian; }
+ Differentiator::Method getDifferentiatorMethod() const {return diffMethod;}
+ Real getEstimatedAccuracyOfObjective() const
+ { return objectiveEstimatedAccuracy; }
+ Real getEstimatedAccuracyOfConstraints() const
+ { return constraintsEstimatedAccuracy; }
+
+ const Differentiator& getGradientDifferentiator() const {
+ assert(gradDiff);
+ return *gradDiff;
+ }
+
+ const Differentiator& getJacobianDifferentiator() const {
+ assert(jacDiff);
+ return *jacDiff;
+ }
+
+ virtual OptimizerAlgorithm getAlgorithm() const = 0;
+
+ static int numericalGradient_static( const OptimizerSystem&, const Vector & parameters, const bool new_parameters, Vector &gradient );
+ static int numericalJacobian_static(const OptimizerSystem&,
+ const Vector& parameters, const bool new_parameters, Matrix& jacobian );
+
+protected:
+ // These methods are to be called by derived classes as an interface
+ // to the OptimizerSystem virtuals. The signature must match that required by
+ // IpOpt's matching callbacks. We're using the "user data" argument to pass in
+ // the current OptimizerRep, making these behave like non-static members.
+
+ static int objectiveFuncWrapper ( int n, const Real* x, int new_x, Real* f, void* rep);
+ static int gradientFuncWrapper ( int n, const Real* x, int new_x, Real* gradient, void* rep);
+ static int constraintFuncWrapper( int n, const Real* x, int new_x, int m, Real* g, void* rep);
+ static int constraintJacobianWrapper( int n, const Real* x, int new_x,int m, int nele_jac,
+ int* iRow, int* jCol, Real* values, void* rep);
+ static int hessianWrapper( int n, const Real* x, int new_x, Real obj_factor,
+ int m, Real* lambda, int new_lambda,
+ int nele_hess, int* iRow, int* jCol,
+ Real* values, void* rep);
+
+ int diagnosticsLevel;
+ Real convergenceTolerance;
+ Real constraintTolerance;
+ int maxIterations;
+ int limitedMemoryHistory;
+ Differentiator::Method diffMethod;
+ Real objectiveEstimatedAccuracy;
+ Real constraintsEstimatedAccuracy;
+
+private:
+ const OptimizerSystem* sysp;
+ bool numericalGradient; // true if optimizer will compute an numerical gradient
+ bool numericalJacobian; // true if optimizer will compute an numerical Jacobian
+ Differentiator *gradDiff;
+ Differentiator *jacDiff;
+
+ SysObjectiveFunc *of;
+ SysConstraintFunc *cf;
+
+ std::map<std::string, std::string> advancedStrOptions;
+ std::map<std::string, Real> advancedRealOptions;
+ std::map<std::string, int> advancedIntOptions;
+ std::map<std::string, bool> advancedBoolOptions;
+
+ friend class Optimizer;
+ Optimizer* myHandle; // The owner handle of this Rep.
+
+}; // end class OptimizerRep
+
+class DefaultOptimizer: public Optimizer::OptimizerRep {
+ Real optimize( Vector &results );
+ OptimizerRep* clone() const;
+ OptimizerAlgorithm getAlgorithm() const;
+};
+
+} // namespace SimTK
+
+
+#endif // SimTK_SIMMATH_OPTIMIZER_REP_H_
diff --git a/SimTKmath/include/simmath/internal/common.h b/SimTKmath/include/simmath/internal/common.h
new file mode 100644
index 0000000..d2f4b24
--- /dev/null
+++ b/SimTKmath/include/simmath/internal/common.h
@@ -0,0 +1,180 @@
+#ifndef SimTK_SIMMATH_COMMON_H_
+#define SimTK_SIMMATH_COMMON_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: Michael Sherman *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This is the header file that every Simmath compilation unit should include
+ * first.
+ */
+
+#include "SimTKcommon.h"
+
+/* Shared libraries are messy in Visual Studio. We have to distinguish three
+ * cases:
+ * (1) this header is being used to build the simmath shared library (dllexport)
+ * (2) this header is being used by a *client* of the simmath shared
+ * library (dllimport)
+ * (3) we are building the simmath static library, or the client is
+ * being compiled with the expectation of linking with the
+ * simmath static library (nothing special needed)
+ * In the CMake script for building this library, we define one of the symbols
+ * SIMMATH_BUILDING_{SHARED|STATIC}_LIBRARY
+ * Client code normally has no special symbol defined, in which case we'll
+ * assume it wants to use the shared library. However, if the client defines
+ * the symbol SimTK_USE_STATIC_LIBRARIES we'll suppress the dllimport so
+ * that the client code can be linked with static libraries. Note that
+ * the client symbol is not library dependent, while the library symbols
+ * affect only the simmath library, meaning that other libraries can
+ * be clients of this one. However, we are assuming all-static or all-shared.
+*/
+
+#ifdef _WIN32
+ #if defined(SimTK_SIMMATH_BUILDING_SHARED_LIBRARY)
+ #define SimTK_SIMMATH_EXPORT __declspec(dllexport)
+ #elif defined(SimTK_SIMMATH_BUILDING_STATIC_LIBRARY) || defined(SimTK_USE_STATIC_LIBRARIES)
+ #define SimTK_SIMMATH_EXPORT
+ #else
+ /* i.e., a client of a shared library */
+ #define SimTK_SIMMATH_EXPORT __declspec(dllimport)
+ #endif
+#else
+ /* Linux, Mac */
+ #define SimTK_SIMMATH_EXPORT
+#endif
+
+
+// Every SimTK Core library must provide these two routines, with the library
+// name appearing after the "version_" and "about_".
+extern "C" {
+ SimTK_SIMMATH_EXPORT void SimTK_version_simmath(int* major, int* minor, int* build);
+ SimTK_SIMMATH_EXPORT void SimTK_about_simmath(const char* key, int maxlen, char* value);
+}
+
+
+
+const static double POSITIVE_INF = 2e19;
+const static double NEGATIVE_INF = -2e19;
+
+namespace SimTK {
+
+
+namespace Exception {
+
+class OptimizerFailed : public Base {
+public:
+ OptimizerFailed( const char * fn, int ln, String msg) : Base(fn, ln)
+ {
+ setMessage("Optimizer failed: " + msg );
+ }
+private:
+};
+
+class UnrecognizedParameter : public Base {
+public:
+ UnrecognizedParameter( const char * fn, int ln, String msg) : Base(fn, ln)
+ {
+ setMessage("Unrecognized Parameter: " + msg );
+ }
+private:
+};
+
+class IllegalLapackArg : public Base {
+public:
+ IllegalLapackArg( const char *fn, int ln, const char *lapackRoutine,
+ int info ) : Base(fn, ln)
+ {
+ char buf[1024];
+
+ sprintf(buf, "SimTK internal error: %s called with an illegal value to"
+ " argument #%d.\nPlease report this at SimTK.org.",
+ lapackRoutine, -info );
+ setMessage(String(buf));
+
+ }
+private:
+};
+class IncorrectArrayLength : public Base {
+public:
+ IncorrectArrayLength( const char *fn, int ln, const char *valueName, int length,
+ const char *paramName, int paramValue, const char *where) : Base(fn, ln)
+ {
+ char buf[1024];
+
+ sprintf(buf, "Incorrect array length in %s : %s is %d and must equal %s which is %d",
+ where, valueName, length, paramName, paramValue );
+ setMessage(String(buf));
+
+ }
+private:
+};
+
+class SingularMatrix : public Base {
+public:
+ SingularMatrix( const char *fn, int ln, int index,
+ const char *where) : Base(fn, ln)
+ {
+ char buf[1024];
+
+ sprintf(buf, "%s failed because index %d in matrix was singular and factorization failed",
+ where, index );
+ setMessage(String(buf));
+
+ }
+private:
+};
+
+class ConvergedFailed : public Base {
+public:
+ ConvergedFailed( const char *fn, int ln, const char *algorithm,
+ const char *where) : Base(fn, ln)
+ {
+ char buf[1024];
+
+ sprintf(buf, "%s failed because %s failed to converge", where, algorithm );
+ setMessage(String(buf));
+
+ }
+private:
+};
+
+class NotPositiveDefinite : public Base {
+public:
+ NotPositiveDefinite( const char *fn, int ln, int index,
+ const char *where) : Base(fn, ln)
+ {
+ char buf[1024];
+
+ sprintf(buf, "%s failed because index %d in matrix was not positive definite and factorization failed ",
+ where, index );
+ setMessage(String(buf));
+
+ }
+private:
+};
+} // namespace Exception
+
+} // namespace SimTK
+
+#endif // SimTK_SIMMATH_COMMON_H_
diff --git a/SimTKmath/sharedTarget/CMakeLists.txt b/SimTKmath/sharedTarget/CMakeLists.txt
new file mode 100644
index 0000000..4d7b700
--- /dev/null
+++ b/SimTKmath/sharedTarget/CMakeLists.txt
@@ -0,0 +1,78 @@
+## This whole directory exists just so I could define this extra preprocessor value.
+
+ADD_DEFINITIONS(-DSimTK_SIMMATH_BUILDING_SHARED_LIBRARY)
+
+#
+# Set up file groups for better browsing in Visual Studio.
+#
+
+# Override Api headers to put them in the Api group.
+source_group("Header Files\\Api" FILES ${API_ABS_INCLUDE_FILES})
+
+# Now put things in their specified groups.
+if (SOURCE_GROUPS)
+ list(LENGTH SOURCE_GROUPS NGROUPS)
+ math(EXPR lastgrpnum ${NGROUPS}-1)
+ foreach( grpnum RANGE 0 ${lastgrpnum} )
+ list(GET SOURCE_GROUPS ${grpnum} grp)
+ list(GET SOURCE_GROUP_FILES ${grpnum} grpfile)
+ source_group("${grp}" FILES "${grpfile}")
+ endforeach()
+endif()
+
+# These are defaults for files that don't otherwise get mentioned.
+source_group("Source Files\\Others" FILES ${SOURCE_FILES})
+source_group("Header Files\\Others" FILES ${SOURCE_INCLUDE_FILES})
+
+IF(BUILD_UNVERSIONED_LIBRARIES)
+
+ ADD_LIBRARY(${SHARED_TARGET} SHARED
+ ${SOURCE_FILES} ${SOURCE_INCLUDE_FILES}
+ ${API_ABS_INCLUDE_FILES})
+
+ TARGET_LINK_LIBRARIES(${SHARED_TARGET}
+ debug ${SimTKCOMMON_SHARED_LIBRARY}_d
+ optimized ${SimTKCOMMON_SHARED_LIBRARY}
+ ${MATH_LIBS_TO_USE})
+
+ SET_TARGET_PROPERTIES(${SHARED_TARGET} PROPERTIES
+ PROJECT_LABEL "Code - ${SHARED_TARGET}"
+ SOVERSION ${SIMBODY_SONAME_VERSION})
+
+ # install library; on Windows both .lib and .dll go in the lib directory.
+ INSTALL(TARGETS ${SHARED_TARGET}
+ PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE
+ GROUP_READ GROUP_WRITE GROUP_EXECUTE
+ WORLD_READ WORLD_EXECUTE
+ LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ RUNTIME DESTINATION ${CMAKE_INSTALL_FULL_BINDIR})
+
+ENDIF(BUILD_UNVERSIONED_LIBRARIES)
+
+
+IF(BUILD_VERSIONED_LIBRARIES)
+
+ ADD_LIBRARY(${SHARED_TARGET_VN} SHARED
+ ${SOURCE_FILES} ${SOURCE_INCLUDE_FILES}
+ ${API_ABS_INCLUDE_FILES})
+
+ TARGET_LINK_LIBRARIES(${SHARED_TARGET_VN}
+ debug ${SimTKCOMMON_SHARED_LIBRARY_VN}_d
+ optimized ${SimTKCOMMON_SHARED_LIBRARY_VN}
+ ${MATH_LIBS_TO_USE_VN})
+
+ SET_TARGET_PROPERTIES(${SHARED_TARGET_VN} PROPERTIES
+ PROJECT_LABEL "Code - ${SHARED_TARGET_VN}"
+ SOVERSION ${SIMBODY_SONAME_VERSION})
+
+ # install library; on Windows both .lib and .dll go in the lib directory.
+ INSTALL(TARGETS ${SHARED_TARGET_VN}
+ PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE
+ GROUP_READ GROUP_WRITE GROUP_EXECUTE
+ WORLD_READ WORLD_EXECUTE
+ LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ RUNTIME DESTINATION ${CMAKE_INSTALL_FULL_BINDIR})
+
+ENDIF(BUILD_VERSIONED_LIBRARIES)
diff --git a/SimTKmath/src/About.cpp b/SimTKmath/src/About.cpp
new file mode 100644
index 0000000..373fda7
--- /dev/null
+++ b/SimTKmath/src/About.cpp
@@ -0,0 +1,116 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * Defines the standard SimTK core "version" and "about" routines.
+ */
+
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+
+#include <string>
+#include <cstring>
+#include <cctype>
+
+#define STR(var) #var
+#define MAKE_VERSION_STRING(maj,min,build) STR(maj.min.build)
+#define MAKE_COPYRIGHT_STRING(y,a) \
+ "Copyright (c) " STR(y) " Stanford University, " STR(a)
+#define MAKE_STRING(a) STR(a)
+
+#define GET_VERSION_STRING \
+ MAKE_VERSION_STRING(SimTK_SIMMATH_MAJOR_VERSION, \
+ SimTK_SIMMATH_MINOR_VERSION, \
+ SimTK_SIMMATH_PATCH_VERSION)
+
+#define GET_COPYRIGHT_STRING \
+ MAKE_COPYRIGHT_STRING(SimTK_SIMMATH_COPYRIGHT_YEARS, \
+ SimTK_SIMMATH_AUTHORS)
+
+#define GET_AUTHORS_STRING \
+ MAKE_STRING(SimTK_SIMMATH_AUTHORS)
+
+#define GET_LIBRARY_STRING \
+ MAKE_STRING(SimTK_SIMMATH_LIBRARY_NAME)
+
+#ifndef NDEBUG
+ #define GET_DEBUG_STRING "debug"
+#else
+ #define GET_DEBUG_STRING "release"
+#endif
+
+#if defined(SimTK_SIMMATH_BUILDING_SHARED_LIBRARY)
+ #define GET_TYPE_STRING "shared"
+#elif defined(SimTK_SIMMATH_BUILDING_STATIC_LIBRARY)
+ #define GET_TYPE_STRING "static"
+#else
+ #define GET_TYPE_STRING "<unknown library type?!>"
+#endif
+
+extern "C" {
+
+void SimTK_version_simmath(int* major, int* minor, int* patch) {
+ static const char* l = "SimTK library=" GET_LIBRARY_STRING;
+ static const char* t = "SimTK type=" GET_TYPE_STRING;
+ static const char* d = "SimTK debug=" GET_DEBUG_STRING;
+ static const char* v = "SimTK version=" GET_VERSION_STRING;
+ static const char* c = "SimTK copyright=" GET_COPYRIGHT_STRING;
+
+ if (major) *major = SimTK_SIMMATH_MAJOR_VERSION;
+ if (minor) *minor = SimTK_SIMMATH_MINOR_VERSION;
+ if (patch) *patch = SimTK_SIMMATH_PATCH_VERSION;
+
+ // Force statics to be present in the binary (Release mode otherwise
+ // optimizes them away).
+ volatile int i=0;
+ if (i) { // never true, but compiler doesn't know ...
+ *major = *l + *t + *d + *v + *c;
+ }
+}
+
+void SimTK_about_simmath(const char* key, int maxlen, char* value) {
+ if (maxlen <= 0 || value==0) return;
+ value[0] = '\0'; // in case we don't find a match
+ if (key==0) return;
+
+ // downshift the key
+ std::string skey(key);
+ for (size_t i=0; i<skey.size(); ++i)
+ skey[i] = std::tolower(skey[i]);
+
+ const char* v = 0;
+ if (skey == "version") v = GET_VERSION_STRING;
+ else if (skey == "library") v = GET_LIBRARY_STRING;
+ else if (skey == "type") v = GET_TYPE_STRING;
+ else if (skey == "copyright") v = GET_COPYRIGHT_STRING;
+ else if (skey == "authors") v = GET_AUTHORS_STRING;
+ else if (skey == "debug") v = GET_DEBUG_STRING;
+
+ if (v) {
+ std::strncpy(value,v,maxlen-1);
+ value[maxlen-1] = '\0'; // in case we ran out of room
+ }
+}
+
+}
diff --git a/SimTKmath/src/Differentiator.cpp b/SimTKmath/src/Differentiator.cpp
new file mode 100644
index 0000000..365638c
--- /dev/null
+++ b/SimTKmath/src/Differentiator.cpp
@@ -0,0 +1,794 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This is the private (library side) implementation of the Simmath
+ * Differentiator family of classes.
+ */
+
+#include "SimTKcommon.h"
+#include "simmath/Differentiator.h"
+
+#include <exception>
+
+namespace SimTK {
+
+ ///////////////////////////////
+ // DIFFERENTIATOR EXCEPTIONS //
+ ///////////////////////////////
+
+class Differentiator::OpNotAllowedForFunctionOfThisShape : public Exception::Base {
+public:
+ OpNotAllowedForFunctionOfThisShape(const char* fn, int ln,
+ String op, String reqShape, String funcKind,
+ int nf, int ny) : Base(fn,ln)
+ {
+ setMessage("Differentiator method " + op + "() requires a " + reqShape +
+ " function but was called with a " + funcKind + " that is " +
+ String(nf) + "x" + String(ny));
+ }
+};
+
+class Differentiator::UserFunctionThrewAnException : public Exception::Base {
+public:
+ UserFunctionThrewAnException(const char* fn, int ln,
+ const char* msg) : Base(fn,ln) {
+ setMessage("A user function threw an exception when invoked by Differentiator."
+ " The exception message was: " + String(msg));
+ }
+};
+
+class Differentiator::UserFunctionReturnedNonzeroStatus : public Exception::Base {
+public:
+ UserFunctionReturnedNonzeroStatus(const char* fn, int ln, int status) : Base(fn,ln) {
+ setMessage("A user function returned non-zero status " + String(status)
+ + " when invoked by Differentiator.");
+ }
+};
+
+class Differentiator::UnknownMethodSpecified : public Exception::Base {
+public:
+ UnknownMethodSpecified(const char* fn, int ln, int enumValue,
+ const char* classname, const char* methodname) : Base(fn,ln) {
+ setMessage("An unrecognized Differentiator::Method with enum value " + String(enumValue)
+ + " was passed to " + String(classname) + "::" + String(methodname) +"()");
+ }
+};
+
+
+
+// We want to perturb y0 by h such that the perturbation is
+// exactly representable in binary. hEst is our first guess
+// at h, which we calculate h=(y0+hEst)-y0 taking care not
+// to let a clever compiler optimize that away.
+static Real cleanUpH(Real hEst, Real y0) {
+ volatile Real temp = y0+hEst;
+ return temp-y0;
+}
+
+static void throwIfMethodInvalid(Differentiator::Method m, const char* op) {
+ if (!Differentiator::isValidMethod(m))
+ SimTK_THROW3(Differentiator::UnknownMethodSpecified, (int)m,
+ "Differentiator", op);
+}
+
+// Operation "op" has been given the indicated method m. If m
+// is a valid method, return it. If m is unspecified, return the
+// indicated default. Otherwise, throw a "bad method" exception.
+static Differentiator::Method getMethodOrThrow
+ (Differentiator::Method m, Differentiator::Method def, const char* op)
+{
+ throwIfMethodInvalid(m,op);
+ return (m==Differentiator::UnspecifiedMethod ? def : m);
+}
+
+ ///////////////////////////////////////////////////////////
+ // REP CLASS DECLARATIONS FOR DIFFERENTIATOR & FUNCTIONS //
+ ///////////////////////////////////////////////////////////
+
+// These classes are just local definitions in this compilation unit; they
+// aren't visible anywhere else.
+class ScalarFunctionRep;
+class GradientFunctionRep;
+class JacobianFunctionRep;
+
+// This is used as a value for y in calculating the step size when
+// the actual y is smaller.
+static const Real YMin = Real(0.1);
+class Differentiator::DifferentiatorRep {
+public:
+ DifferentiatorRep(Differentiator* handle,
+ const Differentiator::Function::FunctionRep&,
+ Differentiator::Method defaultMethod);
+ // default destructor, no default constructor, no copy or copy assign
+
+ // This constant is the algorithm we'll use by default.
+ static const Differentiator::Method DefaultDefaultMethod
+ = Differentiator::ForwardDifference;
+
+ void calcDerivative(const ScalarFunctionRep&, Differentiator::Method,
+ Real y0, Real fy0, Real& dfdy) const;
+ void calcGradient(const GradientFunctionRep&, Differentiator::Method,
+ const Vector& y0, Real fy0, Vector& gf) const;
+ void calcJacobian(const JacobianFunctionRep&, Differentiator::Method,
+ const Vector& y0, const Vector& fy0, Matrix& dfdy) const;
+
+ const Real& getAccFac(int order) const {
+ if (order==1) return AccFac1;
+ if (order==2) return AccFac2;
+ assert(!"Unrecognized Differentiator order");
+ return NaN;
+ }
+
+ void resetAllStatistics() {
+ nDifferentiations = nDifferentiationFailures = nCallsToUserFunction = 0;
+ }
+
+ // Statistics
+ mutable int nDifferentiations;
+ mutable int nDifferentiationFailures;
+ mutable int nCallsToUserFunction;
+private:
+ Differentiator* myHandle;
+ friend class Differentiator;
+
+ // Problem dimensions. These are obtained from the Function at
+ // Differentiator construction time and cannot change afterwards.
+
+ const Differentiator::Function::FunctionRep& frep;
+ const int NFunctions, NParameters;
+ const Real EstimatedAccuracy;
+
+ // This is set on construction, but can be changed.
+ Differentiator::Method defaultMethod;
+
+ // These are pre-calculated accuracy factors for 1st order and
+ // 2nd order step size estimates, derived from EstimatedAccuracy
+ // upon construction.
+ const Real AccFac1, AccFac2;
+
+ // These temporaries are kept here so we can reuse their storage space
+ // from call to call once they have been allocated at construction.
+ // The *values* do not persist across calls.
+ mutable Vector ytmp; // [NParameters]
+ mutable Vector fyptmp, fymtmp; // [NFunctions]
+
+ // suppress
+ DifferentiatorRep(const DifferentiatorRep&);
+ DifferentiatorRep& operator=(const DifferentiatorRep&);
+};
+
+Differentiator::~Differentiator() {
+ delete rep;
+}
+
+class Differentiator::Function::FunctionRep {
+ friend class Differentiator::Function;
+public:
+ FunctionRep(int nf, int np, Real acc)
+ : nFunc(nf), nParam(np), estimatedAccuracy(acc)
+ {
+ if (estimatedAccuracy < 0) // use default
+ estimatedAccuracy = SignificantReal; // ~1e-14 in double
+
+ resetAllStatistics();
+ }
+
+ virtual ~FunctionRep() { }
+
+ virtual String functionKind() const=0; // for error messages
+
+ // Each kind of function can be differentiated with any of these calls, provided
+ // the dimensions are reasonable. That is, a scalar function can produce a
+ // scalar derivative or a 1x1 Jacobian. If an initial (unperturbed) value is
+ // available, its address is passed in fy0. Otherwise, fy0 should be null and
+ // we'll attempt to generate our own with an initial call to the user function.
+ virtual void calcDerivative(const DifferentiatorRep&, Differentiator::Method,
+ Real y0, const Real* fy0p, Real& dfdy) const=0;
+ virtual void calcGradient(const DifferentiatorRep&, Differentiator::Method,
+ const Vector& y0, const Real* fy0p, Vector& gf) const=0;
+ virtual void calcJacobian(const DifferentiatorRep&, Differentiator::Method,
+ const Vector& y0, const Vector* fy0p, Matrix& dfdy) const=0;
+
+ int getNumFunctions() const {assert(nFunc>=0); return nFunc;}
+ int getNumParameters() const {assert(nParam>=0); return nParam;}
+ Real getEstimatedAccuracy() const {
+ assert(estimatedAccuracy>0);
+ return estimatedAccuracy;
+ }
+
+ void resetAllStatistics() {
+ nCalls = nFailures = 0;
+ }
+
+protected:
+ // Stats
+ mutable int nCalls;
+ mutable int nFailures;
+
+private:
+ int nFunc, nParam;
+ Real estimatedAccuracy;
+
+};
+
+class ScalarFunctionRep : public Differentiator::Function::FunctionRep {
+public:
+ ScalarFunctionRep(const Differentiator::ScalarFunction& func, Real acc)
+ : FunctionRep(1,1,acc), sf(func) {}
+
+ // Virtuals (from FunctionRep)
+ String functionKind() const {return "ScalarFunction";}
+
+ void calcDerivative(const Differentiator::DifferentiatorRep& diff, Differentiator::Method m,
+ Real y0, const Real* fy0p, Real& dfdy) const
+ {
+ Real fy0;
+ if (fy0p) fy0 = *fy0p;
+ else {diff.nCallsToUserFunction++; call(y0, fy0);}
+ diff.calcDerivative(*this,m,y0,fy0,dfdy);
+ }
+
+ void calcGradient(const Differentiator::DifferentiatorRep& diff, Differentiator::Method m,
+ const Vector& y0, const Real* fy0p, Vector& gf) const
+ {
+ assert(y0.size()==1);
+ gf.resize(1);
+
+ Real fy0;
+ if (fy0p) fy0 = *fy0p;
+ else {diff.nCallsToUserFunction++; call(y0[0], fy0);}
+ diff.calcDerivative(*this,m,y0[0],fy0,gf[0]);
+ }
+
+ void calcJacobian(const Differentiator::DifferentiatorRep& diff, Differentiator::Method m,
+ const Vector& y0, const Vector* fy0p, Matrix& dfdy) const
+ {
+ assert(y0.size()==1 && (!fy0p || (*fy0p).size()==1));
+ dfdy.resize(1,1);
+
+ Real fy0;
+ if (fy0p) fy0 = (*fy0p)[0];
+ else {diff.nCallsToUserFunction++; call(y0[0], fy0);}
+ diff.calcDerivative(*this, m,y0[0],fy0,dfdy(0,0));
+ }
+
+ // Local stuff
+ void call(const Real& y, Real& fy) const {
+ nCalls++;
+ nFailures++; // assume failure unless proven otherwise
+
+ int status;
+ try
+ { status = sf.f(y,fy); }
+ catch (const std::exception& e)
+ { SimTK_THROW1(Differentiator::UserFunctionThrewAnException, e.what()); }
+ catch (...)
+ { SimTK_THROW1(Differentiator::UserFunctionThrewAnException,
+ "UNRECOGNIZED EXCEPTION TYPE"); }
+ if (status != 0)
+ SimTK_THROW1(Differentiator::UserFunctionReturnedNonzeroStatus, status);
+
+ nFailures--;
+ }
+
+ const Differentiator::ScalarFunction& sf;
+};
+
+class GradientFunctionRep : public Differentiator::Function::FunctionRep {
+public:
+ GradientFunctionRep(const Differentiator::GradientFunction& func, int np, Real acc)
+ : FunctionRep(1,np,acc), gf(func) {}
+
+ // Virtuals (from FunctionRep)
+ String functionKind() const {return "GradientFunction";}
+
+ void calcDerivative(const Differentiator::DifferentiatorRep& diff, Differentiator::Method m,
+ Real y0, const Real* fy0p, Real& dfdy) const
+ {
+ if (getNumParameters() != 1)
+ SimTK_THROW5(Differentiator::OpNotAllowedForFunctionOfThisShape,
+ "calcDerivative", "1x1", "GradientFunction", 1, getNumParameters());
+
+ const Vector y0v(1, y0);
+ Real fy0;
+ if (fy0p) fy0 = *fy0p;
+ else {diff.nCallsToUserFunction++; call(y0v, fy0);}
+ Vector dfdyv(1, &dfdy); // refers to dfdy data directly
+ diff.calcGradient(*this,m,y0v,fy0,dfdyv);
+ }
+
+ void calcGradient(const Differentiator::DifferentiatorRep& diff, Differentiator::Method m,
+ const Vector& y0, const Real* fy0p, Vector& gf) const
+ {
+ Real fy0;
+ if (fy0p) fy0 = *fy0p;
+ else {diff.nCallsToUserFunction++; call(y0, fy0);}
+ diff.calcGradient(*this,m,y0,fy0,gf);
+ }
+
+ void calcJacobian(const Differentiator::DifferentiatorRep& diff, Differentiator::Method m,
+ const Vector& y0, const Vector* fy0p, Matrix& dfdy) const
+ {
+ assert(!fy0p || (*fy0p).size()==1);
+ dfdy.resize(1,getNumParameters());
+ Real fy0;
+ if (fy0p) fy0 = (*fy0p)[0];
+ else {diff.nCallsToUserFunction++; call(y0, fy0);}
+ diff.calcGradient(*this,m,y0,fy0,~dfdy[0]);
+ }
+
+ // Local stuff
+ void call(const Vector& y, Real& fy) const {
+ nCalls++;
+ nFailures++; // assume failure unless proven otherwise
+
+ int status;
+ try
+ { status = gf.f(y,fy); }
+ catch (const std::exception& e)
+ { SimTK_THROW1(Differentiator::UserFunctionThrewAnException, e.what()); }
+ catch (...)
+ { SimTK_THROW1(Differentiator::UserFunctionThrewAnException,
+ "UNRECOGNIZED EXCEPTION TYPE"); }
+
+ if (status != 0)
+ SimTK_THROW1(Differentiator::UserFunctionReturnedNonzeroStatus, status);
+
+ --nFailures;
+ }
+
+ const Differentiator::GradientFunction& gf;
+};
+
+class JacobianFunctionRep : public Differentiator::Function::FunctionRep {
+public:
+ JacobianFunctionRep(const Differentiator::JacobianFunction& func, int nf, int np, Real acc)
+ : FunctionRep(nf,np,acc), jf(func) {}
+
+ // Virtuals (from FunctionRep)
+ String functionKind() const {return "JacobianFunction";}
+
+ void calcDerivative(const Differentiator::DifferentiatorRep& diff, Differentiator::Method m,
+ Real y0, const Real* fy0p, Real& dfdy) const
+ {
+ if (getNumFunctions() != 1 || getNumParameters() != 1)
+ SimTK_THROW5(Differentiator::OpNotAllowedForFunctionOfThisShape,
+ "calcDerivative", "1x1", "JacobianFunction", getNumFunctions(), getNumParameters());
+
+ const Vector y0v(1, y0);
+ Vector fy0v(1);
+ if (fy0p) fy0v[0] = *fy0p;
+ else {diff.nCallsToUserFunction++; call(y0v, fy0v);}
+ Matrix dfdym(1,1, &dfdy); // refers to dfdy directly as data
+ diff.calcJacobian(*this,m,y0v,fy0v,dfdym);
+ }
+
+ void calcGradient(const Differentiator::DifferentiatorRep& diff, Differentiator::Method m,
+ const Vector& y0, const Real* fy0p, Vector& gf) const
+ {
+ if (getNumFunctions() != 1)
+ SimTK_THROW5(Differentiator::OpNotAllowedForFunctionOfThisShape,
+ "calcGradient", "1xn", "JacobianFunction", getNumFunctions(), getNumParameters());
+
+ Vector fy0v(1);
+ if (fy0p) fy0v[0] = *fy0p;
+ else {diff.nCallsToUserFunction++; call(y0, fy0v);}
+ diff.calcJacobian(*this,m,y0,fy0v,gf);
+ }
+
+ void calcJacobian(const Differentiator::DifferentiatorRep& diff, Differentiator::Method m,
+ const Vector& y0, const Vector* fy0p, Matrix& dfdy) const
+ {
+ if (fy0p)
+ diff.calcJacobian(*this,m,y0,*fy0p,dfdy);
+ else {
+ Vector fy0(getNumFunctions());
+ diff.nCallsToUserFunction++; call(y0, fy0);
+ diff.calcJacobian(*this,m,y0,fy0,dfdy);
+ }
+ }
+
+ void call(const Vector& y, Vector& fy) const {
+ nCalls++;
+ nFailures++; // assume failure unless proven otherwise
+
+ int status;
+ try
+ { status = jf.f(y,fy); }
+ catch (const std::exception& e)
+ { SimTK_THROW1(Differentiator::UserFunctionThrewAnException, e.what()); }
+ catch (...)
+ { SimTK_THROW1(Differentiator::UserFunctionThrewAnException,
+ "UNRECOGNIZED EXCEPTION TYPE"); }
+
+ if (status != 0)
+ SimTK_THROW1(Differentiator::UserFunctionReturnedNonzeroStatus, status);
+
+ nFailures--;
+ }
+
+ const Differentiator::JacobianFunction& jf;
+};
+
+ //////////////////////////////////////
+ // IMPLEMENTATION OF DIFFERENTIATOR //
+ //////////////////////////////////////
+
+Differentiator::Differentiator(const Function& f, Differentiator::Method defaultMethod) {
+ rep = new DifferentiatorRep(this, *f.rep, defaultMethod);
+}
+
+
+
+Differentiator& Differentiator::setDefaultMethod(Differentiator::Method m) {
+ rep->defaultMethod = getMethodOrThrow(m, DifferentiatorRep::DefaultDefaultMethod,
+ "setDefaultMethod");
+ return *this;
+}
+
+Differentiator::Method Differentiator::getDefaultMethod() const {
+ return rep->defaultMethod;
+}
+
+void Differentiator::calcDerivative
+ (Real y0, Real fy0, Real& dfdy, Differentiator::Method m) const
+{
+ rep->nDifferentiations++;
+ rep->nDifferentiationFailures++; // assume the worst
+
+ rep->frep.calcDerivative(*rep,m,y0,&fy0,dfdy);
+
+ rep->nDifferentiationFailures--;
+}
+
+// The slow version
+Real Differentiator::calcDerivative
+ (Real y0, Differentiator::Method m) const
+{
+ rep->nDifferentiations++;
+ rep->nDifferentiationFailures++; // assume the worst
+
+ Real dfdy;
+ rep->frep.calcDerivative(*rep,m,y0,0,dfdy);
+
+ rep->nDifferentiationFailures--;
+ return dfdy;
+}
+
+void Differentiator::calcGradient
+ (const Vector& y0, Real fy0, Vector& gradf,
+ Differentiator::Method m) const
+{
+ rep->nDifferentiations++;
+ rep->nDifferentiationFailures++; // assume the worst
+
+ rep->frep.calcGradient(*rep,m,y0,&fy0,gradf);
+
+ rep->nDifferentiationFailures--;
+}
+
+// The slow version
+Vector Differentiator::calcGradient
+ (const Vector& y0, Differentiator::Method m) const
+{
+ rep->nDifferentiations++;
+ rep->nDifferentiationFailures++; // assume the worst
+
+ SimTK_APIARGCHECK2_ALWAYS(y0.size()==rep->NParameters, "Differentiator", "calcGradient",
+ "Supplied number of parameters (state length) was %ld but should have been %d",
+ y0.size(), rep->NParameters);
+
+ Vector grad(rep->NParameters);
+ rep->frep.calcGradient(*rep,m,y0,0,grad);
+
+ rep->nDifferentiationFailures--;
+ return grad; // TODO: use DeadVector to avoid copy
+}
+
+
+void Differentiator::calcJacobian
+ (const Vector& y0, const Vector& fy0, Matrix& dfdy,
+ Differentiator::Method m) const
+{
+ rep->nDifferentiations++;
+ rep->nDifferentiationFailures++; // assume the worst
+
+ SimTK_APIARGCHECK2_ALWAYS(y0.size()==rep->NParameters, "Differentiator", "calcJacobian",
+ "Expecting %d elements in the parameter (state) vector but got %d",
+ rep->NParameters, (int)y0.size());
+
+ SimTK_APIARGCHECK2_ALWAYS(fy0.size()==rep->NFunctions, "Differentiator", "calcJacobian",
+ "Expecting %d elements in the unperturbed function value but got %d",
+ rep->NFunctions, (int)fy0.size());
+
+ rep->frep.calcJacobian(*rep,m,y0,&fy0,dfdy);
+
+ rep->nDifferentiationFailures--;
+}
+
+// The slow version
+Matrix Differentiator::calcJacobian
+ (const Vector& y0, Differentiator::Method m) const
+{
+ rep->nDifferentiations++;
+ rep->nDifferentiationFailures++; // assume the worst
+
+ SimTK_APIARGCHECK2_ALWAYS(y0.size()==rep->NParameters, "Differentiator", "calcJacobian",
+ "Expecting %d elements in the parameter (state) vector but got %d",
+ rep->NParameters, (int)y0.size());
+
+ Matrix dfdy(rep->NFunctions, rep->NParameters);
+ rep->frep.calcJacobian(*rep,m,y0,0,dfdy);
+
+ rep->nDifferentiationFailures--;
+ return dfdy;
+}
+
+void Differentiator::resetAllStatistics() {
+ rep->resetAllStatistics();
+}
+
+int Differentiator::getNumDifferentiations() const {
+ return rep->nDifferentiations;
+}
+
+int Differentiator::getNumDifferentiationFailures() const {
+ return rep->nDifferentiationFailures;
+}
+
+int Differentiator::getNumCallsToUserFunction() const {
+ return rep->nCallsToUserFunction;
+}
+
+
+/*static*/ bool
+Differentiator::isValidMethod(Differentiator::Method m) {
+ switch(m) {
+ case UnspecifiedMethod:
+ case ForwardDifference:
+ case CentralDifference:
+ return true;
+ }
+ return false;
+}
+
+/*static*/ const char*
+Differentiator::getMethodName(Differentiator::Method m) {
+ throwIfMethodInvalid(m, "getMethodName");
+ switch(m) {
+ case UnspecifiedMethod: return "UnspecifiedMethod";
+ case ForwardDifference: return "ForwardDifference";
+ case CentralDifference: return "CentralDifference";
+ }
+ return 0; // can't happen
+}
+
+/*static*/ int
+Differentiator::getMethodOrder(Differentiator::Method m) {
+ throwIfMethodInvalid(m, "getMethodOrder");
+ if (m==UnspecifiedMethod)
+ m = DifferentiatorRep::DefaultDefaultMethod;
+ switch(m) {
+ case ForwardDifference: return 1;
+ case CentralDifference: return 2;
+ default:
+ assert(!"Shouldn't be any other cases");
+ }
+ assert(!"Shouldn't have gotten here since method was already checked");
+ return -1;
+}
+
+ ////////////////////////////////////////////////
+ // IMPLEMENTATION OF DIFFERENTIATOR::FUNCTION //
+ ////////////////////////////////////////////////
+
+// The derived classes ScalarFunction, GradientFunction, JacobianFunction
+// are responsible for allocating the right kind of concrete FunctionRep.
+Differentiator::Function::Function()
+ : rep(0) { }
+
+Differentiator::Function::~Function() {
+ delete rep;
+}
+
+Differentiator::Function&
+Differentiator::Function::setNumFunctions(int nf) {
+ SimTK_APIARGCHECK1_ALWAYS(nf>=0, "Differentiator::Function", "setNumFunctions",
+ "The number of functions was %d but must be >= 0", nf);
+
+ rep->nFunc = nf;
+ return *this;
+}
+Differentiator::Function&
+Differentiator::Function::setNumParameters(int ny) {
+ SimTK_APIARGCHECK1_ALWAYS(ny>=0, "Differentiator::Function", "setNumParameters",
+ "The number of parameters was %d but must be >= 0", ny);
+
+ rep->nParam = ny;
+ return *this;
+}
+Differentiator::Function&
+Differentiator::Function::setEstimatedAccuracy(Real ea) {
+ SimTK_APIARGCHECK1_ALWAYS(0<ea&&ea<1, "Differentiator::Function", "setNumParameters",
+ "The estimated accuracy was %g but must be between 0 and 1 (noninclusive)", ea);
+
+ rep->estimatedAccuracy = ea;
+ return *this;
+}
+
+int Differentiator::Function::getNumFunctions() const {
+ return rep->nFunc;
+}
+int Differentiator::Function::getNumParameters() const {
+ return rep->nParam;
+}
+Real Differentiator::Function::getEstimatedAccuracy() const {
+ return rep->estimatedAccuracy;
+}
+
+void Differentiator::Function::resetAllStatistics(){
+ rep->resetAllStatistics();
+}
+int Differentiator::Function::getNumCalls() const {
+ return rep->nCalls;
+}
+int Differentiator::Function::getNumFailures() const {
+ return rep->nFailures;
+}
+
+ ///////////////////////////////////////////////////////////////////
+ // IMPLEMENTATION OF SCALAR, GRADIENT, JACOBIAN FUNCTION CLASSES //
+ ///////////////////////////////////////////////////////////////////
+
+Differentiator::ScalarFunction::ScalarFunction(Real acc) {
+ rep = new ScalarFunctionRep(*this, acc);
+}
+Differentiator::GradientFunction::GradientFunction(int np, Real acc) {
+ rep = new GradientFunctionRep(*this, np, acc);
+}
+Differentiator::JacobianFunction::JacobianFunction(int nf, int np, Real acc) {
+ rep = new JacobianFunctionRep(*this, nf, np, acc);
+}
+
+
+ //////////////////////////////////////////
+ // IMPLEMENTATION OF DIFFERENTIATOR REP //
+ //////////////////////////////////////////
+
+Differentiator::DifferentiatorRep::DifferentiatorRep
+( Differentiator* handle,
+ const Differentiator::Function::FunctionRep& fr,
+ Differentiator::Method defMthd
+)
+: myHandle(handle), frep(fr),
+ NParameters(fr.getNumParameters()),
+ NFunctions(fr.getNumFunctions()),
+ EstimatedAccuracy(fr.getEstimatedAccuracy()),
+ defaultMethod(getMethodOrThrow(defMthd, DefaultDefaultMethod, "Differentiator")),
+ AccFac1(std::sqrt(EstimatedAccuracy)),
+ AccFac2(std::pow(EstimatedAccuracy, OneThird))
+{
+ //TODO
+ assert(NParameters >= 0 && NFunctions >= 0 && EstimatedAccuracy > 0);
+
+ resetAllStatistics();
+ ytmp.resize(NParameters);
+ fyptmp.resize(NFunctions);
+ fymtmp.resize(NFunctions);
+}
+
+void Differentiator::DifferentiatorRep::calcDerivative
+ (const ScalarFunctionRep& f, Differentiator::Method m, Real y0, Real fy0, Real& dfdy) const
+{
+ // This won't return if the method is bad.
+ const Differentiator::Method method = getMethodOrThrow(m, defaultMethod, "calcDerivative");
+
+ //TODO
+ assert(NParameters==1 && NFunctions==1);
+
+ const int order = Differentiator::getMethodOrder(method);
+ const Real hEst = getAccFac(order)*std::max(std::abs(y0), YMin);
+ const Real h = cleanUpH(hEst, y0);
+
+ Real fyplus, fyminus;
+ Real y = y0+h;
+ nCallsToUserFunction++; f.call(y, fyplus);
+ if (order==1) {
+ dfdy = (fyplus-fy0)/h;
+ } else {
+ y = y0-h;
+ nCallsToUserFunction++; f.call(y, fyminus);
+ dfdy = (fyplus-fyminus)/(2*h);
+ }
+}
+
+void Differentiator::DifferentiatorRep::calcGradient
+ (const GradientFunctionRep& f, Differentiator::Method m, const Vector& y0, Real fy0, Vector& gradf) const
+{
+ // This won't return if the method is bad.
+ const Differentiator::Method method = getMethodOrThrow(m, defaultMethod, "calcGradient");
+
+ //TODO
+ assert(NFunctions==1);
+ assert(ytmp.size()==NParameters);
+ assert(y0.size() == NParameters);
+
+ gradf.resize(NParameters);
+
+ ytmp = y0;
+ const int order = Differentiator::getMethodOrder(method);
+
+ for (int i=0; i < f.getNumParameters(); ++i) {
+ const Real hEst = getAccFac(order)*std::max(std::abs(y0[i]), YMin);
+ const Real h = cleanUpH(hEst, y0[i]);
+ Real fyplus, fyminus;
+ ytmp[i] = y0[i]+h;
+ nCallsToUserFunction++; f.call(ytmp, fyplus);
+ if (order==1) {
+ gradf[i] = (fyplus-fy0)/h;
+ } else {
+ ytmp[i] = y0[i]-h;
+ nCallsToUserFunction++; f.call(ytmp, fyminus);
+ gradf[i] = (fyplus-fyminus)/(2*h);
+ }
+ ytmp[i] = y0[i]; // restore
+ }
+}
+
+void Differentiator::DifferentiatorRep::calcJacobian
+ (const JacobianFunctionRep& f, Differentiator::Method m,
+ const Vector& y0, const Vector& fy0, Matrix& dfdy) const
+{
+ // This won't return if the method is bad.
+ const Differentiator::Method method = getMethodOrThrow(m, defaultMethod, "calcJacobian");
+
+ //TODO
+ assert(ytmp.size()==NParameters && fyptmp.size()==NFunctions && fymtmp.size()==NFunctions);
+ assert(y0.size() == NParameters);
+ assert(fy0.size() == NFunctions);
+
+ dfdy.resize(NFunctions,NParameters);
+
+ const int order = Differentiator::getMethodOrder(method);
+
+ ytmp = y0;
+ for (int i=0; i < NParameters; ++i) {
+ const Real hEst = getAccFac(order)*std::max(std::abs(y0[i]), YMin);
+ const Real h = cleanUpH(hEst, y0[i]);
+ ytmp[i] = y0[i]+h;
+ nCallsToUserFunction++; f.call(ytmp, fyptmp);
+ if (order==1) {
+ dfdy(i) = (fyptmp-fy0)/h;
+ } else {
+ ytmp[i] = y0[i]-h;
+ nCallsToUserFunction++; f.call(ytmp, fymtmp);
+ dfdy(i) = (fyptmp-fymtmp)/(2*h);
+ }
+ ytmp[i] = y0[i]; // restore
+ }
+}
+
+} // namespace SimTK
+
+
diff --git a/SimTKmath/src/MultibodyGraphMaker.cpp b/SimTKmath/src/MultibodyGraphMaker.cpp
new file mode 100644
index 0000000..91e9807
--- /dev/null
+++ b/SimTKmath/src/MultibodyGraphMaker.cpp
@@ -0,0 +1,752 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): Multibody Graph Maker *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Kevin He *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* This is the implementation of MultibodyGraphMaker and its auxiliary classes.
+*/
+
+#include "SimTKcommon.h"
+#include "simmath/internal/common.h"
+#include "simmath/MultibodyGraphMaker.h"
+
+#include <exception>
+#include <stdexcept>
+#include <string>
+#include <iostream>
+#include <cstdio>
+
+using std::cout; using std::endl;
+
+namespace SimTK {
+
+//------------------------------------------------------------------------------
+// CONSTRUCTOR
+//------------------------------------------------------------------------------
+MultibodyGraphMaker::MultibodyGraphMaker()
+: weldTypeName("weld"), freeTypeName("free")
+{ initialize(); }
+
+
+//------------------------------------------------------------------------------
+// GET GROUND BODY NAME
+//------------------------------------------------------------------------------
+const std::string& MultibodyGraphMaker::getGroundBodyName() const {
+ if (bodies.empty())
+ throw std::logic_error(
+ "getGroundBodyName(): you can't call this until you have called "
+ "addBody() at least once -- the first body is Ground.");
+ return bodies[0].name;
+}
+
+
+//------------------------------------------------------------------------------
+// ADD JOINT TYPE
+//------------------------------------------------------------------------------
+int MultibodyGraphMaker::
+addJointType(const std::string& name,
+ int numMobilities,
+ bool haveGoodLoopJointAvailable,
+ void* userRef)
+{
+ if (!(0 <= numMobilities && numMobilities <= 6)) throw std::runtime_error
+ ("addJointType(): Illegal number of mobilities for joint type '"
+ + name + "'");
+
+ // Reject duplicate type name, and reserved type names.
+ if (name==getWeldJointTypeName() || name==getFreeJointTypeName())
+ throw std::runtime_error("addJointType(): Joint type '" + name
+ + "' is reserved (you can change the reserved names).");
+ std::map<std::string,int>::const_iterator p = jointTypeName2Num.find(name);
+ if (p != jointTypeName2Num.end()) throw std::runtime_error
+ ("addJointType(): Duplicate joint type '" + name + "'");
+
+ const int jointTypeNum = (int)jointTypes.size(); // next available
+ jointTypeName2Num[name] = jointTypeNum; // provide fast name lookup
+
+ jointTypes.push_back(JointType(name, numMobilities,
+ haveGoodLoopJointAvailable, userRef));
+
+ return jointTypeNum;
+}
+
+
+//------------------------------------------------------------------------------
+// ADD BODY
+//------------------------------------------------------------------------------
+void MultibodyGraphMaker::addBody(const std::string& name,
+ double mass,
+ bool mustBeBaseBody,
+ void* userRef)
+{
+ if (mass < 0) throw std::invalid_argument
+ ("addBody(): Body '" + name + "' specified negative mass");
+
+ // Reject duplicate body name.
+ std::map<std::string,int>::const_iterator p = bodyName2Num.find(name);
+ if (p != bodyName2Num.end()) throw std::runtime_error
+ ("addBody(): Duplicate body name '" + name + "'");
+
+ const int bodyNum = (int)bodies.size(); // next available slot
+ bodyName2Num[name] = bodyNum; // provide fast name lookup
+
+ if (bodyNum == 0) { // First body is Ground; use only the name and ref.
+ Body ground(name, Infinity, false, userRef);
+ ground.level = 0; // already in tree
+ bodies.push_back(ground);
+ } else { // This is a real body.
+ bodies.push_back(Body(name, mass, mustBeBaseBody, userRef));
+ }
+}
+
+//------------------------------------------------------------------------------
+// DELETE BODY
+//------------------------------------------------------------------------------
+bool MultibodyGraphMaker::deleteBody(const std::string& name)
+{
+ // Reject non-existing body name.
+ std::map<std::string,int>::iterator p = bodyName2Num.find(name);
+ if (p == bodyName2Num.end())
+ return false;
+
+ const int bodyNum = p->second;
+
+ std::vector<int>& jointAsParent = updBody(bodyNum).jointsAsParent;
+ while (jointAsParent.size() > 0)
+ deleteJoint(joints[jointAsParent[0]].name);
+
+ std::vector<int>& jointAsChild = updBody(bodyNum).jointsAsChild;
+ while (jointAsChild.size() > 0)
+ deleteJoint(joints[jointAsChild[0]].name);
+
+ bodies.erase(bodies.begin() + bodyNum);
+ bodyName2Num.erase(p);
+
+ // Update body indices due to the deletion of this body
+ for (unsigned int i = 0; i < joints.size(); ++i) {
+ if (joints[i].parentBodyNum > bodyNum)
+ --(updJoint(i).parentBodyNum);
+ if (joints[i].childBodyNum > bodyNum)
+ --(updJoint(i).childBodyNum);
+ }
+
+ for (unsigned int i = bodyNum; i < bodies.size(); ++i)
+ bodyName2Num[bodies[i].name] = i;
+
+ return true;
+}
+
+//------------------------------------------------------------------------------
+// ADD JOINT
+//------------------------------------------------------------------------------
+void MultibodyGraphMaker::addJoint(const std::string& name,
+ const std::string& type,
+ const std::string& parentBodyName,
+ const std::string& childBodyName,
+ bool mustBeLoopJoint,
+ void* userRef)
+{
+ // Reject duplicate joint name, unrecognized type or body names.
+ std::map<std::string,int>::const_iterator p = jointName2Num.find(name);
+ if (p != jointName2Num.end()) throw std::runtime_error
+ ("addJoint(): Duplicate joint name '" + name + "'");
+
+ const int typeNum = getJointTypeNum(type);
+ if (typeNum < 0) throw std::runtime_error
+ ("addJoint(): Joint " + name + " had unrecognized joint type '"
+ + type + "'");
+
+ const int parentBodyNum = getBodyNum(parentBodyName);
+ if (parentBodyNum < 0) throw std::runtime_error
+ ("addJoint(): Joint " + name + " had unrecognized parent body '"
+ + parentBodyName + "'");
+
+ const int childBodyNum = getBodyNum(childBodyName);
+ if (childBodyNum < 0) throw std::runtime_error
+ ("addJoint(): Joint " + name + " had unrecognized child body '"
+ + childBodyName + "'");
+
+ const int jointNum = (int)joints.size(); // next available slot
+ jointName2Num[name] = jointNum; // provide fast name lookup
+
+ joints.push_back(Joint(name, typeNum, parentBodyNum, childBodyNum,
+ mustBeLoopJoint, userRef));
+
+ updBody(parentBodyNum).jointsAsParent.push_back(jointNum);
+ updBody(childBodyNum).jointsAsChild.push_back(jointNum);
+}
+
+//------------------------------------------------------------------------------
+// DELETE JOINT
+//------------------------------------------------------------------------------
+bool MultibodyGraphMaker::deleteJoint(const std::string& name)
+{
+ // Reject duplicate joint name, unrecognized type or body names.
+ std::map<std::string,int>::iterator p = jointName2Num.find(name);
+ if (p == jointName2Num.end())
+ return false;
+
+ const int jointNum = p->second;
+ jointName2Num.erase(p);
+
+ std::vector<int>& jointsAsParent =
+ updBody(joints[jointNum].parentBodyNum).jointsAsParent;
+ std::vector<int>::iterator it =
+ std::find(jointsAsParent.begin(), jointsAsParent.end(), jointNum);
+ if (it == jointsAsParent.end()) throw std::runtime_error
+ ("deleteJoint(): Joint " + name +
+ " doesn't exist in jointsAsParent of parent body ");
+ jointsAsParent.erase(it);
+
+ std::vector<int>& jointsAsChild =
+ updBody(joints[jointNum].childBodyNum).jointsAsChild;
+ it = std::find(jointsAsChild.begin(), jointsAsChild.end(), jointNum);
+ if (it == jointsAsChild.end()) throw std::runtime_error
+ ("deleteJoint(): Joint " + name +
+ " doesn't exist in jointsAsChild of child body ");
+ jointsAsChild.erase(it);
+
+ joints.erase(joints.begin() + jointNum);
+
+ // Update indices due to the deletion of this joint
+ for (unsigned int i = 0; i < bodies.size(); ++i) {
+
+ for (it = updBody(i).jointsAsParent.begin(); it != updBody(i).jointsAsParent.end(); ++it)
+ if (*it > jointNum) {
+ --(*it);
+ jointName2Num[joints[*it].name] = *it;
+ }
+
+ for (it = updBody(i).jointsAsChild.begin(); it != updBody(i).jointsAsChild.end(); ++it)
+ if (*it > jointNum) {
+ --(*it);
+ // no need to adjust jointName2Num here since the first loops has done it already
+ }
+
+ }
+ return true;
+}
+
+//------------------------------------------------------------------------------
+// GENERATE GRAPH
+//------------------------------------------------------------------------------
+void MultibodyGraphMaker::generateGraph() {
+ // Body and Joint inputs have been supplied. Body 0 is Ground.
+
+ // 1. Add free joints to Ground for any body that didn't appear
+ // in any joint, and for any bodies that said they must be base bodies and
+ // don't already have a connection to Ground.
+ //
+ // While we're at it, look for dangling massless bodies -- they are only
+ // allowed if they were originally connected to at least two other bodies
+ // by given tree-eligible joints.
+ // TODO: "must be loop joint" joints shouldn't count but we're not
+ // checking.
+ for (int bn=1; bn < getNumBodies(); ++bn) { // skip Ground
+ const Body& body = getBody(bn);
+ const int nJoints = // how many joints connect to this body?
+ (int)(body.jointsAsChild.size()+body.jointsAsParent.size());
+ if (body.mass == 0 && nJoints < 2)
+ throw std::runtime_error(
+ "generateGraph(): body " + body.name +
+ " is massless but connected by fewer than 2 joints.");
+
+ if (nJoints==0 || (body.mustBeBaseBody && !bodiesAreConnected(bn, 0)))
+ connectBodyToGround(bn);
+ }
+
+ // 2. Repeat until all bodies are in the tree:
+ // - try to build the tree from Ground outwards
+ // - if incomplete, add one missing connection to Ground
+ // This terminates because we add at least one body to the tree each time.
+ while (true) {
+ growTree();
+ const int newBaseBody = chooseNewBaseBody();
+ if (newBaseBody < 0)
+ break; // all bodies are in the tree
+ // Add joint to Ground.
+ connectBodyToGround(newBaseBody);
+ }
+
+ // 3. Split the loops
+ // This requires adding new "slave" bodies to replace the child bodies in
+ // the loop-forming joints. Then each slave is welded back to its master
+ // body (the original child).
+ breakLoops();
+}
+
+
+//------------------------------------------------------------------------------
+// CLEAR GRAPH
+//------------------------------------------------------------------------------
+void MultibodyGraphMaker::clearGraph() {
+ for (int bn=1; bn < getNumBodies(); ++bn) { // skip Ground
+ if (bodies[bn].isSlave()) {
+ // Assumption: all slave bodies are clustered at the end of the body array
+ bodies.erase(bodies.begin() + bn, bodies.begin() + bodies.size());
+ break;
+ }
+ updBody(bn).forgetGraph(*this);
+ }
+
+ for (int jn=0; jn < getNumJoints(); ++jn)
+ if (updJoint(jn).forgetGraph(*this))
+ --jn;
+
+ mobilizers.clear();
+ constraints.clear();
+}
+
+//------------------------------------------------------------------------------
+// DUMP GRAPH
+//------------------------------------------------------------------------------
+void MultibodyGraphMaker::dumpGraph(std::ostream& o) const {
+ char buf[1024];
+ o << "\nMULTIBODY GRAPH\n";
+ o << "---------------\n";
+ o << "\n" << getNumBodies() << " BODIES:\n";
+ for (int i=0; i < getNumBodies(); ++i) {
+ const MultibodyGraphMaker::Body& body = getBody(i);
+ sprintf(buf, "%2d %2d: %s mass=%g mob=%d master=%d %s\n",
+ i, body.level,
+ body.name.c_str(), body.mass, body.mobilizer, body.master,
+ body.mustBeBaseBody?"MUST BE BASE BODY":"");
+ o << buf;
+ o << " jointsAsParent=[";
+ for (unsigned j=0; j<body.jointsAsParent.size(); ++j)
+ o << " " << body.jointsAsParent[j];
+ o << "]\t jointsAsChild=[";
+ for (unsigned j=0; j<body.jointsAsChild.size(); ++j)
+ o << " " << body.jointsAsChild[j];
+ o << "]\t slaves=[";
+ for (unsigned j=0; j<body.slaves.size(); ++j)
+ o << " " << body.slaves[j];
+ o << "]\n";
+ }
+
+ o << "\n" << getNumJoints() << " JOINTS:\n";
+ for (int i=0; i < getNumJoints(); ++i) {
+ const MultibodyGraphMaker::Joint& joint = getJoint(i);
+ sprintf(buf, "%2d %2d: %20s %20s->%-20s %10s loopc=%2d %s %s\n",
+ i, joint.mobilizer, joint.name.c_str(),
+ getBody(joint.parentBodyNum).name.c_str(),
+ getBody(joint.childBodyNum).name.c_str(),
+ getJointType(joint.jointTypeNum).name.c_str(),
+ joint.loopConstraint,
+ joint.mustBeLoopJoint?"MUST BE LOOP":"",
+ joint.isAddedBaseJoint?"ADDED BASE JOINT":"");
+ o << buf;
+ }
+
+ o << "\n" << getNumMobilizers() << " MOBILIZERS:\n";
+ for (int i=0; i < getNumMobilizers(); ++i) {
+ const MultibodyGraphMaker::Mobilizer& mo = getMobilizer(i);
+ const MultibodyGraphMaker::Joint& joint = getJoint(mo.joint);
+ const MultibodyGraphMaker::Body& inb = getBody(mo.inboardBody);
+ const MultibodyGraphMaker::Body& outb = getBody(mo.outboardBody);
+ sprintf(buf, "%2d %2d: %20s %20s->%-20s %10s %2d %3s\n",
+ i, mo.level, joint.name.c_str(),
+ inb.name.c_str(), outb.name.c_str(),
+ mo.getJointTypeName().c_str(),
+ mo.joint, mo.isReversed?"REV":"");
+ o << buf;
+ }
+
+ o << "\n" << getNumLoopConstraints() << " LOOP CONSTRAINTS:\n";
+ for (int i=0; i < getNumLoopConstraints(); ++i) {
+ const MultibodyGraphMaker::LoopConstraint& lc = getLoopConstraint(i);
+ const MultibodyGraphMaker::Body parent = getBody(lc.parentBody);
+ const MultibodyGraphMaker::Body child = getBody(lc.childBody);
+ sprintf(buf, "%d: %s parent=%s child=%s jointNum=%d\n",
+ i, lc.type.c_str(), parent.name.c_str(), child.name.c_str(),
+ lc.joint);
+ o << buf;
+ }
+ o << "\n----- END OF MULTIBODY GRAPH.\n\n";
+}
+
+
+//------------------------------------------------------------------------------
+// INITIALIZE
+//------------------------------------------------------------------------------
+// Clear all data and prime with free and weld joint types.
+void MultibodyGraphMaker::initialize() {
+ clear();
+ jointTypes.push_back(JointType(weldTypeName, 0, true, NULL));
+ jointTypes.push_back(JointType(freeTypeName, 6, true, NULL));
+ jointTypeName2Num[weldTypeName] = 0;
+ jointTypeName2Num[freeTypeName] = 1;
+}
+
+
+//------------------------------------------------------------------------------
+// SPLIT BODY
+//------------------------------------------------------------------------------
+// Create a new slave body for the given master, and add it to the list of
+// bodies. Does not create the related loop constraint. The body
+// number assigned to the slave is returned.
+int MultibodyGraphMaker::splitBody(int masterBodyNum) {
+ const int slaveBodyNum = (int)bodies.size(); // next available
+ Body& master = updBody(masterBodyNum);
+ // First slave is number 1, slave 0 is the master.
+ std::stringstream ss;
+ ss << "#" << master.name << "_slave_" << master.getNumSlaves()+1;
+ Body slave(ss.str(), NaN, false, NULL);
+ slave.master = masterBodyNum;
+ master.slaves.push_back(slaveBodyNum);
+ bodies.push_back(slave);
+ // No name lookup for slave bodies.
+ return slaveBodyNum;
+}
+
+
+//------------------------------------------------------------------------------
+// CHOOSE NEW BASE BODY
+//------------------------------------------------------------------------------
+// We've tried to build the tree but might not have succeeded in using
+// all the bodies. That means we'll have to connect one of them to Ground.
+// Select the best unattached body to use as a base body, or -1 if all bodies
+// are already included in the spanning tree. We hope to find a body that is
+// serving as a parent but has never been listed as a child; that is crying out
+// to be connected directly to Ground. Failing that, we'll pick a body that
+// has been used as a parent often.
+int MultibodyGraphMaker::chooseNewBaseBody() const {
+ bool parentOnlyBodySeen = false;
+ int bestBody = -1; int nChildren=-1;
+ // Skip the Ground body.
+ for (int bx=1; bx < getNumBodies(); ++bx) {
+ const Body& body = bodies[bx];
+ if (body.isInTree())
+ continue; // unavailable
+ if (parentOnlyBodySeen && !body.jointsAsChild.empty())
+ continue; // already seen parent-only bodies; no need for this one
+ if (!parentOnlyBodySeen && body.jointsAsChild.empty()) {
+ // This is our first parent-only body; it is automatically the
+ // best candidate now.
+ parentOnlyBodySeen = true;
+ bestBody = bx; nChildren = (int)body.jointsAsParent.size();
+ } else { // Keep the body that has the most children.
+ if ((int)body.jointsAsParent.size() > nChildren) {
+ bestBody = bx;
+ nChildren = (int)body.jointsAsParent.size();
+ }
+ }
+ }
+ return bestBody;
+}
+
+
+//------------------------------------------------------------------------------
+// CONNECT BODY TO GROUND
+//------------------------------------------------------------------------------
+// Connect the given body to Ground by a free joint with parent Ground and
+// child the given body. This makes the body a base body (level 1 body)
+// for some subtree of the multibody graph.
+void MultibodyGraphMaker::connectBodyToGround(int bodyNum) {
+ const Body& body = getBody(bodyNum);
+ assert(!body.isInTree());
+ const std::string jointName = "#" + getGroundBodyName() + "_" + body.name;
+ addJoint(jointName, getFreeJointTypeName(),
+ getGroundBodyName(), body.name, false, NULL);
+ updJoint(jointName).isAddedBaseJoint = true;
+}
+
+
+//------------------------------------------------------------------------------
+// ADD MOBILIZER FOR JOINT
+//------------------------------------------------------------------------------
+// We've already determined this joint is eligible to become a mobilizer; now
+// make it so. Given a joint for which either the parent or child is in the
+// tree, but not both, create a mobilizer implementing this joint and attaching
+// the unattached body (which will be the mobilizer's outboard body) to the
+// tree. The mobilizer number is returned and also recorded in the joint
+// and outboard body.
+int MultibodyGraphMaker::addMobilizerForJoint(int jointNum) {
+ Joint& joint = updJoint(jointNum);
+ assert(!joint.mustBeLoopJoint); // can't be a tree joint
+ assert(!joint.hasMobilizer()); // already done
+ const int pNum = joint.parentBodyNum, cNum = joint.childBodyNum;
+ Body& parent = updBody(pNum);
+ Body& child = updBody(cNum);
+ // Exactly one of these must already be in the tree.
+ assert(parent.isInTree() ^ child.isInTree());
+
+ const int mobNum = (int)mobilizers.size(); // next available
+ if (parent.isInTree()) {
+ // Child is outboard body (forward joint)
+ child.level = parent.level + 1;
+ mobilizers.push_back(Mobilizer(jointNum,child.level,
+ pNum,cNum,false,this));
+ child.mobilizer = mobNum;
+ } else if (child.isInTree()) {
+ // Parent is outboard body (reverse joint)
+ parent.level = child.level + 1;
+ mobilizers.push_back(Mobilizer(jointNum,parent.level,
+ cNum,pNum,true,this));
+ parent.mobilizer = mobNum;
+ }
+ joint.mobilizer = mobNum;
+ return mobNum;
+}
+
+
+//------------------------------------------------------------------------------
+// FIND HEAVIEST UNASSIGNED FORWARD JOINT
+//------------------------------------------------------------------------------
+// Starting with a given body that is in the tree already, look at its
+// unassigned children (meaning bodies connected by joints that aren't
+// mobilizers yet) and return the joint connecting it to the child with the
+// largest mass. (The idea is that this would be a good direction to extend the
+// chain.) Return -1 if this body has no children.
+int MultibodyGraphMaker::
+findHeaviestUnassignedForwardJoint(int inboardBody) const {
+ const Body& inb = getBody(inboardBody);
+ int jointNum = -1;
+ double maxMass=0;
+ // Search joints for which this is the parent body.
+ for (unsigned i=0; i < inb.jointsAsParent.size(); ++i) {
+ const int jfwd = inb.jointsAsParent[i];
+ const Joint& joint = joints[jfwd];
+ if (joint.hasMobilizer()) continue; // already in the tree
+ if (joint.mustBeLoopJoint) continue; // can't be a tree joint
+ const Body& child = getBody(joint.childBodyNum);
+ if (child.isInTree()) continue; // this is a loop joint
+ if (child.mass > maxMass) {
+ jointNum = jfwd;
+ maxMass = child.mass;
+ }
+ }
+ return jointNum;
+}
+
+
+//------------------------------------------------------------------------------
+// FIND HEAVIEST UNASSIGNED REVERSE JOINT
+//------------------------------------------------------------------------------
+// Same as previous method, but now we are looking for unassigned joints where
+// the given body serves as the child so we would have to reverse the joint to
+// add it to the tree. (This is less desirable so is a fallback.)
+int MultibodyGraphMaker::
+findHeaviestUnassignedReverseJoint(int inboardBody) const {
+ const Body& inb = getBody(inboardBody);
+ int jointNum = -1;
+ double maxMass=0;
+ // Search joints for which this is the child body.
+ for (unsigned i=0; i < inb.jointsAsChild.size(); ++i) {
+ const int jrev = inb.jointsAsChild[i];
+ const Joint& joint = joints[jrev];
+ if (joint.hasMobilizer()) continue; // already in the tree
+ if (joint.mustBeLoopJoint) continue; // can't be a tree joint
+ const Body& parent = getBody(joint.parentBodyNum);
+ if (parent.isInTree()) continue; // this is a loop joint
+ if (parent.mass > maxMass) {
+ jointNum = jrev;
+ maxMass = parent.mass;
+ }
+ }
+ return jointNum;
+}
+
+
+//------------------------------------------------------------------------------
+// GROW TREE
+//------------------------------------------------------------------------------
+// Process unused joints for which one body is in the tree (at level h) and the
+// other is not. Add the other body to the tree at level h+1, marking the joint
+// as forward (other body is child) or reverse (other body is parent). Repeat
+// until no changes are made. Does not assign loop joints or any bodies that
+// don't have a path to Ground. Extend the multibody tree starting with the
+// lowest-level eligible joints and moving outwards. We're doing this
+// breadth-first so that we get roughly even-length chains and we'll stop at
+// the first level at which we fail to find any joint. If we fail to consume
+// all the bodies, the caller will have to add a level 1 joint to attach a
+// previously-disconnected base body to Ground and then call this again.
+//
+// We violate the breadth-first heuristic to avoid ending a branch with a
+// massless body. If we add a massless body, we'll keep going out along its
+// branch until we hit a massful body. It is a disaster if we fail to find a
+// massful body because a tree that terminates in a massless body will generate
+// a singular mass matrix. We'll throw an exception in that case, but note that
+// this may just be a failure of the heuristic; there may be some tree that
+// could have avoided the terminal massless body but we failed to discover it.
+//
+// TODO: keep a list of unprocessed joints so we don't have to go through
+// all of them again at each level.
+void MultibodyGraphMaker::growTree() {
+ for (int level=1; ;++level) { // level of outboard (mobilized) body
+ bool anyMobilizerAdded = false;
+ for (int jNum=0; jNum<getNumJoints(); ++jNum) {
+ // See if this joint is at the right level (meaning its inboard
+ // body is at level-1) and is available to become a mobilizer.
+ const Joint& joint = getJoint(jNum);
+ if (joint.hasMobilizer()) continue; // already done
+ if (joint.mustBeLoopJoint) continue; // can't be a tree joint
+ const Body& parent = getBody(joint.parentBodyNum);
+ const Body& child = getBody(joint.childBodyNum);
+ // Exactly one of parent or child must already be in the tree.
+ if (!(parent.isInTree() ^ child.isInTree())) continue;
+ if (parent.isInTree()) {
+ if (parent.level != level-1) continue; // not time yet
+ } else { // child is in tree
+ if (child.level != level-1) continue; // not time yet
+ }
+ addMobilizerForJoint(jNum);
+ anyMobilizerAdded = true;
+
+ // We just made joint jNum a mobilizer. If its outboard body
+ // is massless, we need to keep extending this branch of the tree
+ // until we can end it with a massful body.
+ if (getBody(mobilizers.back().outboardBody).mass > 0)
+ continue;
+
+ // Pick a further-outboard body with mass and extend branch to it.
+ // Prefer forward-direction joints if possible.
+ // If only massless outboard bodies are available, add one and
+ // keep going.
+ while (true) {
+ const int bNum = mobilizers.back().outboardBody;
+ const int jfwd = findHeaviestUnassignedForwardJoint(bNum);
+ if (jfwd>=0 && getBody(getJoint(jfwd).childBodyNum).mass > 0) {
+ addMobilizerForJoint(jfwd);
+ break;
+ }
+ const int jrev = findHeaviestUnassignedReverseJoint(bNum);
+ if (jrev>=0 && getBody(getJoint(jrev).parentBodyNum).mass > 0) {
+ addMobilizerForJoint(jrev);
+ break;
+ }
+
+ // Couldn't find a massful body to add. Add another massless
+ // body (if there is one) and keep trying.
+ if (jfwd >= 0) {
+ addMobilizerForJoint(jfwd);
+ continue;
+ }
+ if (jrev >= 0) {
+ addMobilizerForJoint(jrev);
+ continue;
+ }
+
+ // Uh oh. Nothing outboard of the massless body we just added.
+ // We're ending this chain with a massless body; not good.
+ throw std::runtime_error("growTree(): algorithm produced an"
+ " invalid tree containing a terminal massless body ("
+ + getBody(bNum).name + "). This may be due to an invalid"
+ " model or failure of heuristics. In the latter case you"
+ " may be able to get a valid tree by forcing a different"
+ " loop break or changing parent->child ordering.");
+ }
+ }
+ if (!anyMobilizerAdded)
+ break;
+ }
+}
+
+
+//------------------------------------------------------------------------------
+// BREAK LOOPS
+//------------------------------------------------------------------------------
+// Find any remaining unused joints, which will have both parent and
+// child bodies already in the tree. This includes joints that the user
+// told us must be loop joints, and ones picked by the algorithm.
+// For each of those, implement the joint with a loop constraint if one
+// is provided, otherwise implement it with a mobilizer and split off a
+// slave body from the child body, use that slave as the outboard body
+// for the mobilizer, and add a weld constraint to reconnect the slave to
+// its master (the original child body).
+void MultibodyGraphMaker::breakLoops() {
+ for (int jx=0; jx < getNumJoints(); ++jx) {
+ Joint& jinfo = joints[jx];
+ if (jinfo.hasMobilizer()) continue; // already done
+ const int px = jinfo.parentBodyNum, cx = jinfo.childBodyNum;
+ assert(getBody(px).isInTree() && getBody(cx).isInTree());
+
+ const JointType& jtype = getJointType(jinfo.jointTypeNum);
+ if (jtype.haveGoodLoopJointAvailable) {
+ const int loopNum = (int)constraints.size();
+ constraints.push_back(LoopConstraint(jtype.name,jx,px,cx,this));
+ jinfo.loopConstraint = loopNum;
+ continue;
+ }
+
+ // No usable loop constraint for this type of joint. Add a new slave
+ // body so we can use a mobilizer. (Body references are invalidated here
+ // since we're adding a new body -- don't reuse them.)
+ const int sx = splitBody(cx);
+ Body& parent = updBody(px);
+ Body& child = updBody(cx);
+ Body& slave = updBody(sx);
+
+ // Mobilize the slave body.
+ const int mobNum = (int)mobilizers.size(); // next available
+ const int level = parent.level+1;
+ jinfo.mobilizer = slave.mobilizer = mobNum;
+ slave.jointsAsChild.push_back(jx);
+ mobilizers.push_back(Mobilizer(jx,level,px,sx,false,this));
+ slave.level = level;
+ }
+}
+
+
+//------------------------------------------------------------------------------
+// BODIES ARE CONNECTED
+//------------------------------------------------------------------------------
+// Return true if there is a joint between two bodies given by body number,
+// regardless of parent/child ordering.
+bool MultibodyGraphMaker::bodiesAreConnected(int b1, int b2) const {
+ const Body& body1 = getBody(b1);
+ for (unsigned i=0; i < body1.jointsAsParent.size(); ++i)
+ if (getJoint(body1.jointsAsParent[i]).childBodyNum == b2)
+ return true;
+ for (unsigned i=0; i < body1.jointsAsChild.size(); ++i)
+ if (getJoint(body1.jointsAsChild[i]).parentBodyNum == b2)
+ return true;
+ return false;
+}
+
+//------------------------------------------------------------------------------
+// FORGET GRAPH
+//------------------------------------------------------------------------------
+// Restore the Body to its state prior to generateGraph()
+void MultibodyGraphMaker::Body::forgetGraph(MultibodyGraphMaker& graph)
+{
+ level = -1;
+ mobilizer = -1;
+ master = -1;
+ slaves.clear();
+}
+
+//------------------------------------------------------------------------------
+// FORGET GRAPH
+//------------------------------------------------------------------------------
+// Restore the Joint to its state prior to generateGraph()
+bool MultibodyGraphMaker::Joint::forgetGraph(MultibodyGraphMaker& graph)
+{
+ if (isAddedBaseJoint) {
+ graph.deleteJoint(name);
+ return true;
+ }
+ mobilizer = -1;
+ loopConstraint = -1;
+ return false;
+}
+} // namespace SimTK
+
diff --git a/SimTKmath/src/Simmath_f2c.h b/SimTKmath/src/Simmath_f2c.h
new file mode 100644
index 0000000..79a9748
--- /dev/null
+++ b/SimTKmath/src/Simmath_f2c.h
@@ -0,0 +1,249 @@
+#ifndef SimTK_SIMMATH_F2C_H_
+#define SimTK_SIMMATH_F2C_H_
+
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+
+/* f2c.h -- Standard Fortran to C header file */
+
+typedef int integer; /* awf changed from long */
+typedef const char *address;
+typedef short int shortint;
+typedef float real;
+typedef double doublereal;
+/*typedef struct { real r, i; } complex; */
+typedef struct { doublereal r, i; } doublecomplex;
+typedef int logical; /* awf changed from long */
+typedef short int shortlogical;
+typedef char logical1;
+typedef char integer1;
+
+#define TRUE_ (1)
+#define FALSE_ (0)
+
+/* Extern is for use with -E */
+#ifndef Extern
+#define Extern extern
+#endif
+
+/* I/O stuff */
+
+#ifdef f2c_i2
+/* for -i2 */
+typedef short flag;
+typedef short ftnlen;
+typedef short ftnint;
+#else
+typedef long flag;
+typedef long ftnlen;
+typedef long ftnint;
+#endif
+
+/*external read, write*/
+typedef struct
+{
+ flag cierr;
+ ftnint ciunit;
+ flag ciend;
+ char *cifmt;
+ ftnint cirec;
+} cilist;
+
+/*internal read, write*/
+typedef struct
+{
+ flag icierr;
+ char *iciunit;
+ flag iciend;
+ char *icifmt;
+ ftnint icirlen;
+ ftnint icirnum;
+} icilist;
+
+/*open*/
+typedef struct
+{
+ flag oerr;
+ ftnint ounit;
+ char *ofnm;
+ ftnlen ofnmlen;
+ char *osta;
+ char *oacc;
+ char *ofm;
+ ftnint orl;
+ char *oblnk;
+} olist;
+
+/*close*/
+typedef struct
+{
+ flag c_err;
+ ftnint cunit;
+ char *csta;
+} cllist;
+
+/*rewind, backspace, endfile*/
+typedef struct
+{
+ flag aerr;
+ ftnint aunit;
+} alist;
+
+/* inquire */
+typedef struct
+{
+ flag inerr;
+ ftnint inunit;
+ char *infile;
+ ftnlen infilen;
+ ftnint *inex; /*parameters in standard's order*/
+ ftnint *inopen;
+ ftnint *innum;
+ ftnint *innamed;
+ char *inname;
+ ftnlen innamlen;
+ char *inacc;
+ ftnlen inacclen;
+ char *inseq;
+ ftnlen inseqlen;
+ char *indir;
+ ftnlen indirlen;
+ char *infmt;
+ ftnlen infmtlen;
+ char *inform;
+ ftnint informlen;
+ char *inunf;
+ ftnlen inunflen;
+ ftnint *inrecl;
+ ftnint *innrec;
+ char *inblank;
+ ftnlen inblanklen;
+} inlist;
+
+#define VOID void
+
+union Multitype { /* for multiple entry points */
+ shortint h;
+ integer i;
+ real r;
+ doublereal d;
+// complex c;
+ doublecomplex z;
+ };
+
+typedef union Multitype Multitype;
+
+typedef long Long; /* No longer used; formerly in Namelist */
+
+struct Vardesc { /* for Namelist */
+ char *name;
+ char *addr;
+ ftnlen *dims;
+ int type;
+ };
+typedef struct Vardesc Vardesc;
+
+struct Namelist {
+ char *name;
+ Vardesc **vars;
+ int nvars;
+ };
+typedef struct Namelist Namelist;
+
+#ifdef __cplusplus
+typedef doublereal (*D_fp)(...), (*E_fp)(...);
+typedef int /* Unknown procedure type */ (*U_fp)(...);
+#else
+typedef doublereal (*D_fp)(), (*E_fp)();
+typedef int /* Unknown procedure type */ (*U_fp)();
+#endif
+
+#ifndef IUE /* These are not used in netlib, and cause the gcc compiler warning
+ "function declaration isn't a prototype" */
+/*
+// d.capel at 2d3.com - Actually, they are used in many netlib functions,
+// just not any that are included in v3p/netlib (yet). However, I am
+// at liberty to use those netlib routines in my own code, and I
+// therefore require that the f2c.h seen by my compiler not be
+// broken. I don't think I should need to have two different versions
+// of f2c.h lying around in order to facilitate this.
+*/
+
+/* procedure parameter types for -A and -C++ */
+
+#define F2C_proc_par_types 1
+#ifdef __cplusplus
+typedef shortint (*J_fp)(...);
+typedef integer (*I_fp)(...);
+typedef real (*R_fp)(...);
+typedef /* Complex */ VOID (*C_fp)(...);
+typedef /* Double Complex */ VOID (*Z_fp)(...);
+typedef logical (*L_fp)(...);
+typedef shortlogical (*K_fp)(...);
+typedef /* Character */ VOID (*H_fp)(...);
+typedef /* Subroutine */ int (*S_fp)(...);
+#else
+typedef shortint (*J_fp)(void);
+typedef integer (*I_fp)(void);
+typedef real (*R_fp)(void);
+typedef /* Complex */ VOID (*C_fp)(void);
+typedef /* Double Complex */ VOID (*Z_fp)(void);
+typedef logical (*L_fp)(void);
+typedef shortlogical (*K_fp)(void);
+typedef /* Character */ VOID (*H_fp)(void);
+typedef /* Subroutine */ int (*S_fp)(void);
+#endif
+/* E_fp is for real functions when -R is not specified */
+typedef VOID C_f; /* complex function */
+typedef VOID H_f; /* character function */
+typedef VOID Z_f; /* double complex function */
+typedef doublereal E_f; /* real function with -R not specified */
+
+#endif /* not used */
+
+/* undef any lower-case symbols that your C compiler predefines, e.g.: */
+
+#ifndef Skip_f2c_Undefs
+#undef cray
+#undef gcos
+#undef mc68010
+#undef mc68020
+#undef mips
+#undef pdp11
+#undef sgi
+#undef sparc
+#undef sun
+#undef sun2
+#undef sun3
+#undef sun4
+#undef u370
+#undef u3b
+#undef u3b2
+#undef u3b5
+#ifndef como4301 /* Comeau C++ does not allow #undef of "unix" */
+#undef unix
+#endif
+#undef vax
+#endif
+#endif // SimTK_SIMMATH_F2C_H_
diff --git a/SimTKmath/staticTarget/CMakeLists.txt b/SimTKmath/staticTarget/CMakeLists.txt
new file mode 100644
index 0000000..5b0b341
--- /dev/null
+++ b/SimTKmath/staticTarget/CMakeLists.txt
@@ -0,0 +1,78 @@
+## This whole directory exists just so I could define these extra
+## preprocessor values.
+
+ADD_DEFINITIONS(-DSimTK_SIMMATH_BUILDING_STATIC_LIBRARY
+ -DSimTK_USE_STATIC_LIBRARIES)
+
+#
+# Set up file groups for better browsing in Visual Studio.
+#
+
+# Override Api headers to put them in the Api group.
+source_group("Header Files\\Api" FILES ${API_ABS_INCLUDE_FILES})
+
+# Now put things in their specified groups.
+if (SOURCE_GROUPS)
+ list(LENGTH SOURCE_GROUPS NGROUPS)
+ math(EXPR lastgrpnum ${NGROUPS}-1)
+ foreach( grpnum RANGE 0 ${lastgrpnum} )
+ list(GET SOURCE_GROUPS ${grpnum} grp)
+ list(GET SOURCE_GROUP_FILES ${grpnum} grpfile)
+ source_group("${grp}" FILES "${grpfile}")
+ endforeach()
+endif()
+
+# These are defaults for files that don't otherwise get mentioned.
+source_group("Source Files\\Others" FILES ${SOURCE_FILES})
+source_group("Header Files\\Others" FILES ${SOURCE_INCLUDE_FILES})
+
+IF(BUILD_UNVERSIONED_LIBRARIES)
+
+ ADD_LIBRARY(${STATIC_TARGET} STATIC
+ ${SOURCE_FILES} ${SOURCE_INCLUDE_FILES}
+ ${API_ABS_INCLUDE_FILES})
+
+ TARGET_LINK_LIBRARIES(${STATIC_TARGET}
+ debug ${SimTKCOMMON_STATIC_LIBRARY}_d
+ optimized ${SimTKCOMMON_STATIC_LIBRARY}
+ ${MATH_LIBS_TO_USE})
+
+ SET_TARGET_PROPERTIES(${STATIC_TARGET} PROPERTIES
+ PROJECT_LABEL "Code - ${STATIC_TARGET}")
+
+ # install library; on Windows both .lib and .dll go in the lib directory.
+ INSTALL(TARGETS ${STATIC_TARGET}
+ PERMISSIONS OWNER_READ OWNER_WRITE
+ GROUP_READ GROUP_WRITE
+ WORLD_READ
+ ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ RUNTIME DESTINATION ${CMAKE_INSTALL_FULL_BINDIR})
+
+ENDIF(BUILD_UNVERSIONED_LIBRARIES)
+
+
+IF(BUILD_VERSIONED_LIBRARIES)
+
+ ADD_LIBRARY(${STATIC_TARGET_VN} STATIC
+ ${SOURCE_FILES} ${SOURCE_INCLUDE_FILES}
+ ${API_ABS_INCLUDE_FILES})
+
+ TARGET_LINK_LIBRARIES(${STATIC_TARGET_VN}
+ debug ${SimTKCOMMON_STATIC_LIBRARY_VN}_d
+ optimized ${SimTKCOMMON_STATIC_LIBRARY_VN}
+ ${MATH_LIBS_TO_USE_VN})
+
+ SET_TARGET_PROPERTIES(${STATIC_TARGET_VN} PROPERTIES
+ PROJECT_LABEL "Code - ${STATIC_TARGET_VN}")
+
+ # install library; on Windows both .lib and .dll go in the lib directory.
+ INSTALL(TARGETS ${STATIC_TARGET_VN}
+ PERMISSIONS OWNER_READ OWNER_WRITE
+ GROUP_READ GROUP_WRITE
+ WORLD_READ
+ ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ RUNTIME DESTINATION ${CMAKE_INSTALL_FULL_BINDIR})
+
+ENDIF(BUILD_VERSIONED_LIBRARIES)
diff --git a/SimTKmath/tests/BestAvailableTest.cpp b/SimTKmath/tests/BestAvailableTest.cpp
new file mode 100644
index 0000000..be14361
--- /dev/null
+++ b/SimTKmath/tests/BestAvailableTest.cpp
@@ -0,0 +1,256 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-13 Stanford University and the Authors. *
+ * Authors: Chris Dembia *
+ * Contributors: Jack Middleton *
+ * *
+ * 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 <iostream>
+using std::cout;
+using std::endl;
+using SimTK::Vector;
+using SimTK::Matrix;
+using SimTK::Real;
+using SimTK::Optimizer;
+using SimTK::OptimizerSystem;
+
+/* This test doesn't actually run any optimizations. It simply creates an
+ * Optimizer, using the same OptimizerSystem's (not the EXACT same) used in the
+ * tests for each of the algorithms. NOTE: Since BestAvailable never selects
+ * CFSQP, we do not test for CFSQP.
+ */
+
+/* See IpoptTest. 'BestAvailable' will select Ipopt because this sytem has
+ * constraints.
+ */
+class IpoptSystem : public OptimizerSystem {
+public:
+
+
+ int objectiveFunc( const Vector &coefficients, bool new_coefficients,
+ Real& f ) const {
+ const Real *x;
+
+ x = &coefficients[0];
+
+ f = x[0] * x[3] * (x[0] + x[1] + x[2]) + x[2];
+ return( 0 );
+ }
+
+ int gradientFunc( const Vector &coefficients, bool new_coefficients,
+ Vector &gradient ) const{
+ const Real *x;
+
+ x = &coefficients[0];
+
+ gradient[0] = x[0] * x[3] + x[3] * (x[0] + x[1] + x[2]);
+ gradient[1] = x[0] * x[3];
+ gradient[2] = x[0] * x[3] + 1;
+ gradient[3] = x[0] * (x[0] + x[1] + x[2]);
+
+ return(0);
+
+ }
+
+ int constraintFunc( const Vector &coefficients, bool new_coefficients,
+ Vector &constraints ) const{
+ const Real *x;
+
+ x = &coefficients[0];
+ constraints[0] = x[0]*x[0] + x[1]*x[1] + x[2]*x[2] + x[3]*x[3] - 40.0;
+ constraints[1] = x[0] * x[1] * x[2] * x[3] - 25.0;
+
+ return(0);
+ }
+
+ int constraintJacobian( const Vector& coefficients, bool new_coefficients,
+ Matrix& jac ) const{
+ const Real *x;
+
+ x = &coefficients[0];
+
+ jac(0,0) = 2*x[0];
+ jac(0,1) = 2*x[1];
+ jac(0,2) = 2*x[2];
+ jac(0,3) = 2*x[3];
+ jac(1,0) = x[1]*x[2]*x[3];
+ jac(1,1) = x[0]*x[2]*x[3];
+ jac(1,2) = x[0]*x[1]*x[3];
+ jac(1,3) = x[0]*x[1]*x[2];
+
+ return(0);
+ }
+
+
+ IpoptSystem() : OptimizerSystem( 4 )
+ {
+ setNumEqualityConstraints( 1 );
+ setNumInequalityConstraints( 1 );
+ }
+
+};
+
+
+/* See LBFGSBTest.cpp. 'BestAvailable' will select LBFGSB because this system
+ * does NOT have constraints and does have parameter limits.
+ */
+class LBFGSBSystem : public OptimizerSystem {
+
+ public:
+
+ LBFGSBSystem() : OptimizerSystem( 25 ) {}
+
+ int objectiveFunc( const Vector &coefficients, bool new_coefficients, Real& f ) const {
+ int i;
+
+ const Real *x = &coefficients[0];
+
+ f = .25 *(x[0]-1.0)*(x[0]-1.0);
+ for(i=1;i<getNumParameters();i++) {
+ f = f + pow(x[i]-x[i-1]*x[i-1], 2.0);
+ }
+
+ f = 4.0* f;
+ return( 0 );
+ }
+
+ int gradientFunc( const Vector &coefficients, bool new_coefficients, Vector &gradient ) const {
+ const Real *x;
+ Real t1,t2;
+ int i;
+
+ x = &coefficients[0];
+
+ t1 = x[1]-(x[0]*x[0]);
+ gradient[0] = 2.0*(x[0]-1.0)-16.0*x[0]*t1;
+ for(i=1;i<getNumParameters()-1;i++) {
+ t2=t1;
+ t1=x[i+1]-(x[i]*x[i]);
+ gradient[i]=8.0*t2-16.0*x[i]*t1;
+ }
+ gradient[getNumParameters()-1]=8.0*t1;
+
+ return(0);
+
+ }
+
+};
+
+
+/* See LBFGSTest.cpp. 'BestAvailable' will select LBFGS because this system
+ * does NOT have constraints and does NOT have parameter limits.
+ */
+class LBFGSSystem : public OptimizerSystem {
+ public:
+
+ LBFGSSystem() : OptimizerSystem( 2 ) {}
+
+ int objectiveFunc( const Vector &coefficients, bool new_coefficients,
+ Real& f ) const {
+
+ const Real x = coefficients[0];
+ const Real y = coefficients[1];
+
+ f = 0.5*(3*x*x+4*x*y+6*y*y) - 2*x + 8*y;
+
+ return(0);
+
+ }
+
+ int gradientFunc( const Vector &coefficients, bool new_coefficients,
+ Vector &gradient ) const {
+
+ const Real x = coefficients[0];
+ const Real y = coefficients[1];
+
+ gradient[0] = 3*x + 2*y -2;
+ gradient[1] = 2*x + 6*y +8;
+
+ return(0);
+
+ }
+};
+
+
+int main() {
+
+ int i;
+
+ // IPOPT
+ // -----
+ /* create the system to be optimized */
+ IpoptSystem sysIPOPT;
+ Vector lowerBoundsIPOPT(sysIPOPT.getNumParameters());
+ Vector upperBoundsIPOPT(sysIPOPT.getNumParameters());
+ for(i=0;i<sysIPOPT.getNumParameters();i++) {
+ lowerBoundsIPOPT[i] = 1.0;
+ upperBoundsIPOPT[i] = 5.0;
+ }
+ sysIPOPT.setParameterLimits(lowerBoundsIPOPT, upperBoundsIPOPT);
+
+
+ // LBFGSB
+ // ------
+ LBFGSBSystem sysLBFGSB;
+ Vector lowerBoundsLBFGSB(sysLBFGSB.getNumParameters());
+ Vector upperBoundsLBFGSB(sysLBFGSB.getNumParameters());
+ for(i=0;i<sysLBFGSB.getNumParameters();i=i+2) { // even numbered
+ lowerBoundsLBFGSB[i] = 1.0;
+ upperBoundsLBFGSB[i] = 100.0;
+ }
+ for(i=1;i<sysLBFGSB.getNumParameters();i=i+2) { // odd numbered
+ lowerBoundsLBFGSB[i] = -100.0;
+ upperBoundsLBFGSB[i] = 100.0;
+ }
+ sysLBFGSB.setParameterLimits( lowerBoundsLBFGSB, upperBoundsLBFGSB );
+
+
+ // LBFSGB
+ // ------
+ LBFGSSystem sysLBFGS;
+
+
+ int returnValue = 0; // assume success
+
+ try {
+
+ // Optimizer will choose the BestAvailable algorithm.
+ Optimizer optIPOPT( sysIPOPT );
+ if (optIPOPT.getAlgorithm() != SimTK::InteriorPoint)
+ { returnValue = 1; }
+
+ Optimizer optLBFGSB( sysLBFGSB );
+ if (optLBFGSB.getAlgorithm() != SimTK::LBFGSB)
+ { returnValue = 1; }
+
+ Optimizer optLBFGS( sysLBFGS );
+ if (optLBFGS.getAlgorithm() != SimTK::LBFGS)
+ { returnValue = 1; }
+
+ }
+ catch (const std::exception& e) {
+ std::cout << e.what() << std::endl;
+ returnValue = 1; // failure
+ printf("BestAvailableTest.cpp: Caught exception \n");
+ }
+
+ return( returnValue );
+}
diff --git a/SimTKmath/tests/CMakeLists.txt b/SimTKmath/tests/CMakeLists.txt
new file mode 100644
index 0000000..ca924ff
--- /dev/null
+++ b/SimTKmath/tests/CMakeLists.txt
@@ -0,0 +1,53 @@
+# Adhoc tests are those test or demo programs which are not intended,
+# or not ready, to be part of the regression suite.
+ADD_SUBDIRECTORY(adhoc)
+
+# Generate regression tests.
+#
+# This is boilerplate code for generating a set of executables, one per
+# .cpp file in a "tests" directory. These are for test cases which
+# have been engineered to be suitable as part of the nightly regression
+# test suite. Ideally, they know their correct answers and return
+# a simple thumbs-up/thumbs-down result, although some regressions
+# may be present and useful just to determine if they compile and
+# run to completion.
+#
+# For IDEs that can deal with PROJECT_LABEL properties (at least
+# Visual Studio) the projects for building each of these adhoc
+# executables will be labeled "Regr - TheTestName" if a file
+# TheTestName.cpp is found in this directory.
+#
+# We check the BUILD_TESTS_AND_EXAMPLES_{SHARED,STATIC} variables to determine
+# whether to build dynamically linked, statically linked, or both
+# versions of the executable.
+
+FILE(GLOB REGR_TESTS "*.cpp")
+FOREACH(TEST_PROG ${REGR_TESTS})
+ GET_FILENAME_COMPONENT(TEST_ROOT ${TEST_PROG} NAME_WE)
+
+ IF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+ # Link with shared library
+ ADD_EXECUTABLE(${TEST_ROOT} ${TEST_PROG})
+ SET_TARGET_PROPERTIES(${TEST_ROOT}
+ PROPERTIES
+ PROJECT_LABEL "Test_Regr - ${TEST_ROOT}")
+ TARGET_LINK_LIBRARIES(${TEST_ROOT}
+ ${TEST_SHARED_TARGET})
+ ADD_TEST(${TEST_ROOT} ${EXECUTABLE_OUTPUT_PATH}/${TEST_ROOT})
+ ENDIF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+
+ IF (BUILD_STATIC_LIBRARIES AND BUILD_TESTS_AND_EXAMPLES_STATIC)
+ # Link with static library
+ SET(TEST_STATIC ${TEST_ROOT}Static)
+ ADD_EXECUTABLE(${TEST_STATIC} ${TEST_PROG})
+ SET_TARGET_PROPERTIES(${TEST_STATIC}
+ PROPERTIES
+ COMPILE_FLAGS "-DSimTK_USE_STATIC_LIBRARIES"
+ PROJECT_LABEL "Test_Regr - ${TEST_STATIC}")
+ TARGET_LINK_LIBRARIES(${TEST_STATIC}
+ ${TEST_STATIC_TARGET})
+ ADD_TEST(${TEST_STATIC} ${EXECUTABLE_OUTPUT_PATH}/${TEST_STATIC})
+ ENDIF()
+
+ENDFOREACH(TEST_PROG ${REGR_TESTS})
+
diff --git a/SimTKmath/tests/CPodesIntegratorTest.cpp b/SimTKmath/tests/CPodesIntegratorTest.cpp
new file mode 100644
index 0000000..659476a
--- /dev/null
+++ b/SimTKmath/tests/CPodesIntegratorTest.cpp
@@ -0,0 +1,82 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "IntegratorTestFramework.h"
+#include "simmath/CPodesIntegrator.h"
+
+int main () {
+ try {
+ PendulumSystem sys;
+ sys.addEventHandler(new ZeroVelocityHandler(sys));
+ sys.addEventHandler(PeriodicHandler::handler = new PeriodicHandler());
+ sys.addEventHandler(new ZeroPositionHandler(sys));
+ sys.addEventReporter(PeriodicReporter::reporter = new PeriodicReporter(sys));
+ sys.addEventReporter(new OnceOnlyEventReporter());
+ sys.addEventReporter(new DiscontinuousReporter());
+ sys.realizeTopology();
+
+ // Test with various intervals for the event handler and event reporter, ones that are either
+ // large or small compared to the expected internal step size of the integrator.
+
+ for (int i = 0; i < 4; ++i) {
+ PeriodicHandler::handler->setEventInterval(i == 0 || i == 1 ? 0.01 : 2.0);
+ PeriodicReporter::reporter->setEventInterval(i == 0 || i == 2 ? 0.015 : 1.5);
+
+ // Test the BDF integrator in both normal and single step modes.
+
+ CPodesIntegrator bdfInteg(sys, CPodes::BDF);
+ testIntegrator(bdfInteg, sys);
+ bdfInteg.setReturnEveryInternalStep(true);
+ testIntegrator(bdfInteg, sys);
+
+ // Test the Adams integrator in both normal and single step modes.
+
+ CPodesIntegrator adamsInteg(sys, CPodes::Adams);
+ testIntegrator(adamsInteg, sys);
+ adamsInteg.setReturnEveryInternalStep(true);
+ testIntegrator(adamsInteg, sys);
+
+ // Calling setUseCPodesProjection() after the integrator has been initialized should
+ // produce an exception.
+
+ try {
+ adamsInteg.setUseCPodesProjection();
+ assert(false);
+ }
+ catch (...) {
+ }
+
+ // Try having CPODES do the projection instead of the System.
+
+ CPodesIntegrator projInteg(sys, CPodes::BDF);
+ projInteg.setUseCPodesProjection();
+ testIntegrator(projInteg, sys);
+ }
+ cout << "Done" << endl;
+ return 0;
+ }
+ catch (std::exception& e) {
+ std::printf("FAILED: %s\n", e.what());
+ return 1;
+ }
+}
diff --git a/SimTKmath/tests/DifferentiatorTest.cpp b/SimTKmath/tests/DifferentiatorTest.cpp
new file mode 100644
index 0000000..d0c8af3
--- /dev/null
+++ b/SimTKmath/tests/DifferentiatorTest.cpp
@@ -0,0 +1,354 @@
+
+/* -------------------------------------------------------------------------- *
+ * SimTK Simbody: SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This is a test program which uses the Differentiator class in various ways.
+ */
+
+#include "SimTKmath.h"
+
+// Just so we can get the version number:
+#include "SimTKlapack.h"
+
+#include <cstdio>
+#include <iostream>
+
+using SimTK::Real;
+using SimTK::Vector;
+using SimTK::Matrix;
+using SimTK::Differentiator;
+using std::printf;
+using std::cout;
+using std::endl;
+
+// This is a system of functions of a particular set of parameters (state).
+// The underlying function wants time also, so we provide that as data in
+// the concrete class. Time should be set prior to calculation of the Jacobian.
+class MyVectorFunc : public Differentiator::JacobianFunction {
+public:
+ MyVectorFunc(int nf, int ny)
+ : Differentiator::JacobianFunction(nf,ny), time(0) { }
+
+ void setTime(Real t) {time=t;}
+ Real getTime() const {return time;}
+
+ // Must provide this pure virtual function.
+ int f(const Vector& y, Vector& fy) const;
+private:
+ Real time;
+};
+
+// This is a single scalar function of a vector of parameters.
+class MyObjectiveFunc : public Differentiator::GradientFunction {
+public:
+ MyObjectiveFunc(int ny)
+ : Differentiator::GradientFunction(ny), time(0) { }
+
+ void setTime(Real t) {time=t;}
+ Real getTime() const {return time;}
+
+ // Must provide this pure virtual function.
+ int f(const Vector& y, Real& fy) const;
+private:
+ Real time;
+};
+
+// This represents a generic scalar function of a scalar parameter,
+// where the actual function has a simple C signature.
+class GenericScalarFunc : public Differentiator::ScalarFunction {
+ typedef Real (*CFunc)(Real);
+public:
+ GenericScalarFunc(CFunc cf)
+ : Differentiator::ScalarFunction(), cp(cf) { }
+
+ // Must provide this pure virtual function.
+ int f(Real x, Real& fx) const {
+ fx = cp(x);
+ return 0;
+ }
+
+ CFunc cp;
+};
+
+#define PREC double
+class SinOmegaX : public Differentiator::ScalarFunction {
+public:
+ SinOmegaX(Real omega, Real acc)
+ : ScalarFunction(acc), w(omega)
+ {
+ }
+
+ Real calc(Real x) const {
+ return std::sin(w*x);
+ }
+ Real calcD1(Real x) const {
+ return w*std::cos(w*x);
+ }
+ Real calcD2(Real x) const {
+ return -w*w*std::sin(w*x);
+ }
+ // Must provide this virtual function.
+ int f(Real x, Real& fx) const {
+ volatile PREC ffx = (PREC)calc(x);
+ fx = ffx;
+ return 0; // success
+ }
+private:
+ const Real w;
+};
+
+class Cubic : public Differentiator::ScalarFunction {
+public:
+ // ax^3+bx^2+cx+d
+ Cubic(Real aa, Real bb, Real cc, Real dd, Real acc)
+ : ScalarFunction(acc), a(aa),b(bb),c(cc),d(dd)
+ {
+ }
+
+ Real calc(Real x) const {
+ return (a*x*x*x + b*x*x + c*x + d)*std::exp(a*x);
+ }
+
+ Real calcD1(Real x) const {
+ return (3*a*x*x+2*b*x+c)*std::exp(a*x) + a*calc(x);
+ }
+
+ Real calcD2(Real x) const {
+ return (6*a*x+2*b)*std::exp(a*x) + (3*a*x*x+2*b*x+c)*a*std::exp(a*x)
+ + a*calcD1(x);
+ }
+
+ // Must provide this virtual function.
+ int f(Real x, Real& fx) const {
+ volatile PREC ffx = (PREC)calc(x);
+ fx = ffx;
+ return 0; // success
+ }
+private:
+ const Real a, b, c, d;
+};
+
+
+
+static void doSinOmegaExample() {
+ for (int digits=0; digits<=41; ++digits) {
+ Real acc;
+ if (digits < 40) acc = std::pow(10., -(digits/(1.5*sizeof(double)/sizeof(PREC))));
+ else if (digits==40) acc=SimTK::NTraits<PREC>::getSignificant();
+ else if (digits==41) acc=SimTK::NTraits<PREC>::getEps();
+
+ const Real w = .01;
+ SinOmegaX func(w, acc);
+ //Cubic func(-1,-2,3,4, acc);
+ Differentiator dsin3x(func);
+
+ const int NEntries = 1000;
+ const Real offs =0.1;
+ const Real increment = (Real)SimTK_PI/NEntries;
+ //printf("%8s %12s %12s %12s\n",
+ // "x", "3cos3x", "err1", "err2");
+
+ Real err1rms=0, err1max=0, err2rms=0, err2max=0;
+ for (int i=0; i < NEntries; ++i) {
+ const Real x = offs + i*increment;
+ if (digits==0) printf("%12g %12g %12g\n", func.calc(x), func.calcD1(x), func.calcD2(x));
+ const Real analytic = func.calcD1(x);
+ const Real approx1st = dsin3x.calcDerivative(x);
+ const Real approx2nd = dsin3x.calcDerivative(x, Differentiator::CentralDifference);
+ const Real err1 = std::abs((approx1st-analytic)/analytic);
+ const Real err2 = std::abs((approx2nd-analytic)/analytic);
+ err1rms += err1*err1; err2rms += err2*err2;
+ if (err1 > err1max) err1max=err1;
+ if (err2 > err2max) err2max=err2;
+ //printf("%8g %12g %12.3e %12.3e\n",
+ // x, analytic, err1, err2);
+ }
+ printf("%.3e: err1: max=%.3e, rms=%.3e err2: max=%.3e, rms=%.3e\n",
+ acc, err1max, std::sqrt(err1rms/NEntries), err2max, std::sqrt(err2rms/NEntries));
+ }
+
+};
+
+
+static Real mysin(Real x) {
+ return std::sin(x);
+}
+static Real mycos(Real x) {
+ return std::cos(x);
+}
+
+int main() {
+ doSinOmegaExample();
+
+ Vector yy, yp;
+
+ yy.resize(4);
+ yp.resize(4);
+
+ /* Initialize y */
+ yy[0] = 1.0; /* x */
+ yy[1] = 0.0; /* y */
+ yy[2] = 0.0; /* xd */
+ yy[3] = 0.0; /* yd */
+
+ // Define a scalar, vector, and system of functions.
+ GenericScalarFunc gf(mysin);
+ MyObjectiveFunc sf(4);
+ MyVectorFunc vf(4,4);
+ vf.setEstimatedAccuracy(1e-6); // claim reduced accuracy (6 digits)
+
+ // Create differentiators for each of the functions.
+ Differentiator dsin(gf,Differentiator::CentralDifference); // use calcDerivative()
+ Differentiator gradf(sf); // use calcGradient()
+ Differentiator df(vf); // use calcJacobian()
+
+ int returnValue = 0; // assume success
+ try {
+ gradf.setDefaultMethod(Differentiator::ForwardDifference);
+ df.setDefaultMethod(Differentiator::UnspecifiedMethod);
+
+ printf("dsin default method: %s\n", Differentiator::getMethodName(dsin.getDefaultMethod()));
+ printf("gradf default method: %s\n", Differentiator::getMethodName(gradf.getDefaultMethod()));
+ printf("df default method: %s\n", Differentiator::getMethodName(df.getDefaultMethod()));
+
+ const Real ang = SimTK_PI/8;
+ cout << "sin(x)=" << mysin(ang) << endl;
+ cout << "d sin(x)/dx=" << dsin.calcDerivative(ang) << endl;
+ cout << "cos(x)=" << mycos(ang) << endl;
+
+ const Real rp[] = {.01,.02,.03,-.14};
+ Vector delta_y(4,rp);
+ Vector y0 = yy+delta_y; // don't start right at 0
+
+
+ printf("Func gf: nf=%d np=%d estacc=%g\n",
+ gf.getNumFunctions(), gf.getNumParameters(), gf.getEstimatedAccuracy());
+ printf("Func sf: nf=%d np=%d estacc=%g\n",
+ sf.getNumFunctions(), sf.getNumParameters(), sf.getEstimatedAccuracy());
+ printf("Func vf: nf=%d np=%d estacc=%g\n",
+ vf.getNumFunctions(), vf.getNumParameters(), vf.getEstimatedAccuracy());
+
+
+ Real sfy0, sfyd;
+ sf.f(y0, sfy0); // calculate unperturbed value
+ Vector grad1;
+ gradf.calcGradient(y0, sfy0, grad1);
+
+ Vector grad2;
+ gradf.calcGradient(y0, sfy0, grad2, Differentiator::CentralDifference);
+ cout << "sf(y0)=" << sfy0 << endl;
+ cout << "order 1 grad(sf)=" << grad1 << endl;
+ cout << "order 2 grad(sf)=" << grad2 << endl;
+
+ sf.f(y0+delta_y, sfyd);
+ cout << "sf(y0+dy)=" << sfyd << endl;
+ cout << "sf(y0)+~grad1*dy=" << sfy0 + ~grad1*delta_y << endl;
+ cout << "err @order1=" << std::abs(sfyd-(sfy0 + ~grad1*delta_y)) << endl;
+ cout << "err @order2=" << std::abs(sfyd-(sfy0 + ~grad2*delta_y)) << endl;
+
+ vf.f(y0, yp);
+ Matrix dfdy;
+ df.calcJacobian(y0,yp,dfdy);
+ const Matrix dfdy2 = df.calcJacobian(y0, Differentiator::CentralDifference);
+
+ cout << "vf(" << y0 << ")=" << yp << endl;
+ cout << "order " << df.getDefaultMethod()
+ << " df/dy=" << dfdy;
+ cout << "2nd order dfdy: " << dfdy2;
+ Vector yp2(4);
+ vf.f(y0+2*delta_y, yp2);
+ cout << "f(y0+dy)=" << yp2 << endl;
+ cout << "1 f(y0)+(df/dy)dy=" << yp+dfdy*2*delta_y << endl;
+ cout << "2 f(y0)+(f/dy)dy=" << yp+dfdy2*2*delta_y << endl;
+ cout << std::setprecision(16);
+ cout << "1 err=" << (yp2-(yp+dfdy*2*delta_y)).norm() << endl;
+ cout << "2 err=" << (yp2-(yp+dfdy2*2*delta_y)).norm() << endl;
+ }
+ catch (const std::exception& e) {
+ std::cout << e.what() << std::endl;
+ returnValue = 1; // failure
+ }
+
+ printf("dsin stats: ndiffs=%d nfail=%d nfcalls=%d\n",
+ dsin.getNumDifferentiations(), dsin.getNumDifferentiationFailures(),
+ dsin.getNumCallsToUserFunction());
+ printf("gradf stats: ndiffs=%d nfail=%d nfcalls=%d\n",
+ gradf.getNumDifferentiations(), gradf.getNumDifferentiationFailures(),
+ gradf.getNumCallsToUserFunction());
+ printf("df stats: ndiffs=%d nfail=%d nfcalls=%d\n",
+ df.getNumDifferentiations(), df.getNumDifferentiationFailures(),
+ df.getNumCallsToUserFunction());
+
+ printf("gf stats: nCalls=%d, nFailures=%d\n",
+ gf.getNumCalls(), gf.getNumFailures());
+ printf("sf stats: nCalls=%d, nFailures=%d\n",
+ sf.getNumCalls(), sf.getNumFailures());
+ printf("vf stats: nCalls=%d, nFailures=%d\n",
+ vf.getNumCalls(), vf.getNumFailures());
+
+ vf.resetAllStatistics();
+ printf("vf after reset: nCalls=%d, nFailures=%d\n",
+ vf.getNumCalls(), vf.getNumFailures());
+
+ return returnValue;
+}
+
+// Here is our system of equations, representing a pendulum. We'll
+// use the system as-is for calculating a Jacobian, and use its
+// norm for testing gradient calculation.
+static int pendODE(Real t, const Vector& yy, Vector& fy)
+{
+ Real x, y, xd, yd, g, tmp;
+
+ g = 13.7503716373294544;
+
+ x = yy[0];
+ y = yy[1];
+ xd = yy[2];
+ yd = yy[3];
+
+ tmp = xd*xd + yd*yd - g*y;
+
+ fy[0] = xd;
+ fy[1] = yd;
+ fy[2] = -x*tmp;
+ fy[3] = -y*tmp - g;
+
+ return 0;
+}
+
+// Implement the virtual method for a JacobianFunction.
+int MyVectorFunc::f(const Vector& yy, Vector& fy) const
+{
+ return pendODE(getTime(), yy, fy);
+}
+
+// Implement the virtual function for a GradientFunction.
+int MyObjectiveFunc::f(const Vector& yy, Real& fy) const
+{
+ Vector tmp(4);
+ const int res = pendODE(getTime(), yy, tmp);
+ fy = tmp.norm();
+ return res;
+}
diff --git a/SimTKmath/tests/EigenTest.cpp b/SimTKmath/tests/EigenTest.cpp
new file mode 100644
index 0000000..c33c79a
--- /dev/null
+++ b/SimTKmath/tests/EigenTest.cpp
@@ -0,0 +1,309 @@
+/* -------------------------------------------------------------------------- *
+ * SimTK Simbody: SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This is a test program which uses the Eigen class to compute
+ * eigen values and eigen vectors
+ */
+
+/*
+The data for this test is from an example FORTRAN program from the
+Numerical Algorithms Group (NAG)
+URL:http://www.nag.com/lapack-ex/lapack-ex.html
+
+
+Solves for the eigen valus and vectors for the
+following system
+
+ Ax = 0 where :
+
+
+
+ 0.35 0.45 -0.14 -0.17
+ 0.09 0.07 -0.54 0.35
+A = -0.44 -0.33 -0.03 0.17
+ 0.25 -0.32 -0.13 0.11
+
+
+
+SOLUTION =
+reciprocal condition number = 9.9E-01
+ Error bound = 1.3E-16
+
+ Eigenvector( 1)
+ -6.5509E-01
+ -5.2363E-01
+ 5.3622E-01
+ -9.5607E-02
+
+ Reciprocal condition number = 8.2E-01
+ Error bound = 1.6E-16
+
+ Eigenvalue( 2) = (-9.9412E-02, 4.0079E-01)
+
+ Reciprocal condition number = 7.0E-01
+ Error bound = 1.8E-16
+
+ Eigenvector( 2)
+ (-1.9330E-01, 2.5463E-01)
+ ( 2.5186E-01,-5.2240E-01)
+ ( 9.7182E-02,-3.0838E-01)
+ ( 6.7595E-01, 0.0000E+00)
+
+ Reciprocal condition number = 4.0E-01
+ Error bound = 3.3E-16
+
+ Eigenvalue( 3) = (-9.9412E-02,-4.0079E-01)
+
+ Reciprocal condition number = 7.0E-01
+ Error bound = 1.8E-16
+
+ Eigenvector( 3)
+ (-1.9330E-01,-2.5463E-01)
+ ( 2.5186E-01, 5.2240E-01)
+ ( 9.7182E-02, 3.0838E-01)
+ ( 6.7595E-01,-0.0000E+00)
+
+ Reciprocal condition number = 4.0E-01
+ Error bound = 3.3E-16
+
+ Eigenvalue( 4) = -1.0066E-01
+
+ Reciprocal condition number = 5.7E-01
+ Error bound = 2.3E-16
+
+ Eigenvector( 4)
+ 1.2533E-01
+ 3.3202E-01
+ 5.9384E-01
+ 7.2209E-01
+
+ Reciprocal condition number = 3.1E-01
+ Error bound = 4.2E-16
+
+
+estimated rank = 4
+
+*/
+
+#include "SimTKmath.h"
+
+#include <cstdio>
+#include <cassert>
+#include <iostream>
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+static const double EPS = 0.00001;
+
+using namespace SimTK;
+
+using std::printf;
+using std::cout;
+using std::endl;
+
+Real A[16] = { 0.35, 0.45, -0.14, -0.17,
+ 0.09, 0.07, -0.54, 0.35,
+ -0.44, -0.33, -0.03, 0.17,
+ 0.25, -0.32, -0.13, 0.11 };
+
+typedef std::complex<double> cd;
+cd expEigen[4] = { cd(0.79948, 0.0),
+ cd(-0.099412, 0.40079),
+ cd(-0.099412, -0.40079),
+ cd(-0.10066, 0.0) };
+
+cd expVectors[16] = { cd( -.65509, 0.0), cd( -.52363, 0.0), cd( .53622, 0.0), cd( -.095607, 0.0),
+ cd(-.1933001, .25463), cd( .2518601, -.52240), cd( .09718202,-.30838), cd( .67595, 0.000),
+ cd(-.1933001, -.25463), cd( .2518601, .52240), cd( .09718202, .30838), cd( .67595, -0.000),
+ cd( .12533, 0.0), cd( .33202, 0.0), cd( .59384, 0.0), cd( .72209, 0.0) };
+ template <typename T>
+T absNormComplex( Vector_<std::complex<T> >& values, Vector_<std::complex<T> >& expected) {
+ T norm = 0;
+
+ for(int i=0;i<values.size(); i++ ) {
+ norm += (fabs(values(i).real()) - fabs(expected(i).real())) * (fabs(values(i).real()) - fabs(expected(i).real())) +
+ (fabs(values(i).imag()) - fabs(expected(i).imag())) * (fabs(values(i).imag()) - fabs(expected(i).imag()));
+ }
+
+ return( sqrt(norm) );
+}
+template <typename T>
+T absNorm( Vector_<T>& values, Vector_<T>& expected) {
+ T norm = 0;
+
+ for(int i=0;i<values.size(); i++ ) {
+ norm += (fabs(values(i)) - fabs(expected(i))) * (fabs(values(i)) - fabs(expected(i))) +
+ (fabs(values(i)) - fabs(expected(i))) * (fabs(values(i)) - fabs(expected(i)));
+ }
+ return( sqrt(norm) );
+}
+
+int main () {
+ double errnorm;
+ try {
+ // Default precision (Real, normally double) test.
+
+ Matrix a(4,4, A);
+ Vector_<std::complex<double> > expectedValues(4);
+ for(int i=0;i<4;i++) expectedValues[i] = expEigen[i];
+ Matrix_<std::complex<double> > expectedVectors(4,4);
+ for(int i=0;i<4;i++) for(int j=0;j<4;j++) expectedVectors(i,j) = expVectors[j*4+i];
+ Vector_<std::complex<double> > values; // should get sized automatically to 4 by getAllEigenValuesAndVectors()
+ Vector_<std::complex<double> > expectVec(4);
+ Vector_<std::complex<double> > computeVec(4);
+ Matrix_<std::complex<double> > vectors; // should get sized automatically to 4x4 by getAllEigenValuesAndVectors()
+
+ Eigen es(a); // setup the eigen system
+
+ es.getAllEigenValuesAndVectors( values, vectors ); // solve for the eigenvalues and eigenvectors of the system
+
+ printf("\n ***** NON-SYMMETRIC: ***** \n\n" );
+ cout << " Real SOLUTION: " << values << " errnorm=" << absNormComplex(values,expectedValues) << endl;
+ ASSERT(absNormComplex(values,expectedValues) < 0.001);
+
+ cout << "Vectors = " << endl;
+ for(int i=0;i<4;i++) {
+ computeVec = vectors(i);
+ expectVec = expectedVectors(i);
+
+ errnorm = absNormComplex( computeVec, expectVec );
+ cout << computeVec << " errnorm=" << errnorm << endl;
+ ASSERT( errnorm < 0.00001 );
+
+ }
+
+ cout << endl << endl;
+
+ Vector_<std::complex<float> > expectedValuesf(4);
+ Matrix_<std::complex<float> > expectedVectorsf(4,4);
+ Vector_<std::complex<float> > expectVecf(4);
+ Vector_<std::complex<float> > computeVecf(4);
+ Matrix_<std::complex<float> > vectorsf; // should get sized automatically to 4x4 by getAllEigenValuesAndVectors()
+ Vector_<std::complex<float> > valuesf; // should get sized automatically to 4 by getAllEigenValuesAndVectors()
+ Matrix_<float> af(4,4); for (int i=0; i<4; ++i) for (int j=0; j<4; ++j) af(i,j)=(float)a(i,j);
+
+ for(int i=0;i<4;i++) expectedValuesf[i] = (std::complex<float>)expEigen[i];
+ for(int i=0;i<4;i++) for(int j=0;j<4;j++) expectedVectorsf(i,j) = (std::complex<float>)expVectors[j*4+i];
+
+ Eigen esf(af); // setup the eigen system
+
+ esf.getAllEigenValuesAndVectors( valuesf, vectorsf); // solve for the eigenvalues and vectors of the system
+
+ cout << " float SOLUTION: " << valuesf << " errnorm=" << absNormComplex(valuesf,expectedValuesf) << endl;
+ ASSERT(absNormComplex(valuesf,expectedValuesf) < 0.001);
+
+ cout << "Vectors = " << endl;
+ for(int i=0;i<4;i++) {
+ computeVecf = vectorsf(i);
+ expectVecf = expectedVectorsf(i);
+
+ errnorm = absNormComplex( computeVecf, expectVecf );
+ cout << computeVecf << " errnorm=" << errnorm << endl;
+ ASSERT( errnorm < 0.0001 );
+ }
+ Real Z[4] = { 0.0, 0.0,
+ 0.0, 0.0 };
+
+ Matrix z(2,2, Z);
+ Eigen zeigen(z);
+ zeigen.getAllEigenValuesAndVectors( values, vectors);
+ cout << "Matrix of all zeros" << endl << "eigenvalues: " << values << endl;
+ cout << "Eigen vectors: " << endl;
+ for(int i=0;i<vectors.nrow();i++ ) {
+ cout << vectors(i) << endl;
+ }
+
+ Matrix_<double> z0;
+ Eigen z0Eigen(z0);
+ zeigen.getAllEigenValuesAndVectors( values, vectors);
+ cout << "Matrix of dimension 0,0" << endl << "eigenvalues: " << values << endl;
+ cout << "Eigen vectors: " << endl;
+ for(int i=0;i<vectors.nrow();i++ ) {
+ cout << vectors(i) << endl;
+ }
+
+/*
+//
+// SYMMETRIC TESTS:
+//
+ Real S[16] = { 1.0, 2.0, 3.0, 4.0,
+ 0.0, 2.0, 3.0, 4.0,
+ 0.0, 0.0, 3.0, 4.0,
+ 0.0, 0.0, 0.0, 4.0 };
+
+ Real expectSymVals[4] = { -2.0531, -0.5146, -0.2943, 12.8621 };
+ Real expectSymVecs[16] = { -0.7003, -0.5144, 0.2767, -0.4103,
+ -0.3592, 0.4851, -0.6634, -0.4422,
+ 0.1569, 0.5420, 0.6504, -0.5085,
+ 0.5965, -0.4543, -0.2457, -0.6144 };
+
+
+
+ Matrix as(4,4, S);
+ as.setMatrixStructure( MatrixStructures::Symmetric ) ;
+
+ Eigen esym(as); // setup the eigen system
+ Vector symValues;
+ Vector expectedSymValues(4);
+ for(int i=0;i<4;i++) expectedSymValues[i] = expectSymVals[i];
+ Matrix symVectors;
+ Vector symVector;
+ Vector expectedSymVector;
+ Matrix expectedSymVectors(4,4,expectSymVecs );
+ esym.getAllEigenValuesAndVectors( symValues, symVectors ); // solve for the eigenvalues and eigenvectors of the system
+ printf("\n ***** SYMMETRIC: ***** \n\n" );
+
+ cout << " Real SOLUTION: All Values and Vectors" << symValues << " errnorm=" << absNorm(symValues,expectedSymValues) << endl;
+
+ cout << " Eigen Vectors = " << endl;
+ for(int i=0;i<4;i++) {
+ symVector = symVectors(i);
+ expectedSymVector = expectedSymVectors(i);
+ errnorm = absNorm(symVector,expectedSymVector);
+ cout << symVectors(i) << " errnorm=" << errnorm << endl;
+// cout << expectedSymVectors(i) << endl;
+ }
+
+ Eigen efewi(as); // setup the eigen system
+ efewi.getFewEigenValuesAndVectors( symValues, symVectors, 2, 3 ); // solve for few eigenvalues and eigenvectors of the system
+ cout << " Real SOLUTION: Values/Vectors between indices 2 and 3" << symValues << endl;
+ for(int j=0;j<symVectors.ncol();j++) cout << symVectors(j) << endl;
+
+ Eigen efewr(as); // setup the eigen system
+
+ // solve for few eigenvalues/vectors between -1, 1
+ efewr.getFewEigenValuesAndVectors( symValues, symVectors, -1.0, 1.0 );
+ cout << " Real SOLUTION: Values/Vectors between -1, 1 " << symValues << endl;
+ for(int j=0;j<symVectors.ncol();j++) cout << symVectors(j) << endl;
+*/
+
+ return 0;
+ }
+ catch (std::exception& e) {
+ std::printf("FAILED: %s\n", e.what());
+ return 1;
+ }
+}
+
+
diff --git a/SimTKmath/tests/ExplicitEulerIntegratorTest.cpp b/SimTKmath/tests/ExplicitEulerIntegratorTest.cpp
new file mode 100644
index 0000000..d873f1a
--- /dev/null
+++ b/SimTKmath/tests/ExplicitEulerIntegratorTest.cpp
@@ -0,0 +1,53 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "IntegratorTestFramework.h"
+#include "simmath/ExplicitEulerIntegrator.h"
+
+int main () {
+ try {
+ PendulumSystem sys;
+ sys.addEventHandler(new ZeroVelocityHandler(sys));
+ sys.addEventHandler(PeriodicHandler::handler = new PeriodicHandler());
+ sys.addEventHandler(new ZeroPositionHandler(sys));
+ sys.addEventReporter(PeriodicReporter::reporter = new PeriodicReporter(sys));
+ sys.addEventReporter(new OnceOnlyEventReporter());
+ sys.addEventReporter(new DiscontinuousReporter());
+ sys.realizeTopology();
+ PeriodicHandler::handler->setEventInterval(0.01);
+ PeriodicReporter::reporter->setEventInterval(0.015);
+
+ // Test the integrator in both normal and single step modes.
+
+ ExplicitEulerIntegrator integ(sys);
+ testIntegrator(integ, sys, 1e-7);
+ integ.setReturnEveryInternalStep(true);
+ testIntegrator(integ, sys, 1e-7);
+ cout << "Done" << endl;
+ return 0;
+ }
+ catch (std::exception& e) {
+ std::printf("FAILED: %s\n", e.what());
+ return 1;
+ }
+}
diff --git a/SimTKmath/tests/FactorLUTest.cpp b/SimTKmath/tests/FactorLUTest.cpp
new file mode 100644
index 0000000..4bde498
--- /dev/null
+++ b/SimTKmath/tests/FactorLUTest.cpp
@@ -0,0 +1,158 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This is a test program which uses the FactorLU class to do an LU
+ * factorization on a system of linear equations and then use the
+ * factored LU matrix to solve for a particular right hand side
+ */
+
+/*
+The data for this test is from an example FORTRAN program from the
+Numerical Algorithms Group (NAG)
+URL:http://www.nag.com/lapack-ex/lapack-ex.html
+
+
+Solves:
+
+Ax = B,
+
+where A is the general matrix
+
+
+ 1.80 2.88 2.05 -0.89 9.52
+A = 5.25 -2.95 -0.95 -3.80 and B = 24.35
+ 1.58 -2.69 -2.90 -1.04 0.77
+ -1.11 -0.66 -0.59 0.80 -6.22
+
+
+ Solution
+ x = 1.0000 -1.0000 3.0000 -5.0000
+
+
+ LU factorization:
+ 1 2 3 4
+ 1 5.2500 -2.9500 -0.9500 -3.8000
+ 2 0.3429 3.8914 2.3757 0.4129
+ 3 0.3010 -0.4631 -1.5139 0.2948
+ 4 -0.2114 -0.3299 0.0047 0.1314
+
+ Pivot indices
+ 2 2 3 4
+
+*/
+
+#include "SimTKmath.h"
+
+#include <iostream>
+using std::cout; using std::endl;
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+using namespace SimTK;
+
+Real A[16] = { 1.80, 2.88, 2.05, -0.89,
+ 5.25, -2.95, -0.95, -3.80,
+ 1.58, -2.69, -2.90, -1.04,
+ -1.11, -0.66, -0.59, 0.80 };
+
+Real B[4] = { 9.52, 24.35, 0.77, -6.22 };
+
+Real X[4] = { 1., -1., 3., -5. };
+
+int main () {
+ try {
+ // Default precision (Real, normally double) test.
+ Matrix a(4,4, A);
+ Vector b(4, B);
+ Vector x_right(4, X);
+ Vector x; // should get sized automatically to 4 by solve()
+
+ FactorLU lu(a); // perform LU factorization
+
+ lu.solve( b, x ); // solve for x given a right hand side
+
+ cout << " Real SOLUTION: " << x << " errnorm=" << (x-x_right).norm() << endl;
+ ASSERT((x-x_right).norm() < 10*SignificantReal);
+
+ // float test
+
+ Matrix_<float> af(4,4); for (int i=0; i<4; ++i) for (int j=0; j<4; ++j) af(i,j)=(float)a(i,j);
+ Vector_<float> bf(4); for (int i=0; i<4; ++i) bf[i] = (float)b[i];
+ Vector_<float> xf_right(4); for (int i=0; i<4; ++i) xf_right[i] = (float)x_right[i];
+ Vector_<float> xf; // should get sized automatically to 4 by solve()
+
+ FactorLU luf;
+ luf.factor(af);
+ luf.solve(bf, xf);
+
+ cout << " float SOLUTION: " << xf << " errnorm=" << (xf-xf_right).norm() << endl;
+ const float SignificantFloat = NTraits<float>::getSignificant();
+ ASSERT((xf-xf_right).norm() < 10*SignificantFloat);
+
+ luf.factor(a);
+ lu.solve( b, x ); // solve for x given a right hand side
+ cout << " Real SOLUTION: " << x << " errnorm=" << (x-x_right).norm() << endl;
+ ASSERT((x-x_right).norm() < 10*SignificantReal);
+
+ Real C[4] = { 1.0, 2.0,
+ 1.0, 3.0 };
+ Matrix c(2,2, C);
+ FactorLU clu(c);
+ Matrix invC;
+ clu.inverse(invC);
+ cout << "Inverse c: " << endl;
+ cout << invC[0] << endl;
+ cout << invC[1] << endl;
+ Real Z[4] = { 0.0, 0.0,
+ 0.0, 0.0 };
+ Matrix z(2,2, Z);
+ FactorLU zlu(z);
+ Vector_<double> xz;
+ Vector_<double> bz(2);
+ bz(1) = bz(0) = 0.0;
+ zlu.solve( bz, xz );
+ cout << " solve with mat all zeros : " << endl;
+ for(int i=0;i<xz.size();i++) printf("%f ", xz(i) ); printf("\n");
+
+ try {
+ Matrix_<double> z0;
+ FactorLU z0lu(z0);
+ Vector_<double> bz0(0);
+ z0lu.solve( bz0, xz );
+ cout << " solve with mat(0,0) : " << endl;
+ for(int i=0;i<xz.size();i++) printf("%f ", xz(i) ); printf("\n");
+ } catch (const std::exception& e) {
+ cout << "(EXPECTED EXCEPTION) NULL matrix test: "
+ << e.what() << endl;
+ }
+ }
+ catch (const std::exception& e) {
+ std::printf("FAILED: %s\n", e.what());
+ return 1;
+ }
+
+ return 0;
+}
+
+
diff --git a/SimTKmath/tests/FactorQTZTest.cpp b/SimTKmath/tests/FactorQTZTest.cpp
new file mode 100644
index 0000000..52379df
--- /dev/null
+++ b/SimTKmath/tests/FactorQTZTest.cpp
@@ -0,0 +1,223 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This is a test program which uses the FactorQTZ class to do an QTZ
+ * factorization on a system of linear equations and then use the
+ * factored QTZ matrix to solve a find a least squares solution
+ * for a particular right hand side
+ */
+
+/*
+The data for this test is from an example FORTRAN program from the
+Numerical Algorithms Group (NAG)
+URL:http://www.nag.com/lapack-ex/lapack-ex.html
+
+
+Solves the least squares problem:
+
+Ax = B,
+
+where A is the general matrix
+
+
+ -0.09 0.14 -0.46 0.68 1.29 7.4
+ -1.56 0.20 0.29 1.09 0.51 4.2
+A = -1.48 -0.43 0.89 -0.71 -0.96 B= -8.3
+ -1.09 0.84 0.77 2.11 -1.27 1.8
+ 0.08 0.55 -1.13 0.14 1.74 8.6
+ -1.59 -0.72 1.06 1.24 0.34 2.1
+
+The default tolerance of 0.01 is used to determine the effective rank of A
+
+
+SOLUTION =
+0.6344 0.9699 -1.4402 3.3678 3.3992
+
+estimated rank = 4
+
+*/
+
+#include "SimTKmath.h"
+
+#include <cstdio>
+#include <cassert>
+#include <iostream>
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+using namespace SimTK;
+
+using std::printf;
+using std::cout;
+using std::endl;
+
+Real A[30] = { -0.09, 0.14, -0.46, 0.68, 1.29,
+ -1.56, 0.20, 0.29, 1.09, 0.51,
+ -1.48, -0.43, 0.89, -0.71, -0.96,
+ -1.09, 0.84, 0.77, 2.11, -1.27,
+ 0.08, 0.55, -1.13, 0.14, 1.74,
+ -1.59, -0.72, 1.06, 1.24, 0.34 };
+
+Real B[6] = { 7.4, 4.2, -8.3, 1.8, 8.6, 2.1 };
+Real X[5] = { 0.6344, 0.9699, -1.4402, 3.3678, 3.3992 };
+
+int main () {
+ try {
+ // Default precision (Real, normally double) test.
+
+ Matrix a(6,5, A);
+ Vector b(6, B);
+ Vector x_right(5, X);
+ Vector x; // should get sized automatically to 5 by solve()
+
+ FactorQTZ qtz; // perform QTZ factorization
+
+ qtz.factor(a);
+ printf("\n Estimated rank with default rcond %d \n\n",qtz.getRank() );
+ ASSERT( qtz.getRank() == 5 );
+
+ qtz.factor(a, 0.01);
+ qtz.solve( b, x ); // solve for x given a right hand side
+
+ printf("\n Estimated rank with rcond = 0.01 : %d \n\n",qtz.getRank() );
+
+
+ cout << " Overdetermined Double SOLUTION: " << x << " errnorm=" << (x-x_right).norm() << endl;
+ ASSERT((x-x_right).norm() < 0.001);
+
+ FactorQTZ qtzCopy( qtz );
+ Vector xc; // should get sized automatically to 5 by solve()
+ qtzCopy.solve(b, xc );
+ cout << " copy constructor SOLUTION: " << xc << " errnorm=" << (xc-x_right).norm() << endl;
+
+ FactorQTZ qtzAssign = qtz;
+ Vector xa; // should get sized automatically to 5 by solve()
+ qtzCopy.solve(b, xa );
+ cout << " copy assign SOLUTION: " << xa << " errnorm=" << (xa-x_right).norm() << endl;
+
+ Matrix_<float> af(6,5); for (int i=0; i<6; ++i) for (int j=0; j<5; ++j) af(i,j)=(float)a(i,j);
+ Vector_<float> bf(6); for (int i=0; i<6; ++i) bf[i] = (float)b[i];
+ Vector_<float> xf_right(5); for (int i=0; i<5; ++i) xf_right[i] = (float)x_right[i];
+ Vector_<float> xf; // should get sized automatically to 5 by solve()
+
+ qtz.factor(af, (float)0.01);
+ qtz.solve(bf,xf);
+
+ cout << " Overdetermined Float SOLUTION: " << xf << " errnorm=" << (xf-xf_right).norm() << endl;
+ const float SignificantFloat = NTraits<float>::getSignificant();
+ ASSERT((xf-xf_right).norm() < 0.001);
+
+ // Underdetermined case adapted from
+ // http://idlastro.gsfc.nasa.gov/idl_html_help/LA_LEAST_SQUARES.html
+
+ Real Au[12] = { 2, 5, 3, 4,
+ 7, 1, 3, 5,
+ 4, 3, 6, 2 };
+ Real Bu[3] = { 3, 1, 6 };
+ Real Xu[4] = { -0.0376844, 0.350628, 0.986164, -0.409066 };
+ Matrix au(3, 4, Au);
+ Vector bu(3, Bu);
+ Vector xu_right(4, Xu);
+ Vector xu; // should get sized automatically to 4 by solve()
+
+ Matrix bu2(3,2);
+ bu2(0) = bu;
+ bu2(1) = 2*bu;
+ Matrix xu2; // should get sized 4x2 by solve
+
+ FactorQTZ qtzu(au); // perform QTZ factorization
+
+ qtzu.solve( bu, xu ); // solve for x given a right hand side
+
+ cout << " Underdetermined Double SOLUTION: " << xu << " errnorm=" << (xu-xu_right).norm() << endl;
+
+ qtzu.solve( bu2, xu2 );
+ cout << " multiple rhs solution, double " << xu2 << endl;
+
+ Matrix_<float> afu(3,4); for (int i=0; i<3; ++i) for (int j=0; j<4; ++j) afu(i,j)=(float)au(i,j);
+ Vector_<float> bfu(3); for (int i=0; i<3; ++i) bfu[i] = (float)bu[i];
+ Vector_<float> xfu_right(4); for (int i=0; i<4; ++i) xfu_right[i] = (float)xu_right[i];
+ Vector_<float> xfu; // should get sized automatically to 4 by solve()
+
+ Matrix_<float> bfu2(3,2);
+ bfu2(0) = bfu;
+ bfu2(1) = 2*bfu;
+ Matrix_<float> xfu2; // should get sized 4x2 by solve
+
+ FactorQTZ qtzfu(afu); // perform QTZ factorization
+
+ qtzfu.solve( bfu, xfu ); // solve for x given a right hand side
+
+ cout << " Underdetermined Float SOLUTION: " << xfu << " errnorm=" << (xfu-xfu_right).norm() << endl;
+
+ qtzfu.solve( bfu2, xfu2 );
+ cout << " multiple rhs solution, float " << xfu2 << endl;
+
+
+ Real C[4] = { 1.0, 2.0,
+ 1.0, 3.0 };
+ Matrix c(2,2, C);
+ FactorQTZ cqtz(c);
+ Matrix invQTZ;
+ cqtz.inverse(invQTZ);
+ cout << " FactorQTZ.inverse : " << endl;
+ cout << invQTZ[0] << endl;
+ cout << invQTZ[1] << endl;
+
+ Real Z[4] = { 0.0, 0.0,
+ 0.0, 0.0 };
+
+ Matrix z(2,2, Z);
+ FactorQTZ zqtz(z);
+ Vector_<double> xz;
+ Vector_<double> bz(2);
+ bz(1) = bz(0) = 0.0;
+ zqtz.solve( bz, xz );
+ cout << " solve with mat all zeros : " << endl;
+ for(int i=0;i<xz.size();i++) printf("%f ", xz(i) ); printf("\n");
+ try {
+ Matrix_<double> z0;
+ FactorQTZ z0qtz(z0);
+ Vector_<double> bz0(0);
+ z0qtz.solve( bz0, xz );
+ cout << " solve with mat(0,0) : " << endl;
+ for(int i=0;i<xz.size();i++) printf("%f ", xz(i) ); printf("\n");
+ } catch (const std::exception& e) {
+ cout << "(EXPECTED EXCEPTION) NULL matrix test: "
+ << e.what() << endl;
+ }
+ }
+ catch (const std::exception& e) {
+ std::printf("FAILED: %s\n", e.what());
+ return 1;
+ }
+ catch (...) {
+ std::printf("FAILED: Unknown exception\n");
+ return 1;
+ }
+
+ return 0;
+}
+
+
diff --git a/SimTKmath/tests/FactorSVDTest.cpp b/SimTKmath/tests/FactorSVDTest.cpp
new file mode 100644
index 0000000..5377584
--- /dev/null
+++ b/SimTKmath/tests/FactorSVDTest.cpp
@@ -0,0 +1,190 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This is a test program which uses the FactorSVD class to compute
+ * eigen values and eigen vectors
+ */
+
+/*
+The data for this test is from an example FORTRAN program from the
+Numerical Algorithms Group (NAG)
+URL:http://www.nag.com/lapack-ex/lapack-ex.html
+
+
+Solves for the singular valus and vectors for the
+following matrix
+
+
+
+A = 2.27 0.28 -0.48 1.07 -2.35 0.62
+ -1.54 -1.67 -3.09 1.22 2.93 -7.39
+ 1.15 0.94 0.99 0.79 -1.45 1.03
+ -1.94 -0.78 -0.21 0.63 2.30 -2.57
+
+
+
+SOLUTION =
+Singular values
+ 9.9966 3.6831 1.3569 0.5000
+ Left singular vectors
+ 1 2 3 4
+ 1 -0.1921 0.8030 0.0041 -0.5642
+ 2 0.8794 0.3926 -0.0752 0.2587
+ 3 -0.2140 0.2980 0.7827 0.5027
+ 4 0.3795 -0.3351 0.6178 -0.6017
+
+ Right singular vectors by row (first m rows of V**T)
+ 1 2 3 4 5 6
+ 1 -0.2774 -0.2020 -0.2918 0.0938 0.4213 -0.7816
+ 2 0.6003 0.0301 -0.3348 0.3699 -0.5266 -0.3353
+ 3 -0.1277 0.2805 0.6453 0.6781 0.0413 -0.1645
+ 4 0.1323 0.7034 0.1906 -0.5399 -0.0575 -0.3957
+
+ Error estimate for the singular values
+ 1.1E-15
+
+ Error estimates for the left singular vectors
+ 1.8E-16 4.8E-16 1.3E-15 1.3E-15
+
+ Error estimates for the right singular vectors
+ 1.8E-16 4.8E-16 1.3E-15 2.2E-15
+
+
+*/
+
+#include "SimTKmath.h"
+
+#include <cstdio>
+#include <cassert>
+#include <iostream>
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+static const double EPS = 0.00001;
+
+using namespace SimTK;
+
+using std::printf;
+using std::cout;
+using std::endl;
+
+Real A[24] = { 2.27, 0.28, -0.48, 1.07, -2.35, 0.62,
+ -1.54, -1.67, -3.09, 1.22, 2.93, -7.39,
+ 1.15, 0.94, 0.99, 0.79, -1.45, 1.03,
+ -1.94, -0.78, -0.21, 0.63, 2.30, -2.57 };
+Real X[4] = { 9.9966, 3.6831, 1.3569, 0.5000 };
+
+
+int main () {
+
+ try {
+ // Default precision (Real, normally double) test.
+
+ Matrix a(4,6, A);
+ Vector singularValues( 4 );
+ Vector expectedValues( 4, X );
+ Matrix rightVectors;
+ Matrix leftVectors;
+
+ FactorSVD svd(a, 0.01); // setup the eigen system
+
+ svd.getSingularValues( singularValues ); // solve for the singular values
+ cout << " SingularValues rcond = 0.01 : " << singularValues << endl;
+
+ svd.factor(a ); // setup the eigen system
+ cout << " SingularValues rcond = default : " << singularValues << " errnorm=" << (singularValues-expectedValues).norm() << endl;
+ svd.getSingularValuesAndVectors( singularValues, leftVectors, rightVectors ); // solve for the singular values
+ ASSERT((singularValues-expectedValues).norm() < 0.001);
+
+
+ printf("Left Vectors = \n");
+ for(int i=0;i<leftVectors.ncol();i++) {
+ for(int j=0;j<leftVectors.nrow();j++) printf("%f ",leftVectors(i,j) );
+ printf("\n");
+ }
+
+
+ printf("Right Vectors = \n");
+ for(int i=0;i<rightVectors.ncol();i++) {
+ for(int j=0;j<rightVectors.nrow();j++) printf("%f ",rightVectors(i,j) );
+ printf("\n");
+ }
+
+ Real C[4] = { 1.0, 2.0,
+ 1.0, 3.0 };
+
+ Matrix c(2,2, C);
+ FactorSVD csvd(c);
+ Matrix invSVD;
+ csvd.inverse(invSVD);
+ cout << " FactorSVD.inverse : " << endl;
+ cout << invSVD[0] << endl;
+ cout << invSVD[1] << endl;
+
+ Real Z[4] = { 0.0, 0.0,
+ 0.0, 0.0 };
+
+ Matrix z(2,2, Z);
+ FactorSVD zsvd(z);
+ Vector_<double> xz;
+ Vector_<double> bz(2);
+ bz(1) = bz(0) = 0.0;
+ zsvd.solve( bz, xz );
+ cout << " solve with mat all zeros : " << endl;
+ for(int i=0;i<xz.size();i++) printf("%f ", xz(i) ); printf("\n");
+
+ Matrix_<double> z0;
+ FactorSVD z0svd(z0);
+ Vector_<double> bz0(0);
+ z0svd.solve( bz0, xz );
+ cout << " solve with mat(0,0) : " << endl;
+ for(int i=0;i<xz.size();i++) printf("%f ", xz(i) ); printf("\n");
+
+
+ cout << " SVD factorization with mat(0,0) : " << endl;
+ FactorSVD z0fsvd(z0);
+ z0fsvd.getSingularValuesAndVectors( singularValues, leftVectors, rightVectors ); // solve for the singular values
+ cout << " Real SOLUTION: " << singularValues << endl;
+
+ printf("Left Vectors = \n");
+ for(int i=0;i<leftVectors.ncol();i++) {
+ for(int j=0;j<leftVectors.nrow();j++) printf("%f ",leftVectors(i,j) );
+ printf("\n");
+ }
+
+
+ printf("Right Vectors = \n");
+ for(int i=0;i<rightVectors.ncol();i++) {
+ for(int j=0;j<rightVectors.nrow();j++) printf("%f ",rightVectors(i,j) );
+ printf("\n");
+ }
+
+ return 0;
+ }
+ catch (std::exception& e) {
+ std::printf("FAILED: %s\n", e.what());
+ return 1;
+ }
+}
+
diff --git a/SimTKmath/tests/IntegratorTest.cpp b/SimTKmath/tests/IntegratorTest.cpp
new file mode 100644
index 0000000..fb9eff7
--- /dev/null
+++ b/SimTKmath/tests/IntegratorTest.cpp
@@ -0,0 +1,803 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This is a test program which uses the Integrator class in various ways.
+ */
+
+#include "SimTKcommon.h"
+#include "SimTKcommon/internal/SystemGuts.h"
+
+#include "SimTKmath.h"
+
+#include <cstdio>
+#include <cassert>
+#include <iostream>
+
+using namespace SimTK;
+
+using std::printf;
+using std::cout;
+using std::endl;
+
+static int qProj, qProjFail;
+static int uProj, uProjFail;
+
+// User-defined system to be integrated. This is a kind of SimTK::System.
+class MyPendulum;
+class MyPendulumGuts: public System::Guts {
+ friend class MyPendulum;
+
+ // TOPOLOGY STATE
+ SubsystemIndex subsysIndex;
+
+ // TOPOLOGY CACHE
+ mutable DiscreteVariableIndex massIndex, lengthIndex, gravityIndex;
+ mutable QIndex q0;
+ mutable UIndex u0;
+ mutable QErrIndex qerr0;
+ mutable UErrIndex uerr0;
+ mutable UDotErrIndex udoterr0;
+ mutable EventTriggerByStageIndex trigger0;
+ mutable CacheEntryIndex mgForceIndex; // a cache entry m*g calculated at Dynamics stage
+ mutable EventId eventId0, eventId1, eventId2;
+public:
+ MyPendulumGuts() : Guts() {
+ // Index types set themselves invalid on construction.
+ }
+
+ inline const MyPendulum& getMyPendulum() const;
+
+ /*virtual*/MyPendulumGuts* cloneImpl() const {return new MyPendulumGuts(*this);}
+
+ /////////////////////////////////////////////////////////
+ // Implementation of continuous DynamicSystem virtuals //
+ /////////////////////////////////////////////////////////
+
+ int realizeTopologyImpl(State&) const OVERRIDE_11;
+ int realizeModelImpl(State&) const OVERRIDE_11;
+ int realizeInstanceImpl(const State&) const OVERRIDE_11;
+ int realizePositionImpl(const State&) const OVERRIDE_11;
+ int realizeVelocityImpl(const State&) const OVERRIDE_11;
+ int realizeDynamicsImpl(const State&) const OVERRIDE_11;
+ int realizeAccelerationImpl(const State&) const OVERRIDE_11;
+
+ // qdot==u here so these are just copies
+ void multiplyByNImpl(const State& state, const Vector& u,
+ Vector& dq) const OVERRIDE_11 {dq=u;}
+ void multiplyByNTransposeImpl(const State& state, const Vector& fq,
+ Vector& fu) const OVERRIDE_11 {fu=fq;}
+ void multiplyByNPInvImpl(const State& state, const Vector& dq,
+ Vector& u) const OVERRIDE_11 {u=dq;}
+ void multiplyByNPInvTransposeImpl(const State& state, const Vector& fu,
+ Vector& fq) const OVERRIDE_11 {fq=fu;}
+
+ // No prescribed motion.
+ bool prescribeQImpl(State&) const OVERRIDE_11 {return false;}
+ bool prescribeUImpl(State&) const OVERRIDE_11 {return false;}
+
+ void projectQImpl(State&, Vector& qErrEst,
+ const ProjectOptions& options, ProjectResults& results) const OVERRIDE_11;
+ void projectUImpl(State&, Vector& uErrEst,
+ const ProjectOptions& options, ProjectResults& results) const OVERRIDE_11;
+
+
+ ////////////////////////////////////////////////
+ // Implementation of discrete System virtuals //
+ ////////////////////////////////////////////////
+
+ int calcEventTriggerInfoImpl
+ (const State& s, Array_<EventTriggerInfo>& eti) const OVERRIDE_11
+ {
+ eti.clear();
+ eti.push_back(EventTriggerInfo(eventId0)
+ .setRequiredLocalizationTimeWindow(1)
+ .setTriggerOnRisingSignTransition(false));
+ eti.push_back(EventTriggerInfo(eventId1));
+ eti.push_back(EventTriggerInfo(eventId2)
+ .setTriggerOnFallingSignTransition(false));
+ return 0;
+ }
+
+ int calcTimeOfNextScheduledEventImpl
+ (const State& s, Real& tNextEvent,
+ Array_<EventId>& eventIds, bool includeCurrentTime) const OVERRIDE_11
+ {
+ // Generate an event every 5.123 seconds.
+ int nFives = (int)(s.getTime() / 5.123); // rounded down
+ tNextEvent = (nFives+1) * Real(5.123);
+ // Careful ...
+ if (tNextEvent <= s.getTime())
+ tNextEvent += Real(5.123);
+ eventIds.push_back(eventId1); // event Id for scheduled pulse
+
+ return 0;
+ }
+
+
+ // This should be called when the integrator returns indicating that
+ // a discontinuity (event trigger) has been detected. The current
+ // state is inconsistent in some way and we expect the event handlers
+ // to correct that. Time will be the same before and after, but the
+ // state may have changed discontinuously.
+ void handleEventsImpl
+ (State& s, Event::Cause cause, const Array_<EventId>& eventIds,
+ const HandleEventsOptions& options, HandleEventsResults& results) const
+ OVERRIDE_11
+ {
+ cout << "===> t=" << s.getTime() << ": HANDLING "
+ << Event::getCauseName(cause) << " EVENT!!!" << endl;
+ if (eventIds.size())
+ cout << " EVENT IDS: " << eventIds << endl;
+ if (cause != Event::Cause::TimeAdvanced) {
+ std::swap(s.updQ()[0],s.updQ()[1]); // invalidates Position stage
+ s.updU()=0;
+ }
+ results.setExitStatus(HandleEventsResults::Succeeded);
+ }
+
+};
+
+// This is the handle class for a MyPendulum System.
+// It must not have any data members. Data, if needed, is
+// in the corresponding "Guts" class.
+
+class MyPendulum: public System {
+public:
+ MyPendulum() : System()
+ {
+ adoptSystemGuts(new MyPendulumGuts());
+ DefaultSystemSubsystem defsub(*this);
+ updGuts().subsysIndex = defsub.getMySubsystemIndex();
+
+ setHasTimeAdvancedEvents(false);
+ (void)realizeTopology();
+ }
+
+ const MyPendulumGuts& getGuts() const {
+ return dynamic_cast<const MyPendulumGuts&>(getSystemGuts());
+ }
+
+ MyPendulumGuts& updGuts() {
+ return dynamic_cast<MyPendulumGuts&>(updSystemGuts());
+ }
+
+ // Instance variables are written to our defaultState.
+ void setDefaultMass(Real mass) {
+ const MyPendulumGuts& guts = getGuts();
+ updDefaultState().updDiscreteVariable(guts.subsysIndex, guts.massIndex) = Value<Real>(mass);
+ }
+
+ void setDefaultLength(Real length) {
+ const MyPendulumGuts& guts = getGuts();
+ updDefaultState().updDiscreteVariable(guts.subsysIndex, guts.lengthIndex) = Value<Real>(length);
+ }
+
+ void setDefaultGravity(Real gravity) {
+ const MyPendulumGuts& guts = getGuts();
+ updDefaultState().updDiscreteVariable(guts.subsysIndex, guts.gravityIndex) = Value<Real>(gravity);
+ }
+
+ void setDefaultTimeAndState(Real t, const Vector& q, const Vector& u) {
+ const MyPendulumGuts& guts = getGuts();
+ updDefaultState().updU(guts.subsysIndex) = u;
+ updDefaultState().updQ(guts.subsysIndex) = q;
+ updDefaultState().updTime() = t;
+ }
+
+ Real getMass(const State& s) const {
+ const MyPendulumGuts& guts = getGuts();
+ const AbstractValue& m = s.getDiscreteVariable(guts.subsysIndex, guts.massIndex);
+ return Value<Real>::downcast(m).get();
+ }
+ Real getDefaultMass() const {return getMass(getDefaultState());}
+
+ Real getLength(const State& s) const {
+ const MyPendulumGuts& guts = getGuts();
+ const AbstractValue& d = s.getDiscreteVariable(guts.subsysIndex, guts.lengthIndex);
+ return Value<Real>::downcast(d).get();
+ }
+ Real getDefaultLength() const {return getLength(getDefaultState());}
+
+ Real getGravity(const State& s) const {
+ const MyPendulumGuts& guts = getGuts();
+ const AbstractValue& g = s.getDiscreteVariable(guts.subsysIndex, guts.gravityIndex);
+ return Value<Real>::downcast(g).get();
+ }
+ Real getDefaultGravity() const {return getGravity(getDefaultState());}
+
+
+ void dump(const char* msg) const {
+ const MyPendulumGuts& guts = getGuts();
+ cout << std::string(msg) << ": MyPendulum default state dump:" << endl;
+ cout << " mass =" << getDefaultMass() << endl;
+ cout << " length =" << getDefaultLength() << endl;
+ cout << " gravity=" << getDefaultGravity() << endl;
+ cout << " time=" << getDefaultState().getTime() << endl;
+ cout << " q=" << getDefaultState().getQ(guts.subsysIndex) << endl;
+ cout << " u=" << getDefaultState().getU(guts.subsysIndex) << endl;
+ }
+
+};
+
+inline const MyPendulum& MyPendulumGuts::getMyPendulum() const {
+ return static_cast<const MyPendulum&>(getSystem());
+}
+
+static void printFinalStats(const Integrator& integ);
+
+static void reportState(const char* msg,
+ const System& sys, const Integrator& integ) {
+ if (*msg) printf("%s\n", msg);
+ const State& s = integ.getState();
+
+ sys.realize(s);
+ printf(
+ " -%6s- %9.6lf(%9.6lf) %14.10lf %14.10lf %14.10lf %14.10lf | %14.10lf %14.10lf %14.10lf %14.10lf %14.10lf\n",
+ integ.isStateInterpolated() ? "INTERP" : "------",
+ s.getTime(), integ.getAdvancedTime(),
+ s.getY()[0], s.getY()[1], s.getY()[2], s.getY()[3],
+ s.getYErr()[0], s.getYErr()[1],
+ s.getEventTriggersByStage(SubsystemIndex(0),Stage::Position)[0],
+ s.getEventTriggersByStage(SubsystemIndex(0),Stage::Position)[1],
+ s.getEventTriggersByStage(SubsystemIndex(0),Stage::Position)[2]);
+
+ cout << "YDot: " << s.getYDot() << endl;
+ cout << "Multipliers: " << s.getMultipliers() << endl;
+ cout << "UDotErrs: " << s.getUDotErr() << endl;
+}
+
+int main () {
+ try
+ { MyPendulum sys;
+ RungeKuttaMersonIntegrator integ(sys);
+ //RungeKuttaFeldbergIntegrator integ(sys);
+ //RungeKutta3Integrator integ(sys);
+ //CPodesIntegrator integ(sys);
+ //VerletIntegrator integ(sys);
+ //ExplicitEulerIntegrator integ(sys);
+
+ const Real t0=0;
+ const Real qi[] = {1,0}; // (x,y)=(1,0)
+ const Real ui[] = {0,0}; // v=0
+ const Vector q0(2, qi);
+ const Vector u0(2, ui);
+
+ sys.setDefaultMass(10);
+ sys.setDefaultTimeAndState(t0, q0, u0);
+ sys.dump("Initial");
+
+ integ.setAccuracy(1e-2);
+ integ.setConstraintTolerance(1e-4);
+
+ //integ.setAllowInterpolation(false);
+ //integ.setProjectEveryStep(true);
+ //integ.setProjectInterpolatedStates(false);
+ //integ.setInitialStepSize(0.1);
+ //integ.setUseInfinityNorm(true);
+ //integ.setReturnEveryInternalStep(true);
+
+ const Real tFinal = 30.003;
+ const Real hReport = 1.;
+
+ integ.setFinalTime(tFinal);
+ integ.initialize(sys.getDefaultState());
+
+ cout << "ACCURACY IN USE = " << integ.getAccuracyInUse() << endl;
+
+
+ for (int reportNo=0; !integ.isSimulationOver();
+ reportNo += (integ.getTime() >= reportNo*hReport))
+ {
+ Array_<EventId> scheduledEventIds;
+ Real nextScheduledEvent = NTraits<Real>::getInfinity();
+ sys.calcTimeOfNextScheduledEvent(integ.getAdvancedState(),
+ nextScheduledEvent, scheduledEventIds, true);
+
+ HandleEventsOptions handleOpts(integ.getAccuracyInUse());
+ HandleEventsResults handleResults;
+
+ switch(integ.stepTo(reportNo*hReport, nextScheduledEvent)) {
+ case Integrator::ReachedStepLimit: printf("STEP LIMIT\n"); break;
+ case Integrator::ReachedReportTime: printf("REPORT TIME AT t=%.17g\n", integ.getTime()); break;
+ case Integrator::StartOfContinuousInterval: printf("START OF CONTINUOUS INTERVAL\n"); break;
+
+ case Integrator::ReachedScheduledEvent: {
+ Stage lowestModified = Stage::Empty;
+ bool shouldTerminate;
+ printf("SCHEDULED EVENT\n");
+ reportState("BEFORE SCHEDULED EVENT:", sys, integ);
+ sys.handleEvents(integ.updAdvancedState(),
+ Event::Cause::Scheduled,
+ scheduledEventIds,handleOpts,handleResults);
+ shouldTerminate =
+ handleResults.getExitStatus()==HandleEventsResults::ShouldTerminate;
+ lowestModified = handleResults.getLowestModifiedStage();
+ integ.reinitialize(lowestModified, shouldTerminate);
+ break;
+ }
+
+ case Integrator::TimeHasAdvanced: {
+ Stage lowestModified = Stage::Empty;
+ bool shouldTerminate;
+ printf("TIME HAS ADVANCED TO %g\n", integ.getTime());
+ sys.handleEvents(integ.updAdvancedState(),
+ Event::Cause::TimeAdvanced,
+ Array_<EventId>(),handleOpts,handleResults);
+ shouldTerminate =
+ handleResults.getExitStatus()==HandleEventsResults::ShouldTerminate;
+ lowestModified = handleResults.getLowestModifiedStage();
+ integ.reinitialize(lowestModified, shouldTerminate);
+ break;
+ }
+
+ case Integrator::ReachedEventTrigger: {
+ Stage lowestModified = Stage::Empty;
+ bool shouldTerminate;
+ printf("EVENT TRIGGERED AT tLow=%.17g tHigh=%.17g!!\n",
+ integ.getTime(), integ.getAdvancedTime());
+ cout << std::setprecision(17);
+ cout << "Event window: " << integ.getEventWindow() << endl;
+ cout << "Triggered events: " << integ.getTriggeredEvents()<<"\n";
+ cout << "Transitions seen:";
+ for (int i=0; i<(int)integ.getEventTransitionsSeen().size(); ++i)
+ cout << " " << Event::eventTriggerString(integ.getEventTransitionsSeen()[i]);
+ cout << endl;
+ cout << "Est event times: " << integ.getEstimatedEventTimes() << "\n";
+ reportState("BEFORE TRIGGERED EVENT:", sys, integ);
+ // state(t-) => state(t+)
+ sys.handleEvents(integ.updAdvancedState(),
+ Event::Cause::Triggered,
+ integ.getTriggeredEvents(),handleOpts,handleResults);
+ shouldTerminate =
+ handleResults.getExitStatus()==HandleEventsResults::ShouldTerminate;
+ lowestModified = handleResults.getLowestModifiedStage();
+ integ.reinitialize(lowestModified, shouldTerminate);
+ break;
+ }
+
+ case Integrator::EndOfSimulation: {
+ Stage lowestModified = Stage::Empty;
+ bool shouldTerminate;
+ String reason = integ.getTerminationReasonString
+ (integ.getTerminationReason());
+
+ printf("SIMULATION IS OVER. TERMINATION REASON=%s\n",
+ reason.c_str());
+ sys.handleEvents(integ.updAdvancedState(),
+ Event::Cause::Termination,
+ Array_<EventId>(),handleOpts,handleResults);
+ shouldTerminate =
+ handleResults.getExitStatus()==HandleEventsResults::ShouldTerminate;
+ lowestModified = handleResults.getLowestModifiedStage();
+ integ.reinitialize(lowestModified, shouldTerminate);
+ break;
+ }
+
+ default: assert(!"Unrecognized return from stepTo()");
+ }
+
+ // fall through to here to report
+ reportState("", sys, integ);
+ }
+
+ printFinalStats(integ);
+
+ return 0;
+ }
+ catch (std::exception& e) {
+ std::printf("FAILED: %s\n", e.what());
+ return 1;
+ }
+}
+
+static void printFinalStats(const Integrator& integ)
+{
+ Real h0u;
+ int nst, nattempt, nfe, nsetups, nje, nfeLS, nni, ncfn, netf, nge;
+ int nproj, nprojq, nproju, nce, nsetupsP, nprf, nprqf, npruf;
+
+ h0u=NaN;
+ nst=nattempt=nfe=nsetups=nje=nfeLS=nni=ncfn=netf=nge=-1;
+ nproj=nprojq=nproju=nce=nsetupsP=nprf=nprqf=npruf=-1;
+
+ /*
+ flag = cpode.getActualInitStep(&h0u);
+ flag = cpode.getNumSteps(&nst);
+ flag = cpode.getNumFctEvals(&nfe);
+ flag = cpode.getNumLinSolvSetups(&nsetups);
+ flag = cpode.getNumErrTestFails(&netf);
+ flag = cpode.getNumNonlinSolvIters(&nni);
+ flag = cpode.getNumNonlinSolvConvFails(&ncfn);
+ flag = cpode.dlsGetNumJacEvals(&nje);
+ flag = cpode.dlsGetNumFctEvals(&nfeLS);
+ flag = cpode.getProjStats(&nproj, &nce, &nsetupsP, &nprf);
+ flag = cpode.getNumGEvals(&nge);
+ */
+
+ h0u = integ.getActualInitialStepSizeTaken();
+ nst = integ.getNumStepsTaken();
+ nattempt = integ.getNumStepsAttempted();
+ nfe = integ.getNumRealizations();
+ netf = integ.getNumErrorTestFailures();
+ nproj = integ.getNumProjections();
+ nprojq = integ.getNumQProjections();
+ nproju = integ.getNumUProjections();
+ nprf = integ.getNumProjectionFailures();
+ nprqf = integ.getNumQProjectionFailures();
+ npruf = integ.getNumUProjectionFailures();
+
+ printf("\nFinal Statistics:\n");
+ printf("h0u = %g\n",h0u);
+ printf("nst = %-6d nattempt = %-6d nfe = %-6d nsetups = %-6d\n",
+ nst, nattempt, nfe, nsetups);
+ printf("nfeLS = %-6d nje = %d\n",
+ nfeLS, nje);
+ printf("nni = %-6d ncfn = %-6d netf = %-6d \n",
+ nni, ncfn, netf);
+ printf("nproj = %-6d nprojq = %-6d nproju = %-6d\n",
+ nproj, nprojq, nproju);
+ printf("nprf = %-6d nprqf = %-6d npruf = %-6d\n",
+ nprf, nprqf, npruf);
+ printf("nge = %d\n", nge);
+
+ printf("qProj=%d qProjFail=%d\n", qProj, qProjFail);
+ printf("uProj=%d uProjFail=%d\n", uProj, uProjFail);
+
+}
+
+/*
+ * This system is a 2d pendulum swinging in gravity. It is modeled as
+ * a point mass free in the plane, plus a distance constraint to model
+ * the rod.
+ *
+ * y | g O
+ * ^ v \ d
+ * | \
+ * | * m
+ * ------> x
+ *
+ * Gravity acts in the y direction, the rod is length d, mass m, pivot
+ * location is the ground origin (0,0).
+ *
+ * The DAE for a generic multibody system is:
+ * qdot = Qu
+ * M udot = f - ~A lambda
+ * A udot = b
+ * perr(t,q) = 0
+ * verr(t,q,u) = 0
+ *
+ * Let r^2 = x^2 + y^2
+ * v^2 = x'^2 + y'^2
+ * We will express the "rod length=d" constraint as
+ * (r^2 - d^2)/2 = 0 (perr)
+ * xx' + yy' = 0 (verr)
+ * xx'' + yy'' = -v^2 (aerr)
+ *
+ * So the matrix A = d perr/dq = [x y] and b = -v^2, and the
+ * equations of motion are:
+ * [ m 0 x ] [ x'' ] [ 0 ]
+ * [ 0 m y ] [ y'' ] = [ -mg ]
+ * [ x y 0 ] [ L ] [-v^2 ]
+ * where L (the Lagrange multiplier) is proportional to
+ * the rod tension. You can solve this to get
+ * L = (m*v^2 - mg*y)/(r^2)
+ * x'' = - x*L/m
+ * y'' = - y*L/m - g
+ *
+ */
+int MyPendulumGuts::realizeTopologyImpl(State& s) const {
+ // Instance variables mass, length, gravity
+ massIndex = s.allocateDiscreteVariable(subsysIndex, Stage::Instance,
+ new Value<Real>(1));
+ lengthIndex = s.allocateDiscreteVariable(subsysIndex, Stage::Instance,
+ new Value<Real>(1));
+ gravityIndex = s.allocateDiscreteVariable(subsysIndex, Stage::Instance,
+ new Value<Real>(13.7503716373294544));
+ const Vector init(2, Real(0));
+ q0 = s.allocateQ(subsysIndex, init);
+ u0 = s.allocateU(subsysIndex, init);
+
+ mgForceIndex = s.allocateCacheEntry(subsysIndex, Stage::Dynamics,
+ new Value<Real>());
+ System::Guts::realizeTopologyImpl(s);
+ return 0;
+}
+int MyPendulumGuts::realizeModelImpl(State& s) const {
+ System::Guts::realizeModelImpl(s);
+ return 0;
+}
+int MyPendulumGuts::realizeInstanceImpl(const State& s) const {
+ qerr0 = s.allocateQErr(subsysIndex, 1);
+ uerr0 = s.allocateUErr(subsysIndex, 1);
+ udoterr0 = s.allocateUDotErr(subsysIndex, 1); // and multiplier
+ trigger0 = s.allocateEventTrigger(subsysIndex, Stage::Position, 3);
+ eventId0 = getSystem().getDefaultSubsystem().createEventId(subsysIndex, s);
+ eventId1 = getSystem().getDefaultSubsystem().createEventId(subsysIndex, s);
+ eventId2 = getSystem().getDefaultSubsystem().createEventId(subsysIndex, s);
+ System::Guts::realizeInstanceImpl(s);
+ return 0;
+}
+int MyPendulumGuts::realizePositionImpl(const State& s) const {
+ const Real d = getMyPendulum().getLength(s);
+ const Vector& q = s.getQ(subsysIndex);
+ // This is the perr() equation.
+ s.updQErr(subsysIndex)[0] = (q[0]*q[0] + q[1]*q[1] - d*d)/2;
+
+ s.updEventTriggersByStage(subsysIndex, Stage::Position)[0] = 100*q[0]-q[1];
+
+ // Make sure this boolean trigger *crosses* zero; it won't work right
+ // if one end is actually zero. We'll use -.5 for false, .5 for true.
+ s.updEventTriggersByStage(subsysIndex, Stage::Position)[1] =
+ (s.getTime() > /*1.49552*/1.49545 && s.getTime() < 12.28937)-0.5;
+
+ s.updEventTriggersByStage(subsysIndex, Stage::Position)[2] =
+ s.getTime()-1.495508;
+ System::Guts::realizePositionImpl(s);
+ return 0;
+}
+
+int MyPendulumGuts::realizeVelocityImpl(const State& s) const {
+ const Vector& q = s.getQ(subsysIndex);
+ const Vector& u = s.getU(subsysIndex);
+ Vector& qdot = s.updQDot(subsysIndex);
+
+ qdot[0] = u[0]; // qdot=u
+ qdot[1] = u[1];
+
+ // This is the verr() equation.
+ s.updUErr(subsysIndex)[0] = q[0]*u[0] + q[1]*u[1];
+ System::Guts::realizeVelocityImpl(s);
+ return 0;
+}
+
+int MyPendulumGuts::realizeDynamicsImpl(const State& s) const {
+ const Real m = getMyPendulum().getMass(s);
+ const Real g = getMyPendulum().getGravity(s);
+
+ Real& mg = Value<Real>::downcast(s.updCacheEntry(subsysIndex, mgForceIndex)).upd();
+ // Calculate the force due to gravity.
+ mg = m*g;
+ System::Guts::realizeDynamicsImpl(s);
+ return 0;
+}
+
+int MyPendulumGuts::realizeAccelerationImpl(const State& s) const {
+ const Real m = getMyPendulum().getMass(s);
+ const Real g = getMyPendulum().getGravity(s);
+ // we're pretending we couldn't calculate this here!
+ const Real mg = Value<Real>::downcast
+ (s.updCacheEntry(subsysIndex, mgForceIndex)).get();
+
+ const Vector& q = s.getQ(subsysIndex);
+ const Vector& u = s.getU(subsysIndex);
+ Vector& udot = s.updUDot(subsysIndex);
+ Vector& qdotdot = s.updQDotDot(subsysIndex);
+
+ const Real r2 = q[0]*q[0] + q[1]*q[1];
+ const Real v2 = u[0]*u[0] + u[1]*u[1];
+ const Real L = (m*v2 - mg*q[1])/r2;
+ udot[0] = - q[0]*L/m;
+ udot[1] = - q[1]*L/m - g;
+ qdotdot = udot; // N=identity for this problem
+ s.updMultipliers(subsysIndex)[0] = L;
+ s.updUDotErr(subsysIndex)[0] = q[0]*udot[0] + q[1]*udot[1] + v2;
+ System::Guts::realizeAccelerationImpl(s);
+ return 0;
+}
+
+/*
+ * Here we want to remove any constraint errors from the current state,
+ * and project out any component of the integrator's error estimate
+ * perpendicular to the constraint manifold. We will do this sequentially
+ * rather than handling position and velocity simultaneously.
+ *
+ * For this system we have P = d perr/dq = V = d verr/du = [x y].
+ * Weighted, we have PW=tp*[x/wx y/wy] VW=tv*[x/wxd y/wyd].
+ * With pinv(A)=~A*(A*~A)^-1, we have:
+ *
+ * pinv(P) = ~[ x y] / ( x ^2+ y ^2)
+ * pinv(PW) = ~(1/tp)*[(wx *wy ^2)*x (wx ^2*wy) *y] / ((wy *x)^2+(wx *y)^2)
+ * pinv(VW) = ~(1/tv)*[(wxd*wyd^2)*x (wxd^2*wyd)*y] / ((wyd*x)^2+(wxd*y)^2)
+ * (the latter assuming x,y already projected on position manifold)
+ *
+ * We want to solve
+ * |perr(q0 - dq)|_TRMS <= accuracy, such that dq=min_WLS(dq)
+ * PW(q0) dq = Tp * perr(q0); q = q0-dq
+ * Then
+ * |verr(q,u0 - du)|_TRMS <= accuracy, du=min_WLS(du)
+ * VW(q) du = Tv * verr(q,u0); u = u0-du
+ *
+ *
+ * To remove the corresponding error estimates:
+ * PW(q) qperp = PW(q) qerrest; qerrest -= qperp
+ * VW(q) uperp = VW(q) uerrest; uerrest -= uperp
+ *
+ *
+ */
+static Real wrms(const Vector& y, const Vector& w) {
+ Real sumsq = 0;
+ for (int i=0; i<y.size(); ++i)
+ sumsq += square(y[i]*w[i]);
+ return std::sqrt(sumsq/y.size());
+}
+
+// qerrest is in/out
+void MyPendulumGuts::projectQImpl(State& s, Vector& qerrest,
+ const ProjectOptions& opts,
+ ProjectResults& results) const
+
+{
+ const Real consAccuracy = opts.getRequiredAccuracy();
+ const Real projLimit = opts.getProjectionLimit();
+ const bool forceProj = opts.isOptionSet(ProjectOptions::ForceProjection);
+ const Vector& uweights = s.getUWeights(subsysIndex);
+ const Vector& ctols = s.getQErrWeights(subsysIndex);
+ // Since qdot=u here we can use uweights directly as qweights.
+ const Vec2& wq = Vec2::getAs(&uweights[0]);
+ const Real& tp = ctols[0]; // inverse tolerances 1/ti
+
+ const Vec2& q = Vec2::getAs(&s.getQ(subsysIndex)[0]); // set up aliases
+ Real& ep = s.updQErr(subsysIndex)[0]; // ep changes as we go
+
+ results.setAnyChangeMade(false);
+
+ //cout << "BEFORE wperr=" << tp*ep << endl;
+ if (!forceProj && std::abs(tp*ep) <= consAccuracy) {
+ results.setExitStatus(ProjectResults::Succeeded);
+ return;
+ }
+ if (std::abs(tp*ep) > projLimit) {
+ results.setProjectionLimitExceeded(true);
+ results.setExitStatus(ProjectResults::FailedToConverge);
+ ++qProjFail;
+ return;
+ }
+ ++qProj;
+ results.setAnyChangeMade(true);
+ Real wqchg;
+ do {
+ // Position projection
+ Real r2 = ~q*q; // x^2+y^2
+ Real wqr2 = square(wq[1]*q[0]) + square(wq[0]*q[1]);
+ Row2 P(~q), PW(tp*q[0]/wq[0], tp*q[1]/wq[1]);
+ Vec2 Pinv(q/r2);
+ Vec2 PWinv = Vec2(square(wq[1])*wq[0]*q[0],
+ square(wq[0])*wq[1]*q[1]) / (tp*wqr2);
+ Vec2 dq = Pinv*(ep); //cout << "dq=" << dq << endl;
+ Vec2 wdq = PWinv*(tp*ep); //cout << "wdq=" << wdq << endl;
+
+ wqchg = std::sqrt(wdq.normSqr()/q.size()); // wrms norm
+
+ s.updQ(subsysIndex)[0] -= wdq[0]/wq[0];
+ s.updQ(subsysIndex)[1] -= wdq[1]/wq[1];
+ realize(s, Stage::Position); // recalc QErr (ep)
+
+ //cout << "AFTER q-=wdq/W wperr=" << tp*ep << " wqchg=" << wqchg << endl;
+ } while (std::abs(tp*ep) > consAccuracy && wqchg >= 0.01*consAccuracy);
+
+ //cout << "...AFTER wperr=" << tp*ep << endl;
+
+ // Now do error estimates.
+
+ if (qerrest.size()) {
+ Vec2& eq = Vec2::updAs(&qerrest[0]);
+
+ // Recalc PW, PWInv:
+ const Real wqr2 = square(wq[1]*q[0]) + square(wq[0]*q[1]);
+ const Row2 PW = Row2(tp*q[0]/wq[0], tp*q[1]/wq[1]);
+ const Vec2 PWinv = Vec2(wq[0]*square(wq[1])*q[0],
+ square(wq[0])*wq[1]*q[1]) / (tp*wqr2);
+
+ Vec2 qperp = PWinv*(PW*eq);
+
+ //cout << "ERREST before=" << yerrest
+ // << " wrms=" << wrms(qerrest,qweights) << endl;
+ //cout << "PW*eq=" << PW*eq << endl;
+ eq -= qperp;
+
+ //cout << "ERREST after=" << yerrest
+ // << " wrms=" << wrms(qerrest,qweights) << endl;
+ //cout << "PW*eq=" << PW*eq << endl;
+ }
+
+ results.setExitStatus(ProjectResults::Succeeded);
+}
+
+void MyPendulumGuts::projectUImpl(State& s, Vector& uerrest,
+ const ProjectOptions& opts, ProjectResults& results) const
+{
+ const Real consAccuracy = opts.getRequiredAccuracy();
+ const Real projLimit = opts.getProjectionLimit();
+ const bool forceProj = opts.isOptionSet(ProjectOptions::ForceProjection);
+ const Vector& uweights = s.getUWeights(subsysIndex);
+ const Vector& ctols = s.getUErrWeights(subsysIndex);
+
+ const Vec2& wu = Vec2::getAs(&uweights[0]);
+ const Real& tv = ctols[0];
+
+ const Vec2& q = Vec2::getAs(&s.getQ(subsysIndex)[0]); // set up aliases
+ const Vec2& u = Vec2::getAs(&s.getU(subsysIndex)[0]);
+ Real& ev = s.updUErr(subsysIndex)[0]; // ev changes as we go
+
+ results.setAnyChangeMade(false);
+
+ //cout << "BEFORE wverr=" << tv*ev << endl;
+ if (!forceProj && std::abs(tv*ev) <= consAccuracy) {
+ results.setExitStatus(ProjectResults::Succeeded);
+ return;
+ }
+ if (std::abs(tv*ev) > projLimit) {
+ results.setProjectionLimitExceeded(true);
+ results.setExitStatus(ProjectResults::FailedToConverge);
+ ++uProjFail;
+ return;
+ }
+
+ ++uProj;
+ results.setAnyChangeMade(true);
+
+ // Do velocity projection at current values of q, which should have
+ // been projected already.
+ Real r2 = ~q*q; // x^2+y^2
+ Real wur2 = square(wu[1]*q[0]) + square(wu[0]*q[1]);
+ Row2 V(~q), VW(tv*q[0]/wu[0], tv*q[1]/wu[1]);
+ Vec2 Vinv(q/r2);
+ Vec2 VWinv = Vec2(square(wu[1])*wu[0]*q[0],
+ square(wu[0])*wu[1]*q[1]) / (tv*wur2);
+ realize(s, Stage::Velocity); // calculate UErr (ev)
+
+ //cout << "BEFORE wverr=" << tv*ev << endl;
+ Vec2 du = Vinv*(ev); //cout << "du=" << du << endl;
+ Vec2 wdu = VWinv*(tv*ev); //cout << "wdu=" << wdu << endl;
+
+ s.updU(subsysIndex)[0] -= wdu[0]/wu[0];
+ s.updU(subsysIndex)[1] -= wdu[1]/wu[1];
+
+ realize(s, Stage::Velocity); // recalc UErr
+ //cout << "AFTER u-=wdu wverr=" << tv*ev << endl;
+
+ //cout << "...AFTER wverr=" << tv*ev << endl;
+
+ // Now do error estimates.
+
+
+ if (uerrest.size()) {
+ Vec2& eu = Vec2::updAs(&uerrest[0]);
+ Vec2 uperp = VWinv*(VW*eu);
+
+ //cout << "ERREST before=" << uerrest
+ // << " wrms=" << wrms(uerrest,uweights) << endl;
+ //cout << " VW*eu=" << VW*eu << endl;
+ eu -= uperp;
+
+ //cout << "ERREST after=" << yerrest
+ // << " wrms=" << wrms(uerrest,uweights) << endl;
+ //cout << " VW*eu=" << VW*eu << endl;
+ }
+
+ results.setExitStatus(ProjectResults::Succeeded);
+}
+
+
+
diff --git a/SimTKmath/tests/IntegratorTestFramework.h b/SimTKmath/tests/IntegratorTestFramework.h
new file mode 100644
index 0000000..e2966a8
--- /dev/null
+++ b/SimTKmath/tests/IntegratorTestFramework.h
@@ -0,0 +1,269 @@
+#ifndef SimTK_SIMMATH_INTEGRATOR_TEST_FRAMEWORK_H_
+#define SimTK_SIMMATH_INTEGRATOR_TEST_FRAMEWORK_H_
+
+/* -------------------------------------------------------------------------- *
+ * SimTK Core: SimTK Simmath(tm) *
+ * -------------------------------------------------------------------------- *
+ * This is part of the SimTK Core 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. *
+ * *
+ * Portions copyright (c) 2006-7 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: *
+ * *
+ * Permission is hereby granted, free of charge, to any person obtaining a *
+ * copy of this software and associated documentation files (the "Software"), *
+ * to deal in the Software without restriction, including without limitation *
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense, *
+ * and/or sell copies of the Software, and to permit persons to whom the *
+ * Software is furnished to do so, subject to the following conditions: *
+ * *
+ * The above copyright notice and this permission notice shall be included in *
+ * all copies or substantial portions of the Software. *
+ * *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL *
+ * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, *
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR *
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE *
+ * USE OR OTHER DEALINGS IN THE SOFTWARE. *
+ * -------------------------------------------------------------------------- */
+
+/**
+ * This file contains code which is used for testing various integrators.
+ */
+
+#include "SimTKcommon.h"
+
+#include "SimTKmath.h"
+#include "simmath/TimeStepper.h"
+
+#include "PendulumSystem.h"
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+using namespace SimTK;
+
+using std::printf;
+using std::cout;
+using std::endl;
+
+class PeriodicHandler : public PeriodicEventHandler {
+public:
+ static int eventCount;
+ static PeriodicHandler* handler;
+ PeriodicHandler() : PeriodicEventHandler(1.0) {
+ }
+ void handleEvent(State& state, Real accuracy, bool& shouldTerminate) const {
+
+ // This should be triggered every (interval) time units.
+
+ ASSERT(state.getTime() == getNextEventTime(state, true));
+ eventCount++;
+ }
+};
+
+class ZeroPositionHandler : public TriggeredEventHandler {
+public:
+ static int eventCount;
+ static Real lastEventTime;
+ static bool hasAccelerated;
+ ZeroPositionHandler(PendulumSystem& pendulum) : TriggeredEventHandler(Stage::Velocity), pendulum(pendulum) {
+ }
+ Real getValue(const State& state) const {
+ return state.getQ(pendulum.getGuts().getSubsysIndex())[0];
+ }
+ void handleEvent(State& state, Real accuracy, bool& shouldTerminate) const {
+
+ // This should be triggered when the pendulum crosses x == 0.
+
+ Real x = state.getQ(pendulum.getGuts().getSubsysIndex())[0];
+ ASSERT(std::abs(x) < 0.01);
+ ASSERT(state.getTime() > lastEventTime);
+ eventCount++;
+ lastEventTime = state.getTime();
+ if (state.getTime() > 7 && !hasAccelerated) {
+
+ // Multiply the pendulum's velocity by sqrt(1.5), which should multiply its total energy by 1.5 (since
+ // at x=0, all of its energy is kinetic).
+
+ hasAccelerated = true;
+ SubsystemIndex subsys = pendulum.getGuts().getSubsysIndex();
+ state.updU(subsys) *= std::sqrt(1.5);
+ }
+ }
+private:
+ PendulumSystem& pendulum;
+};
+
+class ZeroVelocityHandler : public TriggeredEventHandler {
+public:
+ static int eventCount;
+ static Real lastEventTime;
+ ZeroVelocityHandler(PendulumSystem& pendulum) : TriggeredEventHandler(Stage::Velocity), pendulum(pendulum) {
+ getTriggerInfo().setTriggerOnFallingSignTransition(false);
+ }
+ Real getValue(const State& state) const {
+ return state.getU(pendulum.getGuts().getSubsysIndex())[0];
+ }
+ void handleEvent(State& state, Real accuracy, bool& shouldTerminate) const {
+
+ // This should be triggered when the pendulum reaches its farthest point in the
+ // negative direction: q[0] == -1, u[0] == 0.
+
+ Vector u = state.getU(pendulum.getGuts().getSubsysIndex());
+ ASSERT(std::abs(u[0]) < 0.01);
+ ASSERT(state.getTime() > lastEventTime);
+ eventCount++;
+ lastEventTime = state.getTime();
+ }
+private:
+ PendulumSystem& pendulum;
+};
+
+class PeriodicReporter : public PeriodicEventReporter {
+public:
+ static int eventCount;
+ static PeriodicReporter* reporter;
+ PeriodicReporter(PendulumSystem& pendulum) : PeriodicEventReporter(1.0), pendulum(pendulum) {
+ }
+ void handleEvent(const State& state) const {
+
+ // This should be triggered every (interval) time units.
+
+ ASSERT(state.getTime() == getNextEventTime(state, true));
+ eventCount++;
+
+ // Verify conservation of energy.
+
+ const Vector q = state.getQ(pendulum.getGuts().getSubsysIndex());
+ const Vector u = state.getU(pendulum.getGuts().getSubsysIndex());
+ Real energy = pendulum.getMass(state)*(0.5*(u[0]*u[0]+u[1]*u[1])
+ + pendulum.getGravity(state)*(1.0+q[1]));
+ Real expectedEnergy = pendulum.getMass(state)
+ * pendulum.getGravity(state);
+ if (ZeroPositionHandler::hasAccelerated)
+ expectedEnergy *= 1.5;
+ ASSERT(std::abs(1.0-energy/expectedEnergy) < 0.05);
+ }
+private:
+ PendulumSystem& pendulum;
+};
+
+class OnceOnlyEventReporter : public ScheduledEventReporter {
+public:
+ static bool hasOccurred;
+ OnceOnlyEventReporter() {
+ }
+ Real getNextEventTime(const State&, bool includeCurrentTime) const {
+ return 5.0;
+ }
+ void handleEvent(const State& state) const {
+ ASSERT(!hasOccurred);
+ hasOccurred = true;
+ }
+};
+
+class DiscontinuousReporter : public TriggeredEventReporter {
+public:
+ static int eventCount;
+ DiscontinuousReporter() : TriggeredEventReporter(Stage::Time) {
+ }
+ Real getValue(const State& state) const {
+ Real step = std::floor(state.getTime());
+ step = std::fmod(step, 4.0);
+ if (step == 0.0)
+ return 1.0;
+ if (step == 2.0)
+ return -1.0;
+ return 0.0;
+ }
+ void handleEvent(const State& state) const {
+
+ // This should be triggered when the value goes to 0, but not when it leaves 0.
+
+ Real t = state.getTime();
+ Real phase = std::fmod(t, 2.0);
+ ASSERT(std::abs(phase-1.0) < 0.01);
+ eventCount++;
+ }
+};
+
+
+int ZeroVelocityHandler::eventCount = 0;
+Real ZeroVelocityHandler::lastEventTime = 0.0;
+int PeriodicHandler::eventCount = 0;
+PeriodicHandler* PeriodicHandler::handler = 0;
+int ZeroPositionHandler::eventCount = 0;
+Real ZeroPositionHandler::lastEventTime = 0.0;
+bool ZeroPositionHandler::hasAccelerated = false;
+int PeriodicReporter::eventCount = 0;
+PeriodicReporter* PeriodicReporter::reporter = 0;
+bool OnceOnlyEventReporter::hasOccurred = false;
+int DiscontinuousReporter::eventCount = 0;
+
+void testIntegrator (Integrator& integ, PendulumSystem& sys, Real accuracy=1e-4) {
+ ZeroVelocityHandler::eventCount = 0;
+ ZeroVelocityHandler::lastEventTime = 0.0;
+ PeriodicHandler::eventCount = 0;
+ ZeroPositionHandler::eventCount = 0;
+ ZeroPositionHandler::lastEventTime = 0.0;
+ ZeroPositionHandler::hasAccelerated = false;
+ PeriodicReporter::eventCount = 0;
+ OnceOnlyEventReporter::hasOccurred = false;
+ DiscontinuousReporter::eventCount = 0;
+
+ const Real t0=0;
+ const Real tFinal = 20.003;
+ const Real qi[] = {1,0}; // (x,y)=(1,0)
+ const Real ui[] = {0,0}; // v=0
+ const Vector q0(2, qi);
+ const Vector u0(2, ui);
+
+ sys.setDefaultMass(10);
+ sys.setDefaultTimeAndState(t0, q0, u0);
+ integ.setAccuracy(accuracy);
+ integ.setConstraintTolerance(1e-4);
+ integ.setFinalTime(tFinal);
+
+ TimeStepper ts(sys);
+ ts.setIntegrator(integ);
+ ts.initialize(sys.getDefaultState());
+
+ // Try taking a series of steps of constant size.
+
+ ASSERT(ts.getTime() == 0.0);
+ Real time = 1.0;
+ for (; time < 5.0; time += 1.0) {
+ ts.stepTo(time);
+ ASSERT(ts.getTime() == time);
+ }
+ ASSERT(!OnceOnlyEventReporter::hasOccurred);
+ ASSERT(!ZeroPositionHandler::hasAccelerated);
+
+ // Try some steps of random sizes.
+
+ static Random::Uniform random(0.0, 1.0);
+ for (; time < 10.0; time += random.getValue()) {
+ ASSERT(OnceOnlyEventReporter::hasOccurred == (ts.getTime() >= 5.0));
+ ts.stepTo(time);
+ ASSERT(ts.getTime() == time);
+ }
+ ASSERT(ZeroPositionHandler::hasAccelerated);
+
+ // Try one large step that goes beyond tFinal.
+
+ ts.stepTo(50.0);
+ ASSERT(ts.getTime() == tFinal);
+ ASSERT(integ.getTerminationReason() == Integrator::ReachedFinalTime);
+ ASSERT(ZeroVelocityHandler::eventCount > 10);
+ ASSERT(PeriodicHandler::eventCount == (int) (ts.getTime()/PeriodicHandler::handler->getEventInterval())+1);
+ ASSERT(ZeroPositionHandler::eventCount > 10);
+ ASSERT(PeriodicReporter::eventCount == (int) (ts.getTime()/PeriodicReporter::reporter->getEventInterval())+1);
+ ASSERT(DiscontinuousReporter::eventCount == (int) (ts.getTime()/2.0));
+}
+
+#endif /*SimTK_SIMMATH_INTEGRATOR_TEST_FRAMEWORK_H_*/
diff --git a/SimTKmath/tests/IpoptDiffTest.cpp b/SimTKmath/tests/IpoptDiffTest.cpp
new file mode 100644
index 0000000..253b07b
--- /dev/null
+++ b/SimTKmath/tests/IpoptDiffTest.cpp
@@ -0,0 +1,185 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+using std::cout;
+using std::endl;
+using SimTK::Vector;
+using SimTK::Matrix;
+using SimTK::Real;
+using SimTK::Optimizer;
+using SimTK::OptimizerSystem;
+
+
+static int NUMBER_OF_PARAMETERS = 4;
+static int NUMBER_OF_EQUALITY_CONSTRAINTS = 1;
+static int NUMBER_OF_INEQUALITY_CONSTRAINTS = 1;
+
+/*
+ * Adapted from Ipopt's hs071 example
+ *
+ * min x1*x4*(x1 + x2 + x3) + x3
+ * s.t. x1*x2*x3*x4 >= 25
+ * x1**2 + x2**2 + x3**2 + x4**2 = 40
+ * 1 <= x1,x2,x3,x4 <= 5
+ *
+ * Starting point:
+ * x = (1, 5, 5, 1)
+ *
+ * Optimal solution:
+ * x = (1.00000000, 4.74299963, 3.82114998, 1.37940829)
+ *
+ */
+
+class ProblemSystem : public OptimizerSystem {
+public:
+
+
+ int objectiveFunc( const Vector &coefficients, bool new_coefficients, Real& f ) const {
+ const Real *x;
+
+ x = &coefficients[0];
+
+ f = x[0] * x[3] * (x[0] + x[1] + x[2]) + x[2];
+ return( 0 );
+ }
+
+ int gradientFunc( const Vector &coefficients, bool new_coefficients, Vector &gradient ) const{
+ const Real *x;
+
+ x = &coefficients[0];
+
+ gradient[0] = x[0] * x[3] + x[3] * (x[0] + x[1] + x[2]);
+ gradient[1] = x[0] * x[3];
+ gradient[2] = x[0] * x[3] + 1;
+ gradient[3] = x[0] * (x[0] + x[1] + x[2]);
+
+ return(0);
+
+ }
+ int constraintFunc( const Vector &coefficients, bool new_coefficients, Vector &constraints) const{
+ const Real *x;
+
+ x = &coefficients[0];
+ constraints[0] = x[0]*x[0] + x[1]*x[1] + x[2]*x[2] + x[3]*x[3] - 40.0;
+ constraints[1] = x[0] * x[1] * x[2] * x[3] - 25.0;
+
+ return(0);
+ }
+
+ int constraintJacobian( const Vector& coefficients, bool new_coefficients, Matrix& jac) const{
+ const Real *x;
+
+ x = &coefficients[0];
+
+ jac[0][0] = 2*x[0];
+ jac[0][1] = 2*x[1];
+ jac[0][2] = 2*x[2];
+ jac[0][3] = 2*x[3];
+ jac[1][0] = x[1]*x[2]*x[3];
+ jac[1][1] = x[0]*x[2]*x[3];
+ jac[1][2] = x[0]*x[1]*x[3];
+ jac[1][3] = x[0]*x[1]*x[2];
+
+
+ return(0);
+ }
+
+/* ProblemSystem() : OptimizerSystem( NUMBER_OF_PARAMETERS, NUMBER_OF_CONSTRAINTS ) {} */
+
+ ProblemSystem( const int numParams, const int numEqualityConstraints, const int numInequalityConstraints ) :
+ OptimizerSystem( numParams )
+ {
+ setNumEqualityConstraints( numEqualityConstraints );
+ setNumInequalityConstraints( numInequalityConstraints );
+ }
+
+};
+
+
+int main() {
+
+ Real f;
+ int i;
+
+ /* create the system to be optimized */
+ ProblemSystem sys(NUMBER_OF_PARAMETERS, NUMBER_OF_EQUALITY_CONSTRAINTS, NUMBER_OF_INEQUALITY_CONSTRAINTS);
+
+ Vector results(NUMBER_OF_PARAMETERS);
+ Vector lower_bounds(NUMBER_OF_PARAMETERS);
+ Vector upper_bounds(NUMBER_OF_PARAMETERS);
+
+ /* set initial conditions */
+ results[0] = 1.0;
+ results[1] = 5.0;
+ results[2] = 5.0;
+ results[3] = 1.0;
+
+ /* set bounds */
+ for(i=0;i<NUMBER_OF_PARAMETERS;i++) {
+ lower_bounds[i] = 1.0;
+ upper_bounds[i] = 5.0;
+ }
+
+ sys.setParameterLimits( lower_bounds, upper_bounds );
+
+ int returnValue = 0; // assume success
+ try {
+
+ Optimizer opt( sys );
+
+ opt.setConvergenceTolerance( 1e-4 );
+
+ opt.useNumericalGradient( true );
+ opt.useNumericalJacobian( true );
+ opt.setDiagnosticsLevel( 5 );
+
+ /* compute optimization */
+ f = opt.optimize( results );
+ }
+ catch (const std::exception& e) {
+ std::cout << e.what() << std::endl;
+ returnValue = 1; // failure
+ }
+
+ printf("IpoptTest.cpp: f = %f params = ",f);
+ for( i=0; i<NUMBER_OF_PARAMETERS; i++ ) {
+ printf(" %f",results[i]);
+ }
+ printf("\n");
+
+ static const Real TOL = 1e-4;
+ Real expected[] = { 1.00000000, 4.74299963, 3.82114998, 1.37940829 };
+ for( i=0; i<NUMBER_OF_PARAMETERS; i++ ) {
+ if( results[i] > expected[i]+TOL || results[i] < expected[i]-TOL) {
+ printf(" IpoptTest.cpp: error results[%d] = %f expected=%f \n",i,results[i], expected[i]);
+ returnValue = 1;
+ }
+ }
+
+ return( returnValue );
+
+
+}
diff --git a/SimTKmath/tests/IpoptTest.cpp b/SimTKmath/tests/IpoptTest.cpp
new file mode 100644
index 0000000..67550e4
--- /dev/null
+++ b/SimTKmath/tests/IpoptTest.cpp
@@ -0,0 +1,191 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+using std::cout;
+using std::endl;
+using SimTK::Vector;
+using SimTK::Matrix;
+using SimTK::Real;
+using SimTK::Optimizer;
+using SimTK::OptimizerSystem;
+
+
+static int NUMBER_OF_PARAMETERS = 4;
+static int NUMBER_OF_EQUALITY_CONSTRAINTS = 1;
+static int NUMBER_OF_INEQUALITY_CONSTRAINTS = 1;
+
+/*
+ * Adapted from Ipopt's hs071 example problem
+ *
+ * min x1*x4*(x1 + x2 + x3) + x3
+ * s.t. x1*x2*x3*x4 >= 25
+ * x1**2 + x2**2 + x3**2 + x4**2 = 40
+ * 1 <= x1,x2,x3,x4 <= 5
+ *
+ * Starting point:
+ * x = (1, 5, 5, 1)
+ *
+ * Optimal solution:
+ * x = (1.00000000, 4.74299963, 3.82114998, 1.37940829)
+ *
+ */
+
+class ProblemSystem : public OptimizerSystem {
+public:
+
+
+ int objectiveFunc( const Vector &coefficients, bool new_coefficients, Real& f ) const {
+ const Real *x;
+
+ x = &coefficients[0];
+
+ f = x[0] * x[3] * (x[0] + x[1] + x[2]) + x[2];
+ return( 0 );
+ }
+
+ int gradientFunc( const Vector &coefficients, bool new_coefficients, Vector &gradient ) const{
+ const Real *x;
+
+ x = &coefficients[0];
+
+ gradient[0] = x[0] * x[3] + x[3] * (x[0] + x[1] + x[2]);
+ gradient[1] = x[0] * x[3];
+ gradient[2] = x[0] * x[3] + 1;
+ gradient[3] = x[0] * (x[0] + x[1] + x[2]);
+
+ return(0);
+
+ }
+ int constraintFunc( const Vector &coefficients, bool new_coefficients, Vector &constraints) const{
+ const Real *x;
+
+ x = &coefficients[0];
+ constraints[0] = x[0]*x[0] + x[1]*x[1] + x[2]*x[2] + x[3]*x[3] - 40.0;
+ constraints[1] = x[0] * x[1] * x[2] * x[3] - 25.0;
+
+ return(0);
+ }
+
+ int constraintJacobian( const Vector& coefficients, bool new_coefficients, Matrix& jac) const{
+// int constraintJacobian( const Vector& coefficients, const bool new_coefficients, Vector& jac) const{
+ const Real *x;
+
+ x = &coefficients[0];
+
+ jac(0,0) = 2*x[0];
+ jac(0,1) = 2*x[1];
+ jac(0,2) = 2*x[2];
+ jac(0,3) = 2*x[3];
+ jac(1,0) = x[1]*x[2]*x[3];
+ jac(1,1) = x[0]*x[2]*x[3];
+ jac(1,2) = x[0]*x[1]*x[3];
+ jac(1,3) = x[0]*x[1]*x[2];
+
+ return(0);
+ }
+
+/* ProblemSystem() : OptimizerSystem( NUMBER_OF_PARAMETERS, NUMBER_OF_CONSTRAINTS ) {} */
+
+ ProblemSystem( const int numParams, const int numEqualityConstraints, const int numInequalityConstraints ) :
+ OptimizerSystem( numParams )
+ {
+ setNumEqualityConstraints( numEqualityConstraints );
+ setNumInequalityConstraints( numInequalityConstraints );
+ }
+
+};
+
+
+int main() {
+
+ Real f;
+ int i;
+
+ /* create the system to be optimized */
+ ProblemSystem sys(NUMBER_OF_PARAMETERS, NUMBER_OF_EQUALITY_CONSTRAINTS, NUMBER_OF_INEQUALITY_CONSTRAINTS);
+
+ Vector results(NUMBER_OF_PARAMETERS);
+ Vector lower_bounds(NUMBER_OF_PARAMETERS);
+ Vector upper_bounds(NUMBER_OF_PARAMETERS);
+
+ /* set initial conditions */
+ results[0] = 1.0;
+ results[1] = 5.0;
+ results[2] = 5.0;
+ results[3] = 1.0;
+
+ /* set bounds */
+ for(i=0;i<NUMBER_OF_PARAMETERS;i++) {
+ lower_bounds[i] = 1.0;
+ upper_bounds[i] = 5.0;
+ }
+
+ sys.setParameterLimits( lower_bounds, upper_bounds );
+
+
+ int returnValue = 0; // assume success
+
+ try {
+
+ Optimizer opt( sys );
+
+ opt.setConvergenceTolerance( 1e-4 );
+
+ opt.setDiagnosticsLevel( 7 );
+ opt.setLimitedMemoryHistory(500); // works well for our small systems
+
+ opt.setAdvancedBoolOption("warm_start",true);
+
+ opt.setAdvancedRealOption("obj_scaling_factor",1);
+
+ opt.setAdvancedRealOption("nlp_scaling_max_gradient",1);
+
+ /* compute optimization */
+ f = opt.optimize( results );
+ }
+ catch (const std::exception& e) {
+ std::cout << e.what() << std::endl;
+ returnValue = 1; // failure
+ printf("IpoptTest.cpp: Caught exception \n");
+ }
+
+ printf("IpoptTest.cpp: f = %f params = ",f);
+ for( i=0; i<NUMBER_OF_PARAMETERS; i++ ) {
+ printf(" %f",results[i]);
+ }
+ printf("\n");
+
+ static const Real TOL = 1e-4;
+ Real expected[] = { 1.00000000, 4.74299963, 3.82114998, 1.37940829 };
+ for( i=0; i<NUMBER_OF_PARAMETERS; i++ ) {
+ if( results[i] > expected[i]+TOL || results[i] < expected[i]-TOL) {
+ printf(" IpoptTest.cpp: error results[%d] = %f expected=%f \n",i,results[i], expected[i]);
+ returnValue = 1;
+ }
+ }
+
+ return( returnValue );
+}
diff --git a/SimTKmath/tests/LBFGSBDiffTest.cpp b/SimTKmath/tests/LBFGSBDiffTest.cpp
new file mode 100644
index 0000000..31bc4b4
--- /dev/null
+++ b/SimTKmath/tests/LBFGSBDiffTest.cpp
@@ -0,0 +1,155 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+using std::cout;
+using std::endl;
+using SimTK::Vector;
+using SimTK::Real;
+using SimTK::Optimizer;
+using SimTK::OptimizerSystem;
+
+
+const static int NUMBER_OF_PARAMETERS = 25;
+
+class ProblemSystem : public OptimizerSystem {
+
+ public:
+
+ ProblemSystem( const int numParameters ) : OptimizerSystem( numParameters ) {}
+
+ int objectiveFunc( const Vector &coefficients, bool new_coefficients, Real& f ) const {
+ int i;
+
+ const Real *x = &coefficients[0];
+
+ f = .25 *(x[0]-1.0)*(x[0]-1.0);
+ for(i=1;i<getNumParameters();i++) {
+ f = f + pow(x[i]-x[i-1]*x[i-1], 2.0);
+ }
+
+ f = 4.0* f;
+ return( 0 );
+ }
+
+ int gradientFunc( const Vector &coefficients, bool new_coefficients, Vector &gradient ) const {
+ const Real *x;
+ Real t1,t2;
+ int i;
+
+ x = &coefficients[0];
+
+ t1 = x[1]-(x[0]*x[0]);
+ gradient[0] = 2.0*(x[0]-1.0)-16.0*x[0]*t1;
+ for(i=1;i<getNumParameters()-1;i++) {
+ t2=t1;
+ t1=x[i+1]-(x[i]*x[i]);
+ gradient[i]=8.0*t2-16.0*x[i]*t1;
+ }
+ gradient[getNumParameters()-1]=8.0*t1;
+
+ return(0);
+
+ }
+
+};
+
+static bool equalToTol(Real v1, Real v2, Real tol) {
+ const Real scale = std::max(std::max(std::abs(v1), std::abs(v2)), Real(1));
+ return std::abs(v1-v2) < scale*tol;
+}
+
+/* adapted from driver1.f of Lbfgsb.2.1.tar.gz */
+int main() {
+
+ Real f;
+ int i;
+ int n = NUMBER_OF_PARAMETERS;
+
+ Vector results(NUMBER_OF_PARAMETERS);
+ Vector lower_bounds(NUMBER_OF_PARAMETERS);
+ Vector upper_bounds(NUMBER_OF_PARAMETERS);
+
+ ProblemSystem sys(NUMBER_OF_PARAMETERS);
+
+ /* set initial conditions */
+ for(i=0;i<n;i++) {
+ results[i] = 3.0;
+ }
+
+ /* set bounds */
+ for(i=0;i<n;i=i+2) { // even numbered
+ lower_bounds[i] = 1.0;
+ upper_bounds[i] = 100.0;
+ }
+ for(i=1;i<n;i=i+2) { // odd numbered
+ lower_bounds[i] = -100.0;
+ upper_bounds[i] = 100.0;
+ }
+
+ sys.setParameterLimits( lower_bounds, upper_bounds );
+
+ int returnValue = 0; // assume success
+ try {
+ Optimizer opt( sys );
+
+ opt.setConvergenceTolerance( .0001 );
+
+ opt.useNumericalGradient( true );
+
+ f = opt.optimize( results );
+
+ }
+
+ catch (const std::exception& e) {
+ std::cout << e.what() << std::endl;
+ returnValue = 1; // failure
+ }
+
+
+ printf("LBFGSBDiffTest.cpp: f = %f params = ",f);
+ for( i=0; i<NUMBER_OF_PARAMETERS; i++ ) {
+ printf(" %f",results[i]);
+ }
+ printf("\n");
+
+ static const Real TOL = 1e-3;
+ Real expected[] = { 1.000000, 0.999998, 1.000000, 1.000001, 1.000003,
+ 1.000006, 1.000007, 1.000012, 1.000022, 1.000040,
+ 1.000081, 1.000161, 1.000325, 1.000650, 1.001302,
+ 1.002603, 1.005214, 1.010450, 1.021013, 1.042466,
+ 1.086736, 1.180997, 1.394759, 1.945352, 3.784388 };
+ bool fail = false;
+ for( i=0; i<NUMBER_OF_PARAMETERS; i++ ) {
+ if(!equalToTol(results[i], expected[i], TOL)) {
+ printf(" LBFGSBDiffTest.cpp: error results[%d] = %f expected=%f \n",i,results[i], expected[i]);
+ returnValue = 1;
+ }
+ }
+
+ return( returnValue );
+
+
+}
diff --git a/SimTKmath/tests/LBFGSBTest.cpp b/SimTKmath/tests/LBFGSBTest.cpp
new file mode 100644
index 0000000..63f0292
--- /dev/null
+++ b/SimTKmath/tests/LBFGSBTest.cpp
@@ -0,0 +1,154 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+using std::cout;
+using std::endl;
+using SimTK::Vector;
+using SimTK::Real;
+using SimTK::Optimizer;
+using SimTK::OptimizerSystem;
+
+
+const static int NUMBER_OF_PARAMETERS = 25;
+
+class ProblemSystem : public OptimizerSystem {
+
+ public:
+
+ ProblemSystem( const int numParameters ) : OptimizerSystem( numParameters ) {}
+
+ int objectiveFunc( const Vector &coefficients, bool new_coefficients, Real& f ) const {
+ int i;
+
+ const Real *x = &coefficients[0];
+
+ f = .25 *(x[0]-1.0)*(x[0]-1.0);
+ for(i=1;i<getNumParameters();i++) {
+ f = f + pow(x[i]-x[i-1]*x[i-1], 2.0);
+ }
+
+ f = 4.0* f;
+ return( 0 );
+ }
+
+ int gradientFunc( const Vector &coefficients, bool new_coefficients, Vector &gradient ) const {
+ const Real *x;
+ Real t1,t2;
+ int i;
+
+ x = &coefficients[0];
+
+ t1 = x[1]-(x[0]*x[0]);
+ gradient[0] = 2.0*(x[0]-1.0)-16.0*x[0]*t1;
+ for(i=1;i<getNumParameters()-1;i++) {
+ t2=t1;
+ t1=x[i+1]-(x[i]*x[i]);
+ gradient[i]=8.0*t2-16.0*x[i]*t1;
+ }
+ gradient[getNumParameters()-1]=8.0*t1;
+
+ return(0);
+
+ }
+
+};
+
+static bool equalToTol(Real v1, Real v2, Real tol) {
+ const Real scale = std::max(std::max(std::abs(v1), std::abs(v2)), Real(1));
+ return std::abs(v1-v2) < scale*tol;
+}
+
+/* adapted from driver1.f of Lbfgsb.2.1.tar.gz */
+int main() {
+
+ Real f;
+ int i;
+ int n = NUMBER_OF_PARAMETERS;
+
+ Vector results(NUMBER_OF_PARAMETERS);
+ Vector lower_bounds(NUMBER_OF_PARAMETERS);
+ Vector upper_bounds(NUMBER_OF_PARAMETERS);
+
+ ProblemSystem sys(NUMBER_OF_PARAMETERS);
+
+ /* set initial conditions */
+ for(i=0;i<n;i++) {
+ results[i] = 3.0;
+ }
+
+ /* set bounds */
+ for(i=0;i<n;i=i+2) { // even numbered
+ lower_bounds[i] = 1.0;
+ upper_bounds[i] = 100.0;
+ }
+ for(i=1;i<n;i=i+2) { // odd numbered
+ lower_bounds[i] = -100.0;
+ upper_bounds[i] = 100.0;
+ }
+
+ sys.setParameterLimits( lower_bounds, upper_bounds );
+
+ int returnValue = 0; // assume success
+
+ try {
+ Optimizer opt( sys );
+
+ opt.setConvergenceTolerance( .0001 );
+ opt.setDiagnosticsLevel( 5 );
+ opt.setAdvancedRealOption( "factr", 1000);
+ f = opt.optimize( results );
+
+ }
+ catch (const std::exception& e) {
+ std::cout << e.what() << std::endl;
+ returnValue = 1; // failure
+ }
+
+
+
+ printf("LBFGSBTests.cpp: f = %f params = ",f);
+ for( i=0; i<NUMBER_OF_PARAMETERS; i++ ) {
+ printf(" %f",results[i]);
+ }
+ printf("\n");
+
+ static const Real TOL = 1e-3;
+ Real expected[] = { 1.000000, 0.999998, 1.000000, 1.000001, 1.000003,
+ 1.000006, 1.000007, 1.000012, 1.000022, 1.000040,
+ 1.000081, 1.000161, 1.000325, 1.000650, 1.001302,
+ 1.002603, 1.005214, 1.010450, 1.021013, 1.042466,
+ 1.086736, 1.180997, 1.394759, 1.945352, 3.784388 };
+ for( i=0; i<NUMBER_OF_PARAMETERS; i++ ) {
+ if(!equalToTol(results[i], expected[i], TOL)) {
+ printf(" LBFGSBTests.cpp: error results[%d] = %f expected=%f \n",i,results[i], expected[i]);
+ returnValue = 1;
+ }
+ }
+
+ return( returnValue );
+
+
+}
diff --git a/SimTKmath/tests/LBFGSDiffTest.cpp b/SimTKmath/tests/LBFGSDiffTest.cpp
new file mode 100644
index 0000000..037a918
--- /dev/null
+++ b/SimTKmath/tests/LBFGSDiffTest.cpp
@@ -0,0 +1,116 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+using std::cout;
+using std::endl;
+using SimTK::Vector;
+using SimTK::Real;
+using SimTK::Optimizer;
+using SimTK::OptimizerSystem;
+
+/* adapted from itkLBFGSOptimizerTest.cxx */
+
+const static int NUMBER_OF_PARAMETERS = 2;
+
+class ProblemSystem : public OptimizerSystem {
+ public:
+
+ ProblemSystem( int numParameters) : OptimizerSystem( numParameters){}
+
+ int objectiveFunc( const Vector &coefficients, bool new_coefficients, Real& f ) const {
+
+ const Real x = coefficients[0];
+ const Real y = coefficients[1];
+
+ f = 0.5*(3*x*x+4*x*y+6*y*y) - 2*x + 8*y;
+
+ return(0);
+
+ }
+
+ int gradientFunc( const Vector &coefficients, bool new_coefficients, Vector &gradient )const {
+
+ const Real x = coefficients[0];
+ const Real y = coefficients[1];
+
+ gradient[0] = 3*x + 2*y -2;
+ gradient[1] = 2*x + 6*y +8;
+
+ return(0);
+
+ }
+};
+
+static bool equalToTol(Real v1, Real v2, Real tol) {
+ const Real scale = std::max(std::max(std::abs(v1), std::abs(v2)), Real(1));
+ return std::abs(v1-v2) < scale*tol;
+}
+
+int main() {
+
+ int i;
+
+ ProblemSystem sys(NUMBER_OF_PARAMETERS);
+
+ Vector results(NUMBER_OF_PARAMETERS);
+
+ int returnValue = 0; // assume pass
+
+ try {
+
+ Optimizer opt( sys );
+
+ opt.setConvergenceTolerance( .0001 );
+ opt.useNumericalGradient( true );
+
+ results[0] = 100;
+ results[1] = -100;
+
+ opt.optimize( results );
+ }
+ catch (const std::exception& e) {
+ std::cout << e.what() << std::endl;
+ returnValue = 1; // failure
+ }
+
+ printf(" LBFGSDiffTest.cpp: results=");
+ for( i=0; i<NUMBER_OF_PARAMETERS; i++ ) {
+ printf(" %f",results[i]);
+ }
+ printf("\n");
+
+ static const Real TOL = 1e-4;
+ Real expected[] = { 2.0, -2.0 };
+ for( i=0; i<NUMBER_OF_PARAMETERS; i++ ) {
+ if(!equalToTol(results[i], expected[i], TOL)) {
+ printf(" LBFGSDiffTest.cpp: error results[%d] = %f expected=%f \n",i,results[i], expected[i]);
+ returnValue = 1;
+ }
+ }
+
+ return( returnValue );
+
+}
diff --git a/SimTKmath/tests/LBFGSTest.cpp b/SimTKmath/tests/LBFGSTest.cpp
new file mode 100644
index 0000000..c385d19
--- /dev/null
+++ b/SimTKmath/tests/LBFGSTest.cpp
@@ -0,0 +1,123 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Jack Middleton *
+ * Contributors: *
+ * *
+ * 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 <iostream>
+using std::cout;
+using std::endl;
+using namespace SimTK;
+
+/* adapted from itkLBFGSOptimizerTest.cxx */
+
+const static int NUMBER_OF_PARAMETERS = 2;
+
+class ProblemSystem : public OptimizerSystem {
+ public:
+
+ ProblemSystem( int numParameters) : OptimizerSystem( numParameters){}
+
+ int objectiveFunc( const Vector &coefficients, bool new_coefficients, Real& f ) const {
+
+ const Real x = coefficients[0];
+ const Real y = coefficients[1];
+
+ f = 0.5*(3*x*x+4*x*y+6*y*y) - 2*x + 8*y;
+
+ return(0);
+
+ }
+
+ int gradientFunc( const Vector &coefficients, bool new_coefficients, Vector &gradient )const {
+
+ const Real x = coefficients[0];
+ const Real y = coefficients[1];
+
+ gradient[0] = 3*x + 2*y -2;
+ gradient[1] = 2*x + 6*y +8;
+
+ return(0);
+
+ }
+};
+
+static const Real expected[] = { 2.0, -2.0 };
+
+
+static bool equalToTol(Real v1, Real v2, Real tol) {
+ const Real scale = std::max(std::max(std::abs(v1), std::abs(v2)), Real(1));
+ return std::abs(v1-v2) < scale*tol;
+}
+
+int main() {
+
+ int i;
+
+ ProblemSystem sys(NUMBER_OF_PARAMETERS);
+
+ Vector results(NUMBER_OF_PARAMETERS);
+
+ int returnValue = 0; // assume success
+ try {
+
+
+ Optimizer opt( sys );
+
+ opt.setConvergenceTolerance( .0001 );
+ results[0] = 100;
+ results[1] = -100;
+
+ opt.setAdvancedRealOption( "xtol", 1e-6 );
+
+ opt.optimize( results );
+
+ }
+ catch (const std::exception& e) {
+ std::cout << e.what() << std::endl;
+ returnValue = 1; // failure
+ }
+
+
+
+ static const Real TOL = 1e-4;
+
+ for( i=0; i<NUMBER_OF_PARAMETERS; i++ ) {
+ if(!equalToTol(results[i], expected[i], TOL)) {
+ printf(" LBFGSTest.cpp: error results[%d] = %f expected=%f \n",
+ i,results[i], expected[i]);
+ returnValue = 1;
+ }
+ }
+
+ if( returnValue == 0 ) {
+ printf("LBFGSTest.cpp results = ");
+ for( i=0; i<NUMBER_OF_PARAMETERS; i++ ) {
+ printf( "%f ", results[i]);
+ }
+ printf("\n");
+ }
+
+ return( returnValue );
+
+
+}
diff --git a/SimTKmath/tests/PendulumSystem.h b/SimTKmath/tests/PendulumSystem.h
new file mode 100644
index 0000000..bcfb129
--- /dev/null
+++ b/SimTKmath/tests/PendulumSystem.h
@@ -0,0 +1,549 @@
+#ifndef SimTK_SIMMATH_PENDULUMSYSTEM_H_
+#define SimTK_SIMMATH_PENDULUMSYSTEM_H_
+
+/* -------------------------------------------------------------------------- *
+ * SimTK Core: SimTK Simmath(tm) *
+ * -------------------------------------------------------------------------- *
+ * This is part of the SimTK Core 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. *
+ * *
+ * Portions copyright (c) 2006-7 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: *
+ * *
+ * Permission is hereby granted, free of charge, to any person obtaining a *
+ * copy of this software and associated documentation files (the "Software"), *
+ * to deal in the Software without restriction, including without limitation *
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense, *
+ * and/or sell copies of the Software, and to permit persons to whom the *
+ * Software is furnished to do so, subject to the following conditions: *
+ * *
+ * The above copyright notice and this permission notice shall be included in *
+ * all copies or substantial portions of the Software. *
+ * *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL *
+ * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, *
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR *
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE *
+ * USE OR OTHER DEALINGS IN THE SOFTWARE. *
+ * -------------------------------------------------------------------------- */
+
+/**
+ * This is a simple System that is used by various test cases.
+ */
+
+#include "SimTKcommon.h"
+#include "SimTKcommon/internal/SystemGuts.h"
+
+using namespace SimTK;
+
+class PendulumSystem;
+class PendulumSystemGuts: public System::Guts {
+ friend class PendulumSystem;
+
+ // TOPOLOGY STATE
+ SubsystemIndex subsysIndex;
+
+ // TOPOLOGY CACHE
+ mutable DiscreteVariableIndex massIndex, lengthIndex, gravityIndex;
+ mutable QIndex q0;
+ mutable UIndex u0;
+ mutable QErrIndex qerr0;
+ mutable UErrIndex uerr0;
+ mutable UDotErrIndex udoterr0;
+ mutable EventTriggerByStageIndex event0;
+ mutable CacheEntryIndex mgForceIndex; // a cache entry m*g calculated at Dynamics stage
+public:
+ PendulumSystemGuts() : Guts() {
+ // Index types set themselves invalid on construction.
+ }
+
+ const PendulumSystem& getPendulumSystem() const {
+ return reinterpret_cast<const PendulumSystem&>(getSystem());
+ }
+
+ SubsystemIndex getSubsysIndex() const {
+ return subsysIndex;
+ }
+
+ /*virtual*/PendulumSystemGuts* cloneImpl() const {return new PendulumSystemGuts(*this);}
+
+ /////////////////////////////////////////////////////////
+ // Implementation of continuous DynamicSystem virtuals //
+ /////////////////////////////////////////////////////////
+
+ /*virtual*/int realizeTopologyImpl(State&) const;
+ /*virtual*/int realizeModelImpl(State&) const;
+ /*virtual*/int realizeInstanceImpl(const State&) const;
+ /*virtual*/int realizePositionImpl(const State&) const;
+ /*virtual*/int realizeVelocityImpl(const State&) const;
+ /*virtual*/int realizeDynamicsImpl(const State&) const;
+ /*virtual*/int realizeAccelerationImpl(const State&) const;
+
+ // qdot==u here so these are just copies
+ /*virtual*/void multiplyByNImpl(const State& state, const Vector& u,
+ Vector& dq) const {dq=u;}
+ /*virtual*/void multiplyByNTransposeImpl(const State& state, const Vector& fq,
+ Vector& fu) const {fu=fq;}
+ /*virtual*/void multiplyByNPInvImpl(const State& state, const Vector& dq,
+ Vector& u) const {u=dq;}
+ /*virtual*/void multiplyByNPInvTransposeImpl(const State& state, const Vector& fu,
+ Vector& fq) const {fq=fu;}
+
+ // No prescribed motion.
+ /*virtual*/bool prescribeQImpl(State&) const {return false;}
+ /*virtual*/bool prescribeUImpl(State&) const {return false;}
+
+ /*virtual*/void projectQImpl(State&, Vector& qErrEst,
+ const ProjectOptions& options, ProjectResults& results) const;
+ /*virtual*/void projectUImpl(State&, Vector& uErrEst,
+ const ProjectOptions& options, ProjectResults& results) const;
+
+};
+
+class PendulumSystem: public System {
+public:
+ PendulumSystem() : System()
+ {
+ adoptSystemGuts(new PendulumSystemGuts());
+ DefaultSystemSubsystem defsub(*this);
+ updGuts().subsysIndex = defsub.getMySubsystemIndex();
+
+ setHasTimeAdvancedEvents(false);
+ }
+
+ const PendulumSystemGuts& getGuts() const {
+ return dynamic_cast<const PendulumSystemGuts&>(getSystemGuts());
+ }
+
+ PendulumSystemGuts& updGuts() {
+ return dynamic_cast<PendulumSystemGuts&>(updSystemGuts());
+ }
+
+ // Instance variables are written to our defaultState.
+ void setDefaultMass(Real mass) {
+ const PendulumSystemGuts& guts = getGuts();
+ updDefaultState().updDiscreteVariable(guts.subsysIndex, guts.massIndex) = Value<Real>(mass);
+ }
+
+ void setDefaultLength(Real length) {
+ const PendulumSystemGuts& guts = getGuts();
+ updDefaultState().updDiscreteVariable(guts.subsysIndex, guts.lengthIndex) = Value<Real>(length);
+ }
+
+ void setDefaultGravity(Real gravity) {
+ const PendulumSystemGuts& guts = getGuts();
+ updDefaultState().updDiscreteVariable(guts.subsysIndex, guts.gravityIndex) = Value<Real>(gravity);
+ }
+
+ void setDefaultTimeAndState(Real t, const Vector& q, const Vector& u) {
+ const PendulumSystemGuts& guts = getGuts();
+ updDefaultState().updU(guts.subsysIndex) = u;
+ updDefaultState().updQ(guts.subsysIndex) = q;
+ updDefaultState().updTime() = t;
+ }
+
+ Real getMass(const State& s) const {
+ const PendulumSystemGuts& guts = getGuts();
+ const AbstractValue& m = s.getDiscreteVariable(guts.subsysIndex, guts.massIndex);
+ return Value<Real>::downcast(m).get();
+ }
+ Real getDefaultMass() const {return getMass(getDefaultState());}
+
+ Real getLength(const State& s) const {
+ const PendulumSystemGuts& guts = getGuts();
+ const AbstractValue& d = s.getDiscreteVariable(guts.subsysIndex, guts.lengthIndex);
+ return Value<Real>::downcast(d).get();
+ }
+ Real getDefaultLength() const {return getLength(getDefaultState());}
+
+ Real getGravity(const State& s) const {
+ const PendulumSystemGuts& guts = getGuts();
+ const AbstractValue& g = s.getDiscreteVariable(guts.subsysIndex, guts.gravityIndex);
+ return Value<Real>::downcast(g).get();
+ }
+ Real getDefaultGravity() const {return getGravity(getDefaultState());}
+};
+
+/*
+ * This system is a 2d pendulum swinging in gravity. It is modeled as
+ * a point mass free in the plane, plus a distance constraint to model
+ * the rod.
+ *
+ * y | g O
+ * ^ v \ d
+ * | \
+ * | * m
+ * ------> x
+ *
+ * Gravity acts in the y direction, the rod is length d, mass m, pivot
+ * location is the ground origin (0,0).
+ *
+ * The DAE for a generic multibody system is:
+ * qdot = Qu
+ * M udot = f - ~A lambda
+ * A udot = b
+ * perr(t,q) = 0
+ * verr(t,q,u) = 0
+ *
+ * Let r^2 = x^2 + y^2
+ * v^2 = x'^2 + y'^2
+ * We will express the "rod length=d" constraint as
+ * (r^2 - d^2)/2 = 0 (perr)
+ * xx' + yy' = 0 (verr)
+ * xx'' + yy'' = -v^2 (aerr)
+ *
+ * So the matrix A = d perr/dq = [x y] and b = -v^2, and the
+ * equations of motion are:
+ * [ m 0 x ] [ x'' ] [ 0 ]
+ * [ 0 m y ] [ y'' ] = [ -mg ]
+ * [ x y 0 ] [ L ] [-v^2 ]
+ * where L (the Lagrange multiplier) is proportional to
+ * the rod tension. You can solve this to get
+ * L = (m*v^2 - mg*y)/(r^2)
+ * x'' = - x*L/m
+ * y'' = - y*L/m - g
+ *
+ */
+int PendulumSystemGuts::realizeTopologyImpl(State& s) const {
+ // Instance variables mass, length, gravity
+ massIndex = s.allocateDiscreteVariable(subsysIndex, Stage::Instance,
+ new Value<Real>(1));
+ lengthIndex = s.allocateDiscreteVariable(subsysIndex, Stage::Instance,
+ new Value<Real>(1));
+ gravityIndex = s.allocateDiscreteVariable(subsysIndex, Stage::Instance,
+ new Value<Real>(13.7503716373294544));
+ const Vector init(2, Real(0));
+ q0 = s.allocateQ(subsysIndex, init);
+ u0 = s.allocateU(subsysIndex, init);
+
+ mgForceIndex = s.allocateCacheEntry(subsysIndex, Stage::Dynamics,
+ new Value<Real>());
+ System::Guts::realizeTopologyImpl(s);
+ return 0;
+}
+int PendulumSystemGuts::realizeModelImpl(State& s) const {
+ System::Guts::realizeModelImpl(s);
+ return 0;
+}
+int PendulumSystemGuts::realizeInstanceImpl(const State& s) const {
+ qerr0 = s.allocateQErr(subsysIndex, 1);
+ uerr0 = s.allocateUErr(subsysIndex, 1);
+ udoterr0 = s.allocateUDotErr(subsysIndex, 1); // and multiplier
+// event0 = s.allocateEvent(subsysIndex, Stage::Position, 3);
+ System::Guts::realizeInstanceImpl(s);
+ return 0;
+}
+int PendulumSystemGuts::realizePositionImpl(const State& s) const {
+ const Real d = getPendulumSystem().getLength(s);
+ const Vector& q = s.getQ(subsysIndex);
+ // This is the perr() equation.
+ s.updQErr(subsysIndex)[0] = (q[0]*q[0] + q[1]*q[1] - d*d)/2;
+
+// s.updEventsByStage(subsysIndex, Stage::Position)[0] = 100*q[0]-q[1];
+//
+// s.updEventsByStage(subsysIndex, Stage::Position)[1] =
+// s.getTime() > 1.49552 && s.getTime() < 12.28937;
+//
+// s.updEventsByStage(subsysIndex, Stage::Position)[2] = s.getTime()-1.495508;
+ System::Guts::realizePositionImpl(s);
+ return 0;
+}
+
+int PendulumSystemGuts::realizeVelocityImpl(const State& s) const {
+ const Vector& q = s.getQ(subsysIndex);
+ const Vector& u = s.getU(subsysIndex);
+ Vector& qdot = s.updQDot(subsysIndex);
+
+ qdot[0] = u[0]; // qdot=u
+ qdot[1] = u[1];
+
+ // This is the verr() equation.
+ s.updUErr(subsysIndex)[0] = q[0]*u[0] + q[1]*u[1];
+ System::Guts::realizeVelocityImpl(s);
+ return 0;
+}
+
+int PendulumSystemGuts::realizeDynamicsImpl(const State& s) const {
+ const Real m = getPendulumSystem().getMass(s);
+ const Real g = getPendulumSystem().getGravity(s);
+
+ Real& mg = Value<Real>::downcast(s.updCacheEntry(subsysIndex, mgForceIndex)).upd();
+ // Calculate the force due to gravity.
+ mg = m*g;
+ System::Guts::realizeDynamicsImpl(s);
+ return 0;
+}
+
+int PendulumSystemGuts::realizeAccelerationImpl(const State& s) const {
+ const Real m = getPendulumSystem().getMass(s);
+ const Real g = getPendulumSystem().getGravity(s);
+ // we're pretending we couldn't calculate this here!
+ const Real mg = Value<Real>::downcast
+ (s.updCacheEntry(subsysIndex, mgForceIndex)).get();
+
+ const Vector& q = s.getQ(subsysIndex);
+ const Vector& u = s.getU(subsysIndex);
+ Vector& udot = s.updUDot(subsysIndex);
+
+ const Real r2 = q[0]*q[0] + q[1]*q[1];
+ const Real v2 = u[0]*u[0] + u[1]*u[1];
+ const Real L = (m*v2 - mg*q[1])/r2;
+ udot[0] = - q[0]*L/m;
+ udot[1] = - q[1]*L/m - g;
+ s.updQDotDot() = udot;
+ s.updMultipliers(subsysIndex)[0] = L;
+ s.updUDotErr(subsysIndex)[0] = q[0]*udot[0] + q[1]*udot[1] + v2;
+ System::Guts::realizeAccelerationImpl(s);
+ return 0;
+}
+
+/*
+ * Here we want to remove any constraint errors from the current state,
+ * and project out any component of the integrator's error estimate
+ * perpendicular to the constraint manifold. We will do this sequentially
+ * rather than handling position and velocity simultaneously.
+ *
+ * For this system we have P = d perr/dq = V = d verr/du = [x y].
+ * Weighted, we have PW=tp*[x/wx y/wy] VW=tv*[x/wxd y/wyd].
+ * With pinv(A)=~A*(A*~A)^-1, we have:
+ *
+ * pinv(P) = ~[ x y] / ( x ^2+ y ^2)
+ * pinv(PW) = ~(1/tp)*[(wx *wy ^2)*x (wx ^2*wy) *y] / ((wy *x)^2+(wx *y)^2)
+ * pinv(VW) = ~(1/tv)*[(wxd*wyd^2)*x (wxd^2*wyd)*y] / ((wyd*x)^2+(wxd*y)^2)
+ * (the latter assuming x,y already projected on position manifold)
+ *
+ * We want to solve
+ * |perr(q0 - dq)|_TRMS <= accuracy, such that dq=min_WLS(dq)
+ * PW(q0) dq = Tp * perr(q0); q = q0-dq
+ * Then
+ * |verr(q,u0 - du)|_TRMS <= accuracy, du=min_WLS(du)
+ * VW(q) du = Tv * verr(q,u0); u = u0-du
+ *
+ *
+ * To remove the corresponding error estimates:
+ * PW(q) qperp = PW(q) qerrest; qerrest -= qperp
+ * VW(q) uperp = VW(q) uerrest; uerrest -= uperp
+ *
+ *
+ */
+static Real wrms(const Vector& y, const Vector& w) {
+ Real sumsq = 0;
+ for (int i=0; i<y.size(); ++i)
+ sumsq += square(y[i]*w[i]);
+ return std::sqrt(sumsq/y.size());
+}
+
+//int PendulumSystemGuts::projectImpl(State& s, Real consAccuracy,
+// const Vector& yweights, const Vector& ctols,
+// Vector& yerrest, System::ProjectOptions opts) const // yerrest is in/out
+//{
+// const Vec2& wq = Vec2::getAs(&yweights[0]);
+// const Vec2& wu = Vec2::getAs(&yweights[2]);
+// const Real& tp = ctols[0]; // inverse tolerances 1/ti
+// const Real& tv = ctols[1];
+//
+// const Vec2& q = Vec2::getAs(&s.getQ(subsysIndex)[0]); // set up aliases
+// const Vec2& u = Vec2::getAs(&s.getU(subsysIndex)[0]);
+// Real& ep = s.updQErr(subsysIndex)[0];
+// Real& ev = s.updUErr(subsysIndex)[0];
+//
+// //cout << "BEFORE wperr=" << tp*ep << endl;
+//
+// Real wqchg;
+// if (opts.hasAnyPositionOptions()) {
+// do {
+// // Position projection
+// Real r2 = ~q*q; // x^2+y^2
+// Real wqr2 = square(wq[1]*q[0]) + square(wq[0]*q[1]);
+// Row2 P(~q), PW(tp*q[0]/wq[0], tp*q[1]/wq[1]);
+// Vec2 Pinv(q/r2);
+// Vec2 PWinv = Vec2(square(wq[1])*wq[0]*q[0],
+// square(wq[0])*wq[1]*q[1]) / (tp*wqr2);
+// Vec2 dq = Pinv*(ep); //cout << "dq=" << dq << endl;
+// Vec2 wdq = PWinv*(tp*ep); //cout << "wdq=" << wdq << endl;
+//
+// wqchg = std::sqrt(wdq.normSqr()/q.size()); // wrms norm
+//
+// s.updQ(subsysIndex)[0] -= wdq[0]/wq[0];
+// s.updQ(subsysIndex)[1] -= wdq[1]/wq[1];
+// realize(s, Stage::Position); // recalc QErr (ep)
+//
+// //cout << "AFTER q-=wdq/W wperr=" << tp*ep << " wqchg=" << wqchg << endl;
+// } while (std::abs(tp*ep) > consAccuracy && wqchg >= 0.01*consAccuracy);
+// }
+//
+// // Do velocity projection at new values of q
+// Real r2 = ~q*q; // x^2+y^2
+// Real wur2 = square(wu[1]*q[0]) + square(wu[0]*q[1]);
+// Row2 V(~q), VW(tv*q[0]/wu[0], tv*q[1]/wu[1]);
+// Vec2 Vinv(q/r2);
+// Vec2 VWinv = Vec2(square(wu[1])*wu[0]*q[0],
+// square(wu[0])*wu[1]*q[1]) / (tv*wur2);
+// realize(s, Stage::Velocity); // calculate UErr (ev)
+//
+// //cout << "BEFORE wverr=" << tv*ev << endl;
+// Vec2 du = Vinv*(ev); //cout << "du=" << du << endl;
+// Vec2 wdu = VWinv*(tv*ev); //cout << "wdu=" << wdu << endl;
+//
+// s.updU(subsysIndex)[0] -= wdu[0]/wu[0];
+// s.updU(subsysIndex)[1] -= wdu[1]/wu[1];
+//
+// realize(s, Stage::Velocity); // recalc UErr
+// //cout << "AFTER u-=wdu wverr=" << tv*ev << endl;
+//
+// // Now do error estimates.
+//
+//
+// if (yerrest.size()) {
+// Vec2& eq = Vec2::updAs(&yerrest[0]);
+// Vec2& eu = Vec2::updAs(&yerrest[2]);
+//
+// // Recalc PW, PWInv:
+// const Real wqr2 = square(wq[1]*q[0]) + square(wq[0]*q[1]);
+// const Row2 PW = Row2(tp*q[0]/wq[0], tp*q[1]/wq[1]);
+// const Vec2 PWinv = Vec2(wq[0]*square(wq[1])*q[0],
+// square(wq[0])*wq[1]*q[1]) / (tp*wqr2);
+//
+// Vec2 qperp = PWinv*(PW*eq);
+// Vec2 uperp = VWinv*(VW*eu);
+//
+// //cout << "ERREST before=" << yerrest
+// // << " wrms=" << wrms(yerrest,yweights) << endl;
+// //cout << "PW*eq=" << PW*eq << " VW*eu=" << VW*eu << endl;
+// eq -= qperp; eu -= uperp;
+//
+// //cout << "ERREST after=" << yerrest
+// // << " wrms=" << wrms(yerrest,yweights) << endl;
+// //cout << "PW*eq=" << PW*eq << " VW*eu=" << VW*eu << endl;
+// }
+//
+// return 0;
+//}
+
+// qerrest is in/out
+void PendulumSystemGuts::projectQImpl(State& s, Vector& qerrest,
+ const ProjectOptions& opts,
+ ProjectResults& results) const
+
+{
+ const Real consAccuracy = opts.getRequiredAccuracy();
+ const Vector& uweights = s.getUWeights(subsysIndex);
+ const Vector& ctols = s.getQErrWeights(subsysIndex);
+ // Since qdot=u here we can use uweights directly as qweights.
+ const Vec2& wq = Vec2::getAs(&uweights[0]);
+ const Real& tp = ctols[0]; // inverse tolerances 1/ti
+
+ const Vec2& q = Vec2::getAs(&s.getQ(subsysIndex)[0]); // set up aliases
+ Real& ep = s.updQErr(subsysIndex)[0];
+
+ //cout << "BEFORE wperr=" << tp*ep << endl;
+
+ Real wqchg;
+ do {
+ // Position projection
+ Real r2 = ~q*q; // x^2+y^2
+ Real wqr2 = square(wq[1]*q[0]) + square(wq[0]*q[1]);
+ Row2 P(~q), PW(tp*q[0]/wq[0], tp*q[1]/wq[1]);
+ Vec2 Pinv(q/r2);
+ Vec2 PWinv = Vec2(square(wq[1])*wq[0]*q[0],
+ square(wq[0])*wq[1]*q[1]) / (tp*wqr2);
+ Vec2 dq = Pinv*(ep); //cout << "dq=" << dq << endl;
+ Vec2 wdq = PWinv*(tp*ep); //cout << "wdq=" << wdq << endl;
+
+ wqchg = std::sqrt(wdq.normSqr()/q.size()); // wrms norm
+
+ s.updQ(subsysIndex)[0] -= wdq[0]/wq[0];
+ s.updQ(subsysIndex)[1] -= wdq[1]/wq[1];
+ realize(s, Stage::Position); // recalc QErr (ep)
+
+ //cout << "AFTER q-=wdq/W wperr=" << tp*ep << " wqchg=" << wqchg << endl;
+ } while (std::abs(tp*ep) > consAccuracy && wqchg >= 0.01*consAccuracy);
+
+ // Now do error estimates.
+
+ if (qerrest.size()) {
+ Vec2& eq = Vec2::updAs(&qerrest[0]);
+
+ // Recalc PW, PWInv:
+ const Real wqr2 = square(wq[1]*q[0]) + square(wq[0]*q[1]);
+ const Row2 PW = Row2(tp*q[0]/wq[0], tp*q[1]/wq[1]);
+ const Vec2 PWinv = Vec2(wq[0]*square(wq[1])*q[0],
+ square(wq[0])*wq[1]*q[1]) / (tp*wqr2);
+
+ Vec2 qperp = PWinv*(PW*eq);
+
+ //cout << "ERREST before=" << yerrest
+ // << " wrms=" << wrms(qerrest,qweights) << endl;
+ //cout << "PW*eq=" << PW*eq << endl;
+ eq -= qperp;
+
+ //cout << "ERREST after=" << yerrest
+ // << " wrms=" << wrms(qerrest,qweights) << endl;
+ //cout << "PW*eq=" << PW*eq << endl;
+ }
+
+ results.setExitStatus(ProjectResults::Succeeded);
+}
+
+void PendulumSystemGuts::projectUImpl(State& s, Vector& uerrest,
+ const ProjectOptions& opts, ProjectResults& results) const
+{
+ const Real consAccuracy = opts.getRequiredAccuracy();
+ const Vector& uweights = s.getUWeights(subsysIndex);
+ const Vector& ctols = s.getUErrWeights(subsysIndex);
+
+ const Vec2& wu = Vec2::getAs(&uweights[0]);
+ const Real& tv = ctols[0];
+
+ const Vec2& q = Vec2::getAs(&s.getQ(subsysIndex)[0]); // set up aliases
+ const Vec2& u = Vec2::getAs(&s.getU(subsysIndex)[0]);
+ Real& ev = s.updUErr(subsysIndex)[0];
+
+ //cout << "BEFORE wperr=" << tp*ep << endl;
+
+ // Do velocity projection at current values of q, which should have
+ // been projected already.
+ Real r2 = ~q*q; // x^2+y^2
+ Real wur2 = square(wu[1]*q[0]) + square(wu[0]*q[1]);
+ Row2 V(~q), VW(tv*q[0]/wu[0], tv*q[1]/wu[1]);
+ Vec2 Vinv(q/r2);
+ Vec2 VWinv = Vec2(square(wu[1])*wu[0]*q[0],
+ square(wu[0])*wu[1]*q[1]) / (tv*wur2);
+ realize(s, Stage::Velocity); // calculate UErr (ev)
+
+ //cout << "BEFORE wverr=" << tv*ev << endl;
+ Vec2 du = Vinv*(ev); //cout << "du=" << du << endl;
+ Vec2 wdu = VWinv*(tv*ev); //cout << "wdu=" << wdu << endl;
+
+ s.updU(subsysIndex)[0] -= wdu[0]/wu[0];
+ s.updU(subsysIndex)[1] -= wdu[1]/wu[1];
+
+ realize(s, Stage::Velocity); // recalc UErr
+ //cout << "AFTER u-=wdu wverr=" << tv*ev << endl;
+
+ // Now do error estimates.
+
+
+ if (uerrest.size()) {
+ Vec2& eu = Vec2::updAs(&uerrest[0]);
+ Vec2 uperp = VWinv*(VW*eu);
+
+ //cout << "ERREST before=" << uerrest
+ // << " wrms=" << wrms(uerrest,uweights) << endl;
+ //cout << " VW*eu=" << VW*eu << endl;
+ eu -= uperp;
+
+ //cout << "ERREST after=" << yerrest
+ // << " wrms=" << wrms(uerrest,uweights) << endl;
+ //cout << " VW*eu=" << VW*eu << endl;
+ }
+
+ results.setExitStatus(ProjectResults::Succeeded);
+}
+
+#endif /*SimTK_SIMMATH_PENDULUMSYSTEM_H_*/
diff --git a/SimTKmath/tests/RungeKutta2IntegratorTest.cpp b/SimTKmath/tests/RungeKutta2IntegratorTest.cpp
new file mode 100644
index 0000000..962daf8
--- /dev/null
+++ b/SimTKmath/tests/RungeKutta2IntegratorTest.cpp
@@ -0,0 +1,62 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "IntegratorTestFramework.h"
+#include "simmath/RungeKutta2Integrator.h"
+
+int main () {
+ try {
+ PendulumSystem sys;
+ sys.addEventHandler(new ZeroVelocityHandler(sys));
+ sys.addEventHandler(PeriodicHandler::handler = new PeriodicHandler());
+ sys.addEventHandler(new ZeroPositionHandler(sys));
+ sys.addEventReporter(PeriodicReporter::reporter = new PeriodicReporter(sys));
+ sys.addEventReporter(new OnceOnlyEventReporter());
+ sys.addEventReporter(new DiscontinuousReporter());
+ sys.realizeTopology();
+
+ // Test with various intervals for the event handler and event reporter,
+ // ones that are either large or small compared to the expected internal
+ // step size of the integrator.
+
+ for (int i = 0; i < 4; ++i) {
+ PeriodicHandler::handler->setEventInterval
+ (i == 0 || i == 1 ? 0.01 : 2.0);
+ PeriodicReporter::reporter->setEventInterval
+ (i == 0 || i == 2 ? 0.015 : 1.5);
+
+ // Test the integrator in both normal and single step modes.
+
+ RungeKutta2Integrator integ(sys);
+ testIntegrator(integ, sys);
+ integ.setReturnEveryInternalStep(true);
+ testIntegrator(integ, sys);
+ }
+ cout << "Done" << endl;
+ return 0;
+ }
+ catch (std::exception& e) {
+ std::printf("FAILED: %s\n", e.what());
+ return 1;
+ }
+}
diff --git a/SimTKmath/tests/RungeKutta3IntegratorTest.cpp b/SimTKmath/tests/RungeKutta3IntegratorTest.cpp
new file mode 100644
index 0000000..d629639
--- /dev/null
+++ b/SimTKmath/tests/RungeKutta3IntegratorTest.cpp
@@ -0,0 +1,62 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "IntegratorTestFramework.h"
+#include "simmath/RungeKutta3Integrator.h"
+
+int main () {
+ try {
+ PendulumSystem sys;
+ sys.addEventHandler(new ZeroVelocityHandler(sys));
+ sys.addEventHandler(PeriodicHandler::handler = new PeriodicHandler());
+ sys.addEventHandler(new ZeroPositionHandler(sys));
+ sys.addEventReporter(PeriodicReporter::reporter = new PeriodicReporter(sys));
+ sys.addEventReporter(new OnceOnlyEventReporter());
+ sys.addEventReporter(new DiscontinuousReporter());
+ sys.realizeTopology();
+
+ // Test with various intervals for the event handler and event reporter,
+ // ones that are either large or small compared to the expected internal
+ // step size of the integrator.
+
+ for (int i = 0; i < 4; ++i) {
+ PeriodicHandler::handler->setEventInterval
+ (i == 0 || i == 1 ? 0.01 : 2.0);
+ PeriodicReporter::reporter->setEventInterval
+ (i == 0 || i == 2 ? 0.015 : 1.5);
+
+ // Test the integrator in both normal and single step modes.
+
+ RungeKutta3Integrator integ(sys);
+ testIntegrator(integ, sys);
+ integ.setReturnEveryInternalStep(true);
+ testIntegrator(integ, sys);
+ }
+ cout << "Done" << endl;
+ return 0;
+ }
+ catch (std::exception& e) {
+ std::printf("FAILED: %s\n", e.what());
+ return 1;
+ }
+}
diff --git a/SimTKmath/tests/RungeKuttaFeldbergIntegratorTest.cpp b/SimTKmath/tests/RungeKuttaFeldbergIntegratorTest.cpp
new file mode 100644
index 0000000..fe91056
--- /dev/null
+++ b/SimTKmath/tests/RungeKuttaFeldbergIntegratorTest.cpp
@@ -0,0 +1,59 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "IntegratorTestFramework.h"
+#include "simmath/RungeKuttaFeldbergIntegrator.h"
+
+int main () {
+ try {
+ PendulumSystem sys;
+ sys.addEventHandler(new ZeroVelocityHandler(sys));
+ sys.addEventHandler(PeriodicHandler::handler = new PeriodicHandler());
+ sys.addEventHandler(new ZeroPositionHandler(sys));
+ sys.addEventReporter(PeriodicReporter::reporter = new PeriodicReporter(sys));
+ sys.addEventReporter(new OnceOnlyEventReporter());
+ sys.addEventReporter(new DiscontinuousReporter());
+ sys.realizeTopology();
+
+ // Test with various intervals for the event handler and event reporter, ones that are either
+ // large or small compared to the expected internal step size of the integrator.
+
+ for (int i = 0; i < 4; ++i) {
+ PeriodicHandler::handler->setEventInterval(i == 0 || i == 1 ? 0.01 : 2.0);
+ PeriodicReporter::reporter->setEventInterval(i == 0 || i == 2 ? 0.015 : 1.5);
+
+ // Test the integrator in both normal and single step modes.
+
+ RungeKuttaFeldbergIntegrator integ(sys);
+ testIntegrator(integ, sys);
+ integ.setReturnEveryInternalStep(true);
+ testIntegrator(integ, sys);
+ }
+ cout << "Done" << endl;
+ return 0;
+ }
+ catch (std::exception& e) {
+ std::printf("FAILED: %s\n", e.what());
+ return 1;
+ }
+}
diff --git a/SimTKmath/tests/RungeKuttaMersonIntegratorTest.cpp b/SimTKmath/tests/RungeKuttaMersonIntegratorTest.cpp
new file mode 100644
index 0000000..a2cc79c
--- /dev/null
+++ b/SimTKmath/tests/RungeKuttaMersonIntegratorTest.cpp
@@ -0,0 +1,59 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "IntegratorTestFramework.h"
+#include "simmath/RungeKuttaMersonIntegrator.h"
+
+int main () {
+ try {
+ PendulumSystem sys;
+ sys.addEventHandler(new ZeroVelocityHandler(sys));
+ sys.addEventHandler(PeriodicHandler::handler = new PeriodicHandler());
+ sys.addEventHandler(new ZeroPositionHandler(sys));
+ sys.addEventReporter(PeriodicReporter::reporter = new PeriodicReporter(sys));
+ sys.addEventReporter(new OnceOnlyEventReporter());
+ sys.addEventReporter(new DiscontinuousReporter());
+ sys.realizeTopology();
+
+ // Test with various intervals for the event handler and event reporter, ones that are either
+ // large or small compared to the expected internal step size of the integrator.
+
+ for (int i = 0; i < 4; ++i) {
+ PeriodicHandler::handler->setEventInterval(i == 0 || i == 1 ? 0.01 : 2.0);
+ PeriodicReporter::reporter->setEventInterval(i == 0 || i == 2 ? 0.015 : 1.5);
+
+ // Test the integrator in both normal and single step modes.
+
+ RungeKuttaMersonIntegrator integ(sys);
+ testIntegrator(integ, sys);
+ integ.setReturnEveryInternalStep(true);
+ testIntegrator(integ, sys);
+ }
+ cout << "Done" << endl;
+ return 0;
+ }
+ catch (std::exception& e) {
+ std::printf("FAILED: %s\n", e.what());
+ return 1;
+ }
+}
diff --git a/SimTKmath/tests/SemiExplicitEuler2IntegratorTest.cpp b/SimTKmath/tests/SemiExplicitEuler2IntegratorTest.cpp
new file mode 100644
index 0000000..d0a0f96
--- /dev/null
+++ b/SimTKmath/tests/SemiExplicitEuler2IntegratorTest.cpp
@@ -0,0 +1,68 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "IntegratorTestFramework.h"
+#include "simmath/SemiExplicitEuler2Integrator.h"
+
+int main () {
+ try {
+ PendulumSystem sys;
+ sys.addEventHandler(new ZeroVelocityHandler(sys));
+ sys.addEventHandler(PeriodicHandler::handler = new PeriodicHandler());
+ sys.addEventHandler(new ZeroPositionHandler(sys));
+ sys.addEventReporter(PeriodicReporter::reporter = new PeriodicReporter(sys));
+ sys.addEventReporter(new OnceOnlyEventReporter());
+ sys.addEventReporter(new DiscontinuousReporter());
+ sys.realizeTopology();
+
+ // Test with various intervals for the event handler and event reporter,
+ // ones that are either large or small compared to the expected internal
+ // step size of the integrator.
+
+ #ifndef NDEBUG
+ const int NumIters = 4;
+ #else
+ const int NumIters = 1; // takes too long in Debug
+ #endif
+
+ for (int i = 0; i < 4; ++i) {
+ PeriodicHandler::handler->setEventInterval
+ (i == 0 || i == 1 ? 0.01 : 2.0);
+ PeriodicReporter::reporter->setEventInterval
+ (i == 0 || i == 2 ? 0.015 : 1.5);
+
+ // Test the integrator in both normal and single step modes.
+
+ SemiExplicitEuler2Integrator integ(sys);
+ testIntegrator(integ, sys, 1e-6);
+ integ.setReturnEveryInternalStep(true);
+ testIntegrator(integ, sys, 1e-6);
+ }
+ cout << "Done" << endl;
+ return 0;
+ }
+ catch (std::exception& e) {
+ std::printf("FAILED: %s\n", e.what());
+ return 1;
+ }
+}
diff --git a/SimTKmath/tests/SemiExplicitEulerIntegratorTest.cpp b/SimTKmath/tests/SemiExplicitEulerIntegratorTest.cpp
new file mode 100644
index 0000000..18b3079
--- /dev/null
+++ b/SimTKmath/tests/SemiExplicitEulerIntegratorTest.cpp
@@ -0,0 +1,69 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "IntegratorTestFramework.h"
+#include "simmath/SemiExplicitEulerIntegrator.h"
+
+int main () {
+ try {
+ PendulumSystem sys;
+ sys.addEventHandler(new ZeroVelocityHandler(sys));
+ sys.addEventHandler(PeriodicHandler::handler = new PeriodicHandler());
+ sys.addEventHandler(new ZeroPositionHandler(sys));
+ sys.addEventReporter(PeriodicReporter::reporter = new PeriodicReporter(sys));
+ sys.addEventReporter(new OnceOnlyEventReporter());
+ sys.addEventReporter(new DiscontinuousReporter());
+ sys.realizeTopology();
+
+ // Test with various intervals for the event handler and event reporter,
+ // ones that are either large or small compared to the expected internal
+ // step size of the integrator.
+
+ #ifndef NDEBUG
+ const int NumIters = 4;
+ #else
+ const int NumIters = 1; // takes too long in Debug
+ #endif
+
+ for (int i = 0; i < 4; ++i) {
+ PeriodicHandler::handler->setEventInterval
+ (i == 0 || i == 1 ? 0.01 : 2.0);
+ PeriodicReporter::reporter->setEventInterval
+ (i == 0 || i == 2 ? 0.015 : 1.5);
+
+ // Test the integrator in both normal and single step modes.
+
+ const Real FixedStepSize = 1e-4;
+ SemiExplicitEulerIntegrator integ(sys, FixedStepSize);
+ testIntegrator(integ, sys);
+ integ.setReturnEveryInternalStep(true);
+ testIntegrator(integ, sys);
+ }
+ cout << "Done" << endl;
+ return 0;
+ }
+ catch (std::exception& e) {
+ std::printf("FAILED: %s\n", e.what());
+ return 1;
+ }
+}
diff --git a/SimTKmath/tests/SimpleDifferentiatorTest.cpp b/SimTKmath/tests/SimpleDifferentiatorTest.cpp
new file mode 100644
index 0000000..6499a74
--- /dev/null
+++ b/SimTKmath/tests/SimpleDifferentiatorTest.cpp
@@ -0,0 +1,127 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This is a test program which uses the Differentiator class in various ways.
+ */
+
+#include "SimTKmath.h"
+
+// Just so we can get the version number:
+#include "SimTKlapack.h"
+
+#include <cstdio>
+#include <iostream>
+
+using SimTK::Real;
+using SimTK::Vector;
+using SimTK::Matrix;
+using SimTK::Differentiator;
+using std::printf;
+using std::cout;
+using std::endl;
+
+// This is a system of functions of a particular set of parameters (state).
+// The underlying function wants time also, so we provide that as data in
+// the concrete class. Time should be set prior to calculation of the Jacobian.
+class MyVectorFunc : public Differentiator::JacobianFunction {
+public:
+ MyVectorFunc(int nf, int ny)
+ : Differentiator::JacobianFunction(nf,ny), time(0) { }
+
+ void setTime(Real t) {time=t;}
+ Real getTime() const {return time;}
+
+ // Must provide this pure virtual function.
+ int f(const Vector& y, Vector& fy) const;
+private:
+ Real time;
+};
+
+// This is a single scalar function of a vector of parameters.
+class MyObjectiveFunc : public Differentiator::GradientFunction {
+public:
+ MyObjectiveFunc(int ny)
+ : Differentiator::GradientFunction(ny), time(0) { }
+
+ void setTime(Real t) {time=t;}
+ Real getTime() const {return time;}
+
+ // Must provide this pure virtual function.
+ int f(const Vector& y, Real& fy) const;
+private:
+ Real time;
+};
+
+// This represents a generic scalar function of a scalar parameter,
+// where the actual function has a simple C signature.
+class GenericScalarFunc : public Differentiator::ScalarFunction {
+ typedef Real (*CFunc)(Real);
+public:
+ GenericScalarFunc(CFunc cf)
+ : Differentiator::ScalarFunction(), cp(cf) { }
+
+ // Must provide this pure virtual function.
+ int f(Real x, Real& fx) const {
+ fx = cp(x);
+ return 0;
+ }
+
+ CFunc cp;
+};
+
+class SinOmegaX : public Differentiator::ScalarFunction {
+public:
+ SinOmegaX(Real omega) : w(omega) { }
+
+ // Must provide this virtual function.
+ int f(Real x, Real& fx) const {
+ fx = std::sin(w*x);
+ return 0; // success
+ }
+private:
+ const Real w;
+};
+
+
+int main () {
+ try {
+ const Real w=3;
+ SinOmegaX sinwx(w); // user-written class
+ Differentiator dsinwx(sinwx);
+
+ const Real x = 1.234;
+ Real exact = w*std::cos(w*x);
+ Real approx = dsinwx.calcDerivative(x);
+
+ std::printf("exact =%16.12f\n", exact);
+ std::printf("approx=%16.12f err=%.3e\n",
+ approx, std::abs((approx-exact)/exact));
+
+ return 0;
+ }
+ catch (std::exception& e) {
+ std::printf("FAILED: %s\n", e.what());
+ return 1;
+ }
+}
diff --git a/SimTKmath/tests/TestBicubicSurface.cpp b/SimTKmath/tests/TestBicubicSurface.cpp
new file mode 100644
index 0000000..d385d90
--- /dev/null
+++ b/SimTKmath/tests/TestBicubicSurface.cpp
@@ -0,0 +1,1096 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Matthew Millard *
+ * Contributors: Michael Sherman *
+ * *
+ * 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 the private implementation class declaration for testing purposes;
+// this is not part of the API.
+#include "../src/BicubicSurface_Guts.h"
+
+
+#include <cstdio>
+#include <iostream>
+#include <fstream>
+
+using namespace SimTK;
+using namespace std;
+
+/**
+This function computes a standard central difference dy/dx.
+If extrap_endpoints is set to 1, then the derivative at the end points
+is estimated by linearly extrapolating the dy/dx values beside the end points
+
+ @param x domain vector
+ @param y range vector
+ @param extrap_endpoints:(false) Endpoints of the returned vector will be zero,
+ because a central difference is undefined at
+ these endpoints
+ (true) Endpoints are computed by linearly
+ extrapolating using a first difference from
+ the neighboring 2 points
+
+ @returns dy/dx computed using central differences
+*/
+Vector getCentralDifference(Vector x, Vector y, bool extrap_endpoints) {
+ Vector dy(x.size());
+ Real dx1,dx2;
+ Real dy1,dy2;
+ int size = x.size();
+ for(int i=1; i<x.size()-1; i++){
+ dx1 = x(i)-x(i-1);
+ dx2 = x(i+1)-x(i);
+ dy1 = y(i)-y(i-1);
+ dy2 = y(i+1)-y(i);
+ dy(i)= 0.5*dy1/dx1 + 0.5*dy2/dx2;
+ }
+
+ if(extrap_endpoints == true){
+ dy1 = dy(2)-dy(1);
+ dx1 = x(2)-x(1);
+ dy(0) = dy(1) + (dy1/dx1)*(x(0)-x(1));
+
+ dy2 = dy(size-2)-dy(size-3);
+ dx2 = x(size-2)-x(size-3);
+ dy(size-1) = dy(size-2) + (dy2/dx2)*(x(size-1)-x(size-2));
+ }
+ return dy;
+}
+
+/**
+ Return the value, and set of first partial deriviatives of a 2D function f
+ that defines a surface f(x,y). There are 4 functions of choice, which
+ can be specified by the paramter fcnType
+
+ @param x : x argument of the function f(x,y)
+ @param y : y argument of the function f(x,y)
+ @param fcnType [0,1,2,3,4]. Chooses one of the following functions for
+ f(x,y):
+
+ fcnType = 0 :f(x,y) = 0;
+ fcnType = 1 :f(x,y) = 2*x + y
+ fcnType = 2 :f(x,y) = xy
+ fcnType = 3 :f(x,y) = cos( (3x^2+y^2)^0.5 )
+ fcnType = 4 : f(x,y) = 3x^2 + y^2
+*/
+Vector getAnalyticFunction(Real x, Real y, int fcnType){
+ Vector fdF(4);
+ fdF = -1;
+
+ switch(fcnType){
+ case 0: //f(x,y) = 0;
+ fdF = 0;
+ break;
+ case 1: //f(x,y) = 2*x + y
+ fdF(0) = 2*x + y; //f
+ fdF(1) = 2; //fx
+ fdF(2) = 1; //fy
+ fdF(3) = 0; //fxy
+ break;
+ case 2: //f(x,y) = xy
+ fdF(0) = x*y; //f
+ fdF(1) = y; //fx
+ fdF(2) = x; //fy
+ fdF(3) = 1; //fxy
+ break;
+ case 3: //f(x,y) = cos( (3x^2+y^2)^0.5 );
+ //f
+ fdF(0) = cos( sqrt((3*x*x + y*y)) );
+ //fx - exported from Maple (didn't trust myself not to make a typing mistake
+ fdF(1) = -0.3e1*x*sin( sqrt( (3*x*x + y*y)) ) * pow( (3*x*x + y*y + 1e-6), -0.1e1 / 0.2e1) ;
+ //fy
+ fdF(2) = -sin(sqrt( (3 * x * x + y * y))) * pow((3 * x * x + y * y + 1e-6), -0.1e1 / 0.2e1) * y;
+ //fxy
+ fdF(3) = -0.3e1 * cos(sqrt((3 * x * x + y * y)))/(3 * x * x + y * y + 1e-6) * y * x + 0.3e1 * sin(sqrt( (3 * x * x + y * y))) * pow( (3 * x * x + y * y + 1e-6), -0.3e1 / 0.2e1) * x * y;
+
+ break;
+ case 4: //f(x,y) = 3x^2 + y^2
+ fdF(0) = 3*x*x + y*y;
+ fdF(1) = 6*x;
+ fdF(2) = 2*y;
+ fdF(3) = 0;
+ break;
+ default:
+ cout << "Invalid fcnType in testBicubicSurface.cpp: getAnayticFunction";
+ }
+
+
+ return fdF;
+}
+
+
+/**
+ This function will generate a rectangular grid that spans from xmin to xmax
+ in size number of steps, and also from ymin to ymax in size number of steps.
+ Although the grid spacing can be different in the x and y dimensions, within
+ these dimensions the grids are equally spaced (by xDelta and yDelta).
+
+ An analytic function (chosen using the fcnType variable) is used to generate
+ f(x,y) values at each grid point. These values are used to initialize a
+ bicubic surface using the advanced test constructor that sets the partial
+ derivatives fx, fy, and fxy directly.
+
+ The values of the bicubic surface are evaluated at the grid points, we'll
+ call them knot points, and are asserted to be equal to the analytic function
+ at these values. Additionally, every grid is evaluated at its center, and
+ the value of the bicubic surface is asserted to be equal to the analytic
+ function at the mid point to within a tolerance. This tolerance is a function
+ of the grid size. This tolerance has been determined hueristically, so if you
+ try a new function and the test fails, look closely at the values to see if
+ its really failing or if the tolerance is just too tight.
+
+ @params xmin: the minimum value of the x,y grid in the x dimension
+ @params xmax: the maximum value of the x,y grid in the x dimension
+ @params ymin: the minimum value of the y grid in the y dimension
+ @params ymax: the maximum value of the y grid in the y dimension
+ @params size: the number of steps to take to go from xmin to xmax, and
+ ymin to ymax
+ @params fcnType: An integer value [0-4] that picks an analytical function
+ to use for comparision purposes.
+ @params flag_verbosePrint: false: print only the maximum error at the
+ knot points, mid points and the
+ tolerance used at the assertions
+
+ true: Additionally print the values of f, fx,
+ fy, and fxy at the knots and the mid
+ points if there are less than 10 steps
+ at params flag_matlabcompre: true: Will print
+ at returns nothing
+*/
+void testBicubicAgainstAnalyticFcn(Real xmin, Real xmax, Real ymin,
+ Real ymax, int size, int fcnType, bool flag_verbosePrint,
+ bool flag_matlabcompare){
+
+ Real deltaX,deltaY;
+ deltaX = (xmax-xmin)/(size-1);
+ deltaY = (ymax-ymin)/(size-1);
+
+ //Generate initialization data
+ // two constant spaced vectors & height matrix & first derivatives to initialize the grid
+ Vector x(size), y(size);
+ Matrix z(size,size),zx(size,size),zy(size,size),zxy(size,size);
+
+ //Generate test data to evaluate the error of the surface interpolation at the mid
+ //point of each grid square. The `M' stands for mid-point
+ Vector xM(size-1), yM(size-1);
+ Matrix zM(size-1,size-1),zMx(size-1,size-1),zMy(size-1,size-1),zMxy(size-1,size-1);
+
+ for (int i = 0; i < size; i++) {
+ x(i) = xmin + ((Real)i)*deltaX;
+ y(i) = ymin + ((Real)i)*deltaY;
+ if(i<size-1){
+ xM(i) = xmin + deltaX/(Real)2 + ((Real)i)*deltaX;
+ yM(i) = ymin + deltaY/(Real)2 + ((Real)i)*deltaY;
+ }
+ }
+
+
+ switch(fcnType){
+ case 0:
+ cout << "Testing bicubic surface against: f(x,y) = 0" <<endl;
+ break;
+ case 1:
+ cout << "Testing bicubic surface against: f(x,y) = 2*x+y" <<endl;
+ break;
+ case 2:
+ cout << "Testing bicubic surface against: f(x,y) = x*y" <<endl;
+ break;
+ case 3:
+ cout << "Testing bicubic surface against: f(x,y) = cos( (3*x^2 + y^2)^0.5 ) " <<endl;
+ break;
+ case 4:
+ cout << "Testing bicubic surface against: f(x,y) = 3*x^2 + y^2 " <<endl;
+ break;
+ }
+
+
+ Vector fdF(4);
+ Vector fdFM(4);
+ fdF = 0;
+ fdFM= 0;
+
+ for(int i=0; i<size;i++){
+ for(int j=0; j<size; j++){
+ fdF = getAnalyticFunction(x(i),y(j),fcnType);
+ //printf("i:%d, j:%d, x:%f, y:%f, f:%f, fx:%f, fy:%f, fxy:%f\n",i,j,x(i),y(j),fdF(0),fdF(1),fdF(2),fdF(3));
+
+ z(i,j) = fdF(0);
+ zx(i,j) = fdF(1);
+ zy(i,j) = fdF(2);
+ zxy(i,j) = fdF(3);
+
+ if( i < size-1 && j < size-1){
+ fdFM = getAnalyticFunction(xM(i),yM(j),fcnType);
+ zM(i,j) = fdFM(0);
+ zMx(i,j) = fdFM(1);
+ zMy(i,j) = fdFM(2);
+ zMxy(i,j) = fdFM(3);
+ }
+ }
+ }
+
+
+ //Initialize the Bicubic Surface
+ Real smoothness = 0.0;
+ BicubicSurface bcs(x, y, z, zx, zy, zxy);
+ const BicubicSurface::Guts& bcsg = bcs.getGuts();
+ BicubicFunction bcsf(bcs);
+
+ if(flag_verbosePrint == true && size <= 10){
+ cout << "\n\nx:\n" << bcsg.getx() << endl;
+ cout << "\n\ny:\n" << bcsg.gety() << endl;
+ cout << "\n\nff:\n" << bcsg.getff() << endl;
+ }
+
+ //Test it at the knot points, mid grid and compute the error
+ Vector errV(4); //Knot point error vector: f,fx,fy,fxy error
+ Vector errVM(4);//Mid grid error vector: f,fx,fy,fxy error
+
+ Vector bcsV(4); //Spline surface values at the knots
+ Vector bcsMV(4); //Spline surface values at the midpoints
+
+ Vector XY(2); //XY value at the knot points;
+ Vector XYM(2); //XY value at mid grid;
+
+ const int ifxy[] = {1,0};
+ const int ifxx[] = {0,0};
+ const int ifyy[] = {1,1};
+ const int ifxxx[] = {0,0,0};
+ const int ifxxy[] = {0,0,1};
+ const int ifyyy[] = {1,1,1};
+ const int ifxyy[] = {1,1,0};
+
+ Array_<int> fx(1); //Arguments required to get the correct derivative
+ Array_<int> fy(1); // from the calcDerivatie interface
+ Array_<int> fxy(ifxy,ifxy+2);
+ Array_<int> fxx(ifxx,ifxx+2);
+ Array_<int> fyy(ifyy,ifyy+2);
+ Array_<int> fxxx(ifxxx,ifxxx+3);
+ Array_<int> fyyy(ifyyy,ifyyy+3);
+ Array_<int> fxxy(ifxxy,ifxxy+3);
+ Array_<int> fxyy(ifxyy,ifxyy+3);
+
+ fx[0] =0;
+ fy[0] =1;
+
+ errV = 0;
+ errVM = 0;
+
+ Matrix fk(size,size),fxk(size,size),fyk(size,size);
+ Matrix fxyk(size,size),fxxk(size,size),fyyk(size,size);
+ Matrix fxxyk(size,size),fxyyk(size,size);
+ Matrix fxxxk(size,size),fyyyk(size,size);
+
+ Matrix fMk(size-1,size-1),fxMk(size-1,size-1),fyMk(size-1,size-1);
+ Matrix fxyMk(size-1,size-1),fxxMk(size-1,size-1),fyyMk(size-1,size-1);
+ Matrix fxxyMk(size-1,size-1),fxyyMk(size-1,size-1);
+ Matrix fxxxMk(size-1,size-1),fyyyMk(size-1,size-1);
+
+ for(int i=0; i<size; i++){
+ for(int j=0; j<size; j++){
+ XY(0)=x(i);
+ XY(1)=y(j);
+
+ fk(i,j) = bcsf.calcValue(XY);
+ fxk(i,j) = bcsf.calcDerivative(fx,XY);
+ fyk(i,j) = bcsf.calcDerivative(fy,XY);
+ fxyk(i,j) = bcsf.calcDerivative(fxy,XY);
+ fxxk(i,j) = bcsf.calcDerivative(fxx,XY);
+ fyyk(i,j) = bcsf.calcDerivative(fyy,XY);
+
+ fxxxk(i,j) = bcsf.calcDerivative(fxxx,XY);
+ fyyyk(i,j) = bcsf.calcDerivative(fyyy,XY);
+ fxxyk(i,j) = bcsf.calcDerivative(fxxy,XY);
+ fxyyk(i,j) = bcsf.calcDerivative(fxyy,XY);
+
+ if( errV(0) < abs(fk(i,j) - z(i,j)) )
+ errV(0) = abs(fk(i,j) - z(i,j));
+ if( errV(1) < abs(fxk(i,j) - zx(i,j)) )
+ errV(1) = abs(fxk(i,j) - zx(i,j));
+ if( errV(2) < abs(fyk(i,j) - zy(i,j)) )
+ errV(2) = abs(fyk(i,j) - zy(i,j));
+ if( errV(3) < abs(fxyk(i,j) - zxy(i,j)) )
+ errV(3) = abs(fxyk(i,j) - zxy(i,j));
+
+ /*if(abs(errV(0)) > 1e-4 ){
+ printf("Analytic (x,y),f,fx,fy,fxy: (%g,%g),%g, %g, %g, %g\n",
+ x(i),y(j),z(i,j),zx(i,j),zy(i,j),zxy(i,j));
+ printf("Approx. (x,y),f,fx,fy,fxy: (%g,%g),%g, %g, %g, %g\n\n",
+ x(i),y(j),fk(i,j),fxk(i,j),fyk(i,j),fxyk(i,j));
+ bcs.setDebug(true);
+ }*/
+
+ if(i<size-1 && j<size-1){
+ XYM(0)=xM(i);
+ XYM(1)=yM(j);
+ fMk(i,j) = bcsf.calcValue(XYM);
+ fxMk(i,j) = bcsf.calcDerivative(fx,XYM);
+ fyMk(i,j) = bcsf.calcDerivative(fy,XYM);
+ fxyMk(i,j) = bcsf.calcDerivative(fxy,XYM);
+ fxxMk(i,j) = bcsf.calcDerivative(fxx,XYM);
+ fyyMk(i,j) = bcsf.calcDerivative(fyy,XYM);
+
+ fxxxMk(i,j) = bcsf.calcDerivative(fxxx,XYM);
+ fyyyMk(i,j) = bcsf.calcDerivative(fyyy,XYM);
+ fxxyMk(i,j) = bcsf.calcDerivative(fxxy,XYM);
+ fxyyMk(i,j) = bcsf.calcDerivative(fxyy,XYM);
+
+ if( errVM(0) < abs(fMk(i,j) - zM(i,j)) )
+ errVM(0) = abs(fMk(i,j) - zM(i,j));
+ if( errVM(1) < abs(fxMk(i,j) - zMx(i,j)) )
+ errVM(1) = abs(fxMk(i,j) - zMx(i,j));
+ if( errVM(2) < abs(fyMk(i,j) - zMy(i,j)) )
+ errVM(2) = abs(fyMk(i,j) - zMy(i,j));
+ if( errVM(3) < abs(fxyMk(i,j) - zMxy(i,j)) )
+ errVM(3) = abs(fxyMk(i,j) - zMxy(i,j));
+
+ }
+ }
+ }
+
+
+ if(flag_verbosePrint == true && size <= 10){
+
+ cout << "\n\n Err f (@knot, calc):\n" << fk-z << endl;
+ cout << "\n\n Err fx (@knot, calc):\n" << fxk -zx << endl;
+ cout << "\n\n Err fy (@knot, calc):\n" << fyk -zy << endl;
+ cout << "\n\n Err fxy (@knot, calc):\n" << fxyk -zxy << endl;
+
+ if(flag_matlabcompare == true){
+ cout << "\n\n x (@knot):\n" << x << endl;
+ cout << "\n\n x (@knot):\n" << y << endl;
+ cout << "\n\n f (@knot, calc):\n" << fk << endl;
+ cout << "\n\n fx (@knot, calc):\n" << fxk << endl;
+ cout << "\n\n fy (@knot, calc):\n" << fyk << endl;
+ cout << "\n\n fxy (@knot, calc):\n" << fxyk << endl;
+ cout << "\n\n fxx (@knot, calc):\n" << fxxk << endl;
+ cout << "\n\n fyy (@knot, calc):\n" << fyyk << endl;
+ cout << "\n\n fxxx (@knot, calc):\n" << fxxxk << endl;
+ cout << "\n\n fyyy (@knot, calc):\n" << fyyyk << endl;
+ cout << "\n\n fxxy (@knot, calc):\n" << fxxyk << endl;
+ cout << "\n\n fxyy (@knot, calc):\n" << fxyyk << endl;
+
+
+ cout << "\n\n x (@mid):\n" << xM << endl;
+ cout << "\n\n y (@mid):\n" << yM << endl;
+ cout << "\n\n f (@mid, calc):\n" << fMk << endl;
+ cout << "\n\n fx (@mid, calc):\n" << fxMk << endl;
+ cout << "\n\n fy (@mid, calc):\n" << fyMk << endl;
+ cout << "\n\n fxy (@mid, calc):\n" << fxyMk << endl;
+ cout << "\n\n fxx (@mid, calc):\n" << fxxMk << endl;
+ cout << "\n\n fyy (@mid, calc):\n" << fyyMk << endl;
+ cout << "\n\n fxxx (@mid, calc):\n" << fxxxMk << endl;
+ cout << "\n\n fyyy (@mid, calc):\n" << fyyyMk << endl;
+ cout << "\n\n fxxy (@mid, calc):\n" << fxxyMk << endl;
+ cout << "\n\n fxyy (@mid, calc):\n" << fxyyMk << endl;
+ }
+
+ }
+
+
+ Real mid_tol = (1e-1)*(deltaX/2+deltaY/2);
+ Real knot_tol = 0;
+
+ if(flag_verbosePrint == true){
+ printf(" Smoothness set to : %f\n", smoothness);
+ printf(" f: err at knots %f, err at mid %f\n",errV(0),errVM(0));
+ printf(" fx: err at knots %f, err at mid %f\n",errV(1),errVM(1));
+ printf(" fy: err at knots %f, err at mid %f\n",errV(2),errVM(2));
+ printf(" fxy:err at knots %f, err at mid %f\n\n",errV(3),errVM(3));
+ printf(" Test tolerance for f(x,y) @knots : %f, @mid: %f\n\n",
+ knot_tol,mid_tol);
+ cout<< " First derivatives are not tested because these derivatives" << endl;
+ cout<< " shouldn't match: the bicubic interpolation estimates" << endl;
+ cout<< " these derivatives using the derivative of a natural" << endl;
+ cout<< " cubic spline" << endl;
+ }
+
+ //See if the values for f, fx, fy and fxy match the knot points
+ //exactly
+ SimTK_TEST_EQ(fk,z);
+ SimTK_TEST_EQ(fxk,zx);
+ SimTK_TEST_EQ(fyk,zy);
+ SimTK_TEST_EQ_TOL(fxyk,zxy,1e-10);
+ //See if the maximum error at the mid points are acceptable
+ SimTK_TEST_EQ_TOL(errVM(0),0,mid_tol);
+
+}
+
+/**
+This function will construct a single bicubic surface patch that goes from xmin,ymin
+to xmax, ymax. A series of points within this patch will be computed using the bicubic
+interpolation method, and the coefficients will be checked to ensure that the
+relationship between the 16 coefficients, aV, and the 16 corner conditions, fV, are
+related to eachother through the endpoint conditions that define a bicubic surface
+interpolation (http://en.wikipedia.org/wiki/Bicubic_interpolation)
+
+fV = A*aV
+
+aV: [a00, a10 a20 a30,
+ a01 a11 a21 a31,
+ a02 a12 a22 a32,
+ a03 a13 a23 a33]^T
+
+fV:[f(0,0) f(1,0) f(0,1) f(1,1)
+ fx(0,0) fx(1,0) fx(0,1) fx(1,1)
+ fy(0,0) fy(1,0) fy(0,1) fy(1,1)
+ fxy(0,0) fxy(1,0) fxy(0,1) fxy(1,1)]
+
+A is a 16x16 matrix that defines the relationship between the polynomial that enforces
+the conditions that the polynomial has the same values and partial derivatives as the
+function at the corners. To see this matrix in detail refer to the wikipedia page,
+or to the code below. Note that A^(-1) is the one that is shown in the wikipedia page,
+where as the one in the test code is a hand derived version of A.
+
+ @params xmin: the minimum value of the x,y grid in the x dimension
+ @params xmax: the maximum value of the x,y grid in the x dimension
+ @params ymin: the minimum value of the y grid in the y dimension
+ @params ymax: the maximum value of the y grid in the y dimension
+ @params fcnType: An integer value [0-4] that picks an analytical function
+ to use for comparision purposes.
+ @params smoothness: A value of 0 will make sure the patch goes through the
+ desired points exactly. A value between 0 and 1 will
+ relax the surface.
+ @returns nothing
+
+*/
+void testBicubicCoefficients(Real xmin,Real xmax,Real ymin, Real ymax,
+ int fcnType, Real smoothness){
+ int size = 4;
+
+ const Real A[] = {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0,
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 1, 2, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0,
+ 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3,
+ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 1, 0, 0, 0, 2, 0, 0, 0, 3, 0, 0, 0,
+ 0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3,
+ 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 1, 0, 0, 0, 2, 0, 0, 0, 3, 0, 0,
+ 0, 0, 0, 0, 0, 1, 2, 3, 0, 2, 4, 6, 0, 3, 6, 9};
+
+ /*Ok we need at least a 4x4 grid to use the default bicubic surface
+ interpolation because the constructor forms the partial derivatives
+ using natural cubic splines. Natural cubic splines require at least
+ 4 knot points to be defined.
+ */
+ Vector xV(size), yV(size), xeV(2*size-1), yeV(2*size-1);
+ Matrix zM(size,size);
+ Vector tmpV(size);
+
+ Vec<16> fT, aV, fV, fVerr;
+ Mat<16,16> AM(A), ATest;
+
+ //Initialize the grid
+ for(int i=0; i<size; i++){
+ xV(i) = xmin + i*(xmax-xmin)/((Real)size-1.0);
+ yV(i) = xmin + i*(ymax-ymin)/((Real)size-1.0);
+ }
+ for(int i=0; i<size;i++){
+ for(int j=0; j<size; j++){
+ tmpV = getAnalyticFunction(xV(i),yV(j),fcnType);
+ zM(i,j) = tmpV(0);
+ }
+ }
+
+ //Create the bicubic surface using the regular constuctor
+ BicubicSurface bcs(xV, yV, zM, smoothness);
+ const BicubicSurface::Guts& bcsg = bcs.getGuts();
+
+ //Initialize the grid to evaluate the surface at the knots and at
+ //the midpoints
+ for(int i=0; i<(2*size-1); i++){
+ xeV(i) = xmin + i*(xmax-xmin)/((Real)(2*size)-1.0);
+ yeV(i) = ymin + i*(ymax-ymin)/((Real)(2*size)-1.0);
+ }
+
+ //Evaluate the surface at the knot points, and at the
+ //mid grid points and test if fV = A*aV holds
+ Vec2 aXY;
+ for(int i=0; i<(2*size-1); i++){
+ for(int j=0; j<(2*size-1); j++){
+ aXY = Vec2(xeV(i), yeV(j));
+ fV = bcsg.getPatchFunctionVector(aXY);
+ aV = bcsg.getPatchBicubicCoefficients(aXY);
+ fT = AM*aV;
+ fVerr = fV-fT;
+
+ //printf(" (%d,%d) ",i,j);
+ //cout << fVerr.norm() << endl;
+
+ //Due to the relatively large number of floating point
+ //operations required, the tolerance needs to be 1e-12
+ SimTK_TEST_EQ_TOL(fV,fT,1e-12);
+ }
+ }
+
+}
+
+/**
+ This function will check that numerical derivatives of fx, fy, fxy, fxx,
+ fyy, fxyy, fxxy, fxxx and fyyy match the values that the Bicubic surface
+ function are returning. In addition, the surfaces that are defined by fx,
+ fy, fxy, fxx, and fyy will be tested by continuity. Continuity is checked
+ by moving a distance away from the knot point, computing the local derivative
+ at the point along the direction towards the knot point, and then linearly
+ extrapolating back to the knot point. If the linear extrapolation (of f, fx
+ fy, fxy, fxx or fyy) matches the value of the function (f, fx, fy, fxy, fxx
+ or fyy) at the knot point closely, then we can have some confidence that the
+ surface is continuous. I say confidence rather than certaintity because for
+ certaintity we'd have to take the limit as that distance approches zero, and
+ that doesn't make sense in floating point.
+
+ @params xmin: the minimum value of the x,y grid in the x dimension
+ @params xmax: the maximum value of the x,y grid in the x dimension
+ @params ymin: the minimum value of the y grid in the y dimension
+ @params ymax: the maximum value of the y grid in the y dimension
+ @params fcnType: An integer value [0-4] that picks an analytical function
+ to use for comparision purposes.
+ @params smoothness: A value of 0 will make sure the patch goes through the
+ desired points exactly. A value between 0 and 1 will
+ relax the surface.
+ @params verbosePrint: true: will print all of the detailed results for the
+ derivative comparisons, and the continuity
+ checks
+ @returns nothing
+*/
+void testBicubicConsistencyContinuity(Real xmin, Real xmax, Real ymin,
+ Real ymax, int fcnType, Real smoothness, bool verbosePrint){
+ int size = 4;
+ Real minstep = min((xmax-xmin),(ymax-ymin));
+ Real dh = (minstep/(Real)size)/100.0;
+
+ Vector xV(size), yV(size),dxV(4),dyV(4), tmpV(4), aXY(2);
+ Matrix zM(size,size);
+
+
+ //Initialize the 4x4 grid with a non-even grid spacing
+ Real spacingX =1*(xmax-xmin)/((Real)size-1.0);
+ Real spacingY =1*(ymax-ymin)/((Real)size-1.0);
+
+ for(int i=0; i<size; i++){
+ xV(i) = xmin + i*(xmax-xmin)/((Real)size-1.0);
+ yV(i) = xmin + i*(ymax-ymin)/((Real)size-1.0);
+ }
+
+ //Adjust the interior points a little bit to make
+ //the spacing of the grid non-even. This will test
+ //that BicubicSurface correctly handling the stretching
+ //of each individual patch correctly.
+ for(int i=1; i<size-1;i++){
+ xV(i) = xV(i) + 0.1*spacingX*pow(-1.0,i);
+ yV(i) = yV(i) + 0.1*spacingY*pow(-1.0,i);
+ }
+
+ if(verbosePrint==true){
+ cout << "X Spacing: " << xV << endl;
+ cout << "Y Spacing: " << yV << endl;
+ }
+
+ for(int i=0; i<size;i++){
+ for(int j=0; j<size; j++){
+ tmpV = getAnalyticFunction(xV(i),yV(j),fcnType);
+ zM(i,j) = tmpV(0);
+ }
+ }
+
+ //Create the bicubic surface
+ BicubicSurface bcs(xV, yV, zM, smoothness);
+ BicubicFunction bcsf(bcs);
+
+ //Initialize the vectors dxV and dyV to be near an interior knot
+ //with the inner patch a distance h away from the knot point
+ //and the second patch a distance h+dx away from the knot point
+
+ int tsize = 17;
+ Real tsizeh = floor((Real)tsize/2.0);
+
+ Matrix meshX(tsize,tsize), meshY(tsize,tsize);
+
+ aXY(0) = xV(1);
+ aXY(1) = yV(1);
+
+ //Set up all of the partial derivative vectors required for the bench mark
+ Array_<int> derivX(1);
+ Array_<int> derivY(1);
+ Array_<int> derivXY(2);
+ Array_<int> derivXX(2);
+ Array_<int> derivYY(2);
+ Array_<int> derivXXY(3);
+ Array_<int> derivXYY(3);
+ Array_<int> derivXXX(3);
+ Array_<int> derivYYY(3);
+ Array_<int> deriv4X(4);
+ Array_<int> deriv4Y(4);
+
+ derivX[0] = 0;
+ derivY[0] = 1;
+ derivXY[0]= 0;
+ derivXY[1]= 1;
+ derivXX[0]= 0;
+ derivXX[1]= 0;
+ derivYY[0]= 1;
+ derivYY[1]= 1;
+ derivXXY[0]= 0;
+ derivXXY[1]= 0;
+ derivXXY[2]= 1;
+ derivXYY[0]= 0;
+ derivXYY[1]= 1;
+ derivXYY[2]= 1;
+ derivXXX[0]= 0;
+ derivXXX[1]= 0;
+ derivXXX[2]= 0;
+ derivYYY[0]= 1;
+ derivYYY[1]= 1;
+ derivYYY[2]= 1;
+ for(int i=0;i<4;i++){
+ deriv4X[i]=0;
+ deriv4Y[i]=1;
+ }
+
+ //Function computed derivatives
+ Matrix bcsF(tsize,tsize), bcsFx(tsize,tsize), bcsFy(tsize,tsize);
+ Matrix bcsFxy(tsize,tsize), bcsFxx(tsize,tsize), bcsFyy(tsize,tsize);
+ Matrix bcsFxxy(tsize,tsize), bcsFxyy(tsize,tsize), bcsFxxx(tsize,tsize);
+ Matrix bcsFyyy(tsize,tsize), bcsF4x(tsize,tsize), bcsF4y(tsize,tsize);
+
+ //Numerically computed derivatives
+ Matrix numFx(tsize,tsize), numFy(tsize,tsize);
+ Matrix numFxy(tsize,tsize), numFxx(tsize,tsize), numFyy(tsize,tsize);
+ Matrix numFxxy(tsize,tsize), numFxyy(tsize,tsize), numFxxx(tsize,tsize);
+ Matrix numFyyy(tsize,tsize);
+
+ aXY(0) = xV(1);
+ aXY(1) = yV(1);
+
+ //Sample the surface about aXY over a 17x17 grid
+ for(int i=0;i<tsize;i++){
+ for(int j=0;j<tsize;j++){
+ meshX(i,j) = (xV(1) - tsizeh*dh) + dh*i;
+ meshY(i,j) = (yV(1) - tsizeh*dh) + dh*j;
+ aXY(0) = meshX(i,j);
+ aXY(1) = meshY(i,j);
+
+ bcsF(i,j) = bcsf.calcValue(aXY);
+
+ bcsFx(i,j)= bcsf.calcDerivative(derivX,aXY);
+ bcsFy(i,j)= bcsf.calcDerivative(derivY,aXY);
+
+ bcsFxy(i,j)= bcsf.calcDerivative(derivXY,aXY);
+ bcsFxx(i,j)= bcsf.calcDerivative(derivXX,aXY);
+ bcsFyy(i,j)= bcsf.calcDerivative(derivYY,aXY);
+
+ bcsFxxy(i,j)= bcsf.calcDerivative(derivXXY,aXY);
+ bcsFxyy(i,j)= bcsf.calcDerivative(derivXYY,aXY);
+ bcsFxxx(i,j)= bcsf.calcDerivative(derivXXX,aXY);
+ bcsFyyy(i,j)= bcsf.calcDerivative(derivYYY,aXY);
+
+ //Should be zero, just testing.
+ bcsF4x(i,j) = bcsf.calcDerivative(deriv4X,aXY);
+ bcsF4y(i,j) = bcsf.calcDerivative(deriv4Y,aXY);
+ }
+ }
+
+ //Now compute the equivalent numerical derivatives using
+ //central differences on the values in bcsF
+ for(int i=0;i<tsize;i++){
+ numFx(i) = getCentralDifference(meshX(i), bcsF(i), true);
+ numFxx(i) = getCentralDifference(meshX(i), numFx(i), true);
+ numFxxx(i) = getCentralDifference(meshX(i), numFxx(i), true);
+
+ numFy[i] = ~getCentralDifference(~meshY[i], ~bcsF[i], true);
+ numFyy[i] = ~getCentralDifference(~meshY[i], ~numFy[i], true);
+ numFyyy[i] = ~getCentralDifference(~meshY[i], ~numFyy[i], true);
+ }
+ for(int i=0;i<tsize;i++){
+ numFxy[i] = ~getCentralDifference(~meshY[i], ~numFx[i], true);
+ numFxxy[i] = ~getCentralDifference(~meshY[i], ~numFxx[i], true);
+ }
+ for(int i=0;i<tsize;i++){
+ numFxyy[i] = ~getCentralDifference(~meshY[i], ~numFxy[i], true);
+ }
+
+ Real tol1 = dh;
+ Real tol2 = dh*10;
+ Real tol3 = dh*100;
+ Vector dirXY(2);
+
+
+ for(int i=3;i<tsize-3;i++){
+ for(int j=3;j<tsize-3;j++){
+
+ //1. Now compare the inner 10x10 numerical values
+ // for each of the derivatives to the values computed
+ // by the bicubic function
+ if(verbosePrint==true){
+ printf("\n\nCheck Derivatives (i,j): %d, %d", i,j);
+ printf("\nbcs: fx:%f fy:%f" , bcsFx(i,j),bcsFy(i,j));
+ printf("\nnum: fx:%f fy:%f" , numFx(i,j),numFy(i,j));
+ printf("\n\n|bcs: fxy:%f fxx:%f fyy:%f ",
+ bcsFxy(i,j), bcsFxx(i,j), bcsFyy(i,j));
+ printf("\nnum: fxy:%f fxx:%f fyy:%f ",
+ numFxy(i,j), numFxx(i,j), numFyy(i,j));
+ printf("\n\n|bcs: fxxy:%f fxyy:%f fxxx:%f fyyy:%f",
+ bcsFxxy(i,j), bcsFxyy(i,j), bcsFxxx(i,j), bcsFyyy(i,j));
+ printf("\nnum: fxxy:%f fxyy:%f fxxx:%f fyyy:%f",
+ numFxxy(i,j), numFxyy(i,j), numFxxx(i,j), numFyyy(i,j));
+ }
+
+ SimTK_TEST_EQ_TOL(bcsFx(i,j),numFx(i,j),tol1);
+ SimTK_TEST_EQ_TOL(bcsFy(i,j),numFy(i,j),tol1);
+
+ SimTK_TEST_EQ_TOL(bcsFxy(i,j),numFxy(i,j),tol2);
+ SimTK_TEST_EQ_TOL(bcsFxx(i,j),numFxx(i,j),tol2);
+ SimTK_TEST_EQ_TOL(bcsFyy(i,j),numFyy(i,j),tol2);
+
+ /*The numerical 3rd derivatives will not match at the boundaries
+ between patches. They are discontinuous in this region in the
+ formulation, and make the numerical derivatives around these
+ boundaries poorly estimated.*/
+
+ //2. Continuity testing:
+ //Test that a linear extrapolation from the current location
+ //to the knot point matches the value of the knot point
+ if(j != tsizeh || i != tsizeh){
+
+ dirXY(0) = meshX(i,j)-meshX(8,8);
+ dirXY(1) = meshY(i,j)-meshY(8,8);
+
+ Real dist = pow(dirXY(0)*dirXY(0) + dirXY(1)*dirXY(1),0.5);
+
+ //Test for surface continuity
+ Real f0 = bcsF(i,j) -(bcsFx(i,j)*dirXY(0)
+ + bcsFy(i,j)*dirXY(1));
+ Real err0 =f0-bcsF(8,8);
+ Real errR0= abs(err0)/( abs(bcsF(8,8)) + 1e-10);
+
+ //Test for fx derivative continuity
+ Real f1x = bcsFx(i,j) -(bcsFxx(i,j)*dirXY(0));
+ Real err1x =f1x-bcsFx(8,8);
+ Real errR1x= abs(err1x)/( abs(bcsFx(8,8)) + 1e-10);
+
+ //Test for fy derivative continity
+ Real f1y = bcsFy(i,j) -(bcsFyy(i,j)*dirXY(1));
+ Real err1y =f1y-bcsFy(8,8);
+ Real errR1y= abs(err1y)/( abs(bcsFy(8,8)) + 1e-10);
+
+ //Test for fxx derivative continuity
+ Real f2x = bcsFxx(i,j) -(bcsFxxx(i,j)*dirXY(0));
+ Real err2x =f2x-bcsFxx(8,8);
+ Real errR2x= abs(err2x)/( abs(bcsFxx(8,8)) + 1e-10);
+
+ //Test for fyy derivative continuity
+ Real f2y = bcsFyy(i,j) -(bcsFyyy(i,j)*dirXY(1));
+ Real err2y =f2y-bcsFyy(8,8);
+ Real errR2y= abs(err2y)/( abs(bcsFyy(8,8)) + 1e-10);
+
+ //Test for fxy derivative continuity
+ Real fxy = bcsFxy(i,j) -(bcsFxxy(i,j)*dirXY(0) + bcsFxyy(i,j)*dirXY(1));
+ Real errxy =fxy-bcsFxy(8,8);
+ Real errRxy= abs(errxy)/( abs(bcsFxy(8,8)) + 1e-10);
+
+
+ if(verbosePrint==true){
+ printf("\n\nCheck Continuity (i,j): %d, %d", i,j);
+ printf("\nf(x,y) : %f num f : %f errR: %f"
+ , bcsF(8,8), f0, errR0);
+ printf("\nfx(x,y) : %f num fx : %f errR: %f"
+ , bcsFx(8,8), f1x, errR1x);
+ printf("\nfy(x,y) : %f num fx : %f errR: %f"
+ , bcsFy(8,8), f1y, errR1y);
+ printf("\nfxx(x,y): %f num fxx: %f errR: %f"
+ , bcsFxx(8,8),f2x, errR2x);
+ printf("\nfyy(x,y): %f num fyy: %f errR: %f"
+ , bcsFyy(8,8),f2y, errR2y);
+ printf("\nfxy(x,y): %f num fxy: %f errR: %f"
+ , bcsFxy(8,8),fxy, errRxy);
+ }
+
+
+
+ SimTK_TEST_EQ_TOL(errR0,0, dh);
+ SimTK_TEST_EQ_TOL(errR1x,0,dh*5);
+ SimTK_TEST_EQ_TOL(errR1y,0,dh*5);
+ SimTK_TEST_EQ_TOL(errR2x,0,dh*5);
+ SimTK_TEST_EQ_TOL(errR2y,0,dh*5);
+ SimTK_TEST_EQ_TOL(errRxy,0,dh*10);
+ }
+
+ }
+ }
+
+}
+
+/**
+ This test function will create a bicubic surface and then test that
+ a version of this surface initialized using the copy constructor and
+ the equal operator returns the same values over the surface as the original
+*/
+void testCopyConstEqOp(){
+ int fcnType = 4;
+ Real xmin = 0;
+ Real xmax = 2*Pi;
+ Real ymin = 0;
+ Real ymax = Pi;
+ Real smoothness = 0.1;
+
+ int size = 4;
+ Real minstep = min((xmax-xmin),(ymax-ymin));
+ Real dh = (minstep/(Real)size)/100.0;
+
+ Vector xV(size), yV(size),dxV(4),dyV(4), tmpV(4), aXY(2);
+ Matrix zM(size,size);
+
+
+ //Initialize the 4x4 grid with a non-even grid spacing
+ Real spacingX =1*(xmax-xmin)/((Real)size-1.0);
+ Real spacingY =1*(ymax-ymin)/((Real)size-1.0);
+
+ for(int i=0; i<size; i++){
+ xV(i) = xmin + i*(xmax-xmin)/((Real)size-1.0);
+ yV(i) = xmin + i*(ymax-ymin)/((Real)size-1.0);
+ }
+
+ //Adjust the interior points a little bit to make
+ //the spacing of the grid non-even. This will test
+ //that BicubicSurface correctly handling the stretching
+ //of each individual patch correctly.
+ for(int i=1; i<size-1;i++){
+ xV(i) = xV(i) + 0.1*spacingX*pow(-1.0,i);
+ yV(i) = yV(i) + 0.1*spacingY*pow(-1.0,i);
+ }
+
+ /*if(verbosePrint==true){
+ cout << "X Spacing: " << xV << endl;
+ cout << "Y Spacing: " << yV << endl;
+ }*/
+
+ for(int i=0; i<size;i++){
+ for(int j=0; j<size; j++){
+ tmpV = getAnalyticFunction(xV(i),yV(j),fcnType);
+ zM(i,j) = tmpV(0);
+ }
+ }
+
+ //Create the bicubic surface
+ BicubicSurface bcs(xV, yV, zM, smoothness);
+ BicubicSurface bcsCC(bcs);
+ BicubicSurface bcsEQOP;
+ bcsEQOP = bcs;
+
+ // Extract the implementation objects so we can look at the internals.
+ const BicubicSurface::Guts& bcsg = bcs.getGuts();
+ const BicubicSurface::Guts& bcsCCg = bcsCC.getGuts();
+ const BicubicSurface::Guts& bcsEQOPg = bcsEQOP.getGuts();
+
+ // These should all be the same underlying object, and the reference
+ // count should be 3.
+ SimTK_TEST(&bcsCCg == &bcsg);
+ SimTK_TEST(&bcsEQOPg == &bcsg);
+ SimTK_TEST(bcsg.getReferenceCount() == 3);
+
+ // Create Function objects referencing the surface(s).
+ BicubicFunction bcsf(bcs);
+ BicubicFunction bcsCCf(bcs);
+ BicubicFunction bcsEQOPf(bcs);
+
+ // Reference count should now be 6.
+ SimTK_TEST(bcsg.getReferenceCount() == 6);
+
+
+ // These tests are meaningless now if the above ones succeed, since
+ // obviously if they are the same object they will produce the same info!
+
+ //Just to be extra sure, we'll actually check some values
+ //computed from each of these different surfaces as well
+ Real deltaX = (xmax-xmin)/15;
+ Real deltaY = (ymax-ymin)/15;
+ Array_<int> dX(1);
+ Array_<int> dY(1);
+ Array_<int> dXY(2);
+ Array_<int> dXX(2);
+ Array_<int> dYY(2);
+ Array_<int> dXXY(3);
+ Array_<int> dXYY(3);
+ Array_<int> dXXX(3);
+ Array_<int> dYYY(3);
+ Array_<int> d4X(4);
+ Array_<int> d4Y(4);
+
+ dX[0] = 0;
+ dY[0] = 1;
+ dXY[0]= 0;
+ dXY[1]= 1;
+ dXX[0]= 0;
+ dXX[1]= 0;
+ dYY[0]= 1;
+ dYY[1]= 1;
+ dXXY[0]= 0;
+ dXXY[1]= 0;
+ dXXY[2]= 1;
+ dXYY[0]= 0;
+ dXYY[1]= 1;
+ dXYY[2]= 1;
+ dXXX[0]= 0;
+ dXXX[1]= 0;
+ dXXX[2]= 0;
+ dYYY[0]= 1;
+ dYYY[1]= 1;
+ dYYY[2]= 1;
+
+for(int i=0;i<16;i++){
+ aXY(0) = xmin + i*deltaX;
+ for(int j=0;j<16;j++){
+ aXY(1) = ymin + j*deltaY;
+ SimTK_TEST_EQ(bcsf.calcValue(aXY), bcsCCf.calcValue(aXY));
+ SimTK_TEST_EQ(bcsf.calcValue(aXY),bcsEQOPf.calcValue(aXY));
+
+ SimTK_TEST_EQ(bcsf.calcDerivative(dX,aXY), bcsCCf.calcDerivative(dX,aXY));
+ SimTK_TEST_EQ(bcsf.calcDerivative(dX,aXY),bcsEQOPf.calcDerivative(dX,aXY));
+
+ SimTK_TEST_EQ(bcsf.calcDerivative(dY,aXY), bcsCCf.calcDerivative(dY,aXY));
+ SimTK_TEST_EQ(bcsf.calcDerivative(dY,aXY),bcsEQOPf.calcDerivative(dY,aXY));
+
+ SimTK_TEST_EQ(bcsf.calcDerivative(dXY,aXY), bcsCCf.calcDerivative(dXY,aXY));
+ SimTK_TEST_EQ(bcsf.calcDerivative(dXY,aXY),bcsEQOPf.calcDerivative(dXY,aXY));
+
+ SimTK_TEST_EQ(bcsf.calcDerivative(dXXY,aXY), bcsCCf.calcDerivative(dXXY,aXY));
+ SimTK_TEST_EQ(bcsf.calcDerivative(dXXY,aXY),bcsEQOPf.calcDerivative(dXXY,aXY));
+
+ SimTK_TEST_EQ(bcsf.calcDerivative(dXYY,aXY), bcsCCf.calcDerivative(dXYY,aXY));
+ SimTK_TEST_EQ(bcsf.calcDerivative(dXYY,aXY),bcsEQOPf.calcDerivative(dXYY,aXY));
+
+ SimTK_TEST_EQ(bcsf.calcDerivative(dXXX,aXY), bcsCCf.calcDerivative(dXXX,aXY));
+ SimTK_TEST_EQ(bcsf.calcDerivative(dXXX,aXY),bcsEQOPf.calcDerivative(dXXX,aXY));
+
+ SimTK_TEST_EQ(bcsf.calcDerivative(dYYY,aXY), bcsCCf.calcDerivative(dYYY,aXY));
+ SimTK_TEST_EQ(bcsf.calcDerivative(dYYY,aXY),bcsEQOPf.calcDerivative(dYYY,aXY));
+ }
+}
+
+}
+
+void testHint() {
+ const Real xData[4] = { .1, 1, 2, 10 };
+ const Real yData[5] = { -3, -2, 0, 1, 3 };
+ const Real fData[] = { 1, 2, 3, 4, 5,
+ 1.1, 2.1, 3.1, 4.1, 5.1,
+ 1, 2, 3, 4, 5,
+ 1.2, 2.2, 3.2, 4.2, 5.2 };
+ const Vector x(4, xData);
+ const Vector y(5, yData);
+ const Matrix f(4,5, fData);
+ BicubicSurface surf(x, y, f, 0); // not smoothed
+
+ SimTK_TEST(surf.getNumAccesses() == 0);
+
+ BicubicSurface::PatchHint hint;
+ Real val = surf.calcValue(Vec2(.5, .5), hint);
+ SimTK_TEST(surf.getNumAccesses() == 1);
+ val = surf.calcValue(Vec2(.5, .5), hint); // should be free
+ SimTK_TEST(surf.getNumAccesses() == 2);
+ SimTK_TEST(surf.getNumAccessesSamePoint() == 1);
+
+ val = surf.calcValue(Vec2(.50001, .50002), hint);
+ SimTK_TEST(surf.getNumAccessesSamePatch() == 1);
+
+ val = surf.calcValue(Vec2(1.5, -1), hint);
+ SimTK_TEST(surf.getNumAccessesNearbyPatch() == 1);
+
+ // This should report "same patch" rather than "same point" because
+ // derivative info hasn't been calculated yet.
+ Array_<int> deriv1(1, 1), deriv2(2, 0); // fy, fxx
+ val = surf.calcDerivative(deriv2, Vec2(1.5, -1), hint);
+ SimTK_TEST(surf.getNumAccessesSamePatch() == 2);
+
+ // When 2nd deriv info is calculated we get 1st deriv also. So now
+ // we should get "same point" even though we haven't asked for this yet.
+ val = surf.calcDerivative(deriv1, Vec2(1.5, -1), hint);
+ SimTK_TEST(surf.getNumAccessesSamePoint() == 2);
+
+}
+
+int main() {
+ //Evaluate the bicubic surface interpolation against an analytical
+ //function. Throw an error if the values of the function are different
+ //at the knot points, or different within tolerance at the mid grid points
+ SimTK_START_TEST("Testing Bicubic Interpolation");
+ SimTK_SUBTEST(testHint);
+
+ cout << "\n---------------------------------------------"<< endl;
+ cout<< "\n\nANALYTICAL FUNCTION COMPARISON:" << endl;
+ testBicubicAgainstAnalyticFcn(0.0, 1.0, 0.0, 1.0,9,0,false,false);
+ testBicubicAgainstAnalyticFcn(0.0, 1.0, 0.0, 1.0,9,1,false,false);
+ testBicubicAgainstAnalyticFcn(0.0, 1.0, 0.0, 1.0,9,2,false,false);
+ testBicubicAgainstAnalyticFcn(0.0, 1.0, 0.0, 1.0,9,3,false,false);
+ testBicubicAgainstAnalyticFcn(0.0, 1.0, 0.0, 1.0,9,4,false,false);
+ printf("\n\n*Test Passed*. Constructor with x,y,f,fx,fy,fxy specified,"
+ " \n\tSmoothness parameter %f tested\n"
+ "\tAdditional smoothness parameters not tested because"
+ "\n\tsurface will not pass through the knot points",Real(0));
+ cout << "\n---------------------------------------------"<< endl;
+
+ cout << "\n---------------------------------------------"<< endl;
+ cout << "\n\nBICUBIC COEFFICIENT VALIDATION:" << endl;
+ cout << " Testing that the bicubic interpolation coefficients" <<endl;
+ cout << " are being solved correctly by asserting fV - A*aV = 0"<<endl;
+ testBicubicCoefficients( 0.0, 1.0, 0.0, 1.0, 3, 0.0);
+ testBicubicCoefficients( 0.0, 1.0, 0.0, 1.0, 3, 0.5);
+ printf("\n\n*Test Passed*. Constructor with x,y,f specified,"
+ " \n\tSmoothness parameter %f and %f tested",(Real)0.0,(Real)0.5);
+ cout << "\n---------------------------------------------"<< endl;
+
+ cout << "\n\n---------------------------------------------"<< endl;
+ cout << "\n\nBICUBIC DERIVATIVE & CONTINUITY TESTING:" <<endl;
+ cout << " 1. Derivative are tested for consistency by ensuring that" << endl;
+ cout << " numerical derivatives of f(x,y) match values returned " << endl;
+ cout << " by the function." << endl;
+ cout << " Partial derivatives tested: fx,fy,fxy,fxx,fyy" << endl;
+ cout << "\n 2. Continuity is tested by asserting that a linear extrapolation" << endl;
+ cout << " from a a point near a knot is equal to the value of the surface" << endl;
+ cout << " of f(x,y) at the knot. Surfaces tested f, fx, fy, fxy, fxx, fyy." << endl;
+ testBicubicConsistencyContinuity( 0.0, 1.0, 0.0, 1.0, 3, 0.0, false);
+ testBicubicConsistencyContinuity( 0.0, 1.0, 0.0, 1.0, 3, 0.5, false);
+ printf("\n\n*Test Passed*. Constructor with x,y,f specified,"
+ " \n\tSmoothness parameter %f and %f tested",(Real)0.0,(Real)0.5);
+ cout << "\n---------------------------------------------"<< endl;
+
+ cout << "\n\n---------------------------------------------"<< endl;
+ cout << "\n\nCOPY CONSTRUCTOR AND = OPERATOR TESTING:" <<endl;
+ cout <<" Tested by using the copy contructor and equal operator" << endl;
+ cout <<" and comparing the values of the internally stored matrices" << endl;
+ cout <<" of x,y,f,fx,fy,fxy between the different surfaces, and then" <<endl;
+ cout <<" comparing values of f,fx,fy,fxy,fxx,fyy,fxyy,fxxy,fxxx,fyyy" <<endl;
+ cout <<" between the different surface objects across the patch"<<endl;
+ testCopyConstEqOp();
+ cout <<" *Test Passed*." << endl;
+ cout << "\n---------------------------------------------"<< endl;
+
+ SimTK_END_TEST();
+}
+
diff --git a/SimTKmath/tests/TestContactGeometry.cpp b/SimTKmath/tests/TestContactGeometry.cpp
new file mode 100644
index 0000000..53abe26
--- /dev/null
+++ b/SimTKmath/tests/TestContactGeometry.cpp
@@ -0,0 +1,486 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Ian Stavness *
+ * *
+ * 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 <vector>
+#include <exception>
+
+using namespace SimTK;
+using namespace std;
+
+//TODO - had to reduced tol for geodesic tests, should be 1e-10
+const Real TOL = 1e-4;
+
+const Real r = 3.5; // radius used for geodesic tests
+
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+template <class T>
+void assertEqual(T val1, T val2) {
+ const T sz = std::max(std::abs(val1),std::abs(val2));
+ const Real tol = std::max(TOL, sz*TOL);
+ ASSERT(abs(val1-val2) < tol);
+}
+
+template <int N>
+void assertEqual(Vec<N> val1, Vec<N> val2) {
+ for (int i = 0; i < N; ++i) {
+ const Real sz = std::max(std::abs(val1[i]),std::abs(val2[i]));
+ const Real tol = std::max(TOL, sz*TOL);
+ ASSERT(abs(val1[i]-val2[i]) < tol);
+ }
+}
+
+void assertEqual(UnitVec3 v1, UnitVec3 v2) {
+ return assertEqual(v1.asVec3(), v2.asVec3());
+}
+
+void testSurfaceGradient(const ContactGeometry& );
+void testSurfaceHessian(const ContactGeometry& );
+void compareAnalyticalAndNumericGradient(const ContactGeometry&, const Vec3& );
+void compareAnalyticalAndNumericHessian(const ContactGeometry&, const Vec3& );
+
+void testHalfSpace() {
+ ContactGeometry::HalfSpace hs;
+
+ // Check intersections with various rays.
+
+ Real distance;
+ UnitVec3 normal;
+ ASSERT(!hs.intersectsRay(Vec3(4, 0, 0), UnitVec3(1, 0, 0), distance, normal));
+ ASSERT(!hs.intersectsRay(Vec3(-1, 0, 0), UnitVec3(-1, 1, 0), distance, normal));
+ ASSERT(!hs.intersectsRay(Vec3(-1, 0, 0), UnitVec3(0, 1, 0), distance, normal));
+ ASSERT(hs.intersectsRay(Vec3(-1, 0, 0), UnitVec3(1, 0, 0), distance, normal));
+ assertEqual(1.0, distance);
+ assertEqual(Vec3(-1, 0, 0), normal);
+ ASSERT(hs.intersectsRay(Vec3(-2, 15, 37), UnitVec3(1, 0, 0), distance, normal));
+ assertEqual(2.0, distance);
+ assertEqual(Vec3(-1, 0, 0), normal);
+ ASSERT(hs.intersectsRay(Vec3(-3, 1, 2), UnitVec3(1, 1, 1), distance, normal));
+ assertEqual(3*Sqrt3, distance);
+ assertEqual(Vec3(-1, 0, 0), normal);
+ ASSERT(hs.intersectsRay(Vec3(2, 0, 0), UnitVec3(-1, 0, 1), distance, normal));
+ assertEqual(2*Sqrt2, distance);
+ assertEqual(Vec3(-1, 0, 0), normal);
+
+ // Test finding the nearest point.
+
+ Random::Gaussian random(0, 3);
+ for (int i = 0; i < 100; i++) {
+ Vec3 pos(random.getValue(), random.getValue(), random.getValue());
+ bool inside;
+ UnitVec3 normal;
+ Vec3 nearest = hs.findNearestPoint(pos, inside, normal);
+ assertEqual(nearest, Vec3(0, pos[1], pos[2]));
+ ASSERT(inside == (pos[0] >= 0));
+ assertEqual(normal, Vec3(-1, 0, 0));
+ }
+}
+
+void testCylinder() {
+ Real radius = 3.5;
+ ContactGeometry::Cylinder cyl(radius);
+ assert(cyl.getRadius() == radius);
+
+ // Check intersections with various rays.
+
+ Real distance;
+ UnitVec3 normal;
+ ASSERT(!cyl.intersectsRay(Vec3(radius*1.1, 0, 0), UnitVec3(1, 0, 0), distance, normal));
+ ASSERT(!cyl.intersectsRay(Vec3(-radius*1.1, 0, 0), UnitVec3(-1, 1, 0), distance, normal));
+ ASSERT(!cyl.intersectsRay(Vec3(-radius*1.1, 0, 0), UnitVec3(0, 1, 0), distance, normal));
+ ASSERT(!cyl.intersectsRay(Vec3(-radius, -radius, 0), UnitVec3(1, -Eps, 0), distance, normal));
+ ASSERT(cyl.intersectsRay(Vec3(-radius, -radius, 0), UnitVec3(1, Eps, 0), distance, normal));
+
+ ASSERT(cyl.intersectsRay(Vec3(-(radius+1.0), 0, 0), UnitVec3(1, 0, 0), distance, normal));
+ assertEqual(1.0, distance);
+ assertEqual(Vec3(-1, 0, 0), normal);
+
+ ASSERT(cyl.intersectsRay(Vec3(-radius*2, radius*2, 37), UnitVec3(1, -1, 0), distance, normal));
+ assertEqual(radius*(2*Sqrt2-1), distance);
+ assertEqual(UnitVec3(-1, 1, 0), normal);
+
+ ASSERT(cyl.intersectsRay(Vec3(-radius*2, 0, -radius*2), UnitVec3(1, 0, 1), distance, normal));
+ assertEqual(radius*Sqrt2, distance);
+ assertEqual(UnitVec3(-1, 0, 0), normal);
+
+ // Test finding the nearest point.
+
+ Random::Gaussian random(0, 3);
+ for (int i = 0; i < 100; i++) {
+ Vec3 pos(random.getValue(), random.getValue(), random.getValue());
+ Vec3 projpos(pos);
+ projpos(2)=0; // cyl axis is z-axis, project pos to x-y plane
+ bool inside;
+ UnitVec3 normal;
+ Vec3 nearest = cyl.findNearestPoint(pos, inside, normal);
+ assertEqual(nearest, projpos.normalize()*radius+Vec3(0,0,pos(2)));
+ ASSERT(inside == (projpos.norm() <= radius));
+ assertEqual(normal, projpos.normalize());
+ }
+
+ // Test derivatives
+ testSurfaceGradient(cyl);
+ testSurfaceHessian(cyl);
+}
+
+void testSphere() {
+ // Create a sphere.
+
+ Real radius = 3.5;
+ ContactGeometry::Sphere sphere(radius);
+ assert(sphere.getRadius() == radius);
+
+ // Check intersections with various rays.
+
+ Real distance;
+ UnitVec3 normal;
+ ASSERT(!sphere.intersectsRay(Vec3(4, 0, 0), UnitVec3(1, 0, 0), distance, normal));
+ ASSERT(sphere.intersectsRay(Vec3(2, 0, 0), UnitVec3(1, 0, 0), distance, normal));
+ assertEqual(1.5, distance);
+ assertEqual(Vec3(1, 0, 0), normal);
+ ASSERT(sphere.intersectsRay(Vec3(4, 0, 0), UnitVec3(-1, 0, 0), distance, normal));
+ assertEqual(0.5, distance);
+ assertEqual(Vec3(1, 0, 0), normal);
+ ASSERT(sphere.intersectsRay(Vec3(2, 0, 0), UnitVec3(-1, 0, 0), distance, normal));
+ assertEqual(5.5, distance);
+ assertEqual(Vec3(-1, 0, 0), normal);
+ ASSERT(sphere.intersectsRay(Vec3(0, 0, 0), UnitVec3(1, 1, 1), distance, normal));
+ assertEqual(3.5, distance);
+ assertEqual(Vec3(1.0/Sqrt3), normal);
+
+ // Test finding the nearest point.
+
+ Random::Gaussian random(0, 3);
+ for (int i = 0; i < 100; i++) {
+ Vec3 pos(random.getValue(), random.getValue(), random.getValue());
+ bool inside;
+ UnitVec3 normal;
+ Vec3 nearest = sphere.findNearestPoint(pos, inside, normal);
+ assertEqual(nearest, pos.normalize()*radius);
+ ASSERT(inside == (pos.norm() <= radius));
+ assertEqual(normal, pos.normalize());
+ }
+}
+
+void testEllipsoid() {
+ // Create a ellipsoid.
+
+ Vec3 radii(1.5, 2.2, 3.1);
+ ContactGeometry::Ellipsoid ellipsoid(radii);
+ assert(ellipsoid.getRadii() == radii);
+
+ // Check intersections with various rays.
+
+ Real distance;
+ UnitVec3 normal;
+ ASSERT(!ellipsoid.intersectsRay(Vec3(4, 0, 0), UnitVec3(1, 0, 0), distance, normal));
+ ASSERT(ellipsoid.intersectsRay(Vec3(1, 0, 0), UnitVec3(1, 0, 0), distance, normal));
+ assertEqual(0.5, distance);
+ assertEqual(Vec3(1, 0, 0), normal);
+ ASSERT(ellipsoid.intersectsRay(Vec3(4, 0, 0), UnitVec3(-1, 0, 0), distance, normal));
+ assertEqual(2.5, distance);
+ assertEqual(Vec3(1, 0, 0), normal);
+ ASSERT(ellipsoid.intersectsRay(Vec3(0, -5, 0), UnitVec3(0, 1, 0), distance, normal));
+ assertEqual(2.8, distance);
+ assertEqual(Vec3(0, -1, 0), normal);
+ ASSERT(ellipsoid.intersectsRay(Vec3(0, 0, 0), UnitVec3(1, 1, 1), distance, normal));
+ assertEqual(sqrt(3/(1/(radii[0]*radii[0])+1/(radii[1]*radii[1])+1/(radii[2]*radii[2]))), distance);
+ assertEqual(UnitVec3(1/(radii[0]*radii[0]), 1/(radii[1]*radii[1]), 1/(radii[2]*radii[2])), normal);
+
+ // Test finding the nearest point.
+
+ Random::Gaussian random(0, 2);
+ for (int i = 0; i < 100; i++) {
+ Vec3 pos(random.getValue(), random.getValue(), random.getValue());
+ bool inside;
+ UnitVec3 normal;
+ Vec3 nearest = ellipsoid.findNearestPoint(pos, inside, normal);
+ assertEqual(nearest[0]*nearest[0]/(radii[0]*radii[0])+nearest[1]*nearest[1]/(radii[1]*radii[1])+nearest[2]*nearest[2]/(radii[2]*radii[2]), 1.0);
+ Real projectedRadius = pos[0]*pos[0]/(radii[0]*radii[0])+pos[1]*pos[1]/(radii[1]*radii[1])+pos[2]*pos[2]/(radii[2]*radii[2]);
+ ASSERT(inside == (projectedRadius < 1.0));
+ Vec3 projectedPoint = pos/sqrt(projectedRadius);
+ ASSERT((nearest-pos).normSqr() < (projectedPoint-pos).normSqr());
+ assertEqual(normal, UnitVec3(nearest[0]/(radii[0]*radii[0]), nearest[1]/(radii[1]*radii[1]), nearest[2]/(radii[2]*radii[2])));
+ }
+}
+
+void testTorus() {
+ Real radius = r;
+ Real tubeRadius = 0.75;
+ ContactGeometry::Torus torus(radius, tubeRadius);
+ assert(torus.getTorusRadius() == radius);
+ assert(torus.getTubeRadius() == tubeRadius);
+
+ // Check intersections with various rays.
+
+// Real distance;
+// UnitVec3 normal;
+// ASSERT(!torus.intersectsRay(Vec3(radius*1.1, 0, 0), UnitVec3(1, 0, 0), distance, normal));
+// ASSERT(!torus.intersectsRay(Vec3(-radius*1.1, 0, 0), UnitVec3(-1, 1, 0), distance, normal));
+// ASSERT(!torus.intersectsRay(Vec3(-radius*1.1, 0, 0), UnitVec3(0, 1, 0), distance, normal));
+// ASSERT(!torus.intersectsRay(Vec3(-radius, -radius, 0), UnitVec3(1, -Eps, 0), distance, normal));
+// ASSERT(torus.intersectsRay(Vec3(-radius, -radius, 0), UnitVec3(1, Eps, 0), distance, normal));
+//
+// ASSERT(torus.intersectsRay(Vec3(-(radius+1.0), 0, 0), UnitVec3(1, 0, 0), distance, normal));
+// assertEqual(1.0, distance);
+// assertEqual(Vec3(-1, 0, 0), normal);
+//
+// ASSERT(torus.intersectsRay(Vec3(-radius*2, radius*2, 37), UnitVec3(1, -1, 0), distance, normal));
+// assertEqual(radius*(2*Sqrt2-1), distance);
+// assertEqual(UnitVec3(-1, 1, 0), normal);
+//
+// ASSERT(torus.intersectsRay(Vec3(-radius*2, 0, -radius*2), UnitVec3(1, 0, 1), distance, normal));
+// assertEqual(radius*Sqrt2, distance);
+// assertEqual(UnitVec3(-1, 0, 0), normal);
+//
+// // Test finding the nearest point.
+//
+// Random::Gaussian random(0, 3);
+// for (int i = 0; i < 100; i++) {
+// Vec3 pos(random.getValue(), random.getValue(), random.getValue());
+// Vec3 projpos(pos);
+// projpos(2)=0; // cyl axis is z-axis, project pos to x-y plane
+// bool inside;
+// UnitVec3 normal;
+// Vec3 nearest = torus.findNearestPoint(pos, inside, normal);
+// assertEqual(nearest, projpos.normalize()*radius+Vec3(0,0,pos(2)));
+// ASSERT(inside == (projpos.norm() <= radius));
+// assertEqual(normal, projpos.normalize());
+// }
+
+ // Test derivatives
+// Vec3 pt(2*(radius+tubeRadius), 0,0);
+// Vec3 pt(3,4,5);
+// cout << "surfaceValue( " << pt << " ) = " << torus.calcSurfaceValue(pt) << endl;
+// compareAnalyticalAndNumericGradient(torus, pt);
+// compareAnalyticalAndNumericHessian(torus, pt);
+ testSurfaceGradient(torus);
+ testSurfaceHessian(torus);
+
+}
+
+
+//==============================================================================
+// SURFACE EVALUATORS
+//==============================================================================
+
+class ImplicitSurfaceFunction : public Differentiator::GradientFunction {
+public:
+ ImplicitSurfaceFunction(const ContactGeometry& geom)
+ : Differentiator::GradientFunction(3), geom(geom) { }
+
+ int f(const Vector& y, Real& fy) const {
+ fy = geom.calcSurfaceValue(Vec3::getAs(&y[0]));
+ return 0;
+ }
+
+ const ContactGeometry& geom;
+};
+
+class ImplicitSurfaceGradient : public Differentiator::JacobianFunction {
+public:
+ ImplicitSurfaceGradient(const ContactGeometry& geom)
+ : Differentiator::JacobianFunction(3,3), geom(geom) { }
+
+ int f(const Vector& y, Vector& fy) const {
+ fy = (Vector)geom.calcSurfaceGradient(Vec3::getAs(&y(0)));
+ return 0;
+ }
+
+ const ContactGeometry& geom;
+};
+
+void testSurfaceGradient(const ContactGeometry& geom) {
+
+ // setup random test points
+ Random::Gaussian random(0, r);
+ for (int i = 0; i < 100; i++) {
+ Vec3 pt(random.getValue(), random.getValue(), random.getValue());
+ compareAnalyticalAndNumericGradient(geom, pt);
+ }
+}
+
+void compareAnalyticalAndNumericGradient(const ContactGeometry& geom, const Vec3& pt) {
+
+ ImplicitSurfaceFunction surf(geom);
+ Differentiator diff(surf);
+
+ Vector tmp = diff.calcGradient((Vector)pt, Differentiator::CentralDifference);
+ Vec3 gradNumeric = Vec3::getAs(&tmp(0));
+ Vec3 gradAnalytic = geom.calcSurfaceGradient(pt);
+
+// cout << "ana gradient = " << gradAnalytic << endl;
+// cout << "num gradient = " << gradNumeric << endl;
+ assertEqual(gradNumeric, gradAnalytic);
+}
+
+void testSurfaceHessian(const ContactGeometry& geom) {
+
+ // setup random test points
+ Random::Gaussian random(0, r);
+ for (int i = 0; i < 100; i++) {
+ Vec3 pt(random.getValue(), random.getValue(), random.getValue());
+ compareAnalyticalAndNumericHessian(geom, pt);
+ }
+}
+
+void compareAnalyticalAndNumericHessian(const ContactGeometry& geom, const Vec3& pt) {
+
+ ImplicitSurfaceGradient grad(geom);
+ Differentiator diff(grad);
+
+ Matrix tmp = diff.calcJacobian((Vector)pt);
+ Mat33 hessNumeric = Mat33::getAs(&tmp(0,0));
+ Mat33 hessAnalytic = geom.calcSurfaceHessian(pt);
+
+// cout << "ana hessian = " << hessAnalytic << endl;
+// cout << "num hessian = " << hessNumeric << endl;
+ assertEqual(hessNumeric(0), hessAnalytic(0));
+ assertEqual(hessNumeric(1), hessAnalytic(1));
+ assertEqual(hessNumeric(2), hessAnalytic(2));
+}
+
+
+
+//==============================================================================
+// GEODESIC EVALUTORS
+//==============================================================================
+
+
+void compareAnalyticalAndNumericGeodesic(const ContactGeometry& geom, const Vec3& P, const Vec3& Q) {
+
+ Geodesic geodNumeric;
+ Geodesic geodAnalytic;
+
+ //TODO -- tests only pass for very small amounts of noise added
+ // to the initial conditions for the numerical geodesic calculation
+
+ Real lengthNoise = 0;
+ Real tPNoise = 0;
+
+ Real len;
+ Random::Gaussian dlen(0, lengthNoise);
+
+ Vec3 tP(0);
+ Random::Gaussian dtP(0, tPNoise);
+
+ UnitVec3 e_PQ(Q-P);
+// cout << "P = " << P << ", Q = " << Q << endl;
+
+ // short geodesic, tPhint and tQhint point toward each other
+ geom.calcGeodesicAnalytical(P, Q, e_PQ, e_PQ, geodAnalytic);
+
+ // set init length and tangent dir to analytical result + noise
+ len = geodAnalytic.getLength()*(1+dlen.getValue());
+ tP = geodAnalytic.getTangentP() + Vec3(dtP.getValue(), dtP.getValue(), dtP.getValue());
+
+ geom.calcGeodesicUsingOrthogonalMethod(P, Q, tP, len, geodNumeric);
+
+// cout << "ana length = " << geodAnalytic.getLength() << endl;
+// cout << "ini length = " << len << endl;
+// cout << "num length = " << geodNumeric.getLength() << endl;
+ assertEqual(geodNumeric.getLength(), geodAnalytic.getLength());
+
+// cout << "ana tangentP = " << geodAnalytic.getTangentP() << endl;
+// cout << "ini tangentP = " << tP << endl;
+// cout << "num tangentP = " << geodNumeric.getTangentP() << endl;
+ assertEqual(geodNumeric.getTangentP(), geodAnalytic.getTangentP());
+
+// cout << "ana tangentQ = " << geodAnalytic.getTangentQ() << endl;
+// cout << "num tangentQ = " << geodNumeric.getTangentQ() << endl;
+ assertEqual(geodNumeric.getTangentQ(), geodAnalytic.getTangentQ());
+}
+
+
+void testAnalyticalGeodesicRandom(const ContactGeometry& geom) {
+
+ Vec3 P(r,0,0);
+ Vec3 Q(0,r,0);
+ compareAnalyticalAndNumericGeodesic(geom,P,Q);
+
+ // setup random test points
+ Random::Gaussian random(0, r);
+ for (int i = 0; i < 100; i++) {
+ Vec3 P(random.getValue(), random.getValue(), random.getValue());
+ Vec3 Q(random.getValue(), random.getValue(), random.getValue());
+ compareAnalyticalAndNumericGeodesic(geom,P,Q);
+ }
+}
+
+void testAnalyticalSphereGeodesic() {
+ ContactGeometry::Sphere sphere(r);
+ testAnalyticalGeodesicRandom(sphere);
+}
+
+void testAnalyticalCylinderGeodesic() {
+ ContactGeometry::Cylinder cylinder(r);
+ testAnalyticalGeodesicRandom(cylinder);
+}
+
+void testProjectDownhillToNearestPoint(const ContactGeometry& geom, Real r) {
+
+ bool inside;
+ UnitVec3 normal;
+ Vec3 pos(1,1,1);
+
+ Random::Gaussian random(0, r);
+ for (int i = 0; i < 100; i++) {
+ Vec3 pos(random.getValue(), random.getValue(), random.getValue());
+// cout << i << ": pos=" << pos << endl;
+
+ Vec3 nearestAnalytical = geom.findNearestPoint(pos, inside, normal);
+ Vec3 nearestProjected = geom.projectDownhillToNearestPoint(pos);
+
+// cout << "near pt analytical = " << nearestAnalytical << "norm = " << nearestAnalytical.norm() << endl;
+// cout << "near pt projected = " << nearestProjected << "norm = " << nearestProjected.norm() << endl;
+// cout << endl;
+ assertEqual(nearestAnalytical, nearestProjected);
+
+ }
+}
+
+
+int main() {
+ try {
+ testHalfSpace();
+ testSphere();
+ testEllipsoid();
+ testCylinder();
+ testTorus();
+
+ // TODO clean up these tests and use them
+// testAnalyticalSphereGeodesic();
+// testAnalyticalCylinderGeodesic();
+ testProjectDownhillToNearestPoint(ContactGeometry::Sphere(r), r);
+ testProjectDownhillToNearestPoint(ContactGeometry::Ellipsoid(Vec3(1.5, 2.2, 3.1)), r);
+// testProjectDownhillToNearestPoint(ContactGeometry::Torus(3*r, r), 3*r);
+ }
+ catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ cout << "Done" << endl;
+ return 0;
+}
diff --git a/SimTKmath/tests/TestGeo.cpp b/SimTKmath/tests/TestGeo.cpp
new file mode 100644
index 0000000..7d7a8bc
--- /dev/null
+++ b/SimTKmath/tests/TestGeo.cpp
@@ -0,0 +1,428 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* Tests for low-level geometric primitives and algorithms. */
+
+#include "SimTKmath.h"
+#include <vector>
+#include <exception>
+
+using namespace SimTK;
+using namespace std;
+
+static double Tol = NTraits<double>::getSignificant();
+static float fTol = NTraits<float>::getSignificant();
+
+
+template <class P>
+static void checkSphere(const Geo::Sphere_<P>& sph, const Array_<Vec<3,P> > pts) {
+ const P radius = sph.getRadius();
+ for (int i=0; i < (int)pts.size(); ++i) {
+ const P dist = (pts[i]-sph.getCenter()).norm();
+ SimTK_TEST(dist <= radius);
+ }
+}
+
+static void addOctohedron(vector<Vec3>& vertices, vector<int>& faceIndices,
+ Vec3 offset) {
+ int start = (int)vertices.size();
+ vertices.push_back(Vec3(0, 1, 0)+offset);
+ vertices.push_back(Vec3(1, 0, 0)+offset);
+ vertices.push_back(Vec3(0, 0, 1)+offset);
+ vertices.push_back(Vec3(-1, 0, 0)+offset);
+ vertices.push_back(Vec3(0, 0, -1)+offset);
+ vertices.push_back(Vec3(0, -1, 0)+offset);
+ int faces[8][3] = {{0, 2, 1}, {0, 3, 2}, {0, 4, 3}, {0, 1, 4},
+ {5, 1, 2}, {5, 2, 3}, {5, 3, 4}, {5, 4, 1}};
+ for (int i = 0; i < 8; i++)
+ for (int j = 0; j < 3; j++)
+ faceIndices.push_back(faces[i][j]+start);
+}
+
+// From Peter Eastman's tri mesh tests.
+void testTriMeshBoundingSphere() {
+ Random::Uniform random(0, 10);
+ Real ratio=0, worst=0, best=Infinity;
+ const int NTrials = 100;
+
+ for (int i = 0; i < NTrials; i++) {
+ // Create a mesh consisting of a random number of octohedra at random
+ // places.
+
+ vector<Vec3> vertices;
+ vector<int> faceIndices;
+ int numOctohedra = random.getIntValue()+1;
+ for (int j = 0; j < numOctohedra; j++)
+ addOctohedron(vertices, faceIndices,
+ Vec3(random.getValue(), random.getValue(), random.getValue()));
+ ContactGeometry::TriangleMesh mesh(vertices, faceIndices);
+
+ // Verify that all points are inside the bounding sphere.
+
+ Vec3 center;
+ Real radius;
+ mesh.getBoundingSphere(center, radius);
+ for (int j = 0; j < mesh.getNumVertices(); j++) {
+ Real dist = (center-mesh.getVertexPosition(j)).norm();
+ SimTK_TEST(dist <= radius);
+ }
+
+
+ // Make sure the bounding sphere is reasonably compact.
+ Vec3 boxRadius = 0.5*mesh.getOBBTreeNode().getBounds().getSize();
+ SimTK_TEST(radius <= boxRadius.norm());
+
+ // Compare with fast & crude Ritter sphere. On lucky occasions the
+ // Ritter sphere can be just as good, and then roundoff might make
+ // it trivially better but it shouldn't ever actually *be* better.
+ Geo::Sphere ritter = Geo::Point::calcApproxBoundingSphere(vertices);
+ SimTK_TEST(radius <= ritter.getRadius()*(1.01));
+
+ const Real bsoas = cube(radius/ritter.getRadius());
+ ratio += bsoas;
+ if (bsoas > worst) worst=bsoas;
+ if (bsoas < best) best=bsoas;
+ }
+ ratio /= NTrials;
+ printf("avg ratio=%g worst=%g best=%g\n", ratio, worst, best);
+ SimTK_TEST(ratio <= Real(.85)); // volume ratio
+ SimTK_TEST(worst <= Real(1.3));
+
+}
+
+// Generate many sets of random points, at difficult far-away places, then
+// generate single- and double-precision bounding spheres and check them
+// for admissibility (all points in) and optimality (no bigger than needed).
+// For these the right answer is hard to come by so we can only check that
+// the minimal spheres are never larger than Ritter spheres.
+void testRandomPoints() {
+ Random::Uniform random(0, 1000);
+ Array_<Vec3> pts;
+ Array_<fVec3> fpts;
+ Real ratio=0, fratio=0, worst=0, fworst=0, best=Infinity, fbest=Infinity;
+ const int NTrials = 10000;
+ // TODO: At around 5,000,000 a case is generated where the "minimal" sphere
+ // is more than 20% larger than the (bad) Ritter sphere.
+ //const int NTrials = 10000000;
+ for (int trial=0; trial<NTrials; ++trial) {
+ pts.clear(); fpts.clear();
+ int numPoints = random.getIntValue()+1;
+ Vec3 offs = Test::randDouble()*1000*Test::randVec3();
+ fVec3 foffs((float)offs[0],(float)offs[1],(float)offs[2]);
+ Real scale = offs.norm();
+
+ for (int p=0; p<numPoints; ++p) {
+ Vec3 pt(Test::randVec3());
+ fVec3 fpt((float)pt[0],(float)pt[1],(float)pt[2]);
+ pts.push_back(pt+offs); fpts.push_back(fpt+foffs);
+ }
+
+ Geo::Sphere bs = Geo::Point::calcBoundingSphere(pts);
+ checkSphere(bs, pts);
+ Geo::Sphere_<float> fbs =
+ Geo::Point_<float>::calcBoundingSphere(fpts);
+ checkSphere(fbs, fpts);
+ Geo::Sphere as = Geo::Point::calcApproxBoundingSphere(pts);
+ checkSphere(as, pts);
+ Geo::Sphere_<float> fas =
+ Geo::Point_<float>::calcApproxBoundingSphere(fpts);
+ checkSphere(fas, fpts);
+
+ const Real bsoas = cube(bs.getRadius()/as.getRadius());
+ const Real fbsofas = cube((Real)(fbs.getRadius()/fas.getRadius()));
+ ratio += bsoas; fratio += fbsofas;
+ if (bsoas > worst) worst=bsoas;
+ if (bsoas < best) best=bsoas;
+ if (fbsofas > fworst) fworst=fbsofas;
+ if (fbsofas < fbest) fbest=fbsofas;
+
+ // The single and double precision spheres should be the same size
+ // to within a small error. The Ritter sphere is more sensitive.
+ const float frac = std::max((float)scale,1.f)*NTraits<float>::getSqrtEps();
+
+ //if (!Test::numericallyEqual((float)bs.getRadius(), fbs.getRadius(), 1, frac))
+ // printf("bs=%g fbs=%g\n", bs.getRadius(), fbs.getRadius());
+ SimTK_TEST_EQ_TOL((float)bs.getRadius(), fbs.getRadius(), 0.2f);
+ SimTK_TEST_EQ_TOL((float)as.getRadius(), fas.getRadius(), 0.2f);
+
+ // Compare Welzl spheres with fast & crude Ritter spheres. On lucky
+ // occasions the Ritter spheres can be just as good, and then roundoff
+ // might make them trivially better but they shouldn't ever actually
+ // *be* better. TODO: check at the end. There are obscure cases
+ // where the minimal sphere is too big.
+ //SimTK_TEST(bs.getRadius() <= as.getRadius()*(1.2));
+ //SimTK_TEST(fbs.getRadius() <= fas.getRadius()*(1.2f));
+ }
+ ratio /= NTrials; fratio /= NTrials;
+ printf("avg ratio=%g worst=%g best=%g\n", ratio, worst, best);
+ printf("avg fratio=%g worst=%g best=%g\n", fratio, fworst, fbest);
+ SimTK_TEST(ratio <= Real(.85)); // volume ratio
+ SimTK_TEST(worst <= Real(1.3));
+ SimTK_TEST(fratio <= Real(.85)); // volume ratio
+ SimTK_TEST(fworst <= Real(1.3));}
+
+void testCoplanarPoints() {
+ // TODO
+}
+
+void testCosphericalPoints() {
+ // TODO
+}
+
+
+void testCollinearPoints() {
+ Random::Uniform random(0, 1000);
+ Array_<Vec3> pts;
+ Array_<fVec3> fpts;
+ for (int trial=0; trial<1000; ++trial) {
+ pts.clear(); fpts.clear();
+ int numPoints = random.getIntValue()+1;
+ Vec3 offs = Test::randDouble()*1000*Test::randVec3();
+ UnitVec3 dir(Test::randVec3());
+ fVec3 foffs((float)offs[0],(float)offs[1],(float)offs[2]);
+ fUnitVec3 fdir((float)dir[0],(float)dir[1],(float)dir[2]);
+
+ int minpos, maxpos;
+ Real minval=Infinity, maxval=-Infinity;
+ for (int p=0; p<numPoints; ++p) {
+ Real pos = 100*Test::randDouble();
+ if (pos > maxval) maxval=pos, maxpos=p;
+ if (pos < minval) minval=pos, minpos=p;
+ Vec3 pt(offs+pos*dir);
+ fVec3 fpt((float)pt[0],(float)pt[1],(float)pt[2]);
+ pts.push_back(pt); fpts.push_back(fpt);
+ }
+
+ Real radius = (pts[maxpos]-pts[minpos]).norm()/2;
+
+ Geo::Sphere bs = Geo::Point::calcBoundingSphere(pts);
+ checkSphere(bs, pts);
+ Geo::Sphere_<float> fbs =
+ Geo::Point_<float>::calcBoundingSphere(fpts);
+ checkSphere(fbs, fpts);
+ Geo::Sphere as = Geo::Point::calcApproxBoundingSphere(pts);
+ checkSphere(as, pts);
+ Geo::Sphere_<float> fas =
+ Geo::Point_<float>::calcApproxBoundingSphere(fpts);
+ checkSphere(fas, fpts);
+
+ Real scale = std::max(std::max(max(offs.abs()), radius), One);
+ float ftol = float(scale)*fTol;
+ double tol = scale*Tol;
+
+
+ SimTK_TEST_EQ_TOL(bs.getRadius(), radius, tol);
+ SimTK_TEST_EQ_TOL(fbs.getRadius(), radius, ftol);
+ SimTK_TEST_EQ_TOL(as.getRadius(), radius, tol);
+ SimTK_TEST_EQ_TOL(fas.getRadius(), radius, ftol);
+
+ // Repeat test with random noise added.
+ for (int p=0; p<numPoints; ++p) {
+ Vec3 noise(Test::randVec3());
+ fVec3 fnoise((float)noise[0],(float)noise[1],(float)noise[2]);
+ pts[p] += SignificantReal*noise;
+ fpts[p] += NTraits<float>::getSignificant()*fnoise;
+ }
+
+ bs = Geo::Point::calcBoundingSphere(pts);
+ checkSphere(bs, pts);
+ fbs = Geo::Point_<float>::calcBoundingSphere(fpts);
+ checkSphere(fbs, fpts);
+ as = Geo::Point::calcApproxBoundingSphere(pts);
+ checkSphere(as, pts);
+ fas = Geo::Point_<float>::calcApproxBoundingSphere(fpts);
+ checkSphere(fas, fpts);
+
+ SimTK_TEST_EQ_TOL(bs.getRadius(), radius, tol);
+ SimTK_TEST_EQ_TOL(fbs.getRadius(), radius, ftol);
+ SimTK_TEST_EQ_TOL(as.getRadius(), radius, tol);
+ SimTK_TEST_EQ_TOL(fas.getRadius(), radius, ftol);
+ }
+}
+
+void testCollocatedPoints() {
+ Random::Uniform random(0, 1000);
+ Array_<Vec3> pts;
+ Array_<fVec3> fpts;
+ for (int trial=0; trial<1000; ++trial) {
+ pts.clear(); fpts.clear();
+ int numPoints = random.getIntValue()+1;
+ Vec3 offs = Test::randDouble()*1000*Test::randVec3();
+ fVec3 foffs((float)offs[0],(float)offs[1],(float)offs[2]);
+ Real scale = offs.norm();
+
+ for (int p=0; p<numPoints; ++p) {
+ Vec3 pt(offs);
+ fVec3 fpt((float)pt[0],(float)pt[1],(float)pt[2]);
+ pts.push_back(pt); fpts.push_back(fpt);
+ }
+
+ Geo::Sphere bs = Geo::Point::calcBoundingSphere(pts);
+ checkSphere(bs, pts);
+ Geo::Sphere_<float> fbs =
+ Geo::Point_<float>::calcBoundingSphere(fpts);
+ checkSphere(fbs, fpts);
+ Geo::Sphere as = Geo::Point::calcApproxBoundingSphere(pts);
+ checkSphere(as, pts);
+ Geo::Sphere_<float> fas =
+ Geo::Point_<float>::calcApproxBoundingSphere(fpts);
+ checkSphere(fas, fpts);
+
+ SimTK_TEST(bs.getRadius() < SqrtEps)
+ SimTK_TEST(fbs.getRadius() < NTraits<float>::getSqrtEps());
+ SimTK_TEST(as.getRadius() < SqrtEps)
+ SimTK_TEST(fas.getRadius() < NTraits<float>::getSqrtEps());
+
+ // Repeat test with random noise added.
+ for (int p=0; p<numPoints; ++p) {
+ Vec3 noise(Test::randVec3());
+ fVec3 fnoise((float)noise[0],(float)noise[1],(float)noise[2]);
+ pts[p] += SignificantReal*noise;
+ fpts[p] += NTraits<float>::getSignificant()*fnoise;
+ }
+
+ bs = Geo::Point::calcBoundingSphere(pts);
+ checkSphere(bs, pts);
+ fbs = Geo::Point_<float>::calcBoundingSphere(fpts);
+ checkSphere(fbs, fpts);
+ as = Geo::Point::calcApproxBoundingSphere(pts);
+ checkSphere(as, pts);
+ fas = Geo::Point_<float>::calcApproxBoundingSphere(fpts);
+ checkSphere(fas, fpts);
+ }
+}
+
+void testBox() {
+ Geo::Box box(Vec3(3,4,2)); // half lengths
+ SimTK_TEST(box.findVolume() == 8*24);
+ SimTK_TEST(box.getOrderedAxis(0) == ZAxis); // smallest
+ SimTK_TEST(box.getOrderedAxis(1) == XAxis); // medium
+ SimTK_TEST(box.getOrderedAxis(2) == YAxis); // largest
+
+ Geo::AlignedBox abox(Vec3(0), Vec3(1,2,3));
+ abox.setCenter(Vec3(3,4,2)+Vec3(1,2,3)-Vec3(1e-6));
+ SimTK_TEST(box.intersectsAlignedBox(abox));
+
+ abox.setCenter(Vec3(3,4,2)+Vec3(1,2,3)+Vec3(1e-6));
+ SimTK_TEST(!box.intersectsAlignedBox(abox));
+
+ Geo::OrientedBox obox(Transform(), Vec3(1,2,3));
+ SimTK_TEST(box.mayIntersectOrientedBox(obox)); // centers overlap
+ SimTK_TEST(box.intersectsOrientedBox(obox));
+
+ obox.setTransform(Vec3(10,0,0)); // x axis should separate
+ SimTK_TEST(!box.mayIntersectOrientedBox(obox));
+ SimTK_TEST(!box.intersectsOrientedBox(obox));
+
+ obox.setTransform(Vec3(3.123,-1.3,.7)); // parallel boxes that intersect
+ SimTK_TEST(box.mayIntersectOrientedBox(obox));
+ SimTK_TEST(box.intersectsOrientedBox(obox));
+
+ // Non-intersecting box for which no face will serve as separator.
+ // In this case the fast method can't tell they are separated.
+ obox.setTransform(Transform(
+ Rotation(BodyRotationSequence, Pi/4, XAxis, Pi/8, YAxis, -Pi/4, ZAxis),
+ Vec3(1.5, -5, 5.25)));
+ SimTK_TEST(box.mayIntersectOrientedBox(obox));
+ SimTK_TEST(!box.intersectsOrientedBox(obox));
+
+ // This should make them intersect.
+ obox.setTransform(Transform(
+ Rotation(BodyRotationSequence, Pi/4, XAxis, Pi/8, YAxis, -Pi/4, ZAxis),
+ Vec3(1.5, -5, 4.5)));
+ SimTK_TEST(box.mayIntersectOrientedBox(obox));
+ SimTK_TEST(box.intersectsOrientedBox(obox));
+
+}
+
+void testMiscGeo() {
+
+ // TEST GEO::POINT::POINTS ARE NUMERICALLY COINCIDENT
+ Vec3 p0, p1, p2, p3;
+ p1 = Vec3(1,2,3);
+ p2 = Vec3(2,3,4);
+ p3 = p1;
+ SimTK_TEST(!Geo::Point::pointsAreNumericallyCoincident(p1,p2));
+ SimTK_TEST(Geo::Point::pointsAreNumericallyCoincident(p1,p3));
+ p3 = p1 + Eps*p2;
+ // Default tolerance is larger than Eps.
+ SimTK_TEST(Geo::Point::pointsAreNumericallyCoincident(p1,p3));
+ // This should be enough to separate them if they are near the origin.
+ p3 = p1 + 10*Geo::getDefaultTol<Real>()*p2;
+ SimTK_TEST(!Geo::Point::pointsAreNumericallyCoincident(p1,p3));
+ // Shifting by 100 should mean that they are indistinguishable when
+ // perturbed by 10*tol.
+ p3 = 100.*p1 + 10*Geo::getDefaultTol<Real>()*p2;
+ SimTK_TEST(Geo::Point::pointsAreNumericallyCoincident(100.*p1,p3));
+ // But they should be distinguishable at 1000*tol.
+ p3 = 100.*p1 + 1000*Geo::getDefaultTol<Real>()*p2;
+ SimTK_TEST(!Geo::Point::pointsAreNumericallyCoincident(100.*p1,p3));
+ // And again indistinguishable at a looser tolerance.
+ SimTK_TEST(Geo::Point::pointsAreNumericallyCoincident(100.*p1,p3,
+ 10000.*Geo::getDefaultTol<Real>()));
+
+ Vec3 q0, q1, x0, x1; UnitVec3 u0, u1;
+ Vec2 closest; bool parallel;
+ p0 = Vec3(-1.1,0,0); q0 = Vec3(2.1,0,0); u0 = UnitVec3(q0-p0);
+ p1 = Vec3(1.1,-1.2,0); q1 = Vec3(1.1,1.3,0); u1 = UnitVec3(q1-p1);
+ Geo::findClosestPointsOfTwoLines(p0, u0, p1, u1, x0, x1, parallel);
+ SimTK_TEST_EQ(x0, Vec3(1.1,0,0)); SimTK_TEST_EQ(x1, Vec3(1.1,0,0));
+ SimTK_TEST(!parallel);
+
+ // Lift line 1 up 3 units in z.
+ p1 = Vec3(1.1,-1,3); q1 = Vec3(1.1,1,3); u1 = UnitVec3(q1-p1);
+ Geo::findClosestPointsOfTwoLines(p0, u0, p1, u1, x0, x1, parallel);
+ SimTK_TEST_EQ(x0, Vec3(1.1,0,0)); SimTK_TEST_EQ(x1, Vec3(1.1,0,3));
+ SimTK_TEST(!parallel);
+
+ // Make p1 exactly parallel to p0 but offset in y and z
+ p1 = 3*p0+Vec3(0,1,2); q1 = 2*q0+Vec3(0,1,2); u1 = UnitVec3(q1-p1);
+ Geo::findClosestPointsOfTwoLines(p0, u0, p1, u1, x0, x1, parallel);
+ SimTK_TEST_EQ(x0, Vec3(-2.2,0,0)); SimTK_TEST_EQ(x1, Vec3(-2.2,1,2));
+ SimTK_TEST(parallel);
+
+ // Bend p1 a little but still effectively parallel.
+ q1 += 0.1*SignificantReal*Vec3(0,0,1); u1 = UnitVec3(q1-p1);
+ Geo::findClosestPointsOfTwoLines(p0, u0, p1, u1, x0, x1, parallel);
+ SimTK_TEST_EQ(x0, Vec3(-2.2,0,0)); SimTK_TEST_EQ(x1, Vec3(-2.2,1,2));
+ SimTK_TEST(parallel);
+
+ // Now bend it a lot.
+ q1 += 1000*SignificantReal*Vec3(0,0,1); u1 = UnitVec3(q1-p1);
+ Geo::findClosestPointsOfTwoLines(p0, u0, p1, u1, x0, x1, parallel);
+ SimTK_TEST(!parallel);
+ // Nearest points will be very far away from the origin, but
+ // should still be close to each other.
+ SimTK_TEST((x1-x0).norm() < 3);
+}
+
+int main() {
+ SimTK_START_TEST("TestGeo");
+ SimTK_SUBTEST(testMiscGeo);
+ SimTK_SUBTEST(testBox);
+ SimTK_SUBTEST(testTriMeshBoundingSphere);
+ SimTK_SUBTEST(testRandomPoints);
+ SimTK_SUBTEST(testCollinearPoints);
+ SimTK_SUBTEST(testCollocatedPoints);
+ SimTK_END_TEST();
+}
diff --git a/SimTKmath/tests/TestSpline.cpp b/SimTKmath/tests/TestSpline.cpp
new file mode 100644
index 0000000..43a34f1
--- /dev/null
+++ b/SimTKmath/tests/TestSpline.cpp
@@ -0,0 +1,647 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Matthew Millard (the testNaturalCubicSpline code) *
+ * *
+ * 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 <vector> // use some std::vectors to test Array_ interoperability
+#include <iostream>
+#include <fstream>
+#include <cstdio>
+
+using namespace SimTK;
+using namespace std;
+
+const Real TESTTOL = 1e-9;
+
+void testSpline() {
+ Vector_<Vec3> coeff(5);
+ coeff[0] = Vec3(0, 1, 2);
+ coeff[1] = Vec3(1, 4, 1);
+ coeff[2] = Vec3(2, 2, 20);
+ coeff[3] = Vec3(1, -1, 2);
+ coeff[4] = Vec3(0, 0, 1);
+ Vector x(Vec5(0, 1, 2, 5, 10));
+
+ // Create a linear spline, and verify that it interpolates linearly between
+ // the control points.
+
+ Spline_<Vec3> spline(1, x, coeff);
+ for (int i = 0; i < x.size(); ++i)
+ SimTK_TEST_EQ(coeff[i], spline.calcValue(Vector(1, x[i])));
+ std::vector<int> deriv;
+ deriv.push_back(0);
+ for (int i = 0; i < x.size()-1; ++i) {
+ for (int j = 0; j < 10; ++j) {
+ Real fract = (i+1.0)/12.0;
+ Real t = x[i]+fract*(x[i+1]-x[i]);
+ SimTK_TEST_EQ_TOL(spline.calcValue(Vector(1, t)),
+ coeff[i]+fract*(coeff[i+1]-coeff[i]), TESTTOL);
+ SimTK_TEST_EQ_TOL(spline.calcDerivative(deriv, Vector(1, t)),
+ (coeff[i+1]-coeff[i])/(x[i+1]-x[i]),TESTTOL);
+ }
+ }
+
+ // Create a cubic spline and verify the derivative calculations.
+
+ spline = Spline_<Vec3>(3, x, coeff);
+ Real delta = 1e-10;
+ for (int i = 0; i < x.size()-1; ++i) {
+ for (int j = 0; j < 10; ++j) {
+ Real fract = (i+1.0)/12.0;
+ Real t = x[i]+fract*(x[i+1]-x[i]);
+ Vec3 value1 = spline.calcValue(Vector(1, t-delta));
+ Vec3 value2 = spline.calcValue(Vector(1, t+delta));
+ SimTK_TEST_EQ_TOL(spline.calcDerivative(deriv, Vector(1, t)),
+ (value2-value1)/(2*delta), 1e-4);
+ }
+ }
+}
+
+void testSplineFitter() {
+ Real stddev = 0.5;
+ int n = 100;
+ Random::Gaussian random(0.0, stddev);
+ Vector x(n);
+ Vector_<Vec3> truey(n);
+ Vector_<Vec3> y(n);
+ for (int i = 0; i < x.size(); ++i) {
+ x[i] = i*0.1;
+ truey[i] = Vec3(sin(x[i]), 3.0*sin(2*x[i]), cos(x[i]));
+ y[i] = truey[i] + Vec3(random.getValue(),random.getValue(),random.getValue());
+ }
+ SplineFitter<Vec3> fitter = SplineFitter<Vec3>::fitFromGCV(3, x, y);
+ Spline_<Vec3> spline1 = fitter.getSpline();
+
+ // The fitting should have reduced the error.
+
+ Vec3 originalError = mean(abs(y-truey));
+ Vec3 fittedError = mean(abs(spline1.getControlPointValues()-truey));
+ SimTK_TEST(fittedError[0] < originalError[0]);
+ SimTK_TEST(fittedError[1] < originalError[1]);
+ SimTK_TEST(fittedError[2] < originalError[2]);
+
+ // If we perform the fitting again, explicitly specifying the same value for
+ // the smoothing parameter, it should produce identical results.
+ SimTK_TEST_EQ_TOL(SplineFitter<Vec3>::fitForSmoothingParameter
+ (3, x, y, fitter.getSmoothingParameter())
+ .getSpline().getControlPointValues(),
+ spline1.getControlPointValues(),
+ TESTTOL);
+
+ // Likewise, specifying the same number of degrees of freedom should produce
+ // identical results.
+ SimTK_TEST_EQ_TOL(SplineFitter<Vec3>::fitFromDOF
+ (3, x, y, fitter.getDegreesOfFreedom())
+ .getSpline().getControlPointValues(),
+ spline1.getControlPointValues(),
+ TESTTOL);
+
+ // If we specify a smoothing parameter of 0, it should exactly reproduce
+ // the original data.
+ Spline_<Vec3> nosmoothing = SplineFitter<Vec3>::fitForSmoothingParameter
+ (3, x, y, 0.0).getSpline();
+ for (int i = 0; i < x.size(); ++i)
+ SimTK_TEST_EQ_TOL(y[i], nosmoothing.calcValue(Vector(1, x[i])),TESTTOL);
+}
+
+void testRealSpline() {
+ Vector coeff(5);
+ coeff[0] = 0;
+ coeff[1] = 1;
+ coeff[2] = 2;
+ coeff[3] = 1;
+ coeff[4] = 0;
+ Vector x(Vec5(0, 1, 2, 5, 10));
+
+ // Create a linear spline, and verify that it interpolates linearly between
+ // the control points.
+
+ Spline spline(1, x, coeff);
+ for (int i = 0; i < x.size(); ++i)
+ SimTK_TEST_EQ_TOL(coeff[i], spline.calcValue(Vector(1, x[i])), TESTTOL);
+ Array_<int> deriv;
+ deriv.push_back(0);
+ for (int i = 0; i < x.size()-1; ++i) {
+ for (int j = 0; j < 10; ++j) {
+ Real fract = (i+1.0)/12.0;
+ Real t = x[i]+fract*(x[i+1]-x[i]);
+ SimTK_TEST_EQ_TOL(spline.calcValue(Vector(1, t)),
+ coeff[i]+fract*(coeff[i+1]-coeff[i]), TESTTOL);
+ SimTK_TEST_EQ_TOL(spline.calcDerivative(deriv, Vector(1, t)),
+ (coeff[i+1]-coeff[i])/(x[i+1]-x[i]), TESTTOL);
+ }
+ }
+ SimTK_TEST_EQ_TOL(1, spline.getControlPointValues()[1], TESTTOL);
+
+ // Try using a SplineFitter.
+
+ SplineFitter<Real> fitter = SplineFitter<Real>::fitFromGCV(3, x, coeff);
+ Spline spline2 = fitter.getSpline();
+ SimTK_TEST_EQ_TOL(3, spline2.getSplineDegree(),TESTTOL);
+}
+
+//MM bits added to test the numerical accuracy of the natural cubic splines.
+/**
+* This function computes a standard central difference dy/dx.
+* If extrap_endpoints is set to 1, then the derivative at the
+* end points is estimated by linearly extrapolating the dy/dx
+* values beside the end points
+*
+* @param x domain vector
+* @param y range vector
+& @param extrap_endpoints:
+* (false) Endpoints of the returned vector will be zero,
+* because a central difference is undefined at
+* these endpoints
+* (true) Endpoints are computed by linearly extrapolating
+* using a first difference from the neighboring 2
+* points
+*
+* @returns dy/dx computed using central differences
+*/
+Vector getCentralDifference(Vector x, Vector y,
+ bool extrap_endpoints){
+ Vector dy(x.size());
+ double dx1,dx2;
+ double dy1,dy2;
+ int size = x.size();
+ for(int i=1; i<x.size()-1; i++){
+ dx1 = x(i)-x(i-1);
+ dx2 = x(i+1)-x(i);
+ dy1 = y(i)-y(i-1);
+ dy2 = y(i+1)-y(i);
+ dy(i)= 0.5*dy1/dx1 + 0.5*dy2/dx2;
+ }
+
+ if(extrap_endpoints == true){
+ dy1 = dy(2)-dy(1);
+ dx1 = x(2)-x(1);
+ dy(0) = dy(1) + (dy1/dx1)*(x(0)-x(1));
+
+ dy2 = dy(size-2)-dy(size-3);
+ dx2 = x(size-2)-x(size-3);
+ dy(size-1) = dy(size-2) + (dy2/dx2)*(x(size-1)-x(size-2));
+ }
+ return dy;
+}
+
+/**
+* This function will print cvs file of the column vector
+* col0 and the matrix data
+*
+* @params col0: A vector that must have the same number of rows
+* as the data matrix. This column vector is
+* printed as the first column
+* @params data: A matrix of data
+* @params filename: The name of the file to print
+*/
+void printMatrixToFile(Vector col0,Matrix data, string filename){
+ ofstream datafile;
+ datafile.open(filename.c_str());
+
+ for(int i = 0; i < data.nrow(); i++){
+ datafile << col0(i) << ",";
+ for(int j = 0; j < data.ncol(); j++){
+ if(j<data.ncol()-1)
+ datafile << data(i,j) << ",";
+ else
+ datafile << data(i,j) << "\n";
+ }
+ }
+ datafile.close();
+}
+
+/**
+* This function will compute the value and first two
+* derivatives of an analytic function at the point x.
+*
+* @params x the input value
+* @params fcnType the function to compute. There are
+* currently 5 choices (see below)
+* @returns Vector: a 3x1 vector of the value,
+* first derivative and second derivative
+*/
+Vector getAnalyticFunction(double x,int fcnType){
+ Vector fdF(3);
+ fdF = -1;
+
+ switch(fcnType){
+ case 0: //f(x) = 0;
+ fdF = 0;
+ break;
+ case 1: //f(x) = 2*x
+ fdF(0) = 2*x; //f
+ fdF(1) = 2; //fx
+ fdF(2) = 0;
+ break;
+ case 2: //f(x) = x^2
+ fdF(0) = x*x; //f
+ fdF(1) = 2*x; //fx
+ fdF(2) = 2;
+ break;
+ case 3: //f(x) = 2*x + x*x;
+ //f
+ fdF(0) = 2*x + x*x;
+ fdF(1) = 2 + 2*x;
+ fdF(2) = 2;
+ break;
+ case 4: //f(x) =2*x + x*x + 5*x*x*x
+ fdF(0) = 2*x + x*x + 5*x*x*x;
+ fdF(1) = 2 + 2*x + 15*x*x;
+ fdF(2) = 2 + 30*x;
+ break;
+ case 5: //fx(x) = sin(x)
+ fdF(0) = sin(x);
+ fdF(1) = cos(x);
+ fdF(2) = -sin(x);
+ break;
+ default:
+ cout << "Invalid fcnType in testBicubicSurface.cpp: getAnayticFunction";
+ }
+
+
+ return fdF;
+}
+/**
+* This function tests the accuracy of the natural cubic spline sp.
+* The accuracy of the spline is tested in the following manner:
+*
+* a. Spline must pass through the knots given
+* -Error between spline and input data at the knots
+* (should be zero)
+* b. The first derivatives are continuous at the knot points
+* -Error between the value of the first derivative at
+* the knot point, and what a linear extrapolation would
+* predict just to the left and right ofthe knot point.
+* (should be zero, within a tolerace affected by the
+* step size in xD)
+* c. The second derivatives are continuous at the knots points
+* -Error between the value of the numerically calculated
+* derivative at the knot point, and what a linear
+* extrapolation would predict just to the left and
+* right of the knot point. (should be zero, within a
+* tolerace affected by the step size in xD)
+* d. The second derivative is zero at the end points.
+* -Numerically calculated extrapolation of the 2nd
+* derivative should be zero at the end points within
+* some tolerance
+*
+*/
+Vector benchmarkNaturalCubicSpline
+ (Function* sp, Vector xK, Vector yK, Vector xM,Vector xD,
+ string name, bool print)
+{
+ int size = xK.size();
+ int sizeD= xD.size();
+ int sizeDK = xD.size()/(xK.size()-1);
+ double deltaD = (xK(xK.size()-1)-xK(0))/xD.size();
+
+ Matrix ysp_K(size,2),ysp_M(size-1,2),ysp_D(sizeD,4);
+ Vector errVec(4);
+ errVec = 1;
+ ysp_K = 0;
+ ysp_M = 0;
+ ysp_D = 0;
+
+ vector<int> derOrder(1);
+ derOrder[0] = 0;
+
+
+
+ ///////////////////////////////////////////
+ //1. Evaluate the spline at the knots, the mid points and then a dense sample
+ ///////////////////////////////////////////
+ Vector tmpV1(1);
+ double xVal=0;
+ for(int i=0;i<size;i++){
+ xVal = xK(i);
+ tmpV1(0)=xK(i);
+ ysp_K(i,0) = sp->calcValue(tmpV1);
+ ysp_K(i,1) = sp->calcDerivative(derOrder,tmpV1);
+ }
+ for(int i=0;i<size-1;i++){
+ xVal = xM(i);
+ tmpV1(0) = xM(i);
+ ysp_M(i,0) = sp->calcValue(tmpV1);
+ ysp_M(i,1) = sp->calcDerivative(derOrder,tmpV1);
+ }
+ for(int i=0;i<sizeD;i++){
+ xVal = xD(i);
+ tmpV1(0) = xD(i);
+ ysp_D(i,0) = sp->calcValue(tmpV1);
+ ysp_D(i,1) = sp->calcDerivative(derOrder,tmpV1);
+ }
+
+ //////////////////////////////////////
+ //2. Compute the second derivative of the spline (using central
+ //differences), and linearly interpolate to get the end points.
+ //The end points should go to exactly zero because the second
+ // derivative is linear in a cubic spline, as is the linear
+ // extrapolation
+ //
+ // Also compute the 3rd derivative using the same method. The 3rd
+ // derivative is required in the test to determine if the second
+ // derivative is continuous at the knots or not.
+ //////////////////////////////////////
+
+ ysp_D(2) = getCentralDifference(xD, ysp_D(1), true);
+ ysp_D(3) = getCentralDifference(xD, ysp_D(2), true);
+
+ //////////////////////////////////////
+ //3. Now check to see if the splines meet the conditions of a
+ //natural cubic spline:
+ //////////////////////////////////////
+
+ Vector tmpK(size,size),tmpM(size-1,size-1);
+
+ //* a. Spline passes through all knot points given
+ tmpK = yK-ysp_K(0);
+ errVec(0) = tmpK.norm();
+
+ // b. The first derivative is continuous at the knot points.
+ // Apply a continuity test to the data points that define
+ // the second derivative
+ //
+ // Continuity test: a linear extrapolation of first
+ // derivative of the curve in interest
+ // on either side of the point in
+ // interest should equal the point in
+ // interest;
+
+ double ykL,ykR,y0L,dydxL,y0R,dydxR = 0;
+ for(int i=1; i<size-1; i++){
+ y0L = ysp_D(i*sizeDK-1,1);
+ y0R = ysp_D(i*sizeDK+1,1);
+ dydxL = ysp_D(i*sizeDK-1,2); //Found using central differences
+ dydxR = ysp_D(i*sizeDK+1,2); //Found using central differences
+ ykL = y0L + dydxL*deltaD;
+ ykR = y0R - dydxR*deltaD;
+ errVec(1) = (ysp_D(i*sizeDK,1)-ykL)+(ysp_D(i*sizeDK,1)-ykR);
+ }
+
+
+ // c. The second derivative is continuous at the knot points.
+ // Apply a continuity test to the data points that define
+ // the second derivative. This also tests if the first
+ // derivative is smooth.
+ //
+ // Continuity test: a linear extrapolation of first
+ // derivative of the curve in interest
+ // on either side of the point in
+ // interest should equal the point in
+ // interest;
+ for(int i=1; i<size-1; i++){
+ y0L = ysp_D(i*sizeDK-1,2);
+ y0R = ysp_D(i*sizeDK+1,2);
+ dydxL = ysp_D(i*sizeDK-1,3); //Found using central differences
+ dydxR = ysp_D(i*sizeDK+1,3); //Found using central differences
+ ykL = y0L + dydxL*deltaD;
+ ykR = y0R - dydxR*deltaD;
+ errVec(2) = (ysp_D(i*sizeDK,2)-ykL)+(ysp_D(i*sizeDK,2)-ykR);
+ }
+
+ //////////////////////////////////////
+ //* d. The second derivative is zero at the end points
+ //////////////////////////////////////
+
+ errVec(3) = abs(ysp_D(0,2)) + abs(ysp_D(sizeD-1,2));
+
+
+
+ //////////////////////////////////////
+ //print the data for analysis
+ //////////////////////////////////////
+
+ if(print==true){
+ string fname = name;
+ fname.append("_K.csv");
+ printMatrixToFile(xK, ysp_K, fname);
+
+ fname = name;
+ fname.append("_M.csv");
+ printMatrixToFile(xM, ysp_M, fname);
+
+ fname = name;
+ fname.append("_D.csv");
+ printMatrixToFile(xD, ysp_D, fname);
+ }
+
+ return errVec;
+
+}
+
+/**
+* The test works by seeing if the tested splines have the properties of
+* a natural cubic spline. To do so, this test file has several steps
+*
+* User Steps: Configure the script
+* a. Choose the function to be interpolated
+* b. Choose the location and number of knot points
+* c. Choose the density of a high resolution interpolation
+*
+* Test Script Steps:
+* 0. Initialize the input vectors xK, xM, and xD for the knot locations,
+* mid knot location and high resolution step locations respectively
+* 1. Initialize the analytically computed output yK, yM and yD
+* 2. Create each of the spline objects.
+* 3. Evaluate the numerical accuracy of the splines by calling
+* testNaturalCubicSpline
+*/
+void testNaturalCubicSpline() {
+ /////////////////////////////
+ //Configuration Variables
+ ////////////////////////////
+ bool printToTerminal = false; //Setting this to true will print some
+ //useful data to the terminal
+ bool printData = false; //Set to true to print the knot,
+ //mid knot, and
+ //dense vector values, first derivatives,
+ //and second derivatives (for the splines)
+ //for analysis outside of this script.
+ int fcnType = 5; //Chooses what kind of analytical test function to
+ //use to initialize and test the various
+ //spline classes
+ const int size =6; //Number of knot points
+ const int sizeDK = 100; //Number of points per knot in
+ //the densely sampled vector
+ int sizeD=sizeDK*(size-1); //Number of points in a densly sampled
+ //interpolation
+
+ //Domain vector variables
+ double xmin,xmax,deltaX,deltaD;
+ xmin = Pi/4; //Value of first knot
+ xmax = Pi/2; //Value of the final knot
+
+ deltaX = (xmax-xmin)/(size-1);
+ deltaD = (xmax-xmin)/(sizeD-1);
+ double etime = 0;
+ /////////////////////////////
+ //Test Code body
+ ////////////////////////////
+
+ Matrix testResults(4,1); //This matrix stores the results of the
+ //5 tests in each row entry, for each
+ //of the 2 spline classes tested.
+ // SimTK SplineFitter results are stored
+ // in column 0
+ // OpenSim::NaturalCubicSpline results
+ // are stored in column 1
+ //testResults.elementwiseAssign(0.0);
+ testResults = -1;
+ Vector tmpV1(1);
+
+ //Generate initialization knot points (denoted by a 'K')
+ // and the mid points (denoted by a 'M')
+ // and for the densely sampled interpolation vector
+ // (denoted by a 'D')
+ Vector xK(size), xM(size-1), xD(sizeD);
+ Matrix yK(size,3), yM(size-1,3), yD(sizeD,3);
+
+
+
+ ///////////////////////////////////////////
+ //0. Initialize the input vectors xK, xM and xD
+ ///////////////////////////////////////////
+ for (int i = 0; i < size; i++) {
+ xK(i) = xmin + ((double)i)*deltaX;
+ if(i<size-1){
+ xM(i) = xmin + deltaX/(double)2 + ((double)i)*deltaX;
+ }
+ }
+ for(int i = 0; i < sizeD; i++)
+ xD(i) = xmin + deltaD*(double)i;
+
+ ///////////////////////////////////////////
+ //1. Initialize the analytic function vector data to interpolate
+ // Let the user know which function is being used
+ ///////////////////////////////////////////
+ if(printToTerminal==true){
+ switch(fcnType){
+ case 0:
+ cout << "f(x) = 0" <<endl;
+ break;
+ case 1:
+ cout << "f(x) = 2*x" <<endl;
+ break;
+ case 2:
+ cout << "f(x) = x^2" <<endl;
+ break;
+ case 3:
+ cout << "f(x) = 2*x + x^2 " <<endl;
+ break;
+ case 4:
+ cout << "f(x) = 2*x + x^2 + 5x^3 " <<endl;
+ break;
+ }
+ }
+
+
+ //Get the function values at the knot points
+ Vector tmp(3);
+ tmp = 0;
+ for(int i=0; i<size;i++){
+ tmp = getAnalyticFunction(xK(i),fcnType);
+ for(int k=0;k<3;k++)
+ yK(i,k) = tmp(k);
+
+ }
+ Vector yKVal = yK(0);
+
+ //Get the function y, dy, ddy at the mid points
+ for(int i=0; i<size-1;i++){
+ tmp = getAnalyticFunction(xM(i),fcnType);
+ for(int k=0;k<3;k++)
+ yM(i,k) = tmp(k);
+ }
+ //Get the function y, dy, ddy at the dense points
+ for(int i=0; i<sizeD;i++){
+ tmp = getAnalyticFunction(xD(i),fcnType);
+ for(int k=0;k<3;k++)
+ yD(i,k) = tmp(k);
+ }
+
+ ///////////////////////////////////////////
+ //2. Create each of the splines
+ ///////////////////////////////////////////
+
+ //SplineFitter
+ Vector sfDerivs1(xK.size());
+
+
+
+ Spline_<Real> sTK = SplineFitter<Real>::fitForSmoothingParameter(3,xK,yKVal,0.0).getSpline();
+
+ ///////////////////////////////////////////
+ //3. Test the splines
+ ///////////////////////////////////////////
+
+ testResults(0) = benchmarkNaturalCubicSpline(&sTK, xK, yK(0), xM, xD, "simtk_splinefitter",true);
+ if(printToTerminal==true){
+ cout << "Test Result Matrix: 0 or small numbers pass" <<endl;
+ cout << " column 0: SplineFitter, column 1: OpenSim::NaturalCubicSpline" <<endl;
+ cout << " row 0: Passes through knots (tol 1e-14)" << endl;
+ cout << " row 1: First derivative is continuous and smooth (tol " << deltaD << ")" << endl;
+ cout << " row 2: Second derivative is continuous (tol " << 10*deltaD << ")" << endl;
+ cout << " row 3: Second derivative is zero at endpoints (tol "<< deltaD/10 <<")" << endl;
+ cout << testResults << endl;
+ }
+
+ //////////////////////////////////////
+ //4. Run numerical assertions on each test
+ //////////////////////////////////////
+
+ double tol = 0;
+
+ for(int k=0;k<testResults.ncol();k++){
+ for(int i=0;i<testResults.nrow();i++){
+ switch(i){
+ case 0: //Equal at knots
+ tol = 1e-14;
+ break;
+ case 1: //Continuous 1st derivative
+ tol = deltaD;
+ break;
+ case 2: //Continuous 2nd derivative
+ tol = 10*deltaD;
+ break;
+ case 3: //2nd derivative zero at end points
+ tol = deltaD/10;
+ break;
+ default:
+ cout << "testNCSpline: Invalid error type selected" << endl;
+ }
+ //cout << "Testing (i,k) " << i << " " << k << " tol " << tol << " \tval " << testResults(i,k) << endl;
+ SimTK_TEST_EQ_TOL(testResults(i,k),0,tol);
+ }
+ }
+ // getchar();
+
+}
+
+int main () {
+ SimTK_START_TEST("TestSpline");
+ SimTK_SUBTEST(testSpline);
+ SimTK_SUBTEST(testSplineFitter);
+ SimTK_SUBTEST(testRealSpline);
+ SimTK_SUBTEST(testNaturalCubicSpline);
+ SimTK_END_TEST();
+}
diff --git a/SimTKmath/tests/TestTriangleMesh.cpp b/SimTKmath/tests/TestTriangleMesh.cpp
new file mode 100644
index 0000000..55039c8
--- /dev/null
+++ b/SimTKmath/tests/TestTriangleMesh.cpp
@@ -0,0 +1,320 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 <vector>
+#include <exception>
+
+using namespace SimTK;
+using namespace std;
+
+void testTriangleMesh() {
+ // Create a mesh representing a tetrahedron (4 vertices, 4 faces, 6 edges).
+
+ vector<Vec3> vertices;
+ vertices.push_back(Vec3(0, 0, 0));
+ vertices.push_back(Vec3(1, 0, 0));
+ vertices.push_back(Vec3(0, 1, 0));
+ vertices.push_back(Vec3(0, 0, 1));
+ int faces[4][3] = {{0, 2, 1}, {0, 3, 2}, {0, 1, 3}, {1, 2, 3}};
+ vector<int> faceIndices;
+ for (int i = 0; i < 4; i++)
+ for (int j = 0; j < 3; j++)
+ faceIndices.push_back(faces[i][j]);
+ ContactGeometry::TriangleMesh mesh(vertices, faceIndices);
+ SimTK_TEST(mesh.getNumVertices() == 4);
+ SimTK_TEST(mesh.getNumFaces() == 4);
+ SimTK_TEST(mesh.getNumEdges() == 6);
+
+ // Verify that all faces and vertices are correct.
+
+ for (int i = 0; i < (int) vertices.size(); i++)
+ SimTK_TEST_EQ(vertices[i], mesh.getVertexPosition(i));
+ for (int i = 0; i < 4; i++) {
+ SimTK_TEST(faces[i][0] == mesh.getFaceVertex(i, 0));
+ SimTK_TEST(faces[i][1] == mesh.getFaceVertex(i, 1));
+ SimTK_TEST(faces[i][2] == mesh.getFaceVertex(i, 2));
+ }
+
+ // Verify that all indices are consistent.
+
+ for (int i = 0; i < mesh.getNumFaces(); i++) {
+ for (int j = 0; j < 3; j++) {
+ int edge = mesh.getFaceEdge(i, j);
+ SimTK_TEST( mesh.getEdgeFace(edge, 0) == i
+ || mesh.getEdgeFace(edge, 1) == i);
+ for (int k = 0; k < 2; k++)
+ SimTK_TEST(mesh.getEdgeVertex(edge, k) == mesh.getFaceVertex(i, 0) ||
+ mesh.getEdgeVertex(edge, k) == mesh.getFaceVertex(i, 1) ||
+ mesh.getEdgeVertex(edge, k) == mesh.getFaceVertex(i, 2));
+ }
+ }
+ for (int i = 0; i < mesh.getNumEdges(); i++) {
+ for (int j = 0; j < 2; j++) {
+ int face = mesh.getEdgeFace(i, j);
+ SimTK_TEST(mesh.getFaceEdge(face, 0) == i || mesh.getFaceEdge(face, 1) == i || mesh.getFaceEdge(face, 2) == i);
+ }
+ }
+ for (int i = 0; i < mesh.getNumVertices(); i++) {
+ Array_<int> edges;
+ mesh.findVertexEdges(i, edges);
+ SimTK_TEST(edges.size() == 3);
+ for (int j = 0; j < (int) edges.size(); j++)
+ SimTK_TEST(mesh.getEdgeVertex(edges[j], 0) == i || mesh.getEdgeVertex(edges[j], 1) == i);
+ }
+
+ // Check the face normals and areas.
+
+ SimTK_TEST_EQ(mesh.getFaceArea(0), 0.5);
+ SimTK_TEST_EQ(mesh.getFaceArea(1), 0.5);
+ SimTK_TEST_EQ(mesh.getFaceArea(2), 0.5);
+ SimTK_TEST_EQ(mesh.getFaceArea(3), std::sin(Pi/3.0));
+ SimTK_TEST_EQ(mesh.getFaceNormal(0), Vec3(0, 0, -1));
+ SimTK_TEST_EQ(mesh.getFaceNormal(1), Vec3(-1, 0, 0));
+ SimTK_TEST_EQ(mesh.getFaceNormal(2), Vec3(0, -1, 0));
+ SimTK_TEST_EQ(mesh.getFaceNormal(3), Vec3(1, 1, 1)/Sqrt3);
+}
+
+/**
+ * Given an invalid mesh, verify that the constructor throws an exception.
+ */
+
+void verifyMeshThrowsException(vector<Vec3> vertices, int faces[][3], int numFaces) {
+ vector<int> faceIndices;
+ for (int i = 0; i < 4; i++)
+ for (int j = 0; j < 3; j++)
+ faceIndices.push_back(faces[i][j]);
+ SimTK_TEST_MUST_THROW(
+ ContactGeometry::TriangleMesh mesh(vertices, faceIndices));
+}
+
+void testIncorrectMeshes() {
+ vector<Vec3> vertices;
+ vertices.push_back(Vec3(0, 0, 0));
+ vertices.push_back(Vec3(1, 0, 0));
+ vertices.push_back(Vec3(0, 1, 0));
+ vertices.push_back(Vec3(0, 0, 1));
+ {
+ // The last face has its vertices ordered incorrectly.
+
+ int faces[4][3] = {{0, 1, 2}, {0, 2, 3}, {0, 3, 1}, {1, 2, 3}};
+ verifyMeshThrowsException(vertices, faces, 4);
+ }
+ {
+ // The last face repeats a vertex.
+
+ int faces[4][3] = {{0, 1, 2}, {0, 2, 3}, {0, 3, 1}, {1, 3, 3}};
+ verifyMeshThrowsException(vertices, faces, 4);
+ }
+ {
+ // The surface is not closed.
+
+ int faces[3][3] = {{0, 1, 2}, {0, 2, 3}, {0, 3, 1}};
+ verifyMeshThrowsException(vertices, faces, 3);
+ }
+ {
+ // The last face a vertex that is out of range.
+
+ int faces[4][3] = {{0, 1, 2}, {0, 2, 3}, {0, 3, 1}, {1, 2, 4}};
+ verifyMeshThrowsException(vertices, faces, 4);
+ }
+}
+
+void addOctohedron(vector<Vec3>& vertices, vector<int>& faceIndices, Vec3 offset) {
+ int start = (int)vertices.size();
+ vertices.push_back(Vec3(0, 1, 0)+offset);
+ vertices.push_back(Vec3(1, 0, 0)+offset);
+ vertices.push_back(Vec3(0, 0, 1)+offset);
+ vertices.push_back(Vec3(-1, 0, 0)+offset);
+ vertices.push_back(Vec3(0, 0, -1)+offset);
+ vertices.push_back(Vec3(0, -1, 0)+offset);
+ int faces[8][3] = {{0, 2, 1}, {0, 3, 2}, {0, 4, 3}, {0, 1, 4}, {5, 1, 2}, {5, 2, 3}, {5, 3, 4}, {5, 4, 1}};
+ for (int i = 0; i < 8; i++)
+ for (int j = 0; j < 3; j++)
+ faceIndices.push_back(faces[i][j]+start);
+}
+
+void validateOBBTree(const ContactGeometry::TriangleMesh& mesh, ContactGeometry::TriangleMesh::OBBTreeNode node, ContactGeometry::TriangleMesh::OBBTreeNode parent, vector<int>& faceReferenceCount) {
+ if (node.isLeafNode()) {
+ const Array_<int>& triangles = node.getTriangles();
+ SimTK_TEST(triangles.size() > 0);
+ SimTK_TEST(triangles.size() == node.getNumTriangles());
+ for (int i = 0; i < (int) triangles.size(); i++) {
+ faceReferenceCount[triangles[i]]++;
+ for (int j = 0; j < 3; j++) {
+ SimTK_TEST(node.getBounds().containsPoint(mesh.getVertexPosition(mesh.getFaceVertex(triangles[i], j))));
+ SimTK_TEST(parent.getBounds().containsPoint(mesh.getVertexPosition(mesh.getFaceVertex(triangles[i], j))));
+ }
+ }
+ SimTK_TEST_MUST_THROW(node.getFirstChildNode());
+ SimTK_TEST_MUST_THROW(node.getFirstChildNode());
+ }
+ else {
+ SimTK_TEST_MUST_THROW(node.getTriangles());
+ validateOBBTree(mesh, node.getFirstChildNode(), node, faceReferenceCount);
+ validateOBBTree(mesh, node.getSecondChildNode(), node, faceReferenceCount);
+ SimTK_TEST(node.getNumTriangles() == node.getFirstChildNode().getNumTriangles()+node.getSecondChildNode().getNumTriangles());
+ }
+}
+
+void testOBBTree() {
+ // Create a mesh consisting of a bunch of octohedra.
+
+ vector<Vec3> vertices;
+ vector<int> faceIndices;
+ addOctohedron(vertices, faceIndices, Vec3(0, 0, 0));
+ addOctohedron(vertices, faceIndices, Vec3(2.5, 0, 0));
+ addOctohedron(vertices, faceIndices, Vec3(0, 2.5, 0));
+ addOctohedron(vertices, faceIndices, Vec3(2.5, 2.5, 0));
+ addOctohedron(vertices, faceIndices, Vec3(0, 0, 2.5));
+ addOctohedron(vertices, faceIndices, Vec3(2.5, 0, 2.5));
+ addOctohedron(vertices, faceIndices, Vec3(0, 2.5, 2.5));
+ addOctohedron(vertices, faceIndices, Vec3(2.5, 2.5, 2.5));
+ addOctohedron(vertices, faceIndices, Vec3(1.25, 1.25, 1.25));
+ ContactGeometry::TriangleMesh mesh(vertices, faceIndices);
+
+ // Validate the OBBTree.
+
+ vector<int> faceReferenceCount(mesh.getNumFaces(), 0);
+ validateOBBTree(mesh, mesh.getOBBTreeNode(), mesh.getOBBTreeNode(), faceReferenceCount);
+ for (int i = 0; i < (int) faceReferenceCount.size(); i++)
+ SimTK_TEST(faceReferenceCount[i] == 1);
+}
+
+void testRayIntersection() {
+ // Create an octrohedral mesh.
+
+ vector<Vec3> vertices;
+ vector<int> faceIndices;
+ addOctohedron(vertices, faceIndices, Vec3(0, 0, 0));
+ ContactGeometry::TriangleMesh mesh(vertices, faceIndices);
+
+ // Check various rays.
+
+ Real distance;
+ UnitVec3 normal;
+ SimTK_TEST(!mesh.intersectsRay(Vec3(2, 0, 0), UnitVec3(1, 0, 0), distance, normal));
+ SimTK_TEST(mesh.intersectsRay(Vec3(2, 0, 0), UnitVec3(-1, 0, 0), distance, normal));
+ SimTK_TEST_EQ(1.0, distance);
+ SimTK_TEST(mesh.intersectsRay(Vec3(0, 0, 0), UnitVec3(1, 1, 1), distance, normal));
+ SimTK_TEST_EQ(1.0/Sqrt3, distance);
+ SimTK_TEST_EQ(normal, Vec3(1, 1, 1)/Sqrt3);
+ SimTK_TEST(mesh.intersectsRay(Vec3(0.1, 0.1, 0.1), UnitVec3(-1, -1, -1), distance, normal));
+ SimTK_TEST_EQ(std::sqrt(3*0.1*0.1)+1.0/Sqrt3, distance);
+ SimTK_TEST_EQ(normal, Vec3(-1, -1, -1)/Sqrt3);
+ SimTK_TEST(!mesh.intersectsRay(Vec3(-1, -1, -1), UnitVec3(-1, -1, -1), distance, normal));
+}
+
+void testSmoothMesh() {
+ // Create two octrohedral meshes: one smooth and one not.
+
+ vector<Vec3> vertices;
+ vector<int> faceIndices;
+ addOctohedron(vertices, faceIndices, Vec3(0, 0, 0));
+ ContactGeometry::TriangleMesh mesh1(vertices, faceIndices);
+ ContactGeometry::TriangleMesh mesh2(vertices, faceIndices, true);
+
+ // At the center of every face, the normals should be identical.
+
+ for (int i = 0; i < 8; i++)
+ SimTK_TEST_EQ(mesh1.findNormalAtPoint(i, Vec2(1.0/3.0, 1.0/3.0)), mesh2.findNormalAtPoint(i, Vec2(1.0/3.0, 1.0/3.0)));
+
+ // At the vertices, the smooth mesh normal should point directly outward, while the faceted mesh should
+ // be the same as the face normal.
+
+ for (int i = 0; i < 8; i++) {
+ SimTK_TEST_EQ(mesh1.findNormalAtPoint(i, Vec2(1, 0)), mesh1.getFaceNormal(i));
+ SimTK_TEST_EQ(mesh1.findNormalAtPoint(i, Vec2(0, 1)), mesh1.getFaceNormal(i));
+ SimTK_TEST_EQ(mesh1.findNormalAtPoint(i, Vec2(0, 0)), mesh1.getFaceNormal(i));
+ SimTK_TEST_EQ(mesh2.findNormalAtPoint(i, Vec2(1, 0)), mesh2.getVertexPosition(mesh2.getFaceVertex(i, 0)));
+ SimTK_TEST_EQ(mesh2.findNormalAtPoint(i, Vec2(0, 1)), mesh2.getVertexPosition(mesh2.getFaceVertex(i, 1)));
+ SimTK_TEST_EQ(mesh2.findNormalAtPoint(i, Vec2(0, 0)), mesh2.getVertexPosition(mesh2.getFaceVertex(i, 2)));
+ }
+}
+
+void testFindNearestPoint() {
+ // Create an octrohedral mesh.
+
+ vector<Vec3> vertices;
+ vector<int> faceIndices;
+ addOctohedron(vertices, faceIndices, Vec3(0, 0, 0));
+ ContactGeometry::TriangleMesh mesh(vertices, faceIndices);
+
+ // Test some points.
+
+ Random::Gaussian random(0, 1);
+ for (int i = 0; i < 100; i++) {
+ Vec3 pos(random.getValue(), random.getValue(), random.getValue());
+ bool inside;
+ UnitVec3 normal;
+ Vec3 nearest = mesh.findNearestPoint(pos, inside, normal);
+ SimTK_TEST(inside == (~pos*normal < 1/Sqrt3));
+ SimTK_TEST_EQ(~nearest*normal, 1/Sqrt3);
+ for (int j = 0; j < 3; j++) {
+ SimTK_TEST(pos[j]*nearest[j] >= 0 || std::abs(nearest[j]) < 100*Eps);
+ SimTK_TEST(pos[j]*normal[j] >= 0);
+ }
+ }
+}
+
+void testBoundingSphere() {
+ Random::Uniform random(0, 10);
+ for (int i = 0; i < 100; i++) {
+ // Create a mesh consisting of a random number of octohedra at random places.
+
+ vector<Vec3> vertices;
+ vector<int> faceIndices;
+ int numOctohedra = random.getIntValue()+1;
+ for (int i = 0; i < numOctohedra; i++)
+ addOctohedron(vertices, faceIndices,
+ Vec3(random.getValue(), random.getValue(), random.getValue()));
+ ContactGeometry::TriangleMesh mesh(vertices, faceIndices);
+
+ // Verify that all points are inside the bounding sphere.
+
+ Vec3 center;
+ Real radius;
+ mesh.getBoundingSphere(center, radius);
+ for (int i = 0; i < mesh.getNumVertices(); i++) {
+ Real dist = (center-mesh.getVertexPosition(i)).norm();
+ SimTK_TEST(dist <= radius);
+ }
+
+ // Make sure the bounding sphere is reasonably compact.
+
+ Vec3 boxRadius = 0.5*mesh.getOBBTreeNode().getBounds().getSize();
+ SimTK_TEST(radius <= boxRadius.norm());
+ }
+}
+
+int main() {
+ SimTK_START_TEST("TestTriangleMesh");
+ SimTK_SUBTEST(testTriangleMesh);
+ SimTK_SUBTEST(testIncorrectMeshes);
+ SimTK_SUBTEST(testOBBTree);
+ SimTK_SUBTEST(testRayIntersection);
+ SimTK_SUBTEST(testSmoothMesh);
+ SimTK_SUBTEST(testFindNearestPoint);
+ SimTK_SUBTEST(testBoundingSphere);
+ SimTK_END_TEST();
+}
diff --git a/SimTKmath/tests/TimeStepperTest.cpp b/SimTKmath/tests/TimeStepperTest.cpp
new file mode 100644
index 0000000..5dc2eca
--- /dev/null
+++ b/SimTKmath/tests/TimeStepperTest.cpp
@@ -0,0 +1,177 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "PendulumSystem.h"
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+using namespace SimTK;
+
+using std::printf;
+using std::cout;
+using std::endl;
+
+class ZeroVelocityHandler : public TriggeredEventHandler {
+public:
+ static int eventCount;
+ static Real lastEventTime;
+ ZeroVelocityHandler(PendulumSystem& pendulum) : TriggeredEventHandler(Stage::Velocity), pendulum(pendulum) {
+ getTriggerInfo().setTriggerOnFallingSignTransition(false);
+ }
+ Real getValue(const State& state) const {
+ return state.getU(pendulum.getGuts().getSubsysIndex())[0];
+ }
+ void handleEvent(State& state, Real accuracy, bool& shouldTerminate) const {
+
+ // This should be triggered when the pendulum reaches its farthest point in the
+ // negative direction: q == -1, u == 0.
+
+ Real q = state.getQ(pendulum.getGuts().getSubsysIndex())[0];
+ Real u = state.getU(pendulum.getGuts().getSubsysIndex())[0];
+ ASSERT(std::abs(q+1.0) < 0.05);
+ ASSERT(std::abs(u) < 0.01);
+ ASSERT(state.getTime() > lastEventTime);
+ eventCount++;
+ lastEventTime = state.getTime();
+ }
+private:
+ PendulumSystem& pendulum;
+};
+
+class PeriodicHandler : public ScheduledEventHandler {
+public:
+ static int eventCount;
+ static Real lastEventTime;
+ Real getNextEventTime(const State&, bool includeCurrentTime) const {
+ return lastEventTime+1.5;
+ }
+ void handleEvent(State& state, Real accuracy, bool& shouldTerminate) const {
+
+ // This should be triggered every 1.5 time units.
+
+ ASSERT(state.getTime() == lastEventTime+1.5);
+ eventCount++;
+ lastEventTime = state.getTime();
+ }
+};
+
+class ZeroPositionReporter : public TriggeredEventReporter {
+public:
+ static int eventCount;
+ static Real lastEventTime;
+ ZeroPositionReporter(PendulumSystem& pendulum) : TriggeredEventReporter(Stage::Velocity), pendulum(pendulum) {
+ }
+ Real getValue(const State& state) const {
+ return state.getQ(pendulum.getGuts().getSubsysIndex())[0];
+ }
+ void handleEvent(const State& state) const {
+
+ // This should be triggered when the pendulum crosses q == 0.
+
+ Real q = state.getQ(pendulum.getGuts().getSubsysIndex())[0];
+ ASSERT(std::abs(q) < 0.01);
+ ASSERT(state.getTime() > lastEventTime);
+ eventCount++;
+ lastEventTime = state.getTime();
+ }
+private:
+ PendulumSystem& pendulum;
+};
+
+class PeriodicReporter : public ScheduledEventReporter {
+public:
+ static int eventCount;
+ static Real lastEventTime;
+ Real getNextEventTime(const State&, bool includeCurrentTime) const {
+ return lastEventTime*2;
+ }
+ void handleEvent(const State& state) const {
+
+ // This should be triggered every 1.5 time units.
+
+ ASSERT(state.getTime() == lastEventTime*2);
+ eventCount++;
+ lastEventTime = state.getTime();
+ }
+};
+
+int ZeroVelocityHandler::eventCount = 0;
+Real ZeroVelocityHandler::lastEventTime = 0.0;
+int PeriodicHandler::eventCount = 0;
+Real PeriodicHandler::lastEventTime = 0.0;
+int ZeroPositionReporter::eventCount = 0;
+Real ZeroPositionReporter::lastEventTime = 0.0;
+int PeriodicReporter::eventCount = 0;
+Real PeriodicReporter::lastEventTime = 0.5;
+
+int main () {
+ try {
+ PendulumSystem sys;
+ sys.addEventHandler(new ZeroVelocityHandler(sys));
+ sys.addEventHandler(new PeriodicHandler());
+ sys.addEventReporter(new ZeroPositionReporter(sys));
+ sys.addEventReporter(new PeriodicReporter());
+ sys.realizeTopology();
+
+ RungeKuttaMersonIntegrator integ(sys);
+
+ const Real t0=0;
+ const Real qi[] = {1,0}; // (x,y)=(1,0)
+ const Real ui[] = {0,0}; // v=0
+ const Vector q0(2, qi);
+ const Vector u0(2, ui);
+
+ sys.setDefaultMass(10);
+ sys.setDefaultTimeAndState(t0, q0, u0);
+
+ integ.setAccuracy(1e-2);
+ integ.setConstraintTolerance(1e-4);
+
+ const Real tFinal = 20.003;
+ const Real hReport = 1.;
+
+ integ.setFinalTime(tFinal);
+
+ TimeStepper ts(sys);
+ ts.setIntegrator(integ);
+ ts.initialize(sys.getDefaultState());
+ ASSERT(ts.getTime() == 0.0);
+ ts.stepTo(10.0);
+ ASSERT(ts.getTime() == 10.0);
+ ts.stepTo(50.0);
+ ASSERT(ts.getTime() == tFinal);
+ ASSERT(integ.getTerminationReason() == Integrator::ReachedFinalTime);
+ ASSERT(ZeroVelocityHandler::eventCount >= 10);
+ ASSERT(PeriodicHandler::eventCount == (int) (ts.getTime()/1.5));
+ ASSERT(ZeroPositionReporter::eventCount > 10);
+ ASSERT(PeriodicReporter::eventCount == (int) (std::log(ts.getTime())/std::log(2.0))+1);
+ cout << "Done" << endl;
+ return 0;
+ }
+ catch (std::exception& e) {
+ std::printf("FAILED: %s\n", e.what());
+ return 1;
+ }
+}
diff --git a/SimTKmath/tests/VerletIntegratorTest.cpp b/SimTKmath/tests/VerletIntegratorTest.cpp
new file mode 100644
index 0000000..821ea08
--- /dev/null
+++ b/SimTKmath/tests/VerletIntegratorTest.cpp
@@ -0,0 +1,59 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): SimTKmath *
+ * -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "IntegratorTestFramework.h"
+#include "simmath/VerletIntegrator.h"
+
+int main () {
+ try {
+ PendulumSystem sys;
+ sys.addEventHandler(new ZeroVelocityHandler(sys));
+ sys.addEventHandler(PeriodicHandler::handler = new PeriodicHandler());
+ sys.addEventHandler(new ZeroPositionHandler(sys));
+ sys.addEventReporter(PeriodicReporter::reporter = new PeriodicReporter(sys));
+ sys.addEventReporter(new OnceOnlyEventReporter());
+ sys.addEventReporter(new DiscontinuousReporter());
+ sys.realizeTopology();
+
+ // Test with various intervals for the event handler and event reporter, ones that are either
+ // large or small compared to the expected internal step size of the integrator.
+
+ for (int i = 0; i < 4; ++i) {
+ PeriodicHandler::handler->setEventInterval(i == 0 || i == 1 ? 0.01 : 2.0);
+ PeriodicReporter::reporter->setEventInterval(i == 0 || i == 2 ? 0.015 : 1.5);
+
+ // Test the integrator in both normal and single step modes.
+
+ VerletIntegrator integ(sys);
+ testIntegrator(integ, sys);
+ integ.setReturnEveryInternalStep(true);
+ testIntegrator(integ, sys);
+ }
+ cout << "Done" << endl;
+ return 0;
+ }
+ catch (std::exception& e) {
+ std::printf("FAILED: %s\n", e.what());
+ return 1;
+ }
+}
diff --git a/SimTKmath/tests/adhoc/CMakeLists.txt b/SimTKmath/tests/adhoc/CMakeLists.txt
new file mode 100644
index 0000000..c0ba61d
--- /dev/null
+++ b/SimTKmath/tests/adhoc/CMakeLists.txt
@@ -0,0 +1,46 @@
+# Generate adhoc tests.
+#
+# This is boilerplate code for generating a set of executables, one per
+# .cpp file in a "tests/adhoc" subdirectory. These are for "loose" test
+# cases, not suitable as regression tests but perhaps useful to the
+# developer or as demos. Unlike the similar boilerplate code in the "tests"
+# directory, this does not generate CMake ADD_TEST commands so these
+# will never run automatically.
+#
+# For IDEs that can deal with PROJECT_LABEL properties (at least
+# Visual Studio) the projects for building each of these adhoc
+# executables will be labeled "Adhoc - TheTestName" if a file
+# TheTestName.cpp is found in this directory.
+#
+# We check the BUILD_TESTS_AND_EXAMPLES_{SHARED,STATIC} variables to determine
+# whether to build dynamically linked, statically linked, or both
+# versions of the executable.
+
+
+FILE(GLOB ADHOC_TESTS "*.cpp")
+FOREACH(TEST_PROG ${ADHOC_TESTS})
+ GET_FILENAME_COMPONENT(TEST_ROOT ${TEST_PROG} NAME_WE)
+
+ IF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+ # Link with shared library
+ ADD_EXECUTABLE(${TEST_ROOT} ${TEST_PROG})
+ SET_TARGET_PROPERTIES(${TEST_ROOT}
+ PROPERTIES
+ PROJECT_LABEL "Test_Adhoc - ${TEST_ROOT}")
+ TARGET_LINK_LIBRARIES(${TEST_ROOT}
+ ${TEST_SHARED_TARGET})
+ ENDIF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+
+ IF (BUILD_STATIC_LIBRARIES AND BUILD_TESTS_AND_EXAMPLES_STATIC)
+ # Link with static library
+ SET(TEST_STATIC ${TEST_ROOT}Static)
+ ADD_EXECUTABLE(${TEST_STATIC} ${TEST_PROG})
+ SET_TARGET_PROPERTIES(${TEST_STATIC}
+ PROPERTIES
+ COMPILE_FLAGS "-DSimTK_USE_STATIC_LIBRARIES"
+ PROJECT_LABEL "Test_Adhoc - ${TEST_STATIC}")
+ TARGET_LINK_LIBRARIES(${TEST_STATIC}
+ ${TEST_STATIC_TARGET})
+ ENDIF()
+
+ENDFOREACH(TEST_PROG ${ADHOC_TESTS})
diff --git a/SimTKmath/tests/adhoc/nlpqlp.cpp b/SimTKmath/tests/adhoc/nlpqlp.cpp
new file mode 100644
index 0000000..1989799
--- /dev/null
+++ b/SimTKmath/tests/adhoc/nlpqlp.cpp
@@ -0,0 +1,30286 @@
+
+
+#include "SimTKmath.h"
+using SimTK::Real;
+using SimTK::Vector;
+using SimTK::Matrix;
+using SimTK::isNaN;
+using SimTK::isFinite;
+using SimTK::Optimizer;
+using SimTK::OptimizerSystem;
+
+#include <cstdlib>
+#include <cassert>
+#include <cstdio>
+#include <cmath>
+#include <limits>
+
+#include "nlpqlp.h"
+
+static int pow_ii(int *ap, int *bp) {
+ int pow, x, n;
+
+ pow = 1;
+ x = *ap;
+ n = *bp;
+
+ if(n < 0)
+ { }
+ else if(n > 0)
+ for( ; ; )
+ {
+ if(n & 01)
+ pow *= x;
+ if(n >>= 1)
+ x *= x;
+ else
+ break;
+ }
+ return(pow);
+}
+
+static const Real log10e = 0.43429448190325182765;
+Real d_lg10(Real *x)
+{
+
+return( log10e * log(*x) );
+}
+static Real d_sign(Real *a, Real *b)
+{
+ Real x;
+ x = (*a >= 0 ? *a : - *a);
+ return( *b >= 0 ? x : -x);
+}
+static Real pow_di(Real *ap, int *bp)
+{
+Real pow, x;
+int n;
+
+pow = 1;
+x = *ap;
+n = *bp;
+
+if(n != 0)
+ {
+ if(n < 0)
+ {
+ n = -n;
+ x = 1/x;
+ }
+ for( ; ; )
+ {
+ if(n & 01)
+ pow *= x;
+ if(n >>= 1)
+ x *= x;
+ else
+ break;
+ }
+ }
+return(pow);
+}
+
+int (*current_test)(int*);
+
+typedef struct l20_values {
+ bool lex;
+ int nex;
+ Real fex, *xex;
+}*L20_vals;
+
+typedef struct l1_values {
+ int n,nili,ninl,neli,nenl;
+}*L1_vals;
+
+int tp1_(int*),tp2_(int*),tp3_(int*),tp4_(int*),tp5_(int*),tp6_(int*),tp7_(int*),tp8_(int*),tp9_(int*),tp10_(int*);
+int tp11_(int*),tp12_(int*),tp13_(int*),tp14_(int*),tp15_(int*),tp16_(int*),tp17_(int*),tp18_(int*),tp19_(int*),tp20_(int*);
+int tp21_(int*),tp22_(int*),tp23_(int*),tp24_(int*),tp25_(int*),tp26_(int*),tp27_(int*),tp28_(int*),tp29_(int*),tp30_(int*);
+int tp31_(int*),tp32_(int*),tp33_(int*),tp34_(int*),tp35_(int*),tp36_(int*),tp37_(int*),tp38_(int*),tp39_(int*),tp40_(int*);
+int tp41_(int*),tp42_(int*),tp43_(int*),tp44_(int*),tp45_(int*),tp46_(int*),tp47_(int*),tp48_(int*),tp49_(int*),tp50_(int*);
+int tp51_(int*),tp52_(int*),tp53_(int*),tp54_(int*),tp55_(int*),tp56_(int*),tp57_(int*),tp58_(int*),tp59_(int*),tp60_(int*);
+int tp61_(int*),tp62_(int*),tp63_(int*),tp64_(int*),tp65_(int*),tp66_(int*),tp67_(int*),tp68_(int*),tp69_(int*),tp70_(int*);
+int tp71_(int*),tp72_(int*),tp73_(int*),tp74_(int*),tp75_(int*),tp76_(int*),tp77_(int*),tp78_(int*),tp79_(int*),tp80_(int*);
+int tp81_(int*),tp82_(int*),tp83_(int*),tp84_(int*),tp85_(int*),tp86_(int*),tp87_(int*),tp88_(int*),tp89_(int*),tp90_(int*);
+int tp91_(int*),tp92_(int*),tp93_(int*),tp94_(int*),tp95_(int*),tp96_(int*),tp97_(int*),tp98_(int*),tp99_(int*),tp100_(int*);
+int tp101_(int*),tp102_(int*),tp103_(int*),tp104_(int*),tp105_(int*),tp106_(int*),tp107_(int*),tp108_(int*),tp109_(int*),tp110_(int*);
+int tp111_(int*),tp112_(int*),tp113_(int*),tp114_(int*), tp116_(int*),tp117_(int*),tp118_(int*),tp119_(int*);
+int tp201_(int*),tp202_(int*),tp203_(int*),tp204_(int*),tp205_(int*),tp206_(int*),tp207_(int*),tp208_(int*),tp209_(int*),tp210_(int*);
+int tp211_(int*),tp212_(int*),tp213_(int*),tp214_(int*),tp215_(int*),tp216_(int*),tp217_(int*),tp218_(int*),tp219_(int*),tp220_(int*);
+int tp221_(int*),tp222_(int*),tp223_(int*),tp224_(int*),tp225_(int*),tp226_(int*),tp227_(int*),tp228_(int*),tp229_(int*),tp230_(int*);
+int tp231_(int*),tp232_(int*),tp233_(int*),tp234_(int*),tp235_(int*),tp236_(int*),tp237_(int*),tp238_(int*),tp239_(int*),tp240_(int*);
+int tp241_(int*),tp242_(int*),tp243_(int*),tp244_(int*),tp245_(int*),tp246_(int*),tp247_(int*),tp248_(int*),tp249_(int*),tp250_(int*);
+int tp251_(int*),tp252_(int*),tp253_(int*),tp254_(int*),tp255_(int*),tp256_(int*),tp257_(int*),tp258_(int*),tp259_(int*),tp260_(int*);
+int tp261_(int*),tp262_(int*),tp263_(int*),tp264_(int*),tp265_(int*),tp266_(int*),tp267_(int*),tp268_(int*),tp269_(int*),tp270_(int*);
+int tp271_(int*),tp272_(int*),tp273_(int*),tp274_(int*),tp275_(int*),tp276_(int*),tp277_(int*),tp278_(int*),tp279_(int*),tp280_(int*);
+int tp281_(int*),tp282_(int*),tp283_(int*),tp284_(int*),tp285_(int*),tp286_(int*),tp287_(int*),tp288_(int*),tp289_(int*),tp290_(int*);
+int tp291_(int*),tp292_(int*),tp293_(int*),tp294_(int*),tp295_(int*),tp296_(int*),tp297_(int*),tp298_(int*),tp299_(int*),tp300_(int*);
+int tp301_(int*),tp302_(int*),tp303_(int*),tp304_(int*),tp305_(int*),tp306_(int*),tp307_(int*),tp308_(int*),tp309_(int*),tp310_(int*);
+int tp311_(int*),tp312_(int*),tp313_(int*),tp314_(int*),tp315_(int*),tp316_(int*),tp317_(int*),tp318_(int*),tp319_(int*),tp320_(int*);
+int tp321_(int*),tp322_(int*),tp323_(int*),tp324_(int*),tp325_(int*),tp326_(int*),tp327_(int*),tp328_(int*),tp329_(int*),tp330_(int*);
+int tp331_(int*),tp332_(int*),tp333_(int*),tp334_(int*),tp335_(int*),tp336_(int*),tp337_(int*),tp338_(int*),tp339_(int*),tp340_(int*);
+int tp341_(int*),tp342_(int*),tp343_(int*),tp344_(int*),tp345_(int*),tp346_(int*),tp347_(int*),tp348_(int*),tp349_(int*),tp350_(int*);
+int tp351_(int*),tp352_(int*),tp353_(int*),tp354_(int*),tp355_(int*),tp356_(int*),tp357_(int*),tp358_(int*),tp359_(int*),tp360_(int*);
+int tp361_(int*),tp362_(int*),tp363_(int*);
+
+static int (*(tests[]))(int*) = { tp1_, tp2_, tp3_, tp4_, tp5_, tp6_, tp7_, tp8_, tp9_, tp10_,
+ tp11_, tp12_, tp13_, tp14_, tp15_, tp16_, tp17_, tp18_, tp19_, tp20_,
+ tp21_, tp22_, tp23_, tp24_, tp25_, tp26_, tp27_, tp28_, tp29_, tp30_,
+ tp31_, tp32_, tp33_, tp34_, tp35_, tp36_, tp37_, tp38_, tp39_, tp40_,
+ tp41_, tp42_, tp43_, tp44_, tp45_, tp46_, tp47_, tp48_, tp49_, tp50_,
+ tp51_, tp52_, tp53_, tp54_, tp55_, tp56_, tp57_, tp58_, tp59_, tp60_,
+ tp61_, tp62_, tp63_, tp64_, tp65_, tp66_, tp67_, tp68_, tp69_, tp70_,
+ tp71_, tp72_, tp73_, tp74_, tp75_, tp76_, tp77_, tp78_, tp79_, tp80_,
+ tp81_, tp83_, tp84_, tp85_, tp86_, tp87_, tp88_, tp89_, tp90_,
+ tp91_, tp92_, tp93_, tp95_, tp96_, tp97_, tp98_, tp99_, tp100_,
+ tp101_, tp102_, tp103_, tp104_, tp105_, tp106_, tp107_, tp108_, tp109_, tp110_,
+ tp111_, tp112_, tp113_, tp114_, tp116_, tp117_, tp118_, tp119_,
+ tp201_, tp202_, tp203_, tp204_, tp205_, tp206_, tp207_, tp208_, tp209_, tp210_,
+ tp211_, tp212_, tp213_, tp214_, tp215_, tp216_, tp217_, tp218_, tp219_, tp220_,
+ tp221_, tp222_, tp223_, tp224_, tp225_, tp226_, tp227_, tp228_, tp229_, tp230_,
+ tp231_, tp232_, tp233_, tp234_, tp235_, tp236_, tp237_, tp238_, tp239_, tp240_,
+ tp241_, tp242_, tp243_, tp244_, tp245_, tp246_, tp247_, tp248_, tp249_, tp250_,
+ tp251_, tp252_, tp253_, tp254_, tp255_, tp256_, tp257_, tp258_, tp259_, tp260_,
+ tp261_, tp262_, tp263_, tp264_, tp265_, tp266_, tp267_, tp268_, tp269_, tp270_,
+ tp271_, tp272_, tp273_, tp274_, tp275_, tp276_, tp277_, tp278_, tp279_, tp280_,
+ tp281_, tp282_, tp283_, tp284_, tp285_, tp286_, tp287_, tp288_, tp289_, tp290_,
+ tp291_, tp292_, tp293_, tp294_, tp295_, tp296_, tp297_, tp298_, tp299_, tp300_,
+ tp301_, tp302_, tp303_, tp304_, tp305_, tp306_, tp307_, tp308_, tp309_, tp310_,
+ tp311_, tp312_, tp313_, tp314_, tp315_, tp316_, tp317_, tp318_, tp319_, tp320_,
+ tp321_, tp322_, tp323_, tp324_, tp325_, tp326_, tp327_, tp328_, tp329_, tp330_,
+ tp331_, tp332_, tp333_, tp334_, tp335_, tp336_, tp337_, tp338_, tp339_, tp340_,
+ tp341_, tp342_, tp343_, tp344_, tp345_, tp346_, tp347_, tp348_, tp349_, tp350_,
+ tp351_, tp352_, tp353_, tp354_, tp355_, tp356_, tp357_, tp358_, tp359_, tp360_,
+ tp361_, tp362_, tp363_
+ };
+
+static const char *test_names[] = { "tp1_", "tp2_", "tp3_", "tp4_", "tp5_", "tp6_", "tp7_", "tp8_", "tp9_", "tp10_",
+ "tp11_", "tp12_", "tp13_", "tp14_", "tp15_", "tp16_", "tp17_", "tp18_", "tp19_", "tp20_",
+ "tp21_", "tp22_", "tp23_", "tp24_", "tp25_", "tp26_", "tp27_", "tp28_", "tp29_", "tp30_",
+ "tp31_", "tp32_", "tp33_", "tp34_", "tp35_", "tp36_", "tp37_", "tp38_", "tp39_", "tp40_",
+ "tp41_", "tp42_", "tp43_", "tp44_", "tp45_", "tp46_", "tp47_", "tp48_", "tp49_", "tp50_",
+ "tp51_", "tp52_", "tp53_", "tp54_", "tp55_", "tp56_", "tp57_", "tp58_", "tp59_", "tp60_",
+ "tp61_", "tp62_", "tp63_", "tp64_", "tp65_", "tp66_", "tp67_", "tp68_", "tp69_", "tp70_",
+ "tp71_", "tp72_", "tp73_", "tp74_", "tp75_", "tp76_", "tp77_", "tp78_", "tp79_", "tp80_",
+ "tp81_", "tp83_", "tp84_", "tp85_", "tp86_", "tp87_", "tp88_", "tp89_", "tp90_",
+ "tp91_", "tp92_", "tp93_", "tp95_", "tp96_", "tp97_", "tp98_", "tp99_", "tp100_",
+ "tp101_", "tp102_", "tp103_", "tp104_", "tp105_", "tp106_", "tp107_", "tp108_", "tp109_", "tp110_",
+ "tp111_", "tp112_", "tp113_", "tp114_", "tp116_", "tp117_", "tp118_", "tp119_",
+ "tp201_", "tp202_", "tp203_", "tp204_", "tp205_", "tp206_", "tp207_", "tp208_", "tp209_", "tp210_",
+ "tp211_", "tp212_", "tp213_", "tp214_", "tp215_", "tp216_", "tp217_", "tp218_", "tp219_", "tp220_",
+ "tp221_", "tp222_", "tp223_", "tp224_", "tp225_", "tp226_", "tp227_", "tp228_", "tp229_", "tp230_",
+ "tp231_", "tp232_", "tp233_", "tp234_", "tp235_", "tp236_", "tp237_", "tp238_", "tp239_", "tp240_",
+ "tp241_", "tp242_", "tp243_", "tp244_", "tp245_", "tp246_", "tp247_", "tp248_", "tp249_", "tp250_",
+ "tp251_", "tp252_", "tp253_", "tp254_", "tp255_", "tp256_", "tp257_", "tp258_", "tp259_", "tp260_",
+ "tp261_", "tp262_", "tp263_", "tp264_", "tp265_", "tp266_", "tp267_", "tp268_", "tp269_", "tp270_",
+ "tp271_", "tp272_", "tp273_", "tp274_", "tp275_", "tp276_", "tp277_", "tp278_", "tp279_", "tp280_",
+ "tp281_", "tp282_", "tp283_", "tp284_", "tp285_", "tp286_", "tp287_", "tp288_", "tp289_", "tp290_",
+ "tp291_", "tp292_", "tp293_", "tp294_", "tp295_", "tp296_", "tp297_", "tp298_", "tp299_", "tp300_",
+ "tp301_", "tp302_", "tp303_", "tp304_", "tp305_", "tp306_", "tp307_", "tp308_", "tp309_", "tp310_",
+ "tp311_", "tp312_", "tp313_", "tp314_", "tp315_", "tp316_", "tp317_", "tp318_", "tp319_", "tp320_",
+ "tp321_", "tp322_", "tp323_", "tp324_", "tp325_", "tp326_", "tp327_", "tp328_", "tp329_", "tp330_",
+ "tp331_", "tp332_", "tp333_", "tp334_", "tp335_", "tp336_", "tp337_", "tp338_", "tp339_", "tp340_",
+ "tp341_", "tp342_", "tp343_", "tp344_", "tp345_", "tp346_", "tp347_", "tp348_", "tp349_", "tp350_",
+ "tp351_", "tp352_", "tp353_", "tp354_", "tp355_", "tp356_", "tp357_", "tp358_", "tp359_", "tp360_",
+ "tp361_", "tp362_", "tp363_"
+ };
+extern L1 l1_;
+extern L2 l2_;
+extern L3 l3_;
+extern L4 l4_;
+extern L5 l5_;
+extern L6 l6_;
+extern L9 l9_;
+extern L10 l10_;
+extern L11 l11_;
+extern L12 l12_;
+extern L13 l13_;
+extern L14 l14_;
+extern L20 l20_;
+void init_common_blocks() {
+ int *iptr,i;
+ Real *rptr;
+ bool *bptr;
+
+ iptr = (int*)&l1_; for(i=0;i<4;i++) *iptr++= std::numeric_limits<int>::quiet_NaN();
+ rptr = (SimTK::Real*)&l2_; for(i=0;i<100;i++) *rptr++= std::numeric_limits<double>::quiet_NaN();
+ rptr = (SimTK::Real*)&l3_; for(i=0;i<45;i++) *rptr++= std::numeric_limits<double>::quiet_NaN();
+ rptr = (SimTK::Real*)&l4_; for(i=0;i<100;i++) *rptr++= std::numeric_limits<double>::quiet_NaN();
+ rptr = (SimTK::Real*)&l5_; for(i=0;i<435;i++) *rptr++= std::numeric_limits<double>::quiet_NaN();
+ rptr = (SimTK::Real*)&l6_; for(i=0;i<1;i++) *rptr++= std::numeric_limits<double>::quiet_NaN();
+ bptr = (bool*)&l9_; for(i=0;i<45;i++) *bptr++= false;
+ bptr = (bool*)&l10_; for(i=0;i<38;i++) *bptr++= false;
+ bptr = (bool*)&l11_; for(i=0;i<100;i++) *bptr++= false;
+ bptr = (bool*)&l12_; for(i=0;i<100;i++) *bptr++= false;
+ rptr = (SimTK::Real*)&l13_; for(i=0;i<100;i++) *rptr++= std::numeric_limits<double>::quiet_NaN();
+ rptr = (SimTK::Real*)&l14_; for(i=0;i<100;i++) *rptr++= std::numeric_limits<double>::quiet_NaN();
+
+
+ return;
+}
+
+/* Function Implementations */
+class ProblemSystem : public OptimizerSystem {
+public:
+
+ int m;
+
+ int objectiveFunc( const Vector &coefficients, bool new_coefficients, Real& f ) const {
+ int mode = 2;
+ int i;
+
+ Real *nx = (Real *)&l2_;
+ for(i=0;i<getNumParameters();i++) nx[i] = coefficients(i);
+ current_test(&mode);
+
+ f = l6_.fx;
+ if( !isFinite(f) ) {
+ printf(" objective is not finite \n");
+ exit(0);
+ }
+ /*
+ printf("f = %f x=",f);
+ for(i=0;i<numParameters;i++)printf(" %f",coefficients(i)); printf("\n");
+ */
+
+ return (0);
+ }
+
+ int gradientFunc( const Vector &coefficients, bool new_coefficients, Vector &gradient ) const {
+ int i,mode = 3;
+ int fmode = 2;
+
+ Real *nx = (Real *)&l2_;
+ for(i=0;i<getNumParameters();i++) nx[i] = coefficients(i);
+
+ current_test(&fmode); // bug in tp260_ assumes objective has been calculated prior to gradient calc.
+ current_test(&mode);
+
+ Real *g = (Real*)&l4_;
+ for(i=0;i<getNumParameters();i++) {
+ if( !isFinite(g[i]) ) {
+ printf(" gradient is not finite i=%d g[i]=%f\n",i,g[i]);
+ exit(0);
+ }
+ gradient(i) = g[i];
+ }
+
+ return(0);
+ }
+
+ int constraintFunc( const Vector &coefficients, bool new_coefficients, Vector &constraints) const {
+ int i,mode = 4;
+
+ Real *nx = (Real *)&l2_;
+ for(i=0;i<getNumParameters();i++) nx[i] = coefficients[i];
+ current_test(&mode);
+
+ Real *cv = (Real*)&l3_;
+ for(i=0;i<m;i++) {
+ if( !isFinite(cv[i]) ) {
+ printf(" constraint is not finite \n");
+ exit(0);
+ }
+ constraints(i) = cv[i];
+ }
+
+ return(0);
+ }
+
+ int constraintJacobian( const Vector& coefficients, bool new_coefficients, Matrix& jac) const {
+ int i,j;
+ int mode = 5;
+
+
+ Real *nx = (Real *)&l2_;
+ for(i=0;i<getNumParameters();i++) nx[i] = coefficients(i);
+
+ current_test(&mode);
+ Real *jv = (Real*)&l5_;
+ for(j=0;j<getNumEqualityConstraints();j++) {
+ for(i=0;i<getNumParameters();i++) {
+ if( !isFinite(jv[i*m+j]) ) {
+ printf(" jacobian(%d,%d) is not finite index=%d\n",j,i,i*getNumEqualityConstraints()+j);
+ exit(0);
+ }
+ jac(j,i) = jv[i*m+j]; // original
+ // jac(j,i) = jv[i+getNumParameters()*j];
+ }
+ }
+ return (0);
+ }
+
+ ProblemSystem( const int numParams, const int numEqualityConstraints, const int numInEqualityConstraints)
+ : OptimizerSystem( numParams ) {
+ setNumEqualityConstraints( numEqualityConstraints );
+ setNumInequalityConstraints( numInEqualityConstraints );
+
+ m = numEqualityConstraints+ numInEqualityConstraints;
+ }
+
+ ProblemSystem( const int numParams) : OptimizerSystem( numParams ) {}
+
+};
+
+/* Main Program */
+int main()
+{
+ int j, num_tests=0,num_passed=0;
+ bool run_test( int );
+ void init_common_blocks();
+ int failures [] = { 27, 74, 75, 76, 97, 99, 103, 107, 112, 114, 133, 135, 136,
+ 162, 164, 176, 182, 184, 197, 222, 223,
+ 224, 241, 248, 265, 277 };
+ int fail64bit[] = { 76, 99, 103, 136, 164, 184 };
+
+
+ for( j=0;j<279;j++) {
+// for( j=0;j<20;j++) {
+// for( j=83;j<86;j++) {
+// for( l=0;l<26;l++) {
+// for( l=0;l<1;l++) {
+// j = failures[l]-1;
+
+ num_tests++;
+ init_common_blocks();
+ if( run_test( j ) ) num_passed++;
+ }
+
+ printf(" %d PASSED out of %d tests pass rate = %f %%\n",num_passed, num_tests, 100.0*(float)num_passed/(float)num_tests);
+ return 0;
+}
+
+bool run_test(int j ) {
+
+ L20_vals expect;
+ L1_vals ndim;
+
+ Real *initial_values,f;
+ int mode = 1;
+ int n,m,i;
+ bool *index1, *index2;
+
+ current_test = tests[j];
+ current_test(&mode);
+
+ ndim = (L1_vals)&l1_;
+ n = ndim->n;
+ m = ndim->nili + ndim->ninl + ndim->neli + ndim->nenl;
+
+ ProblemSystem sys( n, ndim->neli + ndim->nenl, ndim->nili + ndim->ninl );
+
+ index1 = (bool *)&l9_;
+ for(i=0;i<m;i++) index1[i] = true;
+
+ /* initialize the L10 common block for which constaint gradients are computed for mode 5. */
+ index2 = (bool *)&l10_;
+ for(i=0;i<m;i++) index2[i] = true;
+
+
+ Vector results(n);
+ Vector lower_bounds(n);
+ Vector upper_bounds(n);
+ initial_values = (Real *)&l2_;
+ for(i=0;i<n;i++) {
+ results(i) = initial_values[i];
+ }
+
+ /* set the values for the variable bounds */
+
+ bool *lxl = (bool *)&l11_;
+ bool *lxu = (bool *)&l12_;
+ Real *xl = (Real *)&l13_;
+ Real *xu = (Real *)&l14_;
+ bool has_limits = false;
+ for (i=0; i<n; i++) {
+ if( lxl[i] ) {
+ has_limits = true;
+ lower_bounds(i) = xl[i];
+ } else {
+ lower_bounds(i) = -2e19;
+ }
+ if( lxu[i] ) {
+ has_limits = true;
+ upper_bounds(i) = xu[i];
+ } else {
+ upper_bounds(i) = 2e19;
+ }
+ }
+
+ if( has_limits ) {
+ sys.setParameterLimits( lower_bounds, upper_bounds );
+ }
+ /* set the number of constraints and allocate space for the bounds */
+
+
+ bool converged = true;
+ try {
+
+ Optimizer opt( sys );
+
+// opt.setConvergenceTolerance( .0001 );
+ opt.setConvergenceTolerance( 1e-3 );
+
+// opt.useNumericalGradient( true );
+ opt.useNumericalJacobian( true );
+// opt.setDiagnosticsLevel( 7 );
+
+ // some tests do nor supply gradients
+ if( j == 248-1 ||
+ j == 264-1 ||
+ j == 265-1 ||
+ j == 272-1 ||
+ j == 273-1 ||
+ j == 277-1 ||
+ j == 278-1 ||
+ j == 279-1 ) {
+ opt.useNumericalGradient( true );
+ opt.useNumericalJacobian( true );
+ }
+
+ opt.setDiagnosticsLevel( 0 );
+
+ printf("%d %s ",j+1, test_names[j]);
+ /* compute optimization */
+ f = opt.optimize( results );
+ }
+ catch (const std::exception&) {
+ converged = false;
+ }
+
+ if (converged ) {
+ expect = (L20_vals)&l20_;
+ printf("f(x*) = %e expected = %e diff=%e", f,expect->fex,f - expect->fex);
+ if( fabs(f - expect->fex) < 1.0e-4 ) {
+ printf("\n");
+ } else {
+ printf(" *** %d \n",j+1);
+ }
+ } else {
+ printf("#### Failed to converge #### \n" );
+ }
+
+ return( converged );
+ } /* end of test */
+
+
+/* prob.f -- translated by f2c (version 19991025).
+ You must link the resulting object file with the libraries:
+ -lf2c -lm (in that order)
+*/
+
+L1 l1_;
+L2 l2_;
+L3 l3_;
+L4 l4_;
+L5 l5_;
+L6 l6_;
+L9 l9_;
+L10 l10_;
+L11 l11_;
+L12 l12_;
+L13 l13_;
+L14 l14_;
+L20 l20_;
+
+
+/* Table of constant values */
+
+static Real c_b74 = .33333333333333331;
+static Real c_b305 = 2.;
+static Real c_b306 = -.33333333333333331;
+static Real c_b308 = -.5;
+static Real c_b310 = -.91666666666666663;
+static Real c_b312 = -.25;
+static Real c_b590 = .5;
+static int c_n1 = -1;
+static Real c_b933 = .66666666666666663;
+static Real c_b934 = .25;
+static Real c_b940 = 1.5;
+static Real c_b949 = .75;
+static Real c_b993 = .67;
+static Real c_b997 = -.33;
+static Real c_b998 = -.67;
+static Real c_b1002 = -1.67;
+static Real c_b1008 = -.71;
+static Real c_b1009 = -1.3;
+static Real c_b1015 = -1.71;
+static Real c_b1016 = -2.3;
+static Real c_b1157 = .2;
+static Real c_b1523 = 3.;
+static int c__1 = 1;
+static int c__2 = 2;
+static Real c_b1920 = -.66666666666666663;
+static Real c_b2003 = -1.;
+static Real c_b2368 = .35;
+static Real c_b2380 = .33333333;
+static Real c_b2383 = .782;
+static Real c_b2387 = .546;
+static Real c_b2390 = .3;
+static Real c_b2391 = .467;
+static Real c_b2746 = .666;
+static Real c_b2748 = 1.6;
+static Real c_b2752 = 3.55;
+static Real c_b2753 = 5.66;
+static Real c_b2754 = 1.2;
+static Real c_b2757 = 2.42;
+static Real c_b2758 = 4.17;
+static Real c_b2761 = .803;
+static Real c_b2782 = .334;
+static Real c_b2789 = .6;
+static Real c_b2800 = 4.66;
+static Real c_b2801 = 2.55;
+static Real c_b2813 = 1.42;
+static Real c_b2816 = 3.17;
+static Real c_b2823 = .197;
+static Real c_b3046 = .827;
+static Real c_b3047 = .182;
+static Real c_b3048 = 10.;
+static Real c_b3049 = 3.378;
+static Real c_b3050 = .126;
+static Real c_b3053 = 20.;
+static Real c_b3054 = .656;
+
+/* Initialized data */
+
+static struct {
+ Real e_1[2];
+ Real fill_2[201];
+ int e_3;
+ } b_ = { .3, 7.263e-4, {0}, 98 };
+
+
+/* Subroutine */ int tp1_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = -2.;
+ l2_1.x[1] = 1.;
+ l11_1.lxl[0] = false;
+ l11_1.lxl[1] = true;
+ l12_1.lxu[0] = false;
+ l12_1.lxu[1] = false;
+ l13_1.xl[1] = -1.5;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.xex[0] = 1.;
+ l20_1.xex[1] = 1.;
+ l20_1.fex = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = 1. - l2_1.x[0];
+ l6_1.fx = d__1 * d__1 * 100. + d__3 * d__3;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[1] = (l2_1.x[1] - d__1 * d__1) * 200.;
+ l4_1.gf[0] = (l2_1.x[0] * (l4_1.gf[1] - 1.) + 1.) * -2.;
+labelL4:
+ return 0;
+} /* tp1_ */
+
+
+/* Subroutine */ int tp2_(int *mode)
+{
+ /* System generated locals */
+ Real r__1;
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static Real w1;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = -2.;
+ l2_1.x[1] = 1.;
+ l11_1.lxl[0] = false;
+ l11_1.lxl[1] = true;
+ l12_1.lxu[0] = false;
+ l12_1.lxu[1] = false;
+ l13_1.xl[1] = 1.5;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ w1 = std::sqrt(.49833333333333335);
+/* Computing 3rd power */
+ r__1 = w1;
+ l20_1.xex[0] = w1 * 2. * std::cos(std::acos(.0025 / (r__1 * (r__1 * r__1))) / 3.);
+ l20_1.xex[1] = 1.5;
+/* Computing 2nd power */
+ d__2 = l20_1.xex[0];
+/* Computing 2nd power */
+ d__1 = l20_1.xex[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = 1. - l20_1.xex[0];
+ l20_1.fex = d__1 * d__1 * 100. + d__3 * d__3;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = 1. - l2_1.x[0];
+ l6_1.fx = d__1 * d__1 * 100. + d__3 * d__3;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[1] = (l2_1.x[1] - d__1 * d__1) * 200.;
+ l4_1.gf[0] = (l2_1.x[0] * (l4_1.gf[1] - 1.) + 1.) * -2.;
+labelL4:
+ return 0;
+} /* tp2_ */
+
+
+/* Subroutine */ int tp3_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 10.;
+ l2_1.x[1] = 1.;
+ l11_1.lxl[0] = false;
+ l11_1.lxl[1] = true;
+ l12_1.lxu[0] = false;
+ l12_1.lxu[1] = false;
+ l13_1.xl[1] = 0.;
+ l20_1.lex = true;
+ l20_1.xex[0] = 0.;
+ l20_1.xex[1] = 0.;
+ l20_1.fex = 0.;
+ l20_1.nex = 1;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - l2_1.x[0];
+ l6_1.fx = l2_1.x[1] + d__1 * d__1 * 1e-5;
+ return 0;
+labelL3:
+ l4_1.gf[0] = (l2_1.x[1] - l2_1.x[0]) * -2. * 1e-5;
+ l4_1.gf[1] = 1. - l4_1.gf[0];
+labelL4:
+ return 0;
+} /* tp3_ */
+
+
+/* Subroutine */ int tp4_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 1.125;
+ l2_1.x[1] = .125;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = true;
+ }
+ l13_1.xl[0] = 1.;
+ l13_1.xl[1] = 0.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.xex[0] = 1.;
+ l20_1.xex[1] = 0.;
+ l20_1.fex = 2.6666666666666665;
+ l4_1.gf[1] = 1.;
+ return 0;
+labelL2:
+/* Computing 3rd power */
+ d__1 = l2_1.x[0] + 1.;
+ l6_1.fx = d__1 * (d__1 * d__1) / 3. + l2_1.x[1];
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] + 1.;
+ l4_1.gf[0] = d__1 * d__1;
+labelL4:
+ return 0;
+} /* tp4_ */
+
+
+/* Subroutine */ int tp5_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real a;
+ static int i__;
+ static Real v1, v2;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 0.;
+ l2_1.x[1] = 0.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = true;
+ }
+ l13_1.xl[0] = -1.5;
+ l13_1.xl[1] = -3.;
+ l14_1.xu[0] = 4.;
+ l14_1.xu[1] = 3.;
+ a = std::atan(1.) * 4.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.xex[0] = .5 - a / 3.;
+ l20_1.xex[1] = l20_1.xex[0] - 1.;
+ l20_1.fex = -std::sqrt(3.) / 2. - a / 3.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - l2_1.x[1];
+ l6_1.fx = std::sin(l2_1.x[0] + l2_1.x[1]) + d__1 * d__1 - l2_1.x[0] * 1.5 +
+ l2_1.x[1] * 2.5 + 1.;
+ return 0;
+labelL3:
+ v1 = std::cos(l2_1.x[0] + l2_1.x[1]);
+ v2 = (l2_1.x[0] - l2_1.x[1]) * 2.;
+ l4_1.gf[0] = v1 + v2 - 1.5;
+ l4_1.gf[1] = v1 - v2 + 2.5;
+labelL4:
+ return 0;
+} /* tp5_ */
+
+
+/* Subroutine */ int tp6_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ l2_1.x[0] = -1.2;
+ l2_1.x[1] = 1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.xex[0] = 1.;
+ l20_1.xex[1] = 1.;
+ l20_1.fex = 0.;
+ l5_1.gg[1] = 10.;
+ l4_1.gf[1] = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = 1. - l2_1.x[0];
+ l6_1.fx = d__1 * d__1;
+ return 0;
+labelL3:
+ l4_1.gf[0] = l2_1.x[0] * 2. - 2.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l3_1.g[0] = (l2_1.x[1] - d__1 * d__1) * 10.;
+ }
+ return 0;
+labelL5:
+ if (l10_2.index2[0]) {
+ l5_1.gg[0] = l2_1.x[0] * -20.;
+ }
+ return 0;
+} /* tp6_ */
+
+
+/* Subroutine */ int tp7_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ l2_1.x[0] = 2.;
+ l2_1.x[1] = 2.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.fex = -std::sqrt(3.);
+ l20_1.xex[0] = 0.;
+ l20_1.xex[1] = -l20_1.fex;
+ l20_1.nex = 1;
+ l4_1.gf[1] = -1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l6_1.fx = std::log(d__1 * d__1 + 1.) - l2_1.x[1];
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[0] = l2_1.x[0] * 2. / (d__1 * d__1 + 1.);
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = d__2 * d__2 + 1.;
+/* Computing 2nd power */
+ d__3 = l2_1.x[1];
+ l3_1.g[0] = d__1 * d__1 + d__3 * d__3 - 4.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l5_1.gg[0] = l2_1.x[0] * 4. * (d__1 * d__1 + 1.);
+ l5_1.gg[1] = l2_1.x[1] * 2.;
+L7:
+ return 0;
+} /* tp7_ */
+
+
+/* Subroutine */ int tp8_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static Real a, b;
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL3;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 2;
+ l2_1.x[0] = 2.;
+ l2_1.x[1] = 1.;
+ a = std::sqrt((std::sqrt(301.) + 25.) / 2.);
+ b = std::sqrt((25. - std::sqrt(301.)) / 2.);
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l20_2.lex = true;
+ l20_2.nex = 4;
+ l20_2.xex[0] = a;
+ l20_2.xex[1] = 9. / a;
+ l20_2.xex[4] = b;
+ l20_2.xex[5] = 9. / b;
+ for (i__ = 3; i__ <= 7; i__ += 4) {
+ for (j = 1; j <= 2; ++j) {
+/* L30: */
+ l20_2.xex[i__ + j - 2] = -l20_2.xex[i__ + j - 4];
+ }
+ }
+ l20_2.fex = -1.;
+ l4_1.gf[0] = 0.;
+ l4_1.gf[1] = 0.;
+ l6_1.fx = -1.;
+labelL3:
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_2.g[0] = d__1 * d__1 + d__2 * d__2 - 25.;
+ }
+ if (l9_3.index1[1]) {
+ l3_2.g[1] = l2_1.x[0] * l2_1.x[1] - 9.;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+ l5_2.gg[0] = l2_1.x[0] * 2.;
+ l5_2.gg[2] = l2_1.x[1] * 2.;
+L7:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+ l5_2.gg[1] = l2_1.x[1];
+ l5_2.gg[3] = l2_1.x[0];
+L8:
+ return 0;
+} /* tp8_ */
+
+
+/* Subroutine */ int tp9_(int *mode)
+{
+ /* Local variables */
+ static int i__;
+ static Real v, v1, v2, v3, v4;
+
+ v = std::atan(1.) * 4.;
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 1;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 0.;
+ l2_1.x[1] = 0.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.nex = -1;
+ l20_1.fex = -.5;
+ l20_1.xex[0] = -3.;
+ l20_1.xex[1] = -4.;
+ l5_1.gg[0] = 4.;
+ l5_1.gg[1] = -3.;
+ return 0;
+labelL2:
+ l6_1.fx = std::sin(v * l2_1.x[0] / 12.) * std::cos(v * l2_1.x[1] / 16.);
+ return 0;
+labelL3:
+ v3 = v / 12.;
+ v4 = v / 16.;
+ v1 = v3 * l2_1.x[0];
+ v2 = v4 * l2_1.x[1];
+ l4_1.gf[0] = v3 * std::cos(v1) * std::cos(v2);
+ l4_1.gf[1] = -v4 * std::sin(v1) * std:: sin(v2);
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = l2_1.x[0] * 4. - l2_1.x[1] * 3.;
+ }
+labelL5:
+ return 0;
+} /* tp9_ */
+
+
+/* Subroutine */ int tp10_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = -10.;
+ l2_1.x[1] = 10.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.xex[0] = 0.;
+ l20_1.xex[1] = 1.;
+ l20_1.fex = -1.;
+ l4_1.gf[0] = 1.;
+ l4_1.gf[1] = -1.;
+ return 0;
+labelL2:
+ l6_1.fx = l2_1.x[0] - l2_1.x[1];
+labelL3:
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_1.g[0] = d__1 * d__1 * -3. + l2_1.x[0] * 2. * l2_1.x[1] - d__2 *
+ d__2 + 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+ l5_1.gg[0] = l2_1.x[0] * -6. + l2_1.x[1] * 2.;
+ l5_1.gg[1] = (l2_1.x[0] - l2_1.x[1]) * 2.;
+L7:
+ return 0;
+} /* tp10_ */
+
+
+/* Subroutine */ int tp11_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+ static Real aw, aex, qaw;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l2_1.x[0] = 4.9;
+ l2_1.x[1] = .1;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ aex = std::sqrt(6.) * 7.5;
+/* Computing 2nd power */
+ d__2 = aex;
+ d__1 = std::sqrt(d__2 * d__2 + 1.) + aex;
+ aw = pow_dd(&d__1, &c_b74);
+/* Computing 2nd power */
+ d__1 = aw;
+ qaw = d__1 * d__1;
+ l20_1.xex[0] = (aw - 1. / aw) / std::sqrt(6.);
+ l20_1.xex[1] = (qaw - 2. + 1. / qaw) / 6.;
+/* Computing 2nd power */
+ d__1 = l20_1.xex[0] - 5.;
+/* Computing 2nd power */
+ d__2 = l20_1.xex[1];
+ l20_1.fex = d__1 * d__1 + d__2 * d__2 - 25.;
+ l5_1.gg[1] = 1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 5.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 - 25.;
+ return 0;
+labelL3:
+ l4_1.gf[0] = (l2_1.x[0] - 5.) * 2.;
+ l4_1.gf[1] = l2_1.x[1] * 2.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l3_1.g[0] = -(d__1 * d__1) + l2_1.x[1];
+ }
+ return 0;
+labelL5:
+ if (l10_2.index2[0]) {
+ l5_1.gg[0] = l2_1.x[0] * -2.;
+ }
+ return 0;
+} /* tp11_ */
+
+
+/* Subroutine */ int tp12_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 0.;
+ l2_1.x[1] = 0.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.xex[0] = 2.;
+ l20_1.xex[1] = 3.;
+ l20_1.fex = -30.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l6_1.fx = d__1 * d__1 * .5 + d__2 * d__2 - l2_1.x[0] * l2_1.x[1] - l2_1.x[
+ 0] * 7. - l2_1.x[1] * 7.;
+ return 0;
+labelL3:
+ l4_1.gf[0] = l2_1.x[0] - l2_1.x[1] - 7.;
+ l4_1.gf[1] = l2_1.x[1] * 2. - l2_1.x[0] - 7.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_1.g[0] = 25. - d__1 * d__1 * 4. - d__2 * d__2;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+ l5_1.gg[0] = l2_1.x[0] * -8.;
+ l5_1.gg[1] = l2_1.x[1] * -2.;
+L7:
+ return 0;
+} /* tp12_ */
+
+
+/* Subroutine */ int tp13_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = -2.;
+ l2_1.x[1] = -2.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+ l11_1.lxl[i__ - 1] = true;
+/* labelL6: */
+ l13_1.xl[i__ - 1] = 0.;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.xex[0] = 1.;
+ l20_1.xex[1] = 0.;
+ l20_1.fex = 1.;
+ l5_1.gg[1] = -1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 2.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ return 0;
+labelL3:
+ l4_1.gf[0] = (l2_1.x[0] - 2.) * 2.;
+ l4_1.gf[1] = l2_1.x[1] * 2.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 3rd power */
+ d__1 = 1. - l2_1.x[0];
+ l3_1.g[0] = d__1 * (d__1 * d__1) - l2_1.x[1];
+ }
+ return 0;
+labelL5:
+ if (l10_2.index2[0]) {
+/* Computing 2nd power */
+ d__1 = 1. - l2_1.x[0];
+ l5_1.gg[0] = d__1 * d__1 * -3.;
+ }
+ return 0;
+} /* tp13_ */
+
+
+/* Subroutine */ int tp14_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+ static Real w7;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 1;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 2.;
+ l2_1.x[1] = 2.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ w7 = std::sqrt(7.);
+ l20_1.xex[0] = (w7 - 1.) * .5;
+ l20_1.xex[1] = (w7 + 1.) * .25;
+ l20_1.fex = 9. - w7 * 23. / 8.;
+ l5_2.gg[1] = 1.;
+ l5_2.gg[3] = -2.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 2.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1] - 1.;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ return 0;
+labelL3:
+ l4_1.gf[0] = (l2_1.x[0] - 2.) * 2.;
+ l4_1.gf[1] = (l2_1.x[1] - 1.) * 2.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_2.g[0] = 1. - d__1 * d__1 * .25 - d__2 * d__2;
+ }
+ if (l9_3.index1[1]) {
+ l3_2.g[1] = l2_1.x[0] - l2_1.x[1] * 2. + 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+ l5_2.gg[0] = -l2_1.x[0] * .5;
+ l5_2.gg[2] = l2_1.x[1] * -2.;
+L7:
+ return 0;
+} /* tp14_ */
+
+
+/* Subroutine */ int tp15_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = -2.;
+ l2_1.x[1] = 1.;
+ l11_1.lxl[0] = false;
+ l11_1.lxl[1] = false;
+ l12_1.lxu[0] = true;
+ l12_1.lxu[1] = false;
+ l14_1.xu[0] = .5;
+ l20_1.lex = true;
+ l20_1.xex[0] = .5;
+ l20_1.xex[1] = (float)2.;
+ l20_1.fex = (float)3.065;
+ l20_1.nex = 1;
+ l5_2.gg[1] = 1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = 1. - l2_1.x[0];
+ l6_1.fx = d__1 * d__1 + d__3 * d__3 * (float).01;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[1] = (l2_1.x[1] - d__1 * d__1) * (float)2.;
+ l4_1.gf[0] = (l2_1.x[0] * (l4_1.gf[1] - 1.) + 1.) * -.02;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = l2_1.x[0] * l2_1.x[1] - 1.;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+ l3_2.g[1] = d__1 * d__1 + l2_1.x[0];
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+ l5_2.gg[0] = l2_1.x[1];
+ l5_2.gg[2] = l2_1.x[0];
+L7:
+ if (l10_3.index2[1]) {
+ l5_2.gg[3] = l2_1.x[1] * 2.;
+ }
+ return 0;
+} /* tp15_ */
+
+
+/* Subroutine */ int tp16_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = -2.;
+ l2_1.x[1] = 1.;
+ l11_1.lxl[0] = true;
+ l11_1.lxl[1] = false;
+ l12_1.lxu[0] = true;
+ l12_1.lxu[1] = true;
+ l13_1.xl[0] = -2.;
+ l14_1.xu[0] = .5;
+ l14_1.xu[1] = 1.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.xex[0] = .5;
+ l20_1.xex[1] = .25;
+ l20_1.fex = .25;
+ l5_2.gg[0] = 1.;
+ l5_2.gg[3] = 1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = 1. - l2_1.x[0];
+ l6_1.fx = d__1 * d__1 * 100. + d__3 * d__3;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[1] = (l2_1.x[1] - d__1 * d__1) * 200.;
+ l4_1.gf[0] = (l2_1.x[0] * (l4_1.gf[1] - 1.) + 1.) * -2.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+ l3_2.g[0] = d__1 * d__1 + l2_1.x[0];
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l3_2.g[1] = d__1 * d__1 + l2_1.x[1];
+ }
+ return 0;
+labelL5:
+ if (l10_3.index2[0]) {
+ l5_2.gg[2] = l2_1.x[1] * 2.;
+ }
+ if (l10_3.index2[1]) {
+ l5_2.gg[1] = l2_1.x[0] * 2.;
+ }
+ return 0;
+} /* tp16_ */
+
+
+/* Subroutine */ int tp17_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = (float)-2.;
+ l2_1.x[1] = 1.;
+ l11_1.lxl[0] = true;
+ l11_1.lxl[1] = false;
+ l12_1.lxu[0] = true;
+ l12_1.lxu[1] = true;
+ l13_1.xl[0] = (float)-2.;
+ l14_1.xu[0] = .5;
+ l14_1.xu[1] = 1.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.xex[0] = 0.;
+ l20_1.xex[1] = 0.;
+ l20_1.fex = .01;
+ l5_2.gg[0] = -1.;
+ l5_2.gg[3] = -1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = 1. - l2_1.x[0];
+ l6_1.fx = (d__1 * d__1 * 100. + d__3 * d__3) * (float).01;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[1] = (l2_1.x[1] - d__1 * d__1) * 200. * (float).01;
+ l4_1.gf[0] = (l2_1.x[0] * (l4_1.gf[1] - 1.) + 1.) * -2. * (float).01;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+ l3_2.g[0] = d__1 * d__1 - l2_1.x[0];
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l3_2.g[1] = d__1 * d__1 - l2_1.x[1];
+ }
+ return 0;
+labelL5:
+ if (l10_3.index2[0]) {
+ l5_2.gg[2] = l2_1.x[1] * 2.;
+ }
+ if (l10_3.index2[1]) {
+ l5_2.gg[1] = l2_1.x[0] * 2.;
+ }
+ return 0;
+} /* tp17_ */
+
+
+/* Subroutine */ int tp18_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 2.;
+ l2_1.x[1] = 2.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = true;
+ l12_1.lxu[i__ - 1] = true;
+/* labelL6: */
+ l14_1.xu[i__ - 1] = 50.;
+ }
+ l13_1.xl[0] = 2.;
+ l13_1.xl[1] = 0.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.xex[0] = std::sqrt(250.);
+ l20_1.xex[1] = l20_1.xex[0] * .1;
+ l20_1.fex = 5.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l6_1.fx = d__1 * d__1 * .01 + d__2 * d__2;
+ return 0;
+labelL3:
+ l4_1.gf[0] = l2_1.x[0] * .02;
+ l4_1.gf[1] = l2_1.x[1] * 2.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = l2_1.x[0] * l2_1.x[1] - 25.;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_2.g[1] = d__1 * d__1 + d__2 * d__2 - 25.;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+ l5_2.gg[0] = l2_1.x[1];
+ l5_2.gg[2] = l2_1.x[0];
+L7:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+ l5_2.gg[1] = l2_1.x[0] * 2.;
+ l5_2.gg[3] = l2_1.x[1] * 2.;
+L8:
+ return 0;
+} /* tp18_ */
+
+
+/* Subroutine */ int tp19_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static Real saex;
+ static int i__;
+ static Real aex;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 20.1;
+ l2_1.x[1] = 5.84;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = true;
+ l12_1.lxu[i__ - 1] = true;
+/* labelL6: */
+ l14_1.xu[i__ - 1] = 100.;
+ }
+ l13_1.xl[0] = 13.;
+ l13_1.xl[1] = 0.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ saex = 17.280975;
+ aex = std::sqrt(saex);
+ l20_1.xex[0] = 14.095;
+ l20_1.xex[1] = 5. - aex;
+/* Computing 3rd power */
+ d__1 = aex + 15.;
+ l20_1.fex = (68.669157374999998 - d__1 * (d__1 * d__1)) * (float)1e-4;
+ return 0;
+labelL2:
+/* Computing 3rd power */
+ d__1 = l2_1.x[0] - 10.;
+/* Computing 3rd power */
+ d__2 = l2_1.x[1] - 20.;
+ l6_1.fx = (d__1 * (d__1 * d__1) + d__2 * (d__2 * d__2)) * (float)1e-4;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 10.;
+ l4_1.gf[0] = d__1 * d__1 * 3. * (float)1e-4;
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - 20.;
+ l4_1.gf[1] = d__1 * d__1 * 3. * (float)1e-4;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 5.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1] - 5.;
+ l3_2.g[0] = d__1 * d__1 + d__2 * d__2 - 100.;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 6.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1] - 5.;
+ l3_2.g[1] = 82.81 - d__1 * d__1 - d__2 * d__2;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+ l5_2.gg[0] = (l2_1.x[0] - 5.) * 2.;
+ l5_2.gg[2] = (l2_1.x[1] - 5.) * 2.;
+L7:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+ l5_2.gg[1] = (l2_1.x[0] - 6.) * -2.;
+ l5_2.gg[3] = (l2_1.x[1] - 5.) * -2.;
+L8:
+ return 0;
+} /* tp19_ */
+
+
+/* Subroutine */ int tp20_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 3;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 1.;
+ l2_1.x[1] = 1.;
+ l11_1.lxl[0] = true;
+ l11_1.lxl[1] = false;
+ l12_1.lxu[0] = true;
+ l12_1.lxu[1] = false;
+ l13_1.xl[0] = -.5;
+ l14_1.xu[0] = .5;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.xex[0] = .5;
+ l20_1.xex[1] = std::sqrt(3.) * .5;
+ l20_1.fex = 81.5 - std::sqrt(3.) * 25.;
+ l5_3.gg[0] = 1.;
+ l5_3.gg[4] = 1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = 1. - l2_1.x[0];
+ l6_1.fx = d__1 * d__1 * 100. + d__3 * d__3;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[1] = (l2_1.x[1] - d__1 * d__1) * 200. * (float).01;
+ l4_1.gf[0] = (l2_1.x[0] * (l4_1.gf[1] - 1.) + 1.) * -2. * (float).01;
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+ l3_3.g[0] = d__1 * d__1 + l2_1.x[0];
+ }
+ if (l9_4.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l3_3.g[1] = d__1 * d__1 + l2_1.x[1];
+ }
+ if (l9_4.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_3.g[2] = d__1 * d__1 + d__2 * d__2 - 1.;
+ }
+ return 0;
+labelL5:
+ if (l10_4.index2[0]) {
+ l5_3.gg[3] = l2_1.x[1] * 2.;
+ }
+ if (l10_4.index2[1]) {
+ l5_3.gg[1] = l2_1.x[0] * 2.;
+ }
+ if (! l10_4.index2[2]) {
+ goto labelL9;
+ }
+ l5_3.gg[2] = l2_1.x[0] * 2.;
+ l5_3.gg[5] = l2_1.x[1] * 2.;
+labelL9:
+ return 0;
+} /* tp20_ */
+
+
+/* Subroutine */ int tp21_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 1;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = -1.;
+ l2_1.x[1] = -1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = true;
+ l12_1.lxu[i__ - 1] = true;
+/* labelL6: */
+ l14_1.xu[i__ - 1] = 50.;
+ }
+ l13_1.xl[0] = 2.;
+ l13_1.xl[1] = -50.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.xex[0] = 2.;
+ l20_1.xex[1] = 0.;
+ l20_1.fex = -.99959999999999993;
+ l5_1.gg[0] = 10.;
+ l5_1.gg[1] = -1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l6_1.fx = (d__1 * d__1 * (float).01 + d__2 * d__2 - 100.) * (float).01;
+ return 0;
+labelL3:
+ l4_1.gf[0] = l2_1.x[0] * (float).02 * (float).01;
+ l4_1.gf[1] = l2_1.x[1] * (float)2. * (float).01;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = l2_1.x[0] * 10. - l2_1.x[1] - 10.;
+ }
+labelL5:
+ return 0;
+} /* tp21_ */
+
+
+/* Subroutine */ int tp22_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 1;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 2.;
+ l2_1.x[1] = 2.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.xex[0] = 1.;
+ l20_1.xex[1] = 1.;
+ l20_1.fex = 1.;
+ l5_2.gg[0] = -1.;
+ l5_2.gg[2] = -1.;
+ l5_2.gg[3] = 1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 2.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1] - 1.;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ return 0;
+labelL3:
+ l4_1.gf[0] = (l2_1.x[0] - 2.) * 2.;
+ l4_1.gf[1] = (l2_1.x[1] - 1.) * 2.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = 2. - l2_1.x[0] - l2_1.x[1];
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l3_2.g[1] = l2_1.x[1] - d__1 * d__1;
+ }
+ return 0;
+labelL5:
+ if (l10_3.index2[1]) {
+ l5_2.gg[1] = l2_1.x[0] * -2.;
+ }
+ return 0;
+} /* tp22_ */
+
+
+/* Subroutine */ int tp23_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 1;
+ l1_1.ninl = 4;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 3.;
+ l2_1.x[1] = 1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = true;
+ l12_1.lxu[i__ - 1] = true;
+ l13_1.xl[i__ - 1] = -50.;
+/* labelL6: */
+ l14_1.xu[i__ - 1] = 50.;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.xex[0] = 1.;
+ l20_1.xex[1] = 1.;
+ l20_1.fex = 2.;
+ l5_4.gg[0] = 1.;
+ l5_4.gg[5] = 1.;
+ l5_4.gg[8] = -1.;
+ l5_4.gg[4] = -1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ return 0;
+labelL3:
+ l4_1.gf[0] = l2_1.x[0] * 2.;
+ l4_1.gf[1] = l2_1.x[1] * 2.;
+ return 0;
+labelL4:
+ if (l9_5.index1[0]) {
+ l3_4.g[0] = l2_1.x[0] + l2_1.x[1] - 1.;
+ }
+ if (l9_5.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_4.g[1] = d__1 * d__1 + d__2 * d__2 - 1.;
+ }
+ if (l9_5.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_4.g[2] = d__1 * d__1 * 9. + d__2 * d__2 - 9.;
+ }
+ if (l9_5.index1[3]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l3_4.g[3] = d__1 * d__1 - l2_1.x[1];
+ }
+ if (l9_5.index1[4]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+ l3_4.g[4] = d__1 * d__1 - l2_1.x[0];
+ }
+ return 0;
+labelL5:
+ if (! l10_5.index2[1]) {
+ goto L8;
+ }
+ l5_4.gg[1] = l2_1.x[0] * 2.;
+ l5_4.gg[6] = l2_1.x[1] * 2.;
+L8:
+ if (! l10_5.index2[2]) {
+ goto labelL9;
+ }
+ l5_4.gg[2] = l2_1.x[0] * 18.;
+ l5_4.gg[7] = l2_1.x[1] * 2.;
+labelL9:
+ if (l10_5.index2[3]) {
+ l5_4.gg[3] = l2_1.x[0] * 2.;
+ }
+ if (l10_5.index2[4]) {
+ l5_4.gg[9] = l2_1.x[1] * 2.;
+ }
+ return 0;
+} /* tp23_ */
+
+
+/* Subroutine */ int tp24_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static Real a;
+ static int i__;
+
+ a = std::sqrt(3.);
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 3;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 1.;
+ l2_1.x[1] = .5;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = true;
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l13_1.xl[i__ - 1] = 0.;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.xex[0] = 3.;
+ l20_1.xex[1] = a;
+ l20_1.fex = -1.;
+ l5_3.gg[0] = 1. / a;
+ l5_3.gg[3] = -1.;
+ l5_3.gg[1] = 1.;
+ l5_3.gg[4] = a;
+ l5_3.gg[2] = -1.;
+ l5_3.gg[5] = -a;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 3.;
+/* Computing 3rd power */
+ d__2 = l2_1.x[1];
+ l6_1.fx = (d__1 * d__1 - 9.) * (d__2 * (d__2 * d__2)) / (a * 27.);
+ return 0;
+labelL3:
+/* Computing 3rd power */
+ d__1 = l2_1.x[1];
+ l4_1.gf[0] = (l2_1.x[0] - 3.) * 2. * (d__1 * (d__1 * d__1)) / (a * 27.);
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 3.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l4_1.gf[1] = (d__1 * d__1 - 9.) * (d__2 * d__2) / (a * 9.);
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = l2_1.x[0] / a - l2_1.x[1];
+ }
+ if (l9_4.index1[1]) {
+ l3_3.g[1] = l2_1.x[0] + l2_1.x[1] * a;
+ }
+ if (l9_4.index1[2]) {
+ l3_3.g[2] = 6. - l2_1.x[1] * a - l2_1.x[0];
+ }
+labelL5:
+ return 0;
+} /* tp24_ */
+
+
+/* Subroutine */ int tp25_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real a[99], b[99];
+ static int i__, j;
+ static Real s, t, u[99], v1, v2, da[297] /* was [99][3] */,
+ v11, v22;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_2.x[0] = 100.;
+ l2_2.x[1] = 12.5;
+ l2_2.x[2] = 3.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_2.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = true;
+ }
+ l13_2.xl[0] = .1;
+ l13_2.xl[1] = 1e-5;
+ l13_2.xl[2] = 1e-5;
+ l14_2.xu[0] = 100.;
+ l14_2.xu[1] = 25.6;
+ l14_2.xu[2] = 5.;
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.xex[0] = 50.;
+ l20_3.xex[1] = 25.;
+ l20_3.xex[2] = 3.;
+ l20_3.fex = 0.;
+ return 0;
+labelL2:
+ for (i__ = 1; i__ <= 99; ++i__) {
+ v1 = .66666666666666663;
+ d__1 = std::log((Real) i__ * .01) * -50.;
+ u[i__ - 1] = pow_dd(&d__1, &v1) + 25.;
+ v1 = u[i__ - 1] - l2_2.x[1];
+ if (v1 < 0.) {
+ goto L7;
+ }
+ v11 = -pow_dd(&v1, &l2_2.x[2]) / l2_2.x[0];
+ b[i__ - 1] = std::exp(v11);
+/* L30: */
+ a[i__ - 1] = b[i__ - 1] - (Real) i__ * .01;
+ }
+ t = 0.;
+ for (i__ = 1; i__ <= 99; ++i__) {
+/* L31: */
+/* Computing 2nd power */
+ d__1 = a[i__ - 1];
+ t += d__1 * d__1;
+ }
+ l6_1.fx = t;
+ return 0;
+L7:
+ s = 0.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L8: */
+/* Computing 2nd power */
+ d__1 = l2_2.x[i__ - 1] - 5.;
+ s += d__1 * d__1;
+ }
+ l6_1.fx = s;
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 99; ++i__) {
+ v1 = .66666666666666663;
+ d__1 = std::log((Real) i__ * .01) * -50.;
+ u[i__ - 1] = pow_dd(&d__1, &v1) + 25.;
+ v2 = u[i__ - 1] - l2_2.x[1];
+ if (v2 <= 0.) {
+ goto labelL9;
+ }
+ v22 = -pow_dd(&v2, &l2_2.x[2]) / l2_2.x[0];
+ if (v22 > -150.) {
+ goto L42;
+ }
+ b[i__ - 1] = 0.;
+ goto L43;
+L42:
+ b[i__ - 1] = std::exp(v22);
+L43:
+ a[i__ - 1] = b[i__ - 1] - (Real) i__ * .01;
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+ da[i__ - 1] = pow_dd(&v2, &l2_2.x[2]) / (d__1 * d__1) * b[i__ - 1];
+ d__1 = l2_2.x[2] - 1.;
+ da[i__ + 98] = l2_2.x[2] * pow_dd(&v2, &d__1) / l2_2.x[0] * b[i__ - 1]
+ ;
+/* L36: */
+ da[i__ + 197] = -pow_dd(&v2, &l2_2.x[2]) / l2_2.x[0] * std::log(v2) * b[
+ i__ - 1];
+ }
+ for (i__ = 1; i__ <= 3; ++i__) {
+ t = 0.;
+ for (j = 1; j <= 99; ++j) {
+/* L33: */
+ t += a[j - 1] * 2. * da[j + i__ * 99 - 100];
+ }
+/* L34: */
+ l4_2.gf[i__ - 1] = t;
+ }
+ return 0;
+labelL9:
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* labelL10: */
+ l4_2.gf[i__ - 1] = (l2_2.x[i__ - 1] - 5.) * 2.;
+ }
+labelL4:
+ return 0;
+} /* tp25_ */
+
+
+/* Subroutine */ int tp26_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static Real a;
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ l2_2.x[0] = -2.6;
+ l2_2.x[1] = 2.;
+ l2_2.x[2] = 2.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_2.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = false;
+ }
+ l20_4.lex = true;
+ l20_4.nex = 2;
+ l20_4.xex[0] = 1.;
+ l20_4.xex[1] = 1.;
+ l20_4.xex[2] = 1.;
+ a = std::sqrt(1.287037037037037);
+ d__1 = a - 1.1296296296296295;
+ d__2 = a + 1.1296296296296295;
+ l20_4.xex[3] = pow_dd(&d__1, &c_b74) - pow_dd(&d__2, &c_b74) -
+ .66666666666666663;
+ l20_4.xex[4] = l20_4.xex[3];
+ l20_4.xex[5] = l20_4.xex[3];
+ l20_4.fex = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0] - l2_2.x[1];
+/* Computing 4th power */
+ d__2 = l2_2.x[1] - l2_2.x[2], d__2 *= d__2;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ return 0;
+labelL3:
+ l4_2.gf[0] = (l2_2.x[0] - l2_2.x[1]) * 2.;
+/* Computing 3rd power */
+ d__1 = l2_2.x[1] - l2_2.x[2];
+ l4_2.gf[2] = d__1 * (d__1 * d__1) * -4.;
+ l4_2.gf[1] = -l4_2.gf[0] - l4_2.gf[2];
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[1];
+/* Computing 4th power */
+ d__2 = l2_2.x[2], d__2 *= d__2;
+ l3_1.g[0] = l2_2.x[0] * (d__1 * d__1 + 1.) + d__2 * d__2 - 3.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+/* Computing 2nd power */
+ d__1 = l2_2.x[1];
+ l5_5.gg[0] = d__1 * d__1 + 1.;
+ l5_5.gg[1] = l2_2.x[0] * 2. * l2_2.x[1];
+/* Computing 3rd power */
+ d__1 = l2_2.x[2];
+ l5_5.gg[2] = d__1 * (d__1 * d__1) * 4.;
+L7:
+ return 0;
+} /* tp26_ */
+
+
+/* Subroutine */ int tp27_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 2.;
+ l11_2.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = false;
+ }
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.xex[0] = -1.;
+ l20_3.xex[1] = 1.;
+ l20_3.xex[2] = 0.;
+ l20_3.fex = (float)4.;
+ l4_2.gf[2] = 0.;
+ l5_5.gg[0] = 1.;
+ l5_5.gg[1] = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0] - 1.;
+/* Computing 2nd power */
+ d__3 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1] - d__3 * d__3;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 * (float)100.;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+ l4_2.gf[1] = (l2_2.x[1] - d__1 * d__1) * 200.;
+ l4_2.gf[0] = (l2_2.x[0] * (.01 - l4_2.gf[1]) - .01) * 200.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[2];
+ l3_1.g[0] = l2_2.x[0] + d__1 * d__1 + 1.;
+ }
+ return 0;
+labelL5:
+ if (l10_2.index2[0]) {
+ l5_5.gg[2] = l2_2.x[2] * 2.;
+ }
+ return 0;
+} /* tp27_ */
+
+
+/* Subroutine */ int tp28_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 1;
+ l1_1.nenl = 0;
+ l2_2.x[0] = -4.;
+ l2_2.x[1] = 1.;
+ l2_2.x[2] = 1.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_2.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = false;
+ }
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.xex[0] = .5;
+ l20_3.xex[1] = -.5;
+ l20_3.xex[2] = .5;
+ l20_3.fex = 0.;
+ l5_5.gg[0] = 1.;
+ l5_5.gg[1] = 2.;
+ l5_5.gg[2] = 3.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0] + l2_2.x[1];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1] + l2_2.x[2];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ return 0;
+labelL3:
+ l4_2.gf[0] = (l2_2.x[0] + l2_2.x[1]) * 2.;
+ l4_2.gf[2] = (l2_2.x[1] + l2_2.x[2]) * 2.;
+ l4_2.gf[1] = l4_2.gf[0] + l4_2.gf[2];
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = l2_2.x[0] + l2_2.x[1] * 2. + l2_2.x[2] * 3. - 1.;
+ }
+labelL5:
+ return 0;
+} /* tp28_ */
+
+
+/* Subroutine */ int tp29_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 1.;
+ l11_2.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = false;
+ }
+ l20_5.lex = true;
+ l20_5.nex = 4;
+ l20_5.xex[0] = 4.;
+ l20_5.xex[1] = std::sqrt(2.) * 2.;
+ l20_5.xex[2] = 2.;
+ l20_5.xex[3] = l20_5.xex[0];
+ l20_5.xex[4] = -l20_5.xex[1];
+ l20_5.xex[5] = -l20_5.xex[2];
+ l20_5.xex[6] = -l20_5.xex[0];
+ l20_5.xex[7] = l20_5.xex[1];
+ l20_5.xex[8] = -l20_5.xex[2];
+ l20_5.xex[9] = -l20_5.xex[0];
+ l20_5.xex[10] = -l20_5.xex[1];
+ l20_5.xex[11] = l20_5.xex[2];
+ l20_5.fex = std::sqrt(2.) * -16.;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_2.x[0] * l2_2.x[1] * l2_2.x[2];
+ return 0;
+labelL3:
+ l4_2.gf[0] = -l2_2.x[1] * l2_2.x[2];
+ l4_2.gf[1] = -l2_2.x[0] * l2_2.x[2];
+ l4_2.gf[2] = -l2_2.x[0] * l2_2.x[1];
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2];
+ l3_1.g[0] = 48. - d__1 * d__1 - d__2 * d__2 * 2. - d__3 * d__3 * 4.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+ l5_5.gg[0] = l2_2.x[0] * -2.;
+ l5_5.gg[1] = l2_2.x[1] * -4.;
+ l5_5.gg[2] = l2_2.x[2] * -8.;
+L7:
+ return 0;
+} /* tp29_ */
+
+
+/* Subroutine */ int tp30_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 1.;
+ l11_2.lxl[i__ - 1] = true;
+ l12_2.lxu[i__ - 1] = true;
+/* labelL6: */
+ l14_2.xu[i__ - 1] = 10.;
+ }
+ l13_2.xl[0] = 1.;
+ l13_2.xl[1] = -10.;
+ l13_2.xl[2] = -10.;
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.xex[0] = 1.;
+ l20_3.xex[1] = 0.;
+ l20_3.xex[2] = 0.;
+ l20_3.fex = 1.;
+ l5_5.gg[2] = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3;
+ return 0;
+labelL3:
+ l4_2.gf[0] = l2_2.x[0] * 2.;
+ l4_2.gf[1] = l2_2.x[1] * 2.;
+ l4_2.gf[2] = l2_2.x[2] * 2.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+ l3_1.g[0] = d__1 * d__1 + d__2 * d__2 - 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+ l5_5.gg[0] = l2_2.x[0] * 2.;
+ l5_5.gg[1] = l2_2.x[1] * 2.;
+L7:
+ return 0;
+} /* tp30_ */
+
+
+/* Subroutine */ int tp31_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 1.;
+ l11_2.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = true;
+ }
+ l13_2.xl[0] = -10.;
+ l13_2.xl[1] = 1.;
+ l13_2.xl[2] = -10.;
+ l14_2.xu[0] = 10.;
+ l14_2.xu[1] = 10.;
+ l14_2.xu[2] = 1.;
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.xex[0] = 1. / std::sqrt(3.);
+ l20_3.xex[1] = std::sqrt(3.);
+ l20_3.xex[2] = 0.;
+ l20_3.fex = 6.;
+ l5_5.gg[2] = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2];
+ l6_1.fx = d__1 * d__1 * 9. + d__2 * d__2 + d__3 * d__3 * 9.;
+ return 0;
+labelL3:
+ l4_2.gf[0] = l2_2.x[0] * 18.;
+ l4_2.gf[1] = l2_2.x[1] * 2.;
+ l4_2.gf[2] = l2_2.x[2] * 18.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = l2_2.x[0] * l2_2.x[1] - 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+ l5_5.gg[0] = l2_2.x[1];
+ l5_5.gg[1] = l2_2.x[0];
+L7:
+ return 0;
+} /* tp31_ */
+
+
+/* Subroutine */ int tp32_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 1;
+ l1_1.nenl = 0;
+ l2_2.x[0] = .1;
+ l2_2.x[1] = .7;
+ l2_2.x[2] = .2;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_2.lxl[i__ - 1] = true;
+ l12_2.lxu[i__ - 1] = false;
+/* labelL6: */
+ l13_2.xl[i__ - 1] = 0.;
+ }
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.xex[0] = 0.;
+ l20_3.xex[1] = 0.;
+ l20_3.xex[2] = 1.;
+ l20_3.fex = 1.;
+ l5_3.gg[2] = 6.;
+ l5_3.gg[4] = 4.;
+ l5_3.gg[1] = -1.;
+ l5_3.gg[3] = -1.;
+ l5_3.gg[5] = -1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0] + l2_2.x[1] * 3. + l2_2.x[2];
+/* Computing 2nd power */
+ d__2 = l2_2.x[0] - l2_2.x[1];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 * 4.;
+ return 0;
+labelL3:
+ l4_2.gf[0] = l2_2.x[0] * 10. - l2_2.x[1] * 2. + l2_2.x[2] * 2.;
+ l4_2.gf[1] = l2_2.x[0] * -2. + l2_2.x[1] * 26. + l2_2.x[2] * 6.;
+ l4_2.gf[2] = (l2_2.x[0] + l2_2.x[1] * 3. + l2_2.x[2]) * 2.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 3rd power */
+ d__1 = l2_2.x[0];
+ l3_2.g[0] = -(d__1 * (d__1 * d__1)) + l2_2.x[1] * 6. + l2_2.x[2] * 4.
+ - 3.;
+ }
+ if (l9_3.index1[1]) {
+ l3_2.g[1] = 1. - l2_2.x[0] - l2_2.x[1] - l2_2.x[2];
+ }
+ return 0;
+labelL5:
+ if (l10_3.index2[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+ l5_3.gg[0] = d__1 * d__1 * -3.;
+ }
+ return 0;
+} /* tp32_ */
+
+
+/* Subroutine */ int tp33_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_2.x[0] = 0.;
+ l2_2.x[1] = 0.;
+ l2_2.x[2] = 3.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_2.lxl[i__ - 1] = true;
+/* labelL6: */
+ l13_2.xl[i__ - 1] = 0.;
+ }
+ l12_2.lxu[0] = false;
+ l12_2.lxu[1] = false;
+ l12_2.lxu[2] = true;
+ l14_2.xu[2] = 5.;
+ l20_3.lex = true;
+ l20_3.nex = 2;
+ l20_3.xex[0] = 0.;
+ l20_3.xex[1] = std::sqrt(2.);
+ l20_3.xex[2] = std::sqrt(2.);
+ l20_3.fex = std::sqrt(2.) - (float)6.;
+ l4_2.gf[1] = 0.;
+ l4_2.gf[2] = 1.;
+ return 0;
+labelL2:
+ l6_1.fx = (l2_2.x[0] - 1.) * (l2_2.x[0] - 2.) * (l2_2.x[0] - 3.) + l2_2.x[
+ 2];
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+ l4_2.gf[0] = d__1 * d__1 * 3. - l2_2.x[0] * 12. + 11.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[2];
+/* Computing 2nd power */
+ d__2 = l2_2.x[0];
+/* Computing 2nd power */
+ d__3 = l2_2.x[1];
+ l3_2.g[0] = d__1 * d__1 - d__2 * d__2 - d__3 * d__3;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2];
+ l3_2.g[1] = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 - 4.;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+ l5_3.gg[0] = l2_2.x[0] * -2.;
+ l5_3.gg[2] = l2_2.x[1] * -2.;
+ l5_3.gg[4] = l2_2.x[2] * 2.;
+L7:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+ l5_3.gg[1] = l2_2.x[0] * 2.;
+ l5_3.gg[3] = l2_2.x[1] * 2.;
+ l5_3.gg[5] = l2_2.x[2] * 2.;
+L8:
+ return 0;
+} /* tp33_ */
+
+
+/* Subroutine */ int tp34_(int *mode)
+{
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_2.x[0] = 0.;
+ l2_2.x[1] = 1.05;
+ l2_2.x[2] = 2.9;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_2.lxl[i__ - 1] = true;
+ l12_2.lxu[i__ - 1] = true;
+/* labelL6: */
+ l13_2.xl[i__ - 1] = 0.;
+ }
+ l14_2.xu[0] = 100.;
+ l14_2.xu[1] = 100.;
+ l14_2.xu[2] = 10.;
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.xex[0] = std::log(std::log(10.));
+ l20_3.xex[1] = std::log(10.);
+ l20_3.xex[2] = 10.;
+ l20_3.fex = -l20_3.xex[0];
+ l4_2.gf[0] = -1.;
+ l4_2.gf[1] = 0.;
+ l4_2.gf[2] = 0.;
+ l5_3.gg[2] = 1.;
+ l5_3.gg[4] = 0.;
+ l5_3.gg[1] = 0.;
+ l5_3.gg[5] = 1.;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_2.x[0];
+labelL3:
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = l2_2.x[1] - std::exp(l2_2.x[0]);
+ }
+ if (l9_3.index1[1]) {
+ l3_2.g[1] = l2_2.x[2] - std::exp(l2_2.x[1]);
+ }
+ return 0;
+labelL5:
+ if (l10_3.index2[0]) {
+ l5_3.gg[0] = -std::exp(l2_2.x[0]);
+ }
+ if (l10_3.index2[1]) {
+ l5_3.gg[3] = -std::exp(l2_2.x[1]);
+ }
+ return 0;
+} /* tp34_ */
+
+
+/* Subroutine */ int tp35_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 1;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = .5;
+ l11_2.lxl[i__ - 1] = true;
+ l12_2.lxu[i__ - 1] = false;
+/* labelL6: */
+ l13_2.xl[i__ - 1] = 0.;
+ }
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.xex[0] = 1.3333333333333333;
+ l20_3.xex[1] = .77777777777777779;
+ l20_3.xex[2] = .44444444444444442;
+ l20_3.fex = .1111111111111111;
+ l5_5.gg[0] = -1.;
+ l5_5.gg[1] = -1.;
+ l5_5.gg[2] = -2.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2];
+ l6_1.fx = 9. - l2_2.x[0] * 8. - l2_2.x[1] * 6. - l2_2.x[2] * 4. + d__1 *
+ d__1 * 2. + d__2 * d__2 * 2. + d__3 * d__3 + l2_2.x[0] * 2. *
+ l2_2.x[1] + l2_2.x[0] * 2. * l2_2.x[2];
+ return 0;
+labelL3:
+ l4_2.gf[0] = l2_2.x[0] * 4. - 8. + l2_2.x[1] * 2. + l2_2.x[2] * 2.;
+ l4_2.gf[1] = l2_2.x[1] * 4. - 6. + l2_2.x[0] * 2.;
+ l4_2.gf[2] = l2_2.x[2] * 2. - 4. + l2_2.x[0] * 2.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = -l2_2.x[0] - l2_2.x[1] - l2_2.x[2] * 2. + 3.;
+ }
+labelL5:
+ return 0;
+} /* tp35_ */
+
+
+/* Subroutine */ int tp36_(int *mode)
+{
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 1;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 10.;
+ l11_2.lxl[i__ - 1] = true;
+ l12_2.lxu[i__ - 1] = true;
+/* labelL6: */
+ l13_2.xl[i__ - 1] = 0.;
+ }
+ l14_2.xu[0] = 20.;
+ l14_2.xu[1] = 11.;
+ l14_2.xu[2] = 42.;
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.xex[0] = 20.;
+ l20_3.xex[1] = 11.;
+ l20_3.xex[2] = 15.;
+ l20_3.fex = -3300.;
+ l5_5.gg[0] = -1.;
+ l5_5.gg[1] = -2.;
+ l5_5.gg[2] = -2.;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_2.x[0] * l2_2.x[1] * l2_2.x[2];
+ return 0;
+labelL3:
+ l4_2.gf[0] = -l2_2.x[1] * l2_2.x[2];
+ l4_2.gf[1] = -l2_2.x[0] * l2_2.x[2];
+ l4_2.gf[2] = -l2_2.x[0] * l2_2.x[1];
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = 72. - l2_2.x[0] - l2_2.x[1] * 2. - l2_2.x[2] * 2.;
+ }
+ return 0;
+labelL5:
+ return 0;
+} /* tp36_ */
+
+
+/* Subroutine */ int tp37_(int *mode)
+{
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 2;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 10.;
+ l11_2.lxl[i__ - 1] = true;
+ l12_2.lxu[i__ - 1] = true;
+ l14_2.xu[i__ - 1] = 42.;
+/* labelL6: */
+ l13_2.xl[i__ - 1] = 0.;
+ }
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.xex[0] = 24.;
+ l20_3.xex[1] = 12.;
+ l20_3.xex[2] = 12.;
+ l20_3.fex = -3456.;
+ l5_3.gg[0] = -1.;
+ l5_3.gg[2] = -2.;
+ l5_3.gg[4] = -2.;
+ l5_3.gg[1] = 1.;
+ l5_3.gg[3] = 2.;
+ l5_3.gg[5] = 2.;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_2.x[0] * l2_2.x[1] * l2_2.x[2];
+ return 0;
+labelL3:
+ l4_2.gf[0] = -l2_2.x[1] * l2_2.x[2];
+ l4_2.gf[1] = -l2_2.x[0] * l2_2.x[2];
+ l4_2.gf[2] = -l2_2.x[0] * l2_2.x[1];
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = 72. - l2_2.x[0] - l2_2.x[1] * 2. - l2_2.x[2] * 2.;
+ }
+ if (l9_3.index1[1]) {
+ l3_2.g[1] = l2_2.x[0] + l2_2.x[1] * 2. + l2_2.x[2] * 2.;
+ }
+labelL5:
+ return 0;
+} /* tp37_ */
+
+
+/* Subroutine */ int tp38_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5, d__6, d__7, d__8;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_3.x[0] = -3.;
+ l2_3.x[1] = -1.;
+ l2_3.x[2] = -3.;
+ l2_3.x[3] = -1.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l11_3.lxl[i__ - 1] = true;
+ l12_3.lxu[i__ - 1] = true;
+ l13_3.xl[i__ - 1] = -10.;
+/* labelL6: */
+ l14_3.xu[i__ - 1] = 10.;
+ }
+ l20_6.lex = true;
+ l20_6.nex = 1;
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* L30: */
+ l20_6.xex[i__ - 1] = 1.;
+ }
+ l20_6.fex = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_3.x[0];
+/* Computing 2nd power */
+ d__1 = l2_3.x[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = 1. - l2_3.x[0];
+/* Computing 2nd power */
+ d__5 = l2_3.x[2];
+/* Computing 2nd power */
+ d__4 = l2_3.x[3] - d__5 * d__5;
+/* Computing 2nd power */
+ d__6 = 1. - l2_3.x[2];
+/* Computing 2nd power */
+ d__7 = l2_3.x[1] - 1.;
+/* Computing 2nd power */
+ d__8 = l2_3.x[3] - 1.;
+ l6_1.fx = (d__1 * d__1 * 100. + d__3 * d__3 + d__4 * d__4 * 90. + d__6 *
+ d__6 + (d__7 * d__7 + d__8 * d__8) * 10.1 + (l2_3.x[1] - 1.) *
+ 19.8 * (l2_3.x[3] - 1.)) * 1e-5;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+ l4_3.gf[0] = (l2_3.x[0] * -400. * (l2_3.x[1] - d__1 * d__1) - (1. -
+ l2_3.x[0]) * 2.) * 1e-5;
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+ l4_3.gf[1] = ((l2_3.x[1] - d__1 * d__1) * 200. + (l2_3.x[1] - 1.) * 20.2
+ + (l2_3.x[3] - 1.) * 19.8) * 1e-5;
+/* Computing 2nd power */
+ d__1 = l2_3.x[2];
+ l4_3.gf[2] = (l2_3.x[2] * -360. * (l2_3.x[3] - d__1 * d__1) - (1. -
+ l2_3.x[2]) * 2.) * 1e-5;
+/* Computing 2nd power */
+ d__1 = l2_3.x[2];
+ l4_3.gf[3] = ((l2_3.x[3] - d__1 * d__1) * 180. + (l2_3.x[3] - 1.) * 20.2
+ + (l2_3.x[1] - 1.) * 19.8) * 1e-5;
+labelL4:
+ return 0;
+} /* tp38_ */
+
+
+/* Subroutine */ int tp39_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 2;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l2_3.x[i__ - 1] = 2.;
+ l11_3.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_3.lxu[i__ - 1] = false;
+ }
+ l20_6.nex = 1;
+ l20_6.lex = true;
+ l20_6.xex[0] = 1.;
+ l20_6.xex[1] = 1.;
+ l20_6.xex[2] = 0.;
+ l20_6.xex[3] = 0.;
+ l20_6.fex = -1.;
+ l4_3.gf[0] = -1.;
+ l4_3.gf[1] = 0.;
+ l4_3.gf[2] = 0.;
+ l4_3.gf[3] = 0.;
+ l5_6.gg[2] = 1.;
+ l5_6.gg[6] = 0.;
+ l5_6.gg[3] = -1.;
+ l5_6.gg[5] = 0.;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_3.x[0];
+labelL3:
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 3rd power */
+ d__1 = l2_3.x[0];
+/* Computing 2nd power */
+ d__2 = l2_3.x[2];
+ l3_2.g[0] = l2_3.x[1] - d__1 * (d__1 * d__1) - d__2 * d__2;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+/* Computing 2nd power */
+ d__2 = l2_3.x[3];
+ l3_2.g[1] = d__1 * d__1 - l2_3.x[1] - d__2 * d__2;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+ l5_6.gg[0] = d__1 * d__1 * -3.;
+ l5_6.gg[4] = l2_3.x[2] * -2.;
+L7:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+ l5_6.gg[1] = l2_3.x[0] * 2.;
+ l5_6.gg[7] = l2_3.x[3] * -2.;
+L8:
+ return 0;
+} /* tp39_ */
+
+
+/* Subroutine */ int tp40_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 3;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l2_3.x[i__ - 1] = .8;
+ l11_3.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_3.lxu[i__ - 1] = false;
+ }
+ for (i__ = 1; i__ <= 3; ++i__) {
+ for (j = 1; j <= 4; ++j) {
+/* L15: */
+ l5_7.gg[i__ + j * 3 - 4] = 0.;
+ }
+ }
+ l5_7.gg[7] = -1.;
+ l5_7.gg[5] = -1.;
+ l20_2.lex = true;
+ l20_2.xex[0] = pow_dd(&c_b305, &c_b306);
+ l20_2.xex[1] = pow_dd(&c_b305, &c_b308);
+ l20_2.xex[2] = pow_dd(&c_b305, &c_b310);
+ l20_2.xex[3] = pow_dd(&c_b305, &c_b312);
+ l20_2.xex[4] = l20_2.xex[0];
+ l20_2.xex[5] = l20_2.xex[1];
+ l20_2.xex[6] = -l20_2.xex[2];
+ l20_2.xex[7] = -l20_2.xex[3];
+ l20_2.fex = -.25;
+ l20_2.nex = 2;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_3.x[0] * l2_3.x[1] * l2_3.x[2] * l2_3.x[3];
+ return 0;
+labelL3:
+ l4_3.gf[0] = -l2_3.x[1] * l2_3.x[2] * l2_3.x[3];
+ l4_3.gf[1] = -l2_3.x[0] * l2_3.x[2] * l2_3.x[3];
+ l4_3.gf[2] = -l2_3.x[0] * l2_3.x[1] * l2_3.x[3];
+ l4_3.gf[3] = -l2_3.x[0] * l2_3.x[1] * l2_3.x[2];
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+/* Computing 3rd power */
+ d__1 = l2_3.x[0];
+/* Computing 2nd power */
+ d__2 = l2_3.x[1];
+ l3_3.g[0] = d__1 * (d__1 * d__1) + d__2 * d__2 - 1.;
+ }
+ if (l9_4.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+ l3_3.g[1] = d__1 * d__1 * l2_3.x[3] - l2_3.x[2];
+ }
+ if (l9_4.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_3.x[3];
+ l3_3.g[2] = d__1 * d__1 - l2_3.x[1];
+ }
+ return 0;
+labelL5:
+ if (! l10_4.index2[0]) {
+ goto L7;
+ }
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+ l5_7.gg[0] = d__1 * d__1 * 3.;
+ l5_7.gg[3] = l2_3.x[1] * 2.;
+L7:
+ if (! l10_4.index2[1]) {
+ goto L8;
+ }
+ l5_7.gg[1] = l2_3.x[0] * 2. * l2_3.x[3];
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+ l5_7.gg[10] = d__1 * d__1;
+L8:
+ if (! l10_4.index2[2]) {
+ goto labelL9;
+ }
+ l5_7.gg[11] = l2_3.x[3] * 2.;
+labelL9:
+ return 0;
+} /* tp40_ */
+
+
+/* Subroutine */ int tp41_(int *mode)
+{
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 1;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l2_3.x[i__ - 1] = 2.;
+ l11_3.lxl[i__ - 1] = true;
+ l12_3.lxu[i__ - 1] = true;
+/* labelL6: */
+ l13_3.xl[i__ - 1] = 0.;
+ }
+ l14_3.xu[0] = 1.;
+ l14_3.xu[1] = 1.;
+ l14_3.xu[2] = 1.;
+ l14_3.xu[3] = 2.;
+ l4_3.gf[3] = 0.;
+ l5_2.gg[0] = 1.;
+ l5_2.gg[1] = 2.;
+ l5_2.gg[2] = 2.;
+ l5_2.gg[3] = -1.;
+ l20_6.lex = true;
+ l20_6.nex = 1;
+ l20_6.xex[0] = .66666666666666663;
+ l20_6.xex[1] = .33333333333333331;
+ l20_6.xex[2] = l20_6.xex[1];
+ l20_6.xex[3] = 2.;
+ l20_6.fex = 1.9259259259259258;
+ return 0;
+labelL2:
+ l6_1.fx = 2. - l2_3.x[0] * l2_3.x[1] * l2_3.x[2];
+ return 0;
+labelL3:
+ l4_3.gf[0] = -l2_3.x[1] * l2_3.x[2];
+ l4_3.gf[1] = -l2_3.x[0] * l2_3.x[2];
+ l4_3.gf[2] = -l2_3.x[0] * l2_3.x[1];
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = l2_3.x[0] + l2_3.x[1] * 2. + l2_3.x[2] * 2. - l2_3.x[3];
+ }
+labelL5:
+ return 0;
+} /* tp41_ */
+
+
+/* Subroutine */ int tp42_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 1;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l2_3.x[i__ - 1] = 1.;
+ l12_3.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_3.lxl[i__ - 1] = false;
+ }
+ l5_6.gg[0] = 1.;
+ l5_6.gg[2] = 0.;
+ l5_6.gg[4] = 0.;
+ l5_6.gg[6] = 0.;
+ l5_6.gg[1] = 0.;
+ l5_6.gg[3] = 0.;
+ l20_6.lex = true;
+ l20_6.nex = 1;
+ l20_6.xex[0] = 2.;
+ l20_6.xex[1] = 2.;
+ l20_6.xex[2] = std::sqrt(.72);
+ l20_6.xex[3] = std::sqrt(1.28);
+ l20_6.fex = 28. - std::sqrt(2.) * 10.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_3.x[0] - 1.;
+/* Computing 2nd power */
+ d__2 = l2_3.x[1] - 2.;
+/* Computing 2nd power */
+ d__3 = l2_3.x[2] - 3.;
+/* Computing 2nd power */
+ d__4 = l2_3.x[3] - 4.;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 + d__4 * d__4;
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* L21: */
+ l4_3.gf[i__ - 1] = (l2_3.x[i__ - 1] - (Real) i__) * 2.;
+ }
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = l2_3.x[0] - 2.;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_3.x[2];
+/* Computing 2nd power */
+ d__2 = l2_3.x[3];
+ l3_2.g[1] = d__1 * d__1 + d__2 * d__2 - 2.;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+ l5_6.gg[5] = l2_3.x[2] * 2.;
+ l5_6.gg[7] = l2_3.x[3] * 2.;
+L8:
+ return 0;
+} /* tp42_ */
+
+
+/* Subroutine */ int tp43_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 3;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l2_3.x[i__ - 1] = 0.;
+ l11_3.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_3.lxu[i__ - 1] = false;
+ }
+ l20_6.lex = true;
+ l20_6.nex = 1;
+ l20_6.xex[0] = 0.;
+ l20_6.xex[1] = 1.;
+ l20_6.xex[2] = 2.;
+ l20_6.xex[3] = -1.;
+ l20_6.fex = -44.;
+ l5_7.gg[11] = 1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+/* Computing 2nd power */
+ d__2 = l2_3.x[1];
+/* Computing 2nd power */
+ d__3 = l2_3.x[2];
+/* Computing 2nd power */
+ d__4 = l2_3.x[3];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 * 2. + d__4 * d__4 -
+ l2_3.x[0] * 5. - l2_3.x[1] * 5. - l2_3.x[2] * 21. + l2_3.x[3] *
+ 7.;
+ return 0;
+labelL3:
+ l4_3.gf[0] = l2_3.x[0] * 2. - 5.;
+ l4_3.gf[1] = l2_3.x[1] * 2. - 5.;
+ l4_3.gf[2] = l2_3.x[2] * 4. - 21.;
+ l4_3.gf[3] = l2_3.x[3] * 2. + 7.;
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+/* Computing 2nd power */
+ d__2 = l2_3.x[1];
+/* Computing 2nd power */
+ d__3 = l2_3.x[2];
+/* Computing 2nd power */
+ d__4 = l2_3.x[3];
+ l3_3.g[0] = -(d__1 * d__1) - d__2 * d__2 - d__3 * d__3 - d__4 * d__4
+ - l2_3.x[0] + l2_3.x[1] - l2_3.x[2] + l2_3.x[3] + 8.;
+ }
+ if (l9_4.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+/* Computing 2nd power */
+ d__2 = l2_3.x[1];
+/* Computing 2nd power */
+ d__3 = l2_3.x[2];
+/* Computing 2nd power */
+ d__4 = l2_3.x[3];
+ l3_3.g[1] = -(d__1 * d__1) - d__2 * d__2 * 2. - d__3 * d__3 - d__4 *
+ d__4 * 2. + l2_3.x[0] + l2_3.x[3] + 10.;
+ }
+ if (l9_4.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+/* Computing 2nd power */
+ d__2 = l2_3.x[1];
+/* Computing 2nd power */
+ d__3 = l2_3.x[2];
+ l3_3.g[2] = d__1 * d__1 * -2. - d__2 * d__2 - d__3 * d__3 - l2_3.x[0]
+ * 2. + l2_3.x[1] + l2_3.x[3] + 5.;
+ }
+ return 0;
+labelL5:
+ if (! l10_4.index2[0]) {
+ goto L7;
+ }
+ l5_7.gg[0] = l2_3.x[0] * -2. - 1.;
+ l5_7.gg[3] = l2_3.x[1] * -2. + 1.;
+ l5_7.gg[6] = l2_3.x[2] * -2. - 1.;
+ l5_7.gg[9] = l2_3.x[3] * -2. + 1.;
+L7:
+ if (! l10_4.index2[1]) {
+ goto L8;
+ }
+ l5_7.gg[1] = l2_3.x[0] * -2. + 1.;
+ l5_7.gg[4] = l2_3.x[1] * -4.;
+ l5_7.gg[7] = l2_3.x[2] * -2.;
+ l5_7.gg[10] = l2_3.x[3] * -4. + 1.;
+L8:
+ if (! l10_4.index2[2]) {
+ goto labelL9;
+ }
+ l5_7.gg[2] = l2_3.x[0] * -4. - 2.;
+ l5_7.gg[5] = l2_3.x[1] * -2. + 1.;
+ l5_7.gg[8] = l2_3.x[2] * -2.;
+labelL9:
+ return 0;
+} /* tp43_ */
+
+
+/* Subroutine */ int tp44_(int *mode)
+{
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 6;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l2_3.x[i__ - 1] = 0.;
+ l13_3.xl[i__ - 1] = 0.;
+ l11_3.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_3.lxu[i__ - 1] = false;
+ }
+ for (i__ = 1; i__ <= 6; ++i__) {
+ for (j = 1; j <= 4; ++j) {
+/* L15: */
+ l5_8.gg[i__ + j * 6 - 7] = 0.;
+ }
+ }
+ l5_8.gg[0] = -1.;
+ l5_8.gg[6] = -2.;
+ l5_8.gg[1] = -4.;
+ l5_8.gg[7] = -1.;
+ l5_8.gg[2] = -3.;
+ l5_8.gg[8] = -4.;
+ l5_8.gg[15] = -2.;
+ l5_8.gg[21] = -1.;
+ l5_8.gg[16] = -1.;
+ l5_8.gg[22] = -2.;
+ l5_8.gg[17] = -1.;
+ l5_8.gg[23] = -1.;
+ l20_6.lex = true;
+ l20_6.nex = 1;
+ l20_6.xex[0] = 0.;
+ l20_6.xex[1] = 3.;
+ l20_6.xex[2] = 0.;
+ l20_6.xex[3] = 4.;
+ l20_6.fex = -15.;
+ return 0;
+labelL2:
+ l6_1.fx = l2_3.x[0] - l2_3.x[1] - l2_3.x[2] - l2_3.x[0] * l2_3.x[2] +
+ l2_3.x[0] * l2_3.x[3] + l2_3.x[1] * l2_3.x[2] - l2_3.x[1] *
+ l2_3.x[3];
+ return 0;
+labelL3:
+ l4_3.gf[0] = 1. - l2_3.x[2] + l2_3.x[3];
+ l4_3.gf[1] = l2_3.x[2] - 1. - l2_3.x[3];
+ l4_3.gf[2] = -1. - l2_3.x[0] + l2_3.x[1];
+ l4_3.gf[3] = l2_3.x[0] - l2_3.x[1];
+ return 0;
+labelL4:
+ if (l9_6.index1[0]) {
+ l3_5.g[0] = 8. - l2_3.x[0] - l2_3.x[1] * 2.;
+ }
+ if (l9_6.index1[1]) {
+ l3_5.g[1] = 12. - l2_3.x[0] * 4. - l2_3.x[1];
+ }
+ if (l9_6.index1[2]) {
+ l3_5.g[2] = 12. - l2_3.x[0] * 3. - l2_3.x[1] * 4.;
+ }
+ if (l9_6.index1[3]) {
+ l3_5.g[3] = 8. - l2_3.x[2] * 2. - l2_3.x[3];
+ }
+ if (l9_6.index1[4]) {
+ l3_5.g[4] = 8. - l2_3.x[2] - l2_3.x[3] * 2.;
+ }
+ if (l9_6.index1[5]) {
+ l3_5.g[5] = 5. - l2_3.x[2] - l2_3.x[3];
+ }
+labelL5:
+ return 0;
+} /* tp44_ */
+
+
+/* Subroutine */ int tp45_(int *mode)
+{
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l2_4.x[i__ - 1] = 2.;
+ l11_4.lxl[i__ - 1] = true;
+ l12_4.lxu[i__ - 1] = true;
+ l13_4.xl[i__ - 1] = 0.;
+/* labelL6: */
+ l14_4.xu[i__ - 1] = (Real) i__;
+ }
+ l20_7.lex = true;
+ l20_7.nex = 1;
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L30: */
+ l20_7.xex[i__ - 1] = (Real) i__;
+ }
+ l20_7.fex = 1.;
+ return 0;
+labelL2:
+ l6_1.fx = 2. - l2_4.x[0] * l2_4.x[1] * l2_4.x[2] * l2_4.x[3] * l2_4.x[4] /
+ 120.;
+ return 0;
+labelL3:
+ l4_4.gf[0] = -l2_4.x[1] * l2_4.x[2] * l2_4.x[3] * l2_4.x[4] / 120.;
+ l4_4.gf[1] = -l2_4.x[0] * l2_4.x[2] * l2_4.x[3] * l2_4.x[4] / 120.;
+ l4_4.gf[2] = -l2_4.x[0] * l2_4.x[1] * l2_4.x[3] * l2_4.x[4] / 120.;
+ l4_4.gf[3] = -l2_4.x[0] * l2_4.x[1] * l2_4.x[2] * l2_4.x[4] / 120.;
+ l4_4.gf[4] = -l2_4.x[0] * l2_4.x[1] * l2_4.x[2] * l2_4.x[3] / 120.;
+labelL4:
+ return 0;
+} /* tp45_ */
+
+
+/* Subroutine */ int tp46_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 2;
+ l2_4.x[0] = std::sqrt(2.) * .5;
+ l2_4.x[1] = 1.75;
+ l2_4.x[2] = .5;
+ l2_4.x[3] = 2.;
+ l2_4.x[4] = 2.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l11_4.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_4.lxu[i__ - 1] = false;
+ }
+ l5_4.gg[2] = 0.;
+ l5_4.gg[4] = 0.;
+ l5_4.gg[1] = 0.;
+ l5_4.gg[3] = 1.;
+ l5_4.gg[9] = 0.;
+ l20_7.lex = true;
+ l20_7.nex = 1;
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L30: */
+ l20_7.xex[i__ - 1] = 1.;
+ }
+ l20_7.fex = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_4.x[0] - l2_4.x[1];
+/* Computing 2nd power */
+ d__2 = l2_4.x[2] - 1.;
+/* Computing 4th power */
+ d__3 = l2_4.x[3] - 1., d__3 *= d__3;
+/* Computing 6th power */
+ d__4 = l2_4.x[4] - 1., d__4 *= d__4;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 + d__4 * (d__4 * d__4);
+ return 0;
+labelL3:
+ l4_4.gf[0] = (l2_4.x[0] - l2_4.x[1]) * 2.;
+ l4_4.gf[1] = -l4_4.gf[0];
+ l4_4.gf[2] = (l2_4.x[2] - 1.) * 2.;
+/* Computing 3rd power */
+ d__1 = l2_4.x[3] - 1.;
+ l4_4.gf[3] = d__1 * (d__1 * d__1) * 4.;
+/* Computing 5th power */
+ d__1 = l2_4.x[4] - 1., d__2 = d__1, d__1 *= d__1;
+ l4_4.gf[4] = d__2 * (d__1 * d__1) * 6.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_4.x[0];
+ l3_2.g[0] = d__1 * d__1 * l2_4.x[3] + std::sin(l2_4.x[3] - l2_4.x[4]) - 1.;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 4th power */
+ d__1 = l2_4.x[2], d__1 *= d__1;
+/* Computing 2nd power */
+ d__2 = l2_4.x[3];
+ l3_2.g[1] = l2_4.x[1] + d__1 * d__1 * (d__2 * d__2) - 2.;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+ l5_4.gg[0] = l2_4.x[0] * 2. * l2_4.x[3];
+ l5_4.gg[8] = -std::cos(l2_4.x[3] - l2_4.x[4]);
+/* Computing 2nd power */
+ d__1 = l2_4.x[0];
+ l5_4.gg[6] = d__1 * d__1 - l5_4.gg[8];
+L7:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+/* Computing 3rd power */
+ d__1 = l2_4.x[2];
+/* Computing 2nd power */
+ d__2 = l2_4.x[3];
+ l5_4.gg[5] = d__1 * (d__1 * d__1) * 4. * (d__2 * d__2);
+/* Computing 4th power */
+ d__1 = l2_4.x[2], d__1 *= d__1;
+ l5_4.gg[7] = d__1 * d__1 * 2. * l2_4.x[3];
+L8:
+ return 0;
+} /* tp46_ */
+
+
+/* Subroutine */ int tp47_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__, j;
+ static Real v1, v2, v3, v4;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 3;
+ l2_4.x[0] = 2.;
+ l2_4.x[1] = std::sqrt(2.);
+ l2_4.x[2] = -1.;
+ l2_4.x[3] = 2. - l2_4.x[1];
+ l2_4.x[4] = .5;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l11_4.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_4.lxu[i__ - 1] = false;
+ }
+ for (i__ = 1; i__ <= 3; ++i__) {
+ for (j = 1; j <= 5; ++j) {
+/* L15: */
+ l5_9.gg[i__ + j * 3 - 4] = 0.;
+ }
+ }
+ l5_9.gg[0] = 1.;
+ l5_9.gg[4] = 1.;
+ l5_9.gg[10] = 1.;
+ l20_7.lex = true;
+ l20_7.nex = 1;
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L30: */
+ l20_7.xex[i__ - 1] = 1.;
+ }
+ l20_7.fex = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_4.x[0] - l2_4.x[1];
+/* Computing 2nd power */
+ d__2 = l2_4.x[1] - l2_4.x[2];
+/* Computing 4th power */
+ d__3 = l2_4.x[2] - l2_4.x[3], d__3 *= d__3;
+/* Computing 4th power */
+ d__4 = l2_4.x[3] - l2_4.x[4], d__4 *= d__4;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 + d__4 * d__4;
+ return 0;
+labelL3:
+ v1 = (l2_4.x[0] - l2_4.x[1]) * 2.;
+ v2 = (l2_4.x[1] - l2_4.x[2]) * 2.;
+/* Computing 3rd power */
+ d__1 = l2_4.x[2] - l2_4.x[3];
+ v3 = d__1 * (d__1 * d__1) * 4.;
+/* Computing 3rd power */
+ d__1 = l2_4.x[3] - l2_4.x[4];
+ v4 = d__1 * (d__1 * d__1) * 4.;
+ l4_4.gf[0] = v1;
+ l4_4.gf[1] = -v1 + v2;
+ l4_4.gf[2] = -v2 + v3;
+ l4_4.gf[3] = -v3 + v4;
+ l4_4.gf[4] = -v4;
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_4.x[1];
+/* Computing 3rd power */
+ d__2 = l2_4.x[2];
+ l3_3.g[0] = l2_4.x[0] + d__1 * d__1 + d__2 * (d__2 * d__2) - 3.;
+ }
+ if (l9_4.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_4.x[2];
+ l3_3.g[1] = l2_4.x[1] - d__1 * d__1 + l2_4.x[3] - 1.;
+ }
+ if (l9_4.index1[2]) {
+ l3_3.g[2] = l2_4.x[0] * l2_4.x[4] - 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_4.index2[0]) {
+ goto L7;
+ }
+ l5_9.gg[3] = l2_4.x[1] * 2.;
+/* Computing 2nd power */
+ d__1 = l2_4.x[2];
+ l5_9.gg[6] = d__1 * d__1 * 3.;
+L7:
+ if (l10_4.index2[1]) {
+ l5_9.gg[7] = l2_4.x[2] * -2.;
+ }
+/* L8: */
+ if (! l10_4.index2[2]) {
+ goto labelL9;
+ }
+ l5_9.gg[2] = l2_4.x[4];
+ l5_9.gg[14] = l2_4.x[0];
+labelL9:
+ return 0;
+} /* tp47_ */
+
+
+/* Subroutine */ int tp48_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 2;
+ l1_1.nenl = 0;
+ l2_4.x[0] = 3.;
+ l2_4.x[1] = 5.;
+ l2_4.x[2] = -3.;
+ l2_4.x[3] = 2.;
+ l2_4.x[4] = -2.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l11_4.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_4.lxu[i__ - 1] = false;
+ }
+ l5_4.gg[1] = 0.;
+ l5_4.gg[3] = 0.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* labelL20: */
+ l5_4.gg[(i__ << 1) - 2] = 1.;
+ }
+ l5_4.gg[5] = 1.;
+ l5_4.gg[7] = -2.;
+ l5_4.gg[9] = -2.;
+ l20_7.lex = true;
+ l20_7.nex = 1;
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L30: */
+ l20_7.xex[i__ - 1] = 1.;
+ }
+ l20_7.fex = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_4.x[0] - 1.;
+/* Computing 2nd power */
+ d__2 = l2_4.x[1] - l2_4.x[2];
+/* Computing 2nd power */
+ d__3 = l2_4.x[3] - l2_4.x[4];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3;
+ return 0;
+labelL3:
+ l4_4.gf[0] = (l2_4.x[0] - 1.) * 2.;
+ l4_4.gf[1] = (l2_4.x[1] - l2_4.x[2]) * 2.;
+ l4_4.gf[2] = -l4_4.gf[1];
+ l4_4.gf[3] = (l2_4.x[3] - l2_4.x[4]) * 2.;
+ l4_4.gf[4] = -l4_4.gf[3];
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = l2_4.x[0] + l2_4.x[1] + l2_4.x[2] + l2_4.x[3] + l2_4.x[4]
+ - 5.;
+ }
+ if (l9_3.index1[1]) {
+ l3_2.g[1] = l2_4.x[2] - (l2_4.x[3] + l2_4.x[4]) * 2. + 3.;
+ }
+labelL5:
+ return 0;
+} /* tp48_ */
+
+
+/* Subroutine */ int tp49_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 2;
+ l1_1.nenl = 0;
+ l2_4.x[0] = 10.;
+ l2_4.x[1] = 7.;
+ l2_4.x[2] = 2.;
+ l2_4.x[3] = -3.;
+ l2_4.x[4] = .8;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l11_4.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_4.lxu[i__ - 1] = false;
+ }
+ l5_4.gg[8] = 0.;
+ l5_4.gg[1] = 0.;
+ l5_4.gg[3] = 0.;
+ l5_4.gg[7] = 0.;
+ l5_4.gg[0] = 1.;
+ l5_4.gg[2] = 1.;
+ l5_4.gg[4] = 1.;
+ l5_4.gg[6] = 4.;
+ l5_4.gg[5] = 1.;
+ l5_4.gg[9] = 5.;
+ l20_7.lex = true;
+ l20_7.nex = 1;
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L30: */
+ l20_7.xex[i__ - 1] = 1.;
+ }
+ l20_7.fex = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_4.x[0] - l2_4.x[1];
+/* Computing 2nd power */
+ d__2 = l2_4.x[2] - 1.;
+/* Computing 4th power */
+ d__3 = l2_4.x[3] - 1., d__3 *= d__3;
+/* Computing 6th power */
+ d__4 = l2_4.x[4] - 1., d__4 *= d__4;
+ l6_1.fx = (d__1 * d__1 + d__2 * d__2 + d__3 * d__3 + d__4 * (d__4 * d__4))
+ * .001;
+ return 0;
+labelL3:
+ l4_4.gf[0] = (l2_4.x[0] - l2_4.x[1]) * 2. * .001;
+ l4_4.gf[1] = -l4_4.gf[0];
+ l4_4.gf[2] = (l2_4.x[2] - 1.) * 2. * .001;
+/* Computing 3rd power */
+ d__1 = l2_4.x[3] - 1.;
+ l4_4.gf[3] = d__1 * (d__1 * d__1) * 4. * .001;
+/* Computing 5th power */
+ d__1 = l2_4.x[4] - 1., d__2 = d__1, d__1 *= d__1;
+ l4_4.gf[4] = d__2 * (d__1 * d__1) * 6. * .001;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = l2_4.x[0] + l2_4.x[1] + l2_4.x[2] + l2_4.x[3] * 4. - 7.;
+ }
+ if (l9_3.index1[1]) {
+ l3_2.g[1] = l2_4.x[2] + l2_4.x[4] * 5. - 6.;
+ }
+labelL5:
+ return 0;
+} /* tp49_ */
+
+
+/* Subroutine */ int tp50_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__, j;
+ static Real v1, v2, v3, v4;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 3;
+ l1_1.nenl = 0;
+ l2_4.x[0] = 35.;
+ l2_4.x[1] = -31.;
+ l2_4.x[2] = 11.;
+ l2_4.x[3] = 5.;
+ l2_4.x[4] = -5.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l11_4.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_4.lxu[i__ - 1] = false;
+ }
+ for (i__ = 1; i__ <= 3; ++i__) {
+ for (j = 1; j <= 5; ++j) {
+/* L15: */
+ l5_9.gg[i__ + j * 3 - 4] = 0.;
+ }
+ }
+ l5_9.gg[0] = 1.;
+ l5_9.gg[3] = 2.;
+ l5_9.gg[6] = 3.;
+ l5_9.gg[4] = 1.;
+ l5_9.gg[7] = 2.;
+ l5_9.gg[10] = 3.;
+ l5_9.gg[8] = 1.;
+ l5_9.gg[11] = 2.;
+ l5_9.gg[14] = 3.;
+ l20_7.lex = true;
+ l20_7.nex = 1;
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L30: */
+ l20_7.xex[i__ - 1] = 1.;
+ }
+ l20_7.fex = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_4.x[0] - l2_4.x[1];
+/* Computing 2nd power */
+ d__2 = l2_4.x[1] - l2_4.x[2];
+/* Computing 4th power */
+ d__3 = l2_4.x[2] - l2_4.x[3], d__3 *= d__3;
+/* Computing 4th power */
+ d__4 = l2_4.x[3] - l2_4.x[4], d__4 *= d__4;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 + d__4 * d__4;
+ return 0;
+labelL3:
+ v1 = (l2_4.x[0] - l2_4.x[1]) * 2.;
+ v2 = (l2_4.x[1] - l2_4.x[2]) * 2.;
+/* Computing 3rd power */
+ d__1 = l2_4.x[2] - l2_4.x[3];
+ v3 = d__1 * (d__1 * d__1) * 4.;
+/* Computing 3rd power */
+ d__1 = l2_4.x[3] - l2_4.x[4];
+ v4 = d__1 * (d__1 * d__1) * 4.;
+ l4_4.gf[0] = v1;
+ l4_4.gf[1] = -v1 + v2;
+ l4_4.gf[2] = -v2 + v3;
+ l4_4.gf[3] = -v3 + v4;
+ l4_4.gf[4] = -v4;
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = l2_4.x[0] + l2_4.x[1] * 2. + l2_4.x[2] * 3. - 6.;
+ }
+ if (l9_4.index1[1]) {
+ l3_3.g[1] = l2_4.x[1] + l2_4.x[2] * 2. + l2_4.x[3] * 3. - 6.;
+ }
+ if (l9_4.index1[2]) {
+ l3_3.g[2] = l2_4.x[2] + l2_4.x[3] * 2. + l2_4.x[4] * 3. - 6.;
+ }
+labelL5:
+ return 0;
+} /* tp50_ */
+
+
+/* Subroutine */ int tp51_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 3;
+ l1_1.nenl = 0;
+ l2_4.x[0] = 2.5;
+ l2_4.x[1] = .5;
+ l2_4.x[2] = 2.;
+ l2_4.x[3] = -1.;
+ l2_4.x[4] = .5;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l11_4.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_4.lxu[i__ - 1] = false;
+ }
+ for (i__ = 1; i__ <= 3; ++i__) {
+ for (j = 1; j <= 5; ++j) {
+/* L15: */
+ l5_9.gg[i__ + j * 3 - 4] = 0.;
+ }
+ }
+ l5_9.gg[0] = 1.;
+ l5_9.gg[3] = 3.;
+ l5_9.gg[7] = 1.;
+ l5_9.gg[10] = 1.;
+ l5_9.gg[13] = -2.;
+ l5_9.gg[5] = 1.;
+ l5_9.gg[14] = -1.;
+ l20_7.lex = true;
+ l20_7.nex = 1;
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L30: */
+ l20_7.xex[i__ - 1] = 1.;
+ }
+ l20_7.fex = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_4.x[0] - l2_4.x[1];
+/* Computing 2nd power */
+ d__2 = l2_4.x[1] + l2_4.x[2] - 2.;
+/* Computing 2nd power */
+ d__3 = l2_4.x[3] - 1.;
+/* Computing 2nd power */
+ d__4 = l2_4.x[4] - 1.;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 + d__4 * d__4;
+ return 0;
+labelL3:
+ l4_4.gf[0] = (l2_4.x[0] - l2_4.x[1]) * 2.;
+ l4_4.gf[2] = (l2_4.x[1] + l2_4.x[2] - 2.) * 2.;
+ l4_4.gf[1] = l4_4.gf[2] - l4_4.gf[0];
+ l4_4.gf[3] = (l2_4.x[3] - 1.) * 2.;
+ l4_4.gf[4] = (l2_4.x[4] - 1.) * 2.;
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = l2_4.x[0] + l2_4.x[1] * 3. - 4.;
+ }
+ if (l9_4.index1[1]) {
+ l3_3.g[1] = l2_4.x[2] + l2_4.x[3] - l2_4.x[4] * 2.;
+ }
+ if (l9_4.index1[2]) {
+ l3_3.g[2] = l2_4.x[1] - l2_4.x[4];
+ }
+labelL5:
+ return 0;
+} /* tp51_ */
+
+
+/* Subroutine */ int tp52_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 3;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l2_4.x[i__ - 1] = 2.;
+ l11_4.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_4.lxu[i__ - 1] = false;
+ }
+ for (i__ = 1; i__ <= 3; ++i__) {
+ for (j = 1; j <= 5; ++j) {
+/* L15: */
+ l5_9.gg[i__ + j * 3 - 4] = 0.;
+ }
+ }
+ l5_9.gg[0] = 1.;
+ l5_9.gg[3] = 3.;
+ l5_9.gg[7] = 1.;
+ l5_9.gg[10] = 1.;
+ l5_9.gg[13] = -2.;
+ l5_9.gg[5] = 1.;
+ l5_9.gg[14] = -1.;
+ l20_7.lex = true;
+ l20_7.nex = 1;
+ l20_7.xex[0] = -.094555873925501438;
+ l20_7.xex[1] = .03151862464183381;
+ l20_7.xex[2] = .51575931232091687;
+ l20_7.xex[3] = -.45272206303724927;
+ l20_7.xex[4] = l20_7.xex[1];
+ l20_7.fex = 5.3266475644699138;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_4.x[0] * 4. - l2_4.x[1];
+/* Computing 2nd power */
+ d__2 = l2_4.x[1] + l2_4.x[2] - 2.;
+/* Computing 2nd power */
+ d__3 = l2_4.x[3] - 1.;
+/* Computing 2nd power */
+ d__4 = l2_4.x[4] - 1.;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 + d__4 * d__4;
+ return 0;
+labelL3:
+ l4_4.gf[0] = (l2_4.x[0] * 4. - l2_4.x[1]) * 8.;
+ l4_4.gf[2] = (l2_4.x[1] + l2_4.x[2] - 2.) * 2.;
+ l4_4.gf[1] = l4_4.gf[0] * -.25 + l4_4.gf[2];
+ l4_4.gf[3] = (l2_4.x[3] - 1.) * 2.;
+ l4_4.gf[4] = (l2_4.x[4] - 1.) * 2.;
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = l2_4.x[0] + l2_4.x[1] * 3.;
+ }
+ if (l9_4.index1[1]) {
+ l3_3.g[1] = l2_4.x[2] + l2_4.x[3] - l2_4.x[4] * 2.;
+ }
+ if (l9_4.index1[2]) {
+ l3_3.g[2] = l2_4.x[1] - l2_4.x[4];
+ }
+labelL5:
+ return 0;
+} /* tp52_ */
+
+
+/* Subroutine */ int tp53_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 3;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l2_4.x[i__ - 1] = 2.;
+ l11_4.lxl[i__ - 1] = true;
+ l12_4.lxu[i__ - 1] = true;
+ l13_4.xl[i__ - 1] = -10.;
+/* labelL6: */
+ l14_4.xu[i__ - 1] = 10.;
+ }
+ for (i__ = 1; i__ <= 3; ++i__) {
+ for (j = 1; j <= 5; ++j) {
+/* L15: */
+ l5_9.gg[i__ + j * 3 - 4] = 0.;
+ }
+ }
+ l5_9.gg[0] = 1.;
+ l5_9.gg[3] = 3.;
+ l5_9.gg[7] = 1.;
+ l5_9.gg[10] = 1.;
+ l5_9.gg[13] = -2.;
+ l5_9.gg[5] = 1.;
+ l5_9.gg[14] = -1.;
+ l20_7.lex = true;
+ l20_7.nex = 1;
+ l20_7.xex[0] = -.76744186046511631;
+ l20_7.xex[1] = .2558139534883721;
+ l20_7.xex[2] = .62790697674418605;
+ l20_7.xex[3] = -.11627906976744186;
+ l20_7.xex[4] = .2558139534883721;
+ l20_7.fex = 4.0930232558139537;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_4.x[0] - l2_4.x[1];
+/* Computing 2nd power */
+ d__2 = l2_4.x[1] + l2_4.x[2] - 2.;
+/* Computing 2nd power */
+ d__3 = l2_4.x[3] - 1.;
+/* Computing 2nd power */
+ d__4 = l2_4.x[4] - 1.;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 + d__4 * d__4;
+ return 0;
+labelL3:
+ l4_4.gf[0] = (l2_4.x[0] - l2_4.x[1]) * 2.;
+ l4_4.gf[2] = (l2_4.x[1] + l2_4.x[2] - 2.) * 2.;
+ l4_4.gf[1] = l4_4.gf[2] - l4_4.gf[0];
+ l4_4.gf[3] = (l2_4.x[3] - 1.) * 2.;
+ l4_4.gf[4] = (l2_4.x[4] - 1.) * 2.;
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = l2_4.x[0] + l2_4.x[1] * 3.;
+ }
+ if (l9_4.index1[1]) {
+ l3_3.g[1] = l2_4.x[2] + l2_4.x[3] - l2_4.x[4] * 2.;
+ }
+ if (l9_4.index1[2]) {
+ l3_3.g[2] = l2_4.x[1] - l2_4.x[4];
+ }
+labelL5:
+ return 0;
+} /* tp53_ */
+
+
+/* Subroutine */ int tp54_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5, d__6;
+
+ /* Local variables */
+ static int i__;
+ static Real q, v1, v2, v3, v4, v5, v6, v7, v8, v9, dq[6];
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 6;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 1;
+ l1_1.nenl = 0;
+ l2_5.x[0] = 6e3;
+ l2_5.x[1] = 1.5;
+ l2_5.x[2] = 4e6;
+ l2_5.x[3] = 2.;
+ l2_5.x[4] = .003;
+ l2_5.x[5] = 5e7;
+ for (i__ = 1; i__ <= 6; ++i__) {
+ l11_5.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_5.lxu[i__ - 1] = true;
+ }
+ l13_5.xl[0] = 0.;
+ l13_5.xl[1] = -10.;
+ l13_5.xl[2] = 0.;
+ l13_5.xl[3] = 0.;
+ l13_5.xl[4] = -1.;
+ l13_5.xl[5] = 0.;
+ l14_5.xu[0] = 2e4;
+ l14_5.xu[1] = 10.;
+ l14_5.xu[2] = 1e7;
+ l14_5.xu[3] = 20.;
+ l14_5.xu[4] = 1.;
+ l14_5.xu[5] = 2e8;
+ l5_3.gg[0] = 1.;
+ l5_3.gg[1] = 4e3;
+ for (i__ = 3; i__ <= 6; ++i__) {
+/* L30: */
+ l5_3.gg[i__ - 1] = (float)0.;
+ }
+ l20_4.lex = true;
+ l20_4.nex = 1;
+ l20_4.xex[0] = 13085.714285714286;
+ l20_4.xex[1] = 1.1285714285714286;
+ l20_4.xex[2] = 2e6;
+ l20_4.xex[3] = 10.;
+ l20_4.xex[4] = .001;
+ l20_4.xex[5] = 1e8;
+ l20_4.fex = -std::exp(-.096428571428571433);
+ return 0;
+labelL2:
+ v1 = l2_5.x[0] - 1e4;
+ v2 = l2_5.x[1] - 1.;
+ v3 = l2_5.x[2] - 2e6;
+ v4 = l2_5.x[3] - 10.;
+ v5 = l2_5.x[4] - .001;
+ v6 = l2_5.x[5] - 1e8;
+ v7 = 1.0416666666666667;
+ v8 = 2.0408163265306122e-14;
+ v9 = 4.0816326530612245e-14;
+/* Computing 2nd power */
+ d__1 = v1;
+/* Computing 2nd power */
+ d__2 = v2;
+/* Computing 2nd power */
+ d__3 = l2_5.x[2] - 2e6;
+/* Computing 2nd power */
+ d__4 = l2_5.x[3] - 10.;
+/* Computing 2nd power */
+ d__5 = l2_5.x[4] - .001;
+/* Computing 2nd power */
+ d__6 = l2_5.x[5] - 1e8;
+ q = (d__1 * d__1 * 1.5625e-8 + v1 * 5e-5 * v2 + d__2 * d__2) * v7 + d__3 *
+ d__3 * v8 + d__4 * d__4 * 4e-4 + d__5 * d__5 * 400. + d__6 *
+ d__6 * 4e-18;
+/* Q = ((X(1)-1.0D6)**2/6.4D+7 + (X(1)-1.0D+4)*(X(2)-1.0D0)/2.0D4 */
+/* / + (X(2)-1.0D0)**2)*(X(3)-2.0D6)**2/(0.96*4.9D13) */
+/* / + (X(4)-1.0D1)**2/2.5D3 + (X(5)-1.0D-3)**2/2.5D-3 */
+/* / + (X(6)-1.0D8)**2/2.5D17 */
+ l6_1.fx = -std::exp(q * -.5);
+ return 0;
+labelL3:
+ v1 = l2_5.x[0] - 1e4;
+ v2 = l2_5.x[1] - 1.;
+ v3 = l2_5.x[2] - 2e6;
+ v4 = l2_5.x[3] - 10.;
+ v5 = l2_5.x[4] - .001;
+ v6 = l2_5.x[5] - 1e8;
+ v7 = 1.0416666666666667;
+ v8 = 2.0408163265306122e-14;
+ v9 = 4.0816326530612245e-14;
+/* Computing 2nd power */
+ d__1 = v1;
+/* Computing 2nd power */
+ d__2 = v2;
+/* Computing 2nd power */
+ d__3 = v3;
+/* Computing 2nd power */
+ d__4 = v4;
+/* Computing 2nd power */
+ d__5 = v5;
+/* Computing 2nd power */
+ d__6 = v6;
+ q = (d__1 * d__1 * 1.5625e-8 + v1 * 5e-5 * v2 + d__2 * d__2) * v7 + d__3 *
+ d__3 * v8 + d__4 * d__4 * 4e-4 + d__5 * d__5 * 400. + d__6 *
+ d__6 * 4e-18;
+ dq[0] = (v1 * 3.125e-8 + v2 * 5e-5) * v7;
+ dq[1] = (v1 * 5e-5 + v2 * 2.) * v7;
+ dq[2] = v3 * v9;
+ dq[3] = v4 * 8e-4;
+ dq[4] = v5 * 800.;
+ dq[5] = v6 * 8e-18;
+ for (i__ = 1; i__ <= 6; ++i__) {
+/* L31: */
+ l4_5.gf[i__ - 1] = std::exp(q * -.5) * .5 * dq[i__ - 1];
+ }
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = l2_5.x[0] + l2_5.x[1] * 4e3 - 17600.;
+ }
+labelL5:
+ return 0;
+} /* tp54_ */
+
+
+/* Subroutine */ int tp55_(int *mode)
+{
+ /* Local variables */
+ static int i__, j;
+ static Real v1;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 6;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 6;
+ l1_1.nenl = 0;
+ l2_5.x[0] = 1.;
+ l2_5.x[1] = 2.;
+ l2_5.x[2] = 0.;
+ l2_5.x[3] = 0.;
+ l2_5.x[4] = 0.;
+ l2_5.x[5] = 2.;
+ for (i__ = 1; i__ <= 6; ++i__) {
+ l11_5.lxl[i__ - 1] = true;
+ l12_5.lxu[i__ - 1] = false;
+/* labelL6: */
+ l13_5.xl[i__ - 1] = 0.;
+ }
+ l12_5.lxu[0] = true;
+ l12_5.lxu[3] = true;
+ l14_5.xu[0] = 1.;
+ l14_5.xu[3] = 1.;
+ for (i__ = 1; i__ <= 6; ++i__) {
+ for (j = 1; j <= 6; ++j) {
+/* L15: */
+ l5_10.gg[i__ + j * 6 - 7] = 0.;
+ }
+ }
+ l4_5.gf[1] = 2.;
+ l4_5.gf[2] = 0.;
+ l4_5.gf[4] = 4.;
+ l4_5.gf[5] = 0.;
+ l5_10.gg[0] = 1.;
+ l5_10.gg[6] = 2.;
+ l5_10.gg[24] = 5.;
+ l5_10.gg[1] = 1.;
+ l5_10.gg[7] = 1.;
+ l5_10.gg[13] = 1.;
+ l5_10.gg[20] = 1.;
+ l5_10.gg[26] = 1.;
+ l5_10.gg[32] = 1.;
+ l5_10.gg[3] = 1.;
+ l5_10.gg[21] = 1.;
+ l5_10.gg[10] = 1.;
+ l5_10.gg[28] = 1.;
+ l5_10.gg[17] = 1.;
+ l5_10.gg[35] = 1.;
+ l20_4.lex = true;
+ l20_4.nex = 1;
+ l20_4.xex[0] = 0.;
+ l20_4.xex[1] = 1.3333333333333333;
+ l20_4.xex[2] = 1.6666666666666667;
+ l20_4.xex[3] = 1.;
+ l20_4.xex[4] = .66666666666666663;
+ l20_4.xex[5] = .33333333333333331;
+ l20_4.fex = 6.333333333333333;
+ return 0;
+labelL2:
+ l6_1.fx = l2_5.x[0] + l2_5.x[1] * 2. + l2_5.x[4] * 4. + std::exp(l2_5.x[0] *
+ l2_5.x[3]);
+ return 0;
+labelL3:
+ v1 = std::exp(l2_5.x[0] * l2_5.x[3]);
+ l4_5.gf[0] = l2_5.x[3] * v1 + 1.;
+ l4_5.gf[3] = l2_5.x[0] * v1;
+ return 0;
+labelL4:
+ if (l9_6.index1[0]) {
+ l3_5.g[0] = l2_5.x[0] + l2_5.x[1] * 2. + l2_5.x[4] * 5. - 6.;
+ }
+ if (l9_6.index1[1]) {
+ l3_5.g[1] = l2_5.x[0] + l2_5.x[1] + l2_5.x[2] - 3.;
+ }
+ if (l9_6.index1[2]) {
+ l3_5.g[2] = l2_5.x[3] + l2_5.x[4] + l2_5.x[5] - 2.;
+ }
+ if (l9_6.index1[3]) {
+ l3_5.g[3] = l2_5.x[0] + l2_5.x[3] - 1.;
+ }
+ if (l9_6.index1[4]) {
+ l3_5.g[4] = l2_5.x[1] + l2_5.x[4] - 2.;
+ }
+ if (l9_6.index1[5]) {
+ l3_5.g[5] = l2_5.x[2] + l2_5.x[5] - 2.;
+ }
+labelL5:
+ return 0;
+} /* tp55_ */
+
+
+/* Subroutine */ int tp56_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 7;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 4;
+ l2_6.x[0] = 1.;
+ l2_6.x[1] = 1.;
+ l2_6.x[2] = 1.;
+ for (i__ = 1; i__ <= 7; ++i__) {
+ l11_6.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_6.lxu[i__ - 1] = false;
+ }
+ l2_6.x[3] = std::asin(std::sqrt(.23809523809523808));
+ l2_6.x[4] = l2_6.x[3];
+ l2_6.x[5] = l2_6.x[3];
+ l2_6.x[6] = std::asin(std::sqrt(.69444444444444442));
+ for (i__ = 4; i__ <= 7; ++i__) {
+/* L30: */
+ l4_6.gf[i__ - 1] = 0.;
+ }
+ for (i__ = 1; i__ <= 4; ++i__) {
+ for (j = 1; j <= 7; ++j) {
+/* L15: */
+ l5_11.gg[i__ + (j << 2) - 5] = 0.;
+ }
+ }
+ l5_11.gg[0] = 1.;
+ l5_11.gg[5] = 1.;
+ l5_11.gg[10] = 1.;
+ l5_11.gg[3] = 1.;
+ l5_11.gg[7] = 2.;
+ l5_11.gg[11] = 2.;
+ l20_8.lex = true;
+ l20_8.nex = -1;
+ l20_8.xex[0] = 2.4;
+ l20_8.xex[1] = 1.2;
+ l20_8.xex[2] = 1.2;
+ l20_8.xex[3] = std::asin(std::sqrt(.5714285714285714));
+ l20_8.xex[4] = std::asin(std::sqrt(.2857142857142857));
+ l20_8.xex[5] = l20_8.xex[4];
+ l20_8.xex[6] = std::atan(1.) * 2.;
+ l20_8.fex = -3.456;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_6.x[0] * l2_6.x[1] * l2_6.x[2];
+ return 0;
+labelL3:
+ l4_6.gf[0] = -l2_6.x[1] * l2_6.x[2];
+ l4_6.gf[1] = -l2_6.x[0] * l2_6.x[2];
+ l4_6.gf[2] = -l2_6.x[0] * l2_6.x[1];
+ return 0;
+labelL4:
+ if (l9_7.index1[0]) {
+/* Computing 2nd power */
+ d__1 = std::sin(l2_6.x[3]);
+ l3_6.g[0] = l2_6.x[0] - d__1 * d__1 * 4.2;
+ }
+ if (l9_7.index1[1]) {
+/* Computing 2nd power */
+ d__1 = std::sin(l2_6.x[4]);
+ l3_6.g[1] = l2_6.x[1] - d__1 * d__1 * 4.2;
+ }
+ if (l9_7.index1[2]) {
+/* Computing 2nd power */
+ d__1 = std::sin(l2_6.x[5]);
+ l3_6.g[2] = l2_6.x[2] - d__1 * d__1 * 4.2;
+ }
+ if (l9_7.index1[3]) {
+/* Computing 2nd power */
+ d__1 = std::sin(l2_6.x[6]);
+ l3_6.g[3] = l2_6.x[0] + l2_6.x[1] * 2. + l2_6.x[2] * 2. - d__1 * d__1
+ * 7.2;
+ }
+ return 0;
+labelL5:
+ if (l10_7.index2[0]) {
+ l5_11.gg[12] = std::sin(l2_6.x[3]) * -8.4 * std::cos(l2_6.x[3]);
+ }
+ if (l10_7.index2[1]) {
+ l5_11.gg[17] = std::sin(l2_6.x[4]) * -8.4 * std::cos(l2_6.x[4]);
+ }
+ if (l10_7.index2[2]) {
+ l5_11.gg[22] = std::sin(l2_6.x[5]) * -8.4 * std::cos(l2_6.x[5]);
+ }
+ if (l10_7.index2[3]) {
+ l5_11.gg[27] = std::sin(l2_6.x[6]) * -14.4 * std::cos(l2_6.x[6]);
+ }
+ return 0;
+} /* tp56_ */
+
+
+/* Subroutine */ int tp57_(int *mode)
+{
+ /* System generated locals */
+ int i__1;
+ Real d__1;
+
+ /* Local variables */
+ static Real a[44], b[44], f[44];
+ static int i__, j;
+ static Real s[2], t, v1, df[88] /* was [44][2] */;
+
+ if (*mode - 2 >= 0) {
+ goto L18;
+ } else {
+ goto labelL1;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = .42;
+ l2_1.x[1] = 5.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l13_1.xl[0] = .4;
+ l13_1.xl[1] = -4.;
+ l20_1.lex = false;
+ l20_1.xex[0] = .419952674511;
+ l20_1.xex[1] = 1.2848456293;
+ l20_1.fex = 2.8459669721299998;
+ return 0;
+L18:
+ for (i__ = 1; i__ <= 2; ++i__) {
+ a[i__ - 1] = 8.;
+ a[i__ + 15] = 18.;
+ a[i__ + 29] = 28.;
+ a[i__ + 34] = 32.;
+ a[i__ + 37] = 36.;
+ a[i__ + 39] = 38.;
+ b[i__ - 1] = .49;
+ b[i__ + 5] = .46;
+ b[i__ + 10] = .43;
+ b[i__ + 13] = .43;
+ b[i__ + 17] = .42;
+ b[i__ + 20] = .41;
+ b[i__ + 24] = .4;
+ b[i__ + 28] = .41;
+ b[i__ + 35] = .4;
+ b[i__ + 39] = .4;
+/* labelL20: */
+ b[i__ + 41] = .39;
+ }
+ for (i__ = 1; i__ <= 3; ++i__) {
+ a[i__ + 9] = 14.;
+ a[i__ + 12] = 16.;
+ a[i__ + 17] = 20.;
+ a[i__ + 20] = 22.;
+ a[i__ + 23] = 24.;
+ a[i__ + 26] = 26.;
+ a[i__ + 31] = 30.;
+/* L21: */
+ b[i__ + 30] = .4;
+ }
+ for (i__ = 1; i__ <= 4; ++i__) {
+ a[i__ + 1] = 10.;
+/* L22: */
+ a[i__ + 5] = 12.;
+ }
+ a[37] = 34.;
+ a[42] = 40.;
+ a[43] = 42.;
+ b[2] = .48;
+ b[3] = .47;
+ b[4] = .48;
+ b[5] = .47;
+ b[8] = .45;
+ b[9] = .43;
+ b[10] = .45;
+ b[13] = .44;
+ b[16] = .46;
+ b[17] = .45;
+ b[20] = .43;
+ b[23] = .4;
+ b[24] = .42;
+ b[27] = .41;
+ b[28] = .4;
+ b[34] = .38;
+ b[35] = .41;
+ b[38] = .41;
+ b[39] = .38;
+ if ((i__1 = *mode - 4) < 0) {
+ goto L17;
+ } else if (i__1 == 0) {
+ goto labelL4;
+ } else {
+ goto labelL5;
+ }
+L17:
+ for (i__ = 1; i__ <= 44; ++i__) {
+/* L30: */
+ f[i__ - 1] = b[i__ - 1] - l2_1.x[0] - (.49 - l2_1.x[0]) * std::exp(-l2_1.x[
+ 1] * (a[i__ - 1] - 8.));
+ }
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ }
+labelL2:
+ t = 0.;
+ for (i__ = 1; i__ <= 44; ++i__) {
+/* L19: */
+/* Computing 2nd power */
+ d__1 = f[i__ - 1];
+ t += d__1 * d__1;
+ }
+ l6_1.fx = t * (float)100.;
+ return 0;
+labelL3:
+ s[0] = 0.;
+ s[1] = 0.;
+ for (i__ = 1; i__ <= 44; ++i__) {
+ v1 = std::exp(-l2_1.x[1] * (a[i__ - 1] - 8.));
+ df[i__ - 1] = v1 - 1.;
+ df[i__ + 43] = (a[i__ - 1] - 8.) * (.49 - l2_1.x[0]) * v1;
+ for (j = 1; j <= 2; ++j) {
+/* L32: */
+ s[j - 1] += f[i__ - 1] * 2. * df[i__ + j * 44 - 45];
+ }
+/* L31: */
+ }
+ l4_1.gf[0] = s[0] * (float)100.;
+ l4_1.gf[1] = s[1] * (float)100.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = -l2_1.x[0] * l2_1.x[1] + l2_1.x[1] * .49 - .09;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+ l5_1.gg[0] = -l2_1.x[1];
+ l5_1.gg[1] = -l2_1.x[0] + .49;
+L7:
+ return 0;
+} /* tp57_ */
+
+
+/* Subroutine */ int tp58_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 3;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = -2.;
+ l2_1.x[1] = 1.;
+ l11_1.lxl[0] = true;
+ l12_1.lxu[0] = true;
+ l11_1.lxl[1] = false;
+ l12_1.lxu[1] = false;
+ l13_1.xl[0] = -.5;
+ l14_1.xu[0] = .5;
+ l5_3.gg[0] = -1.;
+ l5_3.gg[4] = -1.;
+ l20_1.lex = false;
+ l20_1.xex[0] = -.786150483331;
+ l20_1.xex[1] = .618034533851;
+ l20_1.fex = 3.19033354957;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = 1. - l2_1.x[0];
+ l6_1.fx = d__1 * d__1 * 100. + d__3 * d__3;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[1] = (l2_1.x[1] - d__1 * d__1) * 200.;
+ l4_1.gf[0] = (l2_1.x[0] * (l4_1.gf[1] - 1.) + 1.) * -2.;
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+ l3_3.g[0] = d__1 * d__1 - l2_1.x[0];
+ }
+ if (l9_4.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l3_3.g[1] = d__1 * d__1 - l2_1.x[1];
+ }
+ if (l9_4.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_3.g[2] = d__1 * d__1 + d__2 * d__2 - 1.;
+ }
+ return 0;
+labelL5:
+ if (l10_4.index2[0]) {
+ l5_3.gg[3] = l2_1.x[1] * 2.;
+ }
+ if (l10_4.index2[1]) {
+ l5_3.gg[1] = l2_1.x[0] * 2.;
+ }
+ if (! l10_4.index2[2]) {
+ goto labelL9;
+ }
+ l5_3.gg[2] = l2_1.x[0] * 2.;
+ l5_3.gg[5] = l2_1.x[1] * 2.;
+labelL9:
+ return 0;
+} /* tp58_ */
+
+
+/* Subroutine */ int tp59_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+ static Real x11, x12, x13, x14, x21, x22, x23, x24, xx12, xx21,
+ xx31;
+ static Real xx11;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 3;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 90.;
+ l2_1.x[1] = 10.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = true;
+ l12_1.lxu[i__ - 1] = true;
+/* labelL6: */
+ l13_1.xl[i__ - 1] = 0.;
+ }
+ l14_1.xu[0] = 75.;
+ l14_1.xu[1] = 65.;
+ l5_3.gg[4] = 1.;
+ l5_3.gg[2] = -5.;
+ l20_1.lex = false;
+/* XEX(1) = 0.463995762710D+2 */
+/* XEX(2) = 0.522196899513D+2 */
+/* FEX = -0.675456604292D+1 */
+ l20_1.xex[0] = 13.5501042366;
+ l20_1.xex[1] = 51.6601812877;
+ l20_1.fex = -7.80422632408;
+ return 0;
+labelL2:
+ x11 = l2_1.x[0];
+ x12 = x11 * x11;
+ x13 = x12 * x11;
+ x14 = x13 * x11;
+ x21 = l2_1.x[1];
+ x22 = x21 * x21;
+ x23 = x22 * x21;
+ x24 = x23 * x21;
+ l6_1.fx = x11 * 3.8112 - 75.196 - x12 * .12694 + x13 * .0020567 - x14 *
+ 1.0345e-5 + x21 * 6.8306 - x11 * .030234 * x21 + x12 * .00128134 *
+ x21 - x13 * 3.5256e-5 * x21 + x14 * 2.266e-7 * x21 - x22 *
+ .25645 + x23 * .0034604 - x24 * 1.3514e-5 + 28.106 / (x21 + 1.) +
+ x12 * 5.2375e-6 * x22 + x13 * 6.3e-8 * x22 - x13 * 7e-10 * x23 -
+ x11 * 3.4054e-4 * x22 + x11 * 1.6638e-6 * x23 + std::exp(x11 * 5e-4 *
+ x21) * 2.8673;
+ return 0;
+labelL3:
+ x11 = l2_1.x[0];
+ x12 = x11 * x11;
+ x13 = x12 * x11;
+ x14 = x13 * x11;
+ x21 = l2_1.x[1];
+ x22 = x21 * x21;
+ x23 = x22 * x21;
+ xx11 = x11 * x21;
+ xx12 = x11 * x22;
+ xx21 = x12 * x21;
+ xx31 = x13 * x21;
+ l4_1.gf[0] = 3.8112 - x11 * .25388 + x12 * .0061701 - x13 * 4.138e-5 -
+ x21 * .030234 + xx11 * .00256268 - xx21 * 1.05768e-4 + xx31 *
+ 9.064e-7 + xx12 * 1.0475e-5 + x12 * 1.89e-7 * x22 - x12 * 2.1e-9 *
+ x23 - x22 * 3.4054e-4 + x23 * 1.6638e-6 + x21 * .00143365 * std::exp(
+ xx11 * 5e-4);
+/* Computing 2nd power */
+ d__1 = x21 + 1.;
+ l4_1.gf[1] = 6.8306 - x11 * .030234 + x12 * .00128134 - x13 * 3.5256e-5 +
+ x14 * 2.266e-7 - x21 * .5129 + x22 * .0103812 - x23 * 5.4056e-5 -
+ 28.106 / (d__1 * d__1) + xx21 * 1.0475e-5 + xx31 * 1.26e-7 - x13 *
+ 2.1e-9 * x22 - xx11 * 6.8108e-4 + xx12 * 4.9914e-6 + x11 *
+ .00143365 * std::exp(xx11 * 5e-4);
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = l2_1.x[0] * l2_1.x[1] - 700.;
+ }
+ if (l9_4.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l3_3.g[1] = l2_1.x[1] - d__1 * d__1 * .008;
+ }
+ if (l9_4.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - 50.;
+ l3_3.g[2] = d__1 * d__1 - (l2_1.x[0] - 55.) * 5.;
+ }
+ return 0;
+labelL5:
+ if (! l10_4.index2[0]) {
+ goto L7;
+ }
+ l5_3.gg[0] = l2_1.x[1];
+ l5_3.gg[3] = l2_1.x[0];
+L7:
+ if (l10_4.index2[1]) {
+ l5_3.gg[1] = l2_1.x[0] * -.016;
+ }
+ if (l10_4.index2[2]) {
+ l5_3.gg[5] = (l2_1.x[1] - 50.) * 2.;
+ }
+ return 0;
+} /* tp59_ */
+
+
+/* Subroutine */ int tp60_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+ static Real v1;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 2.;
+ l11_2.lxl[i__ - 1] = true;
+ l12_2.lxu[i__ - 1] = true;
+ l13_2.xl[i__ - 1] = -10.;
+/* labelL6: */
+ l14_2.xu[i__ - 1] = 10.;
+ }
+ l20_3.lex = false;
+ l20_3.xex[0] = 1.10485902423;
+ l20_3.xex[1] = 1.19667419413;
+ l20_3.xex[2] = 1.53526225739;
+ l20_3.fex = .0325682002513;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0] - 1.;
+/* Computing 2nd power */
+ d__2 = l2_2.x[0] - l2_2.x[1];
+/* Computing 4th power */
+ d__3 = l2_2.x[1] - l2_2.x[2], d__3 *= d__3;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3;
+ return 0;
+labelL3:
+ v1 = (l2_2.x[0] - l2_2.x[1]) * 2.;
+ l4_2.gf[0] = (l2_2.x[0] - 1.) * 2. + v1;
+/* Computing 3rd power */
+ d__1 = l2_2.x[1] - l2_2.x[2];
+ l4_2.gf[2] = d__1 * (d__1 * d__1) * -4.;
+ l4_2.gf[1] = -l4_2.gf[2] - v1;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[1];
+/* Computing 4th power */
+ d__2 = l2_2.x[2], d__2 *= d__2;
+ l3_1.g[0] = l2_2.x[0] * (d__1 * d__1 + 1.) + d__2 * d__2 - 4. - std::sqrt( 2.) * 3.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+/* Computing 2nd power */
+ d__1 = l2_2.x[1];
+ l5_5.gg[0] = d__1 * d__1 + 1.;
+ l5_5.gg[1] = l2_2.x[0] * 2. * l2_2.x[1];
+/* Computing 3rd power */
+ d__1 = l2_2.x[2];
+ l5_5.gg[2] = d__1 * (d__1 * d__1) * 4.;
+L7:
+ return 0;
+} /* tp60_ */
+
+
+/* Subroutine */ int tp61_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 2;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 0.;
+ l11_2.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = false;
+ }
+ l20_3.lex = false;
+ l20_3.xex[0] = 5.32677015744;
+ l20_3.xex[1] = -2.11899863998;
+ l20_3.xex[2] = 3.21046423906;
+ l20_3.fex = -143.646142201;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2];
+ l6_1.fx = d__1 * d__1 * 4. + d__2 * d__2 * 2. + d__3 * d__3 * 2. - l2_2.x[
+ 0] * 33. + l2_2.x[1] * 16. - l2_2.x[2] * 24.;
+ return 0;
+labelL3:
+ l4_2.gf[0] = l2_2.x[0] * 8. - 33.;
+ l4_2.gf[1] = l2_2.x[1] * 4. + 16.;
+ l4_2.gf[2] = l2_2.x[2] * 4. - 24.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[1];
+ l3_2.g[0] = l2_2.x[0] * 3. - d__1 * d__1 * 2. - 7.;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[2];
+ l3_2.g[1] = l2_2.x[0] * 4. - d__1 * d__1 - 11.;
+ }
+ return 0;
+labelL5:
+ if (l10_3.index2[0]) {
+ l5_3.gg[2] = l2_2.x[1] * -4.;
+ }
+ if (l10_3.index2[1]) {
+ l5_3.gg[5] = l2_2.x[2] * -2.;
+ }
+ l5_3.gg[0] = 3.;
+ l5_3.gg[4] = 0.;
+ l5_3.gg[1] = 4.;
+ l5_3.gg[3] = 0.;
+ return 0;
+} /* tp61_ */
+
+
+/* Subroutine */ int tp62_(int *mode)
+{
+ /* System generated locals */
+ int i__1;
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+ static Real s, b1, b2, b3, c1, c2, c3, v1, v2, v3, v4, v5, v6, v7,
+ rb1, rb2, rb3, rc1, rc2, rc3;
+
+ if (*mode - 2 >= 0) {
+ goto L17;
+ } else {
+ goto labelL1;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 1;
+ l1_1.nenl = 0;
+ l2_2.x[0] = .7;
+ l2_2.x[1] = .2;
+ l2_2.x[2] = .1;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_2.lxl[i__ - 1] = true;
+ l12_2.lxu[i__ - 1] = true;
+ l13_2.xl[i__ - 1] = 0.;
+ l14_2.xu[i__ - 1] = 1.;
+/* labelL6: */
+ l5_5.gg[i__ - 1] = 1.;
+ }
+ l20_3.lex = false;
+ l20_3.xex[0] = .61781329821;
+ l20_3.xex[1] = .328202155786;
+ l20_3.xex[2] = .0539845460119;
+ l20_3.fex = -26272.5144873;
+ return 0;
+L17:
+ if ((i__1 = *mode - 4) < 0) {
+ goto L18;
+ } else if (i__1 == 0) {
+ goto labelL4;
+ } else {
+ goto labelL5;
+ }
+L18:
+ b3 = l2_2.x[2] + .03;
+ c3 = l2_2.x[2] * .13 + .03;
+ b2 = b3 + l2_2.x[1];
+ c2 = b3 + l2_2.x[1] * .07;
+ b1 = b2 + l2_2.x[0];
+ c1 = b2 + l2_2.x[0] * .09;
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ }
+labelL2:
+ v5 = b1 / c1;
+ v6 = b2 / c2;
+ v7 = b3 / c3;
+ if (v5 <= 0. || v6 <= 0. || v7 <= 0.) {
+ goto L7;
+ }
+ l6_1.fx = (std::log(v5) * 255. + std::log(v6) * 280. + std::log(v7) * 290.) * -32.174;
+ return 0;
+L7:
+ s = 0.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L8: */
+/* Computing 2nd power */
+ d__1 = l2_2.x[i__ - 1] - 5.;
+ s += d__1 * d__1;
+ }
+ l6_1.fx = s + 1e3 - 26700.;
+ return 0;
+labelL3:
+ rb1 = 1. / b1;
+ rb2 = 1. / b2;
+ rb3 = 1. / b3;
+ rc1 = 1. / c1;
+ rc2 = 1. / c2;
+ rc3 = 1. / c3;
+ v1 = -8204.369999999999;
+ v2 = -9008.7199999999993;
+ v3 = -9330.4599999999991;
+ v4 = v1 * (rb1 - rc1);
+ l4_2.gf[0] = v1 * (rb1 - rc1 * .09);
+ l4_2.gf[1] = v4 + v2 * (rb2 - rc2 * .07);
+ l4_2.gf[2] = v4 + v2 * (rb2 - rc2) + v3 * (rb3 - rc3 * .13);
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = l2_2.x[0] + l2_2.x[1] + l2_2.x[2] - 1.;
+ }
+labelL5:
+ return 0;
+} /* tp62_ */
+
+
+/* Subroutine */ int tp63_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 1;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 2.;
+ l11_2.lxl[i__ - 1] = true;
+ l12_2.lxu[i__ - 1] = false;
+/* labelL6: */
+ l13_2.xl[i__ - 1] = 0.;
+ }
+ l5_3.gg[0] = 8.;
+ l5_3.gg[2] = 14.;
+ l5_3.gg[4] = 7.;
+ l20_3.lex = false;
+ l20_3.xex[0] = 3.51211841492;
+ l20_3.xex[1] = .216988174172;
+ l20_3.xex[2] = 3.55217403459;
+ l20_3.fex = 961.715172127;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2];
+ l6_1.fx = 1e3 - d__1 * d__1 - d__2 * d__2 * 2. - d__3 * d__3 - l2_2.x[0] *
+ l2_2.x[1] - l2_2.x[0] * l2_2.x[2];
+ return 0;
+labelL3:
+ l4_2.gf[0] = l2_2.x[0] * -2. - l2_2.x[1] - l2_2.x[2];
+ l4_2.gf[1] = l2_2.x[1] * -4. - l2_2.x[0];
+ l4_2.gf[2] = l2_2.x[2] * -2. - l2_2.x[0];
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = l2_2.x[0] * 8. + l2_2.x[1] * 14. + l2_2.x[2] * 7. - 56.;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2];
+ l3_2.g[1] = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 - 25.;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+ l5_3.gg[1] = l2_2.x[0] * 2.;
+ l5_3.gg[3] = l2_2.x[1] * 2.;
+ l5_3.gg[5] = l2_2.x[2] * 2.;
+L8:
+ return 0;
+} /* tp63_ */
+
+
+/* Subroutine */ int tp64_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 1.;
+ l11_2.lxl[i__ - 1] = true;
+ l12_2.lxu[i__ - 1] = false;
+/* labelL6: */
+ l13_2.xl[i__ - 1] = 1e-5;
+ }
+ l20_3.lex = false;
+ l20_3.xex[0] = 108.734717597;
+ l20_3.xex[1] = 85.1261394257;
+ l20_3.xex[2] = 204.324707858;
+ l20_3.fex = 6299.84242821;
+ return 0;
+labelL2:
+ l6_1.fx = l2_2.x[0] * 5. + 5e4 / l2_2.x[0] + l2_2.x[1] * 20. + 7.2e4 /
+ l2_2.x[1] + l2_2.x[2] * 10. + 1.44e5 / l2_2.x[2];
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+ l4_2.gf[0] = 5. - 5e4 / (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = l2_2.x[1];
+ l4_2.gf[1] = 20. - 7.2e4 / (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = l2_2.x[2];
+ l4_2.gf[2] = 10. - 1.44e5 / (d__1 * d__1);
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = 1. - 4. / l2_2.x[0] - 32. / l2_2.x[1] - 120. / l2_2.x[2];
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+ l5_5.gg[0] = 4. / (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = l2_2.x[1];
+ l5_5.gg[1] = 32. / (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = l2_2.x[2];
+ l5_5.gg[2] = 120. / (d__1 * d__1);
+L7:
+ return 0;
+} /* tp64_ */
+
+
+/* Subroutine */ int tp65_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+ static Real v1, v2;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_2.x[0] = -5.;
+ l2_2.x[1] = 5.;
+ l2_2.x[2] = 0.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_2.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = true;
+ }
+ l13_2.xl[0] = -4.5;
+ l13_2.xl[1] = -4.5;
+ l13_2.xl[2] = -5.;
+ l14_2.xu[0] = 4.5;
+ l14_2.xu[1] = 4.5;
+ l14_2.xu[2] = 5.;
+ l20_3.lex = false;
+ l20_3.xex[0] = 3.65046182158;
+ l20_3.xex[1] = 3.6504616894;
+ l20_3.xex[2] = 4.62041750754;
+ l20_3.fex = .953528856757;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0] - l2_2.x[1];
+/* Computing 2nd power */
+ d__2 = (l2_2.x[0] + l2_2.x[1] - 10.) / 3.;
+/* Computing 2nd power */
+ d__3 = l2_2.x[2] - 5.;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3;
+ return 0;
+labelL3:
+ v1 = (l2_2.x[0] - l2_2.x[1]) * 2.;
+ v2 = (l2_2.x[0] + l2_2.x[1] - 10.) * 2. / 9.;
+ l4_2.gf[0] = v1 + v2;
+ l4_2.gf[1] = -v1 + v2;
+ l4_2.gf[2] = (l2_2.x[2] - 5.) * 2.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2];
+ l3_1.g[0] = 48. - d__1 * d__1 - d__2 * d__2 - d__3 * d__3;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+ l5_5.gg[0] = l2_2.x[0] * -2.;
+ l5_5.gg[1] = l2_2.x[1] * -2.;
+ l5_5.gg[2] = l2_2.x[2] * -2.;
+L7:
+ return 0;
+} /* tp65_ */
+
+
+/* Subroutine */ int tp66_(int *mode)
+{
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_2.x[0] = 0.;
+ l2_2.x[1] = 1.05;
+ l2_2.x[2] = 2.9;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_2.lxl[i__ - 1] = true;
+ l12_2.lxu[i__ - 1] = true;
+ l13_2.xl[i__ - 1] = 0.;
+/* labelL6: */
+ l14_2.xu[i__ - 1] = 100.;
+ }
+ l14_2.xu[2] = 10.;
+ l4_2.gf[0] = -.8;
+ l4_2.gf[1] = 0.;
+ l4_2.gf[2] = .2;
+ l5_3.gg[2] = 1.;
+ l5_3.gg[4] = 0.;
+ l5_3.gg[1] = 0.;
+ l5_3.gg[5] = 1.;
+ l20_3.lex = false;
+ l20_3.xex[0] = .184126487951;
+ l20_3.xex[1] = 1.20216787321;
+ l20_3.xex[2] = 3.32732232258;
+ l20_3.fex = .518163274159;
+ return 0;
+labelL2:
+ l6_1.fx = l2_2.x[2] * .2 - l2_2.x[0] * .8;
+labelL3:
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = l2_2.x[1] - std::exp(l2_2.x[0]);
+ }
+ if (l9_3.index1[1]) {
+ l3_2.g[1] = l2_2.x[2] - std::exp(l2_2.x[1]);
+ }
+ return 0;
+labelL5:
+ if (l10_3.index2[0]) {
+ l5_3.gg[0] = -std::exp(l2_2.x[0]);
+ }
+ if (l10_3.index2[1]) {
+ l5_3.gg[3] = -std::exp(l2_2.x[1]);
+ }
+ return 0;
+} /* tp66_ */
+
+
+/* Subroutine */ int tp67_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real a[3];
+ static int i__, j;
+ static Real y[8], v1, v2, v3, dy[24] /* was [8][3] */, rx, y2c,
+ y4c, dy2c[3], dy4c[3];
+
+ if (*mode - 2 >= 0) {
+ goto L17;
+ } else {
+ goto labelL1;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 14;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_2.x[0] = 1745.;
+ l2_2.x[1] = 1.2e4;
+ l2_2.x[2] = 110.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_2.lxl[i__ - 1] = true;
+ l12_2.lxu[i__ - 1] = true;
+/* labelL6: */
+ l13_2.xl[i__ - 1] = 1e-5;
+ }
+ l14_2.xu[0] = 2e3;
+ l14_2.xu[1] = 1.6e4;
+ l14_2.xu[2] = 120.;
+ l20_3.lex = false;
+ l20_3.xex[0] = 1728.37128614;
+ l20_3.xex[1] = 1.6e4;
+ l20_3.xex[2] = 98.1415140238;
+ l20_3.fex = -1162.03650728;
+ return 0;
+L17:
+ rx = 1. / l2_2.x[0];
+ y[1] = l2_2.x[0] * 1.6;
+ dy[1] = 1.6;
+ dy[9] = 0.;
+ dy[17] = 0.;
+L100:
+ y[2] = y[1] * 1.22 - l2_2.x[0];
+ dy[2] = dy[1] * 1.22 - 1.;
+ dy[10] = dy[9] * 1.22;
+ dy[18] = dy[17] * 1.22;
+ y[5] = (l2_2.x[1] + y[2]) * rx;
+/* Computing 2nd power */
+ d__1 = rx;
+ dy[5] = (l2_2.x[0] * dy[2] - l2_2.x[1] - y[2]) * (d__1 * d__1);
+ dy[13] = (dy[10] + 1.) * rx;
+ dy[21] = dy[2] * rx;
+ v1 = l2_2.x[0] * .01 * (13.167 - y[5] * 1.3333999999999999);
+ v2 = ((13.167 - y[5] * .6667) * y[5] + 112.) * .01;
+ y2c = l2_2.x[0] * v2;
+ dy2c[0] = v2 + v1 * dy[5];
+ dy2c[1] = v1 * dy[13];
+ dy2c[2] = v1 * dy[21];
+ if ((d__1 = y2c - y[1], std::abs(d__1)) - .001 <= 0.) {
+ goto L102;
+ } else {
+ goto L101;
+ }
+L101:
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L103: */
+ dy[(i__ << 3) - 7] = dy2c[i__ - 1];
+ }
+ y[1] = y2c;
+ goto L100;
+L102:
+ y[3] = 93.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L104: */
+ dy[(i__ << 3) - 5] = 0.;
+ }
+L105:
+/* Computing 2nd power */
+ d__1 = y[5];
+ y[4] = y[5] * 1.098 + 86.35 - d__1 * d__1 * .038 + (y[3] - 89.) * .325;
+ y[7] = y[4] * 3. - 133.;
+ y[6] = 35.82 - y[7] * .222;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ dy[(i__ << 3) - 4] = dy[(i__ << 3) - 3] * 1.098 - y[5] * .076 * dy[(
+ i__ << 3) - 3] + dy[(i__ << 3) - 5] * .325;
+ dy[(i__ << 3) - 1] = dy[(i__ << 3) - 4] * 3.;
+/* L106: */
+ dy[(i__ << 3) - 2] = dy[(i__ << 3) - 1] * -.222;
+ }
+ v3 = 1. / (y[1] * y[6] + l2_2.x[2] * 1e3);
+ y4c = l2_2.x[2] * 9.8e4 * v3;
+ for (i__ = 1; i__ <= 2; ++i__) {
+/* L107: */
+/* Computing 2nd power */
+ d__1 = y[1] * y[6] + l2_2.x[2] * 1e3;
+ dy4c[i__ - 1] = l2_2.x[2] * -9.8e4 * (y[1] * dy[(i__ << 3) - 2] + y[6]
+ * dy[(i__ << 3) - 7]) / (d__1 * d__1);
+ }
+/* Computing 2nd power */
+ d__1 = v3;
+ dy4c[2] = (y[1] * y[6] - l2_2.x[2] * (y[1] * dy[22] + y[6] * dy[17])) *
+ 9.8e4 * (d__1 * d__1);
+ if ((d__1 = y4c - y[3], std::abs(d__1)) - 1e-4 <= 0.) {
+ goto L109;
+ } else {
+ goto L108;
+ }
+L108:
+ y[3] = y4c;
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L110: */
+ dy[(i__ << 3) - 5] = dy4c[i__ - 1];
+ }
+ goto L105;
+L109:
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL2:
+ l6_1.fx = -(y[1] * .063 * y[4] - l2_2.x[0] * 5.04 - y[2] * 3.36 - l2_2.x[
+ 1] * .035 - l2_2.x[2] * 10.);
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L120: */
+ a[i__ - 1] = (dy[(i__ << 3) - 7] * y[4] + dy[(i__ << 3) - 4] * y[1]) *
+ -.063 + dy[(i__ << 3) - 6] * 3.36;
+ }
+ l4_2.gf[0] = a[0] + 5.04;
+ l4_2.gf[1] = a[1] + .035;
+ l4_2.gf[2] = a[2] + 10.;
+ return 0;
+labelL4:
+ if (l9_8.index1[0]) {
+ l3_7.g[0] = y[1];
+ }
+ if (l9_8.index1[1]) {
+ l3_7.g[1] = y[2];
+ }
+ if (l9_8.index1[2]) {
+ l3_7.g[2] = y[3] - 85.;
+ }
+ if (l9_8.index1[3]) {
+ l3_7.g[3] = y[4] - 90.;
+ }
+ if (l9_8.index1[4]) {
+ l3_7.g[4] = y[5] - 3.;
+ }
+ if (l9_8.index1[5]) {
+ l3_7.g[5] = y[6] - .01;
+ }
+ if (l9_8.index1[6]) {
+ l3_7.g[6] = y[7] - 145.;
+ }
+ if (l9_8.index1[7]) {
+ l3_7.g[7] = 5e3 - y[1];
+ }
+ if (l9_8.index1[8]) {
+ l3_7.g[8] = 2e3 - y[2];
+ }
+ if (l9_8.index1[9]) {
+ l3_7.g[9] = 93. - y[3];
+ }
+ if (l9_8.index1[10]) {
+ l3_7.g[10] = 95. - y[4];
+ }
+ if (l9_8.index1[11]) {
+ l3_7.g[11] = 12. - y[5];
+ }
+ if (l9_8.index1[12]) {
+ l3_7.g[12] = 4. - y[6];
+ }
+ if (l9_8.index1[13]) {
+ l3_7.g[13] = 162. - y[7];
+ }
+ return 0;
+labelL5:
+ for (j = 1; j <= 7; ++j) {
+ if (! l10_8.index2[j - 1]) {
+ goto L131;
+ }
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L130: */
+ l5_12.gg[j + i__ * 14 - 15] = dy[j + 1 + (i__ << 3) - 9];
+ }
+L131:
+ if (! l10_8.index2[j + 6]) {
+ goto L133;
+ }
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L132: */
+ l5_12.gg[j + 7 + i__ * 14 - 15] = -dy[j + 1 + (i__ << 3) - 9];
+ }
+L133:
+ ;
+ }
+ return 0;
+} /* tp67_ */
+
+
+/* Subroutine */ int tp68_0_(int n__, int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static Real a[2], b[2], d__[2];
+ static int i__;
+ static Real z__[2], h1, h2, h3, v1, v2, v3, v4, v5;
+ int mdnord_(Real*, Real*);
+ static int kn1;
+
+ switch(n__) {
+ case 1: goto L_tp69;
+ }
+
+ kn1 = 1;
+ l20_6.xex[0] = .0678587452312;
+ l20_6.xex[1] = 3.64617174165;
+ l20_6.xex[2] = 2.66175189694e-4;
+ l20_6.xex[3] = .894862212037;
+ l20_6.fex = -.920425020704;
+ goto labelL9;
+
+L_tp69:
+ kn1 = 2;
+ l20_6.xex[0] = .029371418083;
+ l20_6.xex[1] = 1.19025343488;
+ l20_6.xex[2] = .233946796758;
+ l20_6.xex[3] = .791667815694;
+ l20_6.fex = -956.712887064;
+labelL9:
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 2;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_3.x[i__ - 1] = 1.;
+ l2_3.x[i__ + 1] = 1.;
+ l11_3.lxl[i__ - 1] = true;
+ l11_3.lxl[i__ + 1] = true;
+ l12_3.lxu[i__ - 1] = true;
+ l12_3.lxu[i__ + 1] = true;
+ l13_3.xl[i__ + 1] = 0.;
+ l14_3.xu[i__ - 1] = 100.;
+/* labelL6: */
+ l14_3.xu[i__ + 1] = 2.;
+ }
+ l13_3.xl[0] = 1e-4;
+ l13_3.xl[1] = 0.;
+ a[0] = 1e-4;
+ a[1] = .1;
+ b[0] = 1.;
+ b[1] = 1e3;
+ d__[0] = 1.;
+ d__[1] = 1.;
+ z__[0] = 24.;
+ z__[1] = 4.;
+ l5_6.gg[0] = 0.;
+ l5_6.gg[6] = 0.;
+ l5_6.gg[4] = 1.;
+ l5_6.gg[1] = 0.;
+ l5_6.gg[5] = 0.;
+ l5_6.gg[7] = 1.;
+ l4_3.gf[1] = 0.;
+ l20_6.lex = false;
+ return 0;
+labelL2:
+ v1 = std::exp(l2_3.x[0]) - 1.;
+ l6_1.fx = (a[kn1 - 1] * z__[kn1 - 1] - l2_3.x[3] * (b[kn1 - 1] * v1 -
+ l2_3.x[2]) / (v1 + l2_3.x[3])) / l2_3.x[0];
+ return 0;
+labelL3:
+ v1 = std::exp(l2_3.x[0]);
+ v2 = v1 - 1.;
+ v3 = 1. / (v2 + l2_3.x[3]);
+ v4 = 1. / l2_3.x[0];
+ v5 = (b[kn1 - 1] * v2 - l2_3.x[2]) * v4;
+ l4_3.gf[0] = -((v1 * (l2_3.x[3] * b[kn1 - 1] + l2_3.x[2]) * v3 - v5) *
+ l2_3.x[3] * v3 + a[kn1 - 1] * z__[kn1 - 1] * v4) * v4;
+ l4_3.gf[2] = l2_3.x[3] * v4 * v3;
+/* Computing 2nd power */
+ d__1 = v3;
+ l4_3.gf[3] = -v5 * v2 * (d__1 * d__1);
+ return 0;
+labelL4:
+ if (! l9_3.index1[0]) {
+ goto L30;
+ }
+ d__1 = -l2_3.x[1];
+ mdnord_(&d__1, &h1);
+ l3_2.g[0] = l2_3.x[2] - h1 * 2.;
+L30:
+ if (! l9_3.index1[1]) {
+ return 0;
+ }
+ d__1 = -l2_3.x[1] + std::sqrt(z__[kn1 - 1]);
+ mdnord_(&d__1, &h1);
+ d__1 = -l2_3.x[1] - std::sqrt(z__[kn1 - 1]);
+ mdnord_(&d__1, &h2);
+ l3_2.g[1] = l2_3.x[3] - h1 - h2;
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+/* Computing 2nd power */
+ d__1 = l2_3.x[1];
+ l5_6.gg[2] = std::exp(d__1 * d__1 * -.5) * 2. / std::sqrt(std::atan(1.) * 8.);
+L7:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+ h1 = -l2_3.x[1] - d__[kn1 - 1] * std::sqrt(z__[kn1 - 1]);
+ h2 = -l2_3.x[1] + d__[kn1 - 1] * std::sqrt(z__[kn1 - 1]);
+ h3 = 1. / std::sqrt(std::atan(1.) * 8.);
+/* Computing 2nd power */
+ d__1 = h1;
+/* Computing 2nd power */
+ d__2 = h2;
+ l5_6.gg[3] = (std::exp(d__1 * d__1 * -.5) + std::exp(d__2 * d__2 * -.5)) * h3;
+L8:
+ return 0;
+} /* tp68_ */
+
+/* Subroutine */ int tp68_(int *mode)
+{
+ return tp68_0_(0, mode);
+ }
+
+/* Subroutine */ int tp69_(int *mode)
+{
+ return tp68_0_(1, mode);
+ }
+
+
+/* Subroutine */ int tp70_(int *mode)
+{
+ /* System generated locals */
+ int i__1;
+ Real d__1;
+
+ /* Local variables */
+ static Real b, c__[19], f[19];
+ static int i__, j;
+ static Real s, t, h1, h2, h3, h4, h5, h6, h7, h8, h9, u1[19], u2[19]
+ , v3[19], v4[19], z1, z2, v8[19], v9[19], z5, z6, z7, h10, df[76]
+ /* was [19][4] */, h30, h40, h11, h12, h13, h14, h15, h16, h17,
+ h18, v10, v11, v12, h19, h20, yc[19], h21, h22, h23, yo[19];
+ static bool log__;
+ static Real sum;
+
+ log__ = false;
+ if (*mode - 2 >= 0) {
+ goto L17;
+ } else {
+ goto labelL1;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_3.x[0] = 2.;
+ l2_3.x[1] = 4.;
+ l2_3.x[2] = .04;
+ l2_3.x[3] = 2.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l12_3.lxu[i__ - 1] = true;
+ l11_3.lxl[i__ - 1] = true;
+ l13_3.xl[i__ - 1] = 1e-5;
+/* labelL6: */
+ l14_3.xu[i__ - 1] = 100.;
+ }
+ l14_3.xu[2] = 1.;
+ l5_2.gg[0] = 0.;
+ l5_2.gg[1] = 0.;
+ l20_6.xex[0] = 12.2769537557;
+ l20_6.xex[1] = 4.63178796886;
+ l20_6.xex[2] = .312862470961;
+ l20_6.xex[3] = 2.0292903157;
+ l20_6.fex = .00749846356143;
+ l20_6.lex = false;
+ return 0;
+L17:
+ c__[0] = .1;
+ for (i__ = 2; i__ <= 19; ++i__) {
+/* labelL20: */
+ c__[i__ - 1] = (Real) (i__ - 1);
+ }
+ yo[0] = .00189;
+ yo[1] = .1038;
+ yo[2] = .268;
+ yo[3] = .506;
+ yo[4] = .577;
+ yo[5] = .604;
+ yo[6] = .725;
+ yo[7] = .898;
+ yo[8] = .947;
+ yo[9] = .845;
+ yo[10] = .702;
+ yo[11] = .528;
+ yo[12] = .385;
+ yo[13] = .257;
+ yo[14] = .159;
+ yo[15] = .0869;
+ yo[16] = .0453;
+ yo[17] = .01509;
+ yo[18] = .00189;
+ if ((i__1 = *mode - 4) < 0) {
+ goto L18;
+ } else if (i__1 == 0) {
+ goto labelL4;
+ } else {
+ goto labelL5;
+ }
+L18:
+ b = l2_3.x[2] + (1. - l2_3.x[2]) * l2_3.x[3];
+ h1 = l2_3.x[0] - 1.;
+ h2 = l2_3.x[1] - 1.;
+ h3 = .13058239749281797;
+ h5 = b * h3;
+ h4 = h5 / l2_3.x[3];
+ h6 = l2_3.x[0] * 12. / (l2_3.x[0] * 12. + 1.);
+ h7 = l2_3.x[1] * 12. / (l2_3.x[1] * 12. + 1.);
+ h30 = 0.;
+ h40 = 0.;
+ v10 = l2_3.x[1] / 6.2832;
+ v11 = b / l2_3.x[3];
+ v12 = l2_3.x[0] / 6.2832;
+ if (b < 0. || v10 < 0. || v11 < 0. || v12 < 0.) {
+ log__ = true;
+ }
+ if (log__ && *mode == 2) {
+ goto L8;
+ }
+ if (log__ && *mode == 3) {
+ goto labelL9;
+ }
+ z1 = l2_3.x[2] * pow_dd(&b, &l2_3.x[1]);
+ z2 = pow_dd(&v10, &c_b590);
+ z5 = 1. - l2_3.x[2];
+ z6 = pow_dd(&v11, l2_3.x);
+ z7 = pow_dd(&v12, &c_b590);
+ for (i__ = 1; i__ <= 19; ++i__) {
+ d__1 = c__[i__ - 1] * h3;
+ v3[i__ - 1] = pow_dd(&d__1, &h2);
+ v4[i__ - 1] = std::exp(l2_3.x[1] * (1. - c__[i__ - 1] * h5));
+ d__1 = c__[i__ - 1] * h3;
+ v8[i__ - 1] = pow_dd(&d__1, &h1);
+ v9[i__ - 1] = std::exp(l2_3.x[0] * (1. - c__[i__ - 1] * h4));
+ u1[i__ - 1] = z1 * z2 * v3[i__ - 1] * v4[i__ - 1] * h7;
+ u2[i__ - 1] = z5 * z6 * z7 * v8[i__ - 1] * v9[i__ - 1] * h6;
+ yc[i__ - 1] = u1[i__ - 1] + u2[i__ - 1];
+/* L30: */
+ f[i__ - 1] = yc[i__ - 1] - yo[i__ - 1];
+ }
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ }
+labelL2:
+ t = 0.;
+ for (i__ = 1; i__ <= 19; ++i__) {
+/* L31: */
+/* Computing 2nd power */
+ d__1 = f[i__ - 1];
+ t += d__1 * d__1;
+ }
+ l6_1.fx = t;
+ return 0;
+labelL3:
+ h8 = l2_3.x[3] - 1.;
+ h9 = l2_3.x[2] - 1.;
+ h10 = 1. / b;
+ h11 = 1. / l2_3.x[3];
+ h12 = (1. / (l2_3.x[0] * 12. + 1.) + .5) / l2_3.x[0] + 1.;
+ h13 = (1. / (l2_3.x[1] * 12. + 1.) + .5) / l2_3.x[1] + 1.;
+ h16 = l2_3.x[1] * h8;
+ h17 = l2_3.x[0] * h8;
+ h18 = 1. / l2_3.x[2] - h16 * h10;
+ h19 = 1. / h9 - h17 * h10;
+ h16 *= h3;
+ h17 = h17 * h11 * h3;
+ h20 = l2_3.x[1] * h9;
+/* Computing 2nd power */
+ d__1 = h11;
+ h21 = l2_3.x[0] * l2_3.x[2] * (d__1 * d__1);
+ h22 = h20 * h3;
+ h23 = h21 * h3;
+ h20 *= h10;
+ h21 = h21 * h10 * l2_3.x[3];
+ for (j = 1; j <= 19; ++j) {
+ h14 = h4 * c__[j - 1];
+ h15 = h5 * c__[j - 1];
+ if (h14 > 0.) {
+ goto labelL10;
+ }
+ l4_3.gf[0] = (l2_3.x[0] - 5.) * 2.;
+ h30 = 1.;
+ goto labelL11;
+labelL10:
+ df[j - 1] = u2[j - 1] * (h12 - h14 + std::log(h14));
+labelL11:
+ if (h15 > 0.) {
+ goto labelL12;
+ }
+ l4_3.gf[1] = (l2_3.x[1] - 5.) * 2.;
+ h40 = 1.;
+ goto labelL13;
+labelL12:
+ df[j + 18] = u1[j - 1] * (h13 - h15 + std::log(h15));
+labelL13:
+ df[j + 37] = u1[j - 1] * (h18 + c__[j - 1] * h16) + u2[j - 1] * (h19
+ + c__[j - 1] * h17);
+/* L33: */
+ df[j + 56] = u1[j - 1] * (h22 * c__[j - 1] - h20) + u2[j - 1] * (h23 *
+ c__[j - 1] - h21);
+ }
+ if (h30 == 1. || h40 == 1.) {
+ goto labelL14;
+ }
+ for (i__ = 1; i__ <= 4; ++i__) {
+ s = 0.;
+ for (j = 1; j <= 19; ++j) {
+ s += f[j - 1] * 2. * df[j + i__ * 19 - 20];
+/* L36: */
+ }
+ l4_3.gf[i__ - 1] = s;
+/* L37: */
+ }
+ return 0;
+labelL14:
+ for (i__ = 1; i__ <= 4; ++i__) {
+ if (i__ == 1 && h30 == 1.) {
+ goto L38;
+ }
+ if (i__ == 2 && h40 == (float).1) {
+ goto L38;
+ }
+ s = 0.;
+ for (j = 1; j <= 19; ++j) {
+/* L39: */
+ s += f[j - 1] * 2. * df[j + i__ * 19 - 20];
+ }
+ l4_3.gf[i__ - 1] = s;
+L38:
+ ;
+ }
+ h30 = 0.;
+ h40 = 0.;
+ return 0;
+L8:
+ log__ = false;
+ sum = 0.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* L40: */
+/* Computing 2nd power */
+ d__1 = l2_3.x[i__ - 1] - 5.;
+ sum += d__1 * d__1;
+ }
+ l6_1.fx = sum + 1e3 + .0075;
+ return 0;
+labelL9:
+ log__ = false;
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* L41: */
+ l4_3.gf[i__ - 1] = (l2_3.x[i__ - 1] - 5.) * 2.;
+ }
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = l2_3.x[2] + (1. - l2_3.x[2]) * l2_3.x[3];
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+ l5_2.gg[2] = 1. - l2_3.x[3];
+ l5_2.gg[3] = -l2_3.x[2] + 1.;
+L7:
+ return 0;
+} /* tp70_ */
+
+
+/* Subroutine */ int tp71_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l11_3.lxl[i__ - 1] = true;
+ l12_3.lxu[i__ - 1] = true;
+ l13_3.xl[i__ - 1] = 1.;
+/* labelL6: */
+ l14_3.xu[i__ - 1] = 5.;
+ }
+ l2_3.x[0] = 1.;
+ l2_3.x[1] = 5.;
+ l2_3.x[2] = 5.;
+ l2_3.x[3] = 1.;
+ l20_6.xex[0] = 1.;
+ l20_6.xex[1] = 4.74299937545;
+ l20_6.xex[2] = 3.82115032617;
+ l20_6.xex[3] = 1.37940824585;
+ l20_6.fex = 17.0140172895;
+ l20_6.lex = false;
+ return 0;
+labelL2:
+ l6_1.fx = l2_3.x[0] * l2_3.x[3] * (l2_3.x[0] + l2_3.x[1] + l2_3.x[2]) +
+ l2_3.x[2];
+ return 0;
+labelL3:
+ l4_3.gf[0] = l2_3.x[3] * (l2_3.x[0] * 2. + l2_3.x[1] + l2_3.x[2]);
+ l4_3.gf[1] = l2_3.x[0] * l2_3.x[3];
+ l4_3.gf[2] = l4_3.gf[1] + 1.;
+ l4_3.gf[3] = l2_3.x[0] * (l2_3.x[0] + l2_3.x[1] + l2_3.x[2]);
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = (l2_3.x[0] * l2_3.x[1] * l2_3.x[2] * l2_3.x[3] - 25.) /
+ 25.;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+/* Computing 2nd power */
+ d__2 = l2_3.x[1];
+/* Computing 2nd power */
+ d__3 = l2_3.x[2];
+/* Computing 2nd power */
+ d__4 = l2_3.x[3];
+ l3_2.g[1] = (d__1 * d__1 + d__2 * d__2 + d__3 * d__3 + d__4 * d__4 -
+ 40.) / 40.;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+ l5_6.gg[0] = l2_3.x[1] * l2_3.x[2] * l2_3.x[3] / 25.;
+ l5_6.gg[2] = l2_3.x[0] * l2_3.x[2] * l2_3.x[3] / 25.;
+ l5_6.gg[4] = l2_3.x[0] * l2_3.x[1] * l2_3.x[3] / 25.;
+ l5_6.gg[6] = l2_3.x[0] * l2_3.x[1] * l2_3.x[2] / 25.;
+L7:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* labelL20: */
+ l5_6.gg[(i__ << 1) - 1] = l2_3.x[i__ - 1] * 2. / 40.;
+ }
+L8:
+ return 0;
+} /* tp71_ */
+
+
+/* Subroutine */ int tp72_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l2_3.x[i__ - 1] = 1.;
+ l13_3.xl[i__ - 1] = .001;
+ l14_3.xu[i__ - 1] = (5. - (Real) i__) * 1e5;
+ l11_3.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_3.lxu[i__ - 1] = true;
+ }
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* L30: */
+ l4_3.gf[i__ - 1] = 1.;
+ }
+ l20_6.lex = false;
+ l20_6.xex[0] = 193.407050141;
+ l20_6.xex[1] = 179.547504555;
+ l20_6.xex[2] = 185.018587841;
+ l20_6.xex[3] = 168.706233485;
+ l20_6.fex = 727.679376021;
+ return 0;
+labelL2:
+ l6_1.fx = l2_3.x[0] + 1. + l2_3.x[1] + l2_3.x[2] + l2_3.x[3];
+labelL3:
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = -4. / l2_3.x[0] - 2.25 / l2_3.x[1] - 1. / l2_3.x[2] - .25
+ / l2_3.x[3] + .0401;
+ }
+ if (l9_3.index1[1]) {
+ l3_2.g[1] = -.16 / l2_3.x[0] - .36 / l2_3.x[1] - (1. / l2_3.x[2] + 1.
+ / l2_3.x[3]) * .64 + .010085;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+ l5_6.gg[0] = 4. / (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = l2_3.x[1];
+ l5_6.gg[2] = 2.25 / (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = l2_3.x[2];
+ l5_6.gg[4] = 1. / (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = l2_3.x[3];
+ l5_6.gg[6] = .25 / (d__1 * d__1);
+L7:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+ l5_6.gg[1] = .16 / (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = l2_3.x[1];
+ l5_6.gg[3] = .36 / (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = l2_3.x[2];
+ l5_6.gg[5] = .64 / (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = l2_3.x[3];
+ l5_6.gg[7] = .64 / (d__1 * d__1);
+L8:
+ return 0;
+} /* tp72_ */
+
+
+/* Subroutine */ int tp73_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5;
+
+
+ /* Local variables */
+ static Real a[4];
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 1;
+ l1_1.ninl = 1;
+ l1_1.neli = 1;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l2_3.x[i__ - 1] = 1.;
+ l13_3.xl[i__ - 1] = 0.;
+ l11_3.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_3.lxu[i__ - 1] = false;
+ }
+ l4_3.gf[0] = 24.55;
+ l4_3.gf[1] = 26.75;
+ l4_3.gf[2] = 39.;
+ l4_3.gf[3] = 40.5;
+ l5_7.gg[0] = 2.3;
+ l5_7.gg[3] = 5.6;
+ l5_7.gg[6] = 11.1;
+ l5_7.gg[9] = 1.3;
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* L31: */
+ l5_7.gg[i__ * 3 - 1] = 1.;
+ }
+ l20_6.lex = false;
+ l20_6.xex[0] = .635521568605;
+ l20_6.xex[1] = -1.1786227376e-12;
+ l20_6.xex[2] = .312701880754;
+ l20_6.xex[3] = .0517765506011;
+ l20_6.fex = 29.8943781573;
+ return 0;
+labelL2:
+ l6_1.fx = l2_3.x[0] * 24.55 + l2_3.x[1] * 26.75 + l2_3.x[2] * 39. +
+ l2_3.x[3] * 40.5;
+labelL3:
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = l2_3.x[0] * 2.3 + l2_3.x[1] * 5.6 + l2_3.x[2] * 11.1 +
+ l2_3.x[3] * 1.3 - 5.;
+ }
+ if (l9_4.index1[1]) {
+/* Computing 2nd power */
+ d__2 = l2_3.x[0];
+/* Computing 2nd power */
+ d__3 = l2_3.x[1];
+/* Computing 2nd power */
+ d__4 = l2_3.x[2];
+/* Computing 2nd power */
+ d__5 = l2_3.x[3];
+ d__1 = d__2 * d__2 * .28 + d__3 * d__3 * .19 + d__4 * d__4 * 20.5 +
+ d__5 * d__5 * .62;
+ l3_3.g[1] = l2_3.x[0] * 12. + l2_3.x[1] * 11.9 + l2_3.x[2] * 41.8 +
+ l2_3.x[3] * 52.1 - pow_dd(&d__1, &c_b590) * 1.645 - 21.;
+ }
+ if (l9_4.index1[2]) {
+ l3_3.g[2] = l2_3.x[0] + l2_3.x[1] + l2_3.x[2] + l2_3.x[3] - 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_4.index2[1]) {
+ goto L8;
+ }
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* L30: */
+/* Computing 2nd power */
+ d__2 = l2_3.x[0];
+/* Computing 2nd power */
+ d__3 = l2_3.x[1];
+/* Computing 2nd power */
+ d__4 = l2_3.x[2];
+/* Computing 2nd power */
+ d__5 = l2_3.x[3];
+ d__1 = d__2 * d__2 * .28 + d__3 * d__3 * .19 + d__4 * d__4 * 20.5 +
+ d__5 * d__5 * .62;
+ a[i__ - 1] = l2_3.x[i__ - 1] * 1.645 * pow_dd(&d__1, &c_b308);
+ }
+ l5_7.gg[1] = 12. - a[0] * .28;
+ l5_7.gg[4] = 11.9 - a[1] * .19;
+ l5_7.gg[7] = 41.8 - a[2] * 20.5;
+ l5_7.gg[10] = 52.1 - a[3] * .62;
+L8:
+ return 0;
+} /* tp73_ */
+
+
+/* Subroutine */ int tp74_0_(int n__, int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static Real a[2];
+ static int i__;
+ static Real v1, v2;
+ static int kn1;
+
+ switch(n__) {
+ case 1: goto L_tp75;
+ }
+
+ l20_6.xex[0] = 679.945319802;
+ l20_6.xex[1] = 1026.06713256;
+ l20_6.xex[2] = .11887636449;
+ l20_6.xex[3] = -.39623355318;
+ l20_6.fex = 5126.49810934;
+ kn1 = 1;
+ goto L7;
+
+L_tp75:
+ kn1 = 2;
+ l20_6.xex[0] = 776.159220293;
+ l20_6.xex[1] = 925.194939196;
+ l20_6.xex[2] = .0511087936804;
+ l20_6.xex[3] = -.428891137432;
+ l20_6.fex = 5174.41288686;
+L7:
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 2;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 3;
+ a[0] = .55;
+ a[1] = .48;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l2_3.x[i__ - 1] = 0.;
+ l11_3.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_3.lxu[i__ - 1] = true;
+ }
+ l13_3.xl[0] = 0.;
+ l13_3.xl[1] = 0.;
+ l13_3.xl[2] = -a[kn1 - 1];
+ l13_3.xl[3] = -a[kn1 - 1];
+ l14_3.xu[0] = 1200.;
+ l14_3.xu[1] = 1200.;
+ l14_3.xu[2] = a[kn1 - 1];
+ l14_3.xu[3] = a[kn1 - 1];
+ l4_3.gf[2] = 0.;
+ l4_3.gf[3] = 0.;
+ l5_13.gg[0] = 0.;
+ l5_13.gg[5] = 0.;
+ l5_13.gg[10] = -1.;
+ l5_13.gg[15] = 1.;
+ l5_13.gg[1] = 0.;
+ l5_13.gg[6] = 0.;
+ l5_13.gg[11] = 1.;
+ l5_13.gg[16] = -1.;
+ l5_13.gg[2] = -1.;
+ l5_13.gg[7] = 0.;
+ l5_13.gg[3] = 0.;
+ l5_13.gg[8] = -1.;
+ l5_13.gg[4] = 0.;
+ l5_13.gg[9] = 0.;
+ l20_6.lex = false;
+ return 0;
+labelL2:
+/* Computing 3rd power */
+ d__1 = l2_3.x[0];
+/* Computing 3rd power */
+ d__2 = l2_3.x[1];
+ l6_1.fx = l2_3.x[0] * 3. + d__1 * (d__1 * d__1) * 1e-6 + l2_3.x[1] * 2. +
+ d__2 * (d__2 * d__2) * 6.666666666666666e-7;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+ l4_3.gf[0] = d__1 * d__1 * 3e-6 + 3.;
+/* Computing 2nd power */
+ d__1 = l2_3.x[1];
+ l4_3.gf[1] = d__1 * d__1 * 2e-6 + 2.;
+ return 0;
+labelL4:
+ if (l9_5.index1[0]) {
+ l3_4.g[0] = l2_3.x[3] - l2_3.x[2] + a[kn1 - 1];
+ }
+ if (l9_5.index1[1]) {
+ l3_4.g[1] = l2_3.x[2] - l2_3.x[3] + a[kn1 - 1];
+ }
+ if (l9_5.index1[2]) {
+ l3_4.g[2] = (sin(-l2_3.x[2] - .25) + std::sin(-l2_3.x[3] - .25)) * 1e3 +
+ 894.8 - l2_3.x[0];
+ }
+ if (l9_5.index1[3]) {
+ l3_4.g[3] = (sin(l2_3.x[2] - .25) + std::sin(l2_3.x[2] - l2_3.x[3] - .25))
+ * 1e3 + 894.8 - l2_3.x[1];
+ }
+ if (l9_5.index1[4]) {
+ l3_4.g[4] = (sin(l2_3.x[3] - .25) + std::sin(l2_3.x[3] - l2_3.x[2] - .25))
+ * 1e3 + 1294.8;
+ }
+ return 0;
+labelL5:
+ if (! l10_5.index2[2]) {
+ goto labelL9;
+ }
+ l5_13.gg[12] = std::cos(-l2_3.x[2] - .25) * -1e3;
+ l5_13.gg[17] = std::cos(-l2_3.x[3] - .25) * -1e3;
+labelL9:
+ if (! l10_5.index2[3]) {
+ goto labelL10;
+ }
+ v1 = std::cos(l2_3.x[2] - l2_3.x[3] - .25);
+ l5_13.gg[13] = (std::cos(l2_3.x[2] - .25) + v1) * 1e3;
+ l5_13.gg[18] = v1 * -1e3;
+labelL10:
+ if (! l10_5.index2[4]) {
+ goto labelL11;
+ }
+ v2 = std::cos(l2_3.x[3] - l2_3.x[2] - .25);
+ l5_13.gg[14] = v2 * -1e3;
+ l5_13.gg[19] = (std::cos(l2_3.x[3] - .25) + v2) * 1e3;
+labelL11:
+ return 0;
+} /* tp74_ */
+
+/* Subroutine */ int tp74_(int *mode)
+{
+ return tp74_0_(0, mode);
+ }
+
+/* Subroutine */ int tp75_(int *mode)
+{
+ return tp74_0_(1, mode);
+ }
+
+
+/* Subroutine */ int tp76_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 3;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l11_3.lxl[i__ - 1] = true;
+ l12_3.lxu[i__ - 1] = false;
+ l2_3.x[i__ - 1] = .5;
+/* labelL6: */
+ l13_3.xl[i__ - 1] = 0.;
+ }
+ l5_7.gg[0] = -1.;
+ l5_7.gg[3] = -2.;
+ l5_7.gg[6] = -1.;
+ l5_7.gg[9] = -1.;
+ l5_7.gg[1] = -3.;
+ l5_7.gg[4] = -1.;
+ l5_7.gg[7] = -2.;
+ l5_7.gg[10] = 1.;
+ l5_7.gg[2] = 0.;
+ l5_7.gg[5] = 1.;
+ l5_7.gg[8] = 4.;
+ l5_7.gg[11] = 0.;
+ l20_6.lex = false;
+ l20_6.xex[0] = .272727272717;
+ l20_6.xex[1] = 2.09090909094;
+ l20_6.xex[2] = -2.63371889808e-11;
+ l20_6.xex[3] = .545454545496;
+ l20_6.fex = -4.68181818182;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+/* Computing 2nd power */
+ d__2 = l2_3.x[2];
+/* Computing 2nd power */
+ d__3 = l2_3.x[1];
+/* Computing 2nd power */
+ d__4 = l2_3.x[3];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + (d__3 * d__3 + d__4 * d__4) * .5 -
+ l2_3.x[0] * l2_3.x[2] + l2_3.x[2] * l2_3.x[3] - l2_3.x[0] -
+ l2_3.x[1] * 3. + l2_3.x[2] - l2_3.x[3];
+ return 0;
+labelL3:
+ l4_3.gf[0] = l2_3.x[0] * 2. - l2_3.x[2] - 1.;
+ l4_3.gf[1] = l2_3.x[1] - 3.;
+ l4_3.gf[2] = l2_3.x[2] * 2. - l2_3.x[0] + l2_3.x[3] + 1.;
+ l4_3.gf[3] = l2_3.x[3] + l2_3.x[2] - 1.;
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = -l2_3.x[0] - l2_3.x[1] * 2. - l2_3.x[2] - l2_3.x[3] + 5.;
+ }
+ if (l9_4.index1[1]) {
+ l3_3.g[1] = l2_3.x[0] * -3. - l2_3.x[1] - l2_3.x[2] * 2. + l2_3.x[3]
+ + 4.;
+ }
+ if (l9_4.index1[2]) {
+ l3_3.g[2] = l2_3.x[1] + l2_3.x[2] * 4. - 1.5;
+ }
+labelL5:
+ return 0;
+} /* tp76_ */
+
+
+/* Subroutine */ int tp77_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5;
+
+ /* Local variables */
+ static int i__;
+ static Real v1;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 2;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l2_4.x[i__ - 1] = 2.;
+ l11_4.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_4.lxu[i__ - 1] = false;
+ }
+ l5_4.gg[2] = 0.;
+ l5_4.gg[4] = 0.;
+ l5_4.gg[1] = 0.;
+ l5_4.gg[3] = 1.;
+ l5_4.gg[9] = 0.;
+ l20_7.lex = false;
+ l20_7.xex[0] = 1.16617219726;
+ l20_7.xex[1] = 1.18211138813;
+ l20_7.xex[2] = 1.38025704044;
+ l20_7.xex[3] = 1.50603627961;
+ l20_7.xex[4] = .610920257517;
+ l20_7.fex = .241505128786;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_4.x[0] - 1.;
+/* Computing 2nd power */
+ d__2 = l2_4.x[0] - l2_4.x[1];
+/* Computing 2nd power */
+ d__3 = l2_4.x[2] - 1.;
+/* Computing 4th power */
+ d__4 = l2_4.x[3] - 1., d__4 *= d__4;
+/* Computing 6th power */
+ d__5 = l2_4.x[4] - 1., d__5 *= d__5;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 + d__4 * d__4 + d__5 * (
+ d__5 * d__5);
+ return 0;
+labelL3:
+ l4_4.gf[0] = (l2_4.x[0] * 2. - l2_4.x[1] - 1.) * 2.;
+ l4_4.gf[1] = (l2_4.x[0] - l2_4.x[1]) * -2.;
+ l4_4.gf[2] = (l2_4.x[2] - 1.) * 2.;
+/* Computing 3rd power */
+ d__1 = l2_4.x[3] - 1.;
+ l4_4.gf[3] = d__1 * (d__1 * d__1) * 4.;
+/* Computing 5th power */
+ d__1 = l2_4.x[4] - 1., d__2 = d__1, d__1 *= d__1;
+ l4_4.gf[4] = d__2 * (d__1 * d__1) * 6.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_4.x[0];
+ l3_2.g[0] = d__1 * d__1 * l2_4.x[3] + std::sin(l2_4.x[3] - l2_4.x[4]) -
+ std::sqrt(2.) * 2.;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 4th power */
+ d__1 = l2_4.x[2], d__1 *= d__1;
+/* Computing 2nd power */
+ d__2 = l2_4.x[3];
+ l3_2.g[1] = l2_4.x[1] + d__1 * d__1 * (d__2 * d__2) - 8. - std::sqrt(2.);
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+ v1 = std::cos(l2_4.x[3] - l2_4.x[4]);
+ l5_4.gg[0] = l2_4.x[0] * 2. * l2_4.x[3];
+/* Computing 2nd power */
+ d__1 = l2_4.x[0];
+ l5_4.gg[6] = d__1 * d__1 + v1;
+ l5_4.gg[8] = -v1;
+L7:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+/* Computing 3rd power */
+ d__1 = l2_4.x[2];
+/* Computing 2nd power */
+ d__2 = l2_4.x[3];
+ l5_4.gg[5] = d__1 * (d__1 * d__1) * 4. * (d__2 * d__2);
+/* Computing 4th power */
+ d__1 = l2_4.x[2], d__1 *= d__1;
+ l5_4.gg[7] = d__1 * d__1 * 2. * l2_4.x[3];
+L8:
+ return 0;
+} /* tp77_ */
+
+
+/* Subroutine */ int tp78_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 3;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l11_4.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_4.lxu[i__ - 1] = false;
+ }
+ l2_4.x[0] = -2.;
+ l2_4.x[1] = 1.5;
+ l2_4.x[2] = 2.;
+ l2_4.x[3] = -1.;
+ l2_4.x[4] = -1.;
+ l5_9.gg[1] = 0.;
+ l5_9.gg[8] = 0.;
+ l5_9.gg[11] = 0.;
+ l5_9.gg[14] = 0.;
+ l20_7.lex = false;
+ l20_7.xex[0] = -1.7171423423;
+ l20_7.xex[1] = 1.59570826805;
+ l20_7.xex[2] = 1.82724803488;
+ l20_7.xex[3] = -.7636429466;
+ l20_7.xex[4] = -.763643482853;
+ l20_7.fex = -2.91970040911;
+ return 0;
+labelL2:
+ l6_1.fx = l2_4.x[0] * l2_4.x[1] * l2_4.x[2] * l2_4.x[3] * l2_4.x[4];
+ return 0;
+labelL3:
+ l4_4.gf[0] = l2_4.x[1] * l2_4.x[2] * l2_4.x[3] * l2_4.x[4];
+ l4_4.gf[1] = l2_4.x[0] * l2_4.x[2] * l2_4.x[3] * l2_4.x[4];
+ l4_4.gf[2] = l2_4.x[0] * l2_4.x[1] * l2_4.x[3] * l2_4.x[4];
+ l4_4.gf[3] = l2_4.x[0] * l2_4.x[1] * l2_4.x[2] * l2_4.x[4];
+ l4_4.gf[4] = l2_4.x[0] * l2_4.x[1] * l2_4.x[2] * l2_4.x[3];
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_4.x[0];
+/* Computing 2nd power */
+ d__2 = l2_4.x[1];
+/* Computing 2nd power */
+ d__3 = l2_4.x[2];
+/* Computing 2nd power */
+ d__4 = l2_4.x[3];
+/* Computing 2nd power */
+ d__5 = l2_4.x[4];
+ l3_3.g[0] = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 + d__4 * d__4 +
+ d__5 * d__5 - 10.;
+ }
+ if (l9_4.index1[1]) {
+ l3_3.g[1] = l2_4.x[1] * l2_4.x[2] - l2_4.x[3] * 5. * l2_4.x[4];
+ }
+ if (l9_4.index1[2]) {
+/* Computing 3rd power */
+ d__1 = l2_4.x[0];
+/* Computing 3rd power */
+ d__2 = l2_4.x[1];
+ l3_3.g[2] = d__1 * (d__1 * d__1) + d__2 * (d__2 * d__2) + 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_4.index2[0]) {
+ goto L7;
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L30: */
+ l5_9.gg[i__ * 3 - 3] = l2_4.x[i__ - 1] * 2.;
+ }
+L7:
+ if (! l10_4.index2[1]) {
+ goto L8;
+ }
+ l5_9.gg[4] = l2_4.x[2];
+ l5_9.gg[7] = l2_4.x[1];
+ l5_9.gg[10] = l2_4.x[4] * -5.;
+ l5_9.gg[13] = l2_4.x[3] * -5.;
+L8:
+ if (! l10_4.index2[2]) {
+ goto labelL9;
+ }
+/* Computing 2nd power */
+ d__1 = l2_4.x[0];
+ l5_9.gg[2] = d__1 * d__1 * 3.;
+/* Computing 2nd power */
+ d__1 = l2_4.x[1];
+ l5_9.gg[5] = d__1 * d__1 * 3.;
+labelL9:
+ return 0;
+} /* tp78_ */
+
+
+/* Subroutine */ int tp79_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5;
+
+ /* Local variables */
+ static int i__;
+ static Real v1, v2, v3, v4;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 3;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l2_4.x[i__ - 1] = 2.;
+ l11_4.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_4.lxu[i__ - 1] = false;
+ }
+ l5_9.gg[0] = 1.;
+ l5_9.gg[9] = 0.;
+ l5_9.gg[12] = 0.;
+ l5_9.gg[1] = 0.;
+ l5_9.gg[4] = 1.;
+ l5_9.gg[10] = 1.;
+ l5_9.gg[13] = 0.;
+ l5_9.gg[5] = 0.;
+ l5_9.gg[8] = 0.;
+ l5_9.gg[11] = 0.;
+ l20_7.lex = false;
+ l20_7.xex[0] = 1.19112745626;
+ l20_7.xex[1] = 1.36260316492;
+ l20_7.xex[2] = 1.4728179315;
+ l20_7.xex[3] = 1.63501661894;
+ l20_7.xex[4] = 1.67908143619;
+ l20_7.fex = .0787768208538;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_4.x[0] - 1.;
+/* Computing 2nd power */
+ d__2 = l2_4.x[0] - l2_4.x[1];
+/* Computing 2nd power */
+ d__3 = l2_4.x[1] - l2_4.x[2];
+/* Computing 4th power */
+ d__4 = l2_4.x[2] - l2_4.x[3], d__4 *= d__4;
+/* Computing 4th power */
+ d__5 = l2_4.x[3] - l2_4.x[4], d__5 *= d__5;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 + d__4 * d__4 + d__5 *
+ d__5;
+ return 0;
+labelL3:
+ v1 = l2_4.x[0] - l2_4.x[1];
+ v2 = l2_4.x[1] - l2_4.x[2];
+ v3 = l2_4.x[2] - l2_4.x[3];
+ v4 = l2_4.x[3] - l2_4.x[4];
+ l4_4.gf[0] = (l2_4.x[0] - 1. + v1) * 2.;
+ l4_4.gf[1] = (v2 - v1) * 2.;
+/* Computing 3rd power */
+ d__1 = v3;
+ l4_4.gf[2] = v2 * -2. + d__1 * (d__1 * d__1) * 4.;
+/* Computing 3rd power */
+ d__1 = v4;
+/* Computing 3rd power */
+ d__2 = v3;
+ l4_4.gf[3] = (d__1 * (d__1 * d__1) - d__2 * (d__2 * d__2)) * 4.;
+/* Computing 3rd power */
+ d__1 = v4;
+ l4_4.gf[4] = d__1 * (d__1 * d__1) * -4.;
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_4.x[1];
+/* Computing 3rd power */
+ d__2 = l2_4.x[2];
+ l3_3.g[0] = l2_4.x[0] + d__1 * d__1 + d__2 * (d__2 * d__2) - 2. -
+ std::sqrt(2.) * 3.;
+ }
+ if (l9_4.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_4.x[2];
+ l3_3.g[1] = l2_4.x[1] - d__1 * d__1 + l2_4.x[3] + 2. - std::sqrt(2.) * 2.;
+ }
+ if (l9_4.index1[2]) {
+ l3_3.g[2] = l2_4.x[0] * l2_4.x[4] - 2.;
+ }
+ return 0;
+labelL5:
+ if (! l10_4.index2[0]) {
+ goto L7;
+ }
+ l5_9.gg[3] = l2_4.x[1] * 2.;
+/* Computing 2nd power */
+ d__1 = l2_4.x[2];
+ l5_9.gg[6] = d__1 * d__1 * 3.;
+L7:
+ if (l10_4.index2[1]) {
+ l5_9.gg[7] = l2_4.x[2] * -2.;
+ }
+ if (! l10_4.index2[2]) {
+ goto labelL9;
+ }
+ l5_9.gg[2] = l2_4.x[4];
+ l5_9.gg[14] = l2_4.x[0];
+labelL9:
+ return 0;
+} /* tp79_ */
+
+
+/* Subroutine */ int tp80_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5;
+
+ /* Local variables */
+ static int i__;
+ static Real t, v1, v2;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 3;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l11_4.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_4.lxu[i__ - 1] = true;
+ }
+ l2_4.x[0] = -2.;
+ l2_4.x[1] = 2.;
+ l2_4.x[2] = 2.;
+ l2_4.x[3] = -1.;
+ l2_4.x[4] = -1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l13_4.xl[i__ - 1] = -2.3;
+/* labelL20: */
+ l14_4.xu[i__ - 1] = 2.3;
+ }
+ for (i__ = 3; i__ <= 5; ++i__) {
+ l13_4.xl[i__ - 1] = -3.2;
+/* L21: */
+ l14_4.xu[i__ - 1] = 3.2;
+ }
+ l5_9.gg[1] = 0.;
+ l5_9.gg[8] = 0.;
+ l5_9.gg[11] = 0.;
+ l5_9.gg[14] = 0.;
+ l20_7.lex = false;
+ l20_7.xex[0] = -1.71714294417;
+ l20_7.xex[1] = 1.59570896503;
+ l20_7.xex[2] = 1.82724691654;
+ l20_7.xex[3] = -.763641279311;
+ l20_7.xex[4] = -.763645016315;
+ l20_7.fex = .0539498477624;
+ return 0;
+labelL2:
+ l6_1.fx = std::exp(l2_4.x[0] * l2_4.x[1] * l2_4.x[2] * l2_4.x[3] * l2_4.x[4]);
+ return 0;
+labelL3:
+ v1 = l2_4.x[3] * l2_4.x[4];
+ v2 = l2_4.x[0] * l2_4.x[1];
+ t = std::exp(v2 * l2_4.x[2] * v1);
+ l4_4.gf[0] = l2_4.x[1] * l2_4.x[2] * v1 * t;
+ l4_4.gf[1] = l2_4.x[0] * l2_4.x[2] * v1 * t;
+ l4_4.gf[2] = v1 * v2 * t;
+ l4_4.gf[3] = v2 * l2_4.x[2] * l2_4.x[4] * t;
+ l4_4.gf[4] = v2 * l2_4.x[2] * l2_4.x[3] * t;
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_4.x[0];
+/* Computing 2nd power */
+ d__2 = l2_4.x[1];
+/* Computing 2nd power */
+ d__3 = l2_4.x[2];
+/* Computing 2nd power */
+ d__4 = l2_4.x[3];
+/* Computing 2nd power */
+ d__5 = l2_4.x[4];
+ l3_3.g[0] = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 + d__4 * d__4 +
+ d__5 * d__5 - 10.;
+ }
+ if (l9_4.index1[1]) {
+ l3_3.g[1] = l2_4.x[1] * l2_4.x[2] - l2_4.x[3] * 5. * l2_4.x[4];
+ }
+ if (l9_4.index1[2]) {
+/* Computing 3rd power */
+ d__1 = l2_4.x[0];
+/* Computing 3rd power */
+ d__2 = l2_4.x[1];
+ l3_3.g[2] = d__1 * (d__1 * d__1) + d__2 * (d__2 * d__2) + 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_4.index2[0]) {
+ goto L7;
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L30: */
+ l5_9.gg[i__ * 3 - 3] = l2_4.x[i__ - 1] * 2.;
+ }
+L7:
+ if (! l10_4.index2[1]) {
+ goto L8;
+ }
+ l5_9.gg[4] = l2_4.x[2];
+ l5_9.gg[7] = l2_4.x[1];
+ l5_9.gg[10] = l2_4.x[4] * -5.;
+ l5_9.gg[13] = l2_4.x[3] * -5.;
+L8:
+ if (! l10_4.index2[2]) {
+ goto labelL9;
+ }
+/* Computing 2nd power */
+ d__1 = l2_4.x[0];
+ l5_9.gg[2] = d__1 * d__1 * 3.;
+/* Computing 2nd power */
+ d__1 = l2_4.x[1];
+ l5_9.gg[5] = d__1 * d__1 * 3.;
+labelL9:
+ return 0;
+} /* tp80_ */
+
+
+/* Subroutine */ int tp81_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5;
+
+ /* Local variables */
+ static int i__;
+ static Real t, v1, v2, v3;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 3;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l11_4.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_4.lxu[i__ - 1] = true;
+ }
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l13_4.xl[i__ - 1] = -2.3;
+/* labelL10: */
+ l14_4.xu[i__ - 1] = 2.3;
+ }
+ for (i__ = 3; i__ <= 5; ++i__) {
+ l13_4.xl[i__ - 1] = -3.2;
+/* labelL20: */
+ l14_4.xu[i__ - 1] = 3.2;
+ }
+ l2_4.x[0] = -2.;
+ l2_4.x[1] = 2.;
+ l2_4.x[2] = 2.;
+ l2_4.x[3] = -1.;
+ l2_4.x[4] = -1.;
+ l5_9.gg[1] = 0.;
+ l5_9.gg[8] = 0.;
+ l5_9.gg[11] = 0.;
+ l5_9.gg[14] = 0.;
+ l20_7.lex = false;
+ l20_7.xex[0] = -1.71714240091;
+ l20_7.xex[1] = 1.59570833592;
+ l20_7.xex[2] = 1.82724792592;
+ l20_7.xex[3] = -.763647440817;
+ l20_7.xex[4] = -.763638975604;
+ l20_7.fex = .0539498477749;
+ return 0;
+labelL2:
+/* Computing 3rd power */
+ d__2 = l2_4.x[0];
+/* Computing 3rd power */
+ d__3 = l2_4.x[1];
+/* Computing 2nd power */
+ d__1 = d__2 * (d__2 * d__2) + d__3 * (d__3 * d__3) + 1.;
+ l6_1.fx = std::exp(l2_4.x[0] * l2_4.x[1] * l2_4.x[2] * l2_4.x[3] * l2_4.x[4])
+ - d__1 * d__1 * .5;
+ return 0;
+labelL3:
+/* Computing 3rd power */
+ d__1 = l2_4.x[0];
+/* Computing 3rd power */
+ d__2 = l2_4.x[1];
+ v1 = d__1 * (d__1 * d__1) + d__2 * (d__2 * d__2) + 1.;
+ v2 = l2_4.x[0] * l2_4.x[1];
+ v3 = l2_4.x[3] * l2_4.x[4];
+ t = std::exp(v2 * v3 * l2_4.x[2]);
+/* Computing 2nd power */
+ d__1 = l2_4.x[0];
+ l4_4.gf[0] = l2_4.x[1] * l2_4.x[2] * v3 * t - d__1 * d__1 * 3. * v1;
+/* Computing 2nd power */
+ d__1 = l2_4.x[1];
+ l4_4.gf[1] = l2_4.x[0] * l2_4.x[2] * v3 * t - d__1 * d__1 * 3. * v1;
+ l4_4.gf[2] = v2 * v3 * t;
+ l4_4.gf[3] = v2 * l2_4.x[2] * l2_4.x[4] * t;
+ l4_4.gf[4] = v2 * l2_4.x[2] * l2_4.x[3] * t;
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_4.x[0];
+/* Computing 2nd power */
+ d__2 = l2_4.x[1];
+/* Computing 2nd power */
+ d__3 = l2_4.x[2];
+/* Computing 2nd power */
+ d__4 = l2_4.x[3];
+/* Computing 2nd power */
+ d__5 = l2_4.x[4];
+ l3_3.g[0] = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 + d__4 * d__4 +
+ d__5 * d__5 - 10.;
+ }
+ if (l9_4.index1[1]) {
+ l3_3.g[1] = l2_4.x[1] * l2_4.x[2] - l2_4.x[3] * 5. * l2_4.x[4];
+ }
+ if (l9_4.index1[2]) {
+/* Computing 3rd power */
+ d__1 = l2_4.x[0];
+/* Computing 3rd power */
+ d__2 = l2_4.x[1];
+ l3_3.g[2] = d__1 * (d__1 * d__1) + d__2 * (d__2 * d__2) + 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_4.index2[0]) {
+ goto L7;
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L30: */
+ l5_9.gg[i__ * 3 - 3] = l2_4.x[i__ - 1] * 2.;
+ }
+L7:
+ if (! l10_4.index2[1]) {
+ goto L8;
+ }
+ l5_9.gg[4] = l2_4.x[2];
+ l5_9.gg[7] = l2_4.x[1];
+ l5_9.gg[10] = l2_4.x[4] * -5.;
+ l5_9.gg[13] = l2_4.x[3] * -5.;
+L8:
+ if (! l10_4.index2[2]) {
+ goto labelL9;
+ }
+/* Computing 2nd power */
+ d__1 = l2_4.x[0];
+ l5_9.gg[2] = d__1 * d__1 * 3.;
+/* Computing 2nd power */
+ d__1 = l2_4.x[1];
+ l5_9.gg[5] = d__1 * d__1 * 3.;
+labelL9:
+ return 0;
+} /* tp81_ */
+
+
+/* Subroutine */ int tp83_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real a, b, c__, d__;
+ static int i__;
+ static Real a1, a2, a3, a4, a5, a6, a7, a8, a9, v1, v2, v3, a10,
+ a11, a12;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 6;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_4.x[0] = 78.;
+ l2_4.x[1] = 33.;
+ l2_4.x[2] = 27.;
+ l2_4.x[3] = 27.;
+ l2_4.x[4] = 27.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l11_4.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_4.lxu[i__ - 1] = true;
+ }
+ l13_4.xl[0] = 78.;
+ l13_4.xl[1] = 33.;
+ l14_4.xu[0] = 102.;
+ l14_4.xu[1] = 45.;
+ for (i__ = 3; i__ <= 5; ++i__) {
+ l13_4.xl[i__ - 1] = 27.;
+/* L31: */
+ l14_4.xu[i__ - 1] = 45.;
+ }
+ a = 5.3578547;
+ b = .8356891;
+ c__ = 37.293239;
+ d__ = 40792.141;
+ a1 = 85.334407;
+ a2 = .0056858;
+ a3 = 6.262e-4;
+ a4 = .0022053;
+ a5 = 80.51249;
+ a6 = .0071317;
+ a7 = .0029955;
+ a8 = .0021813;
+ a9 = 9.300961;
+ a10 = .0047026;
+ a11 = .0012547;
+ a12 = .0019085;
+ l4_4.gf[1] = 0.;
+ l4_4.gf[3] = 0.;
+ l5_14.gg[19] = 0.;
+ l5_14.gg[8] = 0.;
+ l5_14.gg[22] = 0.;
+ l5_14.gg[11] = 0.;
+ l20_7.lex = false;
+ l20_7.xex[0] = 78.;
+ l20_7.xex[1] = 33.;
+ l20_7.xex[2] = 29.9952560253;
+ l20_7.xex[3] = 45.;
+ l20_7.xex[4] = 36.7758129081;
+ l20_7.fex = -30665.5386717;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_4.x[2];
+ l6_1.fx = a * (d__1 * d__1) + b * l2_4.x[0] * l2_4.x[4] + c__ * l2_4.x[0]
+ - d__;
+ return 0;
+labelL3:
+ l4_4.gf[0] = b * l2_4.x[4] + c__;
+ l4_4.gf[2] = a * 2. * l2_4.x[2];
+ l4_4.gf[4] = b * l2_4.x[0];
+ return 0;
+labelL4:
+ if (! (l9_6.index1[0] || l9_6.index1[3])) {
+ goto L41;
+ }
+ v1 = a1 + a2 * l2_4.x[1] * l2_4.x[4] + a3 * l2_4.x[0] * l2_4.x[3] - a4 *
+ l2_4.x[2] * l2_4.x[4];
+ if (l9_6.index1[0]) {
+ l3_5.g[0] = v1;
+ }
+ if (l9_6.index1[3]) {
+ l3_5.g[3] = 92. - v1;
+ }
+L41:
+ if (! (l9_6.index1[1] || l9_6.index1[4])) {
+ goto L42;
+ }
+/* Computing 2nd power */
+ d__1 = l2_4.x[2];
+ v2 = a5 + a6 * l2_4.x[1] * l2_4.x[4] + a7 * l2_4.x[0] * l2_4.x[1] + a8 * (
+ d__1 * d__1) - 90.;
+ if (l9_6.index1[1]) {
+ l3_5.g[1] = v2;
+ }
+ if (l9_6.index1[4]) {
+ l3_5.g[4] = 20. - v2;
+ }
+L42:
+ if (! (l9_6.index1[2] || l9_6.index1[5])) {
+ goto L43;
+ }
+ v3 = a9 + a10 * l2_4.x[2] * l2_4.x[4] + a11 * l2_4.x[0] * l2_4.x[2] + a12
+ * l2_4.x[2] * l2_4.x[3] - 20.;
+ if (l9_6.index1[2]) {
+ l3_5.g[2] = v3;
+ }
+ if (l9_6.index1[5]) {
+ l3_5.g[5] = 5. - v3;
+ }
+L43:
+ return 0;
+labelL5:
+ if (! l10_6.index2[0]) {
+ goto L7;
+ }
+ l5_14.gg[0] = a3 * l2_4.x[3];
+ l5_14.gg[6] = a2 * l2_4.x[4];
+ l5_14.gg[12] = -a4 * l2_4.x[4];
+ l5_14.gg[18] = a3 * l2_4.x[0];
+ l5_14.gg[24] = a2 * l2_4.x[1] - a4 * l2_4.x[2];
+L7:
+ if (! l10_6.index2[1]) {
+ goto L8;
+ }
+ l5_14.gg[1] = a7 * l2_4.x[1];
+ l5_14.gg[7] = a6 * l2_4.x[4] + a7 * l2_4.x[0];
+ l5_14.gg[13] = a8 * 2. * l2_4.x[2];
+ l5_14.gg[25] = a6 * l2_4.x[1];
+L8:
+ if (! l10_6.index2[2]) {
+ goto labelL9;
+ }
+ l5_14.gg[2] = a11 * l2_4.x[2];
+ l5_14.gg[14] = a10 * l2_4.x[4] + a11 * l2_4.x[0] + a12 * l2_4.x[3];
+ l5_14.gg[20] = a12 * l2_4.x[2];
+ l5_14.gg[26] = a10 * l2_4.x[2];
+labelL9:
+ if (! l10_6.index2[3]) {
+ goto labelL10;
+ }
+ l5_14.gg[3] = -a3 * l2_4.x[3];
+ l5_14.gg[9] = -a2 * l2_4.x[4];
+ l5_14.gg[15] = a4 * l2_4.x[4];
+ l5_14.gg[21] = -a3 * l2_4.x[0];
+ l5_14.gg[27] = -a2 * l2_4.x[1] + a4 * l2_4.x[2];
+labelL10:
+ if (! l10_6.index2[4]) {
+ goto labelL11;
+ }
+ l5_14.gg[4] = -a7 * l2_4.x[1];
+ l5_14.gg[10] = -a6 * l2_4.x[4] - a7 * l2_4.x[0];
+ l5_14.gg[16] = a8 * -2. * l2_4.x[2];
+ l5_14.gg[28] = -a6 * l2_4.x[1];
+labelL11:
+ if (! l10_6.index2[5]) {
+ goto labelL12;
+ }
+ l5_14.gg[5] = -a11 * l2_4.x[2];
+ l5_14.gg[17] = -a10 * l2_4.x[4] - a11 * l2_4.x[0] - a12 * l2_4.x[3];
+ l5_14.gg[23] = -a12 * l2_4.x[2];
+ l5_14.gg[29] = -a10 * l2_4.x[2];
+labelL12:
+ return 0;
+} /* tp83_ */
+
+
+/* Subroutine */ int tp84_(int *mode)
+{
+ static Real a[21], b[3];
+ static int i__, j, i1;
+ static Real v1;
+
+ a[0] = -24345.;
+ a[1] = -8720288.849;
+ a[2] = 150512.5253;
+ a[3] = -156.6950325;
+ a[4] = 476470.3222;
+ a[5] = 729482.8271;
+ a[6] = -145421.402;
+ a[7] = 2931.1506;
+ a[8] = -40.427932;
+ a[9] = 5106.192;
+ a[10] = 15711.36;
+ a[11] = -155011.1084;
+ a[12] = 4360.53352;
+ a[13] = 12.9492344;
+ a[14] = 10236.884;
+ a[15] = 13176.786;
+ a[16] = -326669.5104;
+ a[17] = 7390.68412;
+ a[18] = -27.8986976;
+ a[19] = 16643.076;
+ a[20] = 30988.146;
+ b[0] = 2.94e5;
+ b[1] = 2.94e5;
+ b[2] = 277200.;
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 6;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_4.x[0] = 2.52;
+ l2_4.x[1] = 2.;
+ l2_4.x[2] = 37.5;
+ l2_4.x[3] = 9.25;
+ l2_4.x[4] = 6.8;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l12_4.lxu[i__ - 1] = true;
+/* labelL6: */
+ l11_4.lxl[i__ - 1] = true;
+ }
+ l13_4.xl[0] = 0.;
+ l13_4.xl[1] = 1.2;
+ l13_4.xl[2] = 20.;
+ l13_4.xl[3] = 9.;
+ l13_4.xl[4] = 6.5;
+ l14_4.xu[0] = 1e3;
+ l14_4.xu[1] = 2.4;
+ l14_4.xu[2] = 60.;
+ l14_4.xu[3] = 9.3;
+ l14_4.xu[4] = 7.;
+ l20_7.lex = false;
+ l20_7.xex[0] = 4.53743097466;
+ l20_7.xex[1] = 2.40000000002;
+ l20_7.xex[2] = 60.;
+ l20_7.xex[3] = 9.29999999999;
+ l20_7.xex[4] = 7.;
+ l20_7.fex = -52.803351330600002;
+ return 0;
+labelL2:
+ l6_1.fx = -(a[0] + l2_4.x[0] * (a[1] + a[2] * l2_4.x[1] + a[3] * l2_4.x[2]
+ + a[4] * l2_4.x[3] + a[5] * l2_4.x[4]));
+ l6_1.fx *= 1e-5;
+ return 0;
+labelL3:
+ l4_4.gf[0] = -(a[1] + a[2] * l2_4.x[1] + a[3] * l2_4.x[2] + a[4] * l2_4.x[
+ 3] + a[5] * l2_4.x[4]) * 1e-5;
+ for (i__ = 2; i__ <= 5; ++i__) {
+/* L30: */
+ l4_4.gf[i__ - 1] = -a[i__] * l2_4.x[0] * 1e-5;
+ }
+ return 0;
+labelL4:
+ for (i__ = 1; i__ <= 3; ++i__) {
+ if (! (l9_6.index1[i__ - 1] || l9_6.index1[i__ + 2])) {
+ goto L80;
+ }
+ i1 = i__ * 5;
+ v1 = l2_4.x[0] * (a[i1 + 1] + a[i1 + 2] * l2_4.x[1] + a[i1 + 3] *
+ l2_4.x[2] + a[i1 + 4] * l2_4.x[3] + a[i1 + 5] * l2_4.x[4]);
+ if (l9_6.index1[i__ - 1]) {
+ l3_5.g[i__ - 1] = v1;
+ }
+ if (l9_6.index1[i__ + 2]) {
+ l3_5.g[i__ + 2] = b[i__ - 1] - v1;
+ }
+L80:
+ ;
+ }
+ return 0;
+labelL5:
+ for (i__ = 1; i__ <= 3; ++i__) {
+ if (! (l10_6.index2[i__ - 1] || l10_6.index2[i__ + 2])) {
+ goto L90;
+ }
+ i1 = i__ * 5 + 1;
+ if (! l10_6.index2[i__ - 1]) {
+ goto L95;
+ }
+ l5_14.gg[i__ - 1] = a[i1] + a[i1 + 1] * l2_4.x[1] + a[i1 + 2] *
+ l2_4.x[2] + a[i1 + 3] * l2_4.x[3] + a[i1 + 4] * l2_4.x[4];
+ for (j = 2; j <= 5; ++j) {
+/* L91: */
+ l5_14.gg[i__ + j * 6 - 7] = a[i1 + j - 1] * l2_4.x[0];
+ }
+ if (! l10_6.index2[i__ + 2]) {
+ goto L90;
+ }
+ for (j = 1; j <= 5; ++j) {
+/* L92: */
+ l5_14.gg[i__ + 3 + j * 6 - 7] = -l5_14.gg[i__ + j * 6 - 7];
+ }
+ goto L90;
+L95:
+ l5_14.gg[i__ + 2] = -(a[i1] + a[i1 + 1] * l2_4.x[1] + a[i1 + 2] *
+ l2_4.x[2] + a[i1 + 3] * l2_4.x[3] + a[i1 + 4] * l2_4.x[4]);
+ for (j = 2; j <= 5; ++j) {
+/* L96: */
+ l5_14.gg[i__ + 3 + j * 6 - 7] = -a[i1 + j - 1] * l2_4.x[0];
+ }
+L90:
+ ;
+ }
+ return 0;
+} /* tp84_ */
+
+
+/* Subroutine */ int tp85_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static Real a[17], b[17], c__[17];
+ static int i__, j;
+ static Real y[17], v1, v2, v3, v4, v5, v6, v7, dc[85] /* was [17][5]
+ */, dy[85] /* was [17][5] */;
+
+ if (*mode - 2 >= 0) {
+ goto L17;
+ } else {
+ goto labelL1;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 3;
+ l1_1.ninl = 35;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_4.x[0] = 900.;
+ l2_4.x[1] = 80.;
+ l2_4.x[2] = 115.;
+ l2_4.x[3] = 267.;
+ l2_4.x[4] = 27.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l11_4.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_4.lxu[i__ - 1] = true;
+ }
+ l13_4.xl[0] = 704.4148;
+ l13_4.xl[1] = 68.6;
+ l13_4.xl[2] = 0.;
+ l13_4.xl[3] = 193.;
+ l13_4.xl[4] = 25.;
+ l14_4.xu[0] = 906.3855;
+ l14_4.xu[1] = 288.88;
+ l14_4.xu[2] = 134.75;
+ l14_4.xu[3] = 287.0966;
+ l14_4.xu[4] = 84.1988;
+ a[1] = 17.505;
+ a[2] = 11.275;
+ a[3] = 214.228;
+ a[4] = 7.458;
+ a[5] = .961;
+ a[6] = 1.612;
+ a[7] = .146;
+ a[8] = 107.99;
+ a[9] = 922.693;
+ a[10] = 926.832;
+ a[11] = 18.766;
+ a[12] = 1072.163;
+ a[13] = 8961.448;
+ a[14] = .063;
+ a[15] = 71084.33;
+ a[16] = 2802713.;
+ b[1] = 1053.6667;
+ b[2] = 35.03;
+ b[3] = 665.585;
+ b[4] = 584.463;
+ b[5] = 265.916;
+ b[6] = 7.046;
+ b[7] = .222;
+ b[8] = 273.366;
+ b[9] = 1286.105;
+ b[10] = 1444.046;
+ b[11] = 537.141;
+ b[12] = 3247.039;
+ b[13] = 26844.086;
+ b[14] = .386;
+ b[15] = 1.4e5;
+ b[16] = 12146108.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l5_15.gg[i__ * 38 - 38] = 0.;
+ l5_15.gg[i__ * 38 - 37] = 0.;
+ l5_15.gg[i__ * 38 - 36] = 0.;
+ dc[i__ * 17 - 17] = 0.;
+ dc[i__ * 17 - 13] = 0.;
+ dc[i__ * 17 - 8] = 0.;
+/* L61: */
+ }
+ l5_15.gg[38] = 1.5;
+ l5_15.gg[76] = -1.;
+ l5_15.gg[39] = 1.;
+ l5_15.gg[77] = 1.;
+ l5_15.gg[40] = -1.;
+ l5_15.gg[78] = -1.;
+ dy[0] = 0.;
+ dy[17] = 1.;
+ dy[34] = 1.;
+ dy[51] = 0.;
+ dy[68] = 0.;
+ dc[51] = .024;
+ dc[21] = 100.;
+ l20_7.lex = false;
+ l20_7.xex[0] = 705.180328772;
+ l20_7.xex[1] = 68.6000529425;
+ l20_7.xex[2] = 102.900013236;
+ l20_7.xex[3] = 282.324998587;
+ l20_7.xex[4] = 37.5850413432;
+ l20_7.fex = -1.90513375046;
+ return 0;
+L17:
+ y[0] = l2_4.x[1] + l2_4.x[2] + 41.6;
+ c__[0] = l2_4.x[3] * .024 - 4.62;
+ y[1] = 12.5 / c__[0] + 12.;
+ v3 = y[1] * l2_4.x[0];
+ c__[1] = (l2_4.x[0] * 3.535e-4 + .5311) * l2_4.x[0] + v3 * .08705;
+ c__[2] = l2_4.x[0] * .052 + 78. + v3 * .002377;
+ y[2] = c__[1] / c__[2];
+ y[3] = y[2] * 19.;
+ v1 = l2_4.x[0] - y[2];
+ c__[3] = (v1 * .1956 / l2_4.x[1] + .04782) * v1 + y[3] * .6376 + y[2] *
+ 1.594;
+ c__[4] = l2_4.x[1] * 100.;
+ c__[5] = v1 - y[3];
+ c__[6] = .95 - c__[3] / c__[4];
+ y[4] = c__[5] * c__[6];
+ v2 = y[4] + y[3];
+ y[5] = v1 - v2;
+ c__[7] = v2 * .995;
+ y[6] = c__[7] / y[0];
+ y[7] = c__[7] / 3798.;
+ c__[8] = y[6] - y[6] * .0663 / y[7] - .3153;
+ y[8] = 96.82 / c__[8] + y[0] * .321;
+ y[9] = y[4] * 1.29 + y[3] * 1.258 + y[2] * 2.29 + y[5] * 1.71;
+ y[10] = l2_4.x[0] * 1.71 - y[3] * .452 + y[2] * .58;
+ c__[9] = .016349860428020738;
+ c__[10] = v3 * 1.74125;
+ c__[11] = y[9] * .995 + 1998.;
+ y[11] = c__[9] * l2_4.x[0] + c__[10] / c__[11];
+ y[12] = c__[11] - y[1] * 1.75;
+ v4 = y[8] + l2_4.x[4];
+ y[13] = l2_4.x[1] * 64.4 + 3623. + l2_4.x[2] * 58.4 + 146312. / v4;
+ c__[12] = y[9] * .995 + l2_4.x[1] * 60.8 + l2_4.x[3] * 48. - y[13] *
+ .1121 - 5095.;
+ y[14] = y[12] / c__[12];
+ y[15] = 1.48e5 - y[14] * 3.31e5 + y[12] * 40. - y[14] * 61. * y[12];
+ c__[13] = y[9] * 2324. - y[1] * 2.874e7;
+ y[16] = 1.413e7 - y[9] * 1328. - y[10] * 531. + c__[13] / c__[11];
+ c__[14] = y[12] / y[14] - y[12] / .52;
+ c__[15] = 1.104 - y[14] * .72;
+ c__[16] = v4;
+ if (*mode == 3 || *mode == 5) {
+ goto L71;
+ }
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+L71:
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L30: */
+/* Computing 2nd power */
+ d__1 = c__[0];
+ dy[i__ * 17 - 16] = dc[i__ * 17 - 17] * -12.5 / (d__1 * d__1);
+ }
+ v5 = y[1] + l2_4.x[0] * dy[1];
+ dc[1] = l2_4.x[0] * 7.07e-4 + .5311 + v5 * .08705;
+ dc[2] = v5 * .002377 + .052;
+ for (i__ = 2; i__ <= 5; ++i__) {
+ v6 = l2_4.x[0] * dy[i__ * 17 - 16];
+ dc[i__ * 17 - 16] = v6 * .08705;
+/* L32: */
+ dc[i__ * 17 - 15] = v6 * .002377;
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* Computing 2nd power */
+ d__1 = c__[2];
+ dy[i__ * 17 - 15] = (c__[2] * dc[i__ * 17 - 16] - c__[1] * dc[i__ *
+ 17 - 15]) / (d__1 * d__1);
+/* L33: */
+ dy[i__ * 17 - 14] = dy[i__ * 17 - 15] * 19.;
+ }
+ dc[3] = (v1 * .3912 / l2_4.x[1] + .04782) * (1. - dy[2]) + dy[3] * .6376
+ + dy[2] * 1.594;
+/* Computing 2nd power */
+ d__1 = l2_4.x[1];
+ dc[20] = v1 * -.1956 * (v1 + l2_4.x[1] * 2. * dy[19]) / (d__1 * d__1) +
+ dy[19] * 1.54618 + dy[20] * .6376;
+ for (i__ = 3; i__ <= 5; ++i__) {
+/* L34: */
+ dc[i__ * 17 - 14] = (1.54618 - v1 * .3912 / l2_4.x[1]) * dy[i__ * 17
+ - 15] + dy[i__ * 17 - 14] * .6376;
+ }
+ dc[5] = 1. - dy[2] - dy[3];
+ for (i__ = 2; i__ <= 5; ++i__) {
+/* L35: */
+ dc[i__ * 17 - 12] = -dy[i__ * 17 - 15] - dy[i__ * 17 - 14];
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* Computing 2nd power */
+ d__1 = c__[4];
+ dc[i__ * 17 - 11] = -(c__[4] * dc[i__ * 17 - 14] - c__[3] * dc[i__ *
+ 17 - 13]) / (d__1 * d__1);
+/* L36: */
+ dy[i__ * 17 - 13] = c__[5] * dc[i__ * 17 - 11] + c__[6] * dc[i__ * 17
+ - 12];
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L37: */
+ dy[i__ * 17 - 12] = -dy[i__ * 17 - 13] - dy[i__ * 17 - 14] - dy[i__ *
+ 17 - 15];
+ }
+ dy[5] += 1.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ dc[i__ * 17 - 10] = (dy[i__ * 17 - 13] + dy[i__ * 17 - 14]) * .995;
+/* Computing 2nd power */
+ d__1 = y[0];
+ dy[i__ * 17 - 11] = (y[0] * dc[i__ * 17 - 10] - c__[7] * dy[i__ * 17
+ - 17]) / (d__1 * d__1);
+ dy[i__ * 17 - 10] = dc[i__ * 17 - 10] / 3798.;
+/* Computing 2nd power */
+ d__1 = y[7];
+ dc[i__ * 17 - 9] = dy[i__ * 17 - 11] - (y[7] * dy[i__ * 17 - 11] - y[
+ 6] * dy[i__ * 17 - 10]) * .0663 / (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = c__[8];
+ dy[i__ * 17 - 9] = dc[i__ * 17 - 9] * -96.82 / (d__1 * d__1) + dy[i__
+ * 17 - 17] * .321;
+/* L38: */
+ dy[i__ * 17 - 8] = dy[i__ * 17 - 13] * 1.29 + dy[i__ * 17 - 14] *
+ 1.258 + dy[i__ * 17 - 15] * 2.29 + dy[i__ * 17 - 12] * 1.71;
+ }
+ dy[10] = 1.71 - dy[3] * .452 + dy[2] * .58;
+ for (i__ = 2; i__ <= 5; ++i__) {
+/* L39: */
+ dy[i__ * 17 - 7] = dy[i__ * 17 - 14] * -.452 + dy[i__ * 17 - 15] *
+ .58;
+ }
+ dc[10] = (y[1] + l2_4.x[0] * dy[1]) * 1.74125;
+ for (i__ = 2; i__ <= 5; ++i__) {
+/* L40: */
+ dc[i__ * 17 - 7] = l2_4.x[0] * 1.74125 * dy[i__ * 17 - 16];
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L41: */
+ dc[i__ * 17 - 6] = dy[i__ * 17 - 8] * .995;
+ }
+/* Computing 2nd power */
+ d__1 = c__[11];
+ dy[11] = c__[9] + l2_4.x[0] * dc[9] + (c__[11] * dc[10] - c__[10] * dc[11]
+ ) / (d__1 * d__1);
+ for (i__ = 2; i__ <= 5; ++i__) {
+/* L42: */
+/* Computing 2nd power */
+ d__1 = c__[11];
+ dy[i__ * 17 - 6] = (c__[11] * dc[i__ * 17 - 7] - c__[10] * dc[i__ *
+ 17 - 6]) / (d__1 * d__1);
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L43: */
+ dy[i__ * 17 - 5] = dc[i__ * 17 - 6] - dy[i__ * 17 - 16] * 1.75;
+ }
+/* Computing 2nd power */
+ d__1 = v4;
+ v7 = -146312. / (d__1 * d__1);
+ dy[13] = v7 * dy[8];
+ dy[30] = v7 * dy[25] + 64.4;
+ dy[47] = v7 * dy[42] + 58.4;
+ dy[64] = v7 * dy[59];
+ dy[81] = v7 * (dy[76] + 1.);
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L44: */
+ dc[i__ * 17 - 5] = dy[i__ * 17 - 8] * .995 - dy[i__ * 17 - 4] * .1121;
+ }
+ dc[29] += 60.8;
+ dc[63] += 48.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* Computing 2nd power */
+ d__1 = c__[12];
+ dy[i__ * 17 - 3] = (c__[12] * dy[i__ * 17 - 5] - y[12] * dc[i__ * 17
+ - 5]) / (d__1 * d__1);
+ dy[i__ * 17 - 2] = dy[i__ * 17 - 3] * -3.31e5 + dy[i__ * 17 - 5] *
+ 40. - (y[14] * dy[i__ * 17 - 5] + y[12] * dy[i__ * 17 - 3]) *
+ 61.;
+ dc[i__ * 17 - 4] = dy[i__ * 17 - 8] * 2324. - dy[i__ * 17 - 16] *
+ 2.874e7;
+/* Computing 2nd power */
+ d__1 = c__[11];
+ dy[i__ * 17 - 1] = dy[i__ * 17 - 8] * -1328. - dy[i__ * 17 - 7] *
+ 531. + (c__[11] * dc[i__ * 17 - 4] - c__[13] * dc[i__ * 17 -
+ 6]) / (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = y[14];
+ dc[i__ * 17 - 3] = (y[14] * dy[i__ * 17 - 5] - y[12] * dy[i__ * 17 -
+ 3]) / (d__1 * d__1) - dy[i__ * 17 - 5] / .52;
+/* L45: */
+ dc[i__ * 17 - 2] = dy[i__ * 17 - 3] * -.72;
+ }
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* L46: */
+ dc[i__ * 17 - 1] = dy[i__ * 17 - 9];
+ }
+ dc[84] = dy[76] + 1.;
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL2:
+ l6_1.fx = -(y[16] * 5.843e-7 - y[13] * 1.17e-4 - .1365 - y[12] * 2.358e-5
+ - y[15] * 1.502e-6 - y[11] * .0321 - y[4] * .004324 - c__[14] *
+ 1e-4 / c__[15] - y[1] * 37.48 / c__[11]);
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* Computing 2nd power */
+ d__1 = c__[15];
+/* Computing 2nd power */
+ d__2 = c__[11];
+ l4_4.gf[i__ - 1] = dy[i__ * 17 - 1] * -5.843e-7 + dy[i__ * 17 - 4] *
+ 1.17e-4 + dy[i__ * 17 - 5] * 2.358e-5 + dy[i__ * 17 - 2] *
+ 1.502e-6 + dy[i__ * 17 - 6] * .0321 + dy[i__ * 17 - 13] *
+ .004324 + (c__[15] * dc[i__ * 17 - 3] - c__[14] * dc[i__ * 17
+ - 2]) * 1e-4 / (d__1 * d__1) + (c__[11] * dy[i__ * 17 - 16] -
+ y[1] * dc[i__ * 17 - 6]) * 37.48 / (d__2 * d__2);
+/* L47: */
+ }
+ return 0;
+labelL4:
+ if (l9_9.index1[0]) {
+ l3_8.g[0] = l2_4.x[1] * 1.5 - l2_4.x[2];
+ }
+ if (l9_9.index1[1]) {
+ l3_8.g[1] = y[0] - 213.1;
+ }
+ if (l9_9.index1[2]) {
+ l3_8.g[2] = 405.23 - y[0];
+ }
+ for (i__ = 1; i__ <= 16; ++i__) {
+ if (l9_9.index1[i__ + 2]) {
+ l3_8.g[i__ + 2] = y[i__] - a[i__];
+ }
+ if (l9_9.index1[i__ + 18]) {
+ l3_8.g[i__ + 18] = b[i__] - y[i__];
+ }
+/* L50: */
+ }
+ if (l9_9.index1[35]) {
+ l3_8.g[35] = y[3] - y[4] * .28 / .72;
+ }
+ if (l9_9.index1[36]) {
+ l3_8.g[36] = 21. - y[1] * 3496. / c__[11];
+ }
+ if (l9_9.index1[37]) {
+ l3_8.g[37] = 62212. / c__[16] - 110.6 - y[0];
+ }
+ return 0;
+labelL5:
+ for (i__ = 1; i__ <= 16; ++i__) {
+ if (! l10_9.index2[i__ + 2]) {
+ goto L52;
+ }
+ for (j = 1; j <= 5; ++j) {
+/* L51: */
+ l5_15.gg[i__ + 3 + j * 38 - 39] = dy[i__ + 1 + j * 17 - 18];
+ }
+L52:
+ if (! l10_9.index2[i__ + 18]) {
+ goto L54;
+ }
+ for (j = 1; j <= 5; ++j) {
+/* L53: */
+ l5_15.gg[i__ + 19 + j * 38 - 39] = -dy[i__ + 1 + j * 17 - 18];
+ }
+L54:
+ ;
+ }
+ if (! l10_9.index2[35]) {
+ goto L56;
+ }
+ for (j = 1; j <= 5; ++j) {
+/* L55: */
+ l5_15.gg[j * 38 - 3] = dy[j * 17 - 14] - dy[j * 17 - 13] * .28 / .72;
+ }
+L56:
+ if (! l10_9.index2[36]) {
+ goto L58;
+ }
+ for (j = 1; j <= 5; ++j) {
+/* L57: */
+/* Computing 2nd power */
+ d__1 = c__[11];
+ l5_15.gg[j * 38 - 2] = (c__[11] * dy[j * 17 - 16] - y[1] * dc[j * 17
+ - 6]) * -3496. / (d__1 * d__1);
+ }
+L58:
+ if (! l10_9.index2[37]) {
+ goto L60;
+ }
+ for (j = 1; j <= 5; ++j) {
+/* L59: */
+/* Computing 2nd power */
+ d__1 = c__[16];
+ l5_15.gg[j * 38 - 1] = dc[j * 17 - 1] * -62212. / (d__1 * d__1) - dy[
+ j * 17 - 17];
+ }
+L60:
+ return 0;
+} /* tp85_ */
+
+
+/* Subroutine */ int tp86_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real a[50] /* was [10][5] */, b[10], c__[25] /*
+ was [5][5] */, d__[5], e[5];
+ static int i__, j;
+ static Real t;
+ static int i1;
+ static Real t1;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 10;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* L26: */
+ l2_4.x[i__ - 1] = 0.;
+ }
+ l2_4.x[4] = 1.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l11_4.lxl[i__ - 1] = true;
+ l12_4.lxu[i__ - 1] = false;
+/* labelL6: */
+ l13_4.xl[i__ - 1] = 0.;
+ }
+ e[0] = -15.;
+ e[1] = -27.;
+ e[2] = -36.;
+ e[3] = -18.;
+ e[4] = -12.;
+ c__[0] = 30.;
+ c__[5] = -20.;
+ c__[10] = -10.;
+ c__[15] = 32.;
+ c__[20] = -10.;
+ c__[6] = 39.;
+ c__[11] = -6.;
+ c__[16] = -31.;
+ c__[21] = 32.;
+ c__[12] = 10.;
+ c__[17] = -6.;
+ c__[22] = -10.;
+ c__[18] = 39.;
+ c__[23] = -20.;
+ c__[24] = 30.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ i1 = i__ + 1;
+ for (j = i1; j <= 5; ++j) {
+/* L27: */
+ c__[j + i__ * 5 - 6] = c__[i__ + j * 5 - 6];
+ }
+ }
+ d__[0] = 4.;
+ d__[1] = 8.;
+ d__[2] = 10.;
+ d__[3] = 6.;
+ d__[4] = 2.;
+ a[0] = -16.;
+ a[10] = 2.;
+ a[20] = 0.;
+ a[30] = 1.;
+ a[40] = 0.;
+ a[1] = 0.;
+ a[11] = -2.;
+ a[21] = 0.;
+ a[31] = .4;
+ a[41] = 2.;
+ a[2] = -3.5;
+ a[12] = 0.;
+ a[22] = 2.;
+ a[32] = 0.;
+ a[42] = 0.;
+ a[3] = 0.;
+ a[13] = -2.;
+ a[23] = 0.;
+ a[33] = -4.;
+ a[43] = -1.;
+ a[4] = 0.;
+ a[14] = -9.;
+ a[24] = -2.;
+ a[34] = 1.;
+ a[44] = -2.8;
+ a[5] = 2.;
+ a[15] = 0.;
+ a[25] = -4.;
+ a[35] = 0.;
+ a[45] = 0.;
+ a[7] = -1.;
+ a[17] = -2.;
+ a[27] = -3.;
+ a[37] = -2.;
+ a[47] = -1.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ a[i__ * 10 - 4] = -1.;
+ a[i__ * 10 - 2] = (Real) i__;
+/* L29: */
+ a[i__ * 10 - 1] = 1.;
+ }
+ b[0] = -40.;
+ b[1] = -2.;
+ b[2] = -.25;
+ b[3] = -4.;
+ b[4] = -4.;
+ b[5] = -1.;
+ b[6] = -40.;
+ b[7] = -60.;
+ b[8] = 5.;
+ b[9] = 1.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ for (j = 1; j <= 5; ++j) {
+/* L25: */
+ l5_16.gg[i__ + j * 10 - 11] = a[i__ + j * 10 - 11];
+ }
+ }
+ l20_7.lex = false;
+ l20_7.xex[0] = .299999999948;
+ l20_7.xex[1] = .333467606492;
+ l20_7.xex[2] = .400000000107;
+ l20_7.xex[3] = .42831010474;
+ l20_7.xex[4] = .223964873676;
+ l20_7.fex = -32.3486789716;
+ return 0;
+labelL2:
+ t = 0.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ t1 = 0.;
+ for (j = 1; j <= 5; ++j) {
+/* L21: */
+ t1 += c__[j + i__ * 5 - 6] * l2_4.x[i__ - 1] * l2_4.x[j - 1];
+ }
+/* labelL20: */
+/* Computing 3rd power */
+ d__1 = l2_4.x[i__ - 1];
+ t = t + e[i__ - 1] * l2_4.x[i__ - 1] + d__[i__ - 1] * (d__1 * (d__1 *
+ d__1)) + t1;
+ }
+ l6_1.fx = t;
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 5; ++i__) {
+ t = 0.;
+ for (j = 1; j <= 5; ++j) {
+/* L22: */
+ t += (c__[i__ + j * 5 - 6] + c__[j + i__ * 5 - 6]) * l2_4.x[j - 1]
+ ;
+ }
+/* L23: */
+/* Computing 2nd power */
+ d__1 = l2_4.x[i__ - 1];
+ l4_4.gf[i__ - 1] = e[i__ - 1] + t + d__[i__ - 1] * 3. * (d__1 * d__1);
+ }
+ return 0;
+labelL4:
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L24: */
+ if (l9_10.index1[i__ - 1]) {
+ l3_9.g[i__ - 1] = a[i__ - 1] * l2_4.x[0] + a[i__ + 9] * l2_4.x[1]
+ + a[i__ + 19] * l2_4.x[2] + a[i__ + 29] * l2_4.x[3] + a[
+ i__ + 39] * l2_4.x[4] - b[i__ - 1];
+ }
+ }
+labelL5:
+ return 0;
+} /* tp86_ */
+
+
+/* Subroutine */ int tp87_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real a, b, c__, d__, e;
+ static int i__;
+ static Real f1, f2, v1, v2, v3, v4, v5;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 6;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 4;
+ l2_5.x[0] = 390.;
+ l2_5.x[1] = 1e3;
+ l2_5.x[2] = 419.5;
+ l2_5.x[3] = 340.5;
+ l2_5.x[4] = 198.175;
+ l2_5.x[5] = .5;
+ for (i__ = 1; i__ <= 6; ++i__) {
+ l11_5.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_5.lxu[i__ - 1] = true;
+ }
+ l13_5.xl[0] = 0.;
+ l13_5.xl[1] = 0.;
+ l13_5.xl[2] = 340.;
+ l13_5.xl[3] = 340.;
+ l13_5.xl[4] = -1e3;
+ l13_5.xl[5] = 0.;
+ l14_5.xu[0] = 400.;
+ l14_5.xu[1] = 1e3;
+ l14_5.xu[2] = 420.;
+ l14_5.xu[3] = 420.;
+ l14_5.xu[4] = 1e3;
+ l14_5.xu[5] = .5236;
+ a = 131.078;
+ b = 1.48477;
+ c__ = .90798;
+ d__ =std::cos(1.47588);
+ e = std::sin(1.47588);
+ for (i__ = 3; i__ <= 6; ++i__) {
+/* L70: */
+ l4_5.gf[i__ - 1] = 0.;
+ }
+ l5_8.gg[0] = -1.;
+ l5_8.gg[4] = 0.;
+ l5_8.gg[16] = 0.;
+ l5_8.gg[1] = 0.;
+ l5_8.gg[5] = -1.;
+ l5_8.gg[17] = 0.;
+ l5_8.gg[2] = 0.;
+ l5_8.gg[6] = 0.;
+ l5_8.gg[18] = -1.;
+ l5_8.gg[3] = 0.;
+ l5_8.gg[7] = 0.;
+ l5_8.gg[19] = 0.;
+ l20_4.lex = false;
+ l20_4.xex[0] = 107.811937779;
+ l20_4.xex[1] = 196.318606955;
+ l20_4.xex[2] = 373.830728516;
+ l20_4.xex[3] = 420.;
+ l20_4.xex[4] = 21.3071293896;
+ l20_4.xex[5] = .153291953422;
+ l20_4.fex = 8927.59773493;
+ return 0;
+labelL2:
+ if (l2_5.x[0] - 300. >= 0.) {
+ goto L32;
+ } else {
+ goto L31;
+ }
+L31:
+ f1 = l2_5.x[0] * 30.;
+ goto L33;
+L32:
+ f1 = l2_5.x[0] * 31.;
+L33:
+ if (l2_5.x[1] - 100. >= 0.) {
+ goto L35;
+ } else {
+ goto L34;
+ }
+L34:
+ f2 = l2_5.x[1] * 28.;
+ goto L46;
+L35:
+ if (l2_5.x[1] - 200. >= 0.) {
+ goto L37;
+ } else {
+ goto L36;
+ }
+L36:
+ f2 = l2_5.x[1] * 29.;
+ goto L46;
+L37:
+ f2 = l2_5.x[1] * 30.;
+L46:
+ l6_1.fx = f1 + f2;
+ return 0;
+labelL3:
+ if (l2_5.x[0] - 300. >= 0.) {
+ goto L39;
+ } else {
+ goto L38;
+ }
+L38:
+ l4_5.gf[0] = 30.;
+ goto L40;
+L39:
+ l4_5.gf[0] = 31.;
+L40:
+ if (l2_5.x[1] - 100. >= 0.) {
+ goto L42;
+ } else {
+ goto L41;
+ }
+L41:
+ l4_5.gf[1] = 28.;
+ goto L45;
+L42:
+ if (l2_5.x[1] - 200. >= 0.) {
+ goto L44;
+ } else {
+ goto L43;
+ }
+L43:
+ l4_5.gf[1] = 29.;
+ goto L45;
+L44:
+ l4_5.gf[1] = 30.;
+L45:
+ return 0;
+labelL4:
+ if (l9_7.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_5.x[2];
+ l3_6.g[0] = -l2_5.x[0] + 300. - l2_5.x[2] * l2_5.x[3] / a *std::cos(b -
+ l2_5.x[5]) + c__ * (d__1 * d__1) / a * d__;
+ }
+ if (l9_7.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_5.x[3];
+ l3_6.g[1] = -l2_5.x[1] - l2_5.x[2] * l2_5.x[3] / a *std::cos(b + l2_5.x[5]
+ ) + c__ * (d__1 * d__1) / a * d__;
+ }
+ if (l9_7.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_5.x[3];
+ l3_6.g[2] = -l2_5.x[4] - l2_5.x[2] * l2_5.x[3] / a * std::sin(b + l2_5.x[5]
+ ) + c__ * (d__1 * d__1) / a * e;
+ }
+ if (l9_7.index1[3]) {
+/* Computing 2nd power */
+ d__1 = l2_5.x[2];
+ l3_6.g[3] = 200. - l2_5.x[2] * l2_5.x[3] / a * std::sin(b - l2_5.x[5]) +
+ c__ * (d__1 * d__1) / a * e;
+ }
+ return 0;
+labelL5:
+ v1 = 1. / a;
+ if (! (l10_7.index2[0] || l10_7.index2[3])) {
+ goto L8;
+ }
+ v2 = b - l2_5.x[5];
+ v3 =std::cos(v2) * v1;
+ v4 = std::sin(v2) * v1;
+ v5 = c__ * 2. * l2_5.x[2] * v1;
+ if (! l10_7.index2[0]) {
+ goto L7;
+ }
+ l5_8.gg[8] = -l2_5.x[3] * v3 + v5 * d__;
+ l5_8.gg[12] = -l2_5.x[2] * v3;
+ l5_8.gg[20] = -l2_5.x[2] * l2_5.x[3] * v4;
+L7:
+ if (! l10_7.index2[3]) {
+ goto L8;
+ }
+ l5_8.gg[11] = -l2_5.x[3] * v4 + v5 * e;
+ l5_8.gg[15] = -l2_5.x[2] * v4;
+ l5_8.gg[23] = l2_5.x[2] * l2_5.x[3] * v3;
+L8:
+ if (! (l10_7.index2[1] || l10_7.index2[2])) {
+ goto labelL10;
+ }
+ v2 = b + l2_5.x[5];
+ v3 =std::cos(v2) * v1;
+ v4 = std::sin(v2) * v1;
+ v5 = c__ * 2. * l2_5.x[3] * v1;
+ if (! l10_7.index2[1]) {
+ goto labelL9;
+ }
+ l5_8.gg[9] = -l2_5.x[3] * v3;
+ l5_8.gg[13] = -l2_5.x[2] * v3 + v5 * d__;
+ l5_8.gg[21] = l2_5.x[2] * l2_5.x[3] * v4;
+labelL9:
+ if (! l10_7.index2[2]) {
+ goto labelL10;
+ }
+ l5_8.gg[10] = -l2_5.x[3] * v4;
+ l5_8.gg[14] = -l2_5.x[2] * v4 + v5 * e;
+ l5_8.gg[22] = -l2_5.x[2] * l2_5.x[3] * v3;
+labelL10:
+ return 0;
+} /* tp87_ */
+
+
+/* Subroutine */ int tp88_0_(int n__, int *mode)
+{
+ /* System generated locals */
+ int i__1, i__2;
+ Real d__1;
+
+ int pow_ii(int*, int*);
+
+ /* Local variables */
+ static Real a[30];
+ static int i__, j;
+ static Real t[6], u, w, z__, a1, intko;
+ static int n1;
+ static Real v1, v2, v3, pi, dv[6];
+ Real gleich_(Real *);
+ static Real dz[6];
+ static Real dcosko[30];
+ static Real ep1;
+ static int kn1;
+ static Real mue[30], rho[30];
+
+ switch(n__) {
+ case 1: goto L_tp89;
+ case 2: goto L_tp90;
+ case 3: goto L_tp91;
+ case 4: goto L_tp92;
+ }
+
+ kn1 = 1;
+ l20_4.xex[0] = 1.0743187294;
+ l20_4.xex[1] = -.456613707247;
+ l20_4.fex = 1.36265680997;
+ goto L7;
+
+L_tp89:
+ kn1 = 2;
+ l20_4.xex[0] = 1.07431872754;
+ l20_4.xex[1] = -.456613706239;
+ l20_4.xex[2] = 3.00836097604e-11;
+ l20_4.fex = 1.36265680508;
+ goto L7;
+
+L_tp90:
+ kn1 = 3;
+ l20_4.xex[0] = .708479399007;
+ l20_4.xex[1] = 2.37919269592e-5;
+ l20_4.xex[2] = .807599939006;
+ l20_4.xex[3] = -.456613723294;
+ l20_4.fex = 1.36265681317;
+ goto L7;
+
+L_tp91:
+ kn1 = 4;
+ l20_4.xex[0] = .701892928031;
+ l20_4.xex[1] = 2.21084326516e-12;
+ l20_4.xex[2] = .813330836201;
+ l20_4.xex[3] = .456613707134;
+ l20_4.xex[4] = 8.99937588382e-12;
+ l20_4.fex = 1.3626568091;
+ goto L7;
+
+L_tp92:
+ kn1 = 5;
+ l20_4.xex[0] = .494144465323;
+ l20_4.xex[1] = -1.03530473697e-5;
+ l20_4.xex[2] = .61495083955;
+ l20_4.xex[3] = -2.42186612731e-6;
+ l20_4.xex[4] = .729258528936;
+ l20_4.xex[5] = -.456613099133;
+ l20_4.fex = 1.36265681213;
+L7:
+ if (*mode - 2 >= 0) {
+ goto L17;
+ } else {
+ goto labelL1;
+ }
+labelL1:
+ l1_1.n = kn1 + 1;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_5.x[(i__ << 1) - 2] = .5;
+/* labelL11: */
+ l2_5.x[(i__ << 1) - 1] = -.5;
+ }
+ for (i__ = 1; i__ <= 6; ++i__) {
+ l11_5.lxl[i__ - 1] = true;
+ l12_5.lxu[i__ - 1] = true;
+ l13_5.xl[i__ - 1] = -10.;
+/* labelL6: */
+ l14_5.xu[i__ - 1] = 10.;
+ }
+ pi = std::atan(1.) * 4.;
+ for (i__ = 1; i__ <= 30; ++i__) {
+ z__ = pi * (Real) (i__ - 1);
+ mue[i__ - 1] = gleich_(&z__);
+ v1 = std::sin(mue[i__ - 1]);
+ v2 =std::cos(mue[i__ - 1]);
+/* Computing 2nd power */
+ d__1 = mue[i__ - 1];
+ dcosko[i__ - 1] = (v1 / mue[i__ - 1] - v2) / (d__1 * d__1);
+/* labelL10: */
+ a[i__ - 1] = v1 * 2. / (mue[i__ - 1] + v1 * v2);
+ }
+ intko = .13333333333333333;
+ l20_4.lex = false;
+ return 0;
+L17:
+ if (*mode - 4 >= 0) {
+ goto L18;
+ } else {
+ goto L19;
+ }
+L18:
+ n1 = l1_1.n - 1;
+/* Computing 2nd power */
+ d__1 = l2_5.x[l1_1.n - 1];
+ t[l1_1.n - 1] = d__1 * d__1;
+ i__1 = n1;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+/* L8: */
+/* Computing 2nd power */
+ d__1 = l2_5.x[l1_1.n - i__ - 1];
+ t[l1_1.n - i__ - 1] = t[l1_1.n - i__] + d__1 * d__1;
+ }
+ v1 = 0.;
+ for (j = 1; j <= 30; ++j) {
+ v2 = mue[j - 1];
+/* Computing 2nd power */
+ d__1 = v2;
+ v3 = -(d__1 * d__1);
+ rho[j - 1] = (Real) pow_ii(&c_n1, &l1_1.n);
+ i__1 = n1;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ ep1 = 0.;
+ a1 = v3 * t[l1_1.n + 1 - i__ - 1];
+ if (a1 > -100.) {
+ ep1 = std::exp(a1);
+ }
+/* labelL14: */
+ i__2 = l1_1.n - i__;
+ rho[j - 1] += (Real) pow_ii(&c_n1, &i__2) * 2. * ep1;
+ }
+ ep1 = 0.;
+ a1 = v3 * t[0];
+ if (a1 > -100.) {
+ ep1 = std::exp(a1);
+ }
+ rho[j - 1] = (rho[j - 1] + ep1) / v3;
+/* labelL13: */
+ v1 -= v3 * a[j - 1] * rho[j - 1] * (v2 * std::sin(v2) * rho[j - 1] -
+ dcosko[j - 1] * 2.);
+ }
+L19:
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL2:
+ u = 0.;
+ i__2 = l1_1.n;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+/* labelL20: */
+/* Computing 2nd power */
+ d__1 = l2_5.x[i__ - 1];
+ u += d__1 * d__1;
+ }
+ l6_1.fx = u;
+ return 0;
+labelL3:
+ i__2 = l1_1.n;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+/* L21: */
+ l4_5.gf[i__ - 1] = l2_5.x[i__ - 1] * 2.;
+ }
+ return 0;
+labelL4:
+ l3_1.g[0] = 1e-4 - v1 - intko;
+ return 0;
+labelL5:
+ i__2 = l1_1.n;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+/* L22: */
+ dv[i__ - 1] = 0.;
+ }
+ for (j = 1; j <= 30; ++j) {
+ w = mue[j - 1];
+/* Computing 2nd power */
+ d__1 = w;
+ v1 = d__1 * d__1 * a[j - 1] * (w * std::sin(w) * rho[j - 1] - dcosko[j - 1]
+ );
+ ep1 = 0.;
+/* Computing 2nd power */
+ d__1 = mue[j - 1];
+ a1 = -(d__1 * d__1) * t[0];
+ if (a1 > -100.) {
+ ep1 = std::exp(a1);
+ }
+ dz[0] = ep1;
+ dv[0] += dz[0] * v1;
+ i__2 = l1_1.n;
+ for (i__ = 2; i__ <= i__2; ++i__) {
+ ep1 = 0.;
+/* Computing 2nd power */
+ d__1 = mue[j - 1];
+ a1 = -(d__1 * d__1) * t[i__ - 1];
+ if (a1 > -100.) {
+ ep1 = std::exp(a1);
+ }
+ i__1 = i__ + 1;
+ dz[i__ - 1] = dz[i__ - 2] + (Real) pow_ii(&c_n1, &i__1) *
+ 2. * ep1;
+/* L23: */
+ dv[i__ - 1] += dz[i__ - 1] * v1;
+ }
+/* L25: */
+ }
+ i__2 = l1_1.n;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+/* L24: */
+ l5_3.gg[i__ - 1] = dv[i__ - 1] * -4. * l2_5.x[i__ - 1];
+ }
+ return 0;
+} /* tp88_ */
+
+/* Subroutine */ int tp88_(int *mode)
+{
+ return tp88_0_(0, mode);
+ }
+
+/* Subroutine */ int tp89_(int *mode)
+{
+ return tp88_0_(1, mode);
+ }
+
+/* Subroutine */ int tp90_(int *mode)
+{
+ return tp88_0_(2, mode);
+ }
+
+/* Subroutine */ int tp91_(int *mode)
+{
+ return tp88_0_(3, mode);
+ }
+
+/* Subroutine */ int tp92_(int *mode)
+{
+ return tp88_0_(4, mode);
+ }
+
+
+/* Subroutine */ int tp93_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+ static Real v1, v2, v3, v4, v5, v6, v7, v8, v9;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 6;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_5.x[0] = 5.54;
+ l2_5.x[1] = 4.4;
+ l2_5.x[2] = 12.02;
+ l2_5.x[3] = 11.82;
+ l2_5.x[4] = .702;
+ l2_5.x[5] = .852;
+ for (i__ = 1; i__ <= 6; ++i__) {
+ l11_5.lxl[i__ - 1] = true;
+ l12_5.lxu[i__ - 1] = false;
+/* labelL6: */
+ l13_5.xl[i__ - 1] = 0.;
+ }
+ l20_4.lex = false;
+ l20_4.xex[0] = 5.33266639884;
+ l20_4.xex[1] = 4.65674439073;
+ l20_4.xex[2] = 10.4329901123;
+ l20_4.xex[3] = 12.0823085893;
+ l20_4.xex[4] = .752607369745;
+ l20_4.xex[5] = .87865083685;
+ l20_4.fex = 135.075961229;
+ return 0;
+labelL2:
+ v1 = l2_5.x[0] + l2_5.x[1] + l2_5.x[2];
+ v2 = l2_5.x[0] + l2_5.x[1] * 1.57 + l2_5.x[3];
+ v3 = l2_5.x[0] * l2_5.x[3];
+ v4 = l2_5.x[2] * l2_5.x[1];
+/* Computing 2nd power */
+ d__1 = l2_5.x[4];
+/* Computing 2nd power */
+ d__2 = l2_5.x[5];
+ l6_1.fx = v3 * .0204 * v1 + v4 * .0187 * v2 + v3 * .0607 * v1 * (d__1 *
+ d__1) + v4 * .0437 * v2 * (d__2 * d__2);
+ return 0;
+labelL3:
+ v1 = l2_5.x[0] * l2_5.x[3];
+ v2 = l2_5.x[1] * l2_5.x[2];
+ v3 = l2_5.x[1] + l2_5.x[2];
+ v4 = l2_5.x[0] + l2_5.x[3];
+ v5 = v3 + l2_5.x[0];
+ v6 = l2_5.x[1] * 1.57 + v4;
+/* Computing 2nd power */
+ d__1 = l2_5.x[4];
+ v7 = l2_5.x[3] * (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = l2_5.x[5];
+ v8 = l2_5.x[2] * (d__1 * d__1);
+ v9 = l2_5.x[0] * .0607 * v7;
+ l4_5.gf[0] = v1 * .0408 + l2_5.x[3] * .0204 * v3 + v2 * .0187 + v9 * 2. +
+ v7 * .0607 * v3 + l2_5.x[1] * .0437 * v8;
+ l4_5.gf[1] = v1 * .0204 + v2 * .058718 + l2_5.x[2] * .0187 * v4 + v9 +
+ l2_5.x[1] * .137218 * v8 + v8 * .0437 * v4;
+/* Computing 2nd power */
+ d__1 = l2_5.x[5];
+ l4_5.gf[2] = v1 * .0204 + l2_5.x[1] * .0187 * v6 + v9 + l2_5.x[1] * .0437
+ * (d__1 * d__1) * v6;
+/* Computing 2nd power */
+ d__1 = l2_5.x[4];
+ l4_5.gf[3] = l2_5.x[0] * .0204 * v5 + v2 * .0187 + l2_5.x[1] * .0437 * v8
+ + l2_5.x[0] * .0607 * (d__1 * d__1) * v5;
+ l4_5.gf[4] = v1 * .1214 * l2_5.x[4] * v5;
+ l4_5.gf[5] = l2_5.x[5] * .0874 * v2 * v6;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = l2_5.x[0] * .001 * l2_5.x[1] * l2_5.x[2] * l2_5.x[3] *
+ l2_5.x[4] * l2_5.x[5] - 2.07;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_5.x[4];
+/* Computing 2nd power */
+ d__2 = l2_5.x[5];
+ l3_2.g[1] = 1. - l2_5.x[0] * 6.2e-4 * l2_5.x[3] * (d__1 * d__1) * (
+ l2_5.x[0] + l2_5.x[1] + l2_5.x[2]) - l2_5.x[1] * 5.8e-4 *
+ l2_5.x[2] * (d__2 * d__2) * (l2_5.x[0] + l2_5.x[1] * 1.57 +
+ l2_5.x[3]);
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+ v1 = l2_5.x[0] * l2_5.x[1] * l2_5.x[2] * .001;
+ v2 = l2_5.x[3] * l2_5.x[4] * l2_5.x[5] * .001;
+ l5_7.gg[0] = l2_5.x[1] * l2_5.x[2] * v2;
+ l5_7.gg[2] = l2_5.x[0] * l2_5.x[2] * v2;
+ l5_7.gg[4] = l2_5.x[0] * l2_5.x[1] * v2;
+ l5_7.gg[6] = l2_5.x[4] * l2_5.x[5] * v1;
+ l5_7.gg[8] = l2_5.x[3] * l2_5.x[5] * v1;
+ l5_7.gg[10] = l2_5.x[3] * l2_5.x[4] * v1;
+L7:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+/* Computing 2nd power */
+ d__1 = l2_5.x[4];
+ v1 = -(d__1 * d__1) * 6.2e-4;
+/* Computing 2nd power */
+ d__1 = l2_5.x[5];
+ v2 = -(d__1 * d__1) * 5.8e-4;
+ v3 = l2_5.x[0] + l2_5.x[1] + l2_5.x[2];
+ v4 = l2_5.x[0] + l2_5.x[1] * 1.57 + l2_5.x[3];
+ v5 = v1 * v3;
+ v6 = v2 * v4;
+ v7 = v1 * l2_5.x[0] * l2_5.x[3];
+ v8 = v2 * l2_5.x[1] * l2_5.x[2];
+ l5_7.gg[1] = v7 + v5 * l2_5.x[3] + v8;
+ l5_7.gg[3] = v7 + v6 * l2_5.x[2] + v8 * 1.57;
+ l5_7.gg[5] = v7 + v6 * l2_5.x[1];
+ l5_7.gg[7] = v5 * l2_5.x[0] + v8;
+ l5_7.gg[9] = l2_5.x[0] * -.00124 * l2_5.x[3] * l2_5.x[4] * v3;
+ l5_7.gg[11] = l2_5.x[1] * -.00116 * l2_5.x[2] * l2_5.x[5] * v4;
+L8:
+ return 0;
+} /* tp93_ */
+
+
+/* Subroutine */ int tp95_0_(int n__, int *mode)
+{
+ /* System generated locals */
+ int i__1;
+
+ /* Local variables */
+ static Real b[4];
+ static int i__, kn1;
+
+ switch(n__) {
+ case 1: goto L_tp96;
+ case 2: goto L_tp97;
+ case 3: goto L_tp98;
+ }
+
+ kn1 = 1;
+ l20_4.xex[0] = -4.76149332788e-12;
+ l20_4.xex[1] = -3.55239427962e-11;
+ l20_4.xex[2] = -7.02611041315e-11;
+ l20_4.xex[3] = -1.71856469485e-11;
+ l20_4.xex[4] = -7.48993551642e-11;
+ l20_4.xex[5] = .00332330328254;
+ l20_4.fex = .0156195144282;
+ goto labelL11;
+
+L_tp96:
+ kn1 = 2;
+ l20_4.xex[0] = -5.19722825686e-12;
+ l20_4.xex[1] = -3.87748184662e-11;
+ l20_4.xex[2] = -7.66908552858e-11;
+ l20_4.xex[3] = -1.87583442974e-11;
+ l20_4.xex[4] = -8.17535626869e-11;
+ l20_4.xex[5] = .00332330328612;
+ l20_4.fex = .0156195134384;
+ goto labelL11;
+
+L_tp97:
+ kn1 = 3;
+ l20_4.xex[0] = .268564912352;
+ l20_4.xex[1] = 0.;
+ l20_4.xex[2] = 0.;
+ l20_4.xex[3] = 0.;
+ l20_4.xex[4] = .028;
+ l20_4.xex[5] = .0134000000001;
+ l20_4.fex = 3.13580912311;
+ goto labelL11;
+
+L_tp98:
+ kn1 = 4;
+ l20_4.xex[0] = .268564912323;
+ l20_4.xex[1] = 0.;
+ l20_4.xex[2] = 0.;
+ l20_4.xex[3] = 0.;
+ l20_4.xex[4] = .028;
+ l20_4.xex[5] = .0134000000001;
+ l20_4.fex = 3.13580912299;
+labelL11:
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 6;
+ l1_1.nili = 0;
+ l1_1.ninl = 4;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 6; ++i__) {
+ l2_5.x[i__ - 1] = 0.;
+ l11_5.lxl[i__ - 1] = true;
+ l12_5.lxu[i__ - 1] = true;
+/* labelL6: */
+ l13_5.xl[i__ - 1] = 0.;
+ }
+ l14_5.xu[0] = .31;
+ l14_5.xu[1] = .046;
+ l14_5.xu[2] = .068;
+ l14_5.xu[3] = .042;
+ l14_5.xu[4] = .028;
+ l14_5.xu[5] = .0134;
+ l4_5.gf[0] = 4.3;
+ l4_5.gf[1] = 31.8;
+ l4_5.gf[2] = 63.3;
+ l4_5.gf[3] = 15.8;
+ l4_5.gf[4] = 68.5;
+ l4_5.gf[5] = 4.7;
+ l5_8.gg[4] = 38.2;
+ l5_8.gg[5] = 36.8;
+ l5_8.gg[2] = 0.;
+ l5_8.gg[6] = -273.;
+ l5_8.gg[10] = 0.;
+ l5_8.gg[22] = 0.;
+ l5_8.gg[7] = -311.;
+ l5_8.gg[11] = 0.;
+ l5_8.gg[15] = 587.;
+ l5_8.gg[19] = 391.;
+ if (kn1 - 2 <= 0) {
+ goto labelL20;
+ } else {
+ goto L21;
+ }
+labelL20:
+ b[0] = 4.97;
+ b[1] = -1.88;
+ goto L22;
+L21:
+ b[0] = 32.97;
+ b[1] = 25.12;
+L22:
+ if ((i__1 = kn1 - 3) < 0) {
+ goto L25;
+ } else if (i__1 == 0) {
+ goto L24;
+ } else {
+ goto L23;
+ }
+L23:
+ b[2] = -124.08;
+ b[3] = -173.02;
+ goto L27;
+L24:
+ b[2] = -29.08;
+ b[3] = -78.02;
+ goto L27;
+L25:
+ if (kn1 - 2 >= 0) {
+ goto L26;
+ } else {
+ goto L24;
+ }
+L26:
+ b[2] = -69.08;
+ b[3] = -118.02;
+L27:
+ l20_4.lex = false;
+ return 0;
+labelL2:
+ l6_1.fx = l2_5.x[0] * 4.3 + l2_5.x[1] * 31.8 + l2_5.x[2] * 63.3 + l2_5.x[
+ 3] * 15.8 + l2_5.x[4] * 68.5 + l2_5.x[5] * 4.7;
+labelL3:
+ return 0;
+labelL4:
+ if (l9_7.index1[0]) {
+ l3_6.g[0] = l2_5.x[0] * (float)17.1 + l2_5.x[1] * (float)38.2 +
+ l2_5.x[2] * (float)204.2 + l2_5.x[3] * (float)212.3 + l2_5.x[
+ 4] * (float)623.4 + l2_5.x[5] * 1495.5 - l2_5.x[0] * (float)
+ 169. * l2_5.x[2] - l2_5.x[2] * 3580. * l2_5.x[4] - l2_5.x[3] *
+ 3810. * l2_5.x[4] - l2_5.x[3] * 18500. * l2_5.x[5] - l2_5.x[
+ 4] * 24300. * l2_5.x[5] - b[0];
+ }
+ if (l9_7.index1[1]) {
+ l3_6.g[1] = l2_5.x[0] * (float)17.9 + l2_5.x[1] * (float)36.8 +
+ l2_5.x[2] * (float)113.9 + l2_5.x[3] * (float)169.7 + l2_5.x[
+ 4] * (float)337.8 + l2_5.x[5] * 1385.2 - l2_5.x[0] * (float)
+ 139. * l2_5.x[2] - l2_5.x[3] * 2450. * l2_5.x[4] - l2_5.x[3] *
+ 16600. * l2_5.x[5] - l2_5.x[4] * 17200. * l2_5.x[5] - b[1];
+ }
+ if (l9_7.index1[2]) {
+ l3_6.g[2] = l2_5.x[1] * (float)-273. - l2_5.x[3] * (float)70. -
+ l2_5.x[4] * (float)819. + l2_5.x[3] * 2.6e4 * l2_5.x[4] - b[2]
+ ;
+ }
+ if (l9_7.index1[3]) {
+ l3_6.g[3] = l2_5.x[0] * 159.9 - l2_5.x[1] * 311. + l2_5.x[3] * 587. +
+ l2_5.x[4] * (float)391. + l2_5.x[5] * 2198. - l2_5.x[0] *
+ 1.4e4 * l2_5.x[5] - b[3];
+ }
+ return 0;
+labelL5:
+ if (! l10_7.index2[0]) {
+ goto L7;
+ }
+ l5_8.gg[0] = (float)17.1 - l2_5.x[2] * (float)169.;
+ l5_8.gg[8] = (float)204.2 - l2_5.x[0] * (float)169. - l2_5.x[4] * 3580.;
+ l5_8.gg[12] = (float)212.3 - l2_5.x[4] * 3810. - l2_5.x[5] * 18500.;
+ l5_8.gg[16] = (float)623.4 - l2_5.x[2] * 3580. - l2_5.x[3] * 3810. -
+ l2_5.x[5] * 24300.;
+ l5_8.gg[20] = 1495.5 - l2_5.x[3] * 18500. - l2_5.x[4] * 24300.;
+L7:
+ if (! l10_7.index2[1]) {
+ goto L8;
+ }
+ l5_8.gg[1] = (float)17.9 - l2_5.x[2] * (float)139.;
+ l5_8.gg[9] = (float)113.9 - l2_5.x[0] * (float)139.;
+ l5_8.gg[13] = (float)169.7 - l2_5.x[4] * 2450. - l2_5.x[5] * 16600.;
+ l5_8.gg[17] = (float)337.8 - l2_5.x[3] * 2450. - l2_5.x[5] * 17200.;
+ l5_8.gg[21] = 1385.2 - l2_5.x[3] * 16600. - l2_5.x[4] * 17200.;
+L8:
+ if (! l10_7.index2[2]) {
+ goto labelL9;
+ }
+ l5_8.gg[14] = l2_5.x[4] * 2.6e4 - 70.;
+ l5_8.gg[18] = l2_5.x[3] * 2.6e4 - 819.;
+labelL9:
+ if (! l10_7.index2[3]) {
+ goto labelL10;
+ }
+ l5_8.gg[3] = (float)159.9 - l2_5.x[5] * 1.4e4;
+ l5_8.gg[23] = 2198. - l2_5.x[0] * 1.4e4;
+labelL10:
+ return 0;
+} /* tp95_ */
+
+/* Subroutine */ int tp95_(int *mode)
+{
+ return tp95_0_(0, mode);
+ }
+
+/* Subroutine */ int tp96_(int *mode)
+{
+ return tp95_0_(1, mode);
+ }
+
+/* Subroutine */ int tp97_(int *mode)
+{
+ return tp95_0_(2, mode);
+ }
+
+/* Subroutine */ int tp98_(int *mode)
+{
+ return tp95_0_(3, mode);
+ }
+
+
+/* Subroutine */ int tp99_(int *mode)
+{
+ /* System generated locals */
+ int i__1;
+ Real d__1;
+
+ /* Local variables */
+ static Real a[8];
+ static int i__, j;
+ static Real p[8], q[8], r__[8], s[8], t[8];
+ static int i1;
+ static Real v1, v2, v3, v4, dp[56] /* was [8][7] */, dq[56]
+ /* was [8][7] */, dr[56] /* was [8][7] */, ds[56] /*
+ was [8][7] */;
+
+ if ((i__1 = *mode - 2) < 0) {
+ goto labelL1;
+ } else if (i__1 == 0) {
+ goto L18;
+ } else {
+ goto L17;
+ }
+labelL1:
+ l1_1.n = 7;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 2;
+ for (i__ = 1; i__ <= 7; ++i__) {
+ l2_6.x[i__ - 1] = .5;
+ l11_6.lxl[i__ - 1] = true;
+ l12_6.lxu[i__ - 1] = true;
+ l13_6.xl[i__ - 1] = 0.;
+/* labelL6: */
+ l14_6.xu[i__ - 1] = 1.58;
+ }
+ a[0] = 0.;
+ a[1] = 50.;
+ a[2] = 50.;
+ a[3] = 75.;
+ a[4] = 75.;
+ a[5] = 75.;
+ a[6] = 100.;
+ a[7] = 100.;
+ t[0] = 0.;
+ t[1] = 25.;
+ t[2] = 50.;
+ t[3] = 100.;
+ t[4] = 150.;
+ t[5] = 200.;
+ t[6] = 290.;
+ t[7] = 380.;
+ p[0] = 0.;
+ q[0] = 0.;
+ r__[0] = 0.;
+ s[0] = 0.;
+ for (j = 1; j <= 7; ++j) {
+ i__1 = j;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ dp[i__ + (j << 3) - 9] = 0.;
+ dq[i__ + (j << 3) - 9] = 0.;
+ dr[i__ + (j << 3) - 9] = 0.;
+/* L31: */
+ ds[i__ + (j << 3) - 9] = 0.;
+ }
+ }
+ l20_8.lex = false;
+ l20_8.xex[0] = .542460319142;
+ l20_8.xex[1] = .529015870015;
+ l20_8.xex[2] = .508450583169;
+ l20_8.xex[3] = .480269265187;
+ l20_8.xex[4] = .451235157238;
+ l20_8.xex[5] = .409187805755;
+ l20_8.xex[6] = .352784693565;
+ l20_8.fex = -831079891.516;
+ return 0;
+L17:
+ if (*mode - 4 <= 0) {
+ goto L18;
+ } else {
+ goto L19;
+ }
+L18:
+ for (i__ = 2; i__ <= 8; ++i__) {
+ i1 = i__ - 1;
+ v1 = a[i__ - 1] * std::sin(l2_6.x[i1 - 1]) - 32.;
+ v2 = a[i__ - 1] *std::cos(l2_6.x[i1 - 1]);
+ v3 = t[i__ - 1] - t[i1 - 1];
+/* Computing 2nd power */
+ d__1 = v3;
+ v4 = d__1 * d__1 * .5;
+ p[i__ - 1] = v2 * v4 + v3 * r__[i1 - 1] + p[i1 - 1];
+ q[i__ - 1] = v1 * v4 + v3 * s[i1 - 1] + q[i1 - 1];
+ r__[i__ - 1] = v2 * v3 + r__[i1 - 1];
+/* L30: */
+ s[i__ - 1] = v1 * v3 + s[i1 - 1];
+ }
+ if (*mode - 3 != 0) {
+ goto L40;
+ } else {
+ goto L19;
+ }
+L19:
+ for (i__ = 2; i__ <= 8; ++i__) {
+ for (j = 1; j <= 7; ++j) {
+ if ((i__1 = j - i__ + 1) < 0) {
+ goto L33;
+ } else if (i__1 == 0) {
+ goto L32;
+ } else {
+ goto L34;
+ }
+L32:
+ i1 = i__ - 1;
+ v1 = a[i__ - 1] * std::sin(l2_6.x[i1 - 1]);
+ v2 = a[i__ - 1] *std::cos(l2_6.x[i1 - 1]);
+ v3 = t[i__ - 1] - t[i1 - 1];
+/* Computing 2nd power */
+ d__1 = v3;
+ v4 = d__1 * d__1 * .5;
+ dp[i__ + (i1 << 3) - 9] = -v1 * v4 + v3 * dr[i1 + (i1 << 3) - 9]
+ + dp[i1 + (i1 << 3) - 9];
+ dq[i__ + (i1 << 3) - 9] = v2 * v4 + v3 * ds[i1 + (i1 << 3) - 9] +
+ dq[i1 + (i1 << 3) - 9];
+ dr[i__ + (i1 << 3) - 9] = -v1 * v3 + dr[i1 + (i1 << 3) - 9];
+ ds[i__ + (i1 << 3) - 9] = v2 * v3 + ds[i1 + (i1 << 3) - 9];
+ goto L34;
+L33:
+ i1 = i__ - 1;
+ v1 = t[i__ - 1] - t[i1 - 1];
+ dp[i__ + (j << 3) - 9] = v1 * dr[i1 + (j << 3) - 9] + dp[i1 + (j
+ << 3) - 9];
+ dq[i__ + (j << 3) - 9] = v1 * ds[i1 + (j << 3) - 9] + dq[i1 + (j
+ << 3) - 9];
+ dr[i__ + (j << 3) - 9] = dr[i1 + (j << 3) - 9];
+ ds[i__ + (j << 3) - 9] = ds[i1 + (j << 3) - 9];
+L34:
+ ;
+ }
+ }
+L40:
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL2:
+/* Computing 2nd power */
+ d__1 = r__[7];
+ l6_1.fx = -(d__1 * d__1);
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 7; ++i__) {
+/* L35: */
+ l4_6.gf[i__ - 1] = r__[7] * -2. * dr[(i__ << 3) - 1];
+ }
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = q[7] - 1e5;
+ }
+ if (l9_3.index1[1]) {
+ l3_2.g[1] = s[7] - 1e3;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+ for (i__ = 1; i__ <= 7; ++i__) {
+/* L36: */
+ l5_17.gg[(i__ << 1) - 2] = dq[(i__ << 3) - 1];
+ }
+L7:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+ for (i__ = 1; i__ <= 7; ++i__) {
+/* L37: */
+ l5_17.gg[(i__ << 1) - 1] = ds[(i__ << 3) - 1];
+ }
+L8:
+ return 0;
+} /* tp99_ */
+
+
+/* Subroutine */ int tp100_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5, d__6, d__7;
+
+ /* Local variables */
+ static int i__;
+ static Real v1, v2;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 7;
+ l1_1.nili = 0;
+ l1_1.ninl = 4;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 7; ++i__) {
+ l11_6.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_6.lxu[i__ - 1] = false;
+ }
+ l2_6.x[0] = 1.;
+ l2_6.x[1] = 2.;
+ l2_6.x[2] = 0.;
+ l2_6.x[3] = 4.;
+ l2_6.x[4] = 0.;
+ l2_6.x[5] = 1.;
+ l2_6.x[6] = 1.;
+ l5_11.gg[8] = -1.;
+ l5_11.gg[16] = -5.;
+ l5_11.gg[20] = 0.;
+ l5_11.gg[24] = 0.;
+ l5_11.gg[1] = -7.;
+ l5_11.gg[5] = -3.;
+ l5_11.gg[13] = -1.;
+ l5_11.gg[17] = 1.;
+ l5_11.gg[21] = 0.;
+ l5_11.gg[25] = 0.;
+ l5_11.gg[2] = -23.;
+ l5_11.gg[10] = 0.;
+ l5_11.gg[14] = 0.;
+ l5_11.gg[18] = 0.;
+ l5_11.gg[26] = 8.;
+ l5_11.gg[15] = 0.;
+ l5_11.gg[19] = 0.;
+ l5_11.gg[23] = -5.;
+ l5_11.gg[27] = 11.;
+ l20_8.lex = false;
+ l20_8.xex[0] = 2.33049937431;
+ l20_8.xex[1] = 1.95137237315;
+ l20_8.xex[2] = -.477541392625;
+ l20_8.xex[3] = 4.36572623462;
+ l20_8.xex[4] = -.624486970475;
+ l20_8.xex[5] = 1.03813101881;
+ l20_8.xex[6] = 1.59422671137;
+ l20_8.fex = 680.630057275;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_6.x[0] - 10.;
+/* Computing 2nd power */
+ d__2 = l2_6.x[1] - 12.;
+/* Computing 4th power */
+ d__3 = l2_6.x[2], d__3 *= d__3;
+/* Computing 2nd power */
+ d__4 = l2_6.x[3] - 11.;
+/* Computing 6th power */
+ d__5 = l2_6.x[4], d__5 *= d__5;
+/* Computing 2nd power */
+ d__6 = l2_6.x[5];
+/* Computing 4th power */
+ d__7 = l2_6.x[6], d__7 *= d__7;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 * 5. + d__3 * d__3 + d__4 * d__4 * 3.
+ + d__5 * (d__5 * d__5) * 10. + d__6 * d__6 * 7. + d__7 * d__7 -
+ l2_6.x[5] * 4. * l2_6.x[6] - l2_6.x[5] * 10. - l2_6.x[6] * 8.;
+ return 0;
+labelL3:
+ l4_6.gf[0] = (l2_6.x[0] - 10.) * 2.;
+ l4_6.gf[1] = (l2_6.x[1] - 12.) * 10.;
+/* Computing 3rd power */
+ d__1 = l2_6.x[2];
+ l4_6.gf[2] = d__1 * (d__1 * d__1) * 4.;
+ l4_6.gf[3] = (l2_6.x[3] - 11.) * 6.;
+/* Computing 5th power */
+ d__1 = l2_6.x[4], d__2 = d__1, d__1 *= d__1;
+ l4_6.gf[4] = d__2 * (d__1 * d__1) * 60.;
+ l4_6.gf[5] = l2_6.x[5] * 14. - l2_6.x[6] * 4. - 10.;
+/* Computing 3rd power */
+ d__1 = l2_6.x[6];
+ l4_6.gf[6] = d__1 * (d__1 * d__1) * 4. - l2_6.x[5] * 4. - 8.;
+ return 0;
+labelL4:
+/* Computing 2nd power */
+ d__1 = l2_6.x[0];
+ v1 = d__1 * d__1 * 2.;
+/* Computing 2nd power */
+ d__1 = l2_6.x[1];
+ v2 = d__1 * d__1;
+ if (l9_7.index1[0]) {
+/* Computing 2nd power */
+ d__1 = v2;
+/* Computing 2nd power */
+ d__2 = l2_6.x[3];
+ l3_6.g[0] = -v1 - d__1 * d__1 * 3. - l2_6.x[2] - d__2 * d__2 * 4. -
+ l2_6.x[4] * 5. + 127.;
+ }
+ if (l9_7.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_6.x[2];
+ l3_6.g[1] = l2_6.x[0] * -7. - l2_6.x[1] * 3. - d__1 * d__1 * 10. -
+ l2_6.x[3] + l2_6.x[4] + 282.;
+ }
+ if (l9_7.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_6.x[5];
+ l3_6.g[2] = l2_6.x[0] * -23. - v2 - d__1 * d__1 * 6. + l2_6.x[6] * 8.
+ + 196.;
+ }
+ if (l9_7.index1[3]) {
+/* Computing 2nd power */
+ d__1 = l2_6.x[2];
+ l3_6.g[3] = v1 * -2. - v2 + l2_6.x[0] * 3. * l2_6.x[1] - d__1 * d__1 *
+ 2. - l2_6.x[5] * 5. + l2_6.x[6] * 11.;
+ }
+ return 0;
+labelL5:
+ if (! l10_7.index2[0]) {
+ goto L7;
+ }
+ l5_11.gg[0] = l2_6.x[0] * -4.;
+/* Computing 3rd power */
+ d__1 = l2_6.x[1];
+ l5_11.gg[4] = d__1 * (d__1 * d__1) * -12.;
+ l5_11.gg[12] = l2_6.x[3] * -8.;
+L7:
+ if (l10_7.index2[1]) {
+ l5_11.gg[9] = l2_6.x[2] * -20.;
+ }
+ if (! l10_7.index2[2]) {
+ goto labelL9;
+ }
+ l5_11.gg[6] = l2_6.x[1] * -2.;
+ l5_11.gg[22] = l2_6.x[5] * -12.;
+labelL9:
+ if (! l10_7.index2[3]) {
+ goto labelL10;
+ }
+ l5_11.gg[3] = l2_6.x[0] * -8. + l2_6.x[1] * 3.;
+ l5_11.gg[7] = l2_6.x[1] * -2. + l2_6.x[0] * 3.;
+ l5_11.gg[11] = l2_6.x[2] * -4.;
+labelL10:
+ return 0;
+} /* tp100_ */
+
+
+/* Subroutine */ int tp101_0_(int n__, int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5, d__6, d__7, d__8, d__9, d__10,
+ d__11, d__12, d__13;
+
+ /* Local variables */
+ static Real fmin[3], a[3];
+ static int i__, k, m;
+ static Real v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13,
+ v14, v15, gv[7], v16, v17, v18, v19, v20, v21, v22, v23, v24, v25,
+ v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38,
+ v39, v40, v41, v42, v43, v44, v45;
+ static int kn1;
+ static Real sum;
+
+ switch(n__) {
+ case 1: goto L_tp102;
+ case 2: goto L_tp103;
+ }
+
+ kn1 = 1;
+ l20_8.xex[0] = 2.85615855584;
+ l20_8.xex[1] = .610823030755;
+ l20_8.xex[2] = 2.15081256203;
+ l20_8.xex[3] = 4.71287370945;
+ l20_8.xex[4] = .999487540961;
+ l20_8.xex[5] = 1.34750750498;
+ l20_8.xex[6] = .0316527664991;
+ l20_8.fex = 1809.76476556;
+ goto labelL13;
+
+L_tp102:
+ kn1 = 2;
+ l20_8.xex[0] = 3.89625319099;
+ l20_8.xex[1] = .809358760118;
+ l20_8.xex[2] = 2.66438599373;
+ l20_8.xex[3] = 4.30091287458;
+ l20_8.xex[4] = .853554935267;
+ l20_8.xex[5] = 1.09528744459;
+ l20_8.xex[6] = .0273104596581;
+ l20_8.fex = 911.880571336;
+ goto labelL13;
+
+L_tp103:
+ kn1 = 3;
+ l20_8.xex[0] = 4.39410451026;
+ l20_8.xex[1] = .854468738817;
+ l20_8.xex[2] = 2.8432303138;
+ l20_8.xex[3] = 3.39997866779;
+ l20_8.xex[4] = .722926133025;
+ l20_8.xex[5] = .87040638184;
+ l20_8.xex[6] = .0246388263302;
+ l20_8.fex = 543.667958424;
+labelL13:
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 7;
+ m = kn1;
+ a[0] = -.25;
+ a[1] = .125;
+ a[2] = .5;
+ l1_1.nili = 0;
+ l1_1.ninl = 6;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 7; ++i__) {
+ l2_6.x[i__ - 1] = 6.;
+ l13_6.xl[i__ - 1] = .1;
+ l14_6.xu[i__ - 1] = 10.;
+ l11_6.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_6.lxu[i__ - 1] = true;
+ }
+ l13_6.xl[6] = .01;
+ l5_12.gg[24] = 0.;
+ l5_12.gg[37] = 0.;
+ l5_12.gg[20] = 0.;
+ l5_12.gg[33] = 0.;
+ l20_8.lex = false;
+ return 0;
+labelL2:
+ for (k = 1; k <= 7; ++k) {
+/* L200: */
+ if (l2_6.x[k - 1] < 0.) {
+ goto labelL14;
+ }
+ }
+/* Computing 2nd power */
+ d__1 = l2_6.x[3];
+/* Computing 3rd power */
+ d__2 = l2_6.x[5];
+/* Computing 2nd power */
+ d__3 = l2_6.x[1];
+/* Computing 2nd power */
+ d__4 = l2_6.x[0];
+/* Computing 2nd power */
+ d__5 = l2_6.x[4];
+/* Computing 2nd power */
+ d__6 = l2_6.x[0];
+/* Computing 2nd power */
+ d__7 = l2_6.x[1];
+/* Computing 2nd power */
+ d__8 = l2_6.x[5];
+ l6_1.fx = l2_6.x[0] * 10. * (d__1 * d__1) * pow_dd(&l2_6.x[6], &a[m - 1])
+ / (l2_6.x[1] * (d__2 * (d__2 * d__2))) + l2_6.x[2] * 15. * l2_6.x[
+ 3] / (l2_6.x[0] * (d__3 * d__3) * l2_6.x[4] * pow_dd(&l2_6.x[6], &
+ c_b590)) + l2_6.x[1] * 20. * l2_6.x[5] / (d__4 * d__4 * l2_6.x[3]
+ * (d__5 * d__5)) + d__6 * d__6 * 25. * (d__7 * d__7) * pow_dd(&
+ l2_6.x[4], &c_b590) * l2_6.x[6] / (l2_6.x[2] * (d__8 * d__8));
+ return 0;
+labelL14:
+ sum = 0.;
+ for (i__ = 1; i__ <= 7; ++i__) {
+/* L40: */
+/* Computing 2nd power */
+ d__1 = l2_6.x[i__ - 1] - 5.;
+ sum += d__1 * d__1;
+ }
+ fmin[0] = 1800.;
+ fmin[1] = 910.;
+ fmin[2] = 540.;
+ l6_1.fx = sum + 1e3 + fmin[kn1 - 1];
+ return 0;
+labelL3:
+ for (k = 1; k <= 7; ++k) {
+/* L201: */
+ if (l2_6.x[k - 1] < 0.) {
+ goto L15;
+ }
+ }
+/* Computing 2nd power */
+ d__1 = l2_6.x[3];
+ v1 = d__1 * d__1 * 10.;
+ v2 = pow_dd(&l2_6.x[6], &a[m - 1]);
+/* Computing 3rd power */
+ d__1 = l2_6.x[5];
+ v3 = l2_6.x[1] * (d__1 * (d__1 * d__1));
+ v4 = l2_6.x[2] * 15. * l2_6.x[3];
+/* Computing 2nd power */
+ d__1 = l2_6.x[1];
+ v5 = l2_6.x[0] * (d__1 * d__1) * l2_6.x[4] * pow_dd(&l2_6.x[6], &c_b590);
+ v6 = l2_6.x[1] * 20. * l2_6.x[5];
+/* Computing 2nd power */
+ d__1 = l2_6.x[0];
+/* Computing 2nd power */
+ d__2 = l2_6.x[4];
+ v7 = d__1 * d__1 * l2_6.x[3] * (d__2 * d__2);
+ v8 = l2_6.x[0] * 25. * l2_6.x[1] * pow_dd(&l2_6.x[4], &c_b590) * l2_6.x[6]
+ ;
+/* Computing 2nd power */
+ d__1 = l2_6.x[5];
+ v9 = l2_6.x[2] * (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = l2_6.x[0];
+/* Computing 2nd power */
+ d__2 = l2_6.x[1];
+ v10 = d__1 * d__1 * 12.5 * (d__2 * d__2) * l2_6.x[6];
+ v11 = pow_dd(&l2_6.x[4], &c_b590);
+ l4_6.gf[0] = v1 * v2 / v3 - v4 / (l2_6.x[0] * v5) - v6 * 2. / (l2_6.x[0] *
+ v7) + l2_6.x[1] * 2. * v8 / v9;
+ l4_6.gf[1] = -v1 * l2_6.x[0] * v2 / (l2_6.x[1] * v3) - v4 * 2. / (l2_6.x[
+ 1] * v5) + l2_6.x[5] * 20. / v7 + l2_6.x[0] * 2. * v8 / v9;
+ l4_6.gf[2] = l2_6.x[3] * 15. / v5 - l2_6.x[0] * l2_6.x[1] * v8 / (l2_6.x[
+ 2] * v9);
+ l4_6.gf[3] = l2_6.x[0] * 20. * l2_6.x[3] * v2 / v3 + l2_6.x[2] * 15. / v5
+ - v6 / (l2_6.x[3] * v7);
+ l4_6.gf[4] = -v4 / (l2_6.x[4] * v5) - v6 * 2. / (l2_6.x[4] * v7) + v10 / (
+ v9 * v11);
+ l4_6.gf[5] = v1 * -3. * l2_6.x[0] * v2 / (l2_6.x[5] * v3) + l2_6.x[1] *
+ 20. / v7 - v10 * 4. * v11 / (l2_6.x[5] * v9);
+ d__1 = a[m - 1] - 1.;
+ l4_6.gf[6] = a[m - 1] * v1 * l2_6.x[0] * pow_dd(&l2_6.x[6], &d__1) / v3 -
+ v4 * .5 / (v5 * l2_6.x[6]) + v8 * l2_6.x[0] * l2_6.x[1] / (l2_6.x[
+ 6] * v9);
+ return 0;
+L15:
+ for (i__ = 1; i__ <= 7; ++i__) {
+/* L50: */
+ l4_6.gf[i__ - 1] = (l2_6.x[i__ - 1] - 5.) * 2.;
+ }
+ return 0;
+labelL4:
+ if (l9_6.index1[0]) {
+ d__1 = std::abs(l2_6.x[0]);
+/* Computing 2nd power */
+ d__2 = l2_6.x[5];
+/* Computing 3rd power */
+ d__3 = l2_6.x[0];
+ d__4 = std::abs(l2_6.x[6]);
+/* Computing 2nd power */
+ d__5 = l2_6.x[2];
+ d__6 = std::abs(l2_6.x[5]);
+ d__7 = std::abs(l2_6.x[6]);
+ d__8 = std::abs(l2_6.x[3]);
+ l3_5.g[0] = 1. - pow_dd(&d__1, &c_b590) * .5 * l2_6.x[6] / (l2_6.x[2]
+ * (d__2 * d__2)) - d__3 * (d__3 * d__3) * .7 * l2_6.x[1] *
+ l2_6.x[5] * pow_dd(&d__4, &c_b590) / (d__5 * d__5) - l2_6.x[2]
+ * .2 * pow_dd(&d__6, &c_b933) * pow_dd(&d__7, &c_b934) / (
+ l2_6.x[1] * pow_dd(&d__8, &c_b590));
+ }
+ if (l9_6.index1[1]) {
+ d__1 = std::abs(l2_6.x[0]);
+/* Computing 2nd power */
+ d__2 = l2_6.x[5];
+ d__3 = std::abs(l2_6.x[1]);
+ d__4 = std::abs(l2_6.x[5]);
+/* Computing 2nd power */
+ d__5 = l2_6.x[3];
+ l3_5.g[1] = 1. - l2_6.x[1] * 1.3 * l2_6.x[5] / (pow_dd(&d__1, &c_b590)
+ * l2_6.x[2] * l2_6.x[4]) - l2_6.x[2] * .8 * (d__2 * d__2) / (
+ l2_6.x[3] * l2_6.x[4]) - pow_dd(&d__3, &c_b590) * 3.1 *
+ pow_dd(&d__4, &c_b74) / (l2_6.x[0] * (d__5 * d__5) * l2_6.x[4]
+ );
+ }
+ if (l9_6.index1[2]) {
+ d__2 = std::abs(l2_6.x[6]);
+ d__3 = std::abs(l2_6.x[2]);
+ d__4 = (d__1 = l2_6.x[2] * l2_6.x[6], std::abs(d__1));
+ d__5 = std::abs(l2_6.x[2]);
+/* Computing 2nd power */
+ d__6 = l2_6.x[1];
+ l3_5.g[2] = 1. - l2_6.x[0] * 2. * l2_6.x[4] * pow_dd(&d__2, &c_b74) /
+ (pow_dd(&d__3, &c_b940) * l2_6.x[5]) - l2_6.x[1] * .1 *
+ l2_6.x[4] / (pow_dd(&d__4, &c_b590) * l2_6.x[5]) - l2_6.x[1] *
+ pow_dd(&d__5, &c_b590) * l2_6.x[4] / l2_6.x[0] - l2_6.x[2] *
+ .65 * l2_6.x[4] * l2_6.x[6] / (d__6 * d__6 * l2_6.x[5]);
+ }
+ if (l9_6.index1[3]) {
+ d__1 = std::abs(l2_6.x[4]);
+ d__2 = std::abs(l2_6.x[6]);
+/* Computing 2nd power */
+ d__3 = l2_6.x[0];
+ d__4 = std::abs(l2_6.x[0]);
+/* Computing 2nd power */
+ d__5 = l2_6.x[1];
+ d__6 = std::abs(l2_6.x[3]);
+ d__7 = std::abs(l2_6.x[6]);
+ d__8 = std::abs(l2_6.x[4]);
+ d__9 = std::abs(l2_6.x[6]);
+/* Computing 3rd power */
+ d__10 = l2_6.x[0];
+/* Computing 2nd power */
+ d__11 = l2_6.x[1];
+ d__12 = std::abs(l2_6.x[6]);
+/* Computing 2nd power */
+ d__13 = l2_6.x[2];
+ l3_5.g[3] = 1. - l2_6.x[1] * .2 * pow_dd(&d__1, &c_b590) * pow_dd(&
+ d__2, &c_b74) / (d__3 * d__3 * l2_6.x[3]) - pow_dd(&d__4, &
+ c_b590) * .3 * (d__5 * d__5) * l2_6.x[2] * pow_dd(&d__6, &
+ c_b74) * pow_dd(&d__7, &c_b934) / pow_dd(&d__8, &c_b933) -
+ l2_6.x[2] * .4 * l2_6.x[4] * pow_dd(&d__9, &c_b949) / (d__10 *
+ (d__10 * d__10) * (d__11 * d__11)) - l2_6.x[3] * .5 * pow_dd(
+ &d__12, &c_b590) / (d__13 * d__13);
+ }
+ if (l9_6.index1[4]) {
+/* Computing 2nd power */
+ d__1 = l2_6.x[3];
+ d__2 = std::abs(l2_6.x[6]);
+/* Computing 3rd power */
+ d__3 = l2_6.x[5];
+/* Computing 2nd power */
+ d__4 = l2_6.x[1];
+ d__5 = std::abs(l2_6.x[6]);
+/* Computing 2nd power */
+ d__6 = l2_6.x[0];
+/* Computing 2nd power */
+ d__7 = l2_6.x[4];
+/* Computing 2nd power */
+ d__8 = l2_6.x[0];
+/* Computing 2nd power */
+ d__9 = l2_6.x[1];
+ d__10 = std::abs(l2_6.x[4]);
+/* Computing 2nd power */
+ d__11 = l2_6.x[5];
+ l3_5.g[4] = l2_6.x[0] * 10. * (d__1 * d__1) * pow_dd(&d__2, &a[m - 1])
+ / (l2_6.x[1] * (d__3 * (d__3 * d__3))) + l2_6.x[2] * 15. *
+ l2_6.x[3] / (l2_6.x[0] * (d__4 * d__4) * l2_6.x[4] * pow_dd(&
+ d__5, &c_b590)) + l2_6.x[1] * 20. * l2_6.x[5] / (d__6 * d__6 *
+ l2_6.x[3] * (d__7 * d__7)) + d__8 * d__8 * 25. * (d__9 *
+ d__9) * pow_dd(&d__10, &c_b590) * l2_6.x[6] / (l2_6.x[2] * (
+ d__11 * d__11)) - 100.;
+ }
+ if (l9_6.index1[5]) {
+/* Computing 2nd power */
+ d__1 = l2_6.x[3];
+ d__2 = std::abs(l2_6.x[6]);
+/* Computing 3rd power */
+ d__3 = l2_6.x[5];
+/* Computing 2nd power */
+ d__4 = l2_6.x[1];
+ d__5 = std::abs(l2_6.x[6]);
+/* Computing 2nd power */
+ d__6 = l2_6.x[0];
+/* Computing 2nd power */
+ d__7 = l2_6.x[4];
+/* Computing 2nd power */
+ d__8 = l2_6.x[0];
+/* Computing 2nd power */
+ d__9 = l2_6.x[1];
+ d__10 = std::abs(l2_6.x[4]);
+/* Computing 2nd power */
+ d__11 = l2_6.x[5];
+ l3_5.g[5] = -(l2_6.x[0] * 10. * (d__1 * d__1) * pow_dd(&d__2, &a[m -
+ 1]) / (l2_6.x[1] * (d__3 * (d__3 * d__3))) + l2_6.x[2] * 15. *
+ l2_6.x[3] / (l2_6.x[0] * (d__4 * d__4) * l2_6.x[4] * pow_dd(&
+ d__5, &c_b590)) + l2_6.x[1] * 20. * l2_6.x[5] / (d__6 * d__6 *
+ l2_6.x[3] * (d__7 * d__7)) + d__8 * d__8 * 25. * (d__9 *
+ d__9) * pow_dd(&d__10, &c_b590) * l2_6.x[6] / (l2_6.x[2] * (
+ d__11 * d__11))) + 3e3;
+ }
+ return 0;
+labelL5:
+ if (! l10_6.index2[0]) {
+ goto L7;
+ }
+ d__1 = std::abs(l2_6.x[0]);
+ v1 = pow_dd(&d__1, &c_b590);
+/* Computing 3rd power */
+ d__1 = l2_6.x[0];
+ v2 = d__1 * (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = l2_6.x[2];
+ v4 = d__1 * d__1;
+/* Computing 2nd power */
+ d__1 = v4;
+ v5 = d__1 * d__1;
+ d__1 = std::abs(l2_6.x[3]);
+ v6 = pow_dd(&d__1, &c_b590);
+/* Computing 2nd power */
+ d__1 = l2_6.x[5];
+ v7 = d__1 * d__1;
+ d__1 = std::abs(l2_6.x[5]);
+ v8 = pow_dd(&d__1, &c_b933);
+ d__1 = std::abs(l2_6.x[6]);
+ v9 = pow_dd(&d__1, &c_b590);
+ d__1 = std::abs(l2_6.x[6]);
+ v10 = pow_dd(&d__1, &c_b934);
+/* Computing 2nd power */
+ d__1 = l2_6.x[0];
+ l5_12.gg[0] = l2_6.x[6] * -.25 / (v1 * l2_6.x[2] * v7) - d__1 * d__1 *
+ 2.1 * l2_6.x[1] * l2_6.x[5] * v9 / v4;
+/* Computing 2nd power */
+ d__1 = l2_6.x[1];
+ l5_12.gg[6] = v2 * -.7 * l2_6.x[5] * v9 / v4 + l2_6.x[2] * .2 * v8 * v10 /
+ (d__1 * d__1 * v6);
+ l5_12.gg[12] = v1 * .5 * l2_6.x[6] / (v4 * v7) + v2 * 1.4 * l2_6.x[1] *
+ l2_6.x[5] * v9 / (l2_6.x[2] * v4) - v8 * .2 * v10 / (l2_6.x[1] *
+ v6);
+ l5_12.gg[18] = l2_6.x[2] * .1 * v8 * v10 / (l2_6.x[1] * l2_6.x[3] * v6);
+ d__1 = std::abs(l2_6.x[5]);
+ l5_12.gg[30] = v1 * l2_6.x[6] / (l2_6.x[2] * v7 * l2_6.x[5]) - v2 * .7 *
+ l2_6.x[1] * v9 / v4 - l2_6.x[2] * .13333333333333333 * v10 / (
+ l2_6.x[1] * v6 * pow_dd(&d__1, &c_b74));
+ l5_12.gg[36] = v1 * -.5 / (l2_6.x[2] * v7) - v2 * .35 * l2_6.x[1] *
+ l2_6.x[5] / (v4 * v9) - l2_6.x[2] * .05 * v8 / (l2_6.x[1] * v6 *
+ v9 * v10);
+L7:
+ if (! l10_6.index2[1]) {
+ goto L8;
+ }
+ d__1 = std::abs(l2_6.x[0]);
+ v11 = pow_dd(&d__1, &c_b590);
+ d__1 = std::abs(l2_6.x[1]);
+ v12 = pow_dd(&d__1, &c_b590);
+/* Computing 2nd power */
+ d__1 = l2_6.x[3];
+ v13 = d__1 * d__1;
+/* Computing 2nd power */
+ d__1 = l2_6.x[4];
+ v14 = d__1 * d__1;
+ d__1 = std::abs(l2_6.x[5]);
+ v15 = pow_dd(&d__1, &c_b74);
+/* Computing 2nd power */
+ d__1 = l2_6.x[5];
+ v16 = d__1 * d__1;
+/* Computing 2nd power */
+ d__1 = l2_6.x[0];
+ l5_12.gg[1] = l2_6.x[1] * .65 * l2_6.x[5] / (l2_6.x[0] * v11 * l2_6.x[2] *
+ l2_6.x[4]) + v12 * 3.1 * v15 / (d__1 * d__1 * v13 * l2_6.x[4]);
+ l5_12.gg[7] = l2_6.x[5] * -1.3 / (v11 * l2_6.x[2] * l2_6.x[4]) - v15 *
+ 1.55 / (l2_6.x[0] * v12 * v13 * l2_6.x[4]);
+/* Computing 2nd power */
+ d__1 = l2_6.x[2];
+ l5_12.gg[13] = l2_6.x[1] * 1.3 * l2_6.x[5] / (v11 * (d__1 * d__1) *
+ l2_6.x[4]) - v16 * .8 / (l2_6.x[3] * l2_6.x[4]);
+ l5_12.gg[19] = l2_6.x[2] * .8 * v16 / (v13 * l2_6.x[4]) + v12 * 6.2 * v15
+ / (l2_6.x[0] * v13 * l2_6.x[3] * l2_6.x[4]);
+ l5_12.gg[25] = l2_6.x[1] * 1.3 * l2_6.x[5] / (v11 * l2_6.x[2] * v14) +
+ l2_6.x[2] * .8 * v16 / (l2_6.x[3] * v14) + v12 * 3.1 * v15 / (
+ l2_6.x[0] * v13 * v14);
+/* Computing 2nd power */
+ d__1 = v15;
+ l5_12.gg[31] = l2_6.x[1] * -1.3 / (v11 * l2_6.x[2] * l2_6.x[4]) - l2_6.x[
+ 2] * 1.6 * l2_6.x[5] / (l2_6.x[3] * l2_6.x[4]) - v12 *
+ 1.0333333333333334 / (l2_6.x[0] * v13 * l2_6.x[4] * (d__1 * d__1))
+ ;
+L8:
+ if (! l10_6.index2[2]) {
+ goto labelL9;
+ }
+/* Computing 2nd power */
+ d__1 = l2_6.x[1];
+ v17 = d__1 * d__1;
+ d__1 = std::abs(l2_6.x[2]);
+ v18 = pow_dd(&d__1, &c_b590);
+ v19 = v18 * l2_6.x[2];
+/* Computing 2nd power */
+ d__1 = l2_6.x[5];
+ v20 = d__1 * d__1;
+ d__1 = std::abs(l2_6.x[6]);
+ v21 = pow_dd(&d__1, &c_b74);
+ d__1 = std::abs(l2_6.x[6]);
+ v22 = pow_dd(&d__1, &c_b590);
+/* Computing 2nd power */
+ d__1 = l2_6.x[0];
+ l5_12.gg[2] = l2_6.x[4] * -2. * v21 / (v19 * l2_6.x[5]) + l2_6.x[1] * v18
+ * l2_6.x[4] / (d__1 * d__1);
+ l5_12.gg[8] = -v18 * l2_6.x[4] / l2_6.x[0] + l2_6.x[2] * 1.3 * l2_6.x[4] *
+ l2_6.x[6] / (v17 * l2_6.x[1] * l2_6.x[5]) - l2_6.x[4] * .1 / (
+ v18 * v22 * l2_6.x[5]);
+ l5_12.gg[14] = l2_6.x[0] * 3. * l2_6.x[4] * v21 / (l2_6.x[2] * v19 *
+ l2_6.x[5]) + l2_6.x[1] * .05 * l2_6.x[4] / (v19 * l2_6.x[5] * v22)
+ - l2_6.x[1] * .5 * l2_6.x[4] / (l2_6.x[0] * v18) - l2_6.x[4] *
+ .65 * l2_6.x[6] / (v17 * l2_6.x[5]);
+ l5_12.gg[26] = l2_6.x[0] * -2. * v21 / (v19 * l2_6.x[5]) - l2_6.x[1] * .1
+ / (v18 * l2_6.x[5] * v22) - l2_6.x[1] * v18 / l2_6.x[0] - l2_6.x[
+ 2] * .65 * l2_6.x[6] / (v17 * l2_6.x[5]);
+ l5_12.gg[32] = l2_6.x[0] * 2. * l2_6.x[4] * v21 / (v19 * v20) + l2_6.x[1]
+ * .1 * l2_6.x[4] / (v18 * v20 * v22) + l2_6.x[2] * .65 * l2_6.x[4]
+ * l2_6.x[6] / (v17 * v20);
+/* Computing 2nd power */
+ d__1 = v21;
+ l5_12.gg[38] = l2_6.x[0] * -.66666666666666663 * l2_6.x[4] / (v19 *
+ l2_6.x[5] * (d__1 * d__1)) + l2_6.x[1] * .05 * l2_6.x[4] / (v18 *
+ l2_6.x[5] * v22 * l2_6.x[6]) - l2_6.x[2] * .65 * l2_6.x[4] / (v17
+ * l2_6.x[5]);
+labelL9:
+ if (! l10_6.index2[3]) {
+ goto labelL10;
+ }
+ d__1 = std::abs(l2_6.x[0]);
+ v23 = pow_dd(&d__1, &c_b590);
+/* Computing 2nd power */
+ d__1 = l2_6.x[0];
+ v24 = d__1 * d__1;
+ v25 = v24 * l2_6.x[0];
+/* Computing 2nd power */
+ d__1 = l2_6.x[1];
+ v26 = d__1 * d__1;
+/* Computing 2nd power */
+ d__1 = l2_6.x[2];
+ v27 = d__1 * d__1;
+ d__1 = std::abs(l2_6.x[3]);
+ v28 = pow_dd(&d__1, &c_b74);
+ d__1 = std::abs(l2_6.x[4]);
+ v29 = pow_dd(&d__1, &c_b933);
+ d__1 = std::abs(l2_6.x[4]);
+ v30 = pow_dd(&d__1, &c_b590);
+ d__1 = std::abs(l2_6.x[6]);
+ v31 = pow_dd(&d__1, &c_b934);
+/* Computing 2nd power */
+ d__1 = v31;
+ v32 = d__1 * d__1;
+ v33 = v31 * v32;
+ d__1 = std::abs(l2_6.x[6]);
+ v34 = pow_dd(&d__1, &c_b74);
+/* Computing 2nd power */
+ d__1 = v24;
+ l5_12.gg[3] = l2_6.x[1] * .4 * v30 * v34 / (v25 * l2_6.x[3]) - v26 * .15 *
+ l2_6.x[2] * v28 * v31 / (v23 * v29) + l2_6.x[2] * 1.2 * l2_6.x[4]
+ * v33 / (d__1 * d__1 * v26);
+ l5_12.gg[9] = v30 * -.2 * v34 / (v24 * l2_6.x[3]) - v23 * .6 * l2_6.x[1] *
+ l2_6.x[2] * v28 * v31 / v29 + l2_6.x[2] * .8 * l2_6.x[4] * v33 /
+ (v25 * v26 * l2_6.x[1]);
+ l5_12.gg[15] = v23 * -.3 * v26 * v28 * v31 / v29 - l2_6.x[4] * .4 * v33 /
+ (v25 * v26) + l2_6.x[3] * v32 / (v27 * l2_6.x[2]);
+/* Computing 2nd power */
+ d__1 = l2_6.x[3];
+/* Computing 2nd power */
+ d__2 = v28;
+ l5_12.gg[21] = l2_6.x[1] * .2 * v30 * v34 / (v24 * (d__1 * d__1)) - v23 *
+ .1 * v26 * l2_6.x[2] * v31 / (d__2 * d__2 * v29) - v32 * .5 / v27;
+ l5_12.gg[27] = l2_6.x[1] * -.1 * v34 / (v24 * l2_6.x[3] * v30) + v23 * .2
+ * v26 * l2_6.x[2] * v28 * v31 / (l2_6.x[4] * v29) - l2_6.x[2] *
+ .4 * v33 / (v25 * v26);
+/* Computing 2nd power */
+ d__1 = v34;
+ l5_12.gg[39] = l2_6.x[1] * -.066666666666666666 * v30 / (v24 * l2_6.x[3] *
+ (d__1 * d__1)) - v23 * .075 * v26 * l2_6.x[2] * v28 / (v29 * v33)
+ - l2_6.x[2] * .3 * l2_6.x[4] / (v25 * v26 * v31) - l2_6.x[3] *
+ .25 / (v27 * v32);
+labelL10:
+ if (! l10_6.index2[4] && ! l10_6.index2[5]) {
+ goto labelL12;
+ }
+/* Computing 2nd power */
+ d__1 = l2_6.x[3];
+ v35 = d__1 * d__1 * 10.;
+ d__1 = std::abs(l2_6.x[6]);
+ v36 = pow_dd(&d__1, &a[m - 1]);
+/* Computing 3rd power */
+ d__1 = l2_6.x[5];
+ v37 = l2_6.x[1] * (d__1 * (d__1 * d__1));
+ v38 = l2_6.x[2] * 15. * l2_6.x[3];
+/* Computing 2nd power */
+ d__1 = l2_6.x[1];
+ d__2 = std::abs(l2_6.x[6]);
+ v39 = l2_6.x[0] * (d__1 * d__1) * l2_6.x[4] * pow_dd(&d__2, &c_b590);
+ v40 = l2_6.x[1] * 20. * l2_6.x[5];
+/* Computing 2nd power */
+ d__1 = l2_6.x[0];
+/* Computing 2nd power */
+ d__2 = l2_6.x[4];
+ v41 = d__1 * d__1 * l2_6.x[3] * (d__2 * d__2);
+ d__1 = std::abs(l2_6.x[4]);
+ v42 = l2_6.x[0] * 25. * l2_6.x[1] * pow_dd(&d__1, &c_b590) * l2_6.x[6];
+/* Computing 2nd power */
+ d__1 = l2_6.x[5];
+ v43 = l2_6.x[2] * (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = l2_6.x[0];
+/* Computing 2nd power */
+ d__2 = l2_6.x[1];
+ v44 = d__1 * d__1 * 12.5 * (d__2 * d__2) * l2_6.x[6];
+ d__1 = std::abs(l2_6.x[4]);
+ v45 = pow_dd(&d__1, &c_b590);
+ gv[0] = v35 * v36 / v37 - v38 / (l2_6.x[0] * v39) - v40 * 2. / (l2_6.x[0]
+ * v41) + l2_6.x[1] * 2. * v42 / v43;
+ gv[1] = -v35 * l2_6.x[0] * v36 / (l2_6.x[1] * v37) - v38 * 2. / (l2_6.x[1]
+ * v39) + l2_6.x[5] * 20. / v41 + l2_6.x[0] * 2. * v42 / v43;
+ gv[2] = l2_6.x[3] * 15. / v39 - l2_6.x[0] * l2_6.x[1] * v42 / (l2_6.x[2] *
+ v43);
+ gv[3] = l2_6.x[0] * 20. * l2_6.x[3] * v36 / v37 + l2_6.x[2] * 15. / v39 -
+ v40 / (l2_6.x[3] * v41);
+ gv[4] = -v38 / (l2_6.x[4] * v39) - v40 * 2. / (l2_6.x[4] * v41) + v44 / (
+ v43 * v45);
+ gv[5] = v35 * -3. * l2_6.x[0] * v36 / (l2_6.x[5] * v37) + l2_6.x[1] * 20.
+ / v41 - v44 * 4. * v45 / (l2_6.x[5] * v43);
+ d__1 = std::abs(l2_6.x[6]);
+ d__2 = a[m - 1] - 1.;
+ gv[6] = a[m - 1] * v35 * l2_6.x[0] * pow_dd(&d__1, &d__2) / v37 - v38 *
+ .5 / (v39 * l2_6.x[6]) + v42 * l2_6.x[0] * l2_6.x[1] / (l2_6.x[6]
+ * v43);
+ if (! l10_6.index2[4]) {
+ goto labelL11;
+ }
+ for (i__ = 1; i__ <= 7; ++i__) {
+/* labelL20: */
+ l5_12.gg[i__ * 6 - 2] = gv[i__ - 1];
+ }
+labelL11:
+ if (! l10_6.index2[5]) {
+ goto labelL12;
+ }
+ for (i__ = 1; i__ <= 7; ++i__) {
+/* L30: */
+ l5_12.gg[i__ * 6 - 1] = -gv[i__ - 1];
+ }
+labelL12:
+ return 0;
+} /* tp101_ */
+
+/* Subroutine */ int tp101_(int *mode)
+{
+ return tp101_0_(0, mode);
+ }
+
+/* Subroutine */ int tp102_(int *mode)
+{
+ return tp101_0_(1, mode);
+ }
+
+/* Subroutine */ int tp103_(int *mode)
+{
+ return tp101_0_(2, mode);
+ }
+
+
+/* Subroutine */ int tp104_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static Real a;
+ static int i__, j;
+ static Real s, v1, v2, bx;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 8;
+ l1_1.nili = 0;
+ l1_1.ninl = 6;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 8; ++i__) {
+ l11_7.lxl[i__ - 1] = true;
+ l12_7.lxu[i__ - 1] = true;
+ l13_7.xl[i__ - 1] = .1;
+ l14_7.xu[i__ - 1] = 10.;
+/* L33: */
+ }
+ a = .0588;
+ l2_7.x[0] = 6.;
+ l2_7.x[1] = 3.;
+ l2_7.x[2] = .4;
+ l2_7.x[3] = .2;
+ l2_7.x[4] = 6.;
+ l2_7.x[5] = 6.;
+ l2_7.x[6] = 1.;
+ l2_7.x[7] = .5;
+ l4_7.gf[2] = 0.;
+ l4_7.gf[3] = 0.;
+ l4_7.gf[4] = 0.;
+ l4_7.gf[5] = 0.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ for (j = 1; j <= 8; ++j) {
+/* L34: */
+ l5_18.gg[i__ + j * 6 - 7] = 0.;
+ }
+ }
+ l5_18.gg[0] = -.1;
+ l5_18.gg[1] = -.1;
+ l5_18.gg[7] = -.1;
+ l20_2.lex = false;
+ l20_2.xex[0] = 6.46511402796;
+ l20_2.xex[1] = 2.23270864907;
+ l20_2.xex[2] = .667397491303;
+ l20_2.xex[3] = .595756422907;
+ l20_2.xex[4] = 5.93267567811;
+ l20_2.xex[5] = 5.52723456506;
+ l20_2.xex[6] = 1.01332200907;
+ l20_2.xex[7] = .400668229166;
+ l20_2.fex = 3.95116343955;
+ return 0;
+labelL2:
+ if (l2_7.x[0] / l2_7.x[6] < 0. || l2_7.x[1] / l2_7.x[7] < 0.) {
+ goto labelL13;
+ }
+ d__1 = l2_7.x[0] / l2_7.x[6];
+ d__2 = l2_7.x[1] / l2_7.x[7];
+ l6_1.fx = (pow_dd(&d__1, &c_b993) + pow_dd(&d__2, &c_b993)) * .4 + 10. -
+ l2_7.x[0] - l2_7.x[1];
+ return 0;
+labelL13:
+ s = 0.;
+ for (i__ = 1; i__ <= 8; ++i__) {
+/* labelL14: */
+/* Computing 2nd power */
+ d__1 = l2_7.x[i__ - 1] - 5.;
+ s += d__1 * d__1;
+ }
+ l6_1.fx = s + 1e3 + 3.9;
+ return 0;
+labelL3:
+ if (l2_7.x[0] < 0. || l2_7.x[1] < 0. || l2_7.x[6] < 0. || l2_7.x[7] < 0.)
+ {
+ goto L15;
+ }
+ l4_7.gf[0] = pow_dd(l2_7.x, &c_b997) * .268 * pow_dd(&l2_7.x[6], &c_b998)
+ - 1.;
+ l4_7.gf[1] = pow_dd(&l2_7.x[1], &c_b997) * .268 * pow_dd(&l2_7.x[7], &
+ c_b998) - 1.;
+ l4_7.gf[6] = pow_dd(l2_7.x, &c_b993) * -.268 * pow_dd(&l2_7.x[6], &
+ c_b1002);
+ l4_7.gf[7] = pow_dd(&l2_7.x[1], &c_b993) * -.268 * pow_dd(&l2_7.x[7], &
+ c_b1002);
+ return 0;
+L15:
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l4_7.gf[i__ - 1] = (l2_7.x[i__ - 1] - 5.) * 2.;
+/* L16: */
+ l4_7.gf[i__ + 5] = (l2_7.x[i__ + 5] - 5.) * 2.;
+ }
+ return 0;
+labelL4:
+ d__3 = (d__1 = l2_7.x[0] / l2_7.x[6], std::abs(d__1));
+ d__4 = (d__2 = l2_7.x[1] / l2_7.x[7], std::abs(d__2));
+ bx = (pow_dd(&d__3, &c_b993) + pow_dd(&d__4, &c_b993)) * .4 + 10. -
+ l2_7.x[0] - l2_7.x[1];
+ if (l9_6.index1[0]) {
+ l3_5.g[0] = -a * l2_7.x[4] * l2_7.x[6] - l2_7.x[0] * .1 + 1.;
+ }
+ if (l9_6.index1[1]) {
+ l3_5.g[1] = -a * l2_7.x[5] * l2_7.x[7] - l2_7.x[0] * .1 - l2_7.x[1] *
+ .1 + 1.;
+ }
+ if (l9_6.index1[2]) {
+ d__1 = std::abs(l2_7.x[2]);
+ d__2 = std::abs(l2_7.x[2]);
+ l3_5.g[2] = (l2_7.x[2] * -4. - pow_dd(&d__1, &c_b1008) * 2.) / l2_7.x[
+ 4] - a * pow_dd(&d__2, &c_b1009) * l2_7.x[6] + 1.;
+ }
+ if (l9_6.index1[3]) {
+ d__1 = std::abs(l2_7.x[3]);
+ d__2 = std::abs(l2_7.x[3]);
+ l3_5.g[3] = (l2_7.x[3] * -4. - pow_dd(&d__1, &c_b1008) * 2.) / l2_7.x[
+ 5] - a * pow_dd(&d__2, &c_b1009) * l2_7.x[7] + 1.;
+ }
+ if (l9_6.index1[4]) {
+ l3_5.g[4] = bx - 1.;
+ }
+ if (l9_6.index1[5]) {
+ l3_5.g[5] = 4.2 - bx;
+ }
+ return 0;
+labelL5:
+ if (! l10_6.index2[0]) {
+ goto L7;
+ }
+ l5_18.gg[24] = -a * l2_7.x[6];
+ l5_18.gg[36] = -a * l2_7.x[4];
+L7:
+ if (! l10_6.index2[1]) {
+ goto L8;
+ }
+ l5_18.gg[31] = -a * l2_7.x[7];
+ l5_18.gg[43] = -a * l2_7.x[5];
+L8:
+ if (! l10_6.index2[2]) {
+ goto labelL9;
+ }
+/* Computing 2nd power */
+ d__1 = l2_7.x[4];
+ v1 = d__1 * d__1;
+ d__1 = std::abs(l2_7.x[2]);
+ d__2 = std::abs(l2_7.x[2]);
+ l5_18.gg[14] = (pow_dd(&d__1, &c_b1015) * 1.42 - 4.) / l2_7.x[4] + a *
+ 1.3 * pow_dd(&d__2, &c_b1016) * l2_7.x[6];
+ d__1 = std::abs(l2_7.x[2]);
+ l5_18.gg[26] = (l2_7.x[2] * 4. + pow_dd(&d__1, &c_b1008) * 2.) / v1;
+ d__1 = std::abs(l2_7.x[2]);
+ l5_18.gg[38] = -a * pow_dd(&d__1, &c_b1009);
+labelL9:
+ if (! l10_6.index2[3]) {
+ goto labelL10;
+ }
+/* Computing 2nd power */
+ d__1 = l2_7.x[5];
+ v2 = d__1 * d__1;
+ d__1 = std::abs(l2_7.x[3]);
+ d__2 = std::abs(l2_7.x[3]);
+ l5_18.gg[21] = (pow_dd(&d__1, &c_b1015) * 1.42 - 4.) / l2_7.x[5] + a *
+ 1.3 * pow_dd(&d__2, &c_b1016) * l2_7.x[7];
+ d__1 = std::abs(l2_7.x[3]);
+ l5_18.gg[33] = (l2_7.x[3] * 4. + pow_dd(&d__1, &c_b1008) * 2.) / v2;
+ d__1 = std::abs(l2_7.x[3]);
+ l5_18.gg[45] = -a * pow_dd(&d__1, &c_b1009);
+labelL10:
+ if (! l10_6.index2[4] && ! l10_6.index2[5]) {
+ goto labelL12;
+ }
+ d__1 = std::abs(l2_7.x[0]);
+ d__2 = std::abs(l2_7.x[6]);
+ l4_7.gf[0] = pow_dd(&d__1, &c_b997) * .268 * pow_dd(&d__2, &c_b998) - 1.;
+ d__1 = std::abs(l2_7.x[1]);
+ d__2 = std::abs(l2_7.x[7]);
+ l4_7.gf[1] = pow_dd(&d__1, &c_b997) * .268 * pow_dd(&d__2, &c_b998) - 1.;
+ d__1 = std::abs(l2_7.x[0]);
+ d__2 = std::abs(l2_7.x[6]);
+ l4_7.gf[6] = pow_dd(&d__1, &c_b993) * -.268 * pow_dd(&d__2, &c_b1002);
+ d__1 = std::abs(l2_7.x[1]);
+ d__2 = std::abs(l2_7.x[7]);
+ l4_7.gf[7] = pow_dd(&d__1, &c_b993) * -.268 * pow_dd(&d__2, &c_b1002);
+ if (! l10_6.index2[4]) {
+ goto labelL11;
+ }
+ for (i__ = 1; i__ <= 8; ++i__) {
+/* L31: */
+ l5_18.gg[i__ * 6 - 2] = l4_7.gf[i__ - 1];
+ }
+labelL11:
+ if (! l10_6.index2[5]) {
+ goto labelL12;
+ }
+ for (i__ = 1; i__ <= 8; ++i__) {
+/* L32: */
+ l5_18.gg[i__ * 6 - 1] = -l4_7.gf[i__ - 1];
+ }
+labelL12:
+ return 0;
+} /* tp104_ */
+
+
+/* Subroutine */ int tp105_(int *mode)
+{
+ /* System generated locals */
+ int i__1;
+ Real d__1;
+
+ /* Local variables */
+ static Real a[235], b[235], c__[235];
+ static int i__, j;
+ static Real s, v, y[235], t1, v0, v1, v2, v3, v4, v5, v6, v7, v8,
+ v9, da[1880] /* was [235][8] */, db[1880] /* was [235][
+ 8] */, dc[1880] /* was [235][8] */, v10, v11;
+ static Real sum;
+
+ if (*mode - 1 <= 0) {
+ goto labelL1;
+ } else {
+ goto labelL20;
+ }
+labelL1:
+ l1_1.n = 8;
+ l1_1.nili = 1;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 8; ++i__) {
+ l11_7.lxl[i__ - 1] = true;
+/* L59: */
+ l12_7.lxu[i__ - 1] = true;
+ }
+ l13_7.xl[0] = .001;
+ l13_7.xl[1] = .001;
+ l13_7.xl[2] = 100.;
+ l13_7.xl[3] = 130.;
+ l13_7.xl[4] = 170.;
+ l14_7.xu[0] = .499;
+ l14_7.xu[1] = .499;
+ l14_7.xu[2] = 180.;
+ l14_7.xu[3] = 210.;
+ l14_7.xu[4] = 240.;
+ for (i__ = 6; i__ <= 8; ++i__) {
+ l13_7.xl[i__ - 1] = 5.;
+/* L62: */
+ l14_7.xu[i__ - 1] = 25.;
+ }
+ l2_7.x[0] = .1;
+ l2_7.x[1] = .2;
+ l2_7.x[2] = 100.;
+ l2_7.x[3] = 125.;
+ l2_7.x[4] = 175.;
+ l2_7.x[5] = 11.2;
+ l2_7.x[6] = 13.2;
+ l2_7.x[7] = 15.8;
+ y[0] = 95.;
+ y[1] = 105.;
+ for (i__ = 3; i__ <= 6; ++i__) {
+/* L30: */
+ y[i__ - 1] = 110.;
+ }
+ for (i__ = 7; i__ <= 10; ++i__) {
+/* L31: */
+ y[i__ - 1] = 115.;
+ }
+ for (i__ = 11; i__ <= 25; ++i__) {
+/* L32: */
+ y[i__ - 1] = 120.;
+ }
+ for (i__ = 26; i__ <= 40; ++i__) {
+/* L33: */
+ y[i__ - 1] = 125.;
+ }
+ for (i__ = 41; i__ <= 55; ++i__) {
+/* L34: */
+ y[i__ - 1] = 130.;
+ }
+ for (i__ = 56; i__ <= 68; ++i__) {
+/* L35: */
+ y[i__ - 1] = 135.;
+ }
+ for (i__ = 69; i__ <= 89; ++i__) {
+/* L36: */
+ y[i__ - 1] = 140.;
+ }
+ for (i__ = 90; i__ <= 101; ++i__) {
+/* L37: */
+ y[i__ - 1] = 145.;
+ }
+ for (i__ = 102; i__ <= 118; ++i__) {
+/* L38: */
+ y[i__ - 1] = 150.;
+ }
+ for (i__ = 119; i__ <= 122; ++i__) {
+/* L39: */
+ y[i__ - 1] = 155.;
+ }
+ for (i__ = 123; i__ <= 142; ++i__) {
+/* L40: */
+ y[i__ - 1] = 160.;
+ }
+ for (i__ = 143; i__ <= 150; ++i__) {
+/* L41: */
+ y[i__ - 1] = 165.;
+ }
+ for (i__ = 151; i__ <= 167; ++i__) {
+/* L42: */
+ y[i__ - 1] = 170.;
+ }
+ for (i__ = 168; i__ <= 175; ++i__) {
+/* L43: */
+ y[i__ - 1] = 175.;
+ }
+ for (i__ = 176; i__ <= 181; ++i__) {
+/* L44: */
+ y[i__ - 1] = 180.;
+ }
+ for (i__ = 182; i__ <= 187; ++i__) {
+/* L45: */
+ y[i__ - 1] = 185.;
+ }
+ for (i__ = 188; i__ <= 194; ++i__) {
+/* L46: */
+ y[i__ - 1] = 190.;
+ }
+ for (i__ = 195; i__ <= 198; ++i__) {
+/* L47: */
+ y[i__ - 1] = 195.;
+ }
+ for (i__ = 199; i__ <= 201; ++i__) {
+/* L48: */
+ y[i__ - 1] = 200.;
+ }
+ for (i__ = 202; i__ <= 204; ++i__) {
+/* L49: */
+ y[i__ - 1] = 205.;
+ }
+ for (i__ = 205; i__ <= 212; ++i__) {
+/* L50: */
+ y[i__ - 1] = 210.;
+ }
+ y[212] = 215.;
+ for (i__ = 214; i__ <= 219; ++i__) {
+/* L51: */
+ y[i__ - 1] = 220.;
+ }
+ for (i__ = 220; i__ <= 224; ++i__) {
+/* L52: */
+ y[i__ - 1] = 230.;
+ }
+ y[224] = 235.;
+ for (i__ = 226; i__ <= 232; ++i__) {
+/* L53: */
+ y[i__ - 1] = 240.;
+ }
+ y[232] = 245.;
+ y[233] = 260.;
+ y[234] = 260.;
+ l5_6.gg[0] = -1.;
+ l5_6.gg[1] = -1.;
+ for (i__ = 3; i__ <= 8; ++i__) {
+/* L58: */
+ l5_6.gg[i__ - 1] = 0.;
+ }
+ l20_2.lex = false;
+ l20_2.xex[0] = .412892753597;
+ l20_2.xex[1] = .403352658261;
+ l20_2.xex[2] = 131.261311486;
+ l20_2.xex[3] = 164.313514476;
+ l20_2.xex[4] = 217.422221771;
+ l20_2.xex[5] = 12.2801780396;
+ l20_2.xex[6] = 15.7716989473;
+ l20_2.xex[7] = 20.7468249193;
+ l20_2.fex = 1138.4162396;
+ return 0;
+labelL20:
+ if ((i__1 = *mode - 4) < 0) {
+ goto L21;
+ } else if (i__1 == 0) {
+ goto labelL4;
+ } else {
+ goto labelL5;
+ }
+L21:
+ s = 0.;
+ v = 1. / std::sqrt(std::atan(1.) * 8.);
+ v1 = l2_7.x[0] / l2_7.x[5];
+ v2 = l2_7.x[1] / l2_7.x[6];
+ v3 = (1. - l2_7.x[0] - l2_7.x[1]) / l2_7.x[7];
+/* Computing 2nd power */
+ d__1 = l2_7.x[5];
+ v4 = 1. / (d__1 * d__1 * 2.);
+/* Computing 2nd power */
+ d__1 = l2_7.x[6];
+ v5 = 1. / (d__1 * d__1 * 2.);
+/* Computing 2nd power */
+ d__1 = l2_7.x[7];
+ v6 = 1. / (d__1 * d__1 * 2.);
+ for (i__ = 1; i__ <= 235; ++i__) {
+/* Computing 2nd power */
+ d__1 = y[i__ - 1] - l2_7.x[2];
+ a[i__ - 1] = v1 * std::exp(-(d__1 * d__1) * v4);
+/* Computing 2nd power */
+ d__1 = y[i__ - 1] - l2_7.x[3];
+ b[i__ - 1] = v2 * std::exp(-(d__1 * d__1) * v5);
+/* Computing 2nd power */
+ d__1 = y[i__ - 1] - l2_7.x[4];
+ c__[i__ - 1] = v3 * std::exp(-(d__1 * d__1) * v6);
+ v11 = (a[i__ - 1] + b[i__ - 1] + c__[i__ - 1]) * v;
+ if (v11 <= 0.) {
+ goto L70;
+ }
+/* L54: */
+ s += std::log(v11);
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+/* labelL2: */
+ l6_1.fx = -s;
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 235; ++i__) {
+ for (j = 1; j <= 8; ++j) {
+ da[i__ + j * 235 - 236] = 0.;
+ db[i__ + j * 235 - 236] = 0.;
+/* L60: */
+ dc[i__ + j * 235 - 236] = 0.;
+ }
+ }
+ for (i__ = 1; i__ <= 235; ++i__) {
+/* Computing 2nd power */
+ d__1 = l2_7.x[5];
+ v0 = d__1 * d__1;
+ v2 = y[i__ - 1] - l2_7.x[2];
+/* Computing 2nd power */
+ d__1 = v2;
+ v1 = std::exp(-(d__1 * d__1) / (v0 * 2.));
+ da[i__ - 1] = v1 / l2_7.x[5];
+/* Computing 3rd power */
+ d__1 = l2_7.x[5];
+ da[i__ + 469] = l2_7.x[0] * v2 / (d__1 * (d__1 * d__1)) * v1;
+/* Computing 2nd power */
+ d__1 = v2;
+ da[i__ + 1174] = l2_7.x[0] / v0 * (d__1 * d__1 / v0 - 1.) * v1;
+/* Computing 2nd power */
+ d__1 = l2_7.x[6];
+ v3 = d__1 * d__1;
+ v4 = y[i__ - 1] - l2_7.x[3];
+/* Computing 2nd power */
+ d__1 = v4;
+ v5 = std::exp(-(d__1 * d__1) / (v3 * 2.));
+ db[i__ + 234] = v5 / l2_7.x[6];
+/* Computing 3rd power */
+ d__1 = l2_7.x[6];
+ db[i__ + 704] = l2_7.x[1] * v4 / (d__1 * (d__1 * d__1)) * v5;
+/* Computing 2nd power */
+ d__1 = v4;
+ db[i__ + 1409] = l2_7.x[1] / v3 * (d__1 * d__1 / v3 - 1.) * v5;
+/* Computing 2nd power */
+ d__1 = l2_7.x[7];
+ v7 = d__1 * d__1;
+ v9 = y[i__ - 1] - l2_7.x[4];
+/* Computing 2nd power */
+ d__1 = v9;
+ v8 = std::exp(-(d__1 * d__1) / (v7 * 2.));
+ v10 = 1. - l2_7.x[0] - l2_7.x[1];
+ dc[i__ - 1] = -v8 / l2_7.x[7];
+ dc[i__ + 234] = dc[i__ - 1];
+/* Computing 3rd power */
+ d__1 = l2_7.x[7];
+ dc[i__ + 939] = v10 * v9 / (d__1 * (d__1 * d__1)) * v8;
+/* Computing 2nd power */
+ d__1 = v9;
+ dc[i__ + 1644] = v10 / v7 * (d__1 * d__1 / v7 - 1.) * v8;
+/* L55: */
+ }
+ for (j = 1; j <= 8; ++j) {
+ t1 = 0.;
+ for (i__ = 1; i__ <= 235; ++i__) {
+/* L56: */
+ t1 += (da[i__ + j * 235 - 236] + db[i__ + j * 235 - 236] + dc[i__
+ + j * 235 - 236]) / (a[i__ - 1] + b[i__ - 1] + c__[i__ -
+ 1]);
+ }
+ l4_7.gf[j - 1] = -t1;
+/* L57: */
+ }
+ return 0;
+L70:
+ for (i__ = 1; i__ <= 8; ++i__) {
+ sum = (float)0.;
+/* L71: */
+/* Computing 2nd power */
+ d__1 = l2_7.x[i__ - 1] - 5.;
+ sum += d__1 * d__1;
+ }
+ l6_1.fx = sum + 2090.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = 1. - l2_7.x[0] - l2_7.x[1];
+ }
+labelL5:
+ return 0;
+} /* tp105_ */
+
+
+/* Subroutine */ int tp106_(int *mode)
+{
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 8;
+ l1_1.nili = 3;
+ l1_1.ninl = 3;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_7.x[i__ - 1] = 5e3;
+ l13_7.xl[i__ - 1] = 1e3;
+/* L23: */
+ l14_7.xu[i__ - 1] = 1e4;
+ }
+ l13_7.xl[0] = 100.;
+ for (i__ = 4; i__ <= 8; ++i__) {
+ l13_7.xl[i__ - 1] = 10.;
+/* L24: */
+ l14_7.xu[i__ - 1] = 1e3;
+ }
+ l2_7.x[3] = 200.;
+ l2_7.x[4] = 350.;
+ l2_7.x[5] = 150.;
+ l2_7.x[6] = 225.;
+ l2_7.x[7] = 425.;
+ for (i__ = 1; i__ <= 8; ++i__) {
+ l11_7.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_7.lxu[i__ - 1] = true;
+ }
+ for (i__ = 1; i__ <= 6; ++i__) {
+ for (j = 1; j <= 8; ++j) {
+/* L22: */
+ l5_18.gg[i__ + j * 6 - 7] = 0.;
+ }
+ }
+ l4_7.gf[0] = 1.;
+ l4_7.gf[1] = 1.;
+ l4_7.gf[2] = 1.;
+ for (i__ = 4; i__ <= 8; ++i__) {
+/* labelL20: */
+ l4_7.gf[i__ - 1] = 0.;
+ }
+ l5_18.gg[18] = -.0025;
+ l5_18.gg[30] = -.0025;
+ l5_18.gg[25] = -.0025;
+ l5_18.gg[37] = -.0025;
+ l5_18.gg[19] = .0025;
+ l5_18.gg[26] = .01;
+ l5_18.gg[44] = -.01;
+ l5_18.gg[21] = -.0083333252000000017;
+ l5_18.gg[28] = -.012500000000000001;
+ l20_2.lex = false;
+ l20_2.xex[0] = 579.316670388;
+ l20_2.xex[1] = 1359.94292292;
+ l20_2.xex[2] = 5110.07132983;
+ l20_2.xex[3] = 182.017426603;
+ l20_2.xex[4] = 295.598526195;
+ l20_2.xex[5] = 217.979874037;
+ l20_2.xex[6] = 286.41620107;
+ l20_2.xex[7] = 395.597851351;
+ l20_2.fex = 7049.33092308;
+ return 0;
+labelL2:
+ l6_1.fx = l2_7.x[0] + l2_7.x[1] + l2_7.x[2];
+labelL3:
+ return 0;
+labelL4:
+ if (l9_6.index1[0]) {
+ l3_5.g[0] = (l2_7.x[3] + l2_7.x[5]) * -.0025 + 1.;
+ }
+ if (l9_6.index1[1]) {
+ l3_5.g[1] = (l2_7.x[4] + l2_7.x[6] - l2_7.x[3]) * -.0025 + 1.;
+ }
+ if (l9_6.index1[2]) {
+ l3_5.g[2] = (l2_7.x[7] - l2_7.x[4]) * -.01 + 1.;
+ }
+ if (l9_6.index1[3]) {
+ l3_5.g[3] = (l2_7.x[3] * -833.33252 - l2_7.x[0] * 100. + 83333.333 +
+ l2_7.x[0] * l2_7.x[5]) * 1e-5;
+ }
+ if (l9_6.index1[4]) {
+ l3_5.g[4] = (l2_7.x[4] * -1250. - l2_7.x[1] * l2_7.x[3] + l2_7.x[3] *
+ 1250. + l2_7.x[1] * l2_7.x[6]) * 1e-5;
+ }
+ if (l9_6.index1[5]) {
+ l3_5.g[5] = (-1.25e6 - l2_7.x[2] * l2_7.x[4] + l2_7.x[4] * 2500. +
+ l2_7.x[2] * l2_7.x[7]) * 1e-5;
+ }
+ return 0;
+labelL5:
+ if (! l10_6.index2[3]) {
+ goto labelL10;
+ }
+ l5_18.gg[3] = (l2_7.x[5] - 100.) * 1e-5;
+ l5_18.gg[33] = l2_7.x[0] * 1e-5;
+labelL10:
+ if (! l10_6.index2[4]) {
+ goto labelL11;
+ }
+ l5_18.gg[10] = (-l2_7.x[3] + l2_7.x[6]) * 1e-5;
+ l5_18.gg[22] = (-l2_7.x[1] + 1250.) * 1e-5;
+ l5_18.gg[40] = l2_7.x[1] * 1e-5;
+labelL11:
+ if (! l10_6.index2[5]) {
+ goto labelL12;
+ }
+ l5_18.gg[17] = (-l2_7.x[4] + l2_7.x[7]) * 1e-5;
+ l5_18.gg[29] = (-l2_7.x[2] + 2500.) * 1e-5;
+ l5_18.gg[47] = l2_7.x[2] * 1e-5;
+labelL12:
+ return 0;
+} /* tp106_ */
+
+
+/* Subroutine */ int tp107_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static Real c__, d__;
+ static int i__, j;
+ static Real v1, v2, v3, v4, v5, v6, v7, v8, v9, y1, y2, y3, y4, y5,
+ y6, v10, v11, v12, v13, v14, v15;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 9;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 6;
+ l2_8.x[0] = .8;
+ l2_8.x[1] = .8;
+ l2_8.x[2] = .2;
+ l2_8.x[3] = .2;
+ l2_8.x[4] = 1.0454;
+ l2_8.x[5] = 1.0454;
+ l2_8.x[6] = 1.0454;
+ l2_8.x[7] = 0.;
+ l2_8.x[8] = 0.;
+ v1 = .96460459183673464;
+ c__ = v1 * std::sin(.25);
+ d__ = v1 *std::cos(.25);
+ for (i__ = 1; i__ <= 6; ++i__) {
+ for (j = 1; j <= 9; ++j) {
+/* labelL20: */
+ l5_19.gg[i__ + j * 6 - 7] = 0.;
+ }
+ }
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_8.lxl[i__ - 1] = true;
+ l12_8.lxu[i__ - 1] = false;
+/* L21: */
+ l13_8.xl[i__ - 1] = 0.;
+ }
+ for (i__ = 3; i__ <= 4; ++i__) {
+ l11_8.lxl[i__ - 1] = false;
+/* L22: */
+ l12_8.lxu[i__ - 1] = false;
+ }
+ for (i__ = 5; i__ <= 7; ++i__) {
+ l11_8.lxl[i__ - 1] = true;
+ l12_8.lxu[i__ - 1] = true;
+ l13_8.xl[i__ - 1] = .90909;
+/* L23: */
+ l14_8.xu[i__ - 1] = 1.0909;
+ }
+ for (i__ = 8; i__ <= 9; ++i__) {
+ l11_8.lxl[i__ - 1] = false;
+/* L24: */
+ l12_8.lxu[i__ - 1] = false;
+ }
+ for (i__ = 3; i__ <= 9; ++i__) {
+/* L25: */
+ l4_8.gf[i__ - 1] = 0.;
+ }
+ l5_19.gg[0] = -1.;
+ l5_19.gg[7] = -1.;
+ l5_19.gg[15] = -1.;
+ l5_19.gg[22] = -1.;
+ l20_9.lex = false;
+ l20_9.xex[0] = .667009506909;
+ l20_9.xex[1] = 1.02238816675;
+ l20_9.xex[2] = .228287932605;
+ l20_9.xex[3] = .184821729352;
+ l20_9.xex[4] = 1.09090000001;
+ l20_9.xex[5] = 1.09090000001;
+ l20_9.xex[6] = 1.06903593236;
+ l20_9.xex[7] = .106612642267;
+ l20_9.xex[8] = -.338786658776;
+ l20_9.fex = 5055.01180339;
+ return 0;
+labelL2:
+/* Computing 3rd power */
+ d__1 = l2_8.x[0];
+/* Computing 3rd power */
+ d__2 = l2_8.x[1];
+ l6_1.fx = l2_8.x[0] * 3e3 + d__1 * (d__1 * d__1) * 1e3 + l2_8.x[1] * 2e3
+ + d__2 * (d__2 * d__2) * 666.667;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_8.x[0];
+ l4_8.gf[0] = d__1 * d__1 * 3e3 + 3e3;
+/* Computing 2nd power */
+ d__1 = l2_8.x[1];
+ l4_8.gf[1] = d__1 * d__1 * 2000.001 + 2e3;
+ return 0;
+labelL4:
+ y1 = std::sin(l2_8.x[7]);
+ y2 =std::cos(l2_8.x[7]);
+ y3 = std::sin(l2_8.x[8]);
+ y4 =std::cos(l2_8.x[8]);
+ y5 = std::sin(l2_8.x[7] - l2_8.x[8]);
+ y6 =std::cos(l2_8.x[7] - l2_8.x[8]);
+ if (*mode == 5) {
+ goto labelL5;
+ }
+ if (l9_6.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[4];
+ l3_5.g[0] = .4 - l2_8.x[0] + c__ * 2. * (d__1 * d__1) + l2_8.x[4] *
+ l2_8.x[5] * (-d__ * y1 - c__ * y2) + l2_8.x[4] * l2_8.x[6] * (
+ -d__ * y3 - c__ * y4);
+ }
+ if (l9_6.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[5];
+ l3_5.g[1] = .4 - l2_8.x[1] + c__ * 2. * (d__1 * d__1) + l2_8.x[4] *
+ l2_8.x[5] * (d__ * y1 - c__ * y2) + l2_8.x[5] * l2_8.x[6] * (
+ d__ * y5 - c__ * y6);
+ }
+ if (l9_6.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[6];
+ l3_5.g[2] = c__ * 2. * (d__1 * d__1) + .8 + l2_8.x[4] * l2_8.x[6] * (
+ d__ * y3 - c__ * y4) + l2_8.x[5] * l2_8.x[6] * (-d__ * y5 -
+ c__ * y6);
+ }
+ if (l9_6.index1[3]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[4];
+ l3_5.g[3] = .2 - l2_8.x[2] + d__ * 2. * (d__1 * d__1) - l2_8.x[4] *
+ l2_8.x[5] * (-c__ * y1 + d__ * y2) - l2_8.x[4] * l2_8.x[6] * (
+ -c__ * y3 + d__ * y4);
+ }
+ if (l9_6.index1[4]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[5];
+ l3_5.g[4] = .2 - l2_8.x[3] + d__ * 2. * (d__1 * d__1) - l2_8.x[4] *
+ l2_8.x[5] * (c__ * y1 + d__ * y2) - l2_8.x[5] * l2_8.x[6] * (
+ c__ * y5 + d__ * y6);
+ }
+ if (l9_6.index1[5]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[6];
+ l3_5.g[5] = d__ * 2. * (d__1 * d__1) - .337 - l2_8.x[4] * l2_8.x[6] *
+ (c__ * y3 + d__ * y4) - l2_8.x[5] * l2_8.x[6] * (-c__ * y5 +
+ d__ * y6);
+ }
+ return 0;
+labelL5:
+ if (! l10_6.index2[0]) {
+ goto L7;
+ }
+ v1 = -d__ * y1 - c__ * y2;
+ v2 = -d__ * y3 - c__ * y4;
+ l5_19.gg[24] = c__ * 4. * l2_8.x[4] + l2_8.x[5] * v1 + l2_8.x[6] * v2;
+ l5_19.gg[30] = l2_8.x[4] * v1;
+ l5_19.gg[36] = l2_8.x[4] * v2;
+ l5_19.gg[42] = l2_8.x[4] * l2_8.x[5] * (-d__ * y2 + c__ * y1);
+ l5_19.gg[48] = l2_8.x[4] * l2_8.x[6] * (-d__ * y4 + c__ * y3);
+L7:
+ if (! l10_6.index2[1]) {
+ goto L8;
+ }
+ v2 = d__ * y1 - c__ * y2;
+ v3 = d__ * y6 + c__ * y5;
+ v4 = d__ * y5 - c__ * y6;
+ l5_19.gg[25] = l2_8.x[5] * v2;
+ l5_19.gg[31] = c__ * 4. * l2_8.x[5] + l2_8.x[4] * v2 + l2_8.x[6] * v4;
+ l5_19.gg[37] = l2_8.x[5] * v4;
+ l5_19.gg[43] = l2_8.x[4] * l2_8.x[5] * (d__ * y2 + c__ * y1) + l2_8.x[5] *
+ l2_8.x[6] * v3;
+ l5_19.gg[49] = -l2_8.x[5] * l2_8.x[6] * v3;
+L8:
+ if (! l10_6.index2[2]) {
+ goto labelL9;
+ }
+ v5 = d__ * y3 - c__ * y4;
+ v6 = -d__ * y5 - c__ * y6;
+ v7 = -d__ * y6 + c__ * y5;
+ l5_19.gg[26] = l2_8.x[6] * v5;
+ l5_19.gg[32] = l2_8.x[6] * v6;
+ l5_19.gg[38] = c__ * 4. * l2_8.x[6] + l2_8.x[4] * v5 + l2_8.x[5] * v6;
+ l5_19.gg[44] = l2_8.x[5] * l2_8.x[6] * v7;
+ l5_19.gg[50] = l2_8.x[4] * l2_8.x[6] * (d__ * y4 + c__ * y3) - l2_8.x[5] *
+ l2_8.x[6] * v7;
+labelL9:
+ if (! l10_6.index2[3]) {
+ goto labelL10;
+ }
+ v8 = -c__ * y1 + d__ * y2;
+ v9 = -c__ * y3 + d__ * y4;
+ l5_19.gg[27] = d__ * 4. * l2_8.x[4] - l2_8.x[5] * v8 - l2_8.x[6] * v9;
+ l5_19.gg[33] = -l2_8.x[4] * v8;
+ l5_19.gg[39] = -l2_8.x[4] * v9;
+ l5_19.gg[45] = l2_8.x[4] * l2_8.x[5] * (c__ * y2 + d__ * y1);
+ l5_19.gg[51] = l2_8.x[4] * l2_8.x[6] * (c__ * y4 + d__ * y3);
+labelL10:
+ if (! l10_6.index2[4]) {
+ goto labelL11;
+ }
+ v10 = c__ * y1 + d__ * y2;
+ v11 = c__ * y5 + d__ * y6;
+ v12 = (c__ * y6 - d__ * y5) * l2_8.x[5];
+ l5_19.gg[28] = -l2_8.x[5] * v10;
+ l5_19.gg[34] = d__ * 4. * l2_8.x[5] - l2_8.x[4] * v10 - l2_8.x[6] * v11;
+ l5_19.gg[40] = -l2_8.x[5] * v11;
+ l5_19.gg[46] = -l2_8.x[4] * l2_8.x[5] * (c__ * y2 - d__ * y1) - l2_8.x[6]
+ * v12;
+ l5_19.gg[52] = l2_8.x[6] * v12;
+labelL11:
+ if (! l10_6.index2[5]) {
+ goto labelL12;
+ }
+ v13 = c__ * y3 + d__ * y4;
+ v14 = -c__ * y5 + d__ * y6;
+ v15 = (c__ * y6 + d__ * y5) * l2_8.x[5] * l2_8.x[6];
+ l5_19.gg[29] = -l2_8.x[6] * v13;
+ l5_19.gg[35] = -l2_8.x[6] * v14;
+ l5_19.gg[41] = d__ * 4. * l2_8.x[6] - l2_8.x[4] * v13 - l2_8.x[5] * v14;
+ l5_19.gg[47] = v15;
+ l5_19.gg[53] = -l2_8.x[4] * l2_8.x[6] * (c__ * y4 - d__ * y3) - v15;
+labelL12:
+ return 0;
+} /* tp107_ */
+
+
+/* Subroutine */ int tp108_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 9;
+ l1_1.nili = 0;
+ l1_1.ninl = 13;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (j = 1; j <= 9; ++j) {
+/* L21: */
+ l2_8.x[j - 1] = 1.;
+ }
+ for (i__ = 1; i__ <= 9; ++i__) {
+/* XL(I)=0.0D0 */
+ l11_8.lxl[i__ - 1] = false;
+/* XU(I)=1.0D0 */
+/* L22: */
+ l12_8.lxu[i__ - 1] = false;
+ }
+ l11_8.lxl[8] = true;
+ l13_8.xl[8] = 0.;
+ l20_9.lex = false;
+ l20_9.xex[0] = .884129216724;
+ l20_9.xex[1] = .467242472598;
+ l20_9.xex[2] = .0374207573677;
+ l20_9.xex[3] = .99929959821;
+ l20_9.xex[4] = .884129216724;
+ l20_9.xex[5] = .467242472594;
+ l20_9.xex[6] = .0374207573618;
+ l20_9.xex[7] = .99929959821;
+ l20_9.xex[8] = 2.61984643608e-20;
+ l20_9.fex = -.866025403841;
+ return 0;
+labelL2:
+ l6_1.fx = (l2_8.x[0] * l2_8.x[3] - l2_8.x[1] * l2_8.x[2] + l2_8.x[2] *
+ l2_8.x[8] - l2_8.x[4] * l2_8.x[8] + l2_8.x[4] * l2_8.x[7] -
+ l2_8.x[5] * l2_8.x[6]) * -.5;
+ return 0;
+labelL3:
+ l4_8.gf[0] = l2_8.x[3] * -.5;
+ l4_8.gf[1] = l2_8.x[2] * .5;
+ l4_8.gf[2] = (l2_8.x[1] - l2_8.x[8]) * .5;
+ l4_8.gf[3] = l2_8.x[0] * -.5;
+ l4_8.gf[4] = (l2_8.x[8] - l2_8.x[7]) * .5;
+ l4_8.gf[5] = l2_8.x[6] * .5;
+ l4_8.gf[6] = l2_8.x[5] * .5;
+ l4_8.gf[7] = l2_8.x[4] * -.5;
+ l4_8.gf[8] = -l4_8.gf[7] - l4_8.gf[1];
+ return 0;
+labelL4:
+ if (l9_11.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[2];
+/* Computing 2nd power */
+ d__2 = l2_8.x[3];
+ l3_10.g[0] = 1. - d__1 * d__1 - d__2 * d__2;
+ }
+ if (l9_11.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[8];
+ l3_10.g[1] = 1. - d__1 * d__1;
+ }
+ if (l9_11.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[4];
+/* Computing 2nd power */
+ d__2 = l2_8.x[5];
+ l3_10.g[2] = 1. - d__1 * d__1 - d__2 * d__2;
+ }
+ if (l9_11.index1[3]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[0];
+/* Computing 2nd power */
+ d__2 = l2_8.x[1] - l2_8.x[8];
+ l3_10.g[3] = 1. - d__1 * d__1 - d__2 * d__2;
+ }
+ if (l9_11.index1[4]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[0] - l2_8.x[4];
+/* Computing 2nd power */
+ d__2 = l2_8.x[1] - l2_8.x[5];
+ l3_10.g[4] = 1. - d__1 * d__1 - d__2 * d__2;
+ }
+ if (l9_11.index1[5]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[0] - l2_8.x[6];
+/* Computing 2nd power */
+ d__2 = l2_8.x[1] - l2_8.x[7];
+ l3_10.g[5] = 1. - d__1 * d__1 - d__2 * d__2;
+ }
+ if (l9_11.index1[6]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[2] - l2_8.x[4];
+/* Computing 2nd power */
+ d__2 = l2_8.x[3] - l2_8.x[5];
+ l3_10.g[6] = 1. - d__1 * d__1 - d__2 * d__2;
+ }
+ if (l9_11.index1[7]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[2] - l2_8.x[6];
+/* Computing 2nd power */
+ d__2 = l2_8.x[3] - l2_8.x[7];
+ l3_10.g[7] = 1. - d__1 * d__1 - d__2 * d__2;
+ }
+ if (l9_11.index1[8]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[6];
+/* Computing 2nd power */
+ d__2 = l2_8.x[7] - l2_8.x[8];
+ l3_10.g[8] = 1. - d__1 * d__1 - d__2 * d__2;
+ }
+ if (l9_11.index1[9]) {
+ l3_10.g[9] = l2_8.x[0] * l2_8.x[3] - l2_8.x[1] * l2_8.x[2];
+ }
+ if (l9_11.index1[10]) {
+ l3_10.g[10] = l2_8.x[2] * l2_8.x[8];
+ }
+ if (l9_11.index1[11]) {
+ l3_10.g[11] = -l2_8.x[4] * l2_8.x[8];
+ }
+ if (l9_11.index1[12]) {
+ l3_10.g[12] = l2_8.x[4] * l2_8.x[7] - l2_8.x[5] * l2_8.x[6];
+ }
+ return 0;
+labelL5:
+ for (i__ = 1; i__ <= 13; ++i__) {
+ for (j = 1; j <= 9; ++j) {
+/* L23: */
+ l5_20.gg[i__ + j * 13 - 14] = 0.;
+ }
+ }
+ if (! l10_11.index2[0]) {
+ goto L7;
+ }
+ l5_20.gg[26] = l2_8.x[2] * -2.;
+ l5_20.gg[39] = l2_8.x[3] * -2.;
+L7:
+ if (! l10_11.index2[1]) {
+ goto L8;
+ }
+ l5_20.gg[105] = l2_8.x[8] * -2.;
+L8:
+ if (! l10_11.index2[2]) {
+ goto labelL9;
+ }
+ l5_20.gg[54] = l2_8.x[4] * -2.;
+ l5_20.gg[67] = l2_8.x[5] * -2.;
+labelL9:
+ if (! l10_11.index2[3]) {
+ goto labelL10;
+ }
+ l5_20.gg[3] = l2_8.x[0] * -2.;
+ l5_20.gg[16] = (l2_8.x[1] - l2_8.x[8]) * -2.;
+ l5_20.gg[107] = -l5_20.gg[16];
+labelL10:
+ if (! l10_11.index2[4]) {
+ goto labelL11;
+ }
+ l5_20.gg[4] = (l2_8.x[0] - l2_8.x[4]) * -2.;
+ l5_20.gg[17] = (l2_8.x[1] - l2_8.x[5]) * -2.;
+ l5_20.gg[56] = -l5_20.gg[4];
+ l5_20.gg[69] = -l5_20.gg[17];
+labelL11:
+ if (! l10_11.index2[5]) {
+ goto labelL12;
+ }
+ l5_20.gg[5] = (l2_8.x[0] - l2_8.x[6]) * -2.;
+ l5_20.gg[18] = (l2_8.x[1] - l2_8.x[7]) * -2.;
+ l5_20.gg[83] = -l5_20.gg[5];
+ l5_20.gg[96] = -l5_20.gg[18];
+labelL12:
+ if (! l10_11.index2[6]) {
+ goto labelL13;
+ }
+ l5_20.gg[32] = (l2_8.x[2] - l2_8.x[4]) * -2.;
+ l5_20.gg[45] = (l2_8.x[3] - l2_8.x[5]) * -2.;
+ l5_20.gg[58] = -l5_20.gg[32];
+ l5_20.gg[71] = -l5_20.gg[45];
+labelL13:
+ if (! l10_11.index2[7]) {
+ goto labelL14;
+ }
+ l5_20.gg[33] = (l2_8.x[2] - l2_8.x[6]) * -2.;
+ l5_20.gg[46] = (l2_8.x[3] - l2_8.x[7]) * -2.;
+ l5_20.gg[85] = -l5_20.gg[33];
+ l5_20.gg[98] = -l5_20.gg[46];
+labelL14:
+ if (! l10_11.index2[8]) {
+ goto L15;
+ }
+ l5_20.gg[86] = l2_8.x[6] * -2.;
+ l5_20.gg[99] = (l2_8.x[7] - l2_8.x[8]) * -2.;
+ l5_20.gg[112] = -l5_20.gg[99];
+L15:
+ if (! l10_11.index2[9]) {
+ goto L16;
+ }
+ l5_20.gg[9] = l2_8.x[3];
+ l5_20.gg[22] = -l2_8.x[2];
+ l5_20.gg[35] = -l2_8.x[1];
+ l5_20.gg[48] = l2_8.x[0];
+L16:
+ if (! l10_11.index2[10]) {
+ goto L17;
+ }
+ l5_20.gg[36] = l2_8.x[8];
+ l5_20.gg[114] = l2_8.x[2];
+L17:
+ if (! l10_11.index2[11]) {
+ goto L18;
+ }
+ l5_20.gg[63] = -l2_8.x[8];
+ l5_20.gg[115] = -l2_8.x[4];
+L18:
+ if (! l10_11.index2[12]) {
+ goto L19;
+ }
+ l5_20.gg[64] = l2_8.x[7];
+ l5_20.gg[77] = -l2_8.x[6];
+ l5_20.gg[90] = -l2_8.x[5];
+ l5_20.gg[103] = l2_8.x[4];
+L19:
+ return 0;
+} /* tp108_ */
+
+
+/* Subroutine */ int tp109_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static Real a, b, c__;
+ static int i__, j;
+ static Real v1, v2, v3, v4, v5, v6, v7, v8, v9, ra, v10, v11, v12,
+ v13, v14, v15, v16, v17, v18, v19, v20, hv1;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 9;
+ l1_1.nili = 2;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 6;
+ a = 50.176;
+ ra = 1. / a;
+ b = std::sin(.25);
+ c__ =std::cos(.25);
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l13_8.xl[i__ - 1] = 0.;
+ l11_8.lxl[i__ - 1] = true;
+ l12_8.lxu[i__ - 1] = false;
+ l13_8.xl[i__ + 1] = -.55;
+ l14_8.xu[i__ + 1] = .55;
+ l13_8.xl[i__ + 6] = -400.;
+ l14_8.xu[i__ + 6] = 800.;
+ l13_8.xl[i__ + 3] = 196.;
+ l14_8.xu[i__ + 3] = 252.;
+/* labelL20: */
+ }
+ l13_8.xl[6] = 196.;
+ l14_8.xu[6] = 252.;
+ for (i__ = 3; i__ <= 9; ++i__) {
+ l11_8.lxl[i__ - 1] = true;
+ l12_8.lxu[i__ - 1] = true;
+/* L21: */
+ }
+ for (j = 1; j <= 9; ++j) {
+ l2_8.x[j - 1] = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ l5_21.gg[i__ + j * 10 - 11] = 0.;
+/* L22: */
+ }
+ }
+ for (i__ = 3; i__ <= 9; ++i__) {
+/* L30: */
+ l4_8.gf[i__ - 1] = 0.;
+ }
+ l5_21.gg[20] = -1.;
+ l5_21.gg[30] = 1.;
+ l5_21.gg[21] = 1.;
+ l5_21.gg[31] = -1.;
+ l5_21.gg[4] = -1.;
+ l5_21.gg[15] = -1.;
+ l5_21.gg[77] = 1.;
+ l5_21.gg[88] = 1.;
+ l20_9.lex = false;
+ l20_9.xex[0] = 674.888100445;
+ l20_9.xex[1] = 1134.1703947;
+ l20_9.xex[2] = .133569060261;
+ l20_9.xex[3] = -.371152592466;
+ l20_9.xex[4] = 252.;
+ l20_9.xex[5] = 252.;
+ l20_9.xex[6] = 201.464535316;
+ l20_9.xex[7] = 426.660777226;
+ l20_9.xex[8] = 368.494083867;
+ l20_9.fex = 5362.06927538;
+ return 0;
+labelL2:
+/* Computing 3rd power */
+ d__1 = l2_8.x[0];
+/* Computing 3rd power */
+ d__2 = l2_8.x[1];
+ l6_1.fx = l2_8.x[0] * 3. + d__1 * (d__1 * d__1) * 1e-6 + d__2 * (d__2 *
+ d__2) * 5.22074e-7 + l2_8.x[1] * 2.;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_8.x[0];
+ l4_8.gf[0] = d__1 * d__1 * 3e-6 + 3.;
+/* Computing 2nd power */
+ d__1 = l2_8.x[1];
+ l4_8.gf[1] = d__1 * d__1 * 1.566222e-6 + 2.;
+ return 0;
+labelL4:
+ if (l9_10.index1[0]) {
+ l3_9.g[0] = l2_8.x[3] - l2_8.x[2] + .55;
+ }
+ if (l9_10.index1[1]) {
+ l3_9.g[1] = l2_8.x[2] - l2_8.x[3] + .55;
+ }
+ if (l9_10.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[0];
+/* Computing 2nd power */
+ d__2 = l2_8.x[7];
+ l3_9.g[2] = 2.25e6 - d__1 * d__1 - d__2 * d__2;
+ }
+ if (l9_10.index1[3]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[1];
+/* Computing 2nd power */
+ d__2 = l2_8.x[8];
+ l3_9.g[3] = 2.25e6 - d__1 * d__1 - d__2 * d__2;
+ }
+ if (l9_10.index1[4]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[4];
+ l3_9.g[4] = (l2_8.x[4] * l2_8.x[5] * std::sin(-l2_8.x[2] - .25) + l2_8.x[4]
+ * l2_8.x[6] * std::sin(-l2_8.x[3] - .25) + d__1 * d__1 * 2. * b) *
+ ra + 400. - l2_8.x[0];
+ }
+ if (l9_10.index1[5]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[5];
+ l3_9.g[5] = (l2_8.x[4] * l2_8.x[5] * std::sin(l2_8.x[2] - .25) + l2_8.x[5]
+ * l2_8.x[6] * std::sin(l2_8.x[2] - l2_8.x[3] - .25) + d__1 * d__1 *
+ 2. * b) * ra + 400. - l2_8.x[1];
+ }
+ if (l9_10.index1[6]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[6];
+ l3_9.g[6] = (l2_8.x[4] * l2_8.x[6] * std::sin(l2_8.x[3] - .25) + l2_8.x[5]
+ * l2_8.x[6] * std::sin(l2_8.x[3] - l2_8.x[2] - .25) + d__1 * d__1 *
+ 2. * b) * ra + 881.779;
+ }
+ if (l9_10.index1[7]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[4];
+/* Computing 2nd power */
+ d__2 = l2_8.x[4];
+ l3_9.g[7] = l2_8.x[7] + (l2_8.x[4] * l2_8.x[5] *std::cos(-l2_8.x[2] - .25)
+ + l2_8.x[4] * l2_8.x[6] *std::cos(-l2_8.x[3] - .25) - d__1 *
+ d__1 * 2. * c__) * ra + d__2 * d__2 * 7.533e-4 - 200.;
+ }
+ if (l9_10.index1[8]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[5];
+/* Computing 2nd power */
+ d__2 = l2_8.x[5];
+ l3_9.g[8] = l2_8.x[8] + (l2_8.x[4] * l2_8.x[5] *std::cos(l2_8.x[2] - .25)
+ + l2_8.x[6] * l2_8.x[5] *std::cos(l2_8.x[2] - l2_8.x[3] - .25) -
+ d__1 * d__1 * 2. * c__) * ra + d__2 * d__2 * 7.533e-4 - 200.;
+ }
+ if (l9_10.index1[9]) {
+/* Computing 2nd power */
+ d__1 = l2_8.x[6];
+/* Computing 2nd power */
+ d__2 = l2_8.x[6];
+ l3_9.g[9] = (l2_8.x[4] * l2_8.x[6] *std::cos(l2_8.x[3] - .25) + l2_8.x[5]
+ * l2_8.x[6] *std::cos(l2_8.x[3] - l2_8.x[2] - .25) - d__1 * d__1 *
+ 2. * c__) * ra + d__2 * d__2 * 7.533e-4 - 22.938;
+ }
+ return 0;
+labelL5:
+/* L8: */
+ if (! l10_10.index2[2]) {
+ goto labelL9;
+ }
+ l5_21.gg[2] = l2_8.x[0] * -2.;
+ l5_21.gg[72] = l2_8.x[7] * -2.;
+labelL9:
+ if (! l10_10.index2[3]) {
+ goto labelL10;
+ }
+ l5_21.gg[13] = l2_8.x[1] * -2.;
+ l5_21.gg[83] = l2_8.x[8] * -2.;
+labelL10:
+ if (! l10_10.index2[4]) {
+ goto labelL11;
+ }
+ v1 = std::sin(-l2_8.x[2] - .25);
+ v2 = std::sin(-l2_8.x[3] - .25);
+ v3 = l2_8.x[4] * ra;
+ l5_21.gg[24] = -l2_8.x[5] * v3 *std::cos(-l2_8.x[2] - .25);
+ l5_21.gg[34] = -l2_8.x[6] * v3 *std::cos(-l2_8.x[3] - .25);
+ l5_21.gg[44] = (l2_8.x[5] * v1 + l2_8.x[6] * v2 + l2_8.x[4] * 4. * b) *
+ ra;
+ l5_21.gg[54] = v3 * v1;
+ l5_21.gg[64] = v3 * v2;
+labelL11:
+ if (! l10_10.index2[5]) {
+ goto labelL12;
+ }
+ hv1 = l2_8.x[2] - l2_8.x[3] - .25;
+ v3 =std::cos(hv1);
+ v4 = std::sin(l2_8.x[2] - .25);
+ v5 = l2_8.x[5] * ra;
+ v6 = std::sin(hv1);
+ l5_21.gg[25] = l2_8.x[4] * v5 *std::cos(l2_8.x[2] - .25) + l2_8.x[6] * v5 *
+ v3;
+ l5_21.gg[35] = -l2_8.x[6] * v5 * v3;
+ l5_21.gg[45] = v5 * v4;
+ l5_21.gg[55] = (l2_8.x[4] * v4 + l2_8.x[6] * v6) * ra + v5 * 4. * b;
+ l5_21.gg[65] = v5 * v6;
+labelL12:
+ if (! l10_10.index2[6]) {
+ goto labelL13;
+ }
+ hv1 = l2_8.x[3] - l2_8.x[2] - .25;
+ v7 = l2_8.x[6] * ra;
+ v8 =std::cos(hv1);
+ v9 = std::sin(l2_8.x[3] - .25);
+ v10 = std::sin(hv1);
+ l5_21.gg[26] = -l2_8.x[5] * v7 * v8;
+ l5_21.gg[36] = l2_8.x[4] * v7 *std::cos(l2_8.x[3] - .25) + l2_8.x[5] * v7 *
+ v8;
+ l5_21.gg[46] = v7 * v9;
+ l5_21.gg[56] = v7 * v10;
+ l5_21.gg[66] = (l2_8.x[4] * v9 + l2_8.x[5] * v10) * ra + v7 * 4. * b;
+labelL13:
+ if (! l10_10.index2[7]) {
+ goto labelL14;
+ }
+ v11 = l2_8.x[4] * ra;
+ v12 =std::cos(-l2_8.x[2] - .25) * ra;
+ v13 =std::cos(-l2_8.x[3] - .25) * ra;
+ l5_21.gg[27] = l2_8.x[5] * v11 * std::sin(-l2_8.x[2] - .25);
+ l5_21.gg[37] = l2_8.x[6] * v11 * std::sin(-l2_8.x[3] - .25);
+ l5_21.gg[47] = l2_8.x[5] * v12 + l2_8.x[6] * v13 - v11 * 4. * c__ +
+ l2_8.x[4] * .0015066;
+ l5_21.gg[57] = l2_8.x[4] * v12;
+ l5_21.gg[67] = l2_8.x[4] * v13;
+labelL14:
+ if (! l10_10.index2[8]) {
+ goto L15;
+ }
+ hv1 = l2_8.x[2] - l2_8.x[3] - .25;
+ v14 = std::sin(hv1) * l2_8.x[5] * ra;
+ v15 =std::cos(l2_8.x[2] - .25) * ra;
+ v16 =std::cos(hv1) * ra;
+ l5_21.gg[28] = -l2_8.x[4] * l2_8.x[5] * std::sin(l2_8.x[2] - .25) * ra -
+ l2_8.x[6] * v14;
+ l5_21.gg[38] = l2_8.x[6] * v14;
+ l5_21.gg[48] = l2_8.x[5] * v15;
+ l5_21.gg[58] = l2_8.x[4] * v15 + l2_8.x[6] * v16 - l2_8.x[5] * 4. * c__ *
+ ra + l2_8.x[5] * .0015066;
+ l5_21.gg[68] = l2_8.x[5] * v16;
+L15:
+ if (! l10_10.index2[9]) {
+ goto L16;
+ }
+ hv1 = l2_8.x[3] - l2_8.x[2] - .25;
+ v17 = std::sin(hv1) * l2_8.x[5] * ra;
+ v18 =std::cos(l2_8.x[3] - .25) * ra;
+ v19 =std::cos(hv1) * ra;
+ v20 = l2_8.x[6] * ra;
+ l5_21.gg[29] = l2_8.x[6] * v17;
+ l5_21.gg[39] = -l2_8.x[4] * v20 * std::sin(l2_8.x[3] - .25) - l2_8.x[6] * v17;
+ l5_21.gg[49] = l2_8.x[6] * v18;
+ l5_21.gg[59] = l2_8.x[6] * v19;
+ l5_21.gg[69] = l2_8.x[4] * v18 + l2_8.x[5] * v19 - v20 * 4. * c__ +
+ l2_8.x[6] * .0015066;
+L16:
+ return 0;
+} /* tp109_ */
+
+
+/* Subroutine */ int tp110_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ Real d_sign(Real*, Real*);
+ static int i__;
+ static Real s, t, u, sum;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 10;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ l11_9.lxl[i__ - 1] = true;
+ l12_9.lxu[i__ - 1] = true;
+ l13_9.xl[i__ - 1] = 2.001;
+ l14_9.xu[i__ - 1] = 9.999;
+ l2_9.x[i__ - 1] = 9.;
+/* labelL20: */
+ }
+ l20_10.lex = false;
+ l20_10.xex[0] = 9.35025654733;
+ l20_10.xex[1] = 9.35025654733;
+ l20_10.xex[2] = 9.35025654733;
+ l20_10.xex[3] = 9.35025654733;
+ l20_10.xex[4] = 9.35025654733;
+ l20_10.xex[5] = 9.35025654733;
+ l20_10.xex[6] = 9.35025654733;
+ l20_10.xex[7] = 9.35025654733;
+ l20_10.xex[8] = 9.35025654733;
+ l20_10.xex[9] = 9.35025654733;
+ l20_10.fex = -45.7784697153;
+ return 0;
+labelL2:
+ t = 1.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L30: */
+ t *= l2_9.x[i__ - 1];
+ }
+ d__1 = std::abs(t);
+ s = pow_dd(&d__1, &c_b1157);
+ s = d_sign(&s, &t);
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ u = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ if (l2_9.x[i__ - 1] - 2. <= 0. || 10. - l2_9.x[i__ - 1] <= 0.) {
+ goto L33;
+ }
+/* L31: */
+/* Computing 2nd power */
+ d__1 = std::log(l2_9.x[i__ - 1] - 2.);
+/* Computing 2nd power */
+ d__2 = std::log(10. - l2_9.x[i__ - 1]);
+ u = u + d__1 * d__1 + d__2 * d__2;
+ }
+ l6_1.fx = u - s;
+ return 0;
+L33:
+ sum = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L34: */
+/* Computing 2nd power */
+ d__1 = l2_9.x[i__ - 1] - 5.;
+ sum += d__1 * d__1;
+ }
+ l6_1.fx = sum + 1e3 - 45.8;
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 10; ++i__) {
+ if (l2_9.x[i__ - 1] - 2. <= 0. || 10. - l2_9.x[i__ - 1] <= 0.) {
+ goto L35;
+ }
+ l4_9.gf[i__ - 1] = (std::log(l2_9.x[i__ - 1] - 2.) / (l2_9.x[i__ - 1] - 2.)
+ - log(10. - l2_9.x[i__ - 1]) / (10. - l2_9.x[i__ - 1])) * 2.
+ - s / l2_9.x[i__ - 1] * .2;
+ goto L32;
+L35:
+ l4_9.gf[i__ - 1] = (l2_9.x[i__ - 1] - 5.) * 2.;
+L32:
+ ;
+ }
+labelL4:
+ return 0;
+} /* tp110_ */
+
+
+/* Subroutine */ int tp111_(int *mode)
+{
+ /* Local variables */
+ static Real c__[10];
+ static int i__, j;
+ static Real s, t;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 10;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 3;
+ for (j = 1; j <= 10; ++j) {
+ l2_9.x[j - 1] = -2.3;
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* labelL20: */
+ l5_14.gg[i__ + j * 3 - 4] = 0.;
+ }
+ }
+ c__[0] = -6.089;
+ c__[1] = -17.164;
+ c__[2] = -34.054;
+ c__[3] = -5.914;
+ c__[4] = -24.721;
+ c__[5] = -14.986;
+ c__[6] = -24.1;
+ c__[7] = -10.708;
+ c__[8] = -26.662;
+ c__[9] = -22.179;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ l13_9.xl[i__ - 1] = -100.;
+ l14_9.xu[i__ - 1] = 100.;
+ l11_9.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_9.lxu[i__ - 1] = true;
+ }
+ l20_10.lex = false;
+ l20_10.xex[0] = -3.20121253241;
+ l20_10.xex[1] = -1.91205959435;
+ l20_10.xex[2] = -.244441308369;
+ l20_10.xex[3] = -6.53748856532;
+ l20_10.xex[4] = -.723152425984;
+ l20_10.xex[5] = -7.26773826993;
+ l20_10.xex[6] = -3.59671064233;
+ l20_10.xex[7] = -4.01776873216;
+ l20_10.xex[8] = -3.28746169619;
+ l20_10.xex[9] = -2.33558183059;
+ l20_10.fex = -47.7610902637;
+ return 0;
+labelL2:
+ t = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L30: */
+ t += std::exp(l2_9.x[i__ - 1]);
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ s = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L31: */
+ s += std::exp(l2_9.x[i__ - 1]) * (c__[i__ - 1] + l2_9.x[i__ - 1] - std::log(t));
+ }
+ l6_1.fx = s;
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L33: */
+ l4_9.gf[i__ - 1] = std::exp(l2_9.x[i__ - 1]) * (c__[i__ - 1] + l2_9.x[i__
+ - 1] - std::log(t));
+ }
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = std::exp(l2_9.x[0]) + std::exp(l2_9.x[1]) * 2. + std::exp(l2_9.x[2]) *
+ 2. + std::exp(l2_9.x[5]) + std::exp(l2_9.x[9]) - 2.;
+ }
+ if (l9_4.index1[1]) {
+ l3_3.g[1] = std::exp(l2_9.x[3]) + std::exp(l2_9.x[4]) * 2. + std::exp(l2_9.x[5]) +
+ std::exp(l2_9.x[6]) - 1.;
+ }
+ if (l9_4.index1[2]) {
+ l3_3.g[2] = std::exp(l2_9.x[2]) + std::exp(l2_9.x[6]) + std::exp(l2_9.x[7]) + std::exp(
+ l2_9.x[8]) * 2. + std::exp(l2_9.x[9]) - 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_4.index2[0]) {
+ goto L7;
+ }
+ l5_14.gg[0] = std::exp(l2_9.x[0]);
+ l5_14.gg[3] = std::exp(l2_9.x[1]) * 2.;
+ l5_14.gg[6] = std::exp(l2_9.x[2]) * 2.;
+ l5_14.gg[15] = std::exp(l2_9.x[5]);
+ l5_14.gg[27] = std::exp(l2_9.x[9]);
+L7:
+ if (! l10_4.index2[1]) {
+ goto L8;
+ }
+ l5_14.gg[10] = std::exp(l2_9.x[3]);
+ l5_14.gg[13] = std::exp(l2_9.x[4]) * 2.;
+ l5_14.gg[16] = std::exp(l2_9.x[5]);
+ l5_14.gg[19] = std::exp(l2_9.x[6]);
+L8:
+ if (! l10_4.index2[2]) {
+ goto labelL9;
+ }
+ l5_14.gg[8] = std::exp(l2_9.x[2]);
+ l5_14.gg[20] = std::exp(l2_9.x[6]);
+ l5_14.gg[23] = std::exp(l2_9.x[7]);
+ l5_14.gg[26] = std::exp(l2_9.x[8]) * 2.;
+ l5_14.gg[29] = std::exp(l2_9.x[9]);
+labelL9:
+ return 0;
+} /* tp111_ */
+
+
+/* Subroutine */ int tp112_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real c__[10];
+ static int i__, j;
+ static Real s, t, dlogt;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 10;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 3;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ l13_9.xl[i__ - 1] = 1e-4;
+ l11_9.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_9.lxu[i__ - 1] = false;
+ }
+ c__[0] = -6.089;
+ c__[1] = -17.164;
+ c__[2] = -34.054;
+ c__[3] = -5.914;
+ c__[4] = -24.721;
+ c__[5] = -14.986;
+ c__[6] = -24.1;
+ c__[7] = -10.708;
+ c__[8] = -26.662;
+ c__[9] = -22.179;
+ for (j = 1; j <= 10; ++j) {
+ l2_9.x[j - 1] = .1;
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* labelL20: */
+ l5_14.gg[i__ + j * 3 - 4] = 0.;
+ }
+ }
+ l5_14.gg[0] = 1.;
+ l5_14.gg[3] = 2.;
+ l5_14.gg[6] = 2.;
+ l5_14.gg[15] = 1.;
+ l5_14.gg[27] = 1.;
+ l5_14.gg[10] = 1.;
+ l5_14.gg[13] = 2.;
+ l5_14.gg[16] = 1.;
+ l5_14.gg[19] = 1.;
+ l5_14.gg[8] = 1.;
+ l5_14.gg[20] = 1.;
+ l5_14.gg[23] = 1.;
+ l5_14.gg[26] = 2.;
+ l5_14.gg[29] = 1.;
+ l20_10.lex = false;
+ l20_10.xex[0] = .0177354776881;
+ l20_10.xex[1] = .0820018011109;
+ l20_10.xex[2] = .88256455892;
+ l20_10.xex[3] = 7.23325625629e-4;
+ l20_10.xex[4] = .490785079062;
+ l20_10.xex[5] = 4.33546900325e-4;
+ l20_10.xex[6] = .0172729773078;
+ l20_10.xex[7] = .00776563912291;
+ l20_10.xex[8] = .0198492864597;
+ l20_10.xex[9] = .0526982611793;
+ l20_10.fex = -.47761086;
+ return 0;
+labelL2:
+ t = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L30: */
+ t += l2_9.x[i__ - 1];
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ if (t < 1e-5) {
+ goto L34;
+ }
+ dlogt = std::log(t);
+ s = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ if (l2_9.x[i__ - 1] < 0.) {
+ goto L34;
+ }
+/* L31: */
+ s += l2_9.x[i__ - 1] * (c__[i__ - 1] + std::log(l2_9.x[i__ - 1]) - dlogt);
+ }
+ l6_1.fx = s * (float).01;
+ return 0;
+L34:
+ s = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L35: */
+ if (l2_9.x[i__ - 1] < 0.) {
+/* Computing 2nd power */
+ d__1 = l2_9.x[i__ - 1] - 5.;
+ s += d__1 * d__1;
+ }
+ }
+ l6_1.fx = (s + 1e3 - 47.8) * (float).01;
+ return 0;
+labelL3:
+ if (t < 1e-5) {
+ goto L36;
+ }
+ dlogt = std::log(t);
+ for (i__ = 1; i__ <= 10; ++i__) {
+ if (l2_9.x[i__ - 1] < 0.) {
+ goto L36;
+ }
+/* L33: */
+ l4_9.gf[i__ - 1] = (c__[i__ - 1] + std::log(l2_9.x[i__ - 1]) - dlogt) * (
+ float).01;
+ }
+ return 0;
+L36:
+ for (i__ = 1; i__ <= 10; ++i__) {
+ l4_9.gf[i__ - 1] = 0.;
+/* L37: */
+ if (l2_9.x[i__ - 1] < 0.) {
+ l4_9.gf[i__ - 1] = (l2_9.x[i__ - 1] - 5.) * 2. * (float).01;
+ }
+ }
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = l2_9.x[0] + l2_9.x[1] * 2. + l2_9.x[2] * 2. + l2_9.x[5] +
+ l2_9.x[9] - (float)2.;
+ }
+ if (l9_4.index1[1]) {
+ l3_3.g[1] = l2_9.x[3] + l2_9.x[4] * 2. + l2_9.x[5] + l2_9.x[6] - (
+ float)1.;
+ }
+ if (l9_4.index1[2]) {
+ l3_3.g[2] = l2_9.x[2] + l2_9.x[6] + l2_9.x[7] + l2_9.x[8] * 2. +
+ l2_9.x[9] - (float)1.;
+ }
+labelL5:
+ return 0;
+} /* tp112_ */
+
+
+/* Subroutine */ int tp113_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5, d__6, d__7, d__8, d__9, d__10;
+
+ /* Local variables */
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 10;
+ l1_1.nili = 3;
+ l1_1.ninl = 5;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 8; ++i__) {
+ for (j = 1; j <= 10; ++j) {
+/* labelL20: */
+ l5_22.gg[i__ + (j << 3) - 9] = 0.;
+ }
+ }
+ l2_9.x[0] = 2.;
+ l2_9.x[1] = 3.;
+ l2_9.x[2] = 5.;
+ l2_9.x[3] = 5.;
+ l2_9.x[4] = 1.;
+ l2_9.x[5] = 2.;
+ l2_9.x[6] = 7.;
+ l2_9.x[7] = 3.;
+ l2_9.x[8] = 6.;
+ l2_9.x[9] = 10.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ l11_9.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_9.lxu[i__ - 1] = false;
+ }
+ l5_22.gg[0] = -4.;
+ l5_22.gg[8] = -5.;
+ l5_22.gg[48] = 3.;
+ l5_22.gg[56] = -9.;
+ l5_22.gg[1] = -10.;
+ l5_22.gg[9] = 8.;
+ l5_22.gg[49] = 17.;
+ l5_22.gg[57] = -2.;
+ l5_22.gg[2] = 8.;
+ l5_22.gg[10] = -2.;
+ l5_22.gg[66] = -5.;
+ l5_22.gg[74] = 2.;
+ l5_22.gg[27] = 7.;
+ l5_22.gg[12] = -8.;
+ l5_22.gg[28] = 2.;
+ l5_22.gg[45] = 1.;
+ l5_22.gg[38] = -14.;
+ l5_22.gg[46] = 6.;
+ l5_22.gg[7] = 3.;
+ l5_22.gg[15] = -6.;
+ l5_22.gg[79] = 7.;
+ l20_10.lex = false;
+ l20_10.xex[0] = 2.17199637118;
+ l20_10.xex[1] = 2.36368297378;
+ l20_10.xex[2] = 8.77392573847;
+ l20_10.xex[3] = 5.09598448797;
+ l20_10.xex[4] = .990654764992;
+ l20_10.xex[5] = 1.43057397893;
+ l20_10.xex[6] = 1.32164420805;
+ l20_10.xex[7] = 9.82872580801;
+ l20_10.xex[8] = 8.28009167017;
+ l20_10.xex[9] = 8.37592666387;
+ l20_10.fex = 24.3062090641;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_9.x[0];
+/* Computing 2nd power */
+ d__2 = l2_9.x[1];
+/* Computing 2nd power */
+ d__3 = l2_9.x[2] - 10.;
+/* Computing 2nd power */
+ d__4 = l2_9.x[3] - 5.;
+/* Computing 2nd power */
+ d__5 = l2_9.x[4] - 3.;
+/* Computing 2nd power */
+ d__6 = l2_9.x[5] - 1.;
+/* Computing 2nd power */
+ d__7 = l2_9.x[6];
+/* Computing 2nd power */
+ d__8 = l2_9.x[7] - 11.;
+/* Computing 2nd power */
+ d__9 = l2_9.x[8] - 10.;
+/* Computing 2nd power */
+ d__10 = l2_9.x[9] - 7.;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + l2_9.x[0] * l2_9.x[1] - l2_9.x[0] *
+ 14. - l2_9.x[1] * 16. + d__3 * d__3 + d__4 * d__4 * 4. + d__5 *
+ d__5 + d__6 * d__6 * 2. + d__7 * d__7 * 5. + d__8 * d__8 * 7. +
+ d__9 * d__9 * 2. + d__10 * d__10 + 45.;
+ return 0;
+labelL3:
+ l4_9.gf[0] = l2_9.x[0] * 2. + l2_9.x[1] - 14.;
+ l4_9.gf[1] = l2_9.x[1] * 2. + l2_9.x[0] - 16.;
+ l4_9.gf[2] = (l2_9.x[2] - 10.) * 2.;
+ l4_9.gf[3] = (l2_9.x[3] - 5.) * 8.;
+ l4_9.gf[4] = (l2_9.x[4] - 3.) * 2.;
+ l4_9.gf[5] = (l2_9.x[5] - 1.) * 4.;
+ l4_9.gf[6] = l2_9.x[6] * 10.;
+ l4_9.gf[7] = (l2_9.x[7] - 11.) * 14.;
+ l4_9.gf[8] = (l2_9.x[8] - 10.) * 4.;
+ l4_9.gf[9] = (l2_9.x[9] - 7.) * 2.;
+ return 0;
+labelL4:
+ if (l9_12.index1[0]) {
+ l3_11.g[0] = l2_9.x[0] * -4. - l2_9.x[1] * 5. + l2_9.x[6] * 3. -
+ l2_9.x[7] * 9. + 105.;
+ }
+ if (l9_12.index1[1]) {
+ l3_11.g[1] = l2_9.x[0] * -10. + l2_9.x[1] * 8. + l2_9.x[6] * 17. -
+ l2_9.x[7] * 2.;
+ }
+ if (l9_12.index1[2]) {
+ l3_11.g[2] = l2_9.x[0] * 8. - l2_9.x[1] * 2. - l2_9.x[8] * 5. +
+ l2_9.x[9] * 2. + 12.;
+ }
+ if (l9_12.index1[3]) {
+/* Computing 2nd power */
+ d__1 = l2_9.x[0] - 2.;
+/* Computing 2nd power */
+ d__2 = l2_9.x[1] - 3.;
+/* Computing 2nd power */
+ d__3 = l2_9.x[2];
+ l3_11.g[3] = d__1 * d__1 * -3. - d__2 * d__2 * 4. - d__3 * d__3 * 2.
+ + l2_9.x[3] * 7. + 120.;
+ }
+ if (l9_12.index1[4]) {
+/* Computing 2nd power */
+ d__1 = l2_9.x[0];
+/* Computing 2nd power */
+ d__2 = l2_9.x[2] - 6.;
+ l3_11.g[4] = d__1 * d__1 * -5. - l2_9.x[1] * 8. - d__2 * d__2 +
+ l2_9.x[3] * 2. + 40.;
+ }
+ if (l9_12.index1[5]) {
+/* Computing 2nd power */
+ d__1 = l2_9.x[0] - 8.;
+/* Computing 2nd power */
+ d__2 = l2_9.x[1] - 4.;
+/* Computing 2nd power */
+ d__3 = l2_9.x[4];
+ l3_11.g[5] = d__1 * d__1 * -.5 - d__2 * d__2 * 2. - d__3 * d__3 * 3.
+ + l2_9.x[5] + 30.;
+ }
+ if (l9_12.index1[6]) {
+/* Computing 2nd power */
+ d__1 = l2_9.x[0];
+/* Computing 2nd power */
+ d__2 = l2_9.x[1] - 2.;
+ l3_11.g[6] = -(d__1 * d__1) - d__2 * d__2 * 2. + l2_9.x[0] * 2. *
+ l2_9.x[1] - l2_9.x[4] * 14. + l2_9.x[5] * 6.;
+ }
+ if (l9_12.index1[7]) {
+/* Computing 2nd power */
+ d__1 = l2_9.x[8] - 8.;
+ l3_11.g[7] = l2_9.x[0] * 3. - l2_9.x[1] * 6. - d__1 * d__1 * 12. +
+ l2_9.x[9] * 7.;
+ }
+ return 0;
+labelL5:
+ if (! l10_12.index2[3]) {
+ goto labelL10;
+ }
+ l5_22.gg[3] = (l2_9.x[0] - 2.) * -6.;
+ l5_22.gg[11] = (l2_9.x[1] - 3.) * -8.;
+ l5_22.gg[19] = l2_9.x[2] * -4.;
+labelL10:
+ if (! l10_12.index2[4]) {
+ goto labelL11;
+ }
+ l5_22.gg[4] = l2_9.x[0] * -10.;
+ l5_22.gg[20] = (l2_9.x[2] - 6.) * -2.;
+labelL11:
+ if (! l10_12.index2[5]) {
+ goto labelL12;
+ }
+ l5_22.gg[5] = 8. - l2_9.x[0];
+ l5_22.gg[13] = (l2_9.x[1] - 4.) * -4.;
+ l5_22.gg[37] = l2_9.x[4] * -6.;
+labelL12:
+ if (! l10_12.index2[6]) {
+ goto labelL13;
+ }
+ l5_22.gg[6] = l2_9.x[0] * -2. + l2_9.x[1] * 2.;
+ l5_22.gg[14] = (l2_9.x[1] - 2.) * -4. + l2_9.x[0] * 2.;
+labelL13:
+ if (l10_12.index2[7]) {
+ l5_22.gg[71] = (l2_9.x[8] - 8.) * -24.;
+ }
+ return 0;
+} /* tp113_ */
+
+
+/* Subroutine */ int tp114_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__, j;
+ static Real v1, v2, v3;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 10;
+ l1_1.nili = 4;
+ l1_1.ninl = 4;
+ l1_1.neli = 1;
+ l1_1.nenl = 2;
+ l2_9.x[0] = 1745.;
+ l2_9.x[1] = 1.2e4;
+ l2_9.x[2] = 110.;
+ l2_9.x[3] = 3048.;
+ l2_9.x[4] = 1974.;
+ l2_9.x[5] = 89.2;
+ l2_9.x[6] = 92.8;
+ l2_9.x[7] = 8.;
+ l2_9.x[8] = 3.6;
+ l2_9.x[9] = 145.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L25: */
+ l13_9.xl[i__ - 1] = 1e-5;
+ }
+ l13_9.xl[5] = 85.;
+ l13_9.xl[6] = 90.;
+ l13_9.xl[7] = 3.;
+ l13_9.xl[8] = 1.2;
+ l13_9.xl[9] = 145.;
+ l14_9.xu[0] = 2e3;
+ l14_9.xu[1] = 1.6e4;
+ l14_9.xu[2] = 120.;
+ l14_9.xu[3] = 5e3;
+ l14_9.xu[4] = 2e3;
+ l14_9.xu[5] = 93.;
+ l14_9.xu[6] = 95.;
+ l14_9.xu[7] = 12.;
+ l14_9.xu[8] = 4.;
+ l14_9.xu[9] = 162.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ l11_9.lxl[i__ - 1] = true;
+ l12_9.lxu[i__ - 1] = true;
+ for (j = 1; j <= 11; ++j) {
+/* labelL6: */
+ l5_23.gg[j + i__ * 11 - 12] = 0.;
+ }
+ }
+ l4_9.gf[0] = 5.04e-4;
+ l4_9.gf[1] = 3.5000000000000004e-6;
+ l4_9.gf[2] = .001;
+ l4_9.gf[4] = 3.3599999999999998e-4;
+ l4_9.gf[5] = 0.;
+ l4_9.gf[7] = 0.;
+ l4_9.gf[8] = 0.;
+ l4_9.gf[9] = 0.;
+ l5_23.gg[88] = -.9;
+ l5_23.gg[99] = -.222;
+ l5_23.gg[67] = 3.;
+ l5_23.gg[100] = -.99;
+ l5_23.gg[90] = 1.1111111111111112;
+ l5_23.gg[101] = .222;
+ l5_23.gg[69] = -3.;
+ l5_23.gg[102] = 1.0101010101010102;
+ l5_23.gg[37] = -.99;
+ l5_23.gg[60] = .325;
+ l5_23.gg[71] = -.99;
+ l5_23.gg[39] = 1.0101010101010102;
+ l5_23.gg[62] = -.325;
+ l5_23.gg[73] = 1.0101010101010102;
+ l5_23.gg[8] = -1.;
+ l5_23.gg[41] = 1.22;
+ l5_23.gg[52] = -1.;
+ l5_23.gg[64] = -1.;
+ l5_23.gg[87] = -1.;
+ l20_10.lex = false;
+ l20_10.xex[0] = 1698.09564792;
+ l20_10.xex[1] = 15818.7256985;
+ l20_10.xex[2] = 54.1022827849;
+ l20_10.xex[3] = 3031.22594099;
+ l20_10.xex[4] = 2e3;
+ l20_10.xex[5] = 90.1153669236;
+ l20_10.xex[6] = 95.;
+ l20_10.xex[7] = 10.4933580864;
+ l20_10.xex[8] = 1.5616363638;
+ l20_10.xex[9] = 153.535353535;
+ l20_10.fex = -17.688069634399998;
+ return 0;
+labelL2:
+ l6_1.fx = l2_9.x[0] * 5.04 + l2_9.x[1] * .035 + l2_9.x[2] * 10. + l2_9.x[
+ 4] * 3.36 - l2_9.x[3] * .063 * l2_9.x[6];
+ l6_1.fx *= .01;
+labelL3:
+ l4_9.gf[3] = l2_9.x[6] * -.063 * .01;
+ l4_9.gf[6] = l2_9.x[3] * -.063 * .01;
+ return 0;
+labelL4:
+ if (l9_13.index1[0]) {
+ l3_12.g[0] = 35.82 - l2_9.x[9] * .222 - l2_9.x[8] * .9;
+ }
+ if (l9_13.index1[1]) {
+ l3_12.g[1] = l2_9.x[6] * 3. - 133. - l2_9.x[9] * .99;
+ }
+ if (l9_13.index1[2]) {
+ l3_12.g[2] = l2_9.x[9] * .222 - 35.82 + l2_9.x[8] *
+ 1.1111111111111112;
+ }
+ if (l9_13.index1[3]) {
+ l3_12.g[3] = 133. - l2_9.x[6] * 3. + l2_9.x[9] / .99;
+ }
+ if (l9_13.index1[4]) {
+/* Computing 2nd power */
+ d__1 = l2_9.x[7];
+ l3_12.g[4] = l2_9.x[0] * 1.12 + l2_9.x[0] * .13167 * l2_9.x[7] -
+ l2_9.x[0] * .00667 * (d__1 * d__1) - l2_9.x[3] * .99;
+ }
+ if (l9_13.index1[5]) {
+/* Computing 2nd power */
+ d__1 = l2_9.x[7];
+ l3_12.g[5] = l2_9.x[7] * 1.098 + 57.425 - d__1 * d__1 * .038 + l2_9.x[
+ 5] * .325 - l2_9.x[6] * .99;
+ }
+ if (l9_13.index1[6]) {
+/* Computing 2nd power */
+ d__1 = l2_9.x[7];
+ l3_12.g[6] = l2_9.x[0] * -1.12 - l2_9.x[0] * .13167 * l2_9.x[7] +
+ l2_9.x[0] * .00667 * (d__1 * d__1) + l2_9.x[3] / .99;
+ }
+ if (l9_13.index1[7]) {
+/* Computing 2nd power */
+ d__1 = l2_9.x[7];
+ l3_12.g[7] = -57.425 - l2_9.x[7] * 1.098 + d__1 * d__1 * .038 -
+ l2_9.x[5] * .325 + l2_9.x[6] / .99;
+ }
+ if (l9_13.index1[8]) {
+ l3_12.g[8] = l2_9.x[3] * 1.22 - l2_9.x[0] - l2_9.x[4];
+ }
+ if (l9_13.index1[9]) {
+ l3_12.g[9] = l2_9.x[2] * 9.8e4 / (l2_9.x[3] * l2_9.x[8] + l2_9.x[2] *
+ 1e3) - l2_9.x[5];
+ }
+ if (l9_13.index1[10]) {
+ l3_12.g[10] = (l2_9.x[1] + l2_9.x[4]) / l2_9.x[0] - l2_9.x[7];
+ }
+ return 0;
+labelL5:
+ if (! l10_13.index2[4]) {
+ goto labelL11;
+ }
+/* Computing 2nd power */
+ d__1 = l2_9.x[7];
+ l5_23.gg[4] = l2_9.x[7] * .13167 + 1.12 - d__1 * d__1 * .00667;
+ l5_23.gg[81] = l2_9.x[0] * .13167 - l2_9.x[0] * .013339999999999999 *
+ l2_9.x[7];
+labelL11:
+ if (l10_13.index2[5]) {
+ l5_23.gg[82] = 1.098 - l2_9.x[7] * .076;
+ }
+ if (! l10_13.index2[6]) {
+ goto labelL13;
+ }
+ l5_23.gg[83] = l2_9.x[0] * -.13167 + l2_9.x[0] * .013339999999999999 *
+ l2_9.x[7];
+/* Computing 2nd power */
+ d__1 = l2_9.x[7];
+ l5_23.gg[6] = l2_9.x[7] * -.13167 + d__1 * d__1 * .00667 - 1.12;
+labelL13:
+ if (l10_13.index2[7]) {
+ l5_23.gg[84] = l2_9.x[7] * .076 - 1.098;
+ }
+ if (! l10_13.index2[9]) {
+ goto L16;
+ }
+/* Computing 2nd power */
+ d__1 = l2_9.x[3] * l2_9.x[8] + l2_9.x[2] * 1e3;
+ v1 = d__1 * d__1;
+ v2 = l2_9.x[8] * 9.8e4;
+ v3 = v2 / v1;
+ l5_23.gg[31] = l2_9.x[3] * v3;
+ l5_23.gg[42] = -l2_9.x[2] * v3;
+ l5_23.gg[97] = l2_9.x[2] * -9.8e4 * l2_9.x[3] / v1;
+L16:
+ if (! l10_13.index2[10]) {
+ goto L17;
+ }
+/* Computing 2nd power */
+ d__1 = l2_9.x[0];
+ l5_23.gg[10] = -(l2_9.x[1] + l2_9.x[4]) / (d__1 * d__1);
+ l5_23.gg[21] = 1. / l2_9.x[0];
+ l5_23.gg[54] = l5_23.gg[21];
+L17:
+ return 0;
+} /* tp114_ */
+
+
+/* Subroutine */ int tp116_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 13;
+ l1_1.nili = 5;
+ l1_1.ninl = 10;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_10.x[0] = .5;
+ l2_10.x[1] = .8;
+ l2_10.x[2] = .9;
+ l2_10.x[3] = .1;
+ l2_10.x[4] = .14;
+ l2_10.x[5] = .5;
+ l2_10.x[6] = 489.;
+ l2_10.x[7] = 80.;
+ l2_10.x[8] = 650.;
+ l2_10.x[9] = 450.;
+ l2_10.x[10] = 150.;
+ l2_10.x[11] = 150.;
+ l2_10.x[12] = 150.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* labelL6: */
+ l13_10.xl[i__ - 1] = .1;
+ }
+ l13_10.xl[3] = 1e-4;
+ l13_10.xl[8] = 500.;
+ l13_10.xl[10] = 1.;
+ l13_10.xl[11] = 1e-4;
+ l13_10.xl[12] = 1e-4;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l14_10.xu[i__ - 1] = 1.;
+ l14_10.xu[i__ + 5] = 1e3;
+/* L7: */
+ l14_10.xu[i__ + 9] = 150.;
+ }
+ l14_10.xu[3] = .1;
+ l14_10.xu[4] = .9;
+ l14_10.xu[5] = .9;
+ l14_10.xu[9] = 500.;
+ for (i__ = 1; i__ <= 13; ++i__) {
+ l11_10.lxl[i__ - 1] = true;
+ l12_10.lxu[i__ - 1] = true;
+ for (j = 1; j <= 15; ++j) {
+/* L32: */
+ l5_24.gg[j + i__ * 15 - 16] = 0.;
+ }
+ }
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L30: */
+ l4_10.gf[i__ - 1] = 0.;
+ }
+ for (i__ = 11; i__ <= 13; ++i__) {
+/* L31: */
+ l4_10.gf[i__ - 1] = 1.;
+ }
+ l5_24.gg[15] = -1.;
+ l5_24.gg[30] = 1.;
+ l5_24.gg[1] = -1.;
+ l5_24.gg[16] = 1.;
+ l5_24.gg[92] = -.002;
+ l5_24.gg[107] = .002;
+ l5_24.gg[153] = 1.;
+ l5_24.gg[168] = 1.;
+ l5_24.gg[183] = 1.;
+ l5_24.gg[154] = -1.;
+ l5_24.gg[169] = -1.;
+ l5_24.gg[184] = -1.;
+ l5_24.gg[185] = 1.;
+ l5_24.gg[163] = 1.;
+ l5_24.gg[179] = 1.;
+ l20_11.lex = false;
+ l20_11.xex[0] = .803770278595;
+ l20_11.xex[1] = .89998603367;
+ l20_11.xex[2] = .970972419495;
+ l20_11.xex[3] = .0999995162129;
+ l20_11.xex[4] = .190815447786;
+ l20_11.xex[5] = .460571745738;
+ l20_11.xex[6] = 574.080310673;
+ l20_11.xex[7] = 74.0804261398;
+ l20_11.xex[8] = 500.016155317;
+ l20_11.xex[9] = .0999999999985;
+ l20_11.xex[10] = 20.2341325935;
+ l20_11.xex[11] = 77.3475459898;
+ l20_11.xex[12] = .00673039736648;
+ l20_11.fex = 97.5884089805;
+ return 0;
+labelL2:
+ l6_1.fx = l2_10.x[10] + l2_10.x[11] + l2_10.x[12];
+labelL3:
+ return 0;
+labelL4:
+ if (l9_14.index1[0]) {
+ l3_13.g[0] = l2_10.x[2] - l2_10.x[1];
+ }
+ if (l9_14.index1[1]) {
+ l3_13.g[1] = l2_10.x[1] - l2_10.x[0];
+ }
+ if (l9_14.index1[2]) {
+ l3_13.g[2] = 1. - (l2_10.x[6] - l2_10.x[7]) * .002;
+ }
+ if (l9_14.index1[3]) {
+ l3_13.g[3] = l2_10.x[10] + l2_10.x[11] + l2_10.x[12] - 50.;
+ }
+ if (l9_14.index1[4]) {
+ l3_13.g[4] = 250. - l2_10.x[10] - l2_10.x[11] - l2_10.x[12];
+ }
+ if (l9_14.index1[5]) {
+ l3_13.g[5] = l2_10.x[12] - l2_10.x[9] * 1.262626 + l2_10.x[2] *
+ 1.231059 * l2_10.x[9];
+ }
+ if (l9_14.index1[6]) {
+/* Computing 2nd power */
+ d__1 = l2_10.x[1];
+ l3_13.g[6] = l2_10.x[4] - l2_10.x[1] * .03475 - l2_10.x[1] * .975 *
+ l2_10.x[4] + d__1 * d__1 * .00975;
+ }
+ if (l9_14.index1[7]) {
+/* Computing 2nd power */
+ d__1 = l2_10.x[2];
+ l3_13.g[7] = l2_10.x[5] - l2_10.x[2] * .03475 - l2_10.x[2] * .975 *
+ l2_10.x[5] + d__1 * d__1 * .00975;
+ }
+ if (l9_14.index1[8]) {
+ l3_13.g[8] = l2_10.x[4] * l2_10.x[6] - l2_10.x[0] * l2_10.x[7] -
+ l2_10.x[3] * l2_10.x[6] + l2_10.x[3] * l2_10.x[7];
+ }
+ if (l9_14.index1[9]) {
+ l3_13.g[9] = (l2_10.x[1] * l2_10.x[8] + l2_10.x[4] * l2_10.x[7] -
+ l2_10.x[0] * l2_10.x[7] - l2_10.x[5] * l2_10.x[8]) * -.002 -
+ l2_10.x[5] - l2_10.x[4] + 1.;
+ }
+ if (l9_14.index1[10]) {
+ l3_13.g[10] = l2_10.x[1] * l2_10.x[8] - l2_10.x[2] * l2_10.x[9] -
+ l2_10.x[5] * l2_10.x[8] - (l2_10.x[1] - l2_10.x[5]) * 500. +
+ l2_10.x[1] * l2_10.x[9];
+ }
+ if (l9_14.index1[11]) {
+ l3_13.g[11] = l2_10.x[1] - .9 - (l2_10.x[1] * l2_10.x[9] - l2_10.x[2]
+ * l2_10.x[9]) * .002;
+ }
+ if (l9_14.index1[12]) {
+/* Computing 2nd power */
+ d__1 = l2_10.x[0];
+ l3_13.g[12] = l2_10.x[3] - l2_10.x[0] * .03475 - l2_10.x[0] * .975 *
+ l2_10.x[3] + d__1 * d__1 * .00975;
+ }
+ if (l9_14.index1[13]) {
+ l3_13.g[13] = l2_10.x[10] - l2_10.x[7] * 1.262626 + l2_10.x[0] *
+ 1.231059 * l2_10.x[7];
+ }
+ if (l9_14.index1[14]) {
+ l3_13.g[14] = l2_10.x[11] - l2_10.x[8] * 1.262626 + l2_10.x[1] *
+ 1.231059 * l2_10.x[8];
+ }
+ return 0;
+labelL5:
+ if (! l10_14.index2[5]) {
+ goto labelL12;
+ }
+ l5_24.gg[35] = l2_10.x[9] * 1.231059;
+ l5_24.gg[140] = l2_10.x[2] * 1.231059 - 1.262626;
+labelL12:
+ if (! l10_14.index2[6]) {
+ goto labelL13;
+ }
+ l5_24.gg[21] = -.03475 - l2_10.x[4] * .975 + l2_10.x[1] * .0195;
+ l5_24.gg[66] = 1. - l2_10.x[1] * .975;
+labelL13:
+ if (! l10_14.index2[7]) {
+ goto labelL14;
+ }
+ l5_24.gg[37] = -.03475 - l2_10.x[5] * .975 + l2_10.x[2] * .0195;
+ l5_24.gg[82] = 1. - l2_10.x[2] * .975;
+labelL14:
+ if (! l10_14.index2[8]) {
+ goto L15;
+ }
+ l5_24.gg[8] = -l2_10.x[7];
+ l5_24.gg[53] = -l2_10.x[6] + l2_10.x[7];
+ l5_24.gg[68] = l2_10.x[6];
+ l5_24.gg[98] = l2_10.x[4] - l2_10.x[3];
+ l5_24.gg[113] = -l2_10.x[0] + l2_10.x[3];
+L15:
+ if (! l10_14.index2[9]) {
+ goto L16;
+ }
+ l5_24.gg[9] = l2_10.x[7] * .002;
+ l5_24.gg[24] = l2_10.x[8] * -.002;
+ l5_24.gg[69] = l2_10.x[7] * -.002 - 1.;
+ l5_24.gg[84] = l2_10.x[8] * .002 - 1.;
+ l5_24.gg[114] = (l2_10.x[4] - l2_10.x[0]) * -.002;
+ l5_24.gg[129] = (l2_10.x[1] - l2_10.x[5]) * -.002;
+L16:
+ if (! l10_14.index2[10]) {
+ goto L17;
+ }
+ l5_24.gg[25] = l2_10.x[8] - 500. + l2_10.x[9];
+ l5_24.gg[40] = -l2_10.x[9];
+ l5_24.gg[85] = -l2_10.x[8] + 500.;
+ l5_24.gg[130] = l2_10.x[1] - l2_10.x[5];
+ l5_24.gg[145] = -l2_10.x[2] + l2_10.x[1];
+L17:
+ if (! l10_14.index2[11]) {
+ goto L18;
+ }
+ l5_24.gg[26] = 1. - l2_10.x[9] * .002;
+ l5_24.gg[41] = l2_10.x[9] * .002;
+ l5_24.gg[146] = (l2_10.x[2] - l2_10.x[1]) * .002;
+L18:
+ if (! l10_14.index2[12]) {
+ goto L19;
+ }
+ l5_24.gg[12] = -.03475 - l2_10.x[3] * .975 + l2_10.x[0] * .0195;
+ l5_24.gg[57] = 1. - l2_10.x[0] * .975;
+L19:
+ if (! l10_14.index2[13]) {
+ goto labelL20;
+ }
+ l5_24.gg[13] = l2_10.x[7] * 1.231059;
+ l5_24.gg[118] = l2_10.x[0] * 1.231059 - 1.262626;
+labelL20:
+ if (! l10_14.index2[14]) {
+ goto L21;
+ }
+ l5_24.gg[29] = l2_10.x[8] * 1.231059;
+ l5_24.gg[134] = l2_10.x[1] * 1.231059 - 1.262626;
+L21:
+ return 0;
+} /* tp116_ */
+
+
+/* Subroutine */ int tp117_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real a[50] /* was [10][5] */, b[10], c__[25] /*
+ was [5][5] */, d__[5], e[5];
+ static int i__, j;
+ static Real t1, t2, t3, t4[5], t5[5], t6[5];
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL2;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 15;
+ l1_1.nili = 0;
+ l1_1.ninl = 5;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 15; ++i__) {
+/* labelL20: */
+ l2_11.x[i__ - 1] = .001;
+ }
+ l2_11.x[6] = 60.;
+ for (i__ = 1; i__ <= 15; ++i__) {
+ l13_11.xl[i__ - 1] = 0.;
+ l11_11.lxl[i__ - 1] = true;
+ l12_11.lxu[i__ - 1] = false;
+ for (j = 1; j <= 5; ++j) {
+/* L22: */
+ l5_25.gg[j + i__ * 5 - 6] = 0.;
+ }
+ }
+ e[0] = -15.;
+ e[1] = -27.;
+ e[2] = -36.;
+ e[3] = -18.;
+ e[4] = -12.;
+ c__[0] = 30.;
+ c__[5] = -20.;
+ c__[10] = -10.;
+ c__[15] = 32.;
+ c__[20] = -10.;
+ c__[6] = 39.;
+ c__[11] = -6.;
+ c__[16] = -31.;
+ c__[21] = 32.;
+ c__[12] = 10.;
+ c__[17] = -6.;
+ c__[22] = -10.;
+ c__[18] = 39.;
+ c__[23] = -20.;
+ c__[24] = 30.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ for (j = 1; j <= 5; ++j) {
+/* L70: */
+ c__[j + i__ * 5 - 6] = c__[i__ + j * 5 - 6];
+ }
+ }
+ d__[0] = 4.;
+ d__[1] = 8.;
+ d__[2] = 10.;
+ d__[3] = 6.;
+ d__[4] = 2.;
+ for (i__ = 1; i__ <= 6; ++i__) {
+ for (j = 1; j <= 5; ++j) {
+/* L72: */
+ a[i__ + j * 10 - 11] = 0.;
+ }
+ }
+ a[0] = -16.;
+ a[10] = 2.;
+ a[30] = 1.;
+ a[11] = -2.;
+ a[31] = .4;
+ a[41] = 2.;
+ a[2] = -3.5;
+ a[22] = 2.;
+ a[13] = -2.;
+ a[33] = -4.;
+ a[43] = -1.;
+ a[14] = -9.;
+ a[24] = -2.;
+ a[34] = 1.;
+ a[44] = -2.8;
+ a[5] = 2.;
+ a[25] = -4.;
+ a[7] = -1.;
+ a[17] = -2.;
+ a[27] = -3.;
+ a[37] = -2.;
+ a[47] = -1.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ a[i__ * 10 - 4] = -1.;
+ a[i__ * 10 - 2] = (Real) i__;
+/* L73: */
+ a[i__ * 10 - 1] = 1.;
+ }
+ b[0] = -40.;
+ b[1] = -2.;
+ b[2] = -.25;
+ b[3] = -4.;
+ b[4] = -4.;
+ b[5] = -1.;
+ b[6] = -40.;
+ b[7] = -60.;
+ b[8] = 5.;
+ b[9] = 1.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L35: */
+ l4_11.gf[i__ - 1] = -b[i__ - 1];
+ }
+ for (i__ = 1; i__ <= 10; ++i__) {
+ for (j = 1; j <= 5; ++j) {
+/* L40: */
+ l5_25.gg[j + i__ * 5 - 6] = -a[i__ + j * 10 - 11];
+ }
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+ for (j = 1; j <= 5; ++j) {
+ if (i__ - j != 0) {
+ goto L45;
+ } else {
+ goto L42;
+ }
+L45:
+ l5_25.gg[j + (i__ + 10) * 5 - 6] = c__[i__ + j * 5 - 6] * 2.;
+L42:
+ ;
+ }
+/* L41: */
+ }
+ l20_12.lex = false;
+ l20_12.xex[0] = 0.;
+ l20_12.xex[1] = 0.;
+ l20_12.xex[2] = 5.1741363068;
+ l20_12.xex[3] = 0.;
+ l20_12.xex[4] = 3.06109271525;
+ l20_12.xex[5] = 11.839676029;
+ l20_12.xex[6] = 0.;
+ l20_12.xex[7] = 0.;
+ l20_12.xex[8] = .103907059194;
+ l20_12.xex[9] = 0.;
+ l20_12.xex[10] = .299992902601;
+ l20_12.xex[11] = .333470928832;
+ l20_12.xex[12] = .399990975915;
+ l20_12.xex[13] = .428314541579;
+ l20_12.xex[14] = .223960749729;
+ l20_12.fex = 32.3486789791;
+ return 0;
+labelL2:
+ t1 = 0.;
+ t2 = 0.;
+ for (j = 1; j <= 5; ++j) {
+/* Computing 3rd power */
+ d__1 = l2_11.x[j + 9];
+ t2 += d__[j - 1] * (d__1 * (d__1 * d__1));
+ for (i__ = 1; i__ <= 5; ++i__) {
+ t1 += c__[i__ + j * 5 - 6] * l2_11.x[i__ + 9] * l2_11.x[j + 9];
+/* L30: */
+ }
+ }
+ t3 = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L31: */
+ t3 += b[i__ - 1] * l2_11.x[i__ - 1];
+ }
+ for (j = 1; j <= 5; ++j) {
+ t4[j - 1] = 0.;
+ t5[j - 1] = 0.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L32: */
+ t4[j - 1] += c__[i__ + j * 5 - 6] * l2_11.x[i__ + 9];
+ }
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L33: */
+ t5[j - 1] += a[i__ + j * 10 - 11] * l2_11.x[i__ - 1];
+ }
+/* L34: */
+ }
+ if (*mode == 4) {
+ goto labelL4;
+ }
+ l6_1.fx = -(t3 - t1 - t2 * 2.);
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 5; ++i__) {
+ t6[i__ - 1] = 0.;
+ for (j = 1; j <= 5; ++j) {
+/* L37: */
+ t6[i__ - 1] += (c__[i__ + j * 5 - 6] + c__[j + i__ * 5 - 6]) *
+ l2_11.x[j + 9];
+ }
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L36: */
+/* Computing 2nd power */
+ d__1 = l2_11.x[i__ + 9];
+ l4_11.gf[i__ + 9] = t6[i__ - 1] + d__[i__ - 1] * 6. * (d__1 * d__1);
+ }
+ return 0;
+labelL4:
+ for (j = 1; j <= 5; ++j) {
+ if (l9_5.index1[j - 1]) {
+/* Computing 2nd power */
+ d__1 = l2_11.x[j + 9];
+ l3_4.g[j - 1] = t4[j - 1] * 2. + d__[j - 1] * 3. * (d__1 * d__1)
+ + e[j - 1] - t5[j - 1];
+ }
+/* L38: */
+ }
+ return 0;
+labelL5:
+ for (j = 1; j <= 5; ++j) {
+ if (! l10_5.index2[j - 1]) {
+ goto L39;
+ }
+ l5_25.gg[j + (j + 10) * 5 - 6] = c__[j + j * 5 - 6] * 2. + d__[j - 1]
+ * 6. * l2_11.x[j + 9];
+L39:
+ ;
+ }
+ return 0;
+} /* tp117_ */
+
+/* Subroutine */ int tp118_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__, j, k, m;
+ static Real t;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 15;
+ l1_1.nili = 29;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 15; ++i__) {
+/* labelL6: */
+ l2_11.x[i__ - 1] = 20.;
+ }
+ l2_11.x[1] = 55.;
+ l2_11.x[2] = 15.;
+ l2_11.x[4] = 60.;
+ l2_11.x[7] = 60.;
+ l2_11.x[10] = 60.;
+ l2_11.x[13] = 60.;
+ l13_11.xl[0] = 8.;
+ l13_11.xl[1] = 43.;
+ l13_11.xl[2] = 3.;
+ l14_11.xu[0] = 21.;
+ l14_11.xu[1] = 57.;
+ l14_11.xu[2] = 16.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l13_11.xl[i__ * 3] = 0.;
+ l13_11.xl[i__ * 3 + 1] = 0.;
+ l13_11.xl[i__ * 3 + 2] = 0.;
+ l14_11.xu[i__ * 3] = 90.;
+ l14_11.xu[i__ * 3 + 1] = 120.;
+ l14_11.xu[i__ * 3 + 2] = 60.;
+/* L22: */
+ }
+ for (i__ = 1; i__ <= 15; ++i__) {
+ l11_11.lxl[i__ - 1] = true;
+ l12_11.lxu[i__ - 1] = true;
+ for (j = 1; j <= 29; ++j) {
+/* L25: */
+ l5_26.gg[j + i__ * 29 - 30] = 0.;
+ }
+ }
+ for (k = 1; k <= 4; ++k) {
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l5_26.gg[k + (i__ << 2) - 4 + (k * 3 + i__) * 29 - 30] = 1.;
+ l5_26.gg[k + (i__ << 2) - 4 + (k * 3 + i__ - 3) * 29 - 30] = -1.;
+ l5_26.gg[k + 8 + (i__ << 2) + (k * 3 + i__) * 29 - 30] = -1.;
+ l5_26.gg[k + 8 + (i__ << 2) + (k * 3 + i__ - 3) * 29 - 30] = 1.;
+/* labelL20: */
+ }
+ }
+ for (k = 1; k <= 5; ++k) {
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L21: */
+ l5_26.gg[k + 24 + (k * 3 - 3 + i__) * 29 - 30] = 1.;
+ }
+ }
+ l20_12.lex = false;
+ l20_12.xex[0] = 8.;
+ l20_12.xex[1] = 49.;
+ l20_12.xex[2] = 3.;
+ l20_12.xex[3] = 1.;
+ l20_12.xex[4] = 56.;
+ l20_12.xex[5] = 0.;
+ l20_12.xex[6] = .999999999545;
+ l20_12.xex[7] = 63.;
+ l20_12.xex[8] = 6.;
+ l20_12.xex[9] = 2.99999999965;
+ l20_12.xex[10] = 70.;
+ l20_12.xex[11] = 12.;
+ l20_12.xex[12] = 4.99999999971;
+ l20_12.xex[13] = 77.;
+ l20_12.xex[14] = 18.;
+ l20_12.fex = 664.820449993;
+ return 0;
+labelL2:
+ t = 0.;
+ for (m = 1; m <= 5; ++m) {
+ i__ = m - 1;
+/* L30: */
+/* Computing 2nd power */
+ d__1 = l2_11.x[i__ * 3];
+/* Computing 2nd power */
+ d__2 = l2_11.x[i__ * 3 + 1];
+/* Computing 2nd power */
+ d__3 = l2_11.x[i__ * 3 + 2];
+ t = t + l2_11.x[i__ * 3] * 2.3 + d__1 * d__1 * 1e-4 + l2_11.x[i__ * 3
+ + 1] * 1.7 + d__2 * d__2 * 1e-4 + l2_11.x[i__ * 3 + 2] * 2.2
+ + d__3 * d__3 * 1.5e-4;
+ }
+ l6_1.fx = t;
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l4_11.gf[i__ * 3 - 3] = l2_11.x[i__ * 3 - 3] * 2e-4 + 2.3;
+ l4_11.gf[i__ * 3 - 2] = l2_11.x[i__ * 3 - 2] * 2e-4 + 1.7;
+/* L31: */
+ l4_11.gf[i__ * 3 - 1] = l2_11.x[i__ * 3 - 1] * 3e-4 + 2.2;
+ }
+ return 0;
+labelL4:
+ for (i__ = 1; i__ <= 4; ++i__) {
+ if (l9_15.index1[i__ - 1]) {
+ l3_14.g[i__ - 1] = l2_11.x[i__ * 3] - l2_11.x[i__ * 3 - 3] + 7.;
+ }
+ if (l9_15.index1[i__ + 3]) {
+ l3_14.g[i__ + 3] = l2_11.x[i__ * 3 + 1] - l2_11.x[i__ * 3 - 2] +
+ 7.;
+ }
+ if (l9_15.index1[i__ + 7]) {
+ l3_14.g[i__ + 7] = l2_11.x[i__ * 3 + 2] - l2_11.x[i__ * 3 - 1] +
+ 7.;
+ }
+ if (l9_15.index1[i__ + 11]) {
+ l3_14.g[i__ + 11] = l2_11.x[i__ * 3 - 3] - l2_11.x[i__ * 3] + 6.;
+ }
+ if (l9_15.index1[i__ + 15]) {
+ l3_14.g[i__ + 15] = l2_11.x[i__ * 3 - 2] - l2_11.x[i__ * 3 + 1] +
+ 7.;
+ }
+/* L32: */
+ if (l9_15.index1[i__ + 19]) {
+ l3_14.g[i__ + 19] = l2_11.x[i__ * 3 - 1] - l2_11.x[i__ * 3 + 2] +
+ 6.;
+ }
+ }
+ if (l9_15.index1[24]) {
+ l3_14.g[24] = l2_11.x[0] + l2_11.x[1] + l2_11.x[2] - 60.;
+ }
+ if (l9_15.index1[25]) {
+ l3_14.g[25] = l2_11.x[3] + l2_11.x[4] + l2_11.x[5] - 50.;
+ }
+ if (l9_15.index1[26]) {
+ l3_14.g[26] = l2_11.x[6] + l2_11.x[7] + l2_11.x[8] - 70.;
+ }
+ if (l9_15.index1[27]) {
+ l3_14.g[27] = l2_11.x[9] + l2_11.x[10] + l2_11.x[11] - 85.;
+ }
+ if (l9_15.index1[28]) {
+ l3_14.g[28] = l2_11.x[12] + l2_11.x[13] + l2_11.x[14] - 100.;
+ }
+labelL5:
+ return 0;
+} /* tp118_ */
+
+/* Subroutine */ int tp119_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static Real a[256] /* was [16][16] */, b[128] /* was [8][16]
+ */, c__[8];
+ static int i__, j;
+ static Real s[16], t;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 16;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 8;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 16; ++i__) {
+ for (j = 1; j <= 16; ++j) {
+/* L21: */
+ a[i__ + (j << 4) - 17] = 0.;
+ }
+ a[i__ + (i__ << 4) - 17] = 1.;
+ for (j = 1; j <= 8; ++j) {
+/* L81: */
+ b[j + (i__ << 3) - 9] = 0.;
+ }
+/* L80: */
+ }
+ for (i__ = 1; i__ <= 8; ++i__) {
+/* L22: */
+ b[i__ + ((i__ + 8) << 3) - 9] = 1.;
+ }
+ a[48] = 1.;
+ a[96] = 1.;
+ a[112] = 1.;
+ a[240] = 1.;
+ a[33] = 1.;
+ a[97] = 1.;
+ a[145] = 1.;
+ a[98] = 1.;
+ a[130] = 1.;
+ a[146] = 1.;
+ a[210] = 1.;
+ a[99] = 1.;
+ a[163] = 1.;
+ a[227] = 1.;
+ a[84] = 1.;
+ a[148] = 1.;
+ a[180] = 1.;
+ a[244] = 1.;
+ a[117] = 1.;
+ a[229] = 1.;
+ a[166] = 1.;
+ a[198] = 1.;
+ a[151] = 1.;
+ a[231] = 1.;
+ a[184] = 1.;
+ a[248] = 1.;
+ a[217] = 1.;
+ a[202] = 1.;
+ a[219] = 1.;
+ a[220] = 1.;
+ b[0] = .22;
+ b[8] = .2;
+ b[16] = .19;
+ b[24] = .25;
+ b[32] = .15;
+ b[40] = .11;
+ b[48] = .12;
+ b[56] = .13;
+ b[1] = -1.46;
+ b[17] = -1.3;
+ b[25] = 1.82;
+ b[33] = -1.15;
+ b[49] = .8;
+ b[2] = 1.29;
+ b[10] = -.89;
+ b[34] = -1.16;
+ b[42] = -.96;
+ b[58] = -.49;
+ b[3] = -1.1;
+ b[11] = -1.06;
+ b[19] = .95;
+ b[27] = -.54;
+ b[43] = -1.78;
+ b[51] = -.41;
+ b[28] = -1.43;
+ b[36] = 1.51;
+ b[44] = .59;
+ b[52] = -.33;
+ b[60] = -.43;
+ b[13] = -1.72;
+ b[21] = -.33;
+ b[37] = 1.62;
+ b[45] = 1.24;
+ b[53] = .21;
+ b[61] = -.26;
+ b[6] = 1.12;
+ b[30] = .31;
+ b[54] = 1.12;
+ b[70] = -.36;
+ b[15] = .45;
+ b[23] = .26;
+ b[31] = -1.1;
+ b[39] = .58;
+ b[55] = -1.03;
+ b[63] = .1;
+ c__[0] = 2.5;
+ c__[1] = 1.1;
+ c__[2] = -3.1;
+ c__[3] = -3.5;
+ c__[4] = 1.3;
+ c__[5] = 2.1;
+ c__[6] = 2.3;
+ c__[7] = -1.5;
+ for (i__ = 1; i__ <= 16; ++i__) {
+ l11_12.lxl[i__ - 1] = true;
+ l12_12.lxu[i__ - 1] = true;
+ l2_12.x[i__ - 1] = 10.;
+ l13_12.xl[i__ - 1] = 0.;
+ l14_12.xu[i__ - 1] = 5.;
+ for (j = 1; j <= 8; ++j) {
+/* labelL20: */
+ l5_27.gg[j + (i__ << 3) - 9] = b[j + (i__ << 3) - 9];
+ }
+ }
+ l20_13.lex = false;
+ l20_13.xex[0] = .0398473514099;
+ l20_13.xex[1] = .791983155694;
+ l20_13.xex[2] = .202870330224;
+ l20_13.xex[3] = .844357916347;
+ l20_13.xex[4] = 1.26990645286;
+ l20_13.xex[5] = .934738707827;
+ l20_13.xex[6] = 1.68196196924;
+ l20_13.xex[7] = .15530087749;
+ l20_13.xex[8] = 1.56787033356;
+ l20_13.xex[9] = -3.59021173251e-12;
+ l20_13.xex[10] = -6.12900888082e-12;
+ l20_13.xex[11] = -8.86794857449e-13;
+ l20_13.xex[12] = .660204066;
+ l20_13.xex[13] = -2.54340725727e-12;
+ l20_13.xex[14] = .674255926901;
+ l20_13.xex[15] = -1.10433723798e-11;
+ l20_13.fex = 244.899697515;
+ return 0;
+labelL2:
+ t = 0.;
+ for (i__ = 1; i__ <= 16; ++i__) {
+ for (j = 1; j <= 16; ++j) {
+/* L30: */
+/* Computing 2nd power */
+ d__1 = l2_12.x[i__ - 1];
+/* Computing 2nd power */
+ d__2 = l2_12.x[j - 1];
+ t += a[i__ + (j << 4) - 17] * (d__1 * d__1 + l2_12.x[i__ - 1] +
+ 1.) * (d__2 * d__2 + l2_12.x[j - 1] + 1.);
+ }
+ }
+ l6_1.fx = t;
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 16; ++i__) {
+ s[i__ - 1] = 0.;
+ for (j = 1; j <= 16; ++j) {
+/* L32: */
+/* Computing 2nd power */
+ d__1 = l2_12.x[j - 1];
+ s[i__ - 1] += (a[i__ + (j << 4) - 17] + a[j + (i__ << 4) - 17]) *
+ (d__1 * d__1 + l2_12.x[j - 1] + 1.) * (l2_12.x[i__ - 1] *
+ 2. + 1.);
+ }
+/* L31: */
+ l4_12.gf[i__ - 1] = s[i__ - 1];
+ }
+ return 0;
+labelL4:
+ for (i__ = 1; i__ <= 8; ++i__) {
+ if (! l9_12.index1[i__ - 1]) {
+ goto L33;
+ }
+ s[i__ - 1] = 0.;
+ for (j = 1; j <= 16; ++j) {
+/* L34: */
+ s[i__ - 1] += b[i__ + (j << 3) - 9] * l2_12.x[j - 1];
+ }
+ l3_11.g[i__ - 1] = s[i__ - 1] - c__[i__ - 1];
+L33:
+ ;
+ }
+labelL5:
+ return 0;
+} /* tp119_ */
+
+
+/* Subroutine */ int tp201_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 8.;
+ l2_1.x[1] = 9.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 0.;
+ l20_1.xex[0] = 5.;
+ l20_1.xex[1] = 6.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 5.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1] - 6.;
+ l6_1.fx = d__1 * d__1 * 4. + d__2 * d__2;
+ return 0;
+labelL3:
+ l4_1.gf[0] = (l2_1.x[0] - 5.) * 8.;
+ l4_1.gf[1] = (l2_1.x[1] - 6.) * 2.;
+labelL4:
+ return 0;
+} /* tp201_ */
+
+
+/* Subroutine */ int tp202_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 15.;
+ l2_1.x[1] = -2.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = true;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = true;
+ }
+ l14_1.xu[0] = (float)20.;
+ l14_1.xu[1] = (float)5.;
+ l13_1.xl[0] = (float)1.;
+ l13_1.xl[1] = (float)-5.;
+ l20_6.lex = true;
+ l20_6.nex = 2;
+ l20_6.fex = 0.;
+ l20_6.xex[0] = 5.;
+ l20_6.xex[1] = 4.;
+ l20_6.xex[2] = 48.98425;
+ l20_6.xex[3] = -.89681;
+ l15_1.lsum = 2;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+/* Computing 3rd power */
+ d__2 = l2_1.x[1];
+ l16_1.f[0] = l2_1.x[0] - 13. - l2_1.x[1] * 2. + d__1 * d__1 * 5. - d__2 *
+ (d__2 * d__2);
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+/* Computing 3rd power */
+ d__2 = l2_1.x[1];
+ l16_1.f[1] = l2_1.x[0] - 29. - l2_1.x[1] * 14. + d__1 * d__1 + d__2 * (
+ d__2 * d__2);
+/* Computing 2nd power */
+ d__1 = l16_1.f[0];
+/* Computing 2nd power */
+ d__2 = l16_1.f[1];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+/* Computing 3rd power */
+ d__2 = l2_1.x[1];
+ l16_1.f[0] = l2_1.x[0] - 13. - l2_1.x[1] * 2. + d__1 * d__1 * 5. - d__2 *
+ (d__2 * d__2);
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+/* Computing 3rd power */
+ d__2 = l2_1.x[1];
+ l16_1.f[1] = l2_1.x[0] - 29. - l2_1.x[1] * 14. + d__1 * d__1 + d__2 * (
+ d__2 * d__2);
+ l17_1.df[0] = 1.;
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+ l17_1.df[2] = l2_1.x[1] * 10. - 2. - d__1 * d__1 * 3.;
+ l17_1.df[1] = 1.;
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+ l17_1.df[3] = l2_1.x[1] * 2. - 14. + d__1 * d__1 * 3.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+/* L7: */
+ l4_1.gf[i__ - 1] = l16_1.f[0] * 2. * l17_1.df[(i__ << 1) - 2] +
+ l16_1.f[1] * 2. * l17_1.df[(i__ << 1) - 1];
+ }
+labelL4:
+ return 0;
+} /* tp202_ */
+
+
+/* Subroutine */ int tp203_(int *mode)
+{
+ /* Initialized data */
+
+ static Real c__[3] = { 1.5,2.25,2.625 };
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Builtin functions */
+ Real pow_di(Real* , int*);
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 2.;
+ l2_1.x[1] = .2;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 0.;
+ l20_1.xex[0] = 3.;
+ l20_1.xex[1] = .5;
+ l15_1.lsum = 3;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l16_2.f[i__ - 1] = c__[i__ - 1] - l2_1.x[0] * (1. - pow_di(&l2_1.x[1],
+ &i__));
+/* L7: */
+/* Computing 2nd power */
+ d__1 = l16_2.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L8: */
+ l16_2.f[i__ - 1] = c__[i__ - 1] - l2_1.x[0] * (1. - pow_di(&l2_1.x[1],
+ &i__));
+ }
+ l17_2.df[0] = l2_1.x[1] - 1.;
+ l17_2.df[3] = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+ l17_2.df[1] = d__1 * d__1 - 1.;
+ l17_2.df[4] = l2_1.x[0] * 2. * l2_1.x[1];
+/* Computing 3rd power */
+ d__1 = l2_1.x[1];
+ l17_2.df[2] = d__1 * (d__1 * d__1) - 1.;
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+ l17_2.df[5] = l2_1.x[0] * 3. * (d__1 * d__1);
+ for (i__ = 1; i__ <= 2; ++i__) {
+/* labelL9: */
+ l4_1.gf[i__ - 1] = l16_2.f[0] * 2. * l17_2.df[i__ * 3 - 3] + l16_2.f[
+ 1] * 2. * l17_2.df[i__ * 3 - 2] + l16_2.f[2] * 2. * l17_2.df[
+ i__ * 3 - 1];
+ }
+labelL4:
+ return 0;
+} /* tp203_ */
+
+
+/* Subroutine */ int tp204_(int *mode)
+{
+ /* Initialized data */
+
+ static Real a[3] = { .13294,-.244378,.325895 };
+ static Real d__[3] = { 2.5074,-1.36401,1.02282 };
+ static Real h__[6] /* was [3][2] */ = { -.564255,-.404979,
+ -.0735084,.392417,.927589,.535493 };
+
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static Real prod;
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = .1;
+ l2_1.x[1] = .1;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = .183601;
+ l20_1.xex[0] = 0.;
+ l20_1.xex[1] = 0.;
+ l15_1.lsum = 3;
+ return 0;
+labelL2:
+ for (i__ = 1; i__ <= 3; ++i__) {
+ prod = h__[i__ - 1] * l2_1.x[0] + h__[i__ + 2] * l2_1.x[1];
+/* labelL10: */
+/* Computing 2nd power */
+ d__1 = prod;
+ l16_2.f[i__ - 1] = a[i__ - 1] + prod + d__1 * d__1 * .5 * d__[i__ - 1]
+ ;
+ }
+/* Computing 2nd power */
+ d__1 = l16_2.f[0];
+/* Computing 2nd power */
+ d__2 = l16_2.f[1];
+/* Computing 2nd power */
+ d__3 = l16_2.f[2];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3;
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 3; ++i__) {
+ prod = h__[i__ - 1] * l2_1.x[0] + h__[i__ + 2] * l2_1.x[1];
+/* labelL11: */
+/* Computing 2nd power */
+ d__1 = prod;
+ l16_2.f[i__ - 1] = a[i__ - 1] + prod + d__1 * d__1 * .5 * d__[i__ - 1]
+ ;
+ }
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L7: */
+ l17_2.df[i__ - 1] = h__[i__ - 1] + (h__[i__ - 1] * l2_1.x[0] + h__[
+ i__ + 2] * l2_1.x[1]) * h__[i__ - 1] * d__[i__ - 1];
+ }
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L8: */
+ l17_2.df[i__ + 2] = h__[i__ + 2] + (h__[i__ - 1] * l2_1.x[0] + h__[
+ i__ + 2] * l2_1.x[1]) * h__[i__ + 2] * d__[i__ - 1];
+ }
+ for (i__ = 1; i__ <= 2; ++i__) {
+/* labelL9: */
+ l4_1.gf[i__ - 1] = (l16_2.f[0] * l17_2.df[i__ * 3 - 3] + l16_2.f[1] *
+ l17_2.df[i__ * 3 - 2] + l16_2.f[2] * l17_2.df[i__ * 3 - 1]) *
+ 2.;
+ }
+labelL4:
+ return 0;
+} /* tp204_ */
+
+
+/* Subroutine */ int tp205_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 0.;
+ l2_1.x[1] = 0.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 0.;
+ l20_1.xex[0] = 3.;
+ l20_1.xex[1] = .5;
+ l15_1.lsum = 3;
+ return 0;
+labelL2:
+ l16_2.f[0] = 1.5 - l2_1.x[0] * (1. - l2_1.x[1]);
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+ l16_2.f[1] = 2.25 - l2_1.x[0] * (1. - d__1 * d__1);
+/* Computing 3rd power */
+ d__1 = l2_1.x[1];
+ l16_2.f[2] = 2.625 - l2_1.x[0] * (1. - d__1 * (d__1 * d__1));
+/* Computing 2nd power */
+ d__1 = l16_2.f[0];
+/* Computing 2nd power */
+ d__2 = l16_2.f[1];
+/* Computing 2nd power */
+ d__3 = l16_2.f[2];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3;
+ return 0;
+labelL3:
+ l16_2.f[0] = 1.5 - l2_1.x[0] * (1. - l2_1.x[1]);
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+ l16_2.f[1] = 2.25 - l2_1.x[0] * (1. - d__1 * d__1);
+/* Computing 3rd power */
+ d__1 = l2_1.x[1];
+ l16_2.f[2] = 2.625 - l2_1.x[0] * (1. - d__1 * (d__1 * d__1));
+ l17_2.df[0] = l2_1.x[1] - 1.;
+ l17_2.df[3] = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+ l17_2.df[1] = d__1 * d__1 - 1.;
+ l17_2.df[4] = l2_1.x[0] * 2. * l2_1.x[1];
+/* Computing 3rd power */
+ d__1 = l2_1.x[1];
+ l17_2.df[2] = d__1 * (d__1 * d__1) - 1.;
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+ l17_2.df[5] = l2_1.x[0] * 3. * (d__1 * d__1);
+ for (i__ = 1; i__ <= 2; ++i__) {
+/* L7: */
+ l4_1.gf[i__ - 1] = l16_2.f[0] * 2. * l17_2.df[i__ * 3 - 3] + l16_2.f[
+ 1] * 2. * l17_2.df[i__ * 3 - 2] + l16_2.f[2] * 2. * l17_2.df[
+ i__ * 3 - 1];
+ }
+labelL4:
+ return 0;
+} /* tp205_ */
+
+
+/* Subroutine */ int tp206_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = -1.2;
+ l2_1.x[1] = 1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 0.;
+ l20_1.xex[0] = 1.;
+ l20_1.xex[1] = 1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = 1. - l2_1.x[0];
+ l6_1.fx = d__1 * d__1 + d__3 * d__3 * 100.;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[0] = (l2_1.x[1] - d__1 * d__1) * -2. * 2. * l2_1.x[0] - (1. -
+ l2_1.x[0]) * 200.;
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[1] = (l2_1.x[1] - d__1 * d__1) * 2.;
+labelL4:
+ return 0;
+} /* tp206_ */
+
+
+/* Subroutine */ int tp207_(int *mode)
+{
+ /* Initialized data */
+
+ static Real c__ = 1.;
+
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int lsum, i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = -1.2;
+ l2_1.x[1] = 1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 0.;
+ l20_1.xex[0] = 1.;
+ l20_1.xex[1] = 1.;
+ lsum = 2;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = 1. - l2_1.x[0];
+ l6_1.fx = c__ * (d__1 * d__1) + d__3 * d__3;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[0] = c__ * -4. * (l2_1.x[1] - d__1 * d__1) * l2_1.x[0] - (1. -
+ l2_1.x[0]) * 2.;
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[1] = c__ * 2. * (l2_1.x[1] - d__1 * d__1);
+labelL4:
+ return 0;
+} /* tp207_ */
+
+
+/* Subroutine */ int tp208_(int *mode)
+{
+ /* Initialized data */
+
+ static Real c__ = 100.;
+
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = -1.2;
+ l2_1.x[1] = 1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 0.;
+ l20_1.xex[0] = 1.;
+ l20_1.xex[1] = 1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = 1. - l2_1.x[0];
+ l6_1.fx = c__ * (d__1 * d__1) + d__3 * d__3;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[0] = c__ * -4. * (l2_1.x[1] - d__1 * d__1) * l2_1.x[0] - (1. -
+ l2_1.x[0]) * 2.;
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[1] = c__ * 2. * (l2_1.x[1] - d__1 * d__1);
+labelL4:
+ return 0;
+} /* tp208_ */
+
+/* Subroutine */ int tp209_(int *mode)
+{
+ /* Initialized data */
+
+ static Real c__ = 1e4;
+
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = -1.2;
+ l2_1.x[1] = 1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 0.;
+ l20_1.xex[0] = 1.;
+ l20_1.xex[1] = 1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = 1. - l2_1.x[0];
+ l6_1.fx = c__ * (d__1 * d__1) + d__3 * d__3;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[0] = c__ * -4. * (l2_1.x[1] - d__1 * d__1) * l2_1.x[0] - (1. -
+ l2_1.x[0]) * 2.;
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[1] = c__ * 2. * (l2_1.x[1] - d__1 * d__1);
+labelL4:
+ return 0;
+} /* tp209_ */
+
+/* Subroutine */ int tp210_(int *mode)
+{
+ /* Initialized data */
+
+ static Real c__ = 1e6;
+
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = -1.2;
+ l2_1.x[1] = 1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 0.;
+ l20_1.xex[0] = 1.;
+ l20_1.xex[1] = 1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = 1. - l2_1.x[0];
+ l6_1.fx = (c__ * (d__1 * d__1) + d__3 * d__3) / c__;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[0] = (c__ * -4. * (l2_1.x[1] - d__1 * d__1) * l2_1.x[0] - (1. -
+ l2_1.x[0]) * 2.) / c__;
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[1] = c__ * 2. * (l2_1.x[1] - d__1 * d__1) / c__;
+labelL4:
+ return 0;
+} /* tp210_ */
+
+/* Subroutine */ int tp211_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = -1.2;
+ l2_1.x[1] = 1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 0.;
+ l20_1.xex[0] = 1.;
+ l20_1.xex[1] = 1.;
+ return 0;
+labelL2:
+/* Computing 3rd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - d__2 * (d__2 * d__2);
+/* Computing 2nd power */
+ d__3 = 1. - l2_1.x[0];
+ l6_1.fx = d__1 * d__1 * 100. + d__3 * d__3;
+ return 0;
+labelL3:
+/* Computing 3rd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+ l4_1.gf[0] = (l2_1.x[1] - d__1 * (d__1 * d__1)) * -200. * 3. * (d__2 *
+ d__2) - (1. - l2_1.x[0]) * 2.;
+/* Computing 3rd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[1] = (l2_1.x[1] - d__1 * (d__1 * d__1)) * 200.;
+labelL4:
+ return 0;
+} /* tp211_ */
+
+/* Subroutine */ int tp212_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 2.;
+ l2_1.x[1] = 0.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 0.;
+ l20_1.xex[0] = 0.;
+ l20_1.xex[1] = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = (l2_1.x[0] + l2_1.x[1]) * 4.;
+/* Computing 2nd power */
+ d__3 = l2_1.x[0] - 2.;
+/* Computing 2nd power */
+ d__4 = l2_1.x[1];
+/* Computing 2nd power */
+ d__2 = (l2_1.x[0] + l2_1.x[1]) * 4. + (l2_1.x[0] - l2_1.x[1]) * (d__3 *
+ d__3 + d__4 * d__4 - 1.);
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 2.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+/* Computing 2nd power */
+ d__3 = l2_1.x[0] - 2.;
+/* Computing 2nd power */
+ d__4 = l2_1.x[1];
+ l4_1.gf[0] = (l2_1.x[0] + l2_1.x[1]) * 32. + ((l2_1.x[0] + l2_1.x[1]) *
+ 4. + (l2_1.x[0] - l2_1.x[1]) * (d__1 * d__1 + d__2 * d__2 - 1.)) *
+ 2. * (d__3 * d__3 + d__4 * d__4 - 1. + 4. + (l2_1.x[0] - l2_1.x[
+ 1]) * 2. * (l2_1.x[0] - 2.));
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 2.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+/* Computing 2nd power */
+ d__3 = l2_1.x[0] - 2.;
+/* Computing 2nd power */
+ d__4 = l2_1.x[1];
+ l4_1.gf[1] = (l2_1.x[0] + l2_1.x[1]) * 32. + ((l2_1.x[0] + l2_1.x[1]) *
+ 4. + (l2_1.x[0] - l2_1.x[1]) * (d__1 * d__1 + d__2 * d__2 - 1.)) *
+ 2. * (4. - d__3 * d__3 + d__4 * d__4 - 1. + (l2_1.x[0] - l2_1.x[
+ 1]) * 2. * l2_1.x[1]);
+labelL4:
+ return 0;
+} /* tp212_ */
+
+
+/* Subroutine */ int tp213_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 3.;
+ l2_1.x[1] = 1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 0.;
+ l20_1.xex[0] = 1.;
+ l20_1.xex[1] = 1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0] - l2_1.x[1];
+/* Computing 2nd power */
+ d__3 = l2_1.x[0] - 1.;
+/* Computing 4th power */
+ d__1 = d__2 * d__2 * 10. + d__3 * d__3, d__1 *= d__1;
+ l6_1.fx = d__1 * d__1 * 1e-6;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0] - l2_1.x[1];
+/* Computing 2nd power */
+ d__3 = l2_1.x[0] - 1.;
+/* Computing 3rd power */
+ d__1 = d__2 * d__2 * 10. + d__3 * d__3;
+ l4_1.gf[0] = d__1 * (d__1 * d__1) * 4. * ((l2_1.x[0] - l2_1.x[1]) * 20. +
+ (l2_1.x[0] - 1.) * 2.) * 1e-6;
+/* Computing 2nd power */
+ d__2 = l2_1.x[0] - l2_1.x[1];
+/* Computing 2nd power */
+ d__3 = l2_1.x[0] - 1.;
+/* Computing 3rd power */
+ d__1 = d__2 * d__2 * 10. + d__3 * d__3;
+ l4_1.gf[1] = d__1 * (d__1 * d__1) * 4. * 20. * (l2_1.x[1] - l2_1.x[0]) *
+ 1e-6;
+labelL4:
+ return 0;
+} /* tp213_ */
+
+
+/* Subroutine */ int tp214_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = -1.2;
+ l2_1.x[1] = 1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 0.;
+ l20_1.xex[0] = 1.;
+ l20_1.xex[1] = 1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0] - l2_1.x[1];
+/* Computing 2nd power */
+ d__3 = l2_1.x[0] - 1.;
+ d__1 = d__2 * d__2 * 10. + d__3 * d__3;
+ l6_1.fx = pow_dd(&d__1, &c_b934) * (float)100.;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0] - l2_1.x[1];
+/* Computing 2nd power */
+ d__3 = l2_1.x[0] - 1.;
+ d__1 = d__2 * d__2 * 10. + d__3 * d__3;
+ l4_1.gf[0] = .25 / pow_dd(&d__1, &c_b949) * (l2_1.x[0] * 22. - l2_1.x[1] *
+ 20. - 2.) * (float)100.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[0] - l2_1.x[1];
+/* Computing 2nd power */
+ d__3 = l2_1.x[0] - 1.;
+ d__1 = d__2 * d__2 * 10. + d__3 * d__3;
+ l4_1.gf[1] = .25 / pow_dd(&d__1, &c_b949) * 20. * (l2_1.x[1] - l2_1.x[0])
+ * (float)100.;
+labelL4:
+ return 0;
+} /* tp214_ */
+
+
+/* Subroutine */ int tp215_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 1.;
+ l2_1.x[1] = 1.;
+ l12_1.lxu[0] = false;
+ l12_1.lxu[1] = false;
+ l11_1.lxl[0] = true;
+ l11_1.lxl[1] = false;
+ l13_1.xl[0] = 0.;
+ l5_1.gg[1] = 1.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 0.;
+ l20_1.xex[0] = 0.;
+ l20_1.xex[1] = 0.;
+ return 0;
+labelL2:
+ l6_1.fx = l2_1.x[1];
+ return 0;
+labelL3:
+ l4_1.gf[0] = 0.;
+ l4_1.gf[1] = 1.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l3_1.g[0] = l2_1.x[1] - d__1 * d__1;
+ }
+ return 0;
+labelL5:
+ if (l10_2.index2[0]) {
+ l5_1.gg[0] = l2_1.x[0] * -2.;
+ }
+ return 0;
+} /* tp215_ */
+
+
+/* Subroutine */ int tp216_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ l2_1.x[0] = -1.2;
+ l2_1.x[1] = 1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l14_1.xu[i__ - 1] = (float)10.;
+ l12_1.lxu[i__ - 1] = true;
+ l13_1.xl[i__ - 1] = (float)-3.;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = true;
+ }
+ l5_1.gg[1] = -2.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 1.;
+ l20_1.xex[0] = 2.;
+ l20_1.xex[1] = 4.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = d__2 * d__2 - l2_1.x[1];
+/* Computing 2nd power */
+ d__3 = l2_1.x[0] - 1.;
+ l6_1.fx = d__1 * d__1 * 100. + d__3 * d__3;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[0] = (d__1 * d__1 - l2_1.x[1]) * 400. * l2_1.x[0] + (l2_1.x[0] -
+ 1.) * 2.;
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[1] = (d__1 * d__1 - l2_1.x[1]) * -200.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = l2_1.x[0] * (l2_1.x[0] - 4.) - l2_1.x[1] * 2. + 12.;
+ }
+ return 0;
+labelL5:
+ if (l10_2.index2[0]) {
+ l5_1.gg[0] = l2_1.x[0] * 2. - 4.;
+ }
+ return 0;
+} /* tp216_ */
+
+
+/* Subroutine */ int tp217_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 1;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ l2_1.x[0] = 10.;
+ l2_1.x[1] = 10.;
+ l12_1.lxu[0] = false;
+ l12_1.lxu[1] = false;
+ l11_1.lxl[0] = true;
+ l11_1.lxl[1] = false;
+ l13_1.xl[0] = 0.;
+ l5_2.gg[0] = 1.;
+ l5_2.gg[2] = -2.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = -.8;
+ l20_1.xex[0] = .6;
+ l20_1.xex[1] = .8;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_1.x[1];
+ return 0;
+labelL3:
+ l4_1.gf[0] = 0.;
+ l4_1.gf[1] = -1.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = l2_1.x[0] + 1. - l2_1.x[1] * 2.;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_2.g[1] = d__1 * d__1 + d__2 * d__2 - 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+ l5_2.gg[1] = l2_1.x[0] * 2.;
+ l5_2.gg[3] = l2_1.x[1] * 2.;
+L8:
+ return 0;
+} /* tp217_ */
+
+
+/* Subroutine */ int tp218_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 9.;
+ l2_1.x[1] = 100.;
+ l12_1.lxu[0] = false;
+ l12_1.lxu[1] = false;
+ l11_1.lxl[0] = false;
+ l11_1.lxl[1] = true;
+ l13_1.xl[1] = 0.;
+ l5_1.gg[1] = 1.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 0.;
+ l20_1.xex[0] = 0.;
+ l20_1.xex[1] = 0.;
+ return 0;
+labelL2:
+ l6_1.fx = l2_1.x[1];
+ return 0;
+labelL3:
+ l4_1.gf[0] = 0.;
+ l4_1.gf[1] = 1.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l3_1.g[0] = l2_1.x[1] - d__1 * d__1;
+ }
+ return 0;
+labelL5:
+ if (l10_2.index2[0]) {
+ l5_1.gg[0] = l2_1.x[0] * -2.;
+ }
+ return 0;
+} /* tp218_ */
+
+
+/* Subroutine */ int tp219_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 2;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l2_3.x[i__ - 1] = 10.;
+ l12_3.lxu[i__ - 1] = false;
+/* L8: */
+ l11_3.lxl[i__ - 1] = false;
+ }
+ l5_6.gg[2] = 1.;
+ l5_6.gg[6] = 0.;
+ l5_6.gg[3] = -1.;
+ l5_6.gg[5] = 0.;
+ l20_6.lex = true;
+ l20_6.nex = 1;
+ l20_6.fex = -1.;
+ l20_6.xex[0] = 1.;
+ l20_6.xex[1] = 1.;
+ l20_6.xex[2] = 0.;
+ l20_6.xex[3] = 0.;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_3.x[0];
+ return 0;
+labelL3:
+ l4_3.gf[0] = -1.0;
+ l4_3.gf[1] = 0.0;
+ l4_3.gf[2] = 0.0;
+ l4_3.gf[3] = 0.0;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 3rd power */
+ d__1 = l2_3.x[0];
+/* Computing 2nd power */
+ d__2 = l2_3.x[2];
+ l3_2.g[0] = l2_3.x[1] - d__1 * (d__1 * d__1) - d__2 * d__2;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+/* Computing 2nd power */
+ d__2 = l2_3.x[3];
+ l3_2.g[1] = d__1 * d__1 - l2_3.x[1] - d__2 * d__2;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto labelL6;
+ }
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+ l5_6.gg[0] = d__1 * d__1 * -3.;
+ l5_6.gg[4] = l2_3.x[2] * -2.;
+labelL6:
+ if (! l10_3.index2[1]) {
+ goto L7;
+ }
+ l5_6.gg[1] = l2_3.x[0] * 2.;
+ l5_6.gg[7] = l2_3.x[3] * -2.;
+L7:
+ return 0;
+} /* tp219_ */
+
+
+/* Subroutine */ int tp220_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = (float)2.5e4;
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = true;
+ }
+ l13_1.xl[0] = (float)1.;
+ l13_1.xl[1] = (float)0.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 1.;
+ l20_1.xex[0] = (float)1.;
+ l20_1.xex[1] = (float)0.;
+ return 0;
+labelL2:
+ l6_1.fx = l2_1.x[0];
+ return 0;
+labelL3:
+ l4_1.gf[0] = (float)1.;
+ l4_1.gf[1] = (float)0.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 3rd power */
+ d__1 = l2_1.x[0] - (float)1.;
+ l3_1.g[0] = d__1 * (d__1 * d__1) - l2_1.x[1];
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - (float)1.;
+ l5_1.gg[0] = d__1 * d__1 * (float)3.;
+ l5_1.gg[1] = (float)-1.;
+L7:
+ return 0;
+} /* tp220_ */
+
+
+/* Subroutine */ int tp221_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = .25;
+ l12_1.lxu[i__ - 1] = true;
+ l14_1.xu[i__ - 1] = (float)1.;
+ l11_1.lxl[i__ - 1] = true;
+/* labelL6: */
+ l13_1.xl[i__ - 1] = (float)0.;
+ }
+ l5_1.gg[1] = (float)-1.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = -1.;
+ l20_1.xex[0] = 1.;
+ l20_1.xex[1] = 0.;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_1.x[0];
+ return 0;
+labelL3:
+ l4_1.gf[0] = -1.;
+ l4_1.gf[1] = 0.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 3rd power */
+ d__1 = l2_1.x[0] - 1.;
+ l3_1.g[0] = -(d__1 * (d__1 * d__1)) - l2_1.x[1];
+ }
+ return 0;
+labelL5:
+ if (l10_2.index2[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 1.;
+ l5_1.gg[0] = d__1 * d__1 * -3.;
+ }
+ return 0;
+} /* tp221_ */
+
+
+/* Subroutine */ int tp222_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 1.3;
+ l2_1.x[1] = .2;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+ l11_1.lxl[i__ - 1] = true;
+/* labelL6: */
+ l13_1.xl[i__ - 1] = 0.;
+ }
+ l5_1.gg[1] = -1.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = -1.5;
+ l20_1.xex[0] = 1.5;
+ l20_1.xex[1] = 0.;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_1.x[0];
+ return 0;
+labelL3:
+ l4_1.gf[0] = -1.;
+ l4_1.gf[1] = 0.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 3rd power */
+ d__1 = l2_1.x[0] - 1.;
+ l3_1.g[0] = .125 - d__1 * (d__1 * d__1) - l2_1.x[1];
+ }
+ return 0;
+labelL5:
+ if (l10_2.index2[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 1.;
+ l5_1.gg[0] = d__1 * d__1 * -3.;
+ }
+ return 0;
+} /* tp222_ */
+
+/* Subroutine */ int tp223_(int *mode)
+{
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = .1;
+ l2_1.x[1] = 3.3;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = true;
+ l11_1.lxl[i__ - 1] = true;
+ l14_1.xu[i__ - 1] = 10.;
+/* labelL6: */
+ l13_1.xl[i__ - 1] = 0.;
+ }
+ l14_1.xu[0] = (float)1.;
+ l5_2.gg[2] = 0.;
+ l5_2.gg[3] = 1.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = -std::log(std::log(10.));
+ l20_1.xex[0] = std::log(std::log(10.));
+ l20_1.xex[1] = 10.;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_1.x[0];
+ return 0;
+labelL3:
+ l4_1.gf[0] = -1.;
+ l4_1.gf[1] = 0.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = std::exp(std::exp(l2_1.x[0]));
+ }
+ if (l9_3.index1[1]) {
+ l3_2.g[1] = l2_1.x[1] - std::exp(std::exp(l2_1.x[0]));
+ }
+ return 0;
+labelL5:
+ if (l10_3.index2[0]) {
+ l5_2.gg[0] = std::exp(l2_1.x[0]) * std::exp(std::exp(l2_1.x[0]));
+ }
+ if (l10_3.index2[1]) {
+ l5_2.gg[1] = -std::exp(l2_1.x[0]) * std::exp(std::exp(l2_1.x[0]));
+ }
+ return 0;
+} /* tp223_ */
+
+/* Subroutine */ int tp224_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 4;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = .1;
+ l12_1.lxu[i__ - 1] = true;
+ l11_1.lxl[i__ - 1] = true;
+ l14_1.xu[i__ - 1] = 6.;
+/* labelL6: */
+ l13_1.xl[i__ - 1] = 0.;
+ }
+ l5_6.gg[0] = 1.;
+ l5_6.gg[4] = 3.;
+ l5_6.gg[1] = -1.;
+ l5_6.gg[5] = -3.;
+ l5_6.gg[2] = 1.;
+ l5_6.gg[6] = 1.;
+ l5_6.gg[3] = -1.;
+ l5_6.gg[7] = -1.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = -304.;
+ l20_1.xex[0] = 4.;
+ l20_1.xex[1] = 4.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l6_1.fx = d__1 * d__1 * 2. + d__2 * d__2 - l2_1.x[0] * 48. - l2_1.x[1] *
+ 40.;
+ return 0;
+labelL3:
+ l4_1.gf[0] = l2_1.x[0] * 4. - 48.;
+ l4_1.gf[1] = l2_1.x[1] * 2. - 40.;
+ return 0;
+labelL4:
+ if (l9_7.index1[0]) {
+ l3_6.g[0] = l2_1.x[0] + l2_1.x[1] * 3.;
+ }
+ if (l9_7.index1[1]) {
+ l3_6.g[1] = 18. - l2_1.x[0] - l2_1.x[1] * 3.;
+ }
+ if (l9_7.index1[2]) {
+ l3_6.g[2] = l2_1.x[0] + l2_1.x[1];
+ }
+ if (l9_7.index1[3]) {
+ l3_6.g[3] = 8. - l2_1.x[0] - l2_1.x[1];
+ }
+labelL5:
+ return 0;
+} /* tp224_ */
+
+/* Subroutine */ int tp225_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 5;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 3.;
+ l2_1.x[1] = 1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l5_4.gg[0] = 1.;
+ l5_4.gg[5] = 1.;
+ l5_4.gg[8] = -1.;
+ l5_4.gg[4] = -1.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 2.;
+ l20_1.xex[0] = 1.;
+ l20_1.xex[1] = 1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ return 0;
+labelL3:
+ l4_1.gf[0] = l2_1.x[0] * 2.;
+ l4_1.gf[1] = l2_1.x[1] * 2.;
+ return 0;
+labelL4:
+ if (l9_5.index1[0]) {
+ l3_4.g[0] = l2_1.x[0] + l2_1.x[1] - 1.;
+ }
+ if (l9_5.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_4.g[1] = d__1 * d__1 + d__2 * d__2 - 1.;
+ }
+ if (l9_5.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_4.g[2] = d__1 * d__1 * 9. + d__2 * d__2 - 9.;
+ }
+ if (l9_5.index1[3]) {
+ l3_4.g[3] = pow_dd(l2_1.x, &c_b305) - l2_1.x[1];
+ }
+ if (l9_5.index1[4]) {
+ l3_4.g[4] = pow_dd(&l2_1.x[1], &c_b305) - l2_1.x[0];
+ }
+ return 0;
+labelL5:
+ if (! l10_5.index2[1]) {
+ goto L7;
+ }
+ l5_4.gg[1] = l2_1.x[0] * 2.;
+ l5_4.gg[6] = l2_1.x[1] * 2.;
+L7:
+ if (! l10_5.index2[2]) {
+ goto L8;
+ }
+ l5_4.gg[2] = l2_1.x[0] * 18.;
+ l5_4.gg[7] = l2_1.x[1] * 2.;
+L8:
+ if (l10_5.index2[3]) {
+ l5_4.gg[3] = l2_1.x[0] * 2.;
+ }
+ if (l10_5.index2[4]) {
+ l5_4.gg[9] = l2_1.x[1] * 2.;
+ }
+ return 0;
+} /* tp225_ */
+
+/* Subroutine */ int tp226_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = .8;
+ l2_1.x[1] = .05;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+ l11_1.lxl[i__ - 1] = true;
+ l13_1.xl[i__ - 1] = 0.;
+/* labelL6: */
+ l20_1.xex[i__ - 1] = 1. / std::sqrt(2.);
+ }
+ l20_1.lex = true;
+ l20_1.fex = -.5;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_1.x[0] * l2_1.x[1];
+ return 0;
+labelL3:
+ l4_1.gf[0] = -l2_1.x[1];
+ l4_1.gf[1] = -l2_1.x[0];
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_2.g[0] = d__1 * d__1 + d__2 * d__2;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_2.g[1] = 1. - d__1 * d__1 - d__2 * d__2;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+ l5_2.gg[0] = l2_1.x[0] * 2.;
+ l5_2.gg[2] = l2_1.x[1] * 2.;
+L7:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+ l5_2.gg[1] = l2_1.x[0] * -2.;
+ l5_2.gg[3] = l2_1.x[1] * -2.;
+L8:
+ return 0;
+} /* tp226_ */
+
+/* Subroutine */ int tp227_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = .5;
+ l12_1.lxu[i__ - 1] = false;
+ l11_1.lxl[i__ - 1] = false;
+/* labelL6: */
+ l20_1.xex[i__ - 1] = 1.;
+ }
+ l5_2.gg[2] = 1.;
+ l5_2.gg[1] = 1.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 2.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1] - 1.;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ return 0;
+labelL3:
+ l4_1.gf[0] = (l2_1.x[0] - 2.) * 2.;
+ l4_1.gf[1] = (l2_1.x[1] - 1.) * 2.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l3_2.g[0] = -(d__1 * d__1) + l2_1.x[1];
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+ l3_2.g[1] = l2_1.x[0] - d__1 * d__1;
+ }
+ return 0;
+labelL5:
+ if (l10_3.index2[0]) {
+ l5_2.gg[0] = l2_1.x[0] * -2.;
+ }
+ if (l10_3.index2[1]) {
+ l5_2.gg[3] = l2_1.x[1] * -2.;
+ }
+ return 0;
+} /* tp227_ */
+
+/* Subroutine */ int tp228_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 1;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = 0.;
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l20_1.xex[0] = 0.;
+ l20_1.xex[1] = -3.;
+ l5_2.gg[0] = -1.;
+ l5_2.gg[2] = -1.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = -3.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l6_1.fx = d__1 * d__1 + l2_1.x[1];
+ return 0;
+labelL3:
+ l4_1.gf[0] = l2_1.x[0] * 2.;
+ l4_1.gf[1] = 1.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = -l2_1.x[0] - l2_1.x[1] + 1.;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_2.g[1] = -(d__1 * d__1 + d__2 * d__2) + 9.;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[1]) {
+ goto L7;
+ }
+ l5_2.gg[1] = l2_1.x[0] * -2.;
+ l5_2.gg[3] = l2_1.x[1] * -2.;
+L7:
+ return 0;
+} /* tp228_ */
+
+/* Subroutine */ int tp229_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = -1.2;
+ l2_1.x[1] = 1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = true;
+ l11_1.lxl[i__ - 1] = true;
+ l14_1.xu[i__ - 1] = 2.;
+ l13_1.xl[i__ - 1] = -2.;
+/* labelL6: */
+ l20_1.xex[i__ - 1] = 1.;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = 1. - l2_1.x[0];
+ l6_1.fx = d__1 * d__1 * 100. + d__3 * d__3;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[0] = l2_1.x[0] * -400. * (l2_1.x[1] - d__1 * d__1) - (1. - l2_1.x[
+ 0]) * 2.;
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[1] = (l2_1.x[1] - d__1 * d__1) * 200.;
+labelL4:
+ return 0;
+} /* tp229_ */
+
+/* Subroutine */ int tp230_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = 0.;
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l20_1.xex[0] = .5;
+ l20_1.xex[1] = .375;
+ l5_2.gg[2] = 1.;
+ l5_2.gg[3] = 1.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = .375;
+ return 0;
+labelL2:
+ l6_1.fx = l2_1.x[1];
+ return 0;
+labelL3:
+ l4_1.gf[0] = 0.;
+ l4_1.gf[1] = 1.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 3rd power */
+ d__2 = l2_1.x[0];
+ l3_2.g[0] = d__1 * d__1 * -2. + d__2 * (d__2 * d__2) + l2_1.x[1];
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = 1. - l2_1.x[0];
+/* Computing 3rd power */
+ d__2 = 1. - l2_1.x[0];
+ l3_2.g[1] = d__1 * d__1 * -2. + d__2 * (d__2 * d__2) + l2_1.x[1];
+ }
+ return 0;
+labelL5:
+ if (l10_3.index2[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l5_2.gg[0] = l2_1.x[0] * -4. + d__1 * d__1 * 3.;
+ }
+ if (l10_3.index2[1]) {
+/* Computing 2nd power */
+ d__1 = 1. - l2_1.x[0];
+ l5_2.gg[1] = (1. - l2_1.x[0]) * 4. - d__1 * d__1 * 3.;
+ }
+ return 0;
+} /* tp230_ */
+
+/* Subroutine */ int tp231_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 2;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = -1.2;
+ l2_1.x[1] = 1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l20_1.xex[i__ - 1] = 1.;
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l5_2.gg[0] = .33333333333333331;
+ l5_2.gg[2] = 1.;
+ l5_2.gg[1] = -.33333333333333331;
+ l5_2.gg[3] = 1.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = 1. - l2_1.x[0];
+ l6_1.fx = d__1 * d__1 * 100. + d__3 * d__3;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[0] = l2_1.x[0] * -400. * (l2_1.x[1] - d__1 * d__1) - (1. - l2_1.x[
+ 0]) * 2.;
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[1] = (l2_1.x[1] - d__1 * d__1) * 200.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = l2_1.x[0] / 3. + l2_1.x[1] + .1;
+ }
+ if (l9_3.index1[1]) {
+ l3_2.g[1] = -l2_1.x[0] / 3. + l2_1.x[1] + .1;
+ }
+labelL5:
+ return 0;
+} /* tp231_ */
+
+/* Subroutine */ int tp232_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+ static Real hv;
+
+ hv = std::sqrt(3.);
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 3;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 2.;
+ l2_1.x[1] = .5;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+ l11_1.lxl[i__ - 1] = true;
+/* labelL6: */
+ l13_1.xl[i__ - 1] = 0.;
+ }
+ l5_3.gg[0] = 1 / hv;
+ l5_3.gg[3] = -1.;
+ l5_3.gg[1] = 1.;
+ l5_3.gg[4] = hv;
+ l5_3.gg[2] = -1.;
+ l5_3.gg[5] = -hv;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = (float)-1.;
+ l20_1.xex[0] = 3.;
+ l20_1.xex[1] = hv;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 3.;
+/* Computing 3rd power */
+ d__2 = l2_1.x[1];
+ l6_1.fx = -(9. - d__1 * d__1) * (d__2 * (d__2 * d__2)) / (hv * 27.);
+ return 0;
+labelL3:
+ l4_1.gf[0] = (l2_1.x[0] - 3.) * 2. * pow_dd(&l2_1.x[1], &c_b1523) / (hv *
+ 27.);
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 3.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l4_1.gf[1] = -(9. - d__1 * d__1) * (d__2 * d__2) / (hv * 9.);
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = l2_1.x[0] / hv - l2_1.x[1];
+ }
+ if (l9_4.index1[1]) {
+ l3_3.g[1] = l2_1.x[0] + hv * l2_1.x[1];
+ }
+ if (l9_4.index1[2]) {
+ l3_3.g[2] = 6. - l2_1.x[0] - hv * l2_1.x[1];
+ }
+labelL5:
+ return 0;
+} /* tp232_ */
+
+/* Subroutine */ int tp233_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 1.2;
+ l2_1.x[1] = 1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+ l11_1.lxl[i__ - 1] = false;
+/* labelL6: */
+ l20_1.xex[i__ - 1] = 1.;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = 1. - l2_1.x[0];
+ l6_1.fx = d__1 * d__1 * 100. + d__3 * d__3;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[0] = l2_1.x[0] * -400. * (l2_1.x[1] - d__1 * d__1) - (1. - l2_1.x[
+ 0]) * 2.;
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[1] = (l2_1.x[1] - d__1 * d__1) * 200.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_1.g[0] = d__1 * d__1 + d__2 * d__2 - .25;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+ l5_1.gg[0] = l2_1.x[0] * 2.;
+ l5_1.gg[1] = l2_1.x[1] * 2.;
+L7:
+ return 0;
+} /* tp233_ */
+
+
+/* Subroutine */ int tp234_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = 1.;
+ l12_1.lxu[i__ - 1] = true;
+ l11_1.lxl[i__ - 1] = true;
+ l13_1.xl[i__ - 1] = .2;
+ l14_1.xu[i__ - 1] = 2.;
+/* labelL6: */
+ l20_1.xex[i__ - 1] = .2;
+ }
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = -.8;
+ return 0;
+labelL2:
+/* Computing 4th power */
+ d__1 = l2_1.x[1] - l2_1.x[0], d__1 *= d__1;
+ l6_1.fx = d__1 * d__1 - (1. - l2_1.x[0]);
+ return 0;
+labelL3:
+/* Computing 3rd power */
+ d__1 = l2_1.x[1] - l2_1.x[0];
+ l4_1.gf[0] = d__1 * (d__1 * d__1) * -4. + 1.;
+/* Computing 3rd power */
+ d__1 = l2_1.x[1] - l2_1.x[0];
+ l4_1.gf[1] = d__1 * (d__1 * d__1) * 4.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_1.g[0] = -(d__1 * d__1) - d__2 * d__2 + 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+ l5_1.gg[0] = l2_1.x[0] * -2.;
+ l5_1.gg[1] = l2_1.x[1] * -2.;
+L7:
+ return 0;
+} /* tp234_ */
+
+
+/* Subroutine */ int tp235_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ l2_2.x[0] = -2.;
+ l2_2.x[1] = 3.;
+ l2_2.x[2] = 1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_2.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_2.lxl[i__ - 1] = false;
+ }
+ l5_5.gg[0] = 1.;
+ l5_5.gg[1] = 0.;
+ l20_3.xex[0] = -1.;
+ l20_3.xex[1] = 1.;
+ l20_3.xex[2] = 0.;
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.fex = .04;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_2.x[0];
+/* Computing 2nd power */
+ d__1 = l2_2.x[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = l2_2.x[0] - 1.;
+ l6_1.fx = d__1 * d__1 + d__3 * d__3 * .01;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+ l4_2.gf[0] = l2_2.x[0] * -4. * (l2_2.x[1] - d__1 * d__1) + (l2_2.x[0] -
+ 1.) * .02;
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+ l4_2.gf[1] = (l2_2.x[1] - d__1 * d__1) * 2.;
+ l4_2.gf[2] = 0.0;
+
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[2];
+ l3_1.g[0] = l2_2.x[0] + d__1 * d__1 + 1.;
+ }
+ return 0;
+labelL5:
+ if (l10_2.index2[0]) {
+ l5_5.gg[2] = l2_2.x[2] * 2.;
+ }
+ return 0;
+} /* tp235_ */
+
+
+/* Subroutine */ int tp236239_(int *imode)
+{
+ /* Initialized data */
+
+ static Real b[20] = { 75.1963666677,-3.8112755343,.1269366345,
+ -.0020567665,1.0345e-5,-6.8306567613,.0302344793,-.0012813448,
+ 3.52559e-5,-2.266e-7,.2564581253,-.003460403,1.35139e-5,
+ -28.1064434908,-5.2375e-6,-6.3e-9,7e-10,3.405462e-4,-1.6638e-6,
+ -2.8673112392 };
+
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5, d__6, d__7, d__8, d__9, d__10,
+ d__11, d__12, d__13, d__14, d__15, d__16, d__17;
+
+
+ switch ((int)*imode) {
+ case 1: goto labelL2;
+ case 2: goto labelL3;
+ }
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 3rd power */
+ d__2 = l2_1.x[0];
+/* Computing 4th power */
+ d__3 = l2_1.x[0], d__3 *= d__3;
+/* Computing 2nd power */
+ d__4 = l2_1.x[0];
+/* Computing 3rd power */
+ d__5 = l2_1.x[0];
+/* Computing 4th power */
+ d__6 = l2_1.x[0], d__6 *= d__6;
+/* Computing 2nd power */
+ d__7 = l2_1.x[1];
+/* Computing 3rd power */
+ d__8 = l2_1.x[1];
+/* Computing 4th power */
+ d__9 = l2_1.x[1], d__9 *= d__9;
+/* Computing 2nd power */
+ d__10 = l2_1.x[0];
+/* Computing 2nd power */
+ d__11 = l2_1.x[1];
+/* Computing 3rd power */
+ d__12 = l2_1.x[0];
+/* Computing 2nd power */
+ d__13 = l2_1.x[1];
+/* Computing 3rd power */
+ d__14 = l2_1.x[0];
+/* Computing 3rd power */
+ d__15 = l2_1.x[1];
+/* Computing 2nd power */
+ d__16 = l2_1.x[1];
+/* Computing 3rd power */
+ d__17 = l2_1.x[1];
+ l6_1.fx = b[0] + b[1] * l2_1.x[0] + b[2] * (d__1 * d__1) + b[3] * (d__2 *
+ (d__2 * d__2)) + b[4] * (d__3 * d__3) + b[5] * l2_1.x[1] + b[6] *
+ l2_1.x[0] * l2_1.x[1] + b[7] * (d__4 * d__4) * l2_1.x[1] + b[8] *
+ (d__5 * (d__5 * d__5)) * l2_1.x[1] + b[9] * (d__6 * d__6) *
+ l2_1.x[1] + b[10] * (d__7 * d__7) + b[11] * (d__8 * (d__8 * d__8))
+ + b[12] * (d__9 * d__9) + b[13] * (1. / (l2_1.x[1] + 1.)) + b[14]
+ * (d__10 * d__10) * (d__11 * d__11) + b[15] * (d__12 * (d__12 *
+ d__12)) * (d__13 * d__13) + b[16] * (d__14 * (d__14 * d__14)) * (
+ d__15 * (d__15 * d__15)) + b[17] * l2_1.x[0] * (d__16 * d__16) +
+ b[18] * l2_1.x[0] * (d__17 * (d__17 * d__17)) + b[19] * std::exp(
+ l2_1.x[0] * 5e-4 * l2_1.x[1]);
+ l6_1.fx = -l6_1.fx;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 3rd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__3 = l2_1.x[0];
+/* Computing 3rd power */
+ d__4 = l2_1.x[0];
+/* Computing 2nd power */
+ d__5 = l2_1.x[1];
+/* Computing 2nd power */
+ d__6 = l2_1.x[0];
+/* Computing 2nd power */
+ d__7 = l2_1.x[1];
+/* Computing 2nd power */
+ d__8 = l2_1.x[0];
+/* Computing 3rd power */
+ d__9 = l2_1.x[1];
+/* Computing 2nd power */
+ d__10 = l2_1.x[1];
+/* Computing 3rd power */
+ d__11 = l2_1.x[1];
+ l4_1.gf[0] = b[1] + b[2] * 2. * l2_1.x[0] + b[3] * 3. * (d__1 * d__1) + b[
+ 4] * 4. * (d__2 * (d__2 * d__2)) + b[6] * l2_1.x[1] + b[7] * 2. *
+ l2_1.x[0] * l2_1.x[1] + b[8] * 3. * (d__3 * d__3) * l2_1.x[1] + b[
+ 9] * 4. * (d__4 * (d__4 * d__4)) * l2_1.x[1] + b[14] * 2. *
+ l2_1.x[0] * (d__5 * d__5) + b[15] * 3. * (d__6 * d__6) * (d__7 *
+ d__7) + b[16] * 3. * (d__8 * d__8) * (d__9 * (d__9 * d__9)) + b[
+ 17] * (d__10 * d__10) + b[18] * (d__11 * (d__11 * d__11)) + b[19]
+ * std::exp(l2_1.x[0] * 5e-4 * l2_1.x[1]) * (l2_1.x[1] * 5e-4);
+ l4_1.gf[0] = -l4_1.gf[0];
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 3rd power */
+ d__2 = l2_1.x[0];
+/* Computing 4th power */
+ d__3 = l2_1.x[0], d__3 *= d__3;
+/* Computing 2nd power */
+ d__4 = l2_1.x[1];
+/* Computing 3rd power */
+ d__5 = l2_1.x[1];
+/* Computing 2nd power */
+ d__6 = l2_1.x[1] + 1.;
+/* Computing 2nd power */
+ d__7 = l2_1.x[0];
+/* Computing 3rd power */
+ d__8 = l2_1.x[0];
+/* Computing 3rd power */
+ d__9 = l2_1.x[0];
+/* Computing 2nd power */
+ d__10 = l2_1.x[1];
+/* Computing 2nd power */
+ d__11 = l2_1.x[1];
+ l4_1.gf[1] = b[5] + b[6] * l2_1.x[0] + b[7] * (d__1 * d__1) + b[8] * (
+ d__2 * (d__2 * d__2)) + b[9] * (d__3 * d__3) + b[10] * 2. *
+ l2_1.x[1] + b[11] * 3. * (d__4 * d__4) + b[12] * 4. * (d__5 * (
+ d__5 * d__5)) + b[13] * (-1. / (d__6 * d__6)) + b[14] * (d__7 *
+ d__7) * 2 * l2_1.x[1] + b[15] * (d__8 * (d__8 * d__8)) * 2 *
+ l2_1.x[1] + b[16] * (d__9 * (d__9 * d__9)) * 3 * (d__10 * d__10)
+ + b[17] * l2_1.x[0] * 2. * l2_1.x[1] + b[18] * l2_1.x[0] * 3. * (
+ d__11 * d__11) + b[19] * std::exp(l2_1.x[0] * 5e-4 * l2_1.x[1]) * (
+ l2_1.x[0] * 5e-4);
+ l4_1.gf[1] = -l4_1.gf[1];
+ return 0;
+} /* tp236239_ */
+
+
+/* Subroutine */ int tp236_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+ int tp236239_(int *);
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 10.;
+ l2_1.x[1] = 10.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = true;
+ l11_1.lxl[i__ - 1] = true;
+/* labelL6: */
+ l13_1.xl[i__ - 1] = 0.;
+ }
+ l14_1.xu[0] = 75.;
+ l14_1.xu[1] = 65.;
+ l5_2.gg[3] = 1.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = -58.903436;
+ l20_1.xex[0] = 75.;
+ l20_1.xex[1] = 65.;
+ return 0;
+labelL2:
+ tp236239_(&c__1);
+ return 0;
+labelL3:
+ tp236239_(&c__2);
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = l2_1.x[0] * l2_1.x[1] - 700.;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] / 25.;
+ l3_2.g[1] = l2_1.x[1] - d__1 * d__1 * 5.;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+ l5_2.gg[0] = l2_1.x[1];
+ l5_2.gg[2] = l2_1.x[0];
+L7:
+ if (l10_3.index2[1]) {
+ l5_2.gg[1] = l2_1.x[0] * -.016;
+ }
+ return 0;
+} /* tp236_ */
+
+
+/* Subroutine */ int tp237_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ int tp236239_(int *);
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 3;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 10.;
+ l2_1.x[1] = 10.;
+ l12_1.lxu[0] = true;
+ l12_1.lxu[1] = true;
+ l11_1.lxl[0] = true;
+ l11_1.lxl[1] = false;
+ l14_1.xu[0] = 75.;
+ l14_1.xu[1] = 65.;
+ l13_1.xl[0] = 54.;
+ l5_3.gg[4] = 1.;
+ l5_3.gg[2] = -5.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = -58.903436;
+ l20_1.xex[0] = 75.;
+ l20_1.xex[1] = 65.;
+ return 0;
+labelL2:
+ tp236239_(&c__1);
+ return 0;
+labelL3:
+ tp236239_(&c__2);
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = l2_1.x[0] * l2_1.x[1] - 700.;
+ }
+ if (l9_4.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] / 25.;
+ l3_3.g[1] = l2_1.x[1] - d__1 * d__1 * 5.;
+ }
+ if (l9_4.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - 50.;
+ l3_3.g[2] = d__1 * d__1 - (l2_1.x[0] - 55.) * 5.;
+ }
+ return 0;
+labelL5:
+ if (! l10_4.index2[0]) {
+ goto labelL6;
+ }
+ l5_3.gg[0] = l2_1.x[1];
+ l5_3.gg[3] = l2_1.x[0];
+labelL6:
+ if (l10_4.index2[1]) {
+ l5_3.gg[1] = l2_1.x[0] * -.016;
+ }
+ if (l10_4.index2[2]) {
+ l5_3.gg[5] = l2_1.x[1] * 2. - 100.;
+ }
+ return 0;
+} /* tp237_ */
+
+
+/* Subroutine */ int tp238_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+ int tp236239_(int *);
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 3;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 10.;
+ l2_1.x[1] = 10.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = true;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l14_1.xu[0] = 75.;
+ l14_1.xu[1] = 65.;
+ l5_3.gg[4] = 1.;
+ l5_3.gg[2] = -5.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = -58.903436;
+ l20_1.xex[0] = 75.;
+ l20_1.xex[1] = 65.;
+ return 0;
+labelL2:
+ tp236239_(&c__1);
+ return 0;
+labelL3:
+ tp236239_(&c__2);
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = l2_1.x[0] * l2_1.x[1] - 700.;
+ }
+ if (l9_4.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] / 25.;
+ l3_3.g[1] = l2_1.x[1] - d__1 * d__1 * 5.;
+ }
+ if (l9_4.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - 50.;
+ l3_3.g[2] = d__1 * d__1 - (l2_1.x[0] - 55.) * 5.;
+ }
+ return 0;
+labelL5:
+ if (! l10_4.index2[0]) {
+ goto L7;
+ }
+ l5_3.gg[0] = l2_1.x[1];
+ l5_3.gg[3] = l2_1.x[0];
+L7:
+ if (l10_4.index2[1]) {
+ l5_3.gg[1] = l2_1.x[0] * -.016;
+ }
+ if (l10_4.index2[2]) {
+ l5_3.gg[5] = l2_1.x[1] * 2. - 100.;
+ }
+ return 0;
+} /* tp238_ */
+
+
+/* Subroutine */ int tp239_(int *mode)
+{
+ static int i__;
+ int tp236239_(int *);
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 10.;
+ l2_1.x[1] = 10.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = true;
+ l11_1.lxl[i__ - 1] = true;
+/* labelL6: */
+ l13_1.xl[i__ - 1] = 0.;
+ }
+ l14_1.xu[0] = 75.;
+ l14_1.xu[1] = 65.;
+ l20_1.lex = true;
+ l20_1.nex = 1;
+ l20_1.fex = (float)-58.903436;
+ l20_1.xex[0] = (float)75.;
+ l20_1.xex[1] = (float)65.;
+ return 0;
+labelL2:
+ tp236239_(&c__1);
+ return 0;
+labelL3:
+ tp236239_(&c__2);
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = l2_1.x[0] * l2_1.x[1] - 700.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+ l5_1.gg[0] = l2_1.x[1];
+ l5_1.gg[1] = l2_1.x[0];
+L7:
+ return 0;
+} /* tp239_ */
+
+
+/* Subroutine */ int tp240_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_2.x[0] = 100.;
+ l2_2.x[1] = -1.;
+ l2_2.x[2] = 2.5;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_2.lxl[i__ - 1] = false;
+ l12_2.lxu[i__ - 1] = false;
+/* labelL6: */
+ l20_3.xex[i__ - 1] = 0.;
+ }
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.fex = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0] - l2_2.x[1] + l2_2.x[2];
+/* Computing 2nd power */
+ d__2 = -l2_2.x[0] + l2_2.x[1] + l2_2.x[2];
+/* Computing 2nd power */
+ d__3 = l2_2.x[0] + l2_2.x[1] - l2_2.x[2];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3;
+labelL3:
+ l4_2.gf[0] = (l2_2.x[0] - l2_2.x[1] + l2_2.x[2]) * 2. - (l2_2.x[1] -
+ l2_2.x[0] + l2_2.x[2]) * 2. + (l2_2.x[0] + l2_2.x[1] - l2_2.x[2])
+ * 2.;
+ l4_2.gf[1] = (l2_2.x[1] - l2_2.x[0] + l2_2.x[2]) * 2. - (l2_2.x[0] -
+ l2_2.x[1] + l2_2.x[2]) * 2. + (l2_2.x[0] + l2_2.x[1] - l2_2.x[2])
+ * 2.;
+ l4_2.gf[2] = (l2_2.x[0] - l2_2.x[1] + l2_2.x[2]) * 2. + (l2_2.x[1] -
+ l2_2.x[0] + l2_2.x[2]) * 2. - (l2_2.x[0] + l2_2.x[1] - l2_2.x[2])
+ * 2.;
+labelL4:
+ return 0;
+} /* tp240_ */
+
+/* Subroutine */ int tp241_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l15_1.lsum = 5;
+ l2_2.x[0] = 1.;
+ l2_2.x[1] = 2.;
+ l2_2.x[2] = 0.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_2.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = false;
+ }
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.fex = 0.;
+ l20_3.xex[0] = 0.;
+ l20_3.xex[1] = 0.;
+ l20_3.xex[2] = 1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2];
+ l16_3.f[0] = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 - 1.;
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2] - 2.;
+ l16_3.f[1] = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 - 1.;
+ l16_3.f[2] = l2_2.x[0] + l2_2.x[1] + l2_2.x[2] - 1.;
+ l16_3.f[3] = l2_2.x[0] + l2_2.x[1] - l2_2.x[2] + 1.;
+/* Computing 3rd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2] * 5. - l2_2.x[0] + 1.;
+ l16_3.f[4] = d__1 * (d__1 * d__1) + d__2 * d__2 * 3. + d__3 * d__3 - 36.;
+ if (*mode == 3) {
+ goto labelL3;
+ }
+/* Computing 2nd power */
+ d__1 = l16_3.f[0];
+/* Computing 2nd power */
+ d__2 = l16_3.f[1];
+/* Computing 2nd power */
+ d__3 = l16_3.f[2];
+/* Computing 2nd power */
+ d__4 = l16_3.f[3];
+/* Computing 2nd power */
+ d__5 = l16_3.f[4];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 + d__4 * d__4 + d__5 *
+ d__5;
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l17_3.df[i__ * 5 - 5] = l2_2.x[i__ - 1] * 2.;
+/* L7: */
+ l17_3.df[i__ * 5 - 3] = 1.;
+ }
+ l17_3.df[1] = l17_3.df[0];
+ l17_3.df[6] = l17_3.df[5];
+ l17_3.df[11] = (l2_2.x[2] - 2.) * 2.;
+ l17_3.df[3] = 1.;
+ l17_3.df[8] = 1.;
+ l17_3.df[13] = -1.;
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+ l17_3.df[4] = d__1 * d__1 * 3. - (l2_2.x[2] * 5. - l2_2.x[0] + 1.) * 2.;
+ l17_3.df[9] = l2_2.x[1] * 6.;
+ l17_3.df[14] = (l2_2.x[2] * 5. - l2_2.x[0] + 1.) * 10.;
+ l4_2.gf[0] = 0.;
+ l4_2.gf[1] = 0.;
+ l4_2.gf[2] = 0.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l4_2.gf[0] += l16_3.f[i__ - 1] * l17_3.df[i__ - 1] * 2.;
+ l4_2.gf[1] += l16_3.f[i__ - 1] * l17_3.df[i__ + 4] * 2.;
+/* L8: */
+ l4_2.gf[2] += l16_3.f[i__ - 1] * l17_3.df[i__ + 9] * 2.;
+ }
+labelL4:
+ return 0;
+} /* tp241_ */
+
+/* Subroutine */ int tp242_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+ static Real ti;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_2.x[0] = 2.5;
+ l2_2.x[1] = 10.;
+ l2_2.x[2] = l2_2.x[1];
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_2.lxl[i__ - 1] = true;
+ l12_2.lxu[i__ - 1] = true;
+ l13_2.xl[i__ - 1] = 0.;
+/* labelL6: */
+ l14_2.xu[i__ - 1] = 10.;
+ }
+ l15_1.lsum = 10;
+ l20_3.lex = true;
+ l20_3.nex = -1;
+ l20_3.fex = 0.;
+ l20_3.xex[0] = 1.;
+ l20_3.xex[1] = 10.;
+ l20_3.xex[2] = 1.;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ ti = (Real) i__ * .1;
+/* L7: */
+ l16_4.f[i__ - 1] = std::exp(-l2_2.x[0] * ti) - std::exp(-l2_2.x[1] * ti) -
+ l2_2.x[2] * (std::exp(-ti) - std::exp(ti * -10.));
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L8: */
+/* Computing 2nd power */
+ d__1 = l16_4.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ l4_2.gf[0] = 0.;
+ l4_2.gf[1] = 0.;
+ l4_2.gf[2] = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ ti = (Real) i__ * .1;
+ l16_4.f[i__ - 1] = std::exp(-l2_2.x[0] * ti) - std::exp(-l2_2.x[1] * ti) -
+ l2_2.x[2] * (std::exp(-ti) - std::exp(ti * -10.));
+ l17_4.df[i__ - 1] = -ti * std::exp(-l2_2.x[0] * ti);
+ l17_4.df[i__ + 9] = ti * std::exp(-l2_2.x[1] * ti);
+ l17_4.df[i__ + 19] = std::exp(ti * -10.) - std::exp(-ti);
+ l4_2.gf[0] += l16_4.f[i__ - 1] * l17_4.df[i__ - 1] * 2.;
+ l4_2.gf[1] += l16_4.f[i__ - 1] * l17_4.df[i__ + 9] * 2.;
+/* labelL9: */
+ l4_2.gf[2] += l16_4.f[i__ - 1] * l17_4.df[i__ + 19] * 2.;
+ }
+labelL4:
+ return 0;
+} /* tp242_ */
+
+/* Subroutine */ int tp243_(int *mode)
+{
+ /* Initialized data */
+
+ static Real a[4] = { .14272,-.184981,-.521869,-.685306 };
+ static Real d__[4] = { 1.75168,-1.35195,-.479048,-.3648 };
+ static Real b[9] /* was [3][3] */ = { 2.95137,4.87407,-2.0506,
+ 4.87407,9.39321,-3.93185,-2.0506,-3.93189,2.64745 };
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real dxbx[3], g[12] /* was [4][3] */;
+ static int i__;
+ static Real xbx;
+
+ g[0] = -.564255;
+ g[4] = .392417;
+ g[8] = -.404979;
+ g[1] = .927589;
+ g[5] = -.0735083;
+ g[9] = .535493;
+ g[2] = .658799;
+ g[6] = -.636666;
+ g[10] = -.681091;
+ g[3] = -.869487;
+ g[7] = .586387;
+ g[11] = .289826;
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l15_1.lsum = 4;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_2.lxl[i__ - 1] = false;
+ l12_2.lxu[i__ - 1] = false;
+ l2_2.x[i__ - 1] = .1;
+/* labelL6: */
+ l20_3.xex[i__ - 1] = 0.;
+ }
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.fex = .7966;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ xbx = (l2_2.x[0] * b[0] + l2_2.x[1] * b[1] + l2_2.x[2] * b[2]) * l2_2.x[0]
+ + (l2_2.x[0] * b[3] + l2_2.x[1] * b[4] + l2_2.x[2] * b[5]) *
+ l2_2.x[1] + (l2_2.x[0] * b[6] + l2_2.x[1] * b[7] + l2_2.x[2] * b[
+ 8]) * l2_2.x[2];
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* L7: */
+ l16_5.f[i__ - 1] = a[i__ - 1] + g[i__ - 1] * l2_2.x[0] + g[i__ + 3] *
+ l2_2.x[1] + g[i__ + 7] * l2_2.x[2] + xbx * .5 * d__[i__ - 1];
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* labelL10: */
+/* Computing 2nd power */
+ d__1 = l16_5.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ dxbx[0] = (l2_2.x[0] * b[0] + l2_2.x[1] * b[1] + l2_2.x[2] * b[2]) * 2.;
+ dxbx[1] = (l2_2.x[0] * b[3] + l2_2.x[1] * b[4] + l2_2.x[2] * b[5]) * 2.;
+ dxbx[2] = (l2_2.x[0] * b[6] + l2_2.x[1] * b[7] + l2_2.x[2] * b[8]) * 2.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* labelL9: */
+ l4_2.gf[i__ - 1] = 0.;
+ }
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l17_5.df[i__ - 1] = g[i__ - 1] + dxbx[0] * d__[i__ - 1] * .5;
+ l17_5.df[i__ + 3] = g[i__ + 3] + dxbx[1] * d__[i__ - 1] * .5;
+ l17_5.df[i__ + 7] = g[i__ + 7] + dxbx[2] * d__[i__ - 1] * .5;
+ l4_2.gf[0] += l16_5.f[i__ - 1] * l17_5.df[i__ - 1] * 2.;
+ l4_2.gf[1] += l16_5.f[i__ - 1] * l17_5.df[i__ + 3] * 2.;
+/* L8: */
+ l4_2.gf[2] += l16_5.f[i__ - 1] * l17_5.df[i__ + 7] * 2.;
+ }
+labelL4:
+ return 0;
+} /* tp243_ */
+
+
+/* Subroutine */ int tp244_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+ static Real yi, zi;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l15_1.lsum = 10;
+ l20_3.nex = 1;
+ l2_2.x[0] = 1.;
+ l2_2.x[1] = 2.;
+ l2_2.x[2] = 1.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l13_2.xl[i__ - 1] = (float)0.;
+ l14_2.xu[i__ - 1] = 1e10;
+ l11_2.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = true;
+ }
+ l20_3.lex = true;
+ l20_3.fex = 0.;
+ l20_3.xex[0] = 1.;
+ l20_3.xex[1] = 10.;
+ l20_3.xex[2] = 5.;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ zi = (Real) i__ * .1;
+ yi = std::exp(-zi) - std::exp(zi * -10.) * 5.;
+/* L7: */
+ l16_4.f[i__ - 1] = std::exp(-l2_2.x[0] * zi) - l2_2.x[2] * std::exp(-l2_2.x[1] *
+ zi) - yi;
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ for (i__ = 1; i__ <= 8; ++i__) {
+/* labelL10: */
+/* Computing 2nd power */
+ d__1 = l16_4.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L8: */
+ l4_2.gf[i__ - 1] = 0.;
+ }
+ for (i__ = 1; i__ <= 10; ++i__) {
+ zi = (Real) i__ * .1;
+ yi = std::exp(-zi) - std::exp(zi * -10.) * 5.;
+ l17_4.df[i__ - 1] = -zi * std::exp(-l2_2.x[0] * zi);
+ l17_4.df[i__ + 9] = zi * l2_2.x[2] * std::exp(-l2_2.x[1] * zi);
+ l17_4.df[i__ + 19] = -exp(-l2_2.x[1] * zi);
+ l4_2.gf[0] += l16_4.f[i__ - 1] * l17_4.df[i__ - 1] * 2.;
+ l4_2.gf[1] += l16_4.f[i__ - 1] * l17_4.df[i__ + 9] * 2.;
+/* labelL9: */
+ l4_2.gf[2] += l16_4.f[i__ - 1] * l17_4.df[i__ + 19] * 2.;
+ }
+labelL4:
+ return 0;
+} /* tp244_ */
+
+
+/* Subroutine */ int tp245_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+ static Real di;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l20_3.nex = -1;
+ l2_2.x[0] = 0.;
+ l2_2.x[1] = 10.;
+ l2_2.x[2] = 20.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l13_13.xl[i__ - 1] = (float)0.;
+ l11_2.lxl[i__ - 1] = true;
+ l14_13.xu[i__ - 1] = (float)20.;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = true;
+ }
+ l14_13.xu[0] = (float)12.;
+ l14_13.xu[1] = (float)12.;
+ l20_3.lex = true;
+ l20_3.fex = 0.;
+ l20_3.xex[0] = 1.;
+ l20_3.xex[1] = 10.;
+ l20_3.xex[2] = 1.;
+ l15_1.lsum = 10;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ di = (Real) i__;
+/* L7: */
+ l16_4.f[i__ - 1] = std::exp(-di * l2_2.x[0] / 10.) - std::exp(-di * l2_2.x[1] /
+ 10.) - l2_2.x[2] * (exp(-di / 10.) - std::exp(-di));
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* labelL10: */
+/* Computing 2nd power */
+ d__1 = l16_4.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L8: */
+ l4_2.gf[i__ - 1] = 0.;
+ }
+ for (i__ = 1; i__ <= 10; ++i__) {
+ di = (Real) i__;
+ l17_4.df[i__ - 1] = -di / 10. * std::exp(-di * l2_2.x[0] / 10.);
+ l17_4.df[i__ + 9] = di / 10. * std::exp(-di * l2_2.x[1] / 10.);
+ l17_4.df[i__ + 19] = std::exp(-di) - std::exp(-di / 10.);
+ l4_2.gf[0] += l16_4.f[i__ - 1] * l17_4.df[i__ - 1] * 2.;
+ l4_2.gf[1] += l16_4.f[i__ - 1] * l17_4.df[i__ + 9] * 2.;
+/* labelL9: */
+ l4_2.gf[2] += l16_4.f[i__ - 1] * l17_4.df[i__ + 19] * 2.;
+ }
+labelL4:
+ return 0;
+} /* tp245_ */
+
+
+/* Subroutine */ int tp246_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_2.x[0] = -1.2;
+ l2_2.x[1] = 2.;
+ l2_2.x[2] = 0.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ for (j = 1; j <= 3; ++j) {
+/* L7: */
+ l17_6.df[i__ + j * 3 - 4] = 0.;
+ }
+ l11_2.lxl[i__ - 1] = false;
+ l12_2.lxu[i__ - 1] = false;
+/* labelL6: */
+ l20_3.xex[i__ - 1] = 1.;
+ }
+ l17_6.df[6] = 10.;
+ l17_6.df[1] = -1.;
+ l17_6.df[5] = -1.;
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.fex = 0.;
+ l15_1.lsum = 3;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = (l2_2.x[0] + l2_2.x[1]) / 2.;
+ l16_2.f[0] = (l2_2.x[2] - d__1 * d__1) * 10.;
+ l16_2.f[1] = 1. - l2_2.x[0];
+ l16_2.f[2] = 1. - l2_2.x[1];
+ if (*mode == 3) {
+ goto labelL3;
+ }
+/* Computing 2nd power */
+ d__1 = l16_2.f[0];
+/* Computing 2nd power */
+ d__2 = l16_2.f[1];
+/* Computing 2nd power */
+ d__3 = l16_2.f[2];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3;
+ return 0;
+labelL3:
+ l17_6.df[0] = (l2_2.x[0] + l2_2.x[1]) * -10.;
+ l17_6.df[3] = (l2_2.x[0] + l2_2.x[1]) * -10.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l4_2.gf[i__ - 1] = 0.;
+ for (j = 1; j <= 3; ++j) {
+/* L8: */
+ l4_2.gf[i__ - 1] += l16_2.f[j - 1] * 2. * l17_6.df[j + i__ * 3 -
+ 4];
+ }
+ }
+labelL4:
+ return 0;
+} /* tp246_ */
+
+
+/* Subroutine */ int tp247_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5;
+
+ /* Local variables */
+ static int i__;
+ static Real theta, dtheta[3], xpi;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l20_3.nex = 1;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_2.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = false;
+ }
+ l11_2.lxl[2] = true;
+ l12_2.lxu[2] = true;
+ l11_2.lxl[0] = true;
+ l13_2.xl[0] = .1;
+ l13_2.xl[2] = -2.5;
+ l14_2.xu[2] = 7.5;
+ l20_3.lex = true;
+ l2_2.x[0] = -1.;
+ l2_2.x[1] = 0.;
+ l2_2.x[2] = 0.;
+ l20_3.fex = 0.;
+ l20_3.xex[0] = 1.;
+ l20_3.xex[1] = 0.;
+ l20_3.xex[2] = 0.;
+ return 0;
+labelL2:
+ xpi = std::asin(1.) * 2.;
+ theta = 1. / (xpi * 2.) * std::atan(l2_2.x[1] / l2_2.x[0]);
+ if (l2_2.x[0] < 0.) {
+ theta += .5;
+ }
+/* Computing 2nd power */
+ d__1 = l2_2.x[2] - theta * 10.;
+/* Computing 2nd power */
+ d__3 = l2_2.x[0];
+/* Computing 2nd power */
+ d__4 = l2_2.x[1];
+/* Computing 2nd power */
+ d__2 = std::sqrt(d__3 * d__3 + d__4 * d__4) - 1.;
+/* Computing 2nd power */
+ d__5 = l2_2.x[2];
+ l6_1.fx = (d__1 * d__1 + d__2 * d__2) * 100. + d__5 * d__5;
+ return 0;
+labelL3:
+ xpi = std::asin(1.) * 2.;
+ theta = 1. / (xpi * 2.) * std::atan(l2_2.x[1] / l2_2.x[0]);
+/* Computing 2nd power */
+ d__1 = l2_2.x[1] / l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[0];
+ dtheta[0] = -l2_2.x[1] / ((d__1 * d__1 + 1.) * (d__2 * d__2));
+/* Computing 2nd power */
+ d__1 = l2_2.x[1] / l2_2.x[0];
+ dtheta[1] = 1. / ((d__1 * d__1 + 1.) * l2_2.x[0]);
+ dtheta[2] = 0.;
+ if (l2_2.x[0] < 0.) {
+ theta += .5;
+ }
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[0];
+/* Computing 2nd power */
+ d__4 = l2_2.x[1];
+ l4_2.gf[0] = ((l2_2.x[2] - theta * 10.) * 20. * dtheta[0] + (sqrt(d__1 *
+ d__1 + d__2 * d__2) - 1.) * 2. / std::sqrt(d__3 * d__3 + d__4 * d__4) *
+ l2_2.x[0]) * 100.;
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[0];
+/* Computing 2nd power */
+ d__4 = l2_2.x[1];
+ l4_2.gf[1] = ((l2_2.x[2] - theta * 10.) * 20. * dtheta[1] + (std::sqrt(d__1 *
+ d__1 + d__2 * d__2) - 1.) * 2. / std::sqrt(d__3 * d__3 + d__4 * d__4) *
+ l2_2.x[1]) * 100.;
+ l4_2.gf[2] = (l2_2.x[2] - theta * 10.) * 2. * 100. + l2_2.x[2] * 2.;
+labelL4:
+ return 0;
+} /* tp247_ */
+
+/* Subroutine */ int tp248_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 1;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ l2_2.x[0] = -.1;
+ l2_2.x[1] = -1.;
+ l2_2.x[2] = .1;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_2.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = false;
+ }
+ l5_3.gg[0] = 1.;
+ l5_3.gg[2] = -2.;
+ l5_3.gg[4] = 0.;
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.fex = -.8;
+ l20_3.xex[0] = .6;
+ l20_3.xex[1] = .8;
+ l20_3.xex[2] = 0.;
+ l4_2.gf[0] = 0.;
+ l4_2.gf[1] = -1.;
+ l4_2.gf[2] = 0.;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_2.x[1];
+labelL3:
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = 1. - l2_2.x[1] * 2. + l2_2.x[0];
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2];
+ l3_2.g[1] = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 - 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* labelL9: */
+ l5_3.gg[(i__ << 1) - 1] = l2_2.x[i__ - 1] * 2.;
+ }
+L8:
+ return 0;
+} /* tp248_ */
+
+/* Subroutine */ int tp249_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 1.;
+ l11_2.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = false;
+ }
+ l11_2.lxl[0] = true;
+ l5_5.gg[2] = 0.;
+ l13_2.xl[0] = 1.;
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.fex = 1.;
+ l20_3.xex[0] = 1.;
+ l20_3.xex[1] = 0.;
+ l20_3.xex[2] = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3;
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L7: */
+ l4_2.gf[i__ - 1] = l2_2.x[i__ - 1] * 2.;
+ }
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+ l3_1.g[0] = d__1 * d__1 + d__2 * d__2 - 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L8;
+ }
+ l5_5.gg[0] = l2_2.x[0] * 2.;
+ l5_5.gg[1] = l2_2.x[1] * 2.;
+L8:
+ return 0;
+} /* tp249_ */
+
+/* Subroutine */ int tp250_(int *mode)
+{
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 2;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l20_3.nex = 1;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_2.lxl[i__ - 1] = true;
+ l12_2.lxu[i__ - 1] = true;
+ l13_2.xl[i__ - 1] = 0.;
+/* labelL6: */
+ l2_2.x[i__ - 1] = 10.;
+ }
+ l14_2.xu[0] = 20.;
+ l14_2.xu[1] = 11.;
+ l14_2.xu[2] = 42.;
+ l20_3.lex = true;
+ l20_3.fex = -3300.;
+ l20_3.xex[0] = 20.;
+ l20_3.xex[1] = 11.;
+ l20_3.xex[2] = 15.;
+ l5_3.gg[0] = 1.;
+ l5_3.gg[2] = 2.;
+ l5_3.gg[4] = 2.;
+ l5_3.gg[1] = -1.;
+ l5_3.gg[3] = -2.;
+ l5_3.gg[5] = -2.;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_2.x[0] * l2_2.x[1] * l2_2.x[2];
+ return 0;
+labelL3:
+ l4_2.gf[0] = -l2_2.x[1] * l2_2.x[2];
+ l4_2.gf[1] = -l2_2.x[0] * l2_2.x[2];
+ l4_2.gf[2] = -l2_2.x[0] * l2_2.x[1];
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = l2_2.x[0] + l2_2.x[1] * 2. + l2_2.x[2] * 2.;
+ }
+ if (l9_3.index1[1]) {
+ l3_2.g[1] = 72. - l2_2.x[0] - l2_2.x[1] * 2. - l2_2.x[2] * 2.;
+ }
+labelL5:
+ return 0;
+} /* tp250_ */
+
+/* Subroutine */ int tp251_(int *mode)
+{
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 1;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 10.;
+ l11_2.lxl[i__ - 1] = true;
+ l12_2.lxu[i__ - 1] = true;
+ l13_2.xl[i__ - 1] = 0.;
+/* labelL6: */
+ l14_2.xu[i__ - 1] = 42.;
+ }
+ l5_5.gg[0] = -1.;
+ l5_5.gg[1] = -2.;
+ l5_5.gg[2] = -2.;
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.fex = -3456.;
+ l20_3.xex[0] = 24.;
+ l20_3.xex[1] = 12.;
+ l20_3.xex[2] = 12.;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_2.x[0] * l2_2.x[1] * l2_2.x[2];
+ return 0;
+labelL3:
+ l4_2.gf[0] = -l2_2.x[1] * l2_2.x[2];
+ l4_2.gf[1] = -l2_2.x[0] * l2_2.x[2];
+ l4_2.gf[2] = -l2_2.x[0] * l2_2.x[1];
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = 72. - l2_2.x[0] - l2_2.x[1] * 2. - l2_2.x[2] * 2.;
+ }
+labelL5:
+ return 0;
+} /* tp251_ */
+
+/* Subroutine */ int tp252_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ l2_2.x[0] = -1.;
+ l2_2.x[1] = 2.;
+ l2_2.x[2] = 2.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_2.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = false;
+ }
+ l12_2.lxu[0] = true;
+ l14_2.xu[0] = -1.;
+ l5_5.gg[0] = 1.;
+ l5_5.gg[1] = 0.;
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.fex = .04;
+ l20_3.xex[0] = -1.;
+ l20_3.xex[1] = 1.;
+ l20_3.xex[2] = 0.;
+ l4_2.gf[2] = 0.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0] - 1.;
+/* Computing 2nd power */
+ d__3 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1] - d__3 * d__3;
+ l6_1.fx = d__1 * d__1 * .01 + d__2 * d__2;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+ l4_2.gf[0] = (l2_2.x[0] - 1.) * .02 - (l2_2.x[1] - d__1 * d__1) * 4. *
+ l2_2.x[0];
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+ l4_2.gf[1] = (l2_2.x[1] - d__1 * d__1) * 2.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[2];
+ l3_1.g[0] = l2_2.x[0] + d__1 * d__1 + 1.;
+ }
+ return 0;
+labelL5:
+ if (l10_2.index2[0]) {
+ l5_5.gg[2] = l2_2.x[2] * 2.;
+ }
+ return 0;
+} /* tp252_ */
+
+
+/* Subroutine */ int tp253_(int *mode)
+{
+ /* Initialized data */
+
+ static Real a[24] /* was [3][8] */ = { 0.,0.,0.,10.,0.,0.,10.,
+ 10.,0.,0.,10.,0.,0.,0.,10.,10.,0.,10.,10.,10.,10.,0.,10.,10. };
+
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__, j;
+
+/* DATA ((A(I,J),J=1,8),I=1,3) */
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 1;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_2.x[0] = 0.;
+ l2_2.x[1] = 2.;
+ l2_2.x[2] = 0.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l12_2.lxu[i__ - 1] = false;
+ l11_2.lxl[i__ - 1] = true;
+/* labelL6: */
+ l13_2.xl[i__ - 1] = 0.;
+ }
+ l5_5.gg[0] = -3.;
+ l5_5.gg[1] = 0.;
+ l5_5.gg[2] = -3.;
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.fex = 69.282032;
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L7: */
+ l20_3.xex[i__ - 1] = (float).5;
+ }
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (j = 1; j <= 8; ++j) {
+/* L8: */
+/* Computing 2nd power */
+ d__1 = a[j * 3 - 3] - l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = a[j * 3 - 2] - l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = a[j * 3 - 1] - l2_2.x[2];
+ l6_1.fx += std::sqrt(d__1 * d__1 + d__2 * d__2 + d__3 * d__3);
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l4_2.gf[i__ - 1] = 0.;
+ for (j = 1; j <= 8; ++j) {
+/* labelL9: */
+/* Computing 2nd power */
+ d__1 = a[j * 3 - 3] - l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = a[j * 3 - 2] - l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = a[j * 3 - 1] - l2_2.x[2];
+ l4_2.gf[i__ - 1] += (l2_2.x[i__ - 1] - a[i__ + j * 3 - 4]) / std::sqrt(
+ d__1 * d__1 + d__2 * d__2 + d__3 * d__3);
+ }
+ }
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = 30. - l2_2.x[0] * 3. - l2_2.x[2] * 3.;
+ }
+labelL5:
+ return 0;
+} /* tp253_ */
+
+
+/* Subroutine */ int tp254_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+ Real d_lg10(Real *);
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 2;
+ l2_2.x[0] = 1.;
+ l2_2.x[1] = 1.;
+ l2_2.x[2] = 1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_2.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_2.lxl[i__ - 1] = false;
+ }
+ l12_2.lxu[2] = false;
+ l11_2.lxl[2] = true;
+ l13_2.xl[2] = 1.;
+ l5_3.gg[0] = 0.;
+ l5_3.gg[3] = 0.;
+ l5_3.gg[5] = 1.;
+ l20_3.lex = true;
+ l20_3.nex = 1;
+ l20_3.fex = -std::sqrt(3.);
+ l20_3.xex[0] = 0.;
+ l20_3.xex[1] = std::sqrt(3.);
+ l20_3.xex[2] = 1.;
+ l4_2.gf[0] = 0.;
+ l4_2.gf[1] = -1.;
+ return 0;
+labelL2:
+ l6_1.fx = d_lg10(&l2_2.x[2]) - l2_2.x[1];
+ return 0;
+labelL3:
+ l4_2.gf[2] = 1. / (l2_2.x[2] * std::log(10.));
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[1];
+/* Computing 2nd power */
+ d__2 = l2_2.x[2];
+ l3_2.g[0] = d__1 * d__1 + d__2 * d__2 - 4.;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+ l3_2.g[1] = l2_2.x[2] - 1. - d__1 * d__1;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+ l5_3.gg[2] = l2_2.x[1] * 2.;
+ l5_3.gg[4] = l2_2.x[2] * 2.;
+L7:
+ if (l10_3.index2[1]) {
+ l5_3.gg[1] = l2_2.x[0] * -2.;
+ }
+ return 0;
+} /* tp254_ */
+
+
+/* Subroutine */ int tp255_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5, d__6;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_3.x[0] = -3.;
+ l2_3.x[1] = 1.;
+ l2_3.x[2] = -3.;
+ l2_3.x[3] = 1.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l14_3.xu[i__ - 1] = (float)10.;
+ l12_3.lxu[i__ - 1] = true;
+ l13_3.xl[i__ - 1] = (float)-10.;
+/* labelL6: */
+ l11_3.lxl[i__ - 1] = true;
+ }
+ l20_6.lex = true;
+ l20_6.nex = 1;
+ l20_6.fex = 0.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* L7: */
+ l20_6.xex[i__ - 1] = 1.;
+ }
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+/* Computing 2nd power */
+ d__2 = (float)1. - l2_3.x[0];
+/* Computing 2nd power */
+ d__3 = l2_3.x[2];
+/* Computing 2nd power */
+ d__4 = (float)1. - l2_3.x[2];
+/* Computing 2nd power */
+ d__5 = l2_3.x[1] - (float)1.;
+/* Computing 2nd power */
+ d__6 = l2_3.x[3] - (float)1.;
+ l6_1.fx = (l2_3.x[1] - d__1 * d__1) * (float)100. + d__2 * d__2 + (l2_3.x[
+ 3] - d__3 * d__3) * (float)90. + d__4 * d__4 + (d__5 * d__5 +
+ d__6 * d__6) * (float)10.1 + (l2_3.x[1] - (float)1.) * (float)
+ 19.8 * (l2_3.x[3] - (float)1.);
+/* Computing 2nd power */
+ d__1 = l6_1.fx;
+ l6_1.fx = d__1 * d__1 * (float).5;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+/* Computing 2nd power */
+ d__2 = (float)1. - l2_3.x[0];
+/* Computing 2nd power */
+ d__3 = l2_3.x[2];
+/* Computing 2nd power */
+ d__4 = (float)1. - l2_3.x[2];
+/* Computing 2nd power */
+ d__5 = l2_3.x[1] - (float)1.;
+/* Computing 2nd power */
+ d__6 = l2_3.x[3] - (float)1.;
+ l6_1.fx = (l2_3.x[1] - d__1 * d__1) * (float)100. + d__2 * d__2 + (l2_3.x[
+ 3] - d__3 * d__3) * (float)90. + d__4 * d__4 + (d__5 * d__5 +
+ d__6 * d__6) * (float)10.1 + (l2_3.x[1] - (float)1.) * (float)
+ 19.8 * (l2_3.x[3] - (float)1.);
+ l4_3.gf[0] = l6_1.fx * (l2_3.x[0] * -198. - 2.);
+ l4_3.gf[1] = l6_1.fx * (l2_3.x[1] * 20.2 + l2_3.x[3] * 19.8 + 60.);
+ l4_3.gf[2] = l6_1.fx * (l2_3.x[2] * -178. - 2.);
+ l4_3.gf[3] = l6_1.fx * (l2_3.x[1] * 19.8 + l2_3.x[3] * 20.2 + 50.);
+labelL4:
+ return 0;
+} /* tp255_ */
+
+
+/* Subroutine */ int tp256_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_3.x[0] = 3.;
+ l2_3.x[1] = -1.;
+ l2_3.x[2] = 0.;
+ l2_3.x[3] = 1.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l12_3.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_3.lxl[i__ - 1] = false;
+ }
+ l20_6.lex = true;
+ l20_6.nex = 1;
+ l20_6.fex = 0.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* L7: */
+ l20_6.xex[i__ - 1] = 0.;
+ }
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_3.x[0] + l2_3.x[1] * 10.;
+/* Computing 2nd power */
+ d__2 = l2_3.x[2] - l2_3.x[3];
+/* Computing 4th power */
+ d__3 = l2_3.x[1] - l2_3.x[2] * 2., d__3 *= d__3;
+/* Computing 4th power */
+ d__4 = l2_3.x[0] - l2_3.x[3], d__4 *= d__4;
+ l6_1.fx = (d__1 * d__1 + d__2 * d__2 * 5. + d__3 * d__3 + d__4 * d__4 *
+ 10.) * 1e-4;
+ return 0;
+labelL3:
+/* Computing 3rd power */
+ d__1 = l2_3.x[0] - l2_3.x[3];
+ l4_3.gf[0] = ((l2_3.x[0] + l2_3.x[1] * 10.) * 2. + d__1 * (d__1 * d__1) *
+ 40.) * 1e-4;
+/* Computing 3rd power */
+ d__1 = l2_3.x[1] - l2_3.x[2] * 2.;
+ l4_3.gf[1] = ((l2_3.x[0] + l2_3.x[1] * 10.) * 20. + d__1 * (d__1 * d__1) *
+ 4.) * 1e-4;
+/* Computing 3rd power */
+ d__1 = l2_3.x[1] - l2_3.x[2] * 2.;
+ l4_3.gf[2] = ((l2_3.x[2] - l2_3.x[3]) * 10. - d__1 * (d__1 * d__1) * 8.) *
+ 1e-4;
+/* Computing 3rd power */
+ d__1 = l2_3.x[0] - l2_3.x[3];
+ l4_3.gf[3] = ((l2_3.x[2] - l2_3.x[3]) * -10. - d__1 * (d__1 * d__1) * 40.)
+ * 1e-4;
+labelL4:
+ return 0;
+} /* tp256_ */
+
+
+/* Subroutine */ int tp257_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5, d__6, d__7, d__8;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_3.x[0] = -3.;
+ l2_3.x[1] = -1.;
+ l2_3.x[2] = -3.;
+ l2_3.x[3] = -1.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* labelL6: */
+ l12_3.lxu[i__ - 1] = false;
+ }
+ l11_3.lxl[0] = true;
+ l11_3.lxl[1] = false;
+ l11_3.lxl[2] = true;
+ l11_3.lxl[3] = false;
+ l13_3.xl[0] = 0.;
+ l13_3.xl[2] = 0.;
+ l20_6.lex = true;
+ l20_6.nex = 1;
+ l20_6.fex = 0.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* L7: */
+ l20_6.xex[i__ - 1] = 1.;
+ }
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_3.x[0];
+/* Computing 2nd power */
+ d__1 = d__2 * d__2 - l2_3.x[1];
+/* Computing 2nd power */
+ d__3 = l2_3.x[0] - 1.;
+/* Computing 2nd power */
+ d__5 = l2_3.x[2];
+/* Computing 2nd power */
+ d__4 = d__5 * d__5 - l2_3.x[3];
+/* Computing 2nd power */
+ d__6 = l2_3.x[2] - 1.;
+/* Computing 2nd power */
+ d__7 = l2_3.x[1] - 1.;
+/* Computing 2nd power */
+ d__8 = l2_3.x[3] - 1.;
+ l6_1.fx = d__1 * d__1 * 100. + d__3 * d__3 + d__4 * d__4 * 90. + d__6 *
+ d__6 + (d__7 * d__7 + d__8 * d__8) * 10.1 + (l2_3.x[0] - 1.) *
+ 19.8 * (l2_3.x[3] - 1.);
+ return 0;
+labelL3:
+/* Computing 3rd power */
+ d__1 = l2_3.x[0];
+ l4_3.gf[0] = (d__1 * (d__1 * d__1) - l2_3.x[0] * l2_3.x[1]) * 400. +
+ l2_3.x[0] * 2. + l2_3.x[3] * 19.8 - 21.8;
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+ l4_3.gf[1] = d__1 * d__1 * -200. + l2_3.x[1] * 220.2 - 20.2;
+/* Computing 3rd power */
+ d__1 = l2_3.x[2];
+ l4_3.gf[2] = (d__1 * (d__1 * d__1) - l2_3.x[2] * l2_3.x[3]) * 360. +
+ l2_3.x[2] * 2. - 2.;
+/* Computing 2nd power */
+ d__1 = l2_3.x[2];
+ l4_3.gf[3] = d__1 * d__1 * -180. + l2_3.x[3] * 200.2 + l2_3.x[0] * 19.8 -
+ 40.;
+labelL4:
+ return 0;
+} /* tp257_ */
+
+
+/* Subroutine */ int tp258_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5, d__6, d__7, d__8;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_3.x[0] = -3.;
+ l2_3.x[1] = -1.;
+ l2_3.x[2] = -3.;
+ l2_3.x[3] = -1.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l12_3.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_3.lxl[i__ - 1] = false;
+ }
+ l20_6.lex = true;
+ l20_6.nex = 1;
+ l20_6.fex = 0.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* L7: */
+ l20_6.xex[i__ - 1] = 1.;
+ }
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_3.x[0];
+/* Computing 2nd power */
+ d__1 = l2_3.x[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = 1. - l2_3.x[0];
+/* Computing 2nd power */
+ d__5 = l2_3.x[2];
+/* Computing 2nd power */
+ d__4 = l2_3.x[3] - d__5 * d__5;
+/* Computing 2nd power */
+ d__6 = 1. - l2_3.x[2];
+/* Computing 2nd power */
+ d__7 = l2_3.x[1] - 1.;
+/* Computing 2nd power */
+ d__8 = l2_3.x[3] - 1.;
+ l6_1.fx = (d__1 * d__1 * 100. + d__3 * d__3 + d__4 * d__4 * 90. + d__6 *
+ d__6 + (d__7 * d__7 + d__8 * d__8) * 10.1 + (l2_3.x[1] - 1.) *
+ 19.8 * (l2_3.x[3] - 1.)) * 1e-4;
+ return 0;
+labelL3:
+/* Computing 3rd power */
+ d__1 = l2_3.x[0];
+ l4_3.gf[0] = ((d__1 * (d__1 * d__1) - l2_3.x[0] * l2_3.x[1]) * 400. +
+ l2_3.x[0] * 2. - 2.) * 1e-4;
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+ l4_3.gf[1] = (d__1 * d__1 * -200. + l2_3.x[1] * 220.2 + l2_3.x[3] * 19.8
+ - 40.) * 1e-4;
+/* Computing 3rd power */
+ d__1 = l2_3.x[2];
+ l4_3.gf[2] = ((d__1 * (d__1 * d__1) - l2_3.x[2] * l2_3.x[3]) * 360. +
+ l2_3.x[2] * 2. - 2.) * 1e-4;
+/* Computing 2nd power */
+ d__1 = l2_3.x[2];
+ l4_3.gf[3] = (d__1 * d__1 * -180. + l2_3.x[3] * 200.2 + l2_3.x[1] * 19.8
+ - 40.) * 1e-4;
+labelL4:
+ return 0;
+} /* tp258_ */
+
+
+/* Subroutine */ int tp259_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5, d__6, d__7, d__8;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l2_3.x[i__ - 1] = 0.;
+ l12_3.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_3.lxl[i__ - 1] = false;
+ }
+ l12_3.lxu[3] = true;
+ l14_3.xu[3] = 1.;
+ l20_6.lex = true;
+ l20_6.nex = 1;
+ l20_6.fex = 0.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* L7: */
+ l20_6.xex[i__ - 1] = 1.;
+ }
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_3.x[0];
+/* Computing 2nd power */
+ d__1 = l2_3.x[1] - d__2 * d__2;
+/* Computing 2nd power */
+ d__3 = 1. - l2_3.x[0];
+/* Computing 2nd power */
+ d__5 = l2_3.x[2];
+/* Computing 2nd power */
+ d__4 = l2_3.x[3] - d__5 * d__5;
+/* Computing 3rd power */
+ d__6 = 1. - l2_3.x[2];
+/* Computing 2nd power */
+ d__7 = l2_3.x[1] - 1.;
+/* Computing 2nd power */
+ d__8 = l2_3.x[3] - 1.;
+ l6_1.fx = d__1 * d__1 * 100. + d__3 * d__3 + d__4 * d__4 * 90. + d__6 * (
+ d__6 * d__6) + d__7 * d__7 * 10.1 + d__8 * d__8 + (l2_3.x[1] - 1.)
+ * 19.8 * (l2_3.x[3] - 1.);
+ return 0;
+labelL3:
+/* Computing 3rd power */
+ d__1 = l2_3.x[0];
+ l4_3.gf[0] = (d__1 * (d__1 * d__1) - l2_3.x[0] * l2_3.x[1]) * 400. +
+ l2_3.x[0] * 2. - 2.;
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+ l4_3.gf[1] = d__1 * d__1 * -200. + l2_3.x[1] * 220.2 + l2_3.x[3] * 19.8 -
+ 40.;
+/* Computing 3rd power */
+ d__1 = l2_3.x[2];
+/* Computing 2nd power */
+ d__2 = 1. - l2_3.x[2];
+ l4_3.gf[2] = (d__1 * (d__1 * d__1) - l2_3.x[2] * l2_3.x[3]) * 360. - d__2
+ * d__2 * 3.;
+/* Computing 2nd power */
+ d__1 = l2_3.x[2];
+ l4_3.gf[3] = d__1 * d__1 * -180. + l2_3.x[3] * 182. + l2_3.x[1] * 19.8 -
+ 21.8;
+labelL4:
+ return 0;
+} /* tp259_ */
+
+
+/* Subroutine */ int tp260_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_3.x[0] = -3.;
+ l2_3.x[1] = -1.;
+ l2_3.x[2] = -3.;
+ l2_3.x[3] = -1.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l12_3.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_3.lxl[i__ - 1] = false;
+ }
+ l20_6.lex = true;
+ l20_6.nex = 1;
+ l20_6.fex = 0.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* L7: */
+ l20_6.xex[i__ - 1] = 1.;
+ }
+ l15_1.lsum = 7;
+ for (i__ = 1; i__ <= 7; ++i__) {
+ for (j = 1; j <= 4; ++j) {
+/* L8: */
+ l17_7.df[i__ + j * 7 - 8] = 0.;
+ }
+ }
+ l17_7.df[7] = 10.;
+ l17_7.df[1] = -1.;
+ l17_7.df[23] = std::sqrt(90.);
+ l17_7.df[17] = -1.;
+ l17_7.df[11] = std::sqrt(9.9);
+ l17_7.df[25] = std::sqrt(9.9);
+ l17_7.df[12] = std::sqrt(.2);
+ l17_7.df[27] = std::sqrt(.2);
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+ l16_6.f[0] = (l2_3.x[1] - d__1 * d__1) * 10.;
+ l16_6.f[1] = 1. - l2_3.x[0];
+/* Computing 2nd power */
+ d__1 = l2_3.x[2];
+ l16_6.f[2] = std::sqrt(90.) * (l2_3.x[3] - d__1 * d__1);
+ l16_6.f[3] = 1. - l2_3.x[2];
+ l16_6.f[4] = std::sqrt(9.9) * (l2_3.x[1] - 1. + (l2_3.x[3] - 1.));
+ l16_6.f[5] = std::sqrt(.2) * (l2_3.x[1] - 1.);
+ l16_6.f[6] = std::sqrt(.2) * (l2_3.x[3] - 1.);
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 7; ++i__) {
+/* labelL9: */
+/* Computing 2nd power */
+ d__1 = l16_6.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ l6_1.fx *= 1e-4;
+ return 0;
+labelL3:
+ l17_7.df[0] = l2_3.x[0] * -20. * 1e-4;
+ l17_7.df[16] = -std::sqrt(90.) * (float)2. * l2_3.x[2] * 1e-4;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l4_3.gf[i__ - 1] = 0.;
+ for (j = 1; j <= 7; ++j) {
+/* labelL10: */
+ l4_3.gf[i__ - 1] += l16_6.f[j - 1] * 2. * l17_7.df[j + i__ * 7 -
+ 8];
+ }
+ }
+labelL4:
+ return 0;
+} /* tp260_ */
+
+
+/* Subroutine */ int tp261_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static Real a, b, c__;
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 4;
+ l15_1.lsum = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l13_14.xl[i__ - 1] = (float)0.;
+ l11_3.lxl[i__ - 1] = true;
+ l14_14.xu[i__ - 1] = (float)1e5;
+ l12_3.lxu[i__ - 1] = true;
+ l2_3.x[i__ - 1] = 0.;
+ for (j = 1; j <= 5; ++j) {
+/* labelL6: */
+ l17_8.df[j + i__ * 5 - 6] = 0.;
+ }
+ }
+ l17_8.df[19] = 1.;
+ l20_6.lex = true;
+ l20_6.nex = 1;
+ l20_6.fex = 0.;
+ l20_6.xex[0] = 0.;
+ for (i__ = 2; i__ <= 4; ++i__) {
+/* L7: */
+ l20_6.xex[i__ - 1] = 1.;
+ }
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = std::exp(l2_3.x[0]) - l2_3.x[1];
+ l16_3.f[0] = d__1 * d__1;
+/* Computing 3rd power */
+ d__1 = l2_3.x[1] - l2_3.x[2];
+ l16_3.f[1] = d__1 * (d__1 * d__1) * 10.;
+/* Computing 2nd power */
+ d__1 = std::tan(l2_3.x[2] - l2_3.x[3]);
+ l16_3.f[2] = d__1 * d__1;
+/* Computing 4th power */
+ d__1 = l2_3.x[0], d__1 *= d__1;
+ l16_3.f[3] = d__1 * d__1;
+ l16_3.f[4] = l2_3.x[3] - 1.;
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L8: */
+/* Computing 2nd power */
+ d__1 = l16_3.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ a = std::exp(l2_3.x[0]) - l2_3.x[1];
+ b = std::tan(l2_3.x[2] - l2_3.x[3]);
+/* Computing 2nd power */
+ d__1 =std::cos(l2_3.x[2] - l2_3.x[3]);
+ c__ = b / (d__1 * d__1);
+ l17_8.df[0] = std::exp(l2_3.x[0]) * 2. * a;
+ l17_8.df[5] = a * -2.;
+/* Computing 2nd power */
+ d__1 = l2_3.x[1] - l2_3.x[2];
+ l17_8.df[6] = d__1 * d__1 * 30.;
+ l17_8.df[11] = -l17_8.df[6];
+ l17_8.df[12] = c__ * 2.;
+ l17_8.df[17] = -l17_8.df[12];
+/* Computing 3rd power */
+ d__1 = l2_3.x[0];
+ l17_8.df[3] = d__1 * (d__1 * d__1) * 4.;
+/* Computing 3rd power */
+ d__1 = a;
+/* Computing 7th power */
+ d__2 = l2_3.x[0], d__3 = d__2, d__2 *= d__2, d__3 *= d__2;
+ l4_3.gf[0] = std::exp(l2_3.x[0]) * 4. * (d__1 * (d__1 * d__1)) + d__3 * (d__2 *
+ d__2) * 8.;
+/* Computing 3rd power */
+ d__1 = a;
+/* Computing 5th power */
+ d__2 = l2_3.x[1] - l2_3.x[2], d__3 = d__2, d__2 *= d__2;
+ l4_3.gf[1] = d__1 * (d__1 * d__1) * -4. + d__3 * (d__2 * d__2) * 600.;
+/* Computing 5th power */
+ d__1 = l2_3.x[1] - l2_3.x[2], d__2 = d__1, d__1 *= d__1;
+ l4_3.gf[2] = b * 4. * b * c__ - d__2 * (d__1 * d__1) * 600.;
+ l4_3.gf[3] = b * -4. * b * c__ + (l2_3.x[3] - 1.) * 2.;
+labelL4:
+ return 0;
+} /* tp261_ */
+
+
+/* Subroutine */ int tp262_(int *mode)
+{
+ /* Initialized data */
+
+ static Real hgg[16] /* was [4][4] */ = { -1.,-.2,-2.,1.,-1.,-.5,
+ -1.,1.,-1.,-1.,-.5,1.,-1.,-2.,-.2,-2. };
+
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL3;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 3;
+ l1_1.ninl = 0;
+ l1_1.neli = 1;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l2_3.x[i__ - 1] = 1.;
+ l11_3.lxl[i__ - 1] = true;
+ l12_3.lxu[i__ - 1] = false;
+ l13_3.xl[i__ - 1] = 0.;
+ for (j = 1; j <= 4; ++j) {
+ l5_28.gg[i__ + (j << 2) - 5] = hgg[i__ + (j << 2) - 5];
+/* labelL6: */
+ }
+ }
+ for (i__ = 1; i__ <= 3; i__ += 2) {
+ l4_3.gf[i__ - 1] = -.5;
+/* L7: */
+ l4_3.gf[i__] = -1.;
+ }
+ l20_6.lex = true;
+ l20_6.nex = 1;
+ l20_6.fex = -10.;
+ l20_6.xex[0] = 0.;
+ l20_6.xex[1] = 8.6666666666666661;
+ l20_6.xex[2] = 0.;
+ l20_6.xex[3] = 1.3333333333333333;
+ return 0;
+labelL2:
+ l6_1.fx = l2_3.x[0] * -.5 - l2_3.x[1] - l2_3.x[2] * .5 - l2_3.x[3];
+labelL3:
+ return 0;
+labelL4:
+ if (l9_7.index1[0]) {
+ l3_6.g[0] = 10. - l2_3.x[0] - l2_3.x[1] - l2_3.x[2] - l2_3.x[3];
+ }
+ if (l9_7.index1[1]) {
+ l3_6.g[1] = 10. - l2_3.x[0] * .2 - l2_3.x[1] * .5 - l2_3.x[2] -
+ l2_3.x[3] * 2.;
+ }
+ if (l9_7.index1[2]) {
+ l3_6.g[2] = 10. - l2_3.x[0] * 2. - l2_3.x[1] - l2_3.x[2] * .5 -
+ l2_3.x[3] * .2;
+ }
+ if (l9_7.index1[3]) {
+ l3_6.g[3] = l2_3.x[0] + l2_3.x[1] + l2_3.x[2] - l2_3.x[3] * 2. - 6.;
+ }
+ return 0;
+} /* tp262_ */
+
+
+/* Subroutine */ int tp263_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 2;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l2_3.x[i__ - 1] = 10.;
+ l11_3.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_3.lxu[i__ - 1] = false;
+ }
+ l5_28.gg[4] = 1.;
+ l5_28.gg[8] = 0.;
+ l5_28.gg[5] = -1.;
+ l5_28.gg[9] = 0.;
+ l5_28.gg[6] = 1.;
+ l5_28.gg[7] = -1.;
+ l5_28.gg[11] = 0.;
+ l4_3.gf[0] = -1.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l5_28.gg[i__ + 11] = 0.;
+/* L7: */
+ l4_3.gf[i__] = 0.;
+ }
+ l20_6.lex = true;
+ l20_6.nex = 1;
+ l20_6.fex = -1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l20_6.xex[i__ - 1] = 1.;
+/* L8: */
+ l20_6.xex[i__ + 1] = 0.;
+ }
+ return 0;
+labelL2:
+ l6_1.fx = -l2_3.x[0];
+labelL3:
+ return 0;
+labelL4:
+ if (l9_7.index1[0]) {
+/* Computing 3rd power */
+ d__1 = l2_3.x[0];
+ l3_6.g[0] = l2_3.x[1] - d__1 * (d__1 * d__1);
+ }
+ if (l9_7.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+ l3_6.g[1] = d__1 * d__1 - l2_3.x[1];
+ }
+ if (l9_7.index1[2]) {
+/* Computing 3rd power */
+ d__1 = l2_3.x[0];
+/* Computing 2nd power */
+ d__2 = l2_3.x[2];
+ l3_6.g[2] = l2_3.x[1] - d__1 * (d__1 * d__1) - d__2 * d__2;
+ }
+ if (l9_7.index1[3]) {
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+/* Computing 2nd power */
+ d__2 = l2_3.x[3];
+ l3_6.g[3] = d__1 * d__1 - l2_3.x[1] - d__2 * d__2;
+ }
+ return 0;
+labelL5:
+ if (l10_7.index2[0]) {
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+ l5_28.gg[0] = d__1 * d__1 * -3.;
+ }
+ if (l10_7.index2[1]) {
+ l5_28.gg[1] = l2_3.x[0] * 2.;
+ }
+ if (! l10_7.index2[2]) {
+ goto labelL9;
+ }
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+ l5_28.gg[2] = d__1 * d__1 * -3.;
+ l5_28.gg[10] = l2_3.x[2] * -2.;
+labelL9:
+ if (! l10_7.index2[3]) {
+ goto labelL10;
+ }
+ l5_28.gg[3] = l2_3.x[0] * 2.;
+ l5_28.gg[15] = l2_3.x[3] * -2.;
+labelL10:
+ return 0;
+} /* tp263_ */
+
+
+/* Subroutine */ int tp264_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 3;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l2_3.x[i__ - 1] = 0.;
+ l11_3.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_3.lxu[i__ - 1] = false;
+ }
+ l5_7.gg[11] = 1.;
+ l20_6.lex = true;
+ l20_6.nex = 1;
+ l20_6.fex = -.44;
+ l20_6.xex[0] = 0.;
+ l20_6.xex[1] = 1.;
+ l20_6.xex[2] = 2.;
+ l20_6.xex[3] = -1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+/* Computing 2nd power */
+ d__2 = l2_3.x[1];
+/* Computing 2nd power */
+ d__3 = l2_3.x[2];
+/* Computing 2nd power */
+ d__4 = l2_3.x[3];
+ l6_1.fx = (d__1 * d__1 + d__2 * d__2 + d__3 * d__3 * 2. + d__4 * d__4 -
+ l2_3.x[0] * 5. - l2_3.x[1] * 5. - l2_3.x[2] * 21. + l2_3.x[3] *
+ 7.) * (float).01;
+ return 0;
+labelL3:
+ l4_3.gf[0] = (l2_3.x[0] * 2. - 5.) * (float).01;
+ l4_3.gf[1] = (l2_3.x[1] * 2. - 5.) * (float).01;
+ l4_3.gf[2] = (l2_3.x[2] * 4. - 21.) * (float).01;
+ l4_3.gf[3] = (l2_3.x[3] * 2. + 7.) * (float).01;
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+/* Computing 2nd power */
+ d__2 = l2_3.x[1];
+/* Computing 2nd power */
+ d__3 = l2_3.x[2];
+/* Computing 2nd power */
+ d__4 = l2_3.x[3];
+ l3_3.g[0] = 8. - d__1 * d__1 - d__2 * d__2 - d__3 * d__3 - d__4 *
+ d__4 - l2_3.x[0] + l2_3.x[1] - l2_3.x[2] + l2_3.x[3];
+ }
+ if (l9_4.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+/* Computing 2nd power */
+ d__2 = l2_3.x[1];
+/* Computing 2nd power */
+ d__3 = l2_3.x[2];
+/* Computing 2nd power */
+ d__4 = l2_3.x[3];
+ l3_3.g[1] = 9. - d__1 * d__1 - d__2 * d__2 * 2. - d__3 * d__3 - d__4 *
+ d__4 * 2. + l2_3.x[0] + l2_3.x[3];
+ }
+ if (l9_4.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+/* Computing 2nd power */
+ d__2 = l2_3.x[1];
+/* Computing 2nd power */
+ d__3 = l2_3.x[2];
+ l3_3.g[2] = 5. - d__1 * d__1 * 2. - d__2 * d__2 - d__3 * d__3 -
+ l2_3.x[0] * 2. + l2_3.x[1] + l2_3.x[3];
+ }
+ return 0;
+labelL5:
+ if (! l10_4.index2[0]) {
+ goto labelL9;
+ }
+ l5_7.gg[0] = -1. - l2_3.x[0] * 2.;
+ l5_7.gg[3] = 1. - l2_3.x[1] * 2.;
+ l5_7.gg[6] = -1. - l2_3.x[2] * 2.;
+ l5_7.gg[9] = 1. - l2_3.x[3] * 2.;
+labelL9:
+ if (! l10_4.index2[1]) {
+ goto labelL10;
+ }
+ l5_7.gg[1] = 1. - l2_3.x[0] * 2.;
+ l5_7.gg[4] = l2_3.x[1] * -4.;
+ l5_7.gg[7] = l2_3.x[2] * -2.;
+ l5_7.gg[10] = -1. - l2_3.x[3] * 4.;
+labelL10:
+ if (! l10_4.index2[2]) {
+ goto labelL11;
+ }
+ l5_7.gg[2] = -2. - l2_3.x[0] * 4.;
+ l5_7.gg[5] = 1. - l2_3.x[1] * 2.;
+ l5_7.gg[8] = l2_3.x[2] * -2.;
+labelL11:
+ return 0;
+} /* tp264_ */
+
+
+/* Subroutine */ int tp265_(int *mode)
+{
+ /* Initialized data */
+
+ static Real hgg[8] /* was [2][4] */ = { 1.,0.,1.,0.,0.,1.,0.,1. }
+ ;
+
+ /* Local variables */
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 2;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l2_3.x[i__ - 1] = 0.;
+ l11_3.lxl[i__ - 1] = true;
+ l13_3.xl[i__ - 1] = 0.;
+ l12_3.lxu[i__ - 1] = false;
+ l14_3.xu[i__ - 1] = 1.;
+ for (j = 1; j <= 2; ++j) {
+ l5_6.gg[j + (i__ << 1) - 3] = hgg[j + (i__ << 1) - 3];
+/* labelL6: */
+ }
+ }
+ l20_2.lex = true;
+ l20_2.nex = 2;
+ l20_2.fex = .97474658;
+ for (i__ = 1; i__ <= 3; i__ += 2) {
+ l20_2.xex[i__ - 1] = 1.;
+/* L7: */
+ l20_2.xex[i__] = 0.;
+ }
+ for (i__ = 5; i__ <= 8; ++i__) {
+/* L8: */
+ l20_2.xex[i__ - 1] = l20_2.xex[9 - i__ - 1];
+ }
+ return 0;
+labelL2:
+ l6_1.fx = 2.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+/* labelL9: */
+ l6_1.fx -= std::exp(l2_3.x[i__ - 1] * -10. * std::exp(-l2_3.x[i__ + 1]));
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l4_3.gf[i__ - 1] = std::exp(l2_3.x[i__ - 1] * -10. * std::exp(-l2_3.x[i__ + 1])
+ - l2_3.x[i__ + 1]) * 10.;
+/* labelL10: */
+ l4_3.gf[i__ + 1] = -l2_3.x[i__ - 1] * l4_3.gf[i__ - 1];
+ }
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = l2_3.x[0] + l2_3.x[1] - 1.;
+ }
+ if (l9_3.index1[1]) {
+ l3_2.g[1] = l2_3.x[2] + l2_3.x[3] - 1.;
+ }
+labelL5:
+ return 0;
+} /* tp265_ */
+
+
+/* Subroutine */ int tp266_(int *mode)
+{
+ /* Initialized data */
+
+ static Real a[10] = { .0426149,.0352053,.0878058,.0330812,.0580924,
+ .649704,.344144,-.627443,.001828,-.224783 };
+ static Real d__[10] = { 2.34659,2.84048,1.13888,3.02286,1.72139,
+ .153917,.290577,-.159378,54.691,-.444873 };
+ static Real c__[50] /* was [10][5] */ = { -.564255,.535493,
+ .586387,.608734,.774227,-.435033,.759468,-.152448,-.821772,
+ .819831,.392417,.658799,.289826,.984915,.325421,-.688583,-.627795,
+ -.546437,-.53412,-.910632,-.404979,-.0636666,.854402,.375699,
+ -.151719,.222278,.0403142,.484134,-.798498,-.480344,.927589,
+ -.681091,.789312,.239547,.448051,-.524653,.724666,.353951,
+ -.658572,-.871758,-.0735083,-.869487,.949721,.463136,.149926,
+ .413248,-.0182537,.887866,.662362,-.978666 };
+ static Real b[25] /* was [5][5] */ = { .354033,-.0230349,
+ -.211938,-.0554288,.220429,-.0230349,.29135,-.00180333,-.111141,
+ .0485461,-.211938,-.00180333,-.815808,-.133538,-.38067,-.0554288,
+ -.111141,-.133538,.389198,-.131586,.220429,.0485461,-.38067,
+ -.131586,.534706 };
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int ninl;
+ Real tp266a_(Real*, Real*, Real*, Real*, int*, Real*);
+ static int i__, k, l;
+ static Real hf;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_2.n = 5;
+ l15_1.lsum = 10;
+ l1_2.nili = 0;
+ ninl = 0;
+ l1_2.neli = 0;
+ l1_2.nenl = 0;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l2_4.x[i__ - 1] = .1;
+ l11_4.lxl[i__ - 1] = false;
+ l12_4.lxu[i__ - 1] = false;
+/* labelL6: */
+ l20_7.xex[i__ - 1] = 0.;
+ }
+ l20_7.nex = 1;
+ l20_7.lex = true;
+/* FEX=0.0 */
+/* DO I=1,10 */
+/* FEX=FEX+A(I)**2 */
+/* ENDDO */
+ l20_7.fex = 1e4;
+ return 0;
+labelL2:
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* labelL20: */
+ l16_4.f[i__ - 1] = tp266a_(&a[i__ - 1], b, c__, &d__[i__ - 1], &i__,
+ l2_4.x);
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L7: */
+/* Computing 2nd power */
+ d__1 = l16_4.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ l6_1.fx *= 1e4;
+ return 0;
+labelL3:
+ for (k = 1; k <= 5; ++k) {
+ l4_4.gf[k - 1] = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ hf = 0.;
+ for (l = 1; l <= 5; ++l) {
+/* labelL10: */
+ hf += (b[k + l * 5 - 6] + b[l + k * 5 - 6]) * l2_4.x[l - 1];
+ }
+ l17_9.df[i__ + k * 10 - 11] = c__[i__ + k * 10 - 11] + d__[i__ -
+ 1] * .5 * hf;
+ l4_4.gf[k - 1] += l16_4.f[i__ - 1] * 2. * l17_9.df[i__ + k * 10 -
+ 11];
+/* L8: */
+ l4_4.gf[k - 1] *= 1e4;
+ }
+ }
+labelL4:
+ return 0;
+} /* tp266_ */
+
+Real tp266a_(Real *ai, Real *b, Real *c__, Real *di, int *i__, Real *x)
+{
+ /* System generated locals */
+ Real ret_val;
+
+ /* Local variables */
+ static int k, l;
+ static Real hf;
+
+
+/* FUNKTION ZUR BERECHNUNG VON */
+/* F(I,X)=(A+C*X+0.5*X'*B*X*D)(I) */
+/* D.H. DAS I-TE ELEMENT DES REAL*8 VEKTORS F(10) FUER I=1,..,10 */
+
+ /* Parameter adjustments */
+ --x;
+ c__ -= 11;
+ b -= 6;
+
+ /* Function Body */
+ ret_val = *ai;
+ for (k = 1; k <= 5; ++k) {
+ hf = 0.;
+ for (l = 1; l <= 5; ++l) {
+/* labelL20: */
+ hf += b[k + l * 5] * x[l];
+ }
+ ret_val += x[k] * (c__[*i__ + k * 10] + *di * .5 * hf);
+/* labelL10: */
+ }
+ return ret_val;
+} /* tp266a_ */
+
+
+/* Subroutine */ int tp267_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real h__;
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l2_4.x[i__ - 1] = 2.;
+ l11_4.lxl[i__ - 1] = false;
+ l14_4.xu[i__ - 1] = (float)15.;
+/* labelL6: */
+ l12_4.lxu[i__ - 1] = true;
+ }
+ l11_4.lxl[0] = true;
+ l11_4.lxl[1] = true;
+ l11_4.lxl[4] = true;
+ l13_4.xl[0] = (float)0.;
+ l13_4.xl[1] = (float)0.;
+ l13_4.xl[4] = (float)0.;
+ l15_1.lsum = 11;
+ l20_10.lex = true;
+ l20_10.nex = 2;
+ l20_10.fex = 0.;
+ l20_10.xex[0] = 1.;
+ l20_10.xex[1] = 10.;
+ l20_10.xex[2] = 1.;
+ l20_10.xex[3] = 5.;
+ l20_10.xex[4] = 4.;
+ l20_10.xex[5] = 10.;
+ l20_10.xex[6] = 1.;
+ l20_10.xex[7] = -5.;
+ l20_10.xex[8] = -1.;
+ l20_10.xex[9] = 4.;
+ return 0;
+labelL2:
+ for (i__ = 1; i__ <= 11; ++i__) {
+ h__ = (Real) i__ * .1;
+/* HF(1)=X(3)*DEXP(-X(1)*H) */
+/* HF(2)=X(4)*DEXP(-X(2)*H) */
+/* HF(3)=3.D+0*DEXP(-X(5)*H) */
+/* HF(4)=-DEXP(-H)+5.D+0*DEXP(-1.D+1*H)-3.D+0*DEXP(-4.D+0*H) */
+/* F(I)=HF(1)-HF(2)+HF(3)+HF(4) */
+/* labelL20: */
+ l16_7.f[i__ - 1] = l2_4.x[2] * std::exp(-l2_4.x[0] * h__) - l2_4.x[3] *
+ exp(-l2_4.x[1] * h__) + std::exp(-l2_4.x[4] * h__) * 3. - (exp(
+ -h__) - std::exp(h__ * -10.) * 5. + std::exp(h__ * -4.) * 3.);
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 11; ++i__) {
+/* L7: */
+/* Computing 2nd power */
+ d__1 = l16_7.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ for (j = 1; j <= 11; ++j) {
+ h__ = (Real) j * .1;
+ l17_10.df[j + 21] = std::exp(-l2_4.x[0] * h__);
+ l17_10.df[j + 32] = -exp(-l2_4.x[1] * h__);
+ l17_10.df[j - 1] = -h__ * l2_4.x[2] * l17_10.df[j + 21];
+ l17_10.df[j + 10] = -h__ * l2_4.x[3] * l17_10.df[j + 32];
+/* L8: */
+ l17_10.df[j + 43] = -h__ * 3. * std::exp(-l2_4.x[4] * h__);
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l4_4.gf[i__ - 1] = 0.;
+ for (j = 1; j <= 11; ++j) {
+/* labelL13: */
+ l4_4.gf[i__ - 1] += l16_7.f[j - 1] * 2. * l17_10.df[j + i__ * 11
+ - 12];
+ }
+ }
+labelL4:
+ return 0;
+} /* tp267_ */
+
+
+/* Subroutine */ int tp268_(int *mode)
+{
+ /* Initialized data */
+
+ static int hgg[25] /* was [5][5] */ = { -1,10,-8,8,-4,-1,10,1,-1,
+ -2,-1,-3,-2,2,3,-1,5,-5,5,-5,-1,4,3,-3,1 };
+ static int dd[25] /* was [5][5] */ = { 10197,-12454,-1013,1948,
+ 329,-12454,20909,-1733,-4914,-186,-1013,-1733,1755,1089,-174,1948,
+ -4914,1089,1515,-22,329,-186,-174,-22,27 };
+ static int ddvekt[5] = { -9170,17099,-2271,-4336,-43 };
+ static int dvdv = 14463;
+
+ static int ninl, i__, j;
+ static Real hf;
+
+/* KONSTANTE DATEN DER ZIELFUNKTION: */
+/* DD=D'*D */
+/* DDVEKT=DVEKT'*D */
+/* DVDV=DVEKT'*DVEKT=14463 */
+/* MIT */
+/* - - */
+/* | -74 80 18 -11 -4 | */
+/* | 14 -69 21 28 0 | */
+/* D= | 66 -72 -5 7 1 | */
+/* | -12 66 -30 -23 3 | */
+/* | 3 8 -7 -4 1 | */
+/* | 4 -12 4 4 0 | */
+/* - - */
+
+/* DVEKT=( 51, -61, -56, 69, 10, -12 )' */
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_2.n = 5;
+ l1_2.nili = 5;
+ ninl = 0;
+ l1_2.neli = 0;
+ l1_2.nenl = 0;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l2_4.x[i__ - 1] = 1.;
+ l11_4.lxl[i__ - 1] = false;
+ l12_4.lxu[i__ - 1] = false;
+ for (j = 1; j <= 5; ++j) {
+/* labelL6: */
+ l5_29.gg[i__ + j * 5 - 6] = (Real) hgg[i__ + j * 5 - 6];
+ }
+ }
+ l20_7.lex = true;
+ l20_7.nex = 1;
+ l20_7.fex = 0.;
+ l20_7.xex[0] = 1.;
+ l20_7.xex[1] = 2.;
+ l20_7.xex[2] = -1.;
+ l20_7.xex[3] = 3.;
+ l20_7.xex[4] = -4.;
+ return 0;
+labelL2:
+ l6_1.fx = (Real) dvdv;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ hf = 0.;
+ for (j = 1; j <= 5; ++j) {
+/* L8: */
+ hf += (Real) dd[i__ + j * 5 - 6] * l2_4.x[j - 1];
+ }
+/* L7: */
+ l6_1.fx += l2_4.x[i__ - 1] * (hf - (Real) ddvekt[i__ - 1] * 2.);
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l4_4.gf[i__ - 1] = (Real) ddvekt[i__ - 1] * -2.;
+ for (j = 1; j <= 5; ++j) {
+/* labelL9: */
+ l4_4.gf[i__ - 1] += (Real) (dd[i__ + j * 5 - 6] + dd[j +
+ i__ * 5 - 6]) * l2_4.x[j - 1];
+ }
+ }
+ return 0;
+labelL4:
+ if (l9_5.index1[0]) {
+ l3_4.g[0] = -l2_4.x[0] - l2_4.x[1] - l2_4.x[2] - l2_4.x[3] - l2_4.x[4]
+ + 5;
+ }
+ if (l9_5.index1[1]) {
+ l3_4.g[1] = l2_4.x[0] * 10. + l2_4.x[1] * 10. - l2_4.x[2] * 3. +
+ l2_4.x[3] * 5. + l2_4.x[4] * 4. - 20.;
+ }
+ if (l9_5.index1[2]) {
+ l3_4.g[2] = l2_4.x[0] * -8. + l2_4.x[1] - l2_4.x[2] * 2. - l2_4.x[3] *
+ 5. + l2_4.x[4] * 3. + 40.;
+ }
+ if (l9_5.index1[3]) {
+ l3_4.g[3] = l2_4.x[0] * 8. - l2_4.x[1] + l2_4.x[2] * 2. + l2_4.x[3] *
+ 5. - l2_4.x[4] * 3. - 11.;
+ }
+ if (l9_5.index1[4]) {
+ l3_4.g[4] = l2_4.x[0] * -4. - l2_4.x[1] * 2. + l2_4.x[2] * 3. -
+ l2_4.x[3] * 5. + l2_4.x[4] + 30.;
+ }
+labelL5:
+ return 0;
+} /* tp268_ */
+
+
+/* Subroutine */ int tp269_(int *mode)
+{
+ /* Initialized data */
+
+ static Real hgg[15] /* was [3][5] */ = { 1.,0.,0.,3.,0.,1.,0.,1.,
+ 0.,0.,1.,0.,0.,-2.,-1. };
+ static Real hdf[20] /* was [4][5] */ = { 1.,0.,0.,0.,-1.,1.,0.,0.,
+ 0.,1.,0.,0.,0.,0.,1.,0.,0.,0.,0.,1. };
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l15_1.lsum = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 3;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l2_4.x[i__ - 1] = 2.;
+ l11_4.lxl[i__ - 1] = false;
+ l12_4.lxu[i__ - 1] = false;
+ for (j = 1; j <= 3; ++j) {
+ l5_9.gg[j + i__ * 3 - 4] = hgg[j + i__ * 3 - 4];
+/* L7: */
+ l17_8.df[j + (i__ << 2) - 5] = hdf[j + (i__ << 2) - 5];
+ }
+/* labelL6: */
+ l17_8.df[(i__ << 2) - 1] = hdf[(i__ << 2) - 1];
+ }
+ l20_7.lex = true;
+ l20_7.nex = 1;
+ l20_7.fex = 4.0930232558139537;
+ l20_7.xex[0] = -.76744186046511631;
+ l20_7.xex[1] = .2558139534883721;
+ l20_7.xex[2] = .62790697674418605;
+ l20_7.xex[3] = -.11627906976744186;
+ l20_7.xex[4] = .2558139534883721;
+ return 0;
+labelL2:
+ l16_3.f[0] = l2_4.x[0] - l2_4.x[1];
+ l16_3.f[1] = l2_4.x[1] + l2_4.x[2] - 2.;
+ l16_3.f[2] = l2_4.x[3] - 1.;
+ l16_3.f[3] = l2_4.x[4] - 1.;
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* L8: */
+/* Computing 2nd power */
+ d__1 = l16_3.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ l4_4.gf[0] = (l2_4.x[0] - l2_4.x[1]) * 2.;
+ l4_4.gf[1] = (l2_4.x[1] * 2. + l2_4.x[2] - l2_4.x[0] - 2.) * 2.;
+ l4_4.gf[2] = (l2_4.x[1] + l2_4.x[2] - 2.) * 2.;
+ l4_4.gf[3] = (l2_4.x[3] - 1.) * 2.;
+ l4_4.gf[4] = (l2_4.x[4] - 1.) * 2.;
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = l2_4.x[0] + l2_4.x[1] * 3.;
+ }
+ if (l9_4.index1[1]) {
+ l3_3.g[1] = l2_4.x[2] + l2_4.x[3] - l2_4.x[4] * 2.;
+ }
+ if (l9_4.index1[2]) {
+ l3_3.g[2] = l2_4.x[1] - l2_4.x[4];
+ }
+labelL5:
+ return 0;
+} /* tp269_ */
+
+/* Subroutine */ int tp270_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l2_4.x[i__ - 1] = (Real) i__ + .1;
+ l20_7.xex[i__ - 1] = (Real) i__;
+ l11_4.lxl[i__ - 1] = true;
+ l12_4.lxu[i__ - 1] = false;
+/* labelL6: */
+ l13_4.xl[i__ - 1] = (Real) i__;
+ }
+ l11_4.lxl[4] = false;
+ l12_4.lxu[4] = false;
+ l2_4.x[4] = -1.;
+ l20_7.xex[4] = 2.;
+ l20_7.nex = 1;
+ l20_7.lex = true;
+ l20_7.fex = -1.;
+ return 0;
+labelL2:
+/* Computing 4th power */
+ d__1 = l2_4.x[4], d__1 *= d__1;
+/* Computing 3rd power */
+ d__2 = l2_4.x[4];
+/* Computing 2nd power */
+ d__3 = l2_4.x[4];
+ l6_1.fx = l2_4.x[0] * l2_4.x[1] * l2_4.x[2] * l2_4.x[3] - l2_4.x[0] * 3. *
+ l2_4.x[1] * l2_4.x[3] - l2_4.x[0] * 4. * l2_4.x[1] * l2_4.x[2] +
+ l2_4.x[0] * 12. * l2_4.x[1] - l2_4.x[1] * l2_4.x[2] * l2_4.x[3] +
+ l2_4.x[1] * 3. * l2_4.x[3] + l2_4.x[1] * 4. * l2_4.x[2] - l2_4.x[
+ 1] * 12. - l2_4.x[0] * 2. * l2_4.x[2] * l2_4.x[3] + l2_4.x[0] *
+ 6. * l2_4.x[3] + l2_4.x[0] * 8. * l2_4.x[2] - l2_4.x[0] * 24. +
+ l2_4.x[2] * 2. * l2_4.x[3] - l2_4.x[3] * 6. - l2_4.x[2] * 8. +
+ 24. + d__1 * d__1 * 1.5 - d__2 * (d__2 * d__2) * 5.75 + d__3 *
+ d__3 * 5.25;
+ return 0;
+labelL3:
+ l4_4.gf[0] = l2_4.x[1] * l2_4.x[2] * l2_4.x[3] - l2_4.x[1] * 3. * l2_4.x[
+ 3] - l2_4.x[1] * 4. * l2_4.x[2] + l2_4.x[1] * 12. - l2_4.x[2] *
+ 2. * l2_4.x[3] + l2_4.x[3] * 6. + l2_4.x[2] * 8. - 24.;
+ l4_4.gf[1] = l2_4.x[0] * l2_4.x[2] * l2_4.x[3] - l2_4.x[0] * 3. * l2_4.x[
+ 3] - l2_4.x[0] * 4. * l2_4.x[2] + l2_4.x[0] * 12. - l2_4.x[2] *
+ l2_4.x[3] + l2_4.x[3] * 3. + l2_4.x[2] * 4. - 12.;
+ l4_4.gf[2] = l2_4.x[0] * l2_4.x[1] * l2_4.x[3] - l2_4.x[0] * 4. * l2_4.x[
+ 1] - l2_4.x[1] * l2_4.x[3] + l2_4.x[1] * 4. - l2_4.x[0] * 2. *
+ l2_4.x[3] + l2_4.x[0] * 8. + l2_4.x[3] * 2. - 8.;
+ l4_4.gf[3] = l2_4.x[0] * l2_4.x[1] * l2_4.x[2] - l2_4.x[0] * 3. * l2_4.x[
+ 1] - l2_4.x[1] * l2_4.x[2] + l2_4.x[1] * 3. - l2_4.x[0] * 2. *
+ l2_4.x[2] + l2_4.x[0] * 6. + l2_4.x[2] * 2. - 6.;
+/* Computing 2nd power */
+ d__1 = l2_4.x[4];
+/* Computing 3rd power */
+ d__2 = l2_4.x[4];
+ l4_4.gf[4] = l2_4.x[4] * 10.5 - d__1 * d__1 * 17.25 + d__2 * (d__2 * d__2)
+ * 6.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_4.x[0];
+/* Computing 2nd power */
+ d__2 = l2_4.x[1];
+/* Computing 2nd power */
+ d__3 = l2_4.x[2];
+/* Computing 2nd power */
+ d__4 = l2_4.x[3];
+/* Computing 2nd power */
+ d__5 = l2_4.x[4];
+ l3_1.g[0] = 34. - d__1 * d__1 - d__2 * d__2 - d__3 * d__3 - d__4 *
+ d__4 - d__5 * d__5;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L8: */
+ l5_30.gg[i__ - 1] = l2_4.x[i__ - 1] * -2.;
+ }
+L7:
+ return 0;
+} /* tp270_ */
+
+/* Subroutine */ int tp271_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 6;
+ l15_1.lsum = 6;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 6; ++i__) {
+ l11_5.lxl[i__ - 1] = false;
+ l12_5.lxu[i__ - 1] = false;
+ l2_5.x[i__ - 1] = 0.;
+/* labelL6: */
+ l20_4.xex[i__ - 1] = 1.;
+ }
+ l20_4.lex = true;
+ l20_4.fex = 0.;
+ return 0;
+labelL2:
+ for (i__ = 1; i__ <= 6; ++i__) {
+/* labelL10: */
+ l16_8.f[i__ - 1] = std::sqrt((Real) ((16 - i__) * 10)) * (l2_5.x[i__
+ - 1] - 1.);
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 6; ++i__) {
+/* L7: */
+/* Computing 2nd power */
+ d__1 = l16_8.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 6; ++i__) {
+ l4_5.gf[i__ - 1] = (Real) ((16 - i__) * 20) * (l2_5.x[i__ - 1]
+ - 1.);
+ for (j = 1; j <= 6; ++j) {
+/* labelL9: */
+ l17_11.df[i__ + j * 6 - 7] = 0.;
+ }
+/* L8: */
+ l17_11.df[i__ + i__ * 6 - 7] = std::sqrt((Real) ((16 - i__) * 10));
+ }
+labelL4:
+ return 0;
+} /* tp271_ */
+
+/* Subroutine */ int tp272_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real h__;
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 6;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 6; ++i__) {
+ l2_5.x[i__ - 1] = 1.;
+ l13_5.xl[i__ - 1] = (float)0.;
+ l11_5.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_5.lxu[i__ - 1] = false;
+ }
+ l2_5.x[1] = 2.;
+ l15_1.lsum = 13;
+ l20_4.lex = true;
+ l20_4.nex = 1;
+ l20_4.fex = 0.;
+ l20_4.xex[0] = 1.;
+ l20_4.xex[1] = 10.;
+ l20_4.xex[2] = 4.;
+ l20_4.xex[3] = 1.;
+ l20_4.xex[4] = 5.;
+ l20_4.xex[5] = 3.;
+ return 0;
+labelL2:
+ for (i__ = 1; i__ <= 13; ++i__) {
+ h__ = (Real) i__ * .1;
+/* labelL20: */
+ l16_9.f[i__ - 1] = l2_5.x[3] * std::exp(-l2_5.x[0] * h__) - l2_5.x[4] *
+ exp(-l2_5.x[1] * h__) + l2_5.x[5] * std::exp(-l2_5.x[2] * h__) -
+ exp(-h__) + std::exp(h__ * -10.) * 5. - std::exp(h__ * -4.) * 3.;
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* L7: */
+/* Computing 2nd power */
+ d__1 = l16_9.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ for (j = 1; j <= 13; ++j) {
+ h__ = (Real) j * .1;
+ l17_12.df[j + 38] = std::exp(-l2_5.x[0] * h__);
+ l17_12.df[j + 51] = -exp(-l2_5.x[1] * h__);
+ l17_12.df[j + 64] = std::exp(-l2_5.x[2] * h__);
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L8: */
+ l17_12.df[j + i__ * 13 - 14] = -h__ * l2_5.x[i__ + 2] * l17_12.df[
+ j + (i__ + 3) * 13 - 14];
+ }
+ }
+ for (i__ = 1; i__ <= 6; ++i__) {
+ l4_5.gf[i__ - 1] = 0.;
+ for (j = 1; j <= 13; ++j) {
+/* labelL13: */
+ l4_5.gf[i__ - 1] += l17_12.df[j + i__ * 13 - 14] * 2. * l16_9.f[j
+ - 1];
+ }
+ }
+labelL4:
+ return 0;
+} /* tp272_ */
+
+/* Subroutine */ int tp273_(int *mode)
+{
+ static int ninl;
+ Real tp273a_(Real *);
+ static int i__;
+ static Real hx;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_2.n = 6;
+ l1_2.nili = 0;
+ ninl = 0;
+ l1_2.neli = 0;
+ l1_2.nenl = 0;
+ for (i__ = 1; i__ <= 6; ++i__) {
+ l2_5.x[i__ - 1] = 0.;
+ l20_4.xex[i__ - 1] = 1.;
+ l11_5.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_5.lxu[i__ - 1] = false;
+ }
+ l20_4.lex = true;
+ l20_4.nex = 1;
+ l20_4.fex = 0.;
+ return 0;
+labelL2:
+ hx = tp273a_(l2_5.x);
+ l6_1.fx = hx * 10. * (hx + 1.);
+ return 0;
+labelL3:
+ hx = tp273a_(l2_5.x);
+ for (i__ = 1; i__ <= 6; ++i__) {
+/* L7: */
+ l4_5.gf[i__ - 1] = (16. - (Real) i__) * 20. * (l2_5.x[i__ - 1]
+ - 1.) * (hx * 2. + 1.);
+ }
+labelL4:
+ return 0;
+} /* tp273_ */
+
+Real tp273a_(Real *x)
+{
+ /* System generated locals */
+ Real ret_val, d__1;
+
+ /* Local variables */
+ static int i__;
+
+
+/* BERECHNUNG VON H(X) IN TP273 */
+/* HX=SUM( (16-I)*(X(I)-1)**2,I=1,..,6) */
+
+ /* Parameter adjustments */
+ --x;
+
+ /* Function Body */
+ ret_val = 0.;
+ for (i__ = 1; i__ <= 6; ++i__) {
+/* labelL10: */
+/* Computing 2nd power */
+ d__1 = x[i__] - 1.;
+ ret_val += (16. - (Real) i__) * (d__1 * d__1);
+ }
+ return ret_val;
+} /* tp273a_ */
+
+/* Subroutine */ int tp274_0_(int n__, int *mode)
+{
+ /* System generated locals */
+ int i__1, i__2;
+
+ /* Local variables */
+ static Real a[36] /* was [6][6] */;
+ static int i__, j;
+
+ switch(n__) {
+ case 1: goto L_tp275;
+ case 2: goto L_tp276;
+ }
+
+ l1_1.n = 2;
+ goto labelL10;
+
+L_tp275:
+ l1_1.n = 4;
+ goto labelL10;
+
+L_tp276:
+ l1_1.n = 6;
+labelL10:
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ i__1 = l1_1.n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ l2_5.x[i__ - 1] = -4. / (Real) i__;
+ i__2 = l1_1.n;
+ for (j = 1; j <= i__2; ++j) {
+/* labelL11: */
+ a[i__ + j * 6 - 7] = 1. / (Real) (i__ + j - 1);
+ }
+ l20_4.xex[i__ - 1] = 0.;
+ l11_5.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_5.lxu[i__ - 1] = false;
+ }
+ l20_4.lex = true;
+ l20_4.nex = 1;
+ l20_4.fex = 0.;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ i__1 = l1_1.n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ i__2 = l1_1.n;
+ for (j = 1; j <= i__2; ++j) {
+/* L7: */
+ l6_1.fx += a[i__ + j * 6 - 7] * l2_5.x[i__ - 1] * l2_5.x[j - 1];
+ }
+ }
+ return 0;
+labelL3:
+ i__2 = l1_1.n;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ l4_5.gf[i__ - 1] = 0.;
+ i__1 = l1_1.n;
+ for (j = 1; j <= i__1; ++j) {
+/* L8: */
+ l4_5.gf[i__ - 1] += l2_5.x[j - 1] * (a[i__ + j * 6 - 7] + a[j +
+ i__ * 6 - 7]);
+ }
+ }
+labelL4:
+ return 0;
+} /* tp274_ */
+
+/* Subroutine */ int tp274_(int *mode)
+{
+ return tp274_0_(0, mode);
+ }
+
+/* Subroutine */ int tp275_(int *mode)
+{
+ return tp274_0_(1, mode);
+ }
+
+/* Subroutine */ int tp276_(int *mode)
+{
+ return tp274_0_(2, mode);
+ }
+
+/* Subroutine */ int tp277_0_(int n__, int *mode)
+{
+ /* System generated locals */
+ int i__1, i__2;
+
+ /* Local variables */
+ static Real h__;
+ static int i__, j;
+
+ switch(n__) {
+ case 1: goto L_tp278;
+ case 2: goto L_tp279;
+ case 3: goto L_tp280;
+ }
+
+ l1_1.n = 4;
+ goto labelL10;
+
+L_tp278:
+ l1_1.n = 6;
+ goto labelL10;
+
+L_tp279:
+ l1_1.n = 8;
+ goto labelL10;
+
+L_tp280:
+ l1_1.n = 10;
+labelL10:
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.nili = l1_1.n;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l20_10.fex = 0.;
+ i__1 = l1_1.n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ l2_9.x[i__ - 1] = 0.;
+ i__2 = l1_1.n;
+ for (j = 1; j <= i__2; ++j) {
+ l20_10.fex += 1. / (Real) (i__ + j - 1);
+/* L16: */
+ l5_31.gg[j + (i__ * l1_1.n) - (l1_1.n+1)] = 1. / (Real) (i__ + j - 1);
+//printf("jac[%d] = %f \n",j + (i__ * l1_1.n) - (l1_1.n+1),l5_31.gg[j + (i__ * l1_1.n) - (l1_1.n+1)] );
+ }
+ l11_9.lxl[i__ - 1] = true;
+ l12_9.lxu[i__ - 1] = false;
+ l13_9.xl[i__ - 1] = 0.;
+/* labelL6: */
+ l20_10.xex[i__ - 1] = 1.;
+ }
+ l20_10.lex = true;
+ l20_10.nex = 1;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ i__1 = l1_1.n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ h__ = 0.;
+ i__2 = l1_1.n;
+ for (j = 1; j <= i__2; ++j) {
+/* L17: */
+ h__ += 1. / (Real) (i__ + j - 1);
+ }
+/* L7: */
+ l6_1.fx += h__ * l2_9.x[i__ - 1];
+ }
+ return 0;
+labelL3:
+ i__1 = l1_1.n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ h__ = 0.;
+ i__2 = l1_1.n;
+ for (j = 1; j <= i__2; ++j) {
+/* L18: */
+ h__ += 1. / (Real) (i__ + j - 1);
+ }
+/* L8: */
+ l4_9.gf[i__ - 1] = h__;
+ }
+ return 0;
+labelL4:
+ i__1 = l1_1.n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ h__ = 0.;
+ i__2 = l1_1.n;
+ for (j = 1; j <= i__2; ++j) {
+/* L19: */
+ h__ += (l2_9.x[j - 1] - 1.) / (Real) (i__ + j - 1);
+ }
+/* labelL9: */
+ if (l9_10.index1[i__ - 1]) {
+ l3_9.g[i__ - 1] = h__;
+ }
+ }
+labelL5:
+ return 0;
+} /* tp277_ */
+
+/* Subroutine */ int tp277_(int *mode)
+{
+ int tp277_0_(int, int *);
+ return tp277_0_(0, mode);
+ }
+
+/* Subroutine */ int tp278_(int *mode)
+{
+ int tp277_0_(int, int *);
+ return tp277_0_(1, mode);
+ }
+
+/* Subroutine */ int tp279_(int *mode)
+{
+ int tp277_0_(int, int *);
+ return tp277_0_(2, mode);
+ }
+
+/* Subroutine */ int tp280_(int *mode)
+{
+ int tp277_0_(int, int *);
+ return tp277_0_(3, mode);
+ }
+
+
+/* Subroutine */ int tp281_(int *mode)
+{
+ /* System generated locals */
+ int i__1;
+ Real d__1;
+
+ /* Local variables */
+ static Real h__;
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 10;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ l2_9.x[i__ - 1] = 0.;
+ l11_9.lxl[i__ - 1] = false;
+ l12_9.lxu[i__ - 1] = false;
+/* labelL6: */
+ l20_10.xex[i__ - 1] = 1.;
+ }
+ l20_10.lex = true;
+ l20_10.nex = 1;
+ l20_10.fex = 0.;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L7: */
+/* Computing 3rd power */
+ i__1 = i__;
+/* Computing 2nd power */
+ d__1 = l2_9.x[i__ - 1] - 1.;
+ l6_1.fx += (Real) (i__1 * (i__1 * i__1)) * (d__1 * d__1);
+ }
+ l6_1.fx = pow_dd(&l6_1.fx, &c_b74);
+ return 0;
+labelL3:
+ h__ = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L8: */
+/* Computing 3rd power */
+ i__1 = i__;
+/* Computing 2nd power */
+ d__1 = l2_9.x[i__ - 1] - 1.;
+ h__ += (Real) (i__1 * (i__1 * i__1)) * (d__1 * d__1);
+ }
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* labelL13: */
+/* Computing 3rd power */
+ i__1 = i__;
+ l4_9.gf[i__ - 1] = (Real) (i__1 * (i__1 * i__1)) *
+ .66666666666666663 * (l2_9.x[i__ - 1] - 1.) * pow_dd(&h__, &
+ c_b1920);
+ if( !isFinite(l4_9.gf[i__ - 1])) l4_9.gf[i__ - 1] = 0.0;
+ }
+labelL4:
+ return 0;
+} /* tp281_ */
+
+
+/* Subroutine */ int tp282_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static Real h__;
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 10;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ l2_9.x[i__ - 1] = 0.;
+ l11_9.lxl[i__ - 1] = false;
+ l12_9.lxu[i__ - 1] = false;
+/* labelL6: */
+ l20_10.xex[i__ - 1] = 1.;
+ }
+ l2_9.x[0] = -1.2;
+ l20_10.lex = true;
+ l20_10.nex = 1;
+ l20_10.fex = 0.;
+ l15_1.lsum = 11;
+ return 0;
+labelL2:
+ l16_7.f[9] = l2_9.x[0] - 1.;
+ l16_7.f[10] = l2_9.x[9] - 1.;
+ for (i__ = 1; i__ <= 9; ++i__) {
+/* labelL20: */
+/* Computing 2nd power */
+ d__1 = l2_9.x[i__ - 1];
+ l16_7.f[i__ - 1] = std::sqrt((10. - (Real) i__) * 10.) * (d__1 *
+ d__1 - l2_9.x[i__]);
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+/* Computing 2nd power */
+ d__1 = l16_7.f[9];
+/* Computing 2nd power */
+ d__2 = l16_7.f[10];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ for (i__ = 1; i__ <= 9; ++i__) {
+/* L7: */
+/* Computing 2nd power */
+ d__1 = l16_7.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 11; ++i__) {
+ for (j = 1; j <= 10; ++j) {
+/* L8: */
+ l17_13.df[i__ + j * 11 - 12] = 0.;
+ }
+ }
+ for (i__ = 1; i__ <= 9; ++i__) {
+ h__ = std::sqrt((10. - (Real) i__) * 10.);
+ l17_13.df[i__ + i__ * 11 - 12] = h__ * 2. * l2_9.x[i__ - 1];
+/* labelL13: */
+ l17_13.df[i__ + (i__ + 1) * 11 - 12] = -h__;
+ }
+ l17_13.df[9] = 1.;
+ l17_13.df[109] = 1.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ l4_9.gf[i__ - 1] = 0.;
+ for (j = 1; j <= 11; ++j) {
+/* L18: */
+ l4_9.gf[i__ - 1] += l17_13.df[j + i__ * 11 - 12] * 2. * l16_7.f[j
+ - 1];
+ }
+ }
+labelL4:
+ return 0;
+} /* tp282_ */
+
+
+/* Subroutine */ int tp283_(int *mode)
+{
+ /* System generated locals */
+ int i__1;
+ Real d__1;
+
+ /* Local variables */
+ static Real h__;
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 10;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ l2_9.x[i__ - 1] = 0.;
+ l11_9.lxl[i__ - 1] = false;
+ l12_9.lxu[i__ - 1] = false;
+/* labelL6: */
+ l20_10.xex[i__ - 1] = 1.;
+ }
+ l20_10.lex = true;
+ l20_10.nex = 1;
+ l20_10.fex = 0.;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L7: */
+/* Computing 3rd power */
+ i__1 = i__;
+/* Computing 2nd power */
+ d__1 = l2_9.x[i__ - 1] - 1.;
+ l6_1.fx += (Real) (i__1 * (i__1 * i__1)) * (d__1 * d__1);
+ }
+/* Computing 3rd power */
+ d__1 = l6_1.fx;
+ l6_1.fx = d__1 * (d__1 * d__1) * 1e-5;
+ return 0;
+labelL3:
+ h__ = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L8: */
+/* Computing 3rd power */
+ i__1 = i__;
+/* Computing 2nd power */
+ d__1 = l2_9.x[i__ - 1] - 1.;
+ h__ += (Real) (i__1 * (i__1 * i__1)) * (d__1 * d__1);
+ }
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* labelL13: */
+/* Computing 3rd power */
+ i__1 = i__;
+/* Computing 2nd power */
+ d__1 = h__;
+ l4_9.gf[i__ - 1] = (Real) (i__1 * (i__1 * i__1)) * (float)6. * (
+ l2_9.x[i__ - 1] - 1.) * (d__1 * d__1) * 1e-5;
+ }
+labelL4:
+ return 0;
+} /* tp283_ */
+
+
+/* Subroutine */ int tp284_(int *mode)
+{
+ /* Initialized data */
+
+ static int c__[15] = { 20,40,400,20,80,20,40,140,380,280,80,40,140,40,
+ 120 };
+ static int b[10] = { 385,470,560,565,645,430,485,455,390,460 };
+ static int a[150] /* was [10][15] */ = { 100,90,70,50,50,40,30,
+ 20,10,5,100,100,50,0,10,0,60,30,70,10,10,10,0,0,70,50,30,40,10,
+ 100,5,35,55,65,60,95,90,25,35,5,10,20,25,35,45,50,0,40,25,20,0,5,
+ 100,100,45,35,30,25,65,5,0,0,40,35,0,10,5,15,0,10,25,35,50,60,35,
+ 60,25,10,30,35,0,55,0,0,65,0,0,80,0,95,10,25,30,15,5,45,70,20,0,
+ 70,55,20,60,0,75,15,20,30,25,20,5,0,10,75,100,20,25,30,0,10,45,40,
+ 30,35,75,0,70,5,15,35,20,25,0,30,10,5,15,65,50,10,0,10,40,65,0,5,
+ 15,20,55,30 };
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__, j;
+ static Real sum;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 15;
+ l1_1.nili = 0;
+ l1_1.ninl = 10;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l20_12.fex = 0.;
+ for (i__ = 1; i__ <= 15; ++i__) {
+ l2_11.x[i__ - 1] = 0.;
+ l20_12.xex[i__ - 1] = 1.;
+ l20_12.fex -= (Real) c__[i__ - 1];
+ l12_11.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_11.lxl[i__ - 1] = false;
+ }
+ l20_12.lex = true;
+ l20_12.nex = 1;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 15; ++i__) {
+/* L7: */
+ l6_1.fx -= (Real) c__[i__ - 1] * l2_11.x[i__ - 1];
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 15; ++i__) {
+/* L8: */
+ l4_11.gf[i__ - 1] = -((Real) c__[i__ - 1]);
+ }
+ return 0;
+labelL4:
+ for (i__ = 1; i__ <= 10; ++i__) {
+ sum = 0.;
+ for (j = 1; j <= 15; ++j) {
+/* labelL9: */
+/* Computing 2nd power */
+ d__1 = l2_11.x[j - 1];
+ sum += (Real) a[i__ + j * 10 - 11] * (d__1 * d__1);
+ }
+/* labelL10: */
+ if (l9_10.index1[i__ - 1]) {
+ l3_9.g[i__ - 1] = (Real) b[i__ - 1] - sum;
+ }
+ }
+ return 0;
+labelL5:
+ for (j = 1; j <= 10; ++j) {
+ for (i__ = 1; i__ <= 15; ++i__) {
+ if (! l10_10.index2[j - 1]) {
+ goto labelL12;
+ }
+/* labelL11: */
+ l5_32.gg[(i__ - 1) * 10 + j - 1] = (Real) a[j + i__ * 10 -
+ 11] * -2. * l2_11.x[i__ - 1];
+ }
+labelL12:
+ ;
+ }
+ return 0;
+} /* tp284_ */
+
+/* Subroutine */ int tp285_(int *mode)
+{
+ /* Initialized data */
+
+ static int c__[15] = { 486,640,758,776,477,707,175,619,627,614,475,
+ 377,524,468,529 };
+ static int b[10] = { 385,470,560,565,645,430,485,455,390,460 };
+ static int a[150] /* was [10][15] */ = { 100,90,70,50,50,40,30,
+ 20,10,5,100,100,50,0,10,0,60,30,70,10,10,10,0,0,70,50,30,40,10,
+ 100,5,35,55,65,60,95,90,25,35,5,10,20,25,35,45,50,0,40,25,20,0,5,
+ 100,100,45,35,30,25,65,5,0,0,40,35,0,10,5,15,0,10,25,35,50,60,35,
+ 60,25,10,30,35,0,55,0,0,65,0,0,80,0,95,10,25,30,15,5,45,70,20,0,
+ 70,55,20,60,0,75,15,20,30,25,20,5,0,10,75,100,20,25,30,0,10,45,40,
+ 30,35,75,0,70,5,15,35,20,25,0,30,10,5,15,65,50,10,0,10,40,65,0,5,
+ 15,20,55,30 };
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__, j;
+ static Real sum;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 15;
+ l1_1.nili = 0;
+ l1_1.ninl = 10;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l20_12.fex = 0.;
+ for (i__ = 1; i__ <= 15; ++i__) {
+ l2_11.x[i__ - 1] = 0.;
+ l20_12.xex[i__ - 1] = 1.;
+ l20_12.fex -= (Real) c__[i__ - 1];
+ l12_11.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_11.lxl[i__ - 1] = false;
+ }
+ l20_12.lex = true;
+ l20_12.nex = 1;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 15; ++i__) {
+/* L7: */
+ l6_1.fx -= (Real) c__[i__ - 1] * l2_11.x[i__ - 1];
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 15; ++i__) {
+/* L8: */
+ l4_11.gf[i__ - 1] = -((Real) c__[i__ - 1]);
+ }
+ return 0;
+labelL4:
+ for (i__ = 1; i__ <= 10; ++i__) {
+ sum = 0.;
+ for (j = 1; j <= 15; ++j) {
+/* labelL9: */
+/* Computing 2nd power */
+ d__1 = l2_11.x[j - 1];
+ sum += (Real) a[i__ + j * 10 - 11] * (d__1 * d__1);
+ }
+/* labelL10: */
+ if (l9_10.index1[i__ - 1]) {
+ l3_9.g[i__ - 1] = (Real) b[i__ - 1] - sum;
+ }
+ }
+ return 0;
+labelL5:
+ for (j = 1; j <= 10; ++j) {
+ for (i__ = 1; i__ <= 15; ++i__) {
+ if (! l10_10.index2[j - 1]) {
+ goto labelL12;
+ }
+/* labelL11: */
+ l5_32.gg[(i__ - 1) * 10 + j - 1] = (Real) a[j + i__ * 10 -
+ 11] * -2. * l2_11.x[i__ - 1];
+ }
+labelL12:
+ ;
+ }
+ return 0;
+} /* tp285_ */
+
+/* Subroutine */ int tp286_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL3;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 20;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 20; ++i__) {
+ l2_13.x[i__ - 1] = -1.2;
+ l20_14.xex[i__ - 1] = 1.;
+ l12_13.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_13.lxl[i__ - 1] = false;
+ }
+ for (i__ = 11; i__ <= 20; ++i__) {
+/* L7: */
+ l2_13.x[i__ - 1] = 1.;
+ }
+ l20_14.lex = true;
+ l20_14.nex = 1;
+ l20_14.fex = 0.;
+ l15_1.lsum = 20;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 20; ++i__) {
+/* L8: */
+/* Computing 2nd power */
+ d__1 = l16_10.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 10; ++i__) {
+ l16_10.f[i__ - 1] = l2_13.x[i__ - 1] - 1.;
+/* labelL12: */
+/* Computing 2nd power */
+ d__1 = l2_13.x[i__ - 1];
+ l16_10.f[i__ + 9] = (d__1 * d__1 - l2_13.x[i__ + 9]) * 10.;
+ }
+ if (*mode == 2) {
+ goto labelL2;
+ }
+ for (i__ = 1; i__ <= 10; ++i__) {
+ for (j = 1; j <= 20; ++j) {
+ l17_14.df[i__ + j * 198 - 199] = 0.;
+ if (i__ == j) {
+ l17_14.df[i__ + j * 198 - 199] = 1.;
+ }
+ l17_14.df[i__ + 10 + j * 198 - 199] = 0.;
+ if (i__ == j) {
+ l17_14.df[i__ + 10 + j * 198 - 199] = l2_13.x[i__ - 1] * 20.;
+ }
+/* labelL9: */
+ if (j == i__ + 10) {
+ l17_14.df[i__ + 10 + j * 198 - 199] = -10.;
+ }
+ }
+ }
+ for (j = 1; j <= 20; ++j) {
+ l4_13.gf[j - 1] = 0.;
+ for (i__ = 1; i__ <= 20; ++i__) {
+/* labelL11: */
+ l4_13.gf[j - 1] += l16_10.f[i__ - 1] * 2. * l17_14.df[i__ + j *
+ 198 - 199];
+ }
+ }
+labelL4:
+ return 0;
+} /* tp286_ */
+
+/* Subroutine */ int tp287_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5, d__6, d__7, d__8;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 20;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l2_13.x[i__ - 1] = -3.;
+ l2_13.x[i__ + 4] = -1.;
+ l2_13.x[i__ + 9] = -3.;
+/* labelL6: */
+ l2_13.x[i__ + 14] = -1.;
+ }
+ for (i__ = 1; i__ <= 20; ++i__) {
+ l12_13.lxu[i__ - 1] = false;
+ l11_13.lxl[i__ - 1] = false;
+/* L7: */
+ l20_14.xex[i__ - 1] = 1.;
+ }
+ l20_14.lex = true;
+ l20_14.nex = 1;
+ l20_14.fex = 0.;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L8: */
+/* Computing 2nd power */
+ d__2 = l2_13.x[i__ - 1];
+/* Computing 2nd power */
+ d__1 = d__2 * d__2 - l2_13.x[i__ + 4];
+/* Computing 2nd power */
+ d__3 = l2_13.x[i__ - 1] - 1.;
+/* Computing 2nd power */
+ d__5 = l2_13.x[i__ + 9];
+/* Computing 2nd power */
+ d__4 = d__5 * d__5 - l2_13.x[i__ + 14];
+/* Computing 2nd power */
+ d__6 = l2_13.x[i__ + 9] - 1.;
+/* Computing 2nd power */
+ d__7 = l2_13.x[i__ + 4] - 1.;
+/* Computing 2nd power */
+ d__8 = l2_13.x[i__ + 14] - 1.;
+ l6_1.fx = l6_1.fx + d__1 * d__1 * 100. + d__3 * d__3 + d__4 * d__4 *
+ 90. + d__6 * d__6 + (d__7 * d__7 + d__8 * d__8) * 10.1 + (
+ l2_13.x[i__ + 4] - 1.) * 19.8 * (l2_13.x[i__ + 14] - 1.);
+ }
+ l6_1.fx *= 1e-5;
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* Computing 2nd power */
+ d__1 = l2_13.x[i__ - 1];
+ l4_13.gf[i__ - 1] = (d__1 * d__1 - l2_13.x[i__ + 4]) * 400. * l2_13.x[
+ i__ - 1] + (l2_13.x[i__ - 1] - 1.) * 2.;
+ l4_13.gf[i__ - 1] *= 1e-5;
+/* Computing 2nd power */
+ d__1 = l2_13.x[i__ - 1];
+ l4_13.gf[i__ + 4] = (d__1 * d__1 - l2_13.x[i__ + 4]) * -200. + (
+ l2_13.x[i__ + 4] - 1.) * 20.2 + (l2_13.x[i__ + 14] - 1.) *
+ 19.8;
+ l4_13.gf[i__ + 4] *= 1e-5;
+/* Computing 2nd power */
+ d__1 = l2_13.x[i__ + 9];
+ l4_13.gf[i__ + 9] = l2_13.x[i__ + 9] * 360. * (d__1 * d__1 - l2_13.x[
+ i__ + 14]) + (l2_13.x[i__ + 9] - 1.) * 2.;
+ l4_13.gf[i__ + 9] *= 1e-5;
+/* Computing 2nd power */
+ d__1 = l2_13.x[i__ + 9];
+ l4_13.gf[i__ + 14] = (d__1 * d__1 - l2_13.x[i__ + 14]) * -180. + (
+ l2_13.x[i__ + 14] - 1.) * 20.2 + (l2_13.x[i__ + 4] - 1.) *
+ 19.8;
+/* labelL9: */
+ l4_13.gf[i__ + 14] *= 1e-5;
+ }
+labelL4:
+ return 0;
+} /* tp287_ */
+
+/* Subroutine */ int tp288_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL3;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 20;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l2_13.x[i__ - 1] = 3.;
+ l2_13.x[i__ + 4] = -1.;
+ l2_13.x[i__ + 9] = 0.;
+/* labelL6: */
+ l2_13.x[i__ + 14] = 1.;
+ }
+ for (i__ = 1; i__ <= 20; ++i__) {
+ l20_14.xex[i__ - 1] = 0.;
+ l11_13.lxl[i__ - 1] = false;
+/* L7: */
+ l12_13.lxu[i__ - 1] = false;
+ }
+ l20_14.fex = 0.;
+ l20_14.lex = true;
+ l20_14.nex = 1;
+ l15_1.lsum = 20;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 20; ++i__) {
+/* labelL9: */
+/* Computing 2nd power */
+ d__1 = l16_10.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l16_10.f[i__ - 1] = l2_13.x[i__ - 1] + l2_13.x[i__ + 4] * 10.;
+ l16_10.f[i__ + 4] = std::sqrt(5.) * (l2_13.x[i__ + 9] - l2_13.x[i__ + 14]);
+/* Computing 2nd power */
+ d__1 = l2_13.x[i__ + 4] - l2_13.x[i__ + 9] * 2.;
+ l16_10.f[i__ + 9] = d__1 * d__1;
+/* L8: */
+/* Computing 2nd power */
+ d__1 = l2_13.x[i__ - 1] - l2_13.x[i__ + 14];
+ l16_10.f[i__ + 14] = std::sqrt(10.) * (d__1 * d__1);
+ }
+ if (*mode == 2) {
+ goto labelL2;
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+ for (j = 1; j <= 20; ++j) {
+ l17_14.df[i__ + j * 198 - 199] = 0.;
+ if (j == i__) {
+ l17_14.df[i__ + j * 198 - 199] = 1.;
+ }
+ if (j == i__ + 5) {
+ l17_14.df[i__ + j * 198 - 199] = 10.;
+ }
+ l17_14.df[i__ + 5 + j * 198 - 199] = 0.;
+ if (j == i__ + 10) {
+ l17_14.df[i__ + 5 + j * 198 - 199] = std::sqrt(5.);
+ }
+ if (j == i__ + 15) {
+ l17_14.df[i__ + 5 + j * 198 - 199] = -std::sqrt(5.);
+ }
+ l17_14.df[i__ + 10 + j * 198 - 199] = 0.;
+ if (j == i__ + 5) {
+ l17_14.df[i__ + 10 + j * 198 - 199] = (l2_13.x[i__ + 4] -
+ l2_13.x[i__ + 9] * 2.) * 2.;
+ }
+ if (j == i__ + 10) {
+ l17_14.df[i__ + 10 + j * 198 - 199] = (l2_13.x[i__ + 4] -
+ l2_13.x[i__ + 9] * 2.) * -4.;
+ }
+ l17_14.df[i__ + 15 + j * 198 - 199] = 0.;
+ if (j == i__) {
+ l17_14.df[i__ + 15 + j * 198 - 199] = std::sqrt(10.) * 2. * (
+ l2_13.x[i__ - 1] - l2_13.x[i__ + 14]);
+ }
+/* labelL11: */
+ if (j == i__ + 15) {
+ l17_14.df[i__ + 15 + j * 198 - 199] = -std::sqrt(10.) * 2. * (
+ l2_13.x[i__ - 1] - l2_13.x[i__ + 14]);
+ }
+ }
+ }
+ for (j = 1; j <= 20; ++j) {
+ l4_13.gf[j - 1] = 0.;
+ for (i__ = 1; i__ <= 20; ++i__) {
+/* labelL10: */
+ l4_13.gf[j - 1] += l16_10.f[i__ - 1] * 2. * l17_14.df[i__ + j *
+ 198 - 199];
+ }
+ }
+labelL4:
+ return 0;
+} /* tp288_ */
+
+/* Subroutine */ int tp289_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+ Real pow_di( Real*, int*);
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 30;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 30; ++i__) {
+ l2_14.x[i__ - 1] = pow_di(&c_b2003, &i__) * ((Real) i__ / 30. +
+ 1.);
+ l20_15.xex[i__ - 1] = 0.;
+ l12_14.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_14.lxl[i__ - 1] = false;
+ }
+ l20_15.lex = true;
+ l20_15.nex = 1;
+ l20_15.fex = 0.;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 30; ++i__) {
+/* L7: */
+/* Computing 2nd power */
+ d__1 = l2_14.x[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ l6_1.fx = 1. - std::exp(-l6_1.fx / 60.);
+ if (*mode == 2) {
+ return 0;
+ }
+ for (i__ = 1; i__ <= 30; ++i__) {
+/* L8: */
+ l4_14.gf[i__ - 1] = (l6_1.fx - 1.) * (l2_14.x[i__ - 1] * -2. / 60.);
+ }
+labelL4:
+ return 0;
+} /* tp289_ */
+
+/* Subroutine */ int tp290_0_(int n__, int *mode)
+{
+ /* System generated locals */
+ int i__1;
+ Real d__1;
+
+ /* Local variables */
+ static int i__, j;
+
+ switch(n__) {
+ case 1: goto L_tp291;
+ case 2: goto L_tp292;
+ case 3: goto L_tp293;
+ }
+
+ l1_1.n = 2;
+ goto labelL10;
+
+L_tp291:
+ l1_1.n = 10;
+ goto labelL10;
+
+L_tp292:
+ l1_1.n = 30;
+ goto labelL10;
+
+L_tp293:
+ l1_1.n = 50;
+labelL10:
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL3;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ i__1 = l1_1.n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ l2_15.x[i__ - 1] = 1.;
+ l20_16.xex[i__ - 1] = 0.;
+ l12_15.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_15.lxl[i__ - 1] = false;
+ }
+ l20_16.fex = 0.;
+ l20_16.lex = true;
+ l20_16.nex = 1;
+ l15_1.lsum = 1;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l16_11.f[0];
+ l6_1.fx = d__1 * d__1;
+ return 0;
+labelL3:
+ l16_11.f[0] = 0.;
+ i__1 = l1_1.n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+/* L7: */
+/* Computing 2nd power */
+ d__1 = l2_15.x[i__ - 1];
+ l16_11.f[0] += (Real) i__ * (d__1 * d__1);
+ }
+ if (*mode == 2) {
+ goto labelL2;
+ }
+ i__1 = l1_1.n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+/* L8: */
+ l17_9.df[i__ - 1] = (Real) i__ * 2. * l2_15.x[i__ - 1];
+ }
+ i__1 = l1_1.n;
+ for (j = 1; j <= i__1; ++j) {
+/* labelL9: */
+ l4_15.gf[j - 1] = l16_11.f[0] * 2. * l17_9.df[j - 1];
+ }
+labelL4:
+ return 0;
+} /* tp290_ */
+
+/* Subroutine */ int tp290_(int *mode)
+{
+ return tp290_0_(0, mode);
+ }
+
+/* Subroutine */ int tp291_(int *mode)
+{
+ return tp290_0_(1, mode);
+ }
+
+/* Subroutine */ int tp292_(int *mode)
+{
+ return tp290_0_(2, mode);
+ }
+
+/* Subroutine */ int tp293_(int *mode)
+{
+ return tp290_0_(3, mode);
+ }
+
+
+/* Subroutine */ int tp294_0_(int n__, int *mode)
+{
+ /* System generated locals */
+ int i__1, i__2;
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__, j, k;
+
+ switch(n__) {
+ case 1: goto L_tp295;
+ case 2: goto L_tp296;
+ case 3: goto L_tp297;
+ case 4: goto L_tp298;
+ case 5: goto L_tp299;
+ }
+
+ l1_1.n = 6;
+ goto labelL20;
+
+L_tp295:
+ l1_1.n = 10;
+ goto labelL20;
+
+L_tp296:
+ l1_1.n = 16;
+ goto labelL20;
+
+L_tp297:
+ l1_1.n = 30;
+ goto labelL20;
+
+L_tp298:
+ l1_1.n = 50;
+ goto labelL20;
+
+L_tp299:
+ l1_1.n = 100;
+labelL20:
+ k = l1_1.n - 1;
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL3;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ i__1 = l1_1.n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ l2_16.x[i__ - 1] = -1.2;
+ l20_17.xex[i__ - 1] = 1.;
+ l12_16.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_16.lxl[i__ - 1] = false;
+ }
+ i__1 = l1_1.n / 2;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+/* L7: */
+ l2_16.x[(i__ << 1) - 1] = 1.;
+ }
+ l20_17.fex = 0.;
+ l20_17.lex = true;
+ l20_17.nex = 1;
+ l15_1.lsum = k << 1;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ i__1 = k;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+/* L8: */
+/* Computing 2nd power */
+ d__1 = l16_12.f[i__ - 1];
+/* Computing 2nd power */
+ d__2 = l16_12.f[i__ + k - 1];
+ l6_1.fx = l6_1.fx + d__1 * d__1 + d__2 * d__2;
+ }
+ l6_1.fx *= 1e-4;
+ return 0;
+labelL3:
+ i__1 = k;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+/* Computing 2nd power */
+ d__1 = l2_16.x[i__ - 1];
+ l16_12.f[i__ - 1] = (l2_16.x[i__] - d__1 * d__1) * 10.;
+/* labelL12: */
+ l16_12.f[i__ + k - 1] = 1. - l2_16.x[i__ - 1];
+ }
+ if (*mode == 2) {
+ goto labelL2;
+ }
+ i__1 = k;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ i__2 = l1_1.n;
+ for (j = 1; j <= i__2; ++j) {
+ l17_14.df[i__ + j * 198 - 199] = 0.;
+ if (j == i__) {
+ l17_14.df[i__ + j * 198 - 199] = l2_16.x[i__ - 1] * -20.;
+ }
+ if (j == i__ + 1) {
+ l17_14.df[i__ + j * 198 - 199] = 10.;
+ }
+ l17_14.df[i__ + k + j * 198 - 199] = 0.;
+/* labelL9: */
+ if (j == i__) {
+ l17_14.df[i__ + k + j * 198 - 199] = -1.;
+ }
+ }
+ }
+ i__2 = l1_1.n;
+ for (j = 1; j <= i__2; ++j) {
+ l4_16.gf[j - 1] = 0.;
+ i__1 = l15_1.lsum;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+/* labelL13: */
+ l4_16.gf[j - 1] += l16_12.f[i__ - 1] * 2. * l17_14.df[i__ + j *
+ 198 - 199];
+ }
+ l4_16.gf[j - 1] *= 1e-4;
+/* labelL11: */
+ }
+labelL4:
+ return 0;
+} /* tp294_ */
+
+/* Subroutine */ int tp294_(int *mode)
+{
+ return tp294_0_(0, mode);
+ }
+
+/* Subroutine */ int tp295_(int *mode)
+{
+ return tp294_0_(1, mode);
+ }
+
+/* Subroutine */ int tp296_(int *mode)
+{
+ return tp294_0_(2, mode);
+ }
+
+/* Subroutine */ int tp297_(int *mode)
+{
+ return tp294_0_(3, mode);
+ }
+
+/* Subroutine */ int tp298_(int *mode)
+{
+ return tp294_0_(4, mode);
+ }
+
+/* Subroutine */ int tp299_(int *mode)
+{
+ return tp294_0_(5, mode);
+ }
+
+
+/* Subroutine */ int tp300_0_(int n__, int *mode)
+{
+ /* System generated locals */
+ int i__1;
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+
+ switch(n__) {
+ case 1: goto L_tp301;
+ case 2: goto L_tp302;
+ }
+
+ l1_1.n = 20;
+ l20_17.fex = -20.;
+ goto labelL10;
+
+L_tp301:
+ l1_1.n = 50;
+ l20_17.fex = -50.;
+ goto labelL10;
+
+L_tp302:
+ l1_1.n = 100;
+ l20_17.fex = -100.;
+labelL10:
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ i__1 = l1_1.n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ l2_16.x[i__ - 1] = 0.;
+ l20_17.xex[i__ - 1] = (Real) (l1_1.n - i__) + 1.;
+ l12_16.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_16.lxl[i__ - 1] = false;
+ }
+ l20_17.lex = true;
+ l20_17.nex = 1;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_16.x[0];
+ l6_1.fx = d__1 * d__1 - l2_16.x[0] * 2.;
+ i__1 = l1_1.n;
+ for (i__ = 2; i__ <= i__1; ++i__) {
+/* L7: */
+/* Computing 2nd power */
+ d__1 = l2_16.x[i__ - 1];
+ l6_1.fx = l6_1.fx + d__1 * d__1 * 2. - l2_16.x[i__ - 2] * 2. *
+ l2_16.x[i__ - 1];
+ }
+ return 0;
+labelL3:
+ l4_16.gf[0] = l2_16.x[0] * 2. - l2_16.x[1] * 2. - 2.;
+ i__1 = l1_1.n - 1;
+ for (i__ = 2; i__ <= i__1; ++i__) {
+/* L8: */
+ l4_16.gf[i__ - 1] = l2_16.x[i__ - 1] * 4. - (l2_16.x[i__ - 2] +
+ l2_16.x[i__]) * 2.;
+ }
+ l4_16.gf[l1_1.n - 1] = l2_16.x[l1_1.n - 1] * 4. - l2_16.x[l1_1.n - 2] *
+ 2.;
+labelL4:
+ return 0;
+} /* tp300_ */
+
+/* Subroutine */ int tp300_(int *mode)
+{
+ return tp300_0_(0, mode);
+ }
+
+/* Subroutine */ int tp301_(int *mode)
+{
+ return tp300_0_(1, mode);
+ }
+
+/* Subroutine */ int tp302_(int *mode)
+{
+ return tp300_0_(2, mode);
+ }
+
+
+/* Subroutine */ int tp303_0_(int n__, int *mode)
+{
+ /* System generated locals */
+ int i__1;
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+ static Real pom;
+
+ switch(n__) {
+ case 1: goto L_tp304;
+ case 2: goto L_tp305;
+ }
+
+ l1_1.n = 20;
+ goto labelL10;
+
+L_tp304:
+ l1_1.n = 50;
+ goto labelL10;
+
+L_tp305:
+ l1_1.n = 100;
+labelL10:
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ i__1 = l1_1.n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ l2_16.x[i__ - 1] = .1;
+ l20_17.xex[i__ - 1] = 0.;
+ l12_16.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_16.lxl[i__ - 1] = false;
+ }
+ l20_17.lex = true;
+ l20_17.nex = 1;
+ l20_17.fex = 0.;
+ return 0;
+labelL2:
+ pom = 0.;
+ i__1 = l1_1.n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+/* L7: */
+ pom += (Real) i__ * .5 * l2_16.x[i__ - 1];
+ }
+/* Computing 2nd power */
+ d__1 = pom;
+/* Computing 4th power */
+ d__2 = pom, d__2 *= d__2;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ i__1 = l1_1.n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+/* L8: */
+/* Computing 2nd power */
+ d__1 = l2_16.x[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ i__1 = l1_1.n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+/* labelL9: */
+/* Computing 3rd power */
+ d__1 = pom;
+ l4_16.gf[i__ - 1] = l2_16.x[i__ - 1] * 2. + pom * (Real) i__ + (
+ Real) i__ * 2. * (d__1 * (d__1 * d__1));
+ }
+labelL4:
+ return 0;
+} /* tp303_ */
+
+/* Subroutine */ int tp303_(int *mode)
+{
+ return tp303_0_(0, mode);
+ }
+
+/* Subroutine */ int tp304_(int *mode)
+{
+ return tp303_0_(1, mode);
+ }
+
+/* Subroutine */ int tp305_(int *mode)
+{
+ return tp303_0_(2, mode);
+ }
+
+
+/* Subroutine */ int tp306_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static Real a, b;
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = 1.;
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l20_6.lex = false;
+ l20_6.nex = 2;
+ l20_6.fex = -1.1036;
+ l20_6.xex[0] = 0.;
+ l20_6.xex[1] = 1.;
+/* XEX(3)=.0D+0 */
+/* XEX(4)=-.1D+1 */
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l6_1.fx = -exp(-l2_1.x[0] - l2_1.x[1]) * (d__1 * d__1 * 2. + d__2 * d__2 *
+ 3.);
+ return 0;
+labelL3:
+ a = std::exp(-l2_1.x[0] - l2_1.x[1]);
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ b = d__1 * d__1 * 2. + d__2 * d__2 * 3.;
+ l4_1.gf[0] = a * (b - l2_1.x[0] * 4.);
+ l4_1.gf[1] = a * (b - l2_1.x[1] * 6.);
+labelL4:
+ return 0;
+} /* tp306_ */
+
+
+/* Subroutine */ int tp307_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+ static Real wi, xi1, xi2;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = (float).3;
+ l2_1.x[1] = (float).4;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = true;
+ l13_1.xl[i__ - 1] = (float)0.;
+ l12_1.lxu[i__ - 1] = true;
+ l14_20.xu[i__ - 1] = (float).26;
+/* labelL6: */
+ l20_1.xex[i__ - 1] = .25783;
+ }
+ l20_1.nex = 1;
+ l20_1.lex = false;
+/* FEX=0.12436*0.001 */
+ l20_1.fex = (float)0.;
+ l15_1.lsum = 10;
+ return 0;
+labelL2:
+ l6_1.fx = (float)0.;
+ goto labelL9;
+labelL3:
+ l4_1.gf[0] = (float)0.;
+ l4_1.gf[1] = (float)0.;
+labelL9:
+ for (i__ = 1; i__ <= 10; ++i__) {
+ wi = (Real) i__;
+ xi1 = wi * l2_1.x[0];
+ if (xi1 > 20.) {
+ xi1 = (float)0.;
+ }
+ xi2 = wi * l2_1.x[1];
+ if (xi2 > 20.) {
+ xi2 = (float)0.;
+ }
+ l16_4.f[i__ - 1] = wi * (float)2. + (float)2. - std::exp(xi1) - std::exp(xi2);
+ if (*mode == 2) {
+ goto L8;
+ }
+ l17_8.df[i__ - 1] = -wi * std::exp(xi1);
+ l17_8.df[i__ + 9] = -wi * std::exp(xi2);
+ l4_1.gf[0] += l16_4.f[i__ - 1] * (float)2. * l17_8.df[i__ - 1];
+ l4_1.gf[1] += l16_4.f[i__ - 1] * (float)2. * l17_8.df[i__ + 9];
+ goto L7;
+L8:
+/* Computing 2nd power */
+ d__1 = l16_4.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ l6_1.fx *= (float).001;
+L7:
+ ;
+ }
+ l4_1.gf[0] *= (float).001;
+ l4_1.gf[1] *= (float).001;
+labelL4:
+ return 0;
+} /* tp307_ */
+
+
+/* Subroutine */ int tp308_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 3.;
+ l2_1.x[1] = .1;
+ l12_1.lxu[0] = false;
+ l12_1.lxu[1] = false;
+ l11_1.lxl[0] = false;
+ l11_1.lxl[1] = false;
+ l20_1.lex = false;
+ l15_1.lsum = 3;
+ l20_1.nex = 1;
+ l20_1.fex = .77319906;
+ l20_1.xex[0] = -.15543724;
+ l20_1.xex[1] = .69456378;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l16_2.f[0] = d__1 * d__1 + d__2 * d__2 + l2_1.x[0] * l2_1.x[1];
+ l16_2.f[1] = std::sin(l2_1.x[0]);
+ l16_2.f[2] =std::cos(l2_1.x[1]);
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* labelL5: */
+/* Computing 2nd power */
+ d__1 = l16_2.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ l17_2.df[0] = l2_1.x[0] * 2. + l2_1.x[1];
+ l17_2.df[3] = l2_1.x[1] * 2. + l2_1.x[0];
+ l17_2.df[1] = std::sin(l2_1.x[0]) * 2. *std::cos(l2_1.x[0]);
+ l17_2.df[4] = 0.;
+ l17_2.df[2] = 0.;
+ l17_2.df[5] =std::cos(l2_1.x[1]) * -2. * std::sin(l2_1.x[1]);
+ l4_1.gf[0] = 0.;
+ l4_1.gf[1] = 0.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l4_1.gf[0] += l16_2.f[i__ - 1] * 2. * l17_2.df[i__ - 1];
+/* labelL6: */
+ l4_1.gf[1] += l16_2.f[i__ - 1] * 2. * l17_2.df[i__ + 2];
+ }
+labelL4:
+ return 0;
+} /* tp308_ */
+
+/* Subroutine */ int tp309_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = 0.;
+ l11_1.lxl[i__ - 1] = false;
+/* labelL5: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.xex[0] = 3.4826826;
+ l20_1.xex[1] = 3.9;
+ l20_1.fex = -3.9871708;
+labelL4:
+ return 0;
+labelL2:
+/* Computing 4th power */
+ d__1 = l2_1.x[0], d__1 *= d__1;
+/* Computing 3rd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__3 = l2_1.x[0];
+/* Computing 2nd power */
+ d__4 = l2_1.x[1] - 3.9;
+ l6_1.fx = d__1 * d__1 * 1.41 - d__2 * (d__2 * d__2) * 12.76 + d__3 * d__3
+ * 39.91 - l2_1.x[0] * 51.93 + 24.37 + d__4 * d__4;
+ return 0;
+labelL3:
+/* Computing 3rd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+ l4_1.gf[0] = d__1 * (d__1 * d__1) * 5.64 - d__2 * d__2 * 38.28 + l2_1.x[0]
+ * 79.82 - 51.93;
+ l4_1.gf[1] = l2_1.x[1] * 2. - 7.8;
+ return 0;
+} /* tp309_ */
+
+/* Subroutine */ int tp310_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5;
+
+ /* Local variables */
+ static Real a, b, c__;
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = -1.2;
+ l2_1.x[1] = 1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+/* L7: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l20_1.lex = false;
+ l20_1.nex = -1;
+ l20_1.fex = 0.;
+ l20_1.xex[0] = -1.2;
+ l20_1.xex[1] = 1.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] * l2_1.x[1];
+/* Computing 2nd power */
+ d__2 = 1. - l2_1.x[0];
+/* Computing 5th power */
+ d__4 = 1. - l2_1.x[0], d__5 = d__4, d__4 *= d__4;
+/* Computing 2nd power */
+ d__3 = 1. - l2_1.x[0] - l2_1.x[1] * (d__5 * (d__4 * d__4));
+ l6_1.fx = d__1 * d__1 * (d__2 * d__2) * (d__3 * d__3);
+ return 0;
+labelL3:
+ a = l2_1.x[0] * l2_1.x[1];
+ b = 1. - l2_1.x[0];
+/* Computing 5th power */
+ d__1 = b, d__2 = d__1, d__1 *= d__1;
+ c__ = b - l2_1.x[1] * (d__2 * (d__1 * d__1));
+/* Computing 4th power */
+ d__1 = b, d__1 *= d__1;
+ l4_1.gf[0] = a * 2. * b * c__ * (l2_1.x[1] - 1. - l2_1.x[1] * 5. * (d__1 *
+ d__1));
+/* Computing 5th power */
+ d__1 = b, d__2 = d__1, d__1 *= d__1;
+ l4_1.gf[1] = a * 2. * b * c__ * (l2_1.x[0] - d__2 * (d__1 * d__1));
+labelL4:
+ return 0;
+} /* tp310_ */
+
+/* Subroutine */ int tp311_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = 1.;
+ l11_1.lxl[i__ - 1] = false;
+/* labelL5: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l20_6.lex = false;
+ l20_6.nex = 2;
+ l20_6.xex[0] = 3.;
+ l20_6.xex[1] = 2.;
+ l20_6.xex[2] = 3.58443;
+ l20_6.xex[3] = -1.84813;
+ l20_6.fex = 0.;
+labelL4:
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+/* Computing 2nd power */
+ d__1 = d__2 * d__2 + l2_1.x[1] - 11.;
+/* Computing 2nd power */
+ d__4 = l2_1.x[1];
+/* Computing 2nd power */
+ d__3 = l2_1.x[0] + d__4 * d__4 - 7.;
+ l6_1.fx = d__1 * d__1 + d__3 * d__3;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l4_1.gf[0] = l2_1.x[0] * 4. * (d__1 * d__1 + l2_1.x[1] - 11.) + (l2_1.x[0]
+ + d__2 * d__2 - 7.) * 2.;
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l4_1.gf[1] = (d__1 * d__1 + l2_1.x[1] - 11.) * 2. + l2_1.x[1] * 4. * (
+ l2_1.x[0] + d__2 * d__2 - 7.);
+ return 0;
+} /* tp311_ */
+
+/* Subroutine */ int tp312_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static Real a, b;
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = 1.;
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = 0.;
+ l20_1.xex[0] = -21.026652;
+ l20_1.xex[1] = -36.760009;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ a = d__1 * d__1 + l2_1.x[1] * 12. - 1.;
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ b = (d__1 * d__1 + d__2 * d__2) * 49. + l2_1.x[0] * 84. + l2_1.x[1] *
+ 2324. - 681.;
+/* Computing 2nd power */
+ d__1 = a;
+/* Computing 2nd power */
+ d__2 = b;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ l6_1.fx *= 1e-4;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ a = d__1 * d__1 + l2_1.x[1] * 12. - 1.;
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ b = (d__1 * d__1 + d__2 * d__2) * 49. + l2_1.x[0] * 84. + l2_1.x[1] *
+ 2324. - 681.;
+ l4_1.gf[0] = (l2_1.x[0] * 2. * a + b * (l2_1.x[0] * 98. + 84.)) * 2. *
+ 1e-4;
+ l4_1.gf[1] = (a * 12. + b * (l2_1.x[1] * 98. + 2324.)) * 2. * 1e-4;
+labelL4:
+ return 0;
+} /* tp312_ */
+
+/* Subroutine */ int tp313_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+ static Real xh;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 0.;
+ l2_1.x[1] = -1.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = false;
+/* labelL5: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = .199786;
+ l20_1.xex[0] = 3.;
+ l20_1.xex[1] = 2.850214;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 3.;
+ l6_1.fx = d__1 * d__1 * 1e-4 - (l2_1.x[1] - l2_1.x[0]) + std::exp((l2_1.x[1] -
+ l2_1.x[0]) * 20.);
+ return 0;
+labelL3:
+ xh = std::exp((l2_1.x[1] - l2_1.x[0]) * 20.) * 20.;
+ l4_1.gf[0] = (l2_1.x[0] - 3.) * 2e-4 + 1. - xh;
+ l4_1.gf[1] = xh - 1.;
+labelL4:
+ return 0;
+} /* tp313_ */
+
+/* Subroutine */ int tp314_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static Real a, b;
+ static int i__;
+ static Real g1, h1;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = 2.;
+ l12_1.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_1.lxl[i__ - 1] = false;
+ }
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = .16904;
+ l20_1.xex[0] = 1.789039;
+ l20_1.xex[1] = 1.3740024;
+ return 0;
+labelL2:
+ a = l2_1.x[0] - 2.;
+ b = l2_1.x[1] - 1.;
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ g1 = d__1 * d__1 / -4. - d__2 * d__2 + 1.;
+ h1 = l2_1.x[0] - l2_1.x[1] * 2. + 1.;
+/* Computing 2nd power */
+ d__1 = a;
+/* Computing 2nd power */
+ d__2 = b;
+/* Computing 2nd power */
+ d__3 = h1;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + .04 / g1 + d__3 * d__3 / .2;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ g1 = d__1 * d__1 / -4. - d__2 * d__2 + 1.;
+ h1 = l2_1.x[0] - l2_1.x[1] * 2. + 1.;
+/* Computing 2nd power */
+ d__1 = g1;
+ l4_1.gf[0] = (l2_1.x[0] - 2. + l2_1.x[0] * .01 / (d__1 * d__1) + h1 * 5.)
+ * 2.;
+/* Computing 2nd power */
+ d__1 = g1;
+ l4_1.gf[1] = (l2_1.x[1] - 1. + l2_1.x[1] * .04 / (d__1 * d__1) - h1 * 10.)
+ * 2.;
+labelL4:
+ return 0;
+} /* tp314_ */
+
+/* Subroutine */ int tp315_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 1;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = -.1;
+ l2_1.x[1] = -.9;
+ l12_1.lxu[0] = false;
+ l12_1.lxu[1] = false;
+ l11_1.lxl[0] = false;
+ l11_1.lxl[1] = false;
+ l5_3.gg[0] = 1.;
+ l5_3.gg[3] = -2.;
+ l4_1.gf[0] = 0.;
+ l4_1.gf[1] = -1.;
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = -.8;
+ l20_1.xex[0] = .6;
+ l20_1.xex[1] = .8;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_1.x[1];
+labelL3:
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = 1. - l2_1.x[1] * 2. + l2_1.x[0];
+ }
+ if (l9_4.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_3.g[1] = d__1 * d__1 + d__2 * d__2;
+ }
+ if (l9_4.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_3.g[2] = 1. - d__1 * d__1 - d__2 * d__2;
+ }
+ return 0;
+labelL5:
+ if (! l10_4.index2[1]) {
+ goto labelL6;
+ }
+ l5_3.gg[1] = l2_1.x[0] * 2.;
+ l5_3.gg[4] = l2_1.x[1] * 2.;
+labelL6:
+ if (! l10_4.index2[2]) {
+ goto L7;
+ }
+ l5_3.gg[2] = l2_1.x[0] * -2.;
+ l5_3.gg[5] = l2_1.x[1] * -2.;
+L7:
+ return 0;
+} /* tp315_ */
+
+/* Subroutine */ int tp316_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = 0.;
+ l11_1.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = 334.31458;
+ l20_1.xex[0] = 7.0710678;
+ l20_1.xex[1] = -7.0710678;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 20.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1] + 20.;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ return 0;
+labelL3:
+ l4_1.gf[0] = l2_1.x[0] * 2. - 40.;
+ l4_1.gf[1] = l2_1.x[1] * 2. + 40.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_1.g[0] = d__1 * d__1 * .01 + d__2 * d__2 * .01 - 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ return 0;
+ }
+ l5_1.gg[0] = l2_1.x[0] * .02;
+ l5_1.gg[1] = l2_1.x[1] * .02;
+ return 0;
+} /* tp316_ */
+
+/* Subroutine */ int tp317_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = 0.;
+ l11_1.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = 372.46661;
+ l20_1.xex[0] = 7.3519262;
+ l20_1.xex[1] = -5.422866;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 20.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1] + 20.;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ return 0;
+labelL3:
+ l4_1.gf[0] = l2_1.x[0] * 2. - 40.;
+ l4_1.gf[1] = l2_1.x[1] * 2. + 40.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_1.g[0] = d__1 * d__1 * .01 + d__2 * d__2 / 64. - 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ return 0;
+ }
+ l5_1.gg[0] = l2_1.x[0] * .02;
+ l5_1.gg[1] = l2_1.x[1] * 2. / 64.;
+ return 0;
+} /* tp317_ */
+
+/* Subroutine */ int tp318_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = 0.;
+ l11_1.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = 412.75005;
+ l20_1.xex[0] = 7.8091266;
+ l20_1.xex[1] = -3.7478414;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 20.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1] + 20.;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ return 0;
+labelL3:
+ l4_1.gf[0] = l2_1.x[0] * 2. - 40.;
+ l4_1.gf[1] = l2_1.x[1] * 2. + 40.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_1.g[0] = d__1 * d__1 * .01 + d__2 * d__2 / 36. - 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ return 0;
+ }
+ l5_1.gg[0] = l2_1.x[0] * .02;
+ l5_1.gg[1] = l2_1.x[1] * 2. / 36.;
+ return 0;
+} /* tp318_ */
+
+/* Subroutine */ int tp319_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = 0.;
+ l11_1.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = 452.4044;
+ l20_1.xex[0] = 8.4922857;
+ l20_1.xex[1] = -2.1121017;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 20.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1] + 20.;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ return 0;
+labelL3:
+ l4_1.gf[0] = l2_1.x[0] * 2. - 40.;
+ l4_1.gf[1] = l2_1.x[1] * 2. + 40.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_1.g[0] = d__1 * d__1 * .01 + d__2 * d__2 / 16. - 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ return 0;
+ }
+ l5_1.gg[0] = l2_1.x[0] * .02;
+ l5_1.gg[1] = l2_1.x[1] * 2. / 16.;
+ return 0;
+} /* tp319_ */
+
+/* Subroutine */ int tp320_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = 0.;
+ l11_1.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = 485.53146;
+ l20_1.xex[0] = 9.39525;
+ l20_1.xex[1] = -.68459019;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 20.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1] + 20.;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ return 0;
+labelL3:
+ l4_1.gf[0] = l2_1.x[0] * 2. - 40.;
+ l4_1.gf[1] = l2_1.x[1] * 2. + 40.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_1.g[0] = d__1 * d__1 * .01 + d__2 * d__2 / 4. - 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ return 0;
+ }
+ l5_1.gg[0] = l2_1.x[0] * .02;
+ l5_1.gg[1] = l2_1.x[1] * .5;
+ return 0;
+} /* tp320_ */
+
+/* Subroutine */ int tp321_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = 0.;
+ l11_1.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = 496.11237;
+ l20_1.xex[0] = 9.8160292;
+ l20_1.xex[1] = -.19093377;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 20.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1] + 20.;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ return 0;
+labelL3:
+ l4_1.gf[0] = l2_1.x[0] * 2. - 40.;
+ l4_1.gf[1] = l2_1.x[1] * 2. + 40.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_1.g[0] = d__1 * d__1 * .01 + d__2 * d__2 - 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ return 0;
+ }
+ l5_1.gg[0] = l2_1.x[0] * .02;
+ l5_1.gg[1] = l2_1.x[1] * 2.;
+ return 0;
+} /* tp321_ */
+
+
+/* Subroutine */ int tp322_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = (float)1e-4;
+ l11_1.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = 499.96001;
+ l20_1.xex[0] = 9.9980018;
+ l20_1.xex[1] = -.0019990011;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - (float)20.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1] + (float)20.;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2;
+ return 0;
+labelL3:
+ l4_1.gf[0] = l2_1.x[0] * 2. - 40.;
+ l4_1.gf[1] = l2_1.x[1] * 2. + 40.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_1.g[0] = d__1 * d__1 * .01 + d__2 * d__2 * 100. - 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ return 0;
+ }
+ l5_1.gg[0] = l2_1.x[0] * .02;
+ l5_1.gg[1] = l2_1.x[1] * 200.;
+ return 0;
+} /* tp322_ */
+
+/* Subroutine */ int tp323_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 1;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 0.;
+ l2_1.x[1] = 1.;
+ l11_1.lxl[0] = true;
+ l13_1.xl[0] = 0.;
+ l11_1.lxl[1] = true;
+ l13_1.xl[1] = 0.;
+ l12_1.lxu[0] = false;
+ l12_1.lxu[1] = false;
+ l5_2.gg[0] = 1.;
+ l5_2.gg[2] = -1.;
+ l5_2.gg[3] = 1.;
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = 3.7989446;
+ l20_1.xex[0] = .55357378;
+ l20_1.xex[1] = 1.3064439;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 - l2_1.x[0] * 4. + 4.;
+ return 0;
+labelL3:
+ l4_1.gf[0] = l2_1.x[0] * 2. - 4.;
+ l4_1.gf[1] = l2_1.x[1] * 2.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = l2_1.x[0] - l2_1.x[1] + 2.;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l3_2.g[1] = d__1 * d__1 * -1. + l2_1.x[1] - 1.;
+ }
+ return 0;
+labelL5:
+ if (l10_3.index2[1]) {
+ l5_2.gg[1] = l2_1.x[0] * -2.;
+ }
+ return 0;
+} /* tp323_ */
+
+/* Subroutine */ int tp324_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = 2.;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l11_1.lxl[0] = true;
+ l11_1.lxl[1] = false;
+ l13_1.xl[0] = 2.;
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = 5.;
+ l20_1.xex[0] = 15.811389;
+ l20_1.xex[1] = 1.5811387;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l6_1.fx = d__1 * d__1 * .01 + d__2 * d__2;
+ return 0;
+labelL3:
+ l4_1.gf[0] = l2_1.x[0] * .02;
+ l4_1.gf[1] = l2_1.x[1] * 2.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = l2_1.x[0] * l2_1.x[1] - 25.;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_2.g[1] = d__1 * d__1 + d__2 * d__2 - 25.;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+ l5_2.gg[0] = l2_1.x[1];
+ l5_2.gg[2] = l2_1.x[0];
+L7:
+ if (! l10_3.index2[1]) {
+ return 0;
+ }
+ l5_2.gg[1] = l2_1.x[0] * 2.;
+ l5_2.gg[3] = l2_1.x[1] * 2.;
+ return 0;
+} /* tp324_ */
+
+/* Subroutine */ int tp325_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 1;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ l2_1.x[0] = -3.;
+ l2_1.x[1] = 0.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l5_3.gg[i__ * 3 - 3] = -1.;
+ l11_1.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_1.lxu[i__ - 1] = false;
+ }
+ l5_3.gg[1] = -1.;
+ l4_1.gf[1] = 1.;
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = 3.7913415;
+ l20_1.xex[0] = -2.3722813;
+ l20_1.xex[1] = -1.8363772;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l6_1.fx = d__1 * d__1 + l2_1.x[1];
+ return 0;
+labelL3:
+ l4_1.gf[0] = l2_1.x[0] * 2.;
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = -(l2_1.x[0] + l2_1.x[1]) + 1.;
+ }
+ if (l9_4.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+ l3_3.g[1] = -(l2_1.x[0] + d__1 * d__1) + 1.;
+ }
+ if (l9_4.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l3_3.g[2] = d__1 * d__1 + d__2 * d__2 - 9.;
+ }
+ return 0;
+labelL5:
+ if (l10_4.index2[1]) {
+ l5_3.gg[4] = l2_1.x[1] * -2.;
+ }
+ if (! l10_4.index2[2]) {
+ return 0;
+ }
+ l5_3.gg[2] = l2_1.x[0] * 2.;
+ l5_3.gg[5] = l2_1.x[1] * 2.;
+ return 0;
+} /* tp325_ */
+
+/* Subroutine */ int tp326_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = 4.;
+ l2_1.x[1] = 3.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_1.lxl[i__ - 1] = true;
+ l12_1.lxu[i__ - 1] = true;
+ l14_1.xu[i__ - 1] = (float)10.;
+/* labelL6: */
+ l13_1.xl[i__ - 1] = 0.;
+ }
+ l5_2.gg[2] = -4.;
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = -79.807821;
+ l20_1.xex[0] = 5.2396091;
+ l20_1.xex[1] = 3.7460378;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 - l2_1.x[0] * 16. - l2_1.x[1] * 10.;
+ return 0;
+labelL3:
+ l4_1.gf[0] = l2_1.x[0] * 2. - 16.;
+ l4_1.gf[1] = l2_1.x[1] * 2. - 10.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l3_2.g[0] = 11. - d__1 * d__1 + l2_1.x[0] * 6. - l2_1.x[1] * 4.;
+ }
+ if (l9_3.index1[1]) {
+ l3_2.g[1] = l2_1.x[0] * l2_1.x[1] - l2_1.x[1] * 3. - std::exp(l2_1.x[0] -
+ 3.) + 1.;
+ }
+ return 0;
+labelL5:
+ if (l10_3.index2[0]) {
+ l5_2.gg[0] = l2_1.x[0] * -2. + 6.;
+ }
+ if (! l10_3.index2[1]) {
+ return 0;
+ }
+ l5_2.gg[1] = l2_1.x[1] - std::exp(l2_1.x[0] - 3.);
+ l5_2.gg[3] = l2_1.x[0] - 3.;
+ return 0;
+} /* tp326_ */
+
+
+/* Subroutine */ int tp327_(int *mode)
+{
+ /* Initialized data */
+
+ static Real y[44] = { .49,.49,.48,.47,.48,.47,.46,.46,.45,.43,.45,
+ .43,.43,.44,.43,.43,.46,.45,.42,.42,.43,.41,.41,.4,.42,.4,.4,.41,
+ .4,.41,.41,.4,.4,.4,.38,.41,.4,.4,.41,.38,.4,.4,.39,.39 };
+ static Real z__[44] = { 8.,8.,10.,10.,10.,10.,12.,12.,12.,12.,14.,
+ 14.,14.,16.,16.,16.,18.,18.,20.,20.,20.,22.,22.,22.,24.,24.,24.,
+ 26.,26.,26.,28.,28.,30.,30.,30.,32.,32.,34.,36.,36.,38.,38.,40.,
+ 42. };
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = .42;
+ l2_1.x[1] = 5.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l12_1.lxu[i__ - 1] = false;
+ l11_1.lxl[i__ - 1] = true;
+/* labelL12: */
+ l13_1.xl[i__ - 1] = .4;
+ }
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = .030646306;
+ l20_1.xex[0] = .42190424;
+ l20_1.xex[1] = 5.0000526;
+ l15_1.lsum = 44;
+ return 0;
+labelL2:
+ for (i__ = 1; i__ <= 44; ++i__) {
+/* labelL6: */
+ l16_13.f[i__ - 1] = y[i__ - 1] - l2_1.x[0] - (.49 - l2_1.x[0]) * std::exp(
+ -l2_1.x[1] * (z__[i__ - 1] - 8.));
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 44; ++i__) {
+/* L7: */
+/* Computing 2nd power */
+ d__1 = l16_13.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ l4_1.gf[0] = 0.;
+ l4_1.gf[1] = 0.;
+ for (i__ = 1; i__ <= 44; ++i__) {
+ l17_15.df[i__ - 1] = std::exp(-l2_1.x[1] * (z__[i__ - 1] - 8.)) - 1.;
+ l17_15.df[i__ + 43] = (.49 - l2_1.x[0]) * std::exp(-l2_1.x[1] * (z__[i__ -
+ 1] - 8.)) * (z__[i__ - 1] - 8.);
+ l4_1.gf[0] += l17_15.df[i__ - 1] * l16_13.f[i__ - 1] * 2.;
+/* L8: */
+ l4_1.gf[1] += l17_15.df[i__ + 43] * l16_13.f[i__ - 1] * 2.;
+ }
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = -.09 - l2_1.x[0] * l2_1.x[1] + l2_1.x[1] * .49;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ return 0;
+ }
+ l5_1.gg[0] = -l2_1.x[1];
+ l5_1.gg[1] = .49 - l2_1.x[0];
+ return 0;
+} /* tp327_ */
+
+/* Subroutine */ int tp328_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5;
+
+ /* Local variables */
+ static Real a, b;
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = .5;
+ l12_1.lxu[i__ - 1] = true;
+ l14_1.xu[i__ - 1] = 3.;
+ l11_1.lxl[i__ - 1] = true;
+/* labelL6: */
+ l13_1.xl[i__ - 1] = 1.;
+ }
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = 1.744152;
+ l20_1.xex[0] = 1.743439;
+ l20_1.xex[1] = 2.0297056;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+ a = (d__1 * d__1 + 1.) / (d__2 * d__2);
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] * l2_1.x[1];
+/* Computing 4th power */
+ d__2 = l2_1.x[0] * l2_1.x[1], d__2 *= d__2;
+ b = (d__1 * d__1 + 100.) / (d__2 * d__2);
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l6_1.fx = (d__1 * d__1 + 12. + a + b) / 10.;
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+/* Computing 3rd power */
+ d__2 = l2_1.x[0];
+ a = (d__1 * d__1 + 1.) / (d__2 * (d__2 * d__2));
+/* Computing 3rd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+/* Computing 5th power */
+ d__3 = l2_1.x[0], d__4 = d__3, d__3 *= d__3;
+/* Computing 4th power */
+ d__5 = l2_1.x[1], d__5 *= d__5;
+ b = 1. / (d__1 * (d__1 * d__1) * (d__2 * d__2)) + 200. / (d__4 * (d__3 *
+ d__3) * (d__5 * d__5));
+ l4_1.gf[0] = (l2_1.x[0] - a - b) / 5.;
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 3rd power */
+ d__2 = l2_1.x[1];
+/* Computing 4th power */
+ d__3 = l2_1.x[0], d__3 *= d__3;
+/* Computing 5th power */
+ d__4 = l2_1.x[1], d__5 = d__4, d__4 *= d__4;
+ a = 1. / (d__1 * d__1 * (d__2 * (d__2 * d__2))) + 200. / (d__3 * d__3 * (
+ d__5 * (d__4 * d__4)));
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+ l4_1.gf[1] = (l2_1.x[1] / (d__1 * d__1) - a) / 5.;
+labelL4:
+ return 0;
+} /* tp328_ */
+
+/* Subroutine */ int tp329_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 3;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l11_1.lxl[0] = true;
+ l11_1.lxl[1] = true;
+ l12_1.lxu[0] = true;
+ l12_1.lxu[1] = true;
+ l2_1.x[0] = 14.35;
+ l2_1.x[1] = 8.6;
+ l13_1.xl[0] = 13.;
+ l13_1.xl[1] = 0.;
+ l14_1.xu[0] = 16.;
+ l14_1.xu[1] = 15.;
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = -6961.8139;
+ l20_1.xex[0] = 14.095;
+ l20_1.xex[1] = .84296079;
+ return 0;
+labelL2:
+/* Computing 3rd power */
+ d__1 = l2_1.x[0] - 10.;
+/* Computing 3rd power */
+ d__2 = l2_1.x[1] - 20.;
+ l6_1.fx = d__1 * (d__1 * d__1) + d__2 * (d__2 * d__2);
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 10.;
+ l4_1.gf[0] = d__1 * d__1 * 3.;
+/* Computing 2nd power */
+ d__1 = l2_1.x[1] - 20.;
+ l4_1.gf[1] = d__1 * d__1 * 3.;
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 5.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1] - 5.;
+ l3_3.g[0] = d__1 * d__1 + d__2 * d__2 - 100.;
+ }
+ if (l9_4.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 6.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1] - 5.;
+ l3_3.g[1] = d__1 * d__1 + d__2 * d__2;
+ }
+ if (l9_4.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_1.x[0] - 6.;
+/* Computing 2nd power */
+ d__2 = l2_1.x[1] - 5.;
+ l3_3.g[2] = 82.81 - d__1 * d__1 - d__2 * d__2;
+ }
+ return 0;
+labelL5:
+ if (! l10_4.index2[0]) {
+ goto labelL6;
+ }
+ l5_3.gg[0] = l2_1.x[0] * 2. - 10.;
+ l5_3.gg[3] = l2_1.x[1] * 2. - 10.;
+labelL6:
+ if (! l10_4.index2[1]) {
+ goto L7;
+ }
+ l5_3.gg[1] = l2_1.x[0] * 2. - 12.;
+ l5_3.gg[4] = l2_1.x[1] * 2. - 10.;
+L7:
+ if (! l10_4.index2[2]) {
+ return 0;
+ }
+ l5_3.gg[2] = l2_1.x[0] * -2. + 12.;
+ l5_3.gg[5] = l2_1.x[1] * -2. + 10.;
+ return 0;
+} /* tp329_ */
+
+/* Subroutine */ int tp330_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = 2.5;
+ l11_1.lxl[i__ - 1] = true;
+ l12_1.lxu[i__ - 1] = true;
+ l13_1.xl[i__ - 1] = 0.;
+/* labelL6: */
+ l14_1.xu[i__ - 1] = 5.;
+ }
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = 1.6205833;
+ l20_1.xex[0] = 1.2866773;
+ l20_1.xex[1] = .53046181;
+ return 0;
+labelL2:
+/* Computing 3rd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+/* Computing 3rd power */
+ d__3 = l2_1.x[1];
+ l6_1.fx = d__1 * (d__1 * d__1) * .044 / (d__2 * d__2) + 1. / l2_1.x[0] +
+ l2_1.x[0] * .0592 / (d__3 * (d__3 * d__3));
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_1.x[0];
+/* Computing 2nd power */
+ d__2 = l2_1.x[1];
+/* Computing 2nd power */
+ d__3 = 1 / l2_1.x[0];
+/* Computing 3rd power */
+ d__4 = l2_1.x[1];
+ l4_1.gf[0] = d__1 * d__1 * .132 / (d__2 * d__2) - d__3 * d__3 + .0592 / (
+ d__4 * (d__4 * d__4));
+/* Computing 3rd power */
+ d__1 = l2_1.x[0];
+/* Computing 3rd power */
+ d__2 = l2_1.x[1];
+/* Computing 4th power */
+ d__3 = l2_1.x[1], d__3 *= d__3;
+ l4_1.gf[1] = d__1 * (d__1 * d__1) * -.088 / (d__2 * (d__2 * d__2)) -
+ l2_1.x[0] * .1776 / (d__3 * d__3);
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 3rd power */
+ d__1 = l2_1.x[1];
+ l3_1.g[0] = 1. - d__1 * (d__1 * d__1) * 8.62 / l2_1.x[0];
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ return 0;
+ }
+/* Computing 3rd power */
+ d__1 = l2_1.x[1];
+/* Computing 2nd power */
+ d__2 = l2_1.x[0];
+ l5_1.gg[0] = d__1 * (d__1 * d__1) * 8.62 / (d__2 * d__2);
+/* Computing 2nd power */
+ d__1 = l2_1.x[1];
+ l5_1.gg[1] = d__1 * d__1 * -25.86 / l2_1.x[0];
+ return 0;
+} /* tp330_ */
+
+
+/* Subroutine */ int tp331_(int *mode)
+{
+
+ /* Local variables */
+ static Real a, b, c__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 1;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_1.x[0] = (float).5;
+ l2_1.x[1] = (float).1;
+ l12_1.lxu[0] = true;
+ l12_1.lxu[1] = true;
+ l14_1.xu[0] = (float).7;
+ l14_1.xu[1] = (float).2;
+ l11_1.lxl[0] = true;
+ l13_1.xl[0] = (float).3;
+ l11_1.lxl[1] = true;
+ l13_1.xl[1] = (float).1;
+ l5_1.gg[0] = -1.;
+ l5_1.gg[1] = -1.;
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = 4.258;
+ l20_1.xex[0] = .6175;
+ l20_1.xex[1] = .1039;
+ return 0;
+labelL2:
+ l6_1.fx = std::log(std::log(l2_1.x[1]) * (float)2. / std::log(l2_1.x[0] + l2_1.x[1])) /
+ l2_1.x[0];
+ return 0;
+labelL3:
+ a = l2_1.x[0] + l2_1.x[1];
+ b = std::log(a);
+ c__ = std::log(l2_1.x[1]) * (float)2.;
+ l4_1.gf[0] = -1. / l2_1.x[0] * (std::log(c__ / b) / l2_1.x[0] + 1. / (b * a));
+ l4_1.gf[1] = (b * 2. / l2_1.x[1] - c__ / a) / (c__ * b * l2_1.x[0]);
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_1.g[0] = (float)1. - l2_1.x[0] - l2_1.x[1];
+ }
+labelL5:
+ return 0;
+} /* tp331_ */
+
+
+/* Subroutine */ int tp332_(int *mode)
+{
+ /* Initialized data */
+
+ static Real pi = 3.141592653;
+
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static Real pbig, a, b, c__;
+ static int i__;
+ static Real tr, pangle, pim, xxx, yyy;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL3;
+ }
+labelL1:
+ l1_1.n = 2;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l2_1.x[i__ - 1] = .75;
+ l12_1.lxu[i__ - 1] = true;
+ l14_1.xu[i__ - 1] = 1.5;
+ l11_1.lxl[i__ - 1] = true;
+/* labelL6: */
+ l13_1.xl[i__ - 1] = 0.;
+ }
+ l20_1.lex = false;
+ l20_1.nex = 1;
+ l20_1.fex = 114.95015;
+ l20_1.xex[0] = .91139872;
+ l20_1.xex[1] = .029280207;
+labelL3:
+ return 0;
+labelL2:
+ pim = pi / 3.6;
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 100; ++i__) {
+ tr = pi * (((Real) i__ - 1.) / 180. + .33333333333333331);
+ a = std::log(tr);
+ b = std::sin(tr);
+ c__ =std::cos(tr);
+ xxx = (a + l2_1.x[1]) * b + l2_1.x[0] * c__;
+ yyy = (a + l2_1.x[1]) * c__ - l2_1.x[0] * b;
+/* L7: */
+/* Computing 2nd power */
+ d__1 = xxx;
+/* Computing 2nd power */
+ d__2 = yyy;
+ l6_1.fx += pim * (d__1 * d__1 + d__2 * d__2);
+ }
+ return 0;
+labelL4:
+ pbig = -360.;
+ pim = 180. / pi;
+ for (i__ = 1; i__ <= 100; ++i__) {
+ tr = pi * (((Real) i__ - 1.) / 180. + .33333333333333331);
+ a = 1. / tr - l2_1.x[0];
+ b = std::log(tr) + l2_1.x[1];
+ pangle = pim * std::atan((d__1 = a / b, std::abs(d__1)));
+/* L8: */
+ if (pangle > pbig) {
+ pbig = pangle;
+ }
+ }
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = 30. - pbig;
+ }
+ if (l9_3.index1[1]) {
+ l3_2.g[1] = pbig + 30.;
+ }
+ return 0;
+} /* tp332_ */
+
+
+/* Subroutine */ int tp333_(int *mode)
+{
+ /* Initialized data */
+
+ static Real a[8] = { 4.,5.75,7.5,24.,32.,48.,72.,96. };
+ static Real y[8] = { 72.1,65.6,55.9,17.1,9.8,4.5,1.3,.6 };
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_2.x[0] = 30.;
+ l2_2.x[1] = .04;
+ l2_2.x[2] = 3.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l12_2.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_2.lxl[i__ - 1] = false;
+ }
+ l11_2.lxl[1] = true;
+ l13_2.xl[1] = (float)0.;
+ l12_2.lxu[1] = true;
+ l14_2.xu[1] = (float).07;
+ l20_3.lex = false;
+ l20_3.nex = 1;
+ l20_3.fex = (float)43.200000000000003;
+ l20_3.xex[0] = 89.902;
+ l20_3.xex[1] = .06699;
+ l20_3.xex[2] = .47809;
+ l15_1.lsum = 8;
+ return 0;
+labelL2:
+ for (i__ = 1; i__ <= 8; ++i__) {
+/* L7: */
+ l16_14.f[i__ - 1] = (y[i__ - 1] - l2_2.x[0] * std::exp(-l2_2.x[1] * a[i__
+ - 1]) - l2_2.x[2]) / y[i__ - 1];
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 8; ++i__) {
+/* L8: */
+/* Computing 2nd power */
+ d__1 = l16_14.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ l6_1.fx *= (float)1e3;
+ return 0;
+labelL3:
+ l4_2.gf[0] = 0.;
+ l4_2.gf[1] = 0.;
+ l4_2.gf[2] = 0.;
+ for (i__ = 1; i__ <= 8; ++i__) {
+ l17_16.df[i__ - 1] = -exp(-l2_2.x[1] * a[i__ - 1]) / y[i__ - 1];
+ l17_16.df[i__ + 7] = l2_2.x[0] * a[i__ - 1] * std::exp(-l2_2.x[1] * a[i__
+ - 1]) / y[i__ - 1];
+ l17_16.df[i__ + 15] = -1. / y[i__ - 1];
+ l4_2.gf[0] += l17_16.df[i__ - 1] * l16_14.f[i__ - 1] * 2.;
+ l4_2.gf[1] += l17_16.df[i__ + 7] * l16_14.f[i__ - 1] * 2.;
+/* labelL9: */
+ l4_2.gf[2] += l17_16.df[i__ + 15] * l16_14.f[i__ - 1] * 2.;
+ }
+ for (i__ = 1; i__ <= 8; ++i__) {
+ l4_2.gf[i__ - 1] *= (float)1e3;
+ }
+labelL4:
+ return 0;
+} /* tp333_ */
+
+
+/* Subroutine */ int tp334_(int *mode)
+{
+ /* Initialized data */
+
+ static Real y[15] = { .14,.18,.22,.25,.29,.32,.35,.39,.37,.58,.73,
+ .96,1.34,2.1,4.39 };
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+ static Real ui, vi, wi;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l15_1.lsum = 15;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 1.;
+ l11_2.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = false;
+ }
+ l20_3.lex = false;
+ l20_3.nex = 1;
+ l20_3.xex[0] = .082481481;
+ l20_3.xex[1] = 1.1354102;
+ l20_3.xex[2] = 2.3413942;
+ l20_3.fex = .0082149184;
+ return 0;
+labelL2:
+ for (i__ = 1; i__ <= 15; ++i__) {
+ ui = (Real) i__;
+ vi = (Real) (16 - i__);
+ wi = std::min(ui,vi);
+/* L7: */
+ l16_15.f[i__ - 1] = y[i__ - 1] - (l2_2.x[0] + i__ / (l2_2.x[1] * vi +
+ l2_2.x[2] * wi));
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 15; ++i__) {
+/* labelL10: */
+/* Computing 2nd power */
+ d__1 = l16_15.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L8: */
+ l4_2.gf[i__ - 1] = 0.;
+ }
+ for (i__ = 1; i__ <= 15; ++i__) {
+ ui = (Real) i__;
+ vi = (Real) (16 - i__);
+ wi = std::min(ui,vi);
+ l17_17.df[i__ - 1] = -1.;
+/* Computing 2nd power */
+ d__1 = l2_2.x[1] * vi + l2_2.x[2] * wi;
+ l17_17.df[i__ + 14] = ui * vi / (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = l2_2.x[1] * vi + l2_2.x[2] * wi;
+ l17_17.df[i__ + 29] = ui * wi / (d__1 * d__1);
+ l4_2.gf[0] += l17_17.df[i__ - 1] * l16_15.f[i__ - 1] * 2.;
+ l4_2.gf[1] += l17_17.df[i__ + 14] * l16_15.f[i__ - 1] * 2.;
+/* labelL9: */
+ l4_2.gf[2] += l17_17.df[i__ + 29] * l16_15.f[i__ - 1] * 2.;
+ }
+labelL4:
+ return 0;
+} /* tp334_ */
+
+/* Subroutine */ int tp335_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 2;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 1.;
+ l11_2.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = false;
+ }
+ l5_3.gg[4] = -1.;
+ l5_3.gg[5] = 1.;
+ l20_3.lex = false;
+ l20_3.nex = 1;
+ l20_3.xex[0] = 2.0309475e-6;
+ l20_3.xex[1] = .0044721349;
+ l20_3.xex[2] = .0020000032;
+ l20_3.fex = -.004472137;
+ return 0;
+labelL2:
+ l6_1.fx = -(l2_2.x[0] * .001 + l2_2.x[1]);
+ return 0;
+labelL3:
+ l4_2.gf[0] = -.001;
+ l4_2.gf[1] = -1.;
+ l4_2.gf[2] = 0.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+ l3_2.g[0] = d__1 * d__1 * 1e3 + d__2 * d__2 * 100. - l2_2.x[2];
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+ l3_2.g[1] = d__1 * d__1 * 100. + d__2 * d__2 * 400. + l2_2.x[2] - .01;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+ l5_3.gg[0] = l2_2.x[0] * 2e3;
+ l5_3.gg[2] = l2_2.x[1] * 200.;
+ if (! l10_3.index2[1]) {
+ goto L7;
+ }
+ l5_3.gg[1] = l2_2.x[0] * 200.;
+ l5_3.gg[3] = l2_2.x[1] * 800.;
+L7:
+ return 0;
+} /* tp335_ */
+
+/* Subroutine */ int tp336_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 1;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 0.;
+ l12_2.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_2.lxl[i__ - 1] = false;
+ }
+ l5_3.gg[0] = 5.;
+ l5_3.gg[2] = 5.;
+ l5_3.gg[4] = -3.;
+ l20_3.lex = false;
+ l20_3.nex = 1;
+ l20_3.xex[0] = .53459441;
+ l20_3.xex[1] = .53397092;
+ l20_3.xex[2] = -.21905778;
+ l20_3.fex = -.33789573;
+ return 0;
+labelL2:
+ l6_1.fx = l2_2.x[0] * 7. - l2_2.x[1] * 6. + l2_2.x[2] * 4.;
+ return 0;
+labelL3:
+ l4_2.gf[0] = 7.;
+ l4_2.gf[1] = -6.;
+ l4_2.gf[2] = 4.;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = l2_2.x[0] * 5. + l2_2.x[1] * 5. - l2_2.x[2] * 3. - 6.;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2];
+ l3_2.g[1] = d__1 * d__1 + d__2 * d__2 * 2. + d__3 * d__3 * 3. - 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+ l5_3.gg[1] = l2_2.x[0] * 2.;
+ l5_3.gg[3] = l2_2.x[1] * 4.;
+ l5_3.gg[5] = l2_2.x[2] * 6.;
+L8:
+ return 0;
+} /* tp336_ */
+
+/* Subroutine */ int tp337_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* labelL6: */
+ l2_2.x[i__ - 1] = 1.;
+ }
+ l12_2.lxu[0] = false;
+ l11_2.lxl[0] = false;
+ l12_2.lxu[1] = false;
+ l11_2.lxl[1] = true;
+ l12_2.lxu[2] = true;
+ l11_2.lxl[2] = false;
+ l13_2.xl[1] = 1.;
+ l14_2.xu[2] = 1.;
+ l5_5.gg[2] = 0.;
+ l20_3.lex = false;
+ l20_3.nex = 1;
+ l20_3.xex[0] = .57735194;
+ l20_3.xex[1] = 1.7320458;
+ l20_3.xex[2] = -2.0256839e-6;
+ l20_3.fex = 6.;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2];
+ l6_1.fx = d__1 * d__1 * 9. + d__2 * d__2 + d__3 * d__3 * 9.;
+ return 0;
+labelL3:
+ l4_2.gf[0] = l2_2.x[0] * 18.;
+ l4_2.gf[1] = l2_2.x[1] * 2.;
+ l4_2.gf[2] = l2_2.x[2] * 18.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = l2_2.x[0] * l2_2.x[1] - 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+ l5_5.gg[0] = l2_2.x[1];
+ l5_5.gg[1] = l2_2.x[0];
+L7:
+ return 0;
+} /* tp337_ */
+
+/* Subroutine */ int tp338_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 1;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 0.;
+ l11_2.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = false;
+ }
+ l5_3.gg[0] = .5;
+ l5_3.gg[2] = 1.;
+ l5_3.gg[4] = 1.;
+ l20_3.lex = false;
+ l20_3.nex = 1;
+ l20_3.xex[0] = .36689438;
+ l20_3.xex[1] = 2.2437202;
+ l20_3.xex[2] = -1.4271674;
+ l20_3.fex = -7.2056984;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2];
+ l6_1.fx = -(d__1 * d__1 + d__2 * d__2 + d__3 * d__3);
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L7: */
+ l4_2.gf[i__ - 1] = l2_2.x[i__ - 1] * -2.;
+ }
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = l2_2.x[0] * .5 + l2_2.x[1] + l2_2.x[2] - 1.;
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2];
+ l3_2.g[1] = d__1 * d__1 + d__2 * d__2 * .66666666666666663 + d__3 *
+ d__3 * .25 - 4.;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+ l5_3.gg[1] = l2_2.x[0] * 2.;
+ l5_3.gg[3] = l2_2.x[1] * 1.3333333333333333;
+ l5_3.gg[5] = l2_2.x[2] * .5;
+L8:
+ return 0;
+} /* tp338_ */
+
+/* Subroutine */ int tp339_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 1.;
+ l12_2.lxu[i__ - 1] = false;
+ l11_2.lxl[i__ - 1] = true;
+/* labelL6: */
+ l13_2.xl[i__ - 1] = 0.;
+ }
+ l20_3.lex = false;
+ l20_3.nex = 1;
+ l20_3.xex[0] = 2.3797626;
+ l20_3.xex[1] = .31622787;
+ l20_3.xex[2] = 1.9429359;
+ l20_3.fex = 3.3616797;
+ return 0;
+labelL2:
+ l6_1.fx = .2 / (l2_2.x[0] * l2_2.x[1] * l2_2.x[2]) + 4. / l2_2.x[0] + 3. /
+ l2_2.x[2];
+ return 0;
+labelL3:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[0];
+ l4_2.gf[0] = -.2 / (l2_2.x[1] * l2_2.x[2] * (d__1 * d__1)) - 4. / (d__2 *
+ d__2);
+/* Computing 2nd power */
+ d__1 = l2_2.x[1];
+ l4_2.gf[1] = -.2 / (l2_2.x[0] * l2_2.x[2] * (d__1 * d__1));
+/* Computing 2nd power */
+ d__1 = l2_2.x[2];
+/* Computing 2nd power */
+ d__2 = l2_2.x[2];
+ l4_2.gf[2] = -.2 / (l2_2.x[0] * l2_2.x[1] * (d__1 * d__1)) - 3. / (d__2 *
+ d__2);
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = 10. - l2_2.x[0] * 2. * l2_2.x[2] - l2_2.x[0] * l2_2.x[1];
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L8;
+ }
+ l5_5.gg[0] = l2_2.x[2] * -2. - l2_2.x[1];
+ l5_5.gg[1] = -l2_2.x[0];
+ l5_5.gg[2] = l2_2.x[0] * -2.;
+L8:
+ return 0;
+} /* tp339_ */
+
+/* Subroutine */ int tp340_(int *mode)
+{
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 1;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 1.;
+ l12_2.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_2.lxl[i__ - 1] = false;
+ }
+ l12_2.lxu[0] = true;
+ l14_2.xu[0] = 1.;
+ l5_5.gg[0] = -1.;
+ l5_5.gg[1] = -2.;
+ l5_5.gg[2] = -2.;
+ l20_3.lex = false;
+ l20_3.nex = 1;
+ l20_3.xex[0] = .60000009;
+ l20_3.xex[1] = .29999998;
+ l20_3.xex[2] = .29999998;
+ l20_3.fex = -.054;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_2.x[0] * l2_2.x[1] * l2_2.x[2];
+ return 0;
+labelL3:
+ l4_2.gf[0] = -l2_2.x[1] * l2_2.x[2];
+ l4_2.gf[1] = -l2_2.x[0] * l2_2.x[2];
+ l4_2.gf[2] = -l2_2.x[0] * l2_2.x[1];
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = 1.8 - l2_2.x[0] - l2_2.x[1] * 2. - l2_2.x[2] * 2.;
+ }
+labelL5:
+ return 0;
+} /* tp340_ */
+
+/* Subroutine */ int tp341_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 1.;
+ l11_2.lxl[i__ - 1] = true;
+ l12_2.lxu[i__ - 1] = false;
+/* labelL6: */
+ l13_2.xl[i__ - 1] = 0.;
+ }
+ l20_3.lex = false;
+ l20_3.nex = 1;
+ l20_3.xex[0] = 4.;
+ l20_3.xex[1] = 2.8284271;
+ l20_3.xex[2] = 2.;
+ l20_3.fex = -22.627417;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_2.x[0] * l2_2.x[1] * l2_2.x[2];
+ return 0;
+labelL3:
+ l4_2.gf[0] = -l2_2.x[1] * l2_2.x[2];
+ l4_2.gf[1] = -l2_2.x[0] * l2_2.x[2];
+ l4_2.gf[2] = -l2_2.x[0] * l2_2.x[1];
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2];
+ l3_1.g[0] = d__1 * d__1 * -1. - d__2 * d__2 * 2. - d__3 * d__3 * 4. +
+ 48.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+ l5_5.gg[0] = l2_2.x[0] * -2.;
+ l5_5.gg[1] = l2_2.x[1] * -4.;
+ l5_5.gg[2] = l2_2.x[2] * -8.;
+L7:
+ return 0;
+} /* tp341_ */
+
+/* Subroutine */ int tp342_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 1.;
+ l12_2.lxu[i__ - 1] = false;
+ l11_2.lxl[i__ - 1] = true;
+/* labelL6: */
+ l13_2.xl[i__ - 1] = 0.;
+ }
+ l20_3.lex = false;
+ l20_3.nex = 1;
+ l20_3.xex[0] = 4.;
+ l20_3.xex[1] = 2.8284271;
+ l20_3.xex[2] = 2.;
+ l20_3.fex = -22.627417;
+ return 0;
+labelL2:
+ l6_1.fx = -l2_2.x[0] * l2_2.x[1] * l2_2.x[2];
+ return 0;
+labelL3:
+ l4_2.gf[0] = -l2_2.x[1] * l2_2.x[2];
+ l4_2.gf[1] = -l2_2.x[0] * l2_2.x[2];
+ l4_2.gf[2] = -l2_2.x[0] * l2_2.x[1];
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[1];
+/* Computing 2nd power */
+ d__3 = l2_2.x[2];
+ l3_1.g[0] = 48. - d__1 * d__1 - d__2 * d__2 * 2. - d__3 * d__3 * 4.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L8;
+ }
+ l5_5.gg[0] = l2_2.x[0] * -2.;
+ l5_5.gg[1] = l2_2.x[1] * -4.;
+ l5_5.gg[2] = l2_2.x[2] * -8.;
+L8:
+ return 0;
+} /* tp342_ */
+
+/* Subroutine */ int tp343_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l12_2.lxu[i__ - 1] = true;
+ l11_2.lxl[i__ - 1] = true;
+/* labelL6: */
+ l13_2.xl[i__ - 1] = 0.;
+ }
+ l2_2.x[0] = 22.3;
+ l2_2.x[1] = .5;
+ l2_2.x[2] = 125.;
+ l14_2.xu[0] = 36.;
+ l14_2.xu[1] = 5.;
+ l14_2.xu[2] = 125.;
+ l5_3.gg[4] = 0.;
+ l5_3.gg[3] = 0.;
+ l20_3.lex = false;
+ l20_3.nex = 1;
+ l20_3.xex[0] = 16.508383;
+ l20_3.xex[1] = 2.4768216;
+ l20_3.xex[2] = 123.99452;
+ l20_3.fex = -5.6847825;
+ return 0;
+labelL2:
+/* Computing 4th power */
+ d__1 = l2_2.x[0], d__1 *= d__1;
+/* Computing 2nd power */
+ d__2 = l2_2.x[2];
+ l6_1.fx = d__1 * d__1 * -.0201 * l2_2.x[1] * (d__2 * d__2) * 1e-7;
+ return 0;
+labelL3:
+/* Computing 3rd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[2];
+ l4_2.gf[0] = d__1 * (d__1 * d__1) * -.0804 * l2_2.x[1] * (d__2 * d__2) *
+ 1e-7;
+/* Computing 4th power */
+ d__1 = l2_2.x[0], d__1 *= d__1;
+/* Computing 2nd power */
+ d__2 = l2_2.x[2];
+ l4_2.gf[1] = d__1 * d__1 * -.0201 * (d__2 * d__2) * 1e-7;
+/* Computing 4th power */
+ d__1 = l2_2.x[0], d__1 *= d__1;
+ l4_2.gf[2] = d__1 * d__1 * -.0402 * l2_2.x[1] * l2_2.x[2] * 1e-7;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+ l3_2.g[0] = 675. - d__1 * d__1 * l2_2.x[1];
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[2];
+ l3_2.g[1] = .419 - d__1 * d__1 * (d__2 * d__2) * 1e-7;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+ l5_3.gg[0] = l2_2.x[0] * -2. * l2_2.x[1];
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+ l5_3.gg[2] = -(d__1 * d__1);
+L7:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+/* Computing 2nd power */
+ d__1 = l2_2.x[2];
+ l5_3.gg[1] = l2_2.x[0] * -2e-7 * (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+ l5_3.gg[5] = d__1 * d__1 * -2e-7 * l2_2.x[2];
+L8:
+ return 0;
+} /* tp343_ */
+
+/* Subroutine */ int tp344_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 2.;
+ l11_2.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = false;
+ }
+ l20_3.lex = false;
+ l20_3.nex = 1;
+ l20_3.xex[0] = 1.104859;
+ l20_3.xex[1] = 1.1966742;
+ l20_3.xex[2] = 1.5352623;
+ l20_3.fex = .0325682;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0] - 1.;
+/* Computing 2nd power */
+ d__2 = l2_2.x[0] - l2_2.x[1];
+/* Computing 4th power */
+ d__3 = l2_2.x[1] - l2_2.x[2], d__3 *= d__3;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3;
+ return 0;
+labelL3:
+ l4_2.gf[0] = (l2_2.x[0] - 1.) * 2. + (l2_2.x[0] - l2_2.x[1]) * 2.;
+/* Computing 3rd power */
+ d__1 = l2_2.x[1] - l2_2.x[2];
+ l4_2.gf[1] = (l2_2.x[0] - l2_2.x[1]) * -2. + d__1 * (d__1 * d__1) * 4.;
+/* Computing 3rd power */
+ d__1 = l2_2.x[1] - l2_2.x[2];
+ l4_2.gf[2] = d__1 * (d__1 * d__1) * -4.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[1];
+/* Computing 4th power */
+ d__2 = l2_2.x[2], d__2 *= d__2;
+ l3_1.g[0] = l2_2.x[0] * (d__1 * d__1 + 1.) + d__2 * d__2 - 4. - std::sqrt(
+ 2.) * 3.;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+/* Computing 2nd power */
+ d__1 = l2_2.x[1];
+ l5_5.gg[0] = d__1 * d__1 + 1.;
+ l5_5.gg[1] = l2_2.x[0] * 2. * l2_2.x[1];
+/* Computing 3rd power */
+ d__1 = l2_2.x[2];
+ l5_5.gg[2] = d__1 * (d__1 * d__1) * 4.;
+L7:
+ return 0;
+} /* tp344_ */
+
+/* Subroutine */ int tp345_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l2_2.x[i__ - 1] = 0.;
+ l12_2.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_2.lxl[i__ - 1] = false;
+ }
+ l20_3.lex = false;
+ l20_3.nex = 1;
+ l20_3.xex[0] = 1.1048584;
+ l20_3.xex[1] = 1.1966752;
+ l20_3.xex[2] = 1.5352622;
+ l20_3.fex = .0325682;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_2.x[0] - 1.;
+/* Computing 2nd power */
+ d__2 = l2_2.x[0] - l2_2.x[1];
+/* Computing 4th power */
+ d__3 = l2_2.x[1] - l2_2.x[2], d__3 *= d__3;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 + d__3 * d__3;
+ return 0;
+labelL3:
+ l4_2.gf[0] = (l2_2.x[0] - 1) * 2. + (l2_2.x[0] - l2_2.x[1]) * 2.;
+/* Computing 3rd power */
+ d__1 = l2_2.x[1] - l2_2.x[2];
+ l4_2.gf[1] = (l2_2.x[0] - l2_2.x[1]) * -2. + d__1 * (d__1 * d__1) * 4.;
+/* Computing 3rd power */
+ d__1 = l2_2.x[1] - l2_2.x[2];
+ l4_2.gf[2] = d__1 * (d__1 * d__1) * -4.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[1];
+/* Computing 4th power */
+ d__2 = l2_2.x[2], d__2 *= d__2;
+ l3_1.g[0] = l2_2.x[0] * (d__1 * d__1 + 1.) + d__2 * d__2 - 4. - std::sqrt(
+ 18.);
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L8;
+ }
+/* Computing 2nd power */
+ d__1 = l2_2.x[1];
+ l5_5.gg[0] = d__1 * d__1 + 1.;
+ l5_5.gg[1] = l2_2.x[0] * 2. * l2_2.x[1];
+/* Computing 3rd power */
+ d__1 = l2_2.x[2];
+ l5_5.gg[2] = d__1 * (d__1 * d__1) * 4.;
+L8:
+ return 0;
+} /* tp345_ */
+
+/* Subroutine */ int tp346_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l12_2.lxu[i__ - 1] = true;
+ l11_2.lxl[i__ - 1] = true;
+/* labelL6: */
+ l13_2.xl[i__ - 1] = 0.;
+ }
+ l2_2.x[0] = 22.3;
+ l2_2.x[1] = .5;
+ l2_2.x[2] = 125.;
+ l14_2.xu[0] = 36.;
+ l14_2.xu[1] = 5.;
+ l14_2.xu[2] = 125.;
+ l5_3.gg[4] = 0.;
+ l5_3.gg[3] = 0.;
+ l20_3.lex = false;
+ l20_3.nex = 1;
+ l20_3.xex[0] = 16.508383;
+ l20_3.xex[1] = 2.4768216;
+ l20_3.xex[2] = 123.99452;
+ l20_3.fex = -5.6847825;
+ return 0;
+labelL2:
+/* Computing 4th power */
+ d__1 = l2_2.x[0], d__1 *= d__1;
+/* Computing 2nd power */
+ d__2 = l2_2.x[2];
+ l6_1.fx = d__1 * d__1 * -.0201 * l2_2.x[1] * (d__2 * d__2) * 1e-7;
+ return 0;
+labelL3:
+/* Computing 3rd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[2];
+ l4_2.gf[0] = d__1 * (d__1 * d__1) * -.0804 * l2_2.x[1] * (d__2 * d__2) *
+ 1e-7;
+/* Computing 4th power */
+ d__1 = l2_2.x[0], d__1 *= d__1;
+/* Computing 2nd power */
+ d__2 = l2_2.x[2];
+ l4_2.gf[1] = d__1 * d__1 * -.0201 * (d__2 * d__2) * 1e-7;
+/* Computing 4th power */
+ d__1 = l2_2.x[0], d__1 *= d__1;
+ l4_2.gf[2] = d__1 * d__1 * -.0402 * l2_2.x[1] * l2_2.x[2] * 1e-7;
+ return 0;
+labelL4:
+ if (l9_3.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+ l3_2.g[0] = 675. - d__1 * d__1 * l2_2.x[1];
+ }
+ if (l9_3.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+/* Computing 2nd power */
+ d__2 = l2_2.x[2];
+ l3_2.g[1] = .419 - d__1 * d__1 * (d__2 * d__2) * 1e-7;
+ }
+ return 0;
+labelL5:
+ if (! l10_3.index2[0]) {
+ goto L7;
+ }
+ l5_3.gg[0] = l2_2.x[0] * -2. * l2_2.x[1];
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+ l5_3.gg[2] = -(d__1 * d__1);
+L7:
+ if (! l10_3.index2[1]) {
+ goto L8;
+ }
+/* Computing 2nd power */
+ d__1 = l2_2.x[2];
+ l5_3.gg[1] = l2_2.x[0] * -2e-7 * (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = l2_2.x[0];
+ l5_3.gg[5] = d__1 * d__1 * -2e-7 * l2_2.x[2];
+L8:
+ return 0;
+} /* tp346_ */
+
+/* Subroutine */ int tp347_(int *mode)
+{
+ /* Initialized data */
+
+ static Real a[3] = { 8204.37,9008.72,9330.46 };
+
+ /* Local variables */
+ static int ninl;
+ static Real h__[8];
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_3.n = 3;
+ l1_3.nili = 0;
+ ninl = 0;
+ l1_3.neli = 1;
+ l1_3.nenl = 0;
+ l2_2.x[0] = .7;
+ l2_2.x[1] = .2;
+ l2_2.x[2] = .1;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_2.lxl[i__ - 1] = true;
+ l12_2.lxu[i__ - 1] = true;
+ l13_2.xl[i__ - 1] = 0.;
+ l14_2.xu[i__ - 1] = 1.;
+/* L7: */
+ l5_5.gg[i__ - 1] = 1.;
+ }
+ l20_3.lex = false;
+ l20_3.nex = 1;
+ l20_3.xex[0] = 0.;
+ l20_3.xex[1] = 0.;
+ l20_3.xex[2] = 1.;
+ l20_3.fex = 17374.625;
+ return 0;
+labelL2:
+ h__[0] = l2_2.x[0] + l2_2.x[1] + l2_2.x[2] + .03;
+ h__[1] = l2_2.x[0] * .09 + l2_2.x[1] + l2_2.x[2] + .03;
+ h__[2] = h__[0] * h__[1];
+ h__[3] = l2_2.x[1] + l2_2.x[2] + .03;
+ h__[4] = l2_2.x[1] * .07 + l2_2.x[2] + .03;
+ h__[5] = h__[3] * h__[4];
+ h__[6] = l2_2.x[2] + .03;
+ h__[7] = l2_2.x[2] * .13 + .03;
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ l6_1.fx = a[0] * std::log(h__[0] / h__[1]) + a[1] * std::log(h__[3] / h__[4]) + a[2]
+ * std::log(h__[6] / h__[7]);
+ return 0;
+labelL3:
+ l4_2.gf[0] = a[0] * (h__[1] - h__[0] * .09) / h__[2];
+ l4_2.gf[1] = a[0] * (h__[1] - h__[0]) / h__[2] + a[1] * (h__[4] - h__[3] *
+ .07) / h__[5];
+ l4_2.gf[2] = a[0] * (h__[1] - h__[0]) / h__[2] + a[1] * (h__[4] - h__[3])
+ / h__[5] + a[2] * (h__[7] - h__[6] * .13) / (h__[6] * h__[7]);
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = l2_2.x[0] + l2_2.x[1] + l2_2.x[2] - 1.;
+ }
+labelL5:
+ return 0;
+} /* tp347_ */
+
+
+/* Subroutine */ int tp348_(int *mode)
+{
+ /* Initialized data */
+
+ static Real rho = .0747;
+ static Real xmu = .0443;
+ static Real cp = .24;
+ static Real pr = .709;
+ static Real pi = 3.14159;
+ static Real d__ = .525;
+ static Real tin = 75.;
+ static Real tsurf = 45.;
+ static Real h__ = 13.13;
+ static Real w = 3.166;
+ static Real rhoc = 559.;
+ static Real rhoa = 169.;
+
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static Real etaf, delp, etas, xval;
+ static int i__;
+ static Real q, costf, costm, h1, xmdot, costt, ac, af, gi, at, re,
+ ho, hef;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_2.lxl[i__ - 1] = false;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = true;
+ }
+ l11_2.lxl[1] = true;
+ l13_2.xl[1] = 13.13;
+ l14_2.xu[0] = .044;
+ l14_2.xu[1] = 24.;
+ l14_2.xu[2] = 600.;
+ l2_2.x[0] = .1;
+ l2_2.x[1] = 18.;
+ l2_2.x[2] = 144.;
+ l20_3.lex = false;
+ l20_3.nex = 1;
+ l20_3.xex[0] = .044;
+ l20_3.xex[1] = 24.;
+ l20_3.xex[2] = 85.607576;
+ l20_3.fex = 36.97084;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = d__;
+ af = l2_2.x[1] / l2_2.x[0] * 2. * (w * h__ - pi * 30. * (d__1 * d__1) /
+ 4.) / 144.;
+ at = pi * 30. * d__ * l2_2.x[1] / 144.;
+ ac = (h__ * l2_2.x[1] - d__ * 10. * l2_2.x[1] - l2_2.x[1] / l2_2.x[0] *
+ .006 * h__) / 144.;
+ gi = rho * l2_2.x[2] * (h__ * l2_2.x[1]) / (ac * 144.) * 60.;
+ re = gi * 1.083 / (xmu * 12.);
+ if (re < 1e-10) {
+ re = 1e-10;
+ }
+ ho = gi * .195 * cp / (pow_dd(&pr, &c_b993) * pow_dd(&re, &c_b2368));
+ xmdot = rho * l2_2.x[2] * h__ * l2_2.x[1] / 144. * 60.;
+/* Computing 2nd power */
+ d__1 = gi;
+ delp = 1.833e-6 / rho * (d__1 * d__1) * 3. * (af / ac * pow_dd(&re, &
+ c_b308) + at * .1 / ac);
+ if (ho < 1e-10) {
+ ho = 1e-10;
+ }
+ xval = std::sqrt(ho) * .0732;
+ etaf = std::tanh(xval) / xval;
+ etas = 1. - af / (af + at) * (1. - etaf);
+ hef = 1. - std::exp(-etas * ho * (af + at) / (xmdot * cp));
+ q = hef * (tin - tsurf) * xmdot * cp;
+ if (*mode == 4) {
+ goto L7;
+ }
+ h1 = delp / rho * xmdot / 1.98e6;
+ if (h1 < 1e-10) {
+ h1 = 1e-10;
+ }
+ costm = std::sqrt(h1) / .0718 + 4.;
+/* Computing 2nd power */
+ d__1 = d__;
+/* Computing 2nd power */
+ d__2 = d__ - .036;
+ costt = l2_2.x[1] * 30.300000000000001 * pi / 4. * (d__1 * d__1 - d__2 *
+ d__2);
+ costf = h__ * .47 * w * .006 * rhoa / 1728. * l2_2.x[1] / l2_2.x[0];
+ costt = costt * rhoc / 1728.;
+ l6_1.fx = costm + costt + costf;
+labelL3:
+ return 0;
+labelL4:
+ if (! l9_2.index1[0]) {
+ goto labelL5;
+ }
+ goto labelL2;
+L7:
+ l3_1.g[0] = 6e3 - q;
+labelL5:
+ return 0;
+} /* tp348_ */
+
+
+/* Subroutine */ int tp349_(int *mode)
+{
+ /* Initialized data */
+
+ static Real p2 = 10.;
+ static Real c1f = .075;
+ static Real c2f = .025;
+ static Real h1 = 8e3;
+ static Real h2 = 8e3;
+ static Real e1 = 1e3;
+ static Real e2 = 1e3;
+ static Real cpp = 3.6938503;
+ static Real p = 20.;
+
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static Real area, heat, argu, wate, cost, vest, temp1, temp2, temp3;
+ static int i__;
+ static Real v, c0, c1, c2, xlmtd, c3, press, c4, c5, c6, c7, p1,
+ a11, a12, a13, a22, a23, a31, a32, a33, ut, xk1, xk2, hea, dia,
+ are, phi[9];
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL3;
+ }
+labelL1:
+ l1_1.n = 3;
+ l1_1.nili = 0;
+ l1_1.ninl = 9;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_2.x[0] = 5e3;
+ l2_2.x[1] = 200.;
+ l2_2.x[2] = 100.;
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_2.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_2.lxu[i__ - 1] = true;
+ }
+ l11_2.lxl[2] = false;
+ l12_2.lxu[2] = false;
+ l13_2.xl[0] = 1e3;
+ l13_2.xl[1] = 100.;
+ l14_2.xu[0] = 8e3;
+ l14_2.xu[1] = 500.;
+ l20_3.lex = false;
+ l20_3.fex = -4.1489499;
+ l20_3.xex[0] = 7828.7954;
+ l20_3.xex[1] = 188.81406;
+ l20_3.xex[2] = 113.81406;
+ return 0;
+labelL2:
+ p1 = 100.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L35: */
+ if (l2_2.x[i__ - 1] < 1e-6) {
+ l2_2.x[i__ - 1] = 1e-6;
+ }
+ }
+ xk1 = p1 * std::exp(-e1 / (l2_2.x[1] + 460.));
+ xk2 = p2 * std::exp(-e2 / (l2_2.x[1] + 460.));
+ v = p * l2_2.x[0] / (xk2 * (l2_2.x[0] * c2f - p));
+ c1 = (l2_2.x[0] * c1f - p) / (l2_2.x[0] + v * xk1);
+ ut = l2_2.x[1] * .0452 + 43.;
+L39:
+ argu = (l2_2.x[1] - l2_2.x[2] - 75.) / (l2_2.x[1] - 100.);
+ if (argu == 0.) {
+ goto L48;
+ }
+ xlmtd = (25. - l2_2.x[2]) / std::log((std::abs(argu)));
+ heat = l2_2.x[0] * cpp * (100. - l2_2.x[1]) + xk1 * (l2_2.x[0] * c1f - p)
+ * v * h1 / (l2_2.x[0] + v * xk1) + p * h2;
+ area = heat / (ut * xlmtd);
+ are = std::abs(area);
+ hea = std::abs(heat);
+ d__1 = v / 12.72;
+ dia = pow_dd(&d__1, &c_b2380);
+ if (l2_2.x[1] < 200.) {
+ goto L40;
+ }
+/* Computing 3rd power */
+ d__1 = l2_2.x[1];
+ press = d__1 * (d__1 * d__1) * 3.3e-6 + 23.6;
+ goto L41;
+L48:
+ l2_2.x[1] *= 1.0001;
+ goto L39;
+L40:
+ press = 50.;
+L41:
+/* Computing 3rd power */
+ d__1 = dia;
+/* Computing 2nd power */
+ d__2 = dia;
+/* Computing 2nd power */
+ d__3 = dia;
+ wate = (d__1 * (d__1 * d__1) * .0909 + d__2 * d__2 * .482) * press + d__3
+ * d__3 * 36.6 + dia * 160.5;
+ c1 = pow_dd(&wate, &c_b2383) * 4.8;
+ if (l2_2.x[1] < 200.) {
+ goto L42;
+ }
+/* Computing 2nd power */
+ d__1 = dia;
+ c2 = (l2_2.x[1] * .0133 + 17.2) * (d__1 * d__1);
+ goto L43;
+L42:
+ c2 = 0.;
+L43:
+ if (press < 150.) {
+ goto L44;
+ }
+/* Computing 3rd power */
+ d__1 = l2_2.x[1];
+ c3 = pow_dd(&are, &c_b2387) * 270. * (d__1 * (d__1 * d__1) * 1.68e-7 +
+ .962);
+ goto L45;
+L44:
+ c3 = pow_dd(&are, &c_b2387) * 270.;
+L45:
+ c4 = dia * 140. + 1400.;
+ d__1 = v * .05;
+ c5 = pow_dd(&d__1, &c_b2390) * 875.;
+/* Computing 3rd power */
+ d__2 = l2_2.x[1];
+ d__1 = d__2 * (d__2 * d__2) * 4.59e-11 + 6.95e-4 + l2_2.x[0];
+ c6 = pow_dd(&d__1, &c_b2391) * 812.;
+ if (l2_2.x[1] < 250.) {
+ goto L46;
+ }
+ d__1 = hea * 298. / l2_2.x[2];
+ c7 = pow_dd(&d__1, &c_b2391) * 1291.;
+ goto L47;
+L46:
+ d__1 = hea * 298. / l2_2.x[2];
+ c7 = pow_dd(&d__1, &c_b2391) * 812.;
+L47:
+ cost = c1 + c2 + c3 + c4 + c5 + c6 + c7;
+ vest = cost * 5.;
+/* Computing 3rd power */
+ d__1 = l2_2.x[1];
+ c0 = vest * .18 + 2.2e4 + v * 3.1 + (d__1 * (d__1 * d__1) * 4.59e-11 +
+ 6.95e-4) * l2_2.x[0] * 61.1 + heat * .00115 + heat * 6.92 +
+ l2_2.x[0] * 574. * (c1f - c1) + 114800.;
+ l6_1.fx = (6.88e5 - c0) / (vest * 2.) * -.001;
+labelL3:
+ return 0;
+labelL4:
+ p1 = 100.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L37: */
+ if (l2_2.x[i__ - 1] < 1e-6) {
+ l2_2.x[i__ - 1] = 1e-6;
+ }
+ }
+ xk1 = p1 * std::exp(-e1 / (l2_2.x[1] + 460.));
+ xk2 = p2 * std::exp(-e2 / (l2_2.x[1] + 460.));
+ v = p * l2_2.x[0] / (xk2 * (l2_2.x[0] * c2f - p));
+ c1 = (l2_2.x[0] * c1f - p) / (l2_2.x[0] + v * xk1);
+ ut = l2_2.x[1] * .0452 + 43.;
+L36:
+ argu = (l2_2.x[1] - l2_2.x[2] - 75.) / (l2_2.x[1] - 100.);
+ if (argu == 0.) {
+ goto L49;
+ }
+ xlmtd = (25. - l2_2.x[2]) / std::log((std::abs(argu)));
+ heat = l2_2.x[0] * cpp * (100. - l2_2.x[1]) + xk1 * (l2_2.x[0] * c1f - p)
+ * v * h1 / (l2_2.x[0] + v * xk1) + p * h2;
+ area = heat / (ut * xlmtd);
+ d__1 = v / 12.72;
+ dia = pow_dd(&d__1, &c_b2380);
+ if (l2_2.x[1] < 200.) {
+ goto L50;
+ }
+/* Computing 3rd power */
+ d__1 = l2_2.x[1];
+ press = d__1 * (d__1 * d__1) * 3.3e-6 + 23.6;
+ goto L51;
+L49:
+ l2_2.x[1] *= 1.0001;
+ goto L36;
+L50:
+ press = 50.;
+L51:
+ phi[0] = dia - 1.25;
+ phi[1] = 9.67 - dia;
+ phi[2] = area - 50.;
+ phi[3] = 4e3 - area;
+ a11 = xk1 + l2_2.x[0] / v;
+ a12 = xk2;
+/* Computing 2nd power */
+ d__1 = l2_2.x[1] + 460.;
+/* Computing 2nd power */
+ d__2 = l2_2.x[1] + 460.;
+ a13 = (l2_2.x[0] * c1f - press) * xk1 * e1 / ((l2_2.x[0] + v * xk1) * (
+ d__1 * d__1)) + press * e2 / (v * (d__2 * d__2));
+ a22 = xk2 + l2_2.x[0] / v;
+/* Computing 2nd power */
+ d__1 = l2_2.x[1] + 460.;
+ a23 = press * e2 / (v * (d__1 * d__1));
+ a31 = -h1 * xk1 / cpp;
+ a32 = -h2 * xk2 / cpp;
+/* Computing 2nd power */
+ d__1 = l2_2.x[1] + 460.;
+/* Computing 2nd power */
+ d__2 = l2_2.x[1] + 460.;
+ a33 = l2_2.x[0] / v + ut * area / (v * cpp) - (l2_2.x[0] * c1f - press) *
+ xk1 * e1 * h1 / ((l2_2.x[0] + v * xk1) * cpp * (d__1 * d__1)) -
+ press * e2 * h2 / (v * cpp * (d__2 * d__2));
+ temp1 = a11 + a22 + a33;
+ phi[4] = temp1;
+ temp2 = a11 * a22 + a22 * a33 + a33 * a11 - a13 * a31 - a23 * a32;
+ phi[5] = temp2;
+ temp3 = a11 * a22 * a33 + a12 * a23 * a31 - a13 * a31 * a22 - a23 * a32 *
+ a11;
+ phi[6] = temp3;
+ phi[7] = temp1 * temp2 - temp3;
+ phi[8] = heat;
+ for (i__ = 1; i__ <= 9; ++i__) {
+/* L7: */
+ if (l9_16.index1[i__ - 1]) {
+ l3_15.g[i__ - 1] = phi[i__ - 1];
+ }
+ }
+ return 0;
+} /* tp349_ */
+
+
+/* Subroutine */ int tp350_(int *mode)
+{
+ /* Initialized data */
+
+ static Real y[11] = { .1957,.1947,.1735,.16,.0844,.0627,.0456,.0342,
+ .0323,.0235,.0246 };
+ static Real u[11] = { 4.,2.,1.,.5,.25,.167,.125,.1,.0833,.0714,
+ .0625 };
+
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static Real h__[11];
+ static int i__, j;
+
+ for (i__ = 1; i__ <= 11; ++i__) {
+/* labelL12: */
+/* Computing 2nd power */
+ d__1 = u[i__ - 1];
+ h__[i__ - 1] = d__1 * d__1 + l2_3.x[2] * u[i__ - 1] + l2_3.x[3];
+ }
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_3.x[0] = .25;
+ l2_3.x[1] = .39;
+ l2_3.x[2] = .415;
+ l2_3.x[3] = .39;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l12_3.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_3.lxl[i__ - 1] = false;
+ }
+ l15_1.lsum = 11;
+ l20_6.lex = false;
+ l20_6.nex = 1;
+ l20_6.fex = 3.0750561000000003;
+ l20_6.xex[0] = .19280644;
+ l20_6.xex[1] = .19126279;
+ l20_6.xex[2] = .12305098;
+ l20_6.xex[3] = .13605235;
+ return 0;
+labelL2:
+ for (i__ = 1; i__ <= 11; ++i__) {
+/* labelL20: */
+/* Computing 2nd power */
+ d__1 = u[i__ - 1];
+ l16_7.f[i__ - 1] = y[i__ - 1] - l2_3.x[0] / h__[i__ - 1] * (d__1 *
+ d__1 + l2_3.x[1] * u[i__ - 1]);
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 11; ++i__) {
+/* L7: */
+/* Computing 2nd power */
+ d__1 = l16_7.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ l6_1.fx *= 1e4;
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 11; ++i__) {
+/* Computing 2nd power */
+ d__1 = u[i__ - 1];
+ l17_18.df[i__ - 1] = (-(d__1 * d__1) - l2_3.x[1] * u[i__ - 1]) / h__[
+ i__ - 1];
+ l17_18.df[i__ + 10] = -l2_3.x[0] * u[i__ - 1] / h__[i__ - 1];
+/* Computing 2nd power */
+ d__1 = u[i__ - 1];
+/* Computing 2nd power */
+ d__2 = h__[i__ - 1];
+ l17_18.df[i__ + 21] = l2_3.x[0] * u[i__ - 1] * (d__1 * d__1 + l2_3.x[
+ 1] * u[i__ - 1]) / (d__2 * d__2);
+/* L8: */
+/* Computing 2nd power */
+ d__1 = u[i__ - 1];
+/* Computing 2nd power */
+ d__2 = h__[i__ - 1];
+ l17_18.df[i__ + 32] = l2_3.x[0] * (d__1 * d__1 + l2_3.x[1] * u[i__ -
+ 1]) / (d__2 * d__2);
+ }
+ for (j = 1; j <= 4; ++j) {
+ l4_3.gf[j - 1] = 0.;
+ for (i__ = 1; i__ <= 11; ++i__) {
+/* labelL10: */
+ l4_3.gf[j - 1] += l16_7.f[i__ - 1] * 2. * l17_18.df[i__ + j * 11
+ - 12];
+ }
+ l4_3.gf[j - 1] *= 1e4;
+/* labelL9: */
+ }
+labelL4:
+ return 0;
+} /* tp350_ */
+
+
+/* Subroutine */ int tp351_(int *mode)
+{
+ /* Initialized data */
+
+ static Real a[7] = { 0.,4.28e-4,.001,.00161,.00209,.00348,.00525 };
+ static Real b[7] = { 7.391,11.18,16.44,16.2,22.2,24.02,31.32 };
+
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__, j;
+ static Real xh1, xh2, xh3, xh4, xh5;
+
+/* Computing 2nd power */
+ d__1 = l2_3.x[0];
+ xh1 = d__1 * d__1;
+/* Computing 2nd power */
+ d__1 = l2_3.x[1];
+ xh2 = d__1 * d__1;
+/* Computing 2nd power */
+ d__1 = l2_3.x[2];
+ xh3 = d__1 * d__1;
+/* Computing 2nd power */
+ d__1 = l2_3.x[3];
+ xh4 = d__1 * d__1;
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_3.x[0] = 2.7;
+ l2_3.x[1] = 90.;
+ l2_3.x[2] = 1500.;
+ l2_3.x[3] = 10.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l12_3.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_3.lxl[i__ - 1] = false;
+ }
+ l20_6.lex = false;
+ l20_6.nex = 1;
+ l20_6.fex = 318.58082;
+ l20_6.xex[0] = 2.714;
+ l20_6.xex[1] = 140.4;
+ l20_6.xex[2] = 1707.;
+ l20_6.xex[3] = 31.51;
+ l15_1.lsum = 7;
+ return 0;
+labelL2:
+ for (i__ = 1; i__ <= 7; ++i__) {
+/* labelL20: */
+ l16_6.f[i__ - 1] = ((xh1 + a[i__ - 1] * xh2 + a[i__ - 1] * a[i__ - 1]
+ * xh3) / (a[i__ - 1] * xh4 + 1.) - b[i__ - 1]) / b[i__ - 1] *
+ 100.;
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 7; ++i__) {
+/* L7: */
+/* Computing 2nd power */
+ d__1 = l16_6.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ for (j = 1; j <= 4; ++j) {
+/* L8: */
+ l4_3.gf[j - 1] = 0.;
+ }
+ for (i__ = 1; i__ <= 7; ++i__) {
+ xh5 = (xh4 * a[i__ - 1] + 1.) * b[i__ - 1];
+ l17_7.df[i__ - 1] = l2_3.x[0] * 200. / xh5;
+ l17_7.df[i__ + 6] = l2_3.x[1] * 200. * a[i__ - 1] / xh5;
+/* Computing 2nd power */
+ d__1 = a[i__ - 1];
+ l17_7.df[i__ + 13] = l2_3.x[2] * 200. * (d__1 * d__1) / xh5;
+/* Computing 2nd power */
+ d__1 = a[i__ - 1];
+/* Computing 2nd power */
+ d__2 = xh5;
+ l17_7.df[i__ + 20] = l2_3.x[3] * -200. * a[i__ - 1] * b[i__ - 1] * (
+ xh1 + xh2 * a[i__ - 1] + xh3 * (d__1 * d__1)) / (d__2 * d__2);
+ for (j = 1; j <= 4; ++j) {
+/* labelL10: */
+ l4_3.gf[j - 1] += l16_6.f[i__ - 1] * 2. * l17_7.df[i__ + j * 7 -
+ 8];
+ }
+ }
+labelL4:
+ return 0;
+} /* tp351_ */
+
+/* Subroutine */ int tp352_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__, j;
+ static Real ti;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_3.x[0] = 25.;
+ l2_3.x[1] = 5.;
+ l2_3.x[2] = -5.;
+ l2_3.x[3] = -1.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l12_3.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_3.lxl[i__ - 1] = false;
+ }
+ l20_6.lex = false;
+ l20_6.nex = 1;
+ l20_6.fex = 903.23433;
+ l20_6.xex[0] = -10.223574;
+ l20_6.xex[1] = 11.908429;
+ l20_6.xex[2] = -.45804134;
+ l20_6.xex[3] = .58031996;
+ l15_1.lsum = 40;
+ return 0;
+labelL2:
+ for (i__ = 1; i__ <= 20; ++i__) {
+ ti = i__ * .2;
+ l16_16.f[i__ - 1] = l2_3.x[0] + l2_3.x[1] * ti - std::exp(ti);
+/* labelL20: */
+ l16_16.f[i__ + 19] = l2_3.x[2] + l2_3.x[3] * std::sin(ti) -std::cos(ti);
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 20; ++i__) {
+/* L7: */
+/* Computing 2nd power */
+ d__1 = l16_16.f[i__ - 1];
+/* Computing 2nd power */
+ d__2 = l16_16.f[i__ + 19];
+ l6_1.fx = l6_1.fx + d__1 * d__1 + d__2 * d__2;
+ }
+ return 0;
+labelL3:
+ for (j = 1; j <= 4; ++j) {
+/* L8: */
+ l4_3.gf[j - 1] = 0.;
+ }
+ for (i__ = 1; i__ <= 20; ++i__) {
+ ti = i__ * .2;
+ l17_19.df[i__ - 1] = 1.;
+ l17_19.df[i__ + 19] = 0.;
+ l17_19.df[i__ + 39] = ti;
+ l17_19.df[i__ + 59] = 0.;
+ l17_19.df[i__ + 79] = 0.;
+ l17_19.df[i__ + 99] = 1.;
+ l17_19.df[i__ + 119] = 0.;
+ l17_19.df[i__ + 139] = std::sin(ti);
+ for (j = 1; j <= 4; ++j) {
+/* labelL10: */
+ l4_3.gf[j - 1] = l4_3.gf[j - 1] + l16_16.f[i__ - 1] * 2. *
+ l17_19.df[i__ + j * 40 - 41] + l16_16.f[i__ + 19] * 2. *
+ l17_19.df[i__ + 20 + j * 40 - 41];
+ }
+ }
+labelL4:
+ return 0;
+} /* tp352_ */
+
+/* Subroutine */ int tp353_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__;
+ static Real q;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 1;
+ l1_1.ninl = 1;
+ l1_1.neli = 1;
+ l1_1.nenl = 0;
+ l2_3.x[0] = 0.;
+ l2_3.x[1] = 0.;
+ l2_3.x[2] = .4;
+ l2_3.x[3] = .6;
+ l5_7.gg[0] = 2.3;
+ l5_7.gg[3] = 5.6;
+ l5_7.gg[6] = 11.1;
+ l5_7.gg[9] = 1.3;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l5_7.gg[i__ * 3 - 1] = 1.;
+ l11_3.lxl[i__ - 1] = true;
+ l12_3.lxu[i__ - 1] = false;
+/* labelL6: */
+ l13_3.xl[i__ - 1] = 0.;
+ }
+ l20_6.lex = false;
+ l20_6.nex = 1;
+ l20_6.fex = -39.933673;
+ l20_6.xex[0] = 0.;
+ l20_6.xex[1] = 0.;
+ l20_6.xex[2] = .37755102;
+ l20_6.xex[3] = .62244898;
+ return 0;
+labelL2:
+ l6_1.fx = l2_3.x[0] * 24.55 + l2_3.x[1] * 26.75 + l2_3.x[2] * 39. +
+ l2_3.x[3] * 40.5;
+ l6_1.fx = -l6_1.fx;
+ return 0;
+labelL3:
+ l4_3.gf[0] = -24.55;
+ l4_3.gf[1] = -26.75;
+ l4_3.gf[2] = -39.;
+ l4_3.gf[3] = -40.5;
+ return 0;
+labelL4:
+/* Computing 2nd power */
+ d__1 = l2_3.x[0] * .53;
+/* Computing 2nd power */
+ d__2 = l2_3.x[1] * .44;
+/* Computing 2nd power */
+ d__3 = l2_3.x[2] * 4.5;
+/* Computing 2nd power */
+ d__4 = l2_3.x[3] * .79;
+ q = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 + d__4 * d__4;
+ if (*mode == 5) {
+ goto labelL5;
+ }
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = l2_3.x[0] * 2.3 + l2_3.x[1] * 5.6 + l2_3.x[2] * 11.1 +
+ l2_3.x[3] * 1.3 - 5.;
+ }
+ if (l9_4.index1[1]) {
+ l3_3.g[1] = l2_3.x[0] * 12. + l2_3.x[1] * 11.9 + l2_3.x[2] * 41.8 +
+ l2_3.x[3] * 52.1 - std::sqrt(q) * 1.645 - 12.;
+ }
+ if (l9_4.index1[2]) {
+ l3_3.g[2] = l2_3.x[0] + l2_3.x[1] + l2_3.x[2] + l2_3.x[3] - 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_4.index2[1]) {
+ goto L7;
+ }
+ l5_7.gg[1] = 12. - l2_3.x[0] * .46208050000000006 / std::sqrt(q);
+ l5_7.gg[4] = 11.9 - l2_3.x[1] * .31847199999999998 / std::sqrt(q);
+ l5_7.gg[7] = 41.8 - l2_3.x[2] * 33.311250000000001 / std::sqrt(q);
+ l5_7.gg[10] = 52.1 - l2_3.x[3] * 1.0266445000000002 / std::sqrt(q);
+L7:
+ return 0;
+} /* tp353_ */
+
+/* Subroutine */ int tp354_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 1;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_3.x[0] = 3.;
+ l2_3.x[1] = -1.;
+ l2_3.x[2] = 0.;
+ l2_3.x[3] = 1.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l5_2.gg[i__ - 1] = 1.;
+ l12_3.lxu[i__ - 1] = true;
+ l11_3.lxl[i__ - 1] = false;
+/* labelL6: */
+ l14_3.xu[i__ - 1] = 20.;
+ }
+ l20_6.lex = false;
+ l20_6.nex = 1;
+ l20_6.fex = .11378386;
+ l20_6.xex[0] = .50336521;
+ l20_6.xex[1] = -.04556907;
+ l20_6.xex[2] = .23580504;
+ l20_6.xex[3] = .30639882;
+ return 0;
+labelL2:
+/* Computing 2nd power */
+ d__1 = l2_3.x[0] + l2_3.x[1] * 10.;
+/* Computing 2nd power */
+ d__2 = l2_3.x[2] - l2_3.x[3];
+/* Computing 4th power */
+ d__3 = l2_3.x[1] - l2_3.x[2] * 2., d__3 *= d__3;
+/* Computing 4th power */
+ d__4 = l2_3.x[0] - l2_3.x[3], d__4 *= d__4;
+ l6_1.fx = d__1 * d__1 + d__2 * d__2 * 5. + d__3 * d__3 + d__4 * d__4 *
+ 10.;
+ return 0;
+labelL3:
+/* Computing 3rd power */
+ d__1 = l2_3.x[0] - l2_3.x[3];
+ l4_3.gf[0] = l2_3.x[0] * 2. + l2_3.x[1] * 20. + d__1 * (d__1 * d__1) *
+ 40.;
+/* Computing 3rd power */
+ d__1 = l2_3.x[1] - l2_3.x[2] * 2.;
+ l4_3.gf[1] = l2_3.x[0] * 20. + l2_3.x[1] * 200. + d__1 * (d__1 * d__1) *
+ 4.;
+/* Computing 3rd power */
+ d__1 = l2_3.x[1] - l2_3.x[2] * 2.;
+ l4_3.gf[2] = l2_3.x[2] * 10. - l2_3.x[3] * 10. - d__1 * (d__1 * d__1) *
+ 8.;
+/* Computing 3rd power */
+ d__1 = l2_3.x[0] - l2_3.x[3];
+ l4_3.gf[3] = l2_3.x[2] * -10. + l2_3.x[3] * 10. - d__1 * (d__1 * d__1) *
+ 40.;
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+ l3_1.g[0] = l2_3.x[0] + l2_3.x[1] + l2_3.x[2] + l2_3.x[3] - 1.;
+ }
+labelL5:
+ return 0;
+} /* tp354_ */
+
+
+/* Subroutine */ int tp355_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__;
+ static Real r__[4], h0, h1, h2, h3, h4, h5, h6;
+
+ h0 = l2_3.x[1] * l2_3.x[3];
+ r__[0] = 11. - l2_3.x[0] * l2_3.x[3] - h0 + l2_3.x[2] * l2_3.x[3];
+ r__[1] = l2_3.x[0] + l2_3.x[1] * 10. - l2_3.x[2] + l2_3.x[3] + h0 * (
+ l2_3.x[2] - l2_3.x[0]);
+ r__[2] = 11. - l2_3.x[0] * 4. * l2_3.x[3] - h0 * 4. + l2_3.x[2] * l2_3.x[
+ 3];
+ r__[3] = l2_3.x[0] * 2. + l2_3.x[1] * 20. - l2_3.x[2] * .5 + l2_3.x[3] *
+ 2. + h0 * 2. * (l2_3.x[2] - l2_3.x[0] * 4.);
+ h1 = r__[0] * 2.;
+ h2 = r__[1] * 2.;
+ h3 = r__[2] * 2.;
+ h4 = r__[3] * 2.;
+ h5 = h1 * l2_3.x[3];
+ h6 = h2 * (1. - h0);
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l12_3.lxu[i__ - 1] = false;
+ l11_3.lxl[i__ - 1] = true;
+/* labelL6: */
+ l2_3.x[i__ - 1] = 0.;
+ }
+ l13_3.xl[0] = .1;
+ l13_3.xl[1] = .1;
+ l13_3.xl[2] = 0.;
+ l13_3.xl[3] = 0.;
+ l20_6.lex = false;
+ l20_6.nex = 1;
+ l20_6.fex = 69.675463;
+ l20_6.xex[0] = 1.916633;
+ l20_6.xex[1] = .1;
+ l20_6.xex[2] = 0.;
+ l20_6.xex[3] = 1.9718118;
+ return 0;
+labelL2:
+ l6_1.fx = r__[0] * r__[0] + r__[1] * r__[1];
+ return 0;
+labelL3:
+ l4_3.gf[0] = -h5 + h6;
+ l4_3.gf[1] = -h5 + h2 * (l2_3.x[3] * (l2_3.x[2] - l2_3.x[0]) + 10.);
+ l4_3.gf[2] = h5 - h6;
+ l4_3.gf[3] = h1 * (-l2_3.x[0] - l2_3.x[1] + l2_3.x[2]) + h2 * (l2_3.x[1] *
+ (l2_3.x[2] - l2_3.x[0]) + 1.);
+ return 0;
+labelL4:
+ if (l9_2.index1[0]) {
+/* Computing 2nd power */
+ d__1 = r__[0];
+/* Computing 2nd power */
+ d__2 = r__[1];
+/* Computing 2nd power */
+ d__3 = r__[2];
+/* Computing 2nd power */
+ d__4 = r__[3];
+ l3_1.g[0] = d__1 * d__1 + d__2 * d__2 - d__3 * d__3 - d__4 * d__4;
+ }
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto L7;
+ }
+ l5_2.gg[0] = -h5 + h6 - h3 * -4. * l2_3.x[3] - h4 * (2. - h0 * 8.);
+ l5_2.gg[1] = -h5 + h2 * (l2_3.x[3] * (l2_3.x[2] - l2_3.x[0]) + 10.) - h3 *
+ -4. * l2_3.x[3] - h4 * (l2_3.x[3] * 2. * (l2_3.x[2] - l2_3.x[0] *
+ 4.) + 20.);
+ l5_2.gg[2] = h5 - h6 - h3 * l2_3.x[3] - h4 * (h0 * 2. - .5);
+ l5_2.gg[3] = h1 * (-l2_3.x[0] - l2_3.x[1] + l2_3.x[2]) + h2 * (l2_3.x[1] *
+ (l2_3.x[2] - l2_3.x[0]) + 1.) - h3 * (l2_3.x[0] * -4. - l2_3.x[1]
+ * 4. + l2_3.x[2]) - h4 * (l2_3.x[1] * 2. * (l2_3.x[2] - l2_3.x[0]
+ * 4.) + 2.);
+L7:
+ return 0;
+} /* tp355_ */
+
+
+/* Subroutine */ int tp356_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real eidc, load, cosa, sigd, eitc, e;
+ static int i__;
+ static Real j, l, m, r__, t, reidc, reitc, t1, t2, fh, ei, gh, gj,
+ pc, td, wp, del, phi[5], sig;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL3;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 1;
+ l1_1.ninl = 4;
+ l1_1.nenl = 0;
+ l1_1.nenl = 0;
+ l2_3.x[0] = 1.;
+ l2_3.x[1] = 7.;
+ l2_3.x[2] = 8.;
+ l2_3.x[3] = 1.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l12_3.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_3.lxl[i__ - 1] = true;
+ }
+ l11_3.lxl[3] = false;
+ l12_3.lxu[3] = false;
+ l13_3.xl[0] = .125;
+ l13_3.xl[1] = 0.;
+ l13_3.xl[2] = 0.;
+ l20_6.lex = false;
+ l20_6.nex = 1;
+ l20_6.fex = 2.3811648;
+ l20_6.xex[0] = .24436898;
+ l20_6.xex[1] = 6.2187934;
+ l20_6.xex[2] = 8.2914714;
+ l20_6.xex[3] = .24436898;
+ return 0;
+labelL2:
+ l6_1.fx = l2_3.x[0] * 1.10471 * l2_3.x[0] * l2_3.x[1] + l2_3.x[2] *
+ .04811 * l2_3.x[3] * (l2_3.x[1] + 14.);
+labelL3:
+ return 0;
+labelL4:
+ l = 14.;
+ load = 6e3;
+ td = 13600.;
+ sigd = 3e4;
+ fh = load;
+ t1 = fh / (l2_3.x[0] * 1.414 * l2_3.x[1]);
+ m = fh * (l + l2_3.x[1] / 2.);
+/* Computing 2nd power */
+ d__1 = (l2_3.x[2] + l2_3.x[0]) / 2.;
+ r__ = std::sqrt(l2_3.x[1] * l2_3.x[1] / 4. + d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = (l2_3.x[2] + l2_3.x[0]) / 2.;
+ j = l2_3.x[0] * .707 * l2_3.x[1] * (l2_3.x[1] * l2_3.x[1] / 12. + d__1 *
+ d__1) * 2.;
+ t2 = m * r__ / j;
+ cosa = l2_3.x[1] / (r__ * 2.);
+ wp = (d__1 = t1 * t1 + t1 * 2. * t2 * cosa + t2 * t2, std::abs(d__1));
+ if (wp > (float)0.) {
+ t = std::sqrt(wp);
+ } else {
+ t = (float)0.;
+ }
+ sig = fh * 6. * l / (l2_3.x[3] * l2_3.x[2] * l2_3.x[2]);
+ phi[0] = (td - t) / 1e4;
+ phi[1] = (sigd - sig) / 1e4;
+ phi[2] = l2_3.x[3] - l2_3.x[0];
+ e = 3e7;
+ ei = e * l2_3.x[2] * l2_3.x[3] * l2_3.x[3] * l2_3.x[3] / 12.;
+ gh = 1.2e7;
+ gj = gh * l2_3.x[2] * l2_3.x[3] * l2_3.x[3] * l2_3.x[3] / 3.;
+ eitc = ei * gj;
+ eidc = ei / gj;
+ if (eitc > (float)0.) {
+ reitc = std::sqrt(eitc);
+ } else {
+ reitc = (float)0.;
+ }
+ if (eidc > (float)0.) {
+ reidc = std::sqrt(eidc);
+ } else {
+ reidc = (float)0.;
+ }
+ pc = reitc * 4.013 * (1. - l2_3.x[2] / (l * 2.) * reidc) / (l * l);
+ phi[3] = (pc - fh) / 1e4;
+ del = fh * 4. * l * l * l / (e * l2_3.x[3] * l2_3.x[2] * l2_3.x[2] *
+ l2_3.x[2]);
+ phi[4] = .25 - del;
+ if (l9_5.index1[0]) {
+ l3_4.g[0] = phi[2];
+ }
+ if (l9_5.index1[1]) {
+ l3_4.g[1] = phi[0];
+ }
+ if (l9_5.index1[2]) {
+ l3_4.g[2] = phi[1];
+ }
+ if (l9_5.index1[3]) {
+ l3_4.g[3] = phi[3];
+ }
+ if (l9_5.index1[4]) {
+ l3_4.g[4] = phi[4];
+ }
+ return 0;
+} /* tp356_ */
+
+
+/* Subroutine */ int tp357_(int *mode)
+{
+ /* Initialized data */
+
+ static Real xpt[36] = { 113.,110.1,106.2,101.3,95.4,88.8,81.6,74.,
+ 66.1,58.4,51.,44.3,38.7,34.5,32.4,32.9,36.4,42.8,50.9,59.,65.8,
+ 71.5,76.5,81.1,85.6,90.2,94.6,98.9,103.,106.7,109.9,112.5,114.4,
+ 115.5,115.7,114.9 };
+ static Real ypt[36] = { 40.2,46.8,53.3,59.4,65.,69.9,73.9,76.9,78.9,
+ 79.8,79.7,78.5,76.5,73.6,70.2,66.,60.9,54.3,45.8,36.1,26.5,18.1,
+ 11.4,6.2,2.6,.3,-.7,-.6,.7,3.1,6.4,10.5,15.5,21.,27.1,33.6 };
+ static Real p0 = 90.;
+ static Real q0 = 0.;
+ static Real r0 = 0.;
+ static Real s0 = 0.;
+
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4, d__5, d__6;
+
+ /* Local variables */
+ static Real test, test1, a, b, c__;
+ static int i__;
+ static Real j;
+ static int k;
+ static Real alpha, calcx, calcy, p1, q1, r1, s1, ca, cp, sa, ph,
+ dalpha, pi, qi, ri, si, sp, sql, sum, aabb;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL3;
+ case 5: goto labelL3;
+ }
+labelL1:
+ l1_1.n = 4;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.nenl = 0;
+ l1_1.nenl = 0;
+ l2_3.x[0] = 136.;
+ l2_3.x[1] = 0.;
+ l2_3.x[2] = 74.8;
+ l2_3.x[3] = 75.7;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l11_3.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_3.lxu[i__ - 1] = true;
+ }
+ l20_6.lex = false;
+ l20_6.nex = 1;
+ l20_6.fex = .3584566;
+ l20_6.xex[0] = 136.00762;
+ l20_6.xex[1] = .031371415;
+ l20_6.xex[2] = 73.59439;
+ l20_6.xex[3] = 72.187426;
+ for (k = 1; k <= 4; ++k) {
+/* L7: */
+ l13_3.xl[k - 1] = 0.;
+ }
+ l14_3.xu[0] = 150.;
+ l14_3.xu[1] = 50.;
+ l14_3.xu[2] = 100.;
+ l14_3.xu[3] = 100.;
+ return 0;
+labelL2:
+ dalpha = .17452927777777777;
+ sum = 0.;
+ p1 = l2_3.x[0];
+ q1 = l2_3.x[1];
+ r1 = l2_3.x[2];
+ s1 = l2_3.x[3];
+ for (i__ = 2; i__ <= 36; ++i__) {
+ alpha = dalpha * (i__ - 1);
+ ca =std::cos(alpha);
+ sa = std::sin(alpha);
+ pi = p1 * ca - q1 * sa + p0 * (1. - ca) + q0 * sa;
+ qi = p1 * sa + q1 * ca + q0 * (1. - ca) - p0 * sa;
+ a = r0 * s1 - s0 * r1 - q1 * r0 + p1 * s0 + pi * q1 - p1 * qi + qi *
+ r1 - pi * s1;
+ b = -r0 * r1 - s0 * s1 + p1 * r0 + q1 * s0 - p1 * pi - q1 * qi + pi *
+ r1 + qi * s1;
+ c__ = -r1 * r0 - s1 * s0 + pi * r0 + qi * s0 + p1 * r1 + q1 * s1 - (
+ p1 * p1 + q1 * q1 + pi * pi + qi * qi) / 2.;
+ aabb = a * a + b * b;
+ if (aabb < 1e-30) {
+ goto L50;
+ }
+ test = c__ / std::sqrt(aabb);
+ if (std::abs(test) > 1.) {
+ goto L51;
+ }
+ j = 1.;
+L52:
+ ph = std::asin(test) - std::atan(b / a);
+L55:
+ sp = std::sin(ph);
+ cp =std::cos(ph);
+ ri = r1 * cp - s1 * sp + pi - p1 * cp + q1 * sp;
+ si = r1 * sp + s1 * cp + qi - p1 * sp - q1 * cp;
+/* Computing 2nd power */
+ d__1 = r1 - r0;
+/* Computing 2nd power */
+ d__2 = s1 - s0;
+ test1 = d__1 * d__1 + d__2 * d__2;
+ if (test1 < 1e-10) {
+ test1 = 1e-10;
+ }
+/* Computing 2nd power */
+ d__2 = ri - r0;
+/* Computing 2nd power */
+ d__3 = si - s0;
+ if ((d__1 = (test1 - d__2 * d__2 - d__3 * d__3) / test1, std::abs(d__1)) <
+ .001) {
+ goto L53;
+ }
+ if (j == 2.) {
+ goto L51;
+ }
+ test = -test;
+ j = 2.;
+ goto L52;
+L50:
+ ph = -std::atan(b / a);
+ goto L55;
+L51:
+ l6_1.fx = 1e20;
+ return 0;
+L53:
+ calcx = xpt[0] * cp - ypt[0] * sp + pi - p1 * cp + q1 * sp;
+ calcy = xpt[0] * sp + ypt[0] * cp + qi - p1 * sp - q1 * cp;
+/* L54: */
+/* Computing 2nd power */
+ d__1 = calcx - xpt[i__ - 1];
+/* Computing 2nd power */
+ d__2 = calcy - ypt[i__ - 1];
+ sum = sum + d__1 * d__1 + d__2 * d__2;
+ }
+/* Computing 2nd power */
+ d__1 = r1 - r0;
+/* Computing 2nd power */
+ d__2 = s1 - s0;
+/* Computing 2nd power */
+ d__3 = r1 - p1;
+/* Computing 2nd power */
+ d__4 = s1 - q1;
+/* Computing 2nd power */
+ d__5 = p1 - p0;
+/* Computing 2nd power */
+ d__6 = q1 - q0;
+ sql = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 + d__4 * d__4 + d__5 * d__5
+ + d__6 * d__6;
+ l6_1.fx = sum / 100. + sql / 62500.;
+labelL3:
+ return 0;
+} /* tp357_ */
+
+
+/* Subroutine */ int tp358_(int *mode)
+{
+ /* Initialized data */
+
+ static Real y[33] = { .844,.908,.932,.936,.925,.908,.881,.85,.818,
+ .784,.751,.718,.685,.658,.628,.603,.58,.558,.538,.522,.506,.49,
+ .478,.467,.457,.448,.438,.431,.424,.42,.414,.411,.406 };
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__, j;
+ static Real ti;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.nenl = 0;
+ l1_1.nenl = 0;
+ l2_4.x[0] = .5;
+ l2_4.x[1] = 1.5;
+ l2_4.x[2] = -1.;
+ l2_4.x[3] = .01;
+ l2_4.x[4] = .02;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l12_4.lxu[i__ - 1] = true;
+/* labelL6: */
+ l11_4.lxl[i__ - 1] = true;
+ }
+ l13_4.xl[0] = -.5;
+ l14_4.xu[0] = .5;
+ l13_4.xl[1] = 1.5;
+ l14_4.xu[1] = 2.5;
+ l13_4.xl[2] = -2.;
+ l14_4.xu[2] = -1.;
+ l13_4.xl[3] = .001;
+ l14_4.xu[3] = .1;
+ l13_4.xl[4] = .001;
+ l14_4.xu[4] = .1;
+ l20_7.lex = false;
+ l20_7.nex = 1;
+ l20_7.fex = 5.46e-5;
+ l20_7.xex[0] = .3754;
+ l20_7.xex[1] = 1.9358;
+ l20_7.xex[2] = -1.4647;
+ l20_7.xex[3] = .01287;
+ l20_7.xex[4] = .02212;
+ l15_1.lsum = 33;
+ return 0;
+labelL2:
+ for (i__ = 1; i__ <= 33; ++i__) {
+ ti = (Real) (i__ - 1) * 10.;
+/* labelL20: */
+ l16_17.f[i__ - 1] = y[i__ - 1] - (l2_4.x[0] + l2_4.x[1] * std::exp(-l2_4.x[
+ 3] * ti) + l2_4.x[2] * std::exp(-l2_4.x[4] * ti));
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 33; ++i__) {
+/* L7: */
+/* Computing 2nd power */
+ d__1 = l16_17.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ for (j = 1; j <= 5; ++j) {
+/* L8: */
+ l4_4.gf[j - 1] = 0.;
+ }
+ for (i__ = 1; i__ <= 33; ++i__) {
+ ti = (Real) (i__ - 1) * 10.;
+ l17_20.df[i__ - 1] = -1.;
+ l17_20.df[i__ + 32] = -exp(-l2_4.x[3] * ti);
+ l17_20.df[i__ + 65] = -exp(-l2_4.x[4] * ti);
+ l17_20.df[i__ + 98] = l2_4.x[1] * std::exp(-l2_4.x[3] * ti) * ti;
+ l17_20.df[i__ + 131] = l2_4.x[2] * std::exp(-l2_4.x[4] * ti) * ti;
+ for (j = 1; j <= 5; ++j) {
+/* labelL10: */
+ l4_4.gf[j - 1] += l16_17.f[i__ - 1] * 2. * l17_20.df[i__ + j * 33
+ - 34];
+ }
+ }
+labelL4:
+ return 0;
+} /* tp358_ */
+
+
+/* Subroutine */ int tp359_(int *mode)
+{
+ /* Initialized data */
+
+ static Real b[5] = { -145421.402,2931.1506,-40.427932,5106.192,
+ 15711.36 };
+ static Real c__[5] = { -155011.1084,4360.53352,12.9492344,10236.884,
+ 13176.786 };
+ static Real d__[5] = { -326669.5104,7390.68412,-27.8986976,
+ 16643.076,30988.146 };
+ static Real a[5] = { -8720288.849,150512.5253,-156.6950325,
+ 476470.3222,729482.8271 };
+
+ static Real h__[6];
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 14;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_4.x[0] = 2.52;
+ l2_4.x[1] = 5.04;
+ l2_4.x[2] = 94.5;
+ l2_4.x[3] = 23.31;
+ l2_4.x[4] = 17.136;
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* labelL10: */
+ l4_4.gf[i__ - 1] = -a[i__ - 1];
+ }
+ for (j = 1; j <= 8; ++j) {
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* labelL6: */
+ l5_33.gg[j + i__ * 14 - 15] = 0.;
+ }
+ }
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l5_33.gg[(i__ << 1) - 1 + (i__ + 1) * 14 - 15] = -1.;
+/* L7: */
+ l5_33.gg[(i__ << 1) + (i__ + 1) * 14 - 15] = 1.;
+ }
+ l5_33.gg[0] = 2.4;
+ l5_33.gg[1] = -1.2;
+ l5_33.gg[2] = 60.;
+ l5_33.gg[3] = -20.;
+ l5_33.gg[4] = 9.3;
+ l5_33.gg[5] = -9.;
+ l5_33.gg[6] = 7.;
+ l5_33.gg[7] = -6.5;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l5_33.gg[i__ * 14 - 6] = b[i__ - 1];
+ l5_33.gg[i__ * 14 - 5] = c__[i__ - 1];
+ l5_33.gg[i__ * 14 - 4] = d__[i__ - 1];
+ l5_33.gg[i__ * 14 - 3] = -b[i__ - 1];
+ l5_33.gg[i__ * 14 - 2] = -c__[i__ - 1];
+ l5_33.gg[i__ * 14 - 1] = -d__[i__ - 1];
+ l12_4.lxu[i__ - 1] = false;
+/* L8: */
+ l11_4.lxl[i__ - 1] = false;
+ }
+ l20_7.lex = false;
+ l20_7.nex = 1;
+ l20_7.fex = -5280416.8;
+ l20_7.xex[0] = 4.5374;
+ l20_7.xex[1] = 10.889;
+ l20_7.xex[2] = 272.24;
+ l20_7.xex[3] = 42.198;
+ l20_7.xex[4] = 31.762;
+ return 0;
+labelL2:
+ l6_1.fx = -24345.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* labelL9: */
+ l6_1.fx += a[i__ - 1] * l2_4.x[i__ - 1];
+ }
+ l6_1.fx = -l6_1.fx;
+labelL3:
+ return 0;
+labelL4:
+ if (l9_8.index1[0]) {
+ l3_7.g[0] = l2_4.x[0] * 2.4 - l2_4.x[1];
+ }
+ if (l9_8.index1[1]) {
+ l3_7.g[1] = l2_4.x[0] * -1.2 + l2_4.x[1];
+ }
+ if (l9_8.index1[2]) {
+ l3_7.g[2] = l2_4.x[0] * 60. - l2_4.x[2];
+ }
+ if (l9_8.index1[3]) {
+ l3_7.g[3] = l2_4.x[0] * -20. + l2_4.x[2];
+ }
+ if (l9_8.index1[4]) {
+ l3_7.g[4] = l2_4.x[0] * 9.3 - l2_4.x[3];
+ }
+ if (l9_8.index1[5]) {
+ l3_7.g[5] = l2_4.x[0] * -9. + l2_4.x[3];
+ }
+ if (l9_8.index1[6]) {
+ l3_7.g[6] = l2_4.x[0] * 7. - l2_4.x[4];
+ }
+ if (l9_8.index1[7]) {
+ l3_7.g[7] = l2_4.x[0] * -6.5 + l2_4.x[4];
+ }
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* labelL11: */
+ h__[i__ - 1] = 0.;
+ }
+ h__[3] = 2.94e5;
+ h__[4] = 2.94e5;
+ h__[5] = 277200.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ h__[0] += b[i__ - 1] * l2_4.x[i__ - 1];
+ h__[1] += c__[i__ - 1] * l2_4.x[i__ - 1];
+ h__[2] += d__[i__ - 1] * l2_4.x[i__ - 1];
+ h__[3] -= b[i__ - 1] * l2_4.x[i__ - 1];
+ h__[4] -= c__[i__ - 1] * l2_4.x[i__ - 1];
+/* labelL12: */
+ h__[5] -= d__[i__ - 1] * l2_4.x[i__ - 1];
+ }
+ for (i__ = 9; i__ <= 14; ++i__) {
+/* labelL13: */
+ if (l9_8.index1[i__ - 1]) {
+ l3_7.g[i__ - 1] = h__[i__ - 9];
+ }
+ }
+labelL5:
+ return 0;
+} /* tp359_ */
+
+
+/* Subroutine */ int tp360_(int *mode)
+{
+ /* Initialized data */
+
+ static Real c__[10] = { -8720288.849,150512.5233,-156.6950325,
+ 476470.3222,729482.8271,-326669.5104,7390.68412,-27.8986976,
+ 16643.076,30988.146 };
+
+ static Real h__;
+ static int i__;
+ static Real hh[5];
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_4.x[0] = 2.52;
+ l2_4.x[1] = 2.;
+ l2_4.x[2] = 37.5;
+ l2_4.x[3] = 9.25;
+ l2_4.x[4] = 6.8;
+ l11_4.lxl[0] = true;
+ l12_4.lxu[0] = false;
+ for (i__ = 2; i__ <= 5; ++i__) {
+ l11_4.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_4.lxu[i__ - 1] = true;
+ }
+ l13_4.xl[0] = 0.;
+ l13_4.xl[1] = 1.2;
+ l13_4.xl[2] = 20.;
+ l13_4.xl[3] = 9.;
+ l13_4.xl[4] = 6.5;
+ l14_4.xu[1] = 2.4;
+ l14_4.xu[2] = 60.;
+ l14_4.xu[3] = 9.3;
+ l14_4.xu[4] = 7.;
+ l20_7.lex = false;
+ l20_7.nex = 1;
+ l20_7.fex = -5280335.1;
+ l20_7.xex[0] = 4.537431;
+ l20_7.xex[1] = 2.4;
+ l20_7.xex[2] = 60.;
+ l20_7.xex[3] = 9.3;
+ l20_7.xex[4] = 7.;
+ return 0;
+labelL2:
+ l6_1.fx = (-c__[0] - c__[1] * l2_4.x[1] - c__[2] * l2_4.x[2] - c__[3] *
+ l2_4.x[3] - c__[4] * l2_4.x[4]) * l2_4.x[0] + 24345.;
+ return 0;
+labelL3:
+ l4_4.gf[0] = -c__[0] - c__[1] * l2_4.x[1] - c__[2] * l2_4.x[2] - c__[3] *
+ l2_4.x[3] - c__[4] * l2_4.x[4];
+ for (i__ = 2; i__ <= 5; ++i__) {
+/* L7: */
+ l4_4.gf[i__ - 1] = -c__[i__ - 1] * l2_4.x[0];
+ }
+ return 0;
+labelL4:
+ h__ = (c__[5] + c__[6] * l2_4.x[1] + c__[7] * l2_4.x[2] + c__[8] * l2_4.x[
+ 3] + c__[9] * l2_4.x[4]) * l2_4.x[0];
+ if (l9_3.index1[0]) {
+ l3_2.g[0] = h__;
+ }
+ if (l9_3.index1[1]) {
+ l3_2.g[1] = 277200. - h__;
+ }
+ return 0;
+labelL5:
+ hh[0] = c__[5] + c__[6] * l2_4.x[1] + c__[7] * l2_4.x[2] + c__[8] *
+ l2_4.x[3] + c__[9] * l2_4.x[4];
+ for (i__ = 2; i__ <= 5; ++i__) {
+/* L8: */
+ hh[i__ - 1] = c__[i__ + 4] * l2_4.x[0];
+ }
+ if (! l10_3.index2[0]) {
+ goto labelL11;
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* labelL9: */
+ l5_4.gg[(i__ << 1) - 2] = hh[i__ - 1];
+ }
+labelL11:
+ if (! l10_3.index2[1]) {
+ goto labelL12;
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* labelL10: */
+ l5_4.gg[(i__ << 1) - 1] = -hh[i__ - 1];
+ }
+labelL12:
+ return 0;
+} /* tp360_ */
+
+
+/* Subroutine */ int tp361_(int *mode)
+{
+ /* Initialized data */
+
+ static Real a[5] = { -8720288.849,150512.5253,-156.6950325,
+ 476470.3222,729482.8271 };
+ static Real b[5] = { -145421.402,2931.1506,-40.427932,5106.192,
+ 15711.36 };
+ static Real c__[5] = { -155011.1084,4360.53352,12.9492344,10236.884,
+ 13176.786 };
+ static Real d__[5] = { -326669.5104,7390.68412,-27.8986976,
+ 16643.076,30988.146 };
+
+ static Real h__[3];
+ static int i__, j;
+ static Real hh[15] /* was [3][5] */;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 6;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_4.x[0] = 2.52;
+ l2_4.x[1] = 2.;
+ l2_4.x[2] = 37.5;
+ l2_4.x[3] = 9.25;
+ l2_4.x[4] = 6.8;
+ l11_4.lxl[4] = false;
+ l12_4.lxu[0] = false;
+ for (i__ = 1; i__ <= 4; ++i__) {
+ l11_4.lxl[i__ - 1] = true;
+/* labelL6: */
+ l12_4.lxu[i__] = true;
+ }
+ l13_4.xl[0] = 0.;
+ l13_4.xl[1] = 1.2;
+ l13_4.xl[2] = 20.;
+ l13_4.xl[3] = 9.;
+ l14_4.xu[1] = 2.4;
+ l14_4.xu[2] = 60.;
+ l14_4.xu[3] = 9.3;
+ l14_4.xu[4] = 7.;
+ l20_7.lex = false;
+ l20_7.nex = 1;
+ l20_7.fex = -776412.12;
+ l20_7.xex[0] = .68128605;
+ l20_7.xex[1] = .24;
+ l20_7.xex[2] = 20.;
+ l20_7.xex[3] = 9.3;
+ l20_7.xex[4] = 7.;
+ return 0;
+labelL2:
+ l6_1.fx = a[0];
+ for (i__ = 2; i__ <= 5; ++i__) {
+/* L7: */
+ l6_1.fx += a[i__ - 1] * l2_4.x[i__ - 1];
+ }
+ l6_1.fx = l2_4.x[0] * l6_1.fx - 24345.;
+ l6_1.fx = -l6_1.fx;
+ return 0;
+labelL3:
+ l4_4.gf[0] = a[0];
+ for (i__ = 2; i__ <= 5; ++i__) {
+ l4_4.gf[0] += a[i__ - 1] * l2_4.x[i__ - 1];
+/* L8: */
+ l4_4.gf[i__ - 1] = a[i__ - 1] * l2_4.x[0];
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* labelL20: */
+ l4_4.gf[i__ - 1] = -l4_4.gf[i__ - 1];
+ }
+ return 0;
+labelL4:
+ h__[0] = b[0];
+ h__[1] = c__[0];
+ h__[2] = d__[0];
+ for (i__ = 2; i__ <= 5; ++i__) {
+ h__[0] += b[i__ - 1] * l2_4.x[i__ - 1];
+ h__[1] += c__[i__ - 1] * l2_4.x[i__ - 1];
+/* labelL9: */
+ h__[2] += d__[i__ - 1] * l2_4.x[i__ - 1];
+ }
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* labelL10: */
+ h__[i__ - 1] = l2_4.x[0] * h__[i__ - 1];
+ }
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* labelL11: */
+ if (l9_6.index1[i__ - 1]) {
+ l3_5.g[i__ - 1] = h__[i__ - 1];
+ }
+ }
+ if (l9_6.index1[3]) {
+ l3_5.g[3] = 29400. - h__[0];
+ }
+ if (l9_6.index1[4]) {
+ l3_5.g[4] = 29400. - h__[1];
+ }
+ if (l9_6.index1[5]) {
+ l3_5.g[5] = 277200. - h__[2];
+ }
+ return 0;
+labelL5:
+ hh[0] = b[0];
+ hh[1] = c__[0];
+ hh[2] = d__[0];
+ for (i__ = 2; i__ <= 5; ++i__) {
+ hh[0] += b[i__ - 1] * l2_4.x[i__ - 1];
+ hh[1] += c__[i__ - 1] * l2_4.x[i__ - 1];
+ hh[2] += d__[i__ - 1] * l2_4.x[i__ - 1];
+ hh[i__ * 3 - 3] = b[i__ - 1] * l2_4.x[0];
+ hh[i__ * 3 - 2] = c__[i__ - 1] * l2_4.x[0];
+/* labelL12: */
+ hh[i__ * 3 - 1] = d__[i__ - 1] * l2_4.x[0];
+ }
+ for (j = 1; j <= 3; ++j) {
+ if (! l10_6.index2[j - 1]) {
+ goto labelL13;
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L130: */
+ l5_14.gg[j + i__ * 6 - 7] = hh[j + i__ * 3 - 4];
+ }
+labelL13:
+ ;
+ }
+ for (j = 4; j <= 6; ++j) {
+ if (! l10_6.index2[j - 1]) {
+ goto labelL14;
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+/* L140: */
+ l5_14.gg[j + i__ * 6 - 7] = -hh[j - 3 + i__ * 3 - 4];
+ }
+labelL14:
+ ;
+ }
+ return 0;
+} /* tp361_ */
+
+
+/* Subroutine */ int tp362_(int *mode)
+{
+ Real tp362a_(Real *);
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 4;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_4.x[0] = 15.;
+ l2_4.x[1] = 9.05;
+ l2_4.x[2] = 6.14;
+ l2_4.x[3] = 4.55;
+ l2_4.x[4] = 3.61;
+ for (i__ = 2; i__ <= 4; ++i__) {
+ l11_4.lxl[i__ - 1] = false;
+/* labelL14: */
+ l12_4.lxu[i__ - 1] = false;
+ }
+ l11_4.lxl[0] = true;
+ l12_4.lxu[0] = true;
+ l11_4.lxl[4] = true;
+ l12_4.lxu[4] = false;
+ l13_4.xl[0] = 15.;
+ l14_4.xu[0] = 20.;
+ l13_4.xl[4] = 2.;
+ l20_7.lex = false;
+ l20_7.nex = 1;
+ l20_7.fex = .26229998;
+ l20_7.xex[0] = 15.050962;
+ l20_7.xex[1] = 8.8751199;
+ l20_7.xex[2] = 5.908823;
+ l20_7.xex[3] = 4.860481;
+ l20_7.xex[4] = 4.399269;
+ return 0;
+labelL2:
+ l6_1.fx = tp362a_(l2_4.x);
+ l6_1.fx = l6_1.fx;
+labelL3:
+ return 0;
+labelL4:
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* L41: */
+ if (l9_7.index1[i__ - 1]) {
+ l3_6.g[i__ - 1] = l2_4.x[i__ - 1] - l2_4.x[i__];
+ }
+ }
+labelL5:
+ return 0;
+} /* tp362_ */
+
+Real tp362a_(Real *x)
+{
+ /* Initialized data */
+
+ static Real rad = 1.085;
+ static Real con1 = 1.466667;
+ static Real con2 = 12.90842;
+ static Real rpmin = 600.;
+ static Real rpmax = 5700.;
+ static Real ei = .6;
+ static Real vi = 98.;
+ static Real dt = .01;
+ static Real vmax = 100.;
+ static Real v0 = 5.;
+ static Real tshift = .25;
+ static Real tmax = 100.;
+
+ /* System generated locals */
+ Real ret_val, d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+ static Real t, v, force;
+ static int it;
+ static Real tt, torque, acc, rpm, acc0;
+
+ /* Parameter adjustments */
+ --x;
+
+ /* Function Body */
+/* labelL13: */
+ it = 0;
+ acc = 0.;
+ v = v0;
+ i__ = 1;
+L302:
+/* Computing 2nd power */
+ d__1 = v;
+ force = d__1 * d__1 * .0239 + 31.2;
+L301:
+ rpm = v * con2 * x[i__];
+ if (rpm < rpmin) {
+ goto L300;
+ }
+ if (rpm >= rpmax) {
+ goto L305;
+ }
+ if (rpm >= 600. && rpm < 1900.) {
+/* Computing 3rd power */
+ d__1 = rpm;
+/* Computing 2nd power */
+ d__2 = rpm;
+ torque = d__1 * (d__1 * d__1) * 3.846154e-8 - d__2 * d__2 *
+ 2.108974359e-4 + rpm * .42455128205133 - 187.11538461540295;
+ }
+ if (rpm >= 1900. && rpm < 3e3) {
+/* Computing 3rd power */
+ d__1 = rpm;
+/* Computing 2nd power */
+ d__2 = rpm;
+ torque = d__1 * (d__1 * d__1) * -4.92424e-9 + d__2 * d__2 *
+ 1.867424242e-5 + rpm * .01229545454547 + 64.999999999986;
+ }
+ if (rpm >= 3e3 && rpm < 4500.) {
+/* Computing 3rd power */
+ d__1 = rpm;
+/* Computing 2nd power */
+ d__2 = rpm;
+ torque = d__1 * (d__1 * d__1) * -2.6667e-10 + d__2 * d__2 * 3e-6 -
+ rpm * .01263333333336 + 155.10000000002947;
+ }
+ if (rpm >= 4500. && rpm < 5600.) {
+/* Computing 3rd power */
+ d__1 = rpm;
+/* Computing 2nd power */
+ d__2 = rpm;
+ torque = d__1 * (d__1 * d__1) * -6.64141e-9 + d__2 * d__2 *
+ 8.337626263e-5 - rpm * .34351868688129 + 597.3636363847145;
+ }
+ if (rpm >= 5600. && rpm < 6e3) {
+/* Computing 3rd power */
+ d__1 = rpm;
+/* Computing 2nd power */
+ d__2 = rpm;
+ torque = d__1 * (d__1 * d__1) * -2.539683e-8 + d__2 * d__2 *
+ 3.8158730157e-4 - rpm * 1.9223492062348 + 3380.66666645715304;
+ }
+ acc0 = acc;
+/* Computing 2nd power */
+ d__1 = x[i__];
+ acc = rad * (x[i__] * torque - force * rad) / (ei * (d__1 * d__1) + vi);
+ ++it;
+ t = dt * it;
+ v += (acc0 + acc) / 2. * dt / con1;
+ if (t > tmax) {
+ goto L311;
+ }
+ if (v >= vmax) {
+ goto L311;
+ }
+ goto L302;
+L300:
+ ret_val = tmax;
+ return ret_val;
+L305:
+ ++i__;
+ if (i__ > 5) {
+ goto L311;
+ }
+ if (t == 0.) {
+ goto L301;
+ }
+ tt = t + tshift;
+L306:
+/* Computing 2nd power */
+ d__1 = rad;
+ acc = -force * (d__1 * d__1) / vi;
+ ++it;
+ t = dt * it;
+ v += acc * dt / con1;
+ if (t < tt) {
+ goto L307;
+ }
+ goto L302;
+L307:
+/* Computing 2nd power */
+ d__1 = v;
+ force = d__1 * d__1 * .0293 + 31.2;
+ goto L306;
+L311:
+ ret_val = t / 100.;
+ return ret_val;
+} /* tp362a_ */
+
+
+/* Subroutine */ int tp363_(int *mode)
+{
+ Real tp363a_(Real *);
+ int tp363b_(bool *, Real *, Real *);
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 5;
+ l1_1.nili = 0;
+ l1_1.ninl = 3;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_4.x[0] = -.3359769;
+ l2_4.x[1] = -1.432398;
+ l2_4.x[2] = 0.;
+ l2_4.x[3] = 4.;
+ l2_4.x[4] = 9.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ l11_4.lxl[i__ - 1] = true;
+ l12_4.lxu[i__ - 1] = true;
+/* labelL11: */
+ l14_4.xu[i__ - 1] = 10.;
+ }
+ l13_4.xl[0] = -10.;
+ l13_4.xl[1] = -10.;
+ l13_4.xl[2] = -10.;
+ l13_4.xl[3] = .1;
+ l13_4.xl[4] = 1.;
+ l20_7.lex = false;
+ l20_7.nex = 1;
+ l20_7.xex[0] = .19852438;
+ l20_7.xex[1] = -3.01059794;
+ l20_7.xex[2] = -.0530266138;
+ l20_7.xex[3] = 2.83165094;
+ l20_7.xex[4] = 10.;
+ l20_7.fex = (float)-5.55840576;
+ return 0;
+labelL2:
+ l6_1.fx = tp363a_(l2_4.x);
+labelL3:
+ return 0;
+labelL4:
+ tp363b_(l9_4.index1, l2_4.x, l3_3.g);
+labelL5:
+ return 0;
+} /* tp363_ */
+
+Real tp363a_(Real *x)
+{
+ /* Initialized data */
+
+ static Real xi = 1.;
+ static Real x6 = 1.;
+ static Real x11 = 1.;
+
+ /* System generated locals */
+ int i__1;
+ Real ret_val;
+
+ /* Local variables */
+ int tp363c_(Real *x, Real *thick, Real *dthick, Real *c__, int *kkk, Real *c0, Real *xi);
+ int tp363d_(Real *, Real *);
+ static int i__;
+ static Real dr, xc[100];
+
+ /* Parameter adjustments */
+ --x;
+
+ /* Function Body */
+ b_1.w2 = x6 * 100.;
+ dr = x[5] - xi;
+ xc[0] = xi;
+ i__1 = b_1.kkk;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+/* L60: */
+ xc[i__] = xc[i__ - 1] + dr / (Real) b_1.kkk;
+ }
+ tp363c_(xc, b_1.thick, b_1.dthick, &x[1], &b_1.kkk, &x11, &xi);
+ tp363d_(xc, &ret_val);
+ ret_val = -ret_val / 1e6;
+ return ret_val;
+} /* tp363a_ */
+
+/* Subroutine */ int tp363b_(bool *index1, Real *c__, Real *g)
+{
+ /* Initialized data */
+
+ static Real yi = 0.;
+ static Real xi = 1.;
+ static Real eps = .1;
+ static Real c6 = 1.;
+
+ /* System generated locals */
+ int i__1;
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static Real smax, root, stot[100];
+ int tp363c_(Real *, Real *, Real *, Real *, int *, Real *, Real *);
+ int tp363e_(Real *, Real *, Real *, Real *, int *, Real *, Real *, Real*);
+ int tp363f_(Real *, Real *, Real *, Real *, Real *, Real *, Real *, Real *, Real *, Real *, Real *, Real *, int *);
+ int tp363g_(Real *, Real *, Real *, Real *, int *, Real *);
+ static int i__;
+ static Real v;
+ static Real x[100], y[100], y1[100];
+ static int ii;
+ static Real dr;
+ static int nn;
+ static Real xl, xr, y1i, fxl, fxr, rst[100], tst[100];
+
+ /* Parameter adjustments */
+ --g;
+ --c__;
+ --index1;
+
+ /* Function Body */
+ b_1.xmu = (float).3;
+ b_1.rho = 7.263e-4;
+ ii = 99;
+ b_1.kkk = 98;
+ b_1.w2 = c6 * 100.;
+ dr = c__[5] - xi;
+ x[0] = xi;
+ i__1 = b_1.kkk;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+/* L60: */
+ x[i__] = x[i__ - 1] + dr / (Real) b_1.kkk;
+ }
+ tp363c_(x, b_1.thick, b_1.dthick, &c__[1], &b_1.kkk, &c__[4], &xi);
+/* labelL5: */
+ y1i = 1e5;
+/* labelL2: */
+ tp363e_(&y1i, &yi, &xi, &c__[5], &ii, y, y1, x);
+ fxl = -y[ii - 1];
+ xl = y1i;
+ yi = 0.;
+ y1i = 2.503e6;
+ tp363e_(&y1i, &yi, &xi, &c__[5], &ii, y, y1, x);
+ fxr = -y[ii - 1];
+ xr = y1i;
+ tp363f_(&xl, &xr, &fxl, &fxr, &eps, y, y1, x, &xi, &c__[5], &yi, &root, &ii);
+ smax = 0.;
+ st_1.value2 = 0.;
+ i__1 = ii;
+ for (nn = 1; nn <= i__1; ++nn) {
+ rst[nn - 1] = y[nn - 1] / (b_1.thick[nn - 1] * x[nn - 1]);
+/* Computing 2nd power */
+ d__1 = b_1.w2;
+/* Computing 2nd power */
+ d__2 = x[nn - 1];
+ tst[nn - 1] = (y1[nn - 1] + b_1.thick[nn - 1] * b_1.rho * (d__1 *
+ d__1) * (d__2 * d__2)) / b_1.thick[nn - 1];
+/* Computing 2nd power */
+ d__1 = rst[nn - 1] - tst[nn - 1];
+/* Computing 2nd power */
+ d__2 = rst[nn - 1];
+/* Computing 2nd power */
+ d__3 = tst[nn - 1];
+ stot[nn - 1] = std::sqrt(d__1 * d__1 + d__2 * d__2 + d__3 * d__3);
+ if (stot[nn - 1] > smax) {
+ smax = stot[nn - 1];
+ }
+/* Computing 2nd power */
+ d__1 = 3e4 - stot[nn - 1];
+ st_1.value2 += d__1 * d__1;
+/* L300: */
+ }
+ st_1.value2 = std::sqrt(st_1.value2);
+ if (index1[1]) {
+ g[1] = (3e4 - smax) / 1e3;
+ }
+ if (index1[2]) {
+ g[2] = 5. - tfn1_1.tmax;
+ }
+ tp363g_(&c__[4], &c__[1], &v, b_1.thick, &b_1.kkk, x);
+ if (index1[3]) {
+ g[3] = (625. - v) / 10.;
+ }
+/* L900: */
+ return 0;
+} /* tp363b_ */
+
+/* Subroutine */ int tp363c_(Real *x, Real *thick, Real *dthick, Real *c__, int *kkk, Real *c0, Real *xi)
+{
+ /* System generated locals */
+ int i__1, i__2;
+
+ /* Local variables */
+ static int nfst, i__, lm;
+ static Real xl;
+ static int jkl;
+
+ /* Parameter adjustments */
+ --c__;
+ --dthick;
+ --thick;
+ --x;
+
+ /* Function Body */
+ thick[1] = *c0;
+ xl = x[*kkk + 1] - x[1];
+ nfst = 2;
+ tfn1_1.tmax = thick[1];
+ i__1 = *kkk;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ thick[i__ + 1] = *c0 + c__[1] * (x[i__ + 1] - *xi);
+ i__2 = nfst;
+ for (lm = 1; lm <= i__2; ++lm) {
+ jkl = lm + 1;
+/* labelL9: */
+ thick[i__ + 1] += c__[jkl] * std::sin(((Real) jkl * (float)2. - (
+ float)3.) * 3.1415926535897932 * (x[i__] - x[1]) / xl);
+ }
+ if (thick[i__ + 1] > tfn1_1.tmax) {
+ tfn1_1.tmax = thick[i__ + 1];
+ }
+/* labelL10: */
+ dthick[i__] = (thick[i__ + 1] - thick[i__]) / (x[i__ + 1] - x[i__]);
+ }
+ return 0;
+} /* tp363c_ */
+
+/* Subroutine */ int tp363d_(Real *x, Real *xke)
+{
+ /* System generated locals */
+ int i__1;
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+ static Real const__;
+
+ /* Parameter adjustments */
+ --x;
+
+ /* Function Body */
+/* Computing 2nd power */
+ d__1 = b_2.w;
+ const__ = b_2.rho * 3.1415926535897932 * (d__1 * d__1);
+ *xke = 0.;
+ i__1 = b_2.kkk;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+/* labelL10: */
+/* Computing 3rd power */
+ d__1 = x[i__];
+ *xke += d__1 * (d__1 * d__1) * b_2.thick[i__ - 1] * (x[i__ + 1] - x[
+ i__]);
+ }
+ *xke *= const__;
+ return 0;
+} /* tp363d_ */
+
+/* Subroutine */ int tp363e_(Real *y1i, Real *yi, Real *xi, Real *xf, int *ii, Real *y, Real *y1, Real *x)
+{
+ /* System generated locals */
+ int i__1;
+
+ /* Local variables */
+ Real tp363h_(Real *, Real *, Real *, int *);
+ static Real h__;
+ static int j;
+ static Real m0, m1, m2, m3;
+ static int kk, ll;
+ static Real xr, yr, y1r;
+
+ /* Parameter adjustments */
+ --x;
+ --y1;
+ --y;
+
+ /* Function Body */
+ x[1] = *xi;
+ y[1] = *yi;
+ y1[1] = *y1i;
+ h__ = (*xf - *xi) / (Real) (*ii - 1);
+ kk = *ii - 1;
+ i__1 = kk;
+ for (j = 1; j <= i__1; ++j) {
+ ll = j;
+ xr = x[j];
+ yr = y[j];
+ y1r = y1[j];
+ m0 = h__ * tp363h_(&xr, &yr, &y1r, &ll);
+ xr = x[j] + h__ / 2.;
+ yr = y[j] + h__ * y1[j] / 2.;
+ y1r = y1[j] + m0 / 2.;
+ m1 = h__ * tp363h_(&xr, &yr, &y1r, &ll);
+ yr += h__ * m0 / 4.;
+ y1r = y1[j] + m1 / 2.;
+ m2 = h__ * tp363h_(&xr, &yr, &y1r, &ll);
+ xr = x[j] + h__;
+ yr = y[j] + h__ * y1[j] + h__ * m1 / 2.;
+ y1r = y1[j] + m2;
+ m3 = h__ * tp363h_(&xr, &yr, &y1r, &ll);
+ y[j + 1] = y[j] + h__ * y1[j] + h__ / 6. * (m0 + m1 + m2);
+ y1[j + 1] = y1[j] + (m0 + m1 * 2. + m2 * 2. + m3) / 6.;
+/* labelL10: */
+ x[j + 1] = x[j] + h__;
+ }
+ return 0;
+} /* tp363e_ */
+
+/* Subroutine */ int tp363f_(Real *xl, Real *xr, Real *fxl, Real *fxr, Real *eps, Real *y, Real *y1, Real *x, Real *xi, Real *xf, Real *yi,
+ Real *root, int *ii)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real xapp;
+ int tp363e_(Real *, Real *, Real *, Real *, int *, Real *, Real *, Real *);
+ static Real value, fxapp, xsave;
+
+ /* Parameter adjustments */
+ --x;
+ --y1;
+ --y;
+
+ /* Function Body */
+ xsave = *xl;
+L105:
+ xapp = *xl + *fxl * (*xr - *xl) / (*fxl - *fxr);
+ tp363e_(&xapp, yi, xi, xf, ii, &y[1], &y1[1], &x[1]);
+ fxapp = -y[*ii];
+ if ((d__1 = xapp - xsave, std::abs(d__1)) / xapp <= *eps) {
+ goto L250;
+ }
+ value = fxapp * *fxl;
+ if (value < 0.) {
+ goto L110;
+ }
+ *xl = xapp;
+ xsave = *xl;
+ *fxl = fxapp;
+ goto L105;
+L110:
+ *xr = xapp;
+ xsave = *xr;
+ *fxr = fxapp;
+ goto L105;
+L250:
+ *root = xapp;
+ return 0;
+} /* tp363f_ */
+
+/* Subroutine */ int tp363g_(Real *c0, Real *c__, Real *v, Real *thick, int *kkk, Real *x)
+{
+ /* System generated locals */
+ int i__1;
+
+ /* Local variables */
+ static int i__;
+ static Real deltx, r1, r2, r3, pi;
+ static int lmn;
+
+ /* Parameter adjustments */
+ --x;
+ --thick;
+ --c__;
+
+ /* Function Body */
+ *v = 0.;
+ pi = 3.141592654;
+ deltx = (x[*kkk + 1] - x[1]) / *kkk;
+ lmn = *kkk - 1;
+ i__1 = lmn;
+ for (i__ = 1; i__ <= i__1; i__ += 2) {
+ r1 = (x[i__ + 1] + x[i__]) / 2.;
+ r2 = (x[i__ + 1] + x[i__ + 2]) / 2.;
+ r3 = (r1 + r2) / 2.;
+/* labelL10: */
+ *v += pi * 2. * deltx / 3. * (thick[i__] * r1 + thick[i__ + 1] * 4. *
+ r3 + thick[i__ + 2] * r2);
+ }
+ return 0;
+} /* tp363g_ */
+
+Real tp363h_(Real *xr, Real *yr, Real *y1r, int *i__)
+{
+ /* System generated locals */
+ Real ret_val, d__1, d__2;
+
+/* Computing 2nd power */
+ d__1 = *xr;
+/* Computing 2nd power */
+ d__2 = b_1.w2;
+ ret_val = (1. / b_1.thick[*i__ - 1] * b_1.dthick[*i__ - 1] - 1. / *xr) * *
+ y1r + (1. / (d__1 * d__1) - b_1.xmu / (*xr * b_1.thick[*i__ - 1])
+ * b_1.dthick[*i__ - 1]) * *yr - (b_1.xmu + 3.) * b_1.rho * (d__2 *
+ d__2) * b_1.thick[*i__ - 1] * *xr;
+ return ret_val;
+} /* tp363h_ */
+
+
+/* Subroutine */ int tp364_(int *mode)
+{
+ /* Local variables */
+ Real tp364a_(Real *);
+ static int i__;
+ static Real xmu1, xmu2;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 6;
+ l1_1.nili = 2;
+ l1_1.ninl = 2;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_5.x[0] = 1.;
+ l2_5.x[1] = 4.5;
+ l2_5.x[2] = 4.;
+ l2_5.x[3] = 5.;
+ l2_5.x[4] = 3.;
+ l2_5.x[5] = 3.;
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* labelL11: */
+ l11_5.lxl[i__ - 1] = true;
+ }
+ l11_5.lxl[4] = false;
+ l11_5.lxl[5] = false;
+ l12_5.lxu[4] = false;
+ l12_5.lxu[5] = false;
+ l12_5.lxu[0] = true;
+ l12_5.lxu[3] = true;
+ l12_5.lxu[1] = false;
+ l12_5.lxu[2] = false;
+ l13_5.xl[0] = .5;
+ l13_5.xl[1] = 0.;
+ l13_5.xl[2] = 0.;
+ l13_5.xl[3] = 2.;
+ l14_5.xu[0] = 3.;
+ l14_5.xu[3] = 10.;
+ l20_4.lex = false;
+ l20_4.nex = 1;
+ l20_4.fex = (float).0606002;
+ l20_4.xex[0] = (float).99616882;
+ l20_4.xex[1] = 4.1960616;
+ l20_4.xex[2] = 2.9771652;
+ l20_4.xex[3] = 3.9631949;
+ l20_4.xex[4] = 1.6536702;
+ l20_4.xex[5] = 1.2543998;
+ return 0;
+labelL2:
+ l6_1.fx = tp364a_(l2_5.x);
+labelL3:
+ return 0;
+labelL4:
+ xmu1 = .7853981633;
+ xmu2 = 2.356194491;
+ if (l9_7.index1[0]) {
+ l3_6.g[0] = -l2_5.x[0] + l2_5.x[1] + l2_5.x[2] - l2_5.x[3];
+ }
+ if (l9_7.index1[1]) {
+ l3_6.g[1] = -l2_5.x[0] - l2_5.x[1] + l2_5.x[2] + l2_5.x[3];
+ }
+ if (l9_7.index1[2]) {
+ l3_6.g[2] = -l2_5.x[1] * l2_5.x[1] - l2_5.x[2] * l2_5.x[2] + (l2_5.x[
+ 3] - l2_5.x[0]) * (l2_5.x[3] - l2_5.x[0]) + l2_5.x[1] * 2. *
+ l2_5.x[2] *std::cos(xmu1);
+ }
+ if (l9_7.index1[3]) {
+ l3_6.g[3] = l2_5.x[1] * l2_5.x[1] + l2_5.x[2] * l2_5.x[2] - (l2_5.x[3]
+ + l2_5.x[0]) * (l2_5.x[3] + l2_5.x[0]) - l2_5.x[1] * 2. *
+ l2_5.x[2] *std::cos(xmu2);
+ }
+labelL5:
+ return 0;
+} /* tp364_ */
+
+Real tp364a_(Real *x)
+{
+ /* System generated locals */
+ Real ret_val, d__1, d__2;
+
+ /* Local variables */
+ static Real xinc, coss, sins, cosy, siny;
+ int tp364b_(Real*, Real*, Real*), tp364c_(Real*, Real*, Real*);
+ static int i__;
+ static Real x1[31], y1[31], pi, wp, x1a[31], y1a[31], phi[31];
+
+ /* Parameter adjustments */
+ --x;
+
+ /* Function Body */
+ pi = 3.141592654;
+ xinc = pi * 2. / 30.;
+ for (i__ = 1; i__ <= 31; ++i__) {
+/* labelL1: */
+ phi[i__ - 1] = xinc * (Real) (i__ - 1);
+ }
+ tp364b_(phi, x1, y1);
+ ret_val = 0.;
+ for (i__ = 1; i__ <= 31; ++i__) {
+ tp364c_(&x[1], &phi[i__ - 1], &coss);
+ wp = (d__1 = 1. - coss * coss, std::abs(d__1));
+ if (wp > (float)0.) {
+ sins = std::sqrt(wp);
+ } else {
+ sins = (float)0.;
+ }
+ cosy = (x[4] + x[3] * coss - x[1] *std::cos(phi[i__ - 1])) / x[2];
+ siny = (x[3] * sins - x[1] * std::sin(phi[i__ - 1])) / x[2];
+ x1a[i__ - 1] = x[1] *std::cos(phi[i__ - 1]) + x[5] * cosy - x[6] * siny;
+ y1a[i__ - 1] = x[1] * std::sin(phi[i__ - 1]) + x[5] * siny + x[6] * cosy;
+/* labelL2: */
+/* Computing 2nd power */
+ d__1 = x1a[i__ - 1] - x1[i__ - 1];
+/* Computing 2nd power */
+ d__2 = y1a[i__ - 1] - y1[i__ - 1];
+ ret_val = ret_val + d__1 * d__1 + d__2 * d__2;
+ }
+ wp = ret_val / 31.;
+ if (wp > (float)0.) {
+ ret_val = std::sqrt(wp);
+ } else {
+ ret_val = (float)0.;
+ }
+ return ret_val;
+} /* tp364a_ */
+
+/* Subroutine */ int tp364b_(Real *phi, Real *x1, Real *y1)
+{
+ /* Local variables */
+ static int i__;
+ static Real pi;
+
+ /* Parameter adjustments */
+ --y1;
+ --x1;
+ --phi;
+
+ /* Function Body */
+ pi = 3.141592654;
+ for (i__ = 1; i__ <= 31; ++i__) {
+ x1[i__] = std::sin(pi * 2. * ((pi - phi[i__]) / (pi * 2.) - .16)) + .4;
+/* labelL1: */
+ y1[i__] = std::sin(pi - phi[i__]) * .9 + 2.;
+ }
+ return 0;
+} /* tp364b_ */
+
+/* Subroutine */ int tp364c_(Real *x, Real *phi, Real *w)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real term, a, b, c__, k, l, m, pi;
+
+ /* Parameter adjustments */
+ --x;
+
+ /* Function Body */
+ pi = 3.141592654;
+ m = x[1] * 2. * x[3] * std::sin(*phi);
+ l = x[3] * 2. * x[4] - x[1] * 2. * x[3] *std::cos(*phi);
+ k = x[1] * x[1] - x[2] * x[2] + x[3] * x[3] + x[4] * x[4] - x[4] * 2. * x[
+ 1] *std::cos(*phi);
+ a = l * l + m * m;
+ b = k * 2. * l;
+ c__ = k * k - m * m;
+ term = std::sqrt((d__1 = b * b - a * 4. * c__, std::abs(d__1)));
+ if (pi - *phi < 0.) {
+ term = -term;
+ }
+ *w = (-b + term) / (a * 2.);
+ return 0;
+} /* tp364c_ */
+
+
+/* Subroutine */ int tp365_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+ static Real p, q;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 7;
+ l1_1.nili = 0;
+ l1_1.ninl = 5;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_6.x[0] = 3.;
+ l2_6.x[1] = 0.;
+ l2_6.x[2] = 2.;
+ l2_6.x[3] = -1.5;
+ l2_6.x[4] = 1.5;
+ l2_6.x[5] = 5.;
+ l2_6.x[6] = 0.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ l11_6.lxl[(i__ << 1) - 1] = false;
+/* labelL11: */
+ l11_6.lxl[(i__ << 1) - 2] = true;
+ }
+ l11_6.lxl[6] = true;
+ for (i__ = 1; i__ <= 7; ++i__) {
+/* labelL12: */
+ l12_6.lxu[i__ - 1] = false;
+ }
+ l13_6.xl[0] = 0.;
+ l13_6.xl[2] = 0.;
+ l13_6.xl[4] = 1.;
+ l13_6.xl[6] = 1.;
+ l20_8.lex = false;
+ l20_8.nex = 1;
+ l20_8.fex = 23.313708;
+ l20_8.xex[0] = 4.8284266;
+ l20_8.xex[1] = 4.7529555e-6;
+ l20_8.xex[2] = 4.8284276;
+ l20_8.xex[3] = 1.0000024;
+ l20_8.xex[4] = 2.4142144;
+ l20_8.xex[5] = 2.4142151;
+ l20_8.xex[6] = 1.;
+ return 0;
+labelL2:
+ l6_1.fx = l2_6.x[0] * l2_6.x[2];
+labelL3:
+ return 0;
+labelL4:
+/* Computing 2nd power */
+ d__1 = l2_6.x[1];
+/* Computing 2nd power */
+ d__2 = l2_6.x[2];
+ p = std::sqrt(d__1 * d__1 + d__2 * d__2);
+/* Computing 2nd power */
+ d__1 = l2_6.x[2];
+/* Computing 2nd power */
+ d__2 = l2_6.x[1] - l2_6.x[0];
+ q = std::sqrt(d__1 * d__1 + d__2 * d__2);
+ if (l9_5.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_6.x[3] - l2_6.x[5];
+/* Computing 2nd power */
+ d__2 = l2_6.x[4] - l2_6.x[6];
+ l3_4.g[0] = d__1 * d__1 + d__2 * d__2 - 4.;
+ }
+ if (l9_5.index1[1]) {
+ l3_4.g[1] = (l2_6.x[2] * l2_6.x[3] - l2_6.x[1] * l2_6.x[4]) / p - 1.;
+ }
+ if (l9_5.index1[2]) {
+ l3_4.g[2] = (l2_6.x[2] * l2_6.x[5] - l2_6.x[1] * l2_6.x[6]) / p - 1.;
+ }
+ if (l9_5.index1[3]) {
+ l3_4.g[3] = (l2_6.x[0] * l2_6.x[2] + (l2_6.x[1] - l2_6.x[0]) * l2_6.x[
+ 4] - l2_6.x[2] * l2_6.x[3]) / q - 1.;
+ }
+ if (l9_5.index1[4]) {
+ l3_4.g[4] = (l2_6.x[0] * l2_6.x[2] + (l2_6.x[1] - l2_6.x[0]) * l2_6.x[
+ 6] - l2_6.x[2] * l2_6.x[5]) / q - 1.;
+ }
+labelL5:
+ return 0;
+} /* tp365_ */
+
+
+/* Subroutine */ int tp366_(int *mode)
+{
+ /* Initialized data */
+
+ static Real c__[38] = { 5.9553571e-4,.88392857,-.1175625,1.1088,
+ .1303533,-.0066033,6.6173269e-4,.017239878,-.0056595559,
+ -.019120592,56.85075,1.08702,.32175,-.03762,.006198,2462.3121,
+ -25.125634,161.18996,5e3,-489510.,44.333333,.33,.022556,-.007595,
+ 6.1e-4,-5e-4,.819672,.819672,24500.,-250.,.010204082,1.2244898e-5,
+ 6.25e-5,6.25e-5,-7.625e-5,1.22,1.,-1. };
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 7;
+ l1_1.nili = 0;
+ l1_1.ninl = 14;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_6.x[0] = 1745.;
+ l2_6.x[1] = 110.;
+ l2_6.x[2] = 3048.;
+ l2_6.x[3] = 89.;
+ l2_6.x[4] = 92.8;
+ l2_6.x[5] = 8.;
+ l2_6.x[6] = 145.;
+ for (i__ = 1; i__ <= 7; ++i__) {
+ l11_6.lxl[i__ - 1] = true;
+/* labelL11: */
+ l12_6.lxu[i__ - 1] = true;
+ }
+ l13_6.xl[0] = 1.;
+ l13_6.xl[1] = 1.;
+ l13_6.xl[2] = 1.;
+ l13_6.xl[3] = 85.;
+ l13_6.xl[4] = 90.;
+ l13_6.xl[5] = 3.;
+ l13_6.xl[6] = 145.;
+ l14_6.xu[0] = 2e3;
+ l14_6.xu[1] = 120.;
+ l14_6.xu[2] = 5e3;
+ l14_6.xu[3] = 93.;
+ l14_6.xu[4] = 95.;
+ l14_6.xu[5] = 12.;
+ l14_6.xu[6] = 162.;
+ l20_8.lex = false;
+ l20_8.nex = 1;
+ l20_8.fex = 704.3056;
+ l20_8.xex[0] = 905.40351;
+ l20_8.xex[1] = 36.394998;
+ l20_8.xex[2] = 2381.4783;
+ l20_8.xex[3] = 88.987691;
+ l20_8.xex[4] = 95.;
+ l20_8.xex[5] = 12.;
+ l20_8.xex[6] = 153.53535;
+ return 0;
+labelL2:
+ l6_1.fx = l2_6.x[0] * 1.715 + l2_6.x[0] * .035 * l2_6.x[5] + l2_6.x[2] *
+ 4.0565 + l2_6.x[1] * 10. + 3e3 - l2_6.x[2] * .063 * l2_6.x[4];
+labelL3:
+ return 0;
+labelL4:
+ if (l9_8.index1[0]) {
+/* Computing 2nd power */
+ d__1 = l2_6.x[5];
+ l3_7.g[0] = 1. - c__[0] * (d__1 * d__1) - c__[1] * l2_6.x[2] / l2_6.x[
+ 0] - c__[2] * l2_6.x[5];
+ }
+ if (l9_8.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_6.x[5];
+ l3_7.g[1] = 1. - c__[3] * l2_6.x[0] / l2_6.x[2] - c__[4] * l2_6.x[0] /
+ l2_6.x[2] * l2_6.x[5] - c__[5] * l2_6.x[0] / l2_6.x[2] * (
+ d__1 * d__1);
+ }
+ if (l9_8.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_6.x[5];
+ l3_7.g[2] = 1. - c__[6] * (d__1 * d__1) - c__[7] * l2_6.x[4] - c__[8]
+ * l2_6.x[3] - c__[9] * l2_6.x[5];
+ }
+ if (l9_8.index1[3]) {
+/* Computing 2nd power */
+ d__1 = l2_6.x[5];
+ l3_7.g[3] = 1. - c__[10] / l2_6.x[4] - c__[11] / l2_6.x[4] * l2_6.x[5]
+ - c__[12] * l2_6.x[3] / l2_6.x[4] - c__[13] / l2_6.x[4] * (
+ d__1 * d__1);
+ }
+ if (l9_8.index1[4]) {
+ l3_7.g[4] = 1. - c__[14] * l2_6.x[6] - c__[15] * l2_6.x[1] / l2_6.x[2]
+ / l2_6.x[3] - c__[16] * l2_6.x[1] / l2_6.x[2];
+ }
+ if (l9_8.index1[5]) {
+ l3_7.g[5] = 1. - c__[17] / l2_6.x[6] - c__[18] * l2_6.x[1] / l2_6.x[2]
+ / l2_6.x[6] - c__[19] * l2_6.x[1] / l2_6.x[2] / l2_6.x[3] /
+ l2_6.x[6];
+ }
+ if (l9_8.index1[6]) {
+ l3_7.g[6] = 1. - c__[20] / l2_6.x[4] - c__[21] * l2_6.x[6] / l2_6.x[4]
+ ;
+ }
+ if (l9_8.index1[7]) {
+ l3_7.g[7] = 1. - c__[22] * l2_6.x[4] - c__[23] * l2_6.x[6];
+ }
+ if (l9_8.index1[8]) {
+ l3_7.g[8] = 1. - c__[24] * l2_6.x[2] - c__[25] * l2_6.x[0];
+ }
+ if (l9_8.index1[9]) {
+ l3_7.g[9] = 1. - c__[26] * l2_6.x[0] / l2_6.x[2] - c__[27] / l2_6.x[2]
+ ;
+ }
+ if (l9_8.index1[10]) {
+ l3_7.g[10] = 1. - c__[28] * l2_6.x[1] / l2_6.x[2] / l2_6.x[3] - c__[
+ 29] * l2_6.x[1] / l2_6.x[2];
+ }
+ if (l9_8.index1[11]) {
+ l3_7.g[11] = 1. - c__[30] * l2_6.x[3] - c__[31] / l2_6.x[1] * l2_6.x[
+ 2] * l2_6.x[3];
+ }
+ if (l9_8.index1[12]) {
+ l3_7.g[12] = 1. - c__[32] * l2_6.x[0] * l2_6.x[5] - c__[33] * l2_6.x[
+ 0] - c__[34] * l2_6.x[2];
+ }
+ if (l9_8.index1[13]) {
+ l3_7.g[13] = 1. - c__[35] / l2_6.x[0] * l2_6.x[2] - c__[36] / l2_6.x[
+ 0] - c__[37] * l2_6.x[5];
+ }
+labelL5:
+ return 0;
+} /* tp366_ */
+
+
+/* Subroutine */ int tp367_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 7;
+ l1_1.nili = 2;
+ l1_1.ninl = 1;
+ l1_1.neli = 1;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 7; ++i__) {
+ l2_6.x[i__ - 1] = .1;
+ l11_6.lxl[i__ - 1] = true;
+ l12_6.lxu[i__ - 1] = false;
+/* labelL11: */
+ l13_6.xl[i__ - 1] = 0.;
+ }
+ l20_8.lex = false;
+ l20_8.nex = 1;
+ l20_8.fex = -37.41296;
+ l20_8.xex[0] = 1.4688103;
+ l20_8.xex[1] = 1.9839711;
+ l20_8.xex[2] = .35187754;
+ l20_8.xex[3] = 1.1953411;
+ l20_8.xex[4] = .56940029;
+ l20_8.xex[5] = .78474478;
+ l20_8.xex[6] = 1.4121216;
+ for (i__ = 1; i__ <= 7; ++i__) {
+/* labelL12: */
+ l5_34.gg[i__ * 5 - 5] = -1.;
+ }
+ for (i__ = 1; i__ <= 4; ++i__) {
+/* labelL13: */
+ l5_34.gg[i__ * 5 - 4] = -1.;
+ }
+ for (i__ = 5; i__ <= 7; ++i__) {
+/* labelL14: */
+ l5_34.gg[i__ * 5 - 4] = 0.;
+ }
+ l5_34.gg[2] = -1.;
+ l5_34.gg[7] = 0.;
+ l5_34.gg[12] = -1.;
+ l5_34.gg[17] = 0.;
+ l5_34.gg[22] = -1.;
+ l5_34.gg[3] = 0.;
+ l5_34.gg[8] = 0.;
+ l5_34.gg[13] = 0.;
+ l5_34.gg[18] = 2.;
+ l5_34.gg[23] = 1.;
+ l5_34.gg[28] = .8;
+ l5_34.gg[33] = 1.;
+ l5_34.gg[4] = 0.;
+ l5_34.gg[19] = 0.;
+ l5_34.gg[34] = 0.;
+ l4_6.gf[1] = -5.;
+ l4_6.gf[3] = -6.;
+ return 0;
+labelL2:
+ l6_1.fx = l2_6.x[0] * -5. - l2_6.x[1] * 5. - l2_6.x[2] * 4. - l2_6.x[0] *
+ l2_6.x[2] - l2_6.x[3] * 6. - l2_6.x[4] * 5. / (l2_6.x[4] + 1.) -
+ l2_6.x[5] * 8. / (l2_6.x[5] + 1.) - (1. - std::exp(-l2_6.x[6]) * 2. +
+ std::exp(l2_6.x[6] * -2.)) * 10.;
+ return 0;
+labelL3:
+ l4_6.gf[0] = -5. - l2_6.x[2];
+ l4_6.gf[2] = -4. - l2_6.x[0];
+/* Computing 2nd power */
+ d__1 = l2_6.x[4] + 1.;
+ l4_6.gf[4] = -5. / (d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = l2_6.x[5] + 1.;
+ l4_6.gf[5] = -8. / (d__1 * d__1);
+ l4_6.gf[6] = (exp(-l2_6.x[6]) - std::exp(l2_6.x[6] * -2.)) * -20.;
+ return 0;
+labelL4:
+ if (l9_5.index1[0]) {
+ l3_4.g[0] = 10. - l2_6.x[0] - l2_6.x[1] - l2_6.x[2] - l2_6.x[3] -
+ l2_6.x[4] - l2_6.x[5] - l2_6.x[6];
+ }
+ if (l9_5.index1[1]) {
+ l3_4.g[1] = 5. - l2_6.x[0] - l2_6.x[1] - l2_6.x[2] - l2_6.x[3];
+ }
+ if (l9_5.index1[2]) {
+/* Computing 2nd power */
+ d__1 = l2_6.x[5];
+/* Computing 2nd power */
+ d__2 = l2_6.x[6];
+ l3_4.g[2] = 5. - l2_6.x[0] - l2_6.x[2] - l2_6.x[4] - d__1 * d__1 -
+ d__2 * d__2;
+ }
+ if (l9_5.index1[3]) {
+ l3_4.g[3] = l2_6.x[3] * 2. + l2_6.x[4] + l2_6.x[5] * .8 + l2_6.x[6] -
+ 5.;
+ }
+ if (l9_5.index1[4]) {
+/* Computing 2nd power */
+ d__1 = l2_6.x[1];
+/* Computing 2nd power */
+ d__2 = l2_6.x[2];
+/* Computing 2nd power */
+ d__3 = l2_6.x[4];
+/* Computing 2nd power */
+ d__4 = l2_6.x[5];
+ l3_4.g[4] = d__1 * d__1 + d__2 * d__2 + d__3 * d__3 + d__4 * d__4 -
+ 5.;
+ }
+ return 0;
+labelL5:
+ if (! l10_5.index2[2]) {
+ goto L51;
+ }
+ l5_34.gg[27] = l2_6.x[5] * -2.;
+ l5_34.gg[32] = l2_6.x[6] * -2.;
+L51:
+ if (! l10_5.index2[4]) {
+ goto L52;
+ }
+ l5_34.gg[9] = l2_6.x[1] * 2.;
+ l5_34.gg[14] = l2_6.x[2] * 2.;
+ l5_34.gg[24] = l2_6.x[4] * 2.;
+ l5_34.gg[29] = l2_6.x[5] * 2.;
+L52:
+ return 0;
+} /* tp367_ */
+
+
+/* Subroutine */ int tp368_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+ static Real s2, s3, s4;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 8;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 8; ++i__) {
+ l2_7.x[i__ - 1] = (float)1. - (float)1. / (Real) i__;
+ l11_7.lxl[i__ - 1] = true;
+ l12_7.lxu[i__ - 1] = true;
+ l13_7.xl[i__ - 1] = 0.;
+/* labelL11: */
+ l14_7.xu[i__ - 1] = 1.;
+ }
+ l2_7.x[6] = (float).7;
+ l2_7.x[7] = (float).7;
+ l20_2.lex = false;
+ l20_2.nex = 1;
+ l20_2.fex = -.74997564;
+ l20_2.xex[0] = .49834105;
+ l20_2.xex[1] = .4997795;
+ l20_2.xex[2] = .50201378;
+ l20_2.xex[3] = .50378302;
+ l20_2.xex[4] = .50263008;
+ l20_2.xex[5] = .50232579;
+ l20_2.xex[6] = 1.;
+ l20_2.xex[7] = 1.;
+ l20_2.lex = true;
+ l20_2.nex = 1;
+ l20_2.fex = (float)-.75;
+ l20_2.xex[0] = (float).5;
+ l20_2.xex[1] = (float).5;
+ l20_2.xex[2] = (float).5;
+ l20_2.xex[3] = (float).5;
+ l20_2.xex[4] = (float).5;
+ l20_2.xex[5] = (float).5;
+ l20_2.xex[6] = 1.;
+ l20_2.xex[7] = 1.;
+ return 0;
+labelL2:
+ s2 = 0.;
+ s3 = 0.;
+ s4 = 0.;
+ for (i__ = 1; i__ <= 8; ++i__) {
+/* Computing 2nd power */
+ d__1 = l2_7.x[i__ - 1];
+ s2 += d__1 * d__1;
+/* Computing 3rd power */
+ d__1 = l2_7.x[i__ - 1];
+ s3 += d__1 * (d__1 * d__1);
+/* labelL10: */
+/* Computing 4th power */
+ d__1 = l2_7.x[i__ - 1], d__1 *= d__1;
+ s4 += d__1 * d__1;
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+/* Computing 2nd power */
+ d__1 = s3;
+ l6_1.fx = -s2 * s4 + d__1 * d__1;
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 8; ++i__) {
+/* L31: */
+/* Computing 3rd power */
+ d__1 = l2_7.x[i__ - 1];
+/* Computing 2nd power */
+ d__2 = l2_7.x[i__ - 1];
+ l4_7.gf[i__ - 1] = l2_7.x[i__ - 1] * -2. * s4 - d__1 * (d__1 * d__1) *
+ 4. * s2 + d__2 * d__2 * 6. * s3;
+ }
+labelL4:
+ return 0;
+} /* tp368_ */
+
+
+/* Subroutine */ int tp369_(int *mode)
+{
+ /* Initialized data */
+
+ static Real c__[16] = { 833.33252,100.,-83333.333,1250.,1.,-1250.,
+ 1.25e6,1.,-2500.,.0025,.0025,.0025,.0025,-.0025,.01,-.01 };
+
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 8;
+ l1_1.nili = 3;
+ l1_1.ninl = 3;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_7.x[0] = 5e3;
+ l2_7.x[1] = 5e3;
+ l2_7.x[2] = 5e3;
+ l2_7.x[3] = 200.;
+ l2_7.x[4] = 350.;
+ l2_7.x[5] = 150.;
+ l2_7.x[6] = 225.;
+ l2_7.x[7] = 425.;
+ for (i__ = 1; i__ <= 8; ++i__) {
+ l12_7.lxu[i__ - 1] = true;
+/* labelL11: */
+ l11_7.lxl[i__ - 1] = true;
+ }
+ l13_7.xl[0] = 100.;
+ l13_7.xl[1] = 1e3;
+ l13_7.xl[2] = 1e3;
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* labelL12: */
+ l14_7.xu[i__ - 1] = 1e4;
+ }
+ for (i__ = 4; i__ <= 8; ++i__) {
+ l13_7.xl[i__ - 1] = 10.;
+/* labelL13: */
+ l14_7.xu[i__ - 1] = 1e3;
+ }
+ l20_2.lex = false;
+ l20_2.nex = 1;
+ l20_2.fex = 7049.248;
+ l20_2.xex[0] = 579.30657;
+ l20_2.xex[1] = 1359.9705;
+ l20_2.xex[2] = 5109.9709;
+ l20_2.xex[3] = 182.01769;
+ l20_2.xex[4] = 295.60116;
+ l20_2.xex[5] = 217.98231;
+ l20_2.xex[6] = 286.41653;
+ l20_2.xex[7] = 395.60116;
+ return 0;
+labelL2:
+ l6_1.fx = l2_7.x[0] + l2_7.x[1] + l2_7.x[2];
+labelL3:
+ return 0;
+labelL4:
+ if (l9_6.index1[0]) {
+ l3_5.g[0] = (float)1. - c__[9] * l2_7.x[3] - c__[10] * l2_7.x[5];
+ }
+ if (l9_6.index1[1]) {
+ l3_5.g[1] = (float)1. - c__[11] * l2_7.x[4] - c__[12] * l2_7.x[6] -
+ c__[13] * l2_7.x[3];
+ }
+ if (l9_6.index1[2]) {
+ l3_5.g[2] = (float)1. - c__[14] * l2_7.x[7] - c__[15] * l2_7.x[4];
+ }
+ if (l9_6.index1[3]) {
+ l3_5.g[3] = (float)1. - c__[0] / l2_7.x[0] * l2_7.x[3] / l2_7.x[5] -
+ c__[1] / l2_7.x[5] - c__[2] / l2_7.x[0] / l2_7.x[5];
+ }
+ if (l9_6.index1[4]) {
+ l3_5.g[4] = (float)1. - c__[3] / l2_7.x[1] * l2_7.x[4] / l2_7.x[6] -
+ c__[4] * l2_7.x[3] / l2_7.x[6] - c__[5] / l2_7.x[1] * l2_7.x[
+ 3] / l2_7.x[6];
+ }
+ if (l9_6.index1[5]) {
+ l3_5.g[5] = (float)1. - c__[6] / l2_7.x[2] / l2_7.x[7] - c__[7] *
+ l2_7.x[4] / l2_7.x[7] - c__[8] / l2_7.x[2] * l2_7.x[4] /
+ l2_7.x[7];
+ }
+labelL5:
+ return 0;
+} /* tp369_ */
+
+
+/* Subroutine */ int tp370_0_(int n__, int *mode)
+{
+ /* System generated locals */
+ int i__1, i__2;
+ Real d__1;
+
+ /* Builtin functions */
+ Real pow_di(Real*, int *);
+
+ /* Local variables */
+ static int i__, j;
+ static Real scale;
+ static Real basis, sum, sum1, sum2[31];
+
+ switch(n__) {
+ case 1: goto L_tp371;
+ }
+
+ l1_1.n = 6;
+ scale = (float)1.;
+ l20_9.fex = .00228767005355;
+ l20_9.xex[0] = -.015725;
+ l20_9.xex[1] = 1.012435;
+ l20_9.xex[2] = -.232992;
+ l20_9.xex[3] = -1.26043;
+ l20_9.xex[4] = -1.513729;
+ l20_9.xex[5] = .992996;
+ goto labelL10;
+
+L_tp371:
+ l1_1.n = 9;
+ scale = (float)1e5;
+ l20_9.fex = scale * 1.3997660138e-6;
+ l20_9.xex[0] = 1.5e-5;
+ l20_9.xex[1] = .99979;
+ l20_9.xex[2] = -.014764;
+ l20_9.xex[3] = -.146342;
+ l20_9.xex[4] = 1.000821;
+ l20_9.xex[5] = -2.617731;
+ l20_9.xex[6] = 4.104403;
+ l20_9.xex[7] = -2.143612;
+ l20_9.xex[8] = 1.052627;
+labelL10:
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL4;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ i__1 = l1_1.n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ l2_8.x[i__ - 1] = 0.;
+ l11_8.lxl[i__ - 1] = false;
+/* labelL11: */
+ l12_8.lxu[i__ - 1] = false;
+ }
+ l15_1.lsum = 31;
+ l20_9.lex = false;
+ l20_9.nex = 1;
+ return 0;
+labelL2:
+ l16_18.f[0] = l2_8.x[0];
+/* Computing 2nd power */
+ d__1 = l2_8.x[0];
+ l16_18.f[1] = l2_8.x[1] - d__1 * d__1 - 1.;
+ for (i__ = 2; i__ <= 30; ++i__) {
+ basis = (Real) (i__ - 1) / 29.;
+ sum1 = 0.;
+ i__1 = l1_1.n;
+ for (j = 2; j <= i__1; ++j) {
+/* L21: */
+ i__2 = j - 2;
+ sum1 += l2_8.x[j - 1] * (Real) (j - 1) * pow_di(&basis, &
+ i__2);
+ }
+ sum = 0.;
+ i__2 = l1_1.n;
+ for (j = 1; j <= i__2; ++j) {
+/* L23: */
+ i__1 = j - 1;
+ sum += l2_8.x[j - 1] * pow_di(&basis, &i__1);
+ }
+ sum2[i__] = sum;
+/* labelL20: */
+/* Computing 2nd power */
+ d__1 = sum2[i__];
+ l16_18.f[i__] = sum1 - d__1 * d__1 - 1.;
+ }
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 31; ++i__) {
+/* L22: */
+/* Computing 2nd power */
+ d__1 = l16_18.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ l6_1.fx *= scale;
+labelL4:
+ return 0;
+} /* tp370_ */
+
+/* Subroutine */ int tp370_(int *mode)
+{
+ return tp370_0_(0, mode);
+ }
+
+/* Subroutine */ int tp371_(int *mode)
+{
+ return tp370_0_(1, mode);
+ }
+
+
+/* Subroutine */ int tp372_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+
+ /* Local variables */
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 9;
+ l1_1.nili = 0;
+ l1_1.ninl = 12;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_8.x[0] = 300.;
+ l2_8.x[1] = -100.;
+ l2_8.x[2] = -.1997;
+ l2_8.x[3] = -127.;
+ l2_8.x[4] = -151.;
+ l2_8.x[5] = 379.;
+ l2_8.x[6] = 421.;
+ l2_8.x[7] = 460.;
+ l2_8.x[8] = 426.;
+ for (i__ = 1; i__ <= 12; ++i__) {
+ for (j = 4; j <= 9; ++j) {
+/* labelL12: */
+ l5_36.gg[i__ + j * 12 - 13] = 0.;
+ }
+ }
+ for (i__ = 1; i__ <= 6; ++i__) {
+ l5_36.gg[i__ - 1] = 1.;
+/* L18: */
+ l5_36.gg[i__ + (i__ + 3) * 12 - 13] = 1.;
+ }
+ for (i__ = 7; i__ <= 12; ++i__) {
+ l5_36.gg[i__ - 1] = -1.;
+/* labelL13: */
+ l5_36.gg[i__ + (i__ - 3) * 12 - 13] = 1.;
+ }
+ l4_8.gf[0] = 0.;
+ l4_8.gf[1] = 0.;
+ l4_8.gf[2] = 0.;
+ for (i__ = 4; i__ <= 9; ++i__) {
+ l11_8.lxl[i__ - 1] = true;
+ l12_8.lxu[i__ - 1] = false;
+/* labelL14: */
+ l13_8.xl[i__ - 1] = 0.;
+ }
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l11_8.lxl[i__ - 1] = false;
+/* L15: */
+ l12_8.lxu[i__ - 1] = false;
+ }
+ l13_8.xl[2] = (float)-1.;
+ l14_8.xu[2] = (float)0.;
+ l11_8.lxl[2] = true;
+ l12_8.lxu[2] = true;
+ l20_9.xex[0] = 523.30555;
+ l20_9.xex[1] = -156.94787;
+ l20_9.xex[2] = -.19966457;
+ l20_9.xex[3] = 29.608067;
+ l20_9.xex[4] = 86.615521;
+ l20_9.xex[5] = 47.326718;
+ l20_9.xex[6] = 26.235604;
+ l20_9.xex[7] = 22.915985;
+ l20_9.xex[8] = 39.470742;
+ l20_9.lex = false;
+ l20_9.nex = 1;
+ l20_9.fex = 13390.093;
+ l15_1.lsum = 6;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 4; i__ <= 9; ++i__) {
+ l16_8.f[i__ - 4] = l2_8.x[i__ - 1];
+/* L21: */
+/* Computing 2nd power */
+ d__1 = l2_8.x[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ for (i__ = 4; i__ <= 9; ++i__) {
+/* L35: */
+ l16_8.f[i__ - 4] = l2_8.x[i__ - 1];
+ }
+ for (i__ = 4; i__ <= 9; ++i__) {
+/* L31: */
+ l4_8.gf[i__ - 1] = l2_8.x[i__ - 1] * 2.;
+ }
+ for (i__ = 1; i__ <= 6; ++i__) {
+ for (j = 1; j <= 9; ++j) {
+/* L33: */
+ l17_22.df[i__ + j * 6 - 7] = 0.;
+ }
+ }
+ for (i__ = 1; i__ <= 6; ++i__) {
+/* L34: */
+ l17_22.df[i__ + (i__ + 3) * 6 - 7] = 1.;
+ }
+ return 0;
+labelL4:
+ if (l9_17.index1[0]) {
+ l3_16.g[0] = l2_8.x[0] + l2_8.x[1] * std::exp(l2_8.x[2] * -5.) + l2_8.x[3]
+ - 127.;
+ }
+ if (l9_17.index1[1]) {
+ l3_16.g[1] = l2_8.x[0] + l2_8.x[1] * std::exp(l2_8.x[2] * -3.) + l2_8.x[4]
+ - 151.;
+ }
+ if (l9_17.index1[2]) {
+ l3_16.g[2] = l2_8.x[0] + l2_8.x[1] * std::exp(-l2_8.x[2]) + l2_8.x[5] -
+ 379.;
+ }
+ if (l9_17.index1[3]) {
+ l3_16.g[3] = l2_8.x[0] + l2_8.x[1] * std::exp(l2_8.x[2]) + l2_8.x[6] -
+ 421.;
+ }
+ if (l9_17.index1[4]) {
+ l3_16.g[4] = l2_8.x[0] + l2_8.x[1] * std::exp(l2_8.x[2] * 3.) + l2_8.x[7]
+ - 460.;
+ }
+ if (l9_17.index1[5]) {
+ l3_16.g[5] = l2_8.x[0] + l2_8.x[1] * std::exp(l2_8.x[2] * 5.) + l2_8.x[8]
+ - 426.;
+ }
+ if (l9_17.index1[6]) {
+ l3_16.g[6] = -l2_8.x[0] - l2_8.x[1] * std::exp(l2_8.x[2] * -5.) + l2_8.x[3]
+ + 127.;
+ }
+ if (l9_17.index1[7]) {
+ l3_16.g[7] = -l2_8.x[0] - l2_8.x[1] * std::exp(l2_8.x[2] * -3.) + l2_8.x[4]
+ + 151.;
+ }
+ if (l9_17.index1[8]) {
+ l3_16.g[8] = -l2_8.x[0] - l2_8.x[1] * std::exp(-l2_8.x[2]) + l2_8.x[5] +
+ 379.;
+ }
+ if (l9_17.index1[9]) {
+ l3_16.g[9] = -l2_8.x[0] - l2_8.x[1] * std::exp(l2_8.x[2]) + l2_8.x[6] +
+ 421.;
+ }
+ if (l9_17.index1[10]) {
+ l3_16.g[10] = -l2_8.x[0] - l2_8.x[1] * std::exp(l2_8.x[2] * 3.) + l2_8.x[7]
+ + 460.;
+ }
+ if (l9_17.index1[11]) {
+ l3_16.g[11] = -l2_8.x[0] - l2_8.x[1] * std::exp(l2_8.x[2] * 5.) + l2_8.x[8]
+ + 426.;
+ }
+ return 0;
+labelL5:
+ for (i__ = 1; i__ <= 6; ++i__) {
+ if (! l10_16.index2[i__ + 5]) {
+ goto L51;
+ }
+ l5_36.gg[i__ + 17] = -exp((Real) ((i__ << 1) - 7) * l2_8.x[2]);
+ l5_36.gg[i__ + 29] = -l2_8.x[1] * (Real) ((i__ << 1) - 7) *
+ l5_36.gg[i__ + 17];
+L51:
+ ;
+ }
+ for (i__ = 1; i__ <= 6; ++i__) {
+ if (! l10_16.index2[i__ - 1]) {
+ goto L52;
+ }
+ l5_36.gg[i__ + 11] = std::exp((Real) ((i__ << 1) - 7) * l2_8.x[2]);
+ l5_36.gg[i__ + 23] = l2_8.x[1] * (Real) ((i__ << 1) - 7) *
+ l5_36.gg[i__ + 11];
+L52:
+ ;
+ }
+ return 0;
+} /* tp372_ */
+
+
+/* Subroutine */ int tp373_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 6;
+ l1_1.n = 9;
+ for (i__ = 1; i__ <= 9; ++i__) {
+ l11_8.lxl[i__ - 1] = false;
+/* labelL11: */
+ l12_8.lxu[i__ - 1] = false;
+ }
+ l11_8.lxl[2] = true;
+ l12_8.lxu[2] = true;
+ l13_8.xl[2] = (float)-1.;
+ l14_8.xu[2] = (float)0.;
+ l2_8.x[0] = 300.;
+ l2_8.x[1] = -100.;
+ l2_8.x[2] = -.1997;
+ l2_8.x[3] = -127.;
+ l2_8.x[4] = -151.;
+ l2_8.x[5] = 379.;
+ l2_8.x[6] = 421.;
+ l2_8.x[7] = 460.;
+ l2_8.x[8] = 426.;
+ l4_8.gf[0] = 0.;
+ l4_8.gf[1] = 0.;
+ l4_8.gf[2] = 0.;
+ for (i__ = 1; i__ <= 6; ++i__) {
+ for (j = 4; j <= 9; ++j) {
+/* L17: */
+ l5_19.gg[i__ + j * 6 - 7] = 0.;
+ }
+ }
+ for (i__ = 1; i__ <= 6; ++i__) {
+ l5_19.gg[i__ - 1] = 1.;
+/* L18: */
+ l5_19.gg[i__ + (i__ + 3) * 6 - 7] = 1.;
+ }
+ l20_9.lex = false;
+ l20_9.nex = 1;
+ l20_9.fex = 13390.093;
+ l20_9.xex[0] = 523.30542;
+ l20_9.xex[1] = -156.9477;
+ l20_9.xex[2] = -.19966472;
+ l20_9.xex[3] = 29.608061;
+ l20_9.xex[4] = -86.615571;
+ l20_9.xex[5] = 47.326669;
+ l20_9.xex[6] = 26.235575;
+ l20_9.xex[7] = 22.915982;
+ l20_9.xex[8] = -39.470718;
+ l15_1.lsum = 6;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 4; i__ <= 9; ++i__) {
+ l16_8.f[i__ - 4] = l2_8.x[i__ - 1];
+/* L21: */
+/* Computing 2nd power */
+ d__1 = l2_8.x[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ for (i__ = 4; i__ <= 9; ++i__) {
+/* L35: */
+ l16_8.f[i__ - 4] = l2_8.x[i__ - 1];
+ }
+ for (i__ = 4; i__ <= 9; ++i__) {
+/* L31: */
+ l4_8.gf[i__ - 1] = l2_8.x[i__ - 1] * 2.;
+ }
+ for (i__ = 1; i__ <= 6; ++i__) {
+ for (j = 1; j <= 9; ++j) {
+/* L33: */
+ l17_22.df[i__ + j * 6 - 7] = 0.;
+ }
+ }
+ for (i__ = 1; i__ <= 6; ++i__) {
+/* L34: */
+ l17_22.df[i__ + (i__ + 3) * 6 - 7] = 1.;
+ }
+ return 0;
+labelL4:
+ if (l9_6.index1[0]) {
+ l3_5.g[0] = l2_8.x[0] + l2_8.x[1] * std::exp(l2_8.x[2] * -5.) + l2_8.x[3]
+ - 127.;
+ }
+ if (l9_6.index1[1]) {
+ l3_5.g[1] = l2_8.x[0] + l2_8.x[1] * std::exp(l2_8.x[2] * -3.) + l2_8.x[4]
+ - 151.;
+ }
+ if (l9_6.index1[2]) {
+ l3_5.g[2] = l2_8.x[0] + l2_8.x[1] * std::exp(-l2_8.x[2]) + l2_8.x[5] -
+ 379.;
+ }
+ if (l9_6.index1[3]) {
+ l3_5.g[3] = l2_8.x[0] + l2_8.x[1] * std::exp(l2_8.x[2]) + l2_8.x[6] - 421.;
+ }
+ if (l9_6.index1[4]) {
+ l3_5.g[4] = l2_8.x[0] + l2_8.x[1] * std::exp(l2_8.x[2] * 3.) + l2_8.x[7] -
+ 460.;
+ }
+ if (l9_6.index1[5]) {
+ l3_5.g[5] = l2_8.x[0] + l2_8.x[1] * std::exp(l2_8.x[2] * 5.) + l2_8.x[8] -
+ 426.;
+ }
+ return 0;
+labelL5:
+ for (i__ = 1; i__ <= 6; ++i__) {
+ if (! l10_6.index2[i__ - 1]) {
+ goto L52;
+ }
+ l5_19.gg[i__ + 5] = std::exp((Real) ((i__ << 1) - 7) * l2_8.x[2]);
+ l5_19.gg[i__ + 11] = l2_8.x[1] * (Real) ((i__ << 1) - 7) *
+ l5_19.gg[i__ + 5];
+L52:
+ ;
+ }
+ return 0;
+} /* tp373_ */
+
+
+/* Subroutine */ int tp374_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ Real tp374a_(Real *, Real*), tp374b_(Real *, Real *), tp374g_(Real *, Real *);
+ static int i__, k;
+ static Real z__, pi;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 10;
+ l1_1.nili = 0;
+ l1_1.ninl = 35;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ l2_9.x[i__ - 1] = .1;
+ l12_9.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_9.lxl[i__ - 1] = false;
+ }
+ l20_14.lex = false;
+ l20_14.nex = 2;
+ l20_14.fex = .233264;
+ l20_14.xex[0] = .218212;
+ l20_14.xex[1] = .23264;
+ l20_14.xex[2] = .278457;
+ l20_14.xex[3] = .268125;
+ l20_14.xex[4] = .21201;
+ l20_14.xex[5] = .125918;
+ l20_14.xex[6] = .034102;
+ l20_14.xex[7] = -.026136;
+ l20_14.xex[8] = -.142233;
+ l20_14.xex[9] = .233264;
+ l20_14.xex[10] = -.142233;
+ l20_14.xex[11] = -.026136;
+ l20_14.xex[12] = .034102;
+ l20_14.xex[13] = .125918;
+ l20_14.xex[14] = .21201;
+ l20_14.xex[15] = .268125;
+ l20_14.xex[16] = .278457;
+ l20_14.xex[17] = .23264;
+ l20_14.xex[18] = .218212;
+ l20_14.xex[19] = .233264;
+ for (i__ = 1; i__ <= 9; ++i__) {
+/* L46: */
+ l4_9.gf[i__ - 1] = 0.;
+ }
+ l4_9.gf[9] = 1.;
+ return 0;
+labelL2:
+ l6_1.fx = l2_9.x[9];
+labelL3:
+ return 0;
+labelL4:
+ pi = std::atan(1.) * 4.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ z__ = pi / 4. * ((Real) (i__ - 1) * .1);
+/* L8: */
+ if (l9_18.index1[i__ - 1]) {
+/* Computing 2nd power */
+ d__1 = 1. - l2_9.x[9];
+ l3_17.g[i__ - 1] = tp374g_(&z__, l2_9.x) - d__1 * d__1;
+ }
+ }
+ for (i__ = 11; i__ <= 20; ++i__) {
+ z__ = pi / 4. * ((Real) (i__ - 11) * .1);
+/* labelL9: */
+ if (l9_18.index1[i__ - 1]) {
+/* Computing 2nd power */
+ d__1 = l2_9.x[9] + 1.;
+ l3_17.g[i__ - 1] = d__1 * d__1 - tp374g_(&z__, l2_9.x);
+ }
+ }
+ for (i__ = 21; i__ <= 35; ++i__) {
+ z__ = pi / 4. * ((Real) (i__ - 21) * .2 + 1.2);
+/* labelL10: */
+ if (l9_18.index1[i__ - 1]) {
+/* Computing 2nd power */
+ d__1 = l2_9.x[9];
+ l3_17.g[i__ - 1] = d__1 * d__1 - tp374g_(&z__, l2_9.x);
+ }
+ }
+ return 0;
+labelL5:
+ pi = std::atan(1.) * 4.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ if (! l10_17.index2[i__ - 1]) {
+ goto L50;
+ }
+ z__ = pi / 4. * ((Real) (i__ - 1) * .1);
+ for (k = 1; k <= 9; ++k) {
+ l5_37.gg[i__ + k * 35 - 36] = (tp374a_(&z__, l2_9.x) *std::cos(k *
+ z__) + tp374b_(&z__, l2_9.x) * std::sin(k * z__)) * 2.;
+/* L51: */
+ l5_37.gg[i__ + 314] = (1. - l2_9.x[9]) * 2.;
+ }
+L50:
+ ;
+ }
+ for (i__ = 11; i__ <= 20; ++i__) {
+ if (! l10_17.index2[i__ - 1]) {
+ goto L52;
+ }
+ z__ = pi / 4. * ((Real) (i__ - 11) * .1);
+ for (k = 1; k <= 9; ++k) {
+ l5_37.gg[i__ + k * 35 - 36] = (tp374a_(&z__, l2_9.x) *std::cos(k *
+ z__) + tp374b_(&z__, l2_9.x) * std::sin(k * z__)) * -2.;
+/* L53: */
+ l5_37.gg[i__ + 314] = (l2_9.x[9] + 1.) * 2.;
+ }
+L52:
+ ;
+ }
+ for (i__ = 21; i__ <= 35; ++i__) {
+ if (! l10_17.index2[i__ - 1]) {
+ goto L54;
+ }
+ z__ = pi / 4. * ((Real) (i__ - 21) * .2 + 1.2);
+ for (k = 1; k <= 9; ++k) {
+ l5_37.gg[i__ + k * 35 - 36] = (tp374a_(&z__, l2_9.x) *std::cos(k *
+ z__) + tp374b_(&z__, l2_9.x) * std::sin(k * z__)) * -2.;
+/* L55: */
+ l5_37.gg[i__ + 314] = l2_9.x[9] * 2.;
+ }
+L54:
+ ;
+ }
+ return 0;
+} /* tp374_ */
+
+Real tp374g_(Real *a, Real *x)
+{
+ /* System generated locals */
+ Real ret_val, d__1, d__2;
+
+ /* Local variables */
+ Real tp374a_(Real *, Real*), tp374b_(Real *, Real *);
+
+ /* Parameter adjustments */
+ --x;
+
+ /* Function Body */
+/* Computing 2nd power */
+ d__1 = tp374a_(a, &x[1]);
+/* Computing 2nd power */
+ d__2 = tp374b_(a, &x[1]);
+ ret_val = d__1 * d__1 + d__2 * d__2;
+ return ret_val;
+} /* tp374g_ */
+
+Real tp374a_(Real *a, Real *x)
+{
+ /* System generated locals */
+ Real ret_val;
+
+
+ /* Local variables */
+ static int k;
+
+ /* Parameter adjustments */
+ --x;
+
+ /* Function Body */
+ ret_val = 0.;
+ for (k = 1; k <= 9; ++k) {
+/* labelL10: */
+ ret_val += x[k] *std::cos(k * *a);
+ }
+ return ret_val;
+} /* tp374a_ */
+
+Real tp374b_(Real *a, Real *x)
+{
+ /* System generated locals */
+ Real ret_val;
+
+ /* Local variables */
+ static int k;
+
+ /* Parameter adjustments */
+ --x;
+
+ /* Function Body */
+ ret_val = 0.;
+ for (k = 1; k <= 9; ++k) {
+/* labelL10: */
+ ret_val += x[k] * std::sin(k * *a);
+ }
+ return ret_val;
+} /* tp374b_ */
+
+
+/* Subroutine */ int tp375_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ Real tp375a_(int *, int *);
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 10;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 8;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ l2_9.x[i__ - 1] = 1.;
+ l12_9.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_9.lxl[i__ - 1] = false;
+ }
+ l20_10.lex = false;
+ l20_10.nex = 1;
+ l20_10.fex = -15.16;
+ for (i__ = 1; i__ <= 8; ++i__) {
+/* L60: */
+ l20_10.xex[i__ - 1] = .1064;
+ }
+ l20_10.xex[8] = 2.843;
+ l20_10.xex[9] = -2.642;
+ for (j = 1; j <= 8; ++j) {
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L7: */
+ l5_21.gg[j + i__ * 9 - 10] = 1.;
+ }
+ }
+ for (i__ = 1; i__ <= 8; ++i__) {
+/* L8: */
+ l5_21.gg[i__ + i__ * 9 - 10] = .5;
+ }
+ l15_1.lsum = 10;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ for (j = 1; j <= 10; ++j) {
+ l17_23.df[i__ + j * 10 - 11] = 0.;
+/* L17: */
+ l17_23.df[i__ + i__ * 10 - 11] = -1.;
+ }
+ }
+ return 0;
+labelL2:
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L16: */
+ l16_4.f[i__ - 1] = -l2_9.x[i__ - 1];
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* labelL9: */
+/* Computing 2nd power */
+ d__1 = l2_9.x[i__ - 1];
+ l6_1.fx -= d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* labelL10: */
+ l4_9.gf[i__ - 1] = l2_9.x[i__ - 1] * -2.;
+ }
+labelL4:
+ for (j = 1; j <= 8; ++j) {
+ if (! l9_16.index1[j - 1]) {
+ goto labelL11;
+ }
+ l3_15.g[j - 1] = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* labelL12: */
+ l3_15.g[j - 1] += l2_9.x[i__ - 1] / tp375a_(&i__, &j);
+ }
+ l3_15.g[j - 1] += -1.;
+labelL11:
+ ;
+ }
+ if (! l9_16.index1[8]) {
+ goto L18;
+ }
+ l3_15.g[8] = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* labelL13: */
+ l3_15.g[8] += pow_dd(&l2_9.x[i__ - 1], &c_b305) / ((Real) (i__
+ - 1) / 3. + 1.);
+ }
+ l3_15.g[8] += -4.;
+L18:
+ return 0;
+labelL5:
+ if (! l10_18.index2[8]) {
+ goto L15;
+ }
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* labelL14: */
+ l5_21.gg[i__ * 9 - 1] = l2_9.x[i__ - 1] * 2. / ((Real) (i__ - 1)
+ / 3. + 1.);
+ }
+L15:
+ return 0;
+} /* tp375_ */
+
+Real tp375a_(int *i__, int *j)
+{
+ /* System generated locals */
+ Real ret_val;
+
+ ret_val = 1.;
+ if (*i__ == *j) {
+ ret_val = 2.;
+ }
+ return ret_val;
+} /* tp375a_ */
+
+
+/* Subroutine */ int tp376_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 10;
+ l1_1.nili = 0;
+ l1_1.ninl = 14;
+ l1_1.neli = 1;
+ l1_1.nenl = 0;
+ l2_9.x[0] = 10.;
+ l2_9.x[1] = .005;
+ l2_9.x[2] = .0081;
+ l2_9.x[3] = 100.;
+ l2_9.x[4] = .0017;
+ l2_9.x[5] = .0013;
+ l2_9.x[6] = .0027;
+ l2_9.x[7] = .002;
+ l2_9.x[8] = .15;
+ l2_9.x[9] = .105;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ l12_9.lxu[i__ - 1] = true;
+/* labelL6: */
+ l11_9.lxl[i__ - 1] = true;
+ }
+ l13_9.xl[0] = 0.;
+ l13_9.xl[1] = 0.;
+ l13_9.xl[2] = 5e-5;
+ l13_9.xl[3] = 10.;
+ for (i__ = 5; i__ <= 10; ++i__) {
+/* L7: */
+ l13_9.xl[i__ - 1] = 5e-5;
+ }
+ l14_9.xu[0] = 10.;
+ l14_9.xu[1] = .1;
+ l14_9.xu[2] = .0081;
+ l14_9.xu[3] = 1e3;
+ l14_9.xu[4] = .0017;
+ l14_9.xu[5] = .0013;
+ l14_9.xu[6] = .0027;
+ l14_9.xu[7] = .002;
+ l14_9.xu[8] = 1.;
+ l14_9.xu[9] = 1.;
+ l5_32.gg[0] = 1.;
+ l5_32.gg[15] = 0.;
+ for (i__ = 5; i__ <= 10; ++i__) {
+/* labelL9: */
+ l5_32.gg[i__ * 15 - 15] = 0.;
+ }
+ l5_32.gg[1] = 1.;
+ l5_32.gg[16] = 0.;
+ l5_32.gg[31] = 0.;
+ for (i__ = 6; i__ <= 8; ++i__) {
+/* labelL10: */
+ l5_32.gg[i__ * 15 - 14] = 0.;
+ }
+ l5_32.gg[136] = 0.;
+ l5_32.gg[2] = 1.;
+ l5_32.gg[17] = 0.;
+ l5_32.gg[32] = 0.;
+ l5_32.gg[62] = 0.;
+ for (i__ = 7; i__ <= 9; ++i__) {
+/* labelL11: */
+ l5_32.gg[i__ * 15 - 13] = 0.;
+ }
+ l5_32.gg[3] = 1.;
+ l5_32.gg[18] = 0.;
+ l5_32.gg[33] = 0.;
+ l5_32.gg[63] = 0.;
+ l5_32.gg[78] = 0.;
+ for (i__ = 8; i__ <= 10; ++i__) {
+/* labelL12: */
+ l5_32.gg[i__ * 15 - 12] = 0.;
+ }
+ l5_32.gg[4] = 1.;
+ l5_32.gg[19] = 0.;
+ l5_32.gg[34] = 0.;
+ for (i__ = 5; i__ <= 7; ++i__) {
+/* labelL13: */
+ l5_32.gg[i__ * 15 - 11] = 0.;
+ }
+ l5_32.gg[124] = 0.;
+ l5_32.gg[139] = 0.;
+ l5_32.gg[5] = 0.;
+ l5_32.gg[20] = 1e4;
+ l5_32.gg[35] = 0.;
+ for (i__ = 6; i__ <= 8; ++i__) {
+/* labelL14: */
+ l5_32.gg[i__ * 15 - 10] = 0.;
+ }
+ l5_32.gg[140] = 0.;
+ l5_32.gg[6] = 0.;
+ l5_32.gg[21] = 1e4;
+ l5_32.gg[36] = 0.;
+ l5_32.gg[66] = 0.;
+ for (i__ = 7; i__ <= 9; ++i__) {
+/* L15: */
+ l5_32.gg[i__ * 15 - 9] = 0.;
+ }
+ l5_32.gg[7] = 0.;
+ l5_32.gg[22] = 1e4;
+ l5_32.gg[37] = 0.;
+ l5_32.gg[67] = 0.;
+ l5_32.gg[82] = 0.;
+ for (i__ = 8; i__ <= 10; ++i__) {
+/* L16: */
+ l5_32.gg[i__ * 15 - 8] = 0.;
+ }
+ l5_32.gg[8] = 0.;
+ l5_32.gg[23] = 1e4;
+ l5_32.gg[38] = 0.;
+ for (i__ = 5; i__ <= 7; ++i__) {
+/* L17: */
+ l5_32.gg[i__ * 15 - 7] = 0.;
+ }
+ l5_32.gg[128] = 0.;
+ l5_32.gg[143] = 0.;
+ l5_32.gg[9] = 0.;
+ l5_32.gg[24] = 1e4;
+ for (i__ = 5; i__ <= 10; ++i__) {
+/* L18: */
+ l5_32.gg[i__ * 15 - 6] = 0.;
+ }
+ l5_32.gg[10] = 0.;
+ l5_32.gg[25] = 1e4;
+ for (i__ = 5; i__ <= 10; ++i__) {
+/* L19: */
+ l5_32.gg[i__ * 15 - 5] = 0.;
+ }
+ l5_32.gg[11] = 0.;
+ l5_32.gg[26] = 1e4;
+ for (i__ = 5; i__ <= 10; ++i__) {
+/* labelL20: */
+ l5_32.gg[i__ * 15 - 4] = 0.;
+ }
+ l5_32.gg[12] = 0.;
+ l5_32.gg[27] = 1e4;
+ for (i__ = 5; i__ <= 10; ++i__) {
+/* L21: */
+ l5_32.gg[i__ * 15 - 3] = 0.;
+ }
+ l5_32.gg[13] = 0.;
+ l5_32.gg[28] = 0.;
+ l5_32.gg[103] = 0.;
+ l5_32.gg[133] = 0.;
+ l5_32.gg[148] = 0.;
+ for (i__ = 1; i__ <= 8; ++i__) {
+/* L8: */
+ l5_32.gg[i__ * 15 - 1] = 0.;
+ }
+ l5_32.gg[134] = 1.;
+ l5_32.gg[149] = 1.;
+ l20_10.lex = false;
+ l20_10.nex = 1;
+ l20_10.fex = -4430.0879;
+ l20_10.xex[0] = (float).14727222;
+ l20_10.xex[1] = (float).1;
+ l20_10.xex[2] = .0081;
+ l20_10.xex[3] = 628.71731;
+ l20_10.xex[4] = .0017;
+ l20_10.xex[5] = .0011816143;
+ l20_10.xex[6] = .0027;
+ l20_10.xex[7] = .00135;
+ l20_10.xex[8] = (float).15740741;
+ l20_10.xex[9] = .097592593;
+ for (i__ = 3; i__ <= 10; ++i__) {
+/* L45: */
+ l4_9.gf[i__ - 1] = 0.;
+ }
+ return 0;
+labelL2:
+ l6_1.fx = (l2_9.x[0] * .15 + l2_9.x[1] * 14. - .06) * 2e4 / (l2_9.x[0] +
+ .002 + l2_9.x[1] * 60.);
+ l6_1.fx = -l6_1.fx;
+ return 0;
+labelL3:
+ d__1 = l2_9.x[0] + .002 + l2_9.x[1] * 60.;
+ l4_9.gf[0] = ((l2_9.x[0] + .002 + l2_9.x[1] * 60.) * .15 - (l2_9.x[0] *
+ .15 + l2_9.x[1] * 14. - .06)) * 2e4 / pow_dd(&d__1, &c_b305);
+ d__1 = l2_9.x[0] + .002 + l2_9.x[1] * 60.;
+ l4_9.gf[1] = ((l2_9.x[0] + .002 + l2_9.x[1] * 60.) * 14. - (l2_9.x[0] *
+ .15 + l2_9.x[1] * 14. - .06) * 60.) * 2e4 / pow_dd(&d__1, &c_b305)
+ ;
+ l4_9.gf[0] = -l4_9.gf[0];
+ l4_9.gf[1] = -l4_9.gf[1];
+ return 0;
+labelL4:
+ if (l9_14.index1[0]) {
+ l3_13.g[0] = l2_9.x[0] - .75 / l2_9.x[2] / l2_9.x[3];
+ }
+ if (l9_14.index1[1]) {
+ l3_13.g[1] = l2_9.x[0] - l2_9.x[8] / l2_9.x[4] / l2_9.x[3];
+ }
+ if (l9_14.index1[2]) {
+ l3_13.g[2] = l2_9.x[0] - l2_9.x[9] / l2_9.x[5] / l2_9.x[3] - 10. /
+ l2_9.x[3];
+ }
+ if (l9_14.index1[3]) {
+ l3_13.g[3] = l2_9.x[0] - .19 / l2_9.x[6] / l2_9.x[3] - 10. / l2_9.x[3]
+ ;
+ }
+ if (l9_14.index1[4]) {
+ l3_13.g[4] = l2_9.x[0] - .125 / l2_9.x[7] / l2_9.x[3];
+ }
+ if (l9_14.index1[5]) {
+ l3_13.g[5] = l2_9.x[1] * 1e4 - l2_9.x[8] * .00131 * pow_dd(&l2_9.x[4],
+ &c_b2746) * pow_dd(&l2_9.x[3], &c_b940);
+ }
+ if (l9_14.index1[6]) {
+ l3_13.g[6] = l2_9.x[1] * 1e4 - l2_9.x[9] * .001038 * pow_dd(&l2_9.x[5]
+ , &c_b2748) * pow_dd(&l2_9.x[3], &c_b1523);
+ }
+ if (l9_14.index1[7]) {
+ l3_13.g[7] = l2_9.x[1] * 1e4 - pow_dd(&l2_9.x[6], &c_b2746) * 2.23e-4
+ * pow_dd(&l2_9.x[3], &c_b940);
+ }
+ if (l9_14.index1[8]) {
+ l3_13.g[8] = l2_9.x[1] * 1e4 - pow_dd(&l2_9.x[7], &c_b2752) * 7.6e-5 *
+ pow_dd(&l2_9.x[3], &c_b2753);
+ }
+ if (l9_14.index1[9]) {
+/* Computing 2nd power */
+ d__1 = l2_9.x[3];
+ l3_13.g[9] = l2_9.x[1] * 1e4 - pow_dd(&l2_9.x[2], &c_b2754) * 6.98e-4
+ * (d__1 * d__1);
+ }
+ if (l9_14.index1[10]) {
+ l3_13.g[10] = l2_9.x[1] * 1e4 - pow_dd(&l2_9.x[2], &c_b2748) * 5e-5 *
+ pow_dd(&l2_9.x[3], &c_b1523);
+ }
+ if (l9_14.index1[11]) {
+ l3_13.g[11] = l2_9.x[1] * 1e4 - pow_dd(&l2_9.x[2], &c_b2757) *
+ 6.54e-6 * pow_dd(&l2_9.x[3], &c_b2758);
+ }
+ if (l9_14.index1[12]) {
+ l3_13.g[12] = l2_9.x[1] * 1e4 - pow_dd(&l2_9.x[2], &c_b2746) *
+ 2.57e-4 * pow_dd(&l2_9.x[3], &c_b940);
+ }
+ if (l9_14.index1[13]) {
+ l3_13.g[13] = 30. - l2_9.x[4] * 2.003 * l2_9.x[3] - l2_9.x[5] * 1.885
+ * l2_9.x[3] - l2_9.x[7] * .184 * l2_9.x[3] - pow_dd(&l2_9.x[2]
+ , &c_b2761) * 2. * l2_9.x[3];
+ }
+ if (l9_14.index1[14]) {
+ l3_13.g[14] = l2_9.x[8] + l2_9.x[9] - .255;
+ }
+ return 0;
+labelL5:
+ if (! l10_14.index2[0]) {
+ goto L50;
+ }
+ l5_32.gg[30] = .75 / pow_dd(&l2_9.x[2], &c_b305) / l2_9.x[3];
+ l5_32.gg[45] = .75 / l2_9.x[2] / pow_dd(&l2_9.x[3], &c_b305);
+L50:
+ if (! l10_14.index2[1]) {
+ goto L51;
+ }
+ l5_32.gg[46] = l2_9.x[8] / l2_9.x[4] / pow_dd(&l2_9.x[3], &c_b305);
+ l5_32.gg[61] = l2_9.x[8] / pow_dd(&l2_9.x[4], &c_b305) / l2_9.x[3];
+ l5_32.gg[121] = -1. / l2_9.x[4] / l2_9.x[3];
+L51:
+ if (! l10_14.index2[2]) {
+ goto L52;
+ }
+ l5_32.gg[47] = l2_9.x[9] / l2_9.x[5] / pow_dd(&l2_9.x[3], &c_b305) + 10. /
+ pow_dd(&l2_9.x[3], &c_b305);
+ l5_32.gg[77] = l2_9.x[9] / pow_dd(&l2_9.x[5], &c_b305) / l2_9.x[3];
+ l5_32.gg[137] = -1. / l2_9.x[5] / l2_9.x[3];
+L52:
+ if (! l10_14.index2[3]) {
+ goto L53;
+ }
+ l5_32.gg[48] = .19 / l2_9.x[6] / pow_dd(&l2_9.x[3], &c_b305) + 10. /
+ pow_dd(&l2_9.x[3], &c_b305);
+ l5_32.gg[93] = .19 / pow_dd(&l2_9.x[6], &c_b305) / l2_9.x[3];
+L53:
+ if (! l10_14.index2[4]) {
+ goto L54;
+ }
+ l5_32.gg[49] = .125 / l2_9.x[7] / pow_dd(&l2_9.x[3], &c_b305);
+ l5_32.gg[109] = .125 / pow_dd(&l2_9.x[7], &c_b305) / l2_9.x[3];
+L54:
+ if (! l10_14.index2[5]) {
+ goto L55;
+ }
+ l5_32.gg[50] = l2_9.x[8] * -.0019649999999999997 * pow_dd(&l2_9.x[4], &
+ c_b2746) * pow_dd(&l2_9.x[3], &c_b590);
+ l5_32.gg[65] = l2_9.x[8] * -8.7246000000000003e-4 / pow_dd(&l2_9.x[4], &
+ c_b2782) * pow_dd(&l2_9.x[3], &c_b940);
+ l5_32.gg[125] = pow_dd(&l2_9.x[4], &c_b2746) * -.00131 * pow_dd(&l2_9.x[3]
+ , &c_b940);
+L55:
+ if (! l10_14.index2[6]) {
+ goto L56;
+ }
+ l5_32.gg[51] = l2_9.x[9] * -.0031140000000000004 * pow_dd(&l2_9.x[5], &
+ c_b2748) * pow_dd(&l2_9.x[3], &c_b305);
+ l5_32.gg[81] = l2_9.x[9] * -.0016608000000000003 * pow_dd(&l2_9.x[5], &
+ c_b2789) * pow_dd(&l2_9.x[3], &c_b1523);
+ l5_32.gg[141] = pow_dd(&l2_9.x[5], &c_b2748) * -.001038 * pow_dd(&l2_9.x[
+ 3], &c_b1523);
+L56:
+ if (! l10_14.index2[7]) {
+ goto L57;
+ }
+ l5_32.gg[52] = pow_dd(&l2_9.x[6], &c_b2746) * -3.345e-4 * pow_dd(&l2_9.x[
+ 3], &c_b590);
+ l5_32.gg[97] = -1.4851800000000002e-4 / pow_dd(&l2_9.x[6], &c_b2782) *
+ pow_dd(&l2_9.x[3], &c_b940);
+L57:
+ if (! l10_14.index2[8]) {
+ goto L58;
+ }
+ l5_32.gg[53] = pow_dd(&l2_9.x[7], &c_b2752) * -4.3016000000000002e-4 *
+ pow_dd(&l2_9.x[3], &c_b2800);
+ l5_32.gg[113] = pow_dd(&l2_9.x[7], &c_b2801) * -2.698e-4 * pow_dd(&l2_9.x[
+ 3], &c_b2753);
+L58:
+ if (! l10_14.index2[9]) {
+ goto L59;
+ }
+ l5_32.gg[39] = pow_dd(&l2_9.x[2], &c_b1157) * -8.3760000000000008e-4 *
+ pow_dd(&l2_9.x[3], &c_b305);
+ l5_32.gg[54] = pow_dd(&l2_9.x[2], &c_b2754) * -.0013960000000000001 *
+ l2_9.x[3];
+L59:
+ if (! l10_14.index2[10]) {
+ goto L60;
+ }
+ l5_32.gg[40] = pow_dd(&l2_9.x[2], &c_b2789) * -8.0000000000000007e-5 *
+ pow_dd(&l2_9.x[3], &c_b1523);
+ l5_32.gg[55] = pow_dd(&l2_9.x[2], &c_b2748) * -1.5000000000000001e-4 *
+ pow_dd(&l2_9.x[3], &c_b305);
+L60:
+ if (! l10_14.index2[11]) {
+ goto L61;
+ }
+ l5_32.gg[41] = pow_dd(&l2_9.x[2], &c_b2813) * -1.5826799999999999e-5 *
+ pow_dd(&l2_9.x[3], &c_b2758);
+ l5_32.gg[56] = pow_dd(&l2_9.x[2], &c_b2757) * -2.7271799999999999e-5 *
+ pow_dd(&l2_9.x[3], &c_b2816);
+L61:
+ if (! l10_14.index2[12]) {
+ goto L62;
+ }
+ l5_32.gg[42] = -1.7116200000000001e-4 / pow_dd(&l2_9.x[2], &c_b2782) *
+ pow_dd(&l2_9.x[3], &c_b940);
+ l5_32.gg[57] = pow_dd(&l2_9.x[2], &c_b2746) * -3.8549999999999999e-4 *
+ pow_dd(&l2_9.x[3], &c_b590);
+L62:
+ if (! l10_14.index2[13]) {
+ goto L63;
+ }
+ l5_32.gg[43] = -1.6060000000000001 / pow_dd(&l2_9.x[2], &c_b2823) *
+ l2_9.x[3];
+ l5_32.gg[58] = l2_9.x[4] * -2.003 - l2_9.x[5] * 1.885 - l2_9.x[7] * .184
+ - pow_dd(&l2_9.x[2], &c_b2761) * 2.;
+ l5_32.gg[73] = l2_9.x[3] * -2.003;
+ l5_32.gg[88] = l2_9.x[3] * -1.885;
+ l5_32.gg[118] = l2_9.x[3] * -.184;
+L63:
+ return 0;
+} /* tp376_ */
+
+
+/* Subroutine */ int tp377_(int *mode)
+{
+ /* Initialized data */
+
+ static Real a[10] = { -6.089,-17.164,-34.054,-5.914,-24.721,-14.986,
+ -24.1,-10.708,-26.662,-22.179 };
+
+ /* Local variables */
+ static int i__;
+ static Real sum;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 10;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 3;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ l2_9.x[i__ - 1] = .1;
+ l12_9.lxu[i__ - 1] = true;
+ l11_9.lxl[i__ - 1] = true;
+ l13_9.xl[i__ - 1] = 1e-5;
+/* labelL6: */
+ l14_9.xu[i__ - 1] = 10.;
+ }
+ l5_14.gg[0] = 1.;
+ l5_14.gg[3] = -2.;
+ l5_14.gg[6] = 2.;
+ l5_14.gg[9] = 0.;
+ l5_14.gg[12] = 0.;
+ l5_14.gg[15] = 1.;
+ for (i__ = 7; i__ <= 9; ++i__) {
+/* L7: */
+ l5_14.gg[i__ * 3 - 3] = 0.;
+ }
+ l5_14.gg[27] = 1.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+/* L8: */
+ l5_14.gg[i__ * 3 - 2] = 0.;
+ }
+ l5_14.gg[10] = 1.;
+ l5_14.gg[13] = -2.;
+ l5_14.gg[16] = 1.;
+ l5_14.gg[19] = 1.;
+ for (i__ = 8; i__ <= 10; ++i__) {
+/* labelL9: */
+ l5_14.gg[i__ * 3 - 2] = 0.;
+ }
+ l5_14.gg[2] = 0.;
+ l5_14.gg[5] = 0.;
+ l5_14.gg[8] = 1.;
+ for (i__ = 4; i__ <= 6; ++i__) {
+/* labelL10: */
+ l5_14.gg[i__ * 3 - 1] = 0.;
+ }
+ l5_14.gg[20] = 1.;
+ l5_14.gg[23] = 1.;
+ l5_14.gg[26] = 2.;
+ l5_14.gg[29] = 1.;
+ l20_10.lex = false;
+ l20_10.nex = 1;
+ l20_10.fex = (float)-795.001;
+ l20_10.xex[0] = (float)10.;
+ l20_10.xex[1] = (float)10.;
+ l20_10.xex[2] = (float)1.;
+ l20_10.xex[3] = (float)10.;
+ l20_10.xex[4] = (float)9.5;
+ l20_10.xex[5] = (float)10.;
+ l20_10.xex[6] = 1e-4;
+ l20_10.xex[7] = 1e-4;
+ l20_10.xex[8] = 1e-4;
+ l20_10.xex[9] = 1e-4;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ sum = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* labelL11: */
+ sum += l2_9.x[i__ - 1];
+ }
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* labelL12: */
+ l6_1.fx += l2_9.x[i__ - 1] * (a[i__ - 1] + std::log(l2_9.x[i__ - 1] / sum))
+ ;
+ }
+labelL3:
+ return 0;
+ sum = 0.;
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L45: */
+ sum += l2_9.x[i__ - 1];
+ }
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L46: */
+ l4_9.gf[i__ - 1] = a[i__ - 1] + std::log(l2_9.x[i__ - 1] / sum);
+ }
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = l2_9.x[0] - l2_9.x[1] * 2. + l2_9.x[2] * 2. + l2_9.x[5] +
+ l2_9.x[9] - 2.;
+ }
+ if (l9_4.index1[1]) {
+ l3_3.g[1] = l2_9.x[3] - l2_9.x[4] * 2. + l2_9.x[5] + l2_9.x[6] - 1.;
+ }
+ if (l9_4.index1[2]) {
+ l3_3.g[2] = l2_9.x[2] + l2_9.x[6] + l2_9.x[7] + l2_9.x[8] * 2. +
+ l2_9.x[9] - 1.;
+ }
+labelL5:
+ return 0;
+} /* tp377_ */
+
+
+/* Subroutine */ int tp378_(int *mode)
+{
+ /* Initialized data */
+
+ static Real a[10] = { -6.089,-17.164,-34.054,-5.914,-24.721,-14.986,
+ -24.1,-10.708,-26.662,-22.179 };
+
+ /* Local variables */
+ static int i__, j;
+ static Real con;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 10;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 3;
+ for (i__ = 1; i__ <= 10; ++i__) {
+ l2_9.x[i__ - 1] = -2.3;
+ l14_9.xu[i__ - 1] = (float)-.1;
+ l12_9.lxu[i__ - 1] = true;
+ l13_9.xl[i__ - 1] = (float)-16.;
+/* labelL6: */
+ l11_9.lxl[i__ - 1] = true;
+ }
+ l20_10.lex = false;
+ l20_10.nex = 1;
+ l20_10.fex = -47.76;
+ l20_10.xex[0] = -3.2024;
+ l20_10.xex[1] = -1.9123;
+ l20_10.xex[2] = -.2444;
+ l20_10.xex[3] = -15.67;
+ l20_10.xex[4] = -.7217;
+ l20_10.xex[5] = -7.2736;
+ l20_10.xex[6] = -3.5965;
+ l20_10.xex[7] = -4.0206;
+ l20_10.xex[8] = -3.2885;
+ l20_10.xex[9] = -2.3344;
+ l5_14.gg[9] = 0.;
+ l5_14.gg[12] = 0.;
+ l5_14.gg[18] = 0.;
+ l5_14.gg[21] = 0.;
+ l5_14.gg[24] = 0.;
+ l5_14.gg[1] = 0.;
+ l5_14.gg[4] = 0.;
+ l5_14.gg[7] = 0.;
+ l5_14.gg[22] = 0.;
+ l5_14.gg[25] = 0.;
+ l5_14.gg[28] = 0.;
+ l5_14.gg[2] = 0.;
+ l5_14.gg[5] = 0.;
+ l5_14.gg[11] = 0.;
+ l5_14.gg[14] = 0.;
+ l5_14.gg[17] = 0.;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ con = 0.;
+ for (j = 1; j <= 10; ++j) {
+/* L7: */
+ con += std::exp(l2_9.x[j - 1]);
+ }
+ con = std::log(con);
+ for (i__ = 1; i__ <= 10; ++i__) {
+/* L8: */
+ l6_1.fx += std::exp(l2_9.x[i__ - 1]) * (a[i__ - 1] + l2_9.x[i__ - 1] - con)
+ ;
+ }
+labelL3:
+ return 0;
+ con = 0.;
+ for (j = 1; j <= 10; ++j) {
+/* L45: */
+ con += std::exp(l2_9.x[j - 1]);
+ }
+ con = std::log(con);
+ for (i__ = 1; i__ <= 10; ++i__) {
+ l4_9.gf[i__ - 1] = std::exp(l2_9.x[i__ - 1]) * (a[i__ - 1] + l2_9.x[i__ -
+ 1] - con);
+/* L46: */
+ }
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = std::exp(l2_9.x[0]) + std::exp(l2_9.x[1]) * 2. + std::exp(l2_9.x[2]) *
+ 2. + std::exp(l2_9.x[5]) + std::exp(l2_9.x[9]) - 2.;
+ }
+ if (l9_4.index1[1]) {
+ l3_3.g[1] = std::exp(l2_9.x[3]) + std::exp(l2_9.x[4]) * 2. + std::exp(l2_9.x[5]) +
+ exp(l2_9.x[6]) - 1.;
+ }
+ if (l9_4.index1[2]) {
+ l3_3.g[2] = std::exp(l2_9.x[2]) + std::exp(l2_9.x[6]) + std::exp(l2_9.x[7]) + std::exp(
+ l2_9.x[8]) * 2. + std::exp(l2_9.x[9]) - 1.;
+ }
+ return 0;
+labelL5:
+ if (! l10_4.index2[0]) {
+ goto L47;
+ }
+ l5_14.gg[0] = std::exp(l2_9.x[0]);
+ l5_14.gg[3] = std::exp(l2_9.x[1]) * 2.;
+ l5_14.gg[6] = std::exp(l2_9.x[2]) * 2.;
+ l5_14.gg[15] = std::exp(l2_9.x[5]);
+ l5_14.gg[27] = std::exp(l2_9.x[9]);
+L47:
+ if (! l10_4.index2[1]) {
+ goto L48;
+ }
+ l5_14.gg[10] = std::exp(l2_9.x[3]);
+ l5_14.gg[13] = std::exp(l2_9.x[4]) * 2.;
+ l5_14.gg[16] = std::exp(l2_9.x[5]);
+ l5_14.gg[19] = std::exp(l2_9.x[6]);
+L48:
+ if (! l10_4.index2[2]) {
+ goto L49;
+ }
+ l5_14.gg[8] = std::exp(l2_9.x[2]);
+ l5_14.gg[20] = std::exp(l2_9.x[6]);
+ l5_14.gg[23] = std::exp(l2_9.x[7]);
+ l5_14.gg[26] = std::exp(l2_9.x[8]) * 2.;
+ l5_14.gg[29] = std::exp(l2_9.x[9]);
+L49:
+ return 0;
+} /* tp378_ */
+
+/* Subroutine */ int tp379_(int *mode)
+{
+ /* Initialized data */
+
+ static Real y[65] = { 1.366,1.191,1.112,1.013,.991,.885,.831,.847,
+ .786,.725,.746,.679,.608,.655,.616,.606,.602,.626,.651,.724,.649,
+ .649,.694,.644,.624,.661,.612,.558,.533,.495,.5,.423,.395,.375,
+ .372,.391,.396,.405,.428,.429,.523,.562,.607,.653,.672,.708,.633,
+ .668,.645,.632,.591,.559,.597,.625,.739,.71,.729,.72,.636,.581,
+ .428,.292,.162,.098,.054 };
+
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static int i__, j;
+ static Real t;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL2;
+ case 4: goto labelL4;
+ case 5: goto labelL4;
+ }
+labelL1:
+ l1_1.n = 11;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l15_1.lsum = 65;
+ l2_17.x[0] = 1.3;
+ l2_17.x[1] = .65;
+ l2_17.x[2] = .65;
+ l2_17.x[3] = .7;
+ l2_17.x[4] = .6;
+ l2_17.x[5] = 3.;
+ l2_17.x[6] = 5.;
+ l2_17.x[7] = 7.;
+ l2_17.x[8] = 2.;
+ l2_17.x[9] = 4.5;
+ l2_17.x[10] = 5.5;
+ for (i__ = 1; i__ <= 11; ++i__) {
+ l12_17.lxu[i__ - 1] = false;
+ l13_21.xl[i__ - 1] = (float)0.;
+/* labelL6: */
+ l11_17.lxl[i__ - 1] = true;
+ }
+ l20_18.lex = false;
+ l20_18.nex = 1;
+ l20_18.fex = .0401377;
+ l20_18.xex[0] = 1.30997;
+ l20_18.xex[1] = .431554;
+ l20_18.xex[2] = .63366;
+ l20_18.xex[3] = .59943;
+ l20_18.xex[4] = .754183;
+ l20_18.xex[5] = .904286;
+ l20_18.xex[6] = 1.36581;
+ l20_18.xex[7] = 4.82369;
+ l20_18.xex[8] = 2.39868;
+ l20_18.xex[9] = 4.56887;
+ l20_18.xex[10] = 5.67534;
+ return 0;
+labelL2:
+ for (i__ = 1; i__ <= 65; ++i__) {
+ t = (Real) (i__ - 1) * .1;
+/* L7: */
+/* Computing 2nd power */
+ d__1 = t - l2_17.x[8];
+/* Computing 2nd power */
+ d__2 = t - l2_17.x[9];
+/* Computing 2nd power */
+ d__3 = t - l2_17.x[10];
+ l16_19.f[i__ - 1] = y[i__ - 1] - (l2_17.x[0] * std::exp(-l2_17.x[4] * t) +
+ l2_17.x[1] * std::exp(-l2_17.x[5] * (d__1 * d__1)) + l2_17.x[2] *
+ exp(-l2_17.x[6] * (d__2 * d__2)) + l2_17.x[3] * std::exp(-l2_17.x[
+ 7] * (d__3 * d__3)));
+ }
+ if (*mode == 3) {
+ goto labelL3;
+ }
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 65; ++i__) {
+/* L70: */
+/* Computing 2nd power */
+ d__1 = l16_19.f[i__ - 1];
+ l6_1.fx += d__1 * d__1;
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 65; ++i__) {
+ t = (Real) (i__ - 1) * .1;
+ l17_24.df[i__ - 1] = -exp(-l2_17.x[4] * t);
+/* Computing 2nd power */
+ d__1 = t - l2_17.x[8];
+ l17_24.df[i__ + 64] = -exp(-l2_17.x[5] * (d__1 * d__1));
+/* Computing 2nd power */
+ d__1 = t - l2_17.x[9];
+ l17_24.df[i__ + 129] = -exp(-l2_17.x[6] * (d__1 * d__1));
+/* Computing 2nd power */
+ d__1 = t - l2_17.x[10];
+ l17_24.df[i__ + 194] = -exp(-l2_17.x[7] * (d__1 * d__1));
+ l17_24.df[i__ + 259] = l2_17.x[0] * t * std::exp(-l2_17.x[4] * t);
+/* Computing 2nd power */
+ d__1 = t - l2_17.x[8];
+/* Computing 2nd power */
+ d__2 = t - l2_17.x[8];
+ l17_24.df[i__ + 324] = l2_17.x[1] * (d__1 * d__1) * std::exp(-l2_17.x[5] *
+ (d__2 * d__2));
+/* Computing 2nd power */
+ d__1 = t - l2_17.x[9];
+/* Computing 2nd power */
+ d__2 = t - l2_17.x[9];
+ l17_24.df[i__ + 389] = l2_17.x[2] * (d__1 * d__1) * std::exp(-l2_17.x[6] *
+ (d__2 * d__2));
+/* Computing 2nd power */
+ d__1 = t - l2_17.x[10];
+/* Computing 2nd power */
+ d__2 = t - l2_17.x[10];
+ l17_24.df[i__ + 454] = l2_17.x[3] * (d__1 * d__1) * std::exp(-l2_17.x[7] *
+ (d__2 * d__2));
+/* Computing 2nd power */
+ d__1 = t - l2_17.x[8];
+ l17_24.df[i__ + 519] = -l2_17.x[1] * l2_17.x[5] * 2. * (t - l2_17.x[8]
+ ) * std::exp(-l2_17.x[5] * (d__1 * d__1));
+/* Computing 2nd power */
+ d__1 = t - l2_17.x[9];
+ l17_24.df[i__ + 584] = -l2_17.x[2] * l2_17.x[6] * 2. * (t - l2_17.x[9]
+ ) * std::exp(-l2_17.x[6] * (d__1 * d__1));
+/* L8: */
+/* Computing 2nd power */
+ d__1 = t - l2_17.x[10];
+ l17_24.df[i__ + 649] = -l2_17.x[3] * l2_17.x[7] * 2. * (t - l2_17.x[
+ 10]) * std::exp(-l2_17.x[7] * (d__1 * d__1));
+ }
+ for (i__ = 1; i__ <= 11; ++i__) {
+/* L19: */
+ l4_17.gf[i__ - 1] = 0.;
+ }
+ for (j = 1; j <= 11; ++j) {
+ for (i__ = 1; i__ <= 65; ++i__) {
+/* labelL20: */
+ l4_17.gf[j - 1] += l16_19.f[i__ - 1] * 2. * l17_24.df[i__ + j *
+ 65 - 66];
+ }
+ }
+labelL4:
+ return 0;
+} /* tp379_ */
+
+
+/* Subroutine */ int tp380_(int *mode)
+{
+ /* Initialized data */
+
+ static Real a[11] = { -.00133172,-.002270927,-.00248546,-4.67,
+ -4.671973,-.00814,-.008092,-.005,-9.09e-4,-8.8e-4,-.00119 };
+ static Real c__[30] = { .05367373,.021863746,.097733533,.0066940803,
+ 1e-6,1e-5,1e-6,1e-10,1e-8,.01,1e-4,.10898645,1.6108052e-4,1e-23,
+ 1.9304541e-6,.001,1e-6,1e-5,1e-6,1e-9,1e-9,.001,.001,.10898645,
+ 1.6108052e-5,1e-23,1.9304541e-8,1e-5,1.1184059e-4,1e-4 };
+
+ /* System generated locals */
+ Real d__1, d__2, d__3;
+
+ /* Local variables */
+ static Real temp;
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 12;
+ l1_1.nili = 0;
+ l1_1.ninl = 3;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 12; ++i__) {
+ l2_18.x[i__ - 1] = 4.;
+ l12_18.lxu[i__ - 1] = true;
+ l11_18.lxl[i__ - 1] = true;
+ l13_22.xl[i__ - 1] = .1;
+/* labelL6: */
+ l14_22.xu[i__ - 1] = 100.;
+ }
+ l5_10.gg[0] = -c__[0];
+ l5_10.gg[3] = -c__[1];
+ l5_10.gg[6] = -c__[2];
+ for (i__ = 6; i__ <= 12; ++i__) {
+/* L7: */
+ l5_10.gg[i__ * 3 - 3] = 0.;
+ }
+ l5_10.gg[1] = -c__[4];
+ l5_10.gg[7] = -c__[6];
+ l5_10.gg[22] = 0.;
+ l5_10.gg[25] = 0.;
+ l5_10.gg[31] = 0.;
+ l5_10.gg[8] = -c__[18];
+ l5_10.gg[17] = -c__[21];
+ l5_10.gg[20] = 0.;
+ l5_10.gg[23] = -c__[22];
+ l5_10.gg[29] = 0.;
+ l5_10.gg[32] = -c__[29];
+ l5_10.gg[35] = 0.;
+ l20_5.lex = false;
+ l20_5.nex = 1;
+/* FEX=0.316859D+1 */
+ l20_5.fex = 316822.15000000002;
+ l20_5.xex[0] = 2.6631947068;
+ l20_5.xex[1] = 4.517277762;
+ l20_5.xex[2] = 7.133802907;
+ l20_5.xex[3] = 2.237268448;
+ l20_5.xex[4] = 4.07840382657;
+ l20_5.xex[5] = 1.31827569;
+ l20_5.xex[6] = 4.125187034;
+ l20_5.xex[7] = 2.856195978;
+ l20_5.xex[8] = 1.6765929748;
+ l20_5.xex[9] = 2.1789111052;
+ l20_5.xex[10] = 5.12343515;
+ l20_5.xex[11] = 6.659338016;
+ return 0;
+labelL2:
+ l6_1.fx = 1e5;
+ for (i__ = 1; i__ <= 11; ++i__) {
+ temp = l2_18.x[i__ - 1];
+ if (l2_18.x[i__ - 1] < 1e-15) {
+ temp = 1e-15;
+ }
+/* labelL20: */
+ l6_1.fx *= pow_dd(&temp, &a[i__ - 1]);
+ }
+ l6_1.fx *= 1e5;
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 11; ++i__) {
+ temp = l2_18.x[i__ - 1];
+ if (l2_18.x[i__ - 1] < 1e-15) {
+ temp = 1e-15;
+ }
+ l4_18.gf[i__ - 1] = l6_1.fx * (a[i__ - 1] / temp);
+/* labelL10: */
+ l4_18.gf[i__ - 1] *= 1e5;
+ }
+ l4_18.gf[11] = 0.;
+ return 0;
+labelL4:
+ if (l9_4.index1[0]) {
+ l3_3.g[0] = 1. - c__[0] * l2_18.x[0] - c__[1] * l2_18.x[1] - c__[2] *
+ l2_18.x[2] - c__[3] * l2_18.x[3] * l2_18.x[4];
+ }
+ if (l9_4.index1[1]) {
+/* Computing 2nd power */
+ d__1 = l2_18.x[11];
+ l3_3.g[1] = 1. - c__[4] * l2_18.x[0] - c__[5] * l2_18.x[1] - c__[6] *
+ l2_18.x[2] - c__[7] * l2_18.x[3] * l2_18.x[11] - c__[8] *
+ l2_18.x[4] / l2_18.x[11] - c__[9] * l2_18.x[5] / l2_18.x[11]
+ - c__[10] * l2_18.x[6] * l2_18.x[11] - c__[11] * l2_18.x[3] *
+ l2_18.x[4] - c__[12] * l2_18.x[1] * l2_18.x[4] / l2_18.x[11]
+ - c__[13] * l2_18.x[1] * l2_18.x[3] * l2_18.x[4] - c__[14] *
+ l2_18.x[1] / l2_18.x[3] * l2_18.x[4] / (d__1 * d__1) - c__[15]
+ * l2_18.x[9] / l2_18.x[11];
+ }
+ if (l9_4.index1[2]) {
+ l3_3.g[2] = 1. - c__[16] * l2_18.x[0] - c__[17] * l2_18.x[1] - c__[18]
+ * l2_18.x[2] - c__[19] * l2_18.x[3] - c__[20] * l2_18.x[4] -
+ c__[21] * l2_18.x[5] - c__[22] * l2_18.x[7] - c__[23] *
+ l2_18.x[3] * l2_18.x[4] - c__[24] * l2_18.x[1] * l2_18.x[4] -
+ c__[25] * l2_18.x[1] * l2_18.x[3] * l2_18.x[4] - c__[26] *
+ l2_18.x[1] * l2_18.x[4] / l2_18.x[3] - c__[27] * l2_18.x[8] -
+ c__[28] * l2_18.x[0] * l2_18.x[8] - c__[29] * l2_18.x[10];
+ }
+ return 0;
+labelL5:
+ if (! l10_4.index2[0]) {
+ goto L50;
+ }
+ l5_10.gg[9] = -c__[3] * l2_18.x[4];
+ l5_10.gg[12] = -c__[3] * l2_18.x[3];
+L50:
+ if (! l10_4.index2[1]) {
+ goto L51;
+ }
+/* Computing 2nd power */
+ d__1 = l2_18.x[11];
+ l5_10.gg[4] = -c__[5] - c__[12] * l2_18.x[4] / l2_18.x[11] - c__[13] *
+ l2_18.x[3] * l2_18.x[4] - c__[14] / l2_18.x[3] * l2_18.x[4] / (
+ d__1 * d__1);
+/* Computing 2nd power */
+ d__1 = l2_18.x[3];
+/* Computing 2nd power */
+ d__2 = l2_18.x[11];
+ l5_10.gg[10] = -c__[7] * l2_18.x[11] - c__[11] * l2_18.x[4] - c__[13] *
+ l2_18.x[1] * l2_18.x[4] + c__[14] * l2_18.x[1] / (d__1 * d__1) *
+ l2_18.x[4] / (d__2 * d__2);
+/* Computing 2nd power */
+ d__1 = l2_18.x[11];
+ l5_10.gg[13] = -c__[8] / l2_18.x[11] - c__[11] * l2_18.x[3] - c__[12] *
+ l2_18.x[1] / l2_18.x[11] - c__[13] * l2_18.x[1] * l2_18.x[3] -
+ c__[14] * l2_18.x[1] / l2_18.x[3] / (d__1 * d__1);
+ l5_10.gg[16] = -c__[9] / l2_18.x[11];
+ l5_10.gg[19] = -c__[10] * l2_18.x[11];
+ l5_10.gg[28] = -c__[15] / l2_18.x[11];
+/* Computing 2nd power */
+ d__1 = l2_18.x[11];
+/* Computing 2nd power */
+ d__2 = l2_18.x[11];
+/* Computing 2nd power */
+ d__3 = l2_18.x[11];
+ l5_10.gg[34] = -c__[7] * l2_18.x[3] + c__[8] * l2_18.x[4] / (d__1 * d__1)
+ + c__[9] * l2_18.x[5] / pow_dd(&l2_18.x[11], &c_b305) - c__[10] *
+ l2_18.x[6] + c__[12] * l2_18.x[1] * l2_18.x[4] / (d__2 * d__2) +
+ c__[14] * 2 * l2_18.x[1] / l2_18.x[3] * l2_18.x[4] / pow_dd(&
+ l2_18.x[11], &c_b1523) + c__[15] * l2_18.x[9] / (d__3 * d__3);
+L51:
+ if (! l10_4.index2[2]) {
+ goto L52;
+ }
+ l5_10.gg[2] = -c__[16] - c__[28] * l2_18.x[8];
+ l5_10.gg[5] = -c__[17] - c__[24] * l2_18.x[4] - c__[25] * l2_18.x[3] *
+ l2_18.x[4] - c__[26] * l2_18.x[4] / l2_18.x[3];
+/* Computing 2nd power */
+ d__1 = l2_18.x[3];
+ l5_10.gg[11] = -c__[19] - c__[23] * l2_18.x[4] - c__[25] * l2_18.x[1] *
+ l2_18.x[4] + c__[26] * l2_18.x[1] * l2_18.x[4] / (d__1 * d__1);
+ l5_10.gg[14] = -c__[20] - c__[23] * l2_18.x[3] - c__[24] * l2_18.x[1] -
+ c__[25] * l2_18.x[1] * l2_18.x[3] - c__[26] * l2_18.x[1] /
+ l2_18.x[3];
+ l5_10.gg[26] = -c__[27] - c__[28] * l2_18.x[0];
+L52:
+ return 0;
+} /* tp380_ */
+
+
+/* Subroutine */ int tp381_(int *mode)
+{
+ /* Initialized data */
+
+ static Real r__[13] = { .8,1.1,.85,3.45,2.,2.1,3.,.8,.45,.72,1.8,3.,
+ .6 };
+ static Real s[13] = { 11.6,13.7,9.5,48.5,31.9,51.1,65.5,0.,0.,0.,
+ 21.8,46.9,0. };
+ static Real u[13] = { .05,.07,0.,.33,0.,1.27,1.27,23.35,35.84,.81,
+ 1.79,7.34,0. };
+ static Real v[13] = { .35,.37,.1,.62,0.,1.03,1.69,18.21,.01,.08,.31,
+ 1.59,22.45 };
+
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 13;
+ l1_1.nili = 3;
+ l1_1.ninl = 0;
+ l1_1.neli = 1;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 13; ++i__) {
+ l2_10.x[i__ - 1] = .1;
+ l12_10.lxu[i__ - 1] = false;
+ l11_10.lxl[i__ - 1] = true;
+/* labelL6: */
+ l13_10.xl[i__ - 1] = 0.;
+ }
+ for (i__ = 1; i__ <= 13; ++i__) {
+ l5_38.gg[(i__ << 2) - 4] = s[i__ - 1];
+ l5_38.gg[(i__ << 2) - 3] = u[i__ - 1];
+/* L7: */
+ l5_38.gg[(i__ << 2) - 1] = 1.;
+ }
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* L8: */
+ l5_38.gg[(i__ << 2) - 2] = v[i__ - 1];
+ }
+ l20_11.lex = false;
+ l20_11.nex = 1;
+ l20_11.fex = 1.0149;
+ l20_11.xex[0] = .785586;
+ l20_11.xex[1] = 0.;
+ l20_11.xex[2] = 0.;
+ l20_11.xex[3] = 0.;
+ l20_11.xex[4] = 0.;
+ l20_11.xex[5] = .173918;
+ l20_11.xex[6] = 0.;
+ l20_11.xex[7] = 0.;
+ l20_11.xex[8] = .020643;
+ l20_11.xex[9] = 0.;
+ l20_11.xex[10] = 0.;
+ l20_11.xex[11] = 0.;
+ l20_11.xex[12] = .019853;
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* labelL11: */
+ l4_10.gf[i__ - 1] = r__[i__ - 1];
+ }
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* labelL10: */
+ l6_1.fx += r__[i__ - 1] * l2_10.x[i__ - 1];
+ }
+labelL3:
+ return 0;
+labelL4:
+ if (! l9_7.index1[0]) {
+ goto labelL12;
+ }
+ l3_6.g[0] = 0.;
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* labelL13: */
+ l3_6.g[0] += s[i__ - 1] * l2_10.x[i__ - 1];
+ }
+ l3_6.g[0] += -18.;
+labelL12:
+ if (! l9_7.index1[1]) {
+ goto labelL14;
+ }
+ l3_6.g[1] = 0.;
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* L15: */
+ l3_6.g[1] += u[i__ - 1] * l2_10.x[i__ - 1];
+ }
+ l3_6.g[1] += -1.;
+labelL14:
+ if (! l9_7.index1[2]) {
+ goto L16;
+ }
+ l3_6.g[2] = 0.;
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* L17: */
+ l3_6.g[2] += v[i__ - 1] * l2_10.x[i__ - 1];
+ }
+ l3_6.g[2] += -.9;
+L16:
+ if (! l9_7.index1[3]) {
+ goto labelL5;
+ }
+ l3_6.g[3] = 0.;
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* L19: */
+ l3_6.g[3] += l2_10.x[i__ - 1];
+ }
+ l3_6.g[3] += -1.;
+labelL5:
+ return 0;
+} /* tp381_ */
+
+/* Subroutine */ int tp382_(int *mode)
+{
+ /* Initialized data */
+
+ static Real r__[13] = { .8,1.1,.85,3.45,2.,2.1,3.,.8,.45,.72,1.8,3.,
+ .6 };
+ static Real s[13] = { 11.6,13.7,9.5,48.5,31.9,51.1,65.5,0.,0.,0.,
+ 21.8,46.9,0. };
+ static Real z1[13] = { .4844,.3003,.1444,.0588,4.9863,.0653,21.0222,
+ 0.,0.,0.,.297,9.2933,0. };
+ static Real u[13] = { .05,.07,0.,.33,0.,1.27,1.27,23.35,35.84,.81,
+ 1.79,7.34,0. };
+ static Real z2[13] = { 1e-4,0.,0.,0.,0.,.004,.1404,1.3631,.5138,
+ .0289,.0097,.3893,0. };
+ static Real v[13] = { .35,.37,.1,.62,0.,1.03,1.69,18.21,.01,.08,.31,
+ 1.59,22.45 };
+ static Real z3[13] = { .001,9e-4,1e-4,5e-4,0.,.0021,.0825,.2073,0.,
+ 4e-4,5e-4,.0107,1.0206 };
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real help;
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+
+labelL1:
+ l1_1.n = 13;
+ l1_1.nili = 0;
+ l1_1.ninl = 3;
+ l1_1.neli = 1;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 13; ++i__) {
+ l2_10.x[i__ - 1] = .1;
+ l12_10.lxu[i__ - 1] = false;
+ l11_10.lxl[i__ - 1] = true;
+ l13_10.xl[i__ - 1] = 0.;
+/* labelL6: */
+ l5_38.gg[(i__ << 2) - 1] = 1.;
+ }
+ l20_11.lex = false;
+ l20_11.nex = 1;
+ l20_11.fex = 1.03831;
+ l20_11.xex[0] = .13205;
+ l20_11.xex[1] = 0.;
+ l20_11.xex[2] = 0.;
+ l20_11.xex[3] = 0.;
+ l20_11.xex[4] = 0.;
+ l20_11.xex[5] = .32627;
+ l20_11.xex[6] = 0.;
+ l20_11.xex[7] = 0.;
+ l20_11.xex[8] = .51668;
+ l20_11.xex[9] = 0.;
+ l20_11.xex[10] = 0.;
+ l20_11.xex[11] = 0.;
+ l20_11.xex[12] = .025004;
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* L8: */
+ l4_10.gf[i__ - 1] = r__[i__ - 1];
+ }
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* L7: */
+ l6_1.fx += r__[i__ - 1] * l2_10.x[i__ - 1];
+ }
+labelL3:
+ return 0;
+labelL4:
+ if (! l9_7.index1[0]) {
+ goto labelL9;
+ }
+ l3_6.g[0] = 0.;
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* labelL10: */
+/* Computing 2nd power */
+ d__1 = l2_10.x[i__ - 1];
+ l3_6.g[0] += z1[i__ - 1] * (d__1 * d__1);
+ }
+ l3_6.g[0] = -std::sqrt(l3_6.g[0]) * 1.645 - 18.;
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* labelL11: */
+ l3_6.g[0] += s[i__ - 1] * l2_10.x[i__ - 1];
+ }
+labelL9:
+ if (! l9_7.index1[1]) {
+ goto labelL12;
+ }
+ l3_6.g[1] = 0.;
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* labelL13: */
+/* Computing 2nd power */
+ d__1 = l2_10.x[i__ - 1];
+ l3_6.g[1] += z2[i__ - 1] * (d__1 * d__1);
+ }
+ l3_6.g[1] = -std::sqrt(l3_6.g[1]) * 1.645 - 1.;
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* labelL14: */
+ l3_6.g[1] += u[i__ - 1] * l2_10.x[i__ - 1];
+ }
+labelL12:
+ if (! l9_7.index1[2]) {
+ goto L15;
+ }
+ l3_6.g[2] = 0.;
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* L16: */
+/* Computing 2nd power */
+ d__1 = l2_10.x[i__ - 1];
+ l3_6.g[2] += z3[i__ - 1] * (d__1 * d__1);
+ }
+ l3_6.g[2] = -std::sqrt(l3_6.g[2]) * 1.645 - .9;
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* L17: */
+ l3_6.g[2] += v[i__ - 1] * l2_10.x[i__ - 1];
+ }
+L15:
+ if (! l9_7.index1[3]) {
+ goto L18;
+ }
+ l3_6.g[3] = -1.;
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* L19: */
+ l3_6.g[3] += l2_10.x[i__ - 1];
+ }
+L18:
+ return 0;
+labelL5:
+ if (! l10_7.index2[0]) {
+ goto L27;
+ }
+ help = 0.;
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* L21: */
+/* Computing 2nd power */
+ d__1 = l2_10.x[i__ - 1];
+ help += z1[i__ - 1] * (d__1 * d__1);
+ }
+ help = -.82250000000000001 / std::sqrt(help);
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* L22: */
+ l5_38.gg[(i__ << 2) - 4] = s[i__ - 1] + help * 2. * z1[i__ - 1] *
+ l2_10.x[i__ - 1];
+ }
+L27:
+ if (! l10_7.index2[1]) {
+ goto L28;
+ }
+ help = 0.;
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* L23: */
+/* Computing 2nd power */
+ d__1 = l2_10.x[i__ - 1];
+ help += z2[i__ - 1] * (d__1 * d__1);
+ }
+ help = -.82250000000000001 / std::sqrt(help);
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* L24: */
+ l5_38.gg[(i__ << 2) - 3] = u[i__ - 1] + help * 2. * z2[i__ - 1] *
+ l2_10.x[i__ - 1];
+ }
+L28:
+ if (! l10_7.index2[2]) {
+ goto labelL20;
+ }
+ help = 0.;
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* L25: */
+/* Computing 2nd power */
+ d__1 = l2_10.x[i__ - 1];
+ help += z3[i__ - 1] * (d__1 * d__1);
+ }
+ help = -.82250000000000001 / std::sqrt(help);
+ for (i__ = 1; i__ <= 13; ++i__) {
+/* L26: */
+ l5_38.gg[(i__ << 2) - 2] = v[i__ - 1] + help * 2. * z3[i__ - 1] *
+ l2_10.x[i__ - 1];
+ }
+labelL20:
+ return 0;
+} /* tp382_ */
+
+
+/* Subroutine */ int tp383_(int *mode)
+{
+ /* Initialized data */
+
+ static Real a[14] = { 12842.275,634.25,634.25,634.125,1268.,633.875,
+ 633.75,1267.,760.05,633.25,1266.25,632.875,394.46,940.838 };
+ static Real b[14] = { 25.,26.,26.,27.,28.,29.,30.,32.,33.,34.,35.,
+ 37.,38.,36. };
+ static Real c__[14] = { 5.47934,.83234,.94749,1.11082,2.64824,
+ 1.55868,1.73215,3.90896,2.74284,2.60541,5.96184,3.29522,1.83517,
+ 2.81372 };
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 14;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 1;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 14; ++i__) {
+ l2_19.x[i__ - 1] = .01;
+ l12_19.lxu[i__ - 1] = true;
+ l11_19.lxl[i__ - 1] = true;
+ l13_23.xl[i__ - 1] = 0.;
+ l14_23.xu[i__ - 1] = 1. / b[i__ - 1];
+/* labelL6: */
+ l5_17.gg[i__ - 1] = c__[i__ - 1];
+ }
+ l20_19.lex = false;
+ l20_19.nex = 1;
+ l20_19.fex = 7.28566;
+ l20_19.xex[0] = .04;
+ l20_19.xex[1] = .0382;
+ l20_19.xex[2] = .0358;
+ l20_19.xex[3] = .033;
+ l20_19.xex[4] = .0303;
+ l20_19.xex[5] = .0279;
+ l20_19.xex[6] = .0265;
+ l20_19.xex[7] = .0249;
+ l20_19.xex[8] = .023;
+ l20_19.xex[9] = .0216;
+ l20_19.xex[10] = .0202;
+ l20_19.xex[11] = .0192;
+ l20_19.xex[12] = .0203;
+ l20_19.xex[13] = .0253;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 14; ++i__) {
+/* L7: */
+ l6_1.fx += a[i__ - 1] / (l2_19.x[i__ - 1] + 1e-16);
+ }
+ l6_1.fx *= 1e-5;
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 14; ++i__) {
+/* L8: */
+/* Computing 2nd power */
+ d__1 = l2_19.x[i__ - 1];
+ l4_19.gf[i__ - 1] = -a[i__ - 1] / (d__1 * d__1) * 1e-5;
+ }
+ return 0;
+labelL4:
+ if (! l9_2.index1[0]) {
+ goto labelL5;
+ }
+ l3_1.g[0] = 0.;
+ for (i__ = 1; i__ <= 14; ++i__) {
+/* labelL10: */
+ l3_1.g[0] += c__[i__ - 1] * l2_19.x[i__ - 1];
+ }
+ l3_1.g[0] += -1.;
+labelL5:
+ return 0;
+} /* tp383_ */
+
+
+/* Subroutine */ int tp384_(int *mode)
+{
+ /* Initialized data */
+
+ static Real b[10] = { 385.,470.,560.,565.,645.,430.,485.,455.,390.,
+ 860. };
+ static Real a[150] /* was [10][15] */ = { 100.,90.,70.,50.,50.,
+ 40.,30.,20.,10.,5.,100.,100.,50.,0.,10.,0.,60.,30.,70.,10.,10.,
+ 10.,0.,0.,70.,50.,30.,40.,10.,500.,5.,35.,55.,65.,60.,95.,90.,25.,
+ 35.,5.,10.,20.,25.,35.,45.,50.,0.,40.,25.,20.,0.,5.,100.,100.,45.,
+ 35.,30.,25.,65.,5.,0.,0.,40.,35.,0.,10.,5.,15.,0.,10.,25.,35.,50.,
+ 60.,35.,60.,25.,10.,30.,35.,0.,55.,0.,0.,65.,0.,0.,80.,0.,95.,10.,
+ 25.,30.,15.,5.,45.,70.,20.,0.,70.,55.,20.,60.,0.,75.,15.,20.,30.,
+ 25.,20.,5.,0.,10.,75.,100.,20.,25.,30.,0.,10.,45.,40.,30.,35.,75.,
+ 0.,70.,5.,15.,35.,20.,25.,0.,30.,10.,5.,15.,65.,50.,10.,0.,10.,
+ 40.,65.,0.,5.,15.,20.,55.,30. };
+ static Real d__[15] = { 486.,640.,758.,776.,477.,707.,175.,619.,
+ 627.,614.,475.,377.,524.,468.,529. };
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real c__;
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 15;
+ l1_1.nili = 0;
+ l1_1.ninl = 10;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 15; ++i__) {
+ l2_11.x[i__ - 1] = 0.;
+ l12_11.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_11.lxl[i__ - 1] = false;
+ }
+ l20_12.lex = false;
+ l20_12.nex = 1;
+ l20_12.fex = -8310.259;
+ l20_12.xex[0] = .86095379;
+ l20_12.xex[1] = .91736139;
+ l20_12.xex[2] = .91973646;
+ l20_12.xex[3] = .89600562;
+ l20_12.xex[4] = 1.0372946;
+ l20_12.xex[5] = .97308908;
+ l20_12.xex[6] = .82243629;
+ l20_12.xex[7] = 1.1987219;
+ l20_12.xex[8] = 1.156335;
+ l20_12.xex[9] = 1.1443868;
+ l20_12.xex[10] = 1.0305681;
+ l20_12.xex[11] = .90949479;
+ l20_12.xex[12] = 1.082045;
+ l20_12.xex[13] = .84682383;
+ l20_12.xex[14] = 1.172372;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 15; ++i__) {
+/* L15: */
+ l6_1.fx -= d__[i__ - 1] * l2_11.x[i__ - 1];
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 15; ++i__) {
+/* labelL11: */
+ l4_11.gf[i__ - 1] = -d__[i__ - 1];
+ }
+ return 0;
+labelL4:
+ for (i__ = 1; i__ <= 10; ++i__) {
+ if (! l9_10.index1[i__ - 1]) {
+ goto L7;
+ }
+ c__ = 0.;
+ for (j = 1; j <= 15; ++j) {
+/* labelL9: */
+/* Computing 2nd power */
+ d__1 = l2_11.x[j - 1];
+ c__ += a[i__ + j * 10 - 11] * (d__1 * d__1);
+ }
+ l3_9.g[i__ - 1] = b[i__ - 1] - c__;
+L7:
+ ;
+ }
+ return 0;
+labelL5:
+ for (i__ = 1; i__ <= 10; ++i__) {
+ if (! l10_10.index2[i__ - 1]) {
+ goto labelL10;
+ }
+ for (j = 1; j <= 15; ++j) {
+/* labelL13: */
+ l5_32.gg[i__ + j * 10 - 11] = a[i__ + j * 10 - 11] * -2. *
+ l2_11.x[j - 1];
+ }
+labelL10:
+ ;
+ }
+ return 0;
+} /* tp384_ */
+
+/* Subroutine */ int tp385_(int *mode)
+{
+ /* Initialized data */
+
+ static Real b[10] = { 385.,470.,560.,565.,645.,430.,485.,455.,890.,
+ 460. };
+ static Real a[150] /* was [10][15] */ = { 100.,90.,70.,50.,50.,
+ 40.,30.,20.,10.,5.,100.,100.,50.,0.,10.,0.,60.,30.,70.,10.,10.,
+ 10.,0.,0.,70.,50.,30.,40.,10.,100.,5.,35.,55.,65.,60.,95.,90.,25.,
+ 35.,5.,10.,20.,25.,35.,45.,50.,0.,40.,25.,20.,0.,5.,100.,100.,45.,
+ 35.,30.,25.,65.,5.,0.,0.,40.,35.,0.,10.,5.,15.,0.,10.,25.,35.,50.,
+ 60.,35.,60.,25.,10.,30.,35.,0.,55.,0.,0.,65.,0.,0.,80.,500.,95.,
+ 10.,25.,30.,15.,5.,45.,70.,20.,0.,70.,55.,20.,60.,0.,75.,15.,20.,
+ 30.,25.,20.,5.,0.,10.,75.,100.,20.,25.,30.,0.,10.,45.,40.,30.,35.,
+ 75.,0.,70.,5.,15.,35.,20.,25.,0.,30.,10.,5.,15.,65.,50.,10.,0.,
+ 10.,40.,65.,0.,5.,15.,20.,55.,30. };
+ static Real d__[15] = { 486.,640.,758.,776.,477.,707.,175.,619.,
+ 627.,614.,475.,377.,524.,468.,529. };
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real c__;
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 15;
+ l1_1.nili = 0;
+ l1_1.ninl = 10;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 15; ++i__) {
+ l2_11.x[i__ - 1] = 0.;
+ l12_11.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_11.lxl[i__ - 1] = false;
+ }
+ l20_12.lex = false;
+ l20_12.nex = 1;
+ l20_12.fex = -8315.2859;
+ l20_12.xex[0] = .81347016;
+ l20_12.xex[1] = .11327964;
+ l20_12.xex[2] = 1.0861184;
+ l20_12.xex[3] = .99832977;
+ l20_12.xex[4] = 1.0754862;
+ l20_12.xex[5] = 1.0688758;
+ l20_12.xex[6] = .6278155;
+ l20_12.xex[7] = 1.092998;
+ l20_12.xex[8] = .91363225;
+ l20_12.xex[9] = .86191244;
+ l20_12.xex[10] = 1.0047312;
+ l20_12.xex[11] = .87742949;
+ l20_12.xex[12] = .986715;
+ l20_12.xex[13] = 1.0411266;
+ l20_12.xex[14] = 1.1860995;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 15; ++i__) {
+/* L15: */
+ l6_1.fx -= d__[i__ - 1] * l2_11.x[i__ - 1];
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 15; ++i__) {
+/* labelL11: */
+ l4_11.gf[i__ - 1] = -d__[i__ - 1];
+ }
+ return 0;
+labelL4:
+ for (i__ = 1; i__ <= 10; ++i__) {
+ if (! l9_10.index1[i__ - 1]) {
+ goto L7;
+ }
+ c__ = 0.;
+ for (j = 1; j <= 15; ++j) {
+/* labelL9: */
+/* Computing 2nd power */
+ d__1 = l2_11.x[j - 1];
+ c__ += a[i__ + j * 10 - 11] * (d__1 * d__1);
+ }
+ l3_9.g[i__ - 1] = b[i__ - 1] - c__;
+L7:
+ ;
+ }
+ return 0;
+labelL5:
+ for (i__ = 1; i__ <= 10; ++i__) {
+ if (! l10_10.index2[i__ - 1]) {
+ goto labelL10;
+ }
+ for (j = 1; j <= 15; ++j) {
+/* labelL13: */
+ l5_32.gg[i__ + j * 10 - 11] = a[i__ + j * 10 - 11] * -2. *
+ l2_11.x[j - 1];
+ }
+labelL10:
+ ;
+ }
+ return 0;
+} /* tp385_ */
+
+/* Subroutine */ int tp386_(int *mode)
+{
+ /* Initialized data */
+
+ static Real b[10] = { 385.,470.,560.,565.,645.,430.,485.,455.,390.,
+ 460. };
+ static Real a[150] /* was [10][15] */ = { 100.,90.,70.,50.,50.,
+ 40.,30.,20.,10.,5.,100.,100.,50.,0.,10.,0.,60.,30.,70.,10.,10.,
+ 10.,0.,0.,70.,50.,30.,40.,10.,100.,5.,35.,55.,65.,60.,95.,90.,25.,
+ 35.,5.,10.,20.,25.,35.,45.,50.,0.,40.,25.,20.,0.,5.,100.,100.,45.,
+ 35.,30.,25.,65.,5.,0.,0.,40.,35.,0.,10.,5.,15.,0.,10.,25.,35.,50.,
+ 60.,35.,60.,25.,10.,30.,35.,0.,55.,0.,0.,65.,0.,0.,80.,0.,95.,10.,
+ 25.,30.,15.,5.,45.,70.,20.,0.,70.,55.,20.,60.,0.,75.,15.,20.,30.,
+ 25.,20.,5.,0.,10.,75.,100.,20.,25.,30.,0.,10.,45.,40.,30.,35.,75.,
+ 0.,70.,5.,15.,35.,20.,25.,0.,30.,10.,5.,15.,65.,50.,10.,0.,10.,
+ 40.,65.,0.,5.,15.,20.,55.,30. };
+ static Real d__[15] = { 486.,640.,758.,776.,477.,707.,175.,619.,
+ 627.,614.,475.,377.,524.,468.,529. };
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real c__;
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 15;
+ l1_1.nili = 0;
+ l1_1.ninl = 11;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 15; ++i__) {
+ l2_11.x[i__ - 1] = 0.;
+ l12_11.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_11.lxl[i__ - 1] = false;
+ }
+ l20_12.lex = false;
+ l20_12.nex = 1;
+ l20_12.fex = -8164.3688;
+ l20_12.xex[0] = 1.0042725;
+ l20_12.xex[1] = 1.0871174;
+ l20_12.xex[2] = 1.10338;
+ l20_12.xex[3] = 1.0307192;
+ l20_12.xex[4] = .92857958;
+ l20_12.xex[5] = 1.2568055;
+ l20_12.xex[6] = .76058681;
+ l20_12.xex[7] = .85688931;
+ l20_12.xex[8] = 1.089778;
+ l20_12.xex[9] = .98119425;
+ l20_12.xex[10] = .85106387;
+ l20_12.xex[11] = .96555941;
+ l20_12.xex[12] = .9064419;
+ l20_12.xex[13] = .83804049;
+ l20_12.xex[14] = .80932365;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 15; ++i__) {
+/* labelL20: */
+ l6_1.fx -= d__[i__ - 1] * l2_11.x[i__ - 1];
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 15; ++i__) {
+/* labelL11: */
+ l4_11.gf[i__ - 1] = -d__[i__ - 1];
+ }
+ return 0;
+labelL4:
+ for (i__ = 1; i__ <= 10; ++i__) {
+ if (! l9_13.index1[i__ - 1]) {
+ goto L7;
+ }
+ c__ = 0.;
+ for (j = 1; j <= 15; ++j) {
+/* labelL9: */
+/* Computing 2nd power */
+ d__1 = l2_11.x[j - 1];
+ c__ += a[i__ + j * 10 - 11] * (d__1 * d__1);
+ }
+ l3_12.g[i__ - 1] = b[i__ - 1] - c__;
+L7:
+ ;
+ }
+ if (! l9_13.index1[10]) {
+ goto labelL12;
+ }
+ c__ = 0.;
+ for (j = 1; j <= 15; ++j) {
+/* labelL14: */
+/* Computing 2nd power */
+ d__1 = l2_11.x[j - 1] - 2.;
+ c__ += (Real) j * (d__1 * d__1);
+ }
+ l3_12.g[10] = c__ / 2. - 70.;
+labelL12:
+ return 0;
+labelL5:
+ for (i__ = 1; i__ <= 10; ++i__) {
+ if (! l10_13.index2[i__ - 1]) {
+ goto labelL10;
+ }
+ for (j = 1; j <= 15; ++j) {
+/* labelL13: */
+ l5_39.gg[i__ + j * 11 - 12] = a[i__ + j * 10 - 11] * -2. *
+ l2_11.x[j - 1];
+ }
+labelL10:
+ ;
+ }
+ if (! l10_13.index2[10]) {
+ goto L15;
+ }
+ for (j = 1; j <= 15; ++j) {
+/* L16: */
+ l5_39.gg[j * 11 - 1] = (Real) j * (l2_11.x[j - 1] - 2.);
+ }
+L15:
+ return 0;
+} /* tp386_ */
+
+/* Subroutine */ int tp387_(int *mode)
+{
+ /* Initialized data */
+
+ static Real b[10] = { 385.,470.,560.,565.,645.,430.,485.,455.,390.,
+ 460. };
+ static Real a[150] /* was [10][15] */ = { 100.,90.,70.,50.,50.,
+ 40.,30.,20.,10.,5.,100.,100.,50.,0.,10.,0.,60.,30.,70.,10.,10.,
+ 10.,0.,0.,70.,50.,30.,40.,10.,100.,5.,35.,55.,65.,60.,95.,90.,25.,
+ 35.,5.,10.,20.,25.,35.,45.,50.,0.,40.,25.,20.,0.,5.,100.,100.,45.,
+ 35.,30.,25.,65.,5.,0.,0.,40.,35.,0.,10.,5.,15.,0.,10.,25.,35.,50.,
+ 60.,35.,60.,25.,10.,30.,35.,0.,55.,0.,0.,65.,0.,0.,80.,0.,95.,10.,
+ 25.,30.,15.,5.,45.,70.,20.,0.,70.,55.,20.,60.,0.,75.,15.,20.,30.,
+ 25.,20.,5.,0.,10.,75.,100.,20.,25.,30.,0.,10.,45.,40.,30.,35.,75.,
+ 0.,70.,5.,15.,35.,20.,25.,0.,30.,10.,5.,15.,65.,50.,10.,0.,10.,
+ 40.,65.,0.,5.,15.,20.,55.,30. };
+ static Real d__[15] = { 486.,640.,758.,776.,477.,707.,175.,619.,
+ 627.,614.,475.,377.,524.,468.,529. };
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real c__;
+ static int i__, j;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 15;
+ l1_1.nili = 0;
+ l1_1.ninl = 11;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 15; ++i__) {
+ l2_11.x[i__ - 1] = 0.;
+ l12_11.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_11.lxl[i__ - 1] = false;
+ }
+ l20_12.lex = false;
+ l20_12.nex = 1;
+ l20_12.fex = -8250.1417;
+ l20_12.xex[0] = 1.0125415;
+ l20_12.xex[1] = 1.0158505;
+ l20_12.xex[2] = 1.0309039;
+ l20_12.xex[3] = .99697018;
+ l20_12.xex[4] = .98528372;
+ l20_12.xex[5] = 1.0368532;
+ l20_12.xex[6] = .99349349;
+ l20_12.xex[7] = .9720116;
+ l20_12.xex[8] = .99994095;
+ l20_12.xex[9] = .99547294;
+ l20_12.xex[10] = .9695385;
+ l20_12.xex[11] = 1.0080569;
+ l20_12.xex[12] = .98236999;
+ l20_12.xex[13] = .99057993;
+ l20_12.xex[14] = .97760168;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 15; ++i__) {
+/* labelL20: */
+ l6_1.fx -= d__[i__ - 1] * l2_11.x[i__ - 1];
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 15; ++i__) {
+/* labelL11: */
+ l4_11.gf[i__ - 1] = -d__[i__ - 1];
+ }
+ return 0;
+labelL4:
+ for (i__ = 1; i__ <= 10; ++i__) {
+ if (! l9_13.index1[i__ - 1]) {
+ goto L7;
+ }
+ c__ = 0.;
+ for (j = 1; j <= 15; ++j) {
+/* labelL9: */
+/* Computing 2nd power */
+ d__1 = l2_11.x[j - 1];
+ c__ += a[i__ + j * 10 - 11] * (d__1 * d__1);
+ }
+ l3_12.g[i__ - 1] = b[i__ - 1] - c__;
+L7:
+ ;
+ }
+ if (! l9_13.index1[10]) {
+ goto labelL12;
+ }
+ c__ = 0.;
+ for (j = 1; j <= 15; ++j) {
+/* labelL14: */
+/* Computing 2nd power */
+ d__1 = l2_11.x[j - 1] - 2.;
+ c__ += (Real) j * (d__1 * d__1);
+ }
+ l3_12.g[10] = c__ / 2. - 61.;
+labelL12:
+ return 0;
+labelL5:
+ for (i__ = 1; i__ <= 10; ++i__) {
+ if (! l10_13.index2[i__ - 1]) {
+ goto labelL10;
+ }
+ for (j = 1; j <= 15; ++j) {
+/* labelL13: */
+ l5_39.gg[i__ + j * 11 - 12] = a[i__ + j * 10 - 11] * -2. *
+ l2_11.x[j - 1];
+ }
+labelL10:
+ ;
+ }
+ if (! l10_13.index2[10]) {
+ goto L15;
+ }
+ for (j = 1; j <= 15; ++j) {
+/* L16: */
+ l5_39.gg[j * 11 - 1] = (Real) j * (l2_11.x[j - 1] - 2.);
+ }
+L15:
+ return 0;
+} /* tp387_ */
+
+/* Subroutine */ int tp388_(int *mode)
+{
+ /* Initialized data */
+
+ static Real b[15] = { 385.,470.,560.,565.,645.,430.,485.,455.,390.,
+ 460.,0.,70.,361.,265.,395. };
+ static Real a[150] /* was [10][15] */ = { 100.,90.,70.,50.,50.,
+ 40.,30.,20.,10.,5.,100.,100.,50.,0.,10.,0.,60.,30.,70.,10.,10.,
+ 10.,0.,0.,70.,50.,30.,40.,10.,100.,5.,35.,55.,65.,60.,95.,90.,25.,
+ 35.,5.,10.,20.,25.,35.,45.,50.,0.,40.,25.,20.,0.,5.,100.,100.,45.,
+ 35.,30.,25.,65.,5.,0.,0.,40.,35.,0.,10.,5.,15.,0.,10.,25.,35.,50.,
+ 60.,35.,60.,25.,10.,30.,35.,0.,55.,0.,0.,65.,0.,0.,80.,0.,95.,10.,
+ 25.,30.,15.,5.,45.,70.,20.,0.,70.,55.,20.,60.,0.,75.,15.,20.,30.,
+ 25.,20.,5.,0.,10.,75.,100.,20.,25.,30.,0.,10.,45.,40.,30.,35.,75.,
+ 0.,70.,5.,15.,35.,20.,25.,0.,30.,10.,5.,15.,65.,50.,10.,0.,10.,
+ 40.,65.,0.,5.,15.,20.,55.,30. };
+ static Real a1[60] /* was [4][15] */ = { 1.,45.,53.,12.,2.,25.,
+ 74.,43.,3.,35.,26.,51.,4.,85.,17.,39.,5.,40.,25.,58.,6.,73.,25.,
+ 42.,7.,17.,26.,60.,8.,52.,24.,20.,9.,86.,85.,40.,10.,14.,35.,80.,
+ 15.,30.,14.,75.,16.,50.,23.,85.,17.,40.,37.,95.,18.,70.,56.,23.,
+ 19.,60.,10.,67. };
+ static Real d__[15] = { 486.,640.,758.,776.,477.,707.,175.,619.,
+ 627.,614.,475.,377.,524.,468.,529. };
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real c__;
+ static int i__, j, l;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 15;
+ l1_1.nili = 4;
+ l1_1.ninl = 11;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 15; ++i__) {
+ l2_11.x[i__ - 1] = 0.;
+ l12_11.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_11.lxl[i__ - 1] = false;
+ }
+ for (i__ = 12; i__ <= 15; ++i__) {
+ for (j = 1; j <= 15; ++j) {
+/* L19: */
+ l5_40.gg[i__ - 11 + j * 15 - 16] = -a1[i__ - 11 + (j << 2) - 5];
+ }
+ }
+ l20_12.lex = false;
+ l20_12.nex = 1;
+ l20_12.fex = -5821.0842;
+ l20_12.xex[0] = .62683876;
+ l20_12.xex[1] = 1.4330999;
+ l20_12.xex[2] = 1.4625963;
+ l20_12.xex[3] = .73133338;
+ l20_12.xex[4] = .7861424;
+ l20_12.xex[5] = 1.2048598;
+ l20_12.xex[6] = -1.1433978;
+ l20_12.xex[7] = 1.0611103;
+ l20_12.xex[8] = -.13389293;
+ l20_12.xex[9] = 1.1820107;
+ l20_12.xex[10] = .96917757;
+ l20_12.xex[11] = -.84501289;
+ l20_12.xex[12] = .48122454;
+ l20_12.xex[13] = -.33986164;
+ l20_12.xex[14] = .68589012;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 15; ++i__) {
+/* labelL20: */
+ l6_1.fx -= d__[i__ - 1] * l2_11.x[i__ - 1];
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 15; ++i__) {
+/* labelL11: */
+ l4_11.gf[i__ - 1] = -d__[i__ - 1];
+ }
+ return 0;
+labelL4:
+ for (i__ = 12; i__ <= 15; ++i__) {
+ l = i__ - 11;
+ if (! l9_14.index1[l - 1]) {
+ goto L7;
+ }
+ c__ = 0.;
+ for (j = 1; j <= 15; ++j) {
+/* labelL9: */
+ c__ += a1[l + (j << 2) - 5] * l2_11.x[j - 1];
+ }
+ l3_13.g[l - 1] = b[i__ - 1] - c__;
+L7:
+ ;
+ }
+ for (i__ = 1; i__ <= 10; ++i__) {
+ if (! l9_14.index1[i__ + 3]) {
+ goto labelL14;
+ }
+ c__ = 0.;
+ for (j = 1; j <= 15; ++j) {
+/* L16: */
+/* Computing 2nd power */
+ d__1 = l2_11.x[j - 1];
+ c__ += a[i__ + j * 10 - 11] * (d__1 * d__1);
+ }
+ l3_13.g[i__ + 3] = b[i__ - 1] - c__;
+labelL14:
+ ;
+ }
+ if (! l9_14.index1[14]) {
+ goto L17;
+ }
+ c__ = 0.;
+ for (j = 1; j <= 15; ++j) {
+/* L18: */
+/* Computing 2nd power */
+ d__1 = l2_11.x[j - 1] - 2.;
+ c__ += (Real) j * (d__1 * d__1);
+ }
+ l3_13.g[14] = c__ / 2. - 193.121;
+L17:
+ return 0;
+labelL5:
+ for (i__ = 1; i__ <= 10; ++i__) {
+ if (! l10_14.index2[i__ + 3]) {
+ goto L22;
+ }
+ for (j = 1; j <= 15; ++j) {
+/* L24: */
+ l5_40.gg[i__ + 4 + j * 15 - 16] = a[i__ + j * 10 - 11] * -2. *
+ l2_11.x[j - 1];
+ }
+L22:
+ ;
+ }
+ if (! l10_14.index2[14]) {
+ goto L25;
+ }
+ for (j = 1; j <= 15; ++j) {
+/* L26: */
+ l5_40.gg[j * 15 - 1] = (Real) j * (l2_11.x[j - 1] - 2.);
+ }
+L25:
+ return 0;
+} /* tp388_ */
+
+/* Subroutine */ int tp389_(int *mode)
+{
+ /* Initialized data */
+
+ static Real b[15] = { 385.,470.,560.,565.,645.,430.,485.,455.,390.,
+ 460.,0.,70.,361.,265.,395. };
+ static Real a[150] /* was [10][15] */ = { 100.,90.,70.,50.,50.,
+ 40.,30.,20.,10.,5.,100.,100.,50.,0.,10.,0.,60.,30.,70.,10.,10.,
+ 10.,0.,0.,70.,50.,30.,40.,10.,100.,5.,35.,55.,65.,60.,95.,90.,25.,
+ 35.,5.,10.,20.,25.,35.,45.,50.,0.,40.,25.,20.,0.,5.,100.,100.,45.,
+ 35.,30.,25.,65.,5.,0.,0.,40.,35.,0.,10.,5.,15.,0.,10.,25.,35.,50.,
+ 60.,35.,60.,25.,10.,30.,35.,0.,55.,0.,0.,65.,0.,0.,80.,0.,95.,10.,
+ 25.,30.,15.,5.,45.,70.,20.,0.,70.,55.,20.,60.,0.,75.,15.,20.,30.,
+ 25.,20.,5.,0.,10.,75.,100.,20.,25.,30.,0.,10.,45.,40.,30.,35.,75.,
+ 0.,70.,5.,15.,35.,20.,25.,0.,30.,10.,5.,15.,65.,50.,10.,0.,10.,
+ 40.,65.,0.,5.,15.,20.,55.,30. };
+ static Real a1[60] /* was [4][15] */ = { 1.,45.,53.,12.,2.,25.,
+ 74.,43.,3.,35.,26.,51.,4.,85.,17.,39.,5.,40.,25.,58.,6.,73.,25.,
+ 42.,7.,17.,26.,60.,8.,52.,24.,20.,9.,86.,85.,40.,10.,14.,35.,80.,
+ 15.,30.,14.,75.,16.,50.,23.,85.,17.,40.,37.,95.,18.,70.,56.,23.,
+ 19.,60.,10.,67. };
+ static Real d__[15] = { 486.,640.,758.,776.,477.,707.,175.,619.,
+ 627.,614.,475.,377.,524.,468.,529. };
+
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ static Real c__;
+ static int i__, j, l;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 15;
+ l1_1.nili = 4;
+ l1_1.ninl = 11;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 15; ++i__) {
+ l2_11.x[i__ - 1] = 0.;
+ l12_11.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_11.lxl[i__ - 1] = false;
+ }
+ for (i__ = 12; i__ <= 15; ++i__) {
+ for (j = 1; j <= 15; ++j) {
+/* L19: */
+ l5_40.gg[i__ - 11 + j * 15 - 16] = -a1[i__ - 11 + (j << 2) - 5];
+ }
+ }
+ l20_12.lex = false;
+ l20_12.nex = 1;
+ l20_12.fex = -5809.7197;
+ l20_12.xex[0] = .67105172;
+ l20_12.xex[1] = 1.38854;
+ l20_12.xex[2] = 1.4676761;
+ l20_12.xex[3] = .76023633;
+ l20_12.xex[4] = .82935674;
+ l20_12.xex[5] = 1.1638523;
+ l20_12.xex[6] = -1.257829;
+ l20_12.xex[7] = .98193399;
+ l20_12.xex[8] = .068416463;
+ l20_12.xex[9] = 1.1472773;
+ l20_12.xex[10] = .98662969;
+ l20_12.xex[11] = -.88834924;
+ l20_12.xex[12] = .56465631;
+ l20_12.xex[13] = -.58120082;
+ l20_12.xex[14] = .72096897;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 15; ++i__) {
+/* labelL20: */
+ l6_1.fx -= d__[i__ - 1] * l2_11.x[i__ - 1];
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 15; ++i__) {
+/* labelL11: */
+ l4_11.gf[i__ - 1] = -d__[i__ - 1];
+ }
+ return 0;
+labelL4:
+ for (i__ = 12; i__ <= 15; ++i__) {
+ l = i__ - 11;
+ if (! l9_14.index1[l - 1]) {
+ goto L7;
+ }
+ c__ = 0.;
+ for (j = 1; j <= 15; ++j) {
+/* labelL9: */
+ c__ += a1[l + (j << 2) - 5] * l2_11.x[j - 1];
+ }
+ l3_13.g[l - 1] = b[i__ - 1] - c__;
+L7:
+ ;
+ }
+ for (i__ = 1; i__ <= 10; ++i__) {
+ if (! l9_14.index1[i__ + 3]) {
+ goto labelL14;
+ }
+ c__ = 0.;
+ for (j = 1; j <= 15; ++j) {
+/* L16: */
+/* Computing 2nd power */
+ d__1 = l2_11.x[j - 1];
+ c__ += a[i__ + j * 10 - 11] * (d__1 * d__1);
+ }
+ l3_13.g[i__ + 3] = b[i__ - 1] - c__;
+labelL14:
+ ;
+ }
+ if (! l9_14.index1[14]) {
+ goto L17;
+ }
+ c__ = 0.;
+ for (j = 1; j <= 15; ++j) {
+/* L18: */
+/* Computing 2nd power */
+ d__1 = l2_11.x[j - 1] - 2.;
+ c__ += (Real) j * (d__1 * d__1);
+ }
+ l3_13.g[14] = c__ / 2. - 200.;
+L17:
+ return 0;
+labelL5:
+ for (i__ = 1; i__ <= 10; ++i__) {
+ if (! l10_14.index2[i__ + 3]) {
+ goto L22;
+ }
+ for (j = 1; j <= 15; ++j) {
+/* L24: */
+ l5_40.gg[i__ + 4 + j * 15 - 16] = a[i__ + j * 10 - 11] * -2. *
+ l2_11.x[j - 1];
+ }
+L22:
+ ;
+ }
+ if (! l10_14.index2[14]) {
+ goto L25;
+ }
+ for (j = 1; j <= 15; ++j) {
+/* L26: */
+ l5_40.gg[j * 15 - 1] = (Real) j * (l2_11.x[j - 1] - 2.);
+ }
+L25:
+ return 0;
+} /* tp389_ */
+
+/* Subroutine */ int tp390_(int *mode)
+{
+ /* System generated locals */
+ Real d__1;
+
+ /* Local variables */
+ int tp390a_(Real *, Real *);
+ static int i__;
+ static Real zi1, zi2, zi3, psi[11];
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL3;
+ }
+labelL1:
+ l1_1.n = 19;
+ l1_1.nili = 1;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 11;
+ l2_20.x[0] = .02;
+ l2_20.x[1] = 4.;
+ l2_20.x[2] = 100.;
+ l2_20.x[3] = 100.;
+ l2_20.x[4] = 15.;
+ l2_20.x[5] = 15.;
+ l2_20.x[6] = 100.;
+ l2_20.x[7] = 1e3;
+ l2_20.x[8] = 1e3;
+ l2_20.x[9] = 1e3;
+ l2_20.x[10] = 9e3;
+ l2_20.x[11] = .001;
+ l2_20.x[12] = .001;
+ l2_20.x[13] = 1.;
+ l2_20.x[14] = .001;
+ l2_20.x[15] = .001;
+ l2_20.x[16] = .1;
+ l2_20.x[17] = 8e3;
+ l2_20.x[18] = .001;
+ for (i__ = 1; i__ <= 19; ++i__) {
+ l12_20.lxu[i__ - 1] = true;
+ l11_20.lxl[i__ - 1] = true;
+ l14_24.xu[i__ - 1] = 1e5;
+/* labelL6: */
+ l13_24.xl[i__ - 1] = 1e-5;
+ }
+ for (i__ = 1; i__ <= 2; ++i__) {
+ l14_24.xu[i__ - 1] = 50.;
+/* L7: */
+ l14_24.xu[i__ + 14] = 50.;
+ }
+ for (i__ = 3; i__ <= 6; ++i__) {
+/* labelL9: */
+ l14_24.xu[i__ - 1] = 100.;
+ }
+ for (i__ = 12; i__ <= 15; ++i__) {
+/* labelL10: */
+ l14_24.xu[i__ - 1] = 1.;
+ }
+ l20_20.lex = false;
+ l20_20.nex = 1;
+ l20_20.fex = 24.4724654;
+ l20_20.xex[0] = .004473667;
+ l20_20.xex[1] = 3.441565;
+ l20_20.xex[2] = 99.34824;
+ l20_20.xex[3] = 89.130035;
+ l20_20.xex[4] = 15.279316;
+ l20_20.xex[5] = 15.279316;
+ l20_20.xex[6] = 94.726127;
+ l20_20.xex[7] = 12304.197;
+ l20_20.xex[8] = 12313.263;
+ l20_20.xex[9] = 12313.263;
+ l20_20.xex[10] = 95905.631;
+ l20_20.xex[11] = 1e-5;
+ l20_20.xex[12] = 1e-5;
+ l20_20.xex[13] = .999989;
+ l20_20.xex[14] = 1e-5;
+ l20_20.xex[15] = 1e-5;
+ l20_20.xex[16] = .1622235;
+ l20_20.xex[17] = 8305.1515;
+ l20_20.xex[18] = .0014797356;
+ return 0;
+labelL2:
+ d__1 = l2_20.x[15] * 2268. * l2_20.x[0];
+ zi1 = pow_dd(&d__1, &c_b3046) * 25.;
+ zi2 = l2_20.x[16] * 1.75e5 + pow_dd(&l2_20.x[16], &c_b3047) * 36500.;
+ zi3 = l2_20.x[17] * 12.6 + pow_dd(&c_b3048, &c_b3049) * 5.35 / pow_dd(&
+ l2_20.x[17], &c_b3050);
+ l6_1.fx = (zi1 + zi2 + zi3 + 10950. + (l2_20.x[0] * (l2_20.x[12] -
+ l2_20.x[13]) + l2_20.x[1] * (l2_20.x[11] + 1.) - (1. - l2_20.x[18]
+ ) * 3.) * 1150.) * 1.4;
+ l6_1.fx /= 1e4;
+labelL3:
+ return 0;
+labelL4:
+ if (l9_17.index1[0]) {
+ l3_16.g[0] = 1. - l2_20.x[12] - l2_20.x[13];
+ }
+ tp390a_(l2_20.x, psi);
+ for (i__ = 2; i__ <= 12; ++i__) {
+ if (! l9_17.index1[i__ - 1]) {
+ goto L8;
+ }
+ l3_16.g[i__ - 1] = psi[i__ - 2];
+L8:
+ ;
+ }
+ return 0;
+} /* tp390_ */
+
+int tp390a_(Real *x, Real *psi)
+{
+ /* Local variables */
+ static Real test, ak, ck, zj1, zj2, zj3, zj4, zj5, zk7, zj8, xz4,
+ yz4, zj10, qz12;
+
+ /* Parameter adjustments */
+ --psi;
+ --x;
+
+ /* Function Body */
+ ak = .64749999999999996 / pow_dd(&c_b3053, &c_b3054);
+ xz4 = x[3] * std::exp(-ak * x[16]);
+ zj1 = -(x[1] * x[13] * xz4 + x[19] * 300.);
+ psi[1] = zj1 + x[1] * x[3] - x[2] * x[5] * x[12];
+ yz4 = x[7] + (x[3] - xz4) * .5;
+ zj2 = -x[13] * x[1] * yz4;
+ psi[2] = zj2 + x[1] * x[7] - x[2] * x[9] * x[12];
+ zj3 = (1. - x[19]) * -300. + x[6] * 3. * (1. - x[19]) - x[1] * x[14] *
+ xz4;
+ psi[3] = zj3 + x[2] * (x[4] - x[6]) + x[1] * x[6] * x[14];
+ zj4 = x[11] * 3. * (1. - x[19]) + x[1] * x[14] * (x[11] - yz4);
+ psi[4] = zj4 + x[2] * (x[8] - x[11]);
+ zj5 = x[17] * (x[5] * .48 * x[9] / (x[5] + 100.));
+ psi[5] = zj5 * -2. + x[2] * (x[4] - x[5]);
+ psi[6] = zj5 + x[2] * (x[8] - x[9]) - x[9] * .048 * x[17];
+ zk7 = x[1] * (1. - x[13] - x[14]);
+ qz12 = x[1] * (1. - x[13] - x[14]) + x[2] * (1. - x[12]);
+ psi[7] = -zk7 * xz4 + x[6] * qz12 - x[2] * x[5] * (1. - x[12]);
+ zj8 = x[10] * qz12 - zk7 * yz4;
+ psi[8] = zj8 - x[2] * x[9] * (1. - x[12]);
+ psi[9] = (1. - x[15]) * 6. * (20. - x[6]) + x[11] * (x[2] - (1. - x[15]) *
+ 3. - x[1] * x[14]) + x[19] * 3. * x[11] - x[10] * qz12;
+ ck = .0013285402597402597;
+ test = -ck * x[18] / qz12;
+ if (test > 99.) {
+ zj10 = std::sqrt((std::abs(x[10]))) * (float)-2.1 * std::exp(99.);
+ }
+ if (test < 99.) {
+ zj10 = std::sqrt((std::abs(x[10]))) * (float)-2.1 * std::exp(-ck * x[18] / qz12);
+ }
+ psi[10] = zj10 + (20. - x[6]) * 2.;
+ psi[11] = (1. - x[13]) * x[1] - x[12] * x[2] - x[19] * 3.;
+ return 0;
+} /* tp390a_ */
+
+/* Subroutine */ int tp391_(int *mode)
+{
+ /* System generated locals */
+ int i__1;
+ Real d__1, d__2, d__3, d__4;
+
+ /* Local variables */
+ static Real wurz;
+ static int i__, j;
+ static Real sum;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL3;
+ case 5: goto labelL3;
+ }
+labelL1:
+ l1_1.n = 30;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 30; ++i__) {
+ sum = 0.;
+ for (j = 1; j <= 30; ++j) {
+ if (j == i__) {
+ goto L60;
+ }
+ wurz = std::sqrt((Real) i__ / (Real) j);
+/* Computing 5th power */
+ d__1 = std::sin(std::log(wurz)), d__2 = d__1, d__1 *= d__1;
+/* Computing 5th power */
+ d__3 =std::cos(std::log(wurz)), d__4 = d__3, d__3 *= d__3;
+ sum += wurz * (d__2 * (d__1 * d__1) + d__4 * (d__3 * d__3));
+L60:
+ ;
+ }
+/* Computing 3rd power */
+ i__1 = i__ - 15;
+ l2_14.x[i__ - 1] = ((Real) (i__1 * (i__1 * i__1)) + sum) *
+ -2.8742711;
+ l12_14.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_14.lxl[i__ - 1] = false;
+ }
+ l20_15.lex = false;
+ l20_15.nex = 1;
+ l20_15.fex = 0.;
+ l20_15.xex[0] = 7898.8423;
+ l20_15.xex[1] = 6316.093;
+ l20_15.xex[2] = 4957.3076;
+ l20_15.xex[3] = 3806.6336;
+ l20_15.xex[4] = 2846.7099;
+ l20_15.xex[5] = 2060.1112;
+ l20_15.xex[6] = 1429.4605;
+ l20_15.xex[7] = 937.41818;
+ l20_15.xex[8] = 566.66665;
+ l20_15.xex[9] = 299.90186;
+ l20_15.xex[10] = 119.82826;
+ l20_15.xex[11] = 9.1562939;
+ l20_15.xex[12] = -49.399019;
+ l20_15.xex[13] = -73.1188559;
+ l20_15.xex[14] = -79.2811119;
+ l20_15.xex[15] = -85.1607569;
+ l20_15.xex[16] = -108.03012;
+ l20_15.xex[17] = -165.15913;
+ l20_15.xex[18] = -273.81552;
+ l20_15.xex[19] = -451.26501;
+ l20_15.xex[20] = -714.7715;
+ l20_15.xex[21] = -1081.5972;
+ l20_15.xex[22] = -1569.0029;
+ l20_15.xex[23] = -2194.2479;
+ l20_15.xex[24] = -2974.5902;
+ l20_15.xex[25] = -3927.2868;
+ l20_15.xex[26] = -5069.5936;
+ l20_15.xex[27] = -6418.7656;
+ l20_15.xex[28] = -7992.0568;
+ l20_15.xex[29] = -9806.7206;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 30; ++i__) {
+ sum = 0.;
+ for (j = 1; j <= 30; ++j) {
+ if (j == i__) {
+ goto L70;
+ }
+/* Computing 2nd power */
+ d__1 = l2_14.x[j - 1];
+ wurz = std::sqrt(d__1 * d__1 + (Real) i__ / (Real) j);
+/* Computing 5th power */
+ d__1 = std::sin(std::log(wurz)), d__2 = d__1, d__1 *= d__1;
+/* Computing 5th power */
+ d__3 =std::cos(std::log(wurz)), d__4 = d__3, d__3 *= d__3;
+ sum += wurz * (d__2 * (d__1 * d__1) + d__4 * (d__3 * d__3));
+L70:
+ ;
+ }
+/* L7: */
+/* Computing 3rd power */
+ i__1 = i__ - 15;
+/* Computing 2nd power */
+ d__1 = l2_14.x[i__ - 1] * 420. + (Real) (i__1 * (i__1 * i__1))
+ + sum;
+ l6_1.fx += d__1 * d__1;
+ }
+labelL3:
+ return 0;
+} /* tp391_ */
+
+
+/* Subroutine */ int tp392_(int *mode)
+{
+ /* Initialized data */
+
+ static Real r1[15] /* was [3][5] */ = { 1e3,520.,910.,1e3,520.,
+ 910.,1e3,520.,1e3,1100.,600.,1e3,1100.,600.,1e3 };
+ static Real r2[15] /* was [3][5] */ = { .3,.1,.2,.3,.1,.2,.3,.1,
+ .2,.3,.1,.2,.3,.1,.2 };
+ static Real ka[15] /* was [3][5] */ = { 120.,65.,105.,150.,65.,
+ 105.,150.,80.,120.,170.,80.,120.,170.,80.,120. };
+ static Real k1[15] /* was [3][5] */ = { 150.,75.,140.,150.,75.,
+ 140.,150.,75.,140.,170.,90.,150.,170.,90.,150. };
+ static Real kp[15] /* was [3][5] */ = { 160.,75.,140.,160.,75.,
+ 140.,160.,75.,140.,180.,90.,150.,180.,90.,150. };
+ static Real k3[15] /* was [3][5] */ = { .02,.01,.015,.2,.1,.15,
+ .25,.1,.15,.25,.15,.15,.25,.15,.15 };
+ static Real kl1[15] /* was [3][5] */ = { .005,.005,.005,.05,.05,
+ .05,.06,.06,.06,.06,.06,.06,.06,.06,.06 };
+ static Real kl2[15] /* was [3][5] */ = { 80.,45.,75.,80.,45.,75.,
+ 100.,45.,90.,100.,50.,90.,100.,50.,90. };
+ static Real h__[15] /* was [3][5] */ = { 100.,280.,520.,180.,400.,
+ 400.,220.,450.,500.,150.,450.,630.,100.,400.,600. };
+ static Real t[9] /* was [3][3] */ = { .6,.3,.36,.4,.1,.08,.1,
+ .12,.06 };
+ static Real b[15] /* was [3][5] */ = { 170.,170.,180.,170.,170.,
+ 180.,170.,170.,180.,170.,170.,180.,170.,170.,180. };
+
+ /* System generated locals */
+ int i__1;
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__, j, k, l;
+ static Real sum, sum1;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 30;
+ l1_1.nili = 45;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 0;
+ l2_14.x[0] = 80.;
+ l2_14.x[1] = 100.;
+ l2_14.x[2] = 400.;
+ l2_14.x[3] = 100.;
+ l2_14.x[4] = 200.;
+ l2_14.x[5] = 200.;
+ l2_14.x[6] = 100.;
+ l2_14.x[7] = 250.;
+ l2_14.x[8] = 400.;
+ l2_14.x[9] = 50.;
+ l2_14.x[10] = 200.;
+ l2_14.x[11] = 500.;
+ l2_14.x[12] = 50.;
+ l2_14.x[13] = 200.;
+ l2_14.x[14] = 500.;
+ l2_14.x[15] = 100.;
+ l2_14.x[16] = 120.;
+ l2_14.x[17] = 410.;
+ l2_14.x[18] = 120.;
+ l2_14.x[19] = 250.;
+ l2_14.x[20] = 250.;
+ l2_14.x[21] = 150.;
+ l2_14.x[22] = 300.;
+ l2_14.x[23] = 410.;
+ l2_14.x[24] = 600.;
+ l2_14.x[25] = 250.;
+ l2_14.x[26] = 510.;
+ l2_14.x[27] = 100.;
+ l2_14.x[28] = 250.;
+ l2_14.x[29] = 510.;
+ for (i__ = 1; i__ <= 30; ++i__) {
+ l12_14.lxu[i__ - 1] = false;
+ l11_14.lxl[i__ - 1] = true;
+/* labelL6: */
+ l13_17.xl[i__ - 1] = 0.;
+ }
+ l20_15.lex = false;
+ l20_15.nex = 1;
+ l20_15.fex = -1698.878;
+ l20_15.xex[0] = 99.99;
+ l20_15.xex[1] = 142.22;
+ l20_15.xex[2] = 519.88;
+ l20_15.xex[3] = 136.74;
+ l20_15.xex[4] = 103.47;
+ l20_15.xex[5] = 399.99;
+ l20_15.xex[6] = 191.7;
+ l20_15.xex[7] = 1.56;
+ l20_15.xex[8] = 500.;
+ l20_15.xex[9] = 143.43;
+ l20_15.xex[10] = 82.39;
+ l20_15.xex[11] = 629.82;
+ l20_15.xex[12] = 99.92;
+ l20_15.xex[13] = 125.22;
+ l20_15.xex[14] = 600.;
+ l20_15.xex[15] = 101.85;
+ l20_15.xex[16] = 142.25;
+ l20_15.xex[17] = 519.88;
+ l20_15.xex[18] = 144.58;
+ l20_15.xex[19] = 105.73;
+ l20_15.xex[20] = 409.59;
+ l20_15.xex[21] = 182.01;
+ l20_15.xex[22] = 29.34;
+ l20_15.xex[23] = 490.52;
+ l20_15.xex[24] = 143.43;
+ l20_15.xex[25] = 52.43;
+ l20_15.xex[26] = 629.7;
+ l20_15.xex[27] = 99.92;
+ l20_15.xex[28] = 125.12;
+ l20_15.xex[29] = 600.;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 5; ++i__) {
+ sum = 0.;
+ for (j = 1; j <= 3; ++j) {
+ sum1 = 0.;
+ i__1 = i__;
+ for (k = 1; k <= i__1; ++k) {
+/* L72: */
+ sum1 = sum1 + l2_14.x[j + 12 + k * 3 - 1] - l2_14.x[j - 3 + k
+ * 3 - 1];
+ }
+/* L71: */
+/* Computing 2nd power */
+ d__1 = l2_14.x[(i__ - 1) * 3 + j - 1];
+/* Computing 2nd power */
+ d__2 = l2_14.x[i__ * 3 + 12 + j - 1] - l2_14.x[j + i__ * 3 - 4];
+ sum = sum + l2_14.x[(i__ - 1) * 3 + j - 1] * (r1[j + i__ * 3 - 4]
+ - ka[j + i__ * 3 - 4]) - d__1 * d__1 * r2[j + i__ * 3 - 4]
+ - l2_14.x[i__ * 3 + 12 + j - 1] * (k1[j + i__ * 3 - 4] +
+ kp[j + i__ * 3 - 4]) - d__2 * d__2 * (k3[j + i__ * 3 - 4]
+ + kl1[j + i__ * 3 - 4]) - kl2[j + i__ * 3 - 4] * sum1;
+ }
+/* L70: */
+ l6_1.fx -= sum;
+ }
+ l6_1.fx *= .001;
+labelL3:
+ return 0;
+labelL4:
+ for (i__ = 1; i__ <= 5; ++i__) {
+ for (j = 1; j <= 3; ++j) {
+ l = (i__ - 1) * 3 + j;
+/* L8: */
+ if (l9_19.index1[l - 1]) {
+ l3_18.g[l - 1] = h__[j + i__ * 3 - 4] - l2_14.x[l - 1];
+ }
+ }
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+ for (j = 1; j <= 3; ++j) {
+ l = (i__ - 1) * 3 + j + 15;
+ if (! l9_19.index1[l - 1]) {
+ goto labelL9;
+ }
+ l3_18.g[l - 1] = b[j + i__ * 3 - 4];
+ for (k = 1; k <= 3; ++k) {
+/* labelL10: */
+ l3_18.g[l - 1] -= t[j + k * 3 - 4] * l2_14.x[i__ * 3 + 12 + k
+ - 1];
+ }
+labelL9:
+ ;
+ }
+ }
+ for (i__ = 1; i__ <= 5; ++i__) {
+ for (j = 1; j <= 3; ++j) {
+ l = (i__ - 1) * 3 + j + 30;
+ if (! l9_19.index1[l - 1]) {
+ goto labelL11;
+ }
+ l3_18.g[l - 1] = 0.;
+ i__1 = i__;
+ for (k = 1; k <= i__1; ++k) {
+/* labelL12: */
+ l3_18.g[l - 1] = l3_18.g[l - 1] + l2_14.x[k * 3 + 12 + j - 1]
+ - l2_14.x[j - 3 + k * 3 - 1];
+ }
+labelL11:
+ ;
+ }
+ }
+labelL5:
+ return 0;
+} /* tp392_ */
+
+
+/* Subroutine */ int tp393_(int *mode)
+{
+ /* Local variables */
+ int tp393b_(Real *, Real *);
+ static Real c__, e;
+ static int i__;
+ static Real phi[1];
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 48;
+ l1_1.nili = 0;
+ l1_1.ninl = 1;
+ l1_1.neli = 2;
+ l1_1.nenl = 0;
+ for (i__ = 1; i__ <= 24; ++i__) {
+/* labelL6: */
+ l2_21.x[i__ - 1] = 1.;
+ }
+ for (i__ = 25; i__ <= 30; ++i__) {
+/* L7: */
+ l2_21.x[i__ - 1] = 1.3;
+ }
+ for (i__ = 31; i__ <= 48; ++i__) {
+/* L8: */
+ l2_21.x[i__ - 1] = 1.;
+ }
+ for (i__ = 1; i__ <= 48; ++i__) {
+ l11_21.lxl[i__ - 1] = true;
+/* labelL9: */
+ l13_25.xl[i__ - 1] = .002;
+ }
+ for (i__ = 1; i__ <= 24; ++i__) {
+ l12_21.lxu[i__ - 1] = true;
+/* labelL10: */
+ l14_25.xu[i__ - 1] = 2.;
+ }
+ for (i__ = 25; i__ <= 48; ++i__) {
+/* labelL11: */
+ l12_21.lxu[i__ - 1] = false;
+ }
+ l20_21.lex = false;
+ l20_21.nex = 1;
+ l20_21.fex = .86337998;
+ l20_21.xex[0] = 2.;
+ l20_21.xex[1] = .002;
+ l20_21.xex[2] = 2.;
+ l20_21.xex[3] = .0339797;
+ l20_21.xex[4] = .01657455;
+ l20_21.xex[5] = 2.;
+ l20_21.xex[6] = 1.8945347;
+ l20_21.xex[7] = .002;
+ l20_21.xex[8] = 2.;
+ l20_21.xex[9] = .03424074;
+ l20_21.xex[10] = .016670308;
+ l20_21.xex[11] = 2.;
+ l20_21.xex[12] = 2.;
+ l20_21.xex[13] = .002;
+ l20_21.xex[14] = 2.;
+ l20_21.xex[15] = .002;
+ l20_21.xex[16] = .002;
+ l20_21.xex[17] = 1.988;
+ l20_21.xex[18] = 2.;
+ l20_21.xex[19] = .002;
+ l20_21.xex[20] = 2.;
+ l20_21.xex[21] = .002;
+ l20_21.xex[22] = .002;
+ l20_21.xex[23] = 2.;
+ l20_21.xex[24] = 1.0159886;
+ l20_21.xex[25] = .002;
+ l20_21.xex[26] = 1.003163;
+ l20_21.xex[27] = .002;
+ l20_21.xex[28] = .002;
+ l20_21.xex[29] = .999691944;
+ l20_21.xex[30] = 1.11272844;
+ l20_21.xex[31] = .002;
+ l20_21.xex[32] = 1.1024463;
+ l20_21.xex[33] = .002;
+ l20_21.xex[34] = .002;
+ l20_21.xex[35] = 1.1030764;
+ l20_21.xex[36] = .92326572;
+ l20_21.xex[37] = .9343325;
+ l20_21.xex[38] = .92947437;
+ l20_21.xex[39] = .91383802;
+ l20_21.xex[40] = .90517162;
+ l20_21.xex[41] = .89452569;
+ l20_21.xex[42] = 1.174573;
+ l20_21.xex[43] = .002;
+ l20_21.xex[44] = 1.12080408;
+ l20_21.xex[45] = .002;
+ l20_21.xex[46] = .002;
+ l20_21.xex[47] = 1.1163321536;
+ return 0;
+labelL2:
+ e = 0.;
+ for (i__ = 1; i__ <= 12; ++i__) {
+ c__ = 1. - l2_21.x[i__ - 1];
+/* L100: */
+ e += c__ * 10. * c__;
+ }
+ for (i__ = 25; i__ <= 36; ++i__) {
+ c__ = l2_21.x[i__ - 1] - 1.;
+/* L120: */
+ e += (c__ * 2. * (c__ + std::sqrt(c__ * c__ + .1)) + .1) * 1e3 / 4.;
+ }
+ for (i__ = 37; i__ <= 42; ++i__) {
+ c__ = l2_21.x[i__ - 1] - 1.;
+/* L140: */
+ e += (c__ * 2. * (c__ + std::sqrt(c__ * c__ + .1)) + .1) * 2e3 / 4.;
+ }
+ for (i__ = 43; i__ <= 48; ++i__) {
+/* L160: */
+ e += l2_21.x[i__ - 1] * 100.;
+ }
+ l6_1.fx = e / 1e3;
+labelL3:
+ return 0;
+labelL4:
+ if (! l9_4.index1[0]) {
+ goto labelL12;
+ }
+ tp393b_(l2_21.x, phi);
+ l3_3.g[0] = phi[0];
+labelL12:
+ if (! l9_4.index1[1]) {
+ goto labelL14;
+ }
+ l3_3.g[1] = 12.;
+ for (i__ = 1; i__ <= 12; ++i__) {
+/* labelL13: */
+ l3_3.g[1] -= l2_21.x[i__ - 1];
+ }
+labelL14:
+ if (! l9_4.index1[2]) {
+ goto labelL5;
+ }
+ l3_3.g[2] = 12.;
+ for (i__ = 1; i__ <= 12; ++i__) {
+/* L15: */
+ l3_3.g[2] -= l2_21.x[i__ + 11];
+ }
+labelL5:
+ return 0;
+} /* tp393_ */
+
+
+/* Subroutine */ int tp393b_(Real *x, Real *phi)
+{
+ /* Initialized data */
+
+ static Real a[18] = { .9,.8,1.1,1.,.7,1.1,1.,1.,1.1,.9,.8,1.2,.9,
+ 1.2,1.2,1.,1.,.9 };
+
+ static int i__;
+ static Real r__, u[18];
+ static int k1, k2, k3;
+ static Real alp, sum;
+
+ /* Parameter adjustments */
+ --phi;
+ --x;
+
+ /* Function Body */
+/* 1ST TIER OF GASFIERS */
+ for (i__ = 1; i__ <= 6; ++i__) {
+ k1 = i__ + 24;
+ k2 = i__ + 42;
+ k3 = i__ + 12;
+ alp = x[k1] * x[k1] * a[i__ - 1] * 2. * x[k2] / (x[k2] + 1.) * x[k3];
+/* labelL20: */
+ u[i__ - 1] = x[i__] * x[i__] / (x[i__] + alp);
+ }
+/* 2ND TIER OF GASFIERS */
+ for (i__ = 7; i__ <= 12; ++i__) {
+ k1 = i__ + 24;
+ k2 = i__ + 36;
+ k3 = i__ + 12;
+ alp = x[k1] * x[k1] * a[i__ - 1] * 2. * x[k2] / (x[k2] + 1.) * x[k3];
+ sum = x[i__] + u[i__ - 7];
+/* L40: */
+ u[i__ - 1] = sum * sum / (sum + alp);
+ }
+/* 1ST TIER OF METHANATORS */
+ for (i__ = 13; i__ <= 15; ++i__) {
+ k1 = ((i__ - 10) << 1) + 1;
+ k2 = i__ + 24;
+ alp = x[k2] * x[k2] * a[i__ - 1];
+ sum = u[k1 - 1] + u[k1];
+/* L60: */
+ u[i__ - 1] = sum * sum / (sum + alp);
+ }
+/* 2ND TIER OF METHANATORS */
+ for (i__ = 16; i__ <= 18; ++i__) {
+ k1 = i__ + 24;
+ alp = x[k1] * x[k1] * a[i__ - 1];
+ sum = u[i__ - 4];
+/* L80: */
+ u[i__ - 1] = sum * sum / (sum + alp);
+ }
+ r__ = u[15] + u[16] + u[17];
+ phi[1] = 1.5 - r__;
+ return 0;
+} /* tp393b_ */
+
+
+/* Subroutine */ int tp394_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 20;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 20; ++i__) {
+ l2_13.x[i__ - 1] = 2.;
+ l12_13.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_13.lxl[i__ - 1] = false;
+ }
+ l20_14.lex = false;
+ l20_14.nex = 1;
+ l20_14.fex = 1.9166667;
+ l20_14.xex[0] = .9128716;
+ l20_14.xex[1] = .4082468;
+ l20_14.xex[2] = -1.6746493e-5;
+ l20_14.xex[3] = -5.4074613e-6;
+ l20_14.xex[4] = 1.9606096e-6;
+ l20_14.xex[5] = -8.8626385e-6;
+ l20_14.xex[6] = 8.1697576e-6;
+ l20_14.xex[7] = -1.4386551e-5;
+ l20_14.xex[8] = 2.18312e-5;
+ l20_14.xex[9] = -1.3873341e-5;
+ l20_14.xex[10] = 1.3498048e-5;
+ l20_14.xex[11] = -3.9814429e-6;
+ l20_14.xex[12] = -1.1023953e-5;
+ l20_14.xex[13] = -1.280983e-5;
+ l20_14.xex[14] = 7.9408513e-6;
+ l20_14.xex[15] = 2.04589e-5;
+ l20_14.xex[16] = 4.5644559e-6;
+ l20_14.xex[17] = -9.4429887e-6;
+ l20_14.xex[18] = -1.0142804e-5;
+ l20_14.xex[19] = -1.3788343e-6;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 20; ++i__) {
+/* L8: */
+/* Computing 2nd power */
+ d__1 = l2_13.x[i__ - 1];
+/* Computing 4th power */
+ d__2 = l2_13.x[i__ - 1], d__2 *= d__2;
+ l6_1.fx += (Real) i__ * (d__1 * d__1 + d__2 * d__2);
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 20; ++i__) {
+/* labelL9: */
+/* Computing 3rd power */
+ d__1 = l2_13.x[i__ - 1];
+ l4_13.gf[i__ - 1] = (Real) i__ * (l2_13.x[i__ - 1] * 2. + d__1 *
+ (d__1 * d__1) * 4.);
+ }
+ return 0;
+labelL4:
+ if (! l9_2.index1[0]) {
+ goto labelL10;
+ }
+ l3_1.g[0] = 0.;
+ for (i__ = 1; i__ <= 20; ++i__) {
+/* labelL11: */
+/* Computing 2nd power */
+ d__1 = l2_13.x[i__ - 1];
+ l3_1.g[0] += d__1 * d__1;
+ }
+ l3_1.g[0] += -1.;
+labelL10:
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto labelL12;
+ }
+ for (i__ = 1; i__ <= 20; ++i__) {
+/* labelL13: */
+ l5_13.gg[i__ - 1] = l2_13.x[i__ - 1] * 2.;
+ }
+labelL12:
+ return 0;
+} /* tp394_ */
+
+/* Subroutine */ int tp395_(int *mode)
+{
+ /* System generated locals */
+ Real d__1, d__2;
+
+ /* Local variables */
+ static int i__;
+
+ switch ((int)*mode) {
+ case 1: goto labelL1;
+ case 2: goto labelL2;
+ case 3: goto labelL3;
+ case 4: goto labelL4;
+ case 5: goto labelL5;
+ }
+labelL1:
+ l1_1.n = 50;
+ l1_1.nili = 0;
+ l1_1.ninl = 0;
+ l1_1.neli = 0;
+ l1_1.nenl = 1;
+ for (i__ = 1; i__ <= 50; ++i__) {
+ l2_15.x[i__ - 1] = 2.;
+ l12_15.lxu[i__ - 1] = false;
+/* labelL6: */
+ l11_15.lxl[i__ - 1] = false;
+ }
+ l20_16.lex = false;
+ l20_16.nex = 1;
+ l20_16.fex = 1.9166668;
+ l20_16.xex[0] = .91285206;
+ l20_16.xex[1] = .40829045;
+ l20_16.xex[2] = -6.4969989e-6;
+ l20_16.xex[3] = -9.9096716e-5;
+ l20_16.xex[4] = 1.189129e-4;
+ l20_16.xex[5] = -4.6486687e-5;
+ l20_16.xex[6] = 5.7605078e-5;
+ l20_16.xex[7] = -4.8016383e-5;
+ l20_16.xex[8] = 2.5691371e-5;
+ l20_16.xex[9] = 1.1670144e-5;
+ l20_16.xex[10] = -3.0881321e-5;
+ l20_16.xex[11] = 8.7202482e-6;
+ l20_16.xex[12] = 1.998037e-5;
+ l20_16.xex[13] = -1.2338706e-5;
+ l20_16.xex[14] = -1.6390153e-5;
+ l20_16.xex[15] = 7.3383634e-6;
+ l20_16.xex[16] = 1.686298e-5;
+ l20_16.xex[17] = 4.3922807e-6;
+ l20_16.xex[18] = -5.8623189e-6;
+ l20_16.xex[19] = -2.5188987e-6;
+ l20_16.xex[20] = 4.5980202e-6;
+ l20_16.xex[21] = 3.2507205e-6;
+ l20_16.xex[22] = -6.6596023e-6;
+ l20_16.xex[23] = -1.4419491e-5;
+ l20_16.xex[24] = -1.2164937e-5;
+ l20_16.xex[25] = -3.9129061e-6;
+ l20_16.xex[26] = 9.8985037e-7;
+ l20_16.xex[27] = 1.4776535e-7;
+ l20_16.xex[28] = -6.8312704e-7;
+ l20_16.xex[29] = 2.4242977e-6;
+ l20_16.xex[30] = 5.3892372e-6;
+ l20_16.xex[31] = 2.6662956e-6;
+ l20_16.xex[32] = -2.928209e-6;
+ l20_16.xex[33] = -3.8338271e-6;
+ l20_16.xex[34] = 6.1198364e-7;
+ l20_16.xex[35] = 4.367186e-6;
+ l20_16.xex[36] = 4.1104627e-6;
+ l20_16.xex[37] = 1.4549012e-6;
+ l20_16.xex[38] = -1.2562117e-6;
+ l20_16.xex[39] = -3.0092086e-6;
+ l20_16.xex[40] = -3.8620459e-6;
+ l20_16.xex[41] = -4.2627256e-6;
+ l20_16.xex[42] = -4.5080325e-6;
+ l20_16.xex[43] = -4.4852099e-6;
+ l20_16.xex[44] = -3.7953194e-6;
+ l20_16.xex[45] = -2.3440318e-6;
+ l20_16.xex[46] = -7.4816106e-7;
+ l20_16.xex[47] = -5.4626804e-8;
+ l20_16.xex[48] = -1.0972677e-6;
+ l20_16.xex[49] = -2.131277e-6;
+ return 0;
+labelL2:
+ l6_1.fx = 0.;
+ for (i__ = 1; i__ <= 50; ++i__) {
+/* L8: */
+/* Computing 2nd power */
+ d__1 = l2_15.x[i__ - 1];
+/* Computing 4th power */
+ d__2 = l2_15.x[i__ - 1], d__2 *= d__2;
+ l6_1.fx += (Real) i__ * (d__1 * d__1 + d__2 * d__2);
+ }
+ return 0;
+labelL3:
+ for (i__ = 1; i__ <= 50; ++i__) {
+/* Computing 3rd power */
+ d__1 = l2_15.x[i__ - 1];
+ l4_15.gf[i__ - 1] = (Real) i__ * (l2_15.x[i__ - 1] * 2. + d__1 *
+ (d__1 * d__1) * 4.);
+/* labelL9: */
+ l4_15.gf[i__ - 1] = l4_15.gf[i__ - 1];
+ }
+ return 0;
+labelL4:
+ if (! l9_2.index1[0]) {
+ goto labelL10;
+ }
+ l3_1.g[0] = 0.;
+ for (i__ = 1; i__ <= 50; ++i__) {
+/* labelL11: */
+/* Computing 2nd power */
+ d__1 = l2_15.x[i__ - 1];
+ l3_1.g[0] += d__1 * d__1;
+ }
+ l3_1.g[0] += -1.;
+labelL10:
+ return 0;
+labelL5:
+ if (! l10_2.index2[0]) {
+ goto labelL12;
+ }
+ for (i__ = 1; i__ <= 50; ++i__) {
+/* labelL13: */
+ l5_16.gg[i__ - 1] = l2_15.x[i__ - 1] * 2.;
+ }
+labelL12:
+ return 0;
+} /* tp395_ */
+
+
+Real gleich_(Real *p)
+{
+ /* System generated locals */
+ Real ret_val, d__1;
+
+ /* Local variables */
+ static Real a, f, y, eps;
+ static Real eps2;
+
+ eps = 1e-5;
+ y = *p + 1.;
+labelL2:
+ f = y - *p - std::atan(1. / y);
+ if (std::abs(f) <= eps) {
+ goto labelL1;
+ }
+ a = y * y + 1.;
+ a = (a + 1.) / a;
+ y -= f / a;
+ goto labelL2;
+labelL1:
+/* Computing 2nd power */
+ d__1 = eps;
+ eps2 = d__1 * d__1;
+ if (y > eps2) {
+ ret_val = y;
+ } else {
+ ret_val = eps2;
+ }
+ return ret_val;
+} /* gleich_ */
+
+
+/* Subroutine */ int mdnord_(Real *a, Real *b)
+{
+ Real norint_(Real *);
+
+ *b = norint_(a);
+ return 0;
+} /* mdnord_ */
+
+
+Real norint_(Real *x)
+{
+ /* Initialized data */
+
+ static Real p1[9] = { 3723.5079815548067,7113.6632469540499,
+ 6758.2169641104859,4032.2670108300497,1631.7602687537147,
+ 456.26145870609263,86.082762211948595,10.064858974909542,
+ .56418958676181361 };
+ static Real q1[10] = { 3723.5079815548065,11315.192081854405,
+ 15802.535999402043,13349.346561284457,7542.4795101934758,
+ 2968.0049014823087,817.62238630454408,153.07771075036222,
+ 17.839498439139557,1. };
+ static Real p2[6] = { 2.9788656263939929,7.4097406059647418,
+ 6.1602098531096305,5.0190497267842675,1.275366644729966,
+ .56418958354775507 };
+ static Real q2[7] = { 3.3690752069827528,9.6089653271927879,
+ 17.081440747466004,12.048951927855129,9.3960340162350542,
+ 2.260528520767327,1. };
+ static Real sqrt2 = 1.41421356237390505;
+ static Real rsqrtpi = .56418958354775629;
+
+ /* System generated locals */
+ Real ret_val, d__1;
+
+ /* Local variables */
+ static Real erfc, xabs, arg, erf, arg2;
+
+
+/* COMPUTES THE GAUSSIAN NORMAL DISTRIBUTION INTEGRAL */
+/* PRECISION ABOUT 16 DIGITS */
+
+ xabs = std::abs(*x);
+ if (xabs > .5) {
+ if (xabs > 8.) {
+ if (xabs > 100.) {
+ erfc = 0.;
+ } else {
+ arg = xabs / sqrt2;
+/* Computing 2nd power */
+ d__1 = arg;
+ erfc = (((((p2[5] * arg + p2[4]) * arg + p2[3]) * arg + p2[2])
+ * arg + p2[1]) * arg + p2[0]) / ((((((arg + q2[5]) *
+ arg + q2[4]) * arg + q2[3]) * arg + q2[2]) * arg + q2[
+ 1]) * arg + q2[0]) * std::exp(-(d__1 * d__1));
+ }
+ } else {
+ arg = xabs / sqrt2;
+/* Computing 2nd power */
+ d__1 = arg;
+ erfc = ((((((((p1[8] * arg + p1[7]) * arg + p1[6]) * arg + p1[5])
+ * arg + p1[4]) * arg + p1[3]) * arg + p1[2]) * arg + p1[1]
+ ) * arg + p1[0]) / (((((((((arg + q1[8]) * arg + q1[7]) *
+ arg + q1[6]) * arg + q1[5]) * arg + q1[4]) * arg + q1[3])
+ * arg + q1[2]) * arg + q1[1]) * arg + q1[0]) * std::exp(-(d__1
+ * d__1));
+ }
+ if (*x < 0.) {
+ ret_val = erfc * .5;
+ return ret_val;
+ } else {
+ ret_val = (2. - erfc) * .5;
+ return ret_val;
+ }
+ } else {
+ arg = xabs / sqrt2;
+/* Computing 2nd power */
+ d__1 = arg;
+ arg2 = d__1 * d__1;
+ erf = arg * 2. * rsqrtpi * ((((((((((arg2 / 210. -
+ .052631578947368418) * arg2 / 9. + .058823529411764705) *
+ arg2 / 8. - .066666666666666666) * arg2 / 7. +
+ .076923076923076927) * arg2 / 6. - .090909090909090912) *
+ arg2 / 5. + .1111111111111111) * arg2 / 4. -
+ .14285714285714285) * arg2 / 3. + .20000000000000001) * arg2 /
+ 2. - .33333333333333331) * arg2 + 1.);
+ if (*x >= 0.) {
+ ret_val = (erf + 1.) * .5;
+ return ret_val;
+ } else {
+ ret_val = (1. - erf) * .5;
+ return ret_val;
+ }
+ }
+ return ret_val;
+} /* norint_ */
diff --git a/SimTKmath/tests/adhoc/nlpqlp.h b/SimTKmath/tests/adhoc/nlpqlp.h
new file mode 100644
index 0000000..1fa7750
--- /dev/null
+++ b/SimTKmath/tests/adhoc/nlpqlp.h
@@ -0,0 +1,1317 @@
+using SimTK::Real;
+
+double pow_dd( Real *ap, Real *bp ){
+ return( pow(*ap, *bp)) ;
+}
+
+typedef union common1{
+ struct {
+ int n, nili, ninl, neli, nenl;
+ } _1;
+ struct {
+ int n, nili, niml, neli, nenl;
+ } _2;
+ struct {
+ int n, nili, neli, nenl;
+ } _3;
+} L1;
+
+#define l1_1 (l1_._1)
+#define l1_2 (l1_._2)
+#define l1_3 (l1_._3)
+
+typedef union common2 {
+ struct {
+ Real x[2];
+ } _1;
+ struct {
+ Real x[3];
+ } _2;
+ struct {
+ Real x[4];
+ } _3;
+ struct {
+ Real x[5];
+ } _4;
+ struct {
+ Real x[6];
+ } _5;
+ struct {
+ Real x[7];
+ } _6;
+ struct {
+ Real x[8];
+ } _7;
+ struct {
+ Real x[9];
+ } _8;
+ struct {
+ Real x[10];
+ } _9;
+ struct {
+ Real x[13];
+ } _10;
+ struct {
+ Real x[15];
+ } _11;
+ struct {
+ Real x[16];
+ } _12;
+ struct {
+ Real x[20];
+ } _13;
+ struct {
+ Real x[30];
+ } _14;
+ struct {
+ Real x[50];
+ } _15;
+ struct {
+ Real x[100];
+ } _16;
+ struct {
+ Real x[11];
+ } _17;
+ struct {
+ Real x[12];
+ } _18;
+ struct {
+ Real x[14];
+ } _19;
+ struct {
+ Real x[19];
+ } _20;
+ struct {
+ Real x[48];
+ } _21;
+} L2;
+
+#define l2_1 (l2_._1)
+#define l2_2 (l2_._2)
+#define l2_3 (l2_._3)
+#define l2_4 (l2_._4)
+#define l2_5 (l2_._5)
+#define l2_6 (l2_._6)
+#define l2_7 (l2_._7)
+#define l2_8 (l2_._8)
+#define l2_9 (l2_._9)
+#define l2_10 (l2_._10)
+#define l2_11 (l2_._11)
+#define l2_12 (l2_._12)
+#define l2_13 (l2_._13)
+#define l2_14 (l2_._14)
+#define l2_15 (l2_._15)
+#define l2_16 (l2_._16)
+#define l2_17 (l2_._17)
+#define l2_18 (l2_._18)
+#define l2_19 (l2_._19)
+#define l2_20 (l2_._20)
+#define l2_21 (l2_._21)
+
+typedef union common4{
+ struct {
+ Real gf[2];
+ } _1;
+ struct {
+ Real gf[3];
+ } _2;
+ struct {
+ Real gf[4];
+ } _3;
+ struct {
+ Real gf[5];
+ } _4;
+ struct {
+ Real gf[6];
+ } _5;
+ struct {
+ Real gf[7];
+ } _6;
+ struct {
+ Real gf[8];
+ } _7;
+ struct {
+ Real gf[9];
+ } _8;
+ struct {
+ Real gf[10];
+ } _9;
+ struct {
+ Real gf[13];
+ } _10;
+ struct {
+ Real gf[15];
+ } _11;
+ struct {
+ Real gf[16];
+ } _12;
+ struct {
+ Real gf[20];
+ } _13;
+ struct {
+ Real gf[30];
+ } _14;
+ struct {
+ Real gf[50];
+ } _15;
+ struct {
+ Real gf[100];
+ } _16;
+ struct {
+ Real gf[11];
+ } _17;
+ struct {
+ Real gf[12];
+ } _18;
+ struct {
+ Real gf[14];
+ } _19;
+} L4;
+
+#define l4_1 (l4_._1)
+#define l4_2 (l4_._2)
+#define l4_3 (l4_._3)
+#define l4_4 (l4_._4)
+#define l4_5 (l4_._5)
+#define l4_6 (l4_._6)
+#define l4_7 (l4_._7)
+#define l4_8 (l4_._8)
+#define l4_9 (l4_._9)
+#define l4_10 (l4_._10)
+#define l4_11 (l4_._11)
+#define l4_12 (l4_._12)
+#define l4_13 (l4_._13)
+#define l4_14 (l4_._14)
+#define l4_15 (l4_._15)
+#define l4_16 (l4_._16)
+#define l4_17 (l4_._17)
+#define l4_18 (l4_._18)
+#define l4_19 (l4_._19)
+
+typedef struct common6 {
+ Real fx;
+} L6;
+
+#define l6_1 l6_
+
+typedef union common9 {
+ struct {
+ int index1;
+ } _1;
+ struct {
+ bool index1[1];
+ } _2;
+ struct {
+ bool index1[2];
+ } _3;
+ struct {
+ bool index1[3];
+ } _4;
+ struct {
+ bool index1[5];
+ } _5;
+ struct {
+ bool index1[6];
+ } _6;
+ struct {
+ bool index1[4];
+ } _7;
+ struct {
+ bool index1[14];
+ } _8;
+ struct {
+ bool index1[38];
+ } _9;
+ struct {
+ bool index1[10];
+ } _10;
+ struct {
+ bool index1[13];
+ } _11;
+ struct {
+ bool index1[8];
+ } _12;
+ struct {
+ bool index1[11];
+ } _13;
+ struct {
+ bool index1[15];
+ } _14;
+ struct {
+ bool index1[29];
+ } _15;
+ struct {
+ bool index1[9];
+ } _16;
+ struct {
+ bool index1[12];
+ } _17;
+ struct {
+ bool index1[35];
+ } _18;
+ struct {
+ bool index1[45];
+ } _19;
+} L9;
+
+#define l9_1 (l9_._1)
+#define l9_2 (l9_._2)
+#define l9_3 (l9_._3)
+#define l9_4 (l9_._4)
+#define l9_5 (l9_._5)
+#define l9_6 (l9_._6)
+#define l9_7 (l9_._7)
+#define l9_8 (l9_._8)
+#define l9_9 (l9_._9)
+#define l9_10 (l9_._10)
+#define l9_11 (l9_._11)
+#define l9_12 (l9_._12)
+#define l9_13 (l9_._13)
+#define l9_14 (l9_._14)
+#define l9_15 (l9_._15)
+#define l9_16 (l9_._16)
+#define l9_17 (l9_._17)
+#define l9_18 (l9_._18)
+#define l9_19 (l9_._19)
+
+typedef union common10 {
+ struct {
+ int index2;
+ } _1;
+ struct {
+ bool index2[1];
+ } _2;
+ struct {
+ bool index2[2];
+ } _3;
+ struct {
+ bool index2[3];
+ } _4;
+ struct {
+ bool index2[5];
+ } _5;
+ struct {
+ bool index2[6];
+ } _6;
+ struct {
+ bool index2[4];
+ } _7;
+ struct {
+ bool index2[14];
+ } _8;
+ struct {
+ bool index2[38];
+ } _9;
+ struct {
+ bool index2[10];
+ } _10;
+ struct {
+ bool index2[13];
+ } _11;
+ struct {
+ bool index2[8];
+ } _12;
+ struct {
+ bool index2[11];
+ } _13;
+ struct {
+ bool index2[15];
+ } _14;
+ struct {
+ bool index2[29];
+ } _15;
+ struct {
+ bool index2[12];
+ } _16;
+ struct {
+ bool index2[35];
+ } _17;
+ struct {
+ bool index2[9];
+ } _18;
+} L10;
+
+#define l10_1 (l10_._1)
+#define l10_2 (l10_._2)
+#define l10_3 (l10_._3)
+#define l10_4 (l10_._4)
+#define l10_5 (l10_._5)
+#define l10_6 (l10_._6)
+#define l10_7 (l10_._7)
+#define l10_8 (l10_._8)
+#define l10_9 (l10_._9)
+#define l10_10 (l10_._10)
+#define l10_11 (l10_._11)
+#define l10_12 (l10_._12)
+#define l10_13 (l10_._13)
+#define l10_14 (l10_._14)
+#define l10_15 (l10_._15)
+#define l10_16 (l10_._16)
+#define l10_17 (l10_._17)
+#define l10_18 (l10_._18)
+
+typedef union common11 {
+ struct {
+ bool lxl[2];
+ } _1;
+ struct {
+ bool lxl[3];
+ } _2;
+ struct {
+ bool lxl[4];
+ } _3;
+ struct {
+ bool lxl[5];
+ } _4;
+ struct {
+ bool lxl[6];
+ } _5;
+ struct {
+ bool lxl[7];
+ } _6;
+ struct {
+ bool lxl[8];
+ } _7;
+ struct {
+ bool lxl[9];
+ } _8;
+ struct {
+ bool lxl[10];
+ } _9;
+ struct {
+ bool lxl[13];
+ } _10;
+ struct {
+ bool lxl[15];
+ } _11;
+ struct {
+ bool lxl[16];
+ } _12;
+ struct {
+ bool lxl[20];
+ } _13;
+ struct {
+ bool lxl[30];
+ } _14;
+ struct {
+ bool lxl[50];
+ } _15;
+ struct {
+ bool lxl[100];
+ } _16;
+ struct {
+ bool lxl[11];
+ } _17;
+ struct {
+ bool lxl[12];
+ } _18;
+ struct {
+ bool lxl[14];
+ } _19;
+ struct {
+ bool lxl[19];
+ } _20;
+ struct {
+ bool lxl[48];
+ } _21;
+} L11;
+
+#define l11_1 (l11_._1)
+#define l11_2 (l11_._2)
+#define l11_3 (l11_._3)
+#define l11_4 (l11_._4)
+#define l11_5 (l11_._5)
+#define l11_6 (l11_._6)
+#define l11_7 (l11_._7)
+#define l11_8 (l11_._8)
+#define l11_9 (l11_._9)
+#define l11_10 (l11_._10)
+#define l11_11 (l11_._11)
+#define l11_12 (l11_._12)
+#define l11_13 (l11_._13)
+#define l11_14 (l11_._14)
+#define l11_15 (l11_._15)
+#define l11_16 (l11_._16)
+#define l11_17 (l11_._17)
+#define l11_18 (l11_._18)
+#define l11_19 (l11_._19)
+#define l11_20 (l11_._20)
+#define l11_21 (l11_._21)
+
+typedef union common12{
+ struct {
+ bool lxu[2];
+ } _1;
+ struct {
+ bool lxu[3];
+ } _2;
+ struct {
+ bool lxu[4];
+ } _3;
+ struct {
+ bool lxu[5];
+ } _4;
+ struct {
+ bool lxu[6];
+ } _5;
+ struct {
+ bool lxu[7];
+ } _6;
+ struct {
+ bool lxu[8];
+ } _7;
+ struct {
+ bool lxu[9];
+ } _8;
+ struct {
+ bool lxu[10];
+ } _9;
+ struct {
+ bool lxu[13];
+ } _10;
+ struct {
+ bool lxu[15];
+ } _11;
+ struct {
+ bool lxu[16];
+ } _12;
+ struct {
+ bool lxu[20];
+ } _13;
+ struct {
+ bool lxu[30];
+ } _14;
+ struct {
+ bool lxu[50];
+ } _15;
+ struct {
+ bool lxu[100];
+ } _16;
+ struct {
+ bool lxu[11];
+ } _17;
+ struct {
+ bool lxu[12];
+ } _18;
+ struct {
+ bool lxu[14];
+ } _19;
+ struct {
+ bool lxu[19];
+ } _20;
+ struct {
+ bool lxu[48];
+ } _21;
+} L12;
+
+#define l12_1 (l12_._1)
+#define l12_2 (l12_._2)
+#define l12_3 (l12_._3)
+#define l12_4 (l12_._4)
+#define l12_5 (l12_._5)
+#define l12_6 (l12_._6)
+#define l12_7 (l12_._7)
+#define l12_8 (l12_._8)
+#define l12_9 (l12_._9)
+#define l12_10 (l12_._10)
+#define l12_11 (l12_._11)
+#define l12_12 (l12_._12)
+#define l12_13 (l12_._13)
+#define l12_14 (l12_._14)
+#define l12_15 (l12_._15)
+#define l12_16 (l12_._16)
+#define l12_17 (l12_._17)
+#define l12_18 (l12_._18)
+#define l12_19 (l12_._19)
+#define l12_20 (l12_._20)
+#define l12_21 (l12_._21)
+
+typedef union common13 {
+ struct {
+ Real xl[2];
+ } _1;
+ struct {
+ Real xl[3];
+ } _2;
+ struct {
+ Real xl[4];
+ } _3;
+ struct {
+ Real xl[5];
+ } _4;
+ struct {
+ Real xl[6];
+ } _5;
+ struct {
+ Real xl[7];
+ } _6;
+ struct {
+ Real xl[8];
+ } _7;
+ struct {
+ Real xl[9];
+ } _8;
+ struct {
+ Real xl[10];
+ } _9;
+ struct {
+ Real xl[13];
+ } _10;
+ struct {
+ Real xl[15];
+ } _11;
+ struct {
+ Real xl[16];
+ } _12;
+ struct {
+ Real xl[3];
+ } _13;
+ struct {
+ Real xl[4];
+ } _14;
+ struct {
+ Real xl;
+ } _15;
+ struct {
+ Real xl[20];
+ } _16;
+ struct {
+ Real xl[30];
+ } _17;
+ struct {
+ Real xl[50];
+ } _18;
+ struct {
+ Real xl[100];
+ } _19;
+ struct {
+ Real xl[2];
+ } _20;
+ struct {
+ Real xl[11];
+ } _21;
+ struct {
+ Real xl[12];
+ } _22;
+ struct {
+ Real xl[14];
+ } _23;
+ struct {
+ Real xl[19];
+ } _24;
+ struct {
+ Real xl[48];
+ } _25;
+} L13;
+
+#define l13_1 (l13_._1)
+#define l13_2 (l13_._2)
+#define l13_3 (l13_._3)
+#define l13_4 (l13_._4)
+#define l13_5 (l13_._5)
+#define l13_6 (l13_._6)
+#define l13_7 (l13_._7)
+#define l13_8 (l13_._8)
+#define l13_9 (l13_._9)
+#define l13_10 (l13_._10)
+#define l13_11 (l13_._11)
+#define l13_12 (l13_._12)
+#define l13_13 (l13_._13)
+#define l13_14 (l13_._14)
+#define l13_15 (l13_._15)
+#define l13_16 (l13_._16)
+#define l13_17 (l13_._17)
+#define l13_18 (l13_._18)
+#define l13_19 (l13_._19)
+#define l13_20 (l13_._20)
+#define l13_21 (l13_._21)
+#define l13_22 (l13_._22)
+#define l13_23 (l13_._23)
+#define l13_24 (l13_._24)
+#define l13_25 (l13_._25)
+
+typedef union common20 {
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[2];
+ } _1;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[8];
+ } _2;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[3];
+ } _3;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[6];
+ } _4;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[12];
+ } _5;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[4];
+ } _6;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[5];
+ } _7;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[7];
+ } _8;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[9];
+ } _9;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[10];
+ } _10;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[13];
+ } _11;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[15];
+ } _12;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[16];
+ } _13;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[20];
+ } _14;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[30];
+ } _15;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[50];
+ } _16;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[100];
+ } _17;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[11];
+ } _18;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[14];
+ } _19;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[19];
+ } _20;
+ struct {
+ bool lex;
+ int nex;
+ Real fex, xex[48];
+ } _21;
+} L20;
+
+#define l20_1 (l20_._1)
+#define l20_2 (l20_._2)
+#define l20_3 (l20_._3)
+#define l20_4 (l20_._4)
+#define l20_5 (l20_._5)
+#define l20_6 (l20_._6)
+#define l20_7 (l20_._7)
+#define l20_8 (l20_._8)
+#define l20_9 (l20_._9)
+#define l20_10 (l20_._10)
+#define l20_11 (l20_._11)
+#define l20_12 (l20_._12)
+#define l20_13 (l20_._13)
+#define l20_14 (l20_._14)
+#define l20_15 (l20_._15)
+#define l20_16 (l20_._16)
+#define l20_17 (l20_._17)
+#define l20_18 (l20_._18)
+#define l20_19 (l20_._19)
+#define l20_20 (l20_._20)
+#define l20_21 (l20_._21)
+
+typedef union common14 {
+ struct {
+ Real xu[2];
+ } _1;
+ struct {
+ Real xu[3];
+ } _2;
+ struct {
+ Real xu[4];
+ } _3;
+ struct {
+ Real xu[5];
+ } _4;
+ struct {
+ Real xu[6];
+ } _5;
+ struct {
+ Real xu[7];
+ } _6;
+ struct {
+ Real xu[8];
+ } _7;
+ struct {
+ Real xu[9];
+ } _8;
+ struct {
+ Real xu[10];
+ } _9;
+ struct {
+ Real xu[13];
+ } _10;
+ struct {
+ Real xu[15];
+ } _11;
+ struct {
+ Real xu[16];
+ } _12;
+ struct {
+ Real xu[3];
+ } _13;
+ struct {
+ Real xu[4];
+ } _14;
+ struct {
+ Real xu;
+ } _15;
+ struct {
+ Real xu[20];
+ } _16;
+ struct {
+ Real xu[30];
+ } _17;
+ struct {
+ Real xu[50];
+ } _18;
+ struct {
+ Real xu[100];
+ } _19;
+ struct {
+ Real xu[2];
+ } _20;
+ struct {
+ Real xu[11];
+ } _21;
+ struct {
+ Real xu[12];
+ } _22;
+ struct {
+ Real xu[14];
+ } _23;
+ struct {
+ Real xu[19];
+ } _24;
+ struct {
+ Real xu[48];
+ } _25;
+} L14;
+
+#define l14_1 (l14_._1)
+#define l14_2 (l14_._2)
+#define l14_3 (l14_._3)
+#define l14_4 (l14_._4)
+#define l14_5 (l14_._5)
+#define l14_6 (l14_._6)
+#define l14_7 (l14_._7)
+#define l14_8 (l14_._8)
+#define l14_9 (l14_._9)
+#define l14_10 (l14_._10)
+#define l14_11 (l14_._11)
+#define l14_12 (l14_._12)
+#define l14_13 (l14_._13)
+#define l14_14 (l14_._14)
+#define l14_15 (l14_._15)
+#define l14_16 (l14_._16)
+#define l14_17 (l14_._17)
+#define l14_18 (l14_._18)
+#define l14_19 (l14_._19)
+#define l14_20 (l14_._20)
+#define l14_21 (l14_._21)
+#define l14_22 (l14_._22)
+#define l14_23 (l14_._23)
+#define l14_24 (l14_._24)
+#define l14_25 (l14_._25)
+
+typedef union common3 {
+ struct {
+ Real g[1];
+ } _1;
+ struct {
+ Real g[2];
+ } _2;
+ struct {
+ Real g[3];
+ } _3;
+ struct {
+ Real g[5];
+ } _4;
+ struct {
+ Real g[6];
+ } _5;
+ struct {
+ Real g[4];
+ } _6;
+ struct {
+ Real g[14];
+ } _7;
+ struct {
+ Real g[38];
+ } _8;
+ struct {
+ Real g[10];
+ } _9;
+ struct {
+ Real g[13];
+ } _10;
+ struct {
+ Real g[8];
+ } _11;
+ struct {
+ Real g[11];
+ } _12;
+ struct {
+ Real g[15];
+ } _13;
+ struct {
+ Real g[29];
+ } _14;
+ struct {
+ Real g[9];
+ } _15;
+ struct {
+ Real g[12];
+ } _16;
+ struct {
+ Real g[35];
+ } _17;
+ struct {
+ Real g[45];
+ } _18;
+} L3;
+
+#define l3_1 (l3_._1)
+#define l3_2 (l3_._2)
+#define l3_3 (l3_._3)
+#define l3_4 (l3_._4)
+#define l3_5 (l3_._5)
+#define l3_6 (l3_._6)
+#define l3_7 (l3_._7)
+#define l3_8 (l3_._8)
+#define l3_9 (l3_._9)
+#define l3_10 (l3_._10)
+#define l3_11 (l3_._11)
+#define l3_12 (l3_._12)
+#define l3_13 (l3_._13)
+#define l3_14 (l3_._14)
+#define l3_15 (l3_._15)
+#define l3_16 (l3_._16)
+#define l3_17 (l3_._17)
+#define l3_18 (l3_._18)
+
+typedef union common5 {
+ struct {
+ Real gg[2] /* was [1][2] */;
+ } _1;
+ struct {
+ Real gg[4] /* was [2][2] */;
+ } _2;
+ struct {
+ Real gg[6] /* was [3][2] */;
+ } _3;
+ struct {
+ Real gg[10] /* was [5][2] */;
+ } _4;
+ struct {
+ Real gg[3] /* was [1][3] */;
+ } _5;
+ struct {
+ Real gg[8] /* was [2][4] */;
+ } _6;
+ struct {
+ Real gg[12] /* was [3][4] */;
+ } _7;
+ struct {
+ Real gg[24] /* was [6][4] */;
+ } _8;
+ struct {
+ Real gg[15] /* was [3][5] */;
+ } _9;
+ struct {
+ Real gg[36] /* was [6][6] */;
+ } _10;
+ struct {
+ Real gg[28] /* was [4][7] */;
+ } _11;
+ struct {
+ Real gg[42] /* was [14][3] */;
+ } _12;
+ struct {
+ Real gg[20] /* was [5][4] */;
+ } _13;
+ struct {
+ Real gg[30] /* was [6][5] */;
+ } _14;
+ struct {
+ Real gg[190] /* was [38][5] */;
+ } _15;
+ struct {
+ Real gg[50] /* was [10][5] */;
+ } _16;
+ struct {
+ Real gg[14] /* was [2][7] */;
+ } _17;
+ struct {
+ Real gg[48] /* was [6][8] */;
+ } _18;
+ struct {
+ Real gg[54] /* was [6][9] */;
+ } _19;
+ struct {
+ Real gg[117] /* was [13][9] */;
+ } _20;
+ struct {
+ Real gg[90] /* was [10][9] */;
+ } _21;
+ struct {
+ Real gg[80] /* was [8][10] */;
+ } _22;
+ struct {
+ Real gg[110] /* was [11][10] */;
+ } _23;
+ struct {
+ Real gg[195] /* was [15][13] */;
+ } _24;
+ struct {
+ Real gg[75] /* was [5][15] */;
+ } _25;
+ struct {
+ Real gg[435] /* was [29][15] */;
+ } _26;
+ struct {
+ Real gg[128] /* was [8][16] */;
+ } _27;
+ struct {
+ Real gg[16] /* was [4][4] */;
+ } _28;
+ struct {
+ Real gg[25] /* was [5][5] */;
+ } _29;
+ struct {
+ Real gg[5] /* was [1][5] */;
+ } _30;
+ struct {
+ Real gg[100] /* was [10][10] */;
+ } _31;
+ struct {
+ Real gg[150];
+ } _32;
+ struct {
+ Real gg[70] /* was [14][5] */;
+ } _33;
+ struct {
+ Real gg[35] /* was [5][7] */;
+ } _34;
+ struct {
+ Real gg[98] /* was [14][7] */;
+ } _35;
+ struct {
+ Real gg[108] /* was [12][9] */;
+ } _36;
+ struct {
+ Real gg[350] /* was [35][10] */;
+ } _37;
+ struct {
+ Real gg[52] /* was [4][13] */;
+ } _38;
+ struct {
+ Real gg[165] /* was [11][15] */;
+ } _39;
+ struct {
+ Real gg[225] /* was [15][15] */;
+ } _40;
+} L5;
+
+#define l5_1 (l5_._1)
+#define l5_2 (l5_._2)
+#define l5_3 (l5_._3)
+#define l5_4 (l5_._4)
+#define l5_5 (l5_._5)
+#define l5_6 (l5_._6)
+#define l5_7 (l5_._7)
+#define l5_8 (l5_._8)
+#define l5_9 (l5_._9)
+#define l5_10 (l5_._10)
+#define l5_11 (l5_._11)
+#define l5_12 (l5_._12)
+#define l5_13 (l5_._13)
+#define l5_14 (l5_._14)
+#define l5_15 (l5_._15)
+#define l5_16 (l5_._16)
+#define l5_17 (l5_._17)
+#define l5_18 (l5_._18)
+#define l5_19 (l5_._19)
+#define l5_20 (l5_._20)
+#define l5_21 (l5_._21)
+#define l5_22 (l5_._22)
+#define l5_23 (l5_._23)
+#define l5_24 (l5_._24)
+#define l5_25 (l5_._25)
+#define l5_26 (l5_._26)
+#define l5_27 (l5_._27)
+#define l5_28 (l5_._28)
+#define l5_29 (l5_._29)
+#define l5_30 (l5_._30)
+#define l5_31 (l5_._31)
+#define l5_32 (l5_._32)
+#define l5_33 (l5_._33)
+#define l5_34 (l5_._34)
+#define l5_35 (l5_._35)
+#define l5_36 (l5_._36)
+#define l5_37 (l5_._37)
+#define l5_38 (l5_._38)
+#define l5_39 (l5_._39)
+#define l5_40 (l5_._40)
+
+static struct {
+ int lsum;
+} l15_;
+
+#define l15_1 l15_
+
+static union {
+ struct {
+ Real f[2];
+ } _1;
+ struct {
+ Real f[3];
+ } _2;
+ struct {
+ Real f[5];
+ } _3;
+ struct {
+ Real f[10];
+ } _4;
+ struct {
+ Real f[4];
+ } _5;
+ struct {
+ Real f[7];
+ } _6;
+ struct {
+ Real f[11];
+ } _7;
+ struct {
+ Real f[6];
+ } _8;
+ struct {
+ Real f[13];
+ } _9;
+ struct {
+ Real f[20];
+ } _10;
+ struct {
+ Real f[1];
+ } _11;
+ struct {
+ Real f[198];
+ } _12;
+ struct {
+ Real f[44];
+ } _13;
+ struct {
+ Real f[8];
+ } _14;
+ struct {
+ Real f[15];
+ } _15;
+ struct {
+ Real f[40];
+ } _16;
+ struct {
+ Real f[33];
+ } _17;
+ struct {
+ Real f[31];
+ } _18;
+ struct {
+ Real f[65];
+ } _19;
+} l16_;
+
+#define l16_1 (l16_._1)
+#define l16_2 (l16_._2)
+#define l16_3 (l16_._3)
+#define l16_4 (l16_._4)
+#define l16_5 (l16_._5)
+#define l16_6 (l16_._6)
+#define l16_7 (l16_._7)
+#define l16_8 (l16_._8)
+#define l16_9 (l16_._9)
+#define l16_10 (l16_._10)
+#define l16_11 (l16_._11)
+#define l16_12 (l16_._12)
+#define l16_13 (l16_._13)
+#define l16_14 (l16_._14)
+#define l16_15 (l16_._15)
+#define l16_16 (l16_._16)
+#define l16_17 (l16_._17)
+#define l16_18 (l16_._18)
+#define l16_19 (l16_._19)
+
+static union {
+ struct {
+ Real df[4] /* was [2][2] */;
+ } _1;
+ struct {
+ Real df[6] /* was [3][2] */;
+ } _2;
+ struct {
+ Real df[15] /* was [5][3] */;
+ } _3;
+ struct {
+ Real df[30] /* was [10][3] */;
+ } _4;
+ struct {
+ Real df[12] /* was [4][3] */;
+ } _5;
+ struct {
+ Real df[9] /* was [3][3] */;
+ } _6;
+ struct {
+ Real df[28] /* was [7][4] */;
+ } _7;
+ struct {
+ Real df[20] /* was [5][4] */;
+ } _8;
+ struct {
+ Real df[50] /* was [10][5] */;
+ } _9;
+ struct {
+ Real df[55] /* was [11][5] */;
+ } _10;
+ struct {
+ Real df[36] /* was [6][6] */;
+ } _11;
+ struct {
+ Real df[78] /* was [13][6] */;
+ } _12;
+ struct {
+ Real df[110] /* was [11][10] */;
+ } _13;
+ struct {
+ Real df[19800] /* was [198][100] */;
+ } _14;
+ struct {
+ Real df[88] /* was [44][2] */;
+ } _15;
+ struct {
+ Real df[24] /* was [8][3] */;
+ } _16;
+ struct {
+ Real df[45] /* was [15][3] */;
+ } _17;
+ struct {
+ Real df[44] /* was [11][4] */;
+ } _18;
+ struct {
+ Real df[160] /* was [40][4] */;
+ } _19;
+ struct {
+ Real df[165] /* was [33][5] */;
+ } _20;
+ struct {
+ Real df[279] /* was [31][9] */;
+ } _21;
+ struct {
+ Real df[54] /* was [6][9] */;
+ } _22;
+ struct {
+ Real df[100] /* was [10][10] */;
+ } _23;
+ struct {
+ Real df[715] /* was [65][11] */;
+ } _24;
+} l17_;
+
+#define l17_1 (l17_._1)
+#define l17_2 (l17_._2)
+#define l17_3 (l17_._3)
+#define l17_4 (l17_._4)
+#define l17_5 (l17_._5)
+#define l17_6 (l17_._6)
+#define l17_7 (l17_._7)
+#define l17_8 (l17_._8)
+#define l17_9 (l17_._9)
+#define l17_10 (l17_._10)
+#define l17_11 (l17_._11)
+#define l17_12 (l17_._12)
+#define l17_13 (l17_._13)
+#define l17_14 (l17_._14)
+#define l17_15 (l17_._15)
+#define l17_16 (l17_._16)
+#define l17_17 (l17_._17)
+#define l17_18 (l17_._18)
+#define l17_19 (l17_._19)
+#define l17_20 (l17_._20)
+#define l17_21 (l17_._21)
+#define l17_22 (l17_._22)
+#define l17_23 (l17_._23)
+#define l17_24 (l17_._24)
+
+struct b_1_ {
+ Real xmu, rho, thick[100], w2, dthick[100];
+ int kkk;
+};
+struct b_2_ {
+ Real xmu, rho, thick[100], w, dthick[100];
+ int kkk;
+};
+
+#define b_1 (*(struct b_1_ *) &b_)
+#define b_2 (*(struct b_2_ *) &b_)
+
+static struct {
+ Real value2;
+} st_;
+
+#define st_1 st_
+
+static struct {
+ Real tmax;
+} tfn1_;
+
+#define tfn1_1 tfn1_
+
+
diff --git a/Simbody/CMakeLists.txt b/Simbody/CMakeLists.txt
new file mode 100644
index 0000000..128c3ec
--- /dev/null
+++ b/Simbody/CMakeLists.txt
@@ -0,0 +1,165 @@
+#---------------------------------------------------
+# Simbody
+#
+# Creates SimTK Core library, base name=SimTKsimbody.
+# Default libraries are shared & optimized. Variants
+# are created for static (_static) and debug (_d) and
+# provision is made for an optional "namespace" (ns)
+# and version number (vn).
+#
+# Windows:
+# [ns_]SimTKsimbody[_vn][_d].dll
+# [ns_]SimTKsimbody[_vn][_d].lib
+# [ns_]SimTKsimbody[_vn]_static[_d].lib
+# Unix:
+# lib[ns_]SimTKsimbody[_vn][_d].so
+# lib[ns_]SimTKsimbody[_vn]_static[_d].a
+#
+# All libraries are installed in
+# %ProgramFiles%\SimTK\lib[64] (Windows)
+# /usr/local/SimTK/lib[64] (UNIX)
+#
+#----------------------------------------------------
+
+PROJECT (SimTKsimbody)
+
+# SimTKsimbody depends on PlatformFiles, SimTKcommon, and SimTKmath only.
+INCLUDE_DIRECTORIES(${PLATFORM_INCLUDE_DIRECTORIES}
+ ${SimTKCOMMON_INCLUDE_DIRECTORIES}
+ ${SimTKMATH_INCLUDE_DIRECTORIES})
+
+# The source is organized into subdirectories, but we handle them all from
+# this CMakeLists file rather than letting CMake visit them as SUBDIRS.
+# The BUILD_VISUALIZER variable determines whether we actually build the
+# simbody-visualizer, but we still have to build the library-side support for
+# the Visualizer so that Simbody tests won't fail to build.
+SET(SIMBODY_SOURCE_SUBDIRS . Visualizer)
+
+ADD_DEFINITIONS(-DSimTK_SIMBODY_LIBRARY_NAME=${SimTKSIMBODY_LIBRARY_NAME}
+ -DSimTK_SIMBODY_MAJOR_VERSION=${SIMBODY_MAJOR_VERSION}
+ -DSimTK_SIMBODY_MINOR_VERSION=${SIMBODY_MINOR_VERSION}
+ -DSimTK_SIMBODY_PATCH_VERSION=${SIMBODY_PATCH_VERSION})
+
+
+SET(SHARED_TARGET ${SimTKSIMBODY_LIBRARY_NAME})
+SET(STATIC_TARGET ${SimTKSIMBODY_LIBRARY_NAME}_static)
+SET(SHARED_TARGET_VN ${SimTKSIMBODY_LIBRARY_NAME}${VN})
+SET(STATIC_TARGET_VN ${SimTKSIMBODY_LIBRARY_NAME}${VN}_static)
+
+# On Unix or Cygwin we have to add the suffix manually
+IF (UNIX AND CMAKE_BUILD_TYPE MATCHES Debug)
+ SET(SHARED_TARGET ${SHARED_TARGET}_d)
+ SET(STATIC_TARGET ${STATIC_TARGET}_d)
+ SET(SHARED_TARGET_VN ${SHARED_TARGET_VN}_d)
+ SET(STATIC_TARGET_VN ${STATIC_TARGET_VN}_d)
+ENDIF (UNIX AND CMAKE_BUILD_TYPE MATCHES Debug)
+
+## Test against the unversioned libraries if they are being built;
+## otherwise against the versioned libraries.
+IF(BUILD_UNVERSIONED_LIBRARIES)
+ SET(TEST_SHARED_TARGET ${SHARED_TARGET})
+ SET(TEST_STATIC_TARGET ${STATIC_TARGET})
+ELSE(BUILD_UNVERSIONED_LIBRARIES)
+ SET(TEST_SHARED_TARGET ${SHARED_TARGET_VN})
+ SET(TEST_STATIC_TARGET ${STATIC_TARGET_VN})
+ENDIF(BUILD_UNVERSIONED_LIBRARIES)
+
+# These are all the places to search for header files which are
+# to be part of the API.
+SET(API_INCLUDE_DIRS) # start empty
+SET(SimTKSIMBODY_INCLUDE_DIRS) # start empty
+FOREACH(subdir ${SIMBODY_SOURCE_SUBDIRS})
+ # append
+ SET(API_INCLUDE_DIRS ${API_INCLUDE_DIRS}
+ ${PROJECT_SOURCE_DIR}/${subdir}/include
+ ${PROJECT_SOURCE_DIR}/${subdir}/include/simbody
+ ${PROJECT_SOURCE_DIR}/${subdir}/include/simbody/internal)
+
+ # Referencing headers must always be done relative to this level.
+ SET(SimTKSIMBODY_INCLUDE_DIRS ${SimTKSIMBODY_INCLUDE_DIRS}
+ ${PROJECT_SOURCE_DIR}/${subdir}/include)
+ENDFOREACH(subdir)
+
+# Include the Simbody API include directories now so that Simbody code
+# can use them.
+INCLUDE_DIRECTORIES(${SimTKSIMBODY_INCLUDE_DIRS})
+
+# And pass API include directories up to the parent so subsequent libraries
+# can find the headers too.
+SET(SimTKSIMBODY_INCLUDE_DIRECTORIES ${SimTKMATH_INCLUDE_DIRS}
+ PARENT_SCOPE)
+
+
+# We'll need both *relative* path names, starting with their API_INCLUDE_DIRS,
+# and absolute pathnames.
+SET(API_REL_INCLUDE_FILES) # start these out empty
+SET(API_ABS_INCLUDE_FILES)
+
+FOREACH(dir ${API_INCLUDE_DIRS})
+ FILE(GLOB fullpaths ${dir}/*.h) # returns full pathnames
+ SET(API_ABS_INCLUDE_FILES ${API_ABS_INCLUDE_FILES} ${fullpaths})
+
+ FOREACH(pathname ${fullpaths})
+ GET_FILENAME_COMPONENT(filename ${pathname} NAME)
+ SET(API_REL_INCLUDE_FILES ${API_REL_INCLUDE_FILES} ${dir}/${filename})
+ ENDFOREACH(pathname)
+ENDFOREACH(dir)
+
+# collect up source files
+SET(SOURCE_FILES) # empty
+SET(SOURCE_INCLUDE_FILES)
+
+FOREACH(subdir ${SIMBODY_SOURCE_SUBDIRS})
+ FILE(GLOB src_files ${subdir}/src/*.cpp ${subdir}/src/*/*.cpp)
+ FILE(GLOB incl_files ${subdir}/src/*.h)
+ SET(SOURCE_FILES ${SOURCE_FILES} ${src_files}) #append
+ SET(SOURCE_INCLUDE_FILES ${SOURCE_INCLUDE_FILES} ${incl_files})
+
+ #INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/${subdir}/include)
+ENDFOREACH(subdir)
+
+#INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/src)
+
+# libraries and examples are installed from their subdirectories; headers here
+
+# install headers
+FILE(GLOB CORE_HEADERS include/*.h */include/*.h)
+FILE(GLOB TOP_HEADERS include/simbody/*.h */include/simbody/*.h)
+FILE(GLOB INTERNAL_HEADERS include/simbody/internal/*.h */include/simbody/internal/*.h)
+INSTALL_FILES(/${SIMBODY_INCLUDE_INSTALL_DIR}/ FILES ${CORE_HEADERS})
+INSTALL_FILES(/${SIMBODY_INCLUDE_INSTALL_DIR}/simbody/ FILES ${TOP_HEADERS})
+INSTALL_FILES(/${SIMBODY_INCLUDE_INSTALL_DIR}/simbody/internal FILES ${INTERNAL_HEADERS})
+
+# Notice that the (deprecated) INSTALL_FILES(dir ...) command works differently than the
+# newer (recommended) INSTALL(dir FILES ...) command
+# when the target directory starts with a slash
+FILE(GLOB SIMBODY_DOCS doc/*.pdf doc/*.txt)
+INSTALL(FILES ${SIMBODY_DOCS} DESTINATION ${CMAKE_INSTALL_DOCDIR})
+
+# SIMBODY_VISUALIZER_INSTALL_DIR defined in root CMakeLists.txt
+ADD_DEFINITIONS(-DSIMBODY_VISUALIZER_INSTALL_DIR="${SIMBODY_VISUALIZER_INSTALL_DIR}/")
+
+# These are at the end because we want them processed after
+# all the various variables have been set above.
+
+IF(BUILD_STATIC_LIBRARIES)
+ ADD_SUBDIRECTORY( staticTarget )
+ENDIF()
+ADD_SUBDIRECTORY( sharedTarget )
+
+IF (BUILD_VISUALIZER)
+ # NOTE: If you change GUI_NAME, you must also change the value of GuiAppName
+ # in VisualizerProtocol.cpp
+ # TODO even that is not sufficient.
+ SET(GUI_NAME "simbody-visualizer")
+ ADD_SUBDIRECTORY(Visualizer/simbody-visualizer)
+ENDIF (BUILD_VISUALIZER)
+
+IF( BUILD_EXAMPLES )
+ ADD_SUBDIRECTORY( examples )
+ENDIF( BUILD_EXAMPLES )
+
+IF( BUILD_TESTING )
+ ADD_SUBDIRECTORY( tests )
+ENDIF( BUILD_TESTING )
+
diff --git a/Simbody/SimbodyThingsToDoListAndTimeline.doc b/Simbody/SimbodyThingsToDoListAndTimeline.doc
new file mode 100644
index 0000000..92c2026
Binary files /dev/null and b/Simbody/SimbodyThingsToDoListAndTimeline.doc differ
diff --git a/Simbody/ThingsToDoList.doc b/Simbody/ThingsToDoList.doc
new file mode 100644
index 0000000..afc759a
Binary files /dev/null and b/Simbody/ThingsToDoList.doc differ
diff --git a/Simbody/Visualizer/include/simbody/internal/Visualizer.h b/Simbody/Visualizer/include/simbody/internal/Visualizer.h
new file mode 100644
index 0000000..c7f44c4
--- /dev/null
+++ b/Simbody/Visualizer/include/simbody/internal/Visualizer.h
@@ -0,0 +1,762 @@
+#ifndef SimTK_SIMBODY_VISUALIZER_H_
+#define SimTK_SIMBODY_VISUALIZER_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) 2010-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the Visualizer class used for collecting Simbody simulation results
+for display and interaction through the visualizer. **/
+
+#include "simbody/internal/common.h"
+
+#include <utility> // for std::pair
+
+namespace SimTK {
+
+class MultibodySystem;
+class DecorationGenerator;
+
+/** Provide simple visualization of and interaction with a Simbody simulation,
+with real time control of the frame rate.\ There are several operating modes
+available, including real time operation permitting responsive user interaction
+with the simulation.
+
+Frames are sent to the renderer at a regular interval that is selectable, with
+a default rate of 30 frames/second. The various operating modes provide
+different methods of controlling which simulation frames are selected and how
+they are synchronized for display.
+
+<h3>Visualization modes</h3>
+
+There are three operating modes for the Visualizer's display of simulation
+results, selectable via setMode():
+
+- <b>PassThrough</b>. This is the default mode. It sends through to the
+renderer \e every frame that is received from the simulation, slowing down the
+simulation if necessary so that the frames are presented at a selected frame
+rate. But note that the simulation time will not be synchronized to real time;
+because Simbody simulations generally proceed at a variable rate, the
+regularly-spaced output frames will represent different amounts of simulated
+time. If you want real time and simulation time synchronized, use the RealTime
+mode.
+
+- <b>Sampling</b>. This mode is useful for monitoring a simulation that is
+allowed to run at full speed. We send frames for display at a maximum rate
+given by the frame rate setting. After a frame is sent, all subsequent frames
+received from the simulation are ignored until the frame interval has passed;
+then the next received frame is displayed. This allows the simulation to
+proceed at the fastest rate possible but time will be irregular and not all
+frames generated by the simulation will be shown.
+
+- <b>RealTime</b>. Synchronize frame times with the simulated time, slowing
+down the simulation if it is running ahead of real time, as modifed by the
+time scale; see setRealTimeScale(). Frames are sent to the renderer at the
+selected frame rate. Smoothness is maintained by buffering up frames before
+sending them; interactivity is maintained by keeping the buffer length below
+human perception time (150-200ms). The presence and size of the buffer is
+selectable; see setDesiredBufferLengthInSec().
+
+<h3>User interaction</h3>
+
+The Simbody visualizer provides some user interaction of its own, for
+example allowing the user to control the viewpoint and display options. User
+inputs that it does not interpret locally are passed on to the simulation,
+and can be intercepted by registering InputListeners with the Visualizer. The
+Visualizer provides a class Visualizer::InputSilo which is an InputListener
+that simply captures and queues all user input, with the intent that a running
+simulation will occasionally stop to poll the InputSilo to process any input
+that has been collected.
+
+<h3>Implementation notes</h3>
+
+RealTime mode is worth some discussion. There is a simulation thread that
+produces frames at a variable rate, and a draw thread that consumes frames at a
+variable rate (by sending them to the renderer). We want to engineer things so
+that frames are sent to the renderer at a steady rate that is synchronized with
+simulation time (possibly after scaling). When a thread is running too fast,
+that is easily handled by blocking the speeding thread for a while. The "too
+slow" case takes careful handling.
+
+In normal operation, we expect the simulation to take varying amounts of
+real time to generate fixed amounts of simulation time, because we prefer
+to use variable time-step integrators that control errors by taking smaller
+steps in more difficult circumstances, and large steps through the easy
+parts of the simulation. For real time operation, the simulation must of
+course *average* real time performance; we use a frame buffer to smooth
+out variable delivery times. That is, frames go into the buffer at an
+irregular rate but are pulled off at a regular rate. A longer buffer can
+mask wider deviations in frame time, at the expense of interactive response.
+In most circumstances people cannot perceive delays below about 200ms, so
+for good response the total delay should be kept around that level.
+
+Despite the buffering, there will be occasions when the simulation can't
+keep up with real time. A common cause of that is that a user has paused
+either the simulation or the renderer, such as by hitting a breakpoint while
+debugging. In that case we deem the proper behavior to be that when we
+resume we should immediately resume real time behavior at a new start time,
+\e not attempt to catch up to the previous real time by running at high speed.
+As much as possible, we would like the simulation to behave just as it would
+have without the interruption, but with a long pause where interrupted. We
+deal with this situation by introducing a notion of "adjusted real time"
+(AdjRT). That is a clock that tracks the real time interval counter, but uses
+a variable base offset that is used to match it to the expected simulation
+time. When the simulation is long delayed, we modify the AdjRT base when we
+resume so that AdjRT once again matches the simulation time t. Adjustments
+to the AdjRT base occur at the time we deliver frames to the renderer; at that
+moment we compare the AdjRT reading to the frame's simulation time t and
+correct AdjRT for future frames.
+
+You can also run in RealTime mode without buffering. In that case frames are
+sent directly from the simulation thread to the renderer, but the above logic
+still applies. Simulation frames that arrive earlier than the corresponding
+AdjRT are delayed; frames that arrive later are drawn immediately but cause
+AdjRT to be readjusted to resynchronize. Overall performance can be better
+in unbuffered RealTime mode because the States provided by the simulation do
+not have to be copied before being drawn. However, intermittent slower-than-
+realtime frame times cannot be smoothed over; they will cause rendering delays.
+
+PassThrough and Sampling modes are much simpler because no synchronization
+is done to the simulation times. There is only a single thread and draw
+time scheduling works in real time without adjustment.
+
+With the above explanation, you may be able to figure out most of what comes
+out of the dumpStats() method which can be used to help diagnose performance
+problems. **/
+class SimTK_SIMBODY_EXPORT Visualizer {
+public:
+class FrameController; // defined below
+class InputListener; // defined in Visualizer_InputListener.h
+class InputSilo; // "
+class Reporter; // defined in Visualizer_Reporter.h
+
+
+/** Construct a new %Visualizer for the indicated System, and launch the
+visualizer display executable from a default or specified location. The camera's
+"up" direction will initially be set to match the "up" direction hint that is
+stored with the supplied \a system; the default is that "up" is in the
+direction of the positive Y axis. The background will normally include a
+ground plane and sky, but if the \a system has been set to request a uniform
+background we'll use a plain white background instead. You can override the
+chosen defaults using %Visualizer methods setSystemUpDirection() and
+setBackgroundType().
+
+Simbody is shipped with a separate executable program that provides the
+graphics display and collects user input. Normally that executable is
+installed in the "bin" subdirectory of the Simbody installation directory.
+However, first we look in the same directory as the currently-running
+executable and, if found, we will use that visualizer. If no visualizer
+is found with the executable, we check if environment variables SIMBODY_HOME
+or SimTK_INSTALL_DIR exist, and look in their "bin" subdirectories if so.
+Otherwise we'll look in defaultInstallDir/SimTK/bin where defaultInstallDir
+is the ProgramFiles registry entry on Windows, or /usr/local on other platforms.
+The other constructor allows specification of a search path that will be
+checked before attempting to find the installation directory.
+
+The SimTK::Pathname class is used to process the supplied search path, which
+can consist of absolute, working directory-relative, or executable
+directory-relative path names.
+
+ at see SimTK::Pathname **/
+explicit Visualizer(const MultibodySystem& system);
+
+/** Construct a new Visualizer for a given system, with a specified search
+path for locating the SimbodyVisualizer executable. The search path is
+checked \e after looking in the current executable directory, and \e before
+trying to locate the Simbody or SimTK installation directory. See the other
+constructor's documentation for more information. **/
+Visualizer(const MultibodySystem& system,
+ const Array_<String>& searchPath);
+
+/** Copy constructor has reference counted, shallow copy semantics;
+that is, the Visualizer copy is just another reference to the same
+Visualizer object. **/
+Visualizer(const Visualizer& src);
+/** Copy assignment has reference counted, shallow copy semantics;
+that is, the Visualizer copy is just another reference to the same
+Visualizer object. **/
+Visualizer& operator=(const Visualizer& src);
+/** InputListener, FrameController, and DecorationGenerator objects are
+destroyed here when the last reference is deleted. **/
+~Visualizer();
+
+/** Ask the visualizer to shut itself down immediately. This will cause the
+display window to close and the associated process to die. This method returns
+immediately but it may be some time later when the visualizer acts on the
+instruction; there is no way to wait for it to die. Normally the visualizer
+will persist even after the death of the simulator connection unless you have
+called setShutdownWhenDestructed() to make shutdown() get called automatically.
+ at see setShutdownWhenDestructed() **/
+void shutdown();
+
+/** Set the flag that determines whether we will automatically send a Shutdown
+message to the visualizer when this %Visualizer object is destructed.
+Normally we allow the GUI to persist even after death of the simulator
+connection, unless an explicit call to shutdown() is made.
+ at see getShutdownWhenDestructed(), shutdown() **/
+Visualizer& setShutdownWhenDestructed(bool shouldShutdown);
+
+/** Return the current setting of the "shutdown when destructed" flag. By
+default this is false.
+ at see setShutdownWhenDestructed(), shutdown() **/
+bool getShutdownWhenDestructed() const;
+
+/** These are the operating modes for the Visualizer, with PassThrough the
+default mode. See the documentation for the Visualizer class for more
+information about the modes. **/
+enum Mode {
+ /** Send through to the renderer every frame that is received from the
+ simulator (default mode). **/
+ PassThrough = 1,
+ /** Sample the results from the simulation at fixed real time intervals
+ given by the frame rate. **/
+ Sampling = 2,
+ /** Synchronize real frame display times with the simulated time. **/
+ RealTime = 3
+};
+
+/** These are the types of backgrounds the visualizer currently supports.
+You can choose what type to use programmatically, and users can override that
+choice in the GUI. Each of these types may use additional data (such as the
+background color) when the type is selected. **/
+enum BackgroundType {
+ /** Show a ground plane on which shadows may be cast, as well as a sky
+ in the far background. **/
+ GroundAndSky = 1,
+ /** Display a solid background color that has been provided elsewhere. **/
+ SolidColor = 2
+};
+
+/** The visualizer may predefine some menus; if you need to refer to one
+of those use its menu Id as defined here. Note that the id numbers here
+are negative numbers, which are not allowed for user-defined menu ids. **/
+enum PredefinedMenuIds {
+ /** The id of the predefined View pull-down. **/
+ ViewMenuId = -1
+};
+
+/** @name visualizer display options
+These methods provide programmatic control over some of the visualizer's
+display options. Typically these can be overridden by the user directly in
+the GUI, but these are useful for setting sensible defaults. In particular,
+the Ground and Sky background, which is the GUI default, is not appropriate
+for some systems (molecules for example). **/
+/**@{**/
+
+
+/** Change the background mode currently in effect in the GUI.\ By default
+we take the desired background type from the System, which will usually be
+at its default value which is to show a ground plane and sky. You can override
+that default choice with this method.
+ at param background The background type to use.
+ at return A reference to this Visualizer so that you can chain "set" calls.
+ at note Molmodel's CompoundSystem requests a solid background by default, since
+ground and sky is not the best way to display a molecule! **/
+Visualizer& setBackgroundType(BackgroundType background);
+
+
+/** Set the background color.\ This will be used when the solid background
+mode is in effect but has no effect otherwise. This is a const method so you
+can call it from within a FrameController, for example if you want to flash
+the background color.
+ at param color The background color in r,g,b format with [0,1] range.
+ at return A const reference to this Visualizer so that you can chain "set"
+calls, provided subsequent ones are also const. **/
+const Visualizer& setBackgroundColor(const Vec3& color) const;
+
+/** Control whether shadows are generated when the GroundAndSky background
+type is in effect.\ This has no effect if the ground plane is not being
+displayed. The default if for shadows to be displayed. This is a const method
+so you can call it from within a FrameController.
+ at param showShadows Set true to have shadows generated; false for none.
+ at see setBackgroundType()
+ at return A const reference to this Visualizer so that you can chain "set"
+calls, provided subsequent ones are also const. **/
+const Visualizer& setShowShadows(bool showShadows) const;
+
+/** Control whether frame rate is shown in the Visualizer.\ This is a const
+method so you can call it from within a FrameController.
+ at param showFrameRate Set true to show the frame rate; false for none.
+ at return A const reference to this Visualizer so that you can chain "set"
+calls, provided subsequent ones are also const. **/
+const Visualizer& setShowFrameRate(bool showFrameRate) const;
+
+/** Control whether simulation time is shown in the Visualizer.\ This is a const
+method so you can call it from within a FrameController.
+ at param showSimTime Set true to show the simulation time; false for none.
+ at return A const reference to this Visualizer so that you can chain "set"
+calls, provided subsequent ones are also const. **/
+const Visualizer& setShowSimTime(bool showSimTime) const;
+
+/** Control whether frame number is shown in the Visualizer.\ This is a const
+method so you can call it from within a FrameController.
+ at param showFrameNumber Set true to show the frame number; false for none.
+ at return A const reference to this Visualizer so that you can chain "set"
+calls, provided subsequent ones are also const. **/
+const Visualizer& setShowFrameNumber(bool showFrameNumber) const;
+
+/** Change the title on the main visualizer window.\ The default title
+is Simbody \e version : \e exename, where \e version is the current Simbody
+version number in major.minor.patch format and \e exename is the name of the
+executing simulation application's executable file (without suffix if any).
+ at param[in] title
+ The new window title. The amount of room for the title varies; keep
+ it short.
+ at return A const reference to this Visualizer so that you can chain "set"
+calls, provided subsequent ones are also const.
+ at see SimTK_version_simbody(), Pathname::getThisExecutablePath(),
+Pathname::desconstructPathname() **/
+const Visualizer& setWindowTitle(const String& title) const;
+/**@}**/
+
+/** @name Visualizer options
+These methods are used for setting a variety of options for the Visualizer's
+behavior, normally prior to sending it the first frame. **/
+/**@{**/
+
+/** Set the coordinate direction that should be considered the System's "up"
+direction.\ When the ground and sky background is in use, this is the
+direction that serves as the ground plane normal, and is used as the initial
+orientation for the camera's up direction (which is subsequently under user
+or program control and can point anywhere). If you don't set this explicitly
+here, the Visualizer takes the default up direction from the System, which
+provides a method allowing the System's creator to specify it, with the +Y
+axis being the default.
+ at param[in] upDirection
+ This must be one of the CoordinateAxis constants XAxis, YAxis, or ZAxis,
+ or one of the opposite directions -XAxis, -YAxis, or -ZAxis.
+ at return A writable reference to this Visualizer so that you can chain "set"
+calls in the manner of chained assignments. **/
+Visualizer& setSystemUpDirection(const CoordinateDirection& upDirection);
+/** Get the value the Visualizer is using as the System "up" direction (
+not to be confused with the camera "up" direction). **/
+CoordinateDirection getSystemUpDirection() const;
+
+/** Set the height at which the ground plane should be displayed when the
+GroundAndSky background type is in effect.\ This is interpreted along the
+system "up" direction that was specified in the Visualizer's System or was
+overridden with the setSystemUpDirection() method. The default value is zero,
+meaning that the ground plane passes through the ground origin.
+ at param height
+ The position of the ground plane along the system "up" direction that
+ serves as the ground plane normal. Note that \a height is \e along the
+ up direction, meaning that if up is one of the negative coordinate axis
+ directions a positive \a height will move the ground plane to a more
+ negative position.
+ at return A reference to this Visualizer so that you can chain "set" calls.
+ at see setSystemUpDirection(), setBackgroundType() **/
+Visualizer& setGroundHeight(Real height);
+/** Get the value the Visualizer considers to be the height of the ground
+plane for this System.\ The value must be interpreted along the System's "up"
+direction. @see setSystemUpDirection() **/
+Real getGroundHeight() const;
+
+
+/** Set the operating mode for the Visualizer. See \ref Visualizer::Mode for
+choices, and the discussion for the Visualizer class for meanings.
+ at param[in] mode The new Mode to use.
+ at return A reference to this Visualizer so that you can chain "set" calls. **/
+void setMode(Mode mode);
+/** Get the current mode being used by the Visualizer. See \ref Visualizer::Mode
+for the choices, and the discussion for the Visualizer class for meanings. **/
+Mode getMode() const;
+
+/** Set the frame rate in frames/sec (of real time) that you want the
+Visualizer to attempt to achieve. This affects all modes. The default is 30
+frames per second. Set the frame rate to zero to return to the default
+behavior.
+ at param[in] framesPerSec
+ The desired frame rate; specify as zero to get the default.
+ at return A reference to this Visualizer so that you can chain "set" calls.
+**/
+Visualizer& setDesiredFrameRate(Real framesPerSec);
+/** Get the current value of the frame rate the Visualizer has been asked to
+attempt; this is not necessarily the rate actually achieved. A return value of
+zero means the Visualizer is using its default frame rate, which may be
+dependent on the current operating mode.
+ at see setDesiredFrameRate() for more information. **/
+Real getDesiredFrameRate() const;
+
+/** In RealTime mode we normally assume that one unit of simulated time should
+map to one second of real time; however, in some cases the time units are not
+seconds, and in others you may want to run at some multiple or fraction of
+real time. Here you can say how much simulated time should equal one second of
+real time. For example, if your simulation runs in seconds, but you want to
+run twice as fast as real time, then call setRealTimeScale(2.0), meaning that
+two simulated seconds will pass for every one real second. This call will have
+no immediate effect if you are not in RealTime mode, but the value will be
+remembered.
+
+ at param[in] simTimePerRealSecond
+The number of units of simulation time that should be displayed in one second
+of real time. Zero or negative value will be interpeted as the default ratio
+of 1:1.
+ at return A reference to this Visualizer so that you can chain "set" calls.
+**/
+Visualizer& setRealTimeScale(Real simTimePerRealSecond);
+/** Return the current time scale, which will be 1 by default.
+ at see setRealTimeScale() for more information. **/
+Real getRealTimeScale() const;
+
+/** When running an interactive realtime simulation, you can smooth out changes
+in simulation run rate by buffering frames before sending them on for
+rendering. The length of the buffer introduces an intentional response time
+lag from when a user reacts to when he can see a response from the simulator.
+Under most circumstances a lag of 150-200ms is undetectable. The default
+buffer length is the time represented by the number of whole frames
+that comes closest to 150ms; 9 frames at 60fps, 5 at 30fps, 4 at 24fps, etc.
+To avoid frequent block/unblocking of the simulation thread, the buffer is
+not kept completely full; you can use dumpStats() if you want to see how the
+buffer was used during a simulation. Shorten the buffer to improve
+responsiveness at the possible expense of smoothness. Note that the total lag
+time includes not only the buffer length here, but also lag induced by the
+time stepper taking steps that are larger than the frame times. For maximum
+responsiveness you should keep the integrator step sizes limited to about
+100ms, or reduce the buffer length so that worst-case lag doesn't go much over
+200ms.
+ at param[in] bufferLengthInSec
+This is the target time length for the buffer. The actual length is the nearest
+integer number of frames whose frame times add up closest to the request. If
+you ask for a non-zero value, you will always get at least one frame in the
+buffer. If you ask for zero, you'll get no buffering at all. To restore the
+buffer length to its default value, pass in a negative number.
+ at return A reference to this Visualizer so that you can chain "set" calls. **/
+Visualizer& setDesiredBufferLengthInSec(Real bufferLengthInSec);
+/** Get the current value of the desired buffer time length the Visualizer
+has been asked to use for smoothing the frame rate, or the default value
+if none has been requested. The actual value will differ from this number
+because the buffer must contain an integer number of frames.
+ at see getActualBufferTime() to see the frame-rounded buffer length **/
+Real getDesiredBufferLengthInSec() const;
+/** Get the actual length of the real time frame buffer in seconds, which
+may differ from the requested time because the buffer contains an integer
+number of frames. **/
+Real getActualBufferLengthInSec() const;
+/** Get the actual length of the real time frame buffer in number of frames. **/
+int getActualBufferLengthInFrames() const;
+
+/** Add a new input listener to this Visualizer, methods of which will be
+called when the GUI detects user-driven events like key presses, menu picks,
+and slider or mouse moves. See Visualizer::InputListener for more
+information. The Visualizer takes over ownership of the supplied \a listener
+object and deletes it upon destruction of the Visualizer; don't delete it
+yourself.
+ at return An integer index you can use to find this input listener again. **/
+int addInputListener(InputListener* listener);
+/** Return the count of input listeners added with addInputListener(). **/
+int getNumInputListeners() const;
+/** Return a const reference to the i'th input listener. **/
+const InputListener& getInputListener(int i) const;
+/** Return a writable reference to the i'th input listener. **/
+InputListener& updInputListener(int i);
+
+/** Add a new frame controller to this Visualizer, methods of which will be
+called just prior to rendering a frame for the purpose of simulation-controlled
+camera positioning and other frame-specific effects.
+See Visualizer::FrameController for more information. The Visualizer takes
+over ownership of the supplied \a controller object and deletes it upon
+destruction of the Visualizer; don't delete it yourself.
+ at return An integer index you can use to find this frame controller again. **/
+int addFrameController(FrameController* controller);
+/** Return the count of frame controllers added with addFrameController(). **/
+int getNumFrameControllers() const;
+/** Return a const reference to the i'th frame controller. **/
+const FrameController& getFrameController(int i) const;
+/** Return a writable reference to the i'th frame controller. **/
+FrameController& updFrameController(int i);
+
+/**@}**/
+
+
+/** @name Frame drawing methods
+These are used to report simulation frames to the Visualizer. Typically
+the report() method will be called from a Reporter invoked by a TimeStepper,
+but it can also be useful to invoke directly to show preliminary steps in a
+simulation, to replay saved States later, and to display frames when using
+an Integrator directly rather than through a TimeStepper.
+
+How frames are handled after they have been reported depends on the specific
+method called, and on the Visualizer's current Mode. **/
+
+/**@{**/
+/** Report that a new simulation frame is available for rendering. Depending
+on the current Visualizer::Mode, handling of the frame will vary:
+
+ at par PassThrough
+All frames will be rendered, but the calling thread (that is, the simulation)
+may be blocked if the next frame time has not yet been reached or if the
+renderer is unable to keep up with the rate at which frames are being supplied
+by the simulation.
+
+ at par Sampling
+The frame will be rendered immediately if the next sample time has been reached
+or passed, otherwise the frame will be ignored and report() will return
+immediately.
+
+ at par RealTime
+Frames are queued to smooth out the time stepper's variable time steps. The
+calling thread may be blocked if the buffer is full, or if the simulation time
+is too far ahead of real time. Frames will be dropped if they come too
+frequently; only the ones whose simulated times are at or near a frame time
+will be rendered. Frames that come too late will be queued for rendering as
+soon as possible, and also reset the expected times for subsequent frames so
+that real time operation is restored. **/
+void report(const State& state) const;
+
+/** In RealTime mode there will typically be frames still in the buffer at
+the end of a simulation.\ This allows you to wait while the buffer empties.
+When this returns, all frames that had been supplied via report() will have
+been sent to the renderer and the buffer will be empty. Returns immediately
+if not in RealTime mode, if there is no buffer, or if the buffer is already
+empty. **/
+void flushFrames() const;
+
+/** This method draws a frame unconditionally without queuing or checking
+the frame rate. Typically you should use the report() method instead, and
+let the the internal queuing and timing system decide when to call
+drawFrameNow(). **/
+void drawFrameNow(const State& state) const;
+/**@}**/
+
+
+/** @name Scene-building methods
+These methods are used to add permanent elements to the scene being displayed
+by the Visualizer. Once added, these elements will contribute to every frame.
+Calling one of these methods requires writable (non-const) access to the
+Visualizer object; you can't call them from within a FrameController object.
+Note that adding DecorationGenerators does allow different
+geometry to be produced for each frame; however, once added a
+DecorationGenerator will be called for \e every frame generated. **/
+/**@{**/
+
+/** Add a new pull-down menu to the visualizer's display. A label
+for the pull-down button is provided along with an integer identifying the
+particular menu. A list of (string,int) pairs defines the menu and submenu
+item labels and associated item numbers. The item numbers must be unique
+across the entire menu and all its submenus. The strings have a pathname-like
+syntax, like "submenu/item1", "submenu/item2", "submenu/lowermenu/item1", etc.
+that is used to define the pulldown menu layout.
+ at param title the title to display on the menu's pulldown button
+ at param id an integer value >= 0 that uniquely identifies this menu
+ at param items item names, possibly with submenus as specified above, with
+ associated item numbers
+When a user picks an item on a menu displayed in the visualizer, that
+selection is delievered to the simulation application via an InputListener
+associated with this Visualizer. The selection will be identified by
+(\a id, itemNumber) pair.
+ at return A reference to this Visualizer so that you can chain "add" and
+"set" calls. **/
+Visualizer& addMenu(const String& title, int id,
+ const Array_<std::pair<String, int> >& items);
+
+/** Add a new slider to the visualizer's display.
+ at param title the title to display next to the slider
+ at param id an integer value that uniquely identifies this slider
+ at param min the minimum value the slider can have
+ at param max the maximum value the slider can have
+ at param value the initial value of the slider, which must be between
+ min and max
+When a user moves a slider displayed in the visualizer, the new value
+is delievered to the simulation application via an InputListener associated
+with this Visualizer. The slider will be identified by the \a id supplied
+here.
+ at return A reference to this Visualizer so that you can chain "add" and
+"set" calls. **/
+Visualizer& addSlider(const String& title, int id, Real min, Real max, Real value);
+
+/** Add an always-present, body-fixed piece of geometry like the one passed in,
+but attached to the indicated body. The supplied transform is applied on top of
+whatever transform is already contained in the supplied geometry, and any body
+index stored with the geometry is ignored.
+ at return An integer index you can use to find this decoration again. **/
+int addDecoration(MobilizedBodyIndex mobodIx, const Transform& X_BD,
+ const DecorativeGeometry& geometry);
+/** Return the count of decorations added with addDecoration(). **/
+int getNumDecorations() const;
+/** Return a const reference to the i'th decoration. **/
+const DecorativeGeometry& getDecoration(int i) const;
+/** Return a writable reference to the i'th decoration. This is allowed for
+a const %Visualizer since it is just a decoration. **/
+DecorativeGeometry& updDecoration(int i) const;
+
+/** Add an always-present rubber band line, modeled after the DecorativeLine
+supplied here. The end points of the supplied line are ignored, however: at
+run time the spatial locations of the two supplied stations are calculated and
+used as end points.
+ at return An integer index you can use to find this rubber band line again. **/
+int addRubberBandLine(MobilizedBodyIndex b1, const Vec3& station1,
+ MobilizedBodyIndex b2, const Vec3& station2,
+ const DecorativeLine& line);
+/** Return the count of rubber band lines added with addRubberBandLine(). **/
+int getNumRubberBandLines() const;
+/** Return a const reference to the i'th rubber band line. **/
+const DecorativeLine& getRubberBandLine(int i) const;
+/** Return a writable reference to the i'th rubber band line. This is allowed
+for a const %Visualizer since it is just a decoration. **/
+DecorativeLine& updRubberBandLine(int i) const;
+
+/** Add a DecorationGenerator that will be invoked to add dynamically generated
+geometry to each frame of the the scene. The Visualizer assumes ownership of the
+object passed to this method, and will delete it when the Visualizer is
+deleted.
+ at return An integer index you can use to find this decoration generator
+again. **/
+int addDecorationGenerator(DecorationGenerator* generator);
+/** Return the count of decoration generators added with
+addDecorationGenerator(). **/
+int getNumDecorationGenerators() const;
+/** Return a const reference to the i'th decoration generator. **/
+const DecorationGenerator& getDecorationGenerator(int i) const;
+/** Return a writable reference to the i'th decoration generator. **/
+DecorationGenerator& updDecorationGenerator(int i);
+/**@}**/
+
+/** @name Frame control methods
+These methods can be called prior to rendering a frame to control how the
+camera is positioned for that frame. These can be invoked from within a
+FrameController object for runtime camera control and other effects. **/
+/**@{**/
+
+/** Set the transform defining the position and orientation of the camera.
+
+ at param[in] X_GC This is the transform giving the pose of the camera's
+ frame C in the ground frame G; see below for a precise
+ description.
+
+Our camera uses a right-handed frame with origin at the image location,
+with axes oriented as follows: the x axis is to the right, the y axis is the
+"up" direction, and the z axis is the "back" direction; that is, the camera is
+looking in the -z direction. If your simulation coordinate system is different,
+such as the common "virtual world" system where ground is the x-y plane
+(x right and y "in") and z is up, be careful to account for that when
+positioning the camera.
+
+For example, in the virtual world coordinate system, setting \a X_GC to
+identity would put the camera at the ground origin with the x axis as expected,
+but the camera would be looking down (your -z) with the camera's "up" direction
+aligned with your y. In this case to make the camera look in the y direction
+with up in z, you would need to rotate it +90 degrees about the x axis:
+ at code
+Visualizer viz;
+// ...
+
+// Point camera along Ground's y axis with z up, by rotating the camera
+// frame's z axis to align with Ground's -y.
+viz.setCameraTransform(Rotation(Pi/2, XAxis));
+ at endcode **/
+const Visualizer& setCameraTransform(const Transform& X_GC) const;
+
+/** Move the camera forward or backward so that all geometry in the scene is
+visible. **/
+const Visualizer& zoomCameraToShowAllGeometry() const;
+
+/** Rotate the camera so that it looks at a specified point.
+ at param point the point to look at
+ at param upDirection a direction which should point upward as seen by the camera
+**/
+const Visualizer& pointCameraAt(const Vec3& point, const Vec3& upDirection) const;
+
+/** Set the camera's vertical field of view, measured in radians. **/
+const Visualizer& setCameraFieldOfView(Real fov) const;
+
+/** Set the distance from the camera to the near and far clipping planes. **/
+const Visualizer& setCameraClippingPlanes(Real nearPlane, Real farPlane) const;
+
+/** Change the value currently shown on one of the sliders.
+ at param slider the id given to the slider when created
+ at param value a new value for the slider; if out of range it will
+ be at one of the extremes **/
+const Visualizer& setSliderValue(int slider, Real value) const;
+
+/** Change the allowed range for one of the sliders.
+ at param slider the id given to the slider when created
+ at param newMin the new lower limit on the slider range, <= newMax
+ at param newMax the new upper limit on the slider range, >= newMin
+The slider's current value remains unchanged if it still fits in the
+new range, otherwise it is moved to the nearest limit. **/
+const Visualizer& setSliderRange(int slider, Real newMin, Real newMax) const;
+
+
+/**@}**/
+
+/** @name Methods for debugging and statistics **/
+/**@{**/
+/** Dump statistics to the given ostream (for example, std::cout). **/
+void dumpStats(std::ostream& o) const;
+/** Reset all statistics to zero. **/
+void clearStats();
+/**@}**/
+
+/** @name Internal use only **/
+/**@{**/
+const Array_<InputListener*>& getInputListeners() const;
+const Array_<FrameController*>& getFrameControllers() const;
+const MultibodySystem& getSystem() const;
+int getRefCount() const;
+/**@}**/
+
+class Impl;
+//--------------------------------------------------------------------------
+ private:
+explicit Visualizer(Impl* impl);
+Impl* impl;
+
+const Impl& getImpl() const {assert(impl); return *impl;}
+Impl& updImpl() {assert(impl); return *impl;}
+friend class Impl;
+};
+
+/** This abstract class represents an object that will be invoked by the
+Visualizer just prior to rendering each frame. You can use this to call any
+of the const (runtime) methods of the Visualizer, typically to control the
+camera, and you can also add some geometry to the scene, print messages to
+the console, and so on. **/
+class SimTK_SIMBODY_EXPORT Visualizer::FrameController {
+public:
+ /** The Visualizer is just about to generate and render a frame
+ corresponding to the given State.
+ @param[in] viz
+ The Visualizer that is doing the rendering.
+ @param[in] state
+ The State that is being used to generate the frame about to be
+ rendered by \a viz.
+ @param[in,out] geometry
+ DecorativeGeometry being accumulated for rendering in this frame;
+ be sure to \e append if you have anything to add.
+ **/
+ virtual void generateControls(const Visualizer& viz,
+ const State& state,
+ Array_<DecorativeGeometry>& geometry) = 0;
+
+ /** Destructor is virtual; be sure to override it if you have something
+ to clean up at the end. **/
+ virtual ~FrameController() {}
+};
+
+/** OBSOLETE: This provides limited backwards compatibility with the old
+VTK Visualizer that is no longer supported.\ Switch to Visualizer instead. **/
+typedef Visualizer VTKVisualizer;
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_VISUALIZER_H_
diff --git a/Simbody/Visualizer/include/simbody/internal/Visualizer_InputListener.h b/Simbody/Visualizer/include/simbody/internal/Visualizer_InputListener.h
new file mode 100644
index 0000000..1dc6c28
--- /dev/null
+++ b/Simbody/Visualizer/include/simbody/internal/Visualizer_InputListener.h
@@ -0,0 +1,335 @@
+#ifndef SimTK_SIMBODY_VISUALIZER_INPUT_LISTENER_H_
+#define SimTK_SIMBODY_VISUALIZER_INPUT_LISTENER_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) 2010-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+This defines the InputListener class that is internal to the Visualizer, and
+the InputSilo class derived from it. **/
+
+#include "simbody/internal/common.h"
+#include "simbody/internal/Visualizer.h"
+
+namespace SimTK {
+
+//==============================================================================
+// INPUT LISTENER
+//==============================================================================
+/** This abstract class defines methods to be called when the Visualizer
+reports user activity back to the simulation process.\ Derive a concrete event
+listener whose methods take appropriate actions when event of interest occur.
+
+You only need to implement the methods you care about. The Visualizer provides
+an InputListener called InputSilo that is particularly useful for dealing with
+user input that is intended to affect the progress of a running simulation.
+ at see Visualizer::InputSilo **/
+class SimTK_SIMBODY_EXPORT Visualizer::InputListener {
+public:
+/** These represent modifications to the character that is passed into the
+keyPressed() method, including whether any of Shift/Control/Alt were down and
+whether a special non-ASCII code is being supplied, such as is required for an
+arrow key. These values are or'ed together to form the second argument of the
+keyPressed() method. **/
+enum Modifier {
+ ShiftIsDown = 0x01, ///< Shift (left or right)
+ ControlIsDown = 0x02, ///< Ctrl (left or right)
+ AltIsDown = 0x04, ///< Alt (left or right)
+ IsSpecialKey = 0xC0 ///< Special non-ASCII keycode being used
+};
+
+static const unsigned SpecialKeyOffset = 0x100; // Added to each code
+
+/** These are the special keys that the Visualizer may report via the
+keyPressed() method. All other keys are considered "ordinary". **/
+enum KeyCode {
+ KeyControlC = 3, // some notable ASCII codes
+ KeyBeep = 7,
+ KeyBackspace = 8,
+ KeyTab = 9,
+ KeyLF = 10,
+ KeyReturn = 13,
+ KeyEnter = KeyReturn,
+ KeyEsc = 27,
+ KeyDelete = 127,
+
+ KeyF1 = SpecialKeyOffset + 1, // function keys
+ KeyF2 = SpecialKeyOffset + 2,
+ KeyF3 = SpecialKeyOffset + 3,
+ KeyF4 = SpecialKeyOffset + 4,
+ KeyF5 = SpecialKeyOffset + 5,
+ KeyF6 = SpecialKeyOffset + 6,
+ KeyF7 = SpecialKeyOffset + 7,
+ KeyF8 = SpecialKeyOffset + 8,
+ KeyF9 = SpecialKeyOffset + 9,
+ KeyF10 = SpecialKeyOffset + 10,
+ KeyF11 = SpecialKeyOffset + 11,
+ KeyF12 = SpecialKeyOffset + 12,
+
+ KeyLeftArrow = SpecialKeyOffset + 100, // directional keys
+ KeyUpArrow = SpecialKeyOffset + 101,
+ KeyRightArrow = SpecialKeyOffset + 102,
+ KeyDownArrow = SpecialKeyOffset + 103,
+ KeyPageUp = SpecialKeyOffset + 104,
+ KeyPageDown = SpecialKeyOffset + 105,
+ KeyHome = SpecialKeyOffset + 106,
+ KeyEnd = SpecialKeyOffset + 107,
+ KeyInsert = SpecialKeyOffset + 108
+};
+
+/** Destructor is virtual; be sure to override it if you need to clean up. **/
+virtual ~InputListener() {}
+
+/** This method is called when a user hits a keyboard key in the Visualizer
+window, unless that key is being intercepted by the Visualizer for its own
+purposes. Ordinary ASCII characters 0-127 are represented by their own values;
+special keys like arrows and function keys are mapped to unique values > 255.
+You can check whether \a modifers & IsSpecialKey is true if you care; otherwise
+just mix the ordinary and special codes in a case statement. You can tell if
+any or all of Shift/Control/Alt were depressed when the key was hit by checking
+the \a modifiers. Note that for an ordinary capital letter you'll get the ASCII
+code for the capital as well as an indication that the Shift key was down. If
+caps lock was down you'll get the capital letter but no Shift modifier.
+ at param[in] key The ASCII character or special key code.
+ at param[in] modifiers Whether Shift,Ctrl,Alt are down or key is special.
+ at return Return \c true if you have handled this key press and don't want any
+subsequent listeners called. **/
+virtual bool keyPressed(unsigned key, unsigned modifiers) {return false;}
+
+/** The user has clicked one of the menu items you defined; here is
+the integer value you specified when you defined it.
+ at param[in] menu The id number of the menu in use.
+ at param[in] item The menu item number that the user selected.
+ at return Return \c true if you have handled this menu click and don't
+want any subsequent listeners called. **/
+virtual bool menuSelected(int menu, int item) {return false;}
+
+/** The user has moved one of the sliders you defined; here is the integer
+value you specified when you defined it, and the new value of the slider.
+ at param[in] slider The id number of the slider that moved.
+ at param[in] value The new value represented by this slider position.
+ at return Return \c true if you have handled this move and don't want any
+subsequent listeners called. **/
+virtual bool sliderMoved(int slider, Real value) {return false;}
+};
+
+
+
+//==============================================================================
+// INPUT SILO
+//==============================================================================
+/** This pre-built InputListener is extremely useful for processing user
+input that is intended to affect a running simulation. The idea is that this
+object saves up all the user input in a set of "silos", which are
+first-in-first-out (FIFO) queues. The simulation periodically checks ("polls")
+to see if there is anything in the silos that needs processing, pulling off one
+user input at a time until they have all been consumed. This eliminates any
+need for tricky asynchronous handling of user input, and all thread
+synchronization issues are handled invisibly.
+
+You can also request to wait quietly until some input arrives, which is useful
+when you can't proceed without some instruction from the user that you expect
+to get through the visualizer.
+
+When the InputSilo receives user input through one of the InputListener methods
+it implements, it return \c true indicating that it has processed the input and
+that no further InputListeners should be called. So if you have other
+InputListeners that you would like to have called, be sure to add them to the
+Visulizer \e prior to adding an InputSilo, which is the last refuge for
+unwanted user input.
+
+Here's how you can use this:
+
+ at code
+MultibodySystem system;
+// ... build system
+
+// Set up a Visualizer to run in real time mode, and give it an
+// InputSilo to gather user input.
+Visualizer viz(system);
+viz.setMode(Visualizer::RealTime);
+InputSilo* silo = new InputSilo;
+viz.addInputListener(silo);
+
+// You create a PeriodicEventHandler to poll the input. Note that the interval
+// you choose determines how responsive the simulation will be to user input,
+// but it also limits the maximum step size that the integrator can take.
+system.addEventHandler
+ (new MyUserInputHandler(*silo, 0.1)); // check every 100ms
+
+// Then in MyUserInputHandler::handleEvent(...):
+while (silo.isAnyUserInput()) {
+ while (silo.takeKeyHit(key,modifier)) {
+ // Process the key that was hit
+ }
+ while (silo.takeMenuPick(which, item)) {
+ // Process the picked menu item
+ }
+ while (silo.takeSliderMove(which, value)) {
+ // Process the new value for slider "which"
+ }
+}
+ at endcode
+
+If you want to wait until some input arrives, create the InputSilo and add
+it to the Visualizer as above, then in your main program (that is, not
+in the Handler) use code like this:
+ at code
+std::cout << "Hit ENTER in visualizer to continue ...\n";
+unsigned key, modifiers;
+do {silo->waitForKeyHit(key,modifiers);}
+while (key != Visualizer::InputListener::KeyEnter);
+ at endcode
+Similar methods are available for all the different input types, and you
+can also wait on the arrival of \e any input.
+
+<h3>Implementation</h3>
+
+The InputSilo implementations of the InputListener methods are called from the
+Visualizer's listener thread, which is a different thread than the one that
+is simultaneously running the simulation. The internal silos are double-ended
+queues (deques) that allow inputs to be pushed onto one end and pulled off
+the other, so that they can be consumed in FIFO order. There is a single mutex
+lock associated with \e all the silos together, and the lock must be held while
+anything is pushed onto or pulled off of any one of the silos.
+
+Each of the methods for getting the input out of the silos is called from the
+simulation thread, which must obtain the lock before removing anything, thus
+safely synchronizing the listener and simulation threads.
+
+A count is maintained of the total number of items in all the silos. It is
+incremented only when the listener thread holds the lock and adds something
+to a silo; it is decremented only when the simulation thread holds the lock
+and pulls something from a silo. The count may be examined without locking; it
+will have a value that was recently correct and can thus be used for a very
+fast check on whether there is likely to be any input worth holding a lock for;
+the isAnyUserInput() method returns \c true when the count is non-zero. It may
+occasionally return zero in cases where there is input, but only if that input
+just arrived so you can safely pick it up on the next poll.
+
+When possible we optimize for the case where many inputs arrive from the
+same device by just keeping the most recent value. That applies to slider
+and mouse moves. **/
+class SimTK_SIMBODY_EXPORT Visualizer::InputSilo
+: public Visualizer::InputListener {
+public:
+/** Default construction is all that is needed; there are no options. **/
+InputSilo();
+/** Throws away any unprocessed input. **/
+~InputSilo();
+
+/** This is a very fast test that does not require locking; you don't have to
+use this but it is a good idea to do so. **/
+bool isAnyUserInput() const;
+
+/** This will wait quietly until the user has provided some input to the
+visualizer.\ Any kind of input will terminate the wait; you'll have
+to look to see what it was. **/
+void waitForAnyUserInput() const;
+
+/** This will return user key hits until they have all been consumed, in the
+same order they were received. The \a key and \a modifiers values are those that
+were provided to our implementation of the InputListener::keyPressed() method.
+ at param[out] key
+ The key code for the key that was hit. See InputListener::KeyCode for
+ interpretation.
+ at param[out] modifiers
+ Status of Shift,Ctrl,Alt and "special" key code. See InputListener::Modifier
+ for interpretation.
+ at return \c true if a key and modifiers have been returned; \c false if the
+ character silo is now empty in which case both \a key and \a modifiers will
+ be set to zero. **/
+bool takeKeyHit(unsigned& key, unsigned& modifiers);
+
+/** Same as takeKeyHit() except that if there is no key hit input available
+it waits until there is, then returns the first one (which is removed from
+the silo just as takeKeyHit() would do. The behavior is like calling
+waitForAnyUserInput() repeatedly until takeKeyHit() returns \c true.
+ at see takeKeyHit(), waitForAnyUserInput() **/
+void waitForKeyHit(unsigned& key, unsigned& modifiers);
+
+/** This will return user menu picks until they have all been consumed, in the
+same order they were received. The \a item value returned is the value that was
+provided to our implementation of the InputListener::menuSelected() method.
+ at param[out] menu
+ The id number of the menu that was selected. This is the value that was
+ assigned to this menu in the Visualizer::addMenu() call.
+ at param[out] item
+ The menu item number for the entry that the user selected. This is the
+ number that was assigned at the time the menu was added via the
+ Visualizer::addMenu() method.
+ at return \c true if a menu item number has been returned; \c false if the menu
+ pick silo is now empty in which case \a item will be set to zero. **/
+bool takeMenuPick(int& menu, int& item);
+
+/** Same as takeMenuPick() except that if there is no menu pick input available
+it waits until there is, then returns the first one (which is removed from
+the silo just as takeMenuPick() would do. The behavior is like calling
+waitForAnyUserInput() repeatedly until takeMenuPick() returns \c true.
+ at see takeMenuPick(), waitForAnyUserInput() **/
+void waitForMenuPick(int& menu, int& item);
+
+/** This will return user changes to slider positions until they have all been
+consumed, in the same order they were received. The \a slider and \a value
+returns are those that were provided to our implementation of the
+InputListener::sliderMoved() method.
+ at param[out] slider
+ The id number of the slider that was moved. This is the value that was
+ assigned to this slider in the Visualizer::addSlider() call.
+ at param[out] value
+ This is the new value associated with the slider position to which the user
+ moved it.
+ at return \c true if a slider move has been returned; \c false if the slider move
+ silo is now empty in which case \a which will be set to zero and \a value
+ will be NaN. **/
+bool takeSliderMove(int& slider, Real& value);
+
+/** Same as takeSliderMove() except that if there is no slider move input
+available it waits until there is, then returns the first one (which is removed
+from the silo just as takeSliderMove() would do. The behavior is like calling
+waitForAnyUserInput() repeatedly until takeSliderMove() returns \c true.
+ at see takeSliderMove(), waitForAnyUserInput() **/
+void waitForSliderMove(int& slider, Real& value);
+
+/** Throw away any pending unprocessed input of all types. **/
+void clear();
+
+//------------------------------------------------------------------------------
+ private:
+// Each of these will return true to the Visualizer's listener thread, meaning
+// that the input will be absorbed and subsequent listeners (if any) will not
+// be called.
+virtual bool keyPressed(unsigned key, unsigned modifiers);
+virtual bool menuSelected(int menu, int item);
+virtual bool sliderMoved(int slider, Real value);
+
+class Impl;
+const Impl& getImpl() const {assert(m_impl); return *m_impl;}
+Impl& updImpl() {assert(m_impl); return *m_impl;}
+
+Impl* m_impl; // the lone data member in this class
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_VISUALIZER_INPUT_LISTENER_H_
diff --git a/Simbody/Visualizer/include/simbody/internal/Visualizer_Reporter.h b/Simbody/Visualizer/include/simbody/internal/Visualizer_Reporter.h
new file mode 100644
index 0000000..fa4d4df
--- /dev/null
+++ b/Simbody/Visualizer/include/simbody/internal/Visualizer_Reporter.h
@@ -0,0 +1,89 @@
+#ifndef SimTK_SIMBODY_VISUALIZER_REPORTER_H_
+#define SimTK_SIMBODY_VISUALIZER_REPORTER_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) 2010-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Visualizer.h"
+
+namespace SimTK {
+
+class MultibodySystem;
+
+/** This is an EventReporter that makes it easy to generate on-screen movies of
+any simulation. Use it like this:
+ at code
+ MultibodySystem system;
+ // ... build your system
+
+ // Create a Visualizer object for communication with the visualizer.
+ Visualizer viz(system);
+ // ... set visualization options by calling methods on viz
+
+ // Create a Reporter that will make periodic calls to the Visualizer's
+ // report() method to render frames. Note that ownership of the Reporter
+ // is taken by the System; don't delete it yourself.
+ system.addEventReporter(new Visualizer::Reporter(viz, interval));
+ at endcode
+**/
+class SimTK_SIMBODY_EXPORT Visualizer::Reporter : public PeriodicEventReporter {
+public:
+ /** Create a Reporter for the given Visualizer \a viz, and call its
+ report() method every \a reportInterval time units of \e simulation time
+ (not necessarily measured in seconds). Note that if you want to run your
+ simulation in real time and you aren't using seconds as time units, you
+ should set the time scale via the Visualizer's setRealTimeScale() method
+ and set the report interval here to TimeScale/FrameRate. **/
+ explicit Reporter(const Visualizer& viz, Real reportInterval=Infinity);
+
+ /** This constructor will create a Visualizer with all the default
+ settings for the supplied system \a sys. This is an abbreviation for
+ @code Reporter(Visualizer(system), reportInterval); @endcode. **/
+ explicit Reporter(const MultibodySystem& sys, Real reportInterval=Infinity);
+
+ /** Destructor will also destroy the contained Visualizer object if there
+ are no other references to it. **/
+ ~Reporter();
+
+ /** Get the Visualizer which this Reporter is using to generate images. **/
+ const Visualizer& getVisualizer() const;
+
+ /** This satisfies the pure virtual method in EventReporter. **/
+ virtual void handleEvent(const State& state) const;
+
+protected:
+ class Impl;
+ Impl* impl;
+ const Impl& getImpl() const {assert(impl); return *impl;}
+ Impl& updImpl() {assert(impl); return *impl;}
+};
+
+/** OBSOLETE: This provides limited backwards compatibility with the old VTK
+Visualizer that is no longer supported.\ Switch to Visualizer::Reporter instead. **/
+typedef Visualizer::Reporter VTKEventReporter;
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_VISUALIZER_REPORTER_H_
diff --git a/Simbody/Visualizer/simbody-visualizer/CMakeLists.txt b/Simbody/Visualizer/simbody-visualizer/CMakeLists.txt
new file mode 100644
index 0000000..15ab401
--- /dev/null
+++ b/Simbody/Visualizer/simbody-visualizer/CMakeLists.txt
@@ -0,0 +1,52 @@
+# Generate the visualizer application and deal with its dependencies
+# on OpenGL and glut. On Windows we're going to install our own glut; for
+# other platforms we depend on their already being a glut (or freeglut)
+# available.
+
+INCLUDE (FindOpenGL)
+IF(WIN32)
+ SET(glut32dir "${CMAKE_CURRENT_SOURCE_DIR}/glut32")
+ SET(GLUT32_HEADERS "${glut32dir}/glut.h" "${glut32dir}/glext.h")
+ IF(${PLATFORM_ABI} MATCHES "x64")
+ SET(glut32libdir "${glut32dir}/lib64")
+ ELSE()
+ SET(glut32libdir "${glut32dir}/lib")
+ ENDIF()
+ SET(GLUT_LIBRARIES "${glut32libdir}/glut32.lib")
+ # Update the local copy of the dll; destination is Release or Debug.
+ # Must fix slashes so that Windows "copy" command can work.
+ FILE(TO_NATIVE_PATH "${glut32libdir}/glut32.dll" gl32src)
+ FILE(TO_NATIVE_PATH "${BUILD_BINARY_DIR}/${CMAKE_CFG_INTDIR}/glut32.dll"
+ gl32dest)
+ELSE()
+ INCLUDE(FindGLUT) # sets GLUT_LIBRARIES
+ENDIF()
+
+ADD_EXECUTABLE(${GUI_NAME}
+ simbody-visualizer.cpp lodepng.cpp lodepng.h
+ ${GLUT32_HEADERS}) # only on Windows
+
+SET_TARGET_PROPERTIES(${GUI_NAME} PROPERTIES
+ PROJECT_LABEL "Code - ${GUI_NAME}")
+
+TARGET_LINK_LIBRARIES(${GUI_NAME}
+ ${TEST_SHARED_TARGET} ${GLUT_LIBRARIES} ${OPENGL_LIBRARIES})
+
+# SIMBODY_VISUALIZER_INSTALL_DIR is set in the root CMakeLists.txt
+INSTALL(TARGETS ${GUI_NAME}
+ PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE
+ GROUP_READ GROUP_WRITE GROUP_EXECUTE
+ WORLD_READ WORLD_EXECUTE
+ DESTINATION ${SIMBODY_VISUALIZER_INSTALL_DIR})
+
+# on Windows we also have to copy and later install the glut32.dll
+IF(WIN32)
+ # This gets executed whenever the named target gets built.
+ add_custom_command(
+ TARGET ${GUI_NAME}
+ COMMAND copy ${gl32src} ${gl32dest}
+ COMMENT "Copying glut32.dll" VERBATIM)
+
+ INSTALL(FILES ${glut32libdir}/glut32.dll
+ DESTINATION ${CMAKE_INSTALL_FULL_BINDIR}) #same as GUI
+ENDIF()
diff --git a/Simbody/Visualizer/simbody-visualizer/glut32/glext.h b/Simbody/Visualizer/simbody-visualizer/glut32/glext.h
new file mode 100644
index 0000000..f3cb54b
--- /dev/null
+++ b/Simbody/Visualizer/simbody-visualizer/glut32/glext.h
@@ -0,0 +1,11037 @@
+#ifndef __glext_h_
+#define __glext_h_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+** Copyright (c) 2007-2010 The Khronos Group Inc.
+**
+** Permission is hereby granted, free of charge, to any person obtaining a
+** copy of this software and/or associated documentation files (the
+** "Materials"), to deal in the Materials without restriction, including
+** without limitation the rights to use, copy, modify, merge, publish,
+** distribute, sublicense, and/or sell copies of the Materials, and to
+** permit persons to whom the Materials are furnished to do so, subject to
+** the following conditions:
+**
+** The above copyright notice and this permission notice shall be included
+** in all copies or substantial portions of the Materials.
+**
+** THE MATERIALS ARE PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+** EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+** MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+** IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+** CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+** TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+** MATERIALS OR THE USE OR OTHER DEALINGS IN THE MATERIALS.
+*/
+
+/* Header file version number, required by OpenGL ABI for Linux */
+/* glext.h last updated $Date: 2010-09-30 01:33:03 -0700 (Thu, 30 Sep 2010) $ */
+/* Current version at http://www.opengl.org/registry/ */
+#define GL_GLEXT_VERSION 65
+/* Function declaration macros - to move into glplatform.h */
+
+#if defined(_WIN32) && !defined(APIENTRY) && !defined(__CYGWIN__) && !defined(__SCITECH_SNAP__)
+#ifndef WIN32_LEAN_AND_MEAN
+# define WIN32_LEAN_AND_MEAN
+#endif
+#ifndef NOMINMAX
+# define NOMINMAX
+#endif
+#include <windows.h> // needed for wglGetProcAddress
+#define glutGetProcAddress wglGetProcAddress
+typedef BOOL (APIENTRY *PFNWGLSWAPINTERVALFARPROC)( int );
+#endif
+
+#ifndef APIENTRY
+#define APIENTRY
+#endif
+#ifndef APIENTRYP
+#define APIENTRYP APIENTRY *
+#endif
+#ifndef GLAPI
+#define GLAPI extern
+#endif
+
+/*************************************************************/
+
+#ifndef GL_VERSION_1_2
+#define GL_UNSIGNED_BYTE_3_3_2 0x8032
+#define GL_UNSIGNED_SHORT_4_4_4_4 0x8033
+#define GL_UNSIGNED_SHORT_5_5_5_1 0x8034
+#define GL_UNSIGNED_INT_8_8_8_8 0x8035
+#define GL_UNSIGNED_INT_10_10_10_2 0x8036
+#define GL_TEXTURE_BINDING_3D 0x806A
+#define GL_PACK_SKIP_IMAGES 0x806B
+#define GL_PACK_IMAGE_HEIGHT 0x806C
+#define GL_UNPACK_SKIP_IMAGES 0x806D
+#define GL_UNPACK_IMAGE_HEIGHT 0x806E
+#define GL_TEXTURE_3D 0x806F
+#define GL_PROXY_TEXTURE_3D 0x8070
+#define GL_TEXTURE_DEPTH 0x8071
+#define GL_TEXTURE_WRAP_R 0x8072
+#define GL_MAX_3D_TEXTURE_SIZE 0x8073
+#define GL_UNSIGNED_BYTE_2_3_3_REV 0x8362
+#define GL_UNSIGNED_SHORT_5_6_5 0x8363
+#define GL_UNSIGNED_SHORT_5_6_5_REV 0x8364
+#define GL_UNSIGNED_SHORT_4_4_4_4_REV 0x8365
+#define GL_UNSIGNED_SHORT_1_5_5_5_REV 0x8366
+#define GL_UNSIGNED_INT_8_8_8_8_REV 0x8367
+#define GL_UNSIGNED_INT_2_10_10_10_REV 0x8368
+#define GL_BGR 0x80E0
+#define GL_BGRA 0x80E1
+#define GL_MAX_ELEMENTS_VERTICES 0x80E8
+#define GL_MAX_ELEMENTS_INDICES 0x80E9
+#define GL_CLAMP_TO_EDGE 0x812F
+#define GL_TEXTURE_MIN_LOD 0x813A
+#define GL_TEXTURE_MAX_LOD 0x813B
+#define GL_TEXTURE_BASE_LEVEL 0x813C
+#define GL_TEXTURE_MAX_LEVEL 0x813D
+#define GL_SMOOTH_POINT_SIZE_RANGE 0x0B12
+#define GL_SMOOTH_POINT_SIZE_GRANULARITY 0x0B13
+#define GL_SMOOTH_LINE_WIDTH_RANGE 0x0B22
+#define GL_SMOOTH_LINE_WIDTH_GRANULARITY 0x0B23
+#define GL_ALIASED_LINE_WIDTH_RANGE 0x846E
+#endif
+
+#ifndef GL_VERSION_1_2_DEPRECATED
+#define GL_RESCALE_NORMAL 0x803A
+#define GL_LIGHT_MODEL_COLOR_CONTROL 0x81F8
+#define GL_SINGLE_COLOR 0x81F9
+#define GL_SEPARATE_SPECULAR_COLOR 0x81FA
+#define GL_ALIASED_POINT_SIZE_RANGE 0x846D
+#endif
+
+#ifndef GL_ARB_imaging
+#define GL_CONSTANT_COLOR 0x8001
+#define GL_ONE_MINUS_CONSTANT_COLOR 0x8002
+#define GL_CONSTANT_ALPHA 0x8003
+#define GL_ONE_MINUS_CONSTANT_ALPHA 0x8004
+#define GL_BLEND_COLOR 0x8005
+#define GL_FUNC_ADD 0x8006
+#define GL_MIN 0x8007
+#define GL_MAX 0x8008
+#define GL_BLEND_EQUATION 0x8009
+#define GL_FUNC_SUBTRACT 0x800A
+#define GL_FUNC_REVERSE_SUBTRACT 0x800B
+#endif
+
+#ifndef GL_ARB_imaging_DEPRECATED
+#define GL_CONVOLUTION_1D 0x8010
+#define GL_CONVOLUTION_2D 0x8011
+#define GL_SEPARABLE_2D 0x8012
+#define GL_CONVOLUTION_BORDER_MODE 0x8013
+#define GL_CONVOLUTION_FILTER_SCALE 0x8014
+#define GL_CONVOLUTION_FILTER_BIAS 0x8015
+#define GL_REDUCE 0x8016
+#define GL_CONVOLUTION_FORMAT 0x8017
+#define GL_CONVOLUTION_WIDTH 0x8018
+#define GL_CONVOLUTION_HEIGHT 0x8019
+#define GL_MAX_CONVOLUTION_WIDTH 0x801A
+#define GL_MAX_CONVOLUTION_HEIGHT 0x801B
+#define GL_POST_CONVOLUTION_RED_SCALE 0x801C
+#define GL_POST_CONVOLUTION_GREEN_SCALE 0x801D
+#define GL_POST_CONVOLUTION_BLUE_SCALE 0x801E
+#define GL_POST_CONVOLUTION_ALPHA_SCALE 0x801F
+#define GL_POST_CONVOLUTION_RED_BIAS 0x8020
+#define GL_POST_CONVOLUTION_GREEN_BIAS 0x8021
+#define GL_POST_CONVOLUTION_BLUE_BIAS 0x8022
+#define GL_POST_CONVOLUTION_ALPHA_BIAS 0x8023
+#define GL_HISTOGRAM 0x8024
+#define GL_PROXY_HISTOGRAM 0x8025
+#define GL_HISTOGRAM_WIDTH 0x8026
+#define GL_HISTOGRAM_FORMAT 0x8027
+#define GL_HISTOGRAM_RED_SIZE 0x8028
+#define GL_HISTOGRAM_GREEN_SIZE 0x8029
+#define GL_HISTOGRAM_BLUE_SIZE 0x802A
+#define GL_HISTOGRAM_ALPHA_SIZE 0x802B
+#define GL_HISTOGRAM_LUMINANCE_SIZE 0x802C
+#define GL_HISTOGRAM_SINK 0x802D
+#define GL_MINMAX 0x802E
+#define GL_MINMAX_FORMAT 0x802F
+#define GL_MINMAX_SINK 0x8030
+#define GL_TABLE_TOO_LARGE 0x8031
+#define GL_COLOR_MATRIX 0x80B1
+#define GL_COLOR_MATRIX_STACK_DEPTH 0x80B2
+#define GL_MAX_COLOR_MATRIX_STACK_DEPTH 0x80B3
+#define GL_POST_COLOR_MATRIX_RED_SCALE 0x80B4
+#define GL_POST_COLOR_MATRIX_GREEN_SCALE 0x80B5
+#define GL_POST_COLOR_MATRIX_BLUE_SCALE 0x80B6
+#define GL_POST_COLOR_MATRIX_ALPHA_SCALE 0x80B7
+#define GL_POST_COLOR_MATRIX_RED_BIAS 0x80B8
+#define GL_POST_COLOR_MATRIX_GREEN_BIAS 0x80B9
+#define GL_POST_COLOR_MATRIX_BLUE_BIAS 0x80BA
+#define GL_POST_COLOR_MATRIX_ALPHA_BIAS 0x80BB
+#define GL_COLOR_TABLE 0x80D0
+#define GL_POST_CONVOLUTION_COLOR_TABLE 0x80D1
+#define GL_POST_COLOR_MATRIX_COLOR_TABLE 0x80D2
+#define GL_PROXY_COLOR_TABLE 0x80D3
+#define GL_PROXY_POST_CONVOLUTION_COLOR_TABLE 0x80D4
+#define GL_PROXY_POST_COLOR_MATRIX_COLOR_TABLE 0x80D5
+#define GL_COLOR_TABLE_SCALE 0x80D6
+#define GL_COLOR_TABLE_BIAS 0x80D7
+#define GL_COLOR_TABLE_FORMAT 0x80D8
+#define GL_COLOR_TABLE_WIDTH 0x80D9
+#define GL_COLOR_TABLE_RED_SIZE 0x80DA
+#define GL_COLOR_TABLE_GREEN_SIZE 0x80DB
+#define GL_COLOR_TABLE_BLUE_SIZE 0x80DC
+#define GL_COLOR_TABLE_ALPHA_SIZE 0x80DD
+#define GL_COLOR_TABLE_LUMINANCE_SIZE 0x80DE
+#define GL_COLOR_TABLE_INTENSITY_SIZE 0x80DF
+#define GL_CONSTANT_BORDER 0x8151
+#define GL_REPLICATE_BORDER 0x8153
+#define GL_CONVOLUTION_BORDER_COLOR 0x8154
+#endif
+
+#ifndef GL_VERSION_1_3
+#define GL_TEXTURE0 0x84C0
+#define GL_TEXTURE1 0x84C1
+#define GL_TEXTURE2 0x84C2
+#define GL_TEXTURE3 0x84C3
+#define GL_TEXTURE4 0x84C4
+#define GL_TEXTURE5 0x84C5
+#define GL_TEXTURE6 0x84C6
+#define GL_TEXTURE7 0x84C7
+#define GL_TEXTURE8 0x84C8
+#define GL_TEXTURE9 0x84C9
+#define GL_TEXTURE10 0x84CA
+#define GL_TEXTURE11 0x84CB
+#define GL_TEXTURE12 0x84CC
+#define GL_TEXTURE13 0x84CD
+#define GL_TEXTURE14 0x84CE
+#define GL_TEXTURE15 0x84CF
+#define GL_TEXTURE16 0x84D0
+#define GL_TEXTURE17 0x84D1
+#define GL_TEXTURE18 0x84D2
+#define GL_TEXTURE19 0x84D3
+#define GL_TEXTURE20 0x84D4
+#define GL_TEXTURE21 0x84D5
+#define GL_TEXTURE22 0x84D6
+#define GL_TEXTURE23 0x84D7
+#define GL_TEXTURE24 0x84D8
+#define GL_TEXTURE25 0x84D9
+#define GL_TEXTURE26 0x84DA
+#define GL_TEXTURE27 0x84DB
+#define GL_TEXTURE28 0x84DC
+#define GL_TEXTURE29 0x84DD
+#define GL_TEXTURE30 0x84DE
+#define GL_TEXTURE31 0x84DF
+#define GL_ACTIVE_TEXTURE 0x84E0
+#define GL_MULTISAMPLE 0x809D
+#define GL_SAMPLE_ALPHA_TO_COVERAGE 0x809E
+#define GL_SAMPLE_ALPHA_TO_ONE 0x809F
+#define GL_SAMPLE_COVERAGE 0x80A0
+#define GL_SAMPLE_BUFFERS 0x80A8
+#define GL_SAMPLES 0x80A9
+#define GL_SAMPLE_COVERAGE_VALUE 0x80AA
+#define GL_SAMPLE_COVERAGE_INVERT 0x80AB
+#define GL_TEXTURE_CUBE_MAP 0x8513
+#define GL_TEXTURE_BINDING_CUBE_MAP 0x8514
+#define GL_TEXTURE_CUBE_MAP_POSITIVE_X 0x8515
+#define GL_TEXTURE_CUBE_MAP_NEGATIVE_X 0x8516
+#define GL_TEXTURE_CUBE_MAP_POSITIVE_Y 0x8517
+#define GL_TEXTURE_CUBE_MAP_NEGATIVE_Y 0x8518
+#define GL_TEXTURE_CUBE_MAP_POSITIVE_Z 0x8519
+#define GL_TEXTURE_CUBE_MAP_NEGATIVE_Z 0x851A
+#define GL_PROXY_TEXTURE_CUBE_MAP 0x851B
+#define GL_MAX_CUBE_MAP_TEXTURE_SIZE 0x851C
+#define GL_COMPRESSED_RGB 0x84ED
+#define GL_COMPRESSED_RGBA 0x84EE
+#define GL_TEXTURE_COMPRESSION_HINT 0x84EF
+#define GL_TEXTURE_COMPRESSED_IMAGE_SIZE 0x86A0
+#define GL_TEXTURE_COMPRESSED 0x86A1
+#define GL_NUM_COMPRESSED_TEXTURE_FORMATS 0x86A2
+#define GL_COMPRESSED_TEXTURE_FORMATS 0x86A3
+#define GL_CLAMP_TO_BORDER 0x812D
+#endif
+
+#ifndef GL_VERSION_1_3_DEPRECATED
+#define GL_CLIENT_ACTIVE_TEXTURE 0x84E1
+#define GL_MAX_TEXTURE_UNITS 0x84E2
+#define GL_TRANSPOSE_MODELVIEW_MATRIX 0x84E3
+#define GL_TRANSPOSE_PROJECTION_MATRIX 0x84E4
+#define GL_TRANSPOSE_TEXTURE_MATRIX 0x84E5
+#define GL_TRANSPOSE_COLOR_MATRIX 0x84E6
+#define GL_MULTISAMPLE_BIT 0x20000000
+#define GL_NORMAL_MAP 0x8511
+#define GL_REFLECTION_MAP 0x8512
+#define GL_COMPRESSED_ALPHA 0x84E9
+#define GL_COMPRESSED_LUMINANCE 0x84EA
+#define GL_COMPRESSED_LUMINANCE_ALPHA 0x84EB
+#define GL_COMPRESSED_INTENSITY 0x84EC
+#define GL_COMBINE 0x8570
+#define GL_COMBINE_RGB 0x8571
+#define GL_COMBINE_ALPHA 0x8572
+#define GL_SOURCE0_RGB 0x8580
+#define GL_SOURCE1_RGB 0x8581
+#define GL_SOURCE2_RGB 0x8582
+#define GL_SOURCE0_ALPHA 0x8588
+#define GL_SOURCE1_ALPHA 0x8589
+#define GL_SOURCE2_ALPHA 0x858A
+#define GL_OPERAND0_RGB 0x8590
+#define GL_OPERAND1_RGB 0x8591
+#define GL_OPERAND2_RGB 0x8592
+#define GL_OPERAND0_ALPHA 0x8598
+#define GL_OPERAND1_ALPHA 0x8599
+#define GL_OPERAND2_ALPHA 0x859A
+#define GL_RGB_SCALE 0x8573
+#define GL_ADD_SIGNED 0x8574
+#define GL_INTERPOLATE 0x8575
+#define GL_SUBTRACT 0x84E7
+#define GL_CONSTANT 0x8576
+#define GL_PRIMARY_COLOR 0x8577
+#define GL_PREVIOUS 0x8578
+#define GL_DOT3_RGB 0x86AE
+#define GL_DOT3_RGBA 0x86AF
+#endif
+
+#ifndef GL_VERSION_1_4
+#define GL_BLEND_DST_RGB 0x80C8
+#define GL_BLEND_SRC_RGB 0x80C9
+#define GL_BLEND_DST_ALPHA 0x80CA
+#define GL_BLEND_SRC_ALPHA 0x80CB
+#define GL_POINT_FADE_THRESHOLD_SIZE 0x8128
+#define GL_DEPTH_COMPONENT16 0x81A5
+#define GL_DEPTH_COMPONENT24 0x81A6
+#define GL_DEPTH_COMPONENT32 0x81A7
+#define GL_MIRRORED_REPEAT 0x8370
+#define GL_MAX_TEXTURE_LOD_BIAS 0x84FD
+#define GL_TEXTURE_LOD_BIAS 0x8501
+#define GL_INCR_WRAP 0x8507
+#define GL_DECR_WRAP 0x8508
+#define GL_TEXTURE_DEPTH_SIZE 0x884A
+#define GL_TEXTURE_COMPARE_MODE 0x884C
+#define GL_TEXTURE_COMPARE_FUNC 0x884D
+#endif
+
+#ifndef GL_VERSION_1_4_DEPRECATED
+#define GL_POINT_SIZE_MIN 0x8126
+#define GL_POINT_SIZE_MAX 0x8127
+#define GL_POINT_DISTANCE_ATTENUATION 0x8129
+#define GL_GENERATE_MIPMAP 0x8191
+#define GL_GENERATE_MIPMAP_HINT 0x8192
+#define GL_FOG_COORDINATE_SOURCE 0x8450
+#define GL_FOG_COORDINATE 0x8451
+#define GL_FRAGMENT_DEPTH 0x8452
+#define GL_CURRENT_FOG_COORDINATE 0x8453
+#define GL_FOG_COORDINATE_ARRAY_TYPE 0x8454
+#define GL_FOG_COORDINATE_ARRAY_STRIDE 0x8455
+#define GL_FOG_COORDINATE_ARRAY_POINTER 0x8456
+#define GL_FOG_COORDINATE_ARRAY 0x8457
+#define GL_COLOR_SUM 0x8458
+#define GL_CURRENT_SECONDARY_COLOR 0x8459
+#define GL_SECONDARY_COLOR_ARRAY_SIZE 0x845A
+#define GL_SECONDARY_COLOR_ARRAY_TYPE 0x845B
+#define GL_SECONDARY_COLOR_ARRAY_STRIDE 0x845C
+#define GL_SECONDARY_COLOR_ARRAY_POINTER 0x845D
+#define GL_SECONDARY_COLOR_ARRAY 0x845E
+#define GL_TEXTURE_FILTER_CONTROL 0x8500
+#define GL_DEPTH_TEXTURE_MODE 0x884B
+#define GL_COMPARE_R_TO_TEXTURE 0x884E
+#endif
+
+#ifndef GL_VERSION_1_5
+#define GL_BUFFER_SIZE 0x8764
+#define GL_BUFFER_USAGE 0x8765
+#define GL_QUERY_COUNTER_BITS 0x8864
+#define GL_CURRENT_QUERY 0x8865
+#define GL_QUERY_RESULT 0x8866
+#define GL_QUERY_RESULT_AVAILABLE 0x8867
+#define GL_ARRAY_BUFFER 0x8892
+#define GL_ELEMENT_ARRAY_BUFFER 0x8893
+#define GL_ARRAY_BUFFER_BINDING 0x8894
+#define GL_ELEMENT_ARRAY_BUFFER_BINDING 0x8895
+#define GL_VERTEX_ATTRIB_ARRAY_BUFFER_BINDING 0x889F
+#define GL_READ_ONLY 0x88B8
+#define GL_WRITE_ONLY 0x88B9
+#define GL_READ_WRITE 0x88BA
+#define GL_BUFFER_ACCESS 0x88BB
+#define GL_BUFFER_MAPPED 0x88BC
+#define GL_BUFFER_MAP_POINTER 0x88BD
+#define GL_STREAM_DRAW 0x88E0
+#define GL_STREAM_READ 0x88E1
+#define GL_STREAM_COPY 0x88E2
+#define GL_STATIC_DRAW 0x88E4
+#define GL_STATIC_READ 0x88E5
+#define GL_STATIC_COPY 0x88E6
+#define GL_DYNAMIC_DRAW 0x88E8
+#define GL_DYNAMIC_READ 0x88E9
+#define GL_DYNAMIC_COPY 0x88EA
+#define GL_SAMPLES_PASSED 0x8914
+#endif
+
+#ifndef GL_VERSION_1_5_DEPRECATED
+#define GL_VERTEX_ARRAY_BUFFER_BINDING 0x8896
+#define GL_NORMAL_ARRAY_BUFFER_BINDING 0x8897
+#define GL_COLOR_ARRAY_BUFFER_BINDING 0x8898
+#define GL_INDEX_ARRAY_BUFFER_BINDING 0x8899
+#define GL_TEXTURE_COORD_ARRAY_BUFFER_BINDING 0x889A
+#define GL_EDGE_FLAG_ARRAY_BUFFER_BINDING 0x889B
+#define GL_SECONDARY_COLOR_ARRAY_BUFFER_BINDING 0x889C
+#define GL_FOG_COORDINATE_ARRAY_BUFFER_BINDING 0x889D
+#define GL_WEIGHT_ARRAY_BUFFER_BINDING 0x889E
+#define GL_FOG_COORD_SRC 0x8450
+#define GL_FOG_COORD 0x8451
+#define GL_CURRENT_FOG_COORD 0x8453
+#define GL_FOG_COORD_ARRAY_TYPE 0x8454
+#define GL_FOG_COORD_ARRAY_STRIDE 0x8455
+#define GL_FOG_COORD_ARRAY_POINTER 0x8456
+#define GL_FOG_COORD_ARRAY 0x8457
+#define GL_FOG_COORD_ARRAY_BUFFER_BINDING 0x889D
+#define GL_SRC0_RGB 0x8580
+#define GL_SRC1_RGB 0x8581
+#define GL_SRC2_RGB 0x8582
+#define GL_SRC0_ALPHA 0x8588
+#define GL_SRC1_ALPHA 0x8589
+#define GL_SRC2_ALPHA 0x858A
+#endif
+
+#ifndef GL_VERSION_2_0
+#define GL_BLEND_EQUATION_RGB 0x8009
+#define GL_VERTEX_ATTRIB_ARRAY_ENABLED 0x8622
+#define GL_VERTEX_ATTRIB_ARRAY_SIZE 0x8623
+#define GL_VERTEX_ATTRIB_ARRAY_STRIDE 0x8624
+#define GL_VERTEX_ATTRIB_ARRAY_TYPE 0x8625
+#define GL_CURRENT_VERTEX_ATTRIB 0x8626
+#define GL_VERTEX_PROGRAM_POINT_SIZE 0x8642
+#define GL_VERTEX_ATTRIB_ARRAY_POINTER 0x8645
+#define GL_STENCIL_BACK_FUNC 0x8800
+#define GL_STENCIL_BACK_FAIL 0x8801
+#define GL_STENCIL_BACK_PASS_DEPTH_FAIL 0x8802
+#define GL_STENCIL_BACK_PASS_DEPTH_PASS 0x8803
+#define GL_MAX_DRAW_BUFFERS 0x8824
+#define GL_DRAW_BUFFER0 0x8825
+#define GL_DRAW_BUFFER1 0x8826
+#define GL_DRAW_BUFFER2 0x8827
+#define GL_DRAW_BUFFER3 0x8828
+#define GL_DRAW_BUFFER4 0x8829
+#define GL_DRAW_BUFFER5 0x882A
+#define GL_DRAW_BUFFER6 0x882B
+#define GL_DRAW_BUFFER7 0x882C
+#define GL_DRAW_BUFFER8 0x882D
+#define GL_DRAW_BUFFER9 0x882E
+#define GL_DRAW_BUFFER10 0x882F
+#define GL_DRAW_BUFFER11 0x8830
+#define GL_DRAW_BUFFER12 0x8831
+#define GL_DRAW_BUFFER13 0x8832
+#define GL_DRAW_BUFFER14 0x8833
+#define GL_DRAW_BUFFER15 0x8834
+#define GL_BLEND_EQUATION_ALPHA 0x883D
+#define GL_MAX_VERTEX_ATTRIBS 0x8869
+#define GL_VERTEX_ATTRIB_ARRAY_NORMALIZED 0x886A
+#define GL_MAX_TEXTURE_IMAGE_UNITS 0x8872
+#define GL_FRAGMENT_SHADER 0x8B30
+#define GL_VERTEX_SHADER 0x8B31
+#define GL_MAX_FRAGMENT_UNIFORM_COMPONENTS 0x8B49
+#define GL_MAX_VERTEX_UNIFORM_COMPONENTS 0x8B4A
+#define GL_MAX_VARYING_FLOATS 0x8B4B
+#define GL_MAX_VERTEX_TEXTURE_IMAGE_UNITS 0x8B4C
+#define GL_MAX_COMBINED_TEXTURE_IMAGE_UNITS 0x8B4D
+#define GL_SHADER_TYPE 0x8B4F
+#define GL_FLOAT_VEC2 0x8B50
+#define GL_FLOAT_VEC3 0x8B51
+#define GL_FLOAT_VEC4 0x8B52
+#define GL_INT_VEC2 0x8B53
+#define GL_INT_VEC3 0x8B54
+#define GL_INT_VEC4 0x8B55
+#define GL_BOOL 0x8B56
+#define GL_BOOL_VEC2 0x8B57
+#define GL_BOOL_VEC3 0x8B58
+#define GL_BOOL_VEC4 0x8B59
+#define GL_FLOAT_MAT2 0x8B5A
+#define GL_FLOAT_MAT3 0x8B5B
+#define GL_FLOAT_MAT4 0x8B5C
+#define GL_SAMPLER_1D 0x8B5D
+#define GL_SAMPLER_2D 0x8B5E
+#define GL_SAMPLER_3D 0x8B5F
+#define GL_SAMPLER_CUBE 0x8B60
+#define GL_SAMPLER_1D_SHADOW 0x8B61
+#define GL_SAMPLER_2D_SHADOW 0x8B62
+#define GL_DELETE_STATUS 0x8B80
+#define GL_COMPILE_STATUS 0x8B81
+#define GL_LINK_STATUS 0x8B82
+#define GL_VALIDATE_STATUS 0x8B83
+#define GL_INFO_LOG_LENGTH 0x8B84
+#define GL_ATTACHED_SHADERS 0x8B85
+#define GL_ACTIVE_UNIFORMS 0x8B86
+#define GL_ACTIVE_UNIFORM_MAX_LENGTH 0x8B87
+#define GL_SHADER_SOURCE_LENGTH 0x8B88
+#define GL_ACTIVE_ATTRIBUTES 0x8B89
+#define GL_ACTIVE_ATTRIBUTE_MAX_LENGTH 0x8B8A
+#define GL_FRAGMENT_SHADER_DERIVATIVE_HINT 0x8B8B
+#define GL_SHADING_LANGUAGE_VERSION 0x8B8C
+#define GL_CURRENT_PROGRAM 0x8B8D
+#define GL_POINT_SPRITE_COORD_ORIGIN 0x8CA0
+#define GL_LOWER_LEFT 0x8CA1
+#define GL_UPPER_LEFT 0x8CA2
+#define GL_STENCIL_BACK_REF 0x8CA3
+#define GL_STENCIL_BACK_VALUE_MASK 0x8CA4
+#define GL_STENCIL_BACK_WRITEMASK 0x8CA5
+#endif
+
+#ifndef GL_VERSION_2_0_DEPRECATED
+#define GL_VERTEX_PROGRAM_TWO_SIDE 0x8643
+#define GL_POINT_SPRITE 0x8861
+#define GL_COORD_REPLACE 0x8862
+#define GL_MAX_TEXTURE_COORDS 0x8871
+#endif
+
+#ifndef GL_VERSION_2_1
+#define GL_PIXEL_PACK_BUFFER 0x88EB
+#define GL_PIXEL_UNPACK_BUFFER 0x88EC
+#define GL_PIXEL_PACK_BUFFER_BINDING 0x88ED
+#define GL_PIXEL_UNPACK_BUFFER_BINDING 0x88EF
+#define GL_FLOAT_MAT2x3 0x8B65
+#define GL_FLOAT_MAT2x4 0x8B66
+#define GL_FLOAT_MAT3x2 0x8B67
+#define GL_FLOAT_MAT3x4 0x8B68
+#define GL_FLOAT_MAT4x2 0x8B69
+#define GL_FLOAT_MAT4x3 0x8B6A
+#define GL_SRGB 0x8C40
+#define GL_SRGB8 0x8C41
+#define GL_SRGB_ALPHA 0x8C42
+#define GL_SRGB8_ALPHA8 0x8C43
+#define GL_COMPRESSED_SRGB 0x8C48
+#define GL_COMPRESSED_SRGB_ALPHA 0x8C49
+#endif
+
+#ifndef GL_VERSION_2_1_DEPRECATED
+#define GL_CURRENT_RASTER_SECONDARY_COLOR 0x845F
+#define GL_SLUMINANCE_ALPHA 0x8C44
+#define GL_SLUMINANCE8_ALPHA8 0x8C45
+#define GL_SLUMINANCE 0x8C46
+#define GL_SLUMINANCE8 0x8C47
+#define GL_COMPRESSED_SLUMINANCE 0x8C4A
+#define GL_COMPRESSED_SLUMINANCE_ALPHA 0x8C4B
+#endif
+
+#ifndef GL_VERSION_3_0
+#define GL_COMPARE_REF_TO_TEXTURE 0x884E
+#define GL_CLIP_DISTANCE0 0x3000
+#define GL_CLIP_DISTANCE1 0x3001
+#define GL_CLIP_DISTANCE2 0x3002
+#define GL_CLIP_DISTANCE3 0x3003
+#define GL_CLIP_DISTANCE4 0x3004
+#define GL_CLIP_DISTANCE5 0x3005
+#define GL_CLIP_DISTANCE6 0x3006
+#define GL_CLIP_DISTANCE7 0x3007
+#define GL_MAX_CLIP_DISTANCES 0x0D32
+#define GL_MAJOR_VERSION 0x821B
+#define GL_MINOR_VERSION 0x821C
+#define GL_NUM_EXTENSIONS 0x821D
+#define GL_CONTEXT_FLAGS 0x821E
+#define GL_DEPTH_BUFFER 0x8223
+#define GL_STENCIL_BUFFER 0x8224
+#define GL_COMPRESSED_RED 0x8225
+#define GL_COMPRESSED_RG 0x8226
+#define GL_CONTEXT_FLAG_FORWARD_COMPATIBLE_BIT 0x0001
+#define GL_RGBA32F 0x8814
+#define GL_RGB32F 0x8815
+#define GL_RGBA16F 0x881A
+#define GL_RGB16F 0x881B
+#define GL_VERTEX_ATTRIB_ARRAY_INTEGER 0x88FD
+#define GL_MAX_ARRAY_TEXTURE_LAYERS 0x88FF
+#define GL_MIN_PROGRAM_TEXEL_OFFSET 0x8904
+#define GL_MAX_PROGRAM_TEXEL_OFFSET 0x8905
+#define GL_CLAMP_READ_COLOR 0x891C
+#define GL_FIXED_ONLY 0x891D
+#define GL_MAX_VARYING_COMPONENTS 0x8B4B
+#define GL_TEXTURE_1D_ARRAY 0x8C18
+#define GL_PROXY_TEXTURE_1D_ARRAY 0x8C19
+#define GL_TEXTURE_2D_ARRAY 0x8C1A
+#define GL_PROXY_TEXTURE_2D_ARRAY 0x8C1B
+#define GL_TEXTURE_BINDING_1D_ARRAY 0x8C1C
+#define GL_TEXTURE_BINDING_2D_ARRAY 0x8C1D
+#define GL_R11F_G11F_B10F 0x8C3A
+#define GL_UNSIGNED_INT_10F_11F_11F_REV 0x8C3B
+#define GL_RGB9_E5 0x8C3D
+#define GL_UNSIGNED_INT_5_9_9_9_REV 0x8C3E
+#define GL_TEXTURE_SHARED_SIZE 0x8C3F
+#define GL_TRANSFORM_FEEDBACK_VARYING_MAX_LENGTH 0x8C76
+#define GL_TRANSFORM_FEEDBACK_BUFFER_MODE 0x8C7F
+#define GL_MAX_TRANSFORM_FEEDBACK_SEPARATE_COMPONENTS 0x8C80
+#define GL_TRANSFORM_FEEDBACK_VARYINGS 0x8C83
+#define GL_TRANSFORM_FEEDBACK_BUFFER_START 0x8C84
+#define GL_TRANSFORM_FEEDBACK_BUFFER_SIZE 0x8C85
+#define GL_PRIMITIVES_GENERATED 0x8C87
+#define GL_TRANSFORM_FEEDBACK_PRIMITIVES_WRITTEN 0x8C88
+#define GL_RASTERIZER_DISCARD 0x8C89
+#define GL_MAX_TRANSFORM_FEEDBACK_INTERLEAVED_COMPONENTS 0x8C8A
+#define GL_MAX_TRANSFORM_FEEDBACK_SEPARATE_ATTRIBS 0x8C8B
+#define GL_INTERLEAVED_ATTRIBS 0x8C8C
+#define GL_SEPARATE_ATTRIBS 0x8C8D
+#define GL_TRANSFORM_FEEDBACK_BUFFER 0x8C8E
+#define GL_TRANSFORM_FEEDBACK_BUFFER_BINDING 0x8C8F
+#define GL_RGBA32UI 0x8D70
+#define GL_RGB32UI 0x8D71
+#define GL_RGBA16UI 0x8D76
+#define GL_RGB16UI 0x8D77
+#define GL_RGBA8UI 0x8D7C
+#define GL_RGB8UI 0x8D7D
+#define GL_RGBA32I 0x8D82
+#define GL_RGB32I 0x8D83
+#define GL_RGBA16I 0x8D88
+#define GL_RGB16I 0x8D89
+#define GL_RGBA8I 0x8D8E
+#define GL_RGB8I 0x8D8F
+#define GL_RED_INTEGER 0x8D94
+#define GL_GREEN_INTEGER 0x8D95
+#define GL_BLUE_INTEGER 0x8D96
+#define GL_RGB_INTEGER 0x8D98
+#define GL_RGBA_INTEGER 0x8D99
+#define GL_BGR_INTEGER 0x8D9A
+#define GL_BGRA_INTEGER 0x8D9B
+#define GL_SAMPLER_1D_ARRAY 0x8DC0
+#define GL_SAMPLER_2D_ARRAY 0x8DC1
+#define GL_SAMPLER_1D_ARRAY_SHADOW 0x8DC3
+#define GL_SAMPLER_2D_ARRAY_SHADOW 0x8DC4
+#define GL_SAMPLER_CUBE_SHADOW 0x8DC5
+#define GL_UNSIGNED_INT_VEC2 0x8DC6
+#define GL_UNSIGNED_INT_VEC3 0x8DC7
+#define GL_UNSIGNED_INT_VEC4 0x8DC8
+#define GL_INT_SAMPLER_1D 0x8DC9
+#define GL_INT_SAMPLER_2D 0x8DCA
+#define GL_INT_SAMPLER_3D 0x8DCB
+#define GL_INT_SAMPLER_CUBE 0x8DCC
+#define GL_INT_SAMPLER_1D_ARRAY 0x8DCE
+#define GL_INT_SAMPLER_2D_ARRAY 0x8DCF
+#define GL_UNSIGNED_INT_SAMPLER_1D 0x8DD1
+#define GL_UNSIGNED_INT_SAMPLER_2D 0x8DD2
+#define GL_UNSIGNED_INT_SAMPLER_3D 0x8DD3
+#define GL_UNSIGNED_INT_SAMPLER_CUBE 0x8DD4
+#define GL_UNSIGNED_INT_SAMPLER_1D_ARRAY 0x8DD6
+#define GL_UNSIGNED_INT_SAMPLER_2D_ARRAY 0x8DD7
+#define GL_QUERY_WAIT 0x8E13
+#define GL_QUERY_NO_WAIT 0x8E14
+#define GL_QUERY_BY_REGION_WAIT 0x8E15
+#define GL_QUERY_BY_REGION_NO_WAIT 0x8E16
+#define GL_BUFFER_ACCESS_FLAGS 0x911F
+#define GL_BUFFER_MAP_LENGTH 0x9120
+#define GL_BUFFER_MAP_OFFSET 0x9121
+/* Reuse tokens from ARB_depth_buffer_float */
+/* reuse GL_DEPTH_COMPONENT32F */
+/* reuse GL_DEPTH32F_STENCIL8 */
+/* reuse GL_FLOAT_32_UNSIGNED_INT_24_8_REV */
+/* Reuse tokens from ARB_framebuffer_object */
+/* reuse GL_INVALID_FRAMEBUFFER_OPERATION */
+/* reuse GL_FRAMEBUFFER_ATTACHMENT_COLOR_ENCODING */
+/* reuse GL_FRAMEBUFFER_ATTACHMENT_COMPONENT_TYPE */
+/* reuse GL_FRAMEBUFFER_ATTACHMENT_RED_SIZE */
+/* reuse GL_FRAMEBUFFER_ATTACHMENT_GREEN_SIZE */
+/* reuse GL_FRAMEBUFFER_ATTACHMENT_BLUE_SIZE */
+/* reuse GL_FRAMEBUFFER_ATTACHMENT_ALPHA_SIZE */
+/* reuse GL_FRAMEBUFFER_ATTACHMENT_DEPTH_SIZE */
+/* reuse GL_FRAMEBUFFER_ATTACHMENT_STENCIL_SIZE */
+/* reuse GL_FRAMEBUFFER_DEFAULT */
+/* reuse GL_FRAMEBUFFER_UNDEFINED */
+/* reuse GL_DEPTH_STENCIL_ATTACHMENT */
+/* reuse GL_INDEX */
+/* reuse GL_MAX_RENDERBUFFER_SIZE */
+/* reuse GL_DEPTH_STENCIL */
+/* reuse GL_UNSIGNED_INT_24_8 */
+/* reuse GL_DEPTH24_STENCIL8 */
+/* reuse GL_TEXTURE_STENCIL_SIZE */
+/* reuse GL_TEXTURE_RED_TYPE */
+/* reuse GL_TEXTURE_GREEN_TYPE */
+/* reuse GL_TEXTURE_BLUE_TYPE */
+/* reuse GL_TEXTURE_ALPHA_TYPE */
+/* reuse GL_TEXTURE_DEPTH_TYPE */
+/* reuse GL_UNSIGNED_NORMALIZED */
+/* reuse GL_FRAMEBUFFER_BINDING */
+/* reuse GL_DRAW_FRAMEBUFFER_BINDING */
+/* reuse GL_RENDERBUFFER_BINDING */
+/* reuse GL_READ_FRAMEBUFFER */
+/* reuse GL_DRAW_FRAMEBUFFER */
+/* reuse GL_READ_FRAMEBUFFER_BINDING */
+/* reuse GL_RENDERBUFFER_SAMPLES */
+/* reuse GL_FRAMEBUFFER_ATTACHMENT_OBJECT_TYPE */
+/* reuse GL_FRAMEBUFFER_ATTACHMENT_OBJECT_NAME */
+/* reuse GL_FRAMEBUFFER_ATTACHMENT_TEXTURE_LEVEL */
+/* reuse GL_FRAMEBUFFER_ATTACHMENT_TEXTURE_CUBE_MAP_FACE */
+/* reuse GL_FRAMEBUFFER_ATTACHMENT_TEXTURE_LAYER */
+/* reuse GL_FRAMEBUFFER_COMPLETE */
+/* reuse GL_FRAMEBUFFER_INCOMPLETE_ATTACHMENT */
+/* reuse GL_FRAMEBUFFER_INCOMPLETE_MISSING_ATTACHMENT */
+/* reuse GL_FRAMEBUFFER_INCOMPLETE_DRAW_BUFFER */
+/* reuse GL_FRAMEBUFFER_INCOMPLETE_READ_BUFFER */
+/* reuse GL_FRAMEBUFFER_UNSUPPORTED */
+/* reuse GL_MAX_COLOR_ATTACHMENTS */
+/* reuse GL_COLOR_ATTACHMENT0 */
+/* reuse GL_COLOR_ATTACHMENT1 */
+/* reuse GL_COLOR_ATTACHMENT2 */
+/* reuse GL_COLOR_ATTACHMENT3 */
+/* reuse GL_COLOR_ATTACHMENT4 */
+/* reuse GL_COLOR_ATTACHMENT5 */
+/* reuse GL_COLOR_ATTACHMENT6 */
+/* reuse GL_COLOR_ATTACHMENT7 */
+/* reuse GL_COLOR_ATTACHMENT8 */
+/* reuse GL_COLOR_ATTACHMENT9 */
+/* reuse GL_COLOR_ATTACHMENT10 */
+/* reuse GL_COLOR_ATTACHMENT11 */
+/* reuse GL_COLOR_ATTACHMENT12 */
+/* reuse GL_COLOR_ATTACHMENT13 */
+/* reuse GL_COLOR_ATTACHMENT14 */
+/* reuse GL_COLOR_ATTACHMENT15 */
+/* reuse GL_DEPTH_ATTACHMENT */
+/* reuse GL_STENCIL_ATTACHMENT */
+/* reuse GL_FRAMEBUFFER */
+/* reuse GL_RENDERBUFFER */
+/* reuse GL_RENDERBUFFER_WIDTH */
+/* reuse GL_RENDERBUFFER_HEIGHT */
+/* reuse GL_RENDERBUFFER_INTERNAL_FORMAT */
+/* reuse GL_STENCIL_INDEX1 */
+/* reuse GL_STENCIL_INDEX4 */
+/* reuse GL_STENCIL_INDEX8 */
+/* reuse GL_STENCIL_INDEX16 */
+/* reuse GL_RENDERBUFFER_RED_SIZE */
+/* reuse GL_RENDERBUFFER_GREEN_SIZE */
+/* reuse GL_RENDERBUFFER_BLUE_SIZE */
+/* reuse GL_RENDERBUFFER_ALPHA_SIZE */
+/* reuse GL_RENDERBUFFER_DEPTH_SIZE */
+/* reuse GL_RENDERBUFFER_STENCIL_SIZE */
+/* reuse GL_FRAMEBUFFER_INCOMPLETE_MULTISAMPLE */
+/* reuse GL_MAX_SAMPLES */
+/* Reuse tokens from ARB_framebuffer_sRGB */
+/* reuse GL_FRAMEBUFFER_SRGB */
+/* Reuse tokens from ARB_half_float_vertex */
+/* reuse GL_HALF_FLOAT */
+/* Reuse tokens from ARB_map_buffer_range */
+/* reuse GL_MAP_READ_BIT */
+/* reuse GL_MAP_WRITE_BIT */
+/* reuse GL_MAP_INVALIDATE_RANGE_BIT */
+/* reuse GL_MAP_INVALIDATE_BUFFER_BIT */
+/* reuse GL_MAP_FLUSH_EXPLICIT_BIT */
+/* reuse GL_MAP_UNSYNCHRONIZED_BIT */
+/* Reuse tokens from ARB_texture_compression_rgtc */
+/* reuse GL_COMPRESSED_RED_RGTC1 */
+/* reuse GL_COMPRESSED_SIGNED_RED_RGTC1 */
+/* reuse GL_COMPRESSED_RG_RGTC2 */
+/* reuse GL_COMPRESSED_SIGNED_RG_RGTC2 */
+/* Reuse tokens from ARB_texture_rg */
+/* reuse GL_RG */
+/* reuse GL_RG_INTEGER */
+/* reuse GL_R8 */
+/* reuse GL_R16 */
+/* reuse GL_RG8 */
+/* reuse GL_RG16 */
+/* reuse GL_R16F */
+/* reuse GL_R32F */
+/* reuse GL_RG16F */
+/* reuse GL_RG32F */
+/* reuse GL_R8I */
+/* reuse GL_R8UI */
+/* reuse GL_R16I */
+/* reuse GL_R16UI */
+/* reuse GL_R32I */
+/* reuse GL_R32UI */
+/* reuse GL_RG8I */
+/* reuse GL_RG8UI */
+/* reuse GL_RG16I */
+/* reuse GL_RG16UI */
+/* reuse GL_RG32I */
+/* reuse GL_RG32UI */
+/* Reuse tokens from ARB_vertex_array_object */
+/* reuse GL_VERTEX_ARRAY_BINDING */
+#endif
+
+#ifndef GL_VERSION_3_0_DEPRECATED
+#define GL_CLAMP_VERTEX_COLOR 0x891A
+#define GL_CLAMP_FRAGMENT_COLOR 0x891B
+#define GL_ALPHA_INTEGER 0x8D97
+/* Reuse tokens from ARB_framebuffer_object */
+/* reuse GL_TEXTURE_LUMINANCE_TYPE */
+/* reuse GL_TEXTURE_INTENSITY_TYPE */
+#endif
+
+#ifndef GL_VERSION_3_1
+#define GL_SAMPLER_2D_RECT 0x8B63
+#define GL_SAMPLER_2D_RECT_SHADOW 0x8B64
+#define GL_SAMPLER_BUFFER 0x8DC2
+#define GL_INT_SAMPLER_2D_RECT 0x8DCD
+#define GL_INT_SAMPLER_BUFFER 0x8DD0
+#define GL_UNSIGNED_INT_SAMPLER_2D_RECT 0x8DD5
+#define GL_UNSIGNED_INT_SAMPLER_BUFFER 0x8DD8
+#define GL_TEXTURE_BUFFER 0x8C2A
+#define GL_MAX_TEXTURE_BUFFER_SIZE 0x8C2B
+#define GL_TEXTURE_BINDING_BUFFER 0x8C2C
+#define GL_TEXTURE_BUFFER_DATA_STORE_BINDING 0x8C2D
+#define GL_TEXTURE_BUFFER_FORMAT 0x8C2E
+#define GL_TEXTURE_RECTANGLE 0x84F5
+#define GL_TEXTURE_BINDING_RECTANGLE 0x84F6
+#define GL_PROXY_TEXTURE_RECTANGLE 0x84F7
+#define GL_MAX_RECTANGLE_TEXTURE_SIZE 0x84F8
+#define GL_RED_SNORM 0x8F90
+#define GL_RG_SNORM 0x8F91
+#define GL_RGB_SNORM 0x8F92
+#define GL_RGBA_SNORM 0x8F93
+#define GL_R8_SNORM 0x8F94
+#define GL_RG8_SNORM 0x8F95
+#define GL_RGB8_SNORM 0x8F96
+#define GL_RGBA8_SNORM 0x8F97
+#define GL_R16_SNORM 0x8F98
+#define GL_RG16_SNORM 0x8F99
+#define GL_RGB16_SNORM 0x8F9A
+#define GL_RGBA16_SNORM 0x8F9B
+#define GL_SIGNED_NORMALIZED 0x8F9C
+#define GL_PRIMITIVE_RESTART 0x8F9D
+#define GL_PRIMITIVE_RESTART_INDEX 0x8F9E
+/* Reuse tokens from ARB_copy_buffer */
+/* reuse GL_COPY_READ_BUFFER */
+/* reuse GL_COPY_WRITE_BUFFER */
+/* Reuse tokens from ARB_draw_instanced (none) */
+/* Reuse tokens from ARB_uniform_buffer_object */
+/* reuse GL_UNIFORM_BUFFER */
+/* reuse GL_UNIFORM_BUFFER_BINDING */
+/* reuse GL_UNIFORM_BUFFER_START */
+/* reuse GL_UNIFORM_BUFFER_SIZE */
+/* reuse GL_MAX_VERTEX_UNIFORM_BLOCKS */
+/* reuse GL_MAX_FRAGMENT_UNIFORM_BLOCKS */
+/* reuse GL_MAX_COMBINED_UNIFORM_BLOCKS */
+/* reuse GL_MAX_UNIFORM_BUFFER_BINDINGS */
+/* reuse GL_MAX_UNIFORM_BLOCK_SIZE */
+/* reuse GL_MAX_COMBINED_VERTEX_UNIFORM_COMPONENTS */
+/* reuse GL_MAX_COMBINED_FRAGMENT_UNIFORM_COMPONENTS */
+/* reuse GL_UNIFORM_BUFFER_OFFSET_ALIGNMENT */
+/* reuse GL_ACTIVE_UNIFORM_BLOCK_MAX_NAME_LENGTH */
+/* reuse GL_ACTIVE_UNIFORM_BLOCKS */
+/* reuse GL_UNIFORM_TYPE */
+/* reuse GL_UNIFORM_SIZE */
+/* reuse GL_UNIFORM_NAME_LENGTH */
+/* reuse GL_UNIFORM_BLOCK_INDEX */
+/* reuse GL_UNIFORM_OFFSET */
+/* reuse GL_UNIFORM_ARRAY_STRIDE */
+/* reuse GL_UNIFORM_MATRIX_STRIDE */
+/* reuse GL_UNIFORM_IS_ROW_MAJOR */
+/* reuse GL_UNIFORM_BLOCK_BINDING */
+/* reuse GL_UNIFORM_BLOCK_DATA_SIZE */
+/* reuse GL_UNIFORM_BLOCK_NAME_LENGTH */
+/* reuse GL_UNIFORM_BLOCK_ACTIVE_UNIFORMS */
+/* reuse GL_UNIFORM_BLOCK_ACTIVE_UNIFORM_INDICES */
+/* reuse GL_UNIFORM_BLOCK_REFERENCED_BY_VERTEX_SHADER */
+/* reuse GL_UNIFORM_BLOCK_REFERENCED_BY_FRAGMENT_SHADER */
+/* reuse GL_INVALID_INDEX */
+#endif
+
+#ifndef GL_VERSION_3_2
+#define GL_CONTEXT_CORE_PROFILE_BIT 0x00000001
+#define GL_CONTEXT_COMPATIBILITY_PROFILE_BIT 0x00000002
+#define GL_LINES_ADJACENCY 0x000A
+#define GL_LINE_STRIP_ADJACENCY 0x000B
+#define GL_TRIANGLES_ADJACENCY 0x000C
+#define GL_TRIANGLE_STRIP_ADJACENCY 0x000D
+#define GL_PROGRAM_POINT_SIZE 0x8642
+#define GL_MAX_GEOMETRY_TEXTURE_IMAGE_UNITS 0x8C29
+#define GL_FRAMEBUFFER_ATTACHMENT_LAYERED 0x8DA7
+#define GL_FRAMEBUFFER_INCOMPLETE_LAYER_TARGETS 0x8DA8
+#define GL_GEOMETRY_SHADER 0x8DD9
+#define GL_GEOMETRY_VERTICES_OUT 0x8916
+#define GL_GEOMETRY_INPUT_TYPE 0x8917
+#define GL_GEOMETRY_OUTPUT_TYPE 0x8918
+#define GL_MAX_GEOMETRY_UNIFORM_COMPONENTS 0x8DDF
+#define GL_MAX_GEOMETRY_OUTPUT_VERTICES 0x8DE0
+#define GL_MAX_GEOMETRY_TOTAL_OUTPUT_COMPONENTS 0x8DE1
+#define GL_MAX_VERTEX_OUTPUT_COMPONENTS 0x9122
+#define GL_MAX_GEOMETRY_INPUT_COMPONENTS 0x9123
+#define GL_MAX_GEOMETRY_OUTPUT_COMPONENTS 0x9124
+#define GL_MAX_FRAGMENT_INPUT_COMPONENTS 0x9125
+#define GL_CONTEXT_PROFILE_MASK 0x9126
+/* reuse GL_MAX_VARYING_COMPONENTS */
+/* reuse GL_FRAMEBUFFER_ATTACHMENT_TEXTURE_LAYER */
+/* Reuse tokens from ARB_depth_clamp */
+/* reuse GL_DEPTH_CLAMP */
+/* Reuse tokens from ARB_draw_elements_base_vertex (none) */
+/* Reuse tokens from ARB_fragment_coord_conventions (none) */
+/* Reuse tokens from ARB_provoking_vertex */
+/* reuse GL_QUADS_FOLLOW_PROVOKING_VERTEX_CONVENTION */
+/* reuse GL_FIRST_VERTEX_CONVENTION */
+/* reuse GL_LAST_VERTEX_CONVENTION */
+/* reuse GL_PROVOKING_VERTEX */
+/* Reuse tokens from ARB_seamless_cube_map */
+/* reuse GL_TEXTURE_CUBE_MAP_SEAMLESS */
+/* Reuse tokens from ARB_sync */
+/* reuse GL_MAX_SERVER_WAIT_TIMEOUT */
+/* reuse GL_OBJECT_TYPE */
+/* reuse GL_SYNC_CONDITION */
+/* reuse GL_SYNC_STATUS */
+/* reuse GL_SYNC_FLAGS */
+/* reuse GL_SYNC_FENCE */
+/* reuse GL_SYNC_GPU_COMMANDS_COMPLETE */
+/* reuse GL_UNSIGNALED */
+/* reuse GL_SIGNALED */
+/* reuse GL_ALREADY_SIGNALED */
+/* reuse GL_TIMEOUT_EXPIRED */
+/* reuse GL_CONDITION_SATISFIED */
+/* reuse GL_WAIT_FAILED */
+/* reuse GL_TIMEOUT_IGNORED */
+/* reuse GL_SYNC_FLUSH_COMMANDS_BIT */
+/* reuse GL_TIMEOUT_IGNORED */
+/* Reuse tokens from ARB_texture_multisample */
+/* reuse GL_SAMPLE_POSITION */
+/* reuse GL_SAMPLE_MASK */
+/* reuse GL_SAMPLE_MASK_VALUE */
+/* reuse GL_MAX_SAMPLE_MASK_WORDS */
+/* reuse GL_TEXTURE_2D_MULTISAMPLE */
+/* reuse GL_PROXY_TEXTURE_2D_MULTISAMPLE */
+/* reuse GL_TEXTURE_2D_MULTISAMPLE_ARRAY */
+/* reuse GL_PROXY_TEXTURE_2D_MULTISAMPLE_ARRAY */
+/* reuse GL_TEXTURE_BINDING_2D_MULTISAMPLE */
+/* reuse GL_TEXTURE_BINDING_2D_MULTISAMPLE_ARRAY */
+/* reuse GL_TEXTURE_SAMPLES */
+/* reuse GL_TEXTURE_FIXED_SAMPLE_LOCATIONS */
+/* reuse GL_SAMPLER_2D_MULTISAMPLE */
+/* reuse GL_INT_SAMPLER_2D_MULTISAMPLE */
+/* reuse GL_UNSIGNED_INT_SAMPLER_2D_MULTISAMPLE */
+/* reuse GL_SAMPLER_2D_MULTISAMPLE_ARRAY */
+/* reuse GL_INT_SAMPLER_2D_MULTISAMPLE_ARRAY */
+/* reuse GL_UNSIGNED_INT_SAMPLER_2D_MULTISAMPLE_ARRAY */
+/* reuse GL_MAX_COLOR_TEXTURE_SAMPLES */
+/* reuse GL_MAX_DEPTH_TEXTURE_SAMPLES */
+/* reuse GL_MAX_INTEGER_SAMPLES */
+/* Don't need to reuse tokens from ARB_vertex_array_bgra since they're already in 1.2 core */
+#endif
+
+#ifndef GL_VERSION_3_3
+#define GL_VERTEX_ATTRIB_ARRAY_DIVISOR 0x88FE
+/* Reuse tokens from ARB_blend_func_extended */
+/* reuse GL_SRC1_COLOR */
+/* reuse GL_ONE_MINUS_SRC1_COLOR */
+/* reuse GL_ONE_MINUS_SRC1_ALPHA */
+/* reuse GL_MAX_DUAL_SOURCE_DRAW_BUFFERS */
+/* Reuse tokens from ARB_explicit_attrib_location (none) */
+/* Reuse tokens from ARB_occlusion_query2 */
+/* reuse GL_ANY_SAMPLES_PASSED */
+/* Reuse tokens from ARB_sampler_objects */
+/* reuse GL_SAMPLER_BINDING */
+/* Reuse tokens from ARB_shader_bit_encoding (none) */
+/* Reuse tokens from ARB_texture_rgb10_a2ui */
+/* reuse GL_RGB10_A2UI */
+/* Reuse tokens from ARB_texture_swizzle */
+/* reuse GL_TEXTURE_SWIZZLE_R */
+/* reuse GL_TEXTURE_SWIZZLE_G */
+/* reuse GL_TEXTURE_SWIZZLE_B */
+/* reuse GL_TEXTURE_SWIZZLE_A */
+/* reuse GL_TEXTURE_SWIZZLE_RGBA */
+/* Reuse tokens from ARB_timer_query */
+/* reuse GL_TIME_ELAPSED */
+/* reuse GL_TIMESTAMP */
+/* Reuse tokens from ARB_vertex_type_2_10_10_10_rev */
+/* reuse GL_INT_2_10_10_10_REV */
+#endif
+
+#ifndef GL_VERSION_4_0
+#define GL_SAMPLE_SHADING 0x8C36
+#define GL_MIN_SAMPLE_SHADING_VALUE 0x8C37
+#define GL_MIN_PROGRAM_TEXTURE_GATHER_OFFSET 0x8E5E
+#define GL_MAX_PROGRAM_TEXTURE_GATHER_OFFSET 0x8E5F
+#define GL_TEXTURE_CUBE_MAP_ARRAY 0x9009
+#define GL_TEXTURE_BINDING_CUBE_MAP_ARRAY 0x900A
+#define GL_PROXY_TEXTURE_CUBE_MAP_ARRAY 0x900B
+#define GL_SAMPLER_CUBE_MAP_ARRAY 0x900C
+#define GL_SAMPLER_CUBE_MAP_ARRAY_SHADOW 0x900D
+#define GL_INT_SAMPLER_CUBE_MAP_ARRAY 0x900E
+#define GL_UNSIGNED_INT_SAMPLER_CUBE_MAP_ARRAY 0x900F
+/* Reuse tokens from ARB_texture_query_lod (none) */
+/* Reuse tokens from ARB_draw_buffers_blend (none) */
+/* Reuse tokens from ARB_draw_indirect */
+/* reuse GL_DRAW_INDIRECT_BUFFER */
+/* reuse GL_DRAW_INDIRECT_BUFFER_BINDING */
+/* Reuse tokens from ARB_gpu_shader5 */
+/* reuse GL_GEOMETRY_SHADER_INVOCATIONS */
+/* reuse GL_MAX_GEOMETRY_SHADER_INVOCATIONS */
+/* reuse GL_MIN_FRAGMENT_INTERPOLATION_OFFSET */
+/* reuse GL_MAX_FRAGMENT_INTERPOLATION_OFFSET */
+/* reuse GL_FRAGMENT_INTERPOLATION_OFFSET_BITS */
+/* reuse GL_MAX_VERTEX_STREAMS */
+/* Reuse tokens from ARB_gpu_shader_fp64 */
+/* reuse GL_DOUBLE_VEC2 */
+/* reuse GL_DOUBLE_VEC3 */
+/* reuse GL_DOUBLE_VEC4 */
+/* reuse GL_DOUBLE_MAT2 */
+/* reuse GL_DOUBLE_MAT3 */
+/* reuse GL_DOUBLE_MAT4 */
+/* reuse GL_DOUBLE_MAT2x3 */
+/* reuse GL_DOUBLE_MAT2x4 */
+/* reuse GL_DOUBLE_MAT3x2 */
+/* reuse GL_DOUBLE_MAT3x4 */
+/* reuse GL_DOUBLE_MAT4x2 */
+/* reuse GL_DOUBLE_MAT4x3 */
+/* Reuse tokens from ARB_shader_subroutine */
+/* reuse GL_ACTIVE_SUBROUTINES */
+/* reuse GL_ACTIVE_SUBROUTINE_UNIFORMS */
+/* reuse GL_ACTIVE_SUBROUTINE_UNIFORM_LOCATIONS */
+/* reuse GL_ACTIVE_SUBROUTINE_MAX_LENGTH */
+/* reuse GL_ACTIVE_SUBROUTINE_UNIFORM_MAX_LENGTH */
+/* reuse GL_MAX_SUBROUTINES */
+/* reuse GL_MAX_SUBROUTINE_UNIFORM_LOCATIONS */
+/* reuse GL_NUM_COMPATIBLE_SUBROUTINES */
+/* reuse GL_COMPATIBLE_SUBROUTINES */
+/* Reuse tokens from ARB_tessellation_shader */
+/* reuse GL_PATCHES */
+/* reuse GL_PATCH_VERTICES */
+/* reuse GL_PATCH_DEFAULT_INNER_LEVEL */
+/* reuse GL_PATCH_DEFAULT_OUTER_LEVEL */
+/* reuse GL_TESS_CONTROL_OUTPUT_VERTICES */
+/* reuse GL_TESS_GEN_MODE */
+/* reuse GL_TESS_GEN_SPACING */
+/* reuse GL_TESS_GEN_VERTEX_ORDER */
+/* reuse GL_TESS_GEN_POINT_MODE */
+/* reuse GL_ISOLINES */
+/* reuse GL_FRACTIONAL_ODD */
+/* reuse GL_FRACTIONAL_EVEN */
+/* reuse GL_MAX_PATCH_VERTICES */
+/* reuse GL_MAX_TESS_GEN_LEVEL */
+/* reuse GL_MAX_TESS_CONTROL_UNIFORM_COMPONENTS */
+/* reuse GL_MAX_TESS_EVALUATION_UNIFORM_COMPONENTS */
+/* reuse GL_MAX_TESS_CONTROL_TEXTURE_IMAGE_UNITS */
+/* reuse GL_MAX_TESS_EVALUATION_TEXTURE_IMAGE_UNITS */
+/* reuse GL_MAX_TESS_CONTROL_OUTPUT_COMPONENTS */
+/* reuse GL_MAX_TESS_PATCH_COMPONENTS */
+/* reuse GL_MAX_TESS_CONTROL_TOTAL_OUTPUT_COMPONENTS */
+/* reuse GL_MAX_TESS_EVALUATION_OUTPUT_COMPONENTS */
+/* reuse GL_MAX_TESS_CONTROL_UNIFORM_BLOCKS */
+/* reuse GL_MAX_TESS_EVALUATION_UNIFORM_BLOCKS */
+/* reuse GL_MAX_TESS_CONTROL_INPUT_COMPONENTS */
+/* reuse GL_MAX_TESS_EVALUATION_INPUT_COMPONENTS */
+/* reuse GL_MAX_COMBINED_TESS_CONTROL_UNIFORM_COMPONENTS */
+/* reuse GL_MAX_COMBINED_TESS_EVALUATION_UNIFORM_COMPONENTS */
+/* reuse GL_UNIFORM_BLOCK_REFERENCED_BY_TESS_CONTROL_SHADER */
+/* reuse GL_UNIFORM_BLOCK_REFERENCED_BY_TESS_EVALUATION_SHADER */
+/* reuse GL_TESS_EVALUATION_SHADER */
+/* reuse GL_TESS_CONTROL_SHADER */
+/* Reuse tokens from ARB_texture_buffer_object_rgb32 (none) */
+/* Reuse tokens from ARB_transform_feedback2 */
+/* reuse GL_TRANSFORM_FEEDBACK */
+/* reuse GL_TRANSFORM_FEEDBACK_BUFFER_PAUSED */
+/* reuse GL_TRANSFORM_FEEDBACK_BUFFER_ACTIVE */
+/* reuse GL_TRANSFORM_FEEDBACK_BINDING */
+/* Reuse tokens from ARB_transform_feedback3 */
+/* reuse GL_MAX_TRANSFORM_FEEDBACK_BUFFERS */
+/* reuse GL_MAX_VERTEX_STREAMS */
+#endif
+
+#ifndef GL_VERSION_4_1
+/* Reuse tokens from ARB_ES2_compatibility */
+/* reuse GL_FIXED */
+/* reuse GL_IMPLEMENTATION_COLOR_READ_TYPE */
+/* reuse GL_IMPLEMENTATION_COLOR_READ_FORMAT */
+/* reuse GL_LOW_FLOAT */
+/* reuse GL_MEDIUM_FLOAT */
+/* reuse GL_HIGH_FLOAT */
+/* reuse GL_LOW_INT */
+/* reuse GL_MEDIUM_INT */
+/* reuse GL_HIGH_INT */
+/* reuse GL_SHADER_COMPILER */
+/* reuse GL_NUM_SHADER_BINARY_FORMATS */
+/* reuse GL_MAX_VERTEX_UNIFORM_VECTORS */
+/* reuse GL_MAX_VARYING_VECTORS */
+/* reuse GL_MAX_FRAGMENT_UNIFORM_VECTORS */
+/* Reuse tokens from ARB_get_program_binary */
+/* reuse GL_PROGRAM_BINARY_RETRIEVABLE_HINT */
+/* reuse GL_PROGRAM_BINARY_LENGTH */
+/* reuse GL_NUM_PROGRAM_BINARY_FORMATS */
+/* reuse GL_PROGRAM_BINARY_FORMATS */
+/* Reuse tokens from ARB_separate_shader_objects */
+/* reuse GL_VERTEX_SHADER_BIT */
+/* reuse GL_FRAGMENT_SHADER_BIT */
+/* reuse GL_GEOMETRY_SHADER_BIT */
+/* reuse GL_TESS_CONTROL_SHADER_BIT */
+/* reuse GL_TESS_EVALUATION_SHADER_BIT */
+/* reuse GL_ALL_SHADER_BITS */
+/* reuse GL_PROGRAM_SEPARABLE */
+/* reuse GL_ACTIVE_PROGRAM */
+/* reuse GL_PROGRAM_PIPELINE_BINDING */
+/* Reuse tokens from ARB_shader_precision (none) */
+/* Reuse tokens from ARB_vertex_attrib_64bit - all are in GL 3.0 and 4.0 already */
+/* Reuse tokens from ARB_viewport_array - some are in GL 1.1 and ARB_provoking_vertex already */
+/* reuse GL_MAX_VIEWPORTS */
+/* reuse GL_VIEWPORT_SUBPIXEL_BITS */
+/* reuse GL_VIEWPORT_BOUNDS_RANGE */
+/* reuse GL_LAYER_PROVOKING_VERTEX */
+/* reuse GL_VIEWPORT_INDEX_PROVOKING_VERTEX */
+/* reuse GL_UNDEFINED_VERTEX */
+#endif
+
+#ifndef GL_ARB_multitexture
+#define GL_TEXTURE0_ARB 0x84C0
+#define GL_TEXTURE1_ARB 0x84C1
+#define GL_TEXTURE2_ARB 0x84C2
+#define GL_TEXTURE3_ARB 0x84C3
+#define GL_TEXTURE4_ARB 0x84C4
+#define GL_TEXTURE5_ARB 0x84C5
+#define GL_TEXTURE6_ARB 0x84C6
+#define GL_TEXTURE7_ARB 0x84C7
+#define GL_TEXTURE8_ARB 0x84C8
+#define GL_TEXTURE9_ARB 0x84C9
+#define GL_TEXTURE10_ARB 0x84CA
+#define GL_TEXTURE11_ARB 0x84CB
+#define GL_TEXTURE12_ARB 0x84CC
+#define GL_TEXTURE13_ARB 0x84CD
+#define GL_TEXTURE14_ARB 0x84CE
+#define GL_TEXTURE15_ARB 0x84CF
+#define GL_TEXTURE16_ARB 0x84D0
+#define GL_TEXTURE17_ARB 0x84D1
+#define GL_TEXTURE18_ARB 0x84D2
+#define GL_TEXTURE19_ARB 0x84D3
+#define GL_TEXTURE20_ARB 0x84D4
+#define GL_TEXTURE21_ARB 0x84D5
+#define GL_TEXTURE22_ARB 0x84D6
+#define GL_TEXTURE23_ARB 0x84D7
+#define GL_TEXTURE24_ARB 0x84D8
+#define GL_TEXTURE25_ARB 0x84D9
+#define GL_TEXTURE26_ARB 0x84DA
+#define GL_TEXTURE27_ARB 0x84DB
+#define GL_TEXTURE28_ARB 0x84DC
+#define GL_TEXTURE29_ARB 0x84DD
+#define GL_TEXTURE30_ARB 0x84DE
+#define GL_TEXTURE31_ARB 0x84DF
+#define GL_ACTIVE_TEXTURE_ARB 0x84E0
+#define GL_CLIENT_ACTIVE_TEXTURE_ARB 0x84E1
+#define GL_MAX_TEXTURE_UNITS_ARB 0x84E2
+#endif
+
+#ifndef GL_ARB_transpose_matrix
+#define GL_TRANSPOSE_MODELVIEW_MATRIX_ARB 0x84E3
+#define GL_TRANSPOSE_PROJECTION_MATRIX_ARB 0x84E4
+#define GL_TRANSPOSE_TEXTURE_MATRIX_ARB 0x84E5
+#define GL_TRANSPOSE_COLOR_MATRIX_ARB 0x84E6
+#endif
+
+#ifndef GL_ARB_multisample
+#define GL_MULTISAMPLE_ARB 0x809D
+#define GL_SAMPLE_ALPHA_TO_COVERAGE_ARB 0x809E
+#define GL_SAMPLE_ALPHA_TO_ONE_ARB 0x809F
+#define GL_SAMPLE_COVERAGE_ARB 0x80A0
+#define GL_SAMPLE_BUFFERS_ARB 0x80A8
+#define GL_SAMPLES_ARB 0x80A9
+#define GL_SAMPLE_COVERAGE_VALUE_ARB 0x80AA
+#define GL_SAMPLE_COVERAGE_INVERT_ARB 0x80AB
+#define GL_MULTISAMPLE_BIT_ARB 0x20000000
+#endif
+
+#ifndef GL_ARB_texture_env_add
+#endif
+
+#ifndef GL_ARB_texture_cube_map
+#define GL_NORMAL_MAP_ARB 0x8511
+#define GL_REFLECTION_MAP_ARB 0x8512
+#define GL_TEXTURE_CUBE_MAP_ARB 0x8513
+#define GL_TEXTURE_BINDING_CUBE_MAP_ARB 0x8514
+#define GL_TEXTURE_CUBE_MAP_POSITIVE_X_ARB 0x8515
+#define GL_TEXTURE_CUBE_MAP_NEGATIVE_X_ARB 0x8516
+#define GL_TEXTURE_CUBE_MAP_POSITIVE_Y_ARB 0x8517
+#define GL_TEXTURE_CUBE_MAP_NEGATIVE_Y_ARB 0x8518
+#define GL_TEXTURE_CUBE_MAP_POSITIVE_Z_ARB 0x8519
+#define GL_TEXTURE_CUBE_MAP_NEGATIVE_Z_ARB 0x851A
+#define GL_PROXY_TEXTURE_CUBE_MAP_ARB 0x851B
+#define GL_MAX_CUBE_MAP_TEXTURE_SIZE_ARB 0x851C
+#endif
+
+#ifndef GL_ARB_texture_compression
+#define GL_COMPRESSED_ALPHA_ARB 0x84E9
+#define GL_COMPRESSED_LUMINANCE_ARB 0x84EA
+#define GL_COMPRESSED_LUMINANCE_ALPHA_ARB 0x84EB
+#define GL_COMPRESSED_INTENSITY_ARB 0x84EC
+#define GL_COMPRESSED_RGB_ARB 0x84ED
+#define GL_COMPRESSED_RGBA_ARB 0x84EE
+#define GL_TEXTURE_COMPRESSION_HINT_ARB 0x84EF
+#define GL_TEXTURE_COMPRESSED_IMAGE_SIZE_ARB 0x86A0
+#define GL_TEXTURE_COMPRESSED_ARB 0x86A1
+#define GL_NUM_COMPRESSED_TEXTURE_FORMATS_ARB 0x86A2
+#define GL_COMPRESSED_TEXTURE_FORMATS_ARB 0x86A3
+#endif
+
+#ifndef GL_ARB_texture_border_clamp
+#define GL_CLAMP_TO_BORDER_ARB 0x812D
+#endif
+
+#ifndef GL_ARB_point_parameters
+#define GL_POINT_SIZE_MIN_ARB 0x8126
+#define GL_POINT_SIZE_MAX_ARB 0x8127
+#define GL_POINT_FADE_THRESHOLD_SIZE_ARB 0x8128
+#define GL_POINT_DISTANCE_ATTENUATION_ARB 0x8129
+#endif
+
+#ifndef GL_ARB_vertex_blend
+#define GL_MAX_VERTEX_UNITS_ARB 0x86A4
+#define GL_ACTIVE_VERTEX_UNITS_ARB 0x86A5
+#define GL_WEIGHT_SUM_UNITY_ARB 0x86A6
+#define GL_VERTEX_BLEND_ARB 0x86A7
+#define GL_CURRENT_WEIGHT_ARB 0x86A8
+#define GL_WEIGHT_ARRAY_TYPE_ARB 0x86A9
+#define GL_WEIGHT_ARRAY_STRIDE_ARB 0x86AA
+#define GL_WEIGHT_ARRAY_SIZE_ARB 0x86AB
+#define GL_WEIGHT_ARRAY_POINTER_ARB 0x86AC
+#define GL_WEIGHT_ARRAY_ARB 0x86AD
+#define GL_MODELVIEW0_ARB 0x1700
+#define GL_MODELVIEW1_ARB 0x850A
+#define GL_MODELVIEW2_ARB 0x8722
+#define GL_MODELVIEW3_ARB 0x8723
+#define GL_MODELVIEW4_ARB 0x8724
+#define GL_MODELVIEW5_ARB 0x8725
+#define GL_MODELVIEW6_ARB 0x8726
+#define GL_MODELVIEW7_ARB 0x8727
+#define GL_MODELVIEW8_ARB 0x8728
+#define GL_MODELVIEW9_ARB 0x8729
+#define GL_MODELVIEW10_ARB 0x872A
+#define GL_MODELVIEW11_ARB 0x872B
+#define GL_MODELVIEW12_ARB 0x872C
+#define GL_MODELVIEW13_ARB 0x872D
+#define GL_MODELVIEW14_ARB 0x872E
+#define GL_MODELVIEW15_ARB 0x872F
+#define GL_MODELVIEW16_ARB 0x8730
+#define GL_MODELVIEW17_ARB 0x8731
+#define GL_MODELVIEW18_ARB 0x8732
+#define GL_MODELVIEW19_ARB 0x8733
+#define GL_MODELVIEW20_ARB 0x8734
+#define GL_MODELVIEW21_ARB 0x8735
+#define GL_MODELVIEW22_ARB 0x8736
+#define GL_MODELVIEW23_ARB 0x8737
+#define GL_MODELVIEW24_ARB 0x8738
+#define GL_MODELVIEW25_ARB 0x8739
+#define GL_MODELVIEW26_ARB 0x873A
+#define GL_MODELVIEW27_ARB 0x873B
+#define GL_MODELVIEW28_ARB 0x873C
+#define GL_MODELVIEW29_ARB 0x873D
+#define GL_MODELVIEW30_ARB 0x873E
+#define GL_MODELVIEW31_ARB 0x873F
+#endif
+
+#ifndef GL_ARB_matrix_palette
+#define GL_MATRIX_PALETTE_ARB 0x8840
+#define GL_MAX_MATRIX_PALETTE_STACK_DEPTH_ARB 0x8841
+#define GL_MAX_PALETTE_MATRICES_ARB 0x8842
+#define GL_CURRENT_PALETTE_MATRIX_ARB 0x8843
+#define GL_MATRIX_INDEX_ARRAY_ARB 0x8844
+#define GL_CURRENT_MATRIX_INDEX_ARB 0x8845
+#define GL_MATRIX_INDEX_ARRAY_SIZE_ARB 0x8846
+#define GL_MATRIX_INDEX_ARRAY_TYPE_ARB 0x8847
+#define GL_MATRIX_INDEX_ARRAY_STRIDE_ARB 0x8848
+#define GL_MATRIX_INDEX_ARRAY_POINTER_ARB 0x8849
+#endif
+
+#ifndef GL_ARB_texture_env_combine
+#define GL_COMBINE_ARB 0x8570
+#define GL_COMBINE_RGB_ARB 0x8571
+#define GL_COMBINE_ALPHA_ARB 0x8572
+#define GL_SOURCE0_RGB_ARB 0x8580
+#define GL_SOURCE1_RGB_ARB 0x8581
+#define GL_SOURCE2_RGB_ARB 0x8582
+#define GL_SOURCE0_ALPHA_ARB 0x8588
+#define GL_SOURCE1_ALPHA_ARB 0x8589
+#define GL_SOURCE2_ALPHA_ARB 0x858A
+#define GL_OPERAND0_RGB_ARB 0x8590
+#define GL_OPERAND1_RGB_ARB 0x8591
+#define GL_OPERAND2_RGB_ARB 0x8592
+#define GL_OPERAND0_ALPHA_ARB 0x8598
+#define GL_OPERAND1_ALPHA_ARB 0x8599
+#define GL_OPERAND2_ALPHA_ARB 0x859A
+#define GL_RGB_SCALE_ARB 0x8573
+#define GL_ADD_SIGNED_ARB 0x8574
+#define GL_INTERPOLATE_ARB 0x8575
+#define GL_SUBTRACT_ARB 0x84E7
+#define GL_CONSTANT_ARB 0x8576
+#define GL_PRIMARY_COLOR_ARB 0x8577
+#define GL_PREVIOUS_ARB 0x8578
+#endif
+
+#ifndef GL_ARB_texture_env_crossbar
+#endif
+
+#ifndef GL_ARB_texture_env_dot3
+#define GL_DOT3_RGB_ARB 0x86AE
+#define GL_DOT3_RGBA_ARB 0x86AF
+#endif
+
+#ifndef GL_ARB_texture_mirrored_repeat
+#define GL_MIRRORED_REPEAT_ARB 0x8370
+#endif
+
+#ifndef GL_ARB_depth_texture
+#define GL_DEPTH_COMPONENT16_ARB 0x81A5
+#define GL_DEPTH_COMPONENT24_ARB 0x81A6
+#define GL_DEPTH_COMPONENT32_ARB 0x81A7
+#define GL_TEXTURE_DEPTH_SIZE_ARB 0x884A
+#define GL_DEPTH_TEXTURE_MODE_ARB 0x884B
+#endif
+
+#ifndef GL_ARB_shadow
+#define GL_TEXTURE_COMPARE_MODE_ARB 0x884C
+#define GL_TEXTURE_COMPARE_FUNC_ARB 0x884D
+#define GL_COMPARE_R_TO_TEXTURE_ARB 0x884E
+#endif
+
+#ifndef GL_ARB_shadow_ambient
+#define GL_TEXTURE_COMPARE_FAIL_VALUE_ARB 0x80BF
+#endif
+
+#ifndef GL_ARB_window_pos
+#endif
+
+#ifndef GL_ARB_vertex_program
+#define GL_COLOR_SUM_ARB 0x8458
+#define GL_VERTEX_PROGRAM_ARB 0x8620
+#define GL_VERTEX_ATTRIB_ARRAY_ENABLED_ARB 0x8622
+#define GL_VERTEX_ATTRIB_ARRAY_SIZE_ARB 0x8623
+#define GL_VERTEX_ATTRIB_ARRAY_STRIDE_ARB 0x8624
+#define GL_VERTEX_ATTRIB_ARRAY_TYPE_ARB 0x8625
+#define GL_CURRENT_VERTEX_ATTRIB_ARB 0x8626
+#define GL_PROGRAM_LENGTH_ARB 0x8627
+#define GL_PROGRAM_STRING_ARB 0x8628
+#define GL_MAX_PROGRAM_MATRIX_STACK_DEPTH_ARB 0x862E
+#define GL_MAX_PROGRAM_MATRICES_ARB 0x862F
+#define GL_CURRENT_MATRIX_STACK_DEPTH_ARB 0x8640
+#define GL_CURRENT_MATRIX_ARB 0x8641
+#define GL_VERTEX_PROGRAM_POINT_SIZE_ARB 0x8642
+#define GL_VERTEX_PROGRAM_TWO_SIDE_ARB 0x8643
+#define GL_VERTEX_ATTRIB_ARRAY_POINTER_ARB 0x8645
+#define GL_PROGRAM_ERROR_POSITION_ARB 0x864B
+#define GL_PROGRAM_BINDING_ARB 0x8677
+#define GL_MAX_VERTEX_ATTRIBS_ARB 0x8869
+#define GL_VERTEX_ATTRIB_ARRAY_NORMALIZED_ARB 0x886A
+#define GL_PROGRAM_ERROR_STRING_ARB 0x8874
+#define GL_PROGRAM_FORMAT_ASCII_ARB 0x8875
+#define GL_PROGRAM_FORMAT_ARB 0x8876
+#define GL_PROGRAM_INSTRUCTIONS_ARB 0x88A0
+#define GL_MAX_PROGRAM_INSTRUCTIONS_ARB 0x88A1
+#define GL_PROGRAM_NATIVE_INSTRUCTIONS_ARB 0x88A2
+#define GL_MAX_PROGRAM_NATIVE_INSTRUCTIONS_ARB 0x88A3
+#define GL_PROGRAM_TEMPORARIES_ARB 0x88A4
+#define GL_MAX_PROGRAM_TEMPORARIES_ARB 0x88A5
+#define GL_PROGRAM_NATIVE_TEMPORARIES_ARB 0x88A6
+#define GL_MAX_PROGRAM_NATIVE_TEMPORARIES_ARB 0x88A7
+#define GL_PROGRAM_PARAMETERS_ARB 0x88A8
+#define GL_MAX_PROGRAM_PARAMETERS_ARB 0x88A9
+#define GL_PROGRAM_NATIVE_PARAMETERS_ARB 0x88AA
+#define GL_MAX_PROGRAM_NATIVE_PARAMETERS_ARB 0x88AB
+#define GL_PROGRAM_ATTRIBS_ARB 0x88AC
+#define GL_MAX_PROGRAM_ATTRIBS_ARB 0x88AD
+#define GL_PROGRAM_NATIVE_ATTRIBS_ARB 0x88AE
+#define GL_MAX_PROGRAM_NATIVE_ATTRIBS_ARB 0x88AF
+#define GL_PROGRAM_ADDRESS_REGISTERS_ARB 0x88B0
+#define GL_MAX_PROGRAM_ADDRESS_REGISTERS_ARB 0x88B1
+#define GL_PROGRAM_NATIVE_ADDRESS_REGISTERS_ARB 0x88B2
+#define GL_MAX_PROGRAM_NATIVE_ADDRESS_REGISTERS_ARB 0x88B3
+#define GL_MAX_PROGRAM_LOCAL_PARAMETERS_ARB 0x88B4
+#define GL_MAX_PROGRAM_ENV_PARAMETERS_ARB 0x88B5
+#define GL_PROGRAM_UNDER_NATIVE_LIMITS_ARB 0x88B6
+#define GL_TRANSPOSE_CURRENT_MATRIX_ARB 0x88B7
+#define GL_MATRIX0_ARB 0x88C0
+#define GL_MATRIX1_ARB 0x88C1
+#define GL_MATRIX2_ARB 0x88C2
+#define GL_MATRIX3_ARB 0x88C3
+#define GL_MATRIX4_ARB 0x88C4
+#define GL_MATRIX5_ARB 0x88C5
+#define GL_MATRIX6_ARB 0x88C6
+#define GL_MATRIX7_ARB 0x88C7
+#define GL_MATRIX8_ARB 0x88C8
+#define GL_MATRIX9_ARB 0x88C9
+#define GL_MATRIX10_ARB 0x88CA
+#define GL_MATRIX11_ARB 0x88CB
+#define GL_MATRIX12_ARB 0x88CC
+#define GL_MATRIX13_ARB 0x88CD
+#define GL_MATRIX14_ARB 0x88CE
+#define GL_MATRIX15_ARB 0x88CF
+#define GL_MATRIX16_ARB 0x88D0
+#define GL_MATRIX17_ARB 0x88D1
+#define GL_MATRIX18_ARB 0x88D2
+#define GL_MATRIX19_ARB 0x88D3
+#define GL_MATRIX20_ARB 0x88D4
+#define GL_MATRIX21_ARB 0x88D5
+#define GL_MATRIX22_ARB 0x88D6
+#define GL_MATRIX23_ARB 0x88D7
+#define GL_MATRIX24_ARB 0x88D8
+#define GL_MATRIX25_ARB 0x88D9
+#define GL_MATRIX26_ARB 0x88DA
+#define GL_MATRIX27_ARB 0x88DB
+#define GL_MATRIX28_ARB 0x88DC
+#define GL_MATRIX29_ARB 0x88DD
+#define GL_MATRIX30_ARB 0x88DE
+#define GL_MATRIX31_ARB 0x88DF
+#endif
+
+#ifndef GL_ARB_fragment_program
+#define GL_FRAGMENT_PROGRAM_ARB 0x8804
+#define GL_PROGRAM_ALU_INSTRUCTIONS_ARB 0x8805
+#define GL_PROGRAM_TEX_INSTRUCTIONS_ARB 0x8806
+#define GL_PROGRAM_TEX_INDIRECTIONS_ARB 0x8807
+#define GL_PROGRAM_NATIVE_ALU_INSTRUCTIONS_ARB 0x8808
+#define GL_PROGRAM_NATIVE_TEX_INSTRUCTIONS_ARB 0x8809
+#define GL_PROGRAM_NATIVE_TEX_INDIRECTIONS_ARB 0x880A
+#define GL_MAX_PROGRAM_ALU_INSTRUCTIONS_ARB 0x880B
+#define GL_MAX_PROGRAM_TEX_INSTRUCTIONS_ARB 0x880C
+#define GL_MAX_PROGRAM_TEX_INDIRECTIONS_ARB 0x880D
+#define GL_MAX_PROGRAM_NATIVE_ALU_INSTRUCTIONS_ARB 0x880E
+#define GL_MAX_PROGRAM_NATIVE_TEX_INSTRUCTIONS_ARB 0x880F
+#define GL_MAX_PROGRAM_NATIVE_TEX_INDIRECTIONS_ARB 0x8810
+#define GL_MAX_TEXTURE_COORDS_ARB 0x8871
+#define GL_MAX_TEXTURE_IMAGE_UNITS_ARB 0x8872
+#endif
+
+#ifndef GL_ARB_vertex_buffer_object
+#define GL_BUFFER_SIZE_ARB 0x8764
+#define GL_BUFFER_USAGE_ARB 0x8765
+#define GL_ARRAY_BUFFER_ARB 0x8892
+#define GL_ELEMENT_ARRAY_BUFFER_ARB 0x8893
+#define GL_ARRAY_BUFFER_BINDING_ARB 0x8894
+#define GL_ELEMENT_ARRAY_BUFFER_BINDING_ARB 0x8895
+#define GL_VERTEX_ARRAY_BUFFER_BINDING_ARB 0x8896
+#define GL_NORMAL_ARRAY_BUFFER_BINDING_ARB 0x8897
+#define GL_COLOR_ARRAY_BUFFER_BINDING_ARB 0x8898
+#define GL_INDEX_ARRAY_BUFFER_BINDING_ARB 0x8899
+#define GL_TEXTURE_COORD_ARRAY_BUFFER_BINDING_ARB 0x889A
+#define GL_EDGE_FLAG_ARRAY_BUFFER_BINDING_ARB 0x889B
+#define GL_SECONDARY_COLOR_ARRAY_BUFFER_BINDING_ARB 0x889C
+#define GL_FOG_COORDINATE_ARRAY_BUFFER_BINDING_ARB 0x889D
+#define GL_WEIGHT_ARRAY_BUFFER_BINDING_ARB 0x889E
+#define GL_VERTEX_ATTRIB_ARRAY_BUFFER_BINDING_ARB 0x889F
+#define GL_READ_ONLY_ARB 0x88B8
+#define GL_WRITE_ONLY_ARB 0x88B9
+#define GL_READ_WRITE_ARB 0x88BA
+#define GL_BUFFER_ACCESS_ARB 0x88BB
+#define GL_BUFFER_MAPPED_ARB 0x88BC
+#define GL_BUFFER_MAP_POINTER_ARB 0x88BD
+#define GL_STREAM_DRAW_ARB 0x88E0
+#define GL_STREAM_READ_ARB 0x88E1
+#define GL_STREAM_COPY_ARB 0x88E2
+#define GL_STATIC_DRAW_ARB 0x88E4
+#define GL_STATIC_READ_ARB 0x88E5
+#define GL_STATIC_COPY_ARB 0x88E6
+#define GL_DYNAMIC_DRAW_ARB 0x88E8
+#define GL_DYNAMIC_READ_ARB 0x88E9
+#define GL_DYNAMIC_COPY_ARB 0x88EA
+#endif
+
+#ifndef GL_ARB_occlusion_query
+#define GL_QUERY_COUNTER_BITS_ARB 0x8864
+#define GL_CURRENT_QUERY_ARB 0x8865
+#define GL_QUERY_RESULT_ARB 0x8866
+#define GL_QUERY_RESULT_AVAILABLE_ARB 0x8867
+#define GL_SAMPLES_PASSED_ARB 0x8914
+#endif
+
+#ifndef GL_ARB_shader_objects
+#define GL_PROGRAM_OBJECT_ARB 0x8B40
+#define GL_SHADER_OBJECT_ARB 0x8B48
+#define GL_OBJECT_TYPE_ARB 0x8B4E
+#define GL_OBJECT_SUBTYPE_ARB 0x8B4F
+#define GL_FLOAT_VEC2_ARB 0x8B50
+#define GL_FLOAT_VEC3_ARB 0x8B51
+#define GL_FLOAT_VEC4_ARB 0x8B52
+#define GL_INT_VEC2_ARB 0x8B53
+#define GL_INT_VEC3_ARB 0x8B54
+#define GL_INT_VEC4_ARB 0x8B55
+#define GL_BOOL_ARB 0x8B56
+#define GL_BOOL_VEC2_ARB 0x8B57
+#define GL_BOOL_VEC3_ARB 0x8B58
+#define GL_BOOL_VEC4_ARB 0x8B59
+#define GL_FLOAT_MAT2_ARB 0x8B5A
+#define GL_FLOAT_MAT3_ARB 0x8B5B
+#define GL_FLOAT_MAT4_ARB 0x8B5C
+#define GL_SAMPLER_1D_ARB 0x8B5D
+#define GL_SAMPLER_2D_ARB 0x8B5E
+#define GL_SAMPLER_3D_ARB 0x8B5F
+#define GL_SAMPLER_CUBE_ARB 0x8B60
+#define GL_SAMPLER_1D_SHADOW_ARB 0x8B61
+#define GL_SAMPLER_2D_SHADOW_ARB 0x8B62
+#define GL_SAMPLER_2D_RECT_ARB 0x8B63
+#define GL_SAMPLER_2D_RECT_SHADOW_ARB 0x8B64
+#define GL_OBJECT_DELETE_STATUS_ARB 0x8B80
+#define GL_OBJECT_COMPILE_STATUS_ARB 0x8B81
+#define GL_OBJECT_LINK_STATUS_ARB 0x8B82
+#define GL_OBJECT_VALIDATE_STATUS_ARB 0x8B83
+#define GL_OBJECT_INFO_LOG_LENGTH_ARB 0x8B84
+#define GL_OBJECT_ATTACHED_OBJECTS_ARB 0x8B85
+#define GL_OBJECT_ACTIVE_UNIFORMS_ARB 0x8B86
+#define GL_OBJECT_ACTIVE_UNIFORM_MAX_LENGTH_ARB 0x8B87
+#define GL_OBJECT_SHADER_SOURCE_LENGTH_ARB 0x8B88
+#endif
+
+#ifndef GL_ARB_vertex_shader
+#define GL_VERTEX_SHADER_ARB 0x8B31
+#define GL_MAX_VERTEX_UNIFORM_COMPONENTS_ARB 0x8B4A
+#define GL_MAX_VARYING_FLOATS_ARB 0x8B4B
+#define GL_MAX_VERTEX_TEXTURE_IMAGE_UNITS_ARB 0x8B4C
+#define GL_MAX_COMBINED_TEXTURE_IMAGE_UNITS_ARB 0x8B4D
+#define GL_OBJECT_ACTIVE_ATTRIBUTES_ARB 0x8B89
+#define GL_OBJECT_ACTIVE_ATTRIBUTE_MAX_LENGTH_ARB 0x8B8A
+#endif
+
+#ifndef GL_ARB_fragment_shader
+#define GL_FRAGMENT_SHADER_ARB 0x8B30
+#define GL_MAX_FRAGMENT_UNIFORM_COMPONENTS_ARB 0x8B49
+#define GL_FRAGMENT_SHADER_DERIVATIVE_HINT_ARB 0x8B8B
+#endif
+
+#ifndef GL_ARB_shading_language_100
+#define GL_SHADING_LANGUAGE_VERSION_ARB 0x8B8C
+#endif
+
+#ifndef GL_ARB_texture_non_power_of_two
+#endif
+
+#ifndef GL_ARB_point_sprite
+#define GL_POINT_SPRITE_ARB 0x8861
+#define GL_COORD_REPLACE_ARB 0x8862
+#endif
+
+#ifndef GL_ARB_fragment_program_shadow
+#endif
+
+#ifndef GL_ARB_draw_buffers
+#define GL_MAX_DRAW_BUFFERS_ARB 0x8824
+#define GL_DRAW_BUFFER0_ARB 0x8825
+#define GL_DRAW_BUFFER1_ARB 0x8826
+#define GL_DRAW_BUFFER2_ARB 0x8827
+#define GL_DRAW_BUFFER3_ARB 0x8828
+#define GL_DRAW_BUFFER4_ARB 0x8829
+#define GL_DRAW_BUFFER5_ARB 0x882A
+#define GL_DRAW_BUFFER6_ARB 0x882B
+#define GL_DRAW_BUFFER7_ARB 0x882C
+#define GL_DRAW_BUFFER8_ARB 0x882D
+#define GL_DRAW_BUFFER9_ARB 0x882E
+#define GL_DRAW_BUFFER10_ARB 0x882F
+#define GL_DRAW_BUFFER11_ARB 0x8830
+#define GL_DRAW_BUFFER12_ARB 0x8831
+#define GL_DRAW_BUFFER13_ARB 0x8832
+#define GL_DRAW_BUFFER14_ARB 0x8833
+#define GL_DRAW_BUFFER15_ARB 0x8834
+#endif
+
+#ifndef GL_ARB_texture_rectangle
+#define GL_TEXTURE_RECTANGLE_ARB 0x84F5
+#define GL_TEXTURE_BINDING_RECTANGLE_ARB 0x84F6
+#define GL_PROXY_TEXTURE_RECTANGLE_ARB 0x84F7
+#define GL_MAX_RECTANGLE_TEXTURE_SIZE_ARB 0x84F8
+#endif
+
+#ifndef GL_ARB_color_buffer_float
+#define GL_RGBA_FLOAT_MODE_ARB 0x8820
+#define GL_CLAMP_VERTEX_COLOR_ARB 0x891A
+#define GL_CLAMP_FRAGMENT_COLOR_ARB 0x891B
+#define GL_CLAMP_READ_COLOR_ARB 0x891C
+#define GL_FIXED_ONLY_ARB 0x891D
+#endif
+
+#ifndef GL_ARB_half_float_pixel
+#define GL_HALF_FLOAT_ARB 0x140B
+#endif
+
+#ifndef GL_ARB_texture_float
+#define GL_TEXTURE_RED_TYPE_ARB 0x8C10
+#define GL_TEXTURE_GREEN_TYPE_ARB 0x8C11
+#define GL_TEXTURE_BLUE_TYPE_ARB 0x8C12
+#define GL_TEXTURE_ALPHA_TYPE_ARB 0x8C13
+#define GL_TEXTURE_LUMINANCE_TYPE_ARB 0x8C14
+#define GL_TEXTURE_INTENSITY_TYPE_ARB 0x8C15
+#define GL_TEXTURE_DEPTH_TYPE_ARB 0x8C16
+#define GL_UNSIGNED_NORMALIZED_ARB 0x8C17
+#define GL_RGBA32F_ARB 0x8814
+#define GL_RGB32F_ARB 0x8815
+#define GL_ALPHA32F_ARB 0x8816
+#define GL_INTENSITY32F_ARB 0x8817
+#define GL_LUMINANCE32F_ARB 0x8818
+#define GL_LUMINANCE_ALPHA32F_ARB 0x8819
+#define GL_RGBA16F_ARB 0x881A
+#define GL_RGB16F_ARB 0x881B
+#define GL_ALPHA16F_ARB 0x881C
+#define GL_INTENSITY16F_ARB 0x881D
+#define GL_LUMINANCE16F_ARB 0x881E
+#define GL_LUMINANCE_ALPHA16F_ARB 0x881F
+#endif
+
+#ifndef GL_ARB_pixel_buffer_object
+#define GL_PIXEL_PACK_BUFFER_ARB 0x88EB
+#define GL_PIXEL_UNPACK_BUFFER_ARB 0x88EC
+#define GL_PIXEL_PACK_BUFFER_BINDING_ARB 0x88ED
+#define GL_PIXEL_UNPACK_BUFFER_BINDING_ARB 0x88EF
+#endif
+
+#ifndef GL_ARB_depth_buffer_float
+#define GL_DEPTH_COMPONENT32F 0x8CAC
+#define GL_DEPTH32F_STENCIL8 0x8CAD
+#define GL_FLOAT_32_UNSIGNED_INT_24_8_REV 0x8DAD
+#endif
+
+#ifndef GL_ARB_draw_instanced
+#endif
+
+#ifndef GL_ARB_framebuffer_object
+#define GL_INVALID_FRAMEBUFFER_OPERATION 0x0506
+#define GL_FRAMEBUFFER_ATTACHMENT_COLOR_ENCODING 0x8210
+#define GL_FRAMEBUFFER_ATTACHMENT_COMPONENT_TYPE 0x8211
+#define GL_FRAMEBUFFER_ATTACHMENT_RED_SIZE 0x8212
+#define GL_FRAMEBUFFER_ATTACHMENT_GREEN_SIZE 0x8213
+#define GL_FRAMEBUFFER_ATTACHMENT_BLUE_SIZE 0x8214
+#define GL_FRAMEBUFFER_ATTACHMENT_ALPHA_SIZE 0x8215
+#define GL_FRAMEBUFFER_ATTACHMENT_DEPTH_SIZE 0x8216
+#define GL_FRAMEBUFFER_ATTACHMENT_STENCIL_SIZE 0x8217
+#define GL_FRAMEBUFFER_DEFAULT 0x8218
+#define GL_FRAMEBUFFER_UNDEFINED 0x8219
+#define GL_DEPTH_STENCIL_ATTACHMENT 0x821A
+#define GL_MAX_RENDERBUFFER_SIZE 0x84E8
+#define GL_DEPTH_STENCIL 0x84F9
+#define GL_UNSIGNED_INT_24_8 0x84FA
+#define GL_DEPTH24_STENCIL8 0x88F0
+#define GL_TEXTURE_STENCIL_SIZE 0x88F1
+#define GL_TEXTURE_RED_TYPE 0x8C10
+#define GL_TEXTURE_GREEN_TYPE 0x8C11
+#define GL_TEXTURE_BLUE_TYPE 0x8C12
+#define GL_TEXTURE_ALPHA_TYPE 0x8C13
+#define GL_TEXTURE_DEPTH_TYPE 0x8C16
+#define GL_UNSIGNED_NORMALIZED 0x8C17
+#define GL_FRAMEBUFFER_BINDING 0x8CA6
+#define GL_DRAW_FRAMEBUFFER_BINDING GL_FRAMEBUFFER_BINDING
+#define GL_RENDERBUFFER_BINDING 0x8CA7
+#define GL_READ_FRAMEBUFFER 0x8CA8
+#define GL_DRAW_FRAMEBUFFER 0x8CA9
+#define GL_READ_FRAMEBUFFER_BINDING 0x8CAA
+#define GL_RENDERBUFFER_SAMPLES 0x8CAB
+#define GL_FRAMEBUFFER_ATTACHMENT_OBJECT_TYPE 0x8CD0
+#define GL_FRAMEBUFFER_ATTACHMENT_OBJECT_NAME 0x8CD1
+#define GL_FRAMEBUFFER_ATTACHMENT_TEXTURE_LEVEL 0x8CD2
+#define GL_FRAMEBUFFER_ATTACHMENT_TEXTURE_CUBE_MAP_FACE 0x8CD3
+#define GL_FRAMEBUFFER_ATTACHMENT_TEXTURE_LAYER 0x8CD4
+#define GL_FRAMEBUFFER_COMPLETE 0x8CD5
+#define GL_FRAMEBUFFER_INCOMPLETE_ATTACHMENT 0x8CD6
+#define GL_FRAMEBUFFER_INCOMPLETE_MISSING_ATTACHMENT 0x8CD7
+#define GL_FRAMEBUFFER_INCOMPLETE_DRAW_BUFFER 0x8CDB
+#define GL_FRAMEBUFFER_INCOMPLETE_READ_BUFFER 0x8CDC
+#define GL_FRAMEBUFFER_UNSUPPORTED 0x8CDD
+#define GL_MAX_COLOR_ATTACHMENTS 0x8CDF
+#define GL_COLOR_ATTACHMENT0 0x8CE0
+#define GL_COLOR_ATTACHMENT1 0x8CE1
+#define GL_COLOR_ATTACHMENT2 0x8CE2
+#define GL_COLOR_ATTACHMENT3 0x8CE3
+#define GL_COLOR_ATTACHMENT4 0x8CE4
+#define GL_COLOR_ATTACHMENT5 0x8CE5
+#define GL_COLOR_ATTACHMENT6 0x8CE6
+#define GL_COLOR_ATTACHMENT7 0x8CE7
+#define GL_COLOR_ATTACHMENT8 0x8CE8
+#define GL_COLOR_ATTACHMENT9 0x8CE9
+#define GL_COLOR_ATTACHMENT10 0x8CEA
+#define GL_COLOR_ATTACHMENT11 0x8CEB
+#define GL_COLOR_ATTACHMENT12 0x8CEC
+#define GL_COLOR_ATTACHMENT13 0x8CED
+#define GL_COLOR_ATTACHMENT14 0x8CEE
+#define GL_COLOR_ATTACHMENT15 0x8CEF
+#define GL_DEPTH_ATTACHMENT 0x8D00
+#define GL_STENCIL_ATTACHMENT 0x8D20
+#define GL_FRAMEBUFFER 0x8D40
+#define GL_RENDERBUFFER 0x8D41
+#define GL_RENDERBUFFER_WIDTH 0x8D42
+#define GL_RENDERBUFFER_HEIGHT 0x8D43
+#define GL_RENDERBUFFER_INTERNAL_FORMAT 0x8D44
+#define GL_STENCIL_INDEX1 0x8D46
+#define GL_STENCIL_INDEX4 0x8D47
+#define GL_STENCIL_INDEX8 0x8D48
+#define GL_STENCIL_INDEX16 0x8D49
+#define GL_RENDERBUFFER_RED_SIZE 0x8D50
+#define GL_RENDERBUFFER_GREEN_SIZE 0x8D51
+#define GL_RENDERBUFFER_BLUE_SIZE 0x8D52
+#define GL_RENDERBUFFER_ALPHA_SIZE 0x8D53
+#define GL_RENDERBUFFER_DEPTH_SIZE 0x8D54
+#define GL_RENDERBUFFER_STENCIL_SIZE 0x8D55
+#define GL_FRAMEBUFFER_INCOMPLETE_MULTISAMPLE 0x8D56
+#define GL_MAX_SAMPLES 0x8D57
+#endif
+
+#ifndef GL_ARB_framebuffer_object_DEPRECATED
+#define GL_INDEX 0x8222
+#define GL_TEXTURE_LUMINANCE_TYPE 0x8C14
+#define GL_TEXTURE_INTENSITY_TYPE 0x8C15
+#endif
+
+#ifndef GL_ARB_framebuffer_sRGB
+#define GL_FRAMEBUFFER_SRGB 0x8DB9
+#endif
+
+#ifndef GL_ARB_geometry_shader4
+#define GL_LINES_ADJACENCY_ARB 0x000A
+#define GL_LINE_STRIP_ADJACENCY_ARB 0x000B
+#define GL_TRIANGLES_ADJACENCY_ARB 0x000C
+#define GL_TRIANGLE_STRIP_ADJACENCY_ARB 0x000D
+#define GL_PROGRAM_POINT_SIZE_ARB 0x8642
+#define GL_MAX_GEOMETRY_TEXTURE_IMAGE_UNITS_ARB 0x8C29
+#define GL_FRAMEBUFFER_ATTACHMENT_LAYERED_ARB 0x8DA7
+#define GL_FRAMEBUFFER_INCOMPLETE_LAYER_TARGETS_ARB 0x8DA8
+#define GL_FRAMEBUFFER_INCOMPLETE_LAYER_COUNT_ARB 0x8DA9
+#define GL_GEOMETRY_SHADER_ARB 0x8DD9
+#define GL_GEOMETRY_VERTICES_OUT_ARB 0x8DDA
+#define GL_GEOMETRY_INPUT_TYPE_ARB 0x8DDB
+#define GL_GEOMETRY_OUTPUT_TYPE_ARB 0x8DDC
+#define GL_MAX_GEOMETRY_VARYING_COMPONENTS_ARB 0x8DDD
+#define GL_MAX_VERTEX_VARYING_COMPONENTS_ARB 0x8DDE
+#define GL_MAX_GEOMETRY_UNIFORM_COMPONENTS_ARB 0x8DDF
+#define GL_MAX_GEOMETRY_OUTPUT_VERTICES_ARB 0x8DE0
+#define GL_MAX_GEOMETRY_TOTAL_OUTPUT_COMPONENTS_ARB 0x8DE1
+/* reuse GL_MAX_VARYING_COMPONENTS */
+/* reuse GL_FRAMEBUFFER_ATTACHMENT_TEXTURE_LAYER */
+#endif
+
+#ifndef GL_ARB_half_float_vertex
+#define GL_HALF_FLOAT 0x140B
+#endif
+
+#ifndef GL_ARB_instanced_arrays
+#define GL_VERTEX_ATTRIB_ARRAY_DIVISOR_ARB 0x88FE
+#endif
+
+#ifndef GL_ARB_map_buffer_range
+#define GL_MAP_READ_BIT 0x0001
+#define GL_MAP_WRITE_BIT 0x0002
+#define GL_MAP_INVALIDATE_RANGE_BIT 0x0004
+#define GL_MAP_INVALIDATE_BUFFER_BIT 0x0008
+#define GL_MAP_FLUSH_EXPLICIT_BIT 0x0010
+#define GL_MAP_UNSYNCHRONIZED_BIT 0x0020
+#endif
+
+#ifndef GL_ARB_texture_buffer_object
+#define GL_TEXTURE_BUFFER_ARB 0x8C2A
+#define GL_MAX_TEXTURE_BUFFER_SIZE_ARB 0x8C2B
+#define GL_TEXTURE_BINDING_BUFFER_ARB 0x8C2C
+#define GL_TEXTURE_BUFFER_DATA_STORE_BINDING_ARB 0x8C2D
+#define GL_TEXTURE_BUFFER_FORMAT_ARB 0x8C2E
+#endif
+
+#ifndef GL_ARB_texture_compression_rgtc
+#define GL_COMPRESSED_RED_RGTC1 0x8DBB
+#define GL_COMPRESSED_SIGNED_RED_RGTC1 0x8DBC
+#define GL_COMPRESSED_RG_RGTC2 0x8DBD
+#define GL_COMPRESSED_SIGNED_RG_RGTC2 0x8DBE
+#endif
+
+#ifndef GL_ARB_texture_rg
+#define GL_RG 0x8227
+#define GL_RG_INTEGER 0x8228
+#define GL_R8 0x8229
+#define GL_R16 0x822A
+#define GL_RG8 0x822B
+#define GL_RG16 0x822C
+#define GL_R16F 0x822D
+#define GL_R32F 0x822E
+#define GL_RG16F 0x822F
+#define GL_RG32F 0x8230
+#define GL_R8I 0x8231
+#define GL_R8UI 0x8232
+#define GL_R16I 0x8233
+#define GL_R16UI 0x8234
+#define GL_R32I 0x8235
+#define GL_R32UI 0x8236
+#define GL_RG8I 0x8237
+#define GL_RG8UI 0x8238
+#define GL_RG16I 0x8239
+#define GL_RG16UI 0x823A
+#define GL_RG32I 0x823B
+#define GL_RG32UI 0x823C
+#endif
+
+#ifndef GL_ARB_vertex_array_object
+#define GL_VERTEX_ARRAY_BINDING 0x85B5
+#endif
+
+#ifndef GL_ARB_uniform_buffer_object
+#define GL_UNIFORM_BUFFER 0x8A11
+#define GL_UNIFORM_BUFFER_BINDING 0x8A28
+#define GL_UNIFORM_BUFFER_START 0x8A29
+#define GL_UNIFORM_BUFFER_SIZE 0x8A2A
+#define GL_MAX_VERTEX_UNIFORM_BLOCKS 0x8A2B
+#define GL_MAX_GEOMETRY_UNIFORM_BLOCKS 0x8A2C
+#define GL_MAX_FRAGMENT_UNIFORM_BLOCKS 0x8A2D
+#define GL_MAX_COMBINED_UNIFORM_BLOCKS 0x8A2E
+#define GL_MAX_UNIFORM_BUFFER_BINDINGS 0x8A2F
+#define GL_MAX_UNIFORM_BLOCK_SIZE 0x8A30
+#define GL_MAX_COMBINED_VERTEX_UNIFORM_COMPONENTS 0x8A31
+#define GL_MAX_COMBINED_GEOMETRY_UNIFORM_COMPONENTS 0x8A32
+#define GL_MAX_COMBINED_FRAGMENT_UNIFORM_COMPONENTS 0x8A33
+#define GL_UNIFORM_BUFFER_OFFSET_ALIGNMENT 0x8A34
+#define GL_ACTIVE_UNIFORM_BLOCK_MAX_NAME_LENGTH 0x8A35
+#define GL_ACTIVE_UNIFORM_BLOCKS 0x8A36
+#define GL_UNIFORM_TYPE 0x8A37
+#define GL_UNIFORM_SIZE 0x8A38
+#define GL_UNIFORM_NAME_LENGTH 0x8A39
+#define GL_UNIFORM_BLOCK_INDEX 0x8A3A
+#define GL_UNIFORM_OFFSET 0x8A3B
+#define GL_UNIFORM_ARRAY_STRIDE 0x8A3C
+#define GL_UNIFORM_MATRIX_STRIDE 0x8A3D
+#define GL_UNIFORM_IS_ROW_MAJOR 0x8A3E
+#define GL_UNIFORM_BLOCK_BINDING 0x8A3F
+#define GL_UNIFORM_BLOCK_DATA_SIZE 0x8A40
+#define GL_UNIFORM_BLOCK_NAME_LENGTH 0x8A41
+#define GL_UNIFORM_BLOCK_ACTIVE_UNIFORMS 0x8A42
+#define GL_UNIFORM_BLOCK_ACTIVE_UNIFORM_INDICES 0x8A43
+#define GL_UNIFORM_BLOCK_REFERENCED_BY_VERTEX_SHADER 0x8A44
+#define GL_UNIFORM_BLOCK_REFERENCED_BY_GEOMETRY_SHADER 0x8A45
+#define GL_UNIFORM_BLOCK_REFERENCED_BY_FRAGMENT_SHADER 0x8A46
+#define GL_INVALID_INDEX 0xFFFFFFFFu
+#endif
+
+#ifndef GL_ARB_compatibility
+/* ARB_compatibility just defines tokens from core 3.0 */
+#endif
+
+#ifndef GL_ARB_copy_buffer
+#define GL_COPY_READ_BUFFER 0x8F36
+#define GL_COPY_WRITE_BUFFER 0x8F37
+#endif
+
+#ifndef GL_ARB_shader_texture_lod
+#endif
+
+#ifndef GL_ARB_depth_clamp
+#define GL_DEPTH_CLAMP 0x864F
+#endif
+
+#ifndef GL_ARB_draw_elements_base_vertex
+#endif
+
+#ifndef GL_ARB_fragment_coord_conventions
+#endif
+
+#ifndef GL_ARB_provoking_vertex
+#define GL_QUADS_FOLLOW_PROVOKING_VERTEX_CONVENTION 0x8E4C
+#define GL_FIRST_VERTEX_CONVENTION 0x8E4D
+#define GL_LAST_VERTEX_CONVENTION 0x8E4E
+#define GL_PROVOKING_VERTEX 0x8E4F
+#endif
+
+#ifndef GL_ARB_seamless_cube_map
+#define GL_TEXTURE_CUBE_MAP_SEAMLESS 0x884F
+#endif
+
+#ifndef GL_ARB_sync
+#define GL_MAX_SERVER_WAIT_TIMEOUT 0x9111
+#define GL_OBJECT_TYPE 0x9112
+#define GL_SYNC_CONDITION 0x9113
+#define GL_SYNC_STATUS 0x9114
+#define GL_SYNC_FLAGS 0x9115
+#define GL_SYNC_FENCE 0x9116
+#define GL_SYNC_GPU_COMMANDS_COMPLETE 0x9117
+#define GL_UNSIGNALED 0x9118
+#define GL_SIGNALED 0x9119
+#define GL_ALREADY_SIGNALED 0x911A
+#define GL_TIMEOUT_EXPIRED 0x911B
+#define GL_CONDITION_SATISFIED 0x911C
+#define GL_WAIT_FAILED 0x911D
+#define GL_SYNC_FLUSH_COMMANDS_BIT 0x00000001
+#define GL_TIMEOUT_IGNORED 0xFFFFFFFFFFFFFFFFull
+#endif
+
+#ifndef GL_ARB_texture_multisample
+#define GL_SAMPLE_POSITION 0x8E50
+#define GL_SAMPLE_MASK 0x8E51
+#define GL_SAMPLE_MASK_VALUE 0x8E52
+#define GL_MAX_SAMPLE_MASK_WORDS 0x8E59
+#define GL_TEXTURE_2D_MULTISAMPLE 0x9100
+#define GL_PROXY_TEXTURE_2D_MULTISAMPLE 0x9101
+#define GL_TEXTURE_2D_MULTISAMPLE_ARRAY 0x9102
+#define GL_PROXY_TEXTURE_2D_MULTISAMPLE_ARRAY 0x9103
+#define GL_TEXTURE_BINDING_2D_MULTISAMPLE 0x9104
+#define GL_TEXTURE_BINDING_2D_MULTISAMPLE_ARRAY 0x9105
+#define GL_TEXTURE_SAMPLES 0x9106
+#define GL_TEXTURE_FIXED_SAMPLE_LOCATIONS 0x9107
+#define GL_SAMPLER_2D_MULTISAMPLE 0x9108
+#define GL_INT_SAMPLER_2D_MULTISAMPLE 0x9109
+#define GL_UNSIGNED_INT_SAMPLER_2D_MULTISAMPLE 0x910A
+#define GL_SAMPLER_2D_MULTISAMPLE_ARRAY 0x910B
+#define GL_INT_SAMPLER_2D_MULTISAMPLE_ARRAY 0x910C
+#define GL_UNSIGNED_INT_SAMPLER_2D_MULTISAMPLE_ARRAY 0x910D
+#define GL_MAX_COLOR_TEXTURE_SAMPLES 0x910E
+#define GL_MAX_DEPTH_TEXTURE_SAMPLES 0x910F
+#define GL_MAX_INTEGER_SAMPLES 0x9110
+#endif
+
+#ifndef GL_ARB_vertex_array_bgra
+/* reuse GL_BGRA */
+#endif
+
+#ifndef GL_ARB_draw_buffers_blend
+#endif
+
+#ifndef GL_ARB_sample_shading
+#define GL_SAMPLE_SHADING_ARB 0x8C36
+#define GL_MIN_SAMPLE_SHADING_VALUE_ARB 0x8C37
+#endif
+
+#ifndef GL_ARB_texture_cube_map_array
+#define GL_TEXTURE_CUBE_MAP_ARRAY_ARB 0x9009
+#define GL_TEXTURE_BINDING_CUBE_MAP_ARRAY_ARB 0x900A
+#define GL_PROXY_TEXTURE_CUBE_MAP_ARRAY_ARB 0x900B
+#define GL_SAMPLER_CUBE_MAP_ARRAY_ARB 0x900C
+#define GL_SAMPLER_CUBE_MAP_ARRAY_SHADOW_ARB 0x900D
+#define GL_INT_SAMPLER_CUBE_MAP_ARRAY_ARB 0x900E
+#define GL_UNSIGNED_INT_SAMPLER_CUBE_MAP_ARRAY_ARB 0x900F
+#endif
+
+#ifndef GL_ARB_texture_gather
+#define GL_MIN_PROGRAM_TEXTURE_GATHER_OFFSET_ARB 0x8E5E
+#define GL_MAX_PROGRAM_TEXTURE_GATHER_OFFSET_ARB 0x8E5F
+#endif
+
+#ifndef GL_ARB_texture_query_lod
+#endif
+
+#ifndef GL_ARB_shading_language_include
+#define GL_SHADER_INCLUDE_ARB 0x8DAE
+#define GL_NAMED_STRING_LENGTH_ARB 0x8DE9
+#define GL_NAMED_STRING_TYPE_ARB 0x8DEA
+#endif
+
+#ifndef GL_ARB_texture_compression_bptc
+#define GL_COMPRESSED_RGBA_BPTC_UNORM_ARB 0x8E8C
+#define GL_COMPRESSED_SRGB_ALPHA_BPTC_UNORM_ARB 0x8E8D
+#define GL_COMPRESSED_RGB_BPTC_SIGNED_FLOAT_ARB 0x8E8E
+#define GL_COMPRESSED_RGB_BPTC_UNSIGNED_FLOAT_ARB 0x8E8F
+#endif
+
+#ifndef GL_ARB_blend_func_extended
+#define GL_SRC1_COLOR 0x88F9
+/* reuse GL_SRC1_ALPHA */
+#define GL_ONE_MINUS_SRC1_COLOR 0x88FA
+#define GL_ONE_MINUS_SRC1_ALPHA 0x88FB
+#define GL_MAX_DUAL_SOURCE_DRAW_BUFFERS 0x88FC
+#endif
+
+#ifndef GL_ARB_explicit_attrib_location
+#endif
+
+#ifndef GL_ARB_occlusion_query2
+#define GL_ANY_SAMPLES_PASSED 0x8C2F
+#endif
+
+#ifndef GL_ARB_sampler_objects
+#define GL_SAMPLER_BINDING 0x8919
+#endif
+
+#ifndef GL_ARB_shader_bit_encoding
+#endif
+
+#ifndef GL_ARB_texture_rgb10_a2ui
+#define GL_RGB10_A2UI 0x906F
+#endif
+
+#ifndef GL_ARB_texture_swizzle
+#define GL_TEXTURE_SWIZZLE_R 0x8E42
+#define GL_TEXTURE_SWIZZLE_G 0x8E43
+#define GL_TEXTURE_SWIZZLE_B 0x8E44
+#define GL_TEXTURE_SWIZZLE_A 0x8E45
+#define GL_TEXTURE_SWIZZLE_RGBA 0x8E46
+#endif
+
+#ifndef GL_ARB_timer_query
+#define GL_TIME_ELAPSED 0x88BF
+#define GL_TIMESTAMP 0x8E28
+#endif
+
+#ifndef GL_ARB_vertex_type_2_10_10_10_rev
+/* reuse GL_UNSIGNED_INT_2_10_10_10_REV */
+#define GL_INT_2_10_10_10_REV 0x8D9F
+#endif
+
+#ifndef GL_ARB_draw_indirect
+#define GL_DRAW_INDIRECT_BUFFER 0x8F3F
+#define GL_DRAW_INDIRECT_BUFFER_BINDING 0x8F43
+#endif
+
+#ifndef GL_ARB_gpu_shader5
+#define GL_GEOMETRY_SHADER_INVOCATIONS 0x887F
+#define GL_MAX_GEOMETRY_SHADER_INVOCATIONS 0x8E5A
+#define GL_MIN_FRAGMENT_INTERPOLATION_OFFSET 0x8E5B
+#define GL_MAX_FRAGMENT_INTERPOLATION_OFFSET 0x8E5C
+#define GL_FRAGMENT_INTERPOLATION_OFFSET_BITS 0x8E5D
+/* reuse GL_MAX_VERTEX_STREAMS */
+#endif
+
+#ifndef GL_ARB_gpu_shader_fp64
+/* reuse GL_DOUBLE */
+#define GL_DOUBLE_VEC2 0x8FFC
+#define GL_DOUBLE_VEC3 0x8FFD
+#define GL_DOUBLE_VEC4 0x8FFE
+#define GL_DOUBLE_MAT2 0x8F46
+#define GL_DOUBLE_MAT3 0x8F47
+#define GL_DOUBLE_MAT4 0x8F48
+#define GL_DOUBLE_MAT2x3 0x8F49
+#define GL_DOUBLE_MAT2x4 0x8F4A
+#define GL_DOUBLE_MAT3x2 0x8F4B
+#define GL_DOUBLE_MAT3x4 0x8F4C
+#define GL_DOUBLE_MAT4x2 0x8F4D
+#define GL_DOUBLE_MAT4x3 0x8F4E
+#endif
+
+#ifndef GL_ARB_shader_subroutine
+#define GL_ACTIVE_SUBROUTINES 0x8DE5
+#define GL_ACTIVE_SUBROUTINE_UNIFORMS 0x8DE6
+#define GL_ACTIVE_SUBROUTINE_UNIFORM_LOCATIONS 0x8E47
+#define GL_ACTIVE_SUBROUTINE_MAX_LENGTH 0x8E48
+#define GL_ACTIVE_SUBROUTINE_UNIFORM_MAX_LENGTH 0x8E49
+#define GL_MAX_SUBROUTINES 0x8DE7
+#define GL_MAX_SUBROUTINE_UNIFORM_LOCATIONS 0x8DE8
+#define GL_NUM_COMPATIBLE_SUBROUTINES 0x8E4A
+#define GL_COMPATIBLE_SUBROUTINES 0x8E4B
+/* reuse GL_UNIFORM_SIZE */
+/* reuse GL_UNIFORM_NAME_LENGTH */
+#endif
+
+#ifndef GL_ARB_tessellation_shader
+#define GL_PATCHES 0x000E
+#define GL_PATCH_VERTICES 0x8E72
+#define GL_PATCH_DEFAULT_INNER_LEVEL 0x8E73
+#define GL_PATCH_DEFAULT_OUTER_LEVEL 0x8E74
+#define GL_TESS_CONTROL_OUTPUT_VERTICES 0x8E75
+#define GL_TESS_GEN_MODE 0x8E76
+#define GL_TESS_GEN_SPACING 0x8E77
+#define GL_TESS_GEN_VERTEX_ORDER 0x8E78
+#define GL_TESS_GEN_POINT_MODE 0x8E79
+/* reuse GL_TRIANGLES */
+/* reuse GL_QUADS */
+#define GL_ISOLINES 0x8E7A
+/* reuse GL_EQUAL */
+#define GL_FRACTIONAL_ODD 0x8E7B
+#define GL_FRACTIONAL_EVEN 0x8E7C
+/* reuse GL_CCW */
+/* reuse GL_CW */
+#define GL_MAX_PATCH_VERTICES 0x8E7D
+#define GL_MAX_TESS_GEN_LEVEL 0x8E7E
+#define GL_MAX_TESS_CONTROL_UNIFORM_COMPONENTS 0x8E7F
+#define GL_MAX_TESS_EVALUATION_UNIFORM_COMPONENTS 0x8E80
+#define GL_MAX_TESS_CONTROL_TEXTURE_IMAGE_UNITS 0x8E81
+#define GL_MAX_TESS_EVALUATION_TEXTURE_IMAGE_UNITS 0x8E82
+#define GL_MAX_TESS_CONTROL_OUTPUT_COMPONENTS 0x8E83
+#define GL_MAX_TESS_PATCH_COMPONENTS 0x8E84
+#define GL_MAX_TESS_CONTROL_TOTAL_OUTPUT_COMPONENTS 0x8E85
+#define GL_MAX_TESS_EVALUATION_OUTPUT_COMPONENTS 0x8E86
+#define GL_MAX_TESS_CONTROL_UNIFORM_BLOCKS 0x8E89
+#define GL_MAX_TESS_EVALUATION_UNIFORM_BLOCKS 0x8E8A
+#define GL_MAX_TESS_CONTROL_INPUT_COMPONENTS 0x886C
+#define GL_MAX_TESS_EVALUATION_INPUT_COMPONENTS 0x886D
+#define GL_MAX_COMBINED_TESS_CONTROL_UNIFORM_COMPONENTS 0x8E1E
+#define GL_MAX_COMBINED_TESS_EVALUATION_UNIFORM_COMPONENTS 0x8E1F
+#define GL_UNIFORM_BLOCK_REFERENCED_BY_TESS_CONTROL_SHADER 0x84F0
+#define GL_UNIFORM_BLOCK_REFERENCED_BY_TESS_EVALUATION_SHADER 0x84F1
+#define GL_TESS_EVALUATION_SHADER 0x8E87
+#define GL_TESS_CONTROL_SHADER 0x8E88
+#endif
+
+#ifndef GL_ARB_texture_buffer_object_rgb32
+/* reuse GL_RGB32F */
+/* reuse GL_RGB32UI */
+/* reuse GL_RGB32I */
+#endif
+
+#ifndef GL_ARB_transform_feedback2
+#define GL_TRANSFORM_FEEDBACK 0x8E22
+#define GL_TRANSFORM_FEEDBACK_BUFFER_PAUSED 0x8E23
+#define GL_TRANSFORM_FEEDBACK_BUFFER_ACTIVE 0x8E24
+#define GL_TRANSFORM_FEEDBACK_BINDING 0x8E25
+#endif
+
+#ifndef GL_ARB_transform_feedback3
+#define GL_MAX_TRANSFORM_FEEDBACK_BUFFERS 0x8E70
+#define GL_MAX_VERTEX_STREAMS 0x8E71
+#endif
+
+#ifndef GL_ARB_ES2_compatibility
+#define GL_FIXED 0x140C
+#define GL_IMPLEMENTATION_COLOR_READ_TYPE 0x8B9A
+#define GL_IMPLEMENTATION_COLOR_READ_FORMAT 0x8B9B
+#define GL_LOW_FLOAT 0x8DF0
+#define GL_MEDIUM_FLOAT 0x8DF1
+#define GL_HIGH_FLOAT 0x8DF2
+#define GL_LOW_INT 0x8DF3
+#define GL_MEDIUM_INT 0x8DF4
+#define GL_HIGH_INT 0x8DF5
+#define GL_SHADER_COMPILER 0x8DFA
+#define GL_NUM_SHADER_BINARY_FORMATS 0x8DF9
+#define GL_MAX_VERTEX_UNIFORM_VECTORS 0x8DFB
+#define GL_MAX_VARYING_VECTORS 0x8DFC
+#define GL_MAX_FRAGMENT_UNIFORM_VECTORS 0x8DFD
+#endif
+
+#ifndef GL_ARB_get_program_binary
+#define GL_PROGRAM_BINARY_RETRIEVABLE_HINT 0x8257
+#define GL_PROGRAM_BINARY_LENGTH 0x8741
+#define GL_NUM_PROGRAM_BINARY_FORMATS 0x87FE
+#define GL_PROGRAM_BINARY_FORMATS 0x87FF
+#endif
+
+#ifndef GL_ARB_separate_shader_objects
+#define GL_VERTEX_SHADER_BIT 0x00000001
+#define GL_FRAGMENT_SHADER_BIT 0x00000002
+#define GL_GEOMETRY_SHADER_BIT 0x00000004
+#define GL_TESS_CONTROL_SHADER_BIT 0x00000008
+#define GL_TESS_EVALUATION_SHADER_BIT 0x00000010
+#define GL_ALL_SHADER_BITS 0xFFFFFFFF
+#define GL_PROGRAM_SEPARABLE 0x8258
+#define GL_ACTIVE_PROGRAM 0x8259
+#define GL_PROGRAM_PIPELINE_BINDING 0x825A
+#endif
+
+#ifndef GL_ARB_shader_precision
+#endif
+
+#ifndef GL_ARB_vertex_attrib_64bit
+/* reuse GL_RGB32I */
+/* reuse GL_DOUBLE_VEC2 */
+/* reuse GL_DOUBLE_VEC3 */
+/* reuse GL_DOUBLE_VEC4 */
+/* reuse GL_DOUBLE_MAT2 */
+/* reuse GL_DOUBLE_MAT3 */
+/* reuse GL_DOUBLE_MAT4 */
+/* reuse GL_DOUBLE_MAT2x3 */
+/* reuse GL_DOUBLE_MAT2x4 */
+/* reuse GL_DOUBLE_MAT3x2 */
+/* reuse GL_DOUBLE_MAT3x4 */
+/* reuse GL_DOUBLE_MAT4x2 */
+/* reuse GL_DOUBLE_MAT4x3 */
+#endif
+
+#ifndef GL_ARB_viewport_array
+/* reuse GL_SCISSOR_BOX */
+/* reuse GL_VIEWPORT */
+/* reuse GL_DEPTH_RANGE */
+/* reuse GL_SCISSOR_TEST */
+#define GL_MAX_VIEWPORTS 0x825B
+#define GL_VIEWPORT_SUBPIXEL_BITS 0x825C
+#define GL_VIEWPORT_BOUNDS_RANGE 0x825D
+#define GL_LAYER_PROVOKING_VERTEX 0x825E
+#define GL_VIEWPORT_INDEX_PROVOKING_VERTEX 0x825F
+#define GL_UNDEFINED_VERTEX 0x8260
+/* reuse GL_FIRST_VERTEX_CONVENTION */
+/* reuse GL_LAST_VERTEX_CONVENTION */
+/* reuse GL_PROVOKING_VERTEX */
+#endif
+
+#ifndef GL_ARB_cl_event
+#define GL_SYNC_CL_EVENT_ARB 0x8240
+#define GL_SYNC_CL_EVENT_COMPLETE_ARB 0x8241
+#endif
+
+#ifndef GL_ARB_debug_output
+#define GL_DEBUG_OUTPUT_SYNCHRONOUS_ARB 0x8242
+#define GL_DEBUG_NEXT_LOGGED_MESSAGE_LENGTH_ARB 0x8243
+#define GL_DEBUG_CALLBACK_FUNCTION_ARB 0x8244
+#define GL_DEBUG_CALLBACK_USER_PARAM_ARB 0x8245
+#define GL_DEBUG_SOURCE_API_ARB 0x8246
+#define GL_DEBUG_SOURCE_WINDOW_SYSTEM_ARB 0x8247
+#define GL_DEBUG_SOURCE_SHADER_COMPILER_ARB 0x8248
+#define GL_DEBUG_SOURCE_THIRD_PARTY_ARB 0x8249
+#define GL_DEBUG_SOURCE_APPLICATION_ARB 0x824A
+#define GL_DEBUG_SOURCE_OTHER_ARB 0x824B
+#define GL_DEBUG_TYPE_ERROR_ARB 0x824C
+#define GL_DEBUG_TYPE_DEPRECATED_BEHAVIOR_ARB 0x824D
+#define GL_DEBUG_TYPE_UNDEFINED_BEHAVIOR_ARB 0x824E
+#define GL_DEBUG_TYPE_PORTABILITY_ARB 0x824F
+#define GL_DEBUG_TYPE_PERFORMANCE_ARB 0x8250
+#define GL_DEBUG_TYPE_OTHER_ARB 0x8251
+#define GL_MAX_DEBUG_MESSAGE_LENGTH_ARB 0x9143
+#define GL_MAX_DEBUG_LOGGED_MESSAGES_ARB 0x9144
+#define GL_DEBUG_LOGGED_MESSAGES_ARB 0x9145
+#define GL_DEBUG_SEVERITY_HIGH_ARB 0x9146
+#define GL_DEBUG_SEVERITY_MEDIUM_ARB 0x9147
+#define GL_DEBUG_SEVERITY_LOW_ARB 0x9148
+#endif
+
+#ifndef GL_ARB_robustness
+/* reuse GL_NO_ERROR */
+#define GL_CONTEXT_FLAG_ROBUST_ACCESS_BIT_ARB 0x00000004
+#define GL_LOSE_CONTEXT_ON_RESET_ARB 0x8252
+#define GL_GUILTY_CONTEXT_RESET_ARB 0x8253
+#define GL_INNOCENT_CONTEXT_RESET_ARB 0x8254
+#define GL_UNKNOWN_CONTEXT_RESET_ARB 0x8255
+#define GL_RESET_NOTIFICATION_STRATEGY_ARB 0x8256
+#define GL_NO_RESET_NOTIFICATION_ARB 0x8261
+#endif
+
+#ifndef GL_ARB_shader_stencil_export
+#endif
+
+#ifndef GL_EXT_abgr
+#define GL_ABGR_EXT 0x8000
+#endif
+
+#ifndef GL_EXT_blend_color
+#define GL_CONSTANT_COLOR_EXT 0x8001
+#define GL_ONE_MINUS_CONSTANT_COLOR_EXT 0x8002
+#define GL_CONSTANT_ALPHA_EXT 0x8003
+#define GL_ONE_MINUS_CONSTANT_ALPHA_EXT 0x8004
+#define GL_BLEND_COLOR_EXT 0x8005
+#endif
+
+#ifndef GL_EXT_polygon_offset
+#define GL_POLYGON_OFFSET_EXT 0x8037
+#define GL_POLYGON_OFFSET_FACTOR_EXT 0x8038
+#define GL_POLYGON_OFFSET_BIAS_EXT 0x8039
+#endif
+
+#ifndef GL_EXT_texture
+#define GL_ALPHA4_EXT 0x803B
+#define GL_ALPHA8_EXT 0x803C
+#define GL_ALPHA12_EXT 0x803D
+#define GL_ALPHA16_EXT 0x803E
+#define GL_LUMINANCE4_EXT 0x803F
+#define GL_LUMINANCE8_EXT 0x8040
+#define GL_LUMINANCE12_EXT 0x8041
+#define GL_LUMINANCE16_EXT 0x8042
+#define GL_LUMINANCE4_ALPHA4_EXT 0x8043
+#define GL_LUMINANCE6_ALPHA2_EXT 0x8044
+#define GL_LUMINANCE8_ALPHA8_EXT 0x8045
+#define GL_LUMINANCE12_ALPHA4_EXT 0x8046
+#define GL_LUMINANCE12_ALPHA12_EXT 0x8047
+#define GL_LUMINANCE16_ALPHA16_EXT 0x8048
+#define GL_INTENSITY_EXT 0x8049
+#define GL_INTENSITY4_EXT 0x804A
+#define GL_INTENSITY8_EXT 0x804B
+#define GL_INTENSITY12_EXT 0x804C
+#define GL_INTENSITY16_EXT 0x804D
+#define GL_RGB2_EXT 0x804E
+#define GL_RGB4_EXT 0x804F
+#define GL_RGB5_EXT 0x8050
+#define GL_RGB8_EXT 0x8051
+#define GL_RGB10_EXT 0x8052
+#define GL_RGB12_EXT 0x8053
+#define GL_RGB16_EXT 0x8054
+#define GL_RGBA2_EXT 0x8055
+#define GL_RGBA4_EXT 0x8056
+#define GL_RGB5_A1_EXT 0x8057
+#define GL_RGBA8_EXT 0x8058
+#define GL_RGB10_A2_EXT 0x8059
+#define GL_RGBA12_EXT 0x805A
+#define GL_RGBA16_EXT 0x805B
+#define GL_TEXTURE_RED_SIZE_EXT 0x805C
+#define GL_TEXTURE_GREEN_SIZE_EXT 0x805D
+#define GL_TEXTURE_BLUE_SIZE_EXT 0x805E
+#define GL_TEXTURE_ALPHA_SIZE_EXT 0x805F
+#define GL_TEXTURE_LUMINANCE_SIZE_EXT 0x8060
+#define GL_TEXTURE_INTENSITY_SIZE_EXT 0x8061
+#define GL_REPLACE_EXT 0x8062
+#define GL_PROXY_TEXTURE_1D_EXT 0x8063
+#define GL_PROXY_TEXTURE_2D_EXT 0x8064
+#define GL_TEXTURE_TOO_LARGE_EXT 0x8065
+#endif
+
+#ifndef GL_EXT_texture3D
+#define GL_PACK_SKIP_IMAGES_EXT 0x806B
+#define GL_PACK_IMAGE_HEIGHT_EXT 0x806C
+#define GL_UNPACK_SKIP_IMAGES_EXT 0x806D
+#define GL_UNPACK_IMAGE_HEIGHT_EXT 0x806E
+#define GL_TEXTURE_3D_EXT 0x806F
+#define GL_PROXY_TEXTURE_3D_EXT 0x8070
+#define GL_TEXTURE_DEPTH_EXT 0x8071
+#define GL_TEXTURE_WRAP_R_EXT 0x8072
+#define GL_MAX_3D_TEXTURE_SIZE_EXT 0x8073
+#endif
+
+#ifndef GL_SGIS_texture_filter4
+#define GL_FILTER4_SGIS 0x8146
+#define GL_TEXTURE_FILTER4_SIZE_SGIS 0x8147
+#endif
+
+#ifndef GL_EXT_subtexture
+#endif
+
+#ifndef GL_EXT_copy_texture
+#endif
+
+#ifndef GL_EXT_histogram
+#define GL_HISTOGRAM_EXT 0x8024
+#define GL_PROXY_HISTOGRAM_EXT 0x8025
+#define GL_HISTOGRAM_WIDTH_EXT 0x8026
+#define GL_HISTOGRAM_FORMAT_EXT 0x8027
+#define GL_HISTOGRAM_RED_SIZE_EXT 0x8028
+#define GL_HISTOGRAM_GREEN_SIZE_EXT 0x8029
+#define GL_HISTOGRAM_BLUE_SIZE_EXT 0x802A
+#define GL_HISTOGRAM_ALPHA_SIZE_EXT 0x802B
+#define GL_HISTOGRAM_LUMINANCE_SIZE_EXT 0x802C
+#define GL_HISTOGRAM_SINK_EXT 0x802D
+#define GL_MINMAX_EXT 0x802E
+#define GL_MINMAX_FORMAT_EXT 0x802F
+#define GL_MINMAX_SINK_EXT 0x8030
+#define GL_TABLE_TOO_LARGE_EXT 0x8031
+#endif
+
+#ifndef GL_EXT_convolution
+#define GL_CONVOLUTION_1D_EXT 0x8010
+#define GL_CONVOLUTION_2D_EXT 0x8011
+#define GL_SEPARABLE_2D_EXT 0x8012
+#define GL_CONVOLUTION_BORDER_MODE_EXT 0x8013
+#define GL_CONVOLUTION_FILTER_SCALE_EXT 0x8014
+#define GL_CONVOLUTION_FILTER_BIAS_EXT 0x8015
+#define GL_REDUCE_EXT 0x8016
+#define GL_CONVOLUTION_FORMAT_EXT 0x8017
+#define GL_CONVOLUTION_WIDTH_EXT 0x8018
+#define GL_CONVOLUTION_HEIGHT_EXT 0x8019
+#define GL_MAX_CONVOLUTION_WIDTH_EXT 0x801A
+#define GL_MAX_CONVOLUTION_HEIGHT_EXT 0x801B
+#define GL_POST_CONVOLUTION_RED_SCALE_EXT 0x801C
+#define GL_POST_CONVOLUTION_GREEN_SCALE_EXT 0x801D
+#define GL_POST_CONVOLUTION_BLUE_SCALE_EXT 0x801E
+#define GL_POST_CONVOLUTION_ALPHA_SCALE_EXT 0x801F
+#define GL_POST_CONVOLUTION_RED_BIAS_EXT 0x8020
+#define GL_POST_CONVOLUTION_GREEN_BIAS_EXT 0x8021
+#define GL_POST_CONVOLUTION_BLUE_BIAS_EXT 0x8022
+#define GL_POST_CONVOLUTION_ALPHA_BIAS_EXT 0x8023
+#endif
+
+#ifndef GL_SGI_color_matrix
+#define GL_COLOR_MATRIX_SGI 0x80B1
+#define GL_COLOR_MATRIX_STACK_DEPTH_SGI 0x80B2
+#define GL_MAX_COLOR_MATRIX_STACK_DEPTH_SGI 0x80B3
+#define GL_POST_COLOR_MATRIX_RED_SCALE_SGI 0x80B4
+#define GL_POST_COLOR_MATRIX_GREEN_SCALE_SGI 0x80B5
+#define GL_POST_COLOR_MATRIX_BLUE_SCALE_SGI 0x80B6
+#define GL_POST_COLOR_MATRIX_ALPHA_SCALE_SGI 0x80B7
+#define GL_POST_COLOR_MATRIX_RED_BIAS_SGI 0x80B8
+#define GL_POST_COLOR_MATRIX_GREEN_BIAS_SGI 0x80B9
+#define GL_POST_COLOR_MATRIX_BLUE_BIAS_SGI 0x80BA
+#define GL_POST_COLOR_MATRIX_ALPHA_BIAS_SGI 0x80BB
+#endif
+
+#ifndef GL_SGI_color_table
+#define GL_COLOR_TABLE_SGI 0x80D0
+#define GL_POST_CONVOLUTION_COLOR_TABLE_SGI 0x80D1
+#define GL_POST_COLOR_MATRIX_COLOR_TABLE_SGI 0x80D2
+#define GL_PROXY_COLOR_TABLE_SGI 0x80D3
+#define GL_PROXY_POST_CONVOLUTION_COLOR_TABLE_SGI 0x80D4
+#define GL_PROXY_POST_COLOR_MATRIX_COLOR_TABLE_SGI 0x80D5
+#define GL_COLOR_TABLE_SCALE_SGI 0x80D6
+#define GL_COLOR_TABLE_BIAS_SGI 0x80D7
+#define GL_COLOR_TABLE_FORMAT_SGI 0x80D8
+#define GL_COLOR_TABLE_WIDTH_SGI 0x80D9
+#define GL_COLOR_TABLE_RED_SIZE_SGI 0x80DA
+#define GL_COLOR_TABLE_GREEN_SIZE_SGI 0x80DB
+#define GL_COLOR_TABLE_BLUE_SIZE_SGI 0x80DC
+#define GL_COLOR_TABLE_ALPHA_SIZE_SGI 0x80DD
+#define GL_COLOR_TABLE_LUMINANCE_SIZE_SGI 0x80DE
+#define GL_COLOR_TABLE_INTENSITY_SIZE_SGI 0x80DF
+#endif
+
+#ifndef GL_SGIS_pixel_texture
+#define GL_PIXEL_TEXTURE_SGIS 0x8353
+#define GL_PIXEL_FRAGMENT_RGB_SOURCE_SGIS 0x8354
+#define GL_PIXEL_FRAGMENT_ALPHA_SOURCE_SGIS 0x8355
+#define GL_PIXEL_GROUP_COLOR_SGIS 0x8356
+#endif
+
+#ifndef GL_SGIX_pixel_texture
+#define GL_PIXEL_TEX_GEN_SGIX 0x8139
+#define GL_PIXEL_TEX_GEN_MODE_SGIX 0x832B
+#endif
+
+#ifndef GL_SGIS_texture4D
+#define GL_PACK_SKIP_VOLUMES_SGIS 0x8130
+#define GL_PACK_IMAGE_DEPTH_SGIS 0x8131
+#define GL_UNPACK_SKIP_VOLUMES_SGIS 0x8132
+#define GL_UNPACK_IMAGE_DEPTH_SGIS 0x8133
+#define GL_TEXTURE_4D_SGIS 0x8134
+#define GL_PROXY_TEXTURE_4D_SGIS 0x8135
+#define GL_TEXTURE_4DSIZE_SGIS 0x8136
+#define GL_TEXTURE_WRAP_Q_SGIS 0x8137
+#define GL_MAX_4D_TEXTURE_SIZE_SGIS 0x8138
+#define GL_TEXTURE_4D_BINDING_SGIS 0x814F
+#endif
+
+#ifndef GL_SGI_texture_color_table
+#define GL_TEXTURE_COLOR_TABLE_SGI 0x80BC
+#define GL_PROXY_TEXTURE_COLOR_TABLE_SGI 0x80BD
+#endif
+
+#ifndef GL_EXT_cmyka
+#define GL_CMYK_EXT 0x800C
+#define GL_CMYKA_EXT 0x800D
+#define GL_PACK_CMYK_HINT_EXT 0x800E
+#define GL_UNPACK_CMYK_HINT_EXT 0x800F
+#endif
+
+#ifndef GL_EXT_texture_object
+#define GL_TEXTURE_PRIORITY_EXT 0x8066
+#define GL_TEXTURE_RESIDENT_EXT 0x8067
+#define GL_TEXTURE_1D_BINDING_EXT 0x8068
+#define GL_TEXTURE_2D_BINDING_EXT 0x8069
+#define GL_TEXTURE_3D_BINDING_EXT 0x806A
+#endif
+
+#ifndef GL_SGIS_detail_texture
+#define GL_DETAIL_TEXTURE_2D_SGIS 0x8095
+#define GL_DETAIL_TEXTURE_2D_BINDING_SGIS 0x8096
+#define GL_LINEAR_DETAIL_SGIS 0x8097
+#define GL_LINEAR_DETAIL_ALPHA_SGIS 0x8098
+#define GL_LINEAR_DETAIL_COLOR_SGIS 0x8099
+#define GL_DETAIL_TEXTURE_LEVEL_SGIS 0x809A
+#define GL_DETAIL_TEXTURE_MODE_SGIS 0x809B
+#define GL_DETAIL_TEXTURE_FUNC_POINTS_SGIS 0x809C
+#endif
+
+#ifndef GL_SGIS_sharpen_texture
+#define GL_LINEAR_SHARPEN_SGIS 0x80AD
+#define GL_LINEAR_SHARPEN_ALPHA_SGIS 0x80AE
+#define GL_LINEAR_SHARPEN_COLOR_SGIS 0x80AF
+#define GL_SHARPEN_TEXTURE_FUNC_POINTS_SGIS 0x80B0
+#endif
+
+#ifndef GL_EXT_packed_pixels
+#define GL_UNSIGNED_BYTE_3_3_2_EXT 0x8032
+#define GL_UNSIGNED_SHORT_4_4_4_4_EXT 0x8033
+#define GL_UNSIGNED_SHORT_5_5_5_1_EXT 0x8034
+#define GL_UNSIGNED_INT_8_8_8_8_EXT 0x8035
+#define GL_UNSIGNED_INT_10_10_10_2_EXT 0x8036
+#endif
+
+#ifndef GL_SGIS_texture_lod
+#define GL_TEXTURE_MIN_LOD_SGIS 0x813A
+#define GL_TEXTURE_MAX_LOD_SGIS 0x813B
+#define GL_TEXTURE_BASE_LEVEL_SGIS 0x813C
+#define GL_TEXTURE_MAX_LEVEL_SGIS 0x813D
+#endif
+
+#ifndef GL_SGIS_multisample
+#define GL_MULTISAMPLE_SGIS 0x809D
+#define GL_SAMPLE_ALPHA_TO_MASK_SGIS 0x809E
+#define GL_SAMPLE_ALPHA_TO_ONE_SGIS 0x809F
+#define GL_SAMPLE_MASK_SGIS 0x80A0
+#define GL_1PASS_SGIS 0x80A1
+#define GL_2PASS_0_SGIS 0x80A2
+#define GL_2PASS_1_SGIS 0x80A3
+#define GL_4PASS_0_SGIS 0x80A4
+#define GL_4PASS_1_SGIS 0x80A5
+#define GL_4PASS_2_SGIS 0x80A6
+#define GL_4PASS_3_SGIS 0x80A7
+#define GL_SAMPLE_BUFFERS_SGIS 0x80A8
+#define GL_SAMPLES_SGIS 0x80A9
+#define GL_SAMPLE_MASK_VALUE_SGIS 0x80AA
+#define GL_SAMPLE_MASK_INVERT_SGIS 0x80AB
+#define GL_SAMPLE_PATTERN_SGIS 0x80AC
+#endif
+
+#ifndef GL_EXT_rescale_normal
+#define GL_RESCALE_NORMAL_EXT 0x803A
+#endif
+
+#ifndef GL_EXT_vertex_array
+#define GL_VERTEX_ARRAY_EXT 0x8074
+#define GL_NORMAL_ARRAY_EXT 0x8075
+#define GL_COLOR_ARRAY_EXT 0x8076
+#define GL_INDEX_ARRAY_EXT 0x8077
+#define GL_TEXTURE_COORD_ARRAY_EXT 0x8078
+#define GL_EDGE_FLAG_ARRAY_EXT 0x8079
+#define GL_VERTEX_ARRAY_SIZE_EXT 0x807A
+#define GL_VERTEX_ARRAY_TYPE_EXT 0x807B
+#define GL_VERTEX_ARRAY_STRIDE_EXT 0x807C
+#define GL_VERTEX_ARRAY_COUNT_EXT 0x807D
+#define GL_NORMAL_ARRAY_TYPE_EXT 0x807E
+#define GL_NORMAL_ARRAY_STRIDE_EXT 0x807F
+#define GL_NORMAL_ARRAY_COUNT_EXT 0x8080
+#define GL_COLOR_ARRAY_SIZE_EXT 0x8081
+#define GL_COLOR_ARRAY_TYPE_EXT 0x8082
+#define GL_COLOR_ARRAY_STRIDE_EXT 0x8083
+#define GL_COLOR_ARRAY_COUNT_EXT 0x8084
+#define GL_INDEX_ARRAY_TYPE_EXT 0x8085
+#define GL_INDEX_ARRAY_STRIDE_EXT 0x8086
+#define GL_INDEX_ARRAY_COUNT_EXT 0x8087
+#define GL_TEXTURE_COORD_ARRAY_SIZE_EXT 0x8088
+#define GL_TEXTURE_COORD_ARRAY_TYPE_EXT 0x8089
+#define GL_TEXTURE_COORD_ARRAY_STRIDE_EXT 0x808A
+#define GL_TEXTURE_COORD_ARRAY_COUNT_EXT 0x808B
+#define GL_EDGE_FLAG_ARRAY_STRIDE_EXT 0x808C
+#define GL_EDGE_FLAG_ARRAY_COUNT_EXT 0x808D
+#define GL_VERTEX_ARRAY_POINTER_EXT 0x808E
+#define GL_NORMAL_ARRAY_POINTER_EXT 0x808F
+#define GL_COLOR_ARRAY_POINTER_EXT 0x8090
+#define GL_INDEX_ARRAY_POINTER_EXT 0x8091
+#define GL_TEXTURE_COORD_ARRAY_POINTER_EXT 0x8092
+#define GL_EDGE_FLAG_ARRAY_POINTER_EXT 0x8093
+#endif
+
+#ifndef GL_EXT_misc_attribute
+#endif
+
+#ifndef GL_SGIS_generate_mipmap
+#define GL_GENERATE_MIPMAP_SGIS 0x8191
+#define GL_GENERATE_MIPMAP_HINT_SGIS 0x8192
+#endif
+
+#ifndef GL_SGIX_clipmap
+#define GL_LINEAR_CLIPMAP_LINEAR_SGIX 0x8170
+#define GL_TEXTURE_CLIPMAP_CENTER_SGIX 0x8171
+#define GL_TEXTURE_CLIPMAP_FRAME_SGIX 0x8172
+#define GL_TEXTURE_CLIPMAP_OFFSET_SGIX 0x8173
+#define GL_TEXTURE_CLIPMAP_VIRTUAL_DEPTH_SGIX 0x8174
+#define GL_TEXTURE_CLIPMAP_LOD_OFFSET_SGIX 0x8175
+#define GL_TEXTURE_CLIPMAP_DEPTH_SGIX 0x8176
+#define GL_MAX_CLIPMAP_DEPTH_SGIX 0x8177
+#define GL_MAX_CLIPMAP_VIRTUAL_DEPTH_SGIX 0x8178
+#define GL_NEAREST_CLIPMAP_NEAREST_SGIX 0x844D
+#define GL_NEAREST_CLIPMAP_LINEAR_SGIX 0x844E
+#define GL_LINEAR_CLIPMAP_NEAREST_SGIX 0x844F
+#endif
+
+#ifndef GL_SGIX_shadow
+#define GL_TEXTURE_COMPARE_SGIX 0x819A
+#define GL_TEXTURE_COMPARE_OPERATOR_SGIX 0x819B
+#define GL_TEXTURE_LEQUAL_R_SGIX 0x819C
+#define GL_TEXTURE_GEQUAL_R_SGIX 0x819D
+#endif
+
+#ifndef GL_SGIS_texture_edge_clamp
+#define GL_CLAMP_TO_EDGE_SGIS 0x812F
+#endif
+
+#ifndef GL_SGIS_texture_border_clamp
+#define GL_CLAMP_TO_BORDER_SGIS 0x812D
+#endif
+
+#ifndef GL_EXT_blend_minmax
+#define GL_FUNC_ADD_EXT 0x8006
+#define GL_MIN_EXT 0x8007
+#define GL_MAX_EXT 0x8008
+#define GL_BLEND_EQUATION_EXT 0x8009
+#endif
+
+#ifndef GL_EXT_blend_subtract
+#define GL_FUNC_SUBTRACT_EXT 0x800A
+#define GL_FUNC_REVERSE_SUBTRACT_EXT 0x800B
+#endif
+
+#ifndef GL_EXT_blend_logic_op
+#endif
+
+#ifndef GL_SGIX_interlace
+#define GL_INTERLACE_SGIX 0x8094
+#endif
+
+#ifndef GL_SGIX_pixel_tiles
+#define GL_PIXEL_TILE_BEST_ALIGNMENT_SGIX 0x813E
+#define GL_PIXEL_TILE_CACHE_INCREMENT_SGIX 0x813F
+#define GL_PIXEL_TILE_WIDTH_SGIX 0x8140
+#define GL_PIXEL_TILE_HEIGHT_SGIX 0x8141
+#define GL_PIXEL_TILE_GRID_WIDTH_SGIX 0x8142
+#define GL_PIXEL_TILE_GRID_HEIGHT_SGIX 0x8143
+#define GL_PIXEL_TILE_GRID_DEPTH_SGIX 0x8144
+#define GL_PIXEL_TILE_CACHE_SIZE_SGIX 0x8145
+#endif
+
+#ifndef GL_SGIS_texture_select
+#define GL_DUAL_ALPHA4_SGIS 0x8110
+#define GL_DUAL_ALPHA8_SGIS 0x8111
+#define GL_DUAL_ALPHA12_SGIS 0x8112
+#define GL_DUAL_ALPHA16_SGIS 0x8113
+#define GL_DUAL_LUMINANCE4_SGIS 0x8114
+#define GL_DUAL_LUMINANCE8_SGIS 0x8115
+#define GL_DUAL_LUMINANCE12_SGIS 0x8116
+#define GL_DUAL_LUMINANCE16_SGIS 0x8117
+#define GL_DUAL_INTENSITY4_SGIS 0x8118
+#define GL_DUAL_INTENSITY8_SGIS 0x8119
+#define GL_DUAL_INTENSITY12_SGIS 0x811A
+#define GL_DUAL_INTENSITY16_SGIS 0x811B
+#define GL_DUAL_LUMINANCE_ALPHA4_SGIS 0x811C
+#define GL_DUAL_LUMINANCE_ALPHA8_SGIS 0x811D
+#define GL_QUAD_ALPHA4_SGIS 0x811E
+#define GL_QUAD_ALPHA8_SGIS 0x811F
+#define GL_QUAD_LUMINANCE4_SGIS 0x8120
+#define GL_QUAD_LUMINANCE8_SGIS 0x8121
+#define GL_QUAD_INTENSITY4_SGIS 0x8122
+#define GL_QUAD_INTENSITY8_SGIS 0x8123
+#define GL_DUAL_TEXTURE_SELECT_SGIS 0x8124
+#define GL_QUAD_TEXTURE_SELECT_SGIS 0x8125
+#endif
+
+#ifndef GL_SGIX_sprite
+#define GL_SPRITE_SGIX 0x8148
+#define GL_SPRITE_MODE_SGIX 0x8149
+#define GL_SPRITE_AXIS_SGIX 0x814A
+#define GL_SPRITE_TRANSLATION_SGIX 0x814B
+#define GL_SPRITE_AXIAL_SGIX 0x814C
+#define GL_SPRITE_OBJECT_ALIGNED_SGIX 0x814D
+#define GL_SPRITE_EYE_ALIGNED_SGIX 0x814E
+#endif
+
+#ifndef GL_SGIX_texture_multi_buffer
+#define GL_TEXTURE_MULTI_BUFFER_HINT_SGIX 0x812E
+#endif
+
+#ifndef GL_EXT_point_parameters
+#define GL_POINT_SIZE_MIN_EXT 0x8126
+#define GL_POINT_SIZE_MAX_EXT 0x8127
+#define GL_POINT_FADE_THRESHOLD_SIZE_EXT 0x8128
+#define GL_DISTANCE_ATTENUATION_EXT 0x8129
+#endif
+
+#ifndef GL_SGIS_point_parameters
+#define GL_POINT_SIZE_MIN_SGIS 0x8126
+#define GL_POINT_SIZE_MAX_SGIS 0x8127
+#define GL_POINT_FADE_THRESHOLD_SIZE_SGIS 0x8128
+#define GL_DISTANCE_ATTENUATION_SGIS 0x8129
+#endif
+
+#ifndef GL_SGIX_instruments
+#define GL_INSTRUMENT_BUFFER_POINTER_SGIX 0x8180
+#define GL_INSTRUMENT_MEASUREMENTS_SGIX 0x8181
+#endif
+
+#ifndef GL_SGIX_texture_scale_bias
+#define GL_POST_TEXTURE_FILTER_BIAS_SGIX 0x8179
+#define GL_POST_TEXTURE_FILTER_SCALE_SGIX 0x817A
+#define GL_POST_TEXTURE_FILTER_BIAS_RANGE_SGIX 0x817B
+#define GL_POST_TEXTURE_FILTER_SCALE_RANGE_SGIX 0x817C
+#endif
+
+#ifndef GL_SGIX_framezoom
+#define GL_FRAMEZOOM_SGIX 0x818B
+#define GL_FRAMEZOOM_FACTOR_SGIX 0x818C
+#define GL_MAX_FRAMEZOOM_FACTOR_SGIX 0x818D
+#endif
+
+#ifndef GL_SGIX_tag_sample_buffer
+#endif
+
+#ifndef GL_FfdMaskSGIX
+#define GL_TEXTURE_DEFORMATION_BIT_SGIX 0x00000001
+#define GL_GEOMETRY_DEFORMATION_BIT_SGIX 0x00000002
+#endif
+
+#ifndef GL_SGIX_polynomial_ffd
+#define GL_GEOMETRY_DEFORMATION_SGIX 0x8194
+#define GL_TEXTURE_DEFORMATION_SGIX 0x8195
+#define GL_DEFORMATIONS_MASK_SGIX 0x8196
+#define GL_MAX_DEFORMATION_ORDER_SGIX 0x8197
+#endif
+
+#ifndef GL_SGIX_reference_plane
+#define GL_REFERENCE_PLANE_SGIX 0x817D
+#define GL_REFERENCE_PLANE_EQUATION_SGIX 0x817E
+#endif
+
+#ifndef GL_SGIX_flush_raster
+#endif
+
+#ifndef GL_SGIX_depth_texture
+#define GL_DEPTH_COMPONENT16_SGIX 0x81A5
+#define GL_DEPTH_COMPONENT24_SGIX 0x81A6
+#define GL_DEPTH_COMPONENT32_SGIX 0x81A7
+#endif
+
+#ifndef GL_SGIS_fog_function
+#define GL_FOG_FUNC_SGIS 0x812A
+#define GL_FOG_FUNC_POINTS_SGIS 0x812B
+#define GL_MAX_FOG_FUNC_POINTS_SGIS 0x812C
+#endif
+
+#ifndef GL_SGIX_fog_offset
+#define GL_FOG_OFFSET_SGIX 0x8198
+#define GL_FOG_OFFSET_VALUE_SGIX 0x8199
+#endif
+
+#ifndef GL_HP_image_transform
+#define GL_IMAGE_SCALE_X_HP 0x8155
+#define GL_IMAGE_SCALE_Y_HP 0x8156
+#define GL_IMAGE_TRANSLATE_X_HP 0x8157
+#define GL_IMAGE_TRANSLATE_Y_HP 0x8158
+#define GL_IMAGE_ROTATE_ANGLE_HP 0x8159
+#define GL_IMAGE_ROTATE_ORIGIN_X_HP 0x815A
+#define GL_IMAGE_ROTATE_ORIGIN_Y_HP 0x815B
+#define GL_IMAGE_MAG_FILTER_HP 0x815C
+#define GL_IMAGE_MIN_FILTER_HP 0x815D
+#define GL_IMAGE_CUBIC_WEIGHT_HP 0x815E
+#define GL_CUBIC_HP 0x815F
+#define GL_AVERAGE_HP 0x8160
+#define GL_IMAGE_TRANSFORM_2D_HP 0x8161
+#define GL_POST_IMAGE_TRANSFORM_COLOR_TABLE_HP 0x8162
+#define GL_PROXY_POST_IMAGE_TRANSFORM_COLOR_TABLE_HP 0x8163
+#endif
+
+#ifndef GL_HP_convolution_border_modes
+#define GL_IGNORE_BORDER_HP 0x8150
+#define GL_CONSTANT_BORDER_HP 0x8151
+#define GL_REPLICATE_BORDER_HP 0x8153
+#define GL_CONVOLUTION_BORDER_COLOR_HP 0x8154
+#endif
+
+#ifndef GL_INGR_palette_buffer
+#endif
+
+#ifndef GL_SGIX_texture_add_env
+#define GL_TEXTURE_ENV_BIAS_SGIX 0x80BE
+#endif
+
+#ifndef GL_EXT_color_subtable
+#endif
+
+#ifndef GL_PGI_vertex_hints
+#define GL_VERTEX_DATA_HINT_PGI 0x1A22A
+#define GL_VERTEX_CONSISTENT_HINT_PGI 0x1A22B
+#define GL_MATERIAL_SIDE_HINT_PGI 0x1A22C
+#define GL_MAX_VERTEX_HINT_PGI 0x1A22D
+#define GL_COLOR3_BIT_PGI 0x00010000
+#define GL_COLOR4_BIT_PGI 0x00020000
+#define GL_EDGEFLAG_BIT_PGI 0x00040000
+#define GL_INDEX_BIT_PGI 0x00080000
+#define GL_MAT_AMBIENT_BIT_PGI 0x00100000
+#define GL_MAT_AMBIENT_AND_DIFFUSE_BIT_PGI 0x00200000
+#define GL_MAT_DIFFUSE_BIT_PGI 0x00400000
+#define GL_MAT_EMISSION_BIT_PGI 0x00800000
+#define GL_MAT_COLOR_INDEXES_BIT_PGI 0x01000000
+#define GL_MAT_SHININESS_BIT_PGI 0x02000000
+#define GL_MAT_SPECULAR_BIT_PGI 0x04000000
+#define GL_NORMAL_BIT_PGI 0x08000000
+#define GL_TEXCOORD1_BIT_PGI 0x10000000
+#define GL_TEXCOORD2_BIT_PGI 0x20000000
+#define GL_TEXCOORD3_BIT_PGI 0x40000000
+#define GL_TEXCOORD4_BIT_PGI 0x80000000
+#define GL_VERTEX23_BIT_PGI 0x00000004
+#define GL_VERTEX4_BIT_PGI 0x00000008
+#endif
+
+#ifndef GL_PGI_misc_hints
+#define GL_PREFER_DOUBLEBUFFER_HINT_PGI 0x1A1F8
+#define GL_CONSERVE_MEMORY_HINT_PGI 0x1A1FD
+#define GL_RECLAIM_MEMORY_HINT_PGI 0x1A1FE
+#define GL_NATIVE_GRAPHICS_HANDLE_PGI 0x1A202
+#define GL_NATIVE_GRAPHICS_BEGIN_HINT_PGI 0x1A203
+#define GL_NATIVE_GRAPHICS_END_HINT_PGI 0x1A204
+#define GL_ALWAYS_FAST_HINT_PGI 0x1A20C
+#define GL_ALWAYS_SOFT_HINT_PGI 0x1A20D
+#define GL_ALLOW_DRAW_OBJ_HINT_PGI 0x1A20E
+#define GL_ALLOW_DRAW_WIN_HINT_PGI 0x1A20F
+#define GL_ALLOW_DRAW_FRG_HINT_PGI 0x1A210
+#define GL_ALLOW_DRAW_MEM_HINT_PGI 0x1A211
+#define GL_STRICT_DEPTHFUNC_HINT_PGI 0x1A216
+#define GL_STRICT_LIGHTING_HINT_PGI 0x1A217
+#define GL_STRICT_SCISSOR_HINT_PGI 0x1A218
+#define GL_FULL_STIPPLE_HINT_PGI 0x1A219
+#define GL_CLIP_NEAR_HINT_PGI 0x1A220
+#define GL_CLIP_FAR_HINT_PGI 0x1A221
+#define GL_WIDE_LINE_HINT_PGI 0x1A222
+#define GL_BACK_NORMALS_HINT_PGI 0x1A223
+#endif
+
+#ifndef GL_EXT_paletted_texture
+#define GL_COLOR_INDEX1_EXT 0x80E2
+#define GL_COLOR_INDEX2_EXT 0x80E3
+#define GL_COLOR_INDEX4_EXT 0x80E4
+#define GL_COLOR_INDEX8_EXT 0x80E5
+#define GL_COLOR_INDEX12_EXT 0x80E6
+#define GL_COLOR_INDEX16_EXT 0x80E7
+#define GL_TEXTURE_INDEX_SIZE_EXT 0x80ED
+#endif
+
+#ifndef GL_EXT_clip_volume_hint
+#define GL_CLIP_VOLUME_CLIPPING_HINT_EXT 0x80F0
+#endif
+
+#ifndef GL_SGIX_list_priority
+#define GL_LIST_PRIORITY_SGIX 0x8182
+#endif
+
+#ifndef GL_SGIX_ir_instrument1
+#define GL_IR_INSTRUMENT1_SGIX 0x817F
+#endif
+
+#ifndef GL_SGIX_calligraphic_fragment
+#define GL_CALLIGRAPHIC_FRAGMENT_SGIX 0x8183
+#endif
+
+#ifndef GL_SGIX_texture_lod_bias
+#define GL_TEXTURE_LOD_BIAS_S_SGIX 0x818E
+#define GL_TEXTURE_LOD_BIAS_T_SGIX 0x818F
+#define GL_TEXTURE_LOD_BIAS_R_SGIX 0x8190
+#endif
+
+#ifndef GL_SGIX_shadow_ambient
+#define GL_SHADOW_AMBIENT_SGIX 0x80BF
+#endif
+
+#ifndef GL_EXT_index_texture
+#endif
+
+#ifndef GL_EXT_index_material
+#define GL_INDEX_MATERIAL_EXT 0x81B8
+#define GL_INDEX_MATERIAL_PARAMETER_EXT 0x81B9
+#define GL_INDEX_MATERIAL_FACE_EXT 0x81BA
+#endif
+
+#ifndef GL_EXT_index_func
+#define GL_INDEX_TEST_EXT 0x81B5
+#define GL_INDEX_TEST_FUNC_EXT 0x81B6
+#define GL_INDEX_TEST_REF_EXT 0x81B7
+#endif
+
+#ifndef GL_EXT_index_array_formats
+#define GL_IUI_V2F_EXT 0x81AD
+#define GL_IUI_V3F_EXT 0x81AE
+#define GL_IUI_N3F_V2F_EXT 0x81AF
+#define GL_IUI_N3F_V3F_EXT 0x81B0
+#define GL_T2F_IUI_V2F_EXT 0x81B1
+#define GL_T2F_IUI_V3F_EXT 0x81B2
+#define GL_T2F_IUI_N3F_V2F_EXT 0x81B3
+#define GL_T2F_IUI_N3F_V3F_EXT 0x81B4
+#endif
+
+#ifndef GL_EXT_compiled_vertex_array
+#define GL_ARRAY_ELEMENT_LOCK_FIRST_EXT 0x81A8
+#define GL_ARRAY_ELEMENT_LOCK_COUNT_EXT 0x81A9
+#endif
+
+#ifndef GL_EXT_cull_vertex
+#define GL_CULL_VERTEX_EXT 0x81AA
+#define GL_CULL_VERTEX_EYE_POSITION_EXT 0x81AB
+#define GL_CULL_VERTEX_OBJECT_POSITION_EXT 0x81AC
+#endif
+
+#ifndef GL_SGIX_ycrcb
+#define GL_YCRCB_422_SGIX 0x81BB
+#define GL_YCRCB_444_SGIX 0x81BC
+#endif
+
+#ifndef GL_SGIX_fragment_lighting
+#define GL_FRAGMENT_LIGHTING_SGIX 0x8400
+#define GL_FRAGMENT_COLOR_MATERIAL_SGIX 0x8401
+#define GL_FRAGMENT_COLOR_MATERIAL_FACE_SGIX 0x8402
+#define GL_FRAGMENT_COLOR_MATERIAL_PARAMETER_SGIX 0x8403
+#define GL_MAX_FRAGMENT_LIGHTS_SGIX 0x8404
+#define GL_MAX_ACTIVE_LIGHTS_SGIX 0x8405
+#define GL_CURRENT_RASTER_NORMAL_SGIX 0x8406
+#define GL_LIGHT_ENV_MODE_SGIX 0x8407
+#define GL_FRAGMENT_LIGHT_MODEL_LOCAL_VIEWER_SGIX 0x8408
+#define GL_FRAGMENT_LIGHT_MODEL_TWO_SIDE_SGIX 0x8409
+#define GL_FRAGMENT_LIGHT_MODEL_AMBIENT_SGIX 0x840A
+#define GL_FRAGMENT_LIGHT_MODEL_NORMAL_INTERPOLATION_SGIX 0x840B
+#define GL_FRAGMENT_LIGHT0_SGIX 0x840C
+#define GL_FRAGMENT_LIGHT1_SGIX 0x840D
+#define GL_FRAGMENT_LIGHT2_SGIX 0x840E
+#define GL_FRAGMENT_LIGHT3_SGIX 0x840F
+#define GL_FRAGMENT_LIGHT4_SGIX 0x8410
+#define GL_FRAGMENT_LIGHT5_SGIX 0x8411
+#define GL_FRAGMENT_LIGHT6_SGIX 0x8412
+#define GL_FRAGMENT_LIGHT7_SGIX 0x8413
+#endif
+
+#ifndef GL_IBM_rasterpos_clip
+#define GL_RASTER_POSITION_UNCLIPPED_IBM 0x19262
+#endif
+
+#ifndef GL_HP_texture_lighting
+#define GL_TEXTURE_LIGHTING_MODE_HP 0x8167
+#define GL_TEXTURE_POST_SPECULAR_HP 0x8168
+#define GL_TEXTURE_PRE_SPECULAR_HP 0x8169
+#endif
+
+#ifndef GL_EXT_draw_range_elements
+#define GL_MAX_ELEMENTS_VERTICES_EXT 0x80E8
+#define GL_MAX_ELEMENTS_INDICES_EXT 0x80E9
+#endif
+
+#ifndef GL_WIN_phong_shading
+#define GL_PHONG_WIN 0x80EA
+#define GL_PHONG_HINT_WIN 0x80EB
+#endif
+
+#ifndef GL_WIN_specular_fog
+#define GL_FOG_SPECULAR_TEXTURE_WIN 0x80EC
+#endif
+
+#ifndef GL_EXT_light_texture
+#define GL_FRAGMENT_MATERIAL_EXT 0x8349
+#define GL_FRAGMENT_NORMAL_EXT 0x834A
+#define GL_FRAGMENT_COLOR_EXT 0x834C
+#define GL_ATTENUATION_EXT 0x834D
+#define GL_SHADOW_ATTENUATION_EXT 0x834E
+#define GL_TEXTURE_APPLICATION_MODE_EXT 0x834F
+#define GL_TEXTURE_LIGHT_EXT 0x8350
+#define GL_TEXTURE_MATERIAL_FACE_EXT 0x8351
+#define GL_TEXTURE_MATERIAL_PARAMETER_EXT 0x8352
+/* reuse GL_FRAGMENT_DEPTH_EXT */
+#endif
+
+#ifndef GL_SGIX_blend_alpha_minmax
+#define GL_ALPHA_MIN_SGIX 0x8320
+#define GL_ALPHA_MAX_SGIX 0x8321
+#endif
+
+#ifndef GL_SGIX_impact_pixel_texture
+#define GL_PIXEL_TEX_GEN_Q_CEILING_SGIX 0x8184
+#define GL_PIXEL_TEX_GEN_Q_ROUND_SGIX 0x8185
+#define GL_PIXEL_TEX_GEN_Q_FLOOR_SGIX 0x8186
+#define GL_PIXEL_TEX_GEN_ALPHA_REPLACE_SGIX 0x8187
+#define GL_PIXEL_TEX_GEN_ALPHA_NO_REPLACE_SGIX 0x8188
+#define GL_PIXEL_TEX_GEN_ALPHA_LS_SGIX 0x8189
+#define GL_PIXEL_TEX_GEN_ALPHA_MS_SGIX 0x818A
+#endif
+
+#ifndef GL_EXT_bgra
+#define GL_BGR_EXT 0x80E0
+#define GL_BGRA_EXT 0x80E1
+#endif
+
+#ifndef GL_SGIX_async
+#define GL_ASYNC_MARKER_SGIX 0x8329
+#endif
+
+#ifndef GL_SGIX_async_pixel
+#define GL_ASYNC_TEX_IMAGE_SGIX 0x835C
+#define GL_ASYNC_DRAW_PIXELS_SGIX 0x835D
+#define GL_ASYNC_READ_PIXELS_SGIX 0x835E
+#define GL_MAX_ASYNC_TEX_IMAGE_SGIX 0x835F
+#define GL_MAX_ASYNC_DRAW_PIXELS_SGIX 0x8360
+#define GL_MAX_ASYNC_READ_PIXELS_SGIX 0x8361
+#endif
+
+#ifndef GL_SGIX_async_histogram
+#define GL_ASYNC_HISTOGRAM_SGIX 0x832C
+#define GL_MAX_ASYNC_HISTOGRAM_SGIX 0x832D
+#endif
+
+#ifndef GL_INTEL_texture_scissor
+#endif
+
+#ifndef GL_INTEL_parallel_arrays
+#define GL_PARALLEL_ARRAYS_INTEL 0x83F4
+#define GL_VERTEX_ARRAY_PARALLEL_POINTERS_INTEL 0x83F5
+#define GL_NORMAL_ARRAY_PARALLEL_POINTERS_INTEL 0x83F6
+#define GL_COLOR_ARRAY_PARALLEL_POINTERS_INTEL 0x83F7
+#define GL_TEXTURE_COORD_ARRAY_PARALLEL_POINTERS_INTEL 0x83F8
+#endif
+
+#ifndef GL_HP_occlusion_test
+#define GL_OCCLUSION_TEST_HP 0x8165
+#define GL_OCCLUSION_TEST_RESULT_HP 0x8166
+#endif
+
+#ifndef GL_EXT_pixel_transform
+#define GL_PIXEL_TRANSFORM_2D_EXT 0x8330
+#define GL_PIXEL_MAG_FILTER_EXT 0x8331
+#define GL_PIXEL_MIN_FILTER_EXT 0x8332
+#define GL_PIXEL_CUBIC_WEIGHT_EXT 0x8333
+#define GL_CUBIC_EXT 0x8334
+#define GL_AVERAGE_EXT 0x8335
+#define GL_PIXEL_TRANSFORM_2D_STACK_DEPTH_EXT 0x8336
+#define GL_MAX_PIXEL_TRANSFORM_2D_STACK_DEPTH_EXT 0x8337
+#define GL_PIXEL_TRANSFORM_2D_MATRIX_EXT 0x8338
+#endif
+
+#ifndef GL_EXT_pixel_transform_color_table
+#endif
+
+#ifndef GL_EXT_shared_texture_palette
+#define GL_SHARED_TEXTURE_PALETTE_EXT 0x81FB
+#endif
+
+#ifndef GL_EXT_separate_specular_color
+#define GL_LIGHT_MODEL_COLOR_CONTROL_EXT 0x81F8
+#define GL_SINGLE_COLOR_EXT 0x81F9
+#define GL_SEPARATE_SPECULAR_COLOR_EXT 0x81FA
+#endif
+
+#ifndef GL_EXT_secondary_color
+#define GL_COLOR_SUM_EXT 0x8458
+#define GL_CURRENT_SECONDARY_COLOR_EXT 0x8459
+#define GL_SECONDARY_COLOR_ARRAY_SIZE_EXT 0x845A
+#define GL_SECONDARY_COLOR_ARRAY_TYPE_EXT 0x845B
+#define GL_SECONDARY_COLOR_ARRAY_STRIDE_EXT 0x845C
+#define GL_SECONDARY_COLOR_ARRAY_POINTER_EXT 0x845D
+#define GL_SECONDARY_COLOR_ARRAY_EXT 0x845E
+#endif
+
+#ifndef GL_EXT_texture_perturb_normal
+#define GL_PERTURB_EXT 0x85AE
+#define GL_TEXTURE_NORMAL_EXT 0x85AF
+#endif
+
+#ifndef GL_EXT_multi_draw_arrays
+#endif
+
+#ifndef GL_EXT_fog_coord
+#define GL_FOG_COORDINATE_SOURCE_EXT 0x8450
+#define GL_FOG_COORDINATE_EXT 0x8451
+#define GL_FRAGMENT_DEPTH_EXT 0x8452
+#define GL_CURRENT_FOG_COORDINATE_EXT 0x8453
+#define GL_FOG_COORDINATE_ARRAY_TYPE_EXT 0x8454
+#define GL_FOG_COORDINATE_ARRAY_STRIDE_EXT 0x8455
+#define GL_FOG_COORDINATE_ARRAY_POINTER_EXT 0x8456
+#define GL_FOG_COORDINATE_ARRAY_EXT 0x8457
+#endif
+
+#ifndef GL_REND_screen_coordinates
+#define GL_SCREEN_COORDINATES_REND 0x8490
+#define GL_INVERTED_SCREEN_W_REND 0x8491
+#endif
+
+#ifndef GL_EXT_coordinate_frame
+#define GL_TANGENT_ARRAY_EXT 0x8439
+#define GL_BINORMAL_ARRAY_EXT 0x843A
+#define GL_CURRENT_TANGENT_EXT 0x843B
+#define GL_CURRENT_BINORMAL_EXT 0x843C
+#define GL_TANGENT_ARRAY_TYPE_EXT 0x843E
+#define GL_TANGENT_ARRAY_STRIDE_EXT 0x843F
+#define GL_BINORMAL_ARRAY_TYPE_EXT 0x8440
+#define GL_BINORMAL_ARRAY_STRIDE_EXT 0x8441
+#define GL_TANGENT_ARRAY_POINTER_EXT 0x8442
+#define GL_BINORMAL_ARRAY_POINTER_EXT 0x8443
+#define GL_MAP1_TANGENT_EXT 0x8444
+#define GL_MAP2_TANGENT_EXT 0x8445
+#define GL_MAP1_BINORMAL_EXT 0x8446
+#define GL_MAP2_BINORMAL_EXT 0x8447
+#endif
+
+#ifndef GL_EXT_texture_env_combine
+#define GL_COMBINE_EXT 0x8570
+#define GL_COMBINE_RGB_EXT 0x8571
+#define GL_COMBINE_ALPHA_EXT 0x8572
+#define GL_RGB_SCALE_EXT 0x8573
+#define GL_ADD_SIGNED_EXT 0x8574
+#define GL_INTERPOLATE_EXT 0x8575
+#define GL_CONSTANT_EXT 0x8576
+#define GL_PRIMARY_COLOR_EXT 0x8577
+#define GL_PREVIOUS_EXT 0x8578
+#define GL_SOURCE0_RGB_EXT 0x8580
+#define GL_SOURCE1_RGB_EXT 0x8581
+#define GL_SOURCE2_RGB_EXT 0x8582
+#define GL_SOURCE0_ALPHA_EXT 0x8588
+#define GL_SOURCE1_ALPHA_EXT 0x8589
+#define GL_SOURCE2_ALPHA_EXT 0x858A
+#define GL_OPERAND0_RGB_EXT 0x8590
+#define GL_OPERAND1_RGB_EXT 0x8591
+#define GL_OPERAND2_RGB_EXT 0x8592
+#define GL_OPERAND0_ALPHA_EXT 0x8598
+#define GL_OPERAND1_ALPHA_EXT 0x8599
+#define GL_OPERAND2_ALPHA_EXT 0x859A
+#endif
+
+#ifndef GL_APPLE_specular_vector
+#define GL_LIGHT_MODEL_SPECULAR_VECTOR_APPLE 0x85B0
+#endif
+
+#ifndef GL_APPLE_transform_hint
+#define GL_TRANSFORM_HINT_APPLE 0x85B1
+#endif
+
+#ifndef GL_SGIX_fog_scale
+#define GL_FOG_SCALE_SGIX 0x81FC
+#define GL_FOG_SCALE_VALUE_SGIX 0x81FD
+#endif
+
+#ifndef GL_SUNX_constant_data
+#define GL_UNPACK_CONSTANT_DATA_SUNX 0x81D5
+#define GL_TEXTURE_CONSTANT_DATA_SUNX 0x81D6
+#endif
+
+#ifndef GL_SUN_global_alpha
+#define GL_GLOBAL_ALPHA_SUN 0x81D9
+#define GL_GLOBAL_ALPHA_FACTOR_SUN 0x81DA
+#endif
+
+#ifndef GL_SUN_triangle_list
+#define GL_RESTART_SUN 0x0001
+#define GL_REPLACE_MIDDLE_SUN 0x0002
+#define GL_REPLACE_OLDEST_SUN 0x0003
+#define GL_TRIANGLE_LIST_SUN 0x81D7
+#define GL_REPLACEMENT_CODE_SUN 0x81D8
+#define GL_REPLACEMENT_CODE_ARRAY_SUN 0x85C0
+#define GL_REPLACEMENT_CODE_ARRAY_TYPE_SUN 0x85C1
+#define GL_REPLACEMENT_CODE_ARRAY_STRIDE_SUN 0x85C2
+#define GL_REPLACEMENT_CODE_ARRAY_POINTER_SUN 0x85C3
+#define GL_R1UI_V3F_SUN 0x85C4
+#define GL_R1UI_C4UB_V3F_SUN 0x85C5
+#define GL_R1UI_C3F_V3F_SUN 0x85C6
+#define GL_R1UI_N3F_V3F_SUN 0x85C7
+#define GL_R1UI_C4F_N3F_V3F_SUN 0x85C8
+#define GL_R1UI_T2F_V3F_SUN 0x85C9
+#define GL_R1UI_T2F_N3F_V3F_SUN 0x85CA
+#define GL_R1UI_T2F_C4F_N3F_V3F_SUN 0x85CB
+#endif
+
+#ifndef GL_SUN_vertex
+#endif
+
+#ifndef GL_EXT_blend_func_separate
+#define GL_BLEND_DST_RGB_EXT 0x80C8
+#define GL_BLEND_SRC_RGB_EXT 0x80C9
+#define GL_BLEND_DST_ALPHA_EXT 0x80CA
+#define GL_BLEND_SRC_ALPHA_EXT 0x80CB
+#endif
+
+#ifndef GL_INGR_color_clamp
+#define GL_RED_MIN_CLAMP_INGR 0x8560
+#define GL_GREEN_MIN_CLAMP_INGR 0x8561
+#define GL_BLUE_MIN_CLAMP_INGR 0x8562
+#define GL_ALPHA_MIN_CLAMP_INGR 0x8563
+#define GL_RED_MAX_CLAMP_INGR 0x8564
+#define GL_GREEN_MAX_CLAMP_INGR 0x8565
+#define GL_BLUE_MAX_CLAMP_INGR 0x8566
+#define GL_ALPHA_MAX_CLAMP_INGR 0x8567
+#endif
+
+#ifndef GL_INGR_interlace_read
+#define GL_INTERLACE_READ_INGR 0x8568
+#endif
+
+#ifndef GL_EXT_stencil_wrap
+#define GL_INCR_WRAP_EXT 0x8507
+#define GL_DECR_WRAP_EXT 0x8508
+#endif
+
+#ifndef GL_EXT_422_pixels
+#define GL_422_EXT 0x80CC
+#define GL_422_REV_EXT 0x80CD
+#define GL_422_AVERAGE_EXT 0x80CE
+#define GL_422_REV_AVERAGE_EXT 0x80CF
+#endif
+
+#ifndef GL_NV_texgen_reflection
+#define GL_NORMAL_MAP_NV 0x8511
+#define GL_REFLECTION_MAP_NV 0x8512
+#endif
+
+#ifndef GL_EXT_texture_cube_map
+#define GL_NORMAL_MAP_EXT 0x8511
+#define GL_REFLECTION_MAP_EXT 0x8512
+#define GL_TEXTURE_CUBE_MAP_EXT 0x8513
+#define GL_TEXTURE_BINDING_CUBE_MAP_EXT 0x8514
+#define GL_TEXTURE_CUBE_MAP_POSITIVE_X_EXT 0x8515
+#define GL_TEXTURE_CUBE_MAP_NEGATIVE_X_EXT 0x8516
+#define GL_TEXTURE_CUBE_MAP_POSITIVE_Y_EXT 0x8517
+#define GL_TEXTURE_CUBE_MAP_NEGATIVE_Y_EXT 0x8518
+#define GL_TEXTURE_CUBE_MAP_POSITIVE_Z_EXT 0x8519
+#define GL_TEXTURE_CUBE_MAP_NEGATIVE_Z_EXT 0x851A
+#define GL_PROXY_TEXTURE_CUBE_MAP_EXT 0x851B
+#define GL_MAX_CUBE_MAP_TEXTURE_SIZE_EXT 0x851C
+#endif
+
+#ifndef GL_SUN_convolution_border_modes
+#define GL_WRAP_BORDER_SUN 0x81D4
+#endif
+
+#ifndef GL_EXT_texture_env_add
+#endif
+
+#ifndef GL_EXT_texture_lod_bias
+#define GL_MAX_TEXTURE_LOD_BIAS_EXT 0x84FD
+#define GL_TEXTURE_FILTER_CONTROL_EXT 0x8500
+#define GL_TEXTURE_LOD_BIAS_EXT 0x8501
+#endif
+
+#ifndef GL_EXT_texture_filter_anisotropic
+#define GL_TEXTURE_MAX_ANISOTROPY_EXT 0x84FE
+#define GL_MAX_TEXTURE_MAX_ANISOTROPY_EXT 0x84FF
+#endif
+
+#ifndef GL_EXT_vertex_weighting
+#define GL_MODELVIEW0_STACK_DEPTH_EXT GL_MODELVIEW_STACK_DEPTH
+#define GL_MODELVIEW1_STACK_DEPTH_EXT 0x8502
+#define GL_MODELVIEW0_MATRIX_EXT GL_MODELVIEW_MATRIX
+#define GL_MODELVIEW1_MATRIX_EXT 0x8506
+#define GL_VERTEX_WEIGHTING_EXT 0x8509
+#define GL_MODELVIEW0_EXT GL_MODELVIEW
+#define GL_MODELVIEW1_EXT 0x850A
+#define GL_CURRENT_VERTEX_WEIGHT_EXT 0x850B
+#define GL_VERTEX_WEIGHT_ARRAY_EXT 0x850C
+#define GL_VERTEX_WEIGHT_ARRAY_SIZE_EXT 0x850D
+#define GL_VERTEX_WEIGHT_ARRAY_TYPE_EXT 0x850E
+#define GL_VERTEX_WEIGHT_ARRAY_STRIDE_EXT 0x850F
+#define GL_VERTEX_WEIGHT_ARRAY_POINTER_EXT 0x8510
+#endif
+
+#ifndef GL_NV_light_max_exponent
+#define GL_MAX_SHININESS_NV 0x8504
+#define GL_MAX_SPOT_EXPONENT_NV 0x8505
+#endif
+
+#ifndef GL_NV_vertex_array_range
+#define GL_VERTEX_ARRAY_RANGE_NV 0x851D
+#define GL_VERTEX_ARRAY_RANGE_LENGTH_NV 0x851E
+#define GL_VERTEX_ARRAY_RANGE_VALID_NV 0x851F
+#define GL_MAX_VERTEX_ARRAY_RANGE_ELEMENT_NV 0x8520
+#define GL_VERTEX_ARRAY_RANGE_POINTER_NV 0x8521
+#endif
+
+#ifndef GL_NV_register_combiners
+#define GL_REGISTER_COMBINERS_NV 0x8522
+#define GL_VARIABLE_A_NV 0x8523
+#define GL_VARIABLE_B_NV 0x8524
+#define GL_VARIABLE_C_NV 0x8525
+#define GL_VARIABLE_D_NV 0x8526
+#define GL_VARIABLE_E_NV 0x8527
+#define GL_VARIABLE_F_NV 0x8528
+#define GL_VARIABLE_G_NV 0x8529
+#define GL_CONSTANT_COLOR0_NV 0x852A
+#define GL_CONSTANT_COLOR1_NV 0x852B
+#define GL_PRIMARY_COLOR_NV 0x852C
+#define GL_SECONDARY_COLOR_NV 0x852D
+#define GL_SPARE0_NV 0x852E
+#define GL_SPARE1_NV 0x852F
+#define GL_DISCARD_NV 0x8530
+#define GL_E_TIMES_F_NV 0x8531
+#define GL_SPARE0_PLUS_SECONDARY_COLOR_NV 0x8532
+#define GL_UNSIGNED_IDENTITY_NV 0x8536
+#define GL_UNSIGNED_INVERT_NV 0x8537
+#define GL_EXPAND_NORMAL_NV 0x8538
+#define GL_EXPAND_NEGATE_NV 0x8539
+#define GL_HALF_BIAS_NORMAL_NV 0x853A
+#define GL_HALF_BIAS_NEGATE_NV 0x853B
+#define GL_SIGNED_IDENTITY_NV 0x853C
+#define GL_SIGNED_NEGATE_NV 0x853D
+#define GL_SCALE_BY_TWO_NV 0x853E
+#define GL_SCALE_BY_FOUR_NV 0x853F
+#define GL_SCALE_BY_ONE_HALF_NV 0x8540
+#define GL_BIAS_BY_NEGATIVE_ONE_HALF_NV 0x8541
+#define GL_COMBINER_INPUT_NV 0x8542
+#define GL_COMBINER_MAPPING_NV 0x8543
+#define GL_COMBINER_COMPONENT_USAGE_NV 0x8544
+#define GL_COMBINER_AB_DOT_PRODUCT_NV 0x8545
+#define GL_COMBINER_CD_DOT_PRODUCT_NV 0x8546
+#define GL_COMBINER_MUX_SUM_NV 0x8547
+#define GL_COMBINER_SCALE_NV 0x8548
+#define GL_COMBINER_BIAS_NV 0x8549
+#define GL_COMBINER_AB_OUTPUT_NV 0x854A
+#define GL_COMBINER_CD_OUTPUT_NV 0x854B
+#define GL_COMBINER_SUM_OUTPUT_NV 0x854C
+#define GL_MAX_GENERAL_COMBINERS_NV 0x854D
+#define GL_NUM_GENERAL_COMBINERS_NV 0x854E
+#define GL_COLOR_SUM_CLAMP_NV 0x854F
+#define GL_COMBINER0_NV 0x8550
+#define GL_COMBINER1_NV 0x8551
+#define GL_COMBINER2_NV 0x8552
+#define GL_COMBINER3_NV 0x8553
+#define GL_COMBINER4_NV 0x8554
+#define GL_COMBINER5_NV 0x8555
+#define GL_COMBINER6_NV 0x8556
+#define GL_COMBINER7_NV 0x8557
+/* reuse GL_TEXTURE0_ARB */
+/* reuse GL_TEXTURE1_ARB */
+/* reuse GL_ZERO */
+/* reuse GL_NONE */
+/* reuse GL_FOG */
+#endif
+
+#ifndef GL_NV_fog_distance
+#define GL_FOG_DISTANCE_MODE_NV 0x855A
+#define GL_EYE_RADIAL_NV 0x855B
+#define GL_EYE_PLANE_ABSOLUTE_NV 0x855C
+/* reuse GL_EYE_PLANE */
+#endif
+
+#ifndef GL_NV_texgen_emboss
+#define GL_EMBOSS_LIGHT_NV 0x855D
+#define GL_EMBOSS_CONSTANT_NV 0x855E
+#define GL_EMBOSS_MAP_NV 0x855F
+#endif
+
+#ifndef GL_NV_blend_square
+#endif
+
+#ifndef GL_NV_texture_env_combine4
+#define GL_COMBINE4_NV 0x8503
+#define GL_SOURCE3_RGB_NV 0x8583
+#define GL_SOURCE3_ALPHA_NV 0x858B
+#define GL_OPERAND3_RGB_NV 0x8593
+#define GL_OPERAND3_ALPHA_NV 0x859B
+#endif
+
+#ifndef GL_MESA_resize_buffers
+#endif
+
+#ifndef GL_MESA_window_pos
+#endif
+
+#ifndef GL_EXT_texture_compression_s3tc
+#define GL_COMPRESSED_RGB_S3TC_DXT1_EXT 0x83F0
+#define GL_COMPRESSED_RGBA_S3TC_DXT1_EXT 0x83F1
+#define GL_COMPRESSED_RGBA_S3TC_DXT3_EXT 0x83F2
+#define GL_COMPRESSED_RGBA_S3TC_DXT5_EXT 0x83F3
+#endif
+
+#ifndef GL_IBM_cull_vertex
+#define GL_CULL_VERTEX_IBM 103050
+#endif
+
+#ifndef GL_IBM_multimode_draw_arrays
+#endif
+
+#ifndef GL_IBM_vertex_array_lists
+#define GL_VERTEX_ARRAY_LIST_IBM 103070
+#define GL_NORMAL_ARRAY_LIST_IBM 103071
+#define GL_COLOR_ARRAY_LIST_IBM 103072
+#define GL_INDEX_ARRAY_LIST_IBM 103073
+#define GL_TEXTURE_COORD_ARRAY_LIST_IBM 103074
+#define GL_EDGE_FLAG_ARRAY_LIST_IBM 103075
+#define GL_FOG_COORDINATE_ARRAY_LIST_IBM 103076
+#define GL_SECONDARY_COLOR_ARRAY_LIST_IBM 103077
+#define GL_VERTEX_ARRAY_LIST_STRIDE_IBM 103080
+#define GL_NORMAL_ARRAY_LIST_STRIDE_IBM 103081
+#define GL_COLOR_ARRAY_LIST_STRIDE_IBM 103082
+#define GL_INDEX_ARRAY_LIST_STRIDE_IBM 103083
+#define GL_TEXTURE_COORD_ARRAY_LIST_STRIDE_IBM 103084
+#define GL_EDGE_FLAG_ARRAY_LIST_STRIDE_IBM 103085
+#define GL_FOG_COORDINATE_ARRAY_LIST_STRIDE_IBM 103086
+#define GL_SECONDARY_COLOR_ARRAY_LIST_STRIDE_IBM 103087
+#endif
+
+#ifndef GL_SGIX_subsample
+#define GL_PACK_SUBSAMPLE_RATE_SGIX 0x85A0
+#define GL_UNPACK_SUBSAMPLE_RATE_SGIX 0x85A1
+#define GL_PIXEL_SUBSAMPLE_4444_SGIX 0x85A2
+#define GL_PIXEL_SUBSAMPLE_2424_SGIX 0x85A3
+#define GL_PIXEL_SUBSAMPLE_4242_SGIX 0x85A4
+#endif
+
+#ifndef GL_SGIX_ycrcb_subsample
+#endif
+
+#ifndef GL_SGIX_ycrcba
+#define GL_YCRCB_SGIX 0x8318
+#define GL_YCRCBA_SGIX 0x8319
+#endif
+
+#ifndef GL_SGI_depth_pass_instrument
+#define GL_DEPTH_PASS_INSTRUMENT_SGIX 0x8310
+#define GL_DEPTH_PASS_INSTRUMENT_COUNTERS_SGIX 0x8311
+#define GL_DEPTH_PASS_INSTRUMENT_MAX_SGIX 0x8312
+#endif
+
+#ifndef GL_3DFX_texture_compression_FXT1
+#define GL_COMPRESSED_RGB_FXT1_3DFX 0x86B0
+#define GL_COMPRESSED_RGBA_FXT1_3DFX 0x86B1
+#endif
+
+#ifndef GL_3DFX_multisample
+#define GL_MULTISAMPLE_3DFX 0x86B2
+#define GL_SAMPLE_BUFFERS_3DFX 0x86B3
+#define GL_SAMPLES_3DFX 0x86B4
+#define GL_MULTISAMPLE_BIT_3DFX 0x20000000
+#endif
+
+#ifndef GL_3DFX_tbuffer
+#endif
+
+#ifndef GL_EXT_multisample
+#define GL_MULTISAMPLE_EXT 0x809D
+#define GL_SAMPLE_ALPHA_TO_MASK_EXT 0x809E
+#define GL_SAMPLE_ALPHA_TO_ONE_EXT 0x809F
+#define GL_SAMPLE_MASK_EXT 0x80A0
+#define GL_1PASS_EXT 0x80A1
+#define GL_2PASS_0_EXT 0x80A2
+#define GL_2PASS_1_EXT 0x80A3
+#define GL_4PASS_0_EXT 0x80A4
+#define GL_4PASS_1_EXT 0x80A5
+#define GL_4PASS_2_EXT 0x80A6
+#define GL_4PASS_3_EXT 0x80A7
+#define GL_SAMPLE_BUFFERS_EXT 0x80A8
+#define GL_SAMPLES_EXT 0x80A9
+#define GL_SAMPLE_MASK_VALUE_EXT 0x80AA
+#define GL_SAMPLE_MASK_INVERT_EXT 0x80AB
+#define GL_SAMPLE_PATTERN_EXT 0x80AC
+#define GL_MULTISAMPLE_BIT_EXT 0x20000000
+#endif
+
+#ifndef GL_SGIX_vertex_preclip
+#define GL_VERTEX_PRECLIP_SGIX 0x83EE
+#define GL_VERTEX_PRECLIP_HINT_SGIX 0x83EF
+#endif
+
+#ifndef GL_SGIX_convolution_accuracy
+#define GL_CONVOLUTION_HINT_SGIX 0x8316
+#endif
+
+#ifndef GL_SGIX_resample
+#define GL_PACK_RESAMPLE_SGIX 0x842C
+#define GL_UNPACK_RESAMPLE_SGIX 0x842D
+#define GL_RESAMPLE_REPLICATE_SGIX 0x842E
+#define GL_RESAMPLE_ZERO_FILL_SGIX 0x842F
+#define GL_RESAMPLE_DECIMATE_SGIX 0x8430
+#endif
+
+#ifndef GL_SGIS_point_line_texgen
+#define GL_EYE_DISTANCE_TO_POINT_SGIS 0x81F0
+#define GL_OBJECT_DISTANCE_TO_POINT_SGIS 0x81F1
+#define GL_EYE_DISTANCE_TO_LINE_SGIS 0x81F2
+#define GL_OBJECT_DISTANCE_TO_LINE_SGIS 0x81F3
+#define GL_EYE_POINT_SGIS 0x81F4
+#define GL_OBJECT_POINT_SGIS 0x81F5
+#define GL_EYE_LINE_SGIS 0x81F6
+#define GL_OBJECT_LINE_SGIS 0x81F7
+#endif
+
+#ifndef GL_SGIS_texture_color_mask
+#define GL_TEXTURE_COLOR_WRITEMASK_SGIS 0x81EF
+#endif
+
+#ifndef GL_EXT_texture_env_dot3
+#define GL_DOT3_RGB_EXT 0x8740
+#define GL_DOT3_RGBA_EXT 0x8741
+#endif
+
+#ifndef GL_ATI_texture_mirror_once
+#define GL_MIRROR_CLAMP_ATI 0x8742
+#define GL_MIRROR_CLAMP_TO_EDGE_ATI 0x8743
+#endif
+
+#ifndef GL_NV_fence
+#define GL_ALL_COMPLETED_NV 0x84F2
+#define GL_FENCE_STATUS_NV 0x84F3
+#define GL_FENCE_CONDITION_NV 0x84F4
+#endif
+
+#ifndef GL_IBM_texture_mirrored_repeat
+#define GL_MIRRORED_REPEAT_IBM 0x8370
+#endif
+
+#ifndef GL_NV_evaluators
+#define GL_EVAL_2D_NV 0x86C0
+#define GL_EVAL_TRIANGULAR_2D_NV 0x86C1
+#define GL_MAP_TESSELLATION_NV 0x86C2
+#define GL_MAP_ATTRIB_U_ORDER_NV 0x86C3
+#define GL_MAP_ATTRIB_V_ORDER_NV 0x86C4
+#define GL_EVAL_FRACTIONAL_TESSELLATION_NV 0x86C5
+#define GL_EVAL_VERTEX_ATTRIB0_NV 0x86C6
+#define GL_EVAL_VERTEX_ATTRIB1_NV 0x86C7
+#define GL_EVAL_VERTEX_ATTRIB2_NV 0x86C8
+#define GL_EVAL_VERTEX_ATTRIB3_NV 0x86C9
+#define GL_EVAL_VERTEX_ATTRIB4_NV 0x86CA
+#define GL_EVAL_VERTEX_ATTRIB5_NV 0x86CB
+#define GL_EVAL_VERTEX_ATTRIB6_NV 0x86CC
+#define GL_EVAL_VERTEX_ATTRIB7_NV 0x86CD
+#define GL_EVAL_VERTEX_ATTRIB8_NV 0x86CE
+#define GL_EVAL_VERTEX_ATTRIB9_NV 0x86CF
+#define GL_EVAL_VERTEX_ATTRIB10_NV 0x86D0
+#define GL_EVAL_VERTEX_ATTRIB11_NV 0x86D1
+#define GL_EVAL_VERTEX_ATTRIB12_NV 0x86D2
+#define GL_EVAL_VERTEX_ATTRIB13_NV 0x86D3
+#define GL_EVAL_VERTEX_ATTRIB14_NV 0x86D4
+#define GL_EVAL_VERTEX_ATTRIB15_NV 0x86D5
+#define GL_MAX_MAP_TESSELLATION_NV 0x86D6
+#define GL_MAX_RATIONAL_EVAL_ORDER_NV 0x86D7
+#endif
+
+#ifndef GL_NV_packed_depth_stencil
+#define GL_DEPTH_STENCIL_NV 0x84F9
+#define GL_UNSIGNED_INT_24_8_NV 0x84FA
+#endif
+
+#ifndef GL_NV_register_combiners2
+#define GL_PER_STAGE_CONSTANTS_NV 0x8535
+#endif
+
+#ifndef GL_NV_texture_compression_vtc
+#endif
+
+#ifndef GL_NV_texture_rectangle
+#define GL_TEXTURE_RECTANGLE_NV 0x84F5
+#define GL_TEXTURE_BINDING_RECTANGLE_NV 0x84F6
+#define GL_PROXY_TEXTURE_RECTANGLE_NV 0x84F7
+#define GL_MAX_RECTANGLE_TEXTURE_SIZE_NV 0x84F8
+#endif
+
+#ifndef GL_NV_texture_shader
+#define GL_OFFSET_TEXTURE_RECTANGLE_NV 0x864C
+#define GL_OFFSET_TEXTURE_RECTANGLE_SCALE_NV 0x864D
+#define GL_DOT_PRODUCT_TEXTURE_RECTANGLE_NV 0x864E
+#define GL_RGBA_UNSIGNED_DOT_PRODUCT_MAPPING_NV 0x86D9
+#define GL_UNSIGNED_INT_S8_S8_8_8_NV 0x86DA
+#define GL_UNSIGNED_INT_8_8_S8_S8_REV_NV 0x86DB
+#define GL_DSDT_MAG_INTENSITY_NV 0x86DC
+#define GL_SHADER_CONSISTENT_NV 0x86DD
+#define GL_TEXTURE_SHADER_NV 0x86DE
+#define GL_SHADER_OPERATION_NV 0x86DF
+#define GL_CULL_MODES_NV 0x86E0
+#define GL_OFFSET_TEXTURE_MATRIX_NV 0x86E1
+#define GL_OFFSET_TEXTURE_SCALE_NV 0x86E2
+#define GL_OFFSET_TEXTURE_BIAS_NV 0x86E3
+#define GL_OFFSET_TEXTURE_2D_MATRIX_NV GL_OFFSET_TEXTURE_MATRIX_NV
+#define GL_OFFSET_TEXTURE_2D_SCALE_NV GL_OFFSET_TEXTURE_SCALE_NV
+#define GL_OFFSET_TEXTURE_2D_BIAS_NV GL_OFFSET_TEXTURE_BIAS_NV
+#define GL_PREVIOUS_TEXTURE_INPUT_NV 0x86E4
+#define GL_CONST_EYE_NV 0x86E5
+#define GL_PASS_THROUGH_NV 0x86E6
+#define GL_CULL_FRAGMENT_NV 0x86E7
+#define GL_OFFSET_TEXTURE_2D_NV 0x86E8
+#define GL_DEPENDENT_AR_TEXTURE_2D_NV 0x86E9
+#define GL_DEPENDENT_GB_TEXTURE_2D_NV 0x86EA
+#define GL_DOT_PRODUCT_NV 0x86EC
+#define GL_DOT_PRODUCT_DEPTH_REPLACE_NV 0x86ED
+#define GL_DOT_PRODUCT_TEXTURE_2D_NV 0x86EE
+#define GL_DOT_PRODUCT_TEXTURE_CUBE_MAP_NV 0x86F0
+#define GL_DOT_PRODUCT_DIFFUSE_CUBE_MAP_NV 0x86F1
+#define GL_DOT_PRODUCT_REFLECT_CUBE_MAP_NV 0x86F2
+#define GL_DOT_PRODUCT_CONST_EYE_REFLECT_CUBE_MAP_NV 0x86F3
+#define GL_HILO_NV 0x86F4
+#define GL_DSDT_NV 0x86F5
+#define GL_DSDT_MAG_NV 0x86F6
+#define GL_DSDT_MAG_VIB_NV 0x86F7
+#define GL_HILO16_NV 0x86F8
+#define GL_SIGNED_HILO_NV 0x86F9
+#define GL_SIGNED_HILO16_NV 0x86FA
+#define GL_SIGNED_RGBA_NV 0x86FB
+#define GL_SIGNED_RGBA8_NV 0x86FC
+#define GL_SIGNED_RGB_NV 0x86FE
+#define GL_SIGNED_RGB8_NV 0x86FF
+#define GL_SIGNED_LUMINANCE_NV 0x8701
+#define GL_SIGNED_LUMINANCE8_NV 0x8702
+#define GL_SIGNED_LUMINANCE_ALPHA_NV 0x8703
+#define GL_SIGNED_LUMINANCE8_ALPHA8_NV 0x8704
+#define GL_SIGNED_ALPHA_NV 0x8705
+#define GL_SIGNED_ALPHA8_NV 0x8706
+#define GL_SIGNED_INTENSITY_NV 0x8707
+#define GL_SIGNED_INTENSITY8_NV 0x8708
+#define GL_DSDT8_NV 0x8709
+#define GL_DSDT8_MAG8_NV 0x870A
+#define GL_DSDT8_MAG8_INTENSITY8_NV 0x870B
+#define GL_SIGNED_RGB_UNSIGNED_ALPHA_NV 0x870C
+#define GL_SIGNED_RGB8_UNSIGNED_ALPHA8_NV 0x870D
+#define GL_HI_SCALE_NV 0x870E
+#define GL_LO_SCALE_NV 0x870F
+#define GL_DS_SCALE_NV 0x8710
+#define GL_DT_SCALE_NV 0x8711
+#define GL_MAGNITUDE_SCALE_NV 0x8712
+#define GL_VIBRANCE_SCALE_NV 0x8713
+#define GL_HI_BIAS_NV 0x8714
+#define GL_LO_BIAS_NV 0x8715
+#define GL_DS_BIAS_NV 0x8716
+#define GL_DT_BIAS_NV 0x8717
+#define GL_MAGNITUDE_BIAS_NV 0x8718
+#define GL_VIBRANCE_BIAS_NV 0x8719
+#define GL_TEXTURE_BORDER_VALUES_NV 0x871A
+#define GL_TEXTURE_HI_SIZE_NV 0x871B
+#define GL_TEXTURE_LO_SIZE_NV 0x871C
+#define GL_TEXTURE_DS_SIZE_NV 0x871D
+#define GL_TEXTURE_DT_SIZE_NV 0x871E
+#define GL_TEXTURE_MAG_SIZE_NV 0x871F
+#endif
+
+#ifndef GL_NV_texture_shader2
+#define GL_DOT_PRODUCT_TEXTURE_3D_NV 0x86EF
+#endif
+
+#ifndef GL_NV_vertex_array_range2
+#define GL_VERTEX_ARRAY_RANGE_WITHOUT_FLUSH_NV 0x8533
+#endif
+
+#ifndef GL_NV_vertex_program
+#define GL_VERTEX_PROGRAM_NV 0x8620
+#define GL_VERTEX_STATE_PROGRAM_NV 0x8621
+#define GL_ATTRIB_ARRAY_SIZE_NV 0x8623
+#define GL_ATTRIB_ARRAY_STRIDE_NV 0x8624
+#define GL_ATTRIB_ARRAY_TYPE_NV 0x8625
+#define GL_CURRENT_ATTRIB_NV 0x8626
+#define GL_PROGRAM_LENGTH_NV 0x8627
+#define GL_PROGRAM_STRING_NV 0x8628
+#define GL_MODELVIEW_PROJECTION_NV 0x8629
+#define GL_IDENTITY_NV 0x862A
+#define GL_INVERSE_NV 0x862B
+#define GL_TRANSPOSE_NV 0x862C
+#define GL_INVERSE_TRANSPOSE_NV 0x862D
+#define GL_MAX_TRACK_MATRIX_STACK_DEPTH_NV 0x862E
+#define GL_MAX_TRACK_MATRICES_NV 0x862F
+#define GL_MATRIX0_NV 0x8630
+#define GL_MATRIX1_NV 0x8631
+#define GL_MATRIX2_NV 0x8632
+#define GL_MATRIX3_NV 0x8633
+#define GL_MATRIX4_NV 0x8634
+#define GL_MATRIX5_NV 0x8635
+#define GL_MATRIX6_NV 0x8636
+#define GL_MATRIX7_NV 0x8637
+#define GL_CURRENT_MATRIX_STACK_DEPTH_NV 0x8640
+#define GL_CURRENT_MATRIX_NV 0x8641
+#define GL_VERTEX_PROGRAM_POINT_SIZE_NV 0x8642
+#define GL_VERTEX_PROGRAM_TWO_SIDE_NV 0x8643
+#define GL_PROGRAM_PARAMETER_NV 0x8644
+#define GL_ATTRIB_ARRAY_POINTER_NV 0x8645
+#define GL_PROGRAM_TARGET_NV 0x8646
+#define GL_PROGRAM_RESIDENT_NV 0x8647
+#define GL_TRACK_MATRIX_NV 0x8648
+#define GL_TRACK_MATRIX_TRANSFORM_NV 0x8649
+#define GL_VERTEX_PROGRAM_BINDING_NV 0x864A
+#define GL_PROGRAM_ERROR_POSITION_NV 0x864B
+#define GL_VERTEX_ATTRIB_ARRAY0_NV 0x8650
+#define GL_VERTEX_ATTRIB_ARRAY1_NV 0x8651
+#define GL_VERTEX_ATTRIB_ARRAY2_NV 0x8652
+#define GL_VERTEX_ATTRIB_ARRAY3_NV 0x8653
+#define GL_VERTEX_ATTRIB_ARRAY4_NV 0x8654
+#define GL_VERTEX_ATTRIB_ARRAY5_NV 0x8655
+#define GL_VERTEX_ATTRIB_ARRAY6_NV 0x8656
+#define GL_VERTEX_ATTRIB_ARRAY7_NV 0x8657
+#define GL_VERTEX_ATTRIB_ARRAY8_NV 0x8658
+#define GL_VERTEX_ATTRIB_ARRAY9_NV 0x8659
+#define GL_VERTEX_ATTRIB_ARRAY10_NV 0x865A
+#define GL_VERTEX_ATTRIB_ARRAY11_NV 0x865B
+#define GL_VERTEX_ATTRIB_ARRAY12_NV 0x865C
+#define GL_VERTEX_ATTRIB_ARRAY13_NV 0x865D
+#define GL_VERTEX_ATTRIB_ARRAY14_NV 0x865E
+#define GL_VERTEX_ATTRIB_ARRAY15_NV 0x865F
+#define GL_MAP1_VERTEX_ATTRIB0_4_NV 0x8660
+#define GL_MAP1_VERTEX_ATTRIB1_4_NV 0x8661
+#define GL_MAP1_VERTEX_ATTRIB2_4_NV 0x8662
+#define GL_MAP1_VERTEX_ATTRIB3_4_NV 0x8663
+#define GL_MAP1_VERTEX_ATTRIB4_4_NV 0x8664
+#define GL_MAP1_VERTEX_ATTRIB5_4_NV 0x8665
+#define GL_MAP1_VERTEX_ATTRIB6_4_NV 0x8666
+#define GL_MAP1_VERTEX_ATTRIB7_4_NV 0x8667
+#define GL_MAP1_VERTEX_ATTRIB8_4_NV 0x8668
+#define GL_MAP1_VERTEX_ATTRIB9_4_NV 0x8669
+#define GL_MAP1_VERTEX_ATTRIB10_4_NV 0x866A
+#define GL_MAP1_VERTEX_ATTRIB11_4_NV 0x866B
+#define GL_MAP1_VERTEX_ATTRIB12_4_NV 0x866C
+#define GL_MAP1_VERTEX_ATTRIB13_4_NV 0x866D
+#define GL_MAP1_VERTEX_ATTRIB14_4_NV 0x866E
+#define GL_MAP1_VERTEX_ATTRIB15_4_NV 0x866F
+#define GL_MAP2_VERTEX_ATTRIB0_4_NV 0x8670
+#define GL_MAP2_VERTEX_ATTRIB1_4_NV 0x8671
+#define GL_MAP2_VERTEX_ATTRIB2_4_NV 0x8672
+#define GL_MAP2_VERTEX_ATTRIB3_4_NV 0x8673
+#define GL_MAP2_VERTEX_ATTRIB4_4_NV 0x8674
+#define GL_MAP2_VERTEX_ATTRIB5_4_NV 0x8675
+#define GL_MAP2_VERTEX_ATTRIB6_4_NV 0x8676
+#define GL_MAP2_VERTEX_ATTRIB7_4_NV 0x8677
+#define GL_MAP2_VERTEX_ATTRIB8_4_NV 0x8678
+#define GL_MAP2_VERTEX_ATTRIB9_4_NV 0x8679
+#define GL_MAP2_VERTEX_ATTRIB10_4_NV 0x867A
+#define GL_MAP2_VERTEX_ATTRIB11_4_NV 0x867B
+#define GL_MAP2_VERTEX_ATTRIB12_4_NV 0x867C
+#define GL_MAP2_VERTEX_ATTRIB13_4_NV 0x867D
+#define GL_MAP2_VERTEX_ATTRIB14_4_NV 0x867E
+#define GL_MAP2_VERTEX_ATTRIB15_4_NV 0x867F
+#endif
+
+#ifndef GL_SGIX_texture_coordinate_clamp
+#define GL_TEXTURE_MAX_CLAMP_S_SGIX 0x8369
+#define GL_TEXTURE_MAX_CLAMP_T_SGIX 0x836A
+#define GL_TEXTURE_MAX_CLAMP_R_SGIX 0x836B
+#endif
+
+#ifndef GL_SGIX_scalebias_hint
+#define GL_SCALEBIAS_HINT_SGIX 0x8322
+#endif
+
+#ifndef GL_OML_interlace
+#define GL_INTERLACE_OML 0x8980
+#define GL_INTERLACE_READ_OML 0x8981
+#endif
+
+#ifndef GL_OML_subsample
+#define GL_FORMAT_SUBSAMPLE_24_24_OML 0x8982
+#define GL_FORMAT_SUBSAMPLE_244_244_OML 0x8983
+#endif
+
+#ifndef GL_OML_resample
+#define GL_PACK_RESAMPLE_OML 0x8984
+#define GL_UNPACK_RESAMPLE_OML 0x8985
+#define GL_RESAMPLE_REPLICATE_OML 0x8986
+#define GL_RESAMPLE_ZERO_FILL_OML 0x8987
+#define GL_RESAMPLE_AVERAGE_OML 0x8988
+#define GL_RESAMPLE_DECIMATE_OML 0x8989
+#endif
+
+#ifndef GL_NV_copy_depth_to_color
+#define GL_DEPTH_STENCIL_TO_RGBA_NV 0x886E
+#define GL_DEPTH_STENCIL_TO_BGRA_NV 0x886F
+#endif
+
+#ifndef GL_ATI_envmap_bumpmap
+#define GL_BUMP_ROT_MATRIX_ATI 0x8775
+#define GL_BUMP_ROT_MATRIX_SIZE_ATI 0x8776
+#define GL_BUMP_NUM_TEX_UNITS_ATI 0x8777
+#define GL_BUMP_TEX_UNITS_ATI 0x8778
+#define GL_DUDV_ATI 0x8779
+#define GL_DU8DV8_ATI 0x877A
+#define GL_BUMP_ENVMAP_ATI 0x877B
+#define GL_BUMP_TARGET_ATI 0x877C
+#endif
+
+#ifndef GL_ATI_fragment_shader
+#define GL_FRAGMENT_SHADER_ATI 0x8920
+#define GL_REG_0_ATI 0x8921
+#define GL_REG_1_ATI 0x8922
+#define GL_REG_2_ATI 0x8923
+#define GL_REG_3_ATI 0x8924
+#define GL_REG_4_ATI 0x8925
+#define GL_REG_5_ATI 0x8926
+#define GL_REG_6_ATI 0x8927
+#define GL_REG_7_ATI 0x8928
+#define GL_REG_8_ATI 0x8929
+#define GL_REG_9_ATI 0x892A
+#define GL_REG_10_ATI 0x892B
+#define GL_REG_11_ATI 0x892C
+#define GL_REG_12_ATI 0x892D
+#define GL_REG_13_ATI 0x892E
+#define GL_REG_14_ATI 0x892F
+#define GL_REG_15_ATI 0x8930
+#define GL_REG_16_ATI 0x8931
+#define GL_REG_17_ATI 0x8932
+#define GL_REG_18_ATI 0x8933
+#define GL_REG_19_ATI 0x8934
+#define GL_REG_20_ATI 0x8935
+#define GL_REG_21_ATI 0x8936
+#define GL_REG_22_ATI 0x8937
+#define GL_REG_23_ATI 0x8938
+#define GL_REG_24_ATI 0x8939
+#define GL_REG_25_ATI 0x893A
+#define GL_REG_26_ATI 0x893B
+#define GL_REG_27_ATI 0x893C
+#define GL_REG_28_ATI 0x893D
+#define GL_REG_29_ATI 0x893E
+#define GL_REG_30_ATI 0x893F
+#define GL_REG_31_ATI 0x8940
+#define GL_CON_0_ATI 0x8941
+#define GL_CON_1_ATI 0x8942
+#define GL_CON_2_ATI 0x8943
+#define GL_CON_3_ATI 0x8944
+#define GL_CON_4_ATI 0x8945
+#define GL_CON_5_ATI 0x8946
+#define GL_CON_6_ATI 0x8947
+#define GL_CON_7_ATI 0x8948
+#define GL_CON_8_ATI 0x8949
+#define GL_CON_9_ATI 0x894A
+#define GL_CON_10_ATI 0x894B
+#define GL_CON_11_ATI 0x894C
+#define GL_CON_12_ATI 0x894D
+#define GL_CON_13_ATI 0x894E
+#define GL_CON_14_ATI 0x894F
+#define GL_CON_15_ATI 0x8950
+#define GL_CON_16_ATI 0x8951
+#define GL_CON_17_ATI 0x8952
+#define GL_CON_18_ATI 0x8953
+#define GL_CON_19_ATI 0x8954
+#define GL_CON_20_ATI 0x8955
+#define GL_CON_21_ATI 0x8956
+#define GL_CON_22_ATI 0x8957
+#define GL_CON_23_ATI 0x8958
+#define GL_CON_24_ATI 0x8959
+#define GL_CON_25_ATI 0x895A
+#define GL_CON_26_ATI 0x895B
+#define GL_CON_27_ATI 0x895C
+#define GL_CON_28_ATI 0x895D
+#define GL_CON_29_ATI 0x895E
+#define GL_CON_30_ATI 0x895F
+#define GL_CON_31_ATI 0x8960
+#define GL_MOV_ATI 0x8961
+#define GL_ADD_ATI 0x8963
+#define GL_MUL_ATI 0x8964
+#define GL_SUB_ATI 0x8965
+#define GL_DOT3_ATI 0x8966
+#define GL_DOT4_ATI 0x8967
+#define GL_MAD_ATI 0x8968
+#define GL_LERP_ATI 0x8969
+#define GL_CND_ATI 0x896A
+#define GL_CND0_ATI 0x896B
+#define GL_DOT2_ADD_ATI 0x896C
+#define GL_SECONDARY_INTERPOLATOR_ATI 0x896D
+#define GL_NUM_FRAGMENT_REGISTERS_ATI 0x896E
+#define GL_NUM_FRAGMENT_CONSTANTS_ATI 0x896F
+#define GL_NUM_PASSES_ATI 0x8970
+#define GL_NUM_INSTRUCTIONS_PER_PASS_ATI 0x8971
+#define GL_NUM_INSTRUCTIONS_TOTAL_ATI 0x8972
+#define GL_NUM_INPUT_INTERPOLATOR_COMPONENTS_ATI 0x8973
+#define GL_NUM_LOOPBACK_COMPONENTS_ATI 0x8974
+#define GL_COLOR_ALPHA_PAIRING_ATI 0x8975
+#define GL_SWIZZLE_STR_ATI 0x8976
+#define GL_SWIZZLE_STQ_ATI 0x8977
+#define GL_SWIZZLE_STR_DR_ATI 0x8978
+#define GL_SWIZZLE_STQ_DQ_ATI 0x8979
+#define GL_SWIZZLE_STRQ_ATI 0x897A
+#define GL_SWIZZLE_STRQ_DQ_ATI 0x897B
+#define GL_RED_BIT_ATI 0x00000001
+#define GL_GREEN_BIT_ATI 0x00000002
+#define GL_BLUE_BIT_ATI 0x00000004
+#define GL_2X_BIT_ATI 0x00000001
+#define GL_4X_BIT_ATI 0x00000002
+#define GL_8X_BIT_ATI 0x00000004
+#define GL_HALF_BIT_ATI 0x00000008
+#define GL_QUARTER_BIT_ATI 0x00000010
+#define GL_EIGHTH_BIT_ATI 0x00000020
+#define GL_SATURATE_BIT_ATI 0x00000040
+#define GL_COMP_BIT_ATI 0x00000002
+#define GL_NEGATE_BIT_ATI 0x00000004
+#define GL_BIAS_BIT_ATI 0x00000008
+#endif
+
+#ifndef GL_ATI_pn_triangles
+#define GL_PN_TRIANGLES_ATI 0x87F0
+#define GL_MAX_PN_TRIANGLES_TESSELATION_LEVEL_ATI 0x87F1
+#define GL_PN_TRIANGLES_POINT_MODE_ATI 0x87F2
+#define GL_PN_TRIANGLES_NORMAL_MODE_ATI 0x87F3
+#define GL_PN_TRIANGLES_TESSELATION_LEVEL_ATI 0x87F4
+#define GL_PN_TRIANGLES_POINT_MODE_LINEAR_ATI 0x87F5
+#define GL_PN_TRIANGLES_POINT_MODE_CUBIC_ATI 0x87F6
+#define GL_PN_TRIANGLES_NORMAL_MODE_LINEAR_ATI 0x87F7
+#define GL_PN_TRIANGLES_NORMAL_MODE_QUADRATIC_ATI 0x87F8
+#endif
+
+#ifndef GL_ATI_vertex_array_object
+#define GL_STATIC_ATI 0x8760
+#define GL_DYNAMIC_ATI 0x8761
+#define GL_PRESERVE_ATI 0x8762
+#define GL_DISCARD_ATI 0x8763
+#define GL_OBJECT_BUFFER_SIZE_ATI 0x8764
+#define GL_OBJECT_BUFFER_USAGE_ATI 0x8765
+#define GL_ARRAY_OBJECT_BUFFER_ATI 0x8766
+#define GL_ARRAY_OBJECT_OFFSET_ATI 0x8767
+#endif
+
+#ifndef GL_EXT_vertex_shader
+#define GL_VERTEX_SHADER_EXT 0x8780
+#define GL_VERTEX_SHADER_BINDING_EXT 0x8781
+#define GL_OP_INDEX_EXT 0x8782
+#define GL_OP_NEGATE_EXT 0x8783
+#define GL_OP_DOT3_EXT 0x8784
+#define GL_OP_DOT4_EXT 0x8785
+#define GL_OP_MUL_EXT 0x8786
+#define GL_OP_ADD_EXT 0x8787
+#define GL_OP_MADD_EXT 0x8788
+#define GL_OP_FRAC_EXT 0x8789
+#define GL_OP_MAX_EXT 0x878A
+#define GL_OP_MIN_EXT 0x878B
+#define GL_OP_SET_GE_EXT 0x878C
+#define GL_OP_SET_LT_EXT 0x878D
+#define GL_OP_CLAMP_EXT 0x878E
+#define GL_OP_FLOOR_EXT 0x878F
+#define GL_OP_ROUND_EXT 0x8790
+#define GL_OP_EXP_BASE_2_EXT 0x8791
+#define GL_OP_LOG_BASE_2_EXT 0x8792
+#define GL_OP_POWER_EXT 0x8793
+#define GL_OP_RECIP_EXT 0x8794
+#define GL_OP_RECIP_SQRT_EXT 0x8795
+#define GL_OP_SUB_EXT 0x8796
+#define GL_OP_CROSS_PRODUCT_EXT 0x8797
+#define GL_OP_MULTIPLY_MATRIX_EXT 0x8798
+#define GL_OP_MOV_EXT 0x8799
+#define GL_OUTPUT_VERTEX_EXT 0x879A
+#define GL_OUTPUT_COLOR0_EXT 0x879B
+#define GL_OUTPUT_COLOR1_EXT 0x879C
+#define GL_OUTPUT_TEXTURE_COORD0_EXT 0x879D
+#define GL_OUTPUT_TEXTURE_COORD1_EXT 0x879E
+#define GL_OUTPUT_TEXTURE_COORD2_EXT 0x879F
+#define GL_OUTPUT_TEXTURE_COORD3_EXT 0x87A0
+#define GL_OUTPUT_TEXTURE_COORD4_EXT 0x87A1
+#define GL_OUTPUT_TEXTURE_COORD5_EXT 0x87A2
+#define GL_OUTPUT_TEXTURE_COORD6_EXT 0x87A3
+#define GL_OUTPUT_TEXTURE_COORD7_EXT 0x87A4
+#define GL_OUTPUT_TEXTURE_COORD8_EXT 0x87A5
+#define GL_OUTPUT_TEXTURE_COORD9_EXT 0x87A6
+#define GL_OUTPUT_TEXTURE_COORD10_EXT 0x87A7
+#define GL_OUTPUT_TEXTURE_COORD11_EXT 0x87A8
+#define GL_OUTPUT_TEXTURE_COORD12_EXT 0x87A9
+#define GL_OUTPUT_TEXTURE_COORD13_EXT 0x87AA
+#define GL_OUTPUT_TEXTURE_COORD14_EXT 0x87AB
+#define GL_OUTPUT_TEXTURE_COORD15_EXT 0x87AC
+#define GL_OUTPUT_TEXTURE_COORD16_EXT 0x87AD
+#define GL_OUTPUT_TEXTURE_COORD17_EXT 0x87AE
+#define GL_OUTPUT_TEXTURE_COORD18_EXT 0x87AF
+#define GL_OUTPUT_TEXTURE_COORD19_EXT 0x87B0
+#define GL_OUTPUT_TEXTURE_COORD20_EXT 0x87B1
+#define GL_OUTPUT_TEXTURE_COORD21_EXT 0x87B2
+#define GL_OUTPUT_TEXTURE_COORD22_EXT 0x87B3
+#define GL_OUTPUT_TEXTURE_COORD23_EXT 0x87B4
+#define GL_OUTPUT_TEXTURE_COORD24_EXT 0x87B5
+#define GL_OUTPUT_TEXTURE_COORD25_EXT 0x87B6
+#define GL_OUTPUT_TEXTURE_COORD26_EXT 0x87B7
+#define GL_OUTPUT_TEXTURE_COORD27_EXT 0x87B8
+#define GL_OUTPUT_TEXTURE_COORD28_EXT 0x87B9
+#define GL_OUTPUT_TEXTURE_COORD29_EXT 0x87BA
+#define GL_OUTPUT_TEXTURE_COORD30_EXT 0x87BB
+#define GL_OUTPUT_TEXTURE_COORD31_EXT 0x87BC
+#define GL_OUTPUT_FOG_EXT 0x87BD
+#define GL_SCALAR_EXT 0x87BE
+#define GL_VECTOR_EXT 0x87BF
+#define GL_MATRIX_EXT 0x87C0
+#define GL_VARIANT_EXT 0x87C1
+#define GL_INVARIANT_EXT 0x87C2
+#define GL_LOCAL_CONSTANT_EXT 0x87C3
+#define GL_LOCAL_EXT 0x87C4
+#define GL_MAX_VERTEX_SHADER_INSTRUCTIONS_EXT 0x87C5
+#define GL_MAX_VERTEX_SHADER_VARIANTS_EXT 0x87C6
+#define GL_MAX_VERTEX_SHADER_INVARIANTS_EXT 0x87C7
+#define GL_MAX_VERTEX_SHADER_LOCAL_CONSTANTS_EXT 0x87C8
+#define GL_MAX_VERTEX_SHADER_LOCALS_EXT 0x87C9
+#define GL_MAX_OPTIMIZED_VERTEX_SHADER_INSTRUCTIONS_EXT 0x87CA
+#define GL_MAX_OPTIMIZED_VERTEX_SHADER_VARIANTS_EXT 0x87CB
+#define GL_MAX_OPTIMIZED_VERTEX_SHADER_LOCAL_CONSTANTS_EXT 0x87CC
+#define GL_MAX_OPTIMIZED_VERTEX_SHADER_INVARIANTS_EXT 0x87CD
+#define GL_MAX_OPTIMIZED_VERTEX_SHADER_LOCALS_EXT 0x87CE
+#define GL_VERTEX_SHADER_INSTRUCTIONS_EXT 0x87CF
+#define GL_VERTEX_SHADER_VARIANTS_EXT 0x87D0
+#define GL_VERTEX_SHADER_INVARIANTS_EXT 0x87D1
+#define GL_VERTEX_SHADER_LOCAL_CONSTANTS_EXT 0x87D2
+#define GL_VERTEX_SHADER_LOCALS_EXT 0x87D3
+#define GL_VERTEX_SHADER_OPTIMIZED_EXT 0x87D4
+#define GL_X_EXT 0x87D5
+#define GL_Y_EXT 0x87D6
+#define GL_Z_EXT 0x87D7
+#define GL_W_EXT 0x87D8
+#define GL_NEGATIVE_X_EXT 0x87D9
+#define GL_NEGATIVE_Y_EXT 0x87DA
+#define GL_NEGATIVE_Z_EXT 0x87DB
+#define GL_NEGATIVE_W_EXT 0x87DC
+#define GL_ZERO_EXT 0x87DD
+#define GL_ONE_EXT 0x87DE
+#define GL_NEGATIVE_ONE_EXT 0x87DF
+#define GL_NORMALIZED_RANGE_EXT 0x87E0
+#define GL_FULL_RANGE_EXT 0x87E1
+#define GL_CURRENT_VERTEX_EXT 0x87E2
+#define GL_MVP_MATRIX_EXT 0x87E3
+#define GL_VARIANT_VALUE_EXT 0x87E4
+#define GL_VARIANT_DATATYPE_EXT 0x87E5
+#define GL_VARIANT_ARRAY_STRIDE_EXT 0x87E6
+#define GL_VARIANT_ARRAY_TYPE_EXT 0x87E7
+#define GL_VARIANT_ARRAY_EXT 0x87E8
+#define GL_VARIANT_ARRAY_POINTER_EXT 0x87E9
+#define GL_INVARIANT_VALUE_EXT 0x87EA
+#define GL_INVARIANT_DATATYPE_EXT 0x87EB
+#define GL_LOCAL_CONSTANT_VALUE_EXT 0x87EC
+#define GL_LOCAL_CONSTANT_DATATYPE_EXT 0x87ED
+#endif
+
+#ifndef GL_ATI_vertex_streams
+#define GL_MAX_VERTEX_STREAMS_ATI 0x876B
+#define GL_VERTEX_STREAM0_ATI 0x876C
+#define GL_VERTEX_STREAM1_ATI 0x876D
+#define GL_VERTEX_STREAM2_ATI 0x876E
+#define GL_VERTEX_STREAM3_ATI 0x876F
+#define GL_VERTEX_STREAM4_ATI 0x8770
+#define GL_VERTEX_STREAM5_ATI 0x8771
+#define GL_VERTEX_STREAM6_ATI 0x8772
+#define GL_VERTEX_STREAM7_ATI 0x8773
+#define GL_VERTEX_SOURCE_ATI 0x8774
+#endif
+
+#ifndef GL_ATI_element_array
+#define GL_ELEMENT_ARRAY_ATI 0x8768
+#define GL_ELEMENT_ARRAY_TYPE_ATI 0x8769
+#define GL_ELEMENT_ARRAY_POINTER_ATI 0x876A
+#endif
+
+#ifndef GL_SUN_mesh_array
+#define GL_QUAD_MESH_SUN 0x8614
+#define GL_TRIANGLE_MESH_SUN 0x8615
+#endif
+
+#ifndef GL_SUN_slice_accum
+#define GL_SLICE_ACCUM_SUN 0x85CC
+#endif
+
+#ifndef GL_NV_multisample_filter_hint
+#define GL_MULTISAMPLE_FILTER_HINT_NV 0x8534
+#endif
+
+#ifndef GL_NV_depth_clamp
+#define GL_DEPTH_CLAMP_NV 0x864F
+#endif
+
+#ifndef GL_NV_occlusion_query
+#define GL_PIXEL_COUNTER_BITS_NV 0x8864
+#define GL_CURRENT_OCCLUSION_QUERY_ID_NV 0x8865
+#define GL_PIXEL_COUNT_NV 0x8866
+#define GL_PIXEL_COUNT_AVAILABLE_NV 0x8867
+#endif
+
+#ifndef GL_NV_point_sprite
+#define GL_POINT_SPRITE_NV 0x8861
+#define GL_COORD_REPLACE_NV 0x8862
+#define GL_POINT_SPRITE_R_MODE_NV 0x8863
+#endif
+
+#ifndef GL_NV_texture_shader3
+#define GL_OFFSET_PROJECTIVE_TEXTURE_2D_NV 0x8850
+#define GL_OFFSET_PROJECTIVE_TEXTURE_2D_SCALE_NV 0x8851
+#define GL_OFFSET_PROJECTIVE_TEXTURE_RECTANGLE_NV 0x8852
+#define GL_OFFSET_PROJECTIVE_TEXTURE_RECTANGLE_SCALE_NV 0x8853
+#define GL_OFFSET_HILO_TEXTURE_2D_NV 0x8854
+#define GL_OFFSET_HILO_TEXTURE_RECTANGLE_NV 0x8855
+#define GL_OFFSET_HILO_PROJECTIVE_TEXTURE_2D_NV 0x8856
+#define GL_OFFSET_HILO_PROJECTIVE_TEXTURE_RECTANGLE_NV 0x8857
+#define GL_DEPENDENT_HILO_TEXTURE_2D_NV 0x8858
+#define GL_DEPENDENT_RGB_TEXTURE_3D_NV 0x8859
+#define GL_DEPENDENT_RGB_TEXTURE_CUBE_MAP_NV 0x885A
+#define GL_DOT_PRODUCT_PASS_THROUGH_NV 0x885B
+#define GL_DOT_PRODUCT_TEXTURE_1D_NV 0x885C
+#define GL_DOT_PRODUCT_AFFINE_DEPTH_REPLACE_NV 0x885D
+#define GL_HILO8_NV 0x885E
+#define GL_SIGNED_HILO8_NV 0x885F
+#define GL_FORCE_BLUE_TO_ONE_NV 0x8860
+#endif
+
+#ifndef GL_NV_vertex_program1_1
+#endif
+
+#ifndef GL_EXT_shadow_funcs
+#endif
+
+#ifndef GL_EXT_stencil_two_side
+#define GL_STENCIL_TEST_TWO_SIDE_EXT 0x8910
+#define GL_ACTIVE_STENCIL_FACE_EXT 0x8911
+#endif
+
+#ifndef GL_ATI_text_fragment_shader
+#define GL_TEXT_FRAGMENT_SHADER_ATI 0x8200
+#endif
+
+#ifndef GL_APPLE_client_storage
+#define GL_UNPACK_CLIENT_STORAGE_APPLE 0x85B2
+#endif
+
+#ifndef GL_APPLE_element_array
+#define GL_ELEMENT_ARRAY_APPLE 0x8A0C
+#define GL_ELEMENT_ARRAY_TYPE_APPLE 0x8A0D
+#define GL_ELEMENT_ARRAY_POINTER_APPLE 0x8A0E
+#endif
+
+#ifndef GL_APPLE_fence
+#define GL_DRAW_PIXELS_APPLE 0x8A0A
+#define GL_FENCE_APPLE 0x8A0B
+#endif
+
+#ifndef GL_APPLE_vertex_array_object
+#define GL_VERTEX_ARRAY_BINDING_APPLE 0x85B5
+#endif
+
+#ifndef GL_APPLE_vertex_array_range
+#define GL_VERTEX_ARRAY_RANGE_APPLE 0x851D
+#define GL_VERTEX_ARRAY_RANGE_LENGTH_APPLE 0x851E
+#define GL_VERTEX_ARRAY_STORAGE_HINT_APPLE 0x851F
+#define GL_VERTEX_ARRAY_RANGE_POINTER_APPLE 0x8521
+#define GL_STORAGE_CLIENT_APPLE 0x85B4
+#define GL_STORAGE_CACHED_APPLE 0x85BE
+#define GL_STORAGE_SHARED_APPLE 0x85BF
+#endif
+
+#ifndef GL_APPLE_ycbcr_422
+#define GL_YCBCR_422_APPLE 0x85B9
+#define GL_UNSIGNED_SHORT_8_8_APPLE 0x85BA
+#define GL_UNSIGNED_SHORT_8_8_REV_APPLE 0x85BB
+#endif
+
+#ifndef GL_S3_s3tc
+#define GL_RGB_S3TC 0x83A0
+#define GL_RGB4_S3TC 0x83A1
+#define GL_RGBA_S3TC 0x83A2
+#define GL_RGBA4_S3TC 0x83A3
+#endif
+
+#ifndef GL_ATI_draw_buffers
+#define GL_MAX_DRAW_BUFFERS_ATI 0x8824
+#define GL_DRAW_BUFFER0_ATI 0x8825
+#define GL_DRAW_BUFFER1_ATI 0x8826
+#define GL_DRAW_BUFFER2_ATI 0x8827
+#define GL_DRAW_BUFFER3_ATI 0x8828
+#define GL_DRAW_BUFFER4_ATI 0x8829
+#define GL_DRAW_BUFFER5_ATI 0x882A
+#define GL_DRAW_BUFFER6_ATI 0x882B
+#define GL_DRAW_BUFFER7_ATI 0x882C
+#define GL_DRAW_BUFFER8_ATI 0x882D
+#define GL_DRAW_BUFFER9_ATI 0x882E
+#define GL_DRAW_BUFFER10_ATI 0x882F
+#define GL_DRAW_BUFFER11_ATI 0x8830
+#define GL_DRAW_BUFFER12_ATI 0x8831
+#define GL_DRAW_BUFFER13_ATI 0x8832
+#define GL_DRAW_BUFFER14_ATI 0x8833
+#define GL_DRAW_BUFFER15_ATI 0x8834
+#endif
+
+#ifndef GL_ATI_pixel_format_float
+#define GL_TYPE_RGBA_FLOAT_ATI 0x8820
+#define GL_COLOR_CLEAR_UNCLAMPED_VALUE_ATI 0x8835
+#endif
+
+#ifndef GL_ATI_texture_env_combine3
+#define GL_MODULATE_ADD_ATI 0x8744
+#define GL_MODULATE_SIGNED_ADD_ATI 0x8745
+#define GL_MODULATE_SUBTRACT_ATI 0x8746
+#endif
+
+#ifndef GL_ATI_texture_float
+#define GL_RGBA_FLOAT32_ATI 0x8814
+#define GL_RGB_FLOAT32_ATI 0x8815
+#define GL_ALPHA_FLOAT32_ATI 0x8816
+#define GL_INTENSITY_FLOAT32_ATI 0x8817
+#define GL_LUMINANCE_FLOAT32_ATI 0x8818
+#define GL_LUMINANCE_ALPHA_FLOAT32_ATI 0x8819
+#define GL_RGBA_FLOAT16_ATI 0x881A
+#define GL_RGB_FLOAT16_ATI 0x881B
+#define GL_ALPHA_FLOAT16_ATI 0x881C
+#define GL_INTENSITY_FLOAT16_ATI 0x881D
+#define GL_LUMINANCE_FLOAT16_ATI 0x881E
+#define GL_LUMINANCE_ALPHA_FLOAT16_ATI 0x881F
+#endif
+
+#ifndef GL_NV_float_buffer
+#define GL_FLOAT_R_NV 0x8880
+#define GL_FLOAT_RG_NV 0x8881
+#define GL_FLOAT_RGB_NV 0x8882
+#define GL_FLOAT_RGBA_NV 0x8883
+#define GL_FLOAT_R16_NV 0x8884
+#define GL_FLOAT_R32_NV 0x8885
+#define GL_FLOAT_RG16_NV 0x8886
+#define GL_FLOAT_RG32_NV 0x8887
+#define GL_FLOAT_RGB16_NV 0x8888
+#define GL_FLOAT_RGB32_NV 0x8889
+#define GL_FLOAT_RGBA16_NV 0x888A
+#define GL_FLOAT_RGBA32_NV 0x888B
+#define GL_TEXTURE_FLOAT_COMPONENTS_NV 0x888C
+#define GL_FLOAT_CLEAR_COLOR_VALUE_NV 0x888D
+#define GL_FLOAT_RGBA_MODE_NV 0x888E
+#endif
+
+#ifndef GL_NV_fragment_program
+#define GL_MAX_FRAGMENT_PROGRAM_LOCAL_PARAMETERS_NV 0x8868
+#define GL_FRAGMENT_PROGRAM_NV 0x8870
+#define GL_MAX_TEXTURE_COORDS_NV 0x8871
+#define GL_MAX_TEXTURE_IMAGE_UNITS_NV 0x8872
+#define GL_FRAGMENT_PROGRAM_BINDING_NV 0x8873
+#define GL_PROGRAM_ERROR_STRING_NV 0x8874
+#endif
+
+#ifndef GL_NV_half_float
+#define GL_HALF_FLOAT_NV 0x140B
+#endif
+
+#ifndef GL_NV_pixel_data_range
+#define GL_WRITE_PIXEL_DATA_RANGE_NV 0x8878
+#define GL_READ_PIXEL_DATA_RANGE_NV 0x8879
+#define GL_WRITE_PIXEL_DATA_RANGE_LENGTH_NV 0x887A
+#define GL_READ_PIXEL_DATA_RANGE_LENGTH_NV 0x887B
+#define GL_WRITE_PIXEL_DATA_RANGE_POINTER_NV 0x887C
+#define GL_READ_PIXEL_DATA_RANGE_POINTER_NV 0x887D
+#endif
+
+#ifndef GL_NV_primitive_restart
+#define GL_PRIMITIVE_RESTART_NV 0x8558
+#define GL_PRIMITIVE_RESTART_INDEX_NV 0x8559
+#endif
+
+#ifndef GL_NV_texture_expand_normal
+#define GL_TEXTURE_UNSIGNED_REMAP_MODE_NV 0x888F
+#endif
+
+#ifndef GL_NV_vertex_program2
+#endif
+
+#ifndef GL_ATI_map_object_buffer
+#endif
+
+#ifndef GL_ATI_separate_stencil
+#define GL_STENCIL_BACK_FUNC_ATI 0x8800
+#define GL_STENCIL_BACK_FAIL_ATI 0x8801
+#define GL_STENCIL_BACK_PASS_DEPTH_FAIL_ATI 0x8802
+#define GL_STENCIL_BACK_PASS_DEPTH_PASS_ATI 0x8803
+#endif
+
+#ifndef GL_ATI_vertex_attrib_array_object
+#endif
+
+#ifndef GL_OES_read_format
+#define GL_IMPLEMENTATION_COLOR_READ_TYPE_OES 0x8B9A
+#define GL_IMPLEMENTATION_COLOR_READ_FORMAT_OES 0x8B9B
+#endif
+
+#ifndef GL_EXT_depth_bounds_test
+#define GL_DEPTH_BOUNDS_TEST_EXT 0x8890
+#define GL_DEPTH_BOUNDS_EXT 0x8891
+#endif
+
+#ifndef GL_EXT_texture_mirror_clamp
+#define GL_MIRROR_CLAMP_EXT 0x8742
+#define GL_MIRROR_CLAMP_TO_EDGE_EXT 0x8743
+#define GL_MIRROR_CLAMP_TO_BORDER_EXT 0x8912
+#endif
+
+#ifndef GL_EXT_blend_equation_separate
+#define GL_BLEND_EQUATION_RGB_EXT 0x8009
+#define GL_BLEND_EQUATION_ALPHA_EXT 0x883D
+#endif
+
+#ifndef GL_MESA_pack_invert
+#define GL_PACK_INVERT_MESA 0x8758
+#endif
+
+#ifndef GL_MESA_ycbcr_texture
+#define GL_UNSIGNED_SHORT_8_8_MESA 0x85BA
+#define GL_UNSIGNED_SHORT_8_8_REV_MESA 0x85BB
+#define GL_YCBCR_MESA 0x8757
+#endif
+
+#ifndef GL_EXT_pixel_buffer_object
+#define GL_PIXEL_PACK_BUFFER_EXT 0x88EB
+#define GL_PIXEL_UNPACK_BUFFER_EXT 0x88EC
+#define GL_PIXEL_PACK_BUFFER_BINDING_EXT 0x88ED
+#define GL_PIXEL_UNPACK_BUFFER_BINDING_EXT 0x88EF
+#endif
+
+#ifndef GL_NV_fragment_program_option
+#endif
+
+#ifndef GL_NV_fragment_program2
+#define GL_MAX_PROGRAM_EXEC_INSTRUCTIONS_NV 0x88F4
+#define GL_MAX_PROGRAM_CALL_DEPTH_NV 0x88F5
+#define GL_MAX_PROGRAM_IF_DEPTH_NV 0x88F6
+#define GL_MAX_PROGRAM_LOOP_DEPTH_NV 0x88F7
+#define GL_MAX_PROGRAM_LOOP_COUNT_NV 0x88F8
+#endif
+
+#ifndef GL_NV_vertex_program2_option
+/* reuse GL_MAX_PROGRAM_EXEC_INSTRUCTIONS_NV */
+/* reuse GL_MAX_PROGRAM_CALL_DEPTH_NV */
+#endif
+
+#ifndef GL_NV_vertex_program3
+/* reuse GL_MAX_VERTEX_TEXTURE_IMAGE_UNITS_ARB */
+#endif
+
+#ifndef GL_EXT_framebuffer_object
+#define GL_INVALID_FRAMEBUFFER_OPERATION_EXT 0x0506
+#define GL_MAX_RENDERBUFFER_SIZE_EXT 0x84E8
+#define GL_FRAMEBUFFER_BINDING_EXT 0x8CA6
+#define GL_RENDERBUFFER_BINDING_EXT 0x8CA7
+#define GL_FRAMEBUFFER_ATTACHMENT_OBJECT_TYPE_EXT 0x8CD0
+#define GL_FRAMEBUFFER_ATTACHMENT_OBJECT_NAME_EXT 0x8CD1
+#define GL_FRAMEBUFFER_ATTACHMENT_TEXTURE_LEVEL_EXT 0x8CD2
+#define GL_FRAMEBUFFER_ATTACHMENT_TEXTURE_CUBE_MAP_FACE_EXT 0x8CD3
+#define GL_FRAMEBUFFER_ATTACHMENT_TEXTURE_3D_ZOFFSET_EXT 0x8CD4
+#define GL_FRAMEBUFFER_COMPLETE_EXT 0x8CD5
+#define GL_FRAMEBUFFER_INCOMPLETE_ATTACHMENT_EXT 0x8CD6
+#define GL_FRAMEBUFFER_INCOMPLETE_MISSING_ATTACHMENT_EXT 0x8CD7
+#define GL_FRAMEBUFFER_INCOMPLETE_DIMENSIONS_EXT 0x8CD9
+#define GL_FRAMEBUFFER_INCOMPLETE_FORMATS_EXT 0x8CDA
+#define GL_FRAMEBUFFER_INCOMPLETE_DRAW_BUFFER_EXT 0x8CDB
+#define GL_FRAMEBUFFER_INCOMPLETE_READ_BUFFER_EXT 0x8CDC
+#define GL_FRAMEBUFFER_UNSUPPORTED_EXT 0x8CDD
+#define GL_MAX_COLOR_ATTACHMENTS_EXT 0x8CDF
+#define GL_COLOR_ATTACHMENT0_EXT 0x8CE0
+#define GL_COLOR_ATTACHMENT1_EXT 0x8CE1
+#define GL_COLOR_ATTACHMENT2_EXT 0x8CE2
+#define GL_COLOR_ATTACHMENT3_EXT 0x8CE3
+#define GL_COLOR_ATTACHMENT4_EXT 0x8CE4
+#define GL_COLOR_ATTACHMENT5_EXT 0x8CE5
+#define GL_COLOR_ATTACHMENT6_EXT 0x8CE6
+#define GL_COLOR_ATTACHMENT7_EXT 0x8CE7
+#define GL_COLOR_ATTACHMENT8_EXT 0x8CE8
+#define GL_COLOR_ATTACHMENT9_EXT 0x8CE9
+#define GL_COLOR_ATTACHMENT10_EXT 0x8CEA
+#define GL_COLOR_ATTACHMENT11_EXT 0x8CEB
+#define GL_COLOR_ATTACHMENT12_EXT 0x8CEC
+#define GL_COLOR_ATTACHMENT13_EXT 0x8CED
+#define GL_COLOR_ATTACHMENT14_EXT 0x8CEE
+#define GL_COLOR_ATTACHMENT15_EXT 0x8CEF
+#define GL_DEPTH_ATTACHMENT_EXT 0x8D00
+#define GL_STENCIL_ATTACHMENT_EXT 0x8D20
+#define GL_FRAMEBUFFER_EXT 0x8D40
+#define GL_RENDERBUFFER_EXT 0x8D41
+#define GL_RENDERBUFFER_WIDTH_EXT 0x8D42
+#define GL_RENDERBUFFER_HEIGHT_EXT 0x8D43
+#define GL_RENDERBUFFER_INTERNAL_FORMAT_EXT 0x8D44
+#define GL_STENCIL_INDEX1_EXT 0x8D46
+#define GL_STENCIL_INDEX4_EXT 0x8D47
+#define GL_STENCIL_INDEX8_EXT 0x8D48
+#define GL_STENCIL_INDEX16_EXT 0x8D49
+#define GL_RENDERBUFFER_RED_SIZE_EXT 0x8D50
+#define GL_RENDERBUFFER_GREEN_SIZE_EXT 0x8D51
+#define GL_RENDERBUFFER_BLUE_SIZE_EXT 0x8D52
+#define GL_RENDERBUFFER_ALPHA_SIZE_EXT 0x8D53
+#define GL_RENDERBUFFER_DEPTH_SIZE_EXT 0x8D54
+#define GL_RENDERBUFFER_STENCIL_SIZE_EXT 0x8D55
+#endif
+
+#ifndef GL_GREMEDY_string_marker
+#endif
+
+#ifndef GL_EXT_packed_depth_stencil
+#define GL_DEPTH_STENCIL_EXT 0x84F9
+#define GL_UNSIGNED_INT_24_8_EXT 0x84FA
+#define GL_DEPTH24_STENCIL8_EXT 0x88F0
+#define GL_TEXTURE_STENCIL_SIZE_EXT 0x88F1
+#endif
+
+#ifndef GL_EXT_stencil_clear_tag
+#define GL_STENCIL_TAG_BITS_EXT 0x88F2
+#define GL_STENCIL_CLEAR_TAG_VALUE_EXT 0x88F3
+#endif
+
+#ifndef GL_EXT_texture_sRGB
+#define GL_SRGB_EXT 0x8C40
+#define GL_SRGB8_EXT 0x8C41
+#define GL_SRGB_ALPHA_EXT 0x8C42
+#define GL_SRGB8_ALPHA8_EXT 0x8C43
+#define GL_SLUMINANCE_ALPHA_EXT 0x8C44
+#define GL_SLUMINANCE8_ALPHA8_EXT 0x8C45
+#define GL_SLUMINANCE_EXT 0x8C46
+#define GL_SLUMINANCE8_EXT 0x8C47
+#define GL_COMPRESSED_SRGB_EXT 0x8C48
+#define GL_COMPRESSED_SRGB_ALPHA_EXT 0x8C49
+#define GL_COMPRESSED_SLUMINANCE_EXT 0x8C4A
+#define GL_COMPRESSED_SLUMINANCE_ALPHA_EXT 0x8C4B
+#define GL_COMPRESSED_SRGB_S3TC_DXT1_EXT 0x8C4C
+#define GL_COMPRESSED_SRGB_ALPHA_S3TC_DXT1_EXT 0x8C4D
+#define GL_COMPRESSED_SRGB_ALPHA_S3TC_DXT3_EXT 0x8C4E
+#define GL_COMPRESSED_SRGB_ALPHA_S3TC_DXT5_EXT 0x8C4F
+#endif
+
+#ifndef GL_EXT_framebuffer_blit
+#define GL_READ_FRAMEBUFFER_EXT 0x8CA8
+#define GL_DRAW_FRAMEBUFFER_EXT 0x8CA9
+#define GL_DRAW_FRAMEBUFFER_BINDING_EXT GL_FRAMEBUFFER_BINDING_EXT
+#define GL_READ_FRAMEBUFFER_BINDING_EXT 0x8CAA
+#endif
+
+#ifndef GL_EXT_framebuffer_multisample
+#define GL_RENDERBUFFER_SAMPLES_EXT 0x8CAB
+#define GL_FRAMEBUFFER_INCOMPLETE_MULTISAMPLE_EXT 0x8D56
+#define GL_MAX_SAMPLES_EXT 0x8D57
+#endif
+
+#ifndef GL_MESAX_texture_stack
+#define GL_TEXTURE_1D_STACK_MESAX 0x8759
+#define GL_TEXTURE_2D_STACK_MESAX 0x875A
+#define GL_PROXY_TEXTURE_1D_STACK_MESAX 0x875B
+#define GL_PROXY_TEXTURE_2D_STACK_MESAX 0x875C
+#define GL_TEXTURE_1D_STACK_BINDING_MESAX 0x875D
+#define GL_TEXTURE_2D_STACK_BINDING_MESAX 0x875E
+#endif
+
+#ifndef GL_EXT_timer_query
+#define GL_TIME_ELAPSED_EXT 0x88BF
+#endif
+
+#ifndef GL_EXT_gpu_program_parameters
+#endif
+
+#ifndef GL_APPLE_flush_buffer_range
+#define GL_BUFFER_SERIALIZED_MODIFY_APPLE 0x8A12
+#define GL_BUFFER_FLUSHING_UNMAP_APPLE 0x8A13
+#endif
+
+#ifndef GL_NV_gpu_program4
+#define GL_MIN_PROGRAM_TEXEL_OFFSET_NV 0x8904
+#define GL_MAX_PROGRAM_TEXEL_OFFSET_NV 0x8905
+#define GL_PROGRAM_ATTRIB_COMPONENTS_NV 0x8906
+#define GL_PROGRAM_RESULT_COMPONENTS_NV 0x8907
+#define GL_MAX_PROGRAM_ATTRIB_COMPONENTS_NV 0x8908
+#define GL_MAX_PROGRAM_RESULT_COMPONENTS_NV 0x8909
+#define GL_MAX_PROGRAM_GENERIC_ATTRIBS_NV 0x8DA5
+#define GL_MAX_PROGRAM_GENERIC_RESULTS_NV 0x8DA6
+#endif
+
+#ifndef GL_NV_geometry_program4
+#define GL_LINES_ADJACENCY_EXT 0x000A
+#define GL_LINE_STRIP_ADJACENCY_EXT 0x000B
+#define GL_TRIANGLES_ADJACENCY_EXT 0x000C
+#define GL_TRIANGLE_STRIP_ADJACENCY_EXT 0x000D
+#define GL_GEOMETRY_PROGRAM_NV 0x8C26
+#define GL_MAX_PROGRAM_OUTPUT_VERTICES_NV 0x8C27
+#define GL_MAX_PROGRAM_TOTAL_OUTPUT_COMPONENTS_NV 0x8C28
+#define GL_GEOMETRY_VERTICES_OUT_EXT 0x8DDA
+#define GL_GEOMETRY_INPUT_TYPE_EXT 0x8DDB
+#define GL_GEOMETRY_OUTPUT_TYPE_EXT 0x8DDC
+#define GL_MAX_GEOMETRY_TEXTURE_IMAGE_UNITS_EXT 0x8C29
+#define GL_FRAMEBUFFER_ATTACHMENT_LAYERED_EXT 0x8DA7
+#define GL_FRAMEBUFFER_INCOMPLETE_LAYER_TARGETS_EXT 0x8DA8
+#define GL_FRAMEBUFFER_INCOMPLETE_LAYER_COUNT_EXT 0x8DA9
+#define GL_FRAMEBUFFER_ATTACHMENT_TEXTURE_LAYER_EXT 0x8CD4
+#define GL_PROGRAM_POINT_SIZE_EXT 0x8642
+#endif
+
+#ifndef GL_EXT_geometry_shader4
+#define GL_GEOMETRY_SHADER_EXT 0x8DD9
+/* reuse GL_GEOMETRY_VERTICES_OUT_EXT */
+/* reuse GL_GEOMETRY_INPUT_TYPE_EXT */
+/* reuse GL_GEOMETRY_OUTPUT_TYPE_EXT */
+/* reuse GL_MAX_GEOMETRY_TEXTURE_IMAGE_UNITS_EXT */
+#define GL_MAX_GEOMETRY_VARYING_COMPONENTS_EXT 0x8DDD
+#define GL_MAX_VERTEX_VARYING_COMPONENTS_EXT 0x8DDE
+#define GL_MAX_VARYING_COMPONENTS_EXT 0x8B4B
+#define GL_MAX_GEOMETRY_UNIFORM_COMPONENTS_EXT 0x8DDF
+#define GL_MAX_GEOMETRY_OUTPUT_VERTICES_EXT 0x8DE0
+#define GL_MAX_GEOMETRY_TOTAL_OUTPUT_COMPONENTS_EXT 0x8DE1
+/* reuse GL_LINES_ADJACENCY_EXT */
+/* reuse GL_LINE_STRIP_ADJACENCY_EXT */
+/* reuse GL_TRIANGLES_ADJACENCY_EXT */
+/* reuse GL_TRIANGLE_STRIP_ADJACENCY_EXT */
+/* reuse GL_FRAMEBUFFER_INCOMPLETE_LAYER_TARGETS_EXT */
+/* reuse GL_FRAMEBUFFER_INCOMPLETE_LAYER_COUNT_EXT */
+/* reuse GL_FRAMEBUFFER_ATTACHMENT_LAYERED_EXT */
+/* reuse GL_FRAMEBUFFER_ATTACHMENT_TEXTURE_LAYER_EXT */
+/* reuse GL_PROGRAM_POINT_SIZE_EXT */
+#endif
+
+#ifndef GL_NV_vertex_program4
+#define GL_VERTEX_ATTRIB_ARRAY_INTEGER_NV 0x88FD
+#endif
+
+#ifndef GL_EXT_gpu_shader4
+#define GL_SAMPLER_1D_ARRAY_EXT 0x8DC0
+#define GL_SAMPLER_2D_ARRAY_EXT 0x8DC1
+#define GL_SAMPLER_BUFFER_EXT 0x8DC2
+#define GL_SAMPLER_1D_ARRAY_SHADOW_EXT 0x8DC3
+#define GL_SAMPLER_2D_ARRAY_SHADOW_EXT 0x8DC4
+#define GL_SAMPLER_CUBE_SHADOW_EXT 0x8DC5
+#define GL_UNSIGNED_INT_VEC2_EXT 0x8DC6
+#define GL_UNSIGNED_INT_VEC3_EXT 0x8DC7
+#define GL_UNSIGNED_INT_VEC4_EXT 0x8DC8
+#define GL_INT_SAMPLER_1D_EXT 0x8DC9
+#define GL_INT_SAMPLER_2D_EXT 0x8DCA
+#define GL_INT_SAMPLER_3D_EXT 0x8DCB
+#define GL_INT_SAMPLER_CUBE_EXT 0x8DCC
+#define GL_INT_SAMPLER_2D_RECT_EXT 0x8DCD
+#define GL_INT_SAMPLER_1D_ARRAY_EXT 0x8DCE
+#define GL_INT_SAMPLER_2D_ARRAY_EXT 0x8DCF
+#define GL_INT_SAMPLER_BUFFER_EXT 0x8DD0
+#define GL_UNSIGNED_INT_SAMPLER_1D_EXT 0x8DD1
+#define GL_UNSIGNED_INT_SAMPLER_2D_EXT 0x8DD2
+#define GL_UNSIGNED_INT_SAMPLER_3D_EXT 0x8DD3
+#define GL_UNSIGNED_INT_SAMPLER_CUBE_EXT 0x8DD4
+#define GL_UNSIGNED_INT_SAMPLER_2D_RECT_EXT 0x8DD5
+#define GL_UNSIGNED_INT_SAMPLER_1D_ARRAY_EXT 0x8DD6
+#define GL_UNSIGNED_INT_SAMPLER_2D_ARRAY_EXT 0x8DD7
+#define GL_UNSIGNED_INT_SAMPLER_BUFFER_EXT 0x8DD8
+#endif
+
+#ifndef GL_EXT_draw_instanced
+#endif
+
+#ifndef GL_EXT_packed_float
+#define GL_R11F_G11F_B10F_EXT 0x8C3A
+#define GL_UNSIGNED_INT_10F_11F_11F_REV_EXT 0x8C3B
+#define GL_RGBA_SIGNED_COMPONENTS_EXT 0x8C3C
+#endif
+
+#ifndef GL_EXT_texture_array
+#define GL_TEXTURE_1D_ARRAY_EXT 0x8C18
+#define GL_PROXY_TEXTURE_1D_ARRAY_EXT 0x8C19
+#define GL_TEXTURE_2D_ARRAY_EXT 0x8C1A
+#define GL_PROXY_TEXTURE_2D_ARRAY_EXT 0x8C1B
+#define GL_TEXTURE_BINDING_1D_ARRAY_EXT 0x8C1C
+#define GL_TEXTURE_BINDING_2D_ARRAY_EXT 0x8C1D
+#define GL_MAX_ARRAY_TEXTURE_LAYERS_EXT 0x88FF
+#define GL_COMPARE_REF_DEPTH_TO_TEXTURE_EXT 0x884E
+/* reuse GL_FRAMEBUFFER_ATTACHMENT_TEXTURE_LAYER_EXT */
+#endif
+
+#ifndef GL_EXT_texture_buffer_object
+#define GL_TEXTURE_BUFFER_EXT 0x8C2A
+#define GL_MAX_TEXTURE_BUFFER_SIZE_EXT 0x8C2B
+#define GL_TEXTURE_BINDING_BUFFER_EXT 0x8C2C
+#define GL_TEXTURE_BUFFER_DATA_STORE_BINDING_EXT 0x8C2D
+#define GL_TEXTURE_BUFFER_FORMAT_EXT 0x8C2E
+#endif
+
+#ifndef GL_EXT_texture_compression_latc
+#define GL_COMPRESSED_LUMINANCE_LATC1_EXT 0x8C70
+#define GL_COMPRESSED_SIGNED_LUMINANCE_LATC1_EXT 0x8C71
+#define GL_COMPRESSED_LUMINANCE_ALPHA_LATC2_EXT 0x8C72
+#define GL_COMPRESSED_SIGNED_LUMINANCE_ALPHA_LATC2_EXT 0x8C73
+#endif
+
+#ifndef GL_EXT_texture_compression_rgtc
+#define GL_COMPRESSED_RED_RGTC1_EXT 0x8DBB
+#define GL_COMPRESSED_SIGNED_RED_RGTC1_EXT 0x8DBC
+#define GL_COMPRESSED_RED_GREEN_RGTC2_EXT 0x8DBD
+#define GL_COMPRESSED_SIGNED_RED_GREEN_RGTC2_EXT 0x8DBE
+#endif
+
+#ifndef GL_EXT_texture_shared_exponent
+#define GL_RGB9_E5_EXT 0x8C3D
+#define GL_UNSIGNED_INT_5_9_9_9_REV_EXT 0x8C3E
+#define GL_TEXTURE_SHARED_SIZE_EXT 0x8C3F
+#endif
+
+#ifndef GL_NV_depth_buffer_float
+#define GL_DEPTH_COMPONENT32F_NV 0x8DAB
+#define GL_DEPTH32F_STENCIL8_NV 0x8DAC
+#define GL_FLOAT_32_UNSIGNED_INT_24_8_REV_NV 0x8DAD
+#define GL_DEPTH_BUFFER_FLOAT_MODE_NV 0x8DAF
+#endif
+
+#ifndef GL_NV_fragment_program4
+#endif
+
+#ifndef GL_NV_framebuffer_multisample_coverage
+#define GL_RENDERBUFFER_COVERAGE_SAMPLES_NV 0x8CAB
+#define GL_RENDERBUFFER_COLOR_SAMPLES_NV 0x8E10
+#define GL_MAX_MULTISAMPLE_COVERAGE_MODES_NV 0x8E11
+#define GL_MULTISAMPLE_COVERAGE_MODES_NV 0x8E12
+#endif
+
+#ifndef GL_EXT_framebuffer_sRGB
+#define GL_FRAMEBUFFER_SRGB_EXT 0x8DB9
+#define GL_FRAMEBUFFER_SRGB_CAPABLE_EXT 0x8DBA
+#endif
+
+#ifndef GL_NV_geometry_shader4
+#endif
+
+#ifndef GL_NV_parameter_buffer_object
+#define GL_MAX_PROGRAM_PARAMETER_BUFFER_BINDINGS_NV 0x8DA0
+#define GL_MAX_PROGRAM_PARAMETER_BUFFER_SIZE_NV 0x8DA1
+#define GL_VERTEX_PROGRAM_PARAMETER_BUFFER_NV 0x8DA2
+#define GL_GEOMETRY_PROGRAM_PARAMETER_BUFFER_NV 0x8DA3
+#define GL_FRAGMENT_PROGRAM_PARAMETER_BUFFER_NV 0x8DA4
+#endif
+
+#ifndef GL_EXT_draw_buffers2
+#endif
+
+#ifndef GL_NV_transform_feedback
+#define GL_BACK_PRIMARY_COLOR_NV 0x8C77
+#define GL_BACK_SECONDARY_COLOR_NV 0x8C78
+#define GL_TEXTURE_COORD_NV 0x8C79
+#define GL_CLIP_DISTANCE_NV 0x8C7A
+#define GL_VERTEX_ID_NV 0x8C7B
+#define GL_PRIMITIVE_ID_NV 0x8C7C
+#define GL_GENERIC_ATTRIB_NV 0x8C7D
+#define GL_TRANSFORM_FEEDBACK_ATTRIBS_NV 0x8C7E
+#define GL_TRANSFORM_FEEDBACK_BUFFER_MODE_NV 0x8C7F
+#define GL_MAX_TRANSFORM_FEEDBACK_SEPARATE_COMPONENTS_NV 0x8C80
+#define GL_ACTIVE_VARYINGS_NV 0x8C81
+#define GL_ACTIVE_VARYING_MAX_LENGTH_NV 0x8C82
+#define GL_TRANSFORM_FEEDBACK_VARYINGS_NV 0x8C83
+#define GL_TRANSFORM_FEEDBACK_BUFFER_START_NV 0x8C84
+#define GL_TRANSFORM_FEEDBACK_BUFFER_SIZE_NV 0x8C85
+#define GL_TRANSFORM_FEEDBACK_RECORD_NV 0x8C86
+#define GL_PRIMITIVES_GENERATED_NV 0x8C87
+#define GL_TRANSFORM_FEEDBACK_PRIMITIVES_WRITTEN_NV 0x8C88
+#define GL_RASTERIZER_DISCARD_NV 0x8C89
+#define GL_MAX_TRANSFORM_FEEDBACK_INTERLEAVED_ATTRIBS_NV 0x8C8A
+#define GL_MAX_TRANSFORM_FEEDBACK_SEPARATE_ATTRIBS_NV 0x8C8B
+#define GL_INTERLEAVED_ATTRIBS_NV 0x8C8C
+#define GL_SEPARATE_ATTRIBS_NV 0x8C8D
+#define GL_TRANSFORM_FEEDBACK_BUFFER_NV 0x8C8E
+#define GL_TRANSFORM_FEEDBACK_BUFFER_BINDING_NV 0x8C8F
+#define GL_LAYER_NV 0x8DAA
+#define GL_NEXT_BUFFER_NV -2
+#define GL_SKIP_COMPONENTS4_NV -3
+#define GL_SKIP_COMPONENTS3_NV -4
+#define GL_SKIP_COMPONENTS2_NV -5
+#define GL_SKIP_COMPONENTS1_NV -6
+#endif
+
+#ifndef GL_EXT_bindable_uniform
+#define GL_MAX_VERTEX_BINDABLE_UNIFORMS_EXT 0x8DE2
+#define GL_MAX_FRAGMENT_BINDABLE_UNIFORMS_EXT 0x8DE3
+#define GL_MAX_GEOMETRY_BINDABLE_UNIFORMS_EXT 0x8DE4
+#define GL_MAX_BINDABLE_UNIFORM_SIZE_EXT 0x8DED
+#define GL_UNIFORM_BUFFER_EXT 0x8DEE
+#define GL_UNIFORM_BUFFER_BINDING_EXT 0x8DEF
+#endif
+
+#ifndef GL_EXT_texture_integer
+#define GL_RGBA32UI_EXT 0x8D70
+#define GL_RGB32UI_EXT 0x8D71
+#define GL_ALPHA32UI_EXT 0x8D72
+#define GL_INTENSITY32UI_EXT 0x8D73
+#define GL_LUMINANCE32UI_EXT 0x8D74
+#define GL_LUMINANCE_ALPHA32UI_EXT 0x8D75
+#define GL_RGBA16UI_EXT 0x8D76
+#define GL_RGB16UI_EXT 0x8D77
+#define GL_ALPHA16UI_EXT 0x8D78
+#define GL_INTENSITY16UI_EXT 0x8D79
+#define GL_LUMINANCE16UI_EXT 0x8D7A
+#define GL_LUMINANCE_ALPHA16UI_EXT 0x8D7B
+#define GL_RGBA8UI_EXT 0x8D7C
+#define GL_RGB8UI_EXT 0x8D7D
+#define GL_ALPHA8UI_EXT 0x8D7E
+#define GL_INTENSITY8UI_EXT 0x8D7F
+#define GL_LUMINANCE8UI_EXT 0x8D80
+#define GL_LUMINANCE_ALPHA8UI_EXT 0x8D81
+#define GL_RGBA32I_EXT 0x8D82
+#define GL_RGB32I_EXT 0x8D83
+#define GL_ALPHA32I_EXT 0x8D84
+#define GL_INTENSITY32I_EXT 0x8D85
+#define GL_LUMINANCE32I_EXT 0x8D86
+#define GL_LUMINANCE_ALPHA32I_EXT 0x8D87
+#define GL_RGBA16I_EXT 0x8D88
+#define GL_RGB16I_EXT 0x8D89
+#define GL_ALPHA16I_EXT 0x8D8A
+#define GL_INTENSITY16I_EXT 0x8D8B
+#define GL_LUMINANCE16I_EXT 0x8D8C
+#define GL_LUMINANCE_ALPHA16I_EXT 0x8D8D
+#define GL_RGBA8I_EXT 0x8D8E
+#define GL_RGB8I_EXT 0x8D8F
+#define GL_ALPHA8I_EXT 0x8D90
+#define GL_INTENSITY8I_EXT 0x8D91
+#define GL_LUMINANCE8I_EXT 0x8D92
+#define GL_LUMINANCE_ALPHA8I_EXT 0x8D93
+#define GL_RED_INTEGER_EXT 0x8D94
+#define GL_GREEN_INTEGER_EXT 0x8D95
+#define GL_BLUE_INTEGER_EXT 0x8D96
+#define GL_ALPHA_INTEGER_EXT 0x8D97
+#define GL_RGB_INTEGER_EXT 0x8D98
+#define GL_RGBA_INTEGER_EXT 0x8D99
+#define GL_BGR_INTEGER_EXT 0x8D9A
+#define GL_BGRA_INTEGER_EXT 0x8D9B
+#define GL_LUMINANCE_INTEGER_EXT 0x8D9C
+#define GL_LUMINANCE_ALPHA_INTEGER_EXT 0x8D9D
+#define GL_RGBA_INTEGER_MODE_EXT 0x8D9E
+#endif
+
+#ifndef GL_GREMEDY_frame_terminator
+#endif
+
+#ifndef GL_NV_conditional_render
+#define GL_QUERY_WAIT_NV 0x8E13
+#define GL_QUERY_NO_WAIT_NV 0x8E14
+#define GL_QUERY_BY_REGION_WAIT_NV 0x8E15
+#define GL_QUERY_BY_REGION_NO_WAIT_NV 0x8E16
+#endif
+
+#ifndef GL_NV_present_video
+#define GL_FRAME_NV 0x8E26
+#define GL_FIELDS_NV 0x8E27
+#define GL_CURRENT_TIME_NV 0x8E28
+#define GL_NUM_FILL_STREAMS_NV 0x8E29
+#define GL_PRESENT_TIME_NV 0x8E2A
+#define GL_PRESENT_DURATION_NV 0x8E2B
+#endif
+
+#ifndef GL_EXT_transform_feedback
+#define GL_TRANSFORM_FEEDBACK_BUFFER_EXT 0x8C8E
+#define GL_TRANSFORM_FEEDBACK_BUFFER_START_EXT 0x8C84
+#define GL_TRANSFORM_FEEDBACK_BUFFER_SIZE_EXT 0x8C85
+#define GL_TRANSFORM_FEEDBACK_BUFFER_BINDING_EXT 0x8C8F
+#define GL_INTERLEAVED_ATTRIBS_EXT 0x8C8C
+#define GL_SEPARATE_ATTRIBS_EXT 0x8C8D
+#define GL_PRIMITIVES_GENERATED_EXT 0x8C87
+#define GL_TRANSFORM_FEEDBACK_PRIMITIVES_WRITTEN_EXT 0x8C88
+#define GL_RASTERIZER_DISCARD_EXT 0x8C89
+#define GL_MAX_TRANSFORM_FEEDBACK_INTERLEAVED_COMPONENTS_EXT 0x8C8A
+#define GL_MAX_TRANSFORM_FEEDBACK_SEPARATE_ATTRIBS_EXT 0x8C8B
+#define GL_MAX_TRANSFORM_FEEDBACK_SEPARATE_COMPONENTS_EXT 0x8C80
+#define GL_TRANSFORM_FEEDBACK_VARYINGS_EXT 0x8C83
+#define GL_TRANSFORM_FEEDBACK_BUFFER_MODE_EXT 0x8C7F
+#define GL_TRANSFORM_FEEDBACK_VARYING_MAX_LENGTH_EXT 0x8C76
+#endif
+
+#ifndef GL_EXT_direct_state_access
+#define GL_PROGRAM_MATRIX_EXT 0x8E2D
+#define GL_TRANSPOSE_PROGRAM_MATRIX_EXT 0x8E2E
+#define GL_PROGRAM_MATRIX_STACK_DEPTH_EXT 0x8E2F
+#endif
+
+#ifndef GL_EXT_vertex_array_bgra
+/* reuse GL_BGRA */
+#endif
+
+#ifndef GL_EXT_texture_swizzle
+#define GL_TEXTURE_SWIZZLE_R_EXT 0x8E42
+#define GL_TEXTURE_SWIZZLE_G_EXT 0x8E43
+#define GL_TEXTURE_SWIZZLE_B_EXT 0x8E44
+#define GL_TEXTURE_SWIZZLE_A_EXT 0x8E45
+#define GL_TEXTURE_SWIZZLE_RGBA_EXT 0x8E46
+#endif
+
+#ifndef GL_NV_explicit_multisample
+#define GL_SAMPLE_POSITION_NV 0x8E50
+#define GL_SAMPLE_MASK_NV 0x8E51
+#define GL_SAMPLE_MASK_VALUE_NV 0x8E52
+#define GL_TEXTURE_BINDING_RENDERBUFFER_NV 0x8E53
+#define GL_TEXTURE_RENDERBUFFER_DATA_STORE_BINDING_NV 0x8E54
+#define GL_TEXTURE_RENDERBUFFER_NV 0x8E55
+#define GL_SAMPLER_RENDERBUFFER_NV 0x8E56
+#define GL_INT_SAMPLER_RENDERBUFFER_NV 0x8E57
+#define GL_UNSIGNED_INT_SAMPLER_RENDERBUFFER_NV 0x8E58
+#define GL_MAX_SAMPLE_MASK_WORDS_NV 0x8E59
+#endif
+
+#ifndef GL_NV_transform_feedback2
+#define GL_TRANSFORM_FEEDBACK_NV 0x8E22
+#define GL_TRANSFORM_FEEDBACK_BUFFER_PAUSED_NV 0x8E23
+#define GL_TRANSFORM_FEEDBACK_BUFFER_ACTIVE_NV 0x8E24
+#define GL_TRANSFORM_FEEDBACK_BINDING_NV 0x8E25
+#endif
+
+#ifndef GL_ATI_meminfo
+#define GL_VBO_FREE_MEMORY_ATI 0x87FB
+#define GL_TEXTURE_FREE_MEMORY_ATI 0x87FC
+#define GL_RENDERBUFFER_FREE_MEMORY_ATI 0x87FD
+#endif
+
+#ifndef GL_AMD_performance_monitor
+#define GL_COUNTER_TYPE_AMD 0x8BC0
+#define GL_COUNTER_RANGE_AMD 0x8BC1
+#define GL_UNSIGNED_INT64_AMD 0x8BC2
+#define GL_PERCENTAGE_AMD 0x8BC3
+#define GL_PERFMON_RESULT_AVAILABLE_AMD 0x8BC4
+#define GL_PERFMON_RESULT_SIZE_AMD 0x8BC5
+#define GL_PERFMON_RESULT_AMD 0x8BC6
+#endif
+
+#ifndef GL_AMD_texture_texture4
+#endif
+
+#ifndef GL_AMD_vertex_shader_tesselator
+#define GL_SAMPLER_BUFFER_AMD 0x9001
+#define GL_INT_SAMPLER_BUFFER_AMD 0x9002
+#define GL_UNSIGNED_INT_SAMPLER_BUFFER_AMD 0x9003
+#define GL_TESSELLATION_MODE_AMD 0x9004
+#define GL_TESSELLATION_FACTOR_AMD 0x9005
+#define GL_DISCRETE_AMD 0x9006
+#define GL_CONTINUOUS_AMD 0x9007
+#endif
+
+#ifndef GL_EXT_provoking_vertex
+#define GL_QUADS_FOLLOW_PROVOKING_VERTEX_CONVENTION_EXT 0x8E4C
+#define GL_FIRST_VERTEX_CONVENTION_EXT 0x8E4D
+#define GL_LAST_VERTEX_CONVENTION_EXT 0x8E4E
+#define GL_PROVOKING_VERTEX_EXT 0x8E4F
+#endif
+
+#ifndef GL_EXT_texture_snorm
+#define GL_ALPHA_SNORM 0x9010
+#define GL_LUMINANCE_SNORM 0x9011
+#define GL_LUMINANCE_ALPHA_SNORM 0x9012
+#define GL_INTENSITY_SNORM 0x9013
+#define GL_ALPHA8_SNORM 0x9014
+#define GL_LUMINANCE8_SNORM 0x9015
+#define GL_LUMINANCE8_ALPHA8_SNORM 0x9016
+#define GL_INTENSITY8_SNORM 0x9017
+#define GL_ALPHA16_SNORM 0x9018
+#define GL_LUMINANCE16_SNORM 0x9019
+#define GL_LUMINANCE16_ALPHA16_SNORM 0x901A
+#define GL_INTENSITY16_SNORM 0x901B
+/* reuse GL_RED_SNORM */
+/* reuse GL_RG_SNORM */
+/* reuse GL_RGB_SNORM */
+/* reuse GL_RGBA_SNORM */
+/* reuse GL_R8_SNORM */
+/* reuse GL_RG8_SNORM */
+/* reuse GL_RGB8_SNORM */
+/* reuse GL_RGBA8_SNORM */
+/* reuse GL_R16_SNORM */
+/* reuse GL_RG16_SNORM */
+/* reuse GL_RGB16_SNORM */
+/* reuse GL_RGBA16_SNORM */
+/* reuse GL_SIGNED_NORMALIZED */
+#endif
+
+#ifndef GL_AMD_draw_buffers_blend
+#endif
+
+#ifndef GL_APPLE_texture_range
+#define GL_TEXTURE_RANGE_LENGTH_APPLE 0x85B7
+#define GL_TEXTURE_RANGE_POINTER_APPLE 0x85B8
+#define GL_TEXTURE_STORAGE_HINT_APPLE 0x85BC
+#define GL_STORAGE_PRIVATE_APPLE 0x85BD
+/* reuse GL_STORAGE_CACHED_APPLE */
+/* reuse GL_STORAGE_SHARED_APPLE */
+#endif
+
+#ifndef GL_APPLE_float_pixels
+#define GL_HALF_APPLE 0x140B
+#define GL_RGBA_FLOAT32_APPLE 0x8814
+#define GL_RGB_FLOAT32_APPLE 0x8815
+#define GL_ALPHA_FLOAT32_APPLE 0x8816
+#define GL_INTENSITY_FLOAT32_APPLE 0x8817
+#define GL_LUMINANCE_FLOAT32_APPLE 0x8818
+#define GL_LUMINANCE_ALPHA_FLOAT32_APPLE 0x8819
+#define GL_RGBA_FLOAT16_APPLE 0x881A
+#define GL_RGB_FLOAT16_APPLE 0x881B
+#define GL_ALPHA_FLOAT16_APPLE 0x881C
+#define GL_INTENSITY_FLOAT16_APPLE 0x881D
+#define GL_LUMINANCE_FLOAT16_APPLE 0x881E
+#define GL_LUMINANCE_ALPHA_FLOAT16_APPLE 0x881F
+#define GL_COLOR_FLOAT_APPLE 0x8A0F
+#endif
+
+#ifndef GL_APPLE_vertex_program_evaluators
+#define GL_VERTEX_ATTRIB_MAP1_APPLE 0x8A00
+#define GL_VERTEX_ATTRIB_MAP2_APPLE 0x8A01
+#define GL_VERTEX_ATTRIB_MAP1_SIZE_APPLE 0x8A02
+#define GL_VERTEX_ATTRIB_MAP1_COEFF_APPLE 0x8A03
+#define GL_VERTEX_ATTRIB_MAP1_ORDER_APPLE 0x8A04
+#define GL_VERTEX_ATTRIB_MAP1_DOMAIN_APPLE 0x8A05
+#define GL_VERTEX_ATTRIB_MAP2_SIZE_APPLE 0x8A06
+#define GL_VERTEX_ATTRIB_MAP2_COEFF_APPLE 0x8A07
+#define GL_VERTEX_ATTRIB_MAP2_ORDER_APPLE 0x8A08
+#define GL_VERTEX_ATTRIB_MAP2_DOMAIN_APPLE 0x8A09
+#endif
+
+#ifndef GL_APPLE_aux_depth_stencil
+#define GL_AUX_DEPTH_STENCIL_APPLE 0x8A14
+#endif
+
+#ifndef GL_APPLE_object_purgeable
+#define GL_BUFFER_OBJECT_APPLE 0x85B3
+#define GL_RELEASED_APPLE 0x8A19
+#define GL_VOLATILE_APPLE 0x8A1A
+#define GL_RETAINED_APPLE 0x8A1B
+#define GL_UNDEFINED_APPLE 0x8A1C
+#define GL_PURGEABLE_APPLE 0x8A1D
+#endif
+
+#ifndef GL_APPLE_row_bytes
+#define GL_PACK_ROW_BYTES_APPLE 0x8A15
+#define GL_UNPACK_ROW_BYTES_APPLE 0x8A16
+#endif
+
+#ifndef GL_APPLE_rgb_422
+#define GL_RGB_422_APPLE 0x8A1F
+/* reuse GL_UNSIGNED_SHORT_8_8_APPLE */
+/* reuse GL_UNSIGNED_SHORT_8_8_REV_APPLE */
+#endif
+
+#ifndef GL_NV_video_capture
+#define GL_VIDEO_BUFFER_NV 0x9020
+#define GL_VIDEO_BUFFER_BINDING_NV 0x9021
+#define GL_FIELD_UPPER_NV 0x9022
+#define GL_FIELD_LOWER_NV 0x9023
+#define GL_NUM_VIDEO_CAPTURE_STREAMS_NV 0x9024
+#define GL_NEXT_VIDEO_CAPTURE_BUFFER_STATUS_NV 0x9025
+#define GL_VIDEO_CAPTURE_TO_422_SUPPORTED_NV 0x9026
+#define GL_LAST_VIDEO_CAPTURE_STATUS_NV 0x9027
+#define GL_VIDEO_BUFFER_PITCH_NV 0x9028
+#define GL_VIDEO_COLOR_CONVERSION_MATRIX_NV 0x9029
+#define GL_VIDEO_COLOR_CONVERSION_MAX_NV 0x902A
+#define GL_VIDEO_COLOR_CONVERSION_MIN_NV 0x902B
+#define GL_VIDEO_COLOR_CONVERSION_OFFSET_NV 0x902C
+#define GL_VIDEO_BUFFER_INTERNAL_FORMAT_NV 0x902D
+#define GL_PARTIAL_SUCCESS_NV 0x902E
+#define GL_SUCCESS_NV 0x902F
+#define GL_FAILURE_NV 0x9030
+#define GL_YCBYCR8_422_NV 0x9031
+#define GL_YCBAYCR8A_4224_NV 0x9032
+#define GL_Z6Y10Z6CB10Z6Y10Z6CR10_422_NV 0x9033
+#define GL_Z6Y10Z6CB10Z6A10Z6Y10Z6CR10Z6A10_4224_NV 0x9034
+#define GL_Z4Y12Z4CB12Z4Y12Z4CR12_422_NV 0x9035
+#define GL_Z4Y12Z4CB12Z4A12Z4Y12Z4CR12Z4A12_4224_NV 0x9036
+#define GL_Z4Y12Z4CB12Z4CR12_444_NV 0x9037
+#define GL_VIDEO_CAPTURE_FRAME_WIDTH_NV 0x9038
+#define GL_VIDEO_CAPTURE_FRAME_HEIGHT_NV 0x9039
+#define GL_VIDEO_CAPTURE_FIELD_UPPER_HEIGHT_NV 0x903A
+#define GL_VIDEO_CAPTURE_FIELD_LOWER_HEIGHT_NV 0x903B
+#define GL_VIDEO_CAPTURE_SURFACE_ORIGIN_NV 0x903C
+#endif
+
+#ifndef GL_NV_copy_image
+#endif
+
+#ifndef GL_EXT_separate_shader_objects
+#define GL_ACTIVE_PROGRAM_EXT 0x8B8D
+#endif
+
+#ifndef GL_NV_parameter_buffer_object2
+#endif
+
+#ifndef GL_NV_shader_buffer_load
+#define GL_BUFFER_GPU_ADDRESS_NV 0x8F1D
+#define GL_GPU_ADDRESS_NV 0x8F34
+#define GL_MAX_SHADER_BUFFER_ADDRESS_NV 0x8F35
+#endif
+
+#ifndef GL_NV_vertex_buffer_unified_memory
+#define GL_VERTEX_ATTRIB_ARRAY_UNIFIED_NV 0x8F1E
+#define GL_ELEMENT_ARRAY_UNIFIED_NV 0x8F1F
+#define GL_VERTEX_ATTRIB_ARRAY_ADDRESS_NV 0x8F20
+#define GL_VERTEX_ARRAY_ADDRESS_NV 0x8F21
+#define GL_NORMAL_ARRAY_ADDRESS_NV 0x8F22
+#define GL_COLOR_ARRAY_ADDRESS_NV 0x8F23
+#define GL_INDEX_ARRAY_ADDRESS_NV 0x8F24
+#define GL_TEXTURE_COORD_ARRAY_ADDRESS_NV 0x8F25
+#define GL_EDGE_FLAG_ARRAY_ADDRESS_NV 0x8F26
+#define GL_SECONDARY_COLOR_ARRAY_ADDRESS_NV 0x8F27
+#define GL_FOG_COORD_ARRAY_ADDRESS_NV 0x8F28
+#define GL_ELEMENT_ARRAY_ADDRESS_NV 0x8F29
+#define GL_VERTEX_ATTRIB_ARRAY_LENGTH_NV 0x8F2A
+#define GL_VERTEX_ARRAY_LENGTH_NV 0x8F2B
+#define GL_NORMAL_ARRAY_LENGTH_NV 0x8F2C
+#define GL_COLOR_ARRAY_LENGTH_NV 0x8F2D
+#define GL_INDEX_ARRAY_LENGTH_NV 0x8F2E
+#define GL_TEXTURE_COORD_ARRAY_LENGTH_NV 0x8F2F
+#define GL_EDGE_FLAG_ARRAY_LENGTH_NV 0x8F30
+#define GL_SECONDARY_COLOR_ARRAY_LENGTH_NV 0x8F31
+#define GL_FOG_COORD_ARRAY_LENGTH_NV 0x8F32
+#define GL_ELEMENT_ARRAY_LENGTH_NV 0x8F33
+#define GL_DRAW_INDIRECT_UNIFIED_NV 0x8F40
+#define GL_DRAW_INDIRECT_ADDRESS_NV 0x8F41
+#define GL_DRAW_INDIRECT_LENGTH_NV 0x8F42
+#endif
+
+#ifndef GL_NV_texture_barrier
+#endif
+
+#ifndef GL_AMD_shader_stencil_export
+#endif
+
+#ifndef GL_AMD_seamless_cubemap_per_texture
+/* reuse GL_TEXTURE_CUBE_MAP_SEAMLESS */
+#endif
+
+#ifndef GL_AMD_conservative_depth
+#endif
+
+#ifndef GL_EXT_shader_image_load_store
+#define GL_MAX_IMAGE_UNITS_EXT 0x8F38
+#define GL_MAX_COMBINED_IMAGE_UNITS_AND_FRAGMENT_OUTPUTS_EXT 0x8F39
+#define GL_IMAGE_BINDING_NAME_EXT 0x8F3A
+#define GL_IMAGE_BINDING_LEVEL_EXT 0x8F3B
+#define GL_IMAGE_BINDING_LAYERED_EXT 0x8F3C
+#define GL_IMAGE_BINDING_LAYER_EXT 0x8F3D
+#define GL_IMAGE_BINDING_ACCESS_EXT 0x8F3E
+#define GL_IMAGE_1D_EXT 0x904C
+#define GL_IMAGE_2D_EXT 0x904D
+#define GL_IMAGE_3D_EXT 0x904E
+#define GL_IMAGE_2D_RECT_EXT 0x904F
+#define GL_IMAGE_CUBE_EXT 0x9050
+#define GL_IMAGE_BUFFER_EXT 0x9051
+#define GL_IMAGE_1D_ARRAY_EXT 0x9052
+#define GL_IMAGE_2D_ARRAY_EXT 0x9053
+#define GL_IMAGE_CUBE_MAP_ARRAY_EXT 0x9054
+#define GL_IMAGE_2D_MULTISAMPLE_EXT 0x9055
+#define GL_IMAGE_2D_MULTISAMPLE_ARRAY_EXT 0x9056
+#define GL_INT_IMAGE_1D_EXT 0x9057
+#define GL_INT_IMAGE_2D_EXT 0x9058
+#define GL_INT_IMAGE_3D_EXT 0x9059
+#define GL_INT_IMAGE_2D_RECT_EXT 0x905A
+#define GL_INT_IMAGE_CUBE_EXT 0x905B
+#define GL_INT_IMAGE_BUFFER_EXT 0x905C
+#define GL_INT_IMAGE_1D_ARRAY_EXT 0x905D
+#define GL_INT_IMAGE_2D_ARRAY_EXT 0x905E
+#define GL_INT_IMAGE_CUBE_MAP_ARRAY_EXT 0x905F
+#define GL_INT_IMAGE_2D_MULTISAMPLE_EXT 0x9060
+#define GL_INT_IMAGE_2D_MULTISAMPLE_ARRAY_EXT 0x9061
+#define GL_UNSIGNED_INT_IMAGE_1D_EXT 0x9062
+#define GL_UNSIGNED_INT_IMAGE_2D_EXT 0x9063
+#define GL_UNSIGNED_INT_IMAGE_3D_EXT 0x9064
+#define GL_UNSIGNED_INT_IMAGE_2D_RECT_EXT 0x9065
+#define GL_UNSIGNED_INT_IMAGE_CUBE_EXT 0x9066
+#define GL_UNSIGNED_INT_IMAGE_BUFFER_EXT 0x9067
+#define GL_UNSIGNED_INT_IMAGE_1D_ARRAY_EXT 0x9068
+#define GL_UNSIGNED_INT_IMAGE_2D_ARRAY_EXT 0x9069
+#define GL_UNSIGNED_INT_IMAGE_CUBE_MAP_ARRAY_EXT 0x906A
+#define GL_UNSIGNED_INT_IMAGE_2D_MULTISAMPLE_EXT 0x906B
+#define GL_UNSIGNED_INT_IMAGE_2D_MULTISAMPLE_ARRAY_EXT 0x906C
+#define GL_MAX_IMAGE_SAMPLES_EXT 0x906D
+#define GL_IMAGE_BINDING_FORMAT_EXT 0x906E
+#define GL_VERTEX_ATTRIB_ARRAY_BARRIER_BIT_EXT 0x00000001
+#define GL_ELEMENT_ARRAY_BARRIER_BIT_EXT 0x00000002
+#define GL_UNIFORM_BARRIER_BIT_EXT 0x00000004
+#define GL_TEXTURE_FETCH_BARRIER_BIT_EXT 0x00000008
+#define GL_SHADER_IMAGE_ACCESS_BARRIER_BIT_EXT 0x00000020
+#define GL_COMMAND_BARRIER_BIT_EXT 0x00000040
+#define GL_PIXEL_BUFFER_BARRIER_BIT_EXT 0x00000080
+#define GL_TEXTURE_UPDATE_BARRIER_BIT_EXT 0x00000100
+#define GL_BUFFER_UPDATE_BARRIER_BIT_EXT 0x00000200
+#define GL_FRAMEBUFFER_BARRIER_BIT_EXT 0x00000400
+#define GL_TRANSFORM_FEEDBACK_BARRIER_BIT_EXT 0x00000800
+#define GL_ATOMIC_COUNTER_BARRIER_BIT_EXT 0x00001000
+#define GL_ALL_BARRIER_BITS_EXT 0xFFFFFFFF
+#endif
+
+#ifndef GL_EXT_vertex_attrib_64bit
+/* reuse GL_DOUBLE */
+#define GL_DOUBLE_VEC2_EXT 0x8FFC
+#define GL_DOUBLE_VEC3_EXT 0x8FFD
+#define GL_DOUBLE_VEC4_EXT 0x8FFE
+#define GL_DOUBLE_MAT2_EXT 0x8F46
+#define GL_DOUBLE_MAT3_EXT 0x8F47
+#define GL_DOUBLE_MAT4_EXT 0x8F48
+#define GL_DOUBLE_MAT2x3_EXT 0x8F49
+#define GL_DOUBLE_MAT2x4_EXT 0x8F4A
+#define GL_DOUBLE_MAT3x2_EXT 0x8F4B
+#define GL_DOUBLE_MAT3x4_EXT 0x8F4C
+#define GL_DOUBLE_MAT4x2_EXT 0x8F4D
+#define GL_DOUBLE_MAT4x3_EXT 0x8F4E
+#endif
+
+#ifndef GL_NV_gpu_program5
+#define GL_MAX_GEOMETRY_PROGRAM_INVOCATIONS_NV 0x8E5A
+#define GL_MIN_FRAGMENT_INTERPOLATION_OFFSET_NV 0x8E5B
+#define GL_MAX_FRAGMENT_INTERPOLATION_OFFSET_NV 0x8E5C
+#define GL_FRAGMENT_PROGRAM_INTERPOLATION_OFFSET_BITS_NV 0x8E5D
+#define GL_MIN_PROGRAM_TEXTURE_GATHER_OFFSET_NV 0x8E5E
+#define GL_MAX_PROGRAM_TEXTURE_GATHER_OFFSET_NV 0x8E5F
+#define GL_MAX_PROGRAM_SUBROUTINE_PARAMETERS_NV 0x8F44
+#define GL_MAX_PROGRAM_SUBROUTINE_NUM_NV 0x8F45
+#endif
+
+#ifndef GL_NV_gpu_shader5
+#define GL_INT64_NV 0x140E
+#define GL_UNSIGNED_INT64_NV 0x140F
+#define GL_INT8_NV 0x8FE0
+#define GL_INT8_VEC2_NV 0x8FE1
+#define GL_INT8_VEC3_NV 0x8FE2
+#define GL_INT8_VEC4_NV 0x8FE3
+#define GL_INT16_NV 0x8FE4
+#define GL_INT16_VEC2_NV 0x8FE5
+#define GL_INT16_VEC3_NV 0x8FE6
+#define GL_INT16_VEC4_NV 0x8FE7
+#define GL_INT64_VEC2_NV 0x8FE9
+#define GL_INT64_VEC3_NV 0x8FEA
+#define GL_INT64_VEC4_NV 0x8FEB
+#define GL_UNSIGNED_INT8_NV 0x8FEC
+#define GL_UNSIGNED_INT8_VEC2_NV 0x8FED
+#define GL_UNSIGNED_INT8_VEC3_NV 0x8FEE
+#define GL_UNSIGNED_INT8_VEC4_NV 0x8FEF
+#define GL_UNSIGNED_INT16_NV 0x8FF0
+#define GL_UNSIGNED_INT16_VEC2_NV 0x8FF1
+#define GL_UNSIGNED_INT16_VEC3_NV 0x8FF2
+#define GL_UNSIGNED_INT16_VEC4_NV 0x8FF3
+#define GL_UNSIGNED_INT64_VEC2_NV 0x8FF5
+#define GL_UNSIGNED_INT64_VEC3_NV 0x8FF6
+#define GL_UNSIGNED_INT64_VEC4_NV 0x8FF7
+#define GL_FLOAT16_NV 0x8FF8
+#define GL_FLOAT16_VEC2_NV 0x8FF9
+#define GL_FLOAT16_VEC3_NV 0x8FFA
+#define GL_FLOAT16_VEC4_NV 0x8FFB
+/* reuse GL_PATCHES */
+#endif
+
+#ifndef GL_NV_shader_buffer_store
+#define GL_SHADER_GLOBAL_ACCESS_BARRIER_BIT_NV 0x00000010
+/* reuse GL_READ_WRITE */
+/* reuse GL_WRITE_ONLY */
+#endif
+
+#ifndef GL_NV_tessellation_program5
+#define GL_MAX_PROGRAM_PATCH_ATTRIBS_NV 0x86D8
+#define GL_TESS_CONTROL_PROGRAM_NV 0x891E
+#define GL_TESS_EVALUATION_PROGRAM_NV 0x891F
+#define GL_TESS_CONTROL_PROGRAM_PARAMETER_BUFFER_NV 0x8C74
+#define GL_TESS_EVALUATION_PROGRAM_PARAMETER_BUFFER_NV 0x8C75
+#endif
+
+#ifndef GL_NV_vertex_attrib_integer_64bit
+/* reuse GL_INT64_NV */
+/* reuse GL_UNSIGNED_INT64_NV */
+#endif
+
+#ifndef GL_NV_multisample_coverage
+#define GL_COVERAGE_SAMPLES_NV 0x80A9
+#define GL_COLOR_SAMPLES_NV 0x8E20
+#endif
+
+#ifndef GL_AMD_name_gen_delete
+#define GL_DATA_BUFFER_AMD 0x9151
+#define GL_PERFORMANCE_MONITOR_AMD 0x9152
+#define GL_QUERY_OBJECT_AMD 0x9153
+#define GL_VERTEX_ARRAY_OBJECT_AMD 0x9154
+#define GL_SAMPLER_OBJECT_AMD 0x9155
+#endif
+
+#ifndef GL_AMD_debug_output
+#define GL_MAX_DEBUG_LOGGED_MESSAGES_AMD 0x9144
+#define GL_DEBUG_LOGGED_MESSAGES_AMD 0x9145
+#define GL_DEBUG_SEVERITY_HIGH_AMD 0x9146
+#define GL_DEBUG_SEVERITY_MEDIUM_AMD 0x9147
+#define GL_DEBUG_SEVERITY_LOW_AMD 0x9148
+#define GL_DEBUG_CATEGORY_API_ERROR_AMD 0x9149
+#define GL_DEBUG_CATEGORY_WINDOW_SYSTEM_AMD 0x914A
+#define GL_DEBUG_CATEGORY_DEPRECATION_AMD 0x914B
+#define GL_DEBUG_CATEGORY_UNDEFINED_BEHAVIOR_AMD 0x914C
+#define GL_DEBUG_CATEGORY_PERFORMANCE_AMD 0x914D
+#define GL_DEBUG_CATEGORY_SHADER_COMPILER_AMD 0x914E
+#define GL_DEBUG_CATEGORY_APPLICATION_AMD 0x914F
+#define GL_DEBUG_CATEGORY_OTHER_AMD 0x9150
+#endif
+
+#ifndef GL_NV_vdpau_interop
+#define GL_SURFACE_STATE_NV 0x86EB
+#define GL_SURFACE_REGISTERED_NV 0x86FD
+#define GL_SURFACE_MAPPED_NV 0x8700
+#define GL_WRITE_DISCARD_NV 0x88BE
+#endif
+
+#ifndef GL_AMD_transform_feedback3_lines_triangles
+#endif
+
+
+/*************************************************************/
+
+#include <stddef.h>
+#ifndef GL_VERSION_2_0
+/* GL type for program/shader text */
+typedef char GLchar;
+#endif
+
+#ifndef GL_VERSION_1_5
+/* GL types for handling large vertex buffer objects */
+typedef ptrdiff_t GLintptr;
+typedef ptrdiff_t GLsizeiptr;
+#endif
+
+#ifndef GL_ARB_vertex_buffer_object
+/* GL types for handling large vertex buffer objects */
+typedef ptrdiff_t GLintptrARB;
+typedef ptrdiff_t GLsizeiptrARB;
+#endif
+
+#ifndef GL_ARB_shader_objects
+/* GL types for program/shader text and shader object handles */
+typedef char GLcharARB;
+typedef unsigned int GLhandleARB;
+#endif
+
+/* GL type for "half" precision (s10e5) float data in host memory */
+#ifndef GL_ARB_half_float_pixel
+typedef unsigned short GLhalfARB;
+#endif
+
+#ifndef GL_NV_half_float
+typedef unsigned short GLhalfNV;
+#endif
+
+#ifndef GLEXT_64_TYPES_DEFINED
+/* This code block is duplicated in glxext.h, so must be protected */
+#define GLEXT_64_TYPES_DEFINED
+/* Define int32_t, int64_t, and uint64_t types for UST/MSC */
+/* (as used in the GL_EXT_timer_query extension). */
+#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L
+#include <inttypes.h>
+#elif defined(__sun__) || defined(__digital__)
+#include <inttypes.h>
+#if defined(__STDC__)
+#if defined(__arch64__) || defined(_LP64)
+typedef long int int64_t;
+typedef unsigned long int uint64_t;
+#else
+typedef long long int int64_t;
+typedef unsigned long long int uint64_t;
+#endif /* __arch64__ */
+#endif /* __STDC__ */
+#elif defined( __VMS ) || defined(__sgi)
+#include <inttypes.h>
+#elif defined(__SCO__) || defined(__USLC__)
+#include <stdint.h>
+#elif defined(__UNIXOS2__) || defined(__SOL64__)
+typedef long int int32_t;
+typedef long long int int64_t;
+typedef unsigned long long int uint64_t;
+#elif defined(_WIN32) && defined(__GNUC__)
+#include <stdint.h>
+#elif defined(_WIN32)
+typedef __int32 int32_t;
+typedef __int64 int64_t;
+typedef unsigned __int64 uint64_t;
+#else
+/* Fallback if nothing above works */
+#include <inttypes.h>
+#endif
+#endif
+
+#ifndef GL_EXT_timer_query
+typedef int64_t GLint64EXT;
+typedef uint64_t GLuint64EXT;
+#endif
+
+#ifndef GL_ARB_sync
+typedef int64_t GLint64;
+typedef uint64_t GLuint64;
+typedef struct __GLsync *GLsync;
+#endif
+
+#ifndef GL_ARB_cl_event
+/* These incomplete types let us declare types compatible with OpenCL's cl_context and cl_event */
+struct _cl_context;
+struct _cl_event;
+#endif
+
+#ifndef GL_ARB_debug_output
+typedef void (APIENTRY *GLDEBUGPROCARB)(GLenum source,GLenum type,GLuint id,GLenum severity,GLsizei length,const GLchar *message,GLvoid *userParam);
+#endif
+
+#ifndef GL_AMD_debug_output
+typedef void (APIENTRY *GLDEBUGPROCAMD)(GLuint id,GLenum category,GLenum severity,GLsizei length,const GLchar *message,GLvoid *userParam);
+#endif
+
+#ifndef GL_NV_vdpau_interop
+typedef GLintptr GLvdpauSurfaceNV;
+#endif
+
+#ifndef GL_VERSION_1_2
+#define GL_VERSION_1_2 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBlendColor (GLclampf red, GLclampf green, GLclampf blue, GLclampf alpha);
+GLAPI void APIENTRY glBlendEquation (GLenum mode);
+GLAPI void APIENTRY glDrawRangeElements (GLenum mode, GLuint start, GLuint end, GLsizei count, GLenum type, const GLvoid *indices);
+GLAPI void APIENTRY glTexImage3D (GLenum target, GLint level, GLint internalformat, GLsizei width, GLsizei height, GLsizei depth, GLint border, GLenum format, GLenum type, const GLvoid *pixels);
+GLAPI void APIENTRY glTexSubImage3D (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLsizei width, GLsizei height, GLsizei depth, GLenum format, GLenum type, const GLvoid *pixels);
+GLAPI void APIENTRY glCopyTexSubImage3D (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLint x, GLint y, GLsizei width, GLsizei height);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBLENDCOLORPROC) (GLclampf red, GLclampf green, GLclampf blue, GLclampf alpha);
+typedef void (APIENTRYP PFNGLBLENDEQUATIONPROC) (GLenum mode);
+typedef void (APIENTRYP PFNGLDRAWRANGEELEMENTSPROC) (GLenum mode, GLuint start, GLuint end, GLsizei count, GLenum type, const GLvoid *indices);
+typedef void (APIENTRYP PFNGLTEXIMAGE3DPROC) (GLenum target, GLint level, GLint internalformat, GLsizei width, GLsizei height, GLsizei depth, GLint border, GLenum format, GLenum type, const GLvoid *pixels);
+typedef void (APIENTRYP PFNGLTEXSUBIMAGE3DPROC) (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLsizei width, GLsizei height, GLsizei depth, GLenum format, GLenum type, const GLvoid *pixels);
+typedef void (APIENTRYP PFNGLCOPYTEXSUBIMAGE3DPROC) (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLint x, GLint y, GLsizei width, GLsizei height);
+#endif
+
+#ifndef GL_VERSION_1_2_DEPRECATED
+#define GL_VERSION_1_2_DEPRECATED 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glColorTable (GLenum target, GLenum internalformat, GLsizei width, GLenum format, GLenum type, const GLvoid *table);
+GLAPI void APIENTRY glColorTableParameterfv (GLenum target, GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glColorTableParameteriv (GLenum target, GLenum pname, const GLint *params);
+GLAPI void APIENTRY glCopyColorTable (GLenum target, GLenum internalformat, GLint x, GLint y, GLsizei width);
+GLAPI void APIENTRY glGetColorTable (GLenum target, GLenum format, GLenum type, GLvoid *table);
+GLAPI void APIENTRY glGetColorTableParameterfv (GLenum target, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetColorTableParameteriv (GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glColorSubTable (GLenum target, GLsizei start, GLsizei count, GLenum format, GLenum type, const GLvoid *data);
+GLAPI void APIENTRY glCopyColorSubTable (GLenum target, GLsizei start, GLint x, GLint y, GLsizei width);
+GLAPI void APIENTRY glConvolutionFilter1D (GLenum target, GLenum internalformat, GLsizei width, GLenum format, GLenum type, const GLvoid *image);
+GLAPI void APIENTRY glConvolutionFilter2D (GLenum target, GLenum internalformat, GLsizei width, GLsizei height, GLenum format, GLenum type, const GLvoid *image);
+GLAPI void APIENTRY glConvolutionParameterf (GLenum target, GLenum pname, GLfloat params);
+GLAPI void APIENTRY glConvolutionParameterfv (GLenum target, GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glConvolutionParameteri (GLenum target, GLenum pname, GLint params);
+GLAPI void APIENTRY glConvolutionParameteriv (GLenum target, GLenum pname, const GLint *params);
+GLAPI void APIENTRY glCopyConvolutionFilter1D (GLenum target, GLenum internalformat, GLint x, GLint y, GLsizei width);
+GLAPI void APIENTRY glCopyConvolutionFilter2D (GLenum target, GLenum internalformat, GLint x, GLint y, GLsizei width, GLsizei height);
+GLAPI void APIENTRY glGetConvolutionFilter (GLenum target, GLenum format, GLenum type, GLvoid *image);
+GLAPI void APIENTRY glGetConvolutionParameterfv (GLenum target, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetConvolutionParameteriv (GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetSeparableFilter (GLenum target, GLenum format, GLenum type, GLvoid *row, GLvoid *column, GLvoid *span);
+GLAPI void APIENTRY glSeparableFilter2D (GLenum target, GLenum internalformat, GLsizei width, GLsizei height, GLenum format, GLenum type, const GLvoid *row, const GLvoid *column);
+GLAPI void APIENTRY glGetHistogram (GLenum target, GLboolean reset, GLenum format, GLenum type, GLvoid *values);
+GLAPI void APIENTRY glGetHistogramParameterfv (GLenum target, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetHistogramParameteriv (GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetMinmax (GLenum target, GLboolean reset, GLenum format, GLenum type, GLvoid *values);
+GLAPI void APIENTRY glGetMinmaxParameterfv (GLenum target, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetMinmaxParameteriv (GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glHistogram (GLenum target, GLsizei width, GLenum internalformat, GLboolean sink);
+GLAPI void APIENTRY glMinmax (GLenum target, GLenum internalformat, GLboolean sink);
+GLAPI void APIENTRY glResetHistogram (GLenum target);
+GLAPI void APIENTRY glResetMinmax (GLenum target);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCOLORTABLEPROC) (GLenum target, GLenum internalformat, GLsizei width, GLenum format, GLenum type, const GLvoid *table);
+typedef void (APIENTRYP PFNGLCOLORTABLEPARAMETERFVPROC) (GLenum target, GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLCOLORTABLEPARAMETERIVPROC) (GLenum target, GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLCOPYCOLORTABLEPROC) (GLenum target, GLenum internalformat, GLint x, GLint y, GLsizei width);
+typedef void (APIENTRYP PFNGLGETCOLORTABLEPROC) (GLenum target, GLenum format, GLenum type, GLvoid *table);
+typedef void (APIENTRYP PFNGLGETCOLORTABLEPARAMETERFVPROC) (GLenum target, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETCOLORTABLEPARAMETERIVPROC) (GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLCOLORSUBTABLEPROC) (GLenum target, GLsizei start, GLsizei count, GLenum format, GLenum type, const GLvoid *data);
+typedef void (APIENTRYP PFNGLCOPYCOLORSUBTABLEPROC) (GLenum target, GLsizei start, GLint x, GLint y, GLsizei width);
+typedef void (APIENTRYP PFNGLCONVOLUTIONFILTER1DPROC) (GLenum target, GLenum internalformat, GLsizei width, GLenum format, GLenum type, const GLvoid *image);
+typedef void (APIENTRYP PFNGLCONVOLUTIONFILTER2DPROC) (GLenum target, GLenum internalformat, GLsizei width, GLsizei height, GLenum format, GLenum type, const GLvoid *image);
+typedef void (APIENTRYP PFNGLCONVOLUTIONPARAMETERFPROC) (GLenum target, GLenum pname, GLfloat params);
+typedef void (APIENTRYP PFNGLCONVOLUTIONPARAMETERFVPROC) (GLenum target, GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLCONVOLUTIONPARAMETERIPROC) (GLenum target, GLenum pname, GLint params);
+typedef void (APIENTRYP PFNGLCONVOLUTIONPARAMETERIVPROC) (GLenum target, GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLCOPYCONVOLUTIONFILTER1DPROC) (GLenum target, GLenum internalformat, GLint x, GLint y, GLsizei width);
+typedef void (APIENTRYP PFNGLCOPYCONVOLUTIONFILTER2DPROC) (GLenum target, GLenum internalformat, GLint x, GLint y, GLsizei width, GLsizei height);
+typedef void (APIENTRYP PFNGLGETCONVOLUTIONFILTERPROC) (GLenum target, GLenum format, GLenum type, GLvoid *image);
+typedef void (APIENTRYP PFNGLGETCONVOLUTIONPARAMETERFVPROC) (GLenum target, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETCONVOLUTIONPARAMETERIVPROC) (GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETSEPARABLEFILTERPROC) (GLenum target, GLenum format, GLenum type, GLvoid *row, GLvoid *column, GLvoid *span);
+typedef void (APIENTRYP PFNGLSEPARABLEFILTER2DPROC) (GLenum target, GLenum internalformat, GLsizei width, GLsizei height, GLenum format, GLenum type, const GLvoid *row, const GLvoid *column);
+typedef void (APIENTRYP PFNGLGETHISTOGRAMPROC) (GLenum target, GLboolean reset, GLenum format, GLenum type, GLvoid *values);
+typedef void (APIENTRYP PFNGLGETHISTOGRAMPARAMETERFVPROC) (GLenum target, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETHISTOGRAMPARAMETERIVPROC) (GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETMINMAXPROC) (GLenum target, GLboolean reset, GLenum format, GLenum type, GLvoid *values);
+typedef void (APIENTRYP PFNGLGETMINMAXPARAMETERFVPROC) (GLenum target, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETMINMAXPARAMETERIVPROC) (GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLHISTOGRAMPROC) (GLenum target, GLsizei width, GLenum internalformat, GLboolean sink);
+typedef void (APIENTRYP PFNGLMINMAXPROC) (GLenum target, GLenum internalformat, GLboolean sink);
+typedef void (APIENTRYP PFNGLRESETHISTOGRAMPROC) (GLenum target);
+typedef void (APIENTRYP PFNGLRESETMINMAXPROC) (GLenum target);
+#endif
+
+#ifndef GL_VERSION_1_3
+#define GL_VERSION_1_3 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glActiveTexture (GLenum texture);
+GLAPI void APIENTRY glSampleCoverage (GLclampf value, GLboolean invert);
+GLAPI void APIENTRY glCompressedTexImage3D (GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLsizei depth, GLint border, GLsizei imageSize, const GLvoid *data);
+GLAPI void APIENTRY glCompressedTexImage2D (GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLint border, GLsizei imageSize, const GLvoid *data);
+GLAPI void APIENTRY glCompressedTexImage1D (GLenum target, GLint level, GLenum internalformat, GLsizei width, GLint border, GLsizei imageSize, const GLvoid *data);
+GLAPI void APIENTRY glCompressedTexSubImage3D (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLsizei width, GLsizei height, GLsizei depth, GLenum format, GLsizei imageSize, const GLvoid *data);
+GLAPI void APIENTRY glCompressedTexSubImage2D (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLsizei width, GLsizei height, GLenum format, GLsizei imageSize, const GLvoid *data);
+GLAPI void APIENTRY glCompressedTexSubImage1D (GLenum target, GLint level, GLint xoffset, GLsizei width, GLenum format, GLsizei imageSize, const GLvoid *data);
+GLAPI void APIENTRY glGetCompressedTexImage (GLenum target, GLint level, GLvoid *img);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLACTIVETEXTUREPROC) (GLenum texture);
+typedef void (APIENTRYP PFNGLSAMPLECOVERAGEPROC) (GLclampf value, GLboolean invert);
+typedef void (APIENTRYP PFNGLCOMPRESSEDTEXIMAGE3DPROC) (GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLsizei depth, GLint border, GLsizei imageSize, const GLvoid *data);
+typedef void (APIENTRYP PFNGLCOMPRESSEDTEXIMAGE2DPROC) (GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLint border, GLsizei imageSize, const GLvoid *data);
+typedef void (APIENTRYP PFNGLCOMPRESSEDTEXIMAGE1DPROC) (GLenum target, GLint level, GLenum internalformat, GLsizei width, GLint border, GLsizei imageSize, const GLvoid *data);
+typedef void (APIENTRYP PFNGLCOMPRESSEDTEXSUBIMAGE3DPROC) (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLsizei width, GLsizei height, GLsizei depth, GLenum format, GLsizei imageSize, const GLvoid *data);
+typedef void (APIENTRYP PFNGLCOMPRESSEDTEXSUBIMAGE2DPROC) (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLsizei width, GLsizei height, GLenum format, GLsizei imageSize, const GLvoid *data);
+typedef void (APIENTRYP PFNGLCOMPRESSEDTEXSUBIMAGE1DPROC) (GLenum target, GLint level, GLint xoffset, GLsizei width, GLenum format, GLsizei imageSize, const GLvoid *data);
+typedef void (APIENTRYP PFNGLGETCOMPRESSEDTEXIMAGEPROC) (GLenum target, GLint level, GLvoid *img);
+#endif
+
+#ifndef GL_VERSION_1_3_DEPRECATED
+#define GL_VERSION_1_3_DEPRECATED 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glClientActiveTexture (GLenum texture);
+GLAPI void APIENTRY glMultiTexCoord1d (GLenum target, GLdouble s);
+GLAPI void APIENTRY glMultiTexCoord1dv (GLenum target, const GLdouble *v);
+GLAPI void APIENTRY glMultiTexCoord1f (GLenum target, GLfloat s);
+GLAPI void APIENTRY glMultiTexCoord1fv (GLenum target, const GLfloat *v);
+GLAPI void APIENTRY glMultiTexCoord1i (GLenum target, GLint s);
+GLAPI void APIENTRY glMultiTexCoord1iv (GLenum target, const GLint *v);
+GLAPI void APIENTRY glMultiTexCoord1s (GLenum target, GLshort s);
+GLAPI void APIENTRY glMultiTexCoord1sv (GLenum target, const GLshort *v);
+GLAPI void APIENTRY glMultiTexCoord2d (GLenum target, GLdouble s, GLdouble t);
+GLAPI void APIENTRY glMultiTexCoord2dv (GLenum target, const GLdouble *v);
+GLAPI void APIENTRY glMultiTexCoord2f (GLenum target, GLfloat s, GLfloat t);
+GLAPI void APIENTRY glMultiTexCoord2fv (GLenum target, const GLfloat *v);
+GLAPI void APIENTRY glMultiTexCoord2i (GLenum target, GLint s, GLint t);
+GLAPI void APIENTRY glMultiTexCoord2iv (GLenum target, const GLint *v);
+GLAPI void APIENTRY glMultiTexCoord2s (GLenum target, GLshort s, GLshort t);
+GLAPI void APIENTRY glMultiTexCoord2sv (GLenum target, const GLshort *v);
+GLAPI void APIENTRY glMultiTexCoord3d (GLenum target, GLdouble s, GLdouble t, GLdouble r);
+GLAPI void APIENTRY glMultiTexCoord3dv (GLenum target, const GLdouble *v);
+GLAPI void APIENTRY glMultiTexCoord3f (GLenum target, GLfloat s, GLfloat t, GLfloat r);
+GLAPI void APIENTRY glMultiTexCoord3fv (GLenum target, const GLfloat *v);
+GLAPI void APIENTRY glMultiTexCoord3i (GLenum target, GLint s, GLint t, GLint r);
+GLAPI void APIENTRY glMultiTexCoord3iv (GLenum target, const GLint *v);
+GLAPI void APIENTRY glMultiTexCoord3s (GLenum target, GLshort s, GLshort t, GLshort r);
+GLAPI void APIENTRY glMultiTexCoord3sv (GLenum target, const GLshort *v);
+GLAPI void APIENTRY glMultiTexCoord4d (GLenum target, GLdouble s, GLdouble t, GLdouble r, GLdouble q);
+GLAPI void APIENTRY glMultiTexCoord4dv (GLenum target, const GLdouble *v);
+GLAPI void APIENTRY glMultiTexCoord4f (GLenum target, GLfloat s, GLfloat t, GLfloat r, GLfloat q);
+GLAPI void APIENTRY glMultiTexCoord4fv (GLenum target, const GLfloat *v);
+GLAPI void APIENTRY glMultiTexCoord4i (GLenum target, GLint s, GLint t, GLint r, GLint q);
+GLAPI void APIENTRY glMultiTexCoord4iv (GLenum target, const GLint *v);
+GLAPI void APIENTRY glMultiTexCoord4s (GLenum target, GLshort s, GLshort t, GLshort r, GLshort q);
+GLAPI void APIENTRY glMultiTexCoord4sv (GLenum target, const GLshort *v);
+GLAPI void APIENTRY glLoadTransposeMatrixf (const GLfloat *m);
+GLAPI void APIENTRY glLoadTransposeMatrixd (const GLdouble *m);
+GLAPI void APIENTRY glMultTransposeMatrixf (const GLfloat *m);
+GLAPI void APIENTRY glMultTransposeMatrixd (const GLdouble *m);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCLIENTACTIVETEXTUREPROC) (GLenum texture);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD1DPROC) (GLenum target, GLdouble s);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD1DVPROC) (GLenum target, const GLdouble *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD1FPROC) (GLenum target, GLfloat s);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD1FVPROC) (GLenum target, const GLfloat *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD1IPROC) (GLenum target, GLint s);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD1IVPROC) (GLenum target, const GLint *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD1SPROC) (GLenum target, GLshort s);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD1SVPROC) (GLenum target, const GLshort *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD2DPROC) (GLenum target, GLdouble s, GLdouble t);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD2DVPROC) (GLenum target, const GLdouble *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD2FPROC) (GLenum target, GLfloat s, GLfloat t);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD2FVPROC) (GLenum target, const GLfloat *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD2IPROC) (GLenum target, GLint s, GLint t);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD2IVPROC) (GLenum target, const GLint *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD2SPROC) (GLenum target, GLshort s, GLshort t);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD2SVPROC) (GLenum target, const GLshort *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD3DPROC) (GLenum target, GLdouble s, GLdouble t, GLdouble r);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD3DVPROC) (GLenum target, const GLdouble *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD3FPROC) (GLenum target, GLfloat s, GLfloat t, GLfloat r);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD3FVPROC) (GLenum target, const GLfloat *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD3IPROC) (GLenum target, GLint s, GLint t, GLint r);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD3IVPROC) (GLenum target, const GLint *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD3SPROC) (GLenum target, GLshort s, GLshort t, GLshort r);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD3SVPROC) (GLenum target, const GLshort *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD4DPROC) (GLenum target, GLdouble s, GLdouble t, GLdouble r, GLdouble q);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD4DVPROC) (GLenum target, const GLdouble *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD4FPROC) (GLenum target, GLfloat s, GLfloat t, GLfloat r, GLfloat q);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD4FVPROC) (GLenum target, const GLfloat *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD4IPROC) (GLenum target, GLint s, GLint t, GLint r, GLint q);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD4IVPROC) (GLenum target, const GLint *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD4SPROC) (GLenum target, GLshort s, GLshort t, GLshort r, GLshort q);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD4SVPROC) (GLenum target, const GLshort *v);
+typedef void (APIENTRYP PFNGLLOADTRANSPOSEMATRIXFPROC) (const GLfloat *m);
+typedef void (APIENTRYP PFNGLLOADTRANSPOSEMATRIXDPROC) (const GLdouble *m);
+typedef void (APIENTRYP PFNGLMULTTRANSPOSEMATRIXFPROC) (const GLfloat *m);
+typedef void (APIENTRYP PFNGLMULTTRANSPOSEMATRIXDPROC) (const GLdouble *m);
+#endif
+
+#ifndef GL_VERSION_1_4
+#define GL_VERSION_1_4 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBlendFuncSeparate (GLenum sfactorRGB, GLenum dfactorRGB, GLenum sfactorAlpha, GLenum dfactorAlpha);
+GLAPI void APIENTRY glMultiDrawArrays (GLenum mode, const GLint *first, const GLsizei *count, GLsizei primcount);
+GLAPI void APIENTRY glMultiDrawElements (GLenum mode, const GLsizei *count, GLenum type, const GLvoid* *indices, GLsizei primcount);
+GLAPI void APIENTRY glPointParameterf (GLenum pname, GLfloat param);
+GLAPI void APIENTRY glPointParameterfv (GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glPointParameteri (GLenum pname, GLint param);
+GLAPI void APIENTRY glPointParameteriv (GLenum pname, const GLint *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBLENDFUNCSEPARATEPROC) (GLenum sfactorRGB, GLenum dfactorRGB, GLenum sfactorAlpha, GLenum dfactorAlpha);
+typedef void (APIENTRYP PFNGLMULTIDRAWARRAYSPROC) (GLenum mode, const GLint *first, const GLsizei *count, GLsizei primcount);
+typedef void (APIENTRYP PFNGLMULTIDRAWELEMENTSPROC) (GLenum mode, const GLsizei *count, GLenum type, const GLvoid* *indices, GLsizei primcount);
+typedef void (APIENTRYP PFNGLPOINTPARAMETERFPROC) (GLenum pname, GLfloat param);
+typedef void (APIENTRYP PFNGLPOINTPARAMETERFVPROC) (GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLPOINTPARAMETERIPROC) (GLenum pname, GLint param);
+typedef void (APIENTRYP PFNGLPOINTPARAMETERIVPROC) (GLenum pname, const GLint *params);
+#endif
+
+#ifndef GL_VERSION_1_4_DEPRECATED
+#define GL_VERSION_1_4_DEPRECATED 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glFogCoordf (GLfloat coord);
+GLAPI void APIENTRY glFogCoordfv (const GLfloat *coord);
+GLAPI void APIENTRY glFogCoordd (GLdouble coord);
+GLAPI void APIENTRY glFogCoorddv (const GLdouble *coord);
+GLAPI void APIENTRY glFogCoordPointer (GLenum type, GLsizei stride, const GLvoid *pointer);
+GLAPI void APIENTRY glSecondaryColor3b (GLbyte red, GLbyte green, GLbyte blue);
+GLAPI void APIENTRY glSecondaryColor3bv (const GLbyte *v);
+GLAPI void APIENTRY glSecondaryColor3d (GLdouble red, GLdouble green, GLdouble blue);
+GLAPI void APIENTRY glSecondaryColor3dv (const GLdouble *v);
+GLAPI void APIENTRY glSecondaryColor3f (GLfloat red, GLfloat green, GLfloat blue);
+GLAPI void APIENTRY glSecondaryColor3fv (const GLfloat *v);
+GLAPI void APIENTRY glSecondaryColor3i (GLint red, GLint green, GLint blue);
+GLAPI void APIENTRY glSecondaryColor3iv (const GLint *v);
+GLAPI void APIENTRY glSecondaryColor3s (GLshort red, GLshort green, GLshort blue);
+GLAPI void APIENTRY glSecondaryColor3sv (const GLshort *v);
+GLAPI void APIENTRY glSecondaryColor3ub (GLubyte red, GLubyte green, GLubyte blue);
+GLAPI void APIENTRY glSecondaryColor3ubv (const GLubyte *v);
+GLAPI void APIENTRY glSecondaryColor3ui (GLuint red, GLuint green, GLuint blue);
+GLAPI void APIENTRY glSecondaryColor3uiv (const GLuint *v);
+GLAPI void APIENTRY glSecondaryColor3us (GLushort red, GLushort green, GLushort blue);
+GLAPI void APIENTRY glSecondaryColor3usv (const GLushort *v);
+GLAPI void APIENTRY glSecondaryColorPointer (GLint size, GLenum type, GLsizei stride, const GLvoid *pointer);
+GLAPI void APIENTRY glWindowPos2d (GLdouble x, GLdouble y);
+GLAPI void APIENTRY glWindowPos2dv (const GLdouble *v);
+GLAPI void APIENTRY glWindowPos2f (GLfloat x, GLfloat y);
+GLAPI void APIENTRY glWindowPos2fv (const GLfloat *v);
+GLAPI void APIENTRY glWindowPos2i (GLint x, GLint y);
+GLAPI void APIENTRY glWindowPos2iv (const GLint *v);
+GLAPI void APIENTRY glWindowPos2s (GLshort x, GLshort y);
+GLAPI void APIENTRY glWindowPos2sv (const GLshort *v);
+GLAPI void APIENTRY glWindowPos3d (GLdouble x, GLdouble y, GLdouble z);
+GLAPI void APIENTRY glWindowPos3dv (const GLdouble *v);
+GLAPI void APIENTRY glWindowPos3f (GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glWindowPos3fv (const GLfloat *v);
+GLAPI void APIENTRY glWindowPos3i (GLint x, GLint y, GLint z);
+GLAPI void APIENTRY glWindowPos3iv (const GLint *v);
+GLAPI void APIENTRY glWindowPos3s (GLshort x, GLshort y, GLshort z);
+GLAPI void APIENTRY glWindowPos3sv (const GLshort *v);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLFOGCOORDFPROC) (GLfloat coord);
+typedef void (APIENTRYP PFNGLFOGCOORDFVPROC) (const GLfloat *coord);
+typedef void (APIENTRYP PFNGLFOGCOORDDPROC) (GLdouble coord);
+typedef void (APIENTRYP PFNGLFOGCOORDDVPROC) (const GLdouble *coord);
+typedef void (APIENTRYP PFNGLFOGCOORDPOINTERPROC) (GLenum type, GLsizei stride, const GLvoid *pointer);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3BPROC) (GLbyte red, GLbyte green, GLbyte blue);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3BVPROC) (const GLbyte *v);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3DPROC) (GLdouble red, GLdouble green, GLdouble blue);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3DVPROC) (const GLdouble *v);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3FPROC) (GLfloat red, GLfloat green, GLfloat blue);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3FVPROC) (const GLfloat *v);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3IPROC) (GLint red, GLint green, GLint blue);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3IVPROC) (const GLint *v);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3SPROC) (GLshort red, GLshort green, GLshort blue);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3SVPROC) (const GLshort *v);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3UBPROC) (GLubyte red, GLubyte green, GLubyte blue);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3UBVPROC) (const GLubyte *v);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3UIPROC) (GLuint red, GLuint green, GLuint blue);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3UIVPROC) (const GLuint *v);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3USPROC) (GLushort red, GLushort green, GLushort blue);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3USVPROC) (const GLushort *v);
+typedef void (APIENTRYP PFNGLSECONDARYCOLORPOINTERPROC) (GLint size, GLenum type, GLsizei stride, const GLvoid *pointer);
+typedef void (APIENTRYP PFNGLWINDOWPOS2DPROC) (GLdouble x, GLdouble y);
+typedef void (APIENTRYP PFNGLWINDOWPOS2DVPROC) (const GLdouble *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS2FPROC) (GLfloat x, GLfloat y);
+typedef void (APIENTRYP PFNGLWINDOWPOS2FVPROC) (const GLfloat *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS2IPROC) (GLint x, GLint y);
+typedef void (APIENTRYP PFNGLWINDOWPOS2IVPROC) (const GLint *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS2SPROC) (GLshort x, GLshort y);
+typedef void (APIENTRYP PFNGLWINDOWPOS2SVPROC) (const GLshort *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS3DPROC) (GLdouble x, GLdouble y, GLdouble z);
+typedef void (APIENTRYP PFNGLWINDOWPOS3DVPROC) (const GLdouble *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS3FPROC) (GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLWINDOWPOS3FVPROC) (const GLfloat *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS3IPROC) (GLint x, GLint y, GLint z);
+typedef void (APIENTRYP PFNGLWINDOWPOS3IVPROC) (const GLint *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS3SPROC) (GLshort x, GLshort y, GLshort z);
+typedef void (APIENTRYP PFNGLWINDOWPOS3SVPROC) (const GLshort *v);
+#endif
+
+#ifndef GL_VERSION_1_5
+#define GL_VERSION_1_5 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glGenQueries (GLsizei n, GLuint *ids);
+GLAPI void APIENTRY glDeleteQueries (GLsizei n, const GLuint *ids);
+GLAPI GLboolean APIENTRY glIsQuery (GLuint id);
+GLAPI void APIENTRY glBeginQuery (GLenum target, GLuint id);
+GLAPI void APIENTRY glEndQuery (GLenum target);
+GLAPI void APIENTRY glGetQueryiv (GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetQueryObjectiv (GLuint id, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetQueryObjectuiv (GLuint id, GLenum pname, GLuint *params);
+GLAPI void APIENTRY glBindBuffer (GLenum target, GLuint buffer);
+GLAPI void APIENTRY glDeleteBuffers (GLsizei n, const GLuint *buffers);
+GLAPI void APIENTRY glGenBuffers (GLsizei n, GLuint *buffers);
+GLAPI GLboolean APIENTRY glIsBuffer (GLuint buffer);
+GLAPI void APIENTRY glBufferData (GLenum target, GLsizeiptr size, const GLvoid *data, GLenum usage);
+GLAPI void APIENTRY glBufferSubData (GLenum target, GLintptr offset, GLsizeiptr size, const GLvoid *data);
+GLAPI void APIENTRY glGetBufferSubData (GLenum target, GLintptr offset, GLsizeiptr size, GLvoid *data);
+GLAPI GLvoid* APIENTRY glMapBuffer (GLenum target, GLenum access);
+GLAPI GLboolean APIENTRY glUnmapBuffer (GLenum target);
+GLAPI void APIENTRY glGetBufferParameteriv (GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetBufferPointerv (GLenum target, GLenum pname, GLvoid* *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLGENQUERIESPROC) (GLsizei n, GLuint *ids);
+typedef void (APIENTRYP PFNGLDELETEQUERIESPROC) (GLsizei n, const GLuint *ids);
+typedef GLboolean (APIENTRYP PFNGLISQUERYPROC) (GLuint id);
+typedef void (APIENTRYP PFNGLBEGINQUERYPROC) (GLenum target, GLuint id);
+typedef void (APIENTRYP PFNGLENDQUERYPROC) (GLenum target);
+typedef void (APIENTRYP PFNGLGETQUERYIVPROC) (GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETQUERYOBJECTIVPROC) (GLuint id, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETQUERYOBJECTUIVPROC) (GLuint id, GLenum pname, GLuint *params);
+typedef void (APIENTRYP PFNGLBINDBUFFERPROC) (GLenum target, GLuint buffer);
+typedef void (APIENTRYP PFNGLDELETEBUFFERSPROC) (GLsizei n, const GLuint *buffers);
+typedef void (APIENTRYP PFNGLGENBUFFERSPROC) (GLsizei n, GLuint *buffers);
+typedef GLboolean (APIENTRYP PFNGLISBUFFERPROC) (GLuint buffer);
+typedef void (APIENTRYP PFNGLBUFFERDATAPROC) (GLenum target, GLsizeiptr size, const GLvoid *data, GLenum usage);
+typedef void (APIENTRYP PFNGLBUFFERSUBDATAPROC) (GLenum target, GLintptr offset, GLsizeiptr size, const GLvoid *data);
+typedef void (APIENTRYP PFNGLGETBUFFERSUBDATAPROC) (GLenum target, GLintptr offset, GLsizeiptr size, GLvoid *data);
+typedef GLvoid* (APIENTRYP PFNGLMAPBUFFERPROC) (GLenum target, GLenum access);
+typedef GLboolean (APIENTRYP PFNGLUNMAPBUFFERPROC) (GLenum target);
+typedef void (APIENTRYP PFNGLGETBUFFERPARAMETERIVPROC) (GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETBUFFERPOINTERVPROC) (GLenum target, GLenum pname, GLvoid* *params);
+#endif
+
+#ifndef GL_VERSION_2_0
+#define GL_VERSION_2_0 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBlendEquationSeparate (GLenum modeRGB, GLenum modeAlpha);
+GLAPI void APIENTRY glDrawBuffers (GLsizei n, const GLenum *bufs);
+GLAPI void APIENTRY glStencilOpSeparate (GLenum face, GLenum sfail, GLenum dpfail, GLenum dppass);
+GLAPI void APIENTRY glStencilFuncSeparate (GLenum face, GLenum func, GLint ref, GLuint mask);
+GLAPI void APIENTRY glStencilMaskSeparate (GLenum face, GLuint mask);
+GLAPI void APIENTRY glAttachShader (GLuint program, GLuint shader);
+GLAPI void APIENTRY glBindAttribLocation (GLuint program, GLuint index, const GLchar *name);
+GLAPI void APIENTRY glCompileShader (GLuint shader);
+GLAPI GLuint APIENTRY glCreateProgram (void);
+GLAPI GLuint APIENTRY glCreateShader (GLenum type);
+GLAPI void APIENTRY glDeleteProgram (GLuint program);
+GLAPI void APIENTRY glDeleteShader (GLuint shader);
+GLAPI void APIENTRY glDetachShader (GLuint program, GLuint shader);
+GLAPI void APIENTRY glDisableVertexAttribArray (GLuint index);
+GLAPI void APIENTRY glEnableVertexAttribArray (GLuint index);
+GLAPI void APIENTRY glGetActiveAttrib (GLuint program, GLuint index, GLsizei bufSize, GLsizei *length, GLint *size, GLenum *type, GLchar *name);
+GLAPI void APIENTRY glGetActiveUniform (GLuint program, GLuint index, GLsizei bufSize, GLsizei *length, GLint *size, GLenum *type, GLchar *name);
+GLAPI void APIENTRY glGetAttachedShaders (GLuint program, GLsizei maxCount, GLsizei *count, GLuint *obj);
+GLAPI GLint APIENTRY glGetAttribLocation (GLuint program, const GLchar *name);
+GLAPI void APIENTRY glGetProgramiv (GLuint program, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetProgramInfoLog (GLuint program, GLsizei bufSize, GLsizei *length, GLchar *infoLog);
+GLAPI void APIENTRY glGetShaderiv (GLuint shader, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetShaderInfoLog (GLuint shader, GLsizei bufSize, GLsizei *length, GLchar *infoLog);
+GLAPI void APIENTRY glGetShaderSource (GLuint shader, GLsizei bufSize, GLsizei *length, GLchar *source);
+GLAPI GLint APIENTRY glGetUniformLocation (GLuint program, const GLchar *name);
+GLAPI void APIENTRY glGetUniformfv (GLuint program, GLint location, GLfloat *params);
+GLAPI void APIENTRY glGetUniformiv (GLuint program, GLint location, GLint *params);
+GLAPI void APIENTRY glGetVertexAttribdv (GLuint index, GLenum pname, GLdouble *params);
+GLAPI void APIENTRY glGetVertexAttribfv (GLuint index, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetVertexAttribiv (GLuint index, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetVertexAttribPointerv (GLuint index, GLenum pname, GLvoid* *pointer);
+GLAPI GLboolean APIENTRY glIsProgram (GLuint program);
+GLAPI GLboolean APIENTRY glIsShader (GLuint shader);
+GLAPI void APIENTRY glLinkProgram (GLuint program);
+GLAPI void APIENTRY glShaderSource (GLuint shader, GLsizei count, const GLchar* *string, const GLint *length);
+GLAPI void APIENTRY glUseProgram (GLuint program);
+GLAPI void APIENTRY glUniform1f (GLint location, GLfloat v0);
+GLAPI void APIENTRY glUniform2f (GLint location, GLfloat v0, GLfloat v1);
+GLAPI void APIENTRY glUniform3f (GLint location, GLfloat v0, GLfloat v1, GLfloat v2);
+GLAPI void APIENTRY glUniform4f (GLint location, GLfloat v0, GLfloat v1, GLfloat v2, GLfloat v3);
+GLAPI void APIENTRY glUniform1i (GLint location, GLint v0);
+GLAPI void APIENTRY glUniform2i (GLint location, GLint v0, GLint v1);
+GLAPI void APIENTRY glUniform3i (GLint location, GLint v0, GLint v1, GLint v2);
+GLAPI void APIENTRY glUniform4i (GLint location, GLint v0, GLint v1, GLint v2, GLint v3);
+GLAPI void APIENTRY glUniform1fv (GLint location, GLsizei count, const GLfloat *value);
+GLAPI void APIENTRY glUniform2fv (GLint location, GLsizei count, const GLfloat *value);
+GLAPI void APIENTRY glUniform3fv (GLint location, GLsizei count, const GLfloat *value);
+GLAPI void APIENTRY glUniform4fv (GLint location, GLsizei count, const GLfloat *value);
+GLAPI void APIENTRY glUniform1iv (GLint location, GLsizei count, const GLint *value);
+GLAPI void APIENTRY glUniform2iv (GLint location, GLsizei count, const GLint *value);
+GLAPI void APIENTRY glUniform3iv (GLint location, GLsizei count, const GLint *value);
+GLAPI void APIENTRY glUniform4iv (GLint location, GLsizei count, const GLint *value);
+GLAPI void APIENTRY glUniformMatrix2fv (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glUniformMatrix3fv (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glUniformMatrix4fv (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glValidateProgram (GLuint program);
+GLAPI void APIENTRY glVertexAttrib1d (GLuint index, GLdouble x);
+GLAPI void APIENTRY glVertexAttrib1dv (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttrib1f (GLuint index, GLfloat x);
+GLAPI void APIENTRY glVertexAttrib1fv (GLuint index, const GLfloat *v);
+GLAPI void APIENTRY glVertexAttrib1s (GLuint index, GLshort x);
+GLAPI void APIENTRY glVertexAttrib1sv (GLuint index, const GLshort *v);
+GLAPI void APIENTRY glVertexAttrib2d (GLuint index, GLdouble x, GLdouble y);
+GLAPI void APIENTRY glVertexAttrib2dv (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttrib2f (GLuint index, GLfloat x, GLfloat y);
+GLAPI void APIENTRY glVertexAttrib2fv (GLuint index, const GLfloat *v);
+GLAPI void APIENTRY glVertexAttrib2s (GLuint index, GLshort x, GLshort y);
+GLAPI void APIENTRY glVertexAttrib2sv (GLuint index, const GLshort *v);
+GLAPI void APIENTRY glVertexAttrib3d (GLuint index, GLdouble x, GLdouble y, GLdouble z);
+GLAPI void APIENTRY glVertexAttrib3dv (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttrib3f (GLuint index, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glVertexAttrib3fv (GLuint index, const GLfloat *v);
+GLAPI void APIENTRY glVertexAttrib3s (GLuint index, GLshort x, GLshort y, GLshort z);
+GLAPI void APIENTRY glVertexAttrib3sv (GLuint index, const GLshort *v);
+GLAPI void APIENTRY glVertexAttrib4Nbv (GLuint index, const GLbyte *v);
+GLAPI void APIENTRY glVertexAttrib4Niv (GLuint index, const GLint *v);
+GLAPI void APIENTRY glVertexAttrib4Nsv (GLuint index, const GLshort *v);
+GLAPI void APIENTRY glVertexAttrib4Nub (GLuint index, GLubyte x, GLubyte y, GLubyte z, GLubyte w);
+GLAPI void APIENTRY glVertexAttrib4Nubv (GLuint index, const GLubyte *v);
+GLAPI void APIENTRY glVertexAttrib4Nuiv (GLuint index, const GLuint *v);
+GLAPI void APIENTRY glVertexAttrib4Nusv (GLuint index, const GLushort *v);
+GLAPI void APIENTRY glVertexAttrib4bv (GLuint index, const GLbyte *v);
+GLAPI void APIENTRY glVertexAttrib4d (GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+GLAPI void APIENTRY glVertexAttrib4dv (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttrib4f (GLuint index, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+GLAPI void APIENTRY glVertexAttrib4fv (GLuint index, const GLfloat *v);
+GLAPI void APIENTRY glVertexAttrib4iv (GLuint index, const GLint *v);
+GLAPI void APIENTRY glVertexAttrib4s (GLuint index, GLshort x, GLshort y, GLshort z, GLshort w);
+GLAPI void APIENTRY glVertexAttrib4sv (GLuint index, const GLshort *v);
+GLAPI void APIENTRY glVertexAttrib4ubv (GLuint index, const GLubyte *v);
+GLAPI void APIENTRY glVertexAttrib4uiv (GLuint index, const GLuint *v);
+GLAPI void APIENTRY glVertexAttrib4usv (GLuint index, const GLushort *v);
+GLAPI void APIENTRY glVertexAttribPointer (GLuint index, GLint size, GLenum type, GLboolean normalized, GLsizei stride, const GLvoid *pointer);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBLENDEQUATIONSEPARATEPROC) (GLenum modeRGB, GLenum modeAlpha);
+typedef void (APIENTRYP PFNGLDRAWBUFFERSPROC) (GLsizei n, const GLenum *bufs);
+typedef void (APIENTRYP PFNGLSTENCILOPSEPARATEPROC) (GLenum face, GLenum sfail, GLenum dpfail, GLenum dppass);
+typedef void (APIENTRYP PFNGLSTENCILFUNCSEPARATEPROC) (GLenum face, GLenum func, GLint ref, GLuint mask);
+typedef void (APIENTRYP PFNGLSTENCILMASKSEPARATEPROC) (GLenum face, GLuint mask);
+typedef void (APIENTRYP PFNGLATTACHSHADERPROC) (GLuint program, GLuint shader);
+typedef void (APIENTRYP PFNGLBINDATTRIBLOCATIONPROC) (GLuint program, GLuint index, const GLchar *name);
+typedef void (APIENTRYP PFNGLCOMPILESHADERPROC) (GLuint shader);
+typedef GLuint (APIENTRYP PFNGLCREATEPROGRAMPROC) (void);
+typedef GLuint (APIENTRYP PFNGLCREATESHADERPROC) (GLenum type);
+typedef void (APIENTRYP PFNGLDELETEPROGRAMPROC) (GLuint program);
+typedef void (APIENTRYP PFNGLDELETESHADERPROC) (GLuint shader);
+typedef void (APIENTRYP PFNGLDETACHSHADERPROC) (GLuint program, GLuint shader);
+typedef void (APIENTRYP PFNGLDISABLEVERTEXATTRIBARRAYPROC) (GLuint index);
+typedef void (APIENTRYP PFNGLENABLEVERTEXATTRIBARRAYPROC) (GLuint index);
+typedef void (APIENTRYP PFNGLGETACTIVEATTRIBPROC) (GLuint program, GLuint index, GLsizei bufSize, GLsizei *length, GLint *size, GLenum *type, GLchar *name);
+typedef void (APIENTRYP PFNGLGETACTIVEUNIFORMPROC) (GLuint program, GLuint index, GLsizei bufSize, GLsizei *length, GLint *size, GLenum *type, GLchar *name);
+typedef void (APIENTRYP PFNGLGETATTACHEDSHADERSPROC) (GLuint program, GLsizei maxCount, GLsizei *count, GLuint *obj);
+typedef GLint (APIENTRYP PFNGLGETATTRIBLOCATIONPROC) (GLuint program, const GLchar *name);
+typedef void (APIENTRYP PFNGLGETPROGRAMIVPROC) (GLuint program, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETPROGRAMINFOLOGPROC) (GLuint program, GLsizei bufSize, GLsizei *length, GLchar *infoLog);
+typedef void (APIENTRYP PFNGLGETSHADERIVPROC) (GLuint shader, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETSHADERINFOLOGPROC) (GLuint shader, GLsizei bufSize, GLsizei *length, GLchar *infoLog);
+typedef void (APIENTRYP PFNGLGETSHADERSOURCEPROC) (GLuint shader, GLsizei bufSize, GLsizei *length, GLchar *source);
+typedef GLint (APIENTRYP PFNGLGETUNIFORMLOCATIONPROC) (GLuint program, const GLchar *name);
+typedef void (APIENTRYP PFNGLGETUNIFORMFVPROC) (GLuint program, GLint location, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETUNIFORMIVPROC) (GLuint program, GLint location, GLint *params);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBDVPROC) (GLuint index, GLenum pname, GLdouble *params);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBFVPROC) (GLuint index, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBIVPROC) (GLuint index, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBPOINTERVPROC) (GLuint index, GLenum pname, GLvoid* *pointer);
+typedef GLboolean (APIENTRYP PFNGLISPROGRAMPROC) (GLuint program);
+typedef GLboolean (APIENTRYP PFNGLISSHADERPROC) (GLuint shader);
+typedef void (APIENTRYP PFNGLLINKPROGRAMPROC) (GLuint program);
+typedef void (APIENTRYP PFNGLSHADERSOURCEPROC) (GLuint shader, GLsizei count, const GLchar* *string, const GLint *length);
+typedef void (APIENTRYP PFNGLUSEPROGRAMPROC) (GLuint program);
+typedef void (APIENTRYP PFNGLUNIFORM1FPROC) (GLint location, GLfloat v0);
+typedef void (APIENTRYP PFNGLUNIFORM2FPROC) (GLint location, GLfloat v0, GLfloat v1);
+typedef void (APIENTRYP PFNGLUNIFORM3FPROC) (GLint location, GLfloat v0, GLfloat v1, GLfloat v2);
+typedef void (APIENTRYP PFNGLUNIFORM4FPROC) (GLint location, GLfloat v0, GLfloat v1, GLfloat v2, GLfloat v3);
+typedef void (APIENTRYP PFNGLUNIFORM1IPROC) (GLint location, GLint v0);
+typedef void (APIENTRYP PFNGLUNIFORM2IPROC) (GLint location, GLint v0, GLint v1);
+typedef void (APIENTRYP PFNGLUNIFORM3IPROC) (GLint location, GLint v0, GLint v1, GLint v2);
+typedef void (APIENTRYP PFNGLUNIFORM4IPROC) (GLint location, GLint v0, GLint v1, GLint v2, GLint v3);
+typedef void (APIENTRYP PFNGLUNIFORM1FVPROC) (GLint location, GLsizei count, const GLfloat *value);
+typedef void (APIENTRYP PFNGLUNIFORM2FVPROC) (GLint location, GLsizei count, const GLfloat *value);
+typedef void (APIENTRYP PFNGLUNIFORM3FVPROC) (GLint location, GLsizei count, const GLfloat *value);
+typedef void (APIENTRYP PFNGLUNIFORM4FVPROC) (GLint location, GLsizei count, const GLfloat *value);
+typedef void (APIENTRYP PFNGLUNIFORM1IVPROC) (GLint location, GLsizei count, const GLint *value);
+typedef void (APIENTRYP PFNGLUNIFORM2IVPROC) (GLint location, GLsizei count, const GLint *value);
+typedef void (APIENTRYP PFNGLUNIFORM3IVPROC) (GLint location, GLsizei count, const GLint *value);
+typedef void (APIENTRYP PFNGLUNIFORM4IVPROC) (GLint location, GLsizei count, const GLint *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX2FVPROC) (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX3FVPROC) (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX4FVPROC) (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLVALIDATEPROGRAMPROC) (GLuint program);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1DPROC) (GLuint index, GLdouble x);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1DVPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1FPROC) (GLuint index, GLfloat x);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1FVPROC) (GLuint index, const GLfloat *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1SPROC) (GLuint index, GLshort x);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1SVPROC) (GLuint index, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2DPROC) (GLuint index, GLdouble x, GLdouble y);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2DVPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2FPROC) (GLuint index, GLfloat x, GLfloat y);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2FVPROC) (GLuint index, const GLfloat *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2SPROC) (GLuint index, GLshort x, GLshort y);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2SVPROC) (GLuint index, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3DPROC) (GLuint index, GLdouble x, GLdouble y, GLdouble z);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3DVPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3FPROC) (GLuint index, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3FVPROC) (GLuint index, const GLfloat *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3SPROC) (GLuint index, GLshort x, GLshort y, GLshort z);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3SVPROC) (GLuint index, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4NBVPROC) (GLuint index, const GLbyte *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4NIVPROC) (GLuint index, const GLint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4NSVPROC) (GLuint index, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4NUBPROC) (GLuint index, GLubyte x, GLubyte y, GLubyte z, GLubyte w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4NUBVPROC) (GLuint index, const GLubyte *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4NUIVPROC) (GLuint index, const GLuint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4NUSVPROC) (GLuint index, const GLushort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4BVPROC) (GLuint index, const GLbyte *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4DPROC) (GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4DVPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4FPROC) (GLuint index, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4FVPROC) (GLuint index, const GLfloat *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4IVPROC) (GLuint index, const GLint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4SPROC) (GLuint index, GLshort x, GLshort y, GLshort z, GLshort w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4SVPROC) (GLuint index, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4UBVPROC) (GLuint index, const GLubyte *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4UIVPROC) (GLuint index, const GLuint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4USVPROC) (GLuint index, const GLushort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBPOINTERPROC) (GLuint index, GLint size, GLenum type, GLboolean normalized, GLsizei stride, const GLvoid *pointer);
+#endif
+
+#ifndef GL_VERSION_2_1
+#define GL_VERSION_2_1 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glUniformMatrix2x3fv (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glUniformMatrix3x2fv (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glUniformMatrix2x4fv (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glUniformMatrix4x2fv (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glUniformMatrix3x4fv (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glUniformMatrix4x3fv (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX2X3FVPROC) (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX3X2FVPROC) (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX2X4FVPROC) (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX4X2FVPROC) (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX3X4FVPROC) (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX4X3FVPROC) (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+#endif
+
+#ifndef GL_VERSION_3_0
+#define GL_VERSION_3_0 1
+/* OpenGL 3.0 also reuses entry points from these extensions: */
+/* ARB_framebuffer_object */
+/* ARB_map_buffer_range */
+/* ARB_vertex_array_object */
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glColorMaski (GLuint index, GLboolean r, GLboolean g, GLboolean b, GLboolean a);
+GLAPI void APIENTRY glGetBooleani_v (GLenum target, GLuint index, GLboolean *data);
+GLAPI void APIENTRY glGetIntegeri_v (GLenum target, GLuint index, GLint *data);
+GLAPI void APIENTRY glEnablei (GLenum target, GLuint index);
+GLAPI void APIENTRY glDisablei (GLenum target, GLuint index);
+GLAPI GLboolean APIENTRY glIsEnabledi (GLenum target, GLuint index);
+GLAPI void APIENTRY glBeginTransformFeedback (GLenum primitiveMode);
+GLAPI void APIENTRY glEndTransformFeedback (void);
+GLAPI void APIENTRY glBindBufferRange (GLenum target, GLuint index, GLuint buffer, GLintptr offset, GLsizeiptr size);
+GLAPI void APIENTRY glBindBufferBase (GLenum target, GLuint index, GLuint buffer);
+GLAPI void APIENTRY glTransformFeedbackVaryings (GLuint program, GLsizei count, const GLchar* *varyings, GLenum bufferMode);
+GLAPI void APIENTRY glGetTransformFeedbackVarying (GLuint program, GLuint index, GLsizei bufSize, GLsizei *length, GLsizei *size, GLenum *type, GLchar *name);
+GLAPI void APIENTRY glClampColor (GLenum target, GLenum clamp);
+GLAPI void APIENTRY glBeginConditionalRender (GLuint id, GLenum mode);
+GLAPI void APIENTRY glEndConditionalRender (void);
+GLAPI void APIENTRY glVertexAttribIPointer (GLuint index, GLint size, GLenum type, GLsizei stride, const GLvoid *pointer);
+GLAPI void APIENTRY glGetVertexAttribIiv (GLuint index, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetVertexAttribIuiv (GLuint index, GLenum pname, GLuint *params);
+GLAPI void APIENTRY glVertexAttribI1i (GLuint index, GLint x);
+GLAPI void APIENTRY glVertexAttribI2i (GLuint index, GLint x, GLint y);
+GLAPI void APIENTRY glVertexAttribI3i (GLuint index, GLint x, GLint y, GLint z);
+GLAPI void APIENTRY glVertexAttribI4i (GLuint index, GLint x, GLint y, GLint z, GLint w);
+GLAPI void APIENTRY glVertexAttribI1ui (GLuint index, GLuint x);
+GLAPI void APIENTRY glVertexAttribI2ui (GLuint index, GLuint x, GLuint y);
+GLAPI void APIENTRY glVertexAttribI3ui (GLuint index, GLuint x, GLuint y, GLuint z);
+GLAPI void APIENTRY glVertexAttribI4ui (GLuint index, GLuint x, GLuint y, GLuint z, GLuint w);
+GLAPI void APIENTRY glVertexAttribI1iv (GLuint index, const GLint *v);
+GLAPI void APIENTRY glVertexAttribI2iv (GLuint index, const GLint *v);
+GLAPI void APIENTRY glVertexAttribI3iv (GLuint index, const GLint *v);
+GLAPI void APIENTRY glVertexAttribI4iv (GLuint index, const GLint *v);
+GLAPI void APIENTRY glVertexAttribI1uiv (GLuint index, const GLuint *v);
+GLAPI void APIENTRY glVertexAttribI2uiv (GLuint index, const GLuint *v);
+GLAPI void APIENTRY glVertexAttribI3uiv (GLuint index, const GLuint *v);
+GLAPI void APIENTRY glVertexAttribI4uiv (GLuint index, const GLuint *v);
+GLAPI void APIENTRY glVertexAttribI4bv (GLuint index, const GLbyte *v);
+GLAPI void APIENTRY glVertexAttribI4sv (GLuint index, const GLshort *v);
+GLAPI void APIENTRY glVertexAttribI4ubv (GLuint index, const GLubyte *v);
+GLAPI void APIENTRY glVertexAttribI4usv (GLuint index, const GLushort *v);
+GLAPI void APIENTRY glGetUniformuiv (GLuint program, GLint location, GLuint *params);
+GLAPI void APIENTRY glBindFragDataLocation (GLuint program, GLuint color, const GLchar *name);
+GLAPI GLint APIENTRY glGetFragDataLocation (GLuint program, const GLchar *name);
+GLAPI void APIENTRY glUniform1ui (GLint location, GLuint v0);
+GLAPI void APIENTRY glUniform2ui (GLint location, GLuint v0, GLuint v1);
+GLAPI void APIENTRY glUniform3ui (GLint location, GLuint v0, GLuint v1, GLuint v2);
+GLAPI void APIENTRY glUniform4ui (GLint location, GLuint v0, GLuint v1, GLuint v2, GLuint v3);
+GLAPI void APIENTRY glUniform1uiv (GLint location, GLsizei count, const GLuint *value);
+GLAPI void APIENTRY glUniform2uiv (GLint location, GLsizei count, const GLuint *value);
+GLAPI void APIENTRY glUniform3uiv (GLint location, GLsizei count, const GLuint *value);
+GLAPI void APIENTRY glUniform4uiv (GLint location, GLsizei count, const GLuint *value);
+GLAPI void APIENTRY glTexParameterIiv (GLenum target, GLenum pname, const GLint *params);
+GLAPI void APIENTRY glTexParameterIuiv (GLenum target, GLenum pname, const GLuint *params);
+GLAPI void APIENTRY glGetTexParameterIiv (GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetTexParameterIuiv (GLenum target, GLenum pname, GLuint *params);
+GLAPI void APIENTRY glClearBufferiv (GLenum buffer, GLint drawbuffer, const GLint *value);
+GLAPI void APIENTRY glClearBufferuiv (GLenum buffer, GLint drawbuffer, const GLuint *value);
+GLAPI void APIENTRY glClearBufferfv (GLenum buffer, GLint drawbuffer, const GLfloat *value);
+GLAPI void APIENTRY glClearBufferfi (GLenum buffer, GLint drawbuffer, GLfloat depth, GLint stencil);
+GLAPI const GLubyte * APIENTRY glGetStringi (GLenum name, GLuint index);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCOLORMASKIPROC) (GLuint index, GLboolean r, GLboolean g, GLboolean b, GLboolean a);
+typedef void (APIENTRYP PFNGLGETBOOLEANI_VPROC) (GLenum target, GLuint index, GLboolean *data);
+typedef void (APIENTRYP PFNGLGETINTEGERI_VPROC) (GLenum target, GLuint index, GLint *data);
+typedef void (APIENTRYP PFNGLENABLEIPROC) (GLenum target, GLuint index);
+typedef void (APIENTRYP PFNGLDISABLEIPROC) (GLenum target, GLuint index);
+typedef GLboolean (APIENTRYP PFNGLISENABLEDIPROC) (GLenum target, GLuint index);
+typedef void (APIENTRYP PFNGLBEGINTRANSFORMFEEDBACKPROC) (GLenum primitiveMode);
+typedef void (APIENTRYP PFNGLENDTRANSFORMFEEDBACKPROC) (void);
+typedef void (APIENTRYP PFNGLBINDBUFFERRANGEPROC) (GLenum target, GLuint index, GLuint buffer, GLintptr offset, GLsizeiptr size);
+typedef void (APIENTRYP PFNGLBINDBUFFERBASEPROC) (GLenum target, GLuint index, GLuint buffer);
+typedef void (APIENTRYP PFNGLTRANSFORMFEEDBACKVARYINGSPROC) (GLuint program, GLsizei count, const GLchar* *varyings, GLenum bufferMode);
+typedef void (APIENTRYP PFNGLGETTRANSFORMFEEDBACKVARYINGPROC) (GLuint program, GLuint index, GLsizei bufSize, GLsizei *length, GLsizei *size, GLenum *type, GLchar *name);
+typedef void (APIENTRYP PFNGLCLAMPCOLORPROC) (GLenum target, GLenum clamp);
+typedef void (APIENTRYP PFNGLBEGINCONDITIONALRENDERPROC) (GLuint id, GLenum mode);
+typedef void (APIENTRYP PFNGLENDCONDITIONALRENDERPROC) (void);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBIPOINTERPROC) (GLuint index, GLint size, GLenum type, GLsizei stride, const GLvoid *pointer);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBIIVPROC) (GLuint index, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBIUIVPROC) (GLuint index, GLenum pname, GLuint *params);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI1IPROC) (GLuint index, GLint x);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI2IPROC) (GLuint index, GLint x, GLint y);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI3IPROC) (GLuint index, GLint x, GLint y, GLint z);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI4IPROC) (GLuint index, GLint x, GLint y, GLint z, GLint w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI1UIPROC) (GLuint index, GLuint x);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI2UIPROC) (GLuint index, GLuint x, GLuint y);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI3UIPROC) (GLuint index, GLuint x, GLuint y, GLuint z);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI4UIPROC) (GLuint index, GLuint x, GLuint y, GLuint z, GLuint w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI1IVPROC) (GLuint index, const GLint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI2IVPROC) (GLuint index, const GLint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI3IVPROC) (GLuint index, const GLint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI4IVPROC) (GLuint index, const GLint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI1UIVPROC) (GLuint index, const GLuint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI2UIVPROC) (GLuint index, const GLuint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI3UIVPROC) (GLuint index, const GLuint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI4UIVPROC) (GLuint index, const GLuint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI4BVPROC) (GLuint index, const GLbyte *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI4SVPROC) (GLuint index, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI4UBVPROC) (GLuint index, const GLubyte *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI4USVPROC) (GLuint index, const GLushort *v);
+typedef void (APIENTRYP PFNGLGETUNIFORMUIVPROC) (GLuint program, GLint location, GLuint *params);
+typedef void (APIENTRYP PFNGLBINDFRAGDATALOCATIONPROC) (GLuint program, GLuint color, const GLchar *name);
+typedef GLint (APIENTRYP PFNGLGETFRAGDATALOCATIONPROC) (GLuint program, const GLchar *name);
+typedef void (APIENTRYP PFNGLUNIFORM1UIPROC) (GLint location, GLuint v0);
+typedef void (APIENTRYP PFNGLUNIFORM2UIPROC) (GLint location, GLuint v0, GLuint v1);
+typedef void (APIENTRYP PFNGLUNIFORM3UIPROC) (GLint location, GLuint v0, GLuint v1, GLuint v2);
+typedef void (APIENTRYP PFNGLUNIFORM4UIPROC) (GLint location, GLuint v0, GLuint v1, GLuint v2, GLuint v3);
+typedef void (APIENTRYP PFNGLUNIFORM1UIVPROC) (GLint location, GLsizei count, const GLuint *value);
+typedef void (APIENTRYP PFNGLUNIFORM2UIVPROC) (GLint location, GLsizei count, const GLuint *value);
+typedef void (APIENTRYP PFNGLUNIFORM3UIVPROC) (GLint location, GLsizei count, const GLuint *value);
+typedef void (APIENTRYP PFNGLUNIFORM4UIVPROC) (GLint location, GLsizei count, const GLuint *value);
+typedef void (APIENTRYP PFNGLTEXPARAMETERIIVPROC) (GLenum target, GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLTEXPARAMETERIUIVPROC) (GLenum target, GLenum pname, const GLuint *params);
+typedef void (APIENTRYP PFNGLGETTEXPARAMETERIIVPROC) (GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETTEXPARAMETERIUIVPROC) (GLenum target, GLenum pname, GLuint *params);
+typedef void (APIENTRYP PFNGLCLEARBUFFERIVPROC) (GLenum buffer, GLint drawbuffer, const GLint *value);
+typedef void (APIENTRYP PFNGLCLEARBUFFERUIVPROC) (GLenum buffer, GLint drawbuffer, const GLuint *value);
+typedef void (APIENTRYP PFNGLCLEARBUFFERFVPROC) (GLenum buffer, GLint drawbuffer, const GLfloat *value);
+typedef void (APIENTRYP PFNGLCLEARBUFFERFIPROC) (GLenum buffer, GLint drawbuffer, GLfloat depth, GLint stencil);
+typedef const GLubyte * (APIENTRYP PFNGLGETSTRINGIPROC) (GLenum name, GLuint index);
+#endif
+
+#ifndef GL_VERSION_3_1
+#define GL_VERSION_3_1 1
+/* OpenGL 3.1 also reuses entry points from these extensions: */
+/* ARB_copy_buffer */
+/* ARB_uniform_buffer_object */
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glDrawArraysInstanced (GLenum mode, GLint first, GLsizei count, GLsizei primcount);
+GLAPI void APIENTRY glDrawElementsInstanced (GLenum mode, GLsizei count, GLenum type, const GLvoid *indices, GLsizei primcount);
+GLAPI void APIENTRY glTexBuffer (GLenum target, GLenum internalformat, GLuint buffer);
+GLAPI void APIENTRY glPrimitiveRestartIndex (GLuint index);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLDRAWARRAYSINSTANCEDPROC) (GLenum mode, GLint first, GLsizei count, GLsizei primcount);
+typedef void (APIENTRYP PFNGLDRAWELEMENTSINSTANCEDPROC) (GLenum mode, GLsizei count, GLenum type, const GLvoid *indices, GLsizei primcount);
+typedef void (APIENTRYP PFNGLTEXBUFFERPROC) (GLenum target, GLenum internalformat, GLuint buffer);
+typedef void (APIENTRYP PFNGLPRIMITIVERESTARTINDEXPROC) (GLuint index);
+#endif
+
+#ifndef GL_VERSION_3_2
+#define GL_VERSION_3_2 1
+/* OpenGL 3.2 also reuses entry points from these extensions: */
+/* ARB_draw_elements_base_vertex */
+/* ARB_provoking_vertex */
+/* ARB_sync */
+/* ARB_texture_multisample */
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glGetInteger64i_v (GLenum target, GLuint index, GLint64 *data);
+GLAPI void APIENTRY glGetBufferParameteri64v (GLenum target, GLenum pname, GLint64 *params);
+GLAPI void APIENTRY glFramebufferTexture (GLenum target, GLenum attachment, GLuint texture, GLint level);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLGETINTEGER64I_VPROC) (GLenum target, GLuint index, GLint64 *data);
+typedef void (APIENTRYP PFNGLGETBUFFERPARAMETERI64VPROC) (GLenum target, GLenum pname, GLint64 *params);
+typedef void (APIENTRYP PFNGLFRAMEBUFFERTEXTUREPROC) (GLenum target, GLenum attachment, GLuint texture, GLint level);
+#endif
+
+#ifndef GL_VERSION_3_3
+#define GL_VERSION_3_3 1
+/* OpenGL 3.3 also reuses entry points from these extensions: */
+/* ARB_blend_func_extended */
+/* ARB_sampler_objects */
+/* ARB_explicit_attrib_location, but it has none */
+/* ARB_occlusion_query2 (no entry points) */
+/* ARB_shader_bit_encoding (no entry points) */
+/* ARB_texture_rgb10_a2ui (no entry points) */
+/* ARB_texture_swizzle (no entry points) */
+/* ARB_timer_query */
+/* ARB_vertex_type_2_10_10_10_rev */
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glVertexAttribDivisor (GLuint index, GLuint divisor);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLVERTEXATTRIBDIVISORPROC) (GLuint index, GLuint divisor);
+#endif
+
+#ifndef GL_VERSION_4_0
+#define GL_VERSION_4_0 1
+/* OpenGL 4.0 also reuses entry points from these extensions: */
+/* ARB_texture_query_lod (no entry points) */
+/* ARB_draw_indirect */
+/* ARB_gpu_shader5 (no entry points) */
+/* ARB_gpu_shader_fp64 */
+/* ARB_shader_subroutine */
+/* ARB_tessellation_shader */
+/* ARB_texture_buffer_object_rgb32 (no entry points) */
+/* ARB_texture_cube_map_array (no entry points) */
+/* ARB_texture_gather (no entry points) */
+/* ARB_transform_feedback2 */
+/* ARB_transform_feedback3 */
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glMinSampleShading (GLclampf value);
+GLAPI void APIENTRY glBlendEquationi (GLuint buf, GLenum mode);
+GLAPI void APIENTRY glBlendEquationSeparatei (GLuint buf, GLenum modeRGB, GLenum modeAlpha);
+GLAPI void APIENTRY glBlendFunci (GLuint buf, GLenum src, GLenum dst);
+GLAPI void APIENTRY glBlendFuncSeparatei (GLuint buf, GLenum srcRGB, GLenum dstRGB, GLenum srcAlpha, GLenum dstAlpha);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLMINSAMPLESHADINGPROC) (GLclampf value);
+typedef void (APIENTRYP PFNGLBLENDEQUATIONIPROC) (GLuint buf, GLenum mode);
+typedef void (APIENTRYP PFNGLBLENDEQUATIONSEPARATEIPROC) (GLuint buf, GLenum modeRGB, GLenum modeAlpha);
+typedef void (APIENTRYP PFNGLBLENDFUNCIPROC) (GLuint buf, GLenum src, GLenum dst);
+typedef void (APIENTRYP PFNGLBLENDFUNCSEPARATEIPROC) (GLuint buf, GLenum srcRGB, GLenum dstRGB, GLenum srcAlpha, GLenum dstAlpha);
+#endif
+
+#ifndef GL_VERSION_4_1
+#define GL_VERSION_4_1 1
+/* OpenGL 4.1 also reuses entry points from these extensions: */
+/* ARB_ES2_compatibility */
+/* ARB_get_program_binary */
+/* ARB_separate_shader_objects */
+/* ARB_shader_precision (no entry points) */
+/* ARB_vertex_attrib_64bit */
+/* ARB_viewport_array */
+#endif
+
+#ifndef GL_ARB_multitexture
+#define GL_ARB_multitexture 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glActiveTextureARB (GLenum texture);
+GLAPI void APIENTRY glClientActiveTextureARB (GLenum texture);
+GLAPI void APIENTRY glMultiTexCoord1dARB (GLenum target, GLdouble s);
+GLAPI void APIENTRY glMultiTexCoord1dvARB (GLenum target, const GLdouble *v);
+GLAPI void APIENTRY glMultiTexCoord1fARB (GLenum target, GLfloat s);
+GLAPI void APIENTRY glMultiTexCoord1fvARB (GLenum target, const GLfloat *v);
+GLAPI void APIENTRY glMultiTexCoord1iARB (GLenum target, GLint s);
+GLAPI void APIENTRY glMultiTexCoord1ivARB (GLenum target, const GLint *v);
+GLAPI void APIENTRY glMultiTexCoord1sARB (GLenum target, GLshort s);
+GLAPI void APIENTRY glMultiTexCoord1svARB (GLenum target, const GLshort *v);
+GLAPI void APIENTRY glMultiTexCoord2dARB (GLenum target, GLdouble s, GLdouble t);
+GLAPI void APIENTRY glMultiTexCoord2dvARB (GLenum target, const GLdouble *v);
+GLAPI void APIENTRY glMultiTexCoord2fARB (GLenum target, GLfloat s, GLfloat t);
+GLAPI void APIENTRY glMultiTexCoord2fvARB (GLenum target, const GLfloat *v);
+GLAPI void APIENTRY glMultiTexCoord2iARB (GLenum target, GLint s, GLint t);
+GLAPI void APIENTRY glMultiTexCoord2ivARB (GLenum target, const GLint *v);
+GLAPI void APIENTRY glMultiTexCoord2sARB (GLenum target, GLshort s, GLshort t);
+GLAPI void APIENTRY glMultiTexCoord2svARB (GLenum target, const GLshort *v);
+GLAPI void APIENTRY glMultiTexCoord3dARB (GLenum target, GLdouble s, GLdouble t, GLdouble r);
+GLAPI void APIENTRY glMultiTexCoord3dvARB (GLenum target, const GLdouble *v);
+GLAPI void APIENTRY glMultiTexCoord3fARB (GLenum target, GLfloat s, GLfloat t, GLfloat r);
+GLAPI void APIENTRY glMultiTexCoord3fvARB (GLenum target, const GLfloat *v);
+GLAPI void APIENTRY glMultiTexCoord3iARB (GLenum target, GLint s, GLint t, GLint r);
+GLAPI void APIENTRY glMultiTexCoord3ivARB (GLenum target, const GLint *v);
+GLAPI void APIENTRY glMultiTexCoord3sARB (GLenum target, GLshort s, GLshort t, GLshort r);
+GLAPI void APIENTRY glMultiTexCoord3svARB (GLenum target, const GLshort *v);
+GLAPI void APIENTRY glMultiTexCoord4dARB (GLenum target, GLdouble s, GLdouble t, GLdouble r, GLdouble q);
+GLAPI void APIENTRY glMultiTexCoord4dvARB (GLenum target, const GLdouble *v);
+GLAPI void APIENTRY glMultiTexCoord4fARB (GLenum target, GLfloat s, GLfloat t, GLfloat r, GLfloat q);
+GLAPI void APIENTRY glMultiTexCoord4fvARB (GLenum target, const GLfloat *v);
+GLAPI void APIENTRY glMultiTexCoord4iARB (GLenum target, GLint s, GLint t, GLint r, GLint q);
+GLAPI void APIENTRY glMultiTexCoord4ivARB (GLenum target, const GLint *v);
+GLAPI void APIENTRY glMultiTexCoord4sARB (GLenum target, GLshort s, GLshort t, GLshort r, GLshort q);
+GLAPI void APIENTRY glMultiTexCoord4svARB (GLenum target, const GLshort *v);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLACTIVETEXTUREARBPROC) (GLenum texture);
+typedef void (APIENTRYP PFNGLCLIENTACTIVETEXTUREARBPROC) (GLenum texture);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD1DARBPROC) (GLenum target, GLdouble s);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD1DVARBPROC) (GLenum target, const GLdouble *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD1FARBPROC) (GLenum target, GLfloat s);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD1FVARBPROC) (GLenum target, const GLfloat *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD1IARBPROC) (GLenum target, GLint s);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD1IVARBPROC) (GLenum target, const GLint *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD1SARBPROC) (GLenum target, GLshort s);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD1SVARBPROC) (GLenum target, const GLshort *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD2DARBPROC) (GLenum target, GLdouble s, GLdouble t);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD2DVARBPROC) (GLenum target, const GLdouble *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD2FARBPROC) (GLenum target, GLfloat s, GLfloat t);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD2FVARBPROC) (GLenum target, const GLfloat *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD2IARBPROC) (GLenum target, GLint s, GLint t);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD2IVARBPROC) (GLenum target, const GLint *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD2SARBPROC) (GLenum target, GLshort s, GLshort t);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD2SVARBPROC) (GLenum target, const GLshort *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD3DARBPROC) (GLenum target, GLdouble s, GLdouble t, GLdouble r);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD3DVARBPROC) (GLenum target, const GLdouble *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD3FARBPROC) (GLenum target, GLfloat s, GLfloat t, GLfloat r);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD3FVARBPROC) (GLenum target, const GLfloat *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD3IARBPROC) (GLenum target, GLint s, GLint t, GLint r);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD3IVARBPROC) (GLenum target, const GLint *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD3SARBPROC) (GLenum target, GLshort s, GLshort t, GLshort r);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD3SVARBPROC) (GLenum target, const GLshort *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD4DARBPROC) (GLenum target, GLdouble s, GLdouble t, GLdouble r, GLdouble q);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD4DVARBPROC) (GLenum target, const GLdouble *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD4FARBPROC) (GLenum target, GLfloat s, GLfloat t, GLfloat r, GLfloat q);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD4FVARBPROC) (GLenum target, const GLfloat *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD4IARBPROC) (GLenum target, GLint s, GLint t, GLint r, GLint q);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD4IVARBPROC) (GLenum target, const GLint *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD4SARBPROC) (GLenum target, GLshort s, GLshort t, GLshort r, GLshort q);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD4SVARBPROC) (GLenum target, const GLshort *v);
+#endif
+
+#ifndef GL_ARB_transpose_matrix
+#define GL_ARB_transpose_matrix 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glLoadTransposeMatrixfARB (const GLfloat *m);
+GLAPI void APIENTRY glLoadTransposeMatrixdARB (const GLdouble *m);
+GLAPI void APIENTRY glMultTransposeMatrixfARB (const GLfloat *m);
+GLAPI void APIENTRY glMultTransposeMatrixdARB (const GLdouble *m);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLLOADTRANSPOSEMATRIXFARBPROC) (const GLfloat *m);
+typedef void (APIENTRYP PFNGLLOADTRANSPOSEMATRIXDARBPROC) (const GLdouble *m);
+typedef void (APIENTRYP PFNGLMULTTRANSPOSEMATRIXFARBPROC) (const GLfloat *m);
+typedef void (APIENTRYP PFNGLMULTTRANSPOSEMATRIXDARBPROC) (const GLdouble *m);
+#endif
+
+#ifndef GL_ARB_multisample
+#define GL_ARB_multisample 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glSampleCoverageARB (GLclampf value, GLboolean invert);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLSAMPLECOVERAGEARBPROC) (GLclampf value, GLboolean invert);
+#endif
+
+#ifndef GL_ARB_texture_env_add
+#define GL_ARB_texture_env_add 1
+#endif
+
+#ifndef GL_ARB_texture_cube_map
+#define GL_ARB_texture_cube_map 1
+#endif
+
+#ifndef GL_ARB_texture_compression
+#define GL_ARB_texture_compression 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glCompressedTexImage3DARB (GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLsizei depth, GLint border, GLsizei imageSize, const GLvoid *data);
+GLAPI void APIENTRY glCompressedTexImage2DARB (GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLint border, GLsizei imageSize, const GLvoid *data);
+GLAPI void APIENTRY glCompressedTexImage1DARB (GLenum target, GLint level, GLenum internalformat, GLsizei width, GLint border, GLsizei imageSize, const GLvoid *data);
+GLAPI void APIENTRY glCompressedTexSubImage3DARB (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLsizei width, GLsizei height, GLsizei depth, GLenum format, GLsizei imageSize, const GLvoid *data);
+GLAPI void APIENTRY glCompressedTexSubImage2DARB (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLsizei width, GLsizei height, GLenum format, GLsizei imageSize, const GLvoid *data);
+GLAPI void APIENTRY glCompressedTexSubImage1DARB (GLenum target, GLint level, GLint xoffset, GLsizei width, GLenum format, GLsizei imageSize, const GLvoid *data);
+GLAPI void APIENTRY glGetCompressedTexImageARB (GLenum target, GLint level, GLvoid *img);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCOMPRESSEDTEXIMAGE3DARBPROC) (GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLsizei depth, GLint border, GLsizei imageSize, const GLvoid *data);
+typedef void (APIENTRYP PFNGLCOMPRESSEDTEXIMAGE2DARBPROC) (GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLint border, GLsizei imageSize, const GLvoid *data);
+typedef void (APIENTRYP PFNGLCOMPRESSEDTEXIMAGE1DARBPROC) (GLenum target, GLint level, GLenum internalformat, GLsizei width, GLint border, GLsizei imageSize, const GLvoid *data);
+typedef void (APIENTRYP PFNGLCOMPRESSEDTEXSUBIMAGE3DARBPROC) (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLsizei width, GLsizei height, GLsizei depth, GLenum format, GLsizei imageSize, const GLvoid *data);
+typedef void (APIENTRYP PFNGLCOMPRESSEDTEXSUBIMAGE2DARBPROC) (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLsizei width, GLsizei height, GLenum format, GLsizei imageSize, const GLvoid *data);
+typedef void (APIENTRYP PFNGLCOMPRESSEDTEXSUBIMAGE1DARBPROC) (GLenum target, GLint level, GLint xoffset, GLsizei width, GLenum format, GLsizei imageSize, const GLvoid *data);
+typedef void (APIENTRYP PFNGLGETCOMPRESSEDTEXIMAGEARBPROC) (GLenum target, GLint level, GLvoid *img);
+#endif
+
+#ifndef GL_ARB_texture_border_clamp
+#define GL_ARB_texture_border_clamp 1
+#endif
+
+#ifndef GL_ARB_point_parameters
+#define GL_ARB_point_parameters 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glPointParameterfARB (GLenum pname, GLfloat param);
+GLAPI void APIENTRY glPointParameterfvARB (GLenum pname, const GLfloat *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPOINTPARAMETERFARBPROC) (GLenum pname, GLfloat param);
+typedef void (APIENTRYP PFNGLPOINTPARAMETERFVARBPROC) (GLenum pname, const GLfloat *params);
+#endif
+
+#ifndef GL_ARB_vertex_blend
+#define GL_ARB_vertex_blend 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glWeightbvARB (GLint size, const GLbyte *weights);
+GLAPI void APIENTRY glWeightsvARB (GLint size, const GLshort *weights);
+GLAPI void APIENTRY glWeightivARB (GLint size, const GLint *weights);
+GLAPI void APIENTRY glWeightfvARB (GLint size, const GLfloat *weights);
+GLAPI void APIENTRY glWeightdvARB (GLint size, const GLdouble *weights);
+GLAPI void APIENTRY glWeightubvARB (GLint size, const GLubyte *weights);
+GLAPI void APIENTRY glWeightusvARB (GLint size, const GLushort *weights);
+GLAPI void APIENTRY glWeightuivARB (GLint size, const GLuint *weights);
+GLAPI void APIENTRY glWeightPointerARB (GLint size, GLenum type, GLsizei stride, const GLvoid *pointer);
+GLAPI void APIENTRY glVertexBlendARB (GLint count);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLWEIGHTBVARBPROC) (GLint size, const GLbyte *weights);
+typedef void (APIENTRYP PFNGLWEIGHTSVARBPROC) (GLint size, const GLshort *weights);
+typedef void (APIENTRYP PFNGLWEIGHTIVARBPROC) (GLint size, const GLint *weights);
+typedef void (APIENTRYP PFNGLWEIGHTFVARBPROC) (GLint size, const GLfloat *weights);
+typedef void (APIENTRYP PFNGLWEIGHTDVARBPROC) (GLint size, const GLdouble *weights);
+typedef void (APIENTRYP PFNGLWEIGHTUBVARBPROC) (GLint size, const GLubyte *weights);
+typedef void (APIENTRYP PFNGLWEIGHTUSVARBPROC) (GLint size, const GLushort *weights);
+typedef void (APIENTRYP PFNGLWEIGHTUIVARBPROC) (GLint size, const GLuint *weights);
+typedef void (APIENTRYP PFNGLWEIGHTPOINTERARBPROC) (GLint size, GLenum type, GLsizei stride, const GLvoid *pointer);
+typedef void (APIENTRYP PFNGLVERTEXBLENDARBPROC) (GLint count);
+#endif
+
+#ifndef GL_ARB_matrix_palette
+#define GL_ARB_matrix_palette 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glCurrentPaletteMatrixARB (GLint index);
+GLAPI void APIENTRY glMatrixIndexubvARB (GLint size, const GLubyte *indices);
+GLAPI void APIENTRY glMatrixIndexusvARB (GLint size, const GLushort *indices);
+GLAPI void APIENTRY glMatrixIndexuivARB (GLint size, const GLuint *indices);
+GLAPI void APIENTRY glMatrixIndexPointerARB (GLint size, GLenum type, GLsizei stride, const GLvoid *pointer);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCURRENTPALETTEMATRIXARBPROC) (GLint index);
+typedef void (APIENTRYP PFNGLMATRIXINDEXUBVARBPROC) (GLint size, const GLubyte *indices);
+typedef void (APIENTRYP PFNGLMATRIXINDEXUSVARBPROC) (GLint size, const GLushort *indices);
+typedef void (APIENTRYP PFNGLMATRIXINDEXUIVARBPROC) (GLint size, const GLuint *indices);
+typedef void (APIENTRYP PFNGLMATRIXINDEXPOINTERARBPROC) (GLint size, GLenum type, GLsizei stride, const GLvoid *pointer);
+#endif
+
+#ifndef GL_ARB_texture_env_combine
+#define GL_ARB_texture_env_combine 1
+#endif
+
+#ifndef GL_ARB_texture_env_crossbar
+#define GL_ARB_texture_env_crossbar 1
+#endif
+
+#ifndef GL_ARB_texture_env_dot3
+#define GL_ARB_texture_env_dot3 1
+#endif
+
+#ifndef GL_ARB_texture_mirrored_repeat
+#define GL_ARB_texture_mirrored_repeat 1
+#endif
+
+#ifndef GL_ARB_depth_texture
+#define GL_ARB_depth_texture 1
+#endif
+
+#ifndef GL_ARB_shadow
+#define GL_ARB_shadow 1
+#endif
+
+#ifndef GL_ARB_shadow_ambient
+#define GL_ARB_shadow_ambient 1
+#endif
+
+#ifndef GL_ARB_window_pos
+#define GL_ARB_window_pos 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glWindowPos2dARB (GLdouble x, GLdouble y);
+GLAPI void APIENTRY glWindowPos2dvARB (const GLdouble *v);
+GLAPI void APIENTRY glWindowPos2fARB (GLfloat x, GLfloat y);
+GLAPI void APIENTRY glWindowPos2fvARB (const GLfloat *v);
+GLAPI void APIENTRY glWindowPos2iARB (GLint x, GLint y);
+GLAPI void APIENTRY glWindowPos2ivARB (const GLint *v);
+GLAPI void APIENTRY glWindowPos2sARB (GLshort x, GLshort y);
+GLAPI void APIENTRY glWindowPos2svARB (const GLshort *v);
+GLAPI void APIENTRY glWindowPos3dARB (GLdouble x, GLdouble y, GLdouble z);
+GLAPI void APIENTRY glWindowPos3dvARB (const GLdouble *v);
+GLAPI void APIENTRY glWindowPos3fARB (GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glWindowPos3fvARB (const GLfloat *v);
+GLAPI void APIENTRY glWindowPos3iARB (GLint x, GLint y, GLint z);
+GLAPI void APIENTRY glWindowPos3ivARB (const GLint *v);
+GLAPI void APIENTRY glWindowPos3sARB (GLshort x, GLshort y, GLshort z);
+GLAPI void APIENTRY glWindowPos3svARB (const GLshort *v);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLWINDOWPOS2DARBPROC) (GLdouble x, GLdouble y);
+typedef void (APIENTRYP PFNGLWINDOWPOS2DVARBPROC) (const GLdouble *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS2FARBPROC) (GLfloat x, GLfloat y);
+typedef void (APIENTRYP PFNGLWINDOWPOS2FVARBPROC) (const GLfloat *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS2IARBPROC) (GLint x, GLint y);
+typedef void (APIENTRYP PFNGLWINDOWPOS2IVARBPROC) (const GLint *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS2SARBPROC) (GLshort x, GLshort y);
+typedef void (APIENTRYP PFNGLWINDOWPOS2SVARBPROC) (const GLshort *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS3DARBPROC) (GLdouble x, GLdouble y, GLdouble z);
+typedef void (APIENTRYP PFNGLWINDOWPOS3DVARBPROC) (const GLdouble *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS3FARBPROC) (GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLWINDOWPOS3FVARBPROC) (const GLfloat *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS3IARBPROC) (GLint x, GLint y, GLint z);
+typedef void (APIENTRYP PFNGLWINDOWPOS3IVARBPROC) (const GLint *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS3SARBPROC) (GLshort x, GLshort y, GLshort z);
+typedef void (APIENTRYP PFNGLWINDOWPOS3SVARBPROC) (const GLshort *v);
+#endif
+
+#ifndef GL_ARB_vertex_program
+#define GL_ARB_vertex_program 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glVertexAttrib1dARB (GLuint index, GLdouble x);
+GLAPI void APIENTRY glVertexAttrib1dvARB (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttrib1fARB (GLuint index, GLfloat x);
+GLAPI void APIENTRY glVertexAttrib1fvARB (GLuint index, const GLfloat *v);
+GLAPI void APIENTRY glVertexAttrib1sARB (GLuint index, GLshort x);
+GLAPI void APIENTRY glVertexAttrib1svARB (GLuint index, const GLshort *v);
+GLAPI void APIENTRY glVertexAttrib2dARB (GLuint index, GLdouble x, GLdouble y);
+GLAPI void APIENTRY glVertexAttrib2dvARB (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttrib2fARB (GLuint index, GLfloat x, GLfloat y);
+GLAPI void APIENTRY glVertexAttrib2fvARB (GLuint index, const GLfloat *v);
+GLAPI void APIENTRY glVertexAttrib2sARB (GLuint index, GLshort x, GLshort y);
+GLAPI void APIENTRY glVertexAttrib2svARB (GLuint index, const GLshort *v);
+GLAPI void APIENTRY glVertexAttrib3dARB (GLuint index, GLdouble x, GLdouble y, GLdouble z);
+GLAPI void APIENTRY glVertexAttrib3dvARB (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttrib3fARB (GLuint index, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glVertexAttrib3fvARB (GLuint index, const GLfloat *v);
+GLAPI void APIENTRY glVertexAttrib3sARB (GLuint index, GLshort x, GLshort y, GLshort z);
+GLAPI void APIENTRY glVertexAttrib3svARB (GLuint index, const GLshort *v);
+GLAPI void APIENTRY glVertexAttrib4NbvARB (GLuint index, const GLbyte *v);
+GLAPI void APIENTRY glVertexAttrib4NivARB (GLuint index, const GLint *v);
+GLAPI void APIENTRY glVertexAttrib4NsvARB (GLuint index, const GLshort *v);
+GLAPI void APIENTRY glVertexAttrib4NubARB (GLuint index, GLubyte x, GLubyte y, GLubyte z, GLubyte w);
+GLAPI void APIENTRY glVertexAttrib4NubvARB (GLuint index, const GLubyte *v);
+GLAPI void APIENTRY glVertexAttrib4NuivARB (GLuint index, const GLuint *v);
+GLAPI void APIENTRY glVertexAttrib4NusvARB (GLuint index, const GLushort *v);
+GLAPI void APIENTRY glVertexAttrib4bvARB (GLuint index, const GLbyte *v);
+GLAPI void APIENTRY glVertexAttrib4dARB (GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+GLAPI void APIENTRY glVertexAttrib4dvARB (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttrib4fARB (GLuint index, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+GLAPI void APIENTRY glVertexAttrib4fvARB (GLuint index, const GLfloat *v);
+GLAPI void APIENTRY glVertexAttrib4ivARB (GLuint index, const GLint *v);
+GLAPI void APIENTRY glVertexAttrib4sARB (GLuint index, GLshort x, GLshort y, GLshort z, GLshort w);
+GLAPI void APIENTRY glVertexAttrib4svARB (GLuint index, const GLshort *v);
+GLAPI void APIENTRY glVertexAttrib4ubvARB (GLuint index, const GLubyte *v);
+GLAPI void APIENTRY glVertexAttrib4uivARB (GLuint index, const GLuint *v);
+GLAPI void APIENTRY glVertexAttrib4usvARB (GLuint index, const GLushort *v);
+GLAPI void APIENTRY glVertexAttribPointerARB (GLuint index, GLint size, GLenum type, GLboolean normalized, GLsizei stride, const GLvoid *pointer);
+GLAPI void APIENTRY glEnableVertexAttribArrayARB (GLuint index);
+GLAPI void APIENTRY glDisableVertexAttribArrayARB (GLuint index);
+GLAPI void APIENTRY glProgramStringARB (GLenum target, GLenum format, GLsizei len, const GLvoid *string);
+GLAPI void APIENTRY glBindProgramARB (GLenum target, GLuint program);
+GLAPI void APIENTRY glDeleteProgramsARB (GLsizei n, const GLuint *programs);
+GLAPI void APIENTRY glGenProgramsARB (GLsizei n, GLuint *programs);
+GLAPI void APIENTRY glProgramEnvParameter4dARB (GLenum target, GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+GLAPI void APIENTRY glProgramEnvParameter4dvARB (GLenum target, GLuint index, const GLdouble *params);
+GLAPI void APIENTRY glProgramEnvParameter4fARB (GLenum target, GLuint index, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+GLAPI void APIENTRY glProgramEnvParameter4fvARB (GLenum target, GLuint index, const GLfloat *params);
+GLAPI void APIENTRY glProgramLocalParameter4dARB (GLenum target, GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+GLAPI void APIENTRY glProgramLocalParameter4dvARB (GLenum target, GLuint index, const GLdouble *params);
+GLAPI void APIENTRY glProgramLocalParameter4fARB (GLenum target, GLuint index, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+GLAPI void APIENTRY glProgramLocalParameter4fvARB (GLenum target, GLuint index, const GLfloat *params);
+GLAPI void APIENTRY glGetProgramEnvParameterdvARB (GLenum target, GLuint index, GLdouble *params);
+GLAPI void APIENTRY glGetProgramEnvParameterfvARB (GLenum target, GLuint index, GLfloat *params);
+GLAPI void APIENTRY glGetProgramLocalParameterdvARB (GLenum target, GLuint index, GLdouble *params);
+GLAPI void APIENTRY glGetProgramLocalParameterfvARB (GLenum target, GLuint index, GLfloat *params);
+GLAPI void APIENTRY glGetProgramivARB (GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetProgramStringARB (GLenum target, GLenum pname, GLvoid *string);
+GLAPI void APIENTRY glGetVertexAttribdvARB (GLuint index, GLenum pname, GLdouble *params);
+GLAPI void APIENTRY glGetVertexAttribfvARB (GLuint index, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetVertexAttribivARB (GLuint index, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetVertexAttribPointervARB (GLuint index, GLenum pname, GLvoid* *pointer);
+GLAPI GLboolean APIENTRY glIsProgramARB (GLuint program);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1DARBPROC) (GLuint index, GLdouble x);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1DVARBPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1FARBPROC) (GLuint index, GLfloat x);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1FVARBPROC) (GLuint index, const GLfloat *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1SARBPROC) (GLuint index, GLshort x);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1SVARBPROC) (GLuint index, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2DARBPROC) (GLuint index, GLdouble x, GLdouble y);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2DVARBPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2FARBPROC) (GLuint index, GLfloat x, GLfloat y);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2FVARBPROC) (GLuint index, const GLfloat *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2SARBPROC) (GLuint index, GLshort x, GLshort y);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2SVARBPROC) (GLuint index, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3DARBPROC) (GLuint index, GLdouble x, GLdouble y, GLdouble z);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3DVARBPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3FARBPROC) (GLuint index, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3FVARBPROC) (GLuint index, const GLfloat *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3SARBPROC) (GLuint index, GLshort x, GLshort y, GLshort z);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3SVARBPROC) (GLuint index, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4NBVARBPROC) (GLuint index, const GLbyte *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4NIVARBPROC) (GLuint index, const GLint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4NSVARBPROC) (GLuint index, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4NUBARBPROC) (GLuint index, GLubyte x, GLubyte y, GLubyte z, GLubyte w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4NUBVARBPROC) (GLuint index, const GLubyte *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4NUIVARBPROC) (GLuint index, const GLuint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4NUSVARBPROC) (GLuint index, const GLushort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4BVARBPROC) (GLuint index, const GLbyte *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4DARBPROC) (GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4DVARBPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4FARBPROC) (GLuint index, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4FVARBPROC) (GLuint index, const GLfloat *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4IVARBPROC) (GLuint index, const GLint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4SARBPROC) (GLuint index, GLshort x, GLshort y, GLshort z, GLshort w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4SVARBPROC) (GLuint index, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4UBVARBPROC) (GLuint index, const GLubyte *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4UIVARBPROC) (GLuint index, const GLuint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4USVARBPROC) (GLuint index, const GLushort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBPOINTERARBPROC) (GLuint index, GLint size, GLenum type, GLboolean normalized, GLsizei stride, const GLvoid *pointer);
+typedef void (APIENTRYP PFNGLENABLEVERTEXATTRIBARRAYARBPROC) (GLuint index);
+typedef void (APIENTRYP PFNGLDISABLEVERTEXATTRIBARRAYARBPROC) (GLuint index);
+typedef void (APIENTRYP PFNGLPROGRAMSTRINGARBPROC) (GLenum target, GLenum format, GLsizei len, const GLvoid *string);
+typedef void (APIENTRYP PFNGLBINDPROGRAMARBPROC) (GLenum target, GLuint program);
+typedef void (APIENTRYP PFNGLDELETEPROGRAMSARBPROC) (GLsizei n, const GLuint *programs);
+typedef void (APIENTRYP PFNGLGENPROGRAMSARBPROC) (GLsizei n, GLuint *programs);
+typedef void (APIENTRYP PFNGLPROGRAMENVPARAMETER4DARBPROC) (GLenum target, GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+typedef void (APIENTRYP PFNGLPROGRAMENVPARAMETER4DVARBPROC) (GLenum target, GLuint index, const GLdouble *params);
+typedef void (APIENTRYP PFNGLPROGRAMENVPARAMETER4FARBPROC) (GLenum target, GLuint index, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+typedef void (APIENTRYP PFNGLPROGRAMENVPARAMETER4FVARBPROC) (GLenum target, GLuint index, const GLfloat *params);
+typedef void (APIENTRYP PFNGLPROGRAMLOCALPARAMETER4DARBPROC) (GLenum target, GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+typedef void (APIENTRYP PFNGLPROGRAMLOCALPARAMETER4DVARBPROC) (GLenum target, GLuint index, const GLdouble *params);
+typedef void (APIENTRYP PFNGLPROGRAMLOCALPARAMETER4FARBPROC) (GLenum target, GLuint index, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+typedef void (APIENTRYP PFNGLPROGRAMLOCALPARAMETER4FVARBPROC) (GLenum target, GLuint index, const GLfloat *params);
+typedef void (APIENTRYP PFNGLGETPROGRAMENVPARAMETERDVARBPROC) (GLenum target, GLuint index, GLdouble *params);
+typedef void (APIENTRYP PFNGLGETPROGRAMENVPARAMETERFVARBPROC) (GLenum target, GLuint index, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETPROGRAMLOCALPARAMETERDVARBPROC) (GLenum target, GLuint index, GLdouble *params);
+typedef void (APIENTRYP PFNGLGETPROGRAMLOCALPARAMETERFVARBPROC) (GLenum target, GLuint index, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETPROGRAMIVARBPROC) (GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETPROGRAMSTRINGARBPROC) (GLenum target, GLenum pname, GLvoid *string);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBDVARBPROC) (GLuint index, GLenum pname, GLdouble *params);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBFVARBPROC) (GLuint index, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBIVARBPROC) (GLuint index, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBPOINTERVARBPROC) (GLuint index, GLenum pname, GLvoid* *pointer);
+typedef GLboolean (APIENTRYP PFNGLISPROGRAMARBPROC) (GLuint program);
+#endif
+
+#ifndef GL_ARB_fragment_program
+#define GL_ARB_fragment_program 1
+/* All ARB_fragment_program entry points are shared with ARB_vertex_program. */
+#endif
+
+#ifndef GL_ARB_vertex_buffer_object
+#define GL_ARB_vertex_buffer_object 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBindBufferARB (GLenum target, GLuint buffer);
+GLAPI void APIENTRY glDeleteBuffersARB (GLsizei n, const GLuint *buffers);
+GLAPI void APIENTRY glGenBuffersARB (GLsizei n, GLuint *buffers);
+GLAPI GLboolean APIENTRY glIsBufferARB (GLuint buffer);
+GLAPI void APIENTRY glBufferDataARB (GLenum target, GLsizeiptrARB size, const GLvoid *data, GLenum usage);
+GLAPI void APIENTRY glBufferSubDataARB (GLenum target, GLintptrARB offset, GLsizeiptrARB size, const GLvoid *data);
+GLAPI void APIENTRY glGetBufferSubDataARB (GLenum target, GLintptrARB offset, GLsizeiptrARB size, GLvoid *data);
+GLAPI GLvoid* APIENTRY glMapBufferARB (GLenum target, GLenum access);
+GLAPI GLboolean APIENTRY glUnmapBufferARB (GLenum target);
+GLAPI void APIENTRY glGetBufferParameterivARB (GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetBufferPointervARB (GLenum target, GLenum pname, GLvoid* *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBINDBUFFERARBPROC) (GLenum target, GLuint buffer);
+typedef void (APIENTRYP PFNGLDELETEBUFFERSARBPROC) (GLsizei n, const GLuint *buffers);
+typedef void (APIENTRYP PFNGLGENBUFFERSARBPROC) (GLsizei n, GLuint *buffers);
+typedef GLboolean (APIENTRYP PFNGLISBUFFERARBPROC) (GLuint buffer);
+typedef void (APIENTRYP PFNGLBUFFERDATAARBPROC) (GLenum target, GLsizeiptrARB size, const GLvoid *data, GLenum usage);
+typedef void (APIENTRYP PFNGLBUFFERSUBDATAARBPROC) (GLenum target, GLintptrARB offset, GLsizeiptrARB size, const GLvoid *data);
+typedef void (APIENTRYP PFNGLGETBUFFERSUBDATAARBPROC) (GLenum target, GLintptrARB offset, GLsizeiptrARB size, GLvoid *data);
+typedef GLvoid* (APIENTRYP PFNGLMAPBUFFERARBPROC) (GLenum target, GLenum access);
+typedef GLboolean (APIENTRYP PFNGLUNMAPBUFFERARBPROC) (GLenum target);
+typedef void (APIENTRYP PFNGLGETBUFFERPARAMETERIVARBPROC) (GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETBUFFERPOINTERVARBPROC) (GLenum target, GLenum pname, GLvoid* *params);
+#endif
+
+#ifndef GL_ARB_occlusion_query
+#define GL_ARB_occlusion_query 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glGenQueriesARB (GLsizei n, GLuint *ids);
+GLAPI void APIENTRY glDeleteQueriesARB (GLsizei n, const GLuint *ids);
+GLAPI GLboolean APIENTRY glIsQueryARB (GLuint id);
+GLAPI void APIENTRY glBeginQueryARB (GLenum target, GLuint id);
+GLAPI void APIENTRY glEndQueryARB (GLenum target);
+GLAPI void APIENTRY glGetQueryivARB (GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetQueryObjectivARB (GLuint id, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetQueryObjectuivARB (GLuint id, GLenum pname, GLuint *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLGENQUERIESARBPROC) (GLsizei n, GLuint *ids);
+typedef void (APIENTRYP PFNGLDELETEQUERIESARBPROC) (GLsizei n, const GLuint *ids);
+typedef GLboolean (APIENTRYP PFNGLISQUERYARBPROC) (GLuint id);
+typedef void (APIENTRYP PFNGLBEGINQUERYARBPROC) (GLenum target, GLuint id);
+typedef void (APIENTRYP PFNGLENDQUERYARBPROC) (GLenum target);
+typedef void (APIENTRYP PFNGLGETQUERYIVARBPROC) (GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETQUERYOBJECTIVARBPROC) (GLuint id, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETQUERYOBJECTUIVARBPROC) (GLuint id, GLenum pname, GLuint *params);
+#endif
+
+#ifndef GL_ARB_shader_objects
+#define GL_ARB_shader_objects 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glDeleteObjectARB (GLhandleARB obj);
+GLAPI GLhandleARB APIENTRY glGetHandleARB (GLenum pname);
+GLAPI void APIENTRY glDetachObjectARB (GLhandleARB containerObj, GLhandleARB attachedObj);
+GLAPI GLhandleARB APIENTRY glCreateShaderObjectARB (GLenum shaderType);
+GLAPI void APIENTRY glShaderSourceARB (GLhandleARB shaderObj, GLsizei count, const GLcharARB* *string, const GLint *length);
+GLAPI void APIENTRY glCompileShaderARB (GLhandleARB shaderObj);
+GLAPI GLhandleARB APIENTRY glCreateProgramObjectARB (void);
+GLAPI void APIENTRY glAttachObjectARB (GLhandleARB containerObj, GLhandleARB obj);
+GLAPI void APIENTRY glLinkProgramARB (GLhandleARB programObj);
+GLAPI void APIENTRY glUseProgramObjectARB (GLhandleARB programObj);
+GLAPI void APIENTRY glValidateProgramARB (GLhandleARB programObj);
+GLAPI void APIENTRY glUniform1fARB (GLint location, GLfloat v0);
+GLAPI void APIENTRY glUniform2fARB (GLint location, GLfloat v0, GLfloat v1);
+GLAPI void APIENTRY glUniform3fARB (GLint location, GLfloat v0, GLfloat v1, GLfloat v2);
+GLAPI void APIENTRY glUniform4fARB (GLint location, GLfloat v0, GLfloat v1, GLfloat v2, GLfloat v3);
+GLAPI void APIENTRY glUniform1iARB (GLint location, GLint v0);
+GLAPI void APIENTRY glUniform2iARB (GLint location, GLint v0, GLint v1);
+GLAPI void APIENTRY glUniform3iARB (GLint location, GLint v0, GLint v1, GLint v2);
+GLAPI void APIENTRY glUniform4iARB (GLint location, GLint v0, GLint v1, GLint v2, GLint v3);
+GLAPI void APIENTRY glUniform1fvARB (GLint location, GLsizei count, const GLfloat *value);
+GLAPI void APIENTRY glUniform2fvARB (GLint location, GLsizei count, const GLfloat *value);
+GLAPI void APIENTRY glUniform3fvARB (GLint location, GLsizei count, const GLfloat *value);
+GLAPI void APIENTRY glUniform4fvARB (GLint location, GLsizei count, const GLfloat *value);
+GLAPI void APIENTRY glUniform1ivARB (GLint location, GLsizei count, const GLint *value);
+GLAPI void APIENTRY glUniform2ivARB (GLint location, GLsizei count, const GLint *value);
+GLAPI void APIENTRY glUniform3ivARB (GLint location, GLsizei count, const GLint *value);
+GLAPI void APIENTRY glUniform4ivARB (GLint location, GLsizei count, const GLint *value);
+GLAPI void APIENTRY glUniformMatrix2fvARB (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glUniformMatrix3fvARB (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glUniformMatrix4fvARB (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glGetObjectParameterfvARB (GLhandleARB obj, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetObjectParameterivARB (GLhandleARB obj, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetInfoLogARB (GLhandleARB obj, GLsizei maxLength, GLsizei *length, GLcharARB *infoLog);
+GLAPI void APIENTRY glGetAttachedObjectsARB (GLhandleARB containerObj, GLsizei maxCount, GLsizei *count, GLhandleARB *obj);
+GLAPI GLint APIENTRY glGetUniformLocationARB (GLhandleARB programObj, const GLcharARB *name);
+GLAPI void APIENTRY glGetActiveUniformARB (GLhandleARB programObj, GLuint index, GLsizei maxLength, GLsizei *length, GLint *size, GLenum *type, GLcharARB *name);
+GLAPI void APIENTRY glGetUniformfvARB (GLhandleARB programObj, GLint location, GLfloat *params);
+GLAPI void APIENTRY glGetUniformivARB (GLhandleARB programObj, GLint location, GLint *params);
+GLAPI void APIENTRY glGetShaderSourceARB (GLhandleARB obj, GLsizei maxLength, GLsizei *length, GLcharARB *source);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLDELETEOBJECTARBPROC) (GLhandleARB obj);
+typedef GLhandleARB (APIENTRYP PFNGLGETHANDLEARBPROC) (GLenum pname);
+typedef void (APIENTRYP PFNGLDETACHOBJECTARBPROC) (GLhandleARB containerObj, GLhandleARB attachedObj);
+typedef GLhandleARB (APIENTRYP PFNGLCREATESHADEROBJECTARBPROC) (GLenum shaderType);
+typedef void (APIENTRYP PFNGLSHADERSOURCEARBPROC) (GLhandleARB shaderObj, GLsizei count, const GLcharARB* *string, const GLint *length);
+typedef void (APIENTRYP PFNGLCOMPILESHADERARBPROC) (GLhandleARB shaderObj);
+typedef GLhandleARB (APIENTRYP PFNGLCREATEPROGRAMOBJECTARBPROC) (void);
+typedef void (APIENTRYP PFNGLATTACHOBJECTARBPROC) (GLhandleARB containerObj, GLhandleARB obj);
+typedef void (APIENTRYP PFNGLLINKPROGRAMARBPROC) (GLhandleARB programObj);
+typedef void (APIENTRYP PFNGLUSEPROGRAMOBJECTARBPROC) (GLhandleARB programObj);
+typedef void (APIENTRYP PFNGLVALIDATEPROGRAMARBPROC) (GLhandleARB programObj);
+typedef void (APIENTRYP PFNGLUNIFORM1FARBPROC) (GLint location, GLfloat v0);
+typedef void (APIENTRYP PFNGLUNIFORM2FARBPROC) (GLint location, GLfloat v0, GLfloat v1);
+typedef void (APIENTRYP PFNGLUNIFORM3FARBPROC) (GLint location, GLfloat v0, GLfloat v1, GLfloat v2);
+typedef void (APIENTRYP PFNGLUNIFORM4FARBPROC) (GLint location, GLfloat v0, GLfloat v1, GLfloat v2, GLfloat v3);
+typedef void (APIENTRYP PFNGLUNIFORM1IARBPROC) (GLint location, GLint v0);
+typedef void (APIENTRYP PFNGLUNIFORM2IARBPROC) (GLint location, GLint v0, GLint v1);
+typedef void (APIENTRYP PFNGLUNIFORM3IARBPROC) (GLint location, GLint v0, GLint v1, GLint v2);
+typedef void (APIENTRYP PFNGLUNIFORM4IARBPROC) (GLint location, GLint v0, GLint v1, GLint v2, GLint v3);
+typedef void (APIENTRYP PFNGLUNIFORM1FVARBPROC) (GLint location, GLsizei count, const GLfloat *value);
+typedef void (APIENTRYP PFNGLUNIFORM2FVARBPROC) (GLint location, GLsizei count, const GLfloat *value);
+typedef void (APIENTRYP PFNGLUNIFORM3FVARBPROC) (GLint location, GLsizei count, const GLfloat *value);
+typedef void (APIENTRYP PFNGLUNIFORM4FVARBPROC) (GLint location, GLsizei count, const GLfloat *value);
+typedef void (APIENTRYP PFNGLUNIFORM1IVARBPROC) (GLint location, GLsizei count, const GLint *value);
+typedef void (APIENTRYP PFNGLUNIFORM2IVARBPROC) (GLint location, GLsizei count, const GLint *value);
+typedef void (APIENTRYP PFNGLUNIFORM3IVARBPROC) (GLint location, GLsizei count, const GLint *value);
+typedef void (APIENTRYP PFNGLUNIFORM4IVARBPROC) (GLint location, GLsizei count, const GLint *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX2FVARBPROC) (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX3FVARBPROC) (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX4FVARBPROC) (GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLGETOBJECTPARAMETERFVARBPROC) (GLhandleARB obj, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETOBJECTPARAMETERIVARBPROC) (GLhandleARB obj, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETINFOLOGARBPROC) (GLhandleARB obj, GLsizei maxLength, GLsizei *length, GLcharARB *infoLog);
+typedef void (APIENTRYP PFNGLGETATTACHEDOBJECTSARBPROC) (GLhandleARB containerObj, GLsizei maxCount, GLsizei *count, GLhandleARB *obj);
+typedef GLint (APIENTRYP PFNGLGETUNIFORMLOCATIONARBPROC) (GLhandleARB programObj, const GLcharARB *name);
+typedef void (APIENTRYP PFNGLGETACTIVEUNIFORMARBPROC) (GLhandleARB programObj, GLuint index, GLsizei maxLength, GLsizei *length, GLint *size, GLenum *type, GLcharARB *name);
+typedef void (APIENTRYP PFNGLGETUNIFORMFVARBPROC) (GLhandleARB programObj, GLint location, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETUNIFORMIVARBPROC) (GLhandleARB programObj, GLint location, GLint *params);
+typedef void (APIENTRYP PFNGLGETSHADERSOURCEARBPROC) (GLhandleARB obj, GLsizei maxLength, GLsizei *length, GLcharARB *source);
+#endif
+
+#ifndef GL_ARB_vertex_shader
+#define GL_ARB_vertex_shader 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBindAttribLocationARB (GLhandleARB programObj, GLuint index, const GLcharARB *name);
+GLAPI void APIENTRY glGetActiveAttribARB (GLhandleARB programObj, GLuint index, GLsizei maxLength, GLsizei *length, GLint *size, GLenum *type, GLcharARB *name);
+GLAPI GLint APIENTRY glGetAttribLocationARB (GLhandleARB programObj, const GLcharARB *name);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBINDATTRIBLOCATIONARBPROC) (GLhandleARB programObj, GLuint index, const GLcharARB *name);
+typedef void (APIENTRYP PFNGLGETACTIVEATTRIBARBPROC) (GLhandleARB programObj, GLuint index, GLsizei maxLength, GLsizei *length, GLint *size, GLenum *type, GLcharARB *name);
+typedef GLint (APIENTRYP PFNGLGETATTRIBLOCATIONARBPROC) (GLhandleARB programObj, const GLcharARB *name);
+#endif
+
+#ifndef GL_ARB_fragment_shader
+#define GL_ARB_fragment_shader 1
+#endif
+
+#ifndef GL_ARB_shading_language_100
+#define GL_ARB_shading_language_100 1
+#endif
+
+#ifndef GL_ARB_texture_non_power_of_two
+#define GL_ARB_texture_non_power_of_two 1
+#endif
+
+#ifndef GL_ARB_point_sprite
+#define GL_ARB_point_sprite 1
+#endif
+
+#ifndef GL_ARB_fragment_program_shadow
+#define GL_ARB_fragment_program_shadow 1
+#endif
+
+#ifndef GL_ARB_draw_buffers
+#define GL_ARB_draw_buffers 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glDrawBuffersARB (GLsizei n, const GLenum *bufs);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLDRAWBUFFERSARBPROC) (GLsizei n, const GLenum *bufs);
+#endif
+
+#ifndef GL_ARB_texture_rectangle
+#define GL_ARB_texture_rectangle 1
+#endif
+
+#ifndef GL_ARB_color_buffer_float
+#define GL_ARB_color_buffer_float 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glClampColorARB (GLenum target, GLenum clamp);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCLAMPCOLORARBPROC) (GLenum target, GLenum clamp);
+#endif
+
+#ifndef GL_ARB_half_float_pixel
+#define GL_ARB_half_float_pixel 1
+#endif
+
+#ifndef GL_ARB_texture_float
+#define GL_ARB_texture_float 1
+#endif
+
+#ifndef GL_ARB_pixel_buffer_object
+#define GL_ARB_pixel_buffer_object 1
+#endif
+
+#ifndef GL_ARB_depth_buffer_float
+#define GL_ARB_depth_buffer_float 1
+#endif
+
+#ifndef GL_ARB_draw_instanced
+#define GL_ARB_draw_instanced 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glDrawArraysInstancedARB (GLenum mode, GLint first, GLsizei count, GLsizei primcount);
+GLAPI void APIENTRY glDrawElementsInstancedARB (GLenum mode, GLsizei count, GLenum type, const GLvoid *indices, GLsizei primcount);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLDRAWARRAYSINSTANCEDARBPROC) (GLenum mode, GLint first, GLsizei count, GLsizei primcount);
+typedef void (APIENTRYP PFNGLDRAWELEMENTSINSTANCEDARBPROC) (GLenum mode, GLsizei count, GLenum type, const GLvoid *indices, GLsizei primcount);
+#endif
+
+#ifndef GL_ARB_framebuffer_object
+#define GL_ARB_framebuffer_object 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI GLboolean APIENTRY glIsRenderbuffer (GLuint renderbuffer);
+GLAPI void APIENTRY glBindRenderbuffer (GLenum target, GLuint renderbuffer);
+GLAPI void APIENTRY glDeleteRenderbuffers (GLsizei n, const GLuint *renderbuffers);
+GLAPI void APIENTRY glGenRenderbuffers (GLsizei n, GLuint *renderbuffers);
+GLAPI void APIENTRY glRenderbufferStorage (GLenum target, GLenum internalformat, GLsizei width, GLsizei height);
+GLAPI void APIENTRY glGetRenderbufferParameteriv (GLenum target, GLenum pname, GLint *params);
+GLAPI GLboolean APIENTRY glIsFramebuffer (GLuint framebuffer);
+GLAPI void APIENTRY glBindFramebuffer (GLenum target, GLuint framebuffer);
+GLAPI void APIENTRY glDeleteFramebuffers (GLsizei n, const GLuint *framebuffers);
+GLAPI void APIENTRY glGenFramebuffers (GLsizei n, GLuint *framebuffers);
+GLAPI GLenum APIENTRY glCheckFramebufferStatus (GLenum target);
+GLAPI void APIENTRY glFramebufferTexture1D (GLenum target, GLenum attachment, GLenum textarget, GLuint texture, GLint level);
+GLAPI void APIENTRY glFramebufferTexture2D (GLenum target, GLenum attachment, GLenum textarget, GLuint texture, GLint level);
+GLAPI void APIENTRY glFramebufferTexture3D (GLenum target, GLenum attachment, GLenum textarget, GLuint texture, GLint level, GLint zoffset);
+GLAPI void APIENTRY glFramebufferRenderbuffer (GLenum target, GLenum attachment, GLenum renderbuffertarget, GLuint renderbuffer);
+GLAPI void APIENTRY glGetFramebufferAttachmentParameteriv (GLenum target, GLenum attachment, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGenerateMipmap (GLenum target);
+GLAPI void APIENTRY glBlitFramebuffer (GLint srcX0, GLint srcY0, GLint srcX1, GLint srcY1, GLint dstX0, GLint dstY0, GLint dstX1, GLint dstY1, GLbitfield mask, GLenum filter);
+GLAPI void APIENTRY glRenderbufferStorageMultisample (GLenum target, GLsizei samples, GLenum internalformat, GLsizei width, GLsizei height);
+GLAPI void APIENTRY glFramebufferTextureLayer (GLenum target, GLenum attachment, GLuint texture, GLint level, GLint layer);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef GLboolean (APIENTRYP PFNGLISRENDERBUFFERPROC) (GLuint renderbuffer);
+typedef void (APIENTRYP PFNGLBINDRENDERBUFFERPROC) (GLenum target, GLuint renderbuffer);
+typedef void (APIENTRYP PFNGLDELETERENDERBUFFERSPROC) (GLsizei n, const GLuint *renderbuffers);
+typedef void (APIENTRYP PFNGLGENRENDERBUFFERSPROC) (GLsizei n, GLuint *renderbuffers);
+typedef void (APIENTRYP PFNGLRENDERBUFFERSTORAGEPROC) (GLenum target, GLenum internalformat, GLsizei width, GLsizei height);
+typedef void (APIENTRYP PFNGLGETRENDERBUFFERPARAMETERIVPROC) (GLenum target, GLenum pname, GLint *params);
+typedef GLboolean (APIENTRYP PFNGLISFRAMEBUFFERPROC) (GLuint framebuffer);
+typedef void (APIENTRYP PFNGLBINDFRAMEBUFFERPROC) (GLenum target, GLuint framebuffer);
+typedef void (APIENTRYP PFNGLDELETEFRAMEBUFFERSPROC) (GLsizei n, const GLuint *framebuffers);
+typedef void (APIENTRYP PFNGLGENFRAMEBUFFERSPROC) (GLsizei n, GLuint *framebuffers);
+typedef GLenum (APIENTRYP PFNGLCHECKFRAMEBUFFERSTATUSPROC) (GLenum target);
+typedef void (APIENTRYP PFNGLFRAMEBUFFERTEXTURE1DPROC) (GLenum target, GLenum attachment, GLenum textarget, GLuint texture, GLint level);
+typedef void (APIENTRYP PFNGLFRAMEBUFFERTEXTURE2DPROC) (GLenum target, GLenum attachment, GLenum textarget, GLuint texture, GLint level);
+typedef void (APIENTRYP PFNGLFRAMEBUFFERTEXTURE3DPROC) (GLenum target, GLenum attachment, GLenum textarget, GLuint texture, GLint level, GLint zoffset);
+typedef void (APIENTRYP PFNGLFRAMEBUFFERRENDERBUFFERPROC) (GLenum target, GLenum attachment, GLenum renderbuffertarget, GLuint renderbuffer);
+typedef void (APIENTRYP PFNGLGETFRAMEBUFFERATTACHMENTPARAMETERIVPROC) (GLenum target, GLenum attachment, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGENERATEMIPMAPPROC) (GLenum target);
+typedef void (APIENTRYP PFNGLBLITFRAMEBUFFERPROC) (GLint srcX0, GLint srcY0, GLint srcX1, GLint srcY1, GLint dstX0, GLint dstY0, GLint dstX1, GLint dstY1, GLbitfield mask, GLenum filter);
+typedef void (APIENTRYP PFNGLRENDERBUFFERSTORAGEMULTISAMPLEPROC) (GLenum target, GLsizei samples, GLenum internalformat, GLsizei width, GLsizei height);
+typedef void (APIENTRYP PFNGLFRAMEBUFFERTEXTURELAYERPROC) (GLenum target, GLenum attachment, GLuint texture, GLint level, GLint layer);
+#endif
+
+#ifndef GL_ARB_framebuffer_sRGB
+#define GL_ARB_framebuffer_sRGB 1
+#endif
+
+#ifndef GL_ARB_geometry_shader4
+#define GL_ARB_geometry_shader4 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glProgramParameteriARB (GLuint program, GLenum pname, GLint value);
+GLAPI void APIENTRY glFramebufferTextureARB (GLenum target, GLenum attachment, GLuint texture, GLint level);
+GLAPI void APIENTRY glFramebufferTextureLayerARB (GLenum target, GLenum attachment, GLuint texture, GLint level, GLint layer);
+GLAPI void APIENTRY glFramebufferTextureFaceARB (GLenum target, GLenum attachment, GLuint texture, GLint level, GLenum face);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPROGRAMPARAMETERIARBPROC) (GLuint program, GLenum pname, GLint value);
+typedef void (APIENTRYP PFNGLFRAMEBUFFERTEXTUREARBPROC) (GLenum target, GLenum attachment, GLuint texture, GLint level);
+typedef void (APIENTRYP PFNGLFRAMEBUFFERTEXTURELAYERARBPROC) (GLenum target, GLenum attachment, GLuint texture, GLint level, GLint layer);
+typedef void (APIENTRYP PFNGLFRAMEBUFFERTEXTUREFACEARBPROC) (GLenum target, GLenum attachment, GLuint texture, GLint level, GLenum face);
+#endif
+
+#ifndef GL_ARB_half_float_vertex
+#define GL_ARB_half_float_vertex 1
+#endif
+
+#ifndef GL_ARB_instanced_arrays
+#define GL_ARB_instanced_arrays 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glVertexAttribDivisorARB (GLuint index, GLuint divisor);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLVERTEXATTRIBDIVISORARBPROC) (GLuint index, GLuint divisor);
+#endif
+
+#ifndef GL_ARB_map_buffer_range
+#define GL_ARB_map_buffer_range 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI GLvoid* APIENTRY glMapBufferRange (GLenum target, GLintptr offset, GLsizeiptr length, GLbitfield access);
+GLAPI void APIENTRY glFlushMappedBufferRange (GLenum target, GLintptr offset, GLsizeiptr length);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef GLvoid* (APIENTRYP PFNGLMAPBUFFERRANGEPROC) (GLenum target, GLintptr offset, GLsizeiptr length, GLbitfield access);
+typedef void (APIENTRYP PFNGLFLUSHMAPPEDBUFFERRANGEPROC) (GLenum target, GLintptr offset, GLsizeiptr length);
+#endif
+
+#ifndef GL_ARB_texture_buffer_object
+#define GL_ARB_texture_buffer_object 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glTexBufferARB (GLenum target, GLenum internalformat, GLuint buffer);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLTEXBUFFERARBPROC) (GLenum target, GLenum internalformat, GLuint buffer);
+#endif
+
+#ifndef GL_ARB_texture_compression_rgtc
+#define GL_ARB_texture_compression_rgtc 1
+#endif
+
+#ifndef GL_ARB_texture_rg
+#define GL_ARB_texture_rg 1
+#endif
+
+#ifndef GL_ARB_vertex_array_object
+#define GL_ARB_vertex_array_object 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBindVertexArray (GLuint array);
+GLAPI void APIENTRY glDeleteVertexArrays (GLsizei n, const GLuint *arrays);
+GLAPI void APIENTRY glGenVertexArrays (GLsizei n, GLuint *arrays);
+GLAPI GLboolean APIENTRY glIsVertexArray (GLuint array);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBINDVERTEXARRAYPROC) (GLuint array);
+typedef void (APIENTRYP PFNGLDELETEVERTEXARRAYSPROC) (GLsizei n, const GLuint *arrays);
+typedef void (APIENTRYP PFNGLGENVERTEXARRAYSPROC) (GLsizei n, GLuint *arrays);
+typedef GLboolean (APIENTRYP PFNGLISVERTEXARRAYPROC) (GLuint array);
+#endif
+
+#ifndef GL_ARB_uniform_buffer_object
+#define GL_ARB_uniform_buffer_object 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glGetUniformIndices (GLuint program, GLsizei uniformCount, const GLchar* *uniformNames, GLuint *uniformIndices);
+GLAPI void APIENTRY glGetActiveUniformsiv (GLuint program, GLsizei uniformCount, const GLuint *uniformIndices, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetActiveUniformName (GLuint program, GLuint uniformIndex, GLsizei bufSize, GLsizei *length, GLchar *uniformName);
+GLAPI GLuint APIENTRY glGetUniformBlockIndex (GLuint program, const GLchar *uniformBlockName);
+GLAPI void APIENTRY glGetActiveUniformBlockiv (GLuint program, GLuint uniformBlockIndex, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetActiveUniformBlockName (GLuint program, GLuint uniformBlockIndex, GLsizei bufSize, GLsizei *length, GLchar *uniformBlockName);
+GLAPI void APIENTRY glUniformBlockBinding (GLuint program, GLuint uniformBlockIndex, GLuint uniformBlockBinding);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLGETUNIFORMINDICESPROC) (GLuint program, GLsizei uniformCount, const GLchar* *uniformNames, GLuint *uniformIndices);
+typedef void (APIENTRYP PFNGLGETACTIVEUNIFORMSIVPROC) (GLuint program, GLsizei uniformCount, const GLuint *uniformIndices, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETACTIVEUNIFORMNAMEPROC) (GLuint program, GLuint uniformIndex, GLsizei bufSize, GLsizei *length, GLchar *uniformName);
+typedef GLuint (APIENTRYP PFNGLGETUNIFORMBLOCKINDEXPROC) (GLuint program, const GLchar *uniformBlockName);
+typedef void (APIENTRYP PFNGLGETACTIVEUNIFORMBLOCKIVPROC) (GLuint program, GLuint uniformBlockIndex, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETACTIVEUNIFORMBLOCKNAMEPROC) (GLuint program, GLuint uniformBlockIndex, GLsizei bufSize, GLsizei *length, GLchar *uniformBlockName);
+typedef void (APIENTRYP PFNGLUNIFORMBLOCKBINDINGPROC) (GLuint program, GLuint uniformBlockIndex, GLuint uniformBlockBinding);
+#endif
+
+#ifndef GL_ARB_compatibility
+#define GL_ARB_compatibility 1
+#endif
+
+#ifndef GL_ARB_copy_buffer
+#define GL_ARB_copy_buffer 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glCopyBufferSubData (GLenum readTarget, GLenum writeTarget, GLintptr readOffset, GLintptr writeOffset, GLsizeiptr size);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCOPYBUFFERSUBDATAPROC) (GLenum readTarget, GLenum writeTarget, GLintptr readOffset, GLintptr writeOffset, GLsizeiptr size);
+#endif
+
+#ifndef GL_ARB_shader_texture_lod
+#define GL_ARB_shader_texture_lod 1
+#endif
+
+#ifndef GL_ARB_depth_clamp
+#define GL_ARB_depth_clamp 1
+#endif
+
+#ifndef GL_ARB_draw_elements_base_vertex
+#define GL_ARB_draw_elements_base_vertex 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glDrawElementsBaseVertex (GLenum mode, GLsizei count, GLenum type, const GLvoid *indices, GLint basevertex);
+GLAPI void APIENTRY glDrawRangeElementsBaseVertex (GLenum mode, GLuint start, GLuint end, GLsizei count, GLenum type, const GLvoid *indices, GLint basevertex);
+GLAPI void APIENTRY glDrawElementsInstancedBaseVertex (GLenum mode, GLsizei count, GLenum type, const GLvoid *indices, GLsizei primcount, GLint basevertex);
+GLAPI void APIENTRY glMultiDrawElementsBaseVertex (GLenum mode, const GLsizei *count, GLenum type, const GLvoid* *indices, GLsizei primcount, const GLint *basevertex);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLDRAWELEMENTSBASEVERTEXPROC) (GLenum mode, GLsizei count, GLenum type, const GLvoid *indices, GLint basevertex);
+typedef void (APIENTRYP PFNGLDRAWRANGEELEMENTSBASEVERTEXPROC) (GLenum mode, GLuint start, GLuint end, GLsizei count, GLenum type, const GLvoid *indices, GLint basevertex);
+typedef void (APIENTRYP PFNGLDRAWELEMENTSINSTANCEDBASEVERTEXPROC) (GLenum mode, GLsizei count, GLenum type, const GLvoid *indices, GLsizei primcount, GLint basevertex);
+typedef void (APIENTRYP PFNGLMULTIDRAWELEMENTSBASEVERTEXPROC) (GLenum mode, const GLsizei *count, GLenum type, const GLvoid* *indices, GLsizei primcount, const GLint *basevertex);
+#endif
+
+#ifndef GL_ARB_fragment_coord_conventions
+#define GL_ARB_fragment_coord_conventions 1
+#endif
+
+#ifndef GL_ARB_provoking_vertex
+#define GL_ARB_provoking_vertex 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glProvokingVertex (GLenum mode);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPROVOKINGVERTEXPROC) (GLenum mode);
+#endif
+
+#ifndef GL_ARB_seamless_cube_map
+#define GL_ARB_seamless_cube_map 1
+#endif
+
+#ifndef GL_ARB_sync
+#define GL_ARB_sync 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI GLsync APIENTRY glFenceSync (GLenum condition, GLbitfield flags);
+GLAPI GLboolean APIENTRY glIsSync (GLsync sync);
+GLAPI void APIENTRY glDeleteSync (GLsync sync);
+GLAPI GLenum APIENTRY glClientWaitSync (GLsync sync, GLbitfield flags, GLuint64 timeout);
+GLAPI void APIENTRY glWaitSync (GLsync sync, GLbitfield flags, GLuint64 timeout);
+GLAPI void APIENTRY glGetInteger64v (GLenum pname, GLint64 *params);
+GLAPI void APIENTRY glGetSynciv (GLsync sync, GLenum pname, GLsizei bufSize, GLsizei *length, GLint *values);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef GLsync (APIENTRYP PFNGLFENCESYNCPROC) (GLenum condition, GLbitfield flags);
+typedef GLboolean (APIENTRYP PFNGLISSYNCPROC) (GLsync sync);
+typedef void (APIENTRYP PFNGLDELETESYNCPROC) (GLsync sync);
+typedef GLenum (APIENTRYP PFNGLCLIENTWAITSYNCPROC) (GLsync sync, GLbitfield flags, GLuint64 timeout);
+typedef void (APIENTRYP PFNGLWAITSYNCPROC) (GLsync sync, GLbitfield flags, GLuint64 timeout);
+typedef void (APIENTRYP PFNGLGETINTEGER64VPROC) (GLenum pname, GLint64 *params);
+typedef void (APIENTRYP PFNGLGETSYNCIVPROC) (GLsync sync, GLenum pname, GLsizei bufSize, GLsizei *length, GLint *values);
+#endif
+
+#ifndef GL_ARB_texture_multisample
+#define GL_ARB_texture_multisample 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glTexImage2DMultisample (GLenum target, GLsizei samples, GLint internalformat, GLsizei width, GLsizei height, GLboolean fixedsamplelocations);
+GLAPI void APIENTRY glTexImage3DMultisample (GLenum target, GLsizei samples, GLint internalformat, GLsizei width, GLsizei height, GLsizei depth, GLboolean fixedsamplelocations);
+GLAPI void APIENTRY glGetMultisamplefv (GLenum pname, GLuint index, GLfloat *val);
+GLAPI void APIENTRY glSampleMaski (GLuint index, GLbitfield mask);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLTEXIMAGE2DMULTISAMPLEPROC) (GLenum target, GLsizei samples, GLint internalformat, GLsizei width, GLsizei height, GLboolean fixedsamplelocations);
+typedef void (APIENTRYP PFNGLTEXIMAGE3DMULTISAMPLEPROC) (GLenum target, GLsizei samples, GLint internalformat, GLsizei width, GLsizei height, GLsizei depth, GLboolean fixedsamplelocations);
+typedef void (APIENTRYP PFNGLGETMULTISAMPLEFVPROC) (GLenum pname, GLuint index, GLfloat *val);
+typedef void (APIENTRYP PFNGLSAMPLEMASKIPROC) (GLuint index, GLbitfield mask);
+#endif
+
+#ifndef GL_ARB_vertex_array_bgra
+#define GL_ARB_vertex_array_bgra 1
+#endif
+
+#ifndef GL_ARB_draw_buffers_blend
+#define GL_ARB_draw_buffers_blend 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBlendEquationiARB (GLuint buf, GLenum mode);
+GLAPI void APIENTRY glBlendEquationSeparateiARB (GLuint buf, GLenum modeRGB, GLenum modeAlpha);
+GLAPI void APIENTRY glBlendFunciARB (GLuint buf, GLenum src, GLenum dst);
+GLAPI void APIENTRY glBlendFuncSeparateiARB (GLuint buf, GLenum srcRGB, GLenum dstRGB, GLenum srcAlpha, GLenum dstAlpha);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBLENDEQUATIONIARBPROC) (GLuint buf, GLenum mode);
+typedef void (APIENTRYP PFNGLBLENDEQUATIONSEPARATEIARBPROC) (GLuint buf, GLenum modeRGB, GLenum modeAlpha);
+typedef void (APIENTRYP PFNGLBLENDFUNCIARBPROC) (GLuint buf, GLenum src, GLenum dst);
+typedef void (APIENTRYP PFNGLBLENDFUNCSEPARATEIARBPROC) (GLuint buf, GLenum srcRGB, GLenum dstRGB, GLenum srcAlpha, GLenum dstAlpha);
+#endif
+
+#ifndef GL_ARB_sample_shading
+#define GL_ARB_sample_shading 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glMinSampleShadingARB (GLclampf value);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLMINSAMPLESHADINGARBPROC) (GLclampf value);
+#endif
+
+#ifndef GL_ARB_texture_cube_map_array
+#define GL_ARB_texture_cube_map_array 1
+#endif
+
+#ifndef GL_ARB_texture_gather
+#define GL_ARB_texture_gather 1
+#endif
+
+#ifndef GL_ARB_texture_query_lod
+#define GL_ARB_texture_query_lod 1
+#endif
+
+#ifndef GL_ARB_shading_language_include
+#define GL_ARB_shading_language_include 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glNamedStringARB (GLenum type, GLint namelen, const GLchar *name, GLint stringlen, const GLchar *string);
+GLAPI void APIENTRY glDeleteNamedStringARB (GLint namelen, const GLchar *name);
+GLAPI void APIENTRY glCompileShaderIncludeARB (GLuint shader, GLsizei count, const GLchar* *path, const GLint *length);
+GLAPI GLboolean APIENTRY glIsNamedStringARB (GLint namelen, const GLchar *name);
+GLAPI void APIENTRY glGetNamedStringARB (GLint namelen, const GLchar *name, GLsizei bufSize, GLint *stringlen, GLchar *string);
+GLAPI void APIENTRY glGetNamedStringivARB (GLint namelen, const GLchar *name, GLenum pname, GLint *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLNAMEDSTRINGARBPROC) (GLenum type, GLint namelen, const GLchar *name, GLint stringlen, const GLchar *string);
+typedef void (APIENTRYP PFNGLDELETENAMEDSTRINGARBPROC) (GLint namelen, const GLchar *name);
+typedef void (APIENTRYP PFNGLCOMPILESHADERINCLUDEARBPROC) (GLuint shader, GLsizei count, const GLchar* *path, const GLint *length);
+typedef GLboolean (APIENTRYP PFNGLISNAMEDSTRINGARBPROC) (GLint namelen, const GLchar *name);
+typedef void (APIENTRYP PFNGLGETNAMEDSTRINGARBPROC) (GLint namelen, const GLchar *name, GLsizei bufSize, GLint *stringlen, GLchar *string);
+typedef void (APIENTRYP PFNGLGETNAMEDSTRINGIVARBPROC) (GLint namelen, const GLchar *name, GLenum pname, GLint *params);
+#endif
+
+#ifndef GL_ARB_texture_compression_bptc
+#define GL_ARB_texture_compression_bptc 1
+#endif
+
+#ifndef GL_ARB_blend_func_extended
+#define GL_ARB_blend_func_extended 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBindFragDataLocationIndexed (GLuint program, GLuint colorNumber, GLuint index, const GLchar *name);
+GLAPI GLint APIENTRY glGetFragDataIndex (GLuint program, const GLchar *name);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBINDFRAGDATALOCATIONINDEXEDPROC) (GLuint program, GLuint colorNumber, GLuint index, const GLchar *name);
+typedef GLint (APIENTRYP PFNGLGETFRAGDATAINDEXPROC) (GLuint program, const GLchar *name);
+#endif
+
+#ifndef GL_ARB_explicit_attrib_location
+#define GL_ARB_explicit_attrib_location 1
+#endif
+
+#ifndef GL_ARB_occlusion_query2
+#define GL_ARB_occlusion_query2 1
+#endif
+
+#ifndef GL_ARB_sampler_objects
+#define GL_ARB_sampler_objects 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glGenSamplers (GLsizei count, GLuint *samplers);
+GLAPI void APIENTRY glDeleteSamplers (GLsizei count, const GLuint *samplers);
+GLAPI GLboolean APIENTRY glIsSampler (GLuint sampler);
+GLAPI void APIENTRY glBindSampler (GLuint unit, GLuint sampler);
+GLAPI void APIENTRY glSamplerParameteri (GLuint sampler, GLenum pname, GLint param);
+GLAPI void APIENTRY glSamplerParameteriv (GLuint sampler, GLenum pname, const GLint *param);
+GLAPI void APIENTRY glSamplerParameterf (GLuint sampler, GLenum pname, GLfloat param);
+GLAPI void APIENTRY glSamplerParameterfv (GLuint sampler, GLenum pname, const GLfloat *param);
+GLAPI void APIENTRY glSamplerParameterIiv (GLuint sampler, GLenum pname, const GLint *param);
+GLAPI void APIENTRY glSamplerParameterIuiv (GLuint sampler, GLenum pname, const GLuint *param);
+GLAPI void APIENTRY glGetSamplerParameteriv (GLuint sampler, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetSamplerParameterIiv (GLuint sampler, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetSamplerParameterfv (GLuint sampler, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetSamplerParameterIuiv (GLuint sampler, GLenum pname, GLuint *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLGENSAMPLERSPROC) (GLsizei count, GLuint *samplers);
+typedef void (APIENTRYP PFNGLDELETESAMPLERSPROC) (GLsizei count, const GLuint *samplers);
+typedef GLboolean (APIENTRYP PFNGLISSAMPLERPROC) (GLuint sampler);
+typedef void (APIENTRYP PFNGLBINDSAMPLERPROC) (GLuint unit, GLuint sampler);
+typedef void (APIENTRYP PFNGLSAMPLERPARAMETERIPROC) (GLuint sampler, GLenum pname, GLint param);
+typedef void (APIENTRYP PFNGLSAMPLERPARAMETERIVPROC) (GLuint sampler, GLenum pname, const GLint *param);
+typedef void (APIENTRYP PFNGLSAMPLERPARAMETERFPROC) (GLuint sampler, GLenum pname, GLfloat param);
+typedef void (APIENTRYP PFNGLSAMPLERPARAMETERFVPROC) (GLuint sampler, GLenum pname, const GLfloat *param);
+typedef void (APIENTRYP PFNGLSAMPLERPARAMETERIIVPROC) (GLuint sampler, GLenum pname, const GLint *param);
+typedef void (APIENTRYP PFNGLSAMPLERPARAMETERIUIVPROC) (GLuint sampler, GLenum pname, const GLuint *param);
+typedef void (APIENTRYP PFNGLGETSAMPLERPARAMETERIVPROC) (GLuint sampler, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETSAMPLERPARAMETERIIVPROC) (GLuint sampler, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETSAMPLERPARAMETERFVPROC) (GLuint sampler, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETSAMPLERPARAMETERIUIVPROC) (GLuint sampler, GLenum pname, GLuint *params);
+#endif
+
+#ifndef GL_ARB_texture_rgb10_a2ui
+#define GL_ARB_texture_rgb10_a2ui 1
+#endif
+
+#ifndef GL_ARB_texture_swizzle
+#define GL_ARB_texture_swizzle 1
+#endif
+
+#ifndef GL_ARB_timer_query
+#define GL_ARB_timer_query 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glQueryCounter (GLuint id, GLenum target);
+GLAPI void APIENTRY glGetQueryObjecti64v (GLuint id, GLenum pname, GLint64 *params);
+GLAPI void APIENTRY glGetQueryObjectui64v (GLuint id, GLenum pname, GLuint64 *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLQUERYCOUNTERPROC) (GLuint id, GLenum target);
+typedef void (APIENTRYP PFNGLGETQUERYOBJECTI64VPROC) (GLuint id, GLenum pname, GLint64 *params);
+typedef void (APIENTRYP PFNGLGETQUERYOBJECTUI64VPROC) (GLuint id, GLenum pname, GLuint64 *params);
+#endif
+
+#ifndef GL_ARB_vertex_type_2_10_10_10_rev
+#define GL_ARB_vertex_type_2_10_10_10_rev 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glVertexP2ui (GLenum type, GLuint value);
+GLAPI void APIENTRY glVertexP2uiv (GLenum type, const GLuint *value);
+GLAPI void APIENTRY glVertexP3ui (GLenum type, GLuint value);
+GLAPI void APIENTRY glVertexP3uiv (GLenum type, const GLuint *value);
+GLAPI void APIENTRY glVertexP4ui (GLenum type, GLuint value);
+GLAPI void APIENTRY glVertexP4uiv (GLenum type, const GLuint *value);
+GLAPI void APIENTRY glTexCoordP1ui (GLenum type, GLuint coords);
+GLAPI void APIENTRY glTexCoordP1uiv (GLenum type, const GLuint *coords);
+GLAPI void APIENTRY glTexCoordP2ui (GLenum type, GLuint coords);
+GLAPI void APIENTRY glTexCoordP2uiv (GLenum type, const GLuint *coords);
+GLAPI void APIENTRY glTexCoordP3ui (GLenum type, GLuint coords);
+GLAPI void APIENTRY glTexCoordP3uiv (GLenum type, const GLuint *coords);
+GLAPI void APIENTRY glTexCoordP4ui (GLenum type, GLuint coords);
+GLAPI void APIENTRY glTexCoordP4uiv (GLenum type, const GLuint *coords);
+GLAPI void APIENTRY glMultiTexCoordP1ui (GLenum texture, GLenum type, GLuint coords);
+GLAPI void APIENTRY glMultiTexCoordP1uiv (GLenum texture, GLenum type, const GLuint *coords);
+GLAPI void APIENTRY glMultiTexCoordP2ui (GLenum texture, GLenum type, GLuint coords);
+GLAPI void APIENTRY glMultiTexCoordP2uiv (GLenum texture, GLenum type, const GLuint *coords);
+GLAPI void APIENTRY glMultiTexCoordP3ui (GLenum texture, GLenum type, GLuint coords);
+GLAPI void APIENTRY glMultiTexCoordP3uiv (GLenum texture, GLenum type, const GLuint *coords);
+GLAPI void APIENTRY glMultiTexCoordP4ui (GLenum texture, GLenum type, GLuint coords);
+GLAPI void APIENTRY glMultiTexCoordP4uiv (GLenum texture, GLenum type, const GLuint *coords);
+GLAPI void APIENTRY glNormalP3ui (GLenum type, GLuint coords);
+GLAPI void APIENTRY glNormalP3uiv (GLenum type, const GLuint *coords);
+GLAPI void APIENTRY glColorP3ui (GLenum type, GLuint color);
+GLAPI void APIENTRY glColorP3uiv (GLenum type, const GLuint *color);
+GLAPI void APIENTRY glColorP4ui (GLenum type, GLuint color);
+GLAPI void APIENTRY glColorP4uiv (GLenum type, const GLuint *color);
+GLAPI void APIENTRY glSecondaryColorP3ui (GLenum type, GLuint color);
+GLAPI void APIENTRY glSecondaryColorP3uiv (GLenum type, const GLuint *color);
+GLAPI void APIENTRY glVertexAttribP1ui (GLuint index, GLenum type, GLboolean normalized, GLuint value);
+GLAPI void APIENTRY glVertexAttribP1uiv (GLuint index, GLenum type, GLboolean normalized, const GLuint *value);
+GLAPI void APIENTRY glVertexAttribP2ui (GLuint index, GLenum type, GLboolean normalized, GLuint value);
+GLAPI void APIENTRY glVertexAttribP2uiv (GLuint index, GLenum type, GLboolean normalized, const GLuint *value);
+GLAPI void APIENTRY glVertexAttribP3ui (GLuint index, GLenum type, GLboolean normalized, GLuint value);
+GLAPI void APIENTRY glVertexAttribP3uiv (GLuint index, GLenum type, GLboolean normalized, const GLuint *value);
+GLAPI void APIENTRY glVertexAttribP4ui (GLuint index, GLenum type, GLboolean normalized, GLuint value);
+GLAPI void APIENTRY glVertexAttribP4uiv (GLuint index, GLenum type, GLboolean normalized, const GLuint *value);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLVERTEXP2UIPROC) (GLenum type, GLuint value);
+typedef void (APIENTRYP PFNGLVERTEXP2UIVPROC) (GLenum type, const GLuint *value);
+typedef void (APIENTRYP PFNGLVERTEXP3UIPROC) (GLenum type, GLuint value);
+typedef void (APIENTRYP PFNGLVERTEXP3UIVPROC) (GLenum type, const GLuint *value);
+typedef void (APIENTRYP PFNGLVERTEXP4UIPROC) (GLenum type, GLuint value);
+typedef void (APIENTRYP PFNGLVERTEXP4UIVPROC) (GLenum type, const GLuint *value);
+typedef void (APIENTRYP PFNGLTEXCOORDP1UIPROC) (GLenum type, GLuint coords);
+typedef void (APIENTRYP PFNGLTEXCOORDP1UIVPROC) (GLenum type, const GLuint *coords);
+typedef void (APIENTRYP PFNGLTEXCOORDP2UIPROC) (GLenum type, GLuint coords);
+typedef void (APIENTRYP PFNGLTEXCOORDP2UIVPROC) (GLenum type, const GLuint *coords);
+typedef void (APIENTRYP PFNGLTEXCOORDP3UIPROC) (GLenum type, GLuint coords);
+typedef void (APIENTRYP PFNGLTEXCOORDP3UIVPROC) (GLenum type, const GLuint *coords);
+typedef void (APIENTRYP PFNGLTEXCOORDP4UIPROC) (GLenum type, GLuint coords);
+typedef void (APIENTRYP PFNGLTEXCOORDP4UIVPROC) (GLenum type, const GLuint *coords);
+typedef void (APIENTRYP PFNGLMULTITEXCOORDP1UIPROC) (GLenum texture, GLenum type, GLuint coords);
+typedef void (APIENTRYP PFNGLMULTITEXCOORDP1UIVPROC) (GLenum texture, GLenum type, const GLuint *coords);
+typedef void (APIENTRYP PFNGLMULTITEXCOORDP2UIPROC) (GLenum texture, GLenum type, GLuint coords);
+typedef void (APIENTRYP PFNGLMULTITEXCOORDP2UIVPROC) (GLenum texture, GLenum type, const GLuint *coords);
+typedef void (APIENTRYP PFNGLMULTITEXCOORDP3UIPROC) (GLenum texture, GLenum type, GLuint coords);
+typedef void (APIENTRYP PFNGLMULTITEXCOORDP3UIVPROC) (GLenum texture, GLenum type, const GLuint *coords);
+typedef void (APIENTRYP PFNGLMULTITEXCOORDP4UIPROC) (GLenum texture, GLenum type, GLuint coords);
+typedef void (APIENTRYP PFNGLMULTITEXCOORDP4UIVPROC) (GLenum texture, GLenum type, const GLuint *coords);
+typedef void (APIENTRYP PFNGLNORMALP3UIPROC) (GLenum type, GLuint coords);
+typedef void (APIENTRYP PFNGLNORMALP3UIVPROC) (GLenum type, const GLuint *coords);
+typedef void (APIENTRYP PFNGLCOLORP3UIPROC) (GLenum type, GLuint color);
+typedef void (APIENTRYP PFNGLCOLORP3UIVPROC) (GLenum type, const GLuint *color);
+typedef void (APIENTRYP PFNGLCOLORP4UIPROC) (GLenum type, GLuint color);
+typedef void (APIENTRYP PFNGLCOLORP4UIVPROC) (GLenum type, const GLuint *color);
+typedef void (APIENTRYP PFNGLSECONDARYCOLORP3UIPROC) (GLenum type, GLuint color);
+typedef void (APIENTRYP PFNGLSECONDARYCOLORP3UIVPROC) (GLenum type, const GLuint *color);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBP1UIPROC) (GLuint index, GLenum type, GLboolean normalized, GLuint value);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBP1UIVPROC) (GLuint index, GLenum type, GLboolean normalized, const GLuint *value);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBP2UIPROC) (GLuint index, GLenum type, GLboolean normalized, GLuint value);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBP2UIVPROC) (GLuint index, GLenum type, GLboolean normalized, const GLuint *value);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBP3UIPROC) (GLuint index, GLenum type, GLboolean normalized, GLuint value);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBP3UIVPROC) (GLuint index, GLenum type, GLboolean normalized, const GLuint *value);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBP4UIPROC) (GLuint index, GLenum type, GLboolean normalized, GLuint value);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBP4UIVPROC) (GLuint index, GLenum type, GLboolean normalized, const GLuint *value);
+#endif
+
+#ifndef GL_ARB_draw_indirect
+#define GL_ARB_draw_indirect 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glDrawArraysIndirect (GLenum mode, const GLvoid *indirect);
+GLAPI void APIENTRY glDrawElementsIndirect (GLenum mode, GLenum type, const GLvoid *indirect);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLDRAWARRAYSINDIRECTPROC) (GLenum mode, const GLvoid *indirect);
+typedef void (APIENTRYP PFNGLDRAWELEMENTSINDIRECTPROC) (GLenum mode, GLenum type, const GLvoid *indirect);
+#endif
+
+#ifndef GL_ARB_gpu_shader5
+#define GL_ARB_gpu_shader5 1
+#endif
+
+#ifndef GL_ARB_gpu_shader_fp64
+#define GL_ARB_gpu_shader_fp64 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glUniform1d (GLint location, GLdouble x);
+GLAPI void APIENTRY glUniform2d (GLint location, GLdouble x, GLdouble y);
+GLAPI void APIENTRY glUniform3d (GLint location, GLdouble x, GLdouble y, GLdouble z);
+GLAPI void APIENTRY glUniform4d (GLint location, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+GLAPI void APIENTRY glUniform1dv (GLint location, GLsizei count, const GLdouble *value);
+GLAPI void APIENTRY glUniform2dv (GLint location, GLsizei count, const GLdouble *value);
+GLAPI void APIENTRY glUniform3dv (GLint location, GLsizei count, const GLdouble *value);
+GLAPI void APIENTRY glUniform4dv (GLint location, GLsizei count, const GLdouble *value);
+GLAPI void APIENTRY glUniformMatrix2dv (GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glUniformMatrix3dv (GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glUniformMatrix4dv (GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glUniformMatrix2x3dv (GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glUniformMatrix2x4dv (GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glUniformMatrix3x2dv (GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glUniformMatrix3x4dv (GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glUniformMatrix4x2dv (GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glUniformMatrix4x3dv (GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glGetUniformdv (GLuint program, GLint location, GLdouble *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLUNIFORM1DPROC) (GLint location, GLdouble x);
+typedef void (APIENTRYP PFNGLUNIFORM2DPROC) (GLint location, GLdouble x, GLdouble y);
+typedef void (APIENTRYP PFNGLUNIFORM3DPROC) (GLint location, GLdouble x, GLdouble y, GLdouble z);
+typedef void (APIENTRYP PFNGLUNIFORM4DPROC) (GLint location, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+typedef void (APIENTRYP PFNGLUNIFORM1DVPROC) (GLint location, GLsizei count, const GLdouble *value);
+typedef void (APIENTRYP PFNGLUNIFORM2DVPROC) (GLint location, GLsizei count, const GLdouble *value);
+typedef void (APIENTRYP PFNGLUNIFORM3DVPROC) (GLint location, GLsizei count, const GLdouble *value);
+typedef void (APIENTRYP PFNGLUNIFORM4DVPROC) (GLint location, GLsizei count, const GLdouble *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX2DVPROC) (GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX3DVPROC) (GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX4DVPROC) (GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX2X3DVPROC) (GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX2X4DVPROC) (GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX3X2DVPROC) (GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX3X4DVPROC) (GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX4X2DVPROC) (GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLUNIFORMMATRIX4X3DVPROC) (GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLGETUNIFORMDVPROC) (GLuint program, GLint location, GLdouble *params);
+#endif
+
+#ifndef GL_ARB_shader_subroutine
+#define GL_ARB_shader_subroutine 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI GLint APIENTRY glGetSubroutineUniformLocation (GLuint program, GLenum shadertype, const GLchar *name);
+GLAPI GLuint APIENTRY glGetSubroutineIndex (GLuint program, GLenum shadertype, const GLchar *name);
+GLAPI void APIENTRY glGetActiveSubroutineUniformiv (GLuint program, GLenum shadertype, GLuint index, GLenum pname, GLint *values);
+GLAPI void APIENTRY glGetActiveSubroutineUniformName (GLuint program, GLenum shadertype, GLuint index, GLsizei bufsize, GLsizei *length, GLchar *name);
+GLAPI void APIENTRY glGetActiveSubroutineName (GLuint program, GLenum shadertype, GLuint index, GLsizei bufsize, GLsizei *length, GLchar *name);
+GLAPI void APIENTRY glUniformSubroutinesuiv (GLenum shadertype, GLsizei count, const GLuint *indices);
+GLAPI void APIENTRY glGetUniformSubroutineuiv (GLenum shadertype, GLint location, GLuint *params);
+GLAPI void APIENTRY glGetProgramStageiv (GLuint program, GLenum shadertype, GLenum pname, GLint *values);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef GLint (APIENTRYP PFNGLGETSUBROUTINEUNIFORMLOCATIONPROC) (GLuint program, GLenum shadertype, const GLchar *name);
+typedef GLuint (APIENTRYP PFNGLGETSUBROUTINEINDEXPROC) (GLuint program, GLenum shadertype, const GLchar *name);
+typedef void (APIENTRYP PFNGLGETACTIVESUBROUTINEUNIFORMIVPROC) (GLuint program, GLenum shadertype, GLuint index, GLenum pname, GLint *values);
+typedef void (APIENTRYP PFNGLGETACTIVESUBROUTINEUNIFORMNAMEPROC) (GLuint program, GLenum shadertype, GLuint index, GLsizei bufsize, GLsizei *length, GLchar *name);
+typedef void (APIENTRYP PFNGLGETACTIVESUBROUTINENAMEPROC) (GLuint program, GLenum shadertype, GLuint index, GLsizei bufsize, GLsizei *length, GLchar *name);
+typedef void (APIENTRYP PFNGLUNIFORMSUBROUTINESUIVPROC) (GLenum shadertype, GLsizei count, const GLuint *indices);
+typedef void (APIENTRYP PFNGLGETUNIFORMSUBROUTINEUIVPROC) (GLenum shadertype, GLint location, GLuint *params);
+typedef void (APIENTRYP PFNGLGETPROGRAMSTAGEIVPROC) (GLuint program, GLenum shadertype, GLenum pname, GLint *values);
+#endif
+
+#ifndef GL_ARB_tessellation_shader
+#define GL_ARB_tessellation_shader 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glPatchParameteri (GLenum pname, GLint value);
+GLAPI void APIENTRY glPatchParameterfv (GLenum pname, const GLfloat *values);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPATCHPARAMETERIPROC) (GLenum pname, GLint value);
+typedef void (APIENTRYP PFNGLPATCHPARAMETERFVPROC) (GLenum pname, const GLfloat *values);
+#endif
+
+#ifndef GL_ARB_texture_buffer_object_rgb32
+#define GL_ARB_texture_buffer_object_rgb32 1
+#endif
+
+#ifndef GL_ARB_transform_feedback2
+#define GL_ARB_transform_feedback2 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBindTransformFeedback (GLenum target, GLuint id);
+GLAPI void APIENTRY glDeleteTransformFeedbacks (GLsizei n, const GLuint *ids);
+GLAPI void APIENTRY glGenTransformFeedbacks (GLsizei n, GLuint *ids);
+GLAPI GLboolean APIENTRY glIsTransformFeedback (GLuint id);
+GLAPI void APIENTRY glPauseTransformFeedback (void);
+GLAPI void APIENTRY glResumeTransformFeedback (void);
+GLAPI void APIENTRY glDrawTransformFeedback (GLenum mode, GLuint id);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBINDTRANSFORMFEEDBACKPROC) (GLenum target, GLuint id);
+typedef void (APIENTRYP PFNGLDELETETRANSFORMFEEDBACKSPROC) (GLsizei n, const GLuint *ids);
+typedef void (APIENTRYP PFNGLGENTRANSFORMFEEDBACKSPROC) (GLsizei n, GLuint *ids);
+typedef GLboolean (APIENTRYP PFNGLISTRANSFORMFEEDBACKPROC) (GLuint id);
+typedef void (APIENTRYP PFNGLPAUSETRANSFORMFEEDBACKPROC) (void);
+typedef void (APIENTRYP PFNGLRESUMETRANSFORMFEEDBACKPROC) (void);
+typedef void (APIENTRYP PFNGLDRAWTRANSFORMFEEDBACKPROC) (GLenum mode, GLuint id);
+#endif
+
+#ifndef GL_ARB_transform_feedback3
+#define GL_ARB_transform_feedback3 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glDrawTransformFeedbackStream (GLenum mode, GLuint id, GLuint stream);
+GLAPI void APIENTRY glBeginQueryIndexed (GLenum target, GLuint index, GLuint id);
+GLAPI void APIENTRY glEndQueryIndexed (GLenum target, GLuint index);
+GLAPI void APIENTRY glGetQueryIndexediv (GLenum target, GLuint index, GLenum pname, GLint *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLDRAWTRANSFORMFEEDBACKSTREAMPROC) (GLenum mode, GLuint id, GLuint stream);
+typedef void (APIENTRYP PFNGLBEGINQUERYINDEXEDPROC) (GLenum target, GLuint index, GLuint id);
+typedef void (APIENTRYP PFNGLENDQUERYINDEXEDPROC) (GLenum target, GLuint index);
+typedef void (APIENTRYP PFNGLGETQUERYINDEXEDIVPROC) (GLenum target, GLuint index, GLenum pname, GLint *params);
+#endif
+
+#ifndef GL_ARB_ES2_compatibility
+#define GL_ARB_ES2_compatibility 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glReleaseShaderCompiler (void);
+GLAPI void APIENTRY glShaderBinary (GLsizei count, const GLuint *shaders, GLenum binaryformat, const GLvoid *binary, GLsizei length);
+GLAPI void APIENTRY glGetShaderPrecisionFormat (GLenum shadertype, GLenum precisiontype, GLint *range, GLint *precision);
+GLAPI void APIENTRY glDepthRangef (GLclampf n, GLclampf f);
+GLAPI void APIENTRY glClearDepthf (GLclampf d);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLRELEASESHADERCOMPILERPROC) (void);
+typedef void (APIENTRYP PFNGLSHADERBINARYPROC) (GLsizei count, const GLuint *shaders, GLenum binaryformat, const GLvoid *binary, GLsizei length);
+typedef void (APIENTRYP PFNGLGETSHADERPRECISIONFORMATPROC) (GLenum shadertype, GLenum precisiontype, GLint *range, GLint *precision);
+typedef void (APIENTRYP PFNGLDEPTHRANGEFPROC) (GLclampf n, GLclampf f);
+typedef void (APIENTRYP PFNGLCLEARDEPTHFPROC) (GLclampf d);
+#endif
+
+#ifndef GL_ARB_get_program_binary
+#define GL_ARB_get_program_binary 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glGetProgramBinary (GLuint program, GLsizei bufSize, GLsizei *length, GLenum *binaryFormat, GLvoid *binary);
+GLAPI void APIENTRY glProgramBinary (GLuint program, GLenum binaryFormat, const GLvoid *binary, GLsizei length);
+GLAPI void APIENTRY glProgramParameteri (GLuint program, GLenum pname, GLint value);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLGETPROGRAMBINARYPROC) (GLuint program, GLsizei bufSize, GLsizei *length, GLenum *binaryFormat, GLvoid *binary);
+typedef void (APIENTRYP PFNGLPROGRAMBINARYPROC) (GLuint program, GLenum binaryFormat, const GLvoid *binary, GLsizei length);
+typedef void (APIENTRYP PFNGLPROGRAMPARAMETERIPROC) (GLuint program, GLenum pname, GLint value);
+#endif
+
+#ifndef GL_ARB_separate_shader_objects
+#define GL_ARB_separate_shader_objects 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glUseProgramStages (GLuint pipeline, GLbitfield stages, GLuint program);
+GLAPI void APIENTRY glActiveShaderProgram (GLuint pipeline, GLuint program);
+GLAPI GLuint APIENTRY glCreateShaderProgramv (GLenum type, GLsizei count, const GLchar* *strings);
+GLAPI void APIENTRY glBindProgramPipeline (GLuint pipeline);
+GLAPI void APIENTRY glDeleteProgramPipelines (GLsizei n, const GLuint *pipelines);
+GLAPI void APIENTRY glGenProgramPipelines (GLsizei n, GLuint *pipelines);
+GLAPI GLboolean APIENTRY glIsProgramPipeline (GLuint pipeline);
+GLAPI void APIENTRY glGetProgramPipelineiv (GLuint pipeline, GLenum pname, GLint *params);
+GLAPI void APIENTRY glProgramUniform1i (GLuint program, GLint location, GLint v0);
+GLAPI void APIENTRY glProgramUniform1iv (GLuint program, GLint location, GLsizei count, const GLint *value);
+GLAPI void APIENTRY glProgramUniform1f (GLuint program, GLint location, GLfloat v0);
+GLAPI void APIENTRY glProgramUniform1fv (GLuint program, GLint location, GLsizei count, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniform1d (GLuint program, GLint location, GLdouble v0);
+GLAPI void APIENTRY glProgramUniform1dv (GLuint program, GLint location, GLsizei count, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniform1ui (GLuint program, GLint location, GLuint v0);
+GLAPI void APIENTRY glProgramUniform1uiv (GLuint program, GLint location, GLsizei count, const GLuint *value);
+GLAPI void APIENTRY glProgramUniform2i (GLuint program, GLint location, GLint v0, GLint v1);
+GLAPI void APIENTRY glProgramUniform2iv (GLuint program, GLint location, GLsizei count, const GLint *value);
+GLAPI void APIENTRY glProgramUniform2f (GLuint program, GLint location, GLfloat v0, GLfloat v1);
+GLAPI void APIENTRY glProgramUniform2fv (GLuint program, GLint location, GLsizei count, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniform2d (GLuint program, GLint location, GLdouble v0, GLdouble v1);
+GLAPI void APIENTRY glProgramUniform2dv (GLuint program, GLint location, GLsizei count, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniform2ui (GLuint program, GLint location, GLuint v0, GLuint v1);
+GLAPI void APIENTRY glProgramUniform2uiv (GLuint program, GLint location, GLsizei count, const GLuint *value);
+GLAPI void APIENTRY glProgramUniform3i (GLuint program, GLint location, GLint v0, GLint v1, GLint v2);
+GLAPI void APIENTRY glProgramUniform3iv (GLuint program, GLint location, GLsizei count, const GLint *value);
+GLAPI void APIENTRY glProgramUniform3f (GLuint program, GLint location, GLfloat v0, GLfloat v1, GLfloat v2);
+GLAPI void APIENTRY glProgramUniform3fv (GLuint program, GLint location, GLsizei count, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniform3d (GLuint program, GLint location, GLdouble v0, GLdouble v1, GLdouble v2);
+GLAPI void APIENTRY glProgramUniform3dv (GLuint program, GLint location, GLsizei count, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniform3ui (GLuint program, GLint location, GLuint v0, GLuint v1, GLuint v2);
+GLAPI void APIENTRY glProgramUniform3uiv (GLuint program, GLint location, GLsizei count, const GLuint *value);
+GLAPI void APIENTRY glProgramUniform4i (GLuint program, GLint location, GLint v0, GLint v1, GLint v2, GLint v3);
+GLAPI void APIENTRY glProgramUniform4iv (GLuint program, GLint location, GLsizei count, const GLint *value);
+GLAPI void APIENTRY glProgramUniform4f (GLuint program, GLint location, GLfloat v0, GLfloat v1, GLfloat v2, GLfloat v3);
+GLAPI void APIENTRY glProgramUniform4fv (GLuint program, GLint location, GLsizei count, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniform4d (GLuint program, GLint location, GLdouble v0, GLdouble v1, GLdouble v2, GLdouble v3);
+GLAPI void APIENTRY glProgramUniform4dv (GLuint program, GLint location, GLsizei count, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniform4ui (GLuint program, GLint location, GLuint v0, GLuint v1, GLuint v2, GLuint v3);
+GLAPI void APIENTRY glProgramUniform4uiv (GLuint program, GLint location, GLsizei count, const GLuint *value);
+GLAPI void APIENTRY glProgramUniformMatrix2fv (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniformMatrix3fv (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniformMatrix4fv (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniformMatrix2dv (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniformMatrix3dv (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniformMatrix4dv (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniformMatrix2x3fv (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniformMatrix3x2fv (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniformMatrix2x4fv (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniformMatrix4x2fv (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniformMatrix3x4fv (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniformMatrix4x3fv (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniformMatrix2x3dv (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniformMatrix3x2dv (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniformMatrix2x4dv (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniformMatrix4x2dv (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniformMatrix3x4dv (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniformMatrix4x3dv (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glValidateProgramPipeline (GLuint pipeline);
+GLAPI void APIENTRY glGetProgramPipelineInfoLog (GLuint pipeline, GLsizei bufSize, GLsizei *length, GLchar *infoLog);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLUSEPROGRAMSTAGESPROC) (GLuint pipeline, GLbitfield stages, GLuint program);
+typedef void (APIENTRYP PFNGLACTIVESHADERPROGRAMPROC) (GLuint pipeline, GLuint program);
+typedef GLuint (APIENTRYP PFNGLCREATESHADERPROGRAMVPROC) (GLenum type, GLsizei count, const GLchar* *strings);
+typedef void (APIENTRYP PFNGLBINDPROGRAMPIPELINEPROC) (GLuint pipeline);
+typedef void (APIENTRYP PFNGLDELETEPROGRAMPIPELINESPROC) (GLsizei n, const GLuint *pipelines);
+typedef void (APIENTRYP PFNGLGENPROGRAMPIPELINESPROC) (GLsizei n, GLuint *pipelines);
+typedef GLboolean (APIENTRYP PFNGLISPROGRAMPIPELINEPROC) (GLuint pipeline);
+typedef void (APIENTRYP PFNGLGETPROGRAMPIPELINEIVPROC) (GLuint pipeline, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1IPROC) (GLuint program, GLint location, GLint v0);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1IVPROC) (GLuint program, GLint location, GLsizei count, const GLint *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1FPROC) (GLuint program, GLint location, GLfloat v0);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1FVPROC) (GLuint program, GLint location, GLsizei count, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1DPROC) (GLuint program, GLint location, GLdouble v0);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1DVPROC) (GLuint program, GLint location, GLsizei count, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1UIPROC) (GLuint program, GLint location, GLuint v0);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1UIVPROC) (GLuint program, GLint location, GLsizei count, const GLuint *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2IPROC) (GLuint program, GLint location, GLint v0, GLint v1);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2IVPROC) (GLuint program, GLint location, GLsizei count, const GLint *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2FPROC) (GLuint program, GLint location, GLfloat v0, GLfloat v1);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2FVPROC) (GLuint program, GLint location, GLsizei count, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2DPROC) (GLuint program, GLint location, GLdouble v0, GLdouble v1);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2DVPROC) (GLuint program, GLint location, GLsizei count, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2UIPROC) (GLuint program, GLint location, GLuint v0, GLuint v1);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2UIVPROC) (GLuint program, GLint location, GLsizei count, const GLuint *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3IPROC) (GLuint program, GLint location, GLint v0, GLint v1, GLint v2);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3IVPROC) (GLuint program, GLint location, GLsizei count, const GLint *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3FPROC) (GLuint program, GLint location, GLfloat v0, GLfloat v1, GLfloat v2);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3FVPROC) (GLuint program, GLint location, GLsizei count, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3DPROC) (GLuint program, GLint location, GLdouble v0, GLdouble v1, GLdouble v2);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3DVPROC) (GLuint program, GLint location, GLsizei count, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3UIPROC) (GLuint program, GLint location, GLuint v0, GLuint v1, GLuint v2);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3UIVPROC) (GLuint program, GLint location, GLsizei count, const GLuint *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4IPROC) (GLuint program, GLint location, GLint v0, GLint v1, GLint v2, GLint v3);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4IVPROC) (GLuint program, GLint location, GLsizei count, const GLint *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4FPROC) (GLuint program, GLint location, GLfloat v0, GLfloat v1, GLfloat v2, GLfloat v3);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4FVPROC) (GLuint program, GLint location, GLsizei count, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4DPROC) (GLuint program, GLint location, GLdouble v0, GLdouble v1, GLdouble v2, GLdouble v3);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4DVPROC) (GLuint program, GLint location, GLsizei count, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4UIPROC) (GLuint program, GLint location, GLuint v0, GLuint v1, GLuint v2, GLuint v3);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4UIVPROC) (GLuint program, GLint location, GLsizei count, const GLuint *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX2FVPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX3FVPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX4FVPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX2DVPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX3DVPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX4DVPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX2X3FVPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX3X2FVPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX2X4FVPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX4X2FVPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX3X4FVPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX4X3FVPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX2X3DVPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX3X2DVPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX2X4DVPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX4X2DVPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX3X4DVPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX4X3DVPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLVALIDATEPROGRAMPIPELINEPROC) (GLuint pipeline);
+typedef void (APIENTRYP PFNGLGETPROGRAMPIPELINEINFOLOGPROC) (GLuint pipeline, GLsizei bufSize, GLsizei *length, GLchar *infoLog);
+#endif
+
+#ifndef GL_ARB_vertex_attrib_64bit
+#define GL_ARB_vertex_attrib_64bit 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glVertexAttribL1d (GLuint index, GLdouble x);
+GLAPI void APIENTRY glVertexAttribL2d (GLuint index, GLdouble x, GLdouble y);
+GLAPI void APIENTRY glVertexAttribL3d (GLuint index, GLdouble x, GLdouble y, GLdouble z);
+GLAPI void APIENTRY glVertexAttribL4d (GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+GLAPI void APIENTRY glVertexAttribL1dv (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttribL2dv (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttribL3dv (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttribL4dv (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttribLPointer (GLuint index, GLint size, GLenum type, GLsizei stride, const GLvoid *pointer);
+GLAPI void APIENTRY glGetVertexAttribLdv (GLuint index, GLenum pname, GLdouble *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL1DPROC) (GLuint index, GLdouble x);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL2DPROC) (GLuint index, GLdouble x, GLdouble y);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL3DPROC) (GLuint index, GLdouble x, GLdouble y, GLdouble z);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL4DPROC) (GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL1DVPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL2DVPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL3DVPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL4DVPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBLPOINTERPROC) (GLuint index, GLint size, GLenum type, GLsizei stride, const GLvoid *pointer);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBLDVPROC) (GLuint index, GLenum pname, GLdouble *params);
+#endif
+
+#ifndef GL_ARB_viewport_array
+#define GL_ARB_viewport_array 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glViewportArrayv (GLuint first, GLsizei count, const GLfloat *v);
+GLAPI void APIENTRY glViewportIndexedf (GLuint index, GLfloat x, GLfloat y, GLfloat w, GLfloat h);
+GLAPI void APIENTRY glViewportIndexedfv (GLuint index, const GLfloat *v);
+GLAPI void APIENTRY glScissorArrayv (GLuint first, GLsizei count, const GLint *v);
+GLAPI void APIENTRY glScissorIndexed (GLuint index, GLint left, GLint bottom, GLsizei width, GLsizei height);
+GLAPI void APIENTRY glScissorIndexedv (GLuint index, const GLint *v);
+GLAPI void APIENTRY glDepthRangeArrayv (GLuint first, GLsizei count, const GLclampd *v);
+GLAPI void APIENTRY glDepthRangeIndexed (GLuint index, GLclampd n, GLclampd f);
+GLAPI void APIENTRY glGetFloati_v (GLenum target, GLuint index, GLfloat *data);
+GLAPI void APIENTRY glGetDoublei_v (GLenum target, GLuint index, GLdouble *data);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLVIEWPORTARRAYVPROC) (GLuint first, GLsizei count, const GLfloat *v);
+typedef void (APIENTRYP PFNGLVIEWPORTINDEXEDFPROC) (GLuint index, GLfloat x, GLfloat y, GLfloat w, GLfloat h);
+typedef void (APIENTRYP PFNGLVIEWPORTINDEXEDFVPROC) (GLuint index, const GLfloat *v);
+typedef void (APIENTRYP PFNGLSCISSORARRAYVPROC) (GLuint first, GLsizei count, const GLint *v);
+typedef void (APIENTRYP PFNGLSCISSORINDEXEDPROC) (GLuint index, GLint left, GLint bottom, GLsizei width, GLsizei height);
+typedef void (APIENTRYP PFNGLSCISSORINDEXEDVPROC) (GLuint index, const GLint *v);
+typedef void (APIENTRYP PFNGLDEPTHRANGEARRAYVPROC) (GLuint first, GLsizei count, const GLclampd *v);
+typedef void (APIENTRYP PFNGLDEPTHRANGEINDEXEDPROC) (GLuint index, GLclampd n, GLclampd f);
+typedef void (APIENTRYP PFNGLGETFLOATI_VPROC) (GLenum target, GLuint index, GLfloat *data);
+typedef void (APIENTRYP PFNGLGETDOUBLEI_VPROC) (GLenum target, GLuint index, GLdouble *data);
+#endif
+
+#ifndef GL_ARB_cl_event
+#define GL_ARB_cl_event 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI GLsync APIENTRY glCreateSyncFromCLeventARB (struct _cl_context * context, struct _cl_event * event, GLbitfield flags);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef GLsync (APIENTRYP PFNGLCREATESYNCFROMCLEVENTARBPROC) (struct _cl_context * context, struct _cl_event * event, GLbitfield flags);
+#endif
+
+#ifndef GL_ARB_debug_output
+#define GL_ARB_debug_output 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glDebugMessageControlARB (GLenum source, GLenum type, GLenum severity, GLsizei count, const GLuint *ids, GLboolean enabled);
+GLAPI void APIENTRY glDebugMessageInsertARB (GLenum source, GLenum type, GLuint id, GLenum severity, GLsizei length, const GLchar *buf);
+GLAPI void APIENTRY glDebugMessageCallbackARB (GLDEBUGPROCARB callback, const GLvoid *userParam);
+GLAPI GLuint APIENTRY glGetDebugMessageLogARB (GLuint count, GLsizei bufsize, GLenum *sources, GLenum *types, GLuint *ids, GLenum *severities, GLsizei *lengths, GLchar *messageLog);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLDEBUGMESSAGECONTROLARBPROC) (GLenum source, GLenum type, GLenum severity, GLsizei count, const GLuint *ids, GLboolean enabled);
+typedef void (APIENTRYP PFNGLDEBUGMESSAGEINSERTARBPROC) (GLenum source, GLenum type, GLuint id, GLenum severity, GLsizei length, const GLchar *buf);
+typedef void (APIENTRYP PFNGLDEBUGMESSAGECALLBACKARBPROC) (GLDEBUGPROCARB callback, const GLvoid *userParam);
+typedef GLuint (APIENTRYP PFNGLGETDEBUGMESSAGELOGARBPROC) (GLuint count, GLsizei bufsize, GLenum *sources, GLenum *types, GLuint *ids, GLenum *severities, GLsizei *lengths, GLchar *messageLog);
+#endif
+
+#ifndef GL_ARB_robustness
+#define GL_ARB_robustness 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI GLenum APIENTRY glGetGraphicsResetStatusARB (void);
+GLAPI void APIENTRY glGetnMapdvARB (GLenum target, GLenum query, GLsizei bufSize, GLdouble *v);
+GLAPI void APIENTRY glGetnMapfvARB (GLenum target, GLenum query, GLsizei bufSize, GLfloat *v);
+GLAPI void APIENTRY glGetnMapivARB (GLenum target, GLenum query, GLsizei bufSize, GLint *v);
+GLAPI void APIENTRY glGetnPixelMapfvARB (GLenum map, GLsizei bufSize, GLfloat *values);
+GLAPI void APIENTRY glGetnPixelMapuivARB (GLenum map, GLsizei bufSize, GLuint *values);
+GLAPI void APIENTRY glGetnPixelMapusvARB (GLenum map, GLsizei bufSize, GLushort *values);
+GLAPI void APIENTRY glGetnPolygonStippleARB (GLsizei bufSize, GLubyte *pattern);
+GLAPI void APIENTRY glGetnColorTableARB (GLenum target, GLenum format, GLenum type, GLsizei bufSize, GLvoid *table);
+GLAPI void APIENTRY glGetnConvolutionFilterARB (GLenum target, GLenum format, GLenum type, GLsizei bufSize, GLvoid *image);
+GLAPI void APIENTRY glGetnSeparableFilterARB (GLenum target, GLenum format, GLenum type, GLsizei rowBufSize, GLvoid *row, GLsizei columnBufSize, GLvoid *column, GLvoid *span);
+GLAPI void APIENTRY glGetnHistogramARB (GLenum target, GLboolean reset, GLenum format, GLenum type, GLsizei bufSize, GLvoid *values);
+GLAPI void APIENTRY glGetnMinmaxARB (GLenum target, GLboolean reset, GLenum format, GLenum type, GLsizei bufSize, GLvoid *values);
+GLAPI void APIENTRY glGetnTexImageARB (GLenum target, GLint level, GLenum format, GLenum type, GLsizei bufSize, GLvoid *img);
+GLAPI void APIENTRY glReadnPixelsARB (GLint x, GLint y, GLsizei width, GLsizei height, GLenum format, GLenum type, GLsizei bufSize, GLvoid *data);
+GLAPI void APIENTRY glGetnCompressedTexImageARB (GLenum target, GLint lod, GLsizei bufSize, GLvoid *img);
+GLAPI void APIENTRY glGetnUniformfvARB (GLuint program, GLint location, GLsizei bufSize, GLfloat *params);
+GLAPI void APIENTRY glGetnUniformivARB (GLuint program, GLint location, GLsizei bufSize, GLint *params);
+GLAPI void APIENTRY glGetnUniformuivARB (GLuint program, GLint location, GLsizei bufSize, GLuint *params);
+GLAPI void APIENTRY glGetnUniformdvARB (GLuint program, GLint location, GLsizei bufSize, GLdouble *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef GLenum (APIENTRYP PFNGLGETGRAPHICSRESETSTATUSARBPROC) (void);
+typedef void (APIENTRYP PFNGLGETNMAPDVARBPROC) (GLenum target, GLenum query, GLsizei bufSize, GLdouble *v);
+typedef void (APIENTRYP PFNGLGETNMAPFVARBPROC) (GLenum target, GLenum query, GLsizei bufSize, GLfloat *v);
+typedef void (APIENTRYP PFNGLGETNMAPIVARBPROC) (GLenum target, GLenum query, GLsizei bufSize, GLint *v);
+typedef void (APIENTRYP PFNGLGETNPIXELMAPFVARBPROC) (GLenum map, GLsizei bufSize, GLfloat *values);
+typedef void (APIENTRYP PFNGLGETNPIXELMAPUIVARBPROC) (GLenum map, GLsizei bufSize, GLuint *values);
+typedef void (APIENTRYP PFNGLGETNPIXELMAPUSVARBPROC) (GLenum map, GLsizei bufSize, GLushort *values);
+typedef void (APIENTRYP PFNGLGETNPOLYGONSTIPPLEARBPROC) (GLsizei bufSize, GLubyte *pattern);
+typedef void (APIENTRYP PFNGLGETNCOLORTABLEARBPROC) (GLenum target, GLenum format, GLenum type, GLsizei bufSize, GLvoid *table);
+typedef void (APIENTRYP PFNGLGETNCONVOLUTIONFILTERARBPROC) (GLenum target, GLenum format, GLenum type, GLsizei bufSize, GLvoid *image);
+typedef void (APIENTRYP PFNGLGETNSEPARABLEFILTERARBPROC) (GLenum target, GLenum format, GLenum type, GLsizei rowBufSize, GLvoid *row, GLsizei columnBufSize, GLvoid *column, GLvoid *span);
+typedef void (APIENTRYP PFNGLGETNHISTOGRAMARBPROC) (GLenum target, GLboolean reset, GLenum format, GLenum type, GLsizei bufSize, GLvoid *values);
+typedef void (APIENTRYP PFNGLGETNMINMAXARBPROC) (GLenum target, GLboolean reset, GLenum format, GLenum type, GLsizei bufSize, GLvoid *values);
+typedef void (APIENTRYP PFNGLGETNTEXIMAGEARBPROC) (GLenum target, GLint level, GLenum format, GLenum type, GLsizei bufSize, GLvoid *img);
+typedef void (APIENTRYP PFNGLREADNPIXELSARBPROC) (GLint x, GLint y, GLsizei width, GLsizei height, GLenum format, GLenum type, GLsizei bufSize, GLvoid *data);
+typedef void (APIENTRYP PFNGLGETNCOMPRESSEDTEXIMAGEARBPROC) (GLenum target, GLint lod, GLsizei bufSize, GLvoid *img);
+typedef void (APIENTRYP PFNGLGETNUNIFORMFVARBPROC) (GLuint program, GLint location, GLsizei bufSize, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETNUNIFORMIVARBPROC) (GLuint program, GLint location, GLsizei bufSize, GLint *params);
+typedef void (APIENTRYP PFNGLGETNUNIFORMUIVARBPROC) (GLuint program, GLint location, GLsizei bufSize, GLuint *params);
+typedef void (APIENTRYP PFNGLGETNUNIFORMDVARBPROC) (GLuint program, GLint location, GLsizei bufSize, GLdouble *params);
+#endif
+
+#ifndef GL_ARB_shader_stencil_export
+#define GL_ARB_shader_stencil_export 1
+#endif
+
+#ifndef GL_EXT_abgr
+#define GL_EXT_abgr 1
+#endif
+
+#ifndef GL_EXT_blend_color
+#define GL_EXT_blend_color 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBlendColorEXT (GLclampf red, GLclampf green, GLclampf blue, GLclampf alpha);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBLENDCOLOREXTPROC) (GLclampf red, GLclampf green, GLclampf blue, GLclampf alpha);
+#endif
+
+#ifndef GL_EXT_polygon_offset
+#define GL_EXT_polygon_offset 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glPolygonOffsetEXT (GLfloat factor, GLfloat bias);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPOLYGONOFFSETEXTPROC) (GLfloat factor, GLfloat bias);
+#endif
+
+#ifndef GL_EXT_texture
+#define GL_EXT_texture 1
+#endif
+
+#ifndef GL_EXT_texture3D
+#define GL_EXT_texture3D 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glTexImage3DEXT (GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLsizei depth, GLint border, GLenum format, GLenum type, const GLvoid *pixels);
+GLAPI void APIENTRY glTexSubImage3DEXT (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLsizei width, GLsizei height, GLsizei depth, GLenum format, GLenum type, const GLvoid *pixels);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLTEXIMAGE3DEXTPROC) (GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLsizei depth, GLint border, GLenum format, GLenum type, const GLvoid *pixels);
+typedef void (APIENTRYP PFNGLTEXSUBIMAGE3DEXTPROC) (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLsizei width, GLsizei height, GLsizei depth, GLenum format, GLenum type, const GLvoid *pixels);
+#endif
+
+#ifndef GL_SGIS_texture_filter4
+#define GL_SGIS_texture_filter4 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glGetTexFilterFuncSGIS (GLenum target, GLenum filter, GLfloat *weights);
+GLAPI void APIENTRY glTexFilterFuncSGIS (GLenum target, GLenum filter, GLsizei n, const GLfloat *weights);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLGETTEXFILTERFUNCSGISPROC) (GLenum target, GLenum filter, GLfloat *weights);
+typedef void (APIENTRYP PFNGLTEXFILTERFUNCSGISPROC) (GLenum target, GLenum filter, GLsizei n, const GLfloat *weights);
+#endif
+
+#ifndef GL_EXT_subtexture
+#define GL_EXT_subtexture 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glTexSubImage1DEXT (GLenum target, GLint level, GLint xoffset, GLsizei width, GLenum format, GLenum type, const GLvoid *pixels);
+GLAPI void APIENTRY glTexSubImage2DEXT (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLsizei width, GLsizei height, GLenum format, GLenum type, const GLvoid *pixels);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLTEXSUBIMAGE1DEXTPROC) (GLenum target, GLint level, GLint xoffset, GLsizei width, GLenum format, GLenum type, const GLvoid *pixels);
+typedef void (APIENTRYP PFNGLTEXSUBIMAGE2DEXTPROC) (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLsizei width, GLsizei height, GLenum format, GLenum type, const GLvoid *pixels);
+#endif
+
+#ifndef GL_EXT_copy_texture
+#define GL_EXT_copy_texture 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glCopyTexImage1DEXT (GLenum target, GLint level, GLenum internalformat, GLint x, GLint y, GLsizei width, GLint border);
+GLAPI void APIENTRY glCopyTexImage2DEXT (GLenum target, GLint level, GLenum internalformat, GLint x, GLint y, GLsizei width, GLsizei height, GLint border);
+GLAPI void APIENTRY glCopyTexSubImage1DEXT (GLenum target, GLint level, GLint xoffset, GLint x, GLint y, GLsizei width);
+GLAPI void APIENTRY glCopyTexSubImage2DEXT (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint x, GLint y, GLsizei width, GLsizei height);
+GLAPI void APIENTRY glCopyTexSubImage3DEXT (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLint x, GLint y, GLsizei width, GLsizei height);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCOPYTEXIMAGE1DEXTPROC) (GLenum target, GLint level, GLenum internalformat, GLint x, GLint y, GLsizei width, GLint border);
+typedef void (APIENTRYP PFNGLCOPYTEXIMAGE2DEXTPROC) (GLenum target, GLint level, GLenum internalformat, GLint x, GLint y, GLsizei width, GLsizei height, GLint border);
+typedef void (APIENTRYP PFNGLCOPYTEXSUBIMAGE1DEXTPROC) (GLenum target, GLint level, GLint xoffset, GLint x, GLint y, GLsizei width);
+typedef void (APIENTRYP PFNGLCOPYTEXSUBIMAGE2DEXTPROC) (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint x, GLint y, GLsizei width, GLsizei height);
+typedef void (APIENTRYP PFNGLCOPYTEXSUBIMAGE3DEXTPROC) (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLint x, GLint y, GLsizei width, GLsizei height);
+#endif
+
+#ifndef GL_EXT_histogram
+#define GL_EXT_histogram 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glGetHistogramEXT (GLenum target, GLboolean reset, GLenum format, GLenum type, GLvoid *values);
+GLAPI void APIENTRY glGetHistogramParameterfvEXT (GLenum target, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetHistogramParameterivEXT (GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetMinmaxEXT (GLenum target, GLboolean reset, GLenum format, GLenum type, GLvoid *values);
+GLAPI void APIENTRY glGetMinmaxParameterfvEXT (GLenum target, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetMinmaxParameterivEXT (GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glHistogramEXT (GLenum target, GLsizei width, GLenum internalformat, GLboolean sink);
+GLAPI void APIENTRY glMinmaxEXT (GLenum target, GLenum internalformat, GLboolean sink);
+GLAPI void APIENTRY glResetHistogramEXT (GLenum target);
+GLAPI void APIENTRY glResetMinmaxEXT (GLenum target);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLGETHISTOGRAMEXTPROC) (GLenum target, GLboolean reset, GLenum format, GLenum type, GLvoid *values);
+typedef void (APIENTRYP PFNGLGETHISTOGRAMPARAMETERFVEXTPROC) (GLenum target, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETHISTOGRAMPARAMETERIVEXTPROC) (GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETMINMAXEXTPROC) (GLenum target, GLboolean reset, GLenum format, GLenum type, GLvoid *values);
+typedef void (APIENTRYP PFNGLGETMINMAXPARAMETERFVEXTPROC) (GLenum target, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETMINMAXPARAMETERIVEXTPROC) (GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLHISTOGRAMEXTPROC) (GLenum target, GLsizei width, GLenum internalformat, GLboolean sink);
+typedef void (APIENTRYP PFNGLMINMAXEXTPROC) (GLenum target, GLenum internalformat, GLboolean sink);
+typedef void (APIENTRYP PFNGLRESETHISTOGRAMEXTPROC) (GLenum target);
+typedef void (APIENTRYP PFNGLRESETMINMAXEXTPROC) (GLenum target);
+#endif
+
+#ifndef GL_EXT_convolution
+#define GL_EXT_convolution 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glConvolutionFilter1DEXT (GLenum target, GLenum internalformat, GLsizei width, GLenum format, GLenum type, const GLvoid *image);
+GLAPI void APIENTRY glConvolutionFilter2DEXT (GLenum target, GLenum internalformat, GLsizei width, GLsizei height, GLenum format, GLenum type, const GLvoid *image);
+GLAPI void APIENTRY glConvolutionParameterfEXT (GLenum target, GLenum pname, GLfloat params);
+GLAPI void APIENTRY glConvolutionParameterfvEXT (GLenum target, GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glConvolutionParameteriEXT (GLenum target, GLenum pname, GLint params);
+GLAPI void APIENTRY glConvolutionParameterivEXT (GLenum target, GLenum pname, const GLint *params);
+GLAPI void APIENTRY glCopyConvolutionFilter1DEXT (GLenum target, GLenum internalformat, GLint x, GLint y, GLsizei width);
+GLAPI void APIENTRY glCopyConvolutionFilter2DEXT (GLenum target, GLenum internalformat, GLint x, GLint y, GLsizei width, GLsizei height);
+GLAPI void APIENTRY glGetConvolutionFilterEXT (GLenum target, GLenum format, GLenum type, GLvoid *image);
+GLAPI void APIENTRY glGetConvolutionParameterfvEXT (GLenum target, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetConvolutionParameterivEXT (GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetSeparableFilterEXT (GLenum target, GLenum format, GLenum type, GLvoid *row, GLvoid *column, GLvoid *span);
+GLAPI void APIENTRY glSeparableFilter2DEXT (GLenum target, GLenum internalformat, GLsizei width, GLsizei height, GLenum format, GLenum type, const GLvoid *row, const GLvoid *column);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCONVOLUTIONFILTER1DEXTPROC) (GLenum target, GLenum internalformat, GLsizei width, GLenum format, GLenum type, const GLvoid *image);
+typedef void (APIENTRYP PFNGLCONVOLUTIONFILTER2DEXTPROC) (GLenum target, GLenum internalformat, GLsizei width, GLsizei height, GLenum format, GLenum type, const GLvoid *image);
+typedef void (APIENTRYP PFNGLCONVOLUTIONPARAMETERFEXTPROC) (GLenum target, GLenum pname, GLfloat params);
+typedef void (APIENTRYP PFNGLCONVOLUTIONPARAMETERFVEXTPROC) (GLenum target, GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLCONVOLUTIONPARAMETERIEXTPROC) (GLenum target, GLenum pname, GLint params);
+typedef void (APIENTRYP PFNGLCONVOLUTIONPARAMETERIVEXTPROC) (GLenum target, GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLCOPYCONVOLUTIONFILTER1DEXTPROC) (GLenum target, GLenum internalformat, GLint x, GLint y, GLsizei width);
+typedef void (APIENTRYP PFNGLCOPYCONVOLUTIONFILTER2DEXTPROC) (GLenum target, GLenum internalformat, GLint x, GLint y, GLsizei width, GLsizei height);
+typedef void (APIENTRYP PFNGLGETCONVOLUTIONFILTEREXTPROC) (GLenum target, GLenum format, GLenum type, GLvoid *image);
+typedef void (APIENTRYP PFNGLGETCONVOLUTIONPARAMETERFVEXTPROC) (GLenum target, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETCONVOLUTIONPARAMETERIVEXTPROC) (GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETSEPARABLEFILTEREXTPROC) (GLenum target, GLenum format, GLenum type, GLvoid *row, GLvoid *column, GLvoid *span);
+typedef void (APIENTRYP PFNGLSEPARABLEFILTER2DEXTPROC) (GLenum target, GLenum internalformat, GLsizei width, GLsizei height, GLenum format, GLenum type, const GLvoid *row, const GLvoid *column);
+#endif
+
+#ifndef GL_SGI_color_matrix
+#define GL_SGI_color_matrix 1
+#endif
+
+#ifndef GL_SGI_color_table
+#define GL_SGI_color_table 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glColorTableSGI (GLenum target, GLenum internalformat, GLsizei width, GLenum format, GLenum type, const GLvoid *table);
+GLAPI void APIENTRY glColorTableParameterfvSGI (GLenum target, GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glColorTableParameterivSGI (GLenum target, GLenum pname, const GLint *params);
+GLAPI void APIENTRY glCopyColorTableSGI (GLenum target, GLenum internalformat, GLint x, GLint y, GLsizei width);
+GLAPI void APIENTRY glGetColorTableSGI (GLenum target, GLenum format, GLenum type, GLvoid *table);
+GLAPI void APIENTRY glGetColorTableParameterfvSGI (GLenum target, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetColorTableParameterivSGI (GLenum target, GLenum pname, GLint *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCOLORTABLESGIPROC) (GLenum target, GLenum internalformat, GLsizei width, GLenum format, GLenum type, const GLvoid *table);
+typedef void (APIENTRYP PFNGLCOLORTABLEPARAMETERFVSGIPROC) (GLenum target, GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLCOLORTABLEPARAMETERIVSGIPROC) (GLenum target, GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLCOPYCOLORTABLESGIPROC) (GLenum target, GLenum internalformat, GLint x, GLint y, GLsizei width);
+typedef void (APIENTRYP PFNGLGETCOLORTABLESGIPROC) (GLenum target, GLenum format, GLenum type, GLvoid *table);
+typedef void (APIENTRYP PFNGLGETCOLORTABLEPARAMETERFVSGIPROC) (GLenum target, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETCOLORTABLEPARAMETERIVSGIPROC) (GLenum target, GLenum pname, GLint *params);
+#endif
+
+#ifndef GL_SGIX_pixel_texture
+#define GL_SGIX_pixel_texture 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glPixelTexGenSGIX (GLenum mode);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPIXELTEXGENSGIXPROC) (GLenum mode);
+#endif
+
+#ifndef GL_SGIS_pixel_texture
+#define GL_SGIS_pixel_texture 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glPixelTexGenParameteriSGIS (GLenum pname, GLint param);
+GLAPI void APIENTRY glPixelTexGenParameterivSGIS (GLenum pname, const GLint *params);
+GLAPI void APIENTRY glPixelTexGenParameterfSGIS (GLenum pname, GLfloat param);
+GLAPI void APIENTRY glPixelTexGenParameterfvSGIS (GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glGetPixelTexGenParameterivSGIS (GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetPixelTexGenParameterfvSGIS (GLenum pname, GLfloat *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPIXELTEXGENPARAMETERISGISPROC) (GLenum pname, GLint param);
+typedef void (APIENTRYP PFNGLPIXELTEXGENPARAMETERIVSGISPROC) (GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLPIXELTEXGENPARAMETERFSGISPROC) (GLenum pname, GLfloat param);
+typedef void (APIENTRYP PFNGLPIXELTEXGENPARAMETERFVSGISPROC) (GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLGETPIXELTEXGENPARAMETERIVSGISPROC) (GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETPIXELTEXGENPARAMETERFVSGISPROC) (GLenum pname, GLfloat *params);
+#endif
+
+#ifndef GL_SGIS_texture4D
+#define GL_SGIS_texture4D 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glTexImage4DSGIS (GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLsizei depth, GLsizei size4d, GLint border, GLenum format, GLenum type, const GLvoid *pixels);
+GLAPI void APIENTRY glTexSubImage4DSGIS (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLint woffset, GLsizei width, GLsizei height, GLsizei depth, GLsizei size4d, GLenum format, GLenum type, const GLvoid *pixels);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLTEXIMAGE4DSGISPROC) (GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLsizei depth, GLsizei size4d, GLint border, GLenum format, GLenum type, const GLvoid *pixels);
+typedef void (APIENTRYP PFNGLTEXSUBIMAGE4DSGISPROC) (GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLint woffset, GLsizei width, GLsizei height, GLsizei depth, GLsizei size4d, GLenum format, GLenum type, const GLvoid *pixels);
+#endif
+
+#ifndef GL_SGI_texture_color_table
+#define GL_SGI_texture_color_table 1
+#endif
+
+#ifndef GL_EXT_cmyka
+#define GL_EXT_cmyka 1
+#endif
+
+#ifndef GL_EXT_texture_object
+#define GL_EXT_texture_object 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI GLboolean APIENTRY glAreTexturesResidentEXT (GLsizei n, const GLuint *textures, GLboolean *residences);
+GLAPI void APIENTRY glBindTextureEXT (GLenum target, GLuint texture);
+GLAPI void APIENTRY glDeleteTexturesEXT (GLsizei n, const GLuint *textures);
+GLAPI void APIENTRY glGenTexturesEXT (GLsizei n, GLuint *textures);
+GLAPI GLboolean APIENTRY glIsTextureEXT (GLuint texture);
+GLAPI void APIENTRY glPrioritizeTexturesEXT (GLsizei n, const GLuint *textures, const GLclampf *priorities);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef GLboolean (APIENTRYP PFNGLARETEXTURESRESIDENTEXTPROC) (GLsizei n, const GLuint *textures, GLboolean *residences);
+typedef void (APIENTRYP PFNGLBINDTEXTUREEXTPROC) (GLenum target, GLuint texture);
+typedef void (APIENTRYP PFNGLDELETETEXTURESEXTPROC) (GLsizei n, const GLuint *textures);
+typedef void (APIENTRYP PFNGLGENTEXTURESEXTPROC) (GLsizei n, GLuint *textures);
+typedef GLboolean (APIENTRYP PFNGLISTEXTUREEXTPROC) (GLuint texture);
+typedef void (APIENTRYP PFNGLPRIORITIZETEXTURESEXTPROC) (GLsizei n, const GLuint *textures, const GLclampf *priorities);
+#endif
+
+#ifndef GL_SGIS_detail_texture
+#define GL_SGIS_detail_texture 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glDetailTexFuncSGIS (GLenum target, GLsizei n, const GLfloat *points);
+GLAPI void APIENTRY glGetDetailTexFuncSGIS (GLenum target, GLfloat *points);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLDETAILTEXFUNCSGISPROC) (GLenum target, GLsizei n, const GLfloat *points);
+typedef void (APIENTRYP PFNGLGETDETAILTEXFUNCSGISPROC) (GLenum target, GLfloat *points);
+#endif
+
+#ifndef GL_SGIS_sharpen_texture
+#define GL_SGIS_sharpen_texture 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glSharpenTexFuncSGIS (GLenum target, GLsizei n, const GLfloat *points);
+GLAPI void APIENTRY glGetSharpenTexFuncSGIS (GLenum target, GLfloat *points);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLSHARPENTEXFUNCSGISPROC) (GLenum target, GLsizei n, const GLfloat *points);
+typedef void (APIENTRYP PFNGLGETSHARPENTEXFUNCSGISPROC) (GLenum target, GLfloat *points);
+#endif
+
+#ifndef GL_EXT_packed_pixels
+#define GL_EXT_packed_pixels 1
+#endif
+
+#ifndef GL_SGIS_texture_lod
+#define GL_SGIS_texture_lod 1
+#endif
+
+#ifndef GL_SGIS_multisample
+#define GL_SGIS_multisample 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glSampleMaskSGIS (GLclampf value, GLboolean invert);
+GLAPI void APIENTRY glSamplePatternSGIS (GLenum pattern);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLSAMPLEMASKSGISPROC) (GLclampf value, GLboolean invert);
+typedef void (APIENTRYP PFNGLSAMPLEPATTERNSGISPROC) (GLenum pattern);
+#endif
+
+#ifndef GL_EXT_rescale_normal
+#define GL_EXT_rescale_normal 1
+#endif
+
+#ifndef GL_EXT_vertex_array
+#define GL_EXT_vertex_array 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glArrayElementEXT (GLint i);
+GLAPI void APIENTRY glColorPointerEXT (GLint size, GLenum type, GLsizei stride, GLsizei count, const GLvoid *pointer);
+GLAPI void APIENTRY glDrawArraysEXT (GLenum mode, GLint first, GLsizei count);
+GLAPI void APIENTRY glEdgeFlagPointerEXT (GLsizei stride, GLsizei count, const GLboolean *pointer);
+GLAPI void APIENTRY glGetPointervEXT (GLenum pname, GLvoid* *params);
+GLAPI void APIENTRY glIndexPointerEXT (GLenum type, GLsizei stride, GLsizei count, const GLvoid *pointer);
+GLAPI void APIENTRY glNormalPointerEXT (GLenum type, GLsizei stride, GLsizei count, const GLvoid *pointer);
+GLAPI void APIENTRY glTexCoordPointerEXT (GLint size, GLenum type, GLsizei stride, GLsizei count, const GLvoid *pointer);
+GLAPI void APIENTRY glVertexPointerEXT (GLint size, GLenum type, GLsizei stride, GLsizei count, const GLvoid *pointer);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLARRAYELEMENTEXTPROC) (GLint i);
+typedef void (APIENTRYP PFNGLCOLORPOINTEREXTPROC) (GLint size, GLenum type, GLsizei stride, GLsizei count, const GLvoid *pointer);
+typedef void (APIENTRYP PFNGLDRAWARRAYSEXTPROC) (GLenum mode, GLint first, GLsizei count);
+typedef void (APIENTRYP PFNGLEDGEFLAGPOINTEREXTPROC) (GLsizei stride, GLsizei count, const GLboolean *pointer);
+typedef void (APIENTRYP PFNGLGETPOINTERVEXTPROC) (GLenum pname, GLvoid* *params);
+typedef void (APIENTRYP PFNGLINDEXPOINTEREXTPROC) (GLenum type, GLsizei stride, GLsizei count, const GLvoid *pointer);
+typedef void (APIENTRYP PFNGLNORMALPOINTEREXTPROC) (GLenum type, GLsizei stride, GLsizei count, const GLvoid *pointer);
+typedef void (APIENTRYP PFNGLTEXCOORDPOINTEREXTPROC) (GLint size, GLenum type, GLsizei stride, GLsizei count, const GLvoid *pointer);
+typedef void (APIENTRYP PFNGLVERTEXPOINTEREXTPROC) (GLint size, GLenum type, GLsizei stride, GLsizei count, const GLvoid *pointer);
+#endif
+
+#ifndef GL_EXT_misc_attribute
+#define GL_EXT_misc_attribute 1
+#endif
+
+#ifndef GL_SGIS_generate_mipmap
+#define GL_SGIS_generate_mipmap 1
+#endif
+
+#ifndef GL_SGIX_clipmap
+#define GL_SGIX_clipmap 1
+#endif
+
+#ifndef GL_SGIX_shadow
+#define GL_SGIX_shadow 1
+#endif
+
+#ifndef GL_SGIS_texture_edge_clamp
+#define GL_SGIS_texture_edge_clamp 1
+#endif
+
+#ifndef GL_SGIS_texture_border_clamp
+#define GL_SGIS_texture_border_clamp 1
+#endif
+
+#ifndef GL_EXT_blend_minmax
+#define GL_EXT_blend_minmax 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBlendEquationEXT (GLenum mode);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBLENDEQUATIONEXTPROC) (GLenum mode);
+#endif
+
+#ifndef GL_EXT_blend_subtract
+#define GL_EXT_blend_subtract 1
+#endif
+
+#ifndef GL_EXT_blend_logic_op
+#define GL_EXT_blend_logic_op 1
+#endif
+
+#ifndef GL_SGIX_interlace
+#define GL_SGIX_interlace 1
+#endif
+
+#ifndef GL_SGIX_pixel_tiles
+#define GL_SGIX_pixel_tiles 1
+#endif
+
+#ifndef GL_SGIX_texture_select
+#define GL_SGIX_texture_select 1
+#endif
+
+#ifndef GL_SGIX_sprite
+#define GL_SGIX_sprite 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glSpriteParameterfSGIX (GLenum pname, GLfloat param);
+GLAPI void APIENTRY glSpriteParameterfvSGIX (GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glSpriteParameteriSGIX (GLenum pname, GLint param);
+GLAPI void APIENTRY glSpriteParameterivSGIX (GLenum pname, const GLint *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLSPRITEPARAMETERFSGIXPROC) (GLenum pname, GLfloat param);
+typedef void (APIENTRYP PFNGLSPRITEPARAMETERFVSGIXPROC) (GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLSPRITEPARAMETERISGIXPROC) (GLenum pname, GLint param);
+typedef void (APIENTRYP PFNGLSPRITEPARAMETERIVSGIXPROC) (GLenum pname, const GLint *params);
+#endif
+
+#ifndef GL_SGIX_texture_multi_buffer
+#define GL_SGIX_texture_multi_buffer 1
+#endif
+
+#ifndef GL_EXT_point_parameters
+#define GL_EXT_point_parameters 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glPointParameterfEXT (GLenum pname, GLfloat param);
+GLAPI void APIENTRY glPointParameterfvEXT (GLenum pname, const GLfloat *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPOINTPARAMETERFEXTPROC) (GLenum pname, GLfloat param);
+typedef void (APIENTRYP PFNGLPOINTPARAMETERFVEXTPROC) (GLenum pname, const GLfloat *params);
+#endif
+
+#ifndef GL_SGIS_point_parameters
+#define GL_SGIS_point_parameters 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glPointParameterfSGIS (GLenum pname, GLfloat param);
+GLAPI void APIENTRY glPointParameterfvSGIS (GLenum pname, const GLfloat *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPOINTPARAMETERFSGISPROC) (GLenum pname, GLfloat param);
+typedef void (APIENTRYP PFNGLPOINTPARAMETERFVSGISPROC) (GLenum pname, const GLfloat *params);
+#endif
+
+#ifndef GL_SGIX_instruments
+#define GL_SGIX_instruments 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI GLint APIENTRY glGetInstrumentsSGIX (void);
+GLAPI void APIENTRY glInstrumentsBufferSGIX (GLsizei size, GLint *buffer);
+GLAPI GLint APIENTRY glPollInstrumentsSGIX (GLint *marker_p);
+GLAPI void APIENTRY glReadInstrumentsSGIX (GLint marker);
+GLAPI void APIENTRY glStartInstrumentsSGIX (void);
+GLAPI void APIENTRY glStopInstrumentsSGIX (GLint marker);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef GLint (APIENTRYP PFNGLGETINSTRUMENTSSGIXPROC) (void);
+typedef void (APIENTRYP PFNGLINSTRUMENTSBUFFERSGIXPROC) (GLsizei size, GLint *buffer);
+typedef GLint (APIENTRYP PFNGLPOLLINSTRUMENTSSGIXPROC) (GLint *marker_p);
+typedef void (APIENTRYP PFNGLREADINSTRUMENTSSGIXPROC) (GLint marker);
+typedef void (APIENTRYP PFNGLSTARTINSTRUMENTSSGIXPROC) (void);
+typedef void (APIENTRYP PFNGLSTOPINSTRUMENTSSGIXPROC) (GLint marker);
+#endif
+
+#ifndef GL_SGIX_texture_scale_bias
+#define GL_SGIX_texture_scale_bias 1
+#endif
+
+#ifndef GL_SGIX_framezoom
+#define GL_SGIX_framezoom 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glFrameZoomSGIX (GLint factor);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLFRAMEZOOMSGIXPROC) (GLint factor);
+#endif
+
+#ifndef GL_SGIX_tag_sample_buffer
+#define GL_SGIX_tag_sample_buffer 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glTagSampleBufferSGIX (void);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLTAGSAMPLEBUFFERSGIXPROC) (void);
+#endif
+
+#ifndef GL_SGIX_polynomial_ffd
+#define GL_SGIX_polynomial_ffd 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glDeformationMap3dSGIX (GLenum target, GLdouble u1, GLdouble u2, GLint ustride, GLint uorder, GLdouble v1, GLdouble v2, GLint vstride, GLint vorder, GLdouble w1, GLdouble w2, GLint wstride, GLint worder, const GLdouble *points);
+GLAPI void APIENTRY glDeformationMap3fSGIX (GLenum target, GLfloat u1, GLfloat u2, GLint ustride, GLint uorder, GLfloat v1, GLfloat v2, GLint vstride, GLint vorder, GLfloat w1, GLfloat w2, GLint wstride, GLint worder, const GLfloat *points);
+GLAPI void APIENTRY glDeformSGIX (GLbitfield mask);
+GLAPI void APIENTRY glLoadIdentityDeformationMapSGIX (GLbitfield mask);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLDEFORMATIONMAP3DSGIXPROC) (GLenum target, GLdouble u1, GLdouble u2, GLint ustride, GLint uorder, GLdouble v1, GLdouble v2, GLint vstride, GLint vorder, GLdouble w1, GLdouble w2, GLint wstride, GLint worder, const GLdouble *points);
+typedef void (APIENTRYP PFNGLDEFORMATIONMAP3FSGIXPROC) (GLenum target, GLfloat u1, GLfloat u2, GLint ustride, GLint uorder, GLfloat v1, GLfloat v2, GLint vstride, GLint vorder, GLfloat w1, GLfloat w2, GLint wstride, GLint worder, const GLfloat *points);
+typedef void (APIENTRYP PFNGLDEFORMSGIXPROC) (GLbitfield mask);
+typedef void (APIENTRYP PFNGLLOADIDENTITYDEFORMATIONMAPSGIXPROC) (GLbitfield mask);
+#endif
+
+#ifndef GL_SGIX_reference_plane
+#define GL_SGIX_reference_plane 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glReferencePlaneSGIX (const GLdouble *equation);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLREFERENCEPLANESGIXPROC) (const GLdouble *equation);
+#endif
+
+#ifndef GL_SGIX_flush_raster
+#define GL_SGIX_flush_raster 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glFlushRasterSGIX (void);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLFLUSHRASTERSGIXPROC) (void);
+#endif
+
+#ifndef GL_SGIX_depth_texture
+#define GL_SGIX_depth_texture 1
+#endif
+
+#ifndef GL_SGIS_fog_function
+#define GL_SGIS_fog_function 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glFogFuncSGIS (GLsizei n, const GLfloat *points);
+GLAPI void APIENTRY glGetFogFuncSGIS (GLfloat *points);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLFOGFUNCSGISPROC) (GLsizei n, const GLfloat *points);
+typedef void (APIENTRYP PFNGLGETFOGFUNCSGISPROC) (GLfloat *points);
+#endif
+
+#ifndef GL_SGIX_fog_offset
+#define GL_SGIX_fog_offset 1
+#endif
+
+#ifndef GL_HP_image_transform
+#define GL_HP_image_transform 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glImageTransformParameteriHP (GLenum target, GLenum pname, GLint param);
+GLAPI void APIENTRY glImageTransformParameterfHP (GLenum target, GLenum pname, GLfloat param);
+GLAPI void APIENTRY glImageTransformParameterivHP (GLenum target, GLenum pname, const GLint *params);
+GLAPI void APIENTRY glImageTransformParameterfvHP (GLenum target, GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glGetImageTransformParameterivHP (GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetImageTransformParameterfvHP (GLenum target, GLenum pname, GLfloat *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLIMAGETRANSFORMPARAMETERIHPPROC) (GLenum target, GLenum pname, GLint param);
+typedef void (APIENTRYP PFNGLIMAGETRANSFORMPARAMETERFHPPROC) (GLenum target, GLenum pname, GLfloat param);
+typedef void (APIENTRYP PFNGLIMAGETRANSFORMPARAMETERIVHPPROC) (GLenum target, GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLIMAGETRANSFORMPARAMETERFVHPPROC) (GLenum target, GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLGETIMAGETRANSFORMPARAMETERIVHPPROC) (GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETIMAGETRANSFORMPARAMETERFVHPPROC) (GLenum target, GLenum pname, GLfloat *params);
+#endif
+
+#ifndef GL_HP_convolution_border_modes
+#define GL_HP_convolution_border_modes 1
+#endif
+
+#ifndef GL_SGIX_texture_add_env
+#define GL_SGIX_texture_add_env 1
+#endif
+
+#ifndef GL_EXT_color_subtable
+#define GL_EXT_color_subtable 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glColorSubTableEXT (GLenum target, GLsizei start, GLsizei count, GLenum format, GLenum type, const GLvoid *data);
+GLAPI void APIENTRY glCopyColorSubTableEXT (GLenum target, GLsizei start, GLint x, GLint y, GLsizei width);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCOLORSUBTABLEEXTPROC) (GLenum target, GLsizei start, GLsizei count, GLenum format, GLenum type, const GLvoid *data);
+typedef void (APIENTRYP PFNGLCOPYCOLORSUBTABLEEXTPROC) (GLenum target, GLsizei start, GLint x, GLint y, GLsizei width);
+#endif
+
+#ifndef GL_PGI_vertex_hints
+#define GL_PGI_vertex_hints 1
+#endif
+
+#ifndef GL_PGI_misc_hints
+#define GL_PGI_misc_hints 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glHintPGI (GLenum target, GLint mode);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLHINTPGIPROC) (GLenum target, GLint mode);
+#endif
+
+#ifndef GL_EXT_paletted_texture
+#define GL_EXT_paletted_texture 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glColorTableEXT (GLenum target, GLenum internalFormat, GLsizei width, GLenum format, GLenum type, const GLvoid *table);
+GLAPI void APIENTRY glGetColorTableEXT (GLenum target, GLenum format, GLenum type, GLvoid *data);
+GLAPI void APIENTRY glGetColorTableParameterivEXT (GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetColorTableParameterfvEXT (GLenum target, GLenum pname, GLfloat *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCOLORTABLEEXTPROC) (GLenum target, GLenum internalFormat, GLsizei width, GLenum format, GLenum type, const GLvoid *table);
+typedef void (APIENTRYP PFNGLGETCOLORTABLEEXTPROC) (GLenum target, GLenum format, GLenum type, GLvoid *data);
+typedef void (APIENTRYP PFNGLGETCOLORTABLEPARAMETERIVEXTPROC) (GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETCOLORTABLEPARAMETERFVEXTPROC) (GLenum target, GLenum pname, GLfloat *params);
+#endif
+
+#ifndef GL_EXT_clip_volume_hint
+#define GL_EXT_clip_volume_hint 1
+#endif
+
+#ifndef GL_SGIX_list_priority
+#define GL_SGIX_list_priority 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glGetListParameterfvSGIX (GLuint list, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetListParameterivSGIX (GLuint list, GLenum pname, GLint *params);
+GLAPI void APIENTRY glListParameterfSGIX (GLuint list, GLenum pname, GLfloat param);
+GLAPI void APIENTRY glListParameterfvSGIX (GLuint list, GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glListParameteriSGIX (GLuint list, GLenum pname, GLint param);
+GLAPI void APIENTRY glListParameterivSGIX (GLuint list, GLenum pname, const GLint *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLGETLISTPARAMETERFVSGIXPROC) (GLuint list, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETLISTPARAMETERIVSGIXPROC) (GLuint list, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLLISTPARAMETERFSGIXPROC) (GLuint list, GLenum pname, GLfloat param);
+typedef void (APIENTRYP PFNGLLISTPARAMETERFVSGIXPROC) (GLuint list, GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLLISTPARAMETERISGIXPROC) (GLuint list, GLenum pname, GLint param);
+typedef void (APIENTRYP PFNGLLISTPARAMETERIVSGIXPROC) (GLuint list, GLenum pname, const GLint *params);
+#endif
+
+#ifndef GL_SGIX_ir_instrument1
+#define GL_SGIX_ir_instrument1 1
+#endif
+
+#ifndef GL_SGIX_calligraphic_fragment
+#define GL_SGIX_calligraphic_fragment 1
+#endif
+
+#ifndef GL_SGIX_texture_lod_bias
+#define GL_SGIX_texture_lod_bias 1
+#endif
+
+#ifndef GL_SGIX_shadow_ambient
+#define GL_SGIX_shadow_ambient 1
+#endif
+
+#ifndef GL_EXT_index_texture
+#define GL_EXT_index_texture 1
+#endif
+
+#ifndef GL_EXT_index_material
+#define GL_EXT_index_material 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glIndexMaterialEXT (GLenum face, GLenum mode);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLINDEXMATERIALEXTPROC) (GLenum face, GLenum mode);
+#endif
+
+#ifndef GL_EXT_index_func
+#define GL_EXT_index_func 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glIndexFuncEXT (GLenum func, GLclampf ref);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLINDEXFUNCEXTPROC) (GLenum func, GLclampf ref);
+#endif
+
+#ifndef GL_EXT_index_array_formats
+#define GL_EXT_index_array_formats 1
+#endif
+
+#ifndef GL_EXT_compiled_vertex_array
+#define GL_EXT_compiled_vertex_array 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glLockArraysEXT (GLint first, GLsizei count);
+GLAPI void APIENTRY glUnlockArraysEXT (void);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLLOCKARRAYSEXTPROC) (GLint first, GLsizei count);
+typedef void (APIENTRYP PFNGLUNLOCKARRAYSEXTPROC) (void);
+#endif
+
+#ifndef GL_EXT_cull_vertex
+#define GL_EXT_cull_vertex 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glCullParameterdvEXT (GLenum pname, GLdouble *params);
+GLAPI void APIENTRY glCullParameterfvEXT (GLenum pname, GLfloat *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCULLPARAMETERDVEXTPROC) (GLenum pname, GLdouble *params);
+typedef void (APIENTRYP PFNGLCULLPARAMETERFVEXTPROC) (GLenum pname, GLfloat *params);
+#endif
+
+#ifndef GL_SGIX_ycrcb
+#define GL_SGIX_ycrcb 1
+#endif
+
+#ifndef GL_SGIX_fragment_lighting
+#define GL_SGIX_fragment_lighting 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glFragmentColorMaterialSGIX (GLenum face, GLenum mode);
+GLAPI void APIENTRY glFragmentLightfSGIX (GLenum light, GLenum pname, GLfloat param);
+GLAPI void APIENTRY glFragmentLightfvSGIX (GLenum light, GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glFragmentLightiSGIX (GLenum light, GLenum pname, GLint param);
+GLAPI void APIENTRY glFragmentLightivSGIX (GLenum light, GLenum pname, const GLint *params);
+GLAPI void APIENTRY glFragmentLightModelfSGIX (GLenum pname, GLfloat param);
+GLAPI void APIENTRY glFragmentLightModelfvSGIX (GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glFragmentLightModeliSGIX (GLenum pname, GLint param);
+GLAPI void APIENTRY glFragmentLightModelivSGIX (GLenum pname, const GLint *params);
+GLAPI void APIENTRY glFragmentMaterialfSGIX (GLenum face, GLenum pname, GLfloat param);
+GLAPI void APIENTRY glFragmentMaterialfvSGIX (GLenum face, GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glFragmentMaterialiSGIX (GLenum face, GLenum pname, GLint param);
+GLAPI void APIENTRY glFragmentMaterialivSGIX (GLenum face, GLenum pname, const GLint *params);
+GLAPI void APIENTRY glGetFragmentLightfvSGIX (GLenum light, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetFragmentLightivSGIX (GLenum light, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetFragmentMaterialfvSGIX (GLenum face, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetFragmentMaterialivSGIX (GLenum face, GLenum pname, GLint *params);
+GLAPI void APIENTRY glLightEnviSGIX (GLenum pname, GLint param);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLFRAGMENTCOLORMATERIALSGIXPROC) (GLenum face, GLenum mode);
+typedef void (APIENTRYP PFNGLFRAGMENTLIGHTFSGIXPROC) (GLenum light, GLenum pname, GLfloat param);
+typedef void (APIENTRYP PFNGLFRAGMENTLIGHTFVSGIXPROC) (GLenum light, GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLFRAGMENTLIGHTISGIXPROC) (GLenum light, GLenum pname, GLint param);
+typedef void (APIENTRYP PFNGLFRAGMENTLIGHTIVSGIXPROC) (GLenum light, GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLFRAGMENTLIGHTMODELFSGIXPROC) (GLenum pname, GLfloat param);
+typedef void (APIENTRYP PFNGLFRAGMENTLIGHTMODELFVSGIXPROC) (GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLFRAGMENTLIGHTMODELISGIXPROC) (GLenum pname, GLint param);
+typedef void (APIENTRYP PFNGLFRAGMENTLIGHTMODELIVSGIXPROC) (GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLFRAGMENTMATERIALFSGIXPROC) (GLenum face, GLenum pname, GLfloat param);
+typedef void (APIENTRYP PFNGLFRAGMENTMATERIALFVSGIXPROC) (GLenum face, GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLFRAGMENTMATERIALISGIXPROC) (GLenum face, GLenum pname, GLint param);
+typedef void (APIENTRYP PFNGLFRAGMENTMATERIALIVSGIXPROC) (GLenum face, GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLGETFRAGMENTLIGHTFVSGIXPROC) (GLenum light, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETFRAGMENTLIGHTIVSGIXPROC) (GLenum light, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETFRAGMENTMATERIALFVSGIXPROC) (GLenum face, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETFRAGMENTMATERIALIVSGIXPROC) (GLenum face, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLLIGHTENVISGIXPROC) (GLenum pname, GLint param);
+#endif
+
+#ifndef GL_IBM_rasterpos_clip
+#define GL_IBM_rasterpos_clip 1
+#endif
+
+#ifndef GL_HP_texture_lighting
+#define GL_HP_texture_lighting 1
+#endif
+
+#ifndef GL_EXT_draw_range_elements
+#define GL_EXT_draw_range_elements 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glDrawRangeElementsEXT (GLenum mode, GLuint start, GLuint end, GLsizei count, GLenum type, const GLvoid *indices);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLDRAWRANGEELEMENTSEXTPROC) (GLenum mode, GLuint start, GLuint end, GLsizei count, GLenum type, const GLvoid *indices);
+#endif
+
+#ifndef GL_WIN_phong_shading
+#define GL_WIN_phong_shading 1
+#endif
+
+#ifndef GL_WIN_specular_fog
+#define GL_WIN_specular_fog 1
+#endif
+
+#ifndef GL_EXT_light_texture
+#define GL_EXT_light_texture 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glApplyTextureEXT (GLenum mode);
+GLAPI void APIENTRY glTextureLightEXT (GLenum pname);
+GLAPI void APIENTRY glTextureMaterialEXT (GLenum face, GLenum mode);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLAPPLYTEXTUREEXTPROC) (GLenum mode);
+typedef void (APIENTRYP PFNGLTEXTURELIGHTEXTPROC) (GLenum pname);
+typedef void (APIENTRYP PFNGLTEXTUREMATERIALEXTPROC) (GLenum face, GLenum mode);
+#endif
+
+#ifndef GL_SGIX_blend_alpha_minmax
+#define GL_SGIX_blend_alpha_minmax 1
+#endif
+
+#ifndef GL_EXT_bgra
+#define GL_EXT_bgra 1
+#endif
+
+#ifndef GL_SGIX_async
+#define GL_SGIX_async 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glAsyncMarkerSGIX (GLuint marker);
+GLAPI GLint APIENTRY glFinishAsyncSGIX (GLuint *markerp);
+GLAPI GLint APIENTRY glPollAsyncSGIX (GLuint *markerp);
+GLAPI GLuint APIENTRY glGenAsyncMarkersSGIX (GLsizei range);
+GLAPI void APIENTRY glDeleteAsyncMarkersSGIX (GLuint marker, GLsizei range);
+GLAPI GLboolean APIENTRY glIsAsyncMarkerSGIX (GLuint marker);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLASYNCMARKERSGIXPROC) (GLuint marker);
+typedef GLint (APIENTRYP PFNGLFINISHASYNCSGIXPROC) (GLuint *markerp);
+typedef GLint (APIENTRYP PFNGLPOLLASYNCSGIXPROC) (GLuint *markerp);
+typedef GLuint (APIENTRYP PFNGLGENASYNCMARKERSSGIXPROC) (GLsizei range);
+typedef void (APIENTRYP PFNGLDELETEASYNCMARKERSSGIXPROC) (GLuint marker, GLsizei range);
+typedef GLboolean (APIENTRYP PFNGLISASYNCMARKERSGIXPROC) (GLuint marker);
+#endif
+
+#ifndef GL_SGIX_async_pixel
+#define GL_SGIX_async_pixel 1
+#endif
+
+#ifndef GL_SGIX_async_histogram
+#define GL_SGIX_async_histogram 1
+#endif
+
+#ifndef GL_INTEL_parallel_arrays
+#define GL_INTEL_parallel_arrays 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glVertexPointervINTEL (GLint size, GLenum type, const GLvoid* *pointer);
+GLAPI void APIENTRY glNormalPointervINTEL (GLenum type, const GLvoid* *pointer);
+GLAPI void APIENTRY glColorPointervINTEL (GLint size, GLenum type, const GLvoid* *pointer);
+GLAPI void APIENTRY glTexCoordPointervINTEL (GLint size, GLenum type, const GLvoid* *pointer);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLVERTEXPOINTERVINTELPROC) (GLint size, GLenum type, const GLvoid* *pointer);
+typedef void (APIENTRYP PFNGLNORMALPOINTERVINTELPROC) (GLenum type, const GLvoid* *pointer);
+typedef void (APIENTRYP PFNGLCOLORPOINTERVINTELPROC) (GLint size, GLenum type, const GLvoid* *pointer);
+typedef void (APIENTRYP PFNGLTEXCOORDPOINTERVINTELPROC) (GLint size, GLenum type, const GLvoid* *pointer);
+#endif
+
+#ifndef GL_HP_occlusion_test
+#define GL_HP_occlusion_test 1
+#endif
+
+#ifndef GL_EXT_pixel_transform
+#define GL_EXT_pixel_transform 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glPixelTransformParameteriEXT (GLenum target, GLenum pname, GLint param);
+GLAPI void APIENTRY glPixelTransformParameterfEXT (GLenum target, GLenum pname, GLfloat param);
+GLAPI void APIENTRY glPixelTransformParameterivEXT (GLenum target, GLenum pname, const GLint *params);
+GLAPI void APIENTRY glPixelTransformParameterfvEXT (GLenum target, GLenum pname, const GLfloat *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPIXELTRANSFORMPARAMETERIEXTPROC) (GLenum target, GLenum pname, GLint param);
+typedef void (APIENTRYP PFNGLPIXELTRANSFORMPARAMETERFEXTPROC) (GLenum target, GLenum pname, GLfloat param);
+typedef void (APIENTRYP PFNGLPIXELTRANSFORMPARAMETERIVEXTPROC) (GLenum target, GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLPIXELTRANSFORMPARAMETERFVEXTPROC) (GLenum target, GLenum pname, const GLfloat *params);
+#endif
+
+#ifndef GL_EXT_pixel_transform_color_table
+#define GL_EXT_pixel_transform_color_table 1
+#endif
+
+#ifndef GL_EXT_shared_texture_palette
+#define GL_EXT_shared_texture_palette 1
+#endif
+
+#ifndef GL_EXT_separate_specular_color
+#define GL_EXT_separate_specular_color 1
+#endif
+
+#ifndef GL_EXT_secondary_color
+#define GL_EXT_secondary_color 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glSecondaryColor3bEXT (GLbyte red, GLbyte green, GLbyte blue);
+GLAPI void APIENTRY glSecondaryColor3bvEXT (const GLbyte *v);
+GLAPI void APIENTRY glSecondaryColor3dEXT (GLdouble red, GLdouble green, GLdouble blue);
+GLAPI void APIENTRY glSecondaryColor3dvEXT (const GLdouble *v);
+GLAPI void APIENTRY glSecondaryColor3fEXT (GLfloat red, GLfloat green, GLfloat blue);
+GLAPI void APIENTRY glSecondaryColor3fvEXT (const GLfloat *v);
+GLAPI void APIENTRY glSecondaryColor3iEXT (GLint red, GLint green, GLint blue);
+GLAPI void APIENTRY glSecondaryColor3ivEXT (const GLint *v);
+GLAPI void APIENTRY glSecondaryColor3sEXT (GLshort red, GLshort green, GLshort blue);
+GLAPI void APIENTRY glSecondaryColor3svEXT (const GLshort *v);
+GLAPI void APIENTRY glSecondaryColor3ubEXT (GLubyte red, GLubyte green, GLubyte blue);
+GLAPI void APIENTRY glSecondaryColor3ubvEXT (const GLubyte *v);
+GLAPI void APIENTRY glSecondaryColor3uiEXT (GLuint red, GLuint green, GLuint blue);
+GLAPI void APIENTRY glSecondaryColor3uivEXT (const GLuint *v);
+GLAPI void APIENTRY glSecondaryColor3usEXT (GLushort red, GLushort green, GLushort blue);
+GLAPI void APIENTRY glSecondaryColor3usvEXT (const GLushort *v);
+GLAPI void APIENTRY glSecondaryColorPointerEXT (GLint size, GLenum type, GLsizei stride, const GLvoid *pointer);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3BEXTPROC) (GLbyte red, GLbyte green, GLbyte blue);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3BVEXTPROC) (const GLbyte *v);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3DEXTPROC) (GLdouble red, GLdouble green, GLdouble blue);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3DVEXTPROC) (const GLdouble *v);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3FEXTPROC) (GLfloat red, GLfloat green, GLfloat blue);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3FVEXTPROC) (const GLfloat *v);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3IEXTPROC) (GLint red, GLint green, GLint blue);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3IVEXTPROC) (const GLint *v);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3SEXTPROC) (GLshort red, GLshort green, GLshort blue);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3SVEXTPROC) (const GLshort *v);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3UBEXTPROC) (GLubyte red, GLubyte green, GLubyte blue);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3UBVEXTPROC) (const GLubyte *v);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3UIEXTPROC) (GLuint red, GLuint green, GLuint blue);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3UIVEXTPROC) (const GLuint *v);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3USEXTPROC) (GLushort red, GLushort green, GLushort blue);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3USVEXTPROC) (const GLushort *v);
+typedef void (APIENTRYP PFNGLSECONDARYCOLORPOINTEREXTPROC) (GLint size, GLenum type, GLsizei stride, const GLvoid *pointer);
+#endif
+
+#ifndef GL_EXT_texture_perturb_normal
+#define GL_EXT_texture_perturb_normal 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glTextureNormalEXT (GLenum mode);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLTEXTURENORMALEXTPROC) (GLenum mode);
+#endif
+
+#ifndef GL_EXT_multi_draw_arrays
+#define GL_EXT_multi_draw_arrays 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glMultiDrawArraysEXT (GLenum mode, const GLint *first, const GLsizei *count, GLsizei primcount);
+GLAPI void APIENTRY glMultiDrawElementsEXT (GLenum mode, const GLsizei *count, GLenum type, const GLvoid* *indices, GLsizei primcount);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLMULTIDRAWARRAYSEXTPROC) (GLenum mode, const GLint *first, const GLsizei *count, GLsizei primcount);
+typedef void (APIENTRYP PFNGLMULTIDRAWELEMENTSEXTPROC) (GLenum mode, const GLsizei *count, GLenum type, const GLvoid* *indices, GLsizei primcount);
+#endif
+
+#ifndef GL_EXT_fog_coord
+#define GL_EXT_fog_coord 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glFogCoordfEXT (GLfloat coord);
+GLAPI void APIENTRY glFogCoordfvEXT (const GLfloat *coord);
+GLAPI void APIENTRY glFogCoorddEXT (GLdouble coord);
+GLAPI void APIENTRY glFogCoorddvEXT (const GLdouble *coord);
+GLAPI void APIENTRY glFogCoordPointerEXT (GLenum type, GLsizei stride, const GLvoid *pointer);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLFOGCOORDFEXTPROC) (GLfloat coord);
+typedef void (APIENTRYP PFNGLFOGCOORDFVEXTPROC) (const GLfloat *coord);
+typedef void (APIENTRYP PFNGLFOGCOORDDEXTPROC) (GLdouble coord);
+typedef void (APIENTRYP PFNGLFOGCOORDDVEXTPROC) (const GLdouble *coord);
+typedef void (APIENTRYP PFNGLFOGCOORDPOINTEREXTPROC) (GLenum type, GLsizei stride, const GLvoid *pointer);
+#endif
+
+#ifndef GL_REND_screen_coordinates
+#define GL_REND_screen_coordinates 1
+#endif
+
+#ifndef GL_EXT_coordinate_frame
+#define GL_EXT_coordinate_frame 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glTangent3bEXT (GLbyte tx, GLbyte ty, GLbyte tz);
+GLAPI void APIENTRY glTangent3bvEXT (const GLbyte *v);
+GLAPI void APIENTRY glTangent3dEXT (GLdouble tx, GLdouble ty, GLdouble tz);
+GLAPI void APIENTRY glTangent3dvEXT (const GLdouble *v);
+GLAPI void APIENTRY glTangent3fEXT (GLfloat tx, GLfloat ty, GLfloat tz);
+GLAPI void APIENTRY glTangent3fvEXT (const GLfloat *v);
+GLAPI void APIENTRY glTangent3iEXT (GLint tx, GLint ty, GLint tz);
+GLAPI void APIENTRY glTangent3ivEXT (const GLint *v);
+GLAPI void APIENTRY glTangent3sEXT (GLshort tx, GLshort ty, GLshort tz);
+GLAPI void APIENTRY glTangent3svEXT (const GLshort *v);
+GLAPI void APIENTRY glBinormal3bEXT (GLbyte bx, GLbyte by, GLbyte bz);
+GLAPI void APIENTRY glBinormal3bvEXT (const GLbyte *v);
+GLAPI void APIENTRY glBinormal3dEXT (GLdouble bx, GLdouble by, GLdouble bz);
+GLAPI void APIENTRY glBinormal3dvEXT (const GLdouble *v);
+GLAPI void APIENTRY glBinormal3fEXT (GLfloat bx, GLfloat by, GLfloat bz);
+GLAPI void APIENTRY glBinormal3fvEXT (const GLfloat *v);
+GLAPI void APIENTRY glBinormal3iEXT (GLint bx, GLint by, GLint bz);
+GLAPI void APIENTRY glBinormal3ivEXT (const GLint *v);
+GLAPI void APIENTRY glBinormal3sEXT (GLshort bx, GLshort by, GLshort bz);
+GLAPI void APIENTRY glBinormal3svEXT (const GLshort *v);
+GLAPI void APIENTRY glTangentPointerEXT (GLenum type, GLsizei stride, const GLvoid *pointer);
+GLAPI void APIENTRY glBinormalPointerEXT (GLenum type, GLsizei stride, const GLvoid *pointer);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLTANGENT3BEXTPROC) (GLbyte tx, GLbyte ty, GLbyte tz);
+typedef void (APIENTRYP PFNGLTANGENT3BVEXTPROC) (const GLbyte *v);
+typedef void (APIENTRYP PFNGLTANGENT3DEXTPROC) (GLdouble tx, GLdouble ty, GLdouble tz);
+typedef void (APIENTRYP PFNGLTANGENT3DVEXTPROC) (const GLdouble *v);
+typedef void (APIENTRYP PFNGLTANGENT3FEXTPROC) (GLfloat tx, GLfloat ty, GLfloat tz);
+typedef void (APIENTRYP PFNGLTANGENT3FVEXTPROC) (const GLfloat *v);
+typedef void (APIENTRYP PFNGLTANGENT3IEXTPROC) (GLint tx, GLint ty, GLint tz);
+typedef void (APIENTRYP PFNGLTANGENT3IVEXTPROC) (const GLint *v);
+typedef void (APIENTRYP PFNGLTANGENT3SEXTPROC) (GLshort tx, GLshort ty, GLshort tz);
+typedef void (APIENTRYP PFNGLTANGENT3SVEXTPROC) (const GLshort *v);
+typedef void (APIENTRYP PFNGLBINORMAL3BEXTPROC) (GLbyte bx, GLbyte by, GLbyte bz);
+typedef void (APIENTRYP PFNGLBINORMAL3BVEXTPROC) (const GLbyte *v);
+typedef void (APIENTRYP PFNGLBINORMAL3DEXTPROC) (GLdouble bx, GLdouble by, GLdouble bz);
+typedef void (APIENTRYP PFNGLBINORMAL3DVEXTPROC) (const GLdouble *v);
+typedef void (APIENTRYP PFNGLBINORMAL3FEXTPROC) (GLfloat bx, GLfloat by, GLfloat bz);
+typedef void (APIENTRYP PFNGLBINORMAL3FVEXTPROC) (const GLfloat *v);
+typedef void (APIENTRYP PFNGLBINORMAL3IEXTPROC) (GLint bx, GLint by, GLint bz);
+typedef void (APIENTRYP PFNGLBINORMAL3IVEXTPROC) (const GLint *v);
+typedef void (APIENTRYP PFNGLBINORMAL3SEXTPROC) (GLshort bx, GLshort by, GLshort bz);
+typedef void (APIENTRYP PFNGLBINORMAL3SVEXTPROC) (const GLshort *v);
+typedef void (APIENTRYP PFNGLTANGENTPOINTEREXTPROC) (GLenum type, GLsizei stride, const GLvoid *pointer);
+typedef void (APIENTRYP PFNGLBINORMALPOINTEREXTPROC) (GLenum type, GLsizei stride, const GLvoid *pointer);
+#endif
+
+#ifndef GL_EXT_texture_env_combine
+#define GL_EXT_texture_env_combine 1
+#endif
+
+#ifndef GL_APPLE_specular_vector
+#define GL_APPLE_specular_vector 1
+#endif
+
+#ifndef GL_APPLE_transform_hint
+#define GL_APPLE_transform_hint 1
+#endif
+
+#ifndef GL_SGIX_fog_scale
+#define GL_SGIX_fog_scale 1
+#endif
+
+#ifndef GL_SUNX_constant_data
+#define GL_SUNX_constant_data 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glFinishTextureSUNX (void);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLFINISHTEXTURESUNXPROC) (void);
+#endif
+
+#ifndef GL_SUN_global_alpha
+#define GL_SUN_global_alpha 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glGlobalAlphaFactorbSUN (GLbyte factor);
+GLAPI void APIENTRY glGlobalAlphaFactorsSUN (GLshort factor);
+GLAPI void APIENTRY glGlobalAlphaFactoriSUN (GLint factor);
+GLAPI void APIENTRY glGlobalAlphaFactorfSUN (GLfloat factor);
+GLAPI void APIENTRY glGlobalAlphaFactordSUN (GLdouble factor);
+GLAPI void APIENTRY glGlobalAlphaFactorubSUN (GLubyte factor);
+GLAPI void APIENTRY glGlobalAlphaFactorusSUN (GLushort factor);
+GLAPI void APIENTRY glGlobalAlphaFactoruiSUN (GLuint factor);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLGLOBALALPHAFACTORBSUNPROC) (GLbyte factor);
+typedef void (APIENTRYP PFNGLGLOBALALPHAFACTORSSUNPROC) (GLshort factor);
+typedef void (APIENTRYP PFNGLGLOBALALPHAFACTORISUNPROC) (GLint factor);
+typedef void (APIENTRYP PFNGLGLOBALALPHAFACTORFSUNPROC) (GLfloat factor);
+typedef void (APIENTRYP PFNGLGLOBALALPHAFACTORDSUNPROC) (GLdouble factor);
+typedef void (APIENTRYP PFNGLGLOBALALPHAFACTORUBSUNPROC) (GLubyte factor);
+typedef void (APIENTRYP PFNGLGLOBALALPHAFACTORUSSUNPROC) (GLushort factor);
+typedef void (APIENTRYP PFNGLGLOBALALPHAFACTORUISUNPROC) (GLuint factor);
+#endif
+
+#ifndef GL_SUN_triangle_list
+#define GL_SUN_triangle_list 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glReplacementCodeuiSUN (GLuint code);
+GLAPI void APIENTRY glReplacementCodeusSUN (GLushort code);
+GLAPI void APIENTRY glReplacementCodeubSUN (GLubyte code);
+GLAPI void APIENTRY glReplacementCodeuivSUN (const GLuint *code);
+GLAPI void APIENTRY glReplacementCodeusvSUN (const GLushort *code);
+GLAPI void APIENTRY glReplacementCodeubvSUN (const GLubyte *code);
+GLAPI void APIENTRY glReplacementCodePointerSUN (GLenum type, GLsizei stride, const GLvoid* *pointer);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUISUNPROC) (GLuint code);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUSSUNPROC) (GLushort code);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUBSUNPROC) (GLubyte code);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUIVSUNPROC) (const GLuint *code);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUSVSUNPROC) (const GLushort *code);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUBVSUNPROC) (const GLubyte *code);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEPOINTERSUNPROC) (GLenum type, GLsizei stride, const GLvoid* *pointer);
+#endif
+
+#ifndef GL_SUN_vertex
+#define GL_SUN_vertex 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glColor4ubVertex2fSUN (GLubyte r, GLubyte g, GLubyte b, GLubyte a, GLfloat x, GLfloat y);
+GLAPI void APIENTRY glColor4ubVertex2fvSUN (const GLubyte *c, const GLfloat *v);
+GLAPI void APIENTRY glColor4ubVertex3fSUN (GLubyte r, GLubyte g, GLubyte b, GLubyte a, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glColor4ubVertex3fvSUN (const GLubyte *c, const GLfloat *v);
+GLAPI void APIENTRY glColor3fVertex3fSUN (GLfloat r, GLfloat g, GLfloat b, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glColor3fVertex3fvSUN (const GLfloat *c, const GLfloat *v);
+GLAPI void APIENTRY glNormal3fVertex3fSUN (GLfloat nx, GLfloat ny, GLfloat nz, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glNormal3fVertex3fvSUN (const GLfloat *n, const GLfloat *v);
+GLAPI void APIENTRY glColor4fNormal3fVertex3fSUN (GLfloat r, GLfloat g, GLfloat b, GLfloat a, GLfloat nx, GLfloat ny, GLfloat nz, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glColor4fNormal3fVertex3fvSUN (const GLfloat *c, const GLfloat *n, const GLfloat *v);
+GLAPI void APIENTRY glTexCoord2fVertex3fSUN (GLfloat s, GLfloat t, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glTexCoord2fVertex3fvSUN (const GLfloat *tc, const GLfloat *v);
+GLAPI void APIENTRY glTexCoord4fVertex4fSUN (GLfloat s, GLfloat t, GLfloat p, GLfloat q, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+GLAPI void APIENTRY glTexCoord4fVertex4fvSUN (const GLfloat *tc, const GLfloat *v);
+GLAPI void APIENTRY glTexCoord2fColor4ubVertex3fSUN (GLfloat s, GLfloat t, GLubyte r, GLubyte g, GLubyte b, GLubyte a, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glTexCoord2fColor4ubVertex3fvSUN (const GLfloat *tc, const GLubyte *c, const GLfloat *v);
+GLAPI void APIENTRY glTexCoord2fColor3fVertex3fSUN (GLfloat s, GLfloat t, GLfloat r, GLfloat g, GLfloat b, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glTexCoord2fColor3fVertex3fvSUN (const GLfloat *tc, const GLfloat *c, const GLfloat *v);
+GLAPI void APIENTRY glTexCoord2fNormal3fVertex3fSUN (GLfloat s, GLfloat t, GLfloat nx, GLfloat ny, GLfloat nz, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glTexCoord2fNormal3fVertex3fvSUN (const GLfloat *tc, const GLfloat *n, const GLfloat *v);
+GLAPI void APIENTRY glTexCoord2fColor4fNormal3fVertex3fSUN (GLfloat s, GLfloat t, GLfloat r, GLfloat g, GLfloat b, GLfloat a, GLfloat nx, GLfloat ny, GLfloat nz, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glTexCoord2fColor4fNormal3fVertex3fvSUN (const GLfloat *tc, const GLfloat *c, const GLfloat *n, const GLfloat *v);
+GLAPI void APIENTRY glTexCoord4fColor4fNormal3fVertex4fSUN (GLfloat s, GLfloat t, GLfloat p, GLfloat q, GLfloat r, GLfloat g, GLfloat b, GLfloat a, GLfloat nx, GLfloat ny, GLfloat nz, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+GLAPI void APIENTRY glTexCoord4fColor4fNormal3fVertex4fvSUN (const GLfloat *tc, const GLfloat *c, const GLfloat *n, const GLfloat *v);
+GLAPI void APIENTRY glReplacementCodeuiVertex3fSUN (GLuint rc, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glReplacementCodeuiVertex3fvSUN (const GLuint *rc, const GLfloat *v);
+GLAPI void APIENTRY glReplacementCodeuiColor4ubVertex3fSUN (GLuint rc, GLubyte r, GLubyte g, GLubyte b, GLubyte a, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glReplacementCodeuiColor4ubVertex3fvSUN (const GLuint *rc, const GLubyte *c, const GLfloat *v);
+GLAPI void APIENTRY glReplacementCodeuiColor3fVertex3fSUN (GLuint rc, GLfloat r, GLfloat g, GLfloat b, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glReplacementCodeuiColor3fVertex3fvSUN (const GLuint *rc, const GLfloat *c, const GLfloat *v);
+GLAPI void APIENTRY glReplacementCodeuiNormal3fVertex3fSUN (GLuint rc, GLfloat nx, GLfloat ny, GLfloat nz, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glReplacementCodeuiNormal3fVertex3fvSUN (const GLuint *rc, const GLfloat *n, const GLfloat *v);
+GLAPI void APIENTRY glReplacementCodeuiColor4fNormal3fVertex3fSUN (GLuint rc, GLfloat r, GLfloat g, GLfloat b, GLfloat a, GLfloat nx, GLfloat ny, GLfloat nz, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glReplacementCodeuiColor4fNormal3fVertex3fvSUN (const GLuint *rc, const GLfloat *c, const GLfloat *n, const GLfloat *v);
+GLAPI void APIENTRY glReplacementCodeuiTexCoord2fVertex3fSUN (GLuint rc, GLfloat s, GLfloat t, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glReplacementCodeuiTexCoord2fVertex3fvSUN (const GLuint *rc, const GLfloat *tc, const GLfloat *v);
+GLAPI void APIENTRY glReplacementCodeuiTexCoord2fNormal3fVertex3fSUN (GLuint rc, GLfloat s, GLfloat t, GLfloat nx, GLfloat ny, GLfloat nz, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glReplacementCodeuiTexCoord2fNormal3fVertex3fvSUN (const GLuint *rc, const GLfloat *tc, const GLfloat *n, const GLfloat *v);
+GLAPI void APIENTRY glReplacementCodeuiTexCoord2fColor4fNormal3fVertex3fSUN (GLuint rc, GLfloat s, GLfloat t, GLfloat r, GLfloat g, GLfloat b, GLfloat a, GLfloat nx, GLfloat ny, GLfloat nz, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glReplacementCodeuiTexCoord2fColor4fNormal3fVertex3fvSUN (const GLuint *rc, const GLfloat *tc, const GLfloat *c, const GLfloat *n, const GLfloat *v);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCOLOR4UBVERTEX2FSUNPROC) (GLubyte r, GLubyte g, GLubyte b, GLubyte a, GLfloat x, GLfloat y);
+typedef void (APIENTRYP PFNGLCOLOR4UBVERTEX2FVSUNPROC) (const GLubyte *c, const GLfloat *v);
+typedef void (APIENTRYP PFNGLCOLOR4UBVERTEX3FSUNPROC) (GLubyte r, GLubyte g, GLubyte b, GLubyte a, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLCOLOR4UBVERTEX3FVSUNPROC) (const GLubyte *c, const GLfloat *v);
+typedef void (APIENTRYP PFNGLCOLOR3FVERTEX3FSUNPROC) (GLfloat r, GLfloat g, GLfloat b, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLCOLOR3FVERTEX3FVSUNPROC) (const GLfloat *c, const GLfloat *v);
+typedef void (APIENTRYP PFNGLNORMAL3FVERTEX3FSUNPROC) (GLfloat nx, GLfloat ny, GLfloat nz, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLNORMAL3FVERTEX3FVSUNPROC) (const GLfloat *n, const GLfloat *v);
+typedef void (APIENTRYP PFNGLCOLOR4FNORMAL3FVERTEX3FSUNPROC) (GLfloat r, GLfloat g, GLfloat b, GLfloat a, GLfloat nx, GLfloat ny, GLfloat nz, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLCOLOR4FNORMAL3FVERTEX3FVSUNPROC) (const GLfloat *c, const GLfloat *n, const GLfloat *v);
+typedef void (APIENTRYP PFNGLTEXCOORD2FVERTEX3FSUNPROC) (GLfloat s, GLfloat t, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLTEXCOORD2FVERTEX3FVSUNPROC) (const GLfloat *tc, const GLfloat *v);
+typedef void (APIENTRYP PFNGLTEXCOORD4FVERTEX4FSUNPROC) (GLfloat s, GLfloat t, GLfloat p, GLfloat q, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+typedef void (APIENTRYP PFNGLTEXCOORD4FVERTEX4FVSUNPROC) (const GLfloat *tc, const GLfloat *v);
+typedef void (APIENTRYP PFNGLTEXCOORD2FCOLOR4UBVERTEX3FSUNPROC) (GLfloat s, GLfloat t, GLubyte r, GLubyte g, GLubyte b, GLubyte a, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLTEXCOORD2FCOLOR4UBVERTEX3FVSUNPROC) (const GLfloat *tc, const GLubyte *c, const GLfloat *v);
+typedef void (APIENTRYP PFNGLTEXCOORD2FCOLOR3FVERTEX3FSUNPROC) (GLfloat s, GLfloat t, GLfloat r, GLfloat g, GLfloat b, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLTEXCOORD2FCOLOR3FVERTEX3FVSUNPROC) (const GLfloat *tc, const GLfloat *c, const GLfloat *v);
+typedef void (APIENTRYP PFNGLTEXCOORD2FNORMAL3FVERTEX3FSUNPROC) (GLfloat s, GLfloat t, GLfloat nx, GLfloat ny, GLfloat nz, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLTEXCOORD2FNORMAL3FVERTEX3FVSUNPROC) (const GLfloat *tc, const GLfloat *n, const GLfloat *v);
+typedef void (APIENTRYP PFNGLTEXCOORD2FCOLOR4FNORMAL3FVERTEX3FSUNPROC) (GLfloat s, GLfloat t, GLfloat r, GLfloat g, GLfloat b, GLfloat a, GLfloat nx, GLfloat ny, GLfloat nz, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLTEXCOORD2FCOLOR4FNORMAL3FVERTEX3FVSUNPROC) (const GLfloat *tc, const GLfloat *c, const GLfloat *n, const GLfloat *v);
+typedef void (APIENTRYP PFNGLTEXCOORD4FCOLOR4FNORMAL3FVERTEX4FSUNPROC) (GLfloat s, GLfloat t, GLfloat p, GLfloat q, GLfloat r, GLfloat g, GLfloat b, GLfloat a, GLfloat nx, GLfloat ny, GLfloat nz, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+typedef void (APIENTRYP PFNGLTEXCOORD4FCOLOR4FNORMAL3FVERTEX4FVSUNPROC) (const GLfloat *tc, const GLfloat *c, const GLfloat *n, const GLfloat *v);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUIVERTEX3FSUNPROC) (GLuint rc, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUIVERTEX3FVSUNPROC) (const GLuint *rc, const GLfloat *v);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUICOLOR4UBVERTEX3FSUNPROC) (GLuint rc, GLubyte r, GLubyte g, GLubyte b, GLubyte a, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUICOLOR4UBVERTEX3FVSUNPROC) (const GLuint *rc, const GLubyte *c, const GLfloat *v);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUICOLOR3FVERTEX3FSUNPROC) (GLuint rc, GLfloat r, GLfloat g, GLfloat b, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUICOLOR3FVERTEX3FVSUNPROC) (const GLuint *rc, const GLfloat *c, const GLfloat *v);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUINORMAL3FVERTEX3FSUNPROC) (GLuint rc, GLfloat nx, GLfloat ny, GLfloat nz, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUINORMAL3FVERTEX3FVSUNPROC) (const GLuint *rc, const GLfloat *n, const GLfloat *v);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUICOLOR4FNORMAL3FVERTEX3FSUNPROC) (GLuint rc, GLfloat r, GLfloat g, GLfloat b, GLfloat a, GLfloat nx, GLfloat ny, GLfloat nz, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUICOLOR4FNORMAL3FVERTEX3FVSUNPROC) (const GLuint *rc, const GLfloat *c, const GLfloat *n, const GLfloat *v);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUITEXCOORD2FVERTEX3FSUNPROC) (GLuint rc, GLfloat s, GLfloat t, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUITEXCOORD2FVERTEX3FVSUNPROC) (const GLuint *rc, const GLfloat *tc, const GLfloat *v);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUITEXCOORD2FNORMAL3FVERTEX3FSUNPROC) (GLuint rc, GLfloat s, GLfloat t, GLfloat nx, GLfloat ny, GLfloat nz, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUITEXCOORD2FNORMAL3FVERTEX3FVSUNPROC) (const GLuint *rc, const GLfloat *tc, const GLfloat *n, const GLfloat *v);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUITEXCOORD2FCOLOR4FNORMAL3FVERTEX3FSUNPROC) (GLuint rc, GLfloat s, GLfloat t, GLfloat r, GLfloat g, GLfloat b, GLfloat a, GLfloat nx, GLfloat ny, GLfloat nz, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLREPLACEMENTCODEUITEXCOORD2FCOLOR4FNORMAL3FVERTEX3FVSUNPROC) (const GLuint *rc, const GLfloat *tc, const GLfloat *c, const GLfloat *n, const GLfloat *v);
+#endif
+
+#ifndef GL_EXT_blend_func_separate
+#define GL_EXT_blend_func_separate 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBlendFuncSeparateEXT (GLenum sfactorRGB, GLenum dfactorRGB, GLenum sfactorAlpha, GLenum dfactorAlpha);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBLENDFUNCSEPARATEEXTPROC) (GLenum sfactorRGB, GLenum dfactorRGB, GLenum sfactorAlpha, GLenum dfactorAlpha);
+#endif
+
+#ifndef GL_INGR_blend_func_separate
+#define GL_INGR_blend_func_separate 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBlendFuncSeparateINGR (GLenum sfactorRGB, GLenum dfactorRGB, GLenum sfactorAlpha, GLenum dfactorAlpha);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBLENDFUNCSEPARATEINGRPROC) (GLenum sfactorRGB, GLenum dfactorRGB, GLenum sfactorAlpha, GLenum dfactorAlpha);
+#endif
+
+#ifndef GL_INGR_color_clamp
+#define GL_INGR_color_clamp 1
+#endif
+
+#ifndef GL_INGR_interlace_read
+#define GL_INGR_interlace_read 1
+#endif
+
+#ifndef GL_EXT_stencil_wrap
+#define GL_EXT_stencil_wrap 1
+#endif
+
+#ifndef GL_EXT_422_pixels
+#define GL_EXT_422_pixels 1
+#endif
+
+#ifndef GL_NV_texgen_reflection
+#define GL_NV_texgen_reflection 1
+#endif
+
+#ifndef GL_SUN_convolution_border_modes
+#define GL_SUN_convolution_border_modes 1
+#endif
+
+#ifndef GL_EXT_texture_env_add
+#define GL_EXT_texture_env_add 1
+#endif
+
+#ifndef GL_EXT_texture_lod_bias
+#define GL_EXT_texture_lod_bias 1
+#endif
+
+#ifndef GL_EXT_texture_filter_anisotropic
+#define GL_EXT_texture_filter_anisotropic 1
+#endif
+
+#ifndef GL_EXT_vertex_weighting
+#define GL_EXT_vertex_weighting 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glVertexWeightfEXT (GLfloat weight);
+GLAPI void APIENTRY glVertexWeightfvEXT (const GLfloat *weight);
+GLAPI void APIENTRY glVertexWeightPointerEXT (GLsizei size, GLenum type, GLsizei stride, const GLvoid *pointer);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLVERTEXWEIGHTFEXTPROC) (GLfloat weight);
+typedef void (APIENTRYP PFNGLVERTEXWEIGHTFVEXTPROC) (const GLfloat *weight);
+typedef void (APIENTRYP PFNGLVERTEXWEIGHTPOINTEREXTPROC) (GLsizei size, GLenum type, GLsizei stride, const GLvoid *pointer);
+#endif
+
+#ifndef GL_NV_light_max_exponent
+#define GL_NV_light_max_exponent 1
+#endif
+
+#ifndef GL_NV_vertex_array_range
+#define GL_NV_vertex_array_range 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glFlushVertexArrayRangeNV (void);
+GLAPI void APIENTRY glVertexArrayRangeNV (GLsizei length, const GLvoid *pointer);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLFLUSHVERTEXARRAYRANGENVPROC) (void);
+typedef void (APIENTRYP PFNGLVERTEXARRAYRANGENVPROC) (GLsizei length, const GLvoid *pointer);
+#endif
+
+#ifndef GL_NV_register_combiners
+#define GL_NV_register_combiners 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glCombinerParameterfvNV (GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glCombinerParameterfNV (GLenum pname, GLfloat param);
+GLAPI void APIENTRY glCombinerParameterivNV (GLenum pname, const GLint *params);
+GLAPI void APIENTRY glCombinerParameteriNV (GLenum pname, GLint param);
+GLAPI void APIENTRY glCombinerInputNV (GLenum stage, GLenum portion, GLenum variable, GLenum input, GLenum mapping, GLenum componentUsage);
+GLAPI void APIENTRY glCombinerOutputNV (GLenum stage, GLenum portion, GLenum abOutput, GLenum cdOutput, GLenum sumOutput, GLenum scale, GLenum bias, GLboolean abDotProduct, GLboolean cdDotProduct, GLboolean muxSum);
+GLAPI void APIENTRY glFinalCombinerInputNV (GLenum variable, GLenum input, GLenum mapping, GLenum componentUsage);
+GLAPI void APIENTRY glGetCombinerInputParameterfvNV (GLenum stage, GLenum portion, GLenum variable, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetCombinerInputParameterivNV (GLenum stage, GLenum portion, GLenum variable, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetCombinerOutputParameterfvNV (GLenum stage, GLenum portion, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetCombinerOutputParameterivNV (GLenum stage, GLenum portion, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetFinalCombinerInputParameterfvNV (GLenum variable, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetFinalCombinerInputParameterivNV (GLenum variable, GLenum pname, GLint *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCOMBINERPARAMETERFVNVPROC) (GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLCOMBINERPARAMETERFNVPROC) (GLenum pname, GLfloat param);
+typedef void (APIENTRYP PFNGLCOMBINERPARAMETERIVNVPROC) (GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLCOMBINERPARAMETERINVPROC) (GLenum pname, GLint param);
+typedef void (APIENTRYP PFNGLCOMBINERINPUTNVPROC) (GLenum stage, GLenum portion, GLenum variable, GLenum input, GLenum mapping, GLenum componentUsage);
+typedef void (APIENTRYP PFNGLCOMBINEROUTPUTNVPROC) (GLenum stage, GLenum portion, GLenum abOutput, GLenum cdOutput, GLenum sumOutput, GLenum scale, GLenum bias, GLboolean abDotProduct, GLboolean cdDotProduct, GLboolean muxSum);
+typedef void (APIENTRYP PFNGLFINALCOMBINERINPUTNVPROC) (GLenum variable, GLenum input, GLenum mapping, GLenum componentUsage);
+typedef void (APIENTRYP PFNGLGETCOMBINERINPUTPARAMETERFVNVPROC) (GLenum stage, GLenum portion, GLenum variable, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETCOMBINERINPUTPARAMETERIVNVPROC) (GLenum stage, GLenum portion, GLenum variable, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETCOMBINEROUTPUTPARAMETERFVNVPROC) (GLenum stage, GLenum portion, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETCOMBINEROUTPUTPARAMETERIVNVPROC) (GLenum stage, GLenum portion, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETFINALCOMBINERINPUTPARAMETERFVNVPROC) (GLenum variable, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETFINALCOMBINERINPUTPARAMETERIVNVPROC) (GLenum variable, GLenum pname, GLint *params);
+#endif
+
+#ifndef GL_NV_fog_distance
+#define GL_NV_fog_distance 1
+#endif
+
+#ifndef GL_NV_texgen_emboss
+#define GL_NV_texgen_emboss 1
+#endif
+
+#ifndef GL_NV_blend_square
+#define GL_NV_blend_square 1
+#endif
+
+#ifndef GL_NV_texture_env_combine4
+#define GL_NV_texture_env_combine4 1
+#endif
+
+#ifndef GL_MESA_resize_buffers
+#define GL_MESA_resize_buffers 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glResizeBuffersMESA (void);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLRESIZEBUFFERSMESAPROC) (void);
+#endif
+
+#ifndef GL_MESA_window_pos
+#define GL_MESA_window_pos 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glWindowPos2dMESA (GLdouble x, GLdouble y);
+GLAPI void APIENTRY glWindowPos2dvMESA (const GLdouble *v);
+GLAPI void APIENTRY glWindowPos2fMESA (GLfloat x, GLfloat y);
+GLAPI void APIENTRY glWindowPos2fvMESA (const GLfloat *v);
+GLAPI void APIENTRY glWindowPos2iMESA (GLint x, GLint y);
+GLAPI void APIENTRY glWindowPos2ivMESA (const GLint *v);
+GLAPI void APIENTRY glWindowPos2sMESA (GLshort x, GLshort y);
+GLAPI void APIENTRY glWindowPos2svMESA (const GLshort *v);
+GLAPI void APIENTRY glWindowPos3dMESA (GLdouble x, GLdouble y, GLdouble z);
+GLAPI void APIENTRY glWindowPos3dvMESA (const GLdouble *v);
+GLAPI void APIENTRY glWindowPos3fMESA (GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glWindowPos3fvMESA (const GLfloat *v);
+GLAPI void APIENTRY glWindowPos3iMESA (GLint x, GLint y, GLint z);
+GLAPI void APIENTRY glWindowPos3ivMESA (const GLint *v);
+GLAPI void APIENTRY glWindowPos3sMESA (GLshort x, GLshort y, GLshort z);
+GLAPI void APIENTRY glWindowPos3svMESA (const GLshort *v);
+GLAPI void APIENTRY glWindowPos4dMESA (GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+GLAPI void APIENTRY glWindowPos4dvMESA (const GLdouble *v);
+GLAPI void APIENTRY glWindowPos4fMESA (GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+GLAPI void APIENTRY glWindowPos4fvMESA (const GLfloat *v);
+GLAPI void APIENTRY glWindowPos4iMESA (GLint x, GLint y, GLint z, GLint w);
+GLAPI void APIENTRY glWindowPos4ivMESA (const GLint *v);
+GLAPI void APIENTRY glWindowPos4sMESA (GLshort x, GLshort y, GLshort z, GLshort w);
+GLAPI void APIENTRY glWindowPos4svMESA (const GLshort *v);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLWINDOWPOS2DMESAPROC) (GLdouble x, GLdouble y);
+typedef void (APIENTRYP PFNGLWINDOWPOS2DVMESAPROC) (const GLdouble *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS2FMESAPROC) (GLfloat x, GLfloat y);
+typedef void (APIENTRYP PFNGLWINDOWPOS2FVMESAPROC) (const GLfloat *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS2IMESAPROC) (GLint x, GLint y);
+typedef void (APIENTRYP PFNGLWINDOWPOS2IVMESAPROC) (const GLint *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS2SMESAPROC) (GLshort x, GLshort y);
+typedef void (APIENTRYP PFNGLWINDOWPOS2SVMESAPROC) (const GLshort *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS3DMESAPROC) (GLdouble x, GLdouble y, GLdouble z);
+typedef void (APIENTRYP PFNGLWINDOWPOS3DVMESAPROC) (const GLdouble *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS3FMESAPROC) (GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLWINDOWPOS3FVMESAPROC) (const GLfloat *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS3IMESAPROC) (GLint x, GLint y, GLint z);
+typedef void (APIENTRYP PFNGLWINDOWPOS3IVMESAPROC) (const GLint *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS3SMESAPROC) (GLshort x, GLshort y, GLshort z);
+typedef void (APIENTRYP PFNGLWINDOWPOS3SVMESAPROC) (const GLshort *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS4DMESAPROC) (GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+typedef void (APIENTRYP PFNGLWINDOWPOS4DVMESAPROC) (const GLdouble *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS4FMESAPROC) (GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+typedef void (APIENTRYP PFNGLWINDOWPOS4FVMESAPROC) (const GLfloat *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS4IMESAPROC) (GLint x, GLint y, GLint z, GLint w);
+typedef void (APIENTRYP PFNGLWINDOWPOS4IVMESAPROC) (const GLint *v);
+typedef void (APIENTRYP PFNGLWINDOWPOS4SMESAPROC) (GLshort x, GLshort y, GLshort z, GLshort w);
+typedef void (APIENTRYP PFNGLWINDOWPOS4SVMESAPROC) (const GLshort *v);
+#endif
+
+#ifndef GL_IBM_cull_vertex
+#define GL_IBM_cull_vertex 1
+#endif
+
+#ifndef GL_IBM_multimode_draw_arrays
+#define GL_IBM_multimode_draw_arrays 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glMultiModeDrawArraysIBM (const GLenum *mode, const GLint *first, const GLsizei *count, GLsizei primcount, GLint modestride);
+GLAPI void APIENTRY glMultiModeDrawElementsIBM (const GLenum *mode, const GLsizei *count, GLenum type, const GLvoid* const *indices, GLsizei primcount, GLint modestride);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLMULTIMODEDRAWARRAYSIBMPROC) (const GLenum *mode, const GLint *first, const GLsizei *count, GLsizei primcount, GLint modestride);
+typedef void (APIENTRYP PFNGLMULTIMODEDRAWELEMENTSIBMPROC) (const GLenum *mode, const GLsizei *count, GLenum type, const GLvoid* const *indices, GLsizei primcount, GLint modestride);
+#endif
+
+#ifndef GL_IBM_vertex_array_lists
+#define GL_IBM_vertex_array_lists 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glColorPointerListIBM (GLint size, GLenum type, GLint stride, const GLvoid* *pointer, GLint ptrstride);
+GLAPI void APIENTRY glSecondaryColorPointerListIBM (GLint size, GLenum type, GLint stride, const GLvoid* *pointer, GLint ptrstride);
+GLAPI void APIENTRY glEdgeFlagPointerListIBM (GLint stride, const GLboolean* *pointer, GLint ptrstride);
+GLAPI void APIENTRY glFogCoordPointerListIBM (GLenum type, GLint stride, const GLvoid* *pointer, GLint ptrstride);
+GLAPI void APIENTRY glIndexPointerListIBM (GLenum type, GLint stride, const GLvoid* *pointer, GLint ptrstride);
+GLAPI void APIENTRY glNormalPointerListIBM (GLenum type, GLint stride, const GLvoid* *pointer, GLint ptrstride);
+GLAPI void APIENTRY glTexCoordPointerListIBM (GLint size, GLenum type, GLint stride, const GLvoid* *pointer, GLint ptrstride);
+GLAPI void APIENTRY glVertexPointerListIBM (GLint size, GLenum type, GLint stride, const GLvoid* *pointer, GLint ptrstride);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCOLORPOINTERLISTIBMPROC) (GLint size, GLenum type, GLint stride, const GLvoid* *pointer, GLint ptrstride);
+typedef void (APIENTRYP PFNGLSECONDARYCOLORPOINTERLISTIBMPROC) (GLint size, GLenum type, GLint stride, const GLvoid* *pointer, GLint ptrstride);
+typedef void (APIENTRYP PFNGLEDGEFLAGPOINTERLISTIBMPROC) (GLint stride, const GLboolean* *pointer, GLint ptrstride);
+typedef void (APIENTRYP PFNGLFOGCOORDPOINTERLISTIBMPROC) (GLenum type, GLint stride, const GLvoid* *pointer, GLint ptrstride);
+typedef void (APIENTRYP PFNGLINDEXPOINTERLISTIBMPROC) (GLenum type, GLint stride, const GLvoid* *pointer, GLint ptrstride);
+typedef void (APIENTRYP PFNGLNORMALPOINTERLISTIBMPROC) (GLenum type, GLint stride, const GLvoid* *pointer, GLint ptrstride);
+typedef void (APIENTRYP PFNGLTEXCOORDPOINTERLISTIBMPROC) (GLint size, GLenum type, GLint stride, const GLvoid* *pointer, GLint ptrstride);
+typedef void (APIENTRYP PFNGLVERTEXPOINTERLISTIBMPROC) (GLint size, GLenum type, GLint stride, const GLvoid* *pointer, GLint ptrstride);
+#endif
+
+#ifndef GL_SGIX_subsample
+#define GL_SGIX_subsample 1
+#endif
+
+#ifndef GL_SGIX_ycrcba
+#define GL_SGIX_ycrcba 1
+#endif
+
+#ifndef GL_SGIX_ycrcb_subsample
+#define GL_SGIX_ycrcb_subsample 1
+#endif
+
+#ifndef GL_SGIX_depth_pass_instrument
+#define GL_SGIX_depth_pass_instrument 1
+#endif
+
+#ifndef GL_3DFX_texture_compression_FXT1
+#define GL_3DFX_texture_compression_FXT1 1
+#endif
+
+#ifndef GL_3DFX_multisample
+#define GL_3DFX_multisample 1
+#endif
+
+#ifndef GL_3DFX_tbuffer
+#define GL_3DFX_tbuffer 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glTbufferMask3DFX (GLuint mask);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLTBUFFERMASK3DFXPROC) (GLuint mask);
+#endif
+
+#ifndef GL_EXT_multisample
+#define GL_EXT_multisample 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glSampleMaskEXT (GLclampf value, GLboolean invert);
+GLAPI void APIENTRY glSamplePatternEXT (GLenum pattern);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLSAMPLEMASKEXTPROC) (GLclampf value, GLboolean invert);
+typedef void (APIENTRYP PFNGLSAMPLEPATTERNEXTPROC) (GLenum pattern);
+#endif
+
+#ifndef GL_SGIX_vertex_preclip
+#define GL_SGIX_vertex_preclip 1
+#endif
+
+#ifndef GL_SGIX_convolution_accuracy
+#define GL_SGIX_convolution_accuracy 1
+#endif
+
+#ifndef GL_SGIX_resample
+#define GL_SGIX_resample 1
+#endif
+
+#ifndef GL_SGIS_point_line_texgen
+#define GL_SGIS_point_line_texgen 1
+#endif
+
+#ifndef GL_SGIS_texture_color_mask
+#define GL_SGIS_texture_color_mask 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glTextureColorMaskSGIS (GLboolean red, GLboolean green, GLboolean blue, GLboolean alpha);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLTEXTURECOLORMASKSGISPROC) (GLboolean red, GLboolean green, GLboolean blue, GLboolean alpha);
+#endif
+
+#ifndef GL_SGIX_igloo_interface
+#define GL_SGIX_igloo_interface 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glIglooInterfaceSGIX (GLenum pname, const GLvoid *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLIGLOOINTERFACESGIXPROC) (GLenum pname, const GLvoid *params);
+#endif
+
+#ifndef GL_EXT_texture_env_dot3
+#define GL_EXT_texture_env_dot3 1
+#endif
+
+#ifndef GL_ATI_texture_mirror_once
+#define GL_ATI_texture_mirror_once 1
+#endif
+
+#ifndef GL_NV_fence
+#define GL_NV_fence 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glDeleteFencesNV (GLsizei n, const GLuint *fences);
+GLAPI void APIENTRY glGenFencesNV (GLsizei n, GLuint *fences);
+GLAPI GLboolean APIENTRY glIsFenceNV (GLuint fence);
+GLAPI GLboolean APIENTRY glTestFenceNV (GLuint fence);
+GLAPI void APIENTRY glGetFenceivNV (GLuint fence, GLenum pname, GLint *params);
+GLAPI void APIENTRY glFinishFenceNV (GLuint fence);
+GLAPI void APIENTRY glSetFenceNV (GLuint fence, GLenum condition);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLDELETEFENCESNVPROC) (GLsizei n, const GLuint *fences);
+typedef void (APIENTRYP PFNGLGENFENCESNVPROC) (GLsizei n, GLuint *fences);
+typedef GLboolean (APIENTRYP PFNGLISFENCENVPROC) (GLuint fence);
+typedef GLboolean (APIENTRYP PFNGLTESTFENCENVPROC) (GLuint fence);
+typedef void (APIENTRYP PFNGLGETFENCEIVNVPROC) (GLuint fence, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLFINISHFENCENVPROC) (GLuint fence);
+typedef void (APIENTRYP PFNGLSETFENCENVPROC) (GLuint fence, GLenum condition);
+#endif
+
+#ifndef GL_NV_evaluators
+#define GL_NV_evaluators 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glMapControlPointsNV (GLenum target, GLuint index, GLenum type, GLsizei ustride, GLsizei vstride, GLint uorder, GLint vorder, GLboolean packed, const GLvoid *points);
+GLAPI void APIENTRY glMapParameterivNV (GLenum target, GLenum pname, const GLint *params);
+GLAPI void APIENTRY glMapParameterfvNV (GLenum target, GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glGetMapControlPointsNV (GLenum target, GLuint index, GLenum type, GLsizei ustride, GLsizei vstride, GLboolean packed, GLvoid *points);
+GLAPI void APIENTRY glGetMapParameterivNV (GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetMapParameterfvNV (GLenum target, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetMapAttribParameterivNV (GLenum target, GLuint index, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetMapAttribParameterfvNV (GLenum target, GLuint index, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glEvalMapsNV (GLenum target, GLenum mode);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLMAPCONTROLPOINTSNVPROC) (GLenum target, GLuint index, GLenum type, GLsizei ustride, GLsizei vstride, GLint uorder, GLint vorder, GLboolean packed, const GLvoid *points);
+typedef void (APIENTRYP PFNGLMAPPARAMETERIVNVPROC) (GLenum target, GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLMAPPARAMETERFVNVPROC) (GLenum target, GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLGETMAPCONTROLPOINTSNVPROC) (GLenum target, GLuint index, GLenum type, GLsizei ustride, GLsizei vstride, GLboolean packed, GLvoid *points);
+typedef void (APIENTRYP PFNGLGETMAPPARAMETERIVNVPROC) (GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETMAPPARAMETERFVNVPROC) (GLenum target, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETMAPATTRIBPARAMETERIVNVPROC) (GLenum target, GLuint index, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETMAPATTRIBPARAMETERFVNVPROC) (GLenum target, GLuint index, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLEVALMAPSNVPROC) (GLenum target, GLenum mode);
+#endif
+
+#ifndef GL_NV_packed_depth_stencil
+#define GL_NV_packed_depth_stencil 1
+#endif
+
+#ifndef GL_NV_register_combiners2
+#define GL_NV_register_combiners2 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glCombinerStageParameterfvNV (GLenum stage, GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glGetCombinerStageParameterfvNV (GLenum stage, GLenum pname, GLfloat *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCOMBINERSTAGEPARAMETERFVNVPROC) (GLenum stage, GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLGETCOMBINERSTAGEPARAMETERFVNVPROC) (GLenum stage, GLenum pname, GLfloat *params);
+#endif
+
+#ifndef GL_NV_texture_compression_vtc
+#define GL_NV_texture_compression_vtc 1
+#endif
+
+#ifndef GL_NV_texture_rectangle
+#define GL_NV_texture_rectangle 1
+#endif
+
+#ifndef GL_NV_texture_shader
+#define GL_NV_texture_shader 1
+#endif
+
+#ifndef GL_NV_texture_shader2
+#define GL_NV_texture_shader2 1
+#endif
+
+#ifndef GL_NV_vertex_array_range2
+#define GL_NV_vertex_array_range2 1
+#endif
+
+#ifndef GL_NV_vertex_program
+#define GL_NV_vertex_program 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI GLboolean APIENTRY glAreProgramsResidentNV (GLsizei n, const GLuint *programs, GLboolean *residences);
+GLAPI void APIENTRY glBindProgramNV (GLenum target, GLuint id);
+GLAPI void APIENTRY glDeleteProgramsNV (GLsizei n, const GLuint *programs);
+GLAPI void APIENTRY glExecuteProgramNV (GLenum target, GLuint id, const GLfloat *params);
+GLAPI void APIENTRY glGenProgramsNV (GLsizei n, GLuint *programs);
+GLAPI void APIENTRY glGetProgramParameterdvNV (GLenum target, GLuint index, GLenum pname, GLdouble *params);
+GLAPI void APIENTRY glGetProgramParameterfvNV (GLenum target, GLuint index, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetProgramivNV (GLuint id, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetProgramStringNV (GLuint id, GLenum pname, GLubyte *program);
+GLAPI void APIENTRY glGetTrackMatrixivNV (GLenum target, GLuint address, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetVertexAttribdvNV (GLuint index, GLenum pname, GLdouble *params);
+GLAPI void APIENTRY glGetVertexAttribfvNV (GLuint index, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetVertexAttribivNV (GLuint index, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetVertexAttribPointervNV (GLuint index, GLenum pname, GLvoid* *pointer);
+GLAPI GLboolean APIENTRY glIsProgramNV (GLuint id);
+GLAPI void APIENTRY glLoadProgramNV (GLenum target, GLuint id, GLsizei len, const GLubyte *program);
+GLAPI void APIENTRY glProgramParameter4dNV (GLenum target, GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+GLAPI void APIENTRY glProgramParameter4dvNV (GLenum target, GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glProgramParameter4fNV (GLenum target, GLuint index, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+GLAPI void APIENTRY glProgramParameter4fvNV (GLenum target, GLuint index, const GLfloat *v);
+GLAPI void APIENTRY glProgramParameters4dvNV (GLenum target, GLuint index, GLsizei count, const GLdouble *v);
+GLAPI void APIENTRY glProgramParameters4fvNV (GLenum target, GLuint index, GLsizei count, const GLfloat *v);
+GLAPI void APIENTRY glRequestResidentProgramsNV (GLsizei n, const GLuint *programs);
+GLAPI void APIENTRY glTrackMatrixNV (GLenum target, GLuint address, GLenum matrix, GLenum transform);
+GLAPI void APIENTRY glVertexAttribPointerNV (GLuint index, GLint fsize, GLenum type, GLsizei stride, const GLvoid *pointer);
+GLAPI void APIENTRY glVertexAttrib1dNV (GLuint index, GLdouble x);
+GLAPI void APIENTRY glVertexAttrib1dvNV (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttrib1fNV (GLuint index, GLfloat x);
+GLAPI void APIENTRY glVertexAttrib1fvNV (GLuint index, const GLfloat *v);
+GLAPI void APIENTRY glVertexAttrib1sNV (GLuint index, GLshort x);
+GLAPI void APIENTRY glVertexAttrib1svNV (GLuint index, const GLshort *v);
+GLAPI void APIENTRY glVertexAttrib2dNV (GLuint index, GLdouble x, GLdouble y);
+GLAPI void APIENTRY glVertexAttrib2dvNV (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttrib2fNV (GLuint index, GLfloat x, GLfloat y);
+GLAPI void APIENTRY glVertexAttrib2fvNV (GLuint index, const GLfloat *v);
+GLAPI void APIENTRY glVertexAttrib2sNV (GLuint index, GLshort x, GLshort y);
+GLAPI void APIENTRY glVertexAttrib2svNV (GLuint index, const GLshort *v);
+GLAPI void APIENTRY glVertexAttrib3dNV (GLuint index, GLdouble x, GLdouble y, GLdouble z);
+GLAPI void APIENTRY glVertexAttrib3dvNV (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttrib3fNV (GLuint index, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glVertexAttrib3fvNV (GLuint index, const GLfloat *v);
+GLAPI void APIENTRY glVertexAttrib3sNV (GLuint index, GLshort x, GLshort y, GLshort z);
+GLAPI void APIENTRY glVertexAttrib3svNV (GLuint index, const GLshort *v);
+GLAPI void APIENTRY glVertexAttrib4dNV (GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+GLAPI void APIENTRY glVertexAttrib4dvNV (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttrib4fNV (GLuint index, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+GLAPI void APIENTRY glVertexAttrib4fvNV (GLuint index, const GLfloat *v);
+GLAPI void APIENTRY glVertexAttrib4sNV (GLuint index, GLshort x, GLshort y, GLshort z, GLshort w);
+GLAPI void APIENTRY glVertexAttrib4svNV (GLuint index, const GLshort *v);
+GLAPI void APIENTRY glVertexAttrib4ubNV (GLuint index, GLubyte x, GLubyte y, GLubyte z, GLubyte w);
+GLAPI void APIENTRY glVertexAttrib4ubvNV (GLuint index, const GLubyte *v);
+GLAPI void APIENTRY glVertexAttribs1dvNV (GLuint index, GLsizei count, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttribs1fvNV (GLuint index, GLsizei count, const GLfloat *v);
+GLAPI void APIENTRY glVertexAttribs1svNV (GLuint index, GLsizei count, const GLshort *v);
+GLAPI void APIENTRY glVertexAttribs2dvNV (GLuint index, GLsizei count, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttribs2fvNV (GLuint index, GLsizei count, const GLfloat *v);
+GLAPI void APIENTRY glVertexAttribs2svNV (GLuint index, GLsizei count, const GLshort *v);
+GLAPI void APIENTRY glVertexAttribs3dvNV (GLuint index, GLsizei count, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttribs3fvNV (GLuint index, GLsizei count, const GLfloat *v);
+GLAPI void APIENTRY glVertexAttribs3svNV (GLuint index, GLsizei count, const GLshort *v);
+GLAPI void APIENTRY glVertexAttribs4dvNV (GLuint index, GLsizei count, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttribs4fvNV (GLuint index, GLsizei count, const GLfloat *v);
+GLAPI void APIENTRY glVertexAttribs4svNV (GLuint index, GLsizei count, const GLshort *v);
+GLAPI void APIENTRY glVertexAttribs4ubvNV (GLuint index, GLsizei count, const GLubyte *v);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef GLboolean (APIENTRYP PFNGLAREPROGRAMSRESIDENTNVPROC) (GLsizei n, const GLuint *programs, GLboolean *residences);
+typedef void (APIENTRYP PFNGLBINDPROGRAMNVPROC) (GLenum target, GLuint id);
+typedef void (APIENTRYP PFNGLDELETEPROGRAMSNVPROC) (GLsizei n, const GLuint *programs);
+typedef void (APIENTRYP PFNGLEXECUTEPROGRAMNVPROC) (GLenum target, GLuint id, const GLfloat *params);
+typedef void (APIENTRYP PFNGLGENPROGRAMSNVPROC) (GLsizei n, GLuint *programs);
+typedef void (APIENTRYP PFNGLGETPROGRAMPARAMETERDVNVPROC) (GLenum target, GLuint index, GLenum pname, GLdouble *params);
+typedef void (APIENTRYP PFNGLGETPROGRAMPARAMETERFVNVPROC) (GLenum target, GLuint index, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETPROGRAMIVNVPROC) (GLuint id, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETPROGRAMSTRINGNVPROC) (GLuint id, GLenum pname, GLubyte *program);
+typedef void (APIENTRYP PFNGLGETTRACKMATRIXIVNVPROC) (GLenum target, GLuint address, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBDVNVPROC) (GLuint index, GLenum pname, GLdouble *params);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBFVNVPROC) (GLuint index, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBIVNVPROC) (GLuint index, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBPOINTERVNVPROC) (GLuint index, GLenum pname, GLvoid* *pointer);
+typedef GLboolean (APIENTRYP PFNGLISPROGRAMNVPROC) (GLuint id);
+typedef void (APIENTRYP PFNGLLOADPROGRAMNVPROC) (GLenum target, GLuint id, GLsizei len, const GLubyte *program);
+typedef void (APIENTRYP PFNGLPROGRAMPARAMETER4DNVPROC) (GLenum target, GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+typedef void (APIENTRYP PFNGLPROGRAMPARAMETER4DVNVPROC) (GLenum target, GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLPROGRAMPARAMETER4FNVPROC) (GLenum target, GLuint index, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+typedef void (APIENTRYP PFNGLPROGRAMPARAMETER4FVNVPROC) (GLenum target, GLuint index, const GLfloat *v);
+typedef void (APIENTRYP PFNGLPROGRAMPARAMETERS4DVNVPROC) (GLenum target, GLuint index, GLsizei count, const GLdouble *v);
+typedef void (APIENTRYP PFNGLPROGRAMPARAMETERS4FVNVPROC) (GLenum target, GLuint index, GLsizei count, const GLfloat *v);
+typedef void (APIENTRYP PFNGLREQUESTRESIDENTPROGRAMSNVPROC) (GLsizei n, const GLuint *programs);
+typedef void (APIENTRYP PFNGLTRACKMATRIXNVPROC) (GLenum target, GLuint address, GLenum matrix, GLenum transform);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBPOINTERNVPROC) (GLuint index, GLint fsize, GLenum type, GLsizei stride, const GLvoid *pointer);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1DNVPROC) (GLuint index, GLdouble x);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1DVNVPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1FNVPROC) (GLuint index, GLfloat x);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1FVNVPROC) (GLuint index, const GLfloat *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1SNVPROC) (GLuint index, GLshort x);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1SVNVPROC) (GLuint index, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2DNVPROC) (GLuint index, GLdouble x, GLdouble y);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2DVNVPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2FNVPROC) (GLuint index, GLfloat x, GLfloat y);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2FVNVPROC) (GLuint index, const GLfloat *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2SNVPROC) (GLuint index, GLshort x, GLshort y);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2SVNVPROC) (GLuint index, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3DNVPROC) (GLuint index, GLdouble x, GLdouble y, GLdouble z);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3DVNVPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3FNVPROC) (GLuint index, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3FVNVPROC) (GLuint index, const GLfloat *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3SNVPROC) (GLuint index, GLshort x, GLshort y, GLshort z);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3SVNVPROC) (GLuint index, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4DNVPROC) (GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4DVNVPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4FNVPROC) (GLuint index, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4FVNVPROC) (GLuint index, const GLfloat *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4SNVPROC) (GLuint index, GLshort x, GLshort y, GLshort z, GLshort w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4SVNVPROC) (GLuint index, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4UBNVPROC) (GLuint index, GLubyte x, GLubyte y, GLubyte z, GLubyte w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4UBVNVPROC) (GLuint index, const GLubyte *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBS1DVNVPROC) (GLuint index, GLsizei count, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBS1FVNVPROC) (GLuint index, GLsizei count, const GLfloat *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBS1SVNVPROC) (GLuint index, GLsizei count, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBS2DVNVPROC) (GLuint index, GLsizei count, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBS2FVNVPROC) (GLuint index, GLsizei count, const GLfloat *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBS2SVNVPROC) (GLuint index, GLsizei count, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBS3DVNVPROC) (GLuint index, GLsizei count, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBS3FVNVPROC) (GLuint index, GLsizei count, const GLfloat *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBS3SVNVPROC) (GLuint index, GLsizei count, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBS4DVNVPROC) (GLuint index, GLsizei count, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBS4FVNVPROC) (GLuint index, GLsizei count, const GLfloat *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBS4SVNVPROC) (GLuint index, GLsizei count, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBS4UBVNVPROC) (GLuint index, GLsizei count, const GLubyte *v);
+#endif
+
+#ifndef GL_SGIX_texture_coordinate_clamp
+#define GL_SGIX_texture_coordinate_clamp 1
+#endif
+
+#ifndef GL_SGIX_scalebias_hint
+#define GL_SGIX_scalebias_hint 1
+#endif
+
+#ifndef GL_OML_interlace
+#define GL_OML_interlace 1
+#endif
+
+#ifndef GL_OML_subsample
+#define GL_OML_subsample 1
+#endif
+
+#ifndef GL_OML_resample
+#define GL_OML_resample 1
+#endif
+
+#ifndef GL_NV_copy_depth_to_color
+#define GL_NV_copy_depth_to_color 1
+#endif
+
+#ifndef GL_ATI_envmap_bumpmap
+#define GL_ATI_envmap_bumpmap 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glTexBumpParameterivATI (GLenum pname, const GLint *param);
+GLAPI void APIENTRY glTexBumpParameterfvATI (GLenum pname, const GLfloat *param);
+GLAPI void APIENTRY glGetTexBumpParameterivATI (GLenum pname, GLint *param);
+GLAPI void APIENTRY glGetTexBumpParameterfvATI (GLenum pname, GLfloat *param);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLTEXBUMPPARAMETERIVATIPROC) (GLenum pname, const GLint *param);
+typedef void (APIENTRYP PFNGLTEXBUMPPARAMETERFVATIPROC) (GLenum pname, const GLfloat *param);
+typedef void (APIENTRYP PFNGLGETTEXBUMPPARAMETERIVATIPROC) (GLenum pname, GLint *param);
+typedef void (APIENTRYP PFNGLGETTEXBUMPPARAMETERFVATIPROC) (GLenum pname, GLfloat *param);
+#endif
+
+#ifndef GL_ATI_fragment_shader
+#define GL_ATI_fragment_shader 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI GLuint APIENTRY glGenFragmentShadersATI (GLuint range);
+GLAPI void APIENTRY glBindFragmentShaderATI (GLuint id);
+GLAPI void APIENTRY glDeleteFragmentShaderATI (GLuint id);
+GLAPI void APIENTRY glBeginFragmentShaderATI (void);
+GLAPI void APIENTRY glEndFragmentShaderATI (void);
+GLAPI void APIENTRY glPassTexCoordATI (GLuint dst, GLuint coord, GLenum swizzle);
+GLAPI void APIENTRY glSampleMapATI (GLuint dst, GLuint interp, GLenum swizzle);
+GLAPI void APIENTRY glColorFragmentOp1ATI (GLenum op, GLuint dst, GLuint dstMask, GLuint dstMod, GLuint arg1, GLuint arg1Rep, GLuint arg1Mod);
+GLAPI void APIENTRY glColorFragmentOp2ATI (GLenum op, GLuint dst, GLuint dstMask, GLuint dstMod, GLuint arg1, GLuint arg1Rep, GLuint arg1Mod, GLuint arg2, GLuint arg2Rep, GLuint arg2Mod);
+GLAPI void APIENTRY glColorFragmentOp3ATI (GLenum op, GLuint dst, GLuint dstMask, GLuint dstMod, GLuint arg1, GLuint arg1Rep, GLuint arg1Mod, GLuint arg2, GLuint arg2Rep, GLuint arg2Mod, GLuint arg3, GLuint arg3Rep, GLuint arg3Mod);
+GLAPI void APIENTRY glAlphaFragmentOp1ATI (GLenum op, GLuint dst, GLuint dstMod, GLuint arg1, GLuint arg1Rep, GLuint arg1Mod);
+GLAPI void APIENTRY glAlphaFragmentOp2ATI (GLenum op, GLuint dst, GLuint dstMod, GLuint arg1, GLuint arg1Rep, GLuint arg1Mod, GLuint arg2, GLuint arg2Rep, GLuint arg2Mod);
+GLAPI void APIENTRY glAlphaFragmentOp3ATI (GLenum op, GLuint dst, GLuint dstMod, GLuint arg1, GLuint arg1Rep, GLuint arg1Mod, GLuint arg2, GLuint arg2Rep, GLuint arg2Mod, GLuint arg3, GLuint arg3Rep, GLuint arg3Mod);
+GLAPI void APIENTRY glSetFragmentShaderConstantATI (GLuint dst, const GLfloat *value);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef GLuint (APIENTRYP PFNGLGENFRAGMENTSHADERSATIPROC) (GLuint range);
+typedef void (APIENTRYP PFNGLBINDFRAGMENTSHADERATIPROC) (GLuint id);
+typedef void (APIENTRYP PFNGLDELETEFRAGMENTSHADERATIPROC) (GLuint id);
+typedef void (APIENTRYP PFNGLBEGINFRAGMENTSHADERATIPROC) (void);
+typedef void (APIENTRYP PFNGLENDFRAGMENTSHADERATIPROC) (void);
+typedef void (APIENTRYP PFNGLPASSTEXCOORDATIPROC) (GLuint dst, GLuint coord, GLenum swizzle);
+typedef void (APIENTRYP PFNGLSAMPLEMAPATIPROC) (GLuint dst, GLuint interp, GLenum swizzle);
+typedef void (APIENTRYP PFNGLCOLORFRAGMENTOP1ATIPROC) (GLenum op, GLuint dst, GLuint dstMask, GLuint dstMod, GLuint arg1, GLuint arg1Rep, GLuint arg1Mod);
+typedef void (APIENTRYP PFNGLCOLORFRAGMENTOP2ATIPROC) (GLenum op, GLuint dst, GLuint dstMask, GLuint dstMod, GLuint arg1, GLuint arg1Rep, GLuint arg1Mod, GLuint arg2, GLuint arg2Rep, GLuint arg2Mod);
+typedef void (APIENTRYP PFNGLCOLORFRAGMENTOP3ATIPROC) (GLenum op, GLuint dst, GLuint dstMask, GLuint dstMod, GLuint arg1, GLuint arg1Rep, GLuint arg1Mod, GLuint arg2, GLuint arg2Rep, GLuint arg2Mod, GLuint arg3, GLuint arg3Rep, GLuint arg3Mod);
+typedef void (APIENTRYP PFNGLALPHAFRAGMENTOP1ATIPROC) (GLenum op, GLuint dst, GLuint dstMod, GLuint arg1, GLuint arg1Rep, GLuint arg1Mod);
+typedef void (APIENTRYP PFNGLALPHAFRAGMENTOP2ATIPROC) (GLenum op, GLuint dst, GLuint dstMod, GLuint arg1, GLuint arg1Rep, GLuint arg1Mod, GLuint arg2, GLuint arg2Rep, GLuint arg2Mod);
+typedef void (APIENTRYP PFNGLALPHAFRAGMENTOP3ATIPROC) (GLenum op, GLuint dst, GLuint dstMod, GLuint arg1, GLuint arg1Rep, GLuint arg1Mod, GLuint arg2, GLuint arg2Rep, GLuint arg2Mod, GLuint arg3, GLuint arg3Rep, GLuint arg3Mod);
+typedef void (APIENTRYP PFNGLSETFRAGMENTSHADERCONSTANTATIPROC) (GLuint dst, const GLfloat *value);
+#endif
+
+#ifndef GL_ATI_pn_triangles
+#define GL_ATI_pn_triangles 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glPNTrianglesiATI (GLenum pname, GLint param);
+GLAPI void APIENTRY glPNTrianglesfATI (GLenum pname, GLfloat param);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPNTRIANGLESIATIPROC) (GLenum pname, GLint param);
+typedef void (APIENTRYP PFNGLPNTRIANGLESFATIPROC) (GLenum pname, GLfloat param);
+#endif
+
+#ifndef GL_ATI_vertex_array_object
+#define GL_ATI_vertex_array_object 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI GLuint APIENTRY glNewObjectBufferATI (GLsizei size, const GLvoid *pointer, GLenum usage);
+GLAPI GLboolean APIENTRY glIsObjectBufferATI (GLuint buffer);
+GLAPI void APIENTRY glUpdateObjectBufferATI (GLuint buffer, GLuint offset, GLsizei size, const GLvoid *pointer, GLenum preserve);
+GLAPI void APIENTRY glGetObjectBufferfvATI (GLuint buffer, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetObjectBufferivATI (GLuint buffer, GLenum pname, GLint *params);
+GLAPI void APIENTRY glFreeObjectBufferATI (GLuint buffer);
+GLAPI void APIENTRY glArrayObjectATI (GLenum array, GLint size, GLenum type, GLsizei stride, GLuint buffer, GLuint offset);
+GLAPI void APIENTRY glGetArrayObjectfvATI (GLenum array, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetArrayObjectivATI (GLenum array, GLenum pname, GLint *params);
+GLAPI void APIENTRY glVariantArrayObjectATI (GLuint id, GLenum type, GLsizei stride, GLuint buffer, GLuint offset);
+GLAPI void APIENTRY glGetVariantArrayObjectfvATI (GLuint id, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetVariantArrayObjectivATI (GLuint id, GLenum pname, GLint *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef GLuint (APIENTRYP PFNGLNEWOBJECTBUFFERATIPROC) (GLsizei size, const GLvoid *pointer, GLenum usage);
+typedef GLboolean (APIENTRYP PFNGLISOBJECTBUFFERATIPROC) (GLuint buffer);
+typedef void (APIENTRYP PFNGLUPDATEOBJECTBUFFERATIPROC) (GLuint buffer, GLuint offset, GLsizei size, const GLvoid *pointer, GLenum preserve);
+typedef void (APIENTRYP PFNGLGETOBJECTBUFFERFVATIPROC) (GLuint buffer, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETOBJECTBUFFERIVATIPROC) (GLuint buffer, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLFREEOBJECTBUFFERATIPROC) (GLuint buffer);
+typedef void (APIENTRYP PFNGLARRAYOBJECTATIPROC) (GLenum array, GLint size, GLenum type, GLsizei stride, GLuint buffer, GLuint offset);
+typedef void (APIENTRYP PFNGLGETARRAYOBJECTFVATIPROC) (GLenum array, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETARRAYOBJECTIVATIPROC) (GLenum array, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLVARIANTARRAYOBJECTATIPROC) (GLuint id, GLenum type, GLsizei stride, GLuint buffer, GLuint offset);
+typedef void (APIENTRYP PFNGLGETVARIANTARRAYOBJECTFVATIPROC) (GLuint id, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETVARIANTARRAYOBJECTIVATIPROC) (GLuint id, GLenum pname, GLint *params);
+#endif
+
+#ifndef GL_EXT_vertex_shader
+#define GL_EXT_vertex_shader 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBeginVertexShaderEXT (void);
+GLAPI void APIENTRY glEndVertexShaderEXT (void);
+GLAPI void APIENTRY glBindVertexShaderEXT (GLuint id);
+GLAPI GLuint APIENTRY glGenVertexShadersEXT (GLuint range);
+GLAPI void APIENTRY glDeleteVertexShaderEXT (GLuint id);
+GLAPI void APIENTRY glShaderOp1EXT (GLenum op, GLuint res, GLuint arg1);
+GLAPI void APIENTRY glShaderOp2EXT (GLenum op, GLuint res, GLuint arg1, GLuint arg2);
+GLAPI void APIENTRY glShaderOp3EXT (GLenum op, GLuint res, GLuint arg1, GLuint arg2, GLuint arg3);
+GLAPI void APIENTRY glSwizzleEXT (GLuint res, GLuint in, GLenum outX, GLenum outY, GLenum outZ, GLenum outW);
+GLAPI void APIENTRY glWriteMaskEXT (GLuint res, GLuint in, GLenum outX, GLenum outY, GLenum outZ, GLenum outW);
+GLAPI void APIENTRY glInsertComponentEXT (GLuint res, GLuint src, GLuint num);
+GLAPI void APIENTRY glExtractComponentEXT (GLuint res, GLuint src, GLuint num);
+GLAPI GLuint APIENTRY glGenSymbolsEXT (GLenum datatype, GLenum storagetype, GLenum range, GLuint components);
+GLAPI void APIENTRY glSetInvariantEXT (GLuint id, GLenum type, const GLvoid *addr);
+GLAPI void APIENTRY glSetLocalConstantEXT (GLuint id, GLenum type, const GLvoid *addr);
+GLAPI void APIENTRY glVariantbvEXT (GLuint id, const GLbyte *addr);
+GLAPI void APIENTRY glVariantsvEXT (GLuint id, const GLshort *addr);
+GLAPI void APIENTRY glVariantivEXT (GLuint id, const GLint *addr);
+GLAPI void APIENTRY glVariantfvEXT (GLuint id, const GLfloat *addr);
+GLAPI void APIENTRY glVariantdvEXT (GLuint id, const GLdouble *addr);
+GLAPI void APIENTRY glVariantubvEXT (GLuint id, const GLubyte *addr);
+GLAPI void APIENTRY glVariantusvEXT (GLuint id, const GLushort *addr);
+GLAPI void APIENTRY glVariantuivEXT (GLuint id, const GLuint *addr);
+GLAPI void APIENTRY glVariantPointerEXT (GLuint id, GLenum type, GLuint stride, const GLvoid *addr);
+GLAPI void APIENTRY glEnableVariantClientStateEXT (GLuint id);
+GLAPI void APIENTRY glDisableVariantClientStateEXT (GLuint id);
+GLAPI GLuint APIENTRY glBindLightParameterEXT (GLenum light, GLenum value);
+GLAPI GLuint APIENTRY glBindMaterialParameterEXT (GLenum face, GLenum value);
+GLAPI GLuint APIENTRY glBindTexGenParameterEXT (GLenum unit, GLenum coord, GLenum value);
+GLAPI GLuint APIENTRY glBindTextureUnitParameterEXT (GLenum unit, GLenum value);
+GLAPI GLuint APIENTRY glBindParameterEXT (GLenum value);
+GLAPI GLboolean APIENTRY glIsVariantEnabledEXT (GLuint id, GLenum cap);
+GLAPI void APIENTRY glGetVariantBooleanvEXT (GLuint id, GLenum value, GLboolean *data);
+GLAPI void APIENTRY glGetVariantIntegervEXT (GLuint id, GLenum value, GLint *data);
+GLAPI void APIENTRY glGetVariantFloatvEXT (GLuint id, GLenum value, GLfloat *data);
+GLAPI void APIENTRY glGetVariantPointervEXT (GLuint id, GLenum value, GLvoid* *data);
+GLAPI void APIENTRY glGetInvariantBooleanvEXT (GLuint id, GLenum value, GLboolean *data);
+GLAPI void APIENTRY glGetInvariantIntegervEXT (GLuint id, GLenum value, GLint *data);
+GLAPI void APIENTRY glGetInvariantFloatvEXT (GLuint id, GLenum value, GLfloat *data);
+GLAPI void APIENTRY glGetLocalConstantBooleanvEXT (GLuint id, GLenum value, GLboolean *data);
+GLAPI void APIENTRY glGetLocalConstantIntegervEXT (GLuint id, GLenum value, GLint *data);
+GLAPI void APIENTRY glGetLocalConstantFloatvEXT (GLuint id, GLenum value, GLfloat *data);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBEGINVERTEXSHADEREXTPROC) (void);
+typedef void (APIENTRYP PFNGLENDVERTEXSHADEREXTPROC) (void);
+typedef void (APIENTRYP PFNGLBINDVERTEXSHADEREXTPROC) (GLuint id);
+typedef GLuint (APIENTRYP PFNGLGENVERTEXSHADERSEXTPROC) (GLuint range);
+typedef void (APIENTRYP PFNGLDELETEVERTEXSHADEREXTPROC) (GLuint id);
+typedef void (APIENTRYP PFNGLSHADEROP1EXTPROC) (GLenum op, GLuint res, GLuint arg1);
+typedef void (APIENTRYP PFNGLSHADEROP2EXTPROC) (GLenum op, GLuint res, GLuint arg1, GLuint arg2);
+typedef void (APIENTRYP PFNGLSHADEROP3EXTPROC) (GLenum op, GLuint res, GLuint arg1, GLuint arg2, GLuint arg3);
+typedef void (APIENTRYP PFNGLSWIZZLEEXTPROC) (GLuint res, GLuint in, GLenum outX, GLenum outY, GLenum outZ, GLenum outW);
+typedef void (APIENTRYP PFNGLWRITEMASKEXTPROC) (GLuint res, GLuint in, GLenum outX, GLenum outY, GLenum outZ, GLenum outW);
+typedef void (APIENTRYP PFNGLINSERTCOMPONENTEXTPROC) (GLuint res, GLuint src, GLuint num);
+typedef void (APIENTRYP PFNGLEXTRACTCOMPONENTEXTPROC) (GLuint res, GLuint src, GLuint num);
+typedef GLuint (APIENTRYP PFNGLGENSYMBOLSEXTPROC) (GLenum datatype, GLenum storagetype, GLenum range, GLuint components);
+typedef void (APIENTRYP PFNGLSETINVARIANTEXTPROC) (GLuint id, GLenum type, const GLvoid *addr);
+typedef void (APIENTRYP PFNGLSETLOCALCONSTANTEXTPROC) (GLuint id, GLenum type, const GLvoid *addr);
+typedef void (APIENTRYP PFNGLVARIANTBVEXTPROC) (GLuint id, const GLbyte *addr);
+typedef void (APIENTRYP PFNGLVARIANTSVEXTPROC) (GLuint id, const GLshort *addr);
+typedef void (APIENTRYP PFNGLVARIANTIVEXTPROC) (GLuint id, const GLint *addr);
+typedef void (APIENTRYP PFNGLVARIANTFVEXTPROC) (GLuint id, const GLfloat *addr);
+typedef void (APIENTRYP PFNGLVARIANTDVEXTPROC) (GLuint id, const GLdouble *addr);
+typedef void (APIENTRYP PFNGLVARIANTUBVEXTPROC) (GLuint id, const GLubyte *addr);
+typedef void (APIENTRYP PFNGLVARIANTUSVEXTPROC) (GLuint id, const GLushort *addr);
+typedef void (APIENTRYP PFNGLVARIANTUIVEXTPROC) (GLuint id, const GLuint *addr);
+typedef void (APIENTRYP PFNGLVARIANTPOINTEREXTPROC) (GLuint id, GLenum type, GLuint stride, const GLvoid *addr);
+typedef void (APIENTRYP PFNGLENABLEVARIANTCLIENTSTATEEXTPROC) (GLuint id);
+typedef void (APIENTRYP PFNGLDISABLEVARIANTCLIENTSTATEEXTPROC) (GLuint id);
+typedef GLuint (APIENTRYP PFNGLBINDLIGHTPARAMETEREXTPROC) (GLenum light, GLenum value);
+typedef GLuint (APIENTRYP PFNGLBINDMATERIALPARAMETEREXTPROC) (GLenum face, GLenum value);
+typedef GLuint (APIENTRYP PFNGLBINDTEXGENPARAMETEREXTPROC) (GLenum unit, GLenum coord, GLenum value);
+typedef GLuint (APIENTRYP PFNGLBINDTEXTUREUNITPARAMETEREXTPROC) (GLenum unit, GLenum value);
+typedef GLuint (APIENTRYP PFNGLBINDPARAMETEREXTPROC) (GLenum value);
+typedef GLboolean (APIENTRYP PFNGLISVARIANTENABLEDEXTPROC) (GLuint id, GLenum cap);
+typedef void (APIENTRYP PFNGLGETVARIANTBOOLEANVEXTPROC) (GLuint id, GLenum value, GLboolean *data);
+typedef void (APIENTRYP PFNGLGETVARIANTINTEGERVEXTPROC) (GLuint id, GLenum value, GLint *data);
+typedef void (APIENTRYP PFNGLGETVARIANTFLOATVEXTPROC) (GLuint id, GLenum value, GLfloat *data);
+typedef void (APIENTRYP PFNGLGETVARIANTPOINTERVEXTPROC) (GLuint id, GLenum value, GLvoid* *data);
+typedef void (APIENTRYP PFNGLGETINVARIANTBOOLEANVEXTPROC) (GLuint id, GLenum value, GLboolean *data);
+typedef void (APIENTRYP PFNGLGETINVARIANTINTEGERVEXTPROC) (GLuint id, GLenum value, GLint *data);
+typedef void (APIENTRYP PFNGLGETINVARIANTFLOATVEXTPROC) (GLuint id, GLenum value, GLfloat *data);
+typedef void (APIENTRYP PFNGLGETLOCALCONSTANTBOOLEANVEXTPROC) (GLuint id, GLenum value, GLboolean *data);
+typedef void (APIENTRYP PFNGLGETLOCALCONSTANTINTEGERVEXTPROC) (GLuint id, GLenum value, GLint *data);
+typedef void (APIENTRYP PFNGLGETLOCALCONSTANTFLOATVEXTPROC) (GLuint id, GLenum value, GLfloat *data);
+#endif
+
+#ifndef GL_ATI_vertex_streams
+#define GL_ATI_vertex_streams 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glVertexStream1sATI (GLenum stream, GLshort x);
+GLAPI void APIENTRY glVertexStream1svATI (GLenum stream, const GLshort *coords);
+GLAPI void APIENTRY glVertexStream1iATI (GLenum stream, GLint x);
+GLAPI void APIENTRY glVertexStream1ivATI (GLenum stream, const GLint *coords);
+GLAPI void APIENTRY glVertexStream1fATI (GLenum stream, GLfloat x);
+GLAPI void APIENTRY glVertexStream1fvATI (GLenum stream, const GLfloat *coords);
+GLAPI void APIENTRY glVertexStream1dATI (GLenum stream, GLdouble x);
+GLAPI void APIENTRY glVertexStream1dvATI (GLenum stream, const GLdouble *coords);
+GLAPI void APIENTRY glVertexStream2sATI (GLenum stream, GLshort x, GLshort y);
+GLAPI void APIENTRY glVertexStream2svATI (GLenum stream, const GLshort *coords);
+GLAPI void APIENTRY glVertexStream2iATI (GLenum stream, GLint x, GLint y);
+GLAPI void APIENTRY glVertexStream2ivATI (GLenum stream, const GLint *coords);
+GLAPI void APIENTRY glVertexStream2fATI (GLenum stream, GLfloat x, GLfloat y);
+GLAPI void APIENTRY glVertexStream2fvATI (GLenum stream, const GLfloat *coords);
+GLAPI void APIENTRY glVertexStream2dATI (GLenum stream, GLdouble x, GLdouble y);
+GLAPI void APIENTRY glVertexStream2dvATI (GLenum stream, const GLdouble *coords);
+GLAPI void APIENTRY glVertexStream3sATI (GLenum stream, GLshort x, GLshort y, GLshort z);
+GLAPI void APIENTRY glVertexStream3svATI (GLenum stream, const GLshort *coords);
+GLAPI void APIENTRY glVertexStream3iATI (GLenum stream, GLint x, GLint y, GLint z);
+GLAPI void APIENTRY glVertexStream3ivATI (GLenum stream, const GLint *coords);
+GLAPI void APIENTRY glVertexStream3fATI (GLenum stream, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glVertexStream3fvATI (GLenum stream, const GLfloat *coords);
+GLAPI void APIENTRY glVertexStream3dATI (GLenum stream, GLdouble x, GLdouble y, GLdouble z);
+GLAPI void APIENTRY glVertexStream3dvATI (GLenum stream, const GLdouble *coords);
+GLAPI void APIENTRY glVertexStream4sATI (GLenum stream, GLshort x, GLshort y, GLshort z, GLshort w);
+GLAPI void APIENTRY glVertexStream4svATI (GLenum stream, const GLshort *coords);
+GLAPI void APIENTRY glVertexStream4iATI (GLenum stream, GLint x, GLint y, GLint z, GLint w);
+GLAPI void APIENTRY glVertexStream4ivATI (GLenum stream, const GLint *coords);
+GLAPI void APIENTRY glVertexStream4fATI (GLenum stream, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+GLAPI void APIENTRY glVertexStream4fvATI (GLenum stream, const GLfloat *coords);
+GLAPI void APIENTRY glVertexStream4dATI (GLenum stream, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+GLAPI void APIENTRY glVertexStream4dvATI (GLenum stream, const GLdouble *coords);
+GLAPI void APIENTRY glNormalStream3bATI (GLenum stream, GLbyte nx, GLbyte ny, GLbyte nz);
+GLAPI void APIENTRY glNormalStream3bvATI (GLenum stream, const GLbyte *coords);
+GLAPI void APIENTRY glNormalStream3sATI (GLenum stream, GLshort nx, GLshort ny, GLshort nz);
+GLAPI void APIENTRY glNormalStream3svATI (GLenum stream, const GLshort *coords);
+GLAPI void APIENTRY glNormalStream3iATI (GLenum stream, GLint nx, GLint ny, GLint nz);
+GLAPI void APIENTRY glNormalStream3ivATI (GLenum stream, const GLint *coords);
+GLAPI void APIENTRY glNormalStream3fATI (GLenum stream, GLfloat nx, GLfloat ny, GLfloat nz);
+GLAPI void APIENTRY glNormalStream3fvATI (GLenum stream, const GLfloat *coords);
+GLAPI void APIENTRY glNormalStream3dATI (GLenum stream, GLdouble nx, GLdouble ny, GLdouble nz);
+GLAPI void APIENTRY glNormalStream3dvATI (GLenum stream, const GLdouble *coords);
+GLAPI void APIENTRY glClientActiveVertexStreamATI (GLenum stream);
+GLAPI void APIENTRY glVertexBlendEnviATI (GLenum pname, GLint param);
+GLAPI void APIENTRY glVertexBlendEnvfATI (GLenum pname, GLfloat param);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLVERTEXSTREAM1SATIPROC) (GLenum stream, GLshort x);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM1SVATIPROC) (GLenum stream, const GLshort *coords);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM1IATIPROC) (GLenum stream, GLint x);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM1IVATIPROC) (GLenum stream, const GLint *coords);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM1FATIPROC) (GLenum stream, GLfloat x);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM1FVATIPROC) (GLenum stream, const GLfloat *coords);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM1DATIPROC) (GLenum stream, GLdouble x);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM1DVATIPROC) (GLenum stream, const GLdouble *coords);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM2SATIPROC) (GLenum stream, GLshort x, GLshort y);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM2SVATIPROC) (GLenum stream, const GLshort *coords);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM2IATIPROC) (GLenum stream, GLint x, GLint y);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM2IVATIPROC) (GLenum stream, const GLint *coords);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM2FATIPROC) (GLenum stream, GLfloat x, GLfloat y);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM2FVATIPROC) (GLenum stream, const GLfloat *coords);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM2DATIPROC) (GLenum stream, GLdouble x, GLdouble y);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM2DVATIPROC) (GLenum stream, const GLdouble *coords);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM3SATIPROC) (GLenum stream, GLshort x, GLshort y, GLshort z);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM3SVATIPROC) (GLenum stream, const GLshort *coords);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM3IATIPROC) (GLenum stream, GLint x, GLint y, GLint z);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM3IVATIPROC) (GLenum stream, const GLint *coords);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM3FATIPROC) (GLenum stream, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM3FVATIPROC) (GLenum stream, const GLfloat *coords);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM3DATIPROC) (GLenum stream, GLdouble x, GLdouble y, GLdouble z);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM3DVATIPROC) (GLenum stream, const GLdouble *coords);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM4SATIPROC) (GLenum stream, GLshort x, GLshort y, GLshort z, GLshort w);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM4SVATIPROC) (GLenum stream, const GLshort *coords);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM4IATIPROC) (GLenum stream, GLint x, GLint y, GLint z, GLint w);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM4IVATIPROC) (GLenum stream, const GLint *coords);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM4FATIPROC) (GLenum stream, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM4FVATIPROC) (GLenum stream, const GLfloat *coords);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM4DATIPROC) (GLenum stream, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+typedef void (APIENTRYP PFNGLVERTEXSTREAM4DVATIPROC) (GLenum stream, const GLdouble *coords);
+typedef void (APIENTRYP PFNGLNORMALSTREAM3BATIPROC) (GLenum stream, GLbyte nx, GLbyte ny, GLbyte nz);
+typedef void (APIENTRYP PFNGLNORMALSTREAM3BVATIPROC) (GLenum stream, const GLbyte *coords);
+typedef void (APIENTRYP PFNGLNORMALSTREAM3SATIPROC) (GLenum stream, GLshort nx, GLshort ny, GLshort nz);
+typedef void (APIENTRYP PFNGLNORMALSTREAM3SVATIPROC) (GLenum stream, const GLshort *coords);
+typedef void (APIENTRYP PFNGLNORMALSTREAM3IATIPROC) (GLenum stream, GLint nx, GLint ny, GLint nz);
+typedef void (APIENTRYP PFNGLNORMALSTREAM3IVATIPROC) (GLenum stream, const GLint *coords);
+typedef void (APIENTRYP PFNGLNORMALSTREAM3FATIPROC) (GLenum stream, GLfloat nx, GLfloat ny, GLfloat nz);
+typedef void (APIENTRYP PFNGLNORMALSTREAM3FVATIPROC) (GLenum stream, const GLfloat *coords);
+typedef void (APIENTRYP PFNGLNORMALSTREAM3DATIPROC) (GLenum stream, GLdouble nx, GLdouble ny, GLdouble nz);
+typedef void (APIENTRYP PFNGLNORMALSTREAM3DVATIPROC) (GLenum stream, const GLdouble *coords);
+typedef void (APIENTRYP PFNGLCLIENTACTIVEVERTEXSTREAMATIPROC) (GLenum stream);
+typedef void (APIENTRYP PFNGLVERTEXBLENDENVIATIPROC) (GLenum pname, GLint param);
+typedef void (APIENTRYP PFNGLVERTEXBLENDENVFATIPROC) (GLenum pname, GLfloat param);
+#endif
+
+#ifndef GL_ATI_element_array
+#define GL_ATI_element_array 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glElementPointerATI (GLenum type, const GLvoid *pointer);
+GLAPI void APIENTRY glDrawElementArrayATI (GLenum mode, GLsizei count);
+GLAPI void APIENTRY glDrawRangeElementArrayATI (GLenum mode, GLuint start, GLuint end, GLsizei count);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLELEMENTPOINTERATIPROC) (GLenum type, const GLvoid *pointer);
+typedef void (APIENTRYP PFNGLDRAWELEMENTARRAYATIPROC) (GLenum mode, GLsizei count);
+typedef void (APIENTRYP PFNGLDRAWRANGEELEMENTARRAYATIPROC) (GLenum mode, GLuint start, GLuint end, GLsizei count);
+#endif
+
+#ifndef GL_SUN_mesh_array
+#define GL_SUN_mesh_array 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glDrawMeshArraysSUN (GLenum mode, GLint first, GLsizei count, GLsizei width);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLDRAWMESHARRAYSSUNPROC) (GLenum mode, GLint first, GLsizei count, GLsizei width);
+#endif
+
+#ifndef GL_SUN_slice_accum
+#define GL_SUN_slice_accum 1
+#endif
+
+#ifndef GL_NV_multisample_filter_hint
+#define GL_NV_multisample_filter_hint 1
+#endif
+
+#ifndef GL_NV_depth_clamp
+#define GL_NV_depth_clamp 1
+#endif
+
+#ifndef GL_NV_occlusion_query
+#define GL_NV_occlusion_query 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glGenOcclusionQueriesNV (GLsizei n, GLuint *ids);
+GLAPI void APIENTRY glDeleteOcclusionQueriesNV (GLsizei n, const GLuint *ids);
+GLAPI GLboolean APIENTRY glIsOcclusionQueryNV (GLuint id);
+GLAPI void APIENTRY glBeginOcclusionQueryNV (GLuint id);
+GLAPI void APIENTRY glEndOcclusionQueryNV (void);
+GLAPI void APIENTRY glGetOcclusionQueryivNV (GLuint id, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetOcclusionQueryuivNV (GLuint id, GLenum pname, GLuint *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLGENOCCLUSIONQUERIESNVPROC) (GLsizei n, GLuint *ids);
+typedef void (APIENTRYP PFNGLDELETEOCCLUSIONQUERIESNVPROC) (GLsizei n, const GLuint *ids);
+typedef GLboolean (APIENTRYP PFNGLISOCCLUSIONQUERYNVPROC) (GLuint id);
+typedef void (APIENTRYP PFNGLBEGINOCCLUSIONQUERYNVPROC) (GLuint id);
+typedef void (APIENTRYP PFNGLENDOCCLUSIONQUERYNVPROC) (void);
+typedef void (APIENTRYP PFNGLGETOCCLUSIONQUERYIVNVPROC) (GLuint id, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETOCCLUSIONQUERYUIVNVPROC) (GLuint id, GLenum pname, GLuint *params);
+#endif
+
+#ifndef GL_NV_point_sprite
+#define GL_NV_point_sprite 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glPointParameteriNV (GLenum pname, GLint param);
+GLAPI void APIENTRY glPointParameterivNV (GLenum pname, const GLint *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPOINTPARAMETERINVPROC) (GLenum pname, GLint param);
+typedef void (APIENTRYP PFNGLPOINTPARAMETERIVNVPROC) (GLenum pname, const GLint *params);
+#endif
+
+#ifndef GL_NV_texture_shader3
+#define GL_NV_texture_shader3 1
+#endif
+
+#ifndef GL_NV_vertex_program1_1
+#define GL_NV_vertex_program1_1 1
+#endif
+
+#ifndef GL_EXT_shadow_funcs
+#define GL_EXT_shadow_funcs 1
+#endif
+
+#ifndef GL_EXT_stencil_two_side
+#define GL_EXT_stencil_two_side 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glActiveStencilFaceEXT (GLenum face);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLACTIVESTENCILFACEEXTPROC) (GLenum face);
+#endif
+
+#ifndef GL_ATI_text_fragment_shader
+#define GL_ATI_text_fragment_shader 1
+#endif
+
+#ifndef GL_APPLE_client_storage
+#define GL_APPLE_client_storage 1
+#endif
+
+#ifndef GL_APPLE_element_array
+#define GL_APPLE_element_array 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glElementPointerAPPLE (GLenum type, const GLvoid *pointer);
+GLAPI void APIENTRY glDrawElementArrayAPPLE (GLenum mode, GLint first, GLsizei count);
+GLAPI void APIENTRY glDrawRangeElementArrayAPPLE (GLenum mode, GLuint start, GLuint end, GLint first, GLsizei count);
+GLAPI void APIENTRY glMultiDrawElementArrayAPPLE (GLenum mode, const GLint *first, const GLsizei *count, GLsizei primcount);
+GLAPI void APIENTRY glMultiDrawRangeElementArrayAPPLE (GLenum mode, GLuint start, GLuint end, const GLint *first, const GLsizei *count, GLsizei primcount);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLELEMENTPOINTERAPPLEPROC) (GLenum type, const GLvoid *pointer);
+typedef void (APIENTRYP PFNGLDRAWELEMENTARRAYAPPLEPROC) (GLenum mode, GLint first, GLsizei count);
+typedef void (APIENTRYP PFNGLDRAWRANGEELEMENTARRAYAPPLEPROC) (GLenum mode, GLuint start, GLuint end, GLint first, GLsizei count);
+typedef void (APIENTRYP PFNGLMULTIDRAWELEMENTARRAYAPPLEPROC) (GLenum mode, const GLint *first, const GLsizei *count, GLsizei primcount);
+typedef void (APIENTRYP PFNGLMULTIDRAWRANGEELEMENTARRAYAPPLEPROC) (GLenum mode, GLuint start, GLuint end, const GLint *first, const GLsizei *count, GLsizei primcount);
+#endif
+
+#ifndef GL_APPLE_fence
+#define GL_APPLE_fence 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glGenFencesAPPLE (GLsizei n, GLuint *fences);
+GLAPI void APIENTRY glDeleteFencesAPPLE (GLsizei n, const GLuint *fences);
+GLAPI void APIENTRY glSetFenceAPPLE (GLuint fence);
+GLAPI GLboolean APIENTRY glIsFenceAPPLE (GLuint fence);
+GLAPI GLboolean APIENTRY glTestFenceAPPLE (GLuint fence);
+GLAPI void APIENTRY glFinishFenceAPPLE (GLuint fence);
+GLAPI GLboolean APIENTRY glTestObjectAPPLE (GLenum object, GLuint name);
+GLAPI void APIENTRY glFinishObjectAPPLE (GLenum object, GLint name);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLGENFENCESAPPLEPROC) (GLsizei n, GLuint *fences);
+typedef void (APIENTRYP PFNGLDELETEFENCESAPPLEPROC) (GLsizei n, const GLuint *fences);
+typedef void (APIENTRYP PFNGLSETFENCEAPPLEPROC) (GLuint fence);
+typedef GLboolean (APIENTRYP PFNGLISFENCEAPPLEPROC) (GLuint fence);
+typedef GLboolean (APIENTRYP PFNGLTESTFENCEAPPLEPROC) (GLuint fence);
+typedef void (APIENTRYP PFNGLFINISHFENCEAPPLEPROC) (GLuint fence);
+typedef GLboolean (APIENTRYP PFNGLTESTOBJECTAPPLEPROC) (GLenum object, GLuint name);
+typedef void (APIENTRYP PFNGLFINISHOBJECTAPPLEPROC) (GLenum object, GLint name);
+#endif
+
+#ifndef GL_APPLE_vertex_array_object
+#define GL_APPLE_vertex_array_object 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBindVertexArrayAPPLE (GLuint array);
+GLAPI void APIENTRY glDeleteVertexArraysAPPLE (GLsizei n, const GLuint *arrays);
+GLAPI void APIENTRY glGenVertexArraysAPPLE (GLsizei n, GLuint *arrays);
+GLAPI GLboolean APIENTRY glIsVertexArrayAPPLE (GLuint array);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBINDVERTEXARRAYAPPLEPROC) (GLuint array);
+typedef void (APIENTRYP PFNGLDELETEVERTEXARRAYSAPPLEPROC) (GLsizei n, const GLuint *arrays);
+typedef void (APIENTRYP PFNGLGENVERTEXARRAYSAPPLEPROC) (GLsizei n, GLuint *arrays);
+typedef GLboolean (APIENTRYP PFNGLISVERTEXARRAYAPPLEPROC) (GLuint array);
+#endif
+
+#ifndef GL_APPLE_vertex_array_range
+#define GL_APPLE_vertex_array_range 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glVertexArrayRangeAPPLE (GLsizei length, GLvoid *pointer);
+GLAPI void APIENTRY glFlushVertexArrayRangeAPPLE (GLsizei length, GLvoid *pointer);
+GLAPI void APIENTRY glVertexArrayParameteriAPPLE (GLenum pname, GLint param);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLVERTEXARRAYRANGEAPPLEPROC) (GLsizei length, GLvoid *pointer);
+typedef void (APIENTRYP PFNGLFLUSHVERTEXARRAYRANGEAPPLEPROC) (GLsizei length, GLvoid *pointer);
+typedef void (APIENTRYP PFNGLVERTEXARRAYPARAMETERIAPPLEPROC) (GLenum pname, GLint param);
+#endif
+
+#ifndef GL_APPLE_ycbcr_422
+#define GL_APPLE_ycbcr_422 1
+#endif
+
+#ifndef GL_S3_s3tc
+#define GL_S3_s3tc 1
+#endif
+
+#ifndef GL_ATI_draw_buffers
+#define GL_ATI_draw_buffers 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glDrawBuffersATI (GLsizei n, const GLenum *bufs);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLDRAWBUFFERSATIPROC) (GLsizei n, const GLenum *bufs);
+#endif
+
+#ifndef GL_ATI_pixel_format_float
+#define GL_ATI_pixel_format_float 1
+/* This is really a WGL extension, but defines some associated GL enums.
+ * ATI does not export "GL_ATI_pixel_format_float" in the GL_EXTENSIONS string.
+ */
+#endif
+
+#ifndef GL_ATI_texture_env_combine3
+#define GL_ATI_texture_env_combine3 1
+#endif
+
+#ifndef GL_ATI_texture_float
+#define GL_ATI_texture_float 1
+#endif
+
+#ifndef GL_NV_float_buffer
+#define GL_NV_float_buffer 1
+#endif
+
+#ifndef GL_NV_fragment_program
+#define GL_NV_fragment_program 1
+/* Some NV_fragment_program entry points are shared with ARB_vertex_program. */
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glProgramNamedParameter4fNV (GLuint id, GLsizei len, const GLubyte *name, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+GLAPI void APIENTRY glProgramNamedParameter4dNV (GLuint id, GLsizei len, const GLubyte *name, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+GLAPI void APIENTRY glProgramNamedParameter4fvNV (GLuint id, GLsizei len, const GLubyte *name, const GLfloat *v);
+GLAPI void APIENTRY glProgramNamedParameter4dvNV (GLuint id, GLsizei len, const GLubyte *name, const GLdouble *v);
+GLAPI void APIENTRY glGetProgramNamedParameterfvNV (GLuint id, GLsizei len, const GLubyte *name, GLfloat *params);
+GLAPI void APIENTRY glGetProgramNamedParameterdvNV (GLuint id, GLsizei len, const GLubyte *name, GLdouble *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPROGRAMNAMEDPARAMETER4FNVPROC) (GLuint id, GLsizei len, const GLubyte *name, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+typedef void (APIENTRYP PFNGLPROGRAMNAMEDPARAMETER4DNVPROC) (GLuint id, GLsizei len, const GLubyte *name, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+typedef void (APIENTRYP PFNGLPROGRAMNAMEDPARAMETER4FVNVPROC) (GLuint id, GLsizei len, const GLubyte *name, const GLfloat *v);
+typedef void (APIENTRYP PFNGLPROGRAMNAMEDPARAMETER4DVNVPROC) (GLuint id, GLsizei len, const GLubyte *name, const GLdouble *v);
+typedef void (APIENTRYP PFNGLGETPROGRAMNAMEDPARAMETERFVNVPROC) (GLuint id, GLsizei len, const GLubyte *name, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETPROGRAMNAMEDPARAMETERDVNVPROC) (GLuint id, GLsizei len, const GLubyte *name, GLdouble *params);
+#endif
+
+#ifndef GL_NV_half_float
+#define GL_NV_half_float 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glVertex2hNV (GLhalfNV x, GLhalfNV y);
+GLAPI void APIENTRY glVertex2hvNV (const GLhalfNV *v);
+GLAPI void APIENTRY glVertex3hNV (GLhalfNV x, GLhalfNV y, GLhalfNV z);
+GLAPI void APIENTRY glVertex3hvNV (const GLhalfNV *v);
+GLAPI void APIENTRY glVertex4hNV (GLhalfNV x, GLhalfNV y, GLhalfNV z, GLhalfNV w);
+GLAPI void APIENTRY glVertex4hvNV (const GLhalfNV *v);
+GLAPI void APIENTRY glNormal3hNV (GLhalfNV nx, GLhalfNV ny, GLhalfNV nz);
+GLAPI void APIENTRY glNormal3hvNV (const GLhalfNV *v);
+GLAPI void APIENTRY glColor3hNV (GLhalfNV red, GLhalfNV green, GLhalfNV blue);
+GLAPI void APIENTRY glColor3hvNV (const GLhalfNV *v);
+GLAPI void APIENTRY glColor4hNV (GLhalfNV red, GLhalfNV green, GLhalfNV blue, GLhalfNV alpha);
+GLAPI void APIENTRY glColor4hvNV (const GLhalfNV *v);
+GLAPI void APIENTRY glTexCoord1hNV (GLhalfNV s);
+GLAPI void APIENTRY glTexCoord1hvNV (const GLhalfNV *v);
+GLAPI void APIENTRY glTexCoord2hNV (GLhalfNV s, GLhalfNV t);
+GLAPI void APIENTRY glTexCoord2hvNV (const GLhalfNV *v);
+GLAPI void APIENTRY glTexCoord3hNV (GLhalfNV s, GLhalfNV t, GLhalfNV r);
+GLAPI void APIENTRY glTexCoord3hvNV (const GLhalfNV *v);
+GLAPI void APIENTRY glTexCoord4hNV (GLhalfNV s, GLhalfNV t, GLhalfNV r, GLhalfNV q);
+GLAPI void APIENTRY glTexCoord4hvNV (const GLhalfNV *v);
+GLAPI void APIENTRY glMultiTexCoord1hNV (GLenum target, GLhalfNV s);
+GLAPI void APIENTRY glMultiTexCoord1hvNV (GLenum target, const GLhalfNV *v);
+GLAPI void APIENTRY glMultiTexCoord2hNV (GLenum target, GLhalfNV s, GLhalfNV t);
+GLAPI void APIENTRY glMultiTexCoord2hvNV (GLenum target, const GLhalfNV *v);
+GLAPI void APIENTRY glMultiTexCoord3hNV (GLenum target, GLhalfNV s, GLhalfNV t, GLhalfNV r);
+GLAPI void APIENTRY glMultiTexCoord3hvNV (GLenum target, const GLhalfNV *v);
+GLAPI void APIENTRY glMultiTexCoord4hNV (GLenum target, GLhalfNV s, GLhalfNV t, GLhalfNV r, GLhalfNV q);
+GLAPI void APIENTRY glMultiTexCoord4hvNV (GLenum target, const GLhalfNV *v);
+GLAPI void APIENTRY glFogCoordhNV (GLhalfNV fog);
+GLAPI void APIENTRY glFogCoordhvNV (const GLhalfNV *fog);
+GLAPI void APIENTRY glSecondaryColor3hNV (GLhalfNV red, GLhalfNV green, GLhalfNV blue);
+GLAPI void APIENTRY glSecondaryColor3hvNV (const GLhalfNV *v);
+GLAPI void APIENTRY glVertexWeighthNV (GLhalfNV weight);
+GLAPI void APIENTRY glVertexWeighthvNV (const GLhalfNV *weight);
+GLAPI void APIENTRY glVertexAttrib1hNV (GLuint index, GLhalfNV x);
+GLAPI void APIENTRY glVertexAttrib1hvNV (GLuint index, const GLhalfNV *v);
+GLAPI void APIENTRY glVertexAttrib2hNV (GLuint index, GLhalfNV x, GLhalfNV y);
+GLAPI void APIENTRY glVertexAttrib2hvNV (GLuint index, const GLhalfNV *v);
+GLAPI void APIENTRY glVertexAttrib3hNV (GLuint index, GLhalfNV x, GLhalfNV y, GLhalfNV z);
+GLAPI void APIENTRY glVertexAttrib3hvNV (GLuint index, const GLhalfNV *v);
+GLAPI void APIENTRY glVertexAttrib4hNV (GLuint index, GLhalfNV x, GLhalfNV y, GLhalfNV z, GLhalfNV w);
+GLAPI void APIENTRY glVertexAttrib4hvNV (GLuint index, const GLhalfNV *v);
+GLAPI void APIENTRY glVertexAttribs1hvNV (GLuint index, GLsizei n, const GLhalfNV *v);
+GLAPI void APIENTRY glVertexAttribs2hvNV (GLuint index, GLsizei n, const GLhalfNV *v);
+GLAPI void APIENTRY glVertexAttribs3hvNV (GLuint index, GLsizei n, const GLhalfNV *v);
+GLAPI void APIENTRY glVertexAttribs4hvNV (GLuint index, GLsizei n, const GLhalfNV *v);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLVERTEX2HNVPROC) (GLhalfNV x, GLhalfNV y);
+typedef void (APIENTRYP PFNGLVERTEX2HVNVPROC) (const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLVERTEX3HNVPROC) (GLhalfNV x, GLhalfNV y, GLhalfNV z);
+typedef void (APIENTRYP PFNGLVERTEX3HVNVPROC) (const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLVERTEX4HNVPROC) (GLhalfNV x, GLhalfNV y, GLhalfNV z, GLhalfNV w);
+typedef void (APIENTRYP PFNGLVERTEX4HVNVPROC) (const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLNORMAL3HNVPROC) (GLhalfNV nx, GLhalfNV ny, GLhalfNV nz);
+typedef void (APIENTRYP PFNGLNORMAL3HVNVPROC) (const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLCOLOR3HNVPROC) (GLhalfNV red, GLhalfNV green, GLhalfNV blue);
+typedef void (APIENTRYP PFNGLCOLOR3HVNVPROC) (const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLCOLOR4HNVPROC) (GLhalfNV red, GLhalfNV green, GLhalfNV blue, GLhalfNV alpha);
+typedef void (APIENTRYP PFNGLCOLOR4HVNVPROC) (const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLTEXCOORD1HNVPROC) (GLhalfNV s);
+typedef void (APIENTRYP PFNGLTEXCOORD1HVNVPROC) (const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLTEXCOORD2HNVPROC) (GLhalfNV s, GLhalfNV t);
+typedef void (APIENTRYP PFNGLTEXCOORD2HVNVPROC) (const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLTEXCOORD3HNVPROC) (GLhalfNV s, GLhalfNV t, GLhalfNV r);
+typedef void (APIENTRYP PFNGLTEXCOORD3HVNVPROC) (const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLTEXCOORD4HNVPROC) (GLhalfNV s, GLhalfNV t, GLhalfNV r, GLhalfNV q);
+typedef void (APIENTRYP PFNGLTEXCOORD4HVNVPROC) (const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD1HNVPROC) (GLenum target, GLhalfNV s);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD1HVNVPROC) (GLenum target, const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD2HNVPROC) (GLenum target, GLhalfNV s, GLhalfNV t);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD2HVNVPROC) (GLenum target, const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD3HNVPROC) (GLenum target, GLhalfNV s, GLhalfNV t, GLhalfNV r);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD3HVNVPROC) (GLenum target, const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD4HNVPROC) (GLenum target, GLhalfNV s, GLhalfNV t, GLhalfNV r, GLhalfNV q);
+typedef void (APIENTRYP PFNGLMULTITEXCOORD4HVNVPROC) (GLenum target, const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLFOGCOORDHNVPROC) (GLhalfNV fog);
+typedef void (APIENTRYP PFNGLFOGCOORDHVNVPROC) (const GLhalfNV *fog);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3HNVPROC) (GLhalfNV red, GLhalfNV green, GLhalfNV blue);
+typedef void (APIENTRYP PFNGLSECONDARYCOLOR3HVNVPROC) (const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLVERTEXWEIGHTHNVPROC) (GLhalfNV weight);
+typedef void (APIENTRYP PFNGLVERTEXWEIGHTHVNVPROC) (const GLhalfNV *weight);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1HNVPROC) (GLuint index, GLhalfNV x);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB1HVNVPROC) (GLuint index, const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2HNVPROC) (GLuint index, GLhalfNV x, GLhalfNV y);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB2HVNVPROC) (GLuint index, const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3HNVPROC) (GLuint index, GLhalfNV x, GLhalfNV y, GLhalfNV z);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB3HVNVPROC) (GLuint index, const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4HNVPROC) (GLuint index, GLhalfNV x, GLhalfNV y, GLhalfNV z, GLhalfNV w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIB4HVNVPROC) (GLuint index, const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBS1HVNVPROC) (GLuint index, GLsizei n, const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBS2HVNVPROC) (GLuint index, GLsizei n, const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBS3HVNVPROC) (GLuint index, GLsizei n, const GLhalfNV *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBS4HVNVPROC) (GLuint index, GLsizei n, const GLhalfNV *v);
+#endif
+
+#ifndef GL_NV_pixel_data_range
+#define GL_NV_pixel_data_range 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glPixelDataRangeNV (GLenum target, GLsizei length, GLvoid *pointer);
+GLAPI void APIENTRY glFlushPixelDataRangeNV (GLenum target);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPIXELDATARANGENVPROC) (GLenum target, GLsizei length, GLvoid *pointer);
+typedef void (APIENTRYP PFNGLFLUSHPIXELDATARANGENVPROC) (GLenum target);
+#endif
+
+#ifndef GL_NV_primitive_restart
+#define GL_NV_primitive_restart 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glPrimitiveRestartNV (void);
+GLAPI void APIENTRY glPrimitiveRestartIndexNV (GLuint index);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPRIMITIVERESTARTNVPROC) (void);
+typedef void (APIENTRYP PFNGLPRIMITIVERESTARTINDEXNVPROC) (GLuint index);
+#endif
+
+#ifndef GL_NV_texture_expand_normal
+#define GL_NV_texture_expand_normal 1
+#endif
+
+#ifndef GL_NV_vertex_program2
+#define GL_NV_vertex_program2 1
+#endif
+
+#ifndef GL_ATI_map_object_buffer
+#define GL_ATI_map_object_buffer 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI GLvoid* APIENTRY glMapObjectBufferATI (GLuint buffer);
+GLAPI void APIENTRY glUnmapObjectBufferATI (GLuint buffer);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef GLvoid* (APIENTRYP PFNGLMAPOBJECTBUFFERATIPROC) (GLuint buffer);
+typedef void (APIENTRYP PFNGLUNMAPOBJECTBUFFERATIPROC) (GLuint buffer);
+#endif
+
+#ifndef GL_ATI_separate_stencil
+#define GL_ATI_separate_stencil 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glStencilOpSeparateATI (GLenum face, GLenum sfail, GLenum dpfail, GLenum dppass);
+GLAPI void APIENTRY glStencilFuncSeparateATI (GLenum frontfunc, GLenum backfunc, GLint ref, GLuint mask);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLSTENCILOPSEPARATEATIPROC) (GLenum face, GLenum sfail, GLenum dpfail, GLenum dppass);
+typedef void (APIENTRYP PFNGLSTENCILFUNCSEPARATEATIPROC) (GLenum frontfunc, GLenum backfunc, GLint ref, GLuint mask);
+#endif
+
+#ifndef GL_ATI_vertex_attrib_array_object
+#define GL_ATI_vertex_attrib_array_object 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glVertexAttribArrayObjectATI (GLuint index, GLint size, GLenum type, GLboolean normalized, GLsizei stride, GLuint buffer, GLuint offset);
+GLAPI void APIENTRY glGetVertexAttribArrayObjectfvATI (GLuint index, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetVertexAttribArrayObjectivATI (GLuint index, GLenum pname, GLint *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLVERTEXATTRIBARRAYOBJECTATIPROC) (GLuint index, GLint size, GLenum type, GLboolean normalized, GLsizei stride, GLuint buffer, GLuint offset);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBARRAYOBJECTFVATIPROC) (GLuint index, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBARRAYOBJECTIVATIPROC) (GLuint index, GLenum pname, GLint *params);
+#endif
+
+#ifndef GL_OES_read_format
+#define GL_OES_read_format 1
+#endif
+
+#ifndef GL_EXT_depth_bounds_test
+#define GL_EXT_depth_bounds_test 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glDepthBoundsEXT (GLclampd zmin, GLclampd zmax);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLDEPTHBOUNDSEXTPROC) (GLclampd zmin, GLclampd zmax);
+#endif
+
+#ifndef GL_EXT_texture_mirror_clamp
+#define GL_EXT_texture_mirror_clamp 1
+#endif
+
+#ifndef GL_EXT_blend_equation_separate
+#define GL_EXT_blend_equation_separate 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBlendEquationSeparateEXT (GLenum modeRGB, GLenum modeAlpha);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBLENDEQUATIONSEPARATEEXTPROC) (GLenum modeRGB, GLenum modeAlpha);
+#endif
+
+#ifndef GL_MESA_pack_invert
+#define GL_MESA_pack_invert 1
+#endif
+
+#ifndef GL_MESA_ycbcr_texture
+#define GL_MESA_ycbcr_texture 1
+#endif
+
+#ifndef GL_EXT_pixel_buffer_object
+#define GL_EXT_pixel_buffer_object 1
+#endif
+
+#ifndef GL_NV_fragment_program_option
+#define GL_NV_fragment_program_option 1
+#endif
+
+#ifndef GL_NV_fragment_program2
+#define GL_NV_fragment_program2 1
+#endif
+
+#ifndef GL_NV_vertex_program2_option
+#define GL_NV_vertex_program2_option 1
+#endif
+
+#ifndef GL_NV_vertex_program3
+#define GL_NV_vertex_program3 1
+#endif
+
+#ifndef GL_EXT_framebuffer_object
+#define GL_EXT_framebuffer_object 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI GLboolean APIENTRY glIsRenderbufferEXT (GLuint renderbuffer);
+GLAPI void APIENTRY glBindRenderbufferEXT (GLenum target, GLuint renderbuffer);
+GLAPI void APIENTRY glDeleteRenderbuffersEXT (GLsizei n, const GLuint *renderbuffers);
+GLAPI void APIENTRY glGenRenderbuffersEXT (GLsizei n, GLuint *renderbuffers);
+GLAPI void APIENTRY glRenderbufferStorageEXT (GLenum target, GLenum internalformat, GLsizei width, GLsizei height);
+GLAPI void APIENTRY glGetRenderbufferParameterivEXT (GLenum target, GLenum pname, GLint *params);
+GLAPI GLboolean APIENTRY glIsFramebufferEXT (GLuint framebuffer);
+GLAPI void APIENTRY glBindFramebufferEXT (GLenum target, GLuint framebuffer);
+GLAPI void APIENTRY glDeleteFramebuffersEXT (GLsizei n, const GLuint *framebuffers);
+GLAPI void APIENTRY glGenFramebuffersEXT (GLsizei n, GLuint *framebuffers);
+GLAPI GLenum APIENTRY glCheckFramebufferStatusEXT (GLenum target);
+GLAPI void APIENTRY glFramebufferTexture1DEXT (GLenum target, GLenum attachment, GLenum textarget, GLuint texture, GLint level);
+GLAPI void APIENTRY glFramebufferTexture2DEXT (GLenum target, GLenum attachment, GLenum textarget, GLuint texture, GLint level);
+GLAPI void APIENTRY glFramebufferTexture3DEXT (GLenum target, GLenum attachment, GLenum textarget, GLuint texture, GLint level, GLint zoffset);
+GLAPI void APIENTRY glFramebufferRenderbufferEXT (GLenum target, GLenum attachment, GLenum renderbuffertarget, GLuint renderbuffer);
+GLAPI void APIENTRY glGetFramebufferAttachmentParameterivEXT (GLenum target, GLenum attachment, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGenerateMipmapEXT (GLenum target);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef GLboolean (APIENTRYP PFNGLISRENDERBUFFEREXTPROC) (GLuint renderbuffer);
+typedef void (APIENTRYP PFNGLBINDRENDERBUFFEREXTPROC) (GLenum target, GLuint renderbuffer);
+typedef void (APIENTRYP PFNGLDELETERENDERBUFFERSEXTPROC) (GLsizei n, const GLuint *renderbuffers);
+typedef void (APIENTRYP PFNGLGENRENDERBUFFERSEXTPROC) (GLsizei n, GLuint *renderbuffers);
+typedef void (APIENTRYP PFNGLRENDERBUFFERSTORAGEEXTPROC) (GLenum target, GLenum internalformat, GLsizei width, GLsizei height);
+typedef void (APIENTRYP PFNGLGETRENDERBUFFERPARAMETERIVEXTPROC) (GLenum target, GLenum pname, GLint *params);
+typedef GLboolean (APIENTRYP PFNGLISFRAMEBUFFEREXTPROC) (GLuint framebuffer);
+typedef void (APIENTRYP PFNGLBINDFRAMEBUFFEREXTPROC) (GLenum target, GLuint framebuffer);
+typedef void (APIENTRYP PFNGLDELETEFRAMEBUFFERSEXTPROC) (GLsizei n, const GLuint *framebuffers);
+typedef void (APIENTRYP PFNGLGENFRAMEBUFFERSEXTPROC) (GLsizei n, GLuint *framebuffers);
+typedef GLenum (APIENTRYP PFNGLCHECKFRAMEBUFFERSTATUSEXTPROC) (GLenum target);
+typedef void (APIENTRYP PFNGLFRAMEBUFFERTEXTURE1DEXTPROC) (GLenum target, GLenum attachment, GLenum textarget, GLuint texture, GLint level);
+typedef void (APIENTRYP PFNGLFRAMEBUFFERTEXTURE2DEXTPROC) (GLenum target, GLenum attachment, GLenum textarget, GLuint texture, GLint level);
+typedef void (APIENTRYP PFNGLFRAMEBUFFERTEXTURE3DEXTPROC) (GLenum target, GLenum attachment, GLenum textarget, GLuint texture, GLint level, GLint zoffset);
+typedef void (APIENTRYP PFNGLFRAMEBUFFERRENDERBUFFEREXTPROC) (GLenum target, GLenum attachment, GLenum renderbuffertarget, GLuint renderbuffer);
+typedef void (APIENTRYP PFNGLGETFRAMEBUFFERATTACHMENTPARAMETERIVEXTPROC) (GLenum target, GLenum attachment, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGENERATEMIPMAPEXTPROC) (GLenum target);
+#endif
+
+#ifndef GL_GREMEDY_string_marker
+#define GL_GREMEDY_string_marker 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glStringMarkerGREMEDY (GLsizei len, const GLvoid *string);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLSTRINGMARKERGREMEDYPROC) (GLsizei len, const GLvoid *string);
+#endif
+
+#ifndef GL_EXT_packed_depth_stencil
+#define GL_EXT_packed_depth_stencil 1
+#endif
+
+#ifndef GL_EXT_stencil_clear_tag
+#define GL_EXT_stencil_clear_tag 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glStencilClearTagEXT (GLsizei stencilTagBits, GLuint stencilClearTag);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLSTENCILCLEARTAGEXTPROC) (GLsizei stencilTagBits, GLuint stencilClearTag);
+#endif
+
+#ifndef GL_EXT_texture_sRGB
+#define GL_EXT_texture_sRGB 1
+#endif
+
+#ifndef GL_EXT_framebuffer_blit
+#define GL_EXT_framebuffer_blit 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBlitFramebufferEXT (GLint srcX0, GLint srcY0, GLint srcX1, GLint srcY1, GLint dstX0, GLint dstY0, GLint dstX1, GLint dstY1, GLbitfield mask, GLenum filter);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBLITFRAMEBUFFEREXTPROC) (GLint srcX0, GLint srcY0, GLint srcX1, GLint srcY1, GLint dstX0, GLint dstY0, GLint dstX1, GLint dstY1, GLbitfield mask, GLenum filter);
+#endif
+
+#ifndef GL_EXT_framebuffer_multisample
+#define GL_EXT_framebuffer_multisample 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glRenderbufferStorageMultisampleEXT (GLenum target, GLsizei samples, GLenum internalformat, GLsizei width, GLsizei height);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLRENDERBUFFERSTORAGEMULTISAMPLEEXTPROC) (GLenum target, GLsizei samples, GLenum internalformat, GLsizei width, GLsizei height);
+#endif
+
+#ifndef GL_MESAX_texture_stack
+#define GL_MESAX_texture_stack 1
+#endif
+
+#ifndef GL_EXT_timer_query
+#define GL_EXT_timer_query 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glGetQueryObjecti64vEXT (GLuint id, GLenum pname, GLint64EXT *params);
+GLAPI void APIENTRY glGetQueryObjectui64vEXT (GLuint id, GLenum pname, GLuint64EXT *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLGETQUERYOBJECTI64VEXTPROC) (GLuint id, GLenum pname, GLint64EXT *params);
+typedef void (APIENTRYP PFNGLGETQUERYOBJECTUI64VEXTPROC) (GLuint id, GLenum pname, GLuint64EXT *params);
+#endif
+
+#ifndef GL_EXT_gpu_program_parameters
+#define GL_EXT_gpu_program_parameters 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glProgramEnvParameters4fvEXT (GLenum target, GLuint index, GLsizei count, const GLfloat *params);
+GLAPI void APIENTRY glProgramLocalParameters4fvEXT (GLenum target, GLuint index, GLsizei count, const GLfloat *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPROGRAMENVPARAMETERS4FVEXTPROC) (GLenum target, GLuint index, GLsizei count, const GLfloat *params);
+typedef void (APIENTRYP PFNGLPROGRAMLOCALPARAMETERS4FVEXTPROC) (GLenum target, GLuint index, GLsizei count, const GLfloat *params);
+#endif
+
+#ifndef GL_APPLE_flush_buffer_range
+#define GL_APPLE_flush_buffer_range 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBufferParameteriAPPLE (GLenum target, GLenum pname, GLint param);
+GLAPI void APIENTRY glFlushMappedBufferRangeAPPLE (GLenum target, GLintptr offset, GLsizeiptr size);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBUFFERPARAMETERIAPPLEPROC) (GLenum target, GLenum pname, GLint param);
+typedef void (APIENTRYP PFNGLFLUSHMAPPEDBUFFERRANGEAPPLEPROC) (GLenum target, GLintptr offset, GLsizeiptr size);
+#endif
+
+#ifndef GL_NV_gpu_program4
+#define GL_NV_gpu_program4 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glProgramLocalParameterI4iNV (GLenum target, GLuint index, GLint x, GLint y, GLint z, GLint w);
+GLAPI void APIENTRY glProgramLocalParameterI4ivNV (GLenum target, GLuint index, const GLint *params);
+GLAPI void APIENTRY glProgramLocalParametersI4ivNV (GLenum target, GLuint index, GLsizei count, const GLint *params);
+GLAPI void APIENTRY glProgramLocalParameterI4uiNV (GLenum target, GLuint index, GLuint x, GLuint y, GLuint z, GLuint w);
+GLAPI void APIENTRY glProgramLocalParameterI4uivNV (GLenum target, GLuint index, const GLuint *params);
+GLAPI void APIENTRY glProgramLocalParametersI4uivNV (GLenum target, GLuint index, GLsizei count, const GLuint *params);
+GLAPI void APIENTRY glProgramEnvParameterI4iNV (GLenum target, GLuint index, GLint x, GLint y, GLint z, GLint w);
+GLAPI void APIENTRY glProgramEnvParameterI4ivNV (GLenum target, GLuint index, const GLint *params);
+GLAPI void APIENTRY glProgramEnvParametersI4ivNV (GLenum target, GLuint index, GLsizei count, const GLint *params);
+GLAPI void APIENTRY glProgramEnvParameterI4uiNV (GLenum target, GLuint index, GLuint x, GLuint y, GLuint z, GLuint w);
+GLAPI void APIENTRY glProgramEnvParameterI4uivNV (GLenum target, GLuint index, const GLuint *params);
+GLAPI void APIENTRY glProgramEnvParametersI4uivNV (GLenum target, GLuint index, GLsizei count, const GLuint *params);
+GLAPI void APIENTRY glGetProgramLocalParameterIivNV (GLenum target, GLuint index, GLint *params);
+GLAPI void APIENTRY glGetProgramLocalParameterIuivNV (GLenum target, GLuint index, GLuint *params);
+GLAPI void APIENTRY glGetProgramEnvParameterIivNV (GLenum target, GLuint index, GLint *params);
+GLAPI void APIENTRY glGetProgramEnvParameterIuivNV (GLenum target, GLuint index, GLuint *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPROGRAMLOCALPARAMETERI4INVPROC) (GLenum target, GLuint index, GLint x, GLint y, GLint z, GLint w);
+typedef void (APIENTRYP PFNGLPROGRAMLOCALPARAMETERI4IVNVPROC) (GLenum target, GLuint index, const GLint *params);
+typedef void (APIENTRYP PFNGLPROGRAMLOCALPARAMETERSI4IVNVPROC) (GLenum target, GLuint index, GLsizei count, const GLint *params);
+typedef void (APIENTRYP PFNGLPROGRAMLOCALPARAMETERI4UINVPROC) (GLenum target, GLuint index, GLuint x, GLuint y, GLuint z, GLuint w);
+typedef void (APIENTRYP PFNGLPROGRAMLOCALPARAMETERI4UIVNVPROC) (GLenum target, GLuint index, const GLuint *params);
+typedef void (APIENTRYP PFNGLPROGRAMLOCALPARAMETERSI4UIVNVPROC) (GLenum target, GLuint index, GLsizei count, const GLuint *params);
+typedef void (APIENTRYP PFNGLPROGRAMENVPARAMETERI4INVPROC) (GLenum target, GLuint index, GLint x, GLint y, GLint z, GLint w);
+typedef void (APIENTRYP PFNGLPROGRAMENVPARAMETERI4IVNVPROC) (GLenum target, GLuint index, const GLint *params);
+typedef void (APIENTRYP PFNGLPROGRAMENVPARAMETERSI4IVNVPROC) (GLenum target, GLuint index, GLsizei count, const GLint *params);
+typedef void (APIENTRYP PFNGLPROGRAMENVPARAMETERI4UINVPROC) (GLenum target, GLuint index, GLuint x, GLuint y, GLuint z, GLuint w);
+typedef void (APIENTRYP PFNGLPROGRAMENVPARAMETERI4UIVNVPROC) (GLenum target, GLuint index, const GLuint *params);
+typedef void (APIENTRYP PFNGLPROGRAMENVPARAMETERSI4UIVNVPROC) (GLenum target, GLuint index, GLsizei count, const GLuint *params);
+typedef void (APIENTRYP PFNGLGETPROGRAMLOCALPARAMETERIIVNVPROC) (GLenum target, GLuint index, GLint *params);
+typedef void (APIENTRYP PFNGLGETPROGRAMLOCALPARAMETERIUIVNVPROC) (GLenum target, GLuint index, GLuint *params);
+typedef void (APIENTRYP PFNGLGETPROGRAMENVPARAMETERIIVNVPROC) (GLenum target, GLuint index, GLint *params);
+typedef void (APIENTRYP PFNGLGETPROGRAMENVPARAMETERIUIVNVPROC) (GLenum target, GLuint index, GLuint *params);
+#endif
+
+#ifndef GL_NV_geometry_program4
+#define GL_NV_geometry_program4 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glProgramVertexLimitNV (GLenum target, GLint limit);
+GLAPI void APIENTRY glFramebufferTextureEXT (GLenum target, GLenum attachment, GLuint texture, GLint level);
+GLAPI void APIENTRY glFramebufferTextureLayerEXT (GLenum target, GLenum attachment, GLuint texture, GLint level, GLint layer);
+GLAPI void APIENTRY glFramebufferTextureFaceEXT (GLenum target, GLenum attachment, GLuint texture, GLint level, GLenum face);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPROGRAMVERTEXLIMITNVPROC) (GLenum target, GLint limit);
+typedef void (APIENTRYP PFNGLFRAMEBUFFERTEXTUREEXTPROC) (GLenum target, GLenum attachment, GLuint texture, GLint level);
+typedef void (APIENTRYP PFNGLFRAMEBUFFERTEXTURELAYEREXTPROC) (GLenum target, GLenum attachment, GLuint texture, GLint level, GLint layer);
+typedef void (APIENTRYP PFNGLFRAMEBUFFERTEXTUREFACEEXTPROC) (GLenum target, GLenum attachment, GLuint texture, GLint level, GLenum face);
+#endif
+
+#ifndef GL_EXT_geometry_shader4
+#define GL_EXT_geometry_shader4 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glProgramParameteriEXT (GLuint program, GLenum pname, GLint value);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPROGRAMPARAMETERIEXTPROC) (GLuint program, GLenum pname, GLint value);
+#endif
+
+#ifndef GL_NV_vertex_program4
+#define GL_NV_vertex_program4 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glVertexAttribI1iEXT (GLuint index, GLint x);
+GLAPI void APIENTRY glVertexAttribI2iEXT (GLuint index, GLint x, GLint y);
+GLAPI void APIENTRY glVertexAttribI3iEXT (GLuint index, GLint x, GLint y, GLint z);
+GLAPI void APIENTRY glVertexAttribI4iEXT (GLuint index, GLint x, GLint y, GLint z, GLint w);
+GLAPI void APIENTRY glVertexAttribI1uiEXT (GLuint index, GLuint x);
+GLAPI void APIENTRY glVertexAttribI2uiEXT (GLuint index, GLuint x, GLuint y);
+GLAPI void APIENTRY glVertexAttribI3uiEXT (GLuint index, GLuint x, GLuint y, GLuint z);
+GLAPI void APIENTRY glVertexAttribI4uiEXT (GLuint index, GLuint x, GLuint y, GLuint z, GLuint w);
+GLAPI void APIENTRY glVertexAttribI1ivEXT (GLuint index, const GLint *v);
+GLAPI void APIENTRY glVertexAttribI2ivEXT (GLuint index, const GLint *v);
+GLAPI void APIENTRY glVertexAttribI3ivEXT (GLuint index, const GLint *v);
+GLAPI void APIENTRY glVertexAttribI4ivEXT (GLuint index, const GLint *v);
+GLAPI void APIENTRY glVertexAttribI1uivEXT (GLuint index, const GLuint *v);
+GLAPI void APIENTRY glVertexAttribI2uivEXT (GLuint index, const GLuint *v);
+GLAPI void APIENTRY glVertexAttribI3uivEXT (GLuint index, const GLuint *v);
+GLAPI void APIENTRY glVertexAttribI4uivEXT (GLuint index, const GLuint *v);
+GLAPI void APIENTRY glVertexAttribI4bvEXT (GLuint index, const GLbyte *v);
+GLAPI void APIENTRY glVertexAttribI4svEXT (GLuint index, const GLshort *v);
+GLAPI void APIENTRY glVertexAttribI4ubvEXT (GLuint index, const GLubyte *v);
+GLAPI void APIENTRY glVertexAttribI4usvEXT (GLuint index, const GLushort *v);
+GLAPI void APIENTRY glVertexAttribIPointerEXT (GLuint index, GLint size, GLenum type, GLsizei stride, const GLvoid *pointer);
+GLAPI void APIENTRY glGetVertexAttribIivEXT (GLuint index, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetVertexAttribIuivEXT (GLuint index, GLenum pname, GLuint *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI1IEXTPROC) (GLuint index, GLint x);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI2IEXTPROC) (GLuint index, GLint x, GLint y);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI3IEXTPROC) (GLuint index, GLint x, GLint y, GLint z);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI4IEXTPROC) (GLuint index, GLint x, GLint y, GLint z, GLint w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI1UIEXTPROC) (GLuint index, GLuint x);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI2UIEXTPROC) (GLuint index, GLuint x, GLuint y);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI3UIEXTPROC) (GLuint index, GLuint x, GLuint y, GLuint z);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI4UIEXTPROC) (GLuint index, GLuint x, GLuint y, GLuint z, GLuint w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI1IVEXTPROC) (GLuint index, const GLint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI2IVEXTPROC) (GLuint index, const GLint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI3IVEXTPROC) (GLuint index, const GLint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI4IVEXTPROC) (GLuint index, const GLint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI1UIVEXTPROC) (GLuint index, const GLuint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI2UIVEXTPROC) (GLuint index, const GLuint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI3UIVEXTPROC) (GLuint index, const GLuint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI4UIVEXTPROC) (GLuint index, const GLuint *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI4BVEXTPROC) (GLuint index, const GLbyte *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI4SVEXTPROC) (GLuint index, const GLshort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI4UBVEXTPROC) (GLuint index, const GLubyte *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBI4USVEXTPROC) (GLuint index, const GLushort *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBIPOINTEREXTPROC) (GLuint index, GLint size, GLenum type, GLsizei stride, const GLvoid *pointer);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBIIVEXTPROC) (GLuint index, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBIUIVEXTPROC) (GLuint index, GLenum pname, GLuint *params);
+#endif
+
+#ifndef GL_EXT_gpu_shader4
+#define GL_EXT_gpu_shader4 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glGetUniformuivEXT (GLuint program, GLint location, GLuint *params);
+GLAPI void APIENTRY glBindFragDataLocationEXT (GLuint program, GLuint color, const GLchar *name);
+GLAPI GLint APIENTRY glGetFragDataLocationEXT (GLuint program, const GLchar *name);
+GLAPI void APIENTRY glUniform1uiEXT (GLint location, GLuint v0);
+GLAPI void APIENTRY glUniform2uiEXT (GLint location, GLuint v0, GLuint v1);
+GLAPI void APIENTRY glUniform3uiEXT (GLint location, GLuint v0, GLuint v1, GLuint v2);
+GLAPI void APIENTRY glUniform4uiEXT (GLint location, GLuint v0, GLuint v1, GLuint v2, GLuint v3);
+GLAPI void APIENTRY glUniform1uivEXT (GLint location, GLsizei count, const GLuint *value);
+GLAPI void APIENTRY glUniform2uivEXT (GLint location, GLsizei count, const GLuint *value);
+GLAPI void APIENTRY glUniform3uivEXT (GLint location, GLsizei count, const GLuint *value);
+GLAPI void APIENTRY glUniform4uivEXT (GLint location, GLsizei count, const GLuint *value);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLGETUNIFORMUIVEXTPROC) (GLuint program, GLint location, GLuint *params);
+typedef void (APIENTRYP PFNGLBINDFRAGDATALOCATIONEXTPROC) (GLuint program, GLuint color, const GLchar *name);
+typedef GLint (APIENTRYP PFNGLGETFRAGDATALOCATIONEXTPROC) (GLuint program, const GLchar *name);
+typedef void (APIENTRYP PFNGLUNIFORM1UIEXTPROC) (GLint location, GLuint v0);
+typedef void (APIENTRYP PFNGLUNIFORM2UIEXTPROC) (GLint location, GLuint v0, GLuint v1);
+typedef void (APIENTRYP PFNGLUNIFORM3UIEXTPROC) (GLint location, GLuint v0, GLuint v1, GLuint v2);
+typedef void (APIENTRYP PFNGLUNIFORM4UIEXTPROC) (GLint location, GLuint v0, GLuint v1, GLuint v2, GLuint v3);
+typedef void (APIENTRYP PFNGLUNIFORM1UIVEXTPROC) (GLint location, GLsizei count, const GLuint *value);
+typedef void (APIENTRYP PFNGLUNIFORM2UIVEXTPROC) (GLint location, GLsizei count, const GLuint *value);
+typedef void (APIENTRYP PFNGLUNIFORM3UIVEXTPROC) (GLint location, GLsizei count, const GLuint *value);
+typedef void (APIENTRYP PFNGLUNIFORM4UIVEXTPROC) (GLint location, GLsizei count, const GLuint *value);
+#endif
+
+#ifndef GL_EXT_draw_instanced
+#define GL_EXT_draw_instanced 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glDrawArraysInstancedEXT (GLenum mode, GLint start, GLsizei count, GLsizei primcount);
+GLAPI void APIENTRY glDrawElementsInstancedEXT (GLenum mode, GLsizei count, GLenum type, const GLvoid *indices, GLsizei primcount);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLDRAWARRAYSINSTANCEDEXTPROC) (GLenum mode, GLint start, GLsizei count, GLsizei primcount);
+typedef void (APIENTRYP PFNGLDRAWELEMENTSINSTANCEDEXTPROC) (GLenum mode, GLsizei count, GLenum type, const GLvoid *indices, GLsizei primcount);
+#endif
+
+#ifndef GL_EXT_packed_float
+#define GL_EXT_packed_float 1
+#endif
+
+#ifndef GL_EXT_texture_array
+#define GL_EXT_texture_array 1
+#endif
+
+#ifndef GL_EXT_texture_buffer_object
+#define GL_EXT_texture_buffer_object 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glTexBufferEXT (GLenum target, GLenum internalformat, GLuint buffer);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLTEXBUFFEREXTPROC) (GLenum target, GLenum internalformat, GLuint buffer);
+#endif
+
+#ifndef GL_EXT_texture_compression_latc
+#define GL_EXT_texture_compression_latc 1
+#endif
+
+#ifndef GL_EXT_texture_compression_rgtc
+#define GL_EXT_texture_compression_rgtc 1
+#endif
+
+#ifndef GL_EXT_texture_shared_exponent
+#define GL_EXT_texture_shared_exponent 1
+#endif
+
+#ifndef GL_NV_depth_buffer_float
+#define GL_NV_depth_buffer_float 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glDepthRangedNV (GLdouble zNear, GLdouble zFar);
+GLAPI void APIENTRY glClearDepthdNV (GLdouble depth);
+GLAPI void APIENTRY glDepthBoundsdNV (GLdouble zmin, GLdouble zmax);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLDEPTHRANGEDNVPROC) (GLdouble zNear, GLdouble zFar);
+typedef void (APIENTRYP PFNGLCLEARDEPTHDNVPROC) (GLdouble depth);
+typedef void (APIENTRYP PFNGLDEPTHBOUNDSDNVPROC) (GLdouble zmin, GLdouble zmax);
+#endif
+
+#ifndef GL_NV_fragment_program4
+#define GL_NV_fragment_program4 1
+#endif
+
+#ifndef GL_NV_framebuffer_multisample_coverage
+#define GL_NV_framebuffer_multisample_coverage 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glRenderbufferStorageMultisampleCoverageNV (GLenum target, GLsizei coverageSamples, GLsizei colorSamples, GLenum internalformat, GLsizei width, GLsizei height);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLRENDERBUFFERSTORAGEMULTISAMPLECOVERAGENVPROC) (GLenum target, GLsizei coverageSamples, GLsizei colorSamples, GLenum internalformat, GLsizei width, GLsizei height);
+#endif
+
+#ifndef GL_EXT_framebuffer_sRGB
+#define GL_EXT_framebuffer_sRGB 1
+#endif
+
+#ifndef GL_NV_geometry_shader4
+#define GL_NV_geometry_shader4 1
+#endif
+
+#ifndef GL_NV_parameter_buffer_object
+#define GL_NV_parameter_buffer_object 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glProgramBufferParametersfvNV (GLenum target, GLuint buffer, GLuint index, GLsizei count, const GLfloat *params);
+GLAPI void APIENTRY glProgramBufferParametersIivNV (GLenum target, GLuint buffer, GLuint index, GLsizei count, const GLint *params);
+GLAPI void APIENTRY glProgramBufferParametersIuivNV (GLenum target, GLuint buffer, GLuint index, GLsizei count, const GLuint *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPROGRAMBUFFERPARAMETERSFVNVPROC) (GLenum target, GLuint buffer, GLuint index, GLsizei count, const GLfloat *params);
+typedef void (APIENTRYP PFNGLPROGRAMBUFFERPARAMETERSIIVNVPROC) (GLenum target, GLuint buffer, GLuint index, GLsizei count, const GLint *params);
+typedef void (APIENTRYP PFNGLPROGRAMBUFFERPARAMETERSIUIVNVPROC) (GLenum target, GLuint buffer, GLuint index, GLsizei count, const GLuint *params);
+#endif
+
+#ifndef GL_EXT_draw_buffers2
+#define GL_EXT_draw_buffers2 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glColorMaskIndexedEXT (GLuint index, GLboolean r, GLboolean g, GLboolean b, GLboolean a);
+GLAPI void APIENTRY glGetBooleanIndexedvEXT (GLenum target, GLuint index, GLboolean *data);
+GLAPI void APIENTRY glGetIntegerIndexedvEXT (GLenum target, GLuint index, GLint *data);
+GLAPI void APIENTRY glEnableIndexedEXT (GLenum target, GLuint index);
+GLAPI void APIENTRY glDisableIndexedEXT (GLenum target, GLuint index);
+GLAPI GLboolean APIENTRY glIsEnabledIndexedEXT (GLenum target, GLuint index);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCOLORMASKINDEXEDEXTPROC) (GLuint index, GLboolean r, GLboolean g, GLboolean b, GLboolean a);
+typedef void (APIENTRYP PFNGLGETBOOLEANINDEXEDVEXTPROC) (GLenum target, GLuint index, GLboolean *data);
+typedef void (APIENTRYP PFNGLGETINTEGERINDEXEDVEXTPROC) (GLenum target, GLuint index, GLint *data);
+typedef void (APIENTRYP PFNGLENABLEINDEXEDEXTPROC) (GLenum target, GLuint index);
+typedef void (APIENTRYP PFNGLDISABLEINDEXEDEXTPROC) (GLenum target, GLuint index);
+typedef GLboolean (APIENTRYP PFNGLISENABLEDINDEXEDEXTPROC) (GLenum target, GLuint index);
+#endif
+
+#ifndef GL_NV_transform_feedback
+#define GL_NV_transform_feedback 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBeginTransformFeedbackNV (GLenum primitiveMode);
+GLAPI void APIENTRY glEndTransformFeedbackNV (void);
+GLAPI void APIENTRY glTransformFeedbackAttribsNV (GLuint count, const GLint *attribs, GLenum bufferMode);
+GLAPI void APIENTRY glBindBufferRangeNV (GLenum target, GLuint index, GLuint buffer, GLintptr offset, GLsizeiptr size);
+GLAPI void APIENTRY glBindBufferOffsetNV (GLenum target, GLuint index, GLuint buffer, GLintptr offset);
+GLAPI void APIENTRY glBindBufferBaseNV (GLenum target, GLuint index, GLuint buffer);
+GLAPI void APIENTRY glTransformFeedbackVaryingsNV (GLuint program, GLsizei count, const GLint *locations, GLenum bufferMode);
+GLAPI void APIENTRY glActiveVaryingNV (GLuint program, const GLchar *name);
+GLAPI GLint APIENTRY glGetVaryingLocationNV (GLuint program, const GLchar *name);
+GLAPI void APIENTRY glGetActiveVaryingNV (GLuint program, GLuint index, GLsizei bufSize, GLsizei *length, GLsizei *size, GLenum *type, GLchar *name);
+GLAPI void APIENTRY glGetTransformFeedbackVaryingNV (GLuint program, GLuint index, GLint *location);
+GLAPI void APIENTRY glTransformFeedbackStreamAttribsNV (GLsizei count, const GLint *attribs, GLsizei nbuffers, const GLint *bufstreams, GLenum bufferMode);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBEGINTRANSFORMFEEDBACKNVPROC) (GLenum primitiveMode);
+typedef void (APIENTRYP PFNGLENDTRANSFORMFEEDBACKNVPROC) (void);
+typedef void (APIENTRYP PFNGLTRANSFORMFEEDBACKATTRIBSNVPROC) (GLuint count, const GLint *attribs, GLenum bufferMode);
+typedef void (APIENTRYP PFNGLBINDBUFFERRANGENVPROC) (GLenum target, GLuint index, GLuint buffer, GLintptr offset, GLsizeiptr size);
+typedef void (APIENTRYP PFNGLBINDBUFFEROFFSETNVPROC) (GLenum target, GLuint index, GLuint buffer, GLintptr offset);
+typedef void (APIENTRYP PFNGLBINDBUFFERBASENVPROC) (GLenum target, GLuint index, GLuint buffer);
+typedef void (APIENTRYP PFNGLTRANSFORMFEEDBACKVARYINGSNVPROC) (GLuint program, GLsizei count, const GLint *locations, GLenum bufferMode);
+typedef void (APIENTRYP PFNGLACTIVEVARYINGNVPROC) (GLuint program, const GLchar *name);
+typedef GLint (APIENTRYP PFNGLGETVARYINGLOCATIONNVPROC) (GLuint program, const GLchar *name);
+typedef void (APIENTRYP PFNGLGETACTIVEVARYINGNVPROC) (GLuint program, GLuint index, GLsizei bufSize, GLsizei *length, GLsizei *size, GLenum *type, GLchar *name);
+typedef void (APIENTRYP PFNGLGETTRANSFORMFEEDBACKVARYINGNVPROC) (GLuint program, GLuint index, GLint *location);
+typedef void (APIENTRYP PFNGLTRANSFORMFEEDBACKSTREAMATTRIBSNVPROC) (GLsizei count, const GLint *attribs, GLsizei nbuffers, const GLint *bufstreams, GLenum bufferMode);
+#endif
+
+#ifndef GL_EXT_bindable_uniform
+#define GL_EXT_bindable_uniform 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glUniformBufferEXT (GLuint program, GLint location, GLuint buffer);
+GLAPI GLint APIENTRY glGetUniformBufferSizeEXT (GLuint program, GLint location);
+GLAPI GLintptr APIENTRY glGetUniformOffsetEXT (GLuint program, GLint location);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLUNIFORMBUFFEREXTPROC) (GLuint program, GLint location, GLuint buffer);
+typedef GLint (APIENTRYP PFNGLGETUNIFORMBUFFERSIZEEXTPROC) (GLuint program, GLint location);
+typedef GLintptr (APIENTRYP PFNGLGETUNIFORMOFFSETEXTPROC) (GLuint program, GLint location);
+#endif
+
+#ifndef GL_EXT_texture_integer
+#define GL_EXT_texture_integer 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glTexParameterIivEXT (GLenum target, GLenum pname, const GLint *params);
+GLAPI void APIENTRY glTexParameterIuivEXT (GLenum target, GLenum pname, const GLuint *params);
+GLAPI void APIENTRY glGetTexParameterIivEXT (GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetTexParameterIuivEXT (GLenum target, GLenum pname, GLuint *params);
+GLAPI void APIENTRY glClearColorIiEXT (GLint red, GLint green, GLint blue, GLint alpha);
+GLAPI void APIENTRY glClearColorIuiEXT (GLuint red, GLuint green, GLuint blue, GLuint alpha);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLTEXPARAMETERIIVEXTPROC) (GLenum target, GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLTEXPARAMETERIUIVEXTPROC) (GLenum target, GLenum pname, const GLuint *params);
+typedef void (APIENTRYP PFNGLGETTEXPARAMETERIIVEXTPROC) (GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETTEXPARAMETERIUIVEXTPROC) (GLenum target, GLenum pname, GLuint *params);
+typedef void (APIENTRYP PFNGLCLEARCOLORIIEXTPROC) (GLint red, GLint green, GLint blue, GLint alpha);
+typedef void (APIENTRYP PFNGLCLEARCOLORIUIEXTPROC) (GLuint red, GLuint green, GLuint blue, GLuint alpha);
+#endif
+
+#ifndef GL_GREMEDY_frame_terminator
+#define GL_GREMEDY_frame_terminator 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glFrameTerminatorGREMEDY (void);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLFRAMETERMINATORGREMEDYPROC) (void);
+#endif
+
+#ifndef GL_NV_conditional_render
+#define GL_NV_conditional_render 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBeginConditionalRenderNV (GLuint id, GLenum mode);
+GLAPI void APIENTRY glEndConditionalRenderNV (void);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBEGINCONDITIONALRENDERNVPROC) (GLuint id, GLenum mode);
+typedef void (APIENTRYP PFNGLENDCONDITIONALRENDERNVPROC) (void);
+#endif
+
+#ifndef GL_NV_present_video
+#define GL_NV_present_video 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glPresentFrameKeyedNV (GLuint video_slot, GLuint64EXT minPresentTime, GLuint beginPresentTimeId, GLuint presentDurationId, GLenum type, GLenum target0, GLuint fill0, GLuint key0, GLenum target1, GLuint fill1, GLuint key1);
+GLAPI void APIENTRY glPresentFrameDualFillNV (GLuint video_slot, GLuint64EXT minPresentTime, GLuint beginPresentTimeId, GLuint presentDurationId, GLenum type, GLenum target0, GLuint fill0, GLenum target1, GLuint fill1, GLenum target2, GLuint fill2, GLenum target3, GLuint fill3);
+GLAPI void APIENTRY glGetVideoivNV (GLuint video_slot, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetVideouivNV (GLuint video_slot, GLenum pname, GLuint *params);
+GLAPI void APIENTRY glGetVideoi64vNV (GLuint video_slot, GLenum pname, GLint64EXT *params);
+GLAPI void APIENTRY glGetVideoui64vNV (GLuint video_slot, GLenum pname, GLuint64EXT *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPRESENTFRAMEKEYEDNVPROC) (GLuint video_slot, GLuint64EXT minPresentTime, GLuint beginPresentTimeId, GLuint presentDurationId, GLenum type, GLenum target0, GLuint fill0, GLuint key0, GLenum target1, GLuint fill1, GLuint key1);
+typedef void (APIENTRYP PFNGLPRESENTFRAMEDUALFILLNVPROC) (GLuint video_slot, GLuint64EXT minPresentTime, GLuint beginPresentTimeId, GLuint presentDurationId, GLenum type, GLenum target0, GLuint fill0, GLenum target1, GLuint fill1, GLenum target2, GLuint fill2, GLenum target3, GLuint fill3);
+typedef void (APIENTRYP PFNGLGETVIDEOIVNVPROC) (GLuint video_slot, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETVIDEOUIVNVPROC) (GLuint video_slot, GLenum pname, GLuint *params);
+typedef void (APIENTRYP PFNGLGETVIDEOI64VNVPROC) (GLuint video_slot, GLenum pname, GLint64EXT *params);
+typedef void (APIENTRYP PFNGLGETVIDEOUI64VNVPROC) (GLuint video_slot, GLenum pname, GLuint64EXT *params);
+#endif
+
+#ifndef GL_EXT_transform_feedback
+#define GL_EXT_transform_feedback 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBeginTransformFeedbackEXT (GLenum primitiveMode);
+GLAPI void APIENTRY glEndTransformFeedbackEXT (void);
+GLAPI void APIENTRY glBindBufferRangeEXT (GLenum target, GLuint index, GLuint buffer, GLintptr offset, GLsizeiptr size);
+GLAPI void APIENTRY glBindBufferOffsetEXT (GLenum target, GLuint index, GLuint buffer, GLintptr offset);
+GLAPI void APIENTRY glBindBufferBaseEXT (GLenum target, GLuint index, GLuint buffer);
+GLAPI void APIENTRY glTransformFeedbackVaryingsEXT (GLuint program, GLsizei count, const GLchar* *varyings, GLenum bufferMode);
+GLAPI void APIENTRY glGetTransformFeedbackVaryingEXT (GLuint program, GLuint index, GLsizei bufSize, GLsizei *length, GLsizei *size, GLenum *type, GLchar *name);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBEGINTRANSFORMFEEDBACKEXTPROC) (GLenum primitiveMode);
+typedef void (APIENTRYP PFNGLENDTRANSFORMFEEDBACKEXTPROC) (void);
+typedef void (APIENTRYP PFNGLBINDBUFFERRANGEEXTPROC) (GLenum target, GLuint index, GLuint buffer, GLintptr offset, GLsizeiptr size);
+typedef void (APIENTRYP PFNGLBINDBUFFEROFFSETEXTPROC) (GLenum target, GLuint index, GLuint buffer, GLintptr offset);
+typedef void (APIENTRYP PFNGLBINDBUFFERBASEEXTPROC) (GLenum target, GLuint index, GLuint buffer);
+typedef void (APIENTRYP PFNGLTRANSFORMFEEDBACKVARYINGSEXTPROC) (GLuint program, GLsizei count, const GLchar* *varyings, GLenum bufferMode);
+typedef void (APIENTRYP PFNGLGETTRANSFORMFEEDBACKVARYINGEXTPROC) (GLuint program, GLuint index, GLsizei bufSize, GLsizei *length, GLsizei *size, GLenum *type, GLchar *name);
+#endif
+
+#ifndef GL_EXT_direct_state_access
+#define GL_EXT_direct_state_access 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glClientAttribDefaultEXT (GLbitfield mask);
+GLAPI void APIENTRY glPushClientAttribDefaultEXT (GLbitfield mask);
+GLAPI void APIENTRY glMatrixLoadfEXT (GLenum mode, const GLfloat *m);
+GLAPI void APIENTRY glMatrixLoaddEXT (GLenum mode, const GLdouble *m);
+GLAPI void APIENTRY glMatrixMultfEXT (GLenum mode, const GLfloat *m);
+GLAPI void APIENTRY glMatrixMultdEXT (GLenum mode, const GLdouble *m);
+GLAPI void APIENTRY glMatrixLoadIdentityEXT (GLenum mode);
+GLAPI void APIENTRY glMatrixRotatefEXT (GLenum mode, GLfloat angle, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glMatrixRotatedEXT (GLenum mode, GLdouble angle, GLdouble x, GLdouble y, GLdouble z);
+GLAPI void APIENTRY glMatrixScalefEXT (GLenum mode, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glMatrixScaledEXT (GLenum mode, GLdouble x, GLdouble y, GLdouble z);
+GLAPI void APIENTRY glMatrixTranslatefEXT (GLenum mode, GLfloat x, GLfloat y, GLfloat z);
+GLAPI void APIENTRY glMatrixTranslatedEXT (GLenum mode, GLdouble x, GLdouble y, GLdouble z);
+GLAPI void APIENTRY glMatrixFrustumEXT (GLenum mode, GLdouble left, GLdouble right, GLdouble bottom, GLdouble top, GLdouble zNear, GLdouble zFar);
+GLAPI void APIENTRY glMatrixOrthoEXT (GLenum mode, GLdouble left, GLdouble right, GLdouble bottom, GLdouble top, GLdouble zNear, GLdouble zFar);
+GLAPI void APIENTRY glMatrixPopEXT (GLenum mode);
+GLAPI void APIENTRY glMatrixPushEXT (GLenum mode);
+GLAPI void APIENTRY glMatrixLoadTransposefEXT (GLenum mode, const GLfloat *m);
+GLAPI void APIENTRY glMatrixLoadTransposedEXT (GLenum mode, const GLdouble *m);
+GLAPI void APIENTRY glMatrixMultTransposefEXT (GLenum mode, const GLfloat *m);
+GLAPI void APIENTRY glMatrixMultTransposedEXT (GLenum mode, const GLdouble *m);
+GLAPI void APIENTRY glTextureParameterfEXT (GLuint texture, GLenum target, GLenum pname, GLfloat param);
+GLAPI void APIENTRY glTextureParameterfvEXT (GLuint texture, GLenum target, GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glTextureParameteriEXT (GLuint texture, GLenum target, GLenum pname, GLint param);
+GLAPI void APIENTRY glTextureParameterivEXT (GLuint texture, GLenum target, GLenum pname, const GLint *params);
+GLAPI void APIENTRY glTextureImage1DEXT (GLuint texture, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLint border, GLenum format, GLenum type, const GLvoid *pixels);
+GLAPI void APIENTRY glTextureImage2DEXT (GLuint texture, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLint border, GLenum format, GLenum type, const GLvoid *pixels);
+GLAPI void APIENTRY glTextureSubImage1DEXT (GLuint texture, GLenum target, GLint level, GLint xoffset, GLsizei width, GLenum format, GLenum type, const GLvoid *pixels);
+GLAPI void APIENTRY glTextureSubImage2DEXT (GLuint texture, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLsizei width, GLsizei height, GLenum format, GLenum type, const GLvoid *pixels);
+GLAPI void APIENTRY glCopyTextureImage1DEXT (GLuint texture, GLenum target, GLint level, GLenum internalformat, GLint x, GLint y, GLsizei width, GLint border);
+GLAPI void APIENTRY glCopyTextureImage2DEXT (GLuint texture, GLenum target, GLint level, GLenum internalformat, GLint x, GLint y, GLsizei width, GLsizei height, GLint border);
+GLAPI void APIENTRY glCopyTextureSubImage1DEXT (GLuint texture, GLenum target, GLint level, GLint xoffset, GLint x, GLint y, GLsizei width);
+GLAPI void APIENTRY glCopyTextureSubImage2DEXT (GLuint texture, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint x, GLint y, GLsizei width, GLsizei height);
+GLAPI void APIENTRY glGetTextureImageEXT (GLuint texture, GLenum target, GLint level, GLenum format, GLenum type, GLvoid *pixels);
+GLAPI void APIENTRY glGetTextureParameterfvEXT (GLuint texture, GLenum target, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetTextureParameterivEXT (GLuint texture, GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetTextureLevelParameterfvEXT (GLuint texture, GLenum target, GLint level, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetTextureLevelParameterivEXT (GLuint texture, GLenum target, GLint level, GLenum pname, GLint *params);
+GLAPI void APIENTRY glTextureImage3DEXT (GLuint texture, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLsizei depth, GLint border, GLenum format, GLenum type, const GLvoid *pixels);
+GLAPI void APIENTRY glTextureSubImage3DEXT (GLuint texture, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLsizei width, GLsizei height, GLsizei depth, GLenum format, GLenum type, const GLvoid *pixels);
+GLAPI void APIENTRY glCopyTextureSubImage3DEXT (GLuint texture, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLint x, GLint y, GLsizei width, GLsizei height);
+GLAPI void APIENTRY glMultiTexParameterfEXT (GLenum texunit, GLenum target, GLenum pname, GLfloat param);
+GLAPI void APIENTRY glMultiTexParameterfvEXT (GLenum texunit, GLenum target, GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glMultiTexParameteriEXT (GLenum texunit, GLenum target, GLenum pname, GLint param);
+GLAPI void APIENTRY glMultiTexParameterivEXT (GLenum texunit, GLenum target, GLenum pname, const GLint *params);
+GLAPI void APIENTRY glMultiTexImage1DEXT (GLenum texunit, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLint border, GLenum format, GLenum type, const GLvoid *pixels);
+GLAPI void APIENTRY glMultiTexImage2DEXT (GLenum texunit, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLint border, GLenum format, GLenum type, const GLvoid *pixels);
+GLAPI void APIENTRY glMultiTexSubImage1DEXT (GLenum texunit, GLenum target, GLint level, GLint xoffset, GLsizei width, GLenum format, GLenum type, const GLvoid *pixels);
+GLAPI void APIENTRY glMultiTexSubImage2DEXT (GLenum texunit, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLsizei width, GLsizei height, GLenum format, GLenum type, const GLvoid *pixels);
+GLAPI void APIENTRY glCopyMultiTexImage1DEXT (GLenum texunit, GLenum target, GLint level, GLenum internalformat, GLint x, GLint y, GLsizei width, GLint border);
+GLAPI void APIENTRY glCopyMultiTexImage2DEXT (GLenum texunit, GLenum target, GLint level, GLenum internalformat, GLint x, GLint y, GLsizei width, GLsizei height, GLint border);
+GLAPI void APIENTRY glCopyMultiTexSubImage1DEXT (GLenum texunit, GLenum target, GLint level, GLint xoffset, GLint x, GLint y, GLsizei width);
+GLAPI void APIENTRY glCopyMultiTexSubImage2DEXT (GLenum texunit, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint x, GLint y, GLsizei width, GLsizei height);
+GLAPI void APIENTRY glGetMultiTexImageEXT (GLenum texunit, GLenum target, GLint level, GLenum format, GLenum type, GLvoid *pixels);
+GLAPI void APIENTRY glGetMultiTexParameterfvEXT (GLenum texunit, GLenum target, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetMultiTexParameterivEXT (GLenum texunit, GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetMultiTexLevelParameterfvEXT (GLenum texunit, GLenum target, GLint level, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetMultiTexLevelParameterivEXT (GLenum texunit, GLenum target, GLint level, GLenum pname, GLint *params);
+GLAPI void APIENTRY glMultiTexImage3DEXT (GLenum texunit, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLsizei depth, GLint border, GLenum format, GLenum type, const GLvoid *pixels);
+GLAPI void APIENTRY glMultiTexSubImage3DEXT (GLenum texunit, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLsizei width, GLsizei height, GLsizei depth, GLenum format, GLenum type, const GLvoid *pixels);
+GLAPI void APIENTRY glCopyMultiTexSubImage3DEXT (GLenum texunit, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLint x, GLint y, GLsizei width, GLsizei height);
+GLAPI void APIENTRY glBindMultiTextureEXT (GLenum texunit, GLenum target, GLuint texture);
+GLAPI void APIENTRY glEnableClientStateIndexedEXT (GLenum array, GLuint index);
+GLAPI void APIENTRY glDisableClientStateIndexedEXT (GLenum array, GLuint index);
+GLAPI void APIENTRY glMultiTexCoordPointerEXT (GLenum texunit, GLint size, GLenum type, GLsizei stride, const GLvoid *pointer);
+GLAPI void APIENTRY glMultiTexEnvfEXT (GLenum texunit, GLenum target, GLenum pname, GLfloat param);
+GLAPI void APIENTRY glMultiTexEnvfvEXT (GLenum texunit, GLenum target, GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glMultiTexEnviEXT (GLenum texunit, GLenum target, GLenum pname, GLint param);
+GLAPI void APIENTRY glMultiTexEnvivEXT (GLenum texunit, GLenum target, GLenum pname, const GLint *params);
+GLAPI void APIENTRY glMultiTexGendEXT (GLenum texunit, GLenum coord, GLenum pname, GLdouble param);
+GLAPI void APIENTRY glMultiTexGendvEXT (GLenum texunit, GLenum coord, GLenum pname, const GLdouble *params);
+GLAPI void APIENTRY glMultiTexGenfEXT (GLenum texunit, GLenum coord, GLenum pname, GLfloat param);
+GLAPI void APIENTRY glMultiTexGenfvEXT (GLenum texunit, GLenum coord, GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glMultiTexGeniEXT (GLenum texunit, GLenum coord, GLenum pname, GLint param);
+GLAPI void APIENTRY glMultiTexGenivEXT (GLenum texunit, GLenum coord, GLenum pname, const GLint *params);
+GLAPI void APIENTRY glGetMultiTexEnvfvEXT (GLenum texunit, GLenum target, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetMultiTexEnvivEXT (GLenum texunit, GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetMultiTexGendvEXT (GLenum texunit, GLenum coord, GLenum pname, GLdouble *params);
+GLAPI void APIENTRY glGetMultiTexGenfvEXT (GLenum texunit, GLenum coord, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetMultiTexGenivEXT (GLenum texunit, GLenum coord, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetFloatIndexedvEXT (GLenum target, GLuint index, GLfloat *data);
+GLAPI void APIENTRY glGetDoubleIndexedvEXT (GLenum target, GLuint index, GLdouble *data);
+GLAPI void APIENTRY glGetPointerIndexedvEXT (GLenum target, GLuint index, GLvoid* *data);
+GLAPI void APIENTRY glCompressedTextureImage3DEXT (GLuint texture, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLsizei depth, GLint border, GLsizei imageSize, const GLvoid *bits);
+GLAPI void APIENTRY glCompressedTextureImage2DEXT (GLuint texture, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLint border, GLsizei imageSize, const GLvoid *bits);
+GLAPI void APIENTRY glCompressedTextureImage1DEXT (GLuint texture, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLint border, GLsizei imageSize, const GLvoid *bits);
+GLAPI void APIENTRY glCompressedTextureSubImage3DEXT (GLuint texture, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLsizei width, GLsizei height, GLsizei depth, GLenum format, GLsizei imageSize, const GLvoid *bits);
+GLAPI void APIENTRY glCompressedTextureSubImage2DEXT (GLuint texture, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLsizei width, GLsizei height, GLenum format, GLsizei imageSize, const GLvoid *bits);
+GLAPI void APIENTRY glCompressedTextureSubImage1DEXT (GLuint texture, GLenum target, GLint level, GLint xoffset, GLsizei width, GLenum format, GLsizei imageSize, const GLvoid *bits);
+GLAPI void APIENTRY glGetCompressedTextureImageEXT (GLuint texture, GLenum target, GLint lod, GLvoid *img);
+GLAPI void APIENTRY glCompressedMultiTexImage3DEXT (GLenum texunit, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLsizei depth, GLint border, GLsizei imageSize, const GLvoid *bits);
+GLAPI void APIENTRY glCompressedMultiTexImage2DEXT (GLenum texunit, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLint border, GLsizei imageSize, const GLvoid *bits);
+GLAPI void APIENTRY glCompressedMultiTexImage1DEXT (GLenum texunit, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLint border, GLsizei imageSize, const GLvoid *bits);
+GLAPI void APIENTRY glCompressedMultiTexSubImage3DEXT (GLenum texunit, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLsizei width, GLsizei height, GLsizei depth, GLenum format, GLsizei imageSize, const GLvoid *bits);
+GLAPI void APIENTRY glCompressedMultiTexSubImage2DEXT (GLenum texunit, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLsizei width, GLsizei height, GLenum format, GLsizei imageSize, const GLvoid *bits);
+GLAPI void APIENTRY glCompressedMultiTexSubImage1DEXT (GLenum texunit, GLenum target, GLint level, GLint xoffset, GLsizei width, GLenum format, GLsizei imageSize, const GLvoid *bits);
+GLAPI void APIENTRY glGetCompressedMultiTexImageEXT (GLenum texunit, GLenum target, GLint lod, GLvoid *img);
+GLAPI void APIENTRY glNamedProgramStringEXT (GLuint program, GLenum target, GLenum format, GLsizei len, const GLvoid *string);
+GLAPI void APIENTRY glNamedProgramLocalParameter4dEXT (GLuint program, GLenum target, GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+GLAPI void APIENTRY glNamedProgramLocalParameter4dvEXT (GLuint program, GLenum target, GLuint index, const GLdouble *params);
+GLAPI void APIENTRY glNamedProgramLocalParameter4fEXT (GLuint program, GLenum target, GLuint index, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+GLAPI void APIENTRY glNamedProgramLocalParameter4fvEXT (GLuint program, GLenum target, GLuint index, const GLfloat *params);
+GLAPI void APIENTRY glGetNamedProgramLocalParameterdvEXT (GLuint program, GLenum target, GLuint index, GLdouble *params);
+GLAPI void APIENTRY glGetNamedProgramLocalParameterfvEXT (GLuint program, GLenum target, GLuint index, GLfloat *params);
+GLAPI void APIENTRY glGetNamedProgramivEXT (GLuint program, GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetNamedProgramStringEXT (GLuint program, GLenum target, GLenum pname, GLvoid *string);
+GLAPI void APIENTRY glNamedProgramLocalParameters4fvEXT (GLuint program, GLenum target, GLuint index, GLsizei count, const GLfloat *params);
+GLAPI void APIENTRY glNamedProgramLocalParameterI4iEXT (GLuint program, GLenum target, GLuint index, GLint x, GLint y, GLint z, GLint w);
+GLAPI void APIENTRY glNamedProgramLocalParameterI4ivEXT (GLuint program, GLenum target, GLuint index, const GLint *params);
+GLAPI void APIENTRY glNamedProgramLocalParametersI4ivEXT (GLuint program, GLenum target, GLuint index, GLsizei count, const GLint *params);
+GLAPI void APIENTRY glNamedProgramLocalParameterI4uiEXT (GLuint program, GLenum target, GLuint index, GLuint x, GLuint y, GLuint z, GLuint w);
+GLAPI void APIENTRY glNamedProgramLocalParameterI4uivEXT (GLuint program, GLenum target, GLuint index, const GLuint *params);
+GLAPI void APIENTRY glNamedProgramLocalParametersI4uivEXT (GLuint program, GLenum target, GLuint index, GLsizei count, const GLuint *params);
+GLAPI void APIENTRY glGetNamedProgramLocalParameterIivEXT (GLuint program, GLenum target, GLuint index, GLint *params);
+GLAPI void APIENTRY glGetNamedProgramLocalParameterIuivEXT (GLuint program, GLenum target, GLuint index, GLuint *params);
+GLAPI void APIENTRY glTextureParameterIivEXT (GLuint texture, GLenum target, GLenum pname, const GLint *params);
+GLAPI void APIENTRY glTextureParameterIuivEXT (GLuint texture, GLenum target, GLenum pname, const GLuint *params);
+GLAPI void APIENTRY glGetTextureParameterIivEXT (GLuint texture, GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetTextureParameterIuivEXT (GLuint texture, GLenum target, GLenum pname, GLuint *params);
+GLAPI void APIENTRY glMultiTexParameterIivEXT (GLenum texunit, GLenum target, GLenum pname, const GLint *params);
+GLAPI void APIENTRY glMultiTexParameterIuivEXT (GLenum texunit, GLenum target, GLenum pname, const GLuint *params);
+GLAPI void APIENTRY glGetMultiTexParameterIivEXT (GLenum texunit, GLenum target, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetMultiTexParameterIuivEXT (GLenum texunit, GLenum target, GLenum pname, GLuint *params);
+GLAPI void APIENTRY glProgramUniform1fEXT (GLuint program, GLint location, GLfloat v0);
+GLAPI void APIENTRY glProgramUniform2fEXT (GLuint program, GLint location, GLfloat v0, GLfloat v1);
+GLAPI void APIENTRY glProgramUniform3fEXT (GLuint program, GLint location, GLfloat v0, GLfloat v1, GLfloat v2);
+GLAPI void APIENTRY glProgramUniform4fEXT (GLuint program, GLint location, GLfloat v0, GLfloat v1, GLfloat v2, GLfloat v3);
+GLAPI void APIENTRY glProgramUniform1iEXT (GLuint program, GLint location, GLint v0);
+GLAPI void APIENTRY glProgramUniform2iEXT (GLuint program, GLint location, GLint v0, GLint v1);
+GLAPI void APIENTRY glProgramUniform3iEXT (GLuint program, GLint location, GLint v0, GLint v1, GLint v2);
+GLAPI void APIENTRY glProgramUniform4iEXT (GLuint program, GLint location, GLint v0, GLint v1, GLint v2, GLint v3);
+GLAPI void APIENTRY glProgramUniform1fvEXT (GLuint program, GLint location, GLsizei count, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniform2fvEXT (GLuint program, GLint location, GLsizei count, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniform3fvEXT (GLuint program, GLint location, GLsizei count, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniform4fvEXT (GLuint program, GLint location, GLsizei count, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniform1ivEXT (GLuint program, GLint location, GLsizei count, const GLint *value);
+GLAPI void APIENTRY glProgramUniform2ivEXT (GLuint program, GLint location, GLsizei count, const GLint *value);
+GLAPI void APIENTRY glProgramUniform3ivEXT (GLuint program, GLint location, GLsizei count, const GLint *value);
+GLAPI void APIENTRY glProgramUniform4ivEXT (GLuint program, GLint location, GLsizei count, const GLint *value);
+GLAPI void APIENTRY glProgramUniformMatrix2fvEXT (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniformMatrix3fvEXT (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniformMatrix4fvEXT (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniformMatrix2x3fvEXT (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniformMatrix3x2fvEXT (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniformMatrix2x4fvEXT (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniformMatrix4x2fvEXT (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniformMatrix3x4fvEXT (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniformMatrix4x3fvEXT (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+GLAPI void APIENTRY glProgramUniform1uiEXT (GLuint program, GLint location, GLuint v0);
+GLAPI void APIENTRY glProgramUniform2uiEXT (GLuint program, GLint location, GLuint v0, GLuint v1);
+GLAPI void APIENTRY glProgramUniform3uiEXT (GLuint program, GLint location, GLuint v0, GLuint v1, GLuint v2);
+GLAPI void APIENTRY glProgramUniform4uiEXT (GLuint program, GLint location, GLuint v0, GLuint v1, GLuint v2, GLuint v3);
+GLAPI void APIENTRY glProgramUniform1uivEXT (GLuint program, GLint location, GLsizei count, const GLuint *value);
+GLAPI void APIENTRY glProgramUniform2uivEXT (GLuint program, GLint location, GLsizei count, const GLuint *value);
+GLAPI void APIENTRY glProgramUniform3uivEXT (GLuint program, GLint location, GLsizei count, const GLuint *value);
+GLAPI void APIENTRY glProgramUniform4uivEXT (GLuint program, GLint location, GLsizei count, const GLuint *value);
+GLAPI void APIENTRY glNamedBufferDataEXT (GLuint buffer, GLsizeiptr size, const GLvoid *data, GLenum usage);
+GLAPI void APIENTRY glNamedBufferSubDataEXT (GLuint buffer, GLintptr offset, GLsizeiptr size, const GLvoid *data);
+GLAPI GLvoid* APIENTRY glMapNamedBufferEXT (GLuint buffer, GLenum access);
+GLAPI GLboolean APIENTRY glUnmapNamedBufferEXT (GLuint buffer);
+GLAPI GLvoid* APIENTRY glMapNamedBufferRangeEXT (GLuint buffer, GLintptr offset, GLsizeiptr length, GLbitfield access);
+GLAPI void APIENTRY glFlushMappedNamedBufferRangeEXT (GLuint buffer, GLintptr offset, GLsizeiptr length);
+GLAPI void APIENTRY glNamedCopyBufferSubDataEXT (GLuint readBuffer, GLuint writeBuffer, GLintptr readOffset, GLintptr writeOffset, GLsizeiptr size);
+GLAPI void APIENTRY glGetNamedBufferParameterivEXT (GLuint buffer, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetNamedBufferPointervEXT (GLuint buffer, GLenum pname, GLvoid* *params);
+GLAPI void APIENTRY glGetNamedBufferSubDataEXT (GLuint buffer, GLintptr offset, GLsizeiptr size, GLvoid *data);
+GLAPI void APIENTRY glTextureBufferEXT (GLuint texture, GLenum target, GLenum internalformat, GLuint buffer);
+GLAPI void APIENTRY glMultiTexBufferEXT (GLenum texunit, GLenum target, GLenum internalformat, GLuint buffer);
+GLAPI void APIENTRY glNamedRenderbufferStorageEXT (GLuint renderbuffer, GLenum internalformat, GLsizei width, GLsizei height);
+GLAPI void APIENTRY glGetNamedRenderbufferParameterivEXT (GLuint renderbuffer, GLenum pname, GLint *params);
+GLAPI GLenum APIENTRY glCheckNamedFramebufferStatusEXT (GLuint framebuffer, GLenum target);
+GLAPI void APIENTRY glNamedFramebufferTexture1DEXT (GLuint framebuffer, GLenum attachment, GLenum textarget, GLuint texture, GLint level);
+GLAPI void APIENTRY glNamedFramebufferTexture2DEXT (GLuint framebuffer, GLenum attachment, GLenum textarget, GLuint texture, GLint level);
+GLAPI void APIENTRY glNamedFramebufferTexture3DEXT (GLuint framebuffer, GLenum attachment, GLenum textarget, GLuint texture, GLint level, GLint zoffset);
+GLAPI void APIENTRY glNamedFramebufferRenderbufferEXT (GLuint framebuffer, GLenum attachment, GLenum renderbuffertarget, GLuint renderbuffer);
+GLAPI void APIENTRY glGetNamedFramebufferAttachmentParameterivEXT (GLuint framebuffer, GLenum attachment, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGenerateTextureMipmapEXT (GLuint texture, GLenum target);
+GLAPI void APIENTRY glGenerateMultiTexMipmapEXT (GLenum texunit, GLenum target);
+GLAPI void APIENTRY glFramebufferDrawBufferEXT (GLuint framebuffer, GLenum mode);
+GLAPI void APIENTRY glFramebufferDrawBuffersEXT (GLuint framebuffer, GLsizei n, const GLenum *bufs);
+GLAPI void APIENTRY glFramebufferReadBufferEXT (GLuint framebuffer, GLenum mode);
+GLAPI void APIENTRY glGetFramebufferParameterivEXT (GLuint framebuffer, GLenum pname, GLint *params);
+GLAPI void APIENTRY glNamedRenderbufferStorageMultisampleEXT (GLuint renderbuffer, GLsizei samples, GLenum internalformat, GLsizei width, GLsizei height);
+GLAPI void APIENTRY glNamedRenderbufferStorageMultisampleCoverageEXT (GLuint renderbuffer, GLsizei coverageSamples, GLsizei colorSamples, GLenum internalformat, GLsizei width, GLsizei height);
+GLAPI void APIENTRY glNamedFramebufferTextureEXT (GLuint framebuffer, GLenum attachment, GLuint texture, GLint level);
+GLAPI void APIENTRY glNamedFramebufferTextureLayerEXT (GLuint framebuffer, GLenum attachment, GLuint texture, GLint level, GLint layer);
+GLAPI void APIENTRY glNamedFramebufferTextureFaceEXT (GLuint framebuffer, GLenum attachment, GLuint texture, GLint level, GLenum face);
+GLAPI void APIENTRY glTextureRenderbufferEXT (GLuint texture, GLenum target, GLuint renderbuffer);
+GLAPI void APIENTRY glMultiTexRenderbufferEXT (GLenum texunit, GLenum target, GLuint renderbuffer);
+GLAPI void APIENTRY glProgramUniform1dEXT (GLuint program, GLint location, GLdouble x);
+GLAPI void APIENTRY glProgramUniform2dEXT (GLuint program, GLint location, GLdouble x, GLdouble y);
+GLAPI void APIENTRY glProgramUniform3dEXT (GLuint program, GLint location, GLdouble x, GLdouble y, GLdouble z);
+GLAPI void APIENTRY glProgramUniform4dEXT (GLuint program, GLint location, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+GLAPI void APIENTRY glProgramUniform1dvEXT (GLuint program, GLint location, GLsizei count, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniform2dvEXT (GLuint program, GLint location, GLsizei count, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniform3dvEXT (GLuint program, GLint location, GLsizei count, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniform4dvEXT (GLuint program, GLint location, GLsizei count, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniformMatrix2dvEXT (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniformMatrix3dvEXT (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniformMatrix4dvEXT (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniformMatrix2x3dvEXT (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniformMatrix2x4dvEXT (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniformMatrix3x2dvEXT (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniformMatrix3x4dvEXT (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniformMatrix4x2dvEXT (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+GLAPI void APIENTRY glProgramUniformMatrix4x3dvEXT (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCLIENTATTRIBDEFAULTEXTPROC) (GLbitfield mask);
+typedef void (APIENTRYP PFNGLPUSHCLIENTATTRIBDEFAULTEXTPROC) (GLbitfield mask);
+typedef void (APIENTRYP PFNGLMATRIXLOADFEXTPROC) (GLenum mode, const GLfloat *m);
+typedef void (APIENTRYP PFNGLMATRIXLOADDEXTPROC) (GLenum mode, const GLdouble *m);
+typedef void (APIENTRYP PFNGLMATRIXMULTFEXTPROC) (GLenum mode, const GLfloat *m);
+typedef void (APIENTRYP PFNGLMATRIXMULTDEXTPROC) (GLenum mode, const GLdouble *m);
+typedef void (APIENTRYP PFNGLMATRIXLOADIDENTITYEXTPROC) (GLenum mode);
+typedef void (APIENTRYP PFNGLMATRIXROTATEFEXTPROC) (GLenum mode, GLfloat angle, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLMATRIXROTATEDEXTPROC) (GLenum mode, GLdouble angle, GLdouble x, GLdouble y, GLdouble z);
+typedef void (APIENTRYP PFNGLMATRIXSCALEFEXTPROC) (GLenum mode, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLMATRIXSCALEDEXTPROC) (GLenum mode, GLdouble x, GLdouble y, GLdouble z);
+typedef void (APIENTRYP PFNGLMATRIXTRANSLATEFEXTPROC) (GLenum mode, GLfloat x, GLfloat y, GLfloat z);
+typedef void (APIENTRYP PFNGLMATRIXTRANSLATEDEXTPROC) (GLenum mode, GLdouble x, GLdouble y, GLdouble z);
+typedef void (APIENTRYP PFNGLMATRIXFRUSTUMEXTPROC) (GLenum mode, GLdouble left, GLdouble right, GLdouble bottom, GLdouble top, GLdouble zNear, GLdouble zFar);
+typedef void (APIENTRYP PFNGLMATRIXORTHOEXTPROC) (GLenum mode, GLdouble left, GLdouble right, GLdouble bottom, GLdouble top, GLdouble zNear, GLdouble zFar);
+typedef void (APIENTRYP PFNGLMATRIXPOPEXTPROC) (GLenum mode);
+typedef void (APIENTRYP PFNGLMATRIXPUSHEXTPROC) (GLenum mode);
+typedef void (APIENTRYP PFNGLMATRIXLOADTRANSPOSEFEXTPROC) (GLenum mode, const GLfloat *m);
+typedef void (APIENTRYP PFNGLMATRIXLOADTRANSPOSEDEXTPROC) (GLenum mode, const GLdouble *m);
+typedef void (APIENTRYP PFNGLMATRIXMULTTRANSPOSEFEXTPROC) (GLenum mode, const GLfloat *m);
+typedef void (APIENTRYP PFNGLMATRIXMULTTRANSPOSEDEXTPROC) (GLenum mode, const GLdouble *m);
+typedef void (APIENTRYP PFNGLTEXTUREPARAMETERFEXTPROC) (GLuint texture, GLenum target, GLenum pname, GLfloat param);
+typedef void (APIENTRYP PFNGLTEXTUREPARAMETERFVEXTPROC) (GLuint texture, GLenum target, GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLTEXTUREPARAMETERIEXTPROC) (GLuint texture, GLenum target, GLenum pname, GLint param);
+typedef void (APIENTRYP PFNGLTEXTUREPARAMETERIVEXTPROC) (GLuint texture, GLenum target, GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLTEXTUREIMAGE1DEXTPROC) (GLuint texture, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLint border, GLenum format, GLenum type, const GLvoid *pixels);
+typedef void (APIENTRYP PFNGLTEXTUREIMAGE2DEXTPROC) (GLuint texture, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLint border, GLenum format, GLenum type, const GLvoid *pixels);
+typedef void (APIENTRYP PFNGLTEXTURESUBIMAGE1DEXTPROC) (GLuint texture, GLenum target, GLint level, GLint xoffset, GLsizei width, GLenum format, GLenum type, const GLvoid *pixels);
+typedef void (APIENTRYP PFNGLTEXTURESUBIMAGE2DEXTPROC) (GLuint texture, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLsizei width, GLsizei height, GLenum format, GLenum type, const GLvoid *pixels);
+typedef void (APIENTRYP PFNGLCOPYTEXTUREIMAGE1DEXTPROC) (GLuint texture, GLenum target, GLint level, GLenum internalformat, GLint x, GLint y, GLsizei width, GLint border);
+typedef void (APIENTRYP PFNGLCOPYTEXTUREIMAGE2DEXTPROC) (GLuint texture, GLenum target, GLint level, GLenum internalformat, GLint x, GLint y, GLsizei width, GLsizei height, GLint border);
+typedef void (APIENTRYP PFNGLCOPYTEXTURESUBIMAGE1DEXTPROC) (GLuint texture, GLenum target, GLint level, GLint xoffset, GLint x, GLint y, GLsizei width);
+typedef void (APIENTRYP PFNGLCOPYTEXTURESUBIMAGE2DEXTPROC) (GLuint texture, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint x, GLint y, GLsizei width, GLsizei height);
+typedef void (APIENTRYP PFNGLGETTEXTUREIMAGEEXTPROC) (GLuint texture, GLenum target, GLint level, GLenum format, GLenum type, GLvoid *pixels);
+typedef void (APIENTRYP PFNGLGETTEXTUREPARAMETERFVEXTPROC) (GLuint texture, GLenum target, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETTEXTUREPARAMETERIVEXTPROC) (GLuint texture, GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETTEXTURELEVELPARAMETERFVEXTPROC) (GLuint texture, GLenum target, GLint level, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETTEXTURELEVELPARAMETERIVEXTPROC) (GLuint texture, GLenum target, GLint level, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLTEXTUREIMAGE3DEXTPROC) (GLuint texture, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLsizei depth, GLint border, GLenum format, GLenum type, const GLvoid *pixels);
+typedef void (APIENTRYP PFNGLTEXTURESUBIMAGE3DEXTPROC) (GLuint texture, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLsizei width, GLsizei height, GLsizei depth, GLenum format, GLenum type, const GLvoid *pixels);
+typedef void (APIENTRYP PFNGLCOPYTEXTURESUBIMAGE3DEXTPROC) (GLuint texture, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLint x, GLint y, GLsizei width, GLsizei height);
+typedef void (APIENTRYP PFNGLMULTITEXPARAMETERFEXTPROC) (GLenum texunit, GLenum target, GLenum pname, GLfloat param);
+typedef void (APIENTRYP PFNGLMULTITEXPARAMETERFVEXTPROC) (GLenum texunit, GLenum target, GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLMULTITEXPARAMETERIEXTPROC) (GLenum texunit, GLenum target, GLenum pname, GLint param);
+typedef void (APIENTRYP PFNGLMULTITEXPARAMETERIVEXTPROC) (GLenum texunit, GLenum target, GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLMULTITEXIMAGE1DEXTPROC) (GLenum texunit, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLint border, GLenum format, GLenum type, const GLvoid *pixels);
+typedef void (APIENTRYP PFNGLMULTITEXIMAGE2DEXTPROC) (GLenum texunit, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLint border, GLenum format, GLenum type, const GLvoid *pixels);
+typedef void (APIENTRYP PFNGLMULTITEXSUBIMAGE1DEXTPROC) (GLenum texunit, GLenum target, GLint level, GLint xoffset, GLsizei width, GLenum format, GLenum type, const GLvoid *pixels);
+typedef void (APIENTRYP PFNGLMULTITEXSUBIMAGE2DEXTPROC) (GLenum texunit, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLsizei width, GLsizei height, GLenum format, GLenum type, const GLvoid *pixels);
+typedef void (APIENTRYP PFNGLCOPYMULTITEXIMAGE1DEXTPROC) (GLenum texunit, GLenum target, GLint level, GLenum internalformat, GLint x, GLint y, GLsizei width, GLint border);
+typedef void (APIENTRYP PFNGLCOPYMULTITEXIMAGE2DEXTPROC) (GLenum texunit, GLenum target, GLint level, GLenum internalformat, GLint x, GLint y, GLsizei width, GLsizei height, GLint border);
+typedef void (APIENTRYP PFNGLCOPYMULTITEXSUBIMAGE1DEXTPROC) (GLenum texunit, GLenum target, GLint level, GLint xoffset, GLint x, GLint y, GLsizei width);
+typedef void (APIENTRYP PFNGLCOPYMULTITEXSUBIMAGE2DEXTPROC) (GLenum texunit, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint x, GLint y, GLsizei width, GLsizei height);
+typedef void (APIENTRYP PFNGLGETMULTITEXIMAGEEXTPROC) (GLenum texunit, GLenum target, GLint level, GLenum format, GLenum type, GLvoid *pixels);
+typedef void (APIENTRYP PFNGLGETMULTITEXPARAMETERFVEXTPROC) (GLenum texunit, GLenum target, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETMULTITEXPARAMETERIVEXTPROC) (GLenum texunit, GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETMULTITEXLEVELPARAMETERFVEXTPROC) (GLenum texunit, GLenum target, GLint level, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETMULTITEXLEVELPARAMETERIVEXTPROC) (GLenum texunit, GLenum target, GLint level, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLMULTITEXIMAGE3DEXTPROC) (GLenum texunit, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLsizei depth, GLint border, GLenum format, GLenum type, const GLvoid *pixels);
+typedef void (APIENTRYP PFNGLMULTITEXSUBIMAGE3DEXTPROC) (GLenum texunit, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLsizei width, GLsizei height, GLsizei depth, GLenum format, GLenum type, const GLvoid *pixels);
+typedef void (APIENTRYP PFNGLCOPYMULTITEXSUBIMAGE3DEXTPROC) (GLenum texunit, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLint x, GLint y, GLsizei width, GLsizei height);
+typedef void (APIENTRYP PFNGLBINDMULTITEXTUREEXTPROC) (GLenum texunit, GLenum target, GLuint texture);
+typedef void (APIENTRYP PFNGLENABLECLIENTSTATEINDEXEDEXTPROC) (GLenum array, GLuint index);
+typedef void (APIENTRYP PFNGLDISABLECLIENTSTATEINDEXEDEXTPROC) (GLenum array, GLuint index);
+typedef void (APIENTRYP PFNGLMULTITEXCOORDPOINTEREXTPROC) (GLenum texunit, GLint size, GLenum type, GLsizei stride, const GLvoid *pointer);
+typedef void (APIENTRYP PFNGLMULTITEXENVFEXTPROC) (GLenum texunit, GLenum target, GLenum pname, GLfloat param);
+typedef void (APIENTRYP PFNGLMULTITEXENVFVEXTPROC) (GLenum texunit, GLenum target, GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLMULTITEXENVIEXTPROC) (GLenum texunit, GLenum target, GLenum pname, GLint param);
+typedef void (APIENTRYP PFNGLMULTITEXENVIVEXTPROC) (GLenum texunit, GLenum target, GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLMULTITEXGENDEXTPROC) (GLenum texunit, GLenum coord, GLenum pname, GLdouble param);
+typedef void (APIENTRYP PFNGLMULTITEXGENDVEXTPROC) (GLenum texunit, GLenum coord, GLenum pname, const GLdouble *params);
+typedef void (APIENTRYP PFNGLMULTITEXGENFEXTPROC) (GLenum texunit, GLenum coord, GLenum pname, GLfloat param);
+typedef void (APIENTRYP PFNGLMULTITEXGENFVEXTPROC) (GLenum texunit, GLenum coord, GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLMULTITEXGENIEXTPROC) (GLenum texunit, GLenum coord, GLenum pname, GLint param);
+typedef void (APIENTRYP PFNGLMULTITEXGENIVEXTPROC) (GLenum texunit, GLenum coord, GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLGETMULTITEXENVFVEXTPROC) (GLenum texunit, GLenum target, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETMULTITEXENVIVEXTPROC) (GLenum texunit, GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETMULTITEXGENDVEXTPROC) (GLenum texunit, GLenum coord, GLenum pname, GLdouble *params);
+typedef void (APIENTRYP PFNGLGETMULTITEXGENFVEXTPROC) (GLenum texunit, GLenum coord, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETMULTITEXGENIVEXTPROC) (GLenum texunit, GLenum coord, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETFLOATINDEXEDVEXTPROC) (GLenum target, GLuint index, GLfloat *data);
+typedef void (APIENTRYP PFNGLGETDOUBLEINDEXEDVEXTPROC) (GLenum target, GLuint index, GLdouble *data);
+typedef void (APIENTRYP PFNGLGETPOINTERINDEXEDVEXTPROC) (GLenum target, GLuint index, GLvoid* *data);
+typedef void (APIENTRYP PFNGLCOMPRESSEDTEXTUREIMAGE3DEXTPROC) (GLuint texture, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLsizei depth, GLint border, GLsizei imageSize, const GLvoid *bits);
+typedef void (APIENTRYP PFNGLCOMPRESSEDTEXTUREIMAGE2DEXTPROC) (GLuint texture, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLint border, GLsizei imageSize, const GLvoid *bits);
+typedef void (APIENTRYP PFNGLCOMPRESSEDTEXTUREIMAGE1DEXTPROC) (GLuint texture, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLint border, GLsizei imageSize, const GLvoid *bits);
+typedef void (APIENTRYP PFNGLCOMPRESSEDTEXTURESUBIMAGE3DEXTPROC) (GLuint texture, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLsizei width, GLsizei height, GLsizei depth, GLenum format, GLsizei imageSize, const GLvoid *bits);
+typedef void (APIENTRYP PFNGLCOMPRESSEDTEXTURESUBIMAGE2DEXTPROC) (GLuint texture, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLsizei width, GLsizei height, GLenum format, GLsizei imageSize, const GLvoid *bits);
+typedef void (APIENTRYP PFNGLCOMPRESSEDTEXTURESUBIMAGE1DEXTPROC) (GLuint texture, GLenum target, GLint level, GLint xoffset, GLsizei width, GLenum format, GLsizei imageSize, const GLvoid *bits);
+typedef void (APIENTRYP PFNGLGETCOMPRESSEDTEXTUREIMAGEEXTPROC) (GLuint texture, GLenum target, GLint lod, GLvoid *img);
+typedef void (APIENTRYP PFNGLCOMPRESSEDMULTITEXIMAGE3DEXTPROC) (GLenum texunit, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLsizei depth, GLint border, GLsizei imageSize, const GLvoid *bits);
+typedef void (APIENTRYP PFNGLCOMPRESSEDMULTITEXIMAGE2DEXTPROC) (GLenum texunit, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLsizei height, GLint border, GLsizei imageSize, const GLvoid *bits);
+typedef void (APIENTRYP PFNGLCOMPRESSEDMULTITEXIMAGE1DEXTPROC) (GLenum texunit, GLenum target, GLint level, GLenum internalformat, GLsizei width, GLint border, GLsizei imageSize, const GLvoid *bits);
+typedef void (APIENTRYP PFNGLCOMPRESSEDMULTITEXSUBIMAGE3DEXTPROC) (GLenum texunit, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLint zoffset, GLsizei width, GLsizei height, GLsizei depth, GLenum format, GLsizei imageSize, const GLvoid *bits);
+typedef void (APIENTRYP PFNGLCOMPRESSEDMULTITEXSUBIMAGE2DEXTPROC) (GLenum texunit, GLenum target, GLint level, GLint xoffset, GLint yoffset, GLsizei width, GLsizei height, GLenum format, GLsizei imageSize, const GLvoid *bits);
+typedef void (APIENTRYP PFNGLCOMPRESSEDMULTITEXSUBIMAGE1DEXTPROC) (GLenum texunit, GLenum target, GLint level, GLint xoffset, GLsizei width, GLenum format, GLsizei imageSize, const GLvoid *bits);
+typedef void (APIENTRYP PFNGLGETCOMPRESSEDMULTITEXIMAGEEXTPROC) (GLenum texunit, GLenum target, GLint lod, GLvoid *img);
+typedef void (APIENTRYP PFNGLNAMEDPROGRAMSTRINGEXTPROC) (GLuint program, GLenum target, GLenum format, GLsizei len, const GLvoid *string);
+typedef void (APIENTRYP PFNGLNAMEDPROGRAMLOCALPARAMETER4DEXTPROC) (GLuint program, GLenum target, GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+typedef void (APIENTRYP PFNGLNAMEDPROGRAMLOCALPARAMETER4DVEXTPROC) (GLuint program, GLenum target, GLuint index, const GLdouble *params);
+typedef void (APIENTRYP PFNGLNAMEDPROGRAMLOCALPARAMETER4FEXTPROC) (GLuint program, GLenum target, GLuint index, GLfloat x, GLfloat y, GLfloat z, GLfloat w);
+typedef void (APIENTRYP PFNGLNAMEDPROGRAMLOCALPARAMETER4FVEXTPROC) (GLuint program, GLenum target, GLuint index, const GLfloat *params);
+typedef void (APIENTRYP PFNGLGETNAMEDPROGRAMLOCALPARAMETERDVEXTPROC) (GLuint program, GLenum target, GLuint index, GLdouble *params);
+typedef void (APIENTRYP PFNGLGETNAMEDPROGRAMLOCALPARAMETERFVEXTPROC) (GLuint program, GLenum target, GLuint index, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETNAMEDPROGRAMIVEXTPROC) (GLuint program, GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETNAMEDPROGRAMSTRINGEXTPROC) (GLuint program, GLenum target, GLenum pname, GLvoid *string);
+typedef void (APIENTRYP PFNGLNAMEDPROGRAMLOCALPARAMETERS4FVEXTPROC) (GLuint program, GLenum target, GLuint index, GLsizei count, const GLfloat *params);
+typedef void (APIENTRYP PFNGLNAMEDPROGRAMLOCALPARAMETERI4IEXTPROC) (GLuint program, GLenum target, GLuint index, GLint x, GLint y, GLint z, GLint w);
+typedef void (APIENTRYP PFNGLNAMEDPROGRAMLOCALPARAMETERI4IVEXTPROC) (GLuint program, GLenum target, GLuint index, const GLint *params);
+typedef void (APIENTRYP PFNGLNAMEDPROGRAMLOCALPARAMETERSI4IVEXTPROC) (GLuint program, GLenum target, GLuint index, GLsizei count, const GLint *params);
+typedef void (APIENTRYP PFNGLNAMEDPROGRAMLOCALPARAMETERI4UIEXTPROC) (GLuint program, GLenum target, GLuint index, GLuint x, GLuint y, GLuint z, GLuint w);
+typedef void (APIENTRYP PFNGLNAMEDPROGRAMLOCALPARAMETERI4UIVEXTPROC) (GLuint program, GLenum target, GLuint index, const GLuint *params);
+typedef void (APIENTRYP PFNGLNAMEDPROGRAMLOCALPARAMETERSI4UIVEXTPROC) (GLuint program, GLenum target, GLuint index, GLsizei count, const GLuint *params);
+typedef void (APIENTRYP PFNGLGETNAMEDPROGRAMLOCALPARAMETERIIVEXTPROC) (GLuint program, GLenum target, GLuint index, GLint *params);
+typedef void (APIENTRYP PFNGLGETNAMEDPROGRAMLOCALPARAMETERIUIVEXTPROC) (GLuint program, GLenum target, GLuint index, GLuint *params);
+typedef void (APIENTRYP PFNGLTEXTUREPARAMETERIIVEXTPROC) (GLuint texture, GLenum target, GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLTEXTUREPARAMETERIUIVEXTPROC) (GLuint texture, GLenum target, GLenum pname, const GLuint *params);
+typedef void (APIENTRYP PFNGLGETTEXTUREPARAMETERIIVEXTPROC) (GLuint texture, GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETTEXTUREPARAMETERIUIVEXTPROC) (GLuint texture, GLenum target, GLenum pname, GLuint *params);
+typedef void (APIENTRYP PFNGLMULTITEXPARAMETERIIVEXTPROC) (GLenum texunit, GLenum target, GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLMULTITEXPARAMETERIUIVEXTPROC) (GLenum texunit, GLenum target, GLenum pname, const GLuint *params);
+typedef void (APIENTRYP PFNGLGETMULTITEXPARAMETERIIVEXTPROC) (GLenum texunit, GLenum target, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETMULTITEXPARAMETERIUIVEXTPROC) (GLenum texunit, GLenum target, GLenum pname, GLuint *params);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1FEXTPROC) (GLuint program, GLint location, GLfloat v0);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2FEXTPROC) (GLuint program, GLint location, GLfloat v0, GLfloat v1);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3FEXTPROC) (GLuint program, GLint location, GLfloat v0, GLfloat v1, GLfloat v2);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4FEXTPROC) (GLuint program, GLint location, GLfloat v0, GLfloat v1, GLfloat v2, GLfloat v3);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1IEXTPROC) (GLuint program, GLint location, GLint v0);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2IEXTPROC) (GLuint program, GLint location, GLint v0, GLint v1);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3IEXTPROC) (GLuint program, GLint location, GLint v0, GLint v1, GLint v2);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4IEXTPROC) (GLuint program, GLint location, GLint v0, GLint v1, GLint v2, GLint v3);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1FVEXTPROC) (GLuint program, GLint location, GLsizei count, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2FVEXTPROC) (GLuint program, GLint location, GLsizei count, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3FVEXTPROC) (GLuint program, GLint location, GLsizei count, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4FVEXTPROC) (GLuint program, GLint location, GLsizei count, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1IVEXTPROC) (GLuint program, GLint location, GLsizei count, const GLint *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2IVEXTPROC) (GLuint program, GLint location, GLsizei count, const GLint *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3IVEXTPROC) (GLuint program, GLint location, GLsizei count, const GLint *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4IVEXTPROC) (GLuint program, GLint location, GLsizei count, const GLint *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX2FVEXTPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX3FVEXTPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX4FVEXTPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX2X3FVEXTPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX3X2FVEXTPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX2X4FVEXTPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX4X2FVEXTPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX3X4FVEXTPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX4X3FVEXTPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLfloat *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1UIEXTPROC) (GLuint program, GLint location, GLuint v0);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2UIEXTPROC) (GLuint program, GLint location, GLuint v0, GLuint v1);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3UIEXTPROC) (GLuint program, GLint location, GLuint v0, GLuint v1, GLuint v2);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4UIEXTPROC) (GLuint program, GLint location, GLuint v0, GLuint v1, GLuint v2, GLuint v3);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1UIVEXTPROC) (GLuint program, GLint location, GLsizei count, const GLuint *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2UIVEXTPROC) (GLuint program, GLint location, GLsizei count, const GLuint *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3UIVEXTPROC) (GLuint program, GLint location, GLsizei count, const GLuint *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4UIVEXTPROC) (GLuint program, GLint location, GLsizei count, const GLuint *value);
+typedef void (APIENTRYP PFNGLNAMEDBUFFERDATAEXTPROC) (GLuint buffer, GLsizeiptr size, const GLvoid *data, GLenum usage);
+typedef void (APIENTRYP PFNGLNAMEDBUFFERSUBDATAEXTPROC) (GLuint buffer, GLintptr offset, GLsizeiptr size, const GLvoid *data);
+typedef GLvoid* (APIENTRYP PFNGLMAPNAMEDBUFFEREXTPROC) (GLuint buffer, GLenum access);
+typedef GLboolean (APIENTRYP PFNGLUNMAPNAMEDBUFFEREXTPROC) (GLuint buffer);
+typedef GLvoid* (APIENTRYP PFNGLMAPNAMEDBUFFERRANGEEXTPROC) (GLuint buffer, GLintptr offset, GLsizeiptr length, GLbitfield access);
+typedef void (APIENTRYP PFNGLFLUSHMAPPEDNAMEDBUFFERRANGEEXTPROC) (GLuint buffer, GLintptr offset, GLsizeiptr length);
+typedef void (APIENTRYP PFNGLNAMEDCOPYBUFFERSUBDATAEXTPROC) (GLuint readBuffer, GLuint writeBuffer, GLintptr readOffset, GLintptr writeOffset, GLsizeiptr size);
+typedef void (APIENTRYP PFNGLGETNAMEDBUFFERPARAMETERIVEXTPROC) (GLuint buffer, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETNAMEDBUFFERPOINTERVEXTPROC) (GLuint buffer, GLenum pname, GLvoid* *params);
+typedef void (APIENTRYP PFNGLGETNAMEDBUFFERSUBDATAEXTPROC) (GLuint buffer, GLintptr offset, GLsizeiptr size, GLvoid *data);
+typedef void (APIENTRYP PFNGLTEXTUREBUFFEREXTPROC) (GLuint texture, GLenum target, GLenum internalformat, GLuint buffer);
+typedef void (APIENTRYP PFNGLMULTITEXBUFFEREXTPROC) (GLenum texunit, GLenum target, GLenum internalformat, GLuint buffer);
+typedef void (APIENTRYP PFNGLNAMEDRENDERBUFFERSTORAGEEXTPROC) (GLuint renderbuffer, GLenum internalformat, GLsizei width, GLsizei height);
+typedef void (APIENTRYP PFNGLGETNAMEDRENDERBUFFERPARAMETERIVEXTPROC) (GLuint renderbuffer, GLenum pname, GLint *params);
+typedef GLenum (APIENTRYP PFNGLCHECKNAMEDFRAMEBUFFERSTATUSEXTPROC) (GLuint framebuffer, GLenum target);
+typedef void (APIENTRYP PFNGLNAMEDFRAMEBUFFERTEXTURE1DEXTPROC) (GLuint framebuffer, GLenum attachment, GLenum textarget, GLuint texture, GLint level);
+typedef void (APIENTRYP PFNGLNAMEDFRAMEBUFFERTEXTURE2DEXTPROC) (GLuint framebuffer, GLenum attachment, GLenum textarget, GLuint texture, GLint level);
+typedef void (APIENTRYP PFNGLNAMEDFRAMEBUFFERTEXTURE3DEXTPROC) (GLuint framebuffer, GLenum attachment, GLenum textarget, GLuint texture, GLint level, GLint zoffset);
+typedef void (APIENTRYP PFNGLNAMEDFRAMEBUFFERRENDERBUFFEREXTPROC) (GLuint framebuffer, GLenum attachment, GLenum renderbuffertarget, GLuint renderbuffer);
+typedef void (APIENTRYP PFNGLGETNAMEDFRAMEBUFFERATTACHMENTPARAMETERIVEXTPROC) (GLuint framebuffer, GLenum attachment, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGENERATETEXTUREMIPMAPEXTPROC) (GLuint texture, GLenum target);
+typedef void (APIENTRYP PFNGLGENERATEMULTITEXMIPMAPEXTPROC) (GLenum texunit, GLenum target);
+typedef void (APIENTRYP PFNGLFRAMEBUFFERDRAWBUFFEREXTPROC) (GLuint framebuffer, GLenum mode);
+typedef void (APIENTRYP PFNGLFRAMEBUFFERDRAWBUFFERSEXTPROC) (GLuint framebuffer, GLsizei n, const GLenum *bufs);
+typedef void (APIENTRYP PFNGLFRAMEBUFFERREADBUFFEREXTPROC) (GLuint framebuffer, GLenum mode);
+typedef void (APIENTRYP PFNGLGETFRAMEBUFFERPARAMETERIVEXTPROC) (GLuint framebuffer, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLNAMEDRENDERBUFFERSTORAGEMULTISAMPLEEXTPROC) (GLuint renderbuffer, GLsizei samples, GLenum internalformat, GLsizei width, GLsizei height);
+typedef void (APIENTRYP PFNGLNAMEDRENDERBUFFERSTORAGEMULTISAMPLECOVERAGEEXTPROC) (GLuint renderbuffer, GLsizei coverageSamples, GLsizei colorSamples, GLenum internalformat, GLsizei width, GLsizei height);
+typedef void (APIENTRYP PFNGLNAMEDFRAMEBUFFERTEXTUREEXTPROC) (GLuint framebuffer, GLenum attachment, GLuint texture, GLint level);
+typedef void (APIENTRYP PFNGLNAMEDFRAMEBUFFERTEXTURELAYEREXTPROC) (GLuint framebuffer, GLenum attachment, GLuint texture, GLint level, GLint layer);
+typedef void (APIENTRYP PFNGLNAMEDFRAMEBUFFERTEXTUREFACEEXTPROC) (GLuint framebuffer, GLenum attachment, GLuint texture, GLint level, GLenum face);
+typedef void (APIENTRYP PFNGLTEXTURERENDERBUFFEREXTPROC) (GLuint texture, GLenum target, GLuint renderbuffer);
+typedef void (APIENTRYP PFNGLMULTITEXRENDERBUFFEREXTPROC) (GLenum texunit, GLenum target, GLuint renderbuffer);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1DEXTPROC) (GLuint program, GLint location, GLdouble x);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2DEXTPROC) (GLuint program, GLint location, GLdouble x, GLdouble y);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3DEXTPROC) (GLuint program, GLint location, GLdouble x, GLdouble y, GLdouble z);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4DEXTPROC) (GLuint program, GLint location, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1DVEXTPROC) (GLuint program, GLint location, GLsizei count, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2DVEXTPROC) (GLuint program, GLint location, GLsizei count, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3DVEXTPROC) (GLuint program, GLint location, GLsizei count, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4DVEXTPROC) (GLuint program, GLint location, GLsizei count, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX2DVEXTPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX3DVEXTPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX4DVEXTPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX2X3DVEXTPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX2X4DVEXTPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX3X2DVEXTPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX3X4DVEXTPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX4X2DVEXTPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMMATRIX4X3DVEXTPROC) (GLuint program, GLint location, GLsizei count, GLboolean transpose, const GLdouble *value);
+#endif
+
+#ifndef GL_EXT_vertex_array_bgra
+#define GL_EXT_vertex_array_bgra 1
+#endif
+
+#ifndef GL_EXT_texture_swizzle
+#define GL_EXT_texture_swizzle 1
+#endif
+
+#ifndef GL_NV_explicit_multisample
+#define GL_NV_explicit_multisample 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glGetMultisamplefvNV (GLenum pname, GLuint index, GLfloat *val);
+GLAPI void APIENTRY glSampleMaskIndexedNV (GLuint index, GLbitfield mask);
+GLAPI void APIENTRY glTexRenderbufferNV (GLenum target, GLuint renderbuffer);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLGETMULTISAMPLEFVNVPROC) (GLenum pname, GLuint index, GLfloat *val);
+typedef void (APIENTRYP PFNGLSAMPLEMASKINDEXEDNVPROC) (GLuint index, GLbitfield mask);
+typedef void (APIENTRYP PFNGLTEXRENDERBUFFERNVPROC) (GLenum target, GLuint renderbuffer);
+#endif
+
+#ifndef GL_NV_transform_feedback2
+#define GL_NV_transform_feedback2 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBindTransformFeedbackNV (GLenum target, GLuint id);
+GLAPI void APIENTRY glDeleteTransformFeedbacksNV (GLsizei n, const GLuint *ids);
+GLAPI void APIENTRY glGenTransformFeedbacksNV (GLsizei n, GLuint *ids);
+GLAPI GLboolean APIENTRY glIsTransformFeedbackNV (GLuint id);
+GLAPI void APIENTRY glPauseTransformFeedbackNV (void);
+GLAPI void APIENTRY glResumeTransformFeedbackNV (void);
+GLAPI void APIENTRY glDrawTransformFeedbackNV (GLenum mode, GLuint id);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBINDTRANSFORMFEEDBACKNVPROC) (GLenum target, GLuint id);
+typedef void (APIENTRYP PFNGLDELETETRANSFORMFEEDBACKSNVPROC) (GLsizei n, const GLuint *ids);
+typedef void (APIENTRYP PFNGLGENTRANSFORMFEEDBACKSNVPROC) (GLsizei n, GLuint *ids);
+typedef GLboolean (APIENTRYP PFNGLISTRANSFORMFEEDBACKNVPROC) (GLuint id);
+typedef void (APIENTRYP PFNGLPAUSETRANSFORMFEEDBACKNVPROC) (void);
+typedef void (APIENTRYP PFNGLRESUMETRANSFORMFEEDBACKNVPROC) (void);
+typedef void (APIENTRYP PFNGLDRAWTRANSFORMFEEDBACKNVPROC) (GLenum mode, GLuint id);
+#endif
+
+#ifndef GL_ATI_meminfo
+#define GL_ATI_meminfo 1
+#endif
+
+#ifndef GL_AMD_performance_monitor
+#define GL_AMD_performance_monitor 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glGetPerfMonitorGroupsAMD (GLint *numGroups, GLsizei groupsSize, GLuint *groups);
+GLAPI void APIENTRY glGetPerfMonitorCountersAMD (GLuint group, GLint *numCounters, GLint *maxActiveCounters, GLsizei counterSize, GLuint *counters);
+GLAPI void APIENTRY glGetPerfMonitorGroupStringAMD (GLuint group, GLsizei bufSize, GLsizei *length, GLchar *groupString);
+GLAPI void APIENTRY glGetPerfMonitorCounterStringAMD (GLuint group, GLuint counter, GLsizei bufSize, GLsizei *length, GLchar *counterString);
+GLAPI void APIENTRY glGetPerfMonitorCounterInfoAMD (GLuint group, GLuint counter, GLenum pname, GLvoid *data);
+GLAPI void APIENTRY glGenPerfMonitorsAMD (GLsizei n, GLuint *monitors);
+GLAPI void APIENTRY glDeletePerfMonitorsAMD (GLsizei n, GLuint *monitors);
+GLAPI void APIENTRY glSelectPerfMonitorCountersAMD (GLuint monitor, GLboolean enable, GLuint group, GLint numCounters, GLuint *counterList);
+GLAPI void APIENTRY glBeginPerfMonitorAMD (GLuint monitor);
+GLAPI void APIENTRY glEndPerfMonitorAMD (GLuint monitor);
+GLAPI void APIENTRY glGetPerfMonitorCounterDataAMD (GLuint monitor, GLenum pname, GLsizei dataSize, GLuint *data, GLint *bytesWritten);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLGETPERFMONITORGROUPSAMDPROC) (GLint *numGroups, GLsizei groupsSize, GLuint *groups);
+typedef void (APIENTRYP PFNGLGETPERFMONITORCOUNTERSAMDPROC) (GLuint group, GLint *numCounters, GLint *maxActiveCounters, GLsizei counterSize, GLuint *counters);
+typedef void (APIENTRYP PFNGLGETPERFMONITORGROUPSTRINGAMDPROC) (GLuint group, GLsizei bufSize, GLsizei *length, GLchar *groupString);
+typedef void (APIENTRYP PFNGLGETPERFMONITORCOUNTERSTRINGAMDPROC) (GLuint group, GLuint counter, GLsizei bufSize, GLsizei *length, GLchar *counterString);
+typedef void (APIENTRYP PFNGLGETPERFMONITORCOUNTERINFOAMDPROC) (GLuint group, GLuint counter, GLenum pname, GLvoid *data);
+typedef void (APIENTRYP PFNGLGENPERFMONITORSAMDPROC) (GLsizei n, GLuint *monitors);
+typedef void (APIENTRYP PFNGLDELETEPERFMONITORSAMDPROC) (GLsizei n, GLuint *monitors);
+typedef void (APIENTRYP PFNGLSELECTPERFMONITORCOUNTERSAMDPROC) (GLuint monitor, GLboolean enable, GLuint group, GLint numCounters, GLuint *counterList);
+typedef void (APIENTRYP PFNGLBEGINPERFMONITORAMDPROC) (GLuint monitor);
+typedef void (APIENTRYP PFNGLENDPERFMONITORAMDPROC) (GLuint monitor);
+typedef void (APIENTRYP PFNGLGETPERFMONITORCOUNTERDATAAMDPROC) (GLuint monitor, GLenum pname, GLsizei dataSize, GLuint *data, GLint *bytesWritten);
+#endif
+
+#ifndef GL_AMD_texture_texture4
+#define GL_AMD_texture_texture4 1
+#endif
+
+#ifndef GL_AMD_vertex_shader_tesselator
+#define GL_AMD_vertex_shader_tesselator 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glTessellationFactorAMD (GLfloat factor);
+GLAPI void APIENTRY glTessellationModeAMD (GLenum mode);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLTESSELLATIONFACTORAMDPROC) (GLfloat factor);
+typedef void (APIENTRYP PFNGLTESSELLATIONMODEAMDPROC) (GLenum mode);
+#endif
+
+#ifndef GL_EXT_provoking_vertex
+#define GL_EXT_provoking_vertex 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glProvokingVertexEXT (GLenum mode);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPROVOKINGVERTEXEXTPROC) (GLenum mode);
+#endif
+
+#ifndef GL_EXT_texture_snorm
+#define GL_EXT_texture_snorm 1
+#endif
+
+#ifndef GL_AMD_draw_buffers_blend
+#define GL_AMD_draw_buffers_blend 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBlendFuncIndexedAMD (GLuint buf, GLenum src, GLenum dst);
+GLAPI void APIENTRY glBlendFuncSeparateIndexedAMD (GLuint buf, GLenum srcRGB, GLenum dstRGB, GLenum srcAlpha, GLenum dstAlpha);
+GLAPI void APIENTRY glBlendEquationIndexedAMD (GLuint buf, GLenum mode);
+GLAPI void APIENTRY glBlendEquationSeparateIndexedAMD (GLuint buf, GLenum modeRGB, GLenum modeAlpha);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBLENDFUNCINDEXEDAMDPROC) (GLuint buf, GLenum src, GLenum dst);
+typedef void (APIENTRYP PFNGLBLENDFUNCSEPARATEINDEXEDAMDPROC) (GLuint buf, GLenum srcRGB, GLenum dstRGB, GLenum srcAlpha, GLenum dstAlpha);
+typedef void (APIENTRYP PFNGLBLENDEQUATIONINDEXEDAMDPROC) (GLuint buf, GLenum mode);
+typedef void (APIENTRYP PFNGLBLENDEQUATIONSEPARATEINDEXEDAMDPROC) (GLuint buf, GLenum modeRGB, GLenum modeAlpha);
+#endif
+
+#ifndef GL_APPLE_texture_range
+#define GL_APPLE_texture_range 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glTextureRangeAPPLE (GLenum target, GLsizei length, const GLvoid *pointer);
+GLAPI void APIENTRY glGetTexParameterPointervAPPLE (GLenum target, GLenum pname, GLvoid* *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLTEXTURERANGEAPPLEPROC) (GLenum target, GLsizei length, const GLvoid *pointer);
+typedef void (APIENTRYP PFNGLGETTEXPARAMETERPOINTERVAPPLEPROC) (GLenum target, GLenum pname, GLvoid* *params);
+#endif
+
+#ifndef GL_APPLE_float_pixels
+#define GL_APPLE_float_pixels 1
+#endif
+
+#ifndef GL_APPLE_vertex_program_evaluators
+#define GL_APPLE_vertex_program_evaluators 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glEnableVertexAttribAPPLE (GLuint index, GLenum pname);
+GLAPI void APIENTRY glDisableVertexAttribAPPLE (GLuint index, GLenum pname);
+GLAPI GLboolean APIENTRY glIsVertexAttribEnabledAPPLE (GLuint index, GLenum pname);
+GLAPI void APIENTRY glMapVertexAttrib1dAPPLE (GLuint index, GLuint size, GLdouble u1, GLdouble u2, GLint stride, GLint order, const GLdouble *points);
+GLAPI void APIENTRY glMapVertexAttrib1fAPPLE (GLuint index, GLuint size, GLfloat u1, GLfloat u2, GLint stride, GLint order, const GLfloat *points);
+GLAPI void APIENTRY glMapVertexAttrib2dAPPLE (GLuint index, GLuint size, GLdouble u1, GLdouble u2, GLint ustride, GLint uorder, GLdouble v1, GLdouble v2, GLint vstride, GLint vorder, const GLdouble *points);
+GLAPI void APIENTRY glMapVertexAttrib2fAPPLE (GLuint index, GLuint size, GLfloat u1, GLfloat u2, GLint ustride, GLint uorder, GLfloat v1, GLfloat v2, GLint vstride, GLint vorder, const GLfloat *points);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLENABLEVERTEXATTRIBAPPLEPROC) (GLuint index, GLenum pname);
+typedef void (APIENTRYP PFNGLDISABLEVERTEXATTRIBAPPLEPROC) (GLuint index, GLenum pname);
+typedef GLboolean (APIENTRYP PFNGLISVERTEXATTRIBENABLEDAPPLEPROC) (GLuint index, GLenum pname);
+typedef void (APIENTRYP PFNGLMAPVERTEXATTRIB1DAPPLEPROC) (GLuint index, GLuint size, GLdouble u1, GLdouble u2, GLint stride, GLint order, const GLdouble *points);
+typedef void (APIENTRYP PFNGLMAPVERTEXATTRIB1FAPPLEPROC) (GLuint index, GLuint size, GLfloat u1, GLfloat u2, GLint stride, GLint order, const GLfloat *points);
+typedef void (APIENTRYP PFNGLMAPVERTEXATTRIB2DAPPLEPROC) (GLuint index, GLuint size, GLdouble u1, GLdouble u2, GLint ustride, GLint uorder, GLdouble v1, GLdouble v2, GLint vstride, GLint vorder, const GLdouble *points);
+typedef void (APIENTRYP PFNGLMAPVERTEXATTRIB2FAPPLEPROC) (GLuint index, GLuint size, GLfloat u1, GLfloat u2, GLint ustride, GLint uorder, GLfloat v1, GLfloat v2, GLint vstride, GLint vorder, const GLfloat *points);
+#endif
+
+#ifndef GL_APPLE_aux_depth_stencil
+#define GL_APPLE_aux_depth_stencil 1
+#endif
+
+#ifndef GL_APPLE_object_purgeable
+#define GL_APPLE_object_purgeable 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI GLenum APIENTRY glObjectPurgeableAPPLE (GLenum objectType, GLuint name, GLenum option);
+GLAPI GLenum APIENTRY glObjectUnpurgeableAPPLE (GLenum objectType, GLuint name, GLenum option);
+GLAPI void APIENTRY glGetObjectParameterivAPPLE (GLenum objectType, GLuint name, GLenum pname, GLint *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef GLenum (APIENTRYP PFNGLOBJECTPURGEABLEAPPLEPROC) (GLenum objectType, GLuint name, GLenum option);
+typedef GLenum (APIENTRYP PFNGLOBJECTUNPURGEABLEAPPLEPROC) (GLenum objectType, GLuint name, GLenum option);
+typedef void (APIENTRYP PFNGLGETOBJECTPARAMETERIVAPPLEPROC) (GLenum objectType, GLuint name, GLenum pname, GLint *params);
+#endif
+
+#ifndef GL_APPLE_row_bytes
+#define GL_APPLE_row_bytes 1
+#endif
+
+#ifndef GL_APPLE_rgb_422
+#define GL_APPLE_rgb_422 1
+#endif
+
+#ifndef GL_NV_video_capture
+#define GL_NV_video_capture 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBeginVideoCaptureNV (GLuint video_capture_slot);
+GLAPI void APIENTRY glBindVideoCaptureStreamBufferNV (GLuint video_capture_slot, GLuint stream, GLenum frame_region, GLintptrARB offset);
+GLAPI void APIENTRY glBindVideoCaptureStreamTextureNV (GLuint video_capture_slot, GLuint stream, GLenum frame_region, GLenum target, GLuint texture);
+GLAPI void APIENTRY glEndVideoCaptureNV (GLuint video_capture_slot);
+GLAPI void APIENTRY glGetVideoCaptureivNV (GLuint video_capture_slot, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetVideoCaptureStreamivNV (GLuint video_capture_slot, GLuint stream, GLenum pname, GLint *params);
+GLAPI void APIENTRY glGetVideoCaptureStreamfvNV (GLuint video_capture_slot, GLuint stream, GLenum pname, GLfloat *params);
+GLAPI void APIENTRY glGetVideoCaptureStreamdvNV (GLuint video_capture_slot, GLuint stream, GLenum pname, GLdouble *params);
+GLAPI GLenum APIENTRY glVideoCaptureNV (GLuint video_capture_slot, GLuint *sequence_num, GLuint64EXT *capture_time);
+GLAPI void APIENTRY glVideoCaptureStreamParameterivNV (GLuint video_capture_slot, GLuint stream, GLenum pname, const GLint *params);
+GLAPI void APIENTRY glVideoCaptureStreamParameterfvNV (GLuint video_capture_slot, GLuint stream, GLenum pname, const GLfloat *params);
+GLAPI void APIENTRY glVideoCaptureStreamParameterdvNV (GLuint video_capture_slot, GLuint stream, GLenum pname, const GLdouble *params);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBEGINVIDEOCAPTURENVPROC) (GLuint video_capture_slot);
+typedef void (APIENTRYP PFNGLBINDVIDEOCAPTURESTREAMBUFFERNVPROC) (GLuint video_capture_slot, GLuint stream, GLenum frame_region, GLintptrARB offset);
+typedef void (APIENTRYP PFNGLBINDVIDEOCAPTURESTREAMTEXTURENVPROC) (GLuint video_capture_slot, GLuint stream, GLenum frame_region, GLenum target, GLuint texture);
+typedef void (APIENTRYP PFNGLENDVIDEOCAPTURENVPROC) (GLuint video_capture_slot);
+typedef void (APIENTRYP PFNGLGETVIDEOCAPTUREIVNVPROC) (GLuint video_capture_slot, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETVIDEOCAPTURESTREAMIVNVPROC) (GLuint video_capture_slot, GLuint stream, GLenum pname, GLint *params);
+typedef void (APIENTRYP PFNGLGETVIDEOCAPTURESTREAMFVNVPROC) (GLuint video_capture_slot, GLuint stream, GLenum pname, GLfloat *params);
+typedef void (APIENTRYP PFNGLGETVIDEOCAPTURESTREAMDVNVPROC) (GLuint video_capture_slot, GLuint stream, GLenum pname, GLdouble *params);
+typedef GLenum (APIENTRYP PFNGLVIDEOCAPTURENVPROC) (GLuint video_capture_slot, GLuint *sequence_num, GLuint64EXT *capture_time);
+typedef void (APIENTRYP PFNGLVIDEOCAPTURESTREAMPARAMETERIVNVPROC) (GLuint video_capture_slot, GLuint stream, GLenum pname, const GLint *params);
+typedef void (APIENTRYP PFNGLVIDEOCAPTURESTREAMPARAMETERFVNVPROC) (GLuint video_capture_slot, GLuint stream, GLenum pname, const GLfloat *params);
+typedef void (APIENTRYP PFNGLVIDEOCAPTURESTREAMPARAMETERDVNVPROC) (GLuint video_capture_slot, GLuint stream, GLenum pname, const GLdouble *params);
+#endif
+
+#ifndef GL_NV_copy_image
+#define GL_NV_copy_image 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glCopyImageSubDataNV (GLuint srcName, GLenum srcTarget, GLint srcLevel, GLint srcX, GLint srcY, GLint srcZ, GLuint dstName, GLenum dstTarget, GLint dstLevel, GLint dstX, GLint dstY, GLint dstZ, GLsizei width, GLsizei height, GLsizei depth);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLCOPYIMAGESUBDATANVPROC) (GLuint srcName, GLenum srcTarget, GLint srcLevel, GLint srcX, GLint srcY, GLint srcZ, GLuint dstName, GLenum dstTarget, GLint dstLevel, GLint dstX, GLint dstY, GLint dstZ, GLsizei width, GLsizei height, GLsizei depth);
+#endif
+
+#ifndef GL_EXT_separate_shader_objects
+#define GL_EXT_separate_shader_objects 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glUseShaderProgramEXT (GLenum type, GLuint program);
+GLAPI void APIENTRY glActiveProgramEXT (GLuint program);
+GLAPI GLuint APIENTRY glCreateShaderProgramEXT (GLenum type, const GLchar *string);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLUSESHADERPROGRAMEXTPROC) (GLenum type, GLuint program);
+typedef void (APIENTRYP PFNGLACTIVEPROGRAMEXTPROC) (GLuint program);
+typedef GLuint (APIENTRYP PFNGLCREATESHADERPROGRAMEXTPROC) (GLenum type, const GLchar *string);
+#endif
+
+#ifndef GL_NV_parameter_buffer_object2
+#define GL_NV_parameter_buffer_object2 1
+#endif
+
+#ifndef GL_NV_shader_buffer_load
+#define GL_NV_shader_buffer_load 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glMakeBufferResidentNV (GLenum target, GLenum access);
+GLAPI void APIENTRY glMakeBufferNonResidentNV (GLenum target);
+GLAPI GLboolean APIENTRY glIsBufferResidentNV (GLenum target);
+GLAPI void APIENTRY glMakeNamedBufferResidentNV (GLuint buffer, GLenum access);
+GLAPI void APIENTRY glMakeNamedBufferNonResidentNV (GLuint buffer);
+GLAPI GLboolean APIENTRY glIsNamedBufferResidentNV (GLuint buffer);
+GLAPI void APIENTRY glGetBufferParameterui64vNV (GLenum target, GLenum pname, GLuint64EXT *params);
+GLAPI void APIENTRY glGetNamedBufferParameterui64vNV (GLuint buffer, GLenum pname, GLuint64EXT *params);
+GLAPI void APIENTRY glGetIntegerui64vNV (GLenum value, GLuint64EXT *result);
+GLAPI void APIENTRY glUniformui64NV (GLint location, GLuint64EXT value);
+GLAPI void APIENTRY glUniformui64vNV (GLint location, GLsizei count, const GLuint64EXT *value);
+GLAPI void APIENTRY glGetUniformui64vNV (GLuint program, GLint location, GLuint64EXT *params);
+GLAPI void APIENTRY glProgramUniformui64NV (GLuint program, GLint location, GLuint64EXT value);
+GLAPI void APIENTRY glProgramUniformui64vNV (GLuint program, GLint location, GLsizei count, const GLuint64EXT *value);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLMAKEBUFFERRESIDENTNVPROC) (GLenum target, GLenum access);
+typedef void (APIENTRYP PFNGLMAKEBUFFERNONRESIDENTNVPROC) (GLenum target);
+typedef GLboolean (APIENTRYP PFNGLISBUFFERRESIDENTNVPROC) (GLenum target);
+typedef void (APIENTRYP PFNGLMAKENAMEDBUFFERRESIDENTNVPROC) (GLuint buffer, GLenum access);
+typedef void (APIENTRYP PFNGLMAKENAMEDBUFFERNONRESIDENTNVPROC) (GLuint buffer);
+typedef GLboolean (APIENTRYP PFNGLISNAMEDBUFFERRESIDENTNVPROC) (GLuint buffer);
+typedef void (APIENTRYP PFNGLGETBUFFERPARAMETERUI64VNVPROC) (GLenum target, GLenum pname, GLuint64EXT *params);
+typedef void (APIENTRYP PFNGLGETNAMEDBUFFERPARAMETERUI64VNVPROC) (GLuint buffer, GLenum pname, GLuint64EXT *params);
+typedef void (APIENTRYP PFNGLGETINTEGERUI64VNVPROC) (GLenum value, GLuint64EXT *result);
+typedef void (APIENTRYP PFNGLUNIFORMUI64NVPROC) (GLint location, GLuint64EXT value);
+typedef void (APIENTRYP PFNGLUNIFORMUI64VNVPROC) (GLint location, GLsizei count, const GLuint64EXT *value);
+typedef void (APIENTRYP PFNGLGETUNIFORMUI64VNVPROC) (GLuint program, GLint location, GLuint64EXT *params);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMUI64NVPROC) (GLuint program, GLint location, GLuint64EXT value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORMUI64VNVPROC) (GLuint program, GLint location, GLsizei count, const GLuint64EXT *value);
+#endif
+
+#ifndef GL_NV_vertex_buffer_unified_memory
+#define GL_NV_vertex_buffer_unified_memory 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBufferAddressRangeNV (GLenum pname, GLuint index, GLuint64EXT address, GLsizeiptr length);
+GLAPI void APIENTRY glVertexFormatNV (GLint size, GLenum type, GLsizei stride);
+GLAPI void APIENTRY glNormalFormatNV (GLenum type, GLsizei stride);
+GLAPI void APIENTRY glColorFormatNV (GLint size, GLenum type, GLsizei stride);
+GLAPI void APIENTRY glIndexFormatNV (GLenum type, GLsizei stride);
+GLAPI void APIENTRY glTexCoordFormatNV (GLint size, GLenum type, GLsizei stride);
+GLAPI void APIENTRY glEdgeFlagFormatNV (GLsizei stride);
+GLAPI void APIENTRY glSecondaryColorFormatNV (GLint size, GLenum type, GLsizei stride);
+GLAPI void APIENTRY glFogCoordFormatNV (GLenum type, GLsizei stride);
+GLAPI void APIENTRY glVertexAttribFormatNV (GLuint index, GLint size, GLenum type, GLboolean normalized, GLsizei stride);
+GLAPI void APIENTRY glVertexAttribIFormatNV (GLuint index, GLint size, GLenum type, GLsizei stride);
+GLAPI void APIENTRY glGetIntegerui64i_vNV (GLenum value, GLuint index, GLuint64EXT *result);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBUFFERADDRESSRANGENVPROC) (GLenum pname, GLuint index, GLuint64EXT address, GLsizeiptr length);
+typedef void (APIENTRYP PFNGLVERTEXFORMATNVPROC) (GLint size, GLenum type, GLsizei stride);
+typedef void (APIENTRYP PFNGLNORMALFORMATNVPROC) (GLenum type, GLsizei stride);
+typedef void (APIENTRYP PFNGLCOLORFORMATNVPROC) (GLint size, GLenum type, GLsizei stride);
+typedef void (APIENTRYP PFNGLINDEXFORMATNVPROC) (GLenum type, GLsizei stride);
+typedef void (APIENTRYP PFNGLTEXCOORDFORMATNVPROC) (GLint size, GLenum type, GLsizei stride);
+typedef void (APIENTRYP PFNGLEDGEFLAGFORMATNVPROC) (GLsizei stride);
+typedef void (APIENTRYP PFNGLSECONDARYCOLORFORMATNVPROC) (GLint size, GLenum type, GLsizei stride);
+typedef void (APIENTRYP PFNGLFOGCOORDFORMATNVPROC) (GLenum type, GLsizei stride);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBFORMATNVPROC) (GLuint index, GLint size, GLenum type, GLboolean normalized, GLsizei stride);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBIFORMATNVPROC) (GLuint index, GLint size, GLenum type, GLsizei stride);
+typedef void (APIENTRYP PFNGLGETINTEGERUI64I_VNVPROC) (GLenum value, GLuint index, GLuint64EXT *result);
+#endif
+
+#ifndef GL_NV_texture_barrier
+#define GL_NV_texture_barrier 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glTextureBarrierNV (void);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLTEXTUREBARRIERNVPROC) (void);
+#endif
+
+#ifndef GL_AMD_shader_stencil_export
+#define GL_AMD_shader_stencil_export 1
+#endif
+
+#ifndef GL_AMD_seamless_cubemap_per_texture
+#define GL_AMD_seamless_cubemap_per_texture 1
+#endif
+
+#ifndef GL_AMD_conservative_depth
+#define GL_AMD_conservative_depth 1
+#endif
+
+#ifndef GL_EXT_shader_image_load_store
+#define GL_EXT_shader_image_load_store 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glBindImageTextureEXT (GLuint index, GLuint texture, GLint level, GLboolean layered, GLint layer, GLenum access, GLint format);
+GLAPI void APIENTRY glMemoryBarrierEXT (GLbitfield barriers);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLBINDIMAGETEXTUREEXTPROC) (GLuint index, GLuint texture, GLint level, GLboolean layered, GLint layer, GLenum access, GLint format);
+typedef void (APIENTRYP PFNGLMEMORYBARRIEREXTPROC) (GLbitfield barriers);
+#endif
+
+#ifndef GL_EXT_vertex_attrib_64bit
+#define GL_EXT_vertex_attrib_64bit 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glVertexAttribL1dEXT (GLuint index, GLdouble x);
+GLAPI void APIENTRY glVertexAttribL2dEXT (GLuint index, GLdouble x, GLdouble y);
+GLAPI void APIENTRY glVertexAttribL3dEXT (GLuint index, GLdouble x, GLdouble y, GLdouble z);
+GLAPI void APIENTRY glVertexAttribL4dEXT (GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+GLAPI void APIENTRY glVertexAttribL1dvEXT (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttribL2dvEXT (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttribL3dvEXT (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttribL4dvEXT (GLuint index, const GLdouble *v);
+GLAPI void APIENTRY glVertexAttribLPointerEXT (GLuint index, GLint size, GLenum type, GLsizei stride, const GLvoid *pointer);
+GLAPI void APIENTRY glGetVertexAttribLdvEXT (GLuint index, GLenum pname, GLdouble *params);
+GLAPI void APIENTRY glVertexArrayVertexAttribLOffsetEXT (GLuint vaobj, GLuint buffer, GLuint index, GLint size, GLenum type, GLsizei stride, GLintptr offset);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL1DEXTPROC) (GLuint index, GLdouble x);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL2DEXTPROC) (GLuint index, GLdouble x, GLdouble y);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL3DEXTPROC) (GLuint index, GLdouble x, GLdouble y, GLdouble z);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL4DEXTPROC) (GLuint index, GLdouble x, GLdouble y, GLdouble z, GLdouble w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL1DVEXTPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL2DVEXTPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL3DVEXTPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL4DVEXTPROC) (GLuint index, const GLdouble *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBLPOINTEREXTPROC) (GLuint index, GLint size, GLenum type, GLsizei stride, const GLvoid *pointer);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBLDVEXTPROC) (GLuint index, GLenum pname, GLdouble *params);
+typedef void (APIENTRYP PFNGLVERTEXARRAYVERTEXATTRIBLOFFSETEXTPROC) (GLuint vaobj, GLuint buffer, GLuint index, GLint size, GLenum type, GLsizei stride, GLintptr offset);
+#endif
+
+#ifndef GL_NV_gpu_program5
+#define GL_NV_gpu_program5 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glProgramSubroutineParametersuivNV (GLenum target, GLsizei count, const GLuint *params);
+GLAPI void APIENTRY glGetProgramSubroutineParameteruivNV (GLenum target, GLuint index, GLuint *param);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLPROGRAMSUBROUTINEPARAMETERSUIVNVPROC) (GLenum target, GLsizei count, const GLuint *params);
+typedef void (APIENTRYP PFNGLGETPROGRAMSUBROUTINEPARAMETERUIVNVPROC) (GLenum target, GLuint index, GLuint *param);
+#endif
+
+#ifndef GL_NV_gpu_shader5
+#define GL_NV_gpu_shader5 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glUniform1i64NV (GLint location, GLint64EXT x);
+GLAPI void APIENTRY glUniform2i64NV (GLint location, GLint64EXT x, GLint64EXT y);
+GLAPI void APIENTRY glUniform3i64NV (GLint location, GLint64EXT x, GLint64EXT y, GLint64EXT z);
+GLAPI void APIENTRY glUniform4i64NV (GLint location, GLint64EXT x, GLint64EXT y, GLint64EXT z, GLint64EXT w);
+GLAPI void APIENTRY glUniform1i64vNV (GLint location, GLsizei count, const GLint64EXT *value);
+GLAPI void APIENTRY glUniform2i64vNV (GLint location, GLsizei count, const GLint64EXT *value);
+GLAPI void APIENTRY glUniform3i64vNV (GLint location, GLsizei count, const GLint64EXT *value);
+GLAPI void APIENTRY glUniform4i64vNV (GLint location, GLsizei count, const GLint64EXT *value);
+GLAPI void APIENTRY glUniform1ui64NV (GLint location, GLuint64EXT x);
+GLAPI void APIENTRY glUniform2ui64NV (GLint location, GLuint64EXT x, GLuint64EXT y);
+GLAPI void APIENTRY glUniform3ui64NV (GLint location, GLuint64EXT x, GLuint64EXT y, GLuint64EXT z);
+GLAPI void APIENTRY glUniform4ui64NV (GLint location, GLuint64EXT x, GLuint64EXT y, GLuint64EXT z, GLuint64EXT w);
+GLAPI void APIENTRY glUniform1ui64vNV (GLint location, GLsizei count, const GLuint64EXT *value);
+GLAPI void APIENTRY glUniform2ui64vNV (GLint location, GLsizei count, const GLuint64EXT *value);
+GLAPI void APIENTRY glUniform3ui64vNV (GLint location, GLsizei count, const GLuint64EXT *value);
+GLAPI void APIENTRY glUniform4ui64vNV (GLint location, GLsizei count, const GLuint64EXT *value);
+GLAPI void APIENTRY glGetUniformi64vNV (GLuint program, GLint location, GLint64EXT *params);
+GLAPI void APIENTRY glProgramUniform1i64NV (GLuint program, GLint location, GLint64EXT x);
+GLAPI void APIENTRY glProgramUniform2i64NV (GLuint program, GLint location, GLint64EXT x, GLint64EXT y);
+GLAPI void APIENTRY glProgramUniform3i64NV (GLuint program, GLint location, GLint64EXT x, GLint64EXT y, GLint64EXT z);
+GLAPI void APIENTRY glProgramUniform4i64NV (GLuint program, GLint location, GLint64EXT x, GLint64EXT y, GLint64EXT z, GLint64EXT w);
+GLAPI void APIENTRY glProgramUniform1i64vNV (GLuint program, GLint location, GLsizei count, const GLint64EXT *value);
+GLAPI void APIENTRY glProgramUniform2i64vNV (GLuint program, GLint location, GLsizei count, const GLint64EXT *value);
+GLAPI void APIENTRY glProgramUniform3i64vNV (GLuint program, GLint location, GLsizei count, const GLint64EXT *value);
+GLAPI void APIENTRY glProgramUniform4i64vNV (GLuint program, GLint location, GLsizei count, const GLint64EXT *value);
+GLAPI void APIENTRY glProgramUniform1ui64NV (GLuint program, GLint location, GLuint64EXT x);
+GLAPI void APIENTRY glProgramUniform2ui64NV (GLuint program, GLint location, GLuint64EXT x, GLuint64EXT y);
+GLAPI void APIENTRY glProgramUniform3ui64NV (GLuint program, GLint location, GLuint64EXT x, GLuint64EXT y, GLuint64EXT z);
+GLAPI void APIENTRY glProgramUniform4ui64NV (GLuint program, GLint location, GLuint64EXT x, GLuint64EXT y, GLuint64EXT z, GLuint64EXT w);
+GLAPI void APIENTRY glProgramUniform1ui64vNV (GLuint program, GLint location, GLsizei count, const GLuint64EXT *value);
+GLAPI void APIENTRY glProgramUniform2ui64vNV (GLuint program, GLint location, GLsizei count, const GLuint64EXT *value);
+GLAPI void APIENTRY glProgramUniform3ui64vNV (GLuint program, GLint location, GLsizei count, const GLuint64EXT *value);
+GLAPI void APIENTRY glProgramUniform4ui64vNV (GLuint program, GLint location, GLsizei count, const GLuint64EXT *value);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLUNIFORM1I64NVPROC) (GLint location, GLint64EXT x);
+typedef void (APIENTRYP PFNGLUNIFORM2I64NVPROC) (GLint location, GLint64EXT x, GLint64EXT y);
+typedef void (APIENTRYP PFNGLUNIFORM3I64NVPROC) (GLint location, GLint64EXT x, GLint64EXT y, GLint64EXT z);
+typedef void (APIENTRYP PFNGLUNIFORM4I64NVPROC) (GLint location, GLint64EXT x, GLint64EXT y, GLint64EXT z, GLint64EXT w);
+typedef void (APIENTRYP PFNGLUNIFORM1I64VNVPROC) (GLint location, GLsizei count, const GLint64EXT *value);
+typedef void (APIENTRYP PFNGLUNIFORM2I64VNVPROC) (GLint location, GLsizei count, const GLint64EXT *value);
+typedef void (APIENTRYP PFNGLUNIFORM3I64VNVPROC) (GLint location, GLsizei count, const GLint64EXT *value);
+typedef void (APIENTRYP PFNGLUNIFORM4I64VNVPROC) (GLint location, GLsizei count, const GLint64EXT *value);
+typedef void (APIENTRYP PFNGLUNIFORM1UI64NVPROC) (GLint location, GLuint64EXT x);
+typedef void (APIENTRYP PFNGLUNIFORM2UI64NVPROC) (GLint location, GLuint64EXT x, GLuint64EXT y);
+typedef void (APIENTRYP PFNGLUNIFORM3UI64NVPROC) (GLint location, GLuint64EXT x, GLuint64EXT y, GLuint64EXT z);
+typedef void (APIENTRYP PFNGLUNIFORM4UI64NVPROC) (GLint location, GLuint64EXT x, GLuint64EXT y, GLuint64EXT z, GLuint64EXT w);
+typedef void (APIENTRYP PFNGLUNIFORM1UI64VNVPROC) (GLint location, GLsizei count, const GLuint64EXT *value);
+typedef void (APIENTRYP PFNGLUNIFORM2UI64VNVPROC) (GLint location, GLsizei count, const GLuint64EXT *value);
+typedef void (APIENTRYP PFNGLUNIFORM3UI64VNVPROC) (GLint location, GLsizei count, const GLuint64EXT *value);
+typedef void (APIENTRYP PFNGLUNIFORM4UI64VNVPROC) (GLint location, GLsizei count, const GLuint64EXT *value);
+typedef void (APIENTRYP PFNGLGETUNIFORMI64VNVPROC) (GLuint program, GLint location, GLint64EXT *params);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1I64NVPROC) (GLuint program, GLint location, GLint64EXT x);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2I64NVPROC) (GLuint program, GLint location, GLint64EXT x, GLint64EXT y);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3I64NVPROC) (GLuint program, GLint location, GLint64EXT x, GLint64EXT y, GLint64EXT z);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4I64NVPROC) (GLuint program, GLint location, GLint64EXT x, GLint64EXT y, GLint64EXT z, GLint64EXT w);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1I64VNVPROC) (GLuint program, GLint location, GLsizei count, const GLint64EXT *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2I64VNVPROC) (GLuint program, GLint location, GLsizei count, const GLint64EXT *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3I64VNVPROC) (GLuint program, GLint location, GLsizei count, const GLint64EXT *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4I64VNVPROC) (GLuint program, GLint location, GLsizei count, const GLint64EXT *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1UI64NVPROC) (GLuint program, GLint location, GLuint64EXT x);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2UI64NVPROC) (GLuint program, GLint location, GLuint64EXT x, GLuint64EXT y);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3UI64NVPROC) (GLuint program, GLint location, GLuint64EXT x, GLuint64EXT y, GLuint64EXT z);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4UI64NVPROC) (GLuint program, GLint location, GLuint64EXT x, GLuint64EXT y, GLuint64EXT z, GLuint64EXT w);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM1UI64VNVPROC) (GLuint program, GLint location, GLsizei count, const GLuint64EXT *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM2UI64VNVPROC) (GLuint program, GLint location, GLsizei count, const GLuint64EXT *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM3UI64VNVPROC) (GLuint program, GLint location, GLsizei count, const GLuint64EXT *value);
+typedef void (APIENTRYP PFNGLPROGRAMUNIFORM4UI64VNVPROC) (GLuint program, GLint location, GLsizei count, const GLuint64EXT *value);
+#endif
+
+#ifndef GL_NV_shader_buffer_store
+#define GL_NV_shader_buffer_store 1
+#endif
+
+#ifndef GL_NV_tessellation_program5
+#define GL_NV_tessellation_program5 1
+#endif
+
+#ifndef GL_NV_vertex_attrib_integer_64bit
+#define GL_NV_vertex_attrib_integer_64bit 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glVertexAttribL1i64NV (GLuint index, GLint64EXT x);
+GLAPI void APIENTRY glVertexAttribL2i64NV (GLuint index, GLint64EXT x, GLint64EXT y);
+GLAPI void APIENTRY glVertexAttribL3i64NV (GLuint index, GLint64EXT x, GLint64EXT y, GLint64EXT z);
+GLAPI void APIENTRY glVertexAttribL4i64NV (GLuint index, GLint64EXT x, GLint64EXT y, GLint64EXT z, GLint64EXT w);
+GLAPI void APIENTRY glVertexAttribL1i64vNV (GLuint index, const GLint64EXT *v);
+GLAPI void APIENTRY glVertexAttribL2i64vNV (GLuint index, const GLint64EXT *v);
+GLAPI void APIENTRY glVertexAttribL3i64vNV (GLuint index, const GLint64EXT *v);
+GLAPI void APIENTRY glVertexAttribL4i64vNV (GLuint index, const GLint64EXT *v);
+GLAPI void APIENTRY glVertexAttribL1ui64NV (GLuint index, GLuint64EXT x);
+GLAPI void APIENTRY glVertexAttribL2ui64NV (GLuint index, GLuint64EXT x, GLuint64EXT y);
+GLAPI void APIENTRY glVertexAttribL3ui64NV (GLuint index, GLuint64EXT x, GLuint64EXT y, GLuint64EXT z);
+GLAPI void APIENTRY glVertexAttribL4ui64NV (GLuint index, GLuint64EXT x, GLuint64EXT y, GLuint64EXT z, GLuint64EXT w);
+GLAPI void APIENTRY glVertexAttribL1ui64vNV (GLuint index, const GLuint64EXT *v);
+GLAPI void APIENTRY glVertexAttribL2ui64vNV (GLuint index, const GLuint64EXT *v);
+GLAPI void APIENTRY glVertexAttribL3ui64vNV (GLuint index, const GLuint64EXT *v);
+GLAPI void APIENTRY glVertexAttribL4ui64vNV (GLuint index, const GLuint64EXT *v);
+GLAPI void APIENTRY glGetVertexAttribLi64vNV (GLuint index, GLenum pname, GLint64EXT *params);
+GLAPI void APIENTRY glGetVertexAttribLui64vNV (GLuint index, GLenum pname, GLuint64EXT *params);
+GLAPI void APIENTRY glVertexAttribLFormatNV (GLuint index, GLint size, GLenum type, GLsizei stride);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL1I64NVPROC) (GLuint index, GLint64EXT x);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL2I64NVPROC) (GLuint index, GLint64EXT x, GLint64EXT y);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL3I64NVPROC) (GLuint index, GLint64EXT x, GLint64EXT y, GLint64EXT z);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL4I64NVPROC) (GLuint index, GLint64EXT x, GLint64EXT y, GLint64EXT z, GLint64EXT w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL1I64VNVPROC) (GLuint index, const GLint64EXT *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL2I64VNVPROC) (GLuint index, const GLint64EXT *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL3I64VNVPROC) (GLuint index, const GLint64EXT *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL4I64VNVPROC) (GLuint index, const GLint64EXT *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL1UI64NVPROC) (GLuint index, GLuint64EXT x);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL2UI64NVPROC) (GLuint index, GLuint64EXT x, GLuint64EXT y);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL3UI64NVPROC) (GLuint index, GLuint64EXT x, GLuint64EXT y, GLuint64EXT z);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL4UI64NVPROC) (GLuint index, GLuint64EXT x, GLuint64EXT y, GLuint64EXT z, GLuint64EXT w);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL1UI64VNVPROC) (GLuint index, const GLuint64EXT *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL2UI64VNVPROC) (GLuint index, const GLuint64EXT *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL3UI64VNVPROC) (GLuint index, const GLuint64EXT *v);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBL4UI64VNVPROC) (GLuint index, const GLuint64EXT *v);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBLI64VNVPROC) (GLuint index, GLenum pname, GLint64EXT *params);
+typedef void (APIENTRYP PFNGLGETVERTEXATTRIBLUI64VNVPROC) (GLuint index, GLenum pname, GLuint64EXT *params);
+typedef void (APIENTRYP PFNGLVERTEXATTRIBLFORMATNVPROC) (GLuint index, GLint size, GLenum type, GLsizei stride);
+#endif
+
+#ifndef GL_NV_multisample_coverage
+#define GL_NV_multisample_coverage 1
+#endif
+
+#ifndef GL_AMD_name_gen_delete
+#define GL_AMD_name_gen_delete 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glGenNamesAMD (GLenum identifier, GLuint num, GLuint *names);
+GLAPI void APIENTRY glDeleteNamesAMD (GLenum identifier, GLuint num, const GLuint *names);
+GLAPI GLboolean APIENTRY glIsNameAMD (GLenum identifier, GLuint name);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLGENNAMESAMDPROC) (GLenum identifier, GLuint num, GLuint *names);
+typedef void (APIENTRYP PFNGLDELETENAMESAMDPROC) (GLenum identifier, GLuint num, const GLuint *names);
+typedef GLboolean (APIENTRYP PFNGLISNAMEAMDPROC) (GLenum identifier, GLuint name);
+#endif
+
+#ifndef GL_AMD_debug_output
+#define GL_AMD_debug_output 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glDebugMessageEnableAMD (GLenum category, GLenum severity, GLsizei count, const GLuint *ids, GLboolean enabled);
+GLAPI void APIENTRY glDebugMessageInsertAMD (GLenum category, GLenum severity, GLuint id, GLsizei length, const GLchar *buf);
+GLAPI void APIENTRY glDebugMessageCallbackAMD (GLDEBUGPROCAMD callback, GLvoid *userParam);
+GLAPI GLuint APIENTRY glGetDebugMessageLogAMD (GLuint count, GLsizei bufsize, GLenum *categories, GLuint *severities, GLuint *ids, GLsizei *lengths, GLchar *message);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLDEBUGMESSAGEENABLEAMDPROC) (GLenum category, GLenum severity, GLsizei count, const GLuint *ids, GLboolean enabled);
+typedef void (APIENTRYP PFNGLDEBUGMESSAGEINSERTAMDPROC) (GLenum category, GLenum severity, GLuint id, GLsizei length, const GLchar *buf);
+typedef void (APIENTRYP PFNGLDEBUGMESSAGECALLBACKAMDPROC) (GLDEBUGPROCAMD callback, GLvoid *userParam);
+typedef GLuint (APIENTRYP PFNGLGETDEBUGMESSAGELOGAMDPROC) (GLuint count, GLsizei bufsize, GLenum *categories, GLuint *severities, GLuint *ids, GLsizei *lengths, GLchar *message);
+#endif
+
+#ifndef GL_NV_vdpau_interop
+#define GL_NV_vdpau_interop 1
+#ifdef GL_GLEXT_PROTOTYPES
+GLAPI void APIENTRY glVDPAUInitNV (const GLvoid *vdpDevice, const GLvoid *getProcAddress);
+GLAPI void APIENTRY glVDPAUFiniNV (void);
+GLAPI GLvdpauSurfaceNV APIENTRY glVDPAURegisterVideoSurfaceNV (GLvoid *vdpSurface, GLenum target, GLsizei numTextureNames, const GLuint *textureNames);
+GLAPI GLvdpauSurfaceNV APIENTRY glVDPAURegisterOutputSurfaceNV (GLvoid *vdpSurface, GLenum target, GLsizei numTextureNames, const GLuint *textureNames);
+GLAPI void APIENTRY glVDPAUIsSurfaceNV (GLvdpauSurfaceNV surface);
+GLAPI void APIENTRY glVDPAUUnregisterSurfaceNV (GLvdpauSurfaceNV surface);
+GLAPI void APIENTRY glVDPAUGetSurfaceivNV (GLvdpauSurfaceNV surface, GLenum pname, GLsizei bufSize, GLsizei *length, GLint *values);
+GLAPI void APIENTRY glVDPAUSurfaceAccessNV (GLvdpauSurfaceNV surface, GLenum access);
+GLAPI void APIENTRY glVDPAUMapSurfacesNV (GLsizei numSurfaces, const GLvdpauSurfaceNV *surfaces);
+GLAPI void APIENTRY glVDPAUUnmapSurfacesNV (GLsizei numSurface, const GLvdpauSurfaceNV *surfaces);
+#endif /* GL_GLEXT_PROTOTYPES */
+typedef void (APIENTRYP PFNGLVDPAUINITNVPROC) (const GLvoid *vdpDevice, const GLvoid *getProcAddress);
+typedef void (APIENTRYP PFNGLVDPAUFININVPROC) (void);
+typedef GLvdpauSurfaceNV (APIENTRYP PFNGLVDPAUREGISTERVIDEOSURFACENVPROC) (GLvoid *vdpSurface, GLenum target, GLsizei numTextureNames, const GLuint *textureNames);
+typedef GLvdpauSurfaceNV (APIENTRYP PFNGLVDPAUREGISTEROUTPUTSURFACENVPROC) (GLvoid *vdpSurface, GLenum target, GLsizei numTextureNames, const GLuint *textureNames);
+typedef void (APIENTRYP PFNGLVDPAUISSURFACENVPROC) (GLvdpauSurfaceNV surface);
+typedef void (APIENTRYP PFNGLVDPAUUNREGISTERSURFACENVPROC) (GLvdpauSurfaceNV surface);
+typedef void (APIENTRYP PFNGLVDPAUGETSURFACEIVNVPROC) (GLvdpauSurfaceNV surface, GLenum pname, GLsizei bufSize, GLsizei *length, GLint *values);
+typedef void (APIENTRYP PFNGLVDPAUSURFACEACCESSNVPROC) (GLvdpauSurfaceNV surface, GLenum access);
+typedef void (APIENTRYP PFNGLVDPAUMAPSURFACESNVPROC) (GLsizei numSurfaces, const GLvdpauSurfaceNV *surfaces);
+typedef void (APIENTRYP PFNGLVDPAUUNMAPSURFACESNVPROC) (GLsizei numSurface, const GLvdpauSurfaceNV *surfaces);
+#endif
+
+#ifndef GL_AMD_transform_feedback3_lines_triangles
+#define GL_AMD_transform_feedback3_lines_triangles 1
+#endif
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/Simbody/Visualizer/simbody-visualizer/glut32/glut.h b/Simbody/Visualizer/simbody-visualizer/glut32/glut.h
new file mode 100644
index 0000000..ae48436
--- /dev/null
+++ b/Simbody/Visualizer/simbody-visualizer/glut32/glut.h
@@ -0,0 +1,730 @@
+#ifndef __glut_h__
+#define __glut_h__
+
+/* Copyright (c) Mark J. Kilgard, 1994, 1995, 1996, 1998. */
+
+/* This program is freely distributable without licensing fees and is
+ provided without guarantee or warrantee expressed or implied. This
+ program is -not- in the public domain. */
+
+#if defined(_WIN32)
+#if defined(_MSC_VER)
+ #pragma warning(disable:4996)/*"unsafe" strcpy(), etc.*/
+#endif
+
+// In case windows.h gets included by someone anyway.
+#ifndef WIN32_LEAN_AND_MEAN
+# define WIN32_LEAN_AND_MEAN
+#endif
+#ifndef NOMINMAX
+# define NOMINMAX
+#endif
+/* GLUT 3.7 now tries to avoid including <windows.h>
+ to avoid name space pollution, but Win32's <GL/gl.h>
+ needs APIENTRY and WINGDIAPI defined properly. */
+# if 0
+ /* This would put tons of macros and crap in our clean name space. */
+# include <windows.h>
+# else
+ /* XXX This is from Win32's <windef.h> */
+# ifndef APIENTRY
+# define GLUT_APIENTRY_DEFINED
+# if (_MSC_VER >= 800) || defined(_STDCALL_SUPPORTED) || defined(__BORLANDC__) || defined(__LCC__)
+# define APIENTRY __stdcall
+# else
+# define APIENTRY
+# endif
+# endif
+ /* XXX This is from Win32's <winnt.h> */
+# if defined(CALLBACK)
+# else
+# if ((_MSC_VER >= 800) || defined(_STDCALL_SUPPORTED) || defined(_M_MRX000) || defined(_M_IX86) || defined(_M_ALPHA) || defined(_M_PPC)) && !defined(MIDL_PASS) || defined(__LCC__)
+# define CALLBACK __stdcall
+# else
+# define CALLBACK
+# endif
+# endif
+ /* XXX Hack for lcc compiler. It doesn't support __declspec(dllimport), just __stdcall. */
+# if defined( __LCC__ )
+# undef WINGDIAPI
+# define WINGDIAPI __stdcall
+# else
+ /* XXX This is from Win32's <wingdi.h> and <winnt.h> */
+# ifndef WINGDIAPI
+# define GLUT_WINGDIAPI_DEFINED
+# define WINGDIAPI __declspec(dllimport)
+# endif
+# endif
+ /* XXX This is from Win32's <ctype.h> */
+# ifndef _WCHAR_T_DEFINED
+typedef unsigned short wchar_t;
+# define _WCHAR_T_DEFINED
+# endif
+# endif
+
+/* To disable automatic library usage for GLUT, define GLUT_NO_LIB_PRAGMA
+ in your compile preprocessor options. */
+# if !defined(GLUT_BUILDING_LIB) && !defined(GLUT_NO_LIB_PRAGMA)
+# pragma comment (lib, "winmm.lib") /* link with Windows MultiMedia lib */
+/* To enable automatic SGI OpenGL for Windows library usage for GLUT,
+ define GLUT_USE_SGI_OPENGL in your compile preprocessor options. */
+# ifdef GLUT_USE_SGI_OPENGL
+# pragma comment (lib, "opengl.lib") /* link with SGI OpenGL for Windows lib */
+# pragma comment (lib, "glu.lib") /* link with SGI OpenGL Utility lib */
+# pragma comment (lib, "glut.lib") /* link with Win32 GLUT for SGI OpenGL lib */
+# else
+# pragma comment (lib, "opengl32.lib") /* link with Microsoft OpenGL lib */
+# pragma comment (lib, "glu32.lib") /* link with Microsoft OpenGL Utility lib */
+# pragma comment (lib, "glut32.lib") /* link with Win32 GLUT lib */
+# endif
+# endif
+
+/* To disable supression of annoying warnings about floats being promoted
+ to doubles, define GLUT_NO_WARNING_DISABLE in your compile preprocessor
+ options. */
+# ifndef GLUT_NO_WARNING_DISABLE
+# pragma warning (disable:4244) /* Disable bogus VC++ 4.2 conversion warnings. */
+# pragma warning (disable:4305) /* VC++ 5.0 version of above warning. */
+# endif
+
+/* Win32 has an annoying issue where there are multiple C run-time
+ libraries (CRTs). If the executable is linked with a different CRT
+ from the GLUT DLL, the GLUT DLL will not share the same CRT static
+ data seen by the executable. In particular, atexit callbacks registered
+ in the executable will not be called if GLUT calls its (different)
+ exit routine). GLUT is typically built with the
+ "/MD" option (the CRT with multithreading DLL support), but the Visual
+ C++ linker default is "/ML" (the single threaded CRT).
+
+ One workaround to this issue is requiring users to always link with
+ the same CRT as GLUT is compiled with. That requires users supply a
+ non-standard option. GLUT 3.7 has its own built-in workaround where
+ the executable's "exit" function pointer is covertly passed to GLUT.
+ GLUT then calls the executable's exit function pointer to ensure that
+ any "atexit" calls registered by the application are called if GLUT
+ needs to exit.
+
+ Note that the __glut*WithExit routines should NEVER be called directly.
+ To avoid the atexit workaround, #define GLUT_DISABLE_ATEXIT_HACK. */
+
+/* XXX This is from Win32's <process.h> */
+# if !defined(_MSC_VER) && !defined(__cdecl)
+ /* Define __cdecl for non-Microsoft compilers. */
+# define __cdecl
+# define GLUT_DEFINED___CDECL
+# endif
+# ifndef _CRTIMP
+# ifdef _NTSDK
+ /* Definition compatible with NT SDK */
+# define _CRTIMP
+# else
+ /* Current definition */
+# ifdef _DLL
+# define _CRTIMP __declspec(dllimport)
+# else
+# define _CRTIMP
+# endif
+# endif
+# define GLUT_DEFINED__CRTIMP
+# endif
+
+/* GLUT API entry point declarations for Win32. */
+# ifdef GLUT_BUILDING_LIB
+# define GLUTAPI __declspec(dllexport)
+# else
+# ifdef _DLL
+# define GLUTAPI __declspec(dllimport)
+# else
+# define GLUTAPI extern
+# endif
+# endif
+
+/* GLUT callback calling convention for Win32. */
+# define GLUTCALLBACK __cdecl
+
+#endif /* _WIN32 */
+
+#include <GL/gl.h>
+#include <GL/glu.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if defined(_WIN32)
+# ifndef GLUT_BUILDING_LIB
+extern _CRTIMP __declspec(noreturn) void __cdecl exit(int);
+# endif
+#else
+/* non-Win32 case. */
+/* Define APIENTRY and CALLBACK to nothing if we aren't on Win32. */
+# define APIENTRY
+# define GLUT_APIENTRY_DEFINED
+# define CALLBACK
+/* Define GLUTAPI and GLUTCALLBACK as below if we aren't on Win32. */
+# define GLUTAPI extern
+# define GLUTCALLBACK
+/* Prototype exit for the non-Win32 case (see above). */
+extern void exit(int);
+#endif
+
+/**
+ GLUT API revision history:
+
+ GLUT_API_VERSION is updated to reflect incompatible GLUT
+ API changes (interface changes, semantic changes, deletions,
+ or additions).
+
+ GLUT_API_VERSION=1 First public release of GLUT. 11/29/94
+
+ GLUT_API_VERSION=2 Added support for OpenGL/GLX multisampling,
+ extension. Supports new input devices like tablet, dial and button
+ box, and Spaceball. Easy to query OpenGL extensions.
+
+ GLUT_API_VERSION=3 glutMenuStatus added.
+
+ GLUT_API_VERSION=4 glutInitDisplayString, glutWarpPointer,
+ glutBitmapLength, glutStrokeLength, glutWindowStatusFunc, dynamic
+ video resize subAPI, glutPostWindowRedisplay, glutKeyboardUpFunc,
+ glutSpecialUpFunc, glutIgnoreKeyRepeat, glutSetKeyRepeat,
+ glutJoystickFunc, glutForceJoystickFunc (NOT FINALIZED!).
+**/
+#ifndef GLUT_API_VERSION /* allow this to be overriden */
+#define GLUT_API_VERSION 3
+#endif
+
+/**
+ GLUT implementation revision history:
+
+ GLUT_XLIB_IMPLEMENTATION is updated to reflect both GLUT
+ API revisions and implementation revisions (ie, bug fixes).
+
+ GLUT_XLIB_IMPLEMENTATION=1 mjk's first public release of
+ GLUT Xlib-based implementation. 11/29/94
+
+ GLUT_XLIB_IMPLEMENTATION=2 mjk's second public release of
+ GLUT Xlib-based implementation providing GLUT version 2
+ interfaces.
+
+ GLUT_XLIB_IMPLEMENTATION=3 mjk's GLUT 2.2 images. 4/17/95
+
+ GLUT_XLIB_IMPLEMENTATION=4 mjk's GLUT 2.3 images. 6/?/95
+
+ GLUT_XLIB_IMPLEMENTATION=5 mjk's GLUT 3.0 images. 10/?/95
+
+ GLUT_XLIB_IMPLEMENTATION=7 mjk's GLUT 3.1+ with glutWarpPoitner. 7/24/96
+
+ GLUT_XLIB_IMPLEMENTATION=8 mjk's GLUT 3.1+ with glutWarpPoitner
+ and video resize. 1/3/97
+
+ GLUT_XLIB_IMPLEMENTATION=9 mjk's GLUT 3.4 release with early GLUT 4 routines.
+
+ GLUT_XLIB_IMPLEMENTATION=11 Mesa 2.5's GLUT 3.6 release.
+
+ GLUT_XLIB_IMPLEMENTATION=12 mjk's GLUT 3.6 release with early GLUT 4 routines + signal handling.
+
+ GLUT_XLIB_IMPLEMENTATION=13 mjk's GLUT 3.7 beta with GameGLUT support.
+
+ GLUT_XLIB_IMPLEMENTATION=14 mjk's GLUT 3.7 beta with f90gl friend interface.
+
+ GLUT_XLIB_IMPLEMENTATION=15 mjk's GLUT 3.7 beta sync'ed with Mesa <GL/glut.h>
+**/
+#ifndef GLUT_XLIB_IMPLEMENTATION /* Allow this to be overriden. */
+#define GLUT_XLIB_IMPLEMENTATION 15
+#endif
+
+/* Display mode bit masks. */
+#define GLUT_RGB 0
+#define GLUT_RGBA GLUT_RGB
+#define GLUT_INDEX 1
+#define GLUT_SINGLE 0
+#define GLUT_DOUBLE 2
+#define GLUT_ACCUM 4
+#define GLUT_ALPHA 8
+#define GLUT_DEPTH 16
+#define GLUT_STENCIL 32
+#if (GLUT_API_VERSION >= 2)
+#define GLUT_MULTISAMPLE 128
+#define GLUT_STEREO 256
+#endif
+#if (GLUT_API_VERSION >= 3)
+#define GLUT_LUMINANCE 512
+#endif
+
+/* Mouse buttons. */
+#define GLUT_LEFT_BUTTON 0
+#define GLUT_MIDDLE_BUTTON 1
+#define GLUT_RIGHT_BUTTON 2
+#define GLUT_WHEEL_UP 3
+#define GLUT_WHEEL_DOWN 4
+#define GLUT_XBUTTON1 5
+#define GLUT_XBUTTON2 6
+
+/* Mouse button state. */
+#define GLUT_DOWN 0
+#define GLUT_UP 1
+
+#if (GLUT_API_VERSION >= 2)
+/* function keys */
+#define GLUT_KEY_F1 1
+#define GLUT_KEY_F2 2
+#define GLUT_KEY_F3 3
+#define GLUT_KEY_F4 4
+#define GLUT_KEY_F5 5
+#define GLUT_KEY_F6 6
+#define GLUT_KEY_F7 7
+#define GLUT_KEY_F8 8
+#define GLUT_KEY_F9 9
+#define GLUT_KEY_F10 10
+#define GLUT_KEY_F11 11
+#define GLUT_KEY_F12 12
+/* directional keys */
+#define GLUT_KEY_LEFT 100
+#define GLUT_KEY_UP 101
+#define GLUT_KEY_RIGHT 102
+#define GLUT_KEY_DOWN 103
+#define GLUT_KEY_PAGE_UP 104
+#define GLUT_KEY_PAGE_DOWN 105
+#define GLUT_KEY_HOME 106
+#define GLUT_KEY_END 107
+#define GLUT_KEY_INSERT 108
+#endif
+
+/* Entry/exit state. */
+#define GLUT_LEFT 0
+#define GLUT_ENTERED 1
+
+/* Menu usage state. */
+#define GLUT_MENU_NOT_IN_USE 0
+#define GLUT_MENU_IN_USE 1
+
+/* Visibility state. */
+#define GLUT_NOT_VISIBLE 0
+#define GLUT_VISIBLE 1
+
+/* Window status state. */
+#define GLUT_HIDDEN 0
+#define GLUT_FULLY_RETAINED 1
+#define GLUT_PARTIALLY_RETAINED 2
+#define GLUT_FULLY_COVERED 3
+
+/* Color index component selection values. */
+#define GLUT_RED 0
+#define GLUT_GREEN 1
+#define GLUT_BLUE 2
+
+#if defined(_WIN32)
+/* Stroke font constants (use these in GLUT program). */
+#define GLUT_STROKE_ROMAN ((void*)0)
+#define GLUT_STROKE_MONO_ROMAN ((void*)1)
+
+/* Bitmap font constants (use these in GLUT program). */
+#define GLUT_BITMAP_9_BY_15 ((void*)2)
+#define GLUT_BITMAP_8_BY_13 ((void*)3)
+#define GLUT_BITMAP_TIMES_ROMAN_10 ((void*)4)
+#define GLUT_BITMAP_TIMES_ROMAN_24 ((void*)5)
+#if (GLUT_API_VERSION >= 3)
+#define GLUT_BITMAP_HELVETICA_10 ((void*)6)
+#define GLUT_BITMAP_HELVETICA_12 ((void*)7)
+#define GLUT_BITMAP_HELVETICA_18 ((void*)8)
+#endif
+#else
+/* Stroke font opaque addresses (use constants instead in source code). */
+GLUTAPI void *glutStrokeRoman;
+GLUTAPI void *glutStrokeMonoRoman;
+
+/* Stroke font constants (use these in GLUT program). */
+#define GLUT_STROKE_ROMAN (&glutStrokeRoman)
+#define GLUT_STROKE_MONO_ROMAN (&glutStrokeMonoRoman)
+
+/* Bitmap font opaque addresses (use constants instead in source code). */
+GLUTAPI void *glutBitmap9By15;
+GLUTAPI void *glutBitmap8By13;
+GLUTAPI void *glutBitmapTimesRoman10;
+GLUTAPI void *glutBitmapTimesRoman24;
+GLUTAPI void *glutBitmapHelvetica10;
+GLUTAPI void *glutBitmapHelvetica12;
+GLUTAPI void *glutBitmapHelvetica18;
+
+/* Bitmap font constants (use these in GLUT program). */
+#define GLUT_BITMAP_9_BY_15 (&glutBitmap9By15)
+#define GLUT_BITMAP_8_BY_13 (&glutBitmap8By13)
+#define GLUT_BITMAP_TIMES_ROMAN_10 (&glutBitmapTimesRoman10)
+#define GLUT_BITMAP_TIMES_ROMAN_24 (&glutBitmapTimesRoman24)
+#if (GLUT_API_VERSION >= 3)
+#define GLUT_BITMAP_HELVETICA_10 (&glutBitmapHelvetica10)
+#define GLUT_BITMAP_HELVETICA_12 (&glutBitmapHelvetica12)
+#define GLUT_BITMAP_HELVETICA_18 (&glutBitmapHelvetica18)
+#endif
+#endif
+
+/* glutGet parameters. */
+#define GLUT_WINDOW_X ((GLenum) 100)
+#define GLUT_WINDOW_Y ((GLenum) 101)
+#define GLUT_WINDOW_WIDTH ((GLenum) 102)
+#define GLUT_WINDOW_HEIGHT ((GLenum) 103)
+#define GLUT_WINDOW_BUFFER_SIZE ((GLenum) 104)
+#define GLUT_WINDOW_STENCIL_SIZE ((GLenum) 105)
+#define GLUT_WINDOW_DEPTH_SIZE ((GLenum) 106)
+#define GLUT_WINDOW_RED_SIZE ((GLenum) 107)
+#define GLUT_WINDOW_GREEN_SIZE ((GLenum) 108)
+#define GLUT_WINDOW_BLUE_SIZE ((GLenum) 109)
+#define GLUT_WINDOW_ALPHA_SIZE ((GLenum) 110)
+#define GLUT_WINDOW_ACCUM_RED_SIZE ((GLenum) 111)
+#define GLUT_WINDOW_ACCUM_GREEN_SIZE ((GLenum) 112)
+#define GLUT_WINDOW_ACCUM_BLUE_SIZE ((GLenum) 113)
+#define GLUT_WINDOW_ACCUM_ALPHA_SIZE ((GLenum) 114)
+#define GLUT_WINDOW_DOUBLEBUFFER ((GLenum) 115)
+#define GLUT_WINDOW_RGBA ((GLenum) 116)
+#define GLUT_WINDOW_PARENT ((GLenum) 117)
+#define GLUT_WINDOW_NUM_CHILDREN ((GLenum) 118)
+#define GLUT_WINDOW_COLORMAP_SIZE ((GLenum) 119)
+#if (GLUT_API_VERSION >= 2)
+#define GLUT_WINDOW_NUM_SAMPLES ((GLenum) 120)
+#define GLUT_WINDOW_STEREO ((GLenum) 121)
+#endif
+#if (GLUT_API_VERSION >= 3)
+#define GLUT_WINDOW_CURSOR ((GLenum) 122)
+#endif
+#define GLUT_SCREEN_WIDTH ((GLenum) 200)
+#define GLUT_SCREEN_HEIGHT ((GLenum) 201)
+#define GLUT_SCREEN_WIDTH_MM ((GLenum) 202)
+#define GLUT_SCREEN_HEIGHT_MM ((GLenum) 203)
+#define GLUT_MENU_NUM_ITEMS ((GLenum) 300)
+#define GLUT_DISPLAY_MODE_POSSIBLE ((GLenum) 400)
+#define GLUT_INIT_WINDOW_X ((GLenum) 500)
+#define GLUT_INIT_WINDOW_Y ((GLenum) 501)
+#define GLUT_INIT_WINDOW_WIDTH ((GLenum) 502)
+#define GLUT_INIT_WINDOW_HEIGHT ((GLenum) 503)
+#define GLUT_INIT_DISPLAY_MODE ((GLenum) 504)
+#if (GLUT_API_VERSION >= 2)
+#define GLUT_ELAPSED_TIME ((GLenum) 700)
+#endif
+#if (GLUT_API_VERSION >= 4 || GLUT_XLIB_IMPLEMENTATION >= 13)
+#define GLUT_WINDOW_FORMAT_ID ((GLenum) 123)
+#endif
+
+#if (GLUT_API_VERSION >= 2)
+/* glutDeviceGet parameters. */
+#define GLUT_HAS_KEYBOARD ((GLenum) 600)
+#define GLUT_HAS_MOUSE ((GLenum) 601)
+#define GLUT_HAS_SPACEBALL ((GLenum) 602)
+#define GLUT_HAS_DIAL_AND_BUTTON_BOX ((GLenum) 603)
+#define GLUT_HAS_TABLET ((GLenum) 604)
+#define GLUT_NUM_MOUSE_BUTTONS ((GLenum) 605)
+#define GLUT_NUM_SPACEBALL_BUTTONS ((GLenum) 606)
+#define GLUT_NUM_BUTTON_BOX_BUTTONS ((GLenum) 607)
+#define GLUT_NUM_DIALS ((GLenum) 608)
+#define GLUT_NUM_TABLET_BUTTONS ((GLenum) 609)
+#endif
+#if (GLUT_API_VERSION >= 4 || GLUT_XLIB_IMPLEMENTATION >= 13)
+#define GLUT_DEVICE_IGNORE_KEY_REPEAT ((GLenum) 610)
+#define GLUT_DEVICE_KEY_REPEAT ((GLenum) 611)
+#define GLUT_HAS_JOYSTICK ((GLenum) 612)
+#define GLUT_OWNS_JOYSTICK ((GLenum) 613)
+#define GLUT_JOYSTICK_BUTTONS ((GLenum) 614)
+#define GLUT_JOYSTICK_AXES ((GLenum) 615)
+#define GLUT_JOYSTICK_POLL_RATE ((GLenum) 616)
+#endif
+
+#if (GLUT_API_VERSION >= 3)
+/* glutLayerGet parameters. */
+#define GLUT_OVERLAY_POSSIBLE ((GLenum) 800)
+#define GLUT_LAYER_IN_USE ((GLenum) 801)
+#define GLUT_HAS_OVERLAY ((GLenum) 802)
+#define GLUT_TRANSPARENT_INDEX ((GLenum) 803)
+#define GLUT_NORMAL_DAMAGED ((GLenum) 804)
+#define GLUT_OVERLAY_DAMAGED ((GLenum) 805)
+
+#if (GLUT_API_VERSION >= 4 || GLUT_XLIB_IMPLEMENTATION >= 9)
+/* glutVideoResizeGet parameters. */
+#define GLUT_VIDEO_RESIZE_POSSIBLE ((GLenum) 900)
+#define GLUT_VIDEO_RESIZE_IN_USE ((GLenum) 901)
+#define GLUT_VIDEO_RESIZE_X_DELTA ((GLenum) 902)
+#define GLUT_VIDEO_RESIZE_Y_DELTA ((GLenum) 903)
+#define GLUT_VIDEO_RESIZE_WIDTH_DELTA ((GLenum) 904)
+#define GLUT_VIDEO_RESIZE_HEIGHT_DELTA ((GLenum) 905)
+#define GLUT_VIDEO_RESIZE_X ((GLenum) 906)
+#define GLUT_VIDEO_RESIZE_Y ((GLenum) 907)
+#define GLUT_VIDEO_RESIZE_WIDTH ((GLenum) 908)
+#define GLUT_VIDEO_RESIZE_HEIGHT ((GLenum) 909)
+#endif
+
+/* glutUseLayer parameters. */
+#define GLUT_NORMAL ((GLenum) 0)
+#define GLUT_OVERLAY ((GLenum) 1)
+
+/* glutGetModifiers return mask. */
+#define GLUT_ACTIVE_SHIFT 1
+#define GLUT_ACTIVE_CTRL 2
+#define GLUT_ACTIVE_ALT 4
+
+/* glutSetCursor parameters. */
+/* Basic arrows. */
+#define GLUT_CURSOR_RIGHT_ARROW 0
+#define GLUT_CURSOR_LEFT_ARROW 1
+/* Symbolic cursor shapes. */
+#define GLUT_CURSOR_INFO 2
+#define GLUT_CURSOR_DESTROY 3
+#define GLUT_CURSOR_HELP 4
+#define GLUT_CURSOR_CYCLE 5
+#define GLUT_CURSOR_SPRAY 6
+#define GLUT_CURSOR_WAIT 7
+#define GLUT_CURSOR_TEXT 8
+#define GLUT_CURSOR_CROSSHAIR 9
+/* Directional cursors. */
+#define GLUT_CURSOR_UP_DOWN 10
+#define GLUT_CURSOR_LEFT_RIGHT 11
+/* Sizing cursors. */
+#define GLUT_CURSOR_TOP_SIDE 12
+#define GLUT_CURSOR_BOTTOM_SIDE 13
+#define GLUT_CURSOR_LEFT_SIDE 14
+#define GLUT_CURSOR_RIGHT_SIDE 15
+#define GLUT_CURSOR_TOP_LEFT_CORNER 16
+#define GLUT_CURSOR_TOP_RIGHT_CORNER 17
+#define GLUT_CURSOR_BOTTOM_RIGHT_CORNER 18
+#define GLUT_CURSOR_BOTTOM_LEFT_CORNER 19
+/* Inherit from parent window. */
+#define GLUT_CURSOR_INHERIT 100
+/* Blank cursor. */
+#define GLUT_CURSOR_NONE 101
+/* Fullscreen crosshair (if available). */
+#define GLUT_CURSOR_FULL_CROSSHAIR 102
+#endif
+
+/* GLUT initialization sub-API. */
+GLUTAPI void APIENTRY glutInit(int *argcp, char **argv);
+#if defined(_WIN32) && !defined(GLUT_DISABLE_ATEXIT_HACK)
+GLUTAPI void APIENTRY __glutInitWithExit(int *argcp, char **argv, void (__cdecl *exitfunc)(int));
+#ifndef GLUT_BUILDING_LIB
+static void APIENTRY glutInit_ATEXIT_HACK(int *argcp, char **argv) { __glutInitWithExit(argcp, argv, exit); }
+#define glutInit glutInit_ATEXIT_HACK
+#endif
+#endif
+GLUTAPI void APIENTRY glutInitDisplayMode(unsigned int mode);
+#if (GLUT_API_VERSION >= 4 || GLUT_XLIB_IMPLEMENTATION >= 9)
+GLUTAPI void APIENTRY glutInitDisplayString(const char *string);
+#endif
+GLUTAPI void APIENTRY glutInitWindowPosition(int x, int y);
+GLUTAPI void APIENTRY glutInitWindowSize(int width, int height);
+GLUTAPI void APIENTRY glutMainLoop(void);
+
+/* GLUT window sub-API. */
+GLUTAPI int APIENTRY glutCreateWindow(const char *title);
+#if defined(_WIN32) && !defined(GLUT_DISABLE_ATEXIT_HACK)
+GLUTAPI int APIENTRY __glutCreateWindowWithExit(const char *title, void (__cdecl *exitfunc)(int));
+#ifndef GLUT_BUILDING_LIB
+static int APIENTRY glutCreateWindow_ATEXIT_HACK(const char *title) { return __glutCreateWindowWithExit(title, exit); }
+#define glutCreateWindow glutCreateWindow_ATEXIT_HACK
+#endif
+#endif
+GLUTAPI int APIENTRY glutCreateSubWindow(int win, int x, int y, int width, int height);
+GLUTAPI void APIENTRY glutDestroyWindow(int win);
+GLUTAPI void APIENTRY glutPostRedisplay(void);
+#if (GLUT_API_VERSION >= 4 || GLUT_XLIB_IMPLEMENTATION >= 11)
+GLUTAPI void APIENTRY glutPostWindowRedisplay(int win);
+#endif
+GLUTAPI void APIENTRY glutSwapBuffers(void);
+GLUTAPI int APIENTRY glutGetWindow(void);
+GLUTAPI void APIENTRY glutSetWindow(int win);
+GLUTAPI void APIENTRY glutSetWindowTitle(const char *title);
+GLUTAPI void APIENTRY glutSetIconTitle(const char *title);
+GLUTAPI void APIENTRY glutPositionWindow(int x, int y);
+GLUTAPI void APIENTRY glutReshapeWindow(int width, int height);
+GLUTAPI void APIENTRY glutPopWindow(void);
+GLUTAPI void APIENTRY glutPushWindow(void);
+GLUTAPI void APIENTRY glutIconifyWindow(void);
+GLUTAPI void APIENTRY glutShowWindow(void);
+GLUTAPI void APIENTRY glutHideWindow(void);
+#if (GLUT_API_VERSION >= 3)
+GLUTAPI void APIENTRY glutFullScreen(void);
+GLUTAPI void APIENTRY glutSetCursor(int cursor);
+#if (GLUT_API_VERSION >= 4 || GLUT_XLIB_IMPLEMENTATION >= 9)
+GLUTAPI void APIENTRY glutWarpPointer(int x, int y);
+#endif
+
+/* GLUT overlay sub-API. */
+GLUTAPI void APIENTRY glutEstablishOverlay(void);
+GLUTAPI void APIENTRY glutRemoveOverlay(void);
+GLUTAPI void APIENTRY glutUseLayer(GLenum layer);
+GLUTAPI void APIENTRY glutPostOverlayRedisplay(void);
+#if (GLUT_API_VERSION >= 4 || GLUT_XLIB_IMPLEMENTATION >= 11)
+GLUTAPI void APIENTRY glutPostWindowOverlayRedisplay(int win);
+#endif
+GLUTAPI void APIENTRY glutShowOverlay(void);
+GLUTAPI void APIENTRY glutHideOverlay(void);
+#endif
+
+/* GLUT menu sub-API. */
+GLUTAPI int APIENTRY glutCreateMenu(void (GLUTCALLBACK *func)(int));
+#if defined(_WIN32) && !defined(GLUT_DISABLE_ATEXIT_HACK)
+GLUTAPI int APIENTRY __glutCreateMenuWithExit(void (GLUTCALLBACK *func)(int), void (__cdecl *exitfunc)(int));
+#ifndef GLUT_BUILDING_LIB
+static int APIENTRY glutCreateMenu_ATEXIT_HACK(void (GLUTCALLBACK *func)(int)) { return __glutCreateMenuWithExit(func, exit); }
+#define glutCreateMenu glutCreateMenu_ATEXIT_HACK
+#endif
+#endif
+GLUTAPI void APIENTRY glutDestroyMenu(int menu);
+GLUTAPI int APIENTRY glutGetMenu(void);
+GLUTAPI void APIENTRY glutSetMenu(int menu);
+GLUTAPI void APIENTRY glutAddMenuEntry(const char *label, int value);
+GLUTAPI void APIENTRY glutAddSubMenu(const char *label, int submenu);
+GLUTAPI void APIENTRY glutChangeToMenuEntry(int item, const char *label, int value);
+GLUTAPI void APIENTRY glutChangeToSubMenu(int item, const char *label, int submenu);
+GLUTAPI void APIENTRY glutRemoveMenuItem(int item);
+GLUTAPI void APIENTRY glutAttachMenu(int button);
+GLUTAPI void APIENTRY glutDetachMenu(int button);
+
+/* GLUT window callback sub-API. */
+GLUTAPI void APIENTRY glutDisplayFunc(void (GLUTCALLBACK *func)(void));
+GLUTAPI void APIENTRY glutReshapeFunc(void (GLUTCALLBACK *func)(int width, int height));
+GLUTAPI void APIENTRY glutKeyboardFunc(void (GLUTCALLBACK *func)(unsigned char key, int x, int y));
+GLUTAPI void APIENTRY glutMouseFunc(void (GLUTCALLBACK *func)(int button, int state, int x, int y));
+GLUTAPI void APIENTRY glutMotionFunc(void (GLUTCALLBACK *func)(int x, int y));
+GLUTAPI void APIENTRY glutPassiveMotionFunc(void (GLUTCALLBACK *func)(int x, int y));
+GLUTAPI void APIENTRY glutEntryFunc(void (GLUTCALLBACK *func)(int state));
+GLUTAPI void APIENTRY glutVisibilityFunc(void (GLUTCALLBACK *func)(int state));
+GLUTAPI void APIENTRY glutIdleFunc(void (GLUTCALLBACK *func)(void));
+GLUTAPI void APIENTRY glutTimerFunc(unsigned int millis, void (GLUTCALLBACK *func)(int value), int value);
+GLUTAPI void APIENTRY glutMenuStateFunc(void (GLUTCALLBACK *func)(int state));
+#if (GLUT_API_VERSION >= 2)
+GLUTAPI void APIENTRY glutSpecialFunc(void (GLUTCALLBACK *func)(int key, int x, int y));
+GLUTAPI void APIENTRY glutSpaceballMotionFunc(void (GLUTCALLBACK *func)(int x, int y, int z));
+GLUTAPI void APIENTRY glutSpaceballRotateFunc(void (GLUTCALLBACK *func)(int x, int y, int z));
+GLUTAPI void APIENTRY glutSpaceballButtonFunc(void (GLUTCALLBACK *func)(int button, int state));
+GLUTAPI void APIENTRY glutButtonBoxFunc(void (GLUTCALLBACK *func)(int button, int state));
+GLUTAPI void APIENTRY glutDialsFunc(void (GLUTCALLBACK *func)(int dial, int value));
+GLUTAPI void APIENTRY glutTabletMotionFunc(void (GLUTCALLBACK *func)(int x, int y));
+GLUTAPI void APIENTRY glutTabletButtonFunc(void (GLUTCALLBACK *func)(int button, int state, int x, int y));
+#if (GLUT_API_VERSION >= 3)
+GLUTAPI void APIENTRY glutMenuStatusFunc(void (GLUTCALLBACK *func)(int status, int x, int y));
+GLUTAPI void APIENTRY glutOverlayDisplayFunc(void (GLUTCALLBACK *func)(void));
+#if (GLUT_API_VERSION >= 4 || GLUT_XLIB_IMPLEMENTATION >= 9)
+GLUTAPI void APIENTRY glutWindowStatusFunc(void (GLUTCALLBACK *func)(int state));
+#endif
+#if (GLUT_API_VERSION >= 4 || GLUT_XLIB_IMPLEMENTATION >= 13)
+GLUTAPI void APIENTRY glutKeyboardUpFunc(void (GLUTCALLBACK *func)(unsigned char key, int x, int y));
+GLUTAPI void APIENTRY glutSpecialUpFunc(void (GLUTCALLBACK *func)(int key, int x, int y));
+GLUTAPI void APIENTRY glutJoystickFunc(void (GLUTCALLBACK *func)(unsigned int buttonMask, int x, int y, int z), int pollInterval);
+#endif
+#endif
+#endif
+
+/* GLUT color index sub-API. */
+GLUTAPI void APIENTRY glutSetColor(int, GLfloat red, GLfloat green, GLfloat blue);
+GLUTAPI GLfloat APIENTRY glutGetColor(int ndx, int component);
+GLUTAPI void APIENTRY glutCopyColormap(int win);
+
+/* GLUT state retrieval sub-API. */
+GLUTAPI int APIENTRY glutGet(GLenum type);
+GLUTAPI int APIENTRY glutDeviceGet(GLenum type);
+#if (GLUT_API_VERSION >= 2)
+/* GLUT extension support sub-API */
+GLUTAPI int APIENTRY glutExtensionSupported(const char *name);
+#endif
+#if (GLUT_API_VERSION >= 3)
+GLUTAPI int APIENTRY glutGetModifiers(void);
+GLUTAPI int APIENTRY glutLayerGet(GLenum type);
+#endif
+
+/* GLUT font sub-API */
+GLUTAPI void APIENTRY glutBitmapCharacter(void *font, int character);
+GLUTAPI int APIENTRY glutBitmapWidth(void *font, int character);
+GLUTAPI void APIENTRY glutStrokeCharacter(void *font, int character);
+GLUTAPI int APIENTRY glutStrokeWidth(void *font, int character);
+#if (GLUT_API_VERSION >= 4 || GLUT_XLIB_IMPLEMENTATION >= 9)
+GLUTAPI int APIENTRY glutBitmapLength(void *font, const unsigned char *string);
+GLUTAPI int APIENTRY glutStrokeLength(void *font, const unsigned char *string);
+#endif
+
+/* GLUT pre-built models sub-API */
+GLUTAPI void APIENTRY glutWireSphere(GLdouble radius, GLint slices, GLint stacks);
+GLUTAPI void APIENTRY glutSolidSphere(GLdouble radius, GLint slices, GLint stacks);
+GLUTAPI void APIENTRY glutWireCone(GLdouble base, GLdouble height, GLint slices, GLint stacks);
+GLUTAPI void APIENTRY glutSolidCone(GLdouble base, GLdouble height, GLint slices, GLint stacks);
+GLUTAPI void APIENTRY glutWireCube(GLdouble size);
+GLUTAPI void APIENTRY glutSolidCube(GLdouble size);
+GLUTAPI void APIENTRY glutWireTorus(GLdouble innerRadius, GLdouble outerRadius, GLint sides, GLint rings);
+GLUTAPI void APIENTRY glutSolidTorus(GLdouble innerRadius, GLdouble outerRadius, GLint sides, GLint rings);
+GLUTAPI void APIENTRY glutWireDodecahedron(void);
+GLUTAPI void APIENTRY glutSolidDodecahedron(void);
+GLUTAPI void APIENTRY glutWireTeapot(GLdouble size);
+GLUTAPI void APIENTRY glutSolidTeapot(GLdouble size);
+GLUTAPI void APIENTRY glutWireOctahedron(void);
+GLUTAPI void APIENTRY glutSolidOctahedron(void);
+GLUTAPI void APIENTRY glutWireTetrahedron(void);
+GLUTAPI void APIENTRY glutSolidTetrahedron(void);
+GLUTAPI void APIENTRY glutWireIcosahedron(void);
+GLUTAPI void APIENTRY glutSolidIcosahedron(void);
+
+#if (GLUT_API_VERSION >= 4 || GLUT_XLIB_IMPLEMENTATION >= 9)
+/* GLUT video resize sub-API. */
+GLUTAPI int APIENTRY glutVideoResizeGet(GLenum param);
+GLUTAPI void APIENTRY glutSetupVideoResizing(void);
+GLUTAPI void APIENTRY glutStopVideoResizing(void);
+GLUTAPI void APIENTRY glutVideoResize(int x, int y, int width, int height);
+GLUTAPI void APIENTRY glutVideoPan(int x, int y, int width, int height);
+
+/* GLUT debugging sub-API. */
+GLUTAPI void APIENTRY glutReportErrors(void);
+#endif
+
+#if (GLUT_API_VERSION >= 4 || GLUT_XLIB_IMPLEMENTATION >= 13)
+/* GLUT device control sub-API. */
+/* glutSetKeyRepeat modes. */
+#define GLUT_KEY_REPEAT_OFF 0
+#define GLUT_KEY_REPEAT_ON 1
+#define GLUT_KEY_REPEAT_DEFAULT 2
+
+/* Joystick button masks. */
+#define GLUT_JOYSTICK_BUTTON_A 1
+#define GLUT_JOYSTICK_BUTTON_B 2
+#define GLUT_JOYSTICK_BUTTON_C 4
+#define GLUT_JOYSTICK_BUTTON_D 8
+
+GLUTAPI void APIENTRY glutIgnoreKeyRepeat(int ignore);
+GLUTAPI void APIENTRY glutSetKeyRepeat(int repeatMode);
+GLUTAPI void APIENTRY glutForceJoystickFunc(void);
+
+/* GLUT game mode sub-API. */
+/* glutGameModeGet. */
+#define GLUT_GAME_MODE_ACTIVE ((GLenum) 0)
+#define GLUT_GAME_MODE_POSSIBLE ((GLenum) 1)
+#define GLUT_GAME_MODE_WIDTH ((GLenum) 2)
+#define GLUT_GAME_MODE_HEIGHT ((GLenum) 3)
+#define GLUT_GAME_MODE_PIXEL_DEPTH ((GLenum) 4)
+#define GLUT_GAME_MODE_REFRESH_RATE ((GLenum) 5)
+#define GLUT_GAME_MODE_DISPLAY_CHANGED ((GLenum) 6)
+
+GLUTAPI void APIENTRY glutGameModeString(const char *string);
+GLUTAPI int APIENTRY glutEnterGameMode(void);
+GLUTAPI void APIENTRY glutLeaveGameMode(void);
+GLUTAPI int APIENTRY glutGameModeGet(GLenum mode);
+#endif
+
+#ifdef __cplusplus
+}
+
+#endif
+
+#ifdef GLUT_APIENTRY_DEFINED
+# undef GLUT_APIENTRY_DEFINED
+# undef APIENTRY
+#endif
+
+#ifdef GLUT_WINGDIAPI_DEFINED
+# undef GLUT_WINGDIAPI_DEFINED
+# undef WINGDIAPI
+#endif
+
+#ifdef GLUT_DEFINED___CDECL
+# undef GLUT_DEFINED___CDECL
+# undef __cdecl
+#endif
+
+#ifdef GLUT_DEFINED__CRTIMP
+# undef GLUT_DEFINED__CRTIMP
+# undef _CRTIMP
+#endif
+
+#endif /* __glut_h__ */
diff --git a/Simbody/Visualizer/simbody-visualizer/glut32/lib/glut32.dll b/Simbody/Visualizer/simbody-visualizer/glut32/lib/glut32.dll
new file mode 100644
index 0000000..d134b7b
Binary files /dev/null and b/Simbody/Visualizer/simbody-visualizer/glut32/lib/glut32.dll differ
diff --git a/Simbody/Visualizer/simbody-visualizer/glut32/lib/glut32.lib b/Simbody/Visualizer/simbody-visualizer/glut32/lib/glut32.lib
new file mode 100644
index 0000000..fa979a1
Binary files /dev/null and b/Simbody/Visualizer/simbody-visualizer/glut32/lib/glut32.lib differ
diff --git a/Simbody/Visualizer/simbody-visualizer/glut32/lib64/glut32.dll b/Simbody/Visualizer/simbody-visualizer/glut32/lib64/glut32.dll
new file mode 100644
index 0000000..cbfca35
Binary files /dev/null and b/Simbody/Visualizer/simbody-visualizer/glut32/lib64/glut32.dll differ
diff --git a/Simbody/Visualizer/simbody-visualizer/glut32/lib64/glut32.lib b/Simbody/Visualizer/simbody-visualizer/glut32/lib64/glut32.lib
new file mode 100644
index 0000000..bbaa7d2
Binary files /dev/null and b/Simbody/Visualizer/simbody-visualizer/glut32/lib64/glut32.lib differ
diff --git a/Simbody/Visualizer/simbody-visualizer/glut32/readme.txt b/Simbody/Visualizer/simbody-visualizer/glut32/readme.txt
new file mode 100644
index 0000000..ff530c5
--- /dev/null
+++ b/Simbody/Visualizer/simbody-visualizer/glut32/readme.txt
@@ -0,0 +1,22 @@
+On Windows we supply our own glut includes and libraries. We expect Windows to
+provide only GL/gl.h and GL/glu.h; we'll provide glext.h and glut.h, as well as glut32.lib & dll binaries in 32 and 64 bits.
+
+Note "glut32" is generically the name of glut libraries on Windows; it does
+not imply 32 bit binaries. Sadly, even the 64 bit binary is named glut32.
+
+The version of glut we're providing was built from glut 3.7.6 source, patched to
+support the mouse wheel using a patch obtained here:
+ http://www.realmtech.net/opengl/glut.php.
+We also fixed a bug that prevented glutTimerFunc() from working correctly.
+
+During a Windows build (32 or 64 bit), we will copy the 32 or 64 bit dll into
+the local binary directory in which the VisualizationGUI.exe executable is
+begin built. That ensures that it will use our glut32.dll regardless of what's
+on the path. When the INSTALL target is built, we'll copy that dll into the
+SimTK_INSTALL_DIR/bin directory along with VisualizationGUI.exe.
+
+For Mac and Linux we expect to find an installed glut or freeglut that we can
+use to build the Simbody Visualizer.
+
+Sherm 11/16/2010
+
diff --git a/Simbody/Visualizer/simbody-visualizer/lodepng.cpp b/Simbody/Visualizer/simbody-visualizer/lodepng.cpp
new file mode 100644
index 0000000..4c2da8c
--- /dev/null
+++ b/Simbody/Visualizer/simbody-visualizer/lodepng.cpp
@@ -0,0 +1,5114 @@
+/*
+LodePNG version 20101030
+
+Copyright (c) 2005-2010 Lode Vandevenne
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any damages
+arising from the use of this software.
+
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it
+freely, subject to the following restrictions:
+
+ 1. The origin of this software must not be misrepresented; you must not
+ claim that you wrote the original software. If you use this software
+ in a product, an acknowledgment in the product documentation would be
+ appreciated but is not required.
+
+ 2. Altered source versions must be plainly marked as such, and must not be
+ misrepresented as being the original software.
+
+ 3. This notice may not be removed or altered from any source
+ distribution.
+*/
+
+/*
+The manual and changelog can be found in the header file "lodepng.h"
+You are free to name this file lodepng.cpp or lodepng.c depending on your usage.
+*/
+
+#ifdef _MSC_VER
+ // Silence annoying warnings about how getenv(), sscanf(), etc. are unsafe.
+ #pragma warning(disable:4996)
+#endif
+
+#include "lodepng.h"
+
+#define VERSION_STRING "20101030"
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* / Tools For C / */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+/*
+About these tools (vector, uivector, ucvector and string):
+-LodePNG was originally written in C++. The vectors replace the std::vectors that were used in the C++ version.
+-The string tools are made to avoid problems with compilers that declare things like strncat as deprecated.
+-They're not used in the interface, only internally in this file, so all their functions are made static.
+*/
+
+#ifdef LODEPNG_COMPILE_ZLIB
+#ifdef LODEPNG_COMPILE_ENCODER
+
+typedef struct vector /*dynamic vector of void* pointers. This one is used only by the deflate compressor*/
+{
+ void* data;
+ size_t size; /*in groups of bytes depending on type*/
+ size_t allocsize; /*in bytes*/
+ unsigned typesize; /*sizeof the type you store in data*/
+} vector;
+
+static unsigned vector_resize(vector* p, size_t size) /*returns 1 if success, 0 if failure ==> nothing done*/
+{
+ if(size * p->typesize > p->allocsize)
+ {
+ size_t newsize = size * p->typesize * 2;
+ void* data = realloc(p->data, newsize);
+ if(data)
+ {
+ p->allocsize = newsize;
+ p->data = data;
+ p->size = size;
+ }
+ else return 0;
+ }
+ else p->size = size;
+ return 1;
+}
+
+static unsigned vector_resized(vector* p, size_t size, void dtor(void*)) /*resize and use destructor on elements if it gets smaller*/
+{
+ size_t i;
+ if(size < p->size)
+ {
+ for(i = size; i < p->size; i++)
+ {
+ dtor(&((char*)(p->data))[i * p->typesize]);
+ }
+ }
+ return vector_resize(p, size);
+}
+
+static void vector_cleanup(void* p)
+{
+ ((vector*)p)->size = ((vector*)p)->allocsize = 0;
+ free(((vector*)p)->data);
+ ((vector*)p)->data = NULL;
+}
+
+static void vector_cleanupd(vector* p, void dtor(void*)) /*clear and use destructor on elements*/
+{
+ vector_resized(p, 0, dtor);
+ vector_cleanup(p);
+}
+
+static void vector_init(vector* p, unsigned typesize)
+{
+ p->data = NULL;
+ p->size = p->allocsize = 0;
+ p->typesize = typesize;
+}
+
+static void vector_swap(vector* p, vector* q) /*they're supposed to have the same typesize*/
+{
+ size_t tmp;
+ void* tmpp;
+ tmp = p->size; p->size = q->size; q->size = tmp;
+ tmp = p->allocsize; p->allocsize = q->allocsize; q->allocsize = tmp;
+ tmpp = p->data; p->data = q->data; q->data = tmpp;
+}
+
+static void* vector_get(vector* p, size_t index)
+{
+ return &((char*)p->data)[index * p->typesize];
+}
+#endif /*LODEPNG_COMPILE_ENCODER*/
+#endif /*LODEPNG_COMPILE_ZLIB*/
+
+/* /////////////////////////////////////////////////////////////////////////// */
+
+#ifdef LODEPNG_COMPILE_ZLIB
+typedef struct uivector /*dynamic vector of unsigned ints*/
+{
+ unsigned* data;
+ size_t size; /*size in number of unsigned longs*/
+ size_t allocsize; /*allocated size in bytes*/
+} uivector;
+
+static void uivector_cleanup(void* p)
+{
+ ((uivector*)p)->size = ((uivector*)p)->allocsize = 0;
+ free(((uivector*)p)->data);
+ ((uivector*)p)->data = NULL;
+}
+
+static unsigned uivector_resize(uivector* p, size_t size) /*returns 1 if success, 0 if failure ==> nothing done*/
+{
+ if(size * sizeof(unsigned) > p->allocsize)
+ {
+ size_t newsize = size * sizeof(unsigned) * 2;
+ void* data = realloc(p->data, newsize);
+ if(data)
+ {
+ p->allocsize = newsize;
+ p->data = (unsigned*)data;
+ p->size = size;
+ }
+ else return 0;
+ }
+ else p->size = size;
+ return 1;
+}
+
+static unsigned uivector_resizev(uivector* p, size_t size, unsigned value) /*resize and give all new elements the value*/
+{
+ size_t oldsize = p->size, i;
+ if(!uivector_resize(p, size)) return 0;
+ for(i = oldsize; i < size; i++) p->data[i] = value;
+ return 1;
+}
+
+static void uivector_init(uivector* p)
+{
+ p->data = NULL;
+ p->size = p->allocsize = 0;
+}
+
+#ifdef LODEPNG_COMPILE_ENCODER
+static unsigned uivector_push_back(uivector* p, unsigned c) /*returns 1 if success, 0 if failure ==> nothing done*/
+{
+ if(!uivector_resize(p, p->size + 1)) return 0;
+ p->data[p->size - 1] = c;
+ return 1;
+}
+
+static unsigned uivector_copy(uivector* p, const uivector* q) /*copy q to p, returns 1 if success, 0 if failure ==> nothing done*/
+{
+ size_t i;
+ if(!uivector_resize(p, q->size)) return 0;
+ for(i = 0; i < q->size; i++) p->data[i] = q->data[i];
+ return 1;
+}
+
+static void uivector_swap(uivector* p, uivector* q)
+{
+ size_t tmp;
+ unsigned* tmpp;
+ tmp = p->size; p->size = q->size; q->size = tmp;
+ tmp = p->allocsize; p->allocsize = q->allocsize; q->allocsize = tmp;
+ tmpp = p->data; p->data = q->data; q->data = tmpp;
+}
+#endif /*LODEPNG_COMPILE_ENCODER*/
+#endif /*LODEPNG_COMPILE_ZLIB*/
+
+/* /////////////////////////////////////////////////////////////////////////// */
+
+typedef struct ucvector /*dynamic vector of unsigned chars*/
+{
+ unsigned char* data;
+ size_t size; /*used size*/
+ size_t allocsize; /*allocated size*/
+} ucvector;
+
+static void ucvector_cleanup(void* p)
+{
+ ((ucvector*)p)->size = ((ucvector*)p)->allocsize = 0;
+ free(((ucvector*)p)->data);
+ ((ucvector*)p)->data = NULL;
+}
+
+static unsigned ucvector_resize(ucvector* p, size_t size) /*returns 1 if success, 0 if failure ==> nothing done*/
+{
+ if(size * sizeof(unsigned char) > p->allocsize)
+ {
+ size_t newsize = size * sizeof(unsigned char) * 2;
+ void* data = realloc(p->data, newsize);
+ if(data)
+ {
+ p->allocsize = newsize;
+ p->data = (unsigned char*)data;
+ p->size = size;
+ }
+ else return 0; /*error: not enough memory*/
+ }
+ else p->size = size;
+ return 1;
+}
+
+#ifdef LODEPNG_COMPILE_DECODER
+#ifdef LODEPNG_COMPILE_PNG
+static unsigned ucvector_resizev(ucvector* p, size_t size, unsigned char value) /*resize and give all new elements the value*/
+{
+ size_t oldsize = p->size, i;
+ if(!ucvector_resize(p, size)) return 0;
+ for(i = oldsize; i < size; i++) p->data[i] = value;
+ return 1;
+}
+#endif /*LODEPNG_COMPILE_PNG*/
+#endif /*LODEPNG_COMPILE_DECODER*/
+
+static void ucvector_init(ucvector* p)
+{
+ p->data = NULL;
+ p->size = p->allocsize = 0;
+}
+
+#ifdef LODEPNG_COMPILE_ZLIB
+/*you can both convert from vector to buffer&size and vica versa*/
+static void ucvector_init_buffer(ucvector* p, unsigned char* buffer, size_t size)
+{
+ p->data = buffer;
+ p->allocsize = p->size = size;
+}
+#endif /*LODEPNG_COMPILE_ZLIB*/
+
+static unsigned ucvector_push_back(ucvector* p, unsigned char c) /*returns 1 if success, 0 if failure ==> nothing done*/
+{
+ if(!ucvector_resize(p, p->size + 1)) return 0;
+ p->data[p->size - 1] = c;
+ return 1;
+}
+
+/* /////////////////////////////////////////////////////////////////////////// */
+
+#ifdef LODEPNG_COMPILE_PNG
+#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS
+static unsigned string_resize(char** out, size_t size) /*returns 1 if success, 0 if failure ==> nothing done*/
+{
+ char* data = (char*)realloc(*out, size + 1);
+ if(data)
+ {
+ data[size] = 0; /*null termination char*/
+ *out = data;
+ }
+ return data != 0;
+}
+
+static void string_init(char** out) /*init a {char*, size_t} pair for use as string*/
+{
+ *out = NULL;
+ string_resize(out, 0);
+}
+
+static void string_cleanup(char** out) /*free the above pair again*/
+{
+ free(*out);
+ *out = NULL;
+}
+
+static void string_set(char** out, const char* in)
+{
+ size_t insize = strlen(in), i = 0;
+ if(string_resize(out, insize))
+ {
+ for(i = 0; i < insize; i++)
+ {
+ (*out)[i] = in[i];
+ }
+ }
+}
+#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/
+#endif /*LODEPNG_COMPILE_PNG*/
+
+#ifdef LODEPNG_COMPILE_ZLIB
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* / Reading and writing single bits and bytes from/to stream for Deflate / */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+#ifdef LODEPNG_COMPILE_ENCODER
+static void addBitToStream(size_t* bitpointer, ucvector* bitstream, unsigned char bit)
+{
+ if((*bitpointer) % 8 == 0) ucvector_push_back(bitstream, (unsigned char)0); /*add a new byte at the end*/
+ (bitstream->data[bitstream->size - 1]) |= (bit << ((*bitpointer) & 0x7)); /*earlier bit of huffman code is in a lesser significant bit of an earlier byte*/
+ (*bitpointer)++;
+}
+
+static void addBitsToStream(size_t* bitpointer, ucvector* bitstream, unsigned value, size_t nbits)
+{
+ size_t i;
+ for(i = 0; i < nbits; i++) addBitToStream(bitpointer, bitstream, (unsigned char)((value >> i) & 1));
+}
+
+static void addBitsToStreamReversed(size_t* bitpointer, ucvector* bitstream, unsigned value, size_t nbits)
+{
+ size_t i;
+ for(i = 0; i < nbits; i++) addBitToStream(bitpointer, bitstream, (unsigned char)((value >> (nbits - 1 - i)) & 1));
+}
+#endif /*LODEPNG_COMPILE_ENCODER*/
+
+#ifdef LODEPNG_COMPILE_DECODER
+
+#define READBIT(bitpointer, bitstream) ((bitstream[bitpointer >> 3] >> (bitpointer & 0x7)) & (unsigned char)1)
+
+static unsigned char readBitFromStream(size_t* bitpointer, const unsigned char* bitstream)
+{
+ unsigned char result = (unsigned char)(READBIT(*bitpointer, bitstream));
+ (*bitpointer)++;
+ return result;
+}
+
+static unsigned readBitsFromStream(size_t* bitpointer, const unsigned char* bitstream, size_t nbits)
+{
+ unsigned result = 0, i;
+ for(i = 0; i < nbits; i++)
+ {
+ result += ((unsigned)READBIT(*bitpointer, bitstream)) << i;
+ (*bitpointer)++;
+ }
+ return result;
+}
+#endif /*LODEPNG_COMPILE_DECODER*/
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* / Deflate - Huffman / */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+#define FIRST_LENGTH_CODE_INDEX 257
+#define LAST_LENGTH_CODE_INDEX 285
+#define NUM_DEFLATE_CODE_SYMBOLS 288 /*256 literals, the end code, some length codes, and 2 unused codes*/
+#define NUM_DISTANCE_SYMBOLS 32 /*the distance codes have their own symbols, 30 used, 2 unused*/
+#define NUM_CODE_LENGTH_CODES 19 /*the code length codes. 0-15: code lengths, 16: copy previous 3-6 times, 17: 3-10 zeros, 18: 11-138 zeros*/
+
+static const unsigned LENGTHBASE[29] /*the base lengths represented by codes 257-285*/
+ = {3, 4, 5, 6, 7, 8, 9, 10, 11, 13, 15, 17, 19, 23, 27, 31, 35, 43, 51, 59, 67, 83, 99, 115, 131, 163, 195, 227, 258};
+static const unsigned LENGTHEXTRA[29] /*the extra bits used by codes 257-285 (added to base length)*/
+ = {0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3, 4, 4, 4, 4, 5, 5, 5, 5, 0};
+static const unsigned DISTANCEBASE[30] /*the base backwards distances (the bits of distance codes appear after length codes and use their own huffman tree)*/
+ = {1, 2, 3, 4, 5, 7, 9, 13, 17, 25, 33, 49, 65, 97, 129, 193, 257, 385, 513, 769, 1025, 1537, 2049, 3073, 4097, 6145, 8193, 12289, 16385, 24577};
+static const unsigned DISTANCEEXTRA[30] /*the extra bits of backwards distances (added to base)*/
+ = {0, 0, 0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8, 9, 9, 10, 10, 11, 11, 12, 12, 13, 13};
+static const unsigned CLCL[NUM_CODE_LENGTH_CODES] /*the order in which "code length alphabet code lengths" are stored, out of this the huffman tree of the dynamic huffman tree lengths is generated*/
+ = {16, 17, 18, 0, 8, 7, 9, 6, 10, 5, 11, 4, 12, 3, 13, 2, 14, 1, 15};
+
+/* /////////////////////////////////////////////////////////////////////////// */
+
+#ifdef LODEPNG_COMPILE_ENCODER
+/*
+A coin, this is the terminology used for the package-merge algorithm and the
+coin collector's problem. This is used to generate the huffman tree.
+A coin can be multiple coins (when they're merged)
+*/
+typedef struct Coin
+{
+ uivector symbols;
+ float weight; /*the sum of all weights in this coin*/
+} Coin;
+
+static void Coin_init(Coin* c)
+{
+ uivector_init(&c->symbols);
+}
+
+static void Coin_cleanup(void* c) /*void* so that this dtor can be given as function pointer to the vector resize function*/
+{
+ uivector_cleanup(&((Coin*)c)->symbols);
+}
+
+static void Coin_copy(Coin* c1, const Coin* c2)
+{
+ c1->weight = c2->weight;
+ uivector_copy(&c1->symbols, &c2->symbols);
+}
+
+static void addCoins(Coin* c1, const Coin* c2)
+{
+ size_t i;
+ for(i = 0; i < c2->symbols.size; i++) uivector_push_back(&c1->symbols, c2->symbols.data[i]);
+ c1->weight += c2->weight;
+}
+
+/*
+Coin_sort: This uses a simple combsort to sort the data. This function is not critical for
+overall encoding speed and the data amount isn't that large.
+*/
+static void Coin_sort(Coin* data, size_t amount)
+{
+ size_t gap = amount;
+ unsigned char swapped = 0;
+ while((gap > 1) || swapped)
+ {
+ size_t i;
+ gap = (gap * 10) / 13; /*shrink factor 1.3*/
+ if(gap == 9 || gap == 10) gap = 11; /*combsort11*/
+ if(gap < 1) gap = 1;
+ swapped = 0;
+ for(i = 0; i < amount - gap; i++)
+ {
+ size_t j = i + gap;
+ if(data[j].weight < data[i].weight)
+ {
+ float temp = data[j].weight; data[j].weight = data[i].weight; data[i].weight = temp;
+ uivector_swap(&data[i].symbols, &data[j].symbols);
+ swapped = 1;
+ }
+ }
+ }
+}
+#endif /*LODEPNG_COMPILE_ENCODER*/
+
+/*
+Huffman tree struct, containing multiple representations of the tree
+*/
+typedef struct HuffmanTree
+{
+ uivector tree2d;
+ uivector tree1d;
+ uivector lengths; /*the lengths of the codes of the 1d-tree*/
+ unsigned maxbitlen; /*maximum number of bits a single code can get*/
+ unsigned numcodes; /*number of symbols in the alphabet = number of codes*/
+} HuffmanTree;
+
+/*function used for debug purposes*/
+/*#include <iostream>
+static void HuffmanTree_draw(HuffmanTree* tree)
+{
+ std::cout << "tree. length: " << tree->numcodes << " maxbitlen: " << tree->maxbitlen << std::endl;
+ for(size_t i = 0; i < tree->tree1d.size; i++)
+ {
+ if(tree->lengths.data[i])
+ std::cout << i << " " << tree->tree1d.data[i] << " " << tree->lengths.data[i] << std::endl;
+ }
+ std::cout << std::endl;
+}*/
+
+static void HuffmanTree_init(HuffmanTree* tree)
+{
+ uivector_init(&tree->tree2d);
+ uivector_init(&tree->tree1d);
+ uivector_init(&tree->lengths);
+}
+
+static void HuffmanTree_cleanup(HuffmanTree* tree)
+{
+ uivector_cleanup(&tree->tree2d);
+ uivector_cleanup(&tree->tree1d);
+ uivector_cleanup(&tree->lengths);
+}
+
+/*the tree representation used by the decoder. return value is error*/
+static unsigned HuffmanTree_make2DTree(HuffmanTree* tree)
+{
+ unsigned nodefilled = 0; /*up to which node it is filled*/
+ unsigned treepos = 0; /*position in the tree (1 of the numcodes columns)*/
+ unsigned n, i;
+
+ if(!uivector_resize(&tree->tree2d, tree->numcodes * 2)) return 9901;
+ /*convert tree1d[] to tree2d[][]. In the 2D array, a value of 32767 means uninited, a value >= numcodes is an address to another bit, a value < numcodes is a code. The 2 rows are the 2 possible bit values (0 or 1), there are as many columns as codes - 1
+ a good huffmann tree has N * 2 - 1 nodes, of which N - 1 are internal nodes. Here, the internal nodes are stored (what their 0 and 1 option point to). There is only memory for such good tree currently, if there are more nodes (due to too long length codes), error 55 will happen*/
+ for(n = 0; n < tree->numcodes * 2; n++) tree->tree2d.data[n] = 32767; /*32767 here means the tree2d isn't filled there yet*/
+
+ for(n = 0; n < tree->numcodes; n++) /*the codes*/
+ {
+ for(i = 0; i < tree->lengths.data[n]; i++) /*the bits for this code*/
+ {
+ unsigned char bit = (unsigned char)((tree->tree1d.data[n] >> (tree->lengths.data[n] - i - 1)) & 1);
+ if(treepos > tree->numcodes - 2) return 55; /*error 55: oversubscribed; see description in header*/
+ if(tree->tree2d.data[2 * treepos + bit] == 32767) /*not yet filled in*/
+ {
+ if(i + 1 == tree->lengths.data[n]) /*last bit*/
+ {
+ tree->tree2d.data[2 * treepos + bit] = n; /*put the current code in it*/
+ treepos = 0;
+ }
+ else /*put address of the next step in here, first that address has to be found of course (it's just nodefilled + 1)...*/
+ {
+ nodefilled++;
+ tree->tree2d.data[2 * treepos + bit] = nodefilled + tree->numcodes; /*addresses encoded with numcodes added to it*/
+ treepos = nodefilled;
+ }
+ }
+ else treepos = tree->tree2d.data[2 * treepos + bit] - tree->numcodes;
+ }
+ }
+
+ for(n = 0; n < tree->numcodes * 2; n++)
+ {
+ if(tree->tree2d.data[n] == 32767) tree->tree2d.data[n] = 0; /*remove possible remaining 32767's*/
+ }
+
+ return 0;
+}
+
+/*
+HuffmanTree_makeFromLengths2
+numcodes, lengths and maxbitlen must already be filled in correctly.
+return value is error.
+*/
+static unsigned HuffmanTree_makeFromLengths2(HuffmanTree* tree)
+{
+ uivector blcount;
+ uivector nextcode;
+ unsigned bits, n, error = 0;
+
+ uivector_init(&blcount);
+ uivector_init(&nextcode);
+ if(!uivector_resize(&tree->tree1d, tree->numcodes)
+ || !uivector_resizev(&blcount, tree->maxbitlen + 1, 0)
+ || !uivector_resizev(&nextcode, tree->maxbitlen + 1, 0))
+ error = 9902; /*memory allocation failed*/
+
+ if(!error)
+ {
+ /*step 1: count number of instances of each code length*/
+ for(bits = 0; bits < tree->numcodes; bits++) blcount.data[tree->lengths.data[bits]]++;
+ /*step 2: generate the nextcode values*/
+ for(bits = 1; bits <= tree->maxbitlen; bits++) nextcode.data[bits] = (nextcode.data[bits - 1] + blcount.data[bits - 1]) << 1;
+ /*step 3: generate all the codes*/
+ for(n = 0; n < tree->numcodes; n++) if(tree->lengths.data[n] != 0) tree->tree1d.data[n] = nextcode.data[tree->lengths.data[n]]++;
+ }
+
+ uivector_cleanup(&blcount);
+ uivector_cleanup(&nextcode);
+
+ if(!error) return HuffmanTree_make2DTree(tree);
+ else return error;
+}
+
+/*
+HuffmanTree_makeFromLengths
+given the code lengths (as stored in the PNG file), generate the tree as defined
+by Deflate. maxbitlen is the maximum bits that a code in the tree can have.
+return value is error.
+*/
+static unsigned HuffmanTree_makeFromLengths(HuffmanTree* tree, const unsigned* bitlen, size_t numcodes, unsigned maxbitlen)
+{
+ unsigned i;
+ if(!uivector_resize(&tree->lengths, numcodes)) return 9903;
+ for(i = 0; i < numcodes; i++) tree->lengths.data[i] = bitlen[i];
+ tree->numcodes = (unsigned)numcodes; /*number of symbols*/
+ tree->maxbitlen = maxbitlen;
+ return HuffmanTree_makeFromLengths2(tree);
+}
+
+#ifdef LODEPNG_COMPILE_ENCODER
+static unsigned HuffmanTree_fillInCoins(vector* coins, const unsigned* frequencies, unsigned numcodes, size_t sum)
+{
+ unsigned i;
+ for(i = 0; i < numcodes; i++)
+ {
+ Coin* coin;
+ if(frequencies[i] == 0) continue; /*it's important to exclude symbols that aren't present*/
+ if(!vector_resize(coins, coins->size + 1))
+ {
+ vector_cleanup(coins);
+ return 9904; /*memory allocation failed*/
+ }
+ coin = (Coin*)(vector_get(coins, coins->size - 1));
+ Coin_init(coin);
+ coin->weight = frequencies[i] / (float)sum;
+ uivector_push_back(&coin->symbols, i);
+ }
+ if(coins->size) Coin_sort((Coin*)coins->data, coins->size);
+ return 0;
+}
+
+/*
+HuffmanTree_makeFromFrequencies
+Create the Huffman tree given the symbol frequencies
+*/
+static unsigned HuffmanTree_makeFromFrequencies(HuffmanTree* tree, const unsigned* frequencies, size_t numcodes, unsigned maxbitlen)
+{
+ unsigned i, j;
+ size_t sum = 0, numpresent = 0;
+ unsigned error = 0;
+
+ vector prev_row; /*type Coin, the previous row of coins*/
+ vector coins; /*type Coin, the coins of the currently calculated row*/
+
+ tree->maxbitlen = maxbitlen;
+
+ for(i = 0; i < numcodes; i++)
+ {
+ if(frequencies[i] > 0)
+ {
+ numpresent++;
+ sum += frequencies[i];
+ }
+ }
+
+ if(numcodes == 0) return 80; /*error: a tree of 0 symbols is not supposed to be made*/
+ tree->numcodes = (unsigned)numcodes; /*number of symbols*/
+ uivector_resize(&tree->lengths, 0);
+ if(!uivector_resizev(&tree->lengths, tree->numcodes, 0)) return 9905;
+
+ if(numpresent == 0) /*there are no symbols at all, in that case add one symbol of value 0 to the tree (see RFC 1951 section 3.2.7) */
+ {
+ tree->lengths.data[0] = 1;
+ return HuffmanTree_makeFromLengths2(tree);
+ }
+ else if(numpresent == 1) /*the package merge algorithm gives wrong results if there's only one symbol (theoretically 0 bits would then suffice, but we need a proper symbol for zlib)*/
+ {
+ for(i = 0; i < numcodes; i++) if(frequencies[i]) tree->lengths.data[i] = 1;
+ return HuffmanTree_makeFromLengths2(tree);
+ }
+
+ vector_init(&coins, sizeof(Coin));
+ vector_init(&prev_row, sizeof(Coin));
+
+ /*Package-Merge algorithm represented by coin collector's problem
+ For every symbol, maxbitlen coins will be created*/
+
+ /*first row, lowest denominator*/
+ error = HuffmanTree_fillInCoins(&coins, frequencies, tree->numcodes, sum);
+ if(!error)
+ {
+ for(j = 1; j <= maxbitlen && !error; j++) /*each of the remaining rows*/
+ {
+ vector_swap(&coins, &prev_row); /*swap instead of copying*/
+ if(!vector_resized(&coins, 0, Coin_cleanup))
+ {
+ error = 9906; /*memory allocation failed*/
+ break;
+ }
+
+ for(i = 0; i + 1 < prev_row.size; i += 2)
+ {
+ if(!vector_resize(&coins, coins.size + 1))
+ {
+ error = 9907; /*memory allocation failed*/
+ break;
+ }
+ Coin_init((Coin*)vector_get(&coins, coins.size - 1));
+ Coin_copy((Coin*)vector_get(&coins, coins.size - 1), (Coin*)vector_get(&prev_row, i));
+ addCoins((Coin*)vector_get(&coins, coins.size - 1), (Coin*)vector_get(&prev_row, i + 1)); /*merge the coins into packages*/
+ }
+ if(j < maxbitlen)
+ {
+ error = HuffmanTree_fillInCoins(&coins, frequencies, tree->numcodes, sum);
+ }
+ }
+ }
+
+ if(!error)
+ {
+ /*keep the coins with lowest weight, so that they add up to the amount of symbols - 1*/
+ vector_resized(&coins, numpresent - 1, Coin_cleanup);
+
+ /*calculate the lenghts of each symbol, as the amount of times a coin of each symbol is used*/
+ for(i = 0; i < coins.size; i++)
+ {
+ Coin* coin = (Coin*)vector_get(&coins, i);
+ for(j = 0; j < coin->symbols.size; j++) tree->lengths.data[coin->symbols.data[j]]++;
+ }
+
+ error = HuffmanTree_makeFromLengths2(tree);
+ }
+
+ vector_cleanupd(&coins, Coin_cleanup);
+ vector_cleanupd(&prev_row, Coin_cleanup);
+
+ return error;
+}
+
+static unsigned HuffmanTree_getCode(const HuffmanTree* tree, unsigned index)
+{
+ return tree->tree1d.data[index];
+}
+
+static unsigned HuffmanTree_getLength(const HuffmanTree* tree, unsigned index)
+{
+ return tree->lengths.data[index];
+}
+#endif /*LODEPNG_COMPILE_ENCODER*/
+
+/*get the tree of a deflated block with fixed tree, as specified in the deflate specification*/
+static unsigned generateFixedTree(HuffmanTree* tree)
+{
+ unsigned i, error = 0;
+ uivector bitlen;
+ uivector_init(&bitlen);
+ if(!uivector_resize(&bitlen, NUM_DEFLATE_CODE_SYMBOLS)) error = 9909;
+
+ if(!error)
+ {
+ /*288 possible codes: 0-255=literals, 256=endcode, 257-285=lengthcodes, 286-287=unused*/
+ for(i = 0; i <= 143; i++) bitlen.data[i] = 8;
+ for(i = 144; i <= 255; i++) bitlen.data[i] = 9;
+ for(i = 256; i <= 279; i++) bitlen.data[i] = 7;
+ for(i = 280; i <= 287; i++) bitlen.data[i] = 8;
+
+ error = HuffmanTree_makeFromLengths(tree, bitlen.data, NUM_DEFLATE_CODE_SYMBOLS, 15);
+ }
+
+ uivector_cleanup(&bitlen);
+ return error;
+}
+
+static unsigned generateDistanceTree(HuffmanTree* tree)
+{
+ unsigned i, error = 0;
+ uivector bitlen;
+ uivector_init(&bitlen);
+ if(!uivector_resize(&bitlen, NUM_DISTANCE_SYMBOLS)) error = 9910;
+
+ /*there are 32 distance codes, but 30-31 are unused*/
+ if(!error)
+ {
+ for(i = 0; i < NUM_DISTANCE_SYMBOLS; i++) bitlen.data[i] = 5;
+ error = HuffmanTree_makeFromLengths(tree, bitlen.data, NUM_DISTANCE_SYMBOLS, 15);
+ }
+ uivector_cleanup(&bitlen);
+ return error;
+}
+
+#ifdef LODEPNG_COMPILE_DECODER
+
+/*
+returns the code, or (unsigned)(-1) if error happened
+inbitlength is the length of the complete buffer, in bits (so its byte length times 8)
+*/
+static unsigned huffmanDecodeSymbol(const unsigned char* in, size_t* bp,
+ const HuffmanTree* codetree, size_t inbitlength)
+{
+ unsigned treepos = 0, ct;
+ for(;;)
+ {
+ if(*bp > inbitlength) return (unsigned)(-1); /*error: end of input memory reached without endcode*/
+
+ /*
+ decode the symbol from the tree
+ the "readBitFromStream" code is inlined in the expression below because this is the biggest bottleneck while decoding
+ */
+ ct = codetree->tree2d.data[(treepos << 1) + READBIT(*bp, in)];
+ (*bp)++;
+ if(ct < codetree->numcodes) return ct; /*the symbol is decoded, return it*/
+ else treepos = ct - codetree->numcodes; /*symbol not yet decoded, instead move tree position*/
+
+ if(treepos >= codetree->numcodes) return (unsigned)(-1); /*error: it appeared outside the codetree*/
+ }
+}
+#endif /*LODEPNG_COMPILE_DECODER*/
+
+#ifdef LODEPNG_COMPILE_DECODER
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* / Inflator / */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+/*get the tree of a deflated block with fixed tree, as specified in the deflate specification*/
+static void getTreeInflateFixed(HuffmanTree* tree, HuffmanTree* treeD)
+{
+ /*error checking not done, this is fixed stuff, it works, it doesn't depend on the image*/
+ generateFixedTree(tree);
+ generateDistanceTree(treeD);
+}
+
+/*get the tree of a deflated block with dynamic tree, the tree itself is also Huffman compressed with a known tree*/
+static unsigned getTreeInflateDynamic(HuffmanTree* codetree, HuffmanTree* codetreeD, HuffmanTree* codelengthcodetree,
+ const unsigned char* in, size_t* bp, size_t inlength)
+{
+ /*make sure that length values that aren't filled in will be 0, or a wrong tree will be generated*/
+ /*C-code note: use no "return" between ctor and dtor of an uivector!*/
+ unsigned error = 0;
+ unsigned n, HLIT, HDIST, HCLEN, i;
+ uivector bitlen;
+ uivector bitlenD;
+ uivector codelengthcode;
+ size_t inbitlength = inlength * 8;
+
+ if((*bp) >> 3 >= inlength - 2) return 49; /*the bit pointer is or will go past the memory*/
+
+ HLIT = readBitsFromStream(bp, in, 5) + 257; /*number of literal/length codes + 257. Unlike the spec, the value 257 is added to it here already*/
+ HDIST = readBitsFromStream(bp, in, 5) + 1; /*number of distance codes. Unlike the spec, the value 1 is added to it here already*/
+ HCLEN = readBitsFromStream(bp, in, 4) + 4; /*number of code length codes. Unlike the spec, the value 4 is added to it here already*/
+
+ /*read the code length codes out of 3 * (amount of code length codes) bits*/
+ uivector_init(&codelengthcode);
+ if(!uivector_resize(&codelengthcode, NUM_CODE_LENGTH_CODES)) error = 9911;
+
+ if(!error)
+ {
+ for(i = 0; i < NUM_CODE_LENGTH_CODES; i++)
+ {
+ if(i < HCLEN) codelengthcode.data[CLCL[i]] = readBitsFromStream(bp, in, 3);
+ else codelengthcode.data[CLCL[i]] = 0; /*if not, it must stay 0*/
+ }
+
+ error = HuffmanTree_makeFromLengths(codelengthcodetree, codelengthcode.data, codelengthcode.size, 7);
+ }
+
+ uivector_cleanup(&codelengthcode);
+ if(error) return error;
+
+ /*now we can use this tree to read the lengths for the tree that this function will return*/
+ uivector_init(&bitlen);
+ uivector_resizev(&bitlen, NUM_DEFLATE_CODE_SYMBOLS, 0);
+ uivector_init(&bitlenD);
+ uivector_resizev(&bitlenD, NUM_DISTANCE_SYMBOLS, 0);
+ i = 0;
+ if(!bitlen.data || !bitlenD.data)
+ {
+ error = 9912; /*memory allocation failed*/
+ }
+ else while(i < HLIT + HDIST) /*i is the current symbol we're reading in the part that contains the code lengths of lit/len codes and dist codes*/
+ {
+ unsigned code = huffmanDecodeSymbol(in, bp, codelengthcodetree, inbitlength);
+ if(code <= 15) /*a length code*/
+ {
+ if(i < HLIT) bitlen.data[i] = code;
+ else bitlenD.data[i - HLIT] = code;
+ i++;
+ }
+ else if(code == 16) /*repeat previous*/
+ {
+ unsigned replength = 3; /*read in the 2 bits that indicate repeat length (3-6)*/
+ unsigned value; /*set value to the previous code*/
+
+ if((*bp) >> 3 >= inlength)
+ {
+ error = 50; /*error, bit pointer jumps past memory*/
+ break;
+ }
+
+ replength += readBitsFromStream(bp, in, 2);
+
+ if((i - 1) < HLIT) value = bitlen.data[i - 1];
+ else value = bitlenD.data[i - HLIT - 1];
+ /*repeat this value in the next lengths*/
+ for(n = 0; n < replength; n++)
+ {
+ if(i >= HLIT + HDIST)
+ {
+ error = 13; /*error: i is larger than the amount of codes*/
+ break;
+ }
+ if(i < HLIT) bitlen.data[i] = value;
+ else bitlenD.data[i - HLIT] = value;
+ i++;
+ }
+ }
+ else if(code == 17) /*repeat "0" 3-10 times*/
+ {
+ unsigned replength = 3; /*read in the bits that indicate repeat length*/
+ if((*bp) >> 3 >= inlength)
+ {
+ error = 50; /*error, bit pointer jumps past memory*/
+ break;
+ }
+
+ replength += readBitsFromStream(bp, in, 3);
+
+ /*repeat this value in the next lengths*/
+ for(n = 0; n < replength; n++)
+ {
+ if(i >= HLIT + HDIST)
+ {
+ error = 14; /*error: i is larger than the amount of codes*/
+ break;
+ }
+ if(i < HLIT) bitlen.data[i] = 0;
+ else bitlenD.data[i - HLIT] = 0;
+ i++;
+ }
+ }
+ else if(code == 18) /*repeat "0" 11-138 times*/
+ {
+ unsigned replength = 11; /*read in the bits that indicate repeat length*/
+ if((*bp) >> 3 >= inlength)
+ {
+ error = 50; /*error, bit pointer jumps past memory*/
+ break;
+ }
+ replength += readBitsFromStream(bp, in, 7);
+
+ /*repeat this value in the next lengths*/
+ for(n = 0; n < replength; n++)
+ {
+ if(i >= HLIT + HDIST)
+ {
+ error = 15; /*error: i is larger than the amount of codes*/
+ break;
+ }
+ if(i < HLIT) bitlen.data[i] = 0;
+ else bitlenD.data[i - HLIT] = 0;
+ i++;
+ }
+ }
+ else /*if(code == (unsigned)(-1))*/ /*huffmanDecodeSymbol returns (unsigned)(-1) in case of error*/
+ {
+ if(code == (unsigned)(-1))
+ {
+ error = (*bp) > inlength * 8 ? 10 : 11; /*return error code 10 or 11 depending on the situation that happened in huffmanDecodeSymbol (10=no endcode, 11=wrong jump outside of tree)*/
+ }
+ else error = 16; /*unexisting code, this can never happen*/
+ break;
+ }
+ }
+
+ if(!error && bitlen.data[256] == 0) error = 64; /*the length of the end code 256 must be larger than 0*/
+
+ /*now we've finally got HLIT and HDIST, so generate the code trees, and the function is done*/
+ if(!error) error = HuffmanTree_makeFromLengths(codetree, &bitlen.data[0], bitlen.size, 15);
+ if(!error) error = HuffmanTree_makeFromLengths(codetreeD, &bitlenD.data[0], bitlenD.size, 15);
+
+ uivector_cleanup(&bitlen);
+ uivector_cleanup(&bitlenD);
+
+ return error;
+}
+
+/*inflate a block with dynamic of fixed Huffman tree*/
+static unsigned inflateHuffmanBlock(ucvector* out, const unsigned char* in, size_t* bp, size_t* pos, size_t inlength, unsigned btype)
+{
+ unsigned error = 0;
+ HuffmanTree codetree; /*287, the code tree for Huffman codes*/
+ HuffmanTree codetreeD; /*31, the code tree for distance codes*/
+ size_t inbitlength = inlength * 8;
+
+ HuffmanTree_init(&codetree);
+ HuffmanTree_init(&codetreeD);
+
+ if(btype == 1) getTreeInflateFixed(&codetree, &codetreeD);
+ else if(btype == 2)
+ {
+ HuffmanTree codelengthcodetree; /*18, the code tree for code length codes*/
+ HuffmanTree_init(&codelengthcodetree);
+ error = getTreeInflateDynamic(&codetree, &codetreeD, &codelengthcodetree, in, bp, inlength);
+ HuffmanTree_cleanup(&codelengthcodetree);
+ }
+
+ for(;;)
+ {
+ unsigned code = huffmanDecodeSymbol(in, bp, &codetree, inbitlength);
+ if(code <= 255) /*literal symbol*/
+ {
+ if((*pos) >= out->size)
+ {
+ if(!ucvector_resize(out, ((*pos) + 1) * 2)) /*reserve more room at once*/
+ {
+ error = 9913; /*memory allocation failed*/
+ break;
+ }
+ }
+ out->data[(*pos)] = (unsigned char)(code);
+ (*pos)++;
+ }
+ else if(code >= FIRST_LENGTH_CODE_INDEX && code <= LAST_LENGTH_CODE_INDEX) /*length code*/
+ {
+ /*part 1: get length base*/
+ size_t length = LENGTHBASE[code - FIRST_LENGTH_CODE_INDEX];
+ unsigned codeD, distance, numextrabitsD;
+ size_t start, forward, backward, numextrabits;
+
+ /*part 2: get extra bits and add the value of that to length*/
+ numextrabits = LENGTHEXTRA[code - FIRST_LENGTH_CODE_INDEX];
+ if(((*bp) >> 3) >= inlength)
+ {
+ error = 51; /*error, bit pointer will jump past memory*/
+ break;
+ }
+ length += readBitsFromStream(bp, in, numextrabits);
+
+ /*part 3: get distance code*/
+ codeD = huffmanDecodeSymbol(in, bp, &codetreeD, inbitlength);
+ if(codeD > 29)
+ {
+ if(code == (unsigned)(-1)) /*huffmanDecodeSymbol returns (unsigned)(-1) in case of error*/
+ {
+ error = (*bp) > inlength * 8 ? 10 : 11; /*return error code 10 or 11 depending on the situation that happened in huffmanDecodeSymbol (10=no endcode, 11=wrong jump outside of tree)*/
+ }
+ else error = 18; /*error: invalid distance code (30-31 are never used)*/
+ break;
+ }
+ distance = DISTANCEBASE[codeD];
+
+ /*part 4: get extra bits from distance*/
+ numextrabitsD = DISTANCEEXTRA[codeD];
+ if(((*bp) >> 3) >= inlength)
+ {
+ error = 51; /*error, bit pointer will jump past memory*/
+ break;
+ }
+ distance += readBitsFromStream(bp, in, numextrabitsD);
+
+ /*part 5: fill in all the out[n] values based on the length and dist*/
+ start = (*pos);
+ backward = start - distance;
+ if((*pos) + length >= out->size)
+ {
+ if(!ucvector_resize(out, ((*pos) + length) * 2)) /*reserve more room at once*/
+ {
+ error = 9914; /*memory allocation failed*/
+ break;
+ }
+ }
+
+ for(forward = 0; forward < length; forward++)
+ {
+ out->data[(*pos)] = out->data[backward];
+ (*pos)++;
+ backward++;
+ if(backward >= start) backward = start - distance;
+ }
+ }
+ else if(code == 256) break; /*end code, break the loop*/
+ else /*if(code == (unsigned)(-1))*/ /*huffmanDecodeSymbol returns (unsigned)(-1) in case of error*/
+ {
+ error = (*bp) > inlength * 8 ? 10 : 11; /*return error code 10 or 11 depending on the situation that happened in huffmanDecodeSymbol (10=no endcode, 11=wrong jump outside of tree)*/
+ break;
+ }
+ }
+
+ HuffmanTree_cleanup(&codetree);
+ HuffmanTree_cleanup(&codetreeD);
+
+ return error;
+}
+
+static unsigned inflateNoCompression(ucvector* out, const unsigned char* in, size_t* bp, size_t* pos, size_t inlength)
+{
+ /*go to first boundary of byte*/
+ size_t p;
+ unsigned LEN, NLEN, n, error = 0;
+ while(((*bp) & 0x7) != 0) (*bp)++;
+ p = (*bp) / 8; /*byte position*/
+
+ /*read LEN (2 bytes) and NLEN (2 bytes)*/
+ if(p >= inlength - 4) return 52; /*error, bit pointer will jump past memory*/
+ LEN = in[p] + 256 * in[p + 1]; p += 2;
+ NLEN = in[p] + 256 * in[p + 1]; p += 2;
+
+ /*check if 16-bit NLEN is really the one's complement of LEN*/
+ if(LEN + NLEN != 65535) return 21; /*error: NLEN is not one's complement of LEN*/
+
+ if((*pos) + LEN >= out->size)
+ {
+ if(!ucvector_resize(out, (*pos) + LEN)) return 9915;
+ }
+
+ /*read the literal data: LEN bytes are now stored in the out buffer*/
+ if(p + LEN > inlength) return 23; /*error: reading outside of in buffer*/
+ for(n = 0; n < LEN; n++) out->data[(*pos)++] = in[p++];
+
+ (*bp) = p * 8;
+
+ return error;
+}
+
+/*inflate the deflated data (cfr. deflate spec); return value is the error*/
+unsigned LodeFlate_inflate(ucvector* out, const unsigned char* in, size_t insize, size_t inpos)
+{
+ size_t bp = 0; /*bit pointer in the "in" data, current byte is bp >> 3, current bit is bp & 0x7 (from lsb to msb of the byte)*/
+ unsigned BFINAL = 0;
+ size_t pos = 0; /*byte position in the out buffer*/
+
+ unsigned error = 0;
+
+ while(!BFINAL)
+ {
+ unsigned BTYPE;
+ if(bp + 2 >= insize * 8) return 52; /*error, bit pointer will jump past memory*/
+ BFINAL = readBitFromStream(&bp, &in[inpos]);
+ BTYPE = 1 * readBitFromStream(&bp, &in[inpos]);
+ BTYPE += 2 * readBitFromStream(&bp, &in[inpos]);
+
+ if(BTYPE == 3) return 20; /*error: invalid BTYPE*/
+ else if(BTYPE == 0) error = inflateNoCompression(out, &in[inpos], &bp, &pos, insize); /*no compression*/
+ else error = inflateHuffmanBlock(out, &in[inpos], &bp, &pos, insize, BTYPE); /*compression, BTYPE 01 or 10*/
+
+ if(error) return error;
+ }
+
+ if(!ucvector_resize(out, pos)) error = 9916; /*Only now we know the true size of out, resize it to that*/
+
+ return error;
+}
+
+#endif /*LODEPNG_COMPILE_DECODER*/
+
+#ifdef LODEPNG_COMPILE_ENCODER
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* / Deflator / */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+static const size_t MAX_SUPPORTED_DEFLATE_LENGTH = 258;
+
+/*bitlen is the size in bits of the code*/
+static void addHuffmanSymbol(size_t* bp, ucvector* compressed, unsigned code, unsigned bitlen)
+{
+ addBitsToStreamReversed(bp, compressed, code, bitlen);
+}
+
+/*search the index in the array, that has the largest value smaller than or equal to the given value, given array must be sorted (if no value is smaller, it returns the size of the given array)*/
+static size_t searchCodeIndex(const unsigned* array, size_t array_size, size_t value)
+{
+ /*linear search implementation*/
+ /*for(size_t i = 1; i < array_size; i++) if(array[i] > value) return i - 1;
+ return array_size - 1;*/
+
+ /*binary search implementation (not that much faster) (precondition: array_size > 0)*/
+ size_t left = 1;
+ size_t right = array_size - 1;
+ while(left <= right)
+ {
+ size_t mid = (left + right) / 2;
+ if(array[mid] <= value) left = mid + 1; /*the value to find is more to the right*/
+ else if(array[mid - 1] > value) right = mid - 1; /*the value to find is more to the left*/
+ else return mid - 1;
+ }
+ return array_size - 1;
+}
+
+static void addLengthDistance(uivector* values, size_t length, size_t distance)
+{
+ /*values in encoded vector are those used by deflate:
+ 0-255: literal bytes
+ 256: end
+ 257-285: length/distance pair (length code, followed by extra length bits, distance code, extra distance bits)
+ 286-287: invalid*/
+
+ unsigned length_code = (unsigned)searchCodeIndex(LENGTHBASE, 29, length);
+ unsigned extra_length = (unsigned)(length - LENGTHBASE[length_code]);
+ unsigned dist_code = (unsigned)searchCodeIndex(DISTANCEBASE, 30, distance);
+ unsigned extra_distance = (unsigned)(distance - DISTANCEBASE[dist_code]);
+
+ uivector_push_back(values, length_code + FIRST_LENGTH_CODE_INDEX);
+ uivector_push_back(values, extra_length);
+ uivector_push_back(values, dist_code);
+ uivector_push_back(values, extra_distance);
+}
+
+#if 0
+/*the "brute force" version of the encodeLZ7 algorithm, not used anymore, kept here for reference*/
+static void encodeLZ77_brute(uivector* out, const unsigned char* in, size_t insize, unsigned windowSize)
+{
+ size_t pos;
+ /*using pointer instead of vector for input makes it faster when NOT using optimization when compiling; no influence if optimization is used*/
+ for(pos = 0; pos < insize; pos++)
+ {
+ size_t length = 0, offset = 0; /*the length and offset found for the current position*/
+ size_t max_offset = pos < windowSize ? pos : windowSize; /*how far back to test*/
+ size_t current_offset;
+
+ /**search for the longest string**/
+ for(current_offset = 1; current_offset < max_offset; current_offset++) /*search backwards through all possible distances (=offsets)*/
+ {
+ size_t backpos = pos - current_offset;
+ if(in[backpos] == in[pos])
+ {
+ /*test the next characters*/
+ size_t current_length = 1;
+ size_t backtest = backpos + 1;
+ size_t foretest = pos + 1;
+ while(foretest < insize && in[backtest] == in[foretest] && current_length < MAX_SUPPORTED_DEFLATE_LENGTH) /*maximum supporte length by deflate is max length*/
+ {
+ if(backpos >= pos) backpos -= current_offset; /*continue as if we work on the decoded bytes after pos by jumping back before pos*/
+ current_length++;
+ backtest++;
+ foretest++;
+ }
+ if(current_length > length)
+ {
+ length = current_length; /*the longest length*/
+ offset = current_offset; /*the offset that is related to this longest length*/
+ if(current_length == MAX_SUPPORTED_DEFLATE_LENGTH) break; /*you can jump out of this for loop once a length of max length is found (gives significant speed gain)*/
+ }
+ }
+ }
+
+ /**encode it as length/distance pair or literal value**/
+ if(length < 3) /*only lengths of 3 or higher are supported as length/distance pair*/
+ {
+ uivector_push_back(out, in[pos]);
+ }
+ else
+ {
+ addLengthDistance(out, length, offset);
+ pos += (length - 1);
+ }
+ } /*end of the loop through each character of input*/
+}
+#endif
+
+static const unsigned HASH_NUM_VALUES = 65536;
+static const unsigned HASH_NUM_CHARACTERS = 6;
+static const unsigned HASH_SHIFT = 2;
+/*
+Good and fast values: HASH_NUM_VALUES=65536, HASH_NUM_CHARACTERS=6, HASH_SHIFT=2
+making HASH_NUM_CHARACTERS larger (like 8), makes the file size larger but is a bit faster
+making HASH_NUM_CHARACTERS smaller (like 3), makes the file size smaller but is slower
+*/
+
+static unsigned getHash(const unsigned char* data, size_t size, size_t pos)
+{
+ unsigned result = 0;
+ size_t amount, i;
+ if(pos >= size) return 0;
+ amount = HASH_NUM_CHARACTERS;
+ if(pos + amount >= size) amount = size - pos;
+ for(i = 0; i < amount; i++) result ^= (data[pos + i] << (i * HASH_SHIFT));
+ return result % HASH_NUM_VALUES;
+}
+
+static unsigned countInitialZeros(const unsigned char* data, size_t size, size_t pos)
+{
+ unsigned maxCount = MAX_SUPPORTED_DEFLATE_LENGTH;
+ if(maxCount > (unsigned)(size-pos))
+ maxCount = (unsigned)(size-pos);
+ for(unsigned i = 0; i < maxCount; i++)
+ if(data[pos+i] != 0)
+ return i;
+ return maxCount;
+}
+
+/*LZ77-encode the data using a hash table technique to let it encode faster. Return value is error code*/
+static unsigned encodeLZ77(uivector* out, const unsigned char* in, size_t insize, unsigned windowSize)
+{
+ /**generate hash table**/
+ vector table; /*HASH_NUM_VALUES uivectors; this represents what would be an std::vector<std::vector<unsigned> > in C++*/
+ uivector tablepos1, tablepos2, initialZerosTable;
+ unsigned pos, i, error = 0;
+
+ vector_init(&table, sizeof(uivector));
+ if(!vector_resize(&table, HASH_NUM_VALUES)) return 9917;
+ for(i = 0; i < HASH_NUM_VALUES; i++)
+ {
+ uivector* v = (uivector*)vector_get(&table, i);
+ uivector_init(v);
+ }
+
+ /*remember start and end positions in the tables to searching in*/
+ uivector_init(&tablepos1);
+ uivector_init(&tablepos2);
+ uivector_init(&initialZerosTable);
+ if(!uivector_resizev(&tablepos1, HASH_NUM_VALUES, 0)) error = 9918;
+ if(!uivector_resizev(&tablepos2, HASH_NUM_VALUES, 0)) error = 9919;
+
+ if(!error)
+ {
+ for(pos = 0; pos < insize; pos++)
+ {
+ unsigned length = 0, offset = 0; /*the length and offset found for the current position*/
+ unsigned max_offset = pos < windowSize ? pos : windowSize; /*how far back to test*/
+ unsigned tablepos;
+
+ /*/search for the longest string*/
+ /*first find out where in the table to start (the first value that is in the range from "pos - max_offset" to "pos")*/
+ unsigned hash = getHash(in, insize, pos);
+ unsigned initialZeros = countInitialZeros(in, insize, pos);
+ if(!uivector_push_back((uivector*)vector_get(&table, hash), pos))
+ {
+ error = 9920; /*memory allocation failed*/
+ break;
+ }
+ if (hash == 0 && !uivector_push_back(&initialZerosTable, initialZeros))
+ {
+ error = 9920; /*memory allocation failed*/
+ break;
+ }
+
+ while(((uivector*)vector_get(&table, hash))->data[tablepos1.data[hash]] < pos - max_offset)
+ {
+ tablepos1.data[hash]++; /*it now points to the first value in the table for which the index is larger than or equal to pos - max_offset*/
+ }
+ while(((uivector*)vector_get(&table, hash))->data[tablepos2.data[hash]] < pos)
+ {
+ tablepos2.data[hash]++; /*it now points to the first value in the table for which the index is larger than or equal to pos*/
+ }
+
+ for(tablepos = tablepos2.data[hash] - 1; tablepos >= tablepos1.data[hash] && tablepos < tablepos2.data[hash]; tablepos--)
+ {
+ unsigned backpos = ((uivector*)vector_get(&table, hash))->data[tablepos];
+ unsigned current_offset = pos - backpos;
+
+ /*test the next characters*/
+ const unsigned char* foreptr = &in[pos];
+ const unsigned char* backptr = &in[backpos];
+ if(hash == 0)
+ {
+ unsigned skip = initialZerosTable.data[tablepos];
+ if (skip > initialZeros)
+ skip = initialZeros;
+ if (skip > (unsigned)(insize-pos))
+ skip = (unsigned)(insize-pos);
+ backptr += skip;
+ foreptr += skip;
+ }
+ const unsigned char* lastptr = &in[insize < pos+MAX_SUPPORTED_DEFLATE_LENGTH ? insize : pos+MAX_SUPPORTED_DEFLATE_LENGTH];
+ while(foreptr != lastptr && *backptr == *foreptr) /*maximum supporte length by deflate is max length*/
+ {
+ ++backptr;
+ ++foreptr;
+ }
+ unsigned current_length = (unsigned) (foreptr-&in[pos]);
+ if(current_length > length)
+ {
+ length = current_length; /*the longest length*/
+ offset = current_offset; /*the offset that is related to this longest length*/
+ if(current_length == MAX_SUPPORTED_DEFLATE_LENGTH) break; /*you can jump out of this for loop once a length of max length is found (gives significant speed gain)*/
+ }
+ }
+
+ /**encode it as length/distance pair or literal value**/
+ if(length < 3) /*only lengths of 3 or higher are supported as length/distance pair*/
+ {
+ if(!uivector_push_back(out, in[pos]))
+ {
+ error = 9921; /*memory allocation failed*/
+ break;
+ }
+ }
+ else
+ {
+ unsigned j;
+ addLengthDistance(out, length, offset);
+ for(j = 0; j < length - 1; j++)
+ {
+ pos++;
+ unsigned localHash = getHash(in, insize, pos);
+ if(!uivector_push_back((uivector*)vector_get(&table, localHash), pos))
+ {
+ error = 9922; /*memory allocation failed*/
+ break;
+ }
+ if(localHash == 0 && !uivector_push_back(&initialZerosTable, countInitialZeros(in, insize, pos)))
+ {
+ error = 9922; /*memory allocation failed*/
+ break;
+ }
+ }
+ }
+ } /*end of the loop through each character of input*/
+ } /*end of "if(!error)"*/
+
+ /*cleanup*/
+ for(i = 0; i < table.size; i++)
+ {
+ uivector* v = (uivector*)vector_get(&table, i);
+ uivector_cleanup(v);
+ }
+ vector_cleanup(&table);
+ uivector_cleanup(&tablepos1);
+ uivector_cleanup(&tablepos2);
+ uivector_cleanup(&initialZerosTable);
+ return error;
+}
+
+/* /////////////////////////////////////////////////////////////////////////// */
+
+static unsigned deflateNoCompression(ucvector* out, const unsigned char* data, size_t datasize)
+{
+ /*non compressed deflate block data: 1 bit BFINAL,2 bits BTYPE,(5 bits): it jumps to start of next byte, 2 bytes LEN, 2 bytes NLEN, LEN bytes literal DATA*/
+
+ size_t i, j, numdeflateblocks = datasize / 65536 + 1;
+ unsigned datapos = 0;
+ for(i = 0; i < numdeflateblocks; i++)
+ {
+ unsigned BFINAL, BTYPE, LEN, NLEN;
+ unsigned char firstbyte;
+
+ BFINAL = (i == numdeflateblocks - 1);
+ BTYPE = 0;
+
+ firstbyte = (unsigned char)(BFINAL + ((BTYPE & 1) << 1) + ((BTYPE & 2) << 1));
+ ucvector_push_back(out, firstbyte);
+
+ LEN = 65535;
+ if(datasize - datapos < 65535) LEN = (unsigned)datasize - datapos;
+ NLEN = 65535 - LEN;
+
+ ucvector_push_back(out, (unsigned char)(LEN % 256));
+ ucvector_push_back(out, (unsigned char)(LEN / 256));
+ ucvector_push_back(out, (unsigned char)(NLEN % 256));
+ ucvector_push_back(out, (unsigned char)(NLEN / 256));
+
+ /*Decompressed data*/
+ for(j = 0; j < 65535 && datapos < datasize; j++)
+ {
+ ucvector_push_back(out, data[datapos++]);
+ }
+ }
+
+ return 0;
+}
+
+/*write the encoded data, using lit/len as well as distance codes*/
+static void writeLZ77data(size_t* bp, ucvector* out, const uivector* lz77_encoded, const HuffmanTree* codes, const HuffmanTree* codesD)
+{
+ size_t i = 0;
+ for(i = 0; i < lz77_encoded->size; i++)
+ {
+ unsigned val = lz77_encoded->data[i];
+ addHuffmanSymbol(bp, out, HuffmanTree_getCode(codes, val), HuffmanTree_getLength(codes, val));
+ if(val > 256) /*for a length code, 3 more things have to be added*/
+ {
+ unsigned length_index = val - FIRST_LENGTH_CODE_INDEX;
+ unsigned n_length_extra_bits = LENGTHEXTRA[length_index];
+ unsigned length_extra_bits = lz77_encoded->data[++i];
+
+ unsigned distance_code = lz77_encoded->data[++i];
+
+ unsigned distance_index = distance_code;
+ unsigned n_distance_extra_bits = DISTANCEEXTRA[distance_index];
+ unsigned distance_extra_bits = lz77_encoded->data[++i];
+
+ addBitsToStream(bp, out, length_extra_bits, n_length_extra_bits);
+ addHuffmanSymbol(bp, out, HuffmanTree_getCode(codesD, distance_code), HuffmanTree_getLength(codesD, distance_code));
+ addBitsToStream(bp, out, distance_extra_bits, n_distance_extra_bits);
+ }
+ }
+}
+
+static unsigned deflateDynamic(ucvector* out, const unsigned char* data, size_t datasize, const LodeZlib_CompressSettings* settings)
+{
+ /*
+ after the BFINAL and BTYPE, the dynamic block consists out of the following:
+ - 5 bits HLIT, 5 bits HDIST, 4 bits HCLEN
+ - (HCLEN+4)*3 bits code lengths of code length alphabet
+ - HLIT + 257 code lenghts of lit/length alphabet (encoded using the code length alphabet, + possible repetition codes 16, 17, 18)
+ - HDIST + 1 code lengths of distance alphabet (encoded using the code length alphabet, + possible repetition codes 16, 17, 18)
+ - compressed data
+ - 256 (end code)
+ */
+
+ unsigned error = 0;
+
+ uivector lz77_encoded;
+ HuffmanTree codes; /*tree for literal values and length codes*/
+ HuffmanTree codesD; /*tree for distance codes*/
+ HuffmanTree codelengthcodes;
+ uivector frequencies;
+ uivector frequenciesD;
+ uivector amounts; /*the amounts in the "normal" order*/
+ uivector lldl;
+ uivector lldll; /*lit/len & dist code lenghts*/
+ uivector clcls;
+
+ unsigned BFINAL = 1; /*make only one block... the first and final one*/
+ size_t numcodes, numcodesD, i;
+ size_t bp = 0; /*the bit pointer*/
+ unsigned HLIT, HDIST, HCLEN;
+
+ uivector_init(&lz77_encoded);
+ HuffmanTree_init(&codes);
+ HuffmanTree_init(&codesD);
+ HuffmanTree_init(&codelengthcodes);
+ uivector_init(&frequencies);
+ uivector_init(&frequenciesD);
+ uivector_init(&amounts);
+ uivector_init(&lldl);
+ uivector_init(&lldll);
+ uivector_init(&clcls);
+
+ while(!error) /*the goto-avoiding while construct: break out to go to the cleanup phase, a break at the end makes sure the while is never repeated*/
+ {
+ if(settings->useLZ77)
+ {
+ error = encodeLZ77(&lz77_encoded, data, datasize, settings->windowSize); /*LZ77 encoded*/
+ if(error) break;
+ }
+ else
+ {
+ if(!uivector_resize(&lz77_encoded, datasize))
+ {
+ error = 9923; /*memory allocation failed*/
+ break;
+ }
+ for(i = 0; i < datasize; i++) lz77_encoded.data[i] = data[i]; /*no LZ77, but still will be Huffman compressed*/
+ }
+
+ if(!uivector_resizev(&frequencies, 286, 0))
+ {
+ error = 9924; /*memory allocation failed*/
+ break;
+ }
+ if(!uivector_resizev(&frequenciesD, 30, 0))
+ {
+ error = 9925; /*memory allocation failed*/
+ break;
+ }
+ for(i = 0; i < lz77_encoded.size; i++)
+ {
+ unsigned symbol = lz77_encoded.data[i];
+ frequencies.data[symbol]++;
+ if(symbol > 256)
+ {
+ unsigned dist = lz77_encoded.data[i + 2];
+ frequenciesD.data[dist]++;
+ i += 3;
+ }
+ }
+ frequencies.data[256] = 1; /*there will be exactly 1 end code, at the end of the block*/
+
+ error = HuffmanTree_makeFromFrequencies(&codes, frequencies.data, frequencies.size, 15);
+ if(error) break;
+ error = HuffmanTree_makeFromFrequencies(&codesD, frequenciesD.data, frequenciesD.size, 15);
+ if(error) break;
+
+ addBitToStream(&bp, out, BFINAL);
+ addBitToStream(&bp, out, 0); /*first bit of BTYPE "dynamic"*/
+ addBitToStream(&bp, out, 1); /*second bit of BTYPE "dynamic"*/
+
+ numcodes = codes.numcodes; if(numcodes > 286) numcodes = 286;
+ numcodesD = codesD.numcodes; if(numcodesD > 30) numcodesD = 30;
+ for(i = 0; i < numcodes; i++) uivector_push_back(&lldll, HuffmanTree_getLength(&codes, (unsigned)i));
+ for(i = 0; i < numcodesD; i++) uivector_push_back(&lldll, HuffmanTree_getLength(&codesD, (unsigned)i));
+
+ /*make lldl smaller by using repeat codes 16 (copy length 3-6 times), 17 (3-10 zeroes), 18 (11-138 zeroes)*/
+ for(i = 0; i < (unsigned)lldll.size; i++)
+ {
+ unsigned j = 0;
+ while(i + j + 1 < (unsigned)lldll.size && lldll.data[i + j + 1] == lldll.data[i]) j++;
+
+ if(lldll.data[i] == 0 && j >= 2)
+ {
+ j++; /*include the first zero*/
+ if(j <= 10)
+ {
+ uivector_push_back(&lldl, 17);
+ uivector_push_back(&lldl, j - 3);
+ }
+ else
+ {
+ if(j > 138) j = 138;
+ uivector_push_back(&lldl, 18);
+ uivector_push_back(&lldl, j - 11);
+ }
+ i += (j - 1);
+ }
+ else if(j >= 3)
+ {
+ size_t k;
+ unsigned num = j / 6, rest = j % 6;
+ uivector_push_back(&lldl, lldll.data[i]);
+ for(k = 0; k < num; k++)
+ {
+ uivector_push_back(&lldl, 16);
+ uivector_push_back(&lldl, 6 - 3);
+ }
+ if(rest >= 3)
+ {
+ uivector_push_back(&lldl, 16);
+ uivector_push_back(&lldl, rest - 3);
+ }
+ else j -= rest;
+ i += j;
+ }
+ else uivector_push_back(&lldl, lldll.data[i]);
+ }
+
+ /*generate huffmantree for the length codes of lit/len and dist codes*/
+ if(!uivector_resizev(&amounts, 19, 0)) /*16 possible lengths (0-15) and 3 repeat codes (16, 17 and 18)*/
+ {
+ error = 9926; /*memory allocation failed*/
+ break;
+ }
+ for(i = 0; i < lldl.size; i++)
+ {
+ amounts.data[lldl.data[i]]++;
+ if(lldl.data[i] >= 16) i++; /*after a repeat code come the bits that specify the amount, those don't need to be in the amounts calculation*/
+ }
+
+ error = HuffmanTree_makeFromFrequencies(&codelengthcodes, amounts.data, amounts.size, 7);
+ if(error) break;
+
+ if(!uivector_resize(&clcls, 19))
+ {
+ error = 9927; /*memory allocation failed*/
+ break;
+ }
+ for(i = 0; i < 19; i++) clcls.data[i] = HuffmanTree_getLength(&codelengthcodes, CLCL[i]); /*lenghts of code length tree is in the order as specified by deflate*/
+ while(clcls.data[clcls.size - 1] == 0 && clcls.size > 4)
+ {
+ if(!uivector_resize(&clcls, clcls.size - 1)) /*remove zeros at the end, but minimum size must be 4*/
+ {
+ error = 9928; /*memory allocation failed*/
+ break;
+ }
+ }
+ if(error) break;
+
+ /*write the HLIT, HDIST and HCLEN values*/
+ HLIT = (unsigned)(numcodes - 257);
+ HDIST = (unsigned)(numcodesD - 1);
+ HCLEN = (unsigned)clcls.size - 4;
+ addBitsToStream(&bp, out, HLIT, 5);
+ addBitsToStream(&bp, out, HDIST, 5);
+ addBitsToStream(&bp, out, HCLEN, 4);
+
+ /*write the code lenghts of the code length alphabet*/
+ for(i = 0; i < HCLEN + 4; i++) addBitsToStream(&bp, out, clcls.data[i], 3);
+
+ /*write the lenghts of the lit/len AND the dist alphabet*/
+ for(i = 0; i < lldl.size; i++)
+ {
+ addHuffmanSymbol(&bp, out, HuffmanTree_getCode(&codelengthcodes, lldl.data[i]), HuffmanTree_getLength(&codelengthcodes, lldl.data[i]));
+ /*extra bits of repeat codes*/
+ if(lldl.data[i] == 16) addBitsToStream(&bp, out, lldl.data[++i], 2);
+ else if(lldl.data[i] == 17) addBitsToStream(&bp, out, lldl.data[++i], 3);
+ else if(lldl.data[i] == 18) addBitsToStream(&bp, out, lldl.data[++i], 7);
+ }
+
+ /*write the compressed data symbols*/
+ writeLZ77data(&bp, out, &lz77_encoded, &codes, &codesD);
+ if(HuffmanTree_getLength(&codes, 256) == 0)
+ {
+ error = 64; /*the length of the end code 256 must be larger than 0*/
+ break;
+ }
+ addHuffmanSymbol(&bp, out, HuffmanTree_getCode(&codes, 256), HuffmanTree_getLength(&codes, 256)); /*end code*/
+
+ break; /*end of error-while*/
+ }
+
+ /*cleanup*/
+ uivector_cleanup(&lz77_encoded);
+ HuffmanTree_cleanup(&codes);
+ HuffmanTree_cleanup(&codesD);
+ HuffmanTree_cleanup(&codelengthcodes);
+ uivector_cleanup(&frequencies);
+ uivector_cleanup(&frequenciesD);
+ uivector_cleanup(&amounts);
+ uivector_cleanup(&lldl);
+ uivector_cleanup(&lldll);
+ uivector_cleanup(&clcls);
+
+ return error;
+}
+
+static unsigned deflateFixed(ucvector* out, const unsigned char* data, size_t datasize, const LodeZlib_CompressSettings* settings)
+{
+ HuffmanTree codes; /*tree for literal values and length codes*/
+ HuffmanTree codesD; /*tree for distance codes*/
+
+ unsigned BFINAL = 1; /*make only one block... the first and final one*/
+ unsigned error = 0;
+ size_t i, bp = 0; /*the bit pointer*/
+
+ HuffmanTree_init(&codes);
+ HuffmanTree_init(&codesD);
+
+ generateFixedTree(&codes);
+ generateDistanceTree(&codesD);
+
+ addBitToStream(&bp, out, BFINAL);
+ addBitToStream(&bp, out, 1); /*first bit of BTYPE*/
+ addBitToStream(&bp, out, 0); /*second bit of BTYPE*/
+
+ if(settings->useLZ77) /*LZ77 encoded*/
+ {
+ uivector lz77_encoded;
+ uivector_init(&lz77_encoded);
+ error = encodeLZ77(&lz77_encoded, data, datasize, settings->windowSize);
+ if(!error) writeLZ77data(&bp, out, &lz77_encoded, &codes, &codesD);
+ uivector_cleanup(&lz77_encoded);
+ }
+ else /*no LZ77, but still will be Huffman compressed*/
+ {
+ for(i = 0; i < datasize; i++)
+ {
+ addHuffmanSymbol(&bp, out, HuffmanTree_getCode(&codes, data[i]), HuffmanTree_getLength(&codes, data[i]));
+ }
+ }
+ if(!error) addHuffmanSymbol(&bp, out, HuffmanTree_getCode(&codes, 256), HuffmanTree_getLength(&codes, 256)); /*"end" code*/
+
+ /*cleanup*/
+ HuffmanTree_cleanup(&codes);
+ HuffmanTree_cleanup(&codesD);
+
+ return error;
+}
+
+unsigned LodeFlate_deflate(ucvector* out, const unsigned char* data, size_t datasize, const LodeZlib_CompressSettings* settings)
+{
+ unsigned error = 0;
+ if(settings->btype == 0) error = deflateNoCompression(out, data, datasize);
+ else if(settings->btype == 1) error = deflateFixed(out, data, datasize, settings);
+ else if(settings->btype == 2) error = deflateDynamic(out, data, datasize, settings);
+ else error = 61;
+ return error;
+}
+
+#endif /*LODEPNG_COMPILE_DECODER*/
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* / Adler32 */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+static unsigned update_adler32(unsigned adler, const unsigned char* data, unsigned len)
+{
+ unsigned s1 = adler & 0xffff;
+ unsigned s2 = (adler >> 16) & 0xffff;
+
+ while(len > 0)
+ {
+ /*at least 5550 sums can be done before the sums overflow, saving us from a lot of module divisions*/
+ unsigned amount = len > 5550 ? 5550 : len;
+ len -= amount;
+ while(amount > 0)
+ {
+ s1 = (s1 + *data++);
+ s2 = (s2 + s1);
+ amount--;
+ }
+ s1 %= 65521;
+ s2 %= 65521;
+ }
+
+ return (s2 << 16) | s1;
+}
+
+/*Return the adler32 of the bytes data[0..len-1]*/
+static unsigned adler32(const unsigned char* data, unsigned len)
+{
+ return update_adler32(1L, data, len);
+}
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* / Reading and writing single bits and bytes from/to stream for Zlib / */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+#ifdef LODEPNG_COMPILE_ENCODER
+void LodeZlib_add32bitInt(ucvector* buffer, unsigned value)
+{
+ ucvector_push_back(buffer, (unsigned char)((value >> 24) & 0xff));
+ ucvector_push_back(buffer, (unsigned char)((value >> 16) & 0xff));
+ ucvector_push_back(buffer, (unsigned char)((value >> 8) & 0xff));
+ ucvector_push_back(buffer, (unsigned char)((value ) & 0xff));
+}
+#endif /*LODEPNG_COMPILE_ENCODER*/
+
+unsigned LodeZlib_read32bitInt(const unsigned char* buffer)
+{
+ return (buffer[0] << 24) | (buffer[1] << 16) | (buffer[2] << 8) | buffer[3];
+}
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* / Zlib / */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+#ifdef LODEPNG_COMPILE_DECODER
+
+unsigned LodeZlib_decompress(unsigned char** out, size_t* outsize, const unsigned char* in, size_t insize, const LodeZlib_DecompressSettings* settings)
+{
+ unsigned error = 0;
+ unsigned CM, CINFO, FDICT;
+ ucvector outv;
+
+ if(insize < 2) return 53; /*error, size of zlib data too small*/
+ /*read information from zlib header*/
+ if((in[0] * 256 + in[1]) % 31 != 0) return 24; /*error: 256 * in[0] + in[1] must be a multiple of 31, the FCHECK value is supposed to be made that way*/
+
+ CM = in[0] & 15;
+ CINFO = (in[0] >> 4) & 15;
+ /*FCHECK = in[1] & 31; //FCHECK is already tested above*/
+ FDICT = (in[1] >> 5) & 1;
+ /*FLEVEL = (in[1] >> 6) & 3; //not really important, all it does it to give a compiler warning about unused variable, we don't care what encoding setting the encoder used*/
+
+ if(CM != 8 || CINFO > 7) return 25; /*error: only compression method 8: inflate with sliding window of 32k is supported by the PNG spec*/
+ if(FDICT != 0) return 26; /*error: the specification of PNG says about the zlib stream: "The additional flags shall not specify a preset dictionary."*/
+
+ ucvector_init_buffer(&outv, *out, *outsize); /*ucvector-controlled version of the output buffer, for dynamic array*/
+ error = LodeFlate_inflate(&outv, in, insize, 2);
+ *out = outv.data;
+ *outsize = outv.size;
+ if(error) return error;
+
+ if(!settings->ignoreAdler32)
+ {
+ unsigned ADLER32 = LodeZlib_read32bitInt(&in[insize - 4]);
+ unsigned checksum = adler32(outv.data, (unsigned)outv.size);
+ if(checksum != ADLER32) return 58; /*error, adler checksum not correct, data must be corrupted*/
+ }
+
+ return 0; /*no error*/
+}
+
+#endif /*LODEPNG_COMPILE_DECODER*/
+
+#ifdef LODEPNG_COMPILE_ENCODER
+
+unsigned LodeZlib_compress(unsigned char** out, size_t* outsize, const unsigned char* in, size_t insize, const LodeZlib_CompressSettings* settings)
+{
+ /*initially, *out must be NULL and outsize 0, if you just give some random *out that's pointing to a non allocated buffer, this'll crash*/
+ ucvector deflatedata, outv;
+ size_t i;
+ unsigned error;
+
+ unsigned ADLER32;
+ /*zlib data: 1 byte CMF (CM+CINFO), 1 byte FLG, deflate data, 4 byte ADLER32 checksum of the Decompressed data*/
+ unsigned CMF = 120; /*0b01111000: CM 8, CINFO 7. With CINFO 7, any window size up to 32768 can be used.*/
+ unsigned FLEVEL = 0;
+ unsigned FDICT = 0;
+ unsigned CMFFLG = 256 * CMF + FDICT * 32 + FLEVEL * 64;
+ unsigned FCHECK = 31 - CMFFLG % 31;
+ CMFFLG += FCHECK;
+
+ ucvector_init_buffer(&outv, *out, *outsize); /*ucvector-controlled version of the output buffer, for dynamic array*/
+
+ ucvector_push_back(&outv, (unsigned char)(CMFFLG / 256));
+ ucvector_push_back(&outv, (unsigned char)(CMFFLG % 256));
+
+ ucvector_init(&deflatedata);
+ error = LodeFlate_deflate(&deflatedata, in, insize, settings);
+
+ if(!error)
+ {
+ ADLER32 = adler32(in, (unsigned)insize);
+ for(i = 0; i < deflatedata.size; i++) ucvector_push_back(&outv, deflatedata.data[i]);
+ ucvector_cleanup(&deflatedata);
+ LodeZlib_add32bitInt(&outv, ADLER32);
+ }
+
+ *out = outv.data;
+ *outsize = outv.size;
+
+ return error;
+}
+
+#endif /*LODEPNG_COMPILE_ENCODER*/
+
+#endif /*LODEPNG_COMPILE_ZLIB*/
+
+/* ////////////////////////////////////////////////////////////////////////// */
+
+#ifdef LODEPNG_COMPILE_ENCODER
+
+void LodeZlib_CompressSettings_init(LodeZlib_CompressSettings* settings)
+{
+ settings->btype = 2; /*compress with dynamic huffman tree (not in the mathematical sense, just not the predefined one)*/
+ settings->useLZ77 = 1;
+ settings->windowSize = 2048; /*this is a good tradeoff between speed and compression ratio*/
+}
+
+const LodeZlib_CompressSettings LodeZlib_defaultCompressSettings = {2, 1, 2048};
+
+#endif /*LODEPNG_COMPILE_ENCODER*/
+
+#ifdef LODEPNG_COMPILE_DECODER
+
+void LodeZlib_DecompressSettings_init(LodeZlib_DecompressSettings* settings)
+{
+ settings->ignoreAdler32 = 0;
+}
+
+const LodeZlib_DecompressSettings LodeZlib_defaultDecompressSettings = {0};
+
+#endif /*LODEPNG_COMPILE_DECODER*/
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* ////////////////////////////////////////////////////////////////////////// */
+/* ////////////////////////////////////////////////////////////////////////// */
+/* ////////////////////////////////////////////////////////////////////////// */
+/* ////////////////////////////////////////////////////////////////////////// */
+/* // End of Zlib related code, now comes the PNG related code that uses it// */
+/* ////////////////////////////////////////////////////////////////////////// */
+/* ////////////////////////////////////////////////////////////////////////// */
+/* ////////////////////////////////////////////////////////////////////////// */
+/* ////////////////////////////////////////////////////////////////////////// */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+#ifdef LODEPNG_COMPILE_PNG
+
+/*
+The two functions below (LodePNG_decompress and LodePNG_compress) directly call the
+LodeZlib_decompress and LodeZlib_compress functions. The only purpose of the functions
+below, is to provide the ability to let LodePNG use a different Zlib encoder by only
+changing the two functions below, instead of changing it inside the vareous places
+in the other LodePNG functions.
+
+*out must be NULL and *outsize must be 0 initially, and after the function is done,
+*out must point to the decompressed data, *outsize must be the size of it, and must
+be the size of the useful data in bytes, not the alloc size.
+*/
+
+#ifdef LODEPNG_COMPILE_DECODER
+static unsigned LodePNG_decompress(unsigned char** out, size_t* outsize, const unsigned char* in, size_t insize, const LodeZlib_DecompressSettings* settings)
+{
+ return LodeZlib_decompress(out, outsize, in, insize, settings);
+}
+#endif /*LODEPNG_COMPILE_DECODER*/
+#ifdef LODEPNG_COMPILE_ENCODER
+static unsigned LodePNG_compress(unsigned char** out, size_t* outsize, const unsigned char* in, size_t insize, const LodeZlib_CompressSettings* settings)
+{
+ return LodeZlib_compress(out, outsize, in, insize, settings);
+}
+#endif /*LODEPNG_COMPILE_ENCODER*/
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* / CRC32 / */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+static unsigned Crc32_crc_table_computed = 0;
+static unsigned Crc32_crc_table[256];
+
+/*Make the table for a fast CRC.*/
+static void Crc32_make_crc_table(void)
+{
+ unsigned c, k, n;
+ for(n = 0; n < 256; n++)
+ {
+ c = n;
+ for(k = 0; k < 8; k++)
+ {
+ if(c & 1) c = 0xedb88320L ^ (c >> 1);
+ else c = c >> 1;
+ }
+ Crc32_crc_table[n] = c;
+ }
+ Crc32_crc_table_computed = 1;
+}
+
+/*Update a running CRC with the bytes buf[0..len-1]--the CRC should be
+initialized to all 1's, and the transmitted value is the 1's complement of the
+final running CRC (see the crc() routine below).*/
+static unsigned Crc32_update_crc(const unsigned char* buf, unsigned crc, size_t len)
+{
+ unsigned c = crc;
+ size_t n;
+
+ if(!Crc32_crc_table_computed) Crc32_make_crc_table();
+ for(n = 0; n < len; n++)
+ {
+ c = Crc32_crc_table[(c ^ buf[n]) & 0xff] ^ (c >> 8);
+ }
+ return c;
+}
+
+/*Return the CRC of the bytes buf[0..len-1].*/
+static unsigned Crc32_crc(const unsigned char* buf, size_t len)
+{
+ return Crc32_update_crc(buf, 0xffffffffL, len) ^ 0xffffffffL;
+}
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* / Reading and writing single bits and bytes from/to stream for LodePNG / */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+static unsigned char readBitFromReversedStream(size_t* bitpointer, const unsigned char* bitstream)
+{
+ unsigned char result = (unsigned char)((bitstream[(*bitpointer) >> 3] >> (7 - ((*bitpointer) & 0x7))) & 1);
+ (*bitpointer)++;
+ return result;
+}
+
+static unsigned readBitsFromReversedStream(size_t* bitpointer, const unsigned char* bitstream, size_t nbits)
+{
+ unsigned result = 0;
+ size_t i;
+ for(i = nbits - 1; i < nbits; i--)
+ {
+ result += (unsigned)readBitFromReversedStream(bitpointer, bitstream) << i;
+ }
+ return result;
+}
+
+#ifdef LODEPNG_COMPILE_DECODER
+static void setBitOfReversedStream0(size_t* bitpointer, unsigned char* bitstream, unsigned char bit)
+{
+ /*the current bit in bitstream must be 0 for this to work*/
+ if(bit) bitstream[(*bitpointer) >> 3] |= (bit << (7 - ((*bitpointer) & 0x7))); /*earlier bit of huffman code is in a lesser significant bit of an earlier byte*/
+ (*bitpointer)++;
+}
+#endif /*LODEPNG_COMPILE_DECODER*/
+
+static void setBitOfReversedStream(size_t* bitpointer, unsigned char* bitstream, unsigned char bit)
+{
+ /*the current bit in bitstream may be 0 or 1 for this to work*/
+ if(bit == 0) bitstream[(*bitpointer) >> 3] &= (unsigned char)(~(1 << (7 - ((*bitpointer) & 0x7))));
+ else bitstream[(*bitpointer) >> 3] |= (1 << (7 - ((*bitpointer) & 0x7)));
+ (*bitpointer)++;
+}
+
+static unsigned LodePNG_read32bitInt(const unsigned char* buffer)
+{
+ return (buffer[0] << 24) | (buffer[1] << 16) | (buffer[2] << 8) | buffer[3];
+}
+
+static void LodePNG_set32bitInt(unsigned char* buffer, unsigned value) /*buffer must have at least 4 allocated bytes available*/
+{
+ buffer[0] = (unsigned char)((value >> 24) & 0xff);
+ buffer[1] = (unsigned char)((value >> 16) & 0xff);
+ buffer[2] = (unsigned char)((value >> 8) & 0xff);
+ buffer[3] = (unsigned char)((value ) & 0xff);
+}
+
+#ifdef LODEPNG_COMPILE_ENCODER
+static void LodePNG_add32bitInt(ucvector* buffer, unsigned value)
+{
+ ucvector_resize(buffer, buffer->size + 4); /*todo: give error if resize failed*/
+ LodePNG_set32bitInt(&buffer->data[buffer->size - 4], value);
+}
+#endif /*LODEPNG_COMPILE_ENCODER*/
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* / PNG chunks / */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+unsigned LodePNG_chunk_length(const unsigned char* chunk) /*get the length of the data of the chunk. Total chunk length has 12 bytes more.*/
+{
+ return LodePNG_read32bitInt(&chunk[0]);
+}
+
+void LodePNG_chunk_type(char type[5], const unsigned char* chunk) /*puts the 4-byte type in null terminated string*/
+{
+ unsigned i;
+ for(i = 0; i < 4; i++) type[i] = chunk[4 + i];
+ type[4] = 0; /*null termination char*/
+}
+
+unsigned char LodePNG_chunk_type_equals(const unsigned char* chunk, const char* type) /*check if the type is the given type*/
+{
+ if(strlen(type) != 4) return 0;
+ return (chunk[4] == type[0] && chunk[5] == type[1] && chunk[6] == type[2] && chunk[7] == type[3]);
+}
+
+/*properties of PNG chunks gotten from capitalization of chunk type name, as defined by the standard*/
+unsigned char LodePNG_chunk_critical(const unsigned char* chunk) /*0: ancillary chunk, 1: it's one of the critical chunk types*/
+{
+ return((chunk[4] & 32) == 0);
+}
+
+unsigned char LodePNG_chunk_private(const unsigned char* chunk) /*0: public, 1: private*/
+{
+ return((chunk[6] & 32) != 0);
+}
+
+unsigned char LodePNG_chunk_safetocopy(const unsigned char* chunk) /*0: the chunk is unsafe to copy, 1: the chunk is safe to copy*/
+{
+ return((chunk[7] & 32) != 0);
+}
+
+unsigned char* LodePNG_chunk_data(unsigned char* chunk) /*get pointer to the data of the chunk*/
+{
+ return &chunk[8];
+}
+
+const unsigned char* LodePNG_chunk_data_const(const unsigned char* chunk) /*get pointer to the data of the chunk*/
+{
+ return &chunk[8];
+}
+
+unsigned LodePNG_chunk_check_crc(const unsigned char* chunk) /*returns 0 if the crc is correct, error code if it's incorrect*/
+{
+ unsigned length = LodePNG_chunk_length(chunk);
+ unsigned CRC = LodePNG_read32bitInt(&chunk[length + 8]);
+ unsigned checksum = Crc32_crc(&chunk[4], length + 4); /*the CRC is taken of the data and the 4 chunk type letters, not the length*/
+ if(CRC != checksum) return 1;
+ else return 0;
+}
+
+void LodePNG_chunk_generate_crc(unsigned char* chunk) /*generates the correct CRC from the data and puts it in the last 4 bytes of the chunk*/
+{
+ unsigned length = LodePNG_chunk_length(chunk);
+ unsigned CRC = Crc32_crc(&chunk[4], length + 4);
+ LodePNG_set32bitInt(chunk + 8 + length, CRC);
+}
+
+unsigned char* LodePNG_chunk_next(unsigned char* chunk) /*don't use on IEND chunk, as there is no next chunk then*/
+{
+ unsigned total_chunk_length = LodePNG_chunk_length(chunk) + 12;
+ return &chunk[total_chunk_length];
+}
+
+const unsigned char* LodePNG_chunk_next_const(const unsigned char* chunk) /*don't use on IEND chunk, as there is no next chunk then*/
+{
+ unsigned total_chunk_length = LodePNG_chunk_length(chunk) + 12;
+ return &chunk[total_chunk_length];
+}
+
+unsigned LodePNG_append_chunk(unsigned char** out, size_t* outlength, const unsigned char* chunk) /*appends chunk that was already created, to the data. Returns error code.*/
+{
+ unsigned i;
+ unsigned total_chunk_length = LodePNG_chunk_length(chunk) + 12;
+ unsigned char *chunk_start, *new_buffer;
+ size_t new_length = (*outlength) + total_chunk_length;
+ if(new_length < total_chunk_length || new_length < (*outlength)) return 77; /*integer overflow happened*/
+
+ new_buffer = (unsigned char*)realloc(*out, new_length);
+ if(!new_buffer) return 9929; /*memory allocation failed*/
+ (*out) = new_buffer;
+ (*outlength) = new_length;
+ chunk_start = &(*out)[new_length - total_chunk_length];
+
+ for(i = 0; i < total_chunk_length; i++) chunk_start[i] = chunk[i];
+
+ return 0;
+}
+
+unsigned LodePNG_create_chunk(unsigned char** out, size_t* outlength, unsigned length, const char* type, const unsigned char* data) /*appends new chunk to out. Returns error code; may change memory address of out buffer*/
+{
+ unsigned i;
+ unsigned char *chunk, *new_buffer;
+ size_t new_length = (*outlength) + length + 12;
+ if(new_length < length + 12 || new_length < (*outlength)) return 77; /*integer overflow happened*/
+ new_buffer = (unsigned char*)realloc(*out, new_length);
+ if(!new_buffer) return 9930; /*memory allocation failed*/
+ (*out) = new_buffer;
+ (*outlength) = new_length;
+ chunk = &(*out)[(*outlength) - length - 12];
+
+ /*1: length*/
+ LodePNG_set32bitInt(chunk, (unsigned)length);
+
+ /*2: chunk name (4 letters)*/
+ chunk[4] = type[0];
+ chunk[5] = type[1];
+ chunk[6] = type[2];
+ chunk[7] = type[3];
+
+ /*3: the data*/
+ for(i = 0; i < length; i++) chunk[8 + i] = data[i];
+
+ /*4: CRC (of the chunkname characters and the data)*/
+ LodePNG_chunk_generate_crc(chunk);
+
+ return 0;
+}
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* / Color types and such / */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+/*return type is a LodePNG error code*/
+static unsigned checkColorValidity(unsigned colorType, unsigned bd) /*bd = bitDepth*/
+{
+ switch(colorType)
+ {
+ case 0: if(!(bd == 1 || bd == 2 || bd == 4 || bd == 8 || bd == 16)) return 37; break; /*grey*/
+ case 2: if(!( bd == 8 || bd == 16)) return 37; break; /*RGB*/
+ case 3: if(!(bd == 1 || bd == 2 || bd == 4 || bd == 8 )) return 37; break; /*palette*/
+ case 4: if(!( bd == 8 || bd == 16)) return 37; break; /*grey + alpha*/
+ case 6: if(!( bd == 8 || bd == 16)) return 37; break; /*RGBA*/
+ default: return 31;
+ }
+ return 0; /*allowed color type / bits combination*/
+}
+
+static unsigned getNumColorChannels(unsigned colorType)
+{
+ switch(colorType)
+ {
+ case 0: return 1; /*grey*/
+ case 2: return 3; /*RGB*/
+ case 3: return 1; /*palette*/
+ case 4: return 2; /*grey + alpha*/
+ case 6: return 4; /*RGBA*/
+ }
+ return 0; /*unexisting color type*/
+}
+
+static unsigned getBpp(unsigned colorType, unsigned bitDepth)
+{
+ return getNumColorChannels(colorType) * bitDepth; /*bits per pixel is amount of channels * bits per channel*/
+}
+
+/* ////////////////////////////////////////////////////////////////////////// */
+
+void LodePNG_InfoColor_init(LodePNG_InfoColor* info)
+{
+ info->key_defined = 0;
+ info->key_r = info->key_g = info->key_b = 0;
+ info->colorType = 6;
+ info->bitDepth = 8;
+ info->palette = 0;
+ info->palettesize = 0;
+}
+
+void LodePNG_InfoColor_cleanup(LodePNG_InfoColor* info)
+{
+ LodePNG_InfoColor_clearPalette(info);
+}
+
+void LodePNG_InfoColor_clearPalette(LodePNG_InfoColor* info)
+{
+ if(info->palette) free(info->palette);
+ info->palettesize = 0;
+}
+
+unsigned LodePNG_InfoColor_addPalette(LodePNG_InfoColor* info, unsigned char r, unsigned char g, unsigned char b, unsigned char a)
+{
+ unsigned char* data;
+ /*the same resize technique as C++ std::vectors is used, and here it's made so that for a palette with the max of 256 colors, it'll have the exact alloc size*/
+ if(!(info->palettesize & (info->palettesize - 1))) /*if palettesize is 0 or a power of two*/
+ {
+ /*allocated data must be at least 4* palettesize (for 4 color bytes)*/
+ size_t alloc_size = info->palettesize == 0 ? 4 : info->palettesize * 4 * 2;
+ data = (unsigned char*)realloc(info->palette, alloc_size);
+ if(!data) return 9931; /*memory allocation failed*/
+ else info->palette = data;
+ }
+ info->palette[4 * info->palettesize + 0] = r;
+ info->palette[4 * info->palettesize + 1] = g;
+ info->palette[4 * info->palettesize + 2] = b;
+ info->palette[4 * info->palettesize + 3] = a;
+ info->palettesize++;
+ return 0;
+}
+
+unsigned LodePNG_InfoColor_getBpp(const LodePNG_InfoColor* info)
+{
+ return getBpp(info->colorType, info->bitDepth); /*calculate bits per pixel out of colorType and bitDepth*/
+}
+
+unsigned LodePNG_InfoColor_getChannels(const LodePNG_InfoColor* info)
+{
+ return getNumColorChannels(info->colorType);
+}
+
+unsigned LodePNG_InfoColor_isGreyscaleType(const LodePNG_InfoColor* info)
+{
+ return info->colorType == 0 || info->colorType == 4;
+}
+
+unsigned LodePNG_InfoColor_isAlphaType(const LodePNG_InfoColor* info)
+{
+ return (info->colorType & 4) != 0;
+}
+
+unsigned LodePNG_InfoColor_equal(const LodePNG_InfoColor* info1, const LodePNG_InfoColor* info2)
+{
+ return info1->colorType == info2->colorType
+ && info1->bitDepth == info2->bitDepth; /*palette and color key not compared*/
+}
+
+#ifdef LODEPNG_COMPILE_UNKNOWN_CHUNKS
+
+void LodePNG_UnknownChunks_init(LodePNG_UnknownChunks* chunks)
+{
+ unsigned i;
+ for(i = 0; i < 3; i++) chunks->data[i] = 0;
+ for(i = 0; i < 3; i++) chunks->datasize[i] = 0;
+}
+
+void LodePNG_UnknownChunks_cleanup(LodePNG_UnknownChunks* chunks)
+{
+ unsigned i;
+ for(i = 0; i < 3; i++) free(chunks->data[i]);
+}
+
+unsigned LodePNG_UnknownChunks_copy(LodePNG_UnknownChunks* dest, const LodePNG_UnknownChunks* src)
+{
+ unsigned i;
+
+ LodePNG_UnknownChunks_cleanup(dest);
+
+ for(i = 0; i < 3; i++)
+ {
+ size_t j;
+ dest->datasize[i] = src->datasize[i];
+ dest->data[i] = (unsigned char*)malloc(src->datasize[i]);
+ if(!dest->data[i] && dest->datasize[i]) return 9932; /*memory allocation failed*/
+ for(j = 0; j < src->datasize[i]; j++) dest->data[i][j] = src->data[i][j];
+ }
+
+ return 0;
+}
+
+#endif /*LODEPNG_COMPILE_UNKNOWN_CHUNKS*/
+
+#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS
+
+void LodePNG_Text_init(LodePNG_Text* text)
+{
+ text->num = 0;
+ text->keys = NULL;
+ text->strings = NULL;
+}
+
+void LodePNG_Text_cleanup(LodePNG_Text* text)
+{
+ LodePNG_Text_clear(text);
+}
+
+unsigned LodePNG_Text_copy(LodePNG_Text* dest, const LodePNG_Text* source)
+{
+ size_t i = 0;
+ dest->keys = 0;
+ dest->strings = 0;
+ dest->num = 0;
+ for(i = 0; i < source->num; i++)
+ {
+ unsigned error = LodePNG_Text_add(dest, source->keys[i], source->strings[i]);
+ if(error) return error;
+ }
+ return 0;
+}
+
+void LodePNG_Text_clear(LodePNG_Text* text)
+{
+ size_t i;
+ for(i = 0; i < text->num; i++)
+ {
+ string_cleanup(&text->keys[i]);
+ string_cleanup(&text->strings[i]);
+ }
+ free(text->keys);
+ free(text->strings);
+}
+
+unsigned LodePNG_Text_add(LodePNG_Text* text, const char* key, const char* str)
+{
+ char** new_keys = (char**)(realloc(text->keys, sizeof(char*) * (text->num + 1)));
+ char** new_strings = (char**)(realloc(text->strings, sizeof(char*) * (text->num + 1)));
+ if(!new_keys || !new_strings)
+ {
+ free(new_keys);
+ free(new_strings);
+ return 9933; /*memory allocation failed*/
+ }
+
+ text->num++;
+ text->keys = new_keys;
+ text->strings = new_strings;
+
+ string_init(&text->keys[text->num - 1]);
+ string_set(&text->keys[text->num - 1], key);
+
+ string_init(&text->strings[text->num - 1]);
+ string_set(&text->strings[text->num - 1], str);
+
+ return 0;
+}
+
+/******************************************************************************/
+
+void LodePNG_IText_init(LodePNG_IText* text)
+{
+ text->num = 0;
+ text->keys = NULL;
+ text->langtags = NULL;
+ text->transkeys = NULL;
+ text->strings = NULL;
+}
+
+void LodePNG_IText_cleanup(LodePNG_IText* text)
+{
+ LodePNG_IText_clear(text);
+}
+
+unsigned LodePNG_IText_copy(LodePNG_IText* dest, const LodePNG_IText* source)
+{
+ size_t i = 0;
+ dest->keys = 0;
+ dest->langtags = 0;
+ dest->transkeys = 0;
+ dest->strings = 0;
+ dest->num = 0;
+ for(i = 0; i < source->num; i++)
+ {
+ unsigned error = LodePNG_IText_add(dest, source->keys[i], source->langtags[i], source->transkeys[i], source->strings[i]);
+ if(error) return error;
+ }
+ return 0;
+}
+
+void LodePNG_IText_clear(LodePNG_IText* text)
+{
+ size_t i;
+ for(i = 0; i < text->num; i++)
+ {
+ string_cleanup(&text->keys[i]);
+ string_cleanup(&text->langtags[i]);
+ string_cleanup(&text->transkeys[i]);
+ string_cleanup(&text->strings[i]);
+ }
+ free(text->keys);
+ free(text->langtags);
+ free(text->transkeys);
+ free(text->strings);
+}
+
+unsigned LodePNG_IText_add(LodePNG_IText* text, const char* key, const char* langtag, const char* transkey, const char* str)
+{
+ char** new_keys = (char**)(realloc(text->keys, sizeof(char*) * (text->num + 1)));
+ char** new_langtags = (char**)(realloc(text->langtags, sizeof(char*) * (text->num + 1)));
+ char** new_transkeys = (char**)(realloc(text->transkeys, sizeof(char*) * (text->num + 1)));
+ char** new_strings = (char**)(realloc(text->strings, sizeof(char*) * (text->num + 1)));
+ if(!new_keys || !new_langtags || !new_transkeys || !new_strings)
+ {
+ free(new_keys);
+ free(new_langtags);
+ free(new_transkeys);
+ free(new_strings);
+ return 9934; /*memory allocation failed*/
+ }
+
+ text->num++;
+ text->keys = new_keys;
+ text->langtags = new_langtags;
+ text->transkeys = new_transkeys;
+ text->strings = new_strings;
+
+ string_init(&text->keys[text->num - 1]);
+ string_set(&text->keys[text->num - 1], key);
+
+ string_init(&text->langtags[text->num - 1]);
+ string_set(&text->langtags[text->num - 1], langtag);
+
+ string_init(&text->transkeys[text->num - 1]);
+ string_set(&text->transkeys[text->num - 1], transkey);
+
+ string_init(&text->strings[text->num - 1]);
+ string_set(&text->strings[text->num - 1], str);
+
+ return 0;
+}
+
+#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/
+
+void LodePNG_InfoPng_init(LodePNG_InfoPng* info)
+{
+ info->width = info->height = 0;
+ LodePNG_InfoColor_init(&info->color);
+ info->interlaceMethod = 0;
+ info->compressionMethod = 0;
+ info->filterMethod = 0;
+#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS
+ info->background_defined = 0;
+ info->background_r = info->background_g = info->background_b = 0;
+
+ LodePNG_Text_init(&info->text);
+ LodePNG_IText_init(&info->itext);
+
+ info->time_defined = 0;
+ info->phys_defined = 0;
+#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/
+#ifdef LODEPNG_COMPILE_UNKNOWN_CHUNKS
+ LodePNG_UnknownChunks_init(&info->unknown_chunks);
+#endif /*LODEPNG_COMPILE_UNKNOWN_CHUNKS*/
+}
+
+void LodePNG_InfoPng_cleanup(LodePNG_InfoPng* info)
+{
+ LodePNG_InfoColor_cleanup(&info->color);
+#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS
+ LodePNG_Text_cleanup(&info->text);
+ LodePNG_IText_cleanup(&info->itext);
+#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/
+#ifdef LODEPNG_COMPILE_UNKNOWN_CHUNKS
+ LodePNG_UnknownChunks_cleanup(&info->unknown_chunks);
+#endif /*LODEPNG_COMPILE_UNKNOWN_CHUNKS*/
+}
+
+unsigned LodePNG_InfoPng_copy(LodePNG_InfoPng* dest, const LodePNG_InfoPng* source)
+{
+ unsigned error = 0;
+ LodePNG_InfoPng_cleanup(dest);
+ *dest = *source;
+ LodePNG_InfoColor_init(&dest->color);
+ error = LodePNG_InfoColor_copy(&dest->color, &source->color); if(error) return error;
+
+#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS
+ error = LodePNG_Text_copy(&dest->text, &source->text); if(error) return error;
+ error = LodePNG_IText_copy(&dest->itext, &source->itext); if(error) return error;
+#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/
+
+#ifdef LODEPNG_COMPILE_UNKNOWN_CHUNKS
+ LodePNG_UnknownChunks_init(&dest->unknown_chunks);
+ error = LodePNG_UnknownChunks_copy(&dest->unknown_chunks, &source->unknown_chunks); if(error) return error;
+#endif /*LODEPNG_COMPILE_UNKNOWN_CHUNKS*/
+ return error;
+}
+
+void LodePNG_InfoPng_swap(LodePNG_InfoPng* a, LodePNG_InfoPng* b)
+{
+ LodePNG_InfoPng temp = *a;
+ *a = *b;
+ *b = temp;
+}
+
+unsigned LodePNG_InfoColor_copy(LodePNG_InfoColor* dest, const LodePNG_InfoColor* source)
+{
+ size_t i;
+ LodePNG_InfoColor_cleanup(dest);
+ *dest = *source;
+ dest->palette = (unsigned char*)malloc(source->palettesize * 4);
+ if(!dest->palette && source->palettesize) return 9935; /*memory allocation failed*/
+ for(i = 0; i < source->palettesize * 4; i++) dest->palette[i] = source->palette[i];
+ return 0;
+}
+
+void LodePNG_InfoRaw_init(LodePNG_InfoRaw* info)
+{
+ LodePNG_InfoColor_init(&info->color);
+}
+
+void LodePNG_InfoRaw_cleanup(LodePNG_InfoRaw* info)
+{
+ LodePNG_InfoColor_cleanup(&info->color);
+}
+
+unsigned LodePNG_InfoRaw_copy(LodePNG_InfoRaw* dest, const LodePNG_InfoRaw* source)
+{
+ unsigned error = 0;
+ LodePNG_InfoRaw_cleanup(dest);
+ *dest = *source;
+ LodePNG_InfoColor_init(&dest->color);
+ error = LodePNG_InfoColor_copy(&dest->color, &source->color);
+ return error; /*this variable could be removed, but it's more clear what is returned this way*/
+}
+
+/* ////////////////////////////////////////////////////////////////////////// */
+
+/*
+converts from any color type to 24-bit or 32-bit (later maybe more supported). return value = LodePNG error code
+the out buffer must have (w * h * bpp + 7) / 8 bytes, where bpp is the bits per pixel of the output color type (LodePNG_InfoColor_getBpp)
+for < 8 bpp images, there may _not_ be padding bits at the end of scanlines.
+*/
+unsigned LodePNG_convert(unsigned char* out, const unsigned char* in, LodePNG_InfoColor* infoOut, LodePNG_InfoColor* infoIn, unsigned w, unsigned h)
+{
+ const size_t numpixels = w * h; /*amount of pixels*/
+ const unsigned OUT_BYTES = LodePNG_InfoColor_getBpp(infoOut) / 8; /*bytes per pixel in the output image*/
+ const unsigned OUT_ALPHA = LodePNG_InfoColor_isAlphaType(infoOut); /*use 8-bit alpha channel*/
+ size_t i, c, bp = 0; /*bp = bitpointer, used by less-than-8-bit color types*/
+
+ /*cases where in and out already have the same format*/
+ if(LodePNG_InfoColor_equal(infoIn, infoOut))
+ {
+ size_t i;
+ size_t size = (w * h * LodePNG_InfoColor_getBpp(infoIn) + 7) / 8;
+ for(i = 0; i < size; i++) out[i] = in[i];
+ return 0;
+ }
+
+ if((infoOut->colorType == 2 || infoOut->colorType == 6) && infoOut->bitDepth == 8)
+ {
+ if(infoIn->bitDepth == 8)
+ {
+ switch(infoIn->colorType)
+ {
+ case 0: /*greyscale color*/
+ for(i = 0; i < numpixels; i++)
+ {
+ if(OUT_ALPHA) out[OUT_BYTES * i + 3] = 255;
+ out[OUT_BYTES * i + 0] = in[i];
+ out[OUT_BYTES * i + 1] = in[i];
+ out[OUT_BYTES * i + 2] = in[i];
+ if(OUT_ALPHA && infoIn->key_defined && in[i] == infoIn->key_r) out[OUT_BYTES * i + 3] = 0;
+ }
+ break;
+ case 2: /*RGB color*/
+ for(i = 0; i < numpixels; i++)
+ {
+ if(OUT_ALPHA) out[OUT_BYTES * i + 3] = 255;
+ for(c = 0; c < 3; c++) out[OUT_BYTES * i + c] = in[3 * i + c];
+ if(OUT_ALPHA && infoIn->key_defined == 1 && in[3 * i + 0] == infoIn->key_r
+ && in[3 * i + 1] == infoIn->key_g && in[3 * i + 2] == infoIn->key_b)
+ {
+ out[OUT_BYTES * i + 3] = 0;
+ }
+ }
+ break;
+ case 3: /*indexed color (palette)*/
+ for(i = 0; i < numpixels; i++)
+ {
+ if(OUT_ALPHA) out[OUT_BYTES * i + 3] = 255;
+ if(in[i] >= infoIn->palettesize) return 46; /*invalid palette index*/
+ for(c = 0; c < OUT_BYTES; c++)
+ {
+ out[OUT_BYTES * i + c] = infoIn->palette[4 * in[i] + c]; /*get rgb colors from the palette*/
+ }
+ }
+ break;
+ case 4: /*greyscale with alpha*/
+ for(i = 0; i < numpixels; i++)
+ {
+ out[OUT_BYTES * i + 0] = in[2 * i + 0];
+ out[OUT_BYTES * i + 1] = in[2 * i + 0];
+ out[OUT_BYTES * i + 2] = in[2 * i + 0];
+ if(OUT_ALPHA) out[OUT_BYTES * i + 3] = in[2 * i + 1];
+ }
+ break;
+ case 6: /*RGB with alpha*/
+ for(i = 0; i < numpixels; i++)
+ {
+ for(c = 0; c < OUT_BYTES; c++) out[OUT_BYTES * i + c] = in[4 * i + c];
+ }
+ break;
+ default: break;
+ }
+ }
+ else if(infoIn->bitDepth == 16)
+ {
+ switch(infoIn->colorType)
+ {
+ case 0: /*greyscale color*/
+ for(i = 0; i < numpixels; i++)
+ {
+ if(OUT_ALPHA) out[OUT_BYTES * i + 3] = 255;
+ out[OUT_BYTES * i + 0] = in[2 * i];
+ out[OUT_BYTES * i + 1] = in[2 * i];
+ out[OUT_BYTES * i + 2] = in[2 * i];
+ if(OUT_ALPHA && infoIn->key_defined && 256U * in[i] + in[i + 1] == infoIn->key_r)
+ {
+ out[OUT_BYTES * i + 3] = 0;
+ }
+ }
+ break;
+ case 2: /*RGB color*/
+ for(i = 0; i < numpixels; i++)
+ {
+ if(OUT_ALPHA) out[OUT_BYTES * i + 3] = 255;
+ for(c = 0; c < 3; c++) out[OUT_BYTES * i + c] = in[6 * i + 2 * c];
+ if(OUT_ALPHA && infoIn->key_defined && 256U * in[6 * i + 0] + in[6 * i + 1] == infoIn->key_r
+ && 256U * in[6 * i + 2] + in[6 * i + 3] == infoIn->key_g && 256U * in[6 * i + 4] + in[6 * i + 5] == infoIn->key_b)
+ {
+ out[OUT_BYTES * i + 3] = 0;
+ }
+ }
+ break;
+ case 4: /*greyscale with alpha*/
+ for(i = 0; i < numpixels; i++)
+ {
+ out[OUT_BYTES * i + 0] = in[4 * i];
+ out[OUT_BYTES * i + 1] = in[4 * i];
+ out[OUT_BYTES * i + 2] = in[4 * i];
+ if(OUT_ALPHA) out[OUT_BYTES * i + 3] = in[4 * i + 2];
+ }
+ break;
+ case 6: /*RGB with alpha*/
+ for(i = 0; i < numpixels; i++)
+ {
+ for(c = 0; c < OUT_BYTES; c++) out[OUT_BYTES * i + c] = in[8 * i + 2 * c];
+ }
+ break;
+ default: break;
+ }
+ }
+ else /*infoIn->bitDepth is less than 8 bit per channel*/
+ {
+ switch(infoIn->colorType)
+ {
+ case 0: /*greyscale color*/
+ for(i = 0; i < numpixels; i++)
+ {
+ unsigned value = readBitsFromReversedStream(&bp, in, infoIn->bitDepth);
+ if(OUT_ALPHA) out[OUT_BYTES * i + 3] = 255;
+ if(OUT_ALPHA && infoIn->key_defined && value
+ && ((1U << infoIn->bitDepth) - 1U) == infoIn->key_r
+ && ((1U << infoIn->bitDepth) - 1U))
+ {
+ out[OUT_BYTES * i + 3] = 0;
+ }
+ value = (value * 255) / ((1 << infoIn->bitDepth) - 1); /*scale value from 0 to 255*/
+ out[OUT_BYTES * i + 0] = (unsigned char)(value);
+ out[OUT_BYTES * i + 1] = (unsigned char)(value);
+ out[OUT_BYTES * i + 2] = (unsigned char)(value);
+ }
+ break;
+ case 3: /*indexed color (palette)*/
+ for(i = 0; i < numpixels; i++)
+ {
+ unsigned value = readBitsFromReversedStream(&bp, in, infoIn->bitDepth);
+ if(OUT_ALPHA) out[OUT_BYTES * i + 3] = 255;
+ if(value >= infoIn->palettesize) return 47;
+ for(c = 0; c < OUT_BYTES; c++)
+ {
+ out[OUT_BYTES * i + c] = infoIn->palette[4 * value + c]; /*get rgb colors from the palette*/
+ }
+ }
+ break;
+ default: break;
+ }
+ }
+ }
+ else if(LodePNG_InfoColor_isGreyscaleType(infoOut) && infoOut->bitDepth == 8) /*conversion from greyscale to greyscale*/
+ {
+ if(!LodePNG_InfoColor_isGreyscaleType(infoIn)) return 62;
+ if(infoIn->bitDepth == 8)
+ {
+ switch(infoIn->colorType)
+ {
+ case 0: /*greyscale color*/
+ for(i = 0; i < numpixels; i++)
+ {
+ if(OUT_ALPHA) out[OUT_BYTES * i + 1] = 255;
+ out[OUT_BYTES * i] = in[i];
+ if(OUT_ALPHA && infoIn->key_defined && in[i] == infoIn->key_r) out[OUT_BYTES * i + 1] = 0;
+ }
+ break;
+ case 4: /*greyscale with alpha*/
+ for(i = 0; i < numpixels; i++)
+ {
+ out[OUT_BYTES * i + 0] = in[2 * i + 0];
+ if(OUT_ALPHA) out[OUT_BYTES * i + 1] = in[2 * i + 1];
+ }
+ break;
+ default: return 31;
+ }
+ }
+ else if(infoIn->bitDepth == 16)
+ {
+ switch(infoIn->colorType)
+ {
+ case 0: /*greyscale color*/
+ for(i = 0; i < numpixels; i++)
+ {
+ if(OUT_ALPHA) out[OUT_BYTES * i + 1] = 255;
+ out[OUT_BYTES * i] = in[2 * i];
+ if(OUT_ALPHA && infoIn->key_defined && 256U * in[i] + in[i + 1] == infoIn->key_r)
+ {
+ out[OUT_BYTES * i + 1] = 0;
+ }
+ }
+ break;
+ case 4: /*greyscale with alpha*/
+ for(i = 0; i < numpixels; i++)
+ {
+ out[OUT_BYTES * i] = in[4 * i]; /*most significant byte*/
+ if(OUT_ALPHA) out[OUT_BYTES * i + 1] = in[4 * i + 2]; /*most significant byte*/
+ }
+ break;
+ default: return 31;
+ }
+ }
+ else /*infoIn->bitDepth is less than 8 bit per channel*/
+ {
+ if(infoIn->colorType != 0) return 31; /*colorType 0 is the only greyscale type with < 8 bits per channel*/
+ for(i = 0; i < numpixels; i++)
+ {
+ unsigned value = readBitsFromReversedStream(&bp, in, infoIn->bitDepth);
+ if(OUT_ALPHA) out[OUT_BYTES * i + 1] = 255;
+ if(OUT_ALPHA && infoIn->key_defined && value
+ && ((1U << infoIn->bitDepth) - 1U) == infoIn->key_r
+ && ((1U << infoIn->bitDepth) - 1U))
+ {
+ out[OUT_BYTES * i + 1] = 0;
+ }
+ value = (value * 255) / ((1 << infoIn->bitDepth) - 1); /*scale value from 0 to 255*/
+ out[OUT_BYTES * i] = (unsigned char)(value);
+ }
+ }
+ }
+ else return 59; /*invalid color mode*/
+
+ return 0;
+}
+
+/*
+Paeth predicter, used by PNG filter type 4
+The parameters are of type short, but should come from unsigned chars, the shorts
+are only needed to make the paeth calculation correct.
+*/
+static unsigned char paethPredictor(short a, short b, short c)
+{
+ short pa = abs(b - c);
+ short pb = abs(a - c);
+ short pc = abs(a + b - c - c);
+
+ /*short pc = a + b - c;
+ short pa = abs(pc - a);
+ short pb = abs(pc - b);
+ pc = abs(pc - c);*/
+
+ if(pa <= pb && pa <= pc) return (unsigned char)a;
+ else if(pb <= pc) return (unsigned char)b;
+ else return (unsigned char)c;
+}
+
+/*shared values used by multiple Adam7 related functions*/
+
+static const unsigned ADAM7_IX[7] = { 0, 4, 0, 2, 0, 1, 0 }; /*x start values*/
+static const unsigned ADAM7_IY[7] = { 0, 0, 4, 0, 2, 0, 1 }; /*y start values*/
+static const unsigned ADAM7_DX[7] = { 8, 8, 4, 4, 2, 2, 1 }; /*x delta values*/
+static const unsigned ADAM7_DY[7] = { 8, 8, 8, 4, 4, 2, 2 }; /*y delta values*/
+
+static void Adam7_getpassvalues(unsigned passw[7], unsigned passh[7], size_t filter_passstart[8], size_t padded_passstart[8], size_t passstart[8], unsigned w, unsigned h, unsigned bpp)
+{
+ /*the passstart values have 8 values: the 8th one actually indicates the byte after the end of the 7th (= last) pass*/
+ unsigned i;
+
+ /*calculate width and height in pixels of each pass*/
+ for(i = 0; i < 7; i++)
+ {
+ passw[i] = (w + ADAM7_DX[i] - ADAM7_IX[i] - 1) / ADAM7_DX[i];
+ passh[i] = (h + ADAM7_DY[i] - ADAM7_IY[i] - 1) / ADAM7_DY[i];
+ if(passw[i] == 0) passh[i] = 0;
+ if(passh[i] == 0) passw[i] = 0;
+ }
+
+ filter_passstart[0] = padded_passstart[0] = passstart[0] = 0;
+ for(i = 0; i < 7; i++)
+ {
+ filter_passstart[i + 1] = filter_passstart[i] + ((passw[i] && passh[i]) ? passh[i] * (1 + (passw[i] * bpp + 7) / 8) : 0); /*if passw[i] is 0, it's 0 bytes, not 1 (no filtertype-byte)*/
+ padded_passstart[i + 1] = padded_passstart[i] + passh[i] * ((passw[i] * bpp + 7) / 8); /*bits padded if needed to fill full byte at end of each scanline*/
+ passstart[i + 1] = passstart[i] + (passh[i] * passw[i] * bpp + 7) / 8; /*only padded at end of reduced image*/
+ }
+}
+
+#ifdef LODEPNG_COMPILE_DECODER
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* / PNG Decoder / */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+/*read the information from the header and store it in the LodePNG_Info. return value is error*/
+void LodePNG_Decoder_inspect(LodePNG_Decoder* decoder, const unsigned char* in, size_t inlength)
+{
+ if(inlength == 0 || in == 0)
+ {
+ decoder->error = 48; /*the given data is empty*/
+ return;
+ }
+ if(inlength < 29)
+ {
+ decoder->error = 27; /*error: the data length is smaller than the length of a PNG header*/
+ return;
+ }
+
+ /*when decoding a new PNG image, make sure all parameters created after previous decoding are reset*/
+ LodePNG_InfoPng_cleanup(&decoder->infoPng);
+ LodePNG_InfoPng_init(&decoder->infoPng);
+ decoder->error = 0;
+
+ if(in[0] != 137 || in[1] != 80 || in[2] != 78 || in[3] != 71 || in[4] != 13 || in[5] != 10 || in[6] != 26 || in[7] != 10)
+ {
+ decoder->error = 28; /*error: the first 8 bytes are not the correct PNG signature*/
+ return;
+ }
+ if(in[12] != 'I' || in[13] != 'H' || in[14] != 'D' || in[15] != 'R')
+ {
+ decoder->error = 29; /*error: it doesn't start with a IHDR chunk!*/
+ return;
+ }
+
+ /*read the values given in the header*/
+ decoder->infoPng.width = LodePNG_read32bitInt(&in[16]);
+ decoder->infoPng.height = LodePNG_read32bitInt(&in[20]);
+ decoder->infoPng.color.bitDepth = in[24];
+ decoder->infoPng.color.colorType = in[25];
+ decoder->infoPng.compressionMethod = in[26];
+ decoder->infoPng.filterMethod = in[27];
+ decoder->infoPng.interlaceMethod = in[28];
+
+ if(!decoder->settings.ignoreCrc)
+ {
+ unsigned CRC = LodePNG_read32bitInt(&in[29]);
+ unsigned checksum = Crc32_crc(&in[12], 17);
+ if(CRC != checksum)
+ {
+ decoder->error = 57; /*invalid CRC*/
+ return;
+ }
+ }
+
+ if(decoder->infoPng.compressionMethod != 0) { decoder->error = 32; return; } /*error: only compression method 0 is allowed in the specification*/
+ if(decoder->infoPng.filterMethod != 0) { decoder->error = 33; return; } /*error: only filter method 0 is allowed in the specification*/
+ if(decoder->infoPng.interlaceMethod > 1) { decoder->error = 34; return; } /*error: only interlace methods 0 and 1 exist in the specification*/
+
+ decoder->error = checkColorValidity(decoder->infoPng.color.colorType, decoder->infoPng.color.bitDepth);
+}
+
+static unsigned unfilterScanline(unsigned char* recon, const unsigned char* scanline, const unsigned char* precon, size_t bytewidth, unsigned char filterType, size_t length)
+{
+ /*
+ For PNG filter method 0
+ unfilter a PNG image scanline by scanline. when the pixels are smaller than 1 byte, the filter works byte per byte (bytewidth = 1)
+ precon is the previous unfiltered scanline, recon the result, scanline the current one
+ the incoming scanlines do NOT include the filtertype byte, that one is given in the parameter filterType instead
+ recon and scanline MAY be the same memory address! precon must be disjoint.
+ */
+
+ size_t i;
+ switch(filterType)
+ {
+ case 0:
+ for(i = 0; i < length; i++) recon[i] = scanline[i];
+ break;
+ case 1:
+ for(i = 0; i < bytewidth; i++) recon[i] = scanline[i];
+ for(i = bytewidth; i < length; i++) recon[i] = scanline[i] + recon[i - bytewidth];
+ break;
+ case 2:
+ if(precon)
+ {
+ for(i = 0; i < length; i++) recon[i] = scanline[i] + precon[i];
+ }
+ else
+ {
+ for(i = 0; i < length; i++) recon[i] = scanline[i];
+ }
+ break;
+ case 3:
+ if(precon)
+ {
+ for(i = 0; i < bytewidth; i++) recon[i] = scanline[i] + precon[i] / 2;
+ for(i = bytewidth; i < length; i++) recon[i] = scanline[i] + ((recon[i - bytewidth] + precon[i]) / 2);
+ }
+ else
+ {
+ for(i = 0; i < bytewidth; i++) recon[i] = scanline[i];
+ for(i = bytewidth; i < length; i++) recon[i] = scanline[i] + recon[i - bytewidth] / 2;
+ }
+ break;
+ case 4:
+ if(precon)
+ {
+ for(i = 0; i < bytewidth; i++)
+ {
+ recon[i] = (scanline[i] + precon[i]); /*paethPredictor(0, precon[i], 0) is always precon[i]*/
+ }
+ for(i = bytewidth; i < length; i++)
+ {
+ recon[i] = (scanline[i] + paethPredictor(recon[i - bytewidth], precon[i], precon[i - bytewidth]));
+ }
+ }
+ else
+ {
+ for(i = 0; i < bytewidth; i++)
+ {
+ recon[i] = scanline[i];
+ }
+ for(i = bytewidth; i < length; i++)
+ {
+ recon[i] = (scanline[i] + recon[i - bytewidth]); /*paethPredictor(recon[i - bytewidth], 0, 0) is always recon[i - bytewidth]*/
+ }
+ }
+ break;
+ default: return 36; /*error: unexisting filter type given*/
+ }
+ return 0;
+}
+
+static unsigned unfilter(unsigned char* out, const unsigned char* in, unsigned w, unsigned h, unsigned bpp)
+{
+ /*
+ For PNG filter method 0
+ this function unfilters a single image (e.g. without interlacing this is called once, with Adam7 it's called 7 times)
+ out must have enough bytes allocated already, in must have the scanlines + 1 filtertype byte per scanline
+ w and h are image dimensions or dimensions of reduced image, bpp is bits per pixel
+ in and out are allowed to be the same memory address (but are not the same size because in has the extra filter bytes)
+ */
+
+ unsigned y;
+ unsigned char* prevline = 0;
+
+ size_t bytewidth = (bpp + 7) / 8; /*bytewidth is used for filtering, is 1 when bpp < 8, number of bytes per pixel otherwise*/
+ size_t linebytes = (w * bpp + 7) / 8;
+
+ for(y = 0; y < h; y++)
+ {
+ size_t outindex = linebytes * y;
+ size_t inindex = (1 + linebytes) * y; /*the extra filterbyte added to each row*/
+ unsigned char filterType = in[inindex];
+
+ unsigned error = unfilterScanline(&out[outindex], &in[inindex + 1], prevline, bytewidth, filterType, linebytes);
+ if(error) return error;
+
+ prevline = &out[outindex];
+ }
+
+ return 0;
+}
+
+static void Adam7_deinterlace(unsigned char* out, const unsigned char* in, unsigned w, unsigned h, unsigned bpp)
+{
+ /*Note: this function works on image buffers WITHOUT padding bits at end of scanlines with non-multiple-of-8 bit amounts, only between reduced images is padding
+ out must be big enough AND must be 0 everywhere if bpp < 8 in the current implementation (because that's likely a little bit faster)*/
+ unsigned passw[7], passh[7];
+ size_t filter_passstart[8], padded_passstart[8], passstart[8];
+ unsigned i;
+
+ Adam7_getpassvalues(passw, passh, filter_passstart, padded_passstart, passstart, w, h, bpp);
+
+ if(bpp >= 8)
+ {
+ for(i = 0; i < 7; i++)
+ {
+ unsigned x, y, b;
+ size_t bytewidth = bpp / 8;
+ for(y = 0; y < passh[i]; y++)
+ for(x = 0; x < passw[i]; x++)
+ {
+ size_t pixelinstart = passstart[i] + (y * passw[i] + x) * bytewidth;
+ size_t pixeloutstart = ((ADAM7_IY[i] + y * ADAM7_DY[i]) * w + ADAM7_IX[i] + x * ADAM7_DX[i]) * bytewidth;
+ for(b = 0; b < bytewidth; b++)
+ {
+ out[pixeloutstart + b] = in[pixelinstart + b];
+ }
+ }
+ }
+ }
+ else /*bpp < 8: Adam7 with pixels < 8 bit is a bit trickier: with bit pointers*/
+ {
+ for(i = 0; i < 7; i++)
+ {
+ unsigned x, y, b;
+ unsigned ilinebits = bpp * passw[i];
+ unsigned olinebits = bpp * w;
+ size_t obp, ibp; /*bit pointers (for out and in buffer)*/
+ for(y = 0; y < passh[i]; y++)
+ for(x = 0; x < passw[i]; x++)
+ {
+ ibp = (8 * passstart[i]) + (y * ilinebits + x * bpp);
+ obp = (ADAM7_IY[i] + y * ADAM7_DY[i]) * olinebits + (ADAM7_IX[i] + x * ADAM7_DX[i]) * bpp;
+ for(b = 0; b < bpp; b++)
+ {
+ unsigned char bit = readBitFromReversedStream(&ibp, in);
+ setBitOfReversedStream0(&obp, out, bit); /*note that this function assumes the out buffer is completely 0, use setBitOfReversedStream otherwise*/
+ }
+ }
+ }
+ }
+}
+
+static void removePaddingBits(unsigned char* out, const unsigned char* in, size_t olinebits, size_t ilinebits, unsigned h)
+{
+ /*
+ After filtering there are still padding bits if scanlines have non multiple of 8 bit amounts. They need to be removed (except at last scanline of (Adam7-reduced) image) before working with pure image buffers for the Adam7 code, the color convert code and the output to the user.
+ in and out are allowed to be the same buffer, in may also be higher but still overlapping; in must have >= ilinebits*h bits, out must have >= olinebits*h bits, olinebits must be <= ilinebits
+ also used to move bits after earlier such operations happened, e.g. in a sequence of reduced images from Adam7
+ only useful if (ilinebits - olinebits) is a value in the range 1..7
+ */
+ unsigned y;
+ size_t diff = ilinebits - olinebits;
+ size_t ibp = 0, obp = 0; /*input and output bit pointers*/
+ for(y = 0; y < h; y++)
+ {
+ size_t x;
+ for(x = 0; x < olinebits; x++)
+ {
+ unsigned char bit = readBitFromReversedStream(&ibp, in);
+ setBitOfReversedStream(&obp, out, bit);
+ }
+ ibp += diff;
+ }
+}
+
+/*out must be buffer big enough to contain full image, and in must contain the full decompressed data from the IDAT chunks (with filter index bytes and possible padding bits)*/
+static unsigned postProcessScanlines(unsigned char* out, unsigned char* in, const LodePNG_InfoPng* infoPng) /*return value is error*/
+{
+ /*
+ This function converts the filtered-padded-interlaced data into pure 2D image buffer with the PNG's colortype. Steps:
+ *) if no Adam7: 1) unfilter 2) remove padding bits (= posible extra bits per scanline if bpp < 8)
+ *) if adam7: 1) 7x unfilter 2) 7x remove padding bits 3) Adam7_deinterlace
+ NOTE: the in buffer will be overwritten with intermediate data!
+ */
+ unsigned bpp = LodePNG_InfoColor_getBpp(&infoPng->color);
+ unsigned w = infoPng->width;
+ unsigned h = infoPng->height;
+ unsigned error = 0;
+ if(bpp == 0) return 31; /*error: invalid colortype*/
+
+ if(infoPng->interlaceMethod == 0)
+ {
+ if(bpp < 8 && w * bpp != ((w * bpp + 7) / 8) * 8)
+ {
+ error = unfilter(in, in, w, h, bpp);
+ if(error) return error;
+ removePaddingBits(out, in, w * bpp, ((w * bpp + 7) / 8) * 8, h);
+ }
+ else error = unfilter(out, in, w, h, bpp); /*we can immediatly filter into the out buffer, no other steps needed*/
+ }
+ else /*interlaceMethod is 1 (Adam7)*/
+ {
+ unsigned passw[7], passh[7]; size_t filter_passstart[8], padded_passstart[8], passstart[8];
+ unsigned i;
+
+ Adam7_getpassvalues(passw, passh, filter_passstart, padded_passstart, passstart, w, h, bpp);
+
+ for(i = 0; i < 7; i++)
+ {
+ error = unfilter(&in[padded_passstart[i]], &in[filter_passstart[i]], passw[i], passh[i], bpp);
+ if(error) return error;
+ if(bpp < 8) /*TODO: possible efficiency improvement: if in this reduced image the bits fit nicely in 1 scanline, move bytes instead of bits or move not at all*/
+ {
+ /*remove padding bits in scanlines; after this there still may be padding bits between the different reduced images: each reduced image still starts nicely at a byte*/
+ removePaddingBits(&in[passstart[i]], &in[padded_passstart[i]], passw[i] * bpp, ((passw[i] * bpp + 7) / 8) * 8, passh[i]);
+ }
+ }
+
+ Adam7_deinterlace(out, in, w, h, bpp);
+ }
+
+ return error;
+}
+
+/*read a PNG, the result will be in the same color type as the PNG (hence "generic")*/
+static void decodeGeneric(LodePNG_Decoder* decoder, unsigned char** out, size_t* outsize, const unsigned char* in, size_t insize)
+{
+ unsigned char IEND = 0;
+ const unsigned char* chunk;
+ size_t i;
+ ucvector idat; /*the data from idat chunks*/
+
+ /*for unknown chunk order*/
+ unsigned unknown = 0;
+ unsigned critical_pos = 1; /*1 = after IHDR, 2 = after PLTE, 3 = after IDAT*/
+
+ /*provide some proper output values if error will happen*/
+ *out = 0;
+ *outsize = 0;
+
+ LodePNG_Decoder_inspect(decoder, in, insize); /*reads header and resets other parameters in decoder->infoPng*/
+ if(decoder->error) return;
+
+ ucvector_init(&idat);
+
+ chunk = &in[33]; /*first byte of the first chunk after the header*/
+
+ while(!IEND) /*loop through the chunks, ignoring unknown chunks and stopping at IEND chunk. IDAT data is put at the start of the in buffer*/
+ {
+ unsigned chunkLength;
+ const unsigned char* data; /*the data in the chunk*/
+
+ if((size_t)((chunk - in) + 12) > insize || chunk < in)
+ {
+ decoder->error = 30; /*error: size of the in buffer too small to contain next chunk*/
+ break;
+ }
+ chunkLength = LodePNG_chunk_length(chunk); /*length of the data of the chunk, excluding the length bytes, chunk type and CRC bytes*/
+ if(chunkLength > 2147483647)
+ {
+ decoder->error = 63; /*chunk length larger than the max PNG chunk size*/
+ break;
+ }
+ if((size_t)((chunk - in) + chunkLength + 12) > insize || (chunk + chunkLength + 12) < in)
+ {
+ decoder->error = 35; /*error: size of the in buffer too small to contain next chunk*/
+ break;
+ }
+ data = LodePNG_chunk_data_const(chunk);
+
+ /*IDAT chunk, containing compressed image data*/
+ if(LodePNG_chunk_type_equals(chunk, "IDAT"))
+ {
+ size_t oldsize = idat.size;
+ if(!ucvector_resize(&idat, oldsize + chunkLength))
+ {
+ decoder->error = 9936; /*memory allocation failed*/
+ break;
+ }
+ for(i = 0; i < chunkLength; i++) idat.data[oldsize + i] = data[i];
+ critical_pos = 3;
+ }
+ /*IEND chunk*/
+ else if(LodePNG_chunk_type_equals(chunk, "IEND"))
+ {
+ IEND = 1;
+ }
+ /*palette chunk (PLTE)*/
+ else if(LodePNG_chunk_type_equals(chunk, "PLTE"))
+ {
+ unsigned pos = 0;
+ if(decoder->infoPng.color.palette) free(decoder->infoPng.color.palette);
+ decoder->infoPng.color.palettesize = chunkLength / 3;
+ decoder->infoPng.color.palette = (unsigned char*)malloc(4 * decoder->infoPng.color.palettesize);
+ if(!decoder->infoPng.color.palette && decoder->infoPng.color.palettesize)
+ {
+ decoder->error = 9937; /*memory allocation failed*/
+ break;
+ }
+ if(!decoder->infoPng.color.palette) decoder->infoPng.color.palettesize = 0; /*malloc failed...*/
+ if(decoder->infoPng.color.palettesize > 256)
+ {
+ decoder->error = 38; /*error: palette too big*/
+ break;
+ }
+ for(i = 0; i < decoder->infoPng.color.palettesize; i++)
+ {
+ decoder->infoPng.color.palette[4 * i + 0] = data[pos++]; /*R*/
+ decoder->infoPng.color.palette[4 * i + 1] = data[pos++]; /*G*/
+ decoder->infoPng.color.palette[4 * i + 2] = data[pos++]; /*B*/
+ decoder->infoPng.color.palette[4 * i + 3] = 255; /*alpha*/
+ }
+ critical_pos = 2;
+ }
+ /*palette transparency chunk (tRNS)*/
+ else if(LodePNG_chunk_type_equals(chunk, "tRNS"))
+ {
+ if(decoder->infoPng.color.colorType == 3)
+ {
+ if(chunkLength > decoder->infoPng.color.palettesize)
+ {
+ decoder->error = 39; /*error: more alpha values given than there are palette entries*/
+ break;
+ }
+ for(i = 0; i < chunkLength; i++) decoder->infoPng.color.palette[4 * i + 3] = data[i];
+ }
+ else if(decoder->infoPng.color.colorType == 0)
+ {
+ if(chunkLength != 2)
+ {
+ decoder->error = 40; /*error: this chunk must be 2 bytes for greyscale image*/
+ break;
+ }
+ decoder->infoPng.color.key_defined = 1;
+ decoder->infoPng.color.key_r = decoder->infoPng.color.key_g = decoder->infoPng.color.key_b = 256 * data[0] + data[1];
+ }
+ else if(decoder->infoPng.color.colorType == 2)
+ {
+ if(chunkLength != 6)
+ {
+ decoder->error = 41; /*error: this chunk must be 6 bytes for RGB image*/
+ break;
+ }
+ decoder->infoPng.color.key_defined = 1;
+ decoder->infoPng.color.key_r = 256 * data[0] + data[1];
+ decoder->infoPng.color.key_g = 256 * data[2] + data[3];
+ decoder->infoPng.color.key_b = 256 * data[4] + data[5];
+ }
+ else
+ {
+ decoder->error = 42; /*error: tRNS chunk not allowed for other color models*/
+ break;
+ }
+ }
+#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS
+ /*background color chunk (bKGD)*/
+ else if(LodePNG_chunk_type_equals(chunk, "bKGD"))
+ {
+ if(decoder->infoPng.color.colorType == 3)
+ {
+ if(chunkLength != 1)
+ {
+ decoder->error = 43; /*error: this chunk must be 1 byte for indexed color image*/
+ break;
+ }
+ decoder->infoPng.background_defined = 1;
+ decoder->infoPng.background_r = decoder->infoPng.background_g = decoder->infoPng.background_b = data[0];
+ }
+ else if(decoder->infoPng.color.colorType == 0 || decoder->infoPng.color.colorType == 4)
+ {
+ if(chunkLength != 2)
+ {
+ decoder->error = 44; /*error: this chunk must be 2 bytes for greyscale image*/
+ break;
+ }
+ decoder->infoPng.background_defined = 1;
+ decoder->infoPng.background_r = decoder->infoPng.background_g = decoder->infoPng.background_b = 256 * data[0] + data[1];
+ }
+ else if(decoder->infoPng.color.colorType == 2 || decoder->infoPng.color.colorType == 6)
+ {
+ if(chunkLength != 6)
+ {
+ decoder->error = 45; /*error: this chunk must be 6 bytes for greyscale image*/
+ break;
+ }
+ decoder->infoPng.background_defined = 1;
+ decoder->infoPng.background_r = 256 * data[0] + data[1];
+ decoder->infoPng.background_g = 256 * data[2] + data[3];
+ decoder->infoPng.background_b = 256 * data[4] + data[5];
+ }
+ }
+ /*text chunk (tEXt)*/
+ else if(LodePNG_chunk_type_equals(chunk, "tEXt"))
+ {
+ if(decoder->settings.readTextChunks)
+ {
+ char *key = 0, *str = 0;
+
+ while(!decoder->error) /*not really a while loop, only used to break on error*/
+ {
+ unsigned length, string2_begin;
+
+ for(length = 0; length < chunkLength && data[length] != 0; length++) ;
+ if(length + 1 >= chunkLength)
+ {
+ decoder->error = 75; /*error, end reached, no null terminator?*/
+ break;
+ }
+ key = (char*)malloc(length + 1);
+ if(!key)
+ {
+ decoder->error = 9938; /*memory allocation failed*/
+ break;
+ }
+ key[length] = 0;
+ for(i = 0; i < length; i++) key[i] = data[i];
+
+ string2_begin = length + 1;
+ if(string2_begin > chunkLength)
+ {
+ decoder->error = 75; /*error, end reached, no null terminator?*/
+ break;
+ }
+ length = chunkLength - string2_begin;
+ str = (char*)malloc(length + 1);
+ if(!str)
+ {
+ decoder->error = 9939; /*memory allocation failed*/
+ break;
+ }
+ str[length] = 0;
+ for(i = 0; i < length; i++) str[i] = data[string2_begin + i];
+
+ decoder->error = LodePNG_Text_add(&decoder->infoPng.text, key, str);
+
+ break;
+ }
+
+ free(key);
+ free(str);
+ }
+ }
+ /*compressed text chunk (zTXt)*/
+ else if(LodePNG_chunk_type_equals(chunk, "zTXt"))
+ {
+ if(decoder->settings.readTextChunks)
+ {
+ unsigned length, string2_begin;
+ char *key = 0;
+ ucvector decoded;
+
+ ucvector_init(&decoded);
+
+ while(!decoder->error) /*not really a while loop, only used to break on error*/
+ {
+ for(length = 0; length < chunkLength && data[length] != 0; length++) ;
+ if(length + 2 >= chunkLength)
+ {
+ decoder->error = 75; /*no null termination, corrupt?*/
+ break;
+ }
+ key = (char*)malloc(length + 1);
+ if(!key)
+ {
+ decoder->error = 9940; /*memory allocation failed*/
+ break;
+ }
+ key[length] = 0;
+ for(i = 0; i < length; i++) key[i] = data[i];
+
+ if(data[length + 1] != 0)
+ {
+ decoder->error = 72; /*the 0 byte indicating compression must be 0*/
+ break;
+ }
+
+ string2_begin = length + 2;
+ if(string2_begin > chunkLength)
+ {
+ decoder->error = 75; /*no null termination, corrupt?*/
+ break;
+ }
+ length = chunkLength - string2_begin;
+ decoder->error = LodePNG_decompress(&decoded.data, &decoded.size, (unsigned char*)(&data[string2_begin]), length, &decoder->settings.zlibsettings);
+ if(decoder->error) break;
+ ucvector_push_back(&decoded, 0);
+
+ decoder->error = LodePNG_Text_add(&decoder->infoPng.text, key, (char*)decoded.data);
+
+ break;
+ }
+
+ free(key);
+ ucvector_cleanup(&decoded);
+ if(decoder->error) break;
+ }
+ }
+ /*international text chunk (iTXt)*/
+ else if(LodePNG_chunk_type_equals(chunk, "iTXt"))
+ {
+ if(decoder->settings.readTextChunks)
+ {
+ unsigned length, begin, compressed;
+ char *key = 0, *langtag = 0, *transkey = 0;
+ ucvector decoded;
+ ucvector_init(&decoded);
+
+ while(!decoder->error) /*not really a while loop, only used to break on error*/
+ {
+ /*Quick check if the chunk length isn't too small. Even without check it'd still fail with other error checks below if it's too short. This just gives a different error code.*/
+ if(chunkLength < 5)
+ {
+ decoder->error = 76; /*iTXt chunk too short*/
+ break;
+ }
+
+ /*read the key*/
+ for(length = 0; length < chunkLength && data[length] != 0; length++) ;
+ if(length + 2 >= chunkLength)
+ {
+ decoder->error = 75; /*no null termination char found*/
+ break;
+ }
+ key = (char*)malloc(length + 1);
+ if(!key)
+ {
+ decoder->error = 9941; /*memory allocation failed*/
+ break;
+ }
+ key[length] = 0;
+ for(i = 0; i < length; i++) key[i] = data[i];
+
+ /*read the compression method*/
+ compressed = data[length + 1];
+ if(data[length + 2] != 0)
+ {
+ decoder->error = 72; /*the 0 byte indicating compression must be 0*/
+ break;
+ }
+
+ /*read the langtag*/
+ begin = length + 3;
+ length = 0;
+ for(i = begin; i < chunkLength && data[i] != 0; i++) length++;
+ if(begin + length + 1 >= chunkLength)
+ {
+ decoder->error = 75; /*no null termination char found*/
+ break;
+ }
+ langtag = (char*)malloc(length + 1);
+ if(!langtag)
+ {
+ decoder->error = 9942; /*memory allocation failed*/
+ break;
+ }
+ langtag[length] = 0;
+ for(i = 0; i < length; i++) langtag[i] = data[begin + i];
+
+ /*read the transkey*/
+ begin += length + 1;
+ length = 0;
+ for(i = begin; i < chunkLength && data[i] != 0; i++) length++;
+ if(begin + length + 1 >= chunkLength)
+ {
+ decoder->error = 75; /*no null termination, corrupt?*/
+ break;
+ }
+ transkey = (char*)malloc(length + 1);
+ if(!transkey)
+ {
+ decoder->error = 9943; /*memory allocation failed*/
+ break;
+ }
+ transkey[length] = 0;
+ for(i = 0; i < length; i++) transkey[i] = data[begin + i];
+
+ /*read the actual text*/
+ begin += length + 1;
+ if(begin > chunkLength)
+ {
+ decoder->error = 75; /*no null termination, corrupt?*/
+ break;
+ }
+ length = chunkLength - begin;
+
+ if(compressed)
+ {
+ decoder->error = LodePNG_decompress(&decoded.data, &decoded.size, (unsigned char*)(&data[begin]), length, &decoder->settings.zlibsettings);
+ if(decoder->error) break;
+ ucvector_push_back(&decoded, 0);
+ }
+ else
+ {
+ if(!ucvector_resize(&decoded, length + 1))
+ {
+ decoder->error = 9944; /*memory allocation failed*/
+ break;
+ }
+ decoded.data[length] = 0;
+ for(i = 0; i < length; i++) decoded.data[i] = data[begin + i];
+ }
+
+ decoder->error = LodePNG_IText_add(&decoder->infoPng.itext, key, langtag, transkey, (char*)decoded.data);
+
+ break;
+ }
+
+ free(key);
+ free(langtag);
+ free(transkey);
+ ucvector_cleanup(&decoded);
+ if(decoder->error) break;
+ }
+ }
+ else if(LodePNG_chunk_type_equals(chunk, "tIME"))
+ {
+ if(chunkLength != 7)
+ {
+ decoder->error = 73; /*invalid tIME chunk size*/
+ break;
+ }
+ decoder->infoPng.time_defined = 1;
+ decoder->infoPng.time.year = 256 * data[0] + data[+ 1];
+ decoder->infoPng.time.month = data[2];
+ decoder->infoPng.time.day = data[3];
+ decoder->infoPng.time.hour = data[4];
+ decoder->infoPng.time.minute = data[5];
+ decoder->infoPng.time.second = data[6];
+ }
+ else if(LodePNG_chunk_type_equals(chunk, "pHYs"))
+ {
+ if(chunkLength != 9)
+ {
+ decoder->error = 74; /*invalid pHYs chunk size*/
+ break;
+ }
+ decoder->infoPng.phys_defined = 1;
+ decoder->infoPng.phys_x = 16777216 * data[0] + 65536 * data[1] + 256 * data[2] + data[3];
+ decoder->infoPng.phys_y = 16777216 * data[4] + 65536 * data[5] + 256 * data[6] + data[7];
+ decoder->infoPng.phys_unit = data[8];
+ }
+#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/
+ else /*it's not an implemented chunk type, so ignore it: skip over the data*/
+ {
+ if(LodePNG_chunk_critical(chunk))
+ {
+ decoder->error = 69; /*error: unknown critical chunk (5th bit of first byte of chunk type is 0)*/
+ break;
+ }
+ unknown = 1;
+#ifdef LODEPNG_COMPILE_UNKNOWN_CHUNKS
+ if(decoder->settings.rememberUnknownChunks)
+ {
+ LodePNG_UnknownChunks* unknown = &decoder->infoPng.unknown_chunks;
+ decoder->error = LodePNG_append_chunk(&unknown->data[critical_pos - 1], &unknown->datasize[critical_pos - 1], chunk);
+ if(decoder->error) break;
+ }
+#endif /*LODEPNG_COMPILE_UNKNOWN_CHUNKS*/
+ }
+
+ if(!decoder->settings.ignoreCrc && !unknown) /*check CRC if wanted, only on known chunk types*/
+ {
+ if(LodePNG_chunk_check_crc(chunk))
+ {
+ decoder->error = 57; /*invalid CRC*/
+ break;
+ }
+ }
+
+ if(!IEND) chunk = LodePNG_chunk_next_const(chunk);
+ }
+
+ if(!decoder->error)
+ {
+ ucvector scanlines;
+ ucvector_init(&scanlines);
+ if(!ucvector_resize(&scanlines, ((decoder->infoPng.width * (decoder->infoPng.height * LodePNG_InfoColor_getBpp(&decoder->infoPng.color) + 7)) / 8) + decoder->infoPng.height)) decoder->error = 9945; /*maximum final image length is already reserved in the vector's length - this is not really necessary*/
+ if(!decoder->error)
+ {
+ decoder->error = LodePNG_decompress(&scanlines.data, &scanlines.size, idat.data, idat.size, &decoder->settings.zlibsettings); /*decompress with the Zlib decompressor*/
+ }
+
+ if(!decoder->error)
+ {
+ ucvector outv;
+ ucvector_init(&outv);
+ if(!ucvector_resizev(&outv, (decoder->infoPng.height * decoder->infoPng.width * LodePNG_InfoColor_getBpp(&decoder->infoPng.color) + 7) / 8, 0)) decoder->error = 9946;
+ if(!decoder->error) decoder->error = postProcessScanlines(outv.data, scanlines.data, &decoder->infoPng);
+ *out = outv.data;
+ *outsize = outv.size;
+ }
+ ucvector_cleanup(&scanlines);
+ }
+
+ ucvector_cleanup(&idat);
+}
+
+void LodePNG_Decoder_decode(LodePNG_Decoder* decoder, unsigned char** out, size_t* outsize, const unsigned char* in, size_t insize)
+{
+ *out = 0;
+ *outsize = 0;
+ decodeGeneric(decoder, out, outsize, in, insize);
+ if(decoder->error) return;
+ if(!decoder->settings.color_convert || LodePNG_InfoColor_equal(&decoder->infoRaw.color, &decoder->infoPng.color))
+ {
+ /*same color type, no copying or converting of data needed*/
+ /*store the infoPng color settings on the infoRaw so that the infoRaw still reflects what colorType
+ the raw image has to the end user*/
+ if(!decoder->settings.color_convert)
+ {
+ decoder->error = LodePNG_InfoColor_copy(&decoder->infoRaw.color, &decoder->infoPng.color);
+ if(decoder->error) return;
+ }
+ }
+ else
+ {
+ /*color conversion needed; sort of copy of the data*/
+ unsigned char* data = *out;
+
+ /*TODO: check if this works according to the statement in the documentation: "The converter can convert from greyscale input color type, to 8-bit greyscale or greyscale with alpha"*/
+ if(!(decoder->infoRaw.color.colorType == 2 || decoder->infoRaw.color.colorType == 6) && !(decoder->infoRaw.color.bitDepth == 8))
+ {
+ decoder->error = 56; /*unsupported color mode conversion*/
+ return;
+ }
+
+ *outsize = (decoder->infoPng.width * decoder->infoPng.height * LodePNG_InfoColor_getBpp(&decoder->infoRaw.color) + 7) / 8;
+ *out = (unsigned char*)malloc(*outsize);
+ if(!(*out))
+ {
+ decoder->error = 9947; /*memory allocation failed*/
+ *outsize = 0;
+ }
+ else decoder->error = LodePNG_convert(*out, data, &decoder->infoRaw.color, &decoder->infoPng.color, decoder->infoPng.width, decoder->infoPng.height);
+ free(data);
+ }
+}
+
+unsigned LodePNG_decode(unsigned char** out, unsigned* w, unsigned* h, const unsigned char* in, size_t insize, unsigned colorType, unsigned bitDepth)
+{
+ unsigned error;
+ size_t dummy_size;
+ LodePNG_Decoder decoder;
+ LodePNG_Decoder_init(&decoder);
+ decoder.infoRaw.color.colorType = colorType;
+ decoder.infoRaw.color.bitDepth = bitDepth;
+ LodePNG_Decoder_decode(&decoder, out, &dummy_size, in, insize);
+ error = decoder.error;
+ *w = decoder.infoPng.width;
+ *h = decoder.infoPng.height;
+ LodePNG_Decoder_cleanup(&decoder);
+ return error;
+}
+
+unsigned LodePNG_decode32(unsigned char** out, unsigned* w, unsigned* h, const unsigned char* in, size_t insize)
+{
+ return LodePNG_decode(out, w, h, in, insize, 6, 8);
+}
+
+#ifdef LODEPNG_COMPILE_DISK
+unsigned LodePNG_decode_file(unsigned char** out, unsigned* w, unsigned* h, const char* filename, unsigned colorType, unsigned bitDepth)
+{
+ unsigned char* buffer;
+ size_t buffersize;
+ unsigned error;
+ error = LodePNG_loadFile(&buffer, &buffersize, filename);
+ if(!error) error = LodePNG_decode(out, w, h, buffer, buffersize, colorType, bitDepth);
+ free(buffer);
+ return error;
+}
+
+unsigned LodePNG_decode32_file(unsigned char** out, unsigned* w, unsigned* h, const char* filename)
+{
+ return LodePNG_decode_file(out, w, h, filename, 6, 8);
+}
+#endif /*LODEPNG_COMPILE_DISK*/
+
+void LodePNG_DecodeSettings_init(LodePNG_DecodeSettings* settings)
+{
+ settings->color_convert = 1;
+#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS
+ settings->readTextChunks = 1;
+#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/
+ settings->ignoreCrc = 0;
+#ifdef LODEPNG_COMPILE_UNKNOWN_CHUNKS
+ settings->rememberUnknownChunks = 0;
+#endif /*LODEPNG_COMPILE_UNKNOWN_CHUNKS*/
+ LodeZlib_DecompressSettings_init(&settings->zlibsettings);
+}
+
+void LodePNG_Decoder_init(LodePNG_Decoder* decoder)
+{
+ LodePNG_DecodeSettings_init(&decoder->settings);
+ LodePNG_InfoRaw_init(&decoder->infoRaw);
+ LodePNG_InfoPng_init(&decoder->infoPng);
+ decoder->error = 1;
+}
+
+void LodePNG_Decoder_cleanup(LodePNG_Decoder* decoder)
+{
+ LodePNG_InfoRaw_cleanup(&decoder->infoRaw);
+ LodePNG_InfoPng_cleanup(&decoder->infoPng);
+}
+
+void LodePNG_Decoder_copy(LodePNG_Decoder* dest, const LodePNG_Decoder* source)
+{
+ LodePNG_Decoder_cleanup(dest);
+ *dest = *source;
+ LodePNG_InfoRaw_init(&dest->infoRaw);
+ LodePNG_InfoPng_init(&dest->infoPng);
+ dest->error = LodePNG_InfoRaw_copy(&dest->infoRaw, &source->infoRaw); if(dest->error) return;
+ dest->error = LodePNG_InfoPng_copy(&dest->infoPng, &source->infoPng); if(dest->error) return;
+}
+
+#endif /*LODEPNG_COMPILE_DECODER*/
+
+#ifdef LODEPNG_COMPILE_ENCODER
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* / PNG Encoder / */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+/*chunkName must be string of 4 characters*/
+static unsigned addChunk(ucvector* out, const char* chunkName, const unsigned char* data, size_t length)
+{
+ unsigned error = LodePNG_create_chunk(&out->data, &out->size, (unsigned)length, chunkName, data);
+ if(error) return error;
+ out->allocsize = out->size; /*fix the allocsize again*/
+ return 0;
+}
+
+static void writeSignature(ucvector* out)
+{
+ /*8 bytes PNG signature, aka the magic bytes*/
+ ucvector_push_back(out, 137);
+ ucvector_push_back(out, 80);
+ ucvector_push_back(out, 78);
+ ucvector_push_back(out, 71);
+ ucvector_push_back(out, 13);
+ ucvector_push_back(out, 10);
+ ucvector_push_back(out, 26);
+ ucvector_push_back(out, 10);
+}
+
+static unsigned addChunk_IHDR(ucvector* out, unsigned w, unsigned h, unsigned bitDepth, unsigned colorType, unsigned interlaceMethod)
+{
+ unsigned error = 0;
+ ucvector header;
+ ucvector_init(&header);
+
+ LodePNG_add32bitInt(&header, w); /*width*/
+ LodePNG_add32bitInt(&header, h); /*height*/
+ ucvector_push_back(&header, (unsigned char)bitDepth); /*bit depth*/
+ ucvector_push_back(&header, (unsigned char)colorType); /*color type*/
+ ucvector_push_back(&header, 0); /*compression method*/
+ ucvector_push_back(&header, 0); /*filter method*/
+ ucvector_push_back(&header, interlaceMethod); /*interlace method*/
+
+ error = addChunk(out, "IHDR", header.data, header.size);
+ ucvector_cleanup(&header);
+
+ return error;
+}
+
+static unsigned addChunk_PLTE(ucvector* out, const LodePNG_InfoColor* info)
+{
+ unsigned error = 0;
+ size_t i;
+ ucvector PLTE;
+ ucvector_init(&PLTE);
+ for(i = 0; i < info->palettesize * 4; i++)
+ {
+ if(i % 4 != 3) ucvector_push_back(&PLTE, info->palette[i]); /*add all channels except alpha channel*/
+ }
+ error = addChunk(out, "PLTE", PLTE.data, PLTE.size);
+ ucvector_cleanup(&PLTE);
+
+ return error;
+}
+
+static unsigned addChunk_tRNS(ucvector* out, const LodePNG_InfoColor* info)
+{
+ unsigned error = 0;
+ size_t i;
+ ucvector tRNS;
+ ucvector_init(&tRNS);
+ if(info->colorType == 3)
+ {
+ for(i = 0; i < info->palettesize; i++) ucvector_push_back(&tRNS, info->palette[4 * i + 3]); /*add only alpha channel*/
+ }
+ else if(info->colorType == 0)
+ {
+ if(info->key_defined)
+ {
+ ucvector_push_back(&tRNS, (unsigned char)(info->key_r / 256));
+ ucvector_push_back(&tRNS, (unsigned char)(info->key_r % 256));
+ }
+ }
+ else if(info->colorType == 2)
+ {
+ if(info->key_defined)
+ {
+ ucvector_push_back(&tRNS, (unsigned char)(info->key_r / 256));
+ ucvector_push_back(&tRNS, (unsigned char)(info->key_r % 256));
+ ucvector_push_back(&tRNS, (unsigned char)(info->key_g / 256));
+ ucvector_push_back(&tRNS, (unsigned char)(info->key_g % 256));
+ ucvector_push_back(&tRNS, (unsigned char)(info->key_b / 256));
+ ucvector_push_back(&tRNS, (unsigned char)(info->key_b % 256));
+ }
+ }
+
+ error = addChunk(out, "tRNS", tRNS.data, tRNS.size);
+ ucvector_cleanup(&tRNS);
+
+ return error;
+}
+
+static unsigned addChunk_IDAT(ucvector* out, const unsigned char* data, size_t datasize, LodeZlib_CompressSettings* zlibsettings)
+{
+ ucvector zlibdata;
+ unsigned error = 0;
+
+ /*compress with the Zlib compressor*/
+ ucvector_init(&zlibdata);
+ error = LodePNG_compress(&zlibdata.data, &zlibdata.size, data, datasize, zlibsettings);
+ if(!error) error = addChunk(out, "IDAT", zlibdata.data, zlibdata.size);
+ ucvector_cleanup(&zlibdata);
+
+ return error;
+}
+
+static unsigned addChunk_IEND(ucvector* out)
+{
+ unsigned error = 0;
+ error = addChunk(out, "IEND", 0, 0);
+ return error;
+}
+
+#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS
+
+static unsigned addChunk_tEXt(ucvector* out, const char* keyword, const char* textstring) /*add text chunk*/
+{
+ unsigned error = 0;
+ size_t i;
+ ucvector text;
+ ucvector_init(&text);
+ for(i = 0; keyword[i] != 0; i++) ucvector_push_back(&text, (unsigned char)keyword[i]);
+ ucvector_push_back(&text, 0);
+ for(i = 0; textstring[i] != 0; i++) ucvector_push_back(&text, (unsigned char)textstring[i]);
+ error = addChunk(out, "tEXt", text.data, text.size);
+ ucvector_cleanup(&text);
+
+ return error;
+}
+
+static unsigned addChunk_zTXt(ucvector* out, const char* keyword, const char* textstring, LodeZlib_CompressSettings* zlibsettings)
+{
+ unsigned error = 0;
+ ucvector data, compressed;
+ size_t i, textsize = strlen(textstring);
+
+ ucvector_init(&data);
+ ucvector_init(&compressed);
+ for(i = 0; keyword[i] != 0; i++) ucvector_push_back(&data, (unsigned char)keyword[i]);
+ ucvector_push_back(&data, 0); /* 0 termination char*/
+ ucvector_push_back(&data, 0); /*compression method: 0*/
+
+ error = LodePNG_compress(&compressed.data, &compressed.size, (unsigned char*)textstring, textsize, zlibsettings);
+ if(!error)
+ {
+ for(i = 0; i < compressed.size; i++) ucvector_push_back(&data, compressed.data[i]);
+ error = addChunk(out, "zTXt", data.data, data.size);
+ }
+
+ ucvector_cleanup(&compressed);
+ ucvector_cleanup(&data);
+ return error;
+}
+
+static unsigned addChunk_iTXt(ucvector* out, unsigned compressed, const char* keyword, const char* langtag, const char* transkey, const char* textstring, LodeZlib_CompressSettings* zlibsettings)
+{
+ unsigned error = 0;
+ ucvector data, compressed_data;
+ size_t i, textsize = strlen(textstring);
+
+ ucvector_init(&data);
+
+ for(i = 0; keyword[i] != 0; i++) ucvector_push_back(&data, (unsigned char)keyword[i]);
+ ucvector_push_back(&data, 0); /*null termination char*/
+ ucvector_push_back(&data, compressed ? 1 : 0); /*compression flag*/
+ ucvector_push_back(&data, 0); /*compression method*/
+ for(i = 0; langtag[i] != 0; i++) ucvector_push_back(&data, (unsigned char)langtag[i]);
+ ucvector_push_back(&data, 0); /*null termination char*/
+ for(i = 0; transkey[i] != 0; i++) ucvector_push_back(&data, (unsigned char)transkey[i]);
+ ucvector_push_back(&data, 0); /*null termination char*/
+
+ if(compressed)
+ {
+ ucvector_init(&compressed_data);
+ error = LodePNG_compress(&compressed_data.data, &compressed_data.size, (unsigned char*)textstring, textsize, zlibsettings);
+ if(!error)
+ {
+ for(i = 0; i < compressed_data.size; i++) ucvector_push_back(&data, compressed_data.data[i]);
+ for(i = 0; textstring[i] != 0; i++) ucvector_push_back(&data, (unsigned char)textstring[i]);
+ }
+ }
+ else /*not compressed*/
+ {
+ for(i = 0; textstring[i] != 0; i++) ucvector_push_back(&data, (unsigned char)textstring[i]);
+ }
+
+ if(!error) error = addChunk(out, "iTXt", data.data, data.size);
+ ucvector_cleanup(&data);
+ return error;
+}
+
+static unsigned addChunk_bKGD(ucvector* out, const LodePNG_InfoPng* info)
+{
+ unsigned error = 0;
+ ucvector bKGD;
+ ucvector_init(&bKGD);
+ if(info->color.colorType == 0 || info->color.colorType == 4)
+ {
+ ucvector_push_back(&bKGD, (unsigned char)(info->background_r / 256));
+ ucvector_push_back(&bKGD, (unsigned char)(info->background_r % 256));
+ }
+ else if(info->color.colorType == 2 || info->color.colorType == 6)
+ {
+ ucvector_push_back(&bKGD, (unsigned char)(info->background_r / 256));
+ ucvector_push_back(&bKGD, (unsigned char)(info->background_r % 256));
+ ucvector_push_back(&bKGD, (unsigned char)(info->background_g / 256));
+ ucvector_push_back(&bKGD, (unsigned char)(info->background_g % 256));
+ ucvector_push_back(&bKGD, (unsigned char)(info->background_b / 256));
+ ucvector_push_back(&bKGD, (unsigned char)(info->background_b % 256));
+ }
+ else if(info->color.colorType == 3)
+ {
+ ucvector_push_back(&bKGD, (unsigned char)(info->background_r % 256)); /*palette index*/
+ }
+
+ error = addChunk(out, "bKGD", bKGD.data, bKGD.size);
+ ucvector_cleanup(&bKGD);
+
+ return error;
+}
+
+static unsigned addChunk_tIME(ucvector* out, const LodePNG_Time* time)
+{
+ unsigned error = 0;
+ unsigned char* data = (unsigned char*)malloc(7);
+ if(!data) return 9948; /*memory allocation failed*/
+ data[0] = (unsigned char)(time->year / 256);
+ data[1] = (unsigned char)(time->year % 256);
+ data[2] = time->month;
+ data[3] = time->day;
+ data[4] = time->hour;
+ data[5] = time->minute;
+ data[6] = time->second;
+ error = addChunk(out, "tIME", data, 7);
+ free(data);
+ return error;
+}
+
+static unsigned addChunk_pHYs(ucvector* out, const LodePNG_InfoPng* info)
+{
+ unsigned error = 0;
+ ucvector data;
+ ucvector_init(&data);
+
+ LodePNG_add32bitInt(&data, info->phys_x);
+ LodePNG_add32bitInt(&data, info->phys_y);
+ ucvector_push_back(&data, info->phys_unit);
+
+ error = addChunk(out, "pHYs", data.data, data.size);
+ ucvector_cleanup(&data);
+
+ return error;
+}
+
+#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/
+
+static void filterScanline(unsigned char* out, const unsigned char* scanline, const unsigned char* prevline, size_t length, size_t bytewidth, unsigned char filterType)
+{
+ size_t i;
+ switch(filterType)
+ {
+ case 0:
+ for(i = 0; i < length; i++) out[i] = scanline[i];
+ break;
+ case 1:
+ if(prevline)
+ {
+ for(i = 0; i < bytewidth; i++) out[i] = scanline[i];
+ for(i = bytewidth; i < length ; i++) out[i] = scanline[i] - scanline[i - bytewidth];
+ }
+ else
+ {
+ for(i = 0; i < bytewidth; i++) out[i] = scanline[i];
+ for(i = bytewidth; i < length; i++) out[i] = scanline[i] - scanline[i - bytewidth];
+ }
+ break;
+ case 2:
+ if(prevline)
+ {
+ for(i = 0; i < length; i++) out[i] = scanline[i] - prevline[i];
+ }
+ else
+ {
+ for(i = 0; i < length; i++) out[i] = scanline[i];
+ }
+ break;
+ case 3:
+ if(prevline)
+ {
+ for(i = 0; i < bytewidth; i++) out[i] = scanline[i] - prevline[i] / 2;
+ for(i = bytewidth; i < length; i++) out[i] = scanline[i] - ((scanline[i - bytewidth] + prevline[i]) / 2);
+ }
+ else
+ {
+ for(i = 0; i < bytewidth; i++) out[i] = scanline[i];
+ for(i = bytewidth; i < length; i++) out[i] = scanline[i] - scanline[i - bytewidth] / 2;
+ }
+ break;
+ case 4:
+ if(prevline)
+ {
+ for(i = 0; i < bytewidth; i++) out[i] = (scanline[i] - prevline[i]); /*paethPredictor(0, prevline[i], 0) is always prevline[i]*/
+ for(i = bytewidth; i < length; i++) out[i] = (scanline[i] - paethPredictor(scanline[i - bytewidth], prevline[i], prevline[i - bytewidth]));
+ }
+ else
+ {
+ for(i = 0; i < bytewidth; i++) out[i] = scanline[i];
+ for(i = bytewidth; i < length; i++) out[i] = (scanline[i] - scanline[i - bytewidth]); /*paethPredictor(scanline[i - bytewidth], 0, 0) is always scanline[i - bytewidth]*/
+ }
+ break;
+ default: return; /*unexisting filter type given*/
+ }
+}
+
+static unsigned filter(unsigned char* out, const unsigned char* in, unsigned w, unsigned h, const LodePNG_InfoColor* info)
+{
+ /*
+ For PNG filter method 0
+ out must be a buffer with as size: h + (w * h * bpp + 7) / 8, because there are the scanlines with 1 extra byte per scanline
+
+ There is a nice heuristic described here: http://www.cs.toronto.edu/~cosmin/pngtech/optipng.html. It says:
+ * If the image type is Palette, or the bit depth is smaller than 8, then do not filter the image (i.e. use fixed filtering, with the filter None).
+ * (The other case) If the image type is Grayscale or RGB (with or without Alpha), and the bit depth is not smaller than 8, then use adaptive filtering heuristic as follows: independently for each row, apply all five filters and select the filter that produces the smallest sum of absolute values per row.
+
+ Here the above method is used mostly. Note though that it appears to be better to use the adaptive filtering on the plasma 8-bit palette example, but that image isn't the best reference for palette images in general.
+ */
+
+ unsigned bpp = LodePNG_InfoColor_getBpp(info);
+ size_t linebytes = (w * bpp + 7) / 8; /*the width of a scanline in bytes, not including the filter type*/
+ size_t bytewidth = (bpp + 7) / 8; /*bytewidth is used for filtering, is 1 when bpp < 8, number of bytes per pixel otherwise*/
+ const unsigned char* prevline = 0;
+ unsigned x, y;
+ unsigned heuristic;
+ unsigned error = 0;
+
+ if(bpp == 0) return 31; /*error: invalid color type*/
+
+ /*choose heuristic as described above*/
+ if(info->colorType == 3 || info->bitDepth < 8) heuristic = 0;
+ else heuristic = 1;
+
+ if(heuristic == 0) /*None filtertype for everything*/
+ {
+ for(y = 0; y < h; y++)
+ {
+ size_t outindex = (1 + linebytes) * y; /*the extra filterbyte added to each row*/
+ size_t inindex = linebytes * y;
+ const unsigned TYPE = 0;
+ out[outindex] = TYPE; /*filter type byte*/
+ filterScanline(&out[outindex + 1], &in[inindex], prevline, linebytes, bytewidth, TYPE);
+ prevline = &in[inindex];
+ }
+ }
+ else if(heuristic == 1) /*adaptive filtering*/
+ {
+ size_t sum[5];
+ ucvector attempt[5]; /*five filtering attempts, one for each filter type*/
+ size_t smallest = 0;
+ unsigned type, bestType = 0;
+
+ for(type = 0; type < 5; type++) ucvector_init(&attempt[type]);
+ for(type = 0; type < 5; type++)
+ {
+ if(!ucvector_resize(&attempt[type], linebytes))
+ {
+ error = 9949; /*memory allocation failed*/
+ break;
+ }
+ }
+
+ if(!error)
+ {
+ for(y = 0; y < h; y++)
+ {
+ /*try the 5 filter types*/
+ for(type = 0; type < 5; type++)
+ {
+ filterScanline(attempt[type].data, &in[y * linebytes], prevline, linebytes, bytewidth, type);
+
+ /*calculate the sum of the result*/
+ sum[type] = 0;
+ for(x = 0; x < attempt[type].size; x+=3) sum[type] += attempt[type].data[x]; /*note that not all pixels are checked to speed this up while still having probably the best choice*/
+
+ /*check if this is smallest sum (or if type == 0 it's the first case so always store the values)*/
+ if(type == 0 || sum[type] < smallest)
+ {
+ bestType = type;
+ smallest = sum[type];
+ }
+ }
+
+ prevline = &in[y * linebytes];
+
+ /*now fill the out values*/
+ out[y * (linebytes + 1)] = bestType; /*the first byte of a scanline will be the filter type*/
+ for(x = 0; x < linebytes; x++) out[y * (linebytes + 1) + 1 + x] = attempt[bestType].data[x];
+ }
+ }
+
+ for(type = 0; type < 5; type++) ucvector_cleanup(&attempt[type]);
+ }
+ #if 0 /*deflate the scanline with a fixed tree after every filter attempt to see which one deflates best. This is slow, and _does not work as expected_: the heuristic gives smaller result!*/
+ else if(heuristic == 2) /*adaptive filtering by using deflate*/
+ {
+ size_t size[5];
+ ucvector attempt[5]; /*five filtering attempts, one for each filter type*/
+ size_t smallest;
+ unsigned type = 0, bestType = 0;
+ unsigned char* dummy;
+ LodeZlib_CompressSettings deflatesettings = LodeZlib_defaultCompressSettings;
+ deflatesettings.btype = 1; /*use fixed tree on the attempts so that the tree is not adapted to the filtertype on purpose, to simulate the true case where the tree is the same for the whole image*/
+ for(type = 0; type < 5; type++)
+ {
+ ucvector_init(&attempt[type]);
+ ucvector_resize(&attempt[type], linebytes); /*todo: give error if resize failed*/
+ }
+ for(y = 0; y < h; y++) /*try the 5 filter types*/
+ {
+ for(type = 0; type < 5; type++)
+ {
+ filterScanline(attempt[type].data, &in[y * linebytes], prevline, linebytes, bytewidth, type);
+ size[type] = 0;
+ dummy = 0;
+ LodePNG_compress(&dummy, &size[type], attempt[type].data, attempt[type].size, &deflatesettings);
+ free(dummy);
+ /*check if this is smallest size (or if type == 0 it's the first case so always store the values)*/
+ if(type == 0 || size[type] < smallest)
+ {
+ bestType = type;
+ smallest = size[type];
+ }
+ }
+ prevline = &in[y * linebytes];
+ out[y * (linebytes + 1)] = bestType; /*the first byte of a scanline will be the filter type*/
+ for(x = 0; x < linebytes; x++) out[y * (linebytes + 1) + 1 + x] = attempt[bestType].data[x];
+ }
+ for(type = 0; type < 5; type++) ucvector_cleanup(&attempt[type]);
+ }
+ #endif
+
+ return error;
+}
+
+static void addPaddingBits(unsigned char* out, const unsigned char* in, size_t olinebits, size_t ilinebits, unsigned h)
+{
+ /*The opposite of the removePaddingBits function
+ olinebits must be >= ilinebits*/
+ unsigned y;
+ size_t diff = olinebits - ilinebits;
+ size_t obp = 0, ibp = 0; /*bit pointers*/
+ for(y = 0; y < h; y++)
+ {
+ size_t x;
+ for(x = 0; x < ilinebits; x++)
+ {
+ unsigned char bit = readBitFromReversedStream(&ibp, in);
+ setBitOfReversedStream(&obp, out, bit);
+ }
+ /*obp += diff; --> no, fill in some value in the padding bits too, to avoid "Use of uninitialised value of size ###" warning from valgrind*/
+ for(x = 0; x < diff; x++) setBitOfReversedStream(&obp, out, 0);
+ }
+}
+
+static void Adam7_interlace(unsigned char* out, const unsigned char* in, unsigned w, unsigned h, unsigned bpp)
+{
+ /*Note: this function works on image buffers WITHOUT padding bits at end of scanlines with non-multiple-of-8 bit amounts, only between reduced images is padding*/
+ unsigned passw[7], passh[7];
+ size_t filter_passstart[8], padded_passstart[8], passstart[8];
+ unsigned i;
+
+ Adam7_getpassvalues(passw, passh, filter_passstart, padded_passstart, passstart, w, h, bpp);
+
+ if(bpp >= 8)
+ {
+ for(i = 0; i < 7; i++)
+ {
+ unsigned x, y, b;
+ size_t bytewidth = bpp / 8;
+ for(y = 0; y < passh[i]; y++)
+ for(x = 0; x < passw[i]; x++)
+ {
+ size_t pixelinstart = ((ADAM7_IY[i] + y * ADAM7_DY[i]) * w + ADAM7_IX[i] + x * ADAM7_DX[i]) * bytewidth;
+ size_t pixeloutstart = passstart[i] + (y * passw[i] + x) * bytewidth;
+ for(b = 0; b < bytewidth; b++)
+ {
+ out[pixeloutstart + b] = in[pixelinstart + b];
+ }
+ }
+ }
+ }
+ else /*bpp < 8: Adam7 with pixels < 8 bit is a bit trickier: with bit pointers*/
+ {
+ for(i = 0; i < 7; i++)
+ {
+ unsigned x, y, b;
+ unsigned ilinebits = bpp * passw[i];
+ unsigned olinebits = bpp * w;
+ size_t obp, ibp; /*bit pointers (for out and in buffer)*/
+ for(y = 0; y < passh[i]; y++)
+ for(x = 0; x < passw[i]; x++)
+ {
+ ibp = (ADAM7_IY[i] + y * ADAM7_DY[i]) * olinebits + (ADAM7_IX[i] + x * ADAM7_DX[i]) * bpp;
+ obp = (8 * passstart[i]) + (y * ilinebits + x * bpp);
+ for(b = 0; b < bpp; b++)
+ {
+ unsigned char bit = readBitFromReversedStream(&ibp, in);
+ setBitOfReversedStream(&obp, out, bit);
+ }
+ }
+ }
+ }
+}
+
+/*out must be buffer big enough to contain uncompressed IDAT chunk data, and in must contain the full image*/
+static unsigned preProcessScanlines(unsigned char** out, size_t* outsize, const unsigned char* in, const LodePNG_InfoPng* infoPng) /*return value is error*/
+{
+ /*
+ This function converts the pure 2D image with the PNG's colortype, into filtered-padded-interlaced data. Steps:
+ *) if no Adam7: 1) add padding bits (= posible extra bits per scanline if bpp < 8) 2) filter
+ *) if adam7: 1) Adam7_interlace 2) 7x add padding bits 3) 7x filter
+ */
+ unsigned bpp = LodePNG_InfoColor_getBpp(&infoPng->color);
+ unsigned w = infoPng->width;
+ unsigned h = infoPng->height;
+ unsigned error = 0;
+
+ if(infoPng->interlaceMethod == 0)
+ {
+ *outsize = h + (h * ((w * bpp + 7) / 8)); /*image size plus an extra byte per scanline + possible padding bits*/
+ *out = (unsigned char*)malloc(*outsize);
+ if(!(*out) && (*outsize)) error = 9950; /*memory allocation failed*/
+
+ if(!error)
+ {
+ if(bpp < 8 && w * bpp != ((w * bpp + 7) / 8) * 8) /*non multiple of 8 bits per scanline, padding bits needed per scanline*/
+ {
+ ucvector padded;
+ ucvector_init(&padded);
+ if(!ucvector_resize(&padded, h * ((w * bpp + 7) / 8))) error = 9951;
+ if(!error)
+ {
+ addPaddingBits(padded.data, in, ((w * bpp + 7) / 8) * 8, w * bpp, h);
+ error = filter(*out, padded.data, w, h, &infoPng->color);
+ }
+ ucvector_cleanup(&padded);
+ }
+ else error = filter(*out, in, w, h, &infoPng->color); /*we can immediatly filter into the out buffer, no other steps needed*/
+ }
+ }
+ else /*interlaceMethod is 1 (Adam7)*/
+ {
+ unsigned char* adam7 = (unsigned char*)malloc((h * w * bpp + 7) / 8);
+ if(!adam7 && ((h * w * bpp + 7) / 8)) error = 9952; /*memory allocation failed*/
+
+ while(!error) /*not a real while loop, used to break out to cleanup to avoid a goto*/
+ {
+ unsigned passw[7], passh[7];
+ size_t filter_passstart[8], padded_passstart[8], passstart[8];
+ unsigned i;
+
+ Adam7_getpassvalues(passw, passh, filter_passstart, padded_passstart, passstart, w, h, bpp);
+
+ *outsize = filter_passstart[7]; /*image size plus an extra byte per scanline + possible padding bits*/
+ *out = (unsigned char*)malloc(*outsize);
+ if(!(*out) && (*outsize))
+ {
+ error = 9953; /*memory allocation failed*/
+ break;
+ }
+
+ Adam7_interlace(adam7, in, w, h, bpp);
+
+ for(i = 0; i < 7; i++)
+ {
+ if(bpp < 8)
+ {
+ ucvector padded;
+ ucvector_init(&padded);
+ if(!ucvector_resize(&padded, h * ((w * bpp + 7) / 8))) error = 9954;
+ if(!error)
+ {
+ addPaddingBits(&padded.data[padded_passstart[i]], &adam7[passstart[i]], ((passw[i] * bpp + 7) / 8) * 8, passw[i] * bpp, passh[i]);
+ error = filter(&(*out)[filter_passstart[i]], &padded.data[padded_passstart[i]], passw[i], passh[i], &infoPng->color);
+ }
+
+ ucvector_cleanup(&padded);
+ }
+ else
+ {
+ error = filter(&(*out)[filter_passstart[i]], &adam7[padded_passstart[i]], passw[i], passh[i], &infoPng->color);
+ }
+ }
+
+ break;
+ }
+
+ free(adam7);
+ }
+
+ return error;
+}
+
+/*palette must have 4 * palettesize bytes allocated*/
+static unsigned isPaletteFullyOpaque(const unsigned char* palette, size_t palettesize) /*palette given in format RGBARGBARGBARGBA...*/
+{
+ size_t i;
+ for(i = 0; i < palettesize; i++)
+ {
+ if(palette[4 * i + 3] != 255) return 0;
+ }
+ return 1;
+}
+
+/*this function checks if the input image given by the user has no transparent pixels*/
+static unsigned isFullyOpaque(const unsigned char* image, unsigned w, unsigned h, const LodePNG_InfoColor* info)
+{
+ /*TODO: When the user specified a color key for the input image, then this function must also check for pixels that are the same as the color key and treat those as transparent.*/
+
+ unsigned i, numpixels = w * h;
+ if(info->colorType == 6)
+ {
+ if(info->bitDepth == 8)
+ {
+ for(i = 0; i < numpixels; i++)
+ {
+ if(image[i * 4 + 3] != 255) return 0;
+ }
+ }
+ else
+ {
+ for(i = 0; i < numpixels; i++)
+ {
+ if(image[i * 8 + 6] != 255 || image[i * 8 + 7] != 255) return 0;
+ }
+ }
+ return 1; /*no single pixel with alpha channel other than 255 found*/
+ }
+ else if(info->colorType == 4)
+ {
+ if(info->bitDepth == 8)
+ {
+ for(i = 0; i < numpixels; i++)
+ {
+ if(image[i * 2 + 1] != 255) return 0;
+ }
+ }
+ else
+ {
+ for(i = 0; i < numpixels; i++)
+ {
+ if(image[i * 4 + 2] != 255 || image[i * 4 + 3] != 255) return 0;
+ }
+ }
+ return 1; /*no single pixel with alpha channel other than 255 found*/
+ }
+ else if(info->colorType == 3)
+ {
+ /*when there's a palette, we could check every pixel for translucency, but much quicker is to just check the palette*/
+ return(isPaletteFullyOpaque(info->palette, info->palettesize));
+ }
+
+ return 0; /*color type that isn't supported by this function yet, so assume there is transparency to be safe*/
+}
+
+#ifdef LODEPNG_COMPILE_UNKNOWN_CHUNKS
+static unsigned addUnknownChunks(ucvector* out, unsigned char* data, size_t datasize)
+{
+ unsigned char* inchunk = data;
+ while((size_t)(inchunk - data) < datasize)
+ {
+ unsigned error = LodePNG_append_chunk(&out->data, &out->size, inchunk);
+ if(error) return error; /*error: not enough memory*/
+ out->allocsize = out->size; /*fix the allocsize again*/
+ inchunk = LodePNG_chunk_next(inchunk);
+ }
+ return 0;
+}
+#endif /*LODEPNG_COMPILE_UNKNOWN_CHUNKS*/
+
+void LodePNG_Encoder_encode(LodePNG_Encoder* encoder, unsigned char** out, size_t* outsize, const unsigned char* image, unsigned w, unsigned h)
+{
+ LodePNG_InfoPng info;
+ ucvector outv;
+ unsigned char* data = 0; /*uncompressed version of the IDAT chunk data*/
+ size_t datasize = 0;
+
+ /*provide some proper output values if error will happen*/
+ *out = 0;
+ *outsize = 0;
+ encoder->error = 0;
+
+ info = encoder->infoPng; /*UNSAFE copy to avoid having to cleanup! but we will only change primitive parameters, and not invoke the cleanup function nor touch the palette's buffer so we use it safely*/
+ info.width = w;
+ info.height = h;
+
+ if(encoder->settings.autoLeaveOutAlphaChannel && isFullyOpaque(image, w, h, &encoder->infoRaw.color))
+ {
+ /*go to a color type without alpha channel*/
+ if(info.color.colorType == 6) info.color.colorType = 2;
+ else if(info.color.colorType == 4) info.color.colorType = 0;
+ }
+
+ if(encoder->settings.zlibsettings.windowSize > 32768)
+ {
+ encoder->error = 60; /*error: windowsize larger than allowed*/
+ return;
+ }
+ if(encoder->settings.zlibsettings.btype > 2)
+ {
+ encoder->error = 61; /*error: unexisting btype*/
+ return;
+ }
+ if(encoder->infoPng.interlaceMethod > 1)
+ {
+ encoder->error = 71; /*error: unexisting interlace mode*/
+ return;
+ }
+ if((encoder->error = checkColorValidity(info.color.colorType, info.color.bitDepth))) return; /*error: unexisting color type given*/
+ if((encoder->error = checkColorValidity(encoder->infoRaw.color.colorType, encoder->infoRaw.color.bitDepth))) return; /*error: unexisting color type given*/
+
+ if(!LodePNG_InfoColor_equal(&encoder->infoRaw.color, &info.color))
+ {
+ unsigned char* converted;
+ size_t size = (w * h * LodePNG_InfoColor_getBpp(&info.color) + 7) / 8;
+
+ if((info.color.colorType != 6 && info.color.colorType != 2) || (info.color.bitDepth != 8))
+ {
+ encoder->error = 59; /*for the output image, only these types are supported*/
+ return;
+ }
+ converted = (unsigned char*)malloc(size);
+ if(!converted && size) encoder->error = 9955; /*memory allocation failed*/
+ if(!encoder->error) encoder->error = LodePNG_convert(converted, image, &info.color, &encoder->infoRaw.color, w, h);
+ if(!encoder->error) preProcessScanlines(&data, &datasize, converted, &info);/*filter(data.data, converted.data, w, h, LodePNG_InfoColor_getBpp(&info.color));*/
+ free(converted);
+ }
+ else preProcessScanlines(&data, &datasize, image, &info);/*filter(data.data, image, w, h, LodePNG_InfoColor_getBpp(&info.color));*/
+
+ ucvector_init(&outv);
+ while(!encoder->error) /*not really a while loop, this is only used to break out if an error happens to avoid goto's to do the ucvector cleanup*/
+ {
+#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS
+ size_t i;
+#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/
+ /*write signature and chunks*/
+ writeSignature(&outv);
+ /*IHDR*/
+ addChunk_IHDR(&outv, w, h, info.color.bitDepth, info.color.colorType, info.interlaceMethod);
+#ifdef LODEPNG_COMPILE_UNKNOWN_CHUNKS
+ /*unknown chunks between IHDR and PLTE*/
+ if(info.unknown_chunks.data[0])
+ {
+ encoder->error = addUnknownChunks(&outv, info.unknown_chunks.data[0], info.unknown_chunks.datasize[0]);
+ if(encoder->error) break;
+ }
+#endif /*LODEPNG_COMPILE_UNKNOWN_CHUNKS*/
+ /*PLTE*/
+ if(info.color.colorType == 3)
+ {
+ if(info.color.palettesize == 0 || info.color.palettesize > 256)
+ {
+ encoder->error = 68; /*invalid palette size*/
+ break;
+ }
+ addChunk_PLTE(&outv, &info.color);
+ }
+ if(encoder->settings.force_palette && (info.color.colorType == 2 || info.color.colorType == 6))
+ {
+ if(info.color.palettesize == 0 || info.color.palettesize > 256)
+ {
+ encoder->error = 68; /*invalid palette size*/
+ break;
+ }
+ addChunk_PLTE(&outv, &info.color);
+ }
+ /*tRNS*/
+ if(info.color.colorType == 3 && !isPaletteFullyOpaque(info.color.palette, info.color.palettesize))
+ {
+ addChunk_tRNS(&outv, &info.color);
+ }
+ if((info.color.colorType == 0 || info.color.colorType == 2) && info.color.key_defined)
+ {
+ addChunk_tRNS(&outv, &info.color);
+ }
+#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS
+ /*bKGD (must come between PLTE and the IDAt chunks*/
+ if(info.background_defined) addChunk_bKGD(&outv, &info);
+ /*pHYs (must come before the IDAT chunks)*/
+ if(info.phys_defined) addChunk_pHYs(&outv, &info);
+#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/
+#ifdef LODEPNG_COMPILE_UNKNOWN_CHUNKS
+ /*unknown chunks between PLTE and IDAT*/
+ if(info.unknown_chunks.data[1])
+ {
+ encoder->error = addUnknownChunks(&outv, info.unknown_chunks.data[1], info.unknown_chunks.datasize[1]);
+ if(encoder->error) break;
+ }
+#endif /*LODEPNG_COMPILE_UNKNOWN_CHUNKS*/
+ /*IDAT (multiple IDAT chunks must be consecutive)*/
+ encoder->error = addChunk_IDAT(&outv, data, datasize, &encoder->settings.zlibsettings);
+ if(encoder->error) break;
+#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS
+ /*tIME*/
+ if(info.time_defined) addChunk_tIME(&outv, &info.time);
+ /*tEXt and/or zTXt*/
+ for(i = 0; i < info.text.num; i++)
+ {
+ if(strlen(info.text.keys[i]) > 79)
+ {
+ encoder->error = 66; /*text chunk too large*/
+ break;
+ }
+ if(strlen(info.text.keys[i]) < 1)
+ {
+ encoder->error = 67; /*text chunk too small*/
+ break;
+ }
+ if(encoder->settings.text_compression)
+ addChunk_zTXt(&outv, info.text.keys[i], info.text.strings[i], &encoder->settings.zlibsettings);
+ else
+ addChunk_tEXt(&outv, info.text.keys[i], info.text.strings[i]);
+ }
+ /*LodePNG version id in text chunk*/
+ if(encoder->settings.add_id)
+ {
+ unsigned alread_added_id_text = 0;
+ for(i = 0; i < info.text.num; i++)
+ {
+ if(!strcmp(info.text.keys[i], "LodePNG"))
+ {
+ alread_added_id_text = 1;
+ break;
+ }
+ }
+ if(alread_added_id_text == 0)
+ addChunk_tEXt(&outv, "LodePNG", VERSION_STRING); /*it's shorter as tEXt than as zTXt chunk*/
+ }
+ /*iTXt*/
+ for(i = 0; i < info.itext.num; i++)
+ {
+ if(strlen(info.itext.keys[i]) > 79)
+ {
+ encoder->error = 66; /*text chunk too large*/
+ break;
+ }
+ if(strlen(info.itext.keys[i]) < 1)
+ {
+ encoder->error = 67; /*text chunk too small*/
+ break;
+ }
+ addChunk_iTXt(&outv, encoder->settings.text_compression,
+ info.itext.keys[i], info.itext.langtags[i], info.itext.transkeys[i], info.itext.strings[i],
+ &encoder->settings.zlibsettings);
+ }
+#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/
+#ifdef LODEPNG_COMPILE_UNKNOWN_CHUNKS
+ /*unknown chunks between IDAT and IEND*/
+ if(info.unknown_chunks.data[2])
+ {
+ encoder->error = addUnknownChunks(&outv, info.unknown_chunks.data[2], info.unknown_chunks.datasize[2]);
+ if(encoder->error) break;
+ }
+#endif /*LODEPNG_COMPILE_UNKNOWN_CHUNKS*/
+ /*IEND*/
+ addChunk_IEND(&outv);
+
+ break; /*this isn't really a while loop; no error happened so break out now!*/
+ }
+
+ free(data);
+ /*instead of cleaning the vector up, give it to the output*/
+ *out = outv.data;
+ *outsize = outv.size;
+}
+
+unsigned LodePNG_encode(unsigned char** out, size_t* outsize, const unsigned char* image, unsigned w, unsigned h, unsigned colorType, unsigned bitDepth)
+{
+ unsigned error;
+ LodePNG_Encoder encoder;
+ LodePNG_Encoder_init(&encoder);
+ encoder.infoRaw.color.colorType = colorType;
+ encoder.infoRaw.color.bitDepth = bitDepth;
+ LodePNG_Encoder_encode(&encoder, out, outsize, image, w, h);
+ error = encoder.error;
+ LodePNG_Encoder_cleanup(&encoder);
+ return error;
+}
+
+unsigned LodePNG_encode32(unsigned char** out, size_t* outsize, const unsigned char* image, unsigned w, unsigned h)
+{
+ return LodePNG_encode(out, outsize, image, w, h, 6, 8);;
+}
+
+#ifdef LODEPNG_COMPILE_DISK
+unsigned LodePNG_encode_file(const char* filename, const unsigned char* image, unsigned w, unsigned h, unsigned colorType, unsigned bitDepth)
+{
+ unsigned char* buffer;
+ size_t buffersize;
+ unsigned error = LodePNG_encode(&buffer, &buffersize, image, w, h, colorType, bitDepth);
+ LodePNG_saveFile(buffer, buffersize, filename);
+ free(buffer);
+ return error;
+}
+
+unsigned LodePNG_encode32_file(const char* filename, const unsigned char* image, unsigned w, unsigned h)
+{
+ return LodePNG_encode_file(filename, image, w, h, 6, 8);;
+}
+#endif /*LODEPNG_COMPILE_DISK*/
+
+void LodePNG_EncodeSettings_init(LodePNG_EncodeSettings* settings)
+{
+ LodeZlib_CompressSettings_init(&settings->zlibsettings);
+ settings->autoLeaveOutAlphaChannel = 1;
+ settings->force_palette = 0;
+#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS
+ settings->add_id = 1;
+ settings->text_compression = 0;
+#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/
+}
+
+void LodePNG_Encoder_init(LodePNG_Encoder* encoder)
+{
+ LodePNG_EncodeSettings_init(&encoder->settings);
+ LodePNG_InfoPng_init(&encoder->infoPng);
+ LodePNG_InfoRaw_init(&encoder->infoRaw);
+ encoder->error = 1;
+}
+
+void LodePNG_Encoder_cleanup(LodePNG_Encoder* encoder)
+{
+ LodePNG_InfoPng_cleanup(&encoder->infoPng);
+ LodePNG_InfoRaw_cleanup(&encoder->infoRaw);
+}
+
+void LodePNG_Encoder_copy(LodePNG_Encoder* dest, const LodePNG_Encoder* source)
+{
+ LodePNG_Encoder_cleanup(dest);
+ *dest = *source;
+ LodePNG_InfoPng_init(&dest->infoPng);
+ LodePNG_InfoRaw_init(&dest->infoRaw);
+ dest->error = LodePNG_InfoPng_copy(&dest->infoPng, &source->infoPng);
+ if(dest->error) return;
+ dest->error = LodePNG_InfoRaw_copy(&dest->infoRaw, &source->infoRaw);
+ if(dest->error) return;
+}
+
+#endif /*LODEPNG_COMPILE_ENCODER*/
+
+#endif /*LODEPNG_COMPILE_PNG*/
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* / File IO / */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+#ifdef LODEPNG_COMPILE_DISK
+
+unsigned LodePNG_loadFile(unsigned char** out, size_t* outsize, const char* filename) /*designed for loading files from hard disk in a dynamically allocated buffer*/
+{
+ FILE* file;
+ long size;
+
+ /*provide some proper output values if error will happen*/
+ *out = 0;
+ *outsize = 0;
+
+ file = fopen(filename, "rb");
+ if(!file) return 78;
+
+ /*get filesize:*/
+ fseek(file , 0 , SEEK_END);
+ size = ftell(file);
+ rewind(file);
+
+ /*read contents of the file into the vector*/
+ *outsize = 0;
+ *out = (unsigned char*)malloc((size_t)size);
+ if(size && (*out)) (*outsize) = fread(*out, 1, (size_t)size, file);
+
+ fclose(file);
+ if(!(*out) && size) return 80; /*the above malloc failed*/
+ return 0;
+}
+
+/*write given buffer to the file, overwriting the file, it doesn't append to it.*/
+unsigned LodePNG_saveFile(const unsigned char* buffer, size_t buffersize, const char* filename)
+{
+ FILE* file;
+ file = fopen(filename, "wb" );
+ if(!file) return 79;
+ fwrite((char*)buffer , 1 , buffersize, file);
+ fclose(file);
+ return 0;
+}
+
+#endif /*LODEPNG_COMPILE_DISK*/
+
+#ifdef __cplusplus
+/* ////////////////////////////////////////////////////////////////////////// */
+/* / C++ RAII wrapper / */
+/* ////////////////////////////////////////////////////////////////////////// */
+#ifdef LODEPNG_COMPILE_ZLIB
+namespace LodeZlib
+{
+#ifdef LODEPNG_COMPILE_DECODER
+ unsigned decompress(std::vector<unsigned char>& out, const unsigned char* in, size_t insize, const LodeZlib_DecompressSettings& settings)
+ {
+ unsigned char* buffer = 0;
+ size_t buffersize = 0;
+ unsigned error = LodeZlib_decompress(&buffer, &buffersize, in, insize, &settings);
+ if(buffer)
+ {
+ out.insert(out.end(), &buffer[0], &buffer[buffersize]);
+ free(buffer);
+ }
+ return error;
+ }
+
+ unsigned decompress(std::vector<unsigned char>& out, const std::vector<unsigned char>& in, const LodeZlib_DecompressSettings& settings)
+ {
+ return decompress(out, in.empty() ? 0 : &in[0], in.size(), settings);
+ }
+#endif //LODEPNG_COMPILE_DECODER
+#ifdef LODEPNG_COMPILE_ENCODER
+ unsigned compress(std::vector<unsigned char>& out, const unsigned char* in, size_t insize, const LodeZlib_CompressSettings& settings)
+ {
+ unsigned char* buffer = 0;
+ size_t buffersize = 0;
+ unsigned error = LodeZlib_compress(&buffer, &buffersize, in, insize, &settings);
+ if(buffer)
+ {
+ out.insert(out.end(), &buffer[0], &buffer[buffersize]);
+ free(buffer);
+ }
+ return error;
+ }
+
+ unsigned compress(std::vector<unsigned char>& out, const std::vector<unsigned char>& in, const LodeZlib_CompressSettings& settings)
+ {
+ return compress(out, in.empty() ? 0 : &in[0], in.size(), settings);
+ }
+#endif //LODEPNG_COMPILE_ENCODER
+}
+#endif //LODEPNG_COMPILE_ZLIB
+
+namespace LodePNG
+{
+ Decoder::Decoder()
+ {
+ LodePNG_Decoder_init(this);
+ }
+
+ Decoder::~Decoder()
+ {
+ LodePNG_Decoder_cleanup(this);
+ }
+
+ void Decoder::operator=(const LodePNG_Decoder& other)
+ {
+ LodePNG_Decoder_copy(this, &other);
+ }
+
+ bool Decoder::hasError() const
+ {
+ return error != 0;
+ }
+ unsigned Decoder::getError() const
+ {
+ return error;
+ }
+
+ unsigned Decoder::getWidth() const
+ {
+ return infoPng.width;
+ }
+
+ unsigned Decoder::getHeight() const
+ {
+ return infoPng.height;
+ }
+
+ unsigned Decoder::getBpp()
+ {
+ return LodePNG_InfoColor_getBpp(&infoPng.color);
+ }
+
+ unsigned Decoder::getChannels()
+ {
+ return LodePNG_InfoColor_getChannels(&infoPng.color);
+ }
+
+ unsigned Decoder::isGreyscaleType()
+ {
+ return LodePNG_InfoColor_isGreyscaleType(&infoPng.color);
+ }
+
+ unsigned Decoder::isAlphaType()
+ {
+ return LodePNG_InfoColor_isAlphaType(&infoPng.color);
+ }
+
+ void Decoder::decode(std::vector<unsigned char>& out, const unsigned char* in, size_t insize)
+ {
+ unsigned char* buffer;
+ size_t buffersize;
+ LodePNG_Decoder_decode(this, &buffer, &buffersize, in, insize);
+ if(buffer)
+ {
+ out.insert(out.end(), &buffer[0], &buffer[buffersize]);
+ free(buffer);
+ }
+ }
+
+ void Decoder::decode(std::vector<unsigned char>& out, const std::vector<unsigned char>& in)
+ {
+ decode(out, in.empty() ? 0 : &in[0], in.size());
+ }
+
+ void Decoder::inspect(const unsigned char* in, size_t insize)
+ {
+ LodePNG_Decoder_inspect(this, in, insize);
+ }
+
+ void Decoder::inspect(const std::vector<unsigned char>& in)
+ {
+ inspect(in.empty() ? 0 : &in[0], in.size());
+ }
+
+ const LodePNG_DecodeSettings& Decoder::getSettings() const
+ {
+ return settings;
+ }
+
+ LodePNG_DecodeSettings& Decoder::getSettings()
+ {
+ return settings;
+ }
+
+ void Decoder::setSettings(const LodePNG_DecodeSettings& settings)
+ {
+ this->settings = settings;
+ }
+
+ const LodePNG_InfoPng& Decoder::getInfoPng() const
+ {
+ return infoPng;
+ }
+
+ LodePNG_InfoPng& Decoder::getInfoPng()
+ {
+ return infoPng;
+ }
+
+ void Decoder::setInfoPng(const LodePNG_InfoPng& info)
+ {
+ error = LodePNG_InfoPng_copy(&this->infoPng, &info);
+ }
+
+ void Decoder::swapInfoPng(LodePNG_InfoPng& info)
+ {
+ LodePNG_InfoPng_swap(&this->infoPng, &info);
+ }
+
+ const LodePNG_InfoRaw& Decoder::getInfoRaw() const
+ {
+ return infoRaw;
+ }
+
+ LodePNG_InfoRaw& Decoder::getInfoRaw()
+ {
+ return infoRaw;
+ }
+
+ void Decoder::setInfoRaw(const LodePNG_InfoRaw& info)
+ {
+ error = LodePNG_InfoRaw_copy(&this->infoRaw, &info);
+ }
+
+ /* ////////////////////////////////////////////////////////////////////////// */
+
+ Encoder::Encoder()
+ {
+ LodePNG_Encoder_init(this);
+ }
+
+ Encoder::~Encoder()
+ {
+ LodePNG_Encoder_cleanup(this);
+ }
+
+ void Encoder::operator=(const LodePNG_Encoder& other)
+ {
+ LodePNG_Encoder_copy(this, &other);
+ }
+
+ bool Encoder::hasError() const
+ {
+ return error != 0;
+ }
+
+ unsigned Encoder::getError() const
+ {
+ return error;
+ }
+
+ void Encoder::encode(std::vector<unsigned char>& out, const unsigned char* image, unsigned w, unsigned h)
+ {
+ unsigned char* buffer;
+ size_t buffersize;
+ LodePNG_Encoder_encode(this, &buffer, &buffersize, image, w, h);
+ if(buffer)
+ {
+ out.insert(out.end(), &buffer[0], &buffer[buffersize]);
+ free(buffer);
+ }
+ }
+
+ void Encoder::encode(std::vector<unsigned char>& out, const std::vector<unsigned char>& image, unsigned w, unsigned h)
+ {
+ encode(out, image.empty() ? 0 : &image[0], w, h);
+ }
+
+ void Encoder::clearPalette()
+ {
+ LodePNG_InfoColor_clearPalette(&infoPng.color);
+ }
+
+ void Encoder::addPalette(unsigned char r, unsigned char g, unsigned char b, unsigned char a)
+ {
+ error = LodePNG_InfoColor_addPalette(&infoPng.color, r, g, b, a);
+ }
+
+#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS
+ void Encoder::clearText()
+ {
+ LodePNG_Text_clear(&infoPng.text);
+ }
+
+ void Encoder::addText(const std::string& key, const std::string& str)
+ {
+ error = LodePNG_Text_add(&infoPng.text, key.c_str(), str.c_str());
+ }
+
+ void Encoder::clearIText()
+ {
+ LodePNG_IText_clear(&infoPng.itext);
+ }
+
+ void Encoder::addIText(const std::string& key, const std::string& langtag, const std::string& transkey, const std::string& str)
+ {
+ error = LodePNG_IText_add(&infoPng.itext, key.c_str(), langtag.c_str(), transkey.c_str(), str.c_str());
+ }
+
+#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/
+
+ const LodePNG_EncodeSettings& Encoder::getSettings() const
+ {
+ return settings;
+ }
+
+ LodePNG_EncodeSettings& Encoder::getSettings()
+ {
+ return settings;
+ }
+
+ void Encoder::setSettings(const LodePNG_EncodeSettings& settings)
+ {
+ this->settings = settings;
+ }
+
+ const LodePNG_InfoPng& Encoder::getInfoPng() const
+ {
+ return infoPng;
+ }
+
+ LodePNG_InfoPng& Encoder::getInfoPng()
+ {
+ return infoPng;
+ }
+
+ void Encoder::setInfoPng(const LodePNG_InfoPng& info)
+ {
+ error = LodePNG_InfoPng_copy(&this->infoPng, &info);
+ }
+
+ void Encoder::swapInfoPng(LodePNG_InfoPng& info)
+ {
+ LodePNG_InfoPng_swap(&this->infoPng, &info);
+ }
+
+ const LodePNG_InfoRaw& Encoder::getInfoRaw() const
+ {
+ return infoRaw;
+ }
+
+ LodePNG_InfoRaw& Encoder::getInfoRaw()
+ {
+ return infoRaw;
+ }
+
+ void Encoder::setInfoRaw(const LodePNG_InfoRaw& info)
+ {
+ error = LodePNG_InfoRaw_copy(&this->infoRaw, &info);
+ }
+
+ /* ////////////////////////////////////////////////////////////////////////// */
+
+#ifdef LODEPNG_COMPILE_DISK
+
+ void loadFile(std::vector<unsigned char>& buffer, const std::string& filename) //designed for loading files from hard disk in an std::vector
+ {
+ std::ifstream file(filename.c_str(), std::ios::in|std::ios::binary|std::ios::ate);
+
+ /*get filesize*/
+ std::streamsize size = 0;
+ if(file.seekg(0, std::ios::end).good()) size = file.tellg();
+ if(file.seekg(0, std::ios::beg).good()) size -= file.tellg();
+
+ /*read contents of the file into the vector*/
+ buffer.resize(size_t(size));
+ if(size > 0) file.read((char*)(&buffer[0]), size);
+ }
+
+ /*write given buffer to the file, overwriting the file, it doesn't append to it.*/
+ void saveFile(const std::vector<unsigned char>& buffer, const std::string& filename)
+ {
+ std::ofstream file(filename.c_str(), std::ios::out|std::ios::binary);
+ file.write(buffer.empty() ? 0 : (char*)&buffer[0], std::streamsize(buffer.size()));
+ }
+
+#endif /*LODEPNG_COMPILE_DISK*/
+
+ /* ////////////////////////////////////////////////////////////////////////// */
+
+ unsigned decode(std::vector<unsigned char>& out, unsigned& w, unsigned& h, const unsigned char* in, size_t insize, unsigned colorType, unsigned bitDepth)
+ {
+ Decoder decoder;
+ decoder.getInfoRaw().color.colorType = colorType;
+ decoder.getInfoRaw().color.bitDepth = bitDepth;
+ decoder.decode(out, in, insize);
+ w = decoder.getWidth();
+ h = decoder.getHeight();
+ return decoder.getError();
+ }
+
+ unsigned decode(std::vector<unsigned char>& out, unsigned& w, unsigned& h, const std::vector<unsigned char>& in, unsigned colorType, unsigned bitDepth)
+ {
+ return decode(out, w, h, in.empty() ? 0 : &in[0], (unsigned)in.size(), colorType, bitDepth);
+ }
+
+#ifdef LODEPNG_COMPILE_DISK
+ unsigned decode(std::vector<unsigned char>& out, unsigned& w, unsigned& h, const std::string& filename, unsigned colorType, unsigned bitDepth)
+ {
+ std::vector<unsigned char> buffer;
+ loadFile(buffer, filename);
+ return decode(out, w, h, buffer, colorType, bitDepth);
+ }
+#endif /*LODEPNG_COMPILE_DISK*/
+
+ unsigned encode(std::vector<unsigned char>& out, const unsigned char* in, unsigned w, unsigned h, unsigned colorType, unsigned bitDepth)
+ {
+ Encoder encoder;
+ encoder.getInfoRaw().color.colorType = colorType;
+ encoder.getInfoRaw().color.bitDepth = bitDepth;
+ encoder.encode(out, in, w, h);
+ return encoder.getError();
+ }
+
+ unsigned encode(std::vector<unsigned char>& out, const std::vector<unsigned char>& in, unsigned w, unsigned h, unsigned colorType, unsigned bitDepth)
+ {
+ return encode(out, in.empty() ? 0 : &in[0], w, h, colorType, bitDepth);
+ }
+
+#ifdef LODEPNG_COMPILE_DISK
+ unsigned encode(const std::string& filename, const unsigned char* in, unsigned w, unsigned h, unsigned colorType, unsigned bitDepth)
+ {
+ std::vector<unsigned char> buffer;
+ Encoder encoder;
+ encoder.getInfoRaw().color.colorType = colorType;
+ encoder.getInfoRaw().color.bitDepth = bitDepth;
+ encoder.encode(buffer, in, w, h);
+ if(!encoder.hasError()) saveFile(buffer, filename);
+ return encoder.getError();
+ }
+
+ unsigned encode(const std::string& filename, const std::vector<unsigned char>& in, unsigned w, unsigned h, unsigned colorType, unsigned bitDepth)
+ {
+ return encode(filename, in.empty() ? 0 : &in[0], w, h, colorType, bitDepth);
+ }
+#endif /*LODEPNG_COMPILE_DISK*/
+
+}
+#endif /*__cplusplus C++ RAII wrapper*/
diff --git a/Simbody/Visualizer/simbody-visualizer/lodepng.h b/Simbody/Visualizer/simbody-visualizer/lodepng.h
new file mode 100644
index 0000000..2eb82b5
--- /dev/null
+++ b/Simbody/Visualizer/simbody-visualizer/lodepng.h
@@ -0,0 +1,1892 @@
+/*
+LodePNG version 20101030
+
+Copyright (c) 2005-2010 Lode Vandevenne
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any damages
+arising from the use of this software.
+
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it
+freely, subject to the following restrictions:
+
+ 1. The origin of this software must not be misrepresented; you must not
+ claim that you wrote the original software. If you use this software
+ in a product, an acknowledgment in the product documentation would be
+ appreciated but is not required.
+
+ 2. Altered source versions must be plainly marked as such, and must not be
+ misrepresented as being the original software.
+
+ 3. This notice may not be removed or altered from any source
+ distribution.
+*/
+
+#ifndef LODEPNG_H
+#define LODEPNG_H
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#ifdef __cplusplus
+#include <vector>
+#include <string>
+#include <fstream>
+#endif /*__cplusplus*/
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* Code Sections */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+/*
+The following #defines are used to create code sections. They can be disabled
+to disable code sections, which can give faster compile time and smaller binary.
+Also, some text editors allow expanding/collapsing #ifdef sections.
+*/
+
+#define LODEPNG_COMPILE_ZLIB /*deflate&zlib encoder and deflate&zlib decoder*/
+#define LODEPNG_COMPILE_PNG /*png encoder and png decoder*/
+#define LODEPNG_COMPILE_DECODER /*deflate&zlib decoder and png decoder*/
+#define LODEPNG_COMPILE_ENCODER /*deflate&zlib encoder and png encoder*/
+#define LODEPNG_COMPILE_DISK /*the optional built in harddisk file loading and saving functions*/
+#define LODEPNG_COMPILE_ANCILLARY_CHUNKS /*any code or struct datamember related to chunks other than IHDR, IDAT, PLTE, tRNS, IEND*/
+#define LODEPNG_COMPILE_UNKNOWN_CHUNKS /*handling of unknown chunks*/
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* Simple Functions */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+/*
+This are the simple functions, they can be used directly to convert raw data
+to/from PNG data. Both the C and C++ simple functions are declared here.
+
+If more flexibility and settings are required, then the more advanced interface
+below this "simple" part has to be used.
+*/
+
+#ifdef LODEPNG_COMPILE_PNG
+#ifdef LODEPNG_COMPILE_DECODER
+
+/*
+LodePNG_decode
+Converts PNG data in memory to raw pixel data.
+out: Output parameter. Pointer to buffer that will contain the raw pixel data.
+ Its size is w * h * (bytes per pixel), bytes per pixel depends on colorType and bitDepth.
+ Must be freed after usage with free(*out).
+w: Output parameter. Pointer to width of pixel data.
+h: Output parameter. Pointer to height of pixel data.
+in: Memory buffer with the PNG file.
+insize: size of the in buffer.
+colorType: the desired color type for the raw output image. See explanation on PNG color types.
+bitDepth: the desired bit depth for the raw output image. See explanation on PNG color types.
+Return value: LodePNG error code (0 means no error).
+*/
+unsigned LodePNG_decode(unsigned char** out, unsigned* w, unsigned* h, const unsigned char* in, size_t insize, unsigned colorType, unsigned bitDepth); /*return value is error*/
+
+/*
+LodePNG_decode32
+Converts PNG data in memory to 32-bit raw pixel data.
+Same as LodePNG_decode, but uses colorType = 6 and bitDepth = 8 by default.
+out: Output parameter. Pointer to buffer that will contain the raw pixel data.
+ Its size is w * h * 4 bytes.
+ Must be freed after usage with free(*out).
+w: Output parameter. Pointer to width of pixel data.
+h: Output parameter. Pointer to height of pixel data.
+in: Memory buffer with the PNG file.
+insize: size of the in buffer.
+Return value: LodePNG error code (0 means no error).
+*/
+unsigned LodePNG_decode32(unsigned char** out, unsigned* w, unsigned* h, const unsigned char* in, size_t insize); /*return value is error*/
+
+#ifdef LODEPNG_COMPILE_DISK
+
+/*
+LodePNG_decode_file
+Load PNG from disk, from file with given name.
+out: Output parameter. Pointer to buffer that will contain the raw pixel data.
+ Its size is w * h * (bytes per pixel), bytes per pixel depends on colorType and bitDepth.
+ Must be freed after usage with free(*out).
+w: Output parameter. Pointer to width of pixel data.
+h: Output parameter. Pointer to height of pixel data.
+filename: Path on disk of the PNG file.
+colorType: the desired color type for the raw output image. See explanation on PNG color types.
+bitDepth: the desired bit depth for the raw output image. See explanation on PNG color types.
+Return value: LodePNG error code (0 means no error).
+*/
+unsigned LodePNG_decode_file(unsigned char** out, unsigned* w, unsigned* h, const char* filename, unsigned colorType, unsigned bitDepth);
+
+/*
+LodePNG_decode32_file
+Load PNG from disk to 32-bit RGBA pixel buffer, from file with given name.
+Same as LodePNG_decode_file, but uses colorType = 6 and bitDepth = 8 by default.
+out: Output parameter. Pointer to buffer that will contain the raw pixel data.
+ Its size is w * h * 4 bytes.
+ Must be freed after usage with free(*out).
+w: Output parameter. Pointer to width of pixel data.
+h: Output parameter. Pointer to height of pixel data.
+in: Memory buffer with the PNG file.
+insize: size of the in buffer.
+Return value: LodePNG error code (0 means no error).
+*/
+unsigned LodePNG_decode32_file(unsigned char** out, unsigned* w, unsigned* h, const char* filename);
+
+#endif /*LODEPNG_COMPILE_DISK*/
+#endif /*LODEPNG_COMPILE_DECODER*/
+#ifdef LODEPNG_COMPILE_ENCODER
+
+/*
+LodePNG_encode
+Converts raw pixel data into a PNG image in memory. The colorType and bitDepth
+ of the output PNG image cannot be chosen, they are automatically determined
+ by the colorType, bitDepth and content of the input pixel data.
+out: Output parameter. Pointer to buffer that will contain the raw pixel data.
+ Must be freed after usage with free(*out).
+outsize: Output parameter. Pointer to the size in bytes of the out buffer.
+image: The raw pixel data to encode. The size of this buffer should be
+ w * h * (bytes per pixel), bytes per pixel depends on colorType and bitDepth.
+w: width of the raw pixel data in pixels.
+h: height of the raw pixel data in pixels.
+colorType: the color type of the raw input image. See explanation on PNG color types.
+bitDepth: the bit depth of the raw input image. See explanation on PNG color types.
+Return value: LodePNG error code (0 means no error).
+*/
+unsigned LodePNG_encode(unsigned char** out, size_t* outsize, const unsigned char* image, unsigned w, unsigned h, unsigned colorType, unsigned bitDepth); /*return value is error*/
+
+/*
+LodePNG_encode32
+Converts 32-bit RGBA raw pixel data into a PNG image in memory.
+Same as LodePNG_encode, but uses colorType = 6 and bitDepth = 8 by default.
+out: Output parameter. Pointer to buffer that will contain the raw pixel data.
+ Must be freed after usage with free(*out).
+outsize: Output parameter. Pointer to the size in bytes of the out buffer.
+image: The raw pixel data to encode. The size of this buffer should be w * h * 4 bytes.
+w: width of the raw pixel data in pixels.
+h: height of the raw pixel data in pixels.
+Return value: LodePNG error code (0 means no error).
+*/
+unsigned LodePNG_encode32(unsigned char** out, size_t* outsize, const unsigned char* image, unsigned w, unsigned h); /*return value is error*/
+
+#ifdef LODEPNG_COMPILE_DISK
+
+/*
+LodePNG_encode_file
+Converts raw pixel data into a PNG file on disk. Same as LodePNG_encode, but
+outputs to disk instead of memory buffer.
+filename: path to file on disk to write the PNG image to.
+image: The raw pixel data to encode. The size of this buffer should be
+ w * h * (bytes per pixel), bytes per pixel depends on colorType and bitDepth.
+w: width of the raw pixel data in pixels.
+h: height of the raw pixel data in pixels.
+colorType: the color type of the raw input image. See explanation on PNG color types.
+bitDepth: the bit depth of the raw input image. See explanation on PNG color types.
+Return value: LodePNG error code (0 means no error).
+*/
+unsigned LodePNG_encode_file(const char* filename, const unsigned char* image, unsigned w, unsigned h, unsigned colorType, unsigned bitDepth);
+
+/*
+LodePNG_encode_file
+Converts 32-bit RGBA raw pixel data into a PNG file on disk. Same as LodePNG_encode_file,
+but uses colorType = 6 and bitDepth = 8 by default.
+filename: path to file on disk to write the PNG image to.
+image: The raw pixel data to encode. The size of this buffer should be w * h * 4 bytes.
+w: width of the raw pixel data in pixels.
+h: height of the raw pixel data in pixels.
+Return value: LodePNG error code (0 means no error).
+*/
+unsigned LodePNG_encode32_file(const char* filename, const unsigned char* image, unsigned w, unsigned h);
+#endif /*LODEPNG_COMPILE_DISK*/
+#endif /*LODEPNG_COMPILE_ENCODER*/
+
+
+#ifdef __cplusplus
+namespace LodePNG
+{
+#ifdef LODEPNG_COMPILE_DECODER
+
+ /*
+ LodePNG::decode
+ Converts PNG data in memory to raw pixel data.
+ out: Output parameter, std::vector containing the raw pixel data. Its size
+ will be w * h * (bytes per pixel), where bytes per pixel is 4 if the default
+ colorType=6 and bitDepth=8 is used. The pixels are 32-bit RGBA bit in that case.
+ w: Output parameter, width of the image in pixels.
+ h: Output parameter, height of the image in pixels.
+ in: Memory buffer with the PNG file.
+ insize: size of the in buffer.
+ colorType: the desired color type for the raw output image. See explanation on PNG color types.
+ bitDepth: the desired bit depth for the raw output image. See explanation on PNG color types.
+ Return value: LodePNG error code (0 means no error).
+ */
+ unsigned decode(std::vector<unsigned char>& out, unsigned& w, unsigned& h, const unsigned char* in, size_t insize, unsigned colorType = 6, unsigned bitDepth = 8);
+
+ /*
+ LodePNG::decode
+ Exactly the same as the decode function that takes a unsigned char buffer, but instead of giving
+ a pointer and a size, this takes the input buffer as an std::vector.
+ */
+ unsigned decode(std::vector<unsigned char>& out, unsigned& w, unsigned& h, const std::vector<unsigned char>& in, unsigned colorType = 6, unsigned bitDepth = 8);
+#ifdef LODEPNG_COMPILE_DISK
+ /*
+ LodePNG::decode
+ Converts PNG file from disk to raw pixel data in memory.
+ out: Output parameter, std::vector containing the raw pixel data. Its size
+ will be w * h * (bytes per pixel), where bytes per pixel is 4 if the default
+ colorType=6 and bitDepth=8 is used. The pixels are 32-bit RGBA bit in that case.
+ w: Output parameter, width of the image in pixels.
+ h: Output parameter, height of the image in pixels.
+ filename: Path to PNG file on disk.
+ colorType: the desired color type for the raw output image. See explanation on PNG color types.
+ bitDepth: the desired bit depth for the raw output image. See explanation on PNG color types.
+ Return value: LodePNG error code (0 means no error).
+ */
+ unsigned decode(std::vector<unsigned char>& out, unsigned& w, unsigned& h, const std::string& filename, unsigned colorType = 6, unsigned bitDepth = 8);
+#endif //LODEPNG_COMPILE_DISK
+#endif //LODEPNG_COMPILE_DECODER
+
+#ifdef LODEPNG_COMPILE_ENCODER
+
+ /*
+ LodePNG::encode
+ Converts 32-bit RGBA raw pixel data into a PNG image in memory.
+ out: Output parameter, std::vector containing the PNG image data.
+ in: Memory buffer with raw pixel data. The size of this buffer should be
+ w * h * (bytes per pixel), With the default colorType=6 and bitDepth=8, bytes
+ per pixel should be 4 and the data is a 32-bit RGBA pixel buffer.
+ w: Width of the image in pixels.
+ h: Height of the image in pixels.
+ colorType: the color type of the raw input image. See explanation on PNG color types.
+ bitDepth: the bit depth of the raw input image. See explanation on PNG color types.
+ Return value: LodePNG error code (0 means no error).
+ */
+ unsigned encode(std::vector<unsigned char>& out, const unsigned char* in, unsigned w, unsigned h, unsigned colorType = 6, unsigned bitDepth = 8);
+
+ /*
+ LodePNG::encode
+ Exactly the same as the encode function that takes a unsigned char buffer, but instead of giving
+ a pointer and a size, this takes the input buffer as an std::vector.
+ */
+ unsigned encode(std::vector<unsigned char>& out, const std::vector<unsigned char>& in, unsigned w, unsigned h, unsigned colorType = 6, unsigned bitDepth = 8);
+#ifdef LODEPNG_COMPILE_DISK
+ /*
+ LodePNG::encode
+ Converts 32-bit RGBA raw pixel data into a PNG file on disk.
+ filename: Path to the file to write the PNG image to.
+ in: Memory buffer with raw pixel data. The size of this buffer should be
+ w * h * (bytes per pixel), With the default colorType=6 and bitDepth=8, bytes
+ per pixel should be 4 and the data is a 32-bit RGBA pixel buffer.
+ w: Width of the image in pixels.
+ h: Height of the image in pixels.
+ colorType: the color type of the raw input image. See explanation on PNG color types.
+ bitDepth: the bit depth of the raw input image. See explanation on PNG color types.
+ Return value: LodePNG error code (0 means no error).
+ */
+ unsigned encode(const std::string& filename, const unsigned char* in, unsigned w, unsigned h, unsigned colorType = 6, unsigned bitDepth = 8);
+
+ /*
+ LodePNG::encode
+ Exactly the same as the encode function that takes a unsigned char buffer, but instead of giving
+ a pointer and a size, this takes the input buffer as an std::vector.
+ */
+ unsigned encode(const std::string& filename, const std::vector<unsigned char>& in, unsigned w, unsigned h, unsigned colorType = 6, unsigned bitDepth = 8);
+#endif //LODEPNG_COMPILE_DISK
+#endif //LODEPNG_COMPILE_ENCODER
+} //namespace LodePNG
+#endif /*__cplusplus*/
+#endif /*LODEPNG_COMPILE_PNG*/
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* Inflate & Deflate Setting Structs */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+/*
+These structs contain settings for the decompression and compression of the
+PNG files. Typically you won't need these directly.
+*/
+
+#ifdef LODEPNG_COMPILE_DECODER
+typedef struct LodeZlib_DecompressSettings
+{
+ unsigned ignoreAdler32; /*if 1, continue and don't give an error message if the Adler32 checksum is corrupted*/
+} LodeZlib_DecompressSettings;
+
+extern const LodeZlib_DecompressSettings LodeZlib_defaultDecompressSettings;
+void LodeZlib_DecompressSettings_init(LodeZlib_DecompressSettings* settings);
+#endif /*LODEPNG_COMPILE_DECODER*/
+
+#ifdef LODEPNG_COMPILE_ENCODER
+/*
+LodeZlib_CompressSettings
+Compression settings. Tweaking these settings tweaks the balance between
+speed and compression ratio.
+*/
+typedef struct LodeZlib_CompressSettings /*deflate = compress*/
+{
+ /*LZ77 related settings*/
+ unsigned btype; /*the block type for LZ (0, 1, 2 or 3, see zlib standard)*/
+ unsigned useLZ77; /*whether or not to use LZ77. Should be 1 for good compression.*/
+ unsigned windowSize; /*the maximum is 32768, higher gives more compression but is slower*/
+} LodeZlib_CompressSettings;
+
+extern const LodeZlib_CompressSettings LodeZlib_defaultCompressSettings;
+void LodeZlib_CompressSettings_init(LodeZlib_CompressSettings* settings);
+#endif /*LODEPNG_COMPILE_ENCODER*/
+
+#ifdef LODEPNG_COMPILE_PNG
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* PNG and Raw Image Information Structs */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+/*
+LodePNG_InfoColor
+Info about the color type of an image.
+The same LodePNG_InfoColor struct is used for both the PNG and raw image type,
+even though they are two totally different things.
+*/
+typedef struct LodePNG_InfoColor
+{
+ /*header (IHDR)*/
+ unsigned colorType; /*color type, see PNG standard or documentation further in this header file*/
+ unsigned bitDepth; /*bits per sample, see PNG standard or documentation further in this header file*/
+
+ /*
+ palette (PLTE)
+
+ This is a dynamically allocated unsigned char array with the colors of the palette. The value palettesize
+ indicates the amount of colors in the palette. The allocated size of the buffer is 4 * palettesize bytes,
+ because there are 4 values per color: R, G, B and A. Even if less color channels are used, the palette
+ is always in RGBA format, in the order RGBARGBARGBA.....
+
+ When encoding a PNG, to store your colors in the palette of the LodePNG_InfoRaw, first use
+ LodePNG_InfoColor_clearPalette, then for each color use LodePNG_InfoColor_addPalette.
+ In the C++ version the Encoder class also has the above functions available directly in its interface.
+
+ The palette information from the tRNS chunk is also already included in this palette vector.
+
+ If you encode an image with palette, don't forget that you have to set the alpha channels (A) of the palette
+ too, set them to 255 for an opaque palette. If you leave them at zero, the image will be encoded as
+ fully invisible. This both for the palette in the infoRaw and the infoPng if the png is to have a palette.
+ */
+ unsigned char* palette; /*palette in RGBARGBA... order*/
+ size_t palettesize; /*palette size in number of colors (amount of bytes is 4 * palettesize)*/
+
+ /*
+ transparent color key (tRNS)
+ This color is 8-bit for 8-bit PNGs, 16-bit for 16-bit per channel PNGs.
+ For greyscale PNGs, r, g and b will all 3 be set to the same.
+ */
+ unsigned key_defined; /*is a transparent color key given? 0 = false, 1 = true*/
+ unsigned key_r; /*red/greyscale component of color key*/
+ unsigned key_g; /*green component of color key*/
+ unsigned key_b; /*blue component of color key*/
+} LodePNG_InfoColor;
+
+/*init, cleanup and copy functions to use with this struct*/
+void LodePNG_InfoColor_init(LodePNG_InfoColor* info);
+void LodePNG_InfoColor_cleanup(LodePNG_InfoColor* info);
+unsigned LodePNG_InfoColor_copy(LodePNG_InfoColor* dest, const LodePNG_InfoColor* source); /*return value is error code (0 means no error)*/
+
+/*Use these functions instead of allocating palette manually*/
+void LodePNG_InfoColor_clearPalette(LodePNG_InfoColor* info);
+unsigned LodePNG_InfoColor_addPalette(LodePNG_InfoColor* info, unsigned char r, unsigned char g, unsigned char b, unsigned char a); /*add 1 color to the palette*/
+
+/*additional color info*/
+unsigned LodePNG_InfoColor_getBpp(const LodePNG_InfoColor* info); /*get the total amount of bits per pixel, based on colorType and bitDepth in the struct*/
+unsigned LodePNG_InfoColor_getChannels(const LodePNG_InfoColor* info); /*get the amount of color channels used, based on colorType in the struct. If a palette is used, it counts as 1 channel.*/
+unsigned LodePNG_InfoColor_isGreyscaleType(const LodePNG_InfoColor* info); /*is it a greyscale type? (colorType 0 or 4)*/
+unsigned LodePNG_InfoColor_isAlphaType(const LodePNG_InfoColor* info); /*has it got an alpha channel? (colorType 2 or 6)*/
+
+#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS
+/*
+LodePNG_Time
+The information of a Time chunk in PNG.
+To make the encoder add a time chunk, set time_defined to 1 and fill in
+the correct values in all the time parameters. LodePNG will not fill the current
+time in these values itself, all it does is copy them over into the chunk bytes.
+*/
+typedef struct LodePNG_Time
+{
+ unsigned year; /*2 bytes used (0-65535)*/
+ unsigned char month; /*1-12*/
+ unsigned char day; /*1-31*/
+ unsigned char hour; /*0-23*/
+ unsigned char minute; /*0-59*/
+ unsigned char second; /*0-60 (to allow for leap seconds)*/
+} LodePNG_Time;
+
+/*
+LodePNG_Text
+Info about text chunks in a PNG file. The arrays can contain multiple keys
+and strings. The amount of keys and strings is the same. The amount of strings
+ends when the pointer to the string is a null pointer.
+
+They keyword of text chunks gives a short description what the actual text
+represents. There are a few standard standard keywords recognised
+by many programs: Title, Author, Description, Copyright, Creation Time,
+Software, Disclaimer, Warning, Source, Comment. It's allowed to use other keys.
+
+A keyword is minimum 1 character and maximum 79 characters long. It's
+discouraged to use a single line length longer than 79 characters for texts.
+*/
+typedef struct LodePNG_Text /*non-international text*/
+{
+ /*Don't allocate these text buffers yourself. Use the init/cleanup functions
+ correctly and use LodePNG_Text_add and LodePNG_Text_clear.*/
+ size_t num; /*the amount of texts in these char** buffers (there may be more texts in itext)*/
+ char** keys; /*the keyword of a text chunk (e.g. "Comment")*/
+ char** strings; /*the actual text*/
+} LodePNG_Text;
+
+/*init, cleanup and copy functions to use with this struct*/
+void LodePNG_Text_init(LodePNG_Text* text);
+void LodePNG_Text_cleanup(LodePNG_Text* text);
+unsigned LodePNG_Text_copy(LodePNG_Text* dest, const LodePNG_Text* source); /*return value is error code (0 means no error)*/
+
+/*Use these functions instead of allocating the char**s manually*/
+void LodePNG_Text_clear(LodePNG_Text* text); /*use this to clear the texts again after you filled them in*/
+unsigned LodePNG_Text_add(LodePNG_Text* text, const char* key, const char* str); /*push back both texts at once*/
+
+/*
+LodePNG_IText
+Info about international text chunks in a PNG file. The arrays can contain multiple keys
+and strings. The amount of keys, lengtags, transkeys and strings is the same.
+The amount of strings ends when the pointer to the string is a null pointer.
+
+A keyword is minimum 1 character and maximum 79 characters long. It's
+discouraged to use a single line length longer than 79 characters for texts.
+*/
+typedef struct LodePNG_IText /*international text*/
+{
+ /*Don't allocate these text buffers yourself. Use the init/cleanup functions
+ correctly and use LodePNG_IText_add and LodePNG_IText_clear.*/
+ size_t num; /*the amount of international texts in this PNG*/
+ char** keys; /*the English keyword of the text chunk (e.g. "Comment")*/
+ char** langtags; /*the language tag for this text's international language, ISO/IEC 646 string, e.g. ISO 639 language tag*/
+ char** transkeys; /*keyword translated to the international language - UTF-8 string*/
+ char** strings; /*the actual international text - UTF-8 string*/
+} LodePNG_IText;
+
+/*init, cleanup and copy functions to use with this struct*/
+void LodePNG_IText_init(LodePNG_IText* text);
+void LodePNG_IText_cleanup(LodePNG_IText* text);
+unsigned LodePNG_IText_copy(LodePNG_IText* dest, const LodePNG_IText* source); /*return value is error code (0 means no error)*/
+
+/*Use these functions instead of allocating the char**s manually*/
+void LodePNG_IText_clear(LodePNG_IText* text); /*use this to clear the itexts again after you filled them in*/
+unsigned LodePNG_IText_add(LodePNG_IText* text, const char* key, const char* langtag, const char* transkey, const char* str); /*push back the 4 texts of 1 chunk at once*/
+#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/
+
+#ifdef LODEPNG_COMPILE_UNKNOWN_CHUNKS
+/*
+LodePNG_UnknownChunks
+Unknown chunks read from the PNG, or extra chunks the user wants to have added
+in the encoded PNG.
+*/
+typedef struct LodePNG_UnknownChunks
+{
+ /*There are 3 buffers, one for each position in the PNG where unknown chunks can appear
+ each buffer contains all unknown chunks for that position consecutively
+ The 3 buffers are the unknown chunks between certain critical chunks:
+ 0: IHDR-PLTE, 1: PLTE-IDAT, 2: IDAT-IEND
+
+ Do not allocate or traverse this data yourself. Use the chunk traversing functions declared
+ later, such as LodePNG_chunk_next and LodePNG_append_chunk, to read/write this struct.
+ */
+ unsigned char* data[3];
+ size_t datasize[3]; /*size in bytes of the unknown chunks, given for protection*/
+
+} LodePNG_UnknownChunks;
+
+/*init, cleanup and copy functions to use with this struct*/
+void LodePNG_UnknownChunks_init(LodePNG_UnknownChunks* chunks);
+void LodePNG_UnknownChunks_cleanup(LodePNG_UnknownChunks* chunks);
+unsigned LodePNG_UnknownChunks_copy(LodePNG_UnknownChunks* dest, const LodePNG_UnknownChunks* src); /*return value is error code (0 means no error)*/
+#endif /*LODEPNG_COMPILE_UNKNOWN_CHUNKS*/
+
+/*
+LodePNG_InfoPng
+Information about the PNG image, except pixels and sometimes except width and height.
+*/
+typedef struct LodePNG_InfoPng
+{
+ /*header (IHDR), palette (PLTE) and transparency (tRNS)*/
+
+ /*
+ Note: width and height are only used as information of a decoded PNG image. When encoding one, you don't have
+ to specify width and height in an LodePNG_Info struct, but you give them as parameters of the encode function.
+ The rest of the LodePNG_Info struct IS used by the encoder though!
+ */
+ unsigned width; /*width of the image in pixels (ignored by encoder, but filled in by decoder)*/
+ unsigned height; /*height of the image in pixels (ignored by encoder, but filled in by decoder)*/
+ unsigned compressionMethod; /*compression method of the original file. Always 0.*/
+ unsigned filterMethod; /*filter method of the original file*/
+ unsigned interlaceMethod; /*interlace method of the original file*/
+ LodePNG_InfoColor color; /*color type and bits, palette and transparency of the PNG file*/
+
+#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS
+
+ /*
+ suggested background color (bKGD)
+ This color is 8-bit for 8-bit PNGs, 16-bit for 16-bit PNGs
+
+ For greyscale PNGs, r, g and b will all 3 be set to the same. When encoding
+ the encoder writes the red one. For palette PNGs: When decoding, the RGB value
+ will be stored, not a palette index. But when encoding, specify the index of
+ the palette in background_r, the other two are then ignored.
+
+ The decoder does not use this background color to edit the color of pixels.
+ */
+ unsigned background_defined; /*is a suggested background color given?*/
+ unsigned background_r; /*red component of suggested background color*/
+ unsigned background_g; /*green component of suggested background color*/
+ unsigned background_b; /*blue component of suggested background color*/
+
+ /*non-international text chunks (tEXt and zTXt)*/
+ LodePNG_Text text;
+
+ /*international text chunks (iTXt)*/
+ LodePNG_IText itext;
+
+ /*time chunk (tIME)*/
+ unsigned char time_defined; /*if 0, no tIME chunk was or will be generated in the PNG image*/
+ LodePNG_Time time;
+
+ /*phys chunk (pHYs)*/
+ unsigned phys_defined; /*if 0, there is no pHYs chunk and the values below are undefined, if 1 else there is one*/
+ unsigned phys_x; /*pixels per unit in x direction*/
+ unsigned phys_y; /*pixels per unit in y direction*/
+ unsigned char phys_unit; /*may be 0 (unknown unit) or 1 (metre)*/
+
+#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/
+
+#ifdef LODEPNG_COMPILE_UNKNOWN_CHUNKS
+ /*unknown chunks*/
+ LodePNG_UnknownChunks unknown_chunks;
+#endif /*LODEPNG_COMPILE_UNKNOWN_CHUNKS*/
+
+} LodePNG_InfoPng;
+
+/*init, cleanup and copy functions to use with this struct*/
+void LodePNG_InfoPng_init(LodePNG_InfoPng* info);
+void LodePNG_InfoPng_cleanup(LodePNG_InfoPng* info);
+unsigned LodePNG_InfoPng_copy(LodePNG_InfoPng* dest, const LodePNG_InfoPng* source); /*return value is error code (0 means no error)*/
+
+/*
+LodePNG_InfoRaw
+Contains user-chosen information about the raw image data, which is independent of the PNG image
+With raw images, I mean the image data in the form of the simple raw buffer to which the
+compressed PNG data is decoded, or from which a PNG image can be encoded.
+*/
+typedef struct LodePNG_InfoRaw
+{
+ LodePNG_InfoColor color; /*color info of the raw image, note that the same struct as for PNG data is used.*/
+} LodePNG_InfoRaw;
+
+/*init, cleanup and copy functions to use with this struct*/
+void LodePNG_InfoRaw_init(LodePNG_InfoRaw* info);
+void LodePNG_InfoRaw_cleanup(LodePNG_InfoRaw* info);
+unsigned LodePNG_InfoRaw_copy(LodePNG_InfoRaw* dest, const LodePNG_InfoRaw* source); /*return value is error code (0 means no error)*/
+
+/*
+LodePNG_convert
+Converts raw buffer from one color type to another color type, based on
+LodePNG_InfoColor structs to describe the input and output color type.
+See the reference manual at the end of this header file to see which color conversions are supported.
+return value = LodePNG error code (0 if all went ok, an error if the conversion isn't supported)
+The out buffer must have size (w * h * bpp + 7) / 8, where bpp is the bits per pixel
+of the output color type (LodePNG_InfoColor_getBpp)
+*/
+unsigned LodePNG_convert(unsigned char* out, const unsigned char* in, LodePNG_InfoColor* infoOut, LodePNG_InfoColor* infoIn, unsigned w, unsigned h);
+
+#ifdef LODEPNG_COMPILE_DECODER
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* LodePNG Decoder */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+/*
+Settings for the decoder. This contains settings for the PNG and the Zlib
+decoder, but not the Info settings from the Info structs.
+*/
+typedef struct LodePNG_DecodeSettings
+{
+ LodeZlib_DecompressSettings zlibsettings; /*in here is the setting to ignore Adler32 checksums*/
+
+ unsigned ignoreCrc; /*ignore CRC checksums*/
+ unsigned color_convert; /*whether to convert the PNG to the color type you want. Default: yes*/
+
+#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS
+ unsigned readTextChunks; /*if false but rememberUnknownChunks is true, they're stored in the unknown chunks*/
+#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/
+
+#ifdef LODEPNG_COMPILE_UNKNOWN_CHUNKS
+ unsigned rememberUnknownChunks; /*store all bytes from unknown chunks in the InfoPng (off by default, useful for a png editor)*/
+#endif /*LODEPNG_COMPILE_UNKNOWN_CHUNKS*/
+} LodePNG_DecodeSettings;
+
+void LodePNG_DecodeSettings_init(LodePNG_DecodeSettings* settings);
+
+/*
+The LodePNG_Decoder struct has most input and output parameters the decoder uses,
+such as the settings, the info of the PNG and the raw data, and the error. Only
+the pixel buffer is not contained in this struct.
+*/
+typedef struct LodePNG_Decoder
+{
+ LodePNG_DecodeSettings settings; /*the decoding settings*/
+ LodePNG_InfoRaw infoRaw; /*specifies the format in which you would like to get the raw pixel buffer*/
+ LodePNG_InfoPng infoPng; /*info of the PNG image obtained after decoding*/
+ unsigned error;
+} LodePNG_Decoder;
+
+/*init, cleanup and copy functions to use with this struct*/
+void LodePNG_Decoder_init(LodePNG_Decoder* decoder);
+void LodePNG_Decoder_cleanup(LodePNG_Decoder* decoder);
+void LodePNG_Decoder_copy(LodePNG_Decoder* dest, const LodePNG_Decoder* source);
+
+/*
+LodePNG_Decoder_decode
+Decode based on a LodePNG_Decoder.
+This function allocates the out buffer and stores the size in *outsize. This buffer
+needs to be freed after usage.
+Other information about the PNG file, such as the size, colorType and extra chunks
+are stored in the infoPng field of the LodePNG_Decoder.
+*/
+void LodePNG_Decoder_decode(LodePNG_Decoder* decoder, unsigned char** out, size_t* outsize, const unsigned char* in, size_t insize);
+
+/*
+LodePNG_Decoder_inspect
+Read the PNG header, but not the actual data. This returns only the information
+that is in the header chunk of the PNG, such as width, height and color type. The
+information is placed in the infoPng field of the LodePNG_Decoder.
+*/
+void LodePNG_Decoder_inspect(LodePNG_Decoder* decoder, const unsigned char* in, size_t insize); /*read the png header*/
+
+#endif /*LODEPNG_COMPILE_DECODER*/
+
+#ifdef LODEPNG_COMPILE_ENCODER
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* LodePNG Encoder */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+/*
+LodePNG_EncodeSettings
+Extra settings used by the encoder.
+*/
+typedef struct LodePNG_EncodeSettings
+{
+ LodeZlib_CompressSettings zlibsettings; /*settings for the zlib encoder, such as window size, ...*/
+
+ unsigned autoLeaveOutAlphaChannel; /*automatically use color type without alpha instead of given one, if given image is opaque*/
+ unsigned force_palette; /*force creating a PLTE chunk if colortype is 2 or 6 (= a suggested palette). If colortype is 3, PLTE is _always_ created.*/
+#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS
+ unsigned add_id; /*add LodePNG version as text chunk*/
+ unsigned text_compression; /*encode text chunks as zTXt chunks instead of tEXt chunks, and use compression in iTXt chunks*/
+#endif /*LODEPNG_COMPILE_ANCILLARY_CHUNKS*/
+} LodePNG_EncodeSettings;
+
+void LodePNG_EncodeSettings_init(LodePNG_EncodeSettings* settings);
+
+/*
+LodePNG_Encoder
+This struct has most input and output parameters the encoder uses,
+such as the settings, the info of the PNG and the raw data, and the error. Only
+the pixel buffer is not contained in this struct.
+*/
+typedef struct LodePNG_Encoder
+{
+ LodePNG_EncodeSettings settings; /*compression settings of the encoder*/
+ LodePNG_InfoPng infoPng; /*the info specified by the user is not changed by the encoder. The encoder will try to generate a PNG close to the given info.*/
+ LodePNG_InfoRaw infoRaw; /*put the properties of the input raw image in here*/
+ unsigned error; /*error value filled in if error happened, or 0 if all went ok*/
+} LodePNG_Encoder;
+
+/*init, cleanup and copy functions to use with this struct*/
+void LodePNG_Encoder_init(LodePNG_Encoder* encoder);
+void LodePNG_Encoder_cleanup(LodePNG_Encoder* encoder);
+void LodePNG_Encoder_copy(LodePNG_Encoder* dest, const LodePNG_Encoder* source);
+
+/*This function allocates the out buffer with standard malloc and stores the size in *outsize.*/
+void LodePNG_Encoder_encode(LodePNG_Encoder* encoder, unsigned char** out, size_t* outsize, const unsigned char* image, unsigned w, unsigned h);
+
+#endif /*LODEPNG_COMPILE_ENCODER*/
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* Chunk Traversing Utilities */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+/*
+LodePNG_chunk functions:
+These functions need as input a large enough amount of allocated memory.
+These functions can be used on raw PNG data, but they are exposed in the API
+because they are needed if you want to traverse the unknown chunks stored
+in the LodePNG_UnknownChunks struct, or add new ones to it.
+*/
+
+unsigned LodePNG_chunk_length(const unsigned char* chunk); /*get the length of the data of the chunk. Total chunk length has 12 bytes more.*/
+
+void LodePNG_chunk_type(char type[5], const unsigned char* chunk); /*puts the 4-byte type in null terminated string*/
+unsigned char LodePNG_chunk_type_equals(const unsigned char* chunk, const char* type); /*check if the type is the given type*/
+
+/*
+These functions get properties of PNG chunks gotten from capitalization of chunk
+type name, as defined by the PNG standard.
+*/
+unsigned char LodePNG_chunk_critical(const unsigned char* chunk); /*0: ancillary chunk, 1: it's one of the critical chunk types*/
+unsigned char LodePNG_chunk_private(const unsigned char* chunk); /*0: public, 1: private*/
+unsigned char LodePNG_chunk_safetocopy(const unsigned char* chunk); /*0: the chunk is unsafe to copy, 1: the chunk is safe to copy*/
+
+unsigned char* LodePNG_chunk_data(unsigned char* chunk); /*get pointer to the data of the chunk*/
+const unsigned char* LodePNG_chunk_data_const(const unsigned char* chunk); /*get pointer to the data of the chunk*/
+
+unsigned LodePNG_chunk_check_crc(const unsigned char* chunk); /*returns 0 if the crc is correct, 1 if it's incorrect*/
+void LodePNG_chunk_generate_crc(unsigned char* chunk); /*generates the correct CRC from the data and puts it in the last 4 bytes of the chunk*/
+
+/*iterate to next chunks.*/
+unsigned char* LodePNG_chunk_next(unsigned char* chunk);
+const unsigned char* LodePNG_chunk_next_const(const unsigned char* chunk);
+
+/*
+LodePNG_append_chunk
+Appends chunk to the data in out. The given chunk should already have its chunk header.
+The out variable and outlength are updated to reflect the new reallocated buffer.
+Returns error code (0 if it went ok)
+*/
+unsigned LodePNG_append_chunk(unsigned char** out, size_t* outlength, const unsigned char* chunk);
+
+/*
+LodePNG_create_chunk
+Appends new chunk to out. The chunk to append is given by giving its length, type
+and data separately. The type is a 4-letter string.
+The out variable and outlength are updated to reflect the new reallocated buffer.
+Returne error code (0 if it went ok)
+*/
+unsigned LodePNG_create_chunk(unsigned char** out, size_t* outlength, unsigned length, const char* type, const unsigned char* data);
+
+#endif /*LODEPNG_COMPILE_PNG*/
+
+#ifdef LODEPNG_COMPILE_ZLIB
+/* ////////////////////////////////////////////////////////////////////////// */
+/* Zlib encoder and decoder */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+/*
+This is "LodeZlib". A C++ wrapper is available further on.
+
+LodeZlib can be used to zlib compress and decompress a buffer. It cannot be
+used to create gzip files however, and it only supports the part of zlib
+that is required for PNG, it does not support dictionaries.
+*/
+
+#ifdef LODEPNG_COMPILE_DECODER
+/*This function reallocates the out buffer and appends the data.
+Either, *out must be NULL and *outsize must be 0, or, *out must be a valid buffer and *outsize its size in bytes.*/
+unsigned LodeZlib_decompress(unsigned char** out, size_t* outsize, const unsigned char* in, size_t insize, const LodeZlib_DecompressSettings* settings);
+#endif /*LODEPNG_COMPILE_DECODER*/
+
+#ifdef LODEPNG_COMPILE_ENCODER
+/*This function reallocates the out buffer and appends the data.
+Either, *out must be NULL and *outsize must be 0, or, *out must be a valid buffer and *outsize its size in bytes.*/
+unsigned LodeZlib_compress(unsigned char** out, size_t* outsize, const unsigned char* in, size_t insize, const LodeZlib_CompressSettings* settings);
+#endif /*LODEPNG_COMPILE_ENCODER*/
+#endif /*LODEPNG_COMPILE_ZLIB*/
+
+#ifdef LODEPNG_COMPILE_DISK
+/*
+LodePNG_loadFile
+Load a file from disk into buffer. The function allocates the out buffer, and
+after usage you are responsible for freeing it.
+out: output parameter, contains pointer to loaded buffer.
+outsize: output parameter, size of the allocated out buffer
+filename: the path to the file to load
+return value: error code (0 means ok)
+*/
+unsigned LodePNG_loadFile(unsigned char** out, size_t* outsize, const char* filename);
+
+/*
+LodePNG_saveFile
+Save a file from buffer to disk. Warning, this function overwrites the file without warning!
+buffer: the buffer to write
+buffersize: size of the buffer to write
+filename: the path to the file to save to
+return value: error code (0 means ok)
+*/
+unsigned LodePNG_saveFile(const unsigned char* buffer, size_t buffersize, const char* filename);
+#endif /*LODEPNG_COMPILE_DISK*/
+
+#ifdef __cplusplus
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* LodePNG C++ wrapper */
+/* ////////////////////////////////////////////////////////////////////////// */
+
+//The LodePNG C++ wrapper uses classes with handy constructors and destructors
+//instead of manual init and cleanup functions, and uses std::vectors instead of
+//manually allocated memory buffers.
+
+#ifdef LODEPNG_COMPILE_ZLIB
+//The C++ wrapper for LodeZlib
+namespace LodeZlib
+{
+#ifdef LODEPNG_COMPILE_DECODER
+
+ //Zlib-decompress an unsigned char buffer
+ unsigned decompress(std::vector<unsigned char>& out, const unsigned char* in, size_t insize, const LodeZlib_DecompressSettings& settings = LodeZlib_defaultDecompressSettings);
+
+ //Zlib-decompress an std::vector
+ unsigned decompress(std::vector<unsigned char>& out, const std::vector<unsigned char>& in, const LodeZlib_DecompressSettings& settings = LodeZlib_defaultDecompressSettings);
+
+#endif //LODEPNG_COMPILE_DECODER
+#ifdef LODEPNG_COMPILE_ENCODER
+
+ //Zlib-compress an unsigned char buffer
+ unsigned compress(std::vector<unsigned char>& out, const unsigned char* in, size_t insize, const LodeZlib_CompressSettings& settings = LodeZlib_defaultCompressSettings);
+
+ //Zlib-compress an std::vector
+ unsigned compress(std::vector<unsigned char>& out, const std::vector<unsigned char>& in, const LodeZlib_CompressSettings& settings = LodeZlib_defaultCompressSettings);
+
+#endif //LODEPNG_COMPILE_ENCODER
+}
+#endif //LODEPNG_COMPILE_ZLIB
+
+#ifdef LODEPNG_COMPILE_PNG
+namespace LodePNG
+{
+
+#ifdef LODEPNG_COMPILE_DECODER
+ /*
+ LodePNG::Decoder
+ Class to decode a PNG image. Before decoding, settings can be set and
+ after decoding, extra information about the PNG can be retrieved.
+ Extends from the C-struct LodePNG_Decoder to add constructors and destructors
+ to initialize/cleanup it automatically. Beware, no virtual destructor is used.
+ */
+ class Decoder : public LodePNG_Decoder
+ {
+ public:
+
+ Decoder();
+ ~Decoder();
+ void operator=(const LodePNG_Decoder& other);
+
+ //decode PNG buffer to raw out buffer. Width and height can be retrieved with getWidth() and getHeight() and error should be checked with hasError() and getError()
+ void decode(std::vector<unsigned char>& out, const unsigned char* in, size_t insize);
+
+ //decode PNG buffer to raw out buffer. Width and height can be retrieved with getWidth() and getHeight() and error should be checked with hasError() and getError()
+ void decode(std::vector<unsigned char>& out, const std::vector<unsigned char>& in);
+
+ //inspect functions: get only the info from the PNG header. The info can then be retrieved with the functions of this class.
+ void inspect(const unsigned char* in, size_t insize);
+
+ //inspect functions: get only the info from the PNG header. The info can then be retrieved with the functions of this class.
+ void inspect(const std::vector<unsigned char>& in);
+
+ //error checking after decoding
+ bool hasError() const;
+ unsigned getError() const;
+
+ //convenient access to some InfoPng parameters after decoding
+ unsigned getWidth() const; //width of image in pixels
+ unsigned getHeight() const; //height of image in pixels
+ unsigned getBpp(); //bits per pixel
+ unsigned getChannels(); //amount of channels
+ unsigned isGreyscaleType(); //is it a greyscale type? (colorType 0 or 4)
+ unsigned isAlphaType(); //has it an alpha channel? (colorType 2 or 6)
+
+ //getters and setters for the decoding settings
+ const LodePNG_DecodeSettings& getSettings() const;
+ LodePNG_DecodeSettings& getSettings();
+ void setSettings(const LodePNG_DecodeSettings& info);
+
+ //getters and setters for the PNG image info, after decoding this describes information of the PNG image
+ const LodePNG_InfoPng& getInfoPng() const;
+ LodePNG_InfoPng& getInfoPng();
+ void setInfoPng(const LodePNG_InfoPng& info);
+ void swapInfoPng(LodePNG_InfoPng& info); //faster than copying with setInfoPng
+
+ //getters and setters for the raw image info, this determines in what format you get the pixel buffer from the decoder
+ const LodePNG_InfoRaw& getInfoRaw() const;
+ LodePNG_InfoRaw& getInfoRaw();
+ void setInfoRaw(const LodePNG_InfoRaw& info);
+ };
+
+#endif //LODEPNG_COMPILE_DECODER
+
+#ifdef LODEPNG_COMPILE_ENCODER
+ /*
+ LodePNG::Encoder
+ Class to encode a PNG image. Before encoding, settings can be set.
+ Extends from the C-struct LodePNG_Enoder to add constructors and destructors
+ to initialize/cleanup it automatically. Beware, no virtual destructor is used.
+ */
+ class Encoder : public LodePNG_Encoder
+ {
+ public:
+
+ Encoder();
+ ~Encoder();
+ void operator=(const LodePNG_Encoder& other);
+
+ //encoding image to PNG buffer
+ void encode(std::vector<unsigned char>& out, const unsigned char* image, unsigned w, unsigned h);
+
+ //encoding image to PNG buffer
+ void encode(std::vector<unsigned char>& out, const std::vector<unsigned char>& image, unsigned w, unsigned h);
+
+ //error checking after decoding
+ bool hasError() const;
+ unsigned getError() const;
+
+ //convenient direct access to some parameters of the InfoPng
+ void clearPalette();
+ void addPalette(unsigned char r, unsigned char g, unsigned char b, unsigned char a); //add 1 color to the palette
+#ifdef LODEPNG_COMPILE_ANCILLARY_CHUNKS
+ void clearText();
+ void addText(const std::string& key, const std::string& str); //push back both texts at once
+ void clearIText();
+ void addIText(const std::string& key, const std::string& langtag, const std::string& transkey, const std::string& str);
+#endif //LODEPNG_COMPILE_ANCILLARY_CHUNKS
+
+ //getters and setters for the encoding settings
+ const LodePNG_EncodeSettings& getSettings() const;
+ LodePNG_EncodeSettings& getSettings();
+ void setSettings(const LodePNG_EncodeSettings& info);
+
+ //getters and setters for the PNG image info, this describes what color type and other settings the resulting PNG should have
+ const LodePNG_InfoPng& getInfoPng() const;
+ LodePNG_InfoPng& getInfoPng();
+ void setInfoPng(const LodePNG_InfoPng& info);
+ void swapInfoPng(LodePNG_InfoPng& info); //faster than copying with setInfoPng
+
+ //getters and setters for the raw image info, this describes how the encoder should interpret the input pixel buffer
+ const LodePNG_InfoRaw& getInfoRaw() const;
+ LodePNG_InfoRaw& getInfoRaw();
+ void setInfoRaw(const LodePNG_InfoRaw& info);
+ };
+
+#endif //LODEPNG_COMPILE_ENCODER
+
+#ifdef LODEPNG_COMPILE_DISK
+ /*
+ loadFile
+ Load a file from disk into an std::vector. If the vector is empty, then either
+ the file doesn't exist or is an empty file.
+ */
+ void loadFile(std::vector<unsigned char>& buffer, const std::string& filename);
+
+ /*
+ saveFile
+ Save the binary data in an std::vector to a file on disk. The file is overwritten
+ without warning.
+ */
+ void saveFile(const std::vector<unsigned char>& buffer, const std::string& filename);
+#endif //LODEPNG_COMPILE_DISK
+
+} //namespace LodePNG
+
+#endif //LODEPNG_COMPILE_PNG
+
+#endif /*end of __cplusplus wrapper*/
+
+/*
+TODO:
+[ ] test if there are no memory leaks or security exploits - done a lot but needs to be checked often
+[ ] LZ77 encoder more like the one described in zlib - to make sure it's patentfree
+[ ] converting color to 16-bit per channel types
+[ ] read all public PNG chunk types (but never let the color profile and gamma ones ever touch RGB values, that is very annoying for textures as well as images in a browser)
+[ ] make sure encoder generates no chunks with size > (2^31)-1
+[ ] partial decoding (stream processing)
+[ ] let the "isFullyOpaque" function check color keys and transparent palettes too
+[ ] better name for the variables "codes", "codesD", "codelengthcodes", "clcl" and "lldl"
+[ ] check compatibility with vareous compilers - done but needs to be redone for every newer version
+[ ] don't stop decoding on errors like 69, 57, 58 (make warnings that the decoder stores in the error at the very end? and make some errors just let it stop with this one chunk but still do the next ones)
+[ ] make option to choose if the raw image with non multiple of 8 bits per scanline should have padding bits or not, if people like storing raw images that way
+*/
+
+#endif
+
+/*
+LodePNG Documentation
+---------------------
+
+This documentations contains background information and examples. For the function
+and class documentation, see the comments in the declarations above.
+
+0. table of contents
+--------------------
+
+ 1. about
+ 1.1. supported features
+ 1.2. features not supported
+ 2. C and C++ version
+ 3. security
+ 4. decoding
+ 5. encoding
+ 6. color conversions
+ 6.1. PNG color types
+ 6.2. Default Behaviour of LodePNG
+ 6.3. Color Conversions
+ 6.4. More Notes
+ 7. error values
+ 8. chunks and PNG editing
+ 9. compiler support
+ 10. examples
+ 10.1. decoder C++ example
+ 10.2. encoder C++ example
+ 10.3. decoder C example
+ 11. changes
+ 12. contact information
+
+
+1. about
+--------
+
+PNG is a file format to store raster images losslessly with good compression,
+supporting different color types. It can be implemented in a patent-free way.
+
+LodePNG is a PNG codec according to the Portable Network Graphics (PNG)
+Specification (Second Edition) - W3C Recommendation 10 November 2003.
+
+The specifications used are:
+
+*) Portable Network Graphics (PNG) Specification (Second Edition):
+ http://www.w3.org/TR/2003/REC-PNG-20031110
+*) RFC 1950 ZLIB Compressed Data Format version 3.3:
+ http://www.gzip.org/zlib/rfc-zlib.html
+*) RFC 1951 DEFLATE Compressed Data Format Specification ver 1.3:
+ http://www.gzip.org/zlib/rfc-deflate.html
+
+The most recent version of LodePNG can currently be found at
+http://members.gamedev.net/lode/projects/LodePNG/
+
+LodePNG works both in C (ISO C90) and C++, with a C++ wrapper that adds
+extra functionality.
+
+LodePNG exists out of two files:
+-lodepng.h: the header file for both C and C++
+-lodepng.c(pp): give it the name lodepng.c or lodepng.cpp depending on your usage
+
+If you want to start using LodePNG right away without reading this doc, get the
+files lodepng_examples.c or lodepng_examples.cpp to see how to use it in code,
+or check the (smaller) examples in chapter 13 here.
+
+LodePNG is simple but only supports the basic requirements. To achieve
+simplicity, the following design choices were made: There are no dependencies
+on any external library. To decode PNGs, there's a Decoder struct or class that
+can convert any PNG file data into an RGBA image buffer with a single function
+call. To encode PNGs, there's an Encoder struct or class that can convert image
+data into PNG file data with a single function call. To read and write files,
+there are simple functions to convert the files to/from buffers in memory.
+
+This all makes LodePNG suitable for loading textures in games, demoscene
+productions, saving a screenshot, images in programs that require them for simple
+usage, ... It's less suitable for full fledged image editors, loading PNGs
+over network (it requires all the image data to be available before decoding can
+begin), life-critical systems, ...
+LodePNG has a standards conformant decoder and encoder, and supports the ability
+to make a somewhat conformant editor.
+
+1.1. supported features
+-----------------------
+
+The following features are supported by the decoder:
+
+*) decoding of PNGs with any color type, bit depth and interlace mode, to a 24- or 32-bit color raw image, or the same color type as the PNG
+*) encoding of PNGs, from any raw image to 24- or 32-bit color, or the same color type as the raw image
+*) Adam7 interlace and deinterlace for any color type
+*) loading the image from harddisk or decoding it from a buffer from other sources than harddisk
+*) support for alpha channels, including RGBA color model, translucent palettes and color keying
+*) zlib decompression (inflate)
+*) zlib compression (deflate)
+*) CRC32 and ADLER32 checksums
+*) handling of unknown chunks, allowing making a PNG editor that stores custom and unknown chunks.
+*) the following chunks are supported (generated/interpreted) by both encoder and decoder:
+ IHDR: header information
+ PLTE: color palette
+ IDAT: pixel data
+ IEND: the final chunk
+ tRNS: transparency for palettized images
+ tEXt: textual information
+ zTXt: compressed textual information
+ iTXt: international textual information
+ bKGD: suggested background color
+ pHYs: physical dimensions
+ tIME: modification time
+
+1.2. features not supported
+---------------------------
+
+The following features are _not_ supported:
+
+*) some features needed to make a conformant PNG-Editor might be still missing.
+*) partial loading/stream processing. All data must be available and is processed in one call.
+*) The following public chunks are not supported but treated as unknown chunks by LodePNG
+ cHRM, gAMA, iCCP, sRGB, sBIT, hIST, sPLT
+
+
+2. C and C++ version
+--------------------
+
+The C version uses buffers allocated with alloc that you need to free()
+yourself. On top of that, you need to use init and cleanup functions for each
+struct whenever using a struct from the C version to avoid exploits and memory leaks.
+
+The C++ version has constructors and destructors that take care of these things,
+and uses std::vectors in the interface for storing data.
+
+Both the C and the C++ version are contained in this file! The C++ code depends on
+the C code, the C code works on its own.
+
+These files work without modification for both C and C++ compilers because all the
+additional C++ code is in "#ifdef __cplusplus" blocks that make C-compilers ignore
+it, and the C code is made to compile both with strict ISO C90 and C++.
+
+To use the C++ version, you need to rename the source file to lodepng.cpp (instead
+of lodepng.c), and compile it with a C++ compiler.
+
+To use the C version, you need to rename the source file to lodepng.c (instead
+of lodepng.cpp), and compile it with a C compiler.
+
+
+3. Security
+-----------
+
+As with most software, even if carefully designed, it's always possible that
+LodePNG may contain possible exploits.
+
+If you discover a possible exploit, please let me know, and it will be fixed.
+
+When using LodePNG, care has to be taken with the C version of LodePNG, as well as the C-style
+structs when working with C++. The following conventions are used for all C-style structs:
+
+-if a struct has a corresponding init function, always call the init function when making a new one, to avoid exploits
+-if a struct has a corresponding cleanup function, call it before the struct disappears to avoid memory leaks
+-if a struct has a corresponding copy function, use the copy function instead of "=". The destination must also be inited already!
+
+4. Decoding
+-----------
+
+Decoding converts a PNG compressed image to a raw pixel buffer.
+
+Most documentation on using the decoder is at its declarations in the header
+above. For C, simple decoding can be done with functions such as LodePNG_decode32,
+and more advanced decoding can be done with the struct LodePNG_Decoder and its
+functions. For C++, simple decoding can be done with the LodePNG::decode functions
+and advanced decoding with the LodePNG::Decoder class.
+
+The Decoder contains 3 components:
+*) LodePNG_InfoPng: it stores information about the PNG (the input) in an LodePNG_InfoPng struct, don't modify this one yourself
+*) Settings: you can specify a few other settings for the decoder to use
+*) LodePNG_InfoRaw: here you can say what type of raw image (the output) you want to get
+
+Some of the parameters described below may be inside the sub-struct "LodePNG_InfoColor color".
+In the C and C++ version, when using Info structs outside of the decoder or encoder, you need to use their
+init and cleanup functions, but normally you use the ones in the decoder that are already handled
+in the init and cleanup functions of the decoder itself.
+
+=LodePNG_InfoPng=
+
+This contains information such as the original color type of the PNG image, text
+comments, suggested background color, etc... More details about the LodePNG_InfoPng struct
+are at its declaration documentation.
+
+Because the dimensions of the image are important, there are shortcuts to get them in the
+C++ version: use decoder.getWidth() and decoder.getHeight().
+In the C version, use decoder.infoPng.width and decoder.infoPng.height.
+
+=LodePNG_InfoRaw=
+
+In the LodePNG_InfoRaw struct of the Decoder, you can specify which color type you want
+the resulting raw image to be. If this is different from the colorType of the
+PNG, then the decoder will automatically convert the result to your LodePNG_InfoRaw
+settings. Not all combinations of color conversions are supported though, see
+a different section for information about the color modes and supported conversions.
+
+Palette of LodePNG_InfoRaw isn't used by the Decoder, when converting from palette color
+to palette color, the values of the pixels are left untouched so that the colors
+will change if the palette is different. Color key of LodePNG_InfoRaw is not used by the
+Decoder. If setting color_convert is false then LodePNG_InfoRaw is completely ignored,
+but it will be modified to match the color type of the PNG so will be overwritten.
+
+By default, 32-bit color is used for the result.
+
+=Settings=
+
+The Settings can be used to ignore the errors created by invalid CRC and Adler32
+chunks, and to disable the decoding of tEXt chunks.
+
+There's also a setting color_convert, true by default. If false, no conversion
+is done, the resulting data will be as it was in the PNG (after decompression)
+and you'll have to puzzle the colors of the pixels together yourself using the
+color type information in the LodePNG_InfoPng.
+
+
+5. Encoding
+-----------
+
+Encoding converts a raw pixel buffer to a PNG compressed image.
+
+Most documentation on using the encoder is at its declarations in the header
+above. For C, simple encoding can be done with functions such as LodePNG_encode32,
+and more advanced decoding can be done with the struct LodePNG_Encoder and its
+functions. For C++, simple encoding can be done with the LodePNG::encode functions
+and advanced decoding with the LodePNG::Encoder class.
+
+Like the decoder, the encoder can also give errors. However it gives less errors
+since the encoder input is trusted, the decoder input (a PNG image that could
+be forged by anyone) is not trusted.
+
+Like the Decoder, the Encoder has 3 components:
+*) LodePNG_InfoRaw: here you say what color type of the raw image (the input) has
+*) Settings: you can specify a few settings for the encoder to use
+*) LodePNG_InfoPng: the same LodePNG_InfoPng struct as created by the Decoder. For the encoder,
+with this you specify how you want the PNG (the output) to be.
+
+Some of the parameters described below may be inside the sub-struct "LodePNG_InfoColor color".
+In the C and C++ version, when using Info structs outside of the decoder or encoder, you need to use their
+init and cleanup functions, but normally you use the ones in the encoder that are already handled
+in the init and cleanup functions of the decoder itself.
+
+=LodePNG_InfoPng=
+
+The Decoder class stores information about the PNG image in an LodePNG_InfoPng object. With
+the Encoder you can do the opposite: you give it an LodePNG_InfoPng object, and it'll try
+to match the LodePNG_InfoPng you give as close as possible in the PNG it encodes. For
+example in the LodePNG_InfoPng you can specify the color type you want to use, possible
+tEXt chunks you want the PNG to contain, etc... For an explanation of all the
+values in LodePNG_InfoPng see a further section. Not all PNG color types are supported
+by the Encoder.
+
+The encoder will not always exactly match the LodePNG_InfoPng struct you give,
+it tries as close as possible. Some things are ignored by the encoder. The width
+and height of LodePNG_InfoPng are ignored as well, because instead the width and
+height of the raw image you give in the input are used. In fact the encoder
+currently uses only the following settings from it:
+-colorType and bitDepth: the ones it supports
+-text chunks, that you can add to the LodePNG_InfoPng with "addText"
+-the color key, if applicable for the given color type
+-the palette, if you encode to a PNG with colorType 3
+-the background color: it'll add a bKGD chunk to the PNG if one is given
+-the interlaceMethod: None (0) or Adam7 (1)
+
+When encoding to a PNG with colorType 3, the encoder will generate a PLTE chunk.
+If the palette contains any colors for which the alpha channel is not 255 (so
+there are translucent colors in the palette), it'll add a tRNS chunk.
+
+=LodePNG_InfoRaw=
+
+You specify the color type of the raw image that you give to the input here,
+including a possible transparent color key and palette you happen to be using in
+your raw image data.
+
+By default, 32-bit color is assumed, meaning your input has to be in RGBA
+format with 4 bytes (unsigned chars) per pixel.
+
+=Settings=
+
+The following settings are supported (some are in sub-structs):
+*) autoLeaveOutAlphaChannel: when this option is enabled, when you specify a PNG
+color type with alpha channel (not to be confused with the color type of the raw
+image you specify!!), but the encoder detects that all pixels of the given image
+are opaque, then it'll automatically use the corresponding type without alpha
+channel, resulting in a smaller PNG image.
+*) btype: the block type for LZ77. 0 = uncompressed, 1 = fixed huffman tree, 2 = dynamic huffman tree (best compression)
+*) useLZ77: whether or not to use LZ77 for compressed block types
+*) windowSize: the window size used by the LZ77 encoder (1 - 32768)
+*) force_palette: if colorType is 2 or 6, you can make the encoder write a PLTE
+ chunk if force_palette is true. This can used as suggested palette to convert
+ to by viewers that don't support more than 256 colors (if those still exist)
+*) add_id: add text chunk "Encoder: LodePNG <version>" to the image.
+*) text_compression: default 0. If 1, it'll store texts as zTXt instead of tEXt chunks.
+ zTXt chunks use zlib compression on the text. This gives a smaller result on
+ large texts but a larger result on small texts (such as a single program name).
+ It's all tEXt or all zTXt though, there's no separate setting per text yet.
+
+
+6. color conversions
+--------------------
+
+In LodePNG, the color mode (bits, channels and palette) used in the PNG image,
+and the color mode used in the raw data, are separate and independently
+configurable. Therefore, LodePNG needs to do conversions from one color mode to
+another. Not all possible conversions are supported (e.g. converting to a color
+model with palette isn't supported). This section will explain which conversions
+are supported and how to configure this. This explains for example when LodePNG
+uses the settings in LodePNG_InfoPng, LodePNG_InfoRaw and Settings.
+
+6.1. PNG color types
+--------------------
+
+A PNG image can have many color types, ranging from 1-bit color to 64-bit color,
+as well as palettized color modes. After the zlib decompression and unfiltering
+in the PNG image is done, the raw pixel data will have that color type and thus
+a certain amount of bits per pixel. If you want the output raw image after
+decoding to have another color type, a conversion is done by LodePNG.
+
+The PNG specification mentions the following color types:
+
+0: greyscale, bit depths 1, 2, 4, 8, 16
+2: RGB, bit depths 8 and 16
+3: palette, bit depths 1, 2, 4 and 8
+4: greyscale with alpha, bit depths 8 and 16
+6: RGBA, bit depths 8 and 16
+
+Bit depth is the amount of bits per color channel.
+
+6.2. Default Behaviour of LodePNG
+---------------------------------
+
+By default, the Decoder will convert the data from the PNG to 32-bit RGBA color,
+no matter what color type the PNG has, so that the result can be used directly
+as a texture in OpenGL etc... without worries about what color type the original
+image has.
+
+The Encoder assumes by default that the raw input you give it is a 32-bit RGBA
+buffer and will store the PNG as either 32 bit or 24 bit depending on whether
+or not any translucent pixels were detected in it.
+
+To get the default behaviour, don't change the values of LodePNG_InfoRaw and LodePNG_InfoPng of
+the encoder, and don't change the values of LodePNG_InfoRaw of the decoder.
+
+6.3. Color Conversions
+----------------------
+
+As explained in the sections about the Encoder and Decoder, you can specify
+color types and bit depths in LodePNG_InfoPng and LodePNG_InfoRaw, to change the default behaviour
+explained above. (for the Decoder you can only specify the LodePNG_InfoRaw, because the
+LodePNG_InfoPng contains what the PNG file has).
+
+To avoid some confusion:
+-the Decoder converts from PNG to raw image
+-the Encoder converts from raw image to PNG
+-the color type and bit depth in LodePNG_InfoRaw, are those of the raw image
+-the color type and bit depth in LodePNG_InfoPng, are those of the PNG
+-if the color type of the LodePNG_InfoRaw and PNG image aren't the same, a conversion
+between the color types is done if the color types are supported. If it is not
+supported, an error is returned.
+
+Supported color conversions:
+-It's possible to load PNGs from any colortype and to save PNGs of any colorType.
+-Both encoder and decoder use the same converter. So both encoder and decoder
+suport the same color types at the input and the output. So the decoder supports
+any type of PNG image and can convert it to certain types of raw image, while the
+encoder supports any type of raw data but only certain color types for the output PNG.
+-The converter can convert from _any_ input color type, to 24-bit RGB or 32-bit RGBA
+-The converter can convert from greyscale input color type, to 8-bit greyscale or greyscale with alpha
+-If both color types are the same, conversion from anything to anything is possible
+-Color types that are invalid according to the PNG specification are not allowed
+-When converting from a type with alpha channel to one without, the alpha channel information is discarded
+-When converting from a type without alpha channel to one with, the result will be opaque except pixels that have the same color as the color key of the input if one was given
+-When converting from 16-bit bitDepth to 8-bit bitDepth, the 16-bit precision information is lost, only the most significant byte is kept
+-Converting from color to greyscale or to palette is not supported on purpose: there are multiple possible algorithms to do this color reduction, LodePNG does not want to pick one and leaves this choice to the user instead, because it's beyond the scope of PNG encoding.
+-Converting from/to a palette type, only keeps the indices, it ignores the colors defined in the palette
+
+No conversion needed...:
+-If the color type of the PNG image and raw image are the same, then no
+conversion is done, and all color types are supported.
+-In the encoder, you can make it save a PNG with any color by giving the
+LodePNG_InfoRaw and LodePNG_InfoPng the same color type.
+-In the decoder, you can make it store the pixel data in the same color type
+as the PNG has, by setting the color_convert setting to false. Settings in
+infoRaw are then ignored.
+
+The function LodePNG_convert does this, which is available in the interface but
+normally isn't needed since the encoder and decoder already call it.
+
+6.4. More Notes
+---------------
+
+In the PNG file format, if a less than 8-bit per pixel color type is used and the scanlines
+have a bit amount that isn't a multiple of 8, then padding bits are used so that each
+scanline starts at a fresh byte.
+However: The raw input image you give to the encoder, and the raw output image you get from the decoder
+will NOT have these padding bits in that case, e.g. in the case of a 1-bit image with a width
+of 7 pixels, the first pixel of the second scanline will the the 8th bit of the first byte,
+not the first bit of a new byte.
+
+7. error values
+---------------
+
+All functions in LodePNG that return an error code, return 0 if everything went
+OK, or one of the codes described below if there was an error.
+
+This are meanings of the LodePNG error values:
+
+*) 0: no error, everything went ok
+*) 1: the Encoder/Decoder has done nothing yet, so error checking makes no sense yet
+*) 10: while huffman decoding: end of input memory reached without endcode
+*) 11: while huffman decoding: error in code tree made it jump outside of tree
+*) 13: problem while processing dynamic deflate block
+*) 14: problem while processing dynamic deflate block
+*) 15: problem while processing dynamic deflate block
+*) 16: unexisting code while processing dynamic deflate block
+*) 17: while inflating: end of out buffer memory reached
+*) 18: while inflating: invalid distance code
+*) 19: while inflating: end of out buffer memory reached
+*) 20: invalid deflate block BTYPE encountered while decoding
+*) 21: NLEN is not ones complement of LEN in a deflate block
+*) 22: while inflating: end of out buffer memory reached.
+ This can happen if the inflated deflate data is longer than the amount of bytes required to fill up
+ all the pixels of the image, given the color depth and image dimensions. Something that doesn't
+ happen in a normal, well encoded, PNG image.
+*) 23: while inflating: end of in buffer memory reached
+*) 24: invalid FCHECK in zlib header
+*) 25: invalid compression method in zlib header
+*) 26: FDICT encountered in zlib header while it's not used for PNG
+*) 27: PNG file is smaller than a PNG header
+*) 28: incorrect PNG signature (the first 8 bytes of the PNG file)
+ Maybe it's not a PNG, or a PNG file that got corrupted so that the header indicates the corruption.
+*) 29: first chunk is not the header chunk
+*) 30: chunk length too large, chunk broken off at end of file
+*) 31: illegal PNG color type or bpp
+*) 32: illegal PNG compression method
+*) 33: illegal PNG filter method
+*) 34: illegal PNG interlace method
+*) 35: chunk length of a chunk is too large or the chunk too small
+*) 36: illegal PNG filter type encountered
+*) 37: illegal bit depth for this color type given
+*) 38: the palette is too big (more than 256 colors)
+*) 39: more palette alpha values given in tRNS, than there are colors in the palette
+*) 40: tRNS chunk has wrong size for greyscale image
+*) 41: tRNS chunk has wrong size for RGB image
+*) 42: tRNS chunk appeared while it was not allowed for this color type
+*) 43: bKGD chunk has wrong size for palette image
+*) 44: bKGD chunk has wrong size for greyscale image
+*) 45: bKGD chunk has wrong size for RGB image
+*) 46: value encountered in indexed image is larger than the palette size (bitdepth == 8). Is the palette too small?
+*) 47: value encountered in indexed image is larger than the palette size (bitdepth < 8). Is the palette too small?
+*) 48: the input data is empty. Maybe a PNG file you tried to load doesn't exist or is in the wrong path.
+*) 49: jumped past memory while generating dynamic huffman tree
+*) 50: jumped past memory while generating dynamic huffman tree
+*) 51: jumped past memory while inflating huffman block
+*) 52: jumped past memory while inflating
+*) 53: size of zlib data too small
+*) 55: jumped past tree while generating huffman tree, this could be when the
+ tree will have more leaves than symbols after generating it out of the
+ given lenghts. They call this an oversubscribed dynamic bit lengths tree in zlib.
+*) 56: given output image colorType or bitDepth not supported for color conversion
+*) 57: invalid CRC encountered (checking CRC can be disabled)
+*) 58: invalid ADLER32 encountered (checking ADLER32 can be disabled)
+*) 59: conversion to unexisting color mode or color mode conversion not supported.
+*) 60: invalid window size given in the settings of the encoder (must be 0-32768)
+*) 61: invalid BTYPE given in the settings of the encoder (only 0, 1 and 2 are allowed)
+*) 62: conversion from non-greyscale color to greyscale color requested by encoder or decoder. LodePNG
+ leaves the choice of RGB to greyscale conversion formula to the user.
+*) 63: length of a chunk too long, max allowed for PNG is 2147483647 bytes per chunk (2^31-1)
+*) 64: the length of the "end" symbol 256 in the Huffman tree is 0, resulting in the inability of a deflated
+ block to ever contain an end code. It must be at least 1.
+*) 66: the length of a text chunk keyword given to the encoder is longer than the maximum 79 bytes.
+*) 67: the length of a text chunk keyword given to the encoder is smaller than the minimum 1 byte.
+*) 68: tried to encode a PLTE chunk with a palette that has less than 1 or more than 256 colors
+*) 69: unknown chunk type with "critical" flag encountered by the decoder
+*) 71: unexisting interlace mode given to encoder (must be 0 or 1)
+*) 72: while decoding, unexisting compression method encountering in zTXt or iTXt chunk (it must be 0)
+*) 73: invalid tIME chunk size
+*) 74: invalid pHYs chunk size
+*) 75: no null termination char found while decoding any kind of text chunk, or wrong length
+*) 76: iTXt chunk too short to contain required bytes
+*) 77: integer overflow in buffer size happened somewhere
+*) 78: file doesn't exist or couldn't be opened for reading
+*) 79: file couldn't be opened for writing
+*) 80: tried creating a tree for 0 symbols
+*) 9900-9999: out of memory while allocating chunk of memory somewhere
+
+8. chunks and PNG editing
+-------------------------
+
+If you want to add extra chunks to a PNG you encode, or use LodePNG for a PNG
+editor that should follow the rules about handling of unknown chunks, or if you
+program is able to read other types of chunks than the ones handled by LodePNG,
+then that's possible with the chunk functions of LodePNG.
+
+A PNG chunk has the following layout:
+
+4 bytes length
+4 bytes type name
+length bytes data
+4 bytes CRC
+
+
+8.1. iterating through chunks
+-----------------------------
+
+If you have a buffer containing the PNG image data, then the first chunk (the
+IHDR chunk) starts at byte number 8 of that buffer. The first 8 bytes are the
+signature of the PNG and are not part of a chunk. But if you start at byte 8
+then you have a chunk, and can check the following things of it.
+
+NOTE: none of these functions check for memory buffer boundaries. To avoid
+exploits, always make sure the buffer contains all the data of the chunks.
+When using LodePNG_chunk_next, make sure the returned value is within the
+allocated memory.
+
+unsigned LodePNG_chunk_length(const unsigned char* chunk):
+
+Get the length of the chunk's data. The total chunk length is this length + 12.
+
+void LodePNG_chunk_type(char type[5], const unsigned char* chunk):
+unsigned char LodePNG_chunk_type_equals(const unsigned char* chunk, const char* type):
+
+Get the type of the chunk or compare if it's a certain type
+
+unsigned char LodePNG_chunk_critical(const unsigned char* chunk):
+unsigned char LodePNG_chunk_private(const unsigned char* chunk):
+unsigned char LodePNG_chunk_safetocopy(const unsigned char* chunk):
+
+Check if the chunk is critical in the PNG standard (only IHDR, PLTE, IDAT and IEND are).
+Check if the chunk is private (public chunks are part of the standard, private ones not).
+Check if the chunk is safe to copy. If it's not, then, when modifying data in a critical
+chunk, unsafe to copy chunks of the old image may NOT be saved in the new one if your
+program doesn't handle that type of unknown chunk.
+
+unsigned char* LodePNG_chunk_data(unsigned char* chunk):
+const unsigned char* LodePNG_chunk_data_const(const unsigned char* chunk):
+
+Get a pointer to the start of the data of the chunk.
+
+unsigned LodePNG_chunk_check_crc(const unsigned char* chunk):
+void LodePNG_chunk_generate_crc(unsigned char* chunk):
+
+Check if the crc is correct or generate a correct one.
+
+unsigned char* LodePNG_chunk_next(unsigned char* chunk):
+const unsigned char* LodePNG_chunk_next_const(const unsigned char* chunk):
+
+Iterate to the next chunk. This works if you have a buffer with consecutive chunks. Note that these
+functions do no boundary checking of the allocated data whatsoever, so make sure there is enough
+data available in the buffer to be able to go to the next chunk.
+
+unsigned LodePNG_append_chunk(unsigned char** out, size_t* outlength, const unsigned char* chunk):
+unsigned LodePNG_create_chunk(unsigned char** out, size_t* outlength, unsigned length, const char* type, const unsigned char* data):
+
+These functions are used to create new chunks that are appended to the data in *out that has
+length *outlength. The append function appends an existing chunk to the new data. The create
+function creates a new chunk with the given parameters and appends it. Type is the 4-letter
+name of the chunk.
+
+
+8.2. chunks in infoPng
+----------------------
+
+The LodePNG_InfoPng struct contains a struct LodePNG_UnknownChunks in it. This
+struct has 3 buffers (each with size) to contain 3 types of unknown chunks:
+the ones that come before the PLTE chunk, the ones that come between the PLTE
+and the IDAT chunks, and the ones that come after the IDAT chunks.
+It's necessary to make the distionction between these 3 cases because the PNG
+standard forces to keep the ordering of unknown chunks compared to the critical
+chunks, but does not force any other ordering rules.
+
+infoPng.unknown_chunks.data[0] is the chunks before PLTE
+infoPng.unknown_chunks.data[1] is the chunks after PLTE, before IDAT
+infoPng.unknown_chunks.data[2] is the chunks after IDAT
+
+The chunks in these 3 buffers can be iterated through and read by using the same
+way described in the previous subchapter.
+
+When using the decoder to decode a PNG, you can make it store all unknown chunks
+if you set the option settings.rememberUnknownChunks to 1. By default, this option
+is off and is 0.
+
+The encoder will always encode unknown chunks that are stored in the infoPng. If
+you need it to add a particular chunk that isn't known by LodePNG, you can use
+LodePNG_append_chunk or LodePNG_create_chunk to the chunk data in
+infoPng.unknown_chunks.data[x].
+
+Chunks that are known by LodePNG should not be added in that way. E.g. to make
+LodePNG add a bKGD chunk, set background_defined to true and add the correct
+parameters there and LodePNG will generate the chunk.
+
+
+9. compiler support
+-------------------
+
+No libraries other than the current standard C library are needed to compile
+LodePNG. For the C++ version, only the standard C++ library is needed on top.
+Add the files lodepng.c(pp) and lodepng.h to your project, include
+lodepng.h where needed, and your program can read/write PNG files.
+
+Use optimization! For both the encoder and decoder, compiling with the best
+optimizations makes a large difference.
+
+Make sure that LodePNG is compiled with the same compiler of the same version
+and with the same settings as the rest of the program, or the interfaces with
+std::vectors and std::strings in C++ can be incompatible resulting in bad things.
+
+CHAR_BITS must be 8 or higher, because LodePNG uses unsigned chars for octets.
+
+*) gcc and g++
+
+LodePNG is developed in gcc so this compiler is natively supported. It gives no
+warnings with compiler options "-Wall -Wextra -pedantic -ansi", with gcc and g++
+version 4.5.1 on Linux.
+
+*) Mingw and Bloodshed DevC++
+
+The Mingw compiler (a port of gcc) used by Bloodshed DevC++ for Windows is fully
+supported by LodePNG.
+
+*) Visual Studio 2005 and Visual C++ 2005 Express Edition
+
+Versions 20070604 up to 20080107 have been tested on VS2005 and work. Visual
+studio may give warnings about 'fopen' being deprecated. A multiplatform library
+can't support the proposed Visual Studio alternative however.
+
+If you're using LodePNG in VS2005 and don't want to see the deprecated warnings,
+put this on top of lodepng.h before the inclusions: #define _CRT_SECURE_NO_DEPRECATE
+
+*) Visual Studio 6.0
+
+The C++ version of LodePNG was not supported by Visual Studio 6.0 because Visual
+Studio 6.0 doesn't follow the C++ standard and implements it incorrectly.
+The current C version of LodePNG has not been tested in VS6 but may work now.
+
+*) Comeau C/C++
+
+Vesion 20070107 compiles without problems on the Comeau C/C++ Online Test Drive
+at http://www.comeaucomputing.com/tryitout in both C90 and C++ mode.
+
+*) Compilers on Macintosh
+
+LodePNG has been reported to work both with the gcc and LLVM for Macintosh, both
+for C and C++.
+
+*) Other Compilers
+
+If you encounter problems on other compilers, I'm happy to help out make LodePNG
+support the compiler if it supports the ISO C90 and C++ standard well enough and
+the required modification doesn't require using non standard or less good C/C++
+code or headers.
+
+
+10. examples
+------------
+
+This decoder and encoder example show the most basic usage of LodePNG (using the
+classes, not the simple functions, which would be trivial)
+
+More complex examples can be found in:
+-lodepng_examples.c: 9 different examples in C, such as showing the image with SDL, ...
+-lodepng_examples.cpp: the same examples in C++ using the C++ wrapper of LodePNG
+
+These files can be found on the LodePNG website or searched for on the internet.
+
+10.1. decoder C++ example
+-------------------------
+
+////////////////////////////////////////////////////////////////////////////////
+#include "lodepng.h"
+#include <iostream>
+
+int main(int argc, char *argv[])
+{
+ const char* filename = argc > 1 ? argv[1] : "test.png";
+
+ //load and decode
+ std::vector<unsigned char> buffer, image; //buffer will contain the PNG file, image will contain the raw pixels
+ LodePNG::loadFile(buffer, filename); //load the image file with given filename
+ LodePNG::Decoder decoder;
+ decoder.decode(image, buffer.size() ? &buffer[0] : 0, (unsigned)buffer.size()); //decode the png
+
+ //if there's an error, display it
+ if(decoder.hasError()) std::cout << "error: " << decoder.getError() << std::endl;
+
+ int width = decoder.getWidth(); //get the width in pixels
+ int height = decoder.getHeight(); //get the height in pixels
+ //the pixels are now in the vector "image", 4 bytes per pixel, ordered RGBARGBA..., use it as texture, draw it, ...
+}
+
+//alternative version using the "simple" function
+int main(int argc, char *argv[])
+{
+ const char* filename = argc > 1 ? argv[1] : "test.png";
+
+ //load and decode
+ std::vector<unsigned char> image;
+ unsigned width, height;
+ unsigned error = LodePNG::decode(image, width, height, filename);
+
+ //if there's an error, display it
+ if(error != 0) std::cout << "error: " << error << std::endl;
+
+ //the pixels are now in the vector "image", 4 bytes per pixel, ordered RGBARGBA..., use it as texture, draw it, ...
+}
+////////////////////////////////////////////////////////////////////////////////
+
+
+10.2. encoder C++ example
+-------------------------
+
+////////////////////////////////////////////////////////////////////////////////
+#include "lodepng.h"
+#include <iostream>
+
+//saves image to filename given as argument. Warning, this overwrites the file without warning!
+int main(int argc, char *argv[])
+{
+ //check if user gave a filename
+ if(argc <= 1)
+ {
+ std::cout << "please provide a filename to save to\n";
+ return 0;
+ }
+
+ //generate some image
+ std::vector<unsigned char> image;
+ image.resize(512 * 512 * 4);
+ for(unsigned y = 0; y < 512; y++)
+ for(unsigned x = 0; x < 512; x++)
+ {
+ image[4 * 512 * y + 4 * x + 0] = 255 * !(x & y);
+ image[4 * 512 * y + 4 * x + 1] = x ^ y;
+ image[4 * 512 * y + 4 * x + 2] = x | y;
+ image[4 * 512 * y + 4 * x + 3] = 255;
+ }
+
+ //encode and save, using the Encoder class
+ std::vector<unsigned char> buffer;
+ LodePNG::Encoder encoder;
+ encoder.encode(buffer, image, 512, 512);
+ LodePNG::saveFile(buffer, argv[1]);
+
+ //the same as the 4 lines of code above, but in 1 call without the class:
+ //LodePNG::encode(argv[1], image, 512, 512);
+}
+////////////////////////////////////////////////////////////////////////////////
+
+
+10.3. Decoder C example
+-----------------------
+
+This example loads the PNG from a file into a pixel buffer in 1 function call
+
+#include "lodepng.h"
+
+int main(int argc, char *argv[])
+{
+ unsigned error;
+ unsigned char* image;
+ size_t width, height;
+
+ if(argc <= 1) return 0;
+
+ error = LodePNG_decode32_file(&image, &width, &height, filename);
+
+ //use image here
+
+ free(image);
+}
+
+
+11. changes
+-----------
+
+The version number of LodePNG is the date of the change given in the format
+yyyymmdd.
+
+Some changes aren't backwards compatible. Those are indicated with a (!)
+symbol.
+
+*) 30 okt 2010: made decoding slightly faster
+*) 26 okt 2010: (!) changed some C function and struct names (more consistent).
+ Reorganized the documentation and the declaration order in the header.
+*) 08 aug 2010: only changed some comments and external samples.
+*) 05 jul 2010: fixed bug thanks to warnings in the new gcc version.
+*) 14 mar 2010: fixed bug where too much memory was allocated for char buffers.
+*) 02 sep 2008: fixed bug where it could create empty tree that linux apps could
+ read by ignoring the problem but windows apps couldn't.
+*) 06 jun 2008: added more error checks for out of memory cases.
+*) 26 apr 2008: added a few more checks here and there to ensure more safety.
+*) 06 mar 2008: crash with encoding of strings fixed
+*) 02 feb 2008: support for international text chunks added (iTXt)
+*) 23 jan 2008: small cleanups, and #defines to divide code in sections
+*) 20 jan 2008: support for unknown chunks allowing using LodePNG for an editor.
+*) 18 jan 2008: support for tIME and pHYs chunks added to encoder and decoder.
+*) 17 jan 2008: ability to encode and decode compressed zTXt chunks added
+ Also vareous fixes, such as in the deflate and the padding bits code.
+*) 13 jan 2008: Added ability to encode Adam7-interlaced images. Improved
+ filtering code of encoder.
+*) 07 jan 2008: (!) changed LodePNG to use ISO C90 instead of C++. A
+ C++ wrapper around this provides an interface almost identical to before.
+ Having LodePNG be pure ISO C90 makes it more portable. The C and C++ code
+ are together in these files but it works both for C and C++ compilers.
+*) 29 dec 2007: (!) changed most integer types to unsigned int + other tweaks
+*) 30 aug 2007: bug fixed which makes this Borland C++ compatible
+*) 09 aug 2007: some VS2005 warnings removed again
+*) 21 jul 2007: deflate code placed in new namespace separate from zlib code
+*) 08 jun 2007: fixed bug with 2- and 4-bit color, and small interlaced images
+*) 04 jun 2007: improved support for Visual Studio 2005: crash with accessing
+ invalid std::vector element [0] fixed, and level 3 and 4 warnings removed
+*) 02 jun 2007: made the encoder add a tag with version by default
+*) 27 may 2007: zlib and png code separated (but still in the same file),
+ simple encoder/decoder functions added for more simple usage cases
+*) 19 may 2007: minor fixes, some code cleaning, new error added (error 69),
+ moved some examples from here to lodepng_examples.cpp
+*) 12 may 2007: palette decoding bug fixed
+*) 24 apr 2007: changed the license from BSD to the zlib license
+*) 11 mar 2007: very simple addition: ability to encode bKGD chunks.
+*) 04 mar 2007: (!) tEXt chunk related fixes, and support for encoding
+ palettized PNG images. Plus little interface change with palette and texts.
+*) 03 mar 2007: Made it encode dynamic Huffman shorter with repeat codes.
+ Fixed a bug where the end code of a block had length 0 in the Huffman tree.
+*) 26 feb 2007: Huffman compression with dynamic trees (BTYPE 2) now implemented
+ and supported by the encoder, resulting in smaller PNGs at the output.
+*) 27 jan 2007: Made the Adler-32 test faster so that a timewaste is gone.
+*) 24 jan 2007: gave encoder an error interface. Added color conversion from any
+ greyscale type to 8-bit greyscale with or without alpha.
+*) 21 jan 2007: (!) Totally changed the interface. It allows more color types
+ to convert to and is more uniform. See the manual for how it works now.
+*) 07 jan 2007: Some cleanup & fixes, and a few changes over the last days:
+ encode/decode custom tEXt chunks, separate classes for zlib & deflate, and
+ at last made the decoder give errors for incorrect Adler32 or Crc.
+*) 01 jan 2007: Fixed bug with encoding PNGs with less than 8 bits per channel.
+*) 29 dec 2006: Added support for encoding images without alpha channel, and
+ cleaned out code as well as making certain parts faster.
+*) 28 dec 2006: Added "Settings" to the encoder.
+*) 26 dec 2006: The encoder now does LZ77 encoding and produces much smaller files now.
+ Removed some code duplication in the decoder. Fixed little bug in an example.
+*) 09 dec 2006: (!) Placed output parameters of public functions as first parameter.
+ Fixed a bug of the decoder with 16-bit per color.
+*) 15 okt 2006: Changed documentation structure
+*) 09 okt 2006: Encoder class added. It encodes a valid PNG image from the
+ given image buffer, however for now it's not compressed.
+*) 08 sep 2006: (!) Changed to interface with a Decoder class
+*) 30 jul 2006: (!) LodePNG_InfoPng , width and height are now retrieved in different
+ way. Renamed decodePNG to decodePNGGeneric.
+*) 29 jul 2006: (!) Changed the interface: image info is now returned as a
+ struct of type LodePNG::LodePNG_Info, instead of a vector, which was a bit clumsy.
+*) 28 jul 2006: Cleaned the code and added new error checks.
+ Corrected terminology "deflate" into "inflate".
+*) 23 jun 2006: Added SDL example in the documentation in the header, this
+ example allows easy debugging by displaying the PNG and its transparency.
+*) 22 jun 2006: (!) Changed way to obtain error value. Added
+ loadFile function for convenience. Made decodePNG32 faster.
+*) 21 jun 2006: (!) Changed type of info vector to unsigned.
+ Changed position of palette in info vector. Fixed an important bug that
+ happened on PNGs with an uncompressed block.
+*) 16 jun 2006: Internally changed unsigned into unsigned where
+ needed, and performed some optimizations.
+*) 07 jun 2006: (!) Renamed functions to decodePNG and placed them
+ in LodePNG namespace. Changed the order of the parameters. Rewrote the
+ documentation in the header. Renamed files to lodepng.cpp and lodepng.h
+*) 22 apr 2006: Optimized and improved some code
+*) 07 sep 2005: (!) Changed to std::vector interface
+*) 12 aug 2005: Initial release
+
+
+12. contact information
+-----------------------
+
+Feel free to contact me with suggestions, problems, comments, ... concerning
+LodePNG. If you encounter a PNG image that doesn't work properly with this
+decoder, feel free to send it and I'll use it to find and fix the problem.
+
+My email address is (puzzle the account and domain together with an @ symbol):
+Domain: gmail dot com.
+Account: lode dot vandevenne.
+
+
+Copyright (c) 2005-2010 Lode Vandevenne
+*/
diff --git a/Simbody/Visualizer/simbody-visualizer/simbody-visualizer.cpp b/Simbody/Visualizer/simbody-visualizer/simbody-visualizer.cpp
new file mode 100644
index 0000000..ce72b79
--- /dev/null
+++ b/Simbody/Visualizer/simbody-visualizer/simbody-visualizer.cpp
@@ -0,0 +1,2881 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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/Visualizer.h"
+#include "simbody/internal/Visualizer_InputListener.h"
+#include "../src/VisualizerProtocol.h"
+#include "lodepng.h"
+
+#include <cstdlib>
+#include <string>
+#include <algorithm>
+#include <set>
+#include <vector>
+#include <utility>
+#include <limits>
+#include <cstdio>
+#include <cerrno>
+#include <cstring>
+#include <pthread.h>
+#include <sys/stat.h>
+#ifdef _WIN32
+ #include <direct.h>
+#endif
+
+// Get gl and glut using the appropriate platform-dependent incantations.
+#if defined(__APPLE__)
+ // OSX comes with a good glut implementation.
+ #include <GLUT/glut.h>
+#elif defined(_WIN32)
+ #include "glut32/glut.h" // we have our own private headers
+ #include "glut32/glext.h"
+
+ // A Windows-only extension for disabling vsync, allowing unreasonably
+ // high frame rates.
+ PFNWGLSWAPINTERVALFARPROC wglSwapIntervalEXT;
+
+ // These will hold the dynamically-determined function addresses.
+
+ // These functions are needed for basic Visualizer functionality.
+ PFNGLGENBUFFERSPROC glGenBuffers;
+ PFNGLBINDBUFFERPROC glBindBuffer;
+ PFNGLBUFFERDATAPROC glBufferData;
+ PFNGLACTIVETEXTUREPROC glActiveTexture;
+
+ // These are needed only for saving images and movies.
+ // Use old EXT names for these so we only require OpenGL 2.0.
+ PFNGLGENFRAMEBUFFERSEXTPROC glGenFramebuffersEXT;
+ PFNGLGENRENDERBUFFERSEXTPROC glGenRenderbuffersEXT;
+ PFNGLBINDFRAMEBUFFEREXTPROC glBindFramebufferEXT;
+ PFNGLBINDRENDERBUFFEREXTPROC glBindRenderbufferEXT;
+ PFNGLRENDERBUFFERSTORAGEEXTPROC glRenderbufferStorageEXT;
+ PFNGLFRAMEBUFFERRENDERBUFFEREXTPROC glFramebufferRenderbufferEXT;
+ PFNGLDELETERENDERBUFFERSEXTPROC glDeleteRenderbuffersEXT;
+ PFNGLDELETEFRAMEBUFFERSEXTPROC glDeleteFramebuffersEXT;
+
+ // see initGlextFuncPointerIfNeeded() at end of this file
+#else
+ // Linux: assume we have a good OpenGL 2.0 and working glut or freeglut.
+ #define GL_GLEXT_PROTOTYPES
+ #include <GL/gl.h>
+ #include <GL/glu.h>
+ #include <GL/glext.h>
+ #include <GL/glut.h>
+#endif
+
+// Returns true if we were able to find sufficent OpenGL functionality to
+// operate. We'll still limp along if we can't get enough to save images.
+static bool initGlextFuncPointersIfNeeded(bool& canSaveImages);
+static void redrawDisplay();
+static void setKeepAlive(bool enable);
+static void setVsync(bool enable);
+static void shutdown();
+
+// Next, get the functions necessary for reading from and writing to pipes.
+#ifdef _WIN32
+ #include <io.h>
+ #define READ _read
+#else
+ #include <unistd.h>
+ #define READ read
+#endif
+
+// gcc 4.4.3 complains bitterly if you don't check the return
+// status from the write() system call. This avoids those
+// warnings and maybe, someday, will catch an error.
+#define WRITE(pipeno, buf, len) \
+ {int status=write((pipeno), (buf), (len)); \
+ SimTK_ERRCHK4_ALWAYS(status!=-1, "simbody-visualizer", \
+ "An attempt to write() %d bytes to pipe %d failed with errno=%d (%s).", \
+ (len),(pipeno),errno,strerror(errno));}
+
+using namespace SimTK;
+using namespace std;
+
+// This is the transform giving the pose of the camera's local frame in the
+// model's ground frame. The camera local frame has Y as the up direction,
+// -Z as the "look at" direction, and X to the right. We can't know a good
+// default transform for the camera until we know what the SimTK::System
+// we're viewing considers to be its "up" and "look at" directions.
+static fTransform X_GC;
+static int inPipe, outPipe;
+
+static void computeBoundingSphereForVertices(const vector<float>& vertices, float& radius, fVec3& center) {
+ fVec3 lower(vertices[0], vertices[1], vertices[2]);
+ fVec3 upper = lower;
+ for (int i = 3; i < (int) vertices.size(); i += 3) {
+ for (int j = 0; j < 3; j++) {
+ lower[j] = min(lower[j], vertices[i+j]);
+ upper[j] = max(upper[j], vertices[i+j]);
+ }
+ }
+ center = (lower+upper)/2;
+ float rad2 = 0;
+ for (int i = 0; i < (int) vertices.size(); i += 3) {
+ float x = center[0]-vertices[i];
+ float y = center[1]-vertices[i+1];
+ float z = center[2]-vertices[i+2];
+ float norm2 = x*x+y*y+z*z;
+ if (norm2 > rad2)
+ rad2 = norm2;
+ }
+ radius = sqrt(rad2);
+}
+
+class Mesh {
+public:
+ Mesh(vector<float>& vertices, vector<float>& normals, vector<GLushort>& faces)
+ : numVertices((int)(vertices.size()/3)), faces(faces) {
+ // Build OpenGL buffers.
+
+ GLuint buffers[2];
+ glGenBuffers(2, buffers);
+ vertBuffer = buffers[0];
+ normBuffer = buffers[1];
+ glBindBuffer(GL_ARRAY_BUFFER, vertBuffer);
+ glBufferData(GL_ARRAY_BUFFER, vertices.size()*sizeof(float), &vertices[0], GL_STATIC_DRAW);
+ glBindBuffer(GL_ARRAY_BUFFER, normBuffer);
+ glBufferData(GL_ARRAY_BUFFER, normals.size()*sizeof(float), &normals[0], GL_STATIC_DRAW);
+
+ // Create the list of edges.
+
+ set<pair<GLushort, GLushort> > edgeSet;
+ for (int i = 0; i < (int) faces.size(); i += 3) {
+ GLushort v1 = faces[i];
+ GLushort v2 = faces[i+1];
+ GLushort v3 = faces[i+2];
+ edgeSet.insert(make_pair(min(v1, v2), max(v1, v2)));
+ edgeSet.insert(make_pair(min(v2, v3), max(v2, v3)));
+ edgeSet.insert(make_pair(min(v3, v1), max(v3, v1)));
+ }
+ for (set<pair<GLushort, GLushort> >::const_iterator iter = edgeSet.begin(); iter != edgeSet.end(); ++iter) {
+ edges.push_back(iter->first);
+ edges.push_back(iter->second);
+ }
+
+ // Compute the center and radius.
+
+ computeBoundingSphereForVertices(vertices, radius, center);
+ }
+ void draw(short representation) const {
+ glBindBuffer(GL_ARRAY_BUFFER, vertBuffer);
+ glVertexPointer(3, GL_FLOAT, 0, 0);
+ glBindBuffer(GL_ARRAY_BUFFER, normBuffer);
+ glNormalPointer(GL_FLOAT, 0, 0);
+ if (representation == DecorativeGeometry::DrawSurface)
+ glDrawElements(GL_TRIANGLES, (GLsizei)faces.size(), GL_UNSIGNED_SHORT, &faces[0]);
+ else if (representation == DecorativeGeometry::DrawPoints)
+ glDrawArrays(GL_POINTS, 0, numVertices);
+ else if (representation == DecorativeGeometry::DrawWireframe)
+ glDrawElements(GL_LINES, (GLsizei)edges.size(), GL_UNSIGNED_SHORT, &edges[0]);
+ }
+ void getBoundingSphere(float& radius, fVec3& center) {
+ radius = this->radius;
+ center = this->center;
+ }
+private:
+ int numVertices;
+ GLuint vertBuffer, normBuffer;
+ vector<GLushort> edges, faces;
+ fVec3 center;
+ float radius;
+};
+
+static vector<vector<Mesh*> > meshes;
+
+class RenderedMesh {
+public:
+ RenderedMesh(const fTransform& transform, const fVec3& scale, const fVec4& color, short representation, unsigned short meshIndex, unsigned short resolution) :
+ transform(transform), scale(scale), representation(representation), meshIndex(meshIndex), resolution(resolution) {
+ this->color[0] = color[0];
+ this->color[1] = color[1];
+ this->color[2] = color[2];
+ this->color[3] = color[3];
+ }
+ void draw(bool setColor = true) {
+ glPushMatrix();
+ glTranslated(transform.p()[0], transform.p()[1], transform.p()[2]);
+ fVec4 rot = transform.R().convertRotationToAngleAxis();
+ glRotated(rot[0]*SimTK_RADIAN_TO_DEGREE, rot[1], rot[2], rot[3]);
+ glScaled(scale[0], scale[1], scale[2]);
+ if (setColor) {
+ if (representation == DecorativeGeometry::DrawSurface)
+ glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color);
+ else
+ glColor3fv(color);
+ }
+ meshes[meshIndex][resolution]->draw(representation);
+ glPopMatrix();
+ }
+ const fTransform& getTransform() const {
+ return transform;
+ }
+ void computeBoundingSphere(float& radius, fVec3& center) const {
+ meshes[meshIndex][resolution]->getBoundingSphere(radius, center);
+ center += transform.p();
+ radius *= max(abs(scale[0]), max(abs(scale[1]), abs(scale[2])));
+ }
+private:
+ fTransform transform;
+ fVec3 scale;
+ GLfloat color[4];
+ short representation;
+ unsigned short meshIndex, resolution;
+};
+
+class RenderedLine {
+public:
+ RenderedLine(const fVec3& color, float thickness)
+ : color(color), thickness(thickness) {}
+
+ void draw(bool setColor = true) {
+ if (setColor)
+ glColor3d(color[0], color[1], color[2]);
+ glLineWidth(thickness);
+ glBindBuffer(GL_ARRAY_BUFFER, 0);
+ glVertexPointer(3, GL_FLOAT, 0, &lines[0]);
+ glDrawArrays(GL_LINES, 0, (GLsizei)(lines.size()/3));
+ }
+ vector<GLfloat>& getLines() {
+ return lines;
+ }
+ const fVec3& getColor() const {
+ return color;
+ }
+ float getThickness() const {
+ return thickness;
+ }
+ void computeBoundingSphere(float& radius, fVec3& center) const {
+ computeBoundingSphereForVertices(lines, radius, center);
+ }
+private:
+ fVec3 color;
+ float thickness;
+ vector<GLfloat> lines;
+};
+
+class RenderedText {
+public:
+ RenderedText(const fVec3& position, const fVec3& scale, const fVec3& color,
+ const string& text, bool faceCamera = true)
+ : position(position), scale(scale/119), text(text),
+ faceCamera(faceCamera) {
+ this->color[0] = color[0];
+ this->color[1] = color[1];
+ this->color[2] = color[2];
+ }
+ void draw() {
+ glPushMatrix();
+ glTranslated(position[0], position[1], position[2]);
+ fVec4 rot = X_GC.R().convertRotationToAngleAxis();
+ if (faceCamera)
+ glRotated(rot[0]*SimTK_RADIAN_TO_DEGREE, rot[1], rot[2], rot[3]);
+ glScaled(scale[0], scale[1], scale[2]);
+ glColor3fv(color);
+ for (int i = 0; i < (int) text.size(); i++)
+ glutStrokeCharacter(GLUT_STROKE_ROMAN, text[i]);
+ glPopMatrix();
+ }
+ void computeBoundingSphere(float& radius, fVec3& center) const {
+ center = position;
+ radius = glutStrokeLength(GLUT_STROKE_ROMAN,
+ (unsigned char*)text.c_str())*scale[0];
+ }
+private:
+ fVec3 position;
+ fVec3 scale;
+ GLfloat color[3];
+ string text;
+ bool faceCamera;
+};
+
+class ScreenText {
+public:
+ ScreenText(const string& txt)
+ : text(txt) {}
+
+ const string& getString() const {return text;}
+
+private:
+ string text;
+};
+
+
+
+/*==============================================================================
+ SCENE DATA
+================================================================================
+We keep two scenes at a time -- a "front" scene that is currently being
+rendered, and a "back" scene is being filled in by the listener thread in
+parallel, while the front scene is being rendered. Once the back scene is
+complete, the listener thread must block until the renderer has drawn the
+front scene (at least once). After the front scene is rendered, the listener
+thread replaces it with the back scene. When there is no back scene, the front
+scene will be rendered repeatedly whenever an event occurs that may require
+that, such as a user moving the camera.
+
+There is a mutex lock for the front scene. It is held by the renderer when
+drawing, by any operations (such as camera motion) that would require redrawing
+the front scene, and by the listener thread when it wants to swap in the back
+scene. There is a condition variable that the listener can wait on when it is
+done with the back scene but front scene rendering has not yet finished; that
+is signaled by the renderer when it finishes drawing the front scene. */
+
+// This object holds a scene. There are at most two of these around.
+class Scene {
+public:
+ Scene() : simTime(0), sceneHasBeenDrawn(false) {}
+
+ float simTime; // simulated time associated with this frame
+
+ vector<RenderedMesh> drawnMeshes;
+ vector<RenderedMesh> solidMeshes;
+ vector<RenderedMesh> transparentMeshes;
+ vector<RenderedLine> lines;
+ vector<RenderedText> sceneText;
+ vector<ScreenText> screenText;
+
+ bool sceneHasBeenDrawn;
+};
+
+static Scene* scene=0; // This is the front scene.
+
+// This lock must be held whenever the front scene is being
+// modified or drawn, or when it is being swapped with the back scene.
+static pthread_mutex_t sceneLock;
+
+// Wait on this if you want to be notified when the scene has been drawn.
+static pthread_cond_t sceneHasBeenDrawn;
+
+// This is how long it's been since we've done a redisplay for any reason.
+static double lastRedisplayDone = 0; // real time
+
+// When this is true it means something has happened that requires redisplay,
+// but we hope to see it when the next scene is rendered rather than initiate
+// rendering now. The idle function checks this and initiates rendering if
+// no one else does.
+static bool passiveRedisplayRequested = true;
+// This is reset to zero every time we do an active display.
+static int numPassiveRedisplaysSinceLastActive = 0;
+// This is reset to zero whenever we post a passive or active display.
+static int numMopUpDisplaysSinceLastRedisplay = 0;
+
+// This is the rate that the Visualizer class (on the simulation side) is
+// trying to achieve. Normally the GUI simply renders frames whenever they
+// are delivered; it does not attempt to time them locally. However, when
+// no frames are received, the GUI may issue its own redisplays and should
+// not do so any faster than this.
+static float maxFrameRate = 30; // in frames/sec
+
+// This is set during the initial handshake with the simulator process.
+// It is the file name (not full path) of the simulator executable,
+// suitable for use in the title or an "about" message.
+static string simulatorExecutableName;
+
+// This is the Simbody version number from the simulator, as major/minor/patch,
+// as received in the initial handshake.
+static int simbodyVersion[3];
+// Constructed from version numbers -- if patch is 0 will just be major.minor.
+static string simbodyVersionStr;
+
+// These are used when saving a movie.
+static bool savingMovie = false, saveNextFrameToMovie = false;
+static bool canSaveImages = false; // is this OpenGL version up to the job?
+static string movieDir;
+static int movieFrame;
+static ParallelWorkQueue imageSaverQueue(5);
+static void writeImage(const string& filename);
+
+static void forceActiveRedisplay() {
+ passiveRedisplayRequested = false; // cancel if pending
+ numPassiveRedisplaysSinceLastActive = 0;
+ numMopUpDisplaysSinceLastRedisplay = 0;
+ glutPostRedisplay();
+}
+
+static void requestPassiveRedisplay() {
+ passiveRedisplayRequested = true;
+}
+
+static void forcePassiveRedisplay() {
+ passiveRedisplayRequested = false; // cancel if pending
+ ++numPassiveRedisplaysSinceLastActive;
+ numMopUpDisplaysSinceLastRedisplay = 0;
+ glutPostRedisplay();
+}
+
+static void forceMopUpRedisplay() {
+ passiveRedisplayRequested = false; // cancel if pending
+ ++numMopUpDisplaysSinceLastRedisplay;
+ glutPostRedisplay();
+}
+
+// Some commands received by the listener must be executed on the rendering
+// thread. They are saved in concrete objects derived from this abstract
+// class.
+class PendingCommand {
+public:
+ virtual ~PendingCommand() {}
+ virtual void execute() = 0;
+};
+
+static int viewWidth, viewHeight;
+static GLfloat fieldOfView = GLfloat(SimTK_PI/4);
+static GLfloat nearClip = 1;
+static GLfloat farClip = 1000;
+static GLfloat groundHeight = 0;
+static CoordinateDirection groundNormal = YAxis; // the +Y direction
+static bool showGround=true, showShadows=true;
+static bool showFPS=false, showSimTime=false, showFrameNum=false;
+static fVec3 backgroundColor = fVec3(1,1,1); // white is the default
+static vector<PendingCommand*> pendingCommands;
+static float fps = 0.0f;
+static float lastSceneSimTime = 0.0f;
+static int fpsCounter = 0, nextMeshIndex;
+static int frameCounter = 0;
+static double fpsBaseTime = 0;
+
+static vector<string> overlayMessageLines;
+static bool displayOverlayMessage = false;
+static const double OverlayDisplayTimeInSec = 5; // Leave up for 5 s.
+static double overlayStartTime = NaN; // set when overlay is requested
+static double overlayEndTime = NaN; // set when overlay is requested
+
+static void setClearColorToBackgroundColor() {
+ glClearColor(backgroundColor[0],backgroundColor[1],backgroundColor[2],1);
+}
+
+class PendingMesh : public PendingCommand {
+public:
+ PendingMesh() {
+ index = nextMeshIndex++;
+ }
+ void execute() {
+ if ((int) meshes.size() <= index)
+ meshes.resize(index+1);
+ meshes[index].push_back(new Mesh(vertices, normals, faces));
+ }
+ vector<float> vertices;
+ vector<float> normals;
+ vector<GLushort> faces;
+ int index;
+};
+
+
+static void addVec(vector<float>& data, float x, float y, float z) {
+ data.push_back(x);
+ data.push_back(y);
+ data.push_back(z);
+}
+
+static void addVec(vector<unsigned short>& data, int x, int y, int z) {
+ data.push_back((unsigned short) x);
+ data.push_back((unsigned short) y);
+ data.push_back((unsigned short) z);
+}
+
+static Mesh* makeBox() {
+ const float halfx = 1;
+ const float halfy = 1;
+ const float halfz = 1;
+ vector<GLfloat> vertices;
+ vector<GLfloat> normals;
+ vector<GLushort> faces;
+
+ // lower x face
+ addVec(vertices, -halfx, -halfy, -halfz);
+ addVec(vertices, -halfx, -halfy, halfz);
+ addVec(vertices, -halfx, halfy, halfz);
+ addVec(vertices, -halfx, halfy, -halfz);
+ addVec(normals, -1, 0, 0);
+ addVec(normals, -1, 0, 0);
+ addVec(normals, -1, 0, 0);
+ addVec(normals, -1, 0, 0);
+ addVec(faces, 0, 1, 2);
+ addVec(faces, 2, 3, 0);
+ // upper x face
+ addVec(vertices, halfx, halfy, halfz);
+ addVec(vertices, halfx, -halfy, halfz);
+ addVec(vertices, halfx, -halfy, -halfz);
+ addVec(vertices, halfx, halfy, -halfz);
+ addVec(normals, 1, 0, 0);
+ addVec(normals, 1, 0, 0);
+ addVec(normals, 1, 0, 0);
+ addVec(normals, 1, 0, 0);
+ addVec(faces, 4, 5, 6);
+ addVec(faces, 6, 7, 4);
+ // lower y face
+ addVec(vertices, -halfx, -halfy, -halfz);
+ addVec(vertices, halfx, -halfy, -halfz);
+ addVec(vertices, halfx, -halfy, halfz);
+ addVec(vertices, -halfx, -halfy, halfz);
+ addVec(normals, 0, -1, 0);
+ addVec(normals, 0, -1, 0);
+ addVec(normals, 0, -1, 0);
+ addVec(normals, 0, -1, 0);
+ addVec(faces, 8, 9, 10);
+ addVec(faces, 10, 11, 8);
+ // upper y face
+ addVec(vertices, halfx, halfy, halfz);
+ addVec(vertices, halfx, halfy, -halfz);
+ addVec(vertices, -halfx, halfy, -halfz);
+ addVec(vertices, -halfx, halfy, halfz);
+ addVec(normals, 0, 1, 0);
+ addVec(normals, 0, 1, 0);
+ addVec(normals, 0, 1, 0);
+ addVec(normals, 0, 1, 0);
+ addVec(faces, 12, 13, 14);
+ addVec(faces, 14, 15, 12);
+ // lower z face
+ addVec(vertices, -halfx, -halfy, -halfz);
+ addVec(vertices, -halfx, halfy, -halfz);
+ addVec(vertices, halfx, halfy, -halfz);
+ addVec(vertices, halfx, -halfy, -halfz);
+ addVec(normals, 0, 0, -1);
+ addVec(normals, 0, 0, -1);
+ addVec(normals, 0, 0, -1);
+ addVec(normals, 0, 0, -1);
+ addVec(faces, 16, 17, 18);
+ addVec(faces, 18, 19, 16);
+ // upper z face
+ addVec(vertices, halfx, halfy, halfz);
+ addVec(vertices, -halfx, halfy, halfz);
+ addVec(vertices, -halfx, -halfy, halfz);
+ addVec(vertices, halfx, -halfy, halfz);
+ addVec(normals, 0, 0, 1);
+ addVec(normals, 0, 0, 1);
+ addVec(normals, 0, 0, 1);
+ addVec(normals, 0, 0, 1);
+ addVec(faces, 20, 21, 22);
+ addVec(faces, 22, 23, 20);
+ return new Mesh(vertices, normals, faces);
+}
+
+static Mesh* makeSphere(unsigned short resolution) {
+ const int numLatitude = 4*resolution;
+ const int numLongitude = 6*resolution;
+ const float radius = 1.0f;
+ vector<GLfloat> vertices;
+ vector<GLfloat> normals;
+ vector<GLushort> faces;
+ addVec(vertices, 0, radius, 0);
+ addVec(normals, 0, 1, 0);
+ for (int i = 0; i < numLatitude; i++) {
+ float phi = (float) (((i+1)*SimTK_PI)/(numLatitude+1));
+ float sphi = sin(phi);
+ float cphi = cos(phi);
+ float y = radius*cphi;
+ float r = radius*sphi;
+ for (int j = 0; j < numLongitude; j++) {
+ float theta = (float) ((j*2*SimTK_PI)/numLongitude);
+ float stheta = sin(theta);
+ float ctheta = cos(theta);
+ addVec(vertices, r*ctheta, y, r*stheta);
+ addVec(normals, sphi*ctheta, cphi, sphi*stheta);
+ }
+ }
+ addVec(vertices, 0, -radius, 0);
+ addVec(normals, 0, -1, 0);
+ for (int i = 1; i < numLongitude; i++)
+ addVec(faces, 0, i+1, i);
+ addVec(faces, 0, 1, numLongitude);
+ for (int i = 1; i < numLatitude; i++) {
+ int base = (i-1)*numLongitude+1;
+ for (int j = 0; j < numLongitude; j++) {
+ int v1 = base+j;
+ int v2 = (j == numLongitude-1 ? base : v1+1);
+ int v3 = v1+numLongitude;
+ int v4 = v2+numLongitude;
+ addVec(faces, v1, v4, v3);
+ addVec(faces, v1, v2, v4);
+ }
+ }
+ int first = (numLatitude-1)*numLongitude+1;
+ int last = numLatitude*numLongitude+1;
+ for (int i = first; i < last-1; i++)
+ addVec(faces, i, i+1, last);
+ addVec(faces, last-1, first, last);
+ return new Mesh(vertices, normals, faces);
+}
+
+static Mesh* makeCylinder(unsigned short resolution) {
+ const int numSides = 6*resolution;
+ const float halfHeight = 1;
+ const float radius = 1;
+ vector<GLfloat> vertices;
+ vector<GLfloat> normals;
+ vector<GLushort> faces;
+
+ // Create the top face.
+
+ addVec(vertices, 0, halfHeight, 0);
+ addVec(normals, 0, 1.0, 0);
+ for (int i = 0; i < numSides; i++) {
+ float theta = (float) ((i*2*SimTK_PI)/numSides);
+ float stheta = sin(theta);
+ float ctheta = cos(theta);
+ addVec(vertices, radius*ctheta, halfHeight, radius*stheta);
+ addVec(normals, 0, 1.0, 0);
+ }
+ for (int i = 1; i < numSides; i++)
+ addVec(faces, i, 0, i+1);
+ addVec(faces, numSides, 0, 1);
+
+ // Create the bottom face.
+
+ int bottomStart = numSides+1;
+ addVec(vertices, 0, -halfHeight, 0);
+ addVec(normals, 0, -1.0, 0);
+ for (int i = 0; i < numSides; i++) {
+ float theta = (float) ((i*2*SimTK_PI)/numSides);
+ float stheta = sin(theta);
+ float ctheta = cos(theta);
+ addVec(vertices, radius*ctheta, -halfHeight, radius*stheta);
+ addVec(normals, 0, -1.0, 0);
+ }
+ for (int i = 1; i < numSides; i++)
+ addVec(faces, bottomStart+i, bottomStart+i+1, bottomStart);
+ addVec(faces, bottomStart+numSides, bottomStart+1, bottomStart);
+
+ // Create the sides.
+
+ for (int i = 0; i < numSides; i++) {
+ float theta = (float) ((i*2*SimTK_PI)/numSides);
+ float stheta = sin(theta);
+ float ctheta = cos(theta);
+ float x = radius*ctheta;
+ float z = radius*stheta;
+ addVec(vertices, x, halfHeight, z);
+ addVec(normals, ctheta, 0, stheta);
+ addVec(vertices, x, -halfHeight, z);
+ addVec(normals, ctheta, 0, stheta);
+ }
+ int sideStart = 2*numSides+2;
+ for (int i = 0; i < numSides-1; i++) {
+ int base = sideStart+2*i;
+ addVec(faces, base, base+2, base+1);
+ addVec(faces, base+1, base+2, base+3);
+ }
+ addVec(faces, sideStart+2*numSides-2, sideStart, sideStart+2*numSides-1);
+ addVec(faces, sideStart+2*numSides-1, sideStart, sideStart+1);
+ return new Mesh(vertices, normals, faces);
+}
+
+static Mesh* makeCircle(unsigned short resolution) {
+ const int numSides = 6*resolution;
+ const float radius = 1;
+ vector<GLfloat> vertices;
+ vector<GLfloat> normals;
+ vector<GLushort> faces;
+
+ // Create the front face.
+
+ addVec(vertices, 0, 0, 0);
+ addVec(normals, 0, 0, -1.0);
+ for (int i = 0; i < numSides; i++) {
+ float theta = (float) ((i*2*SimTK_PI)/numSides);
+ float stheta = sin(theta);
+ float ctheta = cos(theta);
+ addVec(vertices, radius*ctheta, radius*stheta, 0);
+ addVec(normals, 0, 0, -1.0);
+ }
+ for (int i = 1; i < numSides; i++)
+ addVec(faces, i, 0, i+1);
+ addVec(faces, numSides, 0, 1);
+
+ // Create the back face.
+
+ addVec(vertices, 0, 0, 0);
+ addVec(normals, 0, 0, 1.0);
+ for (int i = 0; i < numSides; i++) {
+ float theta = (float) ((i*2*SimTK_PI)/numSides);
+ float stheta = sin(theta);
+ float ctheta = cos(theta);
+ addVec(vertices, radius*ctheta, radius*stheta, 0);
+ addVec(normals, 0, 0, 1.0);
+ }
+ int backStart = numSides+1;
+ for (int i = 1; i < numSides; i++)
+ addVec(faces, backStart, backStart+i, backStart+i+1);
+ addVec(faces, backStart, backStart+numSides, backStart+1);
+ return new Mesh(vertices, normals, faces);
+}
+
+class PendingStandardMesh : public PendingCommand {
+public:
+ PendingStandardMesh(unsigned short meshIndex, unsigned short resolution) : meshIndex(meshIndex), resolution(resolution) {
+ }
+ void execute() {
+ if ((int) meshes[meshIndex].size() <= resolution)
+ meshes[meshIndex].resize(resolution+1, NULL);
+ if (meshes[meshIndex][resolution] == NULL) {
+ switch (meshIndex) {
+ case MeshBox:
+ meshes[meshIndex][resolution] = makeBox();
+ break;
+ case MeshEllipsoid:
+ meshes[meshIndex][resolution] = makeSphere(resolution);
+ break;
+ case MeshCylinder:
+ meshes[meshIndex][resolution] = makeCylinder(resolution);
+ break;
+ case MeshCircle:
+ meshes[meshIndex][resolution] = makeCircle(resolution);
+ break;
+ }
+ }
+ }
+ unsigned short meshIndex, resolution;
+};
+
+// Caution -- make sure scene is locked before you call this function.
+static void computeSceneBounds(const Scene* scene, float& radius, fVec3& center) {
+ // Record the bounding sphere of every object in the scene.
+
+ vector<fVec3> centers;
+ vector<float> radii;
+ for (int i = 0; i < (int) scene->drawnMeshes.size(); i++) {
+ fVec3 center;
+ float radius;
+ scene->drawnMeshes[i].computeBoundingSphere(radius, center);
+ centers.push_back(center);
+ radii.push_back(radius);
+ }
+ for (int i = 0; i < (int) scene->solidMeshes.size(); i++) {
+ fVec3 center;
+ float radius;
+ scene->solidMeshes[i].computeBoundingSphere(radius, center);
+ centers.push_back(center);
+ radii.push_back(radius);
+ }
+ for (int i = 0; i < (int) scene->transparentMeshes.size(); i++) {
+ fVec3 center;
+ float radius;
+ scene->transparentMeshes[i].computeBoundingSphere(radius, center);
+ centers.push_back(center);
+ radii.push_back(radius);
+ }
+ for (int i = 0; i < (int) scene->lines.size(); i++) {
+ fVec3 center;
+ float radius;
+ scene->lines[i].computeBoundingSphere(radius, center);
+ centers.push_back(center);
+ radii.push_back(radius);
+ }
+ for (int i = 0; i < (int) scene->sceneText.size(); i++) {
+ fVec3 center;
+ float radius;
+ scene->sceneText[i].computeBoundingSphere(radius, center);
+ centers.push_back(center);
+ radii.push_back(radius);
+ }
+
+ // Find the overall bounding sphere of the scene.
+
+ if (centers.size() == 0) {
+ radius = 0;
+ center = fVec3(0);
+ }
+ else {
+ fVec3 lower = centers[0]-radii[0];
+ fVec3 upper = centers[0]+radii[0];
+ for (int i = 1; i < (int) centers.size(); i++) {
+ for (int j = 0; j < 3; j++) {
+ lower[j] = min(lower[j], centers[i][j]-radii[i]);
+ upper[j] = max(upper[j], centers[i][j]+radii[i]);
+ }
+ }
+ center = (lower+upper)/2;
+ radius = 0;
+ for (int i = 0; i < (int) centers.size(); i++)
+ radius = max(radius, (centers[i]-center).norm()+radii[i]);
+ }
+}
+
+static void zoomCameraToShowWholeScene(bool sceneAlreadyLocked=false) {
+ float radius;
+ fVec3 center;
+ if (!sceneAlreadyLocked)
+ pthread_mutex_lock(&sceneLock); //-------- LOCK SCENE --------
+ computeSceneBounds(scene, radius, center);
+ if (!sceneAlreadyLocked)
+ pthread_mutex_unlock(&sceneLock); //----- UNLOCK SCENE ---------
+ float viewDistance =
+ radius/tan(min(fieldOfView, fieldOfView*viewWidth/viewHeight)/2);
+ // Back up 1 unit more to make sure we don't clip at this position.
+ // Also add a modest offset in the (x,y) direction to avoid edge-on views.
+ const float offset = std::max(1.f, viewDistance/10);
+ X_GC.updP() = center+X_GC.R()*fVec3(offset, offset, viewDistance+1);
+ // Now fix the aim to point at the center.
+ fVec3 zdir = X_GC.p() - center;
+ if (zdir.normSqr() >= square(1e-6))
+ X_GC.updR().setRotationFromTwoAxes(fUnitVec3(zdir), ZAxis,
+ X_GC.y(), YAxis);
+}
+
+class PendingCameraZoom : public PendingCommand {
+public:
+ void execute() {
+ zoomCameraToShowWholeScene(true); // scene already locked
+ }
+};
+
+class PendingSetCameraTransform : public PendingCommand {
+public:
+ PendingSetCameraTransform(fVec3 R, fVec3 p) : Rxyz(R), p(p) { }
+
+ void execute() {
+ X_GC.updR().setRotationToBodyFixedXYZ(Rxyz);
+ X_GC.updP() = p;
+ }
+
+private:
+ fVec3 Rxyz;
+ fVec3 p;
+};
+
+class PendingWindowTitleChange : public PendingCommand {
+public:
+ PendingWindowTitleChange(const string& title) : title(title) {}
+ void execute() {glutSetWindowTitle(title.c_str());}
+private:
+ string title;
+};
+
+
+class PendingBackgroundColorChange : public PendingCommand {
+public:
+ PendingBackgroundColorChange(const fVec3& color) : color(color) {}
+ void execute() {
+ backgroundColor=color; setClearColorToBackgroundColor();
+ }
+private:
+ fVec3 color;
+};
+
+
+
+/*==============================================================================
+ MENU
+==============================================================================*/
+
+// The glut callback for menu item picking.
+static void menuSelected(int option);
+static const int InvalidMenuId = std::numeric_limits<int>::min();
+class Menu {
+public:
+ Menu(string title, int id, const vector<pair<string, int> >& items,
+ void(*handler)(int))
+ : title(title), menuId(id), items(items), handler(handler),
+ hasCreated(false) {}
+
+ // This is called once, the first time we try to draw this menu.
+ void createMenu() {
+ glutId = glutCreateMenu(handler);
+ vector<string> components;
+ vector<int> submenuIds;
+ for (int i = 0; i < (int) items.size(); i++) {
+ size_t start = 0;
+ string spec = items[i].first;
+ for (int componentIndex = 0; ; componentIndex++) {
+ size_t end = spec.find('/', start);
+ while (end < spec.size()-1 && spec[end+1] == '/') {
+ // A double slash.
+ spec.erase(end, 1);
+ end = spec.find('/', end+1);
+ continue;
+ }
+ string substring = spec.substr(start, end-start);
+ if (componentIndex < (int)components.size() && substring != components[componentIndex]) {
+ components.resize(componentIndex);
+ submenuIds.resize(componentIndex);
+ }
+ if (componentIndex == components.size())
+ components.push_back(substring);
+ if (end == string::npos)
+ break;
+ start = end+1;
+ }
+ int firstNewSubmenu = (int) submenuIds.size();
+ for (int j = firstNewSubmenu; j < (int) components.size()-1; j++)
+ submenuIds.push_back(glutCreateMenu(handler));
+ glutSetMenu(firstNewSubmenu == 0 ? glutId
+ : submenuIds[firstNewSubmenu-1]);
+ for (int j = firstNewSubmenu; j < (int) components.size()-1; j++) {
+ glutAddSubMenu(components[j].c_str(), submenuIds[j]);
+ glutSetMenu(submenuIds[j]);
+ }
+ glutAddMenuEntry(components[components.size()-1].c_str(),
+ items[i].second);
+ }
+ }
+
+ int draw(int x, int y) {
+ if (!hasCreated) {
+ createMenu();
+ hasCreated = true;
+ }
+ minx = x;
+ miny = y-18;
+ int width = glutBitmapLength(GLUT_BITMAP_HELVETICA_18,
+ (unsigned char*) title.c_str());
+ maxx = x+width+14;
+ maxy = y+3;
+ glColor3f(0.9f, 0.9f, 0.9f);
+ glBegin(GL_POLYGON);
+ glVertex2i(minx+2, miny);
+ glVertex2i(minx, miny+1);
+ glVertex2i(minx, maxy-1);
+ glVertex2i(minx+2, maxy);
+ glVertex2i(maxx-1, maxy);
+ glVertex2i(maxx, maxy-1);
+ glVertex2i(maxx, miny+1);
+ glVertex2i(maxx-1, miny);
+ glEnd();
+ glColor3f(0.2f, 0.2f, 0.2f);
+ glRasterPos2f(GLfloat(x+2), GLfloat(y));
+ for (int i = 0; i < (int) title.size(); i++)
+ glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18, title[i]);
+ glBegin(GL_TRIANGLES);
+ glVertex2i(maxx-2, y-8);
+ glVertex2i(maxx-10,y-8);
+ glVertex2i(maxx-6, y-4);
+ glEnd();
+ return width+25;
+ }
+
+ // When the mouse moves onto this menu's pulldown tab, attach the menu
+ // to the mouse's left button. When it is next seen moving outside the
+ // pulldown tab, unattach the menu if it is still attached. Only one menu
+ // can be displayed at a time.
+ void mouseMoved(int x, int y) {
+ if (!hasCreated)
+ return;
+ if (x >= minx && y >= miny && x <= maxx && y <= maxy) {
+ if (currentMenuGlutId != glutId) {
+ glutSetMenu(glutId);
+ glutAttachMenu(GLUT_LEFT_BUTTON);
+ currentMenuGlutId = glutId;
+ currentMenuId = menuId;
+ }
+ }
+ else if (currentMenuGlutId == glutId) {
+ glutSetMenu(glutId);
+ glutDetachMenu(GLUT_LEFT_BUTTON);
+ currentMenuGlutId = currentMenuId = InvalidMenuId;
+ }
+ }
+
+ int getId() const {return menuId;}
+ const string& getTitle() const {return title;}
+
+ static const int& getCurrentMenuId() {return currentMenuId;}
+ static bool isMenuAttached() {return currentMenuId != InvalidMenuId;}
+
+private:
+ string title;
+ int menuId; // assigned by creator
+ vector<pair<string, int> > items;
+ void (*handler)(int);
+ int glutId; // assigned by glut
+ int minx, miny, maxx, maxy;
+ bool hasCreated;
+
+ static int currentMenuGlutId;
+ static int currentMenuId;
+};
+
+int Menu::currentMenuId = InvalidMenuId; // user assigned id
+int Menu::currentMenuGlutId = InvalidMenuId;
+
+// This is the handler for simulator-defined menus. All it does is send back
+// to the simulator the integer index that was associated with the particular
+// menu entry on which the user clicked.
+void menuSelected(int option) {
+ WRITE(outPipe, &MenuSelected, 1);
+ WRITE(outPipe, &Menu::getCurrentMenuId(), sizeof(int));
+ WRITE(outPipe, &option, sizeof(int));
+}
+
+static vector<Menu> menus;
+
+// Look up a particular menu; returns null pointer if not there.
+// Note that this is the user-assigned id, not the one from glut.
+static Menu* findMenuById(int id) {
+ for (unsigned i=0; i < menus.size(); ++i)
+ if (menus[i].getId() == id)
+ return &menus[i];
+ return 0;
+}
+
+
+
+/*==============================================================================
+ SLIDER
+==============================================================================*/
+class Slider {
+public:
+ Slider(string title, int id, float minValue, float maxValue, float value)
+ : title(title), id(id), minValue(minValue), maxValue(maxValue),
+ position((value-minValue)/(maxValue-minValue))
+ {
+ labelWidth = glutBitmapLength(GLUT_BITMAP_HELVETICA_18,
+ (unsigned char*) title.c_str());
+ if (labelWidth > maxLabelWidth)
+ maxLabelWidth = labelWidth;
+ }
+
+ int draw(int y) {
+ minx = maxLabelWidth+16;
+ maxx = minx+sliderWidth+handleWidth;
+ miny = y-18;
+ maxy = y+3;
+ int frameMinx = 10;
+ int frameMaxx = maxx+2;
+
+ // Draw the background.
+
+ glColor3f(0.9f, 0.9f, 0.9f);
+ glBegin(GL_POLYGON);
+ glVertex2i(frameMinx+2, miny);
+ glVertex2i(frameMinx, miny+1);
+ glVertex2i(frameMinx, maxy-1);
+ glVertex2i(frameMinx+2, maxy);
+ glVertex2i(frameMaxx-1, maxy);
+ glVertex2i(frameMaxx, maxy-1);
+ glVertex2i(frameMaxx, miny+1);
+ glVertex2i(frameMaxx-1, miny);
+ glEnd();
+
+ // Draw the slider.
+
+ glColor3f(0.5f, 0.5f, 0.5f);
+ glBegin(GL_QUADS);
+ glVertex2i(minx, miny+12);
+ glVertex2i(maxx, miny+12);
+ glVertex2i(maxx, miny+9);
+ glVertex2i(minx, miny+9);
+ glEnd();
+ glColor3f(0.2f, 0.2f, 0.2f);
+ handlex = minx+(int) (sliderWidth*position);
+ glBegin(GL_QUADS);
+ glVertex2i(handlex+handleWidth, miny+2);
+ glVertex2i(handlex, miny+2);
+ glVertex2i(handlex, miny+19);
+ glVertex2i(handlex+handleWidth, miny+19);
+ glEnd();
+
+ // Draw the label.
+
+ glRasterPos2f(GLfloat(12+maxLabelWidth-labelWidth), GLfloat(y));
+ for (int i = 0; i < (int) title.size(); i++)
+ glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18, title[i]);
+
+ // Draw the value.
+ char buffer[32];
+ sprintf(buffer, "%g", (double)calcValue());
+ string value(buffer);
+ int valueWidth = glutBitmapLength(GLUT_BITMAP_HELVETICA_10,
+ (unsigned char*) value.c_str());
+ int valueMinx = maxx+4;
+ int valueMaxx = valueMinx+valueWidth+4;
+ int valueMiny = y-14;
+ int valueMaxy = y-1;
+ glColor3f(0.9f, 0.9f, 0.9f);
+ glBegin(GL_POLYGON);
+ glVertex2i(valueMinx+2, valueMiny);
+ glVertex2i(valueMinx, valueMiny+1);
+ glVertex2i(valueMinx, valueMaxy-1);
+ glVertex2i(valueMinx+2, valueMaxy);
+ glVertex2i(valueMaxx-1, valueMaxy);
+ glVertex2i(valueMaxx, valueMaxy-1);
+ glVertex2i(valueMaxx, valueMiny+1);
+ glVertex2i(valueMaxx-1, valueMiny);
+ glEnd();
+ glColor3f(0.2f, 0.2f, 0.2f);
+ glRasterPos2f(GLfloat(valueMinx+2), GLfloat(y-4));
+ for (int i = 0; i < (int) value.size(); i++)
+ glutBitmapCharacter(GLUT_BITMAP_HELVETICA_10, value[i]);
+
+ return y-25;
+ }
+
+ bool mousePressed(int x, int y) {
+ dragging = false;
+ if (x >= minx && y >= miny && x <= maxx && y <= maxy) {
+ if (x < handlex) {
+ position = std::max(position-0.1f, 0.0f);
+ positionChanged();
+ }
+ else if (x > handlex+handleWidth) {
+ position = std::min(position+0.1f, 1.0f);
+ positionChanged();
+ }
+ else {
+ clickOffset = x-handlex;
+ dragging = true;
+ }
+ return true;
+ }
+ else
+ return false;
+ }
+
+ void mouseDragged(int x) {
+ if (dragging) {
+ float oldPosition = position;
+ position = clamp(0.f, (x-clickOffset-minx)/(float)sliderWidth, 1.f);
+ if (position != oldPosition)
+ positionChanged();
+ }
+ }
+
+ // The position is just 0..1; map that into the actual value.
+ float calcValue() const {return minValue+position*(maxValue-minValue);}
+
+ // Use this for programmatic changes to slider value. This does not get
+ // reported back to the simulation process.
+ void changeValue(float newValue) {
+ clampInPlace(minValue, newValue, maxValue); // fix newValue
+ const float range = maxValue-minValue;
+ if (range > 0) position = clamp(0.f, (newValue-minValue)/range, 1.f);
+ else position = 0.5f;
+ requestPassiveRedisplay(); //----- PASSIVE REDISPLAY ----
+ }
+
+ // This is for a programmatic change to the range after a slider has been
+ // defined. Value is unchanged if it still fits, otherwise it moves to
+ // the limit. This is not reported back to the simulation process.
+ void changeRange(float newMin, float newMax) {
+ SimTK_ASSERT_ALWAYS(newMax >= newMin, "Slider::changeRange(): bad range");
+ const float curValue = calcValue();
+ minValue = newMin; maxValue = newMax; // change bounds
+ changeValue(curValue); // recalculate relative position
+ }
+
+ int getId() const {return id;}
+ const string& getTitle() const {return title;}
+
+private:
+ void positionChanged() {
+ WRITE(outPipe, &SliderMoved, 1);
+ WRITE(outPipe, &id, sizeof(int));
+ float value = calcValue();
+ WRITE(outPipe, &value, sizeof(float));
+ requestPassiveRedisplay(); //----- PASSIVE REDISPLAY ----
+ }
+ string title;
+ int labelWidth;
+ int id, minx, miny, maxx, maxy, handlex;
+ int clickOffset;
+ bool dragging;
+ float minValue, maxValue; // actual min,max values
+ float position; // always in range [0,1]
+ static int maxLabelWidth;
+ static const int handleWidth = 5;
+ static const int sliderWidth = 100;
+};
+
+int Slider::maxLabelWidth = 0;
+
+
+static vector<Slider> sliders;
+
+// Look up a particular slider; returns null pointer if not there.
+static Slider* findSliderById(int id) {
+ for (unsigned i=0; i < sliders.size(); ++i)
+ if (sliders[i].getId() == id)
+ return &sliders[i];
+ return 0;
+}
+
+
+
+/*==============================================================================
+ GROUND AND SKY
+==============================================================================*/
+static void drawSkyVertex(fVec3 position, float texture) {
+ glTexCoord1f(texture);
+ glVertex3fv(&position[0]);
+}
+
+static void drawGroundAndSky(float farClipDistance) {
+ static bool initialized = false;
+ static GLuint skyTexture;
+ static GLuint groundTexture;
+ if (!initialized) {
+ initialized = true;
+
+ // Create a texture to use as the sky.
+
+ glGenTextures(1, &skyTexture);
+ glBindTexture(GL_TEXTURE_1D, skyTexture);
+ glTexParameterf(GL_TEXTURE_1D, GL_GENERATE_MIPMAP, GL_TRUE);
+ const int width = 256;
+ float skyImage[3*width];
+ for (int i = 0; i < width; i++) {
+ float fract = pow(i/(float) width, 1.8f);
+ skyImage[3*i] = fract;
+ skyImage[3*i+1] = fract;
+ skyImage[3*i+2] = 1;
+ }
+ glTexImage1D(GL_TEXTURE_1D, 0, 3, width, 0, GL_RGB, GL_FLOAT, skyImage);
+
+ // Create a texture to use as the ground.
+
+ srand(0);
+ glGenTextures(1, &groundTexture);
+ glBindTexture(GL_TEXTURE_2D, groundTexture);
+ glTexParameterf(GL_TEXTURE_2D, GL_GENERATE_MIPMAP, GL_TRUE);
+ float groundImage[width*width];
+ for (int i = 0; i < width; i++) {
+ float x = i/(float) width;
+ for (int j = 0; j < width; j++) {
+ float y = j/(float) width;
+ double line = min(min(min(x, y), 1.0f-x), 1.0f-y);
+ float noise = (rand()%255)/255.0f-0.5f;
+ groundImage[i*width+j] = pow(line,0.1)*(0.35f+noise);
+ }
+ }
+ glTexImage2D(GL_TEXTURE_2D, 0, 1, width, width, 0, GL_RED, GL_FLOAT, groundImage);
+ }
+
+ // Draw the box to represent the sky.
+
+ float viewDistance = farClipDistance*0.5f;
+ fVec3 center = X_GC.p();
+ const float sign = (float)groundNormal.getDirection(); // 1 or -1
+ const CoordinateAxis axis = groundNormal.getAxis();
+ float top = center[axis]+sign*viewDistance;
+ center[axis] = 0;
+ fVec3 offset1 = viewDistance*(X_GC.R()(2)%fUnitVec3(axis));
+ fVec3 offset2 = (offset1%fUnitVec3(axis));
+ fVec3 corner1 = center+(-offset1-offset2);
+ fVec3 corner2 = center+(offset1-offset2);
+ fVec3 corner3 = center+(offset1+offset2);
+ fVec3 corner4 = center+(-offset1+offset2);
+ glActiveTexture(GL_TEXTURE0);
+ glEnable(GL_TEXTURE_1D);
+ glBindTexture(GL_TEXTURE_1D, skyTexture);
+ glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE);
+ glTexParameteri(GL_TEXTURE_1D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+ glDisable(GL_DEPTH_TEST);
+ glDepthMask(GL_FALSE);
+ glBegin(GL_QUAD_STRIP);
+ fVec3 offset3 = sign*fUnitVec3(axis);
+ drawSkyVertex(corner1+top*offset3, 0);
+ drawSkyVertex(corner1+groundHeight*offset3, 1);
+ drawSkyVertex(corner2+top*offset3, 0);
+ drawSkyVertex(corner2+groundHeight*offset3, 1);
+ drawSkyVertex(corner3+top*offset3, 0);
+ drawSkyVertex(corner3+groundHeight*offset3, 1);
+ drawSkyVertex(corner4+top*offset3, 0);
+ drawSkyVertex(corner4+groundHeight*offset3, 1);
+ drawSkyVertex(corner1+top*offset3, 0);
+ drawSkyVertex(corner1+groundHeight*offset3, 1);
+ glEnd();
+ glDisable(GL_TEXTURE_1D);
+ glColor3f(0, 0, 1);
+ glBegin(GL_QUADS);
+ drawSkyVertex(corner1+0.99f*top*offset3, 0);
+ drawSkyVertex(corner2+0.99f*top*offset3, 0);
+ drawSkyVertex(corner3+0.99f*top*offset3, 0);
+ drawSkyVertex(corner4+0.99f*top*offset3, 0);
+ glEnd();
+ glEnable(GL_DEPTH_TEST);
+ glDepthMask(GL_TRUE);
+
+ // Draw the ground plane.
+
+ center = X_GC.p()-X_GC.R()*fVec3(0, 0, 0.9999f*farClipDistance);
+ center[1] = 0;
+ corner1 = center+fVec3(-farClipDistance, 0, -farClipDistance);
+ corner2 = center+fVec3(farClipDistance, 0, -farClipDistance);
+ corner3 = center+fVec3(farClipDistance, 0, farClipDistance);
+ corner4 = center+fVec3(-farClipDistance, 0, farClipDistance);
+ // We need to calculate the GL transform T_GP that gives the ground
+ // plane's coordinate frame P in the ground frame G.
+ Mat<4, 4, GLfloat> T_GP(1);
+ fVec3::updAs(&T_GP(YAxis)[0]) = fVec3(fUnitVec3(groundNormal)); // signed
+ fVec3::updAs(&T_GP(XAxis)[0]) = fVec3(fUnitVec3(axis.getPreviousAxis()));
+ fVec3::updAs(&T_GP(ZAxis)[0]) = fVec3(fUnitVec3(axis.getNextAxis()));
+ T_GP[axis][3] = sign*groundHeight;
+ if (showShadows) {
+ // Draw shadows on the ground. These are drawn in the shadow frame S,
+ // which we'll distort slightly from the ground plane P.
+ Mat<4, 4, GLfloat> T_PS(1);
+ T_PS[axis.getPreviousAxis()][axis] = 0.2f;
+ T_PS[axis][axis] = 0.0f;
+ T_PS[axis.getNextAxis()][axis] = 0.2f;
+ T_PS[axis][3] = sign*groundHeight;
+ glPushMatrix();
+ glMultMatrixf(&T_PS[0][0]);
+ // Solid and transparent shadows are the same color (sorry). Trying to
+ // mix light and dark shadows is much harder and any simple attempts
+ // (e.g. put light shadows on top of dark ones) look terrible.
+ glColor3f(0.3f, 0.2f, 0.0f);
+ for (int i = 0; i < (int) scene->solidMeshes.size(); i++)
+ scene->solidMeshes[i].draw(false);
+ for (int i = 0; i < (int) scene->transparentMeshes.size(); i++)
+ scene->transparentMeshes[i].draw(false);
+ for (int i = 0; i < (int) scene->lines.size(); i++)
+ scene->lines[i].draw(false);
+ glPopMatrix();
+ }
+ glActiveTexture(GL_TEXTURE0);
+ glEnable(GL_TEXTURE_2D);
+ glBindTexture(GL_TEXTURE_2D, groundTexture);
+ glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_BLEND);
+ glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
+ glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+ glDisable(GL_CULL_FACE);
+ glPushMatrix();
+ glMultMatrixf(&T_GP[0][0]);
+ glDepthRange(0.01, 1.0);
+ float color2[] = {1.0f, 0.8f, 0.7f};
+ glBindTexture(GL_TEXTURE_2D, groundTexture);
+ glTexEnvfv(GL_TEXTURE_ENV, GL_TEXTURE_ENV_COLOR, color2);
+ glBegin(GL_QUADS);
+ glColor3f(0.3f, 0.2f, 0.0f);
+ glTexCoord2d(2.0f*corner1[0], 2.0f*corner1[2]);
+ glVertex3f(corner1[0], corner1[1], corner1[2]);
+ glTexCoord2d(2.0f*corner2[0], 2.0f*corner2[2]);
+ glVertex3f(corner2[0], corner2[1], corner2[2]);
+ glTexCoord2d(2.0f*corner3[0], 2.0f*corner3[2]);
+ glVertex3f(corner3[0], corner3[1], corner3[2]);
+ glTexCoord2d(2.0f*corner4[0], 2.0f*corner4[2]);
+ glVertex3f(corner4[0], corner4[1], corner4[2]);
+ glEnd();
+ glDepthRange(0.0, 1.0);
+ glEnable(GL_CULL_FACE);
+ glPopMatrix();
+ glDisable(GL_TEXTURE_2D);
+}
+
+
+
+/*==============================================================================
+ RENDER SCENE
+==============================================================================*/
+static void renderScene(std::vector<std::string>* screenText = NULL) {
+ static bool firstTime = true;
+ static GLfloat prevNearClip; // initialize to near & farClip
+ static GLfloat prevFarClip;
+ if (firstTime) {
+ prevNearClip = nearClip;
+ prevFarClip = farClip;
+ firstTime = false;
+ }
+
+ glEnable(GL_DEPTH_TEST);
+ glLoadIdentity();
+ glDisable(GL_LIGHTING);
+ glDisableClientState(GL_NORMAL_ARRAY);
+ glDisable(GL_BLEND);
+ glDepthMask(GL_TRUE);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ pthread_mutex_lock(&sceneLock); //-------- LOCK SCENE --------
+
+ if (scene != NULL) {
+ // Remember the simulated time associated with the most recent rendered
+ // scene.
+ lastSceneSimTime = scene->simTime;
+
+ // Execute any pending commands that need to be executed on the
+ // rendering thread. This happens only the first time a particular
+ // scene is drawn because we delete the pending commands after
+ // executing them.
+ for (int i = 0; i < (int) pendingCommands.size(); i++) {
+ pendingCommands[i]->execute();
+ delete pendingCommands[i];
+ }
+ pendingCommands.clear();
+
+ // Set up the viewpoint.
+
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glViewport(0, 0, viewWidth, viewHeight);
+ float sceneRadius;
+ fVec3 sceneCenter;
+ // Scene is already locked so OK to call this.
+ computeSceneBounds(scene, sceneRadius, sceneCenter);
+ float sceneScale = std::max(0.1f, sceneRadius);
+
+ float centerDepth = ~(X_GC.p()-sceneCenter)*(X_GC.R().col(2));
+ float nearClipDistance, farClipDistance;
+ if (showGround) {
+ nearClipDistance = nearClip;
+ const float wantFarClipDistance = min(farClip,
+ max(2*sceneScale, 2*(centerDepth+sceneRadius)));
+ if (std::abs(wantFarClipDistance-prevFarClip) <= 0.5*prevFarClip)
+ farClipDistance = prevFarClip; // hysteresis to avoid jiggling ground
+ else farClipDistance = wantFarClipDistance;
+ }
+ else {
+ nearClipDistance = max(nearClip, centerDepth-sceneRadius);
+ farClipDistance = min(farClip, centerDepth+sceneRadius);
+ }
+ prevNearClip = nearClipDistance;
+ prevFarClip = farClipDistance;
+
+ gluPerspective(fieldOfView*SimTK_RADIAN_TO_DEGREE, (GLdouble) viewWidth/viewHeight, nearClipDistance, farClipDistance);
+ glMatrixMode(GL_MODELVIEW);
+ fVec3 cameraPos = X_GC.p();
+ fVec3 centerPos = X_GC.p()+X_GC.R()*fVec3(0, 0, -1);
+ fVec3 upDir = X_GC.R()*fVec3(0, 1, 0);
+ gluLookAt(cameraPos[0], cameraPos[1], cameraPos[2], centerPos[0], centerPos[1], centerPos[2], upDir[0], upDir[1], upDir[2]);
+
+ // Render the objects in the scene.
+
+ if (showGround)
+ drawGroundAndSky(farClipDistance);
+ for (int i = 0; i < (int) scene->lines.size(); i++)
+ scene->lines[i].draw();
+ glLineWidth(2);
+ for (int i = 0; i < (int) scene->sceneText.size(); i++)
+ scene->sceneText[i].draw();
+ glLineWidth(1);
+ glEnableClientState(GL_NORMAL_ARRAY);
+ for (int i = 0; i < (int) scene->drawnMeshes.size(); i++)
+ scene->drawnMeshes[i].draw();
+ glEnable(GL_LIGHTING);
+ for (int i = 0; i < (int) scene->solidMeshes.size(); i++)
+ scene->solidMeshes[i].draw();
+ glEnable(GL_BLEND);
+ glDepthMask(GL_FALSE);
+ vector<pair<float, int> > order(scene->transparentMeshes.size());
+ for (int i = 0; i < (int) order.size(); i++)
+ order[i] = make_pair((float)(~X_GC.R()*scene->transparentMeshes[i].getTransform().p())[2], i);
+ sort(order.begin(), order.end());
+ for (int i = 0; i < (int) order.size(); i++)
+ scene->transparentMeshes[order[i].second].draw();
+
+ // Update the frame rate counter.
+
+ const double now = realTime(); // in seconds at high resolution
+ const double elapsed = now - fpsBaseTime;
+ if (elapsed >= 1) {
+ fps = fpsCounter/elapsed;
+ fpsBaseTime = now;
+ fpsCounter = 0;
+ }
+
+ // Extract the scene text if requested. This will get displayed
+ // later but must be copied out now since the scene will get
+ // overwritten once we say it has been drawn.
+ if (screenText != NULL)
+ for (int i = 0; i < (int)scene->screenText.size(); ++i)
+ screenText->push_back(scene->screenText[i].getString());
+
+ scene->sceneHasBeenDrawn = true;
+ }
+
+ // Signal in case the listener is waiting.
+ pthread_cond_signal(&sceneHasBeenDrawn); //----- SIGNAL CONDITION -----
+ pthread_mutex_unlock(&sceneLock); //----- UNLOCK SCENE ---------
+}
+
+
+// Redraw everything currently being displayed. That means:
+// - the "front" scene
+// - menus
+// - overlay text like the frame rate display
+// - overlay messages (these are up for a while then time out)
+//
+// We also update the frame number and FPS counter. Note that numerous
+// frames may be produced from the same scene, so you can expect the frame
+// numbers to be higher than the number of scenes sent by the simulator.
+static void redrawDisplay() {
+ // If a movie is being generated, save frames.
+ // ------------------------------------------------------------
+ if (saveNextFrameToMovie) {
+ saveNextFrameToMovie = false;
+ stringstream filename;
+ filename << movieDir;
+ filename << "/Frame";
+ filename << setw(4) << setfill('0') << movieFrame++;
+ filename << ".png";
+ writeImage(filename.str());
+ }
+
+ // Render the scene and extract the screen text.
+ // ------------------------------------------------------------
+ std::vector<string> screenText;
+ renderScene(&screenText);
+
+ // Draw menus.
+ // ------------------------------------------------------------
+ glDisable(GL_BLEND);
+ glDepthMask(GL_TRUE);
+ glMatrixMode(GL_PROJECTION);
+ glPushMatrix();
+ glLoadIdentity();
+ gluOrtho2D(0, viewWidth, 0, viewHeight);
+ glScalef(1, -1, 1);
+ glTranslatef(0, GLfloat(-viewHeight), 0);
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+ glDisable(GL_LIGHTING);
+ glDisable(GL_DEPTH_TEST);
+ int menux = 10;
+ for (int i = 0; i < (int) menus.size(); i++)
+ menux += menus[i].draw(menux, viewHeight-10);
+
+ // Draw sliders.
+ // ------------------------------------------------------------
+ int slidery = viewHeight-35;
+ for (int i = 0; i < (int) sliders.size(); i++)
+ slidery = sliders[i].draw(slidery);
+
+ // Draw the "heads-up" display.
+ // ------------------------------------------------------------
+ glColor3f(1.0f, 0.5f, 0.0f);
+ void* font = (void*)GLUT_BITMAP_HELVETICA_18;
+ GLfloat nextLine = 25, lineHeight = 18;
+
+ // Frames per second
+ if (showFPS) {
+ char fpstxt[64]; sprintf(fpstxt, "FPS: %.1f", fps); // 1 decimal place
+ glRasterPos2f(10, nextLine);
+ for (const char* p = fpstxt; *p; ++p)
+ glutBitmapCharacter(font, *p);
+ nextLine += lineHeight;
+ }
+
+ // Simulation time
+ if (showSimTime) {
+ char timetxt[64]; sprintf(timetxt, "Time: %.3f", lastSceneSimTime);
+ glRasterPos2f(10, nextLine);
+ for (const char* p = timetxt; *p; ++p)
+ glutBitmapCharacter(font, *p);
+ nextLine += lineHeight;
+ }
+
+ // Frame number
+ if (showFrameNum) {
+ char cnttxt[64]; sprintf(cnttxt, "Frame: %d", frameCounter);
+ glRasterPos2f(10, nextLine);
+ for (const char* p = cnttxt; *p; ++p)
+ glutBitmapCharacter(font, *p);
+ nextLine += lineHeight;
+ }
+
+ // User specified screen text
+ for (int i=0; i<(int)screenText.size(); ++i) {
+ glRasterPos2f(10, nextLine);
+ for (const char* p = screenText[i].c_str(); *p; ++p)
+ glutBitmapCharacter(font, *p);
+ nextLine += lineHeight;
+ }
+
+ // Draw a message overlay
+ // (center box, with text left justified in box).
+ // ------------------------------------------------------------
+ if (displayOverlayMessage) {
+ void* font = (void*)GLUT_BITMAP_HELVETICA_18;
+ const int lineSpacing = 25;
+ int width = 0;
+ for (unsigned i=0; i < overlayMessageLines.size(); ++i)
+ width = max(width, glutBitmapLength(font,
+ (unsigned char*)overlayMessageLines[i].c_str()));
+ int height = lineSpacing * (int)overlayMessageLines.size();
+
+ GLfloat xpos = GLfloat(max(0, (viewWidth-width)/2));
+ GLfloat ypos = GLfloat(max(0, (viewHeight-height)/2));
+
+ GLfloat xborder = 4;
+ Vec<2,GLfloat> tl(xpos-xborder, ypos-19);
+ Vec<2,GLfloat> br(xpos+(float)width+xborder, ypos-19+(float)height);
+
+ // Draw an orange background.
+ glColor3f(1.0f, 0.5f, 0.f);
+ glBegin(GL_QUADS);
+ glVertex2f(tl[0],tl[1]);
+ glVertex2f(tl[0],br[1]);
+ glVertex2f(br[0],br[1]);
+ glVertex2f(br[0],tl[1]);
+ glEnd();
+
+ // Draw dark gray text.
+ glColor3f(0.2f, 0.2f, 0.2f);
+ for (unsigned line=0; line < overlayMessageLines.size(); ++line) {
+ glRasterPos2f(xpos, ypos);
+ for (unsigned i = 0; i < overlayMessageLines[line].size(); i++)
+ glutBitmapCharacter(font, overlayMessageLines[line][i]);
+ ypos += (float)lineSpacing;
+ }
+ }
+
+ glMatrixMode(GL_PROJECTION);
+ glPopMatrix();
+ glutSwapBuffers();
+ ++fpsCounter;
+ ++frameCounter;
+
+ lastRedisplayDone = realTime();
+}
+
+// These are set when a mouse button is clicked and then referenced
+// while the mouse is dragged with that button down.
+static int clickModifiers;
+static int clickButton;
+static int clickX;
+static int clickY;
+static int clickedSlider = -1;
+static bool rotateCenterValid = false;
+static fVec3 rotateCenter;
+static float sceneScale; // for scaling translations
+
+// Handle the initial press of a mouse button. Standard glut does not
+// deal with the mouse wheel, but freeglut and some tweaked gluts treat
+// it as a "button press" with button codes 3 (up) and 4 (down). This
+// function handles those properly if they are present.
+static void mouseButtonPressedOrReleased(int button, int state, int x, int y) {
+ const int GlutWheelUp = 3, GlutWheelDown = 4;
+
+ if (scene == NULL)
+ return; // probably mouse click during startup
+
+ float radius;
+ fVec3 sceneCenter;
+ pthread_mutex_lock(&sceneLock); //-------- LOCK SCENE --------
+ computeSceneBounds(scene, radius, sceneCenter);
+ pthread_mutex_unlock(&sceneLock); //------ UNLOCK SCENE --------
+ sceneScale = std::max(radius, 0.1f);
+
+ // "state" (pressed/released) is irrelevant for mouse wheel. However, if
+ // we're being called by freeglut we'll get called twice, while (patched)
+ // glut calls just once, with state=GLUT_UP.
+ if ((button == GlutWheelUp || button == GlutWheelDown)
+ && (state==GLUT_UP))
+ {
+ // Scroll wheel.
+ const float ZoomFractionPerWheelClick = 0.15f; // 15% scene radius
+
+ const int direction = button==GlutWheelUp ? -1 : 1;
+ const float zoomBy = direction * (ZoomFractionPerWheelClick * sceneScale);
+
+ pthread_mutex_lock(&sceneLock); //------ LOCK SCENE ----------
+ X_GC.updP() += X_GC.R()*fVec3(0, 0, zoomBy);
+ requestPassiveRedisplay(); //------ PASSIVE REDISPLAY ---
+ pthread_mutex_unlock(&sceneLock); //------ UNLOCK SCENE --------
+ return;
+ }
+
+ // Not mouse wheel; currently ignore "button up" message except to
+ // forget the rotation center. This is needed to get around a glut
+ // bug described in mouseDragged().
+ if (state == GLUT_UP) {
+ if (clickButton == GLUT_LEFT_BUTTON)
+ rotateCenterValid = false;
+ return;
+ }
+
+ // Handle state == GLUT_DOWN:
+
+ // Remember which button was pressed; we'll deal with it in mouseDragged().
+ clickModifiers = glutGetModifiers();
+ clickButton = button;
+ clickX = x;
+ clickY = y;
+
+ // See if this click was on a slider.
+ clickedSlider = -1;
+ for (int i = 0; i < (int) sliders.size(); i++) {
+ if (sliders[i].mousePressed(x, y)) {
+ clickedSlider = i;
+ return;
+ }
+ }
+
+ // Left button is rotation; when it is first pressed we calcuate the center
+ // of rotation; we'll do the actual rotating in mouseDragged().
+ if (clickButton == GLUT_LEFT_BUTTON) {
+ float distToCenter = (sceneCenter-X_GC.p()).norm();
+ float defaultDistance = sceneScale;
+ if (distToCenter > defaultDistance)
+ rotateCenter = sceneCenter;
+ else {
+ fVec3 cameraDir = X_GC.R()*fVec3(0, 0, -1);
+ fVec3 lookAt = X_GC.p()+defaultDistance*cameraDir;
+ float fract = (defaultDistance-distToCenter)/defaultDistance;
+ rotateCenter = fract*lookAt+(1-fract)*sceneCenter;
+ }
+ rotateCenterValid = true;
+ }
+}
+
+// This function is called when the mouse is moved while a button is being held
+// down. When the button was first clicked, we recorded which one it was in
+// clickButton, and where the mouse was then in (clickX,clickY). We update
+// (clickX,clickY) each call here to reflect where it was last seen.
+//
+// (sherm 20101121: on Windows glut I get a spurious "mouse dragged" call
+// when I have a menu displayed and then click outside the menu but inside
+// the graphics window.)
+static void mouseDragged(int x, int y) {
+ if (clickedSlider > -1) {
+ sliders[clickedSlider].mouseDragged(x);
+ return;
+ }
+
+ // 1/4 degree per pixel
+ const float AnglePerPixel = 0.25*((float)SimTK_PI/180);
+
+ // map 1 pixel move to 1% of scale
+ const float TranslateFracPerPixel = 0.01f;
+ const float translatePerPixel = TranslateFracPerPixel * sceneScale;
+ const int dx = clickX-x, dy = clickY-y;
+
+ // translate: right button or shift-left button
+ if ( clickButton == GLUT_RIGHT_BUTTON
+ || (clickButton == GLUT_LEFT_BUTTON && clickModifiers & GLUT_ACTIVE_SHIFT))
+ {
+ pthread_mutex_lock(&sceneLock); //------ LOCK SCENE ----------
+ X_GC.updP() += translatePerPixel*X_GC.R()*fVec3(dx, -dy, 0);
+ pthread_mutex_unlock(&sceneLock); //------ UNLOCK SCENE --------
+ }
+
+ // zoom: middle button or alt-left button (or mouse wheel; see above)
+ else if ( clickButton == GLUT_MIDDLE_BUTTON
+ || (clickButton == GLUT_LEFT_BUTTON && clickModifiers & GLUT_ACTIVE_ALT))
+ {
+ pthread_mutex_lock(&sceneLock); //------ LOCK SCENE ----------
+ X_GC.updP() += translatePerPixel* X_GC.R()*fVec3(0, 0, dy);
+ pthread_mutex_unlock(&sceneLock); //------ UNLOCK SCENE --------
+ }
+
+ // rotate: left button alone: rotate scene left/right or up/down
+ // ctrl-left button: roll scene about camera direction
+ else if (clickButton == GLUT_LEFT_BUTTON && rotateCenterValid) {
+ fVec3 cameraPos = X_GC.p();
+ fVec3 cameraDir = X_GC.R()*fVec3(0, 0, -1);
+ fVec3 upDir = X_GC.R()*fVec3(0, 1, 0);
+ fRotation r;
+ if (clickModifiers & GLUT_ACTIVE_CTRL)
+ r.setRotationFromAngleAboutAxis(AnglePerPixel*(dy-dx), ZAxis);
+ else
+ r.setRotationFromTwoAnglesTwoAxes(SpaceRotationSequence,
+ AnglePerPixel*dy, XAxis, AnglePerPixel*dx, YAxis);
+ r = X_GC.R()*r*~X_GC.R();
+ cameraPos = r*(cameraPos-rotateCenter)+rotateCenter;
+ cameraDir = r*cameraDir;
+ upDir = r*upDir;
+
+ pthread_mutex_lock(&sceneLock); //------ LOCK SCENE ----------
+ X_GC.updP() = cameraPos;
+ X_GC.updR().setRotationFromTwoAxes(fUnitVec3(-cameraDir), ZAxis, upDir, YAxis);
+ pthread_mutex_unlock(&sceneLock); //------ UNLOCK SCENE --------
+ }
+ else
+ return;
+
+ // Something changed.
+
+ clickX = x; clickY = y;
+ requestPassiveRedisplay(); //------ PASSIVE REDISPLAY ---
+}
+
+// Currently the only "passive" mouse motion we care about is if the
+// mouse is in an active menu, in which case its position marks the
+// menu item that would be selected upon clicking.
+// TODO: pass this to the simulator otherwise.
+static void mouseMoved(int x, int y) {
+ for (int i = 0; i < (int) menus.size(); i++)
+ menus[i].mouseMoved(x, y);
+}
+
+// Glut distinguishes ordinary ASCII key presses from special
+// ones like arrows and function keys. We will map either case
+// into the same "key pressed" command to send to the simulator.
+static void ordinaryKeyPressed(unsigned char key, int x, int y) {
+ char command = KeyPressed;
+ WRITE(outPipe, &command, 1);
+ unsigned char buffer[2];
+ buffer[0] = key;
+ buffer[1] = 0;
+ unsigned char modifiers = glutGetModifiers();
+ if ((modifiers & GLUT_ACTIVE_SHIFT) != 0)
+ buffer[1] += Visualizer::InputListener::ShiftIsDown;
+ if ((modifiers & GLUT_ACTIVE_CTRL) != 0)
+ buffer[1] += Visualizer::InputListener::ControlIsDown;
+ if ((modifiers & GLUT_ACTIVE_ALT) != 0)
+ buffer[1] += Visualizer::InputListener::AltIsDown;
+ WRITE(outPipe, buffer, 2);
+}
+
+static void specialKeyPressed(int key, int x, int y) {
+ char command = KeyPressed;
+ WRITE(outPipe, &command, 1);
+ unsigned char buffer[2];
+ buffer[0] = (unsigned char)key; // this is the special key code
+ buffer[1] = Visualizer::InputListener::IsSpecialKey;
+ unsigned char modifiers = glutGetModifiers();
+ if ((modifiers & GLUT_ACTIVE_SHIFT) != 0)
+ buffer[1] &= Visualizer::InputListener::ShiftIsDown;
+ if ((modifiers & GLUT_ACTIVE_CTRL) != 0)
+ buffer[1] &= Visualizer::InputListener::ControlIsDown;
+ if ((modifiers & GLUT_ACTIVE_ALT) != 0)
+ buffer[1] &= Visualizer::InputListener::AltIsDown;
+ WRITE(outPipe, buffer, 2);
+}
+
+// This function is called when the timer goes off after an overlay message
+// has been displayed long enough. On some platforms the timer may go off
+// early -- if so we'll reissue it here for the remaining time.
+static void disableOverlayTimer(int value) {
+ const double now = realTime();
+ if (now + 0.1 < overlayEndTime) { // 100ms slop
+ // went off too early
+ glutTimerFunc((unsigned)((overlayEndTime-now)*1000), // ms
+ disableOverlayTimer, 0);
+ return;
+ }
+ displayOverlayMessage = false;
+ overlayStartTime = overlayEndTime = NaN;
+ requestPassiveRedisplay(); //------ PASSIVE REDISPLAY ---
+}
+
+static void setOverlayMessage(const string& message) {
+ if (displayOverlayMessage) return; // there is already one up
+ overlayMessageLines.clear();
+ string::size_type start = 0;
+ while (start < message.size()) {
+ string::size_type newline = message.find_first_of('\n', start);
+ if (newline == string::npos) newline = message.size();
+ overlayMessageLines.push_back(message.substr(start, newline-start));
+ start = newline+1;
+ }
+
+ if (overlayMessageLines.empty())
+ return;
+
+ displayOverlayMessage = true;
+ requestPassiveRedisplay(); //------ PASSIVE REDISPLAY ---
+
+ overlayStartTime = realTime();
+ overlayEndTime = overlayStartTime + OverlayDisplayTimeInSec;
+ glutTimerFunc((unsigned)(OverlayDisplayTimeInSec*1000), // ms
+ disableOverlayTimer, 0);
+}
+
+class SaveImageTask : public ParallelWorkQueue::Task {
+public:
+ Array_<unsigned char> data;
+ SaveImageTask(const string& filename, int width, int height) : filename(filename), width(width), height(height), data(width*height*3) {
+ }
+ void execute() {
+ // Flip the image vertically, since OpenGL and PNG use different row orders.
+
+ const int rowLength = 3*width;
+ for (int row = 0; row < height/2; ++row) {
+ const int base1 = row*rowLength;
+ const int base2 = (height-1-row)*rowLength;
+ for (int i = 0; i < rowLength; i++) {
+ unsigned char temp = data[base1+i];
+ data[base1+i] = data[base2+i];
+ data[base2+i] = temp;
+ }
+ }
+ LodePNG::encode(filename, data.empty() ? 0 : &data[0], width, height, 2, 8);
+ }
+private:
+ string filename;
+ int width, height;
+};
+
+static void writeImage(const string& filename) {
+ int width = ((viewWidth+3)/4)*4; // must be a multiple of 4 pixels
+ int height = viewHeight;
+
+ // Create offscreen buffers for rendering the image.
+
+ GLuint frameBuffer, colorBuffer, depthBuffer;
+ glGenFramebuffersEXT(1, &frameBuffer);
+ glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, frameBuffer);
+ glGenRenderbuffersEXT(1, &colorBuffer);
+ glBindRenderbufferEXT(GL_RENDERBUFFER_EXT, colorBuffer);
+ glRenderbufferStorageEXT(GL_RENDERBUFFER_EXT, GL_RGB8, width, height);
+ glFramebufferRenderbufferEXT(GL_FRAMEBUFFER_EXT, GL_COLOR_ATTACHMENT0_EXT, GL_RENDERBUFFER_EXT, colorBuffer);
+ glGenRenderbuffersEXT(1, &depthBuffer);
+ glBindRenderbufferEXT(GL_RENDERBUFFER_EXT, depthBuffer);
+ glRenderbufferStorageEXT(GL_RENDERBUFFER_EXT, GL_DEPTH_COMPONENT24, width, height);
+ glFramebufferRenderbufferEXT(GL_FRAMEBUFFER_EXT, GL_DEPTH_ATTACHMENT_EXT, GL_RENDERBUFFER_EXT, depthBuffer);
+
+ // Render the image and load it into memory.
+
+ renderScene();
+ SaveImageTask* task = new SaveImageTask(filename, width, height);
+ Array_<unsigned char>& data = task->data;
+ glReadPixels(0, 0, width, height, GL_RGB, GL_UNSIGNED_BYTE, &data[0]);
+ glDeleteRenderbuffersEXT(1, &colorBuffer);
+ glDeleteRenderbuffersEXT(1, &depthBuffer);
+ glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0);
+ glDeleteFramebuffersEXT(1, &frameBuffer);
+
+ // Add it to the queue to be saved to disk.
+
+ imageSaverQueue.addTask(task);
+}
+
+static void saveImage() {
+ struct stat statInfo;
+ int counter = 0;
+ string filename;
+ do {
+ counter++;
+ stringstream namestream;
+ namestream << simulatorExecutableName.c_str() << "_";
+ namestream << counter;
+ namestream << ".png";
+ filename = namestream.str();
+ } while (stat(filename.c_str(), &statInfo) == 0);
+ writeImage(filename);
+ setOverlayMessage("Image saved as:\n"+filename);
+}
+
+static void saveMovie() {
+ struct stat statInfo;
+ int counter = 0;
+ string dirname;
+ do {
+ counter++;
+ stringstream namestream;
+ namestream << simulatorExecutableName.c_str() << "_";
+ namestream << counter;
+ dirname = namestream.str();
+ } while (stat(dirname.c_str(), &statInfo) == 0);
+#ifdef _WIN32
+ int result = mkdir(dirname.c_str());
+#else
+ int result = mkdir(dirname.c_str(), 0777);
+#endif
+ if (result == -1)
+ setOverlayMessage("Failed to create directory:\n"+dirname);
+ else {
+ movieDir = dirname;
+ movieFrame = 1;
+ savingMovie = true;
+ setOverlayMessage("Capturing frames in:\n"+dirname);
+ }
+}
+
+// Read a particular number of bytes from srcPipe to the given buffer.
+// This will hang until the expected number of bytes has been received.
+static void readDataFromPipe(int srcPipe, unsigned char* buffer, int bytes) {
+ int totalRead = 0;
+ while (totalRead < bytes)
+ totalRead += READ(srcPipe, buffer+totalRead, bytes-totalRead);
+}
+static void readData(unsigned char* buffer, int bytes) {
+ readDataFromPipe(inPipe, buffer, bytes);
+}
+
+// We have just processed a StartOfScene command. Read in all the scene
+// elements until we see an EndOfScene command. We allocate a new Scene
+// object to hold the scene and return a pointer to it. Don't forget to
+// delete that object when you are done with it.
+static Scene* readNewScene() {
+ unsigned char buffer[256];
+ float* floatBuffer = (float*) buffer;
+ int* intBuffer = (int*) buffer;
+ unsigned short* shortBuffer = (unsigned short*) buffer;
+
+ Scene* newScene = new Scene;
+
+ // Simulated time for this frame comes first.
+ readData(buffer, sizeof(float));
+ newScene->simTime = floatBuffer[0];
+
+ bool finished = false;
+ while (!finished) {
+ readData(buffer, 1);
+ char command = buffer[0];
+
+ switch (command) {
+
+ case Shutdown:
+ shutdown(); // doesn't return
+ break;
+
+ case EndOfScene:
+ finished = true;
+ break;
+
+ // Add a scene element that uses an already-cached mesh.
+ case AddPointMesh:
+ case AddWireframeMesh:
+ case AddSolidMesh: {
+ readData(buffer, 13*sizeof(float)+2*sizeof(short));
+ fTransform position;
+ position.updR().setRotationToBodyFixedXYZ(fVec3(floatBuffer[0], floatBuffer[1], floatBuffer[2]));
+ position.updP() = fVec3(floatBuffer[3], floatBuffer[4], floatBuffer[5]);
+ fVec3 scale = fVec3(floatBuffer[6], floatBuffer[7], floatBuffer[8]);
+ fVec4 color = fVec4(floatBuffer[9], floatBuffer[10], floatBuffer[11], floatBuffer[12]);
+ short representation = (command == AddPointMesh ? DecorativeGeometry::DrawPoints : (command == AddWireframeMesh ? DecorativeGeometry::DrawWireframe : DecorativeGeometry::DrawSurface));
+ unsigned short meshIndex = shortBuffer[13*sizeof(float)/sizeof(short)];
+ unsigned short resolution = shortBuffer[13*sizeof(float)/sizeof(short)+1];
+ RenderedMesh mesh(position, scale, color, representation, meshIndex, resolution);
+ if (command != AddSolidMesh)
+ newScene->drawnMeshes.push_back(mesh);
+ else if (color[3] == 1)
+ newScene->solidMeshes.push_back(mesh);
+ else
+ newScene->transparentMeshes.push_back(mesh);
+ if (meshIndex < NumPredefinedMeshes && (meshes[meshIndex].size() <= resolution || meshes[meshIndex][resolution] == NULL)) {
+ // A real mesh will be generated from this the next
+ // time the scene is redrawn.
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE --------
+ pendingCommands.insert(pendingCommands.begin(), new PendingStandardMesh(meshIndex, resolution));
+ pthread_mutex_unlock(&sceneLock); //------ UNLOCK SCENE --------
+ }
+ break;
+ }
+
+ case AddLine: {
+ readData(buffer, 10*sizeof(float));
+ fVec3 color = fVec3(floatBuffer[0], floatBuffer[1], floatBuffer[2]);
+ float thickness = floatBuffer[3];
+ int index;
+ int numLines = (int)newScene->lines.size();
+ for (index = 0; index < numLines && (color != newScene->lines[index].getColor() || thickness != newScene->lines[index].getThickness()); index++)
+ ;
+ if (index == numLines)
+ newScene->lines.push_back(RenderedLine(color, thickness));
+ vector<GLfloat>& line = newScene->lines[index].getLines();
+ line.push_back(floatBuffer[4]);
+ line.push_back(floatBuffer[5]);
+ line.push_back(floatBuffer[6]);
+ line.push_back(floatBuffer[7]);
+ line.push_back(floatBuffer[8]);
+ line.push_back(floatBuffer[9]);
+ break;
+ }
+
+ case AddText: {
+ readData(buffer, 9*sizeof(float)+3*sizeof(short));
+ fVec3 position = fVec3(floatBuffer[0], floatBuffer[1], floatBuffer[2]);
+ fVec3 scale = fVec3(floatBuffer[3], floatBuffer[4], floatBuffer[5]);
+ fVec3 color = fVec3(floatBuffer[6], floatBuffer[7], floatBuffer[8]);
+ unsigned short* shortp = &shortBuffer[9*sizeof(float)/sizeof(short)];
+ bool faceCamera = (shortp[0] != 0);
+ bool isScreenText = (shortp[1] != 0);
+ short length = shortp[2];
+ readData(buffer, length);
+
+ if (isScreenText)
+ newScene->screenText.push_back(
+ ScreenText(string((char*)buffer, length)));
+ else
+ newScene->sceneText.push_back(
+ RenderedText(position, scale, color, string((char*)buffer, length), faceCamera));
+ break;
+ }
+
+ case AddCoords: {
+ readData(buffer, 12*sizeof(float));
+ fRotation rotation;
+ rotation.setRotationToBodyFixedXYZ(fVec3(floatBuffer[0],
+ floatBuffer[1],
+ floatBuffer[2]));
+ fVec3 position(floatBuffer[3], floatBuffer[4], floatBuffer[5]);
+ fVec3 axisLengths(floatBuffer[6], floatBuffer[7], floatBuffer[8]);
+ fVec3 textScale = fVec3(0.2f*min(axisLengths));
+ float lineThickness = 1;
+ fVec3 color = fVec3(floatBuffer[9], floatBuffer[10], floatBuffer[11]);
+ int index;
+ int numLines = (int)newScene->lines.size();
+ for (index = 0;
+ index < numLines && (color != newScene->lines[index].getColor()
+ || newScene->lines[index].getThickness()
+ != lineThickness);
+ index++)
+ ;
+ if (index == numLines)
+ newScene->lines.push_back(RenderedLine(color, lineThickness));
+ vector<GLfloat>& line = newScene->lines[index].getLines();
+ fVec3 end = position+rotation*fVec3(axisLengths[0], 0, 0);
+ line.push_back(position[0]);
+ line.push_back(position[1]);
+ line.push_back(position[2]);
+ line.push_back(end[0]);
+ line.push_back(end[1]);
+ line.push_back(end[2]);
+ newScene->sceneText.push_back(RenderedText(end, textScale, color, "X"));
+ end = position+rotation*fVec3(0, axisLengths[1], 0);
+ line.push_back(position[0]);
+ line.push_back(position[1]);
+ line.push_back(position[2]);
+ line.push_back(end[0]);
+ line.push_back(end[1]);
+ line.push_back(end[2]);
+ newScene->sceneText.push_back(RenderedText(end, textScale, color, "Y"));
+ end = position+rotation*fVec3(0, 0, axisLengths[2]);
+ line.push_back(position[0]);
+ line.push_back(position[1]);
+ line.push_back(position[2]);
+ line.push_back(end[0]);
+ line.push_back(end[1]);
+ line.push_back(end[2]);
+ newScene->sceneText.push_back(RenderedText(end, textScale, color, "Z"));
+ break;
+ }
+
+ // Define a new mesh that will be assigned the next available mesh
+ // index. It will be cached here and then can be referenced in this
+ // scene and others by using it mesh index.
+ case DefineMesh: {
+ readData(buffer, 2*sizeof(short));
+ PendingMesh* mesh = new PendingMesh(); // assigns next mesh index
+ int numVertices = shortBuffer[0];
+ int numFaces = shortBuffer[1];
+ mesh->vertices.resize(3*numVertices, 0);
+ mesh->normals.resize(3*numVertices);
+ mesh->faces.resize(3*numFaces);
+ readData((unsigned char*)&mesh->vertices[0], (int)(mesh->vertices.size()*sizeof(float)));
+ readData((unsigned char*)&mesh->faces[0], (int)(mesh->faces.size()*sizeof(short)));
+
+ // Compute normal vectors for the mesh.
+
+ vector<fVec3> normals(numVertices, fVec3(0));
+ for (int i = 0; i < numFaces; i++) {
+ int v1 = mesh->faces[3*i];
+ int v2 = mesh->faces[3*i+1];
+ int v3 = mesh->faces[3*i+2];
+ fVec3 vert1(mesh->vertices[3*v1], mesh->vertices[3*v1+1], mesh->vertices[3*v1+2]);
+ fVec3 vert2(mesh->vertices[3*v2], mesh->vertices[3*v2+1], mesh->vertices[3*v2+2]);
+ fVec3 vert3(mesh->vertices[3*v3], mesh->vertices[3*v3+1], mesh->vertices[3*v3+2]);
+ fVec3 norm = (vert2-vert1)%(vert3-vert1);
+ float length = norm.norm();
+ if (length > 0) {
+ norm /= length;
+ normals[v1] += norm;
+ normals[v2] += norm;
+ normals[v3] += norm;
+ }
+ }
+ for (int i = 0; i < numVertices; i++) {
+ normals[i] = normals[i].normalize();
+ mesh->normals[3*i] = normals[i][0];
+ mesh->normals[3*i+1] = normals[i][1];
+ mesh->normals[3*i+2] = normals[i][2];
+ }
+
+ // A real mesh will be generated from this the next
+ // time the scene is redrawn.
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE --------
+ pendingCommands.insert(pendingCommands.begin(), mesh);
+ pthread_mutex_unlock(&sceneLock); //------ UNLOCK SCENE --------
+ break;
+ }
+
+ default:
+ SimTK_ASSERT_ALWAYS(false, "Unexpected scene data sent to visualizer");
+ }
+ }
+
+ return newScene;
+}
+
+// This is the main program for the listener thread. It reads continuously
+// from the input pipe, which contains data from the simulator's calls
+// to a Visualizer object. Any changes to the scene must wait until the
+// rendering thread has finished with the current frame. The listener
+// thread must not make any gl or glut calls; any operations that require
+// those are queued for later handing in the rendering thread.
+void* listenForInput(void* args) {
+ unsigned char buffer[256];
+ float* floatBuffer = (float*) buffer;
+ int* intBuffer = (int*) buffer;
+ unsigned short* shortBuffer = (unsigned short*) buffer;
+
+ try
+ { while (true) {
+ bool issuedActiveRedisplay = false;
+ // Read commands from the simulator.
+ readData(buffer, 1);
+ switch (buffer[0]) {
+ case DefineMenu: {
+ readData(buffer, sizeof(short));
+ int titleLength = shortBuffer[0];
+ vector<char> titleBuffer(titleLength);
+ readData((unsigned char*)&titleBuffer[0], titleLength);
+ string title(&titleBuffer[0], titleLength);
+ readData(buffer, sizeof(int));
+ const int menuId = intBuffer[0];
+ readData(buffer, sizeof(short));
+ int numItems = shortBuffer[0];
+ vector<pair<string, int> > items(numItems);
+ for (int index = 0; index < numItems; index++) {
+ readData(buffer, 2*sizeof(int));
+ items[index].second = intBuffer[0];
+ vector<char> textBuffer(intBuffer[1]);
+ readData((unsigned char*)&textBuffer[0], intBuffer[1]);
+ items[index].first = string(&textBuffer[0], intBuffer[1]);
+ }
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ menus.push_back(Menu(title, menuId, items, menuSelected));
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ break;
+ }
+ case DefineSlider: {
+ readData(buffer, sizeof(short));
+ int titleLength = shortBuffer[0];
+ vector<char> titleBuffer(titleLength);
+ readData((unsigned char*)&titleBuffer[0], titleLength);
+ string title(&titleBuffer[0], titleLength);
+ readData(buffer, sizeof(int)+3*sizeof(float));
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ sliders.push_back(Slider(title, intBuffer[0], floatBuffer[1], floatBuffer[2], floatBuffer[3]));
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ break;
+ }
+ case SetSliderValue: {
+ readData(buffer, sizeof(int));
+ const int sliderId = intBuffer[0];
+ readData(buffer, sizeof(float));
+ const float newValue = floatBuffer[0];
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ Slider* sp = findSliderById(sliderId);
+ if (sp) sp->changeValue(newValue);
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ break;
+ }
+ case SetSliderRange: {
+ readData(buffer, sizeof(int));
+ const int sliderId = intBuffer[0];
+ readData(buffer, 2*sizeof(float));
+ const float newMin = floatBuffer[0];
+ const float newMax = floatBuffer[1];
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ Slider* sp = findSliderById(sliderId);
+ if (sp) sp->changeRange(newMin, newMax);
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ break;
+ }
+ case SetCamera: {
+ readData(buffer, 6*sizeof(float));
+ fVec3 R(floatBuffer[0], floatBuffer[1], floatBuffer[2]);
+ fVec3 p(floatBuffer[3], floatBuffer[4], floatBuffer[5]);
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ pendingCommands.push_back(new PendingSetCameraTransform(R, p));
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ break;
+ }
+ case ZoomCamera: {
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ pendingCommands.push_back(new PendingCameraZoom());
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ break;
+ }
+ case LookAt: {
+ readData(buffer, 6*sizeof(float));
+ fVec3 point(floatBuffer[0], floatBuffer[1], floatBuffer[2]);
+ fVec3 updir(floatBuffer[3], floatBuffer[4], floatBuffer[5]);
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ fVec3 pt2camera = X_GC.p()-point;
+ if (pt2camera.normSqr() < square(1e-6))
+ pt2camera = fVec3(X_GC.z()); // leave unchanged
+ X_GC.updR().setRotationFromTwoAxes(fUnitVec3(pt2camera), ZAxis,
+ updir, YAxis);
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ break;
+ }
+ case SetFieldOfView: {
+ readData(buffer, sizeof(float));
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ fieldOfView = floatBuffer[0];
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ break;
+ }
+ case SetClipPlanes: {
+ readData(buffer, 2*sizeof(float));
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ nearClip = floatBuffer[0];
+ farClip = floatBuffer[1];
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ break;
+ }
+ case SetSystemUpDirection: {
+ readData(buffer, 2);
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ groundNormal = CoordinateDirection( CoordinateAxis((int)buffer[0]),
+ (int)(signed char)buffer[1] );
+ X_GC.updR().setRotationFromTwoAxes
+ (groundNormal, YAxis, X_GC.z(), ZAxis); // attempt to keep z
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ break;
+ }
+ case SetGroundHeight: {
+ readData(buffer, sizeof(float));
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ groundHeight = floatBuffer[0];
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ break;
+ }
+ case SetWindowTitle: {
+ readData(buffer, sizeof(short));
+ int titleLength = shortBuffer[0];
+ vector<char> titleBuffer(titleLength);
+ readData((unsigned char*)&titleBuffer[0], titleLength);
+ string title(&titleBuffer[0], titleLength);
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ pendingCommands.push_back(new PendingWindowTitleChange(title));
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ break;
+ }
+ case SetMaxFrameRate: {
+ readData(buffer, sizeof(float));
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ maxFrameRate = floatBuffer[0];
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ break;
+ }
+ case SetBackgroundColor: {
+ readData(buffer, 3*sizeof(float));
+ fVec3 color(floatBuffer[0], floatBuffer[1], floatBuffer[2]);
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ pendingCommands.push_back(new PendingBackgroundColorChange(color));
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ break;
+ }
+ case SetShowShadows: {
+ readData(buffer, sizeof(short));
+ const bool shouldShow = (shortBuffer[0] != 0);
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ showShadows = shouldShow;
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ break;
+ }
+ case SetBackgroundType: {
+ readData(buffer, sizeof(short));
+ const Visualizer::BackgroundType type =
+ Visualizer::BackgroundType(shortBuffer[0]);
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ showGround = (type == Visualizer::GroundAndSky);
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ break;
+ }
+ case SetShowFrameRate: {
+ readData(buffer, sizeof(short));
+ const bool shouldShow = (shortBuffer[0] != 0);
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ showFPS = shouldShow;
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ break;
+ }
+ case SetShowSimTime: {
+ readData(buffer, sizeof(short));
+ const bool shouldShow = (shortBuffer[0] != 0);
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ showSimTime = shouldShow;
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ break;
+ }
+ case SetShowFrameNumber: {
+ readData(buffer, sizeof(short));
+ const bool shouldShow = (shortBuffer[0] != 0);
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ showFrameNum = shouldShow;
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ break;
+ }
+ case StartOfScene: {
+ Scene* newScene = readNewScene();
+ pthread_mutex_lock(&sceneLock); //------- LOCK SCENE ---------
+ if (scene != NULL) {
+ while (!scene->sceneHasBeenDrawn) {
+ // -------- WAIT FOR CONDITION --------
+ // Give up the lock and wait for notice.
+ pthread_cond_wait(&sceneHasBeenDrawn,
+ &sceneLock);
+ // Now we hold the lock again, but this might
+ // be a spurious wakeup so check again.
+ }
+ // Previous scene has been drawn.
+ delete scene; scene = 0;
+ }
+ // Swap in the new scene.
+ scene = newScene;
+ saveNextFrameToMovie = savingMovie;
+ pthread_mutex_unlock(&sceneLock); //------- UNLOCK SCENE -------
+ forceActiveRedisplay(); //------- ACTIVE REDISPLAY ---
+ issuedActiveRedisplay = true;
+ break;
+ }
+
+ case Shutdown:
+ shutdown(); // doesn't return
+ break;
+
+ default:
+ SimTK_ERRCHK1_ALWAYS(!"unrecognized command", "listenForInput()",
+ "Unexpected command %u received from simbody-visualizer. Can't continue.",
+ (unsigned)buffer[0]);
+ }
+
+ // Do this after every received command.
+ if (!issuedActiveRedisplay)
+ requestPassiveRedisplay(); //------- PASSIVE REDISPLAY --
+ }
+ } catch (const std::exception& e) {
+ std::cout << "simbody-visualizer listenerThread: unrecoverable error:\n";
+ std::cout << e.what() << std::endl;
+ return (void*)1;
+ }
+ return (void*)0;
+}
+
+static void dumpAboutMessageToConsole() {
+ printf("\n\n=================== ABOUT SIMBODY VISUALIZER ===================\n");
+ printf("Simbody(tm) %s visualizer (protocol rev. %u)\n",
+ simbodyVersionStr.c_str(), ProtocolVersion);
+ printf("\nName of invoking executable: %s\n", simulatorExecutableName.c_str());
+ printf( "Current working directory:\n %s\n",
+ Pathname::getCurrentWorkingDirectory().c_str());
+ printf( "simbody-visualizer executable:\n %s\n",
+ Pathname::getThisExecutablePath().c_str());
+ printf( "Current window size: %d X %d\n", viewWidth, viewHeight);
+ printf("\nGL version: %s\n", glGetString(GL_VERSION));
+ printf( "GLSL version: %s\n", glGetString(GL_SHADING_LANGUAGE_VERSION));
+ printf( "GL renderer: %s\n", glGetString(GL_RENDERER));
+ printf( "GL vendor: %s\n", glGetString(GL_VENDOR));
+ printf("\nVisualizer authors: Peter Eastman, Michael Sherman\n");
+ printf( "Support: Simbios, Stanford Bioengineering, NIH U54 GM072970\n");
+ printf( "https://simtk.org/home/simbody\n");
+ printf("================================================================\n\n");
+}
+
+static const int MENU_VIEW_FRONT = 0;
+static const int MENU_VIEW_BACK = 1;
+static const int MENU_VIEW_LEFT = 2;
+static const int MENU_VIEW_RIGHT = 3;
+static const int MENU_VIEW_TOP = 4;
+static const int MENU_VIEW_BOTTOM = 5;
+
+static const int MENU_BACKGROUND_BLACK = 6;
+static const int MENU_BACKGROUND_WHITE = 7;
+static const int MENU_BACKGROUND_SKY = 8;
+
+static const int MENU_SHOW_SHADOWS = 9;
+static const int MENU_SHOW_FPS = 10;
+static const int MENU_SHOW_SIM_TIME = 11;
+static const int MENU_SHOW_FRAME_NUM = 12;
+
+static const int MENU_SAVE_IMAGE = 13;
+static const int MENU_SAVE_MOVIE = 14;
+
+static const int MENU_ABOUT = 15;
+
+// This is the handler for our built-in "View" pull down menu.
+void viewMenuSelected(int option) {
+ fRotation groundRotation;
+ if (groundNormal.getAxis() == XAxis)
+ groundRotation.setRotationFromTwoAxes(fUnitVec3(0, 1, 0), ZAxis, fVec3(1, 0, 0), YAxis);
+ else if (groundNormal.getAxis() == ZAxis)
+ groundRotation.setRotationFromTwoAxes(fUnitVec3(1, 0, 0), ZAxis, fVec3(0, 0, 1), YAxis);
+
+ // Flip the camera 180 degress around one of the other axes if in a negative direction.
+ if (groundNormal.getDirection() == -1)
+ groundRotation = fRotation((float)Pi, groundNormal.getAxis().getNextAxis()) * groundRotation;
+
+ switch (option) {
+ case MENU_VIEW_FRONT:
+ X_GC.updR().setRotationToIdentityMatrix();
+ X_GC.updR() = groundRotation*X_GC.R();
+ zoomCameraToShowWholeScene();
+ break;
+ case MENU_VIEW_BACK:
+ X_GC.updR().setRotationFromAngleAboutY((float)SimTK_PI);
+ X_GC.updR() = groundRotation*X_GC.R();
+ zoomCameraToShowWholeScene();
+ break;
+ case MENU_VIEW_LEFT:
+ X_GC.updR().setRotationFromAngleAboutY(-(float)(SimTK_PI/2));
+ X_GC.updR() = groundRotation*X_GC.R();
+ zoomCameraToShowWholeScene();
+ break;
+ case MENU_VIEW_RIGHT:
+ X_GC.updR().setRotationFromAngleAboutY((float)(SimTK_PI/2));
+ X_GC.updR() = groundRotation*X_GC.R();
+ zoomCameraToShowWholeScene();
+ break;
+ case MENU_VIEW_TOP:
+ X_GC.updR().setRotationFromAngleAboutX(-(float)(SimTK_PI/2));
+ X_GC.updR() = groundRotation*X_GC.R();
+ zoomCameraToShowWholeScene();
+ break;
+ case MENU_VIEW_BOTTOM:
+ X_GC.updR().setRotationFromAngleAboutX((float)(SimTK_PI/2));
+ X_GC.updR() = groundRotation*X_GC.R();
+ zoomCameraToShowWholeScene();
+ break;
+ case MENU_BACKGROUND_BLACK:
+ showGround = false;
+ backgroundColor = fVec3(0,0,0);
+ setClearColorToBackgroundColor();
+ break;
+ case MENU_BACKGROUND_WHITE:
+ showGround = false;
+ backgroundColor = fVec3(1,1,1);
+ setClearColorToBackgroundColor();
+ break;
+ case MENU_BACKGROUND_SKY:
+ showGround = true;
+ backgroundColor = fVec3(1,1,1);
+ setClearColorToBackgroundColor();
+ break;
+ case MENU_SHOW_SHADOWS:
+ showShadows = !showShadows;
+ break;
+ case MENU_SHOW_FPS:
+ showFPS = !showFPS;
+ break;
+ case MENU_SHOW_SIM_TIME:
+ showSimTime = !showSimTime;
+ break;
+ case MENU_SHOW_FRAME_NUM:
+ showFrameNum = !showFrameNum;
+ break;
+ case MENU_SAVE_IMAGE:
+ if (canSaveImages) {
+ saveImage();
+ } else
+ setOverlayMessage(
+ "Sorry -- image capture not available due to your\n"
+ "backlevel OpenGL. At least OpenGL 2.0 is required.\n"
+ "See the About message for level information.");
+ break;
+ case MENU_SAVE_MOVIE:
+ if (canSaveImages) {
+ if (savingMovie) {
+ savingMovie = false;
+ setOverlayMessage("Frame capture off.");
+ } else
+ saveMovie();
+ } else
+ setOverlayMessage(
+ "Sorry -- movie capture not available due to your\n"
+ "backlevel OpenGL. At least OpenGL 2.0 is required.\n"
+ "See the About message for level information.");
+ break;
+ case MENU_ABOUT:
+ dumpAboutMessageToConsole();
+ setOverlayMessage("About: see console window");
+ break;
+ }
+
+ requestPassiveRedisplay(); //----- PASSIVE REDISPLAY ----
+}
+
+static const int DefaultWindowWidth = 800;
+static const int DefaultWindowHeight = 600;
+
+
+// The glut callback for chaning window size.
+static void changeSize(int width, int height) {
+ if (height == 0)
+ height = 1;
+ viewWidth = width;
+ viewHeight = height;
+}
+
+
+// This seems to be necessary on Mac and Linux where the built-in glut
+// idle hangs if there is no activity in the gl window. In any case we
+// use it to issue a redisplay when one is needed but no scene is forthcoming,
+// and for final cleanup of the FPS display after scenes stop arriving.
+static void keepAliveIdleFunc() {
+ static const double SleepTime = 1./100; // 10ms
+ static const double MopUpTime = 1.; // to clean up FPS display
+
+ const double passiveRedisplayInterval = 1/(double)maxFrameRate;
+
+ sleepInSec(SleepTime); // take a short break
+
+ // If it has been at least one passive frame time, redisplay if there
+ // has been a passive redisplay request.
+ const double idle = realTime() - lastRedisplayDone;
+ if (passiveRedisplayRequested && idle > passiveRedisplayInterval) {
+ forcePassiveRedisplay(); //------ FORCE REDISPLAY -----
+ return;
+ }
+
+ // If it has been so long since the last frame that we need to mop up
+ // (currently that just means update the FPS display), then do that.
+ // We want to calculate the last real FPS value, and then show a zero on
+ // the subsequent update, and then stop updating.
+ if (numMopUpDisplaysSinceLastRedisplay < 2 && idle >= MopUpTime) {
+ if (numMopUpDisplaysSinceLastRedisplay)
+ fpsCounter = 0; // clear the 1 frame we generated after 1st update
+ forceMopUpRedisplay(); //------ FORCE REDISPLAY -----
+ }
+
+}
+
+static void setKeepAlive(bool enable) {
+ if (enable) glutIdleFunc(keepAliveIdleFunc);
+ else glutIdleFunc(0);
+}
+
+static void setVsync(bool enable) {
+#ifdef _WIN32
+ // I don't know how to disable vsync on Mac or Linux.
+ if( wglSwapIntervalEXT )
+ wglSwapIntervalEXT(enable?1:0); // 1==vsync; 0 is off
+#endif
+}
+
+
+// This is executed from the main thread at startup.
+static void shakeHandsWithSimulator(int fromSimPipe, int toSimPipe) {
+ unsigned char handshakeCommand;
+ readDataFromPipe(fromSimPipe, &handshakeCommand, 1);
+ SimTK_ERRCHK2_ALWAYS(handshakeCommand == StartupHandshake,
+ "simbody-visualizer::shakeHandsWithSimulator()",
+ "Expected initial handshake command %u but received %u. Can't continue.",
+ (unsigned)StartupHandshake, (unsigned)handshakeCommand);
+
+ unsigned SimVersion;
+ readDataFromPipe(fromSimPipe, (unsigned char*)&SimVersion, sizeof(unsigned int));
+ SimTK_ERRCHK2_ALWAYS(SimVersion == ProtocolVersion,
+ "simbody-visualizer::shakeHandsWithSimulator()",
+ "The Simbody Visualizer class protocol version %u is not compatible with "
+ " simbody-visualizer protocol %u; this may be an installation problem."
+ " Can't continue.",
+ SimVersion, ProtocolVersion);
+
+ // Get Simbody version number as major,minor,patch
+ readDataFromPipe(fromSimPipe, (unsigned char*)simbodyVersion, 3*sizeof(int));
+ simbodyVersionStr = String(simbodyVersion[0]) + "." + String(simbodyVersion[1]);
+ if (simbodyVersion[2]) simbodyVersionStr += "." + String(simbodyVersion[2]);
+
+ unsigned exeNameLength;
+ char exeNameBuf[256]; // just a file name, not a path name
+ readDataFromPipe(fromSimPipe, (unsigned char*)&exeNameLength, sizeof(unsigned));
+ SimTK_ASSERT_ALWAYS(exeNameLength <= 255,
+ "simbody-visualizer: executable name length violates protocol.");
+ readDataFromPipe(fromSimPipe, (unsigned char*)exeNameBuf, exeNameLength);
+ exeNameBuf[exeNameLength] = (char)0;
+
+ simulatorExecutableName = std::string(exeNameBuf, exeNameLength);
+
+ WRITE(outPipe, &ReturnHandshake, 1);
+ WRITE(outPipe, &ProtocolVersion, sizeof(unsigned));
+}
+
+// Received Shutdown message from simulator. Die immediately.
+static void shutdown() {
+ printf("\nsimbody-visualizer: received Shutdown message. Goodbye.\n");
+ exit(0);
+}
+
+int main(int argc, char** argv) {
+ try
+ { bool talkingToSimulator = false;
+
+ if (argc >= 3) {
+ stringstream(argv[1]) >> inPipe;
+ stringstream(argv[2]) >> outPipe;
+ talkingToSimulator = true; // presumably those were the pipes
+ } else {
+ printf("\n**** VISUALIZER HAS NO SIMULATOR TO TALK TO ****\n");
+ printf("The simbody-visualizer was invoked directly with no simulator\n");
+ printf("process to talk to. Will attempt to bring up the display anyway\n");
+ printf("in case you want to look at the About message.\n");
+ printf("The simbody-visualizer is intended to be invoked programmatically.\n");
+ }
+
+
+ // Initialize GLUT, then perform initial handshake with the parent
+ // from the main thread here.
+ glutInit(&argc, argv);
+
+ if (talkingToSimulator)
+ shakeHandsWithSimulator(inPipe, outPipe);
+ else {
+ simbodyVersionStr = "?.?.?";
+ simulatorExecutableName = "No simulator";
+ }
+
+ // Construct the default initial title.
+ string title = "Simbody " + simbodyVersionStr + ": " + simulatorExecutableName;
+
+ // Put the upper left corner of the glut window near the upper right
+ // corner of the screen.
+ int screenW = glutGet(GLUT_SCREEN_WIDTH);
+ int screenH = glutGet(GLUT_SCREEN_HEIGHT);
+ int windowPosX = (screenW - DefaultWindowWidth) - 50; // 50 pixel margin
+ int windowPosY = 50;
+
+ glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGBA);
+ glutInitWindowPosition(windowPosX,windowPosY);
+ glutInitWindowSize(DefaultWindowWidth, DefaultWindowHeight);
+ glutCreateWindow(title.c_str());
+
+
+ // Set up callback funtions.
+ glutDisplayFunc(redrawDisplay);
+ glutReshapeFunc(changeSize);
+ glutMouseFunc(mouseButtonPressedOrReleased);
+ glutMotionFunc(mouseDragged);
+ glutPassiveMotionFunc(mouseMoved);
+ glutKeyboardFunc(ordinaryKeyPressed);
+ glutSpecialFunc(specialKeyPressed);
+
+ //dumpAboutMessageToConsole();
+
+ // On some systems (Windows at least), some of the gl functions may
+ // need to be loaded dynamically.
+ bool canFunction = initGlextFuncPointersIfNeeded(canSaveImages);
+ if (!canFunction) {
+ printf("\n\n**** FATAL ERROR ****\n");
+ dumpAboutMessageToConsole();
+ printf("\n**** FATAL ERROR ****\n");
+ printf("Sorry, Simbody Visualizer can't function with this ancient version\n");
+ printf("of OpenGL. See the message above for version information.\n");
+ printf("Are you using an emulation through a remote desktop or\n");
+ printf("virtual machine?\n");
+ printf("**** FATAL ERROR **** Simbody Visualizer terminating.\n");
+ return 1;
+ }
+
+ setVsync(true);
+
+ // Set up lighting.
+
+ GLfloat light_diffuse[] = {0.8f, 0.8f, 0.8f, 1.0f};
+ GLfloat light_position[] = {1.0f, 1.0f, 1.0f, 0.0f};
+ GLfloat light_ambient[] = {0.2f, 0.2f, 0.2f, 1.0f};
+ glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
+ glLightfv(GL_LIGHT0, GL_POSITION, light_position);
+ glLightModelfv(GL_LIGHT_MODEL_AMBIENT, light_ambient);
+ glClearColor(1, 1, 1, 1);
+ glEnable(GL_LIGHT0);
+
+ // Initialize miscellaneous OpenGL state.
+
+ glEnableClientState(GL_VERTEX_ARRAY);
+ glEnableClientState(GL_NORMAL_ARRAY);
+ glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+ glEnable(GL_CULL_FACE);
+ glEnable(GL_NORMALIZE);
+
+ // Make room for the predefined meshes.
+ meshes.resize(NumPredefinedMeshes);
+ // Note the first mesh index available for unique meshes
+ // that are sent to the GUI for caching.
+ nextMeshIndex = NumPredefinedMeshes;
+
+ scene = NULL;
+ pendingCommands.push_back(new PendingCameraZoom());
+
+ vector<pair<string, int> > items;
+ items.push_back(make_pair("View Direction/Front", MENU_VIEW_FRONT));
+ items.push_back(make_pair("View Direction/Back", MENU_VIEW_BACK));
+ items.push_back(make_pair("View Direction/Left", MENU_VIEW_LEFT));
+ items.push_back(make_pair("View Direction/Right", MENU_VIEW_RIGHT));
+ items.push_back(make_pair("View Direction/Top", MENU_VIEW_TOP));
+ items.push_back(make_pair("View Direction/Bottom", MENU_VIEW_BOTTOM));
+ items.push_back(make_pair("Background/Black", MENU_BACKGROUND_BLACK));
+ items.push_back(make_pair("Background/White", MENU_BACKGROUND_WHITE));
+ items.push_back(make_pair("Background/Ground and Sky", MENU_BACKGROUND_SKY));
+ items.push_back(make_pair("Show//Hide/Shadows", MENU_SHOW_SHADOWS));
+ items.push_back(make_pair("Show//Hide/Frame Rate", MENU_SHOW_FPS));
+ items.push_back(make_pair("Show//Hide/Sim Time", MENU_SHOW_SIM_TIME));
+ items.push_back(make_pair("Show//Hide/Frame #", MENU_SHOW_FRAME_NUM));
+ items.push_back(make_pair("Save Image", MENU_SAVE_IMAGE));
+ items.push_back(make_pair("Save Movie", MENU_SAVE_MOVIE));
+ items.push_back(make_pair("About (to console)", MENU_ABOUT));
+ menus.push_back(Menu("View", Visualizer::ViewMenuId, items, viewMenuSelected));
+
+ // Initialize pthread lock and condition variable.
+ pthread_mutex_init(&sceneLock, NULL);
+ pthread_cond_init(&sceneHasBeenDrawn, NULL);
+
+ // Spawn the listener thread. After this it runs independently.
+ if (talkingToSimulator) {
+ pthread_t thread;
+ pthread_create(&thread, NULL, listenForInput, NULL);
+ } else {
+ scene = new Scene;
+ }
+
+ // Avoid hangs on Mac & Linux; posts orphan redisplays on all platforms.
+ setKeepAlive(true);
+
+ // Enter the main loop. If there is nothing else to do we'll check if
+ // someone was hoping for a redisplay and issue one.
+ requestPassiveRedisplay(); //------ PASSIVE REDISPLAY ---
+ fpsBaseTime = realTime();
+ glutMainLoop();
+ } catch(const std::exception& e) {
+ std::cout << "simbody-visualizer failed with exception:\n"
+ << e.what() << std::endl;
+ return 1;
+ }
+
+ return 0;
+}
+
+
+// Initialize function pointers for Windows GL extensions.
+static bool initGlextFuncPointersIfNeeded(bool& glCanSaveImages) {
+ glCanSaveImages = true;
+#ifdef _WIN32
+ wglSwapIntervalEXT = (PFNWGLSWAPINTERVALFARPROC)wglGetProcAddress( "wglSwapIntervalEXT" );
+
+ // These are necessary for basic functioning.
+ glGenBuffers = (PFNGLGENBUFFERSPROC) glutGetProcAddress("glGenBuffers");
+ glBindBuffer = (PFNGLBINDBUFFERPROC) glutGetProcAddress("glBindBuffer");
+ glBufferData = (PFNGLBUFFERDATAPROC) glutGetProcAddress("glBufferData");
+ glActiveTexture = (PFNGLACTIVETEXTUREPROC) glutGetProcAddress("glActiveTexture");
+
+ if (!(glGenBuffers && glBindBuffer && glBufferData && glActiveTexture))
+ return false; // fatal error
+
+ // These are needed only when saving images or movies so the Visualizer can
+ // function without them.
+
+ // Using the "EXT" names here means we only require OpenGL 2.0.
+ glGenFramebuffersEXT = (PFNGLGENFRAMEBUFFERSEXTPROC) glutGetProcAddress("glGenFramebuffersEXT");
+ glGenRenderbuffersEXT = (PFNGLGENRENDERBUFFERSEXTPROC) glutGetProcAddress("glGenRenderbuffersEXT");
+ glBindFramebufferEXT = (PFNGLBINDFRAMEBUFFEREXTPROC) glutGetProcAddress("glBindFramebufferEXT");
+ glBindRenderbufferEXT = (PFNGLBINDRENDERBUFFEREXTPROC) glutGetProcAddress("glBindRenderbufferEXT");
+ glRenderbufferStorageEXT = (PFNGLRENDERBUFFERSTORAGEEXTPROC) glutGetProcAddress("glRenderbufferStorageEXT");
+ glFramebufferRenderbufferEXT = (PFNGLFRAMEBUFFERRENDERBUFFEREXTPROC) glutGetProcAddress("glFramebufferRenderbufferEXT");
+ glDeleteRenderbuffersEXT = (PFNGLDELETERENDERBUFFERSEXTPROC) glutGetProcAddress("glDeleteRenderbuffersEXT");
+ glDeleteFramebuffersEXT = (PFNGLDELETEFRAMEBUFFERSEXTPROC) glutGetProcAddress("glDeleteFramebuffersEXT");
+
+ if (! (glGenFramebuffersEXT && glGenRenderbuffersEXT && glBindFramebufferEXT
+ && glBindRenderbufferEXT && glRenderbufferStorageEXT && glFramebufferRenderbufferEXT
+ && glDeleteRenderbuffersEXT && glDeleteFramebuffersEXT))
+ glCanSaveImages = false;
+#endif
+
+ return true;
+}
+
diff --git a/Simbody/Visualizer/src/Visualizer.cpp b/Simbody/Visualizer/src/Visualizer.cpp
new file mode 100644
index 0000000..9a9588d
--- /dev/null
+++ b/Simbody/Visualizer/src/Visualizer.cpp
@@ -0,0 +1,1153 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "simbody/internal/common.h"
+#include "simbody/internal/MultibodySystem.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "simbody/internal/Visualizer.h"
+#include "simbody/internal/Visualizer_InputListener.h"
+
+#include "VisualizerGeometry.h"
+#include "VisualizerProtocol.h"
+
+#include <cstdlib>
+#include <cstdio>
+#include <pthread.h>
+#include <string>
+#include <ctime>
+#include <iostream>
+#include <limits>
+
+using namespace SimTK;
+using namespace std;
+
+static void* drawingThreadMain(void* visualizerAsVoidp);
+
+static const long long UsToNs = 1000LL; // ns = us * UsToNs
+static const long long MsToNs = 1000LL * UsToNs; // ns = ms * MsToNs
+
+static const Real DefaultFrameRateFPS = 30;
+static const Real DefaultDesiredBufferLengthInSec = Real(0.15); // 150ms
+
+// These are not currently overrideable.
+static const long long DefaultAllowableFrameJitterInNs = 5 * MsToNs; //5ms
+static const Real DefaultSlopAsFractionOfFrameInterval = Real(0.05); //5%
+
+namespace { // local classes
+//==============================================================================
+// VISUALIZER IMPL
+//==============================================================================
+/* This is the private implementation object contained in a Visualizer handle.
+See the "implementation notes" section of the Visualizer class documentation
+for some information about how this works. */
+
+// If we are buffering frames, this is the object that represents a frame
+// in the queue. It consists of a copy of a reported State, and a desired
+// draw time for the frame, in adjusted real time (AdjRT).
+struct Frame {
+ Frame() : desiredDrawTimeAdjRT(-1LL) {}
+ Frame(const State& state, const long long& desiredDrawTimeAdjRT)
+ : state(state), desiredDrawTimeAdjRT(desiredDrawTimeAdjRT) {}
+ // default copy constructor, copy assignment, destructor
+
+ void clear() {desiredDrawTimeAdjRT = -1LL;}
+ bool isValid() const {return desiredDrawTimeAdjRT >= 0LL;}
+
+ State state;
+ long long desiredDrawTimeAdjRT; // in adjusted real time
+};
+
+// This holds the specs for rubber band lines that are added directly
+// to the Visualizer.
+class RubberBandLine {
+public:
+ RubberBandLine(MobilizedBodyIndex b1, const Vec3& station1,
+ MobilizedBodyIndex b2, const Vec3& station2,
+ const DecorativeLine& line)
+ : b1(b1), station1(station1), b2(b2), station2(station2), line(line) {}
+
+ MobilizedBodyIndex b1, b2;
+ Vec3 station1, station2;
+ DecorativeLine line;
+};
+} // end local namespace
+
+// Implementation of the Visualizer.
+class Visualizer::Impl {
+public:
+ // Create a Visualizer and put it in PassThrough mode.
+ Impl(Visualizer* owner, const MultibodySystem& system,
+ const Array_<String>& searchPath)
+ : m_system(system), m_protocol(*owner, searchPath),
+ m_shutdownWhenDestructed(false), m_upDirection(YAxis), m_groundHeight(0),
+ m_mode(PassThrough), m_frameRateFPS(DefaultFrameRateFPS),
+ m_simTimeUnitsPerSec(1),
+ m_desiredBufferLengthInSec(DefaultDesiredBufferLengthInSec),
+ m_timeBetweenFramesInNs(secToNs(1/DefaultFrameRateFPS)),
+ m_allowableFrameJitterInNs(DefaultAllowableFrameJitterInNs),
+ m_allowableFrameTimeSlopInNs(
+ secToNs(DefaultSlopAsFractionOfFrameInterval/DefaultFrameRateFPS)),
+ m_adjustedRealTimeBase(realTimeInNs()),
+ m_prevFrameSimTime(-1), m_nextFrameDueAdjRT(-1),
+ m_oldest(0),m_nframe(0),
+ m_drawThreadIsRunning(false), m_drawThreadShouldSuicide(false),
+ m_refCount(0)
+ {
+ pthread_mutex_init(&m_queueLock, NULL);
+ pthread_cond_init(&m_queueNotFull, NULL);
+ pthread_cond_init(&m_queueNotEmpty, NULL);
+ pthread_cond_init(&m_queueIsEmpty, NULL);
+
+ setMode(PassThrough);
+ clearStats();
+
+ m_protocol.setMaxFrameRate(m_frameRateFPS);
+ m_protocol.setBackgroundColor(White);
+ m_protocol.setBackgroundType(system.getUseUniformBackground()
+ ? SolidColor : GroundAndSky);
+ m_protocol.setSystemUpDirection(system.getUpDirection());
+ }
+
+ ~Impl() {
+ if (m_mode==RealTime && m_pool.size()) {
+ pthread_cancel(m_drawThread);
+ }
+ for (unsigned i = 0; i < m_controllers.size(); i++)
+ delete m_controllers[i];
+ for (unsigned i = 0; i < m_listeners.size(); i++)
+ delete m_listeners[i];
+ for (unsigned i = 0; i < m_generators.size(); i++)
+ delete m_generators[i];
+ pthread_cond_destroy(&m_queueIsEmpty);
+ pthread_cond_destroy(&m_queueNotEmpty);
+ pthread_cond_destroy(&m_queueNotFull);
+ pthread_mutex_destroy(&m_queueLock);
+
+ if (m_shutdownWhenDestructed)
+ m_protocol.shutdownGUI();
+ }
+
+ void setShutdownWhenDestructed(bool shouldShutdown)
+ { m_shutdownWhenDestructed = shouldShutdown; }
+
+ bool getShutdownWhenDestructed() const
+ { return m_shutdownWhenDestructed; }
+
+ // Call from simulation thread.
+ void startDrawThread() {
+ SimTK_ASSERT_ALWAYS(!m_drawThreadIsRunning,
+ "Tried to start the draw thread when it was already running.");
+ m_drawThreadShouldSuicide = false;
+ // Make sure the thread is joinable, although that is probably
+ // the default.
+ pthread_attr_t attr;
+ pthread_attr_init(&attr);
+ pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
+ pthread_create(&m_drawThread, &attr, drawingThreadMain, this);
+ pthread_attr_destroy(&attr);
+ m_drawThreadIsRunning = true;
+ }
+
+ // Call from simulation thread.
+ void killDrawThread() {
+ SimTK_ASSERT_ALWAYS(m_drawThreadIsRunning,
+ "Tried to kill the draw thread when it wasn't running.");
+ m_drawThreadShouldSuicide = true;
+ // The draw thread might be waiting on an empty queue, in which
+ // case we have to wake it up (see getOldestFrameInQueue()).
+ // We'll do it twice 100ms apart to avoid a timing issue where
+ // we signal just before the thread waits.
+ POST_QueueNotEmpty(); // wake it if necessary
+ sleepInSec(0.1);
+ POST_QueueNotEmpty();
+ pthread_join(m_drawThread, 0); // wait for death
+ m_drawThreadIsRunning = m_drawThreadShouldSuicide = false;
+ }
+
+ void startDrawThreadIfNecessary()
+ { if (!m_drawThreadIsRunning) startDrawThread(); }
+
+ void killDrawThreadIfNecessary()
+ { if (m_drawThreadIsRunning) killDrawThread(); }
+
+ // Whenever a "set" method is called that may change one of the
+ // interrelated time quantities, set all of them. We expect
+ // the mode to have been set already.
+ void resetTimeRelatedQuantities(Real framesPerSec,
+ Real timeScale, Real desiredBufLengthSec)
+ {
+ if (framesPerSec <= 0) framesPerSec = DefaultFrameRateFPS;
+ if (timeScale <= 0) timeScale = 1;
+ if (desiredBufLengthSec < 0)
+ desiredBufLengthSec = DefaultDesiredBufferLengthInSec;
+
+ // Frame rate.
+ m_frameRateFPS = framesPerSec;
+ m_timeBetweenFramesInNs = secToNs(1/m_frameRateFPS);
+ m_allowableFrameTimeSlopInNs =
+ secToNs(DefaultSlopAsFractionOfFrameInterval/m_frameRateFPS);
+ m_allowableFrameJitterInNs = DefaultAllowableFrameJitterInNs;
+
+ // Time scale.
+ m_simTimeUnitsPerSec = timeScale;
+
+ // Realtime buffer.
+ m_desiredBufferLengthInSec = desiredBufLengthSec;
+
+ int numFrames =
+ (int)(m_desiredBufferLengthInSec/nsToSec(m_timeBetweenFramesInNs)
+ + 0.5);
+ if (numFrames==0 && m_desiredBufferLengthInSec > 0)
+ numFrames = 1;
+
+ // If we're in RealTime mode and we have changed the number of
+ // frames in the buffer, reallocate the pool and kill or start
+ // the draw thread if necessary.
+ if (m_mode == RealTime && numFrames != m_pool.size()) {
+ if (m_pool.size()) {
+ // draw thread isn't needed if we get rid of the buffer
+ if (numFrames == 0)
+ killDrawThreadIfNecessary();
+ initializePool(numFrames);
+ } else {
+ // draw thread is needed if we don't have one
+ initializePool(numFrames);
+ startDrawThreadIfNecessary();
+ }
+ }
+
+ clearStats();
+
+ // Note that the next frame we see is the first one and we'll need
+ // to initialize adjusted real time then.
+ m_nextFrameDueAdjRT = -1LL; // i.e., now
+ }
+
+ void setMode(Visualizer::Mode newMode) {
+ // If we're not changing modes we just clear the stats and invalidate
+ // the next expected frame time so that we'll take the first one that
+ // shows up.
+ if (newMode==m_mode) {
+ resetTimeRelatedQuantities(m_frameRateFPS,
+ m_simTimeUnitsPerSec,
+ m_desiredBufferLengthInSec);
+ return;
+ }
+
+ // Mode is changing. If it was buffered RealTime before we have
+ // to clean up first.
+ if (m_mode == RealTime && m_pool.size()) {
+ killDrawThreadIfNecessary();
+ initializePool(0); // clear the buffer
+ }
+
+ m_mode = newMode; // change mode
+ resetTimeRelatedQuantities(m_frameRateFPS,
+ m_simTimeUnitsPerSec,
+ m_desiredBufferLengthInSec);
+ }
+
+ void setDesiredFrameRate(Real framesPerSec) {
+ resetTimeRelatedQuantities(framesPerSec,
+ m_simTimeUnitsPerSec,
+ m_desiredBufferLengthInSec);
+ // Make sure the GUI doesn't try to outrace us when it generates
+ // its own frames.
+ m_protocol.setMaxFrameRate(framesPerSec);
+ }
+
+ void setDesiredBufferLengthInSec(Real bufferLengthInSec) {
+ resetTimeRelatedQuantities(m_frameRateFPS,
+ m_simTimeUnitsPerSec,
+ bufferLengthInSec);
+ }
+
+ void setRealTimeScale(Real simTimePerRealSec) {
+ resetTimeRelatedQuantities(m_frameRateFPS,
+ simTimePerRealSec,
+ m_desiredBufferLengthInSec);
+ }
+
+ Real getDesiredBufferLengthInSec() const
+ { return m_desiredBufferLengthInSec; }
+
+ int getActualBufferLengthInFrames() const {return m_pool.size();}
+ Real getActualBufferLengthInSec() const
+ { return (Real)nsToSec(getActualBufferLengthInFrames()
+ *m_timeBetweenFramesInNs); }
+
+ // Generate this frame and send it immediately to the renderer without
+ // thinking too hard about it.
+ void drawFrameNow(const State& state);
+
+ // In RealTime mode we have a frame to draw and a desired draw time in
+ // AdjRT. Draw it when the time comes, and adjust AdjRT if necessary.
+ void drawRealtimeFrameWhenReady
+ (const State& state, const long long& desiredDrawTimeAdjRT);
+
+ // Queuing is used only in RealTime mode.
+
+ // Called from the simulation thread when it wants to report a frame
+ // and we are in RealTime mode.
+ void reportRealtime(const State& state);
+
+ // Set the maximum number of frames in the buffer.
+ void initializePool(int sz) {
+ pthread_mutex_lock(&m_queueLock);
+ m_pool.resize(sz);m_oldest=m_nframe=0;
+ pthread_mutex_unlock(&m_queueLock);
+ }
+
+ int getNFramesInQueue() const {return m_nframe;}
+
+ // Queing is enabled if the pool was allocated.
+ bool queuingIsEnabled() const {return m_pool.size() != 0;}
+ bool queueIsFull() const {return m_nframe==m_pool.size();}
+ bool queueIsEmpty() const {return m_nframe==0;}
+
+ // I'm capitalizing these methods because they are VERY important!
+ void LOCK_Queue() {pthread_mutex_lock(&m_queueLock);}
+ void UNLOCK_Queue() {pthread_mutex_unlock(&m_queueLock);}
+ void WAIT_QueueNotFull() {pthread_cond_wait(&m_queueNotFull,&m_queueLock);}
+ void POST_QueueNotFull() {pthread_cond_signal(&m_queueNotFull);}
+ void WAIT_QueueNotEmpty() {pthread_cond_wait(&m_queueNotEmpty,&m_queueLock);}
+ void POST_QueueNotEmpty() {pthread_cond_signal(&m_queueNotEmpty);}
+ void WAIT_QueueIsEmpty() {pthread_cond_wait(&m_queueIsEmpty,&m_queueLock);}
+ void POST_QueueIsEmpty() {pthread_cond_signal(&m_queueIsEmpty);}
+
+ // Called from simulation thread. Blocks until there is room in
+ // the queue, then inserts this state unconditionally, with the indicated
+ // desired rendering time in adjusted real time. We then update the
+ // "time of next queue slot" to be one ideal frame interval later than
+ // the desired draw time.
+ void addFrameToQueueWithWait(const State& state,
+ const long long& desiredDrawTimeAdjRT)
+ {
+ LOCK_Queue();
+ ++numReportedFramesThatWereQueued;
+ if (queueIsFull()) {
+ ++numQueuedFramesThatHadToWait;
+ do {WAIT_QueueNotFull();} // atomic: unlock, long wait, relock
+ while (queueIsFull()); // must recheck condition
+ }
+
+ // There is room in the queue now. We're holding the lock.
+ Frame& frame = m_pool[(m_oldest+m_nframe)%m_pool.size()];
+ frame.state = state;
+ frame.desiredDrawTimeAdjRT = desiredDrawTimeAdjRT;
+
+ // Record the frame time.
+ m_prevFrameSimTime = state.getTime();
+
+ // Set the expected next frame time (in AdjRT).
+ m_nextFrameDueAdjRT = desiredDrawTimeAdjRT + m_timeBetweenFramesInNs;
+
+ if (++m_nframe == 1)
+ POST_QueueNotEmpty(); // wake up rendering thread on first frame
+
+ UNLOCK_Queue();
+ }
+
+ // Call from simulation thread to allow the drawing thread to flush
+ // any frames currently in the queue.
+ void waitUntilQueueIsEmpty() {
+ if ( !queuingIsEnabled() || m_nframe==0
+ || !m_drawThreadIsRunning || m_drawThreadShouldSuicide)
+ return;
+ LOCK_Queue();
+ while (m_nframe) {WAIT_QueueIsEmpty();}
+ UNLOCK_Queue();
+ }
+
+ // The drawing thread uses this to find the oldest frame in the buffer.
+ // It may then at its leisure use the contained State to generate a screen
+ // image. There is no danger of the simulation thread modifying this
+ // frame; once it has been put in it stays there until the drawing thread
+ // takes it out. When done it should return the frame to the pool.
+ // Returns true if it gets a frame (which will always happen in normal
+ // operation since it waits until one is available), false if the draw
+ // thread should quit.
+ bool getOldestFrameInQueue(const Frame** fp) {
+ LOCK_Queue();
+ if (m_nframe == 0 && !m_drawThreadShouldSuicide) {
+ ++numTimesDrawThreadBlockedOnEmptyQueue;
+ do {WAIT_QueueNotEmpty();} // atomic: unlock, long wait, relock
+ while (m_nframe==0 && !m_drawThreadShouldSuicide); // must recheck
+ } else {
+ sumOfQueueLengths += double(m_nframe);
+ sumSquaredOfQueueLengths += double(square(m_nframe));
+ }
+ // There is at least one frame available now, unless we're supposed
+ // to quit. We are holding the lock.
+ UNLOCK_Queue();
+ if (m_drawThreadShouldSuicide) {*fp=0; return false;}
+ else {*fp=&m_pool[m_oldest]; return true;} // sim thread won't change oldest
+ }
+
+ // Drawing thread uses this to note that it is done with the oldest
+ // frame which may now be reused by the simulation thread. The queueNotFull
+ // condition is posted if there is a reasonable amount of room in the pool
+ // now.
+ void noteThatOldestFrameIsNowAvailable() {
+ LOCK_Queue();
+ m_oldest = (m_oldest+1)%m_pool.size(); // move to next-oldest
+ --m_nframe; // there is now one fewer frame in use
+ if (m_nframe == 0)
+ POST_QueueIsEmpty(); // in case we're flushing
+ // Start the simulation again when the pool is about half empty.
+ if (m_nframe <= m_pool.size()/2+1)
+ POST_QueueNotFull();
+ UNLOCK_Queue();
+ }
+
+ // Given a time t in simulation time units, return the equivalent time r in
+ // seconds of real time. That is the amount of real time that should have
+ // elapsed since t=0 if this simulation were running at exactly the desired
+ // real time rate.
+ long long convertSimTimeToNs(const double& t)
+ { return secToNs(t / m_simTimeUnitsPerSec); }
+
+ // same as ns; that's what AdjRT tries to be
+ long long convertSimTimeToAdjRT(const double& t)
+ { return convertSimTimeToNs(t); }
+
+ double convertAdjRTtoSimTime(const long long& a)
+ { return nsToSec(a) * m_simTimeUnitsPerSec; }
+
+ long long convertRTtoAdjRT(const long long& r)
+ { return r - m_adjustedRealTimeBase; }
+
+ long long convertAdjRTtoRT(const long long& a)
+ { return a + m_adjustedRealTimeBase; }
+
+
+ // Adjust the real time base by a given signed offset in nanoseconds. We're
+ // seeing incorrect adjusted realtime a* = r - r0*, but we know the actual
+ // value is a. Pass in the error e=(a*-a), then we want to calculate a
+ // new base adjustment r0 such that a = r - r0. So:
+ // a = r - r0* - e => r0=r0*+e.
+ void readjustAdjustedRealTimeBy(const long long& e) {
+ m_adjustedRealTimeBase += e;
+ ++numAdjustmentsToRealTimeBase;
+ }
+
+ long long getAdjustedRealTime()
+ { return realTimeInNs() - m_adjustedRealTimeBase; }
+
+ // Increment the reference count and return its new value.
+ int incrRefCount() const {return ++m_refCount;}
+
+ // Decrement the reference count and return its new value.
+ int decrRefCount() const {return --m_refCount;}
+
+ // Get the current value of the reference counter.
+ int getRefCount() const {return m_refCount;}
+
+ const MultibodySystem& m_system;
+ VisualizerProtocol m_protocol;
+ bool m_shutdownWhenDestructed;
+
+ Array_<DecorativeGeometry> m_addedGeometry;
+ Array_<RubberBandLine> m_lines;
+ Array_<DecorationGenerator*> m_generators;
+ Array_<Visualizer::InputListener*> m_listeners;
+ Array_<Visualizer::FrameController*> m_controllers;
+
+ CoordinateDirection m_upDirection;
+ Real m_groundHeight;
+
+ // User control of Visualizer behavior.
+ Visualizer::Mode m_mode;
+ Real m_frameRateFPS; // in frames/sec if > 0, else use default
+ Real m_simTimeUnitsPerSec; // ratio of sim time units to real seconds
+ Real m_desiredBufferLengthInSec; // RT only: how much delay (<0 => default)
+
+ // How many nanoseconds between frames?
+ long long m_timeBetweenFramesInNs;
+ // How much accuracy should we require from sleep()?
+ long long m_allowableFrameJitterInNs;
+ // How much slop is allowed in matching the time of a simulation frame
+ // to the real time at which its frame is drawn?
+ long long m_allowableFrameTimeSlopInNs;
+
+ // The offset r0 to subtract from the interval timer reading to produce
+ // the adjusted real time a that we expect to match the current simulation
+ // time t in ns. That is a = realTimeInNs()-r0. This base is adjusted by
+ // the drawing thread when we see what time we actually were able to
+ // deliver a frame.
+ long long m_adjustedRealTimeBase; // r0
+
+ // This is when we would like the simulation to send us another frame.
+ // It is optimistically set to one frame interval later than the desired
+ // draw time of the most recent frame to be put in the queue. This is
+ // also used in non-RealTime mode where AdjRT==RT.
+ long long m_nextFrameDueAdjRT;
+
+ // In RealTime mode we remember the simulated time in the previous
+ // supplied frame so that we can tell if we see an earlier frame,
+ // meaning (most likely) that we are starting a new simulation or
+ // seeing a playback of an old one.
+ double m_prevFrameSimTime;
+
+ // The frame buffer:
+ Array_<Frame,int> m_pool; // fixed size, old to new order but circular
+ int m_oldest, m_nframe; // oldest is index into pool, nframe is #valid entries
+ pthread_mutex_t m_queueLock;
+ pthread_cond_t m_queueNotFull; // these must use with m_queueLock
+ pthread_cond_t m_queueNotEmpty;
+ pthread_cond_t m_queueIsEmpty;
+
+ pthread_t m_drawThread; // the rendering thread
+ bool m_drawThreadIsRunning;
+ bool m_drawThreadShouldSuicide;
+
+ mutable int m_refCount; // how many Visualizer handles reference
+ // this Impl object?
+
+ // Statistics
+ int numFramesReportedBySimulation;
+ int numReportedFramesThatWereIgnored;
+ int numReportedFramesThatHadToWait;
+ int numReportedFramesThatSkippedAhead;
+ int numReportedFramesThatArrivedTooLate;
+ int numReportedFramesThatWereQueued;
+ int numQueuedFramesThatHadToWait;
+
+ int numFramesSentToRenderer;
+ int numFramesDelayedByRenderer;
+ int numTimesDrawThreadBlockedOnEmptyQueue;
+ int numAdjustmentsToRealTimeBase;
+
+ double sumOfAllJitter; // updated at time sent to renderer (ms)
+ double sumSquaredOfAllJitter; // ms^2
+
+ // These are updated by the drawing thread each time it looks at the
+ // queue to pull off a frame.
+ double sumOfQueueLengths; // for computing the average length
+ double sumSquaredOfQueueLengths; // for std deviation
+
+
+ void clearStats() {
+ numFramesReportedBySimulation=0;
+ numReportedFramesThatWereIgnored=0;
+ numReportedFramesThatHadToWait=0;
+ numReportedFramesThatSkippedAhead=0;
+ numReportedFramesThatArrivedTooLate=0;
+ numReportedFramesThatWereQueued=0;
+ numQueuedFramesThatHadToWait=0;
+
+ numFramesSentToRenderer=0;
+ numFramesDelayedByRenderer=0;
+ numTimesDrawThreadBlockedOnEmptyQueue=0;
+ numAdjustmentsToRealTimeBase=0;
+
+ sumOfAllJitter = 0;
+ sumSquaredOfAllJitter = 0;
+
+ sumOfQueueLengths = 0;
+ sumSquaredOfQueueLengths = 0;
+ }
+
+ void dumpStats(std::ostream& o) const {
+ o << "Visualizer stats:\n";
+ o << " Mode: ";
+ switch(m_mode) {
+ case PassThrough: o << "PassThrough\n"; break;
+ case Sampling: o << "Sampling\n"; break;
+ case RealTime:
+ o << "RealTime, TimeScale=" << m_simTimeUnitsPerSec
+ << " sim time units/real second\n";
+ o << " Desired/actual buffer length(s): "
+ << getDesiredBufferLengthInSec() << "/"
+ << getActualBufferLengthInSec() << " ("
+ << getActualBufferLengthInFrames() << " frames)\n";
+ break;
+ };
+ o << " Desired frame rate: " << m_frameRateFPS << endl;
+ o << " reported frames: " << numFramesReportedBySimulation << endl;
+ o << " | ignored: " << numReportedFramesThatWereIgnored << endl;
+ o << " | had to wait: " << numReportedFramesThatHadToWait << endl;
+ o << " | skipped ahead: " << numReportedFramesThatSkippedAhead << endl;
+ o << " | came too late: " << numReportedFramesThatArrivedTooLate << endl;
+ o << " | were buffered: " << numReportedFramesThatWereQueued << endl;
+ o << " | | full buffer: " << numQueuedFramesThatHadToWait << endl;
+ o << " frames sent to renderer: " << numFramesSentToRenderer << endl;
+ o << " | delayed by renderer : " << numFramesDelayedByRenderer << endl;
+ if (numReportedFramesThatWereQueued && numFramesSentToRenderer) {
+ const double avg = sumOfQueueLengths/numFramesSentToRenderer;
+ o << " | average buffer length (frames): " << avg << endl;
+ o << " | std dev buffer length (frames): "
+ << std::sqrt(std::max(0.,
+ sumSquaredOfQueueLengths/numFramesSentToRenderer
+ - square(avg))) << endl;
+ }
+ o << " draw blocked for empty buffer: "
+ << numTimesDrawThreadBlockedOnEmptyQueue << endl;
+ o << " adjustments to real time base: "
+ << numAdjustmentsToRealTimeBase << endl;
+ if (numFramesSentToRenderer > 0) {
+ const double avg = sumOfAllJitter/numFramesSentToRenderer;
+ o << " average jitter (ms): " << avg << endl;
+ o << " jitter std dev (ms): "
+ << std::sqrt(sumSquaredOfAllJitter/numFramesSentToRenderer
+ - square(avg)) << endl;
+ }
+ }
+
+};
+
+// Generate geometry for the given state and send it to the visualizer using
+// the VisualizerProtocol object. In buffered mode this is called from the
+// rendering thread; otherwise, this is just the main simulation thread.
+void Visualizer::Impl::drawFrameNow(const State& state) {
+ m_system.realize(state, Stage::Position);
+
+ // Collect up the geometry that constitutes this scene.
+ Array_<DecorativeGeometry> geometry;
+ for (Stage stage = Stage::Topology; stage <= state.getSystemStage(); ++stage)
+ m_system.calcDecorativeGeometryAndAppend(state, stage, geometry);
+ for (unsigned i = 0; i < m_generators.size(); i++)
+ m_generators[i]->generateDecorations(state, geometry);
+
+ // Execute frame controls (e.g. camera positioning).
+ for (unsigned i = 0; i < m_controllers.size(); ++i)
+ m_controllers[i]->generateControls(Visualizer(this), state, geometry);
+
+ // Calculate the spatial pose of all the geometry and send it to the
+ // renderer.
+ m_protocol.beginScene(state.getTime());
+ VisualizerGeometry geometryCreator
+ (m_protocol, m_system.getMatterSubsystem(), state);
+ for (unsigned i = 0; i < geometry.size(); ++i)
+ geometry[i].implementGeometry(geometryCreator);
+ for (unsigned i = 0; i < m_addedGeometry.size(); ++i)
+ m_addedGeometry[i].implementGeometry(geometryCreator);
+ const SimbodyMatterSubsystem& matter = m_system.getMatterSubsystem();
+ for (unsigned i = 0; i < m_lines.size(); ++i) {
+ const RubberBandLine& line = m_lines[i];
+ const MobilizedBody& B1 = matter.getMobilizedBody(line.b1);
+ const MobilizedBody& B2 = matter.getMobilizedBody(line.b2);
+ const Transform& X_GB1 = B1.getBodyTransform(state);
+ const Transform& X_GB2 = B2.getBodyTransform(state);
+ const Vec3 end1 = X_GB1*line.station1;
+ const Vec3 end2 = X_GB2*line.station2;
+ const Real thickness = line.line.getLineThickness() == -1
+ ? 1 : line.line.getLineThickness();
+ m_protocol.drawLine(end1, end2,
+ VisualizerGeometry::getColor(line.line), thickness);
+ }
+ m_protocol.finishScene();
+
+ ++numFramesSentToRenderer;
+}
+
+// This is called from the drawing thread if we're buffering, otherwise
+// directly from the simulation thread.
+void Visualizer::Impl::drawRealtimeFrameWhenReady
+ (const State& state, const long long& desiredDrawTimeAdjRT)
+{
+ const long long earliestDrawTimeAdjRT =
+ desiredDrawTimeAdjRT - m_allowableFrameJitterInNs;
+ const long long latestDrawTimeAdjRT =
+ desiredDrawTimeAdjRT + m_allowableFrameJitterInNs;
+
+ // Wait for the next frame time, allowing for a little jitter
+ // since we can't expect sleep to wake us up at the exact time.
+ long long now = getAdjustedRealTime();
+ if (now < earliestDrawTimeAdjRT) {
+ ++numFramesDelayedByRenderer;
+ do {sleepInNs(desiredDrawTimeAdjRT-now);}
+ while ((now=getAdjustedRealTime()) < earliestDrawTimeAdjRT);
+
+ // Keep stats on the jitter situation.
+ const long long jitterInNs = now - desiredDrawTimeAdjRT;
+ const double jitterInMs = jitterInNs*1e-6;
+ sumOfAllJitter += jitterInMs;
+ sumSquaredOfAllJitter += square(jitterInMs);
+ }
+
+ // timingError is signed with + meaning we sent the frame late.
+ const long long timingError = now - desiredDrawTimeAdjRT;
+
+ // If we sent this frame more than one frame time late we're going to
+ // admit we're not making real time and adjust the
+ // AdjRT base to match.
+ if (timingError > m_timeBetweenFramesInNs)
+ readjustAdjustedRealTimeBy(now - desiredDrawTimeAdjRT);
+
+ // It is time to render the frame.
+ drawFrameNow(state);
+}
+
+// Attempt to report a frame while we're in realtime mode.
+void Visualizer::Impl::reportRealtime(const State& state) {
+ // If we see a simulation time that is earlier than the last one,
+ // we are probably starting a new simulation or playback. Flush the
+ // old one. Note that we're using actual simulation time; we don't
+ // want to get tricked by adjustments to the real time base.
+ if (state.getTime() < m_prevFrameSimTime) {
+ waitUntilQueueIsEmpty();
+ m_nextFrameDueAdjRT = -1; // restart time base
+ }
+
+ // scale, convert to ns (doesn't depend on real time base)
+ const long long t = convertSimTimeToAdjRT(state.getTime());
+
+ // If this is the first frame, or first since last setMode(), then
+ // we synchronize Adjusted Real Time to match. Readjustments will occur
+ // if the simulation doesn't keep up with real time.
+ if (m_nextFrameDueAdjRT < 0 || t == 0) {
+ m_adjustedRealTimeBase = realTimeInNs() - t;
+ // now getAdjustedRealTime()==t
+ m_nextFrameDueAdjRT = t; // i.e., now
+ }
+
+ // "timeSlop" is the amount we'll allow a frame's simulation time to
+ // deviate from the real time at which we draw it. That is, if we're
+ // expecting a frame at time t_f and the simulator instead delivers a
+ // frame at t_s, we'll consider that a match if |t_s-t_f|<=slop.
+ // The reason for this is that we prefer to issue frames at regular
+ // intervals, so if the frame time and sim time match closely enough
+ // we won't reschedule the frames. Otherwise, a sim frame whose time
+ // is too early (t_s<t_f-slop) gets thrown away (or used in desperation
+ // if we're not keeping up with real time), and a sim frame
+ // whose time is too late (t_s>t_f+slop) causes us to delay drawing
+ // that frame until real time catches up with what's in it. Typically
+ // timeSlop is set to a small fraction of the frame time, like 5%.
+ const long long timeSlop = m_allowableFrameTimeSlopInNs;
+ const long long next = m_nextFrameDueAdjRT;
+ const long long earliest = next - timeSlop;
+ const long long latest = next + timeSlop;
+
+ if (t < earliest) {
+ ++numReportedFramesThatWereIgnored; // we don't need this one
+ return;
+ }
+
+ long long desiredDrawTimeAdjRT = next;
+ if (t > latest) {
+ ++numReportedFramesThatSkippedAhead;
+ desiredDrawTimeAdjRT = t;
+ }
+
+ // If buffering is enabled, push this onto the queue. Note that we
+ // might have to wait until the queue has some room.
+ if (queuingIsEnabled()) {
+ // This also sets expectations for the next frame.
+ addFrameToQueueWithWait(state, desiredDrawTimeAdjRT);
+ return;
+ }
+
+ // There is no buffer. We'll just render this frame as soon as its
+ // drawing time arrives. No need to copy the state here. Note that
+ // the simulation thread is doing the drawing as well as the simulating.
+ // This method will also readjust adjusted real time if the frame came
+ // too late.
+ drawRealtimeFrameWhenReady(state, desiredDrawTimeAdjRT);
+
+ // Now set expectations for the next frame.
+ m_nextFrameDueAdjRT = t + m_timeBetweenFramesInNs;
+}
+
+
+
+//==============================================================================
+// VISUALIZER
+//==============================================================================
+Visualizer::Visualizer(Visualizer::Impl* srcImpl) : impl(srcImpl) {
+ if (impl) impl->incrRefCount();
+}
+
+Visualizer::Visualizer(const MultibodySystem& system) : impl(0) {
+ impl = new Impl(this, system, Array_<String>());
+ impl->incrRefCount();
+}
+
+Visualizer::Visualizer(const MultibodySystem& system,
+ const Array_<String>& searchPath) : impl(0) {
+ impl = new Impl(this, system, searchPath);
+ impl->incrRefCount();
+}
+
+Visualizer::Visualizer(const Visualizer& source) : impl(0) {
+ if (source.impl) {
+ impl = source.impl;
+ impl->incrRefCount();
+ }
+}
+
+Visualizer& Visualizer::operator=(const Visualizer& source) {
+ if (impl != source.impl) {
+ if (impl&& impl->decrRefCount()==0) delete impl;
+ impl = source.impl;
+ impl->incrRefCount();
+ }
+ return *this;
+}
+
+Visualizer::~Visualizer() {
+ if (impl && impl->decrRefCount()==0)
+ delete impl;
+}
+
+void Visualizer::shutdown()
+{ updImpl().m_protocol.shutdownGUI(); }
+
+Visualizer& Visualizer::setShutdownWhenDestructed(bool shouldShutdown)
+{ updImpl().setShutdownWhenDestructed(shouldShutdown); return *this; }
+
+bool Visualizer::getShutdownWhenDestructed() const
+{ return getImpl().getShutdownWhenDestructed(); }
+
+int Visualizer::getRefCount() const
+{ return impl ? impl->getRefCount() : 0; }
+
+ // Frame drawing methods
+
+void Visualizer::drawFrameNow(const State& state) const
+{ const_cast<Visualizer*>(this)->updImpl().drawFrameNow(state); }
+
+void Visualizer::flushFrames() const
+{ const_cast<Visualizer*>(this)->updImpl().waitUntilQueueIsEmpty(); }
+
+// The simulation thread normally delivers frames here. Handling is dispatched
+// according the current visualization mode.
+void Visualizer::report(const State& state) const {
+ Visualizer::Impl& rep = const_cast<Visualizer*>(this)->updImpl();
+
+ ++rep.numFramesReportedBySimulation;
+ if (rep.m_mode == RealTime) {
+ rep.reportRealtime(state);
+ return;
+ }
+
+ // We're in Sampling or PassThrough mode. AdjRT and RT are the same in
+ // these modes; they are just real time as determined by realTimeInNs(),
+ // the current value of the interval counter. We don't care at all what
+ // time the simulation thinks it is.
+
+ // If this is the first frame, or first since last setMode(), then
+ // we set our expected next frame arrival time to now.
+ if (rep.m_nextFrameDueAdjRT < 0LL)
+ rep.m_nextFrameDueAdjRT = realTimeInNs(); // now
+
+ // If someone asked for an infinite frame rate just send this along now.
+ if (rep.m_timeBetweenFramesInNs == 0LL) {
+ drawFrameNow(state);
+ return;
+ }
+
+ const long long earliestDrawTime = rep.m_nextFrameDueAdjRT
+ - rep.m_allowableFrameJitterInNs;
+ long long now = realTimeInNs();
+ if (now < earliestDrawTime) {
+ // Too early to draw this frame. In Sampling mode that means we
+ // just ignore it.
+ if (rep.m_mode == Sampling) {
+ ++rep.numReportedFramesThatWereIgnored;
+ return;
+ }
+
+ // We're in PassThrough mode.
+ ++rep.numReportedFramesThatHadToWait;
+ do {sleepInNs(rep.m_nextFrameDueAdjRT - now);}
+ while ((now=realTimeInNs()) < earliestDrawTime);
+
+ // We're not going to wake up exactly when we wanted to; keep stats.
+ const double jitterInMs = (now - rep.m_nextFrameDueAdjRT)*1e-6;
+ rep.sumOfAllJitter += jitterInMs;
+ rep.sumSquaredOfAllJitter += square(jitterInMs);
+ }
+
+ // Frame time reached in Sampling or PassThrough modes. Draw the frame.
+ drawFrameNow(state);
+
+ // This frame might have been on time or late; we'll schedule the next
+ // time for one ideal frame interval later to keep the maximum rate down
+ // to the specified rate. Otherwise a late frame could be followed by lots
+ // of fast frames playing catch-up.
+ if (now-rep.m_nextFrameDueAdjRT <= rep.m_timeBetweenFramesInNs)
+ rep.m_nextFrameDueAdjRT += rep.m_timeBetweenFramesInNs;
+ else { // a late frame; delay the next one
+ rep.m_nextFrameDueAdjRT = now + rep.m_timeBetweenFramesInNs;
+ ++rep.numAdjustmentsToRealTimeBase;
+ }
+}
+
+ // Visualizer display options
+
+Visualizer& Visualizer::setBackgroundType(BackgroundType type)
+{ updImpl().m_protocol.setBackgroundType(type); return *this; }
+
+const Visualizer& Visualizer::setBackgroundColor(const Vec3& color) const
+{ getImpl().m_protocol.setBackgroundColor(color); return *this; }
+
+const Visualizer& Visualizer::setShowShadows(bool showShadows) const
+{ getImpl().m_protocol.setShowShadows(showShadows); return *this; }
+
+const Visualizer& Visualizer::setShowFrameRate(bool showFrameRate) const
+{ getImpl().m_protocol.setShowFrameRate(showFrameRate); return *this; }
+
+const Visualizer& Visualizer::setShowSimTime(bool showSimTime) const
+{ getImpl().m_protocol.setShowSimTime(showSimTime); return *this; }
+
+const Visualizer& Visualizer::setShowFrameNumber(bool showFrameNumber) const
+{ getImpl().m_protocol.setShowFrameNumber(showFrameNumber); return *this; }
+
+const Visualizer& Visualizer::setWindowTitle(const String& title) const
+{ getImpl().m_protocol.setWindowTitle(title); return *this; }
+
+ // Visualizer options
+
+Visualizer& Visualizer::setSystemUpDirection(const CoordinateDirection& upDir)
+{ updImpl().m_upDirection = upDir;
+ updImpl().m_protocol.setSystemUpDirection(upDir); return *this; }
+CoordinateDirection Visualizer::getSystemUpDirection() const
+{ return getImpl().m_upDirection; }
+
+
+Visualizer& Visualizer::setGroundHeight(Real height) {
+ updImpl().m_groundHeight = height;
+ updImpl().m_protocol.setGroundHeight(height); return *this;
+}
+Real Visualizer::getGroundHeight() const
+{ return getImpl().m_groundHeight; }
+
+void Visualizer::setMode(Visualizer::Mode mode) {updImpl().setMode(mode);}
+Visualizer::Mode Visualizer::getMode() const {return getImpl().m_mode;}
+
+Visualizer& Visualizer::setDesiredFrameRate(Real fps)
+{ updImpl().setDesiredFrameRate(std::max(fps, Real(0))); return *this; }
+Real Visualizer::getDesiredFrameRate() const
+{ return getImpl().m_frameRateFPS; }
+
+Visualizer& Visualizer::setRealTimeScale(Real simTimePerRealSec)
+{ updImpl().setRealTimeScale(simTimePerRealSec); return *this; }
+Real Visualizer::getRealTimeScale() const
+{ return getImpl().m_simTimeUnitsPerSec; }
+
+Visualizer& Visualizer::setDesiredBufferLengthInSec(Real bufferLengthInSec)
+{ updImpl().setDesiredBufferLengthInSec(bufferLengthInSec); return *this; }
+Real Visualizer::getDesiredBufferLengthInSec() const
+{ return getImpl().getDesiredBufferLengthInSec(); }
+int Visualizer::getActualBufferLengthInFrames() const
+{ return getImpl().getActualBufferLengthInFrames(); }
+Real Visualizer::getActualBufferLengthInSec() const
+{ return getImpl().getActualBufferLengthInSec(); }
+
+
+int Visualizer::addInputListener(Visualizer::InputListener* listener) {
+ Impl& impl = updImpl();
+ const int nxt = (int)impl.m_listeners.size();
+ impl.m_listeners.push_back(listener);
+ return nxt;
+}
+int Visualizer::getNumInputListeners() const
+{ return (int)getImpl().m_listeners.size(); }
+const Visualizer::InputListener& Visualizer::getInputListener(int i) const
+{ return *getImpl().m_listeners[i]; }
+Visualizer::InputListener& Visualizer::updInputListener(int i)
+{ return *updImpl().m_listeners[i]; }
+
+int Visualizer::addFrameController(Visualizer::FrameController* fc) {
+ Impl& impl = updImpl();
+ const int nxt = (int)impl.m_controllers.size();
+ impl.m_controllers.push_back(fc);
+ return nxt;
+}
+int Visualizer::getNumFrameControllers() const
+{ return (int)getImpl().m_controllers.size(); }
+const Visualizer::FrameController& Visualizer::getFrameController(int i) const
+{ return *getImpl().m_controllers[i]; }
+Visualizer::FrameController& Visualizer::updFrameController(int i)
+{ return *updImpl().m_controllers[i]; }
+
+
+ // Scene-building methods
+
+Visualizer& Visualizer::
+addMenu(const String& title, int menuId,
+ const Array_<pair<String, int> >& items)
+{
+ SimTK_ERRCHK2_ALWAYS(menuId >= 0, "Visualizer::addMenu()",
+ "Assigned menu ids must be nonnegative, but an attempt was made to create"
+ " a menu %s with id %d.", title.c_str(), menuId);
+
+ updImpl().m_protocol.addMenu(title, menuId, items);
+ return *this;
+}
+
+Visualizer& Visualizer::
+addSlider(const String& title, int sliderId,
+ Real minVal, Real maxVal, Real value)
+{
+ SimTK_ERRCHK2_ALWAYS(sliderId >= 0, "Visualizer::addSlider()",
+ "Assigned slider ids must be nonnegative, but an attempt was made to create"
+ " a slider %s with id %d.", title.c_str(), sliderId);
+ SimTK_ERRCHK4_ALWAYS(minVal <= value && value <= maxVal, "Visualizer::addSlider()",
+ "Initial slider value %g for slider %s was outside the specified range [%g,%g].",
+ value, title.c_str(), minVal, maxVal);
+
+ updImpl().m_protocol.addSlider(title, sliderId, minVal, maxVal, value);
+ return *this;
+}
+
+int Visualizer::
+addDecoration(MobilizedBodyIndex mobodIx, const Transform& X_BD,
+ const DecorativeGeometry& geom)
+{
+ Array_<DecorativeGeometry>& addedGeometry = updImpl().m_addedGeometry;
+ const int nxt = (int)addedGeometry.size();
+ addedGeometry.push_back(geom);
+ DecorativeGeometry& geomCopy = addedGeometry.back();
+ geomCopy.setBodyId((int)mobodIx);
+ geomCopy.setTransform(X_BD * geomCopy.getTransform());
+ return nxt;
+}
+int Visualizer::getNumDecorations() const
+{ return (int)getImpl().m_addedGeometry.size(); }
+const DecorativeGeometry& Visualizer::getDecoration(int i) const
+{ return getImpl().m_addedGeometry[i]; }
+DecorativeGeometry& Visualizer::updDecoration(int i) const
+{ return const_cast<Visualizer*>(this)->updImpl().m_addedGeometry[i]; }
+
+int Visualizer::
+addRubberBandLine(MobilizedBodyIndex b1, const Vec3& station1,
+ MobilizedBodyIndex b2, const Vec3& station2,
+ const DecorativeLine& line)
+{
+ Impl& impl = updImpl();
+ const int nxt = (int)impl.m_lines.size();
+ impl.m_lines.push_back(RubberBandLine(b1,station1, b2,station2, line));
+ return nxt;
+}
+int Visualizer::getNumRubberBandLines() const
+{ return (int)getImpl().m_lines.size(); }
+const DecorativeLine& Visualizer::getRubberBandLine(int i) const
+{ return getImpl().m_lines[i].line; }
+DecorativeLine& Visualizer::updRubberBandLine(int i) const
+{ return const_cast<Visualizer*>(this)->updImpl().m_lines[i].line; }
+
+int Visualizer::
+addDecorationGenerator(DecorationGenerator* generator)
+{
+ Impl& impl = updImpl();
+ const int nxt = (int)impl.m_generators.size();
+ impl.m_generators.push_back(generator);
+ return nxt;
+}
+int Visualizer::
+getNumDecorationGenerators() const
+{ return (int)getImpl().m_generators.size(); }
+const DecorationGenerator& Visualizer::
+getDecorationGenerator(int i) const
+{ return *getImpl().m_generators[i]; }
+DecorationGenerator& Visualizer::
+updDecorationGenerator(int i)
+{ return *updImpl().m_generators[i]; }
+
+ // Frame control methods
+const Visualizer& Visualizer::
+setCameraTransform(const Transform& transform) const
+{ getImpl().m_protocol.setCameraTransform(transform); return *this; }
+
+const Visualizer& Visualizer::zoomCameraToShowAllGeometry() const
+{ getImpl().m_protocol.zoomCamera(); return *this; }
+
+const Visualizer& Visualizer::
+pointCameraAt(const Vec3& point, const Vec3& upDirection) const
+{ getImpl().m_protocol.lookAt(point, upDirection); return *this; }
+
+const Visualizer& Visualizer::setCameraFieldOfView(Real fov) const
+{ getImpl().m_protocol.setFieldOfView(fov); return *this; }
+
+const Visualizer& Visualizer::
+setCameraClippingPlanes(Real nearPlane, Real farPlane) const
+{ getImpl().m_protocol.setClippingPlanes(nearPlane, farPlane);
+ return *this; }
+
+
+const Visualizer& Visualizer::setSliderValue(int slider, Real newValue) const
+{ getImpl().m_protocol.setSliderValue(slider, newValue); return *this; }
+
+const Visualizer& Visualizer::
+setSliderRange(int slider, Real newMin, Real newMax) const
+{ getImpl().m_protocol.setSliderRange(slider, newMin, newMax); return *this; }
+
+ // Debugging and statistics
+void Visualizer::dumpStats(std::ostream& o) const {getImpl().dumpStats(o);}
+void Visualizer::clearStats() {updImpl().clearStats();}
+
+ // Internal use only
+const Array_<Visualizer::InputListener*>& Visualizer::getInputListeners() const
+{ return getImpl().m_listeners; }
+const Array_<Visualizer::FrameController*>& Visualizer::getFrameControllers() const
+{ return getImpl().m_controllers; }
+const MultibodySystem& Visualizer::getSystem() const {return getImpl().m_system;}
+
+
+//==============================================================================
+// THE DRAWING THREAD
+//==============================================================================
+/* When we're in RealTime mode, we run a separate thread that actually sends
+frames to the renderer. It pulls frames off the back (oldest) end of the
+frame buffer queue, while the simulation thread is pushing frames onto the
+front (newest) end of the queue. The rendering thread is created whenever
+the Visualizer enters RealTime mode, and canceled whenever it leaves
+RealTime mode or is destructed.
+
+This is the main function for the buffered drawing thread. Its job is to
+pull the oldest frames off the queue and send them to the renderer at
+the right real times. We use adjusted real time (AdjRT) which should match
+the simulation time kept with the frame as its desired draw time. If the
+frame is ahead of AdjRT, we'll sleep to let real time catch up. If the
+frame is substantially behind, we'll render it now and then adjust the AdjRT
+base to acknowledge that we have irretrievably slipped from real time and need
+to adjust our expectations for the future. */
+static void* drawingThreadMain(void* visualizerRepAsVoidp) {
+ Visualizer::Impl& vizImpl =
+ *reinterpret_cast<Visualizer::Impl*>(visualizerRepAsVoidp);
+
+ do {
+ // Grab the oldest frame in the queue.
+ // This will wait if necessary until a frame is available.
+ const Frame* framep;
+ if (vizImpl.getOldestFrameInQueue(&framep)) {
+ // Draw this frame as soon as its draw time arrives, and readjust
+ // adjusted real time if necessary.
+ vizImpl.drawRealtimeFrameWhenReady
+ (framep->state, framep->desiredDrawTimeAdjRT);
+
+ // Return the now-rendered frame to circulation in the pool. This may
+ // wake up the simulation thread if it was waiting for space.
+ vizImpl.noteThatOldestFrameIsNowAvailable();
+ }
+ } while (!vizImpl.m_drawThreadShouldSuicide);
+
+ // Attempt to wake up the simulation thread if it is waiting for
+ // the draw thread since there won't be any more notices!
+ vizImpl.POST_QueueNotFull(); // wake up if waiting for queue space
+ vizImpl.POST_QueueIsEmpty(); // wake up if flushing
+
+ return 0;
+}
diff --git a/Simbody/Visualizer/src/VisualizerGeometry.cpp b/Simbody/Visualizer/src/VisualizerGeometry.cpp
new file mode 100644
index 0000000..7a145d0
--- /dev/null
+++ b/Simbody/Visualizer/src/VisualizerGeometry.cpp
@@ -0,0 +1,169 @@
+/* -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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 "simbody/internal/common.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "VisualizerGeometry.h"
+#include "VisualizerProtocol.h"
+
+using namespace SimTK;
+
+static const Vec3 DefaultBodyColor = Gray;
+static const Vec3 DefaultPointColor = Magenta;
+
+VisualizerGeometry::VisualizerGeometry
+ (VisualizerProtocol& protocol, const SimbodyMatterSubsystem& matter,
+ const State& state)
+: protocol(protocol), matter(matter), state(state) {}
+
+// The DecorativeGeometry's frame D is given in the body frame B, via transform
+// X_BD. We want to know X_GD, the pose of the geometry in Ground, which we get
+// via X_GD=X_GB*X_BD.
+Transform VisualizerGeometry::calcX_GD(const DecorativeGeometry& geom) const {
+ const MobilizedBody& mobod =
+ matter.getMobilizedBody(MobilizedBodyIndex(geom.getBodyId()));
+ const Transform& X_GB = mobod.getBodyTransform(state);
+ const Transform& X_BD = geom.getTransform();
+ return X_GB*X_BD;
+}
+
+
+// We're going to draw three short lines aligned with the body axes
+// that intersect at the point.
+void VisualizerGeometry::
+implementPointGeometry(const SimTK::DecorativePoint& geom) {
+ const MobilizedBody& mobod =
+ matter.getMobilizedBody(MobilizedBodyIndex(geom.getBodyId()));
+ const Transform& X_GB = mobod.getBodyTransform(state);
+ const Transform& X_BD = geom.getTransform();
+ const Transform X_GD = X_GB*X_BD;
+ const Vec3 p_GP = X_GD*geom.getPoint();
+ const Real thickness =
+ geom.getLineThickness() == -1 ? Real(1) : geom.getLineThickness();
+
+ const Real DefaultLength = Real(0.05); // 1/20 of a unit length
+ const Vec3 lengths = DefaultLength * getScaleFactors(geom);
+ const Vec4 color = getColor(geom, DefaultPointColor);
+
+ protocol.drawLine(p_GP - lengths[0]*X_GB.x(),
+ p_GP + lengths[0]*X_GB.x(), color, thickness);
+ protocol.drawLine(p_GP - lengths[1]*X_GB.y(),
+ p_GP + lengths[1]*X_GB.y(), color, thickness);
+ protocol.drawLine(p_GP - lengths[2]*X_GB.z(),
+ p_GP + lengths[2]*X_GB.z(), color, thickness);
+}
+
+void VisualizerGeometry::implementLineGeometry(const SimTK::DecorativeLine& geom) {
+ const Transform X_GD = calcX_GD(geom);
+ protocol.drawLine(X_GD*geom.getPoint1(), X_GD*geom.getPoint2(), getColor(geom), geom.getLineThickness() == -1 ? 1 : geom.getLineThickness());
+}
+
+void VisualizerGeometry::implementBrickGeometry(const SimTK::DecorativeBrick& geom) {
+ const Transform X_GD = calcX_GD(geom);
+ const Vec3 hlen = getScaleFactors(geom).elementwiseMultiply(geom.getHalfLengths());
+ protocol.drawBox(X_GD, hlen, getColor(geom), getRepresentation(geom));
+}
+
+void VisualizerGeometry::implementCylinderGeometry(const SimTK::DecorativeCylinder& geom) {
+ const Transform X_GD = calcX_GD(geom);
+ const Vec3 scale = getScaleFactors(geom);
+ protocol.drawCylinder(X_GD, Vec3(scale[0]*geom.getRadius(),
+ scale[1]*geom.getHalfHeight(),
+ scale[2]*geom.getRadius()),
+ getColor(geom), getRepresentation(geom), getResolution(geom));
+}
+
+void VisualizerGeometry::implementCircleGeometry(const SimTK::DecorativeCircle& geom) {
+ const Transform X_GD = calcX_GD(geom);
+ const Vec3 scale = getScaleFactors(geom); // z ignored
+ protocol.drawCircle(X_GD, Vec3(scale[0]*geom.getRadius(),
+ scale[1]*geom.getRadius(), 1),
+ getColor(geom), getRepresentation(geom), getResolution(geom));
+}
+
+void VisualizerGeometry::implementSphereGeometry(const SimTK::DecorativeSphere& geom) {
+ const Transform X_GD = calcX_GD(geom);
+ protocol.drawEllipsoid(X_GD, geom.getRadius()*getScaleFactors(geom),
+ getColor(geom), getRepresentation(geom), getResolution(geom));
+}
+
+void VisualizerGeometry::implementEllipsoidGeometry(const SimTK::DecorativeEllipsoid& geom) {
+ const Transform X_GD = calcX_GD(geom);
+ const Vec3 radii = getScaleFactors(geom).elementwiseMultiply(geom.getRadii());
+ protocol.drawEllipsoid(X_GD, radii, getColor(geom), getRepresentation(geom),
+ getResolution(geom));
+}
+
+void VisualizerGeometry::implementFrameGeometry(const SimTK::DecorativeFrame& geom) {
+ const Transform X_GD = calcX_GD(geom);
+ protocol.drawCoords(X_GD, geom.getAxisLength()*getScaleFactors(geom), getColor(geom));
+}
+
+void VisualizerGeometry::implementTextGeometry(const SimTK::DecorativeText& geom) {
+ const Transform X_GD = calcX_GD(geom);
+ // The default is to face the camera.
+ bool faceCamera = geom.getFaceCamera()<0 ? true : (geom.getFaceCamera()!=0);
+ bool isScreenText = geom.getIsScreenText();
+ protocol.drawText(X_GD.p(), getScaleFactors(geom), getColor(geom),
+ geom.getText(), faceCamera, isScreenText);
+}
+
+void VisualizerGeometry::implementMeshGeometry(const SimTK::DecorativeMesh& geom) {
+ const Transform X_GD = calcX_GD(geom);
+ protocol.drawPolygonalMesh(geom.getMesh(), X_GD, getScaleFactors(geom),
+ getColor(geom), getRepresentation(geom));
+}
+
+Vec4 VisualizerGeometry::getColor(const DecorativeGeometry& geom,
+ const Vec3& defaultColor) {
+ Vec4 result;
+ if (geom.getColor()[0] >= 0)
+ result.updSubVec<3>(0) = geom.getColor();
+ else {
+ const Vec3 def = defaultColor[0] >= 0 ? defaultColor : DefaultBodyColor;
+ result.updSubVec<3>(0) = def;
+ }
+ result[3] = (geom.getOpacity() < 0 ? 1 : geom.getOpacity());
+ return result;
+}
+
+int VisualizerGeometry::getRepresentation(const DecorativeGeometry& geom) const {
+ if (geom.getRepresentation() == DecorativeGeometry::DrawDefault)
+ return DecorativeGeometry::DrawSurface;
+ return geom.getRepresentation();
+}
+
+unsigned short VisualizerGeometry::
+getResolution(const DecorativeGeometry& geom) const {
+ if (geom.getResolution() <= 0)
+ return 2;
+ return std::max((unsigned short) 1, (unsigned short) (geom.getResolution()*2));
+}
+
+Vec3 VisualizerGeometry::getScaleFactors(const DecorativeGeometry& geom) const {
+ const Vec3& scale = geom.getScaleFactors();
+ Vec3 actual;
+ for (int i=0; i<3; ++i)
+ actual[i] = scale[i] <= 0 ? 1 : scale[i];
+ return actual;
+}
diff --git a/Simbody/Visualizer/src/VisualizerGeometry.h b/Simbody/Visualizer/src/VisualizerGeometry.h
new file mode 100644
index 0000000..8ca6e93
--- /dev/null
+++ b/Simbody/Visualizer/src/VisualizerGeometry.h
@@ -0,0 +1,67 @@
+#ifndef SimTK_SIMBODY_VISUALIZER_GEOMETRY_H_
+#define SimTK_SIMBODY_VISUALIZER_GEOMETRY_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) 2010-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Ayman Habib *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This is the implementation of DecorativeGeometry used by
+ * Visualizer::Reporter.
+ */
+
+#include "simbody/internal/common.h"
+
+namespace SimTK {
+class SimbodyMatterSubsystem;
+class VisualizerProtocol;
+
+class VisualizerGeometry : public DecorativeGeometryImplementation {
+public:
+ VisualizerGeometry(VisualizerProtocol& protocol, const SimbodyMatterSubsystem& matter, const State& state);
+ ~VisualizerGeometry() {
+ }
+ void implementPointGeometry(const DecorativePoint& geom);
+ void implementLineGeometry(const DecorativeLine& geom);
+ void implementBrickGeometry(const DecorativeBrick& geom);
+ void implementCylinderGeometry(const DecorativeCylinder& geom);
+ void implementCircleGeometry(const DecorativeCircle& geom);
+ void implementSphereGeometry(const DecorativeSphere& geom);
+ void implementEllipsoidGeometry(const DecorativeEllipsoid& geom);
+ void implementFrameGeometry(const DecorativeFrame& geom);
+ void implementTextGeometry(const DecorativeText& geom);
+ void implementMeshGeometry(const DecorativeMesh& geom);
+ void implementMeshFileGeometry(const DecorativeMeshFile& geom) {}; // Not handled yet by this Visualizer
+ static Vec4 getColor(const DecorativeGeometry& geom,
+ const Vec3& defaultColor = Vec3(-1));
+private:
+ int getRepresentation(const DecorativeGeometry& geom) const;
+ unsigned short getResolution(const DecorativeGeometry& geom) const;
+ Vec3 getScaleFactors(const DecorativeGeometry& geom) const;
+ Transform calcX_GD(const DecorativeGeometry& geom) const;
+ VisualizerProtocol& protocol;
+ const SimbodyMatterSubsystem& matter;
+ const State& state;
+};
+}
+
+#endif // SimTK_SIMBODY_VISUALIZER_GEOMETRY_H_
diff --git a/Simbody/Visualizer/src/VisualizerProtocol.cpp b/Simbody/Visualizer/src/VisualizerProtocol.cpp
new file mode 100644
index 0000000..d4ed026
--- /dev/null
+++ b/Simbody/Visualizer/src/VisualizerProtocol.cpp
@@ -0,0 +1,731 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "simbody/internal/common.h"
+#include "simbody/internal/Visualizer.h"
+#include "simbody/internal/Visualizer_InputListener.h"
+#include "VisualizerProtocol.h"
+
+#include <cstdlib>
+#include <cstdio>
+#include <cctype>
+#include <cerrno>
+#include <cstring>
+#include <string>
+
+using namespace SimTK;
+using namespace std;
+
+#ifdef _WIN32
+ #include <fcntl.h>
+ #include <io.h>
+ #include <process.h>
+ #define READ _read
+#else
+ #include <unistd.h>
+ #define READ read
+#endif
+
+// gcc 4.4.3 complains bitterly if you don't check the return
+// status from the write() system call. This avoids those
+// warnings and maybe, someday, will catch an error.
+#define WRITE(pipeno, buf, len) \
+ {int status=write((pipeno), (buf), (len)); \
+ SimTK_ERRCHK4_ALWAYS(status!=-1, "VisualizerProtocol", \
+ "An attempt to write() %d bytes to pipe %d failed with errno=%d (%s).", \
+ (len),(pipeno),errno,strerror(errno));}
+
+static int inPipe;
+
+// Create a pipe, using the right call for this platform.
+static int createPipe(int pipeHandles[2]) {
+ const int status =
+#ifdef _WIN32
+ _pipe(pipeHandles, 16384, _O_BINARY);
+#else
+ pipe(pipeHandles);
+#endif
+ return status;
+}
+
+// Spawn the visualizer GUI executable, using the right method for
+// this platform. We take two executables to try in order,
+// and return after the first one succeeds. If neither works, we throw
+// an error that is hopefully helful.
+static void spawnViz(const Array_<String>& searchPath,
+ const String& appName, int toSimPipe, int fromSimPipe)
+{
+ int status;
+ char vizPipeToSim[32], vizPipeFromSim[32];
+ sprintf(vizPipeToSim, "%d", toSimPipe);
+ sprintf(vizPipeFromSim, "%d", fromSimPipe);
+
+ String exePath; // search path + appName
+
+#ifdef _WIN32
+ intptr_t handle;
+ for (unsigned i=0; i < searchPath.size(); ++i) {
+ exePath = searchPath[i] + appName;
+ handle = _spawnl(P_NOWAIT, exePath.c_str(), appName.c_str(),
+ vizPipeToSim, vizPipeFromSim, (const char*)0);
+ if (handle != -1)
+ break; // success!
+ }
+ status = (handle==-1) ? -1 : 0;
+#else
+ const pid_t pid = fork();
+ if (pid == 0) {
+ // child process
+ for (unsigned i=0; i < searchPath.size(); ++i) {
+ exePath = searchPath[i] + appName;
+ status = execl(exePath.c_str(), appName.c_str(),
+ vizPipeToSim, vizPipeFromSim, (const char*)0);
+ // if we get here the execl() failed
+ }
+ // fall through -- we failed on every try
+ } else {
+ // parent process
+ status = (pid==-1) ? -1 : 0;
+ }
+#endif
+
+ if (status != 0) {
+ // Create a chronicle of our failures above.
+ String failedPath;
+ for (unsigned i=0; i < searchPath.size(); ++i)
+ failedPath += (" " + searchPath[i] + "\n");
+
+ SimTK_ERRCHK4_ALWAYS(status == 0, "VisualizerProtocol::ctor()",
+ "Unable to spawn executable '%s' from directories:\n%s"
+ "Final system error was errno=%d (%s).",
+ appName.c_str(), failedPath.c_str(), errno, strerror(errno));
+ }
+}
+
+static void readDataFromPipe(int srcPipe, unsigned char* buffer, int bytes) {
+ int totalRead = 0;
+ while (totalRead < bytes)
+ totalRead += READ(srcPipe, buffer+totalRead, bytes-totalRead);
+}
+
+static void readData(unsigned char* buffer, int bytes)
+{
+ readDataFromPipe(inPipe, buffer, bytes);
+}
+
+static void* listenForVisualizerEvents(void* arg) {
+ Visualizer& visualizer = *reinterpret_cast<Visualizer*>(arg);
+ unsigned char buffer[256];
+
+ try
+ { while (true) {
+ // Receive a user input event.
+
+ readData(buffer, 1);
+ switch (buffer[0]) {
+ case KeyPressed: {
+ readData(buffer, 2);
+ const Array_<Visualizer::InputListener*>& listeners = visualizer.getInputListeners();
+ unsigned keyCode = buffer[0];
+ if (buffer[1] & Visualizer::InputListener::IsSpecialKey)
+ keyCode += Visualizer::InputListener::SpecialKeyOffset;
+ for (int i = 0; i < (int) listeners.size(); i++)
+ if (listeners[i]->keyPressed(keyCode, (unsigned)(buffer[1])))
+ break; // key press has been handled
+ break;
+ }
+ case MenuSelected: {
+ int menu, item;
+ readData((unsigned char*) &menu, sizeof(int));
+ readData((unsigned char*) &item, sizeof(int));
+ const Array_<Visualizer::InputListener*>& listeners = visualizer.getInputListeners();
+ for (int i = 0; i < (int) listeners.size(); i++)
+ if (listeners[i]->menuSelected(menu, item))
+ break; // menu event has been handled
+ break;
+ }
+ case SliderMoved: {
+ int slider;
+ readData((unsigned char*) &slider, sizeof(int));
+ float value;
+ readData((unsigned char*) &value, sizeof(float));
+ const Array_<Visualizer::InputListener*>& listeners = visualizer.getInputListeners();
+ for (int i = 0; i < (int) listeners.size(); i++)
+ if (listeners[i]->sliderMoved(slider, value))
+ break; // slider event has been handled
+ break;
+ }
+ default:
+ SimTK_ERRCHK1_ALWAYS(false, "listenForVisualizerEvents()",
+ "Unexpected command %u received from simbody-visualizer. Can't continue.",
+ (unsigned)buffer[0]);
+ }
+ }
+ } catch (const std::exception& e) {
+ std::cout << "Visualizer listenerThread: unrecoverable error:\n";
+ std::cout << e.what() << std::endl;
+ return (void*)1;
+ }
+ return (void*)0;
+}
+
+VisualizerProtocol::VisualizerProtocol
+ (Visualizer& visualizer, const Array_<String>& userSearchPath)
+{
+ // Launch the GUI application. We'll first look for one in the same directory
+ // as the running executable; then if that doesn't work we'll look in the
+ // bin subdirectory of the SimTK installation.
+
+ const char* GuiAppName = "simbody-visualizer";
+
+ Array_<String> actualSearchPath;
+ // Always start with the current executable's directory.
+ actualSearchPath.push_back(Pathname::getThisExecutableDirectory());
+ // User's stuff comes next, if any directories were provided. We're going
+ // to turn these into absolute pathnames, interpreting them as defined
+ // by Pathname, which includes executable-relative names. The "bin"
+ // subdirectory if any must already be present in the directory names.
+ for (unsigned i=0; i < userSearchPath.size(); ++i)
+ actualSearchPath.push_back
+ (Pathname::getAbsoluteDirectoryPathname(userSearchPath[i]));
+
+ if (Pathname::environmentVariableExists("SIMBODY_HOME")) {
+ const std::string e = Pathname::getAbsoluteDirectoryPathname(
+ Pathname::getEnvironmentVariable("SIMBODY_HOME"));
+ actualSearchPath.push_back(Pathname::addDirectoryOffset(e,"bin"));
+ } else if (Pathname::environmentVariableExists("SimTK_INSTALL_DIR")) {
+ const std::string e = Pathname::getAbsoluteDirectoryPathname(
+ Pathname::getEnvironmentVariable("SimTK_INSTALL_DIR"));
+ actualSearchPath.push_back(Pathname::addDirectoryOffset(e,"bin"));
+ }
+
+ // Try the build-time install location:
+ actualSearchPath.push_back(SIMBODY_VISUALIZER_INSTALL_DIR);
+
+ // Our last desperate attempts will
+ // be <platformDefaultInstallDir>/Simbody/bin
+ // and <platformDefaultInstallDir>/SimTK/bin
+ const std::string def = Pathname::getDefaultInstallDir();
+
+ actualSearchPath.push_back(
+ Pathname::addDirectoryOffset(def,
+ Pathname::addDirectoryOffset("Simbody", "bin")));
+ actualSearchPath.push_back(
+ Pathname::addDirectoryOffset(def,
+ Pathname::addDirectoryOffset("SimTK", "bin")));
+
+ int sim2vizPipe[2], viz2simPipe[2], status;
+
+ // Create pipe pair for communication from simulator to visualizer.
+ status = createPipe(sim2vizPipe);
+ SimTK_ASSERT_ALWAYS(status != -1, "VisualizerProtocol: Failed to open pipe");
+ outPipe = sim2vizPipe[1];
+
+ // Create pipe pair for communication from visualizer to simulator.
+ status = createPipe(viz2simPipe);
+ SimTK_ASSERT_ALWAYS(status != -1, "VisualizerProtocol: Failed to open pipe");
+ inPipe = viz2simPipe[0];
+
+ // Spawn the visualizer gui, trying local first then installed version.
+ spawnViz(actualSearchPath,
+ GuiAppName, sim2vizPipe[0], viz2simPipe[1]);
+
+ // Before we do anything else, attempt to exchange handshake messages with
+ // the visualizer. This will throw an exception if anything goes wrong.
+ // Note that this is done on the main thread.
+ shakeHandsWithGUI(outPipe, inPipe);
+
+ // Spawn the thread to listen for events.
+
+ pthread_mutex_init(&sceneLock, NULL);
+ pthread_t thread;
+ pthread_create(&thread, NULL, listenForVisualizerEvents, &visualizer);
+}
+
+// This is executed on the main thread at GUI startup and thus does not
+// require locking.
+void VisualizerProtocol::shakeHandsWithGUI(int toGUIPipe, int fromGUIPipe) {
+
+ // First send handshake message to GUI.
+
+ // The first two items must never change: the handshake command value, and
+ // an unsigned int containing the version number. Anything else might vary so both
+ // sides must stop reading if the version numbers are not compatible.
+ WRITE(outPipe, &StartupHandshake, 1);
+ WRITE(outPipe, &ProtocolVersion, sizeof(unsigned int));
+
+ // Send the current Simbody version number.
+ int major, minor, patch;
+ SimTK_version_simbody(&major, &minor, &patch);
+ WRITE(outPipe, &major, sizeof(int));
+ WRITE(outPipe, &minor, sizeof(int));
+ WRITE(outPipe, &patch, sizeof(int));
+
+ // Send the name of the current executable for use as a default window
+ // title and in "about" info.
+ bool isAbsolutePath;
+ std::string directory, fileName, extension;
+ Pathname::deconstructPathname(Pathname::getThisExecutablePath(),
+ isAbsolutePath, directory, fileName, extension);
+ // We're just sending the file name, not a full path. Keep it short.
+ unsigned nameLength = std::min((unsigned)fileName.size(), (unsigned)255);
+ WRITE(outPipe, &nameLength, sizeof(unsigned));
+ WRITE(outPipe, fileName.c_str(), nameLength);
+
+ // Now wait for handshake response from GUI.
+
+ unsigned char handshakeCommand;
+ readDataFromPipe(fromGUIPipe, &handshakeCommand, 1);
+ SimTK_ERRCHK2_ALWAYS(handshakeCommand == ReturnHandshake,
+ "VisualizerProtocol::shakeHandsWithGUI()",
+ "Expected initial handshake command %u but received %u. Can't continue.",
+ (unsigned)ReturnHandshake, (unsigned)handshakeCommand);
+
+ unsigned int GUIversion;
+ readDataFromPipe(fromGUIPipe, (unsigned char*)&GUIversion, sizeof(unsigned int));
+ SimTK_ERRCHK2_ALWAYS(GUIversion == ProtocolVersion,
+ "VisualizerProtocol::shakeHandsWithGUI()",
+ "simbody-visualizer protocol version %u is not compatible with the Simbody"
+ " Visualizer class protocol %u; this may be an installation problem."
+ " Can't continue.",
+ GUIversion, ProtocolVersion);
+
+ // Handshake was successful.
+}
+
+void VisualizerProtocol::shutdownGUI() {
+ // Don't wait for scene completion; kill GUI now.
+ char command = Shutdown;
+ WRITE(outPipe, &command, 1);
+}
+
+void VisualizerProtocol::beginScene(Real time) {
+ pthread_mutex_lock(&sceneLock);
+ char command = StartOfScene;
+ WRITE(outPipe, &command, 1);
+ float fTime = (float)time;
+ WRITE(outPipe, &fTime, sizeof(float));
+}
+
+void VisualizerProtocol::finishScene() {
+ char command = EndOfScene;
+ WRITE(outPipe, &command, 1);
+ pthread_mutex_unlock(&sceneLock);
+}
+
+void VisualizerProtocol::drawBox(const Transform& X_GB, const Vec3& scale, const Vec4& color, int representation) {
+ drawMesh(X_GB, scale, color, (short) representation, MeshBox, 0);
+}
+
+void VisualizerProtocol::drawEllipsoid(const Transform& X_GB, const Vec3& scale, const Vec4& color, int representation, unsigned short resolution) {
+ drawMesh(X_GB, scale, color, (short) representation, MeshEllipsoid, resolution);
+}
+
+void VisualizerProtocol::drawCylinder(const Transform& X_GB, const Vec3& scale, const Vec4& color, int representation, unsigned short resolution) {
+ drawMesh(X_GB, scale, color, (short) representation, MeshCylinder, resolution);
+}
+
+void VisualizerProtocol::drawCircle(const Transform& X_GB, const Vec3& scale, const Vec4& color, int representation, unsigned short resolution) {
+ drawMesh(X_GB, scale, color, (short) representation, MeshCircle, resolution);
+}
+
+void VisualizerProtocol::drawPolygonalMesh(const PolygonalMesh& mesh, const Transform& X_GM, const Vec3& scale, const Vec4& color, int representation) {
+ const void* impl = &mesh.getImpl();
+ map<const void*, unsigned short>::const_iterator iter = meshes.find(impl);
+
+ if (iter != meshes.end()) {
+ // This mesh was already cached; just reference it by index number.
+ drawMesh(X_GM, scale, color, (short)representation, iter->second, 0);
+ return;
+ }
+
+ // This is a new mesh, so we need to send it to the visualizer. Build lists
+ // of vertices and faces, triangulating as necessary.
+
+ vector<float> vertices;
+ vector<unsigned short> faces;
+ for (int i = 0; i < mesh.getNumVertices(); i++) {
+ Vec3 pos = mesh.getVertexPosition(i);
+ vertices.push_back((float) pos[0]);
+ vertices.push_back((float) pos[1]);
+ vertices.push_back((float) pos[2]);
+ }
+ for (int i = 0; i < mesh.getNumFaces(); i++) {
+ int numVert = mesh.getNumVerticesForFace(i);
+ if (numVert < 3)
+ continue; // Ignore it.
+ if (numVert == 3) {
+ faces.push_back((unsigned short) mesh.getFaceVertex(i, 0));
+ faces.push_back((unsigned short) mesh.getFaceVertex(i, 1));
+ faces.push_back((unsigned short) mesh.getFaceVertex(i, 2));
+ }
+ else if (numVert == 4) {
+ // Split it into two triangles.
+
+ faces.push_back((unsigned short) mesh.getFaceVertex(i, 0));
+ faces.push_back((unsigned short) mesh.getFaceVertex(i, 1));
+ faces.push_back((unsigned short) mesh.getFaceVertex(i, 2));
+ faces.push_back((unsigned short) mesh.getFaceVertex(i, 2));
+ faces.push_back((unsigned short) mesh.getFaceVertex(i, 3));
+ faces.push_back((unsigned short) mesh.getFaceVertex(i, 0));
+ }
+ else {
+ // Add a vertex at the center, then split it into triangles.
+
+ Vec3 center(0);
+ for (int j = 0; j < numVert; j++) {
+ Vec3 pos = mesh.getVertexPosition(mesh.getFaceVertex(i,j));
+ center += pos;
+ }
+ center /= numVert;
+ vertices.push_back((float) center[0]);
+ vertices.push_back((float) center[1]);
+ vertices.push_back((float) center[2]);
+ const unsigned newIndex = (unsigned)(vertices.size()/3-1);
+ for (int j = 0; j < numVert-1; j++) {
+ faces.push_back((unsigned short) mesh.getFaceVertex(i, j));
+ faces.push_back((unsigned short) mesh.getFaceVertex(i, j+1));
+ faces.push_back((unsigned short) newIndex);
+ }
+ }
+ }
+ SimTK_ERRCHK1_ALWAYS(vertices.size() <= 65535*3,
+ "VisualizerProtocol::drawPolygonalMesh()",
+ "Can't display a DecorativeMesh with more than 65535 vertices;"
+ " received one with %llu.", (unsigned long long)vertices.size());
+ SimTK_ERRCHK1_ALWAYS(faces.size() <= 65535*3,
+ "VisualizerProtocol::drawPolygonalMesh()",
+ "Can't display a DecorativeMesh with more than 65535 vertices;"
+ " received one with %llu.", (unsigned long long)faces.size());
+
+ const int index = NumPredefinedMeshes + (int)meshes.size();
+ SimTK_ERRCHK_ALWAYS(index <= 65535,
+ "VisualizerProtocol::drawPolygonalMesh()",
+ "Too many unique DecorativeMesh objects; max is 65535.");
+
+ meshes[impl] = (unsigned short)index; // insert new mesh
+ WRITE(outPipe, &DefineMesh, 1);
+ unsigned short numVertices = (unsigned)vertices.size()/3;
+ unsigned short numFaces = (unsigned)faces.size()/3;
+ WRITE(outPipe, &numVertices, sizeof(short));
+ WRITE(outPipe, &numFaces, sizeof(short));
+ WRITE(outPipe, &vertices[0], (unsigned)(vertices.size()*sizeof(float)));
+ WRITE(outPipe, &faces[0], (unsigned)(faces.size()*sizeof(short)));
+
+ drawMesh(X_GM, scale, color, (short) representation, index, 0);
+}
+
+void VisualizerProtocol::
+drawMesh(const Transform& X_GM, const Vec3& scale, const Vec4& color,
+ short representation, unsigned short meshIndex, unsigned short resolution)
+{
+ char command = (representation == DecorativeGeometry::DrawPoints
+ ? AddPointMesh
+ : (representation == DecorativeGeometry::DrawWireframe
+ ? AddWireframeMesh : AddSolidMesh));
+ WRITE(outPipe, &command, 1);
+ float buffer[13];
+ Vec3 rot = X_GM.R().convertRotationToBodyFixedXYZ();
+ buffer[0] = (float) rot[0];
+ buffer[1] = (float) rot[1];
+ buffer[2] = (float) rot[2];
+ buffer[3] = (float) X_GM.p()[0];
+ buffer[4] = (float) X_GM.p()[1];
+ buffer[5] = (float) X_GM.p()[2];
+ buffer[6] = (float) scale[0];
+ buffer[7] = (float) scale[1];
+ buffer[8] = (float) scale[2];
+ buffer[9] = (float) color[0];
+ buffer[10] = (float) color[1];
+ buffer[11] = (float) color[2];
+ buffer[12] = (float) color[3];
+ WRITE(outPipe, buffer, 13*sizeof(float));
+ unsigned short buffer2[2];
+ buffer2[0] = meshIndex;
+ buffer2[1] = resolution;
+ WRITE(outPipe, buffer2, 2*sizeof(unsigned short));
+}
+
+void VisualizerProtocol::
+drawLine(const Vec3& end1, const Vec3& end2, const Vec4& color, Real thickness)
+{
+ WRITE(outPipe, &AddLine, 1);
+ float buffer[10];
+ buffer[0] = (float) color[0];
+ buffer[1] = (float) color[1];
+ buffer[2] = (float) color[2];
+ buffer[3] = (float) thickness;
+ buffer[4] = (float) end1[0];
+ buffer[5] = (float) end1[1];
+ buffer[6] = (float) end1[2];
+ buffer[7] = (float) end2[0];
+ buffer[8] = (float) end2[1];
+ buffer[9] = (float) end2[2];
+ WRITE(outPipe, buffer, 10*sizeof(float));
+}
+
+void VisualizerProtocol::
+drawText(const Vec3& position, const Vec3& scale, const Vec4& color,
+ const string& string, bool faceCamera, bool isScreenText) {
+ SimTK_ERRCHK1_ALWAYS(string.size() <= 256,
+ "VisualizerProtocol::drawText()",
+ "Can't display DecorativeText longer than 256 characters;"
+ " received text of length %u.", (unsigned)string.size());
+ WRITE(outPipe, &AddText, 1);
+ float buffer[9];
+ buffer[0] = (float) position[0];
+ buffer[1] = (float) position[1];
+ buffer[2] = (float) position[2];
+ buffer[3] = (float) scale[0];
+ buffer[4] = (float) scale[1];
+ buffer[5] = (float) scale[2];
+ buffer[6] = (float) color[0];
+ buffer[7] = (float) color[1];
+ buffer[8] = (float) color[2];
+ WRITE(outPipe, buffer, 9*sizeof(float));
+ short face = (short)faceCamera;
+ WRITE(outPipe, &face, sizeof(short));
+ short screen = (short)isScreenText;
+ WRITE(outPipe, &screen, sizeof(short));
+ short length = (short)string.size();
+ WRITE(outPipe, &length, sizeof(short));
+ WRITE(outPipe, &string[0], length);
+}
+
+void VisualizerProtocol::
+drawCoords(const Transform& X_GF, const Vec3& axisLengths, const Vec4& color) {
+ WRITE(outPipe, &AddCoords, 1);
+ float buffer[12];
+ Vec3 rot = X_GF.R().convertRotationToBodyFixedXYZ();
+ buffer[0] = (float) rot[0];
+ buffer[1] = (float) rot[1];
+ buffer[2] = (float) rot[2];
+ buffer[3] = (float) X_GF.p()[0];
+ buffer[4] = (float) X_GF.p()[1];
+ buffer[5] = (float) X_GF.p()[2];
+ buffer[6] = (float) axisLengths[0];
+ buffer[7] = (float) axisLengths[1];
+ buffer[8] = (float) axisLengths[2];
+ buffer[9] = (float) color[0];
+ buffer[10]= (float) color[1];
+ buffer[11]= (float) color[2];
+ WRITE(outPipe, buffer, 12*sizeof(float));
+}
+
+void VisualizerProtocol::
+addMenu(const String& title, int id, const Array_<pair<String, int> >& items) {
+ pthread_mutex_lock(&sceneLock);
+ WRITE(outPipe, &DefineMenu, 1);
+ short titleLength = title.size();
+ WRITE(outPipe, &titleLength, sizeof(short));
+ WRITE(outPipe, title.c_str(), titleLength);
+ WRITE(outPipe, &id, sizeof(int));
+ short numItems = items.size();
+ WRITE(outPipe, &numItems, sizeof(short));
+ for (int i = 0; i < numItems; i++) {
+ int buffer[] = {items[i].second, items[i].first.size()};
+ WRITE(outPipe, buffer, 2*sizeof(int));
+ WRITE(outPipe, items[i].first.c_str(), items[i].first.size());
+ }
+ pthread_mutex_unlock(&sceneLock);
+}
+
+void VisualizerProtocol::
+addSlider(const String& title, int id, Real minVal, Real maxVal, Real value) {
+ pthread_mutex_lock(&sceneLock);
+ WRITE(outPipe, &DefineSlider, 1);
+ short titleLength = title.size();
+ WRITE(outPipe, &titleLength, sizeof(short));
+ WRITE(outPipe, title.c_str(), titleLength);
+ WRITE(outPipe, &id, sizeof(int));
+ float buffer[3];
+ buffer[0] = (float) minVal;
+ buffer[1] = (float) maxVal;
+ buffer[2] = (float) value;
+ WRITE(outPipe, buffer, 3*sizeof(float));
+ pthread_mutex_unlock(&sceneLock);
+}
+
+
+void VisualizerProtocol::setSliderValue(int id, Real newValue) const {
+ const float value = (float)newValue;
+ pthread_mutex_lock(&sceneLock);
+ WRITE(outPipe, &SetSliderValue, 1);
+ WRITE(outPipe, &id, sizeof(int));
+ WRITE(outPipe, &value, sizeof(float));
+ pthread_mutex_unlock(&sceneLock);
+}
+
+void VisualizerProtocol::setSliderRange(int id, Real newMin, Real newMax) const {
+ float buffer[2];
+ buffer[0] = (float)newMin; buffer[1] = (float)newMax;
+ pthread_mutex_lock(&sceneLock);
+ WRITE(outPipe, &SetSliderRange, 1);
+ WRITE(outPipe, &id, sizeof(int));
+ WRITE(outPipe, buffer, 2*sizeof(float));
+ pthread_mutex_unlock(&sceneLock);
+}
+
+void VisualizerProtocol::setWindowTitle(const String& title) const {
+ pthread_mutex_lock(&sceneLock);
+ WRITE(outPipe, &SetWindowTitle, 1);
+ short titleLength = title.size();
+ WRITE(outPipe, &titleLength, sizeof(short));
+ WRITE(outPipe, title.c_str(), titleLength);
+ pthread_mutex_unlock(&sceneLock);
+}
+
+void VisualizerProtocol::setMaxFrameRate(Real rate) const {
+ const float frameRate = (float)rate;
+ pthread_mutex_lock(&sceneLock);
+ WRITE(outPipe, &SetMaxFrameRate, 1);
+ WRITE(outPipe, &frameRate, sizeof(float));
+ pthread_mutex_unlock(&sceneLock);
+}
+
+
+void VisualizerProtocol::setBackgroundColor(const Vec3& color) const {
+ float buffer[3];
+ buffer[0] = (float)color[0];
+ buffer[1] = (float)color[1];
+ buffer[2] = (float)color[2];
+ pthread_mutex_lock(&sceneLock);
+ WRITE(outPipe, &SetBackgroundColor, 1);
+ WRITE(outPipe, buffer, 3*sizeof(float));
+ pthread_mutex_unlock(&sceneLock);
+}
+
+void VisualizerProtocol::setShowShadows(bool shouldShow) const {
+ const short show = (short)shouldShow; // 0 or 1
+ pthread_mutex_lock(&sceneLock);
+ WRITE(outPipe, &SetShowShadows, 1);
+ WRITE(outPipe, &show, sizeof(short));
+ pthread_mutex_unlock(&sceneLock);
+}
+
+void VisualizerProtocol::setShowFrameRate(bool shouldShow) const {
+ const short show = (short)shouldShow; // 0 or 1
+ pthread_mutex_lock(&sceneLock);
+ WRITE(outPipe, &SetShowFrameRate, 1);
+ WRITE(outPipe, &show, sizeof(short));
+ pthread_mutex_unlock(&sceneLock);
+}
+
+void VisualizerProtocol::setShowSimTime(bool shouldShow) const {
+ const short show = (short)shouldShow; // 0 or 1
+ pthread_mutex_lock(&sceneLock);
+ WRITE(outPipe, &SetShowSimTime, 1);
+ WRITE(outPipe, &show, sizeof(short));
+ pthread_mutex_unlock(&sceneLock);
+}
+
+void VisualizerProtocol::setShowFrameNumber(bool shouldShow) const {
+ const short show = (short)shouldShow; // 0 or 1
+ pthread_mutex_lock(&sceneLock);
+ WRITE(outPipe, &SetShowFrameNumber, 1);
+ WRITE(outPipe, &show, sizeof(short));
+ pthread_mutex_unlock(&sceneLock);
+}
+
+void VisualizerProtocol::setBackgroundType(Visualizer::BackgroundType type) const {
+ const short backgroundType = (short)type;
+ pthread_mutex_lock(&sceneLock);
+ WRITE(outPipe, &SetBackgroundType, 1);
+ WRITE(outPipe, &backgroundType, sizeof(short));
+ pthread_mutex_unlock(&sceneLock);
+}
+
+void VisualizerProtocol::setCameraTransform(const Transform& X_GC) const {
+ pthread_mutex_lock(&sceneLock);
+ WRITE(outPipe, &SetCamera, 1);
+ float buffer[6];
+ Vec3 rot = X_GC.R().convertRotationToBodyFixedXYZ();
+ buffer[0] = (float) rot[0];
+ buffer[1] = (float) rot[1];
+ buffer[2] = (float) rot[2];
+ buffer[3] = (float) X_GC.p()[0];
+ buffer[4] = (float) X_GC.p()[1];
+ buffer[5] = (float) X_GC.p()[2];
+ WRITE(outPipe, buffer, 6*sizeof(float));
+ pthread_mutex_unlock(&sceneLock);
+}
+
+void VisualizerProtocol::zoomCamera() const {
+ pthread_mutex_lock(&sceneLock);
+ WRITE(outPipe, &ZoomCamera, 1);
+ pthread_mutex_unlock(&sceneLock);
+}
+
+void VisualizerProtocol::lookAt(const Vec3& point, const Vec3& upDirection) const {
+ pthread_mutex_lock(&sceneLock);
+ WRITE(outPipe, &LookAt, 1);
+ float buffer[6];
+ buffer[0] = (float) point[0];
+ buffer[1] = (float) point[1];
+ buffer[2] = (float) point[2];
+ buffer[3] = (float) upDirection[0];
+ buffer[4] = (float) upDirection[1];
+ buffer[5] = (float) upDirection[2];
+ WRITE(outPipe, buffer, 6*sizeof(float));
+ pthread_mutex_unlock(&sceneLock);
+}
+
+void VisualizerProtocol::setFieldOfView(Real fov) const {
+ pthread_mutex_lock(&sceneLock);
+ WRITE(outPipe, &SetFieldOfView, 1);
+ float buffer[1];
+ buffer[0] = (float)fov;
+ WRITE(outPipe, buffer, sizeof(float));
+ pthread_mutex_unlock(&sceneLock);
+}
+
+void VisualizerProtocol::setClippingPlanes(Real near, Real far) const {
+ pthread_mutex_lock(&sceneLock);
+ WRITE(outPipe, &SetClipPlanes, 1);
+ float buffer[2];
+ buffer[0] = (float)near;
+ buffer[1] = (float)far;
+ WRITE(outPipe, buffer, 2*sizeof(float));
+ pthread_mutex_unlock(&sceneLock);
+}
+
+void VisualizerProtocol::
+setSystemUpDirection(const CoordinateDirection& upDir) {
+ pthread_mutex_lock(&sceneLock);
+ WRITE(outPipe, &SetSystemUpDirection, 1);
+ const unsigned char axis = (unsigned char)upDir.getAxis();
+ const signed char sign = (signed char)upDir.getDirection();
+ WRITE(outPipe, &axis, 1);
+ WRITE(outPipe, &sign, 1);
+ pthread_mutex_unlock(&sceneLock);
+}
+
+void VisualizerProtocol::setGroundHeight(Real height) {
+ pthread_mutex_lock(&sceneLock);
+ WRITE(outPipe, &SetGroundHeight, 1);
+ float heightBuffer = (float) height;
+ WRITE(outPipe, &heightBuffer, sizeof(float));
+ pthread_mutex_unlock(&sceneLock);
+}
+
+
diff --git a/Simbody/Visualizer/src/VisualizerProtocol.h b/Simbody/Visualizer/src/VisualizerProtocol.h
new file mode 100644
index 0000000..4231432
--- /dev/null
+++ b/Simbody/Visualizer/src/VisualizerProtocol.h
@@ -0,0 +1,165 @@
+#ifndef SimTK_SIMBODY_VISUALIZER_PROTOCOL_H_
+#define SimTK_SIMBODY_VISUALIZER_PROTOCOL_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) 2011-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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 "simbody/internal/common.h"
+#include "simbody/internal/Visualizer.h"
+#include <pthread.h>
+#include <utility>
+
+/** @file
+ * This file defines commands that are used for communication between the
+ * simulation application and the visualization GUI.
+ */
+
+// Increment this every time you make *any* change to the protocol;
+// we insist on an exact match.
+static const unsigned ProtocolVersion = 32;
+
+// The visualizer has several predefined cached meshes for common
+// shapes so that we don't have to send them. These are the mesh
+// indices for them; they must start with zero.
+static const unsigned short MeshBox = 0;
+static const unsigned short MeshEllipsoid = 1; // works for sphere
+static const unsigned short MeshCylinder = 2;
+static const unsigned short MeshCircle = 3;
+
+// This serves as the first index number for unique meshes that are
+// defined during this run.
+static const unsigned short NumPredefinedMeshes = 4;
+
+// Commands sent to the GUI.
+
+// This should always be command #1 so we can reliably check whether
+// we're talking to a compatible protocol.
+static const unsigned char StartupHandshake = 1;
+
+static const unsigned char StartOfScene = 2;
+static const unsigned char EndOfScene = 3;
+static const unsigned char AddSolidMesh = 4;
+static const unsigned char AddPointMesh = 5;
+static const unsigned char AddWireframeMesh = 6;
+static const unsigned char AddLine = 7;
+static const unsigned char AddText = 8;
+static const unsigned char AddCoords = 9;
+static const unsigned char DefineMesh = 10;
+static const unsigned char DefineMenu = 11;
+static const unsigned char DefineSlider = 12;
+static const unsigned char SetSliderValue = 13;
+static const unsigned char SetSliderRange = 14;
+static const unsigned char SetCamera = 15;
+static const unsigned char ZoomCamera = 16;
+static const unsigned char LookAt = 17;
+static const unsigned char SetFieldOfView = 18;
+static const unsigned char SetClipPlanes = 19;
+static const unsigned char SetSystemUpDirection = 20;
+static const unsigned char SetGroundHeight = 21;
+static const unsigned char SetWindowTitle = 22;
+static const unsigned char SetMaxFrameRate = 23;
+static const unsigned char SetBackgroundColor = 24;
+static const unsigned char SetShowShadows = 25;
+static const unsigned char SetBackgroundType = 26;
+static const unsigned char SetShowFrameRate = 27;
+static const unsigned char SetShowSimTime = 28;
+static const unsigned char SetShowFrameNumber = 29;
+static const unsigned char Shutdown = 30;
+
+
+// Events sent from the GUI back to the application.
+
+// This should always be command #1 so we can reliably check whether
+// we're talking to a compatible protocol.
+static const unsigned char ReturnHandshake = 1;
+
+static const unsigned char KeyPressed = 2;
+static const unsigned char MenuSelected = 3;
+static const unsigned char SliderMoved = 4;
+
+namespace SimTK {
+class VisualizerProtocol {
+public:
+ VisualizerProtocol(Visualizer& visualizer,
+ const Array_<String>& searchPath);
+ void shakeHandsWithGUI(int toGUIPipe, int fromGUIPipe);
+ void shutdownGUI();
+ void beginScene(Real simTime);
+ void finishScene();
+ void drawBox(const Transform& transform, const Vec3& scale,
+ const Vec4& color, int representation);
+ void drawEllipsoid(const Transform& transform, const Vec3& scale,
+ const Vec4& color, int representation,
+ unsigned short resolution);
+ void drawCylinder(const Transform& transform, const Vec3& scale,
+ const Vec4& color, int representation,
+ unsigned short resolution);
+ void drawCircle(const Transform& transform, const Vec3& scale,
+ const Vec4& color, int representation,
+ unsigned short resolution);
+ void drawPolygonalMesh(const PolygonalMesh& mesh,
+ const Transform& transform, const Vec3& scale,
+ const Vec4& color, int representation);
+ void drawLine(const Vec3& end1, const Vec3& end2, const
+ Vec4& color, Real thickness);
+ void drawText(const Vec3& position, const Vec3& scale, const Vec4& color,
+ const std::string& string, bool faceCamera, bool isScreenText);
+ void drawCoords(const Transform& transform, const Vec3& axisLengths,
+ const Vec4& color);
+
+ void addMenu(const String& title, int id,
+ const Array_<std::pair<String, int> >& items);
+ void addSlider(const String& title, int id, Real min, Real max, Real value);
+ void setSliderValue(int id, Real newValue) const;
+ void setSliderRange(int id, Real newMin, Real newMax) const;
+
+ void setSystemUpDirection(const CoordinateDirection& upDir);
+ void setGroundHeight(Real height);
+ void setWindowTitle(const String& title) const;
+ void setMaxFrameRate(Real rateInFPS) const;
+ void setBackgroundColor(const Vec3& color) const;
+ void setShowShadows(bool showShadows) const;
+ void setShowFrameRate(bool showFrameRate) const;
+ void setShowSimTime(bool showSimTime) const;
+ void setShowFrameNumber(bool showFrameNumber) const;
+ void setBackgroundType(Visualizer::BackgroundType type) const;
+ void setCameraTransform(const Transform& transform) const;
+ void zoomCamera() const;
+ void lookAt(const Vec3& point, const Vec3& upDirection) const;
+ void setFieldOfView(Real fov) const;
+ void setClippingPlanes(Real near, Real far) const;
+private:
+ void drawMesh(const Transform& transform, const Vec3& scale,
+ const Vec4& color, short representation,
+ unsigned short meshIndex, unsigned short resolution);
+ int outPipe;
+
+ // For user-defined meshes, map their unique memory addresses to the
+ // assigned visualizer cache index.
+ mutable std::map<const void*, unsigned short> meshes;
+ mutable pthread_mutex_t sceneLock;
+};
+}
+
+
+#endif // SimTK_SIMBODY_VISUALIZER_PROTOCOL_H_
diff --git a/Simbody/Visualizer/src/Visualizer_InputListener.cpp b/Simbody/Visualizer/src/Visualizer_InputListener.cpp
new file mode 100644
index 0000000..71874ed
--- /dev/null
+++ b/Simbody/Visualizer/src/Visualizer_InputListener.cpp
@@ -0,0 +1,246 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+This file contains the implementations of Visualizer::InputListener and any
+built-in concrete InputListeners, currently just Visualizer::InputSilo. **/
+
+#include "simbody/internal/common.h"
+#include "simbody/internal/Visualizer_InputListener.h"
+
+#include <deque>
+#include <utility>
+#include <pthread.h>
+
+using namespace SimTK;
+
+//==============================================================================
+// INPUT SILO IMPL
+//==============================================================================
+/* This is the private implementation object contained in a
+Visualizer::InputSilo handle. */
+
+class Visualizer::InputSilo::Impl {
+public:
+ Impl() : m_inputCount(0) {
+ pthread_mutex_init(&m_siloLock,0);
+ pthread_cond_init(&m_someInputAvailable,0);
+ pthread_cond_init(&m_keyHitAvailable,0);
+ pthread_cond_init(&m_menuPickAvailable,0);
+ pthread_cond_init(&m_sliderMoveAvailable,0);
+ }
+ ~Impl() {
+ pthread_cond_destroy(&m_sliderMoveAvailable);
+ pthread_cond_destroy(&m_menuPickAvailable);
+ pthread_cond_destroy(&m_keyHitAvailable);
+ pthread_cond_destroy(&m_someInputAvailable);
+ pthread_mutex_destroy(&m_siloLock);
+ }
+
+ void LOCK_silo() const {pthread_mutex_lock(&m_siloLock);}
+ void UNLOCK_silo() const {pthread_mutex_unlock(&m_siloLock);}
+
+ void WAIT_anyInput() const {pthread_cond_wait(&m_someInputAvailable,
+ &m_siloLock);}
+ void POST_anyInput() const {pthread_cond_signal(&m_someInputAvailable);}
+
+ void WAIT_keyHit() const {pthread_cond_wait(&m_keyHitAvailable,
+ &m_siloLock);}
+ void POST_keyHit() const {pthread_cond_signal(&m_keyHitAvailable);}
+
+ void WAIT_menuPick() const {pthread_cond_wait(&m_menuPickAvailable,
+ &m_siloLock);}
+ void POST_menuPick() const {pthread_cond_signal(&m_menuPickAvailable);}
+
+ void WAIT_sliderMove() const {pthread_cond_wait(&m_sliderMoveAvailable,
+ &m_siloLock);}
+ void POST_sliderMove() const {pthread_cond_signal(&m_sliderMoveAvailable);}
+
+ std::deque<std::pair<unsigned,unsigned> > m_keyHitSilo;
+ std::deque<std::pair<int,int> > m_menuPickSilo;
+ std::deque<std::pair<int,Real> > m_sliderMoveSilo;
+ unsigned m_inputCount;
+
+ mutable pthread_mutex_t m_siloLock;
+ mutable pthread_cond_t m_someInputAvailable; // signal on 1st input, any silo
+ mutable pthread_cond_t m_keyHitAvailable; // signal on 1st key in silo
+ mutable pthread_cond_t m_menuPickAvailable; // signal on 1st pick in silo
+ mutable pthread_cond_t m_sliderMoveAvailable; // signal on 1st move in silo
+};
+
+//==============================================================================
+// INPUT SILO
+//==============================================================================
+Visualizer::InputSilo::InputSilo() {m_impl = new InputSilo::Impl();}
+Visualizer::InputSilo::~InputSilo() {delete m_impl;}
+
+// Fast ... no locking required for this check.
+bool Visualizer::InputSilo::isAnyUserInput() const
+{ return getImpl().m_inputCount != 0; }
+
+// Hang until "anyInput" condition is signaled.
+void Visualizer::InputSilo::waitForAnyUserInput() const {
+ if (isAnyUserInput()) return;
+ const Impl& impl = getImpl();
+ impl.LOCK_silo();
+ while (!isAnyUserInput()) {impl.WAIT_anyInput();}
+ impl.UNLOCK_silo();
+}
+
+bool Visualizer::InputSilo::takeKeyHit(unsigned& key, unsigned& modifiers) {
+ if (!isAnyUserInput()) return false;
+ Impl& impl = updImpl(); bool gotOne;
+ impl.LOCK_silo();
+ if (impl.m_keyHitSilo.empty()) key=0, modifiers=0, gotOne=false;
+ else {
+ key = impl.m_keyHitSilo.front().first;
+ modifiers = impl.m_keyHitSilo.front().second;
+ impl.m_keyHitSilo.pop_front();
+ --impl.m_inputCount;
+ gotOne = true;
+ }
+ impl.UNLOCK_silo();
+ return gotOne;
+}
+
+void Visualizer::InputSilo::waitForKeyHit(unsigned& key, unsigned& modifiers) {
+ Impl& impl = updImpl();
+ impl.LOCK_silo();
+ while (!impl.m_keyHitSilo.size())
+ impl.WAIT_keyHit();
+ key = impl.m_keyHitSilo.front().first;
+ modifiers = impl.m_keyHitSilo.front().second;
+ impl.m_keyHitSilo.pop_front();
+ --impl.m_inputCount;
+ impl.UNLOCK_silo();
+}
+
+
+bool Visualizer::InputSilo::takeMenuPick(int& menuId, int& item) {
+ if (!isAnyUserInput()) return false;
+ Impl& impl = updImpl(); bool gotOne;
+ impl.LOCK_silo();
+ if (impl.m_menuPickSilo.empty()) item=0, gotOne=false;
+ else {
+ menuId = impl.m_menuPickSilo.front().first;
+ item = impl.m_menuPickSilo.front().second;
+ impl.m_menuPickSilo.pop_front();
+ --impl.m_inputCount;
+ gotOne = true;
+ }
+ impl.UNLOCK_silo();
+ return gotOne;
+}
+
+void Visualizer::InputSilo::waitForMenuPick(int& menuId, int& item) {
+ Impl& impl = updImpl();
+ impl.LOCK_silo();
+ while (!impl.m_menuPickSilo.size())
+ impl.WAIT_menuPick();
+ menuId = impl.m_menuPickSilo.front().first;
+ item = impl.m_menuPickSilo.front().second;
+ impl.m_menuPickSilo.pop_front();
+ --impl.m_inputCount;
+ impl.UNLOCK_silo();
+}
+
+bool Visualizer::InputSilo::takeSliderMove(int& slider, Real& value) {
+ if (!isAnyUserInput()) return false;
+ Impl& impl = updImpl(); bool gotOne;
+ impl.LOCK_silo();
+ if (impl.m_sliderMoveSilo.empty()) slider=0, value=NaN, gotOne=false;
+ else {
+ slider = impl.m_sliderMoveSilo.front().first;
+ value = impl.m_sliderMoveSilo.front().second;
+ impl.m_sliderMoveSilo.pop_front();
+ --impl.m_inputCount;
+ gotOne = true;
+ }
+ impl.UNLOCK_silo();
+ return gotOne;
+}
+
+void Visualizer::InputSilo::waitForSliderMove(int& slider, Real& value) {
+ Impl& impl = updImpl();
+ impl.LOCK_silo();
+ while (!impl.m_sliderMoveSilo.size())
+ impl.WAIT_sliderMove();
+ slider = impl.m_sliderMoveSilo.front().first;
+ value = impl.m_sliderMoveSilo.front().second;
+ impl.m_sliderMoveSilo.pop_front();
+ --impl.m_inputCount;
+ impl.UNLOCK_silo();
+}
+
+void Visualizer::InputSilo::clear() {
+ Impl& impl = updImpl();
+ impl.LOCK_silo();
+ impl.m_inputCount = 0;
+ impl.m_keyHitSilo.clear();
+ impl.m_menuPickSilo.clear();
+ impl.m_sliderMoveSilo.clear();
+ impl.UNLOCK_silo();
+}
+
+bool Visualizer::InputSilo::keyPressed(unsigned key, unsigned modifiers) {
+ Impl& impl = updImpl();
+ impl.LOCK_silo();
+ impl.m_keyHitSilo.push_back(std::make_pair(key,modifiers));
+ if (++impl.m_inputCount == 1)
+ impl.POST_anyInput(); // in case someone was waiting for any input
+ if (impl.m_keyHitSilo.size() == 1)
+ impl.POST_keyHit(); // a key hit is now available
+ impl.UNLOCK_silo();
+ return true;
+}
+bool Visualizer::InputSilo::menuSelected(int menu, int item) {
+ Impl& impl = updImpl();
+ impl.LOCK_silo();
+ impl.m_menuPickSilo.push_back(std::make_pair(menu,item));
+ if (++impl.m_inputCount == 1)
+ impl.POST_anyInput(); // in case someone was waiting for any input
+ if (impl.m_menuPickSilo.size() == 1)
+ impl.POST_menuPick(); // a menu pick is now available
+ impl.UNLOCK_silo();
+ return true;
+}
+
+// We optimize here for the common case that the same slider is moving
+// for a while -- in that case we just keep the most recent position.
+bool Visualizer::InputSilo::sliderMoved(int slider, Real value) {
+ Impl& impl = updImpl();
+ impl.LOCK_silo();
+ std::deque<std::pair<int,Real> >& silo = impl.m_sliderMoveSilo;
+ if (!silo.empty() && silo.front().first == slider)
+ silo.front().second = value; // just replace the value; count unchanged
+ else {
+ silo.push_back(std::make_pair(slider,value));
+ if (++impl.m_inputCount == 1)
+ impl.POST_anyInput(); // in case someone was waiting for any input
+ if (silo.size() == 1)
+ impl.POST_sliderMove(); // a slider move is now available
+ }
+ impl.UNLOCK_silo();
+ return true;
+}
+
diff --git a/Simbody/Visualizer/src/Visualizer_Reporter.cpp b/Simbody/Visualizer/src/Visualizer_Reporter.cpp
new file mode 100644
index 0000000..4be118f
--- /dev/null
+++ b/Simbody/Visualizer/src/Visualizer_Reporter.cpp
@@ -0,0 +1,74 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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 "simbody/internal/common.h"
+#include "simbody/internal/MultibodySystem.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "simbody/internal/Visualizer_Reporter.h"
+
+using namespace SimTK;
+
+class Visualizer::Reporter::Impl {
+public:
+ explicit Impl(const Visualizer& viz)
+ : handle(0), visualizer(viz) {}
+ explicit Impl(const MultibodySystem& system)
+ : handle(0), visualizer(system) {}
+
+ const Visualizer& getVisualizer() const {
+ return visualizer;
+ }
+
+ void handleEvent(const State& state) const {
+ visualizer.getSystem().realize(state, Stage::Acceleration);
+ visualizer.report(state);
+ }
+
+ Visualizer::Reporter* handle;
+ Visualizer visualizer; // shallow copy
+};
+
+Visualizer::Reporter::Reporter(const Visualizer& viz, Real reportInterval)
+: PeriodicEventReporter(reportInterval) {
+ impl = new Impl(viz);
+ updImpl().handle = this;
+}
+
+Visualizer::Reporter::Reporter(const MultibodySystem& sys, Real reportInterval)
+: PeriodicEventReporter(reportInterval) {
+ impl = new Impl(sys);
+ updImpl().handle = this;
+}
+
+Visualizer::Reporter::~Reporter() {
+ if (impl->handle == this)
+ delete impl;
+}
+
+const Visualizer& Visualizer::Reporter::getVisualizer() const {
+ return getImpl().getVisualizer();
+}
+
+void Visualizer::Reporter::handleEvent(const State& state) const {
+ getImpl().handleEvent(state);
+}
diff --git a/Simbody/doc/FrictionConstraints.docx b/Simbody/doc/FrictionConstraints.docx
new file mode 100644
index 0000000..f60b25e
Binary files /dev/null and b/Simbody/doc/FrictionConstraints.docx differ
diff --git a/Simbody/doc/HowToComputeMuscleMomentArm.docx b/Simbody/doc/HowToComputeMuscleMomentArm.docx
new file mode 100644
index 0000000..244ff2a
Binary files /dev/null and b/Simbody/doc/HowToComputeMuscleMomentArm.docx differ
diff --git a/Simbody/doc/SimbodyAdvancedProgrammingGuide.docx b/Simbody/doc/SimbodyAdvancedProgrammingGuide.docx
new file mode 100644
index 0000000..3863be5
Binary files /dev/null and b/Simbody/doc/SimbodyAdvancedProgrammingGuide.docx differ
diff --git a/Simbody/doc/SimbodyAdvancedProgrammingGuide.pdf b/Simbody/doc/SimbodyAdvancedProgrammingGuide.pdf
new file mode 100644
index 0000000..e6bb32e
Binary files /dev/null and b/Simbody/doc/SimbodyAdvancedProgrammingGuide.pdf differ
diff --git a/Simbody/doc/SimbodyAndMolmodelUserGuide.docx b/Simbody/doc/SimbodyAndMolmodelUserGuide.docx
new file mode 100644
index 0000000..c1d6e58
Binary files /dev/null and b/Simbody/doc/SimbodyAndMolmodelUserGuide.docx differ
diff --git a/Simbody/doc/SimbodyAndMolmodelUserGuide.pdf b/Simbody/doc/SimbodyAndMolmodelUserGuide.pdf
new file mode 100644
index 0000000..4514af1
Binary files /dev/null and b/Simbody/doc/SimbodyAndMolmodelUserGuide.pdf differ
diff --git a/Simbody/doc/SimbodyTheoryManual.docx b/Simbody/doc/SimbodyTheoryManual.docx
new file mode 100644
index 0000000..7056e4a
Binary files /dev/null and b/Simbody/doc/SimbodyTheoryManual.docx differ
diff --git a/Simbody/doc/SimbodyTheoryManual.pdf b/Simbody/doc/SimbodyTheoryManual.pdf
new file mode 100644
index 0000000..e2dec2e
Binary files /dev/null and b/Simbody/doc/SimbodyTheoryManual.pdf differ
diff --git a/Simbody/doc/images/MobilizerTerminology.png b/Simbody/doc/images/MobilizerTerminology.png
new file mode 100644
index 0000000..e8dd19e
Binary files /dev/null and b/Simbody/doc/images/MobilizerTerminology.png differ
diff --git a/Simbody/examples/Bricard_EVEN_PART.obj b/Simbody/examples/Bricard_EVEN_PART.obj
new file mode 100644
index 0000000..25fa4aa
--- /dev/null
+++ b/Simbody/examples/Bricard_EVEN_PART.obj
@@ -0,0 +1,36 @@
+#Obj File : Exported From RapidForm (INUS Technology Co. Ltd.)
+
+mtllib EVEN_PART.mtl
+
+v 2.000000 0.010000 0.000000
+v 0.000000 0.010000 0.000000
+v 2.000000 0.000000 0.000000
+v 0.000000 0.000000 0.000000
+v -0.000000 0.010000 -1.000000
+v -0.000000 -0.000000 -1.000000
+v 2.000000 -1.000000 0.010000
+v -0.000000 0.000000 0.010000
+v 2.000000 -1.000000 -0.000000
+v 2.000000 0.000000 0.010000
+
+g EVEN_PART
+usemtl EVEN_PART
+s
+
+f 1 2 3
+f 3 2 4
+f 5 1 6
+f 6 1 3
+f 2 5 4
+f 4 5 6
+f 1 5 2
+f 4 6 3
+f 7 8 9
+f 9 8 4
+f 10 7 3
+f 3 7 9
+f 8 10 4
+f 4 10 3
+f 7 10 8
+f 4 3 9
+
diff --git a/Simbody/examples/Bricard_ODD_PART.obj b/Simbody/examples/Bricard_ODD_PART.obj
new file mode 100644
index 0000000..51aa721
--- /dev/null
+++ b/Simbody/examples/Bricard_ODD_PART.obj
@@ -0,0 +1,36 @@
+#Obj File : Exported From RapidForm (INUS Technology Co. Ltd.)
+
+mtllib ODD_PART.mtl
+
+v 0.000000 -0.010000 0.000000
+v 2.000000 -0.010000 0.000000
+v 0.000000 0.000000 0.000000
+v 2.000000 0.000000 0.000000
+v 0.000000 -0.010000 -1.000000
+v 0.000000 0.000000 -1.000000
+v -0.000000 -0.000000 0.010000
+v 2.000000 1.000000 0.010000
+v 2.000000 1.000000 -0.000000
+v 2.000000 0.000000 0.010000
+
+g ODD_PART
+usemtl ODD_PART
+s
+
+f 1 2 3
+f 3 2 4
+f 5 1 6
+f 6 1 3
+f 2 5 4
+f 4 5 6
+f 1 5 2
+f 4 6 3
+f 7 8 3
+f 3 8 9
+f 10 7 4
+f 4 7 3
+f 8 10 9
+f 9 10 4
+f 7 10 8
+f 9 4 3
+
diff --git a/Simbody/examples/CMakeLists.txt b/Simbody/examples/CMakeLists.txt
new file mode 100644
index 0000000..51eb03d
--- /dev/null
+++ b/Simbody/examples/CMakeLists.txt
@@ -0,0 +1,107 @@
+# Generate and install examples.
+#
+# This is boilerplate code for generating a set of executables, one per
+# .cpp file in an "examples" subdirectory. These are intended to
+# be installed with the library but we don't handle installation
+# here. Unlike the similar boilerplate code in the "tests"
+# directory (but like the "tests/adhoc" boilerplate), this does
+# not generate CMake ADD_TEST commands so these will never run
+# automatically.
+#
+# For IDEs that can deal with PROJECT_LABEL properties (at least
+# Visual Studio) the projects for building each of these adhoc
+# executables will be labeled "Example - TheExampleName" if a file
+# TheExampleName.cpp is found in this directory.
+#
+# We check the BUILD_TESTS_AND_EXAMPLES_{SHARED,STATIC} variables to determine
+# whether to build dynamically linked, statically linked, or both
+# versions of the executable.
+
+
+IF(WIN32)
+ SET(EXAMPLES_INSTALL_BIN ${CMAKE_INSTALL_DOCDIR}/examples/bin/)
+ELSE()
+ SET(EXAMPLES_INSTALL_BIN ${CMAKE_INSTALL_FULL_LIBDIR}/simbody/examples/)
+ SET(EXAMPLES_SYMLINK_BIN ${CMAKE_INSTALL_DOCDIR}/examples/)
+ENDIF()
+SET(EXAMPLES_INSTALL_SRC ${CMAKE_INSTALL_DOCDIR}/examples/src/)
+
+FILE(GLOB EXAMPLES "*.cpp")
+FOREACH(EX_PROG ${EXAMPLES})
+ GET_FILENAME_COMPONENT(EX_SRC ${EX_PROG} NAME)
+ GET_FILENAME_COMPONENT(EX_ROOT ${EX_PROG} NAME_WE)
+
+ IF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+ # Link with shared library
+ ADD_EXECUTABLE(${EX_ROOT} ${EX_PROG})
+ IF(GUI_NAME)
+ ADD_DEPENDENCIES(${EX_ROOT} ${GUI_NAME})
+ ENDIF()
+ SET_TARGET_PROPERTIES(${EX_ROOT}
+ PROPERTIES
+ PROJECT_LABEL "Example - ${EX_ROOT}")
+ TARGET_LINK_LIBRARIES(${EX_ROOT} ${TEST_SHARED_TARGET})
+
+ INSTALL(TARGETS ${EX_ROOT} DESTINATION ${EXAMPLES_INSTALL_BIN})
+ ENDIF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+
+ IF (BUILD_STATIC_LIBRARIES AND BUILD_TESTS_AND_EXAMPLES_STATIC)
+ # Link with static library
+ SET(EX_STATIC ${EX_ROOT}Static)
+ ADD_EXECUTABLE(${EX_STATIC} ${EX_PROG})
+ IF(GUI_NAME)
+ ADD_DEPENDENCIES(${EX_STATIC} ${GUI_NAME})
+ ENDIF()
+ SET_TARGET_PROPERTIES(${EX_STATIC}
+ PROPERTIES
+ COMPILE_FLAGS "-DSimTK_USE_STATIC_LIBRARIES"
+ PROJECT_LABEL "Example - ${EX_STATIC}")
+ TARGET_LINK_LIBRARIES(${EX_STATIC} ${TEST_STATIC_TARGET})
+ # Don't install static examples
+ ENDIF()
+
+ENDFOREACH()
+IF(NOT WIN32)
+ EXECUTE_PROCESS(COMMAND "${CMAKE_COMMAND}" -E create_symlink
+ ${EXAMPLES_INSTALL_BIN}
+ ${CMAKE_CURRENT_BINARY_DIR}/bin
+ )
+ INSTALL(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/bin
+ DESTINATION ${EXAMPLES_SYMLINK_BIN}
+ )
+ENDIF()
+
+# Copy extra files associated with these examples
+# to the binary directory that will be the working
+# directory when the tests are run; and include them in
+# the examples installation. ".obj" and ".vtp" files contain geometry.
+# ".sdf" are Gazebo input files.
+FILE(GLOB EXTRA_FILES "*.obj" "*.vtp" "*.sdf")
+FOREACH(XTRA ${EXTRA_FILES})
+ GET_FILENAME_COMPONENT(XTRA_ROOT ${XTRA} NAME)
+ CONFIGURE_FILE(${XTRA}
+ ${CMAKE_CURRENT_BINARY_DIR}/${XTRA_ROOT} COPYONLY)
+ INSTALL(FILES ${XTRA} DESTINATION ${EXAMPLES_INSTALL_SRC})
+ INSTALL(FILES ${XTRA} DESTINATION ${EXAMPLES_INSTALL_BIN})
+ENDFOREACH()
+
+# install source for examples
+FILE(GLOB SRC_FILES "*.cpp" "*.h")
+INSTALL(FILES ${SRC_FILES} DESTINATION ${EXAMPLES_INSTALL_SRC})
+
+# install .txt files except CMakeLists.txt
+FILE(GLOB TXT_FILES "*.txt")
+FOREACH(TXTF ${TXT_FILES})
+ IF(NOT "${TXTF}" MATCHES "CMakeLists.txt")
+ INSTALL(FILES ${TXTF} DESTINATION ${EXAMPLES_INSTALL_SRC})
+ ENDIF()
+ENDFOREACH()
+
+# install the installed version of CMakeLists.txt
+INSTALL(FILES InstalledCMakeLists.txt DESTINATION ${EXAMPLES_INSTALL_SRC}
+ RENAME CMakeLists.txt)
+
+# install Makefile
+INSTALL(FILES "${PROJECT_SOURCE_DIR}/examples/Makefile"
+ DESTINATION ${EXAMPLES_INSTALL_SRC})
+
diff --git a/Simbody/examples/ChainExample.cpp b/Simbody/examples/ChainExample.cpp
new file mode 100644
index 0000000..d458960
--- /dev/null
+++ b/Simbody/examples/ChainExample.cpp
@@ -0,0 +1,333 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): Chain Example *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* Simbody ChainExample
+This example demonstrates how to use the Simbody Visualizer to display and
+interact with a real time simulation. It shows the use of sliders to control
+"wind", uses a FrameController to track a body with the camera and show some
+feedback to the user, and adds a menu to the display. A description of all user
+input received is written to the console, and some inputs are used to control
+the simulation. */
+
+#include "Simbody.h"
+
+#include <cstdio>
+#include <iostream>
+
+using namespace SimTK;
+
+static const int NBodies = 100;
+const Real FrameRate = 30;
+const Real TimeScale = 1; // i.e., 2 -> 2X real time
+
+// We call this "wind" but it is implemented with Force::Gravity.
+static const int GravityX=1, GravityY=2, GravityZ=3, GravityMag=4; // sliders
+static const Real GravityDefault=10, GravityMax=20;
+
+// A FrameController is called by the Visualizer just prior to rendering a
+// frame. Here we'll point the camera and add some geometry showing the direction
+// and magnitude of gravity (which the user can change via sliders).
+class MyFrameController : public Visualizer::FrameController {
+public:
+ MyFrameController(const SimbodyMatterSubsystem& matter,
+ MobilizedBodyIndex whichBody, // tracked with camera
+ const Force::Gravity& gravity)
+ : m_matter(matter), m_whichBody(whichBody), m_gravity(gravity) {}
+
+ virtual void generateControls(const Visualizer& viz,
+ const State& state,
+ Array_<DecorativeGeometry>& geometry)
+ {
+ const MobilizedBody& mobod = m_matter.getMobilizedBody(m_whichBody);
+ const Transform& X_GB = mobod.getBodyTransform(state);
+ const UnitVec3& downDir = m_gravity.getDownDirection(state);
+ const Real gmag = m_gravity.getMagnitude(state);
+
+ // Point the camera at the chosen body.
+ viz.pointCameraAt(X_GB.p(), Vec3(0,1,0));
+
+ // Show gravity as a fat green line.
+ geometry.push_back(DecorativeLine(Vec3(0), gmag*downDir)
+ .setColor(Green).setLineThickness(4).setBodyId(0));
+ }
+
+private:
+ const SimbodyMatterSubsystem& m_matter;
+ const MobilizedBodyIndex m_whichBody;
+ const Force::Gravity& m_gravity;
+};
+
+// This is a custom InputListener. We'll register it prior to the InputSilo so
+// that we can intercept all input and say something about it. No input processing
+// is done here other than that, and we pass on everything we receive down the
+// chain to the next listener (which will be an InputSilo in this case).
+class MyListener : public Visualizer::InputListener {
+public:
+ // Pass in the menu strings just so we can translate the index back
+ // to a string to print out for testing.
+ MyListener(const Array_< std::pair<std::string, int> >& menu1,
+ const Array_< std::pair<std::string, int> >& menu2)
+ : m_menu1(menu1), m_menu2(menu2) {}
+
+ ~MyListener() {}
+
+ virtual bool keyPressed(unsigned key, unsigned modifier) {
+ String mod;
+ if (modifier&ControlIsDown) mod += "CTRL ";
+ if (modifier&ShiftIsDown) mod += "SHIFT ";
+ if (modifier&AltIsDown) mod += "ALT ";
+
+ const char* nm = "NoNickname";
+ switch(key) {
+ case KeyEsc: nm="ESC"; break;
+ case KeyDelete: nm="DEL"; break;
+ case KeyRightArrow: nm="Right"; break;
+ case KeyLeftArrow: nm="Left"; break;
+ case KeyUpArrow: nm="Up"; break;
+ case KeyDownArrow: nm="Down"; break;
+ case KeyEnter: nm="ENTER"; break;
+ case KeyF1: nm="F1"; break;
+ case KeyF12: nm="F12"; break;
+ case 'a': nm="lower a"; break;
+ case 'Z': nm="upper Z"; break;
+ case '}': nm="right brace"; break;
+ }
+ if (modifier&IsSpecialKey)
+ std::cout << "Listener saw special key hit: "
+ << mod << " key=" << key << " glut=" << (key & ~SpecialKeyOffset);
+ else
+ std::cout << "Listener saw ordinary key hit: "
+ << mod << char(key) << " (" << (int)key << ")";
+ std::cout << " " << nm << std::endl;
+
+ return false; // key passed on
+ }
+
+ virtual bool menuSelected(int menuId, int item) {
+ std::cout << "Listener sees pick of menu " << menuId << " item " << item << ": ";
+ Array_< std::pair<std::string, int> >& menu = menuId==1 ? m_menu1 : m_menu2;
+ for (unsigned i=0; i < menu.size(); ++i)
+ if (menu[i].second==item)
+ std::cout << menu[i].first;
+ std::cout << std::endl;
+ return false; // menu click passed on
+ }
+
+ virtual bool sliderMoved(int whichSlider, Real value) {
+ printf("Listener sees slider %d now at %g\n", whichSlider, value);
+ return false; // slider move passed on
+ }
+
+private:
+ Array_< std::pair<std::string, int> > m_menu1, m_menu2;
+};
+
+// This is a periodic event handler that interrupts the simulation on a regular
+// basis to poll the InputSilo for user input. If there has been some, process it.
+class UserInputHandler : public PeriodicEventHandler {
+public:
+ UserInputHandler(Visualizer& viz,
+ Visualizer::InputSilo& silo,
+ const Force::Gravity& gravity,
+ Real interval)
+ : PeriodicEventHandler(interval), m_viz(viz), m_silo(silo), m_gravity(gravity) {}
+
+ virtual void handleEvent(State& state, Real accuracy,
+ bool& shouldTerminate) const
+ {
+ while (m_silo.isAnyUserInput()) {
+ unsigned key, modifiers;
+ int whichMenu, menuItem;
+ int whichSlider; Real sliderValue;
+
+ while (m_silo.takeKeyHit(key,modifiers)) {
+ if (key == Visualizer::InputListener::KeyEsc) {
+ printf("User hit ESC!!\n");
+ shouldTerminate = true;
+ m_silo.clear();
+ return;
+ }
+ printf("Handler sees key=%u, modifiers=%u\n",key,modifiers);
+ }
+
+ while (m_silo.takeMenuPick(whichMenu, menuItem)) {
+ printf("Handler sees menu %d, pick %d\n", whichMenu, menuItem);
+ }
+
+ while (m_silo.takeSliderMove(whichSlider, sliderValue)) {
+ if (whichSlider == GravityMag) {
+ m_gravity.setMagnitude(state, sliderValue);
+ continue;
+ }
+ Vec3 gdir = m_gravity.getDownDirection(state);
+ Real remaining = std::sqrt(std::max(Real(0), 1-square(sliderValue)));
+ CoordinateAxis axis = CoordinateAxis(whichSlider-GravityX);
+ CoordinateAxis prev = axis.getPreviousAxis();
+ CoordinateAxis next = axis.getNextAxis();
+ Vec2 other(gdir[prev], gdir[next]);
+ if (other.norm() >= SignificantReal) other *= (remaining/other.norm());
+ gdir[axis]=sliderValue; gdir[prev]=other[0]; gdir[next]=other[1];
+ if (gdir.norm() < SignificantReal) gdir[next] = 1;
+ m_viz.setSliderValue(GravityX+prev, gdir[prev]);
+ m_viz.setSliderValue(GravityX+next, gdir[next]);
+ m_gravity.setDownDirection(state, gdir);
+ }
+ }
+ }
+
+private:
+ Visualizer& m_viz;
+ Visualizer::InputSilo& m_silo;
+ const Force::Gravity& m_gravity;
+};
+
+int main() {
+ try {
+ // Create the system.
+
+ printf("\n\n************\n");
+ printf( "ESC to quit\n");
+ printf( "************\n\n");
+
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::Gravity gravity(forces, matter, YAxis, GravityDefault); // up! (weird)
+ Force::GlobalDamper(forces, matter, 7);
+ Body::Rigid pendulumBody[2]; // solid, translucent
+ pendulumBody[0].setDefaultRigidBodyMassProperties(MassProperties(1.0, Vec3(0), Inertia(1)));
+ pendulumBody[0].addDecoration(Transform(), DecorativeSphere(Real(0.49)).setOpacity(1));
+ pendulumBody[1].setDefaultRigidBodyMassProperties(MassProperties(1.0, Vec3(0), Inertia(1)));
+ pendulumBody[1].addDecoration(Transform(), DecorativeSphere(Real(0.49)).setOpacity(.5));
+ MobilizedBody lastBody = matter.Ground();
+ for (int i = 0; i < NBodies; ++i) {
+ MobilizedBody::Ball pendulum(lastBody, Transform(Vec3(0)),
+ pendulumBody[i%2], Transform(Vec3(0, 1, 0))); // alternate solid, translucent
+ lastBody = pendulum;
+ }
+
+ // Attach the last body back to ground.
+ Constraint::Ball(matter.Ground(), Vec3(NBodies/2,0,0), lastBody, Vec3(0));
+
+ //matter.setShowDefaultGeometry(false);
+ Visualizer viz(system);
+
+ viz.setWindowTitle("This is the so-called 'ChainExample'.");
+
+ // Add a menu, just for fun.
+ Array_< std::pair<std::string,int> > menu1, menu2;
+ menu1.push_back(std::make_pair("One", 1));
+ menu1.push_back(std::make_pair("Top/SubA/first", 2));
+ menu1.push_back(std::make_pair("Top/SubA/second", 3));
+ menu1.push_back(std::make_pair("Top/SubB/only", 4));
+ menu1.push_back(std::make_pair("Two", 5));
+ viz.addMenu("Test Menu", 1, menu1);
+
+ // And another one, to check the id handling.
+ menu2.push_back(std::make_pair("One", 1));
+ menu2.push_back(std::make_pair("Two", 2));
+ viz.addMenu("More", 2, menu2);
+
+ MyListener* listener = new MyListener(menu1,menu2);
+ Visualizer::InputSilo* silo = new Visualizer::InputSilo();
+ viz.addInputListener(listener); // order matters here
+ viz.addInputListener(silo);
+
+ // Tell the frame controller to track the middle body.
+ viz.addFrameController(new MyFrameController(matter,
+ MobilizedBodyIndex(NBodies/2), gravity));
+
+ viz.setRealTimeScale(TimeScale);
+ //viz.setDesiredBufferLengthInSec(.15);
+ viz.setDesiredFrameRate(FrameRate);
+ //viz.setMode(Visualizer::Sampling);
+ //viz.setMode(Visualizer::PassThrough);
+ viz.setMode(Visualizer::RealTime);
+
+ viz.setCameraTransform(Vec3(0,NBodies/4,2*NBodies));
+
+ system.addEventHandler
+ (new UserInputHandler(viz,*silo, gravity, Real(0.1))); // check input every 100ms
+
+ // Report visualization frames.
+ Visualizer::Reporter* vr = new Visualizer::Reporter(viz, TimeScale/FrameRate);
+ system.addEventReporter(vr);
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ Random::Gaussian random;
+ for (int i = 0; i < state.getNQ(); ++i)
+ state.updQ()[i] = random.getValue();
+
+ // Use the Assembler to satisfy the loop-closing constraint.
+ Assembler assembler(system);
+ std::cout << "ASSEMBLING ... start configuration shown\n";
+ viz.report(state);
+ std::cout << " Type something to continue:\n"; getchar();
+ double asmRTstart=realTime(), asmCPUstart=cpuTime();
+ assembler.addReporter(*vr);
+ assembler.setSystemConstraintsWeight(1);
+ Visualizer::Mode oldMode = viz.getMode();
+ viz.setMode(Visualizer::PassThrough);
+ assembler.assemble(state);
+ viz.setMode(oldMode);
+ printf("...ASSEMBLED in %gs, cpu=%gs. Final configuration shown\n",
+ realTime()-asmRTstart, cpuTime()-asmCPUstart);
+ viz.report(state);
+ std::cout << " Type something to continue:\n"; getchar();
+
+ // Simulate it.
+
+ // Add sliders to control gravity. They will display from bottom up.
+ // Joy Ku thought calling this "wind direction" makes more sense.
+ viz.addSlider("Wind Z", 3, -1, 1, 0);
+ viz.addSlider("Wind Y", 2, -1, 1, 1);
+ viz.addSlider("Wind X", 1, -1, 1, 0);
+ viz.addSlider("Wind Mag", 4, 0, GravityMax, GravityDefault);
+
+ //RungeKutta3Integrator integ(system);
+ RungeKuttaMersonIntegrator integ(system);
+ //RungeKuttaFeldbergIntegrator integ(system);
+ //CPodesIntegrator integ(system);
+ integ.setAccuracy(Real(1e-2));
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+
+ double cpuStart = cpuTime();
+ double realStart = realTime();
+ //ts.stepTo(10);
+ ts.stepTo(Infinity); // user must hit ESC to stop sim
+ std::cout << "cpu time: "<<cpuTime()-cpuStart<< std::endl;
+ std::cout << "real time: "<<realTime()-realStart<< std::endl;
+ std::cout << "steps: "<<integ.getNumStepsTaken()<< std::endl;
+ vr->getVisualizer().dumpStats(std::cout);
+
+ std::cout << "Type something to quit: "; getchar();
+
+ } catch (const std::exception& exc) {
+ std::cout << "EXCEPTION: " << exc.what() << std::endl;
+ }
+}
diff --git a/Simbody/examples/ExampleAmysIKProblem.cpp b/Simbody/examples/ExampleAmysIKProblem.cpp
new file mode 100644
index 0000000..d860d28
--- /dev/null
+++ b/Simbody/examples/ExampleAmysIKProblem.cpp
@@ -0,0 +1,2719 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Amy's IK Problem *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* This is a test of repeated Assembly analysis, also known as
+InverseKinematics (IK). The problem is from a 56 dof OpenSim model built by Amy
+Silder in Scott Delp's lab. The model was read from various OpenSim data
+files and machine-translated into a standalone C++ Simbody model during the
+development of the Assembler study. There are "markers" (distinguished points)
+on many of the bodies, and many frames of laboratory data giving sequentially-
+observed positions of these markers. The idea is to solve for the set of q's
+in each frame that pose the bodies so that their attached marker positions
+are a best fit for the observed data. Sequential frames are expected to be
+spatially close.
+
+There is some commented-out code below for playing with various ways
+of handling constraints.
+*/
+
+
+#include "Simbody.h"
+
+#include <cstdio>
+#include <exception>
+
+using std::cout; using std::cin; using std::endl;
+
+using namespace SimTK;
+
+static int getNumFrames();
+static int getNumObservations();
+static Real getFrameTime(int i);
+static const char* const* getObservationOrder();
+static ArrayViewConst_<Vec3> getFrame(int i);
+
+int main() {
+ try
+ { // Create the system.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+
+// BODY ground
+Markers& markers = *new Markers();
+DecorativeGeometry markerDecoration = DecorativeSphere(0.1).setColor(Blue);
+MobilizedBody& mobod_ground = matter.updGround();
+// END BODY ground
+
+
+// Kludge to get the initial view large enough.
+mobod_ground.addBodyDecoration(Vec3(-1,-1,-1), markerDecoration);
+mobod_ground.addBodyDecoration(Vec3(1,1,1), markerDecoration);
+
+// BODY loadcell1
+Body::Rigid body_loadcell1(MassProperties(0,Vec3(0),Inertia(0,0,0)));
+// Joint name is gnd_loadcell1
+MobilizedBody::Free mobod_loadcell1(mobod_ground,Vec3(0), body_loadcell1,Vec3(0));
+// END BODY loadcell1
+
+// BODY loadcell2
+Body::Rigid body_loadcell2(MassProperties(0,Vec3(0),Inertia(0,0,0)));
+// Joint name is gnd_loadcell2
+MobilizedBody::Free mobod_loadcell2(mobod_ground,Vec3(0), body_loadcell2,Vec3(0));
+// END BODY loadcell2
+
+// BODY pelvis
+Body::Rigid body_pelvis(MassProperties(8.3775,Vec3(-0.0893,-0.025796,0),Inertia(0.0595677,0.128085,0.139644,-0.0192983,0,0)));
+body_pelvis.addDecoration(Vec3(-.1,0,0),DecorativeEllipsoid(Vec3(.1,.06,.15)).setColor(Cyan));
+// Joint name is gnd_pelvis
+MobilizedBody::Free mobod_pelvis(mobod_ground,Vec3(0), body_pelvis,Vec3(0));
+markers.addMarker("S2",mobod_pelvis,Vec3(-0.2,0,0),1);
+body_pelvis.addDecoration(Vec3(-0.2,0,0),markerDecoration);
+markers.addMarker("R.ASIS",mobod_pelvis,Vec3(0.01,0,0.128),1);
+body_pelvis.addDecoration(Vec3(0.01,0,0.128),markerDecoration);
+markers.addMarker("R.Iliac",mobod_pelvis,Vec3(-0.08,0.06,0.15),1);
+body_pelvis.addDecoration(Vec3(-0.08,0.06,0.15),markerDecoration);
+markers.addMarker("R.PSIS",mobod_pelvis,Vec3(-0.2,0,0.055),1);
+body_pelvis.addDecoration(Vec3(-0.2,0,0.055),markerDecoration);
+markers.addMarker("L.ASIS",mobod_pelvis,Vec3(0.01,0,-0.128),1);
+body_pelvis.addDecoration(Vec3(0.01,0,-0.128),markerDecoration);
+markers.addMarker("L.Iliac",mobod_pelvis,Vec3(-0.08,0.06,-0.15),1);
+body_pelvis.addDecoration(Vec3(-0.08,0.06,-0.15),markerDecoration);
+markers.addMarker("L.PSIS",mobod_pelvis,Vec3(-0.2,0,-0.055),1);
+body_pelvis.addDecoration(Vec3(-0.2,0,-0.055),markerDecoration);
+markers.addMarker("R_HJC",mobod_pelvis,Vec3(-0.08,-0.0824,0.0785),1);
+body_pelvis.addDecoration(Vec3(-0.08,-0.0824,0.0785),markerDecoration);
+markers.addMarker("L_HJC",mobod_pelvis,Vec3(-0.08,-0.0824,-0.0785),1);
+body_pelvis.addDecoration(Vec3(-0.08,-0.0824,-0.0785),markerDecoration);
+// END BODY pelvis
+
+// BODY torso
+Body::Rigid body_torso(MassProperties(24.2175,Vec3(0.0145,0.256438,0),Inertia(2.03337,0.281964,2.19416,-0.0900492,0,0)));
+body_torso.addDecoration(Vec3(0,.25,0), DecorativeBrick(Vec3(.08,.3,.165)).setColor(Green).setOpacity(.2));
+// Joint name is pelvis_torso
+MobilizedBody::Ball mobod_torso(mobod_pelvis,Vec3(-0.0872,0.0331,0), body_torso,Vec3(0));
+markers.addMarker("R.Clavicle",mobod_torso,Vec3(0.035,0.43,0.025),1);
+body_torso.addDecoration(Vec3(0.035,0.43,0.025),markerDecoration);
+markers.addMarker("L.Clavicle",mobod_torso,Vec3(0.035,0.43,-0.025),1);
+body_torso.addDecoration(Vec3(0.035,0.43,-0.025),markerDecoration);
+markers.addMarker("L.Scapula",mobod_torso,Vec3(-0.12,-0.1,0.08),1);
+body_torso.addDecoration(Vec3(-0.12,-0.1,0.08),markerDecoration);
+markers.addMarker("R.Shoulder",mobod_torso,Vec3(-0.0488,0.47065,0.165),1);
+body_torso.addDecoration(Vec3(-0.0488,0.47065,0.165),markerDecoration);
+markers.addMarker("L.Shoulder",mobod_torso,Vec3(-0.0488,0.47065,-0.165),1);
+body_torso.addDecoration(Vec3(-0.0488,0.47065,-0.165),markerDecoration);
+// END BODY torso
+
+// BODY neckhead
+Body::Rigid body_neckhead(MassProperties(5.205,Vec3(0.0288,0.161501,0),Inertia(0.166231,0.0252372,0.168272,-0.0242096,0,0)));
+body_neckhead.addDecoration(Vec3(0.0288,0.161501,0), DecorativeSphere(.12).setColor(Yellow));
+// Joint name is torso_neckhead
+MobilizedBody::Ball mobod_neckhead(mobod_torso,Vec3(-0.0253,0.44895,0), body_neckhead,Vec3(0));
+markers.addMarker("C7",mobod_neckhead,Vec3(0),1);
+body_neckhead.addDecoration(Vec3(0),markerDecoration);
+// END BODY neckhead
+
+// BODY humerus_r
+Body::Rigid body_humerus_r(MassProperties(2.0325,Vec3(0,-0.164502,0),Inertia(0.0669473,0.004121,0.0684103)));
+body_humerus_r.addDecoration(Vec3(0,-.15,0), DecorativeEllipsoid(Vec3(.05,.3,.05)).setColor(Yellow));
+// Joint name is acromial_r
+MobilizedBody::Ball mobod_humerus_r(mobod_torso,Vec3(-0.0281,0.41645,0.199), body_humerus_r,Vec3(0));
+markers.addMarker("R.Bicep",mobod_humerus_r,Vec3(0.04,-0.15,0),1);
+body_humerus_r.addDecoration(Vec3(0.04,-0.15,0),markerDecoration);
+markers.addMarker("R.Elbow",mobod_humerus_r,Vec3(-0.01,-0.281,0.04),1);
+body_humerus_r.addDecoration(Vec3(-0.01,-0.281,0.04),markerDecoration);
+markers.addMarker("R.MElbow",mobod_humerus_r,Vec3(0,-0.281,-0.04),1);
+body_humerus_r.addDecoration(Vec3(0,-0.281,-0.04),markerDecoration);
+// END BODY humerus_r
+
+// BODY ulna_r
+Body::Rigid body_ulna_r(MassProperties(0.6075,Vec3(0,-0.120525,0),Inertia(0.0117867,0.000618,0.0120377)));
+body_ulna_r.addDecoration(Vec3(0,-.15,0), DecorativeEllipsoid(Vec3(.03,.3,.03)).setColor(Yellow));
+// Joint name is elbow_r
+MobilizedBody::Pin mobod_ulna_r(mobod_humerus_r,Vec3(0.013144,-0.286273,-0.009595), body_ulna_r,Vec3(0));
+markers.addMarker("R.Forearm",mobod_ulna_r,Vec3(0,-0.15,0),1);
+body_ulna_r.addDecoration(Vec3(0,-0.15,0),markerDecoration);
+// END BODY ulna_r
+
+// BODY radius_r
+Body::Rigid body_radius_r(MassProperties(0.6075,Vec3(0,-0.120525,0),Inertia(0.0117867,0.000618,0.0120377)));
+// Joint name is radioulnar_r
+MobilizedBody::Pin mobod_radius_r(mobod_ulna_r,Vec3(-0.006727,-0.013007,0.026083), body_radius_r,Vec3(0));
+markers.addMarker("R.Wrist",mobod_radius_r,Vec3(-0.025,-0.245,0.006),1);
+body_radius_r.addDecoration(Vec3(-0.025,-0.245,0.006),markerDecoration);
+// END BODY radius_r
+
+// BODY hand_r
+Body::Rigid body_hand_r(MassProperties(0.4575,Vec3(0,-0.068095,0),Inertia(0.0030134,0.000547,0.0034614)));
+// Joint name is radius_hand_r
+MobilizedBody::Pin mobod_hand_r(mobod_radius_r,Vec3(-0.008797,-0.235841,0.01361), body_hand_r,Vec3(0));
+// END BODY hand_r
+
+// BODY humerus_l
+Body::Rigid body_humerus_l(MassProperties(2.0325,Vec3(0,-0.164502,0),Inertia(0.0669473,0.004121,0.0684103)));
+body_humerus_l.addDecoration(Vec3(0,-.15,0), DecorativeEllipsoid(Vec3(.05,.3,.05)).setColor(Yellow));
+// Joint name is acromial_l
+MobilizedBody::Ball mobod_humerus_l(mobod_torso,Vec3(-0.0281,0.41645,-0.199), body_humerus_l,Vec3(0));
+markers.addMarker("L.Bicep",mobod_humerus_l,Vec3(0.04,-0.15,0),1);
+body_humerus_l.addDecoration(Vec3(0.04,-0.15,0),markerDecoration);
+markers.addMarker("L.Elbow",mobod_humerus_l,Vec3(0.01,-0.281,-0.04),1);
+body_humerus_l.addDecoration(Vec3(0.01,-0.281,-0.04),markerDecoration);
+markers.addMarker("L.MElbow",mobod_humerus_l,Vec3(-0.02,-0.281,0.04),1);
+body_humerus_l.addDecoration(Vec3(-0.02,-0.281,0.04),markerDecoration);
+// END BODY humerus_l
+
+// BODY ulna_l
+Body::Rigid body_ulna_l(MassProperties(0.6075,Vec3(0,-0.120525,0),Inertia(0.0117867,0.000618,0.0120377)));
+body_ulna_l.addDecoration(Vec3(0,-.15,0), DecorativeEllipsoid(Vec3(.03,.3,.03)).setColor(Yellow));
+// Joint name is elbow_l
+MobilizedBody::Pin mobod_ulna_l(mobod_humerus_l,Vec3(0.013144,-0.286273,0.009595), body_ulna_l,Vec3(0));
+markers.addMarker("L.Forearm",mobod_ulna_l,Vec3(0,-0.15,0),1);
+body_ulna_l.addDecoration(Vec3(0,-0.15,0),markerDecoration);
+// END BODY ulna_l
+
+// BODY radius_l
+Body::Rigid body_radius_l(MassProperties(0.6075,Vec3(0,-0.120525,0),Inertia(0.0117867,0.000618,0.0120377)));
+// Joint name is radioulnar_l
+MobilizedBody::Pin mobod_radius_l(mobod_ulna_l,Vec3(-0.006727,-0.013007,-0.026083), body_radius_l,Vec3(0));
+markers.addMarker("L.Wrist",mobod_radius_l,Vec3(-0.025,-0.245,-0.006),1);
+body_radius_l.addDecoration(Vec3(-0.025,-0.245,-0.006),markerDecoration);
+// END BODY radius_l
+
+// BODY hand_l
+Body::Rigid body_hand_l(MassProperties(0.4575,Vec3(0,-0.068095,0),Inertia(0.0030134,0.000547,0.0034614)));
+// Joint name is radius_hand_l
+MobilizedBody::Pin mobod_hand_l(mobod_radius_l,Vec3(-0.008797,-0.235841,-0.01361), body_hand_l,Vec3(0));
+// END BODY hand_l
+
+// BODY femur_r
+Body::Rigid body_femur_r(MassProperties(10.5225,Vec3(0,-0.162162,0),Inertia(0.455313,0.036634,0.455313)));
+body_femur_r.addDecoration(Vec3(0,-.2,0), DecorativeEllipsoid(Vec3(.08,.2,.1)).setColor(Yellow));
+// Joint name is hip_r
+MobilizedBody::Ball mobod_femur_r(mobod_pelvis,Vec3(-0.08,-0.0824,0.0785), body_femur_r,Vec3(0));
+markers.addMarker("R.Knee",mobod_femur_r,Vec3(0,-0.404,0.05),1);
+body_femur_r.addDecoration(Vec3(0,-0.404,0.05),markerDecoration);
+markers.addMarker("R.MKnee",mobod_femur_r,Vec3(0,-0.404,-0.05),1);
+body_femur_r.addDecoration(Vec3(0,-0.404,-0.05),markerDecoration);
+markers.addMarker("R.TH1",mobod_femur_r,Vec3(0.0179,-0.224,0.1147),1);
+body_femur_r.addDecoration(Vec3(0.0179,-0.224,0.1147),markerDecoration);
+markers.addMarker("R.TH2",mobod_femur_r,Vec3(0.0179,-0.264,0.0647),1);
+body_femur_r.addDecoration(Vec3(0.0179,-0.264,0.0647),markerDecoration);
+markers.addMarker("R.TH3",mobod_femur_r,Vec3(0.0179,-0.264,0.0647),1);
+body_femur_r.addDecoration(Vec3(0.0179,-0.264,0.0647),markerDecoration);
+markers.addMarker("R.TH4",mobod_femur_r,Vec3(0.08,-0.324,0.0047),1);
+body_femur_r.addDecoration(Vec3(0.08,-0.324,0.0047),markerDecoration);
+// END BODY femur_r
+
+// BODY tibia_r
+Body::Rigid body_tibia_r(MassProperties(3.2475,Vec3(0,-0.171713,0),Inertia(0.125753,0.005157,0.126985)));
+body_tibia_r.addDecoration(Vec3(0,-.2,0), DecorativeEllipsoid(Vec3(.05,.2,.07)).setColor(Yellow));
+// Joint name is knee_r
+MobilizedBody::Pin mobod_tibia_r(mobod_femur_r,Vec3(0,-.4,0), body_tibia_r,Vec3(0));
+markers.addMarker("R.Ankle",mobod_tibia_r,Vec3(-0.005,-0.41,0.053),1);
+body_tibia_r.addDecoration(Vec3(-0.005,-0.41,0.053),markerDecoration);
+markers.addMarker("R.MAnkle",mobod_tibia_r,Vec3(0.006,-0.3888,-0.038),1);
+body_tibia_r.addDecoration(Vec3(0.006,-0.3888,-0.038),markerDecoration);
+markers.addMarker("R.SH1",mobod_tibia_r,Vec3(0.0104,-0.2322,0.0748),1);
+body_tibia_r.addDecoration(Vec3(0.0104,-0.2322,0.0748),markerDecoration);
+markers.addMarker("R.SH2",mobod_tibia_r,Vec3(0.0125,-0.3196,0.06),1);
+body_tibia_r.addDecoration(Vec3(0.0125,-0.3196,0.06),markerDecoration);
+markers.addMarker("R.SH3",mobod_tibia_r,Vec3(0.0125,-0.3196,0.06),1);
+body_tibia_r.addDecoration(Vec3(0.0125,-0.3196,0.06),markerDecoration);
+markers.addMarker("R.SH4",mobod_tibia_r,Vec3(0.0125,-0.3196,0.06),1);
+body_tibia_r.addDecoration(Vec3(0.0125,-0.3196,0.06),markerDecoration);
+// END BODY tibia_r
+
+// BODY patella_r
+Body::Rigid body_patella_r(MassProperties(0.0975,Vec3(0.0018,0.0264,0),Inertia(7.09536e-005,1.53159e-005,8.32695e-005,-4.6332e-006,0,0)));
+// Joint name is tib_pat_r
+MobilizedBody::/*FourDOF*/Cartesian mobod_patella_r(mobod_tibia_r,Vec3(0), body_patella_r,Vec3(0));
+// END BODY patella_r
+
+// BODY talus_r
+Body::Rigid body_talus_r(MassProperties(0.045,Vec3(0.0055,0.0023,0),Inertia(3.23805e-006,2.36125e-006,4.5993e-006,-5.6925e-007,0,0)));
+// Joint name is ankle_r
+MobilizedBody::Pin mobod_talus_r(mobod_tibia_r,Vec3(0,-0.426,0), body_talus_r,Vec3(0));
+// END BODY talus_r
+
+// BODY foot_r
+Body::Rigid body_foot_r(MassProperties(0.885,Vec3(0.101865,0.0156,0),Inertia(0.00112437,0.0114972,0.0120816,-0.00140635,0,0)));
+// Joint name is subtalar_r
+MobilizedBody::Pin mobod_foot_r(mobod_talus_r,Vec3(-0.04877,-0.04195,0.00792), body_foot_r,Vec3(0));
+markers.addMarker("R.Toe",mobod_foot_r,Vec3(0.19,0.018,0),1);
+body_foot_r.addDecoration(Vec3(0.19,0.018,0),markerDecoration);
+markers.addMarker("R.MT1",mobod_foot_r,Vec3(0.2,0.005,-0.045),1);
+body_foot_r.addDecoration(Vec3(0.2,0.005,-0.045),markerDecoration);
+markers.addMarker("R.MT5",mobod_foot_r,Vec3(0.15,0.005,0.055),1);
+body_foot_r.addDecoration(Vec3(0.15,0.005,0.055),markerDecoration);
+markers.addMarker("R.Heel",mobod_foot_r,Vec3(-0.03,0.018,-0.01),1);
+body_foot_r.addDecoration(Vec3(-0.03,0.018,-0.01),markerDecoration);
+// END BODY foot_r
+
+// BODY toes_r
+Body::Rigid body_toes_r(MassProperties(0.0975,Vec3(0.0307,-0.0026,0.0105),Inertia(0.000111408,0.000357642,0.000386552,7.78245e-006,-3.14291e-005,2.66175e-006)));
+// Joint name is toes_r
+MobilizedBody::Pin mobod_toes_r(mobod_foot_r,Vec3(0.1768,-0.002,0.00108), body_toes_r,Vec3(0));
+// END BODY toes_r
+
+// BODY femur_l
+Body::Rigid body_femur_l(MassProperties(10.5225,Vec3(0,-0.162162,0),Inertia(0.455313,0.036634,0.455313)));
+body_femur_l.addDecoration(Vec3(0,-.2,0), DecorativeEllipsoid(Vec3(.08,.2,.1)).setColor(Yellow));
+// Joint name is hip_l
+MobilizedBody::Ball mobod_femur_l(mobod_pelvis,Vec3(-0.08,-0.0824,-0.0785), body_femur_l,Vec3(0));
+markers.addMarker("L.Knee",mobod_femur_l,Vec3(0,-0.404,-0.05),1);
+body_femur_l.addDecoration(Vec3(0,-0.404,-0.05),markerDecoration);
+markers.addMarker("L.MKnee",mobod_femur_l,Vec3(0,-0.404,0.05),1);
+body_femur_l.addDecoration(Vec3(0,-0.404,0.05),markerDecoration);
+markers.addMarker("L.TH1",mobod_femur_l,Vec3(0.0179,-0.224,-0.1147),1);
+body_femur_l.addDecoration(Vec3(0.0179,-0.224,-0.1147),markerDecoration);
+markers.addMarker("L.TH2",mobod_femur_l,Vec3(0.0179,-0.224,-0.1147),1);
+body_femur_l.addDecoration(Vec3(0.0179,-0.224,-0.1147),markerDecoration);
+markers.addMarker("L.TH3",mobod_femur_l,Vec3(0.0179,-0.264,-0.0647),1);
+body_femur_l.addDecoration(Vec3(0.0179,-0.264,-0.0647),markerDecoration);
+markers.addMarker("L.TH4",mobod_femur_l,Vec3(0.08,-0.324,-0.0047),1);
+body_femur_l.addDecoration(Vec3(0.08,-0.324,-0.0047),markerDecoration);
+// END BODY femur_l
+
+// BODY tibia_l
+Body::Rigid body_tibia_l(MassProperties(3.2475,Vec3(0,-0.171713,0),Inertia(0.125753,0.005157,0.126985)));
+body_tibia_l.addDecoration(Vec3(0,-.2,0), DecorativeEllipsoid(Vec3(.05,.2,.07)).setColor(Yellow));
+// Joint name is knee_l
+MobilizedBody::Pin mobod_tibia_l(mobod_femur_l,Vec3(0,-.4,0), body_tibia_l,Vec3(0));
+markers.addMarker("L.Ankle",mobod_tibia_l,Vec3(-0.005,-0.41,-0.053),1);
+body_tibia_l.addDecoration(Vec3(-0.005,-0.41,-0.053),markerDecoration);
+markers.addMarker("L.MAnkle",mobod_tibia_l,Vec3(0.006,-0.3888,0.038),1);
+body_tibia_l.addDecoration(Vec3(0.006,-0.3888,0.038),markerDecoration);
+markers.addMarker("L.SH1",mobod_tibia_l,Vec3(0.0104,-0.2322,-0.0748),1);
+body_tibia_l.addDecoration(Vec3(0.0104,-0.2322,-0.0748),markerDecoration);
+markers.addMarker("L.SH2",mobod_tibia_l,Vec3(0.0425,-0.3596,0),1);
+body_tibia_l.addDecoration(Vec3(0.0425,-0.3596,0),markerDecoration);
+markers.addMarker("L.SH3",mobod_tibia_l,Vec3(0.0125,-0.3196,-0.06),1);
+body_tibia_l.addDecoration(Vec3(0.0125,-0.3196,-0.06),markerDecoration);
+markers.addMarker("L.SH4",mobod_tibia_l,Vec3(0.0125,-0.3196,-0.06),1);
+body_tibia_l.addDecoration(Vec3(0.0125,-0.3196,-0.06),markerDecoration);
+// END BODY tibia_l
+
+// BODY patella_l
+Body::Rigid body_patella_l(MassProperties(0.0975,Vec3(0.0018,0.0264,0),Inertia(7.09536e-005,1.53159e-005,8.32695e-005,-4.6332e-006,0,0)));
+// Joint name is tib_pat_l
+MobilizedBody::/*FourDOF*/Cartesian mobod_patella_l(mobod_tibia_l,Vec3(0), body_patella_l,Vec3(0));
+// END BODY patella_l
+
+// BODY talus_l
+Body::Rigid body_talus_l(MassProperties(0.045,Vec3(0.0055,0.0023,0),Inertia(3.23805e-006,2.36125e-006,4.5993e-006,-5.6925e-007,0,0)));
+// Joint name is ankle_l
+MobilizedBody::Pin mobod_talus_l(mobod_tibia_l,Vec3(0,-0.426,0), body_talus_l,Vec3(0));
+// END BODY talus_l
+
+// BODY foot_l
+Body::Rigid body_foot_l(MassProperties(0.885,Vec3(0.101865,0.0156,0),Inertia(0.00112437,0.0114972,0.0120816,-0.00140635,0,0)));
+// Joint name is subtalar_l
+MobilizedBody::Pin mobod_foot_l(mobod_talus_l,Vec3(-0.04877,-0.04195,-0.00792), body_foot_l,Vec3(0));
+markers.addMarker("L.Toe",mobod_foot_l,Vec3(0.19,0.018,0),1);
+body_foot_l.addDecoration(Vec3(0.19,0.018,0),markerDecoration);
+markers.addMarker("L.Heel",mobod_foot_l,Vec3(-0.03,0.018,0.01),1);
+body_foot_l.addDecoration(Vec3(-0.03,0.018,0.01),markerDecoration);
+markers.addMarker("L.MT1",mobod_foot_l,Vec3(0.2,0.005,0.045),1);
+body_foot_l.addDecoration(Vec3(0.2,0.005,0.045),markerDecoration);
+markers.addMarker("L.MT5",mobod_foot_l,Vec3(0.15,0.005,-0.055),1);
+body_foot_l.addDecoration(Vec3(0.15,0.005,-0.055),markerDecoration);
+// END BODY foot_l
+
+// BODY toes_l
+Body::Rigid body_toes_l(MassProperties(0.0975,Vec3(0.0307,-0.0026,-0.0105),Inertia(0.000111408,0.000357642,0.000386552,7.78245e-006,3.14291e-005,-2.66175e-006)));
+// Joint name is toes_l
+MobilizedBody::Pin mobod_toes_l(mobod_foot_l,Vec3(0.1768,-0.002,-0.00108), body_toes_l,Vec3(0));
+// END BODY toes_l
+
+// Here are some constraints you can try. If you have these enabled,
+// you should experiment with treating these as constraints or as
+// weighted errors merged with the IK objective. See call below for
+// ik.setSystemConstraintsWeight(wt) where wt==Infinity is the default
+// and handles these as constraints; wt==finite value treats constraint
+// satisfaction as part of the objective.
+
+//Constraint::Rod(mobod_tibia_l, mobod_tibia_r, 2*.25);
+//Constraint::Rod(mobod_tibia_l, mobod_hand_r, .25);
+
+ matter.setShowDefaultGeometry(false);
+ Visualizer viz(system);
+ system.addEventReporter(new Visualizer::Reporter(viz, 0.1));
+
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+
+ // Show initial configuration
+ viz.report(state);
+ State tempState = state;
+ matter.setUseEulerAngles(tempState, true);
+ system.realizeModel(tempState);
+ system.realize(tempState, Stage::Position);
+ cout << "INITIAL CONFIGURATION\n";
+ cout << tempState.getNU() << " dofs, "
+ << tempState.getNQErr() << " constraints.\n";
+
+ cout << "Type any character to continue:\n";
+ getchar();
+
+
+ Assembler ik(system);
+ ik.adoptAssemblyGoal(&markers);
+
+ //ik.adoptAssemblyGoal(new QValue(mobod_tibia_r, MobilizerQIndex(0),
+ // Pi/2), 1);
+
+
+ // See comment above near the Constraints. If you use this
+ // weight the constraints will be violated somewhat but the
+ // IK will run much faster.
+ //ik.setSystemConstraintsWeight(2);
+
+ // This is the default treatment of constraints.
+ //ik.setSystemConstraintsWeight(Infinity);
+
+ ik.lockMobilizer(mobod_patella_l);
+ ik.lockMobilizer(mobod_patella_r);
+ ik.lockMobilizer(mobod_loadcell1);
+ ik.lockMobilizer(mobod_loadcell2);
+ //ik.restrictQ(mobod_tibia_r, MobilizerQIndex(0),
+ // -10*Pi/180, 10*Pi/180);
+
+ markers.defineObservationOrder(getNumObservations(), getObservationOrder());
+ markers.moveAllObservations(getFrame(0)); // initial observed locations
+ state.setTime(getFrameTime(0));
+ //ik.setForceNumericalGradient(true);
+ //ik.setForceNumericalJacobian(true);
+
+ ik.initialize(state);
+ printf("UNASSEMBLED CONFIGURATION (err=%g, cost=%g, qerr=%g)\n",
+ ik.calcCurrentErrorNorm(), ik.calcCurrentGoal(),
+ max(abs(ik.getInternalState().getQErr())));
+ cout << "num freeQs: " << ik.getNumFreeQs() << endl;
+
+ const Real Accuracy = 1e-3;
+ ik.setAccuracy(Accuracy);
+ ik.assemble(); // solve first frame.
+ ik.updateFromInternalState(state);
+ viz.report(state);
+ printf("ASSEMBLED CONFIGURATION (acc=%g tol=%g err=%g, cost=%g, qerr=%g)\n",
+ ik.getAccuracyInUse(), ik.getErrorToleranceInUse(),
+ ik.calcCurrentErrorNorm(), ik.calcCurrentGoal(),
+ max(abs(ik.getInternalState().getQErr())));
+ printf("# initializations=%d\n", ik.getNumInitializations());
+ printf("# assembly steps: %d\n", ik.getNumAssemblySteps());
+ printf(" evals: goal=%d grad=%d error=%d jac=%d\n",
+ ik.getNumGoalEvals(), ik.getNumGoalGradientEvals(),
+ ik.getNumErrorEvals(), ik.getNumErrorJacobianEvals());
+
+ cout << "Type any character to continue:\n";
+ getchar();
+
+ const int NSteps = getNumFrames();
+ const int NToSkip = 4; // show every nth frame
+ const double startReal = realTime(), startCPU = cpuTime();
+ for (int f=1; f < NSteps; ++f) {
+ markers.moveAllObservations(getFrame(f));
+ // update internal state to match new observed locations
+ ik.track(getFrameTime(f));
+ if ((f%NToSkip)==0) {
+ ik.updateFromInternalState(state);
+ viz.report(state);
+ }
+ }
+
+ cout << "ASSEMBLED " << NSteps-1 << " steps in " <<
+ cpuTime()-startCPU << " CPU s, " << realTime()-startReal << " REAL s\n";
+
+ printf("FINAL CONFIGURATION (acc=%g tol=%g err=%g, cost=%g, qerr=%g)\n",
+ ik.getAccuracyInUse(), ik.getErrorToleranceInUse(),
+ ik.calcCurrentErrorNorm(), ik.calcCurrentGoal(),
+ max(abs(ik.getInternalState().getQErr())));
+
+ const double oons = Real(1)/ik.getNumAssemblySteps();
+ printf("# initializations=%d\n", ik.getNumInitializations());
+ printf("# assembly steps: %d\n", ik.getNumAssemblySteps());
+ printf(" avg evals: goal=%g grad=%g error=%g jac=%g\n",
+ ik.getNumGoalEvals()*oons, ik.getNumGoalGradientEvals()*oons,
+ ik.getNumErrorEvals()*oons, ik.getNumErrorJacobianEvals()*oons);
+
+
+ cout << "DONE ASSEMBLING\n";
+ viz.report(state);
+
+ cout << "Type any character to continue:\n";
+ getchar();
+
+ } catch (const std::exception& e) {
+ std::printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+
+ } catch (...) {
+ std::printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+ return 0;
+}
+
+/* Below here is the marker observation data at 200Hz. */
+
+static const int NObservations = 44;
+static const int NFrames = 2105;
+static Real frameTimes[NFrames] = {
+/*0*/0,0.005,0.01,0.015,0.02,0.025,0.03,0.035,0.04,0.045,0.05,0.055,0.06,0.065,0.07,0.075,0.08,0.085,0.09,0.095,
+/*20*/0.1,0.105,0.11,0.115,0.12,0.125,0.13,0.135,0.14,0.145,0.15,0.155,0.16,0.165,0.17,0.175,0.18,0.185,0.19,0.195,
+/*40*/0.2,0.205,0.21,0.215,0.22,0.225,0.23,0.235,0.24,0.245,0.25,0.255,0.26,0.265,0.27,0.275,0.28,0.285,0.29,0.295,
+/*60*/0.3,0.305,0.31,0.315,0.32,0.325,0.33,0.335,0.34,0.345,0.35,0.355,0.36,0.365,0.37,0.375,0.38,0.385,0.39,0.395,
+/*80*/0.4,0.405,0.41,0.415,0.42,0.425,0.43,0.435,0.44,0.445,0.45,0.455,0.46,0.465,0.47,0.475,0.48,0.485,0.49,0.495,
+/*100*/0.5,0.505,0.51,0.515,0.52,0.525,0.53,0.535,0.54,0.545,0.55,0.555,0.56,0.565,0.57,0.575,0.58,0.585,0.59,0.595,
+/*120*/0.6,0.605,0.61,0.615,0.62,0.625,0.63,0.635,0.64,0.645,0.65,0.655,0.66,0.665,0.67,0.675,0.68,0.685,0.69,0.695,
+/*140*/0.7,0.705,0.71,0.715,0.72,0.725,0.73,0.735,0.74,0.745,0.75,0.755,0.76,0.765,0.77,0.775,0.78,0.785,0.79,0.795,
+/*160*/0.8,0.805,0.81,0.815,0.82,0.825,0.83,0.835,0.84,0.845,0.85,0.855,0.86,0.865,0.87,0.875,0.88,0.885,0.89,0.895,
+/*180*/0.9,0.905,0.91,0.915,0.92,0.925,0.93,0.935,0.94,0.945,0.95,0.955,0.96,0.965,0.97,0.975,0.98,0.985,0.99,0.995,
+/*200*/1,1.005,1.01,1.015,1.02,1.025,1.03,1.035,1.04,1.045,1.05,1.055,1.06,1.065,1.07,1.075,1.08,1.085,1.09,1.095,
+/*220*/1.1,1.105,1.11,1.115,1.12,1.125,1.13,1.135,1.14,1.145,1.15,1.155,1.16,1.165,1.17,1.175,1.18,1.185,1.19,1.195,
+/*240*/1.2,1.205,1.21,1.215,1.22,1.225,1.23,1.235,1.24,1.245,1.25,1.255,1.26,1.265,1.27,1.275,1.28,1.285,1.29,1.295,
+/*260*/1.3,1.305,1.31,1.315,1.32,1.325,1.33,1.335,1.34,1.345,1.35,1.355,1.36,1.365,1.37,1.375,1.38,1.385,1.39,1.395,
+/*280*/1.4,1.405,1.41,1.415,1.42,1.425,1.43,1.435,1.44,1.445,1.45,1.455,1.46,1.465,1.47,1.475,1.48,1.485,1.49,1.495,
+/*300*/1.5,1.505,1.51,1.515,1.52,1.525,1.53,1.535,1.54,1.545,1.55,1.555,1.56,1.565,1.57,1.575,1.58,1.585,1.59,1.595,
+/*320*/1.6,1.605,1.61,1.615,1.62,1.625,1.63,1.635,1.64,1.645,1.65,1.655,1.66,1.665,1.67,1.675,1.68,1.685,1.69,1.695,
+/*340*/1.7,1.705,1.71,1.715,1.72,1.725,1.73,1.735,1.74,1.745,1.75,1.755,1.76,1.765,1.77,1.775,1.78,1.785,1.79,1.795,
+/*360*/1.8,1.805,1.81,1.815,1.82,1.825,1.83,1.835,1.84,1.845,1.85,1.855,1.86,1.865,1.87,1.875,1.88,1.885,1.89,1.895,
+/*380*/1.9,1.905,1.91,1.915,1.92,1.925,1.93,1.935,1.94,1.945,1.95,1.955,1.96,1.965,1.97,1.975,1.98,1.985,1.99,1.995,
+/*400*/2,2.005,2.01,2.015,2.02,2.025,2.03,2.035,2.04,2.045,2.05,2.055,2.06,2.065,2.07,2.075,2.08,2.085,2.09,2.095,
+/*420*/2.1,2.105,2.11,2.115,2.12,2.125,2.13,2.135,2.14,2.145,2.15,2.155,2.16,2.165,2.17,2.175,2.18,2.185,2.19,2.195,
+/*440*/2.2,2.205,2.21,2.215,2.22,2.225,2.23,2.235,2.24,2.245,2.25,2.255,2.26,2.265,2.27,2.275,2.28,2.285,2.29,2.295,
+/*460*/2.3,2.305,2.31,2.315,2.32,2.325,2.33,2.335,2.34,2.345,2.35,2.355,2.36,2.365,2.37,2.375,2.38,2.385,2.39,2.395,
+/*480*/2.4,2.405,2.41,2.415,2.42,2.425,2.43,2.435,2.44,2.445,2.45,2.455,2.46,2.465,2.47,2.475,2.48,2.485,2.49,2.495,
+/*500*/2.5,2.505,2.51,2.515,2.52,2.525,2.53,2.535,2.54,2.545,2.55,2.555,2.56,2.565,2.57,2.575,2.58,2.585,2.59,2.595,
+/*520*/2.6,2.605,2.61,2.615,2.62,2.625,2.63,2.635,2.64,2.645,2.65,2.655,2.66,2.665,2.67,2.675,2.68,2.685,2.69,2.695,
+/*540*/2.7,2.705,2.71,2.715,2.72,2.725,2.73,2.735,2.74,2.745,2.75,2.755,2.76,2.765,2.77,2.775,2.78,2.785,2.79,2.795,
+/*560*/2.8,2.805,2.81,2.815,2.82,2.825,2.83,2.835,2.84,2.845,2.85,2.855,2.86,2.865,2.87,2.875,2.88,2.885,2.89,2.895,
+/*580*/2.9,2.905,2.91,2.915,2.92,2.925,2.93,2.935,2.94,2.945,2.95,2.955,2.96,2.965,2.97,2.975,2.98,2.985,2.99,2.995,
+/*600*/3,3.005,3.01,3.015,3.02,3.025,3.03,3.035,3.04,3.045,3.05,3.055,3.06,3.065,3.07,3.075,3.08,3.085,3.09,3.095,
+/*620*/3.1,3.105,3.11,3.115,3.12,3.125,3.13,3.135,3.14,3.145,3.15,3.155,3.16,3.165,3.17,3.175,3.18,3.185,3.19,3.195,
+/*640*/3.2,3.205,3.21,3.215,3.22,3.225,3.23,3.235,3.24,3.245,3.25,3.255,3.26,3.265,3.27,3.275,3.28,3.285,3.29,3.295,
+/*660*/3.3,3.305,3.31,3.315,3.32,3.325,3.33,3.335,3.34,3.345,3.35,3.355,3.36,3.365,3.37,3.375,3.38,3.385,3.39,3.395,
+/*680*/3.4,3.405,3.41,3.415,3.42,3.425,3.43,3.435,3.44,3.445,3.45,3.455,3.46,3.465,3.47,3.475,3.48,3.485,3.49,3.495,
+/*700*/3.5,3.505,3.51,3.515,3.52,3.525,3.53,3.535,3.54,3.545,3.55,3.555,3.56,3.565,3.57,3.575,3.58,3.585,3.59,3.595,
+/*720*/3.6,3.605,3.61,3.615,3.62,3.625,3.63,3.635,3.64,3.645,3.65,3.655,3.66,3.665,3.67,3.675,3.68,3.685,3.69,3.695,
+/*740*/3.7,3.705,3.71,3.715,3.72,3.725,3.73,3.735,3.74,3.745,3.75,3.755,3.76,3.765,3.77,3.775,3.78,3.785,3.79,3.795,
+/*760*/3.8,3.805,3.81,3.815,3.82,3.825,3.83,3.835,3.84,3.845,3.85,3.855,3.86,3.865,3.87,3.875,3.88,3.885,3.89,3.895,
+/*780*/3.9,3.905,3.91,3.915,3.92,3.925,3.93,3.935,3.94,3.945,3.95,3.955,3.96,3.965,3.97,3.975,3.98,3.985,3.99,3.995,
+/*800*/4,4.005,4.01,4.015,4.02,4.025,4.03,4.035,4.04,4.045,4.05,4.055,4.06,4.065,4.07,4.075,4.08,4.085,4.09,4.095,
+/*820*/4.1,4.105,4.11,4.115,4.12,4.125,4.13,4.135,4.14,4.145,4.15,4.155,4.16,4.165,4.17,4.175,4.18,4.185,4.19,4.195,
+/*840*/4.2,4.205,4.21,4.215,4.22,4.225,4.23,4.235,4.24,4.245,4.25,4.255,4.26,4.265,4.27,4.275,4.28,4.285,4.29,4.295,
+/*860*/4.3,4.305,4.31,4.315,4.32,4.325,4.33,4.335,4.34,4.345,4.35,4.355,4.36,4.365,4.37,4.375,4.38,4.385,4.39,4.395,
+/*880*/4.4,4.405,4.41,4.415,4.42,4.425,4.43,4.435,4.44,4.445,4.45,4.455,4.46,4.465,4.47,4.475,4.48,4.485,4.49,4.495,
+/*900*/4.5,4.505,4.51,4.515,4.52,4.525,4.53,4.535,4.54,4.545,4.55,4.555,4.56,4.565,4.57,4.575,4.58,4.585,4.59,4.595,
+/*920*/4.6,4.605,4.61,4.615,4.62,4.625,4.63,4.635,4.64,4.645,4.65,4.655,4.66,4.665,4.67,4.675,4.68,4.685,4.69,4.695,
+/*940*/4.7,4.705,4.71,4.715,4.72,4.725,4.73,4.735,4.74,4.745,4.75,4.755,4.76,4.765,4.77,4.775,4.78,4.785,4.79,4.795,
+/*960*/4.8,4.805,4.81,4.815,4.82,4.825,4.83,4.835,4.84,4.845,4.85,4.855,4.86,4.865,4.87,4.875,4.88,4.885,4.89,4.895,
+/*980*/4.9,4.905,4.91,4.915,4.92,4.925,4.93,4.935,4.94,4.945,4.95,4.955,4.96,4.965,4.97,4.975,4.98,4.985,4.99,4.995,
+/*1000*/5,5.005,5.01,5.015,5.02,5.025,5.03,5.035,5.04,5.045,5.05,5.055,5.06,5.065,5.07,5.075,5.08,5.085,5.09,5.095,
+/*1020*/5.1,5.105,5.11,5.115,5.12,5.125,5.13,5.135,5.14,5.145,5.15,5.155,5.16,5.165,5.17,5.175,5.18,5.185,5.19,5.195,
+/*1040*/5.2,5.205,5.21,5.215,5.22,5.225,5.23,5.235,5.24,5.245,5.25,5.255,5.26,5.265,5.27,5.275,5.28,5.285,5.29,5.295,
+/*1060*/5.3,5.305,5.31,5.315,5.32,5.325,5.33,5.335,5.34,5.345,5.35,5.355,5.36,5.365,5.37,5.375,5.38,5.385,5.39,5.395,
+/*1080*/5.4,5.405,5.41,5.415,5.42,5.425,5.43,5.435,5.44,5.445,5.45,5.455,5.46,5.465,5.47,5.475,5.48,5.485,5.49,5.495,
+/*1100*/5.5,5.505,5.51,5.515,5.52,5.525,5.53,5.535,5.54,5.545,5.55,5.555,5.56,5.565,5.57,5.575,5.58,5.585,5.59,5.595,
+/*1120*/5.6,5.605,5.61,5.615,5.62,5.625,5.63,5.635,5.64,5.645,5.65,5.655,5.66,5.665,5.67,5.675,5.68,5.685,5.69,5.695,
+/*1140*/5.7,5.705,5.71,5.715,5.72,5.725,5.73,5.735,5.74,5.745,5.75,5.755,5.76,5.765,5.77,5.775,5.78,5.785,5.79,5.795,
+/*1160*/5.8,5.805,5.81,5.815,5.82,5.825,5.83,5.835,5.84,5.845,5.85,5.855,5.86,5.865,5.87,5.875,5.88,5.885,5.89,5.895,
+/*1180*/5.9,5.905,5.91,5.915,5.92,5.925,5.93,5.935,5.94,5.945,5.95,5.955,5.96,5.965,5.97,5.975,5.98,5.985,5.99,5.995,
+/*1200*/6,6.005,6.01,6.015,6.02,6.025,6.03,6.035,6.04,6.045,6.05,6.055,6.06,6.065,6.07,6.075,6.08,6.085,6.09,6.095,
+/*1220*/6.1,6.105,6.11,6.115,6.12,6.125,6.13,6.135,6.14,6.145,6.15,6.155,6.16,6.165,6.17,6.175,6.18,6.185,6.19,6.195,
+/*1240*/6.2,6.205,6.21,6.215,6.22,6.225,6.23,6.235,6.24,6.245,6.25,6.255,6.26,6.265,6.27,6.275,6.28,6.285,6.29,6.295,
+/*1260*/6.3,6.305,6.31,6.315,6.32,6.325,6.33,6.335,6.34,6.345,6.35,6.355,6.36,6.365,6.37,6.375,6.38,6.385,6.39,6.395,
+/*1280*/6.4,6.405,6.41,6.415,6.42,6.425,6.43,6.435,6.44,6.445,6.45,6.455,6.46,6.465,6.47,6.475,6.48,6.485,6.49,6.495,
+/*1300*/6.5,6.505,6.51,6.515,6.52,6.525,6.53,6.535,6.54,6.545,6.55,6.555,6.56,6.565,6.57,6.575,6.58,6.585,6.59,6.595,
+/*1320*/6.6,6.605,6.61,6.615,6.62,6.625,6.63,6.635,6.64,6.645,6.65,6.655,6.66,6.665,6.67,6.675,6.68,6.685,6.69,6.695,
+/*1340*/6.7,6.705,6.71,6.715,6.72,6.725,6.73,6.735,6.74,6.745,6.75,6.755,6.76,6.765,6.77,6.775,6.78,6.785,6.79,6.795,
+/*1360*/6.8,6.805,6.81,6.815,6.82,6.825,6.83,6.835,6.84,6.845,6.85,6.855,6.86,6.865,6.87,6.875,6.88,6.885,6.89,6.895,
+/*1380*/6.9,6.905,6.91,6.915,6.92,6.925,6.93,6.935,6.94,6.945,6.95,6.955,6.96,6.965,6.97,6.975,6.98,6.985,6.99,6.995,
+/*1400*/7,7.005,7.01,7.015,7.02,7.025,7.03,7.035,7.04,7.045,7.05,7.055,7.06,7.065,7.07,7.075,7.08,7.085,7.09,7.095,
+/*1420*/7.1,7.105,7.11,7.115,7.12,7.125,7.13,7.135,7.14,7.145,7.15,7.155,7.16,7.165,7.17,7.175,7.18,7.185,7.19,7.195,
+/*1440*/7.2,7.205,7.21,7.215,7.22,7.225,7.23,7.235,7.24,7.245,7.25,7.255,7.26,7.265,7.27,7.275,7.28,7.285,7.29,7.295,
+/*1460*/7.3,7.305,7.31,7.315,7.32,7.325,7.33,7.335,7.34,7.345,7.35,7.355,7.36,7.365,7.37,7.375,7.38,7.385,7.39,7.395,
+/*1480*/7.4,7.405,7.41,7.415,7.42,7.425,7.43,7.435,7.44,7.445,7.45,7.455,7.46,7.465,7.47,7.475,7.48,7.485,7.49,7.495,
+/*1500*/7.5,7.505,7.51,7.515,7.52,7.525,7.53,7.535,7.54,7.545,7.55,7.555,7.56,7.565,7.57,7.575,7.58,7.585,7.59,7.595,
+/*1520*/7.6,7.605,7.61,7.615,7.62,7.625,7.63,7.635,7.64,7.645,7.65,7.655,7.66,7.665,7.67,7.675,7.68,7.685,7.69,7.695,
+/*1540*/7.7,7.705,7.71,7.715,7.72,7.725,7.73,7.735,7.74,7.745,7.75,7.755,7.76,7.765,7.77,7.775,7.78,7.785,7.79,7.795,
+/*1560*/7.8,7.805,7.81,7.815,7.82,7.825,7.83,7.835,7.84,7.845,7.85,7.855,7.86,7.865,7.87,7.875,7.88,7.885,7.89,7.895,
+/*1580*/7.9,7.905,7.91,7.915,7.92,7.925,7.93,7.935,7.94,7.945,7.95,7.955,7.96,7.965,7.97,7.975,7.98,7.985,7.99,7.995,
+/*1600*/8,8.005,8.01,8.015,8.02,8.025,8.03,8.035,8.04,8.045,8.05,8.055,8.06,8.065,8.07,8.075,8.08,8.085,8.09,8.095,
+/*1620*/8.1,8.105,8.11,8.115,8.12,8.125,8.13,8.135,8.14,8.145,8.15,8.155,8.16,8.165,8.17,8.175,8.18,8.185,8.19,8.195,
+/*1640*/8.2,8.205,8.21,8.215,8.22,8.225,8.23,8.235,8.24,8.245,8.25,8.255,8.26,8.265,8.27,8.275,8.28,8.285,8.29,8.295,
+/*1660*/8.3,8.305,8.31,8.315,8.32,8.325,8.33,8.335,8.34,8.345,8.35,8.355,8.36,8.365,8.37,8.375,8.38,8.385,8.39,8.395,
+/*1680*/8.4,8.405,8.41,8.415,8.42,8.425,8.43,8.435,8.44,8.445,8.45,8.455,8.46,8.465,8.47,8.475,8.48,8.485,8.49,8.495,
+/*1700*/8.5,8.505,8.51,8.515,8.52,8.525,8.53,8.535,8.54,8.545,8.55,8.555,8.56,8.565,8.57,8.575,8.58,8.585,8.59,8.595,
+/*1720*/8.6,8.605,8.61,8.615,8.62,8.625,8.63,8.635,8.64,8.645,8.65,8.655,8.66,8.665,8.67,8.675,8.68,8.685,8.69,8.695,
+/*1740*/8.7,8.705,8.71,8.715,8.72,8.725,8.73,8.735,8.74,8.745,8.75,8.755,8.76,8.765,8.77,8.775,8.78,8.785,8.79,8.795,
+/*1760*/8.8,8.805,8.81,8.815,8.82,8.825,8.83,8.835,8.84,8.845,8.85,8.855,8.86,8.865,8.87,8.875,8.88,8.885,8.89,8.895,
+/*1780*/8.9,8.905,8.91,8.915,8.92,8.925,8.93,8.935,8.94,8.945,8.95,8.955,8.96,8.965,8.97,8.975,8.98,8.985,8.99,8.995,
+/*1800*/9,9.005,9.01,9.015,9.02,9.025,9.03,9.035,9.04,9.045,9.05,9.055,9.06,9.065,9.07,9.075,9.08,9.085,9.09,9.095,
+/*1820*/9.1,9.105,9.11,9.115,9.12,9.125,9.13,9.135,9.14,9.145,9.15,9.155,9.16,9.165,9.17,9.175,9.18,9.185,9.19,9.195,
+/*1840*/9.2,9.205,9.21,9.215,9.22,9.225,9.23,9.235,9.24,9.245,9.25,9.255,9.26,9.265,9.27,9.275,9.28,9.285,9.29,9.295,
+/*1860*/9.3,9.305,9.31,9.315,9.32,9.325,9.33,9.335,9.34,9.345,9.35,9.355,9.36,9.365,9.37,9.375,9.38,9.385,9.39,9.395,
+/*1880*/9.4,9.405,9.41,9.415,9.42,9.425,9.43,9.435,9.44,9.445,9.45,9.455,9.46,9.465,9.47,9.475,9.48,9.485,9.49,9.495,
+/*1900*/9.5,9.505,9.51,9.515,9.52,9.525,9.53,9.535,9.54,9.545,9.55,9.555,9.56,9.565,9.57,9.575,9.58,9.585,9.59,9.595,
+/*1920*/9.6,9.605,9.61,9.615,9.62,9.625,9.63,9.635,9.64,9.645,9.65,9.655,9.66,9.665,9.67,9.675,9.68,9.685,9.69,9.695,
+/*1940*/9.7,9.705,9.71,9.715,9.72,9.725,9.73,9.735,9.74,9.745,9.75,9.755,9.76,9.765,9.77,9.775,9.78,9.785,9.79,9.795,
+/*1960*/9.8,9.805,9.81,9.815,9.82,9.825,9.83,9.835,9.84,9.845,9.85,9.855,9.86,9.865,9.87,9.875,9.88,9.885,9.89,9.895,
+/*1980*/9.9,9.905,9.91,9.915,9.92,9.925,9.93,9.935,9.94,9.945,9.95,9.955,9.96,9.965,9.97,9.975,9.98,9.985,9.99,9.995,
+/*2000*/10,10.005,10.01,10.015,10.02,10.025,10.03,10.035,10.04,10.045,10.05,10.055,10.06,10.065,10.07,10.075,10.08,10.085,10.09,10.095,
+/*2020*/10.1,10.105,10.11,10.115,10.12,10.125,10.13,10.135,10.14,10.145,10.15,10.155,10.16,10.165,10.17,10.175,10.18,10.185,10.19,10.195,
+/*2040*/10.2,10.205,10.21,10.215,10.22,10.225,10.23,10.235,10.24,10.245,10.25,10.255,10.26,10.265,10.27,10.275,10.28,10.285,10.29,10.295,
+/*2060*/10.3,10.305,10.31,10.315,10.32,10.325,10.33,10.335,10.34,10.345,10.35,10.355,10.36,10.365,10.37,10.375,10.38,10.385,10.39,10.395,
+/*2080*/10.4,10.405,10.41,10.415,10.42,10.425,10.43,10.435,10.44,10.445,10.45,10.455,10.46,10.465,10.47,10.475,10.48,10.485,10.49,10.495,
+/*2100*/10.5,10.505,10.51,10.515,10.52
+};
+static const char* observationOrder[NObservations] = {
+/*0*/"R.Shoulder","R.Clavicle","R.Bicep","R.Elbow","R.Forearm","R.Wrist","L.Shoulder","L.Clavicle","L.Scapula","L.Bicep",
+/*10*/"L.Elbow","L.Forearm","L.Wrist","R.ASIS","R.PSIS","S2","L.PSIS","L.ASIS","R.TH1","R.TH2",
+/*20*/"R.TH3","R.Knee","R.SH1","R.SH2","R.SH3","R.SH4","R.Ankle","R.Heel","R.MT1","R.MT5",
+/*30*/"L.TH1","L.TH2","L.TH3","L.TH4","L.Knee","L.SH1","L.SH2","L.SH3","L.Ankle","L.Heel",
+/*40*/"L.MT1","L.MT5","R_HJC","L_HJC"
+};
+static double observations[NFrames][3*NObservations] = {
+/*0*/{0.090174,1.88432,-1.71703,0.183195,1.8414,-1.85736,-0.03272,1.75705,-1.63169,-0.124639,1.65522,-1.58515,0.018105,1.57584,-1.61167,0.095363,1.52367,-1.612,0.127943,1.92047,-2.10315,0.194393,1.84906,-1.93091,-0.009998,1.87655,-2.0326,-0.014027,1.71457,-2.22251,0.035047,1.62438,-2.26588,0.185972,1.57007,-2.14576,0.244882,1.53837,-2.14,0.108311,1.41565,-1.81786,-0.080276,1.47065,-1.89218,-0.075026,1.49623,-1.95279,-0.067967,1.46745,-2.00846,0.123187,1.40594,-2.07196,0.170297,1.12647,-1 [...]
+/*1*/{0.091297,1.87806,-1.71564,0.182311,1.83703,-1.85764,-0.028513,1.74764,-1.62789,-0.118465,1.64442,-1.57894,0.025159,1.56849,-1.60827,0.103256,1.51788,-1.61005,0.126087,1.91504,-2.10347,0.193485,1.84336,-1.93154,-0.010839,1.87079,-2.03188,-0.020112,1.71092,-2.22225,0.028199,1.61943,-2.26571,0.17971,1.5599,-2.14963,0.237434,1.52621,-2.14531,0.107126,1.41214,-1.81599,-0.081285,1.46489,-1.8934,-0.07621,1.49056,-1.95294,-0.068882,1.46171,-2.00913,0.121952,1.401,-2.07248,0.168246,1.12131, [...]
+/*2*/{0.091335,1.87169,-1.71479,0.183083,1.82953,-1.85712,-0.023951,1.7383,-1.62404,-0.11091,1.63499,-1.57333,0.032871,1.5612,-1.60512,0.110852,1.51258,-1.60793,0.124791,1.90938,-2.10431,0.193996,1.83813,-1.93194,-0.012086,1.86576,-2.03137,-0.024299,1.70718,-2.2217,0.020478,1.61543,-2.26539,0.173088,1.54986,-2.15399,0.230455,1.51413,-2.15068,0.104805,1.40744,-1.81493,-0.083976,1.45985,-1.89355,-0.0781,1.48507,-1.9529,-0.069696,1.45639,-2.01053,0.121724,1.39661,-2.07049,0.164122,1.11498,- [...]
+/*3*/{0.092087,1.86568,-1.71333,0.182831,1.82511,-1.85789,-0.01871,1.72901,-1.62038,-0.103618,1.62466,-1.56734,0.041183,1.55492,-1.60203,0.119592,1.50664,-1.60573,0.124153,1.90441,-2.10496,0.193549,1.83257,-1.9327,-0.01286,1.86058,-2.03072,-0.030531,1.70266,-2.22109,0.012698,1.61085,-2.26538,0.166654,1.53936,-2.15824,0.222259,1.50243,-2.15566,0.102622,1.40302,-1.81331,-0.086237,1.455,-1.89456,-0.07984,1.47883,-1.95377,-0.072053,1.45063,-2.01137,0.121167,1.39182,-2.06906,0.161441,1.11035, [...]
+/*4*/{0.093353,1.85929,-1.71192,0.183228,1.81831,-1.85746,-0.012746,1.7196,-1.61667,-0.095804,1.61451,-1.56114,0.049678,1.54751,-1.59868,0.128215,1.5024,-1.60385,0.123235,1.89968,-2.10598,0.19428,1.82751,-1.93354,-0.01348,1.85534,-2.02912,-0.03415,1.69845,-2.22057,0.004914,1.60661,-2.26578,0.159918,1.53025,-2.16303,0.213638,1.49,-2.16112,0.098635,1.39922,-1.81269,-0.089307,1.45081,-1.89675,-0.082555,1.47378,-1.95558,-0.074797,1.44483,-2.01252,0.122134,1.38754,-2.06714,0.153185,1.10725,-1 [...]
+/*5*/{0.094187,1.85462,-1.71052,0.1833,1.81388,-1.85825,-0.005894,1.71058,-1.61324,-0.085588,1.60545,-1.55552,0.059256,1.54157,-1.59556,0.137858,1.49781,-1.60231,0.122382,1.89488,-2.10714,0.194885,1.8218,-1.93386,-0.01458,1.85216,-2.0276,-0.039699,1.6948,-2.22017,-0.002082,1.60262,-2.2655,0.152843,1.51995,-2.16763,0.204903,1.47882,-2.16672,0.095466,1.39565,-1.81297,-0.093194,1.44607,-1.89718,-0.086238,1.46806,-1.95603,-0.076539,1.43936,-2.01355,0.120146,1.38273,-2.0666,0.150597,1.10498,- [...]
+/*6*/{0.095729,1.84966,-1.70919,0.184067,1.80892,-1.85878,0.001161,1.7015,-1.61103,-0.074648,1.5961,-1.55046,0.068514,1.53624,-1.59393,0.148058,1.49411,-1.60094,0.122291,1.8912,-2.10838,0.19469,1.81667,-1.93506,-0.015233,1.84998,-2.02662,-0.042729,1.69173,-2.22001,-0.009105,1.59889,-2.26398,0.144665,1.50931,-2.17191,0.195652,1.46696,-2.17202,0.091803,1.39296,-1.81113,-0.096951,1.44181,-1.89835,-0.089504,1.46355,-1.95797,-0.080636,1.4341,-2.01495,0.118084,1.37838,-2.0653,0.148764,1.1036,- [...]
+/*7*/{0.096568,1.84563,-1.70836,0.184708,1.80316,-1.85888,0.009135,1.6945,-1.61045,-0.065657,1.58736,-1.54593,0.07816,1.53185,-1.5927,0.159315,1.49178,-1.59935,0.121274,1.88765,-2.10932,0.195721,1.81172,-1.93587,-0.015837,1.84837,-2.02445,-0.046414,1.69033,-2.2188,-0.015707,1.59644,-2.26315,0.137288,1.50196,-2.17557,0.186289,1.45751,-2.17685,0.088901,1.39019,-1.81098,-0.101482,1.43683,-1.90087,-0.092025,1.4587,-1.95859,-0.08381,1.42908,-2.01586,0.116761,1.37474,-2.06398,0.144709,1.10356, [...]
+/*8*/{0.097382,1.84288,-1.70801,0.184091,1.80043,-1.86026,0.015423,1.69006,-1.6092,-0.054602,1.58161,-1.54312,0.088364,1.52854,-1.59238,0.169979,1.49015,-1.59909,0.121337,1.88439,-2.10937,0.195026,1.80842,-1.93745,-0.015983,1.84802,-2.02398,-0.047971,1.69093,-2.21671,-0.021898,1.59518,-2.26112,0.128561,1.49452,-2.17808,0.175944,1.44767,-2.18203,0.085652,1.38856,-1.80907,-0.104585,1.43312,-1.90214,-0.094421,1.45446,-1.95905,-0.087138,1.42308,-2.01735,0.113806,1.37061,-2.06345,0.14252,1.10 [...]
+/*9*/{0.097987,1.83984,-1.70815,0.184473,1.7964,-1.86062,0.020833,1.68652,-1.60865,-0.044085,1.57764,-1.54053,0.099375,1.52665,-1.5917,0.180819,1.48987,-1.59872,0.121746,1.88194,-2.10948,0.19461,1.80464,-1.93839,-0.01577,1.84717,-2.02358,-0.052498,1.68994,-2.21369,-0.028512,1.59583,-2.25865,0.119527,1.4878,-2.18008,0.165742,1.4401,-2.18675,0.082546,1.38708,-1.80751,-0.106813,1.43073,-1.90409,-0.096968,1.45228,-1.95945,-0.090871,1.4191,-2.01805,0.111328,1.36727,-2.06348,0.138474,1.10209,- [...]
+/*10*/{0.099501,1.83777,-1.70854,0.183263,1.79551,-1.86103,0.026735,1.68402,-1.60851,-0.03388,1.57463,-1.53843,0.110259,1.52571,-1.59143,0.19291,1.49062,-1.59942,0.122863,1.87987,-2.10904,0.192505,1.80126,-1.93932,-0.015902,1.84631,-2.02332,-0.057763,1.69149,-2.20986,-0.035401,1.59762,-2.25601,0.110867,1.48296,-2.1823,0.154895,1.43367,-2.19119,0.078954,1.38535,-1.80555,-0.108853,1.42859,-1.90512,-0.097829,1.4527,-1.95972,-0.092736,1.41687,-2.01869,0.108127,1.36533,-2.06377,0.13348,1.0995 [...]
+/*11*/{0.101487,1.8358,-1.70907,0.182986,1.7929,-1.86111,0.031669,1.6824,-1.60778,-0.023637,1.57318,-1.53751,0.120925,1.5257,-1.59127,0.204399,1.49264,-1.60057,0.123425,1.87808,-2.10816,0.192394,1.79888,-1.93926,-0.015896,1.84573,-2.0232,-0.062844,1.69391,-2.20634,-0.04217,1.60022,-2.25316,0.101822,1.48029,-2.18444,0.143903,1.42839,-2.19461,0.075152,1.38256,-1.79922,-0.110244,1.42756,-1.90563,-0.099716,1.45247,-1.95973,-0.094397,1.41592,-2.01929,0.106355,1.36368,-2.06448,0.129567,1.09721 [...]
+/*12*/{0.103362,1.83464,-1.70961,0.182467,1.79045,-1.86165,0.037151,1.6807,-1.60707,-0.014949,1.57092,-1.5353,0.131625,1.52706,-1.59167,0.215737,1.49589,-1.60199,0.123831,1.87653,-2.10792,0.192009,1.79709,-1.93875,-0.016419,1.84547,-2.02338,-0.066142,1.69742,-2.20094,-0.049691,1.60308,-2.25131,0.093253,1.4789,-2.18668,0.133403,1.42539,-2.19744,0.073299,1.38255,-1.80138,-0.112103,1.42768,-1.9061,-0.101243,1.45188,-1.96005,-0.096407,1.41598,-2.01977,0.105074,1.36325,-2.06479,0.124756,1.093 [...]
+/*13*/{0.104943,1.83409,-1.71048,0.182701,1.78677,-1.86125,0.041993,1.68032,-1.60688,-0.005174,1.57048,-1.53451,0.141148,1.52916,-1.59185,0.226856,1.50054,-1.60342,0.123712,1.87524,-2.10741,0.191799,1.79547,-1.93847,-0.01659,1.84528,-2.0233,-0.071916,1.70148,-2.19645,-0.05665,1.60695,-2.24916,0.083929,1.47758,-2.1882,0.122506,1.42404,-2.20003,0.070529,1.38132,-1.79998,-0.112486,1.42758,-1.90506,-0.102572,1.45113,-1.96001,-0.097435,1.41649,-2.0189,0.102265,1.36294,-2.06504,0.11833,1.09041 [...]
+/*14*/{0.107849,1.83391,-1.71058,0.182287,1.78744,-1.86306,0.046045,1.68062,-1.60618,0.002846,1.56922,-1.53304,0.150423,1.53283,-1.59281,0.23755,1.5059,-1.60557,0.1232,1.87454,-2.10766,0.191769,1.79496,-1.93837,-0.016768,1.8453,-2.02336,-0.076225,1.70681,-2.19175,-0.064397,1.61111,-2.24767,0.075621,1.47875,-2.19004,0.112063,1.4233,-2.2022,0.069836,1.38069,-1.79928,-0.113928,1.42688,-1.90501,-0.104225,1.45003,-1.95981,-0.0991,1.41704,-2.01811,0.100798,1.36339,-2.06585,0.112596,1.08634,-1. [...]
+/*15*/{0.109227,1.83504,-1.71077,0.182131,1.78696,-1.86361,0.04963,1.68136,-1.60603,0.010533,1.56969,-1.5326,0.158976,1.53636,-1.59377,0.246765,1.51184,-1.60834,0.121418,1.8745,-2.10768,0.192039,1.7948,-1.93858,-0.017548,1.84555,-2.02326,-0.081668,1.71189,-2.18845,-0.072145,1.61571,-2.24605,0.066569,1.47985,-2.19133,0.101843,1.42411,-2.20445,0.067032,1.37915,-1.79888,-0.114367,1.42695,-1.90534,-0.104346,1.45001,-1.9597,-0.100795,1.41745,-2.01693,0.099771,1.36446,-2.06675,0.108334,1.083,- [...]
+/*16*/{0.111101,1.83622,-1.71111,0.18191,1.78688,-1.86387,0.053408,1.68284,-1.6059,0.018321,1.56918,-1.53087,0.167365,1.54098,-1.59476,0.255318,1.5191,-1.61108,0.120246,1.87475,-2.10808,0.192333,1.79512,-1.93898,-0.01888,1.84682,-2.02271,-0.085611,1.71713,-2.18482,-0.08025,1.62045,-2.2449,0.057505,1.48218,-2.19252,0.091191,1.426,-2.20548,0.066866,1.3791,-1.79904,-0.115974,1.42676,-1.90524,-0.105933,1.45026,-1.95883,-0.102081,1.41798,-2.0168,0.097623,1.36561,-2.06801,0.102081,1.07916,-1.7 [...]
+/*17*/{0.113371,1.83728,-1.71145,0.181486,1.78747,-1.86568,0.057349,1.68487,-1.60605,0.025041,1.5706,-1.53083,0.174766,1.54595,-1.59634,0.263928,1.52666,-1.61446,0.11794,1.87545,-2.10836,0.189345,1.79464,-1.9408,-0.020337,1.84755,-2.02149,-0.091509,1.72297,-2.18171,-0.089031,1.62655,-2.24366,0.049119,1.48624,-2.1939,0.081881,1.42961,-2.20676,0.066727,1.37876,-1.79926,-0.116338,1.42702,-1.90454,-0.106372,1.45109,-1.95857,-0.102951,1.41854,-2.0155,0.09716,1.36782,-2.06844,0.094536,1.07653, [...]
+/*18*/{0.115178,1.83963,-1.71275,0.182032,1.78811,-1.86615,0.06146,1.6873,-1.60589,0.032973,1.57251,-1.53038,0.182003,1.55144,-1.59811,0.271077,1.53504,-1.61821,0.115404,1.8771,-2.10918,0.188895,1.79526,-1.94205,-0.020943,1.84832,-2.02036,-0.097112,1.72694,-2.17747,-0.097084,1.63263,-2.24246,0.040663,1.49031,-2.19399,0.072524,1.43364,-2.20701,0.06614,1.37863,-1.7998,-0.117299,1.42818,-1.9047,-0.106991,1.45219,-1.95804,-0.103754,1.41944,-2.01534,0.096577,1.36976,-2.06864,0.085972,1.0751,- [...]
+/*19*/{0.1169,1.84236,-1.7137,0.181774,1.79032,-1.86751,0.065369,1.69012,-1.60605,0.041126,1.57469,-1.52974,0.189481,1.55733,-1.60041,0.277886,1.54359,-1.62188,0.112567,1.87905,-2.10943,0.188519,1.79694,-1.94391,-0.022641,1.85062,-2.0191,-0.103006,1.73213,-2.17574,-0.104604,1.63866,-2.24189,0.031428,1.49563,-2.1933,0.063,1.43913,-2.20714,0.066133,1.37899,-1.80017,-0.117799,1.42962,-1.9049,-0.10782,1.45362,-1.95781,-0.10444,1.42084,-2.01518,0.096756,1.37186,-2.0692,0.077045,1.07304,-1.740 [...]
+/*20*/{0.119208,1.84549,-1.71445,0.182676,1.7919,-1.86885,0.068922,1.69299,-1.60643,0.047808,1.57712,-1.53073,0.195985,1.56447,-1.60238,0.283817,1.55285,-1.62564,0.10974,1.88094,-2.11021,0.187921,1.79873,-1.94527,-0.024434,1.85156,-2.01741,-0.109792,1.73758,-2.17331,-0.113305,1.64461,-2.23988,0.022727,1.50107,-2.1934,0.054645,1.44513,-2.20672,0.066568,1.37916,-1.80002,-0.116796,1.431,-1.90397,-0.107321,1.45546,-1.9565,-0.104293,1.42275,-2.01527,0.094598,1.37369,-2.06943,0.071114,1.07044, [...]
+/*21*/{0.120733,1.84893,-1.7155,0.183488,1.79466,-1.87033,0.073068,1.69611,-1.60657,0.054771,1.58005,-1.53061,0.201915,1.57127,-1.60552,0.289832,1.56203,-1.62989,0.106101,1.8841,-2.1106,0.186776,1.80102,-1.94738,-0.025426,1.85398,-2.01627,-0.116322,1.74331,-2.17179,-0.121351,1.6511,-2.23852,0.014862,1.50752,-2.19296,0.045816,1.45106,-2.20618,0.066572,1.37966,-1.80009,-0.116274,1.43311,-1.90226,-0.107262,1.45689,-1.95545,-0.103751,1.42489,-2.01502,0.095398,1.37664,-2.06949,0.063834,1.0686 [...]
+/*22*/{0.122665,1.85246,-1.71673,0.183315,1.79822,-1.8723,0.077505,1.69926,-1.60738,0.061629,1.58344,-1.53138,0.206655,1.57782,-1.60817,0.294677,1.57127,-1.63377,0.102947,1.88732,-2.1108,0.18672,1.80427,-1.94887,-0.026879,1.85544,-2.01425,-0.122687,1.74883,-2.17012,-0.128363,1.65727,-2.23737,0.006893,1.5141,-2.19249,0.037231,1.45783,-2.2054,0.066578,1.37986,-1.79999,-0.115928,1.43499,-1.90191,-0.107341,1.45909,-1.95477,-0.103888,1.42687,-2.01396,0.095193,1.37965,-2.06918,0.054795,1.06977 [...]
+/*23*/{0.124548,1.85636,-1.7183,0.183689,1.80127,-1.87388,0.082199,1.70265,-1.60829,0.066992,1.58677,-1.53173,0.211006,1.58529,-1.61116,0.298607,1.58103,-1.63785,0.099084,1.89099,-2.11118,0.186432,1.80743,-1.95145,-0.028625,1.85801,-2.01276,-0.128382,1.7547,-2.16832,-0.135721,1.66357,-2.23586,-0.001159,1.52079,-2.19094,0.030007,1.46431,-2.20414,0.066867,1.38065,-1.79967,-0.11513,1.4371,-1.90096,-0.106182,1.46117,-1.95345,-0.103337,1.42994,-2.01362,0.095196,1.38309,-2.06853,0.048981,1.068 [...]
+/*24*/{0.126595,1.86074,-1.71948,0.184374,1.80498,-1.87604,0.086812,1.7064,-1.60922,0.072869,1.59008,-1.53215,0.215538,1.59312,-1.61423,0.303044,1.59043,-1.64265,0.095239,1.89482,-2.11153,0.186324,1.81103,-1.95339,-0.029986,1.85996,-2.01082,-0.13403,1.7609,-2.16678,-0.142846,1.6698,-2.23422,-0.009007,1.52664,-2.18934,0.022878,1.47173,-2.20326,0.068358,1.38154,-1.79895,-0.114367,1.4395,-1.89997,-0.106364,1.46388,-1.95234,-0.102895,1.4334,-2.01332,0.09556,1.38659,-2.06756,0.044911,1.06981, [...]
+/*25*/{0.127829,1.8654,-1.72109,0.184973,1.80914,-1.87814,0.091491,1.71108,-1.60988,0.077839,1.59397,-1.53268,0.218785,1.60055,-1.61688,0.305522,1.59999,-1.6471,0.091403,1.89898,-2.1113,0.186243,1.81543,-1.95557,-0.031612,1.86331,-2.00926,-0.138517,1.76775,-2.16487,-0.149038,1.67627,-2.23225,-0.015596,1.53384,-2.18713,0.015844,1.47901,-2.20207,0.069347,1.38191,-1.79809,-0.112671,1.44218,-1.89876,-0.104672,1.4665,-1.94997,-0.102018,1.43708,-2.01264,0.09612,1.39044,-2.06639,0.03655,1.06951 [...]
+/*26*/{0.129495,1.8701,-1.72257,0.185783,1.81335,-1.88062,0.096285,1.7149,-1.61081,0.084261,1.59877,-1.53345,0.222392,1.60795,-1.62045,0.307543,1.60911,-1.65111,0.088589,1.90309,-2.11124,0.18568,1.8195,-1.95722,-0.033371,1.86627,-2.00795,-0.143725,1.77396,-2.16419,-0.155777,1.68287,-2.23086,-0.022099,1.54061,-2.18506,0.009074,1.48659,-2.2012,0.07091,1.38346,-1.79746,-0.112364,1.44463,-1.89814,-0.104122,1.46948,-1.9491,-0.101139,1.44109,-2.01187,0.096689,1.39443,-2.06535,0.030176,1.06979, [...]
+/*27*/{0.130871,1.875,-1.72388,0.186187,1.81786,-1.88228,0.100782,1.71964,-1.61187,0.089821,1.60335,-1.53396,0.224795,1.61588,-1.62338,0.310266,1.6186,-1.65552,0.085636,1.90721,-2.11107,0.18536,1.82322,-1.95958,-0.03502,1.86965,-2.0065,-0.147062,1.78069,-2.16138,-0.161253,1.68965,-2.22837,-0.027768,1.54917,-2.18369,0.003117,1.49365,-2.19981,0.072155,1.38495,-1.797,-0.109963,1.44803,-1.8971,-0.103165,1.47301,-1.94655,-0.100015,1.44456,-2.01124,0.097563,1.39909,-2.06448,0.025857,1.07023,-1 [...]
+/*28*/{0.132551,1.88057,-1.72544,0.187249,1.8219,-1.88464,0.105686,1.72384,-1.61364,0.095324,1.60724,-1.53399,0.227853,1.62266,-1.62596,0.312013,1.62732,-1.65929,0.082274,1.91134,-2.11105,0.184627,1.82815,-1.96161,-0.033228,1.87063,-2.00451,-0.151814,1.78724,-2.16052,-0.166819,1.69597,-2.22644,-0.034134,1.55598,-2.18155,-0.002681,1.50115,-2.19854,0.073435,1.38571,-1.79667,-0.108521,1.4514,-1.89595,-0.100752,1.47599,-1.94517,-0.099181,1.44989,-2.01091,0.098478,1.40371,-2.06363,0.021605,1. [...]
+/*29*/{0.134002,1.88444,-1.72741,0.188577,1.82701,-1.88687,0.109384,1.72881,-1.61451,0.099089,1.61261,-1.53505,0.229821,1.63024,-1.62848,0.313622,1.6366,-1.66403,0.0805,1.91493,-2.1112,0.184373,1.83286,-1.96337,-0.035159,1.87445,-2.00277,-0.155134,1.79359,-2.15838,-0.171577,1.7027,-2.22423,-0.040538,1.56291,-2.17941,-0.008403,1.50808,-2.19723,0.075621,1.38724,-1.79662,-0.105957,1.454,-1.89416,-0.099848,1.47983,-1.94297,-0.097126,1.45375,-2.00998,0.098624,1.40788,-2.06257,0.014496,1.07435 [...]
+/*30*/{0.135311,1.88917,-1.72879,0.189713,1.83133,-1.88888,0.113256,1.73376,-1.61551,0.102509,1.61736,-1.53552,0.231416,1.63711,-1.63166,0.314317,1.6441,-1.6683,0.078604,1.91889,-2.11134,0.18415,1.83711,-1.96551,-0.035549,1.87783,-2.00247,-0.15797,1.79979,-2.15617,-0.176789,1.709,-2.22184,-0.044343,1.57054,-2.1768,-0.013134,1.51553,-2.1959,0.078212,1.38814,-1.79626,-0.103451,1.45774,-1.89312,-0.098715,1.48327,-1.94124,-0.096251,1.45916,-2.00907,0.099827,1.41268,-2.06201,0.00984,1.07599,- [...]
+/*31*/{0.136843,1.89459,-1.73068,0.189298,1.83556,-1.89125,0.116252,1.73847,-1.61673,0.107229,1.62231,-1.53598,0.23254,1.644,-1.63371,0.314849,1.6526,-1.67168,0.077054,1.9224,-2.11141,0.183933,1.84182,-1.96744,-0.03542,1.88132,-2.00137,-0.160336,1.80564,-2.15488,-0.181269,1.71543,-2.2192,-0.050111,1.57686,-2.17511,-0.017683,1.52164,-2.19443,0.079877,1.38993,-1.79749,-0.102031,1.46124,-1.89295,-0.097035,1.48672,-1.93955,-0.094147,1.46381,-2.00863,0.100401,1.41746,-2.06212,0.005652,1.07849 [...]
+/*32*/{0.138099,1.89892,-1.7321,0.189753,1.84039,-1.893,0.119535,1.74343,-1.61731,0.110027,1.62737,-1.53713,0.23349,1.65082,-1.63717,0.315779,1.66025,-1.67519,0.075893,1.926,-2.11174,0.183225,1.84578,-1.96872,-0.036477,1.88498,-2.00058,-0.162481,1.8115,-2.15287,-0.185413,1.72161,-2.21613,-0.053792,1.58318,-2.17265,-0.02182,1.52852,-2.19309,0.083063,1.39143,-1.79923,-0.099371,1.46486,-1.89131,-0.095213,1.49111,-1.93794,-0.093438,1.46847,-2.00789,0.100193,1.42191,-2.06153,0.001705,1.08094, [...]
+/*33*/{0.139705,1.90394,-1.73374,0.19112,1.84426,-1.89503,0.121893,1.74842,-1.6187,0.113133,1.63161,-1.53684,0.23496,1.65721,-1.63974,0.316075,1.66746,-1.67919,0.074171,1.92871,-2.11208,0.183189,1.85027,-1.97027,-0.036459,1.88842,-2.00002,-0.165394,1.81691,-2.15155,-0.188941,1.72755,-2.21362,-0.057234,1.58938,-2.17072,-0.025061,1.53446,-2.19224,0.086432,1.39282,-1.8009,-0.097068,1.46852,-1.89034,-0.094591,1.49521,-1.93548,-0.091274,1.47394,-2.007,0.100949,1.42693,-2.06171,-0.002045,1.083 [...]
+/*34*/{0.141664,1.90844,-1.73542,0.190851,1.8491,-1.897,0.125085,1.75301,-1.61941,0.116177,1.63646,-1.53751,0.234943,1.66348,-1.64178,0.315818,1.6749,-1.68265,0.072628,1.93201,-2.11232,0.182837,1.8538,-1.97229,-0.036473,1.89167,-1.99946,-0.166949,1.82235,-2.14923,-0.192844,1.73293,-2.21094,-0.061664,1.5946,-2.16886,-0.029072,1.54015,-2.19085,0.088385,1.39458,-1.80275,-0.095356,1.47279,-1.88852,-0.091802,1.49894,-1.93413,-0.089973,1.47834,-2.00636,0.101197,1.43178,-2.06221,-0.006109,1.087 [...]
+/*35*/{0.142448,1.91274,-1.73734,0.191083,1.85239,-1.89827,0.127285,1.7574,-1.62025,0.119793,1.6408,-1.53862,0.235851,1.66895,-1.64459,0.316163,1.68127,-1.68676,0.071861,1.93528,-2.11275,0.1831,1.85799,-1.97411,-0.036461,1.89409,-1.99918,-0.168549,1.82653,-2.14715,-0.195533,1.73817,-2.20901,-0.063721,1.60007,-2.16706,-0.032048,1.54534,-2.18987,0.090938,1.39654,-1.80453,-0.093524,1.47631,-1.88867,-0.090204,1.50294,-1.93269,-0.088457,1.4838,-2.00583,0.101657,1.43562,-2.06304,-0.007464,1.09 [...]
+/*36*/{0.144206,1.91697,-1.73869,0.193049,1.857,-1.90002,0.129404,1.7617,-1.62115,0.120918,1.645,-1.53954,0.236246,1.67434,-1.64692,0.316741,1.68723,-1.68966,0.071069,1.93773,-2.11344,0.182507,1.86113,-1.9752,-0.035927,1.89706,-1.99878,-0.169851,1.8312,-2.14595,-0.198561,1.74294,-2.20524,-0.066365,1.60535,-2.16533,-0.034848,1.54989,-2.18858,0.093939,1.39867,-1.8059,-0.090914,1.48003,-1.88783,-0.088918,1.50681,-1.932,-0.086468,1.48857,-2.00487,0.101669,1.43979,-2.06344,-0.011773,1.09325,- [...]
+/*37*/{0.146434,1.92114,-1.73999,0.193185,1.86006,-1.90158,0.131025,1.76617,-1.62166,0.123645,1.64913,-1.53968,0.236695,1.67949,-1.64908,0.316523,1.69277,-1.69315,0.070103,1.94052,-2.11368,0.183238,1.86447,-1.97643,-0.03598,1.90008,-1.99933,-0.170283,1.83585,-2.14389,-0.201162,1.74788,-2.20248,-0.068075,1.60956,-2.1644,-0.037086,1.55437,-2.18712,0.096504,1.40136,-1.80789,-0.089454,1.48411,-1.88666,-0.087365,1.51024,-1.93153,-0.085098,1.49391,-2.00427,0.102692,1.44429,-2.06456,-0.014464,1 [...]
+/*38*/{0.147034,1.92448,-1.74204,0.193239,1.86395,-1.90286,0.133215,1.77005,-1.62283,0.125482,1.65322,-1.54081,0.236989,1.6835,-1.65103,0.315839,1.69779,-1.69652,0.069484,1.94292,-2.11396,0.183393,1.86806,-1.9779,-0.035355,1.90236,-1.99888,-0.170918,1.84011,-2.14257,-0.203498,1.75201,-2.19952,-0.069787,1.61345,-2.16265,-0.039521,1.55814,-2.18595,0.097827,1.40328,-1.80925,-0.087068,1.48761,-1.88657,-0.086218,1.51452,-1.93023,-0.082367,1.49786,-2.00321,0.102166,1.44787,-2.06545,-0.017905,1 [...]
+/*39*/{0.149317,1.92832,-1.74267,0.195164,1.86743,-1.90378,0.134653,1.77367,-1.62369,0.127873,1.65591,-1.54121,0.237154,1.68796,-1.65334,0.315565,1.70214,-1.7,0.068826,1.94556,-2.11447,0.183994,1.87127,-1.97942,-0.03504,1.90473,-1.99699,-0.17182,1.84368,-2.14032,-0.205307,1.75567,-2.19647,-0.071426,1.61718,-2.16141,-0.040983,1.56186,-2.18514,0.099971,1.40589,-1.81008,-0.084988,1.49207,-1.88458,-0.084371,1.51787,-1.92944,-0.08087,1.50197,-2.00181,0.102153,1.45139,-2.06624,-0.019317,1.1034 [...]
+/*40*/{0.150145,1.9312,-1.74375,0.195657,1.87076,-1.90568,0.135924,1.77681,-1.62468,0.128175,1.65946,-1.54129,0.237392,1.69189,-1.65516,0.315806,1.70642,-1.70246,0.06841,1.94784,-2.11518,0.184278,1.87431,-1.98038,-0.034089,1.9074,-1.99761,-0.172572,1.84654,-2.13976,-0.207321,1.75878,-2.19355,-0.072848,1.62065,-2.16021,-0.042545,1.56469,-2.1839,0.100995,1.40886,-1.811,-0.083757,1.49525,-1.8844,-0.083394,1.52171,-1.92896,-0.079922,1.50573,-2.00097,0.102896,1.45511,-2.06653,-0.020895,1.1075 [...]
+/*41*/{0.151667,1.93391,-1.7453,0.195878,1.87397,-1.9071,0.137636,1.78055,-1.62558,0.129082,1.66201,-1.5423,0.237741,1.69592,-1.65729,0.315447,1.70966,-1.70526,0.068825,1.94966,-2.11559,0.184502,1.87676,-1.98142,-0.033539,1.90912,-1.9968,-0.172271,1.849,-2.1363,-0.208412,1.76161,-2.19114,-0.074118,1.62321,-2.15939,-0.043149,1.56723,-2.18291,0.102451,1.41179,-1.81201,-0.082531,1.49898,-1.88373,-0.082147,1.52563,-1.92853,-0.078998,1.50957,-1.99976,0.103348,1.45827,-2.06669,-0.021933,1.1097 [...]
+/*42*/{0.153229,1.93662,-1.74647,0.196928,1.87705,-1.90795,0.138829,1.78359,-1.62688,0.1299,1.66438,-1.54318,0.238411,1.69763,-1.65966,0.315199,1.71231,-1.70816,0.06864,1.95177,-2.11647,0.18516,1.87885,-1.98275,-0.032918,1.91132,-1.99714,-0.172693,1.85119,-2.13527,-0.208934,1.76391,-2.18941,-0.074573,1.62525,-2.15785,-0.043895,1.56967,-2.18187,0.102209,1.41421,-1.81233,-0.081373,1.50231,-1.88224,-0.080955,1.52936,-1.92839,-0.078529,1.51254,-1.99931,0.103627,1.46093,-2.06649,-0.022716,1.1 [...]
+/*43*/{0.15465,1.9391,-1.74766,0.19775,1.87952,-1.9094,0.140129,1.78588,-1.62774,0.130465,1.66611,-1.5433,0.238379,1.69929,-1.66004,0.314682,1.71428,-1.71059,0.068766,1.95357,-2.11686,0.185341,1.88074,-1.98392,-0.03213,1.91337,-1.99666,-0.172489,1.85383,-2.13487,-0.209681,1.76512,-2.18718,-0.074728,1.62684,-2.15711,-0.044028,1.57114,-2.18129,0.103671,1.41763,-1.81229,-0.080373,1.50528,-1.88113,-0.081082,1.53232,-1.92731,-0.077754,1.51588,-1.99771,0.103232,1.46333,-2.06597,-0.023981,1.114 [...]
+/*44*/{0.156111,1.9403,-1.74854,0.198782,1.88185,-1.91094,0.141469,1.788,-1.62907,0.131452,1.66881,-1.54353,0.238341,1.70092,-1.66144,0.314608,1.71514,-1.71266,0.069411,1.95478,-2.11745,0.185965,1.88253,-1.98593,-0.031427,1.91566,-1.9969,-0.172775,1.85471,-2.13329,-0.21003,1.76665,-2.18543,-0.074699,1.62797,-2.15605,-0.04444,1.5721,-2.18052,0.103873,1.41962,-1.81155,-0.079835,1.50863,-1.88098,-0.080578,1.53529,-1.92703,-0.076638,1.51825,-1.99729,0.103466,1.46485,-2.06563,-0.025843,1.1176 [...]
+/*45*/{0.157728,1.94247,-1.75074,0.198445,1.8835,-1.91185,0.142096,1.79,-1.63007,0.131088,1.67045,-1.54483,0.237926,1.70183,-1.66351,0.313732,1.71584,-1.71476,0.06925,1.95625,-2.11922,0.186706,1.88398,-1.98625,-0.030078,1.91667,-1.99823,-0.17317,1.85494,-2.13223,-0.209988,1.76681,-2.18377,-0.073263,1.6294,-2.15456,-0.043968,1.57305,-2.18,0.104358,1.42237,-1.81209,-0.079976,1.51077,-1.88039,-0.080107,1.53812,-1.9273,-0.077575,1.52069,-1.99643,0.103444,1.46693,-2.06506,-0.026071,1.11979,-1 [...]
+/*46*/{0.15915,1.94364,-1.75113,0.198594,1.88629,-1.91377,0.142896,1.79136,-1.63126,0.131049,1.67126,-1.54513,0.237614,1.70228,-1.66434,0.311763,1.71612,-1.71684,0.070671,1.9571,-2.11995,0.187447,1.88525,-1.98783,-0.028733,1.91801,-1.99868,-0.17063,1.85573,-2.12966,-0.209798,1.76698,-2.18241,-0.073334,1.62949,-2.15375,-0.043155,1.57266,-2.1791,0.104636,1.42513,-1.81186,-0.079135,1.51286,-1.8805,-0.080448,1.54025,-1.92477,-0.077017,1.52234,-1.99601,0.104384,1.46853,-2.0645,-0.023914,1.120 [...]
+/*47*/{0.160098,1.94429,-1.75252,0.198679,1.88719,-1.91437,0.143776,1.79247,-1.63239,0.130575,1.67222,-1.54558,0.237214,1.70154,-1.66623,0.312194,1.71606,-1.71911,0.07163,1.95744,-2.12098,0.188196,1.88607,-1.98859,-0.027792,1.91932,-1.99878,-0.170834,1.85578,-2.12919,-0.209178,1.76667,-2.18172,-0.072405,1.62868,-2.15269,-0.041627,1.57217,-2.17864,0.104975,1.42681,-1.81132,-0.079185,1.5142,-1.87982,-0.080741,1.54223,-1.92566,-0.077161,1.52363,-1.99559,0.104002,1.46923,-2.06378,-0.022335,1 [...]
+/*48*/{0.161582,1.94457,-1.75316,0.200124,1.88803,-1.9162,0.143548,1.7931,-1.63367,0.129764,1.6729,-1.54682,0.236341,1.7012,-1.66762,0.310909,1.71279,-1.72017,0.073075,1.95798,-2.12206,0.188133,1.88642,-1.98971,-0.02679,1.92048,-2.00038,-0.170339,1.8551,-2.12807,-0.207544,1.76531,-2.18011,-0.069419,1.62805,-2.15197,-0.039799,1.57101,-2.17771,0.104888,1.42876,-1.81133,-0.078888,1.51561,-1.87957,-0.081143,1.54368,-1.92559,-0.076674,1.52395,-1.99553,0.104889,1.4708,-2.06273,-0.01642,1.12093 [...]
+/*49*/{0.162863,1.94485,-1.75438,0.199384,1.88821,-1.91685,0.142953,1.79364,-1.63504,0.128781,1.67267,-1.54707,0.235686,1.69957,-1.66892,0.310354,1.71002,-1.72197,0.074403,1.95786,-2.12357,0.188596,1.88632,-1.99064,-0.025549,1.9209,-2.00187,-0.169358,1.85437,-2.1266,-0.206259,1.76414,-2.18011,-0.068104,1.62619,-2.15091,-0.037832,1.56969,-2.17746,0.104232,1.43064,-1.81093,-0.079088,1.51594,-1.87945,-0.081433,1.54508,-1.926,-0.077103,1.5245,-1.99563,0.106764,1.47154,-2.0619,-0.016291,1.119 [...]
+/*50*/{0.163621,1.94417,-1.7558,0.199706,1.88865,-1.91824,0.142407,1.79336,-1.63631,0.126211,1.67348,-1.54853,0.234177,1.6978,-1.66976,0.30831,1.70697,-1.72328,0.075669,1.9572,-2.12433,0.189339,1.88611,-1.99098,-0.024926,1.92177,-2.00237,-0.167231,1.85271,-2.12574,-0.204491,1.76209,-2.17989,-0.065519,1.62449,-2.15046,-0.036297,1.56731,-2.17727,0.104306,1.43141,-1.81051,-0.079754,1.51644,-1.88024,-0.081442,1.54525,-1.92568,-0.077342,1.52413,-1.99581,0.106586,1.4715,-2.06105,-0.01238,1.117 [...]
+/*51*/{0.16499,1.9441,-1.75668,0.20132,1.88816,-1.91893,0.141457,1.79347,-1.63728,0.124478,1.67335,-1.55011,0.232775,1.69527,-1.67088,0.307055,1.70314,-1.72538,0.076632,1.95641,-2.12592,0.190314,1.88592,-1.99161,-0.023199,1.92138,-2.0052,-0.166097,1.85064,-2.1254,-0.202667,1.75971,-2.18023,-0.0635,1.62196,-2.15037,-0.033736,1.5646,-2.17699,0.104832,1.43242,-1.80997,-0.079782,1.51594,-1.88051,-0.081442,1.54525,-1.92568,-0.077367,1.52332,-1.99547,0.106406,1.47159,-2.05988,0.004514,1.11913, [...]
+/*52*/{0.165338,1.9433,-1.75824,0.200526,1.88763,-1.9197,0.139536,1.79316,-1.63822,0.12195,1.67206,-1.55072,0.231629,1.69242,-1.67216,0.306009,1.69892,-1.72615,0.077757,1.95557,-2.12693,0.190326,1.88555,-1.99206,-0.02308,1.9212,-2.00634,-0.165022,1.84893,-2.12502,-0.200134,1.75684,-2.18046,-0.061235,1.61895,-2.15,-0.030995,1.56122,-2.17685,0.104735,1.43288,-1.8097,-0.079867,1.51574,-1.88097,-0.081586,1.54509,-1.92571,-0.07804,1.52157,-1.99621,0.106693,1.47141,-2.0593,0.009503,1.11771,-1. [...]
+/*53*/{0.16545,1.94126,-1.75878,0.200154,1.88722,-1.92003,0.13819,1.79244,-1.63948,0.119802,1.67176,-1.55239,0.230059,1.68916,-1.67348,0.304261,1.69398,-1.72731,0.078817,1.95409,-2.12789,0.190605,1.88423,-1.99346,-0.022692,1.92083,-2.00807,-0.162066,1.84601,-2.1247,-0.197925,1.75338,-2.18142,-0.057645,1.61556,-2.14975,-0.028227,1.55756,-2.17729,0.104587,1.43283,-1.80919,-0.080466,1.5144,-1.88162,-0.082098,1.54414,-1.92616,-0.078152,1.52063,-1.99663,0.106868,1.47077,-2.05854,0.016831,1.11 [...]
+/*54*/{0.166056,1.94023,-1.76027,0.200167,1.88529,-1.92105,0.13564,1.79108,-1.64065,0.116036,1.67091,-1.55362,0.227476,1.68561,-1.67388,0.302088,1.68863,-1.72862,0.079944,1.95245,-2.12953,0.190076,1.88287,-1.99334,-0.02154,1.91956,-2.0097,-0.16019,1.84226,-2.12447,-0.194833,1.74886,-2.18221,-0.054859,1.61127,-2.15026,-0.024548,1.55357,-2.17775,0.10467,1.43278,-1.80883,-0.080885,1.51299,-1.88164,-0.082378,1.54265,-1.92693,-0.078526,1.51902,-1.99802,0.107509,1.46909,-2.05817,0.023005,1.113 [...]
+/*55*/{0.166007,1.93918,-1.76145,0.20058,1.88344,-1.92158,0.134475,1.78987,-1.64162,0.113068,1.66893,-1.55482,0.225718,1.68139,-1.67467,0.299729,1.68295,-1.72922,0.080897,1.9504,-2.13044,0.190496,1.88115,-1.99374,-0.021424,1.91862,-2.01226,-0.158681,1.83818,-2.12438,-0.191655,1.74452,-2.18327,-0.05142,1.60723,-2.15137,-0.020526,1.54907,-2.17842,0.104697,1.43235,-1.80864,-0.080849,1.51131,-1.88173,-0.083066,1.54085,-1.92769,-0.078607,1.51636,-1.9976,0.106326,1.46768,-2.05843,0.029403,1.11 [...]
+/*56*/{0.165742,1.93786,-1.762,0.200019,1.8819,-1.92192,0.131008,1.78855,-1.64245,0.109085,1.66738,-1.55625,0.222333,1.67609,-1.67542,0.29839,1.67652,-1.72985,0.082317,1.94803,-2.1312,0.190468,1.87878,-1.99404,-0.021123,1.91698,-2.0127,-0.156804,1.83416,-2.12574,-0.1881,1.73934,-2.18529,-0.048451,1.60229,-2.15222,-0.017219,1.54451,-2.17934,0.10464,1.43184,-1.8084,-0.082252,1.50935,-1.88375,-0.082194,1.53858,-1.92797,-0.079273,1.5142,-1.99913,0.10681,1.46553,-2.05891,0.035602,1.10835,-1.7 [...]
+/*57*/{0.165515,1.93491,-1.76276,0.200159,1.88017,-1.92263,0.128367,1.78681,-1.64294,0.105558,1.66569,-1.55751,0.219666,1.67154,-1.67617,0.29465,1.66988,-1.73006,0.083587,1.94509,-2.1328,0.190596,1.87627,-1.99426,-0.020751,1.9152,-2.01629,-0.154905,1.82932,-2.12609,-0.185354,1.73403,-2.18634,-0.044021,1.59763,-2.15365,-0.01404,1.53939,-2.18086,0.104317,1.43051,-1.8084,-0.082016,1.50725,-1.88383,-0.081637,1.53623,-1.92828,-0.07875,1.5119,-1.99961,0.10606,1.46319,-2.05932,0.041639,1.10614, [...]
+/*58*/{0.16453,1.93205,-1.76334,0.200634,1.8764,-1.92287,0.125826,1.78426,-1.64352,0.10208,1.66419,-1.55907,0.216184,1.66674,-1.67671,0.292682,1.66324,-1.7304,0.084066,1.94222,-2.13383,0.190832,1.87378,-1.99461,-0.020347,1.91214,-2.01792,-0.152761,1.82357,-2.1281,-0.182221,1.72796,-2.18876,-0.040386,1.59186,-2.15579,-0.009464,1.53354,-2.18195,0.104538,1.42871,-1.80874,-0.082105,1.50489,-1.88373,-0.082742,1.53374,-1.92902,-0.079608,1.50893,-2.00001,0.106768,1.46088,-2.05979,0.044857,1.104 [...]
+/*59*/{0.163495,1.92923,-1.76335,0.200155,1.87364,-1.92331,0.122698,1.78242,-1.64411,0.097048,1.66085,-1.56008,0.212536,1.66108,-1.67691,0.288604,1.65516,-1.73013,0.084176,1.93892,-2.13441,0.191122,1.87067,-1.99498,-0.021332,1.90994,-2.0194,-0.150815,1.81717,-2.13015,-0.179307,1.72125,-2.19107,-0.037182,1.58606,-2.15753,-0.006131,1.52765,-2.18339,0.104627,1.42733,-1.80869,-0.082702,1.50179,-1.88458,-0.082864,1.53041,-1.92944,-0.080316,1.5057,-2.00054,0.105079,1.45786,-2.06051,0.050539,1. [...]
+/*60*/{0.162156,1.92692,-1.76392,0.200339,1.87001,-1.92354,0.120351,1.77901,-1.64403,0.092562,1.65869,-1.5612,0.208422,1.65586,-1.67731,0.284858,1.64718,-1.72912,0.084219,1.93527,-2.13636,0.191404,1.86766,-1.9955,-0.020293,1.90667,-2.02173,-0.14891,1.81048,-2.13133,-0.176212,1.71427,-2.19329,-0.032132,1.58046,-2.16061,-0.001709,1.52129,-2.1854,0.104611,1.42513,-1.80847,-0.082688,1.49922,-1.88416,-0.082462,1.5275,-1.92943,-0.080736,1.50188,-2.00069,0.104875,1.45517,-2.06162,0.058017,1.098 [...]
+/*61*/{0.161836,1.92375,-1.76414,0.20015,1.86703,-1.92342,0.117061,1.7765,-1.64518,0.087356,1.65596,-1.56347,0.204131,1.64942,-1.67746,0.281071,1.63876,-1.72876,0.085059,1.93106,-2.13721,0.190919,1.86414,-1.99591,-0.020668,1.90336,-2.02368,-0.148911,1.80362,-2.13465,-0.173477,1.70672,-2.19572,-0.028657,1.57334,-2.16269,0.002694,1.51461,-2.18687,0.10553,1.42315,-1.80889,-0.082639,1.49561,-1.88409,-0.083275,1.52347,-1.92944,-0.081551,1.49804,-2.00041,0.104526,1.45269,-2.06213,0.062656,1.09 [...]
+/*62*/{0.160805,1.9202,-1.76401,0.199964,1.86308,-1.92413,0.114056,1.77341,-1.64514,0.081731,1.65306,-1.56497,0.199566,1.64325,-1.67753,0.277426,1.62979,-1.72702,0.085599,1.92684,-2.13786,0.190735,1.86046,-1.9961,-0.020796,1.89895,-2.02608,-0.146029,1.79566,-2.13742,-0.17013,1.69866,-2.19842,-0.025075,1.56622,-2.16597,0.007281,1.50765,-2.18928,0.106088,1.42066,-1.8096,-0.082959,1.49125,-1.88389,-0.083874,1.52042,-1.92945,-0.082575,1.49456,-2.00044,0.103601,1.45002,-2.06301,0.067433,1.093 [...]
+/*63*/{0.160152,1.91641,-1.76404,0.199532,1.85839,-1.92402,0.11073,1.77047,-1.64524,0.076997,1.64991,-1.56539,0.195309,1.63655,-1.67749,0.272779,1.62089,-1.72582,0.086134,1.9222,-2.13915,0.190806,1.85627,-1.99649,-0.02078,1.89505,-2.02793,-0.145451,1.78771,-2.14067,-0.166522,1.69046,-2.20151,-0.021162,1.55871,-2.16948,0.011513,1.50061,-2.19048,0.105812,1.41764,-1.81018,-0.082879,1.48765,-1.884,-0.082933,1.51601,-1.92935,-0.083241,1.49039,-1.99938,0.103121,1.44674,-2.06463,0.07193,1.0904, [...]
+/*64*/{0.158518,1.91147,-1.76382,0.199699,1.85374,-1.92354,0.107329,1.76674,-1.64587,0.070396,1.64686,-1.56718,0.189893,1.63001,-1.67789,0.267921,1.61141,-1.72428,0.086558,1.91713,-2.14026,0.191527,1.85205,-1.99681,-0.021671,1.89025,-2.02983,-0.143457,1.77894,-2.14422,-0.16327,1.68141,-2.20524,-0.015362,1.55178,-2.17246,0.016848,1.49284,-2.19265,0.105634,1.41514,-1.81056,-0.083303,1.48318,-1.88363,-0.083539,1.51214,-1.9291,-0.08421,1.486,-1.99869,0.102295,1.44362,-2.06531,0.076734,1.0883 [...]
+/*65*/{0.157032,1.90776,-1.76408,0.200699,1.84958,-1.92328,0.103678,1.76218,-1.64577,0.065619,1.64423,-1.56857,0.184679,1.62252,-1.67733,0.263245,1.60176,-1.72328,0.086789,1.912,-2.14178,0.191309,1.84736,-1.99701,-0.02196,1.88544,-2.03225,-0.142824,1.77028,-2.14816,-0.160068,1.67269,-2.20827,-0.011273,1.54353,-2.17584,0.022092,1.48513,-2.19377,0.105904,1.4118,-1.81155,-0.084027,1.47867,-1.88397,-0.083694,1.50775,-1.92923,-0.084531,1.48073,-1.99806,0.101517,1.44047,-2.06693,0.081429,1.084 [...]
+/*66*/{0.156295,1.90374,-1.76376,0.199787,1.84401,-1.92321,0.100587,1.75827,-1.64609,0.05874,1.64038,-1.56969,0.178925,1.61518,-1.67674,0.257617,1.59185,-1.72179,0.087433,1.90624,-2.14283,0.191256,1.84256,-1.99749,-0.022687,1.88016,-2.03371,-0.142014,1.76076,-2.15297,-0.156594,1.66303,-2.2116,-0.006799,1.5344,-2.17814,0.027722,1.47738,-2.19566,0.106417,1.40855,-1.8129,-0.083522,1.47376,-1.88331,-0.084436,1.50355,-1.92945,-0.085775,1.47601,-1.99749,0.100689,1.43743,-2.0681,0.085808,1.0823 [...]
+/*67*/{0.155222,1.8986,-1.76356,0.198668,1.83804,-1.92283,0.096896,1.75374,-1.64646,0.051537,1.6381,-1.57051,0.17339,1.6079,-1.67698,0.251675,1.58175,-1.71964,0.087656,1.90071,-2.14405,0.191632,1.83717,-1.99715,-0.023275,1.87419,-2.03558,-0.140104,1.7512,-2.15573,-0.15216,1.65357,-2.21526,-0.001167,1.52759,-2.18127,0.03337,1.46906,-2.19705,0.105979,1.40468,-1.81326,-0.083589,1.46832,-1.88273,-0.085228,1.49887,-1.92879,-0.086223,1.47058,-1.99642,0.101046,1.43457,-2.06963,0.090294,1.07833, [...]
+/*68*/{0.153779,1.89357,-1.7632,0.199478,1.83281,-1.92236,0.093438,1.74917,-1.64703,0.045532,1.6339,-1.57096,0.167301,1.60024,-1.67609,0.245261,1.57113,-1.71737,0.088266,1.89523,-2.14572,0.191961,1.83312,-1.99688,-0.023534,1.86866,-2.03827,-0.138277,1.74088,-2.15996,-0.147939,1.64361,-2.21876,0.004432,1.51869,-2.18333,0.039866,1.46088,-2.19838,0.106643,1.40044,-1.8141,-0.084272,1.46333,-1.88251,-0.086337,1.49349,-1.9301,-0.087875,1.46537,-1.99583,0.099776,1.4304,-2.07134,0.094057,1.07668 [...]
+/*69*/{0.153108,1.88843,-1.76285,0.199672,1.82785,-1.92206,0.089725,1.74462,-1.647,0.039283,1.63125,-1.57187,0.161247,1.59235,-1.67491,0.239054,1.56126,-1.71543,0.089598,1.88926,-2.14716,0.192187,1.82709,-1.99697,-0.024598,1.8636,-2.03998,-0.13507,1.73099,-2.16413,-0.143438,1.63336,-2.22263,0.009808,1.50934,-2.18521,0.04607,1.45239,-2.19923,0.106549,1.39606,-1.8154,-0.085038,1.4574,-1.88249,-0.086363,1.4883,-1.92972,-0.08921,1.45999,-1.99574,0.099522,1.42602,-2.07368,0.09896,1.07214,-1.7 [...]
+/*70*/{0.152139,1.883,-1.76242,0.200546,1.82257,-1.921,0.086379,1.7398,-1.6478,0.033012,1.62756,-1.57287,0.154987,1.58453,-1.67445,0.231715,1.55109,-1.71306,0.090461,1.88322,-2.14833,0.192082,1.82177,-1.99707,-0.02547,1.85676,-2.04229,-0.133357,1.7206,-2.16933,-0.138381,1.62281,-2.22628,0.017115,1.50036,-2.1872,0.053096,1.44358,-2.20047,0.107241,1.39182,-1.81685,-0.086128,1.45254,-1.8814,-0.087653,1.48287,-1.92984,-0.091217,1.45563,-1.99528,0.09777,1.42195,-2.07558,0.104061,1.06885,-1.76 [...]
+/*71*/{0.150978,1.87769,-1.76203,0.200207,1.81597,-1.92084,0.082681,1.73451,-1.64823,0.026408,1.62437,-1.57333,0.148376,1.57698,-1.67322,0.225133,1.54181,-1.71048,0.091722,1.87709,-2.14986,0.192738,1.81635,-1.99632,-0.026041,1.85112,-2.04423,-0.129551,1.70959,-2.17388,-0.133226,1.61234,-2.22956,0.022606,1.4913,-2.18856,0.060128,1.43475,-2.20139,0.106771,1.38677,-1.81804,-0.08692,1.44675,-1.88121,-0.089383,1.47731,-1.93005,-0.09332,1.45141,-1.99533,0.095553,1.41771,-2.0768,0.108359,1.0662 [...]
+/*72*/{0.150477,1.87256,-1.76112,0.200568,1.80969,-1.91942,0.078402,1.72961,-1.64874,0.019888,1.62146,-1.57387,0.141325,1.56944,-1.6719,0.217954,1.53189,-1.70782,0.092946,1.87174,-2.15154,0.193952,1.81057,-1.99676,-0.025457,1.8457,-2.0452,-0.127193,1.69833,-2.17919,-0.126807,1.6016,-2.23362,0.030631,1.4822,-2.19016,0.067503,1.42654,-2.20202,0.106104,1.38137,-1.81888,-0.08959,1.44135,-1.88047,-0.091473,1.47129,-1.92922,-0.097062,1.44687,-1.99515,0.09361,1.41452,-2.07799,0.112731,1.06326,- [...]
+/*73*/{0.149454,1.86677,-1.76095,0.20252,1.8045,-1.91916,0.07429,1.72479,-1.6492,0.012876,1.61847,-1.5751,0.134264,1.56248,-1.6699,0.209697,1.52341,-1.70518,0.094395,1.86638,-2.15355,0.194558,1.80515,-1.99608,-0.024355,1.84362,-2.04533,-0.122707,1.68779,-2.18528,-0.120788,1.59041,-2.23715,0.037798,1.47327,-2.19161,0.075654,1.41867,-2.20276,0.10494,1.37684,-1.82074,-0.091463,1.43601,-1.88028,-0.093506,1.46561,-1.92847,-0.099905,1.44356,-1.99495,0.090313,1.41142,-2.07907,0.117398,1.05983,- [...]
+/*74*/{0.148476,1.86205,-1.75955,0.202141,1.79843,-1.91794,0.071031,1.72071,-1.64959,0.005666,1.61607,-1.57652,0.127636,1.55637,-1.66846,0.201318,1.51464,-1.70226,0.095677,1.86117,-2.15554,0.195917,1.79935,-1.99599,-0.023595,1.84098,-2.04559,-0.117731,1.67735,-2.19197,-0.113375,1.57952,-2.24067,0.045782,1.46565,-2.19253,0.085057,1.41115,-2.20328,0.103725,1.37194,-1.82339,-0.093663,1.43172,-1.87998,-0.096456,1.46114,-1.92745,-0.102731,1.43943,-1.99386,0.087201,1.40855,-2.08053,0.12201,1.0 [...]
+/*75*/{0.148395,1.85805,-1.75853,0.200939,1.79464,-1.9166,0.067171,1.71682,-1.65002,-0.001543,1.61425,-1.5776,0.120345,1.55028,-1.6671,0.193373,1.50709,-1.69951,0.097705,1.85602,-2.15669,0.196287,1.79492,-1.99544,-0.023209,1.8387,-2.04507,-0.109403,1.66721,-2.19921,-0.105687,1.56929,-2.24393,0.0548,1.45803,-2.19331,0.094411,1.40424,-2.20343,0.102567,1.36754,-1.82551,-0.096038,1.42687,-1.87851,-0.098758,1.45768,-1.92757,-0.105655,1.43665,-1.9922,0.085191,1.40615,-2.08226,0.126645,1.05888, [...]
+/*76*/{0.147976,1.85369,-1.7574,0.201374,1.79023,-1.91621,0.063144,1.71426,-1.65004,-0.009334,1.61307,-1.57854,0.112964,1.5453,-1.6654,0.184789,1.50017,-1.69682,0.098641,1.85144,-2.15768,0.196324,1.79067,-1.99434,-0.022383,1.83714,-2.04614,-0.102779,1.65709,-2.20575,-0.095022,1.55876,-2.24642,0.06437,1.45167,-2.19381,0.104448,1.39849,-2.20369,0.101475,1.36414,-1.82758,-0.097517,1.42263,-1.87716,-0.099458,1.45664,-1.92658,-0.108414,1.43427,-1.99023,0.082612,1.40465,-2.08377,0.132987,1.060 [...]
+/*77*/{0.147389,1.8503,-1.75652,0.20102,1.78717,-1.91561,0.057897,1.71259,-1.65096,-0.017168,1.61305,-1.58058,0.10507,1.54154,-1.66394,0.175311,1.49452,-1.69389,0.100943,1.84803,-2.15742,0.196227,1.78694,-1.99366,-0.022185,1.83457,-2.0464,-0.095746,1.64942,-2.2118,-0.084568,1.54976,-2.24836,0.074383,1.44591,-2.19392,0.115418,1.39277,-2.20323,0.100595,1.35968,-1.829,-0.099421,1.42024,-1.87529,-0.100731,1.45473,-1.92592,-0.109384,1.43202,-1.98806,0.080778,1.40352,-2.0854,0.136916,1.06441,- [...]
+/*78*/{0.147369,1.84712,-1.75548,0.202182,1.78438,-1.91488,0.053706,1.71134,-1.65122,-0.024736,1.6142,-1.58244,0.097369,1.53884,-1.66312,0.167512,1.49066,-1.6913,0.102874,1.84509,-2.15701,0.197007,1.78465,-1.99322,-0.021737,1.83313,-2.04704,-0.088149,1.64136,-2.21664,-0.074002,1.54202,-2.25049,0.084764,1.44178,-2.19339,0.126891,1.38904,-2.20279,0.099093,1.35634,-1.82955,-0.100485,1.41878,-1.87426,-0.102471,1.45423,-1.92605,-0.111354,1.43131,-1.98592,0.078726,1.40247,-2.08694,0.143459,1.0 [...]
+/*79*/{0.146653,1.84354,-1.75472,0.200714,1.78212,-1.91444,0.048539,1.71179,-1.65166,-0.032825,1.61693,-1.58399,0.088782,1.5367,-1.66193,0.158569,1.48641,-1.68812,0.105015,1.84308,-2.15603,0.196896,1.78195,-1.99256,-0.020964,1.83116,-2.0475,-0.079994,1.63473,-2.22034,-0.062874,1.5364,-2.25162,0.095405,1.43729,-2.19287,0.138694,1.387,-2.20219,0.097878,1.354,-1.83083,-0.101636,1.41875,-1.87268,-0.104085,1.4532,-1.92524,-0.111324,1.43047,-1.9845,0.077186,1.40134,-2.08848,0.150489,1.06958,-1 [...]
+/*80*/{0.14619,1.84213,-1.75435,0.200608,1.78022,-1.91318,0.043225,1.71231,-1.65199,-0.040416,1.61974,-1.58648,0.080787,1.53669,-1.66119,0.150165,1.48394,-1.68569,0.106982,1.8413,-2.15555,0.197186,1.77941,-1.99171,-0.020216,1.83019,-2.04816,-0.070672,1.62897,-2.22207,-0.052691,1.53161,-2.25222,0.106324,1.43525,-2.19138,0.15103,1.38609,-2.20124,0.095882,1.35197,-1.83018,-0.102208,1.41842,-1.87229,-0.104126,1.45276,-1.92559,-0.112453,1.43049,-1.98353,0.075768,1.40018,-2.08954,0.160627,1.07 [...]
+/*81*/{0.145903,1.84052,-1.75437,0.199016,1.77768,-1.91201,0.038495,1.7143,-1.65273,-0.04812,1.62364,-1.58824,0.073005,1.53748,-1.66013,0.14107,1.4827,-1.68282,0.109264,1.83972,-2.15542,0.197586,1.77851,-1.99057,-0.019929,1.82939,-2.0488,-0.064078,1.62522,-2.2244,-0.043075,1.5283,-2.25297,0.117974,1.4346,-2.19028,0.162498,1.38707,-2.19973,0.094712,1.35035,-1.83038,-0.102494,1.41876,-1.87202,-0.10534,1.45192,-1.92612,-0.112521,1.43057,-1.98338,0.07491,1.39941,-2.09044,0.166042,1.07955,-1. [...]
+/*82*/{0.145118,1.84026,-1.75387,0.198605,1.77703,-1.91171,0.032963,1.71628,-1.65323,-0.055362,1.62772,-1.59018,0.065407,1.53803,-1.65902,0.132791,1.48203,-1.68098,0.111435,1.83923,-2.15484,0.197491,1.77754,-1.99006,-0.018468,1.82816,-2.04955,-0.056249,1.62325,-2.22575,-0.034015,1.52554,-2.25357,0.129654,1.43643,-2.18901,0.174176,1.38951,-2.19771,0.09387,1.35038,-1.8311,-0.102926,1.41876,-1.87244,-0.105392,1.45171,-1.92636,-0.113116,1.43049,-1.98377,0.074236,1.39839,-2.09112,0.173312,1.0 [...]
+/*83*/{0.14353,1.84009,-1.75345,0.19767,1.77561,-1.91,0.028591,1.71832,-1.65363,-0.063579,1.6323,-1.59237,0.058828,1.53986,-1.65773,0.124003,1.48312,-1.67989,0.113976,1.83902,-2.15417,0.197698,1.77634,-1.98847,-0.017893,1.82782,-2.0506,-0.04978,1.62039,-2.22691,-0.025123,1.52346,-2.25421,0.138897,1.43835,-2.18716,0.185828,1.3933,-2.19523,0.092663,1.35118,-1.82977,-0.103282,1.41891,-1.87286,-0.105428,1.45168,-1.92669,-0.113024,1.43019,-1.98461,0.072717,1.39628,-2.09295,0.181233,1.0915,-1. [...]
+/*84*/{0.141083,1.84097,-1.75306,0.19767,1.77561,-1.91,0.022878,1.72238,-1.65436,-0.070652,1.63797,-1.59423,0.050294,1.5421,-1.65681,0.115912,1.48434,-1.67736,0.115838,1.83945,-2.15367,0.197347,1.7762,-1.98798,-0.01675,1.82749,-2.0519,-0.042597,1.61969,-2.22816,-0.01669,1.52275,-2.25463,0.148857,1.44201,-2.18524,0.197528,1.39871,-2.19224,0.091872,1.35238,-1.82885,-0.103552,1.41949,-1.87384,-0.106434,1.45203,-1.92784,-0.112977,1.43101,-1.98558,0.076628,1.39614,-2.09274,0.18792,1.09718,-1. [...]
+/*85*/{0.139014,1.84212,-1.75186,0.196433,1.77618,-1.9077,0.017346,1.7266,-1.65519,-0.07722,1.64276,-1.59589,0.042498,1.54579,-1.65615,0.1075,1.48664,-1.67535,0.118455,1.84065,-2.15309,0.197833,1.77664,-1.98622,-0.015374,1.82786,-2.053,-0.037331,1.61844,-2.22903,-0.00853,1.52256,-2.25464,0.158084,1.44695,-2.18257,0.207954,1.40583,-2.18922,0.09106,1.35375,-1.82821,-0.1046,1.41955,-1.87465,-0.106301,1.45209,-1.92856,-0.113069,1.4314,-1.98703,0.083814,1.39715,-2.09135,0.19638,1.1032,-1.7643 [...]
+/*86*/{0.136568,1.84428,-1.75116,0.196172,1.7786,-1.90648,0.012741,1.73057,-1.65494,-0.084778,1.64832,-1.59743,0.035513,1.55002,-1.65541,0.098468,1.49069,-1.67453,0.120926,1.84227,-2.15181,0.198117,1.77863,-1.98472,-0.014679,1.82808,-2.0545,-0.034142,1.61812,-2.22968,-0.001162,1.5227,-2.25495,0.167127,1.45341,-2.17994,0.218348,1.41378,-2.18584,0.089836,1.35629,-1.82817,-0.10512,1.42011,-1.87613,-0.106681,1.45231,-1.92977,-0.112804,1.43143,-1.98843,0.075855,1.39578,-2.09297,0.201795,1.110 [...]
+/*87*/{0.134118,1.84643,-1.7508,0.196036,1.7798,-1.90481,0.00627,1.73557,-1.65544,-0.091842,1.65331,-1.59869,0.027627,1.5545,-1.65537,0.091689,1.4942,-1.67301,0.122823,1.84479,-2.15104,0.198524,1.77966,-1.98287,-0.014308,1.82826,-2.05474,-0.029752,1.61814,-2.22963,0.006187,1.52416,-2.25501,0.175534,1.46051,-2.17766,0.227947,1.42249,-2.18168,0.089572,1.35724,-1.82633,-0.104959,1.42137,-1.87755,-0.106198,1.45269,-1.93077,-0.112738,1.43211,-1.99009,0.077838,1.39488,-2.09298,0.207207,1.11783 [...]
+/*88*/{0.130987,1.84905,-1.74974,0.195739,1.78168,-1.90283,0.001377,1.74065,-1.65626,-0.099086,1.65936,-1.60026,0.020465,1.5595,-1.65551,0.084116,1.4988,-1.67258,0.124622,1.84783,-2.14992,0.199094,1.78176,-1.98093,-0.013224,1.8298,-2.05537,-0.025277,1.61898,-2.22958,0.012901,1.52598,-2.25537,0.1835,1.46885,-2.1748,0.237508,1.43272,-2.17786,0.088685,1.35849,-1.8257,-0.105551,1.42254,-1.87911,-0.105473,1.45481,-1.93216,-0.112307,1.43367,-1.99116,0.075405,1.39355,-2.09376,0.212453,1.12504,- [...]
+/*89*/{0.127683,1.8521,-1.74901,0.195396,1.78474,-1.90138,-0.003525,1.7457,-1.65633,-0.105529,1.66565,-1.60227,0.013755,1.56535,-1.65569,0.076613,1.50419,-1.67138,0.126508,1.85132,-2.1489,0.198843,1.78438,-1.97926,-0.01229,1.83074,-2.05597,-0.021632,1.62024,-2.22863,0.020261,1.52815,-2.25535,0.190533,1.47628,-2.17171,0.246554,1.44255,-2.17379,0.088651,1.36103,-1.82549,-0.104591,1.42384,-1.88068,-0.104708,1.45569,-1.93341,-0.110905,1.43509,-1.99266,0.077282,1.39404,-2.09342,0.21468,1.1322 [...]
+/*90*/{0.125173,1.85548,-1.74824,0.195876,1.78782,-1.89926,-0.007868,1.75047,-1.65709,-0.111946,1.67216,-1.60396,0.006727,1.57144,-1.65691,0.069377,1.50964,-1.67098,0.128314,1.85541,-2.14788,0.19959,1.78782,-1.97696,-0.011438,1.8323,-2.05685,-0.016147,1.62215,-2.22752,0.02696,1.53066,-2.25552,0.198486,1.48569,-2.16838,0.255493,1.45294,-2.16859,0.090215,1.36378,-1.82427,-0.10492,1.42625,-1.88244,-0.10413,1.45839,-1.93483,-0.109925,1.437,-1.99472,0.079235,1.39423,-2.09322,0.219923,1.13998, [...]
+/*91*/{0.121382,1.85885,-1.74788,0.194648,1.79188,-1.89747,-0.012969,1.75572,-1.65762,-0.119431,1.67815,-1.60609,0.00081,1.57776,-1.65719,0.063181,1.51589,-1.67114,0.129404,1.85886,-2.14672,0.200281,1.79134,-1.97556,-0.010373,1.83449,-2.05752,-0.012341,1.62413,-2.2265,0.03325,1.53387,-2.25595,0.205257,1.49499,-2.16504,0.263117,1.46424,-2.16431,0.089893,1.36639,-1.82491,-0.10405,1.42883,-1.88335,-0.102262,1.46034,-1.93648,-0.108363,1.43884,-1.99625,0.078093,1.39476,-2.0956,0.222053,1.1469 [...]
+/*92*/{0.118627,1.86285,-1.74689,0.194065,1.79492,-1.89536,-0.017384,1.76075,-1.65847,-0.124262,1.68494,-1.60814,-0.004673,1.58489,-1.65813,0.056149,1.52205,-1.67063,0.130362,1.86319,-2.14588,0.200738,1.79518,-1.97387,-0.009739,1.83711,-2.05824,-0.008936,1.62645,-2.22537,0.039969,1.53754,-2.25588,0.211432,1.50487,-2.16105,0.269907,1.47587,-2.1597,0.089542,1.36939,-1.82494,-0.103092,1.43096,-1.88573,-0.101935,1.46215,-1.93846,-0.106654,1.44155,-1.99807,0.080646,1.395,-2.09523,0.225161,1.1 [...]
+/*93*/{0.115203,1.8664,-1.74666,0.194269,1.79826,-1.89307,-0.021673,1.76631,-1.6588,-0.130253,1.69207,-1.61022,-0.010755,1.59132,-1.65956,0.050521,1.52881,-1.67139,0.132003,1.8679,-2.14475,0.201117,1.79906,-1.97143,-0.008394,1.83952,-2.05818,-0.006429,1.63118,-2.22499,0.046766,1.5417,-2.25617,0.216501,1.51456,-2.15705,0.276237,1.48853,-2.15508,0.091362,1.37321,-1.82512,-0.101257,1.43468,-1.88713,-0.099994,1.46548,-1.93953,-0.104534,1.44396,-1.99944,0.082549,1.39558,-2.09496,0.229029,1.16 [...]
+/*94*/{0.112771,1.87085,-1.74613,0.193659,1.80315,-1.89234,-0.025476,1.77178,-1.65971,-0.13627,1.69809,-1.61187,-0.016291,1.59754,-1.66022,0.044412,1.53541,-1.67077,0.133763,1.87264,-2.14321,0.201815,1.80311,-1.97025,-0.008216,1.8425,-2.05848,-0.002144,1.63418,-2.22418,0.053061,1.54608,-2.25563,0.222049,1.52519,-2.15371,0.282356,1.50095,-2.15039,0.092033,1.37674,-1.82518,-0.100161,1.43828,-1.88794,-0.098674,1.46798,-1.94176,-0.101979,1.44686,-2.00109,0.083575,1.39551,-2.09596,0.231141,1. [...]
+/*95*/{0.110561,1.87445,-1.74585,0.194162,1.8061,-1.88998,-0.029027,1.77725,-1.66058,-0.140668,1.7053,-1.61482,-0.021795,1.6046,-1.66168,0.039997,1.54159,-1.67127,0.134608,1.87759,-2.14207,0.202494,1.80758,-1.96819,-0.006748,1.84609,-2.05863,0.001704,1.6381,-2.224,0.058704,1.55082,-2.25623,0.22567,1.534,-2.14987,0.287677,1.5134,-2.14478,0.09326,1.3804,-1.82557,-0.098602,1.44159,-1.88994,-0.095964,1.47096,-1.94296,-0.099976,1.45026,-2.00151,0.087671,1.39562,-2.09494,0.233717,1.17472,-1.76 [...]
+/*96*/{0.10809,1.87871,-1.74551,0.193548,1.81249,-1.88901,-0.031905,1.78244,-1.66114,-0.146185,1.71202,-1.6172,-0.026282,1.61166,-1.66343,0.034709,1.5487,-1.67186,0.135873,1.88275,-2.14112,0.202928,1.81198,-1.96651,-0.005667,1.84935,-2.05875,0.005129,1.6426,-2.22441,0.064609,1.55602,-2.25682,0.230197,1.54546,-2.14622,0.291937,1.52616,-2.14052,0.095943,1.38504,-1.82738,-0.096925,1.44541,-1.89039,-0.094039,1.47398,-1.94498,-0.097102,1.45305,-2.00329,0.089737,1.39587,-2.09486,0.23519,1.1820 [...]
+/*97*/{0.106341,1.88267,-1.74506,0.193594,1.81668,-1.88713,-0.035294,1.78744,-1.66238,-0.150882,1.7188,-1.6193,-0.030666,1.61801,-1.66411,0.030927,1.55485,-1.67249,0.137745,1.88821,-2.1392,0.20385,1.81663,-1.96482,-0.004218,1.85195,-2.05874,0.009506,1.64638,-2.22415,0.070875,1.56142,-2.25689,0.233554,1.555,-2.14224,0.295814,1.53869,-2.13599,0.097865,1.3892,-1.82772,-0.094575,1.44991,-1.89122,-0.092061,1.47768,-1.94691,-0.09391,1.45636,-2.00426,0.092373,1.3965,-2.09556,0.238744,1.18835,-1 [...]
+/*98*/{0.104765,1.88632,-1.74505,0.194039,1.82161,-1.88613,-0.038559,1.79286,-1.66377,-0.154195,1.72547,-1.62203,-0.035594,1.62435,-1.66562,0.025821,1.56108,-1.67257,0.138828,1.89317,-2.13838,0.205428,1.82054,-1.96279,-0.003569,1.85535,-2.05893,0.012847,1.65184,-2.22513,0.075758,1.56662,-2.25712,0.23628,1.56425,-2.13783,0.299026,1.55059,-2.13146,0.099026,1.39254,-1.82878,-0.092675,1.45431,-1.8929,-0.089245,1.48096,-1.94899,-0.09054,1.45937,-2.00541,0.094139,1.39676,-2.09544,0.241284,1.19 [...]
+/*99*/{0.103264,1.88994,-1.74431,0.193867,1.82476,-1.88424,-0.042012,1.79848,-1.66505,-0.157604,1.73203,-1.62521,-0.039559,1.63095,-1.66675,0.022368,1.56783,-1.67322,0.140922,1.89861,-2.13767,0.205251,1.82459,-1.96116,-0.001953,1.85919,-2.05958,0.016099,1.65655,-2.2254,0.08221,1.57196,-2.25732,0.238674,1.57499,-2.1347,0.302388,1.56185,-2.12815,0.101484,1.39746,-1.82901,-0.090984,1.45914,-1.894,-0.086773,1.48497,-1.95062,-0.087648,1.46287,-2.00584,0.097337,1.39761,-2.0955,0.242531,1.20093 [...]
+/*100*/{0.102153,1.89382,-1.74439,0.192625,1.82997,-1.88283,-0.044626,1.80349,-1.66665,-0.162468,1.73816,-1.62772,-0.043367,1.63663,-1.66772,0.017865,1.57329,-1.67339,0.142759,1.90361,-2.13542,0.205877,1.82969,-1.95994,-0.000951,1.862,-2.05965,0.020177,1.66145,-2.22547,0.086989,1.57723,-2.2578,0.240573,1.58447,-2.13198,0.304777,1.57311,-2.12422,0.103087,1.40165,-1.83072,-0.088153,1.46354,-1.89428,-0.084153,1.48821,-1.95275,-0.084798,1.46665,-2.00673,0.100751,1.39759,-2.09507,0.242719,1.2 [...]
+/*101*/{0.101149,1.89703,-1.74405,0.193543,1.83312,-1.88171,-0.046662,1.8084,-1.66792,-0.165796,1.74449,-1.63084,-0.046728,1.64292,-1.66932,0.013983,1.57886,-1.67355,0.143376,1.90883,-2.13449,0.206011,1.83374,-1.95799,0.000491,1.86573,-2.06037,0.024543,1.66727,-2.22658,0.092293,1.58271,-2.25813,0.242397,1.5938,-2.12908,0.306648,1.58421,-2.12069,0.103653,1.4057,-1.83073,-0.085581,1.46795,-1.89442,-0.081924,1.49171,-1.95389,-0.081456,1.46997,-2.00833,0.101536,1.39852,-2.09489,0.244704,1.21 [...]
+/*102*/{0.100218,1.90054,-1.74417,0.193961,1.83819,-1.88106,-0.049193,1.81374,-1.66934,-0.168846,1.75029,-1.6338,-0.049955,1.64795,-1.67073,0.011138,1.58407,-1.67344,0.144661,1.91302,-2.13293,0.206352,1.83728,-1.95705,0.002226,1.86807,-2.061,0.028005,1.67157,-2.22713,0.097831,1.58814,-2.25849,0.244237,1.60243,-2.12576,0.308627,1.59471,-2.11703,0.105964,1.40961,-1.83172,-0.083079,1.4722,-1.89519,-0.078812,1.4946,-1.95587,-0.077597,1.47292,-2.00845,0.105175,1.39907,-2.09419,0.246491,1.2165 [...]
+/*103*/{0.099471,1.90377,-1.74403,0.193577,1.84129,-1.87949,-0.051184,1.81853,-1.67128,-0.172341,1.75608,-1.63736,-0.053283,1.65336,-1.67154,0.007637,1.58895,-1.67349,0.146668,1.91738,-2.13148,0.207121,1.84142,-1.95592,0.003445,1.87132,-2.06128,0.032076,1.67636,-2.2283,0.102191,1.5934,-2.25905,0.246315,1.61118,-2.12347,0.309932,1.60439,-2.11384,0.107041,1.4146,-1.83252,-0.080511,1.47687,-1.89575,-0.077041,1.49855,-1.95666,-0.074945,1.47638,-2.01014,0.107494,1.40011,-2.09379,0.247952,1.22 [...]
+/*104*/{0.098631,1.90659,-1.74386,0.193601,1.8459,-1.87878,-0.052747,1.8229,-1.67326,-0.174481,1.76157,-1.64057,-0.055249,1.65788,-1.67213,0.004768,1.59316,-1.67362,0.147923,1.92164,-2.13048,0.207155,1.84512,-1.9544,0.005393,1.87472,-2.06235,0.035008,1.68129,-2.22932,0.106361,1.59853,-2.25915,0.246301,1.61813,-2.12153,0.311487,1.61358,-2.11077,0.108016,1.41817,-1.83251,-0.077185,1.48109,-1.89599,-0.07402,1.50201,-1.95847,-0.071302,1.47927,-2.01017,0.109928,1.40056,-2.0927,0.248911,1.2242 [...]
+/*105*/{0.098141,1.90935,-1.74383,0.192459,1.84889,-1.87779,-0.054534,1.82745,-1.67505,-0.178656,1.76592,-1.64384,-0.058151,1.66195,-1.67345,0.002324,1.59707,-1.67294,0.149791,1.92546,-2.12842,0.207666,1.84835,-1.95302,0.007072,1.8767,-2.06249,0.039355,1.6845,-2.23037,0.110342,1.60361,-2.26007,0.247624,1.62521,-2.11892,0.312548,1.62213,-2.10775,0.1104,1.42222,-1.83305,-0.075433,1.48624,-1.89681,-0.071432,1.5054,-1.96,-0.06795,1.48199,-2.01121,0.112125,1.4014,-2.09244,0.250548,1.22751,-1. [...]
+/*106*/{0.09771,1.9122,-1.74359,0.193194,1.85229,-1.87727,-0.05551,1.83126,-1.67697,-0.180304,1.77055,-1.64727,-0.059968,1.66634,-1.67434,0.000377,1.60094,-1.67287,0.151,1.92863,-2.12737,0.208261,1.85136,-1.95219,0.008198,1.87926,-2.06274,0.043953,1.68873,-2.23225,0.114726,1.60745,-2.2607,0.249484,1.63173,-2.1177,0.31378,1.62945,-2.10525,0.111513,1.42549,-1.83414,-0.07423,1.48998,-1.89762,-0.068598,1.50906,-1.96072,-0.065927,1.48504,-2.01211,0.114345,1.40152,-2.0914,0.250669,1.22944,-1.7 [...]
+/*107*/{0.097565,1.91443,-1.74363,0.194158,1.85535,-1.8755,-0.056768,1.83557,-1.67836,-0.182833,1.77504,-1.65132,-0.061936,1.66991,-1.67543,-0.001081,1.60418,-1.67257,0.152893,1.93193,-2.12623,0.209627,1.8545,-1.95073,0.010399,1.881,-2.06271,0.047102,1.69261,-2.23361,0.118709,1.611,-2.26102,0.250152,1.63792,-2.11621,0.314247,1.63676,-2.10276,0.112249,1.42895,-1.83414,-0.071538,1.49427,-1.89851,-0.066489,1.51207,-1.96244,-0.06373,1.48891,-2.01324,0.116526,1.40221,-2.09031,0.250965,1.23171 [...]
+/*108*/{0.097591,1.91649,-1.74338,0.194668,1.85745,-1.87482,-0.057975,1.83869,-1.68028,-0.183741,1.77839,-1.65405,-0.063303,1.67328,-1.67599,-0.003126,1.60733,-1.67238,0.154765,1.93427,-2.12451,0.210052,1.85708,-1.94984,0.012191,1.88344,-2.06335,0.052999,1.69606,-2.23556,0.121796,1.61476,-2.26173,0.251412,1.64277,-2.11528,0.315053,1.64222,-2.10057,0.113548,1.43203,-1.83443,-0.069334,1.49751,-1.89822,-0.063981,1.51516,-1.96311,-0.060417,1.49156,-2.01389,0.118062,1.40347,-2.09005,0.253512, [...]
+/*109*/{0.097719,1.91884,-1.74312,0.195136,1.85965,-1.87419,-0.058366,1.84158,-1.68165,-0.185567,1.78113,-1.65695,-0.064454,1.67632,-1.67726,-0.003449,1.61017,-1.67202,0.156336,1.93686,-2.1236,0.211002,1.85973,-1.94845,0.014308,1.88527,-2.06351,0.057202,1.69948,-2.23733,0.125326,1.61774,-2.26233,0.252504,1.64735,-2.11445,0.315692,1.64731,-2.09851,0.112689,1.43489,-1.83632,-0.067754,1.50102,-1.89879,-0.062147,1.51878,-1.96421,-0.058201,1.4943,-2.01364,0.12009,1.40437,-2.08963,0.253309,1.2 [...]
+/*110*/{0.097667,1.92007,-1.7432,0.196158,1.86181,-1.87329,-0.059182,1.84411,-1.68327,-0.185824,1.78337,-1.65951,-0.065081,1.67856,-1.67768,-0.005272,1.6121,-1.67162,0.15854,1.93912,-2.12184,0.211631,1.86156,-1.94751,0.015327,1.88775,-2.06473,0.060723,1.70288,-2.23863,0.128618,1.62002,-2.26353,0.252985,1.65143,-2.11399,0.315918,1.65149,-2.09697,0.114624,1.4384,-1.83624,-0.066257,1.50417,-1.89967,-0.060614,1.52191,-1.96464,-0.056176,1.49727,-2.01448,0.121538,1.40603,-2.08885,0.251359,1.23 [...]
+/*111*/{0.097953,1.92189,-1.74287,0.197398,1.86155,-1.87205,-0.059782,1.84652,-1.68498,-0.185865,1.78527,-1.66129,-0.066352,1.67997,-1.67858,-0.005974,1.61332,-1.67081,0.158495,1.94068,-2.12153,0.213155,1.86363,-1.94673,0.01734,1.88931,-2.06474,0.065743,1.70552,-2.24076,0.131184,1.62223,-2.26439,0.253342,1.6541,-2.11338,0.316297,1.65479,-2.09573,0.115136,1.44083,-1.83615,-0.064132,1.50714,-1.89934,-0.059863,1.52489,-1.96487,-0.0545,1.49986,-2.01438,0.121883,1.40753,-2.08847,0.250994,1.22 [...]
+/*112*/{0.098533,1.92303,-1.74231,0.191808,1.866,-1.87308,-0.06034,1.84844,-1.68623,-0.190168,1.78636,-1.66445,-0.066587,1.68113,-1.67879,-0.00604,1.61457,-1.67083,0.159991,1.94201,-2.1209,0.213521,1.86636,-1.94645,0.018724,1.89097,-2.0645,0.070448,1.70787,-2.24269,0.134233,1.62339,-2.26513,0.254116,1.65695,-2.11359,0.317145,1.65632,-2.09431,0.115979,1.44294,-1.83668,-0.063289,1.50916,-1.89944,-0.057967,1.52711,-1.96442,-0.052951,1.503,-2.01473,0.122836,1.40915,-2.08804,0.248384,1.22674, [...]
+/*113*/{0.098747,1.92466,-1.74214,0.192504,1.86722,-1.87228,-0.059992,1.84924,-1.68733,-0.189857,1.78685,-1.66676,-0.067046,1.68223,-1.67966,-0.005763,1.61517,-1.66984,0.161652,1.94317,-2.12027,0.214169,1.86712,-1.94526,0.020035,1.89337,-2.06498,0.073657,1.70979,-2.24311,0.135992,1.6248,-2.26579,0.255401,1.65839,-2.11321,0.317348,1.65797,-2.09362,0.116713,1.44606,-1.8366,-0.062933,1.51144,-1.90054,-0.05747,1.52921,-1.96498,-0.052287,1.50531,-2.01543,0.124333,1.41165,-2.08791,0.24577,1.22 [...]
+/*114*/{0.099471,1.92529,-1.7419,0.19939,1.86449,-1.86973,-0.059591,1.8498,-1.688,-0.189805,1.78674,-1.66805,-0.066796,1.68281,-1.67977,-0.005539,1.61488,-1.6689,0.162882,1.94369,-2.11958,0.216201,1.86788,-1.94456,0.022113,1.89472,-2.06518,0.077291,1.71193,-2.24449,0.137609,1.62557,-2.2662,0.255576,1.66053,-2.11288,0.317565,1.65812,-2.09287,0.117007,1.44752,-1.83687,-0.062512,1.51418,-1.89978,-0.056971,1.53187,-1.96531,-0.051979,1.50726,-2.01533,0.124457,1.4138,-2.08787,0.243421,1.22051, [...]
+/*115*/{0.100052,1.9256,-1.7417,0.200715,1.86623,-1.86925,-0.059147,1.84996,-1.68872,-0.190783,1.78589,-1.67016,-0.066796,1.68281,-1.67977,-0.004821,1.61372,-1.66846,0.1649,1.94437,-2.11947,0.216357,1.86872,-1.94426,0.023086,1.89464,-2.06415,0.079094,1.71272,-2.24524,0.138902,1.62599,-2.26716,0.255984,1.65972,-2.11245,0.318086,1.65741,-2.09227,0.117156,1.44899,-1.83742,-0.062141,1.51526,-1.90082,-0.056012,1.5343,-1.96455,-0.05103,1.50965,-2.01525,0.125477,1.41535,-2.08754,0.242234,1.2152 [...]
+/*116*/{0.100714,1.92592,-1.74146,0.202349,1.86564,-1.86908,-0.059068,1.84936,-1.68904,-0.190855,1.78416,-1.67184,-0.065377,1.68093,-1.68036,-0.003974,1.61267,-1.66654,0.166357,1.94469,-2.11902,0.218094,1.86958,-1.94377,0.024394,1.89719,-2.06453,0.082355,1.7134,-2.24552,0.139531,1.62608,-2.26777,0.256545,1.65823,-2.11166,0.318982,1.65624,-2.09219,0.116887,1.45067,-1.83737,-0.061406,1.51607,-1.90014,-0.05588,1.53545,-1.96444,-0.05079,1.51103,-2.01471,0.126589,1.4175,-2.08727,0.238221,1.21 [...]
+/*117*/{0.101365,1.92615,-1.74083,0.202533,1.86567,-1.86901,-0.058936,1.84876,-1.68951,-0.189454,1.78316,-1.67303,-0.063968,1.67907,-1.6794,-0.002103,1.61047,-1.66493,0.167181,1.94457,-2.11856,0.218514,1.86954,-1.9429,0.025731,1.89842,-2.06435,0.083168,1.71445,-2.2462,0.139529,1.6257,-2.26806,0.257174,1.65643,-2.1119,0.318796,1.65331,-2.09187,0.119213,1.45168,-1.83756,-0.06177,1.51698,-1.89948,-0.055789,1.53577,-1.9642,-0.050796,1.51123,-2.01418,0.126457,1.41924,-2.08725,0.235398,1.20504 [...]
+/*118*/{0.102437,1.92572,-1.73999,0.203276,1.86691,-1.86912,-0.058538,1.84698,-1.69091,-0.187785,1.78014,-1.67378,-0.063182,1.67681,-1.67952,-0.001174,1.60781,-1.66398,0.169047,1.94351,-2.11833,0.218995,1.86957,-1.94255,0.02648,1.89832,-2.06389,0.083878,1.71369,-2.24529,0.13906,1.62485,-2.2678,0.25788,1.6544,-2.11172,0.31974,1.65025,-2.09207,0.120372,1.45197,-1.83764,-0.061638,1.51655,-1.89992,-0.055856,1.53638,-1.96368,-0.050149,1.51235,-2.01362,0.127737,1.42103,-2.08708,0.230708,1.1991 [...]
+/*119*/{0.102843,1.92555,-1.73987,0.203536,1.86635,-1.86843,-0.057236,1.84514,-1.69045,-0.186435,1.77706,-1.67421,-0.060996,1.67378,-1.67885,0.000628,1.60506,-1.66228,0.170398,1.94267,-2.11788,0.219321,1.86956,-1.94242,0.027241,1.89912,-2.06311,0.083394,1.71367,-2.24485,0.138262,1.62415,-2.26778,0.25752,1.65155,-2.11209,0.320177,1.64604,-2.0918,0.120955,1.45212,-1.8379,-0.061712,1.51645,-1.89865,-0.055865,1.53648,-1.96358,-0.050427,1.51228,-2.01265,0.128055,1.42207,-2.0871,0.227618,1.193 [...]
+/*120*/{0.104568,1.92453,-1.73899,0.20393,1.86634,-1.86807,-0.056627,1.84241,-1.69048,-0.184506,1.77356,-1.67434,-0.059209,1.67085,-1.6785,0.002456,1.60166,-1.66018,0.170886,1.94164,-2.11718,0.21994,1.86892,-1.94168,0.028257,1.89965,-2.06304,0.082098,1.71283,-2.24388,0.136387,1.62374,-2.26721,0.257233,1.64855,-2.11258,0.320531,1.64144,-2.09249,0.121922,1.45215,-1.83794,-0.061098,1.51526,-1.89813,-0.05596,1.53648,-1.96347,-0.050536,1.51245,-2.01217,0.128527,1.42302,-2.08733,0.222716,1.186 [...]
+/*121*/{0.105838,1.92305,-1.73815,0.204828,1.86634,-1.86822,-0.0546,1.83933,-1.69038,-0.181978,1.76918,-1.67467,-0.057082,1.66705,-1.67638,0.004379,1.59754,-1.65833,0.172424,1.9402,-2.11713,0.220769,1.8684,-1.94171,0.028777,1.89963,-2.06263,0.080026,1.71214,-2.24296,0.134445,1.62248,-2.26668,0.258167,1.6437,-2.11294,0.320448,1.63567,-2.09284,0.122519,1.45218,-1.83832,-0.060787,1.51394,-1.89823,-0.056234,1.53565,-1.96283,-0.050723,1.51181,-2.01213,0.128735,1.42333,-2.0874,0.219669,1.18045 [...]
+/*122*/{0.10691,1.92146,-1.73783,0.205115,1.8652,-1.8676,-0.053388,1.836,-1.68928,-0.179733,1.76485,-1.67393,-0.053985,1.6623,-1.67499,0.007336,1.59352,-1.65635,0.172931,1.93853,-2.11659,0.220002,1.86718,-1.94142,0.029229,1.89934,-2.06237,0.076569,1.7117,-2.24155,0.132405,1.62149,-2.26606,0.258429,1.63854,-2.11336,0.32054,1.62917,-2.09385,0.121948,1.45119,-1.8386,-0.06111,1.51254,-1.89789,-0.056346,1.53522,-1.96285,-0.050938,1.51012,-2.01176,0.129699,1.4234,-2.08748,0.214642,1.17491,-1.7 [...]
+/*123*/{0.108045,1.91935,-1.73727,0.206363,1.86285,-1.8675,-0.051689,1.83154,-1.68784,-0.176052,1.76009,-1.67266,-0.051717,1.65772,-1.67326,0.009928,1.58879,-1.65454,0.172494,1.93646,-2.11595,0.220589,1.86576,-1.94135,0.029106,1.89867,-2.06128,0.074043,1.71081,-2.24056,0.129314,1.6204,-2.26511,0.258297,1.63388,-2.11426,0.319976,1.62285,-2.09543,0.12213,1.44954,-1.83854,-0.060318,1.50986,-1.89772,-0.056029,1.53312,-1.96204,-0.050648,1.50844,-2.01152,0.129941,1.42331,-2.08755,0.211589,1.16 [...]
+/*124*/{0.109376,1.91674,-1.7362,0.206665,1.86156,-1.86749,-0.050355,1.82775,-1.68752,-0.173943,1.75423,-1.6716,-0.048093,1.65229,-1.67164,0.013555,1.58435,-1.65241,0.173206,1.93427,-2.11575,0.220749,1.86427,-1.94103,0.029204,1.89756,-2.06016,0.070225,1.70979,-2.23944,0.126065,1.61863,-2.26418,0.258855,1.62899,-2.11582,0.319382,1.61573,-2.09668,0.122052,1.44805,-1.8388,-0.061021,1.50784,-1.89701,-0.055693,1.53129,-1.96215,-0.050213,1.50668,-2.01102,0.130425,1.42303,-2.08774,0.20912,1.162 [...]
+/*125*/{0.110674,1.91394,-1.73556,0.20625,1.86074,-1.86806,-0.048206,1.82318,-1.68548,-0.16963,1.74844,-1.66937,-0.044977,1.64668,-1.66896,0.017506,1.57862,-1.64969,0.173224,1.93221,-2.11554,0.2207,1.86208,-1.94093,0.028991,1.89715,-2.05955,0.066978,1.70775,-2.23795,0.122635,1.6171,-2.26309,0.256846,1.62179,-2.11697,0.318497,1.60713,-2.09859,0.122366,1.44541,-1.83904,-0.060332,1.50503,-1.89654,-0.055298,1.52848,-1.96183,-0.050702,1.50413,-2.01087,0.131148,1.42204,-2.08792,0.206765,1.1580 [...]
+/*126*/{0.11146,1.91042,-1.73437,0.20688,1.85825,-1.86842,-0.046346,1.81798,-1.68416,-0.167979,1.74148,-1.66717,-0.040945,1.64093,-1.66658,0.021394,1.57328,-1.64641,0.172928,1.92886,-2.1157,0.22116,1.86002,-1.94076,0.029442,1.89487,-2.05875,0.062707,1.70634,-2.23654,0.118918,1.61521,-2.26183,0.25569,1.61477,-2.11778,0.316833,1.59851,-2.10064,0.122071,1.44273,-1.83891,-0.06081,1.50233,-1.89693,-0.055884,1.52566,-1.96171,-0.050666,1.50157,-2.01118,0.131652,1.42107,-2.08788,0.203323,1.1543, [...]
+/*127*/{0.112959,1.90673,-1.73364,0.206099,1.85521,-1.8684,-0.044691,1.81231,-1.68237,-0.164364,1.73531,-1.66452,-0.037693,1.6347,-1.6632,0.025933,1.56798,-1.64507,0.17322,1.92637,-2.11583,0.220896,1.85714,-1.9406,0.029453,1.89328,-2.05824,0.058962,1.70469,-2.23518,0.114626,1.61294,-2.26153,0.255049,1.60802,-2.1193,0.315687,1.58965,-2.10337,0.119929,1.43855,-1.83911,-0.060711,1.49916,-1.89655,-0.055561,1.52201,-1.96171,-0.051539,1.49864,-2.01108,0.132043,1.41877,-2.08794,0.200604,1.15123 [...]
+/*128*/{0.113679,1.90276,-1.73323,0.207103,1.8509,-1.86801,-0.042685,1.80607,-1.68069,-0.160975,1.72785,-1.66183,-0.033828,1.62832,-1.66031,0.030726,1.56246,-1.64271,0.171904,1.92285,-2.11599,0.220945,1.85411,-1.94088,0.02894,1.89101,-2.05724,0.054683,1.70277,-2.23327,0.110718,1.61012,-2.26043,0.252891,1.59985,-2.12118,0.313493,1.57995,-2.10608,0.119945,1.43584,-1.83839,-0.061055,1.49621,-1.89598,-0.055474,1.51908,-1.96139,-0.051393,1.49512,-2.01127,0.132402,1.41703,-2.08779,0.200583,1.1 [...]
+/*129*/{0.11473,1.89869,-1.73229,0.206524,1.84823,-1.8685,-0.040688,1.79973,-1.67869,-0.158662,1.71938,-1.6577,-0.029384,1.62132,-1.65667,0.035282,1.55655,-1.64003,0.171568,1.91901,-2.11656,0.220917,1.85156,-1.94116,0.027918,1.88706,-2.05549,0.050005,1.70024,-2.23205,0.105994,1.60718,-2.25978,0.250368,1.59144,-2.12299,0.311054,1.56969,-2.10879,0.120057,1.43393,-1.83702,-0.061152,1.49251,-1.89645,-0.055935,1.51537,-1.96151,-0.051275,1.49169,-2.01139,0.132901,1.41501,-2.08748,0.197605,1.14 [...]
+/*130*/{0.115624,1.89356,-1.73147,0.207099,1.84488,-1.86902,-0.038308,1.79313,-1.67625,-0.154836,1.71189,-1.65359,-0.024555,1.61524,-1.65416,0.041659,1.55077,-1.63761,0.170477,1.91545,-2.11678,0.221157,1.84813,-1.94051,0.028231,1.88371,-2.05489,0.04589,1.6974,-2.23095,0.101881,1.60389,-2.25931,0.247766,1.58374,-2.12499,0.30789,1.55884,-2.11194,0.119919,1.42963,-1.83754,-0.06153,1.48887,-1.89632,-0.057039,1.51276,-1.96185,-0.051513,1.48724,-2.01154,0.132743,1.41091,-2.08825,0.195584,1.143 [...]
+/*131*/{0.11658,1.88895,-1.73059,0.208766,1.83941,-1.86883,-0.035432,1.78617,-1.67352,-0.150756,1.7033,-1.6496,-0.019369,1.60748,-1.65015,0.047186,1.54494,-1.63473,0.169376,1.91138,-2.11699,0.22083,1.84452,-1.94096,0.027148,1.8803,-2.05452,0.042537,1.69262,-2.229,0.096744,1.6002,-2.25857,0.244295,1.57378,-2.12678,0.304825,1.54804,-2.11508,0.119778,1.42638,-1.83708,-0.061687,1.48475,-1.89691,-0.056455,1.50849,-1.96113,-0.05195,1.48328,-2.0115,0.133688,1.4086,-2.08694,0.195107,1.13867,-1.7 [...]
+/*132*/{0.117993,1.88334,-1.72992,0.208701,1.83479,-1.86866,-0.0332,1.77816,-1.67093,-0.147652,1.69382,-1.64504,-0.014721,1.60013,-1.64703,0.053232,1.53911,-1.63216,0.167209,1.90725,-2.11778,0.220916,1.84041,-1.94109,0.026536,1.87562,-2.05348,0.037379,1.68926,-2.22837,0.091903,1.59661,-2.25836,0.241507,1.56462,-2.12946,0.300936,1.53669,-2.11876,0.119476,1.42322,-1.8362,-0.061535,1.48077,-1.8968,-0.056512,1.50445,-1.96116,-0.051155,1.47786,-2.01221,0.133902,1.406,-2.08643,0.193003,1.13576 [...]
+/*133*/{0.119029,1.87781,-1.72874,0.207882,1.83188,-1.86895,-0.030042,1.77011,-1.66778,-0.141801,1.68521,-1.64024,-0.008738,1.59299,-1.6438,0.059377,1.5328,-1.63004,0.16684,1.90287,-2.11842,0.221508,1.83619,-1.94109,0.025823,1.87195,-2.05196,0.032964,1.68632,-2.22747,0.086314,1.59251,-2.25829,0.237464,1.55441,-2.13213,0.297022,1.52424,-2.12253,0.120293,1.42054,-1.83574,-0.062919,1.47654,-1.89701,-0.057239,1.50083,-1.96216,-0.052455,1.47368,-2.01234,0.133195,1.40241,-2.0875,0.191456,1.132 [...]
+/*134*/{0.11992,1.87226,-1.72832,0.209089,1.82578,-1.86874,-0.0277,1.76187,-1.6648,-0.139556,1.67586,-1.63574,-0.003165,1.58512,-1.63944,0.066439,1.52661,-1.62707,0.165855,1.89821,-2.11894,0.221466,1.83157,-1.94188,0.024713,1.86719,-2.05154,0.029487,1.68213,-2.2266,0.08146,1.58822,-2.25807,0.233722,1.54474,-2.13465,0.291531,1.5117,-2.12677,0.119927,1.41592,-1.8337,-0.063097,1.47108,-1.89852,-0.057222,1.49642,-1.96213,-0.052676,1.46861,-2.01268,0.133502,1.39954,-2.08663,0.191526,1.12797,- [...]
+/*135*/{0.12065,1.86667,-1.72762,0.208836,1.82166,-1.86904,-0.023669,1.75285,-1.66146,-0.134028,1.66646,-1.6305,0.00332,1.57786,-1.63586,0.073655,1.52076,-1.62458,0.164119,1.89355,-2.11934,0.221175,1.82644,-1.94213,0.024195,1.86255,-2.05066,0.02527,1.67767,-2.2264,0.076397,1.58419,-2.25832,0.229109,1.53368,-2.13785,0.286302,1.49895,-2.13085,0.120209,1.41227,-1.8325,-0.063179,1.46624,-1.8978,-0.058018,1.49177,-1.96182,-0.052759,1.46326,-2.01304,0.134142,1.39576,-2.08667,0.189598,1.12336,- [...]
+/*136*/{0.121615,1.86103,-1.72689,0.209032,1.81693,-1.86949,-0.019969,1.74437,-1.65812,-0.129343,1.65573,-1.62492,0.010189,1.5704,-1.6323,0.08155,1.5153,-1.62195,0.164134,1.88882,-2.12007,0.221194,1.82209,-1.94279,0.023714,1.85719,-2.04975,0.021042,1.67406,-2.22547,0.070481,1.57948,-2.25849,0.224831,1.52372,-2.14138,0.28098,1.48644,-2.13496,0.119808,1.40896,-1.83147,-0.064285,1.46207,-1.89898,-0.058049,1.48678,-1.96155,-0.053756,1.4584,-2.01408,0.134298,1.39216,-2.08561,0.187912,1.11861, [...]
+/*137*/{0.123299,1.8556,-1.72579,0.210612,1.80994,-1.86904,-0.015582,1.73537,-1.65456,-0.121942,1.6465,-1.61956,0.017164,1.56319,-1.62912,0.090286,1.50938,-1.62007,0.163051,1.88456,-2.12044,0.221279,1.81699,-1.94343,0.022232,1.8524,-2.04786,0.016712,1.66969,-2.22512,0.065103,1.57576,-2.2587,0.219411,1.51228,-2.14472,0.274594,1.47382,-2.13916,0.118659,1.40474,-1.8298,-0.065667,1.45705,-1.89953,-0.059072,1.48176,-1.96139,-0.054754,1.45237,-2.0145,0.133619,1.38685,-2.08527,0.184902,1.11399, [...]
+/*138*/{0.124449,1.8498,-1.72497,0.211694,1.80475,-1.86902,-0.011917,1.72579,-1.65181,-0.116782,1.63616,-1.61407,0.024872,1.55591,-1.62555,0.099217,1.50451,-1.61686,0.162122,1.88002,-2.12111,0.222076,1.81152,-1.94376,0.021843,1.84821,-2.04671,0.01327,1.66547,-2.22483,0.059188,1.57136,-2.25892,0.213403,1.50155,-2.14836,0.268125,1.46083,-2.14309,0.117674,1.40044,-1.82803,-0.067931,1.45268,-1.90035,-0.061291,1.47704,-1.96222,-0.055455,1.44774,-2.01496,0.134402,1.383,-2.08356,0.181926,1.1093 [...]
+/*139*/{0.125715,1.84515,-1.72443,0.211395,1.80011,-1.87011,-0.006106,1.71676,-1.64802,-0.10903,1.62678,-1.60848,0.03325,1.54906,-1.62213,0.108801,1.49903,-1.61471,0.16096,1.87604,-2.12167,0.22243,1.80606,-1.94509,0.020941,1.84385,-2.04508,0.008191,1.66154,-2.22428,0.053981,1.56745,-2.25911,0.207751,1.48977,-2.15196,0.260885,1.44859,-2.14779,0.115965,1.39679,-1.82756,-0.070312,1.44845,-1.90056,-0.063042,1.47099,-1.9627,-0.057861,1.44207,-2.01594,0.133664,1.37909,-2.08381,0.177229,1.10608 [...]
+/*140*/{0.12706,1.84055,-1.72327,0.212156,1.79334,-1.86941,-0.000368,1.70822,-1.64507,-0.101048,1.61597,-1.6022,0.042028,1.54264,-1.61976,0.117815,1.49527,-1.61342,0.160417,1.87218,-2.12216,0.223819,1.80074,-1.94563,0.02038,1.84118,-2.04303,0.005186,1.65859,-2.22413,0.047865,1.56359,-2.25912,0.201758,1.47969,-2.15616,0.251363,1.43785,-2.15298,0.113481,1.39354,-1.82723,-0.073907,1.44508,-1.90244,-0.065189,1.46605,-1.96419,-0.059653,1.43708,-2.01746,0.132794,1.37525,-2.08259,0.173257,1.103 [...]
+/*141*/{0.1287,1.83642,-1.72265,0.213585,1.78888,-1.86985,0.005052,1.69974,-1.64217,-0.091098,1.60722,-1.59692,0.052814,1.53748,-1.6161,0.128333,1.4915,-1.61131,0.159143,1.8689,-2.12372,0.224913,1.79626,-1.94678,0.019466,1.83877,-2.04049,0.001791,1.65565,-2.22445,0.04193,1.56056,-2.25872,0.194232,1.47043,-2.16038,0.242748,1.42676,-2.15748,0.109927,1.39069,-1.82508,-0.077468,1.44212,-1.90292,-0.068734,1.462,-1.96571,-0.062395,1.43257,-2.01871,0.13238,1.37101,-2.08002,0.171766,1.10437,-1.7 [...]
+/*142*/{0.129625,1.83396,-1.72191,0.213229,1.7856,-1.8706,0.011633,1.69282,-1.63993,-0.082956,1.59807,-1.59172,0.061669,1.53283,-1.61447,0.139024,1.48804,-1.60923,0.159051,1.86628,-2.12491,0.225254,1.79357,-1.94845,0.019489,1.83759,-2.03864,-0.000894,1.65349,-2.22427,0.036029,1.55808,-2.25811,0.186303,1.46168,-2.163,0.233715,1.41712,-2.16179,0.107147,1.38804,-1.82377,-0.080646,1.43865,-1.90674,-0.071186,1.45865,-1.96696,-0.065514,1.42755,-2.01992,0.130939,1.36685,-2.07847,0.167521,1.1043 [...]
+/*143*/{0.13055,1.83116,-1.72191,0.213701,1.78244,-1.87236,0.018144,1.68773,-1.6385,-0.073375,1.59062,-1.58704,0.072751,1.52895,-1.61186,0.151015,1.48641,-1.60827,0.159787,1.86427,-2.1255,0.22543,1.78979,-1.95033,0.02018,1.83684,-2.03651,-0.00347,1.65319,-2.22288,0.030713,1.55675,-2.25729,0.178542,1.45555,-2.1652,0.224178,1.40779,-2.1662,0.104143,1.38653,-1.82137,-0.082742,1.43653,-1.90912,-0.073376,1.45709,-1.96746,-0.067119,1.42387,-2.0224,0.129311,1.36414,-2.07704,0.165077,1.1056,-1.7 [...]
+/*144*/{0.131738,1.82868,-1.72208,0.214139,1.78066,-1.87364,0.024571,1.68278,-1.63713,-0.061382,1.58433,-1.58295,0.084814,1.5269,-1.61198,0.162493,1.48493,-1.60764,0.160499,1.86224,-2.12554,0.225433,1.78713,-1.95133,0.020778,1.83669,-2.03651,-0.007374,1.65386,-2.22098,0.024642,1.55731,-2.25637,0.169772,1.45023,-2.16765,0.21408,1.40111,-2.17094,0.101176,1.38704,-1.81823,-0.085112,1.43544,-1.91052,-0.074634,1.45578,-1.96817,-0.070856,1.41828,-2.02458,0.128558,1.36126,-2.07632,0.163127,1.10 [...]
+/*145*/{0.133216,1.82681,-1.72254,0.214008,1.77928,-1.87432,0.029442,1.68018,-1.63619,-0.05151,1.57954,-1.57908,0.095621,1.52404,-1.6099,0.17495,1.4855,-1.60636,0.160887,1.86057,-2.12544,0.225396,1.78512,-1.9517,0.021446,1.83689,-2.03703,-0.011545,1.65592,-2.21872,0.017694,1.55902,-2.2558,0.160573,1.4456,-2.17019,0.204533,1.39495,-2.17525,0.098202,1.38621,-1.81535,-0.08768,1.43499,-1.91311,-0.07645,1.45603,-1.96926,-0.073255,1.41729,-2.02613,0.127114,1.35998,-2.0752,0.159355,1.10436,-1.7 [...]
+/*146*/{0.134979,1.82568,-1.72348,0.212845,1.77772,-1.87481,0.036344,1.67846,-1.6351,-0.042001,1.57539,-1.5761,0.107484,1.52339,-1.60977,0.188096,1.48687,-1.60692,0.162383,1.85869,-2.12532,0.224488,1.78381,-1.95223,0.022253,1.83665,-2.03845,-0.016031,1.65886,-2.21606,0.011203,1.56142,-2.25517,0.151789,1.4437,-2.17291,0.194046,1.39104,-2.17916,0.095033,1.38576,-1.81317,-0.088347,1.43368,-1.91381,-0.077183,1.45551,-1.96943,-0.073962,1.41646,-2.02706,0.124859,1.35855,-2.07398,0.152696,1.101 [...]
+/*147*/{0.137003,1.82525,-1.72406,0.213646,1.77337,-1.87587,0.040796,1.67687,-1.63412,-0.031332,1.57265,-1.57342,0.119281,1.52372,-1.6089,0.199999,1.48934,-1.60745,0.162723,1.85758,-2.12553,0.224618,1.78201,-1.95191,0.022074,1.83678,-2.03918,-0.020519,1.66295,-2.2132,0.003699,1.56506,-2.25484,0.143549,1.44215,-2.17588,0.184014,1.38795,-2.18276,0.092756,1.38663,-1.81063,-0.089794,1.43348,-1.91418,-0.078774,1.4561,-1.96976,-0.075788,1.41761,-2.02825,0.123364,1.35847,-2.0733,0.150447,1.0999 [...]
+/*148*/{0.138325,1.82561,-1.7246,0.213637,1.77325,-1.87613,0.04667,1.67634,-1.63363,-0.020729,1.57021,-1.57067,0.129806,1.52511,-1.60868,0.212907,1.49274,-1.60884,0.163579,1.857,-2.12541,0.224322,1.78198,-1.9523,0.022217,1.83673,-2.03969,-0.025937,1.6675,-2.21045,-0.002714,1.56942,-2.25481,0.134474,1.44162,-2.1788,0.173473,1.38699,-2.1858,0.090624,1.38677,-1.809,-0.091548,1.43341,-1.9143,-0.079971,1.4559,-1.96926,-0.075438,1.42188,-2.0276,0.121951,1.35922,-2.07265,0.142093,1.09456,-1.747 [...]
+/*149*/{0.139592,1.82575,-1.72522,0.213663,1.77289,-1.87641,0.051034,1.67672,-1.6321,-0.012056,1.56889,-1.56835,0.141183,1.52802,-1.60814,0.225771,1.49836,-1.6104,0.16394,1.85686,-2.12548,0.223421,1.78051,-1.9522,0.021873,1.83753,-2.04091,-0.029869,1.67386,-2.20826,-0.010899,1.57432,-2.25496,0.126071,1.44226,-2.18167,0.163116,1.3874,-2.18935,0.088345,1.38853,-1.8071,-0.092918,1.43312,-1.91445,-0.081012,1.45673,-1.96813,-0.075338,1.42147,-2.02654,0.120691,1.3607,-2.07251,0.13666,1.09123,- [...]
+/*150*/{0.14035,1.82719,-1.72569,0.212421,1.77425,-1.87705,0.054903,1.67758,-1.63143,-0.00093,1.5678,-1.56571,0.151639,1.5309,-1.60886,0.237743,1.50402,-1.61244,0.163927,1.85712,-2.12573,0.222524,1.78043,-1.95279,0.0223,1.83914,-2.04123,-0.035548,1.67978,-2.20592,-0.018085,1.58012,-2.25522,0.117743,1.44485,-2.18416,0.153357,1.38984,-2.19183,0.087005,1.38931,-1.80607,-0.093647,1.43343,-1.91445,-0.081153,1.45699,-1.96784,-0.076634,1.42234,-2.02535,0.119137,1.36272,-2.07194,0.129161,1.08677 [...]
+/*151*/{0.141093,1.82929,-1.72691,0.211285,1.77468,-1.87791,0.059611,1.6793,-1.63039,0.008194,1.5668,-1.56336,0.162202,1.53512,-1.60923,0.24906,1.51078,-1.61446,0.16372,1.85765,-2.12611,0.221387,1.7808,-1.95364,0.021224,1.84054,-2.04159,-0.039608,1.68683,-2.20406,-0.025606,1.58565,-2.25572,0.109595,1.44996,-2.18651,0.142803,1.39291,-2.19383,0.085026,1.38934,-1.80464,-0.094714,1.43355,-1.91349,-0.081566,1.45722,-1.96696,-0.078624,1.42381,-2.02387,0.119445,1.36657,-2.0713,0.123603,1.08454, [...]
+/*152*/{0.142377,1.83209,-1.72697,0.210519,1.77615,-1.87851,0.064068,1.68094,-1.63023,0.018201,1.56732,-1.56159,0.172286,1.54007,-1.61019,0.259896,1.5188,-1.61704,0.162238,1.85874,-2.1263,0.221298,1.78246,-1.95422,0.020974,1.84315,-2.04116,-0.044998,1.69321,-2.20264,-0.032997,1.59268,-2.25613,0.101362,1.45388,-2.18802,0.133802,1.39695,-2.19562,0.084637,1.3908,-1.80423,-0.094841,1.4349,-1.91361,-0.083149,1.45834,-1.9663,-0.079794,1.4248,-2.02323,0.11929,1.36937,-2.07115,0.116632,1.08272,- [...]
+/*153*/{0.144299,1.83487,-1.72801,0.211571,1.77783,-1.87959,0.069105,1.68322,-1.6299,0.026871,1.56856,-1.56007,0.182129,1.54604,-1.61158,0.27011,1.52688,-1.62105,0.160269,1.86109,-2.12727,0.220339,1.78359,-1.95571,0.020419,1.84609,-2.04072,-0.049648,1.7002,-2.20144,-0.040517,1.59945,-2.25653,0.093006,1.45985,-2.18901,0.124738,1.40241,-2.19681,0.083239,1.39031,-1.80255,-0.094952,1.43524,-1.91318,-0.083039,1.45989,-1.96595,-0.081139,1.42612,-2.02319,0.118186,1.37214,-2.07098,0.108784,1.082 [...]
+/*154*/{0.146131,1.83809,-1.72852,0.21182,1.78083,-1.88051,0.074492,1.68596,-1.62946,0.037518,1.57087,-1.5591,0.190902,1.55187,-1.61324,0.279501,1.53583,-1.62491,0.158748,1.86328,-2.12747,0.219854,1.78572,-1.95645,0.019567,1.84748,-2.03993,-0.054906,1.70712,-2.20066,-0.046942,1.6069,-2.25664,0.08522,1.46638,-2.19016,0.116002,1.40865,-2.19785,0.082991,1.39083,-1.80203,-0.095771,1.43816,-1.91309,-0.083108,1.46149,-1.96522,-0.081691,1.42803,-2.02273,0.117834,1.37481,-2.07079,0.103768,1.0817 [...]
+/*155*/{0.148654,1.84175,-1.72968,0.212552,1.78335,-1.88182,0.080158,1.68914,-1.62898,0.045215,1.57229,-1.55758,0.199265,1.55817,-1.61466,0.287799,1.54491,-1.62862,0.156587,1.86602,-2.12853,0.219845,1.78893,-1.95803,0.0178,1.8506,-2.03975,-0.060751,1.71402,-2.20046,-0.05428,1.61385,-2.25747,0.077291,1.47321,-2.19151,0.107887,1.41516,-2.19843,0.082949,1.39111,-1.80189,-0.095677,1.43989,-1.91166,-0.083597,1.4633,-1.96479,-0.081173,1.42947,-2.02202,0.116993,1.37671,-2.07074,0.09733,1.07837, [...]
+/*156*/{0.151406,1.84564,-1.73021,0.213086,1.78642,-1.88322,0.086463,1.69245,-1.6284,0.054007,1.5761,-1.5574,0.207284,1.56572,-1.61698,0.295743,1.55471,-1.63248,0.153208,1.86962,-2.12884,0.218968,1.79167,-1.95903,0.016665,1.85316,-2.0386,-0.067005,1.72104,-2.20009,-0.060825,1.62103,-2.25764,0.06921,1.48037,-2.19224,0.100215,1.42254,-2.19915,0.08506,1.3916,-1.80265,-0.095068,1.44232,-1.91091,-0.083128,1.46597,-1.9627,-0.080848,1.43192,-2.02181,0.117153,1.37996,-2.07064,0.089732,1.07806,-1 [...]
+/*157*/{0.153802,1.85013,-1.73133,0.212155,1.79035,-1.88428,0.092132,1.69607,-1.6284,0.063112,1.57992,-1.55706,0.215001,1.57338,-1.62027,0.302877,1.5646,-1.6372,0.150768,1.87343,-2.1291,0.21888,1.79527,-1.96021,0.01471,1.85584,-2.03718,-0.072727,1.72749,-2.19953,-0.06797,1.62855,-2.25743,0.06192,1.48783,-2.1924,0.092652,1.43039,-2.20032,0.085755,1.39243,-1.80205,-0.094342,1.44501,-1.9101,-0.082371,1.46821,-1.96161,-0.080609,1.43429,-2.02112,0.117056,1.38328,-2.07073,0.081842,1.07797,-1.7 [...]
+/*158*/{0.156567,1.85476,-1.73242,0.21284,1.79403,-1.88533,0.098469,1.70032,-1.62865,0.071864,1.58344,-1.55561,0.221122,1.58116,-1.62272,0.30968,1.57451,-1.64147,0.1477,1.87747,-2.13015,0.217629,1.79876,-1.96219,0.013504,1.85829,-2.0357,-0.077507,1.73463,-2.19931,-0.074399,1.63553,-2.2577,0.054627,1.4958,-2.19314,0.085484,1.43839,-2.20096,0.086428,1.39282,-1.80186,-0.092003,1.44803,-1.90799,-0.082778,1.47045,-1.96089,-0.080603,1.43711,-2.02005,0.117396,1.38666,-2.0706,0.075438,1.0777,-1. [...]
+/*159*/{0.158703,1.85979,-1.7339,0.213959,1.79849,-1.88703,0.105632,1.70484,-1.62897,0.079176,1.58744,-1.55501,0.227645,1.58925,-1.62602,0.315666,1.58524,-1.64634,0.14444,1.88211,-2.13026,0.217171,1.8024,-1.96403,0.011898,1.86141,-2.03391,-0.081718,1.74127,-2.19978,-0.080497,1.64285,-2.25724,0.048729,1.50459,-2.19306,0.079597,1.44723,-2.20119,0.087504,1.39369,-1.80183,-0.092258,1.45074,-1.9076,-0.081807,1.47347,-1.95947,-0.079975,1.44065,-2.01943,0.117637,1.39037,-2.06997,0.068079,1.0775 [...]
+/*160*/{0.161579,1.8655,-1.73552,0.214657,1.80262,-1.88864,0.112446,1.70925,-1.62926,0.087124,1.5933,-1.55544,0.232744,1.59775,-1.62926,0.320742,1.59487,-1.65198,0.140614,1.88687,-2.13056,0.217292,1.807,-1.96565,0.010397,1.86458,-2.03211,-0.086824,1.74863,-2.19984,-0.086813,1.65037,-2.25681,0.042356,1.51324,-2.19362,0.073933,1.45552,-2.20208,0.088722,1.39454,-1.80094,-0.090165,1.45329,-1.90559,-0.080821,1.4769,-1.95717,-0.078614,1.4441,-2.01783,0.117945,1.39392,-2.06932,0.064236,1.07679, [...]
+/*161*/{0.163528,1.8713,-1.73713,0.215537,1.80731,-1.89058,0.118569,1.71441,-1.62969,0.093825,1.59833,-1.55577,0.238673,1.60612,-1.63189,0.325866,1.60512,-1.65724,0.13723,1.89186,-2.13062,0.21701,1.81141,-1.96769,0.008979,1.86762,-2.03068,-0.091092,1.75572,-2.19976,-0.091753,1.6577,-2.25609,0.036819,1.52207,-2.19318,0.068576,1.46439,-2.20289,0.091091,1.3959,-1.80072,-0.088861,1.45636,-1.90452,-0.079694,1.48021,-1.95583,-0.07809,1.44832,-2.01669,0.118387,1.39808,-2.06882,0.056744,1.0779,- [...]
+/*162*/{0.166033,1.8769,-1.73833,0.216372,1.8122,-1.89249,0.124502,1.71967,-1.63039,0.101879,1.60324,-1.55452,0.243471,1.61403,-1.6346,0.329359,1.61485,-1.66249,0.133962,1.89657,-2.13053,0.21672,1.81627,-1.96938,0.007167,1.87087,-2.02878,-0.095092,1.76211,-2.19973,-0.098776,1.66514,-2.25454,0.031446,1.53103,-2.19306,0.063611,1.47342,-2.20331,0.092208,1.39662,-1.80008,-0.0872,1.45978,-1.90225,-0.078553,1.48356,-1.95378,-0.077631,1.45237,-2.01606,0.119335,1.40269,-2.06813,0.050137,1.08052, [...]
+/*163*/{0.167831,1.88277,-1.74009,0.216721,1.81692,-1.89397,0.130099,1.72544,-1.6314,0.107499,1.60918,-1.5547,0.246568,1.6219,-1.63707,0.332207,1.62515,-1.66537,0.131242,1.90177,-2.13048,0.216979,1.82118,-1.97153,0.006465,1.87383,-2.02695,-0.099139,1.76865,-2.20038,-0.103531,1.67241,-2.25347,0.0256,1.53942,-2.19244,0.058468,1.48193,-2.20361,0.09469,1.39815,-1.80005,-0.086014,1.46322,-1.90102,-0.07703,1.48699,-1.95134,-0.075753,1.45739,-2.0148,0.119642,1.40707,-2.06726,0.044696,1.08203,-1 [...]
+/*164*/{0.170062,1.88982,-1.74224,0.217355,1.82233,-1.89641,0.135821,1.73127,-1.63214,0.1155,1.61525,-1.55452,0.250572,1.63042,-1.63981,0.335175,1.63503,-1.67012,0.12746,1.90645,-2.13125,0.216412,1.8254,-1.97316,0.005455,1.87747,-2.02521,-0.102539,1.77559,-2.20023,-0.109239,1.67962,-2.2527,0.020206,1.54674,-2.19164,0.05435,1.48995,-2.2039,0.09552,1.39863,-1.79961,-0.08424,1.46636,-1.90005,-0.075703,1.49093,-1.94926,-0.074815,1.46245,-2.01461,0.120776,1.41189,-2.0668,0.037973,1.08259,-1.7 [...]
+/*165*/{0.171457,1.89442,-1.74441,0.218503,1.8265,-1.89815,0.140613,1.73722,-1.63301,0.120441,1.62163,-1.55424,0.253678,1.63887,-1.64286,0.337989,1.6439,-1.67402,0.12528,1.91128,-2.13114,0.216698,1.83087,-1.97489,0.003962,1.88137,-2.02414,-0.106201,1.78099,-2.19975,-0.114097,1.68724,-2.25116,0.016862,1.55427,-2.19037,0.050054,1.49792,-2.20443,0.098949,1.40059,-1.80063,-0.081374,1.46998,-1.89882,-0.074664,1.49522,-1.94758,-0.073238,1.46787,-2.0136,0.121121,1.41658,-2.06659,0.03359,1.08544 [...]
+/*166*/{0.173414,1.90016,-1.74578,0.218423,1.83196,-1.90005,0.14444,1.74356,-1.63363,0.124315,1.62756,-1.55449,0.255593,1.64682,-1.64576,0.339916,1.65359,-1.67967,0.122796,1.91577,-2.13084,0.216056,1.83551,-1.97615,0.002719,1.88477,-2.02276,-0.107492,1.78675,-2.19908,-0.118821,1.69334,-2.24924,0.011181,1.56232,-2.18973,0.045992,1.50629,-2.20424,0.101937,1.40189,-1.80159,-0.079429,1.47401,-1.89746,-0.072389,1.49868,-1.94527,-0.071859,1.47253,-2.01235,0.122051,1.42169,-2.0658,0.028864,1.08 [...]
+/*167*/{0.175182,1.90618,-1.74761,0.220075,1.83671,-1.9023,0.148666,1.74961,-1.63438,0.128578,1.63353,-1.55395,0.257814,1.65403,-1.64808,0.341017,1.66212,-1.68357,0.120821,1.92018,-2.13134,0.215149,1.84043,-1.97789,0.002055,1.88929,-2.02199,-0.110974,1.79172,-2.19849,-0.122959,1.70048,-2.24812,0.008281,1.57008,-2.18895,0.042722,1.51366,-2.20455,0.10358,1.40307,-1.80216,-0.077246,1.47741,-1.89627,-0.070701,1.50336,-1.94304,-0.070054,1.47767,-2.01192,0.123436,1.42719,-2.06555,0.024239,1.09 [...]
+/*168*/{0.177384,1.91146,-1.7491,0.220477,1.84118,-1.90433,0.152322,1.75512,-1.63497,0.133547,1.63969,-1.55383,0.259752,1.66161,-1.65093,0.341558,1.67005,-1.68855,0.118338,1.92444,-2.13114,0.214992,1.84527,-1.97938,0.001305,1.89256,-2.0215,-0.114609,1.7971,-2.19677,-0.127738,1.70682,-2.24681,0.004875,1.57724,-2.18852,0.039068,1.52113,-2.20482,0.106495,1.40423,-1.80356,-0.074895,1.48118,-1.8951,-0.069661,1.50782,-1.9413,-0.068599,1.4826,-2.01057,0.124447,1.43223,-2.06559,0.019626,1.09314, [...]
+/*169*/{0.178999,1.91665,-1.75072,0.220847,1.84639,-1.90645,0.155416,1.76118,-1.63544,0.137393,1.64612,-1.55423,0.260999,1.6684,-1.653,0.343358,1.67743,-1.69148,0.116966,1.92837,-2.13172,0.214939,1.85017,-1.98101,0.001023,1.89583,-2.02124,-0.116808,1.80299,-2.19524,-0.131423,1.71292,-2.24561,0.001359,1.58353,-2.18738,0.035927,1.52752,-2.20483,0.109392,1.40539,-1.80484,-0.072407,1.48528,-1.89287,-0.067314,1.5113,-1.93914,-0.066715,1.48864,-2.01039,0.125284,1.43731,-2.06574,0.015501,1.0961 [...]
+/*170*/{0.181091,1.92194,-1.75242,0.222288,1.85179,-1.90835,0.158584,1.76658,-1.63598,0.140559,1.6516,-1.55368,0.263047,1.67559,-1.6557,0.343275,1.68523,-1.69602,0.115039,1.93191,-2.1316,0.215081,1.85452,-1.98288,-0.000138,1.89938,-2.02094,-0.11992,1.80804,-2.19298,-0.135236,1.71784,-2.24332,-0.001801,1.58978,-2.18695,0.033777,1.53381,-2.20487,0.112469,1.40712,-1.80622,-0.0702,1.48934,-1.89177,-0.065798,1.51543,-1.93718,-0.064609,1.49381,-2.00871,0.123789,1.44282,-2.06611,0.01171,1.09896 [...]
+/*171*/{0.181604,1.92598,-1.75433,0.222217,1.85566,-1.90952,0.160994,1.77198,-1.63611,0.143785,1.65649,-1.55269,0.263218,1.68173,-1.65737,0.343997,1.69166,-1.69918,0.113661,1.93589,-2.13159,0.214714,1.85874,-1.98419,0.001406,1.90112,-2.01855,-0.12145,1.81404,-2.19073,-0.139267,1.72373,-2.24198,-0.004208,1.59512,-2.18647,0.031186,1.54006,-2.20424,0.114253,1.40873,-1.80676,-0.067618,1.49343,-1.89064,-0.063185,1.51878,-1.9358,-0.062538,1.49903,-2.00764,0.125201,1.44771,-2.06642,0.00939,1.10 [...]
+/*172*/{0.183938,1.93062,-1.75563,0.223645,1.85939,-1.91142,0.162843,1.77709,-1.63671,0.146501,1.66165,-1.55343,0.2643,1.68731,-1.65949,0.344324,1.69798,-1.70267,0.112608,1.93888,-2.132,0.215017,1.86298,-1.98524,0.001118,1.90435,-2.01877,-0.123785,1.81913,-2.18885,-0.142123,1.72878,-2.23987,-0.006584,1.60088,-2.18623,0.029004,1.54554,-2.20412,0.11711,1.41067,-1.80776,-0.06588,1.4971,-1.8897,-0.061761,1.52317,-1.93499,-0.060853,1.50433,-2.00674,0.125724,1.45243,-2.06701,0.005498,1.10557,- [...]
+/*173*/{0.185171,1.93452,-1.75715,0.223996,1.86362,-1.91298,0.164771,1.78168,-1.63727,0.149952,1.66676,-1.55277,0.265129,1.6924,-1.6614,0.343778,1.70411,-1.70662,0.111579,1.94243,-2.13237,0.21503,1.86699,-1.98666,0.002184,1.90613,-2.01784,-0.123986,1.82448,-2.18699,-0.144827,1.73358,-2.23838,-0.008261,1.60551,-2.18553,0.027646,1.55065,-2.20386,0.119917,1.41279,-1.80883,-0.062437,1.50064,-1.88862,-0.060035,1.52713,-1.93278,-0.060189,1.51001,-2.00584,0.126342,1.45728,-2.06701,0.002523,1.10 [...]
+/*174*/{0.18639,1.93852,-1.75829,0.224417,1.86776,-1.91422,0.16673,1.7863,-1.63762,0.151729,1.67125,-1.5525,0.265748,1.69798,-1.66333,0.343592,1.70932,-1.70954,0.110856,1.94496,-2.13208,0.215256,1.8704,-1.98727,0.002409,1.90904,-2.01798,-0.125383,1.82742,-2.18202,-0.147717,1.7382,-2.2363,-0.010284,1.61064,-2.18525,0.025712,1.55575,-2.20384,0.121176,1.41404,-1.80961,-0.060691,1.50477,-1.88667,-0.059122,1.53085,-1.93178,-0.056532,1.51362,-2.00357,0.125698,1.46123,-2.06685,0.001199,1.11375, [...]
+/*175*/{0.188441,1.9424,-1.75981,0.225084,1.87212,-1.91512,0.1681,1.79075,-1.63809,0.15429,1.67532,-1.55206,0.265895,1.70221,-1.66479,0.342809,1.71334,-1.71269,0.110334,1.94827,-2.1321,0.215549,1.8738,-1.98836,0.0024,1.91103,-2.01758,-0.126482,1.83158,-2.17988,-0.149744,1.74263,-2.23451,-0.011566,1.61472,-2.18525,0.024272,1.56015,-2.20344,0.123861,1.41693,-1.81008,-0.059459,1.50851,-1.88588,-0.057677,1.53501,-1.93026,-0.055023,1.51809,-2.00169,0.1269,1.46494,-2.06672,-0.003071,1.11622,-1 [...]
+/*176*/{0.189131,1.94538,-1.76113,0.22532,1.87489,-1.91638,0.169456,1.79463,-1.63853,0.155254,1.679,-1.55158,0.265892,1.70712,-1.66687,0.343013,1.71814,-1.71528,0.109606,1.95096,-2.13217,0.216491,1.87708,-1.989,0.003087,1.91375,-2.0167,-0.127479,1.83555,-2.17809,-0.151637,1.74631,-2.23265,-0.012721,1.61873,-2.1851,0.023542,1.56478,-2.20376,0.125271,1.41938,-1.81065,-0.056785,1.51169,-1.88484,-0.056246,1.53861,-1.92823,-0.053673,1.5219,-2.00061,0.126809,1.46799,-2.06655,-0.005115,1.12012, [...]
+/*177*/{0.190537,1.94856,-1.76215,0.227149,1.87905,-1.91796,0.171429,1.79884,-1.63945,0.156457,1.68197,-1.55144,0.266263,1.70977,-1.66813,0.342368,1.72172,-1.71827,0.109559,1.95334,-2.13254,0.21655,1.88001,-1.99055,0.00335,1.91474,-2.01658,-0.128778,1.83946,-2.17475,-0.153083,1.74973,-2.23095,-0.014014,1.62277,-2.18472,0.022232,1.56853,-2.20383,0.127321,1.42232,-1.8114,-0.054428,1.5153,-1.88302,-0.055156,1.54181,-1.92738,-0.052395,1.52581,-2.00038,0.127032,1.47078,-2.06612,-0.005908,1.12 [...]
+/*178*/{0.191252,1.9512,-1.76281,0.227771,1.88206,-1.91859,0.17202,1.80204,-1.6395,0.158173,1.68525,-1.55119,0.265742,1.7127,-1.6699,0.341611,1.72443,-1.72068,0.108703,1.95572,-2.13263,0.217218,1.88156,-1.99095,0.003275,1.91741,-2.01578,-0.128059,1.8426,-2.17249,-0.154751,1.75301,-2.2289,-0.013472,1.626,-2.1842,0.022103,1.57226,-2.20315,0.128609,1.42532,-1.81152,-0.053822,1.51892,-1.88231,-0.053987,1.5454,-1.92599,-0.051658,1.52929,-1.99841,0.127208,1.47352,-2.06563,-0.00635,1.12741,-1.7 [...]
+/*179*/{0.192432,1.95394,-1.76457,0.228446,1.88488,-1.92004,0.172703,1.80522,-1.64009,0.158809,1.68845,-1.55086,0.265469,1.71543,-1.67045,0.341124,1.72681,-1.72322,0.108957,1.95777,-2.133,0.217859,1.88407,-1.99241,0.004156,1.91934,-2.01585,-0.130087,1.8446,-2.16917,-0.156352,1.75584,-2.22719,-0.014031,1.62932,-2.18312,0.021386,1.57502,-2.20321,0.129404,1.42801,-1.81148,-0.052843,1.52159,-1.8815,-0.053338,1.54838,-1.92485,-0.0506,1.53178,-1.99716,0.127745,1.47596,-2.0655,-0.005076,1.13033 [...]
+/*180*/{0.193294,1.95565,-1.7648,0.229106,1.8871,-1.92057,0.173136,1.80787,-1.64078,0.158877,1.69102,-1.55156,0.265249,1.71744,-1.67188,0.340115,1.72877,-1.72544,0.108202,1.95973,-2.1327,0.219102,1.88606,-1.99315,0.0042,1.92197,-2.01548,-0.130551,1.84718,-2.16689,-0.15685,1.75819,-2.22539,-0.013989,1.63198,-2.18272,0.02143,1.57813,-2.20329,0.130413,1.43074,-1.81162,-0.051621,1.52466,-1.88002,-0.05273,1.55122,-1.92451,-0.051287,1.53524,-1.99608,0.127693,1.47846,-2.06493,-0.005503,1.13267, [...]
+/*181*/{0.193918,1.95787,-1.76596,0.230011,1.88926,-1.92145,0.173594,1.81038,-1.6416,0.15907,1.6925,-1.55072,0.264055,1.71905,-1.67306,0.337532,1.73189,-1.72716,0.10909,1.96133,-2.13368,0.218954,1.8873,-1.99448,0.005624,1.92356,-2.01557,-0.130576,1.84827,-2.16536,-0.158113,1.76066,-2.22387,-0.014546,1.63471,-2.18259,0.021381,1.5806,-2.20367,0.130893,1.43318,-1.81137,-0.0513,1.52737,-1.87988,-0.052652,1.5544,-1.92407,-0.050135,1.53698,-1.99578,0.129176,1.48027,-2.06474,-0.00238,1.13404,-1 [...]
+/*182*/{0.195623,1.96064,-1.76667,0.229638,1.89058,-1.92252,0.173412,1.8124,-1.64184,0.158609,1.6951,-1.55114,0.26338,1.71986,-1.67388,0.336774,1.73037,-1.72889,0.109315,1.96263,-2.13397,0.220074,1.8887,-1.99506,0.005773,1.92521,-2.01579,-0.130717,1.84986,-2.16206,-0.15804,1.76258,-2.22268,-0.014794,1.63621,-2.18161,0.021498,1.5826,-2.20391,0.132451,1.4355,-1.81097,-0.050779,1.52925,-1.87953,-0.051678,1.5564,-1.92327,-0.049715,1.53889,-1.99503,0.129301,1.48157,-2.06395,0.002109,1.13516,- [...]
+/*183*/{0.196455,1.96099,-1.76729,0.230792,1.89287,-1.92356,0.17316,1.81431,-1.64238,0.158578,1.69595,-1.55138,0.262498,1.71976,-1.67458,0.335705,1.72905,-1.73065,0.109487,1.96369,-2.13431,0.220326,1.88917,-1.99602,0.006857,1.9266,-2.01588,-0.130105,1.85143,-2.16039,-0.158192,1.76377,-2.22047,-0.013725,1.63797,-2.18141,0.021344,1.58356,-2.20415,0.13338,1.4378,-1.81081,-0.050099,1.53103,-1.87819,-0.051538,1.55919,-1.92358,-0.049556,1.54047,-1.99401,0.130185,1.48251,-2.06346,0.004754,1.135 [...]
+/*184*/{0.196408,1.96107,-1.76799,0.230461,1.89326,-1.92438,0.173184,1.81506,-1.64303,0.156585,1.6967,-1.55154,0.261587,1.71932,-1.67594,0.334304,1.72754,-1.73229,0.110836,1.96427,-2.13497,0.220874,1.88945,-1.99687,0.008298,1.92781,-2.0159,-0.130662,1.85251,-2.15889,-0.158224,1.76513,-2.21941,-0.012793,1.63931,-2.18109,0.022206,1.58538,-2.20463,0.133611,1.43968,-1.81034,-0.049356,1.53226,-1.87807,-0.051741,1.56056,-1.92294,-0.049101,1.54086,-1.99304,0.130749,1.48306,-2.06252,0.010907,1.1 [...]
+/*185*/{0.19667,1.96125,-1.76869,0.230436,1.89383,-1.92504,0.172135,1.81624,-1.64379,0.154807,1.69741,-1.55242,0.259763,1.71873,-1.67662,0.332283,1.72597,-1.73353,0.111582,1.96461,-2.13544,0.22217,1.89022,-1.99715,0.008622,1.92924,-2.0166,-0.131049,1.8527,-2.15719,-0.158272,1.76545,-2.21762,-0.012615,1.63981,-2.18105,0.022731,1.58581,-2.20472,0.133997,1.44153,-1.80959,-0.049224,1.53316,-1.87775,-0.052001,1.5613,-1.92298,-0.048969,1.54125,-1.99307,0.131802,1.4833,-2.06224,0.016683,1.13374 [...]
+/*186*/{0.197541,1.961,-1.7692,0.231108,1.89468,-1.92592,0.171406,1.81676,-1.6442,0.15355,1.69752,-1.55215,0.258436,1.71732,-1.67747,0.331001,1.72345,-1.73505,0.111994,1.96453,-2.13604,0.222619,1.89087,-1.9973,0.009579,1.93022,-2.01716,-0.128527,1.85223,-2.15515,-0.15868,1.76579,-2.21662,-0.012242,1.64,-2.18077,0.02361,1.58605,-2.20516,0.134704,1.44276,-1.80982,-0.049591,1.53323,-1.87814,-0.051884,1.56211,-1.92321,-0.049101,1.54086,-1.99304,0.132677,1.48362,-2.0616,0.023985,1.13237,-1.78 [...]
+/*187*/{0.19833,1.96044,-1.77024,0.231197,1.89453,-1.92718,0.170189,1.81686,-1.645,0.151071,1.69738,-1.55266,0.257315,1.71547,-1.67827,0.329136,1.72052,-1.73576,0.113074,1.96438,-2.13673,0.22257,1.89097,-1.99785,0.009918,1.93023,-2.0181,-0.128716,1.85283,-2.1535,-0.157766,1.76534,-2.21587,-0.011512,1.6397,-2.18085,0.024525,1.58612,-2.20555,0.13492,1.44385,-1.80903,-0.049425,1.53283,-1.87848,-0.051884,1.56211,-1.92321,-0.048994,1.54055,-1.9935,0.133681,1.48356,-2.0607,0.030824,1.13119,-1. [...]
+/*188*/{0.19846,1.96011,-1.77007,0.231,1.89401,-1.92709,0.168849,1.81611,-1.6457,0.148229,1.6973,-1.55365,0.254758,1.7133,-1.67943,0.327171,1.71703,-1.73624,0.113903,1.96443,-2.13708,0.222044,1.89028,-1.99831,0.011517,1.93033,-2.01857,-0.128042,1.851,-2.15173,-0.156895,1.76454,-2.21457,-0.009301,1.63933,-2.18076,0.025364,1.58538,-2.20618,0.134935,1.44406,-1.80893,-0.049274,1.53235,-1.87687,-0.05208,1.56102,-1.92313,-0.048986,1.53832,-1.99366,0.134402,1.48349,-2.06008,0.035934,1.12926,-1. [...]
+/*189*/{0.198788,1.95857,-1.77036,0.230762,1.89355,-1.92735,0.167284,1.81565,-1.64626,0.147652,1.69661,-1.55371,0.252829,1.71012,-1.67959,0.325558,1.71321,-1.7373,0.114782,1.96336,-2.1375,0.22196,1.8896,-1.99873,0.011823,1.93025,-2.01902,-0.127169,1.85174,-2.15084,-0.156376,1.76332,-2.21414,-0.008122,1.63796,-2.18081,0.027224,1.58453,-2.20725,0.134979,1.44417,-1.80871,-0.049716,1.53103,-1.87733,-0.052142,1.56064,-1.92404,-0.048387,1.53659,-1.99429,0.135018,1.48284,-2.05949,0.042947,1.127 [...]
+/*190*/{0.198874,1.95766,-1.77091,0.231297,1.89312,-1.92778,0.165071,1.81449,-1.647,0.144837,1.69573,-1.5547,0.249851,1.70732,-1.68031,0.322725,1.70887,-1.73775,0.114929,1.96236,-2.13757,0.222211,1.88923,-1.99909,0.012638,1.9298,-2.01937,-0.127815,1.85077,-2.14858,-0.155082,1.76207,-2.21344,-0.007489,1.63682,-2.1815,0.028592,1.58276,-2.20736,0.135015,1.44417,-1.80869,-0.049445,1.52959,-1.87753,-0.051466,1.55912,-1.92421,-0.047925,1.53487,-1.99429,0.13592,1.48132,-2.05927,0.050117,1.12473 [...]
+/*191*/{0.198963,1.95563,-1.77095,0.231027,1.89124,-1.92774,0.163146,1.81343,-1.64724,0.140684,1.69425,-1.55501,0.247734,1.70319,-1.6808,0.320823,1.70354,-1.7382,0.1159,1.96077,-2.13779,0.222338,1.88787,-1.99944,0.012834,1.92936,-2.02049,-0.125513,1.84962,-2.14923,-0.153955,1.75968,-2.21286,-0.006081,1.63452,-2.18191,0.029749,1.58138,-2.20797,0.135068,1.44381,-1.80862,-0.04992,1.52843,-1.87845,-0.049734,1.55668,-1.92581,-0.04796,1.53277,-1.99484,0.136228,1.48023,-2.05905,0.058437,1.12357 [...]
+/*192*/{0.198025,1.95363,-1.77129,0.231606,1.89072,-1.92793,0.160557,1.81166,-1.6476,0.137237,1.69257,-1.55608,0.244702,1.69972,-1.68094,0.317631,1.69815,-1.73811,0.116513,1.95939,-2.13807,0.222081,1.88657,-1.99932,0.0137,1.92802,-2.02161,-0.124351,1.84615,-2.14887,-0.152352,1.7575,-2.2131,-0.003731,1.63258,-2.18234,0.032157,1.57892,-2.20903,0.135071,1.44275,-1.80855,-0.049329,1.52539,-1.87921,-0.050528,1.55446,-1.92672,-0.047817,1.53002,-1.99529,0.137363,1.47788,-2.05932,0.06418,1.11985 [...]
+/*193*/{0.197698,1.9508,-1.77098,0.231249,1.88872,-1.92804,0.158883,1.8096,-1.64785,0.132285,1.69027,-1.55745,0.241565,1.69541,-1.68172,0.315262,1.69212,-1.73804,0.117008,1.95744,-2.13841,0.222116,1.88474,-1.99958,0.013354,1.92687,-2.02222,-0.123157,1.8439,-2.14865,-0.151246,1.75414,-2.21312,-0.001592,1.62995,-2.18301,0.033992,1.57636,-2.20943,0.135536,1.44146,-1.8088,-0.048604,1.52344,-1.87945,-0.050768,1.55182,-1.92701,-0.046918,1.52777,-1.99648,0.137597,1.47538,-2.06,0.06991,1.11738,- [...]
+/*194*/{0.197575,1.94904,-1.77114,0.230759,1.88654,-1.92792,0.15635,1.80725,-1.64792,0.128878,1.68737,-1.55804,0.238593,1.69071,-1.68182,0.312463,1.68563,-1.73775,0.117109,1.9554,-2.13846,0.22212,1.88239,-1.99937,0.013414,1.92567,-2.02326,-0.12243,1.84067,-2.1484,-0.150316,1.75068,-2.2133,0.000125,1.62675,-2.18413,0.03583,1.57326,-2.21055,0.135586,1.44028,-1.80888,-0.048684,1.52119,-1.8802,-0.050129,1.54887,-1.92806,-0.047495,1.52482,-1.99715,0.137908,1.47231,-2.06101,0.074656,1.1143,-1. [...]
+/*195*/{0.196571,1.94583,-1.7713,0.231087,1.88419,-1.92815,0.1538,1.80423,-1.64854,0.124559,1.68471,-1.55927,0.234996,1.68568,-1.682,0.309246,1.6789,-1.73723,0.117417,1.9529,-2.1386,0.222705,1.88048,-1.99935,0.014108,1.9231,-2.02391,-0.121534,1.83664,-2.14806,-0.148907,1.74653,-2.21439,0.001854,1.62327,-2.18542,0.038723,1.56995,-2.21148,0.136181,1.43841,-1.80943,-0.049031,1.5176,-1.88051,-0.049553,1.5469,-1.92851,-0.046872,1.52149,-1.99821,0.138792,1.47016,-2.0621,0.080093,1.11311,-1.797 [...]
+/*196*/{0.196307,1.94282,-1.77082,0.230569,1.88181,-1.92818,0.151424,1.80116,-1.64847,0.120124,1.68171,-1.56027,0.231369,1.68017,-1.68196,0.306425,1.6715,-1.73621,0.11677,1.94985,-2.13899,0.222673,1.87802,-1.99868,0.013226,1.92136,-2.02538,-0.119837,1.83295,-2.149,-0.147404,1.74225,-2.21535,0.003624,1.61896,-2.18656,0.04084,1.56588,-2.21212,0.136938,1.43611,-1.81015,-0.049932,1.5142,-1.88257,-0.049848,1.54277,-1.92919,-0.04712,1.51848,-1.99871,0.138868,1.46756,-2.06299,0.086609,1.10895,- [...]
+/*197*/{0.195285,1.93965,-1.77004,0.231033,1.8788,-1.92736,0.148538,1.79778,-1.64848,0.115091,1.67824,-1.56106,0.227193,1.6744,-1.68167,0.302364,1.66433,-1.73622,0.11682,1.94678,-2.13931,0.222667,1.87527,-1.99866,0.012573,1.91835,-2.02606,-0.119476,1.82852,-2.1491,-0.145625,1.73694,-2.21598,0.005453,1.61506,-2.18779,0.043555,1.56241,-2.21375,0.137242,1.43372,-1.81038,-0.049565,1.51069,-1.8825,-0.049906,1.53963,-1.93033,-0.047733,1.51449,-1.99917,0.138928,1.46477,-2.06454,0.091321,1.10661 [...]
+/*198*/{0.193917,1.93588,-1.76944,0.231552,1.8764,-1.92748,0.146065,1.794,-1.64845,0.112161,1.67493,-1.56155,0.222779,1.66818,-1.68142,0.298896,1.65553,-1.73439,0.116863,1.94355,-2.13933,0.222829,1.87248,-1.99893,0.012543,1.91538,-2.02659,-0.119605,1.82364,-2.15129,-0.144466,1.73129,-2.21765,0.008198,1.61017,-2.18916,0.046053,1.55749,-2.21534,0.137133,1.43153,-1.8111,-0.04949,1.50714,-1.88327,-0.049558,1.53642,-1.93102,-0.048031,1.51021,-2.00027,0.139103,1.46186,-2.06547,0.096174,1.10335 [...]
+/*199*/{0.192742,1.93201,-1.7687,0.231223,1.87257,-1.92715,0.143404,1.78957,-1.64795,0.105645,1.6713,-1.56271,0.218558,1.66158,-1.68109,0.295041,1.64737,-1.73327,0.116833,1.93975,-2.13951,0.222701,1.86982,-1.99892,0.012615,1.91135,-2.02775,-0.118279,1.81737,-2.15216,-0.142767,1.72534,-2.21861,0.010087,1.60487,-2.19111,0.04951,1.55249,-2.21628,0.136923,1.42859,-1.81274,-0.049419,1.5029,-1.88366,-0.051228,1.53227,-1.93224,-0.048031,1.50554,-2.00015,0.138751,1.45896,-2.06685,0.100872,1.1009 [...]
+/*200*/{0.192019,1.92781,-1.76759,0.231822,1.86897,-1.92727,0.139864,1.78522,-1.64814,0.100642,1.66692,-1.56422,0.214336,1.65476,-1.68133,0.290629,1.63858,-1.73199,0.117199,1.93553,-2.13954,0.222376,1.86624,-1.99847,0.012319,1.90759,-2.02834,-0.11839,1.81102,-2.15499,-0.140989,1.71906,-2.21975,0.014243,1.59987,-2.19272,0.052099,1.54751,-2.21791,0.138549,1.42604,-1.81398,-0.049866,1.49858,-1.88413,-0.050519,1.52829,-1.93207,-0.049082,1.50205,-2.00109,0.13855,1.45572,-2.06803,0.105458,1.09 [...]
+/*201*/{0.190695,1.92349,-1.76722,0.231811,1.86474,-1.92608,0.136923,1.78035,-1.64752,0.094605,1.66195,-1.5649,0.209787,1.64751,-1.68126,0.285938,1.62899,-1.73012,0.117349,1.93176,-2.13988,0.222072,1.86217,-1.9983,0.010957,1.90259,-2.02874,-0.117637,1.80396,-2.15671,-0.139116,1.71208,-2.22191,0.016961,1.59378,-2.19351,0.055382,1.54219,-2.21919,0.139129,1.42285,-1.81455,-0.050344,1.49351,-1.88484,-0.05067,1.5235,-1.93262,-0.04938,1.49662,-2.00073,0.137506,1.45193,-2.06937,0.109839,1.09508 [...]
+/*202*/{0.189155,1.91888,-1.76614,0.230064,1.86056,-1.92562,0.133782,1.77543,-1.64816,0.088927,1.65798,-1.56678,0.204479,1.6399,-1.68012,0.281919,1.62044,-1.72843,0.117586,1.92765,-2.14027,0.222218,1.85829,-1.998,0.010872,1.89864,-2.03027,-0.11861,1.79665,-2.15781,-0.13692,1.70475,-2.22387,0.019904,1.58797,-2.19635,0.05888,1.53622,-2.2208,0.13918,1.41949,-1.8161,-0.050218,1.48858,-1.88512,-0.050707,1.51817,-1.93392,-0.050127,1.49202,-2.00096,0.137146,1.44868,-2.07076,0.114497,1.09233,-1. [...]
+/*203*/{0.187994,1.9144,-1.76529,0.231036,1.85597,-1.92465,0.130454,1.77034,-1.64812,0.083636,1.65413,-1.56802,0.20008,1.63274,-1.67984,0.277988,1.61061,-1.72737,0.117459,1.92244,-2.14023,0.222168,1.85452,-1.9977,0.010103,1.89372,-2.03141,-0.11846,1.78876,-2.16073,-0.134714,1.69738,-2.22563,0.022557,1.5809,-2.19784,0.062786,1.52991,-2.22211,0.139202,1.41563,-1.81751,-0.050673,1.48396,-1.88449,-0.051605,1.51335,-1.9341,-0.050443,1.48629,-2.00055,0.136853,1.44571,-2.07214,0.118746,1.0889,- [...]
+/*204*/{0.18667,1.90906,-1.76433,0.229679,1.85107,-1.92347,0.126547,1.76498,-1.64774,0.077449,1.65013,-1.56972,0.194254,1.62535,-1.67942,0.272529,1.60048,-1.72543,0.117626,1.91753,-2.14056,0.222812,1.85026,-1.99765,0.008715,1.8878,-2.03229,-0.115652,1.78055,-2.16374,-0.131939,1.68967,-2.22819,0.025865,1.574,-2.19939,0.067055,1.52342,-2.22366,0.140127,1.41215,-1.81877,-0.050342,1.47797,-1.88486,-0.051473,1.50827,-1.93437,-0.052312,1.48044,-2.00011,0.136178,1.44145,-2.07363,0.12264,1.08693 [...]
+/*205*/{0.184947,1.9038,-1.76334,0.230649,1.84542,-1.92281,0.123416,1.75964,-1.64814,0.071842,1.64634,-1.57135,0.189096,1.6172,-1.67911,0.26748,1.59013,-1.7232,0.118365,1.91267,-2.1413,0.222902,1.8448,-1.99718,0.008896,1.88211,-2.03363,-0.115645,1.7728,-2.16705,-0.129013,1.68151,-2.23017,0.031366,1.56712,-2.20114,0.070701,1.51674,-2.22552,0.140001,1.40811,-1.82037,-0.050098,1.47221,-1.88525,-0.051747,1.50206,-1.93472,-0.05287,1.47512,-2.00023,0.13583,1.43731,-2.0752,0.126959,1.0832,-1.78 [...]
+/*206*/{0.18391,1.89942,-1.76232,0.232053,1.83955,-1.92236,0.119076,1.75468,-1.64785,0.065441,1.64165,-1.57175,0.18427,1.60868,-1.67802,0.261404,1.58012,-1.72168,0.118653,1.90758,-2.14164,0.22327,1.84045,-1.99694,0.008304,1.87525,-2.03416,-0.113426,1.76344,-2.16964,-0.125394,1.67268,-2.23259,0.034888,1.55949,-2.2031,0.075489,1.50931,-2.22701,0.140674,1.40395,-1.82147,-0.051236,1.46638,-1.88568,-0.052342,1.49676,-1.9355,-0.053476,1.46848,-1.99978,0.13591,1.43334,-2.07678,0.128951,1.07971, [...]
+/*207*/{0.18279,1.89304,-1.76129,0.231553,1.83378,-1.92106,0.116236,1.74873,-1.64794,0.059739,1.63758,-1.57333,0.178948,1.60119,-1.67766,0.256093,1.56979,-1.71925,0.119192,1.90209,-2.14206,0.223597,1.83475,-1.99589,0.007562,1.86971,-2.03593,-0.112013,1.75473,-2.17257,-0.121927,1.6641,-2.23549,0.038747,1.5535,-2.20589,0.080926,1.50236,-2.22861,0.140459,1.39891,-1.82249,-0.052299,1.46081,-1.8865,-0.052954,1.49034,-1.9362,-0.055193,1.46379,-2.00025,0.135118,1.42857,-2.07861,0.132363,1.0756, [...]
+/*208*/{0.181141,1.8869,-1.76031,0.231133,1.82874,-1.91946,0.111462,1.74308,-1.64813,0.052295,1.63299,-1.57425,0.17319,1.5931,-1.67605,0.249801,1.55941,-1.71642,0.120332,1.89676,-2.14262,0.224398,1.82967,-1.99613,0.006797,1.86231,-2.03695,-0.109227,1.74475,-2.17575,-0.118016,1.6551,-2.23817,0.044493,1.54563,-2.20835,0.086288,1.49531,-2.23032,0.140129,1.3946,-1.82434,-0.052985,1.45436,-1.88556,-0.054424,1.48429,-1.93666,-0.05724,1.45794,-1.9996,0.134174,1.42376,-2.08062,0.137027,1.07217,- [...]
+/*209*/{0.179337,1.88121,-1.75905,0.232528,1.82156,-1.91891,0.107234,1.73722,-1.64866,0.045916,1.63,-1.57546,0.167522,1.5853,-1.67543,0.243572,1.55023,-1.71404,0.121382,1.89156,-2.14394,0.225544,1.82365,-1.9947,0.006098,1.85702,-2.03808,-0.106206,1.73615,-2.17918,-0.112812,1.64605,-2.24138,0.049673,1.53781,-2.21065,0.092297,1.48804,-2.23178,0.139815,1.38948,-1.82462,-0.055071,1.44939,-1.88534,-0.05648,1.47799,-1.93641,-0.059411,1.45354,-2.00042,0.13232,1.41808,-2.082,0.141371,1.06808,-1. [...]
+/*210*/{0.17767,1.87589,-1.75833,0.233136,1.81669,-1.91764,0.102904,1.73173,-1.64909,0.039122,1.62583,-1.57748,0.161209,1.57785,-1.67489,0.23778,1.54134,-1.7117,0.12223,1.88675,-2.14534,0.225815,1.81818,-1.99466,0.006505,1.8505,-2.03919,-0.104342,1.7266,-2.18338,-0.107127,1.63668,-2.24444,0.055622,1.52967,-2.21314,0.098634,1.48077,-2.2335,0.139668,1.38434,-1.82628,-0.057453,1.44335,-1.8857,-0.05835,1.47207,-1.9363,-0.062009,1.44887,-2.00108,0.129976,1.41421,-2.08231,0.146012,1.06625,-1.7 [...]
+/*211*/{0.177203,1.87069,-1.75687,0.233775,1.81031,-1.91628,0.099466,1.72751,-1.64966,0.033235,1.62258,-1.57901,0.155103,1.57018,-1.67229,0.229768,1.53272,-1.70899,0.123817,1.88143,-2.14624,0.228125,1.81285,-1.9935,0.007222,1.8476,-2.03971,-0.099034,1.71732,-2.18699,-0.098869,1.62709,-2.2475,0.06209,1.52237,-2.21626,0.106014,1.47427,-2.23515,0.138923,1.37887,-1.82817,-0.05958,1.43712,-1.88658,-0.060671,1.46621,-1.93534,-0.064591,1.4443,-2.0007,0.12666,1.41056,-2.08352,0.150699,1.06258,-1 [...]
+/*212*/{0.176099,1.86558,-1.75526,0.233657,1.80412,-1.91464,0.095137,1.72272,-1.64983,0.026268,1.6198,-1.58049,0.148475,1.56443,-1.67103,0.223004,1.5251,-1.70573,0.125437,1.87711,-2.14808,0.228424,1.80693,-1.9929,0.00828,1.84695,-2.04025,-0.092749,1.70808,-2.19288,-0.099559,1.61901,-2.25031,0.06958,1.51574,-2.21874,0.114032,1.46797,-2.23649,0.137098,1.3743,-1.83077,-0.061821,1.43302,-1.88544,-0.063115,1.46138,-1.9356,-0.068458,1.44065,-2.00121,0.124309,1.40813,-2.08496,0.15521,1.06032,-1 [...]
+/*213*/{0.17501,1.86097,-1.75379,0.234402,1.80018,-1.91385,0.091145,1.71793,-1.65012,0.018059,1.61727,-1.58189,0.142655,1.55818,-1.67044,0.214838,1.51869,-1.70331,0.1275,1.87297,-2.14936,0.229627,1.80296,-1.99178,0.009047,1.84404,-2.03995,-0.087045,1.69884,-2.19805,-0.086555,1.60805,-2.25357,0.078131,1.50887,-2.22063,0.121861,1.46162,-2.23762,0.135451,1.36947,-1.83132,-0.063955,1.42873,-1.88528,-0.065327,1.4584,-1.93557,-0.070483,1.43745,-1.99959,0.122101,1.40555,-2.08603,0.161124,1.0612 [...]
+/*214*/{0.174461,1.85669,-1.75244,0.235122,1.79595,-1.91244,0.086497,1.71415,-1.64981,0.010663,1.61512,-1.58268,0.135517,1.55396,-1.66918,0.207683,1.51309,-1.70067,0.129761,1.8689,-2.15009,0.230508,1.79806,-1.98994,0.009551,1.84321,-2.04069,-0.080107,1.68968,-2.20326,-0.077763,1.59892,-2.25641,0.086798,1.50324,-2.22155,0.131686,1.4569,-2.23814,0.133795,1.36557,-1.83228,-0.065386,1.42487,-1.88369,-0.066001,1.45731,-1.93444,-0.073077,1.43512,-1.99734,0.12031,1.40432,-2.08783,0.165298,1.065 [...]
+/*215*/{0.17434,1.8529,-1.75063,0.235397,1.79328,-1.91117,0.081824,1.71226,-1.65075,0.003604,1.61448,-1.58404,0.128142,1.5511,-1.66841,0.200147,1.50754,-1.69879,0.131525,1.86548,-2.14963,0.230926,1.79553,-1.98954,0.009607,1.84063,-2.04073,-0.075742,1.6823,-2.20904,-0.068469,1.59069,-2.25862,0.096107,1.4991,-2.22221,0.141839,1.45326,-2.23886,0.131699,1.36197,-1.83385,-0.067317,1.42218,-1.88242,-0.067377,1.4563,-1.93377,-0.074275,1.43382,-1.99552,0.118648,1.40293,-2.08943,0.173058,1.06658, [...]
+/*216*/{0.173541,1.8492,-1.74951,0.234834,1.79096,-1.90997,0.076695,1.71164,-1.65107,-0.005079,1.61515,-1.58647,0.120526,1.54871,-1.66703,0.193149,1.50376,-1.69614,0.13353,1.86321,-2.14929,0.231153,1.79253,-1.98797,0.01042,1.83957,-2.04134,-0.069079,1.67683,-2.21393,-0.059624,1.58374,-2.26089,0.105931,1.49599,-2.22258,0.152992,1.45113,-2.23889,0.130538,1.35877,-1.83357,-0.068613,1.42098,-1.88101,-0.069148,1.45568,-1.93411,-0.075391,1.43249,-1.99382,0.117105,1.40183,-2.09111,0.180061,1.06 [...]
+/*217*/{0.1726,1.84627,-1.74903,0.233637,1.78783,-1.90881,0.071252,1.71188,-1.65136,-0.013871,1.61657,-1.58898,0.113494,1.54727,-1.6667,0.186519,1.50095,-1.6932,0.135572,1.86107,-2.14856,0.231555,1.79033,-1.9871,0.011611,1.83744,-2.04141,-0.062943,1.67071,-2.21655,-0.049993,1.57834,-2.26259,0.116281,1.49358,-2.22225,0.164142,1.45009,-2.23841,0.127954,1.35565,-1.83368,-0.070653,1.42067,-1.88031,-0.06952,1.45445,-1.93376,-0.076043,1.43264,-1.99212,0.115063,1.40161,-2.09247,0.186326,1.07375 [...]
+/*218*/{0.172977,1.84463,-1.74822,0.232717,1.78545,-1.9069,0.066434,1.7123,-1.65201,-0.020257,1.61964,-1.59113,0.105952,1.54705,-1.66604,0.178767,1.49861,-1.69146,0.138023,1.8595,-2.14763,0.231374,1.78842,-1.986,0.012249,1.83622,-2.04225,-0.056852,1.66583,-2.21721,-0.041402,1.57422,-2.2636,0.126878,1.49228,-2.22109,0.175283,1.45001,-2.2377,0.127012,1.35311,-1.83298,-0.070772,1.42069,-1.87967,-0.071208,1.4538,-1.93372,-0.076898,1.43241,-1.99132,0.114596,1.4006,-2.09302,0.194098,1.07943,-1 [...]
+/*219*/{0.171982,1.84336,-1.74754,0.232677,1.78428,-1.90648,0.061039,1.71383,-1.65237,-0.026761,1.62296,-1.59325,0.099594,1.54733,-1.66471,0.170221,1.4984,-1.68906,0.140375,1.85828,-2.14692,0.231897,1.78738,-1.98444,0.012953,1.83608,-2.04306,-0.050645,1.66219,-2.21766,-0.032905,1.57073,-2.26481,0.137463,1.49204,-2.22053,0.187112,1.45116,-2.23605,0.124744,1.35176,-1.83242,-0.070859,1.42064,-1.87962,-0.07137,1.45301,-1.93347,-0.076989,1.43245,-1.99118,0.114339,1.39969,-2.0938,0.201726,1.08 [...]
+/*220*/{0.170745,1.84279,-1.74713,0.231015,1.78271,-1.90457,0.055154,1.71591,-1.6528,-0.034897,1.6275,-1.59579,0.092096,1.54843,-1.66372,0.163286,1.49852,-1.68681,0.142527,1.85803,-2.14667,0.231643,1.78635,-1.98382,0.013559,1.83563,-2.04324,-0.043172,1.66013,-2.21789,-0.024635,1.56792,-2.2658,0.148629,1.49391,-2.21916,0.198114,1.45419,-2.23425,0.124165,1.35093,-1.83066,-0.071257,1.42067,-1.88002,-0.072795,1.4534,-1.9343,-0.077233,1.43293,-1.99138,0.115034,1.3994,-2.09374,0.209274,1.0897, [...]
+/*221*/{0.169016,1.84339,-1.74634,0.231595,1.78261,-1.90387,0.05058,1.71845,-1.65342,-0.042823,1.63145,-1.5976,0.085539,1.55057,-1.66338,0.15666,1.49927,-1.68496,0.145683,1.85746,-2.14528,0.232126,1.78612,-1.98152,0.014604,1.83582,-2.04389,-0.037781,1.65816,-2.21818,-0.016347,1.56595,-2.26674,0.157786,1.49506,-2.21688,0.209105,1.4582,-2.23181,0.121776,1.35106,-1.82869,-0.072184,1.42037,-1.88047,-0.072902,1.4535,-1.93444,-0.077563,1.4333,-1.99248,0.113183,1.39868,-2.09446,0.216158,1.0967, [...]
+/*222*/{0.166856,1.84394,-1.74498,0.229972,1.78386,-1.90126,0.044042,1.72233,-1.65411,-0.049188,1.6358,-1.59989,0.078354,1.5529,-1.66173,0.148662,1.5014,-1.6828,0.147922,1.85781,-2.14486,0.232553,1.78673,-1.9803,0.015779,1.83589,-2.04498,-0.03288,1.65652,-2.21798,-0.00769,1.56432,-2.26709,0.168354,1.49971,-2.21514,0.220603,1.46338,-2.22843,0.121031,1.35167,-1.82752,-0.072504,1.4208,-1.88145,-0.072704,1.45341,-1.93474,-0.077018,1.4335,-1.9934,0.114044,1.39864,-2.09407,0.221803,1.10349,-1. [...]
+/*223*/{0.163955,1.84658,-1.74453,0.230868,1.78424,-1.9008,0.038906,1.72615,-1.65439,-0.056348,1.64068,-1.60165,0.07196,1.55666,-1.66113,0.141467,1.5046,-1.68113,0.150073,1.85913,-2.14442,0.232742,1.78749,-1.97864,0.016797,1.83682,-2.04636,-0.026813,1.65488,-2.21775,0.000621,1.56324,-2.2679,0.177281,1.50352,-2.21273,0.230832,1.46937,-2.2249,0.119774,1.35316,-1.82629,-0.073457,1.42131,-1.88243,-0.072955,1.45427,-1.93601,-0.077262,1.43414,-1.99418,0.114432,1.39838,-2.09371,0.228253,1.10994 [...]
+/*224*/{0.161306,1.84882,-1.74353,0.229428,1.78601,-1.89855,0.033642,1.73034,-1.65537,-0.062909,1.64501,-1.60309,0.065314,1.56095,-1.66117,0.134634,1.50802,-1.67984,0.152904,1.86093,-2.143,0.232549,1.78883,-1.97662,0.017652,1.83696,-2.04668,-0.021086,1.65387,-2.2171,0.008791,1.56335,-2.26843,0.186778,1.50911,-2.2101,0.240798,1.47692,-2.2212,0.118613,1.35507,-1.82566,-0.073316,1.4224,-1.88336,-0.072573,1.45493,-1.93663,-0.076668,1.4349,-1.99587,0.114432,1.39819,-2.09376,0.235203,1.11781,- [...]
+/*225*/{0.15771,1.85119,-1.74316,0.228989,1.78822,-1.89637,0.02764,1.73476,-1.65526,-0.070624,1.65002,-1.60497,0.0586,1.56557,-1.66155,0.127583,1.51273,-1.67928,0.154573,1.86312,-2.14188,0.23365,1.79109,-1.975,0.018133,1.83724,-2.04726,-0.016241,1.65286,-2.21669,0.016351,1.56379,-2.26909,0.195158,1.51499,-2.20681,0.250753,1.48471,-2.21655,0.119704,1.35714,-1.82493,-0.073457,1.42238,-1.8852,-0.072469,1.45499,-1.93709,-0.076271,1.43562,-1.99752,0.116202,1.39732,-2.09322,0.241024,1.12491,-1 [...]
+/*226*/{0.155229,1.85453,-1.74221,0.229194,1.79073,-1.89508,0.023413,1.73914,-1.65644,-0.077233,1.65501,-1.60687,0.05217,1.5706,-1.66159,0.121565,1.51727,-1.67817,0.156357,1.86554,-2.14064,0.233127,1.79286,-1.97288,0.019631,1.83834,-2.04832,-0.011611,1.65276,-2.21663,0.023746,1.56516,-2.27007,0.202401,1.52101,-2.20319,0.260482,1.49327,-2.21182,0.119533,1.35989,-1.82471,-0.072927,1.42385,-1.88684,-0.072155,1.45632,-1.93903,-0.074775,1.43657,-1.99799,0.117059,1.39695,-2.09355,0.245977,1.13 [...]
+/*227*/{0.151202,1.85724,-1.74174,0.22878,1.79344,-1.89262,0.017776,1.74371,-1.65723,-0.08363,1.66066,-1.60955,0.045996,1.57643,-1.66209,0.11547,1.52259,-1.6775,0.157932,1.86877,-2.13949,0.233748,1.79627,-1.9708,0.020359,1.84042,-2.04982,-0.005506,1.65313,-2.21649,0.031844,1.56644,-2.27034,0.211264,1.52927,-2.20016,0.268936,1.50269,-2.20731,0.119135,1.3629,-1.82397,-0.072364,1.42583,-1.88799,-0.070636,1.45814,-1.94038,-0.074519,1.43811,-1.9999,0.117877,1.39698,-2.09349,0.249163,1.13987,- [...]
+/*228*/{0.148174,1.86096,-1.74115,0.228517,1.79785,-1.89053,0.014181,1.74857,-1.6577,-0.090235,1.66554,-1.61153,0.040646,1.58258,-1.66233,0.109303,1.52813,-1.67683,0.160317,1.87252,-2.13831,0.234124,1.79944,-1.96913,0.021156,1.84135,-2.04958,-0.000767,1.6541,-2.21654,0.039451,1.56888,-2.27087,0.217682,1.53615,-2.19675,0.277818,1.5129,-2.20222,0.120697,1.36687,-1.82429,-0.071876,1.42835,-1.88963,-0.069345,1.46003,-1.94095,-0.072683,1.44043,-2.00112,0.118848,1.39716,-2.09362,0.253398,1.147 [...]
+/*229*/{0.144699,1.86459,-1.74101,0.227666,1.79968,-1.88862,0.008788,1.75346,-1.65834,-0.096628,1.67139,-1.61348,0.035178,1.58845,-1.66347,0.103638,1.53417,-1.67603,0.160927,1.8765,-2.13695,0.235039,1.80329,-1.96758,0.022297,1.84369,-2.05023,0.005549,1.65479,-2.21642,0.047249,1.57149,-2.27129,0.225232,1.54565,-2.19298,0.284847,1.5228,-2.19706,0.121189,1.3702,-1.82457,-0.070931,1.43066,-1.8911,-0.068309,1.46257,-1.94255,-0.071299,1.44204,-2.00243,0.119806,1.39702,-2.09376,0.255482,1.15542 [...]
+/*230*/{0.141757,1.86852,-1.74034,0.228357,1.80542,-1.88727,0.004734,1.75819,-1.65888,-0.101186,1.67716,-1.61585,0.029683,1.595,-1.6644,0.097851,1.53992,-1.67583,0.161734,1.88071,-2.1358,0.236909,1.80656,-1.9645,0.0237,1.84549,-2.05042,0.0113,1.65624,-2.21691,0.054439,1.57507,-2.27203,0.231766,1.55396,-2.18896,0.291979,1.53378,-2.19262,0.12305,1.37396,-1.82444,-0.06913,1.43343,-1.89168,-0.065996,1.46406,-1.94337,-0.069186,1.44479,-2.0037,0.121279,1.39736,-2.09383,0.260623,1.16194,-1.7647 [...]
+/*231*/{0.138463,1.87289,-1.73982,0.227447,1.80853,-1.88453,0.000919,1.76335,-1.6603,-0.106837,1.68323,-1.61773,0.024944,1.60109,-1.66531,0.092951,1.54655,-1.67612,0.163921,1.88527,-2.13476,0.238023,1.81081,-1.96248,0.02434,1.84847,-2.05109,0.015807,1.65879,-2.21824,0.061907,1.57835,-2.27196,0.237689,1.56287,-2.18444,0.298473,1.5445,-2.18672,0.123902,1.3779,-1.8248,-0.068492,1.43708,-1.89276,-0.064591,1.46749,-1.94552,-0.067184,1.44768,-2.00493,0.123383,1.39774,-2.09415,0.262854,1.16859, [...]
+/*232*/{0.135515,1.87676,-1.73962,0.22631,1.81372,-1.88334,-0.001953,1.76868,-1.66083,-0.111784,1.68986,-1.62053,0.019192,1.60784,-1.66609,0.088169,1.55256,-1.67539,0.165886,1.88994,-2.13304,0.237693,1.81508,-1.96104,0.025292,1.85165,-2.05109,0.020735,1.66216,-2.22012,0.069057,1.58222,-2.27238,0.242256,1.572,-2.1809,0.304094,1.55614,-2.18153,0.125969,1.38068,-1.82565,-0.066544,1.44066,-1.89432,-0.062709,1.4699,-1.94706,-0.064961,1.45044,-2.00582,0.125343,1.39729,-2.09411,0.267261,1.17604 [...]
+/*233*/{0.133213,1.88069,-1.73941,0.22694,1.81931,-1.88145,-0.005614,1.7737,-1.66143,-0.116796,1.69603,-1.62283,0.01521,1.61395,-1.66719,0.083198,1.55857,-1.67602,0.167144,1.89544,-2.13162,0.237847,1.81967,-1.95857,0.026927,1.85401,-2.05204,0.025431,1.66716,-2.22353,0.076149,1.5869,-2.27294,0.247453,1.58029,-2.17578,0.308933,1.56687,-2.1769,0.127585,1.3849,-1.82614,-0.064558,1.4452,-1.89467,-0.060428,1.47328,-1.94714,-0.061816,1.45303,-2.00658,0.127166,1.39741,-2.09439,0.269163,1.18308,- [...]
+/*234*/{0.130985,1.88486,-1.73919,0.22618,1.82319,-1.88018,-0.009852,1.77913,-1.66297,-0.12109,1.70198,-1.6249,0.011125,1.61976,-1.66822,0.079198,1.56463,-1.67598,0.168724,1.90023,-2.1303,0.23799,1.82464,-1.95726,0.02831,1.85751,-2.05307,0.029766,1.67142,-2.22584,0.08334,1.59173,-2.27359,0.252339,1.58966,-2.17177,0.313214,1.57756,-2.17191,0.130001,1.3883,-1.82664,-0.062761,1.44937,-1.89441,-0.058354,1.47677,-1.94971,-0.058517,1.4559,-2.00726,0.129074,1.39742,-2.09429,0.271443,1.18966,-1. [...]
+/*235*/{0.129085,1.88847,-1.73934,0.22586,1.82898,-1.87847,-0.012785,1.78455,-1.66392,-0.125472,1.70909,-1.62778,0.006839,1.62569,-1.66952,0.075957,1.57055,-1.67591,0.171426,1.90542,-2.12871,0.238201,1.82895,-1.95513,0.029261,1.86072,-2.0537,0.034416,1.67806,-2.22885,0.09097,1.59581,-2.27345,0.255454,1.5976,-2.16756,0.318088,1.5874,-2.16725,0.131493,1.39163,-1.82728,-0.059412,1.45337,-1.8947,-0.055535,1.48025,-1.95051,-0.055848,1.45983,-2.00775,0.131588,1.39739,-2.09422,0.273488,1.19675, [...]
+/*236*/{0.126923,1.89273,-1.73937,0.225744,1.83347,-1.8769,-0.015903,1.78948,-1.66569,-0.128394,1.7149,-1.62993,0.0038,1.63138,-1.66973,0.07221,1.57639,-1.67657,0.172294,1.91118,-2.12718,0.238809,1.83414,-1.95353,0.0306,1.86469,-2.05453,0.039469,1.68236,-2.23073,0.09141,1.60104,-2.27567,0.260159,1.60754,-2.16351,0.321909,1.59758,-2.16188,0.133596,1.39538,-1.82853,-0.05731,1.45826,-1.89453,-0.053751,1.48402,-1.95293,-0.052404,1.46248,-2.00855,0.133601,1.39728,-2.0939,0.275341,1.20286,-1.7 [...]
+/*237*/{0.125661,1.89602,-1.73911,0.226048,1.83664,-1.87542,-0.017656,1.7946,-1.66661,-0.128579,1.72169,-1.6326,0.000844,1.63686,-1.67123,0.069437,1.58192,-1.67666,0.174095,1.91558,-2.12516,0.239511,1.83927,-1.95116,0.031779,1.86778,-2.0547,0.045479,1.68762,-2.23336,0.103537,1.60643,-2.27442,0.262775,1.61582,-2.15955,0.324488,1.6076,-2.15686,0.135677,1.40002,-1.8286,-0.054681,1.46361,-1.89528,-0.050407,1.48738,-1.95401,-0.049681,1.46686,-2.00896,0.135697,1.39754,-2.09375,0.27753,1.20875, [...]
+/*238*/{0.1245,1.89987,-1.73894,0.224982,1.84118,-1.87411,-0.020214,1.80033,-1.66835,-0.133943,1.72607,-1.63554,-0.002299,1.64215,-1.67223,0.066703,1.587,-1.67645,0.176093,1.92087,-2.12389,0.238953,1.84307,-1.95018,0.034401,1.87084,-2.05477,0.051425,1.69265,-2.23582,0.110007,1.61139,-2.2755,0.265031,1.623,-2.15504,0.327275,1.61722,-2.15274,0.137574,1.40391,-1.82913,-0.052891,1.46885,-1.89512,-0.048073,1.49204,-1.95578,-0.04638,1.47002,-2.00949,0.138639,1.39757,-2.09341,0.279989,1.2146,-1 [...]
+/*239*/{0.124135,1.90332,-1.73867,0.225812,1.84602,-1.87267,-0.022352,1.80497,-1.67021,-0.136985,1.731,-1.6381,-0.004806,1.64664,-1.67274,0.064472,1.59163,-1.6763,0.178255,1.92518,-2.12239,0.23986,1.8475,-1.9481,0.035679,1.87379,-2.05525,0.057348,1.6976,-2.23844,0.115864,1.61668,-2.27623,0.266909,1.63061,-2.15168,0.329866,1.62611,-2.14863,0.139512,1.40761,-1.8304,-0.049845,1.47381,-1.89611,-0.045062,1.49529,-1.95722,-0.043289,1.47326,-2.00947,0.1405,1.39765,-2.09265,0.281477,1.22005,-1.7 [...]
+/*240*/{0.123004,1.90654,-1.73843,0.22534,1.84933,-1.87183,-0.023814,1.80992,-1.67152,-0.139956,1.73474,-1.64058,-0.006318,1.65193,-1.67439,0.062482,1.59587,-1.67563,0.179861,1.92931,-2.12077,0.239723,1.85151,-1.94696,0.037568,1.8776,-2.05632,0.062646,1.70226,-2.24023,0.120942,1.62202,-2.27644,0.268638,1.63788,-2.1485,0.331478,1.63402,-2.14513,0.140064,1.4112,-1.82979,-0.047326,1.4788,-1.89586,-0.042279,1.49921,-1.95819,-0.039811,1.47692,-2.01037,0.142967,1.39828,-2.09182,0.283777,1.2246 [...]
+/*241*/{0.122473,1.90913,-1.7383,0.218492,1.8563,-1.87242,-0.025951,1.81408,-1.67332,-0.140928,1.73932,-1.64345,-0.008124,1.65544,-1.67422,0.062088,1.59948,-1.67527,0.181247,1.93321,-2.11916,0.239856,1.85619,-1.94532,0.039041,1.88083,-2.0571,0.068071,1.70706,-2.24172,0.126597,1.62689,-2.27783,0.270865,1.64442,-2.14569,0.333762,1.64089,-2.13635,0.14181,1.41549,-1.8302,-0.04433,1.4838,-1.89573,-0.038931,1.5029,-1.9591,-0.037774,1.48068,-2.01141,0.145707,1.3986,-2.09104,0.28528,1.22929,-1.7 [...]
+/*242*/{0.121862,1.91199,-1.73837,0.21885,1.85926,-1.8712,-0.026931,1.81774,-1.67458,-0.144259,1.743,-1.64564,-0.009449,1.65903,-1.67507,0.06,1.60265,-1.67418,0.183929,1.93715,-2.11778,0.240459,1.85941,-1.94433,0.040939,1.88348,-2.05766,0.072683,1.71156,-2.24295,0.131608,1.63174,-2.27865,0.271612,1.65036,-2.1439,0.335291,1.64738,-2.13462,0.144102,1.41894,-1.83014,-0.041732,1.4885,-1.89608,-0.036292,1.50696,-1.95977,-0.033661,1.48388,-2.01143,0.147678,1.3993,-2.08979,0.285457,1.23237,-1.7 [...]
+/*243*/{0.121992,1.91406,-1.73817,0.22544,1.86042,-1.86864,-0.027482,1.82136,-1.67623,-0.144957,1.74568,-1.64793,-0.011087,1.66181,-1.67542,0.059827,1.60583,-1.67382,0.185711,1.94036,-2.11604,0.241977,1.86185,-1.94257,0.043661,1.88638,-2.05794,0.076977,1.71646,-2.24461,0.13576,1.63598,-2.27952,0.273795,1.65576,-2.14103,0.336469,1.65418,-2.13011,0.14436,1.42193,-1.83084,-0.039346,1.49266,-1.89604,-0.033992,1.51074,-1.96168,-0.03066,1.48684,-2.01159,0.14987,1.40011,-2.0884,0.287205,1.23572 [...]
+/*244*/{0.121541,1.91635,-1.738,0.226907,1.86082,-1.86758,-0.028557,1.82383,-1.67755,-0.145952,1.74793,-1.64941,-0.011402,1.66441,-1.67541,0.058881,1.60801,-1.67265,0.187875,1.94344,-2.115,0.243436,1.86447,-1.94109,0.044642,1.88912,-2.05869,0.081286,1.72024,-2.24583,0.140179,1.63967,-2.2802,0.27445,1.66056,-2.13946,0.337093,1.65974,-2.12784,0.145026,1.42482,-1.83155,-0.037341,1.49665,-1.89715,-0.032576,1.51439,-1.96229,-0.028697,1.49015,-2.01238,0.151163,1.40101,-2.08759,0.287361,1.23814 [...]
+/*245*/{0.122332,1.91849,-1.73755,0.221145,1.86747,-1.86831,-0.028703,1.82647,-1.67818,-0.146981,1.74981,-1.65134,-0.011519,1.66639,-1.67549,0.058209,1.60947,-1.67212,0.18934,1.94623,-2.11337,0.243457,1.86843,-1.94074,0.046967,1.89112,-2.05921,0.085321,1.72536,-2.24661,0.144505,1.64321,-2.28145,0.276131,1.66548,-2.13812,0.339512,1.66336,-2.1241,0.145429,1.42826,-1.83171,-0.034834,1.50132,-1.8972,-0.031189,1.5185,-1.96369,-0.026182,1.49405,-2.01273,0.153519,1.40277,-2.08694,0.287904,1.239 [...]
+/*246*/{0.122272,1.91992,-1.73768,0.22836,1.86508,-1.86577,-0.0288,1.82799,-1.67934,-0.148858,1.7503,-1.65318,-0.011626,1.66835,-1.67585,0.059547,1.61113,-1.6703,0.192076,1.94879,-2.11236,0.245417,1.86918,-1.93896,0.049314,1.89469,-2.05944,0.089198,1.72904,-2.2481,0.147356,1.6472,-2.28282,0.277222,1.66918,-2.13725,0.34034,1.66737,-2.12241,0.146128,1.43143,-1.83148,-0.033279,1.50387,-1.89778,-0.027904,1.52041,-1.96375,-0.024515,1.49694,-2.01376,0.154645,1.40432,-2.08645,0.287058,1.24067,- [...]
+/*247*/{0.122873,1.92159,-1.73715,0.229196,1.86633,-1.86436,-0.029213,1.82964,-1.68044,-0.148124,1.75091,-1.65499,-0.011654,1.66911,-1.67544,0.059527,1.6124,-1.66949,0.193891,1.95098,-2.11182,0.245561,1.87132,-1.93829,0.051451,1.89633,-2.05947,0.092594,1.73237,-2.2486,0.150804,1.65012,-2.28334,0.278871,1.6721,-2.13688,0.339108,1.67093,-2.12333,0.147675,1.43487,-1.83089,-0.033559,1.50788,-1.89837,-0.027206,1.52424,-1.96465,-0.022265,1.49939,-2.01407,0.156003,1.4058,-2.08608,0.286973,1.240 [...]
+/*248*/{0.123064,1.9232,-1.73682,0.231876,1.86739,-1.86328,-0.029097,1.83075,-1.68124,-0.147911,1.75104,-1.6554,-0.010565,1.66957,-1.67514,0.061165,1.61274,-1.66746,0.195814,1.95278,-2.11021,0.247955,1.87315,-1.93671,0.053423,1.89909,-2.05963,0.096572,1.73571,-2.24946,0.153603,1.65278,-2.28492,0.279026,1.67321,-2.13603,0.339863,1.67197,-2.11932,0.15018,1.43738,-1.83104,-0.032017,1.5104,-1.89807,-0.02575,1.52769,-1.96472,-0.021398,1.50289,-2.01425,0.15675,1.40805,-2.08576,0.284195,1.24059 [...]
+/*249*/{0.123811,1.92455,-1.73628,0.231839,1.86807,-1.8627,-0.028073,1.83099,-1.68093,-0.147738,1.75085,-1.65637,-0.009596,1.66938,-1.6748,0.062011,1.61282,-1.66633,0.198105,1.9542,-2.10929,0.249567,1.87475,-1.93548,0.05567,1.90104,-2.05951,0.098695,1.73871,-2.25032,0.15568,1.65515,-2.28586,0.280024,1.6742,-2.13567,0.342014,1.67258,-2.11496,0.151012,1.44029,-1.83088,-0.030396,1.51254,-1.89919,-0.025867,1.53071,-1.96535,-0.019356,1.50597,-2.01401,0.157455,1.40967,-2.08521,0.28222,1.23896, [...]
+/*250*/{0.124222,1.92525,-1.73576,0.234446,1.86922,-1.86121,-0.027727,1.83106,-1.6811,-0.147241,1.75015,-1.65716,-0.008522,1.66954,-1.67487,0.063506,1.61216,-1.66481,0.199779,1.95549,-2.10874,0.25038,1.87577,-1.93521,0.057915,1.90244,-2.05944,0.101408,1.7412,-2.25078,0.15725,1.65705,-2.28673,0.280925,1.67497,-2.13519,0.3411,1.67354,-2.11759,0.150153,1.44269,-1.83093,-0.031064,1.51558,-1.90009,-0.024978,1.53306,-1.96498,-0.018473,1.50796,-2.01509,0.158272,1.41246,-2.08498,0.280543,1.23586 [...]
+/*251*/{0.124772,1.92621,-1.73504,0.234707,1.87015,-1.86068,-0.027117,1.8309,-1.68082,-0.145975,1.74835,-1.65765,-0.007075,1.66851,-1.674,0.065075,1.6115,-1.66277,0.202002,1.9564,-2.10767,0.251921,1.87804,-1.93359,0.05939,1.90511,-2.05875,0.103557,1.7435,-2.25124,0.158542,1.65856,-2.28717,0.281442,1.67535,-2.13485,0.341813,1.6725,-2.11714,0.151457,1.44335,-1.83108,-0.030209,1.51672,-1.89956,-0.024128,1.53538,-1.9653,-0.018459,1.51018,-2.01421,0.15876,1.41462,-2.08491,0.279032,1.23166,-1. [...]
+/*252*/{0.126261,1.92676,-1.73416,0.235126,1.87312,-1.85996,-0.025555,1.82951,-1.68043,-0.144646,1.74619,-1.65775,-0.004416,1.66749,-1.67327,0.067935,1.60998,-1.66063,0.204331,1.95699,-2.10683,0.253067,1.87856,-1.93306,0.061497,1.90669,-2.05878,0.104879,1.74516,-2.25142,0.159126,1.65989,-2.28802,0.283629,1.67531,-2.13534,0.342405,1.67063,-2.11589,0.153595,1.44748,-1.8323,-0.029744,1.51874,-1.89958,-0.024409,1.53771,-1.96549,-0.017933,1.51208,-2.01511,0.15956,1.41654,-2.08471,0.275036,1.2 [...]
+/*253*/{0.126906,1.92723,-1.7334,0.236328,1.87274,-1.85875,-0.024645,1.82849,-1.6802,-0.141924,1.74475,-1.65705,-0.002744,1.66499,-1.67187,0.069157,1.60763,-1.65787,0.20552,1.95721,-2.10618,0.254456,1.87919,-1.93252,0.063078,1.90786,-2.05875,0.106828,1.74621,-2.25133,0.159304,1.66035,-2.28792,0.282731,1.67276,-2.13439,0.343126,1.66887,-2.1183,0.153343,1.44805,-1.83118,-0.029713,1.51957,-1.90004,-0.023856,1.53892,-1.96507,-0.017502,1.51324,-2.01437,0.16053,1.41841,-2.08461,0.270895,1.2231 [...]
+/*254*/{0.127647,1.92744,-1.73255,0.2369,1.87423,-1.85847,-0.023275,1.82658,-1.67916,-0.140703,1.74186,-1.65656,-0.00022,1.66298,-1.67038,0.072606,1.60569,-1.65646,0.208411,1.95745,-2.1056,0.255646,1.87935,-1.93169,0.064339,1.90921,-2.05807,0.107964,1.74721,-2.25146,0.158624,1.66106,-2.28812,0.28329,1.67127,-2.13478,0.343807,1.66703,-2.1185,0.151858,1.44812,-1.832,-0.028931,1.52008,-1.89971,-0.024382,1.54037,-1.96445,-0.017796,1.5148,-2.01493,0.161067,1.42034,-2.08453,0.26805,1.21654,-1. [...]
+/*255*/{0.129084,1.92741,-1.73152,0.238863,1.87384,-1.85788,-0.021659,1.82475,-1.67866,-0.139432,1.7386,-1.65668,0.002192,1.66035,-1.66875,0.074785,1.60308,-1.65397,0.210489,1.95679,-2.1049,0.256143,1.87979,-1.93102,0.06555,1.91061,-2.05828,0.108216,1.74759,-2.2509,0.158138,1.66081,-2.28776,0.282919,1.6682,-2.13499,0.343816,1.66251,-2.11833,0.15071,1.44825,-1.83233,-0.028939,1.51988,-1.89993,-0.02403,1.54075,-1.96439,-0.017348,1.5155,-2.01492,0.162081,1.42117,-2.08448,0.264678,1.21022,-1 [...]
+/*256*/{0.130213,1.92685,-1.73074,0.239207,1.87406,-1.85733,-0.020361,1.82243,-1.67759,-0.135762,1.73534,-1.65552,0.005266,1.6569,-1.66667,0.077238,1.59929,-1.65136,0.211567,1.95595,-2.10456,0.257194,1.87969,-1.93049,0.067221,1.91144,-2.05743,0.107176,1.74792,-2.25015,0.15664,1.66031,-2.28724,0.283417,1.6657,-2.13558,0.344283,1.65801,-2.11818,0.15098,1.44862,-1.8324,-0.028932,1.5198,-1.90001,-0.02403,1.54075,-1.96439,-0.016782,1.51551,-2.01447,0.162611,1.42342,-2.0843,0.260022,1.2034,-1. [...]
+/*257*/{0.131852,1.92579,-1.72945,0.240104,1.87504,-1.85734,-0.018302,1.81974,-1.6764,-0.133802,1.7313,-1.65423,0.008441,1.65324,-1.66487,0.080698,1.59629,-1.64871,0.21334,1.95501,-2.10378,0.258116,1.87887,-1.929,0.068168,1.91157,-2.05648,0.105283,1.74781,-2.24872,0.15533,1.66012,-2.28692,0.282837,1.66206,-2.13582,0.344234,1.65266,-2.11872,0.153704,1.44881,-1.83252,-0.029764,1.51918,-1.9,-0.024109,1.5405,-1.9644,-0.016914,1.51481,-2.01438,0.163966,1.42369,-2.08449,0.254456,1.19674,-1.768 [...]
+/*258*/{0.133104,1.92491,-1.72913,0.240744,1.87379,-1.85707,-0.016207,1.81676,-1.67491,-0.129521,1.72779,-1.65321,0.011569,1.64982,-1.6625,0.084682,1.5926,-1.64575,0.214808,1.95342,-2.10278,0.258438,1.87832,-1.92881,0.069085,1.9118,-2.0555,0.104083,1.74739,-2.24739,0.152998,1.65854,-2.28601,0.28452,1.65707,-2.13667,0.34353,1.64688,-2.12007,0.154824,1.44894,-1.83258,-0.030059,1.51723,-1.90067,-0.024238,1.53965,-1.96546,-0.017063,1.51362,-2.01425,0.164382,1.42414,-2.0845,0.251274,1.18967,- [...]
+/*259*/{0.134855,1.92327,-1.72788,0.241701,1.87318,-1.85666,-0.01422,1.81414,-1.67436,-0.125469,1.72383,-1.65117,0.015682,1.64551,-1.66017,0.088535,1.58887,-1.64365,0.214832,1.95082,-2.10202,0.258706,1.87721,-1.92863,0.070147,1.91123,-2.05448,0.101153,1.74699,-2.24537,0.149888,1.65777,-2.28464,0.283593,1.65189,-2.13845,0.343609,1.64034,-2.12162,0.152908,1.44804,-1.8327,-0.030019,1.51537,-1.90121,-0.023852,1.53876,-1.96532,-0.016438,1.51233,-2.01414,0.164549,1.42413,-2.08446,0.245269,1.18 [...]
+/*260*/{0.136348,1.92159,-1.72691,0.2422,1.87156,-1.85688,-0.011545,1.81007,-1.67236,-0.12127,1.7193,-1.64925,0.019331,1.64135,-1.6574,0.092076,1.58491,-1.64096,0.21628,1.94923,-2.10112,0.259343,1.87577,-1.92844,0.070196,1.91075,-2.05389,0.098324,1.74671,-2.24354,0.146771,1.65699,-2.28369,0.282839,1.64672,-2.13958,0.343472,1.6325,-2.12236,0.153795,1.44628,-1.83212,-0.029437,1.5132,-1.90028,-0.023842,1.53592,-1.9656,-0.016652,1.51019,-2.01429,0.165579,1.42351,-2.08451,0.242,1.17671,-1.766 [...]
+/*261*/{0.138115,1.91947,-1.72591,0.241631,1.86985,-1.85651,-0.009328,1.80614,-1.67037,-0.117562,1.71411,-1.64729,0.023141,1.63586,-1.65453,0.09633,1.57987,-1.63744,0.216544,1.94687,-2.10089,0.259248,1.87378,-1.92819,0.070669,1.90978,-2.05329,0.09551,1.74371,-2.2412,0.143341,1.65561,-2.2826,0.282477,1.64207,-2.13932,0.342566,1.62556,-2.12511,0.154166,1.44466,-1.83168,-0.030037,1.51033,-1.90098,-0.024092,1.53475,-1.96545,-0.016994,1.5081,-2.01506,0.166498,1.42213,-2.08468,0.238899,1.16937 [...]
+/*262*/{0.139348,1.91676,-1.725,0.242635,1.86823,-1.85628,-0.006252,1.80212,-1.66887,-0.114349,1.70839,-1.64467,0.027622,1.63102,-1.65127,0.1001,1.57607,-1.63566,0.216411,1.94418,-2.10011,0.260071,1.87164,-1.92749,0.070826,1.90891,-2.05236,0.09205,1.74243,-2.23949,0.139606,1.65363,-2.28086,0.281243,1.63539,-2.14026,0.340706,1.61746,-2.12728,0.155454,1.44222,-1.8314,-0.029544,1.50696,-1.9009,-0.023072,1.53172,-1.96548,-0.016635,1.50493,-2.01434,0.166893,1.42132,-2.08421,0.235757,1.16286,- [...]
+/*263*/{0.141152,1.91356,-1.72427,0.242657,1.86538,-1.85577,-0.003689,1.79795,-1.66694,-0.109827,1.70308,-1.64182,0.031857,1.6264,-1.64843,0.10485,1.57133,-1.63226,0.217257,1.94156,-2.0996,0.259252,1.86904,-1.92779,0.071005,1.9068,-2.05086,0.087945,1.74151,-2.23746,0.135479,1.65136,-2.27919,0.279544,1.62906,-2.14409,0.339674,1.609,-2.12956,0.155117,1.43921,-1.83117,-0.029609,1.50426,-1.90069,-0.023257,1.52835,-1.96555,-0.015888,1.50237,-2.01559,0.167987,1.41992,-2.08354,0.232744,1.15768, [...]
+/*264*/{0.142669,1.91026,-1.72334,0.243748,1.86227,-1.85581,-0.000557,1.7929,-1.66402,-0.105296,1.69786,-1.63857,0.036337,1.62094,-1.64463,0.110033,1.5669,-1.6301,0.217001,1.93827,-2.09992,0.25979,1.86624,-1.92762,0.071111,1.90443,-2.05,0.084507,1.73935,-2.23509,0.131175,1.64934,-2.27802,0.277417,1.62289,-2.14545,0.337701,1.60104,-2.13218,0.154139,1.43592,-1.83062,-0.02933,1.50092,-1.90029,-0.023185,1.52496,-1.96552,-0.016642,1.49962,-2.01553,0.169408,1.41749,-2.08358,0.230895,1.1534,-1. [...]
+/*265*/{0.1444,1.90662,-1.72224,0.24461,1.85772,-1.85566,0.002329,1.78716,-1.66154,-0.100473,1.6914,-1.63486,0.041496,1.61552,-1.64095,0.115328,1.56226,-1.62672,0.215818,1.93467,-2.09985,0.260207,1.86342,-1.92766,0.070228,1.90144,-2.04982,0.07983,1.7372,-2.23324,0.126674,1.64598,-2.27631,0.273962,1.61486,-2.14696,0.333852,1.5926,-2.13577,0.156252,1.43262,-1.82968,-0.029389,1.49713,-1.89967,-0.023032,1.52111,-1.96535,-0.015749,1.49565,-2.01541,0.170214,1.41559,-2.08305,0.228247,1.15051,-1 [...]
+/*266*/{0.145838,1.9027,-1.72139,0.245236,1.85459,-1.85556,0.005194,1.78208,-1.65907,-0.096819,1.68513,-1.63072,0.046098,1.61016,-1.63808,0.120196,1.55686,-1.62379,0.21488,1.93108,-2.10019,0.259896,1.85949,-1.92747,0.069938,1.89908,-2.04742,0.076737,1.73443,-2.23162,0.121885,1.64324,-2.27489,0.271729,1.60696,-2.14854,0.330615,1.58287,-2.13842,0.153304,1.42789,-1.82946,-0.030536,1.49334,-1.90009,-0.022582,1.51807,-1.96507,-0.015984,1.49118,-2.01549,0.170294,1.41244,-2.08182,0.227233,1.146 [...]
+/*267*/{0.147584,1.89855,-1.72044,0.246551,1.85088,-1.85592,0.008333,1.77605,-1.65657,-0.09268,1.67851,-1.6269,0.050077,1.604,-1.6347,0.126064,1.55256,-1.62046,0.214798,1.92735,-2.10081,0.260293,1.85589,-1.92765,0.069681,1.89506,-2.04665,0.071915,1.73172,-2.22932,0.117043,1.63954,-2.2737,0.268017,1.59924,-2.1505,0.327816,1.57285,-2.14185,0.15406,1.42447,-1.8278,-0.030196,1.48932,-1.90033,-0.023104,1.51449,-1.96485,-0.016502,1.48724,-2.01561,0.1709,1.40949,-2.08127,0.2249,1.14361,-1.75648 [...]
+/*268*/{0.148603,1.89407,-1.7197,0.24521,1.84832,-1.85612,0.011867,1.7701,-1.65329,-0.087219,1.67165,-1.62281,0.056292,1.59842,-1.63064,0.132038,1.54771,-1.61874,0.21299,1.92283,-2.10126,0.260299,1.85216,-1.92776,0.069059,1.89028,-2.04596,0.068544,1.72791,-2.22788,0.11218,1.63589,-2.27224,0.265666,1.59187,-2.15255,0.323946,1.56287,-2.14466,0.153759,1.42083,-1.82694,-0.030369,1.48497,-1.89995,-0.02329,1.51043,-1.9648,-0.01561,1.48194,-2.0163,0.171334,1.40627,-2.08054,0.223951,1.14098,-1.7 [...]
+/*269*/{0.150102,1.88894,-1.71918,0.246152,1.84273,-1.85638,0.014845,1.76341,-1.65088,-0.083295,1.66443,-1.61811,0.062327,1.59254,-1.62694,0.137639,1.54282,-1.6147,0.212248,1.91835,-2.10252,0.260897,1.84845,-1.92789,0.067987,1.88576,-2.04479,0.064202,1.72445,-2.22658,0.106484,1.63145,-2.27133,0.261137,1.58182,-2.15447,0.319579,1.55187,-2.14846,0.15314,1.41676,-1.82603,-0.030747,1.48071,-1.90024,-0.022596,1.5059,-1.9642,-0.015453,1.47738,-2.01627,0.17204,1.40313,-2.07996,0.222335,1.13653, [...]
+/*270*/{0.151125,1.88362,-1.71837,0.246101,1.83916,-1.8564,0.018295,1.75628,-1.64773,-0.078578,1.65642,-1.61346,0.066386,1.58633,-1.62276,0.143911,1.5385,-1.61196,0.210251,1.91361,-2.10318,0.260645,1.84402,-1.92881,0.066631,1.88092,-2.04386,0.059385,1.72,-2.22525,0.101525,1.62715,-2.27006,0.257347,1.57398,-2.15721,0.314861,1.54081,-2.15239,0.154952,1.41315,-1.82438,-0.030812,1.47602,-1.9008,-0.023471,1.50165,-1.96358,-0.015723,1.47248,-2.01618,0.172016,1.39956,-2.08025,0.220156,1.13328,- [...]
+/*271*/{0.151802,1.87843,-1.71819,0.246038,1.83402,-1.85633,0.022241,1.74912,-1.64517,-0.073151,1.64923,-1.6081,0.072551,1.58048,-1.61918,0.150677,1.53414,-1.60984,0.208972,1.90873,-2.10416,0.260478,1.83951,-1.92939,0.06576,1.87609,-2.04327,0.054935,1.71632,-2.22426,0.096527,1.62245,-2.26886,0.252657,1.56333,-2.15954,0.309512,1.5291,-2.15649,0.154973,1.40953,-1.82376,-0.030903,1.47023,-1.9007,-0.024229,1.4976,-1.96352,-0.016075,1.46714,-2.01682,0.172109,1.39525,-2.0788,0.22021,1.12849,-1 [...]
+/*272*/{0.153368,1.87291,-1.71765,0.247348,1.82836,-1.85668,0.026761,1.74184,-1.64131,-0.068646,1.64175,-1.60305,0.078347,1.57436,-1.61464,0.157081,1.52962,-1.60681,0.207862,1.90367,-2.10469,0.261649,1.8347,-1.92991,0.064925,1.86965,-2.04237,0.051781,1.71153,-2.22354,0.090509,1.6175,-2.26798,0.247414,1.55384,-2.1628,0.304065,1.51739,-2.16009,0.153792,1.40565,-1.82214,-0.031599,1.46518,-1.90088,-0.023928,1.49154,-1.96226,-0.015606,1.46178,-2.01566,0.172233,1.39196,-2.07843,0.218003,1.1237 [...]
+/*273*/{0.154682,1.86766,-1.71668,0.247525,1.82476,-1.85729,0.030985,1.73442,-1.63846,-0.062539,1.6342,-1.59829,0.084784,1.56883,-1.61098,0.164019,1.52582,-1.60418,0.205774,1.89909,-2.10585,0.260801,1.83002,-1.93101,0.063706,1.86453,-2.04197,0.04608,1.70688,-2.22239,0.084855,1.61275,-2.26701,0.242273,1.54349,-2.16522,0.29789,1.50571,-2.16427,0.15321,1.40096,-1.82072,-0.031692,1.45966,-1.90093,-0.024892,1.48629,-1.96177,-0.016355,1.4558,-2.01629,0.173641,1.38734,-2.0757,0.216072,1.1184,-1 [...]
+/*274*/{0.156371,1.8625,-1.71571,0.247628,1.81748,-1.85713,0.035338,1.72665,-1.63551,-0.057618,1.6261,-1.59274,0.091545,1.56396,-1.60726,0.171216,1.5225,-1.60163,0.20411,1.89454,-2.10639,0.261621,1.82464,-1.93173,0.062149,1.85849,-2.04133,0.042685,1.70206,-2.22155,0.079736,1.60813,-2.26661,0.236516,1.53279,-2.16791,0.290724,1.49297,-2.16826,0.152677,1.39686,-1.82002,-0.033587,1.45495,-1.90088,-0.025501,1.48041,-1.9611,-0.017335,1.45019,-2.01624,0.173596,1.38282,-2.07516,0.213127,1.11272, [...]
+/*275*/{0.157782,1.85742,-1.71529,0.249516,1.81245,-1.85763,0.03981,1.71929,-1.63236,-0.050042,1.61842,-1.58706,0.09806,1.55947,-1.60491,0.179223,1.51953,-1.59888,0.203106,1.88959,-2.10703,0.261809,1.81976,-1.93275,0.060456,1.85303,-2.03984,0.039427,1.69757,-2.22109,0.073989,1.60355,-2.26603,0.230779,1.52209,-2.17133,0.283326,1.48058,-2.17284,0.151252,1.39273,-1.81831,-0.035861,1.45039,-1.90158,-0.026742,1.47505,-1.96154,-0.018061,1.44508,-2.0169,0.171084,1.3781,-2.07535,0.210806,1.10863 [...]
+/*276*/{0.159555,1.85278,-1.71419,0.250881,1.80677,-1.85801,0.045155,1.71156,-1.62901,-0.044154,1.61117,-1.58213,0.105994,1.5551,-1.60189,0.187257,1.51671,-1.59726,0.20072,1.88563,-2.10804,0.263074,1.81451,-1.93348,0.059324,1.84859,-2.03869,0.033948,1.6928,-2.2206,0.067521,1.59917,-2.2657,0.224315,1.51092,-2.17519,0.275514,1.46804,-2.17683,0.14863,1.39011,-1.81923,-0.038116,1.44612,-1.90186,-0.028431,1.46904,-1.96308,-0.020548,1.44,-2.01744,0.17406,1.37401,-2.07252,0.205519,1.10668,-1.74 [...]
+/*277*/{0.161818,1.84867,-1.71347,0.251712,1.80324,-1.85903,0.051043,1.70432,-1.62648,-0.035946,1.60413,-1.57639,0.113885,1.55107,-1.59915,0.195149,1.51449,-1.5953,0.199641,1.88238,-2.10979,0.264234,1.80977,-1.93494,0.058238,1.84589,-2.0369,0.030498,1.68847,-2.21994,0.061718,1.59489,-2.26517,0.217874,1.50064,-2.17953,0.267692,1.4562,-2.18086,0.145448,1.38818,-1.81879,-0.041516,1.44219,-1.90342,-0.031076,1.46405,-1.96374,-0.022982,1.43478,-2.01886,0.17256,1.36966,-2.07102,0.202388,1.10524 [...]
+/*278*/{0.16398,1.84478,-1.71221,0.252823,1.79898,-1.8598,0.057062,1.6985,-1.62418,-0.027904,1.59718,-1.5717,0.121912,1.54793,-1.5966,0.203511,1.51274,-1.59407,0.198046,1.87969,-2.11175,0.265159,1.80554,-1.93638,0.057234,1.84404,-2.03393,0.027524,1.68489,-2.21936,0.056094,1.59102,-2.2634,0.210585,1.49115,-2.18326,0.258155,1.44492,-2.18486,0.142924,1.38458,-1.81744,-0.044619,1.43878,-1.90396,-0.035076,1.45987,-1.96538,-0.025523,1.43034,-2.01895,0.171891,1.36564,-2.06895,0.20149,1.10618,-1 [...]
+/*279*/{0.165277,1.84156,-1.71156,0.253331,1.7955,-1.86039,0.063364,1.69367,-1.62278,-0.019857,1.59228,-1.56686,0.131163,1.54631,-1.59556,0.213094,1.51239,-1.59293,0.197397,1.87635,-2.11309,0.265226,1.80218,-1.93752,0.056423,1.84299,-2.0314,0.023902,1.68218,-2.21824,0.051542,1.58815,-2.2615,0.202791,1.48214,-2.18532,0.24885,1.43524,-2.18792,0.140163,1.38319,-1.81511,-0.048104,1.43562,-1.90714,-0.036762,1.45574,-1.96559,-0.028968,1.42416,-2.0207,0.16966,1.36167,-2.06768,0.198024,1.10693,- [...]
+/*280*/{0.166467,1.83901,-1.7113,0.253811,1.79177,-1.86175,0.069544,1.69089,-1.62182,-0.012011,1.58736,-1.56282,0.139437,1.54459,-1.59488,0.222228,1.51186,-1.59221,0.198102,1.87456,-2.11392,0.266061,1.79953,-1.93896,0.056899,1.8427,-2.03111,0.020526,1.68105,-2.21579,0.045876,1.58646,-2.25972,0.195001,1.47582,-2.18641,0.238905,1.42644,-2.19076,0.137257,1.3832,-1.81246,-0.051123,1.43416,-1.90849,-0.037932,1.45426,-1.9653,-0.031013,1.41948,-2.02224,0.168403,1.3586,-2.06621,0.196066,1.10785, [...]
+/*281*/{0.16814,1.83812,-1.71138,0.254317,1.79035,-1.86281,0.074476,1.68858,-1.6207,-0.000893,1.58466,-1.55942,0.148559,1.54357,-1.59421,0.231731,1.51268,-1.59173,0.198112,1.87267,-2.11357,0.265724,1.7986,-1.94019,0.057029,1.84265,-2.03101,0.018029,1.68192,-2.2132,0.039983,1.58726,-2.25795,0.187097,1.47177,-2.18595,0.22901,1.41983,-2.19339,0.134322,1.38309,-1.80922,-0.051674,1.43275,-1.90988,-0.038832,1.45399,-1.96601,-0.033328,1.41651,-2.02434,0.166165,1.35647,-2.06567,0.192285,1.1074,- [...]
+/*282*/{0.170509,1.83697,-1.7123,0.253995,1.79032,-1.86329,0.079778,1.68749,-1.62036,0.007387,1.58285,-1.55658,0.157714,1.54372,-1.59381,0.241968,1.51422,-1.59177,0.199144,1.87093,-2.11388,0.265246,1.79685,-1.94052,0.057379,1.84265,-2.03131,0.012663,1.68415,-2.20902,0.034334,1.58925,-2.25547,0.177889,1.46739,-2.18551,0.218726,1.41491,-2.19534,0.131692,1.38252,-1.80707,-0.052493,1.43342,-1.91034,-0.040778,1.45472,-1.96618,-0.035408,1.41668,-2.02463,0.163844,1.35476,-2.06507,0.188264,1.105 [...]
+/*283*/{0.172733,1.83656,-1.71311,0.25375,1.78891,-1.86439,0.085133,1.68699,-1.61911,0.015864,1.58123,-1.55436,0.167181,1.54459,-1.59332,0.251957,1.51689,-1.59225,0.199538,1.86986,-2.1136,0.264825,1.79579,-1.9412,0.057749,1.84293,-2.03238,0.00939,1.68667,-2.205,0.028407,1.59212,-2.25346,0.16958,1.46568,-2.18575,0.208523,1.41107,-2.19613,0.130019,1.38258,-1.80433,-0.053338,1.43344,-1.91048,-0.041199,1.4547,-1.96535,-0.035694,1.417,-2.02433,0.161574,1.35373,-2.06469,0.184212,1.10316,-1.735 [...]
+/*284*/{0.174186,1.83704,-1.71445,0.252782,1.7876,-1.8653,0.088865,1.68686,-1.6188,0.025128,1.58165,-1.55223,0.176027,1.54616,-1.59265,0.261816,1.52026,-1.59298,0.199602,1.869,-2.11418,0.263967,1.79552,-1.94229,0.058212,1.84337,-2.03337,0.004465,1.69074,-2.20099,0.02179,1.59532,-2.25208,0.160754,1.46579,-2.18688,0.198137,1.40923,-2.19832,0.128603,1.38329,-1.80248,-0.054099,1.43342,-1.91036,-0.042037,1.45513,-1.96521,-0.036981,1.41863,-2.02426,0.160247,1.35439,-2.06434,0.177617,1.0986,-1. [...]
+/*285*/{0.175921,1.83789,-1.71524,0.254012,1.78708,-1.86591,0.093933,1.68802,-1.61742,0.034395,1.58148,-1.54965,0.185687,1.54849,-1.59287,0.271109,1.52397,-1.59453,0.199134,1.86895,-2.1145,0.264406,1.79564,-1.94245,0.057953,1.8441,-2.03392,0.000522,1.69671,-2.19716,0.014626,1.59948,-2.25079,0.152715,1.46642,-2.18701,0.188182,1.40907,-2.1984,0.126274,1.38329,-1.8007,-0.054783,1.43407,-1.9095,-0.042601,1.45625,-1.96364,-0.037894,1.42003,-2.02291,0.159296,1.35539,-2.06401,0.17429,1.09547,-1 [...]
+/*286*/{0.177766,1.83822,-1.71552,0.253652,1.78758,-1.86671,0.097991,1.68977,-1.61665,0.041784,1.58213,-1.54763,0.194604,1.55167,-1.59305,0.28094,1.52945,-1.59607,0.198307,1.86908,-2.1151,0.264075,1.79497,-1.94341,0.057567,1.84551,-2.03431,-0.0041,1.702,-2.1938,0.007535,1.60401,-2.24923,0.14443,1.46809,-2.18746,0.178822,1.41052,-2.19926,0.124897,1.38416,-1.79996,-0.055962,1.43444,-1.90818,-0.043807,1.45651,-1.96356,-0.038554,1.42203,-2.02114,0.157655,1.35728,-2.0644,0.16931,1.09175,-1.73 [...]
+/*287*/{0.179499,1.84074,-1.71633,0.253867,1.78883,-1.86799,0.101679,1.69232,-1.61627,0.048695,1.5832,-1.54632,0.203073,1.55561,-1.59284,0.290246,1.53453,-1.59855,0.197237,1.86975,-2.11552,0.26312,1.7952,-1.94406,0.057133,1.84705,-2.03445,-0.007385,1.70792,-2.19069,0.000663,1.60988,-2.24826,0.135624,1.47082,-2.18848,0.169141,1.4135,-2.19961,0.12433,1.38494,-1.79885,-0.056048,1.43466,-1.90783,-0.044706,1.45755,-1.962,-0.040133,1.42336,-2.01969,0.157685,1.35867,-2.06497,0.165206,1.0887,-1. [...]
+/*288*/{0.182057,1.84296,-1.71689,0.254006,1.78951,-1.86932,0.105333,1.69495,-1.61566,0.057819,1.58543,-1.54513,0.210971,1.55955,-1.59316,0.298972,1.54102,-1.60108,0.196617,1.87145,-2.11616,0.262676,1.79598,-1.94502,0.056182,1.84944,-2.03399,-0.010903,1.71224,-2.1875,-0.006817,1.61581,-2.247,0.127427,1.47487,-2.18836,0.160228,1.41757,-2.19952,0.123408,1.38624,-1.79828,-0.057097,1.43584,-1.90789,-0.044984,1.45809,-1.96088,-0.042138,1.42523,-2.01759,0.157508,1.36139,-2.06572,0.158675,1.086 [...]
+/*289*/{0.184073,1.84598,-1.71831,0.254226,1.79146,-1.87042,0.108925,1.69813,-1.61605,0.064856,1.58732,-1.543,0.218318,1.56411,-1.59437,0.306911,1.5473,-1.60309,0.194522,1.87363,-2.11719,0.262527,1.79805,-1.94731,0.055689,1.85083,-2.03418,-0.017347,1.72052,-2.18686,-0.01414,1.62213,-2.24626,0.120111,1.47977,-2.18922,0.152229,1.42269,-2.19886,0.123951,1.38735,-1.79814,-0.056667,1.43744,-1.90656,-0.0455,1.45957,-1.96027,-0.042883,1.42639,-2.0168,0.155243,1.36449,-2.0662,0.151949,1.08441,-1 [...]
+/*290*/{0.186391,1.84918,-1.71885,0.254257,1.79398,-1.8717,0.113199,1.70151,-1.61574,0.071652,1.58993,-1.54202,0.22648,1.5697,-1.59454,0.314383,1.55435,-1.60696,0.19288,1.87598,-2.11789,0.261267,1.79975,-1.9485,0.054555,1.85362,-2.03257,-0.022375,1.7254,-2.1855,-0.021294,1.62857,-2.24573,0.111959,1.48587,-2.18833,0.143316,1.42882,-2.19867,0.125189,1.38808,-1.79883,-0.056343,1.43917,-1.90499,-0.045191,1.46119,-1.95923,-0.043151,1.42837,-2.01566,0.154533,1.36698,-2.06682,0.149259,1.0836,-1 [...]
+/*291*/{0.188919,1.85269,-1.72005,0.255636,1.79589,-1.8732,0.117832,1.70534,-1.61573,0.078834,1.59293,-1.54113,0.233098,1.57568,-1.59668,0.321926,1.5616,-1.6098,0.189121,1.87901,-2.11835,0.261026,1.80218,-1.94992,0.052649,1.85584,-2.03172,-0.028649,1.73141,-2.18494,-0.028314,1.63534,-2.24517,0.103948,1.49217,-2.18753,0.135388,1.43502,-2.1977,0.126499,1.38908,-1.79908,-0.055718,1.44093,-1.90454,-0.045847,1.46313,-1.95851,-0.043176,1.42957,-2.0145,0.15368,1.37042,-2.06808,0.137349,1.08238, [...]
+/*292*/{0.191765,1.85645,-1.72112,0.254408,1.79938,-1.87443,0.122954,1.70893,-1.61556,0.085934,1.59654,-1.54004,0.238928,1.58176,-1.59886,0.327871,1.56936,-1.61347,0.187266,1.88273,-2.11909,0.260811,1.80455,-1.95191,0.051452,1.85854,-2.03043,-0.034938,1.7395,-2.18595,-0.035847,1.64225,-2.24474,0.096229,1.49942,-2.18683,0.127571,1.44189,-2.19669,0.127065,1.3903,-1.79982,-0.055346,1.44373,-1.90281,-0.045057,1.4656,-1.95699,-0.043522,1.43192,-2.01385,0.153671,1.37392,-2.06762,0.135993,1.082 [...]
+/*293*/{0.19448,1.8604,-1.72225,0.257356,1.80342,-1.87706,0.127625,1.71245,-1.61561,0.093576,1.60027,-1.53922,0.245669,1.5886,-1.60051,0.333628,1.57719,-1.61697,0.18274,1.88629,-2.11976,0.260152,1.80768,-1.95372,0.049614,1.86045,-2.02843,-0.041281,1.74598,-2.18498,-0.042665,1.64856,-2.24406,0.089181,1.50667,-2.18602,0.120672,1.45011,-2.19597,0.128035,1.39114,-1.80037,-0.054589,1.44642,-1.90167,-0.044509,1.46827,-1.95501,-0.04287,1.43433,-2.01237,0.153001,1.37725,-2.06819,0.126265,1.08063 [...]
+/*294*/{0.196834,1.86488,-1.72369,0.256664,1.80608,-1.87891,0.132796,1.71654,-1.61546,0.099263,1.60447,-1.53854,0.251487,1.59488,-1.60291,0.339977,1.58533,-1.62138,0.179317,1.8907,-2.12023,0.25975,1.81141,-1.95579,0.048021,1.86385,-2.02707,-0.046281,1.75252,-2.18457,-0.049434,1.65565,-2.24339,0.082571,1.51462,-2.18518,0.113566,1.45772,-2.19503,0.129931,1.39165,-1.80037,-0.05228,1.44938,-1.89995,-0.043854,1.47058,-1.95352,-0.042551,1.43694,-2.01123,0.153826,1.38128,-2.06827,0.117926,1.079 [...]
+/*295*/{0.199853,1.86958,-1.72492,0.256852,1.81038,-1.88084,0.138135,1.7211,-1.61551,0.106871,1.60858,-1.53813,0.256277,1.60162,-1.60563,0.344231,1.59346,-1.62545,0.17498,1.89449,-2.12011,0.259247,1.81492,-1.95794,0.046381,1.867,-2.02569,-0.051697,1.75953,-2.18396,-0.055494,1.6624,-2.24264,0.075628,1.5223,-2.18391,0.107005,1.46565,-2.19413,0.131227,1.39286,-1.80036,-0.052023,1.45238,-1.89792,-0.042829,1.47344,-1.95215,-0.041603,1.44094,-2.01015,0.153856,1.38481,-2.06797,0.112051,1.07956, [...]
+/*296*/{0.202122,1.87428,-1.72646,0.258858,1.81515,-1.88352,0.144299,1.72505,-1.61555,0.113771,1.61294,-1.53696,0.261153,1.60803,-1.60799,0.349833,1.60152,-1.62925,0.171291,1.89917,-2.12052,0.25959,1.81954,-1.95992,0.045193,1.86944,-2.02297,-0.056912,1.76639,-2.18283,-0.062085,1.66862,-2.24158,0.069294,1.53053,-2.18289,0.101253,1.47419,-2.19322,0.132462,1.39432,-1.8004,-0.050267,1.45553,-1.89601,-0.042196,1.47701,-1.95002,-0.040626,1.44395,-2.00885,0.154147,1.38928,-2.06741,0.105237,1.08 [...]
+/*297*/{0.204888,1.87906,-1.72797,0.25968,1.81916,-1.88518,0.150951,1.72938,-1.6164,0.12027,1.61833,-1.53706,0.266537,1.61591,-1.61083,0.353856,1.60975,-1.63352,0.167914,1.90407,-2.1206,0.259181,1.82358,-1.96204,0.04329,1.87271,-2.02153,-0.060677,1.773,-2.1821,-0.068281,1.67572,-2.23981,0.062472,1.53896,-2.18174,0.095507,1.4826,-2.19251,0.134724,1.39536,-1.80023,-0.048724,1.45838,-1.89435,-0.040992,1.47985,-1.94875,-0.040744,1.44866,-2.00766,0.154833,1.39365,-2.06712,0.103342,1.08219,-1. [...]
+/*298*/{0.207574,1.88424,-1.72954,0.259779,1.82377,-1.88712,0.156352,1.73459,-1.61611,0.125661,1.62275,-1.53714,0.270437,1.62196,-1.61298,0.35743,1.61766,-1.63771,0.165088,1.90895,-2.12026,0.258333,1.82852,-1.96432,0.041745,1.87621,-2.01901,-0.065647,1.77928,-2.18131,-0.073793,1.68222,-2.23834,0.056897,1.54717,-2.18003,0.090238,1.49099,-2.19142,0.136095,1.3962,-1.79946,-0.046874,1.46281,-1.89332,-0.039917,1.48348,-1.94615,-0.039707,1.4521,-2.00641,0.155418,1.39813,-2.06633,0.092604,1.081 [...]
+/*299*/{0.209462,1.88945,-1.73082,0.260483,1.82844,-1.88948,0.162716,1.73917,-1.61731,0.132756,1.6281,-1.5361,0.274388,1.62953,-1.61629,0.36083,1.62563,-1.64212,0.161696,1.91355,-2.12032,0.258429,1.83316,-1.96619,0.040493,1.87968,-2.0172,-0.069735,1.78516,-2.18005,-0.079672,1.68949,-2.23719,0.051063,1.55459,-2.17832,0.085435,1.4991,-2.19078,0.138201,1.39801,-1.79929,-0.045261,1.46599,-1.89158,-0.038228,1.48731,-1.94435,-0.038263,1.45737,-2.00594,0.156399,1.40282,-2.06531,0.088197,1.08327 [...]
+/*300*/{0.211821,1.89428,-1.73259,0.261272,1.8337,-1.89124,0.16784,1.74425,-1.61825,0.138105,1.63325,-1.53589,0.278014,1.63655,-1.61868,0.364233,1.63351,-1.64665,0.158382,1.91804,-2.12065,0.257657,1.83855,-1.96829,0.038943,1.88407,-2.01551,-0.073793,1.79197,-2.17955,-0.084689,1.696,-2.23583,0.047164,1.56251,-2.17604,0.080455,1.50721,-2.19044,0.139764,1.39916,-1.79923,-0.043819,1.46912,-1.89059,-0.037218,1.49084,-1.94274,-0.03746,1.46215,-2.00508,0.157091,1.40788,-2.06446,0.083123,1.08438 [...]
+/*301*/{0.213071,1.90003,-1.73477,0.261837,1.83817,-1.89326,0.173174,1.74937,-1.61898,0.143688,1.63899,-1.53592,0.281108,1.64357,-1.6213,0.367048,1.64091,-1.6508,0.156584,1.9226,-2.12051,0.2573,1.84286,-1.97004,0.037629,1.88801,-2.01433,-0.077259,1.79812,-2.17734,-0.089481,1.70318,-2.23347,0.041314,1.57014,-2.17515,0.076099,1.51504,-2.18979,0.142143,1.40119,-1.79871,-0.041667,1.47228,-1.88923,-0.035503,1.49499,-1.94006,-0.035554,1.46772,-2.00435,0.15874,1.41307,-2.06372,0.07754,1.08733,- [...]
+/*302*/{0.214992,1.90515,-1.73686,0.262014,1.84346,-1.89519,0.177891,1.75521,-1.62003,0.14901,1.6443,-1.5357,0.284072,1.65028,-1.62338,0.369511,1.64832,-1.65529,0.153661,1.927,-2.1205,0.256854,1.84774,-1.97184,0.036728,1.89157,-2.01311,-0.080645,1.80359,-2.17508,-0.094492,1.7094,-2.23186,0.03764,1.57731,-2.17339,0.072122,1.52297,-2.18928,0.14405,1.40214,-1.79894,-0.039191,1.47587,-1.88827,-0.034003,1.4991,-1.93881,-0.033867,1.47278,-2.0036,0.159324,1.41775,-2.06295,0.071179,1.09006,-1.76 [...]
+/*303*/{0.21659,1.9107,-1.73842,0.263322,1.84813,-1.89718,0.182518,1.76044,-1.62105,0.154353,1.64983,-1.53571,0.286164,1.65711,-1.62709,0.370859,1.65488,-1.65844,0.15168,1.93103,-2.12073,0.256687,1.85266,-1.97347,0.035833,1.89556,-2.01275,-0.084655,1.80934,-2.17507,-0.098486,1.71594,-2.22912,0.03279,1.58472,-2.17228,0.068631,1.52973,-2.18898,0.146069,1.40364,-1.7997,-0.037272,1.47954,-1.88675,-0.032897,1.5033,-1.93692,-0.032247,1.47791,-2.00294,0.160348,1.42314,-2.06258,0.066972,1.09177, [...]
+/*304*/{0.218954,1.91549,-1.73964,0.262555,1.85186,-1.89945,0.186263,1.76572,-1.62151,0.15885,1.65597,-1.5355,0.288723,1.66321,-1.62907,0.372988,1.66203,-1.66359,0.149612,1.93465,-2.12077,0.25612,1.85717,-1.97551,0.036143,1.89773,-2.01086,-0.087432,1.81478,-2.17273,-0.102756,1.72186,-2.22764,0.029136,1.5915,-2.17115,0.066568,1.53622,-2.18788,0.148399,1.40503,-1.80046,-0.034558,1.48256,-1.88669,-0.030392,1.50737,-1.93545,-0.030425,1.48263,-2.00287,0.16111,1.42821,-2.06243,0.062322,1.09432 [...]
+/*305*/{0.220353,1.92121,-1.74141,0.262797,1.85654,-1.90123,0.189618,1.77096,-1.6227,0.162407,1.66183,-1.53611,0.291065,1.6694,-1.63273,0.374949,1.66856,-1.66706,0.147663,1.93834,-2.12116,0.255947,1.86174,-1.97754,0.036551,1.90123,-2.00921,-0.089874,1.82059,-2.17173,-0.106175,1.72793,-2.22537,0.026415,1.59794,-2.16978,0.063421,1.54272,-2.18751,0.150756,1.40683,-1.80164,-0.032664,1.48594,-1.88497,-0.029134,1.51141,-1.93323,-0.028716,1.48774,-2.00219,0.161818,1.43298,-2.06236,0.062087,1.09 [...]
+/*306*/{0.221245,1.92553,-1.74316,0.264725,1.862,-1.9035,0.193499,1.77616,-1.62345,0.166949,1.66688,-1.53569,0.292563,1.67506,-1.6345,0.376737,1.67391,-1.67059,0.146401,1.94187,-2.12113,0.2551,1.86566,-1.97915,0.036054,1.90359,-2.00964,-0.091802,1.82421,-2.16887,-0.110172,1.73317,-2.22301,0.02336,1.60338,-2.16833,0.059232,1.5495,-2.18817,0.153346,1.40825,-1.80301,-0.030532,1.49048,-1.88442,-0.027342,1.51527,-1.93175,-0.026612,1.49282,-2.00146,0.16258,1.43822,-2.06232,0.05524,1.1006,-1.77 [...]
+/*307*/{0.22309,1.9294,-1.74473,0.264741,1.86509,-1.90497,0.196149,1.78104,-1.62396,0.170144,1.67119,-1.5362,0.294827,1.68054,-1.63645,0.377492,1.67977,-1.67521,0.145037,1.94522,-2.12143,0.254632,1.86974,-1.98099,0.036302,1.90742,-2.00873,-0.093377,1.83013,-2.16651,-0.113438,1.73855,-2.22088,0.021502,1.60852,-2.16729,0.056504,1.55519,-2.18802,0.155389,1.41007,-1.80393,-0.027972,1.49388,-1.88388,-0.025719,1.51912,-1.92967,-0.024512,1.49773,-2.00149,0.16294,1.44242,-2.063,0.051315,1.10362, [...]
+/*308*/{0.224016,1.93356,-1.74631,0.264991,1.86977,-1.90741,0.198513,1.78571,-1.62494,0.17353,1.6772,-1.53668,0.296181,1.6857,-1.63856,0.378748,1.68549,-1.67907,0.143658,1.94822,-2.12196,0.254422,1.87376,-1.9816,0.035578,1.91045,-2.00764,-0.095121,1.83447,-2.16453,-0.116861,1.7434,-2.21789,0.019103,1.61378,-2.16679,0.054538,1.55999,-2.18758,0.157834,1.41191,-1.80511,-0.025895,1.49745,-1.88304,-0.023216,1.52252,-1.93007,-0.022628,1.50284,-2.0004,0.163448,1.44653,-2.06311,0.046594,1.10763, [...]
+/*309*/{0.226079,1.93748,-1.7478,0.26585,1.87272,-1.90861,0.201144,1.78988,-1.62552,0.177614,1.68146,-1.53673,0.298172,1.69013,-1.6406,0.379762,1.69035,-1.68194,0.143177,1.95113,-2.12202,0.254423,1.87703,-1.98321,0.035644,1.9132,-2.00684,-0.095687,1.83842,-2.16199,-0.119347,1.74806,-2.21555,0.017426,1.61838,-2.16618,0.05312,1.56514,-2.18749,0.159282,1.41387,-1.80596,-0.023526,1.50061,-1.88241,-0.021857,1.52618,-1.9288,-0.020706,1.5074,-2.00034,0.164364,1.45119,-2.06348,0.045053,1.10986,- [...]
+/*310*/{0.227243,1.94192,-1.74926,0.266585,1.87748,-1.90971,0.202891,1.79409,-1.62623,0.179607,1.68513,-1.53621,0.299316,1.69445,-1.64264,0.38084,1.69438,-1.6852,0.141772,1.95409,-2.12197,0.254206,1.88027,-1.98458,0.035761,1.9152,-2.00691,-0.09719,1.84243,-2.15967,-0.12223,1.75198,-2.21303,0.016311,1.6229,-2.16551,0.051528,1.56919,-2.18761,0.16194,1.41595,-1.80663,-0.021404,1.50476,-1.88203,-0.020664,1.52964,-1.92836,-0.019454,1.51161,-2.00004,0.164701,1.45523,-2.06404,0.042011,1.1146,-1 [...]
+/*311*/{0.228504,1.94424,-1.75079,0.266691,1.8807,-1.91101,0.205075,1.79835,-1.62719,0.182738,1.68885,-1.53616,0.299837,1.69872,-1.64468,0.381328,1.69865,-1.68899,0.141288,1.95663,-2.12207,0.25453,1.88366,-1.98532,0.035953,1.91798,-2.00625,-0.097827,1.84662,-2.15813,-0.124103,1.75572,-2.21043,0.014932,1.62673,-2.16494,0.049933,1.57342,-2.18752,0.163405,1.41798,-1.80819,-0.020205,1.50826,-1.88082,-0.019185,1.53329,-1.92649,-0.017475,1.51646,-1.99932,0.165693,1.45837,-2.06451,0.036206,1.11 [...]
+/*312*/{0.230068,1.94732,-1.75171,0.266909,1.8836,-1.91198,0.206577,1.80185,-1.62768,0.184018,1.69245,-1.53722,0.300841,1.70185,-1.64652,0.38159,1.70196,-1.69075,0.140348,1.95903,-2.12269,0.254309,1.88643,-1.98661,0.036676,1.91964,-2.00568,-0.09866,1.84951,-2.15501,-0.126151,1.75908,-2.20838,0.013761,1.63009,-2.16479,0.04881,1.57692,-2.18755,0.165016,1.42032,-1.80863,-0.017812,1.51144,-1.88044,-0.017976,1.53758,-1.92617,-0.016186,1.52048,-1.99863,0.165125,1.46136,-2.06465,0.036454,1.1202 [...]
+/*313*/{0.23106,1.95021,-1.75337,0.268379,1.88663,-1.91324,0.208287,1.80562,-1.62844,0.186383,1.6958,-1.53704,0.30248,1.70481,-1.64836,0.381619,1.70489,-1.69406,0.14058,1.96141,-2.12331,0.255004,1.8888,-1.98724,0.036534,1.9218,-2.0059,-0.098804,1.85319,-2.15271,-0.127925,1.76259,-2.20524,0.01244,1.63335,-2.16449,0.047851,1.58049,-2.18826,0.166285,1.42296,-1.80908,-0.01647,1.51397,-1.88007,-0.017076,1.54058,-1.92528,-0.013452,1.52301,-1.99698,0.16636,1.46451,-2.06482,0.034498,1.1238,-1.76 [...]
+/*314*/{0.232809,1.95251,-1.75407,0.268557,1.88998,-1.9153,0.210012,1.80877,-1.62924,0.187834,1.69726,-1.53695,0.302553,1.70789,-1.64967,0.381539,1.70783,-1.69726,0.140034,1.96345,-2.12317,0.255789,1.89115,-1.98898,0.037365,1.92361,-2.00496,-0.099806,1.85503,-2.14963,-0.129297,1.76506,-2.20247,0.012251,1.63551,-2.164,0.047113,1.5836,-2.18839,0.167153,1.42535,-1.80896,-0.015795,1.5177,-1.88022,-0.0146,1.54349,-1.92471,-0.012873,1.52599,-1.99598,0.166877,1.46733,-2.0649,0.033508,1.12727,-1 [...]
+/*315*/{0.234035,1.95519,-1.75567,0.26838,1.89139,-1.9156,0.210789,1.81126,-1.6295,0.189488,1.70033,-1.53664,0.30326,1.70999,-1.65144,0.381439,1.70962,-1.69965,0.1398,1.96511,-2.12359,0.255865,1.89266,-1.98976,0.038023,1.92574,-2.00458,-0.100298,1.85729,-2.14817,-0.130665,1.76714,-2.20041,0.011462,1.63847,-2.16421,0.046729,1.58601,-2.1892,0.169008,1.42798,-1.81024,-0.01464,1.52137,-1.87913,-0.014426,1.54659,-1.92385,-0.011983,1.52937,-1.9954,0.167139,1.46918,-2.06464,0.032744,1.13049,-1. [...]
+/*316*/{0.234822,1.95688,-1.75654,0.269878,1.89416,-1.91715,0.211479,1.81418,-1.63039,0.190454,1.7023,-1.53718,0.303587,1.71168,-1.65367,0.381982,1.71092,-1.70177,0.139748,1.96682,-2.12357,0.256513,1.89448,-1.99067,0.038165,1.92684,-2.00471,-0.101039,1.85897,-2.14535,-0.131502,1.76891,-2.19839,0.010776,1.64059,-2.16443,0.046012,1.588,-2.18957,0.169661,1.43067,-1.81016,-0.013453,1.52387,-1.87809,-0.013706,1.54982,-1.92307,-0.011698,1.53244,-1.99416,0.167206,1.47116,-2.06445,0.032363,1.132 [...]
+/*317*/{0.236751,1.95847,-1.75768,0.269446,1.89625,-1.91766,0.212943,1.81621,-1.63124,0.191689,1.70441,-1.53783,0.302916,1.71249,-1.65494,0.38123,1.71179,-1.70414,0.139639,1.96805,-2.12453,0.256915,1.89552,-1.99182,0.038948,1.92869,-2.00507,-0.101431,1.8594,-2.14275,-0.132315,1.77011,-2.19633,0.010911,1.64193,-2.16459,0.046473,1.5899,-2.19021,0.169955,1.4332,-1.8097,-0.01337,1.52637,-1.87812,-0.013014,1.55229,-1.92358,-0.011289,1.53452,-1.99397,0.167144,1.47319,-2.0637,0.032557,1.13506,- [...]
+/*318*/{0.237642,1.95977,-1.75841,0.270213,1.89761,-1.91908,0.213781,1.81795,-1.63179,0.192185,1.70553,-1.53826,0.303465,1.71387,-1.65699,0.38072,1.71227,-1.70672,0.140405,1.96925,-2.12511,0.25749,1.89687,-1.9928,0.040394,1.93018,-2.00509,-0.100642,1.86068,-2.14098,-0.132893,1.77079,-2.19411,0.011875,1.64405,-2.16525,0.046079,1.59107,-2.191,0.170068,1.43518,-1.80949,-0.012969,1.52805,-1.87708,-0.012649,1.55467,-1.92338,-0.010863,1.53629,-1.99319,0.168768,1.47501,-2.06318,0.034613,1.13638 [...]
+/*319*/{0.238221,1.96056,-1.75949,0.269751,1.89889,-1.91976,0.213422,1.81917,-1.63299,0.191044,1.70717,-1.53928,0.303189,1.71377,-1.65847,0.380709,1.71163,-1.70877,0.141019,1.96995,-2.12584,0.257473,1.89709,-1.99371,0.041257,1.931,-2.00545,-0.101125,1.86109,-2.13865,-0.133342,1.77144,-2.19221,0.01288,1.6447,-2.16528,0.046923,1.59199,-2.19166,0.170758,1.43753,-1.80896,-0.012394,1.52987,-1.87666,-0.013783,1.5567,-1.92322,-0.010785,1.5379,-1.99241,0.168497,1.47568,-2.06213,0.038054,1.1364,- [...]
+/*320*/{0.239811,1.96077,-1.7603,0.271421,1.8997,-1.92162,0.213875,1.8197,-1.634,0.191285,1.70774,-1.53971,0.302923,1.71257,-1.65774,0.378838,1.70935,-1.71005,0.141901,1.97061,-2.12662,0.257493,1.89749,-1.99491,0.041583,1.93153,-2.00616,-0.101203,1.86103,-2.13603,-0.13351,1.77167,-2.19075,0.013522,1.64533,-2.16574,0.047491,1.59251,-2.19253,0.170876,1.43981,-1.80911,-0.011903,1.53127,-1.87654,-0.014049,1.5587,-1.92245,-0.010404,1.53893,-1.99244,0.169307,1.47727,-2.06148,0.042511,1.1365,-1 [...]
+/*321*/{0.240498,1.96067,-1.76067,0.270877,1.89982,-1.92189,0.213793,1.82017,-1.63511,0.190856,1.70761,-1.54004,0.301876,1.71128,-1.659,0.377939,1.7081,-1.7123,0.142502,1.97036,-2.12759,0.258604,1.89829,-1.99544,0.042369,1.93289,-2.00639,-0.100713,1.86054,-2.13446,-0.133406,1.77154,-2.18892,0.013777,1.64532,-2.16584,0.048192,1.59261,-2.19309,0.171523,1.44135,-1.80868,-0.01203,1.53189,-1.87632,-0.013175,1.56001,-1.92246,-0.010192,1.53917,-1.9926,0.170432,1.47791,-2.06069,0.047199,1.13566, [...]
+/*322*/{0.241234,1.96007,-1.76153,0.270413,1.89937,-1.92245,0.213221,1.82003,-1.63592,0.189432,1.70714,-1.54043,0.300821,1.70921,-1.66048,0.37688,1.70585,-1.71387,0.142972,1.97018,-2.1285,0.259275,1.89827,-1.99571,0.043878,1.933,-2.00752,-0.10099,1.86,-2.13238,-0.133383,1.7706,-2.18805,0.014912,1.64449,-2.16574,0.049331,1.59188,-2.19372,0.170632,1.44246,-1.80754,-0.011983,1.53192,-1.87636,-0.013744,1.56,-1.92303,-0.010305,1.53895,-1.99255,0.171259,1.47845,-2.05966,0.052355,1.13401,-1.769 [...]
+/*323*/{0.242297,1.95981,-1.76287,0.269789,1.89937,-1.92315,0.212565,1.81959,-1.63691,0.188357,1.7065,-1.54077,0.300205,1.7079,-1.66194,0.375826,1.70329,-1.71543,0.144109,1.96946,-2.12943,0.259355,1.89778,-1.99629,0.044629,1.93332,-2.00816,-0.099175,1.8589,-2.13047,-0.132553,1.76945,-2.18679,0.017214,1.64354,-2.16594,0.050223,1.59069,-2.19442,0.170254,1.44343,-1.80722,-0.01203,1.53189,-1.87632,-0.013744,1.56,-1.92303,-0.009764,1.53775,-1.99218,0.172033,1.4785,-2.05866,0.058135,1.13253,-1 [...]
+/*324*/{0.242842,1.959,-1.76335,0.270301,1.89966,-1.92375,0.211113,1.81921,-1.63731,0.186383,1.70574,-1.54206,0.29868,1.7053,-1.66314,0.374618,1.69965,-1.71676,0.144646,1.96863,-2.13019,0.259239,1.89731,-1.99661,0.045044,1.93291,-2.00853,-0.09797,1.8569,-2.12868,-0.131545,1.76772,-2.18633,0.018412,1.64205,-2.16609,0.051901,1.58914,-2.19473,0.170107,1.44371,-1.80694,-0.012101,1.53161,-1.87674,-0.013823,1.55979,-1.9236,-0.009545,1.53613,-1.99271,0.173123,1.47867,-2.05722,0.064396,1.13217,- [...]
+/*325*/{0.242571,1.95758,-1.76389,0.269594,1.89869,-1.9241,0.209865,1.81811,-1.63866,0.183372,1.70467,-1.54367,0.297264,1.70277,-1.66393,0.372018,1.69582,-1.71745,0.145895,1.9675,-2.13079,0.259222,1.89649,-1.99716,0.04528,1.93191,-2.00931,-0.09683,1.8544,-2.12752,-0.130494,1.76552,-2.18594,0.019727,1.63981,-2.16653,0.053934,1.58692,-2.1952,0.170198,1.44393,-1.80554,-0.012646,1.53046,-1.87681,-0.014208,1.5582,-1.9233,-0.009864,1.53461,-1.99304,0.173951,1.47845,-2.05616,0.071618,1.12838,-1 [...]
+/*326*/{0.2425,1.95606,-1.76488,0.26948,1.89666,-1.92416,0.208773,1.81722,-1.63967,0.182295,1.70331,-1.54457,0.29469,1.69871,-1.66491,0.370057,1.69107,-1.71843,0.145751,1.96621,-2.13199,0.259444,1.89562,-1.99753,0.046922,1.93108,-2.01058,-0.096472,1.85189,-2.12524,-0.128946,1.763,-2.18606,0.021247,1.63717,-2.16674,0.055754,1.58434,-2.19614,0.169786,1.44304,-1.80527,-0.012918,1.52862,-1.8775,-0.01367,1.55667,-1.9251,-0.009548,1.53306,-1.99448,0.175167,1.47797,-2.05537,0.077857,1.12599,-1. [...]
+/*327*/{0.241558,1.95452,-1.76577,0.270231,1.89551,-1.92454,0.206566,1.81571,-1.64054,0.178597,1.70158,-1.54617,0.293409,1.69531,-1.66559,0.368548,1.68607,-1.71887,0.14637,1.96419,-2.13257,0.259034,1.89392,-1.99767,0.046988,1.93015,-2.01267,-0.095572,1.85231,-2.12585,-0.127745,1.76018,-2.1863,0.024255,1.63438,-2.16716,0.057847,1.58153,-2.19677,0.169846,1.44239,-1.80487,-0.013147,1.52686,-1.87791,-0.013523,1.55471,-1.92585,-0.009364,1.5303,-1.9945,0.175068,1.47621,-2.05468,0.083596,1.1231 [...]
+/*328*/{0.241722,1.95223,-1.76608,0.269396,1.89385,-1.92491,0.204019,1.81366,-1.64128,0.175713,1.69901,-1.54734,0.290216,1.69142,-1.66649,0.365077,1.68126,-1.7197,0.146428,1.96202,-2.13299,0.259296,1.8926,-1.99807,0.046581,1.92832,-2.0132,-0.093066,1.84629,-2.1237,-0.125676,1.75653,-2.18697,0.026682,1.63049,-2.16731,0.060234,1.57823,-2.19751,0.169896,1.44171,-1.80448,-0.013374,1.52474,-1.87853,-0.013092,1.55295,-1.92559,-0.009303,1.52786,-1.99527,0.176251,1.47496,-2.05447,0.088551,1.1216 [...]
+/*329*/{0.240487,1.94994,-1.76654,0.26951,1.89188,-1.92498,0.202054,1.81107,-1.64187,0.172831,1.69734,-1.54898,0.287541,1.68717,-1.6671,0.362585,1.67573,-1.71957,0.147858,1.96013,-2.13339,0.258763,1.88999,-1.99814,0.047036,1.9267,-2.01415,-0.09054,1.84267,-2.12332,-0.124031,1.75287,-2.18814,0.028505,1.62618,-2.16781,0.062925,1.57438,-2.1981,0.170298,1.44057,-1.8046,-0.013329,1.52205,-1.87945,-0.012308,1.54941,-1.92713,-0.008601,1.52505,-1.99621,0.176601,1.47235,-2.05443,0.096038,1.1178,- [...]
+/*330*/{0.240104,1.94707,-1.76674,0.269982,1.88891,-1.92551,0.200119,1.80928,-1.643,0.169122,1.6951,-1.55126,0.284116,1.68269,-1.66769,0.359853,1.67002,-1.71989,0.148153,1.95749,-2.13438,0.258092,1.8872,-1.99765,0.046201,1.92524,-2.01613,-0.089328,1.83915,-2.12369,-0.121369,1.74842,-2.18893,0.030307,1.62262,-2.16881,0.065531,1.57029,-2.19898,0.170985,1.43952,-1.80459,-0.01331,1.519,-1.88016,-0.012682,1.54709,-1.92711,-0.008734,1.52225,-1.99685,0.176883,1.46994,-2.05491,0.10223,1.11584,-1 [...]
+/*331*/{0.239366,1.94578,-1.76683,0.268673,1.88694,-1.92558,0.196349,1.80631,-1.64325,0.164687,1.69154,-1.55159,0.280579,1.67763,-1.668,0.356278,1.66368,-1.71952,0.148988,1.9546,-2.13462,0.258461,1.88516,-1.9978,0.04769,1.92261,-2.01677,-0.087896,1.83454,-2.12413,-0.11893,1.74345,-2.19033,0.032697,1.6178,-2.16958,0.068862,1.56581,-2.19975,0.171393,1.43728,-1.80457,-0.013008,1.51617,-1.88133,-0.012824,1.5442,-1.92821,-0.008172,1.51893,-1.99739,0.17765,1.46717,-2.0559,0.107658,1.11185,-1.7 [...]
+/*332*/{0.237994,1.94262,-1.76682,0.268099,1.88409,-1.92545,0.194371,1.80319,-1.64345,0.160146,1.6886,-1.55296,0.276416,1.67271,-1.66757,0.352942,1.65681,-1.71889,0.14878,1.95157,-2.13505,0.258347,1.88233,-1.99746,0.046783,1.9196,-2.0176,-0.085733,1.8293,-2.12727,-0.116783,1.73814,-2.19197,0.036348,1.61372,-2.17101,0.071379,1.56071,-2.20044,0.171615,1.4348,-1.80451,-0.013238,1.51309,-1.88184,-0.01245,1.54092,-1.92845,-0.008453,1.51541,-1.99805,0.177142,1.46402,-2.05637,0.112369,1.10875,- [...]
+/*333*/{0.236114,1.93893,-1.76654,0.268108,1.8809,-1.92546,0.191278,1.79995,-1.64397,0.15526,1.68518,-1.55481,0.272358,1.66669,-1.6682,0.348445,1.64974,-1.71828,0.149084,1.94819,-2.13543,0.258119,1.87911,-1.99766,0.046255,1.91739,-2.01965,-0.085801,1.82283,-2.12807,-0.114627,1.73236,-2.19355,0.039236,1.60828,-2.17241,0.074325,1.55552,-2.20155,0.171675,1.43226,-1.80473,-0.013128,1.50981,-1.88187,-0.0127,1.53704,-1.92885,-0.008652,1.51195,-1.99878,0.177331,1.46131,-2.0568,0.117374,1.10583, [...]
+/*334*/{0.235658,1.9357,-1.76587,0.268567,1.87732,-1.92549,0.187809,1.79596,-1.64396,0.149524,1.68141,-1.55634,0.267571,1.66117,-1.66809,0.344431,1.64234,-1.71721,0.148774,1.94448,-2.1355,0.258171,1.87559,-1.99797,0.045936,1.91329,-2.02041,-0.083997,1.81787,-2.12869,-0.111729,1.72633,-2.19569,0.041157,1.6024,-2.17392,0.07758,1.54961,-2.20262,0.171852,1.4295,-1.80511,-0.013907,1.50567,-1.88275,-0.013179,1.53328,-1.9295,-0.00856,1.50793,-1.99894,0.177306,1.45874,-2.05762,0.122553,1.10274,- [...]
+/*335*/{0.233833,1.93171,-1.76574,0.267044,1.87347,-1.92451,0.183903,1.79217,-1.64414,0.144493,1.67775,-1.55796,0.262754,1.65496,-1.66834,0.340225,1.63449,-1.71624,0.149312,1.94069,-2.13623,0.258143,1.87184,-1.99829,0.045173,1.90994,-2.02207,-0.083078,1.81249,-2.12882,-0.109199,1.71939,-2.19811,0.044281,1.59676,-2.17567,0.080609,1.54334,-2.20387,0.171971,1.42649,-1.80556,-0.013119,1.50116,-1.88274,-0.011957,1.52944,-1.92999,-0.009309,1.50341,-1.99868,0.176968,1.45581,-2.05841,0.127262,1. [...]
+/*336*/{0.23179,1.92785,-1.76517,0.267153,1.86952,-1.92473,0.18093,1.78777,-1.64489,0.138914,1.67396,-1.55981,0.258259,1.64879,-1.66826,0.335328,1.62606,-1.71483,0.149437,1.93646,-2.13646,0.257573,1.86786,-1.99757,0.044703,1.90533,-2.0236,-0.081753,1.80393,-2.13335,-0.107313,1.71244,-2.20051,0.046988,1.58996,-2.17742,0.083709,1.53668,-2.20512,0.173219,1.42371,-1.80641,-0.012802,1.49672,-1.88305,-0.012612,1.52536,-1.92994,-0.009966,1.49881,-1.9998,0.176351,1.4524,-2.05919,0.131137,1.09697 [...]
+/*337*/{0.231363,1.92391,-1.76445,0.267481,1.86504,-1.92451,0.177138,1.78329,-1.64455,0.132707,1.66936,-1.56086,0.252473,1.64243,-1.66825,0.330582,1.61772,-1.71344,0.149418,1.93177,-2.13709,0.258312,1.86316,-1.99727,0.045266,1.90077,-2.02486,-0.081999,1.79691,-2.13674,-0.104648,1.70463,-2.20319,0.050196,1.58302,-2.17911,0.086928,1.53,-2.20644,0.173563,1.42043,-1.80678,-0.012576,1.49165,-1.88249,-0.012788,1.52079,-1.92968,-0.010786,1.49418,-1.99956,0.176452,1.4491,-2.06051,0.136309,1.0920 [...]
+/*338*/{0.229465,1.91913,-1.76407,0.267357,1.86013,-1.92387,0.173642,1.77898,-1.64496,0.127019,1.66551,-1.56255,0.246788,1.63548,-1.66815,0.325433,1.60917,-1.71246,0.149466,1.92677,-2.13772,0.258076,1.85915,-1.99719,0.044181,1.89516,-2.02673,-0.079658,1.78876,-2.1399,-0.102192,1.69679,-2.20567,0.053382,1.57568,-2.18168,0.089837,1.52249,-2.20706,0.173244,1.41676,-1.80759,-0.013763,1.48749,-1.88357,-0.012557,1.51538,-1.92973,-0.011686,1.48885,-1.9993,0.175996,1.44569,-2.0616,0.140689,1.089 [...]
+/*339*/{0.22749,1.91459,-1.76325,0.267548,1.85485,-1.92356,0.169652,1.77355,-1.64503,0.121528,1.66147,-1.56454,0.24172,1.62861,-1.66778,0.319493,1.60032,-1.71056,0.149827,1.92199,-2.13824,0.257573,1.85389,-1.99716,0.044179,1.88935,-2.02716,-0.079294,1.78038,-2.1432,-0.098996,1.68845,-2.20879,0.056335,1.5682,-2.18355,0.094121,1.51551,-2.2081,0.174851,1.41301,-1.80909,-0.013865,1.48187,-1.88358,-0.013003,1.51063,-1.93052,-0.012143,1.48371,-1.99974,0.175458,1.44185,-2.06292,0.144455,1.08628 [...]
+/*340*/{0.225944,1.90861,-1.76266,0.266463,1.84946,-1.92245,0.165587,1.7688,-1.64542,0.113927,1.65745,-1.56638,0.234971,1.62071,-1.6675,0.314357,1.59131,-1.70906,0.149809,1.91649,-2.1385,0.257599,1.84868,-1.99641,0.042993,1.88404,-2.02941,-0.077784,1.77284,-2.14652,-0.096135,1.67999,-2.21212,0.059728,1.55986,-2.18617,0.098143,1.50688,-2.20883,0.174594,1.40918,-1.81038,-0.014291,1.47634,-1.88333,-0.013027,1.50521,-1.93084,-0.012185,1.47747,-1.99857,0.175374,1.43827,-2.06454,0.146679,1.083 [...]
+/*341*/{0.224509,1.90351,-1.76173,0.265967,1.84329,-1.92208,0.161285,1.76335,-1.64533,0.106359,1.65269,-1.56739,0.229334,1.61402,-1.66741,0.30729,1.58167,-1.70741,0.150626,1.91093,-2.13918,0.257436,1.84381,-1.99611,0.043181,1.87734,-2.02988,-0.07607,1.76338,-2.1513,-0.092955,1.67138,-2.21542,0.063563,1.55173,-2.18769,0.102337,1.49915,-2.20994,0.174681,1.40467,-1.81103,-0.014311,1.47052,-1.88399,-0.013915,1.4999,-1.93074,-0.013399,1.47177,-1.99806,0.174953,1.43398,-2.06625,0.149073,1.0791 [...]
+/*342*/{0.223639,1.89789,-1.76039,0.266536,1.83927,-1.9208,0.157387,1.75763,-1.64552,0.100676,1.64868,-1.56834,0.223,1.60707,-1.66614,0.302319,1.57304,-1.70482,0.151448,1.90514,-2.13995,0.258467,1.83842,-1.99517,0.041948,1.87174,-2.03233,-0.075989,1.75448,-2.15543,-0.089384,1.66195,-2.21901,0.06869,1.54322,-2.18919,0.106745,1.49093,-2.21035,0.175561,1.4003,-1.81224,-0.014511,1.46459,-1.88399,-0.014178,1.49357,-1.93169,-0.014629,1.46633,-1.99815,0.174762,1.43001,-2.06801,0.153922,1.07529, [...]
+/*343*/{0.221863,1.89248,-1.75979,0.266759,1.83237,-1.92013,0.152455,1.75161,-1.64593,0.093189,1.64447,-1.56984,0.216523,1.59963,-1.66541,0.294678,1.56354,-1.70317,0.152545,1.89978,-2.14073,0.258705,1.83294,-1.99511,0.041804,1.86475,-2.03249,-0.072829,1.74541,-2.15924,-0.085425,1.65319,-2.22252,0.072549,1.5344,-2.19145,0.111268,1.48181,-2.21106,0.175989,1.39546,-1.81388,-0.015507,1.45805,-1.88342,-0.014177,1.48794,-1.93128,-0.016372,1.46101,-1.99811,0.173636,1.42567,-2.07014,0.159955,1.0 [...]
+/*344*/{0.220459,1.88653,-1.75894,0.266413,1.82643,-1.9191,0.148347,1.74622,-1.64624,0.085714,1.63982,-1.57076,0.209353,1.59214,-1.66554,0.287927,1.55454,-1.70055,0.153151,1.894,-2.14153,0.259239,1.82745,-1.99376,0.041783,1.85801,-2.03431,-0.071765,1.73578,-2.16307,-0.081056,1.64336,-2.22586,0.077439,1.52683,-2.19337,0.116302,1.47327,-2.21173,0.175622,1.39075,-1.81501,-0.015711,1.45209,-1.88356,-0.015894,1.48189,-1.93189,-0.018801,1.45632,-1.99783,0.17188,1.42035,-2.07193,0.164556,1.0661 [...]
+/*345*/{0.218918,1.88081,-1.75771,0.265831,1.82003,-1.91707,0.143874,1.74055,-1.64642,0.078636,1.63643,-1.57294,0.202307,1.58463,-1.66443,0.280109,1.54558,-1.69807,0.154956,1.88842,-2.14285,0.259301,1.82144,-1.99361,0.041146,1.8524,-2.03597,-0.068043,1.7267,-2.16832,-0.075841,1.63361,-2.2298,0.082664,1.51808,-2.19511,0.121995,1.46488,-2.21233,0.176087,1.38534,-1.8164,-0.018248,1.44686,-1.8832,-0.017789,1.47559,-1.93159,-0.021047,1.45092,-1.99766,0.16936,1.41561,-2.07354,0.167698,1.06206, [...]
+/*346*/{0.217873,1.87509,-1.75643,0.26726,1.81413,-1.9159,0.139276,1.73509,-1.64688,0.071549,1.63314,-1.57495,0.195211,1.57792,-1.6631,0.273236,1.53837,-1.69571,0.156525,1.88279,-2.14408,0.261174,1.81582,-1.99262,0.040805,1.84827,-2.03745,-0.066066,1.71664,-2.1727,-0.070682,1.62399,-2.23327,0.088602,1.50891,-2.19731,0.128147,1.45658,-2.21261,0.17545,1.38036,-1.81829,-0.019652,1.44037,-1.88297,-0.02005,1.46986,-1.9306,-0.024504,1.4467,-1.99774,0.167085,1.41268,-2.07505,0.170923,1.05866,-1 [...]
+/*347*/{0.216782,1.86995,-1.75493,0.267484,1.80692,-1.91453,0.134905,1.72999,-1.64745,0.063737,1.62988,-1.57604,0.18789,1.57147,-1.66183,0.265047,1.53023,-1.69334,0.158244,1.87811,-2.14531,0.261221,1.80991,-1.99236,0.042545,1.84577,-2.0371,-0.062077,1.70739,-2.17854,-0.064405,1.61402,-2.23646,0.094766,1.50026,-2.19914,0.13448,1.44816,-2.21299,0.174397,1.37523,-1.82062,-0.02173,1.43546,-1.88255,-0.022255,1.46374,-1.92954,-0.027302,1.44229,-1.99691,0.164921,1.40982,-2.07701,0.178091,1.0572 [...]
+/*348*/{0.215105,1.8645,-1.75346,0.267689,1.80227,-1.91355,0.130472,1.72563,-1.6475,0.054995,1.62719,-1.57828,0.181592,1.56579,-1.66111,0.25672,1.52333,-1.69127,0.159715,1.87299,-2.14689,0.262785,1.80504,-1.99089,0.043568,1.84309,-2.03731,-0.054835,1.69885,-2.18504,-0.057779,1.60447,-2.24002,0.101156,1.49172,-2.20024,0.14228,1.44085,-2.21313,0.173603,1.3708,-1.82189,-0.024002,1.43083,-1.88108,-0.025216,1.46003,-1.93018,-0.030192,1.43881,-1.99573,0.16277,1.4064,-2.07927,0.184081,1.05557,- [...]
+/*349*/{0.215183,1.86017,-1.75177,0.268124,1.79737,-1.91181,0.125132,1.72175,-1.64799,0.048039,1.6247,-1.57959,0.174594,1.56107,-1.6596,0.247754,1.51684,-1.68891,0.161102,1.86828,-2.14829,0.263476,1.79963,-1.98966,0.044645,1.84032,-2.03753,-0.04937,1.68926,-2.19227,-0.049881,1.59426,-2.24294,0.109429,1.48464,-2.20109,0.150456,1.43283,-2.21249,0.172829,1.36706,-1.82459,-0.025251,1.42565,-1.87997,-0.02613,1.45702,-1.92942,-0.031915,1.43498,-1.99345,0.160831,1.40477,-2.08107,0.187902,1.055, [...]
+/*350*/{0.214461,1.85584,-1.7497,0.269268,1.79248,-1.91026,0.119504,1.71894,-1.64842,0.037893,1.62345,-1.58174,0.166498,1.55682,-1.65864,0.240225,1.51217,-1.68655,0.163825,1.86422,-2.14839,0.263729,1.79547,-1.98826,0.045327,1.83916,-2.0379,-0.043329,1.68189,-2.19847,-0.040707,1.58512,-2.24585,0.118085,1.47829,-2.20054,0.158902,1.42693,-2.21199,0.171229,1.36258,-1.82552,-0.027839,1.42275,-1.87798,-0.027753,1.4549,-1.92828,-0.034072,1.43291,-1.99163,0.158361,1.40256,-2.08306,0.194808,1.056 [...]
+/*351*/{0.213689,1.85143,-1.74839,0.268274,1.78878,-1.90862,0.114668,1.71783,-1.64939,0.029931,1.62379,-1.58405,0.158261,1.55379,-1.65812,0.232563,1.50739,-1.68331,0.166153,1.86095,-2.14763,0.264095,1.79195,-1.98687,0.045905,1.83633,-2.0378,-0.038098,1.67538,-2.20522,-0.030745,1.57712,-2.248,0.126841,1.47331,-2.20004,0.169304,1.42277,-2.21136,0.169214,1.35819,-1.82614,-0.028632,1.42042,-1.87614,-0.027992,1.45386,-1.92789,-0.035736,1.43139,-1.98921,0.156449,1.4016,-2.08471,0.202495,1.0591 [...]
+/*352*/{0.21309,1.84848,-1.74695,0.267891,1.78613,-1.90652,0.108142,1.71724,-1.65006,0.022025,1.62494,-1.58602,0.150339,1.55212,-1.65741,0.223396,1.50372,-1.68107,0.168465,1.85817,-2.14729,0.264242,1.78866,-1.98532,0.04672,1.83542,-2.03826,-0.031238,1.66838,-2.20956,-0.020619,1.56981,-2.24898,0.137402,1.46942,-2.19894,0.17979,1.41896,-2.20977,0.168414,1.35529,-1.82781,-0.029331,1.41894,-1.87564,-0.030728,1.45277,-1.92781,-0.036606,1.43011,-1.98694,0.155005,1.40037,-2.08608,0.209404,1.061 [...]
+/*353*/{0.212166,1.84573,-1.74671,0.266643,1.78333,-1.90592,0.10336,1.71776,-1.65097,0.013683,1.62699,-1.58959,0.14251,1.55062,-1.65712,0.215009,1.50027,-1.67875,0.170998,1.85558,-2.14618,0.264337,1.78661,-1.9835,0.048013,1.83305,-2.0384,-0.024102,1.66187,-2.2127,-0.011699,1.56496,-2.25056,0.146693,1.46677,-2.19726,0.190725,1.41723,-2.20795,0.166548,1.35211,-1.82687,-0.02986,1.41761,-1.87446,-0.030919,1.45234,-1.92807,-0.037154,1.42965,-1.98599,0.153912,1.39966,-2.08735,0.216656,1.06307, [...]
+/*354*/{0.211479,1.84412,-1.74577,0.265776,1.78075,-1.90398,0.096827,1.71825,-1.65148,0.006827,1.6301,-1.59261,0.134246,1.55068,-1.65697,0.206212,1.4988,-1.67723,0.173511,1.85383,-2.14565,0.264866,1.78447,-1.98235,0.048841,1.83205,-2.0391,-0.017584,1.65658,-2.21501,-0.002479,1.56041,-2.25141,0.157447,1.46559,-2.19562,0.202,1.41673,-2.20596,0.165761,1.3502,-1.82745,-0.030381,1.41764,-1.87375,-0.031325,1.45116,-1.92802,-0.037154,1.42965,-1.98599,0.153593,1.39877,-2.08839,0.223669,1.06688,- [...]
+/*355*/{0.210325,1.84337,-1.74441,0.265868,1.77954,-1.90297,0.092504,1.72007,-1.6522,-0.001379,1.63388,-1.59583,0.126085,1.55129,-1.65761,0.198019,1.49732,-1.67518,0.177259,1.8519,-2.14456,0.264489,1.78261,-1.98091,0.049883,1.83128,-2.04036,-0.008517,1.65303,-2.21635,0.006047,1.55653,-2.25206,0.166968,1.46331,-2.19311,0.213176,1.41747,-2.20313,0.16487,1.34867,-1.82716,-0.030662,1.418,-1.87358,-0.032098,1.45032,-1.92769,-0.037154,1.42965,-1.98599,0.152907,1.39772,-2.08939,0.229893,1.07073 [...]
+/*356*/{0.20762,1.84301,-1.74451,0.264999,1.77892,-1.9017,0.086328,1.72202,-1.65293,-0.009281,1.63797,-1.59868,0.118275,1.55156,-1.65603,0.188785,1.49748,-1.67291,0.179477,1.85099,-2.14408,0.265457,1.78142,-1.9793,0.051062,1.83115,-2.04122,-0.001782,1.65036,-2.21785,0.014777,1.55311,-2.25327,0.176843,1.4644,-2.19092,0.223564,1.41895,-2.19996,0.163167,1.34891,-1.82619,-0.032025,1.41807,-1.87491,-0.031882,1.45,-1.92903,-0.037797,1.42996,-1.98628,0.152274,1.397,-2.08986,0.240117,1.07547,-1. [...]
+/*357*/{0.206223,1.84317,-1.74283,0.264444,1.77793,-1.89967,0.080113,1.72509,-1.65374,-0.017849,1.64153,-1.60065,0.110791,1.55381,-1.65556,0.179618,1.49782,-1.67209,0.18351,1.85031,-2.14353,0.265089,1.78066,-1.97765,0.052057,1.83093,-2.04164,0.005348,1.64833,-2.21987,0.023676,1.55063,-2.25413,0.187219,1.46709,-2.18822,0.234884,1.42232,-2.19565,0.161948,1.34888,-1.82584,-0.032024,1.41799,-1.8756,-0.033304,1.44971,-1.92951,-0.03757,1.42994,-1.98748,0.152494,1.39541,-2.08972,0.246584,1.0814 [...]
+/*358*/{0.202867,1.84407,-1.74191,0.264017,1.77806,-1.8978,0.074999,1.72841,-1.65454,-0.025967,1.64582,-1.60386,0.10197,1.55632,-1.65563,0.171166,1.4999,-1.67147,0.186487,1.8506,-2.14229,0.265688,1.78092,-1.97616,0.054019,1.83067,-2.04271,0.010861,1.64667,-2.22038,0.031587,1.54891,-2.25476,0.197097,1.47016,-2.18525,0.245413,1.42701,-2.19116,0.161654,1.35019,-1.82538,-0.031989,1.41795,-1.87573,-0.033199,1.44965,-1.92968,-0.036911,1.43035,-1.98826,0.152521,1.3948,-2.08978,0.25298,1.08751,- [...]
+/*359*/{0.200582,1.84601,-1.74099,0.263386,1.77861,-1.89662,0.069391,1.73153,-1.65528,-0.033066,1.65047,-1.60548,0.094204,1.55966,-1.65588,0.161934,1.50213,-1.66995,0.188541,1.85122,-2.14092,0.266183,1.78137,-1.97407,0.054689,1.83123,-2.04406,0.016561,1.645,-2.22135,0.040147,1.54853,-2.25485,0.205978,1.47422,-2.18105,0.256008,1.43319,-2.18609,0.160325,1.35046,-1.82486,-0.03261,1.41897,-1.87722,-0.032776,1.44958,-1.93114,-0.037464,1.43087,-1.98978,0.151308,1.39369,-2.08999,0.25695,1.09524 [...]
+/*360*/{0.197072,1.84756,-1.74027,0.261894,1.77951,-1.89363,0.063479,1.73552,-1.65552,-0.040255,1.6554,-1.60824,0.086325,1.56315,-1.65552,0.154303,1.50561,-1.66926,0.190873,1.8527,-2.13942,0.266813,1.78261,-1.97191,0.055717,1.8316,-2.04444,0.022189,1.64427,-2.22217,0.048198,1.54803,-2.25499,0.214153,1.4795,-2.17741,0.265964,1.44027,-2.18132,0.159265,1.35236,-1.82392,-0.032964,1.41913,-1.87876,-0.03297,1.45075,-1.93228,-0.036852,1.43105,-1.99092,0.153281,1.39366,-2.09001,0.26321,1.10221,- [...]
+/*361*/{0.194266,1.8498,-1.73893,0.262304,1.78125,-1.89164,0.058093,1.73991,-1.6566,-0.048352,1.66019,-1.61022,0.079054,1.56775,-1.65647,0.1456,1.50937,-1.66865,0.193629,1.85487,-2.13773,0.266952,1.78386,-1.96976,0.05674,1.83178,-2.04506,0.027964,1.64403,-2.22237,0.056114,1.54842,-2.25469,0.222175,1.48495,-2.17333,0.275318,1.44807,-2.17609,0.159959,1.35309,-1.82238,-0.032406,1.41995,-1.87945,-0.032563,1.45081,-1.93331,-0.036132,1.4321,-1.99252,0.154086,1.39282,-2.08992,0.270118,1.10763,- [...]
+/*362*/{0.190971,1.85238,-1.73801,0.261481,1.7834,-1.88902,0.053191,1.7434,-1.65651,-0.055844,1.66508,-1.61285,0.070986,1.57258,-1.65709,0.137998,1.51377,-1.66891,0.195364,1.85742,-2.13596,0.267387,1.78555,-1.96792,0.058188,1.83282,-2.04544,0.032796,1.6446,-2.22226,0.064055,1.54937,-2.25469,0.230183,1.49171,-2.16951,0.285493,1.45678,-2.1709,0.160033,1.35549,-1.82214,-0.032748,1.42109,-1.88161,-0.031179,1.45224,-1.93445,-0.035436,1.43304,-1.9936,0.155703,1.39221,-2.09013,0.275299,1.11557, [...]
+/*363*/{0.187813,1.85497,-1.73691,0.261732,1.78633,-1.88697,0.047902,1.74833,-1.65784,-0.062135,1.67163,-1.61621,0.064518,1.57745,-1.65804,0.130875,1.5181,-1.66764,0.198346,1.86065,-2.13465,0.267545,1.78823,-1.96563,0.058588,1.83368,-2.04554,0.036713,1.64512,-2.2219,0.071835,1.55115,-2.2548,0.238591,1.49945,-2.16536,0.294531,1.46613,-2.16567,0.159568,1.35712,-1.8216,-0.032207,1.42227,-1.88326,-0.030587,1.45397,-1.93548,-0.034725,1.43483,-1.99564,0.156961,1.3923,-2.0892,0.277477,1.12267,- [...]
+/*364*/{0.184264,1.85791,-1.73631,0.26034,1.78882,-1.88514,0.042974,1.75323,-1.65836,-0.068611,1.67714,-1.61785,0.05802,1.58304,-1.65931,0.123556,1.52294,-1.66751,0.200259,1.86433,-2.13333,0.268255,1.79145,-1.96278,0.059593,1.83566,-2.04535,0.04185,1.64524,-2.22121,0.079884,1.55281,-2.25451,0.246119,1.50708,-2.16138,0.303,1.47642,-2.16006,0.160315,1.35971,-1.82164,-0.03106,1.42456,-1.88494,-0.029665,1.45568,-1.93717,-0.032673,1.43606,-1.99726,0.156032,1.39164,-2.09189,0.283258,1.12895,-1 [...]
+/*365*/{0.181708,1.86104,-1.73542,0.259791,1.79346,-1.88243,0.038295,1.75753,-1.65919,-0.075077,1.68379,-1.62114,0.051367,1.58928,-1.66069,0.116428,1.52827,-1.66754,0.201194,1.86809,-2.13173,0.268123,1.79431,-1.96068,0.061119,1.83742,-2.04596,0.046911,1.64676,-2.22044,0.087588,1.55495,-2.25403,0.252625,1.51522,-2.1573,0.311078,1.48646,-2.15457,0.161867,1.3634,-1.82163,-0.030293,1.4268,-1.88584,-0.027925,1.45761,-1.93835,-0.031098,1.43757,-1.99903,0.155529,1.39167,-2.09291,0.28828,1.13592 [...]
+/*366*/{0.179302,1.86426,-1.73507,0.260045,1.79614,-1.88083,0.033437,1.76214,-1.66026,-0.081595,1.69004,-1.62332,0.044959,1.59518,-1.66163,0.109912,1.53459,-1.66753,0.202356,1.87263,-2.13001,0.268855,1.79773,-1.95848,0.06216,1.83969,-2.04615,0.051766,1.64817,-2.22031,0.094665,1.55757,-2.25355,0.260082,1.52421,-2.15263,0.319005,1.49756,-2.14911,0.16147,1.36588,-1.82166,-0.029366,1.42905,-1.88735,-0.026247,1.45973,-1.94001,-0.029357,1.44037,-1.99939,0.15942,1.39132,-2.09153,0.290794,1.1424 [...]
+/*367*/{0.176014,1.86823,-1.73391,0.259477,1.79994,-1.87865,0.030018,1.76772,-1.66039,-0.086763,1.69585,-1.62624,0.039987,1.60127,-1.66318,0.103385,1.53968,-1.66809,0.203474,1.8773,-2.12872,0.269567,1.80155,-1.95673,0.063302,1.84189,-2.04551,0.055257,1.65178,-2.22047,0.102733,1.56092,-2.25375,0.265966,1.53318,-2.14812,0.325912,1.50832,-2.14346,0.162843,1.36979,-1.82117,-0.027922,1.43207,-1.88901,-0.02453,1.46207,-1.94061,-0.027881,1.44192,-2.00084,0.162061,1.39115,-2.09109,0.295115,1.150 [...]
+/*368*/{0.173729,1.8714,-1.73341,0.259873,1.80376,-1.87662,0.025978,1.77275,-1.66146,-0.092072,1.70272,-1.62937,0.033838,1.60725,-1.66435,0.097948,1.54505,-1.66845,0.205309,1.88184,-2.12723,0.270514,1.80515,-1.95466,0.064032,1.84499,-2.04622,0.060717,1.65452,-2.22016,0.109772,1.56432,-2.25337,0.271586,1.54298,-2.14413,0.333038,1.51917,-2.13772,0.16436,1.37417,-1.82249,-0.02635,1.4359,-1.88955,-0.023045,1.46505,-1.94336,-0.024565,1.44446,-2.00204,0.165363,1.39064,-2.09097,0.29786,1.15615, [...]
+/*369*/{0.171812,1.87528,-1.73276,0.260168,1.80734,-1.87487,0.022272,1.77793,-1.66292,-0.096338,1.70956,-1.63215,0.029689,1.61368,-1.6657,0.092999,1.5516,-1.66851,0.206597,1.88711,-2.1262,0.272733,1.8085,-1.95167,0.065989,1.84696,-2.04565,0.064995,1.65752,-2.22027,0.117097,1.56829,-2.25263,0.277446,1.55222,-2.14024,0.338747,1.53095,-2.13246,0.16618,1.37795,-1.82289,-0.024369,1.43901,-1.89074,-0.01992,1.46789,-1.94399,-0.021799,1.44787,-2.00267,0.165867,1.39112,-2.09091,0.300071,1.16305,- [...]
+/*370*/{0.169635,1.87843,-1.73237,0.2597,1.81199,-1.87321,0.018477,1.78352,-1.66386,-0.101174,1.71613,-1.6349,0.02465,1.61941,-1.66691,0.087522,1.55713,-1.66947,0.208426,1.89174,-2.1242,0.27324,1.8127,-1.95003,0.067161,1.85086,-2.04622,0.06996,1.66095,-2.22033,0.123938,1.5729,-2.25274,0.282451,1.56056,-2.1343,0.343686,1.54201,-2.12701,0.168164,1.3819,-1.82312,-0.02236,1.44363,-1.89142,-0.018674,1.47047,-1.94584,-0.019716,1.4501,-2.00416,0.169931,1.39106,-2.09101,0.302991,1.17072,-1.7616, [...]
+/*371*/{0.168204,1.88186,-1.73175,0.260605,1.81421,-1.87039,0.015154,1.78881,-1.6655,-0.105776,1.72263,-1.63825,0.021365,1.62503,-1.66797,0.082858,1.56265,-1.66933,0.209402,1.89711,-2.12255,0.272841,1.81662,-1.94817,0.068184,1.8537,-2.046,0.074772,1.66481,-2.22064,0.130832,1.57708,-2.25249,0.287987,1.57103,-2.13009,0.348047,1.55269,-2.12177,0.169198,1.38563,-1.82419,-0.021049,1.44762,-1.89139,-0.016301,1.47391,-1.94791,-0.016239,1.45387,-2.00461,0.171082,1.39084,-2.09111,0.306553,1.17647 [...]
+/*372*/{0.166696,1.88535,-1.73141,0.259788,1.81998,-1.87006,0.012591,1.7939,-1.66711,-0.108031,1.729,-1.64077,0.016026,1.63057,-1.66949,0.07852,1.56764,-1.67011,0.211522,1.90155,-2.12055,0.272988,1.82043,-1.94645,0.069829,1.85619,-2.0462,0.079427,1.66876,-2.22174,0.13808,1.58173,-2.25185,0.290708,1.57977,-2.12598,0.352653,1.56321,-2.11739,0.171477,1.38939,-1.82464,-0.017748,1.45121,-1.89263,-0.013745,1.47714,-1.94896,-0.013293,1.45681,-2.00611,0.173846,1.39085,-2.09065,0.309107,1.18276,- [...]
+/*373*/{0.1652,1.88798,-1.73105,0.259267,1.82357,-1.86786,0.009698,1.79878,-1.66845,-0.112399,1.73475,-1.64414,0.012405,1.63586,-1.67024,0.074865,1.57297,-1.6704,0.212842,1.90659,-2.1191,0.273659,1.82453,-1.94473,0.0712,1.85948,-2.04638,0.084787,1.67313,-2.22181,0.14339,1.58645,-2.25175,0.295643,1.58889,-2.12215,0.356481,1.574,-2.11277,0.172644,1.39378,-1.82548,-0.015914,1.45564,-1.8927,-0.01029,1.48003,-1.94993,-0.010059,1.45967,-2.00649,0.176324,1.39034,-2.09002,0.310515,1.18885,-1.760 [...]
+/*374*/{0.163849,1.89094,-1.73081,0.25932,1.82574,-1.86624,0.007721,1.8043,-1.66989,-0.115631,1.74086,-1.64716,0.009357,1.64109,-1.67176,0.071678,1.57791,-1.6711,0.215215,1.9112,-2.11716,0.27408,1.8282,-1.94296,0.07276,1.86247,-2.04696,0.090288,1.67723,-2.2218,0.150643,1.59124,-2.25166,0.297402,1.59653,-2.11919,0.360043,1.58414,-2.10865,0.17479,1.39715,-1.82596,-0.013206,1.46076,-1.89339,-0.008863,1.4838,-1.95182,-0.006302,1.46276,-2.00672,0.178187,1.39097,-2.08929,0.313107,1.19475,-1.76 [...]
+/*375*/{0.162919,1.89386,-1.73033,0.261256,1.83017,-1.86519,0.005562,1.80872,-1.67211,-0.118175,1.74588,-1.64984,0.006393,1.64627,-1.67338,0.068057,1.58235,-1.67112,0.216975,1.91529,-2.11545,0.274642,1.83231,-1.94128,0.074381,1.86535,-2.04719,0.094495,1.68156,-2.22177,0.156679,1.59597,-2.2517,0.300231,1.60475,-2.11591,0.36329,1.59398,-2.10466,0.176622,1.40148,-1.82596,-0.010612,1.46476,-1.89312,-0.006409,1.48658,-1.95324,-0.003907,1.46599,-2.00781,0.181058,1.39076,-2.08893,0.31526,1.2009 [...]
+/*376*/{0.162163,1.89658,-1.73038,0.259599,1.83323,-1.86355,0.004609,1.81321,-1.67355,-0.121156,1.7517,-1.65313,0.002802,1.65093,-1.67438,0.065277,1.58643,-1.67184,0.218485,1.91938,-2.11375,0.274674,1.83524,-1.93969,0.075862,1.86748,-2.0478,0.099934,1.68579,-2.22214,0.162161,1.5999,-2.25168,0.303106,1.61309,-2.11314,0.366259,1.6032,-2.10051,0.177296,1.40513,-1.82705,-0.0083,1.46922,-1.89325,-0.003295,1.49067,-1.95444,-0.000922,1.46882,-2.00807,0.182353,1.39067,-2.08831,0.316505,1.20572,- [...]
+/*377*/{0.161893,1.89911,-1.72965,0.259918,1.83813,-1.86281,0.003039,1.8177,-1.67485,-0.123693,1.75628,-1.6563,0.001271,1.65472,-1.67496,0.062501,1.58999,-1.67192,0.219743,1.92295,-2.11184,0.275804,1.83896,-1.93819,0.078406,1.86968,-2.04819,0.104377,1.68947,-2.22289,0.168065,1.60459,-2.25184,0.30506,1.61924,-2.11037,0.368419,1.6117,-2.09728,0.17919,1.40994,-1.82761,-0.005874,1.47374,-1.89417,-0.001821,1.49412,-1.95516,0.002864,1.47181,-2.00847,0.184685,1.39091,-2.08719,0.318848,1.20984,- [...]
+/*378*/{0.161172,1.90088,-1.7294,0.260794,1.84007,-1.86087,0.001941,1.82122,-1.67631,-0.124567,1.7601,-1.65961,-0.001418,1.65815,-1.67618,0.060875,1.59373,-1.67204,0.221523,1.92636,-2.11055,0.277058,1.84218,-1.93638,0.080017,1.87236,-2.04814,0.109204,1.69371,-2.2229,0.173672,1.60903,-2.25234,0.307005,1.62565,-2.1083,0.37087,1.61909,-2.09366,0.180936,1.41277,-1.82779,-0.003541,1.47738,-1.89369,0.001515,1.49779,-1.95711,0.005411,1.47494,-2.00945,0.186569,1.39088,-2.08615,0.319518,1.21356,- [...]
+/*379*/{0.160925,1.90306,-1.72945,0.254119,1.84611,-1.86186,0.001458,1.82469,-1.67739,-0.127606,1.76303,-1.6617,-0.002981,1.66182,-1.67746,0.058818,1.59622,-1.67226,0.22408,1.9294,-2.1083,0.276855,1.84559,-1.93572,0.081396,1.87431,-2.04857,0.113897,1.69713,-2.22416,0.177952,1.61241,-2.25258,0.308623,1.6323,-2.10607,0.372329,1.62675,-2.09081,0.181292,1.41606,-1.82858,-0.00036,1.48127,-1.89362,0.004702,1.50083,-1.95774,0.008038,1.47785,-2.00965,0.188283,1.39106,-2.08535,0.321006,1.21693,-1 [...]
+/*380*/{0.16102,1.90488,-1.72898,0.255279,1.84856,-1.86095,0.000741,1.82676,-1.67989,-0.12888,1.76715,-1.66474,-0.004294,1.66468,-1.67864,0.057459,1.59904,-1.67213,0.22551,1.93203,-2.10722,0.277816,1.84846,-1.93422,0.083979,1.87656,-2.04886,0.118586,1.7006,-2.22471,0.183126,1.61587,-2.25315,0.311621,1.63873,-2.10467,0.373318,1.63228,-2.08712,0.1818,1.41935,-1.82881,0.001178,1.48515,-1.89369,0.007435,1.50396,-1.95815,0.011648,1.48013,-2.00944,0.189373,1.39166,-2.08444,0.321137,1.22002,-1. [...]
+/*381*/{0.161488,1.90649,-1.72887,0.261517,1.84696,-1.85847,0.000909,1.82969,-1.68073,-0.12849,1.7694,-1.6671,-0.00525,1.66679,-1.67949,0.056633,1.60106,-1.67236,0.228052,1.93421,-2.10565,0.279294,1.84991,-1.93294,0.085315,1.8775,-2.04883,0.123344,1.70396,-2.22593,0.187332,1.61885,-2.25366,0.313112,1.64331,-2.10323,0.374965,1.63766,-2.08462,0.182122,1.42287,-1.82998,0.003765,1.48835,-1.89422,0.009449,1.50651,-1.95896,0.013255,1.48275,-2.00969,0.191142,1.39214,-2.08368,0.323257,1.22179,-1 [...]
+/*382*/{0.162207,1.90793,-1.72832,0.262795,1.84948,-1.85687,0.001016,1.83137,-1.6815,-0.129209,1.77133,-1.66902,-0.005279,1.66848,-1.67992,0.056515,1.60233,-1.67226,0.229923,1.93632,-2.10477,0.280584,1.85258,-1.9317,0.087613,1.87959,-2.04944,0.127665,1.70653,-2.22666,0.191501,1.62137,-2.25411,0.314515,1.6468,-2.10234,0.376274,1.64256,-2.08244,0.183611,1.42569,-1.82992,0.005379,1.49185,-1.89459,0.010645,1.50971,-1.9594,0.015449,1.48645,-2.00983,0.192475,1.39354,-2.08304,0.323656,1.22296,- [...]
+/*383*/{0.162235,1.90881,-1.72819,0.262975,1.85156,-1.85685,0.001374,1.83207,-1.68247,-0.129121,1.7724,-1.67101,-0.005723,1.67017,-1.68065,0.056832,1.60369,-1.67179,0.231027,1.93792,-2.10336,0.281318,1.85382,-1.93033,0.088833,1.88134,-2.04971,0.132126,1.70957,-2.22884,0.19555,1.62369,-2.25486,0.315456,1.65006,-2.10121,0.377306,1.6462,-2.08075,0.184815,1.4286,-1.82996,0.005429,1.49585,-1.89504,0.012233,1.51284,-1.95924,0.017249,1.48821,-2.00993,0.192979,1.39474,-2.08248,0.324068,1.22342,- [...]
+/*384*/{0.162895,1.90994,-1.72752,0.263872,1.8529,-1.85562,0.000667,1.83316,-1.6839,-0.130732,1.77278,-1.67249,-0.00579,1.67054,-1.68128,0.057017,1.60477,-1.67133,0.233136,1.93925,-2.10256,0.282703,1.8559,-1.92948,0.090982,1.88273,-2.04989,0.136512,1.71159,-2.22977,0.19903,1.62532,-2.25547,0.316388,1.65252,-2.09969,0.378417,1.64819,-2.07874,0.185336,1.43139,-1.82978,0.007279,1.49834,-1.89439,0.013147,1.51556,-1.95945,0.018302,1.4912,-2.01009,0.194181,1.39655,-2.08211,0.324006,1.22335,-1. [...]
+/*385*/{0.163853,1.91068,-1.72677,0.264638,1.85381,-1.85469,0.000885,1.83361,-1.68419,-0.1306,1.77293,-1.6741,-0.004741,1.67121,-1.6816,0.058253,1.60477,-1.6705,0.234517,1.94016,-2.10147,0.283448,1.85682,-1.92908,0.093194,1.88415,-2.04999,0.141315,1.71351,-2.23117,0.201957,1.62664,-2.25673,0.317395,1.65471,-2.0994,0.380004,1.64993,-2.07773,0.185949,1.434,-1.83018,0.007624,1.50073,-1.89418,0.01525,1.51772,-1.95882,0.019411,1.49373,-2.00984,0.194506,1.3983,-2.08164,0.320116,1.22222,-1.7648 [...]
+/*386*/{0.164442,1.9117,-1.72626,0.266474,1.85389,-1.85379,0.001064,1.83323,-1.68456,-0.130468,1.77224,-1.67507,-0.003111,1.67148,-1.6817,0.059366,1.60443,-1.67017,0.23662,1.94121,-2.10145,0.285018,1.85836,-1.92725,0.094927,1.88636,-2.05024,0.145561,1.71472,-2.23198,0.20422,1.62715,-2.25696,0.319468,1.65481,-2.09933,0.380541,1.65064,-2.07685,0.186737,1.43618,-1.83076,0.008815,1.50263,-1.89456,0.014922,1.52033,-1.95932,0.020423,1.49638,-2.00876,0.195113,1.40032,-2.08162,0.319358,1.21988,- [...]
+/*387*/{0.164922,1.91211,-1.72592,0.267301,1.85475,-1.85301,0.002597,1.83279,-1.68441,-0.128397,1.77098,-1.67542,-0.001972,1.67026,-1.68181,0.061461,1.60359,-1.66931,0.238676,1.94201,-2.10085,0.286391,1.85926,-1.92711,0.096838,1.88784,-2.04984,0.148118,1.71613,-2.23339,0.206436,1.62817,-2.25752,0.319678,1.65495,-2.09856,0.381181,1.65057,-2.076,0.186756,1.43784,-1.83125,0.009817,1.50467,-1.8939,0.015743,1.52296,-1.95831,0.020424,1.49753,-2.00837,0.195913,1.4022,-2.08164,0.315954,1.21658,- [...]
+/*388*/{0.166535,1.91235,-1.72518,0.269561,1.85526,-1.85279,0.003671,1.83185,-1.68455,-0.126185,1.76876,-1.67565,7.7e-005,1.66932,-1.68181,0.063586,1.60188,-1.66848,0.240495,1.94179,-2.10023,0.287704,1.85971,-1.92639,0.098387,1.88894,-2.04964,0.151011,1.71707,-2.23376,0.207293,1.62805,-2.25772,0.321464,1.65575,-2.09889,0.381558,1.64938,-2.07532,0.187007,1.44008,-1.83131,0.009727,1.50536,-1.89338,0.016034,1.52421,-1.95818,0.021072,1.49957,-2.00795,0.197055,1.40464,-2.08145,0.315682,1.2117 [...]
+/*389*/{0.16724,1.91241,-1.72463,0.270286,1.85573,-1.85238,0.004223,1.8303,-1.68456,-0.12492,1.76577,-1.67589,0.002708,1.66702,-1.68109,0.066139,1.60035,-1.66645,0.242319,1.94165,-2.09997,0.288594,1.85999,-1.9255,0.1,1.88981,-2.04942,0.152724,1.71701,-2.23372,0.208382,1.62759,-2.25804,0.32259,1.65328,-2.09885,0.382829,1.64684,-2.07473,0.187339,1.44152,-1.83154,0.010303,1.50743,-1.89206,0.016791,1.52539,-1.95747,0.021509,1.50104,-2.00696,0.197005,1.40619,-2.08129,0.312792,1.20692,-1.7644, [...]
+/*390*/{0.168301,1.91206,-1.72397,0.271837,1.85486,-1.8516,0.006001,1.82838,-1.68338,-0.123527,1.76252,-1.67562,0.005429,1.66456,-1.68056,0.068911,1.59749,-1.66563,0.243462,1.9414,-2.0998,0.289733,1.85998,-1.92518,0.10089,1.89154,-2.04925,0.154064,1.71698,-2.23373,0.208258,1.62737,-2.25813,0.323039,1.65072,-2.09804,0.383689,1.64391,-2.0748,0.187117,1.44211,-1.83179,0.010387,1.50758,-1.89203,0.016118,1.52693,-1.95718,0.021348,1.50241,-2.00647,0.197812,1.40808,-2.08125,0.307871,1.2017,-1.7 [...]
+/*391*/{0.16981,1.91161,-1.72306,0.272582,1.85597,-1.85172,0.007219,1.82542,-1.68302,-0.122129,1.75897,-1.67437,0.008334,1.66169,-1.67954,0.072615,1.5953,-1.66398,0.245494,1.94058,-2.09928,0.290919,1.86021,-1.9249,0.102574,1.89214,-2.04858,0.154696,1.71692,-2.23351,0.207857,1.62694,-2.25811,0.323925,1.64849,-2.09799,0.38389,1.64014,-2.07475,0.189959,1.44382,-1.83265,0.01065,1.50806,-1.89196,0.016067,1.52786,-1.95706,0.021251,1.50296,-2.00583,0.198002,1.40949,-2.08135,0.303158,1.19573,-1. [...]
+/*392*/{0.17136,1.91063,-1.72241,0.273591,1.85608,-1.85121,0.009163,1.82339,-1.68148,-0.118708,1.75472,-1.67346,0.011697,1.65876,-1.67825,0.07601,1.59195,-1.6623,0.246784,1.9396,-2.09815,0.291574,1.86006,-1.92448,0.103407,1.89229,-2.04831,0.153886,1.71695,-2.23296,0.206516,1.6262,-2.25763,0.325072,1.64486,-2.09824,0.385073,1.63531,-2.07527,0.191373,1.44355,-1.83134,0.010528,1.50796,-1.89137,0.016417,1.52832,-1.95597,0.021149,1.50312,-2.00497,0.198818,1.41099,-2.08145,0.300432,1.18933,-1. [...]
+/*393*/{0.172218,1.90985,-1.7218,0.275582,1.8563,-1.85133,0.011507,1.81975,-1.68043,-0.115192,1.75022,-1.67202,0.015284,1.6543,-1.67614,0.080122,1.58792,-1.65985,0.24774,1.93832,-2.09811,0.292309,1.85937,-1.92447,0.104564,1.89253,-2.0473,0.152508,1.7159,-2.23183,0.204664,1.62512,-2.25723,0.324774,1.6407,-2.09873,0.384957,1.63004,-2.07586,0.192091,1.44335,-1.83152,0.011906,1.50631,-1.89026,0.016317,1.52836,-1.95582,0.021814,1.50305,-2.00424,0.199486,1.41144,-2.08162,0.296901,1.1828,-1.763 [...]
+/*394*/{0.174385,1.90807,-1.72082,0.276627,1.85413,-1.85073,0.013678,1.8162,-1.67947,-0.112362,1.74509,-1.67004,0.019235,1.64968,-1.67434,0.084235,1.58357,-1.6574,0.249276,1.93662,-2.0976,0.292977,1.85862,-1.92362,0.104647,1.89298,-2.04592,0.150405,1.71563,-2.23107,0.202984,1.62422,-2.25666,0.324804,1.63589,-2.09962,0.385326,1.62469,-2.07675,0.192485,1.44272,-1.83196,0.011973,1.50499,-1.88992,0.017495,1.52705,-1.95546,0.021828,1.50233,-2.00319,0.20078,1.41181,-2.08156,0.292455,1.17626,-1 [...]
+/*395*/{0.175981,1.90609,-1.72009,0.276989,1.85265,-1.85126,0.015683,1.81298,-1.67798,-0.107864,1.73975,-1.66719,0.023882,1.6449,-1.67221,0.089545,1.57918,-1.6551,0.249922,1.93506,-2.09687,0.293475,1.85708,-1.92349,0.105511,1.89199,-2.04476,0.147864,1.71484,-2.22921,0.200139,1.62284,-2.25551,0.325264,1.63072,-2.10105,0.385218,1.61781,-2.07808,0.188976,1.43852,-1.83096,0.010811,1.50337,-1.89061,0.01686,1.52566,-1.95559,0.022149,1.50147,-2.0026,0.201138,1.4118,-2.08189,0.28723,1.16964,-1.7 [...]
+/*396*/{0.177216,1.90377,-1.71926,0.277858,1.8514,-1.85123,0.018123,1.80848,-1.67613,-0.103319,1.73417,-1.66453,0.028176,1.63995,-1.66919,0.094398,1.57415,-1.65315,0.249349,1.93267,-2.0961,0.293976,1.85592,-1.92351,0.105929,1.89163,-2.04415,0.144614,1.71406,-2.2284,0.197037,1.62178,-2.25441,0.325209,1.62615,-2.10217,0.38434,1.61058,-2.07988,0.191687,1.44008,-1.83212,0.012541,1.50197,-1.88945,0.01673,1.52368,-1.95532,0.022268,1.49916,-2.00277,0.201676,1.41166,-2.08202,0.28421,1.16297,-1.7 [...]
+/*397*/{0.17945,1.90108,-1.71821,0.27833,1.84943,-1.8514,0.020865,1.80399,-1.67356,-0.099312,1.72744,-1.66162,0.032895,1.63414,-1.66657,0.099632,1.56885,-1.65083,0.249592,1.93061,-2.09599,0.293838,1.85387,-1.92359,0.105482,1.89115,-2.04275,0.140963,1.71299,-2.22677,0.193451,1.62052,-2.2532,0.324647,1.62007,-2.10336,0.383191,1.6028,-2.08128,0.190572,1.43742,-1.83208,0.010603,1.49815,-1.89003,0.017069,1.52168,-1.95519,0.02196,1.49789,-2.00281,0.202502,1.41081,-2.08216,0.280733,1.15678,-1.7 [...]
+/*398*/{0.181113,1.89828,-1.71714,0.278989,1.84659,-1.8513,0.023709,1.79894,-1.6704,-0.095904,1.72036,-1.65783,0.038107,1.62812,-1.6637,0.104689,1.56322,-1.64797,0.249809,1.9281,-2.09549,0.294245,1.8515,-1.92336,0.105831,1.88922,-2.04169,0.137,1.71158,-2.22491,0.189269,1.61903,-2.25211,0.323377,1.61331,-2.10418,0.382269,1.59466,-2.08357,0.191741,1.43616,-1.83212,0.011651,1.49551,-1.88897,0.018208,1.51881,-1.95496,0.023754,1.49586,-2.00217,0.20313,1.40897,-2.08196,0.275789,1.15119,-1.7592 [...]
+/*399*/{0.182629,1.89478,-1.71612,0.279482,1.84439,-1.85144,0.026409,1.79406,-1.66922,-0.091195,1.71259,-1.65358,0.043406,1.62151,-1.65991,0.111095,1.55828,-1.6454,0.249017,1.92478,-2.09508,0.293803,1.84959,-1.92314,0.105897,1.88802,-2.04086,0.133167,1.71049,-2.22349,0.185054,1.61717,-2.25096,0.321098,1.60516,-2.10473,0.380856,1.58667,-2.08631,0.191894,1.43308,-1.8323,0.011393,1.49312,-1.8893,0.018594,1.51568,-1.95473,0.023488,1.49154,-2.00238,0.204321,1.40733,-2.08137,0.272936,1.14659,- [...]
+/*400*/{0.184516,1.89077,-1.71511,0.278696,1.84224,-1.85155,0.029058,1.78856,-1.66692,-0.085892,1.70534,-1.64885,0.048623,1.61522,-1.65613,0.117344,1.55251,-1.64232,0.248377,1.92199,-2.09515,0.293794,1.84634,-1.92342,0.105173,1.88493,-2.03975,0.128589,1.70853,-2.22136,0.180642,1.6148,-2.25007,0.318822,1.59776,-2.10685,0.37841,1.57659,-2.08903,0.192573,1.42943,-1.83201,0.011885,1.49006,-1.88928,0.018503,1.51196,-1.95511,0.023103,1.48887,-2.00302,0.205319,1.40508,-2.08085,0.272938,1.14183, [...]
+/*401*/{0.18614,1.88666,-1.71443,0.279907,1.83918,-1.85174,0.032616,1.7825,-1.66416,-0.081703,1.69677,-1.64377,0.054873,1.60875,-1.65235,0.123283,1.54669,-1.63989,0.247306,1.91866,-2.09552,0.294186,1.84332,-1.92317,0.104152,1.88229,-2.03813,0.12404,1.70642,-2.21949,0.176079,1.61196,-2.24907,0.316862,1.59019,-2.10829,0.375833,1.56718,-2.09241,0.191709,1.42598,-1.83086,0.012423,1.48554,-1.88948,0.017781,1.5091,-1.95519,0.024339,1.48522,-2.00335,0.206726,1.40303,-2.08026,0.2703,1.1389,-1.75 [...]
+/*402*/{0.187546,1.88182,-1.71314,0.279571,1.83522,-1.85226,0.035914,1.77549,-1.66064,-0.076277,1.68878,-1.63879,0.059982,1.60133,-1.64834,0.129805,1.54121,-1.63773,0.246019,1.91522,-2.09553,0.294277,1.84002,-1.92328,0.103912,1.87881,-2.03688,0.119101,1.70429,-2.21746,0.170895,1.60927,-2.24816,0.313494,1.58211,-2.1101,0.373213,1.55669,-2.09549,0.193188,1.4223,-1.83072,0.011497,1.48249,-1.88958,0.019176,1.50602,-1.95522,0.024008,1.48116,-2.00412,0.207366,1.40045,-2.0796,0.269402,1.13598,- [...]
+/*403*/{0.189264,1.87756,-1.71243,0.280764,1.83112,-1.85182,0.03932,1.76854,-1.65765,-0.071134,1.68058,-1.63342,0.066558,1.59441,-1.64445,0.136927,1.53557,-1.63425,0.245128,1.91162,-2.09624,0.29415,1.83663,-1.92305,0.103054,1.87531,-2.03539,0.114519,1.70148,-2.21622,0.165835,1.60614,-2.24671,0.310442,1.57369,-2.11225,0.369392,1.54645,-2.09915,0.192275,1.41836,-1.82915,0.011892,1.47874,-1.88944,0.01927,1.5048,-1.95528,0.024379,1.47684,-2.00451,0.208412,1.39729,-2.07908,0.267433,1.13307,-1 [...]
+/*404*/{0.190842,1.8726,-1.7119,0.281692,1.8275,-1.85225,0.042652,1.76139,-1.65388,-0.066036,1.67175,-1.62814,0.071845,1.58706,-1.6402,0.144391,1.52987,-1.63185,0.242643,1.90732,-2.09622,0.294256,1.83291,-1.92349,0.10207,1.87076,-2.03353,0.109438,1.69819,-2.21461,0.159932,1.60268,-2.24658,0.306377,1.56416,-2.11492,0.366162,1.53558,-2.10332,0.193178,1.41703,-1.82862,0.011736,1.4742,-1.89007,0.018612,1.49794,-1.95457,0.024531,1.4723,-2.00477,0.20875,1.39404,-2.07873,0.266917,1.13065,-1.753 [...]
+/*405*/{0.192861,1.86712,-1.71049,0.281143,1.82335,-1.85213,0.046613,1.7535,-1.64991,-0.06142,1.66274,-1.62207,0.078852,1.57975,-1.63537,0.151293,1.52407,-1.62995,0.240929,1.90324,-2.0976,0.294678,1.82925,-1.92376,0.101592,1.86699,-2.03304,0.104723,1.69301,-2.21285,0.154561,1.59885,-2.24564,0.303286,1.55607,-2.11778,0.361089,1.52419,-2.10788,0.191194,1.41022,-1.82772,0.010653,1.47,-1.89061,0.018493,1.49403,-1.95445,0.024808,1.46734,-2.00509,0.210042,1.39123,-2.07671,0.26542,1.12642,-1.75 [...]
+/*406*/{0.194667,1.86163,-1.70995,0.28256,1.81791,-1.85229,0.050899,1.74516,-1.64602,-0.055871,1.65318,-1.6162,0.085721,1.57257,-1.63168,0.159771,1.5188,-1.62725,0.239585,1.89843,-2.09775,0.294371,1.82514,-1.92418,0.100169,1.86187,-2.03223,0.100793,1.68946,-2.21109,0.149062,1.59456,-2.24493,0.298562,1.54531,-2.12031,0.357017,1.51215,-2.11216,0.192283,1.40699,-1.82656,0.011207,1.46534,-1.89032,0.019125,1.49008,-1.954,0.02507,1.46252,-2.00562,0.210049,1.38729,-2.07721,0.264158,1.12404,-1.7 [...]
+/*407*/{0.195491,1.85628,-1.70922,0.282625,1.81331,-1.85224,0.055291,1.73664,-1.64228,-0.049836,1.64349,-1.6101,0.092723,1.56564,-1.62766,0.167936,1.51355,-1.6253,0.238205,1.89398,-2.0984,0.294876,1.82059,-1.92464,0.098568,1.85757,-2.03008,0.095339,1.68509,-2.21005,0.143026,1.59042,-2.24488,0.293712,1.53509,-2.12354,0.35208,1.50002,-2.11671,0.192619,1.40201,-1.82488,0.011466,1.46023,-1.8907,0.017914,1.48574,-1.95373,0.024161,1.45826,-2.00576,0.210582,1.38361,-2.07615,0.264034,1.11918,-1. [...]
+/*408*/{0.1977,1.85081,-1.70803,0.282123,1.80944,-1.85293,0.060373,1.72819,-1.63835,-0.042007,1.63434,-1.60349,0.100733,1.5589,-1.62357,0.176232,1.5084,-1.62261,0.235423,1.8892,-2.09865,0.294564,1.81608,-1.92498,0.097663,1.85168,-2.02904,0.090372,1.68121,-2.20879,0.136725,1.58625,-2.24429,0.289024,1.52559,-2.12736,0.344802,1.48892,-2.122,0.19166,1.39772,-1.82327,0.009307,1.45582,-1.89103,0.017802,1.48096,-1.95389,0.023696,1.45274,-2.00641,0.211109,1.37991,-2.07502,0.262158,1.11472,-1.751 [...]
+/*409*/{0.198587,1.84585,-1.70789,0.282402,1.80479,-1.85334,0.065335,1.71913,-1.63412,-0.035297,1.62411,-1.59748,0.107949,1.55203,-1.62112,0.185802,1.50373,-1.62055,0.233908,1.88493,-2.0993,0.294418,1.81218,-1.92628,0.096265,1.84694,-2.02774,0.085114,1.67825,-2.20877,0.130503,1.58188,-2.2435,0.284222,1.51451,-2.13143,0.338975,1.47639,-2.12748,0.190914,1.39375,-1.82127,0.008026,1.45154,-1.8926,0.016113,1.47566,-1.95456,0.02307,1.4476,-2.0072,0.210996,1.37696,-2.07525,0.261691,1.10947,-1.7 [...]
+/*410*/{0.200693,1.84116,-1.70685,0.284006,1.79779,-1.85183,0.070575,1.71015,-1.63045,-0.028329,1.61464,-1.59109,0.116833,1.54659,-1.61762,0.194325,1.49897,-1.61905,0.232701,1.88106,-2.10009,0.295102,1.80713,-1.92678,0.095255,1.84165,-2.0255,0.080905,1.67253,-2.20711,0.124056,1.57799,-2.24379,0.278423,1.50398,-2.13596,0.331323,1.46416,-2.13286,0.188403,1.39006,-1.82037,0.004986,1.44829,-1.89304,0.013891,1.47087,-1.95536,0.0216,1.44221,-2.00789,0.211142,1.37214,-2.07336,0.259116,1.10781,- [...]
+/*411*/{0.201884,1.83614,-1.70595,0.286157,1.79268,-1.85266,0.076992,1.70108,-1.62694,-0.019242,1.6047,-1.58486,0.125763,1.53925,-1.61376,0.203585,1.49487,-1.61697,0.231231,1.87702,-2.10144,0.295697,1.80182,-1.92707,0.094091,1.83804,-2.02357,0.07629,1.66911,-2.20615,0.11705,1.57395,-2.24354,0.27171,1.49272,-2.14135,0.32437,1.45256,-2.13828,0.186577,1.38678,-1.8194,0.001306,1.4446,-1.89378,0.011573,1.46522,-1.95676,0.019534,1.43785,-2.00915,0.208415,1.36776,-2.073,0.252135,1.10752,-1.7460 [...]
+/*412*/{0.203971,1.83215,-1.7049,0.286585,1.78876,-1.85295,0.084156,1.69214,-1.62346,-0.010568,1.59499,-1.57856,0.134305,1.53429,-1.6123,0.213798,1.49116,-1.61585,0.230554,1.87365,-2.10246,0.29713,1.79829,-1.92837,0.093868,1.83513,-2.02054,0.071914,1.66481,-2.20574,0.110648,1.57017,-2.24301,0.265443,1.48342,-2.14663,0.31628,1.4412,-2.14375,0.183772,1.38412,-1.81809,-0.002591,1.44056,-1.89613,0.008903,1.46119,-1.95793,0.017268,1.43301,-2.01021,0.209851,1.36367,-2.06945,0.253152,1.10627,-1 [...]
+/*413*/{0.205749,1.8276,-1.7036,0.286217,1.78411,-1.85311,0.091022,1.68441,-1.62052,-0.000471,1.58638,-1.5729,0.144462,1.52925,-1.61005,0.224405,1.48815,-1.61446,0.229445,1.87066,-2.10419,0.297822,1.79473,-1.92992,0.093274,1.83396,-2.01748,0.06762,1.66174,-2.20504,0.103852,1.56673,-2.24183,0.258166,1.47446,-2.15104,0.307536,1.43104,-2.14832,0.181165,1.38176,-1.81566,-0.005451,1.43681,-1.89772,0.006066,1.45753,-1.95878,0.013715,1.4291,-2.01209,0.209226,1.36095,-2.06674,0.248467,1.10652,-1 [...]
+/*414*/{0.206444,1.82486,-1.70308,0.287047,1.77986,-1.85302,0.097537,1.67761,-1.6196,0.010743,1.5784,-1.5672,0.155226,1.52549,-1.60753,0.235041,1.48595,-1.61395,0.228844,1.86838,-2.10495,0.29854,1.7914,-1.93085,0.092838,1.83388,-2.01505,0.064989,1.66018,-2.20437,0.097447,1.56453,-2.2407,0.250081,1.4668,-2.15424,0.298035,1.42256,-2.15412,0.177616,1.38074,-1.813,-0.007606,1.43376,-1.90071,0.003936,1.45577,-1.95935,0.011108,1.42393,-2.01439,0.20726,1.35813,-2.0663,0.245958,1.10834,-1.7361,0 [...]
+/*415*/{0.206362,1.82285,-1.70252,0.286962,1.77851,-1.85441,0.104883,1.67388,-1.61741,0.019782,1.57179,-1.56271,0.165422,1.52271,-1.60626,0.246261,1.48521,-1.61297,0.229695,1.86668,-2.10531,0.298936,1.7882,-1.93139,0.093213,1.83359,-2.01453,0.061978,1.66032,-2.20158,0.091287,1.56459,-2.23961,0.24185,1.4621,-2.15576,0.288202,1.41449,-2.15901,0.17586,1.38118,-1.80901,-0.010199,1.43152,-1.90325,0.002844,1.4534,-1.96019,0.007883,1.41882,-2.01717,0.205791,1.3566,-2.06492,0.242661,1.10774,-1.7 [...]
+/*416*/{0.206593,1.82165,-1.70256,0.287198,1.77745,-1.85523,0.109587,1.67108,-1.61749,0.031622,1.56727,-1.55936,0.176985,1.52093,-1.60649,0.257398,1.4847,-1.61266,0.230444,1.86463,-2.10533,0.299042,1.78617,-1.93314,0.09381,1.8338,-2.01442,0.056657,1.66184,-2.1983,0.083697,1.56615,-2.23769,0.233064,1.45838,-2.15828,0.277717,1.40847,-2.16329,0.172124,1.38027,-1.80565,-0.012258,1.43001,-1.90493,0.001441,1.45316,-1.96066,0.005962,1.41604,-2.01879,0.203572,1.35437,-2.0638,0.238339,1.10612,-1. [...]
+/*417*/{0.207055,1.82096,-1.70344,0.286665,1.77671,-1.85622,0.115792,1.66968,-1.61613,0.043236,1.56371,-1.55626,0.18835,1.51975,-1.60573,0.269956,1.4857,-1.61306,0.230861,1.86337,-2.10485,0.298364,1.78481,-1.93317,0.094006,1.83405,-2.01461,0.05173,1.66494,-2.19526,0.076882,1.56932,-2.23634,0.224084,1.45568,-2.16021,0.267506,1.40444,-2.16703,0.169719,1.38007,-1.80252,-0.013301,1.4295,-1.90494,0.000914,1.45299,-1.96014,0.005219,1.41586,-2.0189,0.202845,1.35371,-2.06252,0.233436,1.1036,-1.7 [...]
+/*418*/{0.20868,1.82071,-1.70399,0.286967,1.77419,-1.85644,0.121876,1.66935,-1.61544,0.05415,1.5612,-1.55298,0.199681,1.51991,-1.6061,0.281291,1.48765,-1.61399,0.231689,1.8626,-2.10461,0.298202,1.78369,-1.93326,0.094324,1.83426,-2.01574,0.046953,1.66936,-2.19119,0.069754,1.57294,-2.23474,0.215366,1.45532,-2.16244,0.25653,1.40199,-2.17092,0.166639,1.38052,-1.79983,-0.014762,1.42982,-1.90649,0.00038,1.45458,-1.96027,0.003077,1.41708,-2.01935,0.201516,1.35389,-2.06155,0.229526,1.09861,-1.73 [...]
+/*419*/{0.209904,1.82123,-1.70514,0.286941,1.77339,-1.85667,0.127608,1.66908,-1.61417,0.063923,1.55986,-1.55129,0.210862,1.52151,-1.60612,0.293105,1.49128,-1.61501,0.232766,1.86229,-2.10427,0.298163,1.78279,-1.93345,0.094027,1.83474,-2.01641,0.041741,1.67494,-2.18747,0.062399,1.57744,-2.23349,0.206591,1.45566,-2.1647,0.246403,1.40111,-2.17372,0.16454,1.38101,-1.7972,-0.015753,1.43017,-1.9054,-0.001575,1.45455,-1.95913,0.003014,1.4184,-2.01882,0.200154,1.35433,-2.06103,0.222243,1.09511,-1 [...]
+/*420*/{0.211162,1.82199,-1.70585,0.286537,1.77301,-1.8573,0.132985,1.66927,-1.61338,0.074052,1.55963,-1.54892,0.221186,1.52371,-1.60597,0.305275,1.4956,-1.61604,0.232497,1.8624,-2.10446,0.296952,1.78234,-1.93371,0.094164,1.83579,-2.017,0.036753,1.68063,-2.1837,0.054724,1.58284,-2.23234,0.197468,1.45578,-2.16675,0.236114,1.40158,-2.17651,0.163639,1.38152,-1.79603,-0.016733,1.43113,-1.90544,-0.003212,1.45507,-1.95912,0.002091,1.41976,-2.0175,0.197725,1.35506,-2.06023,0.220443,1.09283,-1.7 [...]
+/*421*/{0.212694,1.82436,-1.70633,0.285365,1.7745,-1.85864,0.137437,1.67072,-1.6119,0.083593,1.55941,-1.54693,0.23121,1.52735,-1.60654,0.316077,1.50156,-1.61869,0.231702,1.86314,-2.10455,0.296438,1.78265,-1.93423,0.093959,1.83748,-2.0177,0.03216,1.68722,-2.18063,0.046517,1.58866,-2.23167,0.189376,1.45985,-2.16913,0.225358,1.40361,-2.17913,0.160949,1.38243,-1.79421,-0.016347,1.4342,-1.905,-0.003763,1.45551,-1.9573,0.000958,1.42148,-2.0165,0.196161,1.35737,-2.05997,0.2128,1.0877,-1.73337,0 [...]
+/*422*/{0.21439,1.8267,-1.70697,0.284902,1.77465,-1.85889,0.141779,1.67267,-1.6109,0.092059,1.56,-1.5451,0.240447,1.53174,-1.60652,0.326708,1.50812,-1.62041,0.231548,1.8641,-2.10508,0.295791,1.78345,-1.93472,0.09366,1.83906,-2.01748,0.027885,1.69436,-2.17806,0.039171,1.5951,-2.23109,0.180308,1.46278,-2.17092,0.215344,1.40636,-2.1814,0.159502,1.38353,-1.7923,-0.017657,1.43269,-1.90352,-0.004727,1.45607,-1.95732,-0.000653,1.42236,-2.01538,0.195438,1.35948,-2.0595,0.206015,1.08412,-1.73485, [...]
+/*423*/{0.2164,1.82981,-1.70803,0.286104,1.77622,-1.86068,0.145687,1.67549,-1.61058,0.100855,1.56057,-1.54329,0.249626,1.53665,-1.60749,0.335749,1.51514,-1.62285,0.230084,1.86554,-2.10532,0.29523,1.78427,-1.93572,0.09253,1.84159,-2.01787,0.022051,1.70111,-2.17541,0.031061,1.6019,-2.23097,0.171611,1.46809,-2.17318,0.20573,1.4111,-2.18335,0.158064,1.38398,-1.79142,-0.018236,1.43394,-1.90206,-0.005144,1.45673,-1.9559,2.6e-005,1.42542,-2.01325,0.194179,1.36201,-2.05929,0.202935,1.08328,-1.73 [...]
+/*424*/{0.218852,1.83231,-1.70842,0.286406,1.77812,-1.86166,0.151136,1.67868,-1.61064,0.109481,1.56288,-1.54207,0.258497,1.54252,-1.60782,0.345458,1.52268,-1.62586,0.22808,1.86727,-2.10533,0.29462,1.78608,-1.93717,0.091558,1.84407,-2.01748,0.016674,1.70888,-2.17401,0.02266,1.60909,-2.23023,0.163068,1.4734,-2.17472,0.195757,1.41663,-2.18479,0.157898,1.38424,-1.79139,-0.018988,1.43482,-1.90184,-0.005918,1.45847,-1.95518,-0.002804,1.4268,-2.0122,0.193474,1.36378,-2.05884,0.194954,1.08002,-1 [...]
+/*425*/{0.221911,1.83577,-1.70905,0.286569,1.77984,-1.86273,0.156255,1.68166,-1.61013,0.118788,1.56556,-1.54173,0.266272,1.54797,-1.60927,0.353408,1.53095,-1.62833,0.226266,1.86978,-2.10623,0.294063,1.78752,-1.93841,0.09054,1.84655,-2.01569,0.011213,1.71553,-2.17288,0.0148,1.61653,-2.22999,0.154248,1.47939,-2.17572,0.186647,1.42334,-2.18586,0.157804,1.38357,-1.79111,-0.018954,1.43703,-1.9009,-0.006721,1.45972,-1.954,-0.003529,1.42814,-2.01117,0.192601,1.36619,-2.05907,0.189564,1.07892,-1 [...]
+/*426*/{0.225094,1.83932,-1.71016,0.287894,1.78306,-1.86385,0.161073,1.68501,-1.61034,0.12696,1.56837,-1.54075,0.274187,1.55437,-1.61065,0.360621,1.53914,-1.63177,0.223053,1.8726,-2.10679,0.293755,1.79005,-1.94004,0.089252,1.84894,-2.01499,0.00534,1.72257,-2.17164,0.006897,1.62392,-2.22941,0.146303,1.48667,-2.17669,0.178283,1.43007,-2.18683,0.158543,1.3843,-1.79135,-0.01915,1.4386,-1.89985,-0.006976,1.46193,-1.95341,-0.005041,1.43017,-2.01037,0.192143,1.36939,-2.05976,0.181856,1.07752,-1 [...]
+/*427*/{0.228214,1.84293,-1.71129,0.287547,1.78559,-1.86541,0.166025,1.6891,-1.61039,0.134073,1.5712,-1.54031,0.28054,1.56099,-1.61238,0.367559,1.54845,-1.63483,0.220242,1.8757,-2.10732,0.293139,1.79316,-1.94195,0.08766,1.85081,-2.01378,-0.001304,1.72884,-2.16899,-0.000109,1.63125,-2.22942,0.136955,1.49362,-2.17673,0.170032,1.4375,-2.18749,0.159544,1.38453,-1.79159,-0.018637,1.44121,-1.89816,-0.007055,1.46392,-1.95173,-0.005072,1.43184,-2.00975,0.190651,1.37098,-2.05965,0.175248,1.07714, [...]
+/*428*/{0.231088,1.84741,-1.71256,0.287935,1.78946,-1.86684,0.171991,1.69289,-1.61071,0.14243,1.57487,-1.5399,0.287608,1.56785,-1.61415,0.3738,1.55687,-1.63836,0.217318,1.87983,-2.10779,0.292647,1.7961,-1.94365,0.086061,1.85449,-2.01196,-0.00764,1.73608,-2.16853,-0.00834,1.6385,-2.22866,0.12984,1.50157,-2.17746,0.161793,1.44565,-2.18822,0.160827,1.38547,-1.79265,-0.018645,1.44358,-1.89626,-0.006503,1.46623,-1.95054,-0.006206,1.43423,-2.00864,0.189927,1.37382,-2.06012,0.171211,1.07656,-1. [...]
+/*429*/{0.233639,1.85164,-1.71408,0.289607,1.7924,-1.8683,0.177597,1.69691,-1.61138,0.149728,1.57892,-1.53928,0.294202,1.57549,-1.61651,0.37988,1.56597,-1.64216,0.213472,1.88386,-2.10786,0.292137,1.7998,-1.94547,0.084464,1.85588,-2.01099,-0.01374,1.74272,-2.16742,-0.01572,1.64605,-2.228,0.123314,1.50975,-2.17778,0.154452,1.45371,-2.18905,0.161729,1.38612,-1.79364,-0.017569,1.44746,-1.89479,-0.007108,1.46976,-1.94862,-0.004926,1.43624,-2.00663,0.189353,1.37694,-2.06017,0.162313,1.07466,-1 [...]
+/*430*/{0.236459,1.8563,-1.71573,0.290656,1.79612,-1.87027,0.183298,1.70075,-1.61193,0.156392,1.58289,-1.53914,0.299649,1.58286,-1.61825,0.385389,1.57553,-1.64534,0.210045,1.88757,-2.10866,0.291505,1.80311,-1.94752,0.082232,1.85962,-2.00847,-0.019681,1.74941,-2.16632,-0.022956,1.65337,-2.22727,0.114198,1.51797,-2.17802,0.147758,1.46177,-2.18967,0.162763,1.3867,-1.79349,-0.017029,1.44983,-1.89356,-0.006587,1.47228,-1.947,-0.006042,1.43992,-2.0059,0.188669,1.38049,-2.06084,0.156332,1.07484 [...]
+/*431*/{0.23954,1.8612,-1.71724,0.290739,1.80017,-1.87219,0.190167,1.70477,-1.61187,0.162405,1.58725,-1.53939,0.304446,1.59115,-1.62108,0.389906,1.5849,-1.64926,0.206539,1.89169,-2.10888,0.291789,1.80664,-1.94983,0.080914,1.86287,-2.00629,-0.02401,1.75651,-2.16534,-0.029917,1.66081,-2.22622,0.108626,1.52603,-2.17724,0.140618,1.4704,-2.19052,0.165197,1.38756,-1.79394,-0.016213,1.4535,-1.89101,-0.006452,1.4761,-1.94565,-0.00555,1.44261,-2.00467,0.188397,1.38433,-2.06072,0.148691,1.07591,-1 [...]
+/*432*/{0.241924,1.86655,-1.71874,0.291087,1.80402,-1.87424,0.195537,1.70935,-1.61263,0.169628,1.59155,-1.53955,0.309434,1.59812,-1.62388,0.394401,1.59368,-1.65303,0.203082,1.89651,-2.10882,0.291682,1.81115,-1.95185,0.079983,1.86582,-2.00531,-0.030011,1.76201,-2.16375,-0.036732,1.66801,-2.22522,0.1009,1.53445,-2.17819,0.133889,1.47836,-2.19128,0.16602,1.38875,-1.79411,-0.014828,1.45671,-1.88961,-0.005467,1.47847,-1.94351,-0.005252,1.44657,-2.00331,0.18763,1.38769,-2.06046,0.142657,1.0748 [...]
+/*433*/{0.243877,1.87198,-1.72013,0.291636,1.80871,-1.87615,0.20139,1.71464,-1.61449,0.175834,1.59742,-1.54017,0.313746,1.60559,-1.62613,0.398871,1.60334,-1.65693,0.199139,1.9007,-2.10898,0.291026,1.81561,-1.95388,0.07864,1.8687,-2.00275,-0.035034,1.77,-2.16272,-0.042722,1.67539,-2.22422,0.094227,1.54308,-2.1772,0.127502,1.48661,-2.19151,0.168595,1.38987,-1.79407,-0.014151,1.46005,-1.88837,-0.005106,1.48248,-1.94114,-0.004874,1.45084,-2.00196,0.187805,1.39137,-2.06037,0.137188,1.07566,-1 [...]
+/*434*/{0.246349,1.87742,-1.72271,0.292887,1.81358,-1.8782,0.206875,1.71997,-1.61559,0.181905,1.60215,-1.53963,0.317412,1.61368,-1.6292,0.401195,1.6121,-1.66092,0.196259,1.90547,-2.10911,0.290609,1.82006,-1.95616,0.077779,1.87224,-2.00167,-0.038402,1.77594,-2.16275,-0.048932,1.68256,-2.22232,0.088549,1.54967,-2.17614,0.12187,1.49477,-2.19206,0.169678,1.39118,-1.79366,-0.011978,1.4637,-1.88667,-0.003524,1.48639,-1.93925,-0.005345,1.45576,-2.00059,0.187554,1.39609,-2.0601,0.130574,1.07689, [...]
+/*435*/{0.248557,1.88298,-1.7238,0.294388,1.81845,-1.88043,0.212842,1.7257,-1.61603,0.187987,1.60743,-1.53975,0.320797,1.62106,-1.63148,0.403986,1.62121,-1.66489,0.193334,1.90969,-2.10907,0.290349,1.82478,-1.95806,0.076186,1.87559,-1.99996,-0.042759,1.78291,-2.16229,-0.054916,1.68981,-2.22065,0.082463,1.55776,-2.17597,0.11611,1.50271,-2.19224,0.172195,1.39294,-1.79441,-0.010037,1.46661,-1.88506,-0.002946,1.48962,-1.93688,-0.004867,1.46002,-1.99939,0.187475,1.40134,-2.06017,0.124264,1.079 [...]
+/*436*/{0.251,1.88818,-1.72601,0.295511,1.82262,-1.88317,0.217081,1.73069,-1.61736,0.193381,1.61281,-1.54004,0.323966,1.62849,-1.63376,0.406885,1.63026,-1.66879,0.190912,1.91434,-2.10909,0.290188,1.82936,-1.95981,0.0759,1.87938,-1.99917,-0.046484,1.78903,-2.16082,-0.059977,1.69657,-2.21873,0.07765,1.56526,-2.17449,0.111233,1.51055,-2.19262,0.17364,1.39449,-1.79417,-0.008277,1.46985,-1.88384,-0.001335,1.49352,-1.93475,-0.004107,1.46472,-1.99869,0.188846,1.40655,-2.05973,0.118998,1.08055,- [...]
+/*437*/{0.252099,1.89333,-1.72779,0.296015,1.8279,-1.88506,0.22142,1.73635,-1.61816,0.197622,1.61891,-1.54051,0.326454,1.63649,-1.63661,0.408811,1.63862,-1.67262,0.188663,1.91865,-2.10971,0.290084,1.83427,-1.96152,0.074063,1.8835,-1.9979,-0.049412,1.79492,-2.16013,-0.06475,1.70361,-2.21669,0.072245,1.57287,-2.17385,0.106654,1.51769,-2.19296,0.176086,1.3964,-1.79461,-0.007204,1.4734,-1.88258,-0.000607,1.49723,-1.93286,-0.002837,1.47011,-1.99793,0.188807,1.4122,-2.05961,0.114643,1.0828,-1. [...]
+/*438*/{0.254116,1.89882,-1.73018,0.296883,1.83243,-1.88698,0.224927,1.74182,-1.61906,0.202659,1.62493,-1.54129,0.328792,1.64294,-1.63845,0.410162,1.64688,-1.6762,0.187125,1.92267,-2.10965,0.290065,1.83888,-1.96314,0.075272,1.88613,-1.99615,-0.052086,1.79998,-2.15773,-0.069017,1.70986,-2.2146,0.068962,1.58002,-2.17246,0.101856,1.52456,-2.1933,0.177652,1.39835,-1.79518,-0.00407,1.47723,-1.88161,0.000285,1.50122,-1.93016,-0.001635,1.47496,-1.99675,0.188104,1.41743,-2.05938,0.109538,1.08537 [...]
+/*439*/{0.255524,1.90423,-1.73152,0.297149,1.83743,-1.8889,0.22873,1.74707,-1.61976,0.206626,1.63014,-1.54091,0.330348,1.6504,-1.64186,0.411914,1.65404,-1.68022,0.185418,1.92663,-2.10981,0.289686,1.84407,-1.9645,0.07518,1.88988,-1.99576,-0.055231,1.80521,-2.1559,-0.073026,1.71633,-2.21224,0.064714,1.58618,-2.17215,0.098534,1.53127,-2.19295,0.180992,1.39984,-1.79572,-0.001979,1.48045,-1.87976,0.002203,1.50616,-1.92851,-0.000741,1.48017,-1.99603,0.188906,1.42285,-2.05961,0.102689,1.08965,- [...]
+/*440*/{0.257721,1.90922,-1.73288,0.297751,1.84229,-1.89076,0.231377,1.75202,-1.62041,0.210093,1.63564,-1.54189,0.332415,1.65716,-1.64347,0.413104,1.66162,-1.68386,0.183663,1.9305,-2.11022,0.289643,1.84799,-1.96606,0.074495,1.89319,-1.9956,-0.056248,1.8112,-2.15403,-0.076913,1.72231,-2.21038,0.061195,1.5922,-2.1708,0.094488,1.53713,-2.1933,0.182393,1.40164,-1.79681,-0.000761,1.48379,-1.87891,0.003608,1.50955,-1.9262,0.001188,1.48476,-1.99529,0.188412,1.42856,-2.05965,0.100259,1.09301,-1. [...]
+/*441*/{0.259092,1.91372,-1.7346,0.298337,1.84642,-1.89255,0.23423,1.75728,-1.62156,0.21362,1.64007,-1.54164,0.333693,1.66323,-1.64497,0.414609,1.66869,-1.68606,0.182363,1.93422,-2.11032,0.288984,1.85242,-1.96789,0.073469,1.89694,-1.99567,-0.058577,1.81678,-2.15236,-0.079897,1.72777,-2.20774,0.058473,1.59818,-2.17007,0.091394,1.54279,-2.19314,0.184773,1.40373,-1.7981,0.001283,1.48688,-1.87738,0.005109,1.51318,-1.92395,0.002356,1.48944,-1.99399,0.189096,1.43365,-2.0599,0.093353,1.0946,-1. [...]
+/*442*/{0.260451,1.9181,-1.73636,0.298334,1.85036,-1.89466,0.237013,1.7623,-1.62176,0.21744,1.6458,-1.54173,0.335307,1.66936,-1.64742,0.415499,1.67515,-1.68978,0.180664,1.93714,-2.11058,0.289164,1.85687,-1.96918,0.073587,1.90006,-1.99543,-0.060282,1.8216,-2.15025,-0.082698,1.73345,-2.20582,0.055595,1.60294,-2.16905,0.088947,1.54789,-2.19305,0.187651,1.4058,-1.79894,0.003945,1.49013,-1.8764,0.006069,1.5171,-1.92229,0.003718,1.49499,-1.99275,0.189481,1.43887,-2.06008,0.090392,1.0977,-1.768 [...]
+/*443*/{0.262478,1.92215,-1.73773,0.29958,1.85563,-1.89658,0.238915,1.76713,-1.62222,0.221452,1.65052,-1.54164,0.336684,1.67456,-1.64937,0.416483,1.68141,-1.69318,0.18025,1.9405,-2.1111,0.288737,1.86066,-1.97053,0.073706,1.90262,-1.9946,-0.061108,1.82528,-2.14705,-0.08513,1.73815,-2.20313,0.053324,1.60758,-2.16882,0.086495,1.55254,-2.19267,0.190244,1.40741,-1.80016,0.005811,1.49407,-1.87519,0.007776,1.5202,-1.92087,0.00557,1.49966,-1.9919,0.18931,1.44399,-2.06079,0.08697,1.1003,-1.76837, [...]
+/*444*/{0.263583,1.92684,-1.73868,0.300712,1.85926,-1.89828,0.241154,1.77151,-1.62302,0.223042,1.65415,-1.54154,0.337338,1.67995,-1.65104,0.416683,1.68755,-1.69535,0.179086,1.94376,-2.11162,0.288746,1.86399,-1.97202,0.074032,1.90606,-1.9941,-0.060584,1.83064,-2.14389,-0.087316,1.74301,-2.20089,0.050708,1.61193,-2.16821,0.083954,1.55705,-2.1925,0.19236,1.4087,-1.80155,0.008476,1.49737,-1.87349,0.010067,1.52425,-1.91996,0.006953,1.50433,-1.99099,0.190073,1.44865,-2.06127,0.083027,1.10286,- [...]
+/*445*/{0.265301,1.92956,-1.74053,0.300892,1.86347,-1.89984,0.242594,1.77607,-1.62357,0.225965,1.65863,-1.5412,0.338094,1.68479,-1.6524,0.417364,1.69297,-1.69891,0.178098,1.94674,-2.11174,0.288713,1.86756,-1.97341,0.074329,1.909,-1.99418,-0.061987,1.83408,-2.14223,-0.089168,1.74714,-2.19832,0.049336,1.61592,-2.16765,0.082282,1.56103,-2.19191,0.194727,1.41074,-1.80256,0.010338,1.50083,-1.87311,0.011742,1.52734,-1.91837,0.007948,1.50932,-1.99027,0.189898,1.45308,-2.06156,0.080293,1.10732,- [...]
+/*446*/{0.267015,1.93362,-1.74211,0.300913,1.86655,-1.90082,0.244977,1.78022,-1.62428,0.227861,1.66204,-1.54155,0.339759,1.68856,-1.65446,0.417253,1.69695,-1.70073,0.17773,1.94948,-2.11157,0.288499,1.8711,-1.97437,0.074576,1.91115,-1.99376,-0.062644,1.83911,-2.14046,-0.090743,1.75094,-2.19614,0.048083,1.61943,-2.1674,0.081062,1.56456,-2.1915,0.196248,1.41249,-1.80353,0.011656,1.50439,-1.87191,0.012635,1.53111,-1.91718,0.009074,1.51453,-1.98987,0.190008,1.45762,-2.06119,0.076292,1.10957,- [...]
+/*447*/{0.268114,1.93688,-1.74334,0.302613,1.87108,-1.90304,0.24634,1.78382,-1.62465,0.230728,1.6667,-1.54145,0.339851,1.69307,-1.65613,0.417552,1.70099,-1.70332,0.176698,1.95223,-2.11199,0.288997,1.87405,-1.97561,0.074542,1.91374,-1.9931,-0.062624,1.84207,-2.13669,-0.092626,1.7544,-2.19386,0.04771,1.62287,-2.16669,0.079895,1.56808,-2.19134,0.198878,1.41495,-1.80474,0.013992,1.50834,-1.87114,0.013714,1.53434,-1.91609,0.012501,1.51746,-1.98771,0.189972,1.46118,-2.06163,0.074364,1.11383,-1 [...]
+/*448*/{0.269868,1.93978,-1.74471,0.301886,1.87401,-1.9039,0.248078,1.78733,-1.62482,0.231607,1.6694,-1.54182,0.341293,1.69611,-1.65709,0.417678,1.70499,-1.70562,0.176677,1.95452,-2.11262,0.289449,1.87694,-1.97673,0.0746,1.91611,-1.99381,-0.063012,1.84584,-2.13653,-0.093992,1.75733,-2.1914,0.047205,1.62624,-2.16613,0.07896,1.57131,-2.1911,0.199623,1.41735,-1.80624,0.015531,1.51082,-1.87067,0.015134,1.53848,-1.91487,0.01364,1.52152,-1.98684,0.190717,1.46409,-2.06178,0.071412,1.11753,-1.76 [...]
+/*449*/{0.270554,1.94214,-1.74523,0.302655,1.87702,-1.90497,0.250081,1.79064,-1.62597,0.234199,1.67304,-1.54154,0.341496,1.69961,-1.65838,0.417815,1.7088,-1.70848,0.176055,1.95673,-2.11294,0.290283,1.8792,-1.97783,0.075663,1.9177,-1.99287,-0.063329,1.84832,-2.13412,-0.095306,1.75988,-2.18922,0.046571,1.62895,-2.16563,0.078314,1.57408,-2.19067,0.202187,1.41942,-1.80676,0.016912,1.51425,-1.86985,0.015608,1.54129,-1.91369,0.015564,1.52462,-1.98525,0.190857,1.46728,-2.06168,0.067846,1.12196, [...]
+/*450*/{0.272975,1.94489,-1.74694,0.303386,1.87981,-1.90586,0.250837,1.79338,-1.62624,0.23507,1.67485,-1.54124,0.341613,1.7023,-1.66096,0.417707,1.71093,-1.71089,0.175724,1.95902,-2.11306,0.290763,1.88144,-1.97914,0.075449,1.92051,-1.99305,-0.063762,1.84983,-2.13066,-0.096191,1.76187,-2.18785,0.045945,1.63168,-2.16502,0.077515,1.57639,-2.19056,0.202594,1.42225,-1.80762,0.017931,1.51799,-1.86861,0.016628,1.54496,-1.91304,0.015047,1.52845,-1.9842,0.191241,1.46958,-2.06175,0.067679,1.12562, [...]
+/*451*/{0.273127,1.94742,-1.74737,0.303505,1.88171,-1.90683,0.25215,1.79626,-1.6273,0.236165,1.67768,-1.54104,0.34234,1.70345,-1.66071,0.418004,1.71283,-1.71242,0.1757,1.96033,-2.11337,0.29111,1.88317,-1.97992,0.076355,1.92093,-1.9926,-0.063834,1.85266,-2.13041,-0.096814,1.76336,-2.18573,0.045781,1.63338,-2.16464,0.077153,1.5784,-2.19038,0.203633,1.42485,-1.80779,0.018911,1.52144,-1.86697,0.017215,1.54749,-1.91257,0.016072,1.53106,-1.98379,0.191311,1.47162,-2.06122,0.066128,1.1291,-1.759 [...]
+/*452*/{0.273692,1.94926,-1.74838,0.304908,1.88465,-1.90849,0.253529,1.79865,-1.62783,0.236968,1.67936,-1.54095,0.342119,1.70559,-1.66074,0.417576,1.71383,-1.71456,0.175734,1.9622,-2.11365,0.291411,1.88466,-1.98099,0.076887,1.92366,-1.99257,-0.064327,1.85348,-2.12805,-0.097836,1.76454,-2.18451,0.045171,1.63501,-2.16455,0.077389,1.57988,-2.19002,0.203848,1.42696,-1.8079,0.020565,1.52377,-1.8665,0.018101,1.55083,-1.91139,0.016793,1.53371,-1.98203,0.192243,1.47412,-2.0609,0.066379,1.13073,- [...]
+/*453*/{0.275663,1.95106,-1.74951,0.304081,1.88624,-1.90912,0.254242,1.80084,-1.62876,0.236695,1.68195,-1.54199,0.342486,1.70705,-1.66292,0.416864,1.71496,-1.71629,0.17624,1.96329,-2.11444,0.291951,1.88581,-1.98178,0.077335,1.925,-1.99266,-0.065272,1.85434,-2.12653,-0.097947,1.76555,-2.18277,0.045256,1.63585,-2.16407,0.077419,1.58089,-2.19017,0.204503,1.42977,-1.80735,0.020924,1.52661,-1.86626,0.017806,1.55344,-1.91193,0.017856,1.53609,-1.98191,0.192158,1.47511,-2.06065,0.06677,1.13259,- [...]
+/*454*/{0.276325,1.95229,-1.75022,0.306259,1.88836,-1.91047,0.254723,1.80256,-1.62908,0.237307,1.6838,-1.5419,0.342657,1.70802,-1.66432,0.416677,1.71532,-1.71802,0.176122,1.96426,-2.11472,0.292306,1.88723,-1.98314,0.077932,1.92637,-1.99277,-0.065584,1.85471,-2.12504,-0.098504,1.76579,-2.1816,0.045485,1.63626,-2.16337,0.077995,1.58164,-2.1902,0.205056,1.43243,-1.80742,0.02154,1.52826,-1.86514,0.017759,1.55522,-1.91244,0.017881,1.53806,-1.98067,0.192925,1.47648,-2.06022,0.069159,1.13406,-1 [...]
+/*455*/{0.276574,1.95318,-1.75129,0.305996,1.88911,-1.91103,0.255053,1.80366,-1.62968,0.238126,1.68478,-1.54198,0.341788,1.70804,-1.66538,0.417728,1.71567,-1.71965,0.176291,1.96527,-2.11552,0.293495,1.88831,-1.98368,0.078337,1.92763,-1.99341,-0.065703,1.85507,-2.12373,-0.099089,1.76555,-2.17998,0.047873,1.63697,-2.16316,0.078395,1.58172,-2.19018,0.205355,1.43427,-1.80692,0.021447,1.5299,-1.86571,0.018418,1.55823,-1.91178,0.018092,1.53911,-1.9803,0.193445,1.47706,-2.05972,0.071676,1.13495 [...]
+/*456*/{0.277124,1.95352,-1.75154,0.306445,1.89003,-1.91257,0.25559,1.80448,-1.63056,0.237432,1.68507,-1.54196,0.341125,1.70751,-1.66651,0.414927,1.71278,-1.72109,0.176415,1.96571,-2.11631,0.294065,1.88911,-1.98393,0.078745,1.92853,-1.9929,-0.065269,1.8538,-2.12312,-0.098725,1.76533,-2.17923,0.047949,1.63636,-2.1627,0.079255,1.58142,-2.1902,0.205264,1.43639,-1.80619,0.022024,1.53141,-1.86502,0.01862,1.5589,-1.91183,0.018185,1.54007,-1.98053,0.193867,1.47798,-2.05912,0.074674,1.1343,-1.76 [...]
+/*457*/{0.278942,1.95418,-1.75211,0.306331,1.89076,-1.91267,0.254882,1.80546,-1.63135,0.236012,1.68587,-1.54303,0.340928,1.70685,-1.667,0.414212,1.71232,-1.72211,0.177369,1.96549,-2.11703,0.293934,1.88978,-1.98539,0.079368,1.92909,-1.99374,-0.065051,1.85284,-2.12123,-0.098189,1.76445,-2.17809,0.048292,1.63604,-2.16248,0.080041,1.58071,-2.19009,0.205759,1.43792,-1.80591,0.021177,1.53237,-1.86467,0.018294,1.5601,-1.91242,0.018185,1.54007,-1.98053,0.195095,1.47824,-2.05828,0.078706,1.13304, [...]
+/*458*/{0.279448,1.95408,-1.75283,0.306236,1.89137,-1.91299,0.254572,1.80531,-1.63188,0.235955,1.68611,-1.54331,0.340177,1.70539,-1.66794,0.413506,1.70925,-1.7232,0.177362,1.96553,-2.11747,0.293929,1.88989,-1.9855,0.080806,1.92943,-1.99435,-0.065198,1.8514,-2.11983,-0.097682,1.76348,-2.17765,0.049502,1.63403,-2.16211,0.081188,1.57946,-2.19019,0.205717,1.43939,-1.80521,0.021177,1.53237,-1.86467,0.018543,1.56053,-1.91272,0.018185,1.54007,-1.98053,0.195897,1.47763,-2.0578,0.078854,1.13287,- [...]
+/*459*/{0.279559,1.9537,-1.75397,0.30572,1.89196,-1.91402,0.253792,1.80532,-1.6328,0.234672,1.68661,-1.54401,0.339561,1.70326,-1.66825,0.412836,1.70687,-1.72395,0.178266,1.96539,-2.1188,0.293961,1.88965,-1.98611,0.081019,1.92966,-1.99499,-0.064704,1.84989,-2.11933,-0.096253,1.76174,-2.17638,0.051126,1.63284,-2.1618,0.082519,1.57787,-2.19014,0.206188,1.43995,-1.80464,0.02114,1.53225,-1.8647,0.018543,1.56053,-1.91272,0.018771,1.53861,-1.98023,0.197022,1.47697,-2.05653,0.090409,1.13225,-1.7 [...]
+/*460*/{0.279169,1.95288,-1.75461,0.305084,1.89116,-1.91443,0.252888,1.80502,-1.63383,0.232735,1.68582,-1.54439,0.338696,1.70165,-1.66973,0.411878,1.70354,-1.7249,0.179048,1.96477,-2.11979,0.29381,1.88969,-1.98634,0.081082,1.9295,-1.99676,-0.062631,1.84817,-2.11874,-0.095355,1.76004,-2.17607,0.053343,1.63059,-2.16138,0.084135,1.57579,-2.19053,0.205987,1.44002,-1.80424,0.021118,1.53209,-1.86516,0.018196,1.55989,-1.91279,0.018812,1.53799,-1.98111,0.197643,1.47654,-2.05612,0.096235,1.13016, [...]
+/*461*/{0.279143,1.95215,-1.75498,0.305166,1.89014,-1.91467,0.251412,1.80473,-1.63444,0.23126,1.68501,-1.54485,0.337602,1.69883,-1.67011,0.410676,1.69993,-1.72549,0.179318,1.96394,-2.12049,0.293591,1.88894,-1.98641,0.081313,1.92912,-1.99727,-0.060707,1.84653,-2.11806,-0.0933,1.75761,-2.1762,0.054727,1.62837,-2.16123,0.085608,1.57304,-2.19047,0.205924,1.44021,-1.80294,0.020614,1.53083,-1.86592,0.018226,1.55885,-1.91276,0.019312,1.53565,-1.98125,0.198452,1.47544,-2.05497,0.102844,1.12745,- [...]
+/*462*/{0.27891,1.95063,-1.75642,0.304717,1.88932,-1.91528,0.25018,1.80343,-1.6356,0.227653,1.68484,-1.54704,0.335462,1.69651,-1.67104,0.409823,1.69586,-1.72616,0.179609,1.9628,-2.12108,0.293926,1.8883,-1.98711,0.081753,1.9289,-1.99838,-0.060754,1.84363,-2.11772,-0.091807,1.75481,-2.17608,0.056479,1.62538,-2.16204,0.08765,1.57038,-2.19034,0.206011,1.44032,-1.80269,0.020665,1.52969,-1.86646,0.017605,1.55796,-1.91385,0.019406,1.53403,-1.9818,0.199124,1.47395,-2.05414,0.108551,1.12493,-1.77 [...]
+/*463*/{0.278586,1.94951,-1.75729,0.304339,1.88909,-1.91542,0.24884,1.80254,-1.63631,0.225581,1.68426,-1.54772,0.33371,1.69289,-1.67108,0.407438,1.69117,-1.72595,0.18014,1.9615,-2.12168,0.293689,1.88732,-1.98774,0.082461,1.92743,-2.00063,-0.058974,1.84171,-2.11714,-0.089616,1.75199,-2.17676,0.05792,1.62243,-2.16185,0.089689,1.56731,-2.19085,0.205972,1.4392,-1.80191,0.020108,1.52713,-1.86637,0.017886,1.55602,-1.91368,0.019327,1.53232,-1.98269,0.200297,1.47231,-2.05391,0.114352,1.12265,-1. [...]
+/*464*/{0.277697,1.94781,-1.75804,0.303581,1.88692,-1.91569,0.246058,1.80119,-1.63702,0.22399,1.68165,-1.54839,0.332124,1.6889,-1.67174,0.405635,1.68633,-1.7265,0.180844,1.9598,-2.1227,0.292979,1.8857,-1.98769,0.082306,1.92635,-2.00085,-0.058263,1.83789,-2.11668,-0.087699,1.74839,-2.17723,0.060658,1.61888,-2.16214,0.092013,1.56376,-2.19117,0.2061,1.43825,-1.80148,0.020258,1.52553,-1.86647,0.017853,1.55382,-1.91417,0.019881,1.52952,-1.98386,0.2014,1.46968,-2.05393,0.119512,1.11938,-1.7775 [...]
+/*465*/{0.276692,1.94593,-1.75803,0.304088,1.88543,-1.91633,0.244051,1.79914,-1.63751,0.21983,1.68013,-1.54995,0.32926,1.68519,-1.67114,0.404194,1.68073,-1.72648,0.18111,1.95794,-2.1234,0.293302,1.88399,-1.98834,0.082095,1.92461,-2.00245,-0.056407,1.83523,-2.11774,-0.086229,1.74455,-2.17776,0.062213,1.61513,-2.16251,0.094204,1.56068,-2.19156,0.206344,1.43709,-1.80118,0.020366,1.5231,-1.86837,0.016979,1.55092,-1.91473,0.019639,1.52601,-1.98397,0.201413,1.46705,-2.05424,0.124961,1.11643,-1 [...]
+/*466*/{0.275501,1.94373,-1.75861,0.303737,1.88342,-1.91658,0.241909,1.79767,-1.63804,0.215243,1.67769,-1.5505,0.32647,1.68045,-1.67167,0.401348,1.67481,-1.72623,0.180423,1.95535,-2.12323,0.29285,1.88181,-1.98782,0.082365,1.92316,-2.00347,-0.055428,1.83107,-2.11831,-0.083785,1.74022,-2.17863,0.064934,1.6106,-2.16324,0.096551,1.55616,-2.19214,0.20638,1.43543,-1.80074,0.020382,1.52057,-1.8687,0.017009,1.54798,-1.91568,0.01985,1.52358,-1.98552,0.202079,1.46424,-2.0543,0.130988,1.11465,-1.77 [...]
+/*467*/{0.274081,1.94156,-1.75896,0.303612,1.88215,-1.91762,0.238415,1.79531,-1.63827,0.212051,1.67545,-1.55182,0.324582,1.67603,-1.67165,0.399579,1.66849,-1.7255,0.181578,1.95303,-2.12402,0.292174,1.87966,-1.98844,0.081583,1.92127,-2.00453,-0.053443,1.8271,-2.11854,-0.081611,1.7352,-2.18072,0.067038,1.6062,-2.16449,0.099349,1.55184,-2.19265,0.206703,1.43403,-1.80065,0.020833,1.51765,-1.86964,0.018313,1.54495,-1.91619,0.020082,1.52055,-1.98626,0.201836,1.4606,-2.05501,0.134765,1.11021,-1 [...]
+/*468*/{0.272615,1.9384,-1.75914,0.30167,1.87941,-1.91653,0.235987,1.79283,-1.63845,0.208276,1.6729,-1.55321,0.320924,1.67122,-1.67093,0.396503,1.66203,-1.72475,0.181148,1.94996,-2.12463,0.292094,1.87708,-1.98859,0.080854,1.91886,-2.00678,-0.052515,1.82202,-2.12103,-0.079683,1.72988,-2.18177,0.068995,1.60173,-2.16549,0.102273,1.54715,-2.1929,0.20618,1.43118,-1.80001,0.019992,1.51407,-1.87041,0.017661,1.54204,-1.91682,0.019269,1.51709,-1.98698,0.202043,1.45735,-2.05563,0.139527,1.10811,-1 [...]
+/*469*/{0.271995,1.9363,-1.75906,0.301036,1.8764,-1.91656,0.233039,1.78992,-1.63864,0.2028,1.67042,-1.55479,0.317271,1.6666,-1.67085,0.39353,1.65509,-1.72306,0.181394,1.9468,-2.12527,0.29215,1.874,-1.98859,0.080133,1.91536,-2.00737,-0.050522,1.81709,-2.12143,-0.077784,1.72411,-2.18351,0.071507,1.59663,-2.16629,0.105935,1.54201,-2.19335,0.206951,1.42959,-1.80095,0.020315,1.51064,-1.87,0.017535,1.5385,-1.91779,0.019398,1.51309,-1.98686,0.202122,1.45468,-2.05615,0.143876,1.1046,-1.77736,0.0 [...]
+/*470*/{0.269762,1.93297,-1.75902,0.301581,1.87239,-1.91711,0.229327,1.78679,-1.63862,0.198735,1.66744,-1.55574,0.31401,1.66085,-1.67088,0.390084,1.64747,-1.72236,0.181289,1.94318,-2.12602,0.291805,1.8709,-1.98867,0.079484,1.9122,-2.00895,-0.051351,1.81281,-2.12235,-0.075927,1.71742,-2.18604,0.073999,1.59084,-2.16783,0.109148,1.53642,-2.19441,0.208051,1.42684,-1.80132,0.01939,1.50685,-1.87067,0.018423,1.53471,-1.91784,0.018975,1.50899,-1.98807,0.201553,1.45164,-2.05641,0.149241,1.1023,-1 [...]
+/*471*/{0.267963,1.9297,-1.75887,0.300612,1.86973,-1.91689,0.226171,1.78388,-1.63841,0.194505,1.66442,-1.55679,0.31025,1.65488,-1.67065,0.386986,1.6399,-1.72096,0.18161,1.93956,-2.12674,0.291121,1.86767,-1.98884,0.078047,1.90856,-2.01075,-0.049788,1.80626,-2.12306,-0.073446,1.711,-2.18791,0.077469,1.58538,-2.16875,0.112482,1.53128,-2.19528,0.20797,1.42379,-1.80144,0.019233,1.50284,-1.87086,0.017943,1.53042,-1.91867,0.018271,1.50458,-1.98762,0.201861,1.44868,-2.05708,0.154077,1.09769,-1.7 [...]
+/*472*/{0.266771,1.92619,-1.75869,0.300653,1.86559,-1.91705,0.222513,1.77983,-1.63858,0.188278,1.66184,-1.55881,0.30619,1.64853,-1.67058,0.383252,1.63224,-1.71965,0.180085,1.9349,-2.12728,0.291143,1.86404,-1.98902,0.077742,1.90426,-2.01257,-0.056628,1.80471,-2.12765,-0.071162,1.70392,-2.19023,0.080985,1.57925,-2.1708,0.115838,1.526,-2.19588,0.208049,1.4208,-1.80183,0.018683,1.4983,-1.87134,0.017471,1.52602,-1.91898,0.017476,1.4997,-1.98755,0.201801,1.44523,-2.05801,0.158727,1.0937,-1.774 [...]
+/*473*/{0.265018,1.92215,-1.75814,0.301236,1.86214,-1.91794,0.218525,1.77549,-1.63842,0.182863,1.65746,-1.55936,0.300965,1.64272,-1.66975,0.379224,1.62397,-1.71789,0.180458,1.93018,-2.12803,0.290602,1.8604,-1.98926,0.077284,1.89948,-2.01331,-0.048389,1.79164,-2.12983,-0.069316,1.69652,-2.19257,0.083906,1.57308,-2.17255,0.119241,1.51937,-2.1967,0.207863,1.41748,-1.80239,0.0184,1.49334,-1.87202,0.017103,1.5214,-1.91933,0.017117,1.49471,-1.98793,0.200984,1.44229,-2.05913,0.161388,1.09192,-1 [...]
+/*474*/{0.263479,1.91812,-1.75757,0.300078,1.85732,-1.9172,0.215428,1.77126,-1.63879,0.178199,1.6538,-1.56009,0.296431,1.63566,-1.66902,0.375021,1.61533,-1.71673,0.17978,1.92503,-2.1282,0.290198,1.85631,-1.98964,0.076676,1.89426,-2.01578,-0.048321,1.78341,-2.13244,-0.066924,1.68844,-2.19495,0.086957,1.56618,-2.17384,0.123578,1.51317,-2.19721,0.208536,1.41418,-1.80311,0.018471,1.48784,-1.87181,0.016836,1.51676,-1.91929,0.016493,1.48944,-1.98732,0.201075,1.43906,-2.06001,0.166859,1.08822,- [...]
+/*475*/{0.261323,1.9131,-1.75713,0.300103,1.85362,-1.91694,0.212359,1.76702,-1.6386,0.173691,1.65039,-1.56062,0.291414,1.62888,-1.66879,0.370765,1.60697,-1.71456,0.180821,1.92007,-2.12965,0.290602,1.85201,-1.98968,0.075396,1.88875,-2.01754,-0.04612,1.77605,-2.13531,-0.064683,1.68087,-2.19775,0.091016,1.55983,-2.17539,0.128182,1.50724,-2.19823,0.208823,1.41042,-1.80425,0.01865,1.48242,-1.87157,0.016725,1.51107,-1.91935,0.016369,1.48406,-1.98707,0.201167,1.43555,-2.06102,0.169242,1.08497,- [...]
+/*476*/{0.259435,1.90827,-1.75688,0.30102,1.84868,-1.91657,0.208427,1.76175,-1.6387,0.166175,1.64602,-1.56138,0.286462,1.62225,-1.66831,0.365195,1.59872,-1.71257,0.180907,1.91461,-2.13031,0.290552,1.84726,-1.98944,0.074852,1.88349,-2.01948,-0.046275,1.76745,-2.13829,-0.061895,1.67236,-2.20111,0.095233,1.55248,-2.17688,0.133234,1.50113,-2.19871,0.209229,1.40717,-1.80529,0.018144,1.47657,-1.87176,0.016112,1.50568,-1.91946,0.015154,1.47824,-1.98618,0.20042,1.43128,-2.06191,0.172822,1.08193, [...]
+/*477*/{0.25772,1.90308,-1.75573,0.300861,1.8431,-1.91649,0.204633,1.75673,-1.63924,0.159961,1.64174,-1.56206,0.281547,1.61549,-1.66754,0.360077,1.58962,-1.71046,0.181486,1.90927,-2.13141,0.290688,1.84269,-1.98906,0.074686,1.8773,-2.02038,-0.044415,1.75759,-2.14121,-0.058829,1.66381,-2.20426,0.099525,1.54539,-2.17872,0.138398,1.49429,-2.19922,0.20923,1.40306,-1.80614,0.017864,1.47087,-1.8709,0.015154,1.49987,-1.92016,0.014189,1.47246,-1.98645,0.200176,1.42749,-2.06317,0.17602,1.07812,-1. [...]
+/*478*/{0.256688,1.89895,-1.75566,0.29979,1.83783,-1.91537,0.200412,1.75116,-1.63917,0.15361,1.63859,-1.56272,0.275812,1.60834,-1.6658,0.355201,1.58102,-1.7081,0.181264,1.90319,-2.13287,0.290193,1.83763,-1.98882,0.07328,1.87138,-2.02324,-0.044067,1.74847,-2.14377,-0.056014,1.65504,-2.20722,0.103895,1.53982,-2.17997,0.143498,1.48771,-2.20016,0.209669,1.39839,-1.80704,0.016842,1.46484,-1.87114,0.015037,1.49472,-1.91996,0.013035,1.46672,-1.98578,0.199169,1.42349,-2.06444,0.178789,1.07404,-1 [...]
+/*479*/{0.254794,1.8927,-1.75433,0.298659,1.83307,-1.91481,0.196215,1.74573,-1.63948,0.147058,1.63363,-1.56344,0.270528,1.6018,-1.66472,0.348878,1.57288,-1.70538,0.182099,1.89744,-2.134,0.29025,1.83248,-1.98923,0.072537,1.86464,-2.02466,-0.042163,1.739,-2.14686,-0.052512,1.64547,-2.21029,0.109176,1.53265,-2.18141,0.149404,1.48085,-2.20062,0.209552,1.39386,-1.80856,0.016206,1.45984,-1.87068,0.014305,1.48817,-1.92052,0.01087,1.46115,-1.98601,0.197873,1.41893,-2.06578,0.185916,1.07033,-1.76 [...]
+/*480*/{0.252851,1.88749,-1.7539,0.299719,1.82743,-1.91378,0.192292,1.74056,-1.63942,0.140226,1.63023,-1.56411,0.264244,1.59478,-1.66429,0.343699,1.5647,-1.70317,0.183021,1.89149,-2.135,0.290685,1.82689,-1.98891,0.071399,1.85897,-2.02728,-0.040528,1.72909,-2.15162,-0.048074,1.63622,-2.21349,0.113843,1.52529,-2.18258,0.155616,1.47428,-2.20086,0.209002,1.3889,-1.80908,0.014913,1.45326,-1.87097,0.012441,1.48193,-1.92024,0.008437,1.4566,-1.98561,0.196454,1.41389,-2.06702,0.191291,1.06507,-1. [...]
+/*481*/{0.251403,1.88172,-1.75292,0.300324,1.82138,-1.91311,0.18773,1.73523,-1.63988,0.134,1.62677,-1.56574,0.258313,1.5883,-1.6631,0.336834,1.55713,-1.70077,0.183365,1.88616,-2.13657,0.291637,1.82166,-1.98872,0.070193,1.85195,-2.02887,-0.037843,1.72018,-2.15658,-0.04394,1.62651,-2.21678,0.120208,1.51809,-2.18394,0.162447,1.46775,-2.20149,0.208668,1.38371,-1.81074,0.012277,1.44786,-1.87,0.010966,1.47548,-1.91955,0.004497,1.45248,-1.98621,0.194019,1.40988,-2.06779,0.195719,1.0626,-1.75765 [...]
+/*482*/{0.250258,1.87671,-1.75177,0.301571,1.81539,-1.91212,0.185005,1.73012,-1.64003,0.127095,1.62284,-1.56652,0.25187,1.58187,-1.66131,0.329938,1.54955,-1.69802,0.184643,1.88079,-2.13848,0.291993,1.81614,-1.98863,0.070026,1.84843,-2.02992,-0.036047,1.70988,-2.16132,-0.038662,1.6164,-2.21976,0.126821,1.51121,-2.18537,0.169198,1.46145,-2.2018,0.207304,1.37866,-1.81303,0.010315,1.44217,-1.87025,0.008105,1.47037,-1.91926,0.001148,1.44768,-1.98593,0.191228,1.40821,-2.06822,0.198156,1.05865, [...]
+/*483*/{0.249747,1.87167,-1.75063,0.301404,1.81021,-1.91107,0.180959,1.72441,-1.64014,0.119928,1.61877,-1.56648,0.245487,1.57605,-1.65983,0.322935,1.54394,-1.69544,0.186116,1.87578,-2.14066,0.293179,1.81044,-1.98763,0.071047,1.84632,-2.03019,-0.032059,1.70134,-2.16748,-0.032983,1.60653,-2.2229,0.133038,1.50448,-2.1863,0.17722,1.45573,-2.20222,0.20614,1.37377,-1.81535,0.007301,1.43686,-1.87003,0.005221,1.4649,-1.91818,-0.001945,1.44372,-1.98582,0.188517,1.40619,-2.06977,0.200975,1.05495,- [...]
+/*484*/{0.248308,1.86683,-1.74972,0.301037,1.8055,-1.91013,0.176749,1.72015,-1.64035,0.112016,1.61585,-1.5676,0.238772,1.57099,-1.65846,0.315712,1.53709,-1.69283,0.187078,1.87098,-2.14253,0.294133,1.80611,-1.98749,0.071159,1.84397,-2.03041,-0.02615,1.69056,-2.17435,-0.025811,1.59632,-2.22598,0.141153,1.49831,-2.18713,0.18507,1.45032,-2.20223,0.20413,1.36885,-1.81649,0.004949,1.43247,-1.8696,0.002957,1.46147,-1.91857,-0.005379,1.43994,-1.9831,0.186159,1.40412,-2.07184,0.205586,1.05432,-1. [...]
+/*485*/{0.248255,1.86266,-1.74819,0.301544,1.80082,-1.9088,0.171496,1.71644,-1.64092,0.103704,1.61409,-1.56853,0.230999,1.56677,-1.65718,0.307833,1.53146,-1.69073,0.188492,1.86655,-2.14384,0.294705,1.80215,-1.98696,0.071176,1.84252,-2.03064,-0.021291,1.68177,-2.18221,-0.018017,1.58663,-2.22869,0.149237,1.49287,-2.18767,0.193827,1.44516,-2.20222,0.202806,1.36443,-1.81858,0.002171,1.42774,-1.86788,0.001115,1.45956,-1.91675,-0.007714,1.43704,-1.9812,0.182956,1.40302,-2.07381,0.209922,1.0550 [...]
+/*486*/{0.24811,1.85824,-1.74603,0.301933,1.79764,-1.9078,0.166845,1.71474,-1.64117,0.094321,1.61367,-1.56983,0.223652,1.56263,-1.65592,0.300495,1.52638,-1.68797,0.190032,1.86351,-2.14393,0.294679,1.79924,-1.98584,0.071752,1.84044,-2.03048,-0.014182,1.67238,-2.18784,-0.009232,1.57749,-2.231,0.158117,1.48829,-2.18808,0.203702,1.44114,-2.20219,0.201011,1.36088,-1.81939,0.000371,1.42545,-1.86533,-0.000413,1.45775,-1.91615,-0.009643,1.43588,-1.97866,0.180455,1.40246,-2.07589,0.216829,1.05692 [...]
+/*487*/{0.248243,1.85493,-1.74515,0.301766,1.7952,-1.90728,0.161373,1.71395,-1.64162,0.087115,1.61428,-1.57212,0.216055,1.55984,-1.65447,0.29222,1.52233,-1.68511,0.191287,1.86057,-2.1436,0.294413,1.79643,-1.98505,0.071587,1.83948,-2.03058,-0.008988,1.66575,-2.19511,0.000341,1.56937,-2.2334,0.167487,1.48459,-2.18808,0.213901,1.43814,-2.20125,0.198428,1.35747,-1.82038,-0.000929,1.42389,-1.86286,-0.002234,1.45716,-1.9162,-0.011418,1.43541,-1.97535,0.178512,1.40181,-2.07768,0.223587,1.05946, [...]
+/*488*/{0.24787,1.85224,-1.74491,0.301611,1.7922,-1.90661,0.156165,1.714,-1.64223,0.079019,1.6157,-1.57412,0.208342,1.55923,-1.65518,0.284638,1.51907,-1.68243,0.192489,1.85896,-2.14317,0.294884,1.79406,-1.98415,0.072168,1.83747,-2.03049,-0.001633,1.65837,-2.19896,0.011358,1.56237,-2.23541,0.177255,1.48166,-2.18722,0.2248,1.43622,-2.20037,0.195888,1.35435,-1.82123,-0.001774,1.42337,-1.86251,-0.004044,1.45693,-1.91619,-0.012603,1.43523,-1.97395,0.176051,1.40117,-2.07883,0.230663,1.0617,-1. [...]
+/*489*/{0.248227,1.85064,-1.74433,0.301007,1.79075,-1.90528,0.150513,1.71449,-1.64258,0.071046,1.61846,-1.57712,0.200748,1.55851,-1.65459,0.276884,1.51706,-1.68115,0.194302,1.85742,-2.14182,0.294678,1.79244,-1.98348,0.072715,1.83666,-2.0307,0.005725,1.65188,-2.2024,0.020048,1.55758,-2.23642,0.187587,1.4803,-2.18684,0.236312,1.4357,-2.1993,0.193776,1.35221,-1.82117,-0.003734,1.42344,-1.86145,-0.005933,1.45652,-1.91556,-0.013667,1.436,-1.97305,0.174817,1.40042,-2.07955,0.237655,1.06522,-1. [...]
+/*490*/{0.247879,1.84935,-1.7439,0.300551,1.78951,-1.90441,0.145041,1.71557,-1.64362,0.0627,1.62204,-1.57962,0.192918,1.55929,-1.65398,0.26827,1.516,-1.67847,0.196147,1.85644,-2.14143,0.295251,1.79082,-1.98191,0.073003,1.83577,-2.03188,0.013428,1.64837,-2.20407,0.029074,1.55353,-2.23703,0.197789,1.47937,-2.18548,0.247696,1.43607,-2.19736,0.192923,1.3511,-1.82126,-0.00416,1.42418,-1.86148,-0.006773,1.45656,-1.91583,-0.013744,1.43603,-1.97255,0.174785,1.4002,-2.07998,0.244729,1.06884,-1.75 [...]
+/*491*/{0.247118,1.84901,-1.74301,0.300718,1.78889,-1.9032,0.139001,1.71758,-1.6439,0.055335,1.62641,-1.58216,0.18517,1.55929,-1.65285,0.260271,1.51555,-1.67686,0.197934,1.85574,-2.14106,0.294938,1.78992,-1.98094,0.073989,1.8357,-2.03246,0.0209,1.6454,-2.20531,0.038368,1.55099,-2.238,0.208357,1.47941,-2.18418,0.259317,1.43835,-2.19504,0.189963,1.35025,-1.81963,-0.004912,1.42498,-1.86197,-0.007551,1.45674,-1.91669,-0.014734,1.43613,-1.97386,0.173411,1.3992,-2.08056,0.250897,1.07366,-1.753 [...]
+/*492*/{0.245319,1.84918,-1.74247,0.298802,1.78809,-1.90229,0.133923,1.72034,-1.64448,0.046629,1.62996,-1.58428,0.177674,1.56104,-1.65198,0.25183,1.51627,-1.67432,0.200477,1.85558,-2.1404,0.295148,1.78973,-1.97951,0.074968,1.8358,-2.03288,0.026592,1.64347,-2.20595,0.047061,1.5485,-2.23869,0.218504,1.47999,-2.18206,0.27023,1.44123,-2.19262,0.188863,1.35053,-1.81849,-0.00577,1.42518,-1.86258,-0.008717,1.45711,-1.91703,-0.015424,1.437,-1.97492,0.173411,1.3992,-2.08056,0.257423,1.07805,-1.75 [...]
+/*493*/{0.242546,1.85037,-1.74123,0.297618,1.78835,-1.90044,0.127784,1.7239,-1.64516,0.038191,1.6344,-1.58642,0.170255,1.56354,-1.65117,0.244437,1.51706,-1.67262,0.203247,1.85596,-2.13971,0.29545,1.78988,-1.97853,0.075412,1.83595,-2.03362,0.033969,1.64157,-2.20812,0.055725,1.54744,-2.2393,0.228246,1.48373,-2.18005,0.281045,1.44591,-2.18975,0.187032,1.35112,-1.81729,-0.007663,1.42602,-1.863,-0.009411,1.45717,-1.9175,-0.015927,1.43713,-1.97573,0.171072,1.39757,-2.0813,0.263758,1.08605,-1.7 [...]
+/*494*/{0.240161,1.8523,-1.74112,0.297203,1.78881,-1.89932,0.121801,1.72767,-1.64589,0.030768,1.63952,-1.58846,0.162514,1.56672,-1.65102,0.235906,1.51934,-1.67127,0.204663,1.85711,-2.13969,0.295343,1.7908,-1.97673,0.076883,1.83669,-2.03473,0.0393,1.64091,-2.2089,0.063448,1.54638,-2.24005,0.238764,1.48794,-2.17779,0.291557,1.4516,-2.18638,0.184893,1.35223,-1.81608,-0.008144,1.42624,-1.86418,-0.009943,1.45761,-1.91824,-0.015679,1.43778,-1.97724,0.172861,1.39748,-2.08087,0.269854,1.0918,-1. [...]
+/*495*/{0.236811,1.85444,-1.74032,0.296422,1.79006,-1.8975,0.115215,1.7324,-1.64638,0.021665,1.64407,-1.59058,0.154983,1.56995,-1.64972,0.228447,1.52243,-1.66935,0.207384,1.85889,-2.139,0.29573,1.79159,-1.97506,0.077453,1.83708,-2.03591,0.044696,1.63947,-2.20938,0.071694,1.54659,-2.2405,0.247301,1.49337,-2.17513,0.301126,1.45826,-2.18246,0.184419,1.35425,-1.81579,-0.009214,1.42741,-1.8663,-0.010116,1.45812,-1.91905,-0.016889,1.43923,-1.9795,0.172896,1.39718,-2.08072,0.275618,1.09938,-1.7 [...]
+/*496*/{0.233922,1.85711,-1.73924,0.296773,1.79221,-1.89515,0.109945,1.73675,-1.64694,0.013882,1.64993,-1.59236,0.147853,1.57508,-1.64985,0.221012,1.52672,-1.66795,0.209674,1.86132,-2.13795,0.296181,1.79404,-1.97373,0.078469,1.83748,-2.03681,0.048717,1.63943,-2.2095,0.079511,1.54714,-2.24082,0.255495,1.49903,-2.17222,0.310832,1.46658,-2.17861,0.18304,1.3554,-1.81545,-0.010048,1.42779,-1.86765,-0.011258,1.45979,-1.92092,-0.016695,1.44007,-1.98017,0.172868,1.39699,-2.08079,0.280913,1.10685 [...]
+/*497*/{0.23028,1.86003,-1.73865,0.295463,1.79452,-1.89365,0.103748,1.74137,-1.64783,0.00707,1.65542,-1.59463,0.140704,1.57998,-1.64908,0.212702,1.53066,-1.66689,0.212126,1.86384,-2.13663,0.295751,1.79544,-1.97144,0.078521,1.83935,-2.0377,0.055512,1.6402,-2.20916,0.086168,1.54831,-2.24111,0.262817,1.50517,-2.16864,0.319463,1.47465,-2.17426,0.183102,1.3585,-1.8143,-0.010307,1.42955,-1.87012,-0.010082,1.46028,-1.92128,-0.016427,1.44096,-1.98198,0.173319,1.39713,-2.0807,0.285644,1.11346,-1. [...]
+/*498*/{0.22683,1.86303,-1.73788,0.294912,1.79774,-1.89172,0.098209,1.74616,-1.64843,-0.000565,1.661,-1.59699,0.132373,1.58533,-1.65001,0.205571,1.53505,-1.66567,0.213896,1.86747,-2.13583,0.295934,1.7986,-1.96917,0.079939,1.84026,-2.03935,0.059516,1.64083,-2.20937,0.09387,1.54985,-2.24141,0.269053,1.51221,-2.16576,0.328383,1.48369,-2.16946,0.182085,1.36023,-1.81363,-0.010642,1.4309,-1.8711,-0.009982,1.46306,-1.92271,-0.015894,1.44343,-1.98365,0.173451,1.39712,-2.08072,0.290288,1.12214,-1 [...]
+/*499*/{0.22303,1.86655,-1.737,0.293282,1.8001,-1.88913,0.092154,1.75139,-1.64911,-0.008123,1.66622,-1.59888,0.126126,1.5901,-1.64957,0.198447,1.54009,-1.6637,0.215252,1.87033,-2.13505,0.296245,1.80146,-1.96761,0.081432,1.84152,-2.04033,0.063135,1.64221,-2.20872,0.100883,1.55233,-2.2424,0.277108,1.52122,-2.1624,0.335133,1.49431,-2.16521,0.181706,1.36321,-1.81322,-0.01068,1.43309,-1.87327,-0.008672,1.46484,-1.92393,-0.015378,1.44536,-1.98557,0.173744,1.39736,-2.08061,0.295355,1.12884,-1.7 [...]
+/*500*/{0.218449,1.86983,-1.73661,0.29401,1.80424,-1.88705,0.086691,1.75635,-1.64992,-0.014853,1.67262,-1.601,0.119239,1.59621,-1.64996,0.191092,1.54602,-1.66353,0.217655,1.87422,-2.13376,0.296602,1.80465,-1.96565,0.08145,1.84379,-2.04081,0.066849,1.6444,-2.20844,0.107197,1.55515,-2.24288,0.282644,1.52873,-2.15879,0.342395,1.50459,-2.16107,0.182373,1.36633,-1.81311,-0.010619,1.436,-1.87509,-0.009662,1.46738,-1.92528,-0.013787,1.44736,-1.98665,0.174122,1.39767,-2.08054,0.298725,1.13573,-1 [...]
+/*501*/{0.215257,1.87384,-1.73628,0.293555,1.80737,-1.88563,0.081695,1.76167,-1.6505,-0.023182,1.67884,-1.60341,0.112338,1.6023,-1.6513,0.184469,1.55145,-1.6625,0.219475,1.87857,-2.13273,0.297762,1.80815,-1.96316,0.082736,1.84618,-2.0413,0.070578,1.64612,-2.20777,0.114648,1.55836,-2.2436,0.288611,1.53745,-2.15533,0.34923,1.51477,-2.15628,0.18182,1.37015,-1.81322,-0.010453,1.43914,-1.87619,-0.009006,1.46961,-1.92626,-0.013103,1.45046,-1.98782,0.174993,1.39884,-2.0808,0.303083,1.14351,-1.7 [...]
+/*502*/{0.21078,1.87795,-1.73557,0.29133,1.81196,-1.88261,0.07616,1.76691,-1.65133,-0.029207,1.68512,-1.60541,0.105991,1.60865,-1.65139,0.17786,1.55718,-1.66187,0.22086,1.88266,-2.13173,0.298267,1.81223,-1.96084,0.084248,1.8487,-2.04267,0.075943,1.64906,-2.20744,0.120904,1.56205,-2.24396,0.293997,1.54648,-2.15178,0.35451,1.52566,-2.15156,0.182328,1.37387,-1.81322,-0.010063,1.44245,-1.8774,-0.006354,1.47285,-1.92859,-0.011054,1.45284,-1.98923,0.176093,1.39936,-2.08104,0.305599,1.15037,-1. [...]
+/*503*/{0.207515,1.88285,-1.73503,0.29255,1.8144,-1.88078,0.071103,1.7723,-1.65234,-0.034491,1.69195,-1.60721,0.099662,1.61457,-1.65193,0.170842,1.56334,-1.66181,0.223945,1.8878,-2.13069,0.298615,1.81635,-1.95961,0.085307,1.85223,-2.04368,0.080348,1.65232,-2.20816,0.127067,1.56634,-2.24484,0.298969,1.55615,-2.14818,0.360202,1.53624,-2.14676,0.182863,1.3776,-1.81373,-0.00861,1.44619,-1.87913,-0.006491,1.47518,-1.92983,-0.009856,1.45629,-1.9912,0.17796,1.40021,-2.08087,0.307453,1.15879,-1. [...]
+/*504*/{0.204186,1.88624,-1.73432,0.291273,1.82088,-1.87961,0.066879,1.77813,-1.65277,-0.041705,1.69874,-1.6103,0.09318,1.62112,-1.65303,0.16512,1.56914,-1.66103,0.224997,1.89213,-2.12889,0.298933,1.82084,-1.95714,0.086443,1.85554,-2.04478,0.083609,1.65666,-2.20922,0.133921,1.57073,-2.24506,0.30281,1.56414,-2.14452,0.364475,1.54767,-2.14221,0.18499,1.38178,-1.81358,-0.006974,1.44953,-1.88016,-0.005131,1.47865,-1.93123,-0.007926,1.45983,-1.99205,0.178808,1.40079,-2.08095,0.310898,1.16514, [...]
+/*505*/{0.201488,1.89064,-1.73384,0.291572,1.82447,-1.87707,0.062321,1.78352,-1.65422,-0.048433,1.70513,-1.6128,0.087581,1.62717,-1.65407,0.159133,1.57507,-1.66176,0.22657,1.89688,-2.12788,0.299543,1.82529,-1.95505,0.087677,1.8589,-2.04506,0.089181,1.66259,-2.21013,0.140246,1.5754,-2.24571,0.307069,1.57268,-2.14087,0.369147,1.55803,-2.13754,0.185031,1.38547,-1.81443,-0.006107,1.45436,-1.88054,-0.003512,1.48282,-1.93322,-0.005371,1.46313,-1.9929,0.180156,1.4016,-2.08175,0.313551,1.17192,- [...]
+/*506*/{0.198335,1.89477,-1.73402,0.290931,1.83037,-1.87583,0.058213,1.78946,-1.65521,-0.05329,1.71214,-1.61553,0.083326,1.63403,-1.65558,0.153392,1.58126,-1.66125,0.229361,1.90251,-2.12638,0.299409,1.82998,-1.95359,0.088975,1.86275,-2.04614,0.093015,1.66609,-2.21124,0.146525,1.58072,-2.24625,0.310011,1.58151,-2.13779,0.372325,1.5687,-2.13332,0.187226,1.38994,-1.81515,-0.004319,1.45913,-1.88094,-0.001468,1.48606,-1.93421,-0.003125,1.46686,-1.9946,0.182182,1.40219,-2.08198,0.315711,1.1793 [...]
+/*507*/{0.195724,1.899,-1.73379,0.290191,1.83348,-1.87303,0.054366,1.79474,-1.6567,-0.058904,1.71895,-1.61835,0.077422,1.63971,-1.65616,0.147586,1.58664,-1.66124,0.231349,1.90765,-2.12503,0.301267,1.83441,-1.95052,0.091252,1.86556,-2.04718,0.097882,1.67245,-2.21268,0.152399,1.58587,-2.24703,0.313502,1.59194,-2.1347,0.376852,1.57783,-2.12845,0.188337,1.39408,-1.8158,-0.002479,1.46336,-1.8826,0.000516,1.4897,-1.93655,-0.000738,1.46985,-1.99485,0.183195,1.40309,-2.0822,0.318494,1.18656,-1.7 [...]
+/*508*/{0.193478,1.90357,-1.73364,0.290056,1.84009,-1.87245,0.049878,1.80013,-1.65855,-0.062882,1.72573,-1.62098,0.072419,1.64587,-1.65738,0.143307,1.59255,-1.66147,0.233492,1.91297,-2.12388,0.301224,1.83927,-1.94877,0.092197,1.86906,-2.04766,0.099827,1.67742,-2.21427,0.157796,1.59109,-2.24817,0.316149,1.59895,-2.13102,0.378635,1.5878,-2.12358,0.189302,1.39712,-1.81726,-0.000354,1.46833,-1.88274,0.002007,1.49431,-1.93887,0.002262,1.47332,-1.99587,0.185339,1.40379,-2.08247,0.319579,1.1936 [...]
+/*509*/{0.190989,1.9079,-1.73344,0.289533,1.84391,-1.86979,0.046857,1.80642,-1.66022,-0.06783,1.73213,-1.62459,0.067405,1.65148,-1.6582,0.138342,1.5983,-1.66106,0.235707,1.91801,-2.12217,0.301518,1.8445,-1.94712,0.09387,1.8724,-2.04832,0.104133,1.68241,-2.21522,0.163632,1.59618,-2.24866,0.318581,1.60702,-2.12766,0.381177,1.59654,-2.12071,0.19106,1.40121,-1.81719,0.001584,1.47253,-1.88288,0.004819,1.49709,-1.94013,0.004605,1.47704,-1.99665,0.187419,1.40454,-2.08234,0.320687,1.20022,-1.749 [...]
+/*510*/{0.189193,1.91235,-1.73332,0.28845,1.84869,-1.86852,0.043207,1.81193,-1.66206,-0.070945,1.73872,-1.62738,0.06334,1.65719,-1.65996,0.134133,1.60347,-1.66168,0.236656,1.92254,-2.12077,0.301766,1.84892,-1.94548,0.095093,1.87656,-2.04907,0.107674,1.68757,-2.21653,0.168878,1.60164,-2.24914,0.320395,1.61518,-2.12496,0.383738,1.606,-2.11663,0.193698,1.406,-1.818,0.003214,1.47801,-1.88354,0.0064,1.50121,-1.94201,0.006402,1.48029,-1.99731,0.189595,1.40515,-2.0819,0.322662,1.20642,-1.74947, [...]
+/*511*/{0.187345,1.91574,-1.7329,0.28769,1.85408,-1.86797,0.04048,1.81707,-1.66369,-0.075862,1.7446,-1.63142,0.058851,1.66188,-1.66081,0.129888,1.60875,-1.66117,0.238074,1.92715,-2.11887,0.301002,1.85343,-1.94358,0.095897,1.87979,-2.04985,0.112615,1.69245,-2.21754,0.174172,1.60704,-2.24963,0.322085,1.62235,-2.1221,0.385886,1.61501,-2.11379,0.194737,1.40982,-1.81868,0.004764,1.48297,-1.88404,0.008047,1.50505,-1.94294,0.009731,1.48312,-1.99768,0.191548,1.4062,-2.08143,0.324378,1.21122,-1.7 [...]
+/*512*/{0.185534,1.91964,-1.7332,0.287232,1.85759,-1.86601,0.036912,1.82269,-1.66581,-0.080752,1.75042,-1.63443,0.055665,1.66681,-1.66184,0.125975,1.61318,-1.66127,0.24051,1.932,-2.11802,0.301487,1.85801,-1.94201,0.097521,1.88293,-2.0506,0.116986,1.69718,-2.21944,0.178988,1.61165,-2.25046,0.323732,1.62953,-2.11915,0.38805,1.62292,-2.11014,0.195913,1.4137,-1.81899,0.007306,1.48666,-1.88393,0.010278,1.50942,-1.94468,0.01216,1.48706,-1.99828,0.19373,1.40709,-2.08091,0.326764,1.21599,-1.7489 [...]
+/*513*/{0.18358,1.92293,-1.73361,0.286032,1.86225,-1.86533,0.034063,1.8276,-1.66776,-0.084046,1.75578,-1.63839,0.052061,1.67122,-1.66272,0.122697,1.61761,-1.6613,0.241639,1.93635,-2.11638,0.301217,1.86202,-1.94067,0.098338,1.88628,-2.05178,0.121426,1.70123,-2.22098,0.183637,1.61703,-2.25129,0.325911,1.6363,-2.11654,0.389558,1.6306,-2.10635,0.197864,1.41809,-1.81858,0.009782,1.49136,-1.88385,0.013765,1.51227,-1.94592,0.015478,1.48942,-1.9989,0.19569,1.40797,-2.0806,0.327942,1.22143,-1.749 [...]
+/*514*/{0.182359,1.92673,-1.7335,0.286307,1.8663,-1.86359,0.031974,1.83255,-1.6693,-0.087507,1.76112,-1.64162,0.04899,1.67625,-1.66428,0.119785,1.62147,-1.66135,0.243023,1.94037,-2.11481,0.300465,1.86681,-1.93927,0.100023,1.88878,-2.05201,0.125756,1.70581,-2.22233,0.187596,1.62178,-2.2524,0.327142,1.64255,-2.11467,0.390925,1.63742,-2.10343,0.1995,1.42178,-1.81909,0.01124,1.49603,-1.8845,0.014871,1.5158,-1.94754,0.017452,1.49301,-1.99973,0.197514,1.40891,-2.07955,0.328187,1.22566,-1.74895 [...]
+/*515*/{0.181605,1.92948,-1.73324,0.285886,1.86996,-1.86243,0.029931,1.83686,-1.67131,-0.089517,1.76551,-1.64531,0.04587,1.6802,-1.6655,0.116541,1.62502,-1.66112,0.243965,1.94379,-2.11375,0.30132,1.86907,-1.93756,0.10112,1.89206,-2.0523,0.129804,1.71011,-2.22385,0.192758,1.62598,-2.25262,0.327826,1.64798,-2.11264,0.392175,1.6431,-2.10015,0.199983,1.42596,-1.81848,0.013535,1.4996,-1.88517,0.017255,1.51939,-1.94826,0.019773,1.49622,-2.0004,0.19933,1.41009,-2.07829,0.330157,1.22894,-1.74968 [...]
+/*516*/{0.181101,1.93216,-1.73321,0.285204,1.87137,-1.86137,0.027575,1.8412,-1.67386,-0.09389,1.76955,-1.64872,0.043084,1.68392,-1.66644,0.113335,1.62802,-1.66038,0.245769,1.94747,-2.11221,0.301323,1.87213,-1.93651,0.102344,1.89529,-2.05299,0.134546,1.71455,-2.2259,0.196372,1.63025,-2.25351,0.330298,1.65403,-2.11168,0.392858,1.64887,-2.09786,0.200244,1.42929,-1.81844,0.014742,1.50392,-1.88455,0.01956,1.52239,-1.94944,0.022031,1.49869,-2.00072,0.201366,1.41114,-2.07729,0.330189,1.23084,-1 [...]
+/*517*/{0.180615,1.93495,-1.73401,0.278764,1.87743,-1.86249,0.026336,1.84532,-1.67541,-0.095403,1.77404,-1.65224,0.040941,1.68708,-1.66758,0.110714,1.63117,-1.65991,0.247744,1.95116,-2.11123,0.300259,1.87587,-1.93581,0.103494,1.89732,-2.05366,0.138957,1.7187,-2.22771,0.200339,1.63432,-2.25422,0.331983,1.65845,-2.11042,0.394636,1.65364,-2.09549,0.201496,1.43266,-1.81872,0.01699,1.50773,-1.88516,0.021861,1.52551,-1.94957,0.024494,1.50145,-2.00097,0.203509,1.41212,-2.07664,0.330429,1.23338, [...]
+/*518*/{0.180244,1.93726,-1.73397,0.278113,1.87902,-1.86163,0.024213,1.84888,-1.67738,-0.097526,1.77756,-1.65566,0.038382,1.68959,-1.66841,0.108074,1.63387,-1.65959,0.248684,1.95383,-2.10986,0.300959,1.87886,-1.93441,0.104975,1.90039,-2.05496,0.142899,1.72303,-2.22973,0.203802,1.63795,-2.25522,0.332656,1.66165,-2.10882,0.395825,1.65741,-2.09289,0.20407,1.43562,-1.8184,0.018747,1.51035,-1.88568,0.022812,1.52862,-1.95089,0.026494,1.50457,-2.00131,0.20537,1.41377,-2.07577,0.330691,1.23456,- [...]
+/*519*/{0.179836,1.93953,-1.73384,0.27782,1.88125,-1.86055,0.023374,1.85207,-1.6787,-0.100674,1.78022,-1.65902,0.037085,1.69281,-1.66955,0.10589,1.63587,-1.6596,0.249149,1.95661,-2.10844,0.301398,1.88144,-1.93384,0.106028,1.90291,-2.05545,0.147439,1.72666,-2.23086,0.207089,1.64113,-2.25581,0.333551,1.66516,-2.10828,0.396316,1.66102,-2.09133,0.20524,1.43836,-1.81815,0.019992,1.51437,-1.8858,0.024935,1.53142,-1.95052,0.028845,1.50702,-2.00159,0.205949,1.41578,-2.0752,0.331341,1.23474,-1.75 [...]
+/*520*/{0.179607,1.94095,-1.73363,0.279435,1.88315,-1.86,0.022395,1.85398,-1.68096,-0.10122,1.78312,-1.66173,0.034661,1.69441,-1.67061,0.104582,1.63762,-1.65884,0.249624,1.95905,-2.10756,0.300977,1.88348,-1.93296,0.106896,1.90521,-2.05563,0.150904,1.72965,-2.23242,0.21013,1.6441,-2.25693,0.334887,1.66691,-2.10789,0.396981,1.66284,-2.08931,0.205583,1.44022,-1.81814,0.020771,1.51703,-1.8854,0.02591,1.53468,-1.95064,0.029909,1.51033,-2.0018,0.206944,1.41694,-2.07475,0.329713,1.23477,-1.7549 [...]
+/*521*/{0.179487,1.9426,-1.73386,0.278608,1.88486,-1.85918,0.021554,1.85618,-1.68323,-0.104123,1.78448,-1.66477,0.033288,1.69677,-1.6715,0.102266,1.63902,-1.65843,0.250148,1.9612,-2.10727,0.301596,1.88558,-1.93194,0.107903,1.90789,-2.05604,0.154629,1.73248,-2.23405,0.212881,1.64664,-2.25769,0.335105,1.66881,-2.10601,0.398017,1.66533,-2.08809,0.20722,1.44387,-1.81745,0.021881,1.51973,-1.88506,0.026824,1.53733,-1.95049,0.031304,1.51345,-2.00142,0.207852,1.41913,-2.0743,0.329119,1.23373,-1. [...]
+/*522*/{0.17909,1.94408,-1.73357,0.278688,1.88683,-1.85878,0.021555,1.8577,-1.68395,-0.104004,1.78647,-1.66765,0.032044,1.69785,-1.67213,0.101489,1.64015,-1.65813,0.250573,1.96265,-2.10586,0.301292,1.88725,-1.93145,0.108359,1.90906,-2.05615,0.157776,1.73538,-2.23467,0.215006,1.64865,-2.25781,0.3361,1.67146,-2.10595,0.398749,1.66583,-2.08691,0.207024,1.44619,-1.8177,0.022184,1.52167,-1.88674,0.027931,1.53971,-1.95026,0.031643,1.51557,-2.00148,0.208247,1.42138,-2.0736,0.327524,1.23099,-1.7 [...]
+/*523*/{0.1795,1.94474,-1.7332,0.286486,1.88432,-1.85577,0.021001,1.85914,-1.68525,-0.104647,1.7878,-1.67063,0.030271,1.69921,-1.67306,0.09987,1.64061,-1.65759,0.251186,1.96415,-2.1052,0.302972,1.88841,-1.93043,0.109218,1.9112,-2.05606,0.160642,1.73711,-2.23612,0.217043,1.65008,-2.25865,0.337638,1.67141,-2.10521,0.399332,1.66589,-2.0858,0.207856,1.44759,-1.81763,0.023076,1.52387,-1.88603,0.027648,1.54218,-1.9501,0.032512,1.51717,-2.00134,0.208695,1.42336,-2.07313,0.325462,1.22692,-1.7572 [...]
+/*524*/{0.18022,1.94581,-1.7323,0.286799,1.88589,-1.85561,0.020759,1.85903,-1.68643,-0.106363,1.78854,-1.67287,0.030265,1.69935,-1.67301,0.099453,1.64063,-1.6574,0.251402,1.96532,-2.1048,0.302841,1.88951,-1.92978,0.110523,1.91252,-2.05616,0.162484,1.73869,-2.2363,0.218072,1.65124,-2.25885,0.337842,1.6714,-2.10512,0.399813,1.66518,-2.08493,0.207334,1.45,-1.81774,0.024155,1.52489,-1.8856,0.028313,1.54376,-1.94957,0.032848,1.51921,-2.00093,0.2096,1.42566,-2.07291,0.323012,1.22307,-1.75785,0 [...]
+/*525*/{0.180521,1.94633,-1.73159,0.28559,1.88792,-1.85532,0.020499,1.85913,-1.68683,-0.106411,1.78786,-1.6739,0.029748,1.7,-1.67356,0.099373,1.64024,-1.65564,0.25192,1.96606,-2.10445,0.303109,1.89078,-1.9295,0.110593,1.91333,-2.05578,0.163847,1.7401,-2.23655,0.219116,1.65167,-2.25913,0.338467,1.6701,-2.10469,0.40067,1.66403,-2.08447,0.208484,1.45116,-1.81812,0.024013,1.5262,-1.88506,0.027781,1.54566,-1.94929,0.033094,1.52087,-2.00088,0.210284,1.42766,-2.07243,0.321038,1.21727,-1.75811,0 [...]
+/*526*/{0.180952,1.94667,-1.73104,0.286169,1.88864,-1.85527,0.021004,1.8594,-1.68647,-0.106209,1.788,-1.67585,0.029414,1.6992,-1.67363,0.099205,1.6397,-1.65495,0.252716,1.96608,-2.10384,0.30328,1.89164,-1.92904,0.11123,1.91422,-2.05568,0.165035,1.74087,-2.23649,0.219589,1.65244,-2.25965,0.339041,1.66922,-2.10523,0.401059,1.6623,-2.0844,0.206567,1.45136,-1.81865,0.023806,1.52644,-1.88462,0.027541,1.5468,-1.94841,0.033656,1.5213,-2.00021,0.210513,1.42933,-2.07227,0.318524,1.21329,-1.75898, [...]
+/*527*/{0.181376,1.94649,-1.73038,0.287983,1.88853,-1.85388,0.021301,1.85828,-1.68671,-0.105893,1.7871,-1.6768,0.029877,1.69801,-1.67387,0.099378,1.63839,-1.65428,0.252854,1.96598,-2.10357,0.303691,1.8925,-1.9285,0.111539,1.91455,-2.05482,0.165618,1.74155,-2.23644,0.219335,1.6522,-2.2599,0.340814,1.66803,-2.10508,0.401649,1.65926,-2.08381,0.206228,1.45173,-1.81875,0.023605,1.52626,-1.88424,0.026837,1.54744,-1.94863,0.032623,1.5227,-1.99918,0.210887,1.43168,-2.07188,0.315117,1.20709,-1.75 [...]
+/*528*/{0.182043,1.94618,-1.72983,0.287966,1.88828,-1.85326,0.021585,1.85691,-1.68714,-0.104904,1.78491,-1.67766,0.030245,1.69648,-1.67346,0.099799,1.63586,-1.65274,0.253701,1.96521,-2.10312,0.304004,1.89263,-1.9279,0.111982,1.91437,-2.05424,0.165563,1.74139,-2.23596,0.21907,1.65182,-2.26012,0.341465,1.66586,-2.10543,0.401533,1.65588,-2.08391,0.206116,1.4518,-1.81892,0.023537,1.52619,-1.88382,0.02657,1.54757,-1.94786,0.032623,1.5227,-1.99918,0.211286,1.43295,-2.07169,0.310703,1.20057,-1. [...]
+/*529*/{0.183297,1.94527,-1.72881,0.288229,1.8882,-1.85298,0.022588,1.85543,-1.68701,-0.104687,1.78255,-1.6783,0.031592,1.69485,-1.67343,0.100624,1.63407,-1.6519,0.254691,1.96457,-2.10282,0.30457,1.89252,-1.92742,0.112359,1.91448,-2.05321,0.164181,1.74093,-2.23554,0.217538,1.65077,-2.25958,0.340958,1.66309,-2.10562,0.402353,1.65192,-2.08433,0.208565,1.45351,-1.81909,0.022985,1.52517,-1.88253,0.026145,1.54743,-1.94689,0.032294,1.52289,-1.99828,0.210812,1.43417,-2.07176,0.305854,1.19487,-1 [...]
+/*530*/{0.183827,1.94426,-1.72812,0.288566,1.88739,-1.85272,0.023774,1.85312,-1.6864,-0.103089,1.78013,-1.67876,0.033178,1.69101,-1.67228,0.102202,1.63044,-1.65109,0.254243,1.96332,-2.10231,0.305191,1.8919,-1.92711,0.11234,1.91427,-2.05307,0.163468,1.74067,-2.2348,0.215944,1.64998,-2.25982,0.341561,1.65929,-2.10634,0.401945,1.64716,-2.0848,0.207547,1.45304,-1.81964,0.022895,1.52402,-1.88325,0.026593,1.54603,-1.94647,0.031355,1.52195,-1.99791,0.211634,1.43486,-2.07164,0.303069,1.18736,-1. [...]
+/*531*/{0.184666,1.94302,-1.72764,0.288923,1.88646,-1.85252,0.024589,1.85009,-1.68569,-0.100954,1.77636,-1.67825,0.035504,1.68844,-1.67213,0.103916,1.62775,-1.64891,0.25443,1.96153,-2.10141,0.305148,1.89105,-1.9271,0.112344,1.91358,-2.05207,0.160843,1.73923,-2.23399,0.213873,1.64866,-2.25876,0.341418,1.65595,-2.10731,0.401731,1.64169,-2.086,0.208724,1.45294,-1.81967,0.022913,1.52198,-1.88219,0.026238,1.54556,-1.94574,0.031193,1.52039,-1.99714,0.211264,1.43493,-2.07236,0.29745,1.18158,-1. [...]
+/*532*/{0.185533,1.94154,-1.72665,0.289217,1.88534,-1.85282,0.02547,1.84734,-1.68487,-0.099838,1.77136,-1.6775,0.036588,1.68424,-1.67026,0.105914,1.62362,-1.64751,0.25499,1.95984,-2.10081,0.304413,1.88942,-1.92645,0.112118,1.91303,-2.05158,0.158713,1.73819,-2.23283,0.211779,1.6469,-2.25838,0.341087,1.65197,-2.10787,0.401204,1.63593,-2.08721,0.209234,1.4513,-1.82062,0.02254,1.51909,-1.88218,0.025121,1.54377,-1.94597,0.030978,1.51909,-1.99673,0.211181,1.43483,-2.07245,0.294502,1.17447,-1.7 [...]
+/*533*/{0.186494,1.93926,-1.72573,0.289301,1.88375,-1.85254,0.027032,1.84373,-1.68346,-0.096562,1.76776,-1.67623,0.039093,1.68043,-1.6688,0.108133,1.61939,-1.64588,0.255197,1.95784,-2.1008,0.304678,1.88782,-1.92603,0.111655,1.91122,-2.04977,0.155416,1.73654,-2.23141,0.208886,1.6454,-2.25789,0.340375,1.64658,-2.10859,0.40058,1.62951,-2.08814,0.207677,1.44976,-1.82052,0.021487,1.51763,-1.88155,0.024096,1.54146,-1.94602,0.030075,1.51746,-1.99675,0.211061,1.43368,-2.07225,0.291682,1.16832,-1 [...]
+/*534*/{0.186859,1.93684,-1.7254,0.288542,1.88219,-1.85227,0.028204,1.83976,-1.68144,-0.094575,1.76299,-1.67472,0.041984,1.67571,-1.66686,0.11077,1.61495,-1.64384,0.254928,1.95559,-2.10056,0.304294,1.8856,-1.92607,0.110947,1.90988,-2.04944,0.152842,1.7351,-2.22968,0.205547,1.6433,-2.25691,0.339934,1.64126,-2.11,0.399561,1.62326,-2.09027,0.206794,1.44729,-1.82074,0.021709,1.5142,-1.88088,0.024923,1.53853,-1.94576,0.029584,1.51431,-1.99614,0.211238,1.43226,-2.07296,0.287071,1.16361,-1.7563 [...]
+/*535*/{0.187559,1.93442,-1.72474,0.28887,1.87992,-1.85234,0.02934,1.83507,-1.68053,-0.09204,1.7569,-1.67251,0.044177,1.67063,-1.66468,0.113494,1.61023,-1.64185,0.254533,1.9529,-2.0999,0.304461,1.88307,-1.92587,0.11056,1.90789,-2.04871,0.14888,1.734,-2.22868,0.202454,1.64111,-2.25606,0.338947,1.63527,-2.11148,0.398292,1.61565,-2.092,0.208306,1.44498,-1.82032,0.020654,1.51155,-1.88045,0.024031,1.53556,-1.94556,0.029885,1.51108,-1.99571,0.211631,1.4312,-2.073,0.28578,1.15926,-1.75471,0.163 [...]
+/*536*/{0.18828,1.93117,-1.72401,0.288838,1.87737,-1.85235,0.030976,1.8303,-1.67806,-0.088408,1.75128,-1.67009,0.047569,1.66477,-1.6623,0.11753,1.60515,-1.63978,0.254332,1.94995,-2.09967,0.304072,1.87995,-1.92536,0.110524,1.90645,-2.04784,0.145123,1.73124,-2.22694,0.198582,1.63898,-2.25513,0.336737,1.62889,-2.11247,0.396581,1.60799,-2.09407,0.208403,1.44149,-1.82052,0.020722,1.50783,-1.88022,0.023234,1.53215,-1.94576,0.0289,1.50802,-1.9954,0.212191,1.42905,-2.07299,0.283014,1.15464,-1.75 [...]
+/*537*/{0.189265,1.92763,-1.72332,0.288575,1.8742,-1.85228,0.032189,1.82516,-1.67704,-0.085475,1.74454,-1.66733,0.050347,1.65889,-1.66041,0.120027,1.59961,-1.63816,0.25323,1.94707,-2.09963,0.303618,1.87749,-1.92562,0.110504,1.90383,-2.0466,0.141491,1.72903,-2.22508,0.195176,1.63638,-2.25405,0.335059,1.62234,-2.11367,0.394412,1.59977,-2.09665,0.205768,1.43827,-1.82043,0.019592,1.50415,-1.88055,0.022741,1.52892,-1.94622,0.02843,1.50419,-1.99625,0.212349,1.42667,-2.07316,0.280681,1.15244,-1 [...]
+/*538*/{0.189956,1.9241,-1.72236,0.28859,1.87047,-1.85251,0.033909,1.81925,-1.67482,-0.082552,1.73794,-1.66389,0.053812,1.65337,-1.6574,0.124138,1.59405,-1.63523,0.25313,1.94323,-2.09992,0.303627,1.87434,-1.9251,0.109556,1.90087,-2.04564,0.136788,1.72667,-2.22432,0.190204,1.63305,-2.25339,0.334133,1.61597,-2.11578,0.392361,1.59093,-2.0993,0.20858,1.43884,-1.82281,0.019789,1.50012,-1.88041,0.022042,1.52441,-1.9459,0.027703,1.5006,-1.99655,0.212212,1.42364,-2.07273,0.277488,1.15046,-1.7512 [...]
+/*539*/{0.190545,1.91969,-1.72186,0.288597,1.86683,-1.85259,0.035839,1.8138,-1.67153,-0.080188,1.73058,-1.66035,0.057058,1.64601,-1.6543,0.128375,1.58867,-1.63368,0.252293,1.93966,-2.09987,0.303851,1.8704,-1.92482,0.108509,1.89758,-2.0449,0.132793,1.7232,-2.22234,0.185268,1.62941,-2.25255,0.330041,1.60771,-2.11662,0.389602,1.58185,-2.10131,0.204532,1.43171,-1.8196,0.017525,1.49616,-1.88041,0.022156,1.52115,-1.94585,0.027197,1.49588,-1.99669,0.211919,1.42063,-2.07256,0.277359,1.14761,-1.7 [...]
+/*540*/{0.190181,1.91493,-1.72178,0.289628,1.8617,-1.85238,0.03765,1.80692,-1.66951,-0.076165,1.72374,-1.65647,0.060492,1.63933,-1.6509,0.13321,1.58268,-1.63108,0.251278,1.93579,-2.1002,0.302842,1.86614,-1.9254,0.108928,1.8942,-2.0443,0.128119,1.72068,-2.22069,0.18127,1.62585,-2.25131,0.327305,1.59976,-2.11819,0.386706,1.57209,-2.10427,0.204782,1.42831,-1.81876,0.017559,1.4928,-1.88099,0.021289,1.51726,-1.9458,0.027549,1.49408,-1.99692,0.21159,1.41734,-2.07273,0.275542,1.14581,-1.74959,0 [...]
+/*541*/{0.190832,1.9096,-1.72095,0.288528,1.85811,-1.85254,0.039677,1.80009,-1.66708,-0.074607,1.7151,-1.65195,0.06519,1.63246,-1.64739,0.136874,1.57667,-1.62949,0.250822,1.93152,-2.1007,0.303473,1.8621,-1.92521,0.107372,1.89088,-2.04325,0.124412,1.71538,-2.21955,0.17624,1.62232,-2.25054,0.323885,1.59137,-2.12045,0.383246,1.56176,-2.10792,0.203321,1.42447,-1.81854,0.01675,1.48848,-1.88104,0.020837,1.51301,-1.94597,0.026058,1.48639,-1.99678,0.211475,1.41399,-2.07198,0.274081,1.14345,-1.74 [...]
+/*542*/{0.191748,1.90469,-1.72036,0.289293,1.8522,-1.8523,0.041726,1.79279,-1.66426,-0.070101,1.70621,-1.64793,0.069014,1.62557,-1.64449,0.141782,1.57077,-1.62823,0.249127,1.92619,-2.10104,0.303081,1.85739,-1.92558,0.106924,1.88579,-2.0418,0.120295,1.71171,-2.21804,0.170676,1.61802,-2.25022,0.320518,1.58291,-2.12242,0.378966,1.5517,-2.11099,0.202654,1.42161,-1.81806,0.014746,1.48388,-1.88225,0.0208,1.51053,-1.94648,0.025482,1.48137,-1.99783,0.212139,1.41048,-2.07039,0.274057,1.14139,-1.7 [...]
+/*543*/{0.191793,1.89878,-1.7194,0.289246,1.84904,-1.85235,0.044173,1.78554,-1.66128,-0.066645,1.6968,-1.64253,0.073237,1.61829,-1.64113,0.147428,1.56446,-1.6258,0.249065,1.92174,-2.1019,0.303307,1.85262,-1.92604,0.10613,1.88166,-2.04096,0.116343,1.70777,-2.21714,0.165395,1.61362,-2.24942,0.316431,1.57441,-2.12465,0.374842,1.54104,-2.11426,0.202338,1.41816,-1.81701,0.014735,1.47858,-1.88241,0.019824,1.50466,-1.94706,0.023865,1.47653,-1.99827,0.211166,1.40573,-2.07141,0.272021,1.13713,-1. [...]
+/*544*/{0.192625,1.89327,-1.71854,0.288727,1.84354,-1.85313,0.046688,1.7771,-1.65831,-0.06355,1.68833,-1.63783,0.078509,1.6109,-1.63784,0.15273,1.55817,-1.62329,0.247244,1.91663,-2.1022,0.302948,1.84817,-1.92643,0.105591,1.8769,-2.04078,0.111302,1.70367,-2.2165,0.160372,1.60906,-2.24875,0.312754,1.56569,-2.12723,0.371372,1.52954,-2.11727,0.200655,1.4148,-1.81648,0.012654,1.47395,-1.88293,0.018184,1.49946,-1.947,0.023879,1.4704,-1.99901,0.211047,1.40205,-2.07116,0.270216,1.13389,-1.75014, [...]
+/*545*/{0.193249,1.88669,-1.7176,0.289046,1.83802,-1.85326,0.04977,1.76838,-1.65479,-0.059687,1.67893,-1.63227,0.083141,1.60292,-1.63411,0.15765,1.55216,-1.62109,0.246874,1.91147,-2.10307,0.30289,1.84279,-1.92683,0.103901,1.8718,-2.0406,0.106989,1.69958,-2.21591,0.154935,1.60484,-2.24845,0.307872,1.55542,-2.12948,0.364412,1.51896,-2.12202,0.199999,1.41077,-1.81499,0.012413,1.46782,-1.88324,0.017621,1.49454,-1.94677,0.023169,1.46456,-1.99928,0.210367,1.39794,-2.07004,0.270714,1.12881,-1.7 [...]
+/*546*/{0.194158,1.88065,-1.71633,0.288973,1.83318,-1.85351,0.053165,1.76013,-1.65144,-0.055148,1.66941,-1.62751,0.088881,1.59574,-1.63094,0.164499,1.54658,-1.61973,0.245259,1.90592,-2.10341,0.302839,1.83743,-1.92766,0.103107,1.86656,-2.03994,0.103331,1.69475,-2.21467,0.14885,1.59996,-2.24738,0.303446,1.54533,-2.13226,0.358842,1.50772,-2.12567,0.198272,1.40697,-1.81428,0.011137,1.46301,-1.88444,0.017202,1.48951,-1.94682,0.022096,1.45869,-1.99974,0.210299,1.39389,-2.06973,0.269209,1.1238, [...]
+/*547*/{0.194846,1.87453,-1.71549,0.290507,1.82616,-1.85356,0.05654,1.7513,-1.64828,-0.050318,1.65859,-1.62133,0.094324,1.58812,-1.62774,0.170762,1.54083,-1.61849,0.244631,1.90044,-2.10409,0.302668,1.83162,-1.92808,0.101852,1.86135,-2.0394,0.098346,1.69028,-2.21463,0.143924,1.5953,-2.24719,0.297739,1.53508,-2.13531,0.352089,1.49594,-2.1296,0.197714,1.40395,-1.81287,0.010662,1.45755,-1.88526,0.015933,1.4829,-1.94605,0.021171,1.45216,-2.00046,0.210482,1.38837,-2.06827,0.268069,1.11784,-1.7 [...]
+/*548*/{0.195857,1.86857,-1.71427,0.289116,1.82151,-1.85436,0.060236,1.74178,-1.6444,-0.044667,1.64979,-1.61597,0.099706,1.58078,-1.6255,0.177923,1.53533,-1.61775,0.243459,1.89522,-2.10515,0.302095,1.82635,-1.92896,0.100576,1.85634,-2.03906,0.094538,1.68575,-2.21353,0.137661,1.59087,-2.24754,0.292207,1.52418,-2.13837,0.345703,1.48437,-2.13351,0.197297,1.4003,-1.81094,0.008649,1.45212,-1.88607,0.014758,1.4772,-1.94617,0.019581,1.44729,-2.00094,0.209821,1.38372,-2.06736,0.264821,1.11079,-1 [...]
+/*549*/{0.197364,1.86278,-1.71316,0.290914,1.81498,-1.85424,0.0648,1.73268,-1.6411,-0.039428,1.63923,-1.61025,0.106725,1.57374,-1.62186,0.185082,1.53058,-1.61624,0.242004,1.89028,-2.10567,0.302683,1.82119,-1.92953,0.098383,1.85099,-2.03851,0.088781,1.68238,-2.21389,0.13157,1.58595,-2.24738,0.285967,1.51375,-2.14188,0.338288,1.47229,-2.13754,0.19577,1.39584,-1.81057,0.006543,1.44752,-1.88701,0.01338,1.47144,-1.94683,0.01854,1.44131,-2.00266,0.209501,1.37922,-2.06758,0.260992,1.1063,-1.746 [...]
+/*550*/{0.198627,1.85762,-1.71181,0.291039,1.81052,-1.85446,0.069926,1.72317,-1.63746,-0.032802,1.62981,-1.60469,0.11407,1.5672,-1.61919,0.192544,1.52474,-1.61483,0.241477,1.88597,-2.10639,0.302895,1.81529,-1.93068,0.097668,1.84658,-2.03628,0.085592,1.67676,-2.21314,0.124854,1.58094,-2.24726,0.279899,1.50335,-2.14526,0.331421,1.4614,-2.14119,0.192859,1.39249,-1.80992,0.003466,1.4428,-1.88783,0.010946,1.46501,-1.9481,0.016556,1.43567,-2.00375,0.208326,1.37363,-2.06681,0.254276,1.10417,-1. [...]
+/*551*/{0.199822,1.85277,-1.71073,0.291705,1.80332,-1.85498,0.074964,1.71475,-1.63492,-0.024712,1.62069,-1.59901,0.120931,1.56137,-1.61745,0.200694,1.52059,-1.61421,0.238657,1.88208,-2.10803,0.303852,1.81035,-1.93125,0.096475,1.84327,-2.03522,0.08163,1.67275,-2.21271,0.119116,1.57685,-2.24705,0.273708,1.49392,-2.1488,0.322498,1.45128,-2.14559,0.190346,1.38868,-1.80947,0.000246,1.43914,-1.88889,0.008148,1.46025,-1.94942,0.01349,1.43012,-2.00384,0.207282,1.36914,-2.0653,0.253821,1.10149,-1 [...]
+/*552*/{0.201172,1.8483,-1.70956,0.290853,1.79984,-1.85663,0.08086,1.70633,-1.63228,-0.01764,1.61176,-1.59377,0.129294,1.55604,-1.61634,0.208732,1.51658,-1.61354,0.237709,1.87819,-2.10926,0.303821,1.80624,-1.93344,0.095416,1.84211,-2.03287,0.0775,1.66905,-2.21243,0.112406,1.57285,-2.24622,0.266771,1.48604,-2.15217,0.314893,1.44152,-2.14884,0.187682,1.38607,-1.80882,-0.003329,1.43586,-1.88997,0.005895,1.4551,-1.9509,0.010863,1.42487,-2.00579,0.208241,1.36481,-2.06132,0.252183,1.10162,-1.7 [...]
+/*553*/{0.202681,1.84416,-1.70879,0.291729,1.79482,-1.85763,0.086353,1.69907,-1.63016,-0.009923,1.60347,-1.58863,0.137878,1.5512,-1.61518,0.21802,1.51273,-1.61322,0.23686,1.87481,-2.11084,0.304379,1.80212,-1.93478,0.094999,1.84007,-2.03054,0.073371,1.6665,-2.21168,0.106242,1.56983,-2.24556,0.25876,1.47688,-2.15476,0.30575,1.4325,-2.15251,0.185271,1.38372,-1.80746,-0.006453,1.43231,-1.89275,0.002959,1.4525,-1.951,0.00791,1.41991,-2.00732,0.205552,1.36049,-2.06118,0.248857,1.10186,-1.73487 [...]
+/*554*/{0.204193,1.84089,-1.70845,0.292385,1.79086,-1.858,0.092124,1.6932,-1.62883,-0.000274,1.59688,-1.58432,0.14764,1.54728,-1.61386,0.227687,1.51026,-1.61258,0.236248,1.87224,-2.11163,0.30427,1.79848,-1.93611,0.094007,1.83948,-2.02947,0.069468,1.6656,-2.20958,0.100156,1.56849,-2.24478,0.250776,1.47154,-2.15577,0.296637,1.42485,-2.15584,0.183102,1.38206,-1.80552,-0.009236,1.42978,-1.89483,0.001191,1.45068,-1.95172,0.005549,1.41486,-2.00791,0.204044,1.35695,-2.06003,0.244649,1.10242,-1. [...]
+/*555*/{0.205594,1.83878,-1.70913,0.291829,1.79001,-1.86025,0.097787,1.6889,-1.62735,0.009429,1.59145,-1.57946,0.156741,1.54384,-1.61293,0.237714,1.50846,-1.6125,0.236321,1.87008,-2.11214,0.302733,1.79706,-1.93758,0.093944,1.83917,-2.02955,0.065856,1.6655,-2.20735,0.092858,1.56858,-2.24354,0.241929,1.46666,-2.15758,0.28761,1.41806,-2.15939,0.17994,1.38117,-1.80351,-0.011214,1.42832,-1.89578,-0.00013,1.45079,-1.95311,0.003634,1.41174,-2.00972,0.202735,1.35484,-2.05966,0.24066,1.1016,-1.73 [...]
+/*556*/{0.206947,1.83692,-1.70953,0.291066,1.7868,-1.86017,0.103975,1.68539,-1.62587,0.019209,1.58629,-1.57608,0.165988,1.54191,-1.61186,0.248207,1.50759,-1.61176,0.23635,1.86867,-2.11121,0.302618,1.79559,-1.93768,0.093303,1.83805,-2.0293,0.060888,1.66713,-2.20509,0.086132,1.5703,-2.24218,0.233457,1.46315,-2.15845,0.277754,1.41324,-2.163,0.177696,1.38003,-1.8009,-0.012033,1.42781,-1.89593,-0.000873,1.44952,-1.95299,0.001667,1.41113,-2.01033,0.20094,1.35208,-2.0591,0.23762,1.09853,-1.7341 [...]
+/*557*/{0.207535,1.83551,-1.71039,0.29002,1.78628,-1.86065,0.109273,1.68295,-1.62474,0.027688,1.5821,-1.57252,0.176739,1.54027,-1.61109,0.258817,1.50818,-1.61327,0.23508,1.86692,-2.11097,0.300377,1.79368,-1.93831,0.093287,1.83799,-2.02951,0.055053,1.6702,-2.20142,0.079918,1.57286,-2.2409,0.225548,1.46135,-2.16024,0.268335,1.40922,-2.16597,0.174585,1.37965,-1.79997,-0.013711,1.42684,-1.89702,-0.003035,1.4494,-1.95287,0.000433,1.4111,-2.01084,0.198534,1.35064,-2.05917,0.234105,1.0972,-1.73 [...]
+/*558*/{0.208778,1.83402,-1.71086,0.290082,1.78511,-1.86115,0.114401,1.68131,-1.62374,0.037332,1.57957,-1.56932,0.186337,1.53973,-1.61021,0.269455,1.50965,-1.61366,0.235061,1.86595,-2.11068,0.300226,1.79302,-1.93791,0.092858,1.83813,-2.03009,0.049918,1.67384,-2.19795,0.07208,1.57622,-2.24022,0.216633,1.46031,-2.16204,0.25878,1.40791,-2.16861,0.173307,1.37954,-1.79822,-0.014466,1.42711,-1.89705,-0.003773,1.44972,-1.9531,-0.001042,1.41185,-2.01113,0.198002,1.35069,-2.05906,0.227707,1.09376 [...]
+/*559*/{0.209582,1.83376,-1.71169,0.290188,1.78434,-1.86148,0.119118,1.68039,-1.62272,0.046567,1.57572,-1.56614,0.196145,1.54009,-1.61034,0.280194,1.51189,-1.61487,0.234599,1.86585,-2.11066,0.299916,1.79202,-1.93823,0.092277,1.8383,-2.0308,0.043956,1.67861,-2.19504,0.064995,1.5802,-2.23958,0.20857,1.46122,-2.16376,0.248923,1.40692,-2.17125,0.170579,1.37924,-1.79689,-0.014879,1.42692,-1.89644,-0.005033,1.44861,-1.95198,-0.001383,1.41293,-2.00958,0.196567,1.35161,-2.05899,0.222637,1.08927, [...]
+/*560*/{0.210654,1.83389,-1.71169,0.289247,1.78259,-1.86203,0.124066,1.68031,-1.62139,0.056656,1.57362,-1.5629,0.205812,1.54123,-1.60974,0.290597,1.51559,-1.61632,0.233515,1.86611,-2.11073,0.298585,1.7915,-1.9386,0.091721,1.83905,-2.03052,0.039212,1.68444,-2.19135,0.058117,1.58507,-2.23949,0.200601,1.46261,-2.16632,0.239725,1.40793,-2.17349,0.169287,1.37911,-1.79673,-0.016216,1.42718,-1.8963,-0.00656,1.44929,-1.95171,-0.003106,1.41353,-2.00854,0.195623,1.35284,-2.05927,0.217443,1.0863,-1 [...]
+/*561*/{0.211817,1.83406,-1.7116,0.288748,1.78495,-1.8634,0.128273,1.68097,-1.62061,0.064829,1.57217,-1.56018,0.215213,1.54331,-1.60957,0.301341,1.52016,-1.61851,0.232238,1.86669,-2.11135,0.297819,1.79162,-1.93968,0.090683,1.84126,-2.02974,0.033602,1.69004,-2.18893,0.050139,1.59059,-2.23925,0.191591,1.46465,-2.16839,0.22977,1.4102,-2.1763,0.168206,1.3795,-1.79591,-0.017608,1.42761,-1.89607,-0.007698,1.44976,-1.95163,-0.00461,1.41475,-2.0074,0.193163,1.35525,-2.06048,0.21046,1.08283,-1.73 [...]
+/*562*/{0.213121,1.83608,-1.71182,0.288558,1.78529,-1.86373,0.132575,1.68162,-1.61878,0.074329,1.57086,-1.55723,0.224957,1.54688,-1.60994,0.311685,1.52517,-1.62054,0.230627,1.86809,-2.1115,0.296848,1.79269,-1.94003,0.089086,1.84219,-2.02929,0.027892,1.69604,-2.18667,0.042612,1.59631,-2.23946,0.183801,1.46819,-2.17082,0.220401,1.41308,-2.17817,0.167452,1.37975,-1.79504,-0.01806,1.42813,-1.89598,-0.00784,1.45068,-1.95132,-0.005818,1.41621,-2.00774,0.19273,1.35678,-2.06106,0.205226,1.08249, [...]
+/*563*/{0.214148,1.8384,-1.71224,0.288214,1.78737,-1.86499,0.136927,1.6835,-1.61839,0.083714,1.57126,-1.55542,0.233963,1.55091,-1.6102,0.320541,1.53109,-1.62312,0.22833,1.86996,-2.1117,0.295887,1.79364,-1.94103,0.087543,1.84482,-2.02898,0.023487,1.70246,-2.18507,0.035116,1.60257,-2.23928,0.175312,1.47242,-2.17259,0.211794,1.41711,-2.17988,0.167048,1.38056,-1.79582,-0.018734,1.42907,-1.8958,-0.008538,1.45154,-1.95078,-0.006093,1.41702,-2.0066,0.191992,1.35919,-2.0617,0.198542,1.08068,-1.7 [...]
+/*564*/{0.215754,1.84092,-1.71282,0.287575,1.78911,-1.86568,0.140658,1.6859,-1.61712,0.092044,1.57205,-1.5531,0.242285,1.55531,-1.61142,0.329939,1.53815,-1.62623,0.225537,1.87243,-2.11203,0.294419,1.79554,-1.94228,0.086025,1.84614,-2.02747,0.017651,1.70855,-2.18452,0.026968,1.60922,-2.23976,0.167669,1.47779,-2.17435,0.202415,1.42189,-2.18161,0.166911,1.38085,-1.79528,-0.018558,1.4303,-1.89618,-0.009061,1.45345,-1.95087,-0.006827,1.41839,-2.00644,0.19169,1.36218,-2.06244,0.189945,1.07792, [...]
+/*565*/{0.216782,1.84411,-1.71362,0.28679,1.79103,-1.86698,0.145932,1.68838,-1.6169,0.100953,1.57289,-1.55158,0.250618,1.56051,-1.61323,0.33808,1.54515,-1.6294,0.222304,1.8749,-2.11175,0.293587,1.79774,-1.94309,0.083595,1.84822,-2.02627,0.011335,1.71471,-2.18373,0.019758,1.61604,-2.23956,0.159184,1.48343,-2.17554,0.193991,1.42754,-2.18275,0.166912,1.38172,-1.79604,-0.019123,1.4324,-1.89555,-0.008282,1.45513,-1.95042,-0.007439,1.42035,-2.00611,0.190983,1.36468,-2.06278,0.182629,1.07574,-1 [...]
+/*566*/{0.218708,1.84759,-1.71411,0.287456,1.79443,-1.86806,0.149777,1.69094,-1.61647,0.108891,1.57482,-1.55035,0.258006,1.56583,-1.61463,0.345702,1.55288,-1.63282,0.218611,1.87826,-2.11222,0.292445,1.80019,-1.94462,0.081882,1.85119,-2.02503,0.005254,1.72156,-2.1838,0.012157,1.62291,-2.24005,0.151346,1.48986,-2.17663,0.18596,1.43397,-2.18382,0.167318,1.38189,-1.79621,-0.018268,1.43433,-1.89485,-0.008583,1.4566,-1.95022,-0.006517,1.4223,-2.00614,0.190858,1.36775,-2.06316,0.178195,1.07684, [...]
+/*567*/{0.220109,1.85118,-1.71467,0.286626,1.79801,-1.86945,0.155142,1.69396,-1.61584,0.118007,1.5771,-1.54962,0.264744,1.57192,-1.61641,0.35231,1.56084,-1.63637,0.215456,1.8819,-2.11251,0.291257,1.8035,-1.9458,0.080306,1.85274,-2.02268,-0.000626,1.72792,-2.18344,0.004464,1.62974,-2.24047,0.143809,1.49696,-2.17671,0.177954,1.44077,-2.18464,0.167266,1.38341,-1.79643,-0.016403,1.43815,-1.89486,-0.008004,1.45868,-1.94959,-0.006098,1.42403,-2.00608,0.190709,1.37092,-2.06304,0.170494,1.07377, [...]
+/*568*/{0.22198,1.85506,-1.71549,0.286467,1.80085,-1.87118,0.161355,1.69705,-1.61533,0.125703,1.58,-1.5483,0.271792,1.57852,-1.61872,0.358928,1.56994,-1.64007,0.211627,1.88607,-2.11272,0.290183,1.80704,-1.94792,0.077966,1.85537,-2.0209,-0.006249,1.73449,-2.18299,-0.002032,1.63674,-2.24001,0.136203,1.50406,-2.17765,0.170685,1.44784,-2.18577,0.167752,1.38439,-1.79661,-0.017586,1.43869,-1.89455,-0.007848,1.46015,-1.94873,-0.006328,1.42812,-2.00633,0.190981,1.37432,-2.06329,0.162556,1.07169, [...]
+/*569*/{0.223606,1.85954,-1.71619,0.286357,1.8042,-1.87147,0.166707,1.70024,-1.61549,0.132807,1.58244,-1.54772,0.277356,1.58539,-1.6211,0.365066,1.57866,-1.64425,0.206401,1.88982,-2.11182,0.289927,1.81091,-1.9487,0.075359,1.85785,-2.01923,-0.011879,1.74195,-2.18195,-0.009065,1.64368,-2.24006,0.12886,1.51125,-2.17848,0.163464,1.45592,-2.18627,0.168532,1.3856,-1.79662,-0.015746,1.44113,-1.89426,-0.007024,1.46347,-1.94741,-0.004904,1.42942,-2.0058,0.190756,1.37792,-2.06269,0.156587,1.07171, [...]
+/*570*/{0.225549,1.86437,-1.71686,0.287444,1.80955,-1.87363,0.172717,1.70416,-1.61558,0.141641,1.58611,-1.54751,0.284059,1.59267,-1.62352,0.369315,1.58734,-1.64844,0.202634,1.89456,-2.11191,0.288658,1.81547,-1.9502,0.073901,1.85995,-2.01694,-0.017224,1.74908,-2.18123,-0.015775,1.65049,-2.23972,0.122711,1.51988,-2.17852,0.155976,1.46346,-2.18666,0.169554,1.38707,-1.79576,-0.015418,1.44358,-1.89291,-0.006603,1.4663,-1.94716,-0.004039,1.43276,-2.00511,0.192965,1.38292,-2.06244,0.149887,1.07 [...]
+/*571*/{0.227375,1.86906,-1.71773,0.286792,1.81297,-1.87488,0.17888,1.70777,-1.61595,0.148285,1.5905,-1.54736,0.288471,1.59945,-1.62585,0.375129,1.59657,-1.65267,0.198665,1.89907,-2.1119,0.287793,1.81972,-1.9517,0.071491,1.86209,-2.01554,-0.022437,1.75614,-2.18065,-0.021801,1.65808,-2.23908,0.117228,1.52746,-2.17771,0.149841,1.47185,-2.18772,0.170738,1.38746,-1.79571,-0.013838,1.44594,-1.89215,-0.005174,1.46897,-1.94503,-0.003573,1.43637,-2.0052,0.193136,1.38691,-2.0614,0.144221,1.07454, [...]
+/*572*/{0.228731,1.87419,-1.71863,0.28628,1.81837,-1.87662,0.184502,1.71215,-1.61636,0.154457,1.59451,-1.54658,0.293035,1.60681,-1.62808,0.378699,1.60547,-1.6566,0.194919,1.90428,-2.1119,0.287424,1.82443,-1.95345,0.069283,1.8659,-2.01295,-0.026938,1.76289,-2.17934,-0.028339,1.66508,-2.2383,0.108987,1.5355,-2.17844,0.14345,1.4798,-2.18854,0.172613,1.38906,-1.79479,-0.012731,1.44926,-1.89047,-0.005015,1.47167,-1.94381,-0.002586,1.44063,-2.00541,0.193778,1.39114,-2.06044,0.13806,1.07319,-1. [...]
+/*573*/{0.230059,1.87961,-1.72003,0.286552,1.82287,-1.87761,0.190812,1.71663,-1.61722,0.161072,1.59882,-1.54681,0.297628,1.61454,-1.63052,0.382517,1.6145,-1.66081,0.19089,1.90867,-2.11114,0.285974,1.82922,-1.95492,0.06779,1.86933,-2.01054,-0.032053,1.76973,-2.17864,-0.034109,1.67231,-2.23719,0.102429,1.54398,-2.17861,0.13753,1.48819,-2.18904,0.172569,1.39048,-1.7947,-0.01092,1.45212,-1.89111,-0.002931,1.47498,-1.94332,-0.001599,1.44552,-2.00508,0.194741,1.39539,-2.05954,0.133059,1.07537, [...]
+/*574*/{0.231359,1.88491,-1.72103,0.28764,1.82809,-1.87931,0.195364,1.72187,-1.61808,0.167568,1.60365,-1.54661,0.301183,1.62162,-1.63296,0.385908,1.62312,-1.66468,0.187409,1.91334,-2.11064,0.284946,1.83427,-1.95603,0.066909,1.87153,-2.01068,-0.036076,1.7759,-2.17791,-0.040125,1.67994,-2.23618,0.096943,1.55094,-2.17795,0.132237,1.49655,-2.18972,0.174266,1.39149,-1.79394,-0.008969,1.45562,-1.89058,-0.002218,1.47862,-1.9417,0.000559,1.44998,-2.00508,0.19639,1.40099,-2.05918,0.127656,1.0761, [...]
+/*575*/{0.231915,1.89031,-1.72271,0.287353,1.83298,-1.88083,0.200195,1.72687,-1.61882,0.172798,1.60846,-1.54616,0.304302,1.62873,-1.63541,0.388702,1.63243,-1.66874,0.184638,1.91793,-2.11006,0.284894,1.83927,-1.95769,0.06414,1.87698,-2.00739,-0.04064,1.78252,-2.1773,-0.046037,1.68705,-2.23493,0.090996,1.55881,-2.17767,0.126975,1.50481,-2.19064,0.175958,1.39312,-1.7943,-0.007485,1.459,-1.89026,-0.00042,1.48265,-1.94056,0.001382,1.45465,-2.00496,0.197135,1.40568,-2.05802,0.122894,1.07713,-1 [...]
+/*576*/{0.232954,1.89585,-1.72419,0.287128,1.8385,-1.8827,0.204559,1.73242,-1.61971,0.17787,1.61322,-1.54631,0.307627,1.63633,-1.6376,0.390536,1.64106,-1.67255,0.181694,1.92253,-2.10955,0.284419,1.84437,-1.95887,0.065157,1.87885,-2.00592,-0.045077,1.78811,-2.17609,-0.051043,1.69382,-2.23272,0.086258,1.56706,-2.17675,0.122098,1.51235,-2.191,0.177721,1.39454,-1.79446,-0.005083,1.46284,-1.88989,0.000887,1.48639,-1.93948,0.003056,1.45992,-2.00476,0.19731,1.40978,-2.05698,0.115673,1.07905,-1. [...]
+/*577*/{0.234392,1.90156,-1.72535,0.287128,1.84216,-1.8843,0.207391,1.73777,-1.62047,0.183377,1.61904,-1.54656,0.310334,1.64319,-1.64015,0.393049,1.64878,-1.67633,0.17917,1.92684,-2.10901,0.283373,1.84914,-1.96055,0.063433,1.88202,-2.00397,-0.048666,1.79437,-2.17521,-0.056478,1.70083,-2.23168,0.080762,1.5745,-2.17618,0.117322,1.52043,-2.19148,0.179954,1.39616,-1.79475,-0.003526,1.46627,-1.88866,0.002932,1.49012,-1.93792,0.004913,1.46491,-2.00448,0.198506,1.41541,-2.05641,0.111765,1.08164 [...]
+/*578*/{0.234531,1.90626,-1.72681,0.287392,1.84756,-1.8861,0.211219,1.74318,-1.62103,0.186192,1.62399,-1.54667,0.311881,1.65052,-1.64277,0.394359,1.65712,-1.67995,0.17721,1.9309,-2.10916,0.281932,1.85413,-1.96217,0.062941,1.88561,-2.00351,-0.05136,1.80031,-2.17449,-0.061569,1.70746,-2.22958,0.077259,1.58199,-2.17542,0.113133,1.52712,-2.19205,0.181566,1.39808,-1.79535,-0.001225,1.47,-1.8884,0.004952,1.49416,-1.93689,0.007118,1.47006,-2.00462,0.199254,1.41992,-2.05628,0.109665,1.0839,-1.76 [...]
+/*579*/{0.235947,1.91157,-1.72811,0.286572,1.85189,-1.88721,0.214584,1.748,-1.62188,0.190701,1.62996,-1.54681,0.314354,1.6571,-1.64481,0.396278,1.66423,-1.68322,0.174761,1.93438,-2.10865,0.281772,1.85864,-1.9638,0.061904,1.88878,-2.00193,-0.055331,1.80569,-2.17233,-0.065648,1.71387,-2.22774,0.071926,1.5885,-2.17536,0.109339,1.53456,-2.1926,0.184161,1.39944,-1.79651,0.001038,1.47365,-1.88815,0.006306,1.49858,-1.93618,0.008269,1.47508,-2.00456,0.199201,1.42457,-2.0561,0.105806,1.08684,-1.7 [...]
+/*580*/{0.236608,1.91619,-1.72956,0.287282,1.85596,-1.88858,0.216563,1.75336,-1.62231,0.194083,1.63409,-1.5471,0.315395,1.6638,-1.64757,0.396736,1.67164,-1.68597,0.173463,1.93793,-2.1089,0.281427,1.86264,-1.96485,0.06119,1.89201,-2.00107,-0.058845,1.81056,-2.17114,-0.070605,1.71976,-2.22586,0.068939,1.59526,-2.17461,0.105269,1.54084,-2.19306,0.186844,1.40084,-1.79737,0.00272,1.4778,-1.88782,0.008079,1.50247,-1.93493,0.010827,1.48029,-2.00424,0.20007,1.42938,-2.05623,0.100581,1.09037,-1.7 [...]
+/*581*/{0.237361,1.92137,-1.73061,0.28759,1.86084,-1.89027,0.21848,1.75842,-1.62274,0.196853,1.6392,-1.54721,0.316555,1.66996,-1.64948,0.397146,1.67903,-1.69001,0.171696,1.94118,-2.10866,0.280554,1.86667,-1.9657,0.060497,1.89508,-2.00065,-0.060883,1.81478,-2.16722,-0.074739,1.72552,-2.22315,0.06485,1.60101,-2.17388,0.102065,1.54706,-2.19318,0.188887,1.40298,-1.79887,0.004848,1.48094,-1.88752,0.008814,1.50608,-1.93312,0.012734,1.48482,-2.00419,0.199823,1.43375,-2.05611,0.096806,1.09223,-1 [...]
+/*582*/{0.237607,1.9256,-1.73156,0.288312,1.86537,-1.89204,0.220133,1.76321,-1.62328,0.200754,1.64423,-1.54739,0.317321,1.67614,-1.6509,0.398126,1.68604,-1.69298,0.170449,1.94421,-2.10882,0.28037,1.87035,-1.96746,0.060819,1.89654,-2,-0.061135,1.81905,-2.16605,-0.078224,1.73068,-2.22148,0.061509,1.60609,-2.17307,0.099339,1.55294,-2.19373,0.191257,1.405,-1.79998,0.007487,1.48475,-1.88631,0.010271,1.50973,-1.93251,0.014372,1.48976,-2.00363,0.200835,1.43875,-2.05653,0.094016,1.0956,-1.76674, [...]
+/*583*/{0.239113,1.92926,-1.73353,0.287757,1.86865,-1.89324,0.220884,1.76753,-1.62375,0.20246,1.64833,-1.54704,0.318792,1.6805,-1.65293,0.398562,1.69127,-1.69553,0.169137,1.94678,-2.10865,0.279738,1.87455,-1.96773,0.060846,1.89967,-1.99919,-0.061334,1.82406,-2.16462,-0.08145,1.73587,-2.21901,0.059309,1.61134,-2.17237,0.096447,1.55797,-2.19396,0.193795,1.4067,-1.80175,0.009302,1.48881,-1.88553,0.012635,1.514,-1.93126,0.0161,1.4943,-2.00291,0.20034,1.44258,-2.05648,0.091123,1.09806,-1.7669 [...]
+/*584*/{0.240359,1.93421,-1.73502,0.288591,1.87299,-1.89401,0.222135,1.77261,-1.62397,0.204831,1.65262,-1.54764,0.319567,1.68576,-1.654,0.398383,1.69693,-1.69833,0.168676,1.94957,-2.1084,0.279764,1.87744,-1.96895,0.060992,1.90112,-1.99827,-0.063696,1.8283,-2.16384,-0.084719,1.74086,-2.21618,0.057012,1.6162,-2.17226,0.094508,1.56268,-2.1938,0.195745,1.40953,-1.80222,0.011053,1.49211,-1.88542,0.014276,1.51681,-1.92975,0.017711,1.49879,-2.00265,0.20095,1.4468,-2.05715,0.087108,1.10091,-1.76 [...]
+/*585*/{0.241191,1.93696,-1.73598,0.288403,1.87616,-1.89556,0.223329,1.77694,-1.62479,0.206548,1.65664,-1.54724,0.319618,1.68997,-1.65567,0.397335,1.70238,-1.70183,0.168646,1.95112,-2.10815,0.279924,1.88075,-1.96972,0.060486,1.90365,-1.99818,-0.067293,1.83338,-2.15635,-0.086713,1.74477,-2.21402,0.055825,1.62049,-2.17201,0.092696,1.56688,-2.19367,0.198052,1.41088,-1.80409,0.012739,1.496,-1.88456,0.015474,1.52053,-1.9293,0.019369,1.50251,-2.00181,0.20164,1.45022,-2.05719,0.085559,1.10406,- [...]
+/*586*/{0.24163,1.94034,-1.73735,0.289463,1.87919,-1.8964,0.223755,1.78098,-1.62494,0.207629,1.65992,-1.5479,0.319435,1.69392,-1.65758,0.397564,1.70642,-1.7039,0.166547,1.95401,-2.10844,0.279578,1.8833,-1.97102,0.061295,1.90608,-1.99736,-0.065388,1.83797,-2.15399,-0.088746,1.74864,-2.21185,0.054543,1.62339,-2.17158,0.091141,1.57066,-2.19411,0.199108,1.4133,-1.80406,0.014153,1.4994,-1.88435,0.016403,1.52382,-1.92797,0.020499,1.50671,-2.00038,0.201457,1.45316,-2.05748,0.083446,1.10807,-1.7 [...]
+/*587*/{0.242921,1.94303,-1.7379,0.289486,1.8821,-1.89736,0.224193,1.78505,-1.62563,0.208215,1.66313,-1.54713,0.319795,1.69744,-1.6586,0.397013,1.71028,-1.7061,0.166985,1.95559,-2.10866,0.279435,1.88576,-1.97115,0.060745,1.90832,-1.99713,-0.066835,1.84132,-2.15136,-0.090584,1.75205,-2.20935,0.053034,1.62665,-2.17084,0.090017,1.57396,-2.19395,0.200624,1.41515,-1.80486,0.015737,1.50259,-1.88254,0.017828,1.52769,-1.92697,0.020592,1.51041,-1.99876,0.202088,1.45686,-2.05719,0.080503,1.11008,- [...]
+/*588*/{0.244078,1.94625,-1.73969,0.290625,1.88523,-1.89833,0.225066,1.78838,-1.62553,0.208593,1.66655,-1.54796,0.31963,1.70086,-1.6604,0.396716,1.71334,-1.70809,0.166922,1.95781,-2.109,0.280174,1.88792,-1.97277,0.060765,1.91035,-1.99696,-0.067468,1.84291,-2.1486,-0.091696,1.75509,-2.20729,0.052276,1.62907,-2.17072,0.089816,1.5771,-2.19411,0.202015,1.41794,-1.80529,0.01706,1.50612,-1.88187,0.018623,1.53108,-1.9253,0.021599,1.51391,-1.99789,0.202203,1.45895,-2.05695,0.080056,1.11281,-1.76 [...]
+/*589*/{0.245403,1.94875,-1.74036,0.290895,1.88737,-1.89949,0.225902,1.79114,-1.62661,0.208735,1.66926,-1.5472,0.319026,1.7033,-1.66101,0.396288,1.71593,-1.71004,0.166516,1.95944,-2.1098,0.280283,1.89029,-1.97332,0.062737,1.91174,-1.99735,-0.066445,1.84629,-2.14707,-0.092432,1.75747,-2.20562,0.051836,1.63151,-2.17015,0.089281,1.57917,-2.19395,0.202875,1.41973,-1.8058,0.017852,1.50876,-1.88077,0.019217,1.53487,-1.9245,0.020929,1.51726,-1.99639,0.201985,1.46167,-2.05645,0.078494,1.1153,-1. [...]
+/*590*/{0.246221,1.95043,-1.74138,0.291733,1.88953,-1.90052,0.226146,1.79408,-1.62679,0.209409,1.67205,-1.54702,0.319151,1.70547,-1.66319,0.395015,1.71872,-1.71301,0.166213,1.96087,-2.10962,0.281234,1.89147,-1.97486,0.062885,1.91334,-1.9973,-0.066702,1.84821,-2.14459,-0.092666,1.7592,-2.20376,0.051884,1.63293,-2.17011,0.089519,1.58114,-2.19421,0.20323,1.42208,-1.80553,0.019217,1.51167,-1.87977,0.019212,1.53707,-1.92446,0.020783,1.52065,-1.99471,0.202107,1.46423,-2.05638,0.079128,1.11894, [...]
+/*591*/{0.247302,1.95236,-1.74245,0.29078,1.89112,-1.90158,0.226349,1.79632,-1.62718,0.210087,1.67393,-1.54649,0.318266,1.70628,-1.66261,0.39443,1.71938,-1.71342,0.166654,1.96153,-2.11035,0.282019,1.89236,-1.97545,0.064422,1.91541,-1.99757,-0.066745,1.84882,-2.14248,-0.093094,1.76026,-2.2025,0.051965,1.63417,-2.16947,0.089693,1.5825,-2.19379,0.203589,1.42403,-1.80503,0.0198,1.51373,-1.87848,0.019949,1.53937,-1.92363,0.013628,1.52789,-1.99518,0.202219,1.4667,-2.05598,0.081185,1.1185,-1.77 [...]
+/*592*/{0.248631,1.95369,-1.74282,0.291907,1.89279,-1.90272,0.226524,1.79838,-1.6278,0.209281,1.67565,-1.54689,0.317681,1.70773,-1.66247,0.392976,1.71995,-1.71526,0.168578,1.96411,-2.11157,0.282353,1.89331,-1.97633,0.063537,1.917,-1.99777,-0.066948,1.84984,-2.14092,-0.093269,1.76028,-2.20115,0.053767,1.63552,-2.16917,0.090317,1.58356,-2.19413,0.203497,1.4259,-1.80519,0.020943,1.51546,-1.87692,0.020631,1.54204,-1.92238,0.023472,1.52389,-1.99198,0.202779,1.46808,-2.05604,0.083144,1.1198,-1 [...]
+/*593*/{0.24934,1.95465,-1.74383,0.292012,1.89411,-1.90398,0.226203,1.80025,-1.62799,0.209057,1.67802,-1.54715,0.316435,1.70789,-1.66348,0.391856,1.71948,-1.71664,0.169104,1.96461,-2.11224,0.28266,1.89377,-1.97774,0.064828,1.91848,-1.99741,-0.06551,1.85016,-2.13984,-0.093137,1.76032,-2.19992,0.053949,1.6357,-2.16839,0.091871,1.58411,-2.19411,0.204142,1.42727,-1.80457,0.021157,1.51772,-1.87555,0.020165,1.5435,-1.92205,0.024339,1.52418,-1.99188,0.203641,1.46988,-2.05534,0.086831,1.11966,-1 [...]
+/*594*/{0.250429,1.95496,-1.74475,0.292577,1.89474,-1.90442,0.225622,1.80117,-1.62876,0.207125,1.67805,-1.54681,0.315587,1.70748,-1.66486,0.390682,1.719,-1.71778,0.170409,1.96508,-2.11362,0.283065,1.8935,-1.97791,0.065044,1.91927,-1.99832,-0.065322,1.84913,-2.1379,-0.092339,1.75956,-2.19884,0.055127,1.63513,-2.16813,0.093234,1.58372,-2.19431,0.204925,1.42859,-1.8054,0.020931,1.51813,-1.87514,0.020373,1.54486,-1.92154,0.024399,1.52634,-1.99089,0.203262,1.47087,-2.05466,0.090176,1.11979,-1 [...]
+/*595*/{0.250897,1.95496,-1.74554,0.292325,1.89482,-1.90554,0.225569,1.80174,-1.62937,0.205951,1.6796,-1.54756,0.314535,1.70809,-1.66652,0.390616,1.71852,-1.71914,0.170694,1.96486,-2.11415,0.283388,1.89365,-1.97882,0.067045,1.92009,-1.99982,-0.064705,1.84825,-2.13722,-0.092139,1.75852,-2.19863,0.057035,1.63488,-2.16821,0.094603,1.58315,-2.19429,0.204775,1.42961,-1.80546,0.020658,1.51936,-1.87346,0.019812,1.54606,-1.92102,0.02345,1.52668,-1.99013,0.204231,1.47217,-2.0544,0.095293,1.11905, [...]
+/*596*/{0.251427,1.95504,-1.74637,0.292715,1.895,-1.90628,0.225354,1.80173,-1.63047,0.204379,1.67952,-1.54758,0.312756,1.70619,-1.66659,0.389983,1.71762,-1.71955,0.171244,1.96482,-2.11503,0.28365,1.89357,-1.97947,0.06752,1.9205,-2.00091,-0.064576,1.84676,-2.13542,-0.090664,1.75699,-2.198,0.058454,1.63349,-2.16792,0.096115,1.58231,-2.19446,0.205092,1.43021,-1.80508,0.020461,1.51969,-1.87336,0.018776,1.54641,-1.92016,0.023173,1.52664,-1.98984,0.204295,1.47232,-2.05376,0.099291,1.11845,-1.7 [...]
+/*597*/{0.25277,1.95446,-1.74707,0.29305,1.89477,-1.90674,0.223921,1.80181,-1.63123,0.201932,1.67912,-1.54778,0.311539,1.70471,-1.66691,0.386052,1.71228,-1.72048,0.171997,1.96447,-2.1171,0.284484,1.89352,-1.97996,0.067984,1.92035,-2.00175,-0.063593,1.84368,-2.13451,-0.089321,1.755,-2.19717,0.059989,1.63129,-2.16764,0.098208,1.58018,-2.19464,0.205111,1.43116,-1.80528,0.020633,1.51927,-1.8733,0.019729,1.54596,-1.92066,0.022961,1.52637,-1.98911,0.204457,1.47251,-2.05347,0.104915,1.11678,-1. [...]
+/*598*/{0.253471,1.95396,-1.74737,0.29336,1.89497,-1.90716,0.222915,1.80087,-1.63191,0.199969,1.67788,-1.54806,0.309947,1.70249,-1.66783,0.385214,1.70993,-1.72111,0.173205,1.96311,-2.11721,0.284426,1.89325,-1.98046,0.068909,1.92007,-2.00227,-0.062985,1.84178,-2.13404,-0.087801,1.75259,-2.19723,0.06333,1.62945,-2.16825,0.100259,1.57812,-2.19509,0.205106,1.43148,-1.8051,0.02064,1.51817,-1.87266,0.019729,1.54596,-1.92066,0.02218,1.52516,-1.98892,0.204159,1.47239,-2.05251,0.109911,1.11518,-1 [...]
+/*599*/{0.253321,1.95248,-1.74778,0.293625,1.89364,-1.90779,0.22251,1.80025,-1.63237,0.197554,1.67767,-1.54862,0.307915,1.69944,-1.66759,0.383355,1.70555,-1.72095,0.173139,1.96194,-2.1178,0.284544,1.892,-1.98054,0.069939,1.91962,-2.00381,-0.061276,1.83894,-2.13399,-0.085928,1.74966,-2.19754,0.064665,1.62722,-2.16781,0.102716,1.57545,-2.19556,0.205179,1.43124,-1.80508,0.020632,1.51721,-1.87217,0.019266,1.54522,-1.92004,0.021667,1.52435,-1.98869,0.204317,1.47192,-2.0523,0.116252,1.11358,-1 [...]
+/*600*/{0.253094,1.95081,-1.74792,0.292226,1.89237,-1.90727,0.220144,1.79859,-1.63333,0.195235,1.67648,-1.5491,0.305924,1.69709,-1.66844,0.381346,1.70136,-1.72137,0.174474,1.9608,-2.11883,0.284472,1.89091,-1.98074,0.070424,1.91909,-2.00498,-0.060226,1.83488,-2.1332,-0.08386,1.74653,-2.19779,0.066055,1.62414,-2.16814,0.105454,1.57245,-2.19622,0.205159,1.43029,-1.80496,0.020361,1.51566,-1.87185,0.019117,1.54416,-1.91968,0.021654,1.5215,-1.98851,0.204589,1.47151,-2.05246,0.121396,1.11147,-1 [...]
+/*601*/{0.253169,1.94906,-1.74905,0.292571,1.89054,-1.90815,0.218741,1.79748,-1.63386,0.19142,1.67443,-1.54967,0.303586,1.69339,-1.66928,0.379658,1.69678,-1.72128,0.175653,1.95897,-2.11972,0.284059,1.88973,-1.98073,0.070717,1.91868,-2.00637,-0.058389,1.83203,-2.1335,-0.082087,1.74281,-2.19852,0.068703,1.62004,-2.16902,0.107458,1.56891,-2.19711,0.205288,1.42988,-1.8053,0.019993,1.51345,-1.87213,0.018388,1.54229,-1.91974,0.020892,1.52023,-1.9887,0.204839,1.47021,-2.0524,0.126617,1.10889,-1 [...]
+/*602*/{0.25311,1.94717,-1.74956,0.292282,1.88982,-1.9087,0.216406,1.79521,-1.63461,0.187289,1.67272,-1.55122,0.301229,1.68919,-1.66883,0.377361,1.69128,-1.7212,0.176141,1.95686,-2.12023,0.283867,1.88764,-1.98068,0.071178,1.91641,-2.00784,-0.057332,1.82781,-2.13335,-0.07965,1.73888,-2.19895,0.072228,1.61673,-2.17003,0.109993,1.56508,-2.19781,0.205523,1.4289,-1.80485,0.019265,1.51167,-1.87225,0.017461,1.54041,-1.91946,0.020208,1.51735,-1.98881,0.204476,1.46815,-2.05258,0.132313,1.1066,-1. [...]
+/*603*/{0.251898,1.94487,-1.75013,0.291253,1.88765,-1.90878,0.214455,1.79337,-1.63497,0.183499,1.67047,-1.55166,0.298333,1.6847,-1.66802,0.375629,1.68585,-1.72116,0.177357,1.95486,-2.12087,0.283717,1.8862,-1.98057,0.071294,1.9155,-2.0087,-0.055118,1.8249,-2.1343,-0.077467,1.73463,-2.20057,0.074244,1.61255,-2.17102,0.112684,1.56108,-2.19843,0.205673,1.42768,-1.80524,0.019358,1.50968,-1.87211,0.017902,1.53761,-1.91968,0.02033,1.51502,-1.98866,0.204097,1.46566,-2.05357,0.136786,1.10433,-1.7 [...]
+/*604*/{0.251314,1.94291,-1.75026,0.290442,1.88534,-1.90886,0.211932,1.7913,-1.6357,0.179645,1.66856,-1.55281,0.29488,1.68022,-1.66914,0.372324,1.67946,-1.72035,0.177318,1.9522,-2.12117,0.283103,1.88356,-1.98074,0.071612,1.91292,-2.00998,-0.054735,1.82021,-2.13576,-0.074857,1.72998,-2.20195,0.07697,1.60782,-2.17199,0.115649,1.55634,-2.19933,0.206053,1.4259,-1.80567,0.01889,1.5061,-1.87277,0.016804,1.53561,-1.92018,0.0195,1.51301,-1.98949,0.203489,1.46308,-2.05458,0.142084,1.10136,-1.7948 [...]
+/*605*/{0.250318,1.93948,-1.75047,0.288764,1.88195,-1.90859,0.209104,1.78845,-1.63593,0.174178,1.66599,-1.55423,0.292188,1.67564,-1.66889,0.370414,1.67248,-1.71937,0.178374,1.94905,-2.12174,0.283343,1.88086,-1.98017,0.071401,1.91116,-2.01169,-0.053,1.81541,-2.13687,-0.072051,1.72492,-2.2034,0.079154,1.60292,-2.17438,0.118183,1.55163,-2.20022,0.206398,1.42411,-1.80612,0.01813,1.50338,-1.87296,0.016414,1.53247,-1.92008,0.018832,1.50923,-1.98929,0.203447,1.4604,-2.05538,0.147209,1.09866,-1. [...]
+/*606*/{0.249665,1.9367,-1.75058,0.289743,1.87932,-1.90812,0.205642,1.7854,-1.63624,0.17064,1.66341,-1.55531,0.288544,1.66975,-1.66892,0.366673,1.66496,-1.71918,0.179131,1.94603,-2.12202,0.28306,1.87756,-1.97979,0.071036,1.9085,-2.01269,-0.050884,1.8106,-2.13854,-0.069564,1.71938,-2.20525,0.082588,1.59758,-2.17519,0.121435,1.54647,-2.2012,0.206232,1.42221,-1.80637,0.018271,1.49957,-1.87244,0.016787,1.52955,-1.92066,0.018838,1.50662,-1.99044,0.203053,1.4578,-2.05615,0.150619,1.09483,-1.79 [...]
+/*607*/{0.24806,1.9334,-1.75022,0.289308,1.87612,-1.90875,0.202034,1.78233,-1.63658,0.165025,1.66067,-1.55711,0.284438,1.6638,-1.66861,0.363283,1.6573,-1.71757,0.179535,1.94262,-2.12266,0.282514,1.87394,-1.98027,0.070888,1.90532,-2.01461,-0.049483,1.80483,-2.14008,-0.066558,1.71366,-2.20716,0.086459,1.592,-2.17648,0.124861,1.5411,-2.20202,0.205982,1.41956,-1.80732,0.01775,1.49678,-1.87274,0.015983,1.52584,-1.92025,0.017359,1.50298,-1.99034,0.20178,1.45496,-2.05702,0.154204,1.09218,-1.794 [...]
+/*608*/{0.246969,1.93026,-1.75002,0.288574,1.87196,-1.90834,0.198342,1.77894,-1.63671,0.159461,1.65705,-1.55873,0.280635,1.65801,-1.66807,0.360645,1.64955,-1.71585,0.179914,1.93863,-2.12296,0.282113,1.87043,-1.97995,0.07064,1.90214,-2.01527,-0.047871,1.79929,-2.14227,-0.063856,1.70731,-2.20979,0.089008,1.5865,-2.17881,0.128704,1.53596,-2.20275,0.205726,1.41687,-1.80757,0.017402,1.49259,-1.87323,0.015662,1.52188,-1.92061,0.016651,1.49884,-1.99039,0.201747,1.45271,-2.05812,0.157889,1.08962 [...]
+/*609*/{0.245707,1.9259,-1.74937,0.288465,1.86849,-1.90825,0.19499,1.77488,-1.63642,0.153656,1.65306,-1.55956,0.276571,1.65124,-1.66798,0.356732,1.64058,-1.71455,0.179269,1.93468,-2.12336,0.281856,1.86675,-1.97972,0.070716,1.89858,-2.01692,-0.046156,1.79256,-2.14462,-0.060778,1.70036,-2.21172,0.091842,1.58014,-2.18018,0.132179,1.52981,-2.20371,0.205815,1.4143,-1.80807,0.016335,1.48865,-1.8733,0.015054,1.51889,-1.92053,0.016028,1.4948,-1.99057,0.200901,1.45042,-2.05917,0.161676,1.08539,-1 [...]
+/*610*/{0.244016,1.92147,-1.74859,0.287662,1.86415,-1.90702,0.191439,1.77053,-1.63635,0.148489,1.64971,-1.56152,0.271874,1.6444,-1.66758,0.35304,1.63198,-1.71272,0.179576,1.92999,-2.12375,0.28149,1.86199,-1.97864,0.069557,1.89445,-2.01829,-0.045205,1.7852,-2.14734,-0.058252,1.69306,-2.21446,0.095335,1.57367,-2.18196,0.136412,1.52391,-2.2042,0.205201,1.41102,-1.80943,0.016052,1.48454,-1.87342,0.014358,1.51416,-1.92061,0.015188,1.49025,-1.99039,0.200204,1.44741,-2.0606,0.166935,1.0815,-1.7 [...]
+/*611*/{0.242773,1.91646,-1.74818,0.288085,1.85945,-1.90738,0.187492,1.76624,-1.63635,0.142054,1.64516,-1.56262,0.267028,1.63763,-1.66711,0.348952,1.62277,-1.71129,0.179846,1.92515,-2.12381,0.281206,1.85752,-1.97909,0.068967,1.88957,-2.01929,-0.043701,1.77901,-2.1503,-0.055058,1.68593,-2.21749,0.098999,1.56853,-2.18375,0.139976,1.51788,-2.205,0.205422,1.40706,-1.81023,0.015693,1.47938,-1.87391,0.013401,1.50988,-1.92034,0.01435,1.48532,-1.98995,0.199373,1.44488,-2.06143,0.167823,1.07899,- [...]
+/*612*/{0.241851,1.91215,-1.74755,0.287424,1.85493,-1.90614,0.183704,1.7621,-1.63643,0.136695,1.64212,-1.56446,0.261312,1.63027,-1.66666,0.342572,1.6134,-1.70883,0.179641,1.92043,-2.12426,0.281007,1.85329,-1.9789,0.06942,1.88443,-2.0204,-0.042655,1.77125,-2.15365,-0.052126,1.67813,-2.21995,0.103602,1.56174,-2.18549,0.144699,1.5111,-2.20584,0.205712,1.40405,-1.81146,0.014837,1.47487,-1.87398,0.013055,1.50554,-1.92136,0.013132,1.48059,-1.98931,0.198984,1.44238,-2.06256,0.174904,1.07567,-1. [...]
+/*613*/{0.240334,1.90679,-1.74657,0.28601,1.84912,-1.90532,0.179037,1.75724,-1.63667,0.129924,1.63742,-1.5657,0.256589,1.62256,-1.66631,0.338074,1.60349,-1.70741,0.17969,1.91475,-2.12493,0.281029,1.84837,-1.97813,0.068148,1.87853,-2.02122,-0.040381,1.76365,-2.15678,-0.048988,1.67046,-2.22298,0.107989,1.55514,-2.18683,0.149466,1.50472,-2.20668,0.205801,1.40006,-1.81235,0.014713,1.46883,-1.87393,0.012263,1.50013,-1.92143,0.011933,1.47515,-1.98956,0.198038,1.43961,-2.06428,0.176878,1.0717,- [...]
+/*614*/{0.238362,1.90129,-1.74594,0.286957,1.84352,-1.90482,0.175104,1.75168,-1.63682,0.123697,1.6347,-1.5666,0.251105,1.61505,-1.66532,0.33321,1.59374,-1.70537,0.180954,1.90985,-2.12561,0.280252,1.84304,-1.97782,0.067591,1.87348,-2.02309,-0.038953,1.75454,-2.16042,-0.045434,1.66197,-2.22576,0.111634,1.54808,-2.18847,0.154097,1.49757,-2.20743,0.205204,1.39654,-1.81349,0.0142,1.46397,-1.87385,0.012458,1.49486,-1.92206,0.01096,1.47025,-1.98859,0.197802,1.43642,-2.06594,0.178319,1.06822,-1. [...]
+/*615*/{0.236467,1.89585,-1.74525,0.286482,1.8381,-1.90425,0.171334,1.74684,-1.63663,0.117106,1.63028,-1.56797,0.24507,1.60706,-1.6646,0.326847,1.58359,-1.70348,0.180378,1.90446,-2.12586,0.28059,1.83797,-1.97745,0.067354,1.86708,-2.02395,-0.037267,1.74648,-2.16442,-0.042012,1.65358,-2.2287,0.116627,1.54123,-2.19063,0.159769,1.49162,-2.20815,0.204994,1.39241,-1.8143,0.01299,1.45835,-1.87462,0.010876,1.48923,-1.92269,0.00926,1.46445,-1.98876,0.19645,1.43254,-2.06774,0.186467,1.06347,-1.780 [...]
+/*616*/{0.235606,1.89006,-1.7441,0.286409,1.83241,-1.90305,0.166656,1.74145,-1.63695,0.109886,1.62546,-1.56814,0.239471,1.59917,-1.66408,0.32173,1.5737,-1.70107,0.181379,1.89894,-2.12667,0.28122,1.83254,-1.97684,0.066528,1.86181,-2.02444,-0.035216,1.73736,-2.16846,-0.038112,1.64481,-2.23232,0.121576,1.53431,-2.19177,0.165013,1.48522,-2.209,0.204057,1.38783,-1.81521,0.011964,1.45204,-1.87467,0.009305,1.48354,-1.92339,0.007015,1.45962,-1.98904,0.195496,1.42819,-2.07005,0.190598,1.06015,-1. [...]
+/*617*/{0.233921,1.88428,-1.74287,0.286368,1.82559,-1.90164,0.162653,1.7357,-1.63718,0.102559,1.62237,-1.56923,0.233324,1.59218,-1.66314,0.314498,1.56402,-1.69894,0.18191,1.89407,-2.12739,0.281222,1.82685,-1.97664,0.065769,1.85446,-2.02642,-0.03231,1.7288,-2.17234,-0.033418,1.63617,-2.23543,0.126622,1.52737,-2.19386,0.171122,1.47911,-2.20987,0.204141,1.38342,-1.81667,0.010793,1.44675,-1.87339,0.007372,1.47793,-1.92323,0.004844,1.45446,-1.98896,0.193718,1.42374,-2.07152,0.193102,1.05805,- [...]
+/*618*/{0.232574,1.87857,-1.74175,0.286081,1.81978,-1.90053,0.157617,1.73029,-1.63769,0.096039,1.61844,-1.57064,0.227653,1.58467,-1.66129,0.308341,1.55489,-1.69668,0.183409,1.88915,-2.12845,0.281652,1.82148,-1.97585,0.064573,1.8499,-2.02723,-0.031009,1.72002,-2.17632,-0.030688,1.62842,-2.23873,0.132488,1.52124,-2.19571,0.176876,1.47304,-2.21068,0.203192,1.37845,-1.81816,0.009077,1.4408,-1.87435,0.005644,1.47154,-1.92316,0.001804,1.45059,-1.98929,0.190232,1.41917,-2.07261,0.199577,1.05414 [...]
+/*619*/{0.231101,1.87375,-1.74072,0.285897,1.81403,-1.8992,0.154044,1.72543,-1.63854,0.089762,1.61574,-1.57166,0.220411,1.57727,-1.66172,0.301672,1.54563,-1.69431,0.183845,1.88446,-2.12966,0.282501,1.81572,-1.97558,0.064545,1.84658,-2.02723,-0.027756,1.71128,-2.1821,-0.023018,1.61836,-2.24152,0.138273,1.51468,-2.19683,0.183704,1.46721,-2.21089,0.201915,1.37399,-1.82046,0.005537,1.43552,-1.87441,0.002996,1.46647,-1.92273,-0.001861,1.4461,-1.99018,0.187593,1.41619,-2.07481,0.199813,1.05068 [...]
+/*620*/{0.229724,1.86935,-1.73947,0.286329,1.8069,-1.89754,0.149391,1.72117,-1.63892,0.081716,1.6123,-1.57405,0.21418,1.57098,-1.65983,0.294122,1.53792,-1.69124,0.184861,1.88029,-2.13126,0.283514,1.81001,-1.97476,0.065297,1.84323,-2.02679,-0.023625,1.70254,-2.18765,-0.016771,1.61001,-2.24452,0.145489,1.50882,-2.19744,0.190837,1.46139,-2.21106,0.200386,1.36826,-1.82149,0.003247,1.43034,-1.87481,0.000873,1.46107,-1.92293,-0.005259,1.44192,-1.98987,0.185345,1.41357,-2.07728,0.20566,1.04929, [...]
+/*621*/{0.228547,1.86458,-1.73785,0.286767,1.8024,-1.8963,0.145714,1.71731,-1.63939,0.073653,1.61036,-1.57628,0.207267,1.56397,-1.65865,0.286954,1.53049,-1.68909,0.186848,1.87631,-2.13261,0.283955,1.80495,-1.9742,0.065965,1.84159,-2.02654,-0.018436,1.69466,-2.19365,-0.008553,1.60103,-2.24764,0.152736,1.50391,-2.19835,0.198636,1.45676,-2.21091,0.198872,1.36478,-1.82459,-0.000122,1.42574,-1.874,-0.001589,1.45682,-1.92278,-0.008506,1.43823,-1.98932,0.182621,1.41098,-2.07931,0.208702,1.04879 [...]
+/*622*/{0.226801,1.86116,-1.73688,0.285425,1.79777,-1.89465,0.140186,1.71467,-1.6401,0.067005,1.60968,-1.57763,0.200459,1.55968,-1.65779,0.278309,1.52371,-1.68611,0.187879,1.87254,-2.1335,0.284483,1.80114,-1.97308,0.066183,1.84006,-2.02603,-0.012702,1.68722,-2.19918,-0.001481,1.5928,-2.24927,0.160858,1.49935,-2.19826,0.207367,1.45312,-2.21072,0.196969,1.36088,-1.82616,-0.001847,1.42155,-1.87437,-0.003039,1.45573,-1.92231,-0.012256,1.43546,-1.98707,0.180016,1.40902,-2.08141,0.21451,1.0507 [...]
+/*623*/{0.226478,1.85759,-1.73506,0.286908,1.79392,-1.89392,0.134478,1.71327,-1.64109,0.058651,1.60935,-1.57957,0.192915,1.55605,-1.65694,0.269699,1.51819,-1.68358,0.189756,1.86979,-2.13336,0.283899,1.79776,-1.97236,0.066436,1.83927,-2.02654,-0.007198,1.68034,-2.20372,0.006645,1.58653,-2.25105,0.169397,1.49596,-2.19747,0.216162,1.45033,-2.2099,0.195484,1.3567,-1.82696,-0.00468,1.41879,-1.87309,-0.00516,1.45522,-1.92243,-0.013758,1.43448,-1.98514,0.178021,1.40837,-2.08356,0.219167,1.05324 [...]
+/*624*/{0.226018,1.85583,-1.73425,0.286666,1.79172,-1.89255,0.130281,1.71239,-1.64121,0.050082,1.60962,-1.58186,0.184745,1.55333,-1.65607,0.262779,1.51315,-1.68105,0.1902,1.86785,-2.13254,0.283974,1.79554,-1.97108,0.066441,1.83685,-2.02614,-0.001032,1.67399,-2.20721,0.015235,1.58069,-2.25261,0.177497,1.49365,-2.1969,0.225516,1.44839,-2.20867,0.192913,1.35322,-1.8284,-0.005542,1.41849,-1.8714,-0.005807,1.45498,-1.92175,-0.014673,1.43454,-1.98312,0.175729,1.40775,-2.08491,0.22557,1.05554,- [...]
+/*625*/{0.225376,1.85235,-1.73335,0.284685,1.78957,-1.8912,0.124424,1.71187,-1.64192,0.04236,1.61152,-1.58438,0.176543,1.55121,-1.65477,0.254987,1.50915,-1.67732,0.192089,1.86624,-2.1311,0.284232,1.79377,-1.96963,0.066827,1.83608,-2.02613,0.005778,1.66846,-2.20968,0.024015,1.57589,-2.25338,0.186998,1.49167,-2.19542,0.236035,1.44871,-2.20694,0.191379,1.35088,-1.82886,-0.00744,1.41875,-1.87002,-0.008114,1.45485,-1.9219,-0.015103,1.43493,-1.98175,0.173787,1.40753,-2.08664,0.227828,1.059,-1. [...]
+/*626*/{0.224875,1.85026,-1.73254,0.284211,1.78764,-1.89005,0.118792,1.71262,-1.64252,0.034682,1.61511,-1.58684,0.169068,1.55031,-1.65379,0.246477,1.50701,-1.6753,0.19335,1.86507,-2.13009,0.28407,1.79224,-1.96843,0.066825,1.83597,-2.02637,0.011829,1.66491,-2.2117,0.03285,1.57244,-2.25381,0.196447,1.49164,-2.19395,0.245734,1.44882,-2.20455,0.188485,1.34884,-1.82868,-0.008176,1.41931,-1.87057,-0.00918,1.45379,-1.92231,-0.01642,1.435,-1.98134,0.173829,1.40721,-2.087,0.235523,1.06308,-1.7674 [...]
+/*627*/{0.224252,1.84933,-1.73206,0.283331,1.78628,-1.88941,0.113877,1.71368,-1.64264,0.02664,1.61826,-1.58924,0.161073,1.5504,-1.6533,0.237567,1.50504,-1.67266,0.195371,1.86476,-2.12903,0.283669,1.79062,-1.96678,0.067662,1.83572,-2.02681,0.019497,1.66227,-2.21203,0.040221,1.56952,-2.2546,0.20544,1.49059,-2.19149,0.256039,1.45016,-2.2017,0.187395,1.34827,-1.82808,-0.009122,1.41908,-1.87047,-0.010607,1.4542,-1.92314,-0.017511,1.43616,-1.98159,0.171839,1.40688,-2.08784,0.243869,1.0671,-1.7 [...]
+/*628*/{0.222056,1.84876,-1.73164,0.283335,1.78518,-1.88783,0.108059,1.71552,-1.64344,0.018576,1.62197,-1.59171,0.153802,1.55179,-1.65231,0.2291,1.50469,-1.67028,0.197545,1.86463,-2.12833,0.28376,1.78936,-1.96589,0.068099,1.8357,-2.0272,0.024821,1.66058,-2.21283,0.048322,1.56703,-2.25486,0.214703,1.49273,-2.18967,0.266102,1.45303,-2.19881,0.184482,1.34805,-1.82687,-0.011024,1.41956,-1.87065,-0.011678,1.45404,-1.92341,-0.017476,1.43631,-1.9815,0.169112,1.40523,-2.08912,0.250525,1.0713,-1. [...]
+/*629*/{0.220047,1.8489,-1.73052,0.281803,1.78469,-1.88726,0.102482,1.71818,-1.64475,0.009732,1.62648,-1.59373,0.145193,1.55267,-1.65033,0.220407,1.50536,-1.6674,0.199354,1.86486,-2.12792,0.283729,1.78938,-1.96462,0.069449,1.83573,-2.02792,0.029866,1.65871,-2.2131,0.056507,1.56603,-2.25528,0.224048,1.49513,-2.18699,0.27665,1.45673,-2.19552,0.18358,1.3488,-1.82623,-0.011349,1.42039,-1.87118,-0.012694,1.45426,-1.92433,-0.018214,1.43742,-1.98288,0.170037,1.40555,-2.08888,0.254973,1.07614,-1 [...]
+/*630*/{0.218003,1.85062,-1.72987,0.281507,1.78481,-1.88537,0.096772,1.7212,-1.64492,0.00193,1.63147,-1.5958,0.137309,1.55534,-1.64929,0.211764,1.5067,-1.66554,0.202852,1.86649,-2.12656,0.283634,1.7898,-1.96324,0.070325,1.8364,-2.02908,0.034741,1.65789,-2.21263,0.064407,1.56521,-2.25512,0.2335,1.49918,-2.18439,0.285863,1.46219,-2.19227,0.181389,1.35032,-1.82509,-0.012161,1.42142,-1.87266,-0.013189,1.45549,-1.925,-0.01894,1.43755,-1.98374,0.169818,1.40524,-2.0888,0.262386,1.08425,-1.76551 [...]
+/*631*/{0.214706,1.85123,-1.72895,0.27883,1.7854,-1.88385,0.091351,1.7252,-1.64511,-0.006661,1.63701,-1.59805,0.129167,1.55883,-1.64809,0.202929,1.50912,-1.66335,0.204894,1.86762,-2.12611,0.283036,1.79031,-1.96147,0.071016,1.83751,-2.0296,0.039548,1.65786,-2.21268,0.072346,1.5652,-2.25509,0.241405,1.50279,-2.18146,0.296029,1.46826,-2.18819,0.179204,1.35175,-1.82403,-0.012688,1.42214,-1.87381,-0.014547,1.45602,-1.92625,-0.019189,1.43843,-1.9859,0.170316,1.40473,-2.08871,0.269159,1.08957,- [...]
+/*632*/{0.211869,1.85271,-1.7276,0.278683,1.78655,-1.88214,0.0853,1.72924,-1.64576,-0.013328,1.64181,-1.59941,0.121738,1.56276,-1.64724,0.194282,1.51194,-1.66133,0.206284,1.86973,-2.12518,0.283117,1.79107,-1.95985,0.071615,1.83937,-2.03074,0.044724,1.65718,-2.21222,0.079353,1.56564,-2.25481,0.249667,1.50793,-2.17827,0.304713,1.47538,-2.18458,0.178288,1.35299,-1.82325,-0.014631,1.42388,-1.8755,-0.013975,1.45641,-1.92713,-0.019313,1.43865,-1.98723,0.17075,1.40472,-2.0888,0.274018,1.09741,- [...]
+/*633*/{0.208792,1.85543,-1.72718,0.277344,1.78795,-1.87986,0.079191,1.73373,-1.64692,-0.021857,1.64757,-1.6013,0.112729,1.56722,-1.64674,0.18543,1.51584,-1.65953,0.208955,1.87204,-2.12374,0.282905,1.79319,-1.95818,0.072444,1.84014,-2.03223,0.049192,1.65728,-2.21137,0.086858,1.56644,-2.25482,0.257306,1.51404,-2.17522,0.3139,1.48285,-2.18,0.176608,1.35488,-1.82152,-0.015277,1.42472,-1.87711,-0.014558,1.45773,-1.92867,-0.019627,1.44,-1.98925,0.171177,1.40449,-2.08855,0.280255,1.10421,-1.76 [...]
+/*634*/{0.204999,1.85816,-1.72671,0.276887,1.79021,-1.87782,0.073213,1.73821,-1.64683,-0.029016,1.654,-1.60319,0.10581,1.57306,-1.64691,0.177247,1.52038,-1.65846,0.211221,1.87481,-2.12255,0.282864,1.79501,-1.95581,0.073807,1.84132,-2.03293,0.053701,1.65748,-2.21012,0.09334,1.56795,-2.25475,0.264781,1.52139,-2.17225,0.321501,1.49171,-2.176,0.175759,1.35796,-1.82112,-0.016293,1.42642,-1.8789,-0.014762,1.45985,-1.93042,-0.019674,1.44145,-1.99135,0.171215,1.40441,-2.08863,0.283519,1.11307,-1 [...]
+/*635*/{0.201654,1.86133,-1.72563,0.27448,1.79312,-1.87567,0.067271,1.74343,-1.6475,-0.037283,1.65988,-1.60457,0.097548,1.57841,-1.64701,0.169607,1.52523,-1.65631,0.213307,1.87779,-2.12096,0.282758,1.79772,-1.95376,0.074474,1.84341,-2.03414,0.057642,1.65824,-2.20973,0.100533,1.57013,-2.25528,0.272528,1.52889,-2.1696,0.329589,1.50082,-2.17138,0.174925,1.36038,-1.82056,-0.01685,1.42848,-1.88087,-0.014146,1.46152,-1.93233,-0.019044,1.44401,-1.99319,0.172325,1.4042,-2.08835,0.287604,1.12071, [...]
+/*636*/{0.19775,1.86465,-1.72504,0.274746,1.79669,-1.87368,0.061886,1.74894,-1.64806,-0.04527,1.66644,-1.60679,0.090418,1.5846,-1.64706,0.161765,1.53133,-1.65603,0.215604,1.88084,-2.12001,0.282152,1.80007,-1.95122,0.075379,1.8447,-2.0343,0.062678,1.65899,-2.20916,0.10797,1.57203,-2.25524,0.278202,1.53535,-2.16565,0.337333,1.51002,-2.16684,0.174662,1.36297,-1.81973,-0.016597,1.43002,-1.88293,-0.01377,1.46386,-1.93339,-0.018779,1.44542,-1.99528,0.173154,1.4043,-2.08816,0.291668,1.12866,-1. [...]
+/*637*/{0.194214,1.86833,-1.72419,0.274166,1.79841,-1.87038,0.056594,1.75437,-1.64883,-0.051356,1.67321,-1.60895,0.083971,1.59177,-1.64741,0.153838,1.53783,-1.65515,0.217182,1.88454,-2.11814,0.282436,1.80387,-1.94911,0.076053,1.84736,-2.03543,0.067809,1.6606,-2.20807,0.114749,1.57472,-2.2554,0.285012,1.54393,-2.16215,0.34522,1.52076,-2.16201,0.174696,1.36647,-1.81939,-0.016358,1.4335,-1.8838,-0.01352,1.46637,-1.93499,-0.017006,1.44769,-1.99682,0.173563,1.40458,-2.08796,0.296785,1.13603,- [...]
+/*638*/{0.190096,1.8722,-1.72413,0.272879,1.80246,-1.86954,0.050834,1.76014,-1.6496,-0.058852,1.68013,-1.61142,0.076934,1.59844,-1.64807,0.146899,1.54336,-1.65448,0.218564,1.8884,-2.11708,0.282575,1.80727,-1.94722,0.077517,1.84923,-2.03609,0.072233,1.66258,-2.20805,0.121578,1.5777,-2.25524,0.290853,1.55186,-2.15839,0.351556,1.53056,-2.15766,0.17441,1.37034,-1.81897,-0.015662,1.43598,-1.88632,-0.011876,1.46929,-1.93657,-0.015868,1.45107,-1.99879,0.175742,1.40416,-2.08739,0.298709,1.14405, [...]
+/*639*/{0.186829,1.87632,-1.72328,0.270592,1.8077,-1.86723,0.045328,1.76599,-1.65053,-0.065614,1.68661,-1.61352,0.070468,1.6053,-1.64885,0.140397,1.55105,-1.65414,0.220548,1.89231,-2.11568,0.282078,1.81103,-1.94452,0.078394,1.85189,-2.03706,0.076692,1.66492,-2.20806,0.128231,1.58116,-2.25534,0.297469,1.56094,-2.15503,0.357619,1.54066,-2.15277,0.175097,1.37392,-1.81914,-0.014752,1.44015,-1.8879,-0.01077,1.47181,-1.93817,-0.014576,1.45338,-2.0006,0.176309,1.40509,-2.08749,0.303183,1.15166, [...]
+/*640*/{0.18279,1.88058,-1.72317,0.271573,1.81058,-1.86461,0.040529,1.77163,-1.65158,-0.070372,1.69486,-1.61619,0.06373,1.61247,-1.64986,0.133334,1.55648,-1.65238,0.222462,1.89693,-2.11425,0.282299,1.81452,-1.9429,0.079291,1.85545,-2.03743,0.082262,1.66728,-2.20867,0.134972,1.58501,-2.25546,0.301855,1.5688,-2.15099,0.364128,1.55044,-2.14737,0.176429,1.37839,-1.81898,-0.013905,1.44388,-1.88856,-0.010055,1.47498,-1.9401,-0.012456,1.4567,-2.00254,0.177534,1.40528,-2.08756,0.305254,1.15857,- [...]
+/*641*/{0.180129,1.88521,-1.72251,0.270346,1.81484,-1.86345,0.035646,1.77773,-1.65271,-0.079267,1.70312,-1.61938,0.057833,1.61958,-1.6506,0.126957,1.5639,-1.6523,0.223512,1.90138,-2.11254,0.283583,1.81818,-1.94032,0.080729,1.85863,-2.03834,0.086521,1.67184,-2.21045,0.141506,1.58892,-2.25548,0.306671,1.57739,-2.14748,0.369607,1.56112,-2.14271,0.177142,1.38285,-1.82006,-0.013912,1.44806,-1.89087,-0.008221,1.4788,-1.94171,-0.010087,1.46019,-2.00252,0.17971,1.40496,-2.08724,0.307484,1.16655, [...]
+/*642*/{0.176245,1.88919,-1.72288,0.268616,1.81877,-1.86121,0.030969,1.78421,-1.65391,-0.083185,1.71059,-1.62184,0.051787,1.62616,-1.65116,0.121496,1.57111,-1.65176,0.224816,1.9063,-2.11075,0.28315,1.82221,-1.93874,0.081931,1.8623,-2.03943,0.091985,1.67547,-2.21193,0.138241,1.59359,-2.25883,0.312254,1.58647,-2.14293,0.374368,1.57176,-2.13756,0.177527,1.38795,-1.8203,-0.012407,1.45278,-1.89058,-0.00752,1.48218,-1.94345,-0.007965,1.46396,-2.00367,0.180427,1.40577,-2.08724,0.31077,1.17469,- [...]
+/*643*/{0.173484,1.89355,-1.72281,0.267652,1.82466,-1.86002,0.026047,1.78998,-1.65535,-0.089719,1.71687,-1.62468,0.045578,1.63318,-1.65245,0.116084,1.57724,-1.65194,0.226725,1.91106,-2.10976,0.283367,1.82707,-1.93653,0.082788,1.8648,-2.03949,0.097634,1.6802,-2.2141,0.155908,1.5978,-2.25557,0.316267,1.59603,-2.14017,0.378522,1.58136,-2.13251,0.178458,1.39202,-1.82016,-0.010667,1.45723,-1.89201,-0.005316,1.48581,-1.94421,-0.005456,1.46727,-2.00467,0.183492,1.40548,-2.08686,0.312973,1.18205 [...]
+/*644*/{0.171121,1.89743,-1.7231,0.267917,1.82864,-1.85792,0.021984,1.79574,-1.65688,-0.09473,1.72446,-1.6279,0.040526,1.64042,-1.6537,0.110067,1.58461,-1.65142,0.229098,1.91615,-2.10771,0.28279,1.83086,-1.93472,0.084172,1.86857,-2.04024,0.103931,1.68445,-2.21617,0.163127,1.60329,-2.25589,0.319339,1.60361,-2.13627,0.382415,1.5917,-2.12742,0.180505,1.39619,-1.82149,-0.008705,1.46253,-1.89185,-0.00316,1.48968,-1.94571,-0.003384,1.47133,-2.00481,0.184843,1.40605,-2.08649,0.314813,1.18849,-1 [...]
+/*645*/{0.168127,1.90157,-1.72279,0.266573,1.83389,-1.8568,0.018105,1.80209,-1.65835,-0.099476,1.73176,-1.63108,0.035152,1.64685,-1.65459,0.104345,1.59081,-1.65116,0.230399,1.92079,-2.10643,0.283294,1.83536,-1.93253,0.085133,1.8723,-2.04111,0.10927,1.68878,-2.21748,0.16941,1.60819,-2.25598,0.322823,1.61141,-2.13287,0.386226,1.60157,-2.12338,0.181414,1.40097,-1.82201,-0.00783,1.46739,-1.89271,-0.001866,1.4936,-1.94743,-0.000894,1.47472,-2.00617,0.185484,1.4067,-2.08595,0.316226,1.19572,-1 [...]
+/*646*/{0.166124,1.90546,-1.7226,0.26682,1.8377,-1.85521,0.014273,1.80802,-1.66067,-0.105278,1.7384,-1.6342,0.03029,1.65292,-1.65498,0.098892,1.59737,-1.65128,0.231016,1.92545,-2.10429,0.282709,1.83939,-1.93139,0.086557,1.87559,-2.04149,0.114503,1.69372,-2.21962,0.176466,1.61303,-2.25616,0.326847,1.6209,-2.12942,0.389347,1.61096,-2.11936,0.182408,1.4054,-1.82152,-0.006542,1.47251,-1.89278,-2e-005,1.4976,-1.94819,0.002016,1.47826,-2.00598,0.188162,1.40693,-2.08505,0.317937,1.2026,-1.75891 [...]
+/*647*/{0.164089,1.9093,-1.7227,0.265478,1.84167,-1.85379,0.011464,1.81366,-1.66267,-0.109276,1.74564,-1.63796,0.025595,1.65986,-1.65657,0.094382,1.60293,-1.65098,0.232604,1.93008,-2.10294,0.283203,1.84406,-1.9297,0.087257,1.87881,-2.04189,0.120506,1.69808,-2.22134,0.182659,1.61756,-2.25611,0.329021,1.62784,-2.12575,0.392023,1.61989,-2.11563,0.183199,1.40993,-1.82205,-0.004188,1.47742,-1.89215,0.002015,1.50167,-1.9492,0.00453,1.48164,-2.00634,0.188948,1.40744,-2.08457,0.31979,1.20872,-1. [...]
+/*648*/{0.162266,1.9128,-1.72288,0.25923,1.84918,-1.85455,0.007851,1.81946,-1.6651,-0.113776,1.75238,-1.64186,0.020393,1.66522,-1.65751,0.089445,1.60855,-1.65074,0.233381,1.93424,-2.10127,0.282794,1.84892,-1.92863,0.088875,1.88157,-2.04234,0.125697,1.70233,-2.22267,0.188849,1.62252,-2.25616,0.331415,1.63538,-2.12289,0.395337,1.62859,-2.1119,0.184259,1.41464,-1.82258,-0.002036,1.48255,-1.89228,0.004073,1.50543,-1.95042,0.007065,1.48577,-2.00645,0.190314,1.40863,-2.08369,0.321118,1.21413,- [...]
+/*649*/{0.160796,1.91642,-1.72305,0.259819,1.85282,-1.85374,0.005032,1.82512,-1.66698,-0.117727,1.75902,-1.64558,0.016665,1.67091,-1.65906,0.08448,1.61385,-1.65083,0.234865,1.9385,-2.10004,0.283146,1.8524,-1.92731,0.089886,1.8844,-2.04252,0.130561,1.70681,-2.22453,0.19482,1.62729,-2.25686,0.333072,1.64224,-2.11977,0.397327,1.63687,-2.10761,0.186218,1.41839,-1.82311,-0.000834,1.48726,-1.89218,0.005552,1.50934,-1.9512,0.009154,1.48878,-2.00657,0.191693,1.40912,-2.08295,0.320853,1.2192,-1.7 [...]
+/*650*/{0.158487,1.91931,-1.72344,0.264351,1.85389,-1.85044,0.002048,1.83033,-1.66922,-0.121349,1.76517,-1.64926,0.01332,1.67653,-1.65999,0.08037,1.61882,-1.65044,0.23644,1.94221,-2.09895,0.283548,1.85561,-1.92545,0.091275,1.88718,-2.04348,0.135701,1.71093,-2.22633,0.200373,1.63154,-2.25677,0.336279,1.65071,-2.11674,0.399242,1.64525,-2.10479,0.186798,1.42267,-1.82266,0.00148,1.49174,-1.89184,0.007421,1.51289,-1.95199,0.011732,1.49186,-2.00604,0.193402,1.4098,-2.08174,0.322479,1.22388,-1. [...]
+/*651*/{0.157151,1.9224,-1.72397,0.259803,1.86069,-1.85081,-1.1e-005,1.83562,-1.67074,-0.125809,1.77082,-1.65353,0.008622,1.68124,-1.66113,0.076918,1.62385,-1.64995,0.237893,1.94576,-2.09708,0.283205,1.85991,-1.92489,0.092749,1.88888,-2.04385,0.140932,1.71579,-2.22782,0.205273,1.63558,-2.25717,0.336836,1.65642,-2.11422,0.400655,1.65203,-2.10078,0.188179,1.42654,-1.82254,0.003963,1.49649,-1.8912,0.009996,1.5173,-1.95228,0.012893,1.4953,-2.00625,0.194323,1.41122,-2.08102,0.323475,1.22811,- [...]
+/*652*/{0.156222,1.92495,-1.72433,0.259086,1.86412,-1.84986,-0.002271,1.84021,-1.67449,-0.129387,1.77586,-1.65824,0.005919,1.68576,-1.66222,0.073255,1.62792,-1.64939,0.237936,1.94831,-2.0963,0.283818,1.8627,-1.9233,0.093926,1.89169,-2.04438,0.14522,1.71878,-2.22839,0.209934,1.63995,-2.25763,0.339387,1.66155,-2.1122,0.402133,1.65897,-2.09781,0.18863,1.42981,-1.82229,0.006082,1.50054,-1.89096,0.011579,1.52059,-1.95264,0.015838,1.49806,-2.00543,0.19489,1.41208,-2.07996,0.324473,1.23129,-1.7 [...]
+/*653*/{0.155546,1.92784,-1.72418,0.263964,1.86392,-1.84759,-0.004912,1.84457,-1.6767,-0.131758,1.78125,-1.66159,0.001567,1.69031,-1.66369,0.068502,1.63148,-1.64946,0.239199,1.95121,-2.09472,0.28357,1.86593,-1.92238,0.095676,1.89331,-2.04492,0.149736,1.72242,-2.2298,0.214434,1.64311,-2.25808,0.340427,1.66656,-2.11007,0.403832,1.66439,-2.09485,0.189364,1.43405,-1.82219,0.007835,1.50385,-1.8903,0.013525,1.52399,-1.95258,0.017899,1.50048,-2.00483,0.196112,1.4127,-2.07878,0.324372,1.23316,-1 [...]
+/*654*/{0.154621,1.93015,-1.72436,0.260601,1.86958,-1.84826,-0.005586,1.84791,-1.67891,-0.134423,1.7863,-1.66554,-0.000955,1.69337,-1.66471,0.064983,1.63459,-1.64897,0.240506,1.95354,-2.09364,0.283941,1.8684,-1.92156,0.096669,1.89571,-2.04502,0.153796,1.72577,-2.2302,0.218734,1.64664,-2.25803,0.341937,1.67137,-2.10888,0.404714,1.67003,-2.09171,0.189679,1.43719,-1.82267,0.008446,1.50693,-1.88945,0.014899,1.52635,-1.95193,0.019438,1.50427,-2.00485,0.197035,1.41403,-2.07767,0.324419,1.23538 [...]
+/*655*/{0.153917,1.93286,-1.72469,0.260503,1.87157,-1.84703,-0.007656,1.85195,-1.68096,-0.137057,1.79053,-1.66975,-0.003999,1.69782,-1.66569,0.062275,1.63786,-1.64909,0.242101,1.95601,-2.09308,0.284349,1.8711,-1.92007,0.098294,1.89731,-2.04541,0.158649,1.72848,-2.23129,0.222082,1.64933,-2.25884,0.342993,1.67542,-2.10729,0.405884,1.67409,-2.08939,0.189965,1.43969,-1.82241,0.010222,1.51043,-1.88813,0.015532,1.53005,-1.95214,0.020598,1.50714,-2.00471,0.197607,1.41487,-2.07697,0.324257,1.236 [...]
+/*656*/{0.153622,1.93445,-1.72525,0.264353,1.87041,-1.84507,-0.009008,1.85545,-1.68356,-0.138625,1.79435,-1.6731,-0.006911,1.70043,-1.66677,0.059214,1.64096,-1.64891,0.243781,1.95823,-2.09203,0.28559,1.87227,-1.91881,0.099475,1.89945,-2.04603,0.162184,1.73148,-2.23188,0.225443,1.65208,-2.25936,0.34454,1.68014,-2.10648,0.406957,1.67737,-2.08684,0.189985,1.44186,-1.82196,0.010218,1.51434,-1.88819,0.016625,1.53319,-1.95187,0.021681,1.50971,-2.00311,0.197624,1.41708,-2.07593,0.32464,1.23672, [...]
+/*657*/{0.153591,1.93657,-1.72532,0.26106,1.87584,-1.84515,-0.009439,1.85879,-1.68528,-0.140585,1.79757,-1.6769,-0.008908,1.70377,-1.66862,0.057138,1.64253,-1.64822,0.244417,1.95966,-2.09115,0.285247,1.87513,-1.91865,0.100315,1.90105,-2.04603,0.165225,1.73367,-2.23247,0.227858,1.6544,-2.25971,0.346227,1.68169,-2.10547,0.407541,1.68048,-2.08515,0.1906,1.44501,-1.82205,0.011658,1.5174,-1.8879,0.0174,1.53565,-1.95122,0.02187,1.51345,-2.00306,0.198601,1.41881,-2.07526,0.321939,1.23703,-1.766 [...]
+/*658*/{0.153974,1.93839,-1.72502,0.261848,1.87731,-1.84517,-0.011022,1.86158,-1.68806,-0.141779,1.79974,-1.68046,-0.010358,1.7058,-1.66966,0.05394,1.64426,-1.64766,0.245911,1.9614,-2.09045,0.285782,1.87662,-1.91779,0.101579,1.90286,-2.0459,0.16899,1.73612,-2.23315,0.230973,1.65507,-2.2603,0.34757,1.68398,-2.10449,0.408331,1.68258,-2.08323,0.191411,1.44784,-1.82196,0.012137,1.51944,-1.88662,0.018057,1.53859,-1.95026,0.022706,1.51562,-2.00241,0.198304,1.42104,-2.07457,0.320493,1.23454,-1. [...]
+/*659*/{0.153896,1.93976,-1.72501,0.26579,1.87526,-1.84311,-0.010637,1.86321,-1.68921,-0.143309,1.80276,-1.68362,-0.011877,1.70802,-1.67046,0.052615,1.64677,-1.64753,0.246449,1.9623,-2.09028,0.286971,1.8777,-1.91662,0.102364,1.90517,-2.0463,0.171133,1.73752,-2.23362,0.233053,1.65673,-2.26045,0.348767,1.68557,-2.10425,0.409195,1.6835,-2.08105,0.191845,1.45057,-1.82222,0.012395,1.52199,-1.88545,0.017554,1.54099,-1.94976,0.023056,1.51854,-2.0012,0.198285,1.42261,-2.07411,0.318542,1.23255,-1 [...]
+/*660*/{0.154408,1.94094,-1.72494,0.265896,1.87562,-1.84256,-0.010796,1.86525,-1.6909,-0.144945,1.8042,-1.68733,-0.013896,1.70867,-1.67183,0.050696,1.64749,-1.6472,0.24724,1.96302,-2.08986,0.287423,1.87847,-1.91607,0.103641,1.90687,-2.04634,0.173993,1.73847,-2.23372,0.23481,1.65693,-2.26044,0.349215,1.68782,-2.10363,0.409524,1.68399,-2.07976,0.191872,1.45233,-1.82181,0.013105,1.52341,-1.8854,0.017677,1.5429,-1.94943,0.023494,1.51971,-1.99944,0.198364,1.42499,-2.07362,0.315819,1.22868,-1. [...]
+/*661*/{0.154779,1.94167,-1.72461,0.262576,1.88095,-1.84317,-0.01154,1.86729,-1.693,-0.145122,1.8063,-1.69012,-0.014397,1.70984,-1.67255,0.049113,1.64812,-1.64749,0.248906,1.96413,-2.08943,0.286883,1.88014,-1.91589,0.104521,1.90889,-2.04621,0.175511,1.73938,-2.23377,0.236399,1.65711,-2.26051,0.351035,1.68809,-2.10237,0.40986,1.68411,-2.07809,0.191673,1.45332,-1.82171,0.013487,1.52531,-1.88375,0.018349,1.54513,-1.94808,0.023411,1.52217,-1.99933,0.198909,1.42749,-2.07305,0.313198,1.22467,- [...]
+/*662*/{0.156259,1.94296,-1.72389,0.264223,1.88153,-1.84301,-0.010908,1.86787,-1.69347,-0.14503,1.807,-1.69251,-0.015358,1.71105,-1.67385,0.048692,1.64734,-1.6469,0.249642,1.96435,-2.08914,0.287512,1.8812,-1.91547,0.104861,1.90975,-2.04632,0.177176,1.7396,-2.23278,0.237317,1.65686,-2.26005,0.351414,1.68644,-2.10122,0.410858,1.68294,-2.07729,0.191684,1.45528,-1.82176,0.01373,1.52619,-1.88322,0.018198,1.54676,-1.94719,0.023318,1.52353,-1.99717,0.198925,1.42897,-2.07245,0.311441,1.21937,-1. [...]
+/*663*/{0.156544,1.94323,-1.72346,0.263791,1.88162,-1.8423,-0.009956,1.86857,-1.69449,-0.144852,1.80758,-1.69475,-0.015988,1.71091,-1.67464,0.047637,1.64711,-1.64689,0.250707,1.96414,-2.08849,0.287949,1.88179,-1.91553,0.105855,1.91128,-2.04591,0.178183,1.73973,-2.23188,0.237755,1.65656,-2.25964,0.35187,1.68508,-2.10031,0.410966,1.68101,-2.07628,0.192032,1.45576,-1.82216,0.013177,1.52722,-1.88227,0.018046,1.54777,-1.94632,0.023567,1.52389,-1.99567,0.198933,1.43066,-2.07253,0.307625,1.2143 [...]
+/*664*/{0.157688,1.94332,-1.72263,0.264363,1.88155,-1.84189,-0.009273,1.86806,-1.69549,-0.14398,1.80734,-1.69724,-0.015962,1.70955,-1.67507,0.046769,1.64593,-1.64637,0.250713,1.96356,-2.08835,0.287991,1.88205,-1.91463,0.105715,1.91223,-2.0461,0.177198,1.73945,-2.23115,0.236693,1.65502,-2.25849,0.352371,1.68337,-2.10007,0.412894,1.67823,-2.07609,0.191704,1.45593,-1.82213,0.012981,1.52669,-1.88124,0.017972,1.54792,-1.94567,0.022795,1.52544,-1.99502,0.199016,1.4324,-2.07234,0.304929,1.2091, [...]
+/*665*/{0.159062,1.94361,-1.72198,0.267619,1.87814,-1.84056,-0.00871,1.86762,-1.69568,-0.143237,1.80588,-1.69826,-0.015405,1.70776,-1.67541,0.046094,1.64333,-1.64504,0.251037,1.96264,-2.08813,0.288752,1.88135,-1.91388,0.106811,1.91312,-2.04598,0.176644,1.73851,-2.22939,0.235914,1.65388,-2.25772,0.352312,1.68042,-2.09888,0.412517,1.67496,-2.07482,0.192382,1.45679,-1.82244,0.01251,1.52647,-1.88008,0.017,1.54758,-1.94411,0.022504,1.52573,-1.99423,0.19887,1.43342,-2.07226,0.299555,1.2029,-1. [...]
+/*666*/{0.15981,1.94328,-1.72121,0.264366,1.88144,-1.8417,-0.008659,1.86667,-1.6963,-0.141993,1.80462,-1.70057,-0.014528,1.70569,-1.67588,0.046241,1.64123,-1.6449,0.252186,1.96199,-2.08756,0.288258,1.88179,-1.9139,0.106999,1.91315,-2.04586,0.174855,1.73773,-2.22886,0.23362,1.65198,-2.25644,0.352173,1.67703,-2.09827,0.412068,1.67035,-2.07502,0.19172,1.45666,-1.82258,0.011959,1.52577,-1.8793,0.017101,1.54802,-1.94365,0.021959,1.52479,-1.9926,0.199326,1.43456,-2.07209,0.296098,1.19765,-1.76 [...]
+/*667*/{0.16086,1.94229,-1.72094,0.264978,1.88138,-1.84235,-0.007305,1.86464,-1.69558,-0.140389,1.80246,-1.70142,-0.013285,1.7035,-1.67534,0.047003,1.63817,-1.64396,0.252763,1.96042,-2.08699,0.287309,1.88098,-1.91366,0.107029,1.91315,-2.04582,0.172126,1.73651,-2.22729,0.231525,1.6502,-2.25505,0.351841,1.67368,-2.09805,0.412124,1.66606,-2.07463,0.194276,1.45668,-1.82246,0.011611,1.52484,-1.87859,0.016141,1.54708,-1.94343,0.021573,1.52441,-1.99211,0.199221,1.43491,-2.07213,0.291233,1.18977 [...]
+/*668*/{0.162143,1.94126,-1.72052,0.26856,1.87643,-1.84033,-0.006407,1.86253,-1.69528,-0.1382,1.79967,-1.70173,-0.012786,1.69997,-1.67485,0.047716,1.63491,-1.64269,0.251885,1.95848,-2.08633,0.288693,1.87949,-1.91304,0.107091,1.91287,-2.04522,0.17015,1.73485,-2.22527,0.229916,1.64866,-2.25369,0.351885,1.66933,-2.09791,0.411184,1.6606,-2.07403,0.194587,1.45656,-1.82228,0.011231,1.52259,-1.87816,0.015518,1.54574,-1.94289,0.020442,1.52285,-1.99109,0.199616,1.43525,-2.07222,0.288055,1.18372,- [...]
+/*669*/{0.162577,1.93994,-1.71984,0.268707,1.87593,-1.84058,-0.006321,1.86019,-1.69532,-0.1353,1.7957,-1.70091,-0.011538,1.69541,-1.67327,0.048865,1.6308,-1.64098,0.252164,1.9567,-2.08617,0.288018,1.87807,-1.9131,0.10683,1.91114,-2.0441,0.166508,1.73365,-2.22431,0.227256,1.64663,-2.25219,0.351534,1.66427,-2.09802,0.411007,1.65432,-2.07441,0.191554,1.45486,-1.82271,0.010657,1.5203,-1.87798,0.014364,1.54407,-1.94216,0.020608,1.52119,-1.99122,0.19926,1.4349,-2.07212,0.282925,1.17669,-1.7607 [...]
+/*670*/{0.163561,1.93799,-1.71834,0.263521,1.87782,-1.8419,-0.005745,1.85662,-1.69423,-0.134033,1.79208,-1.70144,-0.009789,1.69177,-1.67289,0.049796,1.62643,-1.6396,0.252602,1.95456,-2.0864,0.286827,1.87781,-1.91366,0.106201,1.91028,-2.04304,0.162323,1.73186,-2.22241,0.223109,1.64432,-2.25065,0.350158,1.65928,-2.09836,0.409814,1.64808,-2.07511,0.191366,1.45342,-1.82269,0.010249,1.51768,-1.87744,0.014463,1.54167,-1.94179,0.019283,1.51973,-1.9907,0.198699,1.43379,-2.07164,0.279677,1.1709,- [...]
+/*671*/{0.164148,1.9359,-1.71836,0.264161,1.87599,-1.84203,-0.004529,1.85216,-1.6929,-0.132369,1.78664,-1.69939,-0.00821,1.68757,-1.67082,0.051327,1.62224,-1.63831,0.251188,1.95209,-2.0859,0.286805,1.87583,-1.91375,0.105394,1.90938,-2.04226,0.158581,1.72923,-2.22056,0.219772,1.64177,-2.2489,0.34914,1.65284,-2.09796,0.408417,1.64079,-2.07566,0.190975,1.45138,-1.82288,0.009362,1.51525,-1.8766,0.013281,1.53864,-1.94147,0.019073,1.51656,-1.99065,0.199584,1.43263,-2.07147,0.274484,1.1658,-1.7 [...]
+/*672*/{0.165084,1.9334,-1.71762,0.263988,1.8743,-1.84226,-0.003202,1.84849,-1.6912,-0.13012,1.78163,-1.69856,-0.007025,1.68169,-1.66899,0.052767,1.6171,-1.6364,0.250714,1.94952,-2.08584,0.286275,1.87338,-1.91342,0.104944,1.90694,-2.0413,0.154718,1.7266,-2.21855,0.2158,1.63893,-2.24722,0.347928,1.64738,-2.09869,0.406271,1.63278,-2.07634,0.19092,1.44885,-1.82247,0.009119,1.51223,-1.87619,0.012783,1.53627,-1.94169,0.018224,1.51347,-1.99044,0.199416,1.43083,-2.07108,0.272084,1.15947,-1.7566 [...]
+/*673*/{0.165112,1.93042,-1.71723,0.263505,1.87187,-1.84221,-0.002714,1.84394,-1.6898,-0.128742,1.77572,-1.69646,-0.0041,1.67682,-1.66747,0.055239,1.61192,-1.63513,0.250141,1.94638,-2.08604,0.286187,1.87094,-1.91281,0.104385,1.90513,-2.0409,0.150656,1.72407,-2.21726,0.211125,1.63578,-2.24605,0.345173,1.64079,-2.09857,0.404824,1.62462,-2.07755,0.190505,1.44624,-1.82149,0.008176,1.50983,-1.87586,0.012179,1.53301,-1.94118,0.018396,1.50973,-1.99036,0.199736,1.42951,-2.07059,0.26809,1.15525,- [...]
+/*674*/{0.164781,1.92672,-1.71688,0.263537,1.86854,-1.84238,-0.001936,1.83891,-1.68821,-0.126549,1.76913,-1.69371,-0.002995,1.67123,-1.66512,0.057849,1.60639,-1.63284,0.248973,1.94288,-2.08643,0.285899,1.86818,-1.91283,0.103277,1.90216,-2.03935,0.145856,1.72177,-2.21555,0.207012,1.63236,-2.24441,0.342693,1.63419,-2.0993,0.40212,1.61597,-2.07907,0.188832,1.44354,-1.8215,0.008226,1.50584,-1.87531,0.011963,1.52934,-1.94078,0.017835,1.50651,-1.99064,0.200198,1.42694,-2.06953,0.265182,1.15162 [...]
+/*675*/{0.165389,1.92316,-1.71597,0.263456,1.86557,-1.84255,-0.001158,1.83261,-1.68514,-0.125729,1.76205,-1.69155,-0.000424,1.66497,-1.66293,0.060498,1.60047,-1.63131,0.24801,1.93972,-2.08643,0.285235,1.86528,-1.91237,0.101864,1.89913,-2.03842,0.141443,1.71829,-2.21356,0.201546,1.62877,-2.24276,0.340216,1.62648,-2.10006,0.399648,1.60699,-2.08063,0.187281,1.43997,-1.82085,0.006977,1.50213,-1.87554,0.011574,1.52646,-1.94079,0.017353,1.50266,-1.99071,0.200202,1.42522,-2.06896,0.262212,1.147 [...]
+/*676*/{0.165792,1.91899,-1.71537,0.261774,1.86176,-1.84304,0.000138,1.82717,-1.68402,-0.12456,1.75465,-1.68736,0.001816,1.65836,-1.66012,0.063978,1.5944,-1.62935,0.247041,1.93531,-2.08768,0.284786,1.86169,-1.91308,0.101347,1.89579,-2.03791,0.136692,1.71512,-2.21197,0.196339,1.62492,-2.24192,0.33718,1.61897,-2.10058,0.397271,1.5982,-2.08321,0.186594,1.43667,-1.81999,0.006796,1.49812,-1.87495,0.011041,1.52265,-1.94079,0.016698,1.49843,-1.99091,0.200041,1.42236,-2.06828,0.258958,1.14534,-1 [...]
+/*677*/{0.166209,1.9141,-1.71463,0.262761,1.85873,-1.8432,0.000655,1.82067,-1.68169,-0.122679,1.74744,-1.68356,0.005235,1.65112,-1.6573,0.067659,1.58802,-1.62763,0.24491,1.93108,-2.08788,0.28435,1.85833,-1.91341,0.100213,1.89173,-2.03747,0.131596,1.71123,-2.2111,0.191676,1.6212,-2.24068,0.333453,1.61081,-2.10163,0.393965,1.58798,-2.08582,0.186277,1.43351,-1.81886,0.005315,1.49409,-1.87529,0.010218,1.51854,-1.94041,0.016212,1.49354,-1.99142,0.20019,1.41944,-2.06692,0.258384,1.14163,-1.747 [...]
+/*678*/{0.165848,1.90881,-1.71411,0.261179,1.85439,-1.843,0.002147,1.81406,-1.67929,-0.120492,1.73914,-1.67932,0.006712,1.64446,-1.65472,0.07196,1.58194,-1.6257,0.24436,1.92648,-2.08864,0.283662,1.85415,-1.9132,0.098778,1.88725,-2.03643,0.126941,1.70741,-2.20944,0.186264,1.61645,-2.23944,0.330948,1.60302,-2.10305,0.390555,1.57779,-2.08764,0.185262,1.43082,-1.81769,0.004657,1.48944,-1.87552,0.009655,1.51428,-1.94023,0.015201,1.48878,-1.99122,0.200301,1.41658,-2.06629,0.256182,1.13807,-1.7 [...]
+/*679*/{0.166061,1.90335,-1.71351,0.266331,1.84694,-1.84185,0.002843,1.80672,-1.67699,-0.118706,1.7301,-1.67437,0.010201,1.63647,-1.65103,0.075666,1.57497,-1.62391,0.24193,1.92178,-2.08932,0.283453,1.84983,-1.91315,0.097706,1.88356,-2.03488,0.122281,1.70348,-2.20869,0.180827,1.61173,-2.23863,0.326438,1.59291,-2.10408,0.386219,1.56662,-2.09006,0.184003,1.42751,-1.81685,0.003246,1.48542,-1.87628,0.008814,1.51088,-1.94038,0.014787,1.484,-1.99211,0.199854,1.41309,-2.06533,0.255397,1.13451,-1 [...]
+/*680*/{0.16608,1.89745,-1.71221,0.265953,1.84234,-1.842,0.004864,1.79849,-1.67382,-0.115881,1.72161,-1.67007,0.013827,1.62862,-1.64855,0.07996,1.56786,-1.62251,0.241031,1.91653,-2.09018,0.28247,1.84504,-1.91387,0.096357,1.87887,-2.03438,0.117369,1.69854,-2.20778,0.174858,1.60737,-2.23775,0.321685,1.58319,-2.10591,0.381865,1.55582,-2.09291,0.182897,1.4242,-1.81535,0.003301,1.4822,-1.87633,0.008316,1.50596,-1.94023,0.013094,1.47936,-1.99254,0.200045,1.40943,-2.06443,0.25259,1.12926,-1.744 [...]
+/*681*/{0.166664,1.89115,-1.71121,0.266588,1.83622,-1.84145,0.005972,1.79045,-1.67094,-0.114262,1.71265,-1.66575,0.017664,1.62058,-1.64541,0.085246,1.56095,-1.62046,0.238525,1.91108,-2.09089,0.282886,1.84063,-1.91379,0.094828,1.87417,-2.03391,0.113436,1.69391,-2.20743,0.168958,1.60238,-2.23688,0.317317,1.57357,-2.10766,0.376869,1.54385,-2.09588,0.182749,1.42061,-1.8136,0.000373,1.47508,-1.87679,0.007625,1.50047,-1.94006,0.012969,1.47377,-1.99197,0.199943,1.40578,-2.06361,0.251267,1.12505 [...]
+/*682*/{0.167338,1.88477,-1.70992,0.266257,1.83265,-1.84232,0.008678,1.78151,-1.66774,-0.112113,1.70179,-1.66087,0.021789,1.61341,-1.64212,0.091061,1.55341,-1.61887,0.237631,1.90601,-2.09172,0.281791,1.83545,-1.91431,0.093787,1.86891,-2.0331,0.107917,1.68951,-2.20718,0.162653,1.59693,-2.23638,0.31279,1.5647,-2.11007,0.371543,1.53199,-2.09868,0.181723,1.41698,-1.81234,-0.00022,1.47065,-1.87767,0.006274,1.49679,-1.94007,0.012786,1.47006,-1.99245,0.199103,1.40195,-2.06308,0.247787,1.1213,-1 [...]
+/*683*/{0.167564,1.87799,-1.70877,0.266307,1.8261,-1.84155,0.01051,1.77264,-1.66382,-0.108145,1.69233,-1.65561,0.02725,1.60436,-1.63889,0.096594,1.5461,-1.61718,0.235161,1.9006,-2.09212,0.281339,1.83019,-1.91498,0.091755,1.86372,-2.03224,0.10361,1.68427,-2.20625,0.156129,1.59168,-2.2361,0.307923,1.55396,-2.11197,0.36593,1.51994,-2.10218,0.18174,1.4131,-1.81035,-0.000598,1.46496,-1.87784,0.005679,1.49097,-1.93963,0.010772,1.46309,-1.9934,0.198615,1.39806,-2.06281,0.246504,1.11588,-1.74361 [...]
+/*684*/{0.168492,1.87108,-1.70729,0.266264,1.82056,-1.84267,0.013101,1.76333,-1.66052,-0.104377,1.68136,-1.64965,0.031471,1.59525,-1.63618,0.102364,1.53889,-1.61567,0.23405,1.89529,-2.09349,0.281332,1.82526,-1.9153,0.090095,1.85881,-2.03082,0.099809,1.67787,-2.20605,0.150133,1.58613,-2.23583,0.302349,1.5438,-2.11473,0.359802,1.50763,-2.10568,0.179711,1.40773,-1.80813,-0.00266,1.46049,-1.87834,0.005163,1.48526,-1.93958,0.009089,1.45691,-1.99333,0.199055,1.39305,-2.06144,0.246585,1.10973,- [...]
+/*685*/{0.168981,1.86461,-1.70573,0.266384,1.8142,-1.84158,0.016073,1.75357,-1.65642,-0.099875,1.67097,-1.64373,0.036895,1.58712,-1.63265,0.108452,1.53198,-1.61501,0.231901,1.88972,-2.09471,0.281401,1.81944,-1.91573,0.08861,1.85339,-2.02981,0.096058,1.6735,-2.2056,0.143661,1.58078,-2.2356,0.296951,1.53225,-2.11719,0.353554,1.49559,-2.10899,0.179285,1.40426,-1.80622,-0.005678,1.4553,-1.8793,0.001962,1.48122,-1.94011,0.007995,1.45154,-1.99376,0.198085,1.38817,-2.06019,0.24263,1.10698,-1.74 [...]
+/*686*/{0.170309,1.8578,-1.7041,0.266299,1.80818,-1.84212,0.019204,1.74346,-1.65252,-0.095387,1.65993,-1.63758,0.043179,1.57805,-1.62916,0.115459,1.52456,-1.61276,0.230274,1.88507,-2.09542,0.281469,1.8142,-1.91648,0.087448,1.84847,-2.02815,0.090694,1.66816,-2.20571,0.136682,1.57542,-2.23548,0.290851,1.52196,-2.12032,0.346638,1.48358,-2.11242,0.17649,1.40003,-1.80464,-0.008219,1.45075,-1.87954,0.000412,1.47364,-1.93967,0.005656,1.44585,-1.99471,0.198187,1.38458,-2.05755,0.240755,1.10298,- [...]
+/*687*/{0.171321,1.85241,-1.70291,0.267003,1.80244,-1.84237,0.023576,1.73305,-1.64806,-0.090729,1.64889,-1.63153,0.050405,1.56964,-1.62654,0.122871,1.51833,-1.61159,0.228659,1.88081,-2.09648,0.281992,1.80857,-1.91685,0.084874,1.84473,-2.02624,0.086569,1.66354,-2.20525,0.130197,1.57062,-2.2353,0.285147,1.51132,-2.12331,0.338987,1.47153,-2.11609,0.173523,1.39601,-1.80371,-0.012088,1.44596,-1.88003,-0.002879,1.46865,-1.94204,0.000996,1.44005,-1.99415,0.196553,1.37939,-2.0575,0.236068,1.1021 [...]
+/*688*/{0.172703,1.84763,-1.70197,0.26749,1.79829,-1.84248,0.028476,1.72175,-1.64427,-0.084546,1.63696,-1.62425,0.057035,1.56132,-1.62266,0.131666,1.51145,-1.60995,0.226685,1.87598,-2.09781,0.282166,1.80386,-1.91801,0.084322,1.8418,-2.02267,0.081894,1.65944,-2.2054,0.122594,1.56587,-2.23475,0.277787,1.4997,-2.12641,0.329691,1.46001,-2.12015,0.170348,1.39201,-1.80187,-0.015484,1.44217,-1.88147,-0.006625,1.46398,-1.9432,0.002462,1.43298,-1.99766,0.194729,1.37411,-2.0568,0.232022,1.10106,-1 [...]
+/*689*/{0.173612,1.84292,-1.70064,0.267988,1.79166,-1.84246,0.033628,1.71127,-1.64075,-0.07686,1.62626,-1.61812,0.065484,1.55327,-1.62024,0.140056,1.50498,-1.60926,0.226015,1.87242,-2.09913,0.283061,1.79925,-1.91881,0.08305,1.83947,-2.02106,0.077605,1.65561,-2.20519,0.116928,1.56231,-2.23397,0.270168,1.49025,-2.13017,0.320973,1.44909,-2.12403,0.167838,1.38986,-1.8,-0.019916,1.43831,-1.88318,-0.009772,1.4599,-1.94364,-0.00341,1.43031,-1.99728,0.193327,1.37072,-2.05493,0.231319,1.10231,-1. [...]
+/*690*/{0.174661,1.83893,-1.69985,0.267524,1.78886,-1.84321,0.039092,1.70182,-1.63769,-0.070508,1.61435,-1.61145,0.07493,1.54662,-1.61741,0.150043,1.49958,-1.60801,0.225544,1.86903,-2.09985,0.282865,1.79447,-1.92046,0.082336,1.83793,-2.01871,0.073376,1.65281,-2.20359,0.10955,1.55933,-2.23314,0.262263,1.48139,-2.13245,0.311537,1.43926,-2.12756,0.164758,1.38726,-1.79793,-0.022889,1.43434,-1.88481,-0.012011,1.45651,-1.94386,-0.006486,1.42443,-1.99971,0.190393,1.36674,-2.05439,0.227231,1.102 [...]
+/*691*/{0.17523,1.8357,-1.69917,0.266939,1.78426,-1.84443,0.045764,1.69433,-1.63478,-0.060726,1.60487,-1.6053,0.083464,1.53963,-1.61559,0.160386,1.49512,-1.60667,0.225177,1.86659,-2.09997,0.283312,1.79036,-1.92131,0.082401,1.83731,-2.01783,0.068826,1.65249,-2.20166,0.102454,1.55808,-2.23281,0.253879,1.4755,-2.1341,0.30267,1.43035,-2.13114,0.161931,1.38647,-1.79553,-0.026041,1.43112,-1.88745,-0.01346,1.45402,-1.94369,-0.009004,1.41913,-2.00074,0.188531,1.36371,-2.05354,0.223107,1.10097,-1 [...]
+/*692*/{0.175854,1.83347,-1.69921,0.267686,1.77992,-1.84492,0.050902,1.68826,-1.63287,-0.049451,1.59652,-1.59998,0.094308,1.53521,-1.61382,0.17092,1.49126,-1.60536,0.224811,1.86331,-2.09938,0.282653,1.78672,-1.92264,0.082578,1.8358,-2.01767,0.062104,1.65483,-2.19949,0.095037,1.55825,-2.23201,0.24484,1.46937,-2.13582,0.292086,1.42269,-2.13436,0.15791,1.38553,-1.79305,-0.028472,1.42952,-1.88872,-0.014857,1.45336,-1.94508,-0.01149,1.41619,-2.0026,0.189297,1.36125,-2.05035,0.218343,1.0991,-1 [...]
+/*693*/{0.1769,1.83131,-1.6997,0.266329,1.77703,-1.84587,0.056205,1.68401,-1.6313,-0.041592,1.58921,-1.59495,0.105043,1.53081,-1.61249,0.182327,1.48905,-1.6038,0.224954,1.86102,-2.09893,0.282356,1.78382,-1.92281,0.082211,1.83472,-2.01753,0.057164,1.65541,-2.19666,0.087477,1.55946,-2.23056,0.235771,1.46514,-2.13813,0.281538,1.41687,-2.13767,0.155009,1.38422,-1.79053,-0.029242,1.42798,-1.88942,-0.016391,1.45249,-1.94493,-0.013423,1.41496,-2.003,0.186173,1.35931,-2.04967,0.215908,1.09787,-1 [...]
+/*694*/{0.177756,1.82969,-1.70005,0.266755,1.77534,-1.84607,0.062013,1.68097,-1.62903,-0.03102,1.58373,-1.59008,0.115836,1.5279,-1.61053,0.194825,1.48759,-1.6035,0.225539,1.85915,-2.09884,0.282023,1.78262,-1.9234,0.082022,1.834,-2.01759,0.051193,1.6579,-2.1936,0.080067,1.56154,-2.2299,0.226624,1.46182,-2.14009,0.271177,1.4129,-2.14084,0.152578,1.38353,-1.78829,-0.031457,1.42729,-1.89057,-0.018436,1.45234,-1.94526,-0.015129,1.41426,-2.00315,0.184904,1.35776,-2.04956,0.208662,1.09228,-1.72 [...]
+/*695*/{0.178893,1.82873,-1.70049,0.266696,1.77333,-1.84728,0.067096,1.67825,-1.62709,-0.02127,1.57894,-1.58574,0.127611,1.52632,-1.6101,0.207082,1.48773,-1.60286,0.22463,1.85739,-2.0988,0.280701,1.77941,-1.92291,0.081592,1.83406,-2.01809,0.045657,1.66183,-2.19089,0.071586,1.56477,-2.22955,0.217668,1.46128,-2.1417,0.260215,1.4094,-2.14355,0.149028,1.3826,-1.78659,-0.031936,1.4273,-1.89002,-0.019975,1.45139,-1.94518,-0.015946,1.41413,-2.00334,0.182169,1.3567,-2.04874,0.202992,1.08846,-1.7 [...]
+/*696*/{0.180483,1.828,-1.7003,0.26592,1.77357,-1.84736,0.072228,1.67612,-1.62516,-0.011703,1.57434,-1.58109,0.138488,1.52534,-1.60826,0.219491,1.48898,-1.60313,0.223796,1.85643,-2.09851,0.280057,1.77909,-1.92329,0.080967,1.83399,-2.01834,0.040247,1.66657,-2.18773,0.063039,1.56851,-2.22953,0.207318,1.4593,-2.14394,0.249371,1.40777,-2.14605,0.148044,1.38244,-1.78558,-0.034312,1.42638,-1.89048,-0.02145,1.45027,-1.94501,-0.017821,1.41453,-2.00274,0.181749,1.35692,-2.04852,0.194824,1.08453,- [...]
+/*697*/{0.181831,1.82778,-1.7003,0.264802,1.77259,-1.84839,0.077129,1.67533,-1.62322,-0.002534,1.57094,-1.57673,0.149776,1.52555,-1.60687,0.232313,1.49176,-1.60332,0.223678,1.8561,-2.0985,0.279508,1.77816,-1.92351,0.080651,1.83414,-2.0183,0.034069,1.67186,-2.18482,0.055195,1.5729,-2.22946,0.198399,1.45974,-2.1463,0.238757,1.40732,-2.14867,0.145212,1.38178,-1.78341,-0.035519,1.4264,-1.88945,-0.02247,1.45059,-1.94438,-0.019249,1.41479,-2.00102,0.180254,1.35786,-2.0485,0.189301,1.07935,-1.7 [...]
+/*698*/{0.182778,1.82778,-1.70086,0.264802,1.77259,-1.84839,0.082465,1.67465,-1.62146,0.007657,1.56738,-1.57253,0.160622,1.527,-1.60533,0.245301,1.49528,-1.60432,0.222995,1.85602,-2.09867,0.278255,1.7773,-1.92396,0.079706,1.83473,-2.01844,0.027658,1.67803,-2.18236,0.046788,1.57824,-2.22919,0.188722,1.46228,-2.14866,0.227818,1.40868,-2.15177,0.143068,1.3822,-1.78273,-0.037794,1.42668,-1.88965,-0.024461,1.45028,-1.94383,-0.02092,1.41605,-2.00033,0.179177,1.35896,-2.04811,0.181459,1.07666,- [...]
+/*699*/{0.184842,1.82869,-1.70059,0.26459,1.77069,-1.84803,0.086561,1.67496,-1.61981,0.017037,1.56552,-1.56888,0.17115,1.52919,-1.60424,0.257086,1.50005,-1.6054,0.22154,1.8562,-2.09861,0.27802,1.77738,-1.92425,0.078823,1.836,-2.01797,0.021413,1.68365,-2.18033,0.038604,1.58358,-2.22902,0.17924,1.46383,-2.1506,0.21731,1.41046,-2.1539,0.142035,1.382,-1.78274,-0.039066,1.42679,-1.88934,-0.026247,1.45053,-1.94411,-0.022153,1.41649,-1.99948,0.178127,1.36046,-2.0481,0.173555,1.07526,-1.72084,0. [...]
+/*700*/{0.186147,1.83013,-1.70058,0.264978,1.77178,-1.84951,0.091148,1.67571,-1.61804,0.027309,1.56309,-1.56473,0.181342,1.53189,-1.60371,0.268553,1.50551,-1.60675,0.219644,1.85739,-2.09844,0.277507,1.77833,-1.92466,0.077322,1.83736,-2.01691,0.017096,1.6903,-2.17872,0.029671,1.59024,-2.22945,0.170183,1.46891,-2.15231,0.207249,1.41405,-2.15575,0.140128,1.38072,-1.78164,-0.039929,1.42714,-1.88918,-0.026015,1.45139,-1.94376,-0.02327,1.41764,-1.99905,0.177579,1.36271,-2.04759,0.165922,1.0733 [...]
+/*701*/{0.188492,1.83203,-1.70083,0.264423,1.77369,-1.85033,0.09662,1.67715,-1.61621,0.03754,1.56253,-1.56133,0.191575,1.53545,-1.60361,0.279039,1.51173,-1.60882,0.216781,1.85842,-2.09819,0.275332,1.77991,-1.92585,0.076565,1.83876,-2.01556,0.011053,1.69681,-2.17743,0.021347,1.59623,-2.22962,0.161717,1.47406,-2.15402,0.19737,1.4184,-2.15756,0.139368,1.38084,-1.78168,-0.040008,1.42879,-1.88843,-0.027184,1.45281,-1.9428,-0.024646,1.41799,-1.99857,0.176736,1.36521,-2.0476,0.159342,1.07292,-1 [...]
+/*702*/{0.19105,1.83416,-1.70101,0.263714,1.77565,-1.85088,0.102674,1.67839,-1.61437,0.046783,1.56319,-1.55792,0.201316,1.54028,-1.60355,0.289823,1.51896,-1.61083,0.214648,1.86062,-2.09845,0.274252,1.78137,-1.92692,0.074794,1.84074,-2.01508,0.004897,1.70325,-2.17688,0.012457,1.60326,-2.22981,0.151687,1.47916,-2.15532,0.187085,1.42321,-2.15941,0.138414,1.38023,-1.78165,-0.04085,1.42993,-1.8884,-0.027695,1.45365,-1.94284,-0.024708,1.41868,-1.99819,0.176423,1.36682,-2.04751,0.15194,1.07179, [...]
+/*703*/{0.193192,1.83646,-1.70163,0.263398,1.77744,-1.85207,0.108433,1.6803,-1.61347,0.055981,1.56373,-1.55554,0.21086,1.54483,-1.60401,0.299789,1.52686,-1.61396,0.212012,1.86314,-2.09886,0.274338,1.78328,-1.92774,0.073111,1.84191,-2.01226,-0.00151,1.70973,-2.17621,0.00499,1.6107,-2.22965,0.143093,1.48477,-2.15644,0.178103,1.42925,-2.1608,0.138218,1.38022,-1.78186,-0.040023,1.43245,-1.88766,-0.028116,1.45485,-1.94163,-0.025448,1.42011,-1.99811,0.174059,1.36777,-2.04764,0.142979,1.06854,- [...]
+/*704*/{0.195703,1.83961,-1.70192,0.265057,1.78021,-1.85277,0.11441,1.68239,-1.61199,0.06678,1.56512,-1.55351,0.219808,1.55143,-1.60481,0.308145,1.53535,-1.6164,0.208925,1.86579,-2.09908,0.273154,1.78581,-1.92926,0.072006,1.8444,-2.01142,-0.007708,1.71643,-2.17573,-0.003792,1.6175,-2.22959,0.134505,1.49154,-2.15778,0.168651,1.43583,-2.16245,0.13795,1.38031,-1.78147,-0.041163,1.43319,-1.88735,-0.028699,1.45684,-1.94122,-0.025237,1.42244,-1.99808,0.174023,1.3703,-2.04699,0.134831,1.06691,- [...]
+/*705*/{0.197713,1.84294,-1.70268,0.263835,1.78227,-1.8538,0.120734,1.68484,-1.61104,0.074368,1.56676,-1.55074,0.228026,1.5572,-1.60663,0.31714,1.54449,-1.62006,0.205824,1.86902,-2.09921,0.272649,1.78807,-1.92979,0.070473,1.84576,-2.00868,-0.015068,1.72333,-2.17498,-0.01086,1.62491,-2.22923,0.125663,1.49775,-2.15859,0.159779,1.44092,-2.16331,0.13826,1.38083,-1.78164,-0.041483,1.43558,-1.88664,-0.028332,1.45843,-1.94071,-0.025361,1.42443,-1.99774,0.174246,1.37244,-2.04623,0.12696,1.066,-1 [...]
+/*706*/{0.200149,1.847,-1.70315,0.265004,1.78574,-1.85433,0.12811,1.68723,-1.60961,0.084315,1.56922,-1.54874,0.236611,1.56456,-1.60786,0.325029,1.55356,-1.62335,0.201297,1.87263,-2.09892,0.271829,1.79096,-1.93113,0.068637,1.84819,-2.00693,-0.02018,1.73017,-2.1743,-0.018694,1.6321,-2.22886,0.117805,1.50481,-2.15974,0.152772,1.44992,-2.16541,0.13805,1.38072,-1.7796,-0.040475,1.43741,-1.8854,-0.028076,1.46076,-1.93893,-0.02496,1.42625,-1.99649,0.173432,1.37469,-2.04557,0.118759,1.06535,-1.7 [...]
+/*707*/{0.202867,1.85116,-1.70371,0.265712,1.78967,-1.85585,0.134481,1.69024,-1.60874,0.093827,1.57225,-1.54696,0.243875,1.57092,-1.60889,0.332312,1.56288,-1.62609,0.198394,1.87664,-2.0988,0.271893,1.79485,-1.93263,0.066587,1.85069,-2.00527,-0.026179,1.73662,-2.17373,-0.02597,1.63963,-2.2283,0.110404,1.51276,-2.16009,0.144934,1.45735,-2.16681,0.138728,1.38169,-1.7794,-0.039973,1.43986,-1.88498,-0.028115,1.46347,-1.93761,-0.02528,1.4297,-1.99684,0.173414,1.37775,-2.04453,0.112173,1.06502, [...]
+/*708*/{0.205157,1.85566,-1.7044,0.266591,1.79318,-1.85639,0.141761,1.69394,-1.60821,0.102176,1.57545,-1.54545,0.250423,1.57868,-1.61115,0.338253,1.57227,-1.62992,0.195072,1.88071,-2.09902,0.271534,1.7982,-1.93308,0.065259,1.85296,-2.00203,-0.030366,1.74367,-2.17266,-0.033136,1.64715,-2.22758,0.102691,1.52004,-2.16026,0.137623,1.46543,-2.16804,0.139213,1.3823,-1.77802,-0.039323,1.44187,-1.88389,-0.027584,1.46567,-1.93631,-0.024347,1.43275,-1.99653,0.174371,1.38175,-2.04312,0.107969,1.067 [...]
+/*709*/{0.207826,1.86092,-1.70553,0.26665,1.7964,-1.85753,0.14847,1.69788,-1.6081,0.109856,1.57887,-1.54416,0.256566,1.58665,-1.61275,0.344428,1.58239,-1.63329,0.191757,1.88487,-2.09869,0.271392,1.80241,-1.93449,0.064467,1.85574,-2.00064,-0.035908,1.7507,-2.17216,-0.039936,1.65439,-2.22624,0.09457,1.5282,-2.16122,0.129939,1.4729,-2.1696,0.139372,1.38321,-1.77662,-0.038533,1.44501,-1.88356,-0.026465,1.46865,-1.93492,-0.025272,1.43688,-1.99544,0.173684,1.38485,-2.04183,0.099675,1.06631,-1. [...]
+/*710*/{0.209469,1.86632,-1.70649,0.267534,1.80109,-1.8595,0.153915,1.70266,-1.6078,0.118965,1.58402,-1.5433,0.262651,1.5934,-1.6139,0.349997,1.59212,-1.63669,0.189349,1.88913,-2.0982,0.271555,1.8061,-1.93605,0.063544,1.85781,-1.99856,-0.041246,1.75871,-2.17155,-0.046882,1.66201,-2.22488,0.088965,1.53607,-2.16086,0.123596,1.48049,-2.17069,0.14029,1.38415,-1.77554,-0.036596,1.44714,-1.88199,-0.025921,1.47168,-1.93251,-0.023134,1.4404,-1.99512,0.174326,1.38846,-2.04064,0.094164,1.06751,-1. [...]
+/*711*/{0.212293,1.8714,-1.70761,0.26893,1.80508,-1.86029,0.160345,1.70743,-1.608,0.12496,1.58819,-1.54253,0.267231,1.60116,-1.61538,0.354403,1.60193,-1.6402,0.186813,1.89317,-2.09772,0.271462,1.8101,-1.93722,0.06235,1.86099,-1.99611,-0.044393,1.76453,-2.17065,-0.052981,1.66987,-2.22344,0.082022,1.54382,-2.16108,0.117033,1.48864,-2.17175,0.140807,1.38516,-1.7744,-0.035822,1.44984,-1.88166,-0.024945,1.47464,-1.9312,-0.021123,1.44458,-1.99445,0.174912,1.39229,-2.03929,0.086765,1.06888,-1.7 [...]
+/*712*/{0.214089,1.87713,-1.70897,0.26971,1.80941,-1.86144,0.16554,1.71246,-1.60782,0.131092,1.59257,-1.54076,0.272047,1.60948,-1.61746,0.358817,1.61237,-1.64323,0.184443,1.89752,-2.09751,0.271435,1.8146,-1.93844,0.061507,1.86456,-1.99487,-0.04751,1.77131,-2.16908,-0.058481,1.67778,-2.22125,0.075571,1.55046,-2.16016,0.110917,1.49594,-2.17285,0.141874,1.38598,-1.77336,-0.035397,1.45348,-1.88022,-0.023516,1.47817,-1.92868,-0.02101,1.45005,-1.99401,0.175537,1.39675,-2.03799,0.082099,1.07025 [...]
+/*713*/{0.215923,1.88217,-1.71016,0.269335,1.81389,-1.86315,0.170082,1.71774,-1.60785,0.138088,1.5982,-1.54064,0.275714,1.61685,-1.61896,0.362133,1.62135,-1.64673,0.181902,1.90127,-2.097,0.271446,1.81925,-1.93934,0.060532,1.86782,-1.9933,-0.052085,1.77743,-2.16763,-0.064248,1.68501,-2.21965,0.069699,1.55746,-2.16013,0.104667,1.50308,-2.1739,0.142932,1.38697,-1.77253,-0.031972,1.45663,-1.87871,-0.02275,1.48192,-1.9269,-0.019739,1.45441,-1.99369,0.175903,1.40108,-2.0369,0.074936,1.07146,-1 [...]
+/*714*/{0.216675,1.88805,-1.71199,0.269937,1.81806,-1.86391,0.174354,1.72321,-1.60828,0.143254,1.60272,-1.53989,0.279735,1.6252,-1.62142,0.365439,1.63031,-1.65,0.180399,1.90444,-2.09681,0.270952,1.82295,-1.94045,0.060089,1.87153,-1.99244,-0.054441,1.78353,-2.16538,-0.069972,1.6928,-2.21743,0.064256,1.56491,-2.16002,0.098982,1.51078,-2.17425,0.145155,1.38808,-1.77256,-0.03111,1.46035,-1.87777,-0.022051,1.48558,-1.92536,-0.018587,1.45974,-1.99217,0.176107,1.40547,-2.03616,0.070304,1.07386, [...]
+/*715*/{0.217996,1.89277,-1.71338,0.270252,1.82256,-1.86551,0.178498,1.72874,-1.60822,0.148399,1.60839,-1.53943,0.282837,1.63221,-1.62329,0.367618,1.63917,-1.65309,0.180314,1.90859,-2.09672,0.270048,1.8271,-1.94203,0.060128,1.87535,-1.99187,-0.057212,1.79053,-2.16364,-0.074799,1.70042,-2.21534,0.059253,1.57207,-2.15945,0.093541,1.51748,-2.17484,0.146059,1.38842,-1.77218,-0.029756,1.46378,-1.877,-0.020343,1.48941,-1.92299,-0.017777,1.46428,-1.99143,0.175662,1.40989,-2.03544,0.064298,1.075 [...]
+/*716*/{0.220143,1.89877,-1.7145,0.271611,1.82687,-1.86717,0.181596,1.73376,-1.6087,0.152397,1.6131,-1.53913,0.28596,1.63986,-1.62475,0.369599,1.64777,-1.65622,0.178921,1.91206,-2.0966,0.270558,1.83152,-1.94283,0.059566,1.87854,-1.99091,-0.060341,1.79686,-2.16072,-0.079123,1.70748,-2.21245,0.054375,1.57823,-2.15878,0.088644,1.52385,-2.17527,0.148882,1.38942,-1.77423,-0.027548,1.46729,-1.87532,-0.018587,1.49294,-1.92078,-0.016278,1.46947,-1.99042,0.176689,1.41501,-2.03524,0.061615,1.07838 [...]
+/*717*/{0.221319,1.90333,-1.71507,0.272682,1.83147,-1.86829,0.185092,1.73925,-1.60832,0.156389,1.6183,-1.53806,0.287595,1.64657,-1.62619,0.371382,1.65584,-1.65925,0.178063,1.91476,-2.09703,0.270566,1.83546,-1.94403,0.059513,1.88154,-1.98962,-0.06044,1.80413,-2.15746,-0.083503,1.7145,-2.20987,0.049815,1.5851,-2.15832,0.084271,1.53016,-2.17545,0.151673,1.38991,-1.7752,-0.025363,1.47111,-1.8746,-0.017037,1.4969,-1.91912,-0.014988,1.47433,-1.98893,0.176657,1.42018,-2.03505,0.05441,1.08165,-1 [...]
+/*718*/{0.221956,1.90784,-1.7167,0.27273,1.8351,-1.86944,0.187643,1.74421,-1.60851,0.160332,1.62342,-1.53777,0.289327,1.65362,-1.62795,0.373258,1.66298,-1.66254,0.177073,1.9176,-2.09734,0.270518,1.839,-1.94484,0.059528,1.88494,-1.98958,-0.063078,1.81067,-2.1547,-0.087244,1.72164,-2.20764,0.045809,1.59089,-2.15781,0.079629,1.53631,-2.17578,0.153686,1.39155,-1.77544,-0.023132,1.47506,-1.87241,-0.015895,1.50052,-1.91677,-0.013648,1.47906,-1.98791,0.1775,1.42538,-2.03569,0.05211,1.08335,-1.7 [...]
+/*719*/{0.224586,1.91265,-1.71777,0.274375,1.84011,-1.8717,0.18983,1.74883,-1.60804,0.164533,1.62857,-1.53778,0.290542,1.65988,-1.62979,0.374008,1.67018,-1.66581,0.177384,1.92043,-2.09707,0.270914,1.84246,-1.94573,0.059973,1.88916,-1.98947,-0.0664,1.81937,-2.15067,-0.090997,1.72851,-2.20463,0.042508,1.59654,-2.15626,0.075663,1.54187,-2.1756,0.156262,1.39234,-1.77659,-0.021419,1.47831,-1.87085,-0.0152,1.50418,-1.91553,-0.012154,1.48445,-1.98649,0.177043,1.42982,-2.03593,0.048094,1.08653,- [...]
+/*720*/{0.226185,1.9165,-1.71877,0.273943,1.84319,-1.87184,0.192119,1.75393,-1.60844,0.166667,1.63243,-1.53651,0.29224,1.66532,-1.63102,0.374437,1.67672,-1.66804,0.176659,1.92343,-2.0969,0.271408,1.84569,-1.94575,0.059938,1.89235,-1.98843,-0.067561,1.82534,-2.14652,-0.094108,1.7346,-2.20151,0.03822,1.6017,-2.15609,0.071866,1.54624,-2.17513,0.158984,1.39378,-1.77845,-0.019153,1.4823,-1.86974,-0.01333,1.50791,-1.91242,-0.010155,1.48904,-1.98526,0.177867,1.434,-2.03643,0.045076,1.08972,-1.7 [...]
+/*721*/{0.227341,1.91983,-1.71992,0.275837,1.84767,-1.87337,0.19328,1.7581,-1.60882,0.16873,1.6367,-1.5362,0.292793,1.66988,-1.63257,0.374892,1.6822,-1.6698,0.176898,1.92624,-2.09716,0.271661,1.84931,-1.94741,0.06004,1.89567,-1.98804,-0.068197,1.83101,-2.14199,-0.096617,1.74076,-2.19857,0.036017,1.60719,-2.15515,0.068353,1.55125,-2.17456,0.161093,1.3951,-1.77876,-0.017346,1.48576,-1.86808,-0.011424,1.51157,-1.91136,-0.008517,1.49422,-1.98361,0.177191,1.43849,-2.03621,0.04185,1.09201,-1.7 [...]
+/*722*/{0.22914,1.9235,-1.72054,0.276721,1.85083,-1.87413,0.196119,1.76281,-1.60871,0.170697,1.64081,-1.53602,0.2927,1.67459,-1.63429,0.375207,1.68731,-1.67272,0.175795,1.92836,-2.0963,0.271949,1.85228,-1.94744,0.060373,1.8976,-1.987,-0.068671,1.8361,-2.13875,-0.098932,1.74636,-2.19541,0.03373,1.61166,-2.15439,0.06573,1.55592,-2.17406,0.163272,1.39705,-1.78001,-0.015283,1.48891,-1.86582,-0.011734,1.51575,-1.90846,-0.007504,1.49811,-1.98209,0.178052,1.44273,-2.03699,0.038224,1.09594,-1.75 [...]
+/*723*/{0.230843,1.92703,-1.72106,0.27722,1.85415,-1.8749,0.197389,1.76616,-1.60875,0.173615,1.6444,-1.53525,0.293036,1.67849,-1.63491,0.374856,1.69168,-1.67523,0.176214,1.93097,-2.09629,0.273067,1.85558,-1.94782,0.062794,1.89897,-1.98629,-0.067264,1.83621,-2.13291,-0.100625,1.75162,-2.19279,0.031622,1.61558,-2.15272,0.063312,1.55978,-2.17359,0.165397,1.39897,-1.78075,-0.014201,1.49243,-1.86389,-0.011508,1.51909,-1.90703,-0.006468,1.50342,-1.98058,0.177417,1.44643,-2.03728,0.035285,1.099 [...]
+/*724*/{0.231963,1.92953,-1.72177,0.278222,1.85634,-1.87563,0.198707,1.76993,-1.60845,0.173814,1.64704,-1.53499,0.293467,1.68227,-1.63601,0.37445,1.69491,-1.67694,0.175981,1.93319,-2.09599,0.273882,1.85797,-1.94881,0.063607,1.90136,-1.98615,-0.06742,1.84088,-2.13,-0.102788,1.75632,-2.19012,0.02996,1.61927,-2.15143,0.060676,1.5627,-2.17335,0.16684,1.40106,-1.78206,-0.012946,1.4955,-1.86266,-0.010084,1.5231,-1.90514,-0.006347,1.50738,-1.97839,0.177084,1.44984,-2.0371,0.033462,1.10404,-1.75 [...]
+/*725*/{0.233899,1.93227,-1.72245,0.279366,1.86042,-1.87679,0.199921,1.77295,-1.60869,0.175425,1.6501,-1.53375,0.293731,1.68462,-1.63648,0.374377,1.69752,-1.67928,0.176057,1.93518,-2.09578,0.27432,1.86022,-1.9494,0.064349,1.90406,-1.98528,-0.067302,1.8439,-2.12551,-0.104106,1.7602,-2.18719,0.028466,1.62267,-2.15083,0.058555,1.56589,-2.17253,0.167858,1.40359,-1.78206,-0.011419,1.49865,-1.8602,-0.009093,1.52609,-1.90352,-0.004879,1.51053,-1.97614,0.177556,1.45382,-2.03732,0.033545,1.10622, [...]
+/*726*/{0.235106,1.93427,-1.72275,0.279109,1.86256,-1.87662,0.200808,1.77586,-1.60834,0.175665,1.6519,-1.5333,0.293909,1.68702,-1.63756,0.374181,1.70006,-1.68098,0.175228,1.93698,-2.09558,0.274723,1.86248,-1.94945,0.06534,1.90577,-1.98401,-0.06778,1.84785,-2.12309,-0.105862,1.76351,-2.18501,0.027012,1.6252,-2.15003,0.057213,1.56844,-2.17204,0.169352,1.40592,-1.78314,-0.010226,1.5021,-1.85859,-0.008582,1.52951,-1.90266,-0.004555,1.5138,-1.97451,0.177228,1.45668,-2.03734,0.031122,1.11083,- [...]
+/*727*/{0.237265,1.93567,-1.7227,0.280944,1.86475,-1.87725,0.201299,1.7783,-1.60821,0.176673,1.65493,-1.53306,0.293646,1.68808,-1.63887,0.37383,1.70126,-1.68329,0.176522,1.93891,-2.0958,0.275372,1.86441,-1.95072,0.065417,1.90854,-1.98335,-0.068424,1.85067,-2.12119,-0.106252,1.76612,-2.18238,0.026483,1.62722,-2.14845,0.055497,1.57021,-2.17184,0.170794,1.40899,-1.78321,-0.010215,1.50467,-1.85739,-0.008814,1.53244,-1.90222,-0.004349,1.51718,-1.97282,0.177052,1.45924,-2.03694,0.031148,1.1142 [...]
+/*728*/{0.237986,1.9372,-1.72301,0.280955,1.86681,-1.87784,0.201833,1.78045,-1.60826,0.176813,1.65661,-1.53229,0.293614,1.68937,-1.63946,0.3728,1.70187,-1.68491,0.176031,1.94089,-2.09585,0.275759,1.86621,-1.95094,0.066156,1.90937,-1.98305,-0.068002,1.8518,-2.11749,-0.106932,1.76833,-2.18052,0.025109,1.62881,-2.14736,0.055163,1.57144,-2.171,0.17047,1.41135,-1.78318,-0.008791,1.50719,-1.856,-0.008036,1.53561,-1.90057,-0.004552,1.51977,-1.97168,0.176388,1.46139,-2.03659,0.032716,1.116,-1.74 [...]
+/*729*/{0.239593,1.93796,-1.72319,0.282637,1.86935,-1.87903,0.202534,1.7821,-1.60812,0.176956,1.65778,-1.53184,0.29336,1.68972,-1.63986,0.372665,1.70238,-1.68618,0.176674,1.94203,-2.09599,0.276367,1.86766,-1.95123,0.066268,1.91214,-1.98303,-0.068263,1.85407,-2.11645,-0.107606,1.76979,-2.17889,0.02522,1.62989,-2.14682,0.054061,1.5728,-2.17075,0.171488,1.41419,-1.78315,-0.008767,1.50941,-1.85525,-0.007967,1.53799,-1.89993,-0.00444,1.5215,-1.96988,0.17634,1.46374,-2.03661,0.033199,1.11738,- [...]
+/*730*/{0.240651,1.93888,-1.72322,0.282995,1.8704,-1.87884,0.202266,1.78334,-1.60838,0.176735,1.65879,-1.53183,0.293287,1.68955,-1.64066,0.372663,1.70002,-1.68713,0.1768,1.94389,-2.09613,0.277283,1.86826,-1.95186,0.067972,1.91349,-1.98193,-0.068635,1.8547,-2.11423,-0.107797,1.77064,-2.1769,0.025294,1.63066,-2.14616,0.054083,1.57316,-2.17037,0.171868,1.4169,-1.78251,-0.008019,1.51212,-1.85401,-0.008983,1.54065,-1.8993,-0.005286,1.5235,-1.96832,0.176513,1.46488,-2.0366,0.037122,1.11817,-1. [...]
+/*731*/{0.241274,1.9391,-1.7236,0.283724,1.87173,-1.87985,0.202557,1.78375,-1.60861,0.176552,1.65894,-1.53107,0.292992,1.6882,-1.6412,0.37193,1.69828,-1.68799,0.177576,1.94477,-2.09627,0.277948,1.86918,-1.95301,0.067535,1.91602,-1.98207,-0.069077,1.85499,-2.11308,-0.108139,1.77057,-2.17492,0.025018,1.63016,-2.14491,0.053649,1.57287,-2.16953,0.17223,1.41905,-1.78217,-0.007532,1.51273,-1.85259,-0.009577,1.54235,-1.89895,-0.005018,1.52476,-1.96756,0.176176,1.46644,-2.03613,0.04165,1.11797,- [...]
+/*732*/{0.242029,1.93862,-1.72382,0.28352,1.87236,-1.88082,0.202827,1.7842,-1.60858,0.176487,1.65941,-1.53091,0.292262,1.68719,-1.64162,0.371215,1.69566,-1.68873,0.177694,1.94579,-2.09651,0.27827,1.8704,-1.95285,0.068779,1.91562,-1.98194,-0.069203,1.85499,-2.11123,-0.107822,1.77066,-2.17446,0.025697,1.62971,-2.14444,0.054353,1.57149,-2.16908,0.172736,1.42077,-1.78161,-0.007539,1.51422,-1.85189,-0.009434,1.54387,-1.89863,-0.005139,1.52497,-1.96689,0.176073,1.46669,-2.03595,0.048113,1.1171 [...]
+/*733*/{0.243823,1.93848,-1.7233,0.283962,1.87342,-1.8812,0.202855,1.7841,-1.60871,0.175366,1.65916,-1.53044,0.291955,1.68498,-1.64237,0.370137,1.6925,-1.68921,0.178642,1.9464,-2.09638,0.278726,1.87073,-1.95308,0.069944,1.9177,-1.98079,-0.069185,1.85489,-2.11083,-0.107495,1.76995,-2.17186,0.025994,1.6285,-2.14359,0.054874,1.57088,-2.16873,0.173196,1.42277,-1.78143,-0.007713,1.51459,-1.85176,-0.008567,1.54484,-1.89912,-0.005303,1.52495,-1.9664,0.1761,1.46737,-2.03535,0.053163,1.11527,-1.7 [...]
+/*734*/{0.244451,1.93762,-1.72333,0.28384,1.87345,-1.88121,0.20232,1.78355,-1.60859,0.173444,1.6584,-1.5313,0.290428,1.68301,-1.64262,0.370007,1.68868,-1.68985,0.178729,1.94667,-2.09637,0.278812,1.87075,-1.95322,0.070323,1.9181,-1.98091,-0.067924,1.85363,-2.10991,-0.106362,1.76883,-2.1712,0.027406,1.62786,-2.14319,0.055191,1.56826,-2.16893,0.173156,1.42366,-1.78103,-0.007914,1.51498,-1.85206,-0.008634,1.54536,-1.89941,-0.005481,1.52478,-1.96605,0.176267,1.46751,-2.03481,0.058436,1.11344, [...]
+/*735*/{0.244714,1.93713,-1.72345,0.283723,1.8736,-1.8814,0.201641,1.78276,-1.60917,0.172588,1.65833,-1.53187,0.290055,1.6799,-1.64307,0.369342,1.684,-1.69046,0.17918,1.94672,-2.09646,0.278899,1.87074,-1.95335,0.070566,1.91824,-1.98146,-0.067185,1.85319,-2.10929,-0.10529,1.76719,-2.16993,0.029702,1.62488,-2.14237,0.05575,1.56646,-2.1682,0.173207,1.42465,-1.78072,-0.008051,1.51461,-1.85207,-0.008634,1.54536,-1.89941,-0.006162,1.52366,-1.96594,0.17674,1.4672,-2.03409,0.063532,1.11144,-1.75 [...]
+/*736*/{0.245032,1.93619,-1.72353,0.283661,1.87314,-1.88146,0.20052,1.78188,-1.60907,0.170203,1.6573,-1.53165,0.28875,1.67641,-1.64343,0.36782,1.67896,-1.69089,0.179349,1.94672,-2.09643,0.278389,1.8701,-1.95308,0.071156,1.91854,-1.98208,-0.066216,1.85114,-2.10886,-0.103605,1.76461,-2.16942,0.030991,1.62238,-2.14248,0.057107,1.56368,-2.16794,0.173409,1.42511,-1.78063,-0.007786,1.51387,-1.85158,-0.008567,1.54484,-1.89912,-0.006292,1.52246,-1.96545,0.177107,1.4666,-2.03368,0.072452,1.11048, [...]
+/*737*/{0.245648,1.93421,-1.72348,0.283361,1.87249,-1.88182,0.199412,1.78046,-1.60958,0.169317,1.65587,-1.53171,0.28773,1.67315,-1.64327,0.36675,1.67397,-1.69066,0.179528,1.94653,-2.09638,0.27826,1.86921,-1.9535,0.071816,1.91857,-1.98249,-0.065238,1.84871,-2.10772,-0.102522,1.76194,-2.1686,0.031938,1.61934,-2.14187,0.058044,1.56048,-2.16841,0.173861,1.42512,-1.77975,-0.007959,1.51268,-1.85183,-0.008736,1.54379,-1.89897,-0.006565,1.52054,-1.96583,0.177855,1.46605,-2.03339,0.078624,1.1084, [...]
+/*738*/{0.244657,1.93215,-1.72372,0.282331,1.87218,-1.8824,0.198363,1.77897,-1.60992,0.166673,1.65353,-1.53235,0.286146,1.66791,-1.64386,0.365525,1.66739,-1.69028,0.179311,1.94556,-2.09636,0.278003,1.86888,-1.95324,0.071544,1.91837,-1.98302,-0.065539,1.84597,-2.10793,-0.100736,1.75851,-2.16831,0.033313,1.61623,-2.14257,0.059562,1.55667,-2.1685,0.173783,1.42493,-1.7791,-0.008351,1.51173,-1.85276,-0.008721,1.54245,-1.89945,-0.006553,1.5186,-1.9663,0.177678,1.46418,-2.03274,0.084552,1.10598 [...]
+/*739*/{0.244291,1.93008,-1.72395,0.282714,1.87078,-1.88219,0.197199,1.77704,-1.61039,0.164508,1.65198,-1.5334,0.283934,1.66354,-1.64376,0.363531,1.66096,-1.69018,0.179771,1.94462,-2.09628,0.277326,1.86807,-1.9534,0.071173,1.91744,-1.98362,-0.06431,1.84387,-2.10788,-0.099471,1.75508,-2.16825,0.034485,1.61261,-2.1425,0.061428,1.55274,-2.16859,0.174202,1.42408,-1.77914,-0.009057,1.50992,-1.85319,-0.008202,1.54079,-1.89921,-0.006843,1.51638,-1.96636,0.177622,1.46307,-2.03206,0.092122,1.1034 [...]
+/*740*/{0.24415,1.92767,-1.7236,0.282209,1.86972,-1.88209,0.194833,1.77485,-1.61035,0.161371,1.65081,-1.5345,0.28161,1.65882,-1.64373,0.36224,1.65358,-1.68981,0.179653,1.94284,-2.09619,0.277279,1.86738,-1.95338,0.07121,1.91711,-1.98396,-0.063322,1.84061,-2.1083,-0.097501,1.75077,-2.16861,0.035876,1.60798,-2.1432,0.063301,1.54867,-2.16892,0.17442,1.4233,-1.77901,-0.008443,1.50799,-1.85251,-0.007947,1.53878,-1.89895,-0.006927,1.51452,-1.96737,0.17829,1.46077,-2.03236,0.098567,1.10077,-1.76 [...]
+/*741*/{0.243788,1.92538,-1.72388,0.280898,1.86757,-1.88201,0.193506,1.77282,-1.61069,0.15757,1.64759,-1.53505,0.279891,1.65261,-1.64352,0.360879,1.64645,-1.6894,0.178986,1.9412,-2.09589,0.275854,1.86507,-1.9535,0.070745,1.91489,-1.98478,-0.063286,1.83633,-2.10885,-0.095776,1.746,-2.16928,0.037825,1.60353,-2.14438,0.065542,1.54389,-2.16939,0.174835,1.42131,-1.77844,-0.008592,1.50528,-1.8528,-0.008535,1.53621,-1.89958,-0.00712,1.51141,-1.968,0.178362,1.45811,-2.03268,0.103516,1.09855,-1.7 [...]
+/*742*/{0.24249,1.92206,-1.72315,0.281193,1.86576,-1.88188,0.190574,1.76993,-1.61078,0.155785,1.6463,-1.53663,0.277503,1.64772,-1.64348,0.358543,1.63847,-1.6881,0.179459,1.93915,-2.09586,0.275823,1.86409,-1.95344,0.07007,1.9136,-1.98598,-0.062855,1.83272,-2.10991,-0.094098,1.74048,-2.16977,0.039868,1.59832,-2.14506,0.06743,1.53919,-2.16997,0.176367,1.42009,-1.77999,-0.008765,1.50285,-1.85353,-0.007785,1.53374,-1.90037,-0.006418,1.50851,-1.96899,0.17808,1.45502,-2.03354,0.110093,1.09638,- [...]
+/*743*/{0.242475,1.91989,-1.72269,0.281059,1.86416,-1.88193,0.1892,1.76727,-1.61038,0.150658,1.64352,-1.53847,0.274766,1.64135,-1.64313,0.356281,1.63021,-1.68736,0.178484,1.93632,-2.09574,0.275286,1.86153,-1.95324,0.070041,1.91106,-1.98623,-0.061304,1.82712,-2.11117,-0.092428,1.73499,-2.17059,0.04193,1.59313,-2.14595,0.069889,1.53348,-2.17096,0.1766,1.41795,-1.77998,-0.008976,1.49967,-1.85372,-0.00817,1.53053,-1.90011,-0.007167,1.50614,-1.97024,0.177304,1.45168,-2.03447,0.11609,1.0932,-1 [...]
+/*744*/{0.241187,1.91693,-1.72219,0.281192,1.86153,-1.88201,0.18616,1.76404,-1.61031,0.147667,1.63975,-1.53877,0.271923,1.63499,-1.64264,0.353214,1.62124,-1.68596,0.177292,1.93322,-2.09578,0.274731,1.86019,-1.95354,0.068905,1.90809,-1.98687,-0.061387,1.82158,-2.1124,-0.090963,1.72875,-2.17165,0.044526,1.58754,-2.14761,0.072574,1.52807,-2.1716,0.177131,1.41595,-1.78038,-0.008679,1.4966,-1.85476,-0.007583,1.52731,-1.9003,-0.006602,1.50245,-1.97101,0.177189,1.44908,-2.03533,0.120601,1.0895, [...]
+/*745*/{0.239731,1.91376,-1.72147,0.280471,1.85809,-1.88133,0.183968,1.76069,-1.61024,0.144167,1.63749,-1.5401,0.269134,1.62806,-1.64284,0.351204,1.61236,-1.68466,0.176876,1.93,-2.09591,0.2749,1.85788,-1.95303,0.068199,1.90466,-1.98841,-0.060252,1.81558,-2.11421,-0.089453,1.72169,-2.17266,0.046692,1.58175,-2.14911,0.075055,1.52237,-2.17287,0.177658,1.41335,-1.781,-0.007808,1.49306,-1.85472,-0.008104,1.52377,-1.90042,-0.007116,1.49967,-1.971,0.176301,1.44623,-2.03573,0.126025,1.08707,-1.7 [...]
+/*746*/{0.238399,1.91023,-1.72073,0.280919,1.855,-1.88096,0.181246,1.75726,-1.61002,0.139853,1.63494,-1.54141,0.265787,1.62073,-1.64234,0.347554,1.60283,-1.68319,0.175564,1.92646,-2.09618,0.274444,1.85484,-1.95233,0.065894,1.90148,-1.98894,-0.061239,1.80883,-2.11517,-0.087914,1.71462,-2.17394,0.049466,1.57533,-2.15046,0.078847,1.5164,-2.17335,0.17855,1.41058,-1.78203,-0.007686,1.48956,-1.85524,-0.008311,1.52033,-1.9009,-0.007468,1.49503,-1.97075,0.175575,1.44332,-2.03623,0.129647,1.08476 [...]
+/*747*/{0.237085,1.90589,-1.7199,0.280648,1.85231,-1.88046,0.178044,1.75318,-1.60953,0.135214,1.63173,-1.54269,0.261696,1.61406,-1.64169,0.344352,1.59337,-1.68152,0.174961,1.92214,-2.09641,0.27427,1.85163,-1.95249,0.065264,1.8976,-1.98924,-0.061181,1.80134,-2.11824,-0.086238,1.70729,-2.17602,0.052707,1.56899,-2.15217,0.082621,1.51009,-2.17448,0.178626,1.40805,-1.78258,-0.008475,1.48609,-1.85531,-0.008127,1.51623,-1.90122,-0.008488,1.49168,-1.971,0.174744,1.44019,-2.03667,0.134186,1.08234 [...]
+/*748*/{0.236127,1.90236,-1.71885,0.280567,1.84898,-1.8792,0.175911,1.74891,-1.60947,0.130711,1.62802,-1.54314,0.258219,1.6064,-1.64061,0.340296,1.58397,-1.67987,0.174318,1.91803,-2.09723,0.273872,1.84831,-1.95237,0.06415,1.89237,-1.98988,-0.061095,1.79378,-2.1196,-0.084529,1.69866,-2.1772,0.056868,1.56191,-2.15349,0.086042,1.50353,-2.17581,0.179036,1.40523,-1.78299,-0.008677,1.4817,-1.85476,-0.008135,1.51176,-1.9012,-0.008947,1.48612,-1.96992,0.174802,1.43772,-2.03749,0.139524,1.07788,- [...]
+/*749*/{0.234138,1.89754,-1.7176,0.279557,1.84428,-1.87884,0.173017,1.74424,-1.60954,0.126846,1.62435,-1.54377,0.254652,1.59892,-1.64027,0.336615,1.57372,-1.67772,0.174036,1.91352,-2.09766,0.273408,1.84498,-1.9528,0.06247,1.88777,-1.99089,-0.059688,1.78551,-2.1212,-0.082481,1.69053,-2.1791,0.060105,1.55463,-2.15516,0.090565,1.49651,-2.1769,0.180763,1.402,-1.78388,-0.009097,1.47729,-1.85499,-0.008212,1.5071,-1.90102,-0.009819,1.48199,-1.96984,0.174538,1.43489,-2.03815,0.141825,1.07587,-1. [...]
+/*750*/{0.233036,1.8939,-1.71721,0.280186,1.83961,-1.8777,0.169878,1.73946,-1.60925,0.121586,1.62037,-1.54356,0.250298,1.59104,-1.63936,0.331917,1.56366,-1.676,0.172584,1.90854,-2.09785,0.27385,1.84124,-1.95185,0.061881,1.88236,-1.99264,-0.06073,1.7769,-2.12362,-0.080522,1.68196,-2.18098,0.063243,1.54711,-2.15647,0.094792,1.48975,-2.17782,0.180594,1.39863,-1.78469,-0.008272,1.47254,-1.85368,-0.007536,1.5027,-1.90019,-0.010977,1.47695,-1.96838,0.174406,1.43186,-2.03907,0.147653,1.07284,-1 [...]
+/*751*/{0.23128,1.88865,-1.71579,0.280092,1.83565,-1.87715,0.166788,1.73465,-1.60923,0.115933,1.61432,-1.54343,0.246358,1.58363,-1.63847,0.327203,1.55391,-1.67382,0.172267,1.90359,-2.09832,0.27404,1.83768,-1.95127,0.060154,1.87613,-1.99307,-0.059988,1.76726,-2.12695,-0.078177,1.67297,-2.18273,0.06768,1.53937,-2.15822,0.099767,1.48212,-2.17861,0.180914,1.39522,-1.78572,-0.008747,1.46722,-1.85329,-0.00964,1.49803,-1.90074,-0.010657,1.47185,-1.96806,0.174361,1.42865,-2.03976,0.151986,1.0686 [...]
+/*752*/{0.229535,1.88419,-1.71509,0.279444,1.83057,-1.87573,0.163817,1.72946,-1.60937,0.112411,1.61166,-1.54366,0.242369,1.57595,-1.63702,0.322533,1.54402,-1.67163,0.172651,1.89837,-2.09904,0.273893,1.83352,-1.95148,0.059447,1.87034,-1.99388,-0.059833,1.75756,-2.12939,-0.075332,1.66391,-2.18529,0.073042,1.53226,-2.15934,0.105196,1.47452,-2.1796,0.181808,1.39123,-1.78695,-0.008284,1.46166,-1.85292,-0.008359,1.49249,-1.90015,-0.011991,1.4668,-1.96697,0.174047,1.42561,-2.04075,0.155195,1.06 [...]
+/*753*/{0.227715,1.87916,-1.71403,0.280056,1.82547,-1.87542,0.161066,1.72467,-1.60947,0.106982,1.60813,-1.54492,0.237364,1.56834,-1.63598,0.317731,1.53564,-1.67068,0.173425,1.89315,-2.10028,0.273898,1.82849,-1.95024,0.05885,1.86379,-1.99531,-0.05846,1.74746,-2.13087,-0.071967,1.65428,-2.18731,0.076811,1.52404,-2.16057,0.110421,1.46762,-2.18063,0.181663,1.3863,-1.78726,-0.00881,1.45582,-1.8515,-0.010105,1.48713,-1.89967,-0.013132,1.46179,-1.96613,0.17352,1.42201,-2.04224,0.157223,1.06231, [...]
+/*754*/{0.226262,1.87449,-1.71296,0.280959,1.82054,-1.87379,0.158213,1.72002,-1.60889,0.103445,1.60381,-1.54416,0.232997,1.56097,-1.63466,0.311379,1.52679,-1.66756,0.17321,1.88806,-2.1007,0.274652,1.82366,-1.95018,0.057164,1.85778,-1.99577,-0.056461,1.73783,-2.13402,-0.068731,1.64439,-2.1894,0.082571,1.51624,-2.16227,0.116782,1.46025,-2.18157,0.181087,1.38203,-1.7883,-0.010318,1.45115,-1.85114,-0.011253,1.48155,-1.89905,-0.014743,1.45715,-1.96528,0.171919,1.41776,-2.0436,0.161147,1.05945 [...]
+/*755*/{0.225661,1.86987,-1.71168,0.280935,1.81482,-1.87276,0.155003,1.71507,-1.60925,0.098302,1.60103,-1.54555,0.22743,1.55377,-1.63374,0.305729,1.51906,-1.66586,0.17427,1.88273,-2.10151,0.274493,1.81827,-1.94958,0.056665,1.85116,-1.99693,-0.054377,1.72801,-2.13762,-0.064066,1.63488,-2.1919,0.088436,1.50949,-2.16403,0.123715,1.45273,-2.18265,0.182051,1.37706,-1.79047,-0.012575,1.44575,-1.84997,-0.012501,1.4765,-1.89839,-0.01711,1.45295,-1.96537,0.170621,1.413,-2.04518,0.164612,1.05475,- [...]
+/*756*/{0.223703,1.86528,-1.71111,0.28122,1.81025,-1.87192,0.15118,1.71037,-1.6092,0.09282,1.59779,-1.54615,0.221786,1.54801,-1.63178,0.299718,1.51173,-1.66329,0.175244,1.87784,-2.10268,0.276215,1.81332,-1.94922,0.056467,1.84699,-1.99753,-0.051941,1.71799,-2.14154,-0.05923,1.62438,-2.19405,0.094561,1.50163,-2.16536,0.130456,1.44562,-2.18271,0.181609,1.37197,-1.79146,-0.012832,1.44083,-1.84953,-0.013753,1.47009,-1.89718,-0.020455,1.4499,-1.96477,0.167486,1.40845,-2.04665,0.170372,1.05221, [...]
+/*757*/{0.222752,1.86085,-1.70985,0.282619,1.80486,-1.87078,0.148961,1.7058,-1.60937,0.086435,1.59509,-1.54607,0.216349,1.54305,-1.63101,0.292505,1.50517,-1.66098,0.175733,1.87317,-2.10422,0.276321,1.80791,-1.94793,0.056673,1.84416,-1.99707,-0.046712,1.70785,-2.14609,-0.053131,1.61417,-2.196,0.101563,1.49384,-2.16651,0.138141,1.43929,-2.18356,0.180438,1.36718,-1.79383,-0.01429,1.43578,-1.84907,-0.016099,1.46515,-1.89605,-0.023052,1.44585,-1.96402,0.165369,1.40682,-2.04807,0.174629,1.0489 [...]
+/*758*/{0.222055,1.8568,-1.70838,0.282247,1.80069,-1.86971,0.146247,1.7023,-1.60914,0.07947,1.5927,-1.54634,0.209792,1.53857,-1.62873,0.286148,1.4999,-1.65885,0.176945,1.8688,-2.10485,0.277279,1.80376,-1.94759,0.057117,1.84281,-1.99671,-0.040239,1.69806,-2.15033,-0.046472,1.60434,-2.1988,0.109149,1.48723,-2.1669,0.146212,1.43293,-2.18355,0.180017,1.36292,-1.79686,-0.017518,1.43218,-1.84812,-0.019074,1.46067,-1.89544,-0.0265,1.44276,-1.96386,0.163557,1.40493,-2.04945,0.17828,1.04724,-1.73 [...]
+/*759*/{0.222119,1.85314,-1.70697,0.283606,1.79777,-1.86862,0.142105,1.69961,-1.60921,0.072107,1.59186,-1.54674,0.203302,1.5352,-1.6278,0.279439,1.49477,-1.65601,0.177688,1.86501,-2.10595,0.278779,1.79963,-1.94604,0.057435,1.84066,-1.99605,-0.034677,1.6881,-2.15641,-0.038443,1.59444,-2.20091,0.117564,1.48155,-2.16723,0.15539,1.42804,-2.18367,0.178302,1.35861,-1.79787,-0.019088,1.42751,-1.84711,-0.021503,1.45845,-1.89603,-0.02918,1.43945,-1.96209,0.161001,1.40269,-2.05107,0.182951,1.04769 [...]
+/*760*/{0.222891,1.84946,-1.70559,0.282261,1.79426,-1.86693,0.138025,1.69813,-1.60915,0.065113,1.5913,-1.54707,0.196563,1.53273,-1.62632,0.271519,1.49198,-1.6535,0.179402,1.86205,-2.1062,0.279005,1.79624,-1.94564,0.057677,1.83909,-1.99489,-0.030267,1.67969,-2.16028,-0.030239,1.58528,-2.2025,0.126484,1.4764,-2.16767,0.165287,1.42278,-2.1832,0.177081,1.35482,-1.80008,-0.021507,1.42361,-1.84529,-0.021612,1.45678,-1.89445,-0.03129,1.43693,-1.95941,0.158607,1.4021,-2.05256,0.189056,1.04996,-1 [...]
+/*761*/{0.222792,1.84575,-1.70341,0.282855,1.78967,-1.86613,0.133247,1.6974,-1.6089,0.05782,1.59203,-1.54843,0.189394,1.53178,-1.62491,0.264713,1.4884,-1.65119,0.180547,1.85943,-2.10529,0.279029,1.79323,-1.94401,0.058079,1.83753,-1.99497,-0.024731,1.67296,-2.16382,-0.020503,1.57717,-2.20483,0.136762,1.47231,-2.16693,0.176757,1.42049,-2.18221,0.174069,1.35191,-1.8006,-0.022877,1.42092,-1.84393,-0.023656,1.45646,-1.89382,-0.033394,1.43566,-1.95785,0.156513,1.40172,-2.05389,0.195057,1.05285 [...]
+/*762*/{0.223948,1.84357,-1.7031,0.283625,1.7879,-1.86519,0.128457,1.69741,-1.6088,0.051158,1.59426,-1.5492,0.183095,1.53121,-1.62376,0.257562,1.48646,-1.64948,0.181603,1.85736,-2.10407,0.279238,1.79068,-1.94375,0.058498,1.83634,-1.99455,-0.018483,1.6664,-2.1656,-0.011558,1.57111,-2.20578,0.146811,1.46923,-2.16629,0.188062,1.41775,-2.1812,0.172782,1.3492,-1.80286,-0.025105,1.41976,-1.84229,-0.025193,1.45593,-1.89427,-0.033841,1.43456,-1.9547,0.154936,1.4011,-2.05526,0.203252,1.05553,-1.7 [...]
+/*763*/{0.224504,1.8416,-1.70238,0.283483,1.78643,-1.86453,0.125146,1.69842,-1.60877,0.044125,1.59684,-1.54954,0.17538,1.5314,-1.62369,0.251147,1.48588,-1.64662,0.183496,1.85642,-2.1029,0.278948,1.78838,-1.9423,0.05917,1.83504,-1.99428,-0.011679,1.66189,-2.16653,-0.003664,1.56665,-2.20585,0.157922,1.46767,-2.16499,0.199708,1.4175,-2.17999,0.170931,1.34628,-1.80237,-0.025723,1.41925,-1.84226,-0.026629,1.45521,-1.89333,-0.035843,1.43406,-1.9537,0.152997,1.40072,-2.05641,0.209603,1.05877,-1 [...]
+/*764*/{0.224891,1.84008,-1.70193,0.283047,1.7843,-1.86322,0.120291,1.7002,-1.60865,0.03768,1.60117,-1.55129,0.168538,1.53288,-1.62255,0.243791,1.48611,-1.64528,0.185039,1.85543,-2.10231,0.279699,1.78708,-1.94108,0.060037,1.83468,-1.99435,-0.006488,1.65791,-2.16632,0.004143,1.5627,-2.20657,0.168443,1.46751,-2.16397,0.212036,1.41925,-2.17808,0.169155,1.34446,-1.801,-0.027518,1.41884,-1.84198,-0.027693,1.455,-1.89354,-0.036204,1.43419,-1.95263,0.151311,1.4003,-2.05693,0.216787,1.06176,-1.7 [...]
+/*765*/{0.224129,1.83918,-1.70108,0.281646,1.78358,-1.86196,0.115317,1.70225,-1.60862,0.030615,1.60575,-1.55279,0.16206,1.53459,-1.62135,0.235723,1.48715,-1.6436,0.187326,1.85449,-2.10143,0.278956,1.78603,-1.93943,0.060315,1.83454,-1.99468,-0.001049,1.65537,-2.16599,0.01259,1.55966,-2.20662,0.178574,1.46802,-2.16202,0.223073,1.4209,-2.17594,0.168053,1.34314,-1.80046,-0.028253,1.42014,-1.84052,-0.029137,1.45495,-1.89315,-0.036783,1.4348,-1.95233,0.150827,1.40021,-2.05705,0.223363,1.06867, [...]
+/*766*/{0.223707,1.83961,-1.70022,0.281639,1.78297,-1.86079,0.111304,1.70506,-1.60914,0.02481,1.61124,-1.55421,0.154843,1.53736,-1.62002,0.228586,1.48871,-1.64198,0.189398,1.85429,-2.10032,0.279333,1.78577,-1.93794,0.061273,1.83487,-1.99476,0.003667,1.65339,-2.1655,0.020799,1.55714,-2.20731,0.188964,1.46919,-2.16008,0.235147,1.42493,-2.1737,0.165727,1.34268,-1.79879,-0.028301,1.42021,-1.84031,-0.029518,1.45504,-1.89264,-0.037237,1.43546,-1.95277,0.152086,1.40003,-2.05701,0.231649,1.07377 [...]
+/*767*/{0.222327,1.84038,-1.70011,0.279721,1.78256,-1.85981,0.106698,1.70833,-1.60872,0.01698,1.61566,-1.55539,0.14849,1.54029,-1.61974,0.221748,1.49128,-1.64015,0.191931,1.85517,-2.09977,0.279281,1.78566,-1.93692,0.062097,1.83464,-1.99506,0.008523,1.65122,-2.16453,0.028796,1.55583,-2.20748,0.199563,1.47408,-2.15838,0.246721,1.43021,-2.17105,0.165408,1.34356,-1.79778,-0.029445,1.42069,-1.84169,-0.029343,1.45525,-1.89336,-0.037484,1.43609,-1.95317,0.151529,1.3992,-2.05733,0.238565,1.07905 [...]
+/*768*/{0.220107,1.84177,-1.69924,0.280331,1.78292,-1.85834,0.101734,1.71254,-1.60918,0.010959,1.62117,-1.55626,0.142316,1.54408,-1.61911,0.214814,1.49434,-1.63867,0.193872,1.85664,-2.09935,0.279222,1.78575,-1.93542,0.063031,1.83426,-1.99593,0.012733,1.64959,-2.16395,0.036752,1.55515,-2.20719,0.209731,1.47816,-2.15557,0.258291,1.43636,-2.1679,0.163609,1.34495,-1.79552,-0.02941,1.4215,-1.84222,-0.029834,1.45565,-1.89364,-0.03762,1.43654,-1.95482,0.149417,1.39811,-2.05817,0.246116,1.08582, [...]
+/*769*/{0.218539,1.84414,-1.69816,0.279644,1.78348,-1.85639,0.097009,1.71683,-1.60922,0.00347,1.62575,-1.55806,0.135677,1.54827,-1.61775,0.207657,1.49805,-1.6374,0.196397,1.85848,-2.09813,0.279016,1.78698,-1.93366,0.063382,1.83648,-1.9974,0.018243,1.64943,-2.16309,0.044124,1.55501,-2.20744,0.218596,1.48329,-2.15271,0.268955,1.4447,-2.16495,0.163076,1.34677,-1.79465,-0.030279,1.42239,-1.84275,-0.029553,1.4567,-1.8946,-0.037511,1.43755,-1.95497,0.150575,1.39819,-2.05726,0.252288,1.09417,-1 [...]
+/*770*/{0.215682,1.84709,-1.69737,0.278892,1.78543,-1.85485,0.091925,1.7213,-1.60937,-0.002383,1.63166,-1.55856,0.129238,1.55399,-1.61752,0.200504,1.5027,-1.63589,0.19748,1.8607,-2.0971,0.279058,1.78905,-1.93187,0.064593,1.83731,-1.99763,0.023922,1.64865,-2.16265,0.05034,1.55557,-2.20728,0.227887,1.48988,-2.15017,0.279481,1.45249,-2.16145,0.161556,1.34906,-1.79273,-0.030476,1.42356,-1.84454,-0.029545,1.45807,-1.89616,-0.03713,1.43919,-1.95582,0.151092,1.39817,-2.05736,0.258666,1.10076,-1 [...]
+/*771*/{0.212882,1.84913,-1.69637,0.277949,1.78737,-1.8526,0.087151,1.72603,-1.60922,-0.009071,1.6366,-1.55998,0.123687,1.5585,-1.61681,0.193776,1.50762,-1.63514,0.199677,1.86418,-2.09608,0.27926,1.79053,-1.92986,0.065299,1.83827,-1.99805,0.027678,1.64838,-2.16107,0.05771,1.55707,-2.20792,0.236339,1.49724,-2.14745,0.289042,1.46183,-2.1575,0.160879,1.35109,-1.79171,-0.030839,1.42539,-1.84546,-0.029804,1.4598,-1.89665,-0.036827,1.44071,-1.95759,0.151334,1.39804,-2.05735,0.264413,1.10879,-1 [...]
+/*772*/{0.210546,1.85238,-1.6952,0.277601,1.79007,-1.85043,0.081832,1.73064,-1.60939,-0.016324,1.64224,-1.56108,0.116957,1.56464,-1.61698,0.188022,1.5134,-1.63391,0.201095,1.86728,-2.09508,0.279808,1.79315,-1.92885,0.065985,1.83949,-1.9984,0.03229,1.64832,-2.1595,0.064176,1.55923,-2.20805,0.244119,1.50594,-2.1449,0.298756,1.47224,-2.15374,0.161387,1.3539,-1.79113,-0.030937,1.42739,-1.84611,-0.029378,1.46127,-1.89695,-0.035453,1.44251,-1.95849,0.151996,1.39791,-2.057,0.269687,1.11686,-1.7 [...]
+/*773*/{0.20714,1.8556,-1.69452,0.277218,1.79367,-1.84897,0.077292,1.73562,-1.60962,-0.022373,1.64834,-1.56231,0.110775,1.5708,-1.61679,0.181574,1.51885,-1.6332,0.202165,1.87107,-2.09386,0.280611,1.79678,-1.92633,0.066989,1.84142,-1.99952,0.036749,1.65033,-2.15831,0.070713,1.56172,-2.20857,0.251453,1.51371,-2.1418,0.307296,1.48252,-2.1499,0.161531,1.35693,-1.78972,-0.02962,1.42973,-1.84733,-0.027783,1.46379,-1.89902,-0.035249,1.44426,-1.95984,0.153908,1.39768,-2.05607,0.274489,1.12404,-1 [...]
+/*774*/{0.203752,1.85978,-1.69419,0.276672,1.7966,-1.84653,0.072579,1.74048,-1.60982,-0.027411,1.65473,-1.56327,0.104376,1.5765,-1.6172,0.176021,1.52508,-1.63225,0.203752,1.87495,-2.09269,0.280989,1.79974,-1.92413,0.068245,1.84333,-2.00094,0.041544,1.65199,-2.15826,0.077526,1.56439,-2.20899,0.259515,1.52282,-2.139,0.316239,1.49347,-2.14592,0.160803,1.35972,-1.78883,-0.02918,1.43273,-1.84889,-0.026657,1.4657,-1.9001,-0.033193,1.44694,-1.96141,0.15516,1.39786,-2.05574,0.28012,1.13105,-1.72 [...]
+/*775*/{0.200875,1.86337,-1.69362,0.275905,1.80072,-1.84445,0.068289,1.7457,-1.60969,-0.032972,1.66111,-1.56474,0.099171,1.58247,-1.61744,0.169961,1.53125,-1.63159,0.206178,1.87969,-2.09166,0.281607,1.80391,-1.92262,0.068808,1.84597,-2.00101,0.045463,1.65442,-2.15827,0.084224,1.56832,-2.20954,0.265684,1.53133,-2.13557,0.323776,1.50487,-2.14153,0.161865,1.36362,-1.78837,-0.028985,1.43581,-1.85055,-0.025436,1.46815,-1.90068,-0.031764,1.45008,-1.96264,0.155815,1.3978,-2.05574,0.283536,1.138 [...]
+/*776*/{0.197365,1.86762,-1.69305,0.276049,1.80484,-1.84304,0.064339,1.75108,-1.61069,-0.039012,1.66674,-1.56575,0.093896,1.58968,-1.61744,0.164083,1.53761,-1.63057,0.207411,1.88396,-2.09,0.282382,1.80764,-1.9204,0.07008,1.84884,-2.00193,0.049724,1.65676,-2.15904,0.091321,1.57207,-2.21057,0.272399,1.54122,-2.13222,0.330729,1.51611,-2.13727,0.162212,1.36733,-1.78792,-0.028296,1.43958,-1.85069,-0.024092,1.47105,-1.90223,-0.029219,1.45233,-1.96366,0.157726,1.39768,-2.05528,0.28647,1.14653,- [...]
+/*777*/{0.194315,1.87215,-1.69216,0.276441,1.80755,-1.84095,0.060188,1.75638,-1.61102,-0.045008,1.67364,-1.56792,0.088966,1.59621,-1.61809,0.159394,1.54397,-1.63012,0.209539,1.88868,-2.08874,0.282655,1.81193,-1.91835,0.071316,1.85117,-2.00245,0.056187,1.66063,-2.16077,0.097428,1.57603,-2.21123,0.277357,1.55058,-2.12864,0.337493,1.52808,-2.13224,0.163116,1.37184,-1.78797,-0.025835,1.44318,-1.85253,-0.022675,1.47389,-1.90388,-0.027101,1.45494,-1.96466,0.158977,1.39733,-2.05512,0.289173,1.1 [...]
+/*778*/{0.191379,1.87651,-1.69203,0.275601,1.8121,-1.83884,0.055968,1.76184,-1.6114,-0.049186,1.68041,-1.56943,0.083469,1.60253,-1.61784,0.154301,1.54973,-1.62895,0.212324,1.89371,-2.08742,0.282881,1.81587,-1.9166,0.072942,1.85472,-2.00363,0.059938,1.66473,-2.16363,0.103686,1.581,-2.21278,0.28304,1.56144,-2.12507,0.342916,1.54007,-2.12799,0.165394,1.37621,-1.78797,-0.024902,1.44748,-1.85329,-0.020888,1.47679,-1.90558,-0.024556,1.45832,-1.96624,0.161064,1.39734,-2.05497,0.293284,1.16173,- [...]
+/*779*/{0.188462,1.88073,-1.69194,0.274913,1.81814,-1.83751,0.052538,1.76777,-1.61243,-0.055052,1.68663,-1.57076,0.078341,1.60896,-1.61912,0.149198,1.55635,-1.62826,0.213544,1.89842,-2.08613,0.283374,1.82093,-1.91475,0.073919,1.8581,-2.00482,0.064944,1.66812,-2.1656,0.110357,1.58535,-2.2132,0.28683,1.57069,-2.12167,0.348737,1.55198,-2.12334,0.166703,1.38039,-1.78825,-0.022715,1.45172,-1.8545,-0.018332,1.48061,-1.90696,-0.022162,1.46172,-1.96752,0.163414,1.39735,-2.05477,0.296403,1.16867, [...]
+/*780*/{0.186325,1.88507,-1.69175,0.27416,1.82242,-1.83524,0.049562,1.77326,-1.61275,-0.058836,1.69328,-1.57314,0.074474,1.61563,-1.61897,0.14531,1.56224,-1.62858,0.216121,1.90328,-2.08421,0.284564,1.82547,-1.91281,0.07616,1.86204,-2.00507,0.070307,1.67482,-2.16972,0.116806,1.59032,-2.2149,0.290949,1.58009,-2.11828,0.352972,1.56374,-2.11898,0.16918,1.38455,-1.78841,-0.020331,1.45605,-1.85416,-0.016272,1.48427,-1.90839,-0.018751,1.46449,-1.96844,0.16515,1.39756,-2.05437,0.298848,1.17693,- [...]
+/*781*/{0.183992,1.88934,-1.69179,0.275446,1.82688,-1.83304,0.046219,1.77859,-1.61358,-0.064398,1.70053,-1.5753,0.069391,1.62172,-1.61965,0.140471,1.56851,-1.62774,0.218731,1.90856,-2.08299,0.285804,1.82968,-1.91052,0.076965,1.86542,-2.00596,0.075573,1.68161,-2.17255,0.123473,1.59563,-2.21603,0.293988,1.5898,-2.11491,0.356434,1.57522,-2.11497,0.170469,1.38901,-1.78904,-0.019822,1.46135,-1.85537,-0.014838,1.48782,-1.91048,-0.016028,1.46772,-1.96889,0.167262,1.39751,-2.05416,0.301994,1.183 [...]
+/*782*/{0.181928,1.89378,-1.69157,0.274115,1.83063,-1.83211,0.042715,1.78431,-1.61486,-0.066808,1.70673,-1.57672,0.065612,1.62752,-1.61997,0.136851,1.57465,-1.62711,0.220633,1.913,-2.0811,0.286766,1.83414,-1.90845,0.078939,1.86922,-2.00642,0.081158,1.68448,-2.17545,0.129716,1.60092,-2.21736,0.296943,1.59936,-2.11127,0.359525,1.58584,-2.1111,0.171458,1.39279,-1.78906,-0.01622,1.46639,-1.85571,-0.012493,1.49223,-1.91199,-0.012818,1.47121,-1.9696,0.169178,1.39725,-2.0535,0.303645,1.19026,-1 [...]
+/*783*/{0.180335,1.89753,-1.69139,0.273533,1.837,-1.8309,0.039232,1.78989,-1.61561,-0.072139,1.71253,-1.57936,0.06246,1.63368,-1.62062,0.132148,1.57942,-1.62593,0.222569,1.91754,-2.0798,0.286368,1.83862,-1.90717,0.080471,1.87259,-2.00745,0.085645,1.68962,-2.17923,0.135351,1.60635,-2.21904,0.30023,1.60912,-2.10823,0.362508,1.59619,-2.1074,0.172586,1.39687,-1.78918,-0.014663,1.47173,-1.85528,-0.009838,1.49581,-1.91348,-0.010038,1.47459,-1.96988,0.171491,1.39679,-2.05264,0.305539,1.19738,-1 [...]
+/*784*/{0.178802,1.90144,-1.69149,0.273509,1.8423,-1.82958,0.037078,1.79542,-1.61736,-0.075192,1.71902,-1.58165,0.058998,1.63873,-1.62101,0.128828,1.58451,-1.62578,0.225491,1.92239,-2.07863,0.287255,1.84275,-1.90507,0.082497,1.87579,-2.00844,0.091272,1.69451,-2.18307,0.141847,1.61111,-2.22047,0.302042,1.61798,-2.10517,0.365229,1.6063,-2.10345,0.175779,1.40172,-1.79001,-0.012422,1.4768,-1.85514,-0.007579,1.49961,-1.91532,-0.006627,1.47812,-1.96992,0.172957,1.397,-2.05159,0.307868,1.20334, [...]
+/*785*/{0.177657,1.90534,-1.6911,0.273118,1.8464,-1.82841,0.034251,1.8013,-1.61889,-0.079183,1.72483,-1.584,0.055347,1.64436,-1.62179,0.126699,1.58992,-1.62599,0.226638,1.92667,-2.07688,0.286974,1.84663,-1.90399,0.083885,1.87875,-2.00907,0.096278,1.69983,-2.18622,0.146916,1.61623,-2.22182,0.303707,1.62646,-2.10209,0.367148,1.61628,-2.10001,0.17816,1.40623,-1.79036,-0.010314,1.48195,-1.85636,-0.005426,1.50355,-1.91566,-0.004346,1.48152,-1.97019,0.175613,1.39707,-2.05044,0.309293,1.20917,- [...]
+/*786*/{0.176123,1.90857,-1.69154,0.273548,1.84966,-1.8272,0.031751,1.80612,-1.62004,-0.082144,1.72967,-1.58673,0.052801,1.64947,-1.62233,0.123622,1.59475,-1.62522,0.228173,1.93056,-2.07578,0.287791,1.8509,-1.90232,0.085373,1.88257,-2.00966,0.103901,1.70561,-2.18861,0.152185,1.62116,-2.22317,0.305106,1.63418,-2.09969,0.369146,1.62552,-2.09668,0.178605,1.41018,-1.79019,-0.0082,1.48718,-1.8549,-0.003302,1.50782,-1.91673,-0.001977,1.48492,-1.97041,0.177567,1.39793,-2.04948,0.311391,1.21463, [...]
+/*787*/{0.175089,1.9123,-1.69161,0.273966,1.85482,-1.82658,0.029804,1.81107,-1.62195,-0.08458,1.73501,-1.58957,0.049865,1.65331,-1.62242,0.120638,1.59891,-1.6247,0.2301,1.93427,-2.07435,0.288169,1.85466,-1.90092,0.087053,1.88528,-2.01011,0.108205,1.7103,-2.19136,0.157215,1.6263,-2.22506,0.306327,1.64149,-2.09807,0.369555,1.63346,-2.09339,0.180124,1.41424,-1.79005,-0.005332,1.49076,-1.85609,-0.001096,1.51091,-1.91813,0.001483,1.48771,-1.97089,0.179934,1.39857,-2.04814,0.3133,1.21941,-1.72 [...]
+/*788*/{0.174395,1.91521,-1.69147,0.27341,1.85712,-1.82546,0.027977,1.81614,-1.62374,-0.087844,1.73975,-1.59248,0.047812,1.65785,-1.62272,0.11791,1.60274,-1.62423,0.232005,1.9381,-2.07314,0.288458,1.85797,-1.89971,0.088657,1.88765,-2.0103,0.112637,1.71441,-2.19321,0.161246,1.63091,-2.22655,0.307523,1.64779,-2.09665,0.370536,1.64166,-2.09025,0.181347,1.41967,-1.78975,-0.00311,1.49622,-1.85503,0.001379,1.51489,-1.91835,0.003684,1.49118,-1.97027,0.181612,1.39967,-2.04746,0.31409,1.22321,-1. [...]
+/*789*/{0.17354,1.91795,-1.69168,0.273433,1.86019,-1.82444,0.026002,1.82085,-1.62501,-0.089786,1.74411,-1.59446,0.045761,1.66217,-1.62416,0.115832,1.60588,-1.62416,0.233497,1.94145,-2.07165,0.289057,1.86156,-1.89852,0.090541,1.89073,-2.01182,0.116911,1.71893,-2.19551,0.16605,1.63542,-2.22871,0.308118,1.6539,-2.09512,0.371627,1.64764,-2.08745,0.181565,1.4229,-1.78939,-0.000994,1.50019,-1.85547,0.004106,1.51794,-1.91916,0.005899,1.49433,-1.97048,0.182942,1.40107,-2.04578,0.31577,1.22675,-1 [...]
+/*790*/{0.17283,1.92045,-1.69207,0.266698,1.86597,-1.82589,0.024618,1.82472,-1.62657,-0.093011,1.74778,-1.59674,0.04454,1.66564,-1.62407,0.115481,1.6097,-1.62233,0.235205,1.94453,-2.07055,0.289231,1.86521,-1.898,0.09139,1.89379,-2.01247,0.121405,1.72301,-2.19708,0.16939,1.63916,-2.23033,0.308971,1.65928,-2.09371,0.372671,1.65388,-2.08546,0.183838,1.42708,-1.78792,0.001087,1.50424,-1.85481,0.006399,1.52149,-1.91917,0.008446,1.4969,-1.97029,0.185012,1.40245,-2.04494,0.315516,1.22914,-1.729 [...]
+/*791*/{0.172181,1.92262,-1.69204,0.267475,1.86889,-1.82462,0.023247,1.82849,-1.62755,-0.093808,1.75121,-1.59874,0.042702,1.66843,-1.62381,0.113543,1.61181,-1.62073,0.236813,1.94719,-2.06978,0.289839,1.86829,-1.89694,0.093425,1.89596,-2.01239,0.124612,1.72722,-2.19883,0.172267,1.64333,-2.23138,0.310325,1.6637,-2.09325,0.37326,1.658,-2.08105,0.182925,1.43034,-1.78687,0.002141,1.5076,-1.85415,0.008035,1.52466,-1.91903,0.010895,1.49978,-1.96983,0.186802,1.40442,-2.04365,0.316162,1.23126,-1. [...]
+/*792*/{0.172212,1.92489,-1.6922,0.273837,1.8672,-1.82205,0.022226,1.83153,-1.6301,-0.097215,1.7536,-1.60111,0.040887,1.67118,-1.62483,0.112689,1.61443,-1.62027,0.238736,1.94992,-2.06877,0.291032,1.86988,-1.89555,0.094738,1.89886,-2.01307,0.128642,1.73074,-2.20039,0.176291,1.64637,-2.23342,0.311834,1.66751,-2.09291,0.373736,1.66324,-2.08146,0.186168,1.43361,-1.78683,0.004036,1.51152,-1.85377,0.009673,1.5274,-1.91845,0.01159,1.50292,-1.96983,0.187679,1.40625,-2.04311,0.31808,1.23289,-1.73 [...]
+/*793*/{0.171979,1.92665,-1.69202,0.275606,1.86934,-1.82099,0.021223,1.83453,-1.63139,-0.097638,1.75602,-1.60291,0.039726,1.67342,-1.62486,0.111796,1.61639,-1.61955,0.239657,1.95242,-2.06755,0.291749,1.87248,-1.89457,0.096098,1.90058,-2.01307,0.133019,1.73418,-2.20111,0.179071,1.64937,-2.23486,0.311849,1.66965,-2.09236,0.373843,1.66685,-2.08018,0.186245,1.43683,-1.7864,0.00579,1.51454,-1.85354,0.010779,1.53048,-1.91822,0.014253,1.50571,-1.96897,0.188815,1.40872,-2.04237,0.315054,1.23295, [...]
+/*794*/{0.172093,1.92807,-1.69181,0.275814,1.87134,-1.82055,0.020543,1.83708,-1.63231,-0.098247,1.75841,-1.60445,0.039013,1.67482,-1.62491,0.1111,1.61803,-1.61816,0.240912,1.95424,-2.06693,0.292548,1.87459,-1.89401,0.096721,1.90334,-2.01289,0.131257,1.73764,-2.20277,0.181553,1.65209,-2.23601,0.312468,1.67209,-2.09151,0.374526,1.6686,-2.07793,0.186496,1.43954,-1.78601,0.006991,1.51701,-1.85296,0.010959,1.5343,-1.9189,0.015051,1.50895,-1.96851,0.189461,1.41079,-2.04163,0.314815,1.23317,-1. [...]
+/*795*/{0.173085,1.92964,-1.69102,0.276984,1.87295,-1.81965,0.02053,1.83862,-1.63326,-0.098631,1.7608,-1.60569,0.038762,1.67635,-1.62463,0.110791,1.61928,-1.61715,0.241525,1.95672,-2.0663,0.292605,1.8764,-1.89353,0.098607,1.90498,-2.01294,0.133322,1.74035,-2.20345,0.183702,1.65437,-2.23681,0.313605,1.67403,-2.0914,0.375591,1.67055,-2.07689,0.185729,1.44194,-1.78573,0.008184,1.51941,-1.8527,0.01229,1.53638,-1.91783,0.015935,1.51196,-1.9684,0.190315,1.41346,-2.04131,0.312795,1.23152,-1.732 [...]
+/*796*/{0.172886,1.93047,-1.69086,0.277976,1.87239,-1.81844,0.020159,1.84005,-1.63442,-0.099791,1.76129,-1.607,0.038247,1.67683,-1.6246,0.111216,1.61966,-1.61566,0.242443,1.95821,-2.06544,0.294147,1.87826,-1.8921,0.099248,1.9079,-2.0131,0.135439,1.74285,-2.2039,0.185172,1.65603,-2.23826,0.313832,1.67494,-2.09122,0.376022,1.67082,-2.07584,0.190057,1.44411,-1.78529,0.008514,1.52201,-1.85271,0.012648,1.53932,-1.91631,0.016203,1.51448,-1.96792,0.190707,1.41608,-2.04065,0.311886,1.2283,-1.730 [...]
+/*797*/{0.173285,1.93186,-1.69006,0.278166,1.87527,-1.81806,0.020695,1.84107,-1.63439,-0.100936,1.76049,-1.60805,0.038484,1.6773,-1.62455,0.111173,1.61963,-1.61446,0.243318,1.95974,-2.06451,0.294498,1.87996,-1.8917,0.1003,1.90936,-2.01268,0.136857,1.74509,-2.20435,0.185637,1.6582,-2.23887,0.31455,1.6751,-2.09057,0.376626,1.67057,-2.07535,0.190882,1.447,-1.78514,0.007924,1.5238,-1.8524,0.013688,1.54145,-1.91607,0.016476,1.51677,-1.96702,0.191873,1.41843,-2.04103,0.310035,1.22521,-1.73093, [...]
+/*798*/{0.17449,1.93237,-1.68955,0.279211,1.8761,-1.81793,0.021484,1.84105,-1.63423,-0.100588,1.76016,-1.60901,0.039763,1.67702,-1.6241,0.112504,1.61926,-1.61235,0.243685,1.96084,-2.06384,0.294788,1.88126,-1.8914,0.101171,1.91175,-2.01228,0.138511,1.74666,-2.20449,0.185473,1.65866,-2.23872,0.316446,1.67556,-2.09038,0.377121,1.6692,-2.07474,0.190179,1.4472,-1.78554,0.010064,1.5253,-1.85193,0.014074,1.54388,-1.91588,0.017416,1.51899,-1.96659,0.191844,1.4212,-2.04076,0.30662,1.22025,-1.7309 [...]
+/*799*/{0.175661,1.93295,-1.68865,0.280195,1.87667,-1.81705,0.021629,1.84061,-1.63425,-0.09931,1.75948,-1.60944,0.040229,1.67656,-1.62337,0.112939,1.61852,-1.61063,0.244284,1.96169,-2.06348,0.295844,1.88252,-1.89056,0.101362,1.91253,-2.01196,0.13809,1.74825,-2.20366,0.185943,1.65982,-2.239,0.317546,1.67495,-2.09038,0.377022,1.6666,-2.07462,0.19239,1.44924,-1.78558,0.010954,1.52545,-1.85085,0.014498,1.54437,-1.9148,0.017964,1.52,-1.96532,0.19267,1.42371,-2.04055,0.303652,1.21549,-1.72945, [...]
+/*800*/{0.176472,1.93314,-1.68789,0.280004,1.87716,-1.8166,0.02276,1.84005,-1.63425,-0.099212,1.7583,-1.60923,0.042145,1.67561,-1.62264,0.113921,1.61734,-1.60851,0.244829,1.96195,-2.06308,0.296255,1.8841,-1.89086,0.102134,1.91382,-2.01066,0.137893,1.74888,-2.20284,0.184717,1.6604,-2.2387,0.317019,1.67222,-2.09055,0.377708,1.66435,-2.07475,0.192503,1.45014,-1.78523,0.010901,1.5262,-1.85098,0.013979,1.54588,-1.9146,0.018086,1.5216,-1.96481,0.193169,1.42586,-2.04067,0.298615,1.21063,-1.7297 [...]
+/*801*/{0.177626,1.93333,-1.68688,0.280024,1.87947,-1.8165,0.023848,1.83874,-1.6335,-0.096577,1.75717,-1.60904,0.042891,1.67344,-1.62086,0.115592,1.61559,-1.60651,0.245181,1.96222,-2.06262,0.296372,1.88447,-1.89046,0.101878,1.91485,-2.01007,0.136197,1.7499,-2.20123,0.183363,1.66072,-2.23839,0.316389,1.6697,-2.09011,0.377996,1.66114,-2.07523,0.194491,1.45105,-1.78567,0.011237,1.52623,-1.8499,0.013536,1.54717,-1.91392,0.017938,1.52259,-1.9643,0.193972,1.42773,-2.04056,0.295296,1.20447,-1.7 [...]
+/*802*/{0.178275,1.93308,-1.68564,0.28079,1.88035,-1.81642,0.024707,1.83697,-1.63244,-0.095472,1.75473,-1.60892,0.045064,1.6715,-1.61959,0.117798,1.61302,-1.60398,0.245233,1.96206,-2.0617,0.296211,1.88486,-1.88952,0.102597,1.91575,-2.00964,0.134033,1.75066,-2.19945,0.182335,1.66092,-2.23746,0.316002,1.66647,-2.0907,0.378153,1.65686,-2.07676,0.194472,1.45127,-1.78569,0.011394,1.52566,-1.84936,0.014028,1.54748,-1.91268,0.018516,1.52304,-1.96311,0.194477,1.42924,-2.04083,0.291962,1.19844,-1 [...]
+/*803*/{0.179062,1.93252,-1.68499,0.280906,1.88066,-1.81604,0.026303,1.83502,-1.63137,-0.092817,1.75309,-1.60818,0.047189,1.66806,-1.61802,0.119639,1.61064,-1.60188,0.245123,1.96135,-2.06072,0.296145,1.8848,-1.88917,0.102838,1.91608,-2.00866,0.13128,1.75126,-2.19752,0.17949,1.66113,-2.23601,0.318017,1.66452,-2.09206,0.378335,1.65218,-2.07734,0.194449,1.45113,-1.78626,0.011128,1.52527,-1.84907,0.013844,1.54718,-1.91273,0.019077,1.52216,-1.96298,0.195014,1.42985,-2.04096,0.287421,1.19177,- [...]
+/*804*/{0.180599,1.93199,-1.68395,0.280906,1.88066,-1.81604,0.027767,1.83264,-1.63007,-0.090524,1.74934,-1.60709,0.049531,1.66534,-1.61598,0.122219,1.60711,-1.59951,0.245236,1.9608,-2.05972,0.296039,1.88453,-1.88916,0.102304,1.91623,-2.00677,0.129464,1.75027,-2.19518,0.177097,1.66096,-2.23463,0.31714,1.66051,-2.09329,0.377437,1.64676,-2.07798,0.194438,1.45082,-1.78695,0.011088,1.5229,-1.84835,0.014528,1.54561,-1.91193,0.018723,1.52159,-1.96203,0.19545,1.43022,-2.04092,0.28413,1.18532,-1. [...]
+/*805*/{0.181753,1.93092,-1.68313,0.28122,1.87986,-1.81589,0.029197,1.82971,-1.62889,-0.087087,1.74554,-1.60503,0.053952,1.66251,-1.6147,0.125147,1.60353,-1.59729,0.245036,1.95957,-2.05945,0.295937,1.88316,-1.88858,0.102069,1.91575,-2.0063,0.1254,1.74994,-2.1931,0.174314,1.66104,-2.23291,0.315992,1.65615,-2.09417,0.376622,1.64127,-2.07979,0.196649,1.45019,-1.78769,0.011038,1.52166,-1.84827,0.013868,1.54474,-1.9125,0.018388,1.52036,-1.96149,0.196083,1.4303,-2.04116,0.279163,1.17851,-1.723 [...]
+/*806*/{0.182466,1.92959,-1.68223,0.282749,1.87771,-1.81524,0.03122,1.82666,-1.62715,-0.084564,1.74131,-1.6033,0.055641,1.65817,-1.61124,0.128618,1.6005,-1.59478,0.243975,1.95801,-2.05863,0.296114,1.8826,-1.88849,0.101763,1.91485,-2.00534,0.121403,1.75083,-2.19132,0.171368,1.6604,-2.23113,0.316151,1.65037,-2.09492,0.3758,1.63503,-2.08171,0.195576,1.44871,-1.78822,0.011712,1.51863,-1.84702,0.014089,1.54264,-1.91168,0.018279,1.51813,-1.96097,0.19674,1.43009,-2.04133,0.275925,1.17203,-1.721 [...]
+/*807*/{0.183638,1.92819,-1.68124,0.281922,1.87789,-1.81519,0.032833,1.8227,-1.62522,-0.080303,1.73655,-1.60097,0.059064,1.6542,-1.6083,0.132119,1.59638,-1.59216,0.243547,1.95648,-2.05745,0.295888,1.88148,-1.88831,0.1009,1.91425,-2.00339,0.117914,1.75031,-2.18904,0.167302,1.65995,-2.22972,0.315399,1.64592,-2.09652,0.376089,1.62697,-2.08296,0.198315,1.44737,-1.78774,0.011024,1.51669,-1.84718,0.014541,1.541,-1.91158,0.018831,1.51592,-1.96117,0.196896,1.42948,-2.04148,0.273426,1.16567,-1.71 [...]
+/*808*/{0.184275,1.92559,-1.68069,0.282171,1.87555,-1.81505,0.035396,1.81898,-1.62315,-0.077044,1.7323,-1.59836,0.062493,1.64923,-1.60556,0.136302,1.59218,-1.58941,0.243041,1.95471,-2.05725,0.295224,1.87886,-1.88791,0.100104,1.91285,-2.0028,0.114654,1.74744,-2.18582,0.163635,1.6588,-2.22832,0.313643,1.64059,-2.09797,0.372991,1.62106,-2.08671,0.196431,1.44504,-1.78847,0.01191,1.51371,-1.84651,0.013959,1.5374,-1.91171,0.01859,1.51286,-1.96099,0.198359,1.42769,-2.04146,0.268759,1.16089,-1.7 [...]
+/*809*/{0.185828,1.92336,-1.67945,0.282082,1.87189,-1.81506,0.037128,1.81422,-1.6211,-0.07271,1.72664,-1.59505,0.066491,1.6441,-1.60214,0.139778,1.58749,-1.58667,0.241184,1.95253,-2.05686,0.294217,1.87687,-1.88776,0.099228,1.91063,-2.00154,0.110792,1.74646,-2.18346,0.159323,1.65727,-2.22607,0.310758,1.63359,-2.09886,0.371194,1.61348,-2.08869,0.197679,1.4423,-1.78737,0.011267,1.51137,-1.84669,0.014233,1.53425,-1.91148,0.01904,1.50967,-1.96071,0.198225,1.42611,-2.04135,0.267403,1.15546,-1. [...]
+/*810*/{0.18664,1.92027,-1.67892,0.282905,1.86989,-1.81506,0.039343,1.80996,-1.6181,-0.069774,1.72082,-1.59176,0.070196,1.63822,-1.5988,0.14396,1.58356,-1.58374,0.240148,1.95017,-2.0565,0.294508,1.87476,-1.88722,0.098465,1.90819,-1.99938,0.106845,1.74488,-2.1815,0.155274,1.65532,-2.22453,0.308921,1.62715,-2.10038,0.36899,1.60512,-2.09169,0.198736,1.43916,-1.78718,0.011305,1.50753,-1.84611,0.014404,1.53098,-1.91119,0.018821,1.50688,-1.96165,0.199295,1.42404,-2.04121,0.264254,1.15068,-1.71 [...]
+/*811*/{0.18769,1.91726,-1.6778,0.282205,1.8665,-1.81439,0.041453,1.80541,-1.61548,-0.065426,1.71496,-1.58772,0.074377,1.63395,-1.59563,0.148103,1.57848,-1.58114,0.239161,1.9472,-2.05647,0.29457,1.87171,-1.88735,0.097217,1.90596,-1.99852,0.102336,1.74273,-2.17897,0.150485,1.65287,-2.22391,0.306093,1.62128,-2.10228,0.366449,1.59616,-2.09536,0.196724,1.43517,-1.78703,0.010325,1.50382,-1.84665,0.014612,1.52801,-1.91102,0.019099,1.50293,-1.96142,0.199992,1.42156,-2.04078,0.261613,1.14755,-1. [...]
+/*812*/{0.188491,1.91336,-1.67755,0.281822,1.86519,-1.81488,0.04432,1.80054,-1.61273,-0.061676,1.70882,-1.58362,0.078645,1.62843,-1.59207,0.153338,1.57379,-1.57775,0.236975,1.94419,-2.05628,0.293513,1.86892,-1.88769,0.095887,1.90276,-1.99706,0.098184,1.74052,-2.17712,0.145971,1.65003,-2.22191,0.303044,1.61307,-2.10368,0.363331,1.58736,-2.09888,0.198311,1.43237,-1.78643,0.011053,1.50018,-1.84607,0.014065,1.5245,-1.91105,0.018585,1.49949,-1.96198,0.200343,1.41911,-2.04039,0.260736,1.14466, [...]
+/*813*/{0.188561,1.90973,-1.6765,0.281957,1.86033,-1.81418,0.046613,1.79469,-1.61033,-0.056694,1.70196,-1.57945,0.083419,1.62315,-1.58808,0.158654,1.56811,-1.57521,0.234953,1.94092,-2.05651,0.293726,1.86622,-1.88756,0.09526,1.89953,-1.99512,0.094173,1.73729,-2.17504,0.141208,1.64697,-2.22052,0.300141,1.60673,-2.10571,0.359704,1.57799,-2.10262,0.197839,1.42836,-1.78555,0.011432,1.49654,-1.84624,0.013518,1.52061,-1.9111,0.018147,1.4948,-1.96201,0.200376,1.4162,-2.03933,0.258622,1.1412,-1.7 [...]
+/*814*/{0.189318,1.90536,-1.67594,0.28225,1.85669,-1.81368,0.049615,1.78866,-1.60701,-0.052905,1.69499,-1.57405,0.0877,1.61708,-1.58434,0.163083,1.56371,-1.57238,0.233564,1.93747,-2.0565,0.293572,1.8628,-1.88747,0.093715,1.89555,-1.99485,0.088461,1.7363,-2.17475,0.136446,1.64368,-2.21959,0.296374,1.59797,-2.10834,0.355832,1.56789,-2.10659,0.195612,1.42521,-1.78545,0.00996,1.49221,-1.84675,0.014368,1.51661,-1.91064,0.01866,1.49248,-1.96294,0.201289,1.4132,-2.03871,0.255954,1.13876,-1.7068 [...]
+/*815*/{0.190443,1.90057,-1.67465,0.282406,1.85404,-1.81414,0.051905,1.78256,-1.60434,-0.048944,1.68726,-1.56973,0.093085,1.61056,-1.57971,0.169476,1.55882,-1.5703,0.231678,1.93366,-2.05693,0.293225,1.85894,-1.88782,0.092414,1.89114,-1.99229,0.08409,1.73286,-2.17301,0.131322,1.63989,-2.21797,0.293101,1.59051,-2.11154,0.35162,1.55802,-2.11082,0.193117,1.42074,-1.78314,0.010075,1.48714,-1.84663,0.013678,1.51287,-1.91039,0.018346,1.48562,-1.96289,0.201007,1.41007,-2.03805,0.256324,1.13577,- [...]
+/*816*/{0.191029,1.89576,-1.67443,0.281672,1.84838,-1.81423,0.054828,1.77611,-1.60098,-0.04477,1.67986,-1.5652,0.097414,1.60422,-1.57591,0.174975,1.55367,-1.56714,0.229999,1.92919,-2.05713,0.293427,1.85559,-1.88796,0.091224,1.88735,-1.99158,0.081026,1.72795,-2.17142,0.126024,1.63659,-2.21713,0.288527,1.58124,-2.11458,0.346968,1.54741,-2.11576,0.195583,1.4173,-1.78373,0.009846,1.48256,-1.84613,0.013241,1.50878,-1.91051,0.018228,1.48051,-1.96364,0.201397,1.40657,-2.03712,0.256237,1.13262,- [...]
+/*817*/{0.192214,1.89014,-1.67324,0.282598,1.84468,-1.81397,0.058775,1.76822,-1.59786,-0.040919,1.67155,-1.55959,0.103427,1.59849,-1.57229,0.181166,1.5484,-1.56436,0.228,1.92482,-2.05778,0.293182,1.8516,-1.88774,0.090389,1.88263,-1.99084,0.077563,1.72391,-2.17039,0.120704,1.63253,-2.21605,0.284102,1.57264,-2.11751,0.341591,1.53682,-2.12063,0.1948,1.41396,-1.782,0.008594,1.47849,-1.84644,0.013069,1.50398,-1.9094,0.017692,1.47492,-1.96354,0.201041,1.40307,-2.03671,0.253895,1.12866,-1.70427 [...]
+/*818*/{0.192153,1.88495,-1.67328,0.281987,1.84098,-1.8147,0.061889,1.76169,-1.59417,-0.03645,1.66336,-1.5547,0.108994,1.59176,-1.56829,0.188109,1.54321,-1.5614,0.226226,1.92047,-2.05835,0.29309,1.84647,-1.88812,0.088905,1.87707,-1.98941,0.073727,1.71977,-2.16961,0.115064,1.62864,-2.21439,0.279604,1.56411,-2.12089,0.335789,1.5253,-2.12526,0.196911,1.40913,-1.78301,0.008446,1.47273,-1.84692,0.011932,1.49897,-1.90923,0.016794,1.46977,-1.96346,0.20113,1.39929,-2.03659,0.254873,1.12527,-1.70 [...]
+/*819*/{0.193888,1.87971,-1.67169,0.282885,1.83501,-1.81408,0.065739,1.75398,-1.59086,-0.031268,1.65553,-1.54911,0.114953,1.5851,-1.56485,0.1948,1.53853,-1.55941,0.22434,1.91547,-2.05889,0.293347,1.84236,-1.88857,0.087406,1.8724,-1.98785,0.068957,1.71581,-2.16902,0.109978,1.62474,-2.21385,0.274314,1.55511,-2.12494,0.329023,1.51415,-2.13036,0.196954,1.40535,-1.77933,0.007698,1.46772,-1.84724,0.011448,1.49355,-1.90856,0.016433,1.46417,-1.96379,0.202135,1.39531,-2.0331,0.253742,1.12007,-1.7 [...]
+/*820*/{0.194855,1.87399,-1.67105,0.283614,1.82994,-1.81401,0.069917,1.74629,-1.58718,-0.02348,1.64748,-1.54328,0.121711,1.57903,-1.56094,0.201468,1.53366,-1.55713,0.223158,1.91085,-2.05906,0.292838,1.83781,-1.88944,0.086573,1.86703,-1.98677,0.065624,1.71173,-2.16797,0.103581,1.62035,-2.21341,0.269312,1.54542,-2.12901,0.321973,1.50204,-2.13581,0.195584,1.40212,-1.77772,0.006309,1.46282,-1.84733,0.011079,1.48786,-1.9076,0.015175,1.45887,-1.9641,0.201658,1.39053,-2.03289,0.254538,1.1161,-1 [...]
+/*821*/{0.195397,1.86892,-1.67055,0.283417,1.82642,-1.81494,0.074118,1.7381,-1.58409,-0.018539,1.63871,-1.5389,0.128796,1.57284,-1.55781,0.208534,1.52876,-1.55505,0.222224,1.90644,-2.05964,0.292602,1.83301,-1.88996,0.085551,1.86141,-1.98591,0.061496,1.70765,-2.16731,0.097169,1.61632,-2.21328,0.263048,1.53572,-2.13307,0.31461,1.49039,-2.14055,0.194934,1.39764,-1.77652,0.004792,1.45805,-1.84777,0.009089,1.48194,-1.90697,0.01408,1.45342,-1.9637,0.200652,1.38614,-2.03238,0.25287,1.11122,-1.7 [...]
+/*822*/{0.196835,1.8638,-1.66936,0.284482,1.8194,-1.81501,0.078892,1.7295,-1.58088,-0.012693,1.62953,-1.5325,0.136257,1.56649,-1.55451,0.21613,1.52442,-1.55288,0.219712,1.90134,-2.06019,0.291066,1.82792,-1.89114,0.083725,1.85706,-1.98437,0.056375,1.70323,-2.16668,0.091425,1.61209,-2.21307,0.256627,1.52558,-2.13777,0.306104,1.47867,-2.14556,0.192902,1.39428,-1.77546,0.002022,1.45374,-1.84883,0.00762,1.47552,-1.90816,0.011941,1.44831,-1.96503,0.201909,1.38176,-2.0308,0.250929,1.10986,-1.70 [...]
+/*823*/{0.197894,1.8587,-1.66832,0.285424,1.81408,-1.81517,0.084746,1.72169,-1.57736,-0.004335,1.62135,-1.52673,0.143292,1.56071,-1.55156,0.223798,1.52032,-1.55132,0.219492,1.89772,-2.06146,0.292104,1.82302,-1.89171,0.083168,1.8537,-1.98286,0.052544,1.69943,-2.16631,0.085028,1.60796,-2.21263,0.249692,1.51531,-2.14243,0.297199,1.46754,-2.15061,0.190574,1.39162,-1.77533,-0.001543,1.44982,-1.84892,0.005371,1.47107,-1.90904,0.010167,1.44303,-1.96552,0.201051,1.37742,-2.02908,0.248112,1.10837 [...]
+/*824*/{0.199129,1.85427,-1.66698,0.285555,1.80991,-1.81596,0.090631,1.71327,-1.57378,0.003284,1.613,-1.52152,0.151476,1.55657,-1.55019,0.232326,1.51693,-1.55023,0.217993,1.89397,-2.06254,0.294621,1.81876,-1.89249,0.082851,1.85135,-1.98027,0.049687,1.69586,-2.16558,0.078911,1.60411,-2.21221,0.241854,1.5047,-2.14673,0.287906,1.45653,-2.15542,0.187167,1.38999,-1.77549,-0.004475,1.44569,-1.8495,0.002232,1.46591,-1.90962,0.007887,1.43746,-1.96634,0.200736,1.37291,-2.02774,0.246737,1.10916,-1 [...]
+/*825*/{0.200814,1.85026,-1.66585,0.286299,1.80453,-1.8162,0.096968,1.70499,-1.57152,0.012872,1.60574,-1.51653,0.160061,1.55197,-1.54789,0.241204,1.51464,-1.54871,0.217618,1.89025,-2.06351,0.29402,1.81428,-1.89374,0.082851,1.84894,-1.97878,0.046196,1.69297,-2.16494,0.0728,1.60112,-2.21214,0.234415,1.49636,-2.15103,0.278313,1.44626,-2.15954,0.183917,1.38763,-1.7736,-0.007925,1.44202,-1.85008,-0.001038,1.46193,-1.91126,0.004038,1.43236,-1.96717,0.198266,1.36939,-2.02635,0.24597,1.11007,-1. [...]
+/*826*/{0.203052,1.84583,-1.66426,0.28726,1.80143,-1.81678,0.102679,1.69885,-1.57021,0.020957,1.59757,-1.51181,0.168942,1.548,-1.54615,0.250867,1.51228,-1.54756,0.21799,1.88744,-2.06509,0.295507,1.81122,-1.89448,0.082449,1.84883,-1.97731,0.042749,1.69047,-2.16367,0.067955,1.59868,-2.21088,0.225562,1.48663,-2.15404,0.267463,1.43713,-2.16324,0.181641,1.38572,-1.77157,-0.01143,1.4384,-1.85252,-0.002995,1.45872,-1.91149,0.001641,1.42741,-1.96703,0.197216,1.36631,-2.02508,0.242528,1.11064,-1. [...]
+/*827*/{0.203876,1.84265,-1.66316,0.287065,1.80013,-1.81787,0.107955,1.69397,-1.5696,0.03043,1.59287,-1.50883,0.178252,1.54555,-1.54591,0.260506,1.51093,-1.54745,0.219308,1.8853,-2.06509,0.29562,1.80811,-1.89534,0.082443,1.84823,-1.97623,0.038693,1.69071,-2.16115,0.062456,1.59774,-2.21014,0.216726,1.48112,-2.15604,0.257106,1.429,-2.16692,0.178831,1.38513,-1.76871,-0.013518,1.4363,-1.85373,-0.005043,1.45667,-1.91084,-0.00203,1.42265,-1.96942,0.195255,1.36283,-2.02389,0.240579,1.11085,-1.6 [...]
+/*828*/{0.204182,1.84067,-1.66335,0.287124,1.7975,-1.81837,0.112716,1.69034,-1.5693,0.039584,1.58777,-1.5053,0.188018,1.54376,-1.54526,0.270374,1.51063,-1.54741,0.219254,1.88312,-2.06446,0.295743,1.80541,-1.89586,0.082509,1.8482,-1.97635,0.035038,1.69195,-2.15729,0.056636,1.59884,-2.2088,0.207933,1.47693,-2.1579,0.246292,1.4225,-2.17024,0.176629,1.38478,-1.76553,-0.015618,1.43418,-1.85529,-0.006216,1.45647,-1.91169,-0.004296,1.4199,-1.96988,0.192223,1.36043,-2.02346,0.237685,1.10868,-1.6 [...]
+/*829*/{0.204916,1.83896,-1.66403,0.287589,1.79396,-1.81865,0.119542,1.68798,-1.56808,0.049887,1.58424,-1.50299,0.19679,1.54284,-1.54549,0.281198,1.51185,-1.54795,0.219611,1.88133,-2.06357,0.294896,1.80379,-1.89645,0.083166,1.84771,-1.97724,0.030746,1.69398,-2.15339,0.051023,1.60138,-2.20733,0.198452,1.47268,-2.15832,0.235165,1.41672,-2.17258,0.17373,1.38449,-1.76345,-0.016491,1.43295,-1.85627,-0.007772,1.45626,-1.911,-0.00601,1.41926,-1.97003,0.190911,1.35905,-2.02327,0.23408,1.10546,-1 [...]
+/*830*/{0.206141,1.83802,-1.66457,0.28707,1.79234,-1.81862,0.124917,1.68621,-1.56751,0.058101,1.58119,-1.50034,0.206856,1.54268,-1.54517,0.291404,1.51347,-1.5487,0.219977,1.88022,-2.06353,0.294101,1.80168,-1.89574,0.082834,1.84786,-1.97751,0.027207,1.69725,-2.14944,0.044329,1.6047,-2.20583,0.189804,1.47064,-2.15907,0.223839,1.41304,-2.17506,0.171854,1.38427,-1.76105,-0.018309,1.43288,-1.85558,-0.008741,1.45575,-1.91015,-0.007013,1.41955,-1.96959,0.188653,1.35868,-2.02283,0.228923,1.1018, [...]
+/*831*/{0.207685,1.83757,-1.66503,0.285997,1.79266,-1.81925,0.129219,1.68577,-1.56691,0.068582,1.58016,-1.49879,0.217008,1.5436,-1.54566,0.301357,1.51642,-1.55064,0.220724,1.87926,-2.06308,0.293192,1.80073,-1.89559,0.083269,1.84842,-1.97813,0.022274,1.70181,-2.14551,0.039194,1.60825,-2.20407,0.180462,1.46931,-2.15966,0.213295,1.41122,-2.17658,0.170487,1.38407,-1.75951,-0.018671,1.43304,-1.8544,-0.009966,1.45636,-1.90942,-0.007897,1.42016,-1.96915,0.187288,1.35948,-2.02233,0.223809,1.0980 [...]
+/*832*/{0.209104,1.8376,-1.66584,0.284848,1.79167,-1.8203,0.13427,1.68557,-1.56638,0.076175,1.57853,-1.49757,0.226531,1.54568,-1.54667,0.31153,1.52045,-1.5523,0.220766,1.87882,-2.06276,0.292905,1.80063,-1.89593,0.082703,1.84897,-1.97818,0.018748,1.70565,-2.1409,0.032416,1.61295,-2.20281,0.171386,1.46919,-2.16038,0.202468,1.41009,-2.1783,0.167928,1.38413,-1.75881,-0.01979,1.4335,-1.85345,-0.011618,1.45622,-1.90918,-0.010065,1.42211,-1.96733,0.186232,1.35994,-2.02237,0.218493,1.0936,-1.692 [...]
+/*833*/{0.210543,1.83797,-1.66606,0.284785,1.79114,-1.82021,0.138178,1.6861,-1.56603,0.084109,1.5774,-1.49606,0.23495,1.54773,-1.54686,0.321261,1.52543,-1.55452,0.220133,1.87881,-2.06271,0.29229,1.79998,-1.89636,0.082126,1.84998,-1.9784,0.015517,1.7117,-2.13708,0.025912,1.61822,-2.2018,0.162889,1.47045,-2.16122,0.191319,1.41043,-2.17948,0.167311,1.38377,-1.7564,-0.020165,1.4333,-1.8529,-0.012477,1.45593,-1.90768,-0.011567,1.42324,-1.96624,0.184639,1.36229,-2.02243,0.213096,1.09025,-1.692 [...]
+/*834*/{0.212161,1.8391,-1.66633,0.285134,1.79167,-1.82077,0.14171,1.6869,-1.56543,0.093824,1.57735,-1.49422,0.244749,1.55135,-1.54752,0.331113,1.53112,-1.55703,0.218929,1.87925,-2.06295,0.291931,1.8002,-1.8968,0.081305,1.85085,-1.97752,0.011223,1.71709,-2.13388,0.019276,1.62354,-2.20084,0.154133,1.47233,-2.16232,0.181611,1.41234,-2.18035,0.166498,1.38446,-1.75608,-0.020442,1.43324,-1.85225,-0.013624,1.45626,-1.90639,-0.012616,1.42387,-1.96432,0.183609,1.36418,-2.02235,0.206203,1.08621,- [...]
+/*835*/{0.213101,1.84116,-1.66685,0.284189,1.79232,-1.82184,0.144789,1.68841,-1.56524,0.101114,1.57799,-1.49378,0.252337,1.55521,-1.54879,0.340551,1.53714,-1.56053,0.217338,1.88052,-2.06345,0.290642,1.80038,-1.89732,0.080737,1.85228,-1.97628,0.00694,1.72306,-2.13169,0.012806,1.62978,-2.20032,0.145329,1.47566,-2.16382,0.172043,1.41574,-2.1816,0.165507,1.38479,-1.75584,-0.021624,1.43314,-1.85171,-0.014434,1.45697,-1.90646,-0.013104,1.42455,-1.96306,0.182125,1.36599,-2.02293,0.200282,1.0830 [...]
+/*836*/{0.214691,1.84327,-1.66748,0.284762,1.7937,-1.82273,0.148856,1.69135,-1.56466,0.108377,1.57792,-1.49331,0.260202,1.56002,-1.55044,0.348721,1.54423,-1.56401,0.215386,1.88162,-2.06349,0.289767,1.80103,-1.89828,0.079394,1.8539,-1.97583,0.003553,1.72954,-2.12956,0.005685,1.63557,-2.19963,0.135692,1.48007,-2.16498,0.166497,1.42008,-2.18191,0.164574,1.38415,-1.75534,-0.021844,1.43381,-1.84997,-0.014682,1.45771,-1.90517,-0.01481,1.426,-1.96242,0.181406,1.36821,-2.02341,0.196777,1.08271,- [...]
+/*837*/{0.216534,1.84581,-1.66802,0.284679,1.79464,-1.82323,0.152757,1.69352,-1.56437,0.116044,1.57932,-1.49187,0.267617,1.56536,-1.55236,0.356916,1.55159,-1.56771,0.212762,1.88313,-2.06333,0.288732,1.80248,-1.89962,0.078525,1.85498,-1.97439,-0.002194,1.73564,-2.12889,-0.001053,1.64278,-2.19918,0.127299,1.48523,-2.16556,0.152407,1.42585,-2.18268,0.164888,1.38437,-1.75601,-0.022323,1.4359,-1.85003,-0.014415,1.45919,-1.90394,-0.01522,1.42776,-1.96143,0.18132,1.37105,-2.02431,0.189271,1.080 [...]
+/*838*/{0.218705,1.84927,-1.66887,0.284379,1.79746,-1.82447,0.156926,1.69661,-1.56486,0.125333,1.58164,-1.49173,0.274702,1.57089,-1.55475,0.363771,1.55913,-1.57131,0.210582,1.88547,-2.06364,0.288286,1.80394,-1.90072,0.07731,1.8576,-1.97304,-0.008064,1.74204,-2.12843,-0.008251,1.64897,-2.19907,0.118369,1.49159,-2.16598,0.14351,1.43159,-2.18248,0.165249,1.3843,-1.75545,-0.021827,1.43748,-1.84923,-0.014322,1.46025,-1.90282,-0.014945,1.42856,-1.96105,0.180147,1.37371,-2.02416,0.178632,1.0771 [...]
+/*839*/{0.221065,1.8524,-1.66973,0.284971,1.79961,-1.82559,0.160406,1.69912,-1.56547,0.133237,1.58426,-1.49126,0.282379,1.57742,-1.55724,0.370494,1.56805,-1.57596,0.207899,1.8882,-2.06461,0.288133,1.80666,-1.90192,0.075676,1.85868,-1.97206,-0.014261,1.74804,-2.12741,-0.016476,1.65594,-2.19885,0.110406,1.49802,-2.16597,0.134946,1.4379,-2.18237,0.166892,1.38428,-1.75637,-0.021737,1.43931,-1.8479,-0.01367,1.46236,-1.90216,-0.015581,1.43014,-1.96031,0.180228,1.37587,-2.02437,0.174919,1.07834 [...]
+/*840*/{0.222716,1.85562,-1.67065,0.285962,1.80265,-1.82753,0.165647,1.70232,-1.5654,0.139223,1.58598,-1.49103,0.288184,1.58353,-1.56024,0.376915,1.57618,-1.5806,0.205287,1.89186,-2.06458,0.287818,1.8088,-1.90313,0.07413,1.86132,-1.96966,-0.020307,1.75438,-2.127,-0.023627,1.66303,-2.19799,0.102552,1.50446,-2.16537,0.127002,1.44513,-2.18182,0.167351,1.38393,-1.75621,-0.020738,1.4413,-1.84662,-0.014365,1.46436,-1.90112,-0.015475,1.43215,-1.95948,0.179539,1.3788,-2.0246,0.167211,1.07476,-1. [...]
+/*841*/{0.22428,1.85979,-1.67179,0.285516,1.80544,-1.8288,0.170602,1.70523,-1.56518,0.146799,1.58954,-1.49173,0.293813,1.59016,-1.56336,0.381597,1.58534,-1.58497,0.20223,1.89481,-2.06437,0.287141,1.81162,-1.90553,0.072504,1.86281,-1.96757,-0.023801,1.76096,-2.12635,-0.030765,1.66959,-2.19723,0.094553,1.51074,-2.16473,0.119635,1.45277,-2.18141,0.168305,1.38509,-1.757,-0.019881,1.44382,-1.84653,-0.013574,1.46668,-1.89964,-0.014573,1.43479,-1.95891,0.180743,1.38177,-2.02478,0.157,1.07385,-1 [...]
+/*842*/{0.226926,1.86374,-1.67258,0.286509,1.80849,-1.83052,0.17592,1.70882,-1.5655,0.152455,1.59252,-1.49153,0.299773,1.59712,-1.56662,0.386831,1.59389,-1.59019,0.198732,1.8984,-2.06468,0.287312,1.81506,-1.90681,0.070723,1.8656,-1.96585,-0.030087,1.76742,-2.12491,-0.037717,1.67664,-2.19633,0.087378,1.51853,-2.16324,0.112296,1.46046,-2.18048,0.168975,1.38523,-1.75674,-0.018886,1.44696,-1.84484,-0.012701,1.46935,-1.89838,-0.01433,1.43776,-1.95771,0.180208,1.38504,-2.0243,0.150437,1.07274, [...]
+/*843*/{0.229055,1.86785,-1.67375,0.287032,1.81165,-1.83186,0.18151,1.71245,-1.56603,0.160157,1.5957,-1.49169,0.304257,1.60355,-1.5692,0.39106,1.60311,-1.5953,0.195354,1.90226,-2.06475,0.287657,1.819,-1.90852,0.070404,1.86752,-1.96419,-0.035913,1.77373,-2.12459,-0.044598,1.68365,-2.19526,0.080247,1.52613,-2.16199,0.104994,1.46832,-2.18009,0.170301,1.38591,-1.75657,-0.017412,1.44913,-1.84276,-0.011944,1.47164,-1.8974,-0.012885,1.4413,-1.95729,0.180871,1.38892,-2.0239,0.143382,1.07408,-1.7 [...]
+/*844*/{0.231435,1.87277,-1.67491,0.288234,1.81606,-1.83354,0.188076,1.7163,-1.56691,0.167292,1.60014,-1.49181,0.30847,1.61103,-1.57203,0.395001,1.61192,-1.59933,0.192178,1.90566,-2.0647,0.286906,1.82247,-1.9101,0.068755,1.87042,-1.96212,-0.041248,1.78073,-2.12415,-0.051071,1.69056,-2.19415,0.073351,1.53467,-2.16096,0.098895,1.47632,-2.17858,0.171958,1.38622,-1.75615,-0.016377,1.45161,-1.84205,-0.011054,1.47426,-1.89588,-0.012081,1.44498,-1.95678,0.181586,1.3921,-2.02314,0.138795,1.07316 [...]
+/*845*/{0.233283,1.8773,-1.67678,0.289178,1.82065,-1.83523,0.193282,1.72061,-1.5676,0.174217,1.60472,-1.4925,0.312831,1.61818,-1.57505,0.39798,1.62064,-1.60479,0.188984,1.91016,-2.06414,0.286796,1.82633,-1.91211,0.067962,1.87363,-1.96072,-0.044741,1.78724,-2.12311,-0.056643,1.69794,-2.19238,0.066762,1.54261,-2.15883,0.092055,1.48502,-2.17782,0.173048,1.38732,-1.75544,-0.014227,1.45431,-1.84106,-0.008888,1.47743,-1.89343,-0.010743,1.44796,-1.95585,0.182216,1.39616,-2.02236,0.131734,1.0733 [...]
+/*846*/{0.235105,1.88249,-1.67809,0.28937,1.82461,-1.83733,0.199327,1.72504,-1.56867,0.179332,1.60762,-1.49198,0.315593,1.62464,-1.57862,0.400802,1.62965,-1.60954,0.186249,1.91427,-2.0642,0.286959,1.83055,-1.91381,0.06772,1.87555,-1.95814,-0.049794,1.79363,-2.12261,-0.062604,1.70451,-2.19049,0.060475,1.55098,-2.15684,0.086266,1.49275,-2.1769,0.174445,1.3889,-1.75501,-0.012266,1.45755,-1.84025,-0.007033,1.48225,-1.8923,-0.01005,1.45219,-1.95502,0.183603,1.39995,-2.02191,0.124793,1.0756,-1 [...]
+/*847*/{0.236528,1.88718,-1.67975,0.29025,1.82922,-1.83924,0.20406,1.72946,-1.5695,0.185558,1.61238,-1.49277,0.319387,1.63227,-1.58151,0.403418,1.63785,-1.61447,0.184205,1.91792,-2.06368,0.287012,1.8354,-1.91583,0.067769,1.8786,-1.95601,-0.053426,1.79985,-2.12183,-0.06834,1.71172,-2.18899,0.055355,1.55912,-2.15551,0.080658,1.50093,-2.17612,0.176456,1.38938,-1.75462,-0.010537,1.46002,-1.83954,-0.006462,1.48411,-1.89082,-0.00789,1.45683,-1.95455,0.184197,1.40359,-2.02092,0.118647,1.07697,- [...]
+/*848*/{0.238343,1.89193,-1.68118,0.291929,1.8343,-1.84162,0.209273,1.73383,-1.57066,0.191242,1.61738,-1.49315,0.322365,1.63945,-1.58536,0.405524,1.64633,-1.61911,0.182318,1.92181,-2.06376,0.286447,1.83937,-1.91776,0.066763,1.88201,-1.95589,-0.056131,1.80709,-2.12135,-0.073351,1.71858,-2.18643,0.048933,1.56683,-2.15318,0.075373,1.50908,-2.17522,0.177993,1.39109,-1.75461,-0.007945,1.46336,-1.83854,-0.006143,1.48729,-1.88806,-0.006819,1.46112,-1.95479,0.185165,1.40808,-2.02028,0.111838,1.0 [...]
+/*849*/{0.239896,1.89671,-1.68326,0.292026,1.8378,-1.84297,0.213623,1.73861,-1.57213,0.196279,1.62175,-1.49431,0.324537,1.64606,-1.58901,0.407194,1.65408,-1.6238,0.18027,1.92617,-2.06375,0.286127,1.84395,-1.91946,0.066365,1.88578,-1.95394,-0.059227,1.81372,-2.11968,-0.078806,1.72561,-2.18406,0.044486,1.57454,-2.15154,0.069786,1.51728,-2.17463,0.179947,1.39172,-1.75467,-0.006418,1.46606,-1.8387,-0.003069,1.49087,-1.88653,-0.004683,1.46557,-1.95435,0.18687,1.41237,-2.01932,0.106456,1.07984 [...]
+/*850*/{0.2407,1.90139,-1.68469,0.291922,1.84302,-1.84509,0.217286,1.74361,-1.57319,0.20133,1.62668,-1.49416,0.325684,1.65228,-1.59119,0.40749,1.66168,-1.62829,0.178613,1.92931,-2.06419,0.285789,1.84849,-1.92143,0.066281,1.88962,-1.95323,-0.062125,1.81965,-2.11885,-0.083539,1.73221,-2.18131,0.038877,1.58267,-2.14977,0.06507,1.52448,-2.17376,0.182037,1.39326,-1.75527,-0.004902,1.46947,-1.83733,-0.001464,1.49507,-1.88491,-0.002488,1.47046,-1.95425,0.187457,1.41613,-2.01891,0.103911,1.08507 [...]
+/*851*/{0.242731,1.90599,-1.68639,0.292676,1.84769,-1.84737,0.221367,1.74831,-1.57426,0.204822,1.63124,-1.49481,0.328037,1.65862,-1.59466,0.409368,1.66898,-1.63291,0.177879,1.93285,-2.06436,0.285129,1.85245,-1.92308,0.066378,1.89268,-1.95262,-0.064633,1.82617,-2.11716,-0.087688,1.73887,-2.17795,0.034897,1.58953,-2.14755,0.060686,1.53126,-2.17269,0.184305,1.39501,-1.75576,-0.001449,1.47294,-1.83736,0.000317,1.49846,-1.88372,-0.000508,1.47502,-1.95317,0.187974,1.42045,-2.01811,0.094865,1.0 [...]
+/*852*/{0.243326,1.90989,-1.6885,0.293722,1.8511,-1.84908,0.224084,1.75236,-1.57507,0.208867,1.63596,-1.49498,0.328893,1.66463,-1.59641,0.409656,1.67592,-1.63702,0.176435,1.93608,-2.06436,0.28547,1.85656,-1.9243,0.066992,1.89554,-1.95165,-0.065711,1.83206,-2.11521,-0.092142,1.74511,-2.17479,0.031556,1.59662,-2.14575,0.057018,1.53826,-2.17183,0.186852,1.39595,-1.7566,-0.000103,1.47616,-1.83641,0.003266,1.50217,-1.88278,0.000808,1.4799,-1.95277,0.189623,1.4245,-2.01861,0.089686,1.08758,-1. [...]
+/*853*/{0.244876,1.91422,-1.68978,0.294358,1.85555,-1.85098,0.22708,1.75658,-1.57642,0.213032,1.63982,-1.49529,0.330416,1.66998,-1.59873,0.409671,1.68203,-1.641,0.175644,1.93909,-2.06449,0.285722,1.86,-1.92607,0.067686,1.89876,-1.95059,-0.067934,1.83724,-2.11275,-0.095238,1.75118,-2.1716,0.027129,1.60244,-2.14451,0.052593,1.54504,-2.17142,0.188293,1.39738,-1.75737,0.003021,1.47987,-1.83609,0.005347,1.50583,-1.88108,0.003495,1.48484,-1.9531,0.191104,1.42858,-2.01852,0.084699,1.09059,-1.71 [...]
+/*854*/{0.247232,1.91881,-1.69142,0.294318,1.85881,-1.85288,0.229673,1.76086,-1.5766,0.216144,1.64417,-1.49634,0.331369,1.6747,-1.60179,0.409623,1.6883,-1.64479,0.174387,1.94179,-2.06484,0.285516,1.8636,-1.92733,0.06813,1.90163,-1.95063,-0.069764,1.84342,-2.11045,-0.098897,1.75726,-2.16831,0.023659,1.60829,-2.14238,0.049389,1.55069,-2.17005,0.190739,1.39914,-1.75904,0.004506,1.48323,-1.83525,0.006458,1.50955,-1.8804,0.005406,1.48907,-1.95211,0.191867,1.43281,-2.01842,0.080163,1.09361,-1. [...]
+/*855*/{0.247879,1.92129,-1.69322,0.29502,1.86324,-1.85417,0.232364,1.76436,-1.57797,0.218159,1.64681,-1.49623,0.331918,1.67896,-1.60366,0.410448,1.69281,-1.64905,0.174871,1.9447,-2.06515,0.285569,1.86702,-1.92884,0.068619,1.90484,-1.95011,-0.068981,1.8485,-2.10764,-0.101648,1.76246,-2.16491,0.021607,1.61296,-2.14064,0.045811,1.55582,-2.16885,0.191943,1.40033,-1.75979,0.006861,1.48702,-1.83508,0.009096,1.51216,-1.87972,0.007495,1.49336,-1.95124,0.193292,1.43614,-2.01895,0.075139,1.09593, [...]
+/*856*/{0.250218,1.92455,-1.69404,0.296193,1.86581,-1.85613,0.234221,1.76819,-1.57868,0.22075,1.65035,-1.49704,0.332602,1.68317,-1.60651,0.410001,1.69765,-1.65244,0.175445,1.94731,-2.06541,0.28586,1.86992,-1.93008,0.069008,1.90649,-1.95033,-0.070314,1.85382,-2.10439,-0.104771,1.76794,-2.16095,0.018819,1.61853,-2.13895,0.043142,1.56112,-2.16751,0.194619,1.40187,-1.76119,0.009188,1.49042,-1.83438,0.010866,1.51607,-1.87936,0.009721,1.49818,-1.9513,0.193446,1.43928,-2.01942,0.070616,1.10064, [...]
+/*857*/{0.251239,1.92714,-1.69586,0.296396,1.86958,-1.85769,0.236615,1.77178,-1.57977,0.224551,1.65378,-1.49688,0.333018,1.68679,-1.60785,0.409403,1.70123,-1.65651,0.174441,1.94986,-2.06505,0.287105,1.87293,-1.93135,0.069791,1.90907,-1.9498,-0.070073,1.85852,-2.10086,-0.106856,1.77306,-2.15754,0.016977,1.62292,-2.13729,0.040255,1.56553,-2.16604,0.196225,1.40378,-1.76192,0.01014,1.49391,-1.8339,0.01186,1.51983,-1.87779,0.011796,1.50249,-1.95117,0.193413,1.44232,-2.01948,0.067932,1.10343,- [...]
+/*858*/{0.253812,1.9303,-1.69712,0.298488,1.8729,-1.85898,0.237519,1.77458,-1.58016,0.224937,1.65565,-1.49761,0.333867,1.68999,-1.61072,0.40982,1.70465,-1.65857,0.17433,1.95233,-2.06532,0.287746,1.87524,-1.93202,0.07058,1.91057,-1.94887,-0.069477,1.86331,-2.09814,-0.108522,1.77787,-2.15392,0.015354,1.62694,-2.13603,0.038172,1.56999,-2.16451,0.197847,1.40533,-1.76257,0.012101,1.49673,-1.83264,0.013108,1.52294,-1.87659,0.013207,1.50624,-1.95035,0.195377,1.44525,-2.02003,0.064447,1.10686,-1 [...]
+/*859*/{0.254655,1.93201,-1.69823,0.298206,1.87576,-1.85974,0.239931,1.77693,-1.58027,0.227146,1.65854,-1.49828,0.333722,1.69259,-1.61281,0.408621,1.70757,-1.66257,0.175002,1.95416,-2.06564,0.288205,1.87749,-1.93334,0.071374,1.9133,-1.94845,-0.069897,1.86586,-2.09471,-0.11019,1.78196,-2.15013,0.013587,1.63112,-2.13409,0.036786,1.5738,-2.1631,0.198755,1.40798,-1.76258,0.013395,1.50024,-1.83216,0.013511,1.52645,-1.87652,0.014173,1.51022,-1.94947,0.194294,1.44747,-2.02015,0.061852,1.11039,- [...]
+/*860*/{0.257063,1.93397,-1.6992,0.299326,1.87821,-1.861,0.241002,1.77977,-1.58215,0.228467,1.65981,-1.49824,0.334482,1.69358,-1.61383,0.408634,1.70958,-1.66557,0.175485,1.95696,-2.06596,0.289144,1.87998,-1.93437,0.072264,1.91479,-1.94801,-0.068842,1.8705,-2.09139,-0.111367,1.78552,-2.1469,0.012023,1.6344,-2.13253,0.035778,1.5774,-2.16156,0.198726,1.4101,-1.76352,0.014468,1.50348,-1.83191,0.014883,1.53023,-1.87554,0.014434,1.51396,-1.94841,0.19478,1.45026,-2.02014,0.059715,1.11485,-1.714 [...]
+/*861*/{0.257946,1.93558,-1.70058,0.29927,1.87993,-1.86142,0.243018,1.78204,-1.58277,0.229162,1.66103,-1.49858,0.33473,1.69571,-1.61564,0.408664,1.71064,-1.66815,0.175588,1.95875,-2.06612,0.289609,1.88164,-1.93554,0.073697,1.91694,-1.94827,-0.069504,1.87283,-2.08802,-0.112175,1.78862,-2.14317,0.01141,1.6368,-2.13076,0.034127,1.58009,-2.15994,0.199062,1.41239,-1.76373,0.015774,1.50608,-1.83069,0.014864,1.53241,-1.87581,0.014933,1.51759,-1.94819,0.194717,1.45237,-2.01963,0.059179,1.11773,- [...]
+/*862*/{0.2603,1.93718,-1.70135,0.300812,1.88253,-1.86333,0.243715,1.78373,-1.58387,0.231485,1.66307,-1.4993,0.334392,1.69551,-1.61747,0.408248,1.71109,-1.671,0.175729,1.96013,-2.06616,0.290967,1.88323,-1.93596,0.075248,1.91881,-1.948,-0.068737,1.87435,-2.08445,-0.112848,1.79132,-2.14051,0.01016,1.63911,-2.12959,0.033711,1.5826,-2.15844,0.198934,1.41437,-1.76404,0.016837,1.50908,-1.83001,0.015656,1.53546,-1.8751,0.016073,1.51941,-1.94661,0.195547,1.45446,-2.01938,0.059236,1.11973,-1.7150 [...]
+/*863*/{0.261429,1.93866,-1.70265,0.30236,1.88451,-1.86453,0.244398,1.78445,-1.58429,0.231517,1.66384,-1.49971,0.334211,1.69648,-1.61986,0.407947,1.71111,-1.67278,0.176608,1.96145,-2.06693,0.29139,1.88419,-1.93749,0.076111,1.92022,-1.94805,-0.068898,1.87627,-2.08099,-0.113776,1.79352,-2.13715,0.010001,1.6411,-2.12803,0.033286,1.58468,-2.15757,0.199432,1.41664,-1.76348,0.017175,1.51185,-1.8294,0.015956,1.53854,-1.87471,0.016627,1.52149,-1.94621,0.195036,1.45693,-2.019,0.058353,1.12178,-1. [...]
+/*864*/{0.263928,1.93961,-1.70295,0.302862,1.88573,-1.86574,0.244692,1.78574,-1.58533,0.231641,1.66365,-1.50045,0.334265,1.69627,-1.62157,0.408913,1.71157,-1.67529,0.177121,1.9619,-2.06788,0.292139,1.88561,-1.93839,0.077192,1.92212,-1.9473,-0.067904,1.87824,-2.07759,-0.114377,1.79496,-2.13455,0.011188,1.64342,-2.12634,0.033866,1.58596,-2.15623,0.199579,1.41918,-1.76352,0.017084,1.51388,-1.8298,0.016081,1.54083,-1.87532,0.016554,1.52369,-1.94584,0.195319,1.45809,-2.01837,0.061022,1.12216, [...]
+/*865*/{0.264286,1.93942,-1.70299,0.303606,1.88694,-1.86676,0.246066,1.78602,-1.58519,0.231952,1.66369,-1.50157,0.333915,1.6959,-1.62366,0.406649,1.70798,-1.67753,0.178532,1.96342,-2.0682,0.292673,1.88583,-1.93943,0.079066,1.92254,-1.94909,-0.067649,1.87834,-2.07499,-0.11362,1.7958,-2.13191,0.011082,1.64381,-2.12517,0.033374,1.5868,-2.15478,0.199594,1.42145,-1.76357,0.01764,1.51558,-1.82956,0.015623,1.5431,-1.87538,0.016614,1.52465,-1.94512,0.194707,1.4598,-2.01797,0.063267,1.12193,-1.71 [...]
+/*866*/{0.265738,1.93985,-1.70505,0.303541,1.88754,-1.86711,0.245524,1.78639,-1.58698,0.231733,1.66364,-1.50203,0.333735,1.69423,-1.62479,0.406317,1.70559,-1.67933,0.180213,1.96385,-2.0694,0.294047,1.88651,-1.94007,0.080149,1.92445,-1.94911,-0.067897,1.87877,-2.0725,-0.113889,1.79615,-2.12932,0.010981,1.64432,-2.12362,0.034593,1.58707,-2.15343,0.199621,1.4232,-1.76374,0.017594,1.51716,-1.82881,0.016054,1.54517,-1.87541,0.016721,1.52672,-1.94499,0.194917,1.46067,-2.01793,0.066447,1.12182, [...]
+/*867*/{0.267348,1.93965,-1.70522,0.303892,1.8878,-1.86833,0.245052,1.7864,-1.58777,0.230569,1.6627,-1.503,0.333647,1.69222,-1.62627,0.405214,1.70351,-1.68141,0.180835,1.96383,-2.07034,0.294593,1.88634,-1.94085,0.081591,1.9254,-1.94983,-0.065948,1.8787,-2.06919,-0.113523,1.7964,-2.12791,0.013468,1.64455,-2.12219,0.035061,1.58688,-2.15277,0.199189,1.4248,-1.76343,0.016813,1.51785,-1.82908,0.015106,1.54644,-1.87606,0.016653,1.52679,-1.94485,0.194487,1.46186,-2.01709,0.070657,1.12177,-1.721 [...]
+/*868*/{0.268703,1.93855,-1.70561,0.305071,1.88751,-1.86926,0.244914,1.78519,-1.58911,0.230804,1.66181,-1.50418,0.333168,1.69002,-1.62772,0.404753,1.69986,-1.68308,0.182037,1.96347,-2.07076,0.294861,1.88654,-1.94131,0.083305,1.92603,-1.95028,-0.065515,1.87802,-2.06643,-0.11277,1.79558,-2.12564,0.014367,1.6434,-2.12069,0.036251,1.58627,-2.15164,0.198838,1.42623,-1.76343,0.01607,1.51852,-1.82936,0.014724,1.54714,-1.87692,0.015203,1.52666,-1.94518,0.193828,1.46254,-2.0167,0.075918,1.11942,- [...]
+/*869*/{0.269517,1.9379,-1.70628,0.305038,1.88746,-1.86928,0.24471,1.78483,-1.59007,0.228255,1.66097,-1.50593,0.332851,1.68699,-1.62933,0.404825,1.69563,-1.68413,0.18266,1.96278,-2.07156,0.294829,1.88613,-1.94206,0.084001,1.9263,-1.95055,-0.06391,1.87649,-2.06393,-0.112347,1.7945,-2.12446,0.015256,1.64234,-2.11984,0.037626,1.58505,-2.15073,0.197712,1.42673,-1.76284,0.015558,1.51879,-1.82901,0.013695,1.54764,-1.87639,0.0146,1.52588,-1.94457,0.194738,1.46377,-2.01607,0.081875,1.11786,-1.72 [...]
+/*870*/{0.270467,1.93654,-1.70676,0.304933,1.88575,-1.86982,0.243672,1.78388,-1.59092,0.227315,1.65941,-1.50664,0.331895,1.68351,-1.63061,0.403847,1.69072,-1.68588,0.184614,1.96254,-2.0729,0.29537,1.88451,-1.94266,0.085409,1.92545,-1.95206,-0.062677,1.87475,-2.06137,-0.110757,1.79285,-2.1234,0.018196,1.6404,-2.11859,0.039907,1.58317,-2.14988,0.197838,1.4274,-1.7633,0.015086,1.51834,-1.82976,0.013559,1.54761,-1.87619,0.013917,1.52465,-1.9456,0.193869,1.46389,-2.01521,0.087699,1.11707,-1.7 [...]
+/*871*/{0.27128,1.93511,-1.70735,0.304914,1.88548,-1.8702,0.242076,1.78188,-1.59201,0.224857,1.6574,-1.50809,0.330963,1.67969,-1.63157,0.403624,1.68543,-1.68693,0.185699,1.96117,-2.07338,0.296007,1.8841,-1.94231,0.086002,1.92568,-1.95303,-0.061535,1.87278,-2.06025,-0.108922,1.79058,-2.12189,0.019491,1.63814,-2.1174,0.041996,1.58099,-2.14927,0.197706,1.42773,-1.76295,0.01481,1.51729,-1.82991,0.013451,1.54725,-1.87611,0.013928,1.52328,-1.94634,0.194033,1.46382,-2.0146,0.091686,1.11468,-1.7 [...]
+/*872*/{0.272133,1.93309,-1.7081,0.305509,1.88297,-1.87058,0.241437,1.78045,-1.5927,0.223854,1.65549,-1.51002,0.32917,1.67507,-1.63264,0.402296,1.67939,-1.68752,0.186158,1.95956,-2.07353,0.296825,1.88207,-1.94287,0.086736,1.92501,-1.9537,-0.059327,1.87078,-2.05851,-0.107633,1.78785,-2.12173,0.022372,1.63583,-2.11654,0.044365,1.57794,-2.14897,0.197667,1.42751,-1.76307,0.014339,1.51558,-1.8306,0.01262,1.54515,-1.87648,0.013009,1.52065,-1.94624,0.19455,1.46358,-2.0138,0.100681,1.11207,-1.73 [...]
+/*873*/{0.272107,1.93098,-1.7086,0.306137,1.88142,-1.87095,0.240208,1.77792,-1.59433,0.21987,1.65318,-1.51145,0.327865,1.67025,-1.63368,0.401875,1.67287,-1.68854,0.186811,1.95784,-2.07422,0.296746,1.88043,-1.94338,0.087787,1.92434,-1.9556,-0.057134,1.86678,-2.05605,-0.105176,1.78403,-2.12177,0.02445,1.63245,-2.11609,0.047058,1.57489,-2.14856,0.198021,1.42669,-1.76285,0.013811,1.51378,-1.83136,0.012826,1.54334,-1.8776,0.012023,1.51895,-1.94729,0.194146,1.46299,-2.01355,0.10613,1.1087,-1.7 [...]
+/*874*/{0.271981,1.92944,-1.70922,0.305925,1.87885,-1.8715,0.238612,1.77589,-1.59548,0.215976,1.64944,-1.51319,0.326119,1.6656,-1.63428,0.399517,1.66612,-1.68855,0.188095,1.95579,-2.0748,0.296512,1.87805,-1.94348,0.088068,1.92203,-1.95618,-0.055396,1.86322,-2.05605,-0.103304,1.78007,-2.12189,0.027514,1.6286,-2.11552,0.050121,1.57099,-2.14853,0.197996,1.42598,-1.76315,0.013176,1.51204,-1.83208,0.011971,1.54124,-1.87732,0.012591,1.51609,-1.94774,0.19346,1.46128,-2.01311,0.112449,1.1062,-1. [...]
+/*875*/{0.271803,1.92572,-1.71,0.306326,1.87643,-1.87164,0.236215,1.77279,-1.59619,0.212695,1.64713,-1.51506,0.324034,1.65965,-1.63515,0.398002,1.6581,-1.68897,0.18902,1.95364,-2.0754,0.296257,1.87573,-1.94365,0.088118,1.92103,-1.95686,-0.054321,1.85946,-2.05584,-0.10113,1.77519,-2.1227,0.030777,1.62453,-2.11545,0.053521,1.56684,-2.14884,0.198284,1.4246,-1.76284,0.012623,1.50905,-1.83288,0.012221,1.53882,-1.87695,0.012167,1.5132,-1.94821,0.193527,1.45944,-2.01374,0.11834,1.10446,-1.73773 [...]
+/*876*/{0.272457,1.92261,-1.71056,0.306142,1.87332,-1.87156,0.234922,1.77013,-1.59664,0.209385,1.64512,-1.51726,0.32104,1.65388,-1.63511,0.396823,1.65045,-1.68895,0.189151,1.95101,-2.07618,0.296664,1.87348,-1.94366,0.087958,1.91866,-1.95815,-0.052396,1.85506,-2.05659,-0.098664,1.7701,-2.1236,0.033154,1.62009,-2.11589,0.057104,1.56191,-2.14891,0.19744,1.42273,-1.76284,0.011876,1.50672,-1.83373,0.011541,1.53617,-1.87836,0.011473,1.51062,-1.94973,0.193041,1.45669,-2.01457,0.125136,1.10108,- [...]
+/*877*/{0.272413,1.91966,-1.71045,0.306252,1.8696,-1.87152,0.231937,1.76669,-1.59705,0.205346,1.64207,-1.51922,0.318838,1.64749,-1.63539,0.393837,1.64222,-1.68781,0.189569,1.94818,-2.07667,0.296361,1.87022,-1.94401,0.087883,1.91614,-1.9602,-0.051066,1.85031,-2.05766,-0.095988,1.76406,-2.12507,0.037207,1.61505,-2.11666,0.060422,1.55678,-2.14987,0.19818,1.42107,-1.76331,0.011709,1.50412,-1.83396,0.01251,1.53258,-1.8796,0.011219,1.50749,-1.94964,0.192458,1.45407,-2.01508,0.130036,1.09823,-1 [...]
+/*878*/{0.271378,1.91633,-1.71075,0.306034,1.86645,-1.87156,0.230259,1.76309,-1.59758,0.200855,1.63861,-1.52113,0.315238,1.64066,-1.63586,0.391941,1.63338,-1.68763,0.189764,1.94431,-2.07718,0.296957,1.8673,-1.94387,0.088363,1.91315,-1.9612,-0.051473,1.84438,-2.05965,-0.093942,1.75727,-2.1264,0.039823,1.60962,-2.11751,0.063972,1.5516,-2.1503,0.198915,1.41902,-1.76376,0.011323,1.50082,-1.83437,0.012717,1.52985,-1.87965,0.011802,1.50477,-1.95129,0.192619,1.45182,-2.01571,0.132338,1.09571,-1 [...]
+/*879*/{0.27113,1.91319,-1.71097,0.30722,1.86257,-1.87192,0.226574,1.75885,-1.5982,0.196855,1.6353,-1.52234,0.31182,1.63371,-1.63588,0.388044,1.62402,-1.68644,0.189797,1.94038,-2.07806,0.297443,1.86388,-1.94392,0.088295,1.90969,-1.96235,-0.049795,1.83845,-2.06204,-0.091424,1.75004,-2.12886,0.042864,1.60332,-2.11942,0.06834,1.54566,-2.15126,0.198345,1.4163,-1.76395,0.010834,1.4976,-1.83491,0.012551,1.52665,-1.88008,0.011041,1.50159,-1.9513,0.191925,1.44893,-2.01646,0.138275,1.09309,-1.738 [...]
+/*880*/{0.269879,1.90858,-1.71096,0.30554,1.85802,-1.8716,0.22333,1.75475,-1.59847,0.19052,1.63177,-1.52401,0.307622,1.62638,-1.63644,0.385127,1.61404,-1.68533,0.190261,1.93603,-2.07935,0.297459,1.86,-1.94423,0.087498,1.90529,-1.96339,-0.050349,1.83084,-2.0646,-0.089575,1.74248,-2.1312,0.046097,1.59681,-2.12154,0.072239,1.53953,-2.15249,0.198897,1.41411,-1.76436,0.010296,1.49405,-1.835,0.011827,1.52278,-1.87991,0.010183,1.49776,-1.9509,0.19133,1.44597,-2.01703,0.144794,1.08842,-1.73599,0 [...]
+/*881*/{0.269467,1.90439,-1.71004,0.306114,1.8542,-1.87183,0.221497,1.75056,-1.59774,0.185167,1.62781,-1.52603,0.303941,1.61924,-1.63631,0.381736,1.60477,-1.68469,0.190715,1.93152,-2.08033,0.297147,1.85558,-1.94367,0.08645,1.90177,-1.96581,-0.050127,1.82354,-2.06861,-0.087704,1.73415,-2.13361,0.049043,1.59021,-2.12367,0.076549,1.53301,-2.15366,0.198567,1.41153,-1.76457,0.010626,1.49014,-1.83511,0.011093,1.51894,-1.87969,0.009808,1.49387,-1.95116,0.191293,1.44293,-2.01786,0.1474,1.08686,- [...]
+/*882*/{0.268435,1.89991,-1.70957,0.305445,1.84865,-1.87067,0.218281,1.74642,-1.59782,0.179584,1.62474,-1.52675,0.298497,1.61137,-1.63566,0.377625,1.5944,-1.68277,0.190709,1.92643,-2.08152,0.297028,1.85141,-1.94403,0.086196,1.89707,-1.96675,-0.05012,1.81526,-2.07082,-0.085446,1.72567,-2.13649,0.052732,1.58267,-2.12599,0.081375,1.5259,-2.1547,0.198463,1.40846,-1.76426,0.009959,1.48551,-1.8354,0.010617,1.51493,-1.87981,0.008808,1.4896,-1.95071,0.190169,1.43941,-2.01848,0.150758,1.0841,-1.7 [...]
+/*883*/{0.266929,1.89492,-1.70903,0.305979,1.84405,-1.87039,0.21416,1.74201,-1.5988,0.173476,1.62042,-1.52808,0.294344,1.60339,-1.63521,0.37308,1.58388,-1.68115,0.190931,1.92046,-2.08271,0.296834,1.84691,-1.94426,0.085671,1.89219,-1.9696,-0.048529,1.80699,-2.07445,-0.082957,1.71632,-2.13981,0.057457,1.57563,-2.12925,0.086006,1.51899,-2.15609,0.198376,1.40537,-1.76495,0.009482,1.48121,-1.83461,0.009955,1.51093,-1.87971,0.007997,1.48475,-1.9498,0.190029,1.43633,-2.01912,0.154973,1.08127,-1 [...]
+/*884*/{0.26639,1.89051,-1.70887,0.306332,1.83895,-1.87,0.210503,1.73722,-1.59867,0.168078,1.61664,-1.52854,0.289401,1.59573,-1.63482,0.367788,1.57324,-1.67933,0.191501,1.91486,-2.08383,0.297489,1.84233,-1.94408,0.08571,1.8871,-1.97073,-0.048218,1.79722,-2.07874,-0.080531,1.70724,-2.14336,0.061144,1.56796,-2.13208,0.091378,1.5112,-2.15705,0.199081,1.4024,-1.76562,0.010365,1.47602,-1.83463,0.009799,1.50645,-1.87973,0.007347,1.47995,-1.94901,0.190001,1.4336,-2.01977,0.158367,1.07728,-1.731 [...]
+/*885*/{0.265159,1.88561,-1.70819,0.305449,1.83394,-1.86931,0.205933,1.732,-1.59858,0.161307,1.61283,-1.52949,0.284222,1.58727,-1.6341,0.362047,1.56321,-1.67673,0.192598,1.90924,-2.08561,0.298142,1.83726,-1.9438,0.084816,1.88182,-1.97292,-0.047,1.78736,-2.08373,-0.077383,1.69734,-2.14714,0.065856,1.55946,-2.1345,0.096808,1.50356,-2.15862,0.199235,1.39863,-1.76632,0.010058,1.47146,-1.83399,0.010134,1.5022,-1.88006,0.006682,1.47489,-1.94853,0.190129,1.43022,-2.02105,0.164132,1.07302,-1.727 [...]
+/*886*/{0.263134,1.88068,-1.70756,0.306697,1.8291,-1.86892,0.202578,1.7267,-1.59924,0.155802,1.60862,-1.52947,0.277693,1.57937,-1.63359,0.356806,1.55246,-1.67465,0.192738,1.90324,-2.08662,0.298229,1.83194,-1.94355,0.083989,1.8761,-1.97456,-0.046599,1.77773,-2.08804,-0.074297,1.68691,-2.15123,0.070376,1.55074,-2.13742,0.102843,1.49537,-2.16005,0.198697,1.39469,-1.76688,0.009859,1.46575,-1.83392,0.008845,1.49698,-1.88077,0.006052,1.47004,-1.94763,0.189818,1.42634,-2.02219,0.168609,1.06964, [...]
+/*887*/{0.261529,1.87525,-1.70676,0.305814,1.82304,-1.86779,0.198101,1.72111,-1.59938,0.148978,1.60554,-1.53053,0.272379,1.57114,-1.63245,0.349897,1.54232,-1.67227,0.193877,1.89725,-2.0879,0.298102,1.82667,-1.94371,0.082767,1.87091,-1.97661,-0.045364,1.76709,-2.09297,-0.070832,1.67672,-2.15504,0.076954,1.54183,-2.13934,0.109565,1.48744,-2.1613,0.19917,1.39086,-1.76791,0.008925,1.46114,-1.83313,0.008851,1.49201,-1.88015,0.004056,1.46476,-1.94662,0.189895,1.42231,-2.02362,0.170907,1.06736, [...]
+/*888*/{0.260935,1.86993,-1.70543,0.305794,1.81674,-1.86709,0.194268,1.71644,-1.59964,0.141347,1.60171,-1.53143,0.266162,1.56365,-1.63102,0.343211,1.53316,-1.66986,0.195366,1.8919,-2.08943,0.299057,1.82089,-1.94236,0.082329,1.86396,-1.97858,-0.043382,1.75632,-2.09892,-0.066929,1.66616,-2.15913,0.083497,1.53404,-2.14162,0.116594,1.4793,-2.16267,0.198546,1.38645,-1.76854,0.007797,1.45546,-1.83343,0.007563,1.48639,-1.88008,0.002664,1.46083,-1.94633,0.188903,1.4184,-2.02489,0.171344,1.06474, [...]
+/*889*/{0.258971,1.86527,-1.70433,0.305099,1.8118,-1.86594,0.189916,1.71197,-1.60035,0.133802,1.59843,-1.53268,0.258768,1.55735,-1.62921,0.335889,1.52448,-1.66728,0.196201,1.88622,-2.09166,0.300204,1.8161,-1.94235,0.082115,1.85859,-1.98062,-0.040065,1.74635,-2.10453,-0.061717,1.65472,-2.16355,0.090077,1.52528,-2.1433,0.124529,1.47209,-2.16361,0.198814,1.38208,-1.76918,0.006461,1.45031,-1.83194,0.005727,1.48122,-1.87977,0.000105,1.45677,-1.94608,0.187409,1.41381,-2.02649,0.177892,1.05923, [...]
+/*890*/{0.257703,1.8604,-1.70342,0.306371,1.80641,-1.86483,0.185783,1.70722,-1.60051,0.126734,1.59584,-1.53342,0.252757,1.55052,-1.62799,0.328347,1.51616,-1.66407,0.197777,1.88099,-2.09245,0.300634,1.81026,-1.94157,0.082042,1.85506,-1.9825,-0.037691,1.73574,-2.11137,-0.055793,1.64372,-2.16796,0.096148,1.51822,-2.14509,0.132833,1.46449,-2.16482,0.197961,1.37658,-1.77047,0.004512,1.44599,-1.83134,0.003708,1.47534,-1.87942,-0.002559,1.4526,-1.94585,0.183284,1.4093,-2.02749,0.182291,1.05644, [...]
+/*891*/{0.256568,1.85579,-1.7025,0.307079,1.80135,-1.86338,0.180691,1.70357,-1.60064,0.119572,1.59381,-1.53427,0.245267,1.54482,-1.62634,0.320948,1.5097,-1.66097,0.198254,1.87569,-2.09412,0.302304,1.80576,-1.94162,0.082591,1.85321,-1.98313,-0.032848,1.72491,-2.11816,-0.049028,1.63217,-2.17243,0.104868,1.51137,-2.14679,0.142039,1.4579,-2.16554,0.196982,1.37225,-1.77265,0.00228,1.44141,-1.83179,0.000854,1.47035,-1.87814,-0.007317,1.4495,-1.94586,0.181309,1.40627,-2.02959,0.185847,1.0531,-1 [...]
+/*892*/{0.2556,1.8517,-1.70102,0.306707,1.79585,-1.86245,0.17669,1.70045,-1.60061,0.111208,1.59289,-1.53517,0.237281,1.54081,-1.62481,0.312433,1.50372,-1.6582,0.200164,1.87111,-2.09604,0.302684,1.80005,-1.93998,0.08362,1.85002,-1.98376,-0.026838,1.71362,-2.12547,-0.04119,1.62091,-2.17695,0.114214,1.50367,-2.14775,0.152604,1.45174,-2.16666,0.195978,1.36762,-1.77551,-0.000767,1.4369,-1.83019,-0.001993,1.46682,-1.87719,-0.010923,1.4469,-1.94449,0.177613,1.40372,-2.03166,0.189987,1.05208,-1. [...]
+/*893*/{0.254396,1.84827,-1.69944,0.308396,1.7915,-1.8617,0.171795,1.69895,-1.60102,0.102404,1.59294,-1.53537,0.229715,1.53754,-1.62306,0.303816,1.49854,-1.65533,0.201715,1.86684,-2.09672,0.302989,1.79611,-1.93967,0.084445,1.84819,-1.98343,-0.022058,1.70282,-2.13461,-0.032043,1.60971,-2.18093,0.124127,1.49753,-2.14869,0.163345,1.44628,-2.16698,0.194045,1.36369,-1.77772,-0.002924,1.43359,-1.82867,-0.003668,1.46566,-1.87628,-0.013629,1.44497,-1.94253,0.174476,1.40221,-2.0341,0.193718,1.052 [...]
+/*894*/{0.253997,1.84543,-1.69801,0.308075,1.78913,-1.85995,0.167319,1.69776,-1.60048,0.093334,1.59357,-1.53658,0.221312,1.53546,-1.62123,0.295329,1.49416,-1.65258,0.203264,1.86377,-2.09695,0.303302,1.79326,-1.93826,0.084553,1.84732,-1.9829,-0.016304,1.69344,-2.14091,-0.020741,1.59786,-2.18441,0.135342,1.49286,-2.15022,0.175138,1.44248,-2.16765,0.191509,1.3604,-1.77981,-0.004967,1.43162,-1.82677,-0.005692,1.46624,-1.87649,-0.016629,1.44417,-1.93968,0.171911,1.4021,-2.03685,0.199358,1.055 [...]
+/*895*/{0.253813,1.84263,-1.69551,0.308095,1.78691,-1.85932,0.160231,1.69842,-1.60113,0.085984,1.59623,-1.53746,0.212321,1.53439,-1.62009,0.287084,1.49156,-1.64922,0.204499,1.8613,-2.09661,0.303403,1.7908,-1.93787,0.084244,1.84624,-1.98225,-0.013376,1.68628,-2.14804,-0.011516,1.59048,-2.18839,0.146935,1.48856,-2.15087,0.187774,1.43873,-2.1678,0.190349,1.35756,-1.78087,-0.007187,1.43039,-1.82444,-0.006128,1.46638,-1.87587,-0.017681,1.44441,-1.93736,0.168578,1.40216,-2.03912,0.204581,1.058 [...]
+/*896*/{0.254324,1.84091,-1.69618,0.307776,1.78607,-1.85791,0.155711,1.69937,-1.60036,0.076143,1.59959,-1.5393,0.204269,1.53421,-1.61833,0.277211,1.48959,-1.64604,0.206085,1.8603,-2.09561,0.303927,1.78952,-1.93653,0.084789,1.84485,-1.98188,-0.005556,1.67921,-2.15232,-0.001255,1.58324,-2.19097,0.159323,1.48639,-2.15121,0.201594,1.43739,-2.16771,0.188904,1.35473,-1.78329,-0.007725,1.43069,-1.82235,-0.008854,1.46627,-1.87531,-0.019298,1.44485,-1.93448,0.166008,1.4021,-2.04139,0.211311,1.061 [...]
+/*897*/{0.255314,1.8393,-1.69543,0.307595,1.78399,-1.85742,0.150723,1.70169,-1.60026,0.068752,1.60455,-1.5412,0.195702,1.5351,-1.61738,0.268521,1.48883,-1.64293,0.20742,1.85927,-2.09501,0.303261,1.78783,-1.93602,0.084985,1.84478,-1.98191,0.001758,1.67294,-2.15515,0.009525,1.5779,-2.19343,0.17084,1.48425,-2.15088,0.21536,1.43722,-2.16683,0.185606,1.35274,-1.78353,-0.008825,1.43185,-1.82184,-0.010344,1.46704,-1.87555,-0.020424,1.44552,-1.93333,0.163466,1.40272,-2.04289,0.218993,1.06558,-1. [...]
+/*898*/{0.255269,1.83877,-1.69485,0.306871,1.78322,-1.85706,0.146119,1.70459,-1.60032,0.059608,1.60983,-1.54312,0.188378,1.5367,-1.61584,0.259999,1.48846,-1.64004,0.208509,1.85858,-2.09347,0.302955,1.78676,-1.93448,0.085298,1.84476,-1.98155,0.010761,1.66883,-2.15693,0.019093,1.57269,-2.19475,0.183371,1.48395,-2.15091,0.229128,1.43865,-2.16591,0.184722,1.35229,-1.78443,-0.01012,1.43355,-1.82178,-0.012262,1.46789,-1.87522,-0.022087,1.44686,-1.93213,0.158997,1.40375,-2.04463,0.22651,1.07185 [...]
+/*899*/{0.254913,1.83836,-1.69494,0.305539,1.78225,-1.85634,0.140362,1.70806,-1.60085,0.051478,1.61604,-1.54438,0.179618,1.53991,-1.61506,0.250629,1.49038,-1.63808,0.211309,1.85928,-2.09301,0.303106,1.78654,-1.9338,0.086045,1.8463,-1.98228,0.017675,1.66484,-2.15931,0.028996,1.56946,-2.19684,0.196896,1.48453,-2.14962,0.243151,1.44129,-2.16432,0.182044,1.35323,-1.78478,-0.01081,1.43495,-1.8206,-0.013652,1.46812,-1.87579,-0.022889,1.44772,-1.93281,0.15921,1.40407,-2.04567,0.233807,1.07734,- [...]
+/*900*/{0.254256,1.83953,-1.69423,0.305214,1.78242,-1.85526,0.136005,1.71226,-1.6012,0.045807,1.62285,-1.54581,0.172208,1.54319,-1.61365,0.241752,1.49276,-1.63618,0.212827,1.85974,-2.09273,0.302762,1.7864,-1.93318,0.086828,1.84714,-1.98359,0.026175,1.66218,-2.16066,0.039152,1.56683,-2.19772,0.209083,1.48709,-2.14863,0.257358,1.44619,-2.16206,0.181458,1.35489,-1.78431,-0.01166,1.43652,-1.82107,-0.01478,1.4692,-1.87679,-0.024394,1.44899,-1.93268,0.157515,1.40506,-2.04649,0.242911,1.08302,- [...]
+/*901*/{0.253036,1.84091,-1.69355,0.30343,1.7823,-1.8542,0.130283,1.71722,-1.60175,0.036651,1.62948,-1.54752,0.164213,1.54708,-1.61196,0.233964,1.49588,-1.63467,0.216262,1.86169,-2.0923,0.302693,1.78748,-1.93125,0.08754,1.84774,-1.98554,0.032265,1.66001,-2.16234,0.048936,1.56549,-2.19855,0.220845,1.49107,-2.14676,0.270482,1.45204,-2.1597,0.17925,1.35795,-1.7839,-0.013296,1.43806,-1.82224,-0.01549,1.47027,-1.87695,-0.024903,1.45019,-1.93344,0.156374,1.40593,-2.04753,0.251268,1.09037,-1.72 [...]
+/*902*/{0.251465,1.84355,-1.69282,0.303543,1.7836,-1.8533,0.125012,1.72211,-1.60217,0.029354,1.63657,-1.54841,0.15647,1.55244,-1.61174,0.225322,1.4995,-1.63269,0.217755,1.86426,-2.09152,0.302239,1.78801,-1.93035,0.088124,1.84996,-1.98667,0.038504,1.65934,-2.16346,0.058848,1.56547,-2.19999,0.233033,1.49768,-2.14507,0.283765,1.46018,-2.15687,0.178463,1.36122,-1.78476,-0.014333,1.43941,-1.82317,-0.017035,1.47172,-1.87764,-0.025856,1.45158,-1.93471,0.155222,1.40664,-2.04878,0.260999,1.0968,- [...]
+/*903*/{0.248772,1.84621,-1.69232,0.301452,1.78503,-1.85137,0.120599,1.7283,-1.60202,0.021429,1.64382,-1.54987,0.148451,1.55804,-1.61052,0.216586,1.50477,-1.63103,0.219964,1.86737,-2.0906,0.301816,1.79016,-1.92912,0.088617,1.8505,-1.98782,0.044669,1.65892,-2.1641,0.067731,1.56605,-2.20057,0.243446,1.50392,-2.14271,0.296135,1.46936,-2.1537,0.17683,1.36392,-1.78441,-0.016172,1.44148,-1.82372,-0.017723,1.47317,-1.87938,-0.026412,1.45254,-1.93652,0.155256,1.40756,-2.04922,0.265819,1.10558,-1 [...]
+/*904*/{0.245688,1.84909,-1.69105,0.299895,1.78717,-1.8499,0.114942,1.73384,-1.60243,0.014924,1.65185,-1.55087,0.141223,1.56423,-1.60965,0.209366,1.50984,-1.62934,0.222408,1.87139,-2.08992,0.301244,1.7923,-1.92762,0.089537,1.85183,-1.98902,0.049035,1.65929,-2.16397,0.076721,1.56723,-2.20126,0.25389,1.51198,-2.14066,0.308083,1.47877,-2.15041,0.176121,1.36682,-1.78471,-0.016147,1.44327,-1.82582,-0.017669,1.47521,-1.88058,-0.026931,1.45422,-1.93801,0.15496,1.40866,-2.0503,0.275063,1.11496,- [...]
+/*905*/{0.243293,1.85318,-1.69074,0.298751,1.79052,-1.84857,0.110195,1.73964,-1.6027,0.009007,1.65881,-1.55237,0.134847,1.57136,-1.60946,0.200753,1.51607,-1.62825,0.223717,1.87546,-2.089,0.301455,1.79509,-1.92538,0.090256,1.85422,-1.99015,0.055579,1.65978,-2.16342,0.085541,1.56926,-2.20214,0.264062,1.52065,-2.13776,0.319159,1.48996,-2.14649,0.174929,1.36981,-1.78435,-0.017188,1.44514,-1.82791,-0.018629,1.47671,-1.88188,-0.027698,1.45677,-1.94043,0.154218,1.40958,-2.05096,0.281116,1.12362 [...]
+/*906*/{0.240094,1.85739,-1.69019,0.297925,1.79428,-1.84609,0.105409,1.74606,-1.60271,0.000988,1.6656,-1.553,0.126424,1.57879,-1.6101,0.194072,1.52288,-1.62758,0.225683,1.87972,-2.08799,0.301192,1.79904,-1.92381,0.090302,1.85669,-1.99165,0.059689,1.66118,-2.16285,0.092938,1.57244,-2.20352,0.273138,1.53046,-2.13518,0.330006,1.50192,-2.14244,0.174262,1.37346,-1.78359,-0.017672,1.44738,-1.82931,-0.018567,1.47946,-1.88356,-0.02755,1.45872,-1.94128,0.154889,1.41093,-2.05105,0.286495,1.13258,- [...]
+/*907*/{0.236928,1.86172,-1.68944,0.296649,1.79856,-1.84463,0.10111,1.75239,-1.60251,-0.005049,1.67341,-1.55381,0.119986,1.58596,-1.61013,0.186619,1.52897,-1.62671,0.227236,1.88491,-2.08656,0.300813,1.8025,-1.92213,0.091248,1.85941,-1.99302,0.063981,1.66515,-2.16235,0.101508,1.57572,-2.20401,0.281043,1.53951,-2.13153,0.33921,1.51408,-2.13865,0.173378,1.37793,-1.78347,-0.018332,1.44994,-1.83194,-0.018468,1.48212,-1.88538,-0.026922,1.46151,-1.94307,0.156217,1.41213,-2.05086,0.291389,1.1415 [...]
+/*908*/{0.23283,1.8664,-1.68866,0.295897,1.80271,-1.84247,0.096505,1.75794,-1.60315,-0.012108,1.68045,-1.55512,0.113818,1.59311,-1.60938,0.179398,1.53671,-1.62567,0.229912,1.89014,-2.08559,0.301366,1.8067,-1.92043,0.09155,1.86221,-1.99401,0.068502,1.66803,-2.16176,0.109409,1.57987,-2.20557,0.289107,1.54984,-2.12878,0.348843,1.52642,-2.13438,0.172951,1.38249,-1.7829,-0.018418,1.45334,-1.8333,-0.017978,1.48513,-1.88748,-0.026085,1.46417,-1.94529,0.156244,1.41354,-2.05096,0.293898,1.15005,- [...]
+/*909*/{0.229634,1.87172,-1.68824,0.295045,1.80707,-1.84035,0.091522,1.76442,-1.60286,-0.018595,1.68827,-1.5563,0.107609,1.60077,-1.60991,0.17329,1.54388,-1.62526,0.231238,1.89566,-2.08468,0.300996,1.81077,-1.91861,0.092533,1.86539,-1.9955,0.072662,1.67193,-2.16263,0.117141,1.58378,-2.20635,0.296227,1.55985,-2.12535,0.356774,1.53971,-2.12995,0.173331,1.3872,-1.78295,-0.018223,1.45668,-1.83573,-0.017402,1.48805,-1.88914,-0.025403,1.46789,-1.94802,0.157034,1.41473,-2.05078,0.297952,1.15962 [...]
+/*910*/{0.225778,1.87689,-1.68786,0.294448,1.81157,-1.83916,0.087471,1.77063,-1.60298,-0.02242,1.69627,-1.55746,0.100669,1.60844,-1.61166,0.166996,1.55113,-1.62463,0.233048,1.90105,-2.08265,0.300875,1.81526,-1.91626,0.093625,1.86955,-1.99681,0.07898,1.67405,-2.16233,0.124867,1.58903,-2.20792,0.303784,1.57212,-2.12179,0.36395,1.55239,-2.1255,0.172817,1.39198,-1.78303,-0.018463,1.46048,-1.83722,-0.017297,1.49221,-1.89101,-0.024798,1.47132,-1.94953,0.158577,1.416,-2.05062,0.300709,1.16784,- [...]
+/*911*/{0.221673,1.88176,-1.68808,0.293754,1.81683,-1.83706,0.082945,1.77725,-1.60267,-0.028394,1.70434,-1.55848,0.095844,1.61658,-1.61076,0.160707,1.55862,-1.62467,0.235914,1.90653,-2.08126,0.301132,1.82023,-1.91532,0.094493,1.87311,-1.99775,0.083802,1.6788,-2.16418,0.132476,1.59455,-2.20915,0.309382,1.58284,-2.11871,0.370099,1.56628,-2.12117,0.173515,1.39646,-1.78325,-0.018092,1.46476,-1.84011,-0.015799,1.49587,-1.89279,-0.022704,1.47499,-1.95199,0.15968,1.41656,-2.0508,0.304568,1.1771 [...]
+/*912*/{0.218603,1.88706,-1.68762,0.291808,1.82136,-1.83536,0.078139,1.78395,-1.60385,-0.033714,1.71272,-1.56006,0.089717,1.62413,-1.61166,0.155273,1.56602,-1.6243,0.237616,1.91178,-2.07978,0.301504,1.82483,-1.9125,0.095147,1.87752,-1.99828,0.089102,1.68413,-2.16607,0.139879,1.60001,-2.21026,0.314702,1.59345,-2.11544,0.375912,1.57843,-2.11703,0.173739,1.40149,-1.78388,-0.017779,1.46954,-1.84101,-0.014971,1.49916,-1.89571,-0.020803,1.47818,-1.95327,0.161044,1.41751,-2.0508,0.306376,1.1850 [...]
+/*913*/{0.215429,1.89273,-1.68758,0.291571,1.8267,-1.83371,0.074125,1.79061,-1.60467,-0.039451,1.7207,-1.5614,0.085187,1.63187,-1.61225,0.149791,1.57311,-1.62362,0.238987,1.91745,-2.0779,0.30081,1.8295,-1.91112,0.096032,1.88269,-1.99983,0.094153,1.68901,-2.16835,0.146542,1.60619,-2.21214,0.318622,1.60317,-2.11216,0.380661,1.59122,-2.11259,0.174291,1.40572,-1.78534,-0.016415,1.47438,-1.8435,-0.013587,1.50317,-1.89732,-0.018827,1.48241,-1.95574,0.162595,1.41844,-2.05078,0.309058,1.19334,-1 [...]
+/*914*/{0.212186,1.89648,-1.68691,0.291261,1.83229,-1.83201,0.070022,1.79738,-1.60492,-0.043952,1.72859,-1.56324,0.079825,1.63861,-1.61271,0.144331,1.58059,-1.6239,0.241601,1.92349,-2.07643,0.301562,1.83487,-1.90923,0.097122,1.88664,-2.00088,0.099595,1.6951,-2.17043,0.153299,1.6121,-2.21351,0.32326,1.61465,-2.10904,0.384248,1.60185,-2.10699,0.175989,1.41071,-1.78578,-0.015402,1.47932,-1.8439,-0.012451,1.50751,-1.89876,-0.017322,1.48616,-1.95637,0.163766,1.41985,-2.05122,0.311939,1.20174, [...]
+/*915*/{0.209844,1.9019,-1.68681,0.29126,1.83562,-1.83013,0.066102,1.80379,-1.60593,-0.049696,1.73558,-1.56468,0.075158,1.64607,-1.61291,0.14019,1.58709,-1.62339,0.243468,1.92908,-2.07445,0.301584,1.83945,-1.90719,0.098528,1.89118,-2.0017,0.105575,1.70047,-2.17184,0.160188,1.61853,-2.2156,0.326493,1.62384,-2.10637,0.388556,1.61289,-2.10148,0.177985,1.41566,-1.78685,-0.014245,1.48391,-1.8463,-0.009878,1.5113,-1.90147,-0.014941,1.49025,-1.95872,0.165008,1.42124,-2.05184,0.314049,1.20931,-1 [...]
+/*916*/{0.207515,1.90662,-1.68642,0.289641,1.84256,-1.82857,0.062583,1.81072,-1.60723,-0.053234,1.7439,-1.56671,0.07047,1.65326,-1.61413,0.134865,1.59387,-1.62359,0.244434,1.9346,-2.07268,0.301964,1.84439,-1.90576,0.09972,1.89575,-2.00216,0.108138,1.70952,-2.1763,0.166572,1.62516,-2.21722,0.328342,1.63345,-2.10426,0.391664,1.62339,-2.09826,0.177952,1.42099,-1.78767,-0.013495,1.48953,-1.84736,-0.009191,1.51648,-1.90339,-0.012308,1.49425,-1.95945,0.166952,1.42247,-2.05196,0.315313,1.21703, [...]
+/*917*/{0.205376,1.91121,-1.68676,0.290147,1.84687,-1.82739,0.058899,1.81761,-1.60799,-0.058747,1.75072,-1.56838,0.065159,1.65966,-1.61451,0.129911,1.60051,-1.62446,0.246845,1.94015,-2.07112,0.301831,1.84946,-1.9038,0.100317,1.89984,-2.0028,0.113389,1.71394,-2.17881,0.171911,1.63201,-2.21934,0.332069,1.64363,-2.10153,0.394583,1.63436,-2.09477,0.179182,1.42539,-1.78883,-0.01098,1.49445,-1.84799,-0.007628,1.52059,-1.90528,-0.010436,1.49905,-1.96138,0.168494,1.42387,-2.05231,0.314964,1.2233 [...]
+/*918*/{0.203495,1.91542,-1.68628,0.288973,1.85133,-1.82645,0.055489,1.82403,-1.60896,-0.063175,1.75832,-1.57147,0.061182,1.66671,-1.61528,0.125656,1.60598,-1.62324,0.248152,1.94526,-2.06981,0.301458,1.85424,-1.90257,0.101497,1.90342,-2.00329,0.117869,1.7204,-2.18116,0.177673,1.63817,-2.2212,0.333576,1.65249,-2.10013,0.396063,1.64435,-2.09172,0.179871,1.42992,-1.78936,-0.009631,1.50023,-1.84973,-0.005404,1.5257,-1.9072,-0.008301,1.50334,-1.96265,0.169567,1.42543,-2.05247,0.316352,1.22968 [...]
+/*919*/{0.201413,1.92009,-1.68624,0.287975,1.85785,-1.82567,0.052163,1.83046,-1.61081,-0.066695,1.76486,-1.57331,0.057105,1.67271,-1.61646,0.122619,1.61185,-1.62328,0.249074,1.94993,-2.06778,0.302561,1.85845,-1.90092,0.10239,1.90779,-2.00446,0.122613,1.72645,-2.18366,0.182278,1.64413,-2.22316,0.335416,1.66042,-2.09813,0.397343,1.65369,-2.09052,0.18039,1.43441,-1.79003,-0.008399,1.50622,-1.84993,-0.003741,1.5299,-1.90941,-0.005621,1.50737,-1.96342,0.171522,1.42684,-2.05253,0.317686,1.2363 [...]
+/*920*/{0.199543,1.92423,-1.68645,0.287786,1.86151,-1.82409,0.049217,1.83591,-1.61194,-0.070924,1.7713,-1.5754,0.052901,1.67786,-1.61698,0.1183,1.61741,-1.6233,0.250197,1.95452,-2.0669,0.302367,1.86285,-1.89953,0.103407,1.91107,-2.0049,0.127158,1.73116,-2.18623,0.187236,1.65055,-2.22528,0.336574,1.66771,-2.09643,0.398815,1.66236,-2.08695,0.182368,1.44044,-1.79031,-0.005979,1.51138,-1.85103,-0.001366,1.53402,-1.91188,-0.00292,1.51139,-1.96413,0.173572,1.42855,-2.05244,0.317378,1.24056,-1. [...]
+/*921*/{0.197922,1.92839,-1.68656,0.287588,1.86669,-1.82358,0.04682,1.8419,-1.61355,-0.075228,1.77776,-1.57838,0.049928,1.68308,-1.61778,0.114443,1.62228,-1.62278,0.250962,1.9591,-2.06534,0.30207,1.86686,-1.89847,0.103862,1.91486,-2.00603,0.131333,1.73714,-2.18897,0.190854,1.65558,-2.22717,0.336331,1.67464,-2.09546,0.399557,1.66994,-2.08538,0.182629,1.44481,-1.79075,-0.004659,1.51626,-1.85288,4.8e-005,1.53849,-1.9134,-0.00036,1.5155,-1.9654,0.175187,1.4307,-2.05248,0.318752,1.24496,-1.73 [...]
+/*922*/{0.196141,1.93193,-1.68665,0.28815,1.86986,-1.8226,0.044621,1.84757,-1.61481,-0.078942,1.78231,-1.58031,0.046931,1.68733,-1.61889,0.111146,1.6264,-1.62246,0.25226,1.96323,-2.0642,0.302026,1.87158,-1.89713,0.10502,1.91812,-2.00625,0.135537,1.74198,-2.19166,0.194135,1.66114,-2.22914,0.337138,1.68147,-2.09514,0.399909,1.67752,-2.08376,0.183277,1.44923,-1.7912,-0.002916,1.52204,-1.85324,0.002917,1.54288,-1.91478,0.001418,1.51915,-1.96628,0.177334,1.43264,-2.05213,0.319044,1.24831,-1.7 [...]
+/*923*/{0.195039,1.93548,-1.68685,0.286711,1.87358,-1.82162,0.042763,1.85261,-1.61654,-0.08176,1.78814,-1.58319,0.043549,1.69242,-1.61876,0.108062,1.62963,-1.62173,0.252222,1.96717,-2.06346,0.30207,1.87506,-1.89677,0.105435,1.92119,-2.00658,0.139503,1.74746,-2.19439,0.1969,1.66629,-2.23103,0.337108,1.68742,-2.09483,0.400217,1.68342,-2.08198,0.184579,1.4532,-1.79163,-0.000319,1.52652,-1.85369,0.003884,1.54747,-1.91591,0.004251,1.52306,-1.96732,0.178746,1.43489,-2.05151,0.318341,1.25125,-1 [...]
+/*924*/{0.193614,1.93867,-1.68747,0.285962,1.87711,-1.8218,0.03971,1.85712,-1.61837,-0.085229,1.79234,-1.58557,0.040813,1.69643,-1.62007,0.105091,1.63378,-1.62143,0.253014,1.97094,-2.06253,0.302311,1.87884,-1.8955,0.10683,1.9241,-2.00685,0.142182,1.75198,-2.19743,0.198953,1.67133,-2.23278,0.337607,1.69126,-2.09331,0.400394,1.68811,-2.08071,0.184923,1.45765,-1.79187,0.001613,1.5308,-1.85359,0.005171,1.5509,-1.91688,0.00617,1.52703,-1.96773,0.181898,1.43598,-2.0509,0.317568,1.25337,-1.7381 [...]
+/*925*/{0.19303,1.94183,-1.68749,0.285256,1.88102,-1.82072,0.038041,1.86191,-1.6192,-0.086221,1.79713,-1.58802,0.038833,1.69997,-1.62039,0.102608,1.63697,-1.62121,0.253293,1.97442,-2.06163,0.302636,1.88254,-1.89494,0.10682,1.92665,-2.00712,0.145999,1.75636,-2.19924,0.200938,1.67549,-2.23489,0.337789,1.69396,-2.09322,0.40033,1.69227,-2.07983,0.186174,1.46142,-1.79267,0.002694,1.53489,-1.85413,0.007515,1.55484,-1.91697,0.008428,1.53035,-1.96838,0.182374,1.43781,-2.05071,0.315389,1.25479,-1 [...]
+/*926*/{0.192517,1.94508,-1.68768,0.286163,1.88305,-1.81961,0.03661,1.8659,-1.62091,-0.090591,1.80051,-1.59026,0.03612,1.70406,-1.62168,0.100259,1.63971,-1.62044,0.253297,1.97743,-2.06074,0.301913,1.8852,-1.8942,0.107275,1.92926,-2.00748,0.148311,1.76108,-2.20117,0.201977,1.68005,-2.23634,0.337782,1.69693,-2.09304,0.399925,1.69601,-2.07815,0.185544,1.46547,-1.79344,0.004285,1.53851,-1.85504,0.008773,1.55803,-1.91783,0.009824,1.53444,-1.96912,0.183424,1.44054,-2.05027,0.313933,1.25576,-1. [...]
+/*927*/{0.191783,1.94756,-1.68777,0.285843,1.88524,-1.81921,0.03549,1.86964,-1.62212,-0.091416,1.80516,-1.59259,0.033569,1.70626,-1.62201,0.097751,1.6424,-1.61959,0.254224,1.98079,-2.06032,0.303064,1.88852,-1.89344,0.108929,1.93217,-2.00838,0.150409,1.76485,-2.20192,0.203373,1.68385,-2.2374,0.337651,1.70002,-2.0931,0.39931,1.69849,-2.07881,0.186353,1.46947,-1.79403,0.006602,1.54166,-1.85496,0.009887,1.56103,-1.91804,0.011803,1.53761,-1.96938,0.184108,1.44304,-2.05005,0.31293,1.25529,-1.7 [...]
+/*928*/{0.191418,1.95026,-1.6881,0.285943,1.88903,-1.8192,0.034083,1.87281,-1.62412,-0.094816,1.80781,-1.59522,0.032375,1.70898,-1.62199,0.096177,1.64416,-1.6189,0.254261,1.98305,-2.05987,0.302585,1.89107,-1.89322,0.109493,1.93471,-2.00814,0.151803,1.76908,-2.20332,0.203712,1.68714,-2.23889,0.337325,1.7034,-2.09445,0.398764,1.70005,-2.07817,0.186424,1.47246,-1.79488,0.005829,1.54521,-1.85554,0.010368,1.56416,-1.91863,0.012791,1.54076,-1.96961,0.185497,1.44588,-2.0496,0.311205,1.25206,-1. [...]
+/*929*/{0.190783,1.95291,-1.68796,0.285696,1.89142,-1.81855,0.03263,1.87555,-1.62483,-0.095485,1.81057,-1.59741,0.029895,1.71101,-1.62254,0.093615,1.64555,-1.61774,0.255008,1.98525,-2.05963,0.302305,1.8938,-1.89287,0.109641,1.93681,-2.0086,0.153027,1.77259,-2.20399,0.203002,1.6904,-2.24039,0.336969,1.70483,-2.09471,0.398243,1.70116,-2.07934,0.187298,1.4758,-1.79529,0.007284,1.54792,-1.85568,0.011975,1.56726,-1.91851,0.013873,1.54385,-1.96971,0.186716,1.44897,-2.04928,0.308105,1.25118,-1. [...]
+/*930*/{0.190401,1.95483,-1.68865,0.285697,1.8926,-1.81821,0.032006,1.87814,-1.62633,-0.097708,1.81335,-1.59934,0.027995,1.71318,-1.62261,0.092462,1.64763,-1.61618,0.254907,1.98748,-2.05959,0.302598,1.89577,-1.89239,0.110578,1.9386,-2.00944,0.152932,1.77538,-2.20459,0.202723,1.69287,-2.24151,0.335247,1.70723,-2.09584,0.397468,1.70139,-2.07955,0.187506,1.47792,-1.7953,0.008223,1.55036,-1.85599,0.01212,1.57022,-1.91768,0.015265,1.54614,-1.96986,0.188116,1.45146,-2.04882,0.30759,1.24755,-1. [...]
+/*931*/{0.189905,1.957,-1.68887,0.286403,1.89344,-1.81764,0.031395,1.88,-1.62717,-0.099387,1.81533,-1.60184,0.027328,1.71417,-1.62275,0.09083,1.64795,-1.61547,0.255082,1.98864,-2.05914,0.302578,1.89795,-1.89229,0.110581,1.94165,-2.0097,0.152232,1.77859,-2.2048,0.201717,1.69495,-2.24269,0.334421,1.70633,-2.09674,0.396958,1.70168,-2.08027,0.188104,1.48061,-1.79558,0.008273,1.5525,-1.85548,0.012753,1.57289,-1.91797,0.015674,1.54849,-1.96963,0.188384,1.45416,-2.04873,0.30291,1.2443,-1.73977, [...]
+/*932*/{0.189744,1.95889,-1.68869,0.286707,1.89454,-1.81778,0.030914,1.88195,-1.62816,-0.099275,1.81698,-1.60357,0.026255,1.71569,-1.62298,0.088974,1.64835,-1.61416,0.255123,1.98956,-2.05922,0.302645,1.89931,-1.89197,0.111513,1.94314,-2.00935,0.151006,1.78159,-2.20462,0.199798,1.6969,-2.24323,0.333937,1.70623,-2.09793,0.395984,1.69993,-2.08138,0.188081,1.48271,-1.79642,0.008205,1.55359,-1.85617,0.013465,1.574,-1.91729,0.015896,1.5506,-1.96958,0.189449,1.45714,-2.04867,0.299643,1.24011,-1 [...]
+/*933*/{0.189584,1.96005,-1.68896,0.286265,1.89602,-1.81764,0.029808,1.88307,-1.62912,-0.099415,1.81831,-1.60509,0.024851,1.71609,-1.62269,0.087807,1.64829,-1.61265,0.254935,1.99063,-2.059,0.302098,1.90106,-1.89211,0.111177,1.94479,-2.00935,0.149688,1.78288,-2.20414,0.19768,1.69807,-2.24371,0.333658,1.70548,-2.0991,0.395169,1.69813,-2.08231,0.189428,1.48429,-1.79691,0.00869,1.55483,-1.85554,0.013312,1.57684,-1.91621,0.016524,1.55218,-1.96905,0.190239,1.46003,-2.04821,0.29721,1.23412,-1.7 [...]
+/*934*/{0.189657,1.96166,-1.68899,0.285541,1.89826,-1.81833,0.029761,1.88395,-1.62987,-0.099941,1.81958,-1.60731,0.02353,1.71635,-1.62223,0.08624,1.64821,-1.60998,0.254331,1.99069,-2.05894,0.301725,1.90181,-1.89192,0.111206,1.94544,-2.00995,0.147576,1.78445,-2.20395,0.195365,1.69934,-2.24419,0.331614,1.70381,-2.10103,0.393714,1.69611,-2.08214,0.190933,1.48544,-1.79713,0.009593,1.55574,-1.85453,0.013058,1.57796,-1.9164,0.017106,1.55323,-1.96843,0.190987,1.46255,-2.04803,0.293814,1.22933,- [...]
+/*935*/{0.189575,1.9623,-1.6891,0.285374,1.89885,-1.818,0.028991,1.88505,-1.63049,-0.100604,1.81958,-1.60884,0.023326,1.7162,-1.62159,0.085204,1.6471,-1.60886,0.254061,1.99065,-2.05901,0.301522,1.90341,-1.89182,0.110149,1.94732,-2.00861,0.14366,1.78604,-2.204,0.192563,1.69986,-2.2447,0.330366,1.70243,-2.10209,0.391062,1.69443,-2.08803,0.190445,1.48649,-1.79755,0.010661,1.5555,-1.85406,0.013774,1.57847,-1.91532,0.018313,1.55383,-1.96765,0.192092,1.46435,-2.04752,0.290587,1.22383,-1.73804, [...]
+/*936*/{0.189814,1.96267,-1.6889,0.284999,1.89969,-1.81793,0.028987,1.88514,-1.63102,-0.100459,1.81976,-1.60957,0.023148,1.71596,-1.62071,0.083696,1.64599,-1.60674,0.252935,1.99019,-2.05929,0.301094,1.90388,-1.89204,0.109648,1.94792,-2.00797,0.140681,1.78647,-2.20327,0.188829,1.70038,-2.24465,0.328223,1.69975,-2.10398,0.389447,1.69079,-2.09052,0.191704,1.48717,-1.79759,0.010688,1.55472,-1.85364,0.013494,1.57893,-1.91451,0.018054,1.55435,-1.96679,0.192997,1.46646,-2.04729,0.286361,1.21742 [...]
+/*937*/{0.189797,1.96293,-1.68903,0.285215,1.89928,-1.81825,0.02901,1.88491,-1.63123,-0.099851,1.81949,-1.61091,0.022351,1.714,-1.61992,0.082988,1.64425,-1.60476,0.252379,1.98987,-2.05945,0.300147,1.90402,-1.89201,0.108879,1.94836,-2.00765,0.136103,1.78784,-2.20159,0.185639,1.70046,-2.24455,0.326189,1.69632,-2.10533,0.386973,1.68636,-2.09253,0.191579,1.48733,-1.79811,0.010211,1.55441,-1.85264,0.01342,1.57889,-1.91411,0.017819,1.55441,-1.96609,0.194031,1.46771,-2.04708,0.283235,1.2117,-1. [...]
+/*938*/{0.19014,1.96278,-1.68885,0.284437,1.89939,-1.81851,0.028979,1.88383,-1.63163,-0.099427,1.81857,-1.61143,0.021833,1.71192,-1.61813,0.082608,1.64242,-1.60258,0.249814,1.98842,-2.05936,0.298881,1.90368,-1.89222,0.108007,1.9486,-2.00742,0.132631,1.78746,-2.20058,0.181213,1.70022,-2.24448,0.324837,1.69426,-2.10743,0.384982,1.68173,-2.09454,0.191812,1.48667,-1.79852,0.010371,1.55279,-1.85143,0.013465,1.57785,-1.91303,0.017513,1.5539,-1.96573,0.194244,1.46901,-2.04681,0.280366,1.20578,- [...]
+/*939*/{0.190439,1.96203,-1.68862,0.284344,1.89909,-1.81869,0.02902,1.88286,-1.63143,-0.099205,1.81694,-1.61136,0.022319,1.71002,-1.61666,0.082038,1.6404,-1.59992,0.248968,1.98734,-2.05962,0.29817,1.90391,-1.89233,0.106831,1.9483,-2.0067,0.128026,1.7872,-2.1998,0.176195,1.69978,-2.24412,0.321326,1.68897,-2.10861,0.382042,1.6766,-2.09693,0.191852,1.48604,-1.79916,0.010374,1.5508,-1.85193,0.013117,1.57665,-1.91267,0.017326,1.5534,-1.96544,0.195494,1.47005,-2.04658,0.276026,1.19884,-1.73422 [...]
+/*940*/{0.189808,1.96103,-1.6887,0.282892,1.8994,-1.81953,0.028596,1.88187,-1.63172,-0.099154,1.8144,-1.61098,0.022393,1.70818,-1.61525,0.081818,1.63779,-1.59717,0.24709,1.98614,-2.0602,0.297458,1.90349,-1.89233,0.105423,1.94741,-2.00557,0.122279,1.78703,-2.19797,0.171323,1.69918,-2.2438,0.319589,1.68606,-2.11084,0.378905,1.67118,-2.0996,0.191517,1.48453,-1.79872,0.010829,1.54868,-1.8502,0.012974,1.57542,-1.91218,0.017749,1.55134,-1.96471,0.195981,1.47018,-2.04609,0.272209,1.1927,-1.7330 [...]
+/*941*/{0.189559,1.95964,-1.68893,0.282463,1.89862,-1.81961,0.029012,1.87962,-1.63132,-0.09755,1.81181,-1.61059,0.022934,1.70535,-1.61298,0.08223,1.63509,-1.59497,0.244129,1.98374,-2.06036,0.296218,1.90227,-1.89285,0.103857,1.94499,-2.0046,0.117733,1.78582,-2.19696,0.166129,1.69806,-2.24363,0.315908,1.68085,-2.11283,0.375711,1.66536,-2.10249,0.192083,1.48301,-1.79837,0.010722,1.54633,-1.84986,0.012893,1.57352,-1.91132,0.017252,1.54993,-1.96413,0.196126,1.47019,-2.04604,0.2698,1.1873,-1.7 [...]
+/*942*/{0.190254,1.95824,-1.68877,0.282047,1.89632,-1.81911,0.029076,1.87771,-1.63106,-0.096716,1.80895,-1.60979,0.023014,1.70201,-1.61097,0.082874,1.63178,-1.5926,0.242099,1.982,-2.06126,0.295204,1.90115,-1.89311,0.102616,1.94396,-2.00377,0.111783,1.78482,-2.19518,0.161134,1.69698,-2.24324,0.311892,1.67551,-2.11508,0.372047,1.65932,-2.10477,0.192708,1.48016,-1.79798,0.011013,1.54282,-1.84937,0.012645,1.57108,-1.91153,0.017091,1.54714,-1.96403,0.197496,1.46939,-2.04541,0.266963,1.18159,- [...]
+/*943*/{0.189856,1.95616,-1.68872,0.280913,1.89622,-1.81997,0.029151,1.87489,-1.63071,-0.095908,1.80533,-1.6089,0.024501,1.69859,-1.60923,0.084838,1.62848,-1.58975,0.240953,1.97984,-2.06169,0.294313,1.89926,-1.89347,0.100115,1.94165,-2.00283,0.106195,1.78389,-2.19381,0.155066,1.69543,-2.24272,0.309033,1.67124,-2.11732,0.368022,1.65192,-2.10835,0.19342,1.47772,-1.79789,0.010539,1.54018,-1.84997,0.013469,1.56768,-1.91041,0.017638,1.54449,-1.9635,0.197734,1.46821,-2.04498,0.264299,1.17628,- [...]
+/*944*/{0.189602,1.95364,-1.68879,0.279867,1.89398,-1.82057,0.029406,1.87161,-1.62971,-0.096419,1.80046,-1.6066,0.025359,1.69463,-1.60676,0.086202,1.62444,-1.58681,0.238428,1.97701,-2.06184,0.292876,1.89784,-1.89397,0.09821,1.93935,-2.00135,0.101911,1.78011,-2.19258,0.149228,1.69339,-2.24206,0.304652,1.66485,-2.11976,0.36402,1.64526,-2.11202,0.192982,1.47427,-1.79803,0.010291,1.53657,-1.849,0.012953,1.56449,-1.90995,0.017432,1.54194,-1.96389,0.198636,1.46727,-2.04482,0.261587,1.17284,-1. [...]
+/*945*/{0.188844,1.95054,-1.68896,0.2797,1.89296,-1.82098,0.029424,1.86755,-1.62895,-0.094084,1.79662,-1.60487,0.026825,1.69052,-1.60334,0.087934,1.62149,-1.58436,0.236412,1.97468,-2.06273,0.292149,1.89521,-1.89416,0.097229,1.93645,-2.00039,0.095517,1.77978,-2.19103,0.143538,1.69155,-2.24171,0.299482,1.65893,-2.1221,0.359619,1.63734,-2.11561,0.19327,1.47144,-1.79746,0.011319,1.53333,-1.84868,0.012902,1.56097,-1.90965,0.016805,1.53808,-1.96368,0.199724,1.46503,-2.04442,0.258613,1.16898,-1 [...]
+/*946*/{0.188486,1.94745,-1.68871,0.277872,1.89017,-1.82223,0.030139,1.86311,-1.62778,-0.092944,1.79079,-1.6023,0.029011,1.68685,-1.60086,0.090614,1.61773,-1.58175,0.233518,1.97102,-2.06362,0.291234,1.89333,-1.89485,0.094922,1.93328,-1.99979,0.091186,1.77579,-2.18927,0.137676,1.68906,-2.24152,0.294528,1.65147,-2.12466,0.354367,1.62933,-2.11918,0.1929,1.46812,-1.79676,0.010703,1.52949,-1.84861,0.012265,1.55818,-1.90962,0.016627,1.5342,-1.96351,0.200051,1.46395,-2.04372,0.255578,1.16645,-1 [...]
+/*947*/{0.188088,1.94358,-1.68871,0.276208,1.88674,-1.82233,0.029993,1.85897,-1.62646,-0.0926,1.78503,-1.59967,0.030899,1.68169,-1.59814,0.093247,1.61313,-1.57852,0.231525,1.96768,-2.06439,0.29054,1.88988,-1.89523,0.093398,1.9296,-1.99898,0.086225,1.77292,-2.18823,0.131088,1.68618,-2.24067,0.289952,1.64447,-2.12656,0.349787,1.62089,-2.12301,0.192681,1.46501,-1.7963,0.010017,1.52554,-1.84854,0.011814,1.55366,-1.9092,0.016642,1.53055,-1.96439,0.200283,1.46143,-2.04294,0.255063,1.16182,-1.7 [...]
+/*948*/{0.187635,1.93989,-1.68816,0.275632,1.88377,-1.82273,0.030511,1.85344,-1.62463,-0.090192,1.77889,-1.5966,0.033906,1.67659,-1.59518,0.096719,1.60905,-1.57588,0.228955,1.96382,-2.06527,0.289226,1.88666,-1.89562,0.091871,1.92606,-1.99739,0.079686,1.77034,-2.18698,0.125332,1.68324,-2.23999,0.284536,1.63756,-2.12909,0.34401,1.61212,-2.12694,0.192104,1.46216,-1.79561,0.009889,1.52107,-1.84817,0.011651,1.54957,-1.90914,0.016798,1.5254,-1.96441,0.200908,1.45897,-2.04298,0.253395,1.15841,- [...]
+/*949*/{0.186713,1.93558,-1.68842,0.275438,1.8805,-1.82275,0.03136,1.84773,-1.62303,-0.088992,1.77247,-1.59348,0.035748,1.67048,-1.59088,0.100844,1.60463,-1.57355,0.226565,1.96004,-2.06589,0.287814,1.88299,-1.89669,0.089408,1.92177,-1.99678,0.074813,1.76801,-2.18631,0.119306,1.68037,-2.23978,0.279413,1.62995,-2.132,0.338018,1.60312,-2.13088,0.192042,1.45825,-1.79488,0.008859,1.51661,-1.84847,0.012117,1.54513,-1.90851,0.015913,1.52096,-1.96452,0.200647,1.4566,-2.04201,0.251502,1.15481,-1. [...]
+/*950*/{0.186803,1.93086,-1.68794,0.273818,1.87598,-1.82341,0.032217,1.84117,-1.62155,-0.086127,1.76567,-1.58985,0.038673,1.6651,-1.58782,0.104579,1.59934,-1.57058,0.224318,1.95608,-2.06733,0.286928,1.87902,-1.89746,0.088054,1.91754,-1.99629,0.069298,1.76291,-2.18503,0.11207,1.6766,-2.23958,0.273376,1.62222,-2.13478,0.332177,1.59355,-2.13487,0.19188,1.45504,-1.79422,0.008737,1.51178,-1.84837,0.010894,1.54107,-1.90907,0.01576,1.51607,-1.96521,0.200428,1.4533,-2.04133,0.250543,1.15077,-1.7 [...]
+/*951*/{0.185732,1.92594,-1.68776,0.273737,1.87158,-1.8239,0.033004,1.83455,-1.61873,-0.085305,1.75771,-1.58665,0.041599,1.65849,-1.58472,0.108236,1.59426,-1.56835,0.22173,1.95096,-2.0684,0.285523,1.87476,-1.89794,0.086022,1.9128,-1.99561,0.063188,1.76113,-2.18436,0.106274,1.6726,-2.23936,0.267534,1.61425,-2.13752,0.325756,1.58423,-2.13902,0.19165,1.45155,-1.793,0.00796,1.50696,-1.84879,0.010933,1.53672,-1.90881,0.014702,1.51117,-1.96495,0.200815,1.4507,-2.0408,0.250062,1.14657,-1.72339, [...]
+/*952*/{0.185677,1.92079,-1.68701,0.274088,1.86616,-1.8241,0.034315,1.8274,-1.61684,-0.083187,1.75026,-1.58269,0.0459,1.65191,-1.58162,0.112469,1.58919,-1.56537,0.219514,1.94613,-2.06906,0.284601,1.87069,-1.89881,0.0846,1.90803,-1.99482,0.057166,1.75694,-2.18353,0.099711,1.66878,-2.23878,0.261044,1.60595,-2.14051,0.319251,1.57381,-2.14324,0.191545,1.44723,-1.79097,0.007277,1.50186,-1.84965,0.009287,1.53141,-1.90841,0.013292,1.50601,-1.96633,0.200391,1.44734,-2.03972,0.247793,1.14163,-1.7 [...]
+/*953*/{0.185061,1.91543,-1.68699,0.272478,1.86202,-1.82504,0.035238,1.81968,-1.61474,-0.081613,1.74124,-1.57896,0.049852,1.64484,-1.57807,0.11762,1.58335,-1.56299,0.217367,1.94143,-2.07025,0.284305,1.86607,-1.89938,0.082636,1.90318,-1.99412,0.052608,1.75332,-2.18286,0.092867,1.6645,-2.2384,0.254735,1.59647,-2.14313,0.31245,1.56374,-2.1473,0.191078,1.44336,-1.78973,0.006172,1.49508,-1.84985,0.009313,1.52607,-1.90813,0.013032,1.50061,-1.96602,0.19945,1.44351,-2.03889,0.245728,1.13748,-1.7 [...]
+/*954*/{0.18482,1.9101,-1.68644,0.272001,1.85754,-1.82548,0.037305,1.81201,-1.61241,-0.078717,1.73217,-1.5751,0.055049,1.63824,-1.57513,0.123106,1.57733,-1.55917,0.214197,1.93595,-2.0707,0.283353,1.86109,-1.89982,0.08078,1.89752,-1.99329,0.048717,1.74776,-2.18186,0.086406,1.66021,-2.23832,0.24865,1.58676,-2.14552,0.304719,1.55301,-2.15159,0.190939,1.44012,-1.78857,0.00491,1.48995,-1.85116,0.008325,1.52107,-1.90832,0.012414,1.4945,-1.96609,0.199228,1.43969,-2.03833,0.244079,1.13242,-1.723 [...]
+/*955*/{0.184912,1.9042,-1.68601,0.27225,1.85073,-1.8252,0.039462,1.80346,-1.60964,-0.074007,1.72339,-1.57098,0.059077,1.63044,-1.5722,0.128989,1.57138,-1.55751,0.212669,1.93062,-2.07175,0.282312,1.85559,-1.90088,0.079262,1.8919,-1.99266,0.042733,1.74341,-2.18151,0.079263,1.65572,-2.23778,0.241552,1.57772,-2.1491,0.297488,1.54202,-2.15584,0.190533,1.43609,-1.7871,0.004315,1.48394,-1.85102,0.006899,1.51433,-1.90783,0.011402,1.48847,-1.96691,0.198967,1.43602,-2.03751,0.242521,1.12717,-1.72 [...]
+/*956*/{0.184546,1.89782,-1.68565,0.271863,1.84627,-1.82644,0.04181,1.79505,-1.60741,-0.071186,1.7137,-1.56672,0.064819,1.62316,-1.56842,0.136222,1.56552,-1.55406,0.209638,1.92488,-2.07228,0.281761,1.85061,-1.90151,0.077291,1.88675,-1.99211,0.038895,1.73931,-2.18054,0.073276,1.65137,-2.23759,0.235485,1.56844,-2.15225,0.289551,1.53119,-2.16053,0.190336,1.43162,-1.78541,0.003404,1.47843,-1.8516,0.00676,1.5092,-1.90747,0.009947,1.4823,-1.96742,0.198821,1.43192,-2.03735,0.242369,1.12178,-1.7 [...]
+/*957*/{0.184892,1.89189,-1.68507,0.271926,1.84164,-1.8271,0.04422,1.78623,-1.60468,-0.066861,1.70358,-1.56196,0.070325,1.61538,-1.56638,0.143112,1.55943,-1.5522,0.20697,1.9198,-2.07314,0.281343,1.84482,-1.90219,0.075838,1.88113,-1.99184,0.033894,1.73512,-2.17952,0.066223,1.64627,-2.23682,0.22786,1.55796,-2.15546,0.281478,1.52022,-2.16469,0.19024,1.42757,-1.78368,0.001113,1.47263,-1.85262,0.005846,1.50268,-1.90721,0.008605,1.47684,-1.96793,0.198262,1.42678,-2.03655,0.239102,1.11534,-1.72 [...]
+/*958*/{0.185224,1.88552,-1.6848,0.27129,1.83541,-1.82783,0.047323,1.77705,-1.60208,-0.060777,1.69372,-1.55752,0.07654,1.60783,-1.5632,0.1501,1.55404,-1.54987,0.205396,1.91449,-2.07425,0.281004,1.83984,-1.90408,0.074304,1.87586,-1.99059,0.029116,1.73122,-2.17962,0.059566,1.64174,-2.23624,0.220985,1.54846,-2.15853,0.273001,1.50893,-2.16903,0.189652,1.42206,-1.78202,-0.000185,1.46686,-1.8535,0.004463,1.49603,-1.90709,0.007295,1.46999,-1.96857,0.197334,1.42207,-2.03694,0.236614,1.11126,-1.7 [...]
+/*959*/{0.185471,1.8798,-1.68425,0.27097,1.82954,-1.8282,0.050795,1.7677,-1.59938,-0.054628,1.68353,-1.55298,0.084021,1.60033,-1.56008,0.157333,1.54771,-1.54767,0.203203,1.90944,-2.0746,0.280446,1.83437,-1.90487,0.072135,1.87139,-1.98948,0.025775,1.72706,-2.17869,0.052918,1.63763,-2.23568,0.213631,1.53939,-2.16177,0.264279,1.49812,-2.1737,0.187516,1.41585,-1.78059,-0.002551,1.46198,-1.85392,0.002316,1.49002,-1.90843,0.005059,1.46473,-1.96848,0.196419,1.41672,-2.0363,0.235479,1.10816,-1.7 [...]
+/*960*/{0.186515,1.87418,-1.68367,0.271534,1.8238,-1.8291,0.054801,1.75821,-1.59678,-0.049318,1.67259,-1.5479,0.091032,1.5933,-1.55719,0.166063,1.54245,-1.54541,0.201963,1.90457,-2.07553,0.280688,1.82887,-1.90601,0.070745,1.86646,-1.98789,0.019941,1.7229,-2.17805,0.046887,1.63297,-2.23497,0.205754,1.52955,-2.16513,0.254726,1.48731,-2.17806,0.185591,1.41041,-1.78089,-0.005918,1.45737,-1.85408,-0.00074,1.48417,-1.91012,0.001591,1.45876,-1.96985,0.195988,1.41199,-2.03596,0.230705,1.10583,-1 [...]
+/*961*/{0.186967,1.86989,-1.68281,0.272073,1.81864,-1.83001,0.059633,1.74833,-1.59332,-0.042104,1.66249,-1.54349,0.100068,1.58642,-1.55483,0.175854,1.53745,-1.54357,0.199587,1.9003,-2.07674,0.279842,1.82384,-1.90831,0.069274,1.8627,-1.98615,0.017919,1.71771,-2.17745,0.040855,1.62882,-2.2341,0.198806,1.52128,-2.16887,0.24603,1.47741,-2.18226,0.182623,1.40575,-1.77984,-0.008965,1.45316,-1.85413,-0.004055,1.4785,-1.91118,-0.000927,1.45348,-1.97083,0.194651,1.4066,-2.03605,0.226926,1.10373,- [...]
+/*962*/{0.187985,1.8642,-1.68199,0.272149,1.81364,-1.83147,0.064961,1.73919,-1.59091,-0.033272,1.65203,-1.53856,0.108787,1.57908,-1.55121,0.18518,1.5332,-1.54208,0.197301,1.89616,-2.07817,0.281837,1.81857,-1.90904,0.067849,1.86029,-1.98391,0.013757,1.71445,-2.17671,0.034345,1.62537,-2.23282,0.189779,1.51149,-2.1718,0.236625,1.46807,-2.1868,0.180111,1.40172,-1.77927,-0.012243,1.44928,-1.85577,-0.007311,1.47283,-1.91241,-0.004059,1.44812,-1.97218,0.193087,1.40172,-2.03557,0.225164,1.10285, [...]
+/*963*/{0.188625,1.85994,-1.68114,0.271642,1.80867,-1.83289,0.070859,1.7304,-1.58874,-0.02495,1.64123,-1.53361,0.118572,1.57318,-1.54932,0.19575,1.5293,-1.54067,0.195549,1.89274,-2.07943,0.279653,1.81504,-1.91194,0.067115,1.85816,-1.98196,0.010731,1.71174,-2.17513,0.028411,1.62214,-2.23193,0.182022,1.50409,-2.17512,0.226707,1.45892,-2.19118,0.177682,1.39821,-1.77889,-0.015754,1.44606,-1.85724,-0.009176,1.46965,-1.91296,-0.00736,1.44225,-1.97226,0.19056,1.39699,-2.03543,0.225466,1.10463,- [...]
+/*964*/{0.189595,1.85624,-1.6809,0.271562,1.8053,-1.83476,0.0767,1.72287,-1.58696,-0.014549,1.63242,-1.52945,0.128563,1.56883,-1.54874,0.207497,1.527,-1.54012,0.194646,1.88973,-2.08108,0.279123,1.81136,-1.91328,0.066789,1.85746,-1.98129,0.005123,1.70975,-2.17257,0.022387,1.61992,-2.2301,0.174032,1.49796,-2.17789,0.216969,1.45144,-2.19471,0.175225,1.39621,-1.77739,-0.017948,1.44266,-1.85859,-0.011038,1.46864,-1.9138,-0.010452,1.43757,-1.97304,0.188873,1.39271,-2.03553,0.221311,1.10336,-1. [...]
+/*965*/{0.190437,1.85294,-1.6811,0.270857,1.80187,-1.83592,0.082356,1.71666,-1.58592,-0.005399,1.62458,-1.5264,0.139363,1.56502,-1.54772,0.219451,1.52493,-1.5392,0.194832,1.88715,-2.0818,0.278346,1.80853,-1.91491,0.065714,1.85599,-1.98218,0.000232,1.70911,-2.16859,0.015831,1.61969,-2.22812,0.165494,1.49218,-2.18014,0.206468,1.44502,-2.19825,0.172401,1.39382,-1.77603,-0.020411,1.44151,-1.85965,-0.013201,1.46815,-1.9143,-0.013043,1.43415,-1.97451,0.185579,1.38937,-2.03603,0.217911,1.10249, [...]
+/*966*/{0.191059,1.85087,-1.68156,0.270459,1.79796,-1.83596,0.087699,1.71216,-1.58588,0.003868,1.61766,-1.52354,0.151388,1.56223,-1.54653,0.231944,1.52395,-1.53965,0.194254,1.88535,-2.08207,0.277175,1.80647,-1.91555,0.065815,1.8546,-1.98199,-0.003819,1.70948,-2.16533,0.00916,1.62113,-2.22622,0.157064,1.48801,-2.18235,0.19672,1.43903,-2.20132,0.169786,1.3914,-1.77385,-0.02268,1.44024,-1.86071,-0.01507,1.46721,-1.91466,-0.014675,1.43243,-1.97567,0.183664,1.38642,-2.03688,0.213653,1.10085,- [...]
+/*967*/{0.191818,1.84906,-1.68241,0.26966,1.79729,-1.8375,0.091837,1.70924,-1.58491,0.013738,1.61234,-1.52101,0.162725,1.56045,-1.54655,0.244256,1.5247,-1.5398,0.193649,1.88339,-2.08215,0.276226,1.80535,-1.91567,0.064833,1.85317,-1.98309,-0.008555,1.71012,-2.15931,0.002418,1.62278,-2.22419,0.148141,1.48505,-2.18381,0.186107,1.43569,-2.20398,0.167926,1.39027,-1.77296,-0.023417,1.43879,-1.8611,-0.016239,1.46581,-1.91501,-0.016087,1.43134,-1.97531,0.182608,1.38502,-2.03751,0.209212,1.09827, [...]
+/*968*/{0.192203,1.84804,-1.68325,0.267126,1.79557,-1.83808,0.095687,1.70673,-1.58501,0.023849,1.60869,-1.51954,0.173821,1.55923,-1.54707,0.257064,1.52624,-1.54112,0.192574,1.88253,-2.08205,0.275142,1.80381,-1.91637,0.063772,1.85299,-1.98379,-0.012054,1.7123,-2.15337,-0.004382,1.62548,-2.22233,0.139524,1.48341,-2.18483,0.176248,1.43287,-2.20583,0.165439,1.38945,-1.77133,-0.024192,1.43871,-1.8611,-0.018333,1.46476,-1.91579,-0.017181,1.43066,-1.97559,0.180906,1.38401,-2.03763,0.202408,1.09 [...]
+/*969*/{0.192836,1.84679,-1.68423,0.265989,1.7947,-1.83831,0.100307,1.70582,-1.58538,0.032363,1.60498,-1.51795,0.186113,1.55938,-1.54735,0.270249,1.52896,-1.54369,0.191213,1.88207,-2.08227,0.273632,1.80264,-1.91627,0.062684,1.85323,-1.98397,-0.016439,1.71523,-2.14839,-0.010418,1.62897,-2.22058,0.131436,1.48328,-2.18574,0.1667,1.4316,-2.20706,0.164168,1.38901,-1.77047,-0.026375,1.43822,-1.86174,-0.018775,1.46453,-1.9152,-0.018804,1.4309,-1.97506,0.17941,1.38467,-2.03846,0.198204,1.09077,- [...]
+/*970*/{0.193386,1.84692,-1.68481,0.264596,1.79438,-1.83879,0.103985,1.70481,-1.58492,0.041525,1.60162,-1.51581,0.197085,1.56074,-1.54796,0.281663,1.53213,-1.54602,0.189441,1.88226,-2.08263,0.272703,1.80251,-1.91737,0.061178,1.85339,-1.98422,-0.020423,1.71975,-2.14341,-0.016877,1.63339,-2.21924,0.122977,1.48383,-2.18662,0.156931,1.43172,-2.20815,0.161612,1.38874,-1.7692,-0.026833,1.43778,-1.86155,-0.019461,1.46344,-1.91536,-0.019942,1.43084,-1.97382,0.179133,1.38603,-2.03901,0.189892,1.0 [...]
+/*971*/{0.194279,1.84768,-1.68566,0.265459,1.79529,-1.84079,0.108338,1.70424,-1.58461,0.051203,1.59946,-1.51519,0.206649,1.56226,-1.54979,0.294633,1.53689,-1.5497,0.188126,1.88285,-2.08266,0.271392,1.80251,-1.91801,0.059456,1.85416,-1.98442,-0.024282,1.72459,-2.13745,-0.02385,1.63791,-2.21768,0.115163,1.48592,-2.18683,0.147953,1.43271,-2.20899,0.161118,1.38886,-1.76838,-0.027076,1.43676,-1.86109,-0.020735,1.46335,-1.91484,-0.020517,1.43133,-1.97273,0.178313,1.3885,-2.03899,0.183362,1.083 [...]
+/*972*/{0.19426,1.84876,-1.6864,0.264075,1.79532,-1.8411,0.111693,1.70441,-1.58466,0.061622,1.59743,-1.51313,0.217665,1.56533,-1.55177,0.305922,1.54237,-1.55363,0.185476,1.88428,-2.08295,0.270104,1.80297,-1.91896,0.058271,1.85499,-1.98442,-0.027081,1.72881,-2.13292,-0.029928,1.64378,-2.21661,0.107118,1.48908,-2.18743,0.138819,1.43514,-2.20927,0.160014,1.38998,-1.76789,-0.027103,1.43659,-1.8612,-0.020997,1.46354,-1.9143,-0.02067,1.43144,-1.97255,0.177914,1.39146,-2.03897,0.172119,1.07887, [...]
+/*973*/{0.195045,1.85136,-1.6871,0.263008,1.7974,-1.84224,0.115989,1.7057,-1.58498,0.070614,1.59586,-1.51269,0.227505,1.56879,-1.55378,0.317025,1.54902,-1.55832,0.183175,1.88558,-2.08339,0.268906,1.80451,-1.92037,0.057267,1.85686,-1.9843,-0.032451,1.73411,-2.12937,-0.035752,1.64948,-2.21529,0.099271,1.49232,-2.18789,0.12998,1.43835,-2.20947,0.159549,1.39034,-1.76721,-0.028959,1.43744,-1.86188,-0.02142,1.46414,-1.91437,-0.021148,1.43242,-1.97306,0.177299,1.3936,-2.03881,0.164279,1.07941,- [...]
+/*974*/{0.196537,1.85277,-1.68815,0.263066,1.79904,-1.84337,0.121182,1.70658,-1.58495,0.080371,1.59493,-1.51168,0.237278,1.57307,-1.55679,0.32703,1.55611,-1.56396,0.180248,1.88765,-2.08363,0.268033,1.80597,-1.92083,0.055486,1.8579,-1.98333,-0.035999,1.73952,-2.12652,-0.042038,1.65563,-2.21392,0.09206,1.49713,-2.18728,0.12191,1.44368,-2.2095,0.158501,1.39059,-1.76616,-0.027922,1.43845,-1.8617,-0.021088,1.46536,-1.91449,-0.020908,1.43395,-1.97418,0.17739,1.39671,-2.03845,0.156102,1.07776,- [...]
+/*975*/{0.198107,1.8554,-1.68931,0.262023,1.80103,-1.84462,0.125859,1.70764,-1.58564,0.090875,1.59484,-1.51119,0.245903,1.5781,-1.56053,0.336841,1.56392,-1.56936,0.17778,1.88989,-2.08355,0.266177,1.80815,-1.9222,0.053939,1.8605,-1.98231,-0.040621,1.74449,-2.12601,-0.048396,1.66198,-2.21302,0.083842,1.50106,-2.18655,0.113656,1.44863,-2.20896,0.158766,1.39103,-1.76516,-0.027129,1.43985,-1.86176,-0.020611,1.46604,-1.91431,-0.021014,1.43447,-1.97421,0.177388,1.39959,-2.03851,0.145574,1.07448 [...]
+/*976*/{0.199574,1.85856,-1.69003,0.262076,1.8042,-1.84539,0.132068,1.70874,-1.58519,0.101128,1.59528,-1.511,0.255238,1.58375,-1.56438,0.344932,1.5723,-1.57552,0.174581,1.89265,-2.0836,0.265385,1.81135,-1.92396,0.05211,1.86164,-1.98142,-0.046723,1.75038,-2.12563,-0.054685,1.66849,-2.2123,0.076949,1.50731,-2.18606,0.106399,1.45429,-2.20779,0.158811,1.39168,-1.76507,-0.02708,1.4407,-1.86225,-0.019702,1.46718,-1.91369,-0.019841,1.43655,-1.97446,0.178228,1.402,-2.03815,0.137337,1.07444,-1.71 [...]
+/*977*/{0.200649,1.86279,-1.69139,0.26127,1.80708,-1.84716,0.138072,1.71087,-1.58547,0.109723,1.59604,-1.51105,0.262563,1.58996,-1.56893,0.353576,1.58102,-1.58214,0.171753,1.89563,-2.08392,0.264471,1.81413,-1.92505,0.050178,1.86345,-1.97991,-0.052532,1.7564,-2.12575,-0.060637,1.6741,-2.21151,0.07004,1.51292,-2.18538,0.098441,1.46003,-2.20692,0.159224,1.39261,-1.76529,-0.026625,1.44238,-1.86274,-0.020278,1.4689,-1.91327,-0.019092,1.43846,-1.97509,0.178862,1.40545,-2.03749,0.130214,1.0742, [...]
+/*978*/{0.20187,1.86603,-1.69238,0.261035,1.81097,-1.84872,0.144341,1.71323,-1.58655,0.119478,1.59729,-1.5112,0.271266,1.5956,-1.57272,0.360375,1.58996,-1.58887,0.167862,1.89895,-2.08385,0.263478,1.81798,-1.92638,0.047908,1.86542,-1.97795,-0.058397,1.76209,-2.12612,-0.066791,1.68046,-2.21067,0.06333,1.51969,-2.18443,0.091636,1.46633,-2.20564,0.160178,1.39277,-1.76465,-0.024389,1.44409,-1.86237,-0.0186,1.47059,-1.9126,-0.018026,1.44052,-1.97449,0.180048,1.40865,-2.03673,0.122737,1.07362,- [...]
+/*979*/{0.203285,1.87029,-1.69347,0.261599,1.8148,-1.85069,0.151573,1.71604,-1.58659,0.129143,1.5997,-1.51161,0.278478,1.60291,-1.57746,0.367259,1.59964,-1.59567,0.164752,1.9023,-2.08364,0.262132,1.82131,-1.92817,0.046235,1.86786,-1.97748,-0.062695,1.7689,-2.12731,-0.072832,1.68613,-2.20979,0.056974,1.52649,-2.1833,0.085201,1.4733,-2.20407,0.16104,1.39399,-1.76456,-0.024266,1.44651,-1.8627,-0.017846,1.47204,-1.91146,-0.016246,1.44326,-1.97494,0.181012,1.41203,-2.03559,0.114792,1.07505,-1 [...]
+/*980*/{0.203583,1.87442,-1.69444,0.260539,1.81764,-1.85135,0.158736,1.71872,-1.58749,0.137304,1.6016,-1.51234,0.284941,1.60976,-1.58187,0.373393,1.60915,-1.60245,0.16148,1.90625,-2.08356,0.261482,1.8251,-1.92948,0.044488,1.86953,-1.97525,-0.066582,1.77483,-2.12723,-0.079072,1.69178,-2.20873,0.050312,1.53342,-2.18158,0.078602,1.48012,-2.20259,0.162477,1.39451,-1.76424,-0.022052,1.44833,-1.86222,-0.015265,1.47441,-1.9109,-0.015152,1.44671,-1.97536,0.182691,1.41531,-2.03477,0.108678,1.0758 [...]
+/*981*/{0.205091,1.87909,-1.69587,0.260744,1.82181,-1.85281,0.165563,1.72187,-1.58826,0.14679,1.60474,-1.51298,0.290057,1.61668,-1.58652,0.378731,1.6186,-1.60878,0.158754,1.90981,-2.08315,0.260457,1.82834,-1.93156,0.042711,1.87311,-1.97519,-0.071954,1.78082,-2.12833,-0.084714,1.69757,-2.208,0.044224,1.54055,-2.17954,0.07332,1.48729,-2.20112,0.163999,1.39519,-1.76371,-0.020377,1.4517,-1.86233,-0.015414,1.47722,-1.91101,-0.013367,1.45029,-1.97544,0.184012,1.41882,-2.03374,0.102725,1.07724, [...]
+/*982*/{0.20548,1.88381,-1.69787,0.261634,1.82658,-1.85468,0.172635,1.72532,-1.58942,0.154302,1.60775,-1.51362,0.296519,1.62472,-1.59128,0.383283,1.62842,-1.61584,0.155811,1.91337,-2.08287,0.259395,1.83293,-1.93204,0.043461,1.87413,-1.97293,-0.076984,1.78722,-2.1298,-0.089959,1.70344,-2.20769,0.038437,1.54749,-2.17703,0.067536,1.49456,-2.19952,0.164618,1.39634,-1.76364,-0.019367,1.45392,-1.86214,-0.013111,1.47959,-1.90917,-0.011312,1.45395,-1.97594,0.18592,1.42241,-2.03242,0.098311,1.078 [...]
+/*983*/{0.206532,1.88931,-1.69918,0.261695,1.83106,-1.85679,0.179103,1.72913,-1.59047,0.162249,1.612,-1.51453,0.300199,1.63117,-1.59599,0.387054,1.63753,-1.62224,0.15339,1.91738,-2.08227,0.258987,1.83741,-1.93353,0.042592,1.87547,-1.97192,-0.079223,1.79371,-2.13081,-0.094573,1.70923,-2.20594,0.033079,1.55487,-2.17457,0.063686,1.50055,-2.19656,0.166538,1.3968,-1.76423,-0.017765,1.45789,-1.86197,-0.01106,1.48281,-1.90871,-0.008842,1.45834,-1.97608,0.18668,1.4253,-2.0309,0.092283,1.08115,-1 [...]
+/*984*/{0.206689,1.89347,-1.70043,0.261718,1.83532,-1.85866,0.1843,1.73355,-1.59161,0.169179,1.6158,-1.51607,0.304507,1.63874,-1.60006,0.390135,1.64662,-1.62912,0.151166,1.92121,-2.08227,0.25846,1.84133,-1.93563,0.042211,1.87876,-1.97031,-0.084204,1.79982,-2.13252,-0.09937,1.71462,-2.20411,0.027485,1.56216,-2.17276,0.058652,1.50732,-2.19509,0.167997,1.39788,-1.76537,-0.014642,1.46126,-1.86161,-0.009634,1.48658,-1.90851,-0.007612,1.46319,-1.97665,0.188513,1.42898,-2.02972,0.087291,1.0838, [...]
+/*985*/{0.207532,1.89918,-1.7018,0.262087,1.83959,-1.85962,0.189417,1.73818,-1.59358,0.176036,1.62073,-1.51698,0.307904,1.64565,-1.60427,0.393028,1.65501,-1.6371,0.149307,1.92453,-2.082,0.257921,1.84553,-1.93701,0.041387,1.88216,-1.96892,-0.08658,1.80586,-2.13363,-0.103898,1.72006,-2.20275,0.023255,1.56909,-2.17039,0.054597,1.51438,-2.19329,0.169843,1.39872,-1.76612,-0.013186,1.46506,-1.86118,-0.007902,1.49043,-1.90748,-0.005541,1.46796,-1.97635,0.189939,1.43234,-2.02822,0.08317,1.08641, [...]
+/*986*/{0.208586,1.90398,-1.7038,0.261532,1.84372,-1.86132,0.193894,1.74239,-1.59492,0.180612,1.62422,-1.51764,0.310662,1.65289,-1.60875,0.394166,1.66375,-1.64247,0.147721,1.92788,-2.08207,0.256987,1.85017,-1.93773,0.039808,1.88487,-1.9676,-0.088922,1.8113,-2.13391,-0.10798,1.72557,-2.20126,0.018375,1.57603,-2.16804,0.049454,1.52137,-2.19211,0.171718,1.40027,-1.76677,-0.011872,1.46953,-1.86103,-0.006222,1.49463,-1.90689,-0.003715,1.4732,-1.97732,0.19192,1.43579,-2.02734,0.078737,1.08737, [...]
+/*987*/{0.209723,1.90913,-1.70559,0.2626,1.84853,-1.86321,0.197997,1.74737,-1.59699,0.186995,1.62939,-1.51888,0.312415,1.65984,-1.61335,0.395552,1.6721,-1.64888,0.145919,1.9309,-2.08222,0.257184,1.85436,-1.93939,0.040045,1.88794,-1.967,-0.092029,1.81729,-2.13477,-0.112309,1.73096,-2.19884,0.014316,1.58201,-2.1656,0.045434,1.52742,-2.1901,0.172496,1.40074,-1.76786,-0.00985,1.47373,-1.86175,-0.004224,1.49873,-1.90553,-0.00138,1.47803,-1.97689,0.192533,1.4393,-2.02673,0.075153,1.09184,-1.73 [...]
+/*988*/{0.210494,1.91348,-1.70696,0.262711,1.85179,-1.86438,0.201953,1.75163,-1.59814,0.190377,1.63422,-1.52008,0.313954,1.66615,-1.61712,0.395484,1.67989,-1.65532,0.144694,1.934,-2.08209,0.256536,1.85765,-1.94131,0.040085,1.89054,-1.96771,-0.093732,1.82106,-2.1353,-0.115948,1.73644,-2.19748,0.010934,1.58796,-2.16375,0.042718,1.53373,-2.18842,0.174258,1.40238,-1.76829,-0.00753,1.47846,-1.86067,-0.002928,1.50371,-1.90502,-6.5e-005,1.48344,-1.97729,0.193756,1.44237,-2.02604,0.072442,1.0943 [...]
+/*989*/{0.211752,1.91771,-1.70836,0.263333,1.85697,-1.86666,0.204839,1.7555,-1.59874,0.195745,1.63836,-1.52099,0.314401,1.67236,-1.62084,0.396042,1.68713,-1.66064,0.143806,1.93734,-2.0822,0.256279,1.86138,-1.94242,0.040114,1.89338,-1.96702,-0.095162,1.82699,-2.13451,-0.11944,1.74126,-2.19502,0.007078,1.59317,-2.16116,0.038833,1.53927,-2.18673,0.175206,1.4049,-1.76848,-0.006637,1.48199,-1.86072,-0.000818,1.50802,-1.90409,0.002043,1.4888,-1.97717,0.192327,1.44604,-2.02528,0.069032,1.09827, [...]
+/*990*/{0.212551,1.92259,-1.70979,0.263982,1.86109,-1.86787,0.206871,1.76087,-1.59987,0.19814,1.64227,-1.5213,0.315637,1.67881,-1.62378,0.395628,1.69401,-1.66632,0.143222,1.94002,-2.08268,0.256942,1.86568,-1.94344,0.040259,1.89549,-1.96679,-0.096111,1.83197,-2.13335,-0.122111,1.74581,-2.19218,0.003391,1.59838,-2.15947,0.03574,1.54398,-2.18514,0.176983,1.40671,-1.76991,-0.005282,1.4866,-1.85947,-0.000753,1.51189,-1.90321,0.003743,1.49371,-1.9768,0.192529,1.4495,-2.02469,0.066626,1.1016,-1 [...]
+/*991*/{0.213673,1.92568,-1.71096,0.264568,1.86436,-1.86927,0.209251,1.76521,-1.60106,0.200037,1.6459,-1.5221,0.31498,1.68352,-1.62784,0.395197,1.69958,-1.67029,0.142648,1.94228,-2.0829,0.256957,1.86904,-1.94472,0.040783,1.8981,-1.96612,-0.096809,1.83586,-2.13109,-0.124406,1.75029,-2.19023,0.001496,1.60271,-2.15749,0.033686,1.54843,-2.1835,0.178245,1.40899,-1.7698,-0.003866,1.49054,-1.85963,0.000325,1.51588,-1.90213,0.005239,1.49842,-1.97601,0.192231,1.45248,-2.02466,0.063268,1.10516,-1. [...]
+/*992*/{0.215236,1.93011,-1.71225,0.266121,1.8686,-1.87071,0.210306,1.76899,-1.60196,0.203385,1.65031,-1.52265,0.31549,1.68816,-1.63014,0.393578,1.70442,-1.6752,0.142542,1.94489,-2.08314,0.25782,1.87213,-1.9455,0.040521,1.90083,-1.96663,-0.096221,1.84009,-2.12843,-0.126073,1.75394,-2.18828,-0.000445,1.60653,-2.15559,0.032013,1.55242,-2.18177,0.178955,1.4117,-1.77022,-0.002025,1.4938,-1.85938,0.001384,1.52024,-1.90157,0.006021,1.50334,-1.97547,0.192743,1.45585,-2.02422,0.06159,1.10881,-1. [...]
+/*993*/{0.215952,1.93231,-1.71358,0.266324,1.87142,-1.87193,0.210875,1.77257,-1.60242,0.203059,1.65335,-1.52386,0.314719,1.69094,-1.63303,0.392687,1.70862,-1.68011,0.14251,1.94737,-2.08379,0.258869,1.87526,-1.94662,0.041958,1.90282,-1.96648,-0.096076,1.84368,-2.1272,-0.127544,1.75738,-2.18533,-0.002139,1.6107,-2.15388,0.03056,1.55567,-2.18029,0.179724,1.4142,-1.77031,-0.000913,1.49765,-1.8585,0.001368,1.52341,-1.90107,0.006747,1.50705,-1.9746,0.193471,1.45907,-2.02445,0.060523,1.11336,-1 [...]
+/*994*/{0.218371,1.93533,-1.71457,0.267058,1.87517,-1.87307,0.211829,1.77641,-1.60298,0.204149,1.65705,-1.52418,0.313671,1.69484,-1.63622,0.390953,1.71232,-1.68399,0.142939,1.94975,-2.08446,0.259299,1.87783,-1.94738,0.041893,1.90562,-1.96637,-0.094693,1.84707,-2.12462,-0.128604,1.76101,-2.18238,-0.001683,1.61374,-2.15241,0.029594,1.55886,-2.17855,0.18062,1.41727,-1.7708,-0.000815,1.50095,-1.8584,0.002385,1.52698,-1.9007,0.007958,1.51124,-1.97431,0.192767,1.46199,-2.02395,0.060345,1.11519 [...]
+/*995*/{0.218932,1.93851,-1.7159,0.267461,1.87753,-1.87441,0.212218,1.77974,-1.60328,0.204878,1.65903,-1.52408,0.31237,1.69751,-1.637,0.388969,1.71453,-1.68787,0.143678,1.95226,-2.08482,0.259508,1.88052,-1.9482,0.042882,1.90818,-1.96597,-0.094317,1.84981,-2.12339,-0.128751,1.76408,-2.17981,-0.001384,1.61562,-2.15045,0.030506,1.55989,-2.17673,0.181894,1.42005,-1.77149,0.00024,1.50431,-1.85754,0.002761,1.53019,-1.89999,0.007727,1.51462,-1.97403,0.191688,1.4646,-2.02408,0.060636,1.11794,-1. [...]
+/*996*/{0.221218,1.94034,-1.71672,0.269079,1.88048,-1.87561,0.212515,1.78287,-1.60412,0.20556,1.66219,-1.52506,0.311455,1.6988,-1.63962,0.386793,1.71689,-1.69142,0.144366,1.9542,-2.08519,0.260719,1.88274,-1.94882,0.043875,1.91096,-1.96617,-0.093035,1.85234,-2.12114,-0.129084,1.76677,-2.17764,-0.001697,1.6171,-2.14831,0.030322,1.56259,-2.17509,0.18237,1.42308,-1.77149,0.000744,1.50721,-1.85686,0.002441,1.53319,-1.8997,0.007577,1.51798,-1.97294,0.19155,1.46753,-2.02364,0.061398,1.12077,-1. [...]
+/*997*/{0.222109,1.94209,-1.71809,0.268875,1.88268,-1.87647,0.212065,1.78546,-1.60424,0.205543,1.6642,-1.52472,0.310889,1.70096,-1.64125,0.385977,1.71752,-1.69466,0.144933,1.95579,-2.08585,0.261065,1.88467,-1.94969,0.045978,1.91291,-1.9672,-0.091338,1.85518,-2.1197,-0.128328,1.7687,-2.17522,-0.001046,1.61845,-2.14668,0.030521,1.56344,-2.17348,0.183411,1.42569,-1.77143,0.001378,1.50988,-1.85623,0.001968,1.53632,-1.8983,0.009316,1.51899,-1.97077,0.191383,1.47036,-2.02388,0.064672,1.12309,- [...]
+/*998*/{0.223483,1.94384,-1.71897,0.269652,1.88479,-1.87742,0.212144,1.78784,-1.60504,0.205364,1.66567,-1.52489,0.310186,1.70188,-1.64397,0.383801,1.71777,-1.69725,0.146028,1.95746,-2.08626,0.261961,1.88632,-1.95027,0.04698,1.91505,-1.96716,-0.090099,1.85599,-2.11722,-0.12692,1.76998,-2.17317,2.9e-005,1.62001,-2.14521,0.032367,1.56469,-2.17186,0.184201,1.42824,-1.77113,0.001865,1.51192,-1.85548,0.002327,1.53923,-1.89778,0.009333,1.52144,-1.97006,0.191698,1.47307,-2.02395,0.069532,1.12306 [...]
+/*999*/{0.224923,1.94518,-1.72021,0.270114,1.88631,-1.87884,0.211609,1.78977,-1.60543,0.204928,1.6679,-1.52604,0.30772,1.70146,-1.6453,0.382215,1.71746,-1.69948,0.148318,1.95871,-2.08738,0.2633,1.88711,-1.95096,0.047943,1.91761,-1.96832,-0.088812,1.85758,-2.11464,-0.125704,1.77074,-2.17123,0.001413,1.61998,-2.14286,0.031703,1.56649,-2.17153,0.185104,1.43079,-1.77136,0.001534,1.51385,-1.85508,0.002175,1.54111,-1.89732,0.008848,1.52284,-1.96882,0.191354,1.47515,-2.02408,0.073911,1.12273,-1 [...]
+/*1000*/{0.225577,1.94602,-1.72125,0.270293,1.88761,-1.8794,0.211281,1.79148,-1.60615,0.204113,1.66901,-1.52648,0.30623,1.70158,-1.647,0.381107,1.71617,-1.70117,0.149398,1.95917,-2.08776,0.263614,1.88777,-1.95148,0.050689,1.91894,-1.96975,-0.087733,1.85776,-2.11316,-0.124195,1.771,-2.16894,0.003353,1.62008,-2.1412,0.034391,1.56506,-2.17111,0.185642,1.43273,-1.77095,0.001607,1.51493,-1.85441,0.001792,1.54253,-1.89681,0.008117,1.52408,-1.96771,0.19066,1.47693,-2.02365,0.079656,1.12226,-1.7 [...]
+/*1001*/{0.226619,1.94663,-1.72191,0.270364,1.88817,-1.88029,0.210365,1.79225,-1.60688,0.201849,1.66982,-1.52715,0.305084,1.70037,-1.64813,0.378803,1.71331,-1.70355,0.151514,1.96019,-2.08919,0.263783,1.88852,-1.95264,0.051558,1.92098,-1.97077,-0.086374,1.85724,-2.10959,-0.122382,1.77095,-2.16834,0.005173,1.61977,-2.14037,0.03518,1.56487,-2.16965,0.185943,1.43422,-1.77116,0.001968,1.51575,-1.85228,0.001511,1.54411,-1.89667,0.008007,1.52453,-1.96714,0.190689,1.47848,-2.02347,0.085991,1.121 [...]
+/*1002*/{0.227436,1.94651,-1.7235,0.27008,1.88832,-1.88084,0.209437,1.79292,-1.60717,0.20102,1.67011,-1.52728,0.302529,1.69841,-1.64904,0.376609,1.71064,-1.70503,0.152448,1.96011,-2.08955,0.264038,1.8883,-1.95281,0.052761,1.92204,-1.97196,-0.085124,1.85689,-2.10824,-0.120054,1.76966,-2.16677,0.006813,1.61921,-2.1392,0.03738,1.56381,-2.16913,0.186343,1.43561,-1.77083,0.00201,1.51612,-1.85202,0.001557,1.54434,-1.89614,0.007451,1.52457,-1.96578,0.190021,1.47963,-2.0236,0.093115,1.12065,-1.7 [...]
+/*1003*/{0.228011,1.94662,-1.72434,0.269638,1.88806,-1.88115,0.208433,1.79318,-1.60805,0.198531,1.67055,-1.52811,0.301574,1.69751,-1.6502,0.374618,1.7074,-1.70646,0.155072,1.95984,-2.09042,0.264388,1.88752,-1.95331,0.054214,1.92287,-1.9729,-0.082697,1.8558,-2.10616,-0.118061,1.76812,-2.166,0.009955,1.61787,-2.13848,0.039182,1.56243,-2.16855,0.187655,1.43665,-1.77094,0.002414,1.51629,-1.85139,0.001335,1.54434,-1.89543,0.007055,1.52465,-1.96476,0.190374,1.48051,-2.02322,0.102849,1.11982,-1 [...]
+/*1004*/{0.227765,1.94617,-1.72589,0.269968,1.88802,-1.88202,0.207022,1.79309,-1.60862,0.195341,1.66921,-1.52807,0.299652,1.69475,-1.65163,0.372633,1.70318,-1.7075,0.156136,1.95911,-2.09166,0.264743,1.88697,-1.95362,0.055024,1.92311,-1.97438,-0.082012,1.85427,-2.10419,-0.116364,1.76576,-2.16503,0.011642,1.61658,-2.13837,0.041181,1.55999,-2.16777,0.188354,1.43737,-1.77156,0.003024,1.51573,-1.85049,0.001299,1.54431,-1.89542,0.00632,1.5242,-1.9644,0.190355,1.48054,-2.02322,0.108086,1.11638, [...]
+/*1005*/{0.227706,1.94519,-1.72662,0.269327,1.8877,-1.88281,0.205582,1.7923,-1.60926,0.194135,1.66834,-1.52801,0.296939,1.69141,-1.65212,0.370379,1.69875,-1.70812,0.157731,1.95825,-2.09257,0.265297,1.88596,-1.95357,0.056173,1.92373,-1.97636,-0.081734,1.8518,-2.10336,-0.114053,1.76303,-2.16469,0.013494,1.61371,-2.13839,0.044068,1.55791,-2.16784,0.188928,1.43763,-1.77179,0.002946,1.51443,-1.8509,0.002408,1.54428,-1.89553,0.006334,1.52283,-1.96404,0.190374,1.48051,-2.02322,0.11693,1.11484,- [...]
+/*1006*/{0.227702,1.94421,-1.72728,0.268917,1.88653,-1.88333,0.203673,1.79196,-1.61001,0.190752,1.66769,-1.52919,0.294812,1.68817,-1.65218,0.368453,1.69355,-1.70843,0.15837,1.95656,-2.09286,0.265262,1.88482,-1.95414,0.057151,1.92354,-1.97724,-0.077886,1.84917,-2.10164,-0.112274,1.75962,-2.16509,0.017183,1.61127,-2.13898,0.046575,1.5551,-2.16823,0.189563,1.43706,-1.77292,0.003865,1.5131,-1.84974,0.00246,1.54321,-1.89474,0.006946,1.52104,-1.96359,0.190481,1.48006,-2.02372,0.122806,1.11285, [...]
+/*1007*/{0.227197,1.94306,-1.72799,0.268939,1.88572,-1.88348,0.201931,1.79055,-1.61065,0.187351,1.66571,-1.52889,0.292341,1.68468,-1.65311,0.365938,1.68833,-1.70863,0.159093,1.95455,-2.0936,0.265313,1.88315,-1.95373,0.056627,1.92139,-1.98024,-0.077145,1.84606,-2.10006,-0.110777,1.75588,-2.1654,0.019382,1.60808,-2.14006,0.049593,1.55248,-2.16839,0.190636,1.43655,-1.77339,0.003818,1.51162,-1.84946,0.002378,1.54095,-1.89524,0.006297,1.51929,-1.9637,0.190877,1.4788,-2.02386,0.131569,1.11035, [...]
+/*1008*/{0.226421,1.94075,-1.72844,0.269031,1.88429,-1.88398,0.199574,1.78919,-1.61096,0.183054,1.66462,-1.5301,0.288949,1.68016,-1.65294,0.363929,1.68218,-1.70905,0.159735,1.95247,-2.09458,0.26523,1.88145,-1.95411,0.057087,1.92154,-1.98082,-0.076074,1.84222,-2.09961,-0.108958,1.75159,-2.16657,0.022083,1.6049,-2.14084,0.052303,1.54896,-2.16886,0.191752,1.43506,-1.77435,0.004263,1.50919,-1.84881,0.002725,1.53916,-1.89458,0.006282,1.51653,-1.964,0.190957,1.47691,-2.0248,0.137983,1.10806,-1 [...]
+/*1009*/{0.225931,1.93874,-1.72876,0.268188,1.88197,-1.88402,0.196921,1.78691,-1.61107,0.178697,1.66197,-1.53064,0.285861,1.67541,-1.65244,0.360862,1.67514,-1.70855,0.159732,1.94969,-2.09531,0.264665,1.87903,-1.9543,0.05751,1.92052,-1.98322,-0.075055,1.83764,-2.09989,-0.107406,1.7463,-2.1675,0.025708,1.60104,-2.14292,0.055649,1.54487,-2.16931,0.191509,1.43421,-1.77552,0.004164,1.50736,-1.84914,0.002765,1.53779,-1.89556,0.006804,1.51333,-1.96429,0.190199,1.47399,-2.02609,0.146706,1.10808, [...]
+/*1010*/{0.224266,1.93648,-1.72905,0.269101,1.88047,-1.88459,0.194367,1.78458,-1.61157,0.174347,1.66009,-1.53158,0.282908,1.66985,-1.65255,0.357994,1.66747,-1.70806,0.16036,1.94652,-2.0962,0.2645,1.87706,-1.9553,0.057805,1.91786,-1.98395,-0.075141,1.83328,-2.10195,-0.106094,1.74102,-2.16927,0.027768,1.59669,-2.14419,0.059243,1.54082,-2.16946,0.192864,1.43204,-1.77657,0.004242,1.50533,-1.84891,0.00322,1.53461,-1.89578,0.006687,1.51162,-1.96518,0.191461,1.47101,-2.02738,0.151709,1.10376,-1 [...]
+/*1011*/{0.22292,1.93396,-1.72889,0.268382,1.87803,-1.88468,0.191424,1.78252,-1.61138,0.169436,1.65841,-1.53268,0.279192,1.66418,-1.6521,0.355561,1.66013,-1.70692,0.160804,1.94341,-2.0967,0.264436,1.87503,-1.95532,0.05696,1.91512,-1.98713,-0.074203,1.82728,-2.10255,-0.103836,1.73492,-2.17073,0.031584,1.59189,-2.14551,0.063758,1.53613,-2.17023,0.193492,1.4301,-1.77712,0.005056,1.50189,-1.84935,0.00304,1.53154,-1.89627,0.006325,1.50777,-1.96612,0.190893,1.46827,-2.02864,0.15774,1.10328,-1. [...]
+/*1012*/{0.22175,1.93084,-1.72862,0.269071,1.87513,-1.88511,0.187747,1.77965,-1.61126,0.165024,1.65571,-1.53319,0.275795,1.65866,-1.65167,0.35205,1.65199,-1.70547,0.160804,1.93959,-2.09728,0.264082,1.87199,-1.95562,0.056913,1.91231,-1.98782,-0.07293,1.8215,-2.10477,-0.101933,1.72858,-2.17295,0.035272,1.58669,-2.14683,0.067604,1.53086,-2.17105,0.194311,1.42805,-1.77825,0.005092,1.49881,-1.85029,0.002699,1.52917,-1.89652,0.006192,1.50447,-1.96645,0.191026,1.46548,-2.03011,0.164946,1.09888, [...]
+/*1013*/{0.22144,1.92833,-1.72801,0.267861,1.87203,-1.88494,0.184187,1.77643,-1.61085,0.159412,1.65297,-1.53494,0.271796,1.65204,-1.65142,0.348361,1.64364,-1.70418,0.160832,1.93554,-2.09887,0.263641,1.86912,-1.95597,0.05634,1.90856,-1.98979,-0.073153,1.81459,-2.10653,-0.100018,1.72183,-2.17482,0.03858,1.58088,-2.14842,0.072012,1.52578,-2.17206,0.19563,1.42583,-1.77979,0.005617,1.49568,-1.8498,0.003878,1.52547,-1.89713,0.006268,1.50066,-1.96605,0.191646,1.46269,-2.03201,0.169031,1.09783,- [...]
+/*1014*/{0.219928,1.92514,-1.72761,0.268277,1.86891,-1.88453,0.180385,1.77302,-1.61072,0.154277,1.64931,-1.53512,0.26766,1.64613,-1.65074,0.344396,1.6345,-1.70271,0.160524,1.93191,-2.10012,0.263788,1.86636,-1.956,0.055869,1.90521,-1.9913,-0.073448,1.80745,-2.10979,-0.097773,1.71406,-2.17747,0.042484,1.5749,-2.14919,0.076832,1.51985,-2.17229,0.195535,1.4233,-1.78121,0.005464,1.49198,-1.84975,0.003255,1.52229,-1.89735,0.005723,1.49707,-1.96642,0.191136,1.45984,-2.03327,0.174364,1.09586,-1. [...]
+/*1015*/{0.218028,1.92036,-1.72716,0.268691,1.86616,-1.88414,0.177198,1.76933,-1.61032,0.14799,1.64714,-1.53629,0.262823,1.63867,-1.64944,0.341425,1.62498,-1.70096,0.160347,1.92733,-2.10085,0.263357,1.86259,-1.95609,0.054852,1.90057,-1.99341,-0.073868,1.79993,-2.11243,-0.095484,1.70693,-2.17974,0.046639,1.56896,-2.15046,0.081784,1.51423,-2.17283,0.196386,1.42012,-1.78253,0.004955,1.48828,-1.85038,0.002959,1.51844,-1.89796,0.005087,1.49272,-1.96622,0.191462,1.45611,-2.03518,0.179743,1.093 [...]
+/*1016*/{0.217135,1.91647,-1.72598,0.269208,1.86203,-1.8837,0.173426,1.76547,-1.61004,0.142342,1.64285,-1.5363,0.25847,1.63169,-1.64904,0.336478,1.61558,-1.69815,0.160636,1.92283,-2.10154,0.263656,1.85921,-1.95612,0.054183,1.89591,-1.99407,-0.071242,1.79187,-2.11492,-0.092327,1.69844,-2.18256,0.050304,1.56186,-2.15145,0.086579,1.50854,-2.17353,0.1972,1.4175,-1.78375,0.005489,1.4838,-1.84985,0.003302,1.51471,-1.89803,0.003691,1.48867,-1.96649,0.189331,1.45253,-2.03664,0.186976,1.08958,-1. [...]
+/*1017*/{0.215722,1.91195,-1.72563,0.268818,1.85831,-1.88426,0.169798,1.76103,-1.60992,0.135568,1.64019,-1.53701,0.253279,1.62417,-1.64788,0.331546,1.60564,-1.69631,0.16052,1.91792,-2.10224,0.264169,1.85492,-1.95668,0.053736,1.89092,-1.99652,-0.071368,1.7833,-2.1183,-0.090034,1.68996,-2.18527,0.055783,1.55547,-2.15251,0.092253,1.50245,-2.1746,0.197391,1.41464,-1.78626,0.006354,1.47977,-1.84948,0.002843,1.50986,-1.89861,0.003058,1.48418,-1.96577,0.189014,1.44901,-2.03875,0.189083,1.0892,- [...]
+/*1018*/{0.214605,1.90705,-1.72478,0.269539,1.85395,-1.88344,0.166416,1.75622,-1.60989,0.12988,1.63477,-1.53665,0.247725,1.61724,-1.64656,0.326694,1.59655,-1.69407,0.16125,1.91347,-2.1036,0.263819,1.85071,-1.95664,0.052375,1.88593,-1.99725,-0.071363,1.77396,-2.12141,-0.086885,1.68111,-2.18838,0.061263,1.5486,-2.15331,0.097856,1.49567,-2.17493,0.198058,1.41135,-1.78771,0.006197,1.47442,-1.84969,0.00296,1.5055,-1.89893,0.002199,1.47875,-1.96533,0.188003,1.44517,-2.04105,0.195549,1.08444,-1 [...]
+/*1019*/{0.213663,1.90243,-1.72416,0.269164,1.84924,-1.88267,0.163032,1.75116,-1.60952,0.122863,1.63058,-1.5375,0.242932,1.60964,-1.64616,0.321472,1.58657,-1.69123,0.160896,1.90851,-2.10496,0.264114,1.8462,-1.95688,0.051683,1.87894,-1.99971,-0.07072,1.76476,-2.12515,-0.083934,1.67171,-2.19092,0.06526,1.54136,-2.15452,0.103584,1.48924,-2.1753,0.198391,1.40796,-1.78958,0.006002,1.46938,-1.849,0.00216,1.50043,-1.89909,0.001206,1.47376,-1.96512,0.187463,1.44097,-2.04366,0.196714,1.08378,-1.7 [...]
+/*1020*/{0.212357,1.89691,-1.72329,0.269466,1.84439,-1.88288,0.15936,1.74657,-1.60967,0.116903,1.62701,-1.53773,0.237522,1.60156,-1.64511,0.315396,1.57627,-1.68852,0.160855,1.90365,-2.10576,0.26466,1.84161,-1.95672,0.049512,1.87517,-2.00058,-0.068953,1.75627,-2.12947,-0.08036,1.66229,-2.19425,0.071311,1.53453,-2.15568,0.110868,1.48265,-2.17631,0.198725,1.40348,-1.79156,0.005926,1.46459,-1.84834,0.002249,1.49482,-1.89972,5e-006,1.4683,-1.96406,0.186929,1.43656,-2.04622,0.203336,1.07983,-1 [...]
+/*1021*/{0.211362,1.8914,-1.72311,0.270015,1.83822,-1.88168,0.155527,1.74087,-1.60969,0.110477,1.62285,-1.53834,0.231701,1.59413,-1.6438,0.310786,1.5668,-1.68665,0.16151,1.89843,-2.10663,0.26544,1.83708,-1.95669,0.048557,1.86823,-2.00158,-0.067312,1.74638,-2.13401,-0.076268,1.65265,-2.19754,0.077395,1.52703,-2.15655,0.116971,1.47627,-2.17722,0.199341,1.39976,-1.7937,0.004327,1.4594,-1.84929,0.0009,1.48927,-1.9006,-0.000829,1.46319,-1.96418,0.185565,1.43146,-2.04875,0.204263,1.07813,-1.74 [...]
+/*1022*/{0.211132,1.88595,-1.72188,0.271088,1.83433,-1.88137,0.15205,1.73579,-1.61049,0.104223,1.61887,-1.539,0.226525,1.58638,-1.64246,0.30384,1.55724,-1.68398,0.161691,1.89386,-2.10756,0.265782,1.83146,-1.9567,0.04798,1.86196,-2.00316,-0.065066,1.73618,-2.13816,-0.071161,1.64253,-2.20066,0.083793,1.51954,-2.15828,0.123968,1.46928,-2.17793,0.199033,1.39526,-1.79539,0.003646,1.45361,-1.84865,-0.000672,1.48302,-1.90117,-0.003095,1.45775,-1.96325,0.184811,1.42677,-2.05102,0.207727,1.07529, [...]
+/*1023*/{0.210166,1.88059,-1.72103,0.271594,1.82786,-1.88026,0.149076,1.73032,-1.61074,0.098945,1.61579,-1.54026,0.220021,1.57972,-1.64133,0.298883,1.54844,-1.68176,0.161878,1.88927,-2.10887,0.266179,1.82661,-1.95668,0.047618,1.85602,-2.00319,-0.062528,1.72634,-2.14331,-0.066106,1.63245,-2.20333,0.089729,1.51315,-2.16006,0.131448,1.46262,-2.17937,0.198531,1.39029,-1.79623,0.003256,1.44856,-1.848,-0.002397,1.47712,-1.90113,-0.00589,1.45371,-1.96414,0.182547,1.42118,-2.05327,0.214002,1.070 [...]
+/*1024*/{0.210347,1.8755,-1.72034,0.271203,1.82153,-1.8793,0.145262,1.72526,-1.61081,0.091559,1.61215,-1.54105,0.214862,1.57296,-1.64029,0.292257,1.53989,-1.67827,0.162439,1.88465,-2.11022,0.267561,1.82114,-1.95676,0.047591,1.85172,-2.00265,-0.05897,1.71615,-2.14838,-0.060444,1.62246,-2.20682,0.097343,1.50652,-2.16233,0.139235,1.45673,-2.18052,0.199354,1.38511,-1.79889,0.001323,1.44359,-1.84852,-0.003898,1.47157,-1.90041,-0.008824,1.44933,-1.96357,0.179111,1.41756,-2.05506,0.217741,1.068 [...]
+/*1025*/{0.209536,1.87036,-1.71999,0.273291,1.81559,-1.87892,0.141577,1.72068,-1.61124,0.084917,1.60954,-1.54289,0.209191,1.56675,-1.63912,0.285679,1.53227,-1.67559,0.163247,1.8806,-2.11153,0.26856,1.81616,-1.95645,0.0484,1.8485,-2.0018,-0.053456,1.7066,-2.1543,-0.053167,1.61263,-2.20965,0.105524,1.49992,-2.16443,0.1477,1.45022,-2.18188,0.198177,1.38019,-1.80157,-0.001545,1.43828,-1.84852,-0.00624,1.46578,-1.90029,-0.01165,1.44535,-1.96483,0.176392,1.41483,-2.05733,0.222783,1.06485,-1.74 [...]
+/*1026*/{0.209724,1.86598,-1.71919,0.273614,1.80921,-1.87776,0.138215,1.71661,-1.6123,0.077486,1.6068,-1.54421,0.202926,1.56135,-1.63792,0.279551,1.52525,-1.67317,0.163997,1.8767,-2.1132,0.270063,1.81101,-1.95622,0.048888,1.84688,-2.00121,-0.048006,1.69769,-2.16133,-0.045951,1.60229,-2.21204,0.113426,1.49343,-2.1659,0.156552,1.4449,-2.18324,0.197912,1.37595,-1.8037,-0.003427,1.43381,-1.84846,-0.008383,1.46078,-1.89979,-0.015176,1.44135,-1.96442,0.174001,1.41183,-2.05924,0.227286,1.06378, [...]
+/*1027*/{0.209507,1.862,-1.71816,0.275848,1.80502,-1.87781,0.134797,1.71379,-1.61306,0.071037,1.60482,-1.54556,0.196425,1.55632,-1.63694,0.273395,1.51859,-1.67059,0.164709,1.87332,-2.11401,0.270558,1.80662,-1.95562,0.049139,1.84468,-2.00073,-0.039921,1.68795,-2.16692,-0.037078,1.59307,-2.21437,0.122323,1.48771,-2.16731,0.165735,1.44022,-2.18401,0.196559,1.37179,-1.806,-0.005955,1.42908,-1.849,-0.010113,1.45737,-1.89962,-0.018321,1.43799,-1.9633,0.17064,1.40976,-2.06081,0.231232,1.06419,- [...]
+/*1028*/{0.208765,1.85874,-1.71722,0.275393,1.80101,-1.87613,0.130121,1.71187,-1.61399,0.063622,1.60448,-1.54694,0.190481,1.55257,-1.63543,0.266259,1.51404,-1.66839,0.166316,1.87084,-2.11423,0.27112,1.80382,-1.95501,0.049718,1.843,-2.00047,-0.035197,1.68076,-2.17278,-0.026191,1.58456,-2.21724,0.132331,1.48259,-2.1694,0.176502,1.43557,-2.18483,0.194707,1.36756,-1.8078,-0.008139,1.42529,-1.84857,-0.012158,1.45595,-1.89894,-0.020707,1.4356,-1.96125,0.168659,1.40879,-2.06268,0.236882,1.06638 [...]
+/*1029*/{0.208907,1.8559,-1.71603,0.275342,1.79859,-1.87548,0.126171,1.71067,-1.61444,0.05577,1.60535,-1.54963,0.184186,1.54956,-1.63491,0.25914,1.5085,-1.6661,0.168131,1.86857,-2.11362,0.270827,1.80141,-1.95441,0.050418,1.84138,-2.00111,-0.028092,1.67392,-2.17609,-0.016751,1.57757,-2.2184,0.142545,1.47929,-2.17048,0.18715,1.43284,-2.18555,0.19324,1.36464,-1.81081,-0.010595,1.42268,-1.84708,-0.013779,1.45684,-1.89891,-0.02196,1.43471,-1.95952,0.166311,1.40808,-2.06443,0.238773,1.06906,-1 [...]
+/*1030*/{0.209566,1.85305,-1.71489,0.274874,1.79696,-1.87493,0.121253,1.71042,-1.61518,0.048892,1.60742,-1.55165,0.177312,1.54714,-1.63422,0.252447,1.50477,-1.66278,0.170512,1.8672,-2.113,0.272112,1.79984,-1.953,0.050447,1.84083,-2.00139,-0.0199,1.66774,-2.17858,-0.007058,1.57187,-2.21911,0.153942,1.47686,-2.17135,0.198851,1.43079,-2.18611,0.189385,1.36193,-1.811,-0.011532,1.42185,-1.84629,-0.015742,1.45634,-1.89909,-0.023613,1.43493,-1.95808,0.164384,1.40769,-2.06652,0.246908,1.07429,-1 [...]
+/*1031*/{0.210195,1.85236,-1.71434,0.274455,1.7956,-1.8739,0.11782,1.71093,-1.61566,0.041714,1.60921,-1.55319,0.170426,1.54668,-1.63292,0.245379,1.50293,-1.66084,0.172196,1.86612,-2.11224,0.272198,1.79849,-1.95267,0.051253,1.83969,-2.00179,-0.012856,1.66351,-2.17951,0.002287,1.56803,-2.2199,0.164364,1.47512,-2.17134,0.2103,1.43008,-2.18611,0.188694,1.35929,-1.81085,-0.012568,1.42216,-1.84476,-0.016642,1.45599,-1.89885,-0.024083,1.43541,-1.95644,0.162641,1.40737,-2.06795,0.254481,1.08045, [...]
+/*1032*/{0.209865,1.85104,-1.71413,0.273483,1.79458,-1.87306,0.112775,1.71235,-1.61644,0.036323,1.61281,-1.5555,0.164553,1.54665,-1.63202,0.239443,1.5011,-1.65824,0.174712,1.86554,-2.11187,0.272279,1.79715,-1.95094,0.051678,1.83908,-2.00214,-0.006785,1.66055,-2.1802,0.010999,1.56561,-2.22081,0.176019,1.47505,-2.17189,0.222345,1.43106,-2.18557,0.18625,1.35741,-1.81086,-0.014211,1.42226,-1.8448,-0.017763,1.45576,-1.89878,-0.025831,1.4362,-1.95605,0.161254,1.40695,-2.06869,0.26253,1.08609,- [...]
+/*1033*/{0.210026,1.85073,-1.71393,0.274007,1.79385,-1.87207,0.108295,1.71426,-1.61652,0.027955,1.61703,-1.55745,0.158298,1.54628,-1.6304,0.233168,1.50063,-1.65557,0.176509,1.86558,-2.11117,0.272574,1.79682,-1.95022,0.052255,1.83996,-2.0035,-0.000408,1.65831,-2.18035,0.019049,1.56327,-2.22131,0.186924,1.47501,-2.17074,0.234189,1.43326,-2.18449,0.184412,1.35704,-1.81103,-0.014811,1.42291,-1.84482,-0.019082,1.45597,-1.89952,-0.025765,1.43699,-1.95662,0.160333,1.40677,-2.06911,0.267985,1.09 [...]
+/*1034*/{0.209166,1.85103,-1.71347,0.273292,1.79283,-1.87107,0.103878,1.71687,-1.61759,0.02215,1.62179,-1.55932,0.151946,1.54845,-1.62923,0.226385,1.50095,-1.65297,0.179182,1.86621,-2.11096,0.272967,1.7968,-1.94939,0.053879,1.84021,-2.00478,0.005618,1.65637,-2.17985,0.027435,1.56159,-2.2219,0.197806,1.47696,-2.17033,0.245937,1.43692,-2.18312,0.182266,1.35636,-1.80849,-0.016244,1.42442,-1.84607,-0.019641,1.45598,-1.90006,-0.02588,1.43751,-1.95785,0.15857,1.40647,-2.06988,0.275348,1.10077, [...]
+/*1035*/{0.208136,1.85207,-1.71316,0.272118,1.79375,-1.87022,0.099872,1.71994,-1.61764,0.01548,1.62585,-1.56165,0.145493,1.55016,-1.62801,0.219647,1.50215,-1.65197,0.181425,1.86734,-2.11086,0.273242,1.79738,-1.94823,0.055027,1.84116,-2.00582,0.009819,1.65522,-2.17908,0.035757,1.56065,-2.22255,0.20726,1.47998,-2.16873,0.256908,1.4416,-2.18134,0.180428,1.35819,-1.80772,-0.016479,1.42507,-1.84694,-0.019838,1.45731,-1.90141,-0.026219,1.43861,-1.95919,0.160461,1.40577,-2.07006,0.281416,1.1076 [...]
+/*1036*/{0.206193,1.85314,-1.71241,0.272494,1.79408,-1.86897,0.094556,1.72403,-1.61836,0.009023,1.63082,-1.56307,0.140423,1.5535,-1.62701,0.212939,1.50466,-1.64968,0.184686,1.86921,-2.11052,0.273553,1.79863,-1.94712,0.056107,1.8416,-2.00685,0.014685,1.65451,-2.17826,0.042786,1.56107,-2.22236,0.217194,1.48612,-2.16742,0.267781,1.44803,-2.17924,0.178587,1.35976,-1.80576,-0.018059,1.42631,-1.84861,-0.020548,1.45845,-1.90315,-0.025743,1.43874,-1.96085,0.160059,1.40538,-2.07076,0.287755,1.116 [...]
+/*1037*/{0.204339,1.85525,-1.71191,0.271985,1.79549,-1.86768,0.090526,1.72755,-1.61906,0.002119,1.63535,-1.5643,0.132818,1.5566,-1.62577,0.206901,1.50751,-1.64857,0.186118,1.87181,-2.1101,0.273993,1.79997,-1.94552,0.056902,1.84186,-2.00851,0.018842,1.65473,-2.17737,0.049857,1.56208,-2.22288,0.226513,1.49194,-2.16554,0.277722,1.45543,-2.17646,0.177252,1.36213,-1.80494,-0.018996,1.42782,-1.85003,-0.020598,1.45892,-1.90472,-0.026152,1.44001,-1.96284,0.162134,1.40542,-2.07039,0.29435,1.12579 [...]
+/*1038*/{0.202401,1.8573,-1.7112,0.271308,1.79739,-1.86611,0.084995,1.73218,-1.61953,-0.003628,1.64028,-1.5656,0.127795,1.56185,-1.62583,0.200609,1.51112,-1.64715,0.18797,1.87431,-2.10885,0.273673,1.80162,-1.94431,0.05812,1.84402,-2.00965,0.0233,1.65413,-2.17587,0.057275,1.56379,-2.22306,0.233867,1.49797,-2.16339,0.287387,1.46395,-2.17355,0.175866,1.36478,-1.80341,-0.019345,1.42943,-1.85278,-0.020073,1.46054,-1.90658,-0.02552,1.44096,-1.96516,0.163384,1.40543,-2.07034,0.298585,1.1335,-1. [...]
+/*1039*/{0.199235,1.8606,-1.7105,0.271628,1.79938,-1.86491,0.080718,1.73697,-1.61975,-0.010223,1.64599,-1.56681,0.122014,1.56617,-1.62491,0.194312,1.5157,-1.64511,0.191808,1.87818,-2.10822,0.274619,1.80434,-1.94263,0.059931,1.84529,-2.01139,0.028041,1.65583,-2.17365,0.063207,1.56602,-2.22367,0.241665,1.50549,-2.1609,0.296529,1.47361,-2.17044,0.174276,1.36833,-1.80262,-0.019365,1.43106,-1.85397,-0.019942,1.46195,-1.90818,-0.025281,1.44301,-1.96664,0.163761,1.40529,-2.07019,0.302677,1.1420 [...]
+/*1040*/{0.196553,1.86378,-1.7098,0.2707,1.80298,-1.86305,0.075772,1.74134,-1.62014,-0.016949,1.65065,-1.56822,0.115739,1.57104,-1.62439,0.188566,1.52039,-1.64411,0.193295,1.8812,-2.10711,0.275145,1.80749,-1.94096,0.060786,1.8479,-2.01268,0.03244,1.6573,-2.17287,0.069102,1.56893,-2.224,0.250298,1.51501,-2.15816,0.305017,1.4831,-2.16685,0.1734,1.37039,-1.80019,-0.019424,1.43329,-1.85589,-0.019771,1.46423,-1.91034,-0.024283,1.44557,-1.96915,0.165663,1.40539,-2.06959,0.306371,1.15027,-1.744 [...]
+/*1041*/{0.193853,1.86727,-1.70938,0.270235,1.80629,-1.86084,0.07125,1.74615,-1.62074,-0.021728,1.65654,-1.5699,0.109412,1.57691,-1.62523,0.182479,1.52593,-1.64277,0.195681,1.88528,-2.1063,0.27547,1.81061,-1.93905,0.06215,1.84981,-2.01338,0.036103,1.65971,-2.17139,0.07553,1.57258,-2.22415,0.256876,1.52275,-2.15504,0.312769,1.49394,-2.16329,0.173084,1.37362,-1.79902,-0.019119,1.4356,-1.8584,-0.019305,1.4672,-1.91252,-0.022594,1.44852,-1.97177,0.168077,1.40562,-2.06892,0.311959,1.1585,-1.7 [...]
+/*1042*/{0.189972,1.8711,-1.70895,0.270173,1.80971,-1.85921,0.066238,1.75133,-1.62119,-0.027266,1.66194,-1.57047,0.103882,1.58372,-1.62553,0.176895,1.53153,-1.6421,0.198354,1.88997,-2.10482,0.276356,1.81425,-1.9369,0.062884,1.8524,-2.01474,0.042043,1.66222,-2.16987,0.082017,1.57659,-2.22527,0.262616,1.53099,-2.15217,0.320216,1.50448,-2.15996,0.172608,1.37779,-1.79805,-0.01902,1.43875,-1.86107,-0.017899,1.47033,-1.91484,-0.021687,1.45085,-1.97458,0.169554,1.40515,-2.06852,0.313342,1.16613 [...]
+/*1043*/{0.186963,1.87484,-1.70826,0.26953,1.81439,-1.8572,0.062529,1.7562,-1.62126,-0.033368,1.66773,-1.57187,0.100307,1.58937,-1.62529,0.17085,1.53756,-1.64185,0.201322,1.89455,-2.10397,0.277214,1.81806,-1.93469,0.064716,1.8552,-2.01626,0.045053,1.66619,-2.17144,0.087991,1.58057,-2.22558,0.269008,1.54072,-2.14876,0.327548,1.51576,-2.15577,0.172847,1.38161,-1.79744,-0.017675,1.44231,-1.86272,-0.016621,1.47282,-1.91672,-0.01889,1.45362,-1.97665,0.171226,1.40517,-2.0682,0.315639,1.17406,- [...]
+/*1044*/{0.183723,1.87908,-1.70788,0.269818,1.81759,-1.85525,0.058379,1.76146,-1.62201,-0.038219,1.67408,-1.57311,0.094548,1.59569,-1.62536,0.166197,1.54411,-1.64092,0.203079,1.89922,-2.10302,0.277937,1.82251,-1.93297,0.066589,1.85812,-2.0168,0.049511,1.66903,-2.17254,0.093916,1.58509,-2.22636,0.274693,1.55025,-2.14602,0.333755,1.52712,-2.15169,0.173666,1.38607,-1.79714,-0.017209,1.44632,-1.86501,-0.014635,1.4766,-1.91909,-0.016529,1.45717,-1.97832,0.172891,1.40515,-2.06747,0.317675,1.18 [...]
+/*1045*/{0.180534,1.88375,-1.70766,0.268915,1.82173,-1.85306,0.053475,1.76708,-1.62226,-0.043752,1.67998,-1.57418,0.089001,1.60205,-1.62651,0.161153,1.54993,-1.63984,0.205819,1.90403,-2.10122,0.278915,1.82625,-1.93043,0.068519,1.86119,-2.01802,0.054155,1.67313,-2.1742,0.100326,1.59011,-2.22753,0.279602,1.5601,-2.14343,0.339612,1.53863,-2.14803,0.174604,1.39056,-1.79741,-0.016334,1.45064,-1.86674,-0.013778,1.47994,-1.92116,-0.014268,1.45995,-1.98039,0.175372,1.40547,-2.06746,0.320463,1.18 [...]
+/*1046*/{0.177664,1.88844,-1.70752,0.267032,1.82862,-1.85116,0.050518,1.77261,-1.62285,-0.048802,1.68642,-1.57586,0.084824,1.60834,-1.62709,0.156211,1.55639,-1.63969,0.207991,1.90879,-2.09995,0.27942,1.83057,-1.92833,0.069916,1.86463,-2.01986,0.059678,1.67725,-2.17692,0.105838,1.59512,-2.2287,0.284076,1.56978,-2.14044,0.34522,1.55016,-2.14414,0.175938,1.39514,-1.7976,-0.014358,1.4554,-1.86964,-0.010679,1.48318,-1.92401,-0.011742,1.46371,-1.98278,0.177213,1.40556,-2.06732,0.323143,1.19697 [...]
+/*1047*/{0.17443,1.89267,-1.70733,0.267548,1.83147,-1.84945,0.046916,1.77793,-1.62395,-0.053513,1.69156,-1.5768,0.081225,1.61519,-1.62748,0.15203,1.56295,-1.64047,0.210033,1.91354,-2.09898,0.279728,1.83528,-1.92623,0.071079,1.8684,-2.02068,0.063929,1.68247,-2.17994,0.111626,1.60028,-2.22968,0.288948,1.58033,-2.13776,0.3501,1.56166,-2.14053,0.176797,1.40053,-1.79901,-0.013487,1.4598,-1.87078,-0.009164,1.48715,-1.92597,-0.008494,1.46699,-1.98435,0.179511,1.40526,-2.06736,0.323748,1.20362,- [...]
+/*1048*/{0.171933,1.89699,-1.7074,0.267048,1.83796,-1.84802,0.043545,1.78346,-1.62424,-0.055863,1.69814,-1.57867,0.076491,1.62124,-1.62841,0.147932,1.56918,-1.63931,0.212275,1.91802,-2.09752,0.279731,1.83985,-1.92431,0.073495,1.87254,-2.02165,0.069599,1.68711,-2.18222,0.118153,1.60494,-2.231,0.292562,1.5888,-2.13466,0.353553,1.57208,-2.13717,0.179898,1.40434,-1.80002,-0.010959,1.46426,-1.87234,-0.007335,1.49069,-1.92886,-0.005608,1.47093,-1.986,0.181347,1.40524,-2.06676,0.324268,1.21134, [...]
+/*1049*/{0.169533,1.90172,-1.70724,0.266724,1.84271,-1.84647,0.041094,1.78857,-1.62519,-0.060875,1.70406,-1.58054,0.072336,1.62779,-1.6298,0.143847,1.57498,-1.63938,0.215453,1.92265,-2.0962,0.280805,1.84445,-1.92255,0.075232,1.87544,-2.02335,0.074514,1.69226,-2.186,0.124519,1.60994,-2.23206,0.296786,1.59897,-2.13152,0.357776,1.58239,-2.13297,0.180641,1.4082,-1.80071,-0.00963,1.4699,-1.8743,-0.004453,1.49511,-1.93058,-0.002536,1.47497,-1.98782,0.183668,1.40587,-2.06667,0.326296,1.21645,-1 [...]
+/*1050*/{0.167763,1.90639,-1.70736,0.265494,1.84746,-1.84514,0.037661,1.79374,-1.6263,-0.065599,1.70947,-1.58221,0.068102,1.63369,-1.63009,0.140231,1.58125,-1.6394,0.217663,1.92728,-2.09506,0.281276,1.84879,-1.92078,0.076591,1.87873,-2.02411,0.08009,1.69712,-2.18941,0.130494,1.61489,-2.23327,0.299316,1.60697,-2.12878,0.361331,1.59203,-2.13071,0.183051,1.41281,-1.80147,-0.008028,1.47526,-1.87527,-0.001303,1.49904,-1.93279,-2.2e-005,1.47825,-1.98955,0.185548,1.40659,-2.06654,0.327677,1.222 [...]
+/*1051*/{0.165645,1.91074,-1.70723,0.266337,1.85158,-1.8434,0.03494,1.79923,-1.62731,-0.069581,1.71546,-1.58437,0.064726,1.63963,-1.63065,0.136162,1.58678,-1.63989,0.219759,1.9308,-2.09345,0.281221,1.85368,-1.91903,0.078581,1.88154,-2.02488,0.084253,1.70345,-2.19449,0.136074,1.6202,-2.23497,0.301443,1.61595,-2.12643,0.365077,1.60124,-2.12337,0.185121,1.41671,-1.80275,-0.005432,1.48057,-1.8764,0.000993,1.50372,-1.93474,0.002639,1.48216,-1.99043,0.186801,1.40721,-2.06666,0.328898,1.22836,- [...]
+/*1052*/{0.163893,1.91531,-1.70792,0.25916,1.85914,-1.84386,0.032163,1.8037,-1.62853,-0.07322,1.72062,-1.58641,0.061082,1.64533,-1.63246,0.133476,1.59295,-1.63972,0.22163,1.93475,-2.09241,0.281421,1.85881,-1.91777,0.080467,1.88446,-2.02608,0.088995,1.70732,-2.19832,0.141085,1.62456,-2.23645,0.303673,1.62368,-2.12419,0.36729,1.61082,-2.12037,0.185936,1.42116,-1.8037,-0.003804,1.48572,-1.87706,0.0027,1.50825,-1.93683,0.005642,1.48671,-1.99156,0.188077,1.40843,-2.06622,0.32981,1.23309,-1.74 [...]
+/*1053*/{0.162727,1.9195,-1.70812,0.258669,1.86326,-1.84288,0.029837,1.80866,-1.63033,-0.077136,1.72608,-1.58906,0.058198,1.65093,-1.63302,0.130496,1.59771,-1.63988,0.223453,1.938,-2.09163,0.281646,1.86281,-1.91589,0.082528,1.88765,-2.0272,0.095671,1.71189,-2.20195,0.146736,1.62893,-2.23856,0.305358,1.63111,-2.12194,0.369719,1.61966,-2.11801,0.188363,1.42503,-1.80363,-0.001258,1.49143,-1.87863,0.004532,1.51239,-1.93752,0.00772,1.49084,-1.99267,0.189626,1.41006,-2.06626,0.330379,1.2369,-1 [...]
+/*1054*/{0.160989,1.92339,-1.70878,0.259105,1.86764,-1.84117,0.027269,1.81351,-1.63173,-0.079731,1.73224,-1.59149,0.054831,1.6556,-1.63411,0.127067,1.60271,-1.64083,0.22581,1.94128,-2.09036,0.2821,1.8664,-1.91476,0.083763,1.88981,-2.02788,0.100351,1.7156,-2.20502,0.151741,1.63311,-2.23996,0.308118,1.63929,-2.12073,0.37116,1.62904,-2.11817,0.189152,1.42928,-1.80457,0.000587,1.49646,-1.87893,0.007495,1.51701,-1.93893,0.010487,1.495,-1.99369,0.191298,1.4116,-2.06551,0.331331,1.24106,-1.7501 [...]
+/*1055*/{0.160033,1.92699,-1.70894,0.259503,1.8712,-1.84031,0.024862,1.81799,-1.63314,-0.083452,1.7368,-1.59428,0.051924,1.66067,-1.63504,0.124355,1.60751,-1.64041,0.227523,1.94422,-2.08966,0.282941,1.87037,-1.91374,0.085328,1.89245,-2.02872,0.107292,1.72042,-2.20757,0.156224,1.63673,-2.2419,0.310012,1.64603,-2.11896,0.373042,1.63735,-2.11573,0.189492,1.43268,-1.80478,0.001418,1.50202,-1.87921,0.008811,1.52211,-1.94,0.013007,1.49958,-1.99366,0.191658,1.41371,-2.0652,0.332856,1.24436,-1.7 [...]
+/*1056*/{0.159632,1.93053,-1.70906,0.260004,1.87502,-1.83932,0.022534,1.82257,-1.63449,-0.087193,1.74231,-1.59739,0.049408,1.66513,-1.63653,0.121436,1.61162,-1.64053,0.230091,1.94683,-2.08941,0.283372,1.87338,-1.91229,0.087605,1.89445,-2.0299,0.11255,1.72442,-2.2102,0.160971,1.63996,-2.24395,0.311797,1.65181,-2.11804,0.374379,1.64322,-2.11258,0.193985,1.43741,-1.80451,0.003674,1.50704,-1.87916,0.011307,1.52632,-1.94032,0.015041,1.50368,-1.99371,0.192926,1.41583,-2.06475,0.333016,1.24664, [...]
+/*1057*/{0.15899,1.93372,-1.70943,0.261432,1.87826,-1.8382,0.020434,1.82687,-1.63629,-0.089427,1.74606,-1.60036,0.047087,1.66955,-1.63717,0.119267,1.61589,-1.64082,0.231471,1.94906,-2.08895,0.284141,1.87664,-1.91149,0.089054,1.89748,-2.0307,0.117519,1.72735,-2.21185,0.164744,1.64337,-2.24591,0.312029,1.65623,-2.11797,0.376196,1.64949,-2.11163,0.196662,1.44164,-1.80428,0.005763,1.51115,-1.87856,0.012078,1.53026,-1.93932,0.016357,1.50754,-1.99269,0.193105,1.41828,-2.06402,0.332092,1.24817, [...]
+/*1058*/{0.158356,1.93721,-1.70993,0.261655,1.88024,-1.83764,0.019138,1.83195,-1.63775,-0.091985,1.75021,-1.60334,0.044466,1.67305,-1.63801,0.117372,1.61939,-1.64086,0.232776,1.9508,-2.08845,0.284739,1.87972,-1.91069,0.091286,1.9002,-2.03098,0.12139,1.72972,-2.21369,0.16832,1.64634,-2.24754,0.313527,1.66157,-2.11809,0.377348,1.65496,-2.10975,0.192471,1.44382,-1.80558,0.006615,1.51566,-1.87863,0.013237,1.53371,-1.9393,0.017873,1.51154,-1.99224,0.19358,1.42092,-2.06379,0.332538,1.24934,-1. [...]
+/*1059*/{0.158024,1.94015,-1.71051,0.262761,1.88281,-1.83651,0.017015,1.83585,-1.63946,-0.096239,1.75398,-1.60634,0.042474,1.67724,-1.63951,0.115312,1.62275,-1.64058,0.235435,1.95324,-2.08853,0.284994,1.88251,-1.90967,0.092944,1.90244,-2.03149,0.125761,1.73226,-2.21504,0.172425,1.64858,-2.24958,0.315763,1.66617,-2.11773,0.378694,1.65923,-2.10784,0.19435,1.44588,-1.80482,0.008345,1.51945,-1.87656,0.014734,1.5375,-1.9375,0.017923,1.51564,-1.99127,0.193795,1.42383,-2.06365,0.332194,1.24958, [...]
+/*1060*/{0.159107,1.94323,-1.71044,0.262968,1.88461,-1.83628,0.015493,1.83986,-1.64195,-0.096451,1.75793,-1.60955,0.040555,1.6799,-1.6404,0.113156,1.62547,-1.64055,0.237003,1.9548,-2.0884,0.286187,1.88424,-1.90878,0.093731,1.90539,-2.03212,0.129111,1.73496,-2.21706,0.17598,1.65085,-2.25125,0.316646,1.66876,-2.11725,0.379834,1.66205,-2.10534,0.195681,1.44658,-1.8045,0.009814,1.52301,-1.87449,0.014835,1.54109,-1.93671,0.019242,1.51927,-1.99062,0.194612,1.42689,-2.06359,0.329927,1.24931,-1. [...]
+/*1061*/{0.159142,1.94572,-1.7108,0.264833,1.88722,-1.83639,0.013989,1.84428,-1.64364,-0.099612,1.76128,-1.61247,0.038503,1.68264,-1.6415,0.110994,1.62818,-1.64061,0.239325,1.95684,-2.08836,0.287539,1.88691,-1.90804,0.096032,1.90732,-2.03271,0.133128,1.73741,-2.218,0.178744,1.653,-2.25295,0.317656,1.671,-2.11629,0.380704,1.66553,-2.10413,0.197173,1.45,-1.80544,0.010148,1.52601,-1.87377,0.015444,1.54495,-1.93582,0.01998,1.52256,-1.98877,0.194303,1.42943,-2.06352,0.328336,1.24773,-1.75669, [...]
+/*1062*/{0.159228,1.94808,-1.71137,0.264958,1.88842,-1.83563,0.012757,1.84765,-1.64531,-0.101219,1.76532,-1.61551,0.037451,1.68576,-1.64332,0.109616,1.63038,-1.64072,0.240365,1.95813,-2.08831,0.287956,1.88809,-1.90806,0.097156,1.91002,-2.0334,0.135387,1.73852,-2.21868,0.181449,1.655,-2.25404,0.31911,1.67348,-2.11645,0.382327,1.66794,-2.10216,0.198272,1.45143,-1.80498,0.011465,1.52841,-1.87167,0.015278,1.54731,-1.93436,0.020628,1.52539,-1.98692,0.194086,1.43285,-2.06298,0.325601,1.24465,- [...]
+/*1063*/{0.160848,1.95008,-1.71137,0.27123,1.88695,-1.83368,0.011659,1.85097,-1.64806,-0.104595,1.76807,-1.61853,0.035509,1.68814,-1.64396,0.108532,1.63233,-1.64057,0.24196,1.9593,-2.08758,0.288976,1.88989,-1.90782,0.098571,1.91236,-2.03399,0.13709,1.74069,-2.22015,0.183374,1.65612,-2.25474,0.320518,1.67376,-2.11577,0.383353,1.66799,-2.10075,0.196499,1.45304,-1.80512,0.012706,1.53072,-1.87054,0.015466,1.55049,-1.93312,0.020522,1.52817,-1.98546,0.194622,1.43617,-2.0628,0.32462,1.24048,-1. [...]
+/*1064*/{0.161703,1.95209,-1.71135,0.266765,1.89202,-1.83554,0.011379,1.85426,-1.6499,-0.105788,1.77003,-1.62133,0.034603,1.68913,-1.64539,0.106677,1.63425,-1.64064,0.242283,1.96076,-2.08754,0.289775,1.89171,-1.90781,0.099463,1.91437,-2.03474,0.138966,1.74142,-2.2205,0.185451,1.65676,-2.25592,0.322493,1.67531,-2.11521,0.384303,1.66947,-2.09963,0.197927,1.45486,-1.80542,0.012978,1.53226,-1.86905,0.016276,1.55313,-1.93152,0.020631,1.53037,-1.98409,0.194216,1.43899,-2.06294,0.322,1.23541,-1 [...]
+/*1065*/{0.163243,1.95359,-1.71153,0.273648,1.88921,-1.83375,0.011135,1.85703,-1.65187,-0.104994,1.77362,-1.62436,0.034022,1.69147,-1.64617,0.106104,1.63516,-1.64079,0.243565,1.96137,-2.08741,0.290595,1.89207,-1.90755,0.099851,1.91724,-2.03511,0.141561,1.74269,-2.22017,0.186876,1.65759,-2.25621,0.32349,1.67548,-2.11426,0.385597,1.66923,-2.09881,0.19813,1.45539,-1.80519,0.013432,1.53334,-1.86748,0.015956,1.55523,-1.93046,0.020851,1.5324,-1.98248,0.194398,1.4417,-2.06254,0.318381,1.23146,- [...]
+/*1066*/{0.164462,1.95479,-1.71198,0.27302,1.89294,-1.83466,0.011231,1.85919,-1.65295,-0.107989,1.77546,-1.62756,0.033881,1.69329,-1.64777,0.104612,1.636,-1.64062,0.244885,1.96172,-2.08697,0.290757,1.89317,-1.90778,0.10061,1.91886,-2.035,0.141606,1.74361,-2.22002,0.187922,1.65791,-2.25634,0.325146,1.67596,-2.11411,0.387277,1.66673,-2.09488,0.199726,1.45597,-1.80554,0.015067,1.53437,-1.86728,0.015989,1.55683,-1.92857,0.019994,1.53428,-1.98138,0.194606,1.44429,-2.06253,0.315081,1.22649,-1. [...]
+/*1067*/{0.166377,1.95601,-1.71167,0.273637,1.89396,-1.83478,0.011374,1.86126,-1.65493,-0.108141,1.77761,-1.6301,0.032919,1.6943,-1.64812,0.103606,1.63645,-1.64029,0.244949,1.96243,-2.08612,0.290947,1.89387,-1.90866,0.101257,1.92057,-2.03497,0.141425,1.7441,-2.21946,0.189006,1.65799,-2.25611,0.325833,1.67367,-2.11277,0.3881,1.66619,-2.0957,0.200208,1.45649,-1.80581,0.015263,1.53518,-1.86498,0.017061,1.55772,-1.92707,0.019578,1.53512,-1.9797,0.194531,1.44611,-2.06226,0.31173,1.21935,-1.75 [...]
+/*1068*/{0.167597,1.95657,-1.71203,0.273895,1.89398,-1.83473,0.012331,1.86269,-1.65586,-0.107387,1.77956,-1.63244,0.032605,1.69513,-1.64961,0.103497,1.63631,-1.63955,0.24465,1.96245,-2.08594,0.290943,1.89446,-1.90883,0.100716,1.9218,-2.03498,0.141772,1.74431,-2.2185,0.188744,1.65797,-2.25542,0.326213,1.67221,-2.11181,0.38906,1.66382,-2.09495,0.199088,1.45666,-1.80618,0.015553,1.5339,-1.86371,0.016596,1.55786,-1.92547,0.020095,1.53552,-1.97864,0.194416,1.44798,-2.06223,0.308416,1.21263,-1 [...]
+/*1069*/{0.169789,1.95697,-1.71217,0.274007,1.89453,-1.83582,0.012763,1.86413,-1.65709,-0.107495,1.78082,-1.63514,0.032059,1.69541,-1.65014,0.103171,1.63591,-1.6386,0.244216,1.96228,-2.08557,0.290603,1.89451,-1.9093,0.100506,1.92233,-2.03445,0.141061,1.74481,-2.21739,0.189237,1.65794,-2.25439,0.326036,1.67061,-2.1117,0.389475,1.66083,-2.09451,0.201858,1.45666,-1.80581,0.015211,1.53366,-1.86281,0.016785,1.5579,-1.92381,0.019497,1.53574,-1.97663,0.194825,1.44888,-2.06235,0.303518,1.20569,- [...]
+/*1070*/{0.170617,1.95747,-1.71208,0.27694,1.89341,-1.83617,0.013627,1.86486,-1.65731,-0.106303,1.78219,-1.63661,0.032205,1.69584,-1.65052,0.10255,1.63472,-1.63803,0.243623,1.96184,-2.08538,0.290288,1.89438,-1.91019,0.100106,1.92272,-2.03341,0.139043,1.74422,-2.21689,0.18851,1.6579,-2.25343,0.328056,1.66776,-2.11196,0.389808,1.65785,-2.09413,0.20344,1.45607,-1.80614,0.016515,1.5321,-1.86137,0.0165,1.55793,-1.92345,0.020165,1.53515,-1.97556,0.195398,1.44934,-2.06231,0.300578,1.19896,-1.75 [...]
+/*1071*/{0.171868,1.95695,-1.7124,0.265654,1.89576,-1.83939,0.014674,1.8645,-1.65886,-0.104468,1.78294,-1.63792,0.032523,1.69521,-1.65076,0.10226,1.6339,-1.63683,0.243261,1.96107,-2.08498,0.289516,1.89437,-1.91098,0.099386,1.92308,-2.03268,0.137542,1.74429,-2.21486,0.187411,1.65746,-2.25148,0.329194,1.6655,-2.1127,0.390317,1.65372,-2.09551,0.203163,1.45527,-1.80652,0.015601,1.53088,-1.86022,0.016129,1.55729,-1.92239,0.019994,1.53361,-1.97415,0.19572,1.44938,-2.06239,0.295356,1.19228,-1.7 [...]
+/*1072*/{0.173226,1.95648,-1.71244,0.264537,1.89467,-1.83957,0.015633,1.8642,-1.65943,-0.102953,1.78273,-1.63859,0.032938,1.69402,-1.65046,0.102135,1.63197,-1.63568,0.240772,1.96065,-2.08409,0.288802,1.89344,-1.91192,0.097817,1.92275,-2.03213,0.13546,1.74346,-2.21405,0.185593,1.65697,-2.2501,0.329054,1.66179,-2.11296,0.390371,1.65033,-2.09829,0.203376,1.45437,-1.80704,0.017056,1.52809,-1.85929,0.016994,1.55448,-1.92149,0.020251,1.53254,-1.97401,0.195954,1.44924,-2.06224,0.293551,1.18484, [...]
+/*1073*/{0.174714,1.95529,-1.7125,0.264761,1.89339,-1.83995,0.017033,1.86319,-1.65876,-0.102229,1.78173,-1.63929,0.033756,1.69213,-1.64952,0.102353,1.62969,-1.63347,0.240111,1.95962,-2.08362,0.288823,1.89257,-1.91195,0.096245,1.92176,-2.03168,0.132581,1.74273,-2.21214,0.183459,1.65598,-2.24809,0.32884,1.65795,-2.11349,0.389887,1.64607,-2.09879,0.203579,1.4532,-1.80714,0.01765,1.52558,-1.85796,0.017151,1.55248,-1.92081,0.019603,1.52981,-1.97253,0.196301,1.44882,-2.06179,0.290641,1.17793,- [...]
+/*1074*/{0.175639,1.95392,-1.71227,0.264853,1.89239,-1.84032,0.017683,1.86189,-1.65937,-0.099847,1.7801,-1.63886,0.034012,1.69036,-1.64847,0.103616,1.62727,-1.63275,0.237119,1.95747,-2.08349,0.28853,1.89141,-1.91255,0.09523,1.9206,-2.0302,0.129447,1.74178,-2.2103,0.180786,1.65509,-2.24653,0.328375,1.65367,-2.11414,0.388968,1.641,-2.09957,0.203014,1.45064,-1.80762,0.016921,1.5233,-1.85766,0.016735,1.55016,-1.92033,0.019946,1.52767,-1.97212,0.197019,1.44713,-2.06149,0.286267,1.1709,-1.7501 [...]
+/*1075*/{0.176597,1.95178,-1.71226,0.264125,1.89069,-1.84117,0.019264,1.85933,-1.65837,-0.098728,1.77818,-1.6383,0.035772,1.68899,-1.64752,0.104606,1.62546,-1.63114,0.236363,1.95662,-2.08377,0.287453,1.88995,-1.9129,0.093376,1.91798,-2.02882,0.12524,1.74039,-2.20804,0.177589,1.65317,-2.24483,0.326799,1.64861,-2.11447,0.38725,1.63546,-2.10047,0.203537,1.44914,-1.80677,0.017739,1.51999,-1.8567,0.016829,1.54718,-1.91976,0.019624,1.52543,-1.97205,0.197436,1.4458,-2.0612,0.284067,1.16338,-1.7 [...]
+/*1076*/{0.176863,1.94967,-1.71196,0.263041,1.88921,-1.84234,0.020512,1.85727,-1.65711,-0.096977,1.77461,-1.63661,0.037124,1.6854,-1.64648,0.105077,1.62177,-1.62924,0.234017,1.95444,-2.08413,0.287766,1.88815,-1.91351,0.092496,1.9152,-2.02771,0.121022,1.73819,-2.20641,0.173237,1.65068,-2.24338,0.325617,1.64417,-2.11497,0.386173,1.6289,-2.10154,0.204666,1.44609,-1.80708,0.018357,1.51703,-1.85623,0.016244,1.54352,-1.9195,0.020579,1.52182,-1.97148,0.198199,1.44391,-2.06064,0.280973,1.15856,- [...]
+/*1077*/{0.177801,1.94708,-1.71183,0.263391,1.88716,-1.84263,0.022277,1.85382,-1.65487,-0.094407,1.77102,-1.63464,0.037818,1.682,-1.64386,0.107986,1.61826,-1.62729,0.231384,1.95208,-2.08484,0.286731,1.8859,-1.91388,0.090216,1.91257,-2.02691,0.116873,1.73566,-2.20509,0.169353,1.64795,-2.24235,0.322778,1.63879,-2.11561,0.383975,1.62281,-2.1037,0.204419,1.44291,-1.80707,0.017874,1.51383,-1.85594,0.016989,1.54004,-1.91847,0.019924,1.51853,-1.97137,0.199249,1.4422,-2.06007,0.27852,1.15397,-1. [...]
+/*1078*/{0.178687,1.94384,-1.71181,0.264445,1.88449,-1.84292,0.02318,1.84954,-1.65391,-0.092959,1.76662,-1.63219,0.040649,1.67833,-1.64137,0.110139,1.61407,-1.62515,0.229447,1.9495,-2.08564,0.286319,1.88329,-1.91482,0.08898,1.90902,-2.02539,0.113487,1.73326,-2.20307,0.165464,1.64487,-2.24155,0.320139,1.63327,-2.11652,0.381123,1.61682,-2.10485,0.204809,1.44029,-1.80679,0.017097,1.51012,-1.85483,0.017101,1.53687,-1.91794,0.020072,1.51411,-1.97135,0.199655,1.43972,-2.0593,0.276115,1.14923,- [...]
+/*1079*/{0.179049,1.93993,-1.71156,0.275149,1.8791,-1.84058,0.02463,1.84518,-1.65281,-0.091781,1.76071,-1.62922,0.042446,1.67311,-1.6383,0.112314,1.60943,-1.62263,0.226613,1.94667,-2.08647,0.286727,1.88023,-1.91487,0.087481,1.90544,-2.02504,0.108314,1.73022,-2.20156,0.160264,1.64108,-2.24029,0.316805,1.62691,-2.1175,0.378047,1.6087,-2.10676,0.203786,1.43665,-1.8069,0.017168,1.50667,-1.85468,0.016739,1.53328,-1.91791,0.02041,1.51039,-1.97118,0.200469,1.43725,-2.05849,0.275366,1.14558,-1.7 [...]
+/*1080*/{0.179032,1.93582,-1.7113,0.27418,1.87725,-1.8417,0.026782,1.83912,-1.65061,-0.087763,1.75573,-1.62557,0.044898,1.66751,-1.63563,0.115817,1.60541,-1.62053,0.224249,1.94349,-2.0873,0.286346,1.87779,-1.91542,0.085934,1.90074,-2.02382,0.105664,1.72631,-2.20071,0.155434,1.6371,-2.23955,0.313919,1.62073,-2.11908,0.376358,1.60098,-2.10879,0.204189,1.43367,-1.80655,0.017555,1.50174,-1.85422,0.016689,1.52882,-1.91787,0.019705,1.50621,-1.97176,0.201258,1.43468,-2.05807,0.273114,1.14267,-1 [...]
+/*1081*/{0.179773,1.93179,-1.71086,0.273994,1.87415,-1.84177,0.027965,1.83407,-1.64827,-0.0867,1.74929,-1.6217,0.048595,1.6617,-1.63221,0.119506,1.59936,-1.61752,0.221878,1.93976,-2.08888,0.286249,1.8743,-1.9159,0.084374,1.89588,-2.02336,0.10024,1.72283,-2.19929,0.150608,1.63316,-2.23867,0.310026,1.61279,-2.12011,0.370378,1.59319,-2.11219,0.203842,1.43035,-1.80634,0.017842,1.49749,-1.85414,0.017305,1.52509,-1.91723,0.020243,1.50105,-1.9709,0.201295,1.43185,-2.05753,0.272452,1.13959,-1.74 [...]
+/*1082*/{0.180195,1.92701,-1.7107,0.275024,1.86916,-1.84162,0.030284,1.82787,-1.64589,-0.084141,1.74169,-1.61734,0.052551,1.65593,-1.62834,0.123289,1.59453,-1.616,0.219587,1.9355,-2.08965,0.285642,1.87104,-1.91626,0.083425,1.8907,-2.02151,0.093543,1.71719,-2.19867,0.144495,1.62891,-2.23827,0.305464,1.60507,-2.12183,0.366076,1.58418,-2.11485,0.203291,1.42698,-1.80632,0.017898,1.49325,-1.85514,0.017404,1.52046,-1.91706,0.020016,1.49606,-1.97147,0.201486,1.42887,-2.05655,0.270122,1.13547,-1 [...]
+/*1083*/{0.180214,1.92177,-1.71034,0.274833,1.86521,-1.8426,0.032416,1.82099,-1.64307,-0.080658,1.73519,-1.61307,0.055576,1.64844,-1.62476,0.127888,1.58835,-1.61337,0.2186,1.93133,-2.09091,0.285992,1.86791,-1.91694,0.081282,1.88621,-2.02117,0.090565,1.71277,-2.19738,0.139619,1.62427,-2.23771,0.300833,1.59766,-2.12378,0.362185,1.57539,-2.11774,0.203611,1.42376,-1.80599,0.017543,1.48849,-1.85512,0.016013,1.51622,-1.9161,0.019716,1.49122,-1.97154,0.202369,1.42528,-2.05638,0.26839,1.13165,-1 [...]
+/*1084*/{0.181243,1.91663,-1.7099,0.275396,1.85995,-1.84312,0.034768,1.81424,-1.6407,-0.077139,1.72676,-1.60868,0.05985,1.64157,-1.62144,0.132781,1.58199,-1.61091,0.215777,1.92649,-2.09213,0.285592,1.86315,-1.9178,0.080383,1.8811,-2.02043,0.084313,1.7084,-2.19672,0.133103,1.62022,-2.23735,0.296981,1.59,-2.12528,0.356956,1.5654,-2.12072,0.204131,1.42189,-1.80628,0.016686,1.48335,-1.85562,0.016792,1.51069,-1.91637,0.019424,1.48562,-1.97195,0.202782,1.42124,-2.05518,0.266948,1.12741,-1.7423 [...]
+/*1085*/{0.181835,1.91093,-1.70962,0.276182,1.85518,-1.84377,0.03711,1.80656,-1.63723,-0.074141,1.71876,-1.60347,0.064565,1.63428,-1.6176,0.138274,1.57554,-1.609,0.214203,1.92166,-2.09335,0.285915,1.85855,-1.91859,0.079663,1.8759,-2.0199,0.079584,1.70396,-2.19577,0.127867,1.61567,-2.23654,0.291285,1.58155,-2.12769,0.351231,1.55548,-2.12348,0.204617,1.41822,-1.80488,0.015992,1.47813,-1.85594,0.016066,1.50589,-1.91704,0.019199,1.48001,-1.97216,0.203651,1.41708,-2.05409,0.266448,1.12308,-1. [...]
+/*1086*/{0.182483,1.90534,-1.70925,0.275515,1.8502,-1.84477,0.040084,1.79871,-1.63481,-0.069125,1.71008,-1.59916,0.06984,1.62671,-1.61415,0.143102,1.56917,-1.60617,0.21253,1.91683,-2.09395,0.285253,1.85456,-1.92015,0.077936,1.87075,-2.01939,0.074816,1.69922,-2.19543,0.121027,1.61111,-2.23583,0.286443,1.57324,-2.12907,0.345487,1.54544,-2.12672,0.20548,1.41488,-1.80466,0.015846,1.47324,-1.8558,0.016316,1.5002,-1.91694,0.017663,1.4734,-1.97174,0.202884,1.41328,-2.05467,0.264833,1.11865,-1.7 [...]
+/*1087*/{0.182959,1.89958,-1.70885,0.275961,1.84552,-1.84465,0.043546,1.79026,-1.63119,-0.064126,1.70125,-1.59379,0.074903,1.6179,-1.61054,0.149238,1.56183,-1.60415,0.210631,1.91184,-2.0951,0.285759,1.8499,-1.92054,0.076593,1.86594,-2.01843,0.070304,1.69442,-2.19481,0.114839,1.6061,-2.23545,0.280664,1.56433,-2.13166,0.339494,1.5346,-2.13007,0.204954,1.40941,-1.80309,0.014697,1.46695,-1.85699,0.015693,1.49459,-1.91674,0.017309,1.46886,-1.97279,0.20262,1.40846,-2.05489,0.263013,1.11253,-1. [...]
+/*1088*/{0.183498,1.89366,-1.70768,0.276167,1.84187,-1.84595,0.046786,1.7821,-1.62789,-0.057976,1.69134,-1.58872,0.081169,1.60968,-1.60699,0.155825,1.55501,-1.6023,0.20965,1.90744,-2.09603,0.285465,1.84387,-1.92136,0.076289,1.86071,-2.01708,0.06409,1.69084,-2.19445,0.108284,1.60146,-2.23541,0.274254,1.55405,-2.13415,0.332697,1.52339,-2.13407,0.203469,1.40527,-1.80265,0.013418,1.46166,-1.85771,0.014342,1.4907,-1.91703,0.016003,1.46315,-1.9736,0.202758,1.40376,-2.05465,0.260562,1.10724,-1. [...]
+/*1089*/{0.185159,1.88772,-1.707,0.277231,1.83549,-1.84652,0.050835,1.7731,-1.62485,-0.052603,1.68164,-1.5834,0.087853,1.6016,-1.60305,0.162767,1.5479,-1.59983,0.208507,1.90251,-2.0971,0.286361,1.84016,-1.92235,0.075268,1.85694,-2.01608,0.060039,1.68765,-2.19381,0.101501,1.59707,-2.23558,0.268406,1.54453,-2.13595,0.325907,1.51261,-2.13745,0.202047,1.40047,-1.80076,0.010573,1.45716,-1.85833,0.012341,1.48233,-1.91731,0.014655,1.45803,-1.97496,0.202214,1.39933,-2.0546,0.25415,1.1042,-1.7435 [...]
+/*1090*/{0.186224,1.88231,-1.70578,0.278304,1.83145,-1.84766,0.055511,1.7643,-1.62114,-0.046802,1.67153,-1.57814,0.095064,1.59396,-1.59984,0.170109,1.54132,-1.59813,0.2072,1.89809,-2.09842,0.286776,1.83481,-1.92328,0.074402,1.85361,-2.01549,0.053318,1.68212,-2.1932,0.094953,1.59219,-2.23545,0.262141,1.53586,-2.13921,0.318298,1.50153,-2.14112,0.200726,1.39558,-1.79958,0.007541,1.45254,-1.85931,0.010229,1.47689,-1.91917,0.012171,1.4523,-1.9755,0.200984,1.39434,-2.0539,0.25616,1.10172,-1.73 [...]
+/*1091*/{0.187208,1.8766,-1.70502,0.279585,1.82643,-1.84785,0.060064,1.75459,-1.61769,-0.03763,1.66146,-1.57286,0.101917,1.58509,-1.59675,0.178873,1.53514,-1.59704,0.206418,1.89348,-2.09951,0.287845,1.8302,-1.92446,0.074192,1.85165,-2.01381,0.049507,1.67892,-2.19238,0.087401,1.5875,-2.23538,0.255224,1.52606,-2.14228,0.310857,1.49001,-2.14454,0.198064,1.3907,-1.79844,0.00407,1.44919,-1.85942,0.007871,1.47186,-1.92036,0.009712,1.44674,-1.97645,0.202083,1.38927,-2.05181,0.252411,1.10088,-1. [...]
+/*1092*/{0.188746,1.87257,-1.70369,0.279653,1.82051,-1.8485,0.06664,1.74524,-1.61379,-0.031915,1.65072,-1.56763,0.110198,1.5772,-1.59331,0.187332,1.52904,-1.59482,0.205735,1.88992,-2.10137,0.28859,1.82653,-1.92562,0.073567,1.84931,-2.01243,0.045452,1.6768,-2.19218,0.080264,1.58357,-2.23451,0.247111,1.51568,-2.14483,0.302648,1.47878,-2.14817,0.195948,1.38876,-1.79808,0.000461,1.4464,-1.86292,0.003954,1.4679,-1.92203,0.006873,1.442,-1.97818,0.201045,1.38519,-2.05114,0.250545,1.10177,-1.730 [...]
+/*1093*/{0.189917,1.86807,-1.70223,0.281124,1.81625,-1.84977,0.072718,1.7357,-1.61081,-0.022299,1.6406,-1.56157,0.118934,1.57056,-1.59144,0.196637,1.52369,-1.59423,0.205781,1.88673,-2.10227,0.289469,1.82251,-1.92771,0.073839,1.84881,-2.01182,0.042336,1.675,-2.19043,0.073169,1.58011,-2.2335,0.240452,1.50728,-2.14796,0.29387,1.46838,-2.15181,0.193597,1.38717,-1.79675,-0.003337,1.44403,-1.86405,0.001609,1.46544,-1.9226,0.003452,1.43791,-1.98039,0.198741,1.38113,-2.05045,0.247736,1.10111,-1. [...]
+/*1094*/{0.191071,1.86443,-1.70091,0.281121,1.81438,-1.85072,0.079394,1.72667,-1.60763,-0.011631,1.63048,-1.55696,0.129126,1.56334,-1.58875,0.206709,1.51891,-1.59307,0.20667,1.88449,-2.10297,0.28961,1.81874,-1.92927,0.073863,1.84856,-2.01146,0.038985,1.6737,-2.18826,0.06681,1.57892,-2.23162,0.232079,1.4971,-2.15005,0.284912,1.45765,-2.15496,0.190577,1.38648,-1.79479,-0.006398,1.44182,-1.86684,3.2e-005,1.46324,-1.92402,0.001491,1.43264,-1.98232,0.196166,1.37751,-2.04962,0.245078,1.10107,- [...]
+/*1095*/{0.192807,1.86096,-1.70021,0.281164,1.80976,-1.85253,0.086108,1.71907,-1.60585,-0.00189,1.62112,-1.55198,0.139601,1.55832,-1.58754,0.217542,1.51587,-1.59202,0.207394,1.88223,-2.10273,0.290178,1.81572,-1.9302,0.074017,1.84847,-2.01147,0.033868,1.67518,-2.18522,0.061924,1.57922,-2.22934,0.225153,1.4903,-2.15223,0.275684,1.44817,-2.15814,0.18796,1.38569,-1.79295,-0.00675,1.4399,-1.86728,-0.001687,1.46305,-1.92534,-0.000676,1.42969,-1.98476,0.194844,1.37519,-2.0493,0.243099,1.10057,- [...]
+/*1096*/{0.19357,1.85817,-1.70014,0.280806,1.80706,-1.85269,0.092756,1.71286,-1.60473,0.007769,1.61384,-1.54867,0.150337,1.55504,-1.58801,0.228225,1.5129,-1.59157,0.207696,1.88074,-2.10259,0.289717,1.81319,-1.93117,0.074506,1.84844,-2.01155,0.029883,1.67582,-2.18206,0.057447,1.5813,-2.22675,0.217311,1.48398,-2.15298,0.265762,1.43974,-2.1616,0.18497,1.38552,-1.7905,-0.009452,1.439,-1.8692,-0.002991,1.46202,-1.92592,-0.002475,1.42802,-1.98579,0.192854,1.37263,-2.049,0.239748,1.09812,-1.729 [...]
+/*1097*/{0.193982,1.85665,-1.70076,0.281004,1.80661,-1.8535,0.098084,1.70792,-1.60366,0.018559,1.60793,-1.54546,0.161359,1.55189,-1.58748,0.241143,1.51169,-1.59088,0.208991,1.87917,-2.10223,0.287939,1.81177,-1.93192,0.074668,1.84841,-2.01186,0.026325,1.67817,-2.17909,0.052185,1.58408,-2.22409,0.208558,1.47925,-2.15363,0.2555,1.43215,-2.16498,0.18203,1.38549,-1.78851,-0.010392,1.43806,-1.87002,-0.004354,1.46189,-1.92565,-0.004589,1.42716,-1.98602,0.192333,1.37103,-2.04841,0.233787,1.09567 [...]
+/*1098*/{0.194834,1.85531,-1.70109,0.28044,1.80471,-1.85401,0.102836,1.70536,-1.60319,0.028733,1.60398,-1.54281,0.173052,1.54984,-1.58703,0.252699,1.51174,-1.59138,0.209683,1.87835,-2.10183,0.287464,1.81038,-1.93141,0.074645,1.84835,-2.01205,0.02287,1.68165,-2.17557,0.045672,1.5875,-2.22344,0.199876,1.47589,-2.1558,0.245221,1.427,-2.16865,0.18042,1.38479,-1.78777,-0.012594,1.43767,-1.87069,-0.006567,1.46131,-1.92566,-0.005583,1.42706,-1.98614,0.189179,1.37023,-2.0483,0.228168,1.09371,-1. [...]
+/*1099*/{0.196075,1.85372,-1.70122,0.279734,1.80193,-1.853,0.108265,1.70294,-1.60217,0.038658,1.60075,-1.54041,0.184175,1.54948,-1.58653,0.265579,1.51282,-1.59198,0.20987,1.8777,-2.10119,0.286941,1.80901,-1.93107,0.074023,1.84794,-2.0121,0.018406,1.68511,-2.17216,0.039271,1.59082,-2.22195,0.190776,1.47355,-2.15792,0.234568,1.42354,-2.17133,0.177813,1.38454,-1.78532,-0.01286,1.43757,-1.87047,-0.007069,1.46085,-1.92496,-0.007525,1.42792,-1.98587,0.187689,1.37043,-2.04833,0.222668,1.08869,- [...]
+/*1100*/{0.197605,1.85346,-1.7012,0.279546,1.80132,-1.85319,0.113445,1.70171,-1.60204,0.04835,1.59674,-1.53818,0.195166,1.54996,-1.58665,0.277665,1.51549,-1.59338,0.209236,1.87707,-2.10126,0.286121,1.80823,-1.93063,0.074172,1.8484,-2.01244,0.0154,1.69018,-2.16874,0.032377,1.59423,-2.22047,0.182479,1.47326,-2.16011,0.224664,1.42093,-2.17384,0.176084,1.38426,-1.78501,-0.01487,1.43694,-1.87079,-0.008625,1.4606,-1.92526,-0.007895,1.42824,-1.98457,0.186309,1.37126,-2.04871,0.216981,1.085,-1.7 [...]
+/*1101*/{0.199223,1.85236,-1.70125,0.279575,1.80026,-1.8532,0.118084,1.70113,-1.60156,0.057984,1.59497,-1.53667,0.205988,1.55137,-1.58656,0.290267,1.51903,-1.59478,0.208374,1.87655,-2.10062,0.285118,1.80663,-1.93055,0.073475,1.84882,-2.01208,0.012316,1.69575,-2.16551,0.025706,1.59843,-2.22026,0.174428,1.47347,-2.16266,0.214093,1.42074,-2.17678,0.175057,1.38434,-1.78411,-0.015739,1.4364,-1.87055,-0.010422,1.46045,-1.92495,-0.010264,1.42973,-1.98399,0.184713,1.37231,-2.04893,0.210586,1.080 [...]
+/*1102*/{0.201167,1.85285,-1.70101,0.277761,1.80118,-1.85349,0.124014,1.70127,-1.60053,0.06833,1.5926,-1.53399,0.216572,1.55364,-1.58747,0.302792,1.52359,-1.59758,0.207317,1.87659,-2.10041,0.285018,1.80645,-1.9307,0.073252,1.84924,-2.01188,0.007063,1.70172,-2.16338,0.018634,1.60391,-2.22012,0.165103,1.47448,-2.16554,0.204132,1.42094,-2.17903,0.173018,1.38428,-1.7822,-0.017431,1.43608,-1.87039,-0.01067,1.46046,-1.92465,-0.010641,1.43007,-1.98334,0.183609,1.37352,-2.04903,0.202845,1.07775, [...]
+/*1103*/{0.202659,1.854,-1.70136,0.277856,1.80248,-1.85391,0.127592,1.70188,-1.60007,0.077741,1.59153,-1.53213,0.227041,1.55678,-1.58772,0.314066,1.52918,-1.6009,0.206385,1.87742,-2.10063,0.284091,1.80652,-1.931,0.072067,1.84925,-2.01057,0.00354,1.70708,-2.16073,0.01088,1.60937,-2.22008,0.156516,1.47687,-2.16852,0.193882,1.42298,-2.18126,0.171987,1.38439,-1.78177,-0.018186,1.43585,-1.87042,-0.010879,1.46054,-1.92435,-0.011589,1.43046,-1.98315,0.183542,1.37535,-2.04882,0.195236,1.07534,-1 [...]
+/*1104*/{0.204342,1.85548,-1.7017,0.277263,1.80264,-1.85421,0.132535,1.70354,-1.59921,0.086894,1.59023,-1.53069,0.236732,1.56056,-1.58925,0.324674,1.53568,-1.60411,0.20483,1.87819,-2.10066,0.283425,1.80746,-1.93188,0.070763,1.85159,-2.00953,-0.001913,1.71305,-2.16008,0.002777,1.61576,-2.21985,0.14812,1.48003,-2.17077,0.184439,1.42601,-2.1822,0.171152,1.38415,-1.78203,-0.018963,1.43615,-1.87081,-0.011568,1.46138,-1.92437,-0.012405,1.43116,-1.98286,0.182231,1.37722,-2.04934,0.185268,1.0728 [...]
+/*1105*/{0.20624,1.85833,-1.70213,0.278085,1.80462,-1.85555,0.137187,1.70529,-1.59865,0.097632,1.59034,-1.52931,0.245872,1.56543,-1.59117,0.33473,1.54313,-1.60772,0.203207,1.87964,-2.10064,0.283038,1.80876,-1.93256,0.070244,1.85284,-2.00871,-0.006373,1.71901,-2.15884,-0.003695,1.62183,-2.22005,0.139168,1.48487,-2.17216,0.173923,1.4303,-2.18331,0.170988,1.38443,-1.78196,-0.019633,1.43791,-1.87043,-0.011489,1.46226,-1.92446,-0.012465,1.43223,-1.98231,0.183085,1.37973,-2.04985,0.179197,1.07 [...]
+/*1106*/{0.208307,1.86147,-1.70276,0.277617,1.80602,-1.85592,0.142742,1.70703,-1.59827,0.105872,1.59201,-1.52885,0.255364,1.57098,-1.59334,0.343308,1.55127,-1.61115,0.201173,1.88155,-2.10032,0.282197,1.80998,-1.9332,0.068605,1.85483,-2.00683,-0.011713,1.72505,-2.15784,-0.011601,1.62892,-2.21983,0.13121,1.49023,-2.17222,0.164453,1.43548,-2.18338,0.170435,1.38459,-1.78174,-0.018697,1.43937,-1.87078,-0.012249,1.46416,-1.92361,-0.012417,1.43297,-1.98289,0.182824,1.38215,-2.04959,0.170929,1.0 [...]
+/*1107*/{0.209745,1.86428,-1.70351,0.277622,1.80785,-1.85743,0.148529,1.70975,-1.5985,0.114322,1.59355,-1.52792,0.26345,1.57692,-1.59567,0.35214,1.55996,-1.61568,0.197067,1.88362,-2.09994,0.280859,1.81225,-1.93416,0.067543,1.85602,-2.00539,-0.017823,1.73206,-2.15791,-0.018835,1.63586,-2.21928,0.121832,1.496,-2.17225,0.154802,1.44153,-2.1836,0.169889,1.3851,-1.78166,-0.018574,1.44144,-1.86999,-0.012351,1.4643,-1.92354,-0.012856,1.43504,-1.98324,0.182452,1.38468,-2.04948,0.163548,1.06876,- [...]
+/*1108*/{0.212202,1.86806,-1.70424,0.277797,1.8102,-1.85803,0.154623,1.71245,-1.598,0.124276,1.59601,-1.52697,0.271202,1.58451,-1.59846,0.35963,1.5692,-1.6201,0.194801,1.88653,-2.1004,0.280723,1.81431,-1.93508,0.06566,1.85778,-2.00304,-0.023758,1.73854,-2.15725,-0.026133,1.64262,-2.21967,0.113055,1.50279,-2.17181,0.146461,1.44829,-2.18333,0.169724,1.38484,-1.78088,-0.018225,1.44369,-1.86939,-0.011991,1.46701,-1.92298,-0.01179,1.43639,-1.98287,0.183059,1.38795,-2.04893,0.161154,1.07141,-1 [...]
+/*1109*/{0.214782,1.87201,-1.70494,0.278978,1.81376,-1.8597,0.160677,1.71563,-1.59797,0.131944,1.59849,-1.52748,0.278855,1.59079,-1.60152,0.366971,1.57932,-1.62476,0.192358,1.88947,-2.10077,0.279843,1.81693,-1.93653,0.064459,1.85965,-2.00177,-0.029963,1.74482,-2.15629,-0.033105,1.64982,-2.21948,0.105493,1.51029,-2.17121,0.137774,1.45527,-2.18246,0.169979,1.38525,-1.7814,-0.018633,1.44552,-1.86937,-0.010352,1.46925,-1.92128,-0.011449,1.4392,-1.98273,0.183382,1.3911,-2.04775,0.149499,1.068 [...]
+/*1110*/{0.216938,1.87643,-1.70533,0.278606,1.81633,-1.86113,0.167246,1.71887,-1.59793,0.14193,1.60139,-1.52689,0.285763,1.59921,-1.60423,0.373186,1.58883,-1.62924,0.189088,1.89357,-2.1002,0.279787,1.8203,-1.93828,0.063612,1.86265,-1.99942,-0.035908,1.75214,-2.15601,-0.039413,1.65694,-2.21858,0.097184,1.51834,-2.17043,0.129759,1.46307,-2.18167,0.17021,1.38557,-1.78077,-0.017595,1.44804,-1.86905,-0.010886,1.47086,-1.92152,-0.010653,1.44229,-1.98275,0.183949,1.39409,-2.0468,0.142238,1.0681 [...]
+/*1111*/{0.219516,1.88064,-1.7065,0.279939,1.8202,-1.86165,0.173435,1.72271,-1.59813,0.14972,1.6054,-1.52698,0.291763,1.60626,-1.60725,0.378838,1.59876,-1.63395,0.18482,1.89705,-2.09967,0.279575,1.82303,-1.93878,0.062187,1.86418,-1.99722,-0.040269,1.75946,-2.15519,-0.04618,1.66409,-2.21801,0.090137,1.52469,-2.16833,0.121993,1.4714,-2.18089,0.1709,1.38599,-1.77992,-0.016338,1.45099,-1.86843,-0.010183,1.47366,-1.92073,-0.010443,1.44549,-1.9831,0.185476,1.39732,-2.04605,0.13494,1.06833,-1.7 [...]
+/*1112*/{0.22177,1.88538,-1.70734,0.280321,1.82372,-1.86382,0.180227,1.72682,-1.59886,0.158143,1.6089,-1.52588,0.297878,1.61417,-1.61,0.383902,1.60858,-1.63899,0.182645,1.90097,-2.09899,0.279294,1.82698,-1.94052,0.060799,1.8676,-1.9952,-0.045692,1.76664,-2.15486,-0.052466,1.6713,-2.21705,0.082525,1.53326,-2.16708,0.114442,1.47925,-2.18002,0.171558,1.38653,-1.77878,-0.015213,1.45323,-1.86793,-0.009031,1.47601,-1.91928,-0.008472,1.44853,-1.9821,0.186102,1.40059,-2.04491,0.128537,1.06969,-1 [...]
+/*1113*/{0.223664,1.89014,-1.70888,0.280491,1.8269,-1.86549,0.186016,1.73106,-1.59962,0.165977,1.61274,-1.52571,0.302454,1.62183,-1.6126,0.388474,1.61923,-1.64409,0.179237,1.90504,-2.09859,0.278842,1.83129,-1.94233,0.059782,1.86992,-1.99298,-0.051009,1.77353,-2.15412,-0.057976,1.67837,-2.2158,0.075862,1.54192,-2.16524,0.108186,1.48811,-2.17889,0.172119,1.38736,-1.77727,-0.0138,1.45563,-1.86844,-0.008085,1.47944,-1.91818,-0.00741,1.45272,-1.98221,0.187346,1.40408,-2.04334,0.122279,1.07068 [...]
+/*1114*/{0.225352,1.89451,-1.71048,0.282473,1.83178,-1.86645,0.192116,1.7357,-1.60061,0.173311,1.61725,-1.52543,0.307047,1.62993,-1.61537,0.392988,1.62908,-1.64888,0.175803,1.90927,-2.09813,0.278926,1.83492,-1.94386,0.058379,1.87323,-1.99145,-0.054506,1.78059,-2.15319,-0.064011,1.68595,-2.21457,0.069164,1.55053,-2.16344,0.101939,1.49633,-2.17788,0.172808,1.38789,-1.77686,-0.012339,1.45807,-1.86647,-0.006928,1.48229,-1.91686,-0.006196,1.45712,-1.98253,0.188197,1.40767,-2.04239,0.114995,1. [...]
+/*1115*/{0.227031,1.89986,-1.71181,0.28273,1.83535,-1.86798,0.198803,1.74079,-1.60069,0.180333,1.62234,-1.52546,0.311983,1.63791,-1.61835,0.395803,1.63861,-1.65342,0.173574,1.91279,-2.09753,0.278184,1.83853,-1.94468,0.057362,1.87699,-1.98934,-0.057326,1.78824,-2.15288,-0.069093,1.69303,-2.21244,0.063605,1.55899,-2.16125,0.096006,1.50476,-2.17721,0.174671,1.38897,-1.77566,-0.01077,1.46117,-1.86605,-0.005593,1.48572,-1.91476,-0.004231,1.46151,-1.98195,0.189595,1.41071,-2.0409,0.110607,1.07 [...]
+/*1116*/{0.229126,1.90435,-1.71319,0.282244,1.83891,-1.86983,0.203544,1.74575,-1.60157,0.186038,1.62667,-1.52551,0.315417,1.64496,-1.62064,0.398305,1.64815,-1.65786,0.17159,1.91677,-2.09664,0.277796,1.84278,-1.94635,0.057081,1.8801,-1.98873,-0.062782,1.7946,-2.15234,-0.075058,1.70049,-2.21088,0.057548,1.56742,-2.15963,0.090556,1.51307,-2.17628,0.174765,1.38932,-1.77521,-0.00862,1.46388,-1.86598,-0.00381,1.48936,-1.91405,-0.003266,1.46596,-1.98188,0.190586,1.41494,-2.03998,0.103088,1.0747 [...]
+/*1117*/{0.230071,1.90901,-1.71479,0.283018,1.84315,-1.87182,0.208331,1.74991,-1.60248,0.1921,1.6315,-1.52572,0.318228,1.65278,-1.62331,0.400555,1.65676,-1.66223,0.170061,1.92077,-2.09612,0.277793,1.84689,-1.94816,0.057883,1.8823,-1.98587,-0.065514,1.80146,-2.15078,-0.080342,1.70811,-2.20851,0.05137,1.57545,-2.15736,0.085023,1.52092,-2.1747,0.176077,1.39062,-1.7751,-0.007292,1.46717,-1.86543,-0.002766,1.49278,-1.91241,-0.00145,1.47061,-1.98059,0.191488,1.41831,-2.03902,0.098046,1.07785,- [...]
+/*1118*/{0.232375,1.91321,-1.71649,0.283103,1.84706,-1.87316,0.212742,1.75443,-1.60301,0.197222,1.63615,-1.52537,0.320312,1.65934,-1.62533,0.401968,1.66579,-1.66659,0.168613,1.92427,-2.0958,0.277833,1.85058,-1.9495,0.057622,1.88507,-1.98448,-0.068479,1.8075,-2.14959,-0.084604,1.71512,-2.20629,0.04705,1.58341,-2.15575,0.080617,1.52894,-2.17384,0.177862,1.39171,-1.77603,-0.005995,1.47028,-1.86465,-0.001527,1.49633,-1.91044,-0.000188,1.47551,-1.98082,0.192489,1.42194,-2.03861,0.091843,1.080 [...]
+/*1119*/{0.233796,1.91767,-1.71753,0.283702,1.85113,-1.87456,0.217055,1.75879,-1.60348,0.202554,1.6408,-1.52496,0.322828,1.66638,-1.62785,0.404067,1.67391,-1.67072,0.167259,1.92829,-2.09557,0.277118,1.85462,-1.95064,0.057562,1.88847,-1.98289,-0.070437,1.81421,-2.14779,-0.089005,1.72222,-2.20332,0.041914,1.59054,-2.1534,0.07609,1.53599,-2.17238,0.178854,1.39239,-1.77662,-0.003321,1.47337,-1.86339,-0.00023,1.49995,-1.90896,0.001825,1.48016,-1.98055,0.193606,1.42625,-2.03847,0.088767,1.0830 [...]
+/*1120*/{0.235492,1.9211,-1.71949,0.2847,1.85509,-1.87641,0.220925,1.76355,-1.60419,0.20722,1.64541,-1.52492,0.324595,1.67236,-1.63021,0.404416,1.68142,-1.67467,0.166484,1.93101,-2.0951,0.277431,1.85849,-1.95235,0.057319,1.89229,-1.98276,-0.072422,1.81991,-2.14654,-0.09325,1.72926,-2.20093,0.037673,1.59698,-2.15116,0.072116,1.54301,-2.17062,0.181428,1.39418,-1.77731,-0.00141,1.47729,-1.8627,0.001337,1.50357,-1.90698,0.003595,1.48475,-1.98003,0.193486,1.43033,-2.03784,0.084084,1.08727,-1. [...]
+/*1121*/{0.236751,1.92496,-1.72078,0.285215,1.85873,-1.87815,0.224015,1.76714,-1.60465,0.212204,1.64901,-1.52378,0.326511,1.67908,-1.63308,0.404241,1.68838,-1.67769,0.165681,1.93414,-2.09524,0.2771,1.86174,-1.95364,0.057925,1.89556,-1.98112,-0.072651,1.82592,-2.14396,-0.097006,1.73541,-2.19761,0.034577,1.60352,-2.14927,0.068422,1.55029,-2.16985,0.182566,1.39569,-1.77827,0.000358,1.48065,-1.86161,0.004584,1.50707,-1.90738,0.005131,1.48933,-1.97923,0.194836,1.43436,-2.03826,0.078643,1.0884 [...]
+/*1122*/{0.238823,1.92822,-1.72168,0.285688,1.86292,-1.87975,0.227135,1.77117,-1.60472,0.214551,1.65295,-1.52335,0.327149,1.68273,-1.63445,0.404466,1.69466,-1.68151,0.165468,1.93733,-2.09521,0.277377,1.865,-1.95424,0.058081,1.89877,-1.98087,-0.074133,1.83172,-2.14072,-0.100463,1.7419,-2.19489,0.032369,1.60934,-2.14729,0.066935,1.55526,-2.16783,0.184931,1.39754,-1.77959,0.001634,1.48387,-1.86156,0.005178,1.51046,-1.90556,0.006385,1.49387,-1.97714,0.195518,1.43833,-2.03881,0.074869,1.09115 [...]
+/*1123*/{0.239767,1.93098,-1.72304,0.28658,1.86619,-1.88161,0.230214,1.77422,-1.60523,0.218825,1.65672,-1.52369,0.327387,1.68689,-1.6364,0.40484,1.69963,-1.68439,0.164803,1.94061,-2.09497,0.277551,1.86843,-1.95571,0.057063,1.90149,-1.97955,-0.075531,1.83741,-2.13788,-0.103599,1.74797,-2.19157,0.029465,1.61572,-2.14558,0.062214,1.5619,-2.16741,0.186949,1.39979,-1.78016,0.003847,1.48749,-1.861,0.006032,1.51449,-1.90524,0.007599,1.49793,-1.97701,0.196248,1.44213,-2.03952,0.072293,1.09475,-1 [...]
+/*1124*/{0.241689,1.93392,-1.72384,0.287321,1.87,-1.88211,0.232236,1.77771,-1.60527,0.222139,1.65935,-1.52304,0.328871,1.69114,-1.63823,0.40523,1.70335,-1.68757,0.164698,1.94394,-2.09469,0.277734,1.87154,-1.95638,0.058501,1.90376,-1.97943,-0.07568,1.84266,-2.13464,-0.106128,1.7533,-2.18804,0.027335,1.62064,-2.14341,0.059504,1.56692,-2.16603,0.188424,1.40219,-1.78125,0.005344,1.49118,-1.85977,0.007365,1.51817,-1.90369,0.009015,1.50266,-1.97627,0.195872,1.44618,-2.03947,0.068224,1.09803,-1 [...]
+/*1125*/{0.243643,1.93626,-1.72441,0.287754,1.87325,-1.88345,0.233866,1.78057,-1.60591,0.224065,1.66226,-1.5231,0.329334,1.69406,-1.63841,0.405588,1.70688,-1.69022,0.16405,1.94624,-2.09421,0.277813,1.87444,-1.95745,0.059265,1.90727,-1.97852,-0.075779,1.8467,-2.13047,-0.108387,1.75901,-2.18481,0.024508,1.62563,-2.14231,0.057733,1.57129,-2.16469,0.189551,1.4051,-1.78174,0.006522,1.49422,-1.85823,0.008333,1.52111,-1.90227,0.010674,1.50684,-1.97524,0.19663,1.4494,-2.04021,0.065382,1.10112,-1 [...]
+/*1126*/{0.244791,1.93862,-1.72545,0.288682,1.87549,-1.88407,0.236462,1.78342,-1.60617,0.225845,1.66409,-1.52287,0.33065,1.69668,-1.64011,0.405949,1.7092,-1.69309,0.164748,1.94959,-2.09408,0.278617,1.87732,-1.95743,0.06012,1.90972,-1.97713,-0.074573,1.85126,-2.12755,-0.11026,1.76403,-2.1807,0.02314,1.62937,-2.14047,0.055426,1.576,-2.16343,0.19093,1.40757,-1.78177,0.007943,1.49781,-1.85808,0.009061,1.52497,-1.90175,0.009886,1.51086,-1.97339,0.196415,1.45271,-2.04042,0.063443,1.10582,-1.75 [...]
+/*1127*/{0.24738,1.94056,-1.72556,0.288093,1.87845,-1.88449,0.238124,1.78588,-1.60628,0.22867,1.66591,-1.52356,0.331618,1.69809,-1.64179,0.405766,1.71177,-1.6952,0.164914,1.95285,-2.09417,0.279153,1.87968,-1.95895,0.061748,1.91279,-1.97683,-0.074252,1.85499,-2.12412,-0.112068,1.76888,-2.17753,0.021323,1.63357,-2.13914,0.053846,1.57961,-2.16209,0.192187,1.4102,-1.78341,0.008995,1.50094,-1.8566,0.009943,1.52828,-1.90037,0.009975,1.51495,-1.97289,0.196571,1.45661,-2.0409,0.06195,1.10997,-1. [...]
+/*1128*/{0.24859,1.9418,-1.72615,0.289793,1.88163,-1.88539,0.238762,1.78766,-1.60641,0.230121,1.66664,-1.52233,0.331981,1.69977,-1.64333,0.406898,1.71447,-1.69709,0.165251,1.95539,-2.0938,0.279458,1.88241,-1.95934,0.062084,1.91529,-1.97671,-0.075016,1.85806,-2.1202,-0.112995,1.77276,-2.17418,0.020648,1.63668,-2.1372,0.052287,1.5829,-2.16087,0.192925,1.41301,-1.78366,0.009795,1.50424,-1.85646,0.010207,1.5315,-1.90035,0.010185,1.51854,-1.97232,0.196965,1.45967,-2.04099,0.061185,1.11389,-1. [...]
+/*1129*/{0.250809,1.94356,-1.7261,0.291228,1.88515,-1.88604,0.239962,1.78925,-1.60671,0.231916,1.66862,-1.52337,0.333341,1.70043,-1.64468,0.406086,1.71257,-1.6993,0.165717,1.95812,-2.09374,0.280812,1.88463,-1.95987,0.063925,1.91762,-1.97505,-0.073765,1.86226,-2.11692,-0.114061,1.77672,-2.17076,0.019616,1.64003,-2.13613,0.050902,1.58553,-2.16015,0.193247,1.41607,-1.78427,0.010662,1.50703,-1.85496,0.010721,1.53523,-1.89986,0.014164,1.52033,-1.97148,0.196198,1.46249,-2.04064,0.060526,1.1170 [...]
+/*1130*/{0.251753,1.94487,-1.7266,0.291344,1.88699,-1.88723,0.240905,1.79022,-1.6067,0.232559,1.66886,-1.52336,0.334213,1.7003,-1.64595,0.406023,1.71256,-1.70129,0.166254,1.96065,-2.09365,0.281345,1.88675,-1.96065,0.06428,1.92082,-1.97535,-0.074868,1.86503,-2.11318,-0.115705,1.77934,-2.16708,0.018326,1.64296,-2.13509,0.050134,1.58812,-2.15974,0.193831,1.41897,-1.78377,0.010749,1.51058,-1.8552,0.010921,1.53893,-1.89972,0.014028,1.52391,-1.97053,0.195586,1.46472,-2.0406,0.06198,1.12062,-1. [...]
+/*1131*/{0.253326,1.94624,-1.7267,0.292484,1.88931,-1.88753,0.240961,1.79151,-1.60702,0.235122,1.66934,-1.52389,0.334691,1.69928,-1.64704,0.407376,1.71125,-1.70303,0.167309,1.96311,-2.09404,0.281986,1.8885,-1.96101,0.065303,1.92284,-1.97474,-0.073776,1.86731,-2.10924,-0.116016,1.78219,-2.16364,0.017814,1.64506,-2.13339,0.048916,1.59039,-2.15853,0.194103,1.42194,-1.78405,0.011843,1.51284,-1.85476,0.011086,1.54162,-1.89986,0.013673,1.52534,-1.97049,0.196514,1.46747,-2.04065,0.062961,1.1212 [...]
+/*1132*/{0.254554,1.94661,-1.72679,0.292633,1.89151,-1.88816,0.241078,1.79239,-1.60727,0.235345,1.66913,-1.52417,0.335875,1.69888,-1.64857,0.407205,1.70952,-1.70453,0.16771,1.96493,-2.09438,0.2825,1.88994,-1.96119,0.067157,1.92487,-1.975,-0.074303,1.86871,-2.10623,-0.116065,1.78394,-2.1603,0.018138,1.64704,-2.13241,0.048204,1.59155,-2.15813,0.194226,1.42447,-1.78476,0.011785,1.51515,-1.85478,0.009847,1.54429,-1.89935,0.013172,1.52754,-1.96952,0.195078,1.46953,-2.04036,0.065972,1.12187,-1 [...]
+/*1133*/{0.255021,1.94673,-1.72682,0.292051,1.89337,-1.88855,0.241182,1.79259,-1.60762,0.235975,1.66889,-1.5253,0.336202,1.69685,-1.64987,0.407423,1.7073,-1.7056,0.170083,1.96658,-2.09438,0.283077,1.8914,-1.96168,0.068715,1.92646,-1.97469,-0.074692,1.87049,-2.1024,-0.116461,1.78563,-2.15749,0.018841,1.64824,-2.13093,0.047918,1.592,-2.15723,0.194361,1.42692,-1.78478,0.01174,1.51716,-1.8543,0.010175,1.54562,-1.90049,0.012539,1.52902,-1.96972,0.196228,1.47155,-2.04035,0.070576,1.12258,-1.74 [...]
+/*1134*/{0.256092,1.94789,-1.72692,0.292925,1.89382,-1.88929,0.241076,1.79253,-1.60845,0.234938,1.66707,-1.52626,0.336125,1.69494,-1.65108,0.408523,1.7037,-1.70761,0.170647,1.96774,-2.09445,0.283474,1.89233,-1.96189,0.069183,1.92928,-1.97469,-0.074271,1.87148,-2.09894,-0.116197,1.78648,-2.15465,0.018838,1.64905,-2.12983,0.047274,1.59247,-2.15647,0.194422,1.42906,-1.78468,0.011511,1.51842,-1.85537,0.009737,1.54798,-1.89983,0.012488,1.52945,-1.96909,0.195003,1.47277,-2.04016,0.075599,1.122 [...]
+/*1135*/{0.257199,1.9471,-1.727,0.29327,1.8953,-1.88983,0.240987,1.79223,-1.60878,0.233869,1.66576,-1.52789,0.337613,1.69208,-1.6521,0.40875,1.70014,-1.7087,0.170758,1.96882,-2.09466,0.283492,1.89261,-1.96216,0.071208,1.93058,-1.97515,-0.07287,1.87294,-2.09582,-0.116074,1.78691,-2.15165,0.018661,1.64914,-2.12864,0.047544,1.5923,-2.15636,0.194933,1.43054,-1.78487,0.011918,1.51916,-1.85482,0.01032,1.54909,-1.90043,0.012387,1.52943,-1.9692,0.195106,1.47392,-2.03998,0.081019,1.12154,-1.75218 [...]
+/*1136*/{0.257864,1.94642,-1.72707,0.293059,1.89527,-1.88988,0.239927,1.79079,-1.60939,0.234294,1.66452,-1.52932,0.337306,1.68887,-1.65351,0.409893,1.69567,-1.70951,0.171797,1.96925,-2.09431,0.283635,1.89289,-1.96185,0.072137,1.93126,-1.97568,-0.071578,1.87239,-2.09275,-0.115673,1.78667,-2.14916,0.020225,1.64891,-2.12759,0.047959,1.5914,-2.15594,0.195258,1.43162,-1.78507,0.011401,1.52005,-1.85545,0.009668,1.54998,-1.90135,0.010826,1.52965,-1.97037,0.194339,1.47447,-2.03999,0.085214,1.120 [...]
+/*1137*/{0.25795,1.94536,-1.7266,0.292641,1.8948,-1.88936,0.239342,1.79022,-1.61011,0.232295,1.6627,-1.53002,0.336836,1.6853,-1.6545,0.41014,1.69094,-1.71026,0.173313,1.96947,-2.09443,0.284096,1.89315,-1.96174,0.073196,1.93248,-1.9755,-0.071959,1.8725,-2.09018,-0.114535,1.78653,-2.14705,0.021284,1.6477,-2.12669,0.048074,1.59067,-2.15559,0.195043,1.43293,-1.78547,0.011372,1.51986,-1.8558,0.009624,1.54985,-1.90157,0.011624,1.5285,-1.97074,0.194457,1.47479,-2.03953,0.093999,1.11843,-1.75619 [...]
+/*1138*/{0.258192,1.94451,-1.72723,0.292704,1.89368,-1.88937,0.2381,1.7888,-1.61073,0.230049,1.66121,-1.53227,0.336532,1.68118,-1.65543,0.410422,1.68534,-1.71107,0.174466,1.96933,-2.09465,0.283879,1.89269,-1.96118,0.073604,1.93249,-1.97552,-0.07012,1.87183,-2.08678,-0.113238,1.78522,-2.14468,0.022322,1.64666,-2.12593,0.048339,1.58883,-2.15541,0.195102,1.43363,-1.78563,0.011088,1.51888,-1.8567,0.009606,1.54982,-1.902,0.011522,1.52769,-1.97094,0.193817,1.47502,-2.03906,0.100867,1.11703,-1. [...]
+/*1139*/{0.258541,1.94303,-1.72727,0.29305,1.89284,-1.88963,0.237188,1.78691,-1.61102,0.22879,1.65805,-1.53361,0.336334,1.67663,-1.65651,0.410153,1.67942,-1.71136,0.174682,1.96871,-2.09431,0.283824,1.89254,-1.96121,0.074081,1.93264,-1.97535,-0.069571,1.87037,-2.08444,-0.112777,1.78357,-2.14289,0.023369,1.64445,-2.12553,0.048843,1.58682,-2.15554,0.195267,1.43386,-1.7861,0.010428,1.51814,-1.85766,0.009413,1.54834,-1.90325,0.010586,1.52584,-1.97225,0.193862,1.47489,-2.03894,0.107863,1.11484 [...]
+/*1140*/{0.257739,1.9404,-1.72699,0.293046,1.89116,-1.88845,0.236407,1.7848,-1.61199,0.22773,1.65618,-1.53609,0.335202,1.67122,-1.65733,0.410229,1.67253,-1.71268,0.174578,1.96758,-2.0936,0.283707,1.89147,-1.96083,0.075673,1.93198,-1.9761,-0.066797,1.86963,-2.08261,-0.111221,1.78128,-2.14155,0.023706,1.64256,-2.12554,0.049881,1.58406,-2.15575,0.195399,1.43358,-1.78669,0.010743,1.51644,-1.85748,0.010599,1.54636,-1.9038,0.010987,1.52384,-1.97321,0.193752,1.47399,-2.0393,0.116037,1.11289,-1. [...]
+/*1141*/{0.257193,1.93855,-1.72668,0.292892,1.88973,-1.8881,0.235012,1.78228,-1.61199,0.226195,1.65263,-1.53764,0.334618,1.66621,-1.65774,0.41014,1.66529,-1.71232,0.17482,1.96601,-2.09303,0.283133,1.88975,-1.96084,0.07564,1.93106,-1.97627,-0.065318,1.86677,-2.08064,-0.109807,1.77908,-2.1404,0.02451,1.63967,-2.12506,0.051068,1.58072,-2.15622,0.195981,1.43297,-1.78707,0.010297,1.51488,-1.85915,0.01037,1.54537,-1.90449,0.010198,1.52196,-1.97461,0.193857,1.47283,-2.03922,0.122287,1.11029,-1. [...]
+/*1142*/{0.257017,1.93658,-1.72689,0.292829,1.88772,-1.88775,0.233621,1.77963,-1.61266,0.222572,1.64987,-1.54027,0.333303,1.6603,-1.65825,0.409377,1.65767,-1.71233,0.17498,1.96464,-2.09263,0.283536,1.88909,-1.96006,0.075806,1.92905,-1.97559,-0.065229,1.86455,-2.07928,-0.109163,1.77563,-2.13973,0.026068,1.6359,-2.12555,0.051565,1.57731,-2.15652,0.196436,1.43191,-1.78772,0.010892,1.51247,-1.86063,0.010203,1.54293,-1.90522,0.009884,1.51953,-1.97601,0.19307,1.47191,-2.03962,0.129672,1.10951, [...]
+/*1143*/{0.256718,1.93472,-1.72653,0.292136,1.88516,-1.88761,0.231838,1.77626,-1.61283,0.220734,1.64768,-1.54172,0.33199,1.65422,-1.65857,0.408261,1.64974,-1.71269,0.174567,1.96326,-2.09235,0.283071,1.88671,-1.95971,0.075088,1.92747,-1.97627,-0.063381,1.86143,-2.0788,-0.107729,1.77164,-2.13923,0.027441,1.63234,-2.12651,0.052632,1.57259,-2.15632,0.197274,1.43099,-1.78867,0.010931,1.51006,-1.86187,0.010038,1.54089,-1.90644,0.01063,1.51713,-1.97671,0.192364,1.4695,-2.04053,0.136609,1.10566, [...]
+/*1144*/{0.255832,1.93159,-1.72584,0.292478,1.88246,-1.8871,0.230702,1.77278,-1.61368,0.216355,1.64401,-1.54367,0.330148,1.64761,-1.65943,0.40741,1.64178,-1.71212,0.17339,1.96055,-2.09196,0.282756,1.88475,-1.95971,0.07443,1.92522,-1.97686,-0.062507,1.85747,-2.07878,-0.106813,1.76737,-2.13915,0.028967,1.62768,-2.1274,0.054392,1.56893,-2.15762,0.197443,1.42904,-1.78939,0.010738,1.50724,-1.8624,0.009932,1.53804,-1.90658,0.01007,1.51377,-1.97778,0.192283,1.46708,-2.04121,0.143221,1.10452,-1. [...]
+/*1145*/{0.255314,1.92869,-1.72485,0.291713,1.87931,-1.88647,0.228607,1.76925,-1.61348,0.213956,1.64019,-1.54489,0.327708,1.64169,-1.65983,0.406037,1.63279,-1.71183,0.173656,1.95815,-2.09201,0.282893,1.88246,-1.95934,0.073952,1.92256,-1.97774,-0.06203,1.85229,-2.07809,-0.105473,1.76185,-2.13939,0.031002,1.62235,-2.12782,0.056144,1.56393,-2.15837,0.197726,1.42748,-1.79023,0.010446,1.5049,-1.8626,0.009897,1.53566,-1.90659,0.011062,1.5108,-1.97747,0.192126,1.46491,-2.04204,0.149907,1.10091, [...]
+/*1146*/{0.253391,1.92489,-1.72447,0.29258,1.876,-1.88638,0.226676,1.76518,-1.6138,0.209862,1.63709,-1.54679,0.325657,1.63498,-1.65982,0.402755,1.62372,-1.71086,0.173118,1.9552,-2.09223,0.282651,1.87962,-1.95952,0.073022,1.91918,-1.97827,-0.062752,1.84738,-2.08015,-0.104726,1.75641,-2.14014,0.032665,1.61771,-2.1292,0.05792,1.5581,-2.15933,0.199793,1.42602,-1.79119,0.010418,1.50221,-1.86302,0.010008,1.53217,-1.9069,0.01046,1.50821,-1.97876,0.191966,1.46289,-2.04256,0.15492,1.09955,-1.7696 [...]
+/*1147*/{0.253092,1.92225,-1.72373,0.292075,1.87352,-1.88578,0.225056,1.76097,-1.61398,0.206402,1.63352,-1.54778,0.322096,1.62778,-1.65985,0.401088,1.61485,-1.71013,0.172744,1.95096,-2.0926,0.28208,1.8767,-1.95917,0.072631,1.91572,-1.9787,-0.061921,1.84137,-2.08102,-0.103796,1.74975,-2.14076,0.034634,1.61125,-2.13052,0.0603,1.55233,-2.16029,0.199976,1.42347,-1.79234,0.010337,1.49908,-1.86312,0.010623,1.52884,-1.9087,0.009681,1.5042,-1.97884,0.19204,1.46085,-2.04298,0.160002,1.09739,-1.76 [...]
+/*1148*/{0.252075,1.9181,-1.72318,0.292398,1.86948,-1.88548,0.222388,1.75644,-1.61375,0.200831,1.62937,-1.548,0.319385,1.62052,-1.66007,0.397789,1.60569,-1.70893,0.172958,1.9468,-2.0935,0.282434,1.87322,-1.95908,0.071814,1.91161,-1.97909,-0.062446,1.83423,-2.08235,-0.102337,1.7427,-2.14232,0.037244,1.60566,-2.13208,0.063021,1.5456,-2.16134,0.200428,1.42162,-1.79317,0.010561,1.49502,-1.86288,0.010797,1.5257,-1.90876,0.008787,1.50085,-1.97867,0.191677,1.45891,-2.04358,0.165364,1.09452,-1.7 [...]
+/*1149*/{0.251841,1.91468,-1.72257,0.292795,1.8661,-1.88523,0.220093,1.75167,-1.61415,0.197578,1.62464,-1.54781,0.31593,1.61337,-1.66018,0.39489,1.59653,-1.7081,0.172141,1.94285,-2.09413,0.282708,1.86982,-1.95905,0.071248,1.9071,-1.97931,-0.063129,1.82721,-2.08453,-0.101045,1.7352,-2.14393,0.03999,1.59866,-2.13372,0.065687,1.53878,-2.16275,0.201117,1.41935,-1.79425,0.010668,1.49116,-1.86321,0.010433,1.52166,-1.90914,0.008964,1.49735,-1.97842,0.190974,1.45616,-2.04406,0.173004,1.09101,-1. [...]
+/*1150*/{0.250714,1.9104,-1.72193,0.291886,1.86136,-1.8847,0.218021,1.74712,-1.61453,0.192472,1.62068,-1.54852,0.312038,1.60577,-1.65965,0.391137,1.58693,-1.70701,0.172292,1.93804,-2.09469,0.282899,1.86581,-1.95867,0.07019,1.90297,-1.98055,-0.063245,1.81802,-2.08596,-0.099276,1.72737,-2.14612,0.042708,1.59154,-2.13539,0.068321,1.53088,-2.16386,0.200977,1.41691,-1.79497,0.010936,1.48744,-1.86306,0.01042,1.51745,-1.90917,0.008824,1.49245,-1.97711,0.191211,1.45346,-2.04446,0.176508,1.09064, [...]
+/*1151*/{0.250558,1.90645,-1.72159,0.292915,1.85629,-1.8839,0.215452,1.74209,-1.61449,0.187322,1.6159,-1.54926,0.307668,1.59794,-1.65925,0.386957,1.57809,-1.70628,0.171679,1.93287,-2.0953,0.28295,1.86148,-1.95892,0.06934,1.89806,-1.98183,-0.063119,1.80967,-2.0883,-0.097517,1.71889,-2.1485,0.046107,1.58313,-2.13783,0.07187,1.52295,-2.16459,0.200978,1.41413,-1.7962,0.010691,1.48295,-1.86168,0.010478,1.51334,-1.90853,0.008171,1.4881,-1.97715,0.190751,1.45055,-2.04566,0.182072,1.087,-1.76358 [...]
+/*1152*/{0.249432,1.90161,-1.72066,0.292222,1.85271,-1.8836,0.212547,1.73772,-1.61428,0.182892,1.61163,-1.54965,0.303629,1.5908,-1.65873,0.381641,1.56914,-1.70478,0.172181,1.92748,-2.09627,0.282958,1.8573,-1.95856,0.068543,1.893,-1.9822,-0.063042,1.80115,-2.09052,-0.095075,1.71007,-2.15113,0.048697,1.57436,-2.13926,0.075872,1.51462,-2.16564,0.201251,1.41097,-1.79678,0.010751,1.47796,-1.86079,0.009538,1.50925,-1.9084,0.007956,1.4831,-1.97648,0.190747,1.44717,-2.04654,0.183971,1.08534,-1.7 [...]
+/*1153*/{0.248957,1.89687,-1.71988,0.293728,1.84834,-1.88359,0.210584,1.73247,-1.61489,0.177832,1.60809,-1.55033,0.29889,1.58368,-1.65831,0.376274,1.56062,-1.70323,0.17264,1.92231,-2.09716,0.283193,1.85226,-1.95833,0.06765,1.88756,-1.98302,-0.061426,1.79162,-2.09285,-0.092538,1.70058,-2.15393,0.052444,1.56538,-2.14158,0.079034,1.5055,-2.16664,0.201816,1.40763,-1.79782,0.010936,1.4728,-1.86035,0.009829,1.5046,-1.90818,0.007492,1.47844,-1.97516,0.191315,1.4446,-2.0473,0.18976,1.08158,-1.75 [...]
+/*1154*/{0.247879,1.89122,-1.71879,0.293066,1.84225,-1.88254,0.208089,1.7276,-1.61497,0.171355,1.60458,-1.55103,0.294127,1.57701,-1.65728,0.3714,1.55254,-1.70179,0.172919,1.9166,-2.09812,0.283712,1.84684,-1.95841,0.066942,1.88207,-1.98441,-0.061528,1.78212,-2.09646,-0.089651,1.69107,-2.1567,0.056198,1.55588,-2.14377,0.083841,1.49658,-2.16777,0.201749,1.404,-1.79873,0.010723,1.46701,-1.86011,0.009994,1.49943,-1.90826,0.006354,1.47344,-1.97456,0.191576,1.44116,-2.04818,0.192661,1.07964,-1. [...]
+/*1155*/{0.247477,1.88654,-1.71889,0.293915,1.83769,-1.88244,0.204391,1.72222,-1.61539,0.166263,1.59977,-1.55073,0.288112,1.57066,-1.65663,0.364918,1.54458,-1.69991,0.173412,1.91073,-2.0991,0.284253,1.84209,-1.9583,0.065765,1.87636,-1.98565,-0.061153,1.77127,-2.09927,-0.086034,1.68117,-2.15953,0.060387,1.54696,-2.14576,0.088897,1.48755,-2.16859,0.20227,1.40008,-1.79919,0.010381,1.46179,-1.85879,0.008932,1.49409,-1.90822,0.0062,1.46855,-1.97281,0.192284,1.4374,-2.04963,0.198225,1.07581,-1 [...]
+/*1156*/{0.247041,1.88105,-1.71805,0.293408,1.83148,-1.88174,0.201056,1.71762,-1.6153,0.160631,1.5967,-1.55116,0.28232,1.56467,-1.65527,0.358904,1.53742,-1.69814,0.174498,1.9048,-2.10042,0.285002,1.83686,-1.95736,0.065879,1.87105,-1.98648,-0.058848,1.76205,-2.1024,-0.082296,1.671,-2.16295,0.064867,1.5369,-2.14805,0.093772,1.47833,-2.16954,0.202003,1.39531,-1.7996,0.009802,1.4569,-1.85792,0.008196,1.48858,-1.90792,0.005456,1.46348,-1.97206,0.193216,1.43301,-2.05093,0.202026,1.07212,-1.749 [...]
+/*1157*/{0.246003,1.8761,-1.7176,0.293423,1.82661,-1.88125,0.19817,1.7133,-1.61509,0.152883,1.59337,-1.55208,0.275661,1.55943,-1.6534,0.352694,1.53069,-1.69587,0.17455,1.8993,-2.10144,0.286142,1.8314,-1.95761,0.066691,1.86553,-1.98855,-0.056474,1.75218,-2.10665,-0.077775,1.66045,-2.16681,0.069683,1.52874,-2.15066,0.099583,1.46941,-2.17062,0.202233,1.39039,-1.80006,0.008983,1.45149,-1.85779,0.008199,1.4825,-1.90706,0.00438,1.45913,-1.97159,0.191507,1.42756,-2.05207,0.206199,1.06939,-1.747 [...]
+/*1158*/{0.245768,1.87098,-1.71763,0.295102,1.8216,-1.88068,0.193659,1.70868,-1.61543,0.145901,1.59049,-1.55139,0.268909,1.5543,-1.65261,0.345885,1.5251,-1.69388,0.17621,1.89303,-2.10257,0.286548,1.82566,-1.95746,0.066697,1.86091,-1.98916,-0.055131,1.74201,-2.11177,-0.072591,1.65,-2.17017,0.075761,1.51744,-2.15176,0.106219,1.46039,-2.17154,0.202842,1.38516,-1.80033,0.008133,1.4467,-1.85668,0.007059,1.47589,-1.90667,0.003798,1.45448,-1.97141,0.190689,1.4223,-2.0532,0.209896,1.06641,-1.744 [...]
+/*1159*/{0.245342,1.86614,-1.71651,0.295802,1.81642,-1.8804,0.189788,1.705,-1.61547,0.138637,1.5889,-1.55058,0.262035,1.55002,-1.65053,0.338378,1.51925,-1.69143,0.178303,1.88746,-2.10426,0.287765,1.82086,-1.95678,0.068306,1.85674,-1.9912,-0.050969,1.732,-2.11726,-0.067004,1.63928,-2.17341,0.082561,1.50855,-2.15391,0.113308,1.45214,-2.17238,0.202081,1.37969,-1.80061,0.006793,1.44132,-1.85589,0.004918,1.47013,-1.90576,0.000487,1.4516,-1.97132,0.189407,1.4167,-2.05477,0.213759,1.06302,-1.74 [...]
+/*1160*/{0.244828,1.86204,-1.71539,0.296923,1.81138,-1.87938,0.185008,1.70207,-1.61482,0.130616,1.58673,-1.55018,0.254785,1.54574,-1.64816,0.330993,1.51359,-1.68881,0.179951,1.8818,-2.10627,0.288972,1.81576,-1.9567,0.067645,1.85392,-1.99196,-0.04592,1.72248,-2.12392,-0.060914,1.62835,-2.17726,0.09029,1.50016,-2.15542,0.121484,1.44401,-2.17305,0.201835,1.37479,-1.80219,0.00588,1.43607,-1.85584,0.002309,1.46554,-1.90489,-0.001851,1.44745,-1.97138,0.187786,1.41252,-2.05563,0.218225,1.0611,- [...]
+/*1161*/{0.244436,1.85805,-1.71486,0.297015,1.80573,-1.87857,0.179521,1.69909,-1.61467,0.122448,1.58506,-1.55021,0.247366,1.54255,-1.64681,0.323042,1.50917,-1.68545,0.181152,1.8764,-2.1075,0.289742,1.81102,-1.95541,0.068196,1.85093,-1.99217,-0.041153,1.71193,-2.13091,-0.053327,1.61699,-2.18084,0.097076,1.49367,-2.15701,0.131023,1.43718,-2.17374,0.200849,1.37006,-1.80364,0.003368,1.43165,-1.85587,0.001185,1.46087,-1.90389,-0.00491,1.44352,-1.97138,0.18491,1.40977,-2.05653,0.223939,1.05903 [...]
+/*1162*/{0.244023,1.85466,-1.71361,0.298509,1.80252,-1.87718,0.174194,1.69827,-1.61365,0.113251,1.58339,-1.54915,0.23855,1.53969,-1.64448,0.314531,1.50539,-1.68267,0.18314,1.87169,-2.10856,0.291059,1.80661,-1.95509,0.06846,1.84838,-1.99224,-0.036984,1.70294,-2.13683,-0.046187,1.60649,-2.18406,0.106442,1.48711,-2.15792,0.140267,1.43098,-2.17434,0.199771,1.36537,-1.80525,0.001306,1.42863,-1.8531,-9.7e-005,1.45899,-1.90442,-0.00778,1.44037,-1.97003,0.183157,1.40705,-2.05795,0.228205,1.05909 [...]
+/*1163*/{0.244378,1.85157,-1.71234,0.298929,1.79801,-1.87546,0.168991,1.69753,-1.61309,0.10535,1.58521,-1.54972,0.230868,1.53738,-1.64323,0.306868,1.50224,-1.68042,0.184281,1.86717,-2.11023,0.291592,1.80285,-1.95353,0.069787,1.84585,-1.9925,-0.033152,1.69451,-2.14219,-0.038243,1.59668,-2.18728,0.115622,1.48098,-2.15805,0.151666,1.42606,-2.17521,0.197898,1.36164,-1.80673,-0.001735,1.42529,-1.85008,-0.000671,1.45878,-1.90297,-0.009931,1.4376,-1.96843,0.181226,1.40611,-2.05925,0.231775,1.06 [...]
+/*1164*/{0.244617,1.84889,-1.71145,0.297662,1.79469,-1.87447,0.162869,1.6975,-1.61298,0.096057,1.58683,-1.5498,0.222791,1.53571,-1.64166,0.298104,1.49995,-1.67835,0.185506,1.86397,-2.11007,0.29156,1.79929,-1.95335,0.069981,1.84457,-1.99197,-0.02811,1.68496,-2.14638,-0.030007,1.58811,-2.18981,0.126909,1.47709,-2.15822,0.163376,1.42194,-2.17459,0.196175,1.35772,-1.80644,-0.002058,1.42279,-1.85331,-0.002488,1.4586,-1.90348,-0.011372,1.43565,-1.96644,0.179185,1.40558,-2.06105,0.237194,1.0646 [...]
+/*1165*/{0.245202,1.84683,-1.71062,0.299132,1.79243,-1.8738,0.156977,1.69901,-1.61219,0.088274,1.5893,-1.54997,0.213997,1.53572,-1.64046,0.288465,1.497,-1.67626,0.186962,1.86117,-2.10953,0.292414,1.79746,-1.95186,0.07031,1.84291,-1.99123,-0.024306,1.67733,-2.15048,-0.021914,1.58085,-2.19193,0.137841,1.47397,-2.15828,0.175719,1.41941,-2.17422,0.194981,1.35445,-1.80662,-0.002824,1.42117,-1.85188,-0.004268,1.4583,-1.9035,-0.012303,1.43514,-1.96468,0.177603,1.40525,-2.06244,0.244715,1.06824, [...]
+/*1166*/{0.245418,1.84536,-1.71007,0.298457,1.7913,-1.8732,0.151471,1.70067,-1.61231,0.079644,1.5933,-1.55127,0.205886,1.53546,-1.63998,0.28039,1.49577,-1.67352,0.188551,1.85972,-2.10898,0.29186,1.79557,-1.95164,0.070529,1.84215,-1.99083,-0.017662,1.67088,-2.15257,-0.013443,1.57487,-2.19404,0.148845,1.4721,-2.15786,0.188363,1.41897,-2.17322,0.193929,1.35198,-1.80763,-0.004081,1.42108,-1.85098,-0.003678,1.45686,-1.90316,-0.012942,1.4353,-1.9627,0.176034,1.40457,-2.06377,0.250606,1.07264,- [...]
+/*1167*/{0.245614,1.84444,-1.70941,0.297986,1.78958,-1.87247,0.146053,1.70287,-1.61237,0.071184,1.59749,-1.55243,0.197554,1.5368,-1.63884,0.27034,1.49483,-1.67181,0.189976,1.85885,-2.10822,0.292261,1.79432,-1.95071,0.070685,1.84099,-1.9913,-0.012608,1.66832,-2.15664,-0.004754,1.57046,-2.19588,0.159461,1.47165,-2.15679,0.201717,1.42015,-2.17175,0.193199,1.35047,-1.80626,-0.004564,1.42115,-1.8501,-0.005194,1.45651,-1.90327,-0.01343,1.43558,-1.96109,0.175198,1.40383,-2.06463,0.258704,1.0794 [...]
+/*1168*/{0.244417,1.84414,-1.70911,0.298387,1.78911,-1.87146,0.141137,1.70568,-1.61227,0.062538,1.60315,-1.5549,0.1892,1.53799,-1.63822,0.261733,1.49537,-1.66944,0.192255,1.85873,-2.10744,0.291888,1.7933,-1.94981,0.07193,1.84073,-1.99176,-0.007039,1.66444,-2.15797,0.003871,1.56654,-2.19736,0.171214,1.4731,-2.15566,0.213981,1.42268,-2.16985,0.191591,1.34994,-1.8045,-0.004427,1.42184,-1.85017,-0.005428,1.45616,-1.90314,-0.012696,1.43592,-1.96114,0.174913,1.40354,-2.0652,0.26596,1.08455,-1. [...]
+/*1169*/{0.243322,1.84439,-1.70806,0.297279,1.78891,-1.87078,0.13613,1.70924,-1.61231,0.054868,1.60844,-1.55554,0.181047,1.54004,-1.63741,0.253367,1.4962,-1.66798,0.193712,1.85906,-2.10716,0.292193,1.79311,-1.94885,0.07239,1.8406,-1.99251,-0.00158,1.66099,-2.15935,0.012868,1.56382,-2.19825,0.181911,1.47565,-2.15414,0.22648,1.42736,-2.16752,0.190307,1.35091,-1.80257,-0.005047,1.423,-1.85048,-0.005492,1.45627,-1.90297,-0.013131,1.4373,-1.96144,0.176466,1.40294,-2.06526,0.271627,1.09166,-1. [...]
+/*1170*/{0.241629,1.8459,-1.70805,0.296878,1.78873,-1.86943,0.132242,1.71235,-1.61308,0.047176,1.61495,-1.55779,0.17369,1.54304,-1.63605,0.244177,1.49732,-1.66571,0.195124,1.86023,-2.10648,0.292205,1.79329,-1.9476,0.072639,1.84053,-1.99336,0.003909,1.65927,-2.16068,0.021305,1.5624,-2.19932,0.192181,1.47954,-2.15217,0.238603,1.43276,-2.16453,0.188687,1.35261,-1.80044,-0.00506,1.42415,-1.85076,-0.006168,1.45736,-1.9034,-0.012825,1.43747,-1.96226,0.17591,1.40278,-2.06559,0.280788,1.09669,-1 [...]
+/*1171*/{0.239898,1.84861,-1.70773,0.296834,1.78976,-1.86835,0.126347,1.71638,-1.61354,0.038909,1.62078,-1.55978,0.165719,1.54657,-1.63527,0.236532,1.49893,-1.66408,0.196605,1.8625,-2.10654,0.292941,1.79436,-1.94692,0.073839,1.84101,-1.99449,0.010141,1.65762,-2.16248,0.029229,1.56148,-2.19991,0.202394,1.48465,-2.14964,0.251074,1.44025,-2.16168,0.188349,1.35556,-1.79902,-0.004894,1.42563,-1.85148,-0.00604,1.45897,-1.90392,-0.013098,1.43904,-1.96318,0.17644,1.40248,-2.06568,0.28757,1.10407 [...]
+/*1172*/{0.238156,1.85069,-1.70713,0.296634,1.79102,-1.8672,0.122149,1.72058,-1.61404,0.032685,1.62664,-1.55973,0.158459,1.55076,-1.63505,0.227878,1.50211,-1.66173,0.197515,1.86524,-2.106,0.293777,1.79607,-1.94534,0.074378,1.84173,-1.99503,0.01652,1.65633,-2.16322,0.037174,1.56135,-2.20037,0.213164,1.49093,-2.14724,0.262579,1.44878,-2.15819,0.18689,1.35759,-1.79651,-0.005274,1.42645,-1.85229,-0.005696,1.45924,-1.90468,-0.011885,1.43995,-1.96416,0.177745,1.40264,-2.06539,0.294133,1.11113, [...]
+/*1173*/{0.23555,1.85331,-1.70623,0.295749,1.79377,-1.86519,0.116835,1.7252,-1.61422,0.023915,1.63318,-1.56242,0.15047,1.5552,-1.63436,0.220639,1.50511,-1.66015,0.199229,1.86831,-2.10564,0.294082,1.79796,-1.94372,0.075477,1.84249,-1.99678,0.021041,1.65669,-2.16346,0.045145,1.56227,-2.201,0.221894,1.49722,-2.14445,0.273927,1.4583,-2.15461,0.186605,1.35974,-1.79549,-0.004649,1.42881,-1.85221,-0.004516,1.46097,-1.90572,-0.011354,1.44104,-1.96481,0.176927,1.40189,-2.06558,0.298334,1.11951,-1 [...]
+/*1174*/{0.233292,1.85649,-1.7053,0.293619,1.79548,-1.8627,0.112482,1.73015,-1.61441,0.018047,1.63952,-1.56229,0.143653,1.55981,-1.63281,0.212449,1.50887,-1.65777,0.200973,1.87253,-2.10505,0.294336,1.80083,-1.94165,0.075955,1.84465,-1.99781,0.024935,1.65727,-2.1635,0.052222,1.56324,-2.20119,0.231008,1.50552,-2.14176,0.285219,1.46868,-2.15102,0.18656,1.36261,-1.79445,-0.004528,1.43013,-1.85309,-0.004033,1.46265,-1.90596,-0.010606,1.44266,-1.96595,0.178849,1.40236,-2.06528,0.30532,1.12679, [...]
+/*1175*/{0.231152,1.85962,-1.70456,0.293786,1.79985,-1.86195,0.107519,1.73568,-1.61444,0.010742,1.64681,-1.56407,0.136503,1.56625,-1.6324,0.205429,1.5132,-1.65563,0.202917,1.87638,-2.104,0.294522,1.80389,-1.94031,0.076616,1.84542,-1.99895,0.029703,1.65839,-2.16325,0.060035,1.565,-2.20132,0.239877,1.51468,-2.13931,0.295219,1.47957,-2.14756,0.18716,1.36587,-1.7935,-0.004151,1.43244,-1.85511,-0.002852,1.4645,-1.90737,-0.009669,1.44443,-1.96761,0.180957,1.40297,-2.06401,0.310078,1.13404,-1.7 [...]
+/*1176*/{0.228006,1.86315,-1.70374,0.293202,1.80252,-1.85974,0.102584,1.74114,-1.61497,0.004084,1.65358,-1.56522,0.129471,1.57088,-1.63078,0.197036,1.51785,-1.65416,0.205029,1.88109,-2.10346,0.295431,1.80719,-1.93862,0.07809,1.84845,-2.00026,0.035688,1.66029,-2.16297,0.067685,1.56751,-2.20214,0.247367,1.52306,-2.13651,0.304822,1.49114,-2.14377,0.188103,1.36983,-1.79411,-0.003811,1.43462,-1.856,-0.001653,1.46714,-1.90839,-0.006638,1.44672,-1.96818,0.182229,1.40337,-2.06363,0.315004,1.1421 [...]
+/*1177*/{0.224783,1.86717,-1.70348,0.292941,1.80614,-1.85834,0.098021,1.74685,-1.61503,-0.002387,1.66102,-1.56621,0.122666,1.57684,-1.63036,0.190701,1.52236,-1.65176,0.206619,1.8853,-2.10243,0.295739,1.81071,-1.93696,0.078988,1.84985,-2.00106,0.039916,1.66149,-2.16242,0.074941,1.5707,-2.20265,0.255176,1.53293,-2.1337,0.313262,1.50337,-2.13995,0.189469,1.37424,-1.7935,-0.001981,1.43743,-1.85709,-0.000587,1.46946,-1.91002,-0.00577,1.44957,-1.96982,0.182934,1.40394,-2.06331,0.317098,1.15034 [...]
+/*1178*/{0.222206,1.87106,-1.70272,0.293469,1.81069,-1.85653,0.092697,1.75235,-1.61559,-0.0097,1.66743,-1.56829,0.115746,1.58261,-1.6304,0.184083,1.52807,-1.6504,0.20853,1.88994,-2.10156,0.296111,1.81451,-1.93548,0.07988,1.85269,-2.00242,0.045039,1.66452,-2.1619,0.082668,1.57413,-2.20336,0.262148,1.54224,-2.13039,0.321862,1.51544,-2.13562,0.189845,1.37785,-1.79378,-0.001393,1.44073,-1.85934,0.001107,1.47227,-1.91095,-0.003568,1.45242,-1.97115,0.185202,1.40463,-2.06307,0.322832,1.1575,-1. [...]
+/*1179*/{0.218882,1.87546,-1.70251,0.292355,1.81383,-1.85503,0.088117,1.75844,-1.61607,-0.016079,1.67528,-1.5698,0.109847,1.58919,-1.62987,0.176722,1.5333,-1.64851,0.210816,1.89467,-2.10025,0.296832,1.81835,-1.93313,0.081733,1.85616,-2.00387,0.049319,1.66736,-2.1619,0.089462,1.57744,-2.20409,0.269655,1.5536,-2.1273,0.330282,1.52805,-2.13154,0.191257,1.38251,-1.7943,-5e-005,1.44397,-1.85959,0.00292,1.47485,-1.91237,-0.000812,1.455,-1.97235,0.186994,1.40504,-2.06312,0.324592,1.16532,-1.735 [...]
+/*1180*/{0.215422,1.87951,-1.70194,0.292688,1.81856,-1.85324,0.083445,1.76471,-1.61623,-0.022578,1.68293,-1.57146,0.104124,1.59542,-1.62977,0.170293,1.53898,-1.6472,0.213288,1.89956,-2.09883,0.297173,1.82199,-1.9317,0.082662,1.85941,-2.00504,0.054775,1.66997,-2.16261,0.096948,1.58151,-2.20488,0.276234,1.56387,-2.12405,0.337199,1.54083,-2.12759,0.194067,1.38681,-1.79414,0.001978,1.44791,-1.86096,0.004715,1.47792,-1.91331,0.001259,1.458,-1.97287,0.189089,1.40533,-2.06277,0.32893,1.17216,-1 [...]
+/*1181*/{0.213181,1.88394,-1.70166,0.291638,1.82244,-1.85093,0.079584,1.7709,-1.61694,-0.028092,1.68974,-1.57391,0.097648,1.60216,-1.6296,0.163981,1.54519,-1.64601,0.21546,1.90409,-2.09759,0.298115,1.82657,-1.9299,0.084245,1.86232,-2.00552,0.059698,1.67379,-2.16319,0.10341,1.5858,-2.20547,0.282396,1.57415,-2.12087,0.343623,1.55361,-2.12375,0.195428,1.39132,-1.79555,0.003273,1.45239,-1.86154,0.007365,1.48073,-1.915,0.004186,1.46144,-1.97381,0.19084,1.40584,-2.06262,0.331491,1.17876,-1.735 [...]
+/*1182*/{0.21061,1.88826,-1.70151,0.29201,1.82666,-1.84952,0.075226,1.77716,-1.61773,-0.034402,1.69713,-1.57536,0.09206,1.60828,-1.62979,0.157772,1.5506,-1.64502,0.21825,1.90894,-2.09641,0.298809,1.83084,-1.92799,0.085786,1.8657,-2.00708,0.064565,1.67738,-2.16451,0.109993,1.59048,-2.20737,0.287821,1.58502,-2.11757,0.349556,1.56647,-2.11983,0.197829,1.39543,-1.79619,0.005228,1.45625,-1.86267,0.00859,1.48459,-1.91669,0.00706,1.46503,-1.97456,0.19314,1.40605,-2.06245,0.335055,1.18645,-1.735 [...]
+/*1183*/{0.208154,1.89273,-1.7015,0.292073,1.83114,-1.8479,0.070965,1.78349,-1.61873,-0.039136,1.70457,-1.57712,0.08687,1.61491,-1.6299,0.15321,1.55681,-1.6443,0.221306,1.91419,-2.09535,0.299742,1.83482,-1.92643,0.0873,1.86897,-2.00806,0.070839,1.68179,-2.16596,0.11691,1.59512,-2.20789,0.291803,1.59475,-2.1146,0.354625,1.57916,-2.11619,0.199845,1.39988,-1.79734,0.007831,1.46088,-1.8627,0.011264,1.48863,-1.91729,0.010235,1.46835,-1.97535,0.195808,1.40593,-2.06222,0.337888,1.19345,-1.73584 [...]
+/*1184*/{0.205409,1.89725,-1.70154,0.291787,1.83522,-1.84629,0.06738,1.78949,-1.6199,-0.044744,1.71172,-1.57925,0.082229,1.62209,-1.63036,0.147502,1.56281,-1.64405,0.224328,1.919,-2.09374,0.30048,1.83902,-1.92406,0.089819,1.87292,-2.00906,0.076253,1.68656,-2.16887,0.123531,1.60023,-2.20921,0.296198,1.60429,-2.11126,0.359268,1.59163,-2.11301,0.201955,1.40351,-1.79822,0.009108,1.46602,-1.86319,0.013537,1.49158,-1.91869,0.012689,1.47218,-1.97589,0.197652,1.40642,-2.06143,0.340553,1.19945,-1 [...]
+/*1185*/{0.203935,1.90112,-1.70165,0.291259,1.84033,-1.84524,0.064022,1.79615,-1.62073,-0.048594,1.71939,-1.58192,0.076809,1.6284,-1.63046,0.14233,1.56892,-1.64323,0.22707,1.92384,-2.09243,0.301017,1.84331,-1.92268,0.091454,1.87697,-2.01019,0.081046,1.69121,-2.17092,0.130026,1.6049,-2.2108,0.300928,1.61548,-2.10967,0.363568,1.60341,-2.1103,0.203793,1.40791,-1.79846,0.012272,1.4714,-1.86411,0.016235,1.49579,-1.91969,0.015576,1.47645,-1.97665,0.198733,1.40649,-2.06105,0.342254,1.20619,-1.7 [...]
+/*1186*/{0.202211,1.90503,-1.70145,0.29259,1.84366,-1.84299,0.060144,1.80253,-1.62229,-0.053976,1.72682,-1.58342,0.071923,1.63512,-1.63161,0.13671,1.57489,-1.6427,0.228804,1.92782,-2.09117,0.301733,1.84672,-1.9208,0.093472,1.8802,-2.01144,0.087258,1.69532,-2.17287,0.136175,1.60992,-2.21244,0.303224,1.62411,-2.10699,0.367208,1.61446,-2.10772,0.205714,1.41165,-1.79964,0.013969,1.47646,-1.86356,0.019,1.50016,-1.9207,0.018736,1.48,-1.97633,0.200754,1.40705,-2.0607,0.344167,1.21171,-1.73681,0 [...]
+/*1187*/{0.200601,1.90902,-1.70149,0.291125,1.84747,-1.84152,0.057046,1.8088,-1.62357,-0.05802,1.7344,-1.5864,0.067218,1.64118,-1.63211,0.132783,1.58006,-1.6424,0.231563,1.93184,-2.08992,0.303641,1.85038,-1.91864,0.09541,1.8836,-2.01206,0.091304,1.70072,-2.17626,0.142384,1.61463,-2.21344,0.306101,1.63421,-2.10518,0.369874,1.62588,-2.10427,0.206832,1.41561,-1.80034,0.017087,1.48151,-1.8631,0.021748,1.50411,-1.92126,0.021577,1.48419,-1.97709,0.202486,1.40761,-2.06069,0.346257,1.21735,-1.73 [...]
+/*1188*/{0.199052,1.91274,-1.70148,0.292846,1.85136,-1.84013,0.054065,1.8148,-1.62515,-0.061654,1.74174,-1.58931,0.063896,1.64734,-1.63273,0.127922,1.58626,-1.64243,0.234046,1.93584,-2.08816,0.304469,1.85389,-1.91667,0.097315,1.88583,-2.01257,0.096293,1.70619,-2.17979,0.147964,1.61899,-2.21515,0.309715,1.6439,-2.10306,0.372436,1.63654,-2.10178,0.208758,1.41976,-1.80087,0.01904,1.4866,-1.86291,0.024248,1.50838,-1.92222,0.024193,1.48791,-1.97691,0.204972,1.40854,-2.06022,0.348301,1.22199,- [...]
+/*1189*/{0.198195,1.91618,-1.70154,0.291965,1.85556,-1.83941,0.051613,1.8205,-1.62668,-0.065011,1.74875,-1.59204,0.060103,1.65334,-1.63446,0.124332,1.59149,-1.64216,0.235908,1.93921,-2.08686,0.305208,1.85715,-1.91521,0.099272,1.88841,-2.0133,0.101726,1.70953,-2.18215,0.153679,1.62368,-2.21718,0.310958,1.65119,-2.10169,0.375066,1.64675,-2.09981,0.210954,1.42262,-1.80102,0.022257,1.49112,-1.86256,0.026337,1.51304,-1.92311,0.026669,1.49089,-1.97633,0.206866,1.40907,-2.05959,0.350062,1.22569 [...]
+/*1190*/{0.197294,1.9193,-1.70113,0.293179,1.8592,-1.83772,0.048831,1.82548,-1.62868,-0.069272,1.75547,-1.59531,0.055705,1.6586,-1.63517,0.119454,1.59686,-1.64153,0.238878,1.94253,-2.08573,0.305832,1.86041,-1.91382,0.10089,1.89119,-2.01341,0.106904,1.71301,-2.18502,0.159268,1.62748,-2.21876,0.312796,1.65848,-2.10052,0.377066,1.65501,-2.09503,0.212396,1.4265,-1.80124,0.024527,1.49687,-1.8612,0.028653,1.51705,-1.92352,0.029824,1.49513,-1.97632,0.207968,1.41005,-2.05892,0.351407,1.22923,-1. [...]
+/*1191*/{0.196688,1.92224,-1.70136,0.29263,1.86083,-1.83606,0.046805,1.83119,-1.63035,-0.071983,1.7619,-1.59823,0.052252,1.6636,-1.63617,0.116092,1.60154,-1.64232,0.240794,1.9455,-2.08487,0.306996,1.8638,-1.91217,0.103835,1.89356,-2.01423,0.113021,1.71646,-2.18697,0.163714,1.63102,-2.22043,0.314155,1.66554,-2.09927,0.378823,1.66289,-2.09315,0.212223,1.4304,-1.80173,0.026441,1.50039,-1.8614,0.030661,1.52072,-1.92409,0.032381,1.49855,-1.97625,0.208486,1.41111,-2.05835,0.3534,1.23186,-1.739 [...]
+/*1192*/{0.196967,1.92501,-1.70134,0.293897,1.86416,-1.83426,0.045572,1.83667,-1.63193,-0.075909,1.76818,-1.60174,0.048775,1.66867,-1.63669,0.113125,1.60615,-1.64191,0.2427,1.94804,-2.08406,0.308447,1.86626,-1.91037,0.105689,1.89513,-2.015,0.116838,1.71941,-2.18908,0.168063,1.63412,-2.22227,0.316122,1.6713,-2.097,0.380153,1.67019,-2.09107,0.213109,1.43313,-1.80192,0.028651,1.50411,-1.86102,0.031987,1.52358,-1.92458,0.033774,1.50106,-1.97605,0.210961,1.41202,-2.05747,0.353346,1.23461,-1.7 [...]
+/*1193*/{0.196461,1.92743,-1.70146,0.294474,1.86782,-1.83384,0.043354,1.84072,-1.63361,-0.077556,1.77394,-1.60485,0.046015,1.67336,-1.63779,0.109774,1.61053,-1.64163,0.245118,1.94991,-2.08247,0.309043,1.86872,-1.90945,0.107167,1.89755,-2.01553,0.122117,1.72263,-2.19169,0.171977,1.63739,-2.22412,0.318673,1.67774,-2.09706,0.381301,1.67723,-2.08997,0.213914,1.43614,-1.80174,0.030823,1.50831,-1.86005,0.034606,1.52732,-1.92477,0.036014,1.50502,-1.97601,0.211593,1.41317,-2.05658,0.353555,1.234 [...]
+/*1194*/{0.196364,1.9297,-1.70171,0.295609,1.86874,-1.83266,0.042337,1.84513,-1.63594,-0.081616,1.77777,-1.60757,0.044277,1.6784,-1.63873,0.107006,1.61435,-1.64097,0.2479,1.95276,-2.08207,0.310837,1.87112,-1.90816,0.109824,1.89879,-2.0156,0.126177,1.7257,-2.19329,0.176124,1.64013,-2.22587,0.319758,1.68348,-2.09782,0.382735,1.68291,-2.08866,0.214718,1.43919,-1.80177,0.032585,1.51248,-1.85876,0.035551,1.53049,-1.92465,0.037202,1.50775,-1.97479,0.212378,1.4152,-2.05592,0.352985,1.23589,-1.7 [...]
+/*1195*/{0.196442,1.93164,-1.70167,0.296315,1.87026,-1.83191,0.041263,1.84953,-1.63738,-0.082394,1.78308,-1.6104,0.041636,1.68221,-1.64006,0.104149,1.6182,-1.64108,0.249183,1.95424,-2.08133,0.311433,1.873,-1.90734,0.111953,1.90061,-2.01542,0.13053,1.72789,-2.19529,0.179579,1.64244,-2.22779,0.320346,1.687,-2.0977,0.3832,1.68707,-2.08654,0.216404,1.4416,-1.80204,0.033335,1.51498,-1.85907,0.036348,1.53361,-1.92402,0.038913,1.51061,-1.97414,0.213112,1.41693,-2.05574,0.353226,1.23626,-1.74291 [...]
+/*1196*/{0.19746,1.93341,-1.70156,0.298488,1.87204,-1.83054,0.040827,1.85298,-1.63934,-0.086286,1.78762,-1.61405,0.039155,1.68552,-1.64091,0.10245,1.62092,-1.64076,0.251625,1.95621,-2.08007,0.313087,1.87456,-1.90625,0.113681,1.9022,-2.01569,0.134451,1.72947,-2.19688,0.182781,1.64458,-2.22917,0.321261,1.69002,-2.09738,0.383685,1.69073,-2.0845,0.216224,1.44368,-1.8019,0.034588,1.51785,-1.85709,0.037742,1.53656,-1.92341,0.039313,1.51386,-1.97371,0.213759,1.41866,-2.05496,0.349389,1.23544,-1 [...]
+/*1197*/{0.198075,1.93529,-1.70145,0.299116,1.87338,-1.83017,0.039942,1.85625,-1.64089,-0.08673,1.7914,-1.61663,0.03794,1.68925,-1.64189,0.100973,1.62346,-1.64062,0.25429,1.95788,-2.07932,0.314349,1.87632,-1.90539,0.115811,1.90381,-2.01573,0.138298,1.7316,-2.19745,0.185354,1.64597,-2.23058,0.321775,1.69201,-2.09686,0.383579,1.69403,-2.08208,0.216701,1.44653,-1.80215,0.034737,1.5208,-1.85797,0.037539,1.53949,-1.92316,0.041206,1.51608,-1.97267,0.214925,1.42062,-2.05446,0.347177,1.23309,-1. [...]
+/*1198*/{0.198468,1.93668,-1.70127,0.292796,1.87806,-1.83127,0.039756,1.85918,-1.64238,-0.088362,1.79512,-1.61942,0.036508,1.69157,-1.64297,0.098629,1.62637,-1.64034,0.255476,1.95901,-2.07881,0.315144,1.87834,-1.90492,0.117208,1.90492,-2.01586,0.141402,1.73317,-2.19868,0.187763,1.64743,-2.23174,0.323416,1.69561,-2.09755,0.384942,1.69592,-2.08161,0.217446,1.44802,-1.80235,0.036035,1.52266,-1.85662,0.039266,1.54126,-1.92293,0.041254,1.51838,-1.97291,0.215136,1.42283,-2.05389,0.344756,1.231 [...]
+/*1199*/{0.199883,1.93811,-1.70096,0.301105,1.87676,-1.82846,0.039888,1.86163,-1.64428,-0.089292,1.79773,-1.62253,0.035753,1.69403,-1.64332,0.098277,1.62809,-1.64004,0.257259,1.95999,-2.07842,0.316224,1.87885,-1.9038,0.118976,1.90645,-2.01567,0.143454,1.73509,-2.19945,0.189934,1.64856,-2.23265,0.323957,1.69516,-2.09697,0.386561,1.69722,-2.08164,0.217719,1.44982,-1.80218,0.035705,1.5245,-1.85664,0.039161,1.54396,-1.9226,0.042342,1.51993,-1.97159,0.216252,1.42499,-2.05321,0.342818,1.22917, [...]
+/*1200*/{0.201048,1.93871,-1.70115,0.301306,1.87731,-1.82832,0.03978,1.86348,-1.6454,-0.090178,1.80012,-1.62553,0.034242,1.69636,-1.64454,0.097179,1.6297,-1.63944,0.259257,1.96063,-2.07752,0.318168,1.87868,-1.90271,0.1202,1.90861,-2.0156,0.145235,1.73614,-2.19927,0.191615,1.64899,-2.23356,0.325701,1.69514,-2.0968,0.387209,1.69706,-2.08079,0.217396,1.45224,-1.80199,0.035774,1.52595,-1.85629,0.039904,1.54502,-1.92153,0.043426,1.52167,-1.97063,0.217778,1.42714,-2.05285,0.340188,1.22496,-1.7 [...]
+/*1201*/{0.202787,1.94008,-1.70022,0.302097,1.87724,-1.828,0.04095,1.86488,-1.6465,-0.090671,1.80198,-1.6278,0.034328,1.69793,-1.64525,0.096084,1.63029,-1.63901,0.26153,1.96068,-2.07711,0.31862,1.87903,-1.90253,0.121981,1.9102,-2.01553,0.147005,1.73717,-2.19894,0.19233,1.64933,-2.23376,0.326117,1.69505,-2.09554,0.388668,1.69657,-2.08105,0.217929,1.45338,-1.80248,0.035914,1.52627,-1.85654,0.039542,1.54698,-1.92202,0.042937,1.52338,-1.97063,0.218204,1.42909,-2.05222,0.336192,1.22053,-1.744 [...]
+/*1202*/{0.203951,1.94041,-1.70038,0.303123,1.87714,-1.8273,0.041629,1.86525,-1.64821,-0.090534,1.8039,-1.63051,0.034075,1.69877,-1.64591,0.096119,1.63111,-1.63823,0.263678,1.96109,-2.07713,0.31915,1.87918,-1.90162,0.123015,1.91059,-2.01526,0.147875,1.73808,-2.19868,0.192825,1.64924,-2.23386,0.326974,1.69509,-2.09533,0.390419,1.69572,-2.08253,0.217979,1.45502,-1.8019,0.035901,1.52649,-1.85614,0.038993,1.54843,-1.92175,0.043132,1.52383,-1.96999,0.218877,1.43073,-2.05204,0.332883,1.21602,- [...]
+/*1203*/{0.205331,1.94062,-1.69989,0.303311,1.87712,-1.82727,0.041971,1.8668,-1.64948,-0.089939,1.80406,-1.63207,0.034807,1.69878,-1.6467,0.096238,1.6307,-1.63774,0.265496,1.96065,-2.07663,0.319771,1.879,-1.90101,0.123622,1.91266,-2.016,0.148087,1.73889,-2.19799,0.192762,1.6489,-2.23359,0.327695,1.6921,-2.09477,0.390443,1.69268,-2.08184,0.218148,1.45514,-1.80183,0.036408,1.52676,-1.85543,0.039409,1.54868,-1.92121,0.043058,1.52469,-1.96903,0.219534,1.43178,-2.05178,0.32853,1.2116,-1.74483 [...]
+/*1204*/{0.206906,1.94069,-1.69966,0.304741,1.87699,-1.82692,0.043225,1.86691,-1.65003,-0.089174,1.80441,-1.63431,0.034817,1.69856,-1.64678,0.096689,1.63001,-1.63673,0.265959,1.95969,-2.07547,0.32027,1.87895,-1.90087,0.124663,1.91383,-2.01577,0.147161,1.73904,-2.19696,0.192337,1.64882,-2.23311,0.32949,1.69023,-2.09317,0.391458,1.68853,-2.08164,0.217848,1.45673,-1.80186,0.036166,1.52658,-1.85504,0.03978,1.54851,-1.92067,0.043556,1.52492,-1.96859,0.220287,1.43316,-2.05129,0.325186,1.20536, [...]
+/*1205*/{0.208243,1.94012,-1.6993,0.305067,1.87496,-1.82654,0.044037,1.86662,-1.65085,-0.088589,1.80307,-1.63596,0.035678,1.69803,-1.64715,0.096921,1.62866,-1.63572,0.267333,1.95852,-2.07483,0.32079,1.87841,-1.90072,0.125382,1.91423,-2.01558,0.146456,1.73865,-2.19594,0.1912,1.6482,-2.23217,0.328804,1.68664,-2.09316,0.391515,1.68395,-2.08051,0.218656,1.45742,-1.80196,0.036023,1.52566,-1.8548,0.039603,1.54819,-1.92012,0.043546,1.52433,-1.96883,0.22048,1.43364,-2.05121,0.322416,1.19977,-1.7 [...]
+/*1206*/{0.209876,1.93934,-1.69864,0.305066,1.87391,-1.82681,0.045157,1.86491,-1.6509,-0.087582,1.80217,-1.63747,0.035986,1.69665,-1.64711,0.09789,1.62709,-1.63475,0.267248,1.95667,-2.07441,0.320917,1.87757,-1.90072,0.125652,1.9144,-2.01555,0.145135,1.73869,-2.19464,0.189603,1.64727,-2.23073,0.330883,1.68415,-2.09384,0.390826,1.67902,-2.08006,0.218688,1.45786,-1.80178,0.036703,1.52399,-1.85407,0.039788,1.54666,-1.91981,0.044145,1.52301,-1.96862,0.221722,1.43388,-2.05097,0.319078,1.19484, [...]
+/*1207*/{0.211268,1.93831,-1.69792,0.305223,1.87292,-1.82699,0.046311,1.86362,-1.65134,-0.084496,1.80049,-1.63826,0.037658,1.69423,-1.64658,0.099411,1.6246,-1.63373,0.2688,1.95536,-2.07414,0.320469,1.87694,-1.90047,0.126619,1.91441,-2.01518,0.143266,1.73836,-2.19254,0.188019,1.64671,-2.22942,0.330854,1.67944,-2.0946,0.391952,1.67318,-2.08089,0.218971,1.45758,-1.80156,0.036287,1.52241,-1.85464,0.039785,1.54545,-1.91894,0.044479,1.52122,-1.96795,0.222206,1.43352,-2.05055,0.315096,1.18813,- [...]
+/*1208*/{0.212753,1.93683,-1.69787,0.306314,1.87346,-1.82709,0.047053,1.86112,-1.65148,-0.083378,1.79778,-1.63899,0.039697,1.69206,-1.64616,0.100634,1.62163,-1.63203,0.269527,1.95304,-2.07358,0.320551,1.87449,-1.90067,0.126559,1.91417,-2.01431,0.14037,1.73786,-2.19101,0.186063,1.64566,-2.2281,0.329717,1.67288,-2.09388,0.391371,1.66658,-2.08033,0.218736,1.45674,-1.80121,0.036275,1.51952,-1.8538,0.039758,1.5433,-1.91964,0.043585,1.51921,-1.96856,0.222947,1.43326,-2.05012,0.311995,1.18172,- [...]
+/*1209*/{0.213434,1.9348,-1.69763,0.306288,1.87115,-1.82715,0.047962,1.85877,-1.6509,-0.081372,1.79416,-1.63876,0.041565,1.6885,-1.64477,0.103044,1.61812,-1.63028,0.269786,1.95108,-2.07327,0.320341,1.87415,-1.90093,0.127012,1.91361,-2.01395,0.138234,1.73782,-2.18916,0.182941,1.64448,-2.22698,0.329602,1.66729,-2.09394,0.390915,1.65912,-2.08102,0.218952,1.45531,-1.80085,0.036213,1.51805,-1.85468,0.039932,1.54144,-1.9189,0.044144,1.51681,-1.96872,0.223961,1.43245,-2.04966,0.309107,1.17603,- [...]
+/*1210*/{0.214865,1.93326,-1.69694,0.306872,1.86921,-1.82671,0.048789,1.85537,-1.65046,-0.080294,1.79028,-1.6389,0.042626,1.68451,-1.64396,0.104523,1.615,-1.62869,0.269356,1.94863,-2.07308,0.320141,1.87162,-1.90087,0.12697,1.91222,-2.01288,0.134582,1.73614,-2.18717,0.180155,1.6432,-2.22518,0.329251,1.662,-2.09442,0.389553,1.65121,-2.08199,0.218637,1.45465,-1.80045,0.035793,1.51504,-1.85472,0.04058,1.53872,-1.91912,0.044633,1.51411,-1.96859,0.224424,1.43178,-2.04929,0.306272,1.16984,-1.73 [...]
+/*1211*/{0.215169,1.93027,-1.69691,0.306913,1.86762,-1.82697,0.050066,1.85128,-1.64975,-0.078471,1.78425,-1.63704,0.045219,1.68,-1.64218,0.107233,1.6109,-1.62712,0.268231,1.94581,-2.07317,0.320425,1.86945,-1.90106,0.126906,1.91069,-2.01257,0.132225,1.73502,-2.18527,0.177873,1.641,-2.2238,0.327815,1.6555,-2.09486,0.388845,1.64329,-2.08269,0.220726,1.45256,-1.79946,0.035968,1.51101,-1.85455,0.039804,1.53586,-1.91905,0.044221,1.51123,-1.96933,0.225127,1.42952,-2.04823,0.303858,1.16462,-1.73 [...]
+/*1212*/{0.215801,1.92727,-1.69665,0.306865,1.8655,-1.82721,0.051232,1.84728,-1.64848,-0.076356,1.77969,-1.63588,0.047814,1.67566,-1.64063,0.110177,1.60625,-1.62543,0.268474,1.94272,-2.07315,0.320324,1.86732,-1.90128,0.126434,1.90775,-2.01197,0.128518,1.73293,-2.18314,0.17485,1.63872,-2.22232,0.326445,1.64911,-2.09556,0.386526,1.63424,-2.08351,0.21963,1.44978,-1.79869,0.036436,1.50805,-1.85507,0.039477,1.53317,-1.91895,0.045405,1.50751,-1.9693,0.226067,1.42743,-2.04756,0.301392,1.1592,-1 [...]
+/*1213*/{0.216386,1.92403,-1.69602,0.306415,1.86348,-1.82753,0.052252,1.8417,-1.64714,-0.07402,1.77377,-1.63384,0.050024,1.67086,-1.6386,0.114106,1.60209,-1.62311,0.266731,1.93946,-2.07272,0.320367,1.86437,-1.90087,0.126016,1.90499,-2.01129,0.124752,1.73084,-2.18161,0.170855,1.63621,-2.22099,0.323997,1.64075,-2.09576,0.384684,1.62476,-2.08486,0.219026,1.44677,-1.79791,0.035767,1.50465,-1.85501,0.039888,1.53034,-1.91838,0.045147,1.50366,-1.97033,0.227168,1.42611,-2.04669,0.298566,1.15437, [...]
+/*1214*/{0.217214,1.92023,-1.69524,0.30633,1.85929,-1.82751,0.053338,1.83705,-1.64559,-0.071361,1.76639,-1.63163,0.052711,1.66507,-1.63601,0.116242,1.59747,-1.62161,0.266225,1.93602,-2.0737,0.319948,1.86117,-1.90121,0.125031,1.90151,-2.01075,0.121612,1.72771,-2.17968,0.16747,1.63326,-2.21952,0.32201,1.63319,-2.09628,0.38364,1.61534,-2.08655,0.218132,1.44328,-1.79744,0.035752,1.50035,-1.85441,0.040692,1.52669,-1.91814,0.04458,1.49985,-1.97025,0.227516,1.42361,-2.04593,0.296123,1.15004,-1. [...]
+/*1215*/{0.218088,1.91641,-1.69497,0.307416,1.85599,-1.8281,0.054602,1.83076,-1.64412,-0.070468,1.75908,-1.62844,0.056097,1.65925,-1.63342,0.12101,1.59174,-1.61879,0.265216,1.93272,-2.07451,0.320169,1.85786,-1.90148,0.124782,1.89799,-2.00933,0.116957,1.72567,-2.17898,0.163635,1.62952,-2.2181,0.320248,1.62545,-2.09674,0.380801,1.60457,-2.08818,0.2184,1.43983,-1.79619,0.035607,1.49752,-1.85506,0.040695,1.5227,-1.9176,0.045075,1.495,-1.97041,0.227972,1.42149,-2.04524,0.293703,1.14707,-1.731 [...]
+/*1216*/{0.218215,1.91124,-1.6941,0.30646,1.85302,-1.82831,0.056032,1.82463,-1.64116,-0.06719,1.75152,-1.6259,0.059856,1.65192,-1.63118,0.125031,1.58634,-1.61667,0.263328,1.92823,-2.0749,0.319304,1.8542,-1.90165,0.12357,1.89389,-2.00832,0.114639,1.72187,-2.17782,0.159088,1.62569,-2.21698,0.316821,1.61541,-2.09722,0.378375,1.59362,-2.08974,0.218191,1.43592,-1.79557,0.035651,1.49287,-1.85462,0.039696,1.51899,-1.91825,0.044759,1.49106,-1.9701,0.22863,1.41864,-2.04445,0.292578,1.14188,-1.729 [...]
+/*1217*/{0.218,1.90634,-1.69412,0.306962,1.84764,-1.8286,0.057647,1.81727,-1.63949,-0.065263,1.74391,-1.62249,0.063066,1.64579,-1.62767,0.12949,1.58047,-1.615,0.262478,1.92445,-2.076,0.319297,1.85094,-1.90191,0.123329,1.8896,-2.00766,0.110926,1.71804,-2.17697,0.15538,1.62122,-2.21586,0.314341,1.60593,-2.09784,0.374737,1.58254,-2.09164,0.218318,1.43261,-1.79445,0.035706,1.48853,-1.85478,0.040137,1.51492,-1.91698,0.044921,1.48605,-1.97068,0.228361,1.41566,-2.04392,0.290125,1.13787,-1.72783 [...]
+/*1218*/{0.219174,1.90115,-1.69318,0.307664,1.8436,-1.82856,0.059096,1.80985,-1.63762,-0.062905,1.735,-1.6187,0.068684,1.63902,-1.62515,0.134491,1.57414,-1.61217,0.260142,1.91964,-2.0764,0.318802,1.8468,-1.90239,0.122027,1.8847,-2.00688,0.10682,1.71365,-2.17607,0.150648,1.61701,-2.2143,0.309886,1.59593,-2.09938,0.371381,1.57051,-2.09386,0.220561,1.4307,-1.79332,0.035306,1.48436,-1.8552,0.040461,1.51015,-1.91683,0.044633,1.48103,-1.97053,0.228782,1.41216,-2.04316,0.287559,1.1331,-1.72645, [...]
+/*1219*/{0.220213,1.89529,-1.69239,0.307607,1.83966,-1.82928,0.061033,1.80195,-1.63532,-0.059065,1.72617,-1.61486,0.071777,1.6312,-1.62186,0.140106,1.56907,-1.61003,0.258377,1.91486,-2.07692,0.319082,1.84232,-1.90279,0.120376,1.88012,-2.00573,0.104107,1.70921,-2.1756,0.146346,1.61288,-2.21293,0.30729,1.58571,-2.10068,0.367135,1.55851,-2.09588,0.220001,1.42492,-1.7916,0.034851,1.47917,-1.85541,0.040322,1.50637,-1.91691,0.044551,1.47626,-1.97096,0.229249,1.40909,-2.04257,0.285561,1.12953,- [...]
+/*1220*/{0.219858,1.88935,-1.69218,0.307343,1.83546,-1.82978,0.06377,1.79328,-1.63268,-0.056143,1.71663,-1.61082,0.076595,1.62365,-1.61842,0.146627,1.56205,-1.60806,0.256749,1.91029,-2.07748,0.318577,1.83775,-1.90336,0.119636,1.87464,-2.0042,0.099981,1.7047,-2.17475,0.142414,1.60817,-2.21202,0.303378,1.57465,-2.10175,0.362401,1.54573,-2.09827,0.219646,1.42131,-1.78998,0.036059,1.4765,-1.85484,0.042844,1.50037,-1.91706,0.045848,1.47295,-1.97033,0.229716,1.40544,-2.04132,0.285279,1.12433,- [...]
+/*1221*/{0.220957,1.88309,-1.69098,0.307278,1.83008,-1.83008,0.066212,1.78432,-1.62966,-0.053537,1.70601,-1.60625,0.081273,1.61593,-1.61552,0.151372,1.55555,-1.60679,0.255469,1.90533,-2.07882,0.319022,1.83338,-1.90376,0.118334,1.86989,-2.00332,0.096807,1.69846,-2.17417,0.137712,1.60359,-2.21082,0.29937,1.56465,-2.10352,0.357691,1.53262,-2.10043,0.217071,1.41659,-1.78803,0.034319,1.46921,-1.8545,0.039693,1.49604,-1.91552,0.043592,1.46634,-1.97042,0.229761,1.4012,-2.04013,0.282762,1.11935, [...]
+/*1222*/{0.221826,1.87719,-1.69028,0.308159,1.82369,-1.8303,0.069576,1.77491,-1.62652,-0.047864,1.696,-1.60157,0.086894,1.60704,-1.61249,0.158171,1.54804,-1.60357,0.252925,1.90043,-2.07941,0.319003,1.82816,-1.90456,0.117596,1.86512,-2.00263,0.093726,1.69367,-2.17401,0.132554,1.59844,-2.20993,0.294997,1.55258,-2.10518,0.352438,1.51917,-2.10321,0.218245,1.4119,-1.78773,0.033564,1.46458,-1.85466,0.03979,1.49012,-1.91495,0.042582,1.46138,-1.97082,0.229662,1.39724,-2.04096,0.280172,1.11463,-1 [...]
+/*1223*/{0.222463,1.87068,-1.68952,0.308874,1.81893,-1.83071,0.072973,1.76509,-1.62346,-0.043079,1.68541,-1.59647,0.093116,1.59851,-1.60894,0.164706,1.54216,-1.60265,0.251916,1.89577,-2.08047,0.319172,1.82328,-1.9055,0.116271,1.8591,-2.00121,0.090427,1.68826,-2.17358,0.127437,1.59352,-2.20936,0.290181,1.54116,-2.1073,0.346185,1.5052,-2.10548,0.217833,1.40735,-1.78596,0.032114,1.45923,-1.85454,0.038008,1.48553,-1.91502,0.041202,1.45605,-1.97106,0.230108,1.3938,-2.03881,0.278499,1.10816,-1 [...]
+/*1224*/{0.223488,1.8645,-1.68855,0.30923,1.81391,-1.83087,0.076445,1.7552,-1.62023,-0.036889,1.67417,-1.59172,0.099772,1.59054,-1.60631,0.172177,1.53513,-1.59944,0.250369,1.89094,-2.08131,0.320259,1.81812,-1.90591,0.115562,1.85507,-1.99915,0.085954,1.68393,-2.17291,0.122192,1.58855,-2.20879,0.285335,1.52927,-2.1093,0.339831,1.4922,-2.10864,0.216669,1.40223,-1.78386,0.029548,1.45544,-1.8553,0.035618,1.47974,-1.91531,0.04062,1.45039,-1.97032,0.229942,1.38816,-2.03872,0.275028,1.10593,-1.7 [...]
+/*1225*/{0.224408,1.85897,-1.6873,0.309508,1.80883,-1.83155,0.08058,1.7449,-1.61698,-0.031395,1.66238,-1.58678,0.107235,1.58153,-1.60221,0.1802,1.52891,-1.59799,0.2484,1.88677,-2.08189,0.32064,1.81302,-1.90726,0.114716,1.85099,-1.99697,0.082094,1.67968,-2.17289,0.116746,1.58379,-2.2083,0.278853,1.51713,-2.11192,0.332619,1.47826,-2.11089,0.214841,1.39738,-1.78359,0.026304,1.45142,-1.85521,0.033065,1.47472,-1.91607,0.038565,1.44539,-1.97143,0.228734,1.3841,-2.03815,0.271067,1.104,-1.71349, [...]
+/*1226*/{0.226104,1.85365,-1.6866,0.310794,1.80317,-1.83176,0.08573,1.73419,-1.61385,-0.025079,1.6509,-1.58068,0.114857,1.57354,-1.59982,0.189061,1.52239,-1.59624,0.247321,1.88318,-2.08388,0.321593,1.80886,-1.909,0.113997,1.84809,-1.99525,0.078922,1.67498,-2.17156,0.111401,1.57922,-2.20803,0.272816,1.50494,-2.11432,0.32474,1.46514,-2.11361,0.211658,1.39414,-1.78251,0.022939,1.44798,-1.85547,0.030755,1.47015,-1.91663,0.036225,1.4412,-1.97173,0.228963,1.37888,-2.03581,0.26775,1.10424,-1.70 [...]
+/*1227*/{0.226874,1.84968,-1.68602,0.311562,1.79905,-1.83237,0.091438,1.72356,-1.61054,-0.0168,1.63938,-1.57553,0.123069,1.56572,-1.59661,0.198737,1.51716,-1.59512,0.246509,1.87948,-2.08509,0.321478,1.805,-1.9108,0.113917,1.84568,-1.99193,0.075603,1.67189,-2.17141,0.10591,1.57513,-2.20675,0.266983,1.49334,-2.11694,0.31673,1.45241,-2.1165,0.210042,1.39117,-1.781,0.019834,1.4443,-1.85676,0.027663,1.46553,-1.91735,0.032839,1.43615,-1.97184,0.226429,1.37596,-2.03658,0.263355,1.10415,-1.70547 [...]
+/*1228*/{0.227426,1.84576,-1.68533,0.312584,1.7944,-1.83435,0.097439,1.71341,-1.60759,-0.008259,1.6282,-1.57052,0.133356,1.55887,-1.59451,0.208553,1.51185,-1.59392,0.24618,1.87686,-2.08608,0.321942,1.80092,-1.91213,0.113539,1.84553,-1.99032,0.073875,1.67009,-2.16966,0.10122,1.57202,-2.20553,0.260148,1.48321,-2.11946,0.307649,1.44027,-2.11933,0.20791,1.38922,-1.77924,0.017097,1.44081,-1.8583,0.025756,1.46246,-1.91798,0.029547,1.43181,-1.97352,0.226011,1.37256,-2.03313,0.261392,1.10355,-1. [...]
+/*1229*/{0.228379,1.84241,-1.68489,0.311773,1.79046,-1.83515,0.103634,1.705,-1.60588,0.000572,1.61719,-1.56519,0.1436,1.55265,-1.59162,0.218914,1.50749,-1.59258,0.246344,1.87421,-2.08642,0.322373,1.79737,-1.91392,0.113705,1.84479,-1.98958,0.07013,1.66881,-2.16761,0.096919,1.5704,-2.20397,0.252367,1.47335,-2.12017,0.298375,1.42935,-2.12177,0.205219,1.38834,-1.77698,0.014355,1.43887,-1.86166,0.024036,1.46126,-1.91886,0.027389,1.42744,-1.97476,0.225143,1.37002,-2.0327,0.259702,1.10369,-1.70 [...]
+/*1230*/{0.229204,1.84,-1.68421,0.312144,1.78775,-1.83643,0.109435,1.69745,-1.60479,0.010364,1.60714,-1.56036,0.154487,1.54674,-1.59073,0.230372,1.50416,-1.59177,0.247449,1.87209,-2.08625,0.322659,1.79416,-1.91498,0.114343,1.84357,-1.98932,0.066617,1.66925,-2.16473,0.091754,1.5705,-2.20211,0.243782,1.46609,-2.12089,0.288773,1.41905,-2.12476,0.202619,1.38837,-1.77407,0.012898,1.43772,-1.86166,0.023169,1.46096,-1.91899,0.025193,1.42426,-1.97672,0.224482,1.36789,-2.03139,0.256017,1.10301,-1 [...]
+/*1231*/{0.230178,1.83803,-1.68461,0.311807,1.78533,-1.83778,0.11544,1.69267,-1.60344,0.021246,1.59903,-1.55647,0.16474,1.54286,-1.59036,0.242745,1.50194,-1.59145,0.24816,1.87028,-2.08621,0.322537,1.79281,-1.91524,0.114295,1.84222,-1.98995,0.063333,1.67049,-2.16134,0.086475,1.57246,-2.20099,0.235149,1.46007,-2.12163,0.278688,1.4104,-2.1276,0.200271,1.3882,-1.77167,0.011409,1.43667,-1.86326,0.022103,1.4601,-1.91878,0.023616,1.42281,-1.97692,0.218676,1.36501,-2.03298,0.252128,1.1008,-1.703 [...]
+/*1232*/{0.229803,1.83682,-1.68565,0.310706,1.78402,-1.83792,0.121365,1.68838,-1.60241,0.031918,1.59261,-1.55302,0.176583,1.53958,-1.58905,0.25505,1.50077,-1.59025,0.248686,1.86877,-2.08601,0.322159,1.79161,-1.91514,0.11491,1.84244,-1.99089,0.05935,1.673,-2.15807,0.081497,1.57456,-2.19969,0.226445,1.45562,-2.12306,0.267964,1.40437,-2.13104,0.197751,1.38733,-1.76934,0.010676,1.43618,-1.86341,0.020437,1.46005,-1.9193,0.022089,1.42279,-1.97707,0.220043,1.36522,-2.03084,0.245155,1.0971,-1.70 [...]
+/*1233*/{0.231063,1.83521,-1.68593,0.31059,1.78248,-1.83781,0.126653,1.68603,-1.60146,0.042986,1.58716,-1.54953,0.188471,1.53757,-1.58879,0.268535,1.50116,-1.59078,0.248979,1.86707,-2.08588,0.322763,1.7887,-1.91459,0.114995,1.84223,-1.99139,0.055943,1.67676,-2.15458,0.075511,1.57737,-2.1986,0.217895,1.45259,-2.12465,0.256931,1.39929,-2.13381,0.195687,1.38727,-1.76739,0.009733,1.43607,-1.86335,0.019789,1.45984,-1.91928,0.021678,1.42296,-1.97735,0.218263,1.36451,-2.03032,0.242414,1.09445,- [...]
+/*1234*/{0.23216,1.83472,-1.68659,0.310172,1.7817,-1.83814,0.131883,1.68445,-1.60026,0.05243,1.58303,-1.54697,0.200351,1.53711,-1.58814,0.281838,1.50288,-1.59123,0.248893,1.86627,-2.08559,0.320677,1.78839,-1.91488,0.114962,1.84215,-1.99158,0.051969,1.68123,-2.15172,0.06883,1.58061,-2.19817,0.209875,1.45194,-2.12678,0.246458,1.39685,-2.13654,0.194261,1.38682,-1.76572,0.008703,1.43566,-1.86296,0.019114,1.45985,-1.9185,0.020714,1.42358,-1.97682,0.218224,1.36508,-2.03013,0.234057,1.08909,-1. [...]
+/*1235*/{0.233238,1.83444,-1.68663,0.309419,1.78106,-1.83809,0.136864,1.68316,-1.59951,0.063819,1.57904,-1.54415,0.212271,1.53756,-1.58743,0.294962,1.50561,-1.59169,0.248877,1.86541,-2.08526,0.319778,1.78783,-1.91493,0.115134,1.84237,-1.99213,0.048098,1.68633,-2.14907,0.061969,1.58496,-2.19805,0.200657,1.45138,-2.12912,0.23592,1.39514,-2.13935,0.191842,1.38724,-1.76354,0.007884,1.43573,-1.86309,0.018355,1.45934,-1.91864,0.020024,1.42435,-1.97622,0.216521,1.36594,-2.03042,0.228551,1.0842, [...]
+/*1236*/{0.233505,1.83504,-1.68689,0.309421,1.78084,-1.83817,0.141835,1.68303,-1.59889,0.074228,1.57605,-1.54173,0.224217,1.53852,-1.5871,0.307656,1.50966,-1.59315,0.248704,1.86514,-2.08507,0.319221,1.78722,-1.91489,0.114717,1.84345,-1.99266,0.045131,1.69072,-2.1455,0.055181,1.58974,-2.19801,0.191798,1.45191,-2.13214,0.225149,1.39524,-2.14159,0.191627,1.38759,-1.76297,0.006809,1.43549,-1.86256,0.017198,1.45911,-1.91815,0.019517,1.42526,-1.97447,0.215974,1.36752,-2.03043,0.220297,1.08142, [...]
+/*1237*/{0.234846,1.8364,-1.68723,0.308688,1.78107,-1.83827,0.145481,1.68371,-1.59835,0.084255,1.57287,-1.53944,0.235625,1.5407,-1.58718,0.320769,1.51493,-1.59522,0.248369,1.86507,-2.08546,0.3187,1.78763,-1.91486,0.114297,1.84496,-1.99268,0.039227,1.697,-2.14362,0.048332,1.59552,-2.19854,0.182653,1.454,-2.13494,0.21502,1.39726,-2.14364,0.189902,1.38819,-1.76126,0.006707,1.43546,-1.86247,0.017099,1.4616,-1.91812,0.017985,1.42621,-1.97408,0.215458,1.36969,-2.02983,0.213889,1.079,-1.70154,0 [...]
+/*1238*/{0.236212,1.83795,-1.6875,0.309266,1.78196,-1.83874,0.150353,1.68425,-1.59752,0.094484,1.57064,-1.5366,0.246556,1.54405,-1.58746,0.332574,1.52119,-1.59674,0.246845,1.86569,-2.08574,0.318548,1.78809,-1.91522,0.113973,1.84558,-1.99213,0.036051,1.7037,-2.14249,0.041141,1.60189,-2.19893,0.174194,1.45752,-2.13718,0.205049,1.40039,-2.1455,0.189005,1.3875,-1.7603,0.005955,1.43598,-1.86221,0.01626,1.46032,-1.91722,0.017202,1.42763,-1.97376,0.215551,1.372,-2.02993,0.206914,1.0764,-1.70257 [...]
+/*1239*/{0.236867,1.84013,-1.68781,0.309714,1.78357,-1.84015,0.154864,1.68566,-1.59636,0.104799,1.56954,-1.53452,0.257167,1.54805,-1.5882,0.344497,1.52808,-1.59931,0.246568,1.86648,-2.0861,0.318024,1.78863,-1.91596,0.113346,1.84782,-1.99211,0.031489,1.7092,-2.142,0.03411,1.60871,-2.19929,0.165709,1.4632,-2.13891,0.195215,1.40532,-2.14728,0.188819,1.3876,-1.76055,0.005694,1.43673,-1.86245,0.016008,1.46077,-1.916,0.016507,1.42836,-1.97307,0.215433,1.37442,-2.02963,0.204136,1.07684,-1.70432 [...]
+/*1240*/{0.238791,1.84287,-1.68854,0.308979,1.78463,-1.83988,0.16089,1.68705,-1.59538,0.116412,1.57032,-1.53258,0.268065,1.55356,-1.5891,0.355119,1.53605,-1.6021,0.244507,1.86809,-2.08601,0.31774,1.7902,-1.91697,0.112597,1.84902,-1.99147,0.027284,1.71643,-2.14153,0.026968,1.61591,-2.20002,0.156898,1.46887,-2.13994,0.185886,1.41064,-2.14837,0.188224,1.38801,-1.76072,0.006085,1.43753,-1.86186,0.016258,1.46152,-1.91549,0.016355,1.42883,-1.97254,0.214826,1.3761,-2.02946,0.196743,1.07545,-1.7 [...]
+/*1241*/{0.240637,1.84596,-1.6891,0.309831,1.78693,-1.84063,0.166139,1.68854,-1.59449,0.12406,1.56986,-1.5315,0.277089,1.55886,-1.59019,0.365023,1.54498,-1.60519,0.242798,1.8701,-2.08605,0.317441,1.79195,-1.91726,0.111437,1.85068,-1.98944,0.021018,1.723,-2.14121,0.019294,1.62278,-2.20043,0.148777,1.47537,-2.14143,0.176896,1.41773,-2.14937,0.189358,1.3874,-1.7602,0.008423,1.44019,-1.86082,0.017614,1.46303,-1.91499,0.018401,1.43105,-1.97249,0.215364,1.37889,-2.02969,0.186803,1.07213,-1.707 [...]
+/*1242*/{0.242396,1.84905,-1.69011,0.310048,1.78887,-1.84177,0.172384,1.69123,-1.59409,0.134317,1.57186,-1.53014,0.286838,1.56496,-1.59139,0.374548,1.55387,-1.60866,0.240709,1.87274,-2.08611,0.317298,1.79399,-1.91856,0.110447,1.85263,-1.98845,0.015221,1.72988,-2.14089,0.012682,1.63025,-2.20045,0.140154,1.48243,-2.14135,0.168666,1.4254,-2.15012,0.189016,1.38635,-1.75975,0.007581,1.44141,-1.86053,0.017012,1.46457,-1.91456,0.017361,1.43219,-1.97202,0.21562,1.38014,-2.02935,0.17676,1.07144,- [...]
+/*1243*/{0.244599,1.85201,-1.68995,0.310973,1.79158,-1.84233,0.178592,1.69392,-1.59443,0.144352,1.57459,-1.52891,0.294784,1.57156,-1.59377,0.382864,1.5631,-1.61165,0.238237,1.87537,-2.0868,0.316828,1.79652,-1.91913,0.109776,1.85375,-1.98671,0.009766,1.73734,-2.14082,0.005503,1.63801,-2.20084,0.132874,1.49037,-2.14202,0.160409,1.43315,-2.15102,0.189502,1.38688,-1.76035,0.007795,1.4441,-1.85963,0.017824,1.46685,-1.91371,0.018343,1.43418,-1.97178,0.215991,1.38314,-2.02862,0.1711,1.0707,-1.7 [...]
+/*1244*/{0.247077,1.85628,-1.69112,0.310924,1.79489,-1.84339,0.185249,1.69664,-1.59352,0.153425,1.57615,-1.52795,0.303238,1.57904,-1.59546,0.390251,1.57285,-1.61542,0.234084,1.87883,-2.08644,0.316308,1.79936,-1.92104,0.108343,1.85654,-1.9851,0.005182,1.7443,-2.14029,-0.000886,1.64548,-2.20074,0.125398,1.49911,-2.14277,0.153622,1.44149,-2.15182,0.191081,1.38679,-1.76032,0.009436,1.44589,-1.85924,0.018662,1.46902,-1.91257,0.019198,1.43639,-1.97167,0.216603,1.3865,-2.02796,0.1643,1.07042,-1 [...]
+/*1245*/{0.249596,1.86028,-1.69209,0.312252,1.79753,-1.84447,0.191955,1.69989,-1.59368,0.163075,1.58024,-1.52708,0.311004,1.58595,-1.59771,0.397737,1.58255,-1.61942,0.23209,1.88272,-2.08625,0.316379,1.80243,-1.92189,0.107375,1.85931,-1.98273,0.000561,1.75169,-2.14077,-0.00766,1.65306,-2.19993,0.118532,1.50681,-2.14293,0.146827,1.4509,-2.15271,0.192732,1.387,-1.76035,0.010675,1.44819,-1.85903,0.019862,1.47106,-1.91172,0.019893,1.43925,-1.97101,0.217136,1.38878,-2.02717,0.157308,1.0708,-1. [...]
+/*1246*/{0.251521,1.86477,-1.6933,0.313484,1.8015,-1.84601,0.198941,1.70354,-1.5937,0.17136,1.58274,-1.52617,0.317407,1.59407,-1.59975,0.404309,1.59315,-1.62255,0.22924,1.88678,-2.08526,0.316547,1.80558,-1.92344,0.107072,1.86082,-1.98113,-0.003214,1.75979,-2.13972,-0.01319,1.66118,-2.19934,0.112016,1.51606,-2.14298,0.140194,1.46005,-2.15386,0.193253,1.38701,-1.75967,0.01147,1.45059,-1.85786,0.020438,1.47386,-1.91046,0.021451,1.44251,-1.97077,0.218273,1.39128,-2.02617,0.149799,1.07062,-1. [...]
+/*1247*/{0.254061,1.86945,-1.69408,0.31394,1.8051,-1.84663,0.20532,1.70732,-1.59373,0.180087,1.58638,-1.52576,0.323921,1.60133,-1.60175,0.410118,1.60328,-1.62609,0.226789,1.89082,-2.08512,0.316688,1.8094,-1.92476,0.105856,1.86358,-1.97923,-0.007757,1.7668,-2.13932,-0.019478,1.66931,-2.19904,0.106117,1.52535,-2.1431,0.134606,1.46943,-2.15499,0.194511,1.38766,-1.75925,0.012953,1.45337,-1.8566,0.021943,1.47611,-1.90896,0.022132,1.44603,-1.97087,0.218887,1.39464,-2.02564,0.144451,1.07292,-1. [...]
+/*1248*/{0.256181,1.87368,-1.69551,0.314475,1.80842,-1.84801,0.211564,1.71128,-1.59406,0.188489,1.59023,-1.52415,0.329883,1.60926,-1.60392,0.415022,1.61312,-1.62938,0.224118,1.89498,-2.0847,0.316494,1.81356,-1.92632,0.105789,1.86633,-1.97784,-0.010529,1.77355,-2.14011,-0.025275,1.67736,-2.19743,0.100591,1.53483,-2.14261,0.129992,1.47875,-2.15587,0.196532,1.3884,-1.75893,0.015701,1.45579,-1.85708,0.023572,1.4786,-1.90831,0.024518,1.44912,-1.97028,0.220572,1.39778,-2.02479,0.13745,1.07399, [...]
+/*1249*/{0.257929,1.8781,-1.69654,0.31506,1.81227,-1.84994,0.218762,1.71644,-1.59393,0.196449,1.59468,-1.52409,0.334471,1.61668,-1.60632,0.419608,1.62274,-1.63322,0.221546,1.89902,-2.08416,0.316924,1.81721,-1.92707,0.104578,1.86951,-1.97558,-0.015351,1.78086,-2.13913,-0.030436,1.68556,-2.19613,0.094026,1.54434,-2.1422,0.124965,1.48847,-2.15623,0.197415,1.38888,-1.75837,0.016683,1.45855,-1.85689,0.024938,1.48212,-1.90712,0.025677,1.45335,-1.97063,0.221325,1.40102,-2.02416,0.132315,1.07427 [...]
+/*1250*/{0.260176,1.88361,-1.69878,0.316565,1.81681,-1.85171,0.224344,1.7208,-1.59458,0.203466,1.59966,-1.52339,0.33946,1.6252,-1.60814,0.423684,1.63232,-1.63711,0.22055,1.9034,-2.08405,0.31685,1.82107,-1.92926,0.104623,1.87287,-1.97441,-0.017907,1.788,-2.13853,-0.035308,1.6937,-2.19454,0.089823,1.55372,-2.14198,0.120293,1.49768,-2.15711,0.199414,1.3898,-1.75864,0.019213,1.46153,-1.85581,0.02712,1.48514,-1.90492,0.028033,1.45753,-1.97072,0.222642,1.4049,-2.02311,0.126191,1.07549,-1.72686 [...]
+/*1251*/{0.261985,1.88844,-1.70026,0.317515,1.82049,-1.85308,0.229544,1.72541,-1.59447,0.210638,1.60462,-1.5232,0.343279,1.63173,-1.60991,0.427565,1.64166,-1.64123,0.218822,1.90751,-2.08427,0.316801,1.82494,-1.93072,0.104364,1.87596,-1.97237,-0.020406,1.79516,-2.13802,-0.040463,1.70227,-2.19331,0.085873,1.56285,-2.14066,0.115736,1.50705,-2.15771,0.201257,1.39023,-1.75884,0.021405,1.46392,-1.8555,0.029595,1.48783,-1.90397,0.030039,1.46232,-1.97054,0.224268,1.40892,-2.02237,0.119946,1.0763 [...]
+/*1252*/{0.264219,1.89265,-1.70198,0.318373,1.82435,-1.85472,0.23416,1.73021,-1.59528,0.217117,1.60934,-1.5226,0.34654,1.63908,-1.61144,0.429944,1.65064,-1.64373,0.217785,1.9113,-2.08307,0.316924,1.82923,-1.93198,0.103633,1.87905,-1.97159,-0.02325,1.80238,-2.13669,-0.044568,1.71,-2.19076,0.080581,1.57113,-2.13995,0.111991,1.51574,-2.15778,0.203066,1.39038,-1.75922,0.023748,1.46737,-1.85484,0.031158,1.49148,-1.90269,0.032218,1.46664,-1.9706,0.225252,1.41251,-2.02216,0.115338,1.07943,-1.73 [...]
+/*1253*/{0.265368,1.89682,-1.70376,0.318913,1.82886,-1.85622,0.238068,1.73521,-1.59595,0.222456,1.6136,-1.52134,0.350317,1.64675,-1.61319,0.432678,1.65926,-1.64685,0.216848,1.9148,-2.08275,0.317305,1.83334,-1.93273,0.104011,1.88299,-1.97155,-0.025762,1.80781,-2.13304,-0.049306,1.71741,-2.18953,0.07665,1.57959,-2.13937,0.108403,1.52389,-2.15828,0.204858,1.39148,-1.75999,0.02657,1.47066,-1.85488,0.032804,1.49514,-1.90151,0.033806,1.47177,-1.9707,0.227059,1.41642,-2.02188,0.110993,1.08108,- [...]
+/*1254*/{0.266918,1.90157,-1.7048,0.320094,1.8328,-1.85802,0.242702,1.73979,-1.59583,0.227546,1.61845,-1.52112,0.353338,1.65278,-1.61475,0.435388,1.66709,-1.65015,0.21642,1.91856,-2.08289,0.316807,1.83711,-1.93452,0.103808,1.88633,-1.97093,-0.02674,1.81404,-2.13109,-0.052492,1.72497,-2.18677,0.073708,1.58727,-2.13834,0.105278,1.53201,-2.15855,0.20744,1.39209,-1.76115,0.028172,1.47402,-1.85417,0.035361,1.49893,-1.90085,0.035822,1.47538,-1.96976,0.22829,1.42046,-2.0221,0.10553,1.08506,-1.7 [...]
+/*1255*/{0.268561,1.90557,-1.70613,0.321545,1.83734,-1.85983,0.246144,1.744,-1.59585,0.232968,1.62247,-1.52023,0.355812,1.65929,-1.61707,0.436571,1.67444,-1.65447,0.215256,1.92177,-2.08311,0.317206,1.84076,-1.9357,0.105817,1.88824,-1.96942,-0.029312,1.81946,-2.12884,-0.056104,1.73239,-2.18472,0.07085,1.59466,-2.13752,0.102402,1.53914,-2.15848,0.210072,1.3926,-1.76269,0.031272,1.47709,-1.85374,0.036729,1.50257,-1.89873,0.038035,1.48082,-1.97013,0.227741,1.42461,-2.02229,0.10138,1.08756,-1 [...]
+/*1256*/{0.270002,1.9094,-1.70722,0.322228,1.84029,-1.86148,0.249608,1.74826,-1.59612,0.23772,1.62744,-1.5198,0.35799,1.66487,-1.61857,0.438273,1.68183,-1.6563,0.215149,1.92467,-2.08294,0.317688,1.8446,-1.93672,0.105892,1.89124,-1.96893,-0.029848,1.82515,-2.12636,-0.059032,1.73904,-2.18189,0.068582,1.60139,-2.13659,0.099921,1.54597,-2.15841,0.211424,1.39352,-1.76328,0.032562,1.4804,-1.85284,0.037982,1.5061,-1.89805,0.040423,1.48541,-1.97006,0.229662,1.42907,-2.0228,0.095835,1.08991,-1.73 [...]
+/*1257*/{0.271527,1.91254,-1.70859,0.323246,1.84471,-1.86261,0.252353,1.75206,-1.59609,0.24194,1.63085,-1.51885,0.359482,1.67046,-1.61953,0.439591,1.68834,-1.65945,0.214887,1.92789,-2.08292,0.318338,1.84753,-1.93823,0.106169,1.89365,-1.96859,-0.029823,1.83132,-2.12424,-0.061718,1.74544,-2.17976,0.066138,1.60781,-2.13562,0.097125,1.55257,-2.158,0.213725,1.39439,-1.76443,0.03546,1.48371,-1.85298,0.040802,1.50932,-1.89675,0.042799,1.49012,-1.96923,0.229955,1.43273,-2.02274,0.092328,1.09236, [...]
+/*1258*/{0.273719,1.91577,-1.70895,0.323356,1.84744,-1.86388,0.255273,1.75585,-1.59581,0.24575,1.63464,-1.51844,0.361691,1.67529,-1.62101,0.439967,1.69424,-1.66213,0.214684,1.93076,-2.0827,0.319049,1.85117,-1.939,0.106957,1.8959,-1.9681,-0.032655,1.83767,-2.12053,-0.0637,1.75194,-2.17742,0.064336,1.61275,-2.13466,0.095566,1.55812,-2.15803,0.216105,1.39557,-1.76529,0.037408,1.48679,-1.85156,0.042719,1.51267,-1.89552,0.044806,1.49474,-1.96815,0.231107,1.4366,-2.02337,0.088045,1.09562,-1.73 [...]
+/*1259*/{0.274672,1.91874,-1.71035,0.32461,1.85128,-1.86467,0.257977,1.75878,-1.59524,0.24829,1.63711,-1.51758,0.362857,1.6796,-1.62185,0.440875,1.69955,-1.66465,0.215068,1.93344,-2.08242,0.319357,1.85421,-1.93963,0.107808,1.89869,-1.96714,-0.032053,1.84243,-2.11801,-0.065862,1.75756,-2.17453,0.062816,1.6183,-2.13385,0.093391,1.56373,-2.1578,0.217463,1.39686,-1.76676,0.039261,1.49006,-1.85153,0.043545,1.51594,-1.89474,0.0472,1.49886,-1.96801,0.230649,1.43993,-2.02303,0.085403,1.09919,-1. [...]
+/*1260*/{0.277066,1.92112,-1.71084,0.325329,1.85448,-1.86617,0.259785,1.76227,-1.59647,0.251507,1.63972,-1.51676,0.364689,1.68374,-1.62324,0.441896,1.7041,-1.66718,0.215304,1.93616,-2.08222,0.319721,1.8573,-1.94008,0.108532,1.90074,-1.96666,-0.031902,1.84775,-2.11419,-0.067098,1.76301,-2.17203,0.061494,1.6228,-2.13258,0.091705,1.56811,-2.15807,0.218996,1.39855,-1.76711,0.041812,1.49302,-1.84995,0.044976,1.51919,-1.89336,0.048943,1.50275,-1.96735,0.231996,1.44331,-2.02353,0.083149,1.1033, [...]
+/*1261*/{0.277591,1.92361,-1.7114,0.326753,1.85796,-1.86655,0.261993,1.76471,-1.59657,0.254099,1.64286,-1.51699,0.365562,1.68659,-1.62459,0.442855,1.70791,-1.66941,0.215834,1.939,-2.08179,0.320208,1.85907,-1.94062,0.109289,1.9028,-1.96539,-0.030632,1.85237,-2.11145,-0.068269,1.76796,-2.16952,0.060046,1.62725,-2.13235,0.090287,1.5723,-2.15765,0.221074,1.40015,-1.76791,0.043086,1.49581,-1.84937,0.045872,1.52224,-1.89256,0.050242,1.50686,-1.96647,0.231655,1.44642,-2.02382,0.07999,1.1071,-1. [...]
+/*1262*/{0.279411,1.92587,-1.71208,0.32808,1.8605,-1.86756,0.264366,1.76758,-1.59654,0.256643,1.64512,-1.51592,0.365722,1.68952,-1.62587,0.443099,1.71119,-1.67153,0.216482,1.94096,-2.08185,0.321163,1.86219,-1.94174,0.111402,1.90429,-1.96562,-0.030361,1.85632,-2.10803,-0.069148,1.77244,-2.16745,0.059652,1.631,-2.13145,0.08949,1.57566,-2.15744,0.221897,1.40243,-1.76835,0.043976,1.49952,-1.84847,0.046825,1.5254,-1.89173,0.051017,1.50952,-1.9652,0.232314,1.44977,-2.02345,0.07717,1.11027,-1.7 [...]
+/*1263*/{0.280727,1.92747,-1.71272,0.328388,1.86307,-1.86831,0.266096,1.76979,-1.59696,0.258694,1.64712,-1.51612,0.367199,1.6921,-1.62634,0.443387,1.71332,-1.67365,0.216824,1.94347,-2.08137,0.322428,1.86443,-1.94205,0.112226,1.90608,-1.96519,-0.029836,1.85899,-2.10568,-0.069407,1.77571,-2.16465,0.059734,1.63436,-2.13099,0.088476,1.57844,-2.15724,0.22311,1.40463,-1.76903,0.046072,1.50128,-1.8477,0.048055,1.52816,-1.89118,0.052019,1.51322,-1.96432,0.23218,1.45245,-2.0231,0.076131,1.11384,- [...]
+/*1264*/{0.282422,1.92917,-1.71276,0.329644,1.86545,-1.8684,0.2674,1.77204,-1.59661,0.25903,1.64801,-1.51588,0.368231,1.69333,-1.62707,0.443542,1.71481,-1.67532,0.216942,1.94518,-2.08112,0.323451,1.86621,-1.94252,0.11293,1.90788,-1.96541,-0.025565,1.86158,-2.10527,-0.069655,1.77862,-2.16307,0.05942,1.63666,-2.13027,0.08784,1.58078,-2.15707,0.223974,1.40696,-1.76933,0.046729,1.50419,-1.848,0.04853,1.53116,-1.89143,0.05212,1.51572,-1.9635,0.233133,1.45505,-2.02312,0.077477,1.11691,-1.72635 [...]
+/*1265*/{0.283553,1.93084,-1.71307,0.329877,1.8678,-1.86944,0.268666,1.77302,-1.59618,0.260981,1.6496,-1.51581,0.369023,1.69466,-1.62836,0.443763,1.71603,-1.67668,0.218754,1.94658,-2.08099,0.323791,1.86744,-1.9431,0.113889,1.91026,-1.96457,-0.026501,1.86356,-2.10274,-0.069394,1.78096,-2.16127,0.058608,1.63915,-2.13026,0.087661,1.58253,-2.15703,0.225717,1.40969,-1.76929,0.04835,1.50602,-1.84746,0.050094,1.53378,-1.8907,0.051317,1.51838,-1.96315,0.233303,1.45699,-2.0228,0.078417,1.11849,-1 [...]
+/*1266*/{0.285224,1.93169,-1.71307,0.331411,1.86926,-1.86984,0.269297,1.77453,-1.59614,0.261976,1.65081,-1.51592,0.369055,1.69455,-1.62921,0.443798,1.71674,-1.67891,0.219604,1.94827,-2.08093,0.324777,1.86904,-1.94348,0.114928,1.91128,-1.96547,-0.024832,1.86586,-2.10036,-0.069238,1.78272,-2.15941,0.058674,1.63985,-2.12995,0.086839,1.58361,-2.15725,0.226165,1.41174,-1.76918,0.048623,1.50835,-1.84695,0.049156,1.53571,-1.8899,0.051874,1.5203,-1.96246,0.232981,1.45891,-2.02242,0.080855,1.1192 [...]
+/*1267*/{0.286705,1.9324,-1.71351,0.332351,1.87084,-1.8705,0.270251,1.77549,-1.59705,0.262194,1.65084,-1.51611,0.369819,1.69455,-1.63026,0.443459,1.71481,-1.68084,0.220551,1.94968,-2.08129,0.326237,1.87019,-1.94401,0.116964,1.91281,-1.96483,-0.023525,1.86671,-2.09924,-0.069052,1.78357,-2.15829,0.059812,1.64089,-2.12956,0.087124,1.58426,-2.15712,0.226647,1.41372,-1.76951,0.049422,1.50988,-1.84613,0.048989,1.53794,-1.8904,0.053791,1.5203,-1.96091,0.233739,1.46043,-2.02219,0.084569,1.11951, [...]
+/*1268*/{0.287562,1.93245,-1.71388,0.332172,1.87178,-1.87108,0.269983,1.7757,-1.59723,0.262194,1.65084,-1.51611,0.369792,1.69337,-1.63153,0.443775,1.71313,-1.68159,0.221925,1.95037,-2.08144,0.32706,1.87046,-1.94419,0.117546,1.91431,-1.96504,-0.022322,1.86742,-2.09752,-0.068363,1.78411,-2.15655,0.0613,1.64079,-2.12912,0.087154,1.58345,-2.15728,0.227155,1.41592,-1.76895,0.04905,1.51143,-1.84576,0.049652,1.53912,-1.88912,0.053325,1.5212,-1.96068,0.233255,1.46145,-2.02218,0.089775,1.11857,-1 [...]
+/*1269*/{0.289059,1.93263,-1.71366,0.331845,1.87301,-1.87162,0.269837,1.77574,-1.59755,0.26076,1.65016,-1.51651,0.370334,1.69189,-1.63219,0.444145,1.71107,-1.68274,0.223029,1.95105,-2.08146,0.328062,1.87115,-1.94409,0.11902,1.9152,-1.96471,-0.021892,1.86771,-2.09563,-0.067603,1.78424,-2.1559,0.061628,1.6402,-2.12917,0.087382,1.58249,-2.15757,0.227888,1.41788,-1.76856,0.048923,1.5119,-1.84658,0.049133,1.54091,-1.89,0.054007,1.52195,-1.96017,0.233648,1.46262,-2.02185,0.095706,1.11721,-1.73 [...]
+/*1270*/{0.290349,1.932,-1.71372,0.333342,1.87361,-1.87241,0.269221,1.77511,-1.59838,0.260075,1.649,-1.51678,0.370299,1.68947,-1.63295,0.443359,1.70747,-1.68399,0.223785,1.95116,-2.08141,0.328484,1.8708,-1.94476,0.120769,1.91667,-1.96518,-0.021486,1.86671,-2.09456,-0.066259,1.78364,-2.15467,0.061842,1.6389,-2.12879,0.088385,1.5813,-2.15776,0.227939,1.41902,-1.76873,0.048988,1.5126,-1.84724,0.049188,1.54129,-1.88992,0.053426,1.52189,-1.96037,0.234556,1.46323,-2.0214,0.101576,1.11594,-1.73 [...]
+/*1271*/{0.291332,1.93118,-1.71367,0.334033,1.87335,-1.87261,0.268177,1.77469,-1.59945,0.259145,1.64807,-1.51826,0.369901,1.68665,-1.63372,0.443415,1.70393,-1.68456,0.225277,1.95101,-2.08178,0.329082,1.87083,-1.94518,0.121501,1.91721,-1.96521,-0.01962,1.86542,-2.09273,-0.06506,1.78205,-2.15393,0.063019,1.63681,-2.12886,0.088889,1.57905,-2.15816,0.229417,1.42004,-1.76822,0.048988,1.5126,-1.84724,0.049188,1.54129,-1.88992,0.053685,1.52071,-1.96045,0.234879,1.46327,-2.02104,0.1094,1.11267,- [...]
+/*1272*/{0.291916,1.92979,-1.7139,0.33413,1.87297,-1.87317,0.267254,1.77362,-1.59948,0.257585,1.64725,-1.51932,0.368801,1.68304,-1.63482,0.443078,1.69918,-1.68487,0.22671,1.95036,-2.08193,0.330146,1.87075,-1.94523,0.122503,1.91724,-1.96526,-0.017752,1.86387,-2.09242,-0.063577,1.78031,-2.15389,0.064175,1.63402,-2.12889,0.090369,1.57608,-2.15811,0.22971,1.42054,-1.76866,0.049235,1.51163,-1.84606,0.049487,1.54134,-1.89009,0.053527,1.51944,-1.96089,0.235291,1.46313,-2.02075,0.11496,1.10941,- [...]
+/*1273*/{0.293304,1.92852,-1.71465,0.334192,1.8715,-1.87314,0.266439,1.77222,-1.59987,0.25542,1.64425,-1.51971,0.368053,1.67958,-1.63548,0.443167,1.69354,-1.68558,0.227694,1.94969,-2.08221,0.330382,1.86962,-1.94509,0.124297,1.91703,-1.96609,-0.016541,1.86133,-2.09151,-0.06193,1.77739,-2.15347,0.065666,1.6303,-2.12908,0.091744,1.57287,-2.15835,0.230137,1.42055,-1.76871,0.049482,1.50983,-1.8466,0.051179,1.53968,-1.89033,0.053167,1.51747,-1.96039,0.235671,1.46261,-2.02023,0.122934,1.10626,- [...]
+/*1274*/{0.29382,1.92674,-1.71433,0.334592,1.87116,-1.87369,0.26587,1.77011,-1.60042,0.253761,1.64262,-1.52129,0.366247,1.67487,-1.63598,0.442398,1.6881,-1.68619,0.229055,1.9486,-2.08224,0.330764,1.86842,-1.94491,0.125006,1.91693,-1.96629,-0.015847,1.85917,-2.09064,-0.059818,1.77445,-2.15295,0.068165,1.62894,-2.12972,0.093375,1.56923,-2.15907,0.230617,1.42017,-1.76855,0.049396,1.50953,-1.84679,0.05057,1.53813,-1.89045,0.052745,1.51587,-1.96123,0.236102,1.46146,-2.02026,0.131643,1.10504,- [...]
+/*1275*/{0.293824,1.92372,-1.71453,0.334981,1.86896,-1.8735,0.264509,1.76833,-1.60129,0.250637,1.63939,-1.52194,0.364844,1.67064,-1.63665,0.440947,1.68153,-1.6863,0.230422,1.94753,-2.08237,0.330574,1.8668,-1.94495,0.12519,1.9153,-1.96734,-0.014261,1.8559,-2.09067,-0.05769,1.77052,-2.15316,0.071156,1.62326,-2.12963,0.094726,1.56477,-2.15918,0.231664,1.41956,-1.76841,0.04904,1.50688,-1.84764,0.050449,1.53615,-1.89132,0.053273,1.51365,-1.96148,0.236437,1.46027,-2.02027,0.141052,1.10427,-1.7 [...]
+/*1276*/{0.294397,1.92189,-1.71526,0.335533,1.86733,-1.87391,0.262079,1.76614,-1.60198,0.247211,1.63705,-1.52353,0.362987,1.66509,-1.63745,0.439412,1.67527,-1.68711,0.231241,1.94541,-2.08274,0.330635,1.86489,-1.94461,0.12636,1.91387,-1.96796,-0.012916,1.85232,-2.09142,-0.055296,1.76637,-2.15348,0.072784,1.61847,-2.13004,0.096968,1.56012,-2.15984,0.231818,1.41898,-1.76885,0.050253,1.50445,-1.84755,0.050305,1.5344,-1.89168,0.053883,1.5107,-1.96193,0.237215,1.45857,-2.02061,0.148024,1.10113 [...]
+/*1277*/{0.2952,1.91944,-1.71551,0.336415,1.86472,-1.87402,0.260542,1.76339,-1.60279,0.244414,1.63509,-1.52539,0.36111,1.65913,-1.63734,0.438211,1.66763,-1.68674,0.231441,1.9436,-2.08272,0.330111,1.86298,-1.94435,0.126219,1.91268,-1.96915,-0.011516,1.84782,-2.09167,-0.052591,1.76134,-2.15417,0.075431,1.61297,-2.13085,0.099559,1.5546,-2.16024,0.233207,1.41734,-1.76918,0.050561,1.5022,-1.84818,0.050769,1.53189,-1.89158,0.053914,1.50803,-1.96358,0.237359,1.456,-2.02108,0.155572,1.0974,-1.75 [...]
+/*1278*/{0.295705,1.91663,-1.71515,0.335974,1.86247,-1.87401,0.258396,1.76044,-1.60341,0.240522,1.63138,-1.52675,0.358674,1.65331,-1.6381,0.436719,1.65931,-1.68656,0.232358,1.94116,-2.08269,0.330705,1.86113,-1.94456,0.126274,1.91047,-1.97077,-0.01047,1.84367,-2.09275,-0.050083,1.75562,-2.15539,0.077448,1.60816,-2.13254,0.102398,1.54902,-2.16089,0.233798,1.4158,-1.76945,0.050917,1.49921,-1.8482,0.050902,1.52914,-1.89098,0.054082,1.50524,-1.96357,0.237616,1.45315,-2.02202,0.162047,1.09409, [...]
+/*1279*/{0.294592,1.91317,-1.71525,0.33596,1.85926,-1.87352,0.256524,1.75676,-1.60303,0.236379,1.62808,-1.52802,0.355969,1.64704,-1.63864,0.434394,1.65082,-1.68621,0.233133,1.93832,-2.08289,0.330623,1.85821,-1.94478,0.126074,1.90859,-1.97188,-0.008912,1.83809,-2.09327,-0.047409,1.74967,-2.15623,0.080117,1.60168,-2.13325,0.104902,1.54252,-2.16213,0.234635,1.4143,-1.77012,0.050575,1.49644,-1.84949,0.052147,1.52553,-1.89333,0.05434,1.50168,-1.9649,0.237066,1.4498,-2.02338,0.168468,1.09099,- [...]
+/*1280*/{0.294661,1.90989,-1.71429,0.335929,1.85651,-1.87332,0.254255,1.75292,-1.60339,0.232271,1.6247,-1.52965,0.352928,1.64043,-1.63875,0.431564,1.64206,-1.68607,0.232134,1.93501,-2.08292,0.331047,1.85601,-1.94413,0.126204,1.90557,-1.97138,-0.006585,1.8325,-2.09487,-0.045123,1.74271,-2.15809,0.083009,1.59468,-2.1351,0.108155,1.53604,-2.16284,0.235722,1.4124,-1.7717,0.050415,1.49308,-1.84926,0.050763,1.52286,-1.89366,0.052795,1.49905,-1.96603,0.236868,1.44684,-2.02413,0.174243,1.08787,- [...]
+/*1281*/{0.294202,1.90522,-1.71363,0.336315,1.85238,-1.87274,0.251776,1.74942,-1.60408,0.227617,1.62154,-1.53086,0.349205,1.63315,-1.63863,0.429955,1.63288,-1.68523,0.23288,1.93147,-2.08343,0.331479,1.85273,-1.94363,0.125687,1.90201,-1.97322,-0.006645,1.8263,-2.09708,-0.042492,1.7351,-2.15951,0.086229,1.5887,-2.13683,0.111409,1.52901,-2.16377,0.236017,1.41004,-1.7719,0.050669,1.48992,-1.85045,0.052383,1.51941,-1.89421,0.05198,1.49535,-1.96619,0.237397,1.44386,-2.02528,0.178867,1.08559,-1 [...]
+/*1282*/{0.293479,1.90192,-1.71386,0.336383,1.84951,-1.87256,0.248479,1.74513,-1.60423,0.223434,1.61838,-1.53281,0.345086,1.62568,-1.63858,0.426027,1.6232,-1.68535,0.232664,1.92765,-2.08383,0.331874,1.84972,-1.94372,0.125537,1.89858,-1.97447,-0.005787,1.81896,-2.09936,-0.039723,1.72731,-2.16155,0.089456,1.58133,-2.13864,0.115027,1.52196,-2.16483,0.236221,1.40794,-1.77259,0.050523,1.48615,-1.85011,0.05337,1.51563,-1.89448,0.052724,1.49178,-1.96628,0.236961,1.44136,-2.02591,0.18383,1.08357 [...]
+/*1283*/{0.293197,1.89795,-1.7133,0.337337,1.84507,-1.8724,0.246245,1.74088,-1.60476,0.216699,1.61475,-1.53468,0.341143,1.61887,-1.639,0.42214,1.61297,-1.68375,0.232281,1.92321,-2.08432,0.331764,1.84607,-1.94364,0.125208,1.89503,-1.97645,-0.005068,1.8113,-2.102,-0.037606,1.71909,-2.16322,0.093742,1.5739,-2.14022,0.119426,1.51401,-2.16594,0.236829,1.40545,-1.77337,0.051462,1.48216,-1.84957,0.051656,1.51152,-1.89523,0.052095,1.48704,-1.96589,0.236736,1.43837,-2.02689,0.18964,1.07984,-1.750 [...]
+/*1284*/{0.291399,1.89408,-1.71263,0.336931,1.84123,-1.87097,0.243111,1.73618,-1.6048,0.211342,1.61016,-1.53526,0.336286,1.61026,-1.63872,0.417894,1.6026,-1.68285,0.232132,1.91859,-2.08483,0.331931,1.84281,-1.94412,0.124854,1.88977,-1.97762,-0.003254,1.80299,-2.10414,-0.034289,1.7103,-2.16547,0.097728,1.56577,-2.14245,0.123543,1.50621,-2.1669,0.23794,1.40291,-1.77401,0.051977,1.47807,-1.84961,0.052332,1.50767,-1.89492,0.052017,1.48256,-1.96564,0.237549,1.43566,-2.02777,0.194088,1.07778,- [...]
+/*1285*/{0.290377,1.88941,-1.71207,0.338184,1.83736,-1.87041,0.238726,1.73149,-1.60497,0.204636,1.60711,-1.53702,0.331732,1.60234,-1.63883,0.413351,1.59198,-1.68139,0.232476,1.91381,-2.08578,0.332443,1.83849,-1.94354,0.123929,1.88502,-1.97821,-0.003602,1.79398,-2.10641,-0.03175,1.70114,-2.16843,0.101531,1.55741,-2.14483,0.128085,1.49819,-2.16803,0.238415,1.39994,-1.77429,0.051169,1.4731,-1.84908,0.052764,1.50356,-1.89509,0.051387,1.47733,-1.9649,0.237449,1.4324,-2.02863,0.199546,1.07373, [...]
+/*1286*/{0.290173,1.88529,-1.71072,0.337964,1.83234,-1.86952,0.236687,1.72605,-1.60452,0.198797,1.60227,-1.53711,0.327028,1.59507,-1.63829,0.408495,1.58166,-1.67974,0.232644,1.90881,-2.08687,0.333314,1.83448,-1.94343,0.123313,1.88035,-1.97951,-0.002903,1.78464,-2.10947,-0.028886,1.69146,-2.17023,0.105378,1.54902,-2.14726,0.133287,1.48998,-2.16936,0.239458,1.39692,-1.77526,0.052056,1.46789,-1.84931,0.052152,1.49858,-1.8959,0.050812,1.4725,-1.96432,0.23729,1.42934,-2.02993,0.201969,1.07268 [...]
+/*1287*/{0.288316,1.87961,-1.71056,0.338895,1.82737,-1.86872,0.233598,1.72125,-1.60466,0.191648,1.5983,-1.53854,0.321737,1.58659,-1.63824,0.403242,1.57068,-1.67836,0.233144,1.90345,-2.08791,0.334073,1.83044,-1.94337,0.122256,1.8742,-1.98113,-0.001901,1.77517,-2.11336,-0.025101,1.68187,-2.17306,0.110014,1.53977,-2.14974,0.138391,1.48117,-2.17002,0.239717,1.39354,-1.77656,0.052093,1.46327,-1.84906,0.051405,1.49372,-1.89583,0.049887,1.46746,-1.96339,0.237496,1.42634,-2.03078,0.205597,1.0685 [...]
+/*1288*/{0.286159,1.87532,-1.7094,0.33859,1.82309,-1.86826,0.229045,1.71625,-1.60568,0.185764,1.59467,-1.53955,0.314996,1.57855,-1.63773,0.396742,1.56033,-1.67644,0.23363,1.89755,-2.08924,0.333762,1.82575,-1.94359,0.121168,1.86929,-1.98295,0.000689,1.76493,-2.11629,-0.021695,1.67168,-2.17537,0.116198,1.53098,-2.15179,0.144821,1.47296,-2.17086,0.239695,1.38973,-1.77762,0.051128,1.45841,-1.84789,0.051364,1.48867,-1.89539,0.049416,1.46287,-1.96252,0.23737,1.42228,-2.03215,0.207614,1.06362,- [...]
+/*1289*/{0.285167,1.8708,-1.7084,0.339186,1.81706,-1.86702,0.224978,1.71131,-1.60646,0.177932,1.5912,-1.54075,0.309045,1.57106,-1.63725,0.391495,1.55019,-1.67502,0.234336,1.89203,-2.09074,0.335341,1.82075,-1.9431,0.121542,1.86313,-1.98491,0.002224,1.75493,-2.12047,-0.01807,1.6614,-2.17851,0.121131,1.52254,-2.15336,0.152763,1.46552,-2.17161,0.240541,1.38572,-1.77793,0.050707,1.4529,-1.84702,0.05149,1.48331,-1.8952,0.048116,1.45753,-1.9618,0.236733,1.41784,-2.03441,0.213216,1.06046,-1.7339 [...]
+/*1290*/{0.284126,1.86607,-1.70738,0.339498,1.81245,-1.86605,0.220399,1.7064,-1.60663,0.17104,1.58762,-1.5411,0.302362,1.56336,-1.63646,0.384189,1.53964,-1.67255,0.235121,1.88678,-2.09188,0.336472,1.81634,-1.94258,0.120575,1.85609,-1.9863,0.002658,1.7446,-2.12483,-0.013364,1.65059,-2.18109,0.127512,1.51391,-2.15512,0.151644,1.45652,-2.17271,0.241268,1.38118,-1.77894,0.050372,1.44854,-1.846,0.05021,1.47836,-1.89497,0.04648,1.45343,-1.96155,0.236127,1.41311,-2.03574,0.217427,1.05825,-1.729 [...]
+/*1291*/{0.282149,1.86139,-1.70666,0.34054,1.80679,-1.86501,0.216453,1.70267,-1.60729,0.164137,1.58416,-1.54226,0.295284,1.55575,-1.6354,0.377232,1.53026,-1.67069,0.236254,1.88158,-2.09413,0.336458,1.81094,-1.94283,0.120049,1.85076,-1.98772,0.00735,1.73328,-2.12834,-0.008669,1.63958,-2.18375,0.134584,1.50485,-2.15553,0.166206,1.4496,-2.1727,0.240729,1.37665,-1.78072,0.047689,1.44316,-1.84643,0.048271,1.47264,-1.89455,0.042873,1.44962,-1.96137,0.234418,1.40892,-2.0373,0.22144,1.05448,-1.7 [...]
+/*1292*/{0.281477,1.85742,-1.70528,0.341997,1.80101,-1.86498,0.212071,1.69882,-1.60827,0.156344,1.58101,-1.54319,0.288337,1.54921,-1.63386,0.369024,1.52144,-1.66852,0.237103,1.8762,-2.09576,0.338728,1.8062,-1.94176,0.121253,1.84762,-1.9882,0.009028,1.72359,-2.13481,-0.002937,1.62899,-2.18605,0.141767,1.49723,-2.15649,0.174431,1.4424,-2.17239,0.240546,1.37176,-1.78231,0.046961,1.43879,-1.84565,0.046149,1.46711,-1.89358,0.03997,1.44564,-1.9612,0.231489,1.40608,-2.0385,0.223821,1.05206,-1.7 [...]
+/*1293*/{0.280326,1.85361,-1.70445,0.342487,1.79605,-1.86326,0.206915,1.6956,-1.60802,0.147669,1.5794,-1.54472,0.281209,1.54313,-1.6333,0.361212,1.5132,-1.66647,0.238426,1.87188,-2.0981,0.339514,1.80134,-1.94151,0.122457,1.84552,-1.98828,0.013603,1.71263,-2.14022,0.003308,1.61757,-2.1885,0.150815,1.49164,-2.15691,0.183914,1.43558,-2.17265,0.239295,1.36702,-1.78393,0.044796,1.43453,-1.84478,0.043652,1.46288,-1.8931,0.036882,1.44252,-1.95982,0.228851,1.40341,-2.04026,0.226741,1.04835,-1.72 [...]
+/*1294*/{0.280284,1.84987,-1.70245,0.343389,1.79282,-1.8624,0.201242,1.69401,-1.60878,0.140048,1.57869,-1.54615,0.272728,1.53759,-1.63216,0.351988,1.50659,-1.66389,0.239163,1.86791,-2.10006,0.341142,1.79698,-1.94086,0.122482,1.84414,-1.9881,0.01828,1.7032,-2.14585,0.010759,1.60763,-2.19122,0.159796,1.48537,-2.15593,0.194551,1.42961,-2.17161,0.238641,1.36313,-1.78735,0.042379,1.43023,-1.84417,0.042317,1.46047,-1.8921,0.033819,1.44001,-1.95848,0.22651,1.40225,-2.04215,0.230151,1.04805,-1.7 [...]
+/*1295*/{0.279966,1.8477,-1.70143,0.344661,1.78926,-1.86173,0.197067,1.69272,-1.60935,0.13036,1.57908,-1.54783,0.26401,1.53349,-1.63169,0.343228,1.49944,-1.66188,0.241182,1.86431,-2.10082,0.341177,1.79311,-1.94014,0.123534,1.84285,-1.98776,0.022969,1.6938,-2.15081,0.018888,1.59743,-2.19369,0.169357,1.4802,-2.15578,0.20564,1.42536,-2.17059,0.236897,1.35932,-1.78889,0.040448,1.4277,-1.84273,0.039973,1.45979,-1.89152,0.033067,1.43751,-1.95549,0.224131,1.40138,-2.04397,0.236282,1.0498,-1.719 [...]
+/*1296*/{0.280537,1.84497,-1.70099,0.343431,1.78629,-1.86122,0.191832,1.69257,-1.60917,0.122067,1.58126,-1.54981,0.255265,1.53074,-1.63067,0.334045,1.49435,-1.65895,0.242339,1.86122,-2.1011,0.341265,1.79041,-1.9396,0.123209,1.84186,-1.98745,0.029661,1.68602,-2.15488,0.026815,1.58882,-2.19522,0.180245,1.47604,-2.15456,0.217207,1.42147,-2.1697,0.235463,1.35593,-1.7899,0.039408,1.42516,-1.84023,0.039315,1.45881,-1.89046,0.031559,1.43746,-1.95392,0.222429,1.40122,-2.04596,0.241845,1.05227,-1 [...]
+/*1297*/{0.280916,1.84154,-1.70023,0.344203,1.78469,-1.86011,0.187109,1.69258,-1.60929,0.113234,1.58395,-1.55158,0.246646,1.52855,-1.62984,0.323857,1.48914,-1.65637,0.243222,1.85954,-2.10036,0.341545,1.78857,-1.93921,0.123562,1.84097,-1.98756,0.034197,1.67887,-2.15685,0.036093,1.58176,-2.1964,0.191176,1.47306,-2.15322,0.229539,1.41835,-2.16762,0.234207,1.35332,-1.7917,0.038237,1.42396,-1.83838,0.038622,1.4585,-1.89074,0.030386,1.43724,-1.95092,0.220852,1.40077,-2.04767,0.248877,1.0545,-1 [...]
+/*1298*/{0.282644,1.83944,-1.70003,0.344578,1.78304,-1.86036,0.181683,1.69391,-1.60983,0.105551,1.58789,-1.55342,0.237176,1.52758,-1.62892,0.313553,1.4852,-1.65402,0.245269,1.85854,-2.09965,0.342108,1.78711,-1.93871,0.124147,1.84063,-1.98751,0.040507,1.67257,-2.15869,0.045727,1.57635,-2.19771,0.202819,1.4706,-2.1516,0.242028,1.41743,-2.16564,0.232431,1.35094,-1.79188,0.036823,1.42407,-1.83694,0.036611,1.45839,-1.89045,0.029455,1.43738,-1.94909,0.219173,1.40059,-2.04878,0.256193,1.05779,- [...]
+/*1299*/{0.28314,1.83722,-1.69973,0.344514,1.78144,-1.86007,0.175891,1.69584,-1.61163,0.096165,1.59261,-1.55593,0.227697,1.52839,-1.62847,0.304307,1.48373,-1.65152,0.24678,1.85745,-2.09879,0.341624,1.7856,-1.93802,0.124382,1.84057,-1.98769,0.04842,1.66837,-2.15895,0.054007,1.57076,-2.198,0.213989,1.46986,-2.15003,0.255231,1.41812,-2.16311,0.230659,1.34889,-1.7922,0.036118,1.42427,-1.83701,0.035621,1.45815,-1.89,0.028101,1.43793,-1.94737,0.217816,1.40046,-2.04979,0.265314,1.06157,-1.71878 [...]
+/*1300*/{0.283555,1.8365,-1.69932,0.342557,1.78055,-1.85912,0.171787,1.69784,-1.61098,0.087391,1.59947,-1.55895,0.219458,1.5278,-1.62717,0.293527,1.48223,-1.64917,0.24804,1.85733,-2.09796,0.342133,1.78441,-1.93746,0.125202,1.84033,-1.98808,0.054611,1.66514,-2.16016,0.063062,1.56687,-2.19898,0.225456,1.47042,-2.14736,0.267931,1.4208,-2.15991,0.230046,1.3479,-1.79208,0.036072,1.42499,-1.83682,0.035512,1.45862,-1.89026,0.028323,1.43845,-1.94725,0.217099,1.40083,-2.05065,0.272697,1.06519,-1. [...]
+/*1301*/{0.28271,1.83615,-1.69928,0.343291,1.78026,-1.85825,0.166654,1.70037,-1.61163,0.077987,1.6049,-1.56116,0.209973,1.52975,-1.62603,0.283433,1.48163,-1.64711,0.24871,1.85667,-2.09696,0.342096,1.78413,-1.93638,0.125539,1.84034,-1.98904,0.059513,1.66218,-2.16006,0.072022,1.56362,-2.19947,0.236898,1.47229,-2.14501,0.28144,1.42429,-2.1562,0.22912,1.34742,-1.79157,0.03557,1.42557,-1.83672,0.034675,1.45859,-1.89128,0.028086,1.4393,-1.94772,0.217422,1.40067,-2.05156,0.281587,1.07096,-1.717 [...]
+/*1302*/{0.28152,1.83592,-1.69909,0.342804,1.78041,-1.85814,0.161715,1.70425,-1.61239,0.069449,1.61095,-1.56247,0.201156,1.53247,-1.62524,0.273973,1.4821,-1.64471,0.251117,1.85751,-2.09679,0.342012,1.78425,-1.93607,0.126588,1.84106,-1.99032,0.064741,1.65981,-2.16049,0.080915,1.56116,-2.20024,0.247868,1.4747,-2.14187,0.29387,1.42951,-2.15263,0.227217,1.34829,-1.78995,0.03496,1.42623,-1.83698,0.03442,1.45951,-1.89152,0.028243,1.4399,-1.94865,0.216325,1.40013,-2.05221,0.28858,1.07632,-1.718 [...]
+/*1303*/{0.279985,1.83694,-1.69831,0.341474,1.78019,-1.85722,0.156165,1.70838,-1.61347,0.061358,1.61723,-1.56436,0.192591,1.5352,-1.62385,0.263636,1.48357,-1.64264,0.253057,1.85935,-2.09669,0.342202,1.78502,-1.93479,0.126752,1.84101,-1.99094,0.070003,1.65734,-2.16078,0.089841,1.5598,-2.2003,0.25876,1.4795,-2.13888,0.306574,1.43594,-2.148,0.226816,1.34946,-1.78968,0.034528,1.42717,-1.83739,0.033784,1.45982,-1.89183,0.027646,1.44068,-1.9501,0.21652,1.39988,-2.05243,0.297831,1.08215,-1.7175 [...]
+/*1304*/{0.278327,1.83862,-1.69751,0.341232,1.78124,-1.85557,0.149974,1.71388,-1.61396,0.053686,1.62389,-1.56527,0.184364,1.53899,-1.62246,0.253696,1.48631,-1.64094,0.255303,1.86158,-2.0965,0.342251,1.78641,-1.93385,0.127146,1.84119,-1.99174,0.074789,1.65571,-2.16078,0.097633,1.55878,-2.20018,0.269486,1.48527,-2.13552,0.319362,1.4439,-2.14396,0.226793,1.35171,-1.79001,0.033647,1.42805,-1.83944,0.0333,1.46042,-1.89233,0.026937,1.44066,-1.95158,0.215771,1.39961,-2.05367,0.304115,1.08956,-1 [...]
+/*1305*/{0.275845,1.84055,-1.6968,0.339972,1.78233,-1.85448,0.144308,1.71875,-1.61409,0.046514,1.63073,-1.56728,0.174828,1.5434,-1.62154,0.244257,1.48927,-1.63893,0.257014,1.86462,-2.09522,0.342506,1.78763,-1.93283,0.127506,1.84149,-1.99264,0.079905,1.65482,-2.16037,0.106306,1.55856,-2.2006,0.279497,1.49203,-2.1325,0.330947,1.4525,-2.13944,0.225839,1.35439,-1.7892,0.033353,1.42923,-1.84086,0.033757,1.46147,-1.89347,0.027279,1.44131,-1.9528,0.215852,1.39929,-2.05418,0.313365,1.0965,-1.717 [...]
+/*1306*/{0.272634,1.84288,-1.69612,0.338911,1.78417,-1.85251,0.1387,1.72416,-1.6144,0.037902,1.63757,-1.56868,0.167043,1.54881,-1.6212,0.235045,1.49307,-1.63741,0.257167,1.86764,-2.09436,0.34303,1.78976,-1.93149,0.127729,1.84196,-1.99346,0.084614,1.65456,-2.15987,0.114175,1.55901,-2.20044,0.28933,1.50002,-2.12914,0.34205,1.46261,-2.13466,0.225675,1.35711,-1.78879,0.032888,1.43074,-1.84219,0.034265,1.46212,-1.89541,0.027653,1.44221,-1.95528,0.216282,1.39949,-2.05423,0.318379,1.10527,-1.71 [...]
+/*1307*/{0.269785,1.84651,-1.69574,0.338861,1.78728,-1.85157,0.13322,1.72948,-1.61496,0.030445,1.6439,-1.57017,0.158575,1.55498,-1.62133,0.226333,1.49727,-1.63704,0.258243,1.87084,-2.09384,0.342982,1.79248,-1.92964,0.127596,1.84364,-1.9942,0.090083,1.65388,-2.15949,0.122885,1.55978,-2.20072,0.298297,1.50765,-2.12553,0.352798,1.47291,-2.1299,0.224981,1.35961,-1.78785,0.033245,1.43168,-1.84443,0.035259,1.46383,-1.89581,0.028308,1.4436,-1.95681,0.215573,1.40023,-2.05554,0.324167,1.11321,-1. [...]
+/*1308*/{0.266156,1.84894,-1.69434,0.337771,1.7908,-1.84935,0.12752,1.73548,-1.61541,0.023497,1.65136,-1.57187,0.151567,1.56092,-1.62117,0.217216,1.50345,-1.63581,0.259466,1.87499,-2.09323,0.343208,1.79554,-1.92789,0.128095,1.8448,-1.99507,0.093775,1.65425,-2.15933,0.130663,1.56173,-2.20103,0.30621,1.51559,-2.12128,0.362247,1.4844,-2.1252,0.224978,1.36271,-1.78725,0.034284,1.43323,-1.84665,0.035224,1.46629,-1.89776,0.029172,1.44545,-1.95854,0.216403,1.4004,-2.05629,0.333686,1.12189,-1.71 [...]
+/*1309*/{0.26274,1.85273,-1.69379,0.33668,1.79366,-1.84778,0.122748,1.74167,-1.61545,0.016209,1.65823,-1.57348,0.142864,1.56851,-1.62306,0.208434,1.50893,-1.63534,0.260195,1.8798,-2.09247,0.342994,1.79924,-1.92591,0.128411,1.84704,-1.99548,0.098713,1.655,-2.15905,0.137892,1.56343,-2.20104,0.314443,1.52528,-2.11786,0.371751,1.49625,-2.11993,0.225876,1.36596,-1.78625,0.034292,1.43613,-1.84863,0.035548,1.46806,-1.89951,0.030308,1.44759,-1.95992,0.218393,1.40084,-2.05554,0.335659,1.12973,-1. [...]
+/*1310*/{0.25949,1.8571,-1.69353,0.33683,1.79836,-1.84598,0.117357,1.74755,-1.61636,0.009787,1.66601,-1.57479,0.135991,1.57546,-1.62273,0.200944,1.51546,-1.63459,0.260722,1.88376,-2.09146,0.343155,1.80318,-1.92459,0.12892,1.84887,-1.99677,0.103265,1.6561,-2.15802,0.145819,1.56657,-2.20136,0.321317,1.53483,-2.11429,0.380252,1.50881,-2.11509,0.225516,1.36979,-1.78668,0.034475,1.43893,-1.85004,0.03818,1.47065,-1.90064,0.032089,1.45006,-1.96229,0.220518,1.40071,-2.05566,0.340246,1.1374,-1.72 [...]
+/*1311*/{0.255362,1.86199,-1.69328,0.336459,1.8025,-1.84378,0.112194,1.75338,-1.61732,0.003916,1.67366,-1.5768,0.129099,1.58143,-1.62273,0.192357,1.5218,-1.63413,0.262313,1.88852,-2.09062,0.343483,1.80719,-1.92237,0.129362,1.85202,-1.9978,0.109102,1.65832,-2.15817,0.153591,1.56973,-2.20216,0.329209,1.54581,-2.1104,0.387921,1.52125,-2.11017,0.226426,1.37379,-1.78593,0.035212,1.4421,-1.85072,0.03856,1.47331,-1.90315,0.034233,1.45273,-1.96361,0.222422,1.40179,-2.05424,0.345691,1.14486,-1.72 [...]
+/*1312*/{0.251676,1.86653,-1.69325,0.336061,1.80751,-1.84246,0.107938,1.75978,-1.61779,-0.003537,1.6813,-1.57831,0.122333,1.58853,-1.62361,0.185845,1.52809,-1.6343,0.263651,1.89326,-2.08939,0.344549,1.81175,-1.92133,0.130015,1.85491,-1.99891,0.113555,1.66031,-2.15969,0.160386,1.57331,-2.20331,0.335435,1.55625,-2.10625,0.395241,1.53407,-2.10492,0.228918,1.37771,-1.78626,0.037213,1.44523,-1.85303,0.040233,1.47651,-1.90376,0.035506,1.45621,-1.96532,0.224237,1.40231,-2.05425,0.348639,1.15344 [...]
+/*1313*/{0.247445,1.87138,-1.69339,0.334606,1.8116,-1.84052,0.103168,1.76612,-1.61838,-0.009157,1.6893,-1.58106,0.116534,1.59595,-1.62438,0.179628,1.53435,-1.63387,0.265859,1.89857,-2.08838,0.344706,1.8166,-1.91923,0.131328,1.85895,-2.00035,0.11972,1.66343,-2.16097,0.167751,1.577,-2.20351,0.340799,1.56685,-2.10268,0.400888,1.54733,-2.10036,0.22885,1.38141,-1.78645,0.038425,1.44975,-1.85446,0.041583,1.47928,-1.90582,0.037543,1.45921,-1.96686,0.226091,1.40291,-2.05459,0.353047,1.16057,-1.7 [...]
+/*1314*/{0.244286,1.87622,-1.69356,0.333461,1.81694,-1.83867,0.09925,1.77197,-1.61939,-0.015629,1.6976,-1.58245,0.11005,1.60357,-1.6257,0.172631,1.54176,-1.63466,0.267625,1.90328,-2.08727,0.345288,1.82065,-1.9168,0.131626,1.86204,-2.0017,0.12324,1.66823,-2.16403,0.175045,1.58156,-2.20457,0.344652,1.57599,-2.09906,0.406649,1.55908,-2.09487,0.231847,1.38622,-1.78689,0.039381,1.45385,-1.85555,0.044327,1.48281,-1.90742,0.04022,1.46277,-1.968,0.228072,1.40314,-2.05439,0.357893,1.16881,-1.7207 [...]
+/*1315*/{0.241482,1.8811,-1.69354,0.334054,1.82186,-1.83754,0.094995,1.778,-1.62066,-0.019475,1.70582,-1.58521,0.104636,1.61117,-1.62697,0.166609,1.5488,-1.6351,0.270261,1.90867,-2.0862,0.345025,1.82514,-1.91511,0.13286,1.86611,-2.00287,0.129616,1.67261,-2.16675,0.182101,1.58617,-2.20565,0.349607,1.58779,-2.09557,0.411133,1.57173,-2.09076,0.232707,1.39051,-1.78725,0.041654,1.45828,-1.85753,0.045683,1.48648,-1.90944,0.043229,1.46647,-1.96989,0.229902,1.40407,-2.05491,0.360807,1.17517,-1.7 [...]
+/*1316*/{0.238211,1.88582,-1.69439,0.332764,1.82688,-1.8358,0.090866,1.78413,-1.62182,-0.025158,1.71389,-1.5877,0.098672,1.61794,-1.62805,0.161072,1.55612,-1.63501,0.271994,1.91388,-2.08463,0.34551,1.83002,-1.9134,0.133718,1.8698,-2.00397,0.133428,1.67891,-2.16921,0.188934,1.59103,-2.20684,0.352526,1.59709,-2.09229,0.415588,1.58315,-2.08512,0.234907,1.39447,-1.78858,0.043418,1.46328,-1.85724,0.047006,1.49027,-1.91101,0.046474,1.46964,-1.96992,0.232574,1.4048,-2.05501,0.364165,1.1833,-1.7 [...]
+/*1317*/{0.235984,1.89052,-1.69449,0.331908,1.83112,-1.83457,0.087764,1.79072,-1.62316,-0.031052,1.72174,-1.59059,0.093717,1.62535,-1.63004,0.155161,1.5622,-1.63496,0.273525,1.91885,-2.08381,0.345559,1.83461,-1.91134,0.135124,1.87345,-2.00588,0.140025,1.68336,-2.17256,0.195764,1.59619,-2.20777,0.356588,1.60807,-2.08909,0.419029,1.59514,-2.08118,0.236549,1.39852,-1.78894,0.045098,1.46744,-1.85788,0.050042,1.4938,-1.91249,0.04943,1.47385,-1.97121,0.23456,1.40602,-2.05496,0.367133,1.19036,- [...]
+/*1318*/{0.233739,1.89539,-1.69457,0.332398,1.835,-1.83267,0.084408,1.7969,-1.62527,-0.035367,1.7296,-1.59386,0.088141,1.63214,-1.63101,0.150174,1.56909,-1.63591,0.276053,1.92424,-2.08268,0.345457,1.84049,-1.91047,0.135917,1.87713,-2.00655,0.145631,1.68872,-2.1747,0.203123,1.60131,-2.20889,0.359019,1.617,-2.08595,0.421847,1.60616,-2.07757,0.239098,1.40243,-1.78922,0.047568,1.47214,-1.85858,0.052165,1.49794,-1.91454,0.052063,1.4769,-1.97179,0.236178,1.40662,-2.05463,0.369701,1.19735,-1.72 [...]
+/*1319*/{0.232178,1.89962,-1.69507,0.330269,1.84138,-1.83183,0.080774,1.80378,-1.62707,-0.039422,1.73753,-1.59694,0.084149,1.63942,-1.63273,0.14597,1.57592,-1.63569,0.277367,1.9287,-2.08104,0.345232,1.84413,-1.90826,0.13723,1.88068,-2.00788,0.15185,1.69345,-2.17858,0.209784,1.60703,-2.21041,0.361299,1.62608,-2.08324,0.423455,1.61796,-2.07544,0.241221,1.40667,-1.79046,0.049811,1.47771,-1.85844,0.05439,1.50188,-1.91552,0.055342,1.48092,-1.97222,0.239067,1.40719,-2.0547,0.371446,1.20359,-1. [...]
+/*1320*/{0.230672,1.90397,-1.69547,0.330033,1.84566,-1.83095,0.078201,1.80987,-1.62912,-0.042829,1.74555,-1.60089,0.079999,1.64644,-1.63398,0.140886,1.58244,-1.63629,0.279709,1.93396,-2.08036,0.345652,1.84905,-1.90742,0.1384,1.88434,-2.0096,0.157138,1.69826,-2.18173,0.215226,1.61204,-2.21152,0.362625,1.63549,-2.0809,0.426382,1.6283,-2.07053,0.242719,1.41114,-1.78981,0.052714,1.48194,-1.85832,0.056759,1.5059,-1.91762,0.057861,1.48478,-1.97272,0.241073,1.40882,-2.05397,0.374156,1.20984,-1. [...]
+/*1321*/{0.229319,1.90804,-1.69579,0.330206,1.84945,-1.82984,0.075091,1.81624,-1.63087,-0.047929,1.75335,-1.60475,0.075296,1.65237,-1.63576,0.136726,1.58871,-1.63626,0.281103,1.93822,-2.07945,0.345726,1.85325,-1.90597,0.140678,1.88704,-2.01001,0.162321,1.70312,-2.18489,0.221669,1.6171,-2.21315,0.364535,1.64447,-2.07883,0.426931,1.63926,-2.0684,0.244484,1.41539,-1.79076,0.054623,1.48814,-1.85824,0.058377,1.50986,-1.91875,0.061196,1.48764,-1.97276,0.243686,1.40976,-2.05357,0.376452,1.21573 [...]
+/*1322*/{0.227651,1.91213,-1.69614,0.329571,1.85458,-1.8294,0.071957,1.82251,-1.63319,-0.051305,1.76061,-1.60823,0.072508,1.65884,-1.63704,0.133205,1.59392,-1.63711,0.282216,1.94233,-2.07788,0.345921,1.85802,-1.90474,0.14188,1.89082,-2.01093,0.168136,1.70768,-2.18817,0.226585,1.62228,-2.2151,0.365684,1.65192,-2.0769,0.42904,1.64798,-2.06489,0.246517,1.4199,-1.79107,0.057935,1.49227,-1.85846,0.06108,1.51413,-1.91927,0.063662,1.49164,-1.97336,0.245924,1.41071,-2.05297,0.37778,1.22079,-1.72 [...]
+/*1323*/{0.226977,1.91594,-1.69645,0.329594,1.85731,-1.82826,0.070318,1.82867,-1.63531,-0.05449,1.76754,-1.61195,0.068383,1.66457,-1.63835,0.129928,1.59991,-1.63688,0.283963,1.94659,-2.0772,0.346348,1.86138,-1.90416,0.143143,1.89384,-2.01171,0.172968,1.71197,-2.19059,0.2319,1.62754,-2.2166,0.366182,1.65898,-2.07596,0.429757,1.6567,-2.06206,0.247796,1.42464,-1.79174,0.060321,1.49742,-1.85812,0.064047,1.51743,-1.92061,0.066706,1.49454,-1.97314,0.248165,1.41197,-2.05217,0.380487,1.22542,-1. [...]
+/*1324*/{0.226421,1.91978,-1.69686,0.329567,1.86057,-1.82732,0.06853,1.83463,-1.63793,-0.05736,1.77461,-1.61644,0.065027,1.66999,-1.63965,0.125491,1.60469,-1.63801,0.285227,1.95044,-2.07633,0.347425,1.86521,-1.90324,0.144544,1.89731,-2.0134,0.178429,1.71696,-2.19411,0.23646,1.63198,-2.2182,0.366888,1.66652,-2.07431,0.430248,1.66544,-2.05992,0.249957,1.42895,-1.79119,0.063326,1.50169,-1.85768,0.06659,1.52188,-1.92096,0.069315,1.49809,-1.97347,0.250461,1.41348,-2.05169,0.38181,1.22975,-1.7 [...]
+/*1325*/{0.225625,1.92315,-1.69761,0.330107,1.86523,-1.82682,0.066506,1.84086,-1.64039,-0.06094,1.77996,-1.6199,0.062246,1.67535,-1.6411,0.12268,1.60978,-1.63761,0.286676,1.95406,-2.07559,0.347309,1.86824,-1.902,0.145731,1.90064,-2.01439,0.18303,1.72075,-2.1963,0.240868,1.63651,-2.22012,0.366671,1.6746,-2.07366,0.431046,1.67285,-2.05836,0.251131,1.43272,-1.79036,0.066125,1.50601,-1.85831,0.069798,1.52537,-1.92187,0.071638,1.5013,-1.97333,0.252563,1.41485,-2.05064,0.382031,1.2329,-1.72662 [...]
+/*1326*/{0.225224,1.927,-1.69767,0.32949,1.86767,-1.82624,0.06474,1.84667,-1.64274,-0.06353,1.78616,-1.624,0.059379,1.68011,-1.64267,0.120481,1.61414,-1.63825,0.288763,1.95701,-2.07473,0.348262,1.87139,-1.90078,0.14708,1.90291,-2.0146,0.187045,1.72516,-2.19903,0.245147,1.64096,-2.22184,0.367285,1.68009,-2.07309,0.430764,1.67935,-2.05602,0.252408,1.43613,-1.79026,0.067619,1.51012,-1.85799,0.070414,1.52885,-1.92225,0.074199,1.50433,-1.9737,0.254624,1.41632,-2.04946,0.382979,1.2353,-1.72795 [...]
+/*1327*/{0.224852,1.92983,-1.69803,0.330721,1.87173,-1.82543,0.063547,1.85126,-1.64461,-0.065423,1.79168,-1.62833,0.056985,1.68469,-1.64421,0.117636,1.61788,-1.63819,0.289874,1.96046,-2.07335,0.348516,1.87436,-1.90035,0.148424,1.9058,-2.01506,0.191494,1.72962,-2.20101,0.248596,1.64509,-2.22382,0.367249,1.68456,-2.07221,0.429761,1.6869,-2.0559,0.253204,1.43948,-1.78982,0.067978,1.51387,-1.85902,0.072405,1.53209,-1.92249,0.076014,1.50763,-1.97361,0.25674,1.4178,-2.04887,0.384631,1.23772,-1 [...]
+/*1328*/{0.224958,1.93277,-1.69876,0.33021,1.87389,-1.82502,0.062,1.85618,-1.64676,-0.066753,1.79689,-1.63192,0.05464,1.68921,-1.64573,0.115895,1.62166,-1.63773,0.291344,1.96329,-2.07292,0.349716,1.87705,-1.89935,0.150348,1.90884,-2.01637,0.19596,1.7327,-2.203,0.25184,1.64869,-2.22536,0.368864,1.69106,-2.07297,0.432562,1.68995,-2.05358,0.255645,1.44382,-1.79006,0.070668,1.51777,-1.85827,0.074968,1.53507,-1.92283,0.077744,1.51065,-1.97379,0.258155,1.41992,-2.04779,0.385056,1.23875,-1.7298 [...]
+/*1329*/{0.224584,1.93602,-1.69892,0.331287,1.87641,-1.824,0.061629,1.85966,-1.64998,-0.069955,1.80089,-1.63557,0.052708,1.69278,-1.64703,0.113524,1.62559,-1.63788,0.29271,1.9658,-2.07263,0.349628,1.8796,-1.89886,0.152068,1.91051,-2.01696,0.199515,1.7369,-2.20448,0.253235,1.65221,-2.22718,0.368397,1.69315,-2.07193,0.42997,1.69571,-2.05303,0.25597,1.44689,-1.78992,0.072188,1.52098,-1.85823,0.076189,1.53841,-1.92287,0.080573,1.51346,-1.97339,0.258978,1.42169,-2.04708,0.38488,1.23926,-1.731 [...]
+/*1330*/{0.2252,1.939,-1.6991,0.331418,1.87887,-1.82326,0.060567,1.86405,-1.65278,-0.070886,1.80501,-1.63972,0.051964,1.69682,-1.64828,0.111626,1.62876,-1.63852,0.294568,1.96839,-2.07223,0.351172,1.88254,-1.89758,0.153172,1.91332,-2.01761,0.202534,1.74004,-2.20553,0.255106,1.65477,-2.22816,0.368653,1.69663,-2.07219,0.430158,1.69893,-2.05223,0.25566,1.44973,-1.78969,0.072821,1.52339,-1.85857,0.077394,1.54178,-1.92286,0.081827,1.51642,-1.97383,0.260303,1.42435,-2.04636,0.382272,1.23939,-1. [...]
+/*1331*/{0.225863,1.94177,-1.69964,0.331771,1.88052,-1.8227,0.059982,1.86778,-1.6547,-0.072966,1.80891,-1.64257,0.051051,1.69997,-1.64934,0.109964,1.63126,-1.63886,0.29506,1.97043,-2.07101,0.351119,1.88404,-1.89746,0.154877,1.91515,-2.01764,0.20314,1.74327,-2.20686,0.257677,1.65769,-2.23039,0.368734,1.69872,-2.07199,0.429638,1.70118,-2.05106,0.259252,1.45524,-1.78946,0.074847,1.52663,-1.85809,0.078626,1.54443,-1.92261,0.082857,1.51959,-1.97411,0.261055,1.42652,-2.04584,0.382329,1.23801,- [...]
+/*1332*/{0.226014,1.94379,-1.70002,0.333321,1.88266,-1.8228,0.060126,1.87092,-1.65656,-0.073812,1.81193,-1.64646,0.048972,1.70217,-1.65105,0.108712,1.63336,-1.639,0.296669,1.97228,-2.07048,0.352148,1.8858,-1.89693,0.156524,1.91784,-2.01821,0.205816,1.7465,-2.20701,0.259383,1.6601,-2.23159,0.368803,1.70202,-2.07255,0.429943,1.70232,-2.05069,0.259386,1.45466,-1.78979,0.075148,1.52798,-1.85838,0.079156,1.54698,-1.92268,0.084625,1.52161,-1.97393,0.261942,1.42873,-2.04554,0.380087,1.23573,-1. [...]
+/*1333*/{0.226593,1.94533,-1.70047,0.333629,1.88443,-1.82247,0.060413,1.87352,-1.65846,-0.073547,1.81489,-1.64912,0.048533,1.7044,-1.65248,0.107486,1.63507,-1.63864,0.298401,1.97395,-2.06994,0.353156,1.8877,-1.89658,0.157052,1.91903,-2.01816,0.205931,1.74866,-2.20752,0.2604,1.66177,-2.23259,0.369738,1.70375,-2.07284,0.429628,1.70306,-2.04986,0.258036,1.45562,-1.78978,0.076068,1.53026,-1.85783,0.080222,1.54909,-1.92249,0.085164,1.52423,-1.97385,0.262892,1.43125,-2.04481,0.377916,1.23187,- [...]
+/*1334*/{0.227348,1.94796,-1.70048,0.335262,1.88592,-1.82241,0.060641,1.8749,-1.66069,-0.073449,1.81689,-1.65197,0.047946,1.70574,-1.65336,0.107304,1.63624,-1.63844,0.299571,1.97516,-2.06979,0.353675,1.8892,-1.89622,0.158536,1.92059,-2.01847,0.20813,1.75078,-2.20845,0.261493,1.66329,-2.23375,0.369748,1.70409,-2.07316,0.42948,1.70293,-2.04968,0.258696,1.45781,-1.79002,0.077301,1.53201,-1.8582,0.080417,1.55145,-1.92155,0.085977,1.52611,-1.9732,0.26368,1.43314,-2.04435,0.375938,1.22797,-1.7 [...]
+/*1335*/{0.228075,1.94937,-1.70077,0.335212,1.88659,-1.82205,0.061151,1.87733,-1.66186,-0.073513,1.81799,-1.65594,0.048167,1.70717,-1.65441,0.107017,1.63737,-1.63871,0.301119,1.97589,-2.06974,0.355131,1.89031,-1.89606,0.159462,1.9226,-2.01839,0.20843,1.75371,-2.20849,0.261621,1.66449,-2.23482,0.369386,1.70365,-2.0732,0.42938,1.70121,-2.04959,0.259204,1.45927,-1.7904,0.077221,1.53266,-1.85748,0.080537,1.55331,-1.92148,0.085933,1.52775,-1.97302,0.26418,1.43574,-2.04408,0.373156,1.22398,-1. [...]
+/*1336*/{0.228806,1.95085,-1.70086,0.336338,1.88821,-1.82206,0.06182,1.87818,-1.66279,-0.072706,1.81883,-1.6581,0.048379,1.70776,-1.65573,0.106931,1.63775,-1.63863,0.301977,1.97638,-2.06943,0.355689,1.89192,-1.89589,0.160382,1.92362,-2.01836,0.208069,1.75422,-2.20819,0.26091,1.66477,-2.23552,0.3696,1.70202,-2.07367,0.429187,1.69908,-2.04954,0.260174,1.46033,-1.79008,0.077134,1.5331,-1.85692,0.080354,1.55383,-1.92074,0.086307,1.52885,-1.97234,0.264325,1.43782,-2.04395,0.370308,1.21824,-1. [...]
+/*1337*/{0.230405,1.95151,-1.70073,0.337577,1.88743,-1.8215,0.062615,1.87909,-1.66497,-0.072135,1.81887,-1.66063,0.048811,1.70771,-1.65673,0.106971,1.63742,-1.6384,0.302626,1.97625,-2.06918,0.355999,1.89243,-1.89597,0.161529,1.92448,-2.01822,0.20647,1.75582,-2.20761,0.259267,1.66511,-2.23593,0.370355,1.69895,-2.07359,0.428926,1.69594,-2.05013,0.263216,1.46307,-1.79024,0.076969,1.53379,-1.85696,0.080093,1.55482,-1.91963,0.0863,1.53002,-1.97206,0.265501,1.43962,-2.04356,0.366795,1.21249,-1 [...]
+/*1338*/{0.231752,1.95259,-1.70101,0.338094,1.88768,-1.8218,0.06292,1.87926,-1.66594,-0.071161,1.81873,-1.66276,0.050707,1.70781,-1.65724,0.107031,1.63635,-1.63814,0.303397,1.97576,-2.06876,0.356501,1.89252,-1.89598,0.162228,1.92539,-2.01793,0.205152,1.75618,-2.20658,0.257546,1.66495,-2.23598,0.369243,1.69761,-2.07413,0.429081,1.69313,-2.05026,0.262643,1.46219,-1.79033,0.077395,1.53322,-1.85568,0.080532,1.55514,-1.91911,0.086151,1.53072,-1.97126,0.265353,1.44119,-2.04342,0.364034,1.20737 [...]
+/*1339*/{0.23321,1.95267,-1.70092,0.338406,1.88789,-1.82192,0.064823,1.87879,-1.66596,-0.069843,1.81822,-1.66523,0.050997,1.70633,-1.65765,0.107492,1.63453,-1.63716,0.304287,1.97483,-2.06865,0.35683,1.89229,-1.89613,0.162304,1.92565,-2.01775,0.203034,1.75784,-2.206,0.256007,1.66507,-2.23572,0.370374,1.69368,-2.0748,0.428665,1.68853,-2.05092,0.262885,1.46196,-1.79065,0.076545,1.53261,-1.85525,0.079545,1.5549,-1.91839,0.085842,1.53052,-1.97031,0.265971,1.44242,-2.04308,0.359419,1.20098,-1. [...]
+/*1340*/{0.234723,1.95213,-1.70081,0.339101,1.88934,-1.82229,0.065577,1.87751,-1.66702,-0.067308,1.81742,-1.66573,0.051668,1.7051,-1.65792,0.108883,1.6336,-1.63632,0.30549,1.97355,-2.06816,0.357525,1.89217,-1.89648,0.162339,1.92631,-2.01758,0.199882,1.75764,-2.20483,0.252939,1.66434,-2.23537,0.369938,1.6904,-2.07582,0.427525,1.68357,-2.05138,0.262889,1.46171,-1.79107,0.076102,1.53126,-1.85479,0.079398,1.55414,-1.91788,0.086319,1.52969,-1.97003,0.266223,1.44297,-2.04319,0.356992,1.1948,-1 [...]
+/*1341*/{0.23579,1.95119,-1.70078,0.339689,1.88857,-1.82233,0.066835,1.87657,-1.66755,-0.065301,1.81517,-1.66721,0.053676,1.7033,-1.65775,0.110427,1.63165,-1.63567,0.305798,1.9724,-2.06807,0.357269,1.89189,-1.89646,0.162842,1.926,-2.01654,0.196965,1.75747,-2.20338,0.250917,1.66397,-2.23504,0.369395,1.6862,-2.07615,0.427479,1.67851,-2.05217,0.262463,1.46047,-1.79138,0.077127,1.52913,-1.85434,0.079211,1.55339,-1.91777,0.085198,1.52801,-1.96905,0.266925,1.44379,-2.0433,0.353927,1.18805,-1.7 [...]
+/*1342*/{0.237633,1.95062,-1.70044,0.340856,1.88785,-1.82274,0.06876,1.87451,-1.66756,-0.063895,1.81234,-1.66784,0.055793,1.70124,-1.65787,0.112302,1.62907,-1.63446,0.30624,1.97056,-2.06809,0.357458,1.89026,-1.89636,0.16337,1.92541,-2.01538,0.193979,1.75734,-2.20199,0.24713,1.66297,-2.23463,0.368393,1.68161,-2.0768,0.426206,1.67273,-2.05362,0.267354,1.4618,-1.79101,0.076587,1.52784,-1.85326,0.079332,1.55185,-1.91699,0.085486,1.52692,-1.9686,0.2673,1.44389,-2.0432,0.348887,1.18221,-1.7309 [...]
+/*1343*/{0.238792,1.94954,-1.70023,0.340628,1.88716,-1.82287,0.069193,1.87289,-1.66759,-0.060805,1.81065,-1.66831,0.057632,1.69736,-1.65693,0.114536,1.62563,-1.63363,0.306016,1.96916,-2.06806,0.35798,1.88913,-1.89673,0.163239,1.9243,-2.01494,0.190917,1.75664,-2.20083,0.243825,1.66211,-2.23344,0.367923,1.67642,-2.07733,0.425619,1.66618,-2.05484,0.26727,1.45896,-1.79338,0.076724,1.52515,-1.85292,0.078794,1.55007,-1.916,0.085202,1.52486,-1.96807,0.267808,1.44382,-2.04307,0.346149,1.17592,-1 [...]
+/*1344*/{0.240457,1.94791,-1.69967,0.34123,1.88534,-1.82341,0.070919,1.86987,-1.66714,-0.058377,1.80583,-1.66787,0.059517,1.69398,-1.65608,0.116752,1.62256,-1.63258,0.305254,1.96625,-2.06816,0.358062,1.8876,-1.89681,0.163269,1.92391,-2.01488,0.187253,1.75583,-2.19897,0.239626,1.66038,-2.23212,0.366506,1.67096,-2.07851,0.424756,1.65923,-2.05636,0.265411,1.45727,-1.79093,0.075947,1.52239,-1.85179,0.078286,1.54752,-1.91645,0.084369,1.52285,-1.96787,0.268487,1.44292,-2.04295,0.34334,1.16997, [...]
+/*1345*/{0.241586,1.94577,-1.69986,0.341611,1.88402,-1.82326,0.072508,1.86615,-1.66578,-0.05644,1.80033,-1.66701,0.062533,1.69037,-1.65485,0.119435,1.61841,-1.63083,0.305295,1.96409,-2.06824,0.35847,1.88565,-1.89701,0.162718,1.92162,-2.01406,0.183056,1.75419,-2.1979,0.237057,1.65855,-2.23214,0.365587,1.66488,-2.07976,0.422,1.65124,-2.05799,0.264637,1.45457,-1.79099,0.076269,1.51851,-1.85164,0.077952,1.54481,-1.91571,0.084568,1.51983,-1.96719,0.268753,1.44167,-2.04308,0.340866,1.16557,-1. [...]
+/*1346*/{0.24251,1.94304,-1.69922,0.341902,1.88147,-1.8237,0.073873,1.86232,-1.66532,-0.054245,1.79553,-1.66561,0.065008,1.68585,-1.65406,0.122553,1.61484,-1.62908,0.305732,1.96112,-2.06863,0.358397,1.88304,-1.89711,0.162915,1.92022,-2.01365,0.179453,1.7525,-2.19577,0.232545,1.65649,-2.23089,0.363831,1.66009,-2.08121,0.421145,1.64346,-2.05995,0.264805,1.45155,-1.79117,0.075528,1.51626,-1.85128,0.078242,1.54183,-1.91515,0.08401,1.51621,-1.96687,0.269139,1.44058,-2.04275,0.33802,1.16151,-1 [...]
+/*1347*/{0.243787,1.94035,-1.69907,0.342611,1.87713,-1.82421,0.075277,1.85742,-1.6633,-0.051669,1.7904,-1.66363,0.067856,1.68103,-1.65181,0.125199,1.61062,-1.62793,0.304144,1.95802,-2.06855,0.35774,1.88005,-1.89781,0.162487,1.9178,-2.01276,0.174682,1.74974,-2.19447,0.22779,1.65392,-2.23027,0.360938,1.65205,-2.08201,0.419209,1.63447,-2.06215,0.263973,1.44831,-1.79088,0.07542,1.51241,-1.85122,0.077432,1.53785,-1.91507,0.083394,1.51297,-1.96625,0.269792,1.43804,-2.0429,0.334978,1.1593,-1.72 [...]
+/*1348*/{0.244945,1.93675,-1.69869,0.34358,1.87415,-1.82434,0.076725,1.85285,-1.66224,-0.048966,1.78463,-1.66227,0.070474,1.67506,-1.65068,0.129237,1.60602,-1.62584,0.303033,1.95437,-2.06938,0.358427,1.8774,-1.89798,0.161819,1.91463,-2.01193,0.173887,1.74768,-2.19384,0.224106,1.65061,-2.2291,0.359662,1.64486,-2.08286,0.417587,1.62585,-2.065,0.265033,1.44473,-1.79021,0.074593,1.50947,-1.85106,0.07767,1.5337,-1.91492,0.083647,1.50907,-1.96663,0.269879,1.43614,-2.04242,0.333811,1.15592,-1.7 [...]
+/*1349*/{0.245874,1.93325,-1.69859,0.342674,1.87216,-1.82474,0.078002,1.8469,-1.66051,-0.046477,1.77763,-1.65935,0.073825,1.66986,-1.64737,0.133272,1.60103,-1.62413,0.302743,1.95085,-2.06997,0.358329,1.87392,-1.89824,0.160942,1.91117,-2.0108,0.169154,1.74456,-2.1919,0.219415,1.64728,-2.22822,0.357768,1.63861,-2.08466,0.415343,1.61677,-2.06712,0.264185,1.44145,-1.78948,0.074603,1.5048,-1.85136,0.076796,1.5301,-1.9159,0.082853,1.50461,-1.96726,0.270144,1.43345,-2.04212,0.332672,1.15355,-1. [...]
+/*1350*/{0.246604,1.9293,-1.69808,0.343139,1.8667,-1.8249,0.079559,1.84095,-1.65859,-0.045744,1.76997,-1.65693,0.077118,1.66372,-1.64469,0.137978,1.59583,-1.62242,0.302106,1.94719,-2.07083,0.358176,1.87043,-1.89888,0.160272,1.90774,-2.01029,0.165502,1.74176,-2.19075,0.213336,1.6435,-2.22725,0.35392,1.62993,-2.086,0.412906,1.60718,-2.06966,0.262935,1.43789,-1.7893,0.073802,1.50087,-1.85132,0.076438,1.52567,-1.91544,0.083061,1.50065,-1.9673,0.27039,1.43037,-2.04178,0.330966,1.15195,-1.7186 [...]
+/*1351*/{0.24734,1.92458,-1.69773,0.344298,1.86266,-1.82545,0.08099,1.83415,-1.65637,-0.042168,1.7625,-1.65297,0.080913,1.65732,-1.64176,0.141901,1.59026,-1.62026,0.300201,1.94248,-2.07195,0.358145,1.86626,-1.89931,0.159462,1.9037,-2.01009,0.160962,1.73704,-2.18944,0.208984,1.63949,-2.22655,0.351759,1.62134,-2.08784,0.410532,1.59733,-2.07333,0.261925,1.43447,-1.78796,0.072984,1.49614,-1.85133,0.076867,1.52222,-1.9157,0.082367,1.49561,-1.96758,0.270567,1.42742,-2.0415,0.328662,1.14938,-1. [...]
+/*1352*/{0.248008,1.92,-1.69748,0.343469,1.86049,-1.8261,0.083516,1.82743,-1.65356,-0.039157,1.75464,-1.64832,0.084436,1.65112,-1.63909,0.147329,1.58415,-1.61779,0.299439,1.93776,-2.07291,0.358228,1.8621,-1.90011,0.158667,1.89947,-2.00945,0.15652,1.73278,-2.18791,0.203425,1.63462,-2.22461,0.348326,1.61275,-2.08972,0.407318,1.5868,-2.07621,0.25991,1.43047,-1.78791,0.071906,1.4919,-1.85255,0.075378,1.51723,-1.91606,0.082696,1.49027,-1.96782,0.270919,1.42397,-2.04085,0.328806,1.14546,-1.717 [...]
+/*1353*/{0.247819,1.91445,-1.69705,0.344235,1.85406,-1.82672,0.084847,1.81962,-1.6511,-0.038084,1.74597,-1.64471,0.088481,1.64377,-1.63576,0.152277,1.5783,-1.61642,0.298382,1.93314,-2.07391,0.358378,1.85813,-1.90057,0.158175,1.89479,-2.00894,0.152127,1.7284,-2.18755,0.198307,1.63021,-2.2237,0.345362,1.60337,-2.09141,0.404309,1.57599,-2.07913,0.259236,1.4274,-1.78739,0.071099,1.48654,-1.85213,0.076116,1.51337,-1.91583,0.081353,1.48523,-1.96862,0.270805,1.42035,-2.04044,0.327238,1.14292,-1 [...]
+/*1354*/{0.248564,1.90881,-1.69657,0.344862,1.84952,-1.82693,0.086985,1.81172,-1.64858,-0.035143,1.73703,-1.6409,0.094204,1.63725,-1.63254,0.158134,1.57246,-1.61453,0.297178,1.92757,-2.07485,0.358604,1.85362,-1.90121,0.157103,1.8899,-2.00902,0.148675,1.72293,-2.18608,0.193179,1.6255,-2.2227,0.340751,1.59403,-2.09369,0.400156,1.5647,-2.08275,0.259266,1.42417,-1.78612,0.070592,1.48185,-1.85296,0.07486,1.50851,-1.91666,0.081281,1.47965,-1.9686,0.270256,1.41673,-2.04002,0.326692,1.13845,-1.7 [...]
+/*1355*/{0.249416,1.90288,-1.69606,0.344833,1.84583,-1.82761,0.089123,1.80321,-1.6452,-0.031297,1.72758,-1.63551,0.098181,1.62943,-1.62939,0.16411,1.56553,-1.61227,0.295953,1.92246,-2.07607,0.358015,1.84847,-1.90229,0.156952,1.88429,-2.00831,0.145174,1.71807,-2.18491,0.187861,1.62107,-2.22131,0.33808,1.58413,-2.0954,0.395612,1.55307,-2.08601,0.259291,1.42181,-1.78567,0.070204,1.47792,-1.85459,0.075451,1.50339,-1.91638,0.080695,1.4739,-1.9696,0.269911,1.41298,-2.04077,0.325193,1.13428,-1. [...]
+/*1356*/{0.24984,1.89681,-1.69521,0.34528,1.83934,-1.82762,0.09116,1.79506,-1.64247,-0.028141,1.71761,-1.63046,0.103949,1.62182,-1.62596,0.170684,1.56001,-1.61018,0.294326,1.9171,-2.07786,0.358444,1.84372,-1.90339,0.155562,1.87905,-2.00837,0.140386,1.7123,-2.18392,0.181822,1.61561,-2.2201,0.333614,1.57405,-2.09814,0.391085,1.54094,-2.08996,0.258579,1.41838,-1.78385,0.069451,1.4702,-1.85506,0.073781,1.49766,-1.91669,0.07922,1.46799,-1.97048,0.270903,1.40912,-2.03809,0.32495,1.12991,-1.717 [...]
+/*1357*/{0.250199,1.89041,-1.69482,0.345409,1.83585,-1.82902,0.094122,1.7864,-1.63924,-0.024235,1.70736,-1.62521,0.108271,1.61359,-1.62272,0.176733,1.55369,-1.60862,0.292607,1.91148,-2.07852,0.358637,1.83816,-1.90439,0.154542,1.87392,-2.00727,0.136169,1.70766,-2.18329,0.176812,1.61056,-2.21933,0.329264,1.56354,-2.10013,0.38643,1.52869,-2.09372,0.256932,1.41395,-1.78232,0.068203,1.46585,-1.85555,0.073964,1.49286,-1.91664,0.079073,1.46253,-1.97137,0.270062,1.40395,-2.03953,0.324036,1.124,- [...]
+/*1358*/{0.251088,1.88373,-1.6937,0.345695,1.82887,-1.82959,0.097088,1.77702,-1.6362,-0.020579,1.69591,-1.61947,0.113816,1.60574,-1.6201,0.183989,1.54828,-1.60695,0.292084,1.90549,-2.07986,0.357804,1.83392,-1.90573,0.153308,1.86744,-2.00736,0.132668,1.70192,-2.18223,0.17112,1.60547,-2.21813,0.325225,1.55343,-2.10323,0.381102,1.51577,-2.0972,0.256562,1.4104,-1.78169,0.067817,1.45992,-1.85675,0.073035,1.48701,-1.91658,0.078745,1.45599,-1.97164,0.27063,1.40016,-2.03679,0.3232,1.11897,-1.716 [...]
+/*1359*/{0.251759,1.87728,-1.69315,0.345374,1.82423,-1.83018,0.100764,1.76694,-1.63272,-0.015625,1.6845,-1.61318,0.121343,1.59896,-1.61666,0.191914,1.54163,-1.60501,0.29015,1.90003,-2.08101,0.357823,1.82761,-1.90607,0.152501,1.8624,-2.00645,0.12799,1.6971,-2.1813,0.165194,1.60026,-2.21734,0.319931,1.54179,-2.10606,0.374792,1.50316,-2.10105,0.25648,1.40676,-1.78054,0.066003,1.45383,-1.85852,0.071907,1.48004,-1.91613,0.077313,1.45045,-1.97327,0.269218,1.3953,-2.03809,0.319744,1.11097,-1.71 [...]
+/*1360*/{0.253061,1.87108,-1.69227,0.346715,1.8169,-1.83108,0.104556,1.75655,-1.62902,-0.009156,1.67378,-1.60727,0.127408,1.5908,-1.61383,0.200067,1.53608,-1.60273,0.287921,1.89441,-2.08212,0.358572,1.82248,-1.90746,0.150774,1.856,-2.00615,0.12406,1.69155,-2.18057,0.159708,1.59453,-2.21657,0.314594,1.52974,-2.10892,0.368899,1.49042,-2.1047,0.255806,1.40254,-1.77947,0.065333,1.44858,-1.85827,0.070154,1.47426,-1.91645,0.075934,1.44418,-1.97472,0.269949,1.38953,-2.03629,0.318138,1.10644,-1. [...]
+/*1361*/{0.254203,1.86524,-1.69151,0.34759,1.81262,-1.83227,0.108713,1.74611,-1.6256,-0.003138,1.66236,-1.60146,0.136058,1.58276,-1.60986,0.208208,1.53061,-1.60237,0.287772,1.88993,-2.08347,0.358848,1.81677,-1.90875,0.149312,1.8514,-2.00511,0.120078,1.68734,-2.17986,0.153924,1.58941,-2.2162,0.309471,1.51862,-2.11193,0.36217,1.47777,-2.10851,0.253245,1.39812,-1.7787,0.061102,1.44378,-1.86001,0.068061,1.46854,-1.91826,0.074221,1.43795,-1.97496,0.268696,1.38513,-2.03665,0.312782,1.10408,-1. [...]
+/*1362*/{0.255044,1.8598,-1.69091,0.347824,1.80639,-1.83293,0.113453,1.73589,-1.62175,0.002551,1.65095,-1.5951,0.143467,1.57548,-1.60668,0.216857,1.52578,-1.60016,0.285875,1.8854,-2.08528,0.358793,1.8114,-1.91018,0.14844,1.84784,-2.00402,0.115212,1.68209,-2.17911,0.148129,1.58406,-2.21623,0.303357,1.50651,-2.11553,0.354879,1.46534,-2.11219,0.250468,1.39476,-1.77973,0.05888,1.43911,-1.86139,0.066796,1.46214,-1.91962,0.072037,1.43281,-1.97643,0.265277,1.379,-2.03827,0.309846,1.101,-1.70959 [...]
+/*1363*/{0.255865,1.85516,-1.68972,0.349312,1.80076,-1.83398,0.118513,1.72563,-1.61873,0.010263,1.63967,-1.58846,0.151122,1.56829,-1.60443,0.226528,1.52145,-1.59935,0.284772,1.88161,-2.08648,0.359928,1.80698,-1.91178,0.147455,1.84512,-2.00153,0.112583,1.67849,-2.17884,0.142567,1.57971,-2.2151,0.297538,1.49561,-2.11937,0.347357,1.45365,-2.1161,0.247616,1.39147,-1.77983,0.056409,1.43474,-1.86154,0.063137,1.45791,-1.92139,0.06943,1.42651,-1.97795,0.265462,1.37461,-2.03636,0.309654,1.10102,- [...]
+/*1364*/{0.256803,1.85046,-1.68898,0.348287,1.79743,-1.83679,0.124998,1.71575,-1.61582,0.017608,1.62906,-1.58291,0.160752,1.56199,-1.6021,0.237155,1.51717,-1.59782,0.283075,1.878,-2.08853,0.360162,1.80233,-1.91404,0.146899,1.84306,-2.00021,0.108811,1.67518,-2.17792,0.137172,1.57602,-2.2135,0.291268,1.48547,-2.12223,0.338901,1.44254,-2.11992,0.245121,1.38894,-1.77936,0.052552,1.43153,-1.86377,0.060764,1.45262,-1.92306,0.066587,1.42124,-1.97848,0.264413,1.37014,-2.03528,0.308458,1.10219,-1 [...]
+/*1365*/{0.257917,1.84676,-1.68801,0.349293,1.7928,-1.83743,0.130675,1.70644,-1.61392,0.027145,1.61868,-1.57762,0.170875,1.55686,-1.60012,0.247446,1.51361,-1.59703,0.282621,1.87496,-2.08905,0.360259,1.79867,-1.91629,0.145878,1.84151,-1.9994,0.105989,1.67304,-2.17605,0.132214,1.57381,-2.21197,0.284731,1.4784,-2.12432,0.329466,1.43222,-2.12414,0.242993,1.3869,-1.77779,0.049424,1.4277,-1.86621,0.059053,1.45038,-1.92293,0.063743,1.4155,-1.98048,0.265211,1.36572,-2.03247,0.30714,1.10398,-1.70 [...]
+/*1366*/{0.2585,1.84368,-1.68843,0.348294,1.79124,-1.83961,0.136932,1.69942,-1.61215,0.036756,1.60965,-1.57229,0.181095,1.55196,-1.59846,0.258129,1.51105,-1.59615,0.282237,1.87265,-2.08957,0.359709,1.79513,-1.91735,0.14579,1.84086,-2.00003,0.10207,1.67237,-2.17277,0.126491,1.5734,-2.21038,0.277912,1.47222,-2.12613,0.321062,1.42378,-2.12802,0.240573,1.38566,-1.77597,0.047769,1.42632,-1.86761,0.057524,1.4498,-1.92374,0.060916,1.41246,-1.98194,0.263003,1.36236,-2.03197,0.307005,1.10478,-1.7 [...]
+/*1367*/{0.259355,1.84133,-1.689,0.347917,1.7877,-1.83986,0.142342,1.69425,-1.61109,0.047274,1.6018,-1.56699,0.192165,1.54831,-1.59718,0.270548,1.50923,-1.59547,0.282617,1.87122,-2.0896,0.35907,1.79401,-1.9183,0.145825,1.84013,-2.00059,0.097908,1.67397,-2.16956,0.120842,1.5745,-2.20917,0.270027,1.46739,-2.12746,0.311796,1.41642,-2.13202,0.237667,1.3835,-1.77473,0.046287,1.42582,-1.86809,0.055893,1.45022,-1.92381,0.059325,1.41034,-1.98248,0.262211,1.35976,-2.03211,0.304371,1.10308,-1.7065 [...]
+/*1368*/{0.259335,1.83996,-1.69043,0.346572,1.78671,-1.8409,0.148205,1.69013,-1.60967,0.056869,1.59516,-1.56307,0.203039,1.54534,-1.59635,0.282153,1.5085,-1.59521,0.282171,1.86875,-2.08986,0.357037,1.79247,-1.91879,0.145092,1.83858,-2.00115,0.093248,1.67535,-2.16618,0.114839,1.57664,-2.20802,0.262017,1.4636,-2.12972,0.302948,1.41057,-2.13528,0.236188,1.38291,-1.77274,0.045651,1.42436,-1.86918,0.054888,1.44901,-1.92361,0.057703,1.40997,-1.98328,0.260547,1.35806,-2.03207,0.299619,1.10053,- [...]
+/*1369*/{0.25986,1.83895,-1.69138,0.34577,1.78409,-1.84051,0.154732,1.6877,-1.60866,0.067921,1.59048,-1.55878,0.215531,1.54393,-1.59502,0.295271,1.50878,-1.5953,0.282162,1.86764,-2.09017,0.357126,1.79094,-1.91829,0.144783,1.83833,-2.00255,0.090029,1.67907,-2.16277,0.108159,1.57926,-2.20743,0.254039,1.46155,-2.1316,0.292988,1.40677,-2.13871,0.233869,1.38212,-1.77154,0.044718,1.42405,-1.8692,0.053163,1.44787,-1.92424,0.057275,1.40985,-1.9829,0.257655,1.35716,-2.03291,0.296757,1.09717,-1.70 [...]
+/*1370*/{0.260593,1.83793,-1.69229,0.344841,1.78431,-1.84197,0.159168,1.68584,-1.6082,0.078232,1.58543,-1.55411,0.22672,1.54223,-1.59316,0.307101,1.51,-1.59534,0.281866,1.86676,-2.09044,0.356515,1.79022,-1.91844,0.144368,1.83823,-2.00382,0.084843,1.68236,-2.16025,0.101921,1.58209,-2.20714,0.246469,1.46028,-2.13389,0.284119,1.40493,-2.14206,0.2328,1.38187,-1.77111,0.043744,1.42347,-1.86913,0.052544,1.447,-1.92353,0.05557,1.40961,-1.98235,0.25607,1.35747,-2.03354,0.291192,1.09379,-1.70376, [...]
+/*1371*/{0.261483,1.83705,-1.69263,0.343421,1.78283,-1.84244,0.164927,1.68404,-1.60649,0.088062,1.58085,-1.55072,0.237833,1.5429,-1.59273,0.320324,1.51323,-1.59659,0.281454,1.86673,-2.09053,0.355511,1.78956,-1.91834,0.143768,1.83883,-2.00477,0.081174,1.68745,-2.15779,0.095101,1.58636,-2.20753,0.238392,1.45995,-2.13642,0.274385,1.40409,-2.145,0.231683,1.38114,-1.76954,0.04305,1.42248,-1.86907,0.051547,1.44668,-1.92368,0.054829,1.41036,-1.98174,0.254594,1.35893,-2.0348,0.285146,1.09081,-1. [...]
+/*1372*/{0.262141,1.8376,-1.69325,0.342938,1.7828,-1.84247,0.170219,1.6844,-1.60532,0.099076,1.57801,-1.5474,0.249363,1.54423,-1.59244,0.332757,1.51707,-1.59719,0.280305,1.86687,-2.09109,0.352687,1.7899,-1.9197,0.142839,1.83941,-2.00499,0.07654,1.69176,-2.15477,0.088803,1.59105,-2.20697,0.231062,1.46135,-2.13944,0.265318,1.40477,-2.14781,0.230249,1.3815,-1.76961,0.042264,1.42245,-1.86881,0.050538,1.44629,-1.92386,0.053679,1.41097,-1.98034,0.253431,1.36048,-2.03523,0.280883,1.08717,-1.704 [...]
+/*1373*/{0.262324,1.83873,-1.6941,0.341988,1.78528,-1.84299,0.174713,1.68443,-1.60432,0.109525,1.57637,-1.54525,0.26019,1.54653,-1.5926,0.345096,1.5225,-1.59962,0.279224,1.86766,-2.09185,0.352071,1.79009,-1.92003,0.141587,1.84063,-2.00542,0.07285,1.69735,-2.15274,0.081651,1.59656,-2.20817,0.22266,1.46311,-2.14248,0.255988,1.40628,-2.15056,0.229415,1.38134,-1.76894,0.042018,1.42239,-1.86859,0.049734,1.44613,-1.92382,0.052861,1.41175,-1.97966,0.252708,1.36235,-2.03593,0.27478,1.08186,-1.70 [...]
+/*1374*/{0.263141,1.84074,-1.69444,0.340598,1.78607,-1.84436,0.179532,1.68519,-1.60293,0.119908,1.57491,-1.54155,0.271146,1.5498,-1.59226,0.356447,1.52858,-1.60143,0.277436,1.86884,-2.09185,0.350165,1.7916,-1.92151,0.140275,1.84195,-2.00532,0.067812,1.70263,-2.15172,0.075347,1.60284,-2.2087,0.2141,1.46646,-2.14462,0.247178,1.40962,-2.15265,0.228921,1.382,-1.76951,0.040831,1.4227,-1.86896,0.049122,1.44704,-1.92378,0.05208,1.41205,-1.97888,0.252446,1.36429,-2.03689,0.26551,1.07861,-1.70662 [...]
+/*1375*/{0.264216,1.84324,-1.69498,0.340482,1.78786,-1.84516,0.183899,1.68786,-1.60292,0.129936,1.57408,-1.53923,0.281509,1.55514,-1.59302,0.367269,1.53536,-1.60358,0.275866,1.87058,-2.09207,0.349127,1.79253,-1.92186,0.138593,1.84408,-2.00502,0.061476,1.71151,-2.15343,0.069263,1.60872,-2.2094,0.207365,1.47095,-2.14671,0.238793,1.41385,-2.15412,0.228433,1.38198,-1.76965,0.040983,1.423,-1.86939,0.049833,1.44769,-1.92347,0.051597,1.41358,-1.97932,0.252219,1.36684,-2.03771,0.258339,1.07618,- [...]
+/*1376*/{0.264544,1.8463,-1.69596,0.33937,1.7902,-1.84681,0.188379,1.6901,-1.60178,0.139117,1.57471,-1.53786,0.290509,1.56021,-1.59414,0.377426,1.54327,-1.60658,0.273075,1.8729,-2.09203,0.348112,1.79495,-1.92312,0.137553,1.84517,-2.00436,0.056257,1.71817,-2.15343,0.062204,1.61549,-2.21055,0.199809,1.47606,-2.14838,0.230545,1.41873,-2.15572,0.22889,1.38308,-1.77061,0.041199,1.42432,-1.86956,0.049697,1.44887,-1.92363,0.052357,1.41423,-1.97921,0.252,1.36943,-2.03819,0.251103,1.0744,-1.70888 [...]
+/*1377*/{0.265793,1.84973,-1.69641,0.339313,1.79298,-1.84812,0.193504,1.69222,-1.60148,0.148768,1.57614,-1.5356,0.299677,1.56603,-1.59576,0.387105,1.55183,-1.6097,0.269346,1.87538,-2.09274,0.346625,1.79715,-1.92464,0.135515,1.84808,-2.00355,0.050937,1.72383,-2.15369,0.055615,1.62188,-2.21184,0.191978,1.48203,-2.14983,0.222473,1.42442,-2.1561,0.228726,1.38277,-1.77075,0.041236,1.42617,-1.86957,0.049816,1.44981,-1.9235,0.05216,1.41519,-1.97894,0.253348,1.37242,-2.03925,0.244206,1.07264,-1. [...]
+/*1378*/{0.267406,1.85361,-1.69722,0.339685,1.79562,-1.84898,0.198665,1.69531,-1.60061,0.158372,1.57832,-1.5349,0.307856,1.57226,-1.59717,0.395282,1.56058,-1.61282,0.266425,1.87844,-2.09284,0.345064,1.7995,-1.92556,0.133474,1.8492,-2.00196,0.045267,1.73051,-2.15425,0.049718,1.6284,-2.21247,0.184869,1.48804,-2.15097,0.215071,1.43124,-2.15769,0.229628,1.38359,-1.77179,0.041636,1.42816,-1.86909,0.051136,1.45121,-1.92334,0.051957,1.41762,-1.98054,0.252497,1.37505,-2.0395,0.236011,1.07138,-1. [...]
+/*1379*/{0.269044,1.85704,-1.69832,0.33831,1.79845,-1.8505,0.204096,1.69852,-1.59998,0.166486,1.58017,-1.53336,0.315866,1.57939,-1.59918,0.402819,1.56978,-1.61654,0.262438,1.88212,-2.09293,0.3445,1.8027,-1.92737,0.13157,1.85126,-2.00044,0.04043,1.73702,-2.15487,0.042891,1.63495,-2.21333,0.178028,1.49536,-2.1516,0.207624,1.43827,-2.15889,0.229804,1.38387,-1.77209,0.043584,1.42876,-1.86962,0.050938,1.45306,-1.92382,0.053506,1.41913,-1.98034,0.253404,1.37871,-2.03933,0.228687,1.06951,-1.712 [...]
+/*1380*/{0.271173,1.86133,-1.699,0.338251,1.80136,-1.85217,0.209692,1.70166,-1.59969,0.176592,1.58312,-1.53233,0.322557,1.58668,-1.60105,0.409927,1.57994,-1.61942,0.258967,1.8857,-2.09276,0.342944,1.8059,-1.92888,0.129126,1.85323,-1.99872,0.036208,1.74366,-2.15469,0.036942,1.64156,-2.21419,0.171386,1.5028,-2.15164,0.200943,1.44621,-2.16,0.231283,1.38444,-1.77261,0.043595,1.4317,-1.86911,0.051064,1.45475,-1.92356,0.053992,1.42043,-1.98071,0.254693,1.38216,-2.03918,0.222177,1.06867,-1.7160 [...]
+/*1381*/{0.272974,1.86571,-1.70009,0.338926,1.80537,-1.85338,0.215035,1.70541,-1.6001,0.183641,1.58599,-1.53165,0.329558,1.5936,-1.60298,0.416452,1.58858,-1.62411,0.254099,1.88961,-2.09254,0.342262,1.80975,-1.93083,0.12723,1.85504,-1.99695,0.031354,1.75026,-2.15558,0.031056,1.6484,-2.21469,0.164607,1.51036,-2.15281,0.194869,1.4535,-2.16115,0.232313,1.38504,-1.77218,0.044991,1.43257,-1.86898,0.052903,1.45725,-1.92269,0.055785,1.42319,-1.98191,0.254632,1.38546,-2.03865,0.21444,1.06987,-1.7 [...]
+/*1382*/{0.274618,1.86987,-1.70114,0.339381,1.80949,-1.85485,0.221417,1.70925,-1.60055,0.189869,1.5902,-1.53188,0.335744,1.60122,-1.60566,0.422207,1.59868,-1.62783,0.250697,1.89392,-2.09256,0.342012,1.81377,-1.93231,0.12469,1.85738,-1.99497,0.026236,1.75627,-2.1562,0.025407,1.65511,-2.21528,0.158133,1.51842,-2.15359,0.188749,1.46187,-2.16233,0.232788,1.38537,-1.7714,0.048131,1.43604,-1.86844,0.053276,1.45888,-1.9219,0.056413,1.42662,-1.98229,0.256328,1.38894,-2.03844,0.211101,1.06923,-1. [...]
+/*1383*/{0.276025,1.87503,-1.70284,0.338838,1.81343,-1.85665,0.226996,1.71373,-1.60106,0.199301,1.5934,-1.53118,0.340498,1.60878,-1.60779,0.42696,1.60883,-1.63165,0.24718,1.89814,-2.09235,0.341193,1.8181,-1.93414,0.123465,1.85962,-1.99266,0.021692,1.76276,-2.15643,0.019506,1.66174,-2.21528,0.151706,1.52688,-2.15412,0.183813,1.46988,-2.1633,0.234094,1.38678,-1.77145,0.048291,1.43749,-1.86881,0.054989,1.46201,-1.92146,0.058733,1.42996,-1.98239,0.257871,1.39268,-2.03748,0.203959,1.06964,-1. [...]
+/*1384*/{0.277865,1.88041,-1.704,0.339907,1.81748,-1.8585,0.232855,1.71799,-1.60159,0.205035,1.59764,-1.53066,0.345788,1.61589,-1.6101,0.430293,1.61814,-1.63559,0.242737,1.90243,-2.09168,0.34082,1.82187,-1.93531,0.1219,1.8625,-1.99137,0.018927,1.7675,-2.15686,0.014132,1.66876,-2.21514,0.146005,1.53373,-2.15434,0.178526,1.47811,-2.1646,0.236412,1.38762,-1.77046,0.048922,1.4409,-1.86926,0.056144,1.46466,-1.92057,0.060996,1.43401,-1.98234,0.258533,1.39616,-2.03668,0.198894,1.07129,-1.7254,0 [...]
+/*1385*/{0.279826,1.88561,-1.70507,0.340028,1.82296,-1.8605,0.237838,1.7229,-1.60223,0.211357,1.6024,-1.53094,0.350045,1.62349,-1.61223,0.435081,1.62817,-1.63833,0.239499,1.90649,-2.09147,0.339915,1.82722,-1.93727,0.119408,1.8658,-1.98946,0.014213,1.77404,-2.15701,0.008729,1.67539,-2.21512,0.140791,1.54138,-2.15398,0.173853,1.48646,-2.16575,0.237422,1.38881,-1.76997,0.051614,1.44379,-1.87001,0.057364,1.46814,-1.92049,0.060661,1.43896,-1.98315,0.260264,1.40034,-2.03601,0.193658,1.06963,-1 [...]
+/*1386*/{0.280248,1.89104,-1.70722,0.34017,1.82688,-1.86168,0.242395,1.72814,-1.60311,0.217835,1.60762,-1.53098,0.3539,1.63227,-1.61456,0.438097,1.63699,-1.64165,0.236085,1.91062,-2.09105,0.339041,1.83148,-1.93902,0.118235,1.8685,-1.98791,0.012296,1.77939,-2.15716,0.003448,1.68235,-2.2146,0.135605,1.54984,-2.15467,0.168975,1.49505,-2.16685,0.238444,1.39012,-1.76993,0.052801,1.44717,-1.86977,0.059358,1.47167,-1.91928,0.062578,1.44401,-1.98356,0.261703,1.40444,-2.03484,0.189302,1.07184,-1. [...]
+/*1387*/{0.281361,1.89655,-1.70857,0.340849,1.83187,-1.86403,0.246129,1.73338,-1.60443,0.222934,1.61274,-1.53146,0.356192,1.64012,-1.61745,0.440744,1.64704,-1.64681,0.234082,1.91476,-2.0913,0.338651,1.8368,-1.94071,0.116798,1.87185,-1.98615,0.006806,1.78501,-2.15725,-0.001243,1.68917,-2.21391,0.129649,1.55775,-2.15474,0.164463,1.50285,-2.16778,0.240652,1.39156,-1.76999,0.05532,1.44987,-1.87,0.061867,1.47469,-1.91823,0.064708,1.44885,-1.98454,0.262964,1.40845,-2.03406,0.185854,1.07395,-1. [...]
+/*1388*/{0.281969,1.90148,-1.71042,0.34143,1.83704,-1.86608,0.250335,1.73881,-1.60481,0.227473,1.61759,-1.53176,0.359163,1.64707,-1.61989,0.441992,1.65522,-1.65159,0.232667,1.91919,-2.09128,0.33857,1.84132,-1.94266,0.118049,1.87436,-1.98489,0.003616,1.79025,-2.15651,-0.006328,1.69606,-2.21286,0.126363,1.56531,-2.15482,0.161155,1.51053,-2.16934,0.242391,1.39276,-1.77033,0.057297,1.45386,-1.86937,0.064048,1.47889,-1.91704,0.066963,1.45352,-1.98477,0.263436,1.41288,-2.03306,0.181498,1.07616 [...]
+/*1389*/{0.283071,1.90691,-1.71228,0.341314,1.84117,-1.86768,0.253479,1.74401,-1.6057,0.231704,1.62304,-1.53179,0.361816,1.65373,-1.62165,0.44435,1.6641,-1.65274,0.230154,1.92272,-2.09125,0.337412,1.84537,-1.94397,0.116867,1.87769,-1.98463,0.001287,1.79644,-2.15614,-0.011086,1.70242,-2.2116,0.121813,1.57241,-2.15459,0.157208,1.51848,-2.17022,0.244545,1.39387,-1.7718,0.060439,1.45754,-1.87011,0.066744,1.48277,-1.91597,0.068509,1.45877,-1.98491,0.264038,1.4168,-2.03282,0.177409,1.07809,-1. [...]
+/*1390*/{0.283983,1.91222,-1.71377,0.342466,1.84509,-1.86925,0.256499,1.7497,-1.60623,0.235863,1.6281,-1.53146,0.36308,1.66121,-1.62418,0.446119,1.67279,-1.65665,0.228079,1.92604,-2.09135,0.336977,1.85018,-1.94587,0.11652,1.88079,-1.98305,-0.00338,1.80081,-2.15526,-0.015137,1.70857,-2.2108,0.118026,1.57909,-2.15444,0.156013,1.5247,-2.17051,0.24789,1.3956,-1.77264,0.061743,1.46172,-1.86952,0.069087,1.48717,-1.91526,0.070595,1.46424,-1.98512,0.265384,1.42107,-2.03307,0.173692,1.08049,-1.74 [...]
+/*1391*/{0.285254,1.91709,-1.71509,0.342954,1.85012,-1.87095,0.258526,1.75481,-1.60713,0.239089,1.6331,-1.53211,0.365115,1.66796,-1.6266,0.445669,1.68111,-1.66163,0.227151,1.92985,-2.09096,0.336555,1.85434,-1.94736,0.117132,1.88292,-1.98249,-0.005115,1.80618,-2.15348,-0.019282,1.71454,-2.20905,0.115024,1.58594,-2.15445,0.151054,1.53178,-2.17164,0.250166,1.39712,-1.7742,0.064954,1.46561,-1.86957,0.070994,1.49154,-1.91431,0.07297,1.46945,-1.98559,0.266153,1.42525,-2.03321,0.169336,1.08271, [...]
+/*1392*/{0.286306,1.92213,-1.71682,0.342567,1.85449,-1.8725,0.26153,1.7596,-1.6075,0.241785,1.63854,-1.5326,0.366179,1.6747,-1.62869,0.44665,1.68838,-1.66464,0.22622,1.93294,-2.09123,0.336828,1.85877,-1.94856,0.117238,1.88571,-1.98229,-0.005756,1.81154,-2.15217,-0.022863,1.72021,-2.20741,0.112677,1.59209,-2.15411,0.148627,1.53795,-2.17281,0.252074,1.39833,-1.7758,0.06709,1.47018,-1.86808,0.072399,1.49564,-1.91327,0.075193,1.47455,-1.98473,0.266147,1.42906,-2.03358,0.165524,1.08538,-1.744 [...]
+/*1393*/{0.286381,1.92636,-1.71866,0.343111,1.85813,-1.87437,0.262921,1.76486,-1.60817,0.244955,1.6425,-1.53213,0.366845,1.68093,-1.6307,0.447174,1.69547,-1.66754,0.225377,1.93609,-2.09162,0.336881,1.86265,-1.95012,0.11732,1.88869,-1.98224,-0.006501,1.81677,-2.15085,-0.025957,1.72518,-2.2053,0.109479,1.59735,-2.15394,0.147347,1.54294,-2.17253,0.255318,1.40021,-1.77738,0.069454,1.47416,-1.8673,0.074409,1.49893,-1.9127,0.077765,1.47968,-1.98416,0.267343,1.43302,-2.03452,0.164213,1.08826,-1 [...]
+/*1394*/{0.289066,1.93014,-1.72037,0.343948,1.86191,-1.87591,0.264703,1.76954,-1.60892,0.247663,1.64751,-1.53173,0.366811,1.68642,-1.63248,0.447702,1.70171,-1.67132,0.224999,1.93868,-2.09225,0.336505,1.86579,-1.95153,0.116985,1.89142,-1.98232,-0.007984,1.82018,-2.14677,-0.028673,1.73059,-2.20371,0.106685,1.60182,-2.15332,0.145288,1.54793,-2.17337,0.258124,1.40126,-1.77792,0.071839,1.47861,-1.86749,0.075408,1.50305,-1.91069,0.079467,1.48501,-1.98431,0.267089,1.43667,-2.03502,0.160107,1.09 [...]
+/*1395*/{0.289649,1.93418,-1.72177,0.344689,1.86564,-1.87773,0.266927,1.77366,-1.60846,0.250072,1.65167,-1.53135,0.367409,1.69172,-1.6344,0.446735,1.70797,-1.6743,0.224578,1.9418,-2.09209,0.337136,1.86916,-1.95263,0.118394,1.89315,-1.98215,-0.009303,1.82478,-2.14484,-0.031307,1.73529,-2.20127,0.105456,1.60546,-2.1532,0.14396,1.55247,-2.17355,0.259882,1.40306,-1.78023,0.073253,1.48226,-1.86645,0.077595,1.50681,-1.91086,0.081442,1.4895,-1.98366,0.267982,1.44039,-2.03608,0.157235,1.09252,-1 [...]
+/*1396*/{0.291052,1.93741,-1.7227,0.345623,1.86969,-1.87867,0.267483,1.77805,-1.60922,0.250969,1.65609,-1.53209,0.367389,1.6967,-1.63598,0.446364,1.71295,-1.677,0.223776,1.94296,-2.09201,0.338185,1.87236,-1.95371,0.118557,1.89587,-1.98199,-0.00915,1.82854,-2.14111,-0.032698,1.7397,-2.19946,0.104889,1.60962,-2.1523,0.14266,1.55665,-2.17331,0.262175,1.40514,-1.781,0.076161,1.48561,-1.86563,0.078726,1.51072,-1.90894,0.083392,1.49364,-1.98231,0.267951,1.44404,-2.03654,0.15409,1.09539,-1.7498 [...]
+/*1397*/{0.292983,1.94031,-1.72468,0.346569,1.87259,-1.88039,0.26836,1.78174,-1.60972,0.252825,1.66009,-1.53134,0.367461,1.70062,-1.63767,0.445711,1.7181,-1.68004,0.223537,1.94605,-2.0933,0.338641,1.87493,-1.95481,0.119446,1.89721,-1.98279,-0.009883,1.83283,-2.13947,-0.034101,1.74404,-2.19718,0.104431,1.61376,-2.15183,0.142112,1.56067,-2.17345,0.264562,1.40641,-1.78278,0.077706,1.48903,-1.86509,0.080298,1.51444,-1.90708,0.083867,1.49782,-1.98138,0.268567,1.44737,-2.03738,0.152958,1.09942 [...]
+/*1398*/{0.294261,1.94341,-1.72564,0.347202,1.87597,-1.88185,0.269945,1.78581,-1.60972,0.253896,1.66401,-1.53159,0.367656,1.70485,-1.63876,0.444948,1.72179,-1.68234,0.22462,1.9489,-2.09335,0.339317,1.87734,-1.95604,0.120314,1.89949,-1.98187,-0.008797,1.83636,-2.13602,-0.034755,1.748,-2.19515,0.103716,1.6167,-2.15181,0.142649,1.56411,-2.17306,0.266154,1.40814,-1.7837,0.079362,1.49229,-1.86321,0.081443,1.51722,-1.90639,0.084705,1.5016,-1.97951,0.268774,1.45013,-2.03777,0.151332,1.1018,-1.7 [...]
+/*1399*/{0.295576,1.94628,-1.72755,0.348615,1.87876,-1.8829,0.271191,1.78937,-1.61032,0.255557,1.6678,-1.53123,0.367328,1.70788,-1.64004,0.444231,1.72581,-1.68511,0.225651,1.95043,-2.09372,0.339701,1.87952,-1.95717,0.121207,1.90109,-1.98208,-0.009303,1.83892,-2.13458,-0.03493,1.75134,-2.19362,0.103707,1.61903,-2.15112,0.142297,1.56687,-2.17324,0.26728,1.41005,-1.78504,0.080885,1.49512,-1.86222,0.082347,1.52049,-1.90523,0.084965,1.50533,-1.97794,0.268798,1.45309,-2.03813,0.150947,1.10417, [...]
+/*1400*/{0.297318,1.94856,-1.72893,0.349692,1.88121,-1.88447,0.272357,1.79301,-1.61174,0.255889,1.67003,-1.5301,0.367018,1.71153,-1.64167,0.443619,1.72883,-1.68721,0.225702,1.9518,-2.09397,0.340621,1.88163,-1.95809,0.122781,1.90231,-1.9826,-0.008241,1.84191,-2.1313,-0.035532,1.75314,-2.19096,0.104093,1.6215,-2.15095,0.142451,1.56887,-2.17311,0.268498,1.41215,-1.78526,0.082423,1.49777,-1.86119,0.082978,1.52388,-1.90409,0.084964,1.50928,-1.97648,0.268463,1.45568,-2.03867,0.151173,1.10624,- [...]
+/*1401*/{0.299084,1.9507,-1.73006,0.350894,1.8825,-1.88522,0.272816,1.79563,-1.61187,0.255611,1.67308,-1.53057,0.366241,1.71367,-1.64277,0.443333,1.73093,-1.68896,0.227086,1.95422,-2.09454,0.341567,1.88236,-1.95981,0.123589,1.90539,-1.98244,-0.008059,1.84371,-2.12969,-0.035406,1.75535,-2.18924,0.105613,1.62363,-2.15064,0.141729,1.5724,-2.17418,0.269043,1.41434,-1.78651,0.084057,1.50036,-1.85948,0.083722,1.52729,-1.90395,0.084483,1.51181,-1.97497,0.269565,1.45818,-2.03917,0.152367,1.10706 [...]
+/*1402*/{0.300585,1.95147,-1.73123,0.35126,1.8839,-1.88673,0.274234,1.79817,-1.61242,0.256119,1.67586,-1.53027,0.366091,1.71541,-1.64487,0.44241,1.73292,-1.69071,0.228474,1.9551,-2.09511,0.342759,1.88337,-1.96036,0.124695,1.90694,-1.98267,-0.006556,1.84491,-2.12764,-0.035045,1.75641,-2.1879,0.105909,1.62504,-2.15021,0.144046,1.57293,-2.17344,0.269285,1.41615,-1.78681,0.085005,1.50311,-1.85966,0.083538,1.52936,-1.90352,0.083766,1.51477,-1.97358,0.269491,1.46042,-2.03927,0.152964,1.10906,- [...]
+/*1403*/{0.302086,1.95341,-1.73219,0.351919,1.88521,-1.88774,0.275243,1.79987,-1.613,0.256116,1.67844,-1.52975,0.365396,1.71599,-1.64413,0.441457,1.73305,-1.6914,0.228939,1.95572,-2.09606,0.343379,1.88436,-1.96237,0.125784,1.90842,-1.98316,-0.006366,1.84617,-2.12532,-0.034413,1.75727,-2.18693,0.106815,1.62672,-2.14978,0.143917,1.57456,-2.1741,0.270317,1.41809,-1.78757,0.086123,1.50497,-1.85753,0.084368,1.53223,-1.90226,0.087698,1.51545,-1.97306,0.269294,1.4627,-2.03953,0.153644,1.11054,- [...]
+/*1404*/{0.303912,1.95394,-1.73281,0.353507,1.88679,-1.88954,0.276019,1.80191,-1.61414,0.256749,1.68008,-1.52963,0.365259,1.71699,-1.64435,0.440454,1.7328,-1.69308,0.229647,1.95589,-2.09723,0.345137,1.88467,-1.96272,0.12764,1.91013,-1.98349,-0.004824,1.84678,-2.12423,-0.033637,1.75776,-2.1854,0.108499,1.62763,-2.14974,0.145034,1.57579,-2.17393,0.271837,1.41944,-1.78967,0.086175,1.50748,-1.85675,0.084383,1.53486,-1.90202,0.088153,1.51635,-1.9731,0.269237,1.46385,-2.04031,0.159299,1.1116,- [...]
+/*1405*/{0.305575,1.95392,-1.73471,0.354325,1.88744,-1.89052,0.276332,1.80295,-1.61435,0.254391,1.68142,-1.52999,0.364581,1.71695,-1.64531,0.44052,1.73219,-1.69407,0.231735,1.95743,-2.09795,0.345832,1.88489,-1.96396,0.129014,1.91113,-1.98365,-0.003153,1.84656,-2.12261,-0.032686,1.75713,-2.18464,0.109332,1.62751,-2.14936,0.146731,1.57589,-2.17478,0.271373,1.42132,-1.78979,0.086512,1.50862,-1.85636,0.084652,1.53668,-1.9028,0.087174,1.51812,-1.97205,0.269708,1.46538,-2.04039,0.161818,1.1108 [...]
+/*1406*/{0.306616,1.95432,-1.73511,0.354861,1.88749,-1.89186,0.276305,1.80341,-1.61497,0.253903,1.6826,-1.52964,0.363596,1.71606,-1.64575,0.439441,1.73167,-1.6957,0.233242,1.95724,-2.09972,0.346446,1.88485,-1.96494,0.130581,1.91082,-1.98572,-0.003362,1.84597,-2.12198,-0.031998,1.75611,-2.18392,0.112148,1.62821,-2.14917,0.148753,1.57555,-2.17486,0.272017,1.42265,-1.79111,0.086061,1.50885,-1.85559,0.084572,1.53682,-1.90259,0.08708,1.5181,-1.97195,0.269541,1.46629,-2.04065,0.16554,1.11057,- [...]
+/*1407*/{0.30879,1.95365,-1.73585,0.355479,1.88721,-1.89212,0.276063,1.80376,-1.61626,0.251857,1.68306,-1.52932,0.362978,1.71432,-1.64653,0.439249,1.72854,-1.69642,0.235314,1.95681,-2.1006,0.348203,1.88459,-1.96561,0.132133,1.91214,-1.98629,-0.002148,1.84476,-2.12124,-0.030766,1.75461,-2.18369,0.1137,1.62685,-2.14884,0.151123,1.57473,-2.1751,0.272309,1.42354,-1.79177,0.086014,1.50897,-1.85488,0.08398,1.53805,-1.90288,0.086824,1.5181,-1.97166,0.269708,1.46688,-2.04089,0.169877,1.10977,-1. [...]
+/*1408*/{0.309757,1.95258,-1.73648,0.355443,1.88734,-1.89315,0.276057,1.80346,-1.61705,0.251037,1.68268,-1.5292,0.362436,1.7131,-1.6472,0.437706,1.7249,-1.69777,0.2366,1.95602,-2.10212,0.348527,1.88418,-1.96629,0.133326,1.91218,-1.98724,-0.000628,1.84169,-2.11905,-0.028927,1.7528,-2.18358,0.116902,1.6258,-2.14888,0.153069,1.57329,-2.17554,0.271998,1.42433,-1.79187,0.08562,1.509,-1.85529,0.083482,1.53845,-1.90315,0.086537,1.51782,-1.97085,0.269709,1.46691,-2.04088,0.174547,1.1087,-1.7663, [...]
+/*1409*/{0.311036,1.95156,-1.73784,0.356123,1.88626,-1.89399,0.275363,1.8027,-1.61792,0.248919,1.68338,-1.53013,0.360674,1.71073,-1.64704,0.43591,1.72112,-1.69842,0.237879,1.95496,-2.10342,0.348773,1.88379,-1.96674,0.134753,1.91293,-1.9892,0.001556,1.84042,-2.119,-0.027352,1.75015,-2.18392,0.118822,1.62409,-2.14945,0.156152,1.57111,-2.17654,0.272073,1.42533,-1.79216,0.085545,1.50867,-1.85619,0.082554,1.53776,-1.90338,0.085098,1.51708,-1.97172,0.269708,1.46688,-2.04089,0.180688,1.10833,-1 [...]
+/*1410*/{0.311613,1.95039,-1.73805,0.357136,1.88546,-1.89485,0.274524,1.80166,-1.61872,0.247434,1.68138,-1.52956,0.359263,1.70701,-1.64717,0.434483,1.71661,-1.69841,0.239634,1.95347,-2.10471,0.34908,1.88281,-1.9677,0.136271,1.91231,-1.99027,0.002485,1.83684,-2.11828,-0.025558,1.74723,-2.18405,0.122189,1.62176,-2.14958,0.15878,1.56892,-2.17757,0.271996,1.42536,-1.79249,0.084674,1.5076,-1.85636,0.082555,1.53741,-1.90412,0.085135,1.51556,-1.97264,0.27018,1.46676,-2.04118,0.185775,1.10683,-1 [...]
+/*1411*/{0.312207,1.94806,-1.73891,0.35693,1.88533,-1.89493,0.273717,1.80018,-1.61959,0.244138,1.6804,-1.52994,0.358111,1.704,-1.6478,0.433425,1.71171,-1.69912,0.240949,1.95201,-2.1059,0.349228,1.8819,-1.96789,0.136913,1.91142,-1.99272,0.003893,1.83352,-2.11881,-0.023005,1.74384,-2.18432,0.124842,1.61928,-2.15052,0.162591,1.56619,-2.17807,0.272052,1.42524,-1.79329,0.084257,1.50667,-1.85609,0.081695,1.53548,-1.90464,0.084119,1.51385,-1.97328,0.270441,1.46558,-2.04141,0.191456,1.10504,-1.7 [...]
+/*1412*/{0.312823,1.94637,-1.74024,0.356168,1.88366,-1.89562,0.271396,1.79854,-1.6205,0.241643,1.68017,-1.53145,0.356193,1.6991,-1.64817,0.432307,1.70554,-1.69935,0.242102,1.95041,-2.10741,0.35039,1.88006,-1.96827,0.137875,1.91056,-1.99357,0.008494,1.83127,-2.118,-0.020625,1.73985,-2.18537,0.129075,1.61594,-2.15154,0.166333,1.56232,-2.17958,0.272047,1.4249,-1.79362,0.08307,1.50427,-1.85657,0.081569,1.53384,-1.90475,0.084216,1.51122,-1.97364,0.269372,1.4639,-2.04133,0.197103,1.10311,-1.77 [...]
+/*1413*/{0.312567,1.94308,-1.74112,0.356814,1.88131,-1.89681,0.270497,1.79685,-1.62113,0.237568,1.67738,-1.53227,0.353493,1.69473,-1.6484,0.430069,1.69909,-1.69892,0.243186,1.94842,-2.10847,0.350501,1.87839,-1.9686,0.138189,1.90968,-1.99573,0.007406,1.82562,-2.11801,-0.01758,1.73539,-2.18619,0.13203,1.61211,-2.15282,0.169843,1.55902,-2.18075,0.272193,1.42438,-1.79414,0.082508,1.50239,-1.85818,0.080808,1.53227,-1.90588,0.082893,1.50822,-1.97427,0.26936,1.46245,-2.04262,0.201701,1.10183,-1 [...]
+/*1414*/{0.313,1.94041,-1.74149,0.356622,1.87959,-1.8973,0.267747,1.79414,-1.62185,0.233285,1.67551,-1.53378,0.351174,1.6893,-1.64829,0.427254,1.69156,-1.69862,0.244964,1.94588,-2.10967,0.350535,1.87657,-1.96892,0.13879,1.90716,-1.99734,0.011501,1.82104,-2.11975,-0.014345,1.73067,-2.18804,0.136583,1.60805,-2.15374,0.173763,1.5547,-2.18205,0.271797,1.42336,-1.79423,0.083079,1.49955,-1.85895,0.080018,1.53027,-1.90674,0.083566,1.50597,-1.97511,0.268711,1.45971,-2.04355,0.20601,1.10005,-1.77 [...]
+/*1415*/{0.312853,1.93768,-1.74165,0.356424,1.87772,-1.89771,0.265673,1.7916,-1.62237,0.229906,1.67339,-1.53529,0.348133,1.68306,-1.6482,0.425692,1.68433,-1.69836,0.245681,1.94323,-2.11068,0.350321,1.87372,-1.96914,0.139224,1.90615,-1.9992,0.013586,1.81663,-2.1205,-0.01123,1.7254,-2.18945,0.139997,1.60366,-2.15606,0.178437,1.55028,-2.18345,0.271961,1.42236,-1.79453,0.082949,1.49693,-1.85997,0.080897,1.52686,-1.90635,0.082776,1.50293,-1.97649,0.267785,1.45687,-2.04439,0.212703,1.09648,-1. [...]
+/*1416*/{0.311915,1.93348,-1.74174,0.356391,1.87396,-1.8975,0.262992,1.78825,-1.62363,0.224596,1.67007,-1.53656,0.345235,1.67691,-1.6479,0.422813,1.67648,-1.69729,0.246735,1.94002,-2.11126,0.350133,1.87081,-1.96946,0.139645,1.90348,-2.00069,0.015726,1.81069,-2.12224,-0.007772,1.71965,-2.19106,0.144298,1.59899,-2.15745,0.182581,1.54593,-2.18559,0.271759,1.42083,-1.79494,0.082433,1.49426,-1.86105,0.080421,1.52452,-1.90821,0.082388,1.4997,-1.97683,0.268126,1.45411,-2.04546,0.217535,1.09446, [...]
+/*1417*/{0.311681,1.92973,-1.74183,0.355584,1.87095,-1.89793,0.260022,1.78489,-1.62412,0.220052,1.66682,-1.538,0.341076,1.67062,-1.64774,0.41875,1.66772,-1.69618,0.247072,1.93692,-2.11224,0.349949,1.86769,-1.96985,0.138864,1.90124,-2.00322,0.014319,1.80825,-2.12577,-0.004633,1.71285,-2.19345,0.148108,1.59364,-2.16028,0.186779,1.54051,-2.18718,0.272486,1.41854,-1.79529,0.081792,1.49102,-1.86218,0.07973,1.52096,-1.90789,0.081285,1.49618,-1.97867,0.267537,1.45126,-2.04652,0.224319,1.0899,-1 [...]
+/*1418*/{0.310555,1.92572,-1.742,0.356341,1.86787,-1.89839,0.256693,1.78149,-1.62416,0.215124,1.66324,-1.53997,0.337039,1.66406,-1.64764,0.416767,1.65838,-1.69495,0.247265,1.93334,-2.11299,0.350381,1.86464,-1.97004,0.139683,1.89846,-2.00401,0.015956,1.8022,-2.12831,-0.001044,1.70613,-2.1962,0.153005,1.58792,-2.16221,0.191986,1.53525,-2.189,0.272654,1.41649,-1.79624,0.081797,1.48738,-1.8626,0.08032,1.51769,-1.9083,0.081682,1.49176,-1.97857,0.266943,1.44865,-2.04796,0.230189,1.08586,-1.775 [...]
+/*1419*/{0.310073,1.92146,-1.74168,0.356539,1.8637,-1.8985,0.252639,1.7775,-1.6248,0.208748,1.66006,-1.54211,0.332638,1.65687,-1.64703,0.412866,1.6486,-1.69366,0.24846,1.92906,-2.11374,0.350558,1.86085,-1.97025,0.138949,1.89495,-2.00655,0.019582,1.79164,-2.12936,0.003238,1.6992,-2.198,0.157196,1.58159,-2.16506,0.197166,1.52926,-2.19077,0.27302,1.41416,-1.79659,0.080925,1.48337,-1.86341,0.078534,1.51412,-1.9097,0.080396,1.48812,-1.97994,0.265714,1.44615,-2.04888,0.234213,1.08364,-1.77537, [...]
+/*1420*/{0.309171,1.91651,-1.74099,0.355885,1.86033,-1.89873,0.248804,1.77343,-1.6251,0.204682,1.65622,-1.54361,0.327976,1.64935,-1.64743,0.408765,1.63908,-1.69174,0.247689,1.92492,-2.11417,0.351105,1.85694,-1.97059,0.139162,1.89155,-2.00773,0.020639,1.78432,-2.13284,0.006347,1.69101,-2.20089,0.161579,1.57616,-2.16814,0.201966,1.52319,-2.19283,0.272733,1.41133,-1.7969,0.080928,1.47933,-1.86297,0.07933,1.51011,-1.90945,0.079681,1.48339,-1.98008,0.265036,1.44343,-2.04964,0.239268,1.08181,- [...]
+/*1421*/{0.308099,1.91213,-1.74073,0.356655,1.85537,-1.89784,0.244951,1.76915,-1.62533,0.196721,1.6528,-1.54627,0.323177,1.64164,-1.64659,0.404076,1.6283,-1.69011,0.247923,1.92039,-2.1152,0.350716,1.85288,-1.97041,0.138573,1.88708,-2.01046,0.022922,1.77577,-2.13588,0.010084,1.68317,-2.20434,0.166778,1.5691,-2.17167,0.207603,1.51621,-2.19497,0.272856,1.40856,-1.79757,0.08005,1.47547,-1.86456,0.079382,1.50544,-1.91034,0.078336,1.47888,-1.97973,0.264722,1.44042,-2.05075,0.243025,1.07972,-1. [...]
+/*1422*/{0.307227,1.90708,-1.73981,0.356165,1.85028,-1.89767,0.240432,1.76443,-1.62533,0.189887,1.64922,-1.54806,0.31812,1.634,-1.64667,0.399782,1.61735,-1.68802,0.247919,1.9155,-2.11613,0.350576,1.84842,-1.97081,0.138464,1.88209,-2.01141,0.024201,1.76827,-2.13991,0.013624,1.67445,-2.20775,0.171678,1.56231,-2.17438,0.213489,1.51004,-2.19667,0.272675,1.40578,-1.79815,0.079665,1.47125,-1.86435,0.079074,1.50068,-1.91019,0.077476,1.47443,-1.97958,0.264058,1.43751,-2.05169,0.24788,1.07536,-1. [...]
+/*1423*/{0.305883,1.90234,-1.73928,0.356139,1.84629,-1.89771,0.236161,1.75944,-1.62549,0.183486,1.64541,-1.55003,0.311758,1.62586,-1.6467,0.393921,1.60652,-1.68599,0.24834,1.91061,-2.11678,0.350168,1.84377,-1.97147,0.137643,1.8772,-2.01374,0.02633,1.75932,-2.14362,0.017728,1.66553,-2.21127,0.176753,1.55461,-2.17786,0.219317,1.50232,-2.19895,0.272365,1.40213,-1.79861,0.079522,1.46603,-1.86484,0.078428,1.49609,-1.9107,0.076646,1.46988,-1.97956,0.262981,1.43395,-2.05273,0.253481,1.07382,-1. [...]
+/*1424*/{0.304231,1.89715,-1.7391,0.356006,1.83949,-1.89702,0.231833,1.75441,-1.62632,0.176459,1.64068,-1.55103,0.306009,1.61732,-1.64668,0.388262,1.59538,-1.68396,0.248776,1.90595,-2.11811,0.350262,1.83872,-1.97073,0.135916,1.87241,-2.01532,0.026289,1.75284,-2.14891,0.022336,1.65608,-2.21515,0.181626,1.54646,-2.18081,0.225366,1.49499,-2.20033,0.272549,1.39848,-1.79921,0.078273,1.4611,-1.86516,0.077892,1.49131,-1.9113,0.075107,1.464,-1.97971,0.263137,1.43075,-2.05404,0.260394,1.07004,-1. [...]
+/*1425*/{0.302258,1.89114,-1.73839,0.356051,1.83453,-1.89642,0.227448,1.74946,-1.62702,0.168577,1.63649,-1.55306,0.30067,1.60913,-1.64685,0.38168,1.5843,-1.68184,0.2478,1.9,-2.11846,0.350875,1.83419,-1.97078,0.136021,1.86679,-2.01725,0.028875,1.74332,-2.15364,0.026339,1.64679,-2.219,0.1882,1.53805,-2.18331,0.231423,1.48785,-2.20297,0.273102,1.39514,-1.80011,0.078394,1.45504,-1.86535,0.076609,1.48602,-1.91206,0.074049,1.45917,-1.97993,0.261863,1.42693,-2.05593,0.265338,1.06713,-1.76615,0. [...]
+/*1426*/{0.300201,1.88583,-1.73662,0.356799,1.82911,-1.89554,0.223454,1.74415,-1.62799,0.162034,1.6331,-1.55432,0.293561,1.60036,-1.6464,0.374967,1.57305,-1.68038,0.248323,1.8948,-2.11972,0.351516,1.8291,-1.97088,0.135404,1.86014,-2.0193,0.030735,1.73313,-2.15779,0.031176,1.63772,-2.22303,0.194146,1.53012,-2.18664,0.238328,1.47994,-2.20455,0.272664,1.3912,-1.80147,0.077796,1.44967,-1.86535,0.075389,1.48115,-1.91258,0.072323,1.45424,-1.97952,0.260717,1.42328,-2.0579,0.270562,1.06366,-1.76 [...]
+/*1427*/{0.299791,1.88124,-1.73596,0.356695,1.82252,-1.89453,0.218664,1.73962,-1.62838,0.156132,1.62957,-1.55628,0.28747,1.59283,-1.6463,0.368386,1.56236,-1.67803,0.249669,1.88967,-2.12103,0.351892,1.82412,-1.97068,0.13487,1.8549,-2.02069,0.03336,1.72322,-2.16241,0.036543,1.6276,-2.22751,0.200509,1.52174,-2.18974,0.245273,1.47263,-2.20631,0.272189,1.3866,-1.8026,0.076044,1.44465,-1.86556,0.074458,1.47588,-1.91257,0.070271,1.44959,-1.97926,0.25965,1.41888,-2.05997,0.275396,1.06042,-1.7625 [...]
+/*1428*/{0.298065,1.87519,-1.73527,0.356499,1.81615,-1.89352,0.214492,1.73461,-1.62924,0.147056,1.62639,-1.55912,0.280624,1.5841,-1.64621,0.360939,1.5514,-1.67616,0.250828,1.88486,-2.12303,0.351587,1.81842,-1.97015,0.133724,1.84809,-2.02211,0.036288,1.71284,-2.16771,0.041739,1.61799,-2.23157,0.208373,1.51333,-2.19218,0.251963,1.46524,-2.20848,0.271897,1.38199,-1.80354,0.074899,1.43841,-1.8652,0.073921,1.46967,-1.91266,0.066878,1.44538,-1.97944,0.256282,1.41505,-2.06138,0.279727,1.05681,- [...]
+/*1429*/{0.29674,1.87016,-1.73415,0.35736,1.81011,-1.89235,0.209932,1.72988,-1.63008,0.1406,1.62391,-1.56046,0.273707,1.57639,-1.64528,0.353086,1.54134,-1.67348,0.251926,1.87969,-2.12436,0.353362,1.81334,-1.97019,0.132795,1.84282,-2.02363,0.040092,1.70223,-2.172,0.048216,1.60766,-2.23559,0.215187,1.50554,-2.19493,0.260067,1.45722,-2.20979,0.27055,1.37742,-1.80545,0.073147,1.43334,-1.86603,0.07094,1.46484,-1.91161,0.063709,1.44126,-1.97932,0.254408,1.41223,-2.063,0.283675,1.05453,-1.75995 [...]
+/*1430*/{0.295425,1.86529,-1.73315,0.3575,1.80418,-1.8908,0.206254,1.7263,-1.63084,0.134837,1.62174,-1.56282,0.266258,1.56922,-1.64421,0.345368,1.53152,-1.67162,0.252793,1.87534,-2.12649,0.354199,1.80774,-1.96953,0.133592,1.84,-2.02305,0.043948,1.69219,-2.17808,0.055245,1.59809,-2.23982,0.223586,1.49839,-2.19706,0.268752,1.45061,-2.21112,0.269847,1.37291,-1.80802,0.070345,1.42886,-1.86497,0.068612,1.45939,-1.91169,0.059789,1.43709,-1.97928,0.251061,1.40982,-2.06523,0.291299,1.05346,-1.75 [...]
+/*1431*/{0.294363,1.86115,-1.73217,0.358118,1.79863,-1.8908,0.201789,1.72273,-1.63167,0.127495,1.61918,-1.56478,0.259337,1.56272,-1.64401,0.337041,1.52261,-1.6696,0.253958,1.87099,-2.12811,0.355311,1.80242,-1.96889,0.134548,1.83901,-2.02252,0.053444,1.67831,-2.18356,0.063611,1.58762,-2.24354,0.231942,1.49128,-2.19915,0.27774,1.44452,-2.21253,0.268258,1.36818,-1.81022,0.068287,1.42384,-1.86536,0.065845,1.45613,-1.91134,0.055996,1.43471,-1.97776,0.248041,1.40751,-2.06807,0.293385,1.05179,- [...]
+/*1432*/{0.293481,1.85738,-1.73091,0.357979,1.79474,-1.88894,0.197724,1.72036,-1.63306,0.120511,1.61794,-1.56703,0.25252,1.55638,-1.64387,0.328607,1.51489,-1.66768,0.256063,1.86741,-2.13056,0.355866,1.79825,-1.96779,0.134675,1.83691,-2.02252,0.061697,1.67013,-2.19096,0.073352,1.57857,-2.24725,0.24168,1.4857,-2.1996,0.287363,1.4389,-2.21242,0.266747,1.36419,-1.81294,0.064961,1.42042,-1.86424,0.063253,1.45434,-1.91182,0.054445,1.43348,-1.97583,0.244849,1.40662,-2.07048,0.296877,1.05495,-1. [...]
+/*1433*/{0.292952,1.85411,-1.73009,0.357269,1.79164,-1.8883,0.193754,1.71846,-1.63383,0.112238,1.61821,-1.5699,0.244999,1.55176,-1.64303,0.320404,1.50778,-1.66571,0.257879,1.86399,-2.13059,0.355755,1.79517,-1.96726,0.134898,1.83627,-2.02316,0.066778,1.6623,-2.19797,0.083661,1.56997,-2.24946,0.251101,1.48054,-2.1998,0.298325,1.43493,-2.21317,0.265281,1.36026,-1.81459,0.064965,1.41788,-1.86108,0.062515,1.45363,-1.90991,0.052795,1.43249,-1.97348,0.242147,1.4062,-2.07286,0.302512,1.05698,-1. [...]
+/*1434*/{0.293052,1.85194,-1.72927,0.357407,1.78909,-1.88717,0.188601,1.71757,-1.635,0.105045,1.6192,-1.57241,0.237631,1.54831,-1.64303,0.311265,1.50153,-1.66307,0.259885,1.86168,-2.1303,0.355766,1.7926,-1.96647,0.135451,1.83584,-2.02314,0.074524,1.65577,-2.20401,0.094542,1.56355,-2.25178,0.261873,1.47708,-2.20006,0.309083,1.43194,-2.2133,0.264138,1.35721,-1.8174,0.062793,1.41746,-1.85986,0.060603,1.45368,-1.90987,0.051137,1.43171,-1.97119,0.240004,1.40604,-2.0753,0.309113,1.06066,-1.758 [...]
+/*1435*/{0.292351,1.84935,-1.72895,0.357233,1.78808,-1.88699,0.1846,1.71765,-1.6357,0.098115,1.62152,-1.5751,0.228951,1.54522,-1.64245,0.303083,1.49719,-1.66028,0.261687,1.86013,-2.12924,0.355941,1.79147,-1.96564,0.135704,1.83478,-2.0236,0.082491,1.65066,-2.20866,0.106158,1.55804,-2.25322,0.272512,1.47437,-2.19931,0.32144,1.4302,-2.2123,0.261557,1.35444,-1.81784,0.062003,1.41734,-1.85902,0.058952,1.45334,-1.91004,0.050507,1.43182,-1.96923,0.23757,1.40531,-2.07728,0.313034,1.06477,-1.7589 [...]
+/*1436*/{0.29178,1.84755,-1.72882,0.355662,1.78705,-1.88604,0.179979,1.71827,-1.6367,0.091665,1.62456,-1.57748,0.22181,1.54493,-1.64172,0.294141,1.49352,-1.65847,0.264163,1.85905,-2.12861,0.355642,1.7895,-1.96515,0.136382,1.83314,-2.0245,0.091487,1.64647,-2.21138,0.116802,1.55448,-2.25398,0.283951,1.47315,-2.19802,0.333101,1.42948,-2.21104,0.260367,1.35195,-1.81916,0.060922,1.41742,-1.85895,0.057396,1.45276,-1.91074,0.049018,1.4323,-1.96821,0.235345,1.40519,-2.07913,0.320429,1.06972,-1.7 [...]
+/*1437*/{0.290921,1.84668,-1.72832,0.355399,1.78481,-1.8852,0.175636,1.71945,-1.63733,0.08435,1.6283,-1.58021,0.214061,1.54406,-1.64146,0.285096,1.49119,-1.65679,0.266777,1.85809,-2.12781,0.355762,1.78884,-1.96373,0.137162,1.83354,-2.02587,0.09977,1.64466,-2.21251,0.127253,1.55184,-2.2547,0.296464,1.47275,-2.19594,0.345189,1.43048,-2.20971,0.259522,1.35033,-1.8207,0.059736,1.41804,-1.85746,0.056366,1.45316,-1.91053,0.047697,1.43339,-1.96816,0.232581,1.40466,-2.08052,0.325839,1.07426,-1.7 [...]
+/*1438*/{0.290757,1.84664,-1.72785,0.353735,1.78424,-1.88464,0.171255,1.72107,-1.63816,0.076795,1.63278,-1.58246,0.206565,1.54533,-1.6407,0.276879,1.49045,-1.65616,0.268206,1.85785,-2.12731,0.355282,1.78822,-1.9635,0.138282,1.83401,-2.02616,0.106694,1.64146,-2.21252,0.136855,1.54978,-2.25508,0.30559,1.47269,-2.1947,0.356928,1.43244,-2.20773,0.258311,1.35042,-1.8209,0.059585,1.41908,-1.85801,0.054992,1.45364,-1.91126,0.047076,1.43379,-1.96842,0.231844,1.40471,-2.08178,0.334097,1.08004,-1. [...]
+/*1439*/{0.289195,1.84685,-1.72754,0.353124,1.78398,-1.88422,0.166293,1.72415,-1.63873,0.070364,1.63708,-1.58471,0.198983,1.54568,-1.63948,0.267527,1.49029,-1.65418,0.270746,1.85814,-2.12715,0.355461,1.78797,-1.96205,0.139387,1.83442,-2.02717,0.114557,1.64051,-2.21264,0.145861,1.54802,-2.25523,0.317377,1.47444,-2.19311,0.368703,1.4353,-2.20542,0.25647,1.35092,-1.82123,0.058895,1.42029,-1.85877,0.055063,1.45359,-1.91173,0.046032,1.4343,-1.97022,0.231806,1.40512,-2.08264,0.340657,1.08698,- [...]
+/*1440*/{0.287118,1.84771,-1.72729,0.353014,1.78414,-1.8829,0.161833,1.728,-1.63999,0.063465,1.6425,-1.58724,0.190532,1.54795,-1.63883,0.25817,1.49107,-1.65369,0.273401,1.8589,-2.12692,0.355201,1.78831,-1.96119,0.14049,1.83595,-2.02948,0.120182,1.63958,-2.21262,0.155375,1.54777,-2.25538,0.328641,1.47845,-2.19125,0.379521,1.43993,-2.20247,0.255471,1.35232,-1.82055,0.058104,1.42088,-1.86053,0.053855,1.45437,-1.91256,0.04572,1.43549,-1.97163,0.231009,1.40549,-2.08354,0.347307,1.0928,-1.7598 [...]
+/*1441*/{0.285092,1.84924,-1.72706,0.350913,1.78515,-1.88177,0.156867,1.73204,-1.6408,0.057171,1.64802,-1.58867,0.183941,1.55209,-1.63907,0.250226,1.49341,-1.65307,0.275881,1.86044,-2.12639,0.35433,1.78871,-1.96,0.140855,1.83622,-2.0299,0.127024,1.63935,-2.21246,0.163787,1.54781,-2.25548,0.337931,1.48196,-2.18881,0.391125,1.44557,-2.19921,0.253708,1.35457,-1.821,0.056797,1.42232,-1.8611,0.052865,1.45513,-1.91417,0.0459,1.43634,-1.97293,0.231303,1.40531,-2.08463,0.354065,1.10074,-1.76018, [...]
+/*1442*/{0.281841,1.85112,-1.72694,0.350753,1.78592,-1.88055,0.151668,1.73643,-1.64146,0.051336,1.65433,-1.59022,0.175372,1.55526,-1.6386,0.2412,1.49674,-1.65236,0.278832,1.86232,-2.12508,0.354613,1.79032,-1.9585,0.142182,1.83762,-2.03177,0.132391,1.63932,-2.21229,0.171817,1.54855,-2.25514,0.346881,1.48649,-2.18627,0.401242,1.45258,-2.19591,0.252406,1.35657,-1.82022,0.056047,1.42347,-1.86248,0.052235,1.45598,-1.91638,0.045055,1.43724,-1.97432,0.231207,1.40532,-2.08499,0.359823,1.10886,-1 [...]
+/*1443*/{0.279776,1.85331,-1.72617,0.349321,1.78787,-1.87865,0.146634,1.74167,-1.64218,0.043669,1.65943,-1.59195,0.168354,1.56001,-1.63788,0.233582,1.50079,-1.65172,0.281889,1.86469,-2.1245,0.355125,1.79185,-1.95751,0.142956,1.83877,-2.03338,0.136501,1.63905,-2.21172,0.179749,1.55012,-2.25454,0.3555,1.49314,-2.18347,0.410619,1.46012,-2.19182,0.251283,1.35936,-1.81987,0.054834,1.4247,-1.86391,0.052717,1.45792,-1.91699,0.044514,1.43853,-1.97585,0.230904,1.40602,-2.08591,0.364748,1.11702,-1 [...]
+/*1444*/{0.276828,1.85616,-1.72593,0.348572,1.79033,-1.87722,0.141512,1.74671,-1.64249,0.037003,1.66584,-1.59332,0.160511,1.56575,-1.64001,0.225115,1.50567,-1.65237,0.282492,1.86761,-2.12336,0.354807,1.79391,-1.95558,0.143367,1.84043,-2.03444,0.142406,1.64003,-2.21152,0.18655,1.55184,-2.25416,0.364154,1.5013,-2.18077,0.419603,1.46924,-2.18724,0.250173,1.36149,-1.81914,0.055433,1.42587,-1.86541,0.052624,1.45938,-1.91874,0.045301,1.43966,-1.97757,0.231328,1.40629,-2.08666,0.369875,1.12567, [...]
+/*1445*/{0.27369,1.8594,-1.72546,0.347737,1.79369,-1.87634,0.136284,1.75194,-1.64342,0.030643,1.67228,-1.59454,0.153895,1.57227,-1.64079,0.21821,1.51155,-1.65247,0.285183,1.87094,-2.12193,0.354877,1.79671,-1.95376,0.144582,1.84213,-2.03609,0.146873,1.64081,-2.21083,0.194589,1.55447,-2.25405,0.370653,1.50768,-2.17768,0.427328,1.4787,-2.18315,0.248441,1.36425,-1.81793,0.05472,1.42831,-1.86774,0.053085,1.46141,-1.92045,0.045722,1.44187,-1.97977,0.232474,1.40702,-2.08697,0.374277,1.13413,-1. [...]
+/*1446*/{0.27115,1.86267,-1.72503,0.346802,1.79693,-1.87391,0.132572,1.75717,-1.64386,0.023616,1.67819,-1.59641,0.14784,1.57794,-1.64117,0.210655,1.51729,-1.65273,0.287351,1.8747,-2.12142,0.355,1.80011,-1.95223,0.144738,1.8438,-2.0372,0.150834,1.64182,-2.21068,0.201245,1.5572,-2.25378,0.378829,1.51639,-2.17408,0.435325,1.48898,-2.1781,0.251011,1.3682,-1.81814,0.054218,1.43027,-1.86961,0.053029,1.46394,-1.92217,0.045914,1.44449,-1.98166,0.232996,1.4076,-2.08723,0.37988,1.14242,-1.75902,0. [...]
+/*1447*/{0.267052,1.86619,-1.7247,0.34559,1.80049,-1.87256,0.127121,1.76303,-1.64457,0.017668,1.68585,-1.59776,0.140751,1.58561,-1.6427,0.204803,1.52415,-1.65302,0.288766,1.87841,-2.12,0.354806,1.80353,-1.95042,0.146292,1.84706,-2.03795,0.157263,1.64483,-2.20984,0.207299,1.56037,-2.25378,0.384169,1.52425,-2.16999,0.443575,1.49855,-2.172,0.249967,1.37181,-1.81811,0.053904,1.43314,-1.87239,0.054251,1.4667,-1.92444,0.047043,1.44745,-1.98429,0.234376,1.40861,-2.08695,0.382159,1.14986,-1.7604 [...]
+/*1448*/{0.264026,1.87039,-1.72498,0.344934,1.80435,-1.8709,0.123179,1.76863,-1.64548,0.012218,1.69314,-1.60012,0.134387,1.59192,-1.64382,0.197758,1.52995,-1.65426,0.289525,1.88287,-2.11945,0.355431,1.80751,-1.94878,0.147065,1.84947,-2.03871,0.160509,1.64605,-2.20963,0.21407,1.5638,-2.2538,0.39049,1.53363,-2.16608,0.450509,1.50951,-2.16791,0.248929,1.37548,-1.81825,0.05501,1.43568,-1.87454,0.054704,1.46869,-1.92649,0.048144,1.44978,-1.9862,0.236389,1.40958,-2.08727,0.386748,1.1584,-1.761 [...]
+/*1449*/{0.26034,1.87419,-1.72514,0.344351,1.80845,-1.86962,0.118593,1.77409,-1.64634,0.006534,1.70001,-1.60214,0.128616,1.59933,-1.64547,0.191662,1.5375,-1.65466,0.291198,1.88731,-2.11845,0.355286,1.81141,-1.94743,0.148126,1.85203,-2.03999,0.163483,1.65002,-2.21106,0.220263,1.56813,-2.25381,0.39544,1.54226,-2.16222,0.456747,1.52076,-2.16275,0.249568,1.37943,-1.81869,0.056113,1.43914,-1.87681,0.055701,1.4721,-1.92807,0.049052,1.4527,-1.98871,0.238168,1.40982,-2.08741,0.388846,1.1662,-1.7 [...]
+/*1450*/{0.257774,1.87901,-1.72488,0.344189,1.81351,-1.86852,0.11456,1.78001,-1.64687,0.000628,1.70729,-1.60376,0.122733,1.60698,-1.64707,0.185666,1.54443,-1.65471,0.292472,1.89193,-2.11696,0.356969,1.81486,-1.9453,0.148496,1.85519,-2.04123,0.16753,1.65365,-2.21211,0.22654,1.57264,-2.25375,0.401241,1.55265,-2.15798,0.462006,1.53242,-2.1573,0.2505,1.38443,-1.81958,0.056138,1.44331,-1.87891,0.057044,1.47537,-1.93029,0.052002,1.45653,-1.99052,0.238544,1.41086,-2.08805,0.391184,1.17378,-1.76 [...]
+/*1451*/{0.254782,1.88317,-1.72499,0.343906,1.81764,-1.86673,0.110966,1.78583,-1.64804,-0.004205,1.71445,-1.60656,0.118507,1.61446,-1.64853,0.179592,1.55173,-1.65575,0.292994,1.89693,-2.11645,0.356519,1.81927,-1.94366,0.148919,1.85861,-2.04152,0.172426,1.65715,-2.21359,0.233069,1.57708,-2.25404,0.405598,1.56239,-2.15409,0.467285,1.54387,-2.15264,0.25164,1.38929,-1.82086,0.056607,1.44748,-1.88182,0.058462,1.47899,-1.93232,0.053383,1.4601,-1.99247,0.240375,1.41159,-2.08859,0.392217,1.1815, [...]
+/*1452*/{0.25182,1.88734,-1.72544,0.342682,1.82317,-1.86553,0.107084,1.79121,-1.64957,-0.009679,1.72165,-1.60903,0.112682,1.62149,-1.65057,0.174187,1.55914,-1.65685,0.294662,1.90188,-2.11595,0.356411,1.82411,-1.94239,0.150372,1.86229,-2.04213,0.177728,1.66195,-2.21498,0.239287,1.58228,-2.25416,0.409456,1.57132,-2.14978,0.4719,1.55576,-2.14759,0.252966,1.39314,-1.8216,0.057987,1.45158,-1.88245,0.05997,1.48264,-1.93476,0.056252,1.46333,-1.99413,0.242271,1.41255,-2.08853,0.396299,1.18919,-1 [...]
+/*1453*/{0.25006,1.8917,-1.72553,0.34256,1.82625,-1.86368,0.104034,1.79735,-1.65051,-0.012106,1.72943,-1.61136,0.107882,1.62855,-1.65244,0.169248,1.56548,-1.65793,0.295151,1.9074,-2.11462,0.356607,1.82866,-1.94105,0.150774,1.86595,-2.04335,0.181796,1.6668,-2.21652,0.245075,1.58747,-2.2542,0.412497,1.58101,-2.1456,0.475504,1.56698,-2.14297,0.255256,1.3982,-1.82281,0.059542,1.45587,-1.8847,0.062041,1.48564,-1.93599,0.058448,1.46682,-1.9959,0.243881,1.41367,-2.08916,0.398259,1.19577,-1.7620 [...]
+/*1454*/{0.247772,1.89656,-1.72562,0.342492,1.83232,-1.863,0.100314,1.80311,-1.65235,-0.01752,1.73678,-1.61469,0.102663,1.63545,-1.654,0.163691,1.5719,-1.65781,0.29637,1.91246,-2.11302,0.356341,1.83337,-1.9397,0.151843,1.86877,-2.04418,0.186628,1.67244,-2.2178,0.251596,1.59322,-2.25465,0.415932,1.59102,-2.14153,0.478225,1.57802,-2.13807,0.255796,1.40246,-1.82373,0.060846,1.46089,-1.88554,0.06284,1.4902,-1.93847,0.06151,1.47084,-1.99753,0.246279,1.41447,-2.08927,0.399674,1.20289,-1.76217, [...]
+/*1455*/{0.24629,1.90054,-1.72591,0.342097,1.83653,-1.86198,0.097043,1.80901,-1.65384,-0.022808,1.7434,-1.61755,0.099222,1.64251,-1.65649,0.160355,1.57956,-1.65939,0.297933,1.91793,-2.1123,0.356521,1.83833,-1.93849,0.152625,1.87333,-2.04508,0.190556,1.67694,-2.2188,0.257294,1.59907,-2.25464,0.417941,1.59942,-2.13776,0.48104,1.58816,-2.13326,0.256815,1.40667,-1.82492,0.062473,1.46573,-1.88671,0.065613,1.49411,-1.94022,0.063549,1.47494,-1.99872,0.248052,1.41554,-2.08969,0.402214,1.20976,-1 [...]
+/*1456*/{0.244845,1.90489,-1.7263,0.340537,1.84088,-1.86113,0.094405,1.81514,-1.65577,-0.02666,1.75114,-1.62075,0.094246,1.64956,-1.65811,0.154655,1.58537,-1.66067,0.298506,1.92323,-2.11136,0.356083,1.84268,-1.93754,0.153171,1.87682,-2.04534,0.195713,1.68294,-2.22093,0.262553,1.60479,-2.25566,0.420208,1.60941,-2.13427,0.483355,1.59865,-2.12832,0.258741,1.41135,-1.82592,0.064695,1.47041,-1.88778,0.068015,1.49786,-1.9425,0.066251,1.47909,-1.99948,0.249921,1.41717,-2.0897,0.403419,1.21592,- [...]
+/*1457*/{0.243095,1.90911,-1.72651,0.339808,1.84667,-1.86076,0.091747,1.82053,-1.65744,-0.029601,1.75811,-1.6244,0.089984,1.65497,-1.65912,0.150787,1.59099,-1.66147,0.299891,1.9278,-2.11028,0.356431,1.84783,-1.93643,0.15484,1.87997,-2.04624,0.200015,1.68797,-2.22265,0.267519,1.61064,-2.25601,0.421618,1.61775,-2.13103,0.484488,1.60846,-2.12459,0.259384,1.41622,-1.8264,0.065958,1.47489,-1.88865,0.068623,1.50191,-1.9442,0.068982,1.48294,-2.00052,0.251976,1.41816,-2.08961,0.403579,1.22179,-1 [...]
+/*1458*/{0.24168,1.91321,-1.72737,0.340146,1.85115,-1.86047,0.088606,1.82657,-1.65946,-0.034365,1.76533,-1.62819,0.086657,1.66147,-1.6615,0.147258,1.59757,-1.66149,0.300955,1.93282,-2.10953,0.355944,1.85203,-1.93537,0.155351,1.88326,-2.04659,0.203525,1.69305,-2.2243,0.272829,1.61665,-2.25706,0.422582,1.62574,-2.12772,0.486585,1.61803,-2.12077,0.261046,1.42042,-1.82734,0.068367,1.48057,-1.88882,0.071538,1.50585,-1.94575,0.07184,1.48618,-2.00192,0.254024,1.41993,-2.08999,0.404893,1.2269,-1 [...]
+/*1459*/{0.240515,1.91695,-1.72759,0.33881,1.85495,-1.85966,0.087211,1.83211,-1.66131,-0.035672,1.77201,-1.63208,0.083041,1.66672,-1.66251,0.142474,1.60287,-1.6624,0.301728,1.9375,-2.10809,0.355451,1.85599,-1.93532,0.155971,1.8863,-2.04725,0.207245,1.69824,-2.22534,0.277601,1.62202,-2.25742,0.424059,1.63411,-2.12493,0.487686,1.62741,-2.11776,0.263127,1.42494,-1.82779,0.070593,1.48532,-1.89001,0.074237,1.51027,-1.94693,0.074545,1.49043,-2.00248,0.25547,1.42084,-2.08978,0.407552,1.23156,-1 [...]
+/*1460*/{0.239461,1.92052,-1.72812,0.338958,1.85836,-1.85915,0.084314,1.83756,-1.66362,-0.039281,1.77791,-1.63588,0.079387,1.67189,-1.66399,0.139833,1.60792,-1.66269,0.302616,1.94126,-2.10707,0.356204,1.85994,-1.93389,0.156864,1.88942,-2.04909,0.211553,1.7031,-2.22698,0.281639,1.62734,-2.25859,0.424283,1.64031,-2.12207,0.488411,1.63569,-2.11367,0.263827,1.42938,-1.82832,0.072576,1.49109,-1.89043,0.076213,1.51439,-1.94837,0.076215,1.49353,-2.00291,0.257996,1.42209,-2.08971,0.407158,1.2351 [...]
+/*1461*/{0.238315,1.92413,-1.72854,0.338841,1.86167,-1.85801,0.082572,1.84264,-1.66587,-0.042939,1.78415,-1.64003,0.076435,1.67746,-1.66577,0.136103,1.61232,-1.66341,0.30384,1.94575,-2.10643,0.355921,1.86388,-1.93392,0.158014,1.89242,-2.04951,0.212351,1.70718,-2.2283,0.285743,1.63234,-2.25937,0.424637,1.64677,-2.12048,0.489219,1.64396,-2.11066,0.264833,1.43298,-1.82963,0.07522,1.49541,-1.89052,0.077039,1.51779,-1.95026,0.079007,1.49732,-2.0035,0.25905,1.42342,-2.08964,0.408738,1.23905,-1 [...]
+/*1462*/{0.237772,1.92731,-1.72906,0.339027,1.86524,-1.85804,0.080388,1.84807,-1.66794,-0.045643,1.78943,-1.64419,0.07369,1.6817,-1.66692,0.133076,1.61605,-1.66279,0.303626,1.94924,-2.10566,0.356645,1.86697,-1.93259,0.159029,1.89576,-2.0503,0.215799,1.71201,-2.23024,0.28866,1.63746,-2.26027,0.425757,1.65392,-2.11834,0.489224,1.6512,-2.10788,0.266622,1.43644,-1.82993,0.077497,1.49977,-1.89052,0.079456,1.52172,-1.95057,0.081421,1.50119,-2.00421,0.261065,1.42445,-2.08906,0.408558,1.24155,-1 [...]
+/*1463*/{0.236564,1.9305,-1.72945,0.337739,1.86802,-1.85779,0.078642,1.85262,-1.67058,-0.048952,1.79402,-1.64747,0.070835,1.68599,-1.66807,0.13041,1.62066,-1.66291,0.304797,1.95251,-2.10477,0.356201,1.87039,-1.93244,0.160098,1.89804,-2.05069,0.219644,1.71617,-2.23217,0.291154,1.64142,-2.26095,0.425927,1.65898,-2.11656,0.489824,1.65799,-2.10462,0.267463,1.4403,-1.831,0.079621,1.50406,-1.8901,0.081889,1.52566,-1.95079,0.084523,1.50415,-2.00435,0.261773,1.42576,-2.0885,0.40873,1.24301,-1.76 [...]
+/*1464*/{0.236251,1.93343,-1.73022,0.332166,1.8746,-1.85882,0.076564,1.85709,-1.67297,-0.05055,1.79852,-1.65122,0.068517,1.69039,-1.66969,0.127948,1.62409,-1.66354,0.305634,1.95579,-2.10351,0.35606,1.87356,-1.93246,0.161169,1.90069,-2.05172,0.22243,1.72081,-2.23413,0.294123,1.64566,-2.26185,0.425511,1.66544,-2.11537,0.489375,1.66345,-2.10196,0.268851,1.44287,-1.83092,0.081449,1.5083,-1.89023,0.083969,1.52881,-1.95154,0.085775,1.50802,-2.00471,0.263224,1.42702,-2.08809,0.409181,1.24499,-1 [...]
+/*1465*/{0.236013,1.93622,-1.73064,0.332918,1.87759,-1.85872,0.075681,1.86086,-1.67504,-0.054614,1.80258,-1.6549,0.06657,1.69398,-1.67065,0.126181,1.62664,-1.66351,0.306072,1.9587,-2.1029,0.356407,1.87659,-1.93215,0.161989,1.90342,-2.05309,0.224924,1.72441,-2.23587,0.296545,1.64983,-2.26259,0.425757,1.67006,-2.11426,0.489173,1.66823,-2.09977,0.269035,1.4472,-1.83266,0.082096,1.51192,-1.89021,0.084431,1.53215,-1.95186,0.087659,1.51085,-2.00488,0.265378,1.42762,-2.08743,0.409279,1.24522,-1 [...]
+/*1466*/{0.23521,1.93883,-1.73141,0.338303,1.87657,-1.85679,0.074534,1.865,-1.67679,-0.057064,1.8059,-1.65894,0.06492,1.69749,-1.67215,0.12431,1.63067,-1.66332,0.306617,1.96094,-2.1024,0.356717,1.87837,-1.93145,0.162857,1.90555,-2.05341,0.229162,1.72921,-2.23813,0.298751,1.65346,-2.26344,0.425889,1.67306,-2.11389,0.48885,1.67276,-2.09822,0.268243,1.44927,-1.83313,0.084062,1.51487,-1.88974,0.086608,1.53554,-1.95182,0.089276,1.51419,-2.00526,0.265697,1.43015,-2.08719,0.407338,1.24529,-1.77 [...]
+/*1467*/{0.235276,1.94156,-1.73138,0.332776,1.88189,-1.85831,0.073726,1.86836,-1.678,-0.057147,1.80909,-1.66199,0.062382,1.69999,-1.67316,0.121288,1.6333,-1.66348,0.307186,1.96343,-2.10233,0.356435,1.88162,-1.93149,0.1636,1.90829,-2.0543,0.232346,1.73262,-2.23965,0.301178,1.65652,-2.26524,0.425794,1.67658,-2.11295,0.488593,1.67629,-2.09641,0.267524,1.4519,-1.83518,0.085021,1.51789,-1.8895,0.086274,1.53871,-1.95172,0.090602,1.51685,-2.00518,0.266359,1.43151,-2.08689,0.406665,1.24261,-1.77 [...]
+/*1468*/{0.235204,1.94389,-1.73217,0.338106,1.88224,-1.85695,0.072436,1.8707,-1.68151,-0.06083,1.81147,-1.66634,0.061107,1.70336,-1.67534,0.120221,1.63537,-1.6635,0.307081,1.96549,-2.10193,0.356225,1.88389,-1.93126,0.164783,1.91005,-2.05404,0.234532,1.7354,-2.24068,0.302431,1.65887,-2.26587,0.426205,1.67942,-2.11296,0.487872,1.67915,-2.09536,0.271584,1.45497,-1.83414,0.085931,1.52116,-1.88943,0.087733,1.54158,-1.95128,0.09116,1.52059,-2.00472,0.266774,1.4341,-2.08691,0.406185,1.24105,-1. [...]
+/*1469*/{0.23495,1.94608,-1.73281,0.339368,1.88264,-1.85646,0.071991,1.87335,-1.68327,-0.060292,1.81364,-1.66953,0.059325,1.70484,-1.67615,0.118448,1.63691,-1.66369,0.307682,1.96727,-2.10185,0.357222,1.88446,-1.93085,0.165035,1.91274,-2.0542,0.236818,1.73831,-2.24264,0.303043,1.6609,-2.2666,0.425406,1.68079,-2.11226,0.487378,1.68069,-2.09415,0.271362,1.45641,-1.83517,0.087053,1.52326,-1.88945,0.088369,1.54404,-1.95092,0.091746,1.52248,-2.00409,0.26755,1.43621,-2.08662,0.405611,1.23735,-1 [...]
+/*1470*/{0.235207,1.94814,-1.73343,0.33913,1.88348,-1.85621,0.07083,1.8751,-1.68577,-0.06119,1.81631,-1.6731,0.05831,1.70707,-1.67757,0.117125,1.6378,-1.66379,0.307479,1.96836,-2.10143,0.357891,1.88604,-1.93102,0.165479,1.91409,-2.05455,0.238646,1.74095,-2.2428,0.304949,1.6629,-2.26788,0.425613,1.68273,-2.11225,0.486577,1.68118,-2.09293,0.271981,1.45858,-1.83573,0.088218,1.52491,-1.88873,0.088911,1.54605,-1.95078,0.093187,1.52466,-2.00343,0.268052,1.43852,-2.0866,0.403134,1.23419,-1.7768 [...]
+/*1471*/{0.23482,1.94958,-1.73377,0.333491,1.88835,-1.85816,0.070639,1.87667,-1.68714,-0.063172,1.81753,-1.67648,0.058073,1.70777,-1.67917,0.115609,1.63785,-1.66408,0.307955,1.96946,-2.10142,0.357211,1.88828,-1.93142,0.165195,1.91632,-2.05476,0.240436,1.74273,-2.24415,0.30538,1.66365,-2.26899,0.425429,1.68504,-2.11232,0.48613,1.68176,-2.09232,0.272241,1.46005,-1.83627,0.088042,1.5266,-1.88774,0.089467,1.54788,-1.95023,0.093197,1.52611,-2.00331,0.266906,1.44105,-2.08673,0.401378,1.22886,- [...]
+/*1472*/{0.235442,1.95107,-1.73465,0.333471,1.8892,-1.85848,0.07097,1.87803,-1.68955,-0.062599,1.81807,-1.679,0.057135,1.70843,-1.67999,0.114301,1.63776,-1.66319,0.308013,1.97014,-2.10131,0.357768,1.88922,-1.93199,0.165949,1.91747,-2.05464,0.241923,1.74434,-2.24471,0.304994,1.66448,-2.2696,0.424648,1.68486,-2.11237,0.484844,1.68138,-2.09248,0.272231,1.46132,-1.83634,0.088699,1.52705,-1.88738,0.089706,1.54979,-1.94933,0.092324,1.52773,-2.0025,0.267628,1.44291,-2.08683,0.399227,1.22379,-1. [...]
+/*1473*/{0.235205,1.952,-1.73515,0.340097,1.88765,-1.85738,0.070611,1.87888,-1.69083,-0.062937,1.81918,-1.68129,0.057465,1.70841,-1.68115,0.114243,1.6374,-1.66287,0.307973,1.97016,-2.10131,0.357671,1.88936,-1.93135,0.166383,1.91873,-2.05487,0.240524,1.74503,-2.24436,0.304491,1.66485,-2.27063,0.423024,1.68309,-2.11278,0.484735,1.68006,-2.09303,0.272985,1.46299,-1.83726,0.089269,1.52811,-1.88724,0.089261,1.55111,-1.94886,0.09297,1.52896,-2.00161,0.268033,1.44494,-2.08666,0.396209,1.21798,- [...]
+/*1474*/{0.236002,1.95249,-1.73536,0.339782,1.88857,-1.85774,0.070563,1.87896,-1.69266,-0.06218,1.81894,-1.6838,0.056745,1.70731,-1.68186,0.113996,1.63704,-1.66257,0.307737,1.9703,-2.10137,0.357524,1.88998,-1.93165,0.166042,1.91945,-2.05463,0.23945,1.74533,-2.24433,0.302908,1.66418,-2.27097,0.423241,1.68223,-2.11344,0.483803,1.67815,-2.09333,0.27227,1.46338,-1.83782,0.089195,1.52816,-1.88678,0.089128,1.55197,-1.9488,0.093059,1.53004,-2.00131,0.268148,1.44626,-2.08683,0.392637,1.2127,-1.7 [...]
+/*1475*/{0.236215,1.95264,-1.7354,0.339634,1.88902,-1.85806,0.071049,1.87863,-1.69347,-0.061947,1.81741,-1.68577,0.056803,1.70601,-1.68257,0.113593,1.63526,-1.66228,0.307739,1.97007,-2.10139,0.357463,1.8901,-1.93162,0.165573,1.9196,-2.05478,0.237609,1.74595,-2.24426,0.300477,1.66358,-2.27129,0.422091,1.67977,-2.11398,0.482807,1.6751,-2.0939,0.274224,1.46426,-1.83881,0.089797,1.52777,-1.88549,0.088927,1.55225,-1.94827,0.092721,1.53054,-2.00004,0.268686,1.44737,-2.08708,0.390024,1.20588,-1 [...]
+/*1476*/{0.236881,1.95248,-1.73549,0.340111,1.88895,-1.85825,0.071303,1.87792,-1.69478,-0.060962,1.81584,-1.68836,0.057173,1.70397,-1.6828,0.11327,1.63301,-1.66194,0.307234,1.96948,-2.1018,0.357428,1.89007,-1.93166,0.165469,1.91953,-2.05456,0.236168,1.74555,-2.24397,0.298587,1.66268,-2.27197,0.420974,1.67691,-2.11508,0.481366,1.67147,-2.09503,0.275326,1.46429,-1.83886,0.090188,1.52713,-1.88507,0.088665,1.55196,-1.94753,0.093442,1.52981,-1.99958,0.268668,1.44863,-2.08727,0.387195,1.19931, [...]
+/*1477*/{0.238024,1.95202,-1.73556,0.339493,1.88846,-1.85862,0.071384,1.87676,-1.69634,-0.060995,1.81289,-1.69042,0.058412,1.70166,-1.68302,0.113773,1.63029,-1.66002,0.30751,1.96839,-2.10219,0.35757,1.88921,-1.93253,0.165273,1.91947,-2.05432,0.232431,1.74437,-2.24336,0.294664,1.66132,-2.27166,0.420102,1.67338,-2.11626,0.479943,1.66769,-2.09587,0.275446,1.464,-1.83931,0.089891,1.52558,-1.88448,0.088982,1.55037,-1.94695,0.092909,1.52868,-1.99924,0.26961,1.44905,-2.08775,0.382236,1.19303,-1 [...]
+/*1478*/{0.238632,1.95114,-1.73541,0.340247,1.88746,-1.85911,0.072381,1.87531,-1.69703,-0.059345,1.81074,-1.69157,0.059072,1.69931,-1.68288,0.114511,1.62766,-1.65974,0.307073,1.96684,-2.10223,0.356992,1.88917,-1.93277,0.16492,1.91946,-2.05399,0.227834,1.74325,-2.24192,0.290682,1.65953,-2.27132,0.41743,1.67065,-2.11755,0.478186,1.66262,-2.09735,0.275376,1.46383,-1.84018,0.090156,1.52392,-1.88328,0.088941,1.54997,-1.94733,0.092501,1.52729,-1.9988,0.271202,1.44886,-2.08744,0.37921,1.18604,- [...]
+/*1479*/{0.239439,1.9493,-1.73529,0.339791,1.88667,-1.85932,0.073201,1.87287,-1.69726,-0.058265,1.80808,-1.69291,0.060103,1.69581,-1.68235,0.115774,1.62461,-1.65809,0.306,1.96521,-2.10233,0.356648,1.88776,-1.93297,0.164114,1.91809,-2.05356,0.223954,1.74208,-2.24127,0.287106,1.65814,-2.27105,0.415871,1.66517,-2.11873,0.476038,1.65731,-2.09915,0.275076,1.46238,-1.84037,0.089848,1.52188,-1.88359,0.089454,1.548,-1.94675,0.09215,1.52537,-1.99804,0.27149,1.44854,-2.0877,0.37661,1.17994,-1.7805 [...]
+/*1480*/{0.239802,1.94739,-1.73492,0.340165,1.88507,-1.85978,0.073805,1.87002,-1.69705,-0.055933,1.80367,-1.6928,0.060832,1.69212,-1.68141,0.117169,1.62132,-1.65738,0.305764,1.96357,-2.10307,0.355767,1.88674,-1.93334,0.164018,1.91679,-2.05267,0.218628,1.74042,-2.23965,0.282359,1.65593,-2.27099,0.414423,1.66077,-2.12078,0.473726,1.65101,-2.10117,0.275185,1.46098,-1.84089,0.088951,1.51984,-1.88384,0.088905,1.54547,-1.94654,0.092635,1.52326,-1.99823,0.271873,1.44782,-2.08777,0.371962,1.1744 [...]
+/*1481*/{0.24081,1.94487,-1.73468,0.340831,1.8825,-1.85964,0.074331,1.866,-1.69699,-0.055006,1.79928,-1.69322,0.062567,1.68802,-1.6806,0.119344,1.61724,-1.65625,0.304118,1.96111,-2.10351,0.356004,1.88436,-1.93359,0.163121,1.91559,-2.0517,0.21448,1.73815,-2.23911,0.277418,1.65409,-2.2704,0.411051,1.6554,-2.1221,0.471529,1.6444,-2.10354,0.274873,1.45819,-1.84057,0.089755,1.51644,-1.88311,0.088569,1.54316,-1.94669,0.092226,1.52124,-1.99775,0.271958,1.44679,-2.08753,0.369344,1.16797,-1.77943 [...]
+/*1482*/{0.241165,1.94194,-1.7344,0.340122,1.88091,-1.86074,0.074929,1.86178,-1.69681,-0.054144,1.79399,-1.69284,0.063811,1.68332,-1.67951,0.120698,1.61294,-1.65502,0.30324,1.95839,-2.10424,0.355646,1.88325,-1.93375,0.162394,1.9134,-2.05076,0.208882,1.737,-2.23743,0.271829,1.65103,-2.27002,0.408501,1.64939,-2.12376,0.468604,1.63733,-2.10583,0.274839,1.45595,-1.84078,0.090208,1.51359,-1.88305,0.08802,1.54048,-1.94628,0.092083,1.51811,-1.99802,0.272312,1.4454,-2.0878,0.366288,1.16359,-1.77 [...]
+/*1483*/{0.24115,1.93858,-1.73461,0.339986,1.87859,-1.86089,0.075311,1.85766,-1.69648,-0.052582,1.78797,-1.69168,0.066099,1.6781,-1.67809,0.123517,1.60814,-1.65402,0.302909,1.95523,-2.10528,0.355338,1.8803,-1.93405,0.161836,1.91169,-2.05088,0.210422,1.73405,-2.23603,0.266906,1.6485,-2.26944,0.405217,1.64269,-2.12554,0.46569,1.63017,-2.10943,0.27424,1.45297,-1.83982,0.089499,1.51062,-1.88281,0.088991,1.53705,-1.94587,0.091487,1.5154,-1.99856,0.272177,1.44339,-2.08731,0.362634,1.15988,-1.7 [...]
+/*1484*/{0.241082,1.93487,-1.73451,0.340175,1.87446,-1.86145,0.075946,1.85186,-1.69569,-0.050674,1.78154,-1.68971,0.068201,1.67287,-1.67646,0.126674,1.60318,-1.65268,0.302254,1.95204,-2.10591,0.354579,1.87743,-1.93431,0.161911,1.90912,-2.05012,0.198977,1.73121,-2.23511,0.2612,1.64512,-2.26941,0.40184,1.63667,-2.12742,0.462076,1.62186,-2.1118,0.274034,1.44968,-1.83987,0.088358,1.50849,-1.88291,0.087933,1.53347,-1.9468,0.092423,1.51135,-1.99882,0.273333,1.44163,-2.08733,0.36003,1.15652,-1. [...]
+/*1485*/{0.241854,1.93064,-1.73384,0.340302,1.8722,-1.862,0.077356,1.84596,-1.694,-0.04881,1.77511,-1.68767,0.071358,1.66762,-1.67519,0.129625,1.59828,-1.65123,0.301851,1.94799,-2.10631,0.355087,1.87429,-1.93434,0.160779,1.90615,-2.04958,0.192118,1.72758,-2.23376,0.2557,1.64138,-2.26908,0.397694,1.62872,-2.12918,0.458469,1.61284,-2.11547,0.273486,1.44644,-1.8393,0.089218,1.50469,-1.88256,0.087514,1.53045,-1.94741,0.091662,1.50793,-1.99905,0.273101,1.43928,-2.08638,0.356231,1.15395,-1.773 [...]
+/*1486*/{0.242087,1.92585,-1.7334,0.339371,1.86716,-1.86205,0.077474,1.84008,-1.6936,-0.047276,1.76834,-1.68564,0.073819,1.66136,-1.67267,0.133384,1.59263,-1.64964,0.30049,1.94412,-2.10741,0.354094,1.87086,-1.93533,0.160323,1.90195,-2.04879,0.187284,1.72471,-2.23258,0.249046,1.63729,-2.26869,0.393547,1.62101,-2.13161,0.454328,1.60447,-2.11849,0.2734,1.44345,-1.83862,0.090365,1.50197,-1.88205,0.087567,1.52627,-1.9467,0.091441,1.50341,-1.99909,0.273856,1.43652,-2.08602,0.35326,1.15158,-1.7 [...]
+/*1487*/{0.242045,1.92082,-1.73356,0.339976,1.86417,-1.86335,0.078556,1.83338,-1.69149,-0.045633,1.76064,-1.68341,0.076569,1.65528,-1.67091,0.13734,1.58779,-1.64785,0.299517,1.93931,-2.10875,0.354243,1.86647,-1.93532,0.159659,1.89873,-2.04789,0.182046,1.721,-2.23098,0.242928,1.63315,-2.26845,0.388438,1.61219,-2.13324,0.450234,1.59466,-2.12198,0.273089,1.44004,-1.83786,0.087391,1.49724,-1.88328,0.086385,1.52276,-1.94732,0.089539,1.49905,-2.00029,0.273836,1.4334,-2.08502,0.352091,1.14813,- [...]
+/*1488*/{0.242134,1.91532,-1.73284,0.33979,1.85917,-1.86363,0.079674,1.82538,-1.69022,-0.044156,1.75278,-1.68107,0.079881,1.64783,-1.6689,0.140641,1.58149,-1.64699,0.299516,1.93468,-2.10967,0.354334,1.86333,-1.93573,0.158758,1.89416,-2.04712,0.176002,1.7176,-2.22982,0.235997,1.6289,-2.26787,0.383838,1.60377,-2.13586,0.445959,1.58428,-2.12491,0.272637,1.43652,-1.83715,0.085989,1.49251,-1.88376,0.086669,1.51799,-1.94777,0.090232,1.49483,-2.00018,0.274014,1.43034,-2.08441,0.349222,1.14494,- [...]
+/*1489*/{0.242649,1.9097,-1.7325,0.339698,1.85479,-1.8643,0.081146,1.81776,-1.68802,-0.042024,1.74405,-1.67816,0.083787,1.64074,-1.66613,0.146008,1.57513,-1.64492,0.298626,1.93017,-2.11102,0.353751,1.85873,-1.9364,0.159318,1.89013,-2.04695,0.170435,1.71269,-2.22905,0.230836,1.62386,-2.26807,0.380646,1.59597,-2.13857,0.441584,1.57384,-2.12892,0.270963,1.43288,-1.83601,0.084617,1.48851,-1.88427,0.085253,1.51383,-1.94788,0.088321,1.49015,-2.00129,0.274302,1.42718,-2.08395,0.346642,1.14105,- [...]
+/*1490*/{0.242966,1.90339,-1.73204,0.339496,1.84954,-1.86414,0.082513,1.80925,-1.68657,-0.039964,1.73486,-1.67444,0.08669,1.6338,-1.66382,0.150629,1.56888,-1.64305,0.297122,1.92422,-2.11213,0.354662,1.85355,-1.93698,0.158325,1.88553,-2.04662,0.16491,1.70866,-2.22795,0.223757,1.61903,-2.26789,0.374911,1.58603,-2.14057,0.436558,1.56325,-2.13232,0.269423,1.42997,-1.83491,0.084008,1.48362,-1.88512,0.085095,1.50949,-1.94789,0.088364,1.48442,-2.00199,0.273843,1.42363,-2.08311,0.344188,1.13748, [...]
+/*1491*/{0.243433,1.89744,-1.73134,0.340618,1.84312,-1.86442,0.08436,1.8014,-1.68406,-0.037849,1.72579,-1.67082,0.091449,1.62601,-1.66145,0.155878,1.56223,-1.64099,0.296166,1.91823,-2.11377,0.354171,1.8487,-1.93795,0.158391,1.88046,-2.04634,0.160146,1.70445,-2.22736,0.217557,1.61354,-2.26748,0.370059,1.57574,-2.14352,0.430765,1.55129,-2.13638,0.268807,1.4269,-1.83365,0.082639,1.47857,-1.88516,0.083989,1.50502,-1.9488,0.086822,1.47907,-2.0021,0.273799,1.42012,-2.08249,0.34148,1.1321,-1.76 [...]
+/*1492*/{0.243562,1.89095,-1.73073,0.339813,1.83744,-1.86446,0.086635,1.79258,-1.68004,-0.036107,1.7153,-1.66673,0.095499,1.61764,-1.65848,0.160849,1.5559,-1.63876,0.296121,1.91245,-2.11496,0.354573,1.84289,-1.9382,0.156992,1.87468,-2.04599,0.158146,1.69778,-2.22579,0.210483,1.60828,-2.2672,0.364704,1.56527,-2.14662,0.424023,1.53969,-2.14097,0.267847,1.42268,-1.83183,0.080513,1.47359,-1.88636,0.082426,1.49996,-1.94879,0.085991,1.47398,-2.00212,0.273144,1.41622,-2.08146,0.339489,1.12718,- [...]
+/*1493*/{0.24473,1.88437,-1.72984,0.341199,1.83185,-1.86533,0.08873,1.78323,-1.6783,-0.032539,1.70538,-1.6625,0.099946,1.6098,-1.65577,0.166594,1.54959,-1.63782,0.295077,1.90599,-2.11655,0.354255,1.83755,-1.93947,0.156979,1.86969,-2.04633,0.153731,1.69233,-2.22546,0.204215,1.6027,-2.26749,0.359062,1.55418,-2.14901,0.418656,1.52775,-2.14513,0.266907,1.4192,-1.82995,0.079593,1.46712,-1.88698,0.0821,1.49425,-1.94855,0.084754,1.46804,-2.00394,0.272207,1.41213,-2.08091,0.336408,1.12104,-1.767 [...]
+/*1494*/{0.245267,1.87792,-1.72898,0.342276,1.82731,-1.86645,0.091348,1.77366,-1.67588,-0.028626,1.69536,-1.65749,0.105572,1.60096,-1.65302,0.173495,1.54174,-1.63488,0.294171,1.89944,-2.11834,0.355115,1.8321,-1.94083,0.156523,1.86335,-2.04584,0.148965,1.68723,-2.22493,0.196786,1.59678,-2.26704,0.35372,1.54359,-2.1519,0.412191,1.51502,-2.14965,0.265836,1.41522,-1.82785,0.077578,1.46193,-1.88847,0.081161,1.48962,-1.94848,0.083176,1.46247,-2.00503,0.271272,1.40805,-2.08081,0.332621,1.11437, [...]
+/*1495*/{0.246411,1.87148,-1.72859,0.342278,1.82035,-1.86618,0.094462,1.76402,-1.67344,-0.024067,1.68493,-1.65252,0.111205,1.5934,-1.64968,0.180313,1.53564,-1.63322,0.294556,1.89332,-2.11949,0.355164,1.82554,-1.94206,0.155754,1.85817,-2.0462,0.144463,1.68178,-2.22467,0.190492,1.5909,-2.26747,0.347851,1.53227,-2.15504,0.406111,1.50206,-2.15404,0.264371,1.41166,-1.8261,0.075207,1.45757,-1.88866,0.079289,1.48361,-1.94847,0.080706,1.45687,-2.00553,0.270704,1.40367,-2.07996,0.327866,1.10804,- [...]
+/*1496*/{0.247534,1.86554,-1.72746,0.343629,1.8144,-1.8669,0.098099,1.7538,-1.66998,-0.019846,1.6739,-1.64765,0.118923,1.58535,-1.64637,0.187708,1.52874,-1.63079,0.293852,1.88754,-2.12167,0.355477,1.82005,-1.94288,0.155718,1.85249,-2.04613,0.139144,1.67645,-2.22454,0.18302,1.58462,-2.26724,0.341969,1.52115,-2.15839,0.3993,1.48922,-2.1578,0.261566,1.40639,-1.82477,0.072935,1.45273,-1.88992,0.07572,1.47844,-1.94996,0.07854,1.45104,-2.00648,0.269209,1.39871,-2.07901,0.321606,1.10261,-1.7618 [...]
+/*1497*/{0.249216,1.86048,-1.72727,0.343536,1.80929,-1.86736,0.10282,1.74386,-1.66676,-0.013191,1.66304,-1.64169,0.126247,1.57757,-1.64285,0.196063,1.52313,-1.62925,0.293738,1.88231,-2.1231,0.35669,1.81391,-1.9445,0.154537,1.84801,-2.04536,0.135588,1.67083,-2.224,0.176266,1.57872,-2.26733,0.336599,1.51065,-2.16128,0.392088,1.4767,-2.16166,0.25975,1.40265,-1.82313,0.068395,1.44786,-1.89165,0.072697,1.47258,-1.95118,0.076301,1.44635,-2.00774,0.269202,1.39416,-2.07751,0.31539,1.10008,-1.757 [...]
+/*1498*/{0.250281,1.85576,-1.72674,0.344971,1.8026,-1.86889,0.107562,1.73373,-1.66339,-0.006028,1.65168,-1.63518,0.132671,1.5692,-1.63911,0.20484,1.51705,-1.62647,0.292874,1.87766,-2.12559,0.357454,1.80841,-1.94637,0.153841,1.84536,-2.04413,0.13008,1.66754,-2.22389,0.169528,1.57292,-2.26665,0.330069,1.4988,-2.16454,0.384504,1.46381,-2.16597,0.256333,1.39858,-1.82243,0.063417,1.44392,-1.89314,0.069853,1.46817,-1.95337,0.073239,1.44016,-2.00845,0.26826,1.39004,-2.07645,0.311416,1.09799,-1. [...]
+/*1499*/{0.251886,1.85161,-1.72618,0.34481,1.79844,-1.87048,0.113053,1.72452,-1.6606,0.002948,1.64144,-1.6293,0.143058,1.56259,-1.63551,0.214694,1.51248,-1.625,0.291746,1.87438,-2.12719,0.357883,1.80305,-1.94886,0.152845,1.84378,-2.042,0.129287,1.66231,-2.22366,0.163137,1.56773,-2.26493,0.323538,1.48931,-2.16834,0.376887,1.45182,-2.17026,0.252969,1.39466,-1.82047,0.060113,1.44079,-1.89568,0.066552,1.46359,-1.95398,0.070148,1.43511,-2.01034,0.266525,1.38576,-2.07577,0.308562,1.09645,-1.75 [...]
+/*1500*/{0.253624,1.848,-1.72612,0.345677,1.79452,-1.87246,0.11928,1.71601,-1.65817,0.01094,1.63107,-1.62337,0.152134,1.55579,-1.63157,0.225127,1.50766,-1.62244,0.290736,1.87118,-2.12891,0.358652,1.79888,-1.94977,0.15193,1.84246,-2.0407,0.126086,1.65917,-2.22338,0.157198,1.56364,-2.26271,0.316569,1.47812,-2.17164,0.369342,1.44007,-2.17479,0.250026,1.39243,-1.81863,0.056159,1.43741,-1.89803,0.064792,1.46039,-1.95519,0.066986,1.42942,-2.01178,0.264065,1.38169,-2.07498,0.302979,1.09704,-1.7 [...]
+/*1501*/{0.254891,1.8449,-1.72664,0.346516,1.78946,-1.87368,0.125053,1.71045,-1.65649,0.02008,1.62199,-1.61796,0.162192,1.55088,-1.62995,0.236227,1.50444,-1.62047,0.290369,1.86858,-2.1293,0.358969,1.79546,-1.9528,0.151865,1.84164,-2.03986,0.123257,1.65707,-2.22116,0.154105,1.56295,-2.25933,0.309728,1.46917,-2.17445,0.360443,1.429,-2.17982,0.24669,1.39141,-1.8172,0.053921,1.43501,-1.90038,0.061903,1.45972,-1.95702,0.063918,1.42537,-2.01436,0.262377,1.37849,-2.0746,0.299401,1.09793,-1.7534 [...]
+/*1502*/{0.256188,1.84241,-1.72765,0.34578,1.78729,-1.87627,0.13048,1.70582,-1.65454,0.030672,1.61495,-1.61284,0.17379,1.54669,-1.62709,0.24724,1.50168,-1.61839,0.290563,1.86633,-2.12948,0.359087,1.79246,-1.95451,0.151815,1.84042,-2.03992,0.118913,1.65658,-2.21862,0.148393,1.56325,-2.25611,0.303222,1.46339,-2.17645,0.351396,1.42018,-2.18476,0.243229,1.38985,-1.81544,0.051504,1.43424,-1.90168,0.06066,1.45911,-1.95807,0.061479,1.42277,-2.01606,0.26046,1.37644,-2.07502,0.29516,1.09663,-1.75 [...]
+/*1503*/{0.257863,1.84011,-1.72879,0.346012,1.78402,-1.87764,0.136116,1.70221,-1.65336,0.038934,1.60964,-1.60901,0.185625,1.54476,-1.62601,0.260367,1.50049,-1.61692,0.291011,1.86418,-2.12993,0.358121,1.79003,-1.95542,0.150986,1.84023,-2.04075,0.114928,1.65724,-2.21522,0.143005,1.56456,-2.25348,0.295077,1.45781,-2.17878,0.342193,1.41273,-2.18894,0.239981,1.38871,-1.81283,0.049401,1.43304,-1.90382,0.059245,1.45856,-1.9593,0.059364,1.42185,-2.01797,0.258706,1.37375,-2.07524,0.289357,1.0948, [...]
+/*1504*/{0.259702,1.83874,-1.7296,0.345615,1.78399,-1.87952,0.141305,1.70037,-1.65226,0.049518,1.60589,-1.60422,0.197312,1.54272,-1.62388,0.27222,1.50096,-1.61528,0.290097,1.8626,-2.13063,0.357763,1.78895,-1.95614,0.151026,1.84002,-2.04117,0.110699,1.659,-2.21158,0.134657,1.56587,-2.25159,0.286739,1.45426,-2.18194,0.33299,1.40701,-2.19273,0.23701,1.38811,-1.81089,0.047805,1.43354,-1.90469,0.057066,1.45867,-1.95986,0.058494,1.42111,-2.01853,0.255841,1.37203,-2.07575,0.282557,1.0927,-1.752 [...]
+/*1505*/{0.262205,1.83791,-1.7301,0.344688,1.78118,-1.87984,0.146309,1.69805,-1.65098,0.058558,1.60205,-1.6005,0.208254,1.54198,-1.62223,0.285927,1.50287,-1.61451,0.289909,1.86165,-2.13126,0.357797,1.78754,-1.95669,0.150958,1.8396,-2.04221,0.106021,1.6613,-2.20791,0.12807,1.56807,-2.24997,0.27834,1.45084,-2.18434,0.323043,1.40296,-2.19587,0.23408,1.3878,-1.80982,0.04653,1.43301,-1.90609,0.055931,1.45757,-1.96068,0.057047,1.42128,-2.01952,0.254553,1.3713,-2.07617,0.278769,1.08872,-1.75063 [...]
+/*1506*/{0.263823,1.83741,-1.7309,0.344802,1.78078,-1.88118,0.151623,1.69729,-1.64893,0.069291,1.59898,-1.59581,0.220271,1.5431,-1.62007,0.298406,1.50611,-1.61447,0.289261,1.86126,-2.13216,0.354647,1.78671,-1.95834,0.150932,1.83935,-2.0441,0.100566,1.66526,-2.20482,0.119747,1.56998,-2.24918,0.269792,1.45082,-2.18725,0.313147,1.40085,-2.19857,0.23164,1.38789,-1.80871,0.045262,1.43351,-1.90625,0.055293,1.45731,-1.96112,0.055638,1.4221,-2.01984,0.253763,1.37131,-2.07641,0.271253,1.08566,-1. [...]
+/*1507*/{0.265875,1.83766,-1.73165,0.344945,1.78029,-1.88175,0.156912,1.69717,-1.64741,0.079876,1.59657,-1.59154,0.231095,1.54515,-1.6192,0.311695,1.51039,-1.61453,0.288084,1.86099,-2.13282,0.353608,1.78642,-1.95912,0.150174,1.83915,-2.04448,0.095867,1.67044,-2.20164,0.111479,1.57305,-2.24835,0.261773,1.4502,-2.18966,0.302692,1.39986,-2.20143,0.229937,1.38755,-1.80852,0.04441,1.43317,-1.90629,0.05318,1.45661,-1.96129,0.05527,1.42238,-2.01879,0.252589,1.37247,-2.07646,0.263745,1.08237,-1. [...]
+/*1508*/{0.267712,1.83841,-1.73251,0.342965,1.78123,-1.88351,0.162061,1.69776,-1.6462,0.088681,1.59436,-1.58724,0.242386,1.54842,-1.61787,0.324873,1.51613,-1.61502,0.286821,1.8614,-2.13395,0.353134,1.78643,-1.96034,0.14895,1.84037,-2.045,0.08985,1.67557,-2.19917,0.102849,1.57722,-2.24777,0.2526,1.45176,-2.1914,0.292784,1.40139,-2.20327,0.227745,1.38766,-1.80729,0.042433,1.433,-1.90739,0.052055,1.45762,-1.96177,0.054457,1.42364,-2.01864,0.251508,1.37386,-2.07635,0.257079,1.08148,-1.75227, [...]
+/*1509*/{0.269437,1.84088,-1.73381,0.344362,1.78228,-1.88479,0.167072,1.69887,-1.64544,0.098278,1.59412,-1.5837,0.253491,1.55205,-1.61701,0.336936,1.52273,-1.61677,0.284797,1.86207,-2.1343,0.352913,1.78709,-1.96164,0.147608,1.84148,-2.04516,0.083844,1.67986,-2.19635,0.095211,1.58203,-2.24727,0.243845,1.45388,-2.19205,0.282251,1.40319,-2.20404,0.227116,1.38847,-1.80731,0.042275,1.43361,-1.90813,0.051871,1.45804,-1.9615,0.053744,1.42389,-2.01852,0.25124,1.37564,-2.07639,0.248836,1.08009,-1 [...]
+/*1510*/{0.271532,1.84266,-1.73516,0.343187,1.7836,-1.88624,0.17222,1.70066,-1.64405,0.108377,1.59432,-1.58114,0.263651,1.55722,-1.61701,0.348254,1.53023,-1.61826,0.282592,1.86369,-2.13596,0.35183,1.78857,-1.96338,0.147257,1.8424,-2.04489,0.077149,1.68636,-2.19497,0.087142,1.58804,-2.24649,0.235485,1.45819,-2.19328,0.272664,1.40618,-2.20434,0.224844,1.38862,-1.80581,0.042003,1.43571,-1.90837,0.051376,1.45947,-1.9617,0.052987,1.42556,-2.01973,0.250826,1.37813,-2.07589,0.239621,1.07728,-1. [...]
+/*1511*/{0.274066,1.84574,-1.73649,0.343635,1.78551,-1.8874,0.177943,1.70289,-1.64342,0.117261,1.59466,-1.57755,0.273435,1.5628,-1.61678,0.359071,1.53896,-1.61995,0.280328,1.86542,-2.13637,0.350699,1.79045,-1.96507,0.145998,1.84493,-2.04494,0.070815,1.69226,-2.19449,0.07899,1.59405,-2.24588,0.226746,1.46327,-2.19314,0.262565,1.41112,-2.2049,0.224076,1.38943,-1.8058,0.041112,1.43656,-1.90862,0.050767,1.46128,-1.96175,0.052152,1.42705,-2.01937,0.250552,1.38046,-2.07585,0.231982,1.07586,-1. [...]
+/*1512*/{0.276185,1.84891,-1.73767,0.343068,1.78773,-1.88873,0.183428,1.70508,-1.6427,0.126143,1.59662,-1.57597,0.282471,1.56864,-1.61746,0.368738,1.54829,-1.62264,0.277192,1.8679,-2.13747,0.350483,1.79234,-1.96656,0.144589,1.84633,-2.04463,0.064462,1.69834,-2.19403,0.071087,1.60005,-2.2455,0.217591,1.46911,-2.19268,0.253334,1.41598,-2.20428,0.224194,1.38993,-1.80617,0.040929,1.4381,-1.90837,0.050963,1.46245,-1.96213,0.052361,1.42906,-2.02026,0.250487,1.38216,-2.07513,0.221314,1.07444,-1 [...]
+/*1513*/{0.277638,1.85291,-1.73947,0.343738,1.79078,-1.89076,0.190035,1.70803,-1.64206,0.13622,1.5987,-1.57383,0.291193,1.57623,-1.61884,0.378012,1.55816,-1.62531,0.274958,1.87119,-2.13846,0.349684,1.79519,-1.96822,0.143585,1.8488,-2.04362,0.057868,1.70383,-2.19323,0.06347,1.60611,-2.24514,0.209504,1.47496,-2.19244,0.243679,1.42234,-2.20369,0.223964,1.39082,-1.80507,0.04122,1.43947,-1.90883,0.051419,1.46458,-1.96119,0.052353,1.43095,-2.02073,0.250499,1.38479,-2.07458,0.21381,1.07434,-1.7 [...]
+/*1514*/{0.280198,1.85679,-1.74081,0.344319,1.79421,-1.89268,0.196437,1.71072,-1.64164,0.143564,1.60156,-1.57164,0.299195,1.58395,-1.62028,0.385829,1.56892,-1.62838,0.271593,1.87453,-2.13946,0.348709,1.79834,-1.97031,0.14145,1.85102,-2.04249,0.052179,1.71042,-2.19338,0.055665,1.61235,-2.24469,0.200141,1.48144,-2.19166,0.235208,1.42906,-2.2029,0.22458,1.39178,-1.80516,0.04174,1.44238,-1.90848,0.051882,1.46684,-1.96032,0.053329,1.43337,-2.02083,0.251428,1.38788,-2.0743,0.206596,1.0741,-1.7 [...]
+/*1515*/{0.282185,1.86164,-1.74261,0.344894,1.79802,-1.89494,0.202213,1.71395,-1.64171,0.151268,1.60457,-1.57091,0.30629,1.59173,-1.62241,0.393718,1.57892,-1.63225,0.268295,1.87787,-2.1411,0.348534,1.80165,-1.97243,0.140172,1.85322,-2.04157,0.046351,1.71707,-2.19345,0.048543,1.61914,-2.24435,0.191857,1.48841,-2.19077,0.227128,1.43693,-2.20209,0.225507,1.39245,-1.80394,0.042673,1.44473,-1.9088,0.052852,1.46887,-1.95882,0.054369,1.43586,-2.02037,0.252376,1.3909,-2.07317,0.19918,1.0729,-1.7 [...]
+/*1516*/{0.283874,1.86623,-1.74439,0.345278,1.80187,-1.89683,0.209191,1.71775,-1.64162,0.15994,1.6083,-1.56941,0.312952,1.59997,-1.62502,0.400432,1.58925,-1.63722,0.265272,1.88194,-2.14149,0.34828,1.80571,-1.97486,0.137927,1.8557,-2.04109,0.040208,1.72373,-2.19327,0.041139,1.62547,-2.24369,0.184405,1.4968,-2.18972,0.219766,1.44473,-2.2009,0.225961,1.39326,-1.80349,0.044084,1.44682,-1.90849,0.053746,1.47205,-1.95851,0.054375,1.43965,-2.01972,0.253061,1.39441,-2.07237,0.19181,1.07209,-1.76 [...]
+/*1517*/{0.285878,1.87083,-1.74603,0.345309,1.80607,-1.89902,0.216027,1.72204,-1.64186,0.166857,1.61268,-1.56897,0.318332,1.60808,-1.62703,0.406508,1.59966,-1.64189,0.261531,1.88619,-2.14257,0.347726,1.80976,-1.97719,0.137009,1.85849,-2.03925,0.036835,1.73086,-2.19336,0.034875,1.63236,-2.24241,0.177339,1.50515,-2.18836,0.21275,1.45253,-2.19991,0.226853,1.39401,-1.80258,0.044762,1.45004,-1.90761,0.055032,1.47431,-1.95678,0.05595,1.44281,-2.01886,0.253252,1.39785,-2.07125,0.183925,1.07328, [...]
+/*1518*/{0.287506,1.87662,-1.74848,0.346482,1.80961,-1.9009,0.221779,1.72652,-1.64221,0.174568,1.61762,-1.56813,0.323522,1.61618,-1.62951,0.411572,1.60956,-1.64447,0.258917,1.89025,-2.14285,0.347182,1.81404,-1.97926,0.135165,1.86165,-2.03864,0.032747,1.7366,-2.19285,0.02798,1.63908,-2.24156,0.169892,1.51331,-2.18648,0.205735,1.46091,-2.19899,0.229004,1.39437,-1.80161,0.046774,1.45378,-1.90551,0.055872,1.47741,-1.95463,0.056828,1.4469,-2.01849,0.254259,1.40194,-2.07005,0.180024,1.07467,-1 [...]
+/*1519*/{0.289631,1.8821,-1.75036,0.346651,1.81423,-1.90335,0.227555,1.73092,-1.64321,0.180096,1.62175,-1.56765,0.328494,1.62484,-1.63288,0.415906,1.62053,-1.65027,0.25606,1.89437,-2.14314,0.347354,1.81785,-1.9817,0.134351,1.86479,-2.03674,0.028569,1.74428,-2.19273,0.021822,1.64646,-2.24036,0.163091,1.52187,-2.18474,0.199261,1.46908,-2.198,0.229992,1.39515,-1.80184,0.048978,1.45635,-1.90487,0.05769,1.48081,-1.95314,0.058155,1.4518,-2.01777,0.254882,1.40572,-2.06929,0.172952,1.0756,-1.773 [...]
+/*1520*/{0.291406,1.88803,-1.75218,0.348282,1.8199,-1.90595,0.232085,1.73618,-1.64369,0.187779,1.62741,-1.56644,0.332108,1.63295,-1.6348,0.42022,1.63006,-1.65399,0.252905,1.89827,-2.14366,0.346331,1.82291,-1.98396,0.133519,1.8682,-2.03568,0.023207,1.7521,-2.19362,0.015853,1.65329,-2.23852,0.156858,1.53003,-2.18281,0.193838,1.47738,-2.19717,0.232868,1.3953,-1.80131,0.051414,1.46006,-1.90356,0.058932,1.48458,-1.95107,0.059778,1.45644,-2.01794,0.255438,1.41057,-2.06851,0.168192,1.07787,-1.7 [...]
+/*1521*/{0.292636,1.89368,-1.7546,0.348305,1.82441,-1.90852,0.236326,1.74191,-1.64407,0.19249,1.63257,-1.56616,0.335775,1.64135,-1.63833,0.423076,1.63944,-1.6588,0.250487,1.90263,-2.14364,0.345503,1.82707,-1.98609,0.132147,1.87187,-2.03538,0.019682,1.7589,-2.19282,0.011055,1.66067,-2.23755,0.150491,1.53835,-2.18154,0.188783,1.48587,-2.19573,0.234314,1.39595,-1.80236,0.053027,1.46416,-1.90222,0.060129,1.48836,-1.94917,0.06203,1.4613,-2.01783,0.256074,1.41508,-2.06814,0.161414,1.07967,-1.7 [...]
+/*1522*/{0.294077,1.89857,-1.75618,0.348307,1.82927,-1.91102,0.240888,1.74723,-1.64434,0.196895,1.63815,-1.5657,0.338947,1.64903,-1.64006,0.425751,1.6491,-1.66262,0.248083,1.90652,-2.14446,0.345033,1.83198,-1.9878,0.131334,1.87547,-2.03455,0.014895,1.76483,-2.19182,0.005436,1.66806,-2.23585,0.145201,1.54488,-2.17882,0.182946,1.49353,-2.19502,0.23677,1.39656,-1.80338,0.055117,1.4674,-1.90149,0.061169,1.49214,-1.94783,0.063403,1.46671,-2.01625,0.257205,1.41996,-2.0673,0.157301,1.08101,-1.7 [...]
+/*1523*/{0.296007,1.90469,-1.75832,0.34895,1.83398,-1.91329,0.244296,1.75295,-1.64529,0.201523,1.64418,-1.56561,0.341394,1.6579,-1.64326,0.4279,1.65819,-1.66702,0.246167,1.91035,-2.14474,0.344568,1.83653,-1.98963,0.12995,1.87903,-2.0338,0.012522,1.77012,-2.19011,0.001085,1.67443,-2.23439,0.14036,1.55301,-2.17723,0.178752,1.5014,-2.19435,0.238798,1.39732,-1.80425,0.057736,1.4723,-1.89974,0.063325,1.49909,-1.94681,0.064376,1.47223,-2.01562,0.25898,1.42578,-2.06709,0.1515,1.08414,-1.78351,0 [...]
+/*1524*/{0.296888,1.90968,-1.76045,0.348676,1.83799,-1.91501,0.247961,1.75874,-1.64584,0.205817,1.64952,-1.56529,0.343678,1.6643,-1.64496,0.429464,1.66632,-1.6707,0.244225,1.91402,-2.14495,0.344064,1.84094,-1.99142,0.129348,1.8828,-2.03332,0.009612,1.77605,-2.18865,-0.003613,1.68116,-2.23253,0.135546,1.56015,-2.17568,0.174636,1.50855,-2.19349,0.240438,1.39859,-1.80528,0.059117,1.47581,-1.89861,0.065259,1.50076,-1.94527,0.066195,1.47791,-2.01446,0.25965,1.43034,-2.06708,0.146627,1.0876,-1 [...]
+/*1525*/{0.298094,1.91471,-1.76207,0.348918,1.84333,-1.91707,0.250505,1.76433,-1.64679,0.209481,1.65473,-1.56515,0.345743,1.67099,-1.64702,0.431049,1.67452,-1.67462,0.242499,1.9174,-2.14641,0.343698,1.84556,-1.99339,0.128738,1.88608,-2.03327,0.007921,1.78101,-2.18664,-0.007281,1.68695,-2.23126,0.131909,1.56689,-2.17395,0.170907,1.51505,-2.19267,0.243388,1.40062,-1.80712,0.062045,1.47949,-1.89711,0.066943,1.50538,-1.9436,0.068755,1.48358,-2.01376,0.259498,1.4363,-2.06736,0.144661,1.09135, [...]
+/*1526*/{0.300209,1.91993,-1.76389,0.34999,1.8482,-1.91904,0.25268,1.76973,-1.64733,0.212848,1.66004,-1.56503,0.347155,1.67826,-1.6501,0.432759,1.68271,-1.67772,0.241504,1.92121,-2.14634,0.342931,1.84971,-1.99481,0.127904,1.89036,-2.03311,0.004937,1.78641,-2.18567,-0.010693,1.69293,-2.22942,0.128955,1.57268,-2.1725,0.167642,1.52145,-2.19231,0.244587,1.40225,-1.80887,0.064389,1.48423,-1.89528,0.068724,1.50972,-1.94193,0.069744,1.48943,-2.01273,0.259825,1.44099,-2.06791,0.142136,1.09439,-1 [...]
+/*1527*/{0.301023,1.92418,-1.76534,0.350132,1.85229,-1.92047,0.253308,1.77518,-1.64803,0.215986,1.66546,-1.56503,0.348695,1.68443,-1.65188,0.433629,1.6899,-1.68191,0.239994,1.92408,-2.147,0.342351,1.85443,-1.99649,0.127486,1.89403,-2.03307,0.003276,1.79171,-2.18324,-0.01438,1.69848,-2.22759,0.126007,1.5788,-2.17147,0.165054,1.52661,-2.1918,0.246884,1.4046,-1.81002,0.065927,1.48822,-1.89415,0.0689,1.51408,-1.9403,0.070889,1.49486,-2.01167,0.260161,1.44631,-2.06802,0.138867,1.09804,-1.7865 [...]
+/*1528*/{0.302387,1.92834,-1.76655,0.35047,1.85626,-1.92201,0.255818,1.78038,-1.64864,0.217763,1.67011,-1.56447,0.350438,1.69023,-1.65405,0.433994,1.69761,-1.6871,0.238452,1.92726,-2.14719,0.342261,1.85808,-1.99655,0.127146,1.89685,-2.03326,0.001611,1.79576,-2.18123,-0.017118,1.70352,-2.22627,0.124792,1.58305,-2.17084,0.163242,1.53183,-2.19135,0.247969,1.40686,-1.81168,0.067159,1.49262,-1.89323,0.069256,1.51847,-1.93898,0.072743,1.50063,-2.01136,0.260723,1.45111,-2.06853,0.135461,1.10132 [...]
+/*1529*/{0.30387,1.93285,-1.76731,0.35161,1.86115,-1.92318,0.257315,1.78526,-1.64919,0.220384,1.67517,-1.56474,0.350417,1.69607,-1.6568,0.434641,1.70341,-1.68876,0.23741,1.93006,-2.14734,0.341993,1.86255,-1.99852,0.12647,1.90037,-2.0334,4.8e-005,1.79984,-2.18001,-0.01916,1.70824,-2.2241,0.122449,1.58822,-2.17028,0.161514,1.53633,-2.1908,0.25043,1.40967,-1.81205,0.06845,1.49719,-1.89197,0.071575,1.52275,-1.93889,0.073821,1.50586,-2.00994,0.260811,1.45543,-2.0686,0.133042,1.10572,-1.78485, [...]
+/*1530*/{0.304611,1.93573,-1.76917,0.351313,1.86511,-1.92499,0.257995,1.79061,-1.6501,0.221978,1.67889,-1.56424,0.351366,1.70038,-1.65838,0.435348,1.70876,-1.69236,0.236606,1.93289,-2.148,0.342485,1.86611,-1.99917,0.125958,1.90302,-2.03318,-0.000468,1.80457,-2.17715,-0.020711,1.71214,-2.22275,0.121312,1.59211,-2.16988,0.160902,1.54038,-2.19082,0.250601,1.41296,-1.81365,0.070063,1.50105,-1.89099,0.072939,1.52635,-1.93759,0.074146,1.51074,-2.00847,0.26194,1.4603,-2.06848,0.131157,1.10964,- [...]
+/*1531*/{0.305216,1.93919,-1.77049,0.351689,1.86932,-1.92574,0.25875,1.79447,-1.65048,0.224231,1.68337,-1.56416,0.35154,1.70423,-1.66016,0.434744,1.71323,-1.69708,0.236354,1.93596,-2.14848,0.342203,1.87001,-2.0003,0.128795,1.90395,-2.0325,-0.000278,1.80791,-2.17547,-0.022168,1.71603,-2.22136,0.120982,1.59525,-2.16976,0.160915,1.54293,-2.19015,0.252101,1.41625,-1.81405,0.071628,1.50488,-1.89089,0.072632,1.53106,-1.93788,0.075025,1.51494,-2.00831,0.262358,1.46445,-2.06828,0.129185,1.11442, [...]
+/*1532*/{0.305929,1.94274,-1.77116,0.352033,1.87363,-1.92671,0.259726,1.7987,-1.65078,0.225438,1.68669,-1.56399,0.351485,1.70878,-1.662,0.434906,1.71796,-1.69971,0.23583,1.93825,-2.14875,0.343285,1.87367,-2.0009,0.129248,1.90687,-2.03294,-0.000981,1.81106,-2.17345,-0.024,1.71916,-2.21988,0.120292,1.59831,-2.16979,0.15968,1.54709,-2.19032,0.25273,1.41927,-1.81436,0.072473,1.50914,-1.89048,0.074217,1.53466,-1.93728,0.07823,1.51809,-2.00582,0.262898,1.46818,-2.06788,0.127103,1.11844,-1.7779 [...]
+/*1533*/{0.306327,1.94509,-1.77191,0.352228,1.87627,-1.92684,0.260741,1.8019,-1.65104,0.22652,1.69005,-1.56308,0.351507,1.71188,-1.66314,0.434559,1.72083,-1.70301,0.236314,1.94099,-2.14961,0.343152,1.87647,-2.00209,0.129483,1.90802,-2.03308,0.000187,1.81369,-2.17101,-0.024324,1.72151,-2.2191,0.120229,1.60019,-2.16954,0.159831,1.54958,-2.19062,0.253439,1.42335,-1.81493,0.073261,1.51268,-1.88999,0.07429,1.53805,-1.9371,0.079313,1.5217,-2.00591,0.262782,1.47181,-2.06821,0.125912,1.12259,-1. [...]
+/*1534*/{0.306465,1.94789,-1.77241,0.352479,1.88112,-1.92856,0.261052,1.80558,-1.65156,0.228693,1.69359,-1.56273,0.351979,1.71398,-1.66514,0.433905,1.72195,-1.70574,0.235799,1.94299,-2.15033,0.343928,1.87928,-2.00238,0.129742,1.91064,-2.03311,-0.000566,1.81555,-2.1695,-0.024122,1.72357,-2.21817,0.121609,1.60302,-2.16905,0.160146,1.55122,-2.19071,0.2536,1.42728,-1.81435,0.073529,1.51605,-1.88955,0.074893,1.54192,-1.93681,0.079358,1.52478,-2.00492,0.263986,1.47459,-2.06772,0.126345,1.12614 [...]
+/*1535*/{0.306826,1.95053,-1.77348,0.35279,1.88342,-1.92922,0.261673,1.80845,-1.65151,0.227528,1.69648,-1.5625,0.351236,1.71628,-1.667,0.432755,1.72259,-1.70764,0.237057,1.94529,-2.15101,0.344555,1.88163,-2.00387,0.130804,1.91264,-2.03332,-0.000356,1.8173,-2.16899,-0.023841,1.7256,-2.21756,0.121926,1.6037,-2.16869,0.161307,1.5531,-2.19086,0.253638,1.43161,-1.81303,0.074147,1.51911,-1.88889,0.075488,1.54558,-1.93676,0.080456,1.52777,-2.0042,0.264302,1.47702,-2.06699,0.127599,1.12955,-1.77 [...]
+/*1536*/{0.306716,1.95284,-1.7737,0.352232,1.88599,-1.92979,0.261529,1.81082,-1.6523,0.22902,1.69992,-1.56148,0.351,1.7174,-1.66774,0.431888,1.72292,-1.70962,0.23725,1.9469,-2.1519,0.345767,1.8833,-2.00419,0.13126,1.91537,-2.03403,0.001501,1.81851,-2.16653,-0.022813,1.72624,-2.21697,0.122709,1.60537,-2.16917,0.16249,1.55402,-2.19168,0.254192,1.43576,-1.81336,0.073571,1.52237,-1.88889,0.074912,1.54809,-1.93618,0.080143,1.53014,-2.00437,0.264859,1.47935,-2.06666,0.129506,1.1318,-1.77229,0. [...]
+/*1537*/{0.307391,1.95389,-1.77432,0.352603,1.88838,-1.93068,0.261623,1.81314,-1.65225,0.227511,1.70195,-1.56116,0.350187,1.71816,-1.66864,0.430862,1.72322,-1.71204,0.23912,1.94858,-2.1528,0.345309,1.88465,-2.00505,0.132732,1.91691,-2.03421,0.002361,1.81927,-2.16654,-0.021779,1.72726,-2.21694,0.125054,1.60635,-2.16967,0.163597,1.55448,-2.19214,0.254483,1.43923,-1.8118,0.074024,1.52491,-1.88914,0.075215,1.55116,-1.93681,0.081429,1.53252,-2.00446,0.2643,1.4811,-2.06601,0.132182,1.13318,-1. [...]
+/*1538*/{0.306338,1.95547,-1.77535,0.352183,1.89092,-1.93184,0.260621,1.81525,-1.65339,0.227415,1.70421,-1.56077,0.348347,1.71851,-1.66962,0.429416,1.72202,-1.71425,0.239454,1.94987,-2.15444,0.34582,1.88593,-2.00572,0.133621,1.91823,-2.03736,0.005237,1.81972,-2.16548,-0.019816,1.72751,-2.2172,0.126737,1.60603,-2.17024,0.166149,1.55429,-2.19287,0.254749,1.44246,-1.81135,0.074443,1.52674,-1.8892,0.075109,1.55354,-1.93693,0.081193,1.53421,-2.00316,0.265976,1.48253,-2.065,0.136089,1.13383,-1 [...]
+/*1539*/{0.306601,1.95651,-1.77621,0.350967,1.89206,-1.93224,0.26068,1.81709,-1.65384,0.227144,1.7068,-1.56132,0.346839,1.71805,-1.67049,0.427102,1.72041,-1.71541,0.241279,1.95111,-2.15517,0.346015,1.88723,-2.00598,0.134161,1.92128,-2.03727,0.004959,1.81981,-2.16496,-0.018115,1.72731,-2.21801,0.129877,1.60555,-2.17055,0.1688,1.55391,-2.19387,0.254724,1.44485,-1.81035,0.074019,1.52857,-1.89019,0.075969,1.55584,-1.93611,0.082303,1.53494,-2.00379,0.267095,1.48315,-2.06405,0.141072,1.13561,- [...]
+/*1540*/{0.306366,1.95782,-1.77722,0.351999,1.89394,-1.93336,0.259891,1.81876,-1.65461,0.224332,1.70772,-1.56136,0.34599,1.7172,-1.67178,0.425758,1.71842,-1.71728,0.242927,1.95173,-2.15615,0.346061,1.88781,-2.00607,0.1344,1.92283,-2.03885,0.00854,1.81913,-2.16486,-0.015221,1.72712,-2.21876,0.131564,1.60469,-2.17118,0.171654,1.55279,-2.19457,0.25555,1.44742,-1.80877,0.074125,1.52929,-1.88998,0.07565,1.55695,-1.93689,0.082121,1.53647,-2.00457,0.268442,1.48375,-2.06291,0.149164,1.13566,-1.7 [...]
+/*1541*/{0.305124,1.95831,-1.77824,0.35054,1.89481,-1.93422,0.258364,1.81974,-1.65518,0.222638,1.70933,-1.56211,0.343806,1.71663,-1.67253,0.423551,1.71558,-1.71789,0.243571,1.95206,-2.15695,0.346195,1.88834,-2.00612,0.136428,1.92403,-2.04102,0.009654,1.81883,-2.16441,-0.012091,1.72618,-2.21967,0.13485,1.60303,-2.17178,0.174391,1.55155,-2.19573,0.254316,1.44915,-1.80763,0.074887,1.53024,-1.89029,0.075303,1.55861,-1.93729,0.082218,1.53641,-2.00473,0.269724,1.48427,-2.06161,0.153314,1.1327, [...]
+/*1542*/{0.304843,1.95836,-1.77925,0.349626,1.89509,-1.93427,0.257058,1.82059,-1.65569,0.220458,1.71026,-1.56201,0.342317,1.71463,-1.67347,0.421191,1.71236,-1.71933,0.244741,1.95197,-2.15795,0.346096,1.88878,-2.00602,0.136569,1.92498,-2.04254,0.01249,1.81824,-2.16417,-0.009115,1.7245,-2.22164,0.137676,1.60159,-2.17364,0.17712,1.54954,-2.19706,0.254472,1.45089,-1.80607,0.075019,1.53018,-1.89063,0.075381,1.55875,-1.93727,0.082722,1.53582,-2.00468,0.270962,1.48377,-2.06055,0.159108,1.13132, [...]
+/*1543*/{0.304106,1.95893,-1.78042,0.348928,1.89547,-1.93457,0.255147,1.82107,-1.65619,0.217828,1.71098,-1.5627,0.338896,1.71255,-1.67447,0.417756,1.70825,-1.72023,0.246845,1.95195,-2.15914,0.346015,1.88959,-2.00639,0.13687,1.92564,-2.04461,0.014833,1.81654,-2.16611,-0.00553,1.72248,-2.22304,0.141441,1.59958,-2.17461,0.180625,1.54758,-2.19867,0.254543,1.45147,-1.80551,0.075485,1.52997,-1.89182,0.075516,1.55859,-1.93777,0.082985,1.53526,-2.00515,0.272601,1.48377,-2.05933,0.166652,1.13011, [...]
+/*1544*/{0.30233,1.9587,-1.78089,0.34716,1.89515,-1.93422,0.252964,1.8219,-1.65727,0.214572,1.71151,-1.56326,0.335595,1.71048,-1.67427,0.415001,1.70428,-1.72093,0.247604,1.95095,-2.15932,0.344836,1.88996,-2.00642,0.137781,1.92569,-2.04664,0.018607,1.8145,-2.16647,-0.001879,1.72012,-2.22548,0.144768,1.59743,-2.17682,0.184359,1.54514,-2.20036,0.254459,1.45177,-1.80455,0.075505,1.5298,-1.89215,0.076404,1.55772,-1.93841,0.084522,1.53335,-2.00549,0.275048,1.48267,-2.0582,0.172807,1.12864,-1.7 [...]
+/*1545*/{0.300869,1.95817,-1.7824,0.346922,1.89507,-1.93429,0.250535,1.82214,-1.65743,0.210874,1.71225,-1.56438,0.332508,1.7073,-1.67496,0.411694,1.69975,-1.72099,0.248338,1.94995,-2.15984,0.343785,1.8891,-2.00618,0.137743,1.92458,-2.04827,0.021014,1.81183,-2.16869,0.00178,1.71704,-2.22845,0.14818,1.5945,-2.17867,0.188584,1.54262,-2.20168,0.25501,1.45181,-1.80396,0.075155,1.52844,-1.8929,0.076827,1.55666,-1.93819,0.085206,1.53189,-2.00645,0.275692,1.48092,-2.05714,0.180525,1.12746,-1.780 [...]
+/*1546*/{0.298826,1.9572,-1.78272,0.346388,1.89486,-1.93446,0.247194,1.82172,-1.65769,0.207099,1.71179,-1.56476,0.329113,1.7041,-1.67511,0.408215,1.69399,-1.72039,0.248636,1.94855,-2.16019,0.343255,1.88839,-2.00634,0.137936,1.9241,-2.05086,0.023197,1.8095,-2.17068,0.005705,1.71326,-2.23112,0.151442,1.59103,-2.18154,0.192595,1.53913,-2.20392,0.256298,1.45105,-1.80355,0.075341,1.52757,-1.89394,0.076913,1.55506,-1.93855,0.084897,1.52999,-2.00779,0.276176,1.47959,-2.05682,0.187653,1.12619,-1 [...]
+/*1547*/{0.296824,1.95628,-1.78285,0.345492,1.89371,-1.93518,0.244012,1.82152,-1.6583,0.203315,1.71099,-1.56543,0.325015,1.7004,-1.67512,0.404681,1.68851,-1.72009,0.249906,1.94687,-2.16116,0.34309,1.88772,-2.00663,0.137106,1.92429,-2.05264,0.025213,1.80593,-2.17305,0.008944,1.70936,-2.23501,0.156272,1.58734,-2.18433,0.196898,1.53593,-2.2053,0.257427,1.45027,-1.80379,0.075185,1.52595,-1.89403,0.078233,1.5525,-1.93923,0.085743,1.5283,-2.00824,0.276444,1.47729,-2.05676,0.194343,1.12294,-1.7 [...]
+/*1548*/{0.294526,1.95528,-1.78354,0.344373,1.89258,-1.93452,0.240122,1.82125,-1.65878,0.199638,1.7099,-1.56653,0.320354,1.69686,-1.67508,0.400709,1.68276,-1.71945,0.250054,1.94454,-2.16155,0.342308,1.88653,-2.00629,0.136769,1.92223,-2.05499,0.027638,1.8019,-2.17665,0.012816,1.70478,-2.23898,0.159072,1.58333,-2.18745,0.201142,1.53217,-2.20788,0.257225,1.44901,-1.80322,0.076794,1.52343,-1.89521,0.078459,1.55085,-1.94004,0.086602,1.52594,-2.00978,0.277501,1.47439,-2.05716,0.199948,1.12067, [...]
+/*1549*/{0.291976,1.95374,-1.78353,0.344356,1.89145,-1.93456,0.235489,1.82034,-1.65862,0.194268,1.70881,-1.56731,0.315606,1.69244,-1.67439,0.396657,1.67606,-1.71822,0.249578,1.94194,-2.16188,0.341573,1.88509,-2.00626,0.136422,1.92008,-2.05683,0.029595,1.79652,-2.17983,0.016346,1.69979,-2.24289,0.163731,1.58014,-2.19132,0.205838,1.52817,-2.20965,0.258067,1.44755,-1.80317,0.076962,1.52141,-1.89505,0.0796,1.5477,-1.93971,0.087816,1.52259,-2.00936,0.277561,1.47111,-2.05774,0.207472,1.11975,- [...]
+/*1550*/{0.289611,1.95239,-1.78335,0.342908,1.88952,-1.93416,0.231842,1.81898,-1.65828,0.189769,1.70809,-1.5691,0.31113,1.68766,-1.67423,0.392274,1.6692,-1.71681,0.250118,1.93959,-2.1624,0.341661,1.88314,-2.00611,0.135661,1.91776,-2.05907,0.031858,1.79189,-2.18367,0.019685,1.69449,-2.24694,0.168095,1.57586,-2.19494,0.210559,1.52434,-2.21143,0.258245,1.44535,-1.80299,0.077243,1.51848,-1.89498,0.080065,1.54526,-1.93987,0.087678,1.51957,-2.01105,0.278311,1.4687,-2.05813,0.214069,1.1163,-1.7 [...]
+/*1551*/{0.287707,1.95017,-1.78289,0.342225,1.88768,-1.93421,0.227968,1.81741,-1.65847,0.184758,1.70642,-1.57047,0.306919,1.68338,-1.6742,0.387915,1.66249,-1.71556,0.250351,1.93625,-2.16258,0.340816,1.88167,-2.00614,0.135549,1.91456,-2.06161,0.033922,1.78589,-2.1875,0.023787,1.68914,-2.2518,0.17257,1.57159,-2.19841,0.215484,1.52,-2.21422,0.259144,1.4431,-1.80345,0.077393,1.51549,-1.89473,0.079805,1.54186,-1.94062,0.087224,1.5164,-2.01088,0.278129,1.46576,-2.05952,0.222281,1.11655,-1.7830 [...]
+/*1552*/{0.284426,1.94812,-1.78289,0.342276,1.88586,-1.93382,0.22345,1.81549,-1.65891,0.178523,1.70422,-1.57206,0.30214,1.67822,-1.6738,0.382746,1.65512,-1.7138,0.250201,1.93299,-2.16319,0.339653,1.87896,-2.00551,0.134326,1.91124,-2.06319,0.034711,1.77967,-2.19277,0.027728,1.68307,-2.25677,0.177218,1.56657,-2.20195,0.221016,1.51549,-2.2157,0.260088,1.44088,-1.80433,0.077659,1.51266,-1.89557,0.079634,1.53825,-1.94116,0.087696,1.51267,-2.01084,0.277078,1.46358,-2.06075,0.222867,1.11186,-1. [...]
+/*1553*/{0.281762,1.94543,-1.7824,0.341822,1.88245,-1.93269,0.219631,1.81325,-1.65839,0.173091,1.70278,-1.57303,0.296485,1.67267,-1.67281,0.377318,1.64768,-1.71181,0.250102,1.92984,-2.16348,0.339352,1.87616,-2.00531,0.134032,1.90703,-2.06502,0.036717,1.77313,-2.19669,0.031479,1.67648,-2.26098,0.182011,1.56106,-2.20511,0.226295,1.51105,-2.21784,0.260713,1.4381,-1.8053,0.078263,1.50871,-1.89591,0.080078,1.53482,-1.94174,0.087075,1.50886,-2.01069,0.275943,1.46112,-2.06248,0.232063,1.10841,- [...]
+/*1554*/{0.278944,1.94266,-1.78164,0.341335,1.87996,-1.93277,0.214841,1.81049,-1.65833,0.16692,1.70095,-1.57379,0.290791,1.66657,-1.672,0.372167,1.63964,-1.71,0.249887,1.92558,-2.16355,0.339016,1.87325,-2.00488,0.132896,1.90279,-2.06617,0.037548,1.76588,-2.20261,0.036006,1.67031,-2.2664,0.187428,1.55536,-2.20809,0.232163,1.5061,-2.21936,0.26233,1.43567,-1.80701,0.078652,1.50399,-1.89546,0.079523,1.53073,-1.94231,0.085792,1.50598,-2.01101,0.275913,1.45864,-2.06432,0.236912,1.10688,-1.7837 [...]
+/*1555*/{0.275926,1.93922,-1.78126,0.340318,1.87602,-1.93196,0.209919,1.80782,-1.65828,0.160656,1.69814,-1.57427,0.285091,1.661,-1.67113,0.365926,1.63171,-1.70785,0.250438,1.9221,-2.16416,0.3388,1.86971,-2.00466,0.131993,1.89784,-2.06873,0.038427,1.75812,-2.20749,0.04023,1.66258,-2.27104,0.19253,1.55045,-2.2106,0.23769,1.50127,-2.22122,0.262239,1.43311,-1.80878,0.078387,1.50004,-1.89502,0.079174,1.5273,-1.94275,0.084935,1.5013,-2.01061,0.275399,1.45604,-2.06639,0.241473,1.10446,-1.78317, [...]
+/*1556*/{0.273326,1.93545,-1.78053,0.34026,1.87232,-1.93122,0.205656,1.80446,-1.65817,0.155078,1.69603,-1.57531,0.279848,1.65513,-1.6699,0.359792,1.62384,-1.70542,0.249989,1.91775,-2.16474,0.33826,1.86588,-2.00468,0.131117,1.89327,-2.06998,0.040401,1.75062,-2.21327,0.04531,1.65557,-2.27599,0.198573,1.54493,-2.21268,0.243744,1.49644,-2.22253,0.263591,1.4301,-1.8108,0.078065,1.49539,-1.89426,0.079169,1.52234,-1.94229,0.084317,1.49719,-2.01065,0.274806,1.45309,-2.06876,0.244742,1.1027,-1.78 [...]
+/*1557*/{0.271209,1.93079,-1.77966,0.3407,1.86769,-1.93013,0.20146,1.80074,-1.65837,0.147264,1.69317,-1.57602,0.274,1.64878,-1.66905,0.353647,1.61517,-1.70249,0.25016,1.9136,-2.16484,0.338678,1.86184,-2.0038,0.130316,1.88821,-2.07175,0.041996,1.74262,-2.21852,0.04978,1.64772,-2.28065,0.204065,1.53921,-2.21496,0.250712,1.49113,-2.22368,0.26462,1.42716,-1.81367,0.077294,1.49047,-1.89427,0.078405,1.51814,-1.94262,0.082353,1.4918,-2.01043,0.27372,1.44988,-2.07049,0.249358,1.10052,-1.78236,0. [...]
+/*1558*/{0.269277,1.92672,-1.77893,0.34016,1.8627,-1.92914,0.19775,1.79678,-1.65821,0.140176,1.6898,-1.577,0.268001,1.64207,-1.66804,0.347251,1.60712,-1.70019,0.250306,1.90897,-2.16509,0.338141,1.8572,-2.00326,0.128863,1.88217,-2.07287,0.044923,1.73386,-2.22389,0.055021,1.63984,-2.28508,0.210136,1.53303,-2.21626,0.257189,1.48565,-2.22529,0.265739,1.42391,-1.81614,0.077903,1.4852,-1.89417,0.077861,1.51372,-1.9428,0.081688,1.48694,-2.00994,0.272743,1.44677,-2.07309,0.253055,1.09767,-1.7808 [...]
+/*1559*/{0.267879,1.92167,-1.77859,0.340672,1.85817,-1.92861,0.193943,1.79262,-1.6584,0.133258,1.68669,-1.57715,0.261476,1.63551,-1.6672,0.340413,1.59839,-1.69744,0.250795,1.90437,-2.16543,0.338862,1.85257,-2.00296,0.128459,1.87604,-2.07406,0.047536,1.72533,-2.22992,0.060992,1.63182,-2.28922,0.215842,1.52691,-2.2175,0.264327,1.48031,-2.22658,0.265704,1.42028,-1.81895,0.077721,1.47992,-1.89399,0.077695,1.50775,-1.9431,0.080853,1.48116,-2.00854,0.272649,1.44362,-2.07624,0.2563,1.09562,-1.7 [...]
+/*1560*/{0.266707,1.91648,-1.77734,0.340025,1.85272,-1.92776,0.189571,1.78755,-1.65862,0.127783,1.68369,-1.57817,0.254351,1.62885,-1.66567,0.333208,1.59081,-1.69533,0.251553,1.89989,-2.16598,0.338681,1.84768,-2.00224,0.127211,1.87028,-2.07531,0.051099,1.71626,-2.235,0.067629,1.62376,-2.29316,0.223014,1.52095,-2.21873,0.271497,1.4751,-2.22785,0.266642,1.41588,-1.82139,0.077948,1.47505,-1.89311,0.075878,1.503,-1.94472,0.078745,1.47628,-2.00779,0.271362,1.44016,-2.07868,0.264545,1.09172,-1. [...]
+/*1561*/{0.26563,1.91044,-1.77678,0.340017,1.84723,-1.92646,0.185995,1.78301,-1.65887,0.121198,1.68012,-1.57934,0.248564,1.62234,-1.66443,0.325964,1.58235,-1.69272,0.252677,1.89537,-2.16633,0.338628,1.84257,-2.0015,0.126954,1.86413,-2.07613,0.055607,1.70728,-2.24157,0.074444,1.61569,-2.29716,0.230256,1.5149,-2.22016,0.279554,1.46949,-2.22881,0.266238,1.41182,-1.82374,0.077834,1.46935,-1.89293,0.075082,1.49632,-1.94568,0.078384,1.4703,-2.00751,0.270742,1.43644,-2.08209,0.268904,1.08829,-1 [...]
+/*1562*/{0.265593,1.905,-1.77603,0.340207,1.84199,-1.92504,0.182428,1.778,-1.65935,0.114078,1.67696,-1.58053,0.242289,1.61614,-1.66255,0.318362,1.57423,-1.6896,0.252333,1.89045,-2.1668,0.339024,1.83745,-2.00102,0.126745,1.85858,-2.07634,0.060054,1.69895,-2.24806,0.082053,1.60714,-2.30042,0.237518,1.50934,-2.22132,0.287166,1.46448,-2.22958,0.267033,1.40678,-1.82506,0.076083,1.46429,-1.89227,0.072951,1.49053,-1.94674,0.076302,1.46532,-2.0072,0.26942,1.43235,-2.08458,0.273946,1.08592,-1.771 [...]
+/*1563*/{0.26507,1.89897,-1.775,0.340694,1.83567,-1.92482,0.17937,1.77291,-1.65984,0.108017,1.67392,-1.58254,0.235811,1.61044,-1.662,0.311235,1.56724,-1.68775,0.254003,1.88637,-2.16772,0.340229,1.83202,-2.00003,0.126377,1.85433,-2.07552,0.064944,1.6917,-2.25434,0.09025,1.59891,-2.30372,0.244926,1.5032,-2.22267,0.295668,1.45907,-2.23072,0.266738,1.40201,-1.82786,0.07429,1.4587,-1.89206,0.072715,1.4845,-1.94668,0.07391,1.46028,-2.00614,0.268367,1.42761,-2.08706,0.277847,1.08191,-1.7709,0.1 [...]
+/*1564*/{0.265048,1.89314,-1.77399,0.339583,1.82996,-1.92292,0.1761,1.76763,-1.66015,0.101184,1.67151,-1.58446,0.229559,1.60471,-1.66128,0.303463,1.56023,-1.6849,0.255274,1.88114,-2.16842,0.34081,1.82633,-1.99993,0.127327,1.85061,-2.07446,0.072656,1.68403,-2.26026,0.098329,1.59093,-2.30671,0.253461,1.49767,-2.22358,0.304541,1.45403,-2.23115,0.266617,1.39676,-1.82981,0.073308,1.45306,-1.8919,0.070357,1.47766,-1.94676,0.070866,1.45602,-2.00633,0.265493,1.42387,-2.08815,0.283616,1.07918,-1. [...]
+/*1565*/{0.264426,1.88761,-1.77319,0.340867,1.82322,-1.9214,0.173223,1.76292,-1.66112,0.094967,1.6693,-1.58572,0.223022,1.59939,-1.65989,0.296191,1.55391,-1.68252,0.257279,1.87724,-2.16955,0.342167,1.82109,-1.99947,0.128391,1.84776,-2.07444,0.080676,1.67672,-2.2656,0.10742,1.58339,-2.30913,0.26173,1.49141,-2.22454,0.313304,1.4495,-2.23205,0.265336,1.39197,-1.8318,0.070775,1.44794,-1.89171,0.067112,1.47214,-1.94621,0.067633,1.45199,-2.00694,0.262029,1.42118,-2.08946,0.287741,1.07621,-1.76 [...]
+/*1566*/{0.265058,1.88258,-1.77183,0.340785,1.81797,-1.9212,0.169978,1.75833,-1.66195,0.08842,1.66735,-1.58841,0.216285,1.59528,-1.65883,0.288929,1.54851,-1.6801,0.259163,1.87321,-2.1707,0.343039,1.81579,-1.99878,0.129698,1.84541,-2.07448,0.09076,1.66895,-2.27041,0.117251,1.57576,-2.31101,0.271319,1.48765,-2.22542,0.323034,1.44549,-2.23232,0.264305,1.38741,-1.83477,0.068236,1.44341,-1.89152,0.064463,1.46745,-1.94611,0.064207,1.44753,-2.00767,0.259582,1.4183,-2.09107,0.293588,1.07432,-1.7 [...]
+/*1567*/{0.265131,1.87787,-1.77088,0.341912,1.81232,-1.92049,0.165534,1.75406,-1.66285,0.081268,1.6658,-1.59044,0.209827,1.5907,-1.65837,0.281731,1.54324,-1.67771,0.261224,1.86945,-2.1711,0.344302,1.81118,-1.99863,0.130489,1.84298,-2.07434,0.099935,1.66323,-2.27481,0.126965,1.56869,-2.31192,0.280375,1.48278,-2.22618,0.332693,1.44229,-2.2325,0.263224,1.38312,-1.8373,0.064215,1.43886,-1.89146,0.062811,1.4639,-1.94586,0.060895,1.44433,-2.00795,0.256035,1.41582,-2.09281,0.299722,1.07314,-1.7 [...]
+/*1568*/{0.264278,1.87404,-1.76972,0.342149,1.80859,-1.91952,0.161385,1.75158,-1.66393,0.073562,1.66581,-1.59334,0.202831,1.58811,-1.65839,0.27436,1.54051,-1.67692,0.262944,1.86617,-2.17037,0.345283,1.80753,-1.99726,0.131263,1.84192,-2.07466,0.112256,1.65779,-2.27819,0.137789,1.56267,-2.3137,0.290529,1.47955,-2.22707,0.343222,1.43998,-2.23279,0.261436,1.37904,-1.83923,0.061754,1.43471,-1.89266,0.061666,1.46282,-1.9458,0.057615,1.44098,-2.00723,0.253248,1.41323,-2.09421,0.304849,1.07489,- [...]
+/*1569*/{0.264168,1.87074,-1.76769,0.341737,1.80558,-1.91841,0.157151,1.75017,-1.66504,0.066955,1.6659,-1.5968,0.195871,1.58577,-1.65756,0.267642,1.5368,-1.67501,0.265754,1.86396,-2.16925,0.3453,1.80443,-1.99696,0.13188,1.83997,-2.07478,0.119115,1.65115,-2.28084,0.148663,1.55794,-2.31391,0.3011,1.47734,-2.22724,0.353906,1.43812,-2.23172,0.259264,1.37526,-1.84066,0.058633,1.43176,-1.89202,0.0581,1.4628,-1.94525,0.054649,1.43904,-2.00568,0.251586,1.41174,-2.09576,0.311894,1.07951,-1.76935, [...]
+/*1570*/{0.26425,1.86845,-1.76621,0.341794,1.80314,-1.91672,0.153074,1.74933,-1.66616,0.058956,1.66649,-1.60018,0.19018,1.5844,-1.65775,0.260582,1.53419,-1.6732,0.26744,1.86222,-2.16757,0.345748,1.80282,-1.99499,0.132126,1.83943,-2.07526,0.128035,1.64735,-2.28179,0.159221,1.554,-2.31299,0.311312,1.47526,-2.22742,0.365014,1.43634,-2.23096,0.25711,1.37209,-1.84108,0.056967,1.43023,-1.89072,0.057408,1.46253,-1.94555,0.05392,1.4382,-2.00401,0.249292,1.41083,-2.09764,0.317211,1.08376,-1.76973 [...]
+/*1571*/{0.264299,1.86655,-1.76492,0.340916,1.80184,-1.91583,0.149003,1.75003,-1.66747,0.0524,1.66822,-1.60378,0.183029,1.58481,-1.65894,0.254498,1.53257,-1.67137,0.270186,1.86113,-2.16658,0.346254,1.80154,-1.99391,0.133658,1.83858,-2.07639,0.136738,1.64592,-2.28241,0.168987,1.55173,-2.31203,0.322166,1.47386,-2.22569,0.37558,1.4369,-2.22969,0.25546,1.36984,-1.84126,0.055592,1.42957,-1.88972,0.055378,1.46215,-1.94589,0.05228,1.43857,-2.00255,0.245612,1.41001,-2.10018,0.323981,1.08958,-1.7 [...]
+/*1572*/{0.263964,1.86562,-1.76403,0.340279,1.8008,-1.91425,0.143641,1.75144,-1.66882,0.046604,1.67144,-1.60799,0.176089,1.58443,-1.65917,0.247602,1.53141,-1.67072,0.272377,1.86006,-2.16513,0.346346,1.80082,-1.99247,0.134588,1.83806,-2.07712,0.141879,1.64177,-2.28191,0.178689,1.55017,-2.31138,0.333248,1.47409,-2.22392,0.386855,1.43819,-2.22775,0.252777,1.36848,-1.84058,0.054371,1.42971,-1.89014,0.054606,1.46116,-1.94618,0.05116,1.43954,-2.0025,0.24675,1.4099,-2.10036,0.327803,1.09793,-1. [...]
+/*1573*/{0.262961,1.86573,-1.76312,0.34005,1.80034,-1.91337,0.138582,1.75284,-1.67075,0.040111,1.67402,-1.61215,0.170763,1.58576,-1.66002,0.240931,1.53222,-1.67045,0.275228,1.85987,-2.16357,0.346684,1.80026,-1.99101,0.135165,1.83785,-2.07783,0.151503,1.6411,-2.28035,0.187074,1.54958,-2.30999,0.344203,1.47641,-2.22181,0.397599,1.44016,-2.22551,0.250205,1.3685,-1.83895,0.052933,1.43048,-1.8906,0.053796,1.46135,-1.94629,0.051239,1.44023,-2.00198,0.245983,1.40999,-2.10068,0.335133,1.10397,-1 [...]
+/*1574*/{0.261148,1.86588,-1.76317,0.339723,1.80033,-1.91168,0.134748,1.75564,-1.6721,0.034669,1.67731,-1.61622,0.164357,1.58649,-1.66053,0.233699,1.53205,-1.67025,0.277206,1.86032,-2.16296,0.345941,1.80016,-1.98895,0.136588,1.83779,-2.07797,0.15478,1.63903,-2.27869,0.194895,1.54943,-2.30882,0.352997,1.47748,-2.21957,0.407129,1.44344,-2.22223,0.247391,1.36872,-1.83732,0.051799,1.43149,-1.89139,0.052364,1.46188,-1.94692,0.050604,1.44121,-2.00273,0.244809,1.40979,-2.1011,0.342995,1.11087,- [...]
+/*1575*/{0.260082,1.86717,-1.76229,0.338927,1.7999,-1.90993,0.129568,1.75839,-1.67311,0.029442,1.68118,-1.6196,0.158452,1.58846,-1.66195,0.22744,1.53333,-1.67012,0.280384,1.86131,-2.16112,0.346333,1.80104,-1.9878,0.138,1.83891,-2.07844,0.159399,1.63826,-2.27666,0.202605,1.54943,-2.30707,0.362225,1.48116,-2.2162,0.416607,1.44831,-2.21866,0.245688,1.37004,-1.83474,0.050597,1.43185,-1.89319,0.051618,1.46279,-1.94759,0.050148,1.44206,-2.00472,0.245284,1.4093,-2.10082,0.34916,1.11835,-1.77325 [...]
+/*1576*/{0.258372,1.86915,-1.76131,0.338727,1.80176,-1.9086,0.125204,1.76155,-1.67451,0.022024,1.68462,-1.62337,0.153642,1.59175,-1.66261,0.22183,1.53567,-1.66973,0.28265,1.86291,-2.16012,0.345839,1.80176,-1.98608,0.138724,1.83951,-2.07881,0.165179,1.63961,-2.27483,0.20979,1.5505,-2.30584,0.370695,1.4851,-2.21293,0.425599,1.45347,-2.21506,0.243227,1.3716,-1.83269,0.050689,1.43307,-1.89435,0.051127,1.46376,-1.94977,0.050302,1.4429,-2.00617,0.245846,1.40912,-2.10039,0.355684,1.12687,-1.773 [...]
+/*1577*/{0.255806,1.87112,-1.76049,0.337949,1.802,-1.90647,0.120985,1.76504,-1.67608,0.01638,1.68904,-1.62656,0.147204,1.59444,-1.66312,0.216073,1.53848,-1.66959,0.285644,1.86505,-2.15855,0.345986,1.80278,-1.98359,0.140338,1.84049,-2.08008,0.170936,1.64016,-2.27155,0.216508,1.55173,-2.30497,0.378298,1.49091,-2.20963,0.434214,1.45906,-2.21018,0.241716,1.37341,-1.83136,0.049665,1.43425,-1.8969,0.050567,1.46451,-1.95043,0.050262,1.44418,-2.00798,0.246314,1.40908,-2.09993,0.36266,1.13534,-1. [...]
+/*1578*/{0.252467,1.87424,-1.76031,0.336819,1.80528,-1.90434,0.1162,1.76867,-1.67697,0.010826,1.69239,-1.62959,0.140501,1.59811,-1.66465,0.209528,1.54204,-1.66978,0.286988,1.86736,-2.15701,0.346468,1.80523,-1.9826,0.141972,1.84205,-2.08097,0.173441,1.6407,-2.26868,0.222465,1.5538,-2.30423,0.384949,1.4962,-2.2055,0.44202,1.46647,-2.20601,0.239844,1.37585,-1.82898,0.049083,1.43581,-1.89781,0.050831,1.46625,-1.95224,0.050559,1.44586,-2.01014,0.246597,1.40892,-2.09987,0.367654,1.14309,-1.771 [...]
+/*1579*/{0.249808,1.87751,-1.75917,0.336456,1.80817,-1.90274,0.111212,1.77251,-1.67893,0.00476,1.69633,-1.63286,0.136027,1.60244,-1.66612,0.203812,1.54547,-1.67009,0.289548,1.87067,-2.15535,0.346786,1.80812,-1.97997,0.142202,1.84355,-2.08146,0.175288,1.64275,-2.26565,0.228399,1.55678,-2.30332,0.39026,1.50289,-2.2018,0.448833,1.47447,-2.20093,0.237913,1.37824,-1.82651,0.049405,1.43739,-1.90004,0.050679,1.46745,-1.95423,0.051317,1.44742,-2.01262,0.247603,1.40876,-2.09923,0.37221,1.15091,-1 [...]
+/*1580*/{0.246346,1.88029,-1.75922,0.336668,1.81031,-1.90048,0.107118,1.77635,-1.67986,-0.001095,1.70062,-1.63575,0.130628,1.60672,-1.66742,0.198755,1.54981,-1.67115,0.291924,1.87419,-2.15372,0.34863,1.81052,-1.97676,0.144141,1.84574,-2.08183,0.178649,1.64432,-2.26155,0.233589,1.56016,-2.30205,0.396183,1.51045,-2.19737,0.454383,1.48238,-2.1957,0.237058,1.38136,-1.82571,0.049043,1.43942,-1.90207,0.052148,1.47008,-1.95593,0.052768,1.44916,-2.01441,0.249387,1.40852,-2.09905,0.374983,1.15897 [...]
+/*1581*/{0.243524,1.8842,-1.7583,0.334944,1.81523,-1.89814,0.10273,1.78055,-1.68101,-0.007229,1.70478,-1.63877,0.125607,1.61193,-1.66894,0.19236,1.55465,-1.67295,0.293676,1.8782,-2.15221,0.348133,1.81411,-1.97512,0.144999,1.84798,-2.08231,0.180731,1.6472,-2.25822,0.238571,1.56474,-2.30104,0.400515,1.51756,-2.1929,0.460137,1.49208,-2.19016,0.23608,1.38396,-1.82428,0.048826,1.44157,-1.90463,0.053576,1.47242,-1.95747,0.053403,1.45181,-2.01753,0.251051,1.40883,-2.09859,0.379323,1.16694,-1.77 [...]
+/*1582*/{0.2393,1.88758,-1.7585,0.334665,1.81922,-1.89646,0.098462,1.78443,-1.68274,-0.011486,1.70931,-1.6415,0.119792,1.61653,-1.67076,0.187316,1.55949,-1.67335,0.295565,1.88189,-2.15062,0.348723,1.81791,-1.97296,0.146451,1.85085,-2.08272,0.182645,1.65054,-2.25513,0.243896,1.56919,-2.29929,0.405943,1.5254,-2.18883,0.465257,1.50068,-2.1844,0.236264,1.3877,-1.82367,0.049506,1.44446,-1.90513,0.053838,1.47461,-1.95898,0.054836,1.45413,-2.01918,0.253705,1.40897,-2.09758,0.381781,1.17476,-1.7 [...]
+/*1583*/{0.236523,1.89156,-1.75797,0.335229,1.82157,-1.89412,0.09378,1.78897,-1.68406,-0.015927,1.71354,-1.64444,0.115718,1.62212,-1.67296,0.182612,1.56517,-1.67577,0.296686,1.88649,-2.1488,0.348283,1.82143,-1.9711,0.147651,1.85315,-2.08326,0.183792,1.65415,-2.25178,0.248362,1.57388,-2.29891,0.409553,1.53413,-2.18523,0.469702,1.51053,-2.179,0.236942,1.39053,-1.8241,0.050506,1.44786,-1.90801,0.055786,1.47741,-1.96107,0.056312,1.45675,-2.02079,0.254784,1.40952,-2.09731,0.382947,1.18208,-1. [...]
+/*1584*/{0.232752,1.89506,-1.75809,0.333713,1.82716,-1.89179,0.09057,1.79267,-1.68534,-0.021664,1.71787,-1.64643,0.109994,1.6276,-1.67506,0.179094,1.57053,-1.6761,0.298542,1.89157,-2.14795,0.348483,1.82532,-1.96856,0.149123,1.85636,-2.08348,0.184009,1.65807,-2.24965,0.252292,1.57879,-2.29766,0.413754,1.54221,-2.17995,0.473508,1.5198,-2.17316,0.237762,1.39492,-1.82484,0.052316,1.45071,-1.90922,0.057012,1.47992,-1.96283,0.059091,1.45925,-2.02176,0.25752,1.41008,-2.09603,0.385343,1.18892,-1 [...]
+/*1585*/{0.230452,1.89915,-1.75741,0.332798,1.83146,-1.8905,0.086112,1.79788,-1.68637,-0.026053,1.72268,-1.64924,0.105805,1.63299,-1.67752,0.174266,1.57621,-1.67713,0.300132,1.896,-2.1459,0.349956,1.83001,-1.96702,0.150296,1.85967,-2.084,0.185848,1.66232,-2.24751,0.25737,1.58399,-2.29684,0.416728,1.55082,-2.17602,0.477431,1.53004,-2.16782,0.239294,1.39946,-1.82619,0.052301,1.45483,-1.91133,0.058345,1.4832,-1.96474,0.061365,1.4622,-2.02355,0.258646,1.41025,-2.09643,0.386857,1.1963,-1.7726 [...]
+/*1586*/{0.227436,1.90275,-1.75748,0.332575,1.83597,-1.88846,0.082626,1.80205,-1.68711,-0.02989,1.72816,-1.65239,0.102035,1.63868,-1.67928,0.169715,1.58218,-1.67867,0.301024,1.90089,-2.1449,0.348843,1.83451,-1.96505,0.152164,1.8628,-2.08488,0.189041,1.66655,-2.24652,0.261686,1.58895,-2.2955,0.418682,1.5587,-2.17056,0.480005,1.54001,-2.16209,0.241434,1.40374,-1.82653,0.053832,1.45867,-1.91184,0.060135,1.48633,-1.96642,0.06287,1.46532,-2.02506,0.258653,1.41135,-2.09804,0.389245,1.20319,-1. [...]
+/*1587*/{0.224758,1.9065,-1.75739,0.332023,1.84124,-1.88697,0.079145,1.80669,-1.68852,-0.034381,1.73327,-1.655,0.098777,1.64469,-1.68151,0.166682,1.5875,-1.67998,0.302493,1.90592,-2.14288,0.349371,1.83939,-1.96261,0.153034,1.86631,-2.08471,0.192312,1.67133,-2.24632,0.266214,1.59443,-2.29462,0.421635,1.56835,-2.16726,0.482233,1.54969,-2.15723,0.24268,1.40804,-1.82847,0.05616,1.46261,-1.91276,0.062915,1.48935,-1.96778,0.065793,1.46934,-2.02579,0.261034,1.41102,-2.0976,0.390315,1.20951,-1.7 [...]
+/*1588*/{0.222371,1.91035,-1.75717,0.33171,1.84412,-1.88473,0.076335,1.81138,-1.68964,-0.038225,1.73716,-1.65759,0.095195,1.64969,-1.68347,0.163128,1.59321,-1.68111,0.303691,1.91136,-2.14139,0.350299,1.84424,-1.96104,0.154354,1.86943,-2.08499,0.195828,1.67641,-2.24693,0.271165,1.59983,-2.29412,0.423637,1.57638,-2.16274,0.484243,1.55921,-2.15219,0.24448,1.41239,-1.8299,0.056981,1.46689,-1.91359,0.064492,1.49327,-1.97006,0.068929,1.47223,-2.02629,0.261672,1.41215,-2.09803,0.391462,1.21537, [...]
+/*1589*/{0.220163,1.91385,-1.75713,0.324838,1.85168,-1.88483,0.073457,1.81603,-1.6913,-0.042802,1.74198,-1.65996,0.091568,1.65523,-1.68522,0.159957,1.59876,-1.68201,0.305674,1.9163,-2.13985,0.349917,1.84929,-1.95929,0.155414,1.87383,-2.08507,0.199432,1.68203,-2.24838,0.276096,1.60545,-2.29328,0.42372,1.5831,-2.15904,0.486011,1.56826,-2.14725,0.247114,1.41566,-1.83084,0.059333,1.4712,-1.91455,0.065729,1.49688,-1.97147,0.071651,1.47647,-2.02703,0.264654,1.41281,-2.09807,0.393146,1.22171,-1 [...]
+/*1590*/{0.218528,1.91745,-1.75721,0.325501,1.85575,-1.8828,0.070845,1.81988,-1.69247,-0.046654,1.74621,-1.66369,0.088335,1.66029,-1.68717,0.156925,1.60427,-1.68361,0.306729,1.92156,-2.13846,0.349557,1.85413,-1.95761,0.155971,1.8773,-2.0852,0.203521,1.68807,-2.25057,0.280446,1.61071,-2.29229,0.426574,1.59249,-2.15484,0.48765,1.57736,-2.14253,0.249476,1.4199,-1.8323,0.0614,1.47673,-1.91506,0.068632,1.50121,-1.97261,0.073667,1.4804,-2.0281,0.265841,1.4131,-2.09803,0.395052,1.22669,-1.77164 [...]
+/*1591*/{0.216791,1.92147,-1.75655,0.325431,1.86076,-1.88175,0.067808,1.8247,-1.69388,-0.048466,1.75092,-1.66605,0.085248,1.66527,-1.68893,0.15395,1.60928,-1.68458,0.307803,1.92664,-2.1368,0.349719,1.85828,-1.9559,0.158187,1.88149,-2.08534,0.205448,1.69389,-2.25147,0.284677,1.61628,-2.29166,0.426638,1.59962,-2.1501,0.488455,1.58601,-2.13825,0.251095,1.42372,-1.83287,0.06288,1.48175,-1.91551,0.071432,1.50569,-1.97381,0.076795,1.48404,-2.02864,0.266031,1.41435,-2.09797,0.394946,1.23091,-1. [...]
+/*1592*/{0.215124,1.92481,-1.75647,0.325808,1.86497,-1.8806,0.065543,1.82836,-1.69519,-0.052578,1.75494,-1.6687,0.082772,1.66988,-1.69047,0.151494,1.61383,-1.68528,0.308368,1.931,-2.13518,0.349728,1.86276,-1.95427,0.158712,1.88486,-2.08548,0.210906,1.69824,-2.25477,0.288826,1.6212,-2.29134,0.427365,1.60749,-2.14721,0.48919,1.59413,-2.13411,0.253058,1.42709,-1.83424,0.065977,1.48675,-1.91561,0.074229,1.50993,-1.97448,0.078815,1.48818,-2.02883,0.268686,1.41535,-2.09815,0.396254,1.23484,-1. [...]
+/*1593*/{0.21372,1.92802,-1.75669,0.325687,1.8693,-1.87879,0.063587,1.83227,-1.69683,-0.053788,1.7593,-1.67152,0.080429,1.67465,-1.69272,0.148417,1.61867,-1.68625,0.309699,1.93556,-2.13402,0.350268,1.86685,-1.95262,0.160203,1.88784,-2.08507,0.215371,1.70383,-2.25663,0.293376,1.62645,-2.29101,0.427691,1.61493,-2.14388,0.489917,1.60242,-2.1312,0.255115,1.43072,-1.83478,0.067951,1.49289,-1.91535,0.077029,1.51417,-1.97548,0.08138,1.4924,-2.02898,0.268787,1.41709,-2.09799,0.396611,1.23804,-1. [...]
+/*1594*/{0.212645,1.93158,-1.75635,0.325422,1.87269,-1.87766,0.061518,1.836,-1.69838,-0.057099,1.7631,-1.67422,0.078452,1.67857,-1.69442,0.146975,1.6226,-1.68697,0.310948,1.93993,-2.13195,0.350745,1.87093,-1.95171,0.161202,1.89102,-2.08525,0.218582,1.70857,-2.2594,0.297548,1.63163,-2.29113,0.428055,1.62093,-2.14084,0.490262,1.60903,-2.12742,0.257958,1.43491,-1.835,0.06991,1.49859,-1.91578,0.079749,1.51879,-1.97601,0.083986,1.49688,-2.0292,0.269966,1.41817,-2.09779,0.397362,1.24097,-1.773 [...]
+/*1595*/{0.212119,1.93459,-1.7566,0.326062,1.87661,-1.8769,0.060638,1.83915,-1.69965,-0.058637,1.76655,-1.67728,0.077127,1.68276,-1.6957,0.144973,1.62634,-1.68774,0.311349,1.94384,-2.13111,0.350726,1.87535,-1.94998,0.162278,1.89402,-2.08534,0.222956,1.71341,-2.26157,0.301273,1.63614,-2.29047,0.428139,1.62776,-2.13848,0.490428,1.61671,-2.12383,0.258653,1.43825,-1.83654,0.07171,1.50327,-1.9157,0.081369,1.52268,-1.97748,0.085612,1.50017,-2.02935,0.27118,1.41902,-2.09723,0.397088,1.24233,-1. [...]
+/*1596*/{0.211156,1.93714,-1.7563,0.325742,1.87982,-1.87545,0.058366,1.84279,-1.70108,-0.061366,1.77005,-1.68049,0.074529,1.6858,-1.69644,0.143704,1.62976,-1.68805,0.311967,1.9472,-2.12889,0.3511,1.87863,-1.94887,0.16378,1.89719,-2.08555,0.226971,1.71756,-2.26393,0.304385,1.64057,-2.29054,0.427447,1.6331,-2.1358,0.48991,1.62261,-2.12052,0.259114,1.44103,-1.83669,0.074444,1.50766,-1.91527,0.082505,1.52762,-1.97771,0.087485,1.50405,-2.02896,0.272126,1.42106,-2.09697,0.397936,1.24412,-1.774 [...]
+/*1597*/{0.211033,1.93984,-1.75632,0.327468,1.88326,-1.87452,0.057546,1.84569,-1.70245,-0.061952,1.77289,-1.68329,0.073846,1.68922,-1.69833,0.142189,1.63301,-1.68821,0.313117,1.95058,-2.12788,0.351922,1.88192,-1.94753,0.164643,1.90049,-2.08459,0.232799,1.72243,-2.2663,0.306992,1.64388,-2.28972,0.427762,1.63789,-2.13374,0.489413,1.62906,-2.11337,0.262841,1.44399,-1.83687,0.076464,1.51202,-1.91427,0.084285,1.53087,-1.9777,0.090351,1.50798,-2.02929,0.272945,1.42207,-2.09679,0.398293,1.24524 [...]
+/*1598*/{0.210954,1.94209,-1.75609,0.32712,1.88567,-1.87391,0.057019,1.84898,-1.70335,-0.065957,1.77482,-1.68561,0.072306,1.69211,-1.6989,0.141412,1.63544,-1.68863,0.314134,1.95402,-2.12653,0.352436,1.8852,-1.94639,0.16619,1.90296,-2.08402,0.237059,1.72626,-2.26779,0.309977,1.64801,-2.28968,0.428089,1.64374,-2.13163,0.488773,1.63452,-2.11074,0.262621,1.44652,-1.83762,0.078648,1.51642,-1.91376,0.085774,1.53547,-1.97784,0.091952,1.51202,-2.02928,0.273244,1.42434,-2.09628,0.397093,1.24495,- [...]
+/*1599*/{0.21098,1.94425,-1.75565,0.328539,1.88901,-1.87232,0.056345,1.85164,-1.7043,-0.06569,1.778,-1.68869,0.071691,1.69423,-1.69967,0.139362,1.63773,-1.6885,0.314423,1.95696,-2.12492,0.352684,1.88705,-1.94553,0.167013,1.90581,-2.08365,0.241148,1.72993,-2.26983,0.312661,1.65152,-2.28983,0.427376,1.64798,-2.12965,0.488147,1.6386,-2.10752,0.263294,1.44806,-1.83845,0.080011,1.52037,-1.91342,0.085734,1.53862,-1.97811,0.092704,1.51586,-2.02825,0.273923,1.42628,-2.09649,0.395205,1.24516,-1.7 [...]
+/*1600*/{0.211881,1.94621,-1.75479,0.328577,1.89094,-1.87156,0.055289,1.85441,-1.7064,-0.066449,1.78018,-1.69082,0.071417,1.69658,-1.70104,0.139597,1.63993,-1.68857,0.315417,1.95931,-2.12385,0.353601,1.8898,-1.94431,0.168677,1.90762,-2.08289,0.244963,1.7336,-2.26996,0.315392,1.65504,-2.29017,0.427635,1.65132,-2.12823,0.487672,1.64253,-2.10464,0.26361,1.4499,-1.83854,0.080453,1.52377,-1.91289,0.08721,1.54181,-1.97744,0.094015,1.51899,-2.02777,0.273802,1.42835,-2.09635,0.39513,1.24297,-1.7 [...]
+/*1601*/{0.212016,1.94768,-1.75446,0.329669,1.89337,-1.87127,0.055715,1.85615,-1.70776,-0.068,1.78188,-1.69372,0.071906,1.69862,-1.70219,0.138905,1.64084,-1.68865,0.316153,1.96194,-2.12216,0.354055,1.89238,-1.94328,0.169255,1.91065,-2.08284,0.24948,1.73696,-2.2711,0.317683,1.65811,-2.2907,0.427113,1.65436,-2.12641,0.487321,1.64582,-2.10247,0.266202,1.45106,-1.83889,0.081942,1.52633,-1.91135,0.087579,1.54491,-1.97784,0.094548,1.52183,-2.02723,0.274001,1.43043,-2.09664,0.391433,1.24121,-1. [...]
+/*1602*/{0.213128,1.94919,-1.75405,0.330251,1.89457,-1.87024,0.055141,1.85792,-1.7091,-0.068532,1.78316,-1.6959,0.070847,1.70005,-1.70212,0.139166,1.64234,-1.68829,0.317054,1.96432,-2.12149,0.354454,1.89391,-1.94245,0.170654,1.91306,-2.0824,0.252853,1.73992,-2.27173,0.319378,1.66009,-2.29091,0.42733,1.65693,-2.12522,0.486502,1.64817,-2.09976,0.267035,1.45342,-1.83959,0.082013,1.52911,-1.91214,0.088908,1.54789,-1.97652,0.095628,1.52551,-2.02624,0.274176,1.43272,-2.09669,0.389148,1.23904,- [...]
+/*1603*/{0.214042,1.95079,-1.75336,0.330235,1.89513,-1.8695,0.056241,1.85975,-1.70947,-0.067943,1.7843,-1.69756,0.070732,1.7006,-1.70292,0.139011,1.64332,-1.68777,0.317465,1.96595,-2.11982,0.355093,1.89551,-1.94173,0.171433,1.91538,-2.08214,0.255935,1.74267,-2.27162,0.321014,1.66283,-2.29016,0.425959,1.65862,-2.12421,0.486075,1.64995,-2.09883,0.267026,1.45527,-1.84071,0.083316,1.53149,-1.91045,0.088939,1.55035,-1.97654,0.09506,1.52765,-2.02607,0.274071,1.43532,-2.09659,0.385569,1.2353,-1 [...]
+/*1604*/{0.215599,1.95149,-1.75269,0.332309,1.89647,-1.86864,0.056783,1.86143,-1.71042,-0.068882,1.78498,-1.69981,0.070828,1.70176,-1.70358,0.138881,1.64356,-1.687,0.318367,1.96773,-2.11917,0.355936,1.89679,-1.94064,0.172659,1.91715,-2.08155,0.258374,1.74517,-2.27154,0.322134,1.66434,-2.29073,0.426015,1.65996,-2.12281,0.48542,1.65032,-2.0971,0.268851,1.45636,-1.84061,0.083806,1.53319,-1.91041,0.088303,1.553,-1.9762,0.095663,1.52912,-2.0255,0.274222,1.43731,-2.09671,0.382338,1.23138,-1.77 [...]
+/*1605*/{0.216762,1.95229,-1.75227,0.332389,1.89695,-1.86856,0.057166,1.86198,-1.71159,-0.068701,1.78508,-1.70118,0.071035,1.70192,-1.70412,0.139571,1.64333,-1.68676,0.318349,1.9686,-2.11778,0.356453,1.89748,-1.94033,0.172068,1.91943,-2.0788,0.2614,1.74775,-2.27182,0.323127,1.66568,-2.29031,0.425762,1.66045,-2.12221,0.484889,1.65077,-2.09578,0.269938,1.45804,-1.84078,0.083835,1.53415,-1.9104,0.088402,1.55483,-1.97544,0.09539,1.53139,-2.02437,0.274266,1.43934,-2.09717,0.379719,1.22575,-1. [...]
+/*1606*/{0.217852,1.95244,-1.75142,0.332623,1.89737,-1.86769,0.057636,1.8628,-1.71169,-0.067163,1.78565,-1.70268,0.071101,1.70148,-1.70409,0.140305,1.6426,-1.6856,0.31933,1.96946,-2.11684,0.356658,1.89769,-1.94,0.173102,1.92105,-2.07904,0.263037,1.74966,-2.27104,0.323011,1.66651,-2.2894,0.425679,1.66081,-2.12149,0.484473,1.65004,-2.09481,0.269548,1.45861,-1.84174,0.084452,1.53479,-1.90961,0.089015,1.5559,-1.97485,0.095194,1.53203,-2.02475,0.274488,1.44135,-2.09697,0.375053,1.2207,-1.7756 [...]
+/*1607*/{0.219866,1.95265,-1.7512,0.332623,1.89737,-1.86769,0.058934,1.86308,-1.71243,-0.065747,1.78682,-1.70376,0.072899,1.70137,-1.70401,0.141154,1.64132,-1.68403,0.32072,1.96993,-2.11567,0.357274,1.89824,-1.93968,0.173537,1.92206,-2.07814,0.263589,1.75118,-2.2707,0.323017,1.66699,-2.28953,0.425649,1.66049,-2.12071,0.483969,1.64909,-2.09417,0.270415,1.45933,-1.84193,0.084472,1.53487,-1.90957,0.088825,1.55739,-1.97379,0.094979,1.53262,-2.02373,0.274299,1.44228,-2.09721,0.371438,1.21505, [...]
+/*1608*/{0.221337,1.95283,-1.75032,0.332949,1.89688,-1.86725,0.060725,1.86236,-1.71197,-0.063342,1.78506,-1.70398,0.074154,1.70079,-1.70314,0.141928,1.64009,-1.6832,0.321241,1.97013,-2.11417,0.357222,1.89776,-1.93939,0.173812,1.92371,-2.07803,0.26368,1.75196,-2.26993,0.322568,1.66738,-2.28937,0.424908,1.65866,-2.12027,0.483301,1.64708,-2.09435,0.270079,1.45956,-1.84261,0.083553,1.53422,-1.90962,0.088131,1.55782,-1.97366,0.094632,1.53306,-2.02382,0.275268,1.44383,-2.09749,0.366153,1.20977 [...]
+/*1609*/{0.222683,1.95226,-1.74949,0.337398,1.89381,-1.86599,0.061155,1.86187,-1.71197,-0.063031,1.78332,-1.70432,0.075169,1.6984,-1.70306,0.143738,1.63815,-1.68213,0.321474,1.96965,-2.1131,0.357322,1.89661,-1.93893,0.172432,1.92345,-2.07345,0.263758,1.75278,-2.26866,0.320609,1.66708,-2.28914,0.425335,1.65713,-2.12009,0.482198,1.64439,-2.09375,0.272591,1.46011,-1.84278,0.083264,1.53331,-1.90888,0.087737,1.55665,-1.97325,0.094625,1.53259,-2.02301,0.275408,1.44407,-2.09768,0.362937,1.20324 [...]
+/*1610*/{0.223956,1.9516,-1.74847,0.337065,1.89316,-1.86587,0.062974,1.86053,-1.71194,-0.060449,1.78307,-1.70435,0.077101,1.69682,-1.70216,0.144631,1.63562,-1.68038,0.321227,1.96862,-2.11188,0.357568,1.89571,-1.9381,0.173693,1.92436,-2.07566,0.260685,1.75256,-2.26697,0.318694,1.66651,-2.28821,0.424355,1.655,-2.12022,0.48144,1.64147,-2.09374,0.271292,1.45976,-1.8435,0.084165,1.53128,-1.90819,0.087483,1.55519,-1.97285,0.094411,1.53119,-2.02264,0.275446,1.44413,-2.09764,0.357957,1.19641,-1. [...]
+/*1611*/{0.225223,1.95006,-1.74775,0.338553,1.89251,-1.86616,0.064086,1.85859,-1.71085,-0.058156,1.77986,-1.70358,0.078616,1.69387,-1.70067,0.146779,1.63319,-1.67843,0.32088,1.96758,-2.11068,0.357648,1.89452,-1.93809,0.173505,1.92432,-2.07522,0.258627,1.75235,-2.26524,0.315227,1.66513,-2.28715,0.423115,1.65302,-2.12031,0.480399,1.63744,-2.0938,0.272101,1.4587,-1.84321,0.083302,1.52949,-1.90802,0.08704,1.55404,-1.97286,0.093748,1.52958,-2.02238,0.275408,1.44407,-2.09768,0.352611,1.19081,- [...]
+/*1612*/{0.227175,1.94855,-1.74675,0.337656,1.89051,-1.86554,0.065982,1.85602,-1.70978,-0.056069,1.77653,-1.70289,0.080735,1.6909,-1.69885,0.148457,1.6301,-1.6767,0.32074,1.96578,-2.10955,0.357452,1.8933,-1.9381,0.172583,1.92386,-2.07467,0.254904,1.75209,-2.26317,0.312806,1.66408,-2.28583,0.421847,1.64903,-2.12075,0.478761,1.63355,-2.09453,0.27223,1.45774,-1.84341,0.082653,1.52695,-1.90791,0.087185,1.55237,-1.97191,0.093587,1.5272,-2.02197,0.275284,1.44335,-2.09783,0.349492,1.1832,-1.772 [...]
+/*1613*/{0.228638,1.94689,-1.74575,0.338269,1.88855,-1.86577,0.067481,1.85296,-1.70857,-0.053594,1.77289,-1.70088,0.082734,1.68726,-1.69685,0.151278,1.62618,-1.67447,0.320886,1.96402,-2.10871,0.35764,1.89147,-1.93801,0.172096,1.92242,-2.07229,0.249839,1.75029,-2.2613,0.309135,1.66237,-2.28474,0.420708,1.64518,-2.12146,0.477474,1.62834,-2.09562,0.273178,1.45688,-1.84358,0.083186,1.5246,-1.9067,0.086696,1.54952,-1.97137,0.093159,1.52548,-2.02245,0.27546,1.44222,-2.09738,0.34656,1.17699,-1. [...]
+/*1614*/{0.229277,1.94402,-1.74491,0.338433,1.88721,-1.86616,0.068558,1.84942,-1.70717,-0.051533,1.76857,-1.69884,0.085343,1.68395,-1.69455,0.153539,1.62232,-1.67222,0.319179,1.96135,-2.10798,0.356621,1.88954,-1.93804,0.1714,1.92127,-2.07127,0.244382,1.74859,-2.26003,0.305033,1.66046,-2.28387,0.41916,1.64062,-2.12191,0.476158,1.62398,-2.09719,0.274155,1.45499,-1.8433,0.082484,1.52173,-1.9069,0.085984,1.54623,-1.97118,0.09327,1.52244,-2.02183,0.275997,1.44054,-2.09712,0.341747,1.17148,-1. [...]
+/*1615*/{0.230521,1.94141,-1.74369,0.338663,1.88342,-1.86541,0.070265,1.84589,-1.70503,-0.047914,1.76372,-1.69631,0.088365,1.67887,-1.69203,0.156446,1.61832,-1.67015,0.317724,1.95913,-2.1077,0.356289,1.88673,-1.93777,0.170419,1.91907,-2.07052,0.239194,1.74721,-2.25842,0.300518,1.6581,-2.28308,0.417303,1.63662,-2.12247,0.474432,1.61812,-2.09858,0.272554,1.4521,-1.84343,0.083391,1.51832,-1.90659,0.086093,1.54246,-1.97076,0.09279,1.51856,-2.02191,0.275811,1.43942,-2.09684,0.338853,1.16508,- [...]
+/*1616*/{0.231625,1.93807,-1.74301,0.339122,1.88111,-1.86503,0.071888,1.841,-1.70291,-0.04597,1.75799,-1.69385,0.090527,1.67395,-1.68933,0.159323,1.61341,-1.6674,0.316178,1.95605,-2.10736,0.356491,1.88503,-1.93795,0.168417,1.91729,-2.06869,0.232942,1.74423,-2.25599,0.296256,1.65534,-2.28168,0.415344,1.63155,-2.12377,0.472235,1.61129,-2.10012,0.273198,1.44915,-1.84207,0.083128,1.51517,-1.90597,0.086328,1.53935,-1.9703,0.092363,1.51524,-2.0216,0.27635,1.43693,-2.09582,0.335548,1.1597,-1.76 [...]
+/*1617*/{0.232057,1.93436,-1.74224,0.337896,1.87964,-1.86494,0.073883,1.83534,-1.70067,-0.044162,1.75179,-1.69005,0.093398,1.66822,-1.68632,0.16351,1.60884,-1.66453,0.315312,1.9532,-2.10807,0.355922,1.8823,-1.93781,0.167523,1.91439,-2.06737,0.226796,1.74166,-2.25399,0.290689,1.65186,-2.28104,0.413171,1.62509,-2.12536,0.469963,1.60494,-2.10283,0.270646,1.44464,-1.84219,0.082079,1.51152,-1.90631,0.085546,1.53606,-1.9694,0.09179,1.51131,-2.02201,0.276476,1.43479,-2.09573,0.332717,1.15449,-1 [...]
+/*1618*/{0.232799,1.93026,-1.74134,0.338198,1.87497,-1.8649,0.075375,1.83003,-1.69746,-0.040648,1.74552,-1.68576,0.097168,1.66264,-1.68289,0.167375,1.60339,-1.66219,0.313464,1.95011,-2.10826,0.355039,1.87931,-1.93727,0.166233,1.91051,-2.06649,0.221756,1.73791,-2.25263,0.285448,1.64812,-2.28074,0.410079,1.61909,-2.12657,0.467868,1.59755,-2.10525,0.269927,1.44087,-1.84082,0.08196,1.50749,-1.90576,0.085444,1.53196,-1.96852,0.09192,1.50751,-2.02156,0.276807,1.43246,-2.09476,0.33012,1.14987,- [...]
+/*1619*/{0.233465,1.92583,-1.74016,0.338843,1.87211,-1.86514,0.077501,1.82354,-1.6946,-0.038476,1.7376,-1.68155,0.100044,1.65656,-1.67919,0.171002,1.59809,-1.65907,0.312297,1.94621,-2.10922,0.355304,1.87607,-1.93724,0.164733,1.90738,-2.06519,0.215967,1.73454,-2.25091,0.279877,1.64408,-2.28042,0.407309,1.61263,-2.12879,0.464518,1.59018,-2.10816,0.268942,1.43684,-1.84036,0.082536,1.50502,-1.90555,0.084895,1.52857,-1.96813,0.092502,1.50525,-2.02166,0.276767,1.42964,-2.09404,0.327387,1.14552 [...]
+/*1620*/{0.234279,1.92154,-1.73869,0.338232,1.86937,-1.86568,0.079588,1.81674,-1.69109,-0.034838,1.73066,-1.67647,0.103013,1.64946,-1.67534,0.176094,1.5923,-1.65679,0.309828,1.94188,-2.10949,0.354112,1.87286,-1.93789,0.163135,1.90313,-2.06385,0.210118,1.73005,-2.24991,0.27403,1.63948,-2.27989,0.40369,1.60634,-2.13067,0.461961,1.58167,-2.11161,0.266378,1.42953,-1.83913,0.080234,1.49828,-1.90516,0.084211,1.52453,-1.9685,0.091143,1.49803,-2.02178,0.277123,1.42667,-2.09357,0.32506,1.14182,-1 [...]
+/*1621*/{0.234479,1.91651,-1.73827,0.338354,1.86548,-1.8658,0.081331,1.80988,-1.68784,-0.033568,1.72172,-1.67157,0.107785,1.64291,-1.67137,0.18023,1.58626,-1.65419,0.308907,1.9375,-2.11075,0.354169,1.86949,-1.93749,0.1612,1.89835,-2.06278,0.205006,1.72525,-2.24823,0.268258,1.6351,-2.2799,0.400531,1.59874,-2.13327,0.458673,1.57386,-2.11466,0.268841,1.43009,-1.83842,0.08072,1.49404,-1.90471,0.084763,1.52006,-1.96782,0.089764,1.49266,-2.02208,0.276658,1.42363,-2.09255,0.322951,1.13936,-1.76 [...]
+/*1622*/{0.235183,1.91117,-1.73716,0.338564,1.86154,-1.86583,0.083901,1.8024,-1.68471,-0.02815,1.71418,-1.66629,0.112326,1.63527,-1.66749,0.185453,1.58054,-1.65121,0.306481,1.93298,-2.11237,0.354458,1.86571,-1.93792,0.159561,1.89377,-2.0615,0.198531,1.71997,-2.24674,0.262247,1.62982,-2.27907,0.396203,1.59094,-2.1355,0.454792,1.565,-2.11799,0.2679,1.42682,-1.83752,0.078962,1.48865,-1.90541,0.083005,1.51536,-1.96724,0.089659,1.48767,-2.02126,0.276673,1.42053,-2.09186,0.322329,1.13497,-1.76 [...]
+/*1623*/{0.235927,1.90553,-1.7362,0.338771,1.85651,-1.86586,0.086011,1.79411,-1.68091,-0.025796,1.70499,-1.6607,0.117132,1.62795,-1.66259,0.191691,1.57403,-1.64815,0.304139,1.92842,-2.11299,0.353601,1.86154,-1.93864,0.158297,1.88842,-2.06013,0.192069,1.71512,-2.24582,0.254981,1.62481,-2.27874,0.392424,1.5836,-2.13794,0.450682,1.55594,-2.12203,0.2671,1.42205,-1.83629,0.078807,1.48327,-1.90487,0.083631,1.51067,-1.96663,0.088513,1.48263,-2.02123,0.275751,1.41535,-2.09203,0.320996,1.13023,-1 [...]
+/*1624*/{0.236735,1.8999,-1.73513,0.338581,1.85063,-1.86567,0.088971,1.78648,-1.67705,-0.021395,1.69573,-1.65491,0.122085,1.62063,-1.6588,0.197512,1.56732,-1.64522,0.302668,1.9231,-2.11411,0.353845,1.85668,-1.93867,0.157706,1.88355,-2.06011,0.185774,1.70991,-2.2443,0.248046,1.61904,-2.27858,0.387923,1.57605,-2.14109,0.446857,1.54623,-2.12586,0.266346,1.41802,-1.83415,0.078382,1.47717,-1.90458,0.082527,1.50501,-1.96582,0.087759,1.47624,-2.02122,0.275124,1.41298,-2.09095,0.317833,1.12605,- [...]
+/*1625*/{0.236796,1.89414,-1.73397,0.33897,1.84686,-1.86616,0.092361,1.77784,-1.67318,-0.016412,1.68621,-1.64846,0.127509,1.61237,-1.65431,0.203316,1.56101,-1.64278,0.299468,1.918,-2.11548,0.353231,1.85196,-1.93971,0.155399,1.87846,-2.05969,0.178951,1.70411,-2.24258,0.241015,1.61324,-2.27829,0.383735,1.56715,-2.14403,0.441892,1.536,-2.13072,0.266219,1.414,-1.83389,0.077757,1.47165,-1.90437,0.082615,1.49906,-1.9647,0.087265,1.47042,-2.02076,0.274943,1.40895,-2.09053,0.316397,1.12216,-1.76 [...]
+/*1626*/{0.238007,1.88788,-1.73334,0.339383,1.84186,-1.86681,0.095964,1.76861,-1.66887,-0.011821,1.67623,-1.64237,0.134145,1.60436,-1.65005,0.209615,1.55473,-1.64048,0.297927,1.91319,-2.11659,0.353212,1.84762,-1.94042,0.153697,1.87216,-2.05833,0.177545,1.69729,-2.24284,0.235169,1.60781,-2.27843,0.378332,1.55724,-2.14687,0.436928,1.52626,-2.13506,0.265713,1.41064,-1.83241,0.076497,1.46541,-1.90458,0.080583,1.49341,-1.96377,0.086008,1.46451,-2.02044,0.273643,1.40512,-2.08951,0.316554,1.116 [...]
+/*1627*/{0.239169,1.8819,-1.73233,0.339997,1.83601,-1.86677,0.100192,1.75993,-1.66505,-0.006171,1.66676,-1.63595,0.139529,1.59596,-1.64538,0.217183,1.54801,-1.63727,0.295542,1.9081,-2.11803,0.353237,1.84242,-1.94178,0.152085,1.86656,-2.05708,0.17116,1.69144,-2.24141,0.226848,1.60183,-2.27805,0.373396,1.54833,-2.14964,0.43162,1.51553,-2.13994,0.264307,1.4059,-1.83029,0.074591,1.45997,-1.9048,0.078286,1.48694,-1.96298,0.084088,1.45834,-2.02046,0.273008,1.40031,-2.08867,0.31504,1.11083,-1.7 [...]
+/*1628*/{0.240286,1.87634,-1.73082,0.34071,1.8319,-1.86776,0.104326,1.75031,-1.6608,-0.000279,1.6563,-1.62942,0.146599,1.58802,-1.64152,0.224267,1.5414,-1.63518,0.293941,1.90345,-2.11925,0.352978,1.83745,-1.94329,0.150422,1.86103,-2.05636,0.164702,1.68578,-2.24075,0.220629,1.5965,-2.27858,0.367953,1.5391,-2.15379,0.425141,1.50505,-2.14504,0.263569,1.40133,-1.82871,0.072096,1.45557,-1.90472,0.076599,1.48108,-1.96282,0.081623,1.45316,-2.02102,0.272811,1.39623,-2.08807,0.311784,1.10635,-1.7 [...]
+/*1629*/{0.241637,1.87078,-1.72977,0.34104,1.82534,-1.86774,0.109467,1.74091,-1.65666,0.006572,1.64585,-1.62285,0.15408,1.57991,-1.63744,0.232637,1.53626,-1.63302,0.291746,1.89859,-2.12051,0.353455,1.83197,-1.94376,0.148994,1.85611,-2.05503,0.158693,1.68006,-2.23957,0.21235,1.59075,-2.27865,0.362176,1.52975,-2.15717,0.419517,1.49448,-2.15069,0.259762,1.39634,-1.82833,0.068031,1.45049,-1.9055,0.074112,1.47541,-1.96349,0.078851,1.44726,-2.02177,0.272231,1.38996,-2.08711,0.307569,1.10636,-1 [...]
+/*1630*/{0.2428,1.86554,-1.72857,0.341954,1.82014,-1.86827,0.115794,1.73155,-1.65244,0.014263,1.63529,-1.61617,0.163231,1.57313,-1.63379,0.241113,1.5305,-1.63123,0.290478,1.89512,-2.12221,0.354611,1.82731,-1.9449,0.14746,1.85382,-2.05281,0.154551,1.67575,-2.23908,0.205029,1.58548,-2.27909,0.357001,1.51989,-2.16141,0.412584,1.48337,-2.15612,0.258088,1.39495,-1.82884,0.064034,1.44643,-1.90546,0.070701,1.4698,-1.96484,0.075439,1.4421,-2.02121,0.269697,1.38598,-2.0862,0.305957,1.10684,-1.754 [...]
+/*1631*/{0.244313,1.86071,-1.72714,0.342056,1.81628,-1.87053,0.121668,1.72227,-1.64901,0.022427,1.62504,-1.60949,0.171106,1.5667,-1.63122,0.250386,1.52578,-1.62889,0.288658,1.89174,-2.12416,0.354782,1.82229,-1.94716,0.145791,1.85157,-2.05011,0.148749,1.67215,-2.23822,0.197128,1.58068,-2.27941,0.350907,1.51046,-2.1661,0.405694,1.47243,-2.16153,0.253613,1.3928,-1.82916,0.059439,1.44403,-1.90825,0.066436,1.46562,-1.96583,0.071471,1.43677,-2.0225,0.269607,1.3815,-2.08414,0.303359,1.10734,-1. [...]
+/*1632*/{0.246029,1.85689,-1.72532,0.343218,1.81268,-1.87169,0.129577,1.7132,-1.64556,0.033113,1.61589,-1.60339,0.180961,1.56115,-1.62935,0.259893,1.5212,-1.62713,0.287333,1.8888,-2.12606,0.355715,1.81869,-1.94838,0.145591,1.85124,-2.04852,0.144655,1.67184,-2.23704,0.189931,1.57669,-2.27821,0.344954,1.501,-2.17038,0.397751,1.46195,-2.16712,0.250455,1.39081,-1.82758,0.05582,1.44317,-1.91062,0.06361,1.4633,-1.96656,0.068275,1.43246,-2.02372,0.269442,1.37807,-2.08045,0.303025,1.10954,-1.749 [...]
+/*1633*/{0.247528,1.85328,-1.72432,0.34386,1.80771,-1.87238,0.136499,1.70573,-1.64329,0.043019,1.60721,-1.5969,0.190612,1.5559,-1.62631,0.270247,1.5175,-1.62568,0.28634,1.88682,-2.12669,0.356178,1.81535,-1.95017,0.14532,1.8514,-2.04708,0.140161,1.67002,-2.23464,0.182268,1.57534,-2.2764,0.338889,1.49283,-2.17573,0.390056,1.45224,-2.1725,0.246346,1.39106,-1.82555,0.055612,1.44283,-1.9111,0.062386,1.46366,-1.96826,0.065612,1.42934,-2.02623,0.265198,1.37388,-2.08181,0.299637,1.11023,-1.74999 [...]
+/*1634*/{0.248544,1.85024,-1.72422,0.343261,1.80642,-1.87432,0.142948,1.70028,-1.64163,0.052806,1.60029,-1.59212,0.200448,1.55148,-1.62443,0.28077,1.51515,-1.625,0.287028,1.88552,-2.12631,0.355354,1.81264,-1.95086,0.145309,1.8516,-2.04704,0.135187,1.67185,-2.23161,0.175917,1.57643,-2.27299,0.330028,1.48412,-2.17911,0.381794,1.443,-2.17723,0.243694,1.39093,-1.82384,0.052456,1.44297,-1.91196,0.060262,1.46345,-1.96928,0.063446,1.42741,-2.0271,0.262696,1.37148,-2.08159,0.297539,1.11151,-1.74 [...]
+/*1635*/{0.250071,1.84787,-1.72442,0.342997,1.8035,-1.87418,0.149822,1.69676,-1.64016,0.063971,1.5955,-1.5874,0.211458,1.54935,-1.62328,0.292692,1.51415,-1.62302,0.287784,1.88449,-2.12548,0.35545,1.81137,-1.95209,0.145781,1.852,-2.04718,0.131527,1.67109,-2.22792,0.169896,1.57825,-2.27042,0.322568,1.47915,-2.18086,0.372177,1.43493,-2.1826,0.240768,1.39057,-1.82215,0.050285,1.44317,-1.91219,0.057922,1.46398,-1.96919,0.06123,1.42703,-2.02788,0.260077,1.36949,-2.08119,0.295493,1.11094,-1.750 [...]
+/*1636*/{0.251558,1.84613,-1.72492,0.342443,1.80169,-1.87451,0.154529,1.69429,-1.63895,0.07509,1.59208,-1.58377,0.222207,1.54783,-1.62246,0.303874,1.51423,-1.6229,0.288642,1.88301,-2.12534,0.355026,1.81013,-1.95209,0.145694,1.85155,-2.04723,0.126881,1.6733,-2.22447,0.163204,1.5812,-2.26798,0.312854,1.47438,-2.182,0.362205,1.42853,-2.18795,0.238718,1.39045,-1.82147,0.049303,1.44237,-1.9125,0.055752,1.46404,-1.96911,0.059537,1.42743,-2.02761,0.257173,1.36829,-2.08096,0.290179,1.1093,-1.749 [...]
+/*1637*/{0.2531,1.84608,-1.72581,0.341844,1.80172,-1.87511,0.160852,1.69302,-1.63802,0.086288,1.58984,-1.57989,0.233478,1.54723,-1.62169,0.315729,1.51518,-1.62258,0.288948,1.88245,-2.12514,0.353695,1.80901,-1.95273,0.145526,1.85252,-2.04785,0.121181,1.6762,-2.22076,0.156238,1.58425,-2.2667,0.303753,1.47165,-2.18424,0.351464,1.42374,-2.19342,0.236012,1.39076,-1.81961,0.046884,1.44261,-1.9126,0.054719,1.46358,-1.9689,0.05753,1.42866,-2.0279,0.255103,1.36869,-2.08094,0.28489,1.10655,-1.7500 [...]
+/*1638*/{0.254656,1.84543,-1.72648,0.341266,1.80119,-1.87553,0.165814,1.69288,-1.63653,0.094732,1.58752,-1.57723,0.24474,1.54769,-1.62022,0.327544,1.51804,-1.6234,0.288786,1.88231,-2.12509,0.352722,1.80771,-1.95235,0.145107,1.85316,-2.04825,0.116998,1.6806,-2.21657,0.148404,1.588,-2.26552,0.295111,1.47129,-2.18741,0.340786,1.42046,-2.19765,0.234072,1.3917,-1.81911,0.045958,1.44207,-1.91244,0.051973,1.46365,-1.9682,0.05527,1.42969,-2.02744,0.253808,1.36931,-2.08126,0.279211,1.10304,-1.749 [...]
+/*1639*/{0.256457,1.84581,-1.72683,0.34068,1.79939,-1.87605,0.169958,1.69276,-1.63569,0.105431,1.58627,-1.57402,0.255288,1.54909,-1.61988,0.339009,1.52117,-1.62454,0.288521,1.8823,-2.12487,0.351783,1.80762,-1.952,0.144528,1.85496,-2.04872,0.110841,1.68664,-2.21315,0.140073,1.59187,-2.26508,0.28589,1.47096,-2.19088,0.330271,1.41871,-2.20217,0.232399,1.39324,-1.81886,0.044118,1.44245,-1.91234,0.051125,1.46419,-1.96836,0.0541,1.4305,-2.02601,0.251381,1.37165,-2.08158,0.274047,1.09954,-1.749 [...]
+/*1640*/{0.258031,1.84715,-1.72709,0.340317,1.79963,-1.87652,0.175586,1.69389,-1.6343,0.114874,1.58522,-1.57033,0.265963,1.55188,-1.61892,0.350959,1.52531,-1.62549,0.287152,1.88263,-2.12456,0.350331,1.80768,-1.95242,0.144054,1.85557,-2.04861,0.105791,1.69215,-2.21032,0.131967,1.59614,-2.26484,0.276299,1.47075,-2.19434,0.31939,1.41819,-2.20658,0.230613,1.3939,-1.81779,0.042275,1.44179,-1.91281,0.049325,1.46502,-1.96771,0.05195,1.43153,-2.02471,0.249174,1.37413,-2.08232,0.267197,1.09599,-1 [...]
+/*1641*/{0.25943,1.8486,-1.72696,0.339875,1.80041,-1.87712,0.17969,1.69588,-1.63332,0.124213,1.58518,-1.56783,0.275673,1.55511,-1.61863,0.362496,1.53153,-1.62788,0.286695,1.88365,-2.12457,0.349719,1.80866,-1.95336,0.143289,1.85742,-2.04778,0.10062,1.6984,-2.20711,0.122801,1.60184,-2.26492,0.267214,1.4726,-2.19832,0.308498,1.42004,-2.21105,0.229169,1.39465,-1.81729,0.04107,1.44203,-1.9124,0.048088,1.46543,-1.96657,0.050533,1.43259,-2.02419,0.246992,1.37671,-2.08325,0.261294,1.09318,-1.751 [...]
+/*1642*/{0.260053,1.8512,-1.72782,0.339473,1.80151,-1.87724,0.183645,1.69848,-1.6318,0.133748,1.58546,-1.56485,0.285634,1.55999,-1.61903,0.373066,1.53793,-1.63016,0.285451,1.88462,-2.12491,0.350019,1.8107,-1.9536,0.142386,1.85913,-2.04751,0.094321,1.70424,-2.20464,0.114054,1.6076,-2.265,0.258435,1.47509,-2.20235,0.297248,1.42175,-2.21472,0.228424,1.39598,-1.81654,0.040363,1.44232,-1.91243,0.047624,1.46633,-1.96633,0.049175,1.43345,-2.02374,0.246423,1.37974,-2.08442,0.252131,1.09126,-1.75 [...]
+/*1643*/{0.261999,1.85435,-1.72791,0.339996,1.80304,-1.87882,0.187815,1.70171,-1.6312,0.143405,1.58743,-1.56285,0.294475,1.56546,-1.61949,0.382796,1.54518,-1.63263,0.283419,1.88626,-2.12583,0.35014,1.81241,-1.95407,0.141304,1.86182,-2.04644,0.088068,1.71065,-2.20364,0.105515,1.61452,-2.2654,0.249227,1.47891,-2.20635,0.286559,1.42497,-2.21735,0.226538,1.39744,-1.81616,0.038327,1.44426,-1.91271,0.046983,1.46813,-1.96595,0.048347,1.43496,-2.02345,0.245416,1.3828,-2.08503,0.242356,1.09009,-1 [...]
+/*1644*/{0.263713,1.85698,-1.72828,0.339825,1.80663,-1.87961,0.192125,1.70481,-1.63028,0.152333,1.58943,-1.56076,0.303912,1.57136,-1.62044,0.391719,1.55342,-1.63553,0.281703,1.88862,-2.12624,0.347245,1.81306,-1.95545,0.140253,1.86325,-2.04541,0.081788,1.71679,-2.20329,0.097095,1.62156,-2.26549,0.239502,1.484,-2.21009,0.27576,1.42915,-2.22016,0.226166,1.39834,-1.81653,0.039086,1.44618,-1.91225,0.046838,1.47002,-1.96563,0.047695,1.43676,-2.02312,0.243995,1.38549,-2.08514,0.234908,1.08869,- [...]
+/*1645*/{0.265594,1.86104,-1.72909,0.338327,1.8092,-1.88061,0.197432,1.70807,-1.62972,0.160346,1.59153,-1.55929,0.31087,1.57733,-1.62277,0.399503,1.56208,-1.6392,0.278869,1.8912,-2.12633,0.34689,1.81535,-1.95643,0.138751,1.86627,-2.04415,0.075756,1.7231,-2.2032,0.08861,1.62877,-2.26619,0.229905,1.48909,-2.21169,0.264972,1.43482,-2.22239,0.22641,1.39956,-1.81678,0.03863,1.44932,-1.91191,0.046055,1.47234,-1.96558,0.046491,1.4392,-2.02355,0.243394,1.38922,-2.08523,0.226962,1.08773,-1.75479, [...]
+/*1646*/{0.268086,1.86527,-1.72955,0.339325,1.81276,-1.88156,0.20308,1.71181,-1.62888,0.168947,1.59474,-1.5577,0.319475,1.58476,-1.62401,0.406608,1.57138,-1.6426,0.276393,1.89419,-2.12691,0.34632,1.81794,-1.95761,0.137205,1.86935,-2.04316,0.069506,1.7301,-2.20333,0.080185,1.63574,-2.26659,0.220556,1.49579,-2.21349,0.255058,1.44069,-2.22397,0.226009,1.39995,-1.81662,0.038381,1.45204,-1.91145,0.047117,1.47486,-1.96469,0.046742,1.44134,-2.02337,0.243135,1.3928,-2.08559,0.219196,1.08707,-1.7 [...]
+/*1647*/{0.269789,1.86974,-1.73053,0.338665,1.81635,-1.88283,0.209118,1.71578,-1.62846,0.177603,1.59886,-1.55753,0.32625,1.59224,-1.62556,0.414174,1.581,-1.64635,0.273613,1.89781,-2.12784,0.345734,1.8212,-1.95935,0.135849,1.87122,-2.04139,0.062134,1.73827,-2.20444,0.072152,1.64373,-2.26715,0.210849,1.50265,-2.21462,0.244549,1.4473,-2.22527,0.226257,1.40121,-1.8167,0.039614,1.45453,-1.90983,0.046237,1.47696,-1.96388,0.046911,1.44392,-2.02339,0.243422,1.39694,-2.08509,0.211746,1.08735,-1.7 [...]
+/*1648*/{0.271789,1.87518,-1.73167,0.339233,1.81862,-1.88424,0.214533,1.72003,-1.62859,0.185213,1.6037,-1.55621,0.332715,1.60046,-1.62821,0.420682,1.59082,-1.65,0.270354,1.90196,-2.12803,0.344872,1.82394,-1.96069,0.134103,1.87437,-2.03962,0.05581,1.74578,-2.20397,0.06364,1.65119,-2.26701,0.201583,1.50989,-2.21567,0.235411,1.45484,-2.22627,0.226495,1.40224,-1.81623,0.039283,1.45783,-1.90979,0.046552,1.48069,-1.96312,0.046998,1.44827,-2.02298,0.242904,1.40047,-2.08442,0.204024,1.08743,-1.7 [...]
+/*1649*/{0.273754,1.87986,-1.73259,0.340892,1.82393,-1.88523,0.220922,1.7248,-1.62828,0.192418,1.60683,-1.55553,0.338246,1.60856,-1.63034,0.425758,1.6007,-1.65426,0.266904,1.90642,-2.12837,0.344756,1.82831,-1.96251,0.132598,1.87731,-2.03774,0.049981,1.7539,-2.2042,0.056532,1.6589,-2.26747,0.193046,1.5178,-2.2154,0.226045,1.46276,-2.22689,0.226323,1.40342,-1.81556,0.04005,1.46054,-1.90994,0.047381,1.48333,-1.96185,0.04719,1.45183,-2.02272,0.242818,1.40442,-2.08339,0.196814,1.08814,-1.7624 [...]
+/*1650*/{0.275892,1.88472,-1.73367,0.341468,1.82785,-1.88721,0.227044,1.72942,-1.62836,0.200003,1.61189,-1.55485,0.343681,1.61699,-1.63322,0.430776,1.61072,-1.65822,0.263625,1.91123,-2.12897,0.343932,1.83274,-1.9647,0.131184,1.88068,-2.0362,0.042877,1.76359,-2.20449,0.049088,1.66676,-2.26715,0.183696,1.52573,-2.21603,0.217714,1.4711,-2.22722,0.226393,1.40367,-1.81512,0.040399,1.46368,-1.90831,0.046968,1.4867,-1.96115,0.047779,1.45608,-2.02196,0.243329,1.4092,-2.08261,0.190582,1.08805,-1. [...]
+/*1651*/{0.278108,1.89056,-1.73545,0.34193,1.8319,-1.88911,0.232986,1.73436,-1.62935,0.20666,1.61676,-1.55462,0.34818,1.62495,-1.63503,0.434621,1.62067,-1.66234,0.259139,1.91598,-2.12887,0.344269,1.83706,-1.96653,0.129746,1.88316,-2.03439,0.037561,1.77145,-2.20395,0.042252,1.67482,-2.26667,0.175631,1.5338,-2.21568,0.209504,1.47922,-2.22777,0.227445,1.40522,-1.81375,0.041912,1.46703,-1.90762,0.047906,1.49055,-1.95957,0.048967,1.46041,-2.02175,0.244493,1.41344,-2.08144,0.182126,1.08956,-1. [...]
+/*1652*/{0.279976,1.89625,-1.73672,0.343046,1.83722,-1.89142,0.239196,1.7399,-1.62994,0.212984,1.62299,-1.55485,0.3524,1.63326,-1.6378,0.438182,1.63121,-1.66635,0.255509,1.92113,-2.12881,0.343415,1.84193,-1.96858,0.128243,1.88609,-2.03306,0.032851,1.77901,-2.20436,0.036241,1.68261,-2.26577,0.167902,1.54313,-2.2148,0.201294,1.48803,-2.22798,0.228092,1.4065,-1.81222,0.042795,1.47087,-1.90671,0.048645,1.4945,-1.95784,0.049636,1.46563,-2.02198,0.245665,1.41782,-2.07953,0.175519,1.0921,-1.769 [...]
+/*1653*/{0.28188,1.90165,-1.73828,0.342718,1.84199,-1.89295,0.243917,1.74513,-1.63096,0.219272,1.62782,-1.554,0.35585,1.64129,-1.63988,0.440848,1.64082,-1.67004,0.252733,1.92582,-2.12881,0.343383,1.8468,-1.97062,0.126541,1.88981,-2.03119,0.030575,1.78506,-2.20313,0.029143,1.69093,-2.2648,0.160709,1.55216,-2.2133,0.194268,1.49608,-2.22815,0.22803,1.40823,-1.81184,0.044267,1.47417,-1.90696,0.049408,1.49824,-1.95641,0.051547,1.46987,-2.02181,0.246216,1.42227,-2.07818,0.171869,1.09572,-1.770 [...]
+/*1654*/{0.284332,1.90738,-1.74003,0.343473,1.84692,-1.89497,0.249159,1.75089,-1.63176,0.225779,1.63431,-1.55423,0.359427,1.65084,-1.64287,0.443058,1.6508,-1.67386,0.250041,1.93058,-2.12827,0.34333,1.85202,-1.97233,0.125412,1.89315,-2.0295,0.025621,1.79269,-2.20274,0.023331,1.69902,-2.26393,0.153481,1.55976,-2.21218,0.187477,1.50524,-2.22789,0.229045,1.41015,-1.81116,0.045384,1.47781,-1.90622,0.051325,1.502,-1.95494,0.052652,1.47553,-2.02115,0.246749,1.42693,-2.07667,0.162611,1.09367,-1. [...]
+/*1655*/{0.285339,1.91387,-1.74197,0.344077,1.8518,-1.89714,0.254143,1.75672,-1.6324,0.229646,1.64089,-1.55367,0.360831,1.65793,-1.64424,0.44537,1.65984,-1.67772,0.247142,1.93538,-2.12851,0.342899,1.85657,-1.97455,0.124354,1.89701,-2.02866,0.021251,1.8003,-2.20222,0.017207,1.70691,-2.26257,0.146474,1.56858,-2.21035,0.18134,1.51405,-2.22785,0.229783,1.41149,-1.81071,0.046514,1.48218,-1.90566,0.051475,1.50649,-1.95296,0.054109,1.48044,-2.02114,0.247374,1.43155,-2.07561,0.154686,1.09787,-1. [...]
+/*1656*/{0.286798,1.91804,-1.74374,0.345282,1.85714,-1.89952,0.258204,1.76225,-1.63363,0.234615,1.64634,-1.55329,0.362875,1.66619,-1.64669,0.446723,1.66859,-1.68107,0.244998,1.93976,-2.12854,0.342087,1.86232,-1.97586,0.123537,1.90139,-2.02689,0.017867,1.80749,-2.20185,0.012502,1.71487,-2.26123,0.141686,1.57706,-2.20888,0.175265,1.5225,-2.22776,0.230567,1.41271,-1.81107,0.048341,1.48522,-1.9041,0.052839,1.51248,-1.95262,0.055071,1.48597,-2.02095,0.247112,1.43572,-2.0743,0.1502,1.10254,-1. [...]
+/*1657*/{0.288439,1.92327,-1.74586,0.345769,1.86114,-1.90166,0.261273,1.76781,-1.63468,0.237363,1.65223,-1.55247,0.364406,1.67382,-1.64876,0.447381,1.677,-1.68459,0.243248,1.94382,-2.12859,0.340903,1.86627,-1.97767,0.122496,1.90491,-2.02599,0.015093,1.8141,-2.20109,0.00761,1.72211,-2.26018,0.134911,1.58481,-2.20802,0.170155,1.53049,-2.22736,0.23294,1.41434,-1.81177,0.048986,1.48905,-1.90367,0.05439,1.51474,-1.95043,0.056601,1.49116,-2.02055,0.247879,1.44063,-2.0734,0.143777,1.10433,-1.77 [...]
+/*1658*/{0.289706,1.92797,-1.7473,0.345612,1.86596,-1.9037,0.264381,1.77376,-1.63534,0.241112,1.65831,-1.55185,0.365127,1.68062,-1.65061,0.448103,1.68591,-1.68902,0.241951,1.94781,-2.1286,0.341011,1.87107,-1.97935,0.121639,1.90825,-2.02588,0.011536,1.82119,-2.19993,0.002701,1.72929,-2.25749,0.130546,1.59417,-2.20732,0.16532,1.53791,-2.22746,0.233421,1.41615,-1.81202,0.05079,1.4934,-1.90357,0.054724,1.51889,-1.94852,0.057718,1.49626,-2.01947,0.248026,1.44527,-2.07277,0.139107,1.10781,-1.7 [...]
+/*1659*/{0.291491,1.93256,-1.74905,0.345665,1.87043,-1.90526,0.267306,1.77893,-1.63601,0.243427,1.66399,-1.55187,0.365798,1.68777,-1.65223,0.44754,1.6938,-1.69212,0.240613,1.95148,-2.1292,0.340535,1.87596,-1.9805,0.121359,1.91184,-2.02515,0.009874,1.82754,-2.19885,-0.000861,1.73632,-2.25615,0.125827,1.60138,-2.20631,0.161767,1.54407,-2.22673,0.235246,1.41825,-1.81287,0.052837,1.49758,-1.90259,0.055795,1.52292,-1.94732,0.057847,1.50134,-2.01939,0.248482,1.44998,-2.07169,0.134367,1.11098,- [...]
+/*1660*/{0.293007,1.9373,-1.75048,0.34569,1.87548,-1.90727,0.270182,1.78413,-1.63676,0.246173,1.66974,-1.55145,0.366314,1.69381,-1.65423,0.447638,1.70104,-1.69557,0.23924,1.95532,-2.12941,0.340613,1.8808,-1.98239,0.122495,1.91379,-2.02405,0.009982,1.83438,-2.19739,-0.004608,1.74246,-2.2546,0.122919,1.60626,-2.20482,0.159265,1.55021,-2.22663,0.236731,1.42031,-1.81379,0.053842,1.50249,-1.90179,0.057116,1.52752,-1.94568,0.060496,1.50676,-2.01794,0.248708,1.45373,-2.07157,0.130143,1.11546,-1 [...]
+/*1661*/{0.29465,1.94115,-1.75212,0.345611,1.87887,-1.90874,0.271873,1.78881,-1.63693,0.248377,1.67503,-1.55059,0.366187,1.70016,-1.65592,0.446829,1.7085,-1.69861,0.237897,1.9584,-2.12996,0.340017,1.8847,-1.98384,0.123272,1.91604,-2.02385,0.006479,1.83908,-2.19601,-0.007845,1.74885,-2.25239,0.120143,1.61277,-2.20428,0.154023,1.55723,-2.22761,0.238242,1.42304,-1.81369,0.054624,1.50661,-1.90079,0.058278,1.53146,-1.94487,0.061747,1.51133,-2.01789,0.248747,1.45821,-2.07107,0.126849,1.11886,- [...]
+/*1662*/{0.295581,1.9447,-1.75393,0.346451,1.88365,-1.91104,0.272936,1.79369,-1.63756,0.249435,1.67992,-1.54995,0.366159,1.70538,-1.65742,0.446588,1.71383,-1.70041,0.237526,1.96205,-2.13039,0.339962,1.88801,-1.98514,0.122741,1.91899,-2.02387,0.005571,1.8443,-2.19521,-0.011145,1.75469,-2.25083,0.116738,1.61802,-2.20385,0.151606,1.5629,-2.22783,0.23821,1.42534,-1.81439,0.056094,1.51062,-1.90002,0.059247,1.53583,-1.9434,0.063112,1.51643,-2.01636,0.249084,1.46207,-2.07105,0.12558,1.12328,-1. [...]
+/*1663*/{0.297112,1.94924,-1.755,0.346182,1.88649,-1.9123,0.274128,1.79818,-1.63813,0.250606,1.68502,-1.54952,0.366233,1.71071,-1.65867,0.444677,1.71945,-1.70389,0.236734,1.96488,-2.13114,0.340529,1.89171,-1.98629,0.122904,1.92178,-2.02379,0.004675,1.84972,-2.19358,-0.014242,1.76029,-2.2483,0.11404,1.62339,-2.20375,0.149405,1.56809,-2.22745,0.240864,1.42836,-1.81456,0.05738,1.5147,-1.89829,0.060525,1.53985,-1.94221,0.064086,1.521,-2.01513,0.249028,1.46546,-2.07116,0.120252,1.12593,-1.774 [...]
+/*1664*/{0.297336,1.95167,-1.75615,0.346986,1.89053,-1.91293,0.274628,1.80264,-1.6388,0.25148,1.6895,-1.54886,0.364179,1.71572,-1.65942,0.4434,1.72478,-1.7065,0.236336,1.96793,-2.1314,0.339963,1.8946,-1.98802,0.122975,1.92464,-2.02403,0.005482,1.85394,-2.19147,-0.017124,1.76536,-2.24695,0.112368,1.62731,-2.20389,0.147417,1.5726,-2.22788,0.241355,1.43115,-1.81467,0.058946,1.51833,-1.89699,0.062028,1.543,-1.94135,0.06489,1.52519,-2.01424,0.248466,1.4685,-2.07074,0.117889,1.12996,-1.77365,- [...]
+/*1665*/{0.298056,1.95499,-1.75802,0.347577,1.89404,-1.91466,0.274268,1.80694,-1.63853,0.251281,1.69406,-1.54951,0.363978,1.71939,-1.66096,0.442868,1.729,-1.70771,0.23632,1.97018,-2.13277,0.34013,1.89708,-1.98916,0.123993,1.92688,-2.02392,0.004935,1.85797,-2.19051,-0.019243,1.77014,-2.24495,0.111315,1.63209,-2.20454,0.1458,1.5766,-2.22832,0.242793,1.43421,-1.81469,0.060191,1.52137,-1.89642,0.062404,1.54775,-1.93947,0.065523,1.52981,-2.01289,0.248259,1.47208,-2.07039,0.114558,1.1347,-1.77 [...]
+/*1666*/{0.299584,1.95926,-1.7591,0.347437,1.89786,-1.91576,0.274163,1.81061,-1.63938,0.250727,1.69724,-1.5482,0.362854,1.72349,-1.66184,0.439895,1.73323,-1.71065,0.236113,1.97284,-2.13317,0.34047,1.89942,-1.99049,0.123743,1.92958,-2.02466,0.00425,1.8621,-2.18814,-0.020921,1.77441,-2.24361,0.11024,1.6358,-2.20445,0.144405,1.58064,-2.22887,0.242662,1.43663,-1.81468,0.061686,1.52554,-1.89456,0.062517,1.55123,-1.93868,0.065266,1.53346,-2.01099,0.24833,1.47505,-2.07022,0.114871,1.13926,-1.77 [...]
+/*1667*/{0.299663,1.96136,-1.7603,0.347753,1.90056,-1.91687,0.27423,1.81422,-1.63962,0.251039,1.70157,-1.54782,0.361589,1.72674,-1.66365,0.439481,1.73604,-1.7118,0.236775,1.97537,-2.13442,0.340445,1.90172,-1.9921,0.124707,1.9316,-2.02443,0.004392,1.86537,-2.18791,-0.022798,1.77781,-2.24217,0.109263,1.63874,-2.20486,0.143602,1.58426,-2.22908,0.243335,1.43982,-1.81444,0.061815,1.52891,-1.89437,0.063157,1.55461,-1.93655,0.066385,1.53743,-2.0105,0.247702,1.47792,-2.0701,0.1132,1.14075,-1.768 [...]
+/*1668*/{0.299867,1.96391,-1.76188,0.34812,1.90387,-1.91859,0.273245,1.8183,-1.63955,0.249154,1.7049,-1.54721,0.360645,1.72882,-1.66288,0.437762,1.73806,-1.71327,0.236932,1.978,-2.13496,0.34114,1.90331,-1.99341,0.125805,1.93446,-2.02553,0.003622,1.86826,-2.18603,-0.023967,1.78089,-2.24122,0.110281,1.64279,-2.20568,0.142984,1.58718,-2.22975,0.244104,1.44273,-1.81442,0.062085,1.53238,-1.89334,0.063075,1.55788,-1.9369,0.065037,1.53994,-2.00875,0.247302,1.48092,-2.06978,0.11274,1.1433,-1.767 [...]
+/*1669*/{0.300126,1.96602,-1.76241,0.347622,1.90628,-1.91997,0.272115,1.8214,-1.6399,0.247826,1.70778,-1.54731,0.359043,1.73232,-1.66284,0.435713,1.73987,-1.71501,0.237551,1.98006,-2.13612,0.340737,1.90475,-1.99444,0.125655,1.93686,-2.02655,0.00308,1.87083,-2.18478,-0.024462,1.7831,-2.24059,0.109994,1.64564,-2.20577,0.143572,1.58957,-2.22967,0.244892,1.44558,-1.81372,0.062497,1.5349,-1.89117,0.063447,1.5615,-1.9357,0.064609,1.54355,-2.00775,0.247481,1.4834,-2.06971,0.115102,1.14667,-1.76 [...]
+/*1670*/{0.30021,1.96829,-1.76388,0.347954,1.90812,-1.9209,0.270749,1.82499,-1.64082,0.24632,1.71033,-1.54709,0.357421,1.73326,-1.66457,0.434499,1.74206,-1.71627,0.23769,1.98202,-2.13696,0.341052,1.90657,-1.99539,0.126403,1.93872,-2.02699,0.004436,1.87353,-2.18372,-0.025584,1.785,-2.24019,0.11029,1.64747,-2.20615,0.143332,1.59198,-2.23036,0.244957,1.44803,-1.81303,0.063098,1.5377,-1.89086,0.063806,1.56395,-1.93439,0.064383,1.54614,-2.00648,0.247224,1.48545,-2.06921,0.115839,1.14632,-1.76 [...]
+/*1671*/{0.30118,1.97044,-1.76489,0.347386,1.90944,-1.9225,0.269001,1.82711,-1.64131,0.24483,1.71399,-1.54758,0.355005,1.73481,-1.6653,0.431681,1.74113,-1.71749,0.237698,1.98346,-2.13799,0.340154,1.90743,-1.99614,0.12672,1.9407,-2.02807,0.003379,1.87487,-2.18238,-0.025833,1.78668,-2.23968,0.109945,1.64937,-2.20689,0.143776,1.59394,-2.23104,0.245408,1.45108,-1.81358,0.063449,1.53972,-1.88964,0.062491,1.56642,-1.93298,0.062864,1.54795,-2.00397,0.246209,1.48722,-2.06818,0.119838,1.14765,-1. [...]
+/*1672*/{0.300886,1.97125,-1.76629,0.347133,1.91126,-1.92383,0.267151,1.82962,-1.64167,0.241844,1.71566,-1.54786,0.353501,1.73521,-1.66626,0.429445,1.74165,-1.71786,0.238746,1.98531,-2.1388,0.341079,1.90966,-1.99683,0.127155,1.94242,-2.0289,0.00343,1.87618,-2.18182,-0.02561,1.7873,-2.2398,0.111146,1.65105,-2.20723,0.144244,1.59518,-2.23141,0.246057,1.4533,-1.8134,0.06292,1.54164,-1.88984,0.062639,1.56796,-1.933,0.062643,1.54934,-2.00331,0.246234,1.48923,-2.0681,0.124886,1.14707,-1.77025, [...]
+/*1673*/{0.301418,1.97193,-1.76693,0.346871,1.91253,-1.92491,0.26512,1.83172,-1.64266,0.240527,1.7174,-1.54773,0.351803,1.73621,-1.66727,0.427481,1.74103,-1.71845,0.23823,1.98581,-2.1395,0.340362,1.91023,-1.99793,0.128082,1.94449,-2.02975,0.002962,1.87674,-2.18071,-0.024699,1.78802,-2.24014,0.112152,1.6514,-2.20703,0.145939,1.59624,-2.23213,0.246339,1.45499,-1.81313,0.063018,1.54255,-1.88874,0.063083,1.56898,-1.93277,0.061765,1.55086,-2.00216,0.246127,1.49002,-2.0672,0.129364,1.14628,-1. [...]
+/*1674*/{0.30087,1.97334,-1.76846,0.34671,1.91325,-1.92517,0.263026,1.83295,-1.64303,0.238062,1.71883,-1.5479,0.349712,1.73508,-1.66696,0.425494,1.73938,-1.71848,0.239375,1.98695,-2.14112,0.339845,1.91103,-1.99832,0.1281,1.94523,-2.03121,0.002842,1.87679,-2.17962,-0.024064,1.7877,-2.24014,0.113382,1.65238,-2.20785,0.146954,1.59675,-2.23292,0.246182,1.45663,-1.81299,0.063315,1.54366,-1.88758,0.062732,1.57025,-1.9321,0.061492,1.55117,-2.00176,0.246772,1.49112,-2.06697,0.13603,1.1454,-1.776 [...]
+/*1675*/{0.300077,1.97289,-1.76923,0.345067,1.91331,-1.9263,0.261122,1.83455,-1.6442,0.233706,1.71999,-1.54845,0.347112,1.73436,-1.66763,0.424178,1.73818,-1.71976,0.239574,1.98728,-2.14115,0.339078,1.91215,-1.99895,0.128554,1.94553,-2.03212,0.002695,1.87674,-2.17906,-0.023256,1.78708,-2.24001,0.115037,1.65219,-2.20807,0.148806,1.59701,-2.23354,0.246711,1.4572,-1.81283,0.062888,1.54389,-1.88668,0.062011,1.57163,-1.93133,0.061492,1.55117,-2.00176,0.245793,1.49226,-2.06582,0.130891,1.13989, [...]
+/*1676*/{0.299656,1.97286,-1.77005,0.344128,1.91345,-1.92684,0.258385,1.83512,-1.64442,0.231587,1.72074,-1.5491,0.344798,1.73307,-1.66796,0.420752,1.73526,-1.71967,0.239523,1.98697,-2.14139,0.338582,1.91273,-1.99933,0.128309,1.94667,-2.0331,0.00378,1.8754,-2.17841,-0.022338,1.78603,-2.24037,0.116541,1.65196,-2.2083,0.150499,1.59621,-2.23408,0.246548,1.45755,-1.81241,0.062545,1.54365,-1.88626,0.061917,1.57162,-1.93123,0.063972,1.54891,-2.00052,0.245812,1.49232,-2.06451,0.150106,1.14016,-1 [...]
+/*1677*/{0.299326,1.97209,-1.77098,0.343922,1.91299,-1.92716,0.256405,1.83547,-1.64507,0.228649,1.72064,-1.54958,0.342466,1.73113,-1.66799,0.419337,1.73235,-1.72018,0.239805,1.98653,-2.14162,0.33728,1.91253,-1.99974,0.1286,1.94716,-2.0344,0.003834,1.87387,-2.179,-0.021163,1.78483,-2.24048,0.117918,1.65094,-2.20917,0.15261,1.59587,-2.23505,0.246419,1.45714,-1.81227,0.061943,1.54248,-1.8862,0.061448,1.57064,-1.93054,0.062706,1.54744,-2.00072,0.246399,1.49246,-2.06412,0.157301,1.13958,-1.78 [...]
+/*1678*/{0.298651,1.97221,-1.77138,0.342617,1.91287,-1.92745,0.25399,1.83513,-1.64606,0.225211,1.72024,-1.55039,0.33931,1.72878,-1.66862,0.416462,1.72878,-1.71989,0.239703,1.98562,-2.14216,0.336272,1.91093,-1.99999,0.128795,1.94705,-2.03538,0.004206,1.87217,-2.17744,-0.02021,1.78286,-2.24117,0.118934,1.65018,-2.21061,0.153985,1.59517,-2.23561,0.246387,1.45687,-1.81237,0.062155,1.54143,-1.88589,0.061876,1.56933,-1.93035,0.063242,1.54585,-2.00046,0.246627,1.49238,-2.06366,0.164193,1.13608, [...]
+/*1679*/{0.297582,1.97,-1.77192,0.342154,1.91146,-1.92759,0.251344,1.83481,-1.64653,0.222093,1.71975,-1.55151,0.337251,1.726,-1.66856,0.413946,1.72443,-1.72017,0.239843,1.98477,-2.14219,0.33588,1.91029,-2.00031,0.129074,1.94605,-2.03588,0.00427,1.8704,-2.17849,-0.019081,1.7803,-2.24238,0.121569,1.64926,-2.21129,0.156724,1.59373,-2.23685,0.246814,1.45603,-1.81257,0.062089,1.54018,-1.88506,0.061867,1.56869,-1.93037,0.061929,1.54435,-2.00002,0.24608,1.49133,-2.06301,0.170826,1.1327,-1.79244 [...]
+/*1680*/{0.296947,1.96853,-1.77225,0.3407,1.91037,-1.92755,0.248374,1.83343,-1.64698,0.216782,1.71842,-1.55271,0.333855,1.72281,-1.66847,0.411255,1.71982,-1.72026,0.240137,1.98294,-2.1421,0.335163,1.90937,-2.00018,0.12804,1.94474,-2.03742,0.006181,1.86737,-2.17843,-0.017528,1.77788,-2.24377,0.123304,1.64667,-2.21202,0.158652,1.59174,-2.23766,0.248006,1.45516,-1.81372,0.061626,1.53788,-1.88517,0.060949,1.56632,-1.93118,0.062878,1.54241,-2.00033,0.246259,1.48941,-2.06277,0.17702,1.13052,-1 [...]
+/*1681*/{0.295009,1.96699,-1.77321,0.339856,1.90889,-1.92794,0.246145,1.83204,-1.6479,0.213167,1.7173,-1.5532,0.330505,1.71932,-1.66899,0.408454,1.71474,-1.7191,0.239832,1.98119,-2.1424,0.333603,1.90729,-1.99972,0.127219,1.94324,-2.03894,0.006024,1.8656,-2.17949,-0.016494,1.77441,-2.24443,0.125241,1.64376,-2.21306,0.161505,1.58958,-2.23916,0.248411,1.45478,-1.81421,0.061752,1.53589,-1.8851,0.059934,1.56426,-1.93105,0.062596,1.5401,-2.00046,0.245679,1.48783,-2.06303,0.184351,1.12903,-1.79 [...]
+/*1682*/{0.293815,1.96446,-1.77311,0.338256,1.9058,-1.92761,0.243161,1.83,-1.64834,0.20981,1.71548,-1.55454,0.326819,1.71543,-1.66893,0.405261,1.70877,-1.71843,0.239585,1.97901,-2.14223,0.333026,1.90536,-1.99949,0.127175,1.94271,-2.03941,0.006595,1.86193,-2.17979,-0.015033,1.77076,-2.24654,0.12717,1.64089,-2.21464,0.163762,1.58741,-2.24006,0.248292,1.45282,-1.81387,0.061936,1.533,-1.88442,0.060892,1.56108,-1.93104,0.062334,1.53663,-2.00136,0.245783,1.48604,-2.06365,0.190839,1.12666,-1.80 [...]
+/*1683*/{0.292719,1.96171,-1.77289,0.337157,1.9041,-1.9277,0.240605,1.8275,-1.64816,0.204132,1.71281,-1.55518,0.322612,1.7105,-1.66863,0.402183,1.7025,-1.7179,0.237844,1.97618,-2.14237,0.332254,1.903,-1.9996,0.125427,1.93939,-2.04078,0.005175,1.86117,-2.18247,-0.014307,1.76688,-2.24759,0.128751,1.63753,-2.21633,0.166695,1.58418,-2.2415,0.248492,1.45113,-1.81512,0.062115,1.52981,-1.88462,0.060039,1.55852,-1.93189,0.061861,1.53432,-2.0014,0.245045,1.48431,-2.06441,0.196697,1.12332,-1.80303 [...]
+/*1684*/{0.290742,1.95919,-1.77286,0.337214,1.90193,-1.9281,0.237595,1.82479,-1.64869,0.199579,1.70997,-1.55664,0.319174,1.70539,-1.6683,0.398714,1.69566,-1.71649,0.237403,1.97298,-2.14245,0.331307,1.90016,-1.99957,0.124843,1.93773,-2.04212,0.006258,1.85478,-2.18311,-0.012796,1.76225,-2.25018,0.130706,1.634,-2.21777,0.169138,1.58105,-2.24297,0.248233,1.44924,-1.81611,0.062163,1.52729,-1.88429,0.058996,1.55619,-1.93235,0.061563,1.53195,-2.00135,0.244356,1.48225,-2.06587,0.2022,1.12169,-1. [...]
+/*1685*/{0.289729,1.95643,-1.77211,0.335489,1.89888,-1.92764,0.234776,1.82159,-1.64864,0.194168,1.70652,-1.5574,0.314385,1.7002,-1.6677,0.394881,1.68858,-1.7152,0.237352,1.97007,-2.14286,0.330489,1.89734,-1.99911,0.123603,1.93405,-2.04247,0.006675,1.84923,-2.18456,-0.012117,1.75683,-2.25187,0.132878,1.63028,-2.21986,0.172477,1.57749,-2.24411,0.248798,1.44719,-1.81739,0.062166,1.52409,-1.88425,0.059944,1.55276,-1.93213,0.060619,1.5281,-2.00115,0.244412,1.48034,-2.06724,0.207669,1.11797,-1 [...]
+/*1686*/{0.287732,1.95247,-1.77177,0.335426,1.89611,-1.92773,0.231148,1.8176,-1.64885,0.18904,1.70292,-1.55872,0.31021,1.69435,-1.66738,0.390759,1.68067,-1.71355,0.236153,1.96637,-2.14301,0.329559,1.89386,-1.99931,0.122676,1.93084,-2.04322,0.006586,1.84362,-2.18578,-0.010802,1.7514,-2.25398,0.134659,1.62534,-2.22152,0.175515,1.57363,-2.24557,0.249398,1.44483,-1.81835,0.061185,1.5205,-1.88416,0.059181,1.54942,-1.93269,0.060785,1.52422,-2.00088,0.243216,1.4788,-2.06832,0.21274,1.1165,-1.80 [...]
+/*1687*/{0.286255,1.94891,-1.77098,0.334372,1.89251,-1.92662,0.228104,1.81345,-1.64917,0.183582,1.69992,-1.55982,0.305652,1.68785,-1.66771,0.38675,1.67289,-1.71257,0.235809,1.96228,-2.14398,0.328715,1.88989,-1.99896,0.121474,1.92639,-2.04443,0.007026,1.83869,-2.19005,-0.010196,1.74537,-2.25592,0.137514,1.62072,-2.22393,0.177876,1.56872,-2.24676,0.248909,1.44211,-1.81999,0.061369,1.51686,-1.88371,0.058859,1.54584,-1.93269,0.058639,1.52061,-2.00115,0.242385,1.47632,-2.06977,0.218644,1.1128 [...]
+/*1688*/{0.284899,1.94546,-1.77013,0.333849,1.8891,-1.92661,0.225381,1.80897,-1.6491,0.177571,1.69501,-1.56105,0.301467,1.68154,-1.66704,0.382705,1.66424,-1.71076,0.234644,1.95833,-2.14386,0.327708,1.88636,-1.9984,0.11998,1.92293,-2.04532,0.00604,1.83231,-2.19109,-0.008322,1.73888,-2.25827,0.140553,1.61538,-2.22512,0.181276,1.56398,-2.24816,0.248981,1.4391,-1.82142,0.060693,1.5117,-1.88337,0.057494,1.54147,-1.93314,0.057995,1.51675,-2.00064,0.241774,1.47357,-2.07174,0.222669,1.11161,-1.8 [...]
+/*1689*/{0.282977,1.94085,-1.76941,0.33262,1.88397,-1.92615,0.221657,1.80423,-1.64918,0.171281,1.69215,-1.56237,0.295544,1.67492,-1.66657,0.377875,1.65546,-1.70905,0.23402,1.95415,-2.14412,0.326875,1.88236,-1.99817,0.118374,1.91733,-2.04562,0.005803,1.82683,-2.19247,-0.006982,1.73218,-2.26069,0.14305,1.61101,-2.22796,0.184282,1.55901,-2.24916,0.249296,1.43664,-1.82356,0.059996,1.5074,-1.88355,0.057084,1.53695,-1.93264,0.056702,1.51143,-2.00001,0.239762,1.47033,-2.07354,0.225304,1.10781,- [...]
+/*1690*/{0.28103,1.93612,-1.76928,0.331393,1.87974,-1.92502,0.217949,1.79985,-1.64926,0.16574,1.68754,-1.56406,0.290712,1.66789,-1.66597,0.371718,1.64611,-1.70719,0.232227,1.94956,-2.14465,0.32626,1.87752,-1.99829,0.116903,1.91256,-2.04639,0.004038,1.82047,-2.19384,-0.005157,1.72471,-2.26288,0.146297,1.60382,-2.2288,0.18837,1.55284,-2.2497,0.249619,1.43301,-1.82586,0.059555,1.50209,-1.88401,0.055492,1.53253,-1.93451,0.054895,1.50675,-2.00006,0.23839,1.46679,-2.07563,0.230989,1.10474,-1.8 [...]
+/*1691*/{0.279324,1.93102,-1.76806,0.330897,1.87523,-1.92481,0.213885,1.79424,-1.64897,0.159827,1.68405,-1.56575,0.285231,1.66011,-1.66557,0.367123,1.6369,-1.70552,0.231995,1.94467,-2.14492,0.326114,1.87378,-1.99749,0.115344,1.90739,-2.04746,0.005656,1.81019,-2.19829,-0.003966,1.71736,-2.26512,0.149695,1.59774,-2.2299,0.191775,1.54721,-2.25072,0.249217,1.43052,-1.8275,0.059443,1.49783,-1.88254,0.05593,1.52733,-1.9339,0.053745,1.50139,-1.99896,0.237178,1.46378,-2.07748,0.234788,1.10112,-1 [...]
+/*1692*/{0.278105,1.9258,-1.76687,0.330919,1.86952,-1.92375,0.210006,1.78901,-1.64938,0.153451,1.67953,-1.56726,0.280206,1.65311,-1.66531,0.361695,1.62712,-1.70351,0.232119,1.94004,-2.14552,0.325809,1.86911,-1.99736,0.114351,1.90124,-2.04805,0.007745,1.80164,-2.20086,-0.001558,1.70916,-2.2673,0.152447,1.59197,-2.23198,0.19555,1.54018,-2.25101,0.249315,1.42678,-1.8298,0.057986,1.4928,-1.88381,0.054683,1.52164,-1.93494,0.052392,1.49581,-1.99911,0.235681,1.45923,-2.07978,0.235577,1.09951,-1 [...]
+/*1693*/{0.276447,1.92065,-1.76595,0.330336,1.86356,-1.92304,0.206165,1.78394,-1.64972,0.147884,1.67581,-1.56948,0.273953,1.64497,-1.66472,0.355241,1.61791,-1.70133,0.231197,1.9348,-2.1462,0.325391,1.86391,-1.99711,0.113368,1.89574,-2.04867,0.008618,1.79232,-2.20373,0.00037,1.70135,-2.26966,0.156656,1.58492,-2.23282,0.200054,1.53381,-2.2517,0.248943,1.42303,-1.83205,0.057491,1.48673,-1.88379,0.0532,1.51573,-1.93557,0.05162,1.48972,-1.99865,0.235294,1.4557,-2.08169,0.239673,1.09539,-1.795 [...]
+/*1694*/{0.275102,1.91444,-1.76525,0.329915,1.85825,-1.92223,0.202439,1.77797,-1.65038,0.139724,1.67141,-1.57095,0.268407,1.63749,-1.66497,0.350049,1.60823,-1.70021,0.231262,1.9291,-2.14664,0.325274,1.85831,-1.99641,0.112337,1.88895,-2.04863,0.008906,1.78305,-2.20573,0.003647,1.69231,-2.27137,0.160943,1.57766,-2.23358,0.204467,1.52633,-2.25216,0.249316,1.41917,-1.83345,0.057244,1.48167,-1.883,0.052927,1.51022,-1.93666,0.049811,1.48353,-1.99842,0.23464,1.45168,-2.08402,0.239986,1.09316,-1 [...]
+/*1695*/{0.273294,1.90869,-1.76431,0.329391,1.85184,-1.92068,0.198373,1.77224,-1.65077,0.134229,1.6671,-1.57242,0.262405,1.63045,-1.66404,0.343526,1.59871,-1.69833,0.231411,1.92399,-2.14749,0.325804,1.8532,-1.99619,0.110135,1.88381,-2.05001,0.009275,1.77415,-2.20866,0.007106,1.68303,-2.27375,0.164727,1.56977,-2.23397,0.209452,1.51939,-2.25223,0.248712,1.41449,-1.83543,0.0557,1.47564,-1.88355,0.052347,1.50441,-1.93763,0.048467,1.47846,-1.99787,0.234106,1.44699,-2.08604,0.244317,1.08873,-1 [...]
+/*1696*/{0.271243,1.90244,-1.76369,0.328133,1.84548,-1.91999,0.193661,1.76653,-1.65148,0.127238,1.66293,-1.57422,0.256474,1.62211,-1.66359,0.336178,1.58965,-1.69683,0.231823,1.919,-2.148,0.325735,1.84735,-1.99589,0.109275,1.87669,-2.05059,0.0119,1.76429,-2.21159,0.01102,1.67394,-2.27494,0.169151,1.56165,-2.23425,0.214355,1.51129,-2.25261,0.248852,1.40999,-1.83571,0.054406,1.46949,-1.88401,0.051641,1.49864,-1.93854,0.046756,1.47263,-1.99808,0.23302,1.4417,-2.08869,0.245811,1.0869,-1.78752 [...]
+/*1697*/{0.269723,1.89603,-1.76283,0.32803,1.83888,-1.91887,0.189351,1.76077,-1.65193,0.119022,1.65886,-1.57623,0.24962,1.61414,-1.66348,0.330007,1.58058,-1.69496,0.231657,1.91368,-2.1484,0.325365,1.84125,-1.99532,0.109098,1.87115,-2.05131,0.013107,1.75461,-2.21445,0.015607,1.66457,-2.27748,0.174512,1.55317,-2.23473,0.219621,1.50373,-2.25272,0.247552,1.40506,-1.83627,0.052965,1.46447,-1.88405,0.049738,1.49236,-1.93874,0.04507,1.46681,-1.998,0.231225,1.43536,-2.09064,0.249715,1.08231,-1.7 [...]
+/*1698*/{0.267451,1.88983,-1.7622,0.328159,1.83275,-1.91796,0.184458,1.75458,-1.65272,0.113256,1.65533,-1.57861,0.243413,1.60742,-1.66348,0.323414,1.57225,-1.69218,0.232232,1.90831,-2.14957,0.325495,1.83558,-1.99518,0.108981,1.86601,-2.0512,0.016092,1.74482,-2.21754,0.020138,1.6554,-2.27919,0.178993,1.54587,-2.235,0.225021,1.49546,-2.25296,0.247314,1.39939,-1.83795,0.050936,1.45885,-1.88427,0.048023,1.48598,-1.93925,0.042154,1.46147,-1.99836,0.22991,1.43002,-2.09253,0.254374,1.07743,-1.7 [...]
+/*1699*/{0.265541,1.88398,-1.76133,0.328581,1.8265,-1.91771,0.179232,1.7495,-1.65372,0.106343,1.65213,-1.58091,0.236697,1.6009,-1.66241,0.31513,1.56432,-1.6911,0.232972,1.90307,-2.15049,0.327082,1.82938,-1.99463,0.10974,1.86175,-2.05089,0.020434,1.73413,-2.22053,0.025471,1.6454,-2.28115,0.185121,1.537,-2.23553,0.230554,1.48753,-2.25311,0.24628,1.39417,-1.83883,0.048343,1.45304,-1.88533,0.044485,1.47964,-1.93906,0.038833,1.45726,-1.99975,0.226874,1.42431,-2.09429,0.256928,1.07491,-1.78103 [...]
+/*1700*/{0.264079,1.87832,-1.76047,0.3279,1.81944,-1.91625,0.174636,1.74451,-1.65445,0.099909,1.64847,-1.58371,0.22995,1.59473,-1.66156,0.307812,1.55654,-1.68927,0.233463,1.89797,-2.15155,0.327343,1.82344,-1.9941,0.109604,1.85851,-2.05184,0.02395,1.72576,-2.22432,0.031567,1.63556,-2.28255,0.191895,1.52966,-2.23637,0.236538,1.48042,-2.25338,0.245422,1.38942,-1.84026,0.046077,1.44764,-1.88633,0.042004,1.47437,-1.93851,0.03569,1.4539,-2.00033,0.224121,1.42033,-2.09506,0.259958,1.07155,-1.77 [...]
+/*1701*/{0.2625,1.87342,-1.75909,0.328207,1.81341,-1.91484,0.170301,1.7401,-1.6549,0.092586,1.64491,-1.58548,0.22267,1.58968,-1.66082,0.300593,1.55019,-1.68723,0.233324,1.89316,-2.15303,0.327707,1.81762,-1.99311,0.110018,1.85563,-2.05269,0.029151,1.7178,-2.22876,0.03922,1.62645,-2.28523,0.197313,1.52132,-2.237,0.243619,1.4735,-2.2537,0.243466,1.38435,-1.84157,0.043101,1.4434,-1.88595,0.038904,1.46972,-1.93888,0.032373,1.44936,-2.00153,0.221607,1.41673,-2.09664,0.264665,1.06836,-1.77757,0 [...]
+/*1702*/{0.261182,1.86851,-1.75782,0.327426,1.80831,-1.91413,0.165566,1.73562,-1.65608,0.084006,1.64329,-1.58763,0.215682,1.58482,-1.66102,0.292595,1.5449,-1.68602,0.234886,1.88911,-2.15385,0.327698,1.81343,-1.99241,0.109893,1.85275,-2.0525,0.036605,1.70826,-2.23369,0.046285,1.61638,-2.28582,0.205644,1.51451,-2.23808,0.251445,1.46692,-2.25344,0.24171,1.3797,-1.84292,0.039165,1.43894,-1.88701,0.037909,1.46721,-1.93866,0.029321,1.44611,-2.00099,0.218888,1.41362,-2.09731,0.268758,1.06924,-1 [...]
+/*1703*/{0.259289,1.86432,-1.75668,0.326995,1.80386,-1.91264,0.160803,1.73258,-1.65681,0.07572,1.64188,-1.58997,0.208296,1.58088,-1.66087,0.28425,1.53894,-1.6834,0.236315,1.88501,-2.15432,0.32724,1.80944,-1.99132,0.110131,1.85068,-2.05186,0.04351,1.69945,-2.23747,0.055004,1.60684,-2.28689,0.213448,1.50842,-2.23834,0.259559,1.46103,-2.25292,0.239124,1.37663,-1.84402,0.036949,1.43513,-1.88728,0.035192,1.46681,-1.93879,0.026771,1.44395,-1.99998,0.217027,1.4113,-2.09795,0.275621,1.07113,-1.7 [...]
+/*1704*/{0.25918,1.86071,-1.75484,0.326591,1.80069,-1.91128,0.154777,1.73043,-1.65728,0.067988,1.64119,-1.59297,0.200653,1.5784,-1.66137,0.276956,1.5346,-1.68128,0.236409,1.8816,-2.1541,0.327065,1.80591,-1.98944,0.110332,1.84894,-2.05297,0.048342,1.69149,-2.24002,0.0639,1.59809,-2.2872,0.222341,1.50267,-2.23826,0.268555,1.45658,-2.252,0.237216,1.3721,-1.84411,0.034963,1.43306,-1.88633,0.034211,1.4665,-1.93858,0.025913,1.44277,-1.99965,0.214763,1.40974,-2.09891,0.27869,1.07355,-1.77607,0. [...]
+/*1705*/{0.258137,1.85724,-1.75339,0.325046,1.79859,-1.91041,0.149348,1.72968,-1.65886,0.060515,1.6418,-1.59573,0.193327,1.57571,-1.66196,0.268805,1.53076,-1.67958,0.237896,1.87846,-2.15307,0.326912,1.8033,-1.98954,0.110191,1.84693,-2.05306,0.055053,1.6838,-2.24253,0.072926,1.59094,-2.28718,0.2307,1.49848,-2.23761,0.278724,1.45307,-2.2504,0.233954,1.36894,-1.84288,0.033831,1.43124,-1.88575,0.031583,1.46508,-1.9394,0.024746,1.44245,-1.99737,0.21305,1.40891,-2.09962,0.285158,1.07735,-1.776 [...]
+/*1706*/{0.257313,1.85486,-1.75284,0.325245,1.79631,-1.90965,0.143899,1.72955,-1.65962,0.052172,1.64287,-1.59911,0.186437,1.57386,-1.66109,0.260801,1.52811,-1.6793,0.238934,1.8763,-2.15271,0.326366,1.80065,-1.98784,0.110788,1.84606,-2.05291,0.060677,1.67765,-2.24378,0.079924,1.58508,-2.28643,0.240082,1.49445,-2.23626,0.288667,1.45066,-2.24868,0.231058,1.36598,-1.84365,0.031378,1.43041,-1.88473,0.030243,1.46411,-1.93873,0.02391,1.44214,-1.99622,0.211958,1.40816,-2.10027,0.292087,1.08187,- [...]
+/*1707*/{0.256354,1.85329,-1.75197,0.32277,1.79424,-1.90839,0.138621,1.73031,-1.66064,0.04438,1.64567,-1.60252,0.179063,1.57376,-1.66133,0.252197,1.52623,-1.67855,0.241206,1.87399,-2.1514,0.325872,1.79885,-1.98727,0.110907,1.84442,-2.05324,0.066772,1.67171,-2.24305,0.087204,1.58045,-2.28556,0.249739,1.49234,-2.23428,0.2995,1.4498,-2.24595,0.22914,1.36428,-1.84173,0.031174,1.42954,-1.88394,0.02848,1.46263,-1.93917,0.022462,1.44215,-1.9962,0.211236,1.40751,-2.10051,0.297849,1.08611,-1.7756 [...]
+/*1708*/{0.254759,1.85225,-1.75204,0.322193,1.7923,-1.90776,0.133928,1.73192,-1.66197,0.037858,1.64807,-1.60592,0.171092,1.5739,-1.66234,0.24493,1.52522,-1.67695,0.243003,1.87261,-2.15102,0.325938,1.79696,-1.98572,0.111204,1.84365,-2.05356,0.072114,1.66805,-2.24159,0.093793,1.57646,-2.28518,0.259815,1.49233,-2.23143,0.310057,1.45042,-2.2428,0.227017,1.36314,-1.83956,0.029749,1.42924,-1.88524,0.028118,1.46155,-1.93951,0.022462,1.44215,-1.9962,0.210076,1.40662,-2.10113,0.305268,1.09159,-1. [...]
+/*1709*/{0.253113,1.85194,-1.75105,0.320872,1.79108,-1.90626,0.128415,1.73383,-1.66314,0.030155,1.6517,-1.60882,0.164072,1.57477,-1.66237,0.237355,1.52519,-1.67612,0.245198,1.87157,-2.14983,0.32526,1.79572,-1.98508,0.111775,1.84272,-2.05439,0.076318,1.6647,-2.23997,0.100797,1.57373,-2.28359,0.268595,1.49222,-2.22831,0.319473,1.45214,-2.23894,0.224433,1.36235,-1.83831,0.028804,1.42881,-1.8858,0.026729,1.46133,-1.93982,0.021306,1.44206,-1.99691,0.210893,1.40572,-2.10128,0.311818,1.09818,-1 [...]
+/*1710*/{0.250368,1.85186,-1.75092,0.32045,1.79069,-1.90578,0.123015,1.73679,-1.66435,0.023647,1.65544,-1.61178,0.157499,1.57638,-1.662,0.229992,1.52626,-1.67608,0.247655,1.87076,-2.14952,0.324311,1.79498,-1.98342,0.112897,1.84243,-2.0553,0.081001,1.66257,-2.23804,0.10791,1.571,-2.28268,0.2771,1.49355,-2.22412,0.329303,1.45522,-2.23402,0.222885,1.3622,-1.83613,0.026875,1.42944,-1.88717,0.026354,1.46075,-1.94069,0.021227,1.44135,-1.99805,0.210997,1.4047,-2.10152,0.318004,1.10451,-1.77571, [...]
+/*1711*/{0.248299,1.85332,-1.75028,0.319186,1.7902,-1.90383,0.11774,1.73989,-1.66547,0.016666,1.65972,-1.61519,0.150907,1.57823,-1.66233,0.222614,1.52837,-1.67635,0.24917,1.87074,-2.14913,0.324011,1.79477,-1.98233,0.113398,1.84235,-2.05666,0.085375,1.66067,-2.23622,0.114636,1.56963,-2.28172,0.285577,1.4973,-2.22023,0.338746,1.4601,-2.2288,0.220575,1.36252,-1.83399,0.026875,1.42944,-1.88717,0.025466,1.46094,-1.9413,0.021571,1.44104,-1.99932,0.21086,1.40446,-2.10147,0.324313,1.11135,-1.775 [...]
+/*1712*/{0.244855,1.85492,-1.75022,0.318542,1.79082,-1.90214,0.112435,1.74315,-1.667,0.010844,1.66388,-1.61801,0.14372,1.58142,-1.6636,0.215541,1.53059,-1.67604,0.251164,1.871,-2.14743,0.32387,1.79518,-1.98055,0.113921,1.84243,-2.05755,0.089468,1.65972,-2.23389,0.12107,1.56777,-2.28049,0.293275,1.50027,-2.21506,0.347915,1.46529,-2.22298,0.218406,1.36307,-1.83232,0.025294,1.43041,-1.88948,0.025288,1.4611,-1.94345,0.021192,1.44135,-2.00025,0.210445,1.40341,-2.10169,0.329999,1.11867,-1.7758 [...]
+/*1713*/{0.242206,1.85738,-1.74949,0.316945,1.79265,-1.90104,0.107532,1.747,-1.66811,0.004132,1.66821,-1.6207,0.137721,1.58528,-1.66433,0.208668,1.53345,-1.67569,0.253289,1.87207,-2.14657,0.32427,1.79655,-1.97889,0.114865,1.84289,-2.05865,0.093654,1.65807,-2.23106,0.128182,1.56739,-2.27953,0.300708,1.50598,-2.21025,0.357143,1.47224,-2.2162,0.216731,1.36493,-1.83155,0.025283,1.43028,-1.89051,0.02569,1.46171,-1.94452,0.020624,1.44184,-2.00341,0.211126,1.40263,-2.10199,0.337178,1.12512,-1.7 [...]
+/*1714*/{0.238649,1.85959,-1.74909,0.316032,1.79436,-1.89858,0.102607,1.75073,-1.66915,-0.003761,1.67193,-1.62324,0.13083,1.5897,-1.66556,0.201551,1.53759,-1.67643,0.254985,1.87383,-2.14555,0.32398,1.79797,-1.97717,0.11531,1.84411,-2.06015,0.096972,1.65793,-2.22951,0.134048,1.56703,-2.27763,0.307128,1.51083,-2.20455,0.364936,1.47942,-2.21029,0.215621,1.36698,-1.83023,0.024798,1.43119,-1.89234,0.026348,1.46166,-1.94586,0.020954,1.44258,-2.00433,0.21183,1.40159,-2.10218,0.339462,1.13289,-1 [...]
+/*1715*/{0.235526,1.86229,-1.74817,0.315653,1.79574,-1.89648,0.09745,1.75477,-1.6696,-0.008653,1.67633,-1.62562,0.124786,1.59444,-1.66648,0.195295,1.5417,-1.6763,0.256163,1.87558,-2.14418,0.323437,1.79965,-1.975,0.11575,1.84443,-2.06069,0.101095,1.65657,-2.22707,0.141134,1.5681,-2.27712,0.314198,1.51749,-2.19888,0.373187,1.48818,-2.20353,0.21575,1.36924,-1.83029,0.02437,1.43266,-1.89511,0.026082,1.46301,-1.94714,0.021334,1.44308,-2.00673,0.211614,1.40066,-2.10257,0.344298,1.14075,-1.7739 [...]
+/*1716*/{0.231569,1.86538,-1.74822,0.314926,1.79792,-1.89453,0.092866,1.75889,-1.67114,-0.015237,1.68189,-1.62826,0.118661,1.59932,-1.66812,0.189616,1.54637,-1.67715,0.258227,1.87783,-2.14311,0.323693,1.80208,-1.97307,0.116545,1.84583,-2.06119,0.105894,1.65586,-2.22499,0.148598,1.56864,-2.27552,0.32174,1.52556,-2.19366,0.380566,1.49692,-2.19706,0.215799,1.37139,-1.83011,0.023824,1.43301,-1.89708,0.026276,1.46423,-1.94897,0.022074,1.4445,-2.00831,0.21254,1.40023,-2.10268,0.34935,1.14769,- [...]
+/*1717*/{0.228131,1.86822,-1.74783,0.314168,1.80053,-1.89273,0.08814,1.76323,-1.67243,-0.02192,1.68599,-1.63098,0.113009,1.60482,-1.6702,0.183654,1.55109,-1.6775,0.260207,1.88097,-2.14176,0.323787,1.8046,-1.97159,0.117342,1.84609,-2.0612,0.109077,1.65518,-2.22336,0.154972,1.57007,-2.27478,0.328023,1.533,-2.18822,0.387928,1.50628,-2.19005,0.216258,1.37461,-1.82968,0.024166,1.43514,-1.89845,0.026545,1.46603,-1.95093,0.023394,1.44603,-2.01105,0.21403,1.39962,-2.10245,0.351491,1.15408,-1.773 [...]
+/*1718*/{0.225221,1.87116,-1.74729,0.313116,1.80493,-1.8916,0.083331,1.76765,-1.67336,-0.026555,1.69164,-1.63345,0.107592,1.61001,-1.67126,0.178029,1.55687,-1.67803,0.260489,1.88383,-2.14077,0.323633,1.80724,-1.96939,0.11821,1.84824,-2.06197,0.113221,1.65571,-2.22187,0.161886,1.57145,-2.2734,0.334002,1.53987,-2.18269,0.39458,1.51598,-2.18309,0.216369,1.37754,-1.8308,0.025026,1.43729,-1.89938,0.028044,1.46767,-1.95235,0.024603,1.44746,-2.01157,0.214501,1.39974,-2.10282,0.353892,1.16124,-1 [...]
+/*1719*/{0.2215,1.87467,-1.74766,0.312438,1.80854,-1.88977,0.079633,1.77153,-1.67416,-0.03155,1.69645,-1.63564,0.101852,1.61578,-1.67275,0.172441,1.56208,-1.67917,0.261818,1.8877,-2.13953,0.323946,1.81073,-1.96738,0.119087,1.84973,-2.06279,0.118815,1.65694,-2.22112,0.1685,1.57345,-2.27232,0.340334,1.54847,-2.17733,0.401937,1.52466,-2.17585,0.217046,1.38112,-1.83173,0.025422,1.44018,-1.90103,0.02964,1.46967,-1.95378,0.026107,1.44908,-2.01317,0.215929,1.39939,-2.10266,0.357449,1.16885,-1.7 [...]
+/*1720*/{0.218753,1.87779,-1.74774,0.312143,1.81038,-1.88716,0.07502,1.77642,-1.67528,-0.036449,1.70181,-1.63775,0.09715,1.62127,-1.67494,0.167136,1.56778,-1.68009,0.262294,1.89095,-2.13802,0.324626,1.81304,-1.96511,0.119625,1.85185,-2.06351,0.125373,1.65906,-2.22029,0.175346,1.57567,-2.27142,0.345513,1.5564,-2.17212,0.407466,1.53511,-2.16923,0.217835,1.38466,-1.83221,0.026221,1.4423,-1.90291,0.029965,1.47178,-1.95571,0.02817,1.4513,-2.01473,0.21692,1.39907,-2.10258,0.359214,1.17534,-1.7 [...]
+/*1721*/{0.216066,1.8813,-1.74744,0.31052,1.81552,-1.88624,0.071554,1.78083,-1.67612,-0.042339,1.70692,-1.64034,0.092366,1.62692,-1.67682,0.162939,1.57351,-1.68128,0.263186,1.89526,-2.13694,0.324919,1.81659,-1.96308,0.120924,1.85373,-2.06342,0.130034,1.66118,-2.22121,0.181856,1.57861,-2.27038,0.349935,1.56423,-2.16723,0.4136,1.54596,-2.16233,0.219543,1.38808,-1.83356,0.027524,1.44604,-1.90287,0.03246,1.47441,-1.95665,0.029914,1.45366,-2.01543,0.218619,1.39898,-2.10254,0.36086,1.18171,-1. [...]
+/*1722*/{0.213295,1.88419,-1.74744,0.310842,1.81957,-1.88476,0.06811,1.78558,-1.67687,-0.046147,1.71267,-1.64244,0.088118,1.63244,-1.67795,0.158039,1.57891,-1.68215,0.264905,1.89955,-2.13595,0.325172,1.81994,-1.96152,0.121391,1.8567,-2.0639,0.129908,1.6655,-2.2247,0.188968,1.58101,-2.2694,0.355089,1.57274,-2.16201,0.418446,1.55617,-2.15621,0.220617,1.39173,-1.83519,0.029273,1.44977,-1.90412,0.032964,1.47711,-1.95881,0.032382,1.4565,-2.0161,0.220548,1.39856,-2.10273,0.362635,1.18815,-1.77 [...]
+/*1723*/{0.211261,1.88728,-1.74716,0.31094,1.82182,-1.88288,0.065004,1.78959,-1.67849,-0.050991,1.71751,-1.64463,0.083799,1.63754,-1.68001,0.154553,1.58429,-1.6832,0.266674,1.90364,-2.13433,0.32538,1.82352,-1.96002,0.122856,1.85943,-2.06402,0.135747,1.66768,-2.22696,0.196842,1.58471,-2.26824,0.359494,1.58184,-2.15715,0.422811,1.5664,-2.15009,0.222137,1.39516,-1.83568,0.031037,1.45339,-1.90415,0.035566,1.4799,-1.95971,0.034713,1.45926,-2.0165,0.221663,1.39857,-2.10267,0.364745,1.19416,-1. [...]
+/*1724*/{0.209604,1.89047,-1.74704,0.309844,1.82671,-1.88225,0.061877,1.79399,-1.67972,-0.05503,1.72295,-1.64744,0.080384,1.6432,-1.68139,0.149438,1.58953,-1.68467,0.268361,1.90855,-2.13266,0.32573,1.82741,-1.95838,0.124059,1.8621,-2.0638,0.141488,1.6712,-2.22926,0.204063,1.58797,-2.26779,0.362498,1.58879,-2.15118,0.426981,1.57543,-2.14397,0.223463,1.39848,-1.83765,0.032006,1.45714,-1.90484,0.036912,1.48275,-1.9606,0.037791,1.46181,-2.01661,0.22373,1.39809,-2.10231,0.366454,1.19995,-1.77 [...]
+/*1725*/{0.208132,1.89295,-1.74688,0.309908,1.83088,-1.88068,0.059425,1.79798,-1.6805,-0.059011,1.72807,-1.65005,0.076703,1.64787,-1.68312,0.145416,1.59455,-1.68466,0.269362,1.91262,-2.13118,0.325977,1.83102,-1.95694,0.124997,1.86536,-2.06468,0.14788,1.67427,-2.23134,0.211137,1.59163,-2.26698,0.367381,1.59848,-2.14781,0.431173,1.58565,-2.13831,0.225172,1.40218,-1.83814,0.034092,1.46138,-1.90398,0.038971,1.4855,-1.96178,0.039563,1.46558,-2.01695,0.224921,1.39804,-2.10173,0.366312,1.20621, [...]
+/*1726*/{0.206542,1.89614,-1.74705,0.309893,1.83429,-1.87964,0.0567,1.80218,-1.68219,-0.0617,1.73283,-1.6524,0.07293,1.65322,-1.68458,0.142553,1.59931,-1.68596,0.270649,1.91672,-2.13015,0.326006,1.83379,-1.95545,0.126669,1.86742,-2.06463,0.154366,1.67777,-2.23387,0.21785,1.59539,-2.26603,0.370101,1.60624,-2.14262,0.434676,1.59488,-2.13381,0.226128,1.40533,-1.83936,0.036517,1.46575,-1.90475,0.041509,1.48943,-1.96292,0.042917,1.46872,-2.01734,0.226457,1.39764,-2.10109,0.367801,1.21039,-1.7 [...]
+/*1727*/{0.205006,1.89837,-1.74726,0.309731,1.83749,-1.87889,0.054486,1.80596,-1.68337,-0.065124,1.73741,-1.65467,0.069459,1.65756,-1.68595,0.139351,1.60403,-1.68607,0.273505,1.92085,-2.12803,0.326058,1.83709,-1.95402,0.127915,1.87049,-2.06489,0.160954,1.68114,-2.23623,0.225013,1.59904,-2.26538,0.372911,1.61353,-2.13853,0.437515,1.60347,-2.1282,0.228298,1.40868,-1.84034,0.038188,1.47017,-1.90342,0.042759,1.49236,-1.96383,0.044772,1.4716,-2.01755,0.229034,1.39765,-2.10043,0.369304,1.2153, [...]
+/*1728*/{0.203811,1.90074,-1.74729,0.310306,1.83944,-1.8775,0.052631,1.80976,-1.68491,-0.06771,1.74276,-1.65672,0.066934,1.66184,-1.68735,0.135804,1.60759,-1.68712,0.274867,1.924,-2.12699,0.326309,1.84078,-1.95306,0.129203,1.87295,-2.06542,0.167559,1.6839,-2.23829,0.231712,1.60282,-2.26459,0.376971,1.62222,-2.13434,0.440775,1.61206,-2.12334,0.228778,1.41239,-1.84148,0.040252,1.47489,-1.90361,0.044563,1.49567,-1.96444,0.046165,1.47443,-2.01845,0.228473,1.39802,-2.10041,0.370052,1.21916,-1 [...]
+/*1729*/{0.203634,1.90285,-1.7472,0.304053,1.84531,-1.87829,0.050284,1.81407,-1.68612,-0.071892,1.74603,-1.6598,0.063885,1.66562,-1.68883,0.135252,1.6115,-1.68738,0.276673,1.92777,-2.12485,0.326447,1.84395,-1.95173,0.130759,1.87496,-2.06535,0.174545,1.6875,-2.2406,0.238256,1.6065,-2.26422,0.379448,1.62946,-2.13089,0.443428,1.62089,-2.11809,0.229499,1.41507,-1.84077,0.041615,1.4797,-1.90464,0.046862,1.49978,-1.96497,0.049626,1.4772,-2.01745,0.229504,1.39833,-2.09941,0.370683,1.22238,-1.76 [...]
+/*1730*/{0.202857,1.90463,-1.74699,0.303827,1.84736,-1.87697,0.049472,1.81734,-1.68806,-0.07273,1.75047,-1.66219,0.061614,1.66937,-1.68945,0.132037,1.61444,-1.68772,0.277693,1.93043,-2.12385,0.327268,1.84673,-1.95067,0.132987,1.87809,-2.06572,0.18075,1.69129,-2.24241,0.244522,1.60993,-2.26393,0.38095,1.63458,-2.12749,0.445012,1.62813,-2.11395,0.229921,1.41898,-1.8418,0.043212,1.48384,-1.90383,0.048619,1.5032,-1.96598,0.052261,1.48035,-2.0171,0.231977,1.39845,-2.09864,0.370485,1.22528,-1. [...]
+/*1731*/{0.202831,1.90667,-1.74678,0.310329,1.84788,-1.87469,0.048595,1.82004,-1.68917,-0.07504,1.75424,-1.66503,0.059993,1.6725,-1.69081,0.129833,1.61768,-1.68795,0.279177,1.93307,-2.1217,0.328668,1.84867,-1.94877,0.134404,1.87846,-2.0674,0.187918,1.69447,-2.24389,0.248971,1.61339,-2.26425,0.382753,1.64109,-2.12476,0.44678,1.63524,-2.10943,0.230353,1.42231,-1.84244,0.044785,1.48791,-1.90317,0.049128,1.50644,-1.96669,0.052618,1.48396,-2.01811,0.232702,1.39837,-2.09814,0.370084,1.22693,-1 [...]
+/*1732*/{0.202615,1.90794,-1.74641,0.305348,1.85251,-1.87491,0.047146,1.82363,-1.69111,-0.076904,1.75755,-1.66716,0.058546,1.67513,-1.69117,0.129098,1.62022,-1.68768,0.280754,1.93551,-2.12063,0.328671,1.8517,-1.94837,0.136599,1.8805,-2.06769,0.193313,1.69711,-2.24431,0.253391,1.61644,-2.26425,0.384161,1.64569,-2.12172,0.448177,1.64128,-2.10557,0.229828,1.4254,-1.84353,0.046748,1.49169,-1.90256,0.051994,1.50978,-1.96743,0.053899,1.48645,-2.01777,0.233114,1.39926,-2.09705,0.368807,1.22921, [...]
+/*1733*/{0.202662,1.90959,-1.74617,0.306763,1.855,-1.87436,0.047124,1.82634,-1.69222,-0.078596,1.75993,-1.66938,0.057435,1.67723,-1.69225,0.127059,1.62188,-1.68748,0.282445,1.93749,-2.11914,0.329423,1.8542,-1.94728,0.137843,1.88244,-2.06808,0.198791,1.70039,-2.24575,0.257999,1.61902,-2.26418,0.38568,1.65164,-2.11967,0.449503,1.64666,-2.10204,0.229215,1.42785,-1.84427,0.048249,1.49433,-1.90292,0.052636,1.51288,-1.96749,0.054974,1.48927,-2.01785,0.23358,1.39979,-2.09681,0.368436,1.22945,-1 [...]
+/*1734*/{0.202996,1.91078,-1.74614,0.311565,1.85342,-1.87179,0.046431,1.828,-1.69332,-0.079645,1.76251,-1.672,0.056949,1.67981,-1.69318,0.126878,1.62315,-1.68697,0.28402,1.93832,-2.11788,0.330525,1.85537,-1.94555,0.139935,1.88393,-2.06816,0.200915,1.70293,-2.24542,0.261812,1.62125,-2.26426,0.387235,1.65517,-2.11818,0.450561,1.65066,-2.09861,0.229406,1.4304,-1.845,0.049191,1.49778,-1.90215,0.053996,1.51611,-1.96771,0.055992,1.49256,-2.01803,0.233586,1.40091,-2.09627,0.36789,1.22958,-1.766 [...]
+/*1735*/{0.203729,1.91204,-1.74532,0.308336,1.85803,-1.87229,0.046771,1.82991,-1.69446,-0.080841,1.76349,-1.67369,0.057247,1.68114,-1.69338,0.126544,1.62434,-1.68696,0.287031,1.94048,-2.11684,0.331544,1.85803,-1.94487,0.142267,1.88517,-2.06868,0.205066,1.7053,-2.24632,0.266264,1.62338,-2.26497,0.389232,1.65707,-2.1161,0.450424,1.65376,-2.09611,0.229177,1.4327,-1.84519,0.049268,1.50102,-1.90262,0.053856,1.51786,-1.9683,0.05728,1.4942,-2.01737,0.234235,1.40199,-2.09579,0.364979,1.22964,-1. [...]
+/*1736*/{0.204451,1.91287,-1.74443,0.308202,1.85895,-1.87153,0.046964,1.83132,-1.69517,-0.08127,1.7644,-1.67511,0.056694,1.68197,-1.6936,0.127637,1.6248,-1.68528,0.28902,1.94153,-2.11605,0.332048,1.85889,-1.94411,0.143561,1.8866,-2.06907,0.209579,1.7081,-2.24716,0.268555,1.62446,-2.26522,0.38961,1.65941,-2.11552,0.45116,1.65591,-2.09371,0.229602,1.43443,-1.84517,0.049706,1.50384,-1.90291,0.054241,1.52064,-1.96805,0.058206,1.49737,-2.01726,0.234269,1.40409,-2.09566,0.362783,1.22817,-1.767 [...]
+/*1737*/{0.205389,1.91383,-1.74401,0.3135,1.85712,-1.8695,0.046772,1.83241,-1.69715,-0.08036,1.76541,-1.67733,0.057096,1.68175,-1.69365,0.128445,1.62473,-1.68413,0.291563,1.94223,-2.11475,0.333329,1.8604,-1.94231,0.145175,1.88816,-2.06927,0.212818,1.70986,-2.2474,0.27184,1.62567,-2.26594,0.390346,1.66066,-2.11443,0.451178,1.65703,-2.09222,0.228955,1.43642,-1.84531,0.05035,1.50648,-1.9028,0.054718,1.5235,-1.96811,0.05761,1.49936,-2.0172,0.234591,1.40583,-2.09506,0.359421,1.22498,-1.76779, [...]
+/*1738*/{0.206217,1.91463,-1.74335,0.314753,1.85764,-1.86889,0.046921,1.83238,-1.69735,-0.079809,1.76484,-1.67844,0.057348,1.68123,-1.69414,0.128981,1.62451,-1.68389,0.293157,1.94266,-2.11395,0.333997,1.86059,-1.9418,0.146708,1.88988,-2.06953,0.215635,1.71122,-2.2473,0.274606,1.62645,-2.26664,0.390736,1.6608,-2.11362,0.452174,1.65681,-2.08988,0.226162,1.43909,-1.84526,0.050417,1.50861,-1.90232,0.055239,1.52606,-1.96853,0.058359,1.50217,-2.01657,0.234138,1.40765,-2.09539,0.358069,1.22163, [...]
+/*1739*/{0.207265,1.91531,-1.74263,0.315183,1.85868,-1.86795,0.047521,1.83229,-1.69773,-0.079099,1.76391,-1.67895,0.058738,1.68096,-1.69336,0.130504,1.6235,-1.68284,0.294443,1.94284,-2.11358,0.334677,1.86121,-1.94081,0.148201,1.89222,-2.07047,0.218681,1.7126,-2.24756,0.276365,1.62721,-2.267,0.391991,1.66111,-2.11346,0.452588,1.65651,-2.08875,0.229701,1.44086,-1.84496,0.049676,1.5098,-1.90212,0.054477,1.52783,-1.96811,0.05882,1.50364,-2.01592,0.234327,1.40978,-2.09527,0.352908,1.21786,-1. [...]
+/*1740*/{0.208216,1.91551,-1.7418,0.316303,1.85755,-1.86746,0.049333,1.8319,-1.69762,-0.078206,1.76247,-1.67984,0.060402,1.67984,-1.693,0.132151,1.62194,-1.68074,0.296175,1.94254,-2.11231,0.33549,1.86208,-1.93988,0.149483,1.8938,-2.07022,0.21941,1.71299,-2.24776,0.277163,1.62694,-2.26715,0.393309,1.65911,-2.11262,0.453647,1.65404,-2.0875,0.229086,1.44251,-1.84466,0.04818,1.51146,-1.90204,0.054156,1.52992,-1.96855,0.057839,1.50478,-2.01624,0.23447,1.41137,-2.09529,0.349167,1.21211,-1.7676 [...]
+/*1741*/{0.209213,1.91598,-1.74141,0.316297,1.85919,-1.86758,0.049651,1.83093,-1.69758,-0.07688,1.75951,-1.67978,0.06167,1.67722,-1.6925,0.133937,1.61956,-1.67929,0.298452,1.94194,-2.11194,0.335486,1.86219,-1.93968,0.150806,1.89487,-2.0704,0.219701,1.71386,-2.24725,0.277827,1.62661,-2.26773,0.394169,1.65704,-2.11186,0.45463,1.65135,-2.08677,0.230316,1.44401,-1.84483,0.048327,1.51247,-1.90179,0.053621,1.53134,-1.96869,0.057725,1.506,-2.01557,0.235428,1.41293,-2.09531,0.343262,1.20704,-1.7 [...]
+/*1742*/{0.210541,1.916,-1.7408,0.316952,1.85919,-1.86732,0.05069,1.82884,-1.69607,-0.075638,1.75689,-1.68004,0.063986,1.67464,-1.69139,0.135973,1.61677,-1.67672,0.299669,1.94138,-2.11108,0.335835,1.86213,-1.93898,0.152252,1.89654,-2.07008,0.219849,1.71373,-2.24692,0.278168,1.62617,-2.26766,0.394055,1.65371,-2.11139,0.454646,1.64771,-2.08637,0.23125,1.4449,-1.84434,0.04829,1.5125,-1.90166,0.053551,1.53144,-1.96852,0.057418,1.50703,-2.01567,0.235382,1.41436,-2.09501,0.341154,1.20057,-1.76 [...]
+/*1743*/{0.211521,1.91558,-1.73966,0.317225,1.85954,-1.86704,0.052063,1.82744,-1.69609,-0.073882,1.75331,-1.67961,0.066367,1.67223,-1.68957,0.138714,1.61409,-1.6757,0.300587,1.94021,-2.11005,0.3359,1.86205,-1.93883,0.15207,1.89775,-2.07011,0.219491,1.7135,-2.2468,0.277119,1.62524,-2.26768,0.394459,1.65175,-2.11137,0.454695,1.64306,-2.08497,0.228763,1.44606,-1.84446,0.04829,1.5125,-1.90166,0.053512,1.53161,-1.96834,0.057444,1.50711,-2.01564,0.235402,1.41511,-2.09506,0.335145,1.1933,-1.767 [...]
+/*1744*/{0.212973,1.91502,-1.73895,0.317624,1.85883,-1.86715,0.05267,1.82554,-1.69615,-0.070495,1.7499,-1.67868,0.068837,1.66836,-1.68806,0.140439,1.61081,-1.67294,0.300387,1.9387,-2.10927,0.335823,1.86141,-1.93857,0.15261,1.89805,-2.06937,0.218033,1.71194,-2.24561,0.27616,1.62376,-2.26697,0.394521,1.64743,-2.11076,0.454931,1.63778,-2.08516,0.227669,1.44593,-1.84441,0.04659,1.51133,-1.90313,0.053363,1.53158,-1.96819,0.057324,1.5066,-2.01568,0.236144,1.41606,-2.09444,0.329661,1.18681,-1.7 [...]
+/*1745*/{0.213694,1.91415,-1.73811,0.31744,1.85744,-1.86677,0.053697,1.82219,-1.69513,-0.06824,1.74556,-1.67773,0.071571,1.66444,-1.68557,0.143333,1.60686,-1.67067,0.300405,1.93705,-2.10876,0.336111,1.86085,-1.93801,0.15309,1.89795,-2.06913,0.215611,1.71087,-2.24487,0.273963,1.62274,-2.2661,0.395701,1.64212,-2.11063,0.455235,1.63253,-2.08533,0.227669,1.44593,-1.84441,0.046924,1.50899,-1.90264,0.053363,1.53158,-1.96819,0.057689,1.5061,-2.01545,0.236947,1.41682,-2.09408,0.324043,1.17926,-1 [...]
+/*1746*/{0.214659,1.9124,-1.73751,0.317097,1.8571,-1.86632,0.056096,1.81928,-1.6936,-0.06475,1.74101,-1.67594,0.075014,1.66049,-1.68372,0.147252,1.60312,-1.66817,0.301102,1.93509,-2.10811,0.33542,1.85921,-1.93783,0.152868,1.89806,-2.06869,0.213065,1.71012,-2.24331,0.271709,1.62088,-2.26554,0.396067,1.63675,-2.11094,0.454554,1.62557,-2.08502,0.227669,1.44593,-1.84441,0.046806,1.50752,-1.90312,0.053276,1.53094,-1.96837,0.057417,1.50459,-2.01566,0.23728,1.4164,-2.09427,0.318807,1.17231,-1.7 [...]
+/*1747*/{0.215753,1.91041,-1.73661,0.317279,1.85642,-1.86636,0.057032,1.815,-1.69169,-0.062619,1.73557,-1.67435,0.077377,1.65563,-1.6814,0.150346,1.59871,-1.66538,0.299359,1.93232,-2.10745,0.334589,1.85786,-1.93753,0.15252,1.89782,-2.06798,0.208832,1.70818,-2.24224,0.268119,1.61927,-2.26432,0.395036,1.63022,-2.11082,0.454131,1.61838,-2.08591,0.228359,1.44572,-1.84401,0.046335,1.50562,-1.9043,0.053556,1.5288,-1.96823,0.058089,1.50249,-2.01564,0.238505,1.4158,-2.0939,0.313072,1.16477,-1.76 [...]
+/*1748*/{0.216388,1.90815,-1.73561,0.317698,1.85303,-1.86588,0.05868,1.8109,-1.69023,-0.058532,1.73013,-1.6712,0.080286,1.65011,-1.67812,0.153346,1.59413,-1.66249,0.299721,1.93066,-2.10695,0.334338,1.856,-1.93758,0.151486,1.89651,-2.06689,0.205334,1.70692,-2.24108,0.264532,1.61721,-2.2637,0.394463,1.62491,-2.11219,0.452983,1.61068,-2.08697,0.227324,1.44341,-1.84376,0.045724,1.50328,-1.90442,0.052754,1.52711,-1.96838,0.058116,1.50034,-2.01565,0.23914,1.41477,-2.09316,0.307603,1.15771,-1.7 [...]
+/*1749*/{0.217101,1.90539,-1.73527,0.31788,1.85203,-1.86603,0.060369,1.80618,-1.68781,-0.057207,1.72374,-1.66847,0.083803,1.64416,-1.67534,0.156873,1.58959,-1.65976,0.297708,1.9274,-2.1068,0.33391,1.85437,-1.93756,0.151214,1.89497,-2.06556,0.201809,1.70512,-2.23958,0.261672,1.61527,-2.26245,0.39272,1.61707,-2.11302,0.45181,1.60229,-2.08857,0.224573,1.44055,-1.84369,0.045864,1.5004,-1.904,0.053325,1.52414,-1.96858,0.058179,1.4975,-2.0169,0.239934,1.41362,-2.09276,0.304241,1.15158,-1.76587 [...]
+/*1750*/{0.217496,1.9029,-1.73433,0.317356,1.85052,-1.86644,0.061819,1.80158,-1.68519,-0.053074,1.71774,-1.66495,0.087462,1.63926,-1.67193,0.161582,1.58504,-1.65674,0.296681,1.92483,-2.10702,0.332693,1.85149,-1.93724,0.149449,1.89254,-2.06452,0.195729,1.7027,-2.23814,0.256386,1.61249,-2.26144,0.391102,1.61078,-2.11451,0.450557,1.59405,-2.0914,0.225845,1.43826,-1.84262,0.045807,1.49765,-1.9044,0.052956,1.52116,-1.96909,0.058791,1.49525,-2.01769,0.241442,1.41164,-2.0918,0.298982,1.14523,-1 [...]
+/*1751*/{0.218724,1.8994,-1.73347,0.315677,1.84712,-1.86612,0.063904,1.79608,-1.68294,-0.050046,1.71133,-1.66168,0.091035,1.63249,-1.66828,0.165454,1.57979,-1.65392,0.294362,1.92179,-2.10775,0.332195,1.84904,-1.937,0.148506,1.88957,-2.0628,0.191495,1.70037,-2.23658,0.252557,1.60965,-2.26082,0.388461,1.60256,-2.11589,0.448155,1.58417,-2.09336,0.223756,1.43448,-1.84187,0.046074,1.49401,-1.90401,0.054104,1.51779,-1.96933,0.058956,1.49104,-2.01748,0.242842,1.40965,-2.09031,0.296017,1.13956,- [...]
+/*1752*/{0.219112,1.89555,-1.73263,0.316907,1.84399,-1.86635,0.065775,1.79033,-1.68049,-0.046132,1.70452,-1.65788,0.094956,1.62751,-1.66451,0.169862,1.57495,-1.65099,0.292192,1.91863,-2.10787,0.33166,1.84628,-1.93704,0.146557,1.88648,-2.06168,0.185379,1.69773,-2.23531,0.247673,1.60666,-2.26044,0.386097,1.59452,-2.1177,0.446427,1.57384,-2.0967,0.223635,1.43041,-1.84013,0.045552,1.49032,-1.90419,0.054327,1.51501,-1.96919,0.058972,1.48778,-2.01881,0.243427,1.40749,-2.08913,0.29318,1.13459,- [...]
+/*1753*/{0.219213,1.89142,-1.73226,0.309819,1.84362,-1.86861,0.067423,1.78413,-1.67765,-0.042149,1.69674,-1.65284,0.098888,1.62134,-1.66109,0.173911,1.57013,-1.6484,0.290395,1.91467,-2.10891,0.330274,1.84379,-1.93733,0.144744,1.88242,-2.0608,0.181401,1.69438,-2.2337,0.242678,1.60268,-2.26013,0.382232,1.58549,-2.12047,0.442943,1.56335,-2.09976,0.222887,1.42671,-1.83873,0.045185,1.48653,-1.90472,0.053627,1.51139,-1.969,0.059248,1.4837,-2.01883,0.244374,1.4043,-2.08836,0.290776,1.13035,-1.7 [...]
+/*1754*/{0.219903,1.88652,-1.73075,0.309062,1.83965,-1.86835,0.070153,1.77758,-1.67474,-0.039483,1.68909,-1.64806,0.103283,1.61558,-1.6574,0.180097,1.56573,-1.64672,0.288377,1.91107,-2.10939,0.329661,1.84061,-1.93745,0.143288,1.87797,-2.05864,0.175962,1.69108,-2.23304,0.23704,1.59882,-2.25923,0.378797,1.57579,-2.12191,0.439952,1.55215,-2.10408,0.22278,1.4222,-1.83811,0.046303,1.48295,-1.90454,0.053453,1.50789,-1.96915,0.060431,1.48012,-2.01897,0.245603,1.40147,-2.08631,0.288076,1.12648,- [...]
+/*1755*/{0.219892,1.882,-1.7306,0.314993,1.8345,-1.86678,0.072536,1.77043,-1.67115,-0.035607,1.6812,-1.64317,0.107557,1.60983,-1.65278,0.184857,1.5607,-1.64313,0.286173,1.90699,-2.10999,0.32983,1.83621,-1.93723,0.141159,1.87283,-2.0572,0.169908,1.68723,-2.23187,0.231298,1.59486,-2.25894,0.376777,1.56736,-2.12558,0.436228,1.54017,-2.10781,0.222396,1.41778,-1.83584,0.044653,1.47851,-1.90485,0.053296,1.50369,-1.96857,0.061366,1.47593,-2.01952,0.246332,1.39816,-2.08567,0.28624,1.1212,-1.7616 [...]
+/*1756*/{0.220572,1.87749,-1.72917,0.315026,1.82912,-1.86689,0.075234,1.76349,-1.66751,-0.032332,1.67249,-1.63776,0.112429,1.60336,-1.64895,0.190737,1.55581,-1.64042,0.283786,1.90256,-2.11101,0.329894,1.83289,-1.93731,0.139871,1.86836,-2.05638,0.166004,1.68202,-2.23017,0.225437,1.59033,-2.25886,0.371788,1.55613,-2.12878,0.431424,1.52815,-2.11203,0.221248,1.41275,-1.83388,0.044328,1.47334,-1.90588,0.052522,1.499,-1.96829,0.060205,1.47022,-2.02013,0.246014,1.39366,-2.08522,0.283612,1.1173, [...]
+/*1757*/{0.221224,1.87214,-1.72849,0.314971,1.82562,-1.86707,0.078157,1.75605,-1.6641,-0.028644,1.66378,-1.63199,0.117881,1.59601,-1.64488,0.196029,1.55073,-1.63822,0.281384,1.89773,-2.11183,0.328583,1.82917,-1.93768,0.137715,1.86316,-2.05472,0.160436,1.67728,-2.2294,0.218868,1.58655,-2.25882,0.366885,1.54583,-2.13162,0.426541,1.51523,-2.11703,0.221503,1.40885,-1.8323,0.043846,1.46904,-1.90523,0.053057,1.49475,-1.96726,0.060188,1.46536,-2.021,0.246873,1.39103,-2.08349,0.281817,1.11408,-1 [...]
+/*1758*/{0.221437,1.86637,-1.72769,0.313935,1.82101,-1.8666,0.081394,1.74798,-1.66041,-0.024273,1.65499,-1.62581,0.124434,1.59034,-1.64098,0.20252,1.54663,-1.63573,0.279319,1.89339,-2.11278,0.32912,1.82479,-1.93759,0.136208,1.85712,-2.05298,0.155951,1.67361,-2.22924,0.212335,1.58191,-2.25921,0.361452,1.53483,-2.13523,0.420641,1.50306,-2.12173,0.220269,1.40413,-1.83068,0.043194,1.46415,-1.90598,0.051199,1.48941,-1.96738,0.059915,1.46102,-2.02137,0.247211,1.38725,-2.08237,0.280646,1.11007, [...]
+/*1759*/{0.222139,1.86112,-1.72653,0.314253,1.81631,-1.86673,0.084783,1.74035,-1.65679,-0.018586,1.64633,-1.62053,0.128921,1.58396,-1.63817,0.209177,1.54184,-1.63294,0.27673,1.88852,-2.11411,0.328125,1.82025,-1.93806,0.134283,1.85175,-2.05232,0.150212,1.66906,-2.22884,0.205558,1.57727,-2.26021,0.356042,1.52411,-2.13918,0.414305,1.49034,-2.12664,0.219658,1.39957,-1.8288,0.040931,1.45967,-1.90616,0.050762,1.4845,-1.96779,0.058739,1.45618,-2.02162,0.247147,1.38352,-2.08117,0.277989,1.10616, [...]
+/*1760*/{0.222933,1.85582,-1.72534,0.315007,1.81031,-1.86656,0.088438,1.73166,-1.65311,-0.013775,1.63763,-1.61403,0.135668,1.57795,-1.63342,0.216338,1.53741,-1.62964,0.275306,1.88408,-2.11507,0.328801,1.81608,-1.93851,0.133419,1.84596,-2.05053,0.143996,1.66405,-2.22862,0.198556,1.57249,-2.26084,0.350829,1.51394,-2.14323,0.406623,1.4774,-2.13196,0.217171,1.39502,-1.82783,0.03838,1.45573,-1.90663,0.048545,1.47971,-1.96885,0.057384,1.45149,-2.02261,0.24703,1.37948,-2.07949,0.274979,1.10461, [...]
+/*1761*/{0.22397,1.85057,-1.72414,0.315488,1.80445,-1.86561,0.093309,1.72342,-1.64877,-0.005785,1.62909,-1.6082,0.143114,1.57264,-1.62963,0.223498,1.53331,-1.62719,0.273772,1.87976,-2.11631,0.329253,1.8111,-1.93854,0.131977,1.84163,-2.04879,0.139683,1.6601,-2.22816,0.191144,1.56781,-2.26107,0.344033,1.50301,-2.14758,0.399766,1.4659,-2.13733,0.21435,1.39098,-1.82665,0.034347,1.45211,-1.90796,0.045633,1.47468,-1.97045,0.056259,1.44676,-2.0227,0.246792,1.37548,-2.07895,0.272345,1.10451,-1.7 [...]
+/*1762*/{0.225076,1.84636,-1.72297,0.316346,1.7994,-1.86545,0.097593,1.71471,-1.64511,-0.000168,1.61992,-1.60168,0.14974,1.56737,-1.6274,0.2309,1.52946,-1.62511,0.27167,1.87583,-2.11738,0.329865,1.80717,-1.93877,0.130665,1.83864,-2.04575,0.134386,1.65579,-2.22841,0.183588,1.56339,-2.2619,0.337389,1.49265,-2.15193,0.39172,1.45454,-2.1426,0.211615,1.38718,-1.8253,0.030284,1.44878,-1.90948,0.042677,1.46967,-1.97155,0.052687,1.44248,-2.02481,0.243967,1.37157,-2.07821,0.265926,1.10648,-1.7544 [...]
+/*1763*/{0.226049,1.84255,-1.72158,0.316106,1.79553,-1.86533,0.103324,1.70661,-1.64148,0.007283,1.61133,-1.59548,0.157388,1.56208,-1.62394,0.238548,1.52672,-1.62319,0.271073,1.87278,-2.11912,0.330187,1.8033,-1.93928,0.129666,1.83712,-2.043,0.130318,1.6526,-2.22799,0.175804,1.55931,-2.26213,0.329767,1.48406,-2.15662,0.383801,1.44421,-2.14746,0.208606,1.38501,-1.82353,0.026708,1.44569,-1.91131,0.03981,1.46669,-1.97259,0.049708,1.43815,-2.0259,0.242512,1.36788,-2.07668,0.267071,1.10742,-1.7 [...]
+/*1764*/{0.226804,1.83917,-1.72061,0.316303,1.79152,-1.86493,0.109843,1.6997,-1.63839,0.016917,1.60318,-1.58916,0.165692,1.55769,-1.62044,0.247965,1.5244,-1.62153,0.271048,1.8697,-2.12066,0.330984,1.79918,-1.94029,0.129157,1.83691,-2.04142,0.126559,1.65003,-2.22808,0.168939,1.55578,-2.26173,0.321796,1.47459,-2.15979,0.373928,1.43517,-2.153,0.205569,1.38439,-1.82141,0.022266,1.44285,-1.91441,0.037137,1.46354,-1.97414,0.045702,1.43397,-2.02864,0.243521,1.3642,-2.07214,0.265259,1.11026,-1.7 [...]
+/*1765*/{0.227084,1.83651,-1.72009,0.316587,1.78833,-1.86497,0.115673,1.69434,-1.6364,0.025983,1.59673,-1.58407,0.174854,1.55444,-1.61858,0.256325,1.5223,-1.61943,0.270739,1.86732,-2.12178,0.331538,1.79587,-1.94108,0.129151,1.83691,-2.03949,0.123016,1.64917,-2.22713,0.162467,1.55413,-2.2607,0.31286,1.46859,-2.16261,0.364958,1.42633,-2.15807,0.201881,1.38449,-1.81823,0.01892,1.44025,-1.91754,0.034361,1.46087,-1.97442,0.043147,1.42939,-2.03085,0.240849,1.36161,-2.07089,0.259469,1.10853,-1. [...]
+/*1766*/{0.227099,1.83514,-1.71963,0.316812,1.78616,-1.86549,0.121456,1.69083,-1.63502,0.035137,1.59075,-1.57911,0.18396,1.55164,-1.61639,0.266446,1.52105,-1.61797,0.271115,1.86534,-2.12111,0.33052,1.79378,-1.94207,0.129077,1.83717,-2.03887,0.118333,1.65026,-2.22515,0.15529,1.55445,-2.25969,0.304143,1.46358,-2.16449,0.355318,1.41954,-2.16303,0.198648,1.38581,-1.81526,0.017615,1.43842,-1.91975,0.032919,1.46046,-1.97545,0.040235,1.42525,-2.03296,0.239191,1.35996,-2.06951,0.254338,1.10673,- [...]
+/*1767*/{0.22735,1.83436,-1.71985,0.315401,1.78615,-1.86706,0.126319,1.68879,-1.63342,0.044466,1.58696,-1.5754,0.193921,1.55057,-1.61501,0.276144,1.52164,-1.61725,0.272018,1.86379,-2.12037,0.330314,1.79137,-1.9427,0.129374,1.83733,-2.0397,0.112606,1.65205,-2.22294,0.147387,1.55711,-2.25823,0.295731,1.46108,-2.16661,0.345478,1.41392,-2.16793,0.19479,1.38625,-1.8122,0.016274,1.43789,-1.92077,0.031369,1.46108,-1.97668,0.03873,1.42347,-2.03393,0.237357,1.3588,-2.06838,0.250343,1.10681,-1.743 [...]
+/*1768*/{0.228285,1.83396,-1.72048,0.315314,1.78351,-1.86763,0.130854,1.68772,-1.63238,0.055772,1.58501,-1.57186,0.203325,1.55041,-1.61422,0.286294,1.52257,-1.61637,0.273264,1.86211,-2.11982,0.330292,1.78971,-1.94366,0.130638,1.8383,-2.04052,0.108089,1.65472,-2.22029,0.139262,1.56005,-2.25667,0.286386,1.45864,-2.16862,0.335086,1.41003,-2.17276,0.191713,1.38628,-1.80985,0.013841,1.43902,-1.9217,0.029888,1.46222,-1.97641,0.036871,1.42423,-2.03433,0.236079,1.35796,-2.06771,0.242991,1.10136, [...]
+/*1769*/{0.229903,1.83398,-1.72092,0.314913,1.78253,-1.86785,0.135694,1.68764,-1.631,0.06466,1.58398,-1.5691,0.21325,1.55049,-1.61332,0.296701,1.52539,-1.61648,0.27377,1.8619,-2.11951,0.329667,1.78833,-1.94399,0.130894,1.83826,-2.04089,0.101771,1.66047,-2.2177,0.131722,1.56398,-2.25567,0.277281,1.45816,-2.17211,0.325106,1.40734,-2.17643,0.188345,1.38671,-1.80789,0.013007,1.43996,-1.92224,0.028339,1.46283,-1.97609,0.035544,1.42481,-2.03437,0.234926,1.35794,-2.0663,0.23609,1.09786,-1.74162 [...]
+/*1770*/{0.231139,1.83512,-1.72178,0.314263,1.78354,-1.86856,0.139914,1.68877,-1.63014,0.073058,1.58251,-1.566,0.223414,1.5521,-1.61232,0.307152,1.52876,-1.61651,0.273976,1.86172,-2.11954,0.329129,1.78761,-1.94438,0.13096,1.83893,-2.04146,0.09639,1.66503,-2.21381,0.123422,1.56859,-2.25493,0.267529,1.45723,-2.17453,0.315117,1.40748,-2.1812,0.18572,1.38752,-1.80529,0.012827,1.44007,-1.92194,0.025991,1.4633,-1.97603,0.033871,1.42649,-2.03403,0.233074,1.35877,-2.06607,0.227975,1.09376,-1.739 [...]
+/*1771*/{0.23304,1.83615,-1.72212,0.313844,1.7832,-1.86977,0.144058,1.68965,-1.62857,0.082389,1.58314,-1.56383,0.233384,1.5546,-1.61172,0.317722,1.53326,-1.6172,0.273903,1.86189,-2.11949,0.328584,1.7873,-1.94449,0.130692,1.84035,-2.04174,0.09079,1.67085,-2.21076,0.114516,1.57372,-2.25418,0.259409,1.45881,-2.17822,0.304582,1.40811,-2.18494,0.183749,1.38893,-1.80346,0.011043,1.44134,-1.92192,0.025299,1.46357,-1.97496,0.033024,1.42799,-2.03304,0.232994,1.36068,-2.06546,0.219224,1.09055,-1.7 [...]
+/*1772*/{0.233703,1.83831,-1.72257,0.313356,1.78316,-1.87021,0.147777,1.69175,-1.62742,0.091288,1.58324,-1.56101,0.242483,1.55794,-1.6107,0.32811,1.53868,-1.61805,0.273765,1.86282,-2.11968,0.3271,1.78802,-1.94519,0.130361,1.84207,-2.04221,0.084553,1.67792,-2.2078,0.10559,1.57974,-2.2539,0.250187,1.46045,-2.18114,0.294214,1.40988,-2.18881,0.180882,1.3901,-1.80223,0.008935,1.44177,-1.92144,0.024135,1.46431,-1.97461,0.032143,1.42994,-2.03266,0.228749,1.3618,-2.06561,0.213431,1.08652,-1.7423 [...]
+/*1773*/{0.235813,1.84038,-1.72301,0.313853,1.7836,-1.87109,0.151104,1.69368,-1.62686,0.099353,1.58421,-1.55865,0.251437,1.56189,-1.61022,0.337826,1.54474,-1.61969,0.272555,1.86403,-2.11986,0.326577,1.78947,-1.94613,0.129613,1.84443,-2.04209,0.080055,1.68525,-2.20544,0.096882,1.58597,-2.25392,0.241358,1.46505,-2.18497,0.284049,1.41298,-2.19151,0.179005,1.39101,-1.80095,0.007593,1.44274,-1.92043,0.022553,1.46529,-1.97385,0.030743,1.43075,-2.03024,0.227895,1.36439,-2.06541,0.205766,1.08392 [...]
+/*1774*/{0.237282,1.84302,-1.72395,0.313814,1.78484,-1.87247,0.15544,1.69604,-1.62667,0.106898,1.58534,-1.55687,0.260366,1.56662,-1.61065,0.346714,1.55159,-1.62153,0.271056,1.86543,-2.12056,0.325724,1.79012,-1.94683,0.129047,1.84588,-2.04197,0.073575,1.69181,-2.20334,0.08804,1.59306,-2.25388,0.231733,1.46967,-2.18711,0.273618,1.41715,-2.19464,0.178394,1.39197,-1.80117,0.007242,1.44317,-1.91982,0.021532,1.46663,-1.97362,0.028841,1.43249,-2.02926,0.22651,1.36722,-2.06528,0.197164,1.08313,- [...]
+/*1775*/{0.239345,1.84607,-1.7246,0.313991,1.78699,-1.87232,0.160081,1.69919,-1.62607,0.114489,1.58647,-1.55473,0.267676,1.57182,-1.61062,0.355498,1.55864,-1.62267,0.270305,1.86812,-2.12105,0.32513,1.79208,-1.94766,0.127953,1.84872,-2.04188,0.067681,1.69952,-2.20178,0.07969,1.60012,-2.25397,0.222945,1.47445,-2.18872,0.26374,1.42182,-2.19643,0.177735,1.39217,-1.79964,0.005812,1.44514,-1.91941,0.02045,1.46785,-1.97242,0.027179,1.43432,-2.0287,0.225451,1.37002,-2.06518,0.191487,1.08205,-1.7 [...]
+/*1776*/{0.242361,1.84885,-1.7253,0.314195,1.78895,-1.87421,0.164925,1.7023,-1.62549,0.12311,1.58916,-1.55244,0.275905,1.57765,-1.61053,0.364217,1.56635,-1.62551,0.267817,1.87029,-2.12151,0.32424,1.79399,-1.94878,0.126864,1.85145,-2.04081,0.062229,1.70659,-2.20029,0.070333,1.60762,-2.2541,0.214527,1.48151,-2.19046,0.253911,1.42831,-2.19818,0.177505,1.3929,-1.80035,0.005675,1.4468,-1.9188,0.021414,1.46919,-1.97127,0.026172,1.4356,-2.02742,0.224319,1.37297,-2.06527,0.183064,1.08182,-1.7448 [...]
+/*1777*/{0.245294,1.85246,-1.7261,0.313646,1.79108,-1.87492,0.170115,1.70501,-1.62521,0.13026,1.59149,-1.55118,0.283282,1.58406,-1.61161,0.371485,1.57457,-1.62827,0.264858,1.87335,-2.12253,0.323951,1.79663,-1.95063,0.125185,1.85342,-2.03954,0.055679,1.71346,-2.19927,0.062917,1.61529,-2.25399,0.205627,1.48858,-2.19162,0.244923,1.43485,-2.19905,0.177727,1.39338,-1.80032,0.005062,1.44973,-1.91693,0.020247,1.47165,-1.96982,0.024895,1.43789,-2.02656,0.222662,1.37513,-2.06545,0.176288,1.08265, [...]
+/*1778*/{0.24827,1.8566,-1.72724,0.314983,1.79494,-1.87646,0.175797,1.70923,-1.62483,0.138352,1.5952,-1.55066,0.289796,1.58967,-1.61379,0.377755,1.58294,-1.6309,0.261808,1.87663,-2.12293,0.323814,1.79889,-1.95192,0.123711,1.85599,-2.03876,0.049471,1.72051,-2.19824,0.054748,1.62267,-2.25447,0.196658,1.4959,-2.19264,0.235821,1.44238,-2.19987,0.177815,1.3938,-1.80052,0.00607,1.45229,-1.91516,0.020105,1.47439,-1.96836,0.024894,1.43977,-2.02551,0.221984,1.37769,-2.06529,0.167708,1.08032,-1.74 [...]
+/*1779*/{0.250989,1.86053,-1.72829,0.315753,1.79828,-1.87779,0.181078,1.71269,-1.62445,0.146141,1.59953,-1.54949,0.296145,1.59688,-1.61447,0.38335,1.59107,-1.63406,0.259293,1.88046,-2.12391,0.323447,1.80238,-1.95364,0.121739,1.85915,-2.03665,0.043151,1.72754,-2.19721,0.047321,1.62997,-2.25415,0.190105,1.5037,-2.19277,0.227671,1.45008,-2.20031,0.17836,1.39426,-1.80011,0.005372,1.45565,-1.91357,0.019383,1.4769,-1.96731,0.023944,1.44322,-2.02478,0.22159,1.38069,-2.06499,0.159526,1.08129,-1. [...]
+/*1780*/{0.253894,1.86531,-1.72926,0.315467,1.80168,-1.87947,0.18729,1.7169,-1.62393,0.151685,1.60387,-1.54957,0.301548,1.60377,-1.61711,0.389298,1.59969,-1.63704,0.25593,1.88486,-2.1244,0.323436,1.80558,-1.95534,0.119831,1.86129,-2.03548,0.036754,1.73468,-2.19661,0.040567,1.63791,-2.25429,0.18136,1.51157,-2.19301,0.220367,1.45825,-2.20098,0.179031,1.39511,-1.79997,0.00551,1.4589,-1.91225,0.019832,1.48046,-1.96495,0.024264,1.44618,-2.02304,0.220302,1.3835,-2.06458,0.152349,1.08047,-1.752 [...]
+/*1781*/{0.256258,1.86987,-1.73073,0.315931,1.80517,-1.88028,0.194017,1.72124,-1.62356,0.159653,1.60765,-1.54788,0.306859,1.61103,-1.61883,0.394081,1.60753,-1.64118,0.25225,1.88888,-2.12525,0.322614,1.8093,-1.9572,0.118726,1.86419,-2.03309,0.030406,1.74166,-2.19611,0.033712,1.64517,-2.25391,0.174123,1.52079,-2.19278,0.212507,1.46652,-2.20091,0.179669,1.39633,-1.7999,0.00663,1.46134,-1.90932,0.01985,1.48349,-1.96321,0.023454,1.44947,-2.02194,0.219782,1.38658,-2.06402,0.144976,1.08278,-1.7 [...]
+/*1782*/{0.258983,1.87541,-1.73224,0.316839,1.80923,-1.88254,0.200375,1.72534,-1.62396,0.166797,1.61251,-1.5471,0.31247,1.61848,-1.62058,0.397827,1.61724,-1.64299,0.249226,1.89362,-2.12527,0.32253,1.81286,-1.95923,0.117061,1.86774,-2.03176,0.024323,1.74938,-2.19603,0.027245,1.65281,-2.25359,0.167725,1.52932,-2.19272,0.205741,1.47521,-2.20108,0.179908,1.3967,-1.79952,0.007741,1.46513,-1.90818,0.020181,1.4867,-1.96068,0.022744,1.45418,-2.02022,0.219367,1.39068,-2.06369,0.141522,1.08127,-1. [...]
+/*1783*/{0.261809,1.88064,-1.73357,0.317867,1.81304,-1.88438,0.205567,1.73043,-1.62392,0.173106,1.61857,-1.54732,0.316469,1.6254,-1.62297,0.402415,1.62562,-1.64722,0.24541,1.89855,-2.12601,0.322319,1.81657,-1.96107,0.115789,1.87096,-2.02972,0.020572,1.7569,-2.19553,0.020838,1.66003,-2.25298,0.160826,1.53844,-2.19158,0.199229,1.48422,-2.20104,0.18079,1.39754,-1.7988,0.008449,1.46833,-1.90643,0.020854,1.4893,-1.95896,0.02389,1.45741,-2.01924,0.218671,1.39444,-2.06345,0.133896,1.08287,-1.75 [...]
+/*1784*/{0.263914,1.88498,-1.73518,0.319388,1.81782,-1.88666,0.212262,1.73525,-1.62476,0.178475,1.62281,-1.54618,0.320648,1.63315,-1.62508,0.405857,1.63428,-1.65016,0.242387,1.90316,-2.12623,0.322498,1.82132,-1.96301,0.114702,1.8741,-2.0285,0.01478,1.76401,-2.19525,0.0145,1.6672,-2.2521,0.154403,1.54696,-2.19095,0.193093,1.4928,-2.20115,0.182861,1.39908,-1.79836,0.008911,1.47189,-1.9039,0.020885,1.4938,-1.95659,0.024274,1.4614,-2.01865,0.218856,1.39894,-2.06269,0.125862,1.08414,-1.75948, [...]
+/*1785*/{0.266204,1.89159,-1.73712,0.320601,1.82279,-1.88815,0.218056,1.74058,-1.62517,0.184889,1.62876,-1.54598,0.324205,1.64046,-1.62672,0.408754,1.64228,-1.65524,0.238917,1.90782,-2.12622,0.32173,1.82545,-1.96486,0.113257,1.87778,-2.02586,0.009602,1.76978,-2.19491,0.008509,1.67456,-2.25097,0.148479,1.55571,-2.18995,0.187577,1.5018,-2.20109,0.183928,1.40004,-1.79755,0.010016,1.47588,-1.90226,0.021321,1.49696,-1.95395,0.02313,1.46661,-2.01691,0.219106,1.40364,-2.06153,0.120898,1.08771,- [...]
+/*1786*/{0.26872,1.89701,-1.73897,0.320484,1.82624,-1.89017,0.223096,1.74592,-1.62589,0.190478,1.63444,-1.54525,0.327322,1.64766,-1.62877,0.411435,1.65036,-1.65749,0.235707,1.91275,-2.12682,0.321167,1.83012,-1.96655,0.111792,1.88102,-2.0249,0.006202,1.77683,-2.19462,0.003308,1.68159,-2.24993,0.142257,1.5639,-2.18842,0.182597,1.51023,-2.20083,0.185034,1.40126,-1.79721,0.012294,1.47905,-1.90178,0.022788,1.50088,-1.95193,0.023295,1.47272,-2.01656,0.218957,1.40825,-2.061,0.113503,1.08848,-1. [...]
+/*1787*/{0.270806,1.90253,-1.74058,0.321179,1.83078,-1.89227,0.227775,1.75158,-1.62699,0.194827,1.64095,-1.54512,0.329741,1.65478,-1.63138,0.414105,1.65824,-1.66127,0.233014,1.91722,-2.1264,0.321223,1.83429,-1.96877,0.111076,1.88418,-2.02237,0.00326,1.78318,-2.19485,-0.002429,1.68858,-2.24863,0.137074,1.57221,-2.18753,0.177666,1.51809,-2.20092,0.187235,1.4029,-1.79785,0.013572,1.48201,-1.90049,0.023237,1.50488,-1.94962,0.026059,1.4762,-2.01491,0.21881,1.41278,-2.06089,0.105866,1.09025,-1 [...]
+/*1788*/{0.273214,1.90747,-1.74239,0.322246,1.83526,-1.89391,0.232372,1.75717,-1.62707,0.199831,1.64668,-1.54444,0.332269,1.66248,-1.63327,0.415496,1.66592,-1.66536,0.230429,1.92113,-2.12616,0.320824,1.83862,-1.97074,0.110265,1.88787,-2.02148,-0.00072,1.78821,-2.19497,-0.007499,1.69542,-2.24724,0.13295,1.58017,-2.18633,0.172669,1.52551,-2.20037,0.188292,1.40383,-1.79815,0.015031,1.486,-1.8982,0.023504,1.5089,-1.94743,0.026774,1.4813,-2.01372,0.21891,1.41891,-2.06078,0.100766,1.09371,-1.7 [...]
+/*1789*/{0.274336,1.91343,-1.74441,0.322509,1.83983,-1.89675,0.236028,1.76277,-1.62763,0.204284,1.65261,-1.5441,0.333879,1.66884,-1.63484,0.417649,1.67344,-1.66902,0.22826,1.92498,-2.12638,0.321067,1.84325,-1.97207,0.109092,1.89143,-2.01999,-0.004532,1.79456,-2.19384,-0.012419,1.70188,-2.24526,0.127379,1.587,-2.1855,0.168349,1.53322,-2.20035,0.190838,1.4052,-1.79896,0.016448,1.4895,-1.89755,0.024962,1.51325,-1.94516,0.026949,1.4866,-2.01279,0.218887,1.42361,-2.0602,0.095775,1.09677,-1.76 [...]
+/*1790*/{0.276279,1.91717,-1.74598,0.323365,1.84379,-1.89807,0.239661,1.7681,-1.62846,0.20803,1.65818,-1.54359,0.336249,1.67556,-1.63693,0.418502,1.68063,-1.6727,0.225949,1.92923,-2.12664,0.320784,1.84763,-1.97327,0.108529,1.8952,-2.01965,-0.006287,1.80014,-2.19304,-0.016841,1.70784,-2.2437,0.123332,1.59377,-2.1845,0.164193,1.53957,-2.2002,0.19192,1.40708,-1.79928,0.017969,1.49248,-1.895,0.026138,1.5169,-1.94341,0.028065,1.49152,-2.01156,0.219612,1.42931,-2.06035,0.089799,1.10018,-1.7693 [...]
+/*1791*/{0.278233,1.92171,-1.74737,0.323795,1.84773,-1.90023,0.242719,1.7732,-1.62859,0.209975,1.66311,-1.54279,0.337015,1.68153,-1.63825,0.419241,1.68737,-1.67573,0.224509,1.93274,-2.12668,0.320936,1.85122,-1.97455,0.107332,1.89856,-2.01885,-0.010476,1.80486,-2.19185,-0.021427,1.71376,-2.24186,0.1195,1.5989,-2.18354,0.161114,1.54604,-2.20008,0.193131,1.40788,-1.80008,0.018468,1.49633,-1.89438,0.026616,1.52118,-1.94118,0.028543,1.49631,-2.01057,0.220186,1.43435,-2.06025,0.085811,1.10315, [...]
+/*1792*/{0.279387,1.92533,-1.74896,0.324389,1.85142,-1.90138,0.244857,1.77805,-1.62919,0.213861,1.66882,-1.5422,0.337888,1.68743,-1.64013,0.42012,1.69348,-1.67874,0.223223,1.93586,-2.12662,0.320276,1.85517,-1.97664,0.106752,1.90203,-2.01751,-0.013503,1.81004,-2.18966,-0.025276,1.71979,-2.2399,0.11699,1.60508,-2.18288,0.157875,1.55199,-2.19994,0.195644,1.40966,-1.80099,0.019917,1.49989,-1.89256,0.027569,1.52503,-1.93928,0.030174,1.50086,-2.00913,0.220484,1.43956,-2.06039,0.081645,1.10615, [...]
+/*1793*/{0.28116,1.92962,-1.75013,0.325423,1.85518,-1.90302,0.247609,1.78255,-1.62975,0.216589,1.67405,-1.54221,0.338577,1.69277,-1.6416,0.421035,1.69914,-1.68152,0.222074,1.93945,-2.12655,0.319763,1.85862,-1.97762,0.106078,1.90519,-2.01679,-0.015607,1.8151,-2.18812,-0.029485,1.72509,-2.23741,0.114092,1.61005,-2.18207,0.15424,1.55718,-2.20013,0.196918,1.41154,-1.8018,0.021558,1.50332,-1.8915,0.028736,1.52833,-1.93675,0.031355,1.50621,-2.00797,0.21873,1.44425,-2.06092,0.077143,1.10956,-1. [...]
+/*1794*/{0.282792,1.93303,-1.75151,0.324707,1.85863,-1.90454,0.24938,1.78731,-1.63047,0.218404,1.67857,-1.54118,0.33945,1.69777,-1.64362,0.420138,1.70451,-1.68486,0.220199,1.94246,-2.12672,0.319741,1.86232,-1.97841,0.108253,1.90641,-2.01612,-0.017225,1.8203,-2.18551,-0.032752,1.73033,-2.23585,0.110602,1.61476,-2.18183,0.151259,1.56238,-2.20028,0.19885,1.41345,-1.80215,0.023487,1.50643,-1.89032,0.029475,1.53159,-1.9355,0.032458,1.51104,-2.00631,0.218881,1.4494,-2.06114,0.073484,1.11415,-1 [...]
+/*1795*/{0.284361,1.93626,-1.75265,0.324839,1.86158,-1.90593,0.250482,1.79127,-1.63015,0.220476,1.68221,-1.53994,0.340038,1.70209,-1.6449,0.420322,1.70951,-1.6879,0.220264,1.94572,-2.1277,0.3192,1.86507,-1.97955,0.107908,1.90943,-2.01646,-0.019683,1.82586,-2.18342,-0.03547,1.73536,-2.23332,0.10805,1.61959,-2.18154,0.148523,1.56711,-2.2005,0.199942,1.41566,-1.80244,0.024283,1.50929,-1.88929,0.0304,1.53517,-1.93413,0.033335,1.51561,-2.00464,0.218624,1.45306,-2.06149,0.069568,1.11595,-1.767 [...]
+/*1796*/{0.285835,1.93952,-1.75376,0.326118,1.86587,-1.90677,0.252571,1.79535,-1.63036,0.223086,1.68672,-1.53972,0.340525,1.70625,-1.64582,0.419989,1.71341,-1.68954,0.219533,1.94816,-2.12754,0.319189,1.86845,-1.98071,0.107329,1.91229,-2.01629,-0.021561,1.83247,-2.18131,-0.038842,1.74064,-2.23142,0.106111,1.62405,-2.18097,0.145723,1.57149,-2.20098,0.201437,1.41808,-1.80303,0.026542,1.51408,-1.8876,0.031373,1.53906,-1.93277,0.034943,1.52005,-2.00402,0.218226,1.45715,-2.06109,0.067586,1.120 [...]
+/*1797*/{0.28719,1.94231,-1.75465,0.326895,1.86888,-1.90842,0.253508,1.79895,-1.63083,0.2246,1.6901,-1.53896,0.3409,1.71066,-1.64714,0.419602,1.7173,-1.69218,0.218642,1.95094,-2.12763,0.319185,1.87141,-1.98221,0.107698,1.9151,-2.01557,-0.022689,1.83754,-2.17863,-0.041199,1.74471,-2.22939,0.104345,1.62797,-2.18104,0.14378,1.57476,-2.20148,0.203095,1.41981,-1.80373,0.027391,1.51782,-1.88664,0.031478,1.54174,-1.93129,0.035083,1.5242,-2.00285,0.218695,1.46073,-2.06114,0.062865,1.12457,-1.763 [...]
+/*1798*/{0.288448,1.9452,-1.75554,0.326619,1.87159,-1.90867,0.254681,1.80269,-1.63084,0.225074,1.69331,-1.53743,0.340022,1.71398,-1.6479,0.418129,1.72133,-1.69522,0.218654,1.95363,-2.12775,0.319798,1.87406,-1.98208,0.108041,1.91795,-2.01504,-0.022916,1.83963,-2.17526,-0.04392,1.74887,-2.22658,0.101836,1.63276,-2.18206,0.14171,1.57851,-2.20194,0.203787,1.42263,-1.80377,0.028258,1.52166,-1.88522,0.03247,1.54524,-1.93021,0.035885,1.52753,-2.00135,0.218571,1.464,-2.06108,0.061061,1.12889,-1. [...]
+/*1799*/{0.289286,1.9476,-1.75648,0.328042,1.87518,-1.91056,0.25497,1.80586,-1.63127,0.225774,1.69631,-1.53728,0.339673,1.71722,-1.64955,0.418013,1.72347,-1.69743,0.217708,1.9559,-2.12784,0.319798,1.87702,-1.98348,0.108008,1.92026,-2.01408,-0.024747,1.84588,-2.17286,-0.045956,1.75297,-2.22486,0.100905,1.63625,-2.18199,0.1398,1.58157,-2.20252,0.204181,1.42541,-1.80352,0.0292,1.5239,-1.88418,0.033314,1.54882,-1.93007,0.036355,1.53156,-2.00024,0.218456,1.46722,-2.06046,0.059287,1.13268,-1.7 [...]
+/*1800*/{0.290431,1.94976,-1.75688,0.327197,1.87825,-1.91043,0.255544,1.80873,-1.63139,0.225653,1.69992,-1.53755,0.338976,1.71877,-1.65065,0.417051,1.72515,-1.69955,0.217018,1.9583,-2.12807,0.319556,1.87943,-1.98372,0.108607,1.92234,-2.0139,-0.024881,1.84925,-2.16986,-0.047523,1.75613,-2.22292,0.099108,1.6389,-2.1822,0.138089,1.58443,-2.20307,0.205113,1.42847,-1.80322,0.029752,1.5276,-1.88314,0.032241,1.55208,-1.92925,0.035996,1.53519,-1.99915,0.217926,1.47004,-2.05976,0.057547,1.13717,- [...]
+/*1801*/{0.291681,1.95195,-1.75785,0.32808,1.88057,-1.91213,0.255839,1.81167,-1.63178,0.22603,1.70229,-1.53611,0.338775,1.72038,-1.65201,0.415693,1.72639,-1.70167,0.217347,1.96034,-2.12846,0.320182,1.88092,-1.98477,0.10864,1.92463,-2.01363,-0.025761,1.85297,-2.16789,-0.049205,1.75934,-2.2202,0.097518,1.64029,-2.18185,0.136438,1.587,-2.20398,0.205454,1.43141,-1.80295,0.030622,1.5297,-1.88241,0.032471,1.55524,-1.9275,0.035709,1.53834,-1.99844,0.217485,1.47262,-2.05924,0.058921,1.13998,-1.7 [...]
+/*1802*/{0.29232,1.95339,-1.75828,0.327601,1.8823,-1.91185,0.255819,1.8138,-1.63171,0.226097,1.70484,-1.53608,0.338183,1.72211,-1.65327,0.4144,1.72696,-1.70266,0.216692,1.96225,-2.12846,0.320613,1.88325,-1.98543,0.108986,1.92598,-2.01374,-0.025853,1.85069,-2.16365,-0.050825,1.76188,-2.21873,0.097271,1.64229,-2.18255,0.134965,1.58907,-2.20471,0.205953,1.43441,-1.80266,0.030532,1.53274,-1.88241,0.032396,1.55819,-1.92821,0.035337,1.54068,-1.99819,0.217471,1.47492,-2.05884,0.058053,1.14321,- [...]
+/*1803*/{0.292732,1.95528,-1.75871,0.327755,1.88511,-1.91338,0.256101,1.81576,-1.63264,0.225483,1.70688,-1.53566,0.337804,1.72206,-1.65269,0.413376,1.72715,-1.70391,0.216304,1.9636,-2.12859,0.320456,1.88403,-1.98594,0.108752,1.9279,-2.01348,-0.023872,1.85289,-2.16092,-0.051755,1.76397,-2.21695,0.096129,1.6442,-2.18289,0.133601,1.59063,-2.20532,0.206396,1.43723,-1.80224,0.029686,1.5344,-1.88269,0.032196,1.56019,-1.9278,0.035376,1.54371,-1.99669,0.217339,1.47639,-2.05826,0.060145,1.1453,-1 [...]
+/*1804*/{0.293239,1.95642,-1.75937,0.327697,1.88681,-1.91406,0.255675,1.81783,-1.63283,0.224305,1.70848,-1.53546,0.335318,1.72228,-1.65306,0.412243,1.72554,-1.7056,0.216631,1.96527,-2.12949,0.320858,1.88536,-1.98693,0.109603,1.92955,-2.0139,-0.025106,1.85405,-2.15771,-0.053019,1.76605,-2.21558,0.095395,1.64517,-2.18301,0.133153,1.59202,-2.2063,0.206926,1.43998,-1.80204,0.031833,1.53687,-1.88047,0.031841,1.56243,-1.92745,0.037405,1.54271,-1.99586,0.217762,1.47782,-2.05774,0.063547,1.14601 [...]
+/*1805*/{0.292918,1.95638,-1.75984,0.326912,1.88747,-1.91449,0.254789,1.81919,-1.63323,0.224561,1.71024,-1.53571,0.334623,1.72194,-1.65413,0.410284,1.72463,-1.70666,0.216333,1.96594,-2.12954,0.321093,1.88634,-1.98782,0.109608,1.93087,-2.01376,-0.025797,1.85492,-2.15551,-0.054113,1.76725,-2.21398,0.094713,1.64759,-2.18359,0.132386,1.59276,-2.207,0.207298,1.44245,-1.80079,0.030609,1.53765,-1.8816,0.032364,1.56407,-1.9272,0.037112,1.54385,-1.99453,0.217843,1.47893,-2.05735,0.067773,1.14524, [...]
+/*1806*/{0.292964,1.9566,-1.76003,0.326256,1.88852,-1.9153,0.253043,1.81989,-1.63359,0.221341,1.71064,-1.53615,0.333111,1.72155,-1.65575,0.408324,1.72257,-1.70788,0.216452,1.96673,-2.13013,0.320341,1.88652,-1.98782,0.109983,1.93232,-2.01387,-0.02561,1.85568,-2.154,-0.054612,1.76806,-2.21251,0.095061,1.64683,-2.18364,0.131543,1.59273,-2.20807,0.207326,1.44492,-1.80013,0.030436,1.53853,-1.88194,0.032506,1.56571,-1.92795,0.036404,1.54471,-1.99436,0.218368,1.47964,-2.0562,0.073632,1.14466,-1 [...]
+/*1807*/{0.292447,1.9572,-1.76049,0.326123,1.88892,-1.91573,0.251667,1.82122,-1.63453,0.220302,1.71223,-1.53643,0.331523,1.72021,-1.65598,0.406997,1.72003,-1.70853,0.217146,1.96716,-2.13047,0.319744,1.88712,-1.98804,0.110497,1.93323,-2.01451,-0.026021,1.85587,-2.15278,-0.055006,1.76851,-2.2113,0.094959,1.64675,-2.1845,0.131384,1.59296,-2.20908,0.207651,1.44691,-1.79993,0.031787,1.53991,-1.88043,0.03225,1.56658,-1.92768,0.036377,1.54464,-1.99439,0.218444,1.4798,-2.05527,0.078919,1.14309,- [...]
+/*1808*/{0.292146,1.958,-1.76122,0.325481,1.88906,-1.91606,0.250287,1.82163,-1.63503,0.217013,1.71167,-1.53678,0.329771,1.71823,-1.65685,0.404505,1.71716,-1.7095,0.217142,1.96744,-2.13074,0.319433,1.88739,-1.98828,0.110488,1.93345,-2.01437,-0.026932,1.85574,-2.15016,-0.0549,1.76875,-2.21047,0.094703,1.6463,-2.18486,0.131082,1.59197,-2.21048,0.207612,1.44834,-1.79922,0.029521,1.54089,-1.88139,0.031667,1.56678,-1.92794,0.037025,1.54388,-1.9944,0.218944,1.47965,-2.05443,0.086612,1.14156,-1. [...]
+/*1809*/{0.291258,1.95653,-1.7619,0.324905,1.88868,-1.91657,0.24866,1.82164,-1.63537,0.214655,1.71232,-1.53753,0.32687,1.7155,-1.65716,0.402593,1.71294,-1.70973,0.216972,1.96746,-2.13109,0.318998,1.88731,-1.98844,0.110739,1.93414,-2.01529,-0.026266,1.85533,-2.14872,-0.055025,1.76827,-2.20952,0.094197,1.64503,-2.18545,0.130974,1.59109,-2.21136,0.207961,1.44922,-1.7983,0.028501,1.54025,-1.88136,0.031077,1.56664,-1.92814,0.036918,1.54285,-1.99451,0.219542,1.47946,-2.05356,0.09307,1.13865,-1 [...]
+/*1810*/{0.290942,1.95542,-1.76207,0.323888,1.8885,-1.91651,0.245998,1.82156,-1.63611,0.213115,1.71276,-1.5383,0.325331,1.71383,-1.65791,0.400445,1.70876,-1.71035,0.217716,1.96694,-2.13135,0.318383,1.8867,-1.9889,0.111071,1.93411,-2.01629,-0.025729,1.85619,-2.14734,-0.054948,1.76749,-2.20915,0.094093,1.6433,-2.18648,0.131506,1.58985,-2.21214,0.207933,1.44947,-1.79761,0.028259,1.53969,-1.88178,0.031119,1.56566,-1.9283,0.035954,1.54175,-1.9948,0.220151,1.47926,-2.05257,0.099914,1.13708,-1. [...]
+/*1811*/{0.289205,1.95496,-1.76228,0.3224,1.88826,-1.91639,0.243863,1.82157,-1.63676,0.209525,1.71262,-1.53975,0.322964,1.71102,-1.65806,0.398099,1.70501,-1.71063,0.217569,1.96633,-2.13174,0.317966,1.88611,-1.98904,0.111272,1.93416,-2.01711,-0.027419,1.85771,-2.14828,-0.053802,1.76655,-2.20912,0.094591,1.64209,-2.18724,0.131775,1.58812,-2.21298,0.208087,1.44929,-1.79741,0.029792,1.53725,-1.88027,0.031637,1.56411,-1.92842,0.03624,1.53959,-1.99534,0.220717,1.47825,-2.05171,0.107398,1.13322 [...]
+/*1812*/{0.287938,1.95426,-1.7633,0.321556,1.88726,-1.91669,0.241145,1.82103,-1.63742,0.206809,1.71148,-1.54025,0.32011,1.70758,-1.65798,0.394834,1.70008,-1.71008,0.21764,1.96534,-2.13184,0.317323,1.88551,-1.98922,0.110619,1.93373,-2.01751,-0.02719,1.85566,-2.14658,-0.053119,1.76501,-2.20921,0.095513,1.63941,-2.18817,0.131974,1.58583,-2.21441,0.208185,1.44901,-1.79722,0.029438,1.53523,-1.88018,0.030816,1.56344,-1.92876,0.036313,1.53703,-1.99537,0.221225,1.47678,-2.05089,0.113919,1.13063, [...]
+/*1813*/{0.285553,1.95323,-1.76376,0.321691,1.88651,-1.91709,0.237546,1.82011,-1.63791,0.203396,1.71065,-1.5411,0.316598,1.70334,-1.65791,0.392853,1.69504,-1.70967,0.217359,1.96385,-2.13221,0.316257,1.88434,-1.98897,0.110365,1.93306,-2.01824,-0.026468,1.85418,-2.14603,-0.052703,1.76292,-2.20947,0.095623,1.63724,-2.18906,0.132191,1.58309,-2.21511,0.208344,1.4485,-1.79704,0.029282,1.53343,-1.88111,0.030028,1.56086,-1.92929,0.036393,1.53484,-1.99611,0.221625,1.47547,-2.05036,0.122226,1.128, [...]
+/*1814*/{0.284219,1.95113,-1.76322,0.319866,1.88501,-1.91669,0.234283,1.81904,-1.63828,0.197315,1.70893,-1.54259,0.313216,1.69982,-1.65782,0.389788,1.68936,-1.70957,0.217501,1.96283,-2.1324,0.314896,1.88275,-1.98884,0.109271,1.93156,-2.01964,-0.024709,1.8485,-2.14369,-0.051347,1.76054,-2.21048,0.096964,1.63428,-2.18976,0.133708,1.5807,-2.21635,0.208922,1.44729,-1.79692,0.029342,1.53083,-1.88263,0.032537,1.56027,-1.92968,0.036719,1.53129,-1.9964,0.222001,1.47305,-2.05039,0.127982,1.1257,- [...]
+/*1815*/{0.281934,1.94935,-1.76416,0.319152,1.88377,-1.91679,0.231332,1.81722,-1.63819,0.193512,1.7081,-1.54413,0.309454,1.6958,-1.65753,0.386803,1.68308,-1.70854,0.217391,1.96091,-2.13236,0.314409,1.881,-1.98893,0.109297,1.93081,-2.02055,-0.023937,1.8454,-2.14389,-0.05068,1.75736,-2.21186,0.098159,1.63082,-2.19055,0.134353,1.57719,-2.21775,0.2093,1.44628,-1.79748,0.029911,1.52801,-1.88229,0.031959,1.55698,-1.93025,0.036646,1.52879,-1.99734,0.222216,1.47064,-2.0508,0.136865,1.12407,-1.77 [...]
+/*1816*/{0.278938,1.94713,-1.76325,0.318011,1.88176,-1.91675,0.228183,1.81587,-1.63825,0.188559,1.70596,-1.54467,0.305567,1.69115,-1.65718,0.382628,1.67672,-1.70697,0.216055,1.9586,-2.13229,0.314008,1.8795,-1.98852,0.10802,1.92919,-2.02166,-0.024259,1.84263,-2.1443,-0.049595,1.75389,-2.21306,0.098069,1.62828,-2.193,0.135263,1.57383,-2.21896,0.210138,1.44469,-1.79828,0.02947,1.52491,-1.8824,0.0318,1.55261,-1.93118,0.037485,1.52605,-1.9977,0.22273,1.46763,-2.0511,0.142367,1.12073,-1.77523, [...]
+/*1817*/{0.277938,1.94591,-1.76321,0.317343,1.87949,-1.91645,0.224374,1.81362,-1.63808,0.184501,1.70429,-1.54606,0.301514,1.68634,-1.65716,0.378913,1.66982,-1.70495,0.215619,1.95588,-2.13244,0.313164,1.87737,-1.98882,0.107507,1.92656,-2.02196,-0.023696,1.83937,-2.14521,-0.048444,1.74982,-2.21538,0.099302,1.62434,-2.19389,0.136647,1.57001,-2.22011,0.210551,1.44264,-1.79925,0.029372,1.52251,-1.88288,0.031747,1.54939,-1.93089,0.036887,1.52281,-1.9987,0.222273,1.46436,-2.05228,0.148012,1.117 [...]
+/*1818*/{0.275543,1.94236,-1.76297,0.315868,1.87725,-1.91663,0.220394,1.81101,-1.63793,0.178374,1.70152,-1.54736,0.297262,1.68141,-1.65631,0.374802,1.66259,-1.70331,0.214845,1.95361,-2.13218,0.312068,1.87526,-1.98889,0.105967,1.92466,-2.02392,-0.023499,1.83526,-2.14574,-0.048094,1.74525,-2.21677,0.100325,1.6188,-2.19567,0.138708,1.56597,-2.22171,0.211022,1.44054,-1.79942,0.029212,1.51893,-1.88322,0.030783,1.54564,-1.93171,0.037251,1.51863,-1.99899,0.222269,1.46141,-2.05311,0.153831,1.114 [...]
+/*1819*/{0.272825,1.93964,-1.76266,0.315596,1.87518,-1.91681,0.216634,1.80829,-1.63769,0.173686,1.69925,-1.54828,0.292391,1.67651,-1.65586,0.370462,1.65513,-1.70161,0.213732,1.94997,-2.13201,0.310408,1.87252,-1.98876,0.105132,1.92121,-2.02448,-0.023529,1.83022,-2.14797,-0.04693,1.73999,-2.2193,0.102042,1.61385,-2.19711,0.140313,1.56144,-2.22302,0.210822,1.4375,-1.79919,0.028802,1.51532,-1.8831,0.030987,1.54253,-1.93196,0.036708,1.51469,-1.99973,0.22186,1.45759,-2.0542,0.158204,1.11097,-1 [...]
+/*1820*/{0.270137,1.93675,-1.76214,0.313568,1.8718,-1.91573,0.212433,1.80502,-1.63746,0.168265,1.69598,-1.5489,0.287355,1.67054,-1.65528,0.366112,1.64758,-1.70002,0.212913,1.94682,-2.13244,0.309885,1.86974,-1.98854,0.103806,1.91717,-2.02543,-0.022068,1.82479,-2.15044,-0.045969,1.73429,-2.22215,0.103738,1.60904,-2.1993,0.142224,1.55646,-2.22466,0.212438,1.43484,-1.80077,0.028845,1.51126,-1.88346,0.030572,1.53843,-1.93309,0.036901,1.51024,-1.99999,0.221812,1.45482,-2.05497,0.164666,1.10834 [...]
+/*1821*/{0.267299,1.93314,-1.76153,0.313656,1.8689,-1.91583,0.208461,1.80194,-1.63711,0.162276,1.69308,-1.54976,0.281815,1.6638,-1.65419,0.360787,1.63965,-1.69807,0.211448,1.94267,-2.13255,0.308939,1.86653,-1.98788,0.102759,1.91276,-2.02729,-0.022864,1.81826,-2.15346,-0.044455,1.72853,-2.22478,0.105911,1.60354,-2.2012,0.144793,1.55129,-2.22656,0.212467,1.43143,-1.80134,0.028979,1.50705,-1.88351,0.030209,1.53487,-1.9331,0.035254,1.50616,-1.99972,0.221484,1.45173,-2.05561,0.170047,1.10536, [...]
+/*1822*/{0.264572,1.92947,-1.76091,0.312517,1.86582,-1.91525,0.204095,1.79796,-1.63751,0.155845,1.6898,-1.55095,0.276851,1.65825,-1.65371,0.355239,1.63106,-1.69565,0.211391,1.9384,-2.1329,0.308591,1.86333,-1.98781,0.101826,1.90867,-2.0278,-0.022911,1.81251,-2.15613,-0.043074,1.72225,-2.22772,0.108521,1.59775,-2.20312,0.147493,1.54554,-2.22798,0.213367,1.42887,-1.80222,0.028781,1.50226,-1.8834,0.030542,1.52964,-1.93267,0.034187,1.50117,-1.99952,0.220579,1.4478,-2.05654,0.17364,1.10136,-1. [...]
+/*1823*/{0.262582,1.926,-1.7603,0.311709,1.86155,-1.91418,0.199938,1.79362,-1.63711,0.150585,1.68722,-1.55172,0.270791,1.65164,-1.65243,0.350363,1.62318,-1.69314,0.211076,1.93347,-2.13362,0.308064,1.85989,-1.98759,0.100353,1.90399,-2.02906,-0.022176,1.80547,-2.15865,-0.041342,1.71542,-2.23044,0.110578,1.59167,-2.2052,0.150248,1.53971,-2.23001,0.214131,1.42532,-1.80322,0.028578,1.49747,-1.88287,0.029636,1.52531,-1.93336,0.033765,1.49592,-1.99996,0.220599,1.44451,-2.05739,0.179472,1.09821, [...]
+/*1824*/{0.260195,1.92154,-1.75928,0.31091,1.85778,-1.91415,0.196117,1.78929,-1.63687,0.143444,1.68328,-1.55204,0.26563,1.64509,-1.65219,0.343828,1.61448,-1.69149,0.21022,1.9282,-2.1336,0.307297,1.85566,-1.98761,0.099148,1.89905,-2.0306,-0.022154,1.79799,-2.16189,-0.039582,1.70878,-2.23351,0.113366,1.58511,-2.20774,0.153705,1.5338,-2.23144,0.214402,1.42195,-1.80504,0.028411,1.49229,-1.88274,0.029629,1.51953,-1.93355,0.03082,1.49302,-2.0002,0.218863,1.44106,-2.0582,0.180371,1.09549,-1.771 [...]
+/*1825*/{0.257476,1.91719,-1.75854,0.309993,1.85275,-1.91293,0.191877,1.78474,-1.63701,0.137015,1.67955,-1.55238,0.258985,1.63861,-1.65118,0.338398,1.60601,-1.68923,0.210055,1.92324,-2.13431,0.306972,1.85145,-1.98651,0.097923,1.89254,-2.03158,-0.022724,1.78974,-2.1646,-0.037236,1.70094,-2.23734,0.116633,1.5784,-2.20965,0.157325,1.52736,-2.23339,0.214245,1.41788,-1.80584,0.028162,1.48651,-1.88237,0.029118,1.51388,-1.93267,0.031907,1.48542,-1.99846,0.21874,1.43712,-2.05935,0.185311,1.09228 [...]
+/*1826*/{0.255267,1.91189,-1.75744,0.309939,1.84851,-1.91226,0.187522,1.78014,-1.63681,0.130685,1.67648,-1.55327,0.253026,1.63186,-1.64975,0.332143,1.59729,-1.68682,0.210562,1.9176,-2.13529,0.306523,1.84689,-1.98661,0.096348,1.88735,-2.03401,-0.022405,1.78193,-2.16822,-0.034622,1.69299,-2.24033,0.120098,1.57118,-2.21206,0.161408,1.51977,-2.23518,0.215867,1.41363,-1.80667,0.027534,1.48072,-1.88214,0.028368,1.50885,-1.93282,0.029342,1.48033,-1.99834,0.217633,1.43405,-2.06058,0.188674,1.088 [...]
+/*1827*/{0.252967,1.90672,-1.75627,0.309807,1.84306,-1.91112,0.183433,1.77516,-1.6365,0.123813,1.67233,-1.55408,0.246791,1.62468,-1.64889,0.325093,1.58885,-1.6843,0.211219,1.91173,-2.13583,0.305897,1.84174,-1.98554,0.094615,1.88036,-2.03468,-0.020686,1.7722,-2.17185,-0.031746,1.68465,-2.244,0.123498,1.5634,-2.21392,0.165629,1.51283,-2.23724,0.215373,1.4093,-1.80864,0.027099,1.47528,-1.88106,0.027473,1.50242,-1.93284,0.027747,1.47329,-1.9973,0.216796,1.43012,-2.06222,0.18766,1.08558,-1.76 [...]
+/*1828*/{0.25116,1.90131,-1.75519,0.309571,1.83834,-1.9105,0.178796,1.7703,-1.63643,0.117288,1.66888,-1.55466,0.240474,1.61796,-1.64731,0.319074,1.58044,-1.68154,0.211004,1.90604,-2.13635,0.305637,1.83665,-1.98498,0.09342,1.87453,-2.0364,-0.018381,1.76394,-2.17554,-0.028562,1.6763,-2.24742,0.12768,1.55552,-2.21619,0.170162,1.5049,-2.23902,0.215962,1.40437,-1.80891,0.025957,1.46923,-1.88146,0.027084,1.49648,-1.93271,0.025549,1.46861,-1.99762,0.215782,1.42648,-2.06376,0.194425,1.08089,-1.7 [...]
+/*1829*/{0.249147,1.89535,-1.75423,0.308691,1.8323,-1.90908,0.174687,1.76502,-1.63635,0.110356,1.66496,-1.5558,0.23437,1.61157,-1.64607,0.311451,1.57259,-1.6794,0.212151,1.89983,-2.13713,0.30535,1.83113,-1.98476,0.092638,1.8671,-2.03776,-0.017087,1.75426,-2.1793,-0.024335,1.66708,-2.25076,0.132285,1.5481,-2.21821,0.175901,1.49724,-2.24073,0.216209,1.39998,-1.81058,0.024749,1.46388,-1.88035,0.025093,1.49055,-1.93207,0.023049,1.46304,-1.99611,0.214137,1.42183,-2.0659,0.199635,1.07659,-1.75 [...]
+/*1830*/{0.247427,1.88962,-1.75299,0.308431,1.82596,-1.90759,0.170309,1.76023,-1.63661,0.103723,1.66116,-1.5564,0.227627,1.60495,-1.64518,0.305047,1.5645,-1.67672,0.212962,1.89363,-2.13786,0.305728,1.82585,-1.98374,0.091391,1.86127,-2.03993,-0.014355,1.74552,-2.18396,-0.019891,1.65774,-2.25413,0.137717,1.53998,-2.22108,0.181239,1.48902,-2.24265,0.215698,1.39496,-1.81224,0.023192,1.45727,-1.88015,0.023063,1.4842,-1.93174,0.019966,1.45829,-1.99537,0.212084,1.417,-2.06766,0.201882,1.07251,- [...]
+/*1831*/{0.245821,1.88331,-1.75114,0.308157,1.8208,-1.90618,0.165728,1.75527,-1.63668,0.097049,1.65773,-1.55695,0.220597,1.59856,-1.6435,0.296401,1.55675,-1.67414,0.213017,1.88824,-2.13875,0.306174,1.81996,-1.98292,0.090098,1.85405,-2.04091,-0.012821,1.73636,-2.18968,-0.014535,1.64809,-2.2573,0.143457,1.53186,-2.22328,0.18666,1.48066,-2.24464,0.214656,1.3892,-1.81364,0.020325,1.45235,-1.87895,0.020652,1.47809,-1.93053,0.016429,1.45369,-1.99473,0.207929,1.41195,-2.06967,0.205228,1.06848,- [...]
+/*1832*/{0.244245,1.87798,-1.75038,0.308418,1.81386,-1.90506,0.16178,1.74979,-1.63611,0.088992,1.65446,-1.55849,0.213567,1.59232,-1.64162,0.288656,1.55005,-1.67176,0.21417,1.88258,-2.14035,0.307423,1.81406,-1.98267,0.091067,1.85073,-2.04128,-0.008644,1.72654,-2.1939,-0.009412,1.6385,-2.26093,0.149318,1.52307,-2.22623,0.19363,1.47239,-2.24631,0.214446,1.38499,-1.81599,0.018402,1.44749,-1.87822,0.017802,1.47233,-1.9296,0.01216,1.4497,-1.99436,0.205197,1.40933,-2.07136,0.210209,1.06468,-1.7 [...]
+/*1833*/{0.243145,1.8728,-1.74879,0.308584,1.80868,-1.90342,0.157324,1.74526,-1.63592,0.082658,1.65124,-1.55881,0.206718,1.58679,-1.64025,0.281626,1.54349,-1.66888,0.216011,1.87801,-2.14227,0.307809,1.8088,-1.98145,0.091794,1.84983,-2.04118,-0.003505,1.71683,-2.20075,-0.003004,1.62836,-2.26411,0.155789,1.51424,-2.22896,0.199696,1.46391,-2.24801,0.212208,1.38,-1.81824,0.015197,1.44199,-1.87768,0.01385,1.4675,-1.9284,0.008308,1.44525,-1.9937,0.202807,1.40655,-2.07325,0.213547,1.06196,-1.75 [...]
+/*1834*/{0.242539,1.86774,-1.74683,0.308555,1.80267,-1.90203,0.152381,1.74074,-1.63579,0.074086,1.64919,-1.56058,0.199799,1.58165,-1.63861,0.273408,1.53769,-1.66705,0.217066,1.87288,-2.14332,0.308017,1.80363,-1.98092,0.092419,1.84733,-2.04152,0.002792,1.7096,-2.20884,0.00381,1.61792,-2.26689,0.163937,1.50618,-2.23179,0.207654,1.45651,-2.24981,0.210559,1.37535,-1.82037,0.011901,1.43799,-1.87618,0.01239,1.46413,-1.92811,0.004293,1.44295,-1.99245,0.199239,1.40471,-2.07459,0.219307,1.06196,- [...]
+/*1835*/{0.242034,1.86401,-1.74551,0.309964,1.79851,-1.90113,0.147879,1.73726,-1.63579,0.067098,1.64739,-1.5604,0.193123,1.5779,-1.63841,0.265376,1.53312,-1.66521,0.218756,1.86854,-2.14405,0.308508,1.79976,-1.97917,0.092896,1.84477,-2.04205,0.009701,1.69917,-2.21651,0.012719,1.60743,-2.27045,0.171889,1.49856,-2.23405,0.216068,1.44911,-2.25062,0.208286,1.37182,-1.82186,0.009844,1.43347,-1.87467,0.009057,1.46239,-1.92741,0.003362,1.43908,-1.98946,0.196608,1.40315,-2.07631,0.225003,1.06402, [...]
+/*1836*/{0.241826,1.85898,-1.74356,0.309678,1.79498,-1.89944,0.143326,1.73501,-1.63612,0.060348,1.64677,-1.56204,0.185268,1.57456,-1.63771,0.25828,1.52778,-1.6637,0.220261,1.86471,-2.14438,0.309186,1.7964,-1.97734,0.093054,1.84415,-2.04258,0.017554,1.68973,-2.22336,0.022501,1.59769,-2.27314,0.181163,1.49153,-2.23488,0.225783,1.44331,-2.25159,0.205792,1.36799,-1.82284,0.007404,1.43057,-1.87311,0.007388,1.46111,-1.92706,0.000379,1.43778,-1.98666,0.193246,1.40205,-2.07761,0.231937,1.06589,- [...]
+/*1837*/{0.241379,1.85565,-1.74223,0.309461,1.79168,-1.8985,0.138384,1.73388,-1.63688,0.052789,1.64742,-1.56438,0.178784,1.57236,-1.63721,0.250211,1.5237,-1.66179,0.220612,1.8616,-2.14349,0.309318,1.79325,-1.97657,0.092818,1.84193,-2.0427,0.023472,1.6813,-2.22889,0.033354,1.58818,-2.27464,0.190696,1.48635,-2.23541,0.236503,1.43871,-2.25131,0.202937,1.36452,-1.8232,0.004826,1.42912,-1.87123,0.005441,1.4602,-1.92592,-0.001024,1.43639,-1.98515,0.190766,1.40112,-2.07916,0.236657,1.068,-1.752 [...]
+/*1838*/{0.241368,1.85349,-1.74115,0.308437,1.78964,-1.89683,0.133054,1.73448,-1.63754,0.045964,1.64849,-1.56704,0.171541,1.57094,-1.63712,0.242433,1.52059,-1.65968,0.222185,1.85936,-2.1424,0.309186,1.7905,-1.97501,0.093713,1.84041,-2.04276,0.030911,1.67325,-2.2338,0.043986,1.58066,-2.27621,0.20095,1.48132,-2.2351,0.247471,1.43544,-2.25086,0.201323,1.36184,-1.82457,0.003135,1.4279,-1.8705,0.00298,1.45947,-1.92578,-0.002777,1.43596,-1.98299,0.188508,1.40033,-2.08046,0.246943,1.07066,-1.75 [...]
+/*1839*/{0.240457,1.85149,-1.74062,0.306987,1.78763,-1.89551,0.128267,1.73545,-1.63852,0.038468,1.65071,-1.57004,0.165047,1.56961,-1.63742,0.235353,1.51859,-1.65804,0.223523,1.85738,-2.14126,0.308675,1.78832,-1.97367,0.093821,1.83861,-2.043,0.038067,1.66689,-2.23554,0.053711,1.57481,-2.27685,0.212378,1.47859,-2.23409,0.259153,1.43376,-2.24935,0.198289,1.35952,-1.82405,0.001716,1.42743,-1.86951,0.001402,1.45897,-1.9255,-0.00462,1.43598,-1.98249,0.186465,1.39954,-2.08142,0.254563,1.07423,- [...]
+/*1840*/{0.239727,1.8503,-1.74039,0.306172,1.78473,-1.89413,0.124112,1.73685,-1.63938,0.032782,1.65434,-1.57364,0.157543,1.56958,-1.63825,0.227505,1.51707,-1.65741,0.226308,1.8554,-2.14035,0.307615,1.78673,-1.97303,0.094329,1.83708,-2.04308,0.046024,1.66299,-2.23544,0.062796,1.57082,-2.27721,0.223166,1.47796,-2.23268,0.271281,1.43324,-2.2471,0.196415,1.35819,-1.823,0.000317,1.42771,-1.87016,-0.000218,1.45886,-1.92551,-0.00585,1.43593,-1.98237,0.185017,1.39928,-2.08202,0.26173,1.07861,-1. [...]
+/*1841*/{0.239123,1.85001,-1.73957,0.305328,1.78438,-1.89327,0.119559,1.73858,-1.64048,0.026073,1.65723,-1.57588,0.151512,1.57043,-1.63796,0.221099,1.5174,-1.65683,0.229126,1.85445,-2.13966,0.307699,1.78537,-1.97081,0.095134,1.83658,-2.0434,0.052418,1.65917,-2.23448,0.070774,1.56769,-2.2768,0.234163,1.47773,-2.23015,0.283683,1.43471,-2.24356,0.194003,1.35768,-1.82163,-0.001281,1.42744,-1.87003,-0.001663,1.45865,-1.92591,-0.006522,1.43594,-1.98261,0.183742,1.39843,-2.0825,0.268559,1.086,- [...]
+/*1842*/{0.237952,1.85021,-1.73926,0.304402,1.7837,-1.89196,0.114292,1.74094,-1.64157,0.01961,1.6614,-1.5791,0.144772,1.5717,-1.63783,0.213836,1.51785,-1.65641,0.231265,1.85349,-2.13849,0.307456,1.78455,-1.97002,0.095801,1.83629,-2.04383,0.059963,1.65707,-2.23366,0.079148,1.56513,-2.27698,0.244091,1.47751,-2.2268,0.295413,1.43684,-2.23947,0.191567,1.35728,-1.81999,-0.002862,1.42792,-1.87079,-0.00191,1.45844,-1.92602,-0.008023,1.43688,-1.98354,0.182954,1.39768,-2.08291,0.275535,1.09264,-1 [...]
+/*1843*/{0.236049,1.85086,-1.73923,0.303808,1.78369,-1.891,0.110679,1.74379,-1.64207,0.014323,1.6658,-1.58192,0.13868,1.57355,-1.63877,0.20694,1.5192,-1.65568,0.233789,1.85301,-2.1383,0.307123,1.78435,-1.9683,0.096718,1.83604,-2.04599,0.065273,1.65478,-2.23243,0.087484,1.56345,-2.27703,0.254208,1.48006,-2.22334,0.307319,1.44129,-2.23489,0.189508,1.3583,-1.81868,-0.003691,1.42764,-1.87158,-0.003509,1.45849,-1.92728,-0.008473,1.43648,-1.98409,0.182464,1.39715,-2.08285,0.283335,1.09841,-1.7 [...]
+/*1844*/{0.233936,1.85216,-1.73824,0.30273,1.7834,-1.88924,0.106294,1.7468,-1.64312,0.007737,1.67005,-1.58387,0.131962,1.57649,-1.63805,0.200416,1.52133,-1.65552,0.236657,1.85326,-2.13756,0.307165,1.78471,-1.9672,0.097428,1.83613,-2.04614,0.069612,1.65279,-2.23113,0.095092,1.56159,-2.27692,0.263739,1.48329,-2.21893,0.318066,1.44659,-2.2297,0.18807,1.35934,-1.8174,-0.005369,1.42836,-1.87236,-0.005212,1.45869,-1.92826,-0.009114,1.43709,-1.98537,0.181697,1.39642,-2.08263,0.288962,1.10605,-1 [...]
+/*1845*/{0.231569,1.85436,-1.73791,0.301504,1.78552,-1.88855,0.101704,1.75037,-1.64424,0.001047,1.67321,-1.58611,0.126322,1.58063,-1.63901,0.193484,1.52398,-1.65514,0.239005,1.85473,-2.13618,0.306697,1.78537,-1.96585,0.097963,1.83659,-2.04711,0.073348,1.65184,-2.23009,0.103422,1.56043,-2.27678,0.272908,1.48828,-2.21464,0.329243,1.45321,-2.22305,0.186321,1.36093,-1.8161,-0.00693,1.42899,-1.87423,-0.005141,1.45884,-1.92847,-0.009994,1.43796,-1.98662,0.182004,1.39611,-2.08194,0.292631,1.114 [...]
+/*1846*/{0.228087,1.85666,-1.73773,0.300702,1.7866,-1.88629,0.097022,1.75394,-1.64485,-0.005222,1.67849,-1.58792,0.120091,1.58473,-1.63928,0.187129,1.52757,-1.65485,0.240609,1.85595,-2.13517,0.306574,1.78659,-1.96368,0.098694,1.83818,-2.04848,0.07728,1.65088,-2.22784,0.111072,1.56036,-2.27612,0.281737,1.49422,-2.20963,0.339071,1.46107,-2.21686,0.184525,1.36218,-1.81502,-0.007032,1.42984,-1.87462,-0.005539,1.45998,-1.92975,-0.010215,1.43842,-1.98838,0.181435,1.39534,-2.08155,0.297724,1.12 [...]
+/*1847*/{0.225858,1.85903,-1.73679,0.299471,1.78859,-1.88522,0.092609,1.75751,-1.6456,-0.009983,1.68297,-1.59071,0.11375,1.58847,-1.64036,0.180752,1.5314,-1.65532,0.242198,1.85817,-2.13403,0.306336,1.78919,-1.96266,0.099191,1.83885,-2.0498,0.081775,1.64988,-2.2262,0.118497,1.56115,-2.27581,0.290778,1.50067,-2.20405,0.348948,1.46921,-2.21023,0.18393,1.36356,-1.81393,-0.008029,1.43114,-1.87702,-0.005578,1.46181,-1.93071,-0.010043,1.44011,-1.99022,0.181103,1.39502,-2.08159,0.302764,1.13035, [...]
+/*1848*/{0.222977,1.8616,-1.73698,0.298897,1.79104,-1.88312,0.088562,1.76133,-1.64662,-0.016672,1.68784,-1.59221,0.107788,1.59341,-1.64138,0.175027,1.53663,-1.65541,0.24292,1.86109,-2.13259,0.306709,1.79169,-1.96099,0.099852,1.84022,-2.05002,0.085206,1.64964,-2.22414,0.126208,1.56225,-2.27548,0.298388,1.50773,-2.19846,0.358065,1.47858,-2.20319,0.182483,1.36583,-1.81329,-0.008426,1.43279,-1.87885,-0.005883,1.46323,-1.93179,-0.009715,1.44246,-1.99105,0.181296,1.39471,-2.0815,0.307132,1.138 [...]
+/*1849*/{0.220008,1.86478,-1.73652,0.298267,1.79444,-1.88171,0.084354,1.7656,-1.64709,-0.022644,1.69221,-1.59425,0.102723,1.59898,-1.6426,0.168733,1.54131,-1.656,0.244453,1.86424,-2.13113,0.306956,1.79467,-1.95964,0.10052,1.84253,-2.05112,0.089875,1.64979,-2.22251,0.133423,1.56362,-2.27483,0.306557,1.51634,-2.19245,0.367391,1.48869,-2.19621,0.181944,1.36779,-1.81239,-0.008318,1.4348,-1.87982,-0.005553,1.46481,-1.9339,-0.009383,1.44441,-1.99275,0.181554,1.39428,-2.08137,0.311121,1.14419,- [...]
+/*1850*/{0.217884,1.86791,-1.73563,0.297789,1.79763,-1.87993,0.080305,1.76952,-1.64829,-0.028051,1.69756,-1.59643,0.09735,1.60461,-1.64374,0.163626,1.54573,-1.65539,0.246396,1.86859,-2.13032,0.30688,1.79769,-1.95749,0.100691,1.84331,-2.05118,0.094995,1.64979,-2.22026,0.14112,1.56573,-2.27398,0.313677,1.52364,-2.18711,0.375151,1.4987,-2.18929,0.181154,1.37058,-1.81233,-0.008786,1.4375,-1.88204,-0.005023,1.46747,-1.93507,-0.008248,1.44626,-1.99459,0.182742,1.39404,-2.08132,0.314399,1.15177 [...]
+/*1851*/{0.214413,1.87111,-1.73571,0.297707,1.80198,-1.87918,0.077042,1.77376,-1.6486,-0.033156,1.70265,-1.59833,0.091664,1.60992,-1.64504,0.158117,1.55085,-1.65613,0.247146,1.87226,-2.1292,0.30738,1.80142,-1.95648,0.101176,1.8462,-2.05205,0.099163,1.65111,-2.21849,0.14921,1.56841,-2.27282,0.320568,1.53248,-2.18126,0.383587,1.50872,-2.18143,0.180897,1.37361,-1.81195,-0.007796,1.44,-1.88227,-0.003249,1.46995,-1.93609,-0.007441,1.4484,-1.99607,0.184538,1.39436,-2.08141,0.318085,1.16003,-1. [...]
+/*1852*/{0.211927,1.87483,-1.73541,0.296513,1.80524,-1.87682,0.072581,1.77868,-1.64994,-0.037949,1.70842,-1.60047,0.086874,1.61563,-1.647,0.152695,1.55673,-1.65793,0.24795,1.87738,-2.12841,0.307143,1.805,-1.95439,0.102057,1.84803,-2.05229,0.103353,1.65245,-2.21789,0.156456,1.57126,-2.27201,0.327797,1.54263,-2.17578,0.390502,1.52005,-2.17439,0.182198,1.37796,-1.8121,-0.006951,1.44331,-1.88465,-0.002854,1.472,-1.9379,-0.00556,1.45063,-1.99703,0.185307,1.39447,-2.08111,0.319666,1.16696,-1.7 [...]
+/*1853*/{0.208928,1.87831,-1.73513,0.296685,1.80936,-1.8762,0.069352,1.78292,-1.65058,-0.042611,1.71399,-1.60292,0.081604,1.62168,-1.64842,0.148257,1.56271,-1.65844,0.248147,1.88163,-2.12702,0.307706,1.8092,-1.95333,0.102654,1.85088,-2.05325,0.106011,1.65761,-2.21908,0.164333,1.57478,-2.27182,0.333825,1.55191,-2.1707,0.397225,1.5315,-2.16715,0.183378,1.38187,-1.81362,-0.005144,1.44713,-1.88535,-0.001995,1.47485,-1.93978,-0.004631,1.45415,-1.99841,0.18578,1.39445,-2.08193,0.321464,1.17357 [...]
+/*1854*/{0.2065,1.88227,-1.73499,0.295898,1.81405,-1.87457,0.065902,1.78739,-1.65148,-0.047653,1.71988,-1.60485,0.077448,1.62732,-1.6507,0.143098,1.56833,-1.65969,0.249986,1.88658,-2.12577,0.309737,1.81281,-1.95058,0.103695,1.85386,-2.05352,0.111197,1.66074,-2.22051,0.171783,1.57842,-2.27081,0.33861,1.56122,-2.16533,0.40237,1.54271,-2.1607,0.184265,1.3856,-1.8135,-0.004767,1.45102,-1.88661,4.5e-005,1.4777,-1.94117,-0.001331,1.45649,-1.99966,0.187032,1.39467,-2.08178,0.323974,1.18071,-1.7 [...]
+/*1855*/{0.204296,1.8864,-1.73448,0.296354,1.81745,-1.87316,0.061823,1.79305,-1.65262,-0.051084,1.72586,-1.60762,0.073229,1.63336,-1.65258,0.139217,1.5746,-1.66041,0.251265,1.89189,-2.12496,0.309221,1.81722,-1.9502,0.104896,1.8573,-2.05429,0.116774,1.66439,-2.22271,0.178975,1.58213,-2.2702,0.343387,1.57068,-2.16033,0.407718,1.55409,-2.15422,0.185371,1.38945,-1.81464,-0.003575,1.45469,-1.88668,0.001219,1.48138,-1.94285,-0.000107,1.46039,-2.00052,0.188276,1.39466,-2.08196,0.32599,1.18807,- [...]
+/*1856*/{0.202022,1.88992,-1.73456,0.294801,1.82196,-1.87229,0.058858,1.79726,-1.65383,-0.055414,1.73158,-1.61044,0.069689,1.63961,-1.65353,0.135029,1.57999,-1.66187,0.251813,1.89736,-2.12403,0.309629,1.82127,-1.94804,0.106032,1.86031,-2.05474,0.122983,1.66717,-2.22406,0.185906,1.5861,-2.26907,0.347445,1.58023,-2.15491,0.412074,1.56578,-2.14743,0.186611,1.39303,-1.81577,-0.002459,1.45919,-1.88871,0.002506,1.48496,-1.94425,0.002461,1.46301,-2.00218,0.189567,1.39494,-2.08243,0.327456,1.194 [...]
+/*1857*/{0.199787,1.894,-1.73444,0.294988,1.82604,-1.87044,0.055905,1.80218,-1.65454,-0.059711,1.73724,-1.61214,0.06473,1.6448,-1.65566,0.131769,1.58612,-1.66319,0.25311,1.902,-2.12252,0.309616,1.8259,-1.94684,0.107187,1.8635,-2.05504,0.128465,1.67111,-2.22609,0.19262,1.59021,-2.26822,0.351778,1.59058,-2.15008,0.415447,1.5768,-2.14049,0.189244,1.39647,-1.81622,-0.000241,1.46365,-1.88913,0.004699,1.48808,-1.94599,0.004747,1.46669,-2.00201,0.190743,1.39507,-2.08257,0.329217,1.20137,-1.7538 [...]
+/*1858*/{0.197075,1.898,-1.73478,0.294632,1.83029,-1.86887,0.052714,1.80714,-1.65589,-0.063918,1.74281,-1.61524,0.061192,1.6507,-1.65701,0.127022,1.59162,-1.66458,0.255353,1.9075,-2.12109,0.309447,1.83065,-1.94509,0.108089,1.86698,-2.05579,0.134939,1.67529,-2.22811,0.199178,1.59488,-2.26774,0.35351,1.59915,-2.14537,0.418984,1.58707,-2.13458,0.190044,1.40012,-1.81752,0.001108,1.46827,-1.88963,0.005799,1.49171,-1.94756,0.007523,1.46968,-2.00305,0.191982,1.39536,-2.08267,0.330549,1.20807,-1 [...]
+/*1859*/{0.195265,1.90173,-1.73475,0.294045,1.83536,-1.86831,0.050496,1.81149,-1.65736,-0.066616,1.74776,-1.61763,0.057554,1.656,-1.65898,0.123264,1.59667,-1.66518,0.256217,1.91177,-2.11992,0.309778,1.83467,-1.9435,0.109513,1.86989,-2.05664,0.140063,1.67977,-2.23006,0.205877,1.59888,-2.26724,0.356339,1.60856,-2.14077,0.421143,1.59748,-2.12947,0.191226,1.40367,-1.81794,0.002674,1.47286,-1.89038,0.008414,1.49506,-1.94886,0.009605,1.47254,-2.0039,0.193055,1.39553,-2.08271,0.33263,1.21385,-1 [...]
+/*1860*/{0.193282,1.90531,-1.73501,0.29469,1.8387,-1.86687,0.047676,1.81631,-1.65883,-0.070583,1.75297,-1.61979,0.054544,1.66091,-1.66052,0.12008,1.60193,-1.66629,0.2579,1.91682,-2.11806,0.309724,1.83902,-1.94222,0.110545,1.87307,-2.05784,0.145248,1.68431,-2.23218,0.212273,1.60368,-2.26657,0.358376,1.6166,-2.13619,0.423672,1.60762,-2.12343,0.193721,1.40792,-1.81946,0.004821,1.4776,-1.89025,0.010018,1.49905,-1.95081,0.011658,1.4759,-2.00402,0.19437,1.39657,-2.0828,0.334094,1.21889,-1.7539 [...]
+/*1861*/{0.191533,1.90881,-1.73552,0.293236,1.84274,-1.86546,0.045111,1.82065,-1.66064,-0.073724,1.75738,-1.62302,0.052136,1.66595,-1.66198,0.118479,1.60707,-1.66737,0.260044,1.92143,-2.11707,0.3098,1.84308,-1.94044,0.111985,1.87518,-2.05809,0.151716,1.68816,-2.23458,0.218017,1.60798,-2.26573,0.360182,1.6248,-2.13248,0.425714,1.61705,-2.11881,0.1944,1.41139,-1.81998,0.006097,1.48196,-1.89068,0.011844,1.5023,-1.95195,0.013905,1.47908,-2.00543,0.195634,1.39697,-2.08272,0.333936,1.22446,-1. [...]
+/*1862*/{0.190583,1.91269,-1.73546,0.286699,1.84941,-1.86621,0.042035,1.82566,-1.66219,-0.077953,1.76193,-1.62623,0.049368,1.67077,-1.6637,0.115754,1.61155,-1.66818,0.26229,1.9253,-2.11484,0.308872,1.84788,-1.93984,0.113408,1.87896,-2.05888,0.157827,1.69203,-2.23635,0.223715,1.61261,-2.26532,0.362056,1.63271,-2.12799,0.426689,1.62594,-2.11411,0.195905,1.41482,-1.82033,0.008695,1.48632,-1.8915,0.013704,1.5062,-1.95314,0.015969,1.48241,-2.00565,0.196743,1.3977,-2.08265,0.334782,1.22836,-1. [...]
+/*1863*/{0.189111,1.91562,-1.73529,0.286723,1.85357,-1.86527,0.039462,1.82966,-1.66417,-0.080359,1.76646,-1.62891,0.04751,1.67456,-1.66551,0.112673,1.61548,-1.66916,0.263432,1.92902,-2.11397,0.30935,1.85192,-1.93836,0.114429,1.88192,-2.05933,0.163796,1.69675,-2.23842,0.228762,1.61683,-2.26528,0.363501,1.63934,-2.1246,0.42753,1.63358,-2.10895,0.197767,1.4189,-1.82111,0.010004,1.49077,-1.89165,0.01608,1.5098,-1.95403,0.018894,1.48498,-2.00526,0.198461,1.39884,-2.08202,0.335308,1.23183,-1.7 [...]
+/*1864*/{0.187947,1.91853,-1.73569,0.291999,1.85397,-1.86263,0.037725,1.83346,-1.66533,-0.082808,1.77063,-1.63244,0.044129,1.67891,-1.66642,0.110794,1.61965,-1.66902,0.264688,1.93263,-2.11255,0.310161,1.85485,-1.93698,0.116915,1.88469,-2.06049,0.168728,1.70105,-2.23934,0.233124,1.62068,-2.26509,0.364878,1.64666,-2.1222,0.42861,1.64083,-2.10526,0.198594,1.42231,-1.82144,0.0118,1.49518,-1.89134,0.018026,1.51274,-1.95476,0.020371,1.48827,-2.00558,0.20019,1.40017,-2.08185,0.336561,1.23447,-1 [...]
+/*1865*/{0.186454,1.92119,-1.73608,0.285755,1.86135,-1.86616,0.035794,1.83733,-1.66719,-0.08563,1.77371,-1.63488,0.04284,1.68258,-1.66846,0.109071,1.62297,-1.66976,0.265738,1.93595,-2.11088,0.309637,1.8584,-1.93635,0.117968,1.88748,-2.06166,0.17402,1.70531,-2.24066,0.237726,1.62483,-2.26521,0.365364,1.6517,-2.11987,0.429295,1.64719,-2.10083,0.199561,1.42495,-1.82153,0.014973,1.49886,-1.89157,0.020468,1.51573,-1.95507,0.022815,1.4912,-2.00594,0.20123,1.40137,-2.08102,0.33728,1.23659,-1.75 [...]
+/*1866*/{0.186118,1.924,-1.73582,0.290987,1.86192,-1.86133,0.033556,1.84148,-1.66944,-0.088636,1.77733,-1.63764,0.040196,1.68579,-1.66923,0.107367,1.62628,-1.6702,0.267205,1.93883,-2.10955,0.310464,1.86098,-1.93434,0.119519,1.89072,-2.06257,0.178631,1.70923,-2.24168,0.241234,1.62825,-2.26503,0.3666,1.65737,-2.11805,0.429138,1.65282,-2.09796,0.200454,1.42827,-1.82151,0.016499,1.50243,-1.89081,0.021983,1.51925,-1.95552,0.024172,1.49392,-2.00582,0.202458,1.40269,-2.08061,0.337654,1.23768,-1 [...]
+/*1867*/{0.185617,1.92642,-1.73652,0.291379,1.86462,-1.8609,0.032809,1.84458,-1.67106,-0.090955,1.78014,-1.64141,0.038182,1.68848,-1.67041,0.105548,1.62942,-1.67035,0.269291,1.94187,-2.10832,0.310679,1.86396,-1.93351,0.120659,1.89292,-2.06295,0.184038,1.71299,-2.24237,0.244797,1.63185,-2.26493,0.367978,1.66156,-2.11602,0.430452,1.65762,-2.09529,0.200412,1.4324,-1.82191,0.017513,1.50613,-1.89022,0.023595,1.52265,-1.95583,0.026311,1.49788,-2.00567,0.204194,1.40426,-2.08073,0.337874,1.23807 [...]
+/*1868*/{0.185518,1.92798,-1.73663,0.29204,1.86762,-1.8605,0.031138,1.84803,-1.67321,-0.091303,1.78298,-1.64394,0.037381,1.69089,-1.67197,0.104199,1.63184,-1.67061,0.270817,1.9441,-2.10729,0.310199,1.86671,-1.93329,0.122629,1.89525,-2.06423,0.187913,1.7162,-2.24293,0.248365,1.6346,-2.26536,0.368198,1.66517,-2.11448,0.430862,1.66144,-2.09317,0.202037,1.43539,-1.82154,0.019353,1.50909,-1.89064,0.0245,1.52532,-1.95653,0.028063,1.50009,-2.00558,0.204613,1.40629,-2.07994,0.335792,1.23831,-1.7 [...]
+/*1869*/{0.184992,1.9298,-1.73661,0.29129,1.86911,-1.85962,0.030421,1.85045,-1.67478,-0.095011,1.78419,-1.64703,0.036431,1.69351,-1.67317,0.103613,1.63345,-1.67117,0.27233,1.94634,-2.10598,0.310935,1.86858,-1.93217,0.122802,1.89797,-2.06488,0.191047,1.72007,-2.24381,0.251248,1.63721,-2.26568,0.368453,1.66778,-2.11341,0.431266,1.66426,-2.09097,0.205477,1.437,-1.82089,0.021122,1.51249,-1.89052,0.02596,1.52841,-1.95626,0.029214,1.50298,-2.00594,0.205488,1.40833,-2.0793,0.334809,1.23707,-1.7 [...]
+/*1870*/{0.18495,1.93177,-1.73651,0.292628,1.86998,-1.85851,0.02965,1.85246,-1.67618,-0.094506,1.78686,-1.65015,0.035456,1.69511,-1.67425,0.102504,1.63553,-1.67125,0.273061,1.94837,-2.10518,0.311058,1.87106,-1.93172,0.124719,1.90005,-2.06532,0.19234,1.72259,-2.24331,0.253392,1.63968,-2.26612,0.369621,1.67003,-2.11282,0.431275,1.66658,-2.08895,0.206287,1.43989,-1.82073,0.020941,1.51524,-1.89072,0.026093,1.53189,-1.9557,0.030197,1.50626,-2.00578,0.206147,1.4109,-2.07884,0.333105,1.2329,-1. [...]
+/*1871*/{0.185435,1.93287,-1.7364,0.292323,1.87111,-1.85736,0.029278,1.85422,-1.67795,-0.097167,1.78727,-1.65251,0.034794,1.69681,-1.67511,0.102272,1.63641,-1.67127,0.273943,1.94933,-2.1042,0.311584,1.87293,-1.93102,0.125356,1.90174,-2.06564,0.194575,1.726,-2.24397,0.255708,1.6416,-2.2666,0.369817,1.67219,-2.11184,0.431326,1.66744,-2.08729,0.204192,1.44039,-1.82125,0.021059,1.51753,-1.89093,0.027264,1.5345,-1.95546,0.031364,1.50842,-2.0047,0.206237,1.41336,-2.07867,0.331045,1.2299,-1.763 [...]
+/*1872*/{0.186196,1.9344,-1.73544,0.288072,1.8753,-1.85886,0.028168,1.8551,-1.67919,-0.097503,1.78858,-1.6549,0.033928,1.69744,-1.67632,0.101861,1.63715,-1.67106,0.274763,1.95113,-2.10338,0.311959,1.87483,-1.93045,0.126385,1.9033,-2.06582,0.197443,1.72806,-2.24507,0.256524,1.64287,-2.26673,0.370457,1.67291,-2.11167,0.431551,1.6678,-2.08618,0.204532,1.4415,-1.82163,0.022199,1.51953,-1.89015,0.027961,1.53675,-1.95472,0.031722,1.51119,-2.00466,0.207392,1.41579,-2.07833,0.328856,1.22565,-1.7 [...]
+/*1873*/{0.186534,1.9351,-1.73509,0.289412,1.87673,-1.85889,0.028373,1.85644,-1.68,-0.099623,1.78837,-1.65726,0.034296,1.69862,-1.67743,0.101591,1.63745,-1.67114,0.27514,1.95168,-2.10299,0.312162,1.87631,-1.93038,0.127743,1.90533,-2.06579,0.198082,1.72895,-2.24491,0.257708,1.64371,-2.26781,0.370617,1.67289,-2.11119,0.432018,1.66704,-2.08514,0.205065,1.44389,-1.82164,0.021961,1.52063,-1.88954,0.027432,1.53857,-1.95448,0.031853,1.51317,-2.00493,0.208134,1.41793,-2.07776,0.325596,1.22099,-1 [...]
+/*1874*/{0.186979,1.93577,-1.73479,0.294552,1.87375,-1.85695,0.028975,1.85645,-1.68077,-0.09929,1.78799,-1.65955,0.034648,1.69867,-1.6778,0.102964,1.6373,-1.67088,0.275906,1.95205,-2.10269,0.312692,1.87677,-1.92967,0.127551,1.90731,-2.06595,0.199788,1.73053,-2.24563,0.257618,1.64413,-2.26827,0.371217,1.6722,-2.11117,0.432224,1.66634,-2.08482,0.205016,1.44439,-1.82178,0.022367,1.52167,-1.88839,0.02688,1.54043,-1.95397,0.031419,1.51519,-2.00418,0.20858,1.42029,-2.07733,0.32266,1.21549,-1.7 [...]
+/*1875*/{0.1878,1.93625,-1.73385,0.294188,1.87451,-1.85602,0.028437,1.85632,-1.6812,-0.098764,1.78748,-1.66072,0.035207,1.69796,-1.67897,0.103417,1.63666,-1.67005,0.275736,1.95178,-2.10233,0.313058,1.87715,-1.92954,0.128115,1.908,-2.06558,0.199647,1.73066,-2.24557,0.2576,1.64411,-2.26828,0.371345,1.67146,-2.11113,0.431759,1.66378,-2.08393,0.205168,1.44571,-1.82222,0.021557,1.52198,-1.88766,0.026791,1.54144,-1.95354,0.032113,1.51616,-2.00346,0.20911,1.42211,-2.07681,0.317506,1.21069,-1.76 [...]
+/*1876*/{0.188956,1.93599,-1.73309,0.289633,1.87775,-1.85699,0.029053,1.85529,-1.6825,-0.098479,1.78637,-1.66227,0.035321,1.69649,-1.67878,0.105005,1.63589,-1.66899,0.275785,1.95166,-2.10229,0.312561,1.87794,-1.92951,0.12841,1.90887,-2.06506,0.199443,1.73099,-2.24549,0.256633,1.64371,-2.26866,0.371649,1.66947,-2.1113,0.432406,1.66058,-2.08424,0.205876,1.44618,-1.82217,0.021557,1.52198,-1.88766,0.025981,1.54326,-1.95297,0.031941,1.51636,-2.0027,0.209614,1.42461,-2.07631,0.314841,1.20427,- [...]
+/*1877*/{0.189355,1.93577,-1.73287,0.2898,1.87767,-1.85673,0.029399,1.85437,-1.68311,-0.098254,1.78447,-1.66357,0.037467,1.69534,-1.67943,0.105816,1.63393,-1.66881,0.27572,1.951,-2.1018,0.312788,1.87792,-1.92909,0.128708,1.90915,-2.06346,0.198125,1.73073,-2.24479,0.255884,1.64359,-2.26873,0.372574,1.66606,-2.11126,0.431881,1.65672,-2.08427,0.205816,1.44604,-1.822,0.021242,1.52169,-1.88677,0.025509,1.54307,-1.95227,0.031665,1.5168,-2.00195,0.209763,1.42515,-2.07624,0.309434,1.1976,-1.7641 [...]
+/*1878*/{0.190622,1.93513,-1.73175,0.295013,1.87473,-1.85491,0.02972,1.85284,-1.68282,-0.096858,1.78196,-1.66418,0.038288,1.69304,-1.6797,0.108209,1.63219,-1.66739,0.275503,1.95012,-2.10092,0.313274,1.87709,-1.92864,0.128973,1.90941,-2.06294,0.195656,1.73015,-2.24452,0.252881,1.64229,-2.26859,0.372335,1.6631,-2.11141,0.431812,1.65245,-2.08419,0.20587,1.44634,-1.82267,0.020525,1.52039,-1.88674,0.025193,1.54293,-1.95139,0.031464,1.51635,-2.00131,0.210301,1.42684,-2.07571,0.307054,1.19044,- [...]
+/*1879*/{0.191473,1.93424,-1.73132,0.295917,1.87418,-1.85516,0.030473,1.85099,-1.68315,-0.095506,1.779,-1.66488,0.040651,1.69096,-1.67936,0.109523,1.62959,-1.66616,0.275643,1.94906,-2.10072,0.312845,1.87633,-1.92807,0.128184,1.90904,-2.06234,0.194413,1.72895,-2.24302,0.251304,1.64154,-2.26864,0.371752,1.65877,-2.11222,0.430945,1.64738,-2.08486,0.204999,1.44545,-1.82344,0.020137,1.51909,-1.88614,0.025118,1.54189,-1.95139,0.030699,1.51591,-2.00144,0.210499,1.4274,-2.07551,0.302243,1.18446, [...]
+/*1880*/{0.192902,1.93293,-1.73077,0.295666,1.87304,-1.85515,0.031748,1.84831,-1.68249,-0.094222,1.77564,-1.66498,0.042074,1.68747,-1.6781,0.111498,1.62627,-1.66516,0.274991,1.9476,-2.10049,0.312827,1.87529,-1.92797,0.127965,1.90891,-2.06178,0.191023,1.72787,-2.24239,0.247884,1.63944,-2.26795,0.37091,1.65542,-2.11263,0.430182,1.64252,-2.08683,0.205266,1.44488,-1.82332,0.019857,1.51731,-1.88648,0.025267,1.54067,-1.95026,0.029955,1.5146,-2.00141,0.210746,1.42753,-2.07531,0.299616,1.17683,- [...]
+/*1881*/{0.193709,1.9314,-1.72935,0.294993,1.87195,-1.855,0.032104,1.84465,-1.68193,-0.091978,1.77121,-1.66542,0.044067,1.68467,-1.67841,0.114244,1.62281,-1.6635,0.274711,1.94593,-2.09997,0.313126,1.87413,-1.92802,0.126904,1.90786,-2.05987,0.18761,1.72722,-2.24094,0.244923,1.63827,-2.2674,0.370371,1.64935,-2.11335,0.429303,1.63594,-2.08701,0.207865,1.44446,-1.82328,0.019787,1.51401,-1.88547,0.024059,1.53888,-1.95063,0.029839,1.51278,-2.00074,0.210619,1.42741,-2.0753,0.296161,1.1714,-1.76 [...]
+/*1882*/{0.194625,1.92948,-1.72935,0.296079,1.87114,-1.8548,0.03316,1.84149,-1.68137,-0.089281,1.76631,-1.66402,0.047097,1.6807,-1.67623,0.116371,1.61905,-1.6616,0.273862,1.94381,-2.09982,0.312807,1.8721,-1.92761,0.12625,1.90623,-2.059,0.18258,1.72572,-2.23997,0.241193,1.63646,-2.26677,0.367892,1.64381,-2.11424,0.427384,1.62938,-2.08843,0.204577,1.4419,-1.82324,0.019259,1.51185,-1.88547,0.023134,1.53622,-1.95056,0.029355,1.51048,-2.00056,0.210796,1.42613,-2.07473,0.29217,1.16489,-1.76081 [...]
+/*1883*/{0.194917,1.92703,-1.72861,0.29608,1.86725,-1.85475,0.035138,1.83696,-1.67988,-0.086128,1.76175,-1.66311,0.048866,1.67549,-1.67462,0.119163,1.61404,-1.65907,0.273679,1.94161,-2.0998,0.312622,1.87016,-1.92743,0.125787,1.90496,-2.05836,0.180777,1.72424,-2.23903,0.236708,1.63461,-2.26577,0.366086,1.63817,-2.11514,0.425708,1.62182,-2.08994,0.204679,1.43946,-1.82328,0.018147,1.50892,-1.88546,0.023889,1.53337,-1.94961,0.028578,1.50739,-2.00065,0.211024,1.42511,-2.07486,0.29172,1.16022, [...]
+/*1884*/{0.195971,1.92443,-1.72788,0.297072,1.86497,-1.85422,0.035959,1.83251,-1.67924,-0.084273,1.75558,-1.66111,0.051551,1.67074,-1.6726,0.122553,1.60996,-1.65768,0.27217,1.93917,-2.09899,0.311492,1.86799,-1.92756,0.124858,1.90266,-2.05641,0.176093,1.72276,-2.23783,0.232947,1.63231,-2.26519,0.364102,1.63158,-2.11688,0.423632,1.61397,-2.0916,0.205376,1.43831,-1.82339,0.018349,1.50601,-1.88457,0.022934,1.53024,-1.94968,0.028244,1.50474,-2.00032,0.211176,1.4232,-2.07501,0.288342,1.15565,- [...]
+/*1885*/{0.196486,1.92089,-1.72734,0.296257,1.86387,-1.85454,0.037866,1.82752,-1.67661,-0.080493,1.74942,-1.65906,0.055539,1.66562,-1.67064,0.12628,1.60556,-1.65509,0.271977,1.93603,-2.09956,0.311737,1.8652,-1.92707,0.12373,1.90098,-2.05532,0.170927,1.72026,-2.23623,0.228109,1.62946,-2.26484,0.361158,1.62506,-2.11829,0.420761,1.60515,-2.09436,0.203874,1.43434,-1.82245,0.017714,1.50248,-1.88444,0.02216,1.52689,-1.94986,0.027638,1.50088,-1.99997,0.211571,1.42151,-2.07459,0.285758,1.15253,- [...]
+/*1886*/{0.197651,1.91727,-1.72636,0.296258,1.85882,-1.85402,0.039207,1.8224,-1.6742,-0.077859,1.74315,-1.65653,0.057792,1.65984,-1.66741,0.129431,1.60029,-1.65289,0.27104,1.93292,-2.09942,0.311614,1.86207,-1.92665,0.123185,1.89786,-2.05472,0.166249,1.71747,-2.23452,0.223226,1.6264,-2.26428,0.357995,1.61735,-2.11919,0.417745,1.59674,-2.09635,0.206738,1.43121,-1.82138,0.017269,1.49882,-1.88455,0.022186,1.52255,-1.94963,0.026961,1.49729,-2.00009,0.211334,1.41898,-2.07395,0.282614,1.15023,- [...]
+/*1887*/{0.19828,1.91349,-1.72586,0.296205,1.85584,-1.8542,0.040961,1.81666,-1.67252,-0.075786,1.73561,-1.65326,0.061289,1.65321,-1.66431,0.133448,1.59482,-1.6501,0.269597,1.9291,-2.09988,0.311303,1.85895,-1.92698,0.122182,1.89473,-2.05314,0.160194,1.71515,-2.23388,0.217769,1.62296,-2.26378,0.354048,1.60957,-2.12192,0.414786,1.58741,-2.09981,0.203249,1.42767,-1.82124,0.016646,1.49498,-1.88434,0.02093,1.51911,-1.94978,0.027143,1.49274,-2.00009,0.211176,1.41595,-2.07369,0.280251,1.14758,-1 [...]
+/*1888*/{0.198201,1.90898,-1.72512,0.295203,1.85342,-1.85454,0.042449,1.81019,-1.67006,-0.072145,1.7282,-1.64933,0.064527,1.64707,-1.66059,0.137287,1.58942,-1.64769,0.268951,1.92522,-2.10054,0.311093,1.85545,-1.92722,0.120793,1.89081,-2.05151,0.154538,1.71214,-2.23209,0.212113,1.61962,-2.26313,0.351205,1.60275,-2.12312,0.411079,1.57754,-2.10228,0.202125,1.42418,-1.8209,0.015971,1.49113,-1.88491,0.021548,1.51552,-1.949,0.02567,1.48848,-2.00065,0.211196,1.41297,-2.07373,0.279617,1.14428,-1 [...]
+/*1889*/{0.198814,1.90418,-1.7245,0.295744,1.84961,-1.85516,0.044221,1.80265,-1.66768,-0.0698,1.72047,-1.6454,0.068503,1.64032,-1.65695,0.141539,1.58372,-1.64489,0.266924,1.92121,-2.10106,0.311322,1.85199,-1.92696,0.119999,1.88743,-2.05095,0.148273,1.70882,-2.23056,0.205469,1.61517,-2.2625,0.346903,1.59272,-2.12567,0.407088,1.56737,-2.10567,0.202868,1.42162,-1.81977,0.015447,1.48628,-1.8849,0.020444,1.51105,-1.94956,0.025295,1.48427,-2.00133,0.21069,1.40927,-2.07303,0.277203,1.14101,-1.7 [...]
+/*1890*/{0.199417,1.89913,-1.72403,0.29608,1.84558,-1.85564,0.04618,1.79596,-1.66554,-0.067706,1.71176,-1.64096,0.072192,1.63281,-1.65306,0.147011,1.5779,-1.64264,0.265965,1.91685,-2.10157,0.310363,1.84806,-1.92732,0.118866,1.88265,-2.0503,0.14355,1.70483,-2.22956,0.198981,1.61128,-2.2618,0.342619,1.58416,-2.12739,0.402428,1.55667,-2.1093,0.200405,1.41784,-1.81934,0.014732,1.48212,-1.88488,0.020072,1.50685,-1.94942,0.024419,1.47796,-2.00159,0.21075,1.40635,-2.07277,0.275559,1.13836,-1.75 [...]
+/*1891*/{0.20032,1.89368,-1.72322,0.296546,1.84062,-1.85549,0.048697,1.78832,-1.66204,-0.063672,1.7026,-1.63578,0.076814,1.62611,-1.64955,0.151506,1.57174,-1.64046,0.264122,1.91162,-2.10249,0.31079,1.84414,-1.92781,0.117375,1.87873,-2.04929,0.137082,1.70144,-2.22805,0.192807,1.60686,-2.26161,0.337337,1.5749,-2.13006,0.397092,1.5457,-2.11277,0.202284,1.41515,-1.81801,0.013739,1.47684,-1.88569,0.01937,1.50217,-1.94864,0.024121,1.47325,-2.00179,0.210064,1.40208,-2.07235,0.275208,1.13431,-1. [...]
+/*1892*/{0.20008,1.88802,-1.72249,0.295332,1.83655,-1.85535,0.050989,1.78033,-1.65889,-0.061344,1.69361,-1.63085,0.08097,1.61762,-1.64535,0.157182,1.566,-1.63846,0.262095,1.9066,-2.10385,0.310632,1.8392,-1.92819,0.116186,1.87405,-2.04807,0.132817,1.69599,-2.22732,0.185797,1.6023,-2.26096,0.332126,1.56468,-2.1325,0.391635,1.53414,-2.11699,0.200964,1.41163,-1.8174,0.012813,1.47154,-1.88665,0.019426,1.49753,-1.94896,0.023454,1.46757,-2.00147,0.209937,1.39863,-2.07298,0.273099,1.1294,-1.7545 [...]
+/*1893*/{0.200731,1.88212,-1.72162,0.296835,1.83135,-1.85581,0.054293,1.77276,-1.65526,-0.056526,1.68467,-1.62568,0.086,1.61036,-1.64172,0.162181,1.55943,-1.63578,0.261825,1.90156,-2.10516,0.310473,1.83513,-1.92864,0.114842,1.86929,-2.0473,0.125918,1.69308,-2.22674,0.179354,1.5982,-2.26083,0.326329,1.55513,-2.1347,0.385468,1.52251,-2.12105,0.201684,1.40833,-1.81624,0.012281,1.46639,-1.88632,0.017875,1.49301,-1.94928,0.021893,1.46194,-2.00232,0.208964,1.3943,-2.0719,0.271481,1.12456,-1.75 [...]
+/*1894*/{0.20185,1.87656,-1.72024,0.296853,1.82436,-1.85565,0.057109,1.76348,-1.65256,-0.051494,1.67493,-1.61979,0.091126,1.6023,-1.63847,0.168327,1.55361,-1.63379,0.260051,1.89649,-2.10585,0.310462,1.83053,-1.92948,0.113984,1.86401,-2.04705,0.12111,1.68889,-2.22577,0.172407,1.59386,-2.26059,0.32043,1.54459,-2.13767,0.379074,1.51096,-2.12501,0.20047,1.40485,-1.81529,0.011356,1.4614,-1.88679,0.018095,1.48741,-1.948,0.021022,1.4561,-2.00255,0.208749,1.39066,-2.07189,0.268062,1.12017,-1.754 [...]
+/*1895*/{0.20218,1.87073,-1.71956,0.297727,1.81927,-1.85573,0.061092,1.75498,-1.64828,-0.047151,1.66514,-1.61364,0.09781,1.59501,-1.63419,0.17389,1.54727,-1.63218,0.258616,1.89123,-2.10705,0.310354,1.82551,-1.93015,0.112382,1.85909,-2.04636,0.116486,1.6833,-2.2248,0.165058,1.58899,-2.26028,0.314597,1.53571,-2.14064,0.372743,1.49963,-2.12899,0.201046,1.40165,-1.81367,0.009731,1.45652,-1.88746,0.016376,1.48155,-1.94794,0.020393,1.45073,-2.00299,0.208341,1.38618,-2.07099,0.266246,1.11304,-1 [...]
+/*1896*/{0.20271,1.86511,-1.71884,0.297165,1.81339,-1.85632,0.064492,1.74612,-1.64432,-0.042248,1.65539,-1.60846,0.103352,1.58668,-1.63061,0.181753,1.54164,-1.62998,0.256961,1.88671,-2.10848,0.310594,1.81994,-1.93039,0.111049,1.85423,-2.04597,0.111284,1.67947,-2.22429,0.15713,1.5843,-2.26064,0.308563,1.52529,-2.14364,0.364145,1.4886,-2.13432,0.196658,1.39682,-1.81365,0.007418,1.45191,-1.88832,0.014148,1.47652,-1.94889,0.018152,1.44556,-2.00437,0.208236,1.3814,-2.07026,0.262774,1.10844,-1 [...]
+/*1897*/{0.204088,1.85994,-1.71741,0.298589,1.80859,-1.85691,0.069147,1.73662,-1.64101,-0.035955,1.64568,-1.60222,0.109999,1.57978,-1.62695,0.189036,1.53553,-1.62735,0.255216,1.88194,-2.10924,0.310914,1.81477,-1.93212,0.109757,1.84957,-2.04518,0.105582,1.67494,-2.22393,0.15011,1.57986,-2.26065,0.301982,1.51559,-2.14721,0.356137,1.47695,-2.13863,0.195229,1.39353,-1.81305,0.0052,1.44752,-1.88895,0.012532,1.47056,-1.94981,0.015687,1.43987,-2.00462,0.207971,1.37695,-2.06968,0.255503,1.105,-1 [...]
+/*1898*/{0.204887,1.85508,-1.71622,0.297901,1.80435,-1.85762,0.074356,1.72719,-1.63685,-0.029709,1.63533,-1.59619,0.117142,1.57286,-1.62445,0.195826,1.53016,-1.62594,0.2537,1.87797,-2.11112,0.312002,1.81015,-1.93303,0.108856,1.84648,-2.04372,0.101002,1.67149,-2.22368,0.142001,1.57524,-2.26039,0.29397,1.50525,-2.1505,0.34851,1.46592,-2.14321,0.19259,1.39029,-1.8133,0.002092,1.44328,-1.88943,0.009365,1.46583,-1.95068,0.013424,1.43518,-2.00594,0.206362,1.37231,-2.06853,0.253214,1.10229,-1.7 [...]
+/*1899*/{0.206254,1.85098,-1.71518,0.298829,1.80045,-1.85838,0.079896,1.7186,-1.63369,-0.020441,1.62587,-1.59057,0.125642,1.56557,-1.62025,0.204095,1.52519,-1.62398,0.253085,1.87472,-2.11271,0.31204,1.80542,-1.9348,0.107812,1.84428,-2.04152,0.095492,1.66769,-2.22352,0.13433,1.57149,-2.25984,0.287462,1.4963,-2.15416,0.340026,1.45473,-2.14781,0.189959,1.38788,-1.8133,-0.001675,1.44164,-1.8917,0.006223,1.46148,-1.95221,0.011631,1.43063,-2.00695,0.205219,1.36832,-2.06705,0.250745,1.10088,-1. [...]
+/*1900*/{0.207448,1.84727,-1.71378,0.299039,1.7947,-1.85945,0.086168,1.71054,-1.63111,-0.011285,1.61708,-1.58471,0.134047,1.55985,-1.61856,0.213142,1.52112,-1.62232,0.252805,1.87176,-2.115,0.313248,1.80151,-1.93684,0.106496,1.843,-2.03977,0.09226,1.66454,-2.22281,0.127256,1.56776,-2.25813,0.27865,1.4865,-2.15795,0.331603,1.44484,-2.1526,0.187206,1.38625,-1.81142,-0.006537,1.43675,-1.89389,0.003704,1.45793,-1.95308,0.007014,1.42467,-2.00799,0.204411,1.36414,-2.06474,0.244663,1.1005,-1.736 [...]
+/*1901*/{0.20876,1.84351,-1.7137,0.300225,1.79339,-1.86195,0.093032,1.70372,-1.62913,-0.002056,1.60939,-1.57922,0.143774,1.55574,-1.61644,0.222391,1.51766,-1.6211,0.251291,1.86894,-2.11573,0.312563,1.79816,-1.93844,0.106305,1.84275,-2.03881,0.088231,1.66293,-2.22117,0.120295,1.56605,-2.25544,0.270369,1.47889,-2.16124,0.322109,1.43585,-2.1575,0.183473,1.38465,-1.80972,-0.008092,1.43572,-1.89684,0.002208,1.45698,-1.95381,0.005514,1.42045,-2.01041,0.203992,1.36097,-2.06384,0.239893,1.10036, [...]
+/*1902*/{0.210033,1.84074,-1.71346,0.299505,1.78986,-1.86234,0.098967,1.69922,-1.62768,0.008385,1.60335,-1.57518,0.152898,1.55175,-1.61468,0.233083,1.51544,-1.61969,0.251495,1.86718,-2.11625,0.312727,1.79519,-1.94003,0.106229,1.84285,-2.03872,0.08343,1.66314,-2.2189,0.113266,1.56664,-2.25343,0.262559,1.47418,-2.16334,0.31247,1.42815,-2.1619,0.181327,1.38397,-1.80769,-0.009141,1.43559,-1.89656,0.000801,1.45646,-1.95489,0.00296,1.41806,-2.01157,0.201365,1.35837,-2.06332,0.239095,1.10117,-1 [...]
+/*1903*/{0.211422,1.83864,-1.71429,0.299828,1.78784,-1.86347,0.104277,1.69615,-1.62665,0.019592,1.59924,-1.57153,0.162947,1.54899,-1.61332,0.24339,1.51391,-1.61903,0.251678,1.86556,-2.1161,0.312639,1.79379,-1.94044,0.106217,1.84287,-2.0387,0.079094,1.66396,-2.21635,0.105525,1.5683,-2.25163,0.252847,1.46966,-2.16495,0.302148,1.4213,-2.16654,0.178909,1.38373,-1.80573,-0.011599,1.43543,-1.89773,-0.00088,1.45672,-1.95507,0.00116,1.41764,-2.01294,0.200047,1.35605,-2.06268,0.234126,1.10162,-1. [...]
+/*1904*/{0.212674,1.83721,-1.71505,0.298942,1.78794,-1.86436,0.10969,1.69359,-1.62585,0.027765,1.5956,-1.56744,0.173744,1.54798,-1.61288,0.254182,1.51414,-1.61843,0.251804,1.86418,-2.11621,0.311128,1.7916,-1.94133,0.106135,1.84289,-2.03886,0.072946,1.66667,-2.21297,0.098455,1.57101,-2.24921,0.24389,1.46709,-2.16643,0.291652,1.41678,-2.17085,0.176458,1.38362,-1.80449,-0.01243,1.43511,-1.89858,-0.001709,1.4566,-1.95516,0.000732,1.41819,-2.01286,0.197805,1.35508,-2.06259,0.230465,1.09902,-1 [...]
+/*1905*/{0.214901,1.83637,-1.7159,0.299169,1.786,-1.86524,0.115804,1.69227,-1.62478,0.038794,1.5937,-1.56406,0.183954,1.54807,-1.6123,0.265351,1.51582,-1.61841,0.251363,1.86318,-2.11625,0.310983,1.79118,-1.94151,0.106134,1.84302,-2.03902,0.068002,1.67072,-2.20948,0.089875,1.57399,-2.24773,0.235651,1.4657,-2.16941,0.281298,1.41404,-2.17522,0.173911,1.38359,-1.80325,-0.013127,1.43556,-1.89787,-0.00334,1.45705,-1.95528,-0.001395,1.41959,-2.01315,0.197326,1.35479,-2.06198,0.222781,1.09608,-1 [...]
+/*1906*/{0.215948,1.83597,-1.71665,0.297713,1.78553,-1.86613,0.120741,1.69137,-1.6241,0.047591,1.59149,-1.56136,0.193821,1.54874,-1.61199,0.276285,1.51794,-1.61884,0.250957,1.86302,-2.11653,0.310403,1.79061,-1.94223,0.105918,1.84347,-2.03954,0.062722,1.6751,-2.20582,0.081283,1.57754,-2.24626,0.226429,1.46559,-2.17243,0.270638,1.4125,-2.17851,0.172987,1.38383,-1.80236,-0.014206,1.43623,-1.89805,-0.005211,1.45702,-1.95504,-0.001662,1.41991,-2.01246,0.194756,1.35527,-2.06229,0.217142,1.0941 [...]
+/*1907*/{0.217423,1.83686,-1.71757,0.298304,1.78387,-1.86607,0.126555,1.69147,-1.62275,0.05705,1.59079,-1.55827,0.204095,1.55086,-1.61105,0.287101,1.52144,-1.6198,0.250619,1.86333,-2.11721,0.310182,1.7904,-1.94265,0.105421,1.84431,-2.03966,0.056208,1.68106,-2.20213,0.072257,1.58228,-2.24558,0.218013,1.46686,-2.17502,0.259997,1.41298,-2.1823,0.170169,1.38467,-1.80002,-0.015731,1.4361,-1.89828,-0.006063,1.45821,-1.95468,-0.002777,1.42148,-2.01154,0.193341,1.35662,-2.0623,0.209734,1.09087,- [...]
+/*1908*/{0.21924,1.83821,-1.71759,0.298282,1.78456,-1.86785,0.131006,1.6928,-1.6213,0.066225,1.59013,-1.55516,0.213743,1.55312,-1.60972,0.29821,1.52608,-1.62111,0.248919,1.86391,-2.11755,0.308623,1.7914,-1.94342,0.105293,1.84611,-2.03971,0.050187,1.68745,-2.19918,0.063492,1.5881,-2.2449,0.209478,1.46944,-2.17855,0.250026,1.41445,-2.18493,0.168268,1.38511,-1.79983,-0.017284,1.43605,-1.8984,-0.006054,1.45841,-1.95446,-0.00436,1.4226,-2.01111,0.192138,1.35887,-2.06308,0.206072,1.08647,-1.73 [...]
+/*1909*/{0.221188,1.84012,-1.71837,0.298435,1.78586,-1.86858,0.135175,1.69467,-1.62038,0.074656,1.5897,-1.55299,0.223071,1.55714,-1.60995,0.308806,1.53161,-1.62301,0.248436,1.86558,-2.11812,0.307412,1.79176,-1.94496,0.104718,1.84828,-2.03993,0.045082,1.69422,-2.19595,0.055073,1.59401,-2.24429,0.200089,1.47245,-2.18078,0.239524,1.41806,-2.1875,0.16741,1.3861,-1.79956,-0.016052,1.43929,-1.89807,-0.007552,1.45913,-1.95405,-0.005315,1.42377,-2.01042,0.191992,1.36124,-2.06307,0.198305,1.08524 [...]
+/*1910*/{0.222989,1.84284,-1.71928,0.297559,1.78846,-1.86995,0.139537,1.69783,-1.61966,0.084826,1.59068,-1.55063,0.232408,1.56125,-1.60993,0.318301,1.53754,-1.62492,0.247036,1.86776,-2.11847,0.30747,1.79318,-1.94602,0.103314,1.8506,-2.0392,0.038976,1.70116,-2.19367,0.046354,1.60111,-2.24405,0.190635,1.47704,-2.18289,0.229854,1.42242,-2.18929,0.166978,1.38725,-1.79889,-0.01689,1.44063,-1.89845,-0.007716,1.4608,-1.95463,-0.006065,1.42881,-2.01046,0.190288,1.3635,-2.06373,0.191183,1.08375,- [...]
+/*1911*/{0.225365,1.84593,-1.71996,0.299613,1.79025,-1.87067,0.144083,1.70071,-1.61901,0.092767,1.59176,-1.54866,0.240753,1.56571,-1.61029,0.327691,1.54494,-1.62727,0.244435,1.86957,-2.11949,0.306682,1.7951,-1.94735,0.102427,1.85291,-2.03878,0.032216,1.70791,-2.19218,0.037812,1.60878,-2.24356,0.182564,1.48242,-2.1842,0.220353,1.42772,-2.19044,0.166035,1.38893,-1.79876,-0.018649,1.44164,-1.89916,-0.008184,1.46294,-1.95388,-0.006636,1.42894,-2.01026,0.189859,1.3663,-2.06483,0.182381,1.0829 [...]
+/*1912*/{0.228121,1.84986,-1.72074,0.298305,1.79252,-1.87286,0.148402,1.70389,-1.6186,0.100389,1.59363,-1.54822,0.249125,1.57198,-1.61153,0.335083,1.55276,-1.63031,0.242358,1.87294,-2.11982,0.306107,1.79773,-1.94875,0.100848,1.85676,-2.03692,0.02558,1.71492,-2.19101,0.029769,1.61633,-2.24278,0.173578,1.48893,-2.1848,0.210821,1.43424,-2.19145,0.165755,1.38969,-1.79843,-0.018821,1.44316,-1.89864,-0.008678,1.46587,-1.95383,-0.007364,1.43018,-2.00994,0.189324,1.36919,-2.06523,0.177841,1.0839 [...]
+/*1913*/{0.230724,1.85373,-1.72138,0.300076,1.79593,-1.87331,0.153511,1.70722,-1.61891,0.109208,1.59678,-1.54617,0.256486,1.57798,-1.61345,0.343015,1.561,-1.6331,0.239479,1.87642,-2.12074,0.305305,1.80032,-1.95019,0.098788,1.8593,-2.03614,0.019508,1.72237,-2.19019,0.021939,1.62404,-2.24226,0.165633,1.49623,-2.18506,0.202382,1.44177,-2.19214,0.165309,1.39158,-1.79918,-0.018194,1.44638,-1.89884,-0.008481,1.46731,-1.95303,-0.007808,1.43234,-2.00977,0.188448,1.37259,-2.06555,0.172434,1.08384 [...]
+/*1914*/{0.233114,1.858,-1.72232,0.299233,1.79879,-1.87508,0.157964,1.71089,-1.61906,0.117003,1.59965,-1.54521,0.263728,1.58555,-1.61488,0.349483,1.57006,-1.63631,0.236319,1.88039,-2.12071,0.304136,1.80377,-1.95183,0.097162,1.86206,-2.03359,0.012812,1.72946,-2.18944,0.015329,1.63143,-2.24278,0.15726,1.50464,-2.18457,0.194152,1.44981,-2.19182,0.166163,1.39257,-1.79854,-0.017501,1.44892,-1.89822,-0.008287,1.47041,-1.95241,-0.007175,1.43502,-2.01001,0.188298,1.37655,-2.06557,0.158457,1.0798 [...]
+/*1915*/{0.23612,1.86234,-1.7233,0.299794,1.8032,-1.87686,0.163153,1.71443,-1.61935,0.124074,1.60288,-1.54475,0.269212,1.59206,-1.61722,0.356137,1.57911,-1.64044,0.232047,1.88494,-2.12142,0.303908,1.80762,-1.95401,0.095688,1.86525,-2.03248,0.007591,1.7375,-2.18845,0.007491,1.6389,-2.2418,0.150324,1.51305,-2.18467,0.187381,1.45815,-2.19225,0.167995,1.39473,-1.79911,-0.01698,1.45143,-1.89749,-0.007925,1.47371,-1.95161,-0.007269,1.43906,-2.00975,0.188644,1.38101,-2.06596,0.149849,1.08083,-1 [...]
+/*1916*/{0.238834,1.86707,-1.7247,0.300254,1.80718,-1.87916,0.171372,1.71859,-1.61853,0.130929,1.60714,-1.5447,0.275795,1.59953,-1.62008,0.362341,1.58873,-1.64424,0.228107,1.88996,-2.12177,0.303474,1.81134,-1.95569,0.093749,1.86896,-2.03021,0.001724,1.74499,-2.18749,0.001346,1.64691,-2.24137,0.142922,1.52192,-2.1835,0.179443,1.46712,-2.19207,0.167973,1.39613,-1.79866,-0.015722,1.45499,-1.89633,-0.007026,1.47694,-1.95061,-0.006312,1.44264,-2.00915,0.189275,1.38538,-2.06541,0.142881,1.0808 [...]
+/*1917*/{0.241162,1.87173,-1.72562,0.30046,1.81232,-1.88105,0.177504,1.72258,-1.61885,0.139194,1.61156,-1.54407,0.280832,1.60719,-1.62204,0.366384,1.59795,-1.64797,0.224621,1.89505,-2.1219,0.303121,1.81637,-1.95755,0.091791,1.8723,-2.02834,-0.004061,1.75277,-2.18655,-0.006186,1.6547,-2.24028,0.136402,1.53071,-2.18285,0.173013,1.47622,-2.1922,0.168943,1.39744,-1.7979,-0.015267,1.45825,-1.89576,-0.006254,1.48012,-1.94925,-0.005297,1.44604,-2.00892,0.189901,1.39011,-2.06495,0.136247,1.08097 [...]
+/*1918*/{0.24353,1.87758,-1.72743,0.300528,1.81726,-1.88316,0.184397,1.7266,-1.61955,0.146317,1.61613,-1.54439,0.285979,1.61519,-1.62433,0.37136,1.60751,-1.65208,0.220196,1.90071,-2.12252,0.302577,1.82074,-1.96013,0.089734,1.8759,-2.02617,-0.008281,1.76101,-2.18605,-0.011582,1.66202,-2.23934,0.129705,1.54021,-2.18206,0.166847,1.48535,-2.19202,0.169828,1.39861,-1.79727,-0.013741,1.46148,-1.89544,-0.004726,1.48381,-1.94729,-0.005075,1.45113,-2.00829,0.190393,1.39541,-2.06478,0.130278,1.083 [...]
+/*1919*/{0.246136,1.88396,-1.72891,0.301365,1.82178,-1.8851,0.189435,1.73162,-1.61992,0.151572,1.62053,-1.5442,0.290727,1.62288,-1.62652,0.375538,1.61733,-1.65644,0.215803,1.90633,-2.12248,0.302684,1.82625,-1.96189,0.087928,1.87943,-2.02362,-0.013524,1.76818,-2.18486,-0.018238,1.66978,-2.2384,0.123438,1.54949,-2.18105,0.160854,1.49465,-2.19134,0.171016,1.40043,-1.79671,-0.011572,1.46445,-1.89385,-0.00447,1.48765,-1.94621,-0.003515,1.45561,-2.00801,0.190768,1.40025,-2.06393,0.122317,1.084 [...]
+/*1920*/{0.2468,1.88868,-1.73072,0.302162,1.82734,-1.88702,0.196046,1.73642,-1.62101,0.157255,1.62556,-1.54434,0.294358,1.63014,-1.62819,0.378876,1.62688,-1.66018,0.212829,1.91194,-2.1225,0.301886,1.83078,-1.96398,0.086352,1.88331,-2.02208,-0.017189,1.7754,-2.18417,-0.023698,1.67762,-2.23748,0.117907,1.55866,-2.17998,0.155323,1.50408,-2.19152,0.173313,1.40199,-1.79671,-0.010981,1.46866,-1.8925,-0.00318,1.49145,-1.94446,-0.00255,1.46059,-2.00768,0.192267,1.40587,-2.0637,0.116125,1.08563,- [...]
+/*1921*/{0.249309,1.89405,-1.73226,0.303336,1.83266,-1.88919,0.200808,1.74191,-1.62209,0.163445,1.63008,-1.54372,0.297555,1.63806,-1.63093,0.381377,1.63623,-1.66387,0.209075,1.9173,-2.12266,0.301151,1.83655,-1.9656,0.084638,1.88699,-2.0204,-0.022642,1.78301,-2.18383,-0.029171,1.68517,-2.23605,0.111839,1.56797,-2.17853,0.150098,1.51323,-2.19116,0.173911,1.40308,-1.79666,-0.009019,1.47263,-1.89247,-0.001596,1.49584,-1.94249,-0.000457,1.46624,-2.0068,0.192906,1.41109,-2.06288,0.109331,1.086 [...]
+/*1922*/{0.250926,1.9002,-1.73389,0.303682,1.83805,-1.89129,0.204748,1.74703,-1.62266,0.169606,1.63564,-1.54366,0.300378,1.64621,-1.63381,0.384004,1.64568,-1.66811,0.206053,1.92265,-2.12224,0.300625,1.84225,-1.96748,0.083388,1.89137,-2.01839,-0.026895,1.78946,-2.18183,-0.034048,1.69306,-2.23526,0.10663,1.57668,-2.17723,0.145307,1.52221,-2.19072,0.175625,1.40515,-1.797,-0.006805,1.47613,-1.89197,0.000151,1.50038,-1.94085,0.001039,1.47206,-2.00652,0.193588,1.41696,-2.06263,0.104263,1.09093 [...]
+/*1923*/{0.252727,1.90624,-1.73515,0.304387,1.84356,-1.89315,0.209709,1.75269,-1.62408,0.173753,1.64175,-1.54433,0.302956,1.65392,-1.63627,0.385898,1.65425,-1.67233,0.203373,1.92808,-2.12219,0.300024,1.84759,-1.96942,0.081972,1.89581,-2.01614,-0.031443,1.79676,-2.18142,-0.039234,1.70078,-2.23363,0.101984,1.58553,-2.17637,0.140518,1.53081,-2.19065,0.177545,1.40665,-1.79808,-0.004715,1.48002,-1.89061,0.001931,1.50464,-1.93848,0.001495,1.4774,-2.00622,0.194578,1.4226,-2.0618,0.097663,1.0935 [...]
+/*1924*/{0.254428,1.91149,-1.73682,0.304315,1.8482,-1.89504,0.213096,1.75835,-1.62468,0.177831,1.64667,-1.54431,0.305256,1.66185,-1.6383,0.387548,1.66327,-1.67625,0.200717,1.9329,-2.1217,0.299633,1.85292,-1.97126,0.081393,1.89958,-2.015,-0.034462,1.8036,-2.18055,-0.044232,1.70788,-2.23196,0.097016,1.59352,-2.17494,0.136253,1.53914,-2.19021,0.179932,1.40786,-1.799,-0.002571,1.48424,-1.89,0.003327,1.509,-1.93722,0.003362,1.48376,-2.00622,0.195337,1.42834,-2.06198,0.093229,1.09629,-1.76628, [...]
+/*1925*/{0.254806,1.9174,-1.7385,0.305631,1.85381,-1.8969,0.216497,1.76419,-1.62545,0.182495,1.65186,-1.54415,0.307294,1.6694,-1.64139,0.388484,1.6712,-1.67906,0.198382,1.93796,-2.12121,0.298455,1.85852,-1.97287,0.08012,1.90405,-2.01404,-0.037124,1.80959,-2.17939,-0.048373,1.71567,-2.22994,0.09279,1.60023,-2.17378,0.132379,1.54638,-2.18973,0.181642,1.4097,-1.80071,-0.00031,1.48891,-1.88861,0.004904,1.51394,-1.93695,0.005366,1.49006,-2.00614,0.19618,1.43377,-2.06195,0.089059,1.10108,-1.76 [...]
+/*1926*/{0.256038,1.92258,-1.73966,0.304823,1.85877,-1.89876,0.219834,1.76933,-1.6265,0.184656,1.65769,-1.5447,0.308338,1.67577,-1.6426,0.389429,1.67965,-1.68314,0.196273,1.94271,-2.12117,0.297715,1.86315,-1.9745,0.078852,1.90757,-2.01314,-0.04033,1.81608,-2.17829,-0.052802,1.72247,-2.2287,0.089113,1.60724,-2.17278,0.128616,1.55397,-2.1898,0.183719,1.4115,-1.80264,0.001452,1.49345,-1.88793,0.006472,1.51809,-1.93542,0.007255,1.49494,-2.00516,0.197024,1.43933,-2.06259,0.084293,1.10439,-1.7 [...]
+/*1927*/{0.257955,1.92718,-1.74099,0.304962,1.86398,-1.90063,0.222199,1.77448,-1.62686,0.188846,1.66219,-1.54415,0.310514,1.68255,-1.64517,0.390338,1.6884,-1.68693,0.194057,1.94706,-2.12143,0.297911,1.86835,-1.97626,0.07831,1.91213,-2.01141,-0.043617,1.82166,-2.17681,-0.057101,1.72919,-2.22605,0.085487,1.61445,-2.17229,0.125078,1.56074,-2.18955,0.186036,1.41363,-1.80427,0.004152,1.49745,-1.88781,0.007815,1.52208,-1.9346,0.009407,1.50119,-2.0053,0.198074,1.44472,-2.06291,0.080032,1.1072,- [...]
+/*1928*/{0.258589,1.93229,-1.74282,0.30533,1.86873,-1.90221,0.224765,1.7801,-1.62756,0.192427,1.66752,-1.5442,0.311265,1.68895,-1.64724,0.391193,1.6953,-1.69084,0.192396,1.95074,-2.12098,0.297497,1.87309,-1.97765,0.077765,1.91541,-2.01102,-0.045317,1.82737,-2.17355,-0.061312,1.73614,-2.2246,0.081505,1.62068,-2.17163,0.121803,1.56725,-2.18964,0.187603,1.41594,-1.80606,0.00586,1.50178,-1.88738,0.009316,1.52683,-1.93388,0.011125,1.50697,-2.00492,0.197991,1.44992,-2.06346,0.075613,1.11081,-1 [...]
+/*1929*/{0.2598,1.93677,-1.74412,0.305522,1.87345,-1.90335,0.225807,1.78516,-1.62824,0.195128,1.6726,-1.54437,0.311928,1.695,-1.64901,0.391559,1.70202,-1.69313,0.191015,1.95439,-2.12088,0.297206,1.87791,-1.97844,0.078427,1.91773,-2.01009,-0.046302,1.8321,-2.17265,-0.064361,1.74227,-2.22257,0.07858,1.62665,-2.17109,0.118647,1.57328,-2.18971,0.190039,1.41806,-1.8084,0.007262,1.5064,-1.88722,0.010727,1.53104,-1.93335,0.012696,1.51149,-2.00394,0.198311,1.45435,-2.06394,0.073786,1.11551,-1.76 [...]
+/*1930*/{0.260152,1.94074,-1.74521,0.305449,1.87824,-1.90431,0.227078,1.78957,-1.62869,0.19662,1.67673,-1.54495,0.312069,1.70063,-1.65098,0.391426,1.70835,-1.69669,0.189565,1.95842,-2.12076,0.296895,1.88243,-1.97958,0.078731,1.92105,-2.00928,-0.049241,1.83868,-2.16928,-0.067991,1.7478,-2.22064,0.076389,1.63221,-2.17068,0.115253,1.57867,-2.18998,0.191258,1.42094,-1.80934,0.009467,1.51076,-1.8866,0.012055,1.53544,-1.93276,0.013919,1.51732,-2.00359,0.198059,1.4589,-2.06428,0.068849,1.1184,- [...]
+/*1931*/{0.261597,1.94497,-1.74648,0.305452,1.88187,-1.90588,0.228174,1.79442,-1.62916,0.199498,1.68149,-1.54454,0.312742,1.7063,-1.65243,0.391469,1.71487,-1.69952,0.189083,1.96168,-2.12056,0.296535,1.88582,-1.98026,0.078439,1.9239,-2.00868,-0.050818,1.84424,-2.1672,-0.071593,1.75341,-2.21806,0.07428,1.6375,-2.17069,0.112632,1.58351,-2.19026,0.193421,1.42397,-1.81039,0.011198,1.51494,-1.8867,0.013398,1.53919,-1.93246,0.015418,1.52155,-2.00308,0.199069,1.4633,-2.0651,0.066252,1.12252,-1.7 [...]
+/*1932*/{0.261836,1.9482,-1.74732,0.306652,1.88617,-1.90645,0.229309,1.79856,-1.62981,0.20147,1.68486,-1.54415,0.313256,1.71089,-1.65486,0.391376,1.72012,-1.70206,0.188379,1.96478,-2.12112,0.296486,1.88941,-1.98129,0.078066,1.92654,-2.0075,-0.051929,1.84945,-2.16391,-0.074165,1.75841,-2.21575,0.072565,1.64221,-2.17041,0.109586,1.58817,-2.19094,0.194496,1.42717,-1.81145,0.012507,1.51948,-1.88576,0.015005,1.54373,-1.93216,0.015686,1.526,-2.00265,0.199582,1.46727,-2.06544,0.062345,1.12833,- [...]
+/*1933*/{0.263054,1.95218,-1.74839,0.305902,1.88903,-1.90733,0.229896,1.80269,-1.62986,0.202101,1.68844,-1.54399,0.313014,1.71538,-1.65627,0.390574,1.72488,-1.70525,0.187326,1.96754,-2.12103,0.296639,1.89293,-1.98235,0.078408,1.92884,-2.00795,-0.052964,1.85398,-2.16126,-0.076752,1.7633,-2.21389,0.069562,1.64553,-2.16997,0.10761,1.59269,-2.1915,0.195757,1.43016,-1.81173,0.013119,1.52313,-1.88596,0.015745,1.54825,-1.93166,0.017486,1.53068,-2.00215,0.199497,1.47048,-2.06587,0.060144,1.13145 [...]
+/*1934*/{0.263174,1.95491,-1.74961,0.30663,1.89372,-1.90836,0.230676,1.80634,-1.63089,0.203068,1.69211,-1.54405,0.31325,1.71952,-1.65762,0.389959,1.72934,-1.70737,0.187143,1.97069,-2.12149,0.296814,1.8961,-1.98323,0.07884,1.93127,-2.00728,-0.052836,1.85736,-2.15826,-0.079323,1.76759,-2.21144,0.067646,1.64935,-2.17031,0.105724,1.59612,-2.19223,0.195993,1.43371,-1.81208,0.014211,1.52696,-1.88551,0.016184,1.55271,-1.93152,0.017925,1.53489,-2.00153,0.200264,1.47322,-2.06591,0.058578,1.13809, [...]
+/*1935*/{0.263567,1.95782,-1.75078,0.306865,1.896,-1.90982,0.230876,1.81009,-1.63104,0.203541,1.69472,-1.54359,0.312983,1.72298,-1.6594,0.388606,1.73298,-1.70951,0.186296,1.97294,-2.12146,0.297069,1.89918,-1.98393,0.079647,1.93404,-2.00716,-0.053493,1.86089,-2.15681,-0.081194,1.77216,-2.20968,0.066118,1.65261,-2.17099,0.103612,1.59938,-2.19313,0.19684,1.43733,-1.81225,0.014997,1.53015,-1.88514,0.016958,1.55605,-1.93134,0.018758,1.53836,-2.00151,0.20074,1.4757,-2.06602,0.058091,1.14016,-1 [...]
+/*1936*/{0.263757,1.96076,-1.75173,0.306678,1.89912,-1.91021,0.231298,1.81352,-1.63153,0.203809,1.6979,-1.5442,0.312486,1.72582,-1.66108,0.387857,1.73614,-1.71158,0.187365,1.97548,-2.12185,0.297074,1.9016,-1.98506,0.07997,1.93634,-2.00702,-0.054326,1.86452,-2.15372,-0.08303,1.77579,-2.20748,0.064949,1.65595,-2.17114,0.10178,1.60235,-2.1943,0.196707,1.44092,-1.81211,0.015396,1.53348,-1.88552,0.01729,1.55962,-1.93135,0.0184,1.54189,-2.00133,0.201259,1.47838,-2.06617,0.057594,1.14393,-1.757 [...]
+/*1937*/{0.264541,1.96255,-1.75224,0.30673,1.90172,-1.91129,0.231182,1.81641,-1.632,0.204035,1.69968,-1.54341,0.312601,1.72773,-1.66171,0.387235,1.73798,-1.71404,0.187292,1.97733,-2.12282,0.297465,1.90303,-1.98567,0.081172,1.93832,-2.00722,-0.054111,1.86777,-2.15101,-0.084203,1.77894,-2.20601,0.063823,1.65906,-2.17192,0.100258,1.60519,-2.19511,0.197315,1.44449,-1.81163,0.016222,1.53635,-1.88509,0.017091,1.56288,-1.93169,0.01896,1.54452,-2.00115,0.201218,1.4801,-2.06586,0.059054,1.14679,- [...]
+/*1938*/{0.264624,1.96515,-1.753,0.307008,1.90371,-1.91194,0.231559,1.81852,-1.633,0.203258,1.70309,-1.54417,0.311734,1.72966,-1.66172,0.386447,1.73975,-1.71579,0.18713,1.97932,-2.12339,0.297781,1.90541,-1.98682,0.082167,1.94112,-2.00799,-0.054346,1.8705,-2.14837,-0.085144,1.78226,-2.20423,0.062902,1.66133,-2.17254,0.098475,1.60708,-2.19653,0.197865,1.44773,-1.81129,0.015646,1.53922,-1.88583,0.017099,1.56609,-1.93154,0.019348,1.5465,-2.00047,0.201997,1.48189,-2.06541,0.060946,1.14861,-1. [...]
+/*1939*/{0.264768,1.9669,-1.75396,0.306736,1.90567,-1.91308,0.231338,1.82088,-1.63318,0.203481,1.70516,-1.54306,0.310098,1.73147,-1.66245,0.384728,1.74095,-1.71699,0.188359,1.98084,-2.12453,0.298024,1.90622,-1.98723,0.082876,1.94242,-2.00808,-0.053894,1.87315,-2.14659,-0.086293,1.7844,-2.2026,0.06194,1.66235,-2.17329,0.097964,1.60844,-2.1972,0.197546,1.45104,-1.81104,0.016389,1.54137,-1.88577,0.017892,1.56846,-1.93189,0.018776,1.54869,-2.00039,0.201984,1.48318,-2.06448,0.064556,1.14964,- [...]
+/*1940*/{0.265021,1.96804,-1.7548,0.307462,1.90726,-1.91426,0.230826,1.82298,-1.63422,0.202403,1.70652,-1.54363,0.309364,1.73202,-1.66392,0.383061,1.74138,-1.71919,0.188429,1.98187,-2.12514,0.298207,1.90777,-1.98861,0.0835,1.94459,-2.00882,-0.054134,1.87427,-2.14468,-0.086442,1.78638,-2.2012,0.062157,1.66411,-2.17415,0.097035,1.61019,-2.19849,0.19783,1.45325,-1.81054,0.016265,1.54295,-1.88547,0.018033,1.57056,-1.93173,0.019365,1.55002,-2.00061,0.202869,1.4852,-2.06336,0.068711,1.14981,-1 [...]
+/*1941*/{0.264924,1.96905,-1.75575,0.306384,1.90834,-1.91514,0.229802,1.82433,-1.63502,0.201164,1.70858,-1.54356,0.308177,1.73199,-1.66494,0.382006,1.74231,-1.72015,0.190039,1.98313,-2.12634,0.298812,1.90802,-1.98943,0.084355,1.94585,-2.00938,-0.052855,1.87605,-2.14327,-0.087438,1.78823,-2.19998,0.06208,1.66442,-2.17484,0.09617,1.61065,-2.19949,0.197274,1.45549,-1.80972,0.015765,1.54429,-1.88635,0.017829,1.57161,-1.9321,0.01958,1.55061,-2.00054,0.20473,1.48627,-2.06251,0.073496,1.14896,- [...]
+/*1942*/{0.264816,1.96983,-1.75664,0.30599,1.90877,-1.91571,0.228939,1.82549,-1.63592,0.200536,1.71012,-1.54389,0.307434,1.73193,-1.66622,0.38054,1.73985,-1.72142,0.191431,1.98364,-2.12746,0.298723,1.90806,-1.98995,0.085458,1.94717,-2.01118,-0.052404,1.87679,-2.14128,-0.087033,1.78946,-2.19921,0.061398,1.66512,-2.17544,0.096234,1.61072,-2.20058,0.196599,1.45723,-1.80813,0.015697,1.54482,-1.88644,0.017875,1.5726,-1.93242,0.019393,1.55011,-2.00053,0.20545,1.48701,-2.06112,0.07963,1.14796,- [...]
+/*1943*/{0.265105,1.97022,-1.75737,0.306557,1.90957,-1.91667,0.228101,1.82646,-1.63674,0.197175,1.71077,-1.54458,0.3054,1.73148,-1.66662,0.378656,1.73832,-1.72209,0.19208,1.98403,-2.12793,0.298525,1.90834,-1.99077,0.086509,1.94833,-2.01223,-0.052607,1.87791,-2.14055,-0.087186,1.7904,-2.19844,0.062097,1.66486,-2.17576,0.095779,1.61047,-2.20174,0.196848,1.4589,-1.80827,0.015369,1.54491,-1.88683,0.016678,1.57323,-1.93309,0.021438,1.54885,-2.00081,0.206313,1.48774,-2.05993,0.086187,1.14782,- [...]
+/*1944*/{0.265026,1.9703,-1.75871,0.30554,1.90977,-1.91764,0.226198,1.82692,-1.63771,0.19607,1.71138,-1.54463,0.304196,1.73002,-1.66716,0.37747,1.73583,-1.72314,0.192509,1.98342,-2.12873,0.29841,1.90807,-1.99126,0.087102,1.94832,-2.0136,-0.050763,1.87725,-2.13843,-0.086472,1.7902,-2.19804,0.062707,1.66423,-2.17625,0.095806,1.60923,-2.20315,0.196459,1.45937,-1.80747,0.015297,1.54473,-1.88669,0.016609,1.57303,-1.93328,0.019181,1.54766,-2.00035,0.206692,1.48804,-2.05818,0.092211,1.14421,-1. [...]
+/*1945*/{0.264435,1.97036,-1.75918,0.305364,1.90918,-1.91839,0.223867,1.82761,-1.63857,0.192665,1.71265,-1.54604,0.301766,1.72851,-1.66762,0.375148,1.73291,-1.72362,0.193714,1.98302,-2.12971,0.29823,1.90777,-1.9918,0.0887,1.94888,-2.01473,-0.050025,1.87661,-2.13657,-0.085451,1.78996,-2.19789,0.062909,1.66274,-2.17731,0.096485,1.60826,-2.20419,0.196262,1.45978,-1.80679,0.014793,1.54427,-1.88715,0.016585,1.57177,-1.93373,0.02036,1.54581,-2.00159,0.208053,1.48828,-2.05695,0.097952,1.14271,- [...]
+/*1946*/{0.263628,1.96953,-1.76055,0.304899,1.90901,-1.91853,0.221645,1.82755,-1.63948,0.189539,1.71247,-1.54665,0.300217,1.72721,-1.66845,0.373017,1.72977,-1.72395,0.194786,1.9826,-2.13042,0.298465,1.90646,-1.99194,0.088785,1.94846,-2.01572,-0.048907,1.87618,-2.13683,-0.084349,1.7893,-2.19862,0.063857,1.66071,-2.17799,0.096808,1.60594,-2.20574,0.195977,1.46035,-1.8055,0.015036,1.54275,-1.88757,0.015715,1.57002,-1.93422,0.021239,1.54503,-2.00282,0.20926,1.48808,-2.05557,0.104198,1.14062, [...]
+/*1947*/{0.262883,1.96873,-1.76151,0.304582,1.90712,-1.91892,0.219759,1.82728,-1.64075,0.186291,1.71215,-1.548,0.297193,1.72429,-1.66908,0.370555,1.72598,-1.72457,0.195297,1.98135,-2.13096,0.297846,1.90544,-1.9924,0.088516,1.9475,-2.01773,-0.048154,1.87574,-2.13626,-0.083074,1.78765,-2.19888,0.064817,1.65849,-2.17865,0.097467,1.60364,-2.20728,0.196025,1.46011,-1.80542,0.014606,1.5413,-1.88818,0.016164,1.56861,-1.93414,0.021002,1.54273,-2.00294,0.209979,1.48754,-2.05446,0.111965,1.13819,- [...]
+/*1948*/{0.262478,1.96766,-1.76203,0.304223,1.90642,-1.91893,0.2175,1.827,-1.64102,0.182361,1.71273,-1.54938,0.29462,1.72182,-1.66929,0.368457,1.72168,-1.72449,0.196027,1.98008,-2.13161,0.297641,1.90421,-1.99237,0.088943,1.94717,-2.01906,-0.046475,1.87382,-2.13487,-0.081213,1.78514,-2.19983,0.066176,1.65592,-2.18107,0.098223,1.6005,-2.20829,0.196079,1.45921,-1.80468,0.014109,1.53901,-1.8884,0.016414,1.56632,-1.93528,0.021591,1.54069,-2.00369,0.210708,1.48635,-2.05415,0.117075,1.13649,-1. [...]
+/*1949*/{0.260807,1.9658,-1.76266,0.301989,1.90391,-1.91855,0.215035,1.82603,-1.64178,0.178634,1.71169,-1.54986,0.291028,1.71807,-1.66947,0.365172,1.71677,-1.72394,0.196244,1.97829,-2.13208,0.297437,1.90204,-1.99256,0.08876,1.94563,-2.02099,-0.045098,1.87106,-2.13515,-0.079981,1.78257,-2.2016,0.066372,1.65299,-2.18183,0.100124,1.59694,-2.21035,0.196443,1.45857,-1.80469,0.014071,1.53698,-1.88894,0.016249,1.56396,-1.93521,0.022288,1.5386,-2.00482,0.211337,1.48414,-2.05416,0.123087,1.13346, [...]
+/*1950*/{0.260036,1.96448,-1.76311,0.302105,1.90194,-1.91968,0.212159,1.82494,-1.64234,0.175841,1.71103,-1.55073,0.287993,1.71494,-1.66969,0.362674,1.71143,-1.72342,0.196185,1.9759,-2.13269,0.296657,1.9005,-1.99271,0.089668,1.94392,-2.0223,-0.04361,1.86766,-2.13611,-0.078119,1.77903,-2.20294,0.067125,1.64859,-2.18381,0.101224,1.59351,-2.21192,0.196357,1.45716,-1.80498,0.013997,1.53455,-1.88898,0.015819,1.56202,-1.9362,0.021549,1.53616,-2.00539,0.211037,1.4815,-2.05448,0.127891,1.13112,-1 [...]
+/*1951*/{0.258723,1.96209,-1.76401,0.301558,1.90004,-1.91956,0.208665,1.82372,-1.64275,0.169443,1.7097,-1.55178,0.284035,1.71014,-1.6697,0.359203,1.70549,-1.7232,0.197181,1.97392,-2.13334,0.295984,1.89792,-1.993,0.088215,1.94246,-2.02413,-0.042793,1.86475,-2.13742,-0.076185,1.7751,-2.20545,0.068875,1.64458,-2.18568,0.10256,1.58918,-2.2138,0.196457,1.45574,-1.80477,0.013746,1.53243,-1.88969,0.01593,1.55934,-1.93719,0.022238,1.53306,-2.006,0.211109,1.47936,-2.05514,0.135257,1.13022,-1.7810 [...]
+/*1952*/{0.257755,1.96009,-1.76402,0.300467,1.89753,-1.92011,0.205214,1.82188,-1.6431,0.1652,1.70788,-1.55235,0.279981,1.70629,-1.66915,0.35585,1.69964,-1.72271,0.196962,1.97118,-2.1339,0.295565,1.89537,-1.99291,0.088016,1.93998,-2.02596,-0.043222,1.86105,-2.14003,-0.074754,1.77075,-2.20758,0.070667,1.64006,-2.18839,0.104588,1.58467,-2.21571,0.196407,1.45413,-1.8049,0.013858,1.52987,-1.8895,0.016332,1.55637,-1.93762,0.022931,1.53018,-2.0071,0.210626,1.47673,-2.05598,0.137451,1.12646,-1.7 [...]
+/*1953*/{0.256246,1.95723,-1.76422,0.299576,1.89469,-1.91973,0.201579,1.81979,-1.64318,0.158951,1.70604,-1.55398,0.27535,1.70108,-1.6693,0.352329,1.69267,-1.7214,0.196952,1.96836,-2.13467,0.294621,1.89278,-1.99319,0.086866,1.93775,-2.02678,-0.043329,1.85906,-2.14267,-0.073019,1.76576,-2.21033,0.072627,1.63503,-2.19098,0.10654,1.5791,-2.21774,0.196219,1.452,-1.80517,0.013753,1.52691,-1.89007,0.0165,1.55397,-1.93849,0.022488,1.52726,-2.00738,0.211555,1.47424,-2.05651,0.144638,1.12377,-1.78 [...]
+/*1954*/{0.254491,1.95444,-1.76438,0.299574,1.89201,-1.92003,0.198857,1.81687,-1.64343,0.155174,1.70384,-1.55425,0.271314,1.6964,-1.66951,0.347939,1.68524,-1.72016,0.196561,1.96495,-2.13496,0.294304,1.89003,-1.99329,0.086985,1.93459,-2.02875,-0.042888,1.85366,-2.14563,-0.071433,1.76002,-2.21331,0.074392,1.6293,-2.19372,0.109137,1.5735,-2.21969,0.196497,1.44997,-1.80555,0.014205,1.52267,-1.88986,0.016404,1.55074,-1.93893,0.022264,1.52356,-2.00754,0.211684,1.47159,-2.05752,0.150589,1.12126 [...]
+/*1955*/{0.253125,1.95152,-1.76431,0.298814,1.88881,-1.91983,0.195284,1.81414,-1.6437,0.149273,1.70086,-1.55488,0.265938,1.69106,-1.66862,0.344098,1.67812,-1.71895,0.19594,1.96119,-2.13492,0.293855,1.88696,-1.99357,0.084802,1.93126,-2.03033,-0.042363,1.84735,-2.14826,-0.069663,1.75396,-2.21731,0.077383,1.62364,-2.19655,0.11142,1.56835,-2.22185,0.196163,1.44729,-1.80586,0.013495,1.51981,-1.8911,0.016114,1.5472,-1.93896,0.021389,1.51993,-2.00758,0.211272,1.46886,-2.05851,0.154822,1.11921,- [...]
+/*1956*/{0.2514,1.94741,-1.76367,0.298452,1.88493,-1.91977,0.192043,1.81105,-1.64349,0.143708,1.69905,-1.55688,0.260853,1.6852,-1.66812,0.339386,1.66938,-1.71684,0.195172,1.95703,-2.13569,0.29285,1.88384,-1.99328,0.084584,1.92735,-2.03204,-0.042664,1.84087,-2.15134,-0.068038,1.74739,-2.22061,0.078365,1.61748,-2.19952,0.113856,1.56169,-2.22395,0.197722,1.44466,-1.80631,0.014177,1.5156,-1.89004,0.016432,1.54323,-1.9392,0.020979,1.51591,-2.00749,0.210552,1.46613,-2.0598,0.159533,1.11589,-1. [...]
+/*1957*/{0.24961,1.94349,-1.76366,0.297691,1.88141,-1.91941,0.187516,1.80733,-1.64393,0.13717,1.69538,-1.5575,0.256203,1.67926,-1.66785,0.334652,1.66175,-1.71503,0.194502,1.9528,-2.13616,0.291791,1.87963,-1.99297,0.084121,1.92341,-2.03281,-0.042893,1.83378,-2.15572,-0.065685,1.74029,-2.22434,0.08148,1.61032,-2.20275,0.117238,1.55526,-2.226,0.198047,1.44177,-1.80646,0.014138,1.51119,-1.89032,0.015207,1.53924,-1.93954,0.020127,1.51138,-2.00712,0.20996,1.46364,-2.06093,0.161946,1.11416,-1.7 [...]
+/*1958*/{0.247949,1.93926,-1.76288,0.296716,1.87704,-1.91904,0.183701,1.80348,-1.64383,0.130984,1.69246,-1.55845,0.250713,1.67277,-1.66681,0.32884,1.65286,-1.71306,0.19406,1.94799,-2.13691,0.291916,1.87579,-1.993,0.082494,1.91844,-2.03559,-0.041906,1.82589,-2.15937,-0.06356,1.73279,-2.22828,0.084015,1.60347,-2.20579,0.120969,1.54875,-2.22802,0.198177,1.43873,-1.80745,0.01358,1.50661,-1.8907,0.016065,1.53518,-1.93966,0.019392,1.50646,-2.0068,0.209732,1.46038,-2.06214,0.166366,1.11201,-1.7 [...]
+/*1959*/{0.246732,1.93487,-1.76249,0.296679,1.8728,-1.91857,0.179163,1.79973,-1.64385,0.12499,1.6889,-1.55958,0.244623,1.66646,-1.66668,0.324062,1.64372,-1.71137,0.193806,1.94342,-2.1381,0.291331,1.8712,-1.99285,0.081837,1.91272,-2.03648,-0.04077,1.81458,-2.16203,-0.061245,1.72466,-2.23237,0.086888,1.59576,-2.20833,0.124627,1.54133,-2.22963,0.199189,1.43537,-1.8088,0.012876,1.50224,-1.89059,0.01447,1.52961,-1.9396,0.01789,1.50168,-2.00654,0.208718,1.45697,-2.064,0.169288,1.10865,-1.77314 [...]
+/*1960*/{0.244947,1.92996,-1.76191,0.296741,1.86748,-1.91813,0.174973,1.79551,-1.64418,0.11938,1.68681,-1.56095,0.239201,1.65937,-1.66542,0.318147,1.63464,-1.70853,0.193679,1.93813,-2.13928,0.290285,1.86711,-1.99322,0.080919,1.90722,-2.03923,-0.039783,1.80791,-2.16735,-0.058125,1.71653,-2.23658,0.090611,1.58822,-2.21154,0.128609,1.53377,-2.23143,0.19917,1.43222,-1.81036,0.01247,1.49677,-1.88984,0.015073,1.52504,-1.93908,0.016537,1.49589,-2.00514,0.20794,1.4539,-2.0656,0.172348,1.10597,-1 [...]
+/*1961*/{0.242574,1.9248,-1.76163,0.295339,1.86218,-1.91719,0.170385,1.7914,-1.64457,0.111096,1.68346,-1.56178,0.232941,1.65213,-1.66502,0.312129,1.62481,-1.70663,0.194518,1.93289,-2.14014,0.290345,1.86238,-1.99225,0.077847,1.902,-2.03903,-0.03947,1.79898,-2.17179,-0.055339,1.70794,-2.24059,0.096688,1.58091,-2.21409,0.133725,1.52672,-2.23343,0.19954,1.42882,-1.81116,0.012056,1.49189,-1.88967,0.012659,1.51975,-1.93877,0.015718,1.49114,-2.00459,0.2075,1.45079,-2.06726,0.175758,1.10264,-1.7 [...]
+/*1962*/{0.240369,1.91942,-1.76099,0.294314,1.85719,-1.91624,0.165846,1.78703,-1.64445,0.104261,1.68015,-1.56369,0.227015,1.64496,-1.66378,0.305981,1.61514,-1.70428,0.194011,1.92714,-2.14097,0.290095,1.8574,-1.99177,0.077991,1.89638,-2.04176,-0.037384,1.78926,-2.17637,-0.05202,1.69902,-2.24455,0.099962,1.57274,-2.21611,0.13842,1.51848,-2.23515,0.200457,1.42436,-1.81321,0.011698,1.48592,-1.88948,0.014194,1.51486,-1.93888,0.014088,1.48505,-2.00451,0.206287,1.44699,-2.06934,0.178261,1.09847 [...]
+/*1963*/{0.238628,1.91419,-1.76014,0.294549,1.85199,-1.91566,0.16121,1.78243,-1.64468,0.096952,1.67673,-1.56526,0.220734,1.63774,-1.66312,0.299327,1.6052,-1.70172,0.194273,1.92153,-2.14159,0.28983,1.85211,-1.99175,0.077048,1.88898,-2.04261,-0.036599,1.77977,-2.18106,-0.048144,1.68957,-2.24863,0.104911,1.56406,-2.21889,0.143514,1.51054,-2.23715,0.200495,1.42029,-1.81443,0.012024,1.48004,-1.88923,0.011763,1.50872,-1.93885,0.012779,1.47957,-2.00364,0.205901,1.44354,-2.07109,0.18224,1.09552, [...]
+/*1964*/{0.236359,1.90878,-1.75942,0.293554,1.84626,-1.91439,0.157094,1.77763,-1.64515,0.090676,1.67465,-1.56609,0.213543,1.62998,-1.66166,0.292561,1.59577,-1.69935,0.194655,1.91609,-2.14251,0.290343,1.84689,-1.99086,0.076099,1.88325,-2.04473,-0.033158,1.77032,-2.18625,-0.043924,1.68029,-2.25287,0.110275,1.55614,-2.22048,0.149089,1.50235,-2.23893,0.200459,1.41607,-1.81603,0.011794,1.4743,-1.88854,0.012681,1.50275,-1.94008,0.012127,1.4742,-2.00224,0.204969,1.43944,-2.073,0.184349,1.0913,- [...]
+/*1965*/{0.23459,1.90271,-1.7588,0.293413,1.84047,-1.91439,0.152357,1.77308,-1.64526,0.083167,1.67182,-1.5677,0.207642,1.62311,-1.66104,0.285002,1.58604,-1.69677,0.195638,1.91064,-2.14366,0.290178,1.84125,-1.99027,0.075607,1.87565,-2.04552,-0.032698,1.76087,-2.19153,-0.03873,1.67063,-2.25671,0.115557,1.54864,-2.22284,0.155094,1.49389,-2.24047,0.200888,1.41108,-1.81776,0.010337,1.46891,-1.88915,0.012485,1.49704,-1.9401,0.010111,1.46912,-2.00256,0.204185,1.43454,-2.07518,0.188045,1.08718,- [...]
+/*1966*/{0.232184,1.89674,-1.75793,0.292514,1.8349,-1.91285,0.147391,1.76774,-1.64566,0.075192,1.66909,-1.56953,0.200262,1.61633,-1.65938,0.277455,1.57692,-1.69374,0.196709,1.90461,-2.14424,0.290334,1.83602,-1.98927,0.074565,1.86927,-2.04741,-0.029086,1.75134,-2.19712,-0.034102,1.6606,-2.26045,0.121868,1.5403,-2.22506,0.161084,1.48612,-2.2422,0.201022,1.40554,-1.81752,0.009505,1.46298,-1.8882,0.011597,1.49036,-1.94059,0.004838,1.46805,-2.00384,0.204001,1.43025,-2.07707,0.192636,1.08298,- [...]
+/*1967*/{0.230487,1.89141,-1.75716,0.292605,1.82899,-1.91127,0.14259,1.76274,-1.646,0.068486,1.66612,-1.57075,0.192961,1.60867,-1.65843,0.269269,1.56815,-1.69099,0.197517,1.89958,-2.14509,0.290124,1.83008,-1.98917,0.074469,1.86299,-2.04807,-0.026116,1.74172,-2.20396,-0.027373,1.6509,-2.26398,0.127017,1.53217,-2.22665,0.167586,1.47798,-2.24402,0.200463,1.40081,-1.81986,0.008292,1.4579,-1.8879,0.009608,1.4842,-1.94015,0.003913,1.46373,-2.00399,0.201893,1.42525,-2.0783,0.198425,1.07869,-1.7 [...]
+/*1968*/{0.228839,1.88599,-1.75621,0.29204,1.82252,-1.91076,0.138093,1.75889,-1.64677,0.060719,1.66393,-1.57275,0.18562,1.60245,-1.65718,0.261045,1.56061,-1.68804,0.198099,1.89411,-2.14707,0.291037,1.82443,-1.9878,0.074604,1.85974,-2.04844,-0.020351,1.73182,-2.20913,-0.021061,1.64077,-2.26746,0.134312,1.52389,-2.2284,0.174613,1.46966,-2.24498,0.200324,1.39491,-1.82052,0.006926,1.45255,-1.88727,0.006936,1.47923,-1.93996,0.002383,1.45958,-2.00436,0.199653,1.42058,-2.0786,0.202627,1.07522,- [...]
+/*1969*/{0.226696,1.88079,-1.75558,0.291691,1.8174,-1.90911,0.132733,1.75426,-1.64754,0.052495,1.66141,-1.57459,0.178126,1.59685,-1.65572,0.252592,1.55281,-1.68521,0.198971,1.8889,-2.14796,0.29223,1.81937,-1.98765,0.075576,1.85624,-2.04841,-0.015111,1.72437,-2.21739,-0.013359,1.63028,-2.27077,0.141857,1.5159,-2.23069,0.182268,1.46139,-2.24608,0.200177,1.39005,-1.82262,0.005351,1.44777,-1.8875,0.004641,1.47374,-1.93832,0.002739,1.45283,-2.00264,0.196819,1.41757,-2.07992,0.20891,1.07127,-1 [...]
+/*1970*/{0.225254,1.87627,-1.75424,0.291668,1.81084,-1.90761,0.128202,1.75068,-1.6478,0.04536,1.65981,-1.57626,0.171086,1.59137,-1.65542,0.244512,1.5461,-1.68269,0.200349,1.88444,-2.1493,0.29219,1.81366,-1.98659,0.076293,1.85312,-2.04851,-0.00867,1.71443,-2.22437,-0.004305,1.62041,-2.27461,0.149789,1.50849,-2.23224,0.190794,1.45464,-2.24698,0.199115,1.38478,-1.82478,0.002013,1.44339,-1.88737,0.001717,1.46955,-1.93732,-0.000577,1.44944,-2.00292,0.194351,1.41417,-2.0811,0.217571,1.06938,-1 [...]
+/*1971*/{0.223862,1.87186,-1.75302,0.291773,1.80653,-1.90676,0.123673,1.74776,-1.64869,0.037383,1.65862,-1.5784,0.163331,1.58698,-1.6549,0.235455,1.54045,-1.67987,0.201157,1.87957,-2.15086,0.2921,1.8088,-1.98528,0.076679,1.85116,-2.04876,-0.000569,1.70514,-2.23078,0.004315,1.61067,-2.27713,0.158616,1.50149,-2.23346,0.19978,1.44864,-2.24682,0.197532,1.3805,-1.82748,0.000643,1.43956,-1.88734,0.000487,1.46719,-1.93772,-0.003214,1.44687,-2.00212,0.192162,1.41177,-2.08231,0.222844,1.06837,-1. [...]
+/*1972*/{0.222519,1.86808,-1.75134,0.291199,1.80264,-1.90572,0.117911,1.7456,-1.64984,0.02915,1.65838,-1.58044,0.155508,1.58372,-1.65452,0.227556,1.53574,-1.67757,0.203705,1.87539,-2.15086,0.29255,1.80399,-1.98437,0.077131,1.84863,-2.04919,0.006694,1.69677,-2.2358,0.013759,1.6013,-2.27902,0.16686,1.49524,-2.2337,0.209175,1.44319,-2.24655,0.19487,1.37546,-1.82726,-0.002008,1.43634,-1.88602,-0.001448,1.46673,-1.93734,-0.005351,1.44509,-2.00102,0.189899,1.40971,-2.08342,0.229623,1.07088,-1. [...]
+/*1973*/{0.221818,1.86446,-1.75016,0.290714,1.79876,-1.90409,0.112277,1.74437,-1.65039,0.021349,1.65903,-1.58348,0.147435,1.58176,-1.6542,0.217695,1.53071,-1.67562,0.20594,1.87206,-2.1506,0.292716,1.80095,-1.98322,0.077051,1.84684,-2.04928,0.013279,1.68829,-2.24015,0.023567,1.59306,-2.28085,0.177557,1.49018,-2.2334,0.220555,1.43988,-2.24528,0.193511,1.37119,-1.82899,-0.00309,1.4335,-1.88497,-0.003035,1.46557,-1.93709,-0.007045,1.44377,-1.99922,0.187365,1.40846,-2.08522,0.237486,1.07362,- [...]
+/*1974*/{0.221105,1.86196,-1.7493,0.289363,1.79586,-1.90321,0.107606,1.74419,-1.65141,0.012969,1.66104,-1.58723,0.140201,1.58009,-1.65362,0.210117,1.52781,-1.67376,0.207039,1.86953,-2.15014,0.29234,1.79838,-1.98191,0.078065,1.84567,-2.0498,0.020429,1.68134,-2.24275,0.033398,1.58599,-2.28202,0.187534,1.48694,-2.23273,0.232225,1.43748,-2.24364,0.192547,1.36828,-1.83058,-0.005359,1.43236,-1.88355,-0.004443,1.46451,-1.93693,-0.008844,1.44269,-1.99696,0.185106,1.4078,-2.08681,0.24353,1.07854, [...]
+/*1975*/{0.219961,1.85956,-1.7487,0.289657,1.79483,-1.90212,0.101996,1.74488,-1.65293,0.005294,1.66411,-1.59014,0.131587,1.57946,-1.65291,0.202059,1.52591,-1.67196,0.209495,1.8668,-2.1487,0.292372,1.79641,-1.98114,0.078232,1.84392,-2.05029,0.027964,1.67624,-2.24399,0.042073,1.58077,-2.28279,0.197581,1.4847,-2.23079,0.243846,1.43666,-2.24126,0.190012,1.36553,-1.83041,-0.006265,1.43098,-1.88243,-0.005577,1.46376,-1.93675,-0.010437,1.44199,-1.995,0.183613,1.40735,-2.08853,0.250361,1.0841,-1 [...]
+/*1976*/{0.219018,1.85839,-1.74819,0.287869,1.79333,-1.90107,0.09661,1.74677,-1.65401,-0.001141,1.6674,-1.59404,0.123752,1.57951,-1.65316,0.192009,1.5248,-1.67106,0.211474,1.86557,-2.14809,0.292171,1.79464,-1.97927,0.078731,1.84307,-2.05107,0.034532,1.67157,-2.24461,0.050164,1.57629,-2.28306,0.208585,1.48385,-2.22855,0.255657,1.43729,-2.23852,0.188403,1.36363,-1.83068,-0.0073,1.43049,-1.88259,-0.007558,1.4634,-1.93632,-0.011669,1.44244,-1.99433,0.181728,1.40696,-2.08974,0.259072,1.09009, [...]
+/*1977*/{0.218436,1.85779,-1.74751,0.287249,1.79208,-1.89964,0.091468,1.74854,-1.65512,-0.008879,1.6715,-1.59756,0.115865,1.58048,-1.65331,0.184599,1.52487,-1.66995,0.213696,1.8641,-2.14673,0.292312,1.79388,-1.97854,0.079608,1.8428,-2.05117,0.040703,1.66835,-2.24453,0.058696,1.57308,-2.28288,0.21874,1.48326,-2.22556,0.267652,1.43963,-2.23424,0.187053,1.36298,-1.82951,-0.009343,1.43103,-1.88215,-0.00849,1.46313,-1.93635,-0.01202,1.44179,-1.99412,0.180536,1.40644,-2.09097,0.265823,1.09606, [...]
+/*1978*/{0.216513,1.85804,-1.74697,0.286245,1.79101,-1.89908,0.086454,1.75096,-1.65628,-0.016142,1.67687,-1.60137,0.108393,1.5826,-1.65321,0.176146,1.52592,-1.66858,0.216096,1.86363,-2.14615,0.292036,1.79335,-1.97695,0.080774,1.843,-2.05196,0.047781,1.66586,-2.24378,0.067671,1.57055,-2.28295,0.229937,1.48622,-2.22214,0.280009,1.44314,-2.23001,0.185124,1.36322,-1.82832,-0.009388,1.43102,-1.8821,-0.008529,1.46304,-1.93677,-0.012636,1.44242,-1.99449,0.18024,1.40646,-2.09167,0.275737,1.10168 [...]
+/*1979*/{0.214259,1.85883,-1.74632,0.2857,1.79069,-1.89707,0.08195,1.75422,-1.65816,-0.023056,1.6803,-1.60424,0.101041,1.58548,-1.65351,0.16849,1.52738,-1.66721,0.21906,1.86392,-2.14494,0.292249,1.79327,-1.97614,0.0811,1.84396,-2.05238,0.052749,1.66338,-2.24281,0.076014,1.56847,-2.28231,0.239999,1.4888,-2.21777,0.291772,1.4482,-2.22487,0.182984,1.36399,-1.82632,-0.010278,1.43133,-1.88302,-0.009615,1.46331,-1.93723,-0.013865,1.44298,-1.99522,0.179617,1.40627,-2.09232,0.282103,1.1088,-1.76 [...]
+/*1980*/{0.212283,1.86053,-1.74526,0.284556,1.79217,-1.8961,0.076159,1.75776,-1.65898,-0.029621,1.68586,-1.60771,0.094519,1.58867,-1.65375,0.160175,1.53053,-1.66716,0.221286,1.86503,-2.14396,0.291227,1.79362,-1.97444,0.082529,1.84481,-2.05396,0.057196,1.66198,-2.24211,0.084818,1.56702,-2.28172,0.249856,1.49315,-2.21372,0.303169,1.45464,-2.21941,0.182238,1.36579,-1.8257,-0.010484,1.43234,-1.88426,-0.009928,1.46353,-1.9386,-0.013789,1.44296,-1.99572,0.180015,1.40624,-2.09282,0.288392,1.117 [...]
+/*1981*/{0.209321,1.86269,-1.74497,0.284888,1.79298,-1.89461,0.071841,1.76178,-1.66045,-0.036285,1.69099,-1.61084,0.086723,1.59274,-1.655,0.152721,1.53358,-1.66687,0.223552,1.86643,-2.14245,0.29155,1.79495,-1.97263,0.083299,1.84575,-2.05444,0.061845,1.66013,-2.24045,0.093316,1.56697,-2.28128,0.259169,1.49854,-2.20897,0.314372,1.4621,-2.21368,0.181118,1.36801,-1.82507,-0.010678,1.43379,-1.88592,-0.010012,1.46418,-1.93942,-0.013879,1.44422,-1.99729,0.179325,1.40586,-2.09346,0.292648,1.1254 [...]
+/*1982*/{0.206452,1.86515,-1.74438,0.282687,1.79391,-1.89234,0.066383,1.76623,-1.66128,-0.043648,1.69653,-1.61388,0.081085,1.59792,-1.65667,0.145835,1.53831,-1.66749,0.225031,1.86866,-2.14102,0.291204,1.79601,-1.97112,0.084323,1.84658,-2.05513,0.066937,1.6589,-2.23915,0.102339,1.56739,-2.27979,0.268992,1.50531,-2.20395,0.325491,1.47049,-2.20798,0.179525,1.36983,-1.82341,-0.011046,1.43433,-1.8866,-0.010374,1.46538,-1.94024,-0.013319,1.44452,-1.99837,0.179139,1.4056,-2.09368,0.299753,1.132 [...]
+/*1983*/{0.203458,1.86777,-1.74414,0.280989,1.79721,-1.89044,0.061908,1.77025,-1.66295,-0.050443,1.70101,-1.61697,0.073822,1.60364,-1.65827,0.139306,1.5441,-1.66807,0.22669,1.87185,-2.13964,0.291117,1.79845,-1.96877,0.084501,1.84852,-2.05545,0.070571,1.65848,-2.23744,0.110127,1.56782,-2.27897,0.277971,1.5123,-2.19825,0.33609,1.48,-2.20169,0.178807,1.37258,-1.82361,-0.01156,1.43607,-1.88811,-0.009882,1.46656,-1.94222,-0.013224,1.44591,-2.00086,0.17878,1.4055,-2.09422,0.304235,1.14002,-1.7 [...]
+/*1984*/{0.200501,1.87108,-1.74421,0.282112,1.8005,-1.88859,0.057301,1.77495,-1.66419,-0.056401,1.70742,-1.61952,0.067929,1.60959,-1.65969,0.132777,1.54947,-1.66895,0.227938,1.87499,-2.13863,0.291725,1.80108,-1.96756,0.085088,1.84945,-2.05651,0.075695,1.65883,-2.23489,0.118241,1.56923,-2.27746,0.287311,1.52123,-2.19386,0.345602,1.48987,-2.19506,0.178383,1.37561,-1.82314,-0.011489,1.43767,-1.88986,-0.009376,1.4683,-1.9435,-0.013186,1.44802,-2.00217,0.179694,1.40507,-2.09452,0.309554,1.147 [...]
+/*1985*/{0.197527,1.87451,-1.74324,0.280504,1.80323,-1.88724,0.051969,1.7798,-1.66557,-0.06271,1.71323,-1.62311,0.060729,1.61543,-1.66215,0.12654,1.55552,-1.67008,0.229515,1.87897,-2.1373,0.290972,1.80409,-1.96545,0.08565,1.85163,-2.05705,0.080089,1.65967,-2.23276,0.125858,1.5711,-2.27653,0.294963,1.52864,-2.18807,0.355471,1.50061,-2.18851,0.179198,1.37887,-1.82355,-0.011148,1.43983,-1.89078,-0.00864,1.47029,-1.9448,-0.01194,1.44971,-2.00345,0.180119,1.40517,-2.09501,0.312309,1.15535,-1. [...]
+/*1986*/{0.194325,1.87817,-1.74322,0.280214,1.80535,-1.88509,0.047904,1.7851,-1.66731,-0.068525,1.71872,-1.62579,0.056263,1.62213,-1.66407,0.119737,1.56138,-1.67044,0.230124,1.88288,-2.13556,0.290589,1.80738,-1.96282,0.086952,1.85306,-2.0578,0.082999,1.66105,-2.23058,0.133707,1.57346,-2.27525,0.303422,1.53859,-2.18322,0.364695,1.51106,-2.18127,0.179098,1.38186,-1.82309,-0.011148,1.44256,-1.89325,-0.008555,1.47286,-1.94587,-0.010438,1.45258,-2.00575,0.18133,1.40571,-2.09509,0.316667,1.162 [...]
+/*1987*/{0.19141,1.88181,-1.74304,0.279321,1.80915,-1.88371,0.043026,1.79035,-1.66835,-0.073453,1.72513,-1.62911,0.051057,1.6283,-1.66625,0.115787,1.56819,-1.67188,0.231379,1.88741,-2.13478,0.290596,1.81122,-1.9617,0.086817,1.85556,-2.05842,0.087585,1.66272,-2.22836,0.141459,1.57604,-2.27407,0.310344,1.54658,-2.17765,0.372827,1.52191,-2.17447,0.179341,1.38581,-1.82406,-0.01096,1.44574,-1.89431,-0.007234,1.47457,-1.94745,-0.009559,1.45413,-2.00724,0.181384,1.4058,-2.09603,0.31938,1.17038, [...]
+/*1988*/{0.188033,1.88571,-1.74299,0.278102,1.81463,-1.88221,0.039401,1.79537,-1.66925,-0.079179,1.73155,-1.6317,0.046006,1.63571,-1.66815,0.109757,1.57492,-1.67357,0.231466,1.89201,-2.13376,0.290794,1.81483,-1.95964,0.087738,1.85802,-2.05842,0.09261,1.66465,-2.22649,0.149309,1.5795,-2.2729,0.316738,1.55587,-2.1724,0.379944,1.53403,-2.16748,0.181618,1.39008,-1.82481,-0.009157,1.44812,-1.89633,-0.005827,1.47756,-1.94912,-0.007597,1.45699,-2.00886,0.183503,1.40605,-2.09434,0.321942,1.17699 [...]
+/*1989*/{0.185294,1.88962,-1.7425,0.277907,1.81727,-1.88026,0.035461,1.80085,-1.67059,-0.082664,1.73775,-1.63507,0.04077,1.64186,-1.67039,0.10579,1.58132,-1.67513,0.23269,1.89756,-2.13272,0.291648,1.81833,-1.95806,0.088467,1.86058,-2.05852,0.097474,1.66678,-2.2257,0.156628,1.58285,-2.27127,0.323167,1.56611,-2.16722,0.386681,1.54591,-2.16083,0.183052,1.39422,-1.82579,-0.007868,1.45168,-1.89728,-0.004587,1.48019,-1.95104,-0.00607,1.46012,-2.00936,0.185088,1.40637,-2.09599,0.325885,1.18453, [...]
+/*1990*/{0.182029,1.89344,-1.74322,0.277447,1.8216,-1.87863,0.032191,1.80601,-1.67189,-0.088528,1.74385,-1.63817,0.036994,1.64859,-1.67266,0.100695,1.58789,-1.67582,0.233639,1.90251,-2.13114,0.291119,1.82308,-1.95607,0.08851,1.86362,-2.05908,0.102287,1.67095,-2.22474,0.163612,1.58709,-2.27049,0.328593,1.57621,-2.16153,0.392706,1.55806,-2.15403,0.183861,1.39838,-1.82754,-0.007385,1.4553,-1.8982,-0.002784,1.48323,-1.95233,-0.004115,1.4624,-2.01076,0.185321,1.40724,-2.09688,0.327898,1.19145 [...]
+/*1991*/{0.179461,1.89779,-1.74324,0.275999,1.82755,-1.87774,0.028124,1.81175,-1.67351,-0.092795,1.75031,-1.64091,0.032916,1.65482,-1.6747,0.096396,1.59433,-1.67741,0.234318,1.90799,-2.12979,0.291617,1.82752,-1.9549,0.089131,1.86728,-2.05972,0.107235,1.67381,-2.22481,0.171085,1.59141,-2.26922,0.334918,1.58696,-2.15638,0.397021,1.57069,-2.14731,0.186364,1.40212,-1.82918,-0.005434,1.45942,-1.89901,-0.000739,1.48645,-1.95384,-0.002098,1.46599,-2.01183,0.185767,1.40736,-2.0973,0.330388,1.198 [...]
+/*1992*/{0.177698,1.90187,-1.74316,0.275899,1.83182,-1.87689,0.025316,1.81723,-1.67538,-0.097257,1.75625,-1.64352,0.028374,1.66097,-1.67669,0.092676,1.60059,-1.67885,0.234995,1.91329,-2.12809,0.291222,1.83174,-1.95303,0.089969,1.87056,-2.05985,0.1126,1.6777,-2.22521,0.178265,1.59642,-2.26835,0.338391,1.59635,-2.15137,0.401883,1.58137,-2.13961,0.187974,1.40631,-1.83037,-0.003803,1.46338,-1.90038,0.000321,1.48935,-1.9552,0.000109,1.46897,-2.01224,0.18732,1.40762,-2.09786,0.332122,1.20445,- [...]
+/*1993*/{0.1752,1.90553,-1.74355,0.274947,1.83474,-1.87479,0.022038,1.82226,-1.67714,-0.101862,1.76142,-1.64642,0.02491,1.66734,-1.67821,0.088491,1.60674,-1.68081,0.236573,1.91873,-2.12646,0.291464,1.83588,-1.95205,0.091119,1.87373,-2.06063,0.118645,1.68202,-2.22581,0.184814,1.60134,-2.26743,0.342713,1.60701,-2.1464,0.405736,1.59358,-2.13353,0.190466,1.41024,-1.83195,-0.002121,1.46748,-1.90088,0.002907,1.49304,-1.95619,0.002683,1.47198,-2.01303,0.188698,1.4078,-2.09822,0.334104,1.21052,- [...]
+/*1994*/{0.173543,1.90911,-1.74362,0.268249,1.84226,-1.87607,0.019304,1.82704,-1.67841,-0.104341,1.76757,-1.64918,0.021421,1.67338,-1.68026,0.085157,1.61266,-1.68195,0.23834,1.92384,-2.1255,0.290427,1.84094,-1.95111,0.092371,1.87714,-2.06135,0.123254,1.68701,-2.227,0.191657,1.60599,-2.26659,0.346003,1.61691,-2.1413,0.409068,1.6054,-2.12817,0.191553,1.41419,-1.83301,0.000222,1.47184,-1.90037,0.004375,1.49684,-1.95812,0.004534,1.47549,-2.01354,0.189663,1.40814,-2.0984,0.336221,1.2175,-1.76 [...]
+/*1995*/{0.171469,1.91266,-1.74419,0.267513,1.84664,-1.8751,0.016069,1.83239,-1.68009,-0.107714,1.77335,-1.65246,0.017889,1.67898,-1.68255,0.081282,1.61824,-1.68363,0.240187,1.92893,-2.1235,0.291205,1.84552,-1.94947,0.093725,1.88094,-2.06226,0.128284,1.69141,-2.22744,0.198444,1.61089,-2.26543,0.347423,1.62544,-2.13691,0.411096,1.61598,-2.12238,0.193666,1.41804,-1.8345,0.001547,1.4768,-1.90152,0.006913,1.49985,-1.95887,0.006686,1.47988,-2.01435,0.190318,1.40841,-2.09823,0.337478,1.22246,- [...]
+/*1996*/{0.170165,1.91618,-1.74376,0.273923,1.84756,-1.87242,0.014289,1.83734,-1.68208,-0.109737,1.77881,-1.65532,0.015137,1.6839,-1.68423,0.077924,1.6231,-1.68466,0.240104,1.93408,-2.12212,0.291651,1.84897,-1.94819,0.094371,1.88427,-2.06295,0.134337,1.6965,-2.22926,0.205113,1.61597,-2.26454,0.349797,1.63543,-2.13207,0.413434,1.62674,-2.11756,0.195101,1.42187,-1.8358,0.003942,1.48181,-1.90182,0.008491,1.50498,-1.96085,0.010147,1.4839,-2.01417,0.191576,1.40905,-2.0988,0.337762,1.22707,-1. [...]
+/*1997*/{0.168997,1.91919,-1.74449,0.267622,1.85453,-1.87282,0.01298,1.84142,-1.68341,-0.113817,1.78409,-1.65803,0.012773,1.68924,-1.68621,0.076165,1.62818,-1.68536,0.241741,1.93856,-2.12014,0.291007,1.85349,-1.94734,0.095019,1.88735,-2.06344,0.139245,1.70122,-2.23117,0.210539,1.62072,-2.26411,0.351231,1.6452,-2.12821,0.414809,1.63629,-2.1122,0.196517,1.42526,-1.83554,0.006537,1.48672,-1.90196,0.011157,1.50827,-1.96138,0.012893,1.48738,-2.01333,0.193436,1.40979,-2.09879,0.338943,1.23167, [...]
+/*1998*/{0.16804,1.92193,-1.74461,0.266725,1.85804,-1.87211,0.010632,1.84581,-1.68529,-0.115741,1.78901,-1.66132,0.010197,1.69377,-1.68757,0.073592,1.63229,-1.68661,0.242844,1.94263,-2.11904,0.291476,1.8572,-1.94582,0.096554,1.89084,-2.06389,0.144826,1.70535,-2.233,0.215537,1.62499,-2.26386,0.351651,1.65304,-2.12441,0.415982,1.64629,-2.10718,0.198404,1.42901,-1.83747,0.008625,1.49161,-1.90172,0.01384,1.51312,-1.96256,0.01366,1.49099,-2.01472,0.194243,1.41078,-2.09883,0.339984,1.23484,-1. [...]
+/*1999*/{0.166963,1.92474,-1.7445,0.266492,1.86143,-1.87146,0.009686,1.84995,-1.6867,-0.118708,1.79319,-1.66447,0.007317,1.69734,-1.68962,0.070958,1.63657,-1.68735,0.244927,1.94701,-2.11782,0.29164,1.86055,-1.94491,0.097616,1.8934,-2.06444,0.149302,1.70993,-2.23521,0.220992,1.63013,-2.2636,0.351893,1.65907,-2.12079,0.415957,1.65522,-2.10248,0.19917,1.43281,-1.83812,0.011116,1.49605,-1.901,0.016352,1.51651,-1.96249,0.016401,1.49438,-2.0149,0.196125,1.41142,-2.09819,0.341486,1.23812,-1.771 [...]
+/*2000*/{0.167037,1.92773,-1.74462,0.267776,1.86485,-1.87062,0.008003,1.85364,-1.68839,-0.120525,1.79744,-1.66735,0.004841,1.7009,-1.69121,0.068452,1.63951,-1.68793,0.246066,1.95019,-2.11596,0.291649,1.86405,-1.94419,0.099055,1.89562,-2.06589,0.153966,1.71413,-2.23769,0.225623,1.63392,-2.26383,0.353328,1.66629,-2.11786,0.416342,1.66355,-2.09827,0.201031,1.43592,-1.83941,0.013624,1.50103,-1.9016,0.017815,1.52078,-1.96412,0.018515,1.49785,-2.01542,0.196988,1.41237,-2.09807,0.341617,1.24077 [...]
+/*2001*/{0.166615,1.92947,-1.74429,0.267075,1.86773,-1.86998,0.00625,1.85702,-1.69019,-0.122645,1.80091,-1.6705,0.00393,1.70476,-1.69266,0.06671,1.6425,-1.68881,0.246723,1.95354,-2.11493,0.292136,1.86714,-1.9433,0.100094,1.89886,-2.06638,0.159285,1.71795,-2.23931,0.229465,1.63829,-2.26374,0.353943,1.67303,-2.11542,0.416349,1.67114,-2.09428,0.201935,1.43872,-1.83994,0.015989,1.50472,-1.90101,0.019348,1.52388,-1.96474,0.020291,1.50174,-2.01573,0.197898,1.41328,-2.09785,0.341259,1.24278,-1. [...]
+/*2002*/{0.166356,1.93228,-1.74434,0.267807,1.87036,-1.86867,0.005741,1.86032,-1.69123,-0.124545,1.80459,-1.67236,0.00201,1.70792,-1.69381,0.064735,1.64514,-1.68948,0.247593,1.95625,-2.1135,0.292627,1.86975,-1.9421,0.101603,1.9008,-2.06651,0.164894,1.7217,-2.24126,0.233183,1.64239,-2.26383,0.353999,1.67981,-2.11376,0.416281,1.67763,-2.09163,0.201813,1.44136,-1.84158,0.018128,1.50895,-1.90037,0.021662,1.52804,-1.96473,0.023245,1.5046,-2.01529,0.198123,1.41423,-2.09755,0.340203,1.24436,-1. [...]
+/*2003*/{0.166154,1.9343,-1.74415,0.26846,1.8729,-1.86754,0.005431,1.86296,-1.69223,-0.126388,1.80685,-1.67486,0.000506,1.71009,-1.69511,0.063491,1.64747,-1.68932,0.24827,1.9587,-2.11249,0.293237,1.87204,-1.94144,0.102207,1.90348,-2.06671,0.168741,1.72548,-2.24294,0.23691,1.6453,-2.26408,0.353905,1.68361,-2.11173,0.414914,1.68302,-2.08814,0.202447,1.44417,-1.8422,0.018074,1.51297,-1.90016,0.022283,1.53127,-1.96531,0.023973,1.50841,-2.01522,0.198697,1.41623,-2.09725,0.340271,1.24482,-1.77 [...]
+/*2004*/{0.166694,1.93632,-1.74428,0.269591,1.87478,-1.86758,0.004872,1.86555,-1.69294,-0.127783,1.8094,-1.67844,-0.000685,1.7123,-1.69581,0.062122,1.64926,-1.68963,0.249789,1.96116,-2.11153,0.293924,1.87453,-1.941,0.104259,1.90509,-2.06673,0.173757,1.72858,-2.24464,0.239771,1.64807,-2.2649,0.353975,1.68813,-2.11088,0.415162,1.68767,-2.08595,0.203077,1.44669,-1.84298,0.019516,1.51663,-1.90018,0.02391,1.53398,-1.96564,0.025353,1.5112,-2.01507,0.199004,1.41768,-2.09743,0.339029,1.24449,-1. [...]
+/*2005*/{0.166718,1.93778,-1.74478,0.270728,1.8769,-1.8672,0.004134,1.86733,-1.69551,-0.127893,1.81172,-1.68044,-0.001673,1.71377,-1.69738,0.061234,1.6501,-1.68936,0.249961,1.9627,-2.11036,0.294157,1.87627,-1.94032,0.105217,1.90768,-2.06695,0.177355,1.73151,-2.24547,0.243232,1.65055,-2.26545,0.354044,1.69151,-2.10879,0.414023,1.69157,-2.08389,0.203435,1.44872,-1.84382,0.020886,1.51971,-1.89982,0.024389,1.53691,-1.96575,0.026143,1.51448,-2.01468,0.199454,1.41989,-2.09729,0.338975,1.2429,- [...]
+/*2006*/{0.167295,1.93955,-1.74444,0.270575,1.87826,-1.86698,0.004537,1.86902,-1.6966,-0.127956,1.81355,-1.68268,-0.002905,1.71498,-1.69839,0.061003,1.65084,-1.68959,0.251314,1.96466,-2.10968,0.295262,1.87846,-1.93993,0.106946,1.90954,-2.0672,0.178529,1.73428,-2.24608,0.244765,1.65293,-2.26595,0.353959,1.69382,-2.10836,0.413535,1.69415,-2.08193,0.203702,1.45069,-1.84432,0.021619,1.52166,-1.89928,0.024856,1.53977,-1.96578,0.026761,1.51751,-2.01431,0.199063,1.42185,-2.09734,0.338985,1.2405 [...]
+/*2007*/{0.16795,1.94105,-1.74449,0.27164,1.87946,-1.86639,0.003764,1.87044,-1.69818,-0.129266,1.81445,-1.68473,-0.00271,1.71608,-1.69924,0.059517,1.65124,-1.68909,0.252255,1.966,-2.10926,0.296285,1.87961,-1.9389,0.107741,1.91102,-2.06707,0.180616,1.73692,-2.24637,0.246701,1.6545,-2.26705,0.352566,1.69707,-2.10823,0.413148,1.69574,-2.08091,0.20425,1.45307,-1.84454,0.020912,1.52514,-1.90003,0.02437,1.54318,-1.96522,0.027189,1.52002,-2.01407,0.199676,1.42431,-2.09749,0.336351,1.23762,-1.77 [...]
+/*2008*/{0.168463,1.9421,-1.74465,0.273196,1.88032,-1.86671,0.004572,1.87127,-1.69875,-0.130628,1.81508,-1.6867,-0.002943,1.71614,-1.69955,0.059291,1.65099,-1.68902,0.25408,1.96687,-2.1088,0.296899,1.88122,-1.93889,0.109347,1.91354,-2.06727,0.184108,1.73849,-2.24794,0.248662,1.65594,-2.26813,0.352541,1.69698,-2.10742,0.411974,1.69689,-2.07995,0.203665,1.45507,-1.84468,0.021535,1.52559,-1.90076,0.02484,1.54506,-1.96542,0.027402,1.52185,-2.0136,0.199955,1.42704,-2.09715,0.3334,1.23387,-1.7 [...]
+/*2009*/{0.169688,1.94359,-1.7439,0.272837,1.8816,-1.86639,0.005533,1.87217,-1.69886,-0.129874,1.81576,-1.68867,-0.002752,1.71585,-1.7,0.059109,1.65005,-1.68822,0.255043,1.96739,-2.10881,0.297541,1.88176,-1.93864,0.109749,1.91489,-2.06691,0.185537,1.73991,-2.24855,0.249228,1.65674,-2.26913,0.352326,1.69749,-2.10757,0.41134,1.69637,-2.07913,0.204051,1.45589,-1.84466,0.02249,1.5286,-1.89844,0.024126,1.54787,-1.96477,0.027485,1.52432,-2.0127,0.200236,1.42923,-2.09732,0.328701,1.23024,-1.781 [...]
+/*2010*/{0.170961,1.94382,-1.74383,0.279514,1.87801,-1.86418,0.005564,1.87251,-1.70001,-0.1295,1.81545,-1.68998,-0.002733,1.71512,-1.70065,0.059265,1.64937,-1.68778,0.256182,1.96762,-2.10833,0.298368,1.88136,-1.93814,0.110602,1.91684,-2.06716,0.187562,1.74103,-2.24923,0.249116,1.65699,-2.26934,0.35185,1.69671,-2.10736,0.410516,1.69554,-2.07807,0.204343,1.45732,-1.84556,0.022313,1.52954,-1.89805,0.024518,1.54969,-1.96396,0.027626,1.52592,-2.01292,0.200441,1.43159,-2.09748,0.325501,1.22453 [...]
+/*2011*/{0.171919,1.9444,-1.74349,0.28083,1.87836,-1.864,0.00625,1.87277,-1.70107,-0.128594,1.81482,-1.69175,-0.00215,1.71417,-1.70078,0.059319,1.64808,-1.68663,0.256568,1.96767,-2.10822,0.299148,1.88217,-1.93797,0.111798,1.91803,-2.06711,0.188315,1.74188,-2.25027,0.249611,1.65695,-2.27077,0.351835,1.69578,-2.10781,0.410392,1.69409,-2.07803,0.204437,1.45795,-1.84585,0.02238,1.5298,-1.89802,0.0239,1.55104,-1.96376,0.026926,1.52752,-2.01256,0.200755,1.4335,-2.09726,0.320374,1.21968,-1.7819 [...]
+/*2012*/{0.173042,1.94488,-1.74335,0.28033,1.87976,-1.86417,0.007919,1.8721,-1.70033,-0.127431,1.8141,-1.69252,-0.001195,1.71216,-1.7003,0.060357,1.64628,-1.6853,0.25773,1.96693,-2.10789,0.299147,1.88214,-1.93792,0.112033,1.91864,-2.06731,0.188689,1.74185,-2.25055,0.248723,1.65657,-2.27149,0.351377,1.69327,-2.10753,0.40919,1.69048,-2.07749,0.204854,1.45991,-1.84591,0.022334,1.52993,-1.89758,0.024214,1.55134,-1.9637,0.026903,1.52794,-2.01201,0.200982,1.43539,-2.09711,0.31683,1.21293,-1.78 [...]
+/*2013*/{0.174407,1.94473,-1.74316,0.28033,1.87976,-1.86417,0.008322,1.87163,-1.70139,-0.125267,1.81288,-1.69301,-3.5e-005,1.71151,-1.70011,0.061341,1.6437,-1.68455,0.259086,1.9662,-2.10784,0.299198,1.88194,-1.93797,0.112745,1.91948,-2.06724,0.188353,1.74197,-2.25063,0.246719,1.65528,-2.27203,0.350515,1.69021,-2.10689,0.408404,1.68694,-2.07765,0.204792,1.45998,-1.84589,0.022061,1.52961,-1.89702,0.024149,1.55139,-1.96324,0.026734,1.52787,-2.01134,0.201958,1.43616,-2.09711,0.311538,1.20703 [...]
+/*2014*/{0.176576,1.944,-1.74191,0.280563,1.87978,-1.86471,0.008877,1.8704,-1.70172,-0.124192,1.81096,-1.69386,0.001629,1.70909,-1.69981,0.061692,1.64109,-1.68275,0.259262,1.96527,-2.10772,0.29905,1.88144,-1.93811,0.113204,1.91983,-2.06673,0.187328,1.74124,-2.25059,0.244958,1.65419,-2.27225,0.350027,1.68675,-2.10711,0.407832,1.6821,-2.07729,0.205396,1.46084,-1.84674,0.020805,1.52946,-1.89696,0.023962,1.55147,-1.96296,0.026517,1.52759,-2.01133,0.203293,1.43728,-2.09702,0.307641,1.20202,-1 [...]
+/*2015*/{0.177509,1.94364,-1.74187,0.281549,1.87873,-1.86528,0.011031,1.86881,-1.70122,-0.122151,1.80891,-1.69403,0.002328,1.70608,-1.69846,0.062672,1.63817,-1.68077,0.259656,1.96372,-2.10759,0.298461,1.88075,-1.9383,0.113157,1.91988,-2.06659,0.18538,1.74055,-2.25039,0.241395,1.65255,-2.27204,0.348242,1.68326,-2.10736,0.406317,1.67683,-2.07719,0.205229,1.46104,-1.8468,0.021037,1.52797,-1.89663,0.023454,1.55082,-1.96246,0.026903,1.52688,-2.01092,0.203396,1.43791,-2.09678,0.303294,1.19448, [...]
+/*2016*/{0.178641,1.94219,-1.74163,0.281718,1.87833,-1.86534,0.011646,1.86753,-1.70074,-0.118896,1.80597,-1.69377,0.004389,1.7028,-1.69728,0.063928,1.63463,-1.67832,0.260304,1.96215,-2.10776,0.298532,1.87982,-1.9382,0.113008,1.91957,-2.06617,0.181663,1.73959,-2.24887,0.239346,1.65082,-2.27226,0.347041,1.67886,-2.10778,0.404983,1.67062,-2.07765,0.204947,1.46068,-1.84786,0.019734,1.52562,-1.89672,0.022635,1.54915,-1.96218,0.026893,1.52527,-2.01069,0.203575,1.43799,-2.0967,0.297466,1.18882, [...]
+/*2017*/{0.17983,1.94045,-1.74164,0.280508,1.87629,-1.86559,0.013174,1.86446,-1.70025,-0.117078,1.80235,-1.69319,0.005161,1.69901,-1.69609,0.065596,1.62985,-1.67654,0.258946,1.9594,-2.10728,0.298347,1.87834,-1.93832,0.112963,1.91758,-2.06637,0.177617,1.73833,-2.24806,0.235471,1.64866,-2.27188,0.346131,1.67269,-2.10836,0.404051,1.66304,-2.07871,0.204749,1.46056,-1.8481,0.019272,1.52411,-1.89638,0.022135,1.54802,-1.96184,0.026302,1.52326,-2.01055,0.204197,1.43723,-2.09645,0.294384,1.18165, [...]
+/*2018*/{0.18152,1.93838,-1.74071,0.281618,1.87535,-1.86566,0.014142,1.8616,-1.69996,-0.115471,1.79837,-1.69269,0.006899,1.69529,-1.69362,0.067042,1.62635,-1.674,0.259106,1.95699,-2.10743,0.298635,1.87684,-1.93863,0.112649,1.91764,-2.06514,0.173794,1.73621,-2.24676,0.232315,1.64644,-2.27081,0.345189,1.66603,-2.1091,0.403681,1.65639,-2.08024,0.20322,1.45911,-1.84771,0.018994,1.52133,-1.89693,0.022513,1.54526,-1.96177,0.026372,1.52134,-2.01043,0.204896,1.4364,-2.09577,0.28983,1.1756,-1.779 [...]
+/*2019*/{0.182125,1.93598,-1.74034,0.281912,1.87191,-1.86543,0.015474,1.85858,-1.69916,-0.113179,1.79383,-1.69071,0.009175,1.69007,-1.69123,0.069368,1.62196,-1.67164,0.258311,1.95422,-2.10741,0.297803,1.87485,-1.9386,0.112128,1.91539,-2.06419,0.169641,1.73419,-2.24594,0.227318,1.64351,-2.27032,0.343957,1.65924,-2.11022,0.402535,1.648,-2.08216,0.2053,1.4577,-1.84655,0.018652,1.51893,-1.89597,0.022205,1.54236,-1.96278,0.025527,1.51859,-2.01027,0.204799,1.43511,-2.09533,0.286349,1.16945,-1. [...]
+/*2020*/{0.183263,1.93281,-1.74028,0.280955,1.87046,-1.86619,0.017145,1.85402,-1.69767,-0.110391,1.78854,-1.6889,0.011324,1.68544,-1.689,0.070934,1.61707,-1.66914,0.257861,1.95111,-2.10797,0.297471,1.87186,-1.93875,0.11221,1.91335,-2.0636,0.163386,1.73233,-2.24438,0.22353,1.64061,-2.26985,0.341961,1.65128,-2.11154,0.400747,1.63912,-2.08413,0.203854,1.45501,-1.8465,0.019001,1.5144,-1.89562,0.021134,1.53889,-1.96145,0.025386,1.51548,-2.01083,0.205282,1.43359,-2.09477,0.283267,1.16418,-1.77 [...]
+/*2021*/{0.183543,1.92978,-1.73987,0.281129,1.86884,-1.86667,0.017813,1.84973,-1.69645,-0.108557,1.78266,-1.68653,0.013167,1.68036,-1.68599,0.074037,1.61193,-1.66625,0.256441,1.94758,-2.1082,0.29719,1.86982,-1.93892,0.110752,1.91069,-2.06332,0.158624,1.7292,-2.2429,0.217713,1.63742,-2.26891,0.341061,1.64405,-2.11298,0.400143,1.63018,-2.08713,0.203271,1.45236,-1.84587,0.018135,1.5126,-1.89581,0.021998,1.53665,-1.9625,0.025636,1.51144,-2.01015,0.20597,1.4317,-2.09372,0.27829,1.16016,-1.774 [...]
+/*2022*/{0.18431,1.92541,-1.7391,0.281769,1.86445,-1.86678,0.019675,1.84462,-1.69482,-0.10699,1.77669,-1.68408,0.016198,1.67489,-1.68245,0.076933,1.60631,-1.6633,0.255396,1.94344,-2.1096,0.296939,1.86659,-1.93924,0.110014,1.90756,-2.06221,0.155547,1.7259,-2.24271,0.212355,1.63371,-2.26849,0.338586,1.6367,-2.11498,0.397172,1.62001,-2.08952,0.201965,1.44867,-1.84411,0.016749,1.50881,-1.89594,0.021463,1.53246,-1.96127,0.025167,1.50816,-2.01126,0.206995,1.42955,-2.09242,0.274689,1.15526,-1.7 [...]
+/*2023*/{0.185336,1.92186,-1.73881,0.274884,1.86396,-1.86939,0.02028,1.83972,-1.6934,-0.105667,1.76991,-1.68101,0.018351,1.66854,-1.67918,0.07965,1.60009,-1.65942,0.253887,1.93946,-2.10996,0.296198,1.86429,-1.94009,0.109335,1.90421,-2.06157,0.150108,1.72264,-2.24066,0.207392,1.62973,-2.26762,0.335709,1.62709,-2.11658,0.395302,1.60939,-2.09226,0.200361,1.44459,-1.84354,0.016877,1.50495,-1.89559,0.020457,1.52921,-1.96178,0.025456,1.50455,-2.01153,0.207038,1.42717,-2.09138,0.272348,1.15098, [...]
+/*2024*/{0.185956,1.91684,-1.73812,0.275072,1.86033,-1.86943,0.021876,1.83378,-1.69135,-0.103393,1.76334,-1.67733,0.020509,1.66177,-1.67572,0.082928,1.59523,-1.65688,0.252598,1.93484,-2.11071,0.295564,1.86123,-1.94032,0.108123,1.90001,-2.06017,0.143781,1.71919,-2.23947,0.201099,1.6254,-2.26705,0.332955,1.61822,-2.11835,0.392586,1.59854,-2.09492,0.199001,1.4406,-1.84247,0.0156,1.50099,-1.89597,0.02099,1.52566,-1.9596,0.025172,1.49979,-2.01159,0.207975,1.42467,-2.09046,0.27111,1.14579,-1.7 [...]
+/*2025*/{0.186651,1.91241,-1.73781,0.281839,1.85352,-1.86792,0.023279,1.82707,-1.68885,-0.10055,1.75617,-1.67302,0.02401,1.65456,-1.67205,0.085879,1.58911,-1.6538,0.252241,1.93004,-2.11234,0.295486,1.8566,-1.94014,0.106935,1.89547,-2.0598,0.137745,1.71417,-2.238,0.195296,1.62039,-2.26657,0.328495,1.60791,-2.12028,0.389491,1.587,-2.09889,0.198755,1.43796,-1.84074,0.015783,1.49677,-1.89636,0.019728,1.52186,-1.96015,0.0241,1.49574,-2.01224,0.207394,1.42222,-2.08933,0.268475,1.14133,-1.76873 [...]
+/*2026*/{0.18651,1.9068,-1.73688,0.280798,1.85094,-1.86838,0.024482,1.82069,-1.6868,-0.098882,1.74818,-1.66883,0.027183,1.64841,-1.66766,0.090471,1.58303,-1.65028,0.250177,1.92506,-2.1138,0.295718,1.8533,-1.94055,0.105483,1.89139,-2.05902,0.132718,1.70997,-2.23635,0.188509,1.61538,-2.26586,0.325596,1.59781,-2.1228,0.386209,1.57525,-2.10257,0.196976,1.43473,-1.83955,0.016007,1.49136,-1.89716,0.019689,1.51767,-1.95956,0.023527,1.49002,-2.01317,0.20799,1.4187,-2.08784,0.266122,1.13654,-1.76 [...]
+/*2027*/{0.187128,1.90153,-1.73646,0.281198,1.84672,-1.86897,0.027067,1.81289,-1.6829,-0.096923,1.7398,-1.66427,0.030086,1.64152,-1.66313,0.09548,1.5767,-1.64686,0.248662,1.91943,-2.11477,0.29425,1.84868,-1.94087,0.104275,1.88592,-2.05818,0.126211,1.70491,-2.23495,0.182397,1.61028,-2.26559,0.321511,1.58789,-2.12538,0.382532,1.56273,-2.10653,0.195269,1.43068,-1.83814,0.014108,1.48748,-1.8972,0.019189,1.51355,-1.96021,0.023815,1.48501,-2.01288,0.207782,1.41527,-2.08698,0.263403,1.13215,-1. [...]
+/*2028*/{0.187503,1.89553,-1.73546,0.280271,1.84148,-1.869,0.028872,1.80516,-1.68109,-0.094456,1.73113,-1.65939,0.034279,1.63369,-1.65884,0.100171,1.56987,-1.64375,0.247234,1.91404,-2.11681,0.294856,1.84443,-1.94118,0.102996,1.88048,-2.0577,0.119885,1.70018,-2.23422,0.176051,1.60436,-2.26486,0.317982,1.5764,-2.12854,0.378143,1.54967,-2.11049,0.195427,1.4273,-1.83586,0.014238,1.48337,-1.89816,0.018481,1.509,-1.96008,0.022753,1.47927,-2.01315,0.208512,1.41163,-2.08588,0.26118,1.12757,-1.76 [...]
+/*2029*/{0.18768,1.88883,-1.73461,0.280901,1.83479,-1.86896,0.030561,1.79687,-1.67792,-0.09171,1.722,-1.6539,0.038725,1.62585,-1.65494,0.10507,1.56363,-1.63992,0.245361,1.90831,-2.11784,0.294479,1.83967,-1.94215,0.101631,1.87614,-2.05677,0.114141,1.69473,-2.23306,0.16984,1.59885,-2.2647,0.313515,1.56509,-2.13089,0.373781,1.53665,-2.11475,0.194818,1.42328,-1.8341,0.011968,1.47634,-1.89797,0.018182,1.50377,-1.95936,0.022745,1.47385,-2.0137,0.20804,1.40806,-2.08541,0.259116,1.12282,-1.76527 [...]
+/*2030*/{0.188757,1.88284,-1.73383,0.280481,1.83096,-1.8698,0.033277,1.78839,-1.67416,-0.088296,1.71216,-1.64882,0.043671,1.61787,-1.65114,0.110771,1.55672,-1.63655,0.243432,1.90264,-2.1193,0.294418,1.83474,-1.94284,0.100355,1.86956,-2.05536,0.108989,1.68911,-2.23224,0.16309,1.59329,-2.26425,0.309278,1.55438,-2.13426,0.367837,1.5223,-2.11939,0.194765,1.41758,-1.8319,0.011509,1.4705,-1.89835,0.017459,1.49888,-1.96003,0.021339,1.46824,-2.0141,0.207996,1.40419,-2.08401,0.257188,1.11724,-1.7 [...]
+/*2031*/{0.189896,1.87645,-1.73307,0.281132,1.82348,-1.86913,0.036395,1.77899,-1.67096,-0.083986,1.70276,-1.643,0.049837,1.60989,-1.64628,0.117056,1.55062,-1.63395,0.241772,1.89717,-2.12091,0.29382,1.82959,-1.94332,0.098605,1.86429,-2.05475,0.10394,1.68246,-2.23092,0.156626,1.5874,-2.2636,0.3037,1.54197,-2.1374,0.36303,1.50846,-2.12396,0.195103,1.41433,-1.82993,0.00965,1.46513,-1.89958,0.016088,1.49273,-1.95922,0.020526,1.46321,-2.01506,0.207682,1.40039,-2.08381,0.255576,1.11317,-1.76387 [...]
+/*2032*/{0.190255,1.87,-1.73219,0.281677,1.81948,-1.87037,0.03969,1.76974,-1.66744,-0.078741,1.69325,-1.63718,0.054936,1.60135,-1.64151,0.122703,1.54363,-1.63219,0.239107,1.89154,-2.1219,0.294554,1.82408,-1.9439,0.097179,1.85908,-2.05417,0.098648,1.67649,-2.23034,0.149364,1.58201,-2.26383,0.298584,1.5292,-2.14079,0.356397,1.49419,-2.12875,0.19404,1.40978,-1.82796,0.007822,1.45991,-1.89997,0.014362,1.48629,-1.95892,0.018859,1.45682,-2.01563,0.207548,1.39617,-2.08251,0.25209,1.10795,-1.762 [...]
+/*2033*/{0.191586,1.86378,-1.73115,0.281501,1.81182,-1.86972,0.04258,1.75998,-1.66373,-0.073335,1.6831,-1.63129,0.060907,1.59392,-1.63789,0.130323,1.53759,-1.62855,0.238202,1.88656,-2.1235,0.294703,1.81861,-1.94511,0.095724,1.85366,-2.05165,0.092832,1.671,-2.23001,0.141937,1.57611,-2.26371,0.293236,1.51665,-2.14415,0.349761,1.48041,-2.13371,0.191962,1.40438,-1.82573,0.005379,1.45417,-1.90044,0.012353,1.48054,-1.95889,0.017097,1.45062,-2.01583,0.207163,1.39042,-2.08087,0.248638,1.10188,-1 [...]
+/*2034*/{0.193145,1.85825,-1.73046,0.28285,1.80599,-1.87031,0.047133,1.74953,-1.66031,-0.06805,1.67181,-1.62488,0.06794,1.58576,-1.63402,0.138774,1.53206,-1.62535,0.235784,1.88147,-2.12516,0.295315,1.81232,-1.94617,0.094317,1.84928,-2.05035,0.086394,1.66662,-2.22999,0.135235,1.57092,-2.26339,0.286837,1.50468,-2.14849,0.344008,1.46651,-2.13825,0.189626,1.39994,-1.82476,0.001707,1.44937,-1.90135,0.010011,1.4745,-1.95967,0.014595,1.44572,-2.01655,0.206732,1.38638,-2.08054,0.245071,1.10026,- [...]
+/*2035*/{0.194742,1.853,-1.72923,0.283028,1.80109,-1.87143,0.052205,1.73991,-1.65613,-0.061583,1.66136,-1.61898,0.07589,1.57822,-1.63032,0.146318,1.52546,-1.62303,0.233345,1.87705,-2.1264,0.29562,1.80721,-1.94786,0.092965,1.84611,-2.0477,0.082828,1.6604,-2.2288,0.127762,1.5655,-2.26271,0.280948,1.49273,-2.15337,0.335644,1.45306,-2.14388,0.187142,1.39551,-1.82291,-0.001973,1.44508,-1.90186,0.007,1.46899,-1.96127,0.011356,1.44025,-2.0173,0.204463,1.38185,-2.0803,0.241476,1.09992,-1.75428,0 [...]
+/*2036*/{0.195576,1.84874,-1.72856,0.284297,1.79704,-1.87209,0.056411,1.73031,-1.65279,-0.053649,1.65125,-1.61323,0.083967,1.57082,-1.62642,0.155611,1.51985,-1.61972,0.2317,1.87392,-2.12786,0.296614,1.80184,-1.94894,0.091628,1.84322,-2.04437,0.079229,1.65757,-2.22882,0.120455,1.56055,-2.26167,0.274606,1.48141,-2.15697,0.326854,1.44098,-2.14997,0.183915,1.39221,-1.82158,-0.005289,1.44143,-1.9033,0.003357,1.46436,-1.96167,0.008794,1.4352,-2.01869,0.204262,1.37733,-2.07844,0.239703,1.09786, [...]
+/*2037*/{0.197038,1.84481,-1.72756,0.284618,1.79274,-1.874,0.062685,1.72128,-1.65003,-0.046116,1.64076,-1.60684,0.093166,1.56489,-1.6242,0.165552,1.51513,-1.61718,0.230692,1.87108,-2.12874,0.296896,1.7979,-1.95119,0.090922,1.84116,-2.04297,0.076368,1.6542,-2.22743,0.113721,1.55757,-2.26017,0.267488,1.4716,-2.16103,0.318425,1.42957,-2.15508,0.180829,1.38948,-1.82021,-0.008403,1.43827,-1.90489,0.001475,1.46033,-1.96251,0.004929,1.42919,-2.01913,0.202414,1.37328,-2.07675,0.235991,1.09842,-1 [...]
+/*2038*/{0.197934,1.84115,-1.72704,0.284685,1.78943,-1.875,0.068893,1.71369,-1.64789,-0.036187,1.63198,-1.60185,0.102649,1.55853,-1.62113,0.176021,1.51146,-1.61411,0.229368,1.8684,-2.12884,0.297023,1.7939,-1.95333,0.090253,1.84046,-2.04066,0.071796,1.65316,-2.22545,0.107818,1.55584,-2.25778,0.258461,1.46287,-2.1638,0.309252,1.41871,-2.16009,0.17853,1.38799,-1.81806,-0.010134,1.43606,-1.9074,-0.000151,1.45981,-1.96319,0.002411,1.42589,-2.02159,0.200911,1.37065,-2.07591,0.232648,1.09884,-1 [...]
+/*2039*/{0.199202,1.83832,-1.7267,0.284915,1.78496,-1.87632,0.074164,1.70775,-1.64572,-0.026656,1.62401,-1.59647,0.112877,1.55411,-1.61894,0.188071,1.50836,-1.61204,0.228787,1.86599,-2.12901,0.297158,1.79076,-1.95443,0.090513,1.83979,-2.04017,0.066895,1.65228,-2.2227,0.100634,1.55589,-2.25575,0.249343,1.45647,-2.1657,0.299102,1.40965,-2.16576,0.175997,1.38723,-1.81623,-0.012902,1.43586,-1.90841,-0.001409,1.45935,-1.96447,-0.000133,1.4229,-2.02283,0.200291,1.36807,-2.07519,0.229796,1.0983 [...]
+/*2040*/{0.200256,1.83635,-1.7276,0.284067,1.78366,-1.87763,0.079674,1.70353,-1.6442,-0.017482,1.61773,-1.59209,0.123809,1.55059,-1.61703,0.199284,1.50635,-1.61075,0.228821,1.86389,-2.12926,0.297035,1.78814,-1.95496,0.09053,1.8394,-2.04027,0.06219,1.65304,-2.21981,0.092928,1.55679,-2.25457,0.240572,1.45237,-2.16771,0.288821,1.40236,-2.17098,0.173351,1.38616,-1.81489,-0.013357,1.43515,-1.90851,-0.003159,1.45899,-1.96471,-0.001463,1.42192,-2.02321,0.196852,1.3658,-2.07566,0.229145,1.09828, [...]
+/*2041*/{0.2011,1.83517,-1.72783,0.284131,1.78029,-1.87861,0.08529,1.70065,-1.64285,-0.008006,1.61179,-1.58821,0.135902,1.54773,-1.61512,0.211678,1.50612,-1.60862,0.229212,1.86209,-2.12995,0.296478,1.78665,-1.95613,0.089869,1.83896,-2.04047,0.056463,1.65512,-2.21694,0.085639,1.55837,-2.25318,0.231188,1.44867,-2.17076,0.277748,1.39646,-2.17646,0.171807,1.38535,-1.81296,-0.015086,1.43549,-1.90942,-0.004893,1.45844,-1.96478,-0.002843,1.42122,-2.02309,0.195183,1.36438,-2.07568,0.219087,1.093 [...]
+/*2042*/{0.201926,1.83393,-1.72904,0.284204,1.77907,-1.87871,0.090132,1.69901,-1.64146,0.002602,1.60772,-1.58439,0.14721,1.54689,-1.61287,0.224444,1.50672,-1.60831,0.22849,1.86084,-2.12988,0.29443,1.78544,-1.95716,0.089532,1.83798,-2.0409,0.051158,1.65739,-2.21338,0.077507,1.5606,-2.2525,0.222134,1.44603,-2.17466,0.267122,1.39282,-2.18175,0.168932,1.38467,-1.81261,-0.016363,1.43456,-1.90914,-0.006543,1.4577,-1.96515,-0.003678,1.42197,-2.02309,0.194337,1.36371,-2.07584,0.213101,1.09056,-1 [...]
+/*2043*/{0.203508,1.83344,-1.72926,0.282596,1.77905,-1.87996,0.09413,1.69777,-1.6402,0.010268,1.60362,-1.58094,0.158613,1.54682,-1.61181,0.236945,1.50834,-1.60791,0.228193,1.85995,-2.13052,0.293655,1.78433,-1.95723,0.089176,1.83805,-2.04204,0.044273,1.66246,-2.20979,0.068079,1.56388,-2.25161,0.211926,1.4438,-2.17866,0.255534,1.39002,-2.18657,0.167452,1.38429,-1.81106,-0.017332,1.43445,-1.9092,-0.007014,1.45766,-1.96464,-0.005508,1.42249,-2.02332,0.192611,1.36373,-2.07666,0.20891,1.08848, [...]
+/*2044*/{0.205167,1.83369,-1.72938,0.282706,1.77853,-1.88022,0.098352,1.69781,-1.63909,0.021642,1.60075,-1.57721,0.169746,1.54717,-1.60974,0.250595,1.51162,-1.60755,0.227708,1.85966,-2.13035,0.292852,1.78383,-1.9578,0.089079,1.8382,-2.04249,0.039226,1.66755,-2.2065,0.059243,1.56843,-2.25155,0.202547,1.44404,-2.1834,0.244419,1.38935,-2.19111,0.164981,1.384,-1.81032,-0.019397,1.43425,-1.90948,-0.008554,1.45721,-1.9652,-0.00624,1.4224,-2.02203,0.191319,1.36396,-2.07694,0.203186,1.08184,-1.7 [...]
+/*2045*/{0.205531,1.83412,-1.72999,0.282047,1.77801,-1.88074,0.102546,1.69767,-1.63746,0.029915,1.59828,-1.57365,0.1813,1.54879,-1.60861,0.262945,1.51588,-1.60861,0.226672,1.85949,-2.13012,0.292718,1.78367,-1.95778,0.088954,1.8394,-2.04246,0.034008,1.67376,-2.20307,0.049503,1.57392,-2.2515,0.193671,1.44561,-2.18698,0.233388,1.39092,-2.19483,0.164024,1.38452,-1.81018,-0.01994,1.43371,-1.90927,-0.01042,1.45687,-1.96493,-0.0078,1.42342,-2.02133,0.189517,1.36579,-2.07727,0.19627,1.07854,-1.7 [...]
+/*2046*/{0.206699,1.83492,-1.73026,0.28112,1.77808,-1.88075,0.106073,1.69791,-1.6366,0.040072,1.59572,-1.57051,0.191904,1.55108,-1.60843,0.275836,1.52052,-1.61032,0.225531,1.86019,-2.13029,0.292156,1.78391,-1.95808,0.087807,1.84103,-2.04199,0.027211,1.68029,-2.1999,0.040754,1.58009,-2.25133,0.184122,1.4493,-2.19096,0.222616,1.39347,-2.19866,0.16326,1.38483,-1.80999,-0.021185,1.43455,-1.90952,-0.011236,1.45835,-1.96439,-0.008972,1.42363,-2.0198,0.189267,1.3677,-2.077,0.185682,1.07722,-1.7 [...]
+/*2047*/{0.208334,1.83637,-1.73023,0.282567,1.77943,-1.88256,0.111132,1.69875,-1.63582,0.049886,1.59365,-1.56734,0.202479,1.55438,-1.60831,0.287209,1.52636,-1.61175,0.224242,1.86077,-2.13025,0.290867,1.78397,-1.95917,0.086684,1.84198,-2.04154,0.021065,1.68721,-2.1985,0.031438,1.58696,-2.25119,0.17364,1.45247,-2.19367,0.211351,1.39741,-2.20205,0.161479,1.38501,-1.80903,-0.022217,1.43479,-1.90965,-0.011254,1.45863,-1.9644,-0.010205,1.42484,-2.01979,0.188402,1.37027,-2.07698,0.17845,1.07541 [...]
+/*2048*/{0.21028,1.83888,-1.73124,0.282384,1.78049,-1.8827,0.116235,1.6994,-1.63504,0.059421,1.59281,-1.56499,0.212685,1.55875,-1.60861,0.29861,1.53326,-1.61415,0.223164,1.8627,-2.13055,0.290197,1.78457,-1.95988,0.086109,1.84431,-2.04076,0.014369,1.69338,-2.19589,0.023344,1.59401,-2.25149,0.164659,1.45873,-2.19597,0.201301,1.40258,-2.20442,0.160723,1.38559,-1.80921,-0.022296,1.43651,-1.90957,-0.011445,1.45888,-1.96385,-0.011369,1.42585,-2.01995,0.187607,1.37263,-2.07699,0.171769,1.07412, [...]
+/*2049*/{0.212488,1.84085,-1.73175,0.28268,1.78157,-1.88373,0.121311,1.70038,-1.63453,0.068235,1.59259,-1.56263,0.222186,1.56281,-1.60918,0.308912,1.5405,-1.61714,0.220575,1.86451,-2.13094,0.289736,1.78675,-1.96078,0.084651,1.84543,-2.03992,0.007766,1.69992,-2.19401,0.014593,1.60117,-2.25148,0.155474,1.46514,-2.19765,0.190595,1.40849,-2.20651,0.160646,1.38562,-1.80921,-0.022596,1.4371,-1.90958,-0.012091,1.46032,-1.9637,-0.011769,1.42701,-2.01982,0.187016,1.37411,-2.07718,0.163322,1.07213 [...]
+/*2050*/{0.214785,1.84309,-1.73269,0.28222,1.78389,-1.885,0.127981,1.70193,-1.6337,0.079553,1.59316,-1.56083,0.23236,1.56799,-1.6111,0.319194,1.54879,-1.62086,0.218323,1.86726,-2.13116,0.288693,1.78912,-1.96185,0.083536,1.84797,-2.03848,0.002628,1.7077,-2.19244,0.006631,1.60913,-2.25181,0.146555,1.47213,-2.19896,0.181056,1.41555,-2.20785,0.160602,1.38638,-1.81023,-0.022596,1.43903,-1.90936,-0.012757,1.46231,-1.96326,-0.011289,1.42879,-2.0204,0.187372,1.37683,-2.07698,0.155991,1.07238,-1. [...]
+/*2051*/{0.217626,1.84658,-1.73313,0.282853,1.78652,-1.88601,0.134357,1.70338,-1.63258,0.087763,1.59382,-1.55876,0.241661,1.57423,-1.61265,0.329133,1.55738,-1.62392,0.214893,1.87006,-2.13123,0.288326,1.79166,-1.96309,0.082315,1.84951,-2.03693,-0.002495,1.71361,-2.19047,-0.002138,1.6165,-2.25184,0.137577,1.47971,-2.19981,0.171921,1.42329,-2.20885,0.160233,1.38618,-1.80915,-0.022127,1.44116,-1.90931,-0.01304,1.46421,-1.96315,-0.011653,1.43078,-2.02034,0.187176,1.37945,-2.07674,0.145448,1.0 [...]
+/*2052*/{0.220286,1.85,-1.73375,0.282398,1.78978,-1.88716,0.141512,1.7054,-1.63206,0.09676,1.59616,-1.5585,0.249725,1.58023,-1.61487,0.337297,1.56636,-1.62794,0.212377,1.87369,-2.13188,0.287888,1.79414,-1.96438,0.080802,1.85204,-2.03573,-0.008469,1.72086,-2.18917,-0.008596,1.62452,-2.25155,0.130145,1.48794,-2.20036,0.163171,1.43145,-2.21003,0.160653,1.3872,-1.80962,-0.021333,1.44469,-1.90876,-0.010779,1.46815,-1.96281,-0.010769,1.43234,-2.02012,0.187453,1.38192,-2.07627,0.137962,1.0706,- [...]
+/*2053*/{0.222771,1.85363,-1.73488,0.28287,1.79235,-1.88885,0.149393,1.70761,-1.63125,0.106919,1.59839,-1.55712,0.258165,1.58728,-1.61745,0.345934,1.57567,-1.63218,0.208526,1.87738,-2.13221,0.287696,1.79756,-1.96594,0.079549,1.85384,-2.03371,-0.014759,1.72824,-2.18863,-0.016213,1.63241,-2.25154,0.121752,1.49656,-2.20029,0.155108,1.43988,-2.2111,0.161303,1.38779,-1.80885,-0.019977,1.44543,-1.90738,-0.011517,1.46851,-1.96129,-0.010364,1.43506,-2.02022,0.187574,1.38507,-2.07547,0.132231,1.0 [...]
+/*2054*/{0.224774,1.85838,-1.73611,0.281957,1.79634,-1.88879,0.157392,1.71061,-1.63102,0.116986,1.60055,-1.55566,0.265079,1.59407,-1.61991,0.353293,1.58526,-1.63664,0.205993,1.88184,-2.13199,0.287302,1.80122,-1.9672,0.078512,1.85634,-2.03289,-0.020496,1.73636,-2.18803,-0.023179,1.64067,-2.25089,0.113979,1.50516,-2.20047,0.147815,1.44916,-2.21193,0.161328,1.38802,-1.80818,-0.020754,1.44736,-1.90803,-0.010764,1.47066,-1.96055,-0.010146,1.43872,-2.01998,0.188141,1.38784,-2.07455,0.126496,1. [...]
+/*2055*/{0.227415,1.86229,-1.7374,0.284937,1.80087,-1.89126,0.163915,1.71354,-1.63139,0.124023,1.60272,-1.55508,0.27257,1.60049,-1.62227,0.359891,1.59443,-1.6414,0.202908,1.88642,-2.13193,0.287347,1.80466,-1.9686,0.076998,1.85931,-2.02948,-0.024711,1.74403,-2.18685,-0.029766,1.64875,-2.25021,0.107674,1.51483,-2.20021,0.141216,1.45808,-2.21213,0.162047,1.38916,-1.80673,-0.01923,1.44963,-1.90663,-0.009741,1.47356,-1.95919,-0.010396,1.44143,-2.02021,0.188877,1.39173,-2.0733,0.118495,1.07094 [...]
+/*2056*/{0.229484,1.86713,-1.73835,0.285665,1.80401,-1.89295,0.171191,1.71685,-1.63144,0.133214,1.60613,-1.55432,0.279031,1.60842,-1.62449,0.365926,1.60448,-1.64623,0.199235,1.89089,-2.132,0.286966,1.8095,-1.97005,0.075758,1.86122,-2.02767,-0.030511,1.75167,-2.18704,-0.036176,1.65699,-2.24959,0.100663,1.5239,-2.20008,0.13383,1.46727,-2.21278,0.162696,1.38961,-1.80602,-0.017945,1.45181,-1.90659,-0.008893,1.47644,-1.95759,-0.007887,1.44554,-2.01942,0.189467,1.39509,-2.07191,0.110809,1.0714 [...]
+/*2057*/{0.231452,1.87193,-1.73996,0.285576,1.80806,-1.89459,0.177332,1.72057,-1.63188,0.14107,1.61011,-1.55393,0.284856,1.61599,-1.62788,0.371194,1.61379,-1.65163,0.196749,1.89547,-2.13127,0.286847,1.81322,-1.97185,0.0745,1.86451,-2.02707,-0.033226,1.75908,-2.18589,-0.042102,1.66496,-2.24849,0.093772,1.53197,-2.19894,0.127795,1.47685,-2.21304,0.163264,1.39034,-1.80518,-0.016455,1.45591,-1.90674,-0.00812,1.47975,-1.95675,-0.006418,1.44985,-2.01936,0.190289,1.39954,-2.07047,0.107585,1.074 [...]
+/*2058*/{0.233546,1.87742,-1.74106,0.284942,1.81245,-1.89604,0.184305,1.72487,-1.63241,0.149013,1.61405,-1.55338,0.290207,1.62398,-1.63103,0.375427,1.62351,-1.65557,0.193809,1.8999,-2.13084,0.285964,1.81663,-1.97302,0.073695,1.86824,-2.02461,-0.038394,1.76694,-2.18479,-0.047742,1.67317,-2.24716,0.088414,1.54118,-2.19773,0.122032,1.48602,-2.21304,0.163531,1.39128,-1.80466,-0.013925,1.45793,-1.90549,-0.006067,1.48259,-1.95497,-0.00479,1.45453,-2.01959,0.191109,1.40333,-2.06905,0.099216,1.0 [...]
+/*2059*/{0.235367,1.88273,-1.74299,0.287663,1.81753,-1.89788,0.189402,1.72983,-1.6332,0.155712,1.61821,-1.5534,0.294611,1.63108,-1.63389,0.379905,1.63322,-1.66093,0.191733,1.90407,-2.13037,0.286024,1.82233,-1.97497,0.073874,1.87228,-2.02343,-0.041468,1.7744,-2.18459,-0.053227,1.68127,-2.2461,0.082209,1.55033,-2.19762,0.116508,1.49514,-2.21311,0.166394,1.39276,-1.80399,-0.013148,1.46167,-1.90537,-0.005176,1.48635,-1.95345,-0.003178,1.45921,-2.01928,0.19221,1.4079,-2.06779,0.09317,1.07641, [...]
+/*2060*/{0.237308,1.88794,-1.74466,0.287899,1.82159,-1.89921,0.193948,1.73451,-1.63394,0.162555,1.62302,-1.55366,0.299197,1.63865,-1.63626,0.384048,1.64238,-1.666,0.189915,1.90836,-2.13024,0.286484,1.82671,-1.97621,0.072781,1.87584,-2.02213,-0.044768,1.78131,-2.18413,-0.057824,1.6893,-2.2447,0.077588,1.55943,-2.19657,0.111407,1.50367,-2.21266,0.166645,1.3935,-1.80408,-0.011506,1.46542,-1.90397,-0.004784,1.48995,-1.95179,-0.001648,1.46439,-2.01919,0.193365,1.41248,-2.06699,0.088974,1.0775 [...]
+/*2061*/{0.238488,1.89329,-1.74671,0.288413,1.82609,-1.90064,0.199082,1.73958,-1.63426,0.168966,1.62738,-1.55357,0.302295,1.64639,-1.63962,0.386119,1.65114,-1.66995,0.188684,1.91201,-2.13022,0.286002,1.83077,-1.97753,0.072843,1.87948,-2.02075,-0.047488,1.78827,-2.18368,-0.06283,1.69716,-2.24239,0.07182,1.56757,-2.1955,0.106716,1.51204,-2.21301,0.168567,1.39469,-1.80439,-0.010359,1.46895,-1.90297,-0.002285,1.49389,-1.95118,-0.000501,1.4694,-2.01927,0.19407,1.41699,-2.06618,0.083765,1.0812 [...]
+/*2062*/{0.240078,1.89842,-1.7478,0.288897,1.83052,-1.9026,0.202816,1.74427,-1.63495,0.174035,1.63131,-1.55353,0.305437,1.6533,-1.64212,0.388636,1.65972,-1.67471,0.188116,1.91586,-2.12999,0.286173,1.8354,-1.97884,0.072618,1.88274,-2.02009,-0.04975,1.79533,-2.18355,-0.067484,1.70448,-2.24079,0.068433,1.57531,-2.19388,0.102242,1.51974,-2.21292,0.170423,1.39574,-1.80555,-0.007182,1.47262,-1.90324,-0.000477,1.49811,-1.94962,0.001824,1.47399,-2.01893,0.195254,1.42192,-2.06572,0.078889,1.08344 [...]
+/*2063*/{0.241926,1.90343,-1.74889,0.289688,1.83495,-1.90406,0.206344,1.74945,-1.63534,0.179387,1.63642,-1.55373,0.308582,1.66006,-1.64438,0.391274,1.66774,-1.67885,0.186325,1.919,-2.12988,0.285949,1.8399,-1.9799,0.07198,1.88598,-2.01942,-0.051901,1.80134,-2.18171,-0.071277,1.71205,-2.23865,0.064474,1.58314,-2.19357,0.09795,1.52724,-2.2128,0.172281,1.39641,-1.80635,-0.005543,1.47656,-1.90198,0.001629,1.50164,-1.94816,0.003034,1.47932,-2.01883,0.195922,1.42681,-2.06562,0.073469,1.08713,-1 [...]
+/*2064*/{0.243123,1.90794,-1.75057,0.291009,1.83906,-1.90607,0.209079,1.75414,-1.63562,0.183339,1.64046,-1.55401,0.310674,1.66629,-1.64761,0.393858,1.67624,-1.68077,0.186063,1.9223,-2.12963,0.286654,1.84365,-1.98106,0.071731,1.88956,-2.01937,-0.054921,1.80757,-2.18067,-0.074824,1.71906,-2.23645,0.060148,1.58941,-2.19282,0.094969,1.53375,-2.21248,0.173988,1.39789,-1.80738,-0.004345,1.48022,-1.90166,0.002436,1.50569,-1.94664,0.00483,1.48475,-2.01783,0.195067,1.43222,-2.06589,0.070483,1.089 [...]
+/*2065*/{0.244141,1.91207,-1.7518,0.291424,1.8435,-1.90704,0.212422,1.7588,-1.63635,0.18907,1.64498,-1.55448,0.31295,1.67223,-1.65049,0.394641,1.68331,-1.68572,0.185766,1.9252,-2.12996,0.287033,1.84792,-1.98235,0.071751,1.89308,-2.01829,-0.055599,1.81372,-2.18001,-0.07866,1.72545,-2.23393,0.05734,1.59627,-2.19229,0.091403,1.53984,-2.2129,0.176606,1.39938,-1.8087,-0.00145,1.48443,-1.90143,0.003271,1.50975,-1.94659,0.006413,1.4896,-2.01768,0.195325,1.43663,-2.06652,0.066815,1.09338,-1.7808 [...]
+/*2066*/{0.246585,1.91652,-1.75307,0.291852,1.8471,-1.90857,0.215555,1.76315,-1.63699,0.192935,1.64866,-1.55437,0.314502,1.67775,-1.65284,0.395134,1.69015,-1.68986,0.185345,1.92792,-2.13054,0.286884,1.85155,-1.98269,0.073917,1.89472,-2.01842,-0.056,1.81909,-2.17748,-0.081558,1.7319,-2.23193,0.054437,1.60197,-2.19139,0.087592,1.54587,-2.21299,0.177825,1.40073,-1.80951,4.6e-005,1.48763,-1.90019,0.004973,1.51407,-1.94492,0.008183,1.49428,-2.01714,0.195371,1.44134,-2.06664,0.064009,1.09659,- [...]
+/*2067*/{0.247627,1.9201,-1.75434,0.293657,1.85219,-1.91066,0.217125,1.76725,-1.63691,0.197741,1.65164,-1.55417,0.315906,1.68301,-1.65519,0.396749,1.69622,-1.69474,0.185009,1.93065,-2.13038,0.286981,1.85528,-1.98388,0.074805,1.89707,-2.01829,-0.056831,1.82415,-2.17565,-0.084379,1.73791,-2.22951,0.052744,1.60732,-2.19046,0.085407,1.55129,-2.21305,0.180054,1.40232,-1.81061,0.001227,1.49144,-1.89973,0.006288,1.51755,-1.94275,0.009886,1.49956,-2.01625,0.1966,1.44654,-2.06706,0.059982,1.09927 [...]
+/*2068*/{0.248632,1.92405,-1.75531,0.293372,1.85506,-1.91061,0.219406,1.77123,-1.63709,0.200621,1.65509,-1.554,0.317876,1.68826,-1.65785,0.397347,1.70203,-1.69856,0.184943,1.93302,-2.13044,0.287591,1.8578,-1.98411,0.075532,1.89965,-2.01788,-0.05747,1.82959,-2.17324,-0.087193,1.74333,-2.22687,0.050263,1.61275,-2.1901,0.082912,1.55602,-2.213,0.180927,1.40466,-1.81166,0.002612,1.49497,-1.8982,0.007746,1.52155,-1.94164,0.011123,1.50419,-2.01489,0.195881,1.45017,-2.06727,0.057851,1.1031,-1.78 [...]
+/*2069*/{0.250866,1.92745,-1.75618,0.294237,1.85873,-1.91201,0.22134,1.77468,-1.63765,0.202332,1.65816,-1.55415,0.319024,1.69164,-1.65945,0.397363,1.7073,-1.70213,0.184892,1.93541,-2.13056,0.28789,1.86167,-1.9853,0.075882,1.90266,-2.01815,-0.057329,1.83454,-2.17152,-0.088728,1.74835,-2.22474,0.048983,1.61662,-2.18976,0.081058,1.56035,-2.21301,0.182983,1.40664,-1.81197,0.004516,1.49855,-1.8973,0.008071,1.52513,-1.94126,0.012861,1.50856,-2.01417,0.196522,1.45415,-2.06707,0.054914,1.10656,- [...]
+/*2070*/{0.2517,1.93025,-1.75723,0.29655,1.8623,-1.91255,0.222913,1.77806,-1.63819,0.205645,1.66049,-1.55325,0.319273,1.69585,-1.66118,0.397434,1.71092,-1.705,0.184618,1.93788,-2.13059,0.288186,1.86431,-1.98575,0.076467,1.90478,-2.01764,-0.057095,1.83937,-2.16864,-0.089998,1.75325,-2.22229,0.048034,1.62036,-2.18934,0.079085,1.5635,-2.21319,0.184042,1.40947,-1.81317,0.005935,1.50216,-1.89624,0.009616,1.52884,-1.93945,0.013641,1.5127,-2.0129,0.196452,1.45764,-2.06744,0.052004,1.11094,-1.77 [...]
+/*2071*/{0.253633,1.93267,-1.75751,0.29648,1.86588,-1.91363,0.224664,1.78099,-1.63844,0.206945,1.66288,-1.55375,0.321293,1.69942,-1.66319,0.398107,1.71479,-1.7083,0.185865,1.94019,-2.131,0.289471,1.86737,-1.98626,0.07723,1.90724,-2.01721,-0.057075,1.84264,-2.16686,-0.091197,1.75742,-2.21977,0.046357,1.62344,-2.18865,0.078008,1.56676,-2.21282,0.185687,1.41182,-1.81334,0.007533,1.50515,-1.89633,0.010346,1.53247,-1.93879,0.013545,1.51688,-2.01171,0.19636,1.46087,-2.06712,0.050112,1.11384,-1 [...]
+/*2072*/{0.254666,1.93507,-1.75848,0.297977,1.86879,-1.91462,0.226366,1.78418,-1.63877,0.209206,1.66618,-1.55376,0.321496,1.70152,-1.66491,0.397848,1.71785,-1.71154,0.186469,1.94215,-2.13122,0.290174,1.86938,-1.98751,0.078661,1.90847,-2.01737,-0.055934,1.84653,-2.16437,-0.092168,1.76061,-2.21795,0.046227,1.62625,-2.18831,0.077227,1.56939,-2.21265,0.186532,1.41461,-1.81339,0.008241,1.50832,-1.89439,0.010952,1.53586,-1.93806,0.013907,1.52,-2.01077,0.196265,1.46447,-2.06699,0.049262,1.11843 [...]
+/*2073*/{0.256254,1.93724,-1.75905,0.298545,1.87211,-1.91509,0.228694,1.78648,-1.63955,0.21065,1.66792,-1.55373,0.322167,1.70384,-1.66501,0.398512,1.71948,-1.71375,0.186816,1.94394,-2.13156,0.2906,1.87143,-1.98774,0.078877,1.91199,-2.01663,-0.057278,1.85335,-2.16364,-0.091908,1.76326,-2.21616,0.045617,1.62867,-2.18822,0.076749,1.57194,-2.21224,0.187621,1.41724,-1.81352,0.008768,1.51135,-1.89467,0.010694,1.53865,-1.93792,0.015983,1.52234,-2.00902,0.196436,1.46743,-2.06689,0.049922,1.12114 [...]
+/*2074*/{0.257183,1.93913,-1.75937,0.298472,1.87397,-1.91603,0.22963,1.78853,-1.64014,0.211214,1.67018,-1.55326,0.321648,1.70465,-1.66702,0.397158,1.7207,-1.71647,0.187588,1.94563,-2.13162,0.291471,1.87307,-1.98845,0.079793,1.91368,-2.01663,-0.056727,1.85649,-2.1618,-0.091987,1.7656,-2.21466,0.045536,1.62977,-2.18753,0.077174,1.57321,-2.21203,0.188131,1.42004,-1.81372,0.009135,1.51402,-1.89305,0.011751,1.54134,-1.93674,0.01643,1.52522,-2.00761,0.197179,1.47006,-2.06664,0.049926,1.12292,- [...]
+/*2075*/{0.258825,1.94066,-1.75973,0.298358,1.87598,-1.91608,0.230475,1.79018,-1.64061,0.211887,1.67245,-1.55448,0.321642,1.70589,-1.66836,0.397327,1.72117,-1.71824,0.188852,1.94722,-2.13195,0.292045,1.8744,-1.98948,0.08074,1.91547,-2.01812,-0.056587,1.85744,-2.15918,-0.091471,1.76668,-2.21331,0.045659,1.6307,-2.18711,0.077191,1.57382,-2.21169,0.189359,1.42275,-1.81329,0.010322,1.51665,-1.89275,0.011731,1.5441,-1.93645,0.016698,1.52738,-2.00784,0.197352,1.4721,-2.06658,0.053099,1.12497,- [...]
+/*2076*/{0.259729,1.94221,-1.76039,0.299067,1.87789,-1.91772,0.231487,1.79163,-1.64094,0.21205,1.67338,-1.55406,0.321552,1.70667,-1.67053,0.396852,1.7218,-1.72088,0.189287,1.94866,-2.13306,0.293134,1.87509,-1.98971,0.081563,1.91617,-2.01775,-0.056057,1.85889,-2.15771,-0.091538,1.76763,-2.21239,0.047998,1.63115,-2.18622,0.07804,1.57471,-2.21106,0.18999,1.42566,-1.81271,0.010247,1.51844,-1.892,0.010639,1.54686,-1.93591,0.017072,1.52896,-2.00649,0.197417,1.47352,-2.06635,0.054854,1.12715,-1 [...]
+/*2077*/{0.26025,1.94253,-1.76107,0.299828,1.87971,-1.91829,0.23233,1.7928,-1.64149,0.213014,1.67463,-1.55427,0.32097,1.70623,-1.67124,0.396265,1.72077,-1.72258,0.19032,1.94965,-2.13362,0.293934,1.87625,-1.99018,0.082673,1.91944,-2.01772,-0.055005,1.85943,-2.1564,-0.09021,1.76812,-2.21105,0.04857,1.6308,-2.1858,0.079752,1.57438,-2.21069,0.18983,1.42814,-1.81252,0.01073,1.52037,-1.89111,0.010923,1.54874,-1.93559,0.01627,1.53064,-2.00594,0.197675,1.47506,-2.06563,0.058022,1.12783,-1.77835, [...]
+/*2078*/{0.261274,1.94299,-1.76165,0.300335,1.87986,-1.91907,0.23243,1.79343,-1.64213,0.211987,1.67495,-1.55403,0.320589,1.7056,-1.67249,0.394618,1.71815,-1.72444,0.191374,1.95091,-2.13396,0.294695,1.87693,-1.99076,0.084094,1.921,-2.01783,-0.051986,1.85504,-2.15353,-0.089249,1.76757,-2.21031,0.049991,1.63002,-2.18513,0.081496,1.57402,-2.21009,0.190654,1.4299,-1.812,0.011139,1.52166,-1.89091,0.011006,1.55021,-1.93463,0.016871,1.53129,-2.00526,0.198179,1.47544,-2.06537,0.062826,1.12677,-1. [...]
+/*2079*/{0.262527,1.94335,-1.76182,0.30132,1.88134,-1.92042,0.232154,1.79393,-1.64297,0.21143,1.67628,-1.55524,0.319274,1.70412,-1.67334,0.393481,1.71612,-1.72588,0.19265,1.95149,-2.13488,0.295078,1.87727,-1.99092,0.084573,1.92148,-2.01847,-0.050601,1.85375,-2.15036,-0.088113,1.76665,-2.20975,0.052496,1.62877,-2.18445,0.083289,1.57262,-2.20964,0.19121,1.43219,-1.81146,0.01179,1.52245,-1.89009,0.011128,1.55164,-1.93468,0.016722,1.53202,-2.00429,0.198794,1.47612,-2.06478,0.069297,1.12572,- [...]
+/*2080*/{0.263028,1.94295,-1.76238,0.300551,1.88167,-1.9209,0.231771,1.79397,-1.6436,0.209738,1.67589,-1.55545,0.318606,1.70197,-1.67488,0.391877,1.7131,-1.72727,0.193485,1.9519,-2.13538,0.294909,1.87701,-1.99169,0.085563,1.92278,-2.01875,-0.050189,1.85261,-2.15019,-0.086266,1.76454,-2.20963,0.053607,1.62701,-2.18397,0.08576,1.571,-2.20909,0.191369,1.43303,-1.81106,0.011635,1.52275,-1.88955,0.01146,1.55237,-1.9349,0.016676,1.53195,-2.00429,0.199312,1.47667,-2.06434,0.074931,1.12504,-1.78 [...]
+/*2081*/{0.26383,1.9426,-1.76318,0.300196,1.88159,-1.92099,0.230438,1.79397,-1.64418,0.20862,1.67596,-1.55539,0.316856,1.70021,-1.675,0.390795,1.70922,-1.72868,0.194045,1.95223,-2.13562,0.29502,1.87719,-1.992,0.08615,1.92372,-2.02049,-0.048742,1.85083,-2.15013,-0.084407,1.76252,-2.20932,0.056951,1.62546,-2.18356,0.08793,1.56885,-2.2087,0.191575,1.43461,-1.81007,0.011613,1.5219,-1.89038,0.011491,1.55247,-1.93478,0.016881,1.53067,-2.00365,0.20039,1.47584,-2.06393,0.081151,1.12276,-1.7841,- [...]
+/*2082*/{0.264217,1.94169,-1.7639,0.300227,1.88157,-1.9212,0.229429,1.79402,-1.64459,0.206027,1.67594,-1.55655,0.316039,1.69746,-1.67637,0.390004,1.70513,-1.72959,0.19543,1.95185,-2.13651,0.295181,1.87697,-1.9921,0.086768,1.92382,-2.02183,-0.047749,1.84891,-2.14864,-0.082382,1.75964,-2.20928,0.058554,1.62236,-2.18291,0.090834,1.56571,-2.20825,0.192266,1.43531,-1.8095,0.011802,1.52246,-1.88955,0.011876,1.55244,-1.9345,0.016338,1.52957,-2.00395,0.201084,1.47579,-2.06333,0.088329,1.12104,-1 [...]
+/*2083*/{0.264286,1.94082,-1.76449,0.299615,1.88034,-1.92181,0.228193,1.79331,-1.64537,0.20385,1.67513,-1.55664,0.313976,1.69432,-1.67694,0.388717,1.70023,-1.73043,0.195838,1.95141,-2.13692,0.295157,1.87667,-1.99208,0.087389,1.92396,-2.02223,-0.045772,1.84564,-2.14842,-0.079136,1.75645,-2.20989,0.060648,1.61916,-2.18266,0.093755,1.56324,-2.20847,0.192822,1.43551,-1.80904,0.011283,1.52124,-1.88983,0.011555,1.55114,-1.93434,0.016829,1.52769,-2.00415,0.201421,1.47498,-2.06276,0.094708,1.118 [...]
+/*2084*/{0.263245,1.93893,-1.76564,0.299173,1.88018,-1.92217,0.226464,1.79234,-1.64594,0.202177,1.67404,-1.55702,0.311896,1.69075,-1.67737,0.385828,1.69499,-1.73083,0.196423,1.9506,-2.13711,0.294955,1.87635,-1.99225,0.087532,1.92329,-2.02421,-0.044099,1.84247,-2.1476,-0.076496,1.75275,-2.21065,0.064228,1.61598,-2.18287,0.096659,1.55952,-2.20798,0.19332,1.43547,-1.80925,0.011752,1.5203,-1.8892,0.012403,1.54933,-1.9348,0.017101,1.52647,-2.00375,0.203293,1.47335,-2.06268,0.103473,1.11637,-1 [...]
+/*2085*/{0.263213,1.93838,-1.76631,0.298544,1.87933,-1.9229,0.224082,1.79127,-1.6463,0.199324,1.67275,-1.55779,0.309458,1.6865,-1.67822,0.384675,1.68889,-1.73138,0.196945,1.94954,-2.13782,0.294509,1.8756,-1.99257,0.088263,1.92235,-2.02567,-0.043485,1.83893,-2.14799,-0.074044,1.74829,-2.21162,0.06609,1.61217,-2.18302,0.099988,1.55545,-2.20808,0.193735,1.43486,-1.80952,0.011578,1.51848,-1.8892,0.013112,1.54868,-1.93486,0.018071,1.52394,-2.00424,0.203549,1.47188,-2.06281,0.108386,1.11342,-1 [...]
+/*2086*/{0.262796,1.93678,-1.76682,0.297653,1.87792,-1.92266,0.222249,1.79036,-1.64713,0.195982,1.67134,-1.55868,0.306657,1.68238,-1.67818,0.382405,1.68247,-1.73184,0.197076,1.94771,-2.13856,0.294008,1.8744,-1.99272,0.088714,1.92154,-2.02638,-0.041454,1.83512,-2.14911,-0.070935,1.74365,-2.21307,0.069436,1.60759,-2.18334,0.103326,1.55114,-2.20804,0.195323,1.43365,-1.80937,0.011603,1.517,-1.88965,0.012841,1.5459,-1.93526,0.017518,1.52192,-2.00463,0.203927,1.46923,-2.06257,0.115494,1.11185, [...]
+/*2087*/{0.261197,1.93448,-1.76733,0.297587,1.87581,-1.92286,0.219909,1.78902,-1.64736,0.192176,1.66977,-1.55952,0.304237,1.6777,-1.67848,0.379365,1.67556,-1.73166,0.19738,1.9463,-2.13881,0.294377,1.87336,-1.99278,0.087996,1.9199,-2.02855,-0.040517,1.83067,-2.14988,-0.068653,1.73826,-2.21476,0.072839,1.60249,-2.18383,0.106946,1.54653,-2.2083,0.195764,1.4328,-1.80937,0.012203,1.51481,-1.8888,0.01261,1.54389,-1.93503,0.017835,1.51876,-2.00497,0.203811,1.46639,-2.06308,0.122619,1.10808,-1.7 [...]
+/*2088*/{0.260304,1.93219,-1.7672,0.297699,1.87475,-1.92322,0.21702,1.78746,-1.6473,0.187436,1.6677,-1.5611,0.301039,1.67276,-1.67893,0.377445,1.66846,-1.73135,0.197806,1.94424,-2.1393,0.293221,1.87132,-1.99333,0.088211,1.91915,-2.02935,-0.039461,1.82504,-2.1504,-0.065147,1.7326,-2.21691,0.075909,1.59744,-2.18545,0.110481,1.5411,-2.20897,0.196198,1.43093,-1.80959,0.013249,1.51157,-1.88903,0.013774,1.54119,-1.9356,0.018056,1.51654,-2.00526,0.203696,1.46312,-2.06355,0.12848,1.105,-1.7942,- [...]
+/*2089*/{0.258569,1.93046,-1.7676,0.297014,1.87301,-1.92323,0.214122,1.78555,-1.64771,0.183872,1.66635,-1.56217,0.297655,1.66739,-1.67893,0.374016,1.66056,-1.73048,0.197153,1.94157,-2.13963,0.2935,1.86949,-1.99321,0.088033,1.91632,-2.03186,-0.037706,1.81894,-2.1524,-0.062481,1.72609,-2.21859,0.078994,1.59201,-2.18619,0.114172,1.5357,-2.2095,0.197474,1.42969,-1.80982,0.013172,1.50881,-1.89001,0.01429,1.53817,-1.93586,0.018376,1.51319,-2.00556,0.203975,1.45998,-2.064,0.136379,1.10349,-1.79 [...]
+/*2090*/{0.256998,1.92829,-1.7674,0.296123,1.86965,-1.92308,0.210757,1.78295,-1.64795,0.179398,1.66392,-1.56334,0.294056,1.66171,-1.6787,0.371083,1.65258,-1.7295,0.19741,1.93898,-2.1403,0.292776,1.86742,-1.99331,0.087508,1.91392,-2.0327,-0.036138,1.81388,-2.15391,-0.059123,1.71992,-2.22086,0.083057,1.58625,-2.18791,0.118358,1.52983,-2.20982,0.198421,1.42732,-1.81052,0.01386,1.50595,-1.8896,0.014128,1.53514,-1.93556,0.018757,1.51002,-2.00574,0.204406,1.45746,-2.0645,0.141519,1.10033,-1.79 [...]
+/*2091*/{0.25564,1.92559,-1.76732,0.295879,1.86707,-1.92314,0.2079,1.78064,-1.64812,0.174592,1.66179,-1.56519,0.290801,1.65602,-1.67835,0.368397,1.64469,-1.72791,0.19773,1.93531,-2.14119,0.292299,1.86473,-1.99331,0.086892,1.91063,-2.03438,-0.035225,1.80714,-2.15589,-0.056463,1.71267,-2.22297,0.08643,1.58049,-2.1896,0.122468,1.52404,-2.21029,0.199115,1.42492,-1.81084,0.013593,1.50308,-1.88929,0.014443,1.53167,-1.93545,0.018129,1.50623,-2.00548,0.204257,1.45453,-2.06555,0.146131,1.09688,-1 [...]
+/*2092*/{0.253511,1.92254,-1.76706,0.29565,1.86446,-1.92286,0.203954,1.77766,-1.64787,0.169203,1.65997,-1.56623,0.286883,1.64945,-1.67791,0.364214,1.63542,-1.7265,0.197368,1.93167,-2.14114,0.291879,1.86188,-1.99368,0.08597,1.90736,-2.03561,-0.034069,1.79928,-2.15879,-0.052998,1.70503,-2.22597,0.090892,1.57359,-2.19086,0.127189,1.51787,-2.21069,0.199999,1.42204,-1.81071,0.014304,1.49871,-1.88954,0.013583,1.52859,-1.93532,0.017446,1.50217,-2.00561,0.203827,1.45152,-2.0664,0.151869,1.09336, [...]
+/*2093*/{0.251139,1.91949,-1.76706,0.29552,1.86187,-1.92264,0.200274,1.77485,-1.64797,0.163028,1.65747,-1.56815,0.281269,1.64303,-1.67761,0.359792,1.62626,-1.72529,0.196548,1.92795,-2.1417,0.292034,1.85914,-1.99362,0.084924,1.9014,-2.0383,-0.031984,1.79292,-2.16248,-0.050499,1.69719,-2.22873,0.095553,1.56701,-2.19314,0.132765,1.51083,-2.21093,0.200857,1.419,-1.81115,0.015182,1.495,-1.8885,0.015058,1.5235,-1.93502,0.016878,1.49824,-2.00565,0.203658,1.44854,-2.06714,0.156285,1.0906,-1.7921 [...]
+/*2094*/{0.249927,1.91637,-1.7663,0.295243,1.85744,-1.92219,0.1965,1.77135,-1.64756,0.157851,1.65448,-1.56819,0.276643,1.63632,-1.67689,0.355563,1.61698,-1.72333,0.19634,1.92382,-2.14225,0.291503,1.85601,-1.99378,0.082786,1.89905,-2.03782,-0.031425,1.78451,-2.16581,-0.046884,1.68875,-2.23116,0.099264,1.5597,-2.19459,0.137502,1.50483,-2.21101,0.201295,1.41615,-1.81225,0.014821,1.49044,-1.88826,0.01466,1.51952,-1.93531,0.017377,1.49345,-2.00442,0.203335,1.4455,-2.06826,0.161091,1.08898,-1. [...]
+/*2095*/{0.247704,1.91287,-1.76574,0.294058,1.85436,-1.92216,0.19261,1.76788,-1.64752,0.150478,1.65221,-1.57007,0.271899,1.62913,-1.67615,0.349557,1.60746,-1.72106,0.195451,1.91867,-2.14296,0.291468,1.85212,-1.99337,0.08168,1.89399,-2.03914,-0.030571,1.77623,-2.16962,-0.043348,1.68009,-2.23384,0.104817,1.55409,-2.19618,0.143098,1.49817,-2.21122,0.202458,1.41297,-1.81284,0.015204,1.48564,-1.88849,0.014438,1.51487,-1.93497,0.017077,1.48883,-2.00383,0.203619,1.44182,-2.06977,0.16568,1.08566 [...]
+/*2096*/{0.245704,1.9083,-1.76493,0.294917,1.84982,-1.92138,0.188282,1.76404,-1.64767,0.144987,1.64873,-1.56986,0.266526,1.62258,-1.67582,0.344424,1.59782,-1.71871,0.195098,1.91402,-2.14357,0.290997,1.84852,-1.99354,0.081958,1.88915,-2.04213,-0.028639,1.76739,-2.17225,-0.040365,1.67085,-2.23699,0.10927,1.54687,-2.19715,0.148768,1.49168,-2.2113,0.203442,1.40948,-1.81426,0.015007,1.48158,-1.88799,0.015378,1.51016,-1.93469,0.01589,1.48407,-2.00332,0.20291,1.43869,-2.07056,0.169327,1.08263,- [...]
+/*2097*/{0.243953,1.90405,-1.76429,0.294547,1.84588,-1.92108,0.184952,1.75986,-1.64763,0.139985,1.64583,-1.5703,0.260397,1.6149,-1.67481,0.337708,1.58863,-1.71703,0.194612,1.90889,-2.1445,0.290357,1.84373,-1.99357,0.080294,1.88422,-2.04323,-0.02766,1.75849,-2.17641,-0.036326,1.66162,-2.23974,0.114446,1.53937,-2.19823,0.154235,1.4844,-2.21142,0.205332,1.4061,-1.8156,0.015436,1.47619,-1.88723,0.015099,1.5046,-1.93416,0.015044,1.47869,-2.0023,0.202551,1.43562,-2.07219,0.174257,1.07743,-1.78 [...]
+/*2098*/{0.241501,1.89937,-1.76381,0.295021,1.84068,-1.92114,0.180599,1.75526,-1.64759,0.132124,1.64204,-1.57039,0.254541,1.60718,-1.67409,0.33243,1.57888,-1.7148,0.195275,1.90362,-2.1454,0.290517,1.83959,-1.9938,0.07933,1.87726,-2.04493,-0.027019,1.7487,-2.18071,-0.03214,1.65181,-2.24242,0.120382,1.53185,-2.19891,0.16112,1.47791,-2.21175,0.205235,1.40196,-1.81678,0.016343,1.47089,-1.88689,0.01457,1.50034,-1.93432,0.014022,1.47338,-2.00187,0.202206,1.4315,-2.07357,0.178402,1.07415,-1.781 [...]
+/*2099*/{0.240463,1.89472,-1.7627,0.294644,1.83601,-1.91959,0.176736,1.75062,-1.64734,0.126266,1.6385,-1.57127,0.248291,1.60008,-1.67318,0.325479,1.56952,-1.71243,0.194218,1.89816,-2.14636,0.290309,1.83483,-1.99349,0.077889,1.87122,-2.04586,-0.023181,1.73892,-2.18484,-0.0283,1.64204,-2.24563,0.125611,1.52493,-2.19945,0.167117,1.4707,-2.21168,0.206308,1.39767,-1.81843,0.016056,1.46562,-1.8864,0.014697,1.49458,-1.93501,0.014271,1.46813,-2.00164,0.20204,1.42739,-2.07551,0.182223,1.07107,-1. [...]
+/*2100*/{0.238958,1.88986,-1.76197,0.29412,1.82995,-1.9185,0.172962,1.74611,-1.6471,0.119802,1.6356,-1.57197,0.241562,1.59345,-1.67195,0.318343,1.56083,-1.71049,0.194764,1.89258,-2.14723,0.290764,1.83008,-1.99313,0.0769,1.86439,-2.04789,-0.023112,1.72962,-2.18934,-0.023524,1.63216,-2.24847,0.131742,1.51771,-2.20044,0.174234,1.46386,-2.212,0.207051,1.39344,-1.81986,0.015449,1.45988,-1.88574,0.014064,1.48902,-1.93562,0.012775,1.46344,-2.00098,0.201018,1.4228,-2.0777,0.18616,1.06802,-1.7755 [...]
+/*2101*/{0.237697,1.88417,-1.76079,0.295083,1.82525,-1.91815,0.168404,1.74137,-1.64786,0.111999,1.63145,-1.5724,0.23509,1.58639,-1.67007,0.310859,1.55221,-1.70761,0.194846,1.88738,-2.148,0.291749,1.82472,-1.99263,0.075339,1.85844,-2.04897,-0.0202,1.71881,-2.19363,-0.018306,1.622,-2.25098,0.138194,1.50994,-2.2006,0.181347,1.45747,-2.21221,0.20727,1.3888,-1.82185,0.014897,1.45425,-1.8853,0.012692,1.48296,-1.93497,0.010169,1.45919,-2.00054,0.199916,1.41849,-2.07911,0.189155,1.06339,-1.77469 [...]
+/*2102*/{0.236161,1.87879,-1.7602,0.295279,1.82012,-1.91677,0.165477,1.73674,-1.648,0.105076,1.62893,-1.57349,0.228262,1.5801,-1.66883,0.303427,1.54409,-1.70514,0.195692,1.88198,-2.14925,0.29132,1.81973,-1.99277,0.074854,1.85128,-2.05066,-0.017263,1.70875,-2.19875,-0.013361,1.61172,-2.25324,0.145263,1.50298,-2.20157,0.188664,1.45061,-2.21214,0.207555,1.38397,-1.82296,0.013843,1.44944,-1.88559,0.010932,1.47733,-1.93483,0.007675,1.4549,-2.00028,0.197592,1.41458,-2.08043,0.193056,1.06082,-1 [...]
+/*2103*/{0.235403,1.8738,-1.75875,0.296617,1.81428,-1.91527,0.16149,1.73197,-1.64816,0.098071,1.62602,-1.57378,0.220737,1.57386,-1.66693,0.295697,1.5367,-1.70261,0.19639,1.87716,-2.15105,0.293236,1.81527,-1.99218,0.074949,1.84801,-2.0501,-0.012961,1.6982,-2.20343,-0.006789,1.60155,-2.25587,0.152075,1.49611,-2.20197,0.195565,1.44428,-2.21247,0.206933,1.3791,-1.82523,0.01202,1.44445,-1.88429,0.00913,1.47159,-1.93352,0.004825,1.45016,-1.99993,0.195108,1.41219,-2.08121,0.19872,1.05675,-1.769 [...]
+/*2104*/{0.234488,1.86823,-1.75785,0.297568,1.80878,-1.91503,0.157697,1.72752,-1.64856,0.091605,1.62372,-1.57493,0.213603,1.56836,-1.66622,0.28764,1.52976,-1.69982,0.196713,1.8722,-2.15206,0.293519,1.80965,-1.99197,0.075394,1.84528,-2.04831,-0.008218,1.6885,-2.21075,0.000451,1.5911,-2.25762,0.159864,1.48968,-2.20275,0.203988,1.43874,-2.21251,0.20637,1.37489,-1.82731,0.009701,1.4397,-1.88482,0.007328,1.46665,-1.93247,0.001883,1.4472,-2.00034,0.192669,1.41013,-2.08238,0.203402,1.05458,-1.7 [...]
+};
+
+
+static int getNumFrames() {return NFrames;}
+static int getNumObservations() {return NObservations;}
+static Real getFrameTime(int i) {return frameTimes[i];}
+static const char* const* getObservationOrder() {return observationOrder;}
+static ArrayViewConst_<Vec3> getFrame(int i) {
+ const Vec3* o = (const Vec3*)observations;
+ return ArrayViewConst_<Vec3>(o+i*getNumObservations(), o+(i+1)*getNumObservations());
+}
+
diff --git a/Simbody/examples/ExampleAssemblerPlayground.cpp b/Simbody/examples/ExampleAssemblerPlayground.cpp
new file mode 100644
index 0000000..cb1656c
--- /dev/null
+++ b/Simbody/examples/ExampleAssemblerPlayground.cpp
@@ -0,0 +1,273 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Assembler Playground *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* This example is for experimenting with the Assembler study that is used
+for initial assembly and for inverse kinematics (IK) such as repeated tracking
+of marker positions. This toy example is easy to understand and captures all
+the basics in a simplified model. For a realistic biomechanical model of about
+the same size but considerably more complexity, see the AmysIKProblem example.
+*/
+
+
+#include "Simbody.h"
+
+#include <cstdio>
+#include <exception>
+
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+static const int NBodies = 10;
+static const int NBodies2 = 7;
+
+int main() {
+ try
+ { // Create the system.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+
+ const Real mass = 0.1;
+ const Vec3 hdims(2,4,1);
+ Body::Rigid pendulumBody = Body::Rigid
+ (MassProperties(mass, Vec3(0), mass*UnitInertia::ellipsoid(hdims)));
+ pendulumBody.addDecoration(DecorativeEllipsoid(hdims).setOpacity(.2));
+
+ MobilizedBody midBody, finalBody;
+ MobilizedBody::Free baseBody(matter.Ground(), Transform(Vec3(0,-hdims[1],0)),
+ pendulumBody, Transform(Vec3(0,hdims[1],0)));
+
+ Force::TwoPointLinearSpring(forces, matter.Ground(), Vec3(0),
+ baseBody, Vec3(0), 2, 10);
+
+ Force::GlobalDamper(forces, matter, 1);
+
+
+ MobilizedBody parent = baseBody;
+ for (int i=0; i < NBodies; ++i) {
+ MobilizedBody::Ball child(parent, Transform(Vec3(0,-hdims[1],0)),
+ pendulumBody, Transform(Vec3(0,hdims[1],0)));
+ if (i==NBodies/2) midBody = child;
+ if (i==NBodies-1) finalBody = child;
+ parent = child;
+ }
+
+ // Second chain
+ MobilizedBody endOfSecondChain;
+ parent = matter.Ground();
+ Vec3 loc(-10,0,0);
+ for (int i=0; i < NBodies2; ++i) {
+ MobilizedBody::Ball child(parent, loc,
+ pendulumBody, Transform(Vec3(0,hdims[1],0)));
+ if (i==NBodies2-1) endOfSecondChain = child;
+ parent = child;
+ loc = Vec3(0,-hdims[1],0);
+ }
+
+ Force::TwoPointLinearSpring(forces, endOfSecondChain, Vec3(0),
+ midBody, Vec3(0), 2, 1);
+
+ Constraint pres[5];
+ for (int i=0; i<5; ++i) {
+ pres[i] = Constraint::PrescribedMotion(matter,
+ new Function::Constant(.1),
+ MobilizedBodyIndex(NBodies+2+i), MobilizerQIndex(2));
+ //pres[i].setDisabledByDefault(true);
+ }
+
+ //Constraint::Ball chainConnection(endOfSecondChain, Vec3(0,-hdims[1],0),
+ // matter.updMobilizedBody(MobilizedBodyIndex(4)), Vec3(0));
+
+ //Constraint::Ball ball(matter.Ground(), Vec3(3*NBodies,-NBodies,0),
+ // finalBody, Vec3(0,-hdims[1],0));
+
+ Visualizer viz(system);
+ system.addEventReporter(new Visualizer::Reporter(viz, 0.1));
+
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+
+ State state = system.getDefaultState();
+ //for (int i=0; i<5; ++i) pres[i].disable(state);
+
+ for (int i=0; i<5; ++i)
+ cout << "state pres[" << i << "].isDisabled="
+ << pres[i].isDisabled(state) << endl;
+
+ const Vec3 midTarget(1.5*NBodies,-NBodies*0.5*hdims[1],3);
+ const Vec3 finalTarget(2*NBodies, -NBodies*0.3*hdims[1],2);
+ viz.addDecoration(midBody, Vec3(0), DecorativeSphere(1).setColor(Red));
+ viz.addDecoration(midBody, Vec3(1,2,3), DecorativeSphere(1).setColor(Red));
+ viz.addDecoration(GroundIndex, midTarget,
+ DecorativeSphere(1).setColor(Green));
+ viz.addDecoration(finalBody, Vec3(0), DecorativeSphere(1).setColor(Red));
+ viz.addDecoration(finalBody, Vec3(1,2,3), DecorativeSphere(1).setColor(Red));
+ viz.addDecoration(GroundIndex, finalTarget,
+ DecorativeSphere(1).setColor(Green));
+
+ // Show initial configuration
+ viz.report(state);
+ State tempState = state;
+ for (int i=0; i<5; ++i)
+ cout << "tempState copy pres[" << i << "].isDisabled="
+ << pres[i].isDisabled(tempState) << endl;
+
+ system.realize(tempState, Stage::Position);
+ const int nQuat = matter.getNumQuaternionsInUse(tempState);
+ cout << "INITIAL CONFIGURATION\n";
+ cout << tempState.getNU() << " dofs, "
+ << tempState.getNQErr()-nQuat << " constraints.\n";
+
+ cout << "Type any character to continue:\n";
+ getchar();
+
+
+ Assembler ik(system);
+ Markers& markers = *new Markers();
+ ik.adoptAssemblyGoal(&markers);
+
+ QValue& qvalue = *new QValue(endOfSecondChain, MobilizerQIndex(2),
+ Pi/2);
+ ik.adoptAssemblyGoal(&qvalue);
+
+ // Set treatment of constraints.
+ //ik.setSystemConstraintsWeight(1); // means penalty rather than constraint
+
+ ik.lockMobilizer(MobilizedBodyIndex(2));
+ ik.lockMobilizer(MobilizedBodyIndex(3));
+ ik.lockMobilizer(MobilizedBodyIndex(4));
+
+ ik.restrictQ(midBody, MobilizerQIndex(0), -Pi/10, Pi/10);
+
+ markers.addMarker(midBody, Vec3(0));
+ markers.addMarker(midBody, Vec3(1,2,3));
+ markers.addMarker(finalBody, Vec3(0));
+ markers.addMarker(finalBody, Vec3(1,2,3));
+
+ // Manual observation/marker correspondence.
+ //Array_<Markers::MarkerIx> observationOrder;
+ //observationOrder.push_back(Markers::MarkerIx(2));
+ //observationOrder.push_back(Markers::MarkerIx(3));
+ //observationOrder.push_back(Markers::MarkerIx(0));
+ //observationOrder.push_back(Markers::MarkerIx(1));
+ //observationOrder.push_back(Markers::MarkerIx()); // unused
+ //observationOrder.push_back(Markers::MarkerIx());
+ //observationOrder.push_back(Markers::MarkerIx());
+ //observationOrder.push_back(Markers::MarkerIx());
+ //observationOrder.push_back(Markers::MarkerIx());
+ //markers.defineTargetOrder(observationOrder);
+
+
+ const Real Accuracy = 1e-3;
+ ik.setAccuracy(Accuracy);
+
+ ik.initialize(state);
+
+ markers.moveOneObservation(Markers::ObservationIx(0), midTarget);
+ markers.moveOneObservation(Markers::ObservationIx(1), midTarget);
+ markers.moveOneObservation(Markers::ObservationIx(2), finalTarget);
+ markers.moveOneObservation(Markers::ObservationIx(3), finalTarget);
+
+ for (Markers::MarkerIx mx(0); mx < markers.getNumMarkers(); ++mx)
+ printf("mx=%d ox=%d err=%g\n",
+ (int)mx, (int)markers.getObservationIxForMarker(mx),
+ markers.findCurrentMarkerError(mx));
+
+
+ ik.assemble(state);
+
+ viz.report(state);
+ cout << "ASSEMBLED CONFIGURATION\n";
+
+ cout << "Type any character to continue:\n";
+ getchar();
+
+ for (Markers::MarkerIx mx(0); mx < markers.getNumMarkers(); ++mx)
+ printf("mx=%d ox=%d err=%g\n",
+ (int)mx, (int)markers.getObservationIxForMarker(mx),
+ markers.findCurrentMarkerError(mx));
+
+ State startState = state;
+
+ const double startCPU = cpuTime(), startReal = realTime();
+
+ const int NSteps = 100;
+ const int NToSkip = 4;
+ for (int iters=0; iters <= NSteps; ++iters) {
+ Vec3 newMidTarget = midTarget + NBodies/2*std::sin(2*Pi*iters/100);
+ Vec3 newFinalTarget = finalTarget - NBodies/3*std::sin(2*Pi*iters/100);
+ markers.moveOneObservation(Markers::ObservationIx(0), newMidTarget);
+ markers.moveOneObservation(Markers::ObservationIx(1), newMidTarget);
+ markers.moveOneObservation(Markers::ObservationIx(2), newFinalTarget);
+ markers.moveOneObservation(Markers::ObservationIx(3), newFinalTarget);
+
+ qvalue.setValue(qvalue.getValue() + Pi/10);
+
+ ik.track();
+ if (iters%NToSkip == 0) {
+ ik.updateFromInternalState(state);
+ viz.report(state);
+ }
+ }
+
+ cout << "ASSEMBLED " << NSteps << " steps in "
+ << cpuTime()-startCPU << " CPU s, "
+ << realTime()-startReal << " REAL s\n";
+
+ cout << "DONE ASSEMBLING -- SIMULATE ...\n";
+ viz.report(state);
+
+ cout << "Type any character to continue:\n";
+ getchar();
+
+ // Simulate it.
+ for (int i=0; i<5; ++i)
+ cout << "state pres[" << i << "].isDisabled="
+ << pres[i].isDisabled(state) << endl;
+
+ RungeKuttaMersonIntegrator integ(system);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(10.0);
+
+ cout << "DONE SIMULATING.\n";
+ cout << "Type any character to quit:\n";
+ getchar();
+
+
+ } catch (const std::exception& e) {
+ std::printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+
+ } catch (...) {
+ std::printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+ return 0;
+}
diff --git a/Simbody/examples/ExampleBricardMechanism.cpp b/Simbody/examples/ExampleBricardMechanism.cpp
new file mode 100644
index 0000000..bab2642
--- /dev/null
+++ b/Simbody/examples/ExampleBricardMechanism.cpp
@@ -0,0 +1,216 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Bricard Mechanism *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Moonki Jung *
+ * *
+ * 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 "Simbody.h"
+
+#include <fstream>
+#include <iostream>
+
+using std::cout; using std::endl;
+
+
+using namespace SimTK;
+
+/* Note: this example is due to Moonki Jung from Seoul National University's
+Human-Centered CAD Laboratory. See SimTK Core open-discussion forum startin
+g 2/4/2010 with message 3292. Moonki kindly gave us permission to include this
+very difficult mechanism as a test case for Simbody and as a user example. The
+Bricard mechanism is particularly difficult because it has six mobilities and
+six constraints but has one net degree of freedom. Thus one of the constraints
+is redundant; but it is not any particular one. The mechanism must be perfectly
+aligned in order to move, but in practice we can only satisfy the position
+constraints approximately. This can hide the redundancy, causing the mechanism
+to appear to have zero dofs and thus lock up. */
+
+
+class EnergyReport : public PeriodicEventReporter {
+public:
+ EnergyReport(const MultibodySystem& system, Real interval)
+ : PeriodicEventReporter(interval), system(system) {}
+
+ void handleEvent(const State& s) const {
+ cout << "\n*** t=" << s.getTime()
+ << " ke=" << system.calcKineticEnergy(s)
+ << " E=" << system.calcEnergy(s)
+ << endl;
+ cout << " u=" << s.getU() << "\n";
+ cout << " uerr=" << s.getUErr() << "\n\n";
+ }
+private:
+ const MultibodySystem& system;
+};
+
+
+int main()
+{
+ try {
+ const String currentWorkingDir = Pathname::getCurrentWorkingDirectory();
+ std::cout << "Current working directory: " << currentWorkingDir << std::endl;
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::Gravity gravity(forces, matter, UnitVec3(0, -1, 0), 9.8);
+
+ const Real Mass = 20;
+ const Vec3 EvenCOM(1.00000000, -0.16416667, -0.16416667);
+ const Vec3 OddCOM(1.00000000, 0.16416667, -0.16416667);
+
+ const Inertia EvenCentralInertia
+ ( 3.33400000, 28.33366667, 28.33366667, // xx, yy, zz
+ -4.96666667, -1.60000000, -0.03333333); // xy, xz, yz
+
+ const Inertia OddCentralInertia
+ ( 3.33400000, 28.33366667, 28.33366667, // xx, yy, zz
+ 4.96666667, -1.60000000, 0.03333333); // xy, xz, yz
+
+ // Inertias must be given about the body origin.
+ const Inertia EvenBodyInertia =
+ EvenCentralInertia.shiftFromMassCenter(-EvenCOM, Mass);
+ const Inertia OddBodyInertia =
+ OddCentralInertia.shiftFromMassCenter(-OddCOM, Mass);
+
+ Body::Rigid EVEN_PART_1(MassProperties(Mass, EvenCOM, EvenBodyInertia));
+ Body::Rigid EVEN_PART_2(MassProperties(Mass, EvenCOM, EvenBodyInertia));
+ Body::Rigid EVEN_PART_3(MassProperties(Mass, EvenCOM, EvenBodyInertia));
+
+
+ Body::Rigid ODD_PART_1(MassProperties(Mass, OddCOM, OddBodyInertia));
+ Body::Rigid ODD_PART_2(MassProperties(Mass, OddCOM, OddBodyInertia));
+
+ // Split the last body and weld back together to close loop.
+ Body::Rigid ODD_PART_3_HALF1(MassProperties(Mass/2, OddCOM,
+ OddBodyInertia/2));
+ Body::Rigid ODD_PART_3_HALF2(MassProperties(Mass/2, OddCOM,
+ OddBodyInertia/2));
+
+ std::ifstream file1, file2;
+ PolygonalMesh Mesh1; file1.open("Bricard_EVEN_PART.obj");
+ if (!file1.good()) {
+ std::cout << "Couldn't open file 'Bricard_EVEN_PART.obj' in current working directory "
+ << currentWorkingDir << std::endl;
+ exit(1);
+ }
+ Mesh1.loadObjFile(file1); file1.close();
+ PolygonalMesh Mesh2; file2.open("Bricard_ODD_PART.obj");
+ if (!file2.good()) {
+ std::cout << "Couldn't open file 'Bricard_ODD_PART.obj' in current working directory "
+ << currentWorkingDir << std::endl;
+ exit(1);
+ }
+ Mesh2.loadObjFile(file2); file2.close();
+
+ EVEN_PART_1.addDecoration(Transform(), DecorativeMesh(Mesh1).setColor(Vec3(0.00000000, 1.00000000, 0.00000000)));
+ EVEN_PART_2.addDecoration(Transform(), DecorativeMesh(Mesh1).setColor(Vec3(1.00000000, 0.00000000, 1.00000000)));
+ EVEN_PART_3.addDecoration(Transform(), DecorativeMesh(Mesh1).setColor(Vec3(1.00000000, 1.00000000, 0.00000000)));
+ ODD_PART_1.addDecoration(Transform(), DecorativeMesh(Mesh2).setColor(Vec3(1.00000000, 0.00000000, 0.00000000)));
+ ODD_PART_2.addDecoration(Transform(), DecorativeMesh(Mesh2).setColor(Vec3(0.00000000, 0.00000000, 1.00000000)));
+ ODD_PART_3_HALF1.addDecoration(Transform(), DecorativeMesh(Mesh2).setColor(Vec3(0.00000000, 1.00000000, 1.00000000)));
+
+ MobilizedBody::Weld EVEN_PART_1_body(matter.updGround(), Transform(Rotation(Mat33(1,0,0,0,-1,0,0,0,-1)), Vec3(0, 0, 0))
+ ,EVEN_PART_1, Transform());
+ MobilizedBody::Pin ODD_PART_1_body(EVEN_PART_1_body, Transform(Rotation(Mat33(0,-1,0,1,0,0,0,0,1)), Vec3(0, 0, 0))
+ ,ODD_PART_1, Transform(Rotation(Mat33(0,-1,0,1,0,0,0,0,1)), Vec3(0,0,0)));
+
+ MobilizedBody::Pin EVEN_PART_2_body(ODD_PART_1_body, Transform(Rotation(Mat33(0,-1,0,0,0,1,-1,0,0)), Vec3(2, 0, 0))
+ ,EVEN_PART_2, Transform(Rotation(Mat33(0,-1,0,-1,0,0,0,0,-1)), Vec3(0,0,0)));
+
+ MobilizedBody::Pin ODD_PART_2_body(EVEN_PART_1_body, Transform(Rotation(Mat33(0,-1,0,0,0,1,-1,0,0)), Vec3(2, 0, 0))
+ ,ODD_PART_2, Transform(Rotation(Mat33(0,-1,0,1,0,0,0,0,1)), Vec3(0,0,0)));
+
+ MobilizedBody::Pin EVEN_PART_3_body(ODD_PART_2_body, Transform(Rotation(Mat33(0,-1,0,0,0,1,-1,0,0)), Vec3(2, 0, 0))
+ ,EVEN_PART_3, Transform(Rotation(Mat33(0,-1,0,0,0,-1,1,0,0)), Vec3(2,0,0)));
+
+ MobilizedBody::Pin ODD_PART_3_HALF1_body(EVEN_PART_3_body, Transform(Rotation(Mat33(0,-1,0,-1,0,0,0,0,-1)), Vec3(0, 0, 0))
+ ,ODD_PART_3_HALF1, Transform(Rotation(Mat33(0,-1,0,0,0,1,-1,0,0)), Vec3(2,0,0)));
+
+ MobilizedBody::Pin ODD_PART_3_HALF2_body(EVEN_PART_2_body, Transform(Rotation(Mat33(0,-1,0,0,0,1,-1,0,0)), Vec3(2, 0, 0))
+ ,ODD_PART_3_HALF2, Transform(Rotation(Mat33(0,-1,0,1,0,0,0,0,1)), Vec3(0,0,0)));
+
+ Constraint::Weld ODD_PART_3_UNION(ODD_PART_3_HALF1_body, Transform(), ODD_PART_3_HALF2_body, Transform());
+
+ //Constraint::ConstantSpeed motion(EVEN_PART_3_body, -.1);
+ //Force::MobilityLinearSpring frc(forces, EVEN_PART_3_body,
+ // MobilizerUIndex(0), 100, 0);
+
+ Visualizer viz(system);
+ viz.setCameraTransform(Vec3(0.5,0.5,0.5));
+ viz.pointCameraAt(Vec3(0), Vec3(0,1,0));
+ viz.setBackgroundType(Visualizer::SolidColor);
+ system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
+
+ system.addEventReporter(new EnergyReport(system, .01));
+ system.realizeTopology();
+ State state = system.getDefaultState();
+
+ // Set initial states (Q's and U's)
+ // Position
+ ODD_PART_1_body.setOneQ(state, 0, 180.0*Pi/180.0);
+ EVEN_PART_3_body.setOneQ(state, 0, 180.0*Pi/180.0);
+ ODD_PART_3_HALF2_body.setOneQ(state, 0, 0.0*Pi/180.0);
+
+ EVEN_PART_2_body.setOneQ(state, 0, -120.0*Pi/180.0);
+ ODD_PART_2_body.setOneQ(state, 0, -120.0*Pi/180.0);
+ ODD_PART_3_HALF1_body.setOneQ(state, 0, 120.0*Pi/180.0);
+
+ // Velocity
+ ODD_PART_1_body.setOneU(state,0, -11.2);
+
+ //RungeKuttaMersonIntegrator integ(system);
+ RungeKutta3Integrator integ(system);
+ //RungeKuttaFeldbergIntegrator integ(system);
+ //VerletIntegrator integ(system);
+ //CPodesIntegrator integ(system);
+
+ // Accuracy needs to be fairly tight to avoid lockup that would occur
+ // if the constraints were allowed to drift.
+ integ.setAccuracy(1e-5);
+ //integ.setConstraintTolerance(1e-8);
+ integ.initialize(state);
+
+
+ const double startCPU = cpuTime(), startReal = realTime();
+
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(20.0);
+
+ cout << "DONE. CPU=" << cpuTime()-startCPU
+ << "s, REAL=" << realTime()-startReal << "s\n";
+
+ printf("Used Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
+
+
+ } catch (const std::exception& e) {
+ std::cout << "std::exception: " << e.what() << std::endl;
+ } catch (...) {
+ std::cout << "UNKNOWN EXCEPTION\n";
+ }
+
+ return 0;
+}
+
diff --git a/Simbody/examples/ExampleCablePath.cpp b/Simbody/examples/ExampleCablePath.cpp
new file mode 100644
index 0000000..eb79638
--- /dev/null
+++ b/Simbody/examples/ExampleCablePath.cpp
@@ -0,0 +1,359 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Cable Path *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* Simbody ExampleCablePath
+This example shows how to use a CableTrackerSubsystem to follow the motion of
+a cable that connects two bodies and passes around obstacles. We'll then
+create a force element that generates spring forces that result from the
+stretching and stretching rate of the cable. The custom force element defined
+here duplicates the Simbody built-in CableSpring force element, and you can
+choose to use either one by changing a #define. */
+
+#include "Simbody.h"
+
+#include <cassert>
+#include <iostream>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+//#define USE_MY_CABLE_SPRING
+#ifdef USE_MY_CABLE_SPRING
+#define CABLE_SPRING MyCableSpring // defined below
+#else
+#define CABLE_SPRING CableSpring // Simbody built-in
+#endif
+
+#ifdef USE_MY_CABLE_SPRING
+// This force element implements an elastic cable of a given nominal length,
+// and a stiffness k that generates a k*x force opposing stretch beyond
+// the slack length. There is also a dissipation term (k*x)*c*xdot. We keep
+// track of dissipated power here so we can use conservation of energy to check
+// that the cable and force element aren't obviously broken.
+class MyCableSpringImpl : public Force::Custom::Implementation {
+public:
+ MyCableSpringImpl(const GeneralForceSubsystem& forces,
+ const CablePath& path,
+ Real stiffness, Real nominal, Real damping)
+ : forces(forces), path(path), k(stiffness), x0(nominal), c(damping)
+ { assert(stiffness >= 0 && nominal >= 0 && damping >= 0); }
+
+ const CablePath& getCablePath() const {return path;}
+
+ // Must be at stage Velocity. Evalutes tension if necessary.
+ Real getTension(const State& state) const {
+ ensureTensionCalculated(state);
+ return Value<Real>::downcast(forces.getCacheEntry(state, tensionx));
+ }
+
+ // Must be at stage Velocity.
+ Real getPowerDissipation(const State& state) const {
+ const Real stretch = calcStretch(state);
+ if (stretch == 0) return 0;
+ const Real rate = path.getCableLengthDot(state);
+ return k*stretch*std::max(c*rate, -1.)*rate;
+ }
+
+ // This integral is always available.
+ Real getDissipatedEnergy(const State& state) const {
+ return forces.getZ(state)[workx];
+ }
+
+ //--------------------------------------------------------------------------
+ // Custom force virtuals
+
+ // Ask the cable to apply body forces given the tension calculated here.
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+ OVERRIDE_11
+ { path.applyBodyForces(state, getTension(state), bodyForces); }
+
+ // Return the potential energy currently stored by the stretch of the cable.
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11 {
+ const Real stretch = calcStretch(state);
+ if (stretch == 0) return 0;
+ return k*square(stretch)/2;
+ }
+
+ // Allocate the state variable for tracking dissipated energy, and a
+ // cache entry to hold the calculated tension.
+ void realizeTopology(State& state) const OVERRIDE_11 {
+ Vector initWork(1, 0.);
+ workx = forces.allocateZ(state, initWork);
+ tensionx = forces.allocateLazyCacheEntry(state, Stage::Velocity,
+ new Value<Real>(NaN));
+ }
+
+ // Report power dissipation as the derivative for the work variable.
+ void realizeAcceleration(const State& state) const OVERRIDE_11 {
+ Real& workDot = forces.updZDot(state)[workx];
+ workDot = getPowerDissipation(state);
+ }
+ //--------------------------------------------------------------------------
+
+private:
+ // Return the amount by which the cable is stretched beyond its nominal
+ // length or zero if the cable is slack. Must be at stage Position.
+ Real calcStretch(const State& state) const {
+ const Real stretch = path.getCableLength(state) - x0;
+ return std::max(stretch, 0.);
+ }
+
+ // Must be at stage Velocity to calculate tension.
+ Real calcTension(const State& state) const {
+ const Real stretch = calcStretch(state);
+ if (stretch == 0) return 0;
+ const Real rate = path.getCableLengthDot(state);
+ if (c*rate < -1)
+ cout << "c*rate=" << c*rate << "; limited to -1\n";
+ const Real tension = k*stretch*(1+std::max(c*rate,-1.));
+ return tension;
+ }
+
+ // If state is at stage Velocity, we can calculate and store tension
+ // in the cache if it hasn't already been calculated.
+ void ensureTensionCalculated(const State& state) const {
+ if (forces.isCacheValueRealized(state, tensionx))
+ return;
+ Value<Real>::updDowncast(forces.updCacheEntry(state, tensionx))
+ = calcTension(state);
+ forces.markCacheValueRealized(state, tensionx);
+ }
+
+ const GeneralForceSubsystem& forces;
+ CablePath path;
+ Real k, x0, c;
+ mutable ZIndex workx;
+ mutable CacheEntryIndex tensionx;
+};
+
+// A nice handle to hide most of the cable spring implementation. This defines
+// a user's API.
+class MyCableSpring : public Force::Custom {
+public:
+ MyCableSpring(GeneralForceSubsystem& forces, const CablePath& path,
+ Real stiffness, Real nominal, Real damping)
+ : Force::Custom(forces, new MyCableSpringImpl(forces,path,
+ stiffness,nominal,damping))
+ {}
+
+ // Expose some useful methods.
+ const CablePath& getCablePath() const
+ { return getImpl().getCablePath(); }
+ Real getTension(const State& state) const
+ { return getImpl().getTension(state); }
+ Real getPowerDissipation(const State& state) const
+ { return getImpl().getPowerDissipation(state); }
+ Real getDissipatedEnergy(const State& state) const
+ { return getImpl().getDissipatedEnergy(state); }
+
+private:
+ const MyCableSpringImpl& getImpl() const
+ { return dynamic_cast<const MyCableSpringImpl&>(getImplementation()); }
+};
+#endif
+
+// This gets called periodically to dump out interesting things about
+// the cables and the system as a whole. It also saves states so that we
+// can play back at the end.
+static Array_<State> saveStates;
+class ShowStuff : public PeriodicEventReporter {
+public:
+ ShowStuff(const MultibodySystem& mbs,
+ const CABLE_SPRING& cable1,
+ const CABLE_SPRING& cable2, Real interval)
+ : PeriodicEventReporter(interval),
+ mbs(mbs), cable1(cable1), cable2(cable2) {}
+
+ static void showHeading(std::ostream& o) {
+ printf("%8s %10s %10s %10s %10s %10s %10s %10s %10s %12s\n",
+ "time", "length", "rate", "integ-rate", "unitpow", "tension", "disswork",
+ "KE", "PE", "KE+PE-W");
+ }
+
+ /** This is the implementation of the EventReporter virtual. **/
+ void handleEvent(const State& state) const OVERRIDE_11 {
+ const CablePath& path1 = cable1.getCablePath();
+ const CablePath& path2 = cable2.getCablePath();
+ printf("%8g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g CPU=%g\n",
+ state.getTime(),
+ path1.getCableLength(state),
+ path1.getCableLengthDot(state),
+ path1.getIntegratedCableLengthDot(state),
+ path1.calcCablePower(state, 1), // unit power
+ cable1.getTension(state),
+ cable1.getDissipatedEnergy(state),
+ cpuTime());
+ printf("%8s %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %12.6g\n",
+ "",
+ path2.getCableLength(state),
+ path2.getCableLengthDot(state),
+ path2.getIntegratedCableLengthDot(state),
+ path2.calcCablePower(state, 1), // unit power
+ cable2.getTension(state),
+ cable2.getDissipatedEnergy(state),
+ mbs.calcKineticEnergy(state),
+ mbs.calcPotentialEnergy(state),
+ mbs.calcEnergy(state)
+ + cable1.getDissipatedEnergy(state)
+ + cable2.getDissipatedEnergy(state));
+ saveStates.push_back(state);
+ }
+private:
+ const MultibodySystem& mbs;
+ CABLE_SPRING cable1, cable2;
+};
+
+int main() {
+ try {
+ // Create the system.
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ CableTrackerSubsystem cables(system);
+ GeneralForceSubsystem forces(system);
+
+ system.setUseUniformBackground(true); // no ground plane in display
+
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ //Force::GlobalDamper(forces, matter, 5);
+
+ Body::Rigid someBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ const Real Rad = .25;
+
+ Body::Rigid biggerBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ const Real BiggerRad = .5;
+
+ const Vec3 radii(.4, .25, .15);
+ Body::Rigid ellipsoidBody(MassProperties(1.0, Vec3(0),
+ 1.*UnitInertia::ellipsoid(radii)));
+
+ const Real CylRad = .3, HalfLen = .5;
+ Body::Rigid cylinderBody(MassProperties(1.0, Vec3(0),
+ 1.*UnitInertia::cylinderAlongX(Rad,HalfLen)));
+
+ Body::Rigid fancyBody = biggerBody; // NOT USING ELLIPSOID
+
+ MobilizedBody Ground = matter.Ground();
+
+ MobilizedBody::Ball body1(Ground, Transform(Vec3(0)),
+ someBody, Transform(Vec3(0, 1, 0)));
+ MobilizedBody::Ball body2(body1, Transform(Vec3(0)),
+ someBody, Transform(Vec3(0, 1, 0)));
+ MobilizedBody::Ball body3(body2, Transform(Vec3(0)),
+ someBody, Transform(Vec3(0, 1, 0)));
+ MobilizedBody::Ball body4(body3, Transform(Vec3(0)),
+ fancyBody, Transform(Vec3(0, 1, 0)));
+ MobilizedBody::Ball body5(body4, Transform(Vec3(0)),
+ someBody, Transform(Vec3(0, 1, 0)));
+
+ CablePath path1(cables, body1, Vec3(Rad,0,0), // origin
+ body5, Vec3(0,0,Rad)); // termination
+
+ CableObstacle::ViaPoint p1(path1, body2, Rad*UnitVec3(1,1,0));
+ //CableObstacle::ViaPoint p2(path1, body3, Rad*UnitVec3(0,1,1));
+ //CableObstacle::ViaPoint p3(path1, body3, Rad*UnitVec3(1,0,1));
+ CableObstacle::Surface obs4(path1, body3, Transform(),
+ ContactGeometry::Sphere(Rad));
+ //obs4.setContactPointHints(Rad*UnitVec3(-1,1,0),Rad*UnitVec3(-1,0,1));
+ obs4.setContactPointHints(Rad*UnitVec3(-.25,.04,0.08),
+ Rad*UnitVec3(-.05,-.25,-.04));
+
+ //CableObstacle::ViaPoint p4(path1, body4, Rad*UnitVec3(0,1,1));
+ //CableObstacle::ViaPoint p5(path1, body4, Rad*UnitVec3(1,0,1));
+ CableObstacle::Surface obs5(path1, body4,
+ // Transform(), ContactGeometry::Ellipsoid(radii));
+ //Rotation(Pi/2, YAxis), ContactGeometry::Cylinder(CylRad)); // along y
+ //Transform(), ContactGeometry::Sphere(Rad));
+ Transform(), ContactGeometry::Sphere(BiggerRad));
+ //obs5.setContactPointHints(Rad*UnitVec3(0,-1,-1),Rad*UnitVec3(0.1,-1,-1));
+ obs5.setContactPointHints(Rad*UnitVec3(.1,.125,-.2),
+ Rad*UnitVec3(0.1,-.1,-.2));
+
+ CABLE_SPRING cable1(forces, path1, 100., 3.5, 0.1);
+
+ CablePath path2(cables, Ground, Vec3(-3,0,0), // origin
+ Ground, Vec3(-2,1,0)); // termination
+ CableObstacle::ViaPoint(path2, body3, 2*Rad*UnitVec3(1,1,1));
+ CableObstacle::ViaPoint(path2, Ground, Vec3(-2.5,1,0));
+ CABLE_SPRING cable2(forces, path2, 100., 2, 0.1);
+
+
+ //obs1.setPathPreferencePoint(Vec3(2,3,4));
+ //obs1.setDecorativeGeometry(DecorativeSphere(0.25).setOpacity(.5));
+
+ Visualizer viz(system);
+ viz.setShowFrameNumber(true);
+ system.addEventReporter(new Visualizer::Reporter(viz, 0.1*1./30));
+ system.addEventReporter(new ShowStuff(system, cable1, cable2, 0.1*0.1));
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ Random::Gaussian random;
+ for (int i = 0; i < state.getNQ(); ++i)
+ state.updQ()[i] = random.getValue();
+ for (int i = 0; i < state.getNU(); ++i)
+ state.updU()[i] = 0.1*random.getValue();
+
+ system.realize(state, Stage::Position);
+ viz.report(state);
+ cout << "path1 init length=" << path1.getCableLength(state) << endl;
+ cout << "path2 init length=" << path2.getCableLength(state) << endl;
+ cout << "Hit ENTER ...";
+ getchar();
+
+ path1.setIntegratedCableLengthDot(state, path1.getCableLength(state));
+ path2.setIntegratedCableLengthDot(state, path2.getCableLength(state));
+
+
+ // Simulate it.
+ saveStates.clear(); saveStates.reserve(2000);
+
+ RungeKuttaMersonIntegrator integ(system);
+ //CPodesIntegrator integ(system);
+ integ.setAccuracy(1e-3);
+ //integ.setAccuracy(1e-6);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ShowStuff::showHeading(cout);
+
+ const Real finalTime = 2;
+ const double startTime = realTime();
+ ts.stepTo(finalTime);
+ cout << "DONE with " << finalTime
+ << "s simulated in " << realTime()-startTime
+ << "s elapsed.\n";
+
+
+ while (true) {
+ cout << "Hit ENTER FOR REPLAY, Q to quit ...";
+ const char ch = getchar();
+ if (ch=='q' || ch=='Q') break;
+ for (unsigned i=0; i < saveStates.size(); ++i)
+ viz.report(saveStates[i]);
+ }
+
+ } catch (const std::exception& e) {
+ cout << "EXCEPTION: " << e.what() << "\n";
+ }
+}
diff --git a/Simbody/examples/ExampleClosedTopologyMechanism.cpp b/Simbody/examples/ExampleClosedTopologyMechanism.cpp
new file mode 100644
index 0000000..7cc681e
--- /dev/null
+++ b/Simbody/examples/ExampleClosedTopologyMechanism.cpp
@@ -0,0 +1,216 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: ClosedTopologyMechanism *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "Simbody.h"
+using namespace SimTK;
+
+/* This example shows two different ways to create a closed-topology mechanism:
+by cutting a joint and by cutting a body. Generally it is easier to cut a body
+because then all joints are treated the same way, and the only constraint
+needed is a Weld constraint to glue the broken body back together.
+
+The mechanism here is a spatial crank/rocker with three bodies. We're using
+a ground frame orientation in which Y is up, X is right, and Z is pointing
+out of the screen at the viewer. The crank is a disk we'll call "rotor", with
+its axis connected by a Pin mobilizer (1 dof revolute internal coordinate
+joint) to ground along the X axis. The rocker is long thin rectangle pinned
+at the top to ground along the Z axis, and initially hanging down in the -Y
+direction. Finally there is a "linker" body connected to a point on the
+surface of the rotor by a Ball mobilizer (3 dof spherical internal coordinate
+joint). The linker will connect to the bottom of the rocker by another ball
+joint, which gives the mechanism a closed topology. One option is to implement
+this final joint directly using a Ball constraint (3 constraint equations).
+Another is to split the linker into two overlapping halves, linkerA and linkerB
+with half the mass properties each, connect linkerB to the rocker with a Ball
+mobilizer, and then glue the two halves back together with a Weld constraint
+(6 constraint equations). We'll do it both ways here and then perform a dynamic
+simulation (applying a torque to the rotor) and output the final state to
+show that the mechanisms are equivalent.
+
+Here the choice of which approach is better is easy -- the direct approach
+gives us a 5 dof tree system and 3 constraints; the second gives an
+8 dof tree system and 6 constraints which is much bigger. And the Ball
+constraint is a Simbody built-in and no harder to work with than a Ball
+mobilizer. But in general the split-body approach can be very appealing --
+for example, if the loop were closed with a pin joint (a very simple
+mobilizer) the corresponding awkward constraint would have 5 equations, isn't
+a built-in, would have no convenient axis along which to apply forces or
+measure rotations, and special treatment would be required to handle multiple
+rotations if they were to be tracked.
+
+(In case you're counting, yes this mechanism has two net degrees of freedom
+because the two ball joints permit the linker to rotate about its long axis.)
+*/
+
+int main() {
+ try { // catch errors if any
+
+ // --------------------------------------------------------------
+ // Create the system, with subsystems for bodies and some forces.
+ // --------------------------------------------------------------
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::Gravity(forces, matter, -YAxis, 9.8);
+
+ // The rotor will be centered at (0,0,0).
+ const Vec3 RockerLocation = Vec3(1.75,1.5,0); // Where to pin rocker.
+ const Real TorqueOnRotor = 4; // How hard to crank.
+ const Vec3 Mech2Offset = Vec3(5,0,0); // Shift for 2nd mechanism.
+
+ // Some useful rotation matrices we'll need below.
+ const Rotation YtoX(-Pi/2, ZAxis); // Reorient to put Y where X was.
+ const Rotation ZtoX( Pi/2, YAxis); // Reorient to put Z where X was.
+
+ // --------------------------------------------------------------
+ // Define body information. These are just convenient collections
+ // of related information useful for constructing the Mobilized
+ // Bodies that actually comprise the multibody system.
+ // --------------------------------------------------------------
+ // Define body "rotor".
+ Real rotorMass = 10, rotorRadius = 1, rotorHalfThickness = .1;
+ Body::Rigid rotorInfo(MassProperties(rotorMass, Vec3(0),
+ UnitInertia::cylinderAlongX(rotorRadius, rotorHalfThickness)));
+ rotorInfo.addDecoration(YtoX,
+ DecorativeCylinder(rotorRadius, rotorHalfThickness)); // along Y
+
+ // Define body "rocker".
+ Real rockerMass = 3; Vec3 rockerHalfDims(.1, 1, .1);
+ Body::Rigid rockerInfo(MassProperties(rockerMass, Vec3(0),
+ UnitInertia::brick(rockerHalfDims)));
+ rockerInfo.addDecoration(Vec3(0),
+ DecorativeBrick(rockerHalfDims).setColor(Red));
+
+ // Define a full "linker" for mechanism 1.
+ Real linkerMass = 0.5; Vec3 linkerHalfDims(.6, .05, .05);
+ Body::Rigid linkerInfo(MassProperties(linkerMass, Vec3(0),
+ UnitInertia::brick(linkerHalfDims)));
+ linkerInfo.addDecoration(Vec3(0),
+ DecorativeBrick(linkerHalfDims).setColor(Blue));
+
+ // Define a half "linker" for mechanism 2 (we'll use it twice). The only
+ // change is to use half the mass (since UnitInertia gets scaled by mass).
+ Body::Rigid halfLinkerInfo(MassProperties(linkerMass/2, Vec3(0),
+ UnitInertia::brick(linkerHalfDims)));
+ halfLinkerInfo.addDecoration(Vec3(0),
+ DecorativeBrick(linkerHalfDims).setColor(Blue).setOpacity(0.5));
+
+ // --------------------------------------------------------------
+ // MECHANISM 1 (Ball constraint)
+ // --------------------------------------------------------------
+ // Note that the Pin mobilizer is defined to rotate about local Z so we
+ // need to create local frames with Z pointing along the bodies' X.
+ MobilizedBody::Pin rotor1(matter.Ground(), ZtoX,
+ rotorInfo, ZtoX);
+ MobilizedBody::Pin rocker1(matter.Ground(), RockerLocation,
+ rockerInfo, Vec3(0,rockerHalfDims[1],0));
+ MobilizedBody::Ball linker1
+ (rotor1, Vec3(rotorHalfThickness,0,.8*rotorRadius),
+ linkerInfo, Vec3(-linkerHalfDims[0],0,0));
+
+ // Add a ball constraint instead of a ball mobilizer to connect
+ // linker to rocker.
+ Constraint::Ball(linker1, Vec3(linkerHalfDims[0],0,0),
+ rocker1, Vec3(0,-rockerHalfDims[1],0));
+
+ // Apply a constant torque about the rotor's Pin axis.
+ Force::MobilityConstantForce(forces, rotor1, 0, TorqueOnRotor);
+
+ // --------------------------------------------------------------
+ // MECHANISM 2 (Split linker + Weld constraint)
+ // --------------------------------------------------------------
+ MobilizedBody::Pin rotor2(matter.Ground(), Transform(ZtoX,Mech2Offset),
+ rotorInfo, ZtoX);
+ MobilizedBody::Pin rocker2(matter.Ground(), RockerLocation+Mech2Offset,
+ rockerInfo, Vec3(0,rockerHalfDims[1],0));
+ // First half-linker connects to the rotor just as above.
+ MobilizedBody::Ball linker2a
+ (rotor2, Vec3(rotorHalfThickness,0,.8*rotorRadius),
+ halfLinkerInfo, Vec3(-linkerHalfDims[0],0,0));
+ // Second half-linker connects to the rocker at the other end.
+ MobilizedBody::Ball linker2b
+ (rocker2, Vec3(0,-rockerHalfDims[1],0),
+ halfLinkerInfo, Vec3(linkerHalfDims[0],0,0));
+
+ // Now add a weld constraint to glue the half-linkers together. We're
+ // taking the default which places the Weld at the body origins.
+ Constraint::Weld(linker2a, linker2b);
+
+ // Apply the same constant torque about the rotor's Pin axis.
+ Force::MobilityConstantForce(forces, rotor2, 0, TorqueOnRotor);
+
+
+ // --------------------------------------------------------------
+ // SET UP VISUALIZATION, RUN SIMULATION
+ // --------------------------------------------------------------
+ // Ask for visualization every 1/30 second.
+ system.setUseUniformBackground(true); // turn off floor
+ Visualizer viz(system);
+ system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
+
+ // Initialize the system and obtain the default state.
+ State state = system.realizeTopology();
+
+ viz.report(state); // draw frame
+ printf("Not yet assembled ... (hit ENTER)\n");
+ getchar();
+
+ // Assemble both systems into identical configurations to a tight
+ // tolerance. (Note that the linker is free to rotate about its long
+ // axis so might not come out exactly the same but it doesn't matter.)
+ Assembler assembler(system);
+ assembler.lockMobilizer(rotor1);
+ assembler.lockMobilizer(rotor2);
+ assembler.setAccuracy(1e-10);
+ assembler.assemble(state);
+
+ viz.report(state);
+ printf("Assembled ... (hit ENTER)\n");
+ getchar();
+
+ // Simulate for 10 seconds.
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setAccuracy(1e-5); // default is 1e-3
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(10);
+ state = ts.getState(); // retrieve final state
+
+ // --------------------------------------------------------------
+ // OUTPUT FINAL ANGLES AND RATES - should match to accuracy.
+ // --------------------------------------------------------------
+ printf("Final angles: rotor1=%g, rocker1=%g\n",
+ rotor1.getAngle(state), rocker1.getAngle(state));
+ printf(" roter2=%g, rocker2=%g\n",
+ rotor2.getAngle(state), rocker2.getAngle(state));
+
+ printf("Final rates: rotor1=%g, rocker1=%g\n",
+ rotor1.getRate(state), rocker1.getRate(state));
+ printf(" roter2=%g, rocker2=%g\n",
+ rotor2.getRate(state), rocker2.getRate(state));
+
+ } catch (const std::exception& e) {
+ std::cout << "ERROR: " << e.what() << std::endl;
+ return 1;
+ }
+ return 0;
+}
diff --git a/Simbody/examples/ExampleContactPlayground.cpp b/Simbody/examples/ExampleContactPlayground.cpp
new file mode 100644
index 0000000..6304a1c
--- /dev/null
+++ b/Simbody/examples/ExampleContactPlayground.cpp
@@ -0,0 +1,626 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Contact Playground *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* This example is for experimenting with the new Simbody contact implementation,
+which was in beta test in the Simbody 2.1 release, with first official release
+in Simbody 2.2. The previous contact implementation is still present and
+functional but will be removed soon.
+
+The example shows how the new system tracks contact events and how you can
+extract contact forces. It also shows off a number of features of the new
+Simbody Visualizer, new in release 2.2. also. Here we display the forces and
+torques as colored lines which remain the same color as long as a particular
+contact event continues. We also track the energy dissipated by the contacts
+and use it to display an energy quantity that should be conserved throughout
+the simulation (that is, the current energy plus the dissipated energy
+should be a constant).
+
+The simulation uses very expensive, detailed contact surfaces using dense
+meshes and the elastic foundation model. Consequently it runs with highly
+variable step sizes, and fails to keep up with real time for some short
+periods. We use the Visualizer's RealTime mode to buffer up some frames and
+smooth out these rough spots so the simulation appears to run at an almost
+steady real time rate, displayed at 30fps (depending on how fast your
+computer is). Then at the end you can watch the action replay.
+
+You can use this example to see how the different integrators behave when
+confronted with a very stiff problem; depending on material properties CPodes
+can be *much* faster than the explicit integrators, and it also exhibits very
+high stability after the motion damps out. However, by changing material
+properties and accuracy setting you can get reasonably good performance out
+of the explicit integrators here, which will scale better to large systems.
+*/
+
+#include "Simbody.h"
+
+#include <cstdio>
+#include <exception>
+#include <algorithm>
+#include <iostream>
+#include <fstream>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+Array_<State> saveEm;
+
+static const Real TimeScale = 1;
+static const Real FrameRate = 30;
+static const Real ReportInterval = TimeScale/FrameRate;
+static const Real ForceScale = .25;
+static const Real MomentScale = .5;
+
+
+class ForceArrowGenerator : public DecorationGenerator {
+public:
+ ForceArrowGenerator(const MultibodySystem& system,
+ const CompliantContactSubsystem& complCont)
+ : m_system(system), m_compliant(complCont) {}
+
+ virtual void generateDecorations(const State& state, Array_<DecorativeGeometry>& geometry) {
+ const Vec3 frcColors[] = {Red,Orange,Cyan};
+ const Vec3 momColors[] = {Blue,Green,Purple};
+ m_system.realize(state, Stage::Velocity);
+
+ const int ncont = m_compliant.getNumContactForces(state);
+ for (int i=0; i < ncont; ++i) {
+ const ContactForce& force = m_compliant.getContactForce(state,i);
+ const ContactId id = force.getContactId();
+ const Vec3& frc = force.getForceOnSurface2()[1];
+ const Vec3& mom = force.getForceOnSurface2()[0];
+ Real frcMag = frc.norm(), momMag=mom.norm();
+ int frcThickness = 1, momThickness = 1;
+ Real frcScale = ForceScale, momScale = ForceScale;
+ while (frcMag > 10)
+ frcThickness++, frcScale /= 10, frcMag /= 10;
+ while (momMag > 10)
+ momThickness++, momScale /= 10, momMag /= 10;
+ DecorativeLine frcLine(force.getContactPoint(),
+ force.getContactPoint() + frcScale*frc);
+ DecorativeLine momLine(force.getContactPoint(),
+ force.getContactPoint() + momScale*mom);
+ frcLine.setColor(frcColors[id%3]);
+ momLine.setColor(momColors[id%3]);
+ frcLine.setLineThickness(2*frcThickness);
+ momLine.setLineThickness(2*momThickness);
+ geometry.push_back(frcLine);
+ geometry.push_back(momLine);
+
+ ContactPatch patch;
+ const bool found = m_compliant.calcContactPatchDetailsById(state,id,patch);
+ //cout << "patch for id" << id << " found=" << found << endl;
+ //cout << "resultant=" << patch.getContactForce() << endl;
+ //cout << "num details=" << patch.getNumDetails() << endl;
+ for (int i=0; i < patch.getNumDetails(); ++i) {
+ const ContactDetail& detail = patch.getContactDetail(i);
+ const Real peakPressure = detail.getPeakPressure();
+ // Make a black line from the element's contact point in the normal
+ // direction, with length proportional to log(peak pressure)
+ // on that element.
+ DecorativeLine normal(detail.getContactPoint(),
+ detail.getContactPoint()+ std::log10(peakPressure)
+ * detail.getContactNormal());
+ normal.setColor(Black);
+ geometry.push_back(normal);
+ // Make a red line that extends from the contact
+ // point in the direction of the slip velocity, of length 3*slipvel.
+ DecorativeLine slip(detail.getContactPoint(),
+ detail.getContactPoint()+3*detail.getSlipVelocity());
+ slip.setColor(Red);
+ geometry.push_back(slip);
+ }
+ }
+ }
+private:
+ const MultibodySystem& m_system;
+ const CompliantContactSubsystem& m_compliant;
+};
+
+class MyReporter : public PeriodicEventReporter {
+public:
+ MyReporter(const MultibodySystem& system,
+ const CompliantContactSubsystem& complCont,
+ Real reportInterval)
+ : PeriodicEventReporter(reportInterval), m_system(system),
+ m_compliant(complCont)
+ {}
+
+ ~MyReporter() {}
+
+ void handleEvent(const State& state) const {
+ m_system.realize(state, Stage::Dynamics);
+ cout << state.getTime() << ": E = " << m_system.calcEnergy(state)
+ << " Ediss=" << m_compliant.getDissipatedEnergy(state)
+ << " E+Ediss=" << m_system.calcEnergy(state)
+ +m_compliant.getDissipatedEnergy(state)
+ << endl;
+ const int ncont = m_compliant.getNumContactForces(state);
+ cout << "Num contacts: " << m_compliant.getNumContactForces(state) << endl;
+ for (int i=0; i < ncont; ++i) {
+ const ContactForce& force = m_compliant.getContactForce(state,i);
+ //cout << force;
+ }
+ saveEm.push_back(state);
+ }
+private:
+ const MultibodySystem& m_system;
+ const CompliantContactSubsystem& m_compliant;
+};
+
+// These are the item numbers for the entries on the Run menu.
+static const int RunMenuId = 3, HelpMenuId = 7;
+static const int GoItem = 1, ReplayItem=2, QuitItem=3;
+
+// This is a periodic event handler that interrupts the simulation on a regular
+// basis to poll the InputSilo for user input. If there has been some, process it.
+// This one does nothing but look for the Run->Quit selection.
+class UserInputHandler : public PeriodicEventHandler {
+public:
+ UserInputHandler(Visualizer::InputSilo& silo, Real interval)
+ : PeriodicEventHandler(interval), m_silo(silo) {}
+
+ virtual void handleEvent(State& state, Real accuracy,
+ bool& shouldTerminate) const
+ {
+ int menuId, item;
+ if (m_silo.takeMenuPick(menuId, item) && menuId==RunMenuId && item==QuitItem)
+ shouldTerminate = true;
+ }
+
+private:
+ Visualizer::InputSilo& m_silo;
+};
+
+
+static void makeCube(Real h, PolygonalMesh& cube);
+static void makeTetrahedron(Real r, PolygonalMesh& tet);
+static void makePyramid(Real baseSideLength, PolygonalMesh& pyramid);
+static void makeOctahedron(Real radius, PolygonalMesh& pyramid);
+
+
+int main() {
+ try
+ { // Create the system.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::Gravity gravity(forces, matter, UnitVec3(2,-10,0), 1);
+
+ ContactTrackerSubsystem tracker(system);
+ CompliantContactSubsystem contactForces(system, tracker);
+ contactForces.setTrackDissipatedEnergy(true);
+ contactForces.setTransitionVelocity(1e-3);
+
+ GeneralContactSubsystem OLDcontact(system);
+ const ContactSetIndex OLDcontactSet = OLDcontact.createContactSet();
+
+ //makeCube(1, pyramidMesh);
+ //makeTetrahedron(1, pyramidMesh);
+ //pyramidMesh.transformMesh(Rotation(Pi/4, UnitVec3(-1,0,1)));
+ //makePyramid(1, pyramidMesh);
+ //makeOctahedron(1, pyramidMesh);
+ PolygonalMesh sphereMesh;
+ sphereMesh = PolygonalMesh::createSphereMesh(1,4);
+
+ ContactGeometry::TriangleMesh sphere(sphereMesh);
+ DecorativeMesh showSphere(sphere.createPolygonalMesh());
+ Array_<DecorativeLine> normals;
+ const Real NormalLength = .02;
+ for (int fx=0; fx < sphere.getNumFaces(); ++fx)
+ normals.push_back(
+ DecorativeLine(sphere.findCentroid(fx),
+ sphere.findCentroid(fx)
+ + NormalLength*sphere.getFaceNormal(fx)));
+ // not displaying mesh normals at the moment
+
+ ContactCliqueId clique1 = ContactSurface::createNewContactClique();
+ ContactCliqueId clique2 = ContactSurface::createNewContactClique();
+ ContactCliqueId clique3 = ContactSurface::createNewContactClique();
+
+ const Real fFac =1; // to turn off friction
+ const Real fDis = .5*0.2; // to turn off dissipation
+ const Real fVis = .1*.1; // to turn off viscous friction
+ const Real fK = 100*1e6; // pascals
+
+ // Right hand wall
+ matter.Ground().updBody().addDecoration(Vec3(.25+.01,0,0),
+ DecorativeBrick(Vec3(.01,4,8)).setColor(Gray).setOpacity(.2));
+ matter.Ground().updBody().addContactSurface(Vec3(.25,0,0),
+ ContactSurface(ContactGeometry::HalfSpace(),
+ ContactMaterial(fK*.01,fDis*.9,fFac*.8,fFac*.7,fVis*10))
+ .joinClique(clique3));
+
+ // Halfspace floor
+ const Rotation R_xdown(-Pi/2,ZAxis);
+ //matter.Ground().updBody().addDecoration(
+ // Transform(R_xdown, Vec3(0,-3-.01,0)),
+ // DecorativeBrick(Vec3(.01,4,8)).setColor(Gray).setOpacity(.1));
+ matter.Ground().updBody().addContactSurface(
+ Transform(R_xdown, Vec3(0,-3,0)),
+ ContactSurface(ContactGeometry::HalfSpace(),
+ ContactMaterial(fK*.1,fDis*.9,fFac*.8,fFac*.7,fVis*10))
+ //ContactMaterial(2e6,.01,.1,.05,.01))
+ .joinClique(clique1));
+
+ //// Big Sphere floor
+ //const Real FloorRadius = 10;
+ //matter.Ground().updBody().addDecoration(
+ // Vec3(0,-FloorRadius-3,0),
+ // DecorativeSphere(FloorRadius).setColor(Green));
+ //matter.Ground().updBody().addContactSurface(
+ // Vec3(0,-FloorRadius-3,0),
+ // ContactSurface(ContactGeometry::Sphere(FloorRadius),
+ // ContactMaterial(1e6,fDis*.9,fFac*.8,fFac*.7,fVis*10))
+ // //ContactMaterial(2e6,.01,.1,.05,.01))
+ // .joinClique(clique1));
+
+ const Real rad = .4;
+ Body::Rigid pendulumBody1(MassProperties(1.0, Vec3(0), Inertia(1)));
+ pendulumBody1.addDecoration(Transform(),
+ DecorativeSphere(rad).setOpacity(.4));
+ pendulumBody1.addContactSurface(Transform(),
+ ContactSurface(ContactGeometry::Sphere(rad),
+ ContactMaterial(fK*.01,fDis*.9,fFac*.8,fFac*.7,fVis*10))
+ .joinClique(clique2));
+
+ Body::Rigid pendulumBody2(MassProperties(1.0, Vec3(0), Inertia(1)));
+ pendulumBody2.addDecoration(Transform(),
+ DecorativeSphere(rad).setColor(Orange).setOpacity(.4));
+ pendulumBody2.addContactSurface(Transform(),
+ ContactSurface(ContactGeometry::Sphere(rad),
+ ContactMaterial(fK*.01,fDis*.9,fFac*.8,fFac*.7,fVis*10))
+ .joinClique(clique1));
+
+ MobilizedBody::Pin pendulum(matter.Ground(), Transform(Vec3(0)),
+ pendulumBody1, Transform(Vec3(0, 1, 0)));
+
+ MobilizedBody::Pin pendulum2(pendulum, Transform(Vec3(0)),
+ pendulumBody2, Transform(Vec3(0, 1, 0)));
+
+ Body::Rigid pendulumBody3(MassProperties(100.0, Vec3(0), 100*Inertia(1)));
+ PolygonalMesh body3contact = PolygonalMesh::createSphereMesh(rad,2);
+ ContactGeometry::TriangleMesh geo3(body3contact);
+ const DecorativeMesh mesh3(geo3.createPolygonalMesh());
+ pendulumBody3.addDecoration(Transform(),
+ DecorativeMesh(mesh3).setOpacity(.2));
+ pendulumBody3.addDecoration(Transform(),
+ DecorativeMesh(mesh3).setColor(Gray)
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setOpacity(.1));
+
+ //ContactGeometry::Sphere geo3(rad);
+ pendulumBody3.addContactSurface(Transform(),
+ ContactSurface(geo3,
+ ContactMaterial(fK*.1,fDis*.9,fFac*.8,fFac*.7,fVis*10),
+ rad/2 /*thickness*/)
+ .joinClique(clique2));
+ MobilizedBody::Pin pendulum3(matter.Ground(), Transform(Vec3(-2,0,0)),
+ pendulumBody3, Transform(Vec3(0, 2, 0)));
+
+ Force::MobilityLinearSpring(forces, pendulum2, MobilizerUIndex(0),
+ 10, 0*(Pi/180));
+
+ const Real ballMass = 200;
+ Body::Rigid ballBody(MassProperties(ballMass, Vec3(0),
+ ballMass*UnitInertia::sphere(1)));
+ //ballBody.addDecoration(Transform(), DecorativeSphere(.3).setColor(Cyan));
+ //ballBody.addContactSurface(Transform(),
+ // ContactSurface(ContactGeometry::Sphere(.3),
+ // ContactMaterial(1e7,.05,fFac*.8,fFac*.7,fVis*10))
+ // .joinClique(clique2));
+ ballBody.addDecoration(Transform(),
+ showSphere.setColor(Cyan).setOpacity(.2));
+ ballBody.addDecoration(Transform(),
+ showSphere.setColor(Gray)
+ .setRepresentation(DecorativeGeometry::DrawWireframe));
+ //Use this to display surface normals if you want to see them.
+ //for (unsigned i=0; i < normals.size(); ++i)
+ // ballBody.addDecoration(Transform(),
+ // normals[i].setColor(Gray));
+ ballBody.addDecoration(Transform(), DecorativeSphere(1).setColor(Gray)
+ .setOpacity(.1).setResolution(10));
+ ballBody.addContactSurface(Transform(),
+ ContactSurface(sphere,
+ ContactMaterial(fK*.1,fDis*.9,
+ .1*fFac*.8,.1*fFac*.7,fVis*1),
+ .5 /*thickness*/)
+ //ContactMaterial(2e6,.01,.1,.05,.01))
+ //.joinClique(clique2)
+ );
+ MobilizedBody::Free ball(matter.Ground(), Transform(Vec3(-2,0,0)),
+ ballBody, Transform(Vec3(0)));
+
+ //// The old way ...
+ //OLDcontact.addBody(OLDcontactSet, ball,
+ // pyramid, Transform());
+
+ //OLDcontact.addBody(OLDcontactSet, matter.updGround(),
+ // ContactGeometry::HalfSpace(), Transform(R_xdown, Vec3(0,-3,0)));
+ //ElasticFoundationForce ef(forces, OLDcontact, OLDcontactSet);
+ //Real stiffness = 1e6, dissipation = 0.01, us = 0.1,
+ // ud = 0.05, uv = 0.01, vt = 0.01;
+ ////Real stiffness = 1e6, dissipation = 0.1, us = 0.8,
+ //// ud = 0.7, uv = 0.01, vt = 0.01;
+
+ //ef.setBodyParameters(ContactSurfaceIndex(0),
+ // stiffness, dissipation, us, ud, uv);
+ //ef.setTransitionVelocity(vt);
+ //// end of old way.
+
+ Visualizer viz(system);
+ viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
+ viz.setMode(Visualizer::RealTime);
+ viz.setDesiredBufferLengthInSec(1);
+ viz.setDesiredFrameRate(FrameRate);
+ viz.setGroundHeight(-3);
+ viz.setShowShadows(true);
+
+ Visualizer::InputSilo* silo = new Visualizer::InputSilo();
+ viz.addInputListener(silo);
+ Array_<std::pair<String,int> > runMenuItems;
+ runMenuItems.push_back(std::make_pair("Go", GoItem));
+ runMenuItems.push_back(std::make_pair("Replay", ReplayItem));
+ runMenuItems.push_back(std::make_pair("Quit", QuitItem));
+ viz.addMenu("Run", RunMenuId, runMenuItems);
+
+ Array_<std::pair<String,int> > helpMenuItems;
+ helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1));
+ viz.addMenu("Help", HelpMenuId, helpMenuItems);
+
+ system.addEventReporter(new MyReporter(system,contactForces,ReportInterval));
+ system.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));
+
+ // Check for a Run->Quit menu pick every 1/4 second.
+ system.addEventHandler(new UserInputHandler(*silo, .25));
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ ball.setQToFitTransform(state, Transform(Rotation(Pi/2,XAxis),
+ Vec3(0,-1.8,0)));
+
+ pendulum.setOneQ(state, 0, -Pi/12);
+ pendulum3.setOneQ(state, 0, -Pi/4);
+
+ viz.report(state);
+ printf("Default state\n");
+ cout << "t=" << state.getTime()
+ << " q=" << pendulum.getQAsVector(state) << pendulum2.getQAsVector(state)
+ << " u=" << pendulum.getUAsVector(state) << pendulum2.getUAsVector(state)
+ << endl;
+
+ cout << "\nChoose 'Go' from Run menu to simulate:\n";
+ int menuId, item;
+ do { silo->waitForMenuPick(menuId, item);
+ if (menuId != RunMenuId || item != GoItem)
+ cout << "\aDude ... follow instructions!\n";
+ } while (menuId != RunMenuId || item != GoItem);
+
+
+
+ pendulum.setOneU(state, 0, 5.0);
+ ball.setOneU(state, 2, -20);
+
+ ball.setOneU(state, 0, .05); // to break symmetry
+
+ // Simulate it.
+
+ // The system as parameterized is very stiff (mostly due to friction)
+ // and thus runs best with CPodes which is extremely stable for
+ // stiff problems. To get reasonable performance out of the explicit
+ // integrators (like the RKs) you'll have to run at a very loose
+ // accuracy like 0.1, or reduce the friction coefficients and
+ // maybe the stiffnesses.
+
+ //ExplicitEulerIntegrator integ(system);
+ CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
+ //RungeKuttaFeldbergIntegrator integ(system);
+ //RungeKuttaMersonIntegrator integ(system);
+ //RungeKutta3Integrator integ(system);
+ //VerletIntegrator integ(system);
+ //integ.setMaximumStepSize(1e-0001);
+ integ.setAccuracy(1e-3); // minimum for CPodes
+ //integ.setAccuracy(.01);
+ TimeStepper ts(system, integ);
+
+
+ ts.initialize(state);
+ double cpuStart = cpuTime();
+ double realStart = realTime();
+
+ ts.stepTo(20.0);
+
+ const double timeInSec = realTime() - realStart;
+ const int evals = integ.getNumRealizations();
+ cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
+ timeInSec << "s elapsed for " << ts.getTime() << "s sim (avg step="
+ << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
+ << (1000*ts.getTime())/evals << "ms/eval\n";
+ cout << " CPU time was " << cpuTime() - cpuStart << "s\n";
+
+ printf("Using Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
+
+ viz.dumpStats(std::cout);
+
+ // Add as slider to control playback speed.
+ viz.addSlider("Speed", 1, 0, 4, 1);
+ viz.setMode(Visualizer::PassThrough);
+
+ silo->clear(); // forget earlier input
+ double speed = 1; // will change if slider moves
+ while(true) {
+ cout << "Choose Run/Replay to see that again ...\n";
+
+ int menuId, item;
+ silo->waitForMenuPick(menuId, item);
+
+
+ if (menuId != RunMenuId) {
+ cout << "\aUse the Run menu!\n";
+ continue;
+ }
+
+ if (item == QuitItem)
+ break;
+ if (item != ReplayItem) {
+ cout << "\aHuh? Try again.\n";
+ continue;
+ }
+
+ for (double i=0; i < (int)saveEm.size(); i += speed ) {
+ int slider; Real newValue;
+ if (silo->takeSliderMove(slider,newValue)) {
+ speed = newValue;
+ }
+ viz.report(saveEm[(int)i]);
+ }
+ }
+
+ } catch (const std::exception& e) {
+ std::printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+
+ } catch (...) {
+ std::printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+ return 0;
+}
+
+
+// Create a triangle mesh in the shape of a pyramid, with the
+// square base in the x-z plane centered at 0,0,0 of given side length s.
+// The base is split into two triangles. The apex will be at (0,s,0).
+static void makePyramid(Real s, PolygonalMesh& pyramidMesh) {
+ const Real h = s/2;
+ Array_<Vec3> vertices;
+ vertices.push_back(Vec3(-h, 0, -h)); // base
+ vertices.push_back(Vec3( h, 0, -h));
+ vertices.push_back(Vec3( h, 0, h));
+ vertices.push_back(Vec3(-h, 0, h));
+ vertices.push_back(Vec3( 0, s, 0)); // apex
+ Array_<int> faceIndices;
+ int faces[6][3] = {{0, 1, 2}, {0, 2, 3}, {1, 0, 4},
+ {2, 1, 4}, {3, 2, 4}, {0, 3, 4}};
+ for (int i = 0; i < 6; i++)
+ for (int j = 0; j < 3; j++)
+ faceIndices.push_back(faces[i][j]);
+
+ for (unsigned i=0; i < vertices.size(); ++i)
+ pyramidMesh.addVertex(vertices[i]);
+ for (unsigned i=0; i < faceIndices.size(); i += 3) {
+ const Array_<int> verts(&faceIndices[i], &faceIndices[i]+3);
+ pyramidMesh.addFace(verts);
+ }
+}
+
+
+// Create a triangle mesh in the shape of a tetrahedron with the
+// points in the corners of a cube inscribed in a sphere of radius r.
+static void makeTetrahedron(Real r, PolygonalMesh& tet) {
+ const Real h = r/std::sqrt(Real(3)); // half-dim of cube
+ Array_<Vec3> vertices;
+ vertices.push_back(Vec3( h, h, h));
+ vertices.push_back(Vec3(-h,-h, h));
+ vertices.push_back(Vec3(-h, h, -h));
+ vertices.push_back(Vec3( h,-h, -h));
+ Array_<int> faceIndices;
+ int faces[4][3] = {{0, 2, 1}, {1, 3, 0}, {0, 3, 2}, {2, 3, 1}};
+ for (int i = 0; i < 4; i++)
+ for (int j = 0; j < 3; j++)
+ faceIndices.push_back(faces[i][j]);
+
+ for (unsigned i=0; i < vertices.size(); ++i)
+ tet.addVertex(vertices[i]);
+ for (unsigned i=0; i < faceIndices.size(); i += 3) {
+ const Array_<int> verts(&faceIndices[i], &faceIndices[i]+3);
+ tet.addFace(verts);
+ }
+}
+
+static void makeOctahedralMesh(const Vec3& r, Array_<Vec3>& vertices,
+ Array_<int>& faceIndices) {
+ vertices.push_back(Vec3( r[0], 0, 0)); //0
+ vertices.push_back(Vec3(-r[0], 0, 0)); //1
+ vertices.push_back(Vec3( 0, r[1], 0)); //2
+ vertices.push_back(Vec3( 0, -r[1], 0)); //3
+ vertices.push_back(Vec3( 0, 0, r[2])); //4
+ vertices.push_back(Vec3( 0, 0, -r[2])); //5
+ int faces[8][3] = {{0, 2, 4}, {4, 2, 1}, {1, 2, 5}, {5, 2, 0},
+ {4, 3, 0}, {1, 3, 4}, {5, 3, 1}, {0, 3, 5}};
+ for (int i = 0; i < 8; i++)
+ for (int j = 0; j < 3; j++)
+ faceIndices.push_back(faces[i][j]);
+}
+
+// Create a triangle mesh in the shape of an octahedron (like two
+// pyramids stacked base-to-base, with the square base in the x-z plane
+// centered at 0,0,0 of given "radius" r.
+// The apexes will be at (0,+/-r,0).
+static void makeOctahedron(Real r, PolygonalMesh& mesh) {
+ Array_<Vec3> vertices;
+ Array_<int> faceIndices;
+ makeOctahedralMesh(Vec3(r), vertices, faceIndices);
+
+ for (unsigned i=0; i < vertices.size(); ++i)
+ mesh.addVertex(vertices[i]);
+ for (unsigned i=0; i < faceIndices.size(); i += 3) {
+ const Array_<int> verts(&faceIndices[i], &faceIndices[i]+3);
+ mesh.addFace(verts);
+ }
+}
+
+static void makeCube(Real h, PolygonalMesh& cube) {
+ Array_<Vec3> vertices;
+ vertices.push_back(Vec3( h, h, h));
+ vertices.push_back(Vec3( h, h, -h));
+ vertices.push_back(Vec3( h,-h, h));
+ vertices.push_back(Vec3( h,-h, -h));
+ vertices.push_back(Vec3(-h, h, h));
+ vertices.push_back(Vec3(-h, h, -h));
+ vertices.push_back(Vec3(-h,-h, h));
+ vertices.push_back(Vec3(-h,-h, -h));
+
+ Array_<int> faceIndices;
+ int faces[6][4] = {{0,2,3,1},{1,5,4,0},{0,4,6,2},
+ {2,6,7,3},{3,7,5,1},{4,5,7,6}};
+ for (int i = 0; i < 6; i++)
+ for (int j = 0; j < 4; j++)
+ faceIndices.push_back(faces[i][j]);
+
+ for (unsigned i=0; i < vertices.size(); ++i)
+ cube.addVertex(vertices[i]);
+ for (unsigned i=0; i < faceIndices.size(); i += 4) {
+ const Array_<int> verts(&faceIndices[i], &faceIndices[i]+4);
+ cube.addFace(verts);
+ }
+}
+
+
diff --git a/Simbody/examples/ExampleCustomConstraint.cpp b/Simbody/examples/ExampleCustomConstraint.cpp
new file mode 100644
index 0000000..502e17f
--- /dev/null
+++ b/Simbody/examples/ExampleCustomConstraint.cpp
@@ -0,0 +1,242 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Custom Constraint *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+// This is the Custom Constraint example from the Simbody Advanced Programming
+// Guide.
+//
+// Here we'll make two pendulums hanging near one another, and connect their
+// body origins together with the massless rod-like constraint given in
+// the above example. This is a simplified version of Simbody's "Rod" (distance)
+// constraint, which is not restricted to body origins.
+//
+// We're going to run and visualize a short simulation and show that energy
+// is conserved by the constraint implementation.
+
+
+#include "Simbody.h"
+
+using namespace SimTK;
+using std::cout; using std::endl;
+
+// This constraint holds the body origins of two bodies apart at a given
+// distance, without constraining any other motion. This is like connecting
+// the points with a massless rod with ball joints at each end. We will
+// use the following equation for this holonomic (position-level) constraint:
+// perr(q) = 0
+// where perr(q) = (dot(r, r) - d^2)/2
+// with r(q) the vector from body1's origin to body2's origin, and d a given
+// constant.
+// We must supply the first and second time derivatives of perr here as a
+// velocity error function verr and acceleration error function aerr:
+// verr(q,u) = d/dt perr = dot(v, r)
+// aerr(q,u,udot) = d/dt verr = dot(a, r) + dot(v, v)
+// where v = d/dt r is the relative velocity between the body origins and
+// a = d/dt v is their relative acceleration.
+//
+class ExampleConstraint : public Constraint::Custom::Implementation {
+public:
+ // Constructor takes two bodies and the desired separation distance
+ // between their body frame origins. Tell the base class that this
+ // constraint generates 1 holonomic (position level), 0 nonholonomic
+ // (velocity level), and 0 acceleration-only constraint equations.
+ ExampleConstraint(MobilizedBody& b1, MobilizedBody& b2, Real distance)
+ : Implementation(b1.updMatterSubsystem(), 1, 0, 0), distance(distance) {
+ body1 = addConstrainedBody(b1);
+ body2 = addConstrainedBody(b2);
+ }
+
+ // Implement required pure virtual method.
+ Implementation* clone () const {return new ExampleConstraint(*this);}
+
+ // Implement the Implementation virtuals required for a holonomic
+ // (position level) constraint.
+
+ // Simbody supplies position information in argument list; we calculate
+ // the constraint error that represents here.
+ void calcPositionErrors
+ (const State& state,
+ const Array_<Transform,ConstrainedBodyIndex>& X_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr) const
+ {
+ Vec3 r1 = getBodyOriginLocation(X_AB, body1);
+ Vec3 r2 = getBodyOriginLocation(X_AB, body2);
+ Vec3 r = r2-r1;
+ perr[0] = (dot(r,r)-distance*distance)/2;
+ }
+
+ // Simbody supplies velocity information in argument list; position info
+ // is in the state. Return time derivative of position constraint error.
+ void calcPositionDotErrors
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr) const
+ {
+ Vec3 r1 = getBodyOriginLocationFromState(state, body1);
+ Vec3 r2 = getBodyOriginLocationFromState(state, body2);
+ Vec3 r = r2-r1;
+ Vec3 v1 = getBodyOriginVelocity(V_AB, body1);
+ Vec3 v2 = getBodyOriginVelocity(V_AB, body2);
+ Vec3 v = v2-v1;
+ pverr[0] = dot(v, r);
+ }
+
+ // Simbody supplies acceleration information in argument list; position and
+ // velocity info is in the state. Return second time derivative of position
+ // constraint error.
+ void calcPositionDotDotErrors
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr) const
+ {
+ Vec3 r1 = getBodyOriginLocationFromState(state, body1);
+ Vec3 r2 = getBodyOriginLocationFromState(state, body2);
+ Vec3 r = r2-r1;
+ Vec3 v1 = getBodyOriginVelocityFromState(state, body1);
+ Vec3 v2 = getBodyOriginVelocityFromState(state, body2);
+ Vec3 v = v2-v1;
+ Vec3 a1 = getBodyOriginAcceleration(A_AB, body1);
+ Vec3 a2 = getBodyOriginAcceleration(A_AB, body2);
+ Vec3 a = a2-a1;
+ paerr[0] = dot(a, r) + dot(v, v);
+ }
+
+ // Simbody provides calculated constraint multiplier in argument list; we
+ // turn that into forces here and apply them to the two bodies as point
+ // forces at the origins.
+ void addInPositionConstraintForces
+ (const State& state,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedQIndex>& qForces) const
+ {
+ Vec3 r1 = getBodyOriginLocationFromState(state, body1);
+ Vec3 r2 = getBodyOriginLocationFromState(state, body2);
+ Vec3 r = r2-r1;
+ Vec3 force = multipliers[0]*r;
+ addInStationForce(state, body2, Vec3(0), force, bodyForcesInA);
+ addInStationForce(state, body1, Vec3(0), -force, bodyForcesInA);
+ }
+
+private:
+ ConstrainedBodyIndex body1, body2;
+ Real distance;
+};
+
+// This will be used to report energy periodically. See TextDataEventReporter
+// for more information.
+class MyEvaluateEnergy : public TextDataEventReporter::UserFunction<Real> {
+public:
+ Real evaluate(const System& system, const State& state) {
+ const MultibodySystem& mbs = MultibodySystem::downcast(system);
+ mbs.realize(state, Stage::Dynamics);
+ return mbs.calcEnergy(state);
+ }
+};
+
+int main() {
+ try {
+ // Create the system, with subsystems for the bodies and some forces.
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+
+ // Hint to Visualizer: don't show ground plane.
+ system.setUseUniformBackground(true);
+
+ // Add gravity as a force element.
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.81, 0));
+
+ // Create the body and some artwork for it.
+ const Vec3 halfLengths(.5, .1, .25); // half-size of brick (m)
+ const Real mass = 2; // total mass of brick (kg)
+ Body::Rigid pendulumBody(MassProperties(mass, Vec3(0),
+ mass*UnitInertia::brick(halfLengths)));
+ pendulumBody.addDecoration(Transform(),
+ DecorativeBrick(halfLengths).setColor(Red));
+
+ // Add an instance of the body to the multibody system by connecting
+ // it to Ground via a Ball mobilizer.
+ MobilizedBody::Ball pendulum1(matter.updGround(), Transform(Vec3(-1,-1, 0)),
+ pendulumBody, Transform(Vec3( 0, 1, 0)));
+
+ // Add a second instance of the pendulum nearby.
+ MobilizedBody::Ball pendulum2(matter.updGround(), Transform(Vec3(1,-1, 0)),
+ pendulumBody, Transform(Vec3(0, 1, 0)));
+
+ // Connect the origins of the two pendulum bodies together with our
+ // rod-like custom constraint.
+ const Real d = 1.5; // desired separation distance
+ Constraint::Custom rod(new ExampleConstraint(pendulum1, pendulum2, d));
+
+ // Visualize with default options.
+ Visualizer viz(system);
+
+ // Add a rubber band line connecting the origins of the two bodies to
+ // represent the rod constraint.
+ viz.addRubberBandLine(pendulum1, Vec3(0), pendulum2, Vec3(0),
+ DecorativeLine().setColor(Blue).setLineThickness(3));
+
+ // Ask for a report every 1/30 of a second to match the Visualizer's
+ // default rate of 30 frames per second.
+ system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
+ // Output total energy to the console once per second.
+ system.addEventReporter(new TextDataEventReporter
+ (system, new MyEvaluateEnergy(), 1.0));
+
+ // Initialize the system and state.
+ State state = system.realizeTopology();
+
+ // Orient the two pendulums asymmetrically so they'll do something more
+ // interesting than just hang there.
+ pendulum1.setQToFitRotation(state, Rotation(Pi/4, ZAxis));
+ pendulum2.setQToFitRotation(state, Rotation(BodyRotationSequence,
+ Pi/4, ZAxis, Pi/4, YAxis));
+
+ // Evaluate the system at the new state and draw one frame manually.
+ system.realize(state);
+ viz.report(state);
+
+ // Simulate it.
+ cout << "Hit ENTER to run a short simulation.\n";
+ cout << "(Energy should be conserved to about four decimal places.)\n";
+ getchar();
+
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setAccuracy(1e-4); // ask for about 4 decimal places (default is 3)
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(10.0);
+
+ } catch (const std::exception& e) {
+ std::cout << "ERROR: " << e.what() << std::endl;
+ return 1;
+ } catch (...) {
+ std::cout << "UNKNOWN EXCEPTION\n";
+ return 1;
+ }
+
+ return 0;
+}
diff --git a/Simbody/examples/ExampleEventHandler.cpp b/Simbody/examples/ExampleEventHandler.cpp
new file mode 100644
index 0000000..122aca8
--- /dev/null
+++ b/Simbody/examples/ExampleEventHandler.cpp
@@ -0,0 +1,76 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Event Handler *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* This example demonstrates the use of an event handler to implement
+discontinuous changes to the State during time stepping. */
+
+#include "Simbody.h"
+
+using namespace SimTK;
+
+class BounceHandler : public TriggeredEventHandler {
+public:
+ BounceHandler() : TriggeredEventHandler(Stage::Position) {
+ getTriggerInfo().setTriggerOnRisingSignTransition(false);
+ }
+ Real getValue(const State& state) const {
+ return state.getQ()[0];
+ }
+ // Note: in general a discontinuous velocity change should be followed by
+ // an impulse-momentum analysis to ensure that momentum is conserved. We're
+ // not doing that here.
+ void handleEvent(State& state, Real accuracy, bool& shouldTerminate) const {
+ state.updU()[0] *= -1;
+ }
+};
+
+int main() {
+
+ // Create the system.
+
+ MultibodySystem system; system.setUseUniformBackground(true);
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1));
+ MobilizedBody::Pin pendulum(matter.updGround(), Transform(Vec3(0)), pendulumBody, Transform(Vec3(0, 1, 0)));
+
+ Visualizer viz(system);
+ system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
+
+ system.addEventHandler(new BounceHandler());
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ pendulum.setOneU(state, 0, 1.0);
+
+ // Simulate it.
+
+ RungeKuttaMersonIntegrator integ(system);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(100.0);
+}
diff --git a/Simbody/examples/ExampleEventReporter.cpp b/Simbody/examples/ExampleEventReporter.cpp
new file mode 100644
index 0000000..5f91463
--- /dev/null
+++ b/Simbody/examples/ExampleEventReporter.cpp
@@ -0,0 +1,78 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Event Reporter *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* This example demonstrates the use of an event reporter to output some
+interesting information during time stepping. The information is displayed
+in the console (terminal) window, not the graphics window. */
+
+#include "Simbody.h"
+
+using namespace SimTK;
+
+class PositionReporter : public PeriodicEventReporter {
+public:
+ PositionReporter(const MultibodySystem& system, const MobilizedBody& body, Real interval) :
+ PeriodicEventReporter(interval), system(system), body(body) {
+ }
+
+ // Show x-y position of the pendulum weight as a function of time.
+ void handleEvent(const State& state) const {
+ system.realize(state, Stage::Position);
+ Vec3 pos = body.getBodyOriginLocation(state);
+ std::cout<<state.getTime()<<"\t"<<pos[0]<<"\t"<<pos[1]<<std::endl;
+ }
+private:
+ const MultibodySystem& system;
+ const MobilizedBody& body;
+};
+
+int main() {
+
+ // Create the system.
+
+ MultibodySystem system; system.setUseUniformBackground(true);
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1));
+ MobilizedBody::Pin pendulum(matter.updGround(), Transform(Vec3(0)), pendulumBody, Transform(Vec3(0, 1, 0)));
+
+ Visualizer viz(system);
+ system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
+
+ system.addEventReporter(new PositionReporter(system, pendulum, 0.1));
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ pendulum.setOneU(state, 0, 1.0);
+
+ // Simulate it.
+
+ RungeKuttaMersonIntegrator integ(system);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(100.0);
+}
diff --git a/Simbody/examples/ExampleGears.cpp b/Simbody/examples/ExampleGears.cpp
new file mode 100644
index 0000000..accd1ac
--- /dev/null
+++ b/Simbody/examples/ExampleGears.cpp
@@ -0,0 +1,83 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Gears *
+ * -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "Simbody.h"
+
+using namespace SimTK;
+
+int main() {
+
+ // Create the system.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+
+ // Create bodies.
+ Body::Rigid gearBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ gearBody.addDecoration(Transform(Rotation(0.5*Pi, XAxis)),
+ DecorativeCylinder(1.0, 0.1));
+ Body::Rigid rodBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ rodBody.addDecoration(Transform(Vec3(0, 1, 0)), DecorativeCylinder(0.05, 1.0));
+
+ // Create instances of the bodies that are connected into the multibody
+ // system via Mobilizers. Note that we use the gear body twice.
+
+ MobilizedBody::Pin gear1(matter.updGround(), Transform(Vec3(1, 0, 0)),
+ gearBody, Transform());
+ MobilizedBody::Pin gear2(matter.updGround(), Transform(Vec3(-1, 0, 0)),
+ gearBody, Transform());
+ MobilizedBody::Pin rod(gear2, Transform(Vec3(0, 0.8, 0.1)), rodBody, Transform());
+
+ // Add constraints.
+ Constraint::ConstantSpeed(gear1, 2*Pi); // i.e., 1 rotation per second
+ Constraint::NoSlip1D(matter.updGround(), Vec3(0), UnitVec3(0, 1, 0), gear1, gear2);
+
+ // We want the rod end point traveling along a line. We'll draw part of the
+ // line to make it clear.
+ Constraint::PointOnLine(matter.updGround(), UnitVec3(0, 1, 0), Vec3(0, 0, 0.1),
+ rod, Vec3(0, 2, 0));
+
+ matter.updGround().addBodyDecoration(Vec3(0,0,.1),
+ DecorativeLine(Vec3(0), 3*UnitVec3(0,1,0))
+ .setColor(Red));
+
+ // Visualize the system, reporting an output frame every 1/30 of a simulated
+ // second. The Visualizer's default frame rate is 30fps, and it will slow the
+ // simulation down to keep to that speed, so we'll get exactly real time
+ // this way. We don't want the default ground and sky background here.
+ Visualizer viz(system);
+ viz.setBackgroundType(Visualizer::SolidColor); // default is white
+ system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+
+ // Simulate it.
+
+ RungeKuttaMersonIntegrator integ(system);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(1000.0);
+}
diff --git a/Simbody/examples/ExampleGeodesic.cpp b/Simbody/examples/ExampleGeodesic.cpp
new file mode 100644
index 0000000..f2af54b
--- /dev/null
+++ b/Simbody/examples/ExampleGeodesic.cpp
@@ -0,0 +1,355 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) ExampleGeodesic *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Ian Stavness, Michael Sherman, Andreas Scholz *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**
+ * This example demonstrates finding the geodesic between two points on
+ * a ContactGeometry object.
+ **/
+
+
+#include "Simbody.h"
+
+using namespace SimTK;
+using std::cos;
+using std::sin;
+using std::cout;
+using std::endl;
+
+const Real vizInterval = 1/30.; // set to 1/30. to vizualize shooting
+
+
+class VizPeriodicReporter : public PeriodicEventReporter {
+public:
+ VizPeriodicReporter(const Visualizer& viz, const State& dummyState, Real interval) :
+ PeriodicEventReporter(interval), viz(viz), dummyState(dummyState) {
+ }
+
+ void handleEvent(const State& state) const {
+ viz.report(dummyState);
+ }
+
+private:
+ const Visualizer& viz;
+ const State& dummyState;
+};
+
+class ExtremePointDecorator : public DecorationGenerator {
+public:
+ ExtremePointDecorator(const ContactGeometry& geom,
+ const Vec3& startPoint)
+ : geom(geom), closestPoint(startPoint), startFrameOnly(false) {}
+
+ void setStartPoint(const Vec3& pt) {closestPoint=pt;}
+ const Vec3& getStartPoint() const {return closestPoint;}
+
+ void setShowStartFrameOnly(bool showStart) {startFrameOnly=showStart;}
+
+ void generateDecorations(const State& state,
+ Array_<DecorativeGeometry>& geometry) OVERRIDE_11
+ {
+ geometry.push_back(DecorativeLine(P,Q));
+ geometry.push_back(
+ DecorativePoint(closestPoint).setColor(Purple)
+ .setScale(2).setLineThickness(2));
+ const UnitVec3 startN = geom.calcSurfaceUnitNormal(closestPoint);
+ geometry.push_back(DecorativeLine(closestPoint,closestPoint+startN)
+ .setColor(Purple));
+
+ if (startFrameOnly) {
+ startFrameOnly = false;
+ return;
+ }
+
+ Vec3 newClosestPoint, closestPointOnLine;
+ Real height;
+ if (!geom.trackSeparationFromLine(P, d, closestPoint,
+ newClosestPoint, closestPointOnLine, height))
+ {
+ std::cout << "\n---TRACKER REPORTED FAILURE---\n\n";
+ }
+
+ std::cout << "HEIGHT=" << height << "\n";
+
+ const UnitVec3 n = geom.calcSurfaceUnitNormal(newClosestPoint);
+ geometry.push_back(DecorativeLine(newClosestPoint,closestPointOnLine)
+ .setColor(height>0?Blue:Red));
+
+ geometry.push_back(
+ DecorativePoint(newClosestPoint).setColor(Green));
+ geometry.push_back(DecorativeLine(newClosestPoint,newClosestPoint+n)
+ .setColor(Green));
+
+
+ geometry.push_back(
+ DecorativePoint(closestPointOnLine).setColor(Red));
+
+ closestPoint = newClosestPoint;
+ }
+
+ void moveLine(const Vec3& P, const Vec3& Q) {
+ cout << "...moveLine P=" << P << " Q=" << Q << "\n";
+ this->P = P; this->Q = Q; d = UnitVec3(Q-P);
+ }
+
+private:
+ const ContactGeometry& geom;
+ Vec3 closestPoint;
+ Vec3 P, Q; // points on line
+ UnitVec3 d; // direction of line
+ bool startFrameOnly; // don't solve just show initial conditions
+};
+
+int main() {
+ try {
+
+ // Create geometry
+ Real r = 0.5;
+ //ContactGeometry::Sphere geom(r);
+// ContactGeometry::Cylinder geom(r);
+ ContactGeometry::Torus geom(2*r, r);
+
+ Vec3 radii(0.2,0.4,0.6);
+ //ContactGeometry::Ellipsoid geom(radii);
+
+ Real startLength = 0.5;
+ //startLength=5;
+
+
+ Real phiP = 0.0*Pi;
+ Real thetaP = 0.0*Pi;
+
+ Real phiQ = 0.0*Pi;
+ Real thetaQ = 1.2*Pi;
+
+ Real heightP = 0.5;
+ Real heightQ = -0.5;
+
+
+ Vec3 P(r*sin(thetaP)*cos(phiP), r*sin(thetaP)*sin(phiP), r*cos(thetaP));
+ Vec3 Q(r*sin(thetaQ)*cos(phiQ), r*sin(thetaQ)*sin(phiQ), r*cos(thetaQ));
+
+ Vec3 O(-2, 0, heightP);
+ Vec3 I(-2, 0, heightQ);
+
+ // move points off surface for testing
+ Q(0) -= r/2;
+ Q(1) -= -r*0.5;
+ P(1) -= r*0.5;
+ P(0) -= r/2;
+
+ //Q=P+Vec3(1.25,-1,0); P+=Vec3(-1,-.9,0);
+ //Q=P+Vec3(1,-1,-1.5); P+=Vec3(-1,-.9,0);
+
+ // project back to surface for testing
+ Vec3 tmpPt;
+ tmpPt = geom.projectDownhillToNearestPoint(P);
+ P = tmpPt;
+ tmpPt = geom.projectDownhillToNearestPoint(Q);
+ Q = tmpPt;
+
+
+ Vec3 r_OP = P - O;
+ Vec3 r_IQ = Q - I;
+ UnitVec3 e_OP(r_OP);
+ UnitVec3 e_IQ(r_IQ);
+
+ Vec3 r_PQ = Q - P;
+
+ int n = 2; // problem size
+ Vector x(n), dx(n), Fx(n), xold(n);
+ Matrix J(n, n);
+
+
+ bool inside; UnitVec3 nP, nQ;
+ cout << "before P,Q=" << P << ", " << Q << " -- "
+ << geom.calcSurfaceValue(P) << " " << geom.calcSurfaceValue(Q) << endl;
+ Vec3 newP = geom.findNearestPoint(P,inside,nP);
+ UnitVec3 tP = nP.perp();
+ Vec3 newQ = geom.findNearestPoint(Q,inside,nQ);
+ UnitVec3 tQ = nQ.perp();
+ cout << "after newP,Q=" << newP << ", " << newQ << " -- "
+ << geom.calcSurfaceValue(newP)
+ << " " << geom.calcSurfaceValue(newQ) << endl;
+
+ cout << "curvature at newP along " << tP << ": "
+ << geom.calcSurfaceCurvatureInDirection(newP,tP) << "\n";
+ cout << "curvature at newQ along " << tQ << ": "
+ << geom.calcSurfaceCurvatureInDirection(newQ,tQ) << "\n";
+ cout << "gradient at newP " << ": "
+ << geom.calcSurfaceGradient(newP) << " |gP|=" <<
+ geom.calcSurfaceGradient(newP).norm() << "\n";
+ cout << "gradient at newQ " << ": "
+ << geom.calcSurfaceGradient(newQ) << " |gQ|=" <<
+ geom.calcSurfaceGradient(newQ).norm() << "\n";
+
+ Rotation R_GP(nP, ZAxis, tP, XAxis);
+ for (int i=0; i <=10; ++i) {
+ Real a = i*(Pi/2)/10;
+ UnitVec3 u_P(-sin(a), cos(a), 0);
+ UnitVec3 dir = R_GP*u_P;
+ cout << a << ": " << geom.calcSurfaceCurvatureInDirection(newP,dir)
+ << " 2*sin^2(a)=" << 2*square(sin(a)) << "\n";
+ }
+
+
+ cout << "Gaussian curvature P,Q="
+ << geom.calcGaussianCurvature(newP) << ","
+ << geom.calcGaussianCurvature(newQ) << endl;
+
+ Geodesic geod;
+
+ // Create a dummy mb system for visualization
+ MultibodySystem dummySystem;
+ SimbodyMatterSubsystem matter(dummySystem);
+
+
+// matter.updGround().addBodyDecoration(Transform(), DecorativeEllipsoid(radii)
+ matter.updGround().addBodyDecoration(Transform(), geom.createDecorativeGeometry()
+ .setColor(Gray)
+ .setOpacity(0.5)
+ .setResolution(5));
+
+ matter.updGround().addBodyDecoration(Transform(),
+ DecorativeLine(Vec3(newP), Vec3(newP)+.5*tP).setColor(Green));
+ matter.updGround().addBodyDecoration(Transform(),
+ DecorativeLine(Vec3(newQ), Vec3(newQ)+.5*tQ).setColor(Red));
+
+ // Visualize with default options; ask for a report every 1/30 of a second
+ // to match the Visualizer's default 30 frames per second rate.
+ Visualizer viz(dummySystem);
+ viz.setBackgroundType(Visualizer::SolidColor);
+
+ // add vizualization callbacks for geodesics, contact points, etc.
+ Vector tmp(6); // tmp = ~[P Q]
+ tmp[0]=P[0]; tmp[1]=P[1]; tmp[2]=P[2]; tmp[3]=Q[0]; tmp[4]=Q[1]; tmp[5]=Q[2];
+ viz.addDecorationGenerator(new PathDecorator(tmp, O, I, Green));
+ //viz.addDecorationGenerator(new PlaneDecorator(geom.getPlane(), Gray));
+ viz.addDecorationGenerator(new GeodesicDecorator(geom.getGeodP(), Red));
+ viz.addDecorationGenerator(new GeodesicDecorator(geom.getGeodQ(), Blue));
+ viz.addDecorationGenerator(new GeodesicDecorator(geod, Orange));
+ //ExtremePointDecorator* expd = new ExtremePointDecorator(geom, P);
+ //viz.addDecorationGenerator(expd);
+ dummySystem.realizeTopology();
+ State dummyState = dummySystem.getDefaultState();
+
+ /* Sherm playing with separation tracking ...
+ expd->setStartPoint(Vec3(1,0,0));
+ for (int outer=0; ; ++outer) {
+ for (int i=0; i <10; ++i) {
+ Real x = i*.2;
+ expd->moveLine(Vec3(x,-3,-2), Vec3(0,3,1));
+ if (outer) expd->setStartPoint(expd->getStartPoint()-Vec3(.1,0,0));
+ expd->setShowStartFrameOnly(true);
+ viz.report(dummyState);
+ if (outer) getchar();
+ viz.report(dummyState);
+ if (outer) getchar(); else sleepInSec(.25);
+ //sleepInSec(.5);
+ }
+ for (int i=0; i <10; ++i) {
+ Real z = 1+i*.2;
+ Real x = 2-i*.2;
+ expd->moveLine(Vec3(x,-3,-2), Vec3(0,3,z));
+ expd->setShowStartFrameOnly(true);
+ viz.report(dummyState);
+ viz.report(dummyState); sleepInSec(.25);
+ //sleepInSec(.5);
+ }
+ for (int i=0; i <10; ++i) {
+ Real z = 3-i*.5;
+ expd->moveLine(Vec3(0,-3,-2), Vec3(0,3,z));
+ expd->setShowStartFrameOnly(true);
+ viz.report(dummyState);
+ viz.report(dummyState); sleepInSec(.25);
+ //sleepInSec(.5);
+ }
+ }
+ exit(0);
+ */
+
+ // calculate the geodesic
+ //geom.addVizReporter(new VizPeriodicReporter(viz, dummyState, vizInterval));
+ viz.report(dummyState);
+
+ const Real startReal = realTime(), startCpu = cpuTime();
+ //geom.calcGeodesic(P, Q, e_OP, -e_IQ, geod);
+ //geom.calcGeodesicAnalytical(P, Q, e_OP, -e_IQ, geod);
+ //geom.calcGeodesicUsingOrthogonalMethod(P, Q, geod);
+ //geom.calcGeodesicUsingOrthogonalMethod(P, Q, e_OP, .5, geod);
+ Rotation R(-Pi/8*0, YAxis); // TODO: 2.7 vs. 2.78
+ geom.calcGeodesicUsingOrthogonalMethod(P, Q, R*Vec3(0.9,0,-.3),
+ startLength, geod);
+ //geom.makeStraightLineGeodesic(P, Q, e_OP, GeodesicOptions(), geod);
+ cout << "realTime=" << realTime()-startReal
+ << " cpuTime=" << cpuTime()-startCpu << endl;
+ viz.report(dummyState);
+ printf("Geodesic has %d points; %d geodesics shot\n",
+ geod.getNumPoints(), geom.getNumGeodesicsShot());
+
+ const Array_<Real>& arcLength = geod.getArcLengths();
+ const Array_<Transform>& frenet = geod.getFrenetFrames();
+ const Array_<Vec2>& rotPtoQ = geod.getDirectionalSensitivityPtoQ();
+ const Array_<Vec2>& rotQtoP = geod.getDirectionalSensitivityQtoP();
+ const Array_<Vec2>& transPtoQ = geod.getPositionalSensitivityPtoQ();
+ const Array_<Vec2>& transQtoP = geod.getPositionalSensitivityQtoP();
+ const Array_<Real>& curvature = geod.getCurvatures();
+ bool showTrans = !transPtoQ.empty();
+ cout << "torsion at P=" << geod.getTorsionP()
+ << " binormal curvature kb at P=" << geod.getBinormalCurvatureP() << endl;
+ for (int i=0; i < geod.getNumPoints(); ++i) {
+ cout << "\ns=" << arcLength[i] << " kt=" << curvature[i] << ":\n";
+ cout << "p=" << frenet[i].p() << "\n";
+ cout << "t=" << frenet[i].y() << "\n";
+ cout << "b=" << frenet[i].x() << "\n";
+ cout << "n=" << frenet[i].z() << "\n";
+ cout << "jrQ=" << rotPtoQ[i] << " jrP=" << rotQtoP[i] << "\n";
+ if (showTrans) cout << "jtQ=" << transPtoQ[i] << " jtP=" << transQtoP[i] << "\n";
+ }
+ cout << "torsion at Q=" << geod.getTorsionQ()
+ << " binormal curvature kb at Q=" << geod.getBinormalCurvatureQ() << endl;
+
+
+// geom.addVizReporter(new VizPeriodicReporter(viz, dummyState, 1/30.));
+// viz.report(dummyState);
+// GeodesicOptions opts;
+// geom.shootGeodesicInDirectionUntilLengthReached(P, UnitVec3(tP), 20, opts, geod);
+// geom.shootGeodesicInDirectionUntilPlaneHit(P, UnitVec3(tP), geom.getPlane(), opts, geod);
+
+ viz.report(dummyState);
+ cout << "geod shooting count = " << geom.getNumGeodesicsShot() << endl;
+ cout << "num geod pts = " << geod.getFrenetFrames().size() << endl;
+
+
+ } catch (const std::exception& e) {
+ std::printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+
+ } catch (...) {
+ std::printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+ return 0;
+}
+
+
diff --git a/Simbody/examples/ExampleKneeJoint.cpp b/Simbody/examples/ExampleKneeJoint.cpp
new file mode 100644
index 0000000..af645c3
--- /dev/null
+++ b/Simbody/examples/ExampleKneeJoint.cpp
@@ -0,0 +1,315 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Custom Knee Joint *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Ajay Seth *
+ * Contributors: Michael Sherman *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+
+/*
+ * This is a 2D knee joint example that demonstrates using custom mobilizers
+ * (FunctionBased) to simulate the effects of joint geometry that leads to the
+ * translation of tibia (shank) with respect to the femur (thigh) during flexion
+ * and extension of the knee.
+ *
+ * For more information see:
+ * A. Seth, M. Sherman, P. Eastman and S. Delp. Minimal formulation of joint
+ * motion for biomechanisms. Nonlinear Dynamics, 2010.
+ * DOI 10.1007/s11071-010-9717-3
+ */
+
+#include "Simbody.h"
+
+using namespace SimTK;
+using namespace std;
+
+static const Real m = 1; // kg
+static const Real g = 9.8; // meters/s^2; apply in �y direction
+static const Real d = 0.4; // meters
+static const Real Deg2Rad = Pi/180;
+
+static const Real Accuracy = 1e-3; // integration accuracy
+
+// The real deviation of a knee from a pin joint is subtle; it is more fun to
+// see if it is exaggerated, but the correct values here should be 1 to match real data.
+static const Real ExaggerateX = 5;
+static const Real ExaggerateY = 5;
+#define SHOULD_EXAGGERATE // by default we'll take the more fun option; comment out for accuracy
+
+//------------------------------------------------------------------------------
+// We'll print out the total system energy to validate that it is conserved
+// to within integrator tolerance (roughly), as it should be for this system since
+// there is no energy input or loss. Try tightening the accuracy setting above to
+// see whether energy conservation follows.
+//------------------------------------------------------------------------------
+class MyEnergyReporter : public PeriodicEventReporter {
+public:
+ MyEnergyReporter(const MultibodySystem& system, Real period)
+ : PeriodicEventReporter(period), system(system) {}
+
+ virtual void handleEvent(const State& state) const {
+ cout << "t=" << state.getTime();
+ cout << " E=" << system.calcEnergy(state) << endl;
+ }
+private:
+ const MultibodySystem& system;
+};
+
+//------------------------------------------------------------------------------
+// This force element is a crude stop that turns on a spring when the knee
+// angle goes outside the range [low,high]. The spline data we have has a
+// limited range that we can't exceed.
+//------------------------------------------------------------------------------
+class MyStop : public Force::Custom::Implementation {
+public:
+ MyStop(const MobilizedBody& knee, Real low, Real high, Real stiffness)
+ : knee(knee), low(low), high(high), k(stiffness)
+ { assert(low <= high && stiffness >= 0); }
+
+ virtual void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+ {
+ const Real q = knee.getOneQ(state, 0);
+ const Real x = q < low ? q-low : (q > high ? q-high : 0);
+ knee.applyOneMobilityForce(state, 0, -k*x, mobilityForces);
+ }
+
+ virtual Real calcPotentialEnergy(const State& state) const {
+ const Real q = knee.getOneQ(state, 0);
+ const Real x = q < low ? q-low : (q > high ? q-high : 0);
+ return k*x*x/2;
+ }
+
+private:
+ const MobilizedBody& knee;
+ Real low, high, k;
+};
+
+//------------------------------------------------------------------------------
+// main program
+//------------------------------------------------------------------------------
+int main(int argc, char** argv) {
+try { // If anything goes wrong, an exception will be thrown.
+
+ int i = 0;
+
+ //--------------------------------------------------------------------------
+ // Experimental data points (x,y) of tibia origin (tibial plateau) measured
+ // w.r.t. to origin of the femur (hip joint center) in the femur frame as a
+ // function of knee joint angle. From Yamaguchi and Zajac, 1989.
+ //--------------------------------------------------------------------------
+ // Tibia X:
+ int npx = 12;
+ Real angX[] = {-2.094395102393, -1.745329251994, -1.396263401595, -1.047197551197,
+ -0.698131700798, -0.349065850399, -0.174532925199, 0.197344221443,
+ 0.337394955864, 0.490177570472, 1.521460267071, 2.094395102393};
+ Real kneeX[] = {-0.003200000000, 0.001790000000, 0.004110000000, 0.004100000000,
+ 0.002120000000, -0.001000000000, -0.003100000000, -0.005227000000,
+ -0.005435000000, -0.005574000000, -0.005435000000, -0.005250000000};
+ // Tibia Y; note that Y data is offset by -0.4 due to body frame placement.
+ int npy = 7;
+ Real angY[] = {-2.094395102393, -1.221730476396, -0.523598775598, -0.349065850399,
+ -0.174532925199, 0.159148563428, 2.094395102393};
+ Real kneeY[] = {-0.422600000000, -0.408200000000, -0.399000000000, -0.397600000000,
+ -0.396600000000, -0.395264000000, -0.396000000000 };
+
+ // Create SimTK Vectors to hold data points, and initialize from above arrays.
+ Vector ka_x (npx, angX); // measured knee angles when X data was collected
+ Vector ka_y (npy, angY); // measured knee angles when Y data was collected
+ Vector tib_x(npx, kneeX);
+ Vector tib_y(npy, kneeY);
+
+ #ifdef SHOULD_EXAGGERATE
+ // See above.
+ tib_x *= ExaggerateX;
+ tib_y = (tib_y+0.4)*ExaggerateY - 0.4; // exaggerate deviation only, not offset
+ #endif
+
+ // Generate splines from vectors of data points.
+ const int Degree = 3; // use cubics
+ SplineFitter<Real> fitterX = SplineFitter<Real>::fitFromGCV(Degree, ka_x, tib_x);
+ SplineFitter<Real> fitterY = SplineFitter<Real>::fitFromGCV(Degree, ka_y, tib_y);
+ Spline fx = fitterX.getSpline();
+ Spline fy = fitterY.getSpline();
+
+ //--------------------------------------------------------------------------
+ // Define the 6-spatial functions that specify the motion of the tibia as a
+ // a FunctionBased MobilizedBody (shank) w.r.t. to its parent (the thigh).
+ //--------------------------------------------------------------------------
+ // Each function has to return 1 Real value
+ std::vector<const Function*> functions(6);
+ // as a function of coordIndices (more than one per function)
+ std::vector< std::vector<int> > coordIndices(6);
+ // about a body-fixed axis for rotations or in parent translations
+ std::vector<Vec3> axes(6);
+
+ // Set the 1st and 2nd spatial rotation about the orthogonal (X then Y) axes as
+ // constant values. That is they don't contribute to motion nor do they have
+ // any coordinates in the equations of motion.
+ // |--------------------------------|
+ // | 1st function: rotation about X |
+ // |--------------------------------|
+ functions[0] = (new Function::Constant(0, 0));
+ std::vector<int> noIndex(0);
+ coordIndices[0] =(noIndex);
+
+ // |--------------------------------|
+ // | 2nd function: rotation about Y |
+ // |--------------------------------|
+ functions[1] = (new Function::Constant(0, 0));
+ coordIndices[1] = (noIndex);
+
+ // Set the spatial rotation about third axis to be the knee-extension
+ // angle (the one q) about the Z-axis of the tibia at the femoral condyles
+ // Define the coefficients of the linear function of the knee-angle with the
+ // spatial rotation about Z.
+ Vector coeff(2);
+ // Linear function x3 = coeff[0]*q + coeff[1]
+ coeff[0] = 1; coeff[1] = 0;
+ // |--------------------------------|
+ // | 3rd function: rotation about Z |
+ // |--------------------------------|
+ functions[2] = new Function::Linear(coeff);
+ // function of coordinate 0 (knee extension angle)
+ std::vector<int> qIndex(1,0);
+ coordIndices[2] = qIndex;
+
+ // Set the spatial translations as a function (splines) along the parent X and Y axes
+ // |-----------------------------------|
+ // | 4th function: translation about X |
+ // |-----------------------------------|
+ functions[3] = new Spline(fx); // Give the mobilizer a copy it can own.
+ coordIndices[3] =(qIndex);
+
+ // |-----------------------------------|
+ // | 5th function: translation about Y |
+ // |-----------------------------------|
+ functions[4] = new Spline(fy); // Give the mobilizer a copy it can own.
+ coordIndices[4] =(qIndex);
+
+ // |-----------------------------------|
+ // | 6th function: translation about Z |
+ // |-----------------------------------|
+ functions[5] = (new Function::Constant(0, 0));
+ coordIndices[5] = (noIndex);
+
+ // Construct the multibody system
+ const Real grav = 9.80665;
+ MultibodySystem system; system.setUseUniformBackground(true);
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::Gravity gravity(forces, matter, -YAxis, grav);
+ matter.setShowDefaultGeometry(true);
+
+ //--------------------------------------------------------------------------
+ // Define the model's physical (body) properties
+ //--------------------------------------------------------------------------
+ //Thigh
+ Body::Rigid femur(MassProperties(8.806, Vec3(0), Inertia(Vec3(0.1268, 0.0332, 0.1337))));
+ femur.addDecoration(Transform(Vec3(0, -0.21+0.1715, 0)),
+ DecorativeCylinder(0.04, 0.21).setColor(Orange).setOpacity(.5));
+
+ //Shank
+ Body::Rigid tibia(MassProperties(3.510, Vec3(0), Inertia(Vec3(0.0477, 0.0048, 0.0484))));
+ tibia.addDecoration(Transform(Vec3(0, -0.235+0.1862, 0)),
+ DecorativeCylinder(0.035, 0.235).setColor(Red));
+
+ //--------------------------------------------------------------------------
+ // Build the multibody system by adding mobilized bodies to the matter subsystem
+ //--------------------------------------------------------------------------
+ // Add the thigh via hip joint
+ MobilizedBody::Pin thigh(matter.Ground(), Transform(Vec3(0)), femur, Transform(Vec3(0.0020, 0.1715, 0)));
+
+ // This is how you might model the knee if you could only use a pin joint.
+ //MobilizedBody::Pin shank(thigh, Transform(Vec3(0.0033, -0.2294, 0)),
+ // tibia, Transform(Vec3(0.0, 0.1862, 0.0)));
+
+ // NOTE: function of Y-translation data was defined int the femur frame
+ // according to Yamaguchi and Zajac, which had the orgin at the hip joint
+ // center and the Y along the long-axis of the femur and Z out of the page.
+ MobilizedBody::FunctionBased shank(thigh, Transform(Vec3(0.0020, 0.1715, 0)),
+ tibia, Transform(Vec3(0.0, 0.1862, 0.0)),
+ 1, // # of mobilities (dofs) for this joint
+ functions, coordIndices);
+
+ // Add some stop springs so the knee angle won't get outside the range of spline
+ // data we have. This custom force element is defined above.
+ Force::Custom(forces, new MyStop(shank, -Pi/2, 0*Pi, 100));
+
+
+ //--------------------------------------------------------------------------
+ // Setup reporters so we can get some output.
+ //--------------------------------------------------------------------------
+ // Vizualizer Animation
+ Visualizer viz(system);
+ system.addEventReporter(new Visualizer::Reporter(viz, 0.01));
+ // Energy -- reporter defined above.
+ system.addEventReporter(new MyEnergyReporter(system, 0.01));
+
+ //--------------------------------------------------------------------------
+ // Complete the construction of the "const" part of the System and
+ // allocate the default state.
+ //--------------------------------------------------------------------------
+ system.realizeTopology();
+ // Get a copy of the default state to work with.
+ State state = system.getDefaultState();
+
+ //--------------------------------------------------------------------------
+ // Set modeling options if any (this one is not actually needed here).
+ //--------------------------------------------------------------------------
+ matter.setUseEulerAngles(state, true);
+ // Complete construction of the model, allocating additional state variables
+ // if necessary.
+ system.realizeModel(state);
+
+ //--------------------------------------------------------------------------
+ // Set initial conditions.
+ //--------------------------------------------------------------------------
+ // Hip and knee coordinates and speeds similar to early swing
+ double hip_angle = -45*Pi/180;
+ double knee_angle = 0*Pi/180;
+ double hip_vel = 1;
+ double knee_vel = -5.0;
+
+ // Set initial states (Q's and U's)
+ // Position
+ thigh.setOneQ(state, 0, hip_angle);
+ shank.setOneQ(state, 0, knee_angle);
+ // Speed
+ thigh.setOneU(state, 0, hip_vel);
+ shank.setOneU(state, 0, knee_vel);
+
+ //--------------------------------------------------------------------------
+ // Run simulation.
+ //--------------------------------------------------------------------------
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setAccuracy(Accuracy);
+ TimeStepper ts(system, integ);
+ ts.initialize(state); // set IC's
+ ts.stepTo(5.0);
+}
+catch (const exception& e) {
+ printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+}
+
+ return 0;
+}
+
+
diff --git a/Simbody/examples/ExampleLongPendulum.cpp b/Simbody/examples/ExampleLongPendulum.cpp
new file mode 100644
index 0000000..3e83d06
--- /dev/null
+++ b/Simbody/examples/ExampleLongPendulum.cpp
@@ -0,0 +1,74 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Long Pendulum *
+ * -------------------------------------------------------------------------- *
+ * 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-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* Simbody ExampleLongPendulum
+This example shows how to build a linked chain of bodies programmatically,
+simulate it, and produce a simple animation while it is simulating. */
+
+#include "Simbody.h"
+#include <iostream>
+
+using namespace SimTK;
+
+int main() {
+ try {
+ // Create the system.
+
+ MultibodySystem system; system.setUseUniformBackground(true);
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1));
+
+ MobilizedBody lastBody = matter.Ground();
+ for (int i = 0; i < 10; ++i) {
+ MobilizedBody::Ball pendulum(lastBody, Transform(Vec3(0)),
+ pendulumBody, Transform(Vec3(0, 1, 0)));
+ lastBody = pendulum;
+ }
+
+ Visualizer viz(system);
+ system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ Random::Gaussian random;
+ for (int i = 0; i < state.getNQ(); ++i)
+ state.updQ()[i] = random.getValue();
+
+ // Simulate it.
+
+ RungeKuttaMersonIntegrator integ(system);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(10.0);
+
+ } catch(const std::exception& e) {
+ std::cout << "EXCEPTION: " << e.what() << std::endl;
+ return 1;
+ }
+ return 0;
+}
diff --git a/Simbody/examples/ExampleMotor-TorqueLimited-Constraint.cpp b/Simbody/examples/ExampleMotor-TorqueLimited-Constraint.cpp
new file mode 100644
index 0000000..a26a6ed
--- /dev/null
+++ b/Simbody/examples/ExampleMotor-TorqueLimited-Constraint.cpp
@@ -0,0 +1,577 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Torque-limited motor using speed control Constraint *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "Simbody.h"
+#include <iostream>
+#include <algorithm>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+/*
+What this example demonstrates
+------------------------------
+1. Implementing a rate-controlled but torque-limited motor using a pair of
+ model elements: a speed-controlling Constraint element when operating
+ below the torque limit, and a constant-torque Force element when operating
+ at the limit (meaning we won't be able to maintain desired speed).
+2. Changing desired motor speed and torque limit externally from user input.
+3. Performing a momentum balance analysis when enabling the constraint, so
+ that discontinuous speed changes are made consistent with Newton's laws.
+4. Integrating with a small maximum step size, using just an Integrator
+ without a TimeStepper to handle events.
+5. Manually checking for events between integration steps, and handling
+ those events by making state changes before resuming integration.
+6. Creating sliders, menus, and screen text in the Visualizer.
+
+Compare this with ExampleMotor-TorqueLimited-Controller to see the same
+system implemented with simpler logic, using a Custom force element with a
+controller rather than a Constraint.
+
+The advantages of using an intermittent constraint to implement this motor are:
+(1) fast execution time and (2) perfect speed tracking while the constraint is
+enabled. The primary disadvantage is coding the logic needed for making the
+switch between speed controlled and torque controlled operation and back. Here
+we are simplifying that by looking for transitions only at discrete times rather
+than having the integrator isolate the event occurrence precisely. As a
+consequence, we have to deal with potentially large discrete state changes.
+
+Strategy used here
+------------------
+Assume initially that we can control speed and use the constraint. Then prior
+to each small integration step:
+1. If speed control is active: Monitor the required torque; if it exceeds the
+ limit then turn off the constraint and turn on the constant-torque element,
+ applying the torque in the same direction that the excessive constraint
+ torque would have been applied (opposing the impending speed error).
+2. If torque control is active: Monitor the speed error. When the applied
+ torque is in the same direction as the error (meaning it is making the error
+ worse), the speed must have gone from too slow to too fast or vice versa. Try
+ matching the speed and enabling the constraint. If the required torque is
+ within range, enable speed control. Otherwise, correct the direction of the
+ applied torque to oppose the speed error and remain in torque control.
+3. Periodically update the Visualizer display and poll for user input. A user
+ change to the desired speed always changes us to torque control until the
+ speed change has been achieved.
+
+Alternative strategies
+----------------------
+1. Because we're checking for events only between steps, our choice of step size
+here limits how accurately we can isolate the transition events between speed-
+and torque-control modes, requiring a small maximum step size for good event
+isolation. An alternative strategy is to let the integrator take whatever steps
+it wants but use a TimeStepper and event witness functions to automatically
+isolate events when they occur. That would allow much larger steps and better
+event isolation.
+2. A simpler implementation is to use only a force element to implement the
+motor, with a controller to generate the appropriate output torque to track a
+desired speed, with a limit set. This method is illustrated in the companion
+Simbody example ExampleMotor-TorqueLimited-Controller. This eliminates the need
+for any constraint-switching logic so makes for a simpler flow of control.
+However, depending on the control gains it may fail to track the desired speed
+well, or may make the problem stiff causing the integrator to require smaller
+time steps.
+*/
+
+namespace { // Use anonymous namespace to keep global symbols private.
+// Motor parameters.
+const Real MaxMotorSpeed = 10; // rad/s
+const Real MaxTorqueLimit = 500; // N-m
+const Real InitialMotorSpeed = 1;
+const Real InitialTorqueLimit = 100;
+
+// Joint stop material parameters.
+const Real StopStiffness = 10000; // stiffness in left joint stop
+const Real StopDissipation = 0.5; // dissipation rate
+
+// Integration step size, display update, user polling rates.
+const Real MaxStepSize = Real(1/240.); // 4 1/6 ms (240 Hz)
+const int DrawEveryN = 8; // 33 1/3 ms frame update (30 Hz)
+const int PollUserEveryN = 16; // 66 2/3 ms user response lag (15 Hz)
+
+
+//==============================================================================
+// MY MECHANISM
+//==============================================================================
+// This class builds the multibody system and supports the various operations
+// we'll need to turn the constraint on and off.
+class MyMechanism {
+public:
+ MyMechanism(); // Uses default destructor.
+
+ const MultibodySystem& getSystem() const {return m_system;}
+ const State& getDefaultState() const {return m_system.getDefaultState();}
+
+ Real getDesiredSpeed(const State& state) const
+ { return m_speedController.getSpeed(state); }
+ Real getTorqueLimit(const State& state) const
+ { return std::abs(m_torqueController.getForce(state)); }
+ Real getActualSpeed(const State& state) const
+ { return m_rtArm.getOneU(state, MobilizerUIndex(0)); }
+ Real getSpeedError(const State& state) const
+ { return getActualSpeed(state) - getDesiredSpeed(state); }
+
+ // Returns the actual torque being applied, whether from the constraint or
+ // the constant-torque element.
+ Real getMotorTorque(const State& state) const {
+ if (!isSpeedControlEnabled(state))
+ return m_torqueController.getForce(state);
+ m_system.realize(state, Stage::Acceleration); // for multipliers
+ return -m_speedController.getMultiplier(state); // watch sign convention
+ }
+
+ // Return true if speed control in effect; otherwise it's torque control.
+ bool isSpeedControlEnabled(const State& state) const
+ { return !m_speedController.isDisabled(state); }
+
+ // Switch from speed control to torque control. This leaves velocity
+ // unchanged but causes a reduction in motor torque. It always succeeds.
+ void switchToTorqueControl(State& state) const;
+
+ // Turn on speed control if possible, meaning that the acceleration can be
+ // made zero with torque below the limit. If the actual and desired
+ // speeds don't match exactly, then we must make an impulsive speed change
+ // that needs to be momentum balanced.
+ void tryToSwitchToSpeedControl(State& state) const;
+
+ // Change the desired speed. This will turn speed control off if it is on,
+ // until the speeds can be made to match using torque control.
+ void changeDesiredSpeed(State& state, Real newSpeed) const;
+
+ // Change the maximum torque limit. If we're already running torque limited
+ // this will cause an immediate change to the torque being applied.
+ void changeTorqueLimit(State& state, Real newTorqueLimit) const;
+
+ // Update the Visualizer, including the sliders.
+ void draw(const State& state) const;
+
+ // Returns true when it is time to quit.
+ bool processUserInput(State& state) const;
+
+private:
+ void constructSystem();
+ void setUpVisualizer();
+
+ MultibodySystem m_system;
+ SimbodyMatterSubsystem m_matter;
+ GeneralForceSubsystem m_forces;
+ Visualizer m_viz;
+ Visualizer::InputSilo* m_userInput; // just a ref; not owned here
+
+ MobilizedBody::Pin m_bodyT, m_leftArm, m_rtArm;
+ Constraint::ConstantSpeed m_speedController;
+ Force::MobilityConstantForce m_torqueController;
+};
+
+// Write interesting integrator info to stdout.
+void dumpIntegratorStats(const Integrator& integ);
+}
+
+
+//==============================================================================
+// MAIN
+//==============================================================================
+// Simulate forever with a small max step size.
+int main() {
+ try { // catch errors if any
+
+ // Create the Simbody MultibodySystem and Visualizer.
+ MyMechanism mech;
+
+ // We're forcing very small step sizes so should use very low order
+ // integration and loose accuracy. We must prevent interpolation so that the
+ // state returned after each step is the integrator's "advanced state",
+ // which is modifiable, in case we need to make a state change.
+ SemiExplicitEuler2Integrator integ(mech.getSystem());
+ integ.setAccuracy(Real(1e-1)); // 10%
+ integ.setAllowInterpolation(false);
+
+ integ.initialize(mech.getDefaultState());
+ unsigned stepNum = 0;
+ while (true) {
+ // Get access to State being advanced by the integrator. Interpolation
+ // must be off so that we're modifying the actual trajectory.
+ State& state = integ.updAdvancedState();
+
+ // Output a frame to the Visualizer if it is time.
+ if (stepNum % DrawEveryN == 0)
+ mech.draw(state);
+
+ // Check for user input periodically.
+ if (stepNum % PollUserEveryN == 0 && mech.processUserInput(state))
+ break; // stop if user picked Quit from the Run menu
+
+ // Check for speed/torque control transition events and handle.
+ const Real trqNow = mech.getMotorTorque(state);
+ if (mech.isSpeedControlEnabled(state)) {
+ if (std::abs(trqNow) > mech.getTorqueLimit(state)) {
+ printf("%d: SWITCH TO TORQUE CONTROL cuz trqNow=%g\n",
+ stepNum, trqNow);
+ mech.switchToTorqueControl(state);
+ }
+ } else { // Currently limiting torque.
+ // If the torque is now in the same direction as the error, try
+ // to switch back to speed control. If that doesn't work we'll at
+ // least reverse the applied maximum torque.
+ const Real errNow = mech.getSpeedError(state);
+ if (errNow * trqNow > 0) {
+ printf("%d: TRY SPEED CONTROL cuz errNow=%g, trqNow=%g\n",
+ stepNum, errNow, trqNow);
+ mech.tryToSwitchToSpeedControl(state);
+ }
+ }
+
+ // Advance time by MaxStepSize. Might take multiple internal steps to
+ // get there, depending on required accuracy.
+ integ.stepBy(MaxStepSize);
+ ++stepNum;
+ }
+
+ // DONE.
+ dumpIntegratorStats(integ);
+
+ } catch (const std::exception& e) {
+ std::cout << "ERROR: " << e.what() << std::endl;
+ return 1;
+ }
+ return 0;
+}
+
+
+
+//==============================================================================
+// MY MECHANISM IMPLEMENTATION
+//==============================================================================
+
+//------------------------------- CONSTRUCTOR ----------------------------------
+MyMechanism::MyMechanism()
+: m_system(), m_matter(m_system), m_forces(m_system), m_viz(m_system),
+ m_userInput(0)
+{
+ constructSystem();
+ setUpVisualizer();
+}
+
+//----------------------------- CONSTRUCT SYSTEM -------------------------------
+void MyMechanism::constructSystem() {
+ Force::Gravity(m_forces, m_matter, -YAxis, Real(9.80665));
+
+ // Describe a body with a point mass at (0, -3, 0) and draw a sphere there.
+ Real mass = 3; Vec3 pos(0,-3,0);
+ Body::Rigid bodyInfo(MassProperties(mass, pos, UnitInertia::pointMassAt(pos)));
+ bodyInfo.addDecoration(pos, DecorativeSphere(Real(.2)).setOpacity(.5));
+
+ // Create the tree of mobilized bodies, reusing the above body description.
+ m_bodyT = MobilizedBody::Pin(m_matter.Ground(),Vec3(0), bodyInfo,Vec3(0));
+ m_leftArm = MobilizedBody::Pin(m_bodyT,Vec3(-2,0,0), bodyInfo,Vec3(0));
+ m_rtArm = MobilizedBody::Pin(m_bodyT,Vec3(2,0,0), bodyInfo,Vec3(0,-1,0));
+
+ // Add some damping.
+ Force::MobilityLinearDamper(m_forces, m_bodyT, MobilizerUIndex(0), 10);
+ Force::MobilityLinearDamper(m_forces, m_leftArm, MobilizerUIndex(0), 30);
+ Force::MobilityLinearDamper(m_forces, m_rtArm, MobilizerUIndex(0), 10);
+
+ // Add a joint stop to the left arm restricting it to q in [0,Pi/5].
+ Force::MobilityLinearStop(m_forces, m_leftArm, MobilizerQIndex(0),
+ StopStiffness, StopDissipation,
+ -Pi/8, // lower stop
+ Pi/8); // upper stop
+
+ // Use built-in ConstantSpeed constraint as a low-budget motor model.
+ m_speedController = Constraint::ConstantSpeed(m_rtArm, InitialMotorSpeed);
+
+ // This is used when we're at the maximum torque.
+ m_torqueController = Force::MobilityConstantForce(m_forces, m_rtArm,
+ InitialTorqueLimit);
+ m_torqueController.setDisabledByDefault(true);
+
+ // We're done with the System; finalize it.
+ m_system.realizeTopology();
+}
+
+//------------------------- SWITCH TO TORQUE CONTROL ---------------------------
+// The switch to torque control causes a sudden drop in torque but that doesn't
+// require any sudden change to velocity.
+void MyMechanism::switchToTorqueControl(State& state) const {
+ assert(isSpeedControlEnabled(state));
+
+ const Real oldTrq = getMotorTorque(state);
+ const Real newTrq = sign(oldTrq)*getTorqueLimit(state);
+ printf(" switchToTorqueControl(): change torque from %g -> %g\n",
+ oldTrq, newTrq);
+
+ m_speedController.disable(state);
+ m_torqueController.enable(state);
+ m_torqueController.setForce(state, newTrq);
+ m_system.realize(state, Stage::Velocity);
+}
+
+//---------------------- TRY TO SWITCH TO SPEED CONTROL ------------------------
+// We want to switch to the constraint to enforce the desired speed.
+// If the actual speed and desired speed don't match, this will first require
+// an impulsive change to the speed. We'll perform a momentum balance analysis
+// here to calculate system wide velocity changes that will satisfy f=ma.
+// Solve GM\~G lambda = deltaV to calculate constraint impulse lambda.
+// Then f = ~G lambda is the corresponding generalized impulse.
+// And deltaU = M\f is the resulting change to the generalized speeds.
+//
+// But after all that, we might find that the torque required is more than
+// we allow. In that case we have to put the speed back where we found it
+// and switch back to torque control, but with the torque applied in the same
+// direction as the excessive constraint torque would have been.
+void MyMechanism::tryToSwitchToSpeedControl(State& state) const {
+ assert(!isSpeedControlEnabled(state));
+
+ const Real curSpeed = getActualSpeed(state);
+ const Real desSpeed = getDesiredSpeed(state);
+ printf(" tryToSwitchToSpeedControl(): torque now is %g, speed err=%g\n",
+ getMotorTorque(state), curSpeed-desSpeed);
+
+
+ // Save the current velocities in case we have to put them back.
+ const Vector prevU = state.getU();
+ m_torqueController.disable(state);
+ m_speedController.enable(state); // Tentatively enable the constraint.
+
+ // Dynamics operators require this stage.
+ m_system.realize(state, Stage::Velocity);
+
+ // Momentum balance analysis: see comment above.
+ Vector deltaV(1, desSpeed-curSpeed);
+ Vector allDeltaV, lambda, f, deltaU;
+ m_speedController.setMyPartInConstraintSpaceVector(state, deltaV, allDeltaV);
+ m_matter.solveForConstraintImpulses(state, allDeltaV, lambda);
+ m_matter.multiplyByGTranspose(state, lambda, f);
+ m_matter.multiplyByMInv(state,f,deltaU);
+ state.updU() += deltaU;
+ // This final projection shouldn't be necessary, but just in case ...
+ m_system.projectU(state); // leaves state at Stage::Velocity
+
+ // Now that the speed constraint is satisfied, try keeping the acceleration
+ // zero using the constraint and check how much torque that requires.
+ m_system.realize(state, Stage::Acceleration);
+ const Real requiredTorque = getMotorTorque(state);
+ if (std::abs(requiredTorque) > getTorqueLimit(state)) {
+ m_speedController.disable(state);
+ m_torqueController.enable(state);
+ const Real newMaxTrq = sign(requiredTorque)*getTorqueLimit(state);
+ m_torqueController.setForce(state, newMaxTrq);
+ printf(" ... NO switch to speed control cuz torque would be %g;"
+ " applying %g instead.\n",
+ requiredTorque, newMaxTrq);
+ state.updU() = prevU; // restore previous speed
+ m_system.realize(state, Stage::Velocity);
+ return;
+ }
+
+ printf(" ... switched to speed control with trq=%g and impulsive speed change:"
+ " %g -> %g\n", requiredTorque, curSpeed, desSpeed);
+ cout << " ... momentum balance deltaU was" << deltaU << endl;
+}
+
+//--------------------------- CHANGE DESIRED SPEED -----------------------------
+// The user has specified a new desired motor speed. This always requires a
+// switch to torque control if we're in speed control now, because an
+// instantaneous speed change would require an infinite torque. The torque
+// direction depends on whether the new desired speed is larger or smaller than
+// the current actual speed.
+void MyMechanism::changeDesiredSpeed(State& state, Real newDesiredSpeed) const {
+ const Real actualSpeed = getActualSpeed(state);
+ const Real changeDirection = (Real)sign(newDesiredSpeed - actualSpeed);
+ if (changeDirection == 0)
+ return; // nothing to do
+
+ printf("Desired speed change: %g -> %g\n", actualSpeed, newDesiredSpeed);
+ m_speedController.setSpeed(state, newDesiredSpeed);
+ if (isSpeedControlEnabled(state)) {
+ printf("...requires switch to torque control\n");
+ m_speedController.disable(state);
+ m_torqueController.enable(state);
+ }
+
+ // Torque control is on; set torque to move in direction of new speed.
+ const Real newTrq = changeDirection*getTorqueLimit(state);
+ m_torqueController.setForce(state, newTrq);
+ printf("...applying max torque %g to change speed\n", newTrq);
+
+ m_system.realize(state, Stage::Velocity);
+}
+
+//--------------------------- CHANGE TORQUE LIMIT ------------------------------
+// The user has specified a new value for the magnitude of the maximum torque.
+// The sign will be inherited from the sign of the torque currently in use. No
+// change to the control state is done here; that will occur in the main loop
+// if needed.
+void MyMechanism::changeTorqueLimit(State& state, Real newTorqueLimit) const {
+ const Real oldTorqueLimit = getTorqueLimit(state);
+ printf("Torque limit change: %g -> %g\n", oldTorqueLimit, newTorqueLimit);
+
+ // Put torque in same direction as current torque (might not be using the
+ // torque controller though).
+ const Real trq = getMotorTorque(state);
+ Real dir = (Real)sign(trq); if (dir==0) dir=1;
+ m_torqueController.setForce(state, dir*newTorqueLimit);
+}
+
+//---------------------------- PROCESS USER INPUT ------------------------------
+namespace {
+// Constants for the user interaction widgets.
+// Ids for the sliders.
+const int SliderIdMotorSpeed = 1, SliderIdTorqueLimit = 2,
+ SliderIdTach = 3, SliderIdTorque = 4; // these two are used for output
+// Ids for things on the Run Menu.
+const int MenuIdRun = 1;
+const int ResetItem=1, QuitItem=2;
+}
+
+// Return true if it is time to quit.
+bool MyMechanism::processUserInput(State& state) const {
+ int whichSlider, whichMenu, whichItem; Real newValue;
+
+ // Did a slider move?
+ if (m_userInput->takeSliderMove(whichSlider, newValue)) {
+ switch(whichSlider) {
+ case SliderIdMotorSpeed:
+ // This will momentum balance if necessary.
+ changeDesiredSpeed(state, newValue);
+ break;
+ case SliderIdTorqueLimit:
+ changeTorqueLimit(state, newValue);
+ m_viz.setSliderRange(SliderIdTorque, -newValue, newValue);
+ break;
+ }
+ }
+
+ // Was there a menu pick?
+ if (m_userInput->takeMenuPick(whichMenu, whichItem)) {
+ if (whichItem == QuitItem)
+ return true; // done
+
+ // If Reset, stop the motor and zero out all the q's and u's.
+ // Tell visualizer to update the sliders to match.
+ if (whichItem == ResetItem) {
+ // Don't momentum balance here!
+ m_speedController.setSpeed(state, 0);
+ m_viz.setSliderValue(SliderIdMotorSpeed, 0)
+ .setSliderValue(SliderIdTach, 0);
+
+ m_torqueController.setForce(state, InitialTorqueLimit);
+ m_viz.setSliderValue(SliderIdTorqueLimit, InitialTorqueLimit)
+ .setSliderRange(SliderIdTorque, -InitialTorqueLimit,
+ InitialTorqueLimit)
+ .setSliderValue(SliderIdTorque, 0);
+
+ state.updQ() = 0; // all positions to zero
+ state.updU() = 0; // all velocities to zero
+ m_system.projectQ(state);
+ m_system.projectU(state);
+ }
+ }
+
+ return false; // keep going
+}
+
+//----------------------------- CLASS SHOW STUFF -------------------------------
+// We'll provide the Visualizer with an object of this class to generate any
+// per-frame text and geometry we want to show on screen.
+namespace {
+class ShowStuff : public DecorationGenerator {
+public:
+ ShowStuff(const MyMechanism& mech) : m_mech(mech) {}
+
+ void generateDecorations(const State& state,
+ Array_<DecorativeGeometry>& geometry) OVERRIDE_11
+ {
+ DecorativeText msg;
+ msg.setIsScreenText(true);
+ if (m_mech.isSpeedControlEnabled(state))
+ msg.setText("OK");
+ else
+ msg.setText("TORQUE LIMITED err="
+ + String(m_mech.getSpeedError(state), "%.2g"));
+ geometry.push_back(msg);
+ }
+private:
+ const MyMechanism& m_mech;
+};
+}
+
+//----------------------------- SET UP VISUALIZER ------------------------------
+void MyMechanism::setUpVisualizer() {
+ m_viz.setShutdownWhenDestructed(true) // make sure display window dies
+ .setBackgroundType(Visualizer::SolidColor); // turn off Ground & Sky
+
+ // Add sliders.
+ m_viz.addSlider("Motor speed", SliderIdMotorSpeed,
+ -MaxMotorSpeed, MaxMotorSpeed, InitialMotorSpeed)
+ .addSlider("Torque limit", SliderIdTorqueLimit,
+ 0, MaxTorqueLimit, InitialTorqueLimit)
+ .addSlider("Tach", SliderIdTach,
+ -MaxMotorSpeed, MaxMotorSpeed, 0)
+ .addSlider("Torque", SliderIdTorque,
+ -InitialTorqueLimit, InitialTorqueLimit, 0);
+
+ // Add Run menu.
+ Array_<std::pair<String,int> > runMenuItems;
+ runMenuItems.push_back(std::make_pair("Reset", ResetItem));
+ runMenuItems.push_back(std::make_pair("Quit", QuitItem));
+ m_viz.addMenu("Run", MenuIdRun, runMenuItems);
+
+ // Add per-frame text and geometry.
+ m_viz.addDecorationGenerator(new ShowStuff(*this));
+
+ // Add an input listener so the user can talk to us.
+ m_userInput = new Visualizer::InputSilo();
+ m_viz.addInputListener(m_userInput);
+}
+
+//------------------------------------ DRAW ------------------------------------
+// Update the Visualizer, including the output sliders.
+void MyMechanism::draw(const State& state) const {
+ m_viz.report(state);
+ m_viz.setSliderValue(SliderIdTach, getActualSpeed(state))
+ .setSliderValue(SliderIdTorque, getMotorTorque(state));
+}
+
+
+
+//==============================================================================
+// DUMP INTEGRATOR STATS
+//==============================================================================
+namespace {
+void dumpIntegratorStats(const Integrator& integ) {
+ const int evals = integ.getNumRealizations();
+ std::cout << "\nDone -- simulated " << integ.getTime() << "s with "
+ << integ.getNumStepsTaken() << " steps, avg step="
+ << (1000*integ.getTime())/integ.getNumStepsTaken() << "ms "
+ << (1000*integ.getTime())/evals << "ms/eval\n";
+
+ printf("Used Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(),
+ integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(),
+ integ.getNumProjections());
+}
+}
+
diff --git a/Simbody/examples/ExampleMotor-TorqueLimited-Controller.cpp b/Simbody/examples/ExampleMotor-TorqueLimited-Controller.cpp
new file mode 100644
index 0000000..86f75cc
--- /dev/null
+++ b/Simbody/examples/ExampleMotor-TorqueLimited-Controller.cpp
@@ -0,0 +1,528 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Torque-limited motor using PI controller *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "Simbody.h"
+#include <iostream>
+#include <algorithm>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+/*
+What this example demonstrates
+------------------------------
+1. Implementing a rate-controlled but torque-limited motor using a Custom force
+ element that uses PI (Proportional-Integral control) to track speed when
+ possible without exceeding the torque limit.
+2. Changing desired motor speed and torque limit externally from user input.
+3. Integrating using just an Integrator without a TimeStepper with a modest
+ maximum step size that allows for output of Visualizer frames and polling for
+ user input between steps.
+4. Creating sliders, menus, and screen text in the Visualizer.
+
+Compare this with ExampleMotor-TorqueLimited-Constraint to see the same
+system implemented more efficiently, but with more complex logic, using
+an intermittent constraint.
+
+Strategy used here
+------------------
+Provide a Custom force element that implements the torque-limited motor. It
+allocates a state variable used for integral control so that a steady-state
+torque can be generated. There is also proportional control that generates a
+torque proportional to the tracking error. These are summed and then capped
+by the user-controlled torque limit. Two gains must be selected, with some
+tradeoff of performance for tracking effectiveness.
+
+The integration consists of repeated stepping by the maximum step size, with
+periodic output of Visualizer frames and polling for user input that may
+result in changes to the desired speed and torque limit. Nothing else need be
+managed in the integration loop since the motor force element is always on.
+
+Alternative strategies
+----------------------
+1. Because we are generating screen updates and polling for user input only
+between steps, we have to restrict the maximum step size accordingly. An
+alternative strategy is to let the integrator take whatever steps it wants but
+use a TimeStepper with a periodic event reporter to generate interpolated
+display updates when needed, and a periodic event handler to poll for user input
+and make state changes as a result. That would allow larger steps, depending on
+how much user response lag is tolerable (usually 100ms is acceptable).
+2. A more difficult approach is to use an intermittent constraint to control
+speed rather than doing it with a speed-tracking force element as is done here.
+That has the advantage of perfect speed tracking when the torque is below the
+limit, requires no arbitrary control gains, and can provide the best performance
+since a constraint-driven simulation is non-stiff. See the companion Simbody
+example ExampleMotor-TorqueLimited-Constraint for an implementation of this
+alternative.
+*/
+
+namespace { // Use anonymous namespace to keep global symbols private.
+// Motor parameters.
+const Real MaxMotorSpeed = 10; // rad/s
+const Real MaxTorqueLimit = 500; // N-m
+const Real InitialMotorSpeed = 1;
+const Real InitialTorqueLimit = 100;
+
+// Joint stop material parameters.
+const Real StopStiffness = 10000; // stiffness in left joint stop
+const Real StopDissipation = 0.5; // dissipation rate
+
+// Integration step size, display update, user polling rates.
+const Real MaxStepSize = Real(1/30.); // 33 1/3 ms (30 Hz)
+const int DrawEveryN = 1; // 33 1/3 ms frame update (30 Hz)
+const int PollUserEveryN = 2; // 66 2/3 ms user response lag (15 Hz)
+
+// Gains for the PI controller.
+const Real ProportionalGain = 1000;
+const Real IntegralGain = 50000;
+
+//==============================================================================
+// MY TORQUE LIMITED MOTOR
+//==============================================================================
+// This Force element implements a torque-limited motor that runs at a user-
+// selected speed unless that would require too much torque.
+class MyTorqueLimitedMotor : public Force::Custom::Implementation {
+public:
+ MyTorqueLimitedMotor(const MobilizedBody& mobod, MobilizerUIndex whichU)
+ : m_matter(mobod.getMatterSubsystem()), m_mobod(mobod), m_whichU(whichU)
+ {}
+
+ void setDesiredSpeed(State& state, Real speed) const {
+ Real& u_desired = Value<Real>::updDowncast(
+ m_matter.updDiscreteVariable(state, m_desiredUIx));
+ u_desired = speed;
+ }
+
+ Real getDesiredSpeed(const State& state) const {
+ const Real u_desired = Value<Real>::downcast(
+ m_matter.getDiscreteVariable(state, m_desiredUIx));
+ return u_desired;
+ }
+
+ void setTorqueLimit(State& state, Real speed) const {
+ Real& torqueLimit = Value<Real>::updDowncast(
+ m_matter.updDiscreteVariable(state, m_torqueLimitIx));
+ torqueLimit = speed;
+ }
+
+ Real getTorqueLimit(const State& state) const {
+ const Real torqueLimit = Value<Real>::downcast(
+ m_matter.getDiscreteVariable(state, m_torqueLimitIx));
+ return torqueLimit;
+ }
+
+ Real getActualSpeed(const State& state) const {
+ const Real u_actual = m_mobod.getOneU(state, m_whichU);
+ return u_actual;
+ }
+
+ Real getSpeedError(const State& state) const {
+ return getActualSpeed(state) - getDesiredSpeed(state);
+ }
+
+ // Combine integrated torque and proportional torque, and clamp to limit.
+ Real getTorque(const State& state) const {
+ const Real integTrq = m_matter.getZ(state)[m_trqIx];
+ const Real trqLimit = getTorqueLimit(state);
+ const Real proportionalTrq = -ProportionalGain*getSpeedError(state);
+ const Real totalTrq = clamp(-trqLimit, integTrq+proportionalTrq,
+ trqLimit);
+ return totalTrq;
+ }
+
+ // Set the integrated torque back to zero.
+ void resetIntegratedTorque(State& state) const
+ { m_matter.updZ(state)[m_trqIx] = 0; }
+
+
+ // Force::Custom::Implementation virtuals:
+
+ void calcForce(const State& state,
+ Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces,
+ Vector& mobilityForces) const OVERRIDE_11
+ {
+ m_mobod.applyOneMobilityForce(state, m_whichU, getTorque(state),
+ mobilityForces);
+ }
+
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11 {
+ return 0;
+ }
+
+ // Allocate state variables:
+ // Discrete variables for desired speed and torque limit;
+ // a continuous variable for the integral torque (a "z").
+ void realizeTopology(State& state) const OVERRIDE_11 {
+ m_desiredUIx = m_matter.allocateDiscreteVariable
+ (state, Stage::Acceleration, new Value<Real>(InitialMotorSpeed));
+ m_torqueLimitIx = m_matter.allocateDiscreteVariable
+ (state, Stage::Acceleration, new Value<Real>(InitialTorqueLimit));
+ m_trqIx = m_matter.allocateZ(state, Vector(1,Real(0)));
+ }
+
+ // Calculate the derivative for the integral control state.
+ void realizeAcceleration(const State& state) const OVERRIDE_11 {
+ const Real integTrq = m_matter.getZ(state)[m_trqIx];
+ const Real trqLimit = getTorqueLimit(state);
+ const Real abstrq=std::abs(integTrq), trqsign = (Real)sign(integTrq);
+
+ Real trqDot = -IntegralGain*getSpeedError(state);
+ // Don't ask for more torque if we're already past the limit
+ if (abstrq >= trqLimit && sign(trqDot)*trqsign >= 0)
+ trqDot = 0;
+
+ m_matter.updZDot(state)[m_trqIx] = trqDot;
+ }
+
+private:
+ const SimbodyMatterSubsystem& m_matter;
+ MobilizedBody m_mobod;
+ MobilizerUIndex m_whichU;
+
+ // Topology cache; written only once by realizeTopology().
+ mutable DiscreteVariableIndex m_desiredUIx;
+ mutable DiscreteVariableIndex m_torqueLimitIx;
+ mutable ZIndex m_trqIx;
+};
+
+
+
+//==============================================================================
+// MY MECHANISM
+//==============================================================================
+// This class builds the multibody system and supports the various operations
+// we'll need to turn the constraint on and off.
+class MyMechanism {
+public:
+ MyMechanism(); // Uses default destructor.
+
+ const MultibodySystem& getSystem() const {return m_system;}
+ const State& getDefaultState() const {return m_system.getDefaultState();}
+
+ Real getDesiredSpeed(const State& state) const
+ { return m_controller->getDesiredSpeed(state); }
+ Real getTorqueLimit(const State& state) const
+ { return m_controller->getTorqueLimit(state); }
+ Real getActualSpeed(const State& state) const
+ { return m_controller->getActualSpeed(state); }
+ Real getSpeedError(const State& state) const
+ { return m_controller->getSpeedError(state); }
+
+ // Returns the torque currently being applied.
+ Real getMotorTorque(const State& state) const
+ { return m_controller->getTorque(state); }
+
+ // Change the desired speed.
+ void changeDesiredSpeed(State& state, Real newSpeed) const;
+
+ // Change the maximum torque limit.
+ void changeTorqueLimit(State& state, Real newTorqueLimit) const;
+
+ // Update the Visualizer, including the sliders.
+ void draw(const State& state) const;
+
+ // Returns true when it is time to quit.
+ bool processUserInput(State& state) const;
+
+private:
+ void constructSystem();
+ void setUpVisualizer();
+
+ MultibodySystem m_system;
+ SimbodyMatterSubsystem m_matter;
+ GeneralForceSubsystem m_forces;
+ Visualizer m_viz;
+ Visualizer::InputSilo* m_userInput; // just a ref; not owned here
+
+ MobilizedBody::Pin m_bodyT, m_leftArm, m_rtArm;
+ MyTorqueLimitedMotor* m_controller; // just a ref; not owned here
+};
+
+// Write interesting integrator info to stdout.
+void dumpIntegratorStats(const Integrator& integ);
+}
+
+
+//==============================================================================
+// MAIN
+//==============================================================================
+// Simulate forever with a maximum step size small enough to provide adequate
+// polling for user input and screen updating.
+int main() {
+ try { // catch errors if any
+
+ // Create the Simbody MultibodySystem and Visualizer.
+ MyMechanism mech;
+
+ // We want real time performance here and don't need high accuracy, so we'll
+ // use semi explicit Euler. However, we hope to take fairly large steps so
+ // need error control to avoid instability. We must also prevent
+ // interpolation so that the state returned after each step is the
+ // integrator's "advanced state", which is modifiable, so that we can update
+ // it in response to user input.
+ SemiExplicitEuler2Integrator integ(mech.getSystem());
+ integ.setAccuracy(Real(1e-1)); // 10%
+ integ.setAllowInterpolation(false);
+
+ integ.initialize(mech.getDefaultState());
+ unsigned stepNum = 0;
+ while (true) {
+ // Get access to State being advanced by the integrator. Interpolation
+ // must be off so that we're modifying the actual trajectory.
+ State& state = integ.updAdvancedState();
+
+ // Output a frame to the Visualizer if it is time.
+ if (stepNum % DrawEveryN == 0)
+ mech.draw(state);
+
+ // Check for user input periodically.
+ if (stepNum % PollUserEveryN == 0 && mech.processUserInput(state))
+ break; // stop if user picked Quit from the Run menu
+
+ // Advance time by MaxStepSize. Might take multiple internal steps to
+ // get there, depending on difficulty and required accuracy.
+ integ.stepBy(MaxStepSize);
+ ++stepNum;
+ }
+
+ // DONE.
+ dumpIntegratorStats(integ);
+
+ } catch (const std::exception& e) {
+ std::cout << "ERROR: " << e.what() << std::endl;
+ return 1;
+ }
+ return 0;
+}
+
+
+
+//==============================================================================
+// MY MECHANISM IMPLEMENTATION
+//==============================================================================
+
+//------------------------------- CONSTRUCTOR ----------------------------------
+MyMechanism::MyMechanism()
+: m_system(), m_matter(m_system), m_forces(m_system), m_viz(m_system),
+ m_userInput(0), m_controller(0)
+{
+ constructSystem();
+ setUpVisualizer();
+}
+
+
+//----------------------------- CONSTRUCT SYSTEM -------------------------------
+void MyMechanism::constructSystem() {
+ Force::Gravity(m_forces, m_matter, -YAxis, Real(9.80665));
+
+ // Describe a body with a point mass at (0, -3, 0) and draw a sphere there.
+ Real mass = 3; Vec3 pos(0,-3,0);
+ Body::Rigid bodyInfo(MassProperties(mass, pos, UnitInertia::pointMassAt(pos)));
+ bodyInfo.addDecoration(pos, DecorativeSphere(Real(.2)).setOpacity(.5));
+
+ // Create the tree of mobilized bodies, reusing the above body description.
+ m_bodyT = MobilizedBody::Pin(m_matter.Ground(),Vec3(0), bodyInfo,Vec3(0));
+ m_leftArm = MobilizedBody::Pin(m_bodyT,Vec3(-2, 0, 0), bodyInfo,Vec3(0));
+ m_rtArm = MobilizedBody::Pin(m_bodyT,Vec3(2, 0, 0), bodyInfo,Vec3(0,-1,0));
+
+ // Add some damping.
+ Force::MobilityLinearDamper(m_forces, m_bodyT, MobilizerUIndex(0), 10);
+ Force::MobilityLinearDamper(m_forces, m_leftArm, MobilizerUIndex(0), 30);
+ Force::MobilityLinearDamper(m_forces, m_rtArm, MobilizerUIndex(0), 10);
+
+ // Add a joint stop to the left arm restricting it to q in [0,Pi/5].
+ Force::MobilityLinearStop(m_forces, m_leftArm, MobilizerQIndex(0),
+ StopStiffness,
+ StopDissipation,
+ -Pi/8, // lower stop
+ Pi/8); // upper stop
+
+ // Use custom controller to track speed up to torque limit.
+ m_controller = new MyTorqueLimitedMotor(m_rtArm, MobilizerUIndex(0));
+ Force::Custom(m_forces, m_controller); // takes over ownership
+
+ // We're done with the System; finalize it.
+ m_system.realizeTopology();
+}
+
+//--------------------------- CHANGE DESIRED SPEED -----------------------------
+// The user has specified a new desired motor speed.
+void MyMechanism::changeDesiredSpeed(State& state, Real newDesiredSpeed) const {
+ const Real actualSpeed = getActualSpeed(state);
+ printf("Desired speed change: %g -> %g\n", actualSpeed, newDesiredSpeed);
+ m_controller->setDesiredSpeed(state, newDesiredSpeed);
+}
+
+
+//--------------------------- CHANGE TORQUE LIMIT ------------------------------
+// The user has specified a new value for the magnitude of the maximum torque.
+// The direction will oppose the speed error; we just need magnitude here.
+void MyMechanism::changeTorqueLimit(State& state, Real newTorqueLimit) const {
+ const Real oldTorqueLimit = getTorqueLimit(state);
+ printf("Torque limit change: %g -> %g\n", oldTorqueLimit, newTorqueLimit);
+ m_controller->setTorqueLimit(state, newTorqueLimit);
+}
+
+
+//---------------------------- PROCESS USER INPUT ------------------------------
+namespace {
+// Constants for the user interaction widgets.
+// Ids for the sliders.
+const int SliderIdMotorSpeed = 1, SliderIdTorqueLimit = 2,
+ SliderIdTach = 3, SliderIdTorque = 4; // these two are used for output
+// Ids for things on the Run Menu.
+const int MenuIdRun = 1;
+const int ResetItem=1, QuitItem=2;
+}
+
+// Return true if it is time to quit.
+bool MyMechanism::processUserInput(State& state) const {
+ int whichSlider, whichMenu, whichItem; Real newValue;
+
+ // Did a slider move?
+ if (m_userInput->takeSliderMove(whichSlider, newValue)) {
+ switch(whichSlider) {
+ case SliderIdMotorSpeed:
+ changeDesiredSpeed(state, newValue);
+ break;
+ case SliderIdTorqueLimit:
+ changeTorqueLimit(state, newValue);
+ m_viz.setSliderRange(SliderIdTorque, -newValue, newValue);
+ break;
+ }
+ }
+
+ // Was there a menu pick?
+ if (m_userInput->takeMenuPick(whichMenu, whichItem)) {
+ if (whichItem == QuitItem)
+ return true; // done
+
+ // If Reset, stop the motor and zero out all the q's and u's.
+ // Tell visualizer to update the sliders to match.
+ if (whichItem == ResetItem) {
+ m_controller->resetIntegratedTorque(state);
+ m_controller->setDesiredSpeed(state, 0);
+ m_viz.setSliderValue(SliderIdMotorSpeed, 0)
+ .setSliderValue(SliderIdTach, 0);
+
+ m_controller->setTorqueLimit(state, InitialTorqueLimit);
+ m_viz.setSliderValue(SliderIdTorqueLimit, InitialTorqueLimit)
+ .setSliderRange(SliderIdTorque, -InitialTorqueLimit,
+ InitialTorqueLimit)
+ .setSliderValue(SliderIdTorque, 0);
+
+ state.updQ() = 0; // all positions to zero
+ state.updU() = 0; // all velocities to zero
+ m_system.projectQ(state);
+ m_system.projectU(state);
+ }
+ }
+
+ return false; // keep going
+}
+
+//----------------------------- CLASS SHOW STUFF -------------------------------
+// We'll provide the Visualizer with an object of this class to generate any
+// per-frame text and geometry we want to show on screen.
+namespace {
+class ShowStuff : public DecorationGenerator {
+public:
+ ShowStuff(const MyMechanism& mech) : m_mech(mech) {}
+
+ void generateDecorations(const State& state,
+ Array_<DecorativeGeometry>& geometry) OVERRIDE_11
+ {
+ DecorativeText msg;
+ msg.setIsScreenText(true);
+ if (std::abs(m_mech.getMotorTorque(state)) < m_mech.getTorqueLimit(state))
+ msg.setText("OK");
+ else
+ msg.setText("TORQUE LIMITED err="
+ + String(m_mech.getSpeedError(state), "%.2g"));
+ geometry.push_back(msg);
+ }
+private:
+ const MyMechanism& m_mech;
+};
+}
+
+
+//----------------------------- SET UP VISUALIZER ------------------------------
+void MyMechanism::setUpVisualizer() {
+ m_viz.setShutdownWhenDestructed(true) // make sure display window dies
+ .setBackgroundType(Visualizer::SolidColor); // turn off Ground & Sky
+
+ // Add sliders.
+ m_viz.addSlider("Motor speed", SliderIdMotorSpeed,
+ -MaxMotorSpeed, MaxMotorSpeed, InitialMotorSpeed)
+ .addSlider("Torque limit", SliderIdTorqueLimit,
+ 0, MaxTorqueLimit, InitialTorqueLimit)
+ .addSlider("Tach", SliderIdTach,
+ -MaxMotorSpeed, MaxMotorSpeed, 0)
+ .addSlider("Torque", SliderIdTorque,
+ -InitialTorqueLimit, InitialTorqueLimit, 0);
+
+ // Add Run menu.
+ Array_<std::pair<String,int> > runMenuItems;
+ runMenuItems.push_back(std::make_pair("Reset", ResetItem));
+ runMenuItems.push_back(std::make_pair("Quit", QuitItem));
+ m_viz.addMenu("Run", MenuIdRun, runMenuItems);
+
+ // Add per-frame text and geometry.
+ m_viz.addDecorationGenerator(new ShowStuff(*this));
+
+ // Add an input listener so the user can talk to us.
+ m_userInput = new Visualizer::InputSilo();
+ m_viz.addInputListener(m_userInput);
+}
+
+//------------------------------------ DRAW ------------------------------------
+// Update the Visualizer, including the sliders.
+void MyMechanism::draw(const State& state) const {
+ m_viz.report(state);
+ m_viz.setSliderValue(SliderIdTach, getActualSpeed(state))
+ .setSliderValue(SliderIdTorque, getMotorTorque(state));
+}
+
+
+
+//==============================================================================
+// DUMP INTEGRATOR STATS
+//==============================================================================
+namespace {
+void dumpIntegratorStats(const Integrator& integ) {
+ const int evals = integ.getNumRealizations();
+ std::cout << "\nDone -- simulated " << integ.getTime() << "s with "
+ << integ.getNumStepsTaken() << " steps, avg step="
+ << (1000*integ.getTime())/integ.getNumStepsTaken() << "ms "
+ << (1000*integ.getTime())/evals << "ms/eval\n";
+
+ printf("Used Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(),
+ integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(),
+ integ.getNumProjections());
+}
+}
diff --git a/Simbody/examples/ExampleMotor-TorqueLimited-Motion.cpp b/Simbody/examples/ExampleMotor-TorqueLimited-Motion.cpp
new file mode 100644
index 0000000..7aa3b95
--- /dev/null
+++ b/Simbody/examples/ExampleMotor-TorqueLimited-Motion.cpp
@@ -0,0 +1,630 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Torque-limited motor using prescribed motion *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "Simbody.h"
+#include <iostream>
+#include <algorithm>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+/*
+What this example demonstrates
+------------------------------
+1. Implementing a rate-controlled but torque-limited motor using a pair of model
+ elements: (a) a prescribed velocity Motion specification when operating below
+ the torque limit, and (b) a constant-torque Force element when operating
+ at the limit (meaning we won't be able to maintain desired speed). Only one
+ of these elements is enabled at a time.
+2. Changing desired motor speed and torque limit externally from user input.
+3. Performing a momentum balance analysis when activating the prescribed motion,
+ so that discontinuous speed changes are made consistent with Newton's laws.
+4. Integrating with a small maximum step size, using just an Integrator
+ without a TimeStepper to handle events.
+5. Manually checking for events between integration steps, and handling
+ those events by making state changes before resuming integration.
+6. Creating sliders, menus, and screen text in the Visualizer.
+
+The advantages of using an intermittent Motion to implement this motor are:
+(1) perfect speed tracking while the Motion is enabled, (2) no need to choose
+control gains, and (3) fastest execution time since there are no stiff forces
+and no Constraints. The primary disadvantage is coding the logic needed for
+making the switch between speed controlled and torque controlled operation and
+back. Here we are simplifying that by looking for transitions only at discrete
+times rather than having the integrator isolate the event occurrence precisely.
+As a consequence, we have to deal with potentially large discrete state changes.
+This could be ignored in some low-accuracy applications, but for physically
+consistent discrete velocity changes we need to perform an impulse ("momentum
+balance") analysis to ensure that the sudden change is consistent with a
+physical impact. You can undefine MOMENTUM_BALANCE below to disable this.
+
+Strategy used here
+------------------
+Assume initially that we can control speed and use the prescribed Motion
+element. Then prior to each small integration step:
+1. If speed control is active: Monitor the required torque; if it exceeds the
+ limit then turn off the Motion and turn on the constant-torque force
+ element, applying the torque in the same direction that the excessive
+ prescribed motion torque would have been applied (opposing the impending
+ speed error).
+2. If torque control is active: Monitor the speed error. When the applied
+ torque is in the same direction as the error (meaning it is making the error
+ worse), the speed must have gone from too slow to too fast or vice versa. Try
+ matching the speed and activating the speed lock. If the required torque is
+ within range, enable speed control. Otherwise, correct the direction of the
+ applied torque to oppose the speed error and remain in torque control.
+3. Periodically update the Visualizer display and poll for user input. A user
+ change to the desired speed always changes us to torque control until the
+ speed change has been achieved.
+
+Alternative strategies
+----------------------
+1. Because we're checking for events only between steps, our choice of step size
+here limits how accurately we can isolate the transition events between speed-
+and torque-control modes, requiring a small maximum step size for good event
+isolation. An alternative strategy is to let the integrator take whatever steps
+it wants but use a TimeStepper and event witness functions to automatically
+isolate events when they occur. That would allow much larger steps and better
+event isolation.
+2. A simpler implementation is to use only a force element to implement the
+motor, with a controller to generate the appropriate output torque to track a
+desired speed, with a limit set. This method is illustrated in the companion
+Simbody example ExampleMotor-TorqueLimited-Controller. This eliminates the need
+for any switching logic so makes for a simpler flow of control. However,
+depending on the control gains it may fail to track the desired speed well, or
+may make the problem stiff causing the integrator to require smaller time steps.
+3. You can use a Constraint element \e added to the system instead of
+prescribing the mobilizer motion with a Motion element. Constraints are more
+flexible in general (for example you can constrain just one coordinate of a
+multi-dof mobilizer), but for systems where prescribed motion can work it is
+always faster. You can see this done with a Constraint in the companion example
+ExampleMotor-TorqueLimited-Constraint.
+*/
+
+// Comment this out to allow enabling speed control (causing some speed
+// discontinuity) without performing an impulse analysis to conserve momentum
+// across the speed change.
+#define MOMENTUM_BALANCE
+
+namespace { // Use anonymous namespace to keep global symbols private.
+// Motor parameters.
+const Real MaxMotorSpeed = 10; // rad/s
+const Real MaxTorqueLimit = 500; // N-m
+const Real InitialMotorSpeed = 1;
+const Real InitialTorqueLimit = 100;
+
+// Joint stop material parameters.
+const Real StopStiffness = 10000; // stiffness in left joint stop
+const Real StopDissipation = 0.5; // dissipation rate
+
+// Integration step size, display update, user polling rates. Larger steps will
+// require greater velocity discontinuities.
+const Real MaxStepSize = Real(1/240.); // 4 1/6 ms (240 Hz)
+const int DrawEveryN = 8; // 33 1/3 ms frame update (30 Hz)
+const int PollUserEveryN = 16; // 66 2/3 ms user response lag (15 Hz)
+
+//==============================================================================
+// MY MECHANISM
+//==============================================================================
+// This class builds the multibody system and supports the various operations
+// we'll need to turn the prescribed Motion on and off.
+class MyMechanism {
+public:
+ MyMechanism(); // Uses default destructor.
+
+ const MultibodySystem& getSystem() const {return m_system;}
+ const State& getDefaultState() const {return m_system.getDefaultState();}
+
+ Real getDesiredSpeed(const State& state) const
+ { return m_speedController.getOneRate(state, MobilizerUIndex(0)); }
+ Real getTorqueLimit(const State& state) const
+ { return std::abs(m_torqueController.getForce(state)); }
+ Real getActualSpeed(const State& state) const
+ { return m_rtArm.getOneU(state, MobilizerUIndex(0)); }
+ Real getSpeedError(const State& state) const
+ { return getActualSpeed(state) - getDesiredSpeed(state); }
+
+ // Returns the actual torque being applied, whether from the Motion or
+ // the constant-torque element. Watch the sign convention -- prescribed
+ // motion torques tau (and constraint multipliers lambda) are on the LHS of
+ // the equations of motion while applied forces are on the RHS. We want to
+ // return the equivalent applied torque.
+ Real getMotorTorque(const State& state) const {
+ if (!isSpeedControlEnabled(state))
+ return m_torqueController.getForce(state);
+ m_system.realize(state, Stage::Acceleration); // for taus
+ return -m_rtArm.getOneTau(state, MobilizerUIndex(0)); // LHS->RHS
+ }
+
+ // Return true if speed control in effect; otherwise it's torque control.
+ bool isSpeedControlEnabled(const State& state) const
+ { return !m_speedController.isDisabled(state); }
+
+ // Switch from speed control to torque control. This leaves velocity
+ // unchanged but causes a reduction in motor torque. It always succeeds.
+ void switchToTorqueControl(State& state) const;
+
+ // Turn on speed control if possible, meaning that the acceleration can be
+ // made zero with torque below the limit. If the actual and desired
+ // speeds don't match exactly, then we must make an impulsive speed change
+ // that needs to be momentum balanced.
+ void tryToSwitchToSpeedControl(State& state) const;
+
+ // Change the desired speed. This will turn speed control off if it is on,
+ // until the speeds can be made to match using torque control.
+ void changeDesiredSpeed(State& state, Real newSpeed) const;
+
+ // Change the maximum torque limit. If we're already running torque limited
+ // this will cause an immediate change to the torque being applied.
+ void changeTorqueLimit(State& state, Real newTorqueLimit) const;
+
+ // Update the Visualizer, including the sliders.
+ void draw(const State& state) const;
+
+ // Returns true when it is time to quit.
+ bool processUserInput(State& state) const;
+
+private:
+ void constructSystem();
+ void setUpVisualizer();
+
+ MultibodySystem m_system;
+ SimbodyMatterSubsystem m_matter;
+ GeneralForceSubsystem m_forces;
+ Visualizer m_viz;
+ Visualizer::InputSilo* m_userInput; // just a ref; not owned here
+
+ MobilizedBody m_bodyT;
+ MobilizedBody::Pin m_leftArm;
+ MobilizedBody::Pin m_rtArm; // this one is speed controlled
+ Motion::Steady m_speedController;
+ Force::MobilityConstantForce m_torqueController;
+};
+
+// Write interesting integrator info to stdout.
+void dumpIntegratorStats(const Integrator& integ);
+}
+
+
+//==============================================================================
+// MAIN
+//==============================================================================
+// Simulate forever with a small max step size.
+int main() {
+ try { // catch errors if any
+
+ // Create the Simbody MultibodySystem and Visualizer.
+ MyMechanism mech;
+
+ // We're forcing very small step sizes so should use very low order
+ // integration and loose accuracy. We must prevent interpolation so that the
+ // state returned after each step is the integrator's "advanced state",
+ // which is modifiable, in case we need to make a state change.
+ SemiExplicitEuler2Integrator integ(mech.getSystem());
+ integ.setAccuracy(Real(1e-1)); // 10%
+ integ.setAllowInterpolation(false);
+
+ integ.initialize(mech.getDefaultState());
+ unsigned stepNum = 0;
+ while (true) {
+ // Get access to State being advanced by the integrator. Interpolation
+ // must be off so that we're modifying the actual trajectory.
+ State& state = integ.updAdvancedState();
+
+ // Output a frame to the Visualizer if it is time.
+ if (stepNum % DrawEveryN == 0)
+ mech.draw(state);
+
+ // Check for user input periodically.
+ if (stepNum % PollUserEveryN == 0 && mech.processUserInput(state))
+ break; // stop if user picked Quit from the Run menu
+
+ // Check for speed/torque control transition events and handle.
+ const Real trqNow = mech.getMotorTorque(state);
+ if (mech.isSpeedControlEnabled(state)) {
+ if (std::abs(trqNow) > mech.getTorqueLimit(state)) {
+ printf("\n%d: SWITCH TO TORQUE CONTROL cuz trqNow=%g\n",
+ stepNum, trqNow);
+ mech.switchToTorqueControl(state);
+ }
+ } else { // Currently limiting torque.
+ // If the torque is now in the same direction as the error, try
+ // to switch back to speed control. If that doesn't work we'll at
+ // least reverse the applied maximum torque.
+ const Real errNow = mech.getSpeedError(state);
+ if (errNow * trqNow > 0) {
+ printf("\n%d: TRY SPEED CONTROL cuz errNow=%g, trqNow=%g\n",
+ stepNum, errNow, trqNow);
+ mech.tryToSwitchToSpeedControl(state);
+ }
+ }
+
+ // Advance time by MaxStepSize. Might take multiple internal steps to
+ // get there, depending on required accuracy.
+ integ.stepBy(MaxStepSize);
+ ++stepNum;
+ }
+
+ // DONE.
+ dumpIntegratorStats(integ);
+
+ } catch (const std::exception& e) {
+ std::cout << "ERROR: " << e.what() << std::endl;
+ return 1;
+ }
+ return 0;
+}
+
+
+
+//==============================================================================
+// MY MECHANISM IMPLEMENTATION
+//==============================================================================
+
+//------------------------------- CONSTRUCTOR ----------------------------------
+MyMechanism::MyMechanism()
+: m_system(), m_matter(m_system), m_forces(m_system), m_viz(m_system),
+ m_userInput(0)
+{
+ constructSystem();
+ setUpVisualizer();
+}
+
+//----------------------------- CONSTRUCT SYSTEM -------------------------------
+void MyMechanism::constructSystem() {
+ Force::Gravity(m_forces, m_matter, -YAxis, Real(9.80665));
+
+ // Describe a body with a point mass at (0, -3, 0) and draw a sphere there.
+ Real mass = 3; Vec3 pos(0,-3,0);
+ Body::Rigid bodyInfo(MassProperties(mass, pos, UnitInertia::pointMassAt(pos)));
+ bodyInfo.addDecoration(pos, DecorativeSphere(Real(.2)).setOpacity(.5));
+
+ // Create the tree of mobilized bodies, reusing the above body description.
+ m_bodyT = MobilizedBody::Pin(m_matter.Ground(),Vec3(0), bodyInfo,Vec3(0));
+ m_leftArm = MobilizedBody::Pin(m_bodyT,Vec3(-2,0,0), bodyInfo,Vec3(0));
+ m_rtArm = MobilizedBody::Pin(m_bodyT,Vec3(2,0,0), bodyInfo,Vec3(0,-1,0));
+
+ // Add some damping.
+ Force::MobilityLinearDamper(m_forces, m_bodyT, MobilizerUIndex(0), 10);
+ Force::MobilityLinearDamper(m_forces, m_leftArm, MobilizerUIndex(0), 30);
+ Force::MobilityLinearDamper(m_forces, m_rtArm, MobilizerUIndex(0), 10);
+
+ // Add a joint stop to the left arm restricting it to q in [0,Pi/5].
+ Force::MobilityLinearStop(m_forces, m_leftArm, MobilizerQIndex(0),
+ StopStiffness, StopDissipation,
+ -Pi/8, // lower stop
+ Pi/8); // upper stop
+
+ // Use built-in Steady Motion as a low-budget motor model.
+ m_speedController = Motion::Steady(m_rtArm, InitialMotorSpeed);
+
+ // This is used when we're at the maximum torque.
+ m_torqueController = Force::MobilityConstantForce(m_forces, m_rtArm,
+ InitialTorqueLimit);
+ m_torqueController.setDisabledByDefault(true);
+
+ // We're done with the System; finalize it.
+ m_system.realizeTopology();
+}
+
+//------------------------- SWITCH TO TORQUE CONTROL ---------------------------
+// The switch to torque control causes a sudden drop in torque but that doesn't
+// require any sudden change to velocity.
+void MyMechanism::switchToTorqueControl(State& state) const {
+ assert(isSpeedControlEnabled(state));
+
+ const Real oldTrq = getMotorTorque(state);
+ const Real newTrq = sign(oldTrq)*getTorqueLimit(state);
+ printf(" switchToTorqueControl(): change torque from %g -> %g\n",
+ oldTrq, newTrq);
+
+ m_speedController.disable(state);
+ m_torqueController.enable(state);
+ m_torqueController.setForce(state, newTrq);
+ m_system.realize(state, Stage::Velocity);
+}
+
+//---------------------- TRY TO SWITCH TO SPEED CONTROL ------------------------
+// We want to switch to prescribed motion to enforce the desired speed.
+// If the actual speed and desired speed don't match, this will first require
+// an impulsive change to the speed. We'll perform a momentum balance analysis
+// here to calculate system wide velocity changes that will satisfy f=ma.
+// To do that we'll set the desired deltaV as though it were a prescribed
+// acceleration, then use the calcAcceleration() operator with all velocities
+// set to zero to calculate the overall deltaU needed to conserve momentum
+// across the impulsive velocity change.
+//
+// But after all that, we might find that the torque required is more than
+// we allow. In that case we have to put the speed back where we found it
+// and switch back to torque control, but with the torque applied in the same
+// direction as the excessive prescribed motion torque would have been.
+//
+// (Actually for a Ground-attached
+// system we won't be able to conserve linear momentum but the angular
+// momentum about the ground origin is conserved. If you want to see linear
+// momentum conserved as well, make the system free flying by changing
+// the bodyT mobilizer from Pin to Planar. You'll also want to turn off gravity
+// in that case so the system will stay where you can see it.)
+void MyMechanism::tryToSwitchToSpeedControl(State& state) const {
+ assert(!isSpeedControlEnabled(state));
+
+ const Real curSpeed = getActualSpeed(state);
+ const Real desSpeed = getDesiredSpeed(state);
+ const Real deltaV = desSpeed - curSpeed;
+ printf(" tryToSwitchToSpeedControl(): torque now is %g, speed err=%g\n",
+ getMotorTorque(state), -deltaV);
+
+ m_system.realize(state, Stage::Velocity);
+ SpatialVec momBefore = m_matter.calcSystemMomentumAboutGroundOrigin(state);
+
+ // Save the current velocities in case we have to put them back.
+ const Vector prevU = state.getU();
+ m_torqueController.disable(state);
+ m_speedController.enable(state); // Tentatively enable the constraint.
+
+ Vector deltaU(state.getNU(), Real(0));
+
+ #ifndef MOMENTUM_BALANCE
+ printf(" Not attempting momentum balance.\n");
+ // Just transfer deltaV directly into deltaU.
+ m_rtArm.updOneFromUPartition(state, 0, deltaU) = deltaV;
+ #else
+ printf(" Momentum balance: angular momentum (first subvector) "
+ "should be conserved:\n");
+ // Momentum balance analysis: see comment above.
+ state.updU().setToZero(); // no Coriolis impulses wanted
+ // Prescribe the desired deltaV as though it were a prescribed udot.
+ m_rtArm.lockAt(state, deltaV, Motion::Acceleration);
+
+ // Dynamics operators require this stage.
+ m_system.realize(state, Stage::Dynamics);
+
+ // Don't apply any forces.
+ const Vector mobForces(m_matter.getNumMobilities(), Real(0));
+ const Vector_<SpatialVec> bodyForces(m_matter.getNumBodies(),
+ SpatialVec(Vec3(0)));
+ Vector_<SpatialVec> A_GB; // unneeded output
+ // We are tricking these equations into calculating deltaU, not udot.
+ m_matter.calcAcceleration(state,mobForces,bodyForces,
+ deltaU, A_GB);
+ m_rtArm.unlock(state); // release the mobilizer
+ #endif
+
+ state.updU() = prevU + deltaU;
+ m_system.realize(state, Stage::Velocity);
+ SpatialVec momAfter = m_matter.calcSystemMomentumAboutGroundOrigin(state);
+
+ cout << " Mom before=" << momBefore << endl;
+ cout << " Mom after =" << momAfter << endl;
+
+ // This final projection shouldn't be necessary, but just in case ...
+ m_system.projectU(state); // leaves state at Stage::Velocity
+
+ // Now that the speed requirement is satisfied, try keeping the acceleration
+ // zero using the prescribed motion and check how much torque that requires.
+ m_system.realize(state, Stage::Acceleration);
+ const Real requiredTorque = getMotorTorque(state);
+ if (std::abs(requiredTorque) > getTorqueLimit(state)) {
+ m_speedController.disable(state);
+ m_torqueController.enable(state);
+ const Real newMaxTrq = sign(requiredTorque)*getTorqueLimit(state);
+ m_torqueController.setForce(state, newMaxTrq);
+ printf(" ... NO switch to speed control cuz torque would be %g;"
+ " applying %g instead.\n", requiredTorque, newMaxTrq);
+ state.updU() = prevU; // restore previous speed
+ m_system.realize(state, Stage::Velocity);
+ return;
+ }
+
+ printf(
+ " ... switched to speed control with trq=%g and impulsive speed change:"
+ " %g -> %g\n", requiredTorque, curSpeed, desSpeed);
+ cout << " ... deltaU was" << deltaU << endl;
+}
+
+//--------------------------- CHANGE DESIRED SPEED -----------------------------
+// The user has specified a new desired motor speed. This always requires a
+// switch to torque control if we're in speed control now, because an
+// instantaneous speed change would require an infinite torque. The torque
+// direction depends on whether the new desired speed is larger or smaller than
+// the current actual speed.
+void MyMechanism::changeDesiredSpeed(State& state, Real newDesiredSpeed) const {
+ const Real actualSpeed = getActualSpeed(state);
+ const Real changeDirection = (Real)sign(newDesiredSpeed - actualSpeed);
+ if (changeDirection == 0)
+ return; // nothing to do
+
+ printf("Desired speed change: %g -> %g\n", actualSpeed, newDesiredSpeed);
+ m_speedController.setOneRate(state, MobilizerUIndex(0), newDesiredSpeed);
+ if (isSpeedControlEnabled(state)) {
+ printf("...requires switch to torque control\n");
+ m_speedController.disable(state);
+ m_torqueController.enable(state);
+ }
+
+ // Torque control is on; set torque to move in direction of new speed.
+ const Real newTrq = changeDirection*getTorqueLimit(state);
+ m_torqueController.setForce(state, newTrq);
+ printf("...applying max torque %g to change speed\n", newTrq);
+
+ m_system.realize(state, Stage::Velocity);
+}
+
+//--------------------------- CHANGE TORQUE LIMIT ------------------------------
+// The user has specified a new value for the magnitude of the maximum torque.
+// The sign will be inherited from the sign of the torque currently in use. No
+// change to the control state is done here; that will occur in the main loop
+// if needed.
+void MyMechanism::changeTorqueLimit(State& state, Real newTorqueLimit) const {
+ const Real oldTorqueLimit = getTorqueLimit(state);
+ printf("Torque limit change: %g -> %g\n", oldTorqueLimit, newTorqueLimit);
+
+ // Put torque in same direction as current torque (might not be using the
+ // torque controller though).
+ const Real trq = getMotorTorque(state);
+ Real dir = (Real)sign(trq); if (dir==0) dir=1;
+ m_torqueController.setForce(state, dir*newTorqueLimit);
+}
+
+//---------------------------- PROCESS USER INPUT ------------------------------
+namespace {
+// Constants for the user interaction widgets.
+// Ids for the sliders.
+const int SliderIdMotorSpeed = 1, SliderIdTorqueLimit = 2,
+ SliderIdTach = 3, SliderIdTorque = 4; // these two are used for output
+// Ids for things on the Run Menu.
+const int MenuIdRun = 1;
+const int ResetItem=1, QuitItem=2;
+}
+
+// Return true if it is time to quit.
+bool MyMechanism::processUserInput(State& state) const {
+ int whichSlider, whichMenu, whichItem; Real newValue;
+
+ // Did a slider move?
+ if (m_userInput->takeSliderMove(whichSlider, newValue)) {
+ switch(whichSlider) {
+ case SliderIdMotorSpeed:
+ // This will switch to torque control if necessary.
+ changeDesiredSpeed(state, newValue);
+ break;
+ case SliderIdTorqueLimit:
+ changeTorqueLimit(state, newValue);
+ m_viz.setSliderRange(SliderIdTorque, -newValue, newValue);
+ break;
+ }
+ }
+
+ // Was there a menu pick?
+ if (m_userInput->takeMenuPick(whichMenu, whichItem)) {
+ if (whichItem == QuitItem)
+ return true; // done
+
+ // If Reset, stop the motor and zero out all the q's and u's.
+ // Tell visualizer to update the sliders to match.
+ if (whichItem == ResetItem) {
+ // Don't momentum balance here!
+ m_speedController.setOneRate(state, MobilizerUIndex(0), 0);
+ m_viz.setSliderValue(SliderIdMotorSpeed, 0)
+ .setSliderValue(SliderIdTach, 0);
+
+ m_torqueController.setForce(state, InitialTorqueLimit);
+ m_viz.setSliderValue(SliderIdTorqueLimit, InitialTorqueLimit)
+ .setSliderRange(SliderIdTorque, -InitialTorqueLimit,
+ InitialTorqueLimit)
+ .setSliderValue(SliderIdTorque, 0);
+
+ state.updQ() = 0; // all positions to zero
+ state.updU() = 0; // all velocities to zero
+ m_system.projectQ(state);
+ m_system.projectU(state);
+ }
+ }
+
+ return false; // keep going
+}
+
+//----------------------------- CLASS SHOW STUFF -------------------------------
+// We'll provide the Visualizer with an object of this class to generate any
+// per-frame text and geometry we want to show on screen.
+namespace {
+class ShowStuff : public DecorationGenerator {
+public:
+ ShowStuff(const MyMechanism& mech) : m_mech(mech) {}
+
+ void generateDecorations(const State& state,
+ Array_<DecorativeGeometry>& geometry) OVERRIDE_11
+ {
+ DecorativeText msg;
+ msg.setIsScreenText(true);
+ if (m_mech.isSpeedControlEnabled(state))
+ msg.setText("OK");
+ else
+ msg.setText("TORQUE LIMITED err="
+ + String(m_mech.getSpeedError(state), "%.2g"));
+ geometry.push_back(msg);
+ }
+private:
+ const MyMechanism& m_mech;
+};
+}
+
+//----------------------------- SET UP VISUALIZER ------------------------------
+void MyMechanism::setUpVisualizer() {
+ m_viz.setShutdownWhenDestructed(true) // make sure display window dies
+ .setBackgroundType(Visualizer::SolidColor); // turn off Ground & Sky
+
+ // Add sliders.
+ m_viz.addSlider("Motor speed", SliderIdMotorSpeed,
+ -MaxMotorSpeed, MaxMotorSpeed, InitialMotorSpeed)
+ .addSlider("Torque limit", SliderIdTorqueLimit,
+ 0, MaxTorqueLimit, InitialTorqueLimit)
+ .addSlider("Tach", SliderIdTach,
+ -MaxMotorSpeed, MaxMotorSpeed, 0)
+ .addSlider("Torque", SliderIdTorque,
+ -InitialTorqueLimit, InitialTorqueLimit, 0);
+
+ // Add Run menu.
+ Array_<std::pair<String,int> > runMenuItems;
+ runMenuItems.push_back(std::make_pair("Reset", ResetItem));
+ runMenuItems.push_back(std::make_pair("Quit", QuitItem));
+ m_viz.addMenu("Run", MenuIdRun, runMenuItems);
+
+ // Add per-frame text and geometry.
+ m_viz.addDecorationGenerator(new ShowStuff(*this));
+
+ // Add an input listener so the user can talk to us.
+ m_userInput = new Visualizer::InputSilo();
+ m_viz.addInputListener(m_userInput);
+}
+
+//------------------------------------ DRAW ------------------------------------
+// Update the Visualizer, including the output sliders.
+void MyMechanism::draw(const State& state) const {
+ m_viz.report(state);
+ m_viz.setSliderValue(SliderIdTach, getActualSpeed(state))
+ .setSliderValue(SliderIdTorque, getMotorTorque(state));
+}
+
+
+
+//==============================================================================
+// DUMP INTEGRATOR STATS
+//==============================================================================
+namespace {
+void dumpIntegratorStats(const Integrator& integ) {
+ const int evals = integ.getNumRealizations();
+ std::cout << "\nDone -- simulated " << integ.getTime() << "s with "
+ << integ.getNumStepsTaken() << " steps, avg step="
+ << (1000*integ.getTime())/integ.getNumStepsTaken() << "ms "
+ << (1000*integ.getTime())/evals << "ms/eval\n";
+
+ printf("Used Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(),
+ integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(),
+ integ.getNumProjections());
+}
+}
+
diff --git a/Simbody/examples/ExampleMotorWithSpeedControl.cpp b/Simbody/examples/ExampleMotorWithSpeedControl.cpp
new file mode 100644
index 0000000..958bd3d
--- /dev/null
+++ b/Simbody/examples/ExampleMotorWithSpeedControl.cpp
@@ -0,0 +1,354 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: MotorWithSpeedControl *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "Simbody.h"
+#include <iostream>
+#include <algorithm>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+// This is an example that shows several things:
+// 1. Integrating with a small maximum step size, using just an integrator
+// without a time stepper.
+// 2. Implementing a fixed-rate motor using a Motion (prescribed motion).
+// 3. Changing motor speed externally from user input.
+// 4. Creating sliders and menus in the Visualizer.
+//
+// Note: we are allowing the user to violently change motor speed, but we are
+// not performing a momentum balance so the change is non-physical modeled
+// this way. A momentum balance would allow us to calculate an impulse to be
+// applied to the whole mechanism that would make the change momentum conserving
+// as though the change had been implemented with a very large force over a very
+// short duration.
+
+const Real InitialMotorRate = 1; // rad/s
+const Real InitialDissipation = 0.5; // damping in left joint stop
+
+// Ids for the two sliders.
+const int SliderIdMotorSpeed = 1, SliderIdDissipation = 2,
+ SliderIdTach = 3, SliderIdTorque = 4;
+
+// Ids for things on the Run Menu.
+const int MenuIdRun = 1;
+static const int ResetItem=1, QuitItem=2;
+
+#define USE_TORQUE_LIMITED_MOTOR
+const Real MaxTorque = 100; // N-m
+const Real MaxTorqueRate = .5; // s / MaxTorque change
+const Real TorqueGain = 50000, DampingGain = 1000;
+const Real TorqueDecay = 100;
+
+#ifdef USE_TORQUE_LIMITED_MOTOR
+// This Force element implements a torque-limited motor that runs at a user-
+// selected speed unless that would require too much torque.
+// trqDot = gain*(u_desired - u_actual)
+// if (trq > MaxTorque) trqDot = min(0, trqDot)
+// else if (trq < -MaxTorque) trqDot = max(0, trqDot)
+//
+
+class MyTorqueLimitedMotor : public Force::Custom::Implementation {
+public:
+ MyTorqueLimitedMotor
+ (const MobilizedBody& mobod, MobilizerUIndex whichU,
+ Real gain, Real torqueLimit)
+ : m_matter(mobod.getMatterSubsystem()), m_mobod(mobod), m_whichU(whichU),
+ m_torqueGain(gain), m_torqueLimit(torqueLimit)
+ { assert(gain >= 0 && torqueLimit >= 0); }
+
+ void setDesiredRate(State& state, Real speed) const {
+ Real& u_desired = Value<Real>::updDowncast(
+ m_matter.updDiscreteVariable(state, m_desiredUIx));
+ u_desired = speed;
+ }
+ // synonym to match name used by Motion
+ void setRate(State& state, Real speed) const {setDesiredRate(state,speed);}
+ // synonym to match name used by ConstantSpeed constraint
+ void setSpeed(State& state, Real speed) const {setDesiredRate(state,speed);}
+
+ Real getDesiredRate(const State& state) const {
+ const Real u_desired = Value<Real>::downcast(
+ m_matter.getDiscreteVariable(state, m_desiredUIx));
+ return u_desired;
+ }
+
+ Real getActualRate(const State& state) const {
+ const Real u_actual = m_mobod.getOneU(state, m_whichU);
+ return u_actual;
+ }
+
+ Real getRateError(const State& state) const {
+ return getActualRate(state) - getDesiredRate(state);
+ }
+
+ // Combine integrated torque and proportional torque, and clamp to limit.
+ Real getTorque(const State& state) const {
+ const Real integTrq = m_matter.getZ(state)[m_trqIx];
+ const Real proportionalTrq = -DampingGain*getRateError(state);
+ const Real totalTrq = clamp(-m_torqueLimit, integTrq+proportionalTrq,
+ m_torqueLimit);
+ return totalTrq;
+ }
+
+
+ // Force::Custom::Implementation virtuals:
+
+ void calcForce(const State& state,
+ Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces,
+ Vector& mobilityForces) const OVERRIDE_11
+ {
+ m_mobod.applyOneMobilityForce(state, m_whichU, getTorque(state),
+ mobilityForces);
+ }
+
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11 {
+ return 0;
+ }
+
+ void realizeTopology(State& state) const OVERRIDE_11 {
+ m_desiredUIx = m_matter.allocateDiscreteVariable
+ (state, Stage::Acceleration, new Value<Real>(0));
+ m_trqIx = m_matter.allocateZ(state, Vector(1,Real(0)));
+ }
+
+ void realizeAcceleration(const State& state) const OVERRIDE_11 {
+ const Real integTrq = m_matter.getZ(state)[m_trqIx];
+ const Real abstrq=std::abs(integTrq), trqsign = sign(integTrq);
+
+ Real trqDot = -m_torqueGain*getRateError(state);
+ // Don't ask for more torque if we're already past the limit
+ if (abstrq >= m_torqueLimit && sign(trqDot)*trqsign >= 0)
+ trqDot = 0;
+
+ //const char* delay="WantLess: ";
+ //if (sign(trqDot)*trqsign >= 0) {
+ // // want more torque
+ // if (abstrq > m_torqueLimit) {
+ // trqDot = -trqsign*TorqueDecay*(abstrq-m_torqueLimit);
+ // delay = "TooBig: ";
+ // } else if (abstrq > 0.9*m_torqueLimit) {
+ // trqDot *= (m_torqueLimit-abstrq)/(0.9*m_torqueLimit);
+ // delay = "Limited: ";
+ // } else
+ // delay = "WantMore: ";
+ //}
+
+ //printf("%suerr=%g (actual=%g, des=%g) trq=%g trqdot=%g\n",
+ // delay, u_actual-u_desired,
+ // u_actual, u_desired, trq, trqDot);
+
+ Real rateLimit = m_torqueLimit/MaxTorqueRate; // full-scale change
+ //clampInPlace(-rateLimit, trqDot, rateLimit);
+
+ m_matter.updZDot(state)[m_trqIx] = trqDot;
+ }
+
+private:
+ const SimbodyMatterSubsystem& m_matter;
+ MobilizedBody m_mobod;
+ MobilizerUIndex m_whichU;
+ Real m_torqueGain;
+ Real m_torqueLimit;
+
+ // Topology cache
+ mutable DiscreteVariableIndex m_desiredUIx;
+ mutable ZIndex m_trqIx;
+};
+#endif
+
+const Real StopStiffness = 10000; // not changeable here
+int main() {
+ try { // catch errors if any
+
+ // Create the system, with subsystems for the bodies and some forces.
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::Gravity gravity(forces, matter, -YAxis, 9.8);
+ system.setUseUniformBackground(true); // request no ground & sky
+
+ // Describe a body with a point mass at (0, -3, 0) and draw a sphere there.
+ Real mass = 3; Vec3 pos(0,-3,0);
+ Body::Rigid bodyInfo(MassProperties(mass, pos, UnitInertia::pointMassAt(pos)));
+ bodyInfo.addDecoration(pos, DecorativeSphere(.2).setOpacity(.5));
+
+ // Create the tree of mobilized bodies, reusing the above body description.
+ MobilizedBody::Pin bodyT (matter.Ground(), Vec3(0), bodyInfo, Vec3(0));
+ MobilizedBody::Pin leftArm(bodyT, Vec3(-2, 0, 0), bodyInfo, Vec3(0));
+ MobilizedBody::Pin rtArm (bodyT, Vec3(2, 0, 0), bodyInfo, Vec3(0,-1,0));
+
+ // Add some damping.
+ Force::MobilityLinearDamper damper1(forces, bodyT, MobilizerUIndex(0), 10);
+ Force::MobilityLinearDamper damper2(forces, leftArm, MobilizerUIndex(0), 30);
+ Force::MobilityLinearDamper damper3(forces, rtArm, MobilizerUIndex(0), 10);
+
+#ifdef USE_TORQUE_LIMITED_MOTOR
+ MyTorqueLimitedMotor* motorp =
+ new MyTorqueLimitedMotor(rtArm, MobilizerUIndex(0), TorqueGain, MaxTorque);
+ const MyTorqueLimitedMotor& motor = *motorp;
+ Force::Custom(forces, motorp); // takes over ownership
+#else
+ // Use built-in Steady Motion as a low-budget motor model.
+ //Motion::Steady motor(rtArm, InitialMotorRate);
+ // Use built-in ConstantSpeed constraint as a low-budget motor model.
+ Constraint::ConstantSpeed motor(rtArm, InitialMotorRate);
+#endif
+
+ // Add a joint stop to the left arm restricting it to q in [0,Pi/5].
+ Force::MobilityLinearStop stop(forces, leftArm, MobilizerQIndex(0),
+ StopStiffness,
+ InitialDissipation,
+ -Pi/8, // lower stop
+ Pi/8); // upper stop
+
+ Visualizer viz(system);
+
+ // Add sliders.
+ viz.addSlider("Motor speed", SliderIdMotorSpeed, -10, 10, InitialMotorRate);
+ viz.addSlider("Dissipation", SliderIdDissipation, 0, 10, InitialDissipation);
+ viz.addSlider("Tach", SliderIdTach, -20, 20, 0);
+ viz.addSlider("Torque", SliderIdTorque, -MaxTorque, MaxTorque, 0);
+
+ // Add Run menu.
+ Array_<std::pair<String,int> > runMenuItems;
+ runMenuItems.push_back(std::make_pair("Reset", ResetItem));
+ runMenuItems.push_back(std::make_pair("Quit", QuitItem));
+ viz.addMenu("Run", MenuIdRun, runMenuItems);
+
+ Visualizer::InputSilo* userInput = new Visualizer::InputSilo();
+ viz.addInputListener(userInput);
+
+ // Initialize the system and state.
+ State initState = system.realizeTopology();
+
+ // Simulate forever with a small max step size. Check for user input
+ // in between steps. Note: an alternate way to do this is to let the
+ // integrator take whatever steps it wants but use a TimeStepper to
+ // manage a periodic event handler to poll for user input. Here we're
+ // treating completion of a step as an event.
+ const Real MaxStepSize = 0.01*3; // 10ms
+ const int DrawEveryN = 3/3; // 3 steps = 30ms
+ //RungeKuttaMersonIntegrator integ(system);
+ //RungeKutta2Integrator integ(system);
+ SemiExplicitEuler2Integrator integ(system);
+ //SemiExplicitEulerIntegrator integ(system, .001);
+
+ integ.setAccuracy(1e-1);
+ //integ.setAccuracy(1e-3);
+
+ // Don't permit interpolation because we want the state returned after
+ // a step to be modifiable.
+ integ.setAllowInterpolation(false);
+
+ integ.initialize(initState);
+
+ int stepsSinceViz = DrawEveryN-1;
+ while (true) {
+ if (++stepsSinceViz % DrawEveryN == 0) {
+ const State& s = integ.getState();
+ viz.report(s);
+ const Real uActual = rtArm.getOneU(s, MobilizerUIndex(0));
+ viz.setSliderValue(SliderIdTach, uActual);
+#ifdef USE_TORQUE_LIMITED_MOTOR
+ viz.setSliderValue(SliderIdTorque, motor.getTorque(s));
+#else
+ system.realize(s); // taus are acceleration stage
+ //viz.setSliderValue(SliderIdTorque,
+ // rtArm.getOneTau(s, MobilizerUIndex(0)));
+ viz.setSliderValue(SliderIdTorque, motor.getMultiplier(s));
+#endif
+
+ stepsSinceViz = 0;
+ }
+
+ // Advance time by MaxStepSize (might take multiple steps to get there).
+ integ.stepBy(MaxStepSize);
+
+ // Now poll for user input.
+ int whichSlider, whichMenu, whichItem; Real newValue;
+
+ // Did a slider move?
+ if (userInput->takeSliderMove(whichSlider, newValue)) {
+ State& state = integ.updAdvancedState();
+ switch(whichSlider) {
+ case SliderIdMotorSpeed:
+ // TODO: momentum balance?
+ //motor.setRate(state, newValue);
+ motor.setSpeed(state, newValue);
+ system.realize(state, Stage::Position);
+ system.prescribeU(state);
+ system.realize(state, Stage::Velocity);
+ system.projectU(state);
+ break;
+ case SliderIdDissipation:
+ stop.setMaterialProperties(state, StopStiffness, newValue);
+ system.realize(state, Stage::Position);
+ break;
+ }
+ }
+
+ // Was there a menu pick?
+ if (userInput->takeMenuPick(whichMenu, whichItem)) {
+ if (whichItem == QuitItem)
+ break; // done
+
+ // If Reset, stop the motor and restore default dissipation.
+ // Tell visualizer to update the sliders to match.
+ // Zero out all the q's and u's.
+ if (whichItem == ResetItem) {
+ State& state = integ.updAdvancedState();
+ //motor.setRate(state, 0);
+ motor.setSpeed(state, 0);
+ viz.setSliderValue(SliderIdMotorSpeed, 0);
+
+ stop.setMaterialProperties(state, StopStiffness, InitialDissipation);
+ viz.setSliderValue(SliderIdDissipation, InitialDissipation);
+
+ state.updQ() = 0; // all positions to zero
+ state.updU() = 0; // all velocities to zero
+ system.realize(state, Stage::Position);
+ system.prescribeU(state);
+ system.realize(state, Stage::Velocity);
+ system.projectU(state);
+ }
+ }
+
+ }
+ const int evals = integ.getNumRealizations();
+ std::cout << "Done -- simulated " << integ.getTime() << "s with "
+ << integ.getNumStepsTaken() << " steps, avg step="
+ << (1000*integ.getTime())/integ.getNumStepsTaken() << "ms "
+ << (1000*integ.getTime())/evals << "ms/eval\n";
+
+ printf("Used Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
+
+ } catch (const std::exception& e) {
+ std::cout << "ERROR: " << e.what() << std::endl;
+ return 1;
+ }
+ return 0;
+}
diff --git a/Simbody/examples/ExamplePendulum.cpp b/Simbody/examples/ExamplePendulum.cpp
new file mode 100644
index 0000000..4175364
--- /dev/null
+++ b/Simbody/examples/ExamplePendulum.cpp
@@ -0,0 +1,220 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Pendulum *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Ajay Seth *
+ * Contributors: Michael Sherman *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+
+// Define two identical double pendulums, one modeled the easy way using
+// two pin mobilizers, the other modeled with free mobilizers plus a ball
+// constraint, plus two "constant angle" constraints to get rid of the extra
+// rotational degrees of freedom.
+// We're going to show that the resulting reaction forces are identical.
+
+
+#include "Simbody.h"
+
+using namespace SimTK;
+using std::cout; using std::endl;
+
+int main() {
+ try {
+ // Create the system, with subsystems for the bodies and some forces.
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+
+ // Add gravity as a force element.
+ Rotation x45(Pi/4, XAxis);
+ Rotation y45(Pi/4, YAxis);
+ Rotation z45(Pi/4, ZAxis);
+ Force::UniformGravity gravity(forces, matter, Vec3(10, Real(-9.8), 3));
+ // Create the body and some artwork for it.
+ Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ pendulumBody.addDecoration(Transform(),
+ DecorativeSphere(Real(0.1)).setColor(Red));
+
+ // Add an instance of the body to the multibody system by connecting
+ // it to Ground via a pin mobilizer.
+ MobilizedBody::Pin pendulum1(matter.updGround(),
+ Transform(/*x45,*/Vec3(0,-1,0)),
+ pendulumBody,
+ Transform(Vec3(0, 1, 0)));
+ MobilizedBody::Pin pendulum1b(pendulum1,
+ Transform(/*x45,*/Vec3(0,-1,0)),
+ pendulumBody,
+ Transform(Vec3(0, 1, 0)));
+
+ // Now make an identical system with pin joints faked up using a
+ // free mobilizer + ball constraint + two angle constraints.
+ MobilizedBody::Free pendulum2(matter.updGround(),
+ Transform(/*x45,*/Vec3(2,-1,0)),
+ pendulumBody,
+ Transform(Vec3(0,1,0)));
+ Constraint::Ball ballcons2(matter.updGround(), Vec3(2,-1,0),
+ pendulum2, Vec3(0,1,0));
+ const Transform& X_GF2 = pendulum2.getDefaultInboardFrame();
+ const Transform& X_P2M = pendulum2.getDefaultOutboardFrame();
+ Constraint::ConstantAngle angx2(matter.Ground(), X_GF2.x(),
+ pendulum2, X_P2M.z());
+ Constraint::ConstantAngle angy2(matter.Ground(), X_GF2.y(),
+ pendulum2, X_P2M.z());
+
+ MobilizedBody::Free pendulum2b(pendulum2,
+ Transform(/*x45,*/Vec3(0,-1,0)),
+ pendulumBody,
+ Transform(Vec3(0,1,0)));
+ Constraint::Ball ballcons2b(pendulum2, Vec3(0,-1,0),
+ pendulum2b, Vec3(0,1,0));
+ const Transform& X_GF2b = pendulum2b.getDefaultInboardFrame();
+ const Transform& X_P2Mb = pendulum2b.getDefaultOutboardFrame();
+ Constraint::ConstantAngle angx2b(pendulum2, X_GF2b.x(),
+ pendulum2b, X_P2Mb.z());
+ Constraint::ConstantAngle angy2b(pendulum2, X_GF2b.y(),
+ pendulum2b, X_P2Mb.z());
+
+
+ // Visualize with default options; ask for a report every 1/30 of a second
+ // to match the Visualizer's default 30 frames per second rate.
+ Visualizer viz(system);
+ system.addEventReporter(new Visualizer::Reporter(viz, Real(1./30)));
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ pendulum1.setOneQ(state, 0, Pi/4);
+ //pendulum1.setOneU(state, 0, 1.0); // initial velocity 1 rad/sec
+
+ //pendulum1b.setOneU(state, 0, 1.0); // initial velocity 1 rad/sec
+ pendulum1b.setOneQ(state, 0, Pi/4);
+
+ pendulum2.setQToFitRotation(state, Rotation(Pi/4, ZAxis));
+ //pendulum2.setUToFitAngularVelocity(state, Vec3(0,0,1));
+ pendulum2b.setQToFitRotation(state, Rotation(Pi/4, ZAxis));
+ //pendulum2b.setUToFitAngularVelocity(state, Vec3(0,0,1));
+
+ system.realize(state);
+ const Vector lambda = state.getMultipliers();
+ Vector_<SpatialVec> consBodyForcesInG;
+ Vector consMobForces;
+ matter.calcConstraintForcesFromMultipliers(state, -lambda, consBodyForcesInG,
+ consMobForces);
+ const MobodIndex p2x = pendulum2.getMobilizedBodyIndex();
+ const MobodIndex p2bx = pendulum2b.getMobilizedBodyIndex();
+ const Rotation& R_G2 = pendulum2.getBodyTransform(state).R();
+ //consBodyForcesInG[p2x] = shiftForceFromTo(consBodyForcesInG[p2x],
+ // Vec3(0), R_G2*Vec3(0,1,0));
+
+ const int nb = matter.getNumBodies();
+ Vector_<SpatialVec> forcesAtMInG;
+ matter.calcMobilizerReactionForces(state, forcesAtMInG);
+
+ // The above method returns reactions *on the child body* at the outboard
+ // mobilizer frame M (that is, the frame fixed on the child body).
+ // Calculate the same reactions, but *on the parent body* and at the
+ // inboard mobilizer frame F (that is, the frame fixed on the parent body).
+ // This is done by shifting the reaction forces across the mobilizer from
+ // M to F, and negating.
+ // Note that for the example here the mobilizers aren't translating so
+ // the origins Mo and Fo are identical. Nevertheless we're using the
+ // generic code here that should work for any system.
+
+ Vector_<SpatialVec> forcesAtFInG(nb); // to hold the result
+ forcesAtFInG[0] = -forcesAtMInG[0]; // Ground is "welded" at origin
+ for (MobilizedBodyIndex i(1); i < matter.getNumBodies(); ++i) {
+ const MobilizedBody& body = matter.getMobilizedBody(i);
+ const MobilizedBody& parent = body.getParentMobilizedBody();
+ // Want to shift negated reaction by p_MF_G, the vector from M
+ // to F across the mobilizer, expressed in Ground. We can get p_FM,
+ // then re-express in Ground for the shift and negate.
+ const Vec3& p_FM = body.getMobilizerTransform(state).p();
+ const Rotation& R_PF = body.getInboardFrame(state).R(); // In parent.
+ const Rotation& R_GP = parent.getBodyTransform(state).R();
+ Rotation R_GF = R_GP*R_PF; // F frame orientation in Ground.
+ Vec3 p_MF_G = -(R_GF*p_FM); // Re-express and negate shift vector.
+ forcesAtFInG[i] = -shiftForceBy(forcesAtMInG[i], p_MF_G);
+ }
+
+ std::cout << "Reactions @M: " << forcesAtMInG << "\n";
+ std::cout << "Reactions @F: " << forcesAtFInG << "\n";
+ std::cout << "norm of difference: " << (forcesAtMInG+forcesAtFInG).norm()
+ << "\n";
+
+ const MobodIndex p1x = pendulum1.getMobilizedBodyIndex();
+ const MobodIndex p1bx = pendulum1b.getMobilizedBodyIndex();
+ const Rotation& R_G1 = pendulum1.getBodyTransform(state).R();
+ const Rotation& R_G1b = pendulum1b.getBodyTransform(state).R();
+
+ // This time shift the child reactions from the mobilizer frames M to the
+ // child body frames B so we can compare with the constraint forces that
+ // are returned at the child body frame.
+ Vector_<SpatialVec> forcesAtBInG(nb);
+ for (MobodIndex i(0); i < nb; ++i) {
+ const Mobod& body = matter.getMobilizedBody(i);
+ const Vec3& p_BM = body.getOutboardFrame(state).p();
+ const Rotation& R_GB = body.getBodyTransform(state).R();
+ forcesAtBInG[i] = shiftForceFromTo(forcesAtMInG[i],
+ R_GB*p_BM, Vec3(0));
+ }
+
+ std::cout << "Pin mobilizer reaction forces:\n";
+ std::cout << "FB_G=" << forcesAtBInG[p1x]
+ << " " << forcesAtBInG[p1bx] << "\n";
+
+ std::cout << "Constraint reaction forces (should be the same):\n";
+ cout << "FC_G=" << -(ballcons2.getConstrainedBodyForcesAsVector(state)
+ + angx2.getConstrainedBodyForcesAsVector(state)
+ + angy2.getConstrainedBodyForcesAsVector(state))[1] << " ";
+ cout << -(ballcons2b.getConstrainedBodyForcesAsVector(state)
+ + angx2b.getConstrainedBodyForcesAsVector(state)
+ + angy2b.getConstrainedBodyForcesAsVector(state))[1] << endl;
+
+ viz.report(state);
+ // Simulate it.
+ cout << "Hit ENTER to run a short simulation ...";
+ getchar();
+
+ RungeKuttaMersonIntegrator integ(system);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(1.0);
+ state = integ.getState();
+ system.realize(state);
+ matter.calcMobilizerReactionForces(state, forcesAtMInG);
+ const Transform& X_GP = pendulum1.getBodyTransform(state);
+ //forcesAtMInG[1][1] = X_GP.R()*forcesAtMInG[1][1];
+ std::cout << "FM_G=" << forcesAtMInG << "\n";
+ ts.stepTo(Real(1.2));
+ state = integ.getState();
+ system.realize(state);
+ matter.calcMobilizerReactionForces(state, forcesAtMInG);
+ std::cout << "FM_G=" << forcesAtMInG << "\n";
+
+ } catch (const std::exception& e) {
+ std::cout << "ERROR: " << e.what() << std::endl;
+ return 1;
+ } catch (...) {
+ std::cout << "UNKNOWN EXCEPTION\n";
+ return 1;
+ }
+
+ return 0;
+}
diff --git a/Simbody/examples/ExampleSampleAndHold.cpp b/Simbody/examples/ExampleSampleAndHold.cpp
new file mode 100644
index 0000000..6d8c84a
--- /dev/null
+++ b/Simbody/examples/ExampleSampleAndHold.cpp
@@ -0,0 +1,166 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Sample and Hold *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* This example demonstrates one way to implement sample and hold behavior
+in Simbody. The outline is:
+ 1 define a discrete variable to hold the sampled signal
+ 2 define a periodic event handler that reads the signal and updates
+ the variable
+ 3 define a system that has some dependence on the variable
+Here the system is just a simple pendulum swinging in gravity. We'll
+sample the position of its end point periodically, and stretch a rubber
+band line from the sampled location to the current location.
+*/
+
+#include "Simbody.h"
+
+using namespace SimTK;
+
+// This is a periodic event handler that interrupts the simulation on a regular
+// basis to record the ground-frame location of a given station on a given
+// mobilized body and record the result in a given variable.
+class StationSampler : public PeriodicEventHandler {
+public:
+ StationSampler(const MultibodySystem& system,
+ const MobilizedBody& mobod,
+ const Vec3& station,
+ const Measure_<Vec3>::Variable& sample,
+ Real interval)
+ : PeriodicEventHandler(interval), m_system(system),
+ m_mobod(mobod), m_station(station),
+ m_sample(sample) {}
+
+ // This method is called whenever this event occurs.
+ virtual void handleEvent
+ (State& state, Real, bool&) const
+ {
+ m_system.realize(state, Stage::Position);
+ Vec3 location = m_mobod.findStationLocationInGround(state,m_station);
+ m_sample.setValue(state, location);
+ }
+
+private:
+ const MultibodySystem& m_system;
+ MobilizedBody m_mobod;
+ Vec3 m_station;
+ Measure_<Vec3>::Variable m_sample;
+};
+
+// The only effect our example sample is going to have on this system is
+// to change what's drawn. For that purpose we're defining a
+// DecorationGenerator here that we can register with the Visualizer which then
+// calls it each time it generates a frame to allow dynamically-generated
+// geometry. Here we'll add a mark at the last-sampled location, and draw a
+// line from there to the current location.
+class RubberBand : public DecorationGenerator {
+public:
+ RubberBand(const MultibodySystem& system,
+ const MobilizedBody& mobod,
+ const Vec3& station,
+ const Measure_<Vec3>::Variable& sample)
+ : m_system(system), m_mobod(mobod), m_station(station),
+ m_sample(sample) {}
+
+ virtual void generateDecorations(const State& state,
+ Array_<DecorativeGeometry>& geometry)
+ {
+ m_system.realize(state, Stage::Position);
+ Vec3 current = m_mobod.findStationLocationInGround(state,m_station);
+ Vec3 previous = m_sample.getValue(state);
+
+ // Draw a small black circle at the sampled location.
+ geometry.push_back(DecorativeCircle(0.05)
+ .setColor(Black)
+ .setTransform(previous));
+ // Draw an orange line from the sampled to the current location.
+ geometry.push_back(DecorativeLine(previous,current)
+ .setColor(Orange)
+ .setLineThickness(3));
+ }
+
+private:
+ const MultibodySystem& m_system;
+ MobilizedBody m_mobod;
+ Vec3 m_station;
+ Measure_<Vec3>::Variable m_sample;
+};
+
+int main() {
+ try {
+ // Create the system, with subsystems for the bodies and some forces.
+ MultibodySystem system; system.setUseUniformBackground(true);
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+
+ Measure_<Vec3>::Variable prevPos(matter, Stage::Position, Vec3(0));
+
+ // Add gravity as a force element.
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+
+ // Create the body and some artwork for it.
+ Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1).setColor(Red));
+
+ // Add an instance of the body to the multibody system by connecting
+ // it to Ground via a pin mobilizer.
+ MobilizedBody::Pin pendulum(matter.updGround(), Transform(Vec3(0)),
+ pendulumBody, Transform(Vec3(0, 1, 0)));
+
+ // Once a second we'll record the pendulum's ground location.
+ const Real Interval = 1;
+ system.addEventHandler(new StationSampler(system,
+ pendulum, Vec3(0),
+ prevPos, Interval));
+
+ // Visualize with default options; ask for a report every 1/30 of a second
+ // to match the Visualizer's default 30 frames per second rate.
+ Visualizer viz(system);
+ viz.addDecorationGenerator(new RubberBand(system,
+ pendulum, Vec3(0),
+ prevPos));
+ system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ pendulum.setOneU(state, 0, 2.0); // initial velocity 2 rad/sec
+
+ // Simulate it.
+
+ RungeKuttaMersonIntegrator integ(system);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(100.0);
+
+ } catch (const std::exception& e) {
+ std::printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+
+ } catch (...) {
+ std::printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+ return 0;
+}
diff --git a/Simbody/examples/ExampleScissorLift.cpp b/Simbody/examples/ExampleScissorLift.cpp
new file mode 100644
index 0000000..1f9f22d
--- /dev/null
+++ b/Simbody/examples/ExampleScissorLift.cpp
@@ -0,0 +1,274 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: SimplePlanarMechanism *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Kevin He (Roblox) *
+ * *
+ * 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 "Simbody.h"
+using namespace SimTK;
+
+#include <cstdio>
+using std::cout; using std::endl;
+
+// This builds a scissor lift mechanism as suggested by Kevin He at Roblox.
+// You can choose the number of scissor levels.
+// This is a planar mechanism. It is rather a worst case for an internal
+// coordinate multibody system since it has lots of tree degrees of freedom,
+// all but one of which is removed by constraints. Execution time will grow
+// as O(m^3) once the number of constraints m is large enough so that factoring
+// the constraint matrix dominates the execution time.
+//
+// You can instead use stiff springs instead of constraints to model the
+// cross-connections. That results in considerable savings in per-evaluation
+// CPU time (I measured almost 10X at 10 levels), but it makes the system stiff
+// and requires *much* smaller time steps.
+//
+// The mechanism is built to operate in the X-Y plane, with Y vertical and
+// X to the right. Joints are all pins in the Z direction.
+
+static const int NumLevels = 10;
+static const bool PrescribeRotor = true;
+static const bool UseSpringsInsteadOfConstraints = false;
+
+// This Force element holds a point on one body (the "follower") onto a plane
+// on another via a spring that acts always along the plane normal.
+class DirectionalSpringDamper : public Force::Custom::Implementation {
+public:
+ DirectionalSpringDamper
+ (const MobilizedBody& plane, const UnitVec3& normal, Real h,
+ const MobilizedBody& follower, const Vec3& point,
+ Real k, Real c) // stiffness and damping
+ : plane(plane), normal(normal), h(h),
+ follower(follower), point(point), k(k), c(c)
+ { assert(k >= 0 && c >= 0); }
+
+ virtual void calcForce(const State& state,
+ Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces,
+ Vector& mobilityForces) const
+ {
+ const Vec3 p = follower.findStationLocationInAnotherBody
+ (state, point, plane);
+ const Vec3 v = follower.findStationVelocityInAnotherBody
+ (state, point, plane);
+ const Real x = dot(p,normal) - h; // height of point over plane
+ const Real s = dot(v,normal); // speed along normal
+ const Vec3 forceOnPlane = plane.expressVectorInGroundFrame
+ (state, k*x*normal + s*c);
+ // Apply equal and opposite forces at the same point in space.
+ plane.applyForceToBodyPoint(state, p, forceOnPlane, bodyForces);
+ follower.applyForceToBodyPoint(state, point, -forceOnPlane, bodyForces);
+ }
+
+ virtual Real calcPotentialEnergy(const State& state) const {
+ const Vec3 p = follower.findStationLocationInAnotherBody
+ (state, point, plane);
+ const Real x = dot(p,normal) - h; // height of point over plane
+ return k*x*x/2;
+ }
+
+private:
+ MobilizedBody plane;
+ UnitVec3 normal;
+ Real h;
+ MobilizedBody follower;
+ Vec3 point;
+ Real k, c;
+};
+
+int main() {
+ try { // catch errors if any
+
+ // Create the system, with subsystems for the bodies and some forces.
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::Gravity gravity(forces, matter, -YAxis, 9.8);
+
+ // Turn off automatically-generated geometry so we just see what we
+ // draw here.
+ matter.setShowDefaultGeometry(false);
+
+ // Height off the ground, and half width of the scissor mechanism's base.
+ Real Height = 2, BaseHalfWidth = .35;
+
+ // Definition of the rotor body, used so the simulation won't be boring.
+ Real rotorMass = .5;
+ Vec3 rotorSz(.25,.05,.025); // half dimensions of rotor body
+ Body::Rigid rotorInfo(MassProperties(rotorMass, Vec3(0),
+ UnitInertia::brick(rotorSz)));
+ rotorInfo.addDecoration(Vec3(0), DecorativeBrick(rotorSz).setColor(Red));
+
+ // Describe a long thin rectangular body, with the long direction in Y.
+ Real linkMass = 1;
+ Vec3 linkSz(.1,1,.025); // half dimensions of link body
+ Body::Rigid linkInfo(MassProperties(linkMass, Vec3(0),
+ UnitInertia::brick(linkSz)));
+ linkInfo.addDecoration(Vec3(0), DecorativeBrick(linkSz).setColor(Green));
+
+ // Attach the rotor to Ground off to the right and push into -z a little
+ // so it is offset from the link.
+ MobilizedBody::Pin rotor(matter.Ground(), Vec3(BaseHalfWidth + 2*rotorSz[0],
+ Height, 0),
+ rotorInfo, Vec3(rotorSz[0],0, rotorSz[2]));
+
+ // Can let the rotor flop or prescribe it to go at a constant velocity.
+ if (PrescribeRotor) {
+ //Vector coef(2); coef[0]=1; coef[1]=0;
+ //Constraint::PrescribedMotion(matter, new Function::Linear(coef),
+ // rotor, MobilizerQIndex(0));
+
+ // Using a Motion rather than a constraint is faster, especially if
+ // there are no other constraints.
+ Motion::Steady(rotor, 1.);
+ }
+
+ // Create the two trees of mobilized bodies, reusing the above link
+ // description.
+ MobilizedBody::Pin right1
+ (rotor, Vec3(-rotorSz[0], 0, rotorSz[2]),
+ linkInfo, Vec3(0, -linkSz[1], -linkSz[2]));
+ MobilizedBody::Pin left1
+ (matter.Ground(), Vec3(-BaseHalfWidth,Height,2*linkSz[2]),
+ linkInfo, Vec3(0, -linkSz[1], -linkSz[2]));
+ right1.setDefaultAngle(Pi/8);
+ left1.setDefaultAngle(-Pi/8);
+
+ MobilizedBody::Pin lastRight = right1, lastLeft = left1;
+ Real sign = -1; // alternate initial angles
+ for (int i=1; i <= NumLevels; ++i) {
+ // Add cross connections between the tree ends using two 1-dof
+ // constraints (that's enough since this is planar).
+ if (UseSpringsInsteadOfConstraints) {
+ const Real k = 3000000, c = 1000;
+ Force::Custom(forces,
+ new DirectionalSpringDamper(lastLeft,YAxis, 0.,
+ lastRight, Vec3(0), k, c));
+ Force::Custom(forces,
+ new DirectionalSpringDamper(lastRight,YAxis, 0.,
+ lastLeft, Vec3(0), k, c));
+ } else {
+ Constraint::PointInPlane(lastLeft, YAxis, 0., // the plane
+ lastRight, Vec3(0)); // the point
+ Constraint::PointInPlane(lastRight, YAxis, 0., // the plane
+ lastLeft, Vec3(0)); // the point
+ }
+ if (i==NumLevels) break;
+
+ // Add generic link pair.
+ lastRight = MobilizedBody::Pin(lastRight, Vec3(0, linkSz[1], 0),
+ linkInfo, Vec3(0, -linkSz[1], 0));
+ lastLeft = MobilizedBody::Pin(lastLeft, Vec3(0, linkSz[1], 0),
+ linkInfo, Vec3(0, -linkSz[1], 0));
+
+ // Set default initial conditions in a zig-zag pattern.
+ lastRight.setDefaultAngle(sign*2*Pi/8);
+ lastLeft.setDefaultAngle(-sign*2*Pi/8);
+ sign = -sign;
+ }
+
+ // Ask for visualization every 1/30 second.
+ Visualizer viz(system);
+ system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
+
+ // Initialize the system and state.
+ State state = system.realizeTopology();
+
+ cout << "Scissors before assembly ... ENTER to assemble.\n";
+ viz.report(state);
+ getchar();
+
+ Assembler(system).assemble(state);
+
+ cout << "Scissors after assembly ... ENTER to "
+ << (UseSpringsInsteadOfConstraints?"minimize energy":"run simulation")
+ << "\n";
+ viz.report(state);
+ getchar();
+
+ if (UseSpringsInsteadOfConstraints) {
+ try {
+ LocalEnergyMinimizer::minimizeEnergy(system,state,10.);
+ } catch(const std::exception& e) {
+ cout << "Minimizer failed with " << e.what() << ". Continuing\n";
+ }
+ cout << "Scissors after static ... ENTER to run simulation.\n";
+ viz.report(state);
+ getchar();
+ }
+
+
+ // Run a simulation. There are a variety of integration settings you
+ // can play with here. If you put a cap on the max step size, you should
+ // use a low order integrator to avoid wasting cycles.
+
+ const Real Accuracy = 0.1; // i.e., 10%. Default is 0.1%.
+ //const Real Accuracy = 0.01; // 1%
+ //const Real Accuracy = 0.2; // 20%
+ //const Real Accuracy = 0.5; // 50%
+
+ const Real MaxStepSize = Infinity;
+ //RungeKuttaMersonIntegrator integ(system); // 4th order
+
+ //const Real MaxStepSize = 0.05; // 50 ms
+ RungeKutta3Integrator integ(system); // 3rd order
+
+ //const Real MaxStepSize = .001;
+ //const Real MaxStepSize = .0005;
+ //RungeKutta2Integrator integ(system); // 2nd order
+ //ExplicitEulerIntegrator integ(system); // 1st order
+
+ integ.setMaximumStepSize(MaxStepSize);
+ integ.setAccuracy(Accuracy);
+
+ // Maintain 1mm tolerance even at very loose integration accuracy.
+ integ.setConstraintTolerance(std::min(.001, Accuracy/10));
+
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+
+ const double startReal = realTime();
+ const double startCPU = cpuTime();
+ ts.stepTo(10); // Run simulation for 10s.
+
+ // Finished simulating; dump out some stats. On Windows CPUtime is not
+ // reliable if very little time is spent in this thread.
+ const double timeInSec = realTime()-startReal;
+ const double cpuInSec = cpuTime()-startCPU;
+ const int evals = integ.getNumRealizations();
+ cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
+ timeInSec << "s for " << integ.getTime() << "s sim (avg step="
+ << (1000*integ.getTime())/integ.getNumStepsTaken() << "ms) "
+ << (1000*integ.getTime())/evals << "sim ms/eval\n";
+ cout << "CPUtime (not reliable when visualizing) " << cpuInSec << endl;
+
+ printf("Used Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(),
+ integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(),
+ integ.getNumProjections());
+
+ } catch (const std::exception& e) {
+ std::cout << "ERROR: " << e.what() << std::endl;
+ return 1;
+ }
+ return 0;
+}
diff --git a/Simbody/examples/ExampleSimplePlanarMechanism.cpp b/Simbody/examples/ExampleSimplePlanarMechanism.cpp
new file mode 100644
index 0000000..b1eb6b2
--- /dev/null
+++ b/Simbody/examples/ExampleSimplePlanarMechanism.cpp
@@ -0,0 +1,95 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: SimplePlanarMechanism *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Andreas Scholz *
+ * *
+ * 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 "Simbody.h"
+using namespace SimTK;
+
+// This very simple example builds a 3-body planar mechanism that does
+// nothing but rock back and forth for 10 seconds. Note that Simbody always
+// works in 3D; this mechanism is planar because of the alignment of its
+// joints not because it uses any special 2D features. The mechanism looks
+// like this:
+// @
+// @--------+--------@
+// Y | | \
+// | | | \
+// | | | \
+// /-----X * * *
+// /
+// Z
+//
+// It consists of a central T-shaped body pinned to ground, and
+// two pendulum bodies pinned to either side of the T. The @'s above represent
+// pin joints rotating about the Z axes. Each body's mass is concentrated into
+// point masses shown by *'s above. Gravity is in the -Y direction.
+//
+// We'll add a joint stop on the left arm to limit its range of motion.
+//
+// This is the Simbody equivalent of a mechanism Andreas Scholz used to
+// demonstrate features of the MOBILE code from Andr�s Kecskem�thy's lab.
+
+int main() {
+ try { // catch errors if any
+
+ // Create the system, with subsystems for the bodies and some forces.
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::Gravity gravity(forces, matter, -YAxis, 9.8);
+
+ // Describe a body with a point mass at (0, -3, 0) and draw a sphere there.
+ Real mass = 3; Vec3 pos(0,-3,0);
+ Body::Rigid bodyInfo(MassProperties(mass, pos, UnitInertia::pointMassAt(pos)));
+ bodyInfo.addDecoration(pos, DecorativeSphere(.2).setOpacity(.5));
+
+ // Create the tree of mobilized bodies, reusing the above body description.
+ MobilizedBody::Pin bodyT (matter.Ground(), Vec3(0), bodyInfo, Vec3(0));
+ MobilizedBody::Pin leftArm(bodyT, Vec3(-2, 0, 0), bodyInfo, Vec3(0));
+ MobilizedBody::Pin rtArm (bodyT, Vec3(2, 0, 0), bodyInfo, Vec3(0));
+
+ // Add a joint stop to the left arm restricting it to q in [0,Pi/5].
+ Force::MobilityLinearStop stop(forces, leftArm, MobilizerQIndex(0),
+ 10000, // stiffness
+ 0.5, // dissipation coefficient
+ 0*Pi, // lower stop
+ Pi/5); // upper stop
+
+ // Ask for visualization every 1/30 second.
+ system.setUseUniformBackground(true); // turn off floor
+ system.addEventReporter(new Visualizer::Reporter(system, 1./30));
+
+ // Initialize the system and state.
+ State state = system.realizeTopology();
+ leftArm.setAngle(state, Pi/5);
+
+ // Simulate for 10 seconds.
+ RungeKuttaMersonIntegrator integ(system);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(10);
+
+ } catch (const std::exception& e) {
+ std::cout << "ERROR: " << e.what() << std::endl;
+ return 1;
+ }
+ return 0;
+}
diff --git a/Simbody/examples/ExampleTwoBoxCollide.cpp b/Simbody/examples/ExampleTwoBoxCollide.cpp
new file mode 100644
index 0000000..f79465b
--- /dev/null
+++ b/Simbody/examples/ExampleTwoBoxCollide.cpp
@@ -0,0 +1,368 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Contact Playground *
+ * -------------------------------------------------------------------------- *
+ * 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) 2010-13 Stanford University and the Authors. *
+ * Authors: Kevin He, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* Kevin He at Roblox created this example starting with
+ExampleContactPlayground. It basically demonstrates why an Elastic Foundation
+(EF) contact model is not a good way to handle coarsely-meshed simple objects,
+like a box. EF uses the centroid of each face to generate an area-weighted
+force, which can be a good physical representation with a dense mesh but
+since the vertices and edges don't participate there is a lot of visible
+penetration for a coarse mesh like the ones here.
+*/
+
+#include "Simbody.h"
+
+#include <cstdio>
+#include <exception>
+#include <algorithm>
+#include <iostream>
+#include <fstream>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+Array_<State> saveEm;
+
+static const Real TimeScale = 1;
+static const Real FrameRate = 30;
+static const Real ReportInterval = TimeScale/FrameRate;
+
+// These are the item numbers for the entries on the Run menu.
+static const int RunMenuId = 3, HelpMenuId = 7;
+static const int GoItem = 1, ReplayItem=2, QuitItem=3;
+
+// This is a periodic event handler that interrupts the simulation on a regular
+// basis to poll the InputSilo for user input. If there has been some, process it.
+// This one does nothing but look for the Run->Quit selection.
+class UserInputHandler : public PeriodicEventHandler {
+public:
+ UserInputHandler(Visualizer::InputSilo& silo, Real interval)
+ : PeriodicEventHandler(interval), m_silo(silo) {}
+
+ virtual void handleEvent(State& state, Real accuracy,
+ bool& shouldTerminate) const
+ {
+ int menuId, item;
+ if (m_silo.takeMenuPick(menuId, item) && menuId==RunMenuId && item==QuitItem)
+ shouldTerminate = true;
+ }
+
+private:
+ Visualizer::InputSilo& m_silo;
+};
+
+
+static void makeCube(Real h, PolygonalMesh& cube);
+static void makeTetrahedron(Real r, PolygonalMesh& tet);
+static void makePyramid(Real baseSideLength, PolygonalMesh& pyramid);
+static void makeOctahedron(Real radius, PolygonalMesh& pyramid);
+
+
+int main() {
+ try
+ { // Create the system.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::Gravity gravity(forces, matter, UnitVec3(2,-10,0), 1);
+
+ ContactTrackerSubsystem tracker(system);
+ CompliantContactSubsystem contactForces(system, tracker);
+ contactForces.setTrackDissipatedEnergy(true);
+ contactForces.setTransitionVelocity(1e-3);
+
+ Vec3 halfSize(3,4,5);
+ Vec3 halfSize2(200, 4, 200);
+ ContactGeometry::TriangleMesh box(PolygonalMesh::createBrickMesh(halfSize));
+ ContactGeometry::TriangleMesh box2(PolygonalMesh::createBrickMesh(halfSize2, 20));
+ DecorativeMesh showBox(box.createPolygonalMesh());
+ DecorativeMesh showBox2(box2.createPolygonalMesh());
+
+ const Real boxMass = halfSize[0] * halfSize[1] * halfSize[2] * 8;
+ const Real boxMass2 = halfSize2[0] * halfSize2[1] * halfSize2[2] * 8;
+ Body::Rigid boxBody(MassProperties(boxMass, Vec3(0),
+ boxMass * UnitInertia::brick(halfSize)));
+ Body::Rigid boxBody2(MassProperties(boxMass2, Vec3(0),
+ boxMass2 * UnitInertia::brick(halfSize2)));
+ boxBody.addDecoration(Transform(),
+ showBox.setColor(Red).setOpacity(1));
+ boxBody.addDecoration(Transform(),
+ showBox.setColor(Gray).setRepresentation(DecorativeGeometry::DrawWireframe));
+ boxBody2.addDecoration(Transform(),
+ showBox2.setColor(Cyan).setOpacity(.6));
+ boxBody2.addDecoration(Transform(),
+ showBox2.setColor(Gray).setRepresentation(DecorativeGeometry::DrawWireframe));
+// boxBody.addDecoration(Transform(),
+// DecorativeSphere(1).setColor(Gray).setOpacity(.1).setResolution(10));
+
+ const Real fFac = 0.3; // to turn off friction
+ const Real fDis = 0.1; // to turn off dissipation
+ const Real fVis = 0.01; // to turn off viscous friction
+ const Real fK = 1e+8; // pascals
+
+ boxBody.addContactSurface(Transform(),
+ ContactSurface(box,
+ ContactMaterial(fK, fDis, fFac, fFac, fVis),
+ .5 /*thickness*/)
+ );
+ boxBody2.addContactSurface(Transform(),
+ ContactSurface(box2,
+ ContactMaterial(fK, fDis, fFac, fFac, fVis),
+ .5 /*thickness*/)
+ );
+ MobilizedBody::Free boxMBody(matter.Ground(), Transform(Vec3(0)), boxBody, Transform(Vec3(0)));
+
+ MobilizedBody::Weld boxMBody2(matter.Ground(), Transform(Vec3(0)), boxBody2, Transform(Vec3(0)));
+
+ Visualizer viz(system);
+ // viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
+ viz.setMode(Visualizer::RealTime);
+ viz.setDesiredBufferLengthInSec(1);
+ viz.setDesiredFrameRate(FrameRate);
+ viz.setGroundHeight(0);
+ viz.setShowShadows(true);
+
+ Visualizer::InputSilo* silo = new Visualizer::InputSilo();
+ viz.addInputListener(silo);
+ Array_<std::pair<String,int> > runMenuItems;
+ runMenuItems.push_back(std::make_pair("Go", GoItem));
+ runMenuItems.push_back(std::make_pair("Replay", ReplayItem));
+ runMenuItems.push_back(std::make_pair("Quit", QuitItem));
+ viz.addMenu("Run", RunMenuId, runMenuItems);
+
+ Array_<std::pair<String,int> > helpMenuItems;
+ helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1));
+ viz.addMenu("Help", HelpMenuId, helpMenuItems);
+
+ // system.addEventReporter(new MyReporter(system,contactForces,ReportInterval));
+ system.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));
+
+ // Check for a Run->Quit menu pick every 1/4 second.
+ system.addEventHandler(new UserInputHandler(*silo, .25));
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ //ball.setQToFitTransform(state, Transform(Rotation(Pi/2,XAxis),
+ // Vec3(0,-1.8,0)));
+ boxMBody.setQToFitTransform(state, Transform(Vec3(0, 10, 0)));
+ boxMBody2.setQToFitTransform(state, Transform(Vec3(0, 0, 0)));
+
+ viz.report(state);
+
+ cout << "\nChoose 'Go' from Run menu to simulate:\n";
+ int menuId, item;
+ do { silo->waitForMenuPick(menuId, item);
+ if (menuId != RunMenuId || item != GoItem)
+ cout << "\aDude ... follow instructions!\n";
+ } while (menuId != RunMenuId || item != GoItem);
+
+ //ball.setOneU(state, 2, -20);
+
+ // ball.setOneU(state, 0, .05); // to break symmetry
+
+ CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
+ integ.setAccuracy(1e-3); // minimum for CPodes
+ TimeStepper ts(system, integ);
+
+ ts.initialize(state);
+
+ double cpuStart = cpuTime();
+ double realStart = realTime();
+
+ ts.stepTo(100.0);
+
+ const double timeInSec = realTime() - realStart;
+ const int evals = integ.getNumRealizations();
+ cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
+ timeInSec << "s elapsed for " << ts.getTime() << "s sim (avg step="
+ << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
+ << (1000*ts.getTime())/evals << "ms/eval\n";
+ cout << " CPU time was " << cpuTime() - cpuStart << "s\n";
+
+ printf("Using Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
+
+ viz.dumpStats(std::cout);
+
+ // Add as slider to control playback speed.
+ viz.addSlider("Speed", 1, 0, 4, 1);
+ viz.setMode(Visualizer::PassThrough);
+
+ silo->clear(); // forget earlier input
+ double speed = 1; // will change if slider moves
+ while(true) {
+ cout << "Choose Run/Replay to see that again ...\n";
+
+ int menuId, item;
+ silo->waitForMenuPick(menuId, item);
+
+
+ if (menuId != RunMenuId) {
+ cout << "\aUse the Run menu!\n";
+ continue;
+ }
+
+ if (item == QuitItem)
+ break;
+ if (item != ReplayItem) {
+ cout << "\aHuh? Try again.\n";
+ continue;
+ }
+
+ for (double i=0; i < (int)saveEm.size(); i += speed ) {
+ int slider; Real newValue;
+ if (silo->takeSliderMove(slider,newValue)) {
+ speed = newValue;
+ }
+ viz.report(saveEm[(int)i]);
+ }
+ }
+
+ } catch (const std::exception& e) {
+ std::printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+
+ } catch (...) {
+ std::printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+ return 0;
+}
+
+
+// Create a triangle mesh in the shape of a pyramid, with the
+// square base in the x-z plane centered at 0,0,0 of given side length s.
+// The base is split into two triangles. The apex will be at (0,s,0).
+static void makePyramid(Real s, PolygonalMesh& pyramidMesh) {
+ const Real h = s/2;
+ Array_<Vec3> vertices;
+ vertices.push_back(Vec3(-h, 0, -h)); // base
+ vertices.push_back(Vec3( h, 0, -h));
+ vertices.push_back(Vec3( h, 0, h));
+ vertices.push_back(Vec3(-h, 0, h));
+ vertices.push_back(Vec3( 0, s, 0)); // apex
+ Array_<int> faceIndices;
+ int faces[6][3] = {{0, 1, 2}, {0, 2, 3}, {1, 0, 4},
+ {2, 1, 4}, {3, 2, 4}, {0, 3, 4}};
+ for (int i = 0; i < 6; i++)
+ for (int j = 0; j < 3; j++)
+ faceIndices.push_back(faces[i][j]);
+
+ for (unsigned i=0; i < vertices.size(); ++i)
+ pyramidMesh.addVertex(vertices[i]);
+ for (unsigned i=0; i < faceIndices.size(); i += 3) {
+ const Array_<int> verts(&faceIndices[i], &faceIndices[i]+3);
+ pyramidMesh.addFace(verts);
+ }
+}
+
+
+// Create a triangle mesh in the shape of a tetrahedron with the
+// points in the corners of a cube inscribed in a sphere of radius r.
+static void makeTetrahedron(Real r, PolygonalMesh& tet) {
+ const Real h = r/std::sqrt(Real(3)); // half-dim of cube
+ Array_<Vec3> vertices;
+ vertices.push_back(Vec3( h, h, h));
+ vertices.push_back(Vec3(-h,-h, h));
+ vertices.push_back(Vec3(-h, h, -h));
+ vertices.push_back(Vec3( h,-h, -h));
+ Array_<int> faceIndices;
+ int faces[4][3] = {{0, 2, 1}, {1, 3, 0}, {0, 3, 2}, {2, 3, 1}};
+ for (int i = 0; i < 4; i++)
+ for (int j = 0; j < 3; j++)
+ faceIndices.push_back(faces[i][j]);
+
+ for (unsigned i=0; i < vertices.size(); ++i)
+ tet.addVertex(vertices[i]);
+ for (unsigned i=0; i < faceIndices.size(); i += 3) {
+ const Array_<int> verts(&faceIndices[i], &faceIndices[i]+3);
+ tet.addFace(verts);
+ }
+}
+
+static void makeOctahedralMesh(const Vec3& r, Array_<Vec3>& vertices,
+ Array_<int>& faceIndices) {
+ vertices.push_back(Vec3( r[0], 0, 0)); //0
+ vertices.push_back(Vec3(-r[0], 0, 0)); //1
+ vertices.push_back(Vec3( 0, r[1], 0)); //2
+ vertices.push_back(Vec3( 0, -r[1], 0)); //3
+ vertices.push_back(Vec3( 0, 0, r[2])); //4
+ vertices.push_back(Vec3( 0, 0, -r[2])); //5
+ int faces[8][3] = {{0, 2, 4}, {4, 2, 1}, {1, 2, 5}, {5, 2, 0},
+ {4, 3, 0}, {1, 3, 4}, {5, 3, 1}, {0, 3, 5}};
+ for (int i = 0; i < 8; i++)
+ for (int j = 0; j < 3; j++)
+ faceIndices.push_back(faces[i][j]);
+}
+
+// Create a triangle mesh in the shape of an octahedron (like two
+// pyramids stacked base-to-base, with the square base in the x-z plane
+// centered at 0,0,0 of given "radius" r.
+// The apexes will be at (0,+/-r,0).
+static void makeOctahedron(Real r, PolygonalMesh& mesh) {
+ Array_<Vec3> vertices;
+ Array_<int> faceIndices;
+ makeOctahedralMesh(Vec3(r), vertices, faceIndices);
+
+ for (unsigned i=0; i < vertices.size(); ++i)
+ mesh.addVertex(vertices[i]);
+ for (unsigned i=0; i < faceIndices.size(); i += 3) {
+ const Array_<int> verts(&faceIndices[i], &faceIndices[i]+3);
+ mesh.addFace(verts);
+ }
+}
+
+static void makeCube(Real h, PolygonalMesh& cube) {
+ Array_<Vec3> vertices;
+ vertices.push_back(Vec3( h, h, h));
+ vertices.push_back(Vec3( h, h, -h));
+ vertices.push_back(Vec3( h,-h, h));
+ vertices.push_back(Vec3( h,-h, -h));
+ vertices.push_back(Vec3(-h, h, h));
+ vertices.push_back(Vec3(-h, h, -h));
+ vertices.push_back(Vec3(-h,-h, h));
+ vertices.push_back(Vec3(-h,-h, -h));
+
+ Array_<int> faceIndices;
+ int faces[6][4] = {{0,2,3,1},{1,5,4,0},{0,4,6,2},
+ {2,6,7,3},{3,7,5,1},{4,5,7,6}};
+ for (int i = 0; i < 6; i++)
+ for (int j = 0; j < 4; j++)
+ faceIndices.push_back(faces[i][j]);
+
+ for (unsigned i=0; i < vertices.size(); ++i)
+ cube.addVertex(vertices[i]);
+ for (unsigned i=0; i < faceIndices.size(); i += 4) {
+ const Array_<int> verts(&faceIndices[i], &faceIndices[i]+4);
+ cube.addFace(verts);
+ }
+}
+
+
diff --git a/Simbody/examples/ExampleWrapping.cpp b/Simbody/examples/ExampleWrapping.cpp
new file mode 100644
index 0000000..3ec24d6
--- /dev/null
+++ b/Simbody/examples/ExampleWrapping.cpp
@@ -0,0 +1,318 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) ExampleWrapping *
+ * -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Ian Stavness, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/*
+ * This example demonstrates cable path wrapping for one obstacle
+ */
+
+#include "Simbody.h"
+
+using namespace SimTK;
+using std::cos;
+using std::sin;
+using std::cout;
+using std::endl;
+
+// Newton solver settings
+const Real ftol = 1e-9;
+const Real xtol = 1e-9;
+const Real minlam = 1e-9;
+const int maxNewtonIterations = 25;
+
+Real pauseBetweenPathIterations = 1; // sec
+Real estimatedPathErrorAccuracy = 1e-12;
+
+const Real vizInterval = Infinity; // set to 1/30. to vizualize shooting
+
+
+class VizPeriodicReporter : public PeriodicEventReporter {
+public:
+ VizPeriodicReporter(const Visualizer& viz, const State& dummyState, Real interval) :
+ PeriodicEventReporter(interval), viz(viz), dummyState(dummyState) {
+ }
+
+ void handleEvent(const State& state) const {
+ viz.report(dummyState);
+ }
+
+private:
+ const Visualizer& viz;
+ const State& dummyState;
+};
+
+/*
+ * This class is used to calculate the path error for a single obstacle
+ */
+class PathError: public Differentiator::JacobianFunction {
+
+public:
+ PathError(int nf, int ny, ContactGeometry& geom, Geodesic& geod,
+ const Vec3& O, const Vec3& I) :
+ Differentiator::JacobianFunction(nf, ny),
+ geom(geom), geod(geod),
+ O(O), I(I) { }
+
+ // x = ~[P, Q]
+ int f(const Vector& x, Vector& fx) const {
+ Vec3 P(&x[0]);
+ Vec3 Q(&x[3]);
+
+ UnitVec3 r_OP(P-O);
+ UnitVec3 r_QI(I-Q);
+
+ geod.clear();
+ geom.calcGeodesic(P, Q, r_OP, -r_QI, geod);
+
+ const UnitVec3& nP = geod.getNormalP();
+ const UnitVec3& nQ = geod.getNormalQ();
+ const UnitVec3& bP = geod.getBinormalP();
+ const UnitVec3& bQ = geod.getBinormalQ();
+
+ fx[0] = ~r_OP*nP;
+ fx[1] = ~r_QI*nQ;
+ fx[2] = ~r_OP*bP;
+ fx[3] = ~r_QI*bQ;
+ fx[4] = geom.calcSurfaceValue(P);
+ fx[5] = geom.calcSurfaceValue(Q);
+
+ return 0;
+ }
+
+ const Geodesic& getGeod() {
+ return geod;
+ }
+
+private:
+ ContactGeometry& geom;
+ const Vec3& O;
+ const Vec3& I;
+
+ // temporary variables
+ Geodesic& geod;
+
+}; // class PathError
+
+/*
+ * This class is used to calculate the path error for a single obstacle
+ * using the split geodesic error
+ */
+class PathErrorSplit: public Differentiator::JacobianFunction {
+
+public:
+ PathErrorSplit(int nf, int ny, ContactGeometry& geom, Geodesic& geod,
+ const Vec3& O, const Vec3& I) :
+ Differentiator::JacobianFunction(nf, ny),
+ geom(geom), geod(geod),
+ O(O), I(I) { }
+
+ // x = ~[P, Q]
+ int f(const Vector& x, Vector& fx) const {
+ Vec3 P(&x[0]);
+ Vec3 Q(&x[3]);
+
+ // calculate plane bisecting P and Q, and use as termination condition for integrator
+ UnitVec3 normal(Q-P);
+ Real offset = (~(P+Q)*normal)/2 ;
+ geom.setPlane(Plane(normal, offset));
+
+ UnitVec3 nP = geom.calcSurfaceUnitNormal(P);
+ UnitVec3 nQ = geom.calcSurfaceUnitNormal(Q);
+
+ UnitVec3 e_OP(P-O);
+ UnitVec3 e_QI(I-Q);
+
+ UnitVec3 tP(e_OP-nP*(~nP*e_OP));
+ UnitVec3 tQ(e_QI-nQ*(~nQ*e_QI));
+
+ geod.clear();
+ Vec2 geodErr = geom.calcSplitGeodErrorAnalytical(P, Q, tP, -tQ);
+
+ fx[0] = ~e_OP*nP;
+ fx[1] = ~e_QI*nQ;
+ fx[2] = geodErr[0];
+ fx[3] = geodErr[1];
+ fx[4] = geom.calcSurfaceValue(P);
+ fx[5] = geom.calcSurfaceValue(Q);
+
+ return 0;
+ }
+
+ const Geodesic& getGeod() {
+ return geod;
+ }
+
+private:
+ ContactGeometry& geom;
+ const Vec3& O;
+ const Vec3& I;
+
+ // temporary variables
+ Geodesic& geod;
+
+}; // class PathErrorSplit
+
+static Real maxabs(Vector x) {
+ Real maxVal = 0;
+ for (int i = 0; i < x.size(); ++i) {
+ if (std::abs(x[i]) > maxVal)
+ maxVal = std::abs(x[i]);
+ }
+ return maxVal;
+}
+
+static Real maxabsdiff(Vector x, Vector xold) {
+// ASSERT(x.size()==xold.size());
+ Real maxVal = 0;
+ for (int i = 0; i < x.size(); ++i) {
+ if (std::abs(x[i]-xold[i])/std::max(x[i],1.0) > maxVal)
+ maxVal = std::abs(x[i]-xold[i])/std::max(x[i],1.0);
+ }
+ return maxVal;
+}
+
+
+int main() {
+ try {
+
+ // setup test problem
+ double r = .5;
+ double uP = -Pi/2;
+ double vP = Pi/3;
+ double uQ = 0;
+ double vQ = 2;
+ Vec3 O(-r, -r, 0.2);
+ Vec3 I(r, r, -r);
+ Vec3 P(r*cos(uP)*sin(vP), r*sin(uP)*sin(vP), r*cos(vP));
+ Vec3 Q(r*cos(uQ)*sin(vQ), r*sin(uQ)*sin(vQ), r*cos(vQ));
+
+ Vec3 r_OP = P-O;
+ Vec3 r_IQ = Q-I;
+ Vec3 tP = r_OP.normalize();
+ Vec3 tQ = r_IQ.normalize();
+
+ int n = 6; // problem size
+ Vector x(n), dx(n), Fx(n), xold(n);
+ Matrix J(n,n);
+
+ ContactGeometry::Sphere geom(r);
+// r = 2;
+// Vec3 radii(1,2,3);
+// ContactGeometry::Ellipsoid geom(radii);
+ Geodesic geod;
+
+ // Create a dummy MultibodySystem for visualization purposes
+ MultibodySystem dummySystem;
+ SimbodyMatterSubsystem matter(dummySystem);
+ matter.updGround().addBodyDecoration(Transform(), geom.createDecorativeGeometry()
+ .setColor(Gray)
+ .setOpacity(0.5)
+ .setResolution(5));
+
+ // Visualize with default options; ask for a report every 1/30 of a second
+ // to match the Visualizer's default 30 frames per second rate.
+ Visualizer viz(dummySystem);
+ viz.setBackgroundType(Visualizer::SolidColor);
+ dummySystem.addEventReporter(new Visualizer::Reporter(viz, 1./30));
+
+ // add vizualization callbacks for geodesics, contact points, etc.
+ viz.addDecorationGenerator(new GeodesicDecorator(geom.getGeodP(), Red));
+ viz.addDecorationGenerator(new GeodesicDecorator(geom.getGeodQ(), Blue));
+ viz.addDecorationGenerator(new GeodesicDecorator(geod, Orange));
+ viz.addDecorationGenerator(new PlaneDecorator(geom.getPlane(), Gray));
+ viz.addDecorationGenerator(new PathDecorator(x, O, I, Green));
+ dummySystem.realizeTopology();
+ State dummyState = dummySystem.getDefaultState();
+
+
+ // calculate the geodesic
+ geom.addVizReporter(new VizPeriodicReporter(viz, dummyState, vizInterval));
+ viz.report(dummyState);
+
+ // creat path error function
+ //PathError pathErrorFnc(n, n, geom, geod, O, I);
+ PathErrorSplit pathErrorFnc(n, n, geom, geod, O, I);
+ pathErrorFnc.setEstimatedAccuracy(estimatedPathErrorAccuracy);
+ Differentiator diff(pathErrorFnc);
+
+ // set initial conditions
+ x[0]=P[0]; x[1]=P[1]; x[2]=P[2];
+ x[3]=Q[0]; x[4]=Q[1]; x[5]=Q[2];
+
+ Real f, fold, lam;
+
+ pathErrorFnc.f(x, Fx);
+ viz.report(dummyState);
+ sleepInSec(pauseBetweenPathIterations);
+
+ f = std::sqrt(~Fx*Fx);
+ for (int i = 0; i < maxNewtonIterations; ++i) {
+ if (f < ftol) {
+ std::cout << "path converged in " << i << " iterations" << std::endl;
+// cout << "obstacle err = " << Fx << ", x = " << x << endl;
+ break;
+ }
+
+ diff.calcJacobian(x, Fx, J, Differentiator::ForwardDifference);
+ dx = J.invert()*Fx;
+
+ fold = f;
+ xold = x;
+
+ // backtracking
+ lam = 1;
+ while (true) {
+ x = xold - lam*dx;
+ cout << "TRY stepsz=" << lam << " sz*dx=" << lam*dx << endl;
+ pathErrorFnc.f(x, Fx);
+ f = std::sqrt(~Fx*Fx);
+ if (f > fold && lam > minlam) {
+ lam = lam / 2;
+ } else {
+ break;
+ }
+ }
+ if (maxabsdiff(x,xold) < xtol) {
+ std::cout << "converged on step size after " << i << " iterations" << std::endl;
+ std::cout << "error = " << Fx << std::endl;
+ break;
+ }
+ viz.report(dummyState);
+ sleepInSec(pauseBetweenPathIterations);
+
+ }
+ cout << "obstacle error = " << Fx << endl;
+
+ cout << "num geodP pts = " << geom.getGeodP().getNumPoints() << endl;
+
+
+ } catch (const std::exception& e) {
+ std::printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+
+ } catch (...) {
+ std::printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+ return 0;
+}
diff --git a/Simbody/examples/Gazebo2Simbody.cpp b/Simbody/examples/Gazebo2Simbody.cpp
new file mode 100644
index 0000000..3aae7d2
--- /dev/null
+++ b/Simbody/examples/Gazebo2Simbody.cpp
@@ -0,0 +1,1123 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Gazebo 2 Simbody *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* This example reads input in the form of a Gazebo ".sdf" file and generates
+a roughly-equivalent Simbody model. Gazebo is a robot simulator from the Open
+Source Robotics Foundation (http://osrfoundation.org). This is not intended to
+be comprehensive -- it is just a proof of concept.
+
+We will construct the Simbody model here and maintain a mapping between the
+Gazebo objects and the Simbody implementation of them to demonstrate how this
+might be done in Gazebo.
+
+Gazebo file format notes
+------------------------
+An sdf file describes a model in XML format using the following objects:
+world The Ground frame. May have associated geometry and lights. Contains one
+ or more "model" objects. Normally the ground is the x-y plane, with
+ +z the "up" direction and a ground plane at z=0. The world also acts
+ as a link whose name is "world".
+model Named grouping of physical objects. Provides a model frame given
+ relative to the world frame, however this frame is still fixed in the
+ world. A model contains links and joints.
+link Named mass- and geometry-carrying object (a body). There is a link
+ frame, and inertial, visual, and collision objects each with their own
+ frame given relative to the link frame. An initial pose for the link
+ frame is given, relative to the model frame.
+joint Connection between two links or between a link and the world.
+ There is a parent and child link, given by name, with "world"
+ the name for the ground link. There is a joint type and then appropriate
+ parameters depending on the type. There is a child frame and a
+ parent frame. However, if only a single frame is given for the joint
+ it is interpreted as the child frame and then the default link frame
+ poses are used to find the coincident frame on the parent.
+
+NOTE: for purposes of this example we have implemented some extensions:
+- You can specify that a link must be a base link, i.e. must be connected
+ directly to World. Use
+ <must_be_base_link> 1 </must_be_base_link>
+ in the <link> element.
+- You can force a loop to be cut at a particular joint. Use
+ <must_be_loop_joint> 1 </must_be_loop_joint>
+ in the <joint> element.
+- We accept John Hsu's proposed modification allowing a joint to specify
+ separate child and parent frames. Use
+ <child link="childName" ><pose>...</pose></child>
+ <parent link="parentName"><pose>...</pose></parent>
+ to specify separate frame. (We also accept the old style.)
+
+A <pose> element (containing six scalars) is equivalent to a Simbody Transform,
+interpreted as x,y,z translation vector followed by an x-y-z body-fixed Euler
+angle sequence given in radians. Gazebo refers to this as pitch-roll-yaw.
+
+A <visual> element has <geometry> and <material> subelements; here we'll use the
+material only to pick a color.
+
+A <collision> element has <geometry> and <surface> subelements, with the
+latter giving the physical properties used in contact. We'll generate a gray
+visual for these.
+
+Generalized coordinate representation
+-------------------------------------
+We use Simbody's MultibodyGraphMaker class to take the Gazebo definition of
+links and joints and construct a spanning tree whose root is
+the World, with every link appearing exactly once. Any link that does not appear
+in any joint will be given six degrees of freedom relative to the World via a
+Simbody "Free" mobilizer. Note: If a disconnected link is a point mass (that is,
+inertialess) it should be mobilized with a 3 dof "Translation" mobilizer but
+we're not doing that in this example. Also if there are disconnected branches we
+will pick one of the bodies as a "base" body and connect it to World by a Free
+(6dof) mobilizer.
+
+MultibodyGraphMaker won't take the parent-child specification too seriously when
+building the tree since these can be arbitrary in the Gazebo description. To
+avoid confusion, we speak of the "inboard" and "outboard" bodies of a mobilizer;
+usually inboard==parent and outboard==child but the relationship is opposite for
+a reverse mobilizer. For example, a three-body chain might be defined
+ link1--j1--link2--j2--link3
+ joint1 parent=link1 child=link2
+ joint2 parent=link3 child=link2
+There is no way to build a tree where inboard/outboard matches parent/child
+for both joints in this example. MultibodyGraphMaker will tell us if we have
+to reverse, which we'll do using Simbody's reverse mobilizer capability so
+that the meaning of the joint is unchanged.
+
+After the tree is built, all the links will have been used but there may still
+be some unused joints. Those joints will involve parent and child links both
+of which are already in the tree. That means these joints form topological
+loops in the multibody graph. MultibodyGraphMaker will normally handle that by
+splitting the child link to create a new "slave" body, mobilizing the slave
+with the given joint, and then introducing a Weld constraint (-6 dofs) to hold
+the master link and its slaves together. Note that in this example the visual
+and contact geometry stays with the master; the mass and inertia are divided
+equally among the master and slaves. We'll also generate some "shadow"
+visualization for the slaves since they are interesting.
+
+An alternative is used if you tell MultibodyGraphMaker that there is a good
+constraint that can be used directly to replace a loop joint. Since Simbody's
+Ball constraint (-3 dofs) is mostly indistinguishable from a Ball mobilizer
+(+3 dofs), we'll substitute a Ball constraint when breaking a loop at a
+ball joint, resulting in a smaller system overall. */
+#include "Simbody.h"
+
+#include <utility>
+#include <string>
+#include <vector>
+#include <map>
+#include <iostream>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+// Skip down to main() first -- these are just some helper classes to keep
+// track of the Gazebo model and connect it to its Simbody implementation.
+
+// Hacks for sherm's testing.
+#define ANIMATE // turn this off for timing tests
+//#define ADD_JOINT_SPRINGS // makes all the revolute & prismatic joints stiff
+//#define RAGDOLL2_ICS // set initial conditions for ragdoll2 model
+//#define USE_CONTACT_MESH // use elastic foundation model for cylinder
+
+//==============================================================================
+// GAZEBO LINK INFO
+//==============================================================================
+// This is one link's information read from the Gazebo input file and translated
+// into Simbody's terminology and conventions. The mapping to Simbody
+// MobilizedBody is written here after we build the Simbody System.
+class GazeboLinkInfo {
+public:
+ explicit GazeboLinkInfo(const std::string& name)
+ : name(name), mustBeBaseLink(false), selfCollide(false) {}
+
+ // When a link is broken into several fragments (master and slaves), they
+ // share the mass equally. Given the number of fragments, this returns the
+ // appropriate mass properties to use for each fragment. Per Simbody's
+ // convention, COM is measured from, and inertia taken about, the link
+ // origin and both are expressed in the link frame.
+ MassProperties getEffectiveMassProps(int numFragments) const {
+ assert(numFragments > 0); // must be at least 1 for the master
+ return MassProperties(massProps.getMass()/numFragments,
+ massProps.getMassCenter(),
+ massProps.getUnitInertia());
+ }
+
+ Xml::Element element; // if this was in the Xml file
+ std::string name;
+ bool mustBeBaseLink;
+
+ bool selfCollide; // if true can collide with other links in
+ // the same Gazebo model.
+
+ // Mass properties converted to Simbody terms: com & inertia expressed in
+ // body frame; inertia taken about body origin *not* body COM. This is the
+ // full mass; if there are slaves the master and slaves will split the mass
+ // properties evenly.
+ MassProperties massProps;
+ Transform defX_ML; // default pose in model frame
+
+ // Which MobilizedBody corresponds to the master instance of this link.
+ MobilizedBody masterMobod;
+
+ // If this link got split into a master and slaves, these are the
+ // MobilizedBodies used to mobilize the slaves.
+ std::vector<MobilizedBody> slaveMobods;
+
+ // And these are the Weld constraints used to attach slaves to master.
+ std::vector<Constraint::Weld> slaveWelds;
+};
+
+
+//==============================================================================
+// GAZEBO JOINT INFO
+//==============================================================================
+// This is one joint's information read from the Gazebo input file and
+// translated into Simbody's terminology and conventions. The joint will
+// typically have been modeled as a Mobilizer, but may have been modeled as
+// a Constraint; in either case the corresponding Simbody element is written
+// here after we build the Simbody System.
+class GazeboJointInfo {
+public:
+ GazeboJointInfo(const std::string& name, const std::string& type)
+ : name(name), type(type), mustBreakLoopHere(false), isReversed(false) {}
+
+ // These are set when we process the input.
+
+ Xml::Element element; // if this was in the Xml file
+ std::string name, type, parent, child;
+ bool mustBreakLoopHere;
+
+ // Normally A=F, B=M. But if reversed, then B=F, A=M.
+ Transform X_PA; // parent body frame to mobilizer frame
+ Transform X_CB; // child body frame to mobilizer frame
+ Transform defX_AB; // default mobilizer pose
+
+ // Members below here are set when we build the Simbody model.
+
+ // How this joint was modeled in the Simbody System. We used either a
+ // mobilizer or a constraint, but not both. The type of either one is the
+ // same as the joint type above.
+ MobilizedBody mobod; // isValid() if we used a mobilizer
+ bool isReversed; // if mobilizer, did it reverse parent&child?
+
+ Constraint constraint; // isValid() if we used a constraint
+};
+
+
+//==============================================================================
+// GAZEBO LINKS
+//==============================================================================
+// This collects all the links for a particular model, and maintains a
+// name->GazeboLinkInfo mapping. Be sure to add a world link first, with the
+// appropriate pose measured from the model frame.
+class GazeboLinks {
+public:
+ // Add a new link to the collection and index it by name.
+ int addLink(const GazeboLinkInfo& info);
+ // Return the number of links so far.
+ int size() const {return (int)linkByIndex.size();}
+
+ bool hasLink(const std::string& name) const
+ { return name2index.find(name) != name2index.end(); }
+ bool hasLink(int index) const
+ { assert(index>=0); return index < size(); }
+
+ // Get link by name (const or writable).
+ const GazeboLinkInfo& getLink(const std::string& name) const
+ { return getLink(getLinkIndex(name)); }
+ GazeboLinkInfo& updLink(const std::string& name)
+ { return updLink(getLinkIndex(name)); }
+
+ // Get link fast by index (const or writable).
+ GazeboLinkInfo& updLink(int index)
+ { return linkByIndex[index]; }
+ const GazeboLinkInfo& getLink(int index) const
+ { return linkByIndex[index];}
+
+private:
+ int getLinkIndex(const std::string& name) const
+ { assert(hasLink(name)); return name2index.find(name)->second; }
+ std::vector<GazeboLinkInfo> linkByIndex;
+ std::map<std::string,int> name2index;
+};
+
+
+//==============================================================================
+// GAZEBO JOINTS
+//==============================================================================
+// This collects all the joints for a particular model, and maintains a
+// name->GazeboJointInfo mapping.
+class GazeboJoints {
+public:
+ // Add a new joint to the collection, and return its assigned joint index,
+ // starting from zero for the first joint and counting up from there.
+ int addJoint(const GazeboJointInfo& info);
+ // Return the number of joints added so far.
+ int size() const {return (int)jointByIndex.size();}
+
+ bool hasJoint(const std::string& name) const
+ { return name2index.find(name) != name2index.end(); }
+ bool hasJoint(int index) const
+ { assert(index>=0); return index < size(); }
+
+ // Get joint by name (const or writable).
+ const GazeboJointInfo& getJoint(const std::string& name) const
+ { return getJoint(getJointIndex(name)); }
+ GazeboJointInfo& updJoint(const std::string& name)
+ { return updJoint(getJointIndex(name)); }
+
+ // Get joint fast by index (const or writable).
+ const GazeboJointInfo& getJoint(int index) const
+ { return jointByIndex[index]; }
+ GazeboJointInfo& updJoint(int index)
+ { return jointByIndex[index]; }
+
+private:
+ int getJointIndex(const std::string& name) const
+ { assert(hasJoint(name)); return name2index.find(name)->second; }
+
+ std::vector<GazeboJointInfo> jointByIndex;
+ std::map<std::string,int> name2index;
+};
+
+
+//==============================================================================
+// GAZEBO MODEL
+//==============================================================================
+// Contains model-level information and the set of links and joints that
+// were found in the input file for this model.
+class GazeboModel {
+public:
+ GazeboModel() : isStatic(false) {}
+ void readModel(Xml::Element modelElt);
+
+ std::string name;
+ Transform X_WM; // model frame in the World frame
+ bool isStatic; // means all bodies are attached to Ground
+ GazeboLinks links;
+ GazeboJoints joints;
+
+ // This is a grouping of contact surfaces that are invisible to one
+ // another. Gazebo's convention is that surfaces in the same model don't
+ // collide unless they have been explicitly marked <self_collide>. The
+ // Simbody equivalent is to put the unmarked ones into the same clique.
+ ContactCliqueId modelClique;
+};
+
+//==============================================================================
+// MAIN
+//==============================================================================
+
+
+
+static void createMultibodyGraph(GazeboModel& model,
+ MultibodyGraphMaker& mbgraph);
+
+static void addModelToSimbodySystem(const MultibodyGraphMaker& mbgraph,
+ GazeboModel& model,
+ MultibodySystem& mbs,
+ SimbodyMatterSubsystem& matter,
+ GeneralForceSubsystem& forces,
+ CompliantContactSubsystem& contact);
+
+static void runSimulation(const MultibodySystem& mbs,
+ const std::vector<GazeboModel>& gzModels);
+
+int main(int argc, const char* argv[]) {
+ std::string sdfFileName;
+ if (argc < 2) {
+ std::cout << "Usage: " << argv[0] << " Gazebo_filename.sdf\n";
+ std::cout << "Trying Gazebo_ragdoll.sdf by default.\n";
+ sdfFileName = "Gazebo_ragdoll.sdf";
+ } else
+ sdfFileName = argv[1];
+
+ try {
+ //---------------------------- OPEN INPUT FILE -----------------------------
+ // Attempt to identify this as a Gazebo input file, find the World
+ // to get gravity and locate the Models.
+ std::cout << "Working directory: "
+ << Pathname::getCurrentWorkingDirectory() << std::endl;
+ std::cout << "Reading file: " << sdfFileName << std::endl;
+ Xml::Document sdf(sdfFileName);
+
+ if (sdf.getRootTag() != "sdf" && sdf.getRootTag() != "gazebo")
+ throw std::runtime_error
+ ("Expected to see document tag <sdf> or <gazebo> but saw <"
+ + sdf.getRootTag() + "> instead.");
+
+ // This is a Gazebo document.
+ Xml::Element root = sdf.getRootElement();
+ cout << "sdf version="
+ << root.getOptionalAttributeValue("version", "unspecified") << endl;
+
+ Xml::Element world = root.getRequiredElement("world");
+ Xml::Element physics = world.getOptionalElement("physics");
+ const Vec3 gravity = // default is std gravity at Earth's surface, m/s^2
+ world.hasElement("gravity")
+ ? world.getRequiredElementValueAs<Vec3>("gravity")
+ : (physics.hasElement("gravity") ? physics.getRequiredElementValueAs<Vec3>("gravity")
+ : Vec3(0,0,-9.80665));
+
+ Array_<Xml::Element> models = world.getAllElements("model");
+ cout << "World '" << world.getOptionalAttributeValue("name","NONAME")
+ << "' contains " << models.size() << " model(s).\n";
+ cout << "Gravity=" << gravity << endl;
+
+ if (models.empty()) {
+ cout << "File contained no model -- nothing to do. Goodbye.\n";
+ return 0;
+ }
+
+ //------------------------ CREATE SIMBODY SYSTEM ---------------------------
+ // Create a Simbody System and populate it with Subsystems we'll need.
+ MultibodySystem mbs;
+ SimbodyMatterSubsystem matter(mbs);
+ GeneralForceSubsystem forces(mbs);
+ ContactTrackerSubsystem tracker(mbs);
+ CompliantContactSubsystem contact(mbs, tracker);
+ // Tweak visualization to get Z up and to prevent Simbody from generating
+ // its own sketchy visuals.
+ mbs.setUpDirection(ZAxis);
+ matter.setShowDefaultGeometry(false);
+ // Set stiction max slip velocity to make it less stiff.
+ contact.setTransitionVelocity(0.05);
+ // Specify gravity (read in above from world).
+ Force::UniformGravity(forces, matter, gravity);
+ // Define a material to use for ground contact. This is not very stiff.
+ ContactMaterial groundMaterial(1e6, // stiffness
+ 0.1, // dissipation
+ 0.7, // mu_static
+ 0.5, // mu_dynamic
+ 0.5); // mu_viscous
+ // Add a contact surface to represent the ground.
+ // Half space normal is -x; must rotate about y to make it +z.
+ matter.Ground().updBody().addContactSurface(Rotation(Pi/2,YAxis),
+ ContactSurface(ContactGeometry::HalfSpace(), groundMaterial));
+ // Draw world frame and model frame.
+ matter.Ground().addBodyDecoration(Vec3(0),
+ DecorativeFrame(1).setColor(Green).setLineThickness(3)); // World
+
+ std::vector<GazeboModel> gzModels(models.size());
+ //------------------------ READ IN GAZEBO MODELS ---------------------------
+ for (unsigned m=0; m < models.size(); ++m) {
+ gzModels[m].readModel(models[m]); // TODO: ignore other models for now
+
+ //----------------------- GENERATE MULTIBODY GRAPH -------------------------
+ MultibodyGraphMaker mbgraph;
+ createMultibodyGraph(gzModels[m], mbgraph);
+ // Optional: dump the graph to stdout for debugging or curiosity.
+ mbgraph.dumpGraph(std::cout);
+
+ addModelToSimbodySystem(mbgraph, gzModels[m],
+ mbs, matter, forces, contact);
+ }
+
+ //--------------------------- RUN A SIMULATION -----------------------------
+ // The Simbody System has been built successfully. Now lets run it for
+ // a while to see what it looks like.
+ runSimulation(mbs, gzModels);
+
+ } catch (const std::exception& e) {
+ cout << "EXCEPTION: " << e.what() << "\n";
+ return 1;
+ }
+
+ printf("DONE.\n");
+ return 0;
+}
+
+
+//==============================================================================
+// CREATE MULTIBODY GRAPH
+//==============================================================================
+// Define Gazebo joint types, then use links and joints in the given model
+// to construct a reasonable spanning-tree-plus-constraints multibody graph
+// to represent that model. An exception will be thrown if this fails.
+// Note that this step is not Simbody dependent.
+static void createMultibodyGraph(GazeboModel& model,
+ MultibodyGraphMaker& mbgraph) {
+ // Step 1: Tell MultibodyGraphMaker about joints it should know about.
+ // Note: "weld" and "free" are always predefined at 0 and 6 dofs, resp.
+ // Gazebo name #dofs Simbody equivalent
+ mbgraph.addJointType("revolute", 1); // Pin
+ mbgraph.addJointType("revolute2", 2); // ?
+ mbgraph.addJointType("prismatic", 1); // Slider
+ mbgraph.addJointType("universal", 2); // Universal
+ mbgraph.addJointType("piston", 2); // Cylinder
+ // Simbody has a Ball constraint that is a good choice if you need to
+ // break a loop at a ball joint.
+ mbgraph.addJointType("ball", 3, true); // Ball
+
+ // Step 2: Tell it about all the links we read from the input file,
+ // starting with world, and provide a reference pointer.
+ for (int lx=0; lx < model.links.size(); ++lx) {
+ GazeboLinkInfo& link = model.links.updLink(lx);
+ mbgraph.addBody(link.name, link.massProps.getMass(),
+ link.mustBeBaseLink, &link);
+ }
+
+ // Step 3: Tell it about all the joints we read from the input file,
+ // and provide a reference pointer.
+ for (int jx=0; jx < model.joints.size(); ++jx) {
+ GazeboJointInfo& joint = model.joints.updJoint(jx);
+ mbgraph.addJoint(joint.name, joint.type, joint.parent, joint.child,
+ joint.mustBreakLoopHere, &joint);
+ }
+
+ // Setp 4. Generate the multibody graph.
+ mbgraph.generateGraph();
+}
+
+
+
+//==============================================================================
+// AUXILIARY FUNCTIONS FOR XML READING
+//==============================================================================
+
+// Convert the given pose in x,y,z,thetax,thetay,thetaz format to a Simbody
+// Transform. The rotation angles are interpreted as a body-fixed sequence,
+// meaning we rotation about x, then about the new y, then about the now twice-
+// rotated z.
+static Transform pose2Transform(const Vec6& pose) {
+ Transform frame(Rotation(SimTK::BodyRotationSequence,
+ pose[3], XAxis, pose[4], YAxis, pose[5], ZAxis),
+ pose.getSubVec<3>(0));
+ return frame;
+}
+
+// Convert a Simbody transform to a pose in x,y,z,thetax,thetay,thetaz format.
+static Vec6 transform2Pose(const Transform& X_AB) {
+ Vec6 pose;
+ pose.updSubVec<3>(0) = X_AB.p(); // position vector
+ pose.updSubVec<3>(3) = X_AB.R().convertRotationToBodyFixedXYZ();
+ return pose;
+}
+
+// If the given element contains a <pose> element, return it as a Transform.
+// Otherwise return the identity Transform. If there is more than one <pose>
+// element, only the first one is processed.
+static Transform getPose(Xml::Element element) {
+ const Vec6 pose = element.getOptionalElementValueAs<Vec6>("pose",Vec6(0));
+ return pose2Transform(pose);
+}
+
+// Look for an <inertial> element within the given element (which is probably
+// a link but we don't care). If found, parse and transform to link (body)
+// frame. Otherwise return unit mass and inertia. Result is returned as a
+// Simbody MassProperties element containing mass, center of mass location,
+// and unit inertia about body origin.
+static MassProperties getMassProperties(Xml::Element link) {
+ Xml::Element inertial = link.getOptionalElement("inertial");
+ if (!inertial.isValid())
+ return MassProperties(1,Vec3(0),UnitInertia(1,1,1));
+ const Real mass = inertial.getOptionalElementValueAs<Real>("mass", 1.);
+ Transform X_LI = getPose(inertial); // identity if not provided
+ const Vec3 com_L = X_LI.p(); // vector from Lo to com, exp. in L
+ Xml::Element inertia = inertial.getOptionalElement("inertia");
+ if (mass==0 || !inertia.isValid())
+ return MassProperties(mass,com_L,UnitInertia(1,1,1));
+ // Get mass-weighted central inertia, expressed in I frame.
+ Inertia Ic_I(inertia.getOptionalElementValueAs<Real>("ixx", 1.),
+ inertia.getOptionalElementValueAs<Real>("iyy", 1.),
+ inertia.getOptionalElementValueAs<Real>("izz", 1.),
+ inertia.getOptionalElementValueAs<Real>("ixy", 0.),
+ inertia.getOptionalElementValueAs<Real>("ixz", 0.),
+ inertia.getOptionalElementValueAs<Real>("iyz", 0.));
+ // Re-express the central inertia from the I frame to the L frame.
+ Inertia Ic_L = Ic_I.reexpress(~X_LI.R()); // Ic_L=R_LI*Ic_I*R_IL
+ // Shift to L frame origin.
+ Inertia Io_L = Ic_L.shiftFromMassCenter(-com_L, mass);
+ return MassProperties(mass, com_L, Io_L); // converts to unit inertia
+}
+
+
+//==============================================================================
+// ADD MODEL TO SIMBODY SYSTEM
+//==============================================================================
+// Given a desired multibody graph, gravity, and the Gazebo model that was
+// used to generate the graph, add elements to the Simbody System to represent
+// it. There are many limitations here, especially in the handling of contact.
+// Any Gazebo features that we haven't modeled are just ignored.
+// The GazeboModel is updated so that its links and joints have references to
+// their corresponding Simbody elements.
+// We set up some visualization here so we can see what's happening but this
+// would not be needed in Gazebo since it does its own visualization.
+static void addModelToSimbodySystem(const MultibodyGraphMaker& mbgraph,
+ GazeboModel& model,
+ MultibodySystem& mbs,
+ SimbodyMatterSubsystem& matter,
+ GeneralForceSubsystem& forces,
+ CompliantContactSubsystem& contact)
+{
+ // Define a material to use for contact. This is not very stiff.
+ ContactMaterial material(1e6, // stiffness
+ 0.1, // dissipation
+ 0.7, // mu_static
+ 0.5, // mu_dynamic
+ 0.5); // mu_viscous
+
+ // Draw the model frame.
+ matter.Ground().addBodyDecoration(model.X_WM,
+ DecorativeFrame(.75).setColor(Orange).setLineThickness(3)); // Model
+ matter.Ground().addBodyDecoration(model.X_WM.p()+Vec3(0,0,.1),
+ DecorativeText(model.name).setScale(.2)
+ .setColor(model.isStatic?Green:Cyan));
+
+ // Generate a contact clique we can put collision geometry in to prevent
+ // self-collisions.
+ model.modelClique = ContactSurface::createNewContactClique();
+
+ // Record the MobilizedBody for the World link.
+ model.links.updLink(0).masterMobod = matter.Ground();
+
+ // Run through all the mobilizers in the multibody graph, adding a Simbody
+ // MobilizedBody for each one. Also add visual and collision geometry to the
+ // bodies when they are mobilized.
+ for (int mobNum=0; mobNum < mbgraph.getNumMobilizers(); ++mobNum) {
+ // Get a mobilizer from the graph, then extract its corresponding
+ // joint and bodies. Note that these don't necessarily have equivalents
+ // in the GazeboLink and GazeboJoint inputs.
+ const MultibodyGraphMaker::Mobilizer& mob = mbgraph.getMobilizer(mobNum);
+ const std::string& type = mob.getJointTypeName();
+
+ // The inboard body always corresponds to one of the input links,
+ // because a slave link is always the outboard body of a mobilizer.
+ // The outboard body may be slave, but its master body is one of the
+ // Gazebo input links.
+ const bool isSlave = mob.isSlaveMobilizer();
+ GazeboLinkInfo& gzInb = *(GazeboLinkInfo*)mob.getInboardBodyRef();
+ GazeboLinkInfo& gzOutb = *(GazeboLinkInfo*)mob.getOutboardMasterBodyRef();
+
+ const MassProperties massProps =
+ gzOutb.getEffectiveMassProps(mob.getNumFragments());
+
+ // This will reference the new mobilized body once we create it.
+ MobilizedBody mobod;
+
+ if (model.isStatic) {
+ mobod = matter.updGround();
+ } else if (mob.isAddedBaseMobilizer()) {
+ // There is no corresponding Gazebo joint for this mobilizer.
+ // Create the joint and set its default position to be the default
+ // pose of the base link relative to the Ground frame.
+ assert(type=="free"); // May add more types later
+ if (type == "free") {
+ MobilizedBody::Free freeJoint(
+ gzInb.masterMobod, Transform(),
+ massProps, Transform());
+ freeJoint.setDefaultTransform(~gzInb.defX_ML*gzOutb.defX_ML);
+ mobod = freeJoint;
+ }
+ } else {
+ // This mobilizer does correspond to one of the input joints.
+ GazeboJointInfo& gzJoint = *(GazeboJointInfo*)mob.getJointRef();
+ const bool isReversed = mob.isReversedFromJoint();
+
+ // Find inboard and outboard frames for the mobilizer; these are
+ // parent and child frames or the reverse.
+
+ const Transform& X_IF0 = isReversed ? gzJoint.X_CB : gzJoint.X_PA;
+ const Transform& X_OM0 = isReversed ? gzJoint.X_PA : gzJoint.X_CB;
+
+ const MobilizedBody::Direction direction =
+ isReversed ? MobilizedBody::Reverse : MobilizedBody::Forward;
+
+ if (type == "free") {
+ MobilizedBody::Free freeJoint(
+ gzInb.masterMobod, X_IF0,
+ massProps, X_OM0,
+ direction);
+ Transform defX_FM = isReversed ? Transform(~gzJoint.defX_AB)
+ : gzJoint.defX_AB;
+ freeJoint.setDefaultTransform(defX_FM);
+ mobod = freeJoint;
+ } else if (type == "revolute") {
+ Xml::Element axisElt = gzJoint.element.getRequiredElement("axis");
+ UnitVec3 axis =
+ UnitVec3(axisElt.getRequiredElementValueAs<Vec3>("xyz"));
+ Rotation R_JZ(axis, ZAxis); // Simbody's pin is along Z
+ Transform X_IF(X_IF0.R()*R_JZ, X_IF0.p());
+ Transform X_OM(X_OM0.R()*R_JZ, X_OM0.p());
+ MobilizedBody::Pin pinJoint(
+ gzInb.masterMobod, X_IF,
+ massProps, X_OM,
+ direction);
+ mobod = pinJoint;
+
+ #ifdef ADD_JOINT_SPRINGS
+ // KLUDGE add spring (stiffness proportional to mass)
+ Force::MobilityLinearSpring(forces,mobod,0,
+ 30*massProps.getMass(),0);
+ #endif
+ } else if (type == "prismatic") {
+ Xml::Element axisElt = gzJoint.element.getRequiredElement("axis");
+ UnitVec3 axis =
+ UnitVec3(axisElt.getRequiredElementValueAs<Vec3>("xyz"));
+ Rotation R_JX(axis, XAxis); // Simbody's slider is along X
+ Transform X_IF(X_IF0.R()*R_JX, X_IF0.p());
+ Transform X_OM(X_OM0.R()*R_JX, X_OM0.p());
+ MobilizedBody::Slider sliderJoint(
+ gzInb.masterMobod, X_IF,
+ massProps, X_OM,
+ direction);
+ mobod = sliderJoint;
+
+ #ifdef ADD_JOINT_SPRINGS
+ // KLUDGE add spring (stiffness proportional to mass)
+ Force::MobilityLinearSpring(forces,mobod,0,
+ 30*massProps.getMass(),0);
+ #endif
+ } else if (type == "ball") {
+ MobilizedBody::Ball ballJoint(
+ gzInb.masterMobod, X_IF0,
+ massProps, X_OM0,
+ direction);
+ Rotation defR_FM = isReversed
+ ? Rotation(~gzJoint.defX_AB.R())
+ : gzJoint.defX_AB.R();
+ ballJoint.setDefaultRotation(defR_FM);
+ mobod = ballJoint;
+ }
+
+ // Created a mobilizer that corresponds to gzJoint. Keep track.
+ gzJoint.mobod = mobod;
+ gzJoint.isReversed = isReversed;
+ }
+
+ // Link gzOutb has been mobilized; keep track for later.
+ if (isSlave) gzOutb.slaveMobods.push_back(mobod);
+ else gzOutb.masterMobod = mobod;
+
+ // A mobilizer has been created; now add the visual and collision
+ // geometry for the new mobilized body.
+
+ Xml::Element master = gzOutb.element;
+ Vec3 color = isSlave ? Red : Cyan;
+ Real scale = isSlave ? 0.9 : 1.;
+ if (master.isValid()) {
+ // VISUAL
+ Array_<Xml::Element> visuals = master.getAllElements("visual");
+ for (unsigned i=0; i < visuals.size(); ++i) {
+ Transform X_LV = getPose(visuals[i]);
+ Xml::Element geo = visuals[i].getRequiredElement("geometry");
+ Xml::Element box = geo.getOptionalElement("box");
+ if (box.isValid()) {
+ Vec3 sz = box.getRequiredElementValueAs<Vec3>("size");
+ mobod.addBodyDecoration(
+ X_LV, DecorativeBrick(sz/2).setOpacity(.5)
+ .setColor(color).setScale(scale));
+ }
+ Xml::Element sphere = geo.getOptionalElement("sphere");
+ if (sphere.isValid()) {
+ Real r = sphere.getRequiredElementValueAs<Real>("radius");
+ mobod.addBodyDecoration(
+ X_LV, DecorativeSphere(r).setOpacity(.5)
+ .setColor(color).setScale(scale));
+ }
+ Xml::Element cyl = geo.getOptionalElement("cylinder");
+ if (cyl.isValid()) {
+ Real r = cyl.getRequiredElementValueAs<Real>("radius");
+ Real l = cyl.getRequiredElementValueAs<Real>("length");
+ // Cylinder is along Z in Gazebo, Y in Simbody
+ Rotation YtoZ(Pi/2, XAxis);
+ mobod.addBodyDecoration(
+ Transform(X_LV.R()*YtoZ, X_LV.p()),
+ DecorativeCylinder(r, l/2).setOpacity(.5)
+ .setColor(color).setScale(scale));
+ }
+
+ }
+
+ // COLLISION
+ Array_<Xml::Element> coll = master.getAllElements("collision");
+ const Vec3 collColor = model.isStatic ? Green : Gray;
+ for (unsigned i=0; i < coll.size(); ++i) {
+ Transform X_LC = getPose(coll[i]);
+ Xml::Element geo = coll[i].getRequiredElement("geometry");
+
+ // Model sphere collision surface.
+ Xml::Element sphere = geo.getOptionalElement("sphere");
+ if (sphere.isValid()) {
+ Real r = sphere.getRequiredElementValueAs<Real>("radius");
+ mobod.addBodyDecoration(
+ X_LC, DecorativeSphere(r)
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setColor(collColor));
+ ContactSurface surface(ContactGeometry::Sphere(r),
+ material);
+ if (!gzOutb.selfCollide)
+ surface.joinClique(model.modelClique);
+ mobod.updBody().addContactSurface(X_LC, surface);
+ }
+
+ // Model cylinder collision surface (must fake with ellipsoid).
+ Xml::Element cyl = geo.getOptionalElement("cylinder");
+ if (cyl.isValid()) {
+ Real r = cyl.getRequiredElementValueAs<Real>("radius");
+ Real len = cyl.getRequiredElementValueAs<Real>("length");
+ // Cylinder is along Z in Gazebo
+#ifndef USE_CONTACT_MESH
+ Vec3 esz = Vec3(r,r,len/2); // Use ellipsoid instead
+ mobod.addBodyDecoration(X_LC,
+ DecorativeEllipsoid(esz)
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setColor(collColor));
+ ContactSurface surface(ContactGeometry::Ellipsoid(esz),
+ material);
+#else
+ const int resolution = 0; // chunky hexagonal shape
+ const PolygonalMesh mesh = PolygonalMesh::
+ createCylinderMesh(ZAxis,r,len/2,resolution);
+ const ContactGeometry::TriangleMesh triMesh(mesh);
+
+ mobod.addBodyDecoration(X_LC,
+ DecorativeMesh(triMesh.createPolygonalMesh())
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setColor(collColor));
+ ContactSurface surface(triMesh, material,1 /*Thickness*/);
+#endif
+ if (!gzOutb.selfCollide)
+ surface.joinClique(model.modelClique);
+ mobod.updBody().addContactSurface(X_LC, surface);
+ }
+
+
+ // Model box collision surface (must fake with ellipsoid).
+ Xml::Element box = geo.getOptionalElement("box");
+ if (box.isValid()) {
+ Vec3 hsz = box.getRequiredElementValueAs<Vec3>("size")/2;
+#ifndef USE_CONTACT_MESH
+ mobod.addBodyDecoration(X_LC,
+ DecorativeEllipsoid(hsz) // use half dimensions
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setColor(collColor));
+ ContactSurface surface(ContactGeometry::Ellipsoid(hsz),
+ material);
+#else
+ const int resolution = 20; // need dense to get near corners
+ const PolygonalMesh mesh = PolygonalMesh::
+ createBrickMesh(hsz,resolution);
+ const ContactGeometry::TriangleMesh triMesh(mesh);
+
+ mobod.addBodyDecoration(X_LC,
+ DecorativeMesh(triMesh.createPolygonalMesh())
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setColor(collColor));
+ ContactSurface surface(triMesh, material,1 /*Thickness*/);
+#endif
+ if (!gzOutb.selfCollide)
+ surface.joinClique(model.modelClique);
+ mobod.updBody().addContactSurface(X_LC, surface);
+ }
+ }
+ }
+ }
+
+ // Weld the slaves to their masters.
+ for (int lx=0; lx < model.links.size(); ++lx) {
+ GazeboLinkInfo& link = model.links.updLink(lx);
+ if (link.slaveMobods.empty()) continue;
+ for (unsigned i=0; i < link.slaveMobods.size(); ++i) {
+ Constraint::Weld weld(link.masterMobod, link.slaveMobods[i]);
+ link.slaveWelds.push_back(weld); // in case we want to know later
+ }
+ }
+
+ // Add the loop joints if any.
+ for (int lcx=0; lcx < mbgraph.getNumLoopConstraints(); ++lcx) {
+ const MultibodyGraphMaker::LoopConstraint& loop =
+ mbgraph.getLoopConstraint(lcx);
+
+ GazeboJointInfo& joint = *(GazeboJointInfo*)loop.getJointRef();
+ GazeboLinkInfo& parent = *(GazeboLinkInfo*) loop.getParentBodyRef();
+ GazeboLinkInfo& child = *(GazeboLinkInfo*) loop.getChildBodyRef();
+
+ if (joint.type == "weld") {
+ Constraint::Weld weld(parent.masterMobod, joint.X_PA,
+ child.masterMobod, joint.X_CB);
+ joint.constraint = weld;
+ } else if (joint.type == "ball") {
+ Constraint::Ball ball(parent.masterMobod, joint.X_PA.p(),
+ child.masterMobod, joint.X_CB.p());
+ joint.constraint = ball;
+ } else if (joint.type == "free") {
+ // A "free" loop constraint is no constraint at all so we can
+ // just ignore it. It might be more convenient if there were
+ // a 0-constraint Constraint::Free, just as there is a 0-mobility
+ // MobilizedBody::Weld.
+ } else
+ throw std::runtime_error(
+ "Unrecognized loop constraint type '" + joint.type + "'.");
+ }
+}
+
+
+//==============================================================================
+// RUN SIMULATION
+//==============================================================================
+// Run a simulation and extract some data from it to show how that can be done.
+static void runSimulation(const MultibodySystem& mbs,
+ const std::vector<GazeboModel>& gzModels) {
+ // Create a Visualizer so we can see what's what.
+ Visualizer viz(mbs);
+ viz.setShowSimTime(true);
+
+ // Initialize the system and obtain the default state.
+ State state = mbs.realizeTopology();
+
+// The ragdoll2 model needs some reasonable initial conditions in order to
+// assemble its loop joints properly.
+#ifdef RAGDOLL2_ICS
+ gzModels[0].joints.getJoint("l_upper_arm_joint").mobod.setOneQ(state,0,-.4);
+ gzModels[0].joints.getJoint("r_upper_arm_joint").mobod.setOneQ(state,0,-.4);
+ gzModels[0].joints.getJoint("torso_joint").mobod.setOneQ(state,0,.3);
+#endif
+
+ viz.report(state); // Show the initial state.
+
+ // If there are any constraints due to topological loops in the graph
+ // then the system might not be assembled yet. Use Simbody's Assembler
+ // to find a state that assembles the system.
+ // Note: assembly mail fail if the system starts out too far from
+ // assembled.
+
+ printf("INITIAL STATE err=%g -- ENTER to assemble\n",
+ state.getQErr().norm());
+ getchar();
+ Assembler assembler(mbs);
+ Real assemblyTol = assembler.assemble(state);
+ viz.report(state);
+ printf("ASSEMBLED to err=%g -- ENTER to simulate\n",
+ state.getQErr().norm());
+ getchar();
+
+ // Assembled. Now simulate.
+
+ const Real RunTime = 20;
+ const Real ReportTime = 1./30.;
+ //const Real Accuracy = 0.001; // 0.1% is Simbody default
+ //const Real Accuracy = 0.05; // 5%
+ const Real Accuracy = 0.1; // 1%
+ //const Real Accuracy = 0.10; // 10%
+ //const Real Accuracy = 0.20; // 20%
+
+ // Use a low order integrator if you force the max step size to be small.
+ //const Real MaxStepSize = .001;
+ //RungeKutta2Integrator integ(mbs);
+ const Real MaxStepSize = Infinity;
+ RungeKutta3Integrator integ(mbs);
+ //RungeKuttaMersonIntegrator integ(mbs); // 4th order
+ //CPodesIntegrator integ(mbs); // implicit integrator
+
+ integ.setAccuracy(Accuracy);
+ integ.setConstraintTolerance(std::min(1e-3, Accuracy/10));
+
+ integ.initialize(state);
+ viz.report(integ.getState());
+ cout << "q=" << state.getQ() << endl;
+ double startReal = realTime();
+ double startCPU = cpuTime();
+ while (integ.getTime() < RunTime) {
+ Real nextReport = std::min(RunTime, integ.getTime() + ReportTime);
+ do {
+ integ.stepTo(nextReport, integ.getTime()+MaxStepSize);
+ } while (integ.getTime() < nextReport);
+
+ const State& state = integ.getState();
+ const Real t = state.getTime();
+
+ #ifdef ANIMATE // suppress for more accurate CPU time measurement
+ viz.report(state);
+ #endif
+
+ // Show body origin locations and joint angles and rates at integer
+ // times to demo data extraction.
+ if (std::abs(std::floor(t+ReportTime/2)-t) > ReportTime/2)
+ continue;
+
+ for (unsigned m=0; m < gzModels.size(); ++m) {
+ const GazeboModel& model = gzModels[m];
+ printf("\nMODEL %s --------------------\n", model.name.c_str());
+ printf("\n LINKS t=%g\n", t);
+ for (int i=0; i < model.links.size(); ++i) {
+ const GazeboLinkInfo& link = model.links.getLink(i);
+ const Vec3& loc = link.masterMobod.getBodyOriginLocation(state);
+ printf(" %20s %10.3g %10.3g %10.3g\n",
+ link.name.c_str(), loc[0], loc[1], loc[2]);
+ }
+ printf("\n JOINTS t=%g\n", t);
+ for (int i=0; i < model.joints.size(); ++i) {
+ const GazeboJointInfo& joint = model.joints.getJoint(i);
+ const Real q0 = joint.mobod.getOneQ(state, 0);
+ const Real u0 = joint.mobod.getOneU(state, 0);
+ printf(" %20s %10.3g %10.3g\n",
+ joint.name.c_str(), q0, u0);
+ }
+ }
+
+ }
+ viz.report(integ.getState()); // show final state
+
+ // Finished simulating; dump out some stats. On Windows CPUtime is not
+ // reliable if very little time is spent in this thread.
+ const double timeInSec = realTime()-startReal;
+ const double cpuInSec = cpuTime()-startCPU;
+ const int evals = integ.getNumRealizations();
+ cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
+ timeInSec << "s for " << integ.getTime() << "s sim (avg step="
+ << (1000*integ.getTime())/integ.getNumStepsTaken() << "ms) "
+ << (1000*integ.getTime())/evals << "sim ms/eval\n";
+ cout << "CPUtime (not reliable when visualizing) " << cpuInSec << endl;
+
+ printf("Used Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(),
+ integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(),
+ integ.getNumProjections());
+}
+
+
+//==============================================================================
+// IMPLEMENTATIONS OF GAZEBO CLASSES
+//==============================================================================
+
+int GazeboLinks::addLink(const GazeboLinkInfo& info) {
+ if (name2index.find(info.name) != name2index.end())
+ throw std::runtime_error("GazeboLinks::addLink(): Link name '"
+ + info.name + " was used more than once.");
+
+ const int lx = (int)linkByIndex.size();
+ linkByIndex.push_back(info);
+ name2index[info.name] = lx;
+ return lx;
+}
+
+
+int GazeboJoints::addJoint(const GazeboJointInfo& info) {
+ if(name2index.find(info.name) != name2index.end())
+ throw std::runtime_error(
+ "GazeboJoints::addJoint(): Joint name '" + info.name
+ + "' was used more than once.");
+
+ const int jx = (int)jointByIndex.size();
+ jointByIndex.push_back(info);
+ name2index[info.name] = jx;
+ return jx;
+}
+
+
+
+void GazeboModel::readModel(Xml::Element modelElt) {
+ name = modelElt.getRequiredAttributeValue("name");
+ X_WM = getPose(modelElt);
+ isStatic = modelElt.getOptionalElementValueAs<bool>("static", false);
+
+ Array_<Xml::Element> linkElts = modelElt.getAllElements("link");
+ Array_<Xml::Element> jointElts = modelElt.getAllElements("joint");
+ printf("Reading %s model '%s' with ground + %d links, %d joints.\n",
+ isStatic ? "STATIC" : "DYNAMIC", name.c_str(),
+ linkElts.size(), jointElts.size());
+ cout << " Model frame X_WM as pose=" << transform2Pose(X_WM) << endl;
+
+ // Create a World link, then read in the real links.
+ GazeboLinkInfo worldLink("world");
+ worldLink.defX_ML = ~X_WM; // different for each model in this world
+ worldLink.massProps = MassProperties(Infinity,Vec3(0),UnitInertia(1));
+ links.addLink(worldLink);
+ for (unsigned i=0; i < linkElts.size(); ++i) {
+ Xml::Element elt = linkElts[i];
+ GazeboLinkInfo linfo(elt.getRequiredAttributeValue("name"));
+ linfo.mustBeBaseLink = elt.getOptionalElementValueAs<bool>
+ ("must_be_base_link", false);
+ linfo.selfCollide = elt.getOptionalElementValueAs<bool>
+ ("self_collide", false);
+ linfo.element = elt;
+ linfo.defX_ML = getPose(elt); // default link pose in Model frame
+ linfo.massProps = getMassProperties(elt);
+ links.addLink(linfo);
+ }
+
+ // Read in the joints.
+ for (unsigned i=0; i < jointElts.size(); ++i) {
+ Xml::Element elt = jointElts[i];
+ const std::string name = elt.getRequiredAttributeValue("name");
+ const std::string type = elt.getRequiredAttributeValue("type");
+ GazeboJointInfo jinfo(name,type);
+ jinfo.mustBreakLoopHere = elt.getOptionalElementValueAs<bool>
+ ("must_be_loop_joint", false);
+ jinfo.element = elt;
+
+ // We'll accept either old style:
+ // <pose>pose on child</pose>
+ // <child>childName</child>
+ // <parent>parentName</parent>
+ // or new style
+ // <child>
+ // <link_name>childName</linkName>
+ // <pose>childPose</pose>
+ // </child>
+ // <parent>
+ // <link_name>parentName</linkName>
+ // <pose>parentPose</pose>
+ // </parent>
+ std::string child, parent;
+ jinfo.X_CB = getPose(elt); // default joint frame on child body
+ Xml::Element cElt = elt.getRequiredElement("child");
+ if (cElt.hasElement("link_name")) {
+ child = cElt.getRequiredElementValue("link_name");
+ if (cElt.hasElement("pose"))
+ jinfo.X_CB = getPose(cElt);
+ // else leave it as it was
+ } else child = cElt.getValue(); // old style
+ // At this point we have the child frame in X_CB
+
+ Xml::Element pElt = elt.getRequiredElement("parent");
+ bool gotParentPose = false;
+ if (pElt.hasElement("link_name")) {
+ parent = pElt.getRequiredElementValue("link_name");
+ if (pElt.hasElement("pose")) {
+ jinfo.X_PA = getPose(pElt);
+ gotParentPose = true;
+ }
+ } else parent = pElt.getValue(); // old style
+
+ assert(links.hasLink(parent) && links.hasLink(child));
+ assert(child != parent);
+
+ jinfo.parent = parent;
+ jinfo.child = child;
+ if (!gotParentPose) {
+ // Must derive joint frame on parent using the default body poses
+ // TODO: this should be fixed in the sdf file
+ const Transform X_MC = links.getLink(child).defX_ML;
+ const Transform X_MP = links.getLink(parent).defX_ML;
+ const Transform X_PC = ~X_MP*X_MC;
+ jinfo.X_PA = X_PC*jinfo.X_CB; // i.e., A spatially coincident with B
+ }
+
+ joints.addJoint(jinfo);
+ }
+}
+
diff --git a/Simbody/examples/Gazebo_double_pendulum.sdf b/Simbody/examples/Gazebo_double_pendulum.sdf
new file mode 100644
index 0000000..69c20f4
--- /dev/null
+++ b/Simbody/examples/Gazebo_double_pendulum.sdf
@@ -0,0 +1,115 @@
+<?xml version="1.0" ?>
+<sdf version="1.3">
+ <world name="default">
+ <gravity>0 0 -9.81</gravity>
+ <!-- A global light source -->
+ <include>
+ <uri>model://sun</uri>
+ </include>
+
+ <!-- A ground plane at z=0 -->
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <!-- Double pendulum model -->
+ <model name="double_pendulum">
+ <!-- pose specified as: x y z roll pitch yaw (angles in radians) -->
+ <!-- this is the model pose, which defines the pin location for the upper link -->
+ <!-- the upper link is 4 meters above the ground plane -->
+ <!-- the initial angle of the upper link is -1.4 radians -->
+ <pose>0 0 4 -1.4 0 0</pose>
+ <!-- start declaration of upper link with length 1 meter -->
+ <link name="upper_link">
+ <!-- start at the model origin -->
+ <pose>0 0 0 0 0 0</pose>
+ <inertial>
+ <!-- this pose specifies the c.g. location of the upper link -->
+ <pose>0 0 -0.5 0 0 0</pose>
+ <mass>10.0</mass>
+ <inertia>
+ <!-- it would be nice if these parameters could be linked but that's something for the future -->
+ <!-- see https://bitbucket.org/osrf/gazebo/issue/210/incorporate-xacro-into-sdf -->
+ <ixx>1.0</ixx><ixy>0.0</ixy><ixz>0.0</ixz>
+ <iyy>1.0</iyy><iyz>0.0</iyz>
+ <izz>1.0</izz>
+ </inertia>
+ </inertial>
+ <!-- this describes what is displayed on screen -->
+ <visual name="visual_cylinder">
+ <pose>0 0 -0.5 0 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.1</radius>
+ <length>1.0</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>Gazebo/Green</script>
+ </material>
+ </visual>
+ <!-- this describes the collision geometry -->
+ <collision name="collision_cylinder">
+ <pose>0 0 -0.5 0 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.1</radius>
+ <length>1.0</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ </link>
+ <!-- now define the lower link of length 1 meter attached to the end of the upper_link -->
+ <link name="lower_link">
+ <pose>0 0 -1.0 0 0 0</pose>
+ <inertial>
+ <pose>0 0 -0.5 0 0 0</pose>
+ <mass>10.0</mass>
+ <inertia>
+ <ixx>1.0</ixx><ixy>0.0</ixy><ixz>0.0</ixz>
+ <iyy>1.0</iyy><iyz>0.0</iyz>
+ <izz>1.0</izz>
+ </inertia>
+ </inertial>
+ <visual name="visual_box">
+ <pose>0 0 -0.5 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.1 0.1 1.0</size>
+ </box>
+ </geometry>
+ <material>
+ <script>Gazebo/Red</script>
+ </material>
+ <cast_shadows>1</cast_shadows>
+ </visual>
+ <collision name="collision_box">
+ <pose>0 0 -0.5 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.100000 0.100000 1.000000</size>
+ </box>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="world_to_upper_pin_joint" type="revolute">
+ <parent>world</parent>
+ <child>upper_link</child>
+ <pose>0 0 0 0 0 0</pose>
+ <axis>
+ <!-- spins about x axis -->
+ <xyz>1 0 0</xyz>
+ </axis>
+ </joint>
+ <joint name="upper_to_lower_pin_joint" type="revolute">
+ <parent>upper_link</parent>
+ <child>lower_link</child>
+ <!-- this pose specifies location revolute joint, relative to the child link -->
+ <pose>0 0 0 0 0 0</pose>
+ <axis>
+ <xyz>1 0 0</xyz>
+ </axis>
+ </joint>
+ </model>
+ </world>
+</sdf>
diff --git a/Simbody/examples/Gazebo_ragdoll.sdf b/Simbody/examples/Gazebo_ragdoll.sdf
new file mode 100644
index 0000000..db90f4b
--- /dev/null
+++ b/Simbody/examples/Gazebo_ragdoll.sdf
@@ -0,0 +1,1866 @@
+<?xml version="1.0" ?>
+<!-- this was initially generated from ragdoll.urdf,
+ but have sence been updated to turn off gravity for certain links
+ and various tweaks -->
+<gazebo version="1.2">
+<world>
+ <gravity>0 0 -9.8</gravity>
+ <model name="ragdoll">
+ <pose>0.000000 0.000000 1.4 0.000000 -0.000000 0.000000</pose>
+ <link name="hip">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.100000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.100000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.100000</izz>
+ </inertia>
+ <mass>10.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="hip_geom">
+ <pose>0.000000 0.000000 0.125000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.100000 0.500000 0.250000</size>
+ </box>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="hip_geom_visual">
+ <pose>0.000000 0.000000 0.125000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.100000 0.500000 0.250000</size>
+ </box>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="l_thigh_pan">
+ <pose>0.000000 0.220000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.010000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.010000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.010000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="l_thigh_pan_geom">
+ <pose>0.000000 0.000000 -0.030000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.060000</radius>
+ </sphere>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="l_thigh_pan_geom_visual">
+ <pose>0.000000 0.000000 -0.030000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.060000</radius>
+ </sphere>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="l_thigh_lift">
+ <pose>0.000000 0.220000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.050000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.050000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.050000</izz>
+ </inertia>
+ <mass>5.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="l_thigh_lift_geom">
+ <pose>0.000000 0.000000 -0.350000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.060000</radius>
+ <length>0.700000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="l_thigh_lift_geom_visual">
+ <pose>0.000000 0.000000 -0.350000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.060000</radius>
+ <length>0.700000</length>
+ </cylinder>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="l_calf">
+ <pose>0.000000 0.220000 -0.700000 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.020000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.020000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.020000</izz>
+ </inertia>
+ <mass>2.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="l_calf_geom">
+ <pose>0.000000 0.000000 -0.300000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.050000</radius>
+ <length>0.600000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="l_calf_geom_visual">
+ <pose>0.000000 0.000000 -0.300000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.050000</radius>
+ <length>0.600000</length>
+ </cylinder>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="l_ankle">
+ <pose>0.000000 0.220000 -1.325000 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.010000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.010000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.010000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="l_ankle_geom">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.050000</radius>
+ </sphere>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="l_ankle_geom_visual">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.050000</radius>
+ </sphere>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="l_foot">
+ <pose>0.000000 0.220000 -1.350000 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.100000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.010000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.010000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.010000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="l_foot_geom">
+ <pose>0.100000 0.000000 -0.025000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.030000 0.050000</size>
+ </box>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="l_foot_geom_visual">
+ <pose>0.100000 0.000000 -0.025000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.030000 0.050000</size>
+ </box>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>1</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="r_thigh_pan">
+ <pose>0.000000 -0.220000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.010000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.010000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.010000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="r_thigh_pan_geom">
+ <pose>0.000000 0.000000 -0.030000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.060000</radius>
+ </sphere>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="r_thigh_pan_geom_visual">
+ <pose>0.000000 0.000000 -0.030000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.060000</radius>
+ </sphere>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="r_thigh_lift">
+ <pose>0.000000 -0.220000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.050000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.050000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.050000</izz>
+ </inertia>
+ <mass>5.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="r_thigh_lift_geom">
+ <pose>0.000000 0.000000 -0.350000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.060000</radius>
+ <length>0.700000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="r_thigh_lift_geom_visual">
+ <pose>0.000000 0.000000 -0.350000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.060000</radius>
+ <length>0.700000</length>
+ </cylinder>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="r_calf">
+ <pose>0.000000 -0.220000 -0.700000 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.020000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.020000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.020000</izz>
+ </inertia>
+ <mass>2.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="r_calf_geom">
+ <pose>0.000000 0.000000 -0.300000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.050000</radius>
+ <length>0.600000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="r_calf_geom_visual">
+ <pose>0.000000 0.000000 -0.300000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.050000</radius>
+ <length>0.600000</length>
+ </cylinder>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="r_ankle">
+ <pose>0.000000 -0.220000 -1.325000 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.010000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.010000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.010000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="r_ankle_geom">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.050000</radius>
+ </sphere>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="r_ankle_geom_visual">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.050000</radius>
+ </sphere>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="r_foot">
+ <pose>0.000000 -0.220000 -1.350000 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.100000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.010000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.010000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.010000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="r_foot_geom">
+ <pose>0.100000 0.000000 -0.025000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.030000 0.050000</size>
+ </box>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="r_foot_geom_visual">
+ <pose>0.100000 0.000000 -0.025000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.030000 0.050000</size>
+ </box>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>1</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="torso">
+ <pose>0.000000 0.000000 0.250000 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.300000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.300000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.300000</izz>
+ </inertia>
+ <mass>30.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="torso_geom">
+ <pose>0.000000 0.000000 0.350000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.100000 0.600000 0.700000</size>
+ </box>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="torso_geom_visual">
+ <pose>0.000000 0.000000 0.350000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.100000 0.600000 0.700000</size>
+ </box>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="l_shoulder">
+ <pose>0.000000 0.327500 0.922500 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.010000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.010000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.010000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="l_shoulder_geom">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.055000</radius>
+ </sphere>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="l_shoulder_geom_visual">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.055000</radius>
+ </sphere>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="l_upper_arm">
+ <pose>0.000000 0.327500 0.922500 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.020000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.020000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.020000</izz>
+ </inertia>
+ <mass>2.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="l_upper_arm_geom">
+ <pose>0.000000 0.000000 -0.300000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.055000</radius>
+ <length>0.600000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="l_upper_arm_geom_visual">
+ <pose>0.000000 0.000000 -0.300000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.055000</radius>
+ <length>0.600000</length>
+ </cylinder>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="l_forearm">
+ <pose>0.000000 0.327500 0.322500 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.010000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.010000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.010000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="l_forearm_geom">
+ <pose>0.000000 0.000000 -0.225000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.050000</radius>
+ <length>0.450000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="l_forearm_geom_visual">
+ <pose>0.000000 0.000000 -0.225000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.050000</radius>
+ <length>0.450000</length>
+ </cylinder>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="l_wrist">
+ <pose>0.000000 0.327500 -0.142500 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.010000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.010000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.010000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="l_wrist_geom">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.030000</radius>
+ </sphere>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="l_wrist_geom_visual">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.030000</radius>
+ </sphere>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="l_hand">
+ <pose>0.000000 0.327500 -0.157500 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.010000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.010000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.010000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="l_hand_geom">
+ <pose>0.000000 0.000000 -0.100000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.030000 0.100000 0.200000</size>
+ </box>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="l_hand_geom_visual">
+ <pose>0.000000 0.000000 -0.100000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.030000 0.100000 0.200000</size>
+ </box>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="neck">
+ <pose>0.000000 0.000000 0.950000 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.010000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.010000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.010000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="neck_geom">
+ <pose>0.000000 0.000000 0.100000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.075000</radius>
+ <length>0.200000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="neck_geom_visual">
+ <pose>0.000000 0.000000 0.100000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.075000</radius>
+ <length>0.200000</length>
+ </cylinder>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="head">
+ <pose>0.000000 0.000000 1.225000 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.100000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.100000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.100000</izz>
+ </inertia>
+ <mass>10.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="head_geom">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.150000</radius>
+ </sphere>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="head_geom_visual">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.150000</radius>
+ </sphere>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="r_shoulder">
+ <pose>0.000000 -0.327500 0.922500 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.010000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.010000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.010000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="r_shoulder_geom">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.055000</radius>
+ </sphere>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="r_shoulder_geom_visual">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.055000</radius>
+ </sphere>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="r_upper_arm">
+ <pose>0.000000 -0.327500 0.922500 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.020000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.020000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.020000</izz>
+ </inertia>
+ <mass>2.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="r_upper_arm_geom">
+ <pose>0.000000 0.000000 -0.300000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.055000</radius>
+ <length>0.600000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="r_upper_arm_geom_visual">
+ <pose>0.000000 0.000000 -0.300000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.055000</radius>
+ <length>0.600000</length>
+ </cylinder>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="r_forearm">
+ <pose>0.000000 -0.327500 0.322500 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.010000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.010000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.010000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="r_forearm_geom">
+ <pose>0.000000 0.000000 -0.225000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.050000</radius>
+ <length>0.450000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="r_forearm_geom_visual">
+ <pose>0.000000 0.000000 -0.225000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.050000</radius>
+ <length>0.450000</length>
+ </cylinder>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="r_wrist">
+ <pose>0.000000 -0.327500 -0.142500 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.010000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.010000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.010000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="r_wrist_geom">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.030000</radius>
+ </sphere>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="r_wrist_geom_visual">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.030000</radius>
+ </sphere>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <link name="r_hand">
+ <pose>0.000000 -0.327500 -0.157500 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.010000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.010000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.010000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ <density>1.000000</density>
+ </inertial>
+ <collision name="r_hand_geom">
+ <pose>0.000000 0.000000 -0.100000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.030000 0.100000 0.200000</size>
+ </box>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>-1.000000</mu>
+ <mu2>-1.000000</mu2>
+ <fdir1>0.000000 0.000000 0.000000</fdir1>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <bounce>
+ <restitution_coefficient>0.000000</restitution_coefficient>
+ <threshold>100000.000000</threshold>
+ </bounce>
+ <contact>
+ <ode>
+ <soft_cfm>0.000000</soft_cfm>
+ <soft_erp>0.200000</soft_erp>
+ <kp>1000000000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>0.000000</max_vel>
+ <min_depth>0.001000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <laser_retro>0.000000</laser_retro>
+ </collision>
+ <visual name="r_hand_geom_visual">
+ <pose>0.000000 0.000000 -0.100000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.030000 0.100000 0.200000</size>
+ </box>
+ </geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0.000000</laser_retro>
+ <transparency>0.000000</transparency>
+ </visual>
+ <gravity>0</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+ <joint name="l_thigh_pan_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>hip</parent>
+ <child>l_thigh_pan</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="l_thigh_lift_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>l_thigh_pan</parent>
+ <child>l_thigh_lift</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="l_calf_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>l_thigh_lift</parent>
+ <child>l_calf</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="l_ankle_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>l_calf</parent>
+ <child>l_ankle</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="l_foot_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>l_ankle</parent>
+ <child>l_foot</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="r_thigh_pan_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>hip</parent>
+ <child>r_thigh_pan</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="r_thigh_lift_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>r_thigh_pan</parent>
+ <child>r_thigh_lift</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="r_calf_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>r_thigh_lift</parent>
+ <child>r_calf</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="r_ankle_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>r_calf</parent>
+ <child>r_ankle</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="r_foot_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>r_ankle</parent>
+ <child>r_foot</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="torso_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>hip</parent>
+ <child>torso</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="l_shoulder_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>torso</parent>
+ <child>l_shoulder</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="l_upper_arm_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>l_shoulder</parent>
+ <child>l_upper_arm</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="l_forearm_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>l_upper_arm</parent>
+ <child>l_forearm</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="l_wrist_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>l_forearm</parent>
+ <child>l_wrist</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="l_hand_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>l_wrist</parent>
+ <child>l_hand</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="neck_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>torso</parent>
+ <child>neck</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="head_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>neck</parent>
+ <child>head</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="r_shoulder_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>torso</parent>
+ <child>r_shoulder</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="r_upper_arm_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>r_shoulder</parent>
+ <child>r_upper_arm</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="r_forearm_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>r_upper_arm</parent>
+ <child>r_forearm</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="r_wrist_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>r_forearm</parent>
+ <child>r_wrist</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <joint name="r_hand_joint" type="revolute">
+ <pose>0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>r_wrist</parent>
+ <child>r_hand</child>
+ <axis>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit lower="-1.570800" upper="1.570800" effort="0.000000" velocity="0.000000" />
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ </axis>
+ </joint>
+ <static>0</static>
+ </model>
+</world>
+</gazebo>
diff --git a/Simbody/examples/Gazebo_revolute_joint_test_merged.sdf b/Simbody/examples/Gazebo_revolute_joint_test_merged.sdf
new file mode 100644
index 0000000..82e3eed
--- /dev/null
+++ b/Simbody/examples/Gazebo_revolute_joint_test_merged.sdf
@@ -0,0 +1,2675 @@
+<?xml version="1.0" ?>
+<sdf version='1.3'>
+ <world name='default'>
+ <physics type='simbody'>
+ <gravity>0.000000 0.000000 -9.800000</gravity>
+ <update_rate>1000.000000</update_rate>
+ <ode>
+ <solver>
+ <dt>0.005000</dt>
+ <type>quick</type>
+ <iters>50</iters>
+ <sor>1.300000</sor>
+ </solver>
+ <constraints>
+ <cfm>0.000000</cfm>
+ <erp>0.200000</erp>
+ <contact_max_correcting_vel>100.000000</contact_max_correcting_vel>
+ <contact_surface_layer>0.001000</contact_surface_layer>
+ </constraints>
+ </ode>
+ <simbody>
+ <dt>0.005000</dt>
+ </simbody>
+ </physics>
+
+ <light name='sun' type='directional'>
+ <cast_shadows>1</cast_shadows>
+ <pose>0.000000 0.000000 10.000000 0.000000 0.000000 0.000000</pose>
+ <diffuse>0.800000 0.800000 0.800000 1.000000</diffuse>
+ <specular>0.100000 0.100000 0.100000 1.000000</specular>
+ <attenuation>
+ <range>1000.000000</range>
+ <constant>0.900000</constant>
+ <linear>0.010000</linear>
+ <quadratic>0.001000</quadratic>
+ </attenuation>
+ <direction>-0.500000 0.500000 -1.000000</direction>
+ </light>
+ <model name='ground_plane'>
+ <static>1</static>
+ <link name='link'>
+ <collision name='collision'>
+ <pose>0 0 1.75 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>1.5</radius>
+ </sphere>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>100.000000</mu>
+ <mu2>50.000000</mu2>
+ </ode>
+ </friction>
+ <bounce/>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='collision'>
+ <geometry>
+ <plane>
+ <normal>0.000000 0.000000 1.000000</normal>
+ <size>100.000000 100.000000</size>
+ </plane>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>100.000000</mu>
+ <mu2>50.000000</mu2>
+ </ode>
+ </friction>
+ <bounce/>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <visual name='visual'>
+ <cast_shadows>0</cast_shadows>
+ <geometry>
+ <plane>
+ <normal>0.000000 0.000000 1.000000</normal>
+ <size>100.000000 100.000000</size>
+ </plane>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ </model>
+ <model name='pendulum_0deg'>
+ <link name='base'>
+ <inertial>
+ <mass>100.000000</mass>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ </inertial>
+ <visual name='vis_plate_on_ground'>
+ <pose>0.000000 0.000000 0.050000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.800000</radius>
+ <length>0.100000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_pole'>
+ <pose>-0.275000 0.000000 1.100000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.200000 2.200000</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_plate_on_ground'>
+ <pose>0.000000 0.000000 0.050000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.800000</radius>
+ <length>0.100000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_pole'>
+ <pose>-0.275000 0.000000 1.100000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.200000 2.200000</size>
+ </box>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <link name='upper_link'>
+ <pose>0.000000 0.000000 2.100000 -1.570800 0.000000 0.000000</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ </inertial>
+ <visual name='vis_upper_joint'>
+ <pose>-0.050000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_lower_joint'>
+ <pose>0.000000 0.000000 1.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.200000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_upper_joint'>
+ <pose>-0.050000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_lower_joint'>
+ <pose>0.000000 0.000000 1.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.200000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <link name='lower_link'>
+ <pose>0.250000 1.000000 2.100000 -2.000000 0.000000 0.000000</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ </inertial>
+ <visual name='vis_lower_joint'>
+ <pose>0.000000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.080000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_lower_joint'>
+ <pose>0.000000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.080000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <joint name='upper_joint' type='revolute'>
+ <parent>base</parent>
+ <child>upper_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ </limit>
+ </axis>
+ </joint>
+ <joint name='lower_joint' type='revolute'>
+ <parent>upper_link</parent>
+ <child>lower_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ </limit>
+ </axis>
+ </joint>
+ <pose>0.000000 2.100000 0.000000 0.000000 0.000000 0.000000</pose>
+ <static>0</static>
+ </model>
+ <model name='pendulum_180deg'>
+ <link name='base'>
+ <inertial>
+ <mass>100.000000</mass>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ </inertial>
+ <visual name='vis_plate_on_ground'>
+ <pose>0.000000 0.000000 0.050000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.800000</radius>
+ <length>0.100000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_pole'>
+ <pose>-0.275000 0.000000 1.100000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.200000 2.200000</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_plate_on_ground'>
+ <pose>0.000000 0.000000 0.050000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.800000</radius>
+ <length>0.100000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_pole'>
+ <pose>-0.275000 0.000000 1.100000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.200000 2.200000</size>
+ </box>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <link name='upper_link'>
+ <pose>0.000000 0.000000 2.100000 -1.570800 0.000000 0.000000</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ </inertial>
+ <visual name='vis_upper_joint'>
+ <pose>-0.050000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_lower_joint'>
+ <pose>0.000000 0.000000 1.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.200000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_upper_joint'>
+ <pose>-0.050000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_lower_joint'>
+ <pose>0.000000 0.000000 1.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.200000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <link name='lower_link'>
+ <pose>0.250000 1.000000 2.100000 -2.000000 0.000000 0.000000</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ </inertial>
+ <visual name='vis_lower_joint'>
+ <pose>0.000000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.080000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_lower_joint'>
+ <pose>0.000000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.080000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <joint name='upper_joint' type='revolute'>
+ <parent>base</parent>
+ <child>upper_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ </limit>
+ </axis>
+ </joint>
+ <joint name='lower_joint' type='revolute'>
+ <parent>upper_link</parent>
+ <child>lower_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ </limit>
+ </axis>
+ </joint>
+ <pose>0.000000 -2.100000 0.000000 0.000000 0.000000 -3.141585</pose>
+ <static>0</static>
+ </model>
+ <model name='pendulum_90deg'>
+ <link name='base'>
+ <inertial>
+ <mass>100.000000</mass>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ </inertial>
+ <visual name='vis_plate_on_ground'>
+ <pose>0.000000 0.000000 0.050000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.800000</radius>
+ <length>0.100000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_pole'>
+ <pose>-0.275000 0.000000 1.100000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.200000 2.200000</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_plate_on_ground'>
+ <pose>0.000000 0.000000 0.050000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.800000</radius>
+ <length>0.100000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_pole'>
+ <pose>-0.275000 0.000000 1.100000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.200000 2.200000</size>
+ </box>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <link name='upper_link'>
+ <pose>0.000000 0.000000 2.100000 -1.570800 0.000000 0.000000</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ </inertial>
+ <visual name='vis_upper_joint'>
+ <pose>-0.050000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_lower_joint'>
+ <pose>0.000000 0.000000 1.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.200000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_upper_joint'>
+ <pose>-0.050000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_lower_joint'>
+ <pose>0.000000 0.000000 1.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.200000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <link name='lower_link'>
+ <pose>0.250000 1.000000 2.100000 -2.000000 0.000000 0.000000</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ </inertial>
+ <visual name='vis_lower_joint'>
+ <pose>0.000000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.080000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_lower_joint'>
+ <pose>0.000000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.080000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <joint name='upper_joint' type='revolute'>
+ <parent>base</parent>
+ <child>upper_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ </limit>
+ </axis>
+ </joint>
+ <joint name='lower_joint' type='revolute'>
+ <parent>upper_link</parent>
+ <child>lower_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ </limit>
+ </axis>
+ </joint>
+ <pose>-2.100000 0.000000 0.000000 0.000000 0.000000 1.570800</pose>
+ <static>0</static>
+ </model>
+ <model name='pendulum_270deg'>
+ <link name='base'>
+ <inertial>
+ <mass>100.000000</mass>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ </inertial>
+ <visual name='vis_plate_on_ground'>
+ <pose>0.000000 0.000000 0.050000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.800000</radius>
+ <length>0.100000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_pole'>
+ <pose>-0.275000 0.000000 1.100000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.200000 2.200000</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_plate_on_ground'>
+ <pose>0.000000 0.000000 0.050000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.800000</radius>
+ <length>0.100000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_pole'>
+ <pose>-0.275000 0.000000 1.100000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.200000 2.200000</size>
+ </box>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <link name='upper_link'>
+ <pose>0.000000 0.000000 2.100000 -1.570800 0.000000 0.000000</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ </inertial>
+ <visual name='vis_upper_joint'>
+ <pose>-0.050000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_lower_joint'>
+ <pose>0.000000 0.000000 1.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.200000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_upper_joint'>
+ <pose>-0.050000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_lower_joint'>
+ <pose>0.000000 0.000000 1.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.200000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <link name='lower_link'>
+ <pose>0.250000 1.000000 2.100000 -2.000000 0.000000 0.000000</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ </inertial>
+ <visual name='vis_lower_joint'>
+ <pose>0.000000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.080000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_lower_joint'>
+ <pose>0.000000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.080000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <joint name='upper_joint' type='revolute'>
+ <parent>base</parent>
+ <child>upper_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ </limit>
+ </axis>
+ </joint>
+ <joint name='lower_joint' type='revolute'>
+ <parent>upper_link</parent>
+ <child>lower_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ </limit>
+ </axis>
+ </joint>
+ <pose>2.100000 0.000000 0.000000 0.000000 0.000000 -1.570800</pose>
+ <static>0</static>
+ </model>
+ <model name='pendulum_315deg'>
+ <link name='base'>
+ <inertial>
+ <mass>100.000000</mass>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ </inertial>
+ <visual name='vis_plate_on_ground'>
+ <pose>0.000000 0.000000 0.050000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.800000</radius>
+ <length>0.100000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_pole'>
+ <pose>-0.275000 0.000000 1.100000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.200000 2.200000</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_plate_on_ground'>
+ <pose>0.000000 0.000000 0.050000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.800000</radius>
+ <length>0.100000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_pole'>
+ <pose>-0.275000 0.000000 1.100000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.200000 2.200000</size>
+ </box>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <link name='upper_link'>
+ <pose>0.000000 0.000000 2.100000 -1.570800 0.000000 0.000000</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ </inertial>
+ <visual name='vis_upper_joint'>
+ <pose>-0.050000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_lower_joint'>
+ <pose>0.000000 0.000000 1.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.200000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_upper_joint'>
+ <pose>-0.050000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_lower_joint'>
+ <pose>0.000000 0.000000 1.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.200000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <link name='lower_link'>
+ <pose>0.250000 1.000000 2.100000 -2.000000 0.000000 0.000000</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ </inertial>
+ <visual name='vis_lower_joint'>
+ <pose>0.000000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.080000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_lower_joint'>
+ <pose>0.000000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.080000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <joint name='upper_joint' type='revolute'>
+ <parent>base</parent>
+ <child>upper_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ </limit>
+ </axis>
+ </joint>
+ <joint name='lower_joint' type='revolute'>
+ <parent>upper_link</parent>
+ <child>lower_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ </limit>
+ </axis>
+ </joint>
+ <pose>1.480000 1.480000 0.000000 0.000000 0.000000 -0.785400</pose>
+ <static>0</static>
+ </model>
+ <model name='pendulum_225deg'>
+ <link name='base'>
+ <inertial>
+ <mass>100.000000</mass>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ </inertial>
+ <visual name='vis_plate_on_ground'>
+ <pose>0.000000 0.000000 0.050000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.800000</radius>
+ <length>0.100000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_pole'>
+ <pose>-0.275000 0.000000 1.100000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.200000 2.200000</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_plate_on_ground'>
+ <pose>0.000000 0.000000 0.050000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.800000</radius>
+ <length>0.100000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_pole'>
+ <pose>-0.275000 0.000000 1.100000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.200000 2.200000</size>
+ </box>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <link name='upper_link'>
+ <pose>0.000000 0.000000 2.100000 -1.570800 0.000000 0.000000</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ </inertial>
+ <visual name='vis_upper_joint'>
+ <pose>-0.050000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_lower_joint'>
+ <pose>0.000000 0.000000 1.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.200000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_upper_joint'>
+ <pose>-0.050000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_lower_joint'>
+ <pose>0.000000 0.000000 1.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.200000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <link name='lower_link'>
+ <pose>0.250000 1.000000 2.100000 -2.000000 0.000000 0.000000</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ </inertial>
+ <visual name='vis_lower_joint'>
+ <pose>0.000000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.080000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_lower_joint'>
+ <pose>0.000000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.080000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <joint name='upper_joint' type='revolute'>
+ <parent>base</parent>
+ <child>upper_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ </limit>
+ </axis>
+ </joint>
+ <joint name='lower_joint' type='revolute'>
+ <parent>upper_link</parent>
+ <child>lower_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ </limit>
+ </axis>
+ </joint>
+ <pose>1.480000 -1.480000 0.000000 0.000000 0.000000 -2.356200</pose>
+ <static>0</static>
+ </model>
+ <model name='pendulum_135deg'>
+ <link name='base'>
+ <inertial>
+ <mass>100.000000</mass>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ </inertial>
+ <visual name='vis_plate_on_ground'>
+ <pose>0.000000 0.000000 0.050000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.800000</radius>
+ <length>0.100000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_pole'>
+ <pose>-0.275000 0.000000 1.100000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.200000 2.200000</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_plate_on_ground'>
+ <pose>0.000000 0.000000 0.050000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.800000</radius>
+ <length>0.100000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_pole'>
+ <pose>-0.275000 0.000000 1.100000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.200000 2.200000</size>
+ </box>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <link name='upper_link'>
+ <pose>0.000000 0.000000 2.100000 -1.570800 0.000000 0.000000</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ </inertial>
+ <visual name='vis_upper_joint'>
+ <pose>-0.050000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_lower_joint'>
+ <pose>0.000000 0.000000 1.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.200000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_upper_joint'>
+ <pose>-0.050000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_lower_joint'>
+ <pose>0.000000 0.000000 1.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.200000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <link name='lower_link'>
+ <pose>0.250000 1.000000 2.100000 -2.000000 0.000000 0.000000</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ </inertial>
+ <visual name='vis_lower_joint'>
+ <pose>0.000000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.080000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_lower_joint'>
+ <pose>0.000000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.080000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <joint name='upper_joint' type='revolute'>
+ <parent>base</parent>
+ <child>upper_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ </limit>
+ </axis>
+ </joint>
+ <joint name='lower_joint' type='revolute'>
+ <parent>upper_link</parent>
+ <child>lower_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ </limit>
+ </axis>
+ </joint>
+ <pose>-1.480000 -1.480000 0.000000 0.000000 0.000000 2.356200</pose>
+ <static>0</static>
+ </model>
+ <model name='pendulum_45deg'>
+ <link name='base'>
+ <inertial>
+ <mass>100.000000</mass>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ </inertial>
+ <visual name='vis_plate_on_ground'>
+ <pose>0.000000 0.000000 0.050000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.800000</radius>
+ <length>0.100000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_pole'>
+ <pose>-0.275000 0.000000 1.100000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.200000 2.200000</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_plate_on_ground'>
+ <pose>0.000000 0.000000 0.050000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.800000</radius>
+ <length>0.100000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_pole'>
+ <pose>-0.275000 0.000000 1.100000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.200000 0.200000 2.200000</size>
+ </box>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <link name='upper_link'>
+ <pose>0.000000 0.000000 2.100000 -1.570800 0.000000 0.000000</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ </inertial>
+ <visual name='vis_upper_joint'>
+ <pose>-0.050000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_lower_joint'>
+ <pose>0.000000 0.000000 1.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.200000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_upper_joint'>
+ <pose>-0.050000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_lower_joint'>
+ <pose>0.000000 0.000000 1.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.200000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <link name='lower_link'>
+ <pose>0.250000 1.000000 2.100000 -2.000000 0.000000 0.000000</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <inertia>
+ <ixx>1.000000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>1.000000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>1.000000</izz>
+ </inertia>
+ <mass>1.000000</mass>
+ </inertial>
+ <visual name='vis_lower_joint'>
+ <pose>0.000000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.080000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name='vis_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='col_lower_joint'>
+ <pose>0.000000 0.000000 0.000000 -3.141592 1.570793 -3.141592</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.080000</radius>
+ <length>0.300000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <collision name='col_cylinder'>
+ <pose>0.000000 0.000000 0.500000 0.000000 0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.900000</length>
+ </cylinder>
+ </geometry>
+ <surface>
+ <bounce/>
+ <friction>
+ <ode/>
+ </friction>
+ <contact>
+ <ode/>
+ </contact>
+ </surface>
+ </collision>
+ <velocity_decay>
+ <linear>0.000000</linear>
+ <angular>0.000000</angular>
+ </velocity_decay>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <joint name='upper_joint' type='revolute'>
+ <parent>base</parent>
+ <child>upper_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ </limit>
+ </axis>
+ </joint>
+ <joint name='lower_joint' type='revolute'>
+ <parent>upper_link</parent>
+ <child>lower_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ </limit>
+ </axis>
+ </joint>
+ <pose>-1.480000 1.480000 0.000000 0.000000 0.000000 0.785400</pose>
+ <static>0</static>
+ </model>
+ <scene>
+ <ambient>0.200000 0.200000 0.200000 1.000000</ambient>
+ <background>0.700000 0.700000 0.700000 1.000000</background>
+ <shadows>1</shadows>
+ </scene>
+ <state world_name='default'>
+ <sim_time>0 0</sim_time>
+ <real_time>0 0</real_time>
+ <wall_time>1362959995 635676850</wall_time>
+ </state>
+ <gui fullscreen='0'>
+ <camera name='user_camera'>
+ <pose>5.000000 -5.000000 2.000000 0.000000 0.275643 2.356190</pose>
+ <view_controller>orbit</view_controller>
+ </camera>
+ </gui>
+ </world>
+</sdf>
diff --git a/Simbody/examples/InstalledCMakeLists.txt b/Simbody/examples/InstalledCMakeLists.txt
new file mode 100644
index 0000000..d6c488c
--- /dev/null
+++ b/Simbody/examples/InstalledCMakeLists.txt
@@ -0,0 +1,114 @@
+# Generate examples.
+#
+# This is boilerplate code for generating a set of executables, one per
+# .cpp file in an "examples" subdirectory. These are intended to
+# be installed with the library but we don't handle installation
+# here. Unlike the similar boilerplate code in the "tests"
+# directory (but like the "tests/adhoc" boilerplate), this does
+# not generate CMake ADD_TEST commands so these will never run
+# automatically.
+#
+# For IDEs that can deal with PROJECT_LABEL properties (at least
+# Visual Studio) the projects for building each of these adhoc
+# executables will be labeled "Example - TheExampleName" if a file
+# TheExampleName.cpp is found in this directory.
+#
+# We check the BUILD_TESTS_AND_EXAMPLES_{SHARED,STATIC} variables to determine
+# whether to build dynamically linked, statically linked, or both
+# versions of the executable.
+
+CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
+
+PROJECT(SimTKExamples)
+
+SET(BUILD_TESTS_AND_EXAMPLES_SHARED TRUE CACHE BOOL
+ "Build examples using dynamic SimTK libraries.")
+SET(BUILD_TESTS_AND_EXAMPLES_STATIC FALSE CACHE BOOL
+ "Build examples using static SimTK libraries.")
+
+SET(SimTK_SHARED_LIBS
+ optimized SimTKsimbody
+ optimized SimTKmath
+ optimized SimTKcommon)
+SET(SimTK_SHARED_LIBS_D
+ debug SimTKsimbody_d
+ debug SimTKmath_d
+ debug SimTKcommon_d)
+
+SET(SimTK_STATIC_LIBS
+ optimized SimTKsimbody_static
+ optimized SimTKmath_static
+ optimized SimTKcommon_static)
+SET(SimTK_STATIC_LIBS_D
+ debug SimTKsimbody_static_d
+ debug SimTKmath_static_d
+ debug SimTKcommon_static_d)
+
+
+# These extra libraries are only available as a shared, optimized.
+IF(WIN32)
+ SET(SimTK_GENERAL_LIBS liblapack libblas pthreadVC2)
+ELSEIF(APPLE)
+ELSE() #Linux
+ SET(SimTK_GENERAL_LIBS dl rt)
+ENDIF()
+
+SET(SimTK $ENV{SimTK_INSTALL_DIR} CACHE PATH
+ "Directory where SimTK is installed, e.g. /usr/local/SimTK.")
+
+IF(NOT SimTK)
+ MESSAGE(FATAL_ERROR
+"Expected SimTK_INSTALL_DIR environment var to be set, or set SimTK in CMake")
+ENDIF(NOT SimTK)
+
+INCLUDE_DIRECTORIES(${SimTK}/include)
+LINK_DIRECTORIES(${SimTK}/lib)
+
+# On Mac, build 32 bit binaries.
+IF(APPLE)
+ SET(CMAKE_OSX_ARCHITECTURES "i386" CACHE STRING "The processor architectures to build for" FORCE)
+ENDIF(APPLE)
+
+FILE(GLOB EXAMPLES "*.cpp")
+FOREACH(EX_PROG ${EXAMPLES})
+ GET_FILENAME_COMPONENT(EX_ROOT ${EX_PROG} NAME_WE)
+
+ IF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+ # Link with shared library
+ ADD_EXECUTABLE(${EX_ROOT} ${EX_PROG})
+ SET_TARGET_PROPERTIES(${EX_ROOT}
+ PROPERTIES
+ PROJECT_LABEL "Example - ${EX_ROOT}")
+ TARGET_LINK_LIBRARIES(${EX_ROOT}
+ ${SimTK_SHARED_LIBS_D}
+ ${SimTK_SHARED_LIBS}
+ ${SimTK_GENERAL_LIBS})
+ ENDIF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+
+ IF (BUILD_TESTS_AND_EXAMPLES_STATIC)
+ # Link with static library
+ SET(EX_STATIC ${EX_ROOT}Static)
+ ADD_EXECUTABLE(${EX_STATIC} ${EX_PROG})
+ SET_TARGET_PROPERTIES(${EX_STATIC}
+ PROPERTIES
+ COMPILE_FLAGS "-DSimTK_USE_STATIC_LIBRARIES"
+ PROJECT_LABEL "Example - ${EX_STATIC}")
+ TARGET_LINK_LIBRARIES(${EX_STATIC}
+ ${SimTK_STATIC_LIBS_D}
+ ${SimTK_STATIC_LIBS}
+ ${SimTK_GENERAL_LIBS})
+ ENDIF (BUILD_TESTS_AND_EXAMPLES_STATIC)
+
+ENDFOREACH(EX_PROG ${EXAMPLES})
+
+# If there are any .obj (geometry) or .pdb (protein data bank)
+# files, copy those to the binary directory which will likely be
+# the current working directory for the examples when run.
+# Also copy Gazebo .sdf files.
+FILE(GLOB EXAMPLE_OBJS "*.obj" "*.pdb" "*.sdf")
+FOREACH(EX_OBJ ${EXAMPLE_OBJS})
+ GET_FILENAME_COMPONENT(OBJ_ROOT ${EX_OBJ} NAME)
+ CONFIGURE_FILE(${EX_OBJ}
+ ${CMAKE_CURRENT_BINARY_DIR}/${OBJ_ROOT} COPYONLY)
+ENDFOREACH(EX_OBJ ${EXAMPLE_OBJS})
+
diff --git a/Simbody/examples/JaredsDude.cpp b/Simbody/examples/JaredsDude.cpp
new file mode 100644
index 0000000..29c9e57
--- /dev/null
+++ b/Simbody/examples/JaredsDude.cpp
@@ -0,0 +1,580 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Jared's Dude *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Jared Duke *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+// -----------------------------------------------------------------------------
+// An attempt at duplicating Jared Duke's simulation for studying performance.
+// -----------------------------------------------------------------------------
+#include "Simbody.h"
+
+#include <utility>
+#include <map>
+
+using namespace SimTK;
+
+class MyFrameController : public Visualizer::FrameController {
+public:
+ MyFrameController(const SimbodyMatterSubsystem& matter,
+ MobilizedBodyIndex whichBody,
+ const Force::Gravity& gravity)
+ : m_matter(matter), m_whichBody(whichBody), m_gravity(gravity) {}
+
+ virtual void generateControls(const Visualizer& viz,
+ const State& state,
+ Array_<DecorativeGeometry>& geometry)
+ {
+ const MobilizedBody& mobod = m_matter.getMobilizedBody(m_whichBody);
+ const Transform& X_GB = mobod.getBodyTransform(state);
+ const UnitVec3& downDir = m_gravity.getDownDirection(state);
+ Vec3 cameraPos(X_GB.p()[0], X_GB.p()[1], 10);
+ UnitVec3 cameraZ(0,0,1);
+ //viz.setCameraTransform(Transform(Rotation(cameraZ, ZAxis, Vec3(0,1,0), YAxis), cameraPos));
+
+ geometry.push_back(DecorativeLine(Vec3(1,4,0), Vec3(1,4,0)+downDir)
+ .setColor(Green).setLineThickness(3).setBodyId(0));
+ }
+
+private:
+ const SimbodyMatterSubsystem& m_matter;
+ const MobilizedBodyIndex m_whichBody;
+ const Force::Gravity& m_gravity;
+};
+
+// Check for user input. If there has been some, process it.
+class UserInputHandler : public PeriodicEventHandler {
+public:
+ UserInputHandler(Visualizer::InputSilo& silo, const Force::Gravity& gravity, Real interval)
+ : PeriodicEventHandler(interval), m_silo(silo), m_gravity(gravity) {}
+
+ virtual void handleEvent(State& state, Real accuracy,
+ bool& shouldTerminate) const
+ {
+ unsigned key, modifiers;
+ if (!m_silo.takeKeyHit(key, modifiers))
+ return;
+
+ if (key == Visualizer::InputListener::KeyEsc) {
+ printf("User hit ESC!!\n");
+ shouldTerminate = true;
+ return;
+ }
+
+ const Real KeyFactor = 0.05;
+
+ const UnitVec3& down = m_gravity.getDownDirection(state);
+ bool control = (modifiers & Visualizer::InputListener::ControlIsDown) != 0;
+ bool gotOne = false;
+ switch(key) {
+ case Visualizer::InputListener::KeyLeftArrow:
+ m_gravity.setDownDirection(state, down + KeyFactor*Vec3(-1,0,0));
+ gotOne = true;
+ break;
+ case Visualizer::InputListener::KeyRightArrow:
+ m_gravity.setDownDirection(state, down + KeyFactor*Vec3(1,0,0));
+ gotOne = true;
+ break;
+ case Visualizer::InputListener::KeyUpArrow:
+ m_gravity.setDownDirection(state, down + KeyFactor*Vec3(0,1,0));
+ gotOne = true;
+ break;
+ case Visualizer::InputListener::KeyDownArrow:
+ m_gravity.setDownDirection(state, down + KeyFactor*Vec3(0,-1,0));
+ gotOne = true;
+ break;
+ case Visualizer::InputListener::KeyPageUp:
+ m_gravity.setDownDirection(state, down + KeyFactor*Vec3(0,0,-1));
+ gotOne = true;
+ break;
+ case Visualizer::InputListener::KeyPageDown:
+ m_gravity.setDownDirection(state, down + KeyFactor*Vec3(0,0,1));
+ gotOne = true;
+ break;
+ }
+ if (gotOne)
+ std::cout << "New gravity down=" << m_gravity.getDownDirection(state) << std::endl;
+
+ }
+
+private:
+ Visualizer::InputSilo& m_silo;
+ const Force::Gravity& m_gravity;
+};
+
+//////////////////////////////////////////////////////////////////////////
+
+class TwoPointMuscleDamperReflex : public Force::Custom::Implementation {
+public:
+
+ TwoPointMuscleDamperReflex(const MobilizedBody& body1,
+ const Vec3& station1,
+ const MobilizedBody& body2,
+ const Vec3& station2,
+ Real k,
+ Real d,
+ Real x0);
+
+ virtual bool dependsOnlyOnPositions() const {
+ return false;
+ }
+
+ virtual void calcForce(const State& state,
+ Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces,
+ Vector& mobilityForces) const;
+
+ virtual Real calcPotentialEnergy(const State& state) const;
+
+ void addDecorativeLine( DecorationSubsystem& viz,
+ const DecorativeLine& line,
+ Real scale1=1.0, Real scale2=1.0) const {
+ viz.addRubberBandLine(mBody1, mStation1*scale1, mBody2, mStation2*scale2, line);
+ }
+
+ Vec3 getStation1( ) const { return mStation1; }
+ Vec3 getStation2( ) const { return mStation2; }
+ void setStation1(const Vec3& station1) { mStation1 = station1; }
+ void setStation2(const Vec3& station2) { mStation2 = station2; }
+
+ Real getK() const { return mK; }
+ void setK(Real k) { mK = k; }
+ Real getDamping() const { return mDamping; }
+ void setDamping(Real damping) { mDamping = damping; }
+ Real getX0() const { return mX0; }
+ void setX0(Real x0) { mX0 = x0; }
+
+ private:
+ const MobilizedBody& mBody1;
+ const MobilizedBody& mBody2;
+ Vec3 mStation1, mStation2;
+ Real mK, mDamping, mX0;
+};
+
+class Dude {
+public:
+ enum Side {Left=0,Right,Only};
+ enum BodyType {Foot=0,Shank,Thigh,Pelvis,Torso};
+ enum Segment {FootFront=0, FootToes, FootHeel,
+ ShankLower, ShankMidLow, ShankMidUp, ShankUpper,
+ ThighLower, ThighWhole,
+ PelvisFront, PelvisBack};
+ enum Muscle {Reflex11,Reflex12,Reflex21,Reflex22};
+ typedef std::pair<BodyType,Side> UniqueBody;
+ typedef std::pair<Muscle,Side> UniqueMuscle;
+
+ static const int NBodyType = Torso-Foot+1;
+ static const int NSegment = PelvisBack-FootFront+1;
+ static const int NMuscle = Reflex22-Reflex11+1;
+
+ Dude(Real scale);
+
+ void loadDefaultState(State& state);
+
+ void scaleBy(Real scale) {
+ m_mass *= scale; m_length *= scale;
+ m_segment *= scale;
+ for (int i=0; i < NMuscle; ++i) {
+ m_springW[i][2] *= scale; // last spring parameter only
+ m_springR[i][2] *= scale;
+ }
+ std::cout << "Masses=" << m_mass << std::endl;
+ std::cout << "Lengths=" << m_length << std::endl;
+ }
+
+ MultibodySystem m_system;
+ SimbodyMatterSubsystem m_matter;
+ GeneralForceSubsystem m_forces;
+ ContactTrackerSubsystem m_tracker;
+ CompliantContactSubsystem m_contactForces;
+ DecorationSubsystem m_viz;
+ Force::Gravity m_gravity;
+
+ Vector m_mass, m_length; // index by BodyType
+ Vector m_segment; // index by Segment
+ Vector_<Vec3> m_springW, m_springR; // index by spring #
+
+ std::map<BodyType, Body> m_body;
+ std::map<UniqueBody, MobilizedBody> m_mobod;
+ std::map<UniqueMuscle,TwoPointMuscleDamperReflex*> m_muscles;
+private:
+ static Real massData[NBodyType], lengthData[NBodyType];
+ static Vec3 springWData[NMuscle], springRData[NMuscle];
+ static Real segmentData[NSegment];
+};
+
+
+//////////////////////////////////////////////////////////////////////////
+int main() {
+ try {
+ const Real FrameRate = 30;
+ const Real TimeScale = 1;
+ const Real scale = 10.;
+
+ Dude dude(scale);
+
+ MultibodySystem& system = dude.m_system;
+
+ Visualizer viz(system);
+
+ printf("\n\n***************************************************************\n");
+ printf( "use arrow keys and page up/down to control green gravity vector\n");
+ printf( "***************************************************************\n\n");
+
+ // This menu does nothing.
+ Array_< std::pair<std::string,int> > items;
+ items.push_back(std::make_pair("One", 1));
+ items.push_back(std::make_pair("Top/SubA/first", 2));
+ items.push_back(std::make_pair("Top/SubA/second", 3));
+ items.push_back(std::make_pair("Top/SubB/only", 4));
+ items.push_back(std::make_pair("Two", 5));
+ viz.addMenu("Test Menu", 1, items);
+
+
+ // This is for per-frame camera control and single-frame geometry.
+ viz.addFrameController(new MyFrameController(dude.m_matter,
+ MobilizedBodyIndex(1), dude.m_gravity));
+
+ viz.setRealTimeScale(TimeScale);
+ //viz.setDesiredBufferLengthInSec(.15);
+ viz.setDesiredFrameRate(FrameRate);
+ //viz.setMode(Visualizer::Sampling);
+ viz.setMode(Visualizer::RealTime);
+
+ // Use this for communication of user input from the GUI to the simulation.
+ // Both the Visualizer and the simulation must know about it.
+ Visualizer::InputSilo* silo = new Visualizer::InputSilo();
+ viz.addInputListener(silo);
+
+ system.addEventHandler(
+ new UserInputHandler(*silo, dude.m_gravity, 0.1)); // 100ms
+ system.addEventReporter(new Visualizer::Reporter(viz, TimeScale/FrameRate));
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ dude.loadDefaultState(state);
+
+ Assembler(system).assemble(state);
+
+ // Simulate it.
+
+
+ //RungeKutta3Integrator integ(system);
+ RungeKuttaMersonIntegrator integ(system);
+ //RungeKuttaFeldbergIntegrator integ(system);
+ //CPodesIntegrator integ(system);
+ integ.setAccuracy(.01);
+ //integ.setAccuracy(1e-2);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+
+ double cpuStart = cpuTime();
+ double realStart = realTime();
+ ts.stepTo(Infinity);
+ std::cout << "cpu time: "<<cpuTime()-cpuStart<< std::endl;
+ std::cout << "real time: "<<realTime()-realStart<< std::endl;
+ std::cout << "steps: "<<integ.getNumStepsTaken()<< std::endl;
+ viz.dumpStats(std::cout);
+
+ std::cout << "Type something to quit: ";
+ char ch; std::cin >> ch;
+
+ } catch (const std::exception& exc) {
+ std::cout << "EXCEPTION: " << exc.what() << std::endl;
+ }
+}
+
+Real Dude::massData[] = {.15, .15, .15, .5, .05};
+Real Dude::lengthData[] = {.06, .1, .1, .1, .1};
+Real Dude::segmentData[] = {lengthData[Foot]*2./3,
+ lengthData[Foot]*1./3,
+ lengthData[Foot]*1./3,
+ lengthData[Shank]*1./3,
+ lengthData[Shank]*1./3,
+ lengthData[Shank]*1./3,
+ lengthData[Shank]*1./5,
+ lengthData[Thigh]*3./10,
+ lengthData[Thigh],
+ lengthData[Pelvis]*1./2,
+ lengthData[Pelvis]*1./2};
+static const Real d = 10; // more damping
+Vec3 Dude::springWData[] = {Vec3(2000,d*10,.122),
+ Vec3(3000,d*10,.045),
+ Vec3(7000,d*10,.133),
+ Vec3(4000,d*10,.065)};
+Vec3 Dude::springRData[] = {Vec3(12000,10,.122),
+ Vec3(8000,10,.045),
+ Vec3(20000,10,.133),
+ Vec3(18000,10,.065)};
+
+Dude::Dude(Real scale)
+: m_matter(m_system), m_forces(m_system), m_tracker(m_system),
+ m_contactForces(m_system, m_tracker), m_viz(m_system),
+ m_gravity(m_forces, m_matter, -YAxis, 9.81),
+ m_mass(NBodyType,massData), m_length(NBodyType,lengthData),
+ m_springW(NMuscle,springWData), m_springR(NMuscle,springRData),
+ m_segment(NSegment,segmentData)
+{
+ const Real opacity = .3;
+ const Real resolution = 3;
+
+ scaleBy(scale);
+ //Force::GlobalDamper(m_forces, m_matter, 100); // fall slowly
+
+ const Real footHeight = m_length[Foot] *.1; // half dimensions
+ const Real footWidth = m_length[Foot] *.3;
+ const Real boneRad = m_length[Thigh] *.1;
+ const Real contactRad = /*m_length[Foot] *.05*/footHeight;
+
+ const Real transitionVelocity = .1;
+ m_contactForces.setTransitionVelocity(transitionVelocity);
+
+ // Create bodies
+ m_body[Pelvis] = Body::Rigid(MassProperties(m_mass[Pelvis]*1.5,Vec3(0),
+ Inertia(1)));
+ //Inertia::cylinderAlongY(boneRad,m_length[Pelvis]/2)*m_mass[Pelvis]*.8)));
+
+ m_body[Torso] = Body::Rigid(MassProperties(m_mass[Torso], Vec3(0),
+ m_mass[Torso]*Inertia::cylinderAlongY(boneRad,m_length[Torso]/2)));
+ m_body[Thigh] = Body::Rigid(MassProperties(m_mass[Thigh], Vec3(0),
+ m_mass[Thigh]*Inertia::cylinderAlongY(boneRad,m_length[Thigh]/2)));
+ m_body[Shank] = Body::Rigid(MassProperties(m_mass[Shank], Vec3(0),
+ m_mass[Shank]*Inertia::cylinderAlongY(boneRad,m_length[Shank]/2)));
+ m_body[Foot] = Body::Rigid(MassProperties(m_mass[Foot], Vec3(0),
+ m_mass[Foot]*Inertia::cylinderAlongY(boneRad,m_length[Foot]/2)));
+
+ // Add DecorativeGeometry to the bodies.
+ m_body[Pelvis].addDecoration(Transform(),
+ DecorativeBrick(Vec3(m_length[Pelvis]/5, footHeight, footWidth))
+ .setColor(Red).setOpacity(opacity));
+ m_body[Torso].addDecoration(Transform(),
+ DecorativeCylinder(boneRad, m_length[Torso]/2)
+ .setColor(Blue).setOpacity(opacity).setResolution(resolution));
+ m_body[Thigh].addDecoration(Transform(),
+ DecorativeCylinder(boneRad, m_length[Thigh]/2)
+ .setColor(Red).setOpacity(opacity).setResolution(resolution));
+ m_body[Shank].addDecoration(Transform(),
+ DecorativeCylinder(boneRad, m_length[Shank]/2)
+ .setColor(Blue).setOpacity(opacity).setResolution(resolution));
+ m_body[Foot].addDecoration(Transform(),
+ DecorativeBrick(Vec3(footHeight, m_length[Foot]/2, footWidth))
+ .setColor(Red).setOpacity(opacity));
+
+ // Add ContactSurfaces to the feet. Since surfaces on the same body can't collide
+ // anyway, the clique membership here ensures that the feet can't contact with
+ // each other.
+ ContactCliqueId clique1 = ContactSurface::createNewContactClique();
+
+ ContactMaterial material(0.02*1e7, // stiffness
+ 0.9, // dissipation
+ 0.8, // mu_static
+ 0.6, // mu_dynamic
+ 1); // mu_viscous
+
+ for (int fb=-1; fb <= 1; fb += 2)
+ for (int lr=-1; lr <= 1; lr += 2) {
+ const Vec3 ctr(0, fb*(m_length[Foot]/2-contactRad), lr*(footWidth-contactRad));
+ m_body[Foot].addContactSurface(ctr,
+ ContactSurface(ContactGeometry::Sphere(contactRad),material)
+ .joinClique(clique1));
+ // Visualize the contact sphere
+ m_body[Foot].addDecoration(ctr,
+ DecorativeSphere(contactRad).setColor(Green));
+ }
+
+ // Half space normal is -x; must rotate to make it +y.
+ m_matter.Ground().updBody().addContactSurface(Rotation(-Pi/2,ZAxis),
+ ContactSurface(ContactGeometry::HalfSpace(), material));
+
+ // Now create the MobilizedBodies (bodies + joints).
+ m_mobod[UniqueBody(Pelvis,Only)] =
+ MobilizedBody::Free(m_matter.Ground(), m_body[Pelvis]);
+ // What about Torso?
+
+ // Add left and right legs.
+ for (Side side=Left; side <= Right; side = Side(side+1)) {
+ m_mobod[UniqueBody(Thigh,side)] =
+ MobilizedBody::Pin( m_mobod[UniqueBody(Pelvis,Only)],
+ Vec3(0,0,side==Left?-footWidth:footWidth),
+ m_body[Thigh],
+ Transform(Vec3(0, m_length[Thigh]/2, 0)));
+
+ m_mobod[UniqueBody(Shank,side)] =
+ MobilizedBody::Pin( m_mobod[UniqueBody(Thigh,side)],
+ Transform(Vec3(0, -m_length[Thigh]/2, 0)),
+ m_body[Shank],
+ Transform(Vec3(0, m_length[Shank]*.5, 0.0)));
+
+ m_mobod[UniqueBody(Foot,side)] =
+ MobilizedBody::Pin( m_mobod[UniqueBody(Shank,side)],
+ Transform(Vec3(0, -m_length[Shank]/2, 0)),
+ m_body[Foot],
+ Transform(Vec3(0, m_length[Foot]/2-m_segment[FootHeel], 0)));
+ }
+
+ DecorativeLine baseLine;
+ baseLine.setColor(Red).setLineThickness(4).setOpacity(.2);
+
+ // Add left and right leg muscles
+ for (Side side=Left; side <= Right; side = Side(side+1)) {
+ TwoPointMuscleDamperReflex* reflex;
+
+ reflex = new TwoPointMuscleDamperReflex(
+ m_mobod[UniqueBody(Pelvis,Only)],
+ Vec3(-m_segment[PelvisBack], 0, 0),
+ m_mobod[UniqueBody(Shank,side)],
+ Vec3(0,m_length[Shank]/2-m_segment[ShankMidUp],0),
+ m_springW[0][0], m_springW[0][1], m_springW[0][2] );
+ reflex->addDecorativeLine(m_viz, baseLine, .25, 1.0),
+ m_muscles[UniqueMuscle(Reflex11,side)] = reflex;
+ Force::Custom(m_forces, reflex);
+
+ reflex = new TwoPointMuscleDamperReflex(
+ m_mobod[UniqueBody(Shank,side)],
+ Vec3(0,-m_length[Shank]/2+m_segment[ShankLower],0),
+ m_mobod[UniqueBody(Foot,side)],
+ Vec3(0,-m_length[Foot]/2+m_segment[FootToes], 0),
+ m_springW[1][0], m_springW[1][1], m_springW[1][2] );
+ reflex->addDecorativeLine(m_viz, baseLine);
+ m_muscles[UniqueMuscle(Reflex12,side)] = reflex;
+ Force::Custom(m_forces, reflex);
+
+ reflex = new TwoPointMuscleDamperReflex(
+ m_mobod[UniqueBody(Thigh,side)],
+ Vec3(0, -m_length[Thigh]/2+m_segment[ThighLower], 0),
+ m_mobod[UniqueBody(Foot,side)],
+ Vec3(0,m_length[Foot]/2, 0), /// @todo double check this
+ m_springW[2][0], m_springW[2][1], m_springW[2][2] );
+ reflex->addDecorativeLine(m_viz, baseLine);
+ m_muscles[UniqueMuscle(Reflex21,side)] = reflex;
+ Force::Custom(m_forces, reflex);
+
+ reflex = new TwoPointMuscleDamperReflex(
+ m_mobod[UniqueBody(Pelvis,Only)],
+ Vec3(m_segment[PelvisFront], 0, 0),
+ m_mobod[UniqueBody(Shank,side)],
+ Vec3(0, m_length[Shank]/2+m_segment[ShankUpper], 0),
+ m_springW[3][0], m_springW[3][1], m_springW[3][2] );
+ reflex->addDecorativeLine(m_viz, baseLine, .25, 1.0),
+ m_muscles[UniqueMuscle(Reflex22,side)] = reflex;
+ Force::Custom(m_forces, reflex);
+ }
+}
+
+void Dude::loadDefaultState(State& state) {
+ const static Real hipAngle = -15*Pi/180;
+ const static Real kneeAngle = -40*Pi/180;
+ const static Real ankleAngle = 70*Pi/180;
+ const static Real hipVelocity = .125;
+ const static Real kneeVelocity = 0;
+
+ const static Real pelvisZAngle = 20*Pi/180;
+
+ m_mobod[UniqueBody(Pelvis,Only)].setQToFitRotation(state, Rotation(pelvisZAngle,ZAxis));
+ m_mobod[UniqueBody(Pelvis,Only)].setQToFitTranslation(state, Vec3(0,2.3,0));
+ m_mobod[UniqueBody(Pelvis,Only)].setOneU(state, 2, -hipVelocity);
+
+ m_mobod[UniqueBody(Thigh,Left)].setOneQ(state, 0, -hipAngle);
+ m_mobod[UniqueBody(Shank,Left)].setOneQ(state, 0, kneeAngle);
+ m_mobod[UniqueBody(Foot,Left)].setOneQ(state, 0, ankleAngle);
+ m_mobod[UniqueBody(Thigh,Left)].setOneU(state, 0, hipVelocity);
+ m_mobod[UniqueBody(Shank,Left)].setOneU(state, 0, kneeVelocity);
+
+ m_mobod[UniqueBody(Thigh,Right)].setOneQ(state, 0, hipAngle);
+ m_mobod[UniqueBody(Shank,Right)].setOneQ(state, 0, kneeAngle);
+ m_mobod[UniqueBody(Foot,Right)].setOneQ(state, 0, ankleAngle);
+ m_mobod[UniqueBody(Thigh,Right)].setOneU(state, 0, hipVelocity);
+ m_mobod[UniqueBody(Shank,Right)].setOneU(state, 0, -kneeVelocity);
+
+}
+
+
+
+//////////////////////////////////////////////////////////////////////////
+
+TwoPointMuscleDamperReflex::TwoPointMuscleDamperReflex
+ (const MobilizedBody& body1, const Vec3& station1,
+ const MobilizedBody& body2, const Vec3& station2,
+ Real k, Real damping, Real x0)
+: mBody1(body1), mStation1(station1), mBody2(body2), mStation2(station2),
+ mK(k),
+ mDamping(damping),
+ mX0(x0) {
+}
+
+void TwoPointMuscleDamperReflex::calcForce(const State& state,
+ Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces,
+ Vector& mobilityForces) const {
+
+ const Transform& X_GB1 = mBody1.getBodyTransform(state);
+ const Transform& X_GB2 = mBody2.getBodyTransform(state);
+
+ const Vec3 s1_G = X_GB1.R() * mStation1;
+ const Vec3 s2_G = X_GB2.R() * mStation2;
+
+ const Vec3 p1_G = X_GB1.p() + s1_G; // mStation measured from ground origin
+ const Vec3 p2_G = X_GB2.p() + s2_G;
+
+ const Vec3 r_G = p2_G - p1_G; // vector from point1 to point2
+ const Real dist = r_G.norm(); // distance between the points
+ if( dist < SignificantReal ) return;
+ const UnitVec3 dir(r_G); // direction from point1 to point2
+
+ const Real stretch = dist - mX0; // + -> tension, - -> compression
+ const Real frcStretch = mK*stretch; // k(x-x0)
+
+ //////////////////////////////////////////////////////////////////////////
+
+ const Vec3 v1_G = mBody1.findStationVelocityInGround(state, mStation1);
+ const Vec3 v2_G = mBody2.findStationVelocityInGround(state, mStation2);
+ const Vec3 vRel = v2_G - v1_G; // relative velocity
+ const Real frcDamp = mDamping*dot(vRel, dir); // c*v
+
+ //////////////////////////////////////////////////////////////////////////
+
+ const Vec3 f1_G = (frcStretch + frcDamp) * dir;
+
+ bodyForces[mBody1.getMobilizedBodyIndex()] += SpatialVec(s1_G % f1_G, f1_G);
+ bodyForces[mBody2.getMobilizedBodyIndex()] -= SpatialVec(s2_G % f1_G, f1_G);
+
+}
+
+Real TwoPointMuscleDamperReflex::calcPotentialEnergy(const State& state) const {
+
+ const Transform& X_GB1 = mBody1.getBodyTransform(state);
+ const Transform& X_GB2 = mBody2.getBodyTransform(state);
+
+ const Vec3 s1_G = X_GB1.R() * mStation1;
+ const Vec3 s2_G = X_GB2.R() * mStation2;
+
+ const Vec3 p1_G = X_GB1.p() + s1_G; // mStation measured from ground origin
+ const Vec3 p2_G = X_GB2.p() + s2_G;
+
+ const Vec3 r_G = p2_G - p1_G; // vector from point1 to point2
+ const Real d = r_G.norm(); // distance between the points
+ const Real stretch = d - mX0; // + -> tension, - -> compression
+
+ return 0.5*mK*stretch*stretch; // 1/2 k (x-x0)^2
+
+}
+
diff --git a/Simbody/examples/Makefile b/Simbody/examples/Makefile
new file mode 100644
index 0000000..3141b27
--- /dev/null
+++ b/Simbody/examples/Makefile
@@ -0,0 +1,63 @@
+# Makefile for building Simbody examples on Linux or Mac.
+# See README_Makefile.txt for more information.
+
+# Comment this out if you're building 64 bit examples (Linux only).
+M32FLAG = -m32
+
+# Uncomment DEBUG to use Simbody debug libraries instead of release libraries
+# DEBUG=_d
+
+# Default install directory
+# SimTK_INSTALL_DIR=/usr/local/SimTK
+#
+ifeq "${SimTK_INSTALL_DIR}" ""
+SimTK_HOME=/usr/local/SimTK
+else
+SimTK_HOME=${SimTK_INSTALL_DIR}
+endif
+
+LIB_DIR=$(SimTK_HOME)/lib
+INCLUDE_DIR=$(SimTK_HOME)/include
+
+# You would need this whole list to link with static libraries; for
+# dynamic libraries (shared objects) the others are picked up as needed.
+# -llapack -lblas -lpthread -lm -lrt
+LIBS= -lSimTKsimbody$(DEBUG) -lSimTKmath$(DEBUG) -lSimTKcommon$(DEBUG)
+
+ifeq "${DEBUG}" ""
+CFLAGS = -O2 ${M32FLAG}
+else
+CFLAGS = -g ${M32FLAG}
+endif
+
+ALL_PROGS = \
+SimbodyInstallTestNoViz \
+SimbodyInstallTest \
+ChainExample \
+ExampleAmysIKProblem \
+ExampleAssemblerPlayground \
+ExampleBricardMechanism \
+ExampleContactPlayground \
+ExampleEventHandler \
+ExampleEventReporter \
+ExampleGears \
+ExampleKneeJoint \
+ExampleLongPendulum \
+ExamplePendulum \
+ExampleSampleAndHold \
+JaredsDude \
+Rattleback
+
+default: SimbodyInstallTestNoViz SimbodyInstallTest
+ @echo Using $(SimTK_HOME) as the SimTK installation directory.
+
+
+all : $(ALL_PROGS)
+ @echo Using $(SimTK_HOME) as the SimTK installation directory.
+
+# Treat all .cpp source files the same way
+.cpp :
+ g++ $(CFLAGS) $< -I$(INCLUDE_DIR) -L$(LIB_DIR) $(LIBS) -o $*
+
+clean :
+ rm $(ALL_PROGS)
diff --git a/Simbody/examples/PendulumNoViz.cpp b/Simbody/examples/PendulumNoViz.cpp
new file mode 100644
index 0000000..0d31fa8
--- /dev/null
+++ b/Simbody/examples/PendulumNoViz.cpp
@@ -0,0 +1,94 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Pendulum No Viz *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "Simbody.h"
+
+#include <cstdio>
+#include <exception>
+
+using namespace SimTK;
+
+/* This is the same as the example named Pendulum except that
+it does not use the Visualizer. Instead it prints out the kinetic
+energy and total energy (the latter should be conserved). This is
+useful for checking that installation is OK on systems where the
+Visualizer wasn't built or isn't working. */
+
+
+class EnergyReporter : public PeriodicEventReporter {
+public:
+ EnergyReporter(const MultibodySystem& system, Real interval)
+ : PeriodicEventReporter(interval), system(system) {}
+
+ void handleEvent(const State& s) const {
+ std::cout << "t=" << s.getTime()
+ << "\tke=" << system.calcKineticEnergy(s)
+ << "\tE=" << system.calcEnergy(s)
+ << std::endl;
+ }
+private:
+ const MultibodySystem& system;
+};
+
+int main() {
+ try
+ { // Create the system.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+
+ Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1));
+ MobilizedBody::Pin pendulum(matter.Ground(), Transform(Vec3(0)),
+ pendulumBody, Transform(Vec3(0, 1, 0)));
+
+ system.addEventReporter(new EnergyReporter(system,1.));
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ pendulum.setOneU(state, 0, 1.0);
+
+
+ // Simulate it.
+
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setAccuracy(1e-6); // ask for *lots* of accuracy here (default is 1e-3)
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(100.0);
+
+ } catch (const std::exception& e) {
+ std::printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+
+ } catch (...) {
+ std::printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+ return 0;
+}
diff --git a/Simbody/examples/README.txt b/Simbody/examples/README.txt
new file mode 100644
index 0000000..ddc1359
--- /dev/null
+++ b/Simbody/examples/README.txt
@@ -0,0 +1,63 @@
+ Simbody 2.2 Examples README
+ June, 2011
+
+This is an eclectic set of examples covering a range of systems that can
+be modeled with Simbody: mechanical systems, biomechanical systems, and
+molecular systems. These were collected rather than built specifically
+for their use here, so you will find a variety of styles.
+
+Precompiled binaries of these programs are installed in the examples/bin
+directory. In particular, you can run SimbodyInstallTest to check installation,
+or SimbodyInstallTestNoViz to check that everything other than the Visualizer
+is working. You should be able to re-create these binaries using the source
+provided here and instructions below.
+
+We have supplied an example Makefile that will work on Linux and Mac, and
+a CMakeLists.txt file that will work on all platforms. Although we have not
+provided a pre-built Visual Studio solution file, CMake will quickly
+create one for you from the CMakeLists.txt and we strongly recommend that
+approach. If you don't have it already, go to cmake.org and download
+CMake 2.8 or later. Then run the CMake GUI, tell it the source directory
+(where it will look for CMakeLists.txt) hit "Configure", tell it what
+compiler you want to use, then hit "Generate" and exit. You will then
+have a nice Visual Studio solution (.sln) file you can click on. This
+should also work for building an XCode project on the Mac although we
+haven't tested that yet. And CMake also creates ordinary make files like
+the one supplied here.
+
+The CMakeLists.txt file here creates a single build environment containing
+separate projects for each of the examples. That works on all platforms,
+using whatever build system is appropriate.
+
+If you want to make your own Visual Studio solution by hand, see
+README_VisualStudio.txt.
+
+If you want to use the supplied Makefile: see README_Makefile.txt.
+
+
+General Instructions
+--------------------
+Download CMake 2.8 or later from http://www.cmake.org. Be sure to tell
+CMake to use the version of the compiler that you intend to use. Note
+that you must use a version that is compatible with the installed
+Simbody binaries. If you can't find a suitable pre-built binary for your
+compiler you'll have to build the Simbody libraries from the source
+distribution. That's not very hard -- see the step-by-step instructions.
+
+There are platform dependencies regarding running CMake; basically either
+click on the icon (Windows) or run the ccmake UI. Make sure it understands
+where Simbody was installed (it should see the SimTK_INSTALL_DIR environment
+variable).
+
+For help, post to the Simbody project help Forum, at
+https://simtk.org/home/simbody, Advanced tab, Public Forums.
+
+
+Running the Examples
+--------------------
+These programs were created by different people and operate differently.
+In particular, some will just start running and others want some input
+before they do anything. Of the input-wanting ones, some ask for input
+at the console (terminal) window, while others expect something from
+the Visualizer window. Try one or the other if nothing seems to be
+happening.
diff --git a/Simbody/examples/README_Makefile.txt b/Simbody/examples/README_Makefile.txt
new file mode 100644
index 0000000..434f6ce
--- /dev/null
+++ b/Simbody/examples/README_Makefile.txt
@@ -0,0 +1,48 @@
+Instructions for using the Makefile provided.
+
+----------------------------------------------------------------------------
+NOTE: The easiest way to create a build project is to use
+CMake (www.cmake.org). We have supplied a CMakeLists.txt file here which
+will build a single Makefile containing all the examples.
+That works on all platforms also, using whatever build system is appropriate,
+even Visual Studio.
+----------------------------------------------------------------------------
+
+
+You must already have the Simbody binaries installed from SimTK.org; go
+to https://simtk.org/home/simbody, Downloads tab.
+
+You may need to slightly edit the Makefile to make it run on your system.
+
+Open the Makefile and make the appropriate changes -- debug libraries or not,
+remove -m32 if you are running 64 bits on Linux, and set the default
+install directory to the correct location.
+
+Type "make" to build just one simple example, SimbodyInstallTestNoViz and
+SimbodyInstallTest, and try running them if they succeed:
+./SimbodyInstallTestNoViz, ./SimbodyInstallTest.
+
+If that works, then try to build all the examples with "make all".
+
+To compile just ExampleChain (for example) type "make ExampleChain".
+
+Before you run the executables, remember to add the Simbody library
+directory to your library path. The simplest way to do this is to
+type the following commands:
+
+First, if you didn't install Simbody in the default location,
+/usr/local/SimTK, tell the Makefile where to look:
+
+ $ export SimTK_INSTALL_DIR=/mysimbody/installdir
+
+Then you have to tell the linker where to find the shared libraries:
+
+ For Linux (for the bash shell):
+ $ export LD_LIBRARY_PATH=$SimTK_INSTALL_DIR/lib
+
+ For Mac (for the bash shell):
+ $ export DYLD_LIBRARY_PATH=$SimTK_INSTALL_DIR/lib
+
+If you didn't set SimTK_INSTALL_DIR, then you would type
+"/usr/local/SimTK/lib" instead above. On 64-bit Linux, the "lib64" directory
+is used rather than "lib".
diff --git a/Simbody/examples/README_VisualStudio.txt b/Simbody/examples/README_VisualStudio.txt
new file mode 100644
index 0000000..71dbb83
--- /dev/null
+++ b/Simbody/examples/README_VisualStudio.txt
@@ -0,0 +1,137 @@
+Instructions for creating a Visual Studio project by hand to run a
+Simbody example program.
+Simbody 2.2, June 2011
+
+----------------------------------------------------------------------------
+NOTE: By far the easiest way to create a Visual Studio project is to use
+CMake (www.cmake.org). We have supplied a CMakeLists.txt file here which
+will build a single Visual Studio project containing all the examples.
+That works on all platforms also, using whatever build system is appropriate.
+----------------------------------------------------------------------------
+
+But, if you insist on building a Visual Studio project by hand, here are
+the instructions.
+
+
+
+Below we will use $(SimTK_INSTALL_DIR) to mean the SimTK installation
+directory, which is typically %ProgramFiles%\SimTK but can be anything.
+(On English systems this will be C:\Program Files\SimTK or
+C:\Program Files (x86)\SimTK for 64 bit platforms.). You
+should create a SimTK_INSTALL_DIR environment variable and
+set it to the appropriate Simbody installation directory.
+
+You must already have the Simbody binaries installed from SimTK.org; go
+to https://simtk.org/home/simbody, Downloads tab.
+
+To use the Visual Studio solution file provided in this directory proceed
+to step 5)
+
+To create a new Visual Studio project for a Simbody example from scratch
+follow all steps:
+
+
+1) Create new Visual Studio project based on an example source file:
+
+ * In Visual Studio, select File->New->Project...->Visual C++->Win32->Win32 Console Application
+
+ * Select a name for the new project, select a location, and select "Create new Solution"
+
+ * Click "Next"; select "Console Application" and "Empty project"
+
+ * Drag the desired example .cpp program into the "Source files" folder on the left.
+
+
+2) Set up include directory
+
+ * Far click the Project name on the left bar, select "Properties"
+
+ * Set the "Configuration:" pulldown to "All Configurations"
+
+ * Under Configuration Properties->C++->General select "Additional Include Directories"
+
+ * Select the "..." button, then the new directory (folder) button
+
+ * Browse to $(SimTK_INSTALL_DIR)/include, and add it to the "Additional Include Directories" section
+
+ * Click "OK"
+
+
+3) Set up library directories
+
+ * Far click the Project name on the left bar, select "Properties"
+
+ * Set the "Configuration:" pulldown to "All Configurations"
+
+ * Under Configuration Properties->Linker->General select "Additional Library Directories"
+
+ * Select the "..." button, then the new directory (folder) button
+
+ * Add $(SimTK_INSTALL_DIR)/lib to the "Additional Library Directories" section.
+
+ * Click "OK"
+
+
+4) Set up library names
+
+ * Far click the Project name on the left bar, select "Properties"
+
+ a) Debug versions
+ NOTE: Debug libraries are very sensitive to compiler version; if you can't
+ use the prebuilt ones follow the Release instructions below unless you
+ have built your own Debug libraries from source.
+ CAUTION: You may not be able to use Release libraries with a main
+ program compiled with Debug.
+
+ * Set the "Configuration:" pulldown to "Debug"
+
+ * Under Configuration Properties->Linker->Input select "Additional Dependencies"
+
+ * Select the "..." button
+
+ * Add the following library names:
+ SimTKsimbody_d.lib
+ SimTKmath_d.lib
+ SimTKcommon_d.lib
+ SimTKlapack.lib
+ Note that there is no "_d" lapack library; that's OK.
+
+ * Click "Apply"
+
+ a) Release versions
+
+ * Set the "Configuration:" pulldown to "Release"
+
+ * Under Configuration Properties->Linker->Input select "Additional Dependencies"
+
+ * Select the "..." button
+
+ * Add the following library names:
+ SimTKsimbody.lib
+ SimTKmath.lib
+ SimTKcommon.lib
+ SimTKlapack.lib
+
+ * Click "Apply"
+
+
+5) Run the program
+
+* select "Release" configuration in the Visual Studio main toolbar if you
+ want speed; Debug is typically 10X slower.
+
+* Type Ctrl-F5, or click Debug->"Start Without Debugging"
+
+* Your program should run
+
+* Type Ctrl-C in the console (Command Prompt) window to stop the program
+
+
+6) To run another example program:
+
+ * Delete the previous example .cpp file from the "Source Files" folder
+
+ * Drag the new example .cpp file into the "Source Files" folder
+
+ * Select "Release" configuration, Ctrl-F5, etc.
+
diff --git a/Simbody/examples/Rattleback.cpp b/Simbody/examples/Rattleback.cpp
new file mode 100644
index 0000000..306510f
--- /dev/null
+++ b/Simbody/examples/Rattleback.cpp
@@ -0,0 +1,361 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Rattleback *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* This example is for experimenting with ellipsoid contact which was
+introduced in Simbody 2.2.
+*/
+
+#include "Simbody.h"
+
+#include <cstdio>
+#include <exception>
+#include <algorithm>
+#include <iostream>
+#include <fstream>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+Array_<State> saveEm;
+
+
+const Real Cm2m = 1e-2;
+const Real CmSq2mSq = Cm2m*Cm2m; // conversion from sq cm to sq m
+const Real Deg2Rad = (Real)SimTK_DEGREE_TO_RADIAN;
+const Real Rad2Deg = (Real)SimTK_RADIAN_TO_DEGREE;
+
+static const Real TimeScale = 1;
+static const Real FrameRate = 30;
+static const Real ReportInterval = TimeScale/FrameRate;
+static const Real ForceScale = .25;
+static const Real MomentScale = .5;
+
+
+class ForceArrowGenerator : public DecorationGenerator {
+public:
+ ForceArrowGenerator(const MultibodySystem& system,
+ const CompliantContactSubsystem& complCont)
+ : m_system(system), m_compliant(complCont) {}
+
+ virtual void generateDecorations(const State& state, Array_<DecorativeGeometry>& geometry) {
+ const Vec3 frcColors[] = {Red,Orange,Cyan};
+ const Vec3 momColors[] = {Blue,Green,Purple};
+ m_system.realize(state, Stage::Velocity);
+
+ const int ncont = m_compliant.getNumContactForces(state);
+ for (int i=0; i < ncont; ++i) {
+ const ContactForce& force = m_compliant.getContactForce(state,i);
+ const ContactId id = force.getContactId();
+ const Vec3& frc = force.getForceOnSurface2()[1];
+ const Vec3& mom = force.getForceOnSurface2()[0];
+ Real frcMag = frc.norm(), momMag=mom.norm();
+ int frcThickness = 1, momThickness = 1;
+ Real frcScale = ForceScale, momScale = ForceScale;
+ while (frcMag > 10)
+ frcThickness++, frcScale /= 10, frcMag /= 10;
+ while (momMag > 10)
+ momThickness++, momScale /= 10, momMag /= 10;
+ DecorativeLine frcLine(force.getContactPoint(),
+ force.getContactPoint() + frcScale*frc);
+ DecorativeLine momLine(force.getContactPoint(),
+ force.getContactPoint() + momScale*mom);
+ frcLine.setColor(frcColors[id%3]);
+ momLine.setColor(momColors[id%3]);
+ frcLine.setLineThickness(2*frcThickness);
+ momLine.setLineThickness(2*momThickness);
+ geometry.push_back(frcLine);
+ geometry.push_back(momLine);
+
+ ContactPatch patch;
+ const bool found = m_compliant.calcContactPatchDetailsById(state,id,patch);
+ //cout << "patch for id" << id << " found=" << found << endl;
+ //cout << "resultant=" << patch.getContactForce() << endl;
+ //cout << "num details=" << patch.getNumDetails() << endl;
+ for (int i=0; i < patch.getNumDetails(); ++i) {
+ const ContactDetail& detail = patch.getContactDetail(i);
+ const Real peakPressure = detail.getPeakPressure();
+ // Make a black line from the element's contact point in the normal
+ // direction, with length proportional to log(peak pressure)
+ // on that element.
+ DecorativeLine normal(detail.getContactPoint(),
+ detail.getContactPoint()+ std::log10(peakPressure)
+ * detail.getContactNormal());
+ normal.setColor(Black);
+ geometry.push_back(normal);
+ // Make a red line that extends from the contact
+ // point in the direction of the slip velocity, of length 3*slipvel.
+ DecorativeLine slip(detail.getContactPoint(),
+ detail.getContactPoint()+3*detail.getSlipVelocity());
+ slip.setColor(Red);
+ geometry.push_back(slip);
+ }
+ }
+ }
+private:
+ const MultibodySystem& m_system;
+ const CompliantContactSubsystem& m_compliant;
+};
+
+class MyReporter : public PeriodicEventReporter {
+public:
+ MyReporter(const MultibodySystem& system,
+ const CompliantContactSubsystem& complCont,
+ Real reportInterval)
+ : PeriodicEventReporter(reportInterval), m_system(system),
+ m_compliant(complCont)
+ {}
+
+ ~MyReporter() {}
+
+ void handleEvent(const State& state) const {
+ m_system.realize(state, Stage::Dynamics);
+ cout << state.getTime() << ": E = " << m_system.calcEnergy(state)
+ << " Ediss=" << m_compliant.getDissipatedEnergy(state)
+ << " E+Ediss=" << m_system.calcEnergy(state)
+ +m_compliant.getDissipatedEnergy(state)
+ << endl;
+ cout << " q0(Deg): " << state.getQ()[0]*Rad2Deg << endl;
+ const int ncont = m_compliant.getNumContactForces(state);
+ cout << "Num contacts: " << m_compliant.getNumContactForces(state) << endl;
+ for (int i=0; i < ncont; ++i) {
+ const ContactForce& force = m_compliant.getContactForce(state,i);
+ //cout << force;
+ }
+ saveEm.push_back(state);
+ }
+private:
+ const MultibodySystem& m_system;
+ const CompliantContactSubsystem& m_compliant;
+};
+
+// These are the item numbers for the entries on the Run menu.
+static const int RunMenuId = 3, HelpMenuId = 7;
+static const int GoItem = 1, ReplayItem=2, QuitItem=3;
+
+// This is a periodic event handler that interrupts the simulation on a regular
+// basis to poll the InputSilo for user input. If there has been some, process it.
+// This one does nothing but look for the Run->Quit selection.
+class UserInputHandler : public PeriodicEventHandler {
+public:
+ UserInputHandler(Visualizer::InputSilo& silo, Real interval)
+ : PeriodicEventHandler(interval), m_silo(silo) {}
+
+ virtual void handleEvent(State& state, Real accuracy,
+ bool& shouldTerminate) const
+ {
+ int menuId, item;
+ if (m_silo.takeMenuPick(menuId, item) && menuId==RunMenuId && item==QuitItem)
+ shouldTerminate = true;
+ }
+
+private:
+ Visualizer::InputSilo& m_silo;
+};
+
+int main() {
+ try
+ { // Create the system.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::Gravity gravity(forces, matter, UnitVec3(-1,0,0), 9.81);
+
+ ContactTrackerSubsystem tracker(system);
+ CompliantContactSubsystem contactForces(system, tracker);
+ contactForces.setTrackDissipatedEnergy(true);
+ contactForces.setTransitionVelocity(1e-2); // m/s
+
+ // Ground's normal is +x for this model
+ system.setUpDirection(+XAxis);
+
+ // Uncomment this if you want a more elegant movie.
+ //matter.setShowDefaultGeometry(false);
+
+ const Real ud = .3; // dynamic
+ const Real us = .6; // static
+ const Real uv = 0; // viscous (force/velocity)
+ const Real k = 1e8; // pascals
+ const Real c = 0.01; // dissipation (1/v)
+
+
+ // Halfspace default is +x, this one occupies -x instead, so flip.
+ const Rotation R_xdown(Pi,ZAxis);
+
+ matter.Ground().updBody().addContactSurface(
+ Transform(R_xdown, Vec3(0,0,0)),
+ ContactSurface(ContactGeometry::HalfSpace(),
+ ContactMaterial(k,c,us,ud,uv)));
+
+
+ const Real ellipsoidMass = 1; // kg
+ const Vec3 halfDims(2*Cm2m, 20*Cm2m, 3*Cm2m); // m (read in cm)
+ const Vec3 comLoc(-1*Cm2m, 0, 0);
+ const Inertia centralInertia(Vec3(17,2,16)*CmSq2mSq, Vec3(0,0,.2)*CmSq2mSq); // now kg-m^2
+ const Inertia inertia(centralInertia.shiftFromMassCenter(-comLoc, ellipsoidMass)); // in S
+ Body::Rigid ellipsoidBody(MassProperties(ellipsoidMass, comLoc, inertia));
+
+ ellipsoidBody.addDecoration(Transform(),
+ DecorativeEllipsoid(halfDims).setColor(Cyan)
+ //.setOpacity(.5)
+ .setResolution(3));
+ ellipsoidBody.addContactSurface(Transform(),
+ ContactSurface(ContactGeometry::Ellipsoid(halfDims),
+ ContactMaterial(k,c,us,ud,uv))
+ );
+ MobilizedBody::Free ellipsoid(matter.Ground(), Transform(Vec3(0,0,0)),
+ ellipsoidBody, Transform(Vec3(0)));
+
+
+ Visualizer viz(system);
+ viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
+ viz.setMode(Visualizer::RealTime);
+ viz.setDesiredFrameRate(FrameRate);
+ viz.setCameraClippingPlanes(0.1, 10);
+
+ Visualizer::InputSilo* silo = new Visualizer::InputSilo();
+ viz.addInputListener(silo);
+ Array_<std::pair<String,int> > runMenuItems;
+ runMenuItems.push_back(std::make_pair("Go", GoItem));
+ runMenuItems.push_back(std::make_pair("Replay", ReplayItem));
+ runMenuItems.push_back(std::make_pair("Quit", QuitItem));
+ viz.addMenu("Run", RunMenuId, runMenuItems);
+
+ Array_<std::pair<String,int> > helpMenuItems;
+ helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1));
+ viz.addMenu("Help", HelpMenuId, helpMenuItems);
+
+ system.addEventReporter(new MyReporter(system,contactForces,ReportInterval));
+ system.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));
+
+ // Check for a Run->Quit menu pick every 1/4 second.
+ system.addEventHandler(new UserInputHandler(*silo, .25));
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ matter.setUseEulerAngles(state, true);
+ system.realizeModel(state);
+
+ ellipsoid.setQToFitTransform(state, Transform(
+ Rotation(BodyRotationSequence, 0 *Deg2Rad, XAxis,
+ 0.5*Deg2Rad, YAxis,
+ -0.5*Deg2Rad, ZAxis),
+ Vec3(2.1*Cm2m, 0, 0)));
+
+ ellipsoid.setUToFitAngularVelocity(state, 2*Vec3(5,0,0)); // rad/s
+
+ viz.report(state);
+ printf("Default state\n");
+
+ cout << "\nChoose 'Go' from Run menu to simulate:\n";
+ int menuId, item;
+ do { silo->waitForMenuPick(menuId, item);
+ if (menuId != RunMenuId || item != GoItem)
+ cout << "\aDude ... follow instructions!\n";
+ } while (menuId != RunMenuId || item != GoItem);
+
+
+
+ // Simulate it.
+
+ //ExplicitEulerIntegrator integ(system);
+ //CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
+ //RungeKuttaFeldbergIntegrator integ(system);
+ RungeKuttaMersonIntegrator integ(system);
+ //RungeKutta3Integrator integ(system);
+ //VerletIntegrator integ(system);
+ //integ.setMaximumStepSize(1e-0001);
+ integ.setAccuracy(1e-4); // minimum for CPodes
+ //integ.setAccuracy(.01);
+ TimeStepper ts(system, integ);
+
+
+ ts.initialize(state);
+ double cpuStart = cpuTime();
+ double realStart = realTime();
+
+ ts.stepTo(10.0);
+
+ const double timeInSec = realTime() - realStart;
+ const int evals = integ.getNumRealizations();
+ cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
+ timeInSec << "s elapsed for " << ts.getTime() << "s sim (avg step="
+ << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
+ << (1000*ts.getTime())/evals << "ms/eval\n";
+ cout << " CPU time was " << cpuTime() - cpuStart << "s\n";
+
+ printf("Using Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
+
+ viz.dumpStats(std::cout);
+
+ // Add as slider to control playback speed.
+ viz.addSlider("Speed", 1, 0, 4, 1);
+ viz.setMode(Visualizer::PassThrough);
+
+ silo->clear(); // forget earlier input
+ double speed = 1; // will change if slider moves
+ while(true) {
+ cout << "Choose Run/Replay to see that again ...\n";
+
+ int menuId, item;
+ silo->waitForMenuPick(menuId, item);
+
+
+ if (menuId != RunMenuId) {
+ cout << "\aUse the Run menu!\n";
+ continue;
+ }
+
+ if (item == QuitItem)
+ break;
+ if (item != ReplayItem) {
+ cout << "\aHuh? Try again.\n";
+ continue;
+ }
+
+ for (double i=0; i < (int)saveEm.size(); i += speed ) {
+ int slider; Real newValue;
+ if (silo->takeSliderMove(slider,newValue)) {
+ speed = newValue;
+ }
+ viz.report(saveEm[(int)i]);
+ }
+ }
+
+ } catch (const std::exception& e) {
+ std::printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+
+ } catch (...) {
+ std::printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+ return 0;
+}
diff --git a/Simbody/examples/SimbodyInstallTest.cpp b/Simbody/examples/SimbodyInstallTest.cpp
new file mode 100644
index 0000000..03b5d01
--- /dev/null
+++ b/Simbody/examples/SimbodyInstallTest.cpp
@@ -0,0 +1,73 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Simbody Install Test *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "Simbody.h"
+
+#include <cstdio>
+#include <exception>
+
+using namespace SimTK;
+
+int main() {
+ try
+ { // Create the system.
+
+ MultibodySystem system; system.setUseUniformBackground(true);
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+
+ Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1));
+ MobilizedBody::Pin pendulum(matter.Ground(), Transform(Vec3(0)),
+ pendulumBody, Transform(Vec3(0, 1, 0)));
+ //Motion::Steady(pendulum, 1);
+
+ Visualizer viz(system);
+ system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ pendulum.setOneU(state, 0, 1.0);
+
+
+ // Simulate it.
+
+ RungeKuttaMersonIntegrator integ(system);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(100.0);
+
+ } catch (const std::exception& e) {
+ std::printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+
+ } catch (...) {
+ std::printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+ return 0;
+}
diff --git a/Simbody/examples/SimbodyInstallTestNoViz.cpp b/Simbody/examples/SimbodyInstallTestNoViz.cpp
new file mode 100644
index 0000000..ac886a5
--- /dev/null
+++ b/Simbody/examples/SimbodyInstallTestNoViz.cpp
@@ -0,0 +1,94 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Simbody Install Test No Viz *
+ * -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "Simbody.h"
+
+#include <cstdio>
+#include <exception>
+
+using namespace SimTK;
+
+/* This is the same as the example named Pendulum except that
+it does not use the Visualizer. Instead it prints out the kinetic
+energy and total energy (the latter should be conserved). This is
+useful for checking that installation is OK on systems where the
+Visualizer wasn't built or isn't working. */
+
+
+class EnergyReporter : public PeriodicEventReporter {
+public:
+ EnergyReporter(const MultibodySystem& system, Real interval)
+ : PeriodicEventReporter(interval), system(system) {}
+
+ void handleEvent(const State& s) const {
+ std::cout << "t=" << s.getTime()
+ << "\tke=" << system.calcKineticEnergy(s)
+ << "\tE=" << system.calcEnergy(s)
+ << std::endl;
+ }
+private:
+ const MultibodySystem& system;
+};
+
+int main() {
+ try
+ { // Create the system.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+
+ Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1));
+ MobilizedBody::Pin pendulum(matter.Ground(), Transform(Vec3(0)),
+ pendulumBody, Transform(Vec3(0, 1, 0)));
+
+ system.addEventReporter(new EnergyReporter(system,1.));
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ pendulum.setOneU(state, 0, 1.0);
+
+
+ // Simulate it.
+
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setAccuracy(1e-6); // ask for *lots* of accuracy here (default is 1e-3)
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(100.0);
+
+ } catch (const std::exception& e) {
+ std::printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+
+ } catch (...) {
+ std::printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+ return 0;
+}
diff --git a/Simbody/examples/TheoJansenStrandbeest.cpp b/Simbody/examples/TheoJansenStrandbeest.cpp
new file mode 100644
index 0000000..a76d941
--- /dev/null
+++ b/Simbody/examples/TheoJansenStrandbeest.cpp
@@ -0,0 +1,482 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: TheoJansenStrandbeest *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "Simbody.h"
+#include <iostream>
+using namespace SimTK;
+using std::cout; using std::endl; using std::cin;
+
+/* This is a walking mechanism due to Theo Jansen. See strandbeest.com and
+many YouTube videos. This is a full 3D simulation although each leg moves
+only in 2D. The user controls the speed. I haven't yet figured out how to
+steer one of these things though so it's not a very exciting drive!
+
+Two different ways of building this model are demonstrated here based on the
+compile-time flag below. One models all the links as they appear; the other
+treats some links as massless resulting in halving the model size and better
+than a 2X speedup.
+*/
+
+// Define this to use a simplified model that replaces some of the links
+// with distance constraints. That reduces the model size by about half without
+// changing the functionality at all.
+//#define USE_MASSLESS_LINKS
+
+// Undefine this to get more accurate CPU times.
+#define ANIMATE
+
+// Put local classes and definitions in the file-scope anonymous namespace.
+namespace {
+
+// This is a periodic event handler that interrupts the simulation on a regular
+// basis to poll the InputSilo for user input, mostly for speed control.
+const int SpeedControlSlider = 1;
+class UserInputHandler : public PeriodicEventHandler {
+public:
+ UserInputHandler(Visualizer::InputSilo& silo,
+ const Motion::Steady& motor,
+ Real interval);
+ void handleEvent(State& state, Real accuracy,
+ bool& shouldTerminate) const OVERRIDE_11;
+private:
+ Visualizer::InputSilo& m_silo;
+ Motion::Steady m_motor;
+};
+
+// Handy utility routine: given two vertices v1, v2 of a triangle in the x-y
+// plane and the lengths of the other two sides, find the location of the third
+// vertex, assuming v1-v2-v3 have counterclockwise ordering about the plane
+// normal. z coordinate is ignored on input and zero on output.
+Vec3 findOtherVertex(const Vec3& v1, const Vec3& v2, Real s1, Real s2);
+
+// Write interesting integrator info to stdout at end of simulation.
+void dumpIntegratorStats(double startCPU, double startTime,
+ const Integrator& integ);
+
+const Real LinkDepth = .01; // half depth of links
+const Real LinkWidth = .02; // half width of links
+
+const Real mu_s = 0.7; // Friction coefficients.
+const Real mu_d = 0.5;
+const Real mu_v = 0;
+const Real transitionVelocity = 1e-3; // slide->stick velocity
+
+// Rubber for feet
+const Real rubber_density = 1100.; // kg/m^3
+const Real rubber_young = 0.01e9/10; // pascals (N/m)
+const Real rubber_poisson = 0.5; // ratio
+const Real rubber_planestrain =
+ ContactMaterial::calcPlaneStrainStiffness(rubber_young,rubber_poisson);
+const Real rubber_dissipation = /*0.005*/10;
+
+const ContactMaterial rubber(rubber_planestrain,rubber_dissipation,
+ mu_s,mu_d,mu_v);
+
+// Concrete for ground
+const Real concrete_density = 2300.; // kg/m^3
+const Real concrete_young = 25e9; // pascals (N/m)
+const Real concrete_poisson = 0.15; // ratio
+const Real concrete_planestrain =
+ ContactMaterial::calcPlaneStrainStiffness(concrete_young,concrete_poisson);
+const Real concrete_dissipation = 0.005;
+
+const ContactMaterial concrete(concrete_planestrain,concrete_dissipation,
+ mu_s,mu_d,mu_v);
+
+// Add one leg to the given torso T with the leg frame's pose in T given.
+void addOneLeg(Visualizer& viz, MobilizedBody& torso, const Transform& X_TL,
+ MobilizedBody& crank, const Vec3& crankConnect);
+
+const Rotation YtoX(-Pi/2,ZAxis); // some useful rotations
+const Rotation YtoZ( Pi/2,XAxis);
+}
+
+
+
+//==============================================================================
+// MAIN
+//==============================================================================
+int main() {
+ try { // catch errors if any
+ // Create the system, with subsystems for the bodies and some forces.
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ ContactTrackerSubsystem tracker(system);
+ CompliantContactSubsystem contact(system, tracker);
+ contact.setTransitionVelocity(transitionVelocity);
+ Force::Gravity(forces, matter, -YAxis, 9.81);
+
+ // Set up visualization and ask for a frame every 1/30 second.
+ Visualizer viz(system);
+ viz.setShowSimTime(true); viz.setShowFrameRate(true);
+ viz.addSlider("Speed", SpeedControlSlider, -10, 10, 0);
+ Visualizer::InputSilo* silo = new Visualizer::InputSilo();
+ viz.addInputListener(silo);
+ #ifdef ANIMATE
+ system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
+ #endif
+ DecorativeText help("Any input to start; ESC to quit");
+ help.setIsScreenText(true);
+ viz.addDecoration(MobilizedBodyIndex(0),Vec3(0),help);
+ matter.setShowDefaultGeometry(false);
+
+ // Add the Ground contact geometry. Contact half space has -XAxis normal
+ // (right hand wall) so we have to rotate.
+ MobilizedBody& Ground = matter.updGround(); // Nicer name for Ground.
+ Ground.updBody().addContactSurface(Transform(YtoX,Vec3(0)),
+ ContactSurface(ContactGeometry::HalfSpace(),concrete));
+
+ // Add some speed bumps.
+ const int NBumps = 2; const Vec3 BumpShape(.8,0.2,2);
+ for (int i=0; i < NBumps; ++i) {
+ const Real x = -2*(i+1);
+ Ground.updBody().addContactSurface(Vec3(x,0,0),
+ ContactSurface(ContactGeometry::Ellipsoid(BumpShape), rubber));
+ Ground.updBody().addDecoration(Vec3(x,0,0),
+ DecorativeEllipsoid(BumpShape).setColor(Gray).setResolution(3));
+ }
+
+ // TORSO
+ const Real TorsoHeight = 1.1;
+ const Vec3 torsoHDims(1,.08,.8);
+ const Real torsoVolume = 8*torsoHDims[0]*torsoHDims[1]*torsoHDims[2];
+ const Real torsoMass = torsoVolume*rubber_density/10;
+ const Vec3 torsoCOM(0,-.75,0); // put it low for stability
+ Body::Rigid torsoInfo(MassProperties(torsoMass,torsoCOM,
+ UnitInertia::brick(torsoHDims).shiftFromCentroidInPlace(-torsoCOM)));
+ torsoInfo.addDecoration(Vec3(0),
+ DecorativeBrick(torsoHDims).setColor(Cyan));
+
+ // CRANK
+ const Vec3 crankCenter(0,0,0); // in torso frame
+ const Vec3 crankOffset(0,0,torsoHDims[2]+LinkDepth); // left/right offset
+ const Real MLen=15/100.; // crank radius
+ Body::Rigid crankInfo(MassProperties(.1,Vec3(0),
+ UnitInertia::cylinderAlongZ(MLen, LinkDepth)));
+ crankInfo.addDecoration(Vec3(0),
+ DecorativeBrick(Vec3(LinkWidth,LinkWidth,torsoHDims[2]))
+ .setColor(Black));
+ const Vec3 CrankConnect(MLen,0,0); // in crank frame
+
+ // Add the torso and crank mobilized bodies.
+ MobilizedBody::Free torso(Ground,Vec3(0,TorsoHeight,0), torsoInfo,Vec3(0));
+ MobilizedBody::Pin crank(torso,crankCenter, crankInfo, Vec3(0));
+
+ // Add the legs.
+ for (int i=-1; i<=1; ++i) {
+ const Vec3 offset = crankCenter + i*crankOffset;
+ const Vec3 linkSpace(0,0,2*LinkDepth);
+ const Rotation R_CP(i*2*Pi/3,ZAxis);
+ // Add crank bars for looks.
+ crank.addBodyDecoration(
+ Transform(R_CP, offset+1.5*MLen/2*R_CP.x()+(i==0?linkSpace:Vec3(0))),
+ DecorativeBrick(Vec3(1.5*MLen/2,LinkWidth,LinkDepth))
+ .setColor(Yellow));
+
+ addOneLeg(viz, torso, offset + i*linkSpace,
+ crank, R_CP*CrankConnect);
+ addOneLeg(viz, torso, Transform(Rotation(Pi,YAxis), offset + i*linkSpace),
+ crank, R_CP*CrankConnect);
+ }
+
+ // Add speed control.
+ Motion::Steady motor(crank, 0); // User controls speed.
+ system.addEventHandler
+ (new UserInputHandler(*silo, motor, Real(0.1))); //check input every 100ms
+
+ // Initialize the system and state.
+ State state = system.realizeTopology();
+ system.realize(state);
+ printf("Theo Jansen Strandbeest in 3D:\n");
+ printf("%d bodies, %d mobilities, -%d constraint equations -%d motions\n",
+ matter.getNumBodies(), state.getNU(), state.getNMultipliers(),
+ matter.getMotionMultipliers(state).size());
+
+ viz.report(state);
+ printf("Hit any key to assemble ...");
+ silo->waitForAnyUserInput(); silo->clear();
+ Assembler(system).assemble(state);
+ printf("ASSEMBLED\n");
+
+ // Simulate.
+ SemiExplicitEuler2Integrator integ(system);
+ integ.setAccuracy(0.1);
+ integ.setConstraintTolerance(.001);
+ integ.setMaximumStepSize(1./60);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ viz.report(ts.getState());
+ printf("Hit ENTER to simulate ... (ESC to quit)\n");
+ silo->waitForAnyUserInput(); silo->clear();
+
+ const double startCPU = cpuTime(), startTime = realTime();
+ ts.stepTo(Infinity); // RUN
+ dumpIntegratorStats(startCPU, startTime, integ);
+
+ } catch (const std::exception& e) {
+ std::cout << "ERROR: " << e.what() << std::endl;
+ return 1;
+ }
+ return 0;
+}
+
+namespace {
+
+//==============================================================================
+// ADD ONE LEG
+//==============================================================================
+// This function can build a leg two different ways, depending on whether
+// USE_MASSLESS_LINKS is defined:
+// (1) A straightforward model where all links have mass, producing 6 mobilities
+// and requiring 6 constraints per leg.
+// (2) Only the shoulder and foot bodies have mass, so the model requires only 3
+// mobilities and 3 distance constraints instead, for much improved
+// performance.
+void addOneLeg(Visualizer& viz, MobilizedBody& torso, const Transform& X_TL,
+ MobilizedBody& crank, const Vec3& crankConnect)
+{
+ // Segment lettering is from TJ's drawing here:
+ // http://www.strandbeest.com/beests_leg.php
+ // These dimensions are what TJ calls the "holy numbers". These are scaled
+ // from cm to m so that the mechanism is about 1m tall.
+ const Real ALen=38/100., LLen=7.8/100.;
+ const Real BLen=41.5/100., ELen=55.8/100., DLen=40.1/100.; // shoulder sides
+ const Real ILen=49/100., HLen=65.7/100., GLen=36.7/100.; // foot sides
+
+ const Real CLen = 39.3/100.; // Link lengths, TJ's lettering
+ const Real JLen = 50/100.;
+ const Real FLen = 39.4/100.;
+ const Real KLen = 61.9/100.;
+
+ // The pivot point is where the shoulder is pinned to the torso.
+ const Vec3 pivotPt = X_TL*Vec3(-ALen,-LLen,0); // to torso frame
+
+ // SHOULDER (a triangular body)
+ // Put the shoulder origin at the pivot, one side in the +Y.
+ Body::Rigid shoulderInfo(MassProperties(1,Vec3(0),UnitInertia(1)));
+ const Vec3 shoulderPivot(0,0,0);
+ const Vec3 shoulderUpper(0,BLen,0);
+ const Vec3 shoulderSide = findOtherVertex(shoulderPivot,shoulderUpper,
+ DLen,ELen);
+ const UnitVec3 BDir(shoulderUpper-shoulderPivot);
+ const UnitVec3 DDir(shoulderSide-shoulderPivot);
+ const UnitVec3 EDir(shoulderUpper-shoulderSide);
+
+ Rotation R_SB(BDir, XAxis, Vec3(0,0,1), ZAxis);
+ Rotation R_SD(DDir, XAxis, Vec3(0,0,1), ZAxis);
+ Rotation R_SE(EDir, XAxis, Vec3(0,0,1), ZAxis);
+ shoulderInfo.addDecoration(Transform(R_SB, shoulderPivot+BLen/2*BDir),
+ DecorativeBrick(Vec3(BLen/2,LinkWidth,LinkDepth)).setColor(Orange));
+ shoulderInfo.addDecoration(Transform(R_SD,shoulderPivot+DLen/2*DDir),
+ DecorativeBrick(Vec3(DLen/2,LinkWidth,LinkDepth)).setColor(Orange));
+ shoulderInfo.addDecoration(Transform(R_SE, shoulderSide+ELen/2*EDir),
+ DecorativeBrick(Vec3(ELen/2,LinkWidth,LinkDepth)).setColor(Orange));
+
+ // FOOT (a triangular body)
+ // Put the foot origin at the pivot, one side in the -Y.
+ Body::Rigid footInfo(MassProperties(10,Vec3(0),UnitInertia(1)));
+ const Vec3 footPivot(0,0,0);
+ const Vec3 footLower(0,-ILen,0);
+ const Vec3 footSide = findOtherVertex(footLower,footPivot,HLen,GLen);
+
+ const UnitVec3 IDir(footLower-footPivot);
+ const UnitVec3 GDir(footSide-footPivot);
+ const UnitVec3 HDir(footLower-footSide);
+
+ Rotation R_SI(IDir, XAxis, Vec3(0,0,1), ZAxis);
+ Rotation R_SG(GDir, XAxis, Vec3(0,0,1), ZAxis);
+ Rotation R_SH(HDir, XAxis, Vec3(0,0,1), ZAxis);
+ footInfo.addDecoration(Transform(R_SI, footPivot+ILen/2*IDir),
+ DecorativeBrick(Vec3(ILen/2,LinkWidth,LinkDepth)).setColor(Orange));
+ footInfo.addDecoration(Transform(R_SG, footPivot+GLen/2*GDir),
+ DecorativeBrick(Vec3(GLen/2,LinkWidth,LinkDepth)).setColor(Orange));
+ footInfo.addDecoration(Transform(R_SH, footSide+HLen/2*HDir),
+ DecorativeBrick(Vec3(HLen/2,LinkWidth,LinkDepth)).setColor(Orange));
+
+ const Real FootRad = .05;
+ footInfo.addContactSurface(footLower-Vec3(0,FootRad/2,0),
+ ContactSurface(ContactGeometry::Sphere(FootRad), rubber));
+ footInfo.addDecoration(footLower-Vec3(0,FootRad/2,0),
+ DecorativeSphere(FootRad).setColor(Orange));
+
+ // LINKS
+
+ // Link C connects foot to pivot point. Start aligned with Y.
+ const Vec3 CDims(LinkWidth,CLen/2,LinkDepth);
+ Body::Rigid linkCInfo(MassProperties(.1,Vec3(0), UnitInertia::brick(CDims)));
+ linkCInfo.addDecoration(Vec3(0), DecorativeBrick(CDims).setColor(Orange));
+
+ // Link J connects upper shoulder to crank. Start aligned with X.
+ const Vec3 JDims(JLen/2,LinkWidth,LinkDepth);
+ Body::Rigid linkJInfo(MassProperties(.1,Vec3(0), UnitInertia::brick(JDims)));
+ linkJInfo.addDecoration(Vec3(0), DecorativeBrick(JDims).setColor(Orange));
+
+ // Link F connects shoulder to foot. Start aligned with Y.
+ const Vec3 FDims(LinkWidth,FLen/2,LinkDepth);
+ Body::Rigid linkFInfo(MassProperties(.1,Vec3(0), UnitInertia::brick(FDims)));
+ linkFInfo.addDecoration(Vec3(0), DecorativeBrick(FDims).setColor(Orange));
+
+
+ // Link K connects foot to crank. Start aligned with X.
+ const Vec3 KDims(KLen/2,LinkWidth,LinkDepth);
+ Body::Rigid linkKInfo(MassProperties(.1,Vec3(0), UnitInertia::brick(KDims)));
+ linkKInfo.addDecoration(Vec3(0), DecorativeBrick(KDims).setColor(Orange));
+
+ // Create the tree of mobilized bodies.
+ MobilizedBody::Pin shoulder(torso, Transform(X_TL.R(), pivotPt),
+ shoulderInfo, shoulderPivot);
+ MobilizedBody::Pin linkC(torso, Transform(X_TL.R(), pivotPt),
+ linkCInfo, Vec3(0,CLen/2,0));
+ MobilizedBody::Pin foot( linkC, Vec3(0,-CLen/2,0),
+ footInfo, footPivot);
+
+ #ifdef USE_MASSLESS_LINKS
+ Vec3 crankAttach(crankConnect[0],crankConnect[1],X_TL.p()[2]);
+ DecorativeLine line; line.setColor(Gray).setLineThickness(3);
+ Constraint::Rod linkF(shoulder, shoulderSide, foot, footSide, FLen);
+ viz.addRubberBandLine(shoulder, shoulderSide, foot, footSide, line);
+ Constraint::Rod linkJ(shoulder, shoulderUpper, crank, crankAttach, JLen);
+ viz.addRubberBandLine(shoulder, shoulderUpper, crank, crankAttach, line);
+ Constraint::Rod linkK(foot, footPivot, crank, crankAttach, KLen);
+ viz.addRubberBandLine(foot, footPivot, crank, crankAttach, line);
+ #else
+ MobilizedBody::Pin linkJ(shoulder, shoulderUpper,
+ linkJInfo, Vec3(-JLen/2,0,0));
+ MobilizedBody::Pin linkF(shoulder, shoulderSide,
+ linkFInfo, Vec3(0,FLen/2,0));
+ MobilizedBody::Pin linkK(foot, footPivot,
+ linkKInfo, Vec3(-KLen/2,0,0));
+ linkJ.setDefaultAngle(-Pi/6); // set default angles to guide assembly
+ linkK.setDefaultAngle( Pi/4);
+
+
+ // Add 2d pin joint constraints (each pair of point-in-planes is a pin).
+ Constraint::PointInPlane f2footx(foot, XAxis, footSide[0],
+ linkF, Vec3(0,-FLen/2,0));
+ Constraint::PointInPlane f2footy(foot, YAxis, footSide[1],
+ linkF, Vec3(0,-FLen/2,0));
+
+ Constraint::PointInPlane k2crankx(crank, XAxis, crankConnect[0],
+ linkK, Vec3(KLen/2,0,0));
+ Constraint::PointInPlane k2cranky(crank, YAxis, crankConnect[1],
+ linkK, Vec3(KLen/2,0,0));
+
+ Constraint::PointInPlane j2crankx(crank, XAxis, crankConnect[0],
+ linkJ, Vec3(JLen/2,0,0));
+ Constraint::PointInPlane j2cranky(crank, YAxis, crankConnect[1],
+ linkJ, Vec3(JLen/2,0,0));
+ #endif
+}
+
+
+//==============================================================================
+// USER INPUT HANDLER
+//==============================================================================
+UserInputHandler::UserInputHandler(Visualizer::InputSilo& silo,
+ const Motion::Steady& motor,
+ Real interval)
+: PeriodicEventHandler(interval), m_silo(silo), m_motor(motor) {}
+
+void UserInputHandler::handleEvent(State& state, Real accuracy,
+ bool& shouldTerminate) const {
+ while (m_silo.isAnyUserInput()) {
+ unsigned key, modifiers;
+ while (m_silo.takeKeyHit(key,modifiers))
+ if (key == Visualizer::InputListener::KeyEsc) {
+ shouldTerminate = true;
+ m_silo.clear();
+ return;
+ }
+
+ int whichSlider; Real sliderValue;
+ while (m_silo.takeSliderMove(whichSlider, sliderValue))
+ if (whichSlider == SpeedControlSlider)
+ m_motor.setRate(state, sliderValue);
+ }
+}
+
+
+//==============================================================================
+// FIND OTHER VERTEX
+//==============================================================================
+// Given two vertices v1, v2 of a triangle in the x-y plane and the lengths of
+// the other two sides, find the location of the third vertex, assuming
+// v1-v2-v3 have counterclockwise ordering about the plane normal.
+//
+// v2
+// *
+// \
+// . \ s0
+// \
+// s2 \
+// a * v1
+// . .
+// s1
+// .
+// *
+// v3
+//
+// Our strategy will be to find the angle a and rotate the unit vector
+// v=(v2-v1)/|v2-v1| ccw by a, giving unit vector w along v1v3. Then v3=v1+s1*w.
+
+// Ignore z component of vectors -- we're working in x-y plane.
+Vec3 findOtherVertex(const Vec3& v1, const Vec3& v2,
+ Real s1, Real s2)
+{
+ const Real s0 = (v2-v1).norm();
+ const Real ca = (s0*s0 + s1*s1 - s2*s2) / (2*s0*s1); // cos(a)
+ const Real a = std::acos(ca);
+ const Real sa = std::sin(a);
+ const Mat22 R(ca, -sa, // 2d rotation matrix
+ sa, ca);
+ const Vec2 v = (v2.drop1(2)-v1.drop1(2))/s0;
+ const Vec2 w = R*v;
+ const Vec2 v3 = v1.drop1(2) + s1*w;
+ return v3.append1(0);
+}
+
+//==============================================================================
+// DUMP INTEGRATOR STATS
+//==============================================================================
+void dumpIntegratorStats(double startCPU, double startTime,
+ const Integrator& integ) {
+ std::cout << "DONE: Simulated " << integ.getTime() << " seconds in " <<
+ realTime()-startTime << " elapsed s, CPU="<< cpuTime()-startCPU << "s\n";
+ #ifdef ANIMATE
+ printf("***CAUTION: CPU time not accurate when animation is enabled.\n");
+ #endif
+
+ const int evals = integ.getNumRealizations();
+ std::cout << "\nUsed " << integ.getNumStepsTaken() << " steps, avg step="
+ << (1000*integ.getTime())/integ.getNumStepsTaken() << "ms "
+ << (1000*integ.getTime())/evals << "ms/eval\n";
+
+ printf("Used Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(),
+ integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(),
+ integ.getNumProjections());
+}
+}
\ No newline at end of file
diff --git a/Simbody/include/SimTKsimbody.h b/Simbody/include/SimTKsimbody.h
new file mode 100644
index 0000000..8e83450
--- /dev/null
+++ b/Simbody/include/SimTKsimbody.h
@@ -0,0 +1,76 @@
+#ifndef SimTK_SIMBODY_SimTKSIMBODY_H_
+#define SimTK_SIMBODY_SimTKSIMBODY_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. *
+ * *
+ * Please cite: *
+ * Michael A. Sherman, Ajay Seth, Scott L. Delp, Simbody: multibody *
+ * dynamics for biomedical research, Procedia IUTAM 2:241-261 (2011) *
+ * http://dx.doi.org/10.1016/j.piutam.2011.04.023. *
+ * *
+ * Portions copyright (c) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: Jack Middleton, Christopher Bruns, Paul Mitiguy, Matthew *
+ * Millard, Charles Schwieters, Abhinandan Jain, Isaac Newton *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+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
+will include this one). **/
+
+// This should be kept self-contained for backwards compatibility since
+// in releases prior to Simbody 2.2 users were told to include "SimTKsimbody.h"
+// rather than the now-preferred "Simbody.h".
+
+#include "SimTKcommon.h"
+#include "SimTKmath.h"
+#include "simbody/internal/common.h"
+#include "simbody/internal/MultibodySystem.h"
+#include "simbody/internal/Body.h"
+#include "simbody/internal/Motion.h"
+#include "simbody/internal/MobilizedBody.h"
+#include "simbody/internal/MobilizedBody_BuiltIns.h"
+#include "simbody/internal/Constraint.h"
+#include "simbody/internal/ElasticFoundationForce.h"
+#include "simbody/internal/Force.h"
+#include "simbody/internal/Force_BuiltIns.h"
+#include "simbody/internal/ForceSubsystem.h"
+#include "simbody/internal/ForceSubsystemGuts.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "simbody/internal/SimbodyMatterSubtree.h"
+#include "simbody/internal/GeneralContactSubsystem.h"
+#include "simbody/internal/GeneralForceSubsystem.h"
+#include "simbody/internal/HuntCrossleyContact.h"
+#include "simbody/internal/HuntCrossleyForce.h"
+#include "simbody/internal/DecorationSubsystem.h"
+#include "simbody/internal/TextDataEventReporter.h"
+#include "simbody/internal/ObservedPointFitter.h"
+#include "simbody/internal/Assembler.h"
+#include "simbody/internal/LocalEnergyMinimizer.h"
+#include "simbody/internal/ContactTrackerSubsystem.h"
+#include "simbody/internal/CompliantContactSubsystem.h"
+#include "simbody/internal/CableTrackerSubsystem.h"
+#include "simbody/internal/CablePath.h"
+#include "simbody/internal/CableSpring.h"
+#include "simbody/internal/Visualizer.h"
+#include "simbody/internal/Visualizer_InputListener.h"
+#include "simbody/internal/Visualizer_Reporter.h"
+
+#endif // SimTK_SIMBODY_SimTKSIMBODY_H_
diff --git a/Simbody/include/SimTKsimbody_aux.h b/Simbody/include/SimTKsimbody_aux.h
new file mode 100644
index 0000000..e02fcfa
--- /dev/null
+++ b/Simbody/include/SimTKsimbody_aux.h
@@ -0,0 +1,12 @@
+#ifndef SimTK_SIMBODY_SimTKSIMBODY_AUX_H_
+#define SimTK_SIMBODY_SimTKSIMBODY_AUX_H_
+
+/** @file
+Obsolete -- this header file is obsolete as of Simbody 2.2. This stub
+version remains to allow old programs that included "SimTKsimbody_aux.h"
+to continue to compile for now. This header will be removed in a future
+release; you should just include "Simbody.h". **/
+
+#include "Simbody.h"
+
+#endif // SimTK_SIMBODY_SimTKSIMBODY_AUX_H_
diff --git a/Simbody/include/Simbody.h b/Simbody/include/Simbody.h
new file mode 100644
index 0000000..1457468
--- /dev/null
+++ b/Simbody/include/Simbody.h
@@ -0,0 +1,42 @@
+#ifndef SimTK_SIMBODY_SIMBODY_H_
+#define SimTK_SIMBODY_SIMBODY_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. *
+ * *
+ * Please cite: *
+ * Michael A. Sherman, Ajay Seth, Scott L. Delp, Simbody: multibody *
+ * dynamics for biomedical research, Procedia IUTAM 2:241-261 (2011) *
+ * http://dx.doi.org/10.1016/j.piutam.2011.04.023. *
+ * *
+ * Portions copyright (c) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: Jack Middleton, Christopher Bruns, Paul Mitiguy, Matthew *
+ * Millard, Charles Schwieters, Abhinandan Jain, Isaac Newton *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+This is the header file that user code should include to pick up all Simbody
+capabilities.\ Note that all symbols defined here will be in the SimTK
+namespace, or (where a namespace can't be used) prefixed by "SimTK_". **/
+
+#include "SimTKcommon.h"
+#include "SimTKmath.h"
+#include "SimTKsimbody.h"
+
+#endif // SimTK_SIMBODY_SIMBODY_H_
diff --git a/Simbody/include/simbody/internal/Assembler.h b/Simbody/include/simbody/internal/Assembler.h
new file mode 100644
index 0000000..8808ded
--- /dev/null
+++ b/Simbody/include/simbody/internal/Assembler.h
@@ -0,0 +1,1445 @@
+#ifndef SimTK_SIMBODY_ASSEMBLER_H_
+#define SimTK_SIMBODY_ASSEMBLER_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) 2010-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/MultibodySystem.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+
+#include <set>
+#include <map>
+#include <cassert>
+#include <cmath>
+
+namespace SimTK {
+
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(AssemblyConditionIndex);
+
+class AssemblyCondition;
+
+/** This Study attempts to find a configuration (set of joint coordinates q)
+of a Simbody MultibodySystem that satisfies the System's position Constraints
+plus optional additional assembly conditions. If successful, the final set
+of q's will satisfy the constraints to within a specified tolerance. The
+Assembler also supports high-performance repeated assembly (also known as
+"inverse kinematics" or "tracking") where only small changes are expected
+between a series of observation frames.
+
+The complete specification for an Assembly study consists of four elements:
+ - The subset of q's which may be modified by the study.
+ - Limits on the allowable range of values for each q.
+ - A set of assembly error conditions that \e must be satisfied.
+ - A set of weighted assembly goals that are to be achieved as best we can.
+
+By default, all q's may be modified with no range restrictions. The q's whose
+value is a prescribed function of time will be set to that value, while free
+q's are available for satisfying the assembly error conditions and goals. The
+assembly error conditions are just the errors in the position (holonomic)
+constraints that are present in the MultibodySystem and currently enabled.
+(Quaternion normalization constraints will also be satisfied, but do not
+generate assembly errors.) There are no default assembly goals. This is very
+similiar in behavior to the System's project() method except that project()
+considers it an error if the constraints aren't already close to being
+satisfied initially, while Assembler will attempt to satisfy them regardless,
+and may take a series of increasingly desperate measures to do so.
+
+<h2>Basic assembly:</h2>
+This is the most common use of the Assembler: modify a System's given State
+so that its configuration (set of generalized coordinates q) satisfies the
+position-affecting Motion and Constraint objects in the System that are
+currently enabled in that State. This is done to a default tolerance if you
+don't provide one, and that tolerance is suitable for use with subsequent
+dynamic studies that are run at their default tolerances. Although the assembly
+begins from the configuration provided in the initial state, no assumption is
+made about how close this initial configuration is to one that satisfies the
+assembly conditions.
+ at code
+ MultibodySystem system;
+ // ... build system; get initial state
+
+ Assembler assembler(system); // construct the Assembler study object
+ try // modify state to satisfy Constraints
+ { assembler.assemble(state); }
+ catch (const std::exception& exc)
+ { std::cout << "Assembly failed: " << exc.what() << std::endl; }
+ at endcode
+
+<h2>Inverse kinematics (repeated assembly):</h2>
+After the initial assembly done as above, an inverse kinematic study consists
+of a series of assembly solutions for a sequence of small changes to the
+assembly conditions. A common example is the tracking of a time series of
+marker observations. Thus each "tracking" assembly computation may assume:
+ - There has been no change to the problem structure.
+ - The internal State is already close to the desired solution and has not
+ been changed since the last tracking frame solution.
+
+Allowable (gradual) changes between tracking frames are:
+ - Time, which may affect prescribed motion or other time-dependent
+ Constraints as well as time-dependent assembly conditions.
+ - Weights on assembly goals. Continuous changes to goal weights are
+ permitted, but changes between 0 and nonzero or between finite and
+ Infinity should not be made because these are structural changes
+ to the form of the problem and require reinitialization.
+ - Any changes allowed by the assembly conditions currently included in this
+ Assembler, for example marker location observations for the Markers
+ assembly condition.
+
+The track() method performs assembly analysis under these assumptions and can
+be much faster than assemble(). Here is an outline of code that performs
+repeated tracking of data from a series of observation frames, each associated
+with a frame time:
+ at code
+ MultibodySystem system;
+ // ... build system; get initial state
+ Assembler assembler(system); // construct the Assembler Study object
+ // ... set up assembly conditions; perform initial assemble() as above;
+ // assume assembled result is in State myState.
+
+ try // track a series of small changes to the assembly conditions
+ { for (int i=0; i < numFrames; ++i) {
+ // ... update assembly conditions for frame[i]
+ assembler.track(frameTime[i]);
+ assembler.updateFromInternalState(myState); // update time and qs
+ // ... do something with the results in myState
+ }
+ }
+ catch (const std::exception& exc)
+ { std::cout << "Tracking failed: " << exc.what() << std::endl; }
+ at endcode
+
+<h2>Optional settings:</h2>
+Optional settings include:
+ - Locking particular mobilizers so that their q's can't be changed.
+ - Setting bounds on the acceptable range of values for some of the q's.
+ - Defining additional assembly conditions.
+ - Assigning assembly conditions to be errors ("needs") or weighted goals
+ ("wants").
+
+Assembly errors are specified by giving an assembly condition a weight of
+Infinity. Anything with a lower weight is a goal and will be combined with all
+the other goals into a single scalar objective. The built-in Constraints are
+normally treated with infinite weight, but you can change them to goals instead
+if you like; sometimes that can be useful as a step in getting a difficult-to-
+assemble system assembled.
+**/
+class SimTK_SIMBODY_EXPORT Assembler : public Study {
+ typedef std::set<MobilizedBodyIndex> LockedMobilizers;
+ typedef std::set<MobilizerQIndex> QSet;
+ typedef std::map<MobilizedBodyIndex, QSet> LockedQs;
+ typedef std::map<MobilizerQIndex, Vec2> QRanges;
+ typedef std::map<MobilizedBodyIndex, QRanges> RestrictedQs;
+public:
+
+/** Assembler::FreeQIndex is a unique integer type used for accessing
+the subset of q's that the Assembler is permitted to change. **/
+SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(Assembler,FreeQIndex);
+
+/** This class is the exception object thrown when a call to assemble()
+is unable to achieve the required error tolerance. It is derived from
+std::exception so details can be obtained via the what() method. **/
+class AssembleFailed;
+/** This class is the exception object thrown when a call to track()
+is unable to achieve the required error tolerance. It is derived from
+std::exception so details can be obtained via the what() method. **/
+class TrackFailed;
+
+/** @name Construction and setup
+By default, the only assembly condition is that any Simbody Constraints
+in the System that are enabled must be satisifed to within the assembly
+tolerance. (Prescribed motions specified with Motion objects are satisfied
+exactly.) You can selectively enable and disable Constraints in the
+state using the ordinary Constraint::disable() and enable() methods. You
+can also apply an overall weighting to these Constraints here if you want;
+if the weight is zero they will be ignored; if Infinity they are treated
+as must-satisfy assembly error conditions; any other number is used to
+weight the RMS Constraint error into the scalar objective along
+with the other goals. Additional assembly conditions may be specified with
+these methods; some predefined conditions are available, most notably
+Markers for tracking observed marker locations. **/
+/*@{*/
+/** Create an Assembler study for the given MultibodySystem. The
+Assembler's current state is set to the System's default state but with
+Euler angles used instead of quaternions. **/
+explicit Assembler(const MultibodySystem& system);
+
+/** Set the assembly error tolerance. This value is tested against a norm
+of all the assembly error conditions to determine whether an assemble() or
+track() operation was successful. Note that assembly errors may have
+arbitrary units (for example, built in Constraint errors may be distances
+or angles); in general they must be scaled so that the same tolerance
+value can be used for all of them. By default, tolerance is set to
+accuracy/10 if accuracy has been set, otherwise 1e-4; calling
+setErrorTolerance() with no argument or with zero restores it to its default
+behavior. **/
+Assembler& setErrorTolerance(Real tolerance=0) {
+ SimTK_ERRCHK1_ALWAYS(0 <= tolerance,
+ "Assembler::setTolerance()", "The requested error tolerance %g"
+ " is illegal; we require 0 <= tolerance, with 0 indicating that"
+ " the default tolerance (accuracy/10) is to be used.", tolerance);
+ this->tolerance = tolerance;
+ return *this;
+}
+/** Obtain the tolerance setting that will be used during the next
+assemble() or track() call. Note that this may be an explicitly-set
+tolerance or a default value calculated as accuracy/10 if accuracy has been
+set, otherwise 1e-4. **/
+Real getErrorToleranceInUse() const {
+ return tolerance > 0 ? tolerance
+ : (accuracy > 0 ? accuracy/10 : Real(0.1)/OODefaultAccuracy);
+}
+
+/** Set the accuracy to which a solution should be pursued. This is a
+unitless value that is roughly interpreted as a request for a certain
+number of "correct" digits in the "answer", so that an accuracy of 0.001
+means "1/10 of 1 percent" or roughly three digits, meaning that we would
+like the assembly to stop when the goal is within 0.1% of its minimum.
+However, if you don't say otherwise, this number is also used to set the
+absolute error tolerance used to determine whether the assembly succeeded
+or failed, by the following formula: error tolerance = accuracy/10. By
+default, we set accuracy=1e-3 and tolerance=1e-4. **/
+Assembler& setAccuracy(Real accuracy=0) {
+ SimTK_ERRCHK2_ALWAYS(0 <= accuracy && accuracy < 1,
+ "Assembler::setAccuracy()", "The requested accuracy %g is illegal;"
+ " we require 0 <= accuracy < 1, with 0 indicating that the default"
+ " accuracy (%g) is to be used.", Real(1)/OODefaultAccuracy, accuracy);
+ this->accuracy = accuracy;
+ return *this;
+}
+/** Obtain the accuracy setting that will be used during the next
+assemble() or track() call. The default is to use 1e-3, i.e., 1/10 of 1%. **/
+Real getAccuracyInUse() const
+{ return accuracy > 0 ? accuracy : Real(1)/OODefaultAccuracy; }
+
+
+/** Change how the System's enabled built-in Constraints are weighted as
+compared to other assembly conditions. If this is Infinity (the default) then
+the built-ins are treated as must-satisfy constraints; otherwise they are
+included in the assembly cost function with the given weight. If the weight is
+given as zero the built-in Constraints will be ignored altogether.
+ at see setAssemblyConditionWeight() **/
+Assembler& setSystemConstraintsWeight(Real weight)
+{ assert(systemConstraints.isValid());
+ setAssemblyConditionWeight(systemConstraints,weight);
+ return *this; }
+
+/** Return the current weight being given to the System's built-in
+Constraints; the default is Infinity.
+ at see getAssemblyConditionWeight() **/
+Real getSystemConstraintsWeight() const
+{ assert(systemConstraints.isValid());
+ return getAssemblyConditionWeight(systemConstraints); }
+
+/** Set the weight to be used for this AssemblyCondition. If the weight is set
+to 0, this condition will be disabled and will be ignored. If the weight is
+set to Infinity, the condition will be treated as an assembly error condition
+that must be satisfied to tolerance. Otherwise (finite weight) the condition
+will be treated as an assembly goal and the weight will be used to combine its
+cost function with that of the other assembly goals. **/
+Assembler& setAssemblyConditionWeight(AssemblyConditionIndex condition,
+ Real weight) {
+ SimTK_INDEXCHECK_ALWAYS(condition, conditions.size(),
+ "Assembler::setAssemblyConditionWeight()");
+ SimTK_ERRCHK1_ALWAYS(weight >= 0, "Assembler::setAssemblyConditionWeight()",
+ "Illegal weight %g; weight must be nonnegative.", weight);
+ uninitialize();
+ weights[condition] = weight;
+ return *this;
+}
+
+/** Return the weight currently in use for this AssemblyCondition. If the
+returned value is 0, this condition is being ignored. If the weight is
+Infinity, then the condition is being treated as an assembly error condition
+that must be satisfied to tolerance. Otherwise (finite weight) this is an
+assembly goal and the weight is used to combine its cost function with that
+of the other assembly goals. **/
+Real getAssemblyConditionWeight(AssemblyConditionIndex condition) const {
+ SimTK_INDEXCHECK_ALWAYS(condition, conditions.size(),
+ "Assembler::getAssemblyConditionWeight()");
+ return weights[condition];
+}
+
+/** Add an assembly error condition to this Assembler study, taking over
+ownership of the heap-allocated AssemblyCondition object. We will use the
+calcErrors() method of this object to determine errors whose norm \e must be
+driven below tolerance for an assembly to be considered successful. **/
+AssemblyConditionIndex
+ adoptAssemblyError(AssemblyCondition* p);
+/** Add an assembly goal to this Assembler study, taking over ownership
+of the heap-allocated AssemblyCondition object. We will use normally use the
+calcGoal() method of this object to calculate its contribution to the
+assembly goal cost function. An optional weight can be provided that is used
+when combining this cost with those of other goals to form the overall cost
+function; the default weight is 1. If the weight is 0 the goal is ignored and
+not evaluated at all; if the weight is Infinity this is actually an assembly
+constraint and we'll use its calcErrors() method instead. **/
+AssemblyConditionIndex
+ adoptAssemblyGoal(AssemblyCondition* p, Real weight=1);
+
+
+/** Set the Assembler's internal state from an existing state which must
+be suitable for use with the Assembler's System as supplied at the time
+the Assembler was constructed. All variables are copied, not just q's, so
+the Assembler must be reinitialized after this call in case modeling
+options, instance variables, or time have changed. **/
+Assembler& setInternalState(const State& state) {
+ uninitialize();
+ getMatterSubsystem().convertToEulerAngles(state, internalState);
+ system.realizeModel(internalState);
+ return *this;
+}
+/** Initialize the Assembler to prepare for performing assembly analysis.
+This is normally called automatically when assemble() is called, but you
+can call it explicitly and then access methods that report on the
+properties of the system on which the analysis will be performed. The
+internal state should already have been set; if you want to provide the
+state now use initialize(State). **/
+void initialize() const;
+/** Set the internal State and initialize. See setInternalState() and
+initialize() methods for more information. **/
+void initialize(const State& state)
+{ setInternalState(state); initialize(); }
+/*@}*/
+
+/** @name Execution
+These methods perform assembly or tracking analysis, determine how
+successful they were, and obtain results. **/
+/*@{*/
+
+/** Starting with the current value of the internally-maintained State,
+modify the q's in it to satisfy all the assembly conditions to within a
+tolerance. The actual tolerance achieved is returned as the function value.
+ at return The goal value actually achieved (not the error norm; that is
+guaranteed to be no greater than the error tolerance if this returns at
+all. **/
+Real assemble();
+
+/** Continue a series of assembly steps that is already in progress,
+without restarting or reanalyzing the system, and optionally providing
+a new frame time. This is designed for use with a series of assembly
+frames that are close together so that no heroic measures are needed to
+go from one to the next. For the first frame, and any time there might be
+a change to the problem structure or a major change to the state, use
+assemble() instead of track(). See the Assembler class documentation for
+more information and usage examples. **/
+Real track(Real frameTime = -1);
+
+/** Given an initial value for the State, modify the q's in it to satisfy
+all the assembly conditions to within a tolerance. The actual tolerance
+achieved is returned as the function value.
+ at param[in,out] state
+ The initial and final State value. Only q's are modified.
+ at return The tolerance actually achieved. **/
+Real assemble(State& state) {
+ setInternalState(state);
+ Real achievedCost = assemble(); // throws if it fails
+ updateFromInternalState(state);
+ return achievedCost;
+}
+
+
+/** Return the goal value attained by the internal State's current settings
+for the free q's; this is a weighted sum of the individual goal values for
+each assembly goal. Goal values are nonnegative scalars. **/
+Real calcCurrentGoal() const;
+/** This is the weighted norm of the assembly constraint errors directly
+comparable with the assembly error tolerance setting. That is, if this
+number is less than or equal to tolerance (as returned by
+getErrorToleranceInUse()), then the current state is a feasible
+assembly solution (although it may not be optimal). Note that by default
+we use the infinity norm (maximum absolute value of any error term) but
+that you can specify use of an RMS norm instead via setUseRMSErrorNorm().
+ at see getErrorToleranceInUse(), setUseRMSErrorNorm() **/
+Real calcCurrentErrorNorm() const;
+
+
+/** Given an existing State that is suitable for the Assembler's System,
+update its q's from those found in the Assembler's internal State, leaving
+everything else unchanged. We will convert from Euler angles to quaternions
+if the destination State is set to use quaternions. **/
+void updateFromInternalState(State& state) const {
+ system.realizeModel(state); // allocates q's if they haven't been yet
+ if (!getMatterSubsystem().getUseEulerAngles(state)) {
+ State tempState;
+ getMatterSubsystem().convertToQuaternions(getInternalState(),
+ tempState);
+ state.updQ() = tempState.getQ();
+ } else
+ state.updQ() = getInternalState().getQ();
+}
+/*@}*/
+
+/** @name Parameter restrictions
+These methods restrict which q's are allowed to be modified while trying
+to assemble the system, or restrict the range within which the final q's
+must lie. A prescribed mobilizer is always treated as locked; its q's
+are set to their prescribed values and are not changed to satisfy assembly
+conditions. **/
+/*@{*/
+
+/** Lock this mobilizer at its starting position. This overrides any
+individual q specifications, so even if a q was specifically unlocked it
+will not move until the mobilizer as a whole is unlocked. **/
+void lockMobilizer(MobilizedBodyIndex mbx)
+{ uninitialize(); userLockedMobilizers.insert(mbx); }
+/** Unlock this mobilizer as a whole; some of its q's may remain locked
+if they were locked individually. It is OK if this mobilizer was already
+unlocked; in that case this does nothing. Attempts to unlock a prescribed
+mobilizer have no effect; you must disable the mobilizer's Motion object
+instead. **/
+void unlockMobilizer(MobilizedBodyIndex mbx)
+{ uninitialize(); userLockedMobilizers.erase(mbx); }
+
+/** Lock one of this mobilizer's q's at its initial value. Be careful with
+this method because it requires that you understand the order of the
+generalized coordinates used by this particular mobilizer during assembly.
+In particular, the mobilizer will be modeled with Euler angles rather than
+quaternions and you must know the Euler sequence it uses in that case (that
+is, body- or space-fixed, 2 or 3 axes, and the rotation order). It is
+preferable to use lockMobilizer() instead since that will lock all the q's
+however they are defined. Note that locking individual q's with this method
+is independent of whole-mobilizer locking. If you unlock the mobilizer with
+unlockMobilizer(), any q's which have been explicitly locked with lockQ()
+will remain locked. **/
+void lockQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
+{ uninitialize(); userLockedQs[mbx].insert(qx); }
+
+/** Unlock one of this mobilizer's q's if it was locked. Note that this will
+not take effect immediately if the mobilizer as a whole has been locked with
+lockMobilizer(); you have to unlockMobilizer() first. This has no effect on
+a prescribed q. **/
+void unlockQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
+{ LockedQs::iterator p = userLockedQs.find(mbx);
+ if (p == userLockedQs.end()) return;
+ QSet& qs = p->second;
+ if (qs.erase(qx)) { // returns 0 if nothing erased
+ uninitialize();
+ if (qs.empty())
+ userLockedQs.erase(p); // remove the whole mobilized body
+ }
+}
+
+/** Restrict a q to remain within a given range. Caution: this requires that
+you understand the order of the generalized coordinates used by this
+particular mobilizer during assembly; see lockQ() for a discussion. You can
+use -Infinity or Infinity to indicate that the q is not bounded in one
+direction. This has no effect on a prescribed q. **/
+void restrictQ(MobilizedBodyIndex mbx, MobilizerQIndex qx,
+ Real lowerBound, Real upperBound)
+{ SimTK_ERRCHK2_ALWAYS(lowerBound <= upperBound, "Assembler::restrictQ()",
+ "The given range [%g,%g] is illegal because the lower bound is"
+ " greater than the upper bound.", lowerBound, upperBound);
+ if (lowerBound == -Infinity && upperBound == Infinity)
+ { unrestrictQ(mbx,qx); return; }
+ uninitialize();
+ userRestrictedQs[mbx][qx] = Vec2(lowerBound,upperBound);
+}
+
+
+/** Unrestrict a particular generalized coordinate q if it was previously
+restricted. Note that this is independent of whether the q has been locked
+with lockMobilizer() or lockQ(); that is, the q may still be locked even
+though it is now unrestricted. This has no effect on a prescribed q. **/
+void unrestrictQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
+{ RestrictedQs::iterator p = userRestrictedQs.find(mbx);
+ if (p == userRestrictedQs.end()) return;
+ QRanges& qranges = p->second;
+ if (qranges.erase(qx)) { // returns 0 if nothing erased
+ uninitialize();
+ if (qranges.empty())
+ userRestrictedQs.erase(p); // remove the whole mobilized body
+ }
+}
+/*@}*/
+
+
+
+/** @name Statistics
+The Assembler keeps counters of various internal operations it performs
+during execution; these methods access those counters. These can be helpful
+in evaluating the effects of various ways of structuring the assembly or
+tracking problem. Counters can also be reset to zero manually by calling
+resetStats(). **/
+/*@{*/
+/** Return the number of goal evaluations. **/
+int getNumGoalEvals() const;
+/** Return the number of assembly error condition evaluations. **/
+int getNumErrorEvals() const;
+/** Return the number of goal gradient evaluations. **/
+int getNumGoalGradientEvals() const;
+/** Return the number of assembly error condition Jacobian evaluations. **/
+int getNumErrorJacobianEvals() const;
+/** Return the number of assembly steps; that is, the number of calls to
+assemble() or track() since last initialization. **/
+int getNumAssemblySteps() const;
+/** Return the number of system initializations performed since this
+Assembler was created or the most recent resetStats() call. **/
+int getNumInitializations() const;
+/** Reset all counters to zero; except for the number of initializations
+counter this also happens whenever the assembler system is reinitialized
+either explicitly or due to system changes. **/
+void resetStats() const;
+/*@}*/
+
+
+/** @name Advanced options
+These are primarily useful for debugging while developing new
+AssemblyCondition classes. **/
+/*@{*/
+
+/** This is useful for debugging but should not be used otherwise
+since the analytic gradient is to be preferred. **/
+void setForceNumericalGradient(bool yesno)
+{ forceNumericalGradient = yesno; }
+/** This is useful for debugging but should not be used otherwise
+since the analytic Jacobian is to be preferred. **/
+void setForceNumericalJacobian(bool yesno)
+{ forceNumericalJacobian = yesno; }
+
+/** Use an RMS norm for the assembly errors rather than the default
+infinity norm (max absolute value). RMS is less stringent and defines
+success based on on a good "average" case rather than a good worst case.
+If there are n error terms ei, the default norm is e=max_i(abs(ei)) and
+the RMS norm is e=sqrt(sum_i(ei^2)/n). **/
+void setUseRMSErrorNorm(bool yesno)
+{ useRMSErrorNorm = yesno; }
+/** Determine whether we are currently using the RMS norm for constraint
+errors; if not we're using the default infinity norm (max absolute value).
+**/
+bool isUsingRMSErrorNorm() const {return useRMSErrorNorm;}
+
+/** Uninitialize the Assembler. After this call the Assembler must be
+initialized again before an assembly study can be performed. Normally this
+is called automatically when changes are made; you can call it explicitly
+if you want. **/
+void uninitialize() const;
+/** Check whether the Assembler has been initialized since the last change
+was made to its contents. **/
+bool isInitialized() const {return alreadyInitialized;}
+
+/** This provides read-only access to the Assembler's internal State; you
+probably should use updateFromInternalState() to transfer just q's from
+the internal state to your own State. Be aware that the internal state is
+always maintained using Euler angles for rotations rather than quaternions,
+while updateFromInternalState() will make sure you get the rotations in the
+form you want. **/
+const State& getInternalState() const {return internalState;}
+
+/** Given a reference to an EventReporter, use this Reporter to provide
+progress reporting. The EventReporter object must be owned by someone
+else and persist throughout the lifetime of this Assembler object. **/
+void addReporter(const EventReporter& reporter) {
+ reporters.push_back(&reporter);
+}
+
+/** Return the number of q's which are free to be changed by this
+already-initialized assembly analysis. The rest of the q's are locked
+at their initial values or set to their prescribed values. **/
+int getNumFreeQs() const
+{ return freeQ2Q.size(); }
+
+/** Return the absolute q index associated with a free q. Every free q
+is associated with a q so this will always return a valid index if the
+free q index is in range. **/
+QIndex getQIndexOfFreeQ(FreeQIndex freeQIndex) const
+{ return freeQ2Q[freeQIndex]; }
+
+/** A subset of the q's will be used as free q's for solving the assembly
+problem. Given an absolute q index, this will return the corresponding
+free q index if there is one; otherwise, the returned index will be
+invalid meaning that this q is currently locked. **/
+FreeQIndex getFreeQIndexOfQ(QIndex qx) const
+{ return q2FreeQ[qx]; }
+
+/** Return the allowable range for a particular free q. If this free q is
+unrestricted the returned range will be [-Infinity,Infinity]. **/
+Vec2 getFreeQBounds(FreeQIndex freeQIndex) const {
+ if (!lower.size()) return Vec2(-Infinity, Infinity);
+ else return Vec2(lower[freeQIndex], upper[freeQIndex]);
+}
+
+/** Return a reference to the MultibodySystem associated with this
+Assembler (that is, the System that was supplied in the Assembler's
+constructor. **/
+const MultibodySystem& getMultibodySystem() const
+{ return system; }
+/** Return a reference to the SimbodyMatterSubsystem that is contained
+in the MultibodySystem that is associated with this Assembler. **/
+const SimbodyMatterSubsystem& getMatterSubsystem() const
+{ return system.getMatterSubsystem(); }
+/*@}*/
+
+/** Destruct the Assembler objects and any Assembly Condition objects it
+contains. **/
+~Assembler();
+
+
+
+//------------------------------------------------------------------------------
+ private: // methods
+//------------------------------------------------------------------------------
+void setInternalStateFromFreeQs(const Vector& freeQs) {
+ assert(freeQs.size() == getNumFreeQs());
+ Vector& q = internalState.updQ();
+ for (FreeQIndex fx(0); fx < getNumFreeQs(); ++fx)
+ q[getQIndexOfFreeQ(fx)] = freeQs[fx];
+ system.realize(internalState, Stage::Position);
+}
+
+Vector getFreeQsFromInternalState() const {
+ Vector freeQs(getNumFreeQs());
+ const Vector& q = internalState.getQ();
+ for (FreeQIndex fx(0); fx < getNumFreeQs(); ++fx)
+ freeQs[fx] = q[getQIndexOfFreeQ(fx)];
+ return freeQs;
+}
+
+void reinitializeWithExtraQsLocked
+ (const Array_<QIndex>& toBeLocked) const;
+
+
+
+//------------------------------------------------------------------------------
+ private: // data members
+//------------------------------------------------------------------------------
+const MultibodySystem& system;
+Array_<const EventReporter*> reporters; // just references; don't delete
+
+// These members affect the behavior of the assembly algorithm.
+static const int OODefaultAccuracy = 1000; // 1/accuracy if acc==0
+Real accuracy; // 0 means use 1/OODefaultAccuracy
+Real tolerance; // 0 means use accuracy/10
+bool forceNumericalGradient; // ignore analytic gradient methods
+bool forceNumericalJacobian; // ignore analytic Jacobian methods
+bool useRMSErrorNorm; // what norm defines success?
+
+// Changes to any of these data members set isInitialized()=false.
+State internalState;
+
+// These are the mobilizers that were set in lockMobilizer(). They are
+// separate from those involved in individually-locked q's.
+LockedMobilizers userLockedMobilizers;
+// These are locks placed on individual q's; they are independent of the
+// locked mobilizer settings.
+LockedQs userLockedQs;
+// These are range restrictions placed on individual q's.
+RestrictedQs userRestrictedQs;
+
+// These are (condition,weight) pairs with weight==Infinity meaning
+// constraint; weight==0 meaning currently ignored; and any other
+// positive weight meaning a goal.
+Array_<AssemblyCondition*,AssemblyConditionIndex>
+ conditions;
+Array_<Real,AssemblyConditionIndex> weights;
+
+// We always have an assembly condition for the Constraints which are
+// enabled in the System; this is the index which can be used to
+// retrieve that condition. The default weight is Infinity.
+AssemblyConditionIndex systemConstraints;
+
+
+// These are filled in when the Assembler is initialized.
+mutable bool alreadyInitialized;
+
+// These are extra q's we removed for numerical reasons.
+mutable Array_<QIndex> extraQsLocked;
+
+// These represent restrictions on the independent variables (q's).
+mutable std::set<QIndex> lockedQs;
+mutable Array_<FreeQIndex,QIndex> q2FreeQ; // nq of these
+mutable Array_<QIndex,FreeQIndex> freeQ2Q; // nfreeQ of these
+// 0 length if no bounds; otherwise, index by FreeQIndex.
+mutable Vector lower, upper;
+
+// These represent the active assembly conditions.
+mutable Array_<AssemblyConditionIndex> errors;
+mutable Array_<int> nTermsPerError;
+mutable Array_<AssemblyConditionIndex> goals;
+
+class AssemblerSystem; // local class
+mutable AssemblerSystem* asmSys;
+mutable Optimizer* optimizer;
+
+mutable int nAssemblySteps; // count assemble() and track() calls
+mutable int nInitializations; // # times we had to reinitialize
+
+friend class AssemblerSystem;
+};
+
+
+
+//------------------------------------------------------------------------------
+// ASSEMBLY CONDITION
+//------------------------------------------------------------------------------
+/** Define an assembly condition consisting of a scalar goal and/or a
+related set of assembly error equations (that is, an objective and/or some
+constraints). Whether the goal or error is used depends on the weighting
+assigned to this AssemblyCondition. A finite weight indicates that the
+goal should be used (and combined with other goals); an infinite weighting
+means that each error must independently be satisfied to tolerance. **/
+class SimTK_SIMBODY_EXPORT AssemblyCondition {
+public:
+
+/** Base class constructor just takes the assembly condition name and
+saves it. **/
+explicit AssemblyCondition(const String& name)
+: name(name), assembler(0) {}
+
+/** Destructor is virtual for use by derived classes. **/
+virtual ~AssemblyCondition() {}
+
+/** This is called whenever the Assembler is initialized in case this
+assembly condition wants to do some internal work before getting started.
+None of the other virtual methods will be called until this one has been,
+except possibly the destructor. The set of free q's and the internal
+State are valid at this point and can be retrieved from the Assembler
+stored in the base class. **/
+virtual int initializeCondition() const {return 0;}
+
+/** This is called whenever the containing Assembler is uninitialized in
+case this assembly condition has some cleanup to do. **/
+virtual void uninitializeCondition() const {}
+
+/** Calculate the amount by which this assembly condition is violated
+by the q values in the given state, with one scalar error per assembly
+equation returned in \a err. The functional return should be zero if
+successful; negative values are reserved with -1 meaning "not implemented";
+return a positive value if your implementation is unable to evaluate the
+error at the current state. If this method is not implemented then you must
+implement calcGoal() and this assembly condition may only be used as a
+goal, not a requirement. **/
+virtual int calcErrors(const State& state, Vector& err) const
+{ return -1; }
+
+/** Override to supply an analytic Jacobian for the assembly errors
+returned by calcErrors(). The returned Jacobian must be nErr X nFreeQs;
+that is, if there is only one assembly error equation the returned matrix
+is a single row (that's the transpose of the gradient). The functional
+return should be zero if this succeeds; negative values are reserved with
+the default implementation returning -1 which indicates that the
+Jacobian must be calculated numerically using the calcErrors() method.
+Return a positive value if your implementation is unable to evaluate the
+Jacobian at the current state. **/
+virtual int calcErrorJacobian(const State& state, Matrix& jacobian) const
+{ return -1; }
+
+/** Override to supply an efficient method for determining how many errors
+will be returned by calcErrors(). Otherwise the default implementation
+determines this by making a call to calcErrors() and returning the size
+of the returned error vector. The functional return should be zero if this
+succeeds; negative values are reserved; return a positive value if your
+implementation of this method can't determine the number of errors with
+the given state (unlikely!). **/
+virtual int getNumErrors(const State& state) const
+{ Vector err;
+ const int status = calcErrors(state, err);
+ if (status == 0)
+ return err.size();
+ SimTK_ERRCHK1_ALWAYS(status != -1, "AssemblyCondition::getNumErrors()",
+ "The default implementation of getNumErrors() depends on"
+ " calcErrors() but that method was not implemented for assembly"
+ " condition '%s'.", name.c_str());
+ SimTK_ERRCHK2_ALWAYS(status == 0, "AssemblyCondition::getNumErrors()",
+ "The default implementation of getNumErrors() uses calcErrors()"
+ " which returned status %d (assembly condition '%s').",
+ status, name.c_str());
+ return -1; // NOTREACHED
+}
+
+/** Calculate the current contribution (>= 0) of this assembly condition to
+the goal value that is being minimized. If this isn't overridden we'll
+generate it by combining the m errors returned by calcErrors() in a mean
+sum of squares: goal = err^2/m. **/
+virtual int calcGoal(const State& state, Real& goal) const
+{ static Vector err;
+ const int status = calcErrors(state, err);
+ if (status == 0)
+ { goal = err.normSqr() / std::max(1,err.size());
+ return 0; }
+ SimTK_ERRCHK1_ALWAYS(status != -1, "AssemblyCondition::calcGoal()",
+ "The default implementation of calcGoal() depends on calcErrors()"
+ " but that method was not implemented for assembly condition '%s'.",
+ name.c_str());
+ SimTK_ERRCHK2_ALWAYS(status == 0, "AssemblyCondition::calcGoal()",
+ "The default implementation of calcGoal() uses calcErrors() which"
+ " returned status %d (assembly condition '%s').",
+ status, name.c_str());
+ return -1; // NOTREACHED
+}
+
+/** Override to supply an analytic gradient for this assembly condition's
+goal. The returned gradient must be nFreeQ X 1; that is, it is a column
+vector giving the partial derivative of the goal with respect to each of
+the free q's in order. The functional return should be zero if this
+succeeds. The default implementation return -1 which indicates that the
+gradient must be calculated numerically using the calcGoal() method. **/
+virtual int calcGoalGradient(const State& state, Vector& gradient) const
+{ return -1; }
+
+/** Return the name assigned to this AssemblyCondition on construction. **/
+const char* getName() const {return name.c_str();}
+
+/** Test whether this AssemblyCondition has already been adopted by an
+Assembler. **/
+bool isInAssembler() const {return assembler != 0;}
+/** Return the Assembler that has adopted this AssemblyCondition. This will
+throw an exception if there is no such Assembler; use isInAssembler() first
+if you're not sure. **/
+const Assembler& getAssembler() const
+{ assert(assembler); return *assembler;}
+/** Return the AssemblyConditionIndex of this concrete AssemblyCondition
+within the Assembler that has adopted it. This returned index will be
+invalid if this AssemblyCondition has not yet been adopted. **/
+AssemblyConditionIndex getAssemblyConditionIndex() const
+{ return myAssemblyConditionIndex; }
+
+//------------------------------------------------------------------------------
+ protected:
+//------------------------------------------------------------------------------
+// These are useful when writing concrete AssemblyConditions.
+
+/** Ask the assembler how many free q's there are; only valid after
+initialization but does not invoke initialization. **/
+int getNumFreeQs() const {return getAssembler().getNumFreeQs();}
+/** Ask the assembler where to find the actual q in the State that corresponds
+to a given free q; only valid after initialization but does not invoke
+initialization. **/
+QIndex getQIndexOfFreeQ(Assembler::FreeQIndex fx) const
+{ return getAssembler().getQIndexOfFreeQ(fx); }
+/** Ask the assembler where to find the free q (if any) that corresponds
+to a given q in the State; only valid after initialization but does not invoke
+initialization. **/
+Assembler::FreeQIndex getFreeQIndexOfQ(QIndex qx) const
+{ return getAssembler().getFreeQIndexOfQ(qx); }
+/** Ask the assembler for the MultibodySystem with which it is associated. **/
+const MultibodySystem& getMultibodySystem() const
+{ return getAssembler().getMultibodySystem(); }
+/** Ask the assembler for the MultibodySystem with which it is associated
+and extract the SimbodyMatterSubsystem contained therein. **/
+const SimbodyMatterSubsystem& getMatterSubsystem() const
+{ return getMultibodySystem().getMatterSubsystem(); }
+
+/** Call this method before doing anything that logically requires the
+Assembler, or at least this AssemblyCondition, to have been initialized. **/
+void initializeAssembler() const {
+ // The Assembler will in turn invoke initializeCondition().
+ if (isInAssembler()) getAssembler().initialize();
+ else initializeCondition();
+}
+
+/** Call this when modifying any parameter of the concrete AssemblyCondition
+that would require reinitialization of the Assembler or the
+AssemblyCondition. **/
+void uninitializeAssembler() const {
+ // The Assembler will in turn invoke uninitializeCondition().
+ if (isInAssembler()) getAssembler().uninitialize();
+ else uninitializeCondition();
+}
+
+//------------------------------------------------------------------------------
+ private:
+//------------------------------------------------------------------------------
+// This method is used by the Assembler when the AssemblyCondition object
+// is adopted.
+friend class Assembler;
+void setAssembler(const Assembler& assembler, AssemblyConditionIndex acx) {
+ assert(!this->assembler);
+ this->assembler = &assembler;
+ this->myAssemblyConditionIndex = acx;
+}
+
+String name; // assembly condition name
+const Assembler* assembler;
+AssemblyConditionIndex myAssemblyConditionIndex;
+};
+
+
+
+//------------------------------------------------------------------------------
+// Q VALUE
+//------------------------------------------------------------------------------
+/** This AssemblyCondition requests that a particular generalized coordinate
+end up with a specified value. You can use this as a goal or a constraint,
+depending on how serious you are about this requirement. **/
+class QValue : public AssemblyCondition {
+public:
+ /** Construct an assembly condition that requests that the specified
+ generalized coordinate be brought to the indicated value. The value
+ can be changed subsequently using setValue(). **/
+ QValue(MobilizedBodyIndex mbx, MobilizerQIndex qx,
+ Real value)
+ : AssemblyCondition("QValue"),
+ mobodIndex(mbx), qIndex(qx), value(value) {}
+
+ /** Return the currently set value to be used for this generalized
+ coordinate. **/
+ Real getValue() const {return value;}
+ /** Change the value to be used for this generalized coordinate; this
+ can be done repeatedly during tracking to follow changing requirements. **/
+ void setValue(Real newValue) {value=newValue;}
+
+ // For constraint:
+ int getNumEquations(const State&) const {return 1;}
+ int calcErrors(const State& state, Vector& error) const {
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+ const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex);
+ error.resize(1);
+ error[0] = mobod.getOneQ(state, qIndex) - value;
+ return 0;
+ }
+ // Error jacobian is a zero-row except for a 1 in this q's entry (if
+ // this q is free).
+ int calcErrorJacobian(const State& state, Matrix& J) const {
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+ const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex);
+ J.resize(1, getNumFreeQs());
+ J = 0; // will have at most one non-zero
+
+ // Find the FreeQIndex corresponding to this q.
+ const QIndex thisIx = QIndex(mobod.getFirstQIndex(state)+qIndex);
+ const Assembler::FreeQIndex thisFreeIx = getFreeQIndexOfQ(thisIx);
+
+ // If this q isn't free then there is no way to affect the error
+ // so the Jacobian stays all-zero.
+ if (thisFreeIx.isValid())
+ J(0,thisFreeIx) = 1;
+
+ return 0;
+ }
+
+ // For goal: goal = (q-value)^2 / 2 (the /2 is for gradient beauty)
+ int calcGoal(const State& state, Real& goal) const {
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+ const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex);
+ goal = square(mobod.getOneQ(state, qIndex) - value) / 2;
+ return 0;
+ }
+ // Return a gradient with only this q's entry non-zero (if
+ // this q is free).
+ int calcGoalGradient(const State& state, Vector& grad) const {
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+ const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex);
+ grad.resize(getNumFreeQs());
+ grad = 0; // will have at most one non-zero
+
+ // Find the FreeQIndex corresponding to this q.
+ const QIndex thisIx = QIndex(mobod.getFirstQIndex(state)+qIndex);
+ const Assembler::FreeQIndex thisFreeIx = getFreeQIndexOfQ(thisIx);
+
+ // If this q isn't free then there is no way to affect the goal
+ // so the gradient stays all-zero.
+ if (thisFreeIx.isValid())
+ grad[thisFreeIx] = mobod.getOneQ(state, qIndex) - value;
+
+ return 0;
+ }
+
+private:
+ MobilizedBodyIndex mobodIndex;
+ MobilizerQIndex qIndex;
+ Real value;
+};
+
+
+
+//------------------------------------------------------------------------------
+// MARKERS
+//------------------------------------------------------------------------------
+/** This AssemblyCondition specifies a correspondence between stations on
+mobilized bodies ("markers") and fixed ground-frame locations ("observations").
+The idea is to adjust the q's so that each marker is located close to its
+corresponding observation. This is normally used as a goal since we don't
+expect a perfect fit, but you can use these as a set of assembly error
+conditions if there are enough degrees of freedom to achieve a near-perfect
+solution.
+
+Markers are defined one at a time and assigned sequential marker index values
+of type Markers::MarkerIx. They may optionally be given unique, case-sensitive
+names, and we will keep a map from name to MarkerIx. A default name will be
+assigned if none is given. A weight is assigned to every marker, with default
+weight=1. We do not expect that all the markers will be used; markers with
+weights of zero will not be included in the study, nor will markers for which
+no observation is given.
+
+Once specified, the marker definitions do not change during a series of inverse
+kinematic (tracking) steps. The observations, on the other hand, are expected
+to come from a time series of experimental measurements of marker locations and
+will be different at every step. They typically come from a file organized by
+"frame", meaning an observation time and a set of observed locations, one per
+marker, corresponding to that time. During initial setup, the number of
+observations per frame and their correspondence to the defined markers is
+specified. They can be in any order, may skip some markers, and may include
+data for markers that are not defined. However, once initialized each frame
+must supply the same information in the same order. Data for an unobserved
+marker can be provided as NaN in which case it will be ignored in that frame.
+The frame time is supplied to the track() method which initiates assembly for
+a frame.
+
+Observation-marker correspondence maps a ObservationIx to a unique MarkerIx.
+By default, we'll expect to get an observation for each marker and that the
+observation order and the marker order are the same, i.e.
+ObservationIx==MarkerIx for every marker. However, you can instead define
+observation/marker correspondence yourself, (\e after all markers have been
+defined), via one of the defineObservationOrder() methods. This is done by
+supplying an array of MarkerIx values, or an array of Marker names, with the
+array elements ordered by ObservationIx. Any invalid marker index or
+unrecognized marker name means we will ignore values provide for that
+observation; similarly, any markers whose index or name is not specified at
+all will be ignored.
+**/
+class SimTK_SIMBODY_EXPORT Markers : public AssemblyCondition {
+
+// This is a private class used in the implementation below but not
+// accessible through the API.
+struct Marker {
+ Marker(const String& name, MobilizedBodyIndex bodyB,
+ const Vec3& markerInB, Real weight = 1)
+ : name(name), bodyB(bodyB), markerInB(markerInB), weight(weight)
+ { assert(weight >= 0); }
+
+ Marker(MobilizedBodyIndex bodyB, const Vec3& markerInB, Real weight=1)
+ : name(""), bodyB(bodyB), markerInB(markerInB), weight(weight)
+ { assert(weight >= 0); }
+
+ String name;
+ MobilizedBodyIndex bodyB;
+ Vec3 markerInB;
+ Real weight;
+};
+
+public:
+
+/** Define the MarkerIx type which is just a uniquely-typed int. **/
+SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(Markers,MarkerIx);
+/** Define the ObservationIx type which is just a uniquely-typed int. **/
+SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(Markers,ObservationIx);
+
+
+
+//------------------------------------------------------------------------------
+/** @name Construction and setup
+These methods are used as an extended construction phase for Markers
+objects, defining the markers and observations that will be used in the
+subsequent tracking steps. **/
+/*@{*/
+
+/** The default constructor creates an empty Markers AssemblyCondition
+object that should be filled in with calls to addMarker() and optionally
+defineObservationOrder(). **/
+Markers() : AssemblyCondition("Markers") {}
+
+/** Define a new marker attached to a particular MobilizedBody. Note that
+a marker will be ignored unless an observation is provided for it.
+ at param[in] name
+ A unique name to be used to identify this marker. If the name is
+ empty or blank, a default name will be supplied.
+ at param[in] bodyB
+ The MobilizedBody to which this marker is fixed. Markers on Ground
+ are allowed but will be ignored.
+ at param[in] markerInB
+ This is the position vector of the marker in \a bodyB's local frame,
+ also known as the marker's "station" on \a bodyB.
+ at param[in] weight
+ An optional weight for use in defining the objective function, which
+ combines errors in this marker's position with errors in other markers'
+ positions. If the weight is zero this marker is ignored.
+ at return The unique marker index number assigned to this marker. These are
+assigned sequentially as the marker are added.
+ at note Adding a marker invalidates any observation/marker correspondence; be
+sure to call defineObservationOrder() \e after defining all your markers. **/
+MarkerIx addMarker(const String& name, MobilizedBodyIndex bodyB,
+ const Vec3& markerInB, Real weight=1)
+{ SimTK_ERRCHK1_ALWAYS(isFinite(weight) && weight >= 0,
+ "Markers::addMarker()", "Illegal marker weight %g.", weight);
+ uninitializeAssembler();
+ // Forget any previously-established observation/marker correspondence.
+ observation2marker.clear(); marker2observation.clear();
+ observations.clear();
+ const MarkerIx ix(markers.size());
+ String nm = String::trimWhiteSpace(name);
+ if (nm.empty())
+ nm = String("_UNNAMED_") + String(ix);
+
+ std::pair< std::map<String,MarkerIx>::iterator, bool >
+ found = markersByName.insert(std::make_pair(nm,ix));
+ SimTK_ERRCHK2_ALWAYS(found.second, // true if insertion was done
+ "Markers::addMarker()",
+ "Marker name '%s' was already use for Marker %d.",
+ nm.c_str(), (int)found.first->second);
+
+ markers.push_back(Marker(nm,bodyB,markerInB,weight));
+ return ix;
+}
+
+/** Define an unnamed marker. A default name will be assigned; that name
+will be "_UNNAMED_XX" where XX is the MarkerIx assigned to that marker
+(don't use names of that form yourself).
+ at see addMarker(name,...) for more information. **/
+MarkerIx addMarker(MobilizedBodyIndex bodyB, const Vec3& markerInB,
+ Real weight=1)
+{ return addMarker("", bodyB, markerInB, weight); }
+
+
+/** Define the meaning of the observation data by giving the MarkerIx
+associated with each observation. The length of the array of marker indices
+defines the expected number of observations to be provided for each observation
+frame. Any marker index that is supplied with an invalid value means that the
+corresponding observation will be present in the supplied data but should be
+ignored.
+ at param[in] observationOrder
+ This is an array of marker index values, one per observation, that defines
+ both the number of expected observations and the marker corresponding
+ to each observation. Markers can be in any order; an invalid marker index
+ means that observation will be provided but should be ignored;
+ markers whose indices are never listed are ignored. If \a observationOrder
+ is supplied as a zero-length array, then we'll assume there are as
+ many observations as markers and that their indices match.
+
+ at note If you don't call this method at all, a default correspondence will
+be defined as described for a zero-length \a observationOrder array (that is,
+same number of observations and markers with matching indices). Whenever you
+add a new marker, any previously defined observation order is forgotten so the
+default correspondence will be used unless you call this again. **/
+void defineObservationOrder(const Array_<MarkerIx>& observationOrder) {
+ uninitializeAssembler();
+ if (observationOrder.empty()) {
+ observation2marker.resize(markers.size());
+ for (MarkerIx mx(0); mx < markers.size(); ++mx)
+ observation2marker[ObservationIx(mx)] = mx;
+ } else
+ observation2marker = observationOrder;
+ marker2observation.clear();
+ // We might need to grow this more, but this is an OK starting guess.
+ marker2observation.resize(observation2marker.size()); // all invalid
+ for (ObservationIx ox(0); ox < observation2marker.size(); ++ox) {
+ const MarkerIx mx = observation2marker[ox];
+ if (!mx.isValid()) continue;
+
+ if (marker2observation.size() <= mx)
+ marker2observation.resize(mx+1);
+ SimTK_ERRCHK4_ALWAYS(!marker2observation[mx].isValid(),
+ "Markers::defineObservationOrder()",
+ "An attempt was made to associate Marker %d (%s) with"
+ " Observations %d and %d; only one Observation per Marker"
+ " is permitted.",
+ (int)mx, getMarkerName(mx).c_str(),
+ (int)marker2observation[mx], (int)ox);
+
+ marker2observation[mx] = ox;
+ }
+ // Make room for marker observations.
+ observations.clear();
+ observations.resize(observation2marker.size(),Vec3(NaN));
+}
+
+/** Define the meaning of the observations by giving the marker name
+corresponding to each observation, as a SimTK::Array_<String>. The length of
+the array of marker indices defines the expected number of observations. Any
+marker name that is unrecognized or empty means that the corresponding
+observation will be present in the supplied data but should be ignored. **/
+void defineObservationOrder(const Array_<String>& observationOrder)
+{ Array_<MarkerIx> markerIxs(observationOrder.size());
+ for (ObservationIx ox(0); ox < observationOrder.size(); ++ox)
+ markerIxs[ox] = getMarkerIx(observationOrder[ox]);
+ defineObservationOrder(markerIxs); }
+
+/** Define observation order using an std::vector of SimTK::String. */
+// no copy required
+void defineObservationOrder(const std::vector<String>& observationOrder)
+{ defineObservationOrder(ArrayViewConst_<String>(observationOrder)); }
+
+
+/** Define observation order using an Array_ of std::string. */
+// must copy
+void defineObservationOrder(const Array_<std::string>& observationOrder)
+{ const Array_<String> observations(observationOrder); // copy
+ defineObservationOrder(observations); }
+
+/** Define observation order using an std::vector of std::string. */
+// must copy
+void defineObservationOrder(const std::vector<std::string>& observationOrder)
+{ const Array_<String> observations(observationOrder); // copy
+ defineObservationOrder(observations); }
+
+/** Define observation order using a C array of const char* names. */
+void defineObservationOrder(int n, const char* const observationOrder[])
+{ Array_<MarkerIx> markerIxs(n);
+ for (ObservationIx ox(0); ox < n; ++ox)
+ markerIxs[ox] = getMarkerIx(String(observationOrder[ox]));
+ defineObservationOrder(markerIxs); }
+/*@}*/
+
+
+
+//------------------------------------------------------------------------------
+/** @name Retrieve setup information
+These methods are used to query information associated with the construction
+and setup of this Markers object. This information does not normally change
+during a marker-tracking study, although marker weights may be changed by
+some inverse kinematics methods. **/
+/*@{*/
+
+/** Return a count n of the number of currently-defined markers. Valid
+marker index values (of type Markers::MarkerIx) are 0..n-1. **/
+int getNumMarkers() const {return markers.size();}
+
+/** Return the unique marker name assigned to the marker whose index
+is provided. If the marker was defined without a name, this will return
+the default name that was assigned to it. **/
+const String& getMarkerName(MarkerIx ix)
+{ return markers[ix].name; }
+/** Return the marker index associated with the given marker name. If the
+name is not recognized the returned index will be invalid (test with
+index.isValid()). **/
+const MarkerIx getMarkerIx(const String& name)
+{ std::map<String,MarkerIx>::const_iterator p = markersByName.find(name);
+ return p == markersByName.end() ? MarkerIx() : p->second; }
+
+/** Get the weight currently in use for the specified marker; this can
+be changed dynamically via changeMarkerWeight(). **/
+Real getMarkerWeight(MarkerIx mx)
+{ return markers[mx].weight; }
+
+/** Get the MobilizedBodyIndex of the body associated with this marker. **/
+MobilizedBodyIndex getMarkerBody(MarkerIx mx) const
+{ return markers[mx].bodyB; }
+
+/** Get the station (fixed location in its body frame) of the given marker. **/
+const Vec3& getMarkerStation(MarkerIx mx) const
+{ return markers[mx].markerInB; }
+
+/** Return the number of observations that were defined via the last call to
+defineObservationOrder(). These are not necessarily all being used. If
+defineObservationOrder() was never called, we'll expect the same number of
+observations as markers although that won't be set up until the Assembler has
+been initialized. **/
+int getNumObservations() const {return observation2marker.size();}
+
+/** Return the ObservationIx of the observation that is currently associated
+with the given marker, or an invalid index if the marker doesn't have any
+corresponding observation (in which case it is being ignored). An exception
+will be thrown if the given MarkerIx is not in the range
+0..getNumMarkers()-1. **/
+ObservationIx getObservationIxForMarker(MarkerIx mx) const
+{ return marker2observation[mx]; }
+
+/** Return true if the supplied marker is currently associated with an
+observation. @see getObservationIxForMarker() **/
+bool hasObservation(MarkerIx mx) const
+{ return getObservationIxForMarker(mx).isValid(); }
+
+/** Return the MarkerIx of the marker that is associated with the
+given observation, or an invalid index if the observation doesn't correspond
+to any marker (in which case it is being ignored). An exception will be
+thrown if the given ObservationIx is not in the range
+0..getNumObservations()-1. **/
+MarkerIx getMarkerIxForObservation(ObservationIx ox) const
+{ return observation2marker[ox]; }
+
+/** Return true if the supplied observation is currently associated with a
+marker. @see getMarkerIxForObservation() **/
+bool hasMarker(ObservationIx ox) const
+{ return getMarkerIxForObservation(ox).isValid();}
+
+/** The Markers assembly condition organizes the markers by body after
+initialization; call this to get the list of markers on any particular body.
+If necessary the Assembler will be initialized. It is an error if this
+assembly condition has not yet been adopted by an Assembler. **/
+const Array_<MarkerIx>& getMarkersOnBody(MobilizedBodyIndex mbx) {
+ static const Array_<MarkerIx> empty;
+ SimTK_ERRCHK_ALWAYS(isInAssembler(), "Markers::getMarkersOnBody()",
+ "This method can't be called until the Markers object has been"
+ " adopted by an Assembler.");
+ initializeAssembler();
+ PerBodyMarkers::const_iterator bodyp = bodiesWithMarkers.find(mbx);
+ return bodyp == bodiesWithMarkers.end() ? empty : bodyp->second;
+}
+/*@}*/
+
+
+
+//------------------------------------------------------------------------------
+/** @name Execution methods
+These methods can be called between tracking steps to make step-to-step
+changes without reinitialization, and to access the current values of
+step-to-step data including the resulting marker errors. **/
+/*@{*/
+
+/** Move a single marker's observed location without moving any of the others.
+If the value contains a NaN, this marker/observation pair will be ignored the
+next time the assembly goal cost function is calculated. **/
+void moveOneObservation(ObservationIx ox, const Vec3& observation)
+{ SimTK_ERRCHK_ALWAYS(!observations.empty(), "Assembler::moveOneObservation()",
+ "There are currently no observations defined. Either the Assembler"
+ " needs to be initialized to get the default observation order, or you"
+ " should call defineObservationOrder() explicitly.");
+ SimTK_ERRCHK2_ALWAYS(ox.isValid() && ox < observations.size(),
+ "Assembler::moveOneObservation()", "ObservationIx %d is invalid or"
+ " out of range; there are %d observations currently defined. Use"
+ " defineObservationOrder() to specify the set of observations and how"
+ " they correspond to markers.",
+ (int)ox, (int)observations.size());
+ observations[ox] = observation;
+}
+
+/** Set the observed marker locations for a new observation frame. These are
+the locations to which we will next attempt to move all the corresponding
+markers. Note that not all observations necessarily have corresponding markers
+defined; locations of those markers must still be provided here but they will
+be ignored. The length of the \a allObservations array must be the same as the
+number of defined observations; you can obtain that using getNumObservations().
+Any observations that contain a NaN will be ignored; that marker/observation
+pair will not be used in the next calculation of the assembly goal cost
+function. **/
+void moveAllObservations(const Array_<Vec3>& observations)
+{ SimTK_ERRCHK2_ALWAYS((int)observations.size() == (int)observation2marker.size(),
+ "Markers::moveAllObservations()",
+ "Number of observations provided (%d) differs from the number of"
+ " observations (%d) last defined with defineObservationOrder().",
+ observations.size(), observation2marker.size());
+ this->observations = observations; }
+
+/** Change the weight associated with a particular marker. If this is just
+a quantitative change (e.g., weight was 0.3 now it is 0.4) then this does
+not require any reinitialization and will affect the goal calculation next
+time it is done. If the weight changes to or from zero (a qualitative change)
+then this will uninitialize the Assembler and all the internal data structures
+will be changed to remove or add this marker from the list of active markers.
+If you want to temporarily ignore a marker without reinitializing, you can
+set its corresponding observation to NaN in which case it will simply be
+skipped when the goal value is calculated. **/
+void changeMarkerWeight(MarkerIx mx, Real weight) {
+ SimTK_ERRCHK1_ALWAYS(isFinite(weight) && weight >= 0,
+ "Markers::changeMarkerWeight()", "Illegal marker weight %g.", weight);
+
+ Marker& marker = markers[mx];
+ if (marker.weight == weight)
+ return;
+
+ if (marker.weight == 0 || weight == 0)
+ uninitializeAssembler(); // qualitative change
+
+ marker.weight = weight;
+}
+
+/** Return the current value of the location for this observation. This
+is where we will try to move the corresponding marker if there is one.
+The result might be NaN if there is no current value for this observation;
+you can check using Vec3's isFinite() method. **/
+const Vec3& getObservation(ObservationIx ox) const {return observations[ox];}
+/** Return the current values of all the observed locations. This is where we
+will try to move the corresponding markers, for those observations that have
+corresponding markers defined. Some of the values may be NaN if there is
+currently no corresponding observation. Note that these are indexed by
+ObservationIx; use getObservationIxForMarker() to map a MarkerIx to its
+corresponding ObservationIx. **/
+const Array_<Vec3,ObservationIx>& getAllObservations() const
+{ return observations; }
+
+/** Using the current value of the internal state, calculate the ground
+frame location of a particular marker. The difference between this location
+and the corresponding observation is the current error for this marker. **/
+Vec3 findCurrentMarkerLocation(MarkerIx mx) const;
+
+/** Using the current value of the internal state, calculate the distance
+between the given marker's current location and its corresponding observed
+location (unweighted). If the marker is not associated with an observation,
+or if the observed location is missing (indicated by a NaN value), then the
+error is reported as zero.
+ at note If you actually want the square of the distance, you can save some
+computation time by using findCurrentMarkerErrorSquared() which avoids the
+square root needed to find the actual distance.
+ at see findCurrentMarkerErrorSquared() **/
+Real findCurrentMarkerError(MarkerIx mx) const
+{ return std::sqrt(findCurrentMarkerErrorSquared(mx)); }
+
+/** Using the current value of the internal state, calculate the (unweighted)
+square of the distance between the given marker's current location and its
+corresponding observed location (the squared distance is less expensive to
+compute than the distance). If the marker is not associated with an
+observation, or if the observed location is missing (indicated by a NaN
+value), then the error is reported as zero.
+ at see findCurrentMarkerError() **/
+Real findCurrentMarkerErrorSquared(MarkerIx mx) const {
+ const ObservationIx ox = getObservationIxForMarker(mx);
+ if (!ox.isValid()) return 0; // no observation for this marker
+ const Vec3& loc = getObservation(ox);
+ if (!loc.isFinite()) return 0; // NaN in observation; error is ignored
+ return (findCurrentMarkerLocation(mx) - loc).normSqr();
+}
+/*@}*/
+
+
+
+//------------------------------------------------------------------------------
+/** @name AssemblyCondition virtuals
+These methods are the implementations of the AssemblyCondition virtuals. **/
+/*@{*/
+int calcErrors(const State& state, Vector& err) const;
+int calcErrorJacobian(const State& state, Matrix& jacobian) const;
+int getNumErrors(const State& state) const;
+int calcGoal(const State& state, Real& goal) const;
+int calcGoalGradient(const State& state, Vector& grad) const;
+int initializeCondition() const;
+void uninitializeCondition() const;
+/*@}*/
+
+//------------------------------------------------------------------------------
+ private:
+//------------------------------------------------------------------------------
+const Marker& getMarker(MarkerIx i) const {return markers[i];}
+Marker& updMarker(MarkerIx i) {uninitializeAssembler(); return markers[i];}
+
+ // data members
+
+// Marker definition. Any change here except a quantitative change to the
+// marker's weight uninitializes the Assembler.
+Array_<Marker,MarkerIx> markers;
+std::map<String,MarkerIx> markersByName;
+
+// Observation-marker corresondence specification. Any change here
+// uninitializes the Assembler.
+Array_<MarkerIx,ObservationIx> observation2marker;
+
+// For convience in mapping from a marker to its corresponding observation.
+// ObservationIx will be invalid if a particular marker has no associated
+// observation.
+Array_<ObservationIx,MarkerIx> marker2observation;
+
+// This is the current set of marker location observations, one per entry in
+// the observation2marker array. Changing the values here does not uninitialize
+// the Assembler.
+Array_<Vec3,ObservationIx> observations;
+
+// After initialize, this groups the markers by body and weeds out
+// any zero-weighted markers. TODO: skip low-weighted markers, at
+// least at the start of the assembly.
+typedef std::map<MobilizedBodyIndex,Array_<MarkerIx> > PerBodyMarkers;
+mutable PerBodyMarkers bodiesWithMarkers;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_ASSEMBLER_H_
diff --git a/Simbody/include/simbody/internal/Body.h b/Simbody/include/simbody/internal/Body.h
new file mode 100644
index 0000000..c3b6ef5
--- /dev/null
+++ b/Simbody/include/simbody/internal/Body.h
@@ -0,0 +1,291 @@
+#ifndef SimTK_SIMBODY_BODY_H_
+#define SimTK_SIMBODY_BODY_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+This defines the API for the Body base class and concrete Body types like
+Body::Rigid that are derived from it. These are handle classes; the
+implementation is hidden. **/
+
+#include "SimTKcommon.h"
+#include "simbody/internal/common.h"
+#include "simbody/internal/ContactSurface.h"
+
+#include <cassert>
+
+namespace SimTK {
+
+class DecorativeGeometry;
+
+//==============================================================================
+// BODY
+//==============================================================================
+/** The Body class represents a reference frame that can be used to describe
+mass properties and geometry. These are in turn used to build MobilizedBodies
+which combine a body and a specified mobilizer that defines how the reference
+frame can move with respect to other MobilizedBodies. Attached geometric
+objects can serve as the basis for a variety of force-generating elements or
+other algorithms that act on bodies.
+
+Body is an abstract base class handle, with concrete classes defined for each
+kind of body. There are a set of built-in body types, with Body::Rigid the
+most common. **/
+class SimTK_SIMBODY_EXPORT Body {
+public:
+/** Default constructor creates an empty Body handle. **/
+Body() : rep(0) { }
+/** Destroy the handle and the body if this is the owner. **/
+~Body();
+/** Copy constructor is a deep copy; the new Body is separate from the
+source Body. **/
+Body(const Body& source);
+/** Copy assignment is a deep copy; the original object is deleted if this
+is the owner, then replaced with a \e copy of the source. **/
+Body& operator=(const Body& source);
+
+/** This is a default conversion from MassProperties to Body. It will result
+in a rigid body (concrete class Body::Rigid) being created using these as its
+default MassProperties. This is what allows you to provide MassProperties
+instead of a Body in the MobilizedBody constructors. **/
+Body(const MassProperties& massProps);
+
+/** Every type of Body should provide an initial set of rigid body mass
+properties defined at Topology stage (i.e., in the System rather than
+the State). This is thus a Topology-stage change which will require a new
+realizeTopology() before use. **/
+Body& setDefaultRigidBodyMassProperties(const MassProperties&);
+
+/** Get the default (that is, Topology stage) mass properties for this Body.
+This may be overridden in a State if this Body has variable mass
+properties. **/
+const MassProperties& getDefaultRigidBodyMassProperties() const;
+
+/** Add a piece of decorative geometry fixed at some pose on this Body.
+This can be used for visualization of the Body's motion. Returns a small
+integer that can be used to identify this decoration in any copy of this
+%Body. The supplied DecorativeGeometry object is copied and transformed by the
+given Transform so that the actual geometry is always stored relative to the
+body's frame. **/
+int addDecoration(const Transform& X_BD, const DecorativeGeometry& geometry);
+
+/** Convenience method for when the decorative geometry is to be placed at the
+body frame. This is the same as addDecoration(Transform(),geometry). **/
+int addDecoration(const DecorativeGeometry& geometry)
+{ return addDecoration(Transform(), geometry); }
+
+/** Obtain a count nd of how many pieces of DecorativeGeometry have been
+attached to this Body. The indices i for individual decorations will be numbered
+0 <= i < nd. **/
+int getNumDecorations() const;
+
+/** Get a read-only reference to the i'th piece of DecorativeGeometry that
+was added to this Body, with 0 <= i < getNumDecorations(). The index i is
+the small integer that was returned by addDecoration(). **/
+const DecorativeGeometry& getDecoration(int i) const;
+
+/** Get a writable reference to the i'th piece of DecorativeGeometry that
+was added to this Body, with 0 <= i < getNumDecorations(). The index i is
+the small integer that was returned by addDecoration(). Note that we allow
+writable access to decorations even on a const Body -- these are after all
+just decorations. **/
+DecorativeGeometry& updDecoration(int i) const;
+
+/** Create a new contact surface on a body and place it using the indicated
+Transform. Returns a small integer that can be used to identify this contact
+surface in any copy of this %Body. Unlike decorations, the transform is kept
+separately rather than used to transform the copied surface. You can obtain it
+later with getContactSurfaceTransform(). **/
+int addContactSurface(const Transform& X_BS,
+ const ContactSurface& shape);
+
+/** Convenience method for when the contact surface is to be placed at the
+body frame. This is the same as addContactSurface(Transform(),shape). **/
+int addContactSurface(const ContactSurface& shape)
+{ return addContactSurface(Transform(), shape); }
+
+/** Obtain the number of contact surfaces ns attached to this Body. The valid
+body-local index values i will be 0 <= i < ns. **/
+int getNumContactSurfaces() const;
+/** Get a reference to the i'th contact surface on this body; be sure to get
+the Transform also. **/
+const ContactSurface& getContactSurface(int i) const;
+/** Get the transform specifying the placement of the i'th contact surface
+on this Body. **/
+const Transform& getContactSurfaceTransform(int i) const;
+/** Get write access to the i'th unique contact surface owned by this Body. This
+is a Topology-stage change that will require a new realizeTopology() call if
+this Body is part of a System. **/
+ContactSurface& updContactSurface(int i);
+/** Get a writable reference to the transform specifying the placement of the
+i'th contact surface on this Body. This is a Topology-stage change that will
+require a new realizeTopology() call if this Body is part of a System. **/
+Transform& updContactSurfaceTransform(int i);
+
+// These are the built-in Body types.
+class Ground; // infinitely massive
+class Massless; // just a reference frame
+class Particle; // point mass only; com=0; no inertia
+class Linear; // point masses along a line; scalar inertia
+class Rigid; // general rigid body
+class Deformable; // base class for bodies with internal deformation coords
+
+bool isOwnerHandle() const;
+bool isEmptyHandle() const;
+
+// Internal use only
+class BodyRep; // local subclass
+explicit Body(class BodyRep* r) : rep(r) { }
+bool hasRep() const {return rep!=0;}
+const BodyRep& getRep() const {assert(rep); return *rep;}
+BodyRep& updRep() const {assert(rep); return *rep;}
+void setRep(BodyRep& r) {assert(!rep); rep = &r;}
+
+protected:
+class BodyRep* rep;
+};
+
+
+
+//==============================================================================
+// BODY::RIGID
+//==============================================================================
+/** A general rigid body. This can represent a body with mass properties that
+are full, linear, inertialess (e.g. a point), or massless. **/
+class SimTK_SIMBODY_EXPORT Body::Rigid : public Body {
+public:
+ /** Construct a rigid body with default mass properties which are
+ (1,Vec3(0),Inertia(1,1,1)) **/
+ Rigid();
+ /** Construct a rigid body with the given mass properties; any set of
+ mass properties is allowed since this is a general rigid body. **/
+ explicit Rigid(const MassProperties&);
+
+ Rigid& setDefaultRigidBodyMassProperties(const MassProperties& m) {
+ (void)Body::setDefaultRigidBodyMassProperties(m);
+ return *this;
+ }
+
+ class RigidRep; // local subclass
+ SimTK_PIMPL_DOWNCAST(Rigid, Body);
+private:
+ RigidRep& updRep();
+ const RigidRep& getRep() const;
+};
+
+
+
+
+//==============================================================================
+// BODY::LINEAR
+//==============================================================================
+/** This is a rigid body in the shape of a line, which is inherently
+inertialess about its axis. Its mass properties may be modified later, but
+only in such a way that the Body remains inertialess about one axis. **/
+class SimTK_SIMBODY_EXPORT Body::Linear : public Body {
+public:
+ Linear(); // default mass properties (1,Vec3(0),Inertia(1,1,0))
+ explicit Linear(const MassProperties&);
+
+ Linear& setDefaultRigidBodyMassProperties(const MassProperties& m) {
+ (void)Body::setDefaultRigidBodyMassProperties(m);
+ return *this;
+ }
+
+ class LinearRep; // local subclass
+ SimTK_PIMPL_DOWNCAST(Linear, Body);
+private:
+ LinearRep& updRep();
+ const LinearRep& getRep() const;
+};
+
+
+// Particles aren't yet supported, so hide this in Doxygen.
+/** @cond **/
+//==============================================================================
+// BODY::PARTICLE
+//==============================================================================
+/** This kind of body can only represent an inertialess point mass, with
+mass center at (0,0,0) in the local frame. You can change mass later (even
+to make it massless) but you can't move the mass center or add any inertia. **/
+class SimTK_SIMBODY_EXPORT Body::Particle : public Body {
+public:
+ Particle(); // default mass properties (1,Vec3(0),Inertia(0))
+ explicit Particle(const Real& mass);
+
+ Particle& setDefaultRigidBodyMassProperties(const MassProperties& m) {
+ (void)Body::setDefaultRigidBodyMassProperties(m);
+ return *this;
+ }
+
+ class ParticleRep; // local subclass
+ SimTK_PIMPL_DOWNCAST(Particle, Body);
+private:
+ ParticleRep& updRep();
+ const ParticleRep& getRep() const;
+};
+/** @endcond **/
+
+
+
+//==============================================================================
+// BODY::MASSLESS
+//==============================================================================
+/** This is a Body that is constitutively massless (and inertialess); meaning
+that no amount of fiddling with it will ever give it any mass or inertia. **/
+class SimTK_SIMBODY_EXPORT Body::Massless : public Body {
+public:
+ Massless();
+
+ class MasslessRep; // local subclass
+ SimTK_PIMPL_DOWNCAST(Massless, Body);
+private:
+ MasslessRep& updRep();
+ const MasslessRep& getRep() const;
+};
+
+
+
+//==============================================================================
+// BODY::GROUND
+//==============================================================================
+/** This is a Body representing something immobile, of effectively infinite
+mass and inertia, that cannot be modified to be anything else. **/
+class SimTK_SIMBODY_EXPORT Body::Ground : public Body {
+public:
+ Ground();
+
+ class GroundRep; // local subclass
+ SimTK_PIMPL_DOWNCAST(Ground, Body);
+private:
+ GroundRep& updRep();
+ const GroundRep& getRep() const;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_BODY_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/CablePath.h b/Simbody/include/simbody/internal/CablePath.h
new file mode 100644
index 0000000..c3f15bf
--- /dev/null
+++ b/Simbody/include/simbody/internal/CablePath.h
@@ -0,0 +1,371 @@
+#ifndef SimTK_SIMBODY_CABLE_PATH_H_
+#define SimTK_SIMBODY_CABLE_PATH_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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Ian Stavness *
+ * Contributors: Andreas Scholz *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+This file declares the CablePath and CableObstacle classes. **/
+
+#include "SimTKmath.h"
+#include "simbody/internal/common.h"
+
+namespace SimTK {
+
+/** @class SimTK::CableObstacleIndex
+This is a unique integer type for identifying obstacles comprising a particular
+cable path. These begin at zero for each cable path. **/
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(CableObstacleIndex);
+
+class CableTrackerSubsystem;
+class MobilizedBody;
+class CableObstacle;
+
+//==============================================================================
+// CABLE PATH
+//==============================================================================
+/** This class represents the path of a frictionless cable from an origin point
+fixed to a body, through via points and over geometric obstacles fixed to other
+bodies, to a final termination point. The cable ends are attached at the origin
+and termination points while the cable is free to slide through the via points
+and along the obstacle surfaces. The cable follows a geodesic curve (usually
+the shortest path) over each surface.
+
+During initialization, if there is more than one possible geodesic over a
+surface, or if a straight line path would miss the surface altogether, we'll
+take the shortest route unless the user has provided a "near point" on the
+surface. In that case whichever of the possible path segments runs closest to
+the near point is chosen. During continuation, only local path movement is
+allowed so the path segment will not flip from one side to the other once it
+has been initialized, even if that means it does not follow the shortest
+possible geodesic. The near point is ignored during continuation.
+
+Note that a %CablePath is a geometric object, not a force or constraint
+element. That is, a %CablePath alone will not influence the behavior of a
+simulation. However, forces and constraint elements can be constructed that make
+use of a %CablePath to generate forces. For an example, see CableSpring.
+
+The auxiliary class CableObstacle and its subclasses are used to specify the
+via points and obstacles that affect a particular %CablePath. %CablePath
+objects must be registered with a CablePathSubsystem which manages their
+runtime evaluation.
+
+<h3>Notation</h3>
+
+For convenience, we include the origin and termination points as obstacles,
+with the origin being obstacle zero, followed by m via point and surface
+obstacles numbered 1 to m, followed by the termination point as obstacle t=m+1.
+Every obstacle is represented by two "contact points", P and Q, which we'll
+number Pi and Qi for obstacle i. The obstacles are
+separated by straight-line segments running from Qi-1 to Pi. Starting at
+the origin Q0, the path first touches the surface of obstacle 1 at P1,
+travels over the surface to Q1, and then leaves the surface towards the
+termination point Pt. That is, Pi's path coordinate must be less than Qi's.
+The segment from P to Q is a geodesic over the surface. For via points,
+points P and Q are in the same location but there are two different tangents
+associated with them in the incoming and outgoing straight-line directions.
+For the origin obstacle, only point Q is relevant and for the termination
+obstacle only point P is relevant.
+
+ at see CableObstacle, CableSpring, CableTrackerSubsystem
+**/
+class SimTK_SIMBODY_EXPORT CablePath {
+public:
+
+/** Create a straight-line cable path connecting a point fixed on one body with
+one fixed on another body. You can add additional obstacles and move the
+end points later. **/
+CablePath(CableTrackerSubsystem& cables,
+ const MobilizedBody& originBody,
+ const Vec3& defaultOriginPoint,
+ const MobilizedBody& terminationBody,
+ const Vec3& defaultTerminationPoint);
+
+/** Constructor taking only the origin and terminal bodies with the expectation
+that you'll set the end point locations later. The default locations are set
+to the body frame origins here. **/
+CablePath(CableTrackerSubsystem& cables,
+ const MobilizedBody& originBody,
+ const MobilizedBody& terminationBody);
+
+/** Copy constructor is shallow and reference counted. **/
+CablePath(const CablePath& source);
+
+/** Copy assignment is shallow and reference counted. **/
+CablePath& operator=(const CablePath& source);
+
+/** Delete the cable path if this handle was the last reference to it. **/
+~CablePath() {clear();}
+
+/** TODO: Calculate the initial cable path, without using any prior solution.
+The result is saved in the supplied State which may then be used as the
+initial condition for a time simulation. This method will work hard to find
+a good starting solution, making use of any hints that have been
+supplied with the obstacles. This is substantially different (and much more
+time consuming) than the method used during a simulation, which always starts
+with the previous solution and is intentionally limited to finding a nearby
+solution.
+
+The initial solution consists of both (a) which of the surface obstacles are
+active, and (b) the intersection of the cable with the active obstacles and
+the path taken by the cable over those obstacles (a geodesic curve). For
+inactive surface obstacles we determine the closest point between the obstacles
+and the path; that point will be tracked continuously during a simulation. The
+user-supplied ordering and near points are respected. The
+cable length and length rate of change are available immediately after this
+call; \a state will have been realized through Velocity stage.
+
+An exception is thrown if no acceptable cable path can be found. In that
+case the \a state is still initialized and can be examined to see where
+the algorithm got stuck. **/
+void solveForInitialCablePath(State& state) const;
+
+/** Return the total number of obstacles (origin point, surfaces and via
+points, and termination point) that were provided for this cable path,
+regardless of whether they are currently in use. **/
+int getNumObstacles() const;
+/** Return a const reference to one of the obstacles along this path, given
+by its index starting at zero for the origin point. **/
+const CableObstacle& getObstacle(CableObstacleIndex obstacleIx) const;
+
+/** Return the total length of the cable that was calculated for the
+configuration supplied in \a state. State must have been realized through
+Position stage. **/
+Real getCableLength(const State& state) const;
+
+/** Return the cable rate (time derivative of cable length) that was
+calculated for the configuration and velocities supplied in \a state. State
+must have been realized through Velocity stage. Calculation of the cable rate
+may be initiated if necessary the first time this is called at this state
+but will be saved in the cache for subsequent accesses. **/
+Real getCableLengthDot(const State& state) const;
+
+/** Given a tension > 0 acting uniformly along this cable, apply the resulting
+forces to the bodies it touches. The body forces are added into the appropriate
+slots in the supplied Array which has one entry per body in the same format
+as is supplied to the calcForce() method of force elements. If the supplied
+tension is <= 0, signifying a slack cable, this method does nothing. **/
+void applyBodyForces(const State& state, Real tension,
+ Vector_<SpatialVec>& bodyForcesInG) const;
+
+/** Calculate the power this cable would apply or dissipate at the given
+\a tension (>0) value, using the velocities provided in the given \a state,
+which must already have been realized to Velocity stage. Power is positive if
+the cable is adding energy to the system; negative when dissipating. If the
+supplied \a tension is <= 0, power may still be dissipated while the cable
+shortens even though it can't apply forces to the system. **/
+Real calcCablePower(const State& state, Real tension) const;
+
+/** (Advanced) Get the time integral of cable length dot. This should be the
+total change in cable length since start of a simulation. This is mostly
+useful for debugging and testing of cables. **/
+Real getIntegratedCableLengthDot(const State& state) const;
+
+/** (Advanced) Initialize the integral of the cable length rate. This is
+used at the start of a simulation to initilize the integral to the value
+returned by getCableLength() so that getIntegratedCableLengthDot() will return
+the same values as getCableLength() during the simulations. **/
+void setIntegratedCableLengthDot(State& state, Real value) const;
+
+
+/** Default constructor creates an empty cable path not associated with any
+subsystem; don't use this. **/
+CablePath() : impl(0) {}
+class Impl;
+const Impl& getImpl() const {assert(impl); return *impl;}
+Impl& updImpl() {assert(impl); return *impl;}
+//--------------------------------------------------------------------------
+private:
+void clear();
+Impl* impl;
+};
+
+
+//==============================================================================
+// CABLE OBSTACLE
+//==============================================================================
+/** An obstacle is any significant object along the cable path -- one of the
+end points, a via point, or a surface. This is the base class that can refer
+to any kind of obstacles; specific types are derived from this one. **/
+class SimTK_SIMBODY_EXPORT CableObstacle {
+public:
+class ViaPoint; // also used for end points
+class Surface;
+
+/** Create an empty obstacle handle that can refer to any type of obstacle. **/
+CableObstacle() : impl(0) {}
+
+/** Insert this obstacle into the given cable path. **/
+explicit CableObstacle(CablePath& path);
+/** Copy constructor is shallow and reference-counted; this handle will
+point to the same object as does the \a source. **/
+CableObstacle(const CableObstacle& source);
+/** Copy assignment is shallow and reference-counted; this handle will
+point to the same object as does the \a source. **/
+CableObstacle& operator=(const CableObstacle& source);
+/** Destructor clears the handle, deleting the referenced object if this
+was the last reference. **/
+~CableObstacle() {clear();}
+
+/** Return the default pose X_BS of the obstacle S on its body B. For a via
+point, the point is located at the S frame origin whose position vector in B
+is X_BS.p(). **/
+const Transform& getDefaultTransform() const;
+/** Get a reference to the Mobilized body to which this obstacle is fixed.
+There can be multiple objects on a single body, so this is not necessarily
+unique within a path. **/
+const MobilizedBody& getMobilizedBody() const;
+/** Return a reference to the CablePath in which this obstacle resides. **/
+const CablePath& getCablePath() const;
+/** Return the obstacle index within this obstacle's CablePath. **/
+CableObstacleIndex getObstacleIndex() const;
+/** Return decorative geometry that can be used to display this obstacle. The
+decorative geometry's coordinate frame is coincident with the obstacle's
+coordinate frame S. **/
+const DecorativeGeometry& getDecorativeGeometry() const;
+/** Obtain writable access to the decorative geometry stored with this obstacle
+so you can modify it. If you want to replace the decorative geometry
+altogether use setDecorativeGeometry(). **/
+DecorativeGeometry& updDecorativeGeometry();
+
+/** Is this obstacle disabled by default? Note that this does not tell you
+whether it is currently disabled, just the setting that determines how it is
+treated when the system is first constructed. **/
+bool isDisabledByDefault() const;
+
+/** Set the "disabled by default" flag. This controls whether the obstacle is
+included for consideration in the cable path when the system is first
+constructed; it does not affect the current setting. **/
+CableObstacle& setDisabledByDefault(bool shouldBeDisabled);
+
+/** Replace the default transform for this obstacle; this is usually set in
+the constructor. **/
+CableObstacle& setDefaultTransform(const Transform& X_BS);
+
+/** Replace the decorative geometry used for automatically-generated
+visualization of this obstacle when visualizing the cable path. It is up to
+you to make sure the geometry is actually representative of the obstacle --
+no one is going to check. **/
+CableObstacle& setDecorativeGeometry(const DecorativeGeometry& viz);
+
+/** Clear this handle, deleting the referenced object if this
+was the last reference. **/
+void clear();
+/** See if this handle is empty. **/
+bool isEmpty() const {return impl==0;}
+
+//--------------------------------------------------------------------------
+class Impl;
+const Impl& getImpl() const {assert(impl); return *impl;}
+Impl& updImpl() {assert(impl); return *impl;}
+
+protected:
+explicit CableObstacle(Impl* impl);
+
+private:
+Impl* impl; // opaque pointer to reference-counted implementation object
+};
+
+
+//==============================================================================
+// CABLE OBSTACLE :: VIA POINT
+//==============================================================================
+/** This is a point through which the cable must pass. **/
+class SimTK_SIMBODY_EXPORT CableObstacle::ViaPoint : public CableObstacle {
+public:
+/** Default constructor creates an empty handle. **/
+ViaPoint() : CableObstacle() {}
+/** Insert a via point obstacle to the given cable path. **/
+ViaPoint(CablePath& path, const MobilizedBody& viaMobod,
+ const Vec3& defaultStation);
+
+/** Return true if the given CableObstacle is a ViaPoint. **/
+static bool isInstance(const CableObstacle&);
+/** Cast the given CableObstacle to a const ViaPoint; will throw an exception
+if the obstacle is not a via point. **/
+static const ViaPoint& downcast(const CableObstacle&);
+/** Cast the given CableObstacle to a writable ViaPoint; will throw an
+exception if the obstacle is not a via point. **/
+static ViaPoint& updDowncast(CableObstacle&);
+
+class Impl;
+};
+
+//==============================================================================
+// CABLE OBSTACLE :: SURFACE
+//==============================================================================
+/** This obstacle is a solid object represented by a ContactGeometry surface.
+The cable cannot penetrate the surface and will instead take the shortest
+path in an allowed direction on the surface. **/
+class SimTK_SIMBODY_EXPORT CableObstacle::Surface : public CableObstacle {
+public:
+/** Default constructor creates an empty handle. **/
+Surface() : CableObstacle() {}
+
+/** Create a new wrapping surface obstacle and insert it into the given
+CablePath. The surface is fixed to mobilized body \a mobod and the surface
+frame S is positioned relative to that body's frame B according to the given
+transform \a X_BS. **/
+Surface(CablePath& path, const MobilizedBody& mobod,
+ const Transform& X_BS, const ContactGeometry& surface);
+
+/** Provide visualization geometry to be used to display this obstacle. The
+frame of the decorative geometry is made coincident with the surface's S
+frame. **/
+Surface& setDecorativeGeometry(const DecorativeGeometry& viz)
+{ CableObstacle::setDecorativeGeometry(viz); return *this; }
+
+/** Optionally provide a "near point" that can be used during
+path initialization to disambiguate when there is more than one geodesic
+that can connect the contact points, or when a straight line from the
+preceding to the following obstacle would miss the obstacle altogether. The
+geodesic or straight line segment that passes closest to
+the near point will be selected. Without this point the obstacle will be
+skipped if possible, otherwise the shortest
+geodesic will be selected. This point is ignored during path continuation
+calculations. The point location is given in the local frame S of the
+contact surface. **/
+Surface& setNearPoint(const Vec3& point);
+
+/** Optionally provide some hints for the initialization algorithm to
+use as starting guesses for the contact point locations. These are
+ignored during path continuation calculations. These locations are given
+in the local frame S of the contact surface. **/
+Surface& setContactPointHints(const Vec3& startHint,
+ const Vec3& endHint);
+
+/** Return true if the given CableObstacle is a Surface. **/
+static bool isInstance(const CableObstacle&);
+/** Cast the given CableObstacle to a const Surface; will throw an exception
+if the obstacle is not a surface. **/
+static const Surface& downcast(const CableObstacle&);
+/** Cast the given CableObstacle to a writable Surface; will throw an
+exception if the obstacle is not a surface. **/
+static Surface& updDowncast(CableObstacle&);
+class Impl;
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_CABLE_PATH_H_
diff --git a/Simbody/include/simbody/internal/CableSpring.h b/Simbody/include/simbody/internal/CableSpring.h
new file mode 100644
index 0000000..2eb75b3
--- /dev/null
+++ b/Simbody/include/simbody/internal/CableSpring.h
@@ -0,0 +1,391 @@
+#ifndef SimTK_SIMBODY_CABLE_SPRING_H_
+#define SimTK_SIMBODY_CABLE_SPRING_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) 2012-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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"
+#include "simbody/internal/CablePath.h"
+
+/** @file
+This contains the user-visible API ("handle" class) for the SimTK::Force
+subclass CableSpring, providing an elastic band force element that follows a
+given CablePath. **/
+
+namespace SimTK {
+
+/** This force element implements a passive elastic element (like a
+rubber band) that follows a frictionless CablePath across a set of "obstacles".
+The element calculates a uniform nonnegative tension that is used to apply
+forces at the end points of the CablePath and to each intermediate obstacle.
+The model provides stiffness and dissipation, and has a slack length below
+which the tension is zero and no forces are generated. Dissipated power is
+calculated and integrated so that work lost to dissipation is available for
+conservation of energy calculations.
+
+\par Theory:
+
+Given current CablePath length L with time derivative Ldot, and slack length
+L0 for this force element, define stretch x=max(0,L-L0) and xdot=Ldot. Then
+calculate the nonnegative tension f(x,xdot), the potential energy pe(x) stored
+in this force element, and dissipating power powerLoss(x,xdot) due to
+rate-dependent resistance to length change, as follows:
+<pre>
+ f_stretch = k*x
+ f_rate = max(-f_stretch, f_stretch*c*xdot)
+ f = f_stretch + f_rate
+ pe = k*x^2/2
+ powerLoss = f_rate * xdot
+ dissipation = integ(powerLoss, dt)
+</pre>
+where \c k >= 0 is the stiffness coefficient (force per unit length) and
+\c c >= 0 is the dissipation coefficient (1/velocity) for this force element.
+
+Note that in this model the tension component \c f_rate, due to rate-dependent
+dissipation in the elastic element, is calculated as a fraction of the
+stretch-dependent tension component \c f_stretch, similar to a Hunt and Crossley
+contact model. That is why the dissipation coefficient \c c has units of
+1/velocity, rather than force/velocity. This makes the generated tension grow
+smoothly from zero as the slack length is exceeded, but even so this is not
+necessarily a good model for any particular physically-realizable elastic
+element. A ligament, for example, while perhaps qualitatively similar to this
+element, is likely to require a more complicated relationship between
+\c x, \c xdot, and \c f to yield quantitative agreement with experimental data.
+
+While the total tension \c f must be nonnegative, the tension \c f_rate due to
+stretch rate can be positive (resists stretching) or negative (resists
+shortening) but can never be less than \c -f_stretch since \c f can't be
+negative. When a stretched cable spring is shortening so rapidly that force due
+to dissipation cancels the force due to stiffness, there will be no tension
+generated but we will still dissipate power at a rate that exactly accounts for
+the continuing loss of potential energy in the spring (<code>k*x*xdot</code>)
+as it shortens to its slack length. In that way total energy
+<code>E=pe+ke+dissipation</code> is conserved, where \c dissipation is the
+time integral of \c powerLoss calculated above (note that \c powerLoss >= 0
+as defined).
+
+ at see CablePath, CableTrackerSubsystem **/
+class SimTK_SIMBODY_EXPORT CableSpring : public Force {
+public:
+
+/** Create an elastic force element that follows a given CablePath and add it
+to a GeneralForceSubsystem. Default values for the cable spring properties
+are given here; you can override them in the State. See the %CableSpring class
+description for a detailed explanation of these parameters.
+
+ at param[in,out] forces
+ The subsystem to which this force element should be added.
+ at param[in] path
+ The CablePath that defines the routing of this elastic element over
+ geometric obstacles.
+ at param[in] defaultStiffness
+ A nonnegative spring constant representing the stiffness of this element,
+ in units of force/length, where the force represents a uniform tension along
+ the element that results from stretching it beyond its slack length.
+ at param[in] defaultSlackLength
+ The maximum length this elastic element can have before it begins to
+ generate force. At or below this length the element is slack and has zero
+ tension and zero power dissipation.
+ at param[in] defaultDissipationCoef
+ A nonnegative dissipation coefficient for this elastic element in units of
+ 1/velocity.
+**/
+CableSpring(GeneralForceSubsystem& forces,
+ const CablePath& path,
+ Real defaultStiffness,
+ Real defaultSlackLength,
+ Real defaultDissipationCoef);
+
+/** Default constructor creates an empty handle. **/
+CableSpring() {}
+
+
+//------------------------------------------------------------------------------
+/** @name Default Parameters
+
+These refer to Topology-stage parameters normally set in the constructor; these
+determine the initial values assigned to the corresponding Instance-stage state
+variables.
+
+\par Notes
+- Changing one of these parameters invalidates the containing System's
+ topology, meaning that realizeTopology() will have to be called
+ before subsequent use.
+- The set...() methods return a reference to "this" CableSpring (in
+ the manner of an assignment operator) so they can be chained in a
+ single expression. **/
+/**@{**/
+
+/** Set the stiffness (spring constant) k that will be used by default
+for this cable spring; must be nonnegative.
+ at param[in] stiffness The spring constant k to be used by default.
+ at return A writable reference to "this" %CableSpring which will now have the new
+default stiffness. **/
+CableSpring& setDefaultStiffness(Real stiffness);
+/** Set the slack length L0 that will be used by default for this cable spring;
+must be nonnegative.
+ at param[in] slackLength The slack length L0 to be used by default.
+ at return A writable reference to "this" %CableSpring which will now have the new
+default slack length. **/
+CableSpring& setDefaultSlackLength(Real slackLength);
+/** Set the dissipation coefficient c that will be used by default for this
+cable spring; must be nonnegative.
+ at param[in] dissipation The dissipation coefficient c to be used by default.
+ at return A writable reference to "this" %CableSpring which will now have the new
+default dissipation coefficient. **/
+CableSpring& setDefaultDissipationCoef(Real dissipation);
+
+/** Return the stiffnesses k that will be used by default for this cable spring.
+ at return The default spring constant. **/
+Real getDefaultStiffness() const;
+/** Return the slack length L0 that will be used by default for this cable spring.
+ at return The default slack length. **/
+Real getDefaultSlackLength() const;
+/** Return the dissipation coefficient c that will be used by default for this
+cable spring.
+ at return The default dissipation coefficient. **/
+Real getDefaultDissipationCoef() const;
+/**@}**/
+
+
+
+//------------------------------------------------------------------------------
+/**@name Instance Parameters
+
+These refer to the Instance-stage state variables that determine the geometry
+and material properties that will be used in computations involving this cable
+spring when performed with the given State. If these are not set explicitly,
+the default values are set to those provided in the constructor or via the
+correponding setDefault...() methods.
+
+\par Notes
+- Changing one of these parameters invalidates the given State's Instance stage,
+ meaning that realize(Instance) will have to be called (explicitly or
+ implicitly by realizing a higher stage) before subsequent use.
+- The set...() methods here return a const reference to "this" %CableSpring (in
+ the manner of an assignment operator, except read-only) so they can be chained
+ in a single expression.
+**/
+/**@{**/
+/** Set the stiffness (spring constant) k that will be used for this cable
+spring when evaluated using this State.
+ at pre \a state realized to Stage::Topology
+ at param[in,out] state The State object to be modified by this method.
+ at param[in] stiffness The spring constant k.
+ at return A const reference to "this" %CableSpring for convenient chaining of
+set...() methods in a single expression. **/
+const CableSpring& setStiffness(State& state,
+ Real stiffness) const;
+/** Set the slack length L0 that will be used for this cable spring when
+evaluated using this State.
+ at pre \a state realized to Stage::Topology
+ at param[in,out] state The State object to be modified by this method.
+ at param[in] slackLength The slack length L0.
+ at return A const reference to "this" %CableSpring for convenient chaining of
+set...() methods in a single expression. **/
+const CableSpring& setSlackLength(State& state,
+ Real slackLength) const;
+/** Set the dissipation coefficient c that will be used for this cable spring
+when evaluated using this State.
+ at pre \a state realized to Stage::Topology
+ at param[in,out] state The State object that is modified by this method.
+ at param[in] dissipationCoef The dissipation coefficient c.
+ at return A const reference to "this" %CableSpring for convenient chaining of
+set...() methods in a single expression. **/
+const CableSpring& setDissipationCoef(State& state,
+ Real dissipationCoef) const;
+
+/** Return the stiffness (spring constant) k currently being used for this cable
+spring by this State.
+ at pre \a state realized to Stage::Topology
+ at param[in] state The State object from which to obtain the stiffness.
+ at return The current value in the given state of the spring constant k. **/
+Real getStiffness(const State& state) const;
+/** Return the slack length L0 currently being used for this cable spring by
+this State.
+ at pre \a state realized to Stage::Topology
+ at param[in] state The State object from which to obtain the slack length.
+ at return The current value in the given state of the slack length L0. **/
+Real getSlackLength(const State& state) const;
+/** Return the dissipation coefficient c currently being used for this cable
+spring by this State.
+ at pre \a state realized to Stage::Topology
+ at param[in] state The State object from which to obtain the dissipation
+ coefficient.
+ at return The current value in the given state of the dissipation
+coefficient c. **/
+Real getDissipationCoef(const State& state) const;
+/**@}**/
+
+
+
+//------------------------------------------------------------------------------
+/** @name Position-related Quantities
+
+These methods return position-dependent quantities that are calculated
+by the cable spring as part of its force calculations and stored in the
+State cache. These can be obtained at no cost, although the first call
+after a position change may initiate computation if forces haven't
+already been computed.
+
+ at pre These methods may be called only after the supplied state has
+ been realized to Stage::Position. **/
+/**@{**/
+/** Return the current length of the CablePath that underlies this cable spring
+element. Note that this may return a value less than the spring's slack length.
+ at pre \a state realized to Stage::Position
+ at param state The State from which the path length is retrieved.
+ at return The current length of the underlying CablePath (nonnegative).
+ at see getLengthDot() **/
+Real getLength(const State& state) const;
+/**@}**/
+
+
+
+//------------------------------------------------------------------------------
+/** @name Velocity-related Quantities
+
+These methods return velocity-dependent quantities that are calculated
+by the cable spring as part of its force calculations and stored in the
+State cache. These can be obtained at no cost, although the first call
+after a velocity change may initiate computation if forces haven't
+already been computed.
+
+ at pre These methods may be called only after the supplied state has
+ been realized to Stage::Velocity. **/
+/**@{**/
+/** Return the current rate of length change (time derivative of length) of the
+CablePath that underlies this cable spring element.
+ at pre \a state realized to Stage::Velocity
+ at param state The State from which the path length rate of change is
+ retrieved.
+ at return The current length change rate of the underlying CablePath.
+ at see getLength() **/
+Real getLengthDot(const State& state) const;
+
+/**@}**/
+
+
+
+//------------------------------------------------------------------------------
+/** @name Forces
+
+These methods return the forces being applied by this cable spring in the
+configuration and velocities contained in the supplied State. These are
+evaluated during force calculation and available at no computational cost
+afterwards, although the first call after a velocity change may initiate
+computation if forces haven't already been computed.
+
+ at pre These methods may be called only after the supplied state has been
+ realized to Stage::Velocity. **/
+/**@{**/
+/** Return the current level of tension in the cable spring.
+ at pre \a state realized to Stage::Velocity
+ at param[in] state
+ The State from which the current tension is retrieved.
+ at return
+ The current tension in the cable spring in the configuration and velocity
+ contained in \a state (a nonnegative scalar). **/
+Real getTension(const State& state) const;
+/**@}**/
+
+
+
+//------------------------------------------------------------------------------
+/** @name Energy, Power, and Work
+
+These methods return the energy, power, and work-related quantities associated
+with this %CableSpring element for the values in the supplied State. **/
+/**@{**/
+/** Obtain the potential energy stored in this cable spring in the current
+configuration.
+ at pre \a state realized to Stage::Position
+ at param[in] state
+ The State from whose cache the potential energy is retrieved.
+ at return
+ The potential energy currently contained in the cable spring in the
+ configuration contained in \a state (a nonnegative scalar). **/
+Real getPotentialEnergy(const State& state) const;
+
+/** Obtain the rate at which energy is being dissipated by this cable spring,
+that is, the power being lost (presumably due to heat). This is in units of
+energy/time which is watts in MKS.
+ at pre \a state realized to Stage::Velocity
+ at param[in] state
+ The State from which to obtain the current value of the power
+ dissipation.
+ at return
+ The instantaneous power dissipation (a nonnegative scalar).
+ at see getDissipatedEnergy() for the time-integrated power loss **/
+Real getPowerDissipation(const State& state) const;
+
+/** Obtain the total amount of energy dissipated by this cable spring since some
+arbitrary starting point. This is the time integral of the power dissipation.
+For a system whose only non-conservative forces are cable springs, the sum of
+potential, kinetic, and dissipated energies should be conserved. This is a
+State variable so you can obtain its value any time after it is allocated.
+ at pre \a state realized to Stage::Model
+ at param[in] state
+ The State from which to obtain the current value of the dissipated
+ energy.
+ at return
+ The total dissipated energy (a nonnegative scalar).
+ at see getPowerDissipation() for the instantaneous power loss **/
+Real getDissipatedEnergy(const State& state) const;
+
+/** Set the accumulated dissipated energy to an arbitrary value. Typically
+this is used only to reset the dissipated energy to zero, but non-zero values
+can be useful if you are trying to match some existing data or continuing a
+simulation. This is a State variable so you can set its value any time after it
+is allocated.
+ at pre \a state realized to Stage::Model
+ at param[in,out] state
+ The State whose dissipated energy variable for this cable spring is to
+ be modified.
+ at param[in] energy
+ The new value for the accumulated dissipated energy (must be a nonnegative
+ scalar). **/
+void setDissipatedEnergy(State& state, Real energy) const;
+/**@}**/
+
+/**@name Advanced/obscure/debugging
+Miscellaneous methods that you probably aren't going to need. **/
+/**@{**/
+/** Get a reference to the CablePath that is used to determine the routing of
+this cable spring force element. This is set during %CableSpring construction
+and cannot be changed subsequently. **/
+const CablePath& getCablePath() const;
+/**@}**/
+
+/** @cond **/ // Don't show this in Doxygen.
+class Impl;
+SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS
+ (CableSpring, CableSpring::Impl, Force);
+/** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_CABLE_SPRING_H_
diff --git a/Simbody/include/simbody/internal/CableTrackerSubsystem.h b/Simbody/include/simbody/internal/CableTrackerSubsystem.h
new file mode 100644
index 0000000..2d4fbd0
--- /dev/null
+++ b/Simbody/include/simbody/internal/CableTrackerSubsystem.h
@@ -0,0 +1,93 @@
+#ifndef SimTK_SIMBODY_CABLE_TRACKER_SUBSYSTEM_H_
+#define SimTK_SIMBODY_CABLE_TRACKER_SUBSYSTEM_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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Ian Stavness, Andreas Scholz *
+ * Contributors: *
+ * *
+ * 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"
+
+namespace SimTK {
+
+/** @class SimTK::CablePathIndex
+This is a unique integer type for quickly identifying specific cables for
+fast lookup purposes. **/
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(CablePathIndex);
+
+class MultibodySystem;
+class CablePath;
+
+//==============================================================================
+// CABLE TRACKER SUBSYSTEM
+//==============================================================================
+/** This subsystem tracks the paths of massless, frictionless cables that take
+the shortest route between two distant points of a multibody system, passing
+smoothly over geometric obstacles that are attached to intermediate bodies.
+The calculated path will consist of a series of straight line segments
+between obstacles, and geodesics over the obstacles.
+
+Force elements defined elsewhere may make use of cable paths to apply forces
+to the system, by calculating a uniform tension in the cable that may depend
+on the cable kinematics calculated here. Cable kinematics includes the path,
+the cable length, and the cable "rate", defined as the time derivative of
+length. The path and length are available at Position stage, the rate is
+available at Velocity stage.
+
+During construction, one or more CablePath objects are defined by giving for
+each CablePath an origin and end point and an ordered set of geometric
+obstacles represented either by surfaces or "via" points. Via points are like
+frictionless eyelets that the cable must pass through and can generate forces
+in any direction perpendicular to the cable; surfaces are one-sided and can
+only apply positive forces to the cable. Thus the cable path does not
+necessarily touch all the obstacles; the obstacles that it does touch are
+called the "active" obstacles. Via points are always active.
+
+Every obstacle and point is rigidly fixed to Ground or some moving body of
+the multibody system, with its pose or station point provided. Any number of
+obstacles may be placed on one body. **/
+class SimTK_SIMBODY_EXPORT CableTrackerSubsystem : public Subsystem {
+public:
+CableTrackerSubsystem();
+explicit CableTrackerSubsystem(MultibodySystem&);
+
+/** Get the number of cable paths being managed by this cable tracker subsystem.
+These are identified by CablePathIndex values from 0 to getNumCablePaths()-1.
+This is available after realizeTopology() and does not change subsequently. **/
+int getNumCablePaths() const;
+
+/** Get const access to a particular cable path. **/
+const CablePath& getCablePath(CablePathIndex cableIx) const;
+/** Get writable access to a particular cable path. **/
+CablePath& updCablePath(CablePathIndex cableIx);
+
+/** @cond **/ // Hide from Doxygen.
+SimTK_PIMPL_DOWNCAST(CableTrackerSubsystem, Subsystem);
+class Impl;
+Impl& updImpl();
+const Impl& getImpl() const;
+/** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_CABLE_TRACKER_SUBSYSTEM_H_
diff --git a/Simbody/include/simbody/internal/CompliantContactSubsystem.h b/Simbody/include/simbody/internal/CompliantContactSubsystem.h
new file mode 100644
index 0000000..eb8ba4c
--- /dev/null
+++ b/Simbody/include/simbody/internal/CompliantContactSubsystem.h
@@ -0,0 +1,866 @@
+#ifndef SimTK_SIMBODY_COMPLIANT_CONTACT_SUBSYSTEM_H_
+#define SimTK_SIMBODY_COMPLIANT_CONTACT_SUBSYSTEM_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-13 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/ForceSubsystem.h"
+
+#include <cassert>
+
+namespace SimTK {
+
+class MultibodySystem;
+class SimbodyMatterSubsystem;
+class ContactTrackerSubsystem;
+class ContactForceGenerator;
+class Contact;
+class ContactForce;
+class ContactPatch;
+
+
+
+//==============================================================================
+// COMPLIANT CONTACT SUBSYSTEM
+//==============================================================================
+/** This is a force subsystem that implements a compliant contact model to
+respond to Contact objects as detected by a ContactTrackerSubsystem. The
+subsystem contains an extendable collection of ContactForceGenerator
+objects, one per type of Contact. For example, a point contact would be
+handled by a different force generator than would a mesh contact. **/
+class SimTK_SIMBODY_EXPORT CompliantContactSubsystem : public ForceSubsystem {
+public:
+/** Default constructor creates an empty handle. **/
+CompliantContactSubsystem() {}
+
+/** Add a new CompliantContactSubsystem to the indicated MultibodySystem,
+specifying the ContactTrackerSubsystem that we will use to obtain the list
+of ActiveContacts from which we will generate compliant forces. The
+MultibodySystem takes over ownership of the new subsystem object, but the
+handle we construct here retains a reference to it. **/
+CompliantContactSubsystem(MultibodySystem&,
+ const ContactTrackerSubsystem&);
+
+/** Get the transition velocity (vt) of the friction model. **/
+Real getTransitionVelocity() const;
+/** Set the transition velocity (vt) of the friction model. **/
+void setTransitionVelocity(Real vt);
+/** Get a precalculated 1/vt to avoid expensive runtime divisions. **/
+Real getOOTransitionVelocity() const;
+
+/** Specify whether to track energy dissipated by contacts. This permits
+conservation of energy tests to be performed in the presence of dissipative
+contact elements. If you enable this, a state variable (z) will be allocated
+to integrate instantaneous dissipated power. By default energy dissipation is
+not tracked and an exception is thrown if it is requested. This is a topological
+change, meaning you'll have to call realizeTopology() and get a new State if
+you change the setting.
+ at see getDissipatedEnergy(),setDissipatedEnergy(),getTrackDissipatedEnergy() **/
+void setTrackDissipatedEnergy(bool shouldTrack);
+/** Obtain the current setting of the "track dissipated energy" flag. You
+must not call getDissipatedEnergy() or setDissipatedEnergy() if this flag
+has not been set.
+ at see getDissipatedEnergy(),setDissipatedEnergy(),setTrackDissipatedEnergy() **/
+bool getTrackDissipatedEnergy() const;
+
+/** Determine how many of the active Contacts are currently generating
+contact forces. You can call this at Velocity stage or later; the contact
+forces will be realized first if necessary before we report how many there
+are. **/
+int getNumContactForces(const State& state) const;
+/** For each active Contact, get a reference to the most recently calculated
+force there; the ContactId that produced this force is available from the
+referenced ContactForce object. The ContactForce object is measured and
+expressed in the Ground frame. You can request this response at Velocity stage
+or later; the contact forces will be realized first if necessary.
+ at see getContactForceById() **/
+const ContactForce& getContactForce(const State& state, int n) const;
+/** Get a reference to the ContactForce currently being produced by a particular
+ContactId; if that Contact is not currently producing forces then a reference to
+an invalid ContactForce is returned (you can check with isValid()). The
+ContactForce object is measured and expressed in the Ground frame. You can call
+this at Velocity stage or later; the contact forces (not just this one) will be
+realized first if necessary.
+ at see getContactForce(), calcContactPatchDetailsById() **/
+const ContactForce& getContactForceById(const State& state, ContactId id) const;
+
+/** Calculate detailed information about a particular active contact patch,
+including deformed geometric information, pressure and friction force
+distribution, and resultant forces and moments. This detailed information is
+only calculated when requested because it may be expensive for some
+ContactForceGenerators; for simulation purposes only the resultants are needed.
+Resultant forces may be obtained cheaply by calling getContactForceById(); don't
+call this method unless you really need these details. The returned information
+will overwrite anything that is already in the supplied ContactPatch object; the
+contact point and vectors are expressed in Ground. You can call this operator at
+Velocity stage or higher. The result is calculated here and not saved internally.
+
+ at pre \a state realized to Stage::Velocity
+ at param[in] state
+ The state from whose active contacts we are selecting.
+ at param[in] id
+ The ContactId of the Contact whose patch details are to be returned. This
+ refers to a Contact within the ContactTrackerSubsystem associated with this
+ ContactForceSubsystem.
+ at param[out] patch
+ The object that will be overwritten with the result or set invalid
+ if the given ContactId is not currently producing a patch. Results are
+ returned in the Ground frame.
+ at return
+ True if the indicated Contact is currently generating contact forces;
+ false otherwise (in which case \a patch is also marked invalid).
+
+ at see getContactForceById() **/
+bool calcContactPatchDetailsById(const State& state,
+ ContactId id,
+ ContactPatch& patch) const;
+
+/** Obtain the total amount of energy dissipated by all the contact responses
+that were generated by this subsystem since some arbitrary starting point. This
+information is available only if you have requested tracking by calling
+setTrackDissipatedEnergy().
+
+This is the time integral of all the power dissipated during any of the contacts
+by material dissipative and friction forces. For a system whose only
+non-conservative forces are contacts, the sum of potential, kinetic, and
+dissipated energies should be conserved with an exception noted below. This is
+particularly useful for debugging new ContactForceGenerators. This is a
+State variable so you can obtain its value any time after it is allocated.
+ at pre \a state realized to Stage::Model
+ at param[in] state
+ The State from which to obtain the current value of the dissipated energy.
+ at return
+ The total dissipated energy (a nonnegative scalar).
+
+An error message will be thrown if you call this method without having
+turned on energy tracking via setTrackDissipatedEnergy().
+
+ at note The Hunt and Crossley dissipation model used by many contact force
+generators can occasionally detect that a body is being "yanked" out of
+a contact (this is more likely with very large dissipation coefficients).
+To track all the energy in that case we would have to allow the
+surfaces to stick together, which is unreasonable. Instead, some of the
+energy will be lost to unmodeled effects because the surface is unable to
+transfer energy back to the bodies and will instead vibrate or ring until
+the energy is dissipated.
+
+ at see setTrackDissipatedEnergy(), setDissipatedEnergy() **/
+Real getDissipatedEnergy(const State& state) const;
+
+/** Set the accumulated dissipated energy to an arbitrary value. This is only
+permitted if you have requested tracking of dissipated energy by calling
+setTrackDissipatedEnergy().
+
+Typically this method is used only to reset the dissipated energy to zero, but
+non-zero values can be useful if you are trying to match some existing data or
+continuing a simulation. This is a State variable so you can set its
+value any time after it is allocated.
+ at pre \a state realized to Stage::Model
+ at param[in,out] state
+ The State whose dissipated energy variable for this subsystem is to
+ be modified.
+ at param[in] energy
+ The new value for the accumulated dissipated energy (must be a
+ nonnegative scalar).
+
+An error message will be thrown if you call this method without having
+turned on energy tracking via setTrackDissipatedEnergy().
+
+ at see getDissipatedEnergy(), setTrackDissipatedEnergy() **/
+void setDissipatedEnergy(State& state, Real energy) const;
+
+
+/** Attach a new generator to this subsystem as the responder to be used when
+we see the kind of Contact type for which this generator is defined,
+replacing the previous generator for this Contact type if there was one. The
+subsystem takes over ownership of the generator; don't delete it yourself. **/
+void adoptForceGenerator(ContactForceGenerator* generator);
+
+/** Attach a new generator to this subsystem as the responder to be used when
+we see a Contact type for which no suitable generator has been defined,
+replacing the previous default generator type if there was one. The
+subsystem takes over ownership of the generator; don't delete it yourself. **/
+void adoptDefaultForceGenerator(ContactForceGenerator* generator);
+
+/** Return true if this subsystem has a force generator registered that can
+respond to this kind of Contact. **/
+bool hasForceGenerator(ContactTypeId contact) const;
+
+/** Return true if this subsystem has a force generator registered that can
+be used for response to an unrecognized type of Contact (typically this will
+be a "do nothing" or "throw an error" generator. **/
+bool hasDefaultForceGenerator() const;
+
+/** Return the force generator to be used for a Contact of the indicated
+type. If no generator was registered for this type of contact, this will
+be the default generator. **/
+const ContactForceGenerator&
+ getContactForceGenerator(ContactTypeId contact) const;
+
+/** Return the force generator to be used for a Contact type for which no
+suitable force generator has been registered. **/
+const ContactForceGenerator& getDefaultForceGenerator() const;
+
+/** Get a read-only reference to the ContactTrackerSubsystem associated
+with this CompliantContactSubsystem. This is the contact tracker that is
+maintaining the list of contacts for which this subsystem will be providing
+the response forces. **/
+const ContactTrackerSubsystem& getContactTrackerSubsystem() const;
+
+/** Every Subsystem is owned by a System; a CompliantContactSubsystem expects
+to be owned by a MultibodySystem. This method returns a const reference
+to the containing MultibodySystem and will throw an exception if there is
+no containing System or it is not a MultibodySystem. **/
+const MultibodySystem& getMultibodySystem() const;
+
+/** @cond **/ // don't show in Doxygen docs
+SimTK_PIMPL_DOWNCAST(CompliantContactSubsystem, ForceSubsystem);
+/** @endcond **/
+
+//--------------------------------------------------------------------------
+ private:
+class CompliantContactSubsystemImpl& updImpl();
+const CompliantContactSubsystemImpl& getImpl() const;
+};
+
+
+
+//==============================================================================
+// CONTACT FORCE
+//==============================================================================
+/** This is a simple class containing the basic force information for a
+single contact between deformable surfaces S1 and S2 mounted on rigid
+bodies B1 and B2. Every contact interaction between two rigid bodies, however
+complex, can be expressed as a resultant that can be contained in this class
+and is sufficient for advancing a simulation. Optionally, you may be able to
+get more details about the deformed geometry and pressure distribution over the
+patch but you have to ask for that separately because it can be expensive to
+calculate or report.
+
+The information stored here is:
+ - A point in space at which equal and opposite forces will be applied to
+ corresponding stations of the two interacting rigid bodies. This is called
+ the "contact point".
+ - The force vector to be applied there (to each body with opposite sign).
+ - A moment vector to be applied to both bodies (with opposite signs).
+ - The potential energy currently stored by the elasticity of the contacting
+ materials.
+ - The instantaneous power dissipation due to inelastic behavior such as
+ friction and internal material damping.
+
+Points and vectors are measured and expressed in some assumed but unspecified
+frame A, which might for example be the frame of one of the two surfaces, or
+one of the two bodies, or Ground. Whenever a method returns a ContactForce
+object it must document what frame is being used; typically that will be
+Ground for end user use.
+
+<h3>Definition of center of pressure</h3>
+
+When the contact patch itself involves many distributed contact points, the
+center of pressure might not be a particularly meaningful quantity but
+serves to provide enough information to proceed with a simulation, and to
+display resultant contact forces, without requiring detailed patch geometry
+information. We define the location r_c of the center of pressure like this:
+ at verbatim
+ sum_i (r_i * |r_i X Fn_i|)
+ r_c = --------------------------
+ sum_i |r_i X Fn_i|
+ at endverbatim
+where r_i is the vector locating contact point i, F_i=Fn_i+Ft_i is the
+contact force at point i resolved into locally-normal and locally-tangential
+components, and M_i is a pure moment if any generated by the contact force
+model as a result of the i'th contact point. Note that the locally-
+tangent contact force Ft_i (presumably from friction) and pure moment M_i
+do not contribute to the center of pressure calculation; we're choosing
+the point that minimizes the net moment generated by normal ("pressure")
+forces only. Note that the normal force Fn_i includes both stiffness and
+dissipation contributions, so the center of pressure can be
+velocity-dependent. **/
+class ContactForce {
+public:
+/** Default constructor has invalid contact id, other fields garbage. **/
+ContactForce() {} // invalid
+
+/** Construct with values for all fields. Contact point and force must
+be in the same but unspecified frame (typically Ground), and force and
+moment must be as applied at the contact point; we can't check so don't
+mess up. **/
+ContactForce(ContactId id, const Vec3& contactPt,
+ const SpatialVec& forceOnSurface2,
+ Real potentialEnergy, Real powerLoss)
+: m_contactId(id), m_contactPt(contactPt),
+ m_forceOnSurface2(forceOnSurface2),
+ m_potentialEnergy(potentialEnergy), m_powerLoss(powerLoss) {}
+
+/** Return the ContactId of the Contact that generated this ContactForce. **/
+ContactId getContactId() const {return m_contactId;}
+/** This is the point at which this contact element applies equal and opposite
+forces to the two bodies, chosen as the center of pressure for composite
+contact patches. **/
+const Vec3& getContactPoint() const {return m_contactPt;}
+/** Get the total spatial force applied to body 2 at the contact point
+(that is, a force and a moment); negate this to find the force applied to
+body 1 at the same point. **/
+const SpatialVec& getForceOnSurface2() const {return m_forceOnSurface2;}
+/** Get the amount of potential energy currently stored in the deformation of
+this contact patch. **/
+Real getPotentialEnergy() const {return m_potentialEnergy;}
+/** Get the energy dissipation rate (power loss) due to the deformation rate
+and friction losses for this contact patch (this is a signed value with
+positive indicating dissipation). **/
+Real getPowerDissipation() const {return m_powerLoss;}
+
+/** Replace the current contents of this ContactForce object with the
+given arguments. This provides values for all fields. Contact point and
+force must be in the same but unspecified frame (typically Ground), and
+force and moment must be as applied at the contact point; we can't check
+so don't mess up. **/
+void setTo(ContactId id, const Vec3& contactPt,
+ const SpatialVec& forceOnSurface2,
+ Real potentialEnergy, Real powerLoss)
+{ m_contactId = id;
+ m_contactPt = contactPt;
+ m_forceOnSurface2 = forceOnSurface2;
+ m_potentialEnergy = potentialEnergy;
+ m_powerLoss = powerLoss; }
+
+/** Change the ContactId contained in this ContactForce object. **/
+void setContactId(ContactId id) {m_contactId=id;}
+/** Change the contact point contained in this ContactForce object. **/
+void setContactPoint(const Vec3& contactPt) {m_contactPt=contactPt;}
+/** Change the value stored in this ContactForce object for the spatial
+force being applied on surface 2. **/
+void setForceOnSurface2(const SpatialVec& forceOnSurface2)
+{ m_forceOnSurface2=forceOnSurface2; }
+/** Change the value stored for potential energy in this ContactForce object. **/
+void setPotentialEnergy(Real potentialEnergy)
+{ m_potentialEnergy=potentialEnergy; }
+/** Change the value stored for power loss in this ContactForce object. **/
+void setPowerDissipation(Real powerLoss) {m_powerLoss=powerLoss;}
+
+/** Restore the ContactForce object to its default-constructed state with
+an invalid contact id and garbage for the other fields. **/
+void clear() {m_contactId.invalidate();}
+/** Return true if this contact force contains a valid ContactId. **/
+bool isValid() const {return m_contactId.isValid();}
+
+/** This object is currently in an assumed frame A; given a transform from
+another frame B to A we'll re-measure and re-express this in B. Cost is
+48 flops. Note that this doesn't change the moment because we're not moving
+the force application point physically; just remeasuring it from B. **/
+void changeFrameInPlace(const Transform& X_BA) {
+ m_contactPt = X_BA*m_contactPt; // shift & reexpress in B
+ m_forceOnSurface2 = X_BA.R()*m_forceOnSurface2; // reexpress in B
+}
+
+private:
+ContactId m_contactId; // Which Contact produced this force?
+Vec3 m_contactPt; // In some frame A
+SpatialVec m_forceOnSurface2; // at contact pt, in A; neg. for Surf1
+Real m_potentialEnergy; // > 0 when due to compression
+Real m_powerLoss; // > 0 means dissipation
+};
+
+// For debugging.
+inline std::ostream& operator<<(std::ostream& o, const ContactForce& f) {
+ o << "ContactForce for ContactId " << f.getContactId() << " (ground frame):\n";
+ o << " contact point=" << f.getContactPoint() << "\n";
+ o << " force on surf2 =" << f.getForceOnSurface2() << "\n";
+ o << " pot. energy=" << f.getPotentialEnergy()
+ << " powerLoss=" << f.getPowerDissipation();
+ return o << "\n";
+}
+
+//==============================================================================
+// CONTACT DETAIL
+//==============================================================================
+/** This provides deformed geometry and force details for one element of a
+contact patch that may be composed of many elements. Every element generates
+equal and opposite forces on both surfaces so we can use a consistent
+patch-centered convention for reporting details regardless of the original
+source of the element.
+
+<h3>Deformed patch geometry</h3>
+
+Vector results are expressed in Ground. We return
+ - the contact point C where equal and opposite forces are applied to
+ <em>material points</em> of the two surfaces.
+ - the contact normal direction n pointing \e away from surface1's exterior
+ and towards surface2's interior
+ - the slip velocity v of surface 2 relative to surface 1, measured at C
+ and in the plane perpendicular to n
+ - the force applied to body 2 by body 1; that is, its normal component
+ is in direction n and its tangential component opposes velocity v
+
+It is important to note that we are interested in the motion (deformations) of
+<em>material points</em> here, \e not the motion of the <em>contact point</em>
+which is a non-material concept and thus not directly involved in producing
+forces. The patch area is provided separately and its significance also depends
+on the model.
+
+<h3>Elasticity and dissipation</h3>
+
+We report the combined deformation (>= 0) of the two surfaces at C, along the
+normal n. We do not attempt to report angular deformation of the element. Elastic
+deformation rate is also provided. This is the rate that the material points
+currently at C are compressing or relaxing along the normal. That is, we are not
+including the fact that the contact point C may be moving along
+the surfaces, we just report what is happening to the material in its own
+frame. Note that we do not attempt to report angular deformation rate.
+
+<h3>Slipping and friction</h3>
+
+Slip velocity measures the relative velocity of the body stations that are
+instantaneously coincident with C. This will be in the plane for which n is the
+normal, but it is expressed in Ground so is a 3d vector.
+
+<h3>%Force and pressure</h3>
+
+%Contact force is the force being
+applied to surface 2 by surface 1 at the contact point C. This includes
+the normal force due to elasticity and dissipation, and
+the tangential force due to friction and to tangential elasticity and
+dissipation, if any.
+
+Peak pressure is a scalar providing the worst-case pressure present somewhere
+in the patch, if the model can provide that. Otherwise it will be the
+normal force divided by the patch area to give the average pressure across
+the patch.
+
+<h3>Energy and power</h3>
+
+Potential energy is the amount of energy currently stored in the elastic
+deformation of this element. Summing this over all the contact detail
+elements should yield the same value as is reported as the resultant potential
+energy for this contact.
+
+Power loss is the rate at which energy is being lost due to dissipation and
+to friction, but not due to elastic deformation because that is contributing
+to potential energy and we expect to get it back. Summing this over all the
+contact detail elements should yield the same value as is reported as the
+resultant power loss for this contact. **/
+class ContactDetail {
+public:
+/** This is the point at which this contact element applies equal and opposite
+forces to the two bodies. **/
+const Vec3& getContactPoint() const {return m_contactPt;}
+/** This is the normal direction for this contact element, pointing away
+from body 1's exterior and towards body 2's interior, that is, in the
+direction of the normal force applied to body 2 by body1. **/
+const UnitVec3& getContactNormal() const {return m_patchNormal;}
+/** Get the relative slip velocity between the bodies at the contact point,
+as body 2's velocity in body 1. This is a vector in the contact plane, that is,
+it is perpendicular to the contact normal. **/
+const Vec3& getSlipVelocity() const {return m_slipVelocity;}
+/** Get the total force applied to body 2 at the contact point by this
+contact element; negate this to find the force on body 1 at the same point. **/
+const Vec3& getForceOnSurface2() const {return m_forceOnSurface2;}
+/** Get the total normal material deformation at the contact point; this is the
+sum of the deformations of the two surfaces there. This is sometimes called the
+"approach" of the two bodies and represents the amount of overlap of the
+\e undeformed surfaces at this point; that is, they have to deform this much
+in order not to overlap. **/
+Real getDeformation() const {return m_deformation;}
+/** Get the instantaneous rate at which the material at the contact point is
+deforming; this is the material derivative of the deformation and does not
+reflect that fact that the contact point itself is changing with time.
+Energy dissipation in the material depends on the rate at which the \e material
+is deforming, it does not depend on contact point changes. **/
+Real getDeformationRate() const {return m_deformationRate;}
+/** This is the surface area represented by this contact element. **/
+Real getPatchArea() const {return m_patchArea;}
+/** This is the peak pressure on this element; typically it is just the
+normal force divided by the patch area since most contact models assume
+constant force across a contact element's patch. **/
+Real getPeakPressure() const {return m_peakPressure;}
+/** Get the amount of potential energy currently stored in the deformation of
+this contact element. **/
+Real getPotentialEnergy() const {return m_potentialEnergy;}
+/** Get the energy dissipation rate (power loss) due to the deformation rate and
+friction losses for this element (this is a signed value with positive indicating
+dissipation). **/
+Real getPowerDissipation() const {return m_powerLoss;}
+
+
+/** This object is currently in an assumed frame A; given a transform from
+another frame B to A we'll re-measure and re-express this in B. Cost is
+63 flops. **/
+void changeFrameInPlace(const Transform& X_BA) {
+ const Rotation& R_BA = X_BA.R();
+ m_contactPt = X_BA*m_contactPt; // shift & reexpress in B (18 flops)
+ m_patchNormal = R_BA*m_patchNormal; // reexpress only (3*15 flops)
+ m_slipVelocity = R_BA*m_slipVelocity; // "
+ m_forceOnSurface2 = R_BA*m_forceOnSurface2; // "
+}
+
+/** Assuming that this object is currently reporting surface 2 information in
+frame A, here we want to both change the frame to B and swap which surface is to
+be considered as surface 2. Cost is 72 flops. **/
+void changeFrameAndSwitchSurfacesInPlace(const Transform& X_BA) {
+ const Rotation& R_BA = X_BA.R();
+ m_contactPt = X_BA*m_contactPt; // shift & reexpress in B (18 flops)
+ m_patchNormal = R_BA*(-m_patchNormal); // reverse & reexpress (3*18 flops)
+ m_slipVelocity = R_BA*(-m_slipVelocity); // "
+ m_forceOnSurface2 = R_BA*(-m_forceOnSurface2); // "
+}
+
+Vec3 m_contactPt; // location of contact point C in A
+UnitVec3 m_patchNormal; // points outwards from body 1, exp. in A
+Vec3 m_slipVelocity; // material slip rate, perp. to normal, in A
+Vec3 m_forceOnSurface2; // applied at C, -force to surf1
+Real m_deformation; // total normal compression (approach)
+Real m_deformationRate; // d/dt deformation, w.r.t. A frame
+Real m_patchArea; // >= 0
+Real m_peakPressure; // > 0 in compression
+Real m_potentialEnergy; // > 0 when due to compression
+Real m_powerLoss; // > 0 means dissipation
+};
+
+// For debugging.
+inline std::ostream& operator<<(std::ostream& o, const ContactDetail& d) {
+ o << "ContactDetail (ground frame):\n";
+ o << " contact point=" << d.m_contactPt << "\n";
+ o << " contact normal=" << d.m_patchNormal << "\n";
+ o << " slip velocity=" << d.m_slipVelocity << "\n";
+ o << " force on surf2 =" << d.m_forceOnSurface2 << "\n";
+ o << " deformation=" << d.m_deformation
+ << " deformation rate=" << d.m_deformationRate << "\n";
+ o << " patch area=" << d.m_patchArea
+ << " peak pressure=" << d.m_peakPressure << "\n";
+ o << " pot. energy=" << d.m_potentialEnergy << " powerLoss=" << d.m_powerLoss;
+ return o << "\n";
+}
+
+
+
+//==============================================================================
+// CONTACT PATCH
+//==============================================================================
+/** A ContactPatch is the description of the forces and the deformed shape of
+the contact surfaces that result from compliant contact interactions. This
+should not be confused with the Contact object that describes only the
+overlap in \e undeformed surface geometry and knows nothing of forces or
+materials. Although there are several qualitatively different kinds of
+compliant contact models, we assume that each can be described by some number
+of contact "elements" and report the detailed results in terms
+of those elements. Depending on the model, the elements may be associated with
+the surfaces or may be associated with the patch. Either way, every element
+applies equal and opposite forces to both surfaces. There is not necessarily
+any direct correspondence between elements on one surface with elements on the
+other.
+
+A Hertz contact will have only a single element that belongs to the patch,
+while an elastic foundation contact will have many, with each element
+associated with one or the other surface. Only the elements that are currently
+participating in contact will have entries here; the surface and element id is
+stored with each piece of ContactDetail information. There is also some basic
+information needed to advance the simulation and that is common to all contact
+patch types and stored as a ContactForce resultant. **/
+class SimTK_SIMBODY_EXPORT ContactPatch {
+public:
+void clear() {m_resultant.clear(); m_elements.clear();}
+bool isValid() const {return m_resultant.isValid();}
+const ContactForce& getContactForce() const {return m_resultant;}
+int getNumDetails() const {return (int)m_elements.size();}
+const ContactDetail& getContactDetail(int n) const {return m_elements[n];}
+
+/** This object is currently in an assumed frame A; given a transform from
+another frame B to A we'll re-measure and re-express this in B. This is an
+expensive operation. **/
+void changeFrameInPlace(const Transform& X_BA) {
+ m_resultant.changeFrameInPlace(X_BA);
+ for (unsigned i=0; i<m_elements.size(); ++i)
+ m_elements[i].changeFrameInPlace(X_BA);
+}
+
+ContactForce m_resultant;
+Array_<ContactDetail> m_elements;
+};
+
+
+
+//==============================================================================
+// CONTACT FORCE GENERATOR
+//==============================================================================
+/** A ContactForceGenerator implements an algorithm for responding to overlaps
+or potential overlaps between pairs of ContactSurface objects, as detected by
+a ContactTrackerSubsystem. This class is used internally by
+CompliantContactSubsystem and there usually is no reason to access it directly.
+The exception is if you are defining a new Contact subclass (very rare). In
+that case, you will also need to define one or more ContactForceGenerators to
+respond to Contacts with the new type, then register it with the
+CompliantContactSubsystem. **/
+class SimTK_SIMBODY_EXPORT ContactForceGenerator {
+public:
+class ElasticFoundation; // for TriangleMeshContact
+class HertzCircular; // for PointContact
+class HertzElliptical; // for EllipticalPointContact
+
+// These are for response to unknown ContactTypeIds.
+class DoNothing; // do nothing if called
+class ThrowError; // throw an error if called
+
+/** Base class constructor for use by the concrete classes. **/
+explicit ContactForceGenerator(ContactTypeId type): m_contactType(type) {}
+
+/** Return the ContactTypeId handled by this force generator. ContactTypeId(0)
+is reserved and is used here for fallback force generators that deal with
+unrecognized ContactTypeIds. **/
+ContactTypeId getContactTypeId() const {return m_contactType;}
+
+const CompliantContactSubsystem& getCompliantContactSubsystem() const
+{ assert(m_compliantContactSubsys); return *m_compliantContactSubsys; }
+void setCompliantContactSubsystem(const CompliantContactSubsystem* sub)
+{ m_compliantContactSubsys = sub; }
+
+/** Base class destructor is virtual but does nothing. **/
+virtual ~ContactForceGenerator() {}
+
+/** The CompliantContactSubsystem will invoke this method on any
+active contact pair of the right Contact type for which there is overlapping
+undeformed geometry. The force generator is expected to calculate a point
+in space where equal and opposite contact forces should be applied to the two
+contacting rigid bodies, the potential energy currently stored in this
+contact, and the power (energy dissipation rate). State should be used
+for instance info only; use position information from \a overlapping and
+velocity information from the supplied arguments. That allows this method
+to be used as an operator, for example to calculate potential energy when
+velocities are not yet available. **/
+virtual void calcContactForce
+ (const State& state,
+ const Contact& overlapping,
+ const SpatialVec& V_S1S2, // relative surface velocity (S2 in S1)
+ ContactForce& contactForce) const = 0;
+
+/** The CompliantContactSubsystem will invoke this method in response to a user
+request for contact patch information; this returns force, potential energy,
+and power as above but may also require expensive computations that can be
+avoided in calcContactForce(). Don't use the state for position or
+velocity information; the only allowed positions are in the Contact object
+and the velocities are supplied explicitly. **/
+virtual void calcContactPatch
+ (const State& state,
+ const Contact& overlapping,
+ const SpatialVec& V_S1S2, // relative surface velocity (S2 in S1)
+ ContactPatch& patch) const = 0;
+
+
+//--------------------------------------------------------------------------
+private:
+ // This generator should be called only for Contact objects of the
+ // indicated type id.
+ ContactTypeId m_contactType;
+ // This is a reference to the owning CompliantContactSubsystem if any;
+ // don't delete on destruction.
+ const CompliantContactSubsystem* m_compliantContactSubsys;
+};
+
+
+
+
+//==============================================================================
+// HERTZ CIRCULAR GENERATOR
+//==============================================================================
+
+/** This ContactForceGenerator handles contact between non-conforming
+objects that meet at a point and generate a circular contact patch; those
+generate a CircularPointContact tracking object. Although this is just a special
+case of elliptical contact we treat it separately so we can take advantage
+of the significant simplifications afforded by circular contact. **/
+class SimTK_SIMBODY_EXPORT ContactForceGenerator::HertzCircular
+: public ContactForceGenerator {
+public:
+HertzCircular()
+: ContactForceGenerator(CircularPointContact::classTypeId()) {}
+
+virtual ~HertzCircular() {}
+virtual void calcContactForce
+ (const State& state,
+ const Contact& overlapping,
+ const SpatialVec& V_S1S2,
+ ContactForce& contactForce) const;
+
+virtual void calcContactPatch
+ (const State& state,
+ const Contact& overlapping,
+ const SpatialVec& V_S1S2,
+ ContactPatch& patch) const;
+};
+
+
+
+//==============================================================================
+// HERTZ ELLIPTICAL GENERATOR
+//==============================================================================
+
+/** This ContactForceGenerator handles contact between non-conforming
+objects that meet at a point and generate an elliptical contact patch; those
+generate an EllipticalPointContact tracking object. For objects that are
+known to produce circular contact, use the specialized HertzCircular
+generator instead. **/
+class SimTK_SIMBODY_EXPORT ContactForceGenerator::HertzElliptical
+: public ContactForceGenerator {
+public:
+HertzElliptical()
+: ContactForceGenerator(EllipticalPointContact::classTypeId()) {}
+
+virtual ~HertzElliptical() {}
+virtual void calcContactForce
+ (const State& state,
+ const Contact& overlapping,
+ const SpatialVec& V_S1S2,
+ ContactForce& contactForce) const;
+
+virtual void calcContactPatch
+ (const State& state,
+ const Contact& overlapping,
+ const SpatialVec& V_S1S2,
+ ContactPatch& patch) const;
+};
+
+
+
+//==============================================================================
+// ELASTIC FOUNDATION GENERATOR
+//==============================================================================
+/** This ContactForceGenerator handles contact between a TriangleMesh
+and a variety of other geometric objects, all of which produce a
+TriangleMeshContact tracking object. **/
+class SimTK_SIMBODY_EXPORT ContactForceGenerator::ElasticFoundation
+: public ContactForceGenerator {
+public:
+ElasticFoundation()
+: ContactForceGenerator(TriangleMeshContact::classTypeId()) {}
+virtual ~ElasticFoundation() {}
+virtual void calcContactForce
+ (const State& state,
+ const Contact& overlapping,
+ const SpatialVec& V_S1S2,
+ ContactForce& contactForce) const;
+
+virtual void calcContactPatch
+ (const State& state,
+ const Contact& overlapping,
+ const SpatialVec& V_S1S2,
+ ContactPatch& patch) const;
+
+private:
+void calcContactForceAndDetails
+ (const State& state,
+ const Contact& overlapping,
+ const SpatialVec& V_S1S2,
+ ContactForce& contactForce,
+ Array_<ContactDetail>* contactDetails) const;
+
+void calcWeightedPatchCentroid
+ (const ContactGeometry::TriangleMesh& mesh,
+ const std::set<int>& insideFaces,
+ Vec3& weightedPatchCentroid,
+ Real& patchArea) const;
+
+void processOneMesh
+ (const State& state,
+ const ContactGeometry::TriangleMesh& mesh,
+ const std::set<int>& insideFaces,
+ const Transform& X_MO,
+ const SpatialVec& V_MO,
+ const ContactGeometry& other,
+ Real meshDeformationFraction, // 0..1
+ Real areaScaleFactor, // >= 0
+ Real k, Real c, Real us, Real ud, Real uv,
+ const Vec3& resultantPt_M, // where to apply forces
+ SpatialVec& resultantForceOnOther_M, // at resultant pt
+ Real& potentialEnergy,
+ Real& powerLoss,
+ Vec3& weightedCenterOfPressure_M, // COP
+ Real& sumOfAllPressureMoments, // COP weight
+ Array_<ContactDetail>* contactDetails) const;
+};
+
+
+
+
+//==============================================================================
+// DO NOTHING FORCE GENERATOR
+//==============================================================================
+/** This ContactForceGenerator silently does nothing. It can be used as a way
+to explicitly ignore a certain ContactTypeId, or more commonly it is used as
+the fallback generator for unrecognized ContactTypeIds. **/
+class SimTK_SIMBODY_EXPORT ContactForceGenerator::DoNothing
+: public ContactForceGenerator {
+public:
+explicit DoNothing(ContactTypeId type = ContactTypeId(0))
+: ContactForceGenerator(type) {}
+virtual ~DoNothing() {}
+virtual void calcContactForce
+ (const State& state,
+ const Contact& overlapping,
+ const SpatialVec& V_S1S2,
+ ContactForce& contactForce) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactForceGenerator::DoNothing::calcContactForce() not implemented yet."); }
+virtual void calcContactPatch
+ (const State& state,
+ const Contact& overlapping,
+ const SpatialVec& V_S1S2,
+ ContactPatch& patch) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactForceGenerator::DoNothing::calcContactPatch() not implemented yet."); }
+};
+
+
+
+//==============================================================================
+// THROW ERROR FORCE GENERATOR
+//==============================================================================
+/** This ContactForceGenerator throws an error if it is every invoked. It can
+be used as a way to explicitly catch a certain ContactTypeId and complain, or
+more commonly it is used as the fallback generator for unrecognized
+ContactTypeIds. **/
+class SimTK_SIMBODY_EXPORT ContactForceGenerator::ThrowError
+: public ContactForceGenerator {
+public:
+explicit ThrowError(ContactTypeId type = ContactTypeId(0))
+: ContactForceGenerator(type) {}
+virtual ~ThrowError() {}
+virtual void calcContactForce
+ (const State& state,
+ const Contact& overlapping,
+ const SpatialVec& V_S1S2,
+ ContactForce& contactForce) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactForceGenerator::ThrowError::calcContactForce() not implemented yet."); }
+virtual void calcContactPatch
+ (const State& state,
+ const Contact& overlapping,
+ const SpatialVec& V_S1S2,
+ ContactPatch& patch) const
+{ SimTK_ASSERT_ALWAYS(!"implemented",
+ "ContactForceGenerator::ThrowError::calcContactPatch() not implemented yet."); }
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_COMPLIANT_CONTACT_SUBSYSTEM_H_
diff --git a/Simbody/include/simbody/internal/Constraint.h b/Simbody/include/simbody/internal/Constraint.h
new file mode 100644
index 0000000..a676a6b
--- /dev/null
+++ b/Simbody/include/simbody/internal/Constraint.h
@@ -0,0 +1,2530 @@
+#ifndef SimTK_SIMBODY_CONSTRAINT_H_
+#define SimTK_SIMBODY_CONSTRAINT_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) 2007-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+This defines the base Constraint class and related classes, which are used to
+specify limitations on the mobility of the mobilized bodies in a
+SimbodyMatterSubsystem. **/
+
+
+#include "SimTKmath.h"
+#include "simbody/internal/common.h"
+
+#include <cassert>
+
+namespace SimTK {
+
+class SimbodyMatterSubsystem;
+class SimbodyMatterSubtree;
+class MobilizedBody;
+class Constraint;
+class ConstraintImpl;
+
+// We only want the template instantiation to occur once. This symbol is
+// defined in the SimTK core compilation unit that defines the Constraint
+// class but should not be defined any other time.
+#ifndef SimTK_SIMBODY_DEFINING_CONSTRAINT
+ extern template class PIMPLHandle<Constraint, ConstraintImpl, true>;
+#endif
+
+ ///////////////////////////
+ // CONSTRAINT BASE CLASS //
+ ///////////////////////////
+
+/** This is the base class for all %Constraint classes, which is just a handle
+for the underlying hidden implementation. There is a set of built-in
+constraints and a generic "Custom" constraint (an abstract base class) from
+which advanced users may derive their own constraints. Each built-in constraint
+type is a local subclass within %Constraint, and is also derived from
+%Constraint.
+
+%Constraint is a PIMPL-style abstract base class, with concrete classes defined
+for each kind of constraint. **/
+class SimTK_SIMBODY_EXPORT Constraint
+: public PIMPLHandle<Constraint, ConstraintImpl, true> {
+public:
+/** Default constructor creates an empty %Constraint handle that can be used
+to reference any %Constraint. **/
+Constraint() { }
+/** For internal use: construct a new %Constraint handle referencing a
+particular implementation object. **/
+explicit Constraint(ConstraintImpl* r) : HandleBase(r) { }
+
+/** Disable this %Constraint, effectively removing it from the system. This
+is an Instance-stage change and affects the allocation of %Constraint-
+related resources in the supplied State. **/
+void disable(State&) const;
+
+/** Enable this %Constraint, without necessarily satisfying it. This is an
+Instance-stage change and affects the allocation of %Constraint-related
+resources in the supplied State. Note that merely enabling a constraint does
+not ensure that the State's positions and velocities satisfy that constraint;
+initial satisfaction requires use of an appropriate project() solver.
+ at see SimTK::System::project() **/
+void enable(State&) const;
+/** Test whether this constraint is currently disabled in the supplied
+State. **/
+bool isDisabled(const State&) const;
+/** Test whether this %Constraint is disabled by default in which case it must
+be explicitly enabled before it will take effect.
+ at see setDisabledByDefault(), enable() **/
+bool isDisabledByDefault() const;
+
+/** Normally Constraints are enabled when defined and can be disabled later. If
+you want to define this constraint but have it be off by default, use
+this method.
+ at see isDisabledByDefault(), enable(), disable(), isDisabled() **/
+void setDisabledByDefault(bool shouldBeDisabled);
+
+/** This is an implicit conversion from Constraint to ConstraintIndex when
+needed. This will fail if the %Constraint is not contained in a subsystem. **/
+operator ConstraintIndex() const {return getConstraintIndex();}
+
+/** Get a const reference to the matter subsystem that contains this
+%Constraint. This will throw an exception if the %Constraint has not yet been
+added to any subsystem; if you aren't sure use isInSubsystem() first to
+check.
+ at see updMatterSubsystem(), isInSubsystem(), getConstraintIndex() **/
+const SimbodyMatterSubsystem& getMatterSubsystem() const;
+
+/** Assuming you have writable access to this %Constraint, get a writable
+reference to the containing matter subsystem. This will throw an exception if
+the %Constraint has not yet been added to any subsystem; if you aren't sure
+use isInSubsystem() first to check.
+ at see getMatterSubsystem(), isInSubsystem(), getConstraintIndex() **/
+SimbodyMatterSubsystem& updMatterSubsystem();
+
+/** Get the ConstraintIndex that was assigned to this %Constraint when it was
+added to the matter subsystem. This will throw an exception if the %Constraint has not yet been
+added to any subsystem; if you aren't sure use isInSubsystem() first to
+check. There is also an implicit conversion from %Constraint to
+ConstraintIndex, so you don't normally need to call this directly.
+ at see getMatterSubsystem(), isInSubsystem() **/
+ConstraintIndex getConstraintIndex() const;
+
+/** Test whether this %Constraint is contained within a matter subsystem. **/
+bool isInSubsystem() const;
+/** Test whether the supplied MobilizedBody is in the same matter subsystem
+as this %Constraint. Also returns false if either the %Constraint or the
+%MobilizedBody is not in any subsystem, or if neither is. **/
+bool isInSameSubsystem(const MobilizedBody& mobod) const;
+
+ // TOPOLOGY STAGE (i.e., post-construction) //
+
+/** Return the number of unique bodies \e directly restricted by this
+constraint. Included are any bodies to which this %Constraint may apply a body
+force (i.e., torque or point force). The Ancestor body is not included unless
+it was specified as a Constrained Body. This is the length of the bodyForces
+array for this %Constraint. **/
+int getNumConstrainedBodies() const;
+
+/** Return a const reference to the actual MobilizedBody corresponding to one
+of the Constrained Bodies included in the count returned by
+getNumConstrainedBodies(). The index must be in the range
+0 <= \a consBodyIx < getNumConstrainedBodies(). **/
+const MobilizedBody& getMobilizedBodyFromConstrainedBody
+ (ConstrainedBodyIndex consBodyIx) const;
+
+/** Return a const reference to the actual MobilizedBody which is serving as
+the Ancestor body for the constrained bodies in this Constraint. This
+will fail if there are no constrained bodies (i.e., if
+getNumConstrainedBodies()==0). **/
+const MobilizedBody& getAncestorMobilizedBody() const;
+
+/** Return the number of unique mobilizers \e directly restricted by this
+%Constraint. Included are any mobilizers to which the %Constraint may
+apply any mobility force. Like bodies, mobilizers are referenced using the
+MobilizedBody containing them. Note that all the mobilities of a Constrained
+Mobilizer are included in the set of constrainable Qs or constrainable Us for
+this %Constraint even if not all of them are constrained. **/
+int getNumConstrainedMobilizers() const;
+
+/** Return a const reference to the actual MobilizedBody corresponding to one
+of the Constrained Mobilizers included in the count returned by
+getNumConstrainedMobilizers(). The index must be in the range
+0 <= \a consMobilizerIx < getNumConstrainedMobilizers(). **/
+const MobilizedBody& getMobilizedBodyFromConstrainedMobilizer
+ (ConstrainedMobilizerIndex consMobilizerIx) const;
+
+/** Return a subtree object indicating which parts of the multibody tree
+are potentially affected by this %Constraint. **/
+const SimbodyMatterSubtree& getSubtree() const;
+
+ // MODEL STAGE //
+// nothing in base class currently
+
+ // INSTANCE STAGE //
+
+/** Return the number of constrainable generalized coordinates q associated
+with a particular constrained mobilizer. This is just the number of generalized
+coordinates for that mobilizer; any or all of them may actually be
+unconstrained. **/
+int getNumConstrainedQ(const State&, ConstrainedMobilizerIndex) const;
+/** Return the number of constrainable mobilities u associated with a
+particular constrained mobilizer. This is just the number of generalized speeds
+for that mobilizer; any or all of them may actually be unconstrained. The
+number of constrainable udots is the same. **/
+int getNumConstrainedU(const State&, ConstrainedMobilizerIndex) const;
+
+/** Return the index into the constrained mobilities u array corresponding to a
+particular mobility of the indicated ConstrainedMobilizer. Don't confuse this
+with the set of \e participating mobilities which also includes all mobilities
+on each branch between the ancestor and a constrained body. The \e constrained
+mobilities are just those belonging to the mobilizers which are directly
+constrained. **/
+ConstrainedUIndex getConstrainedUIndex
+ (const State&, ConstrainedMobilizerIndex, MobilizerUIndex which) const;
+/** Return the index into the constrained coordinates q array corresponding to
+a particular coordinate of the indicated ConstrainedMobilizer. Don't confuse
+this with the set of \e participating coordinates which also includes all
+coordinates on each branch between the ancestor and a constrained body. The
+\e constrained coordinates are just those belonging to the mobilizers which are
+directly constrained. **/
+ConstrainedQIndex getConstrainedQIndex
+ (const State&, ConstrainedMobilizerIndex, MobilizerQIndex which) const;
+
+/** Return the sum of the number of coordinates q associated with each of
+the constrained mobilizers. **/
+int getNumConstrainedQ(const State&) const;
+
+/** Return the sum of the number of mobilities u associated with each of the
+constrained mobilizers. These are the only mobilities to which the constraint
+may directly apply a force, so this is also the dimension of the mobilityForces
+array. **/
+int getNumConstrainedU(const State&) const;
+
+/** Map one of this %Constraint's constrained q's to the corresponding index
+within the matter subsystem's whole q vector. **/
+QIndex getQIndexOfConstrainedQ(const State& state,
+ ConstrainedQIndex consQIndex) const;
+/** Map one of this %Constraint's constrained U's (or mobilities) to the
+corresponding index within the matter subsystem's whole u vector. **/
+UIndex getUIndexOfConstrainedU(const State& state,
+ ConstrainedUIndex consUIndex) const;
+
+/** Find out how many holonomic (position), nonholonomic (velocity), and
+acceleration-only constraint equations are currently being generated
+by this Constraint.
+
+ at param[in] state
+ The State from which the current status of this %Constraint is obtained.
+ Must have been realized through Instance stage.
+ at param[out] mp The number of holonomic constraint equations.
+ at param[out] mv The number of nonholonomic constraint equations.
+ at param[out] ma The number of acceleration-only constraint equations.
+
+Note that the counts here do not include the derivatives of the higher-order
+constraint equations, just the number at the level they were defined. **/
+void getNumConstraintEquationsInUse(const State& state,
+ int& mp, int& mv, int& ma) const;
+
+/** Return the start of the blocks of multipliers (or acceleration errors)
+assigned to this %Constraint. Separate blocks are allocated for holonomic
+(position), nonholonomic (velocity), and acceleration-only constraint
+equations. The size of each block is given by getNumConstraintEquationsInUse();
+this %Constraint's multipliers are assigned contiguously within each block.
+If any size is zero, the corresponding index is returned invalid.
+
+ at param[in] state
+ The State from which the current status of this %Constraint is obtained.
+ Must have been realized through Instance stage.
+ at param[out] px0
+ The index of the first slot for the second time derivatives of position
+ (holonomic) constraint equations.
+ at param[out] vx0
+ The index of the first slot for the time derivatives of velocity
+ (nonholonomic) constraint equations.
+ at param[out] ax0
+ The index of the first slot for the acceleration-only constraint equations.
+
+For position and velocity constraints, the multiplier slots correspond to
+the time derivatives of these constraints that are used to create acceleration
+constraints from them. **/
+void getIndexOfMultipliersInUse(const State& state,
+ MultiplierIndex& px0,
+ MultiplierIndex& vx0,
+ MultiplierIndex& ax0) const;
+
+/** Set the part of a complete constraint-space vector that belongs to this
+constraint. The full vector has dimension m=mp+mv+ma, that is, one entry
+per acceleration-level constraint equation.
+ at param[in] state
+ The State from which the current status of this %Constraint is obtained.
+ Must have been realized through Instance stage.
+ at param[in] myPart
+ The constraint-space scalars for this %Constraint in the order
+ position, velocity, acceleration if this %Constraint produces constraint
+ equations of different types. The number of entries must match the number
+ of constraint equations generated by this %Constraint.
+ at param[in,out] constraintSpace
+ An array of full constraint space dimension m. If it has length zero on
+ entry we'll resize it to m and initialize it to zero, otherwise the size
+ must be exactly m and we'll only modify the slots that belong to this
+ %Constraint.
+
+Note that we're writing only to the output argument; this method does not
+calculate or modify anything else.
+
+ at see getMyPartFromConstraintSpaceVector(), getIndexOfMultipliersInUse() **/
+void setMyPartInConstraintSpaceVector(const State& state,
+ const Vector& myPart,
+ Vector& constraintSpace) const;
+
+/** Get the part of a complete constraint-space vector that belongs to this
+constraint. The full vector has dimension m=mp+mv+ma, that is, one entry
+per acceleration-level constraint equation.
+ at param[in] state
+ The State from which the current status of this %Constraint is obtained.
+ Must have been realized through Instance stage.
+ at param[in] constraintSpace
+ An array of full constraint space dimension m. We will only examine the
+ entries belonging to this %Constraint.
+ at param[out] myPart
+ The constraint-space scalars for this %Constraint in the order
+ position, velocity, acceleration if this %Constraint produces constraint
+ equations of different types. The number of entries will match the number
+ of constraint equations generated by this %Constraint and the argument
+ will be resized if necessary.
+
+ at see setMyPartInConstraintSpaceVector(), getIndexOfMultipliersInUse() **/
+void getMyPartFromConstraintSpaceVector(const State& state,
+ const Vector& constraintSpace,
+ Vector& myPart) const;
+
+ // POSITION STAGE //
+/** Get a Vector containing the position errors. Many subclasses provide
+their own methods for getting this information in a more specific form. **/
+Vector getPositionErrorsAsVector(const State&) const; // mp of these
+Vector calcPositionErrorFromQ(const State&, const Vector& q) const;
+
+// Matrix P = partial(perr_dot)/partial(u). (just the holonomic constraints)
+Matrix calcPositionConstraintMatrixP(const State&) const; // mp X nu
+Matrix calcPositionConstraintMatrixPt(const State&) const; // nu X mp
+
+// Matrix PNInv = partial(perr)/partial(q) = P*N^-1
+Matrix calcPositionConstraintMatrixPNInv(const State&) const; // mp X nq
+
+/** This operator calculates this constraint's body and mobility forces given
+the complete set of multipliers lambda for this Constraint. We expect that
+lambda has been packed to include multipliers associated with the second
+time derivatives of the position (holonomic) constraints, the first time
+derivatives of the velocity (nonholonomic) constraints, and the
+acceleration-only constraints, in that order.
+
+The state must be realized already to Stage::Velocity. Returned body forces
+correspond only to the <em>constrained bodies</em> and the mobility forces
+correspond only to the <em>constrained mobilities</em>; they must be unpacked
+by the caller into the actual system mobilized bodies and actual system
+mobilities. Note that the body forces are in the ancestor body frame A, not
+necessarily the Ground frame G, and that they are opposite in sign from
+applied forces. If you want to calculate forces you can treat as applied
+forces, negate \a lambda before the call. **/
+void calcConstraintForcesFromMultipliers(const State&,
+ const Vector& lambda, // mp+mv+ma of these
+ Vector_<SpatialVec>& bodyForcesInA, // numConstrainedBodies
+ Vector& mobilityForces) const; // numConstrainedU
+
+ // VELOCITY STAGE //
+/** Get a Vector containing the velocity errors. Many subclasses provide
+their own methods for getting this information in a more specific form. **/
+Vector getVelocityErrorsAsVector(const State&) const; // mp+mv of these
+Vector calcVelocityErrorFromU(const State&, // mp+mv of these
+ const Vector& u) const; // numParticipatingU u's
+
+// Matrix V = partial(verr)/partial(u) for just the non-holonomic
+// constraints.
+Matrix calcVelocityConstraintMatrixV(const State&) const; // mv X nu
+Matrix calcVelocityConstraintMatrixVt(const State&) const; // nu X mv
+
+ // DYNAMICS STAGE //
+// nothing in base class currently
+
+ // ACCELERATION STAGE //
+/** Get a Vector containing the acceleration errors. Many subclasses
+provide their own methods for getting this information in a more
+specific form. **/
+Vector getAccelerationErrorsAsVector(const State&) const; // mp+mv+ma of these
+Vector calcAccelerationErrorFromUDot(const State&, // mp+mv+ma of these
+ const Vector& udot) const; // numParticipatingU udot's
+
+/** Get a Vector containing the Lagrange multipliers. Many subclasses
+provide their own methods for getting this information in a more
+specific form. **/
+Vector getMultipliersAsVector(const State&) const; // mp+mv+ma of these
+
+/** Given a State realized through Acceleration stage, return the forces
+that were applied to the system by this %Constraint, with body forces
+expressed in Ground. Note that the sign convention for constraint forces
+is opposite that of applied forces, because constraints appear on the left
+hand side in Simbody's equations of motion, while applied forces are on
+the right hand side.
+
+These forces are the same as what you would get if you get the multipliers
+from this \a state using getMultipliersAsVector(), call
+calcConstraintForcesFromMultipliers(), and re-express the constrained body
+forces in the Ground frame. However, the ones returned here are already
+calculated so require only copying out of the \a state cache. **/
+void getConstraintForcesAsVectors
+ (const State& state,
+ Vector_<SpatialVec>& bodyForcesInG, // numConstrainedBodies
+ Vector& mobilityForces) const; // numConstrainedU
+
+/** For convenience, returns constrained body forces as the function return.
+ at see getConstraintForcesAsVectors() **/
+Vector_<SpatialVec> getConstrainedBodyForcesAsVector(const State& state) const {
+ Vector_<SpatialVec> bodyForcesInG;
+ Vector mobilityForces;
+ getConstraintForcesAsVectors(state,bodyForcesInG,mobilityForces);
+ return bodyForcesInG;
+}
+/** For convenience, returns constrained mobility forces as the function
+return.
+ at see getConstraintForcesAsVectors() **/
+Vector getConstrainedMobilityForcesAsVector(const State& state) const {
+ Vector_<SpatialVec> bodyForcesInG;
+ Vector mobilityForces;
+ getConstraintForcesAsVectors(state,bodyForcesInG,mobilityForces);
+ return mobilityForces;
+}
+
+/** Calculate the power being applied by this %Constraint to the system.
+The \a state must be realized through Acceleration stage so that the
+applied constraint forces are known. Then power is calculated as the
+dot product of the \e applied body spatial forces and body spatial velocities,
+plus the dot product of the \e applied mobility forces and corresponding
+mobilities (generalized speeds) u. I emphasized \e applied here because the
+sign convention is opposite for constraint forces, so the power calculation
+requires negating the constraint forces.
+
+For any non-working %Constraint, power should always be within machine
+precision of zero. This is a very useful test when debugging new Constraints.
+For working Constraints, you can calculate work done as the time integral of
+the power. Then if you embed the %Constraint in an otherwise conservative
+system, the sum of system potential and kinetic energy, minus the work done
+by this constraint, should be constant to within integration accuracy.
+Power and work here are signed quantities with positive sign meaning that
+the %Constraint is adding energy to the system and negative meaning it is
+removing energy from the system.
+
+Computational cost here is low because the forces and velocities are already
+known. Only the dot product need be computed, at a cost of about
+11 ncb + 2 ncu flops, where ncb is the number of constrained bodies and ncu
+is the number of constrained mobilities for this %Constraint. **/
+Real calcPower(const State& state) const;
+
+// Matrix A = partial(aerr)/partial(udot) for just the acceleration-only
+// constraints.
+Matrix calcAccelerationConstraintMatrixA(const State&) const; // ma X nu
+Matrix calcAccelerationConstraintMatrixAt(const State&) const; // nu X ma
+
+
+// These are the built-in Constraint types. Types on the same line are
+// synonymous.
+class Rod; typedef Rod ConstantDistance;
+class Ball; typedef Ball CoincidentPoints; typedef Ball Spherical;
+class Weld; typedef Weld CoincidentFrames;
+class PointInPlane; // translations perpendicular to plane normal only
+class PointOnLine; // translations along a line only
+class ConstantAngle; // prevent rotation about common normal of two vectors
+class ConstantOrientation; // allows any translation but no rotation
+class NoSlip1D; // same velocity at a point along a direction
+class BallRollingOnPlane; // ball in contact and rolling w/o slip against plane
+class ConstantSpeed; // prescribe generalized speed value
+class ConstantAcceleration; // prescribe generalized acceleration value
+class Custom;
+class CoordinateCoupler;
+class SpeedCoupler;
+class PrescribedMotion;
+
+class RodImpl;
+class BallImpl;
+class WeldImpl;
+class PointInPlaneImpl;
+class PointOnLineImpl;
+class ConstantAngleImpl;
+class ConstantOrientationImpl;
+class NoSlip1DImpl;
+class BallRollingOnPlaneImpl;
+class ConstantSpeedImpl;
+class ConstantAccelerationImpl;
+class CustomImpl;
+class CoordinateCouplerImpl;
+class SpeedCouplerImpl;
+class PrescribedMotionImpl;
+};
+
+ ////////////////////////////////////////
+ // ROD (CONSTANT DISTANCE) CONSTRAINT //
+ ////////////////////////////////////////
+
+/**
+ * This constraint consists of one constraint equation that enforces a constant
+ * distance between a point on one body and a point on another body. This is
+ * like connecting them by a rigid, massless rod with ball joints at either end.
+ * The constraint is enforced by a force acting along the rod with opposite
+ * signs at either end. When positive, this represents tension in the rod
+ * pulling the points together; when negative it represents compression keeping
+ * the points separated.
+ *
+ * @warning
+ * You can't use this to enforce a distance of zero between two points.
+ * That takes three constraints because there is no restriction on the force
+ * direction. For a distance of zero (i.e., you want the points to be
+ * coincident) use a Ball constraint, a.k.a. CoincidentPoints constraint.
+ */
+class SimTK_SIMBODY_EXPORT Constraint::Rod : public Constraint {
+public:
+ // no default constructor
+ Rod(MobilizedBody& body1, MobilizedBody& body2,
+ Real defaultLength=1);
+ Rod(MobilizedBody& body1, const Vec3& defaultPoint1,
+ MobilizedBody& body2, const Vec3& defaultPoint2,
+ Real defaultLength=1);
+
+ /** Default constructor creates an empty handle. **/
+ Rod() {}
+
+ // Defaults for Instance variables.
+ Rod& setDefaultPointOnBody1(const Vec3&);
+ Rod& setDefaultPointOnBody2(const Vec3&);
+ Rod& setDefaultRodLength(Real);
+
+ // Stage::Topology
+ MobilizedBodyIndex getBody1MobilizedBodyIndex() const;
+ MobilizedBodyIndex getBody2MobilizedBodyIndex() const;
+ const Vec3& getDefaultPointOnBody1() const;
+ const Vec3& getDefaultPointOnBody2() const;
+ Real getDefaultRodLength() const;
+
+ // Stage::Instance
+ const Vec3& getPointOnBody1(const State&) const;
+ const Vec3& getPointOnBody2(const State&) const;
+ Real getRodLength (const State&) const;
+
+ // Stage::Position, Velocity, Acceleration
+ Real getPositionError(const State&) const;
+ Real getVelocityError(const State&) const;
+
+ // Stage::Acceleration
+ Real getAccelerationError(const State&) const;
+ Real getMultiplier(const State&) const;
+ Real getRodTension(const State&) const; // negative means compression
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Rod, RodImpl, Constraint);
+ /** @endcond **/
+};
+
+ ///////////////////////////////
+ // POINT IN PLANE CONSTRAINT //
+ ///////////////////////////////
+
+/**
+ * One constraint equation. This constraint enforces that a point fixed to
+ * one body (the "follower body") must travel in a plane fixed on another body
+ * (the "plane body"). The constraint is enforced by an internal (non-working)
+ * scalar force acting at the spatial location of the follower point, directed
+ * along the plane normal, and equal and opposite on the two bodies.
+ *
+ * The assembly condition is the same as the run-time constraint: the point
+ * has to be moved into the plane.
+ */
+class SimTK_SIMBODY_EXPORT Constraint::PointInPlane : public Constraint {
+public:
+ // no default constructor
+ PointInPlane(MobilizedBody& planeBody_B, const UnitVec3& defaultPlaneNormal_B, Real defaultHeight,
+ MobilizedBody& followerBody_F, const Vec3& defaultFollowerPoint_F);
+
+ /** Default constructor creates an empty handle. **/
+ PointInPlane() {}
+
+ // These affect only generated decorative geometry for visualization;
+ // the plane is really infinite in extent with zero depth and the
+ // point is really of zero radius.
+ PointInPlane& setPlaneDisplayHalfWidth(Real);
+ PointInPlane& setPointDisplayRadius(Real);
+ Real getPlaneDisplayHalfWidth() const;
+ Real getPointDisplayRadius() const;
+
+ // Defaults for Instance variables.
+ PointInPlane& setDefaultPlaneNormal(const UnitVec3&);
+ PointInPlane& setDefaultPlaneHeight(Real);
+ PointInPlane& setDefaultFollowerPoint(const Vec3&);
+
+ // Stage::Topology
+ MobilizedBodyIndex getPlaneMobilizedBodyIndex() const;
+ MobilizedBodyIndex getFollowerMobilizedBodyIndex() const;
+
+ const UnitVec3& getDefaultPlaneNormal() const;
+ Real getDefaultPlaneHeight() const;
+ const Vec3& getDefaultFollowerPoint() const;
+
+ // Stage::Instance
+ const UnitVec3& getPlaneNormal(const State&) const;
+ Real getPlaneHeight(const State&) const;
+ const Vec3& getFollowerPoint(const State&) const;
+
+ // Stage::Position, Velocity
+ Real getPositionError(const State&) const;
+ Real getVelocityError(const State&) const;
+
+ // Stage::Acceleration
+ Real getAccelerationError(const State&) const;
+ Real getMultiplier(const State&) const;
+ Real getForceOnFollowerPoint(const State&) const; // in normal direction
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS
+ (PointInPlane, PointInPlaneImpl, Constraint);
+ /** @endcond **/
+};
+
+ //////////////////////////////
+ // POINT ON LINE CONSTRAINT //
+ //////////////////////////////
+
+/**
+ * Two constraint equations. This constraint enforces that a point fixed to
+ * one body (the "follower body") must travel along a line fixed on another body (the
+ * "line body"). The constraint is enforced by an internal (non-working)
+ * scalar force acting at the spatial location of the follower point, directed in the
+ * plane for which the line is a normal, and equal and opposite on the two bodies.
+ *
+ * The assembly condition is the same as the run-time constraint: the point
+ * has to be moved onto the line.
+ */
+class SimTK_SIMBODY_EXPORT Constraint::PointOnLine : public Constraint {
+public:
+ // no default constructor
+ PointOnLine(MobilizedBody& lineBody_B, const UnitVec3& defaultLineDirection_B, const Vec3& defaultPointOnLine_B,
+ MobilizedBody& followerBody_F, const Vec3& defaultFollowerPoint_F);
+
+ /** Default constructor creates an empty handle. **/
+ PointOnLine() {}
+
+ // These affect only generated decorative geometry for visualization;
+ // the line is really infinite in extent and the
+ // point is really of zero radius.
+ PointOnLine& setLineDisplayHalfLength(Real);
+ PointOnLine& setPointDisplayRadius(Real);
+ Real getLineDisplayHalfLength() const;
+ Real getPointDisplayRadius() const;
+
+ // Defaults for Instance variables.
+ PointOnLine& setDefaultLineDirection(const UnitVec3&);
+ PointOnLine& setDefaultPointOnLine(const Vec3&);
+ PointOnLine& setDefaultFollowerPoint(const Vec3&);
+
+ // Stage::Topology
+ MobilizedBodyIndex getLineMobilizedBodyIndex() const;
+ MobilizedBodyIndex getFollowerMobilizedBodyIndex() const;
+
+ const UnitVec3& getDefaultLineDirection() const;
+ const Vec3& getDefaultPointOnLine() const;
+ const Vec3& getDefaultFollowerPoint() const;
+
+ // Stage::Instance
+ const UnitVec3& getLineDirection(const State&) const;
+ const Vec3& getPointOnLine(const State&) const;
+ const Vec3& getFollowerPoint(const State&) const;
+
+ // Stage::Position, Velocity
+ Vec2 getPositionErrors(const State&) const;
+ Vec2 getVelocityErrors(const State&) const;
+
+ // Stage::Acceleration
+ Vec2 getAccelerationErrors(const State&) const;
+ Vec2 getMultipliers(const State&) const;
+ const Vec2& getForceOnFollowerPoint(const State&) const; // in normal direction
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS
+ (PointOnLine, PointOnLineImpl, Constraint);
+ /** @endcond **/
+};
+
+ ///////////////////////////////
+ // CONSTANT ANGLE CONSTRAINT //
+ ///////////////////////////////
+
+/**
+ * This constraint consists of a single constraint equation that enforces that
+ * a unit vector v1 fixed to one body (the "base body") must maintain a fixed
+ * angle theta with respect to a unit vector v2 fixed on the other body (the
+ * "follower body"). This can be done with a single constraint equation as long
+ * as theta is sufficiently far away from 0 and +/-Pi (180 degrees), with the
+ * numerically best performance at theta=Pi/2 (90 degrees).
+ *
+ * @warning
+ * Do not use this constraint to \e align the vectors, that is for angles near
+ * 0 or +/- Pi; performance will noticeably degrade within a few degrees of
+ * these limits and numerical integration will eventually fail at the limits.
+ *
+ * If you want to enforce that two axes are aligned with one another (that
+ * is, the angle between them is 0 or +/-Pi), that takes \e two constraint
+ * equations since the only remaining rotation is about the common axis. (That
+ * is, two rotational degrees of freedom are removed; that can't be done with
+ * one constraint equation -- the situation is analogous to the inability of
+ * a Rod (distance) constraint to keep two points at 0 distance.) Instead,
+ * you can use two ConstantAngle constraints on pairs of vectors perpendicular
+ * to the aligned ones, so that each ConstantAngle is set to the optimal 90 degrees.
+ *
+ * This constraint is enforced by an internal scalar torque applied equal and
+ * opposite on each body, about the mutual perpendicular to the two vectors.
+ *
+ * The assembly condition is the same as the run-time constraint: the
+ * bodies must be rotated until the vectors have the right angle between them.
+ */
+class SimTK_SIMBODY_EXPORT Constraint::ConstantAngle : public Constraint {
+public:
+ // no default constructor
+ ConstantAngle(MobilizedBody& baseBody_B, const UnitVec3& defaultAxis_B,
+ MobilizedBody& followerBody_F, const UnitVec3& defaultAxis_F,
+ Real angle = Pi/2);
+
+ /** Default constructor creates an empty handle. **/
+ ConstantAngle() {}
+
+ // These affect only generated decorative geometry for visualization.
+ ConstantAngle& setAxisDisplayLength(Real);
+ ConstantAngle& setAxisDisplayWidth(Real);
+ Real getAxisDisplayLength() const;
+ Real getAxisDisplayWidth() const;
+
+ // Defaults for Instance variables.
+ ConstantAngle& setDefaultBaseAxis(const UnitVec3&);
+ ConstantAngle& setDefaultFollowerAxis(const UnitVec3&);
+ ConstantAngle& setDefaultAngle(Real);
+
+ // Stage::Topology
+ MobilizedBodyIndex getBaseMobilizedBodyIndex() const;
+ MobilizedBodyIndex getFollowerMobilizedBodyIndex() const;
+
+ const UnitVec3& getDefaultBaseAxis() const;
+ const UnitVec3& getDefaultFollowerAxis() const;
+ Real getDefaultAngle() const;
+
+ // Stage::Instance
+ const UnitVec3& getBaseAxis(const State&) const;
+ const UnitVec3& getFollowerAxis(const State&) const;
+ Real getAngle(const State&) const;
+
+ // Stage::Position, Velocity
+ Real getPositionError(const State&) const;
+ Real getVelocityError(const State&) const;
+
+ // Stage::Acceleration
+ Real getAccelerationError(const State&) const;
+ Real getMultiplier(const State&) const;
+ Real getTorqueOnFollowerBody(const State&) const; // about f X b
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(ConstantAngle, ConstantAngleImpl, Constraint);
+ /** @endcond **/
+};
+
+ /////////////////////////////////////////
+ // BALL (COINCIDENT POINTS) CONSTRAINT //
+ /////////////////////////////////////////
+
+/** Enforce that a fixed station on one body remains coincident with a fixed
+station on a second body, as though there were a ball joint connecting them at
+those points. Uses three position-level (holonomic) constraint equations to
+prevent relative translation in three orthogonal directions.
+
+At construction you specify the two bodies to be connected by the %Constraint,
+and give default values for a station on each body. The State is initialized
+with those default stations, but you can change them later.
+
+The constraint is enforced by an internal (non-working) force applied at the
+spatial location of the point on body 2, on material points of each body that
+are coincident with that spatial location. Note that this is somewhat asymmetric
+when the ball is not properly assembled -- it acts as though the contact occurs
+at the point on body 2, \e not at the point on body 1. That is critical to
+ensure that Newton's 3rd law is satisified -- the action and reaction must
+occur at the same point.
+
+The assembly condition is the same as the runtime constraint -- the two points
+can be brought together by driving the perr to zero. **/
+class SimTK_SIMBODY_EXPORT Constraint::Ball : public Constraint {
+public:
+ /** Connect the origin of \a body1 to the origin of \a body2. That is,
+ the default stations will both be (0,0,0). You can change those later
+ in the State using setPointOnBody1() and setPointOnBody2(). **/
+ Ball(MobilizedBody& body1, MobilizedBody& body2);
+ /** Connect \a body1 and \a body2 at given station points, given in the
+ body frame of the corresponding body. You can change
+ those later in the State using setPointOnBody1() and setPointOnBody2(). **/
+ Ball(MobilizedBody& body1, const Vec3& defaultPoint1,
+ MobilizedBody& body2, const Vec3& defaultPoint2);
+
+ /** Default constructor creates an empty handle. **/
+ Ball() {}
+
+ /** Change the station point on body 1 at which this %Constraint acts.
+ Provide the station location in the body 1 local frame.
+ This overrides the default point that was supplied on construction. This
+ is an Instance-stage change. **/
+ void setPointOnBody1(State& state, const Vec3& point_B1) const;
+ /** Change the station point on body 2 at which this %Constraint acts.
+ Provide the station location in the body 2 local frame.
+ This overrides the default point that was supplied on construction. This
+ is an Instance-stage change. **/
+ void setPointOnBody2(State& state, const Vec3& point_B2) const;
+
+ /** Return from the given \a state the constrained station on body 1, in
+ the body 1 frame. **/
+ const Vec3& getPointOnBody1(const State& state) const;
+ /** Return from the given \a state the constrained station on body 2, in
+ the body 2 frame. **/
+ const Vec3& getPointOnBody2(const State& state) const;
+
+ /** Change the default station location on body 1. This is a topological
+ change meaning you'll have to call realizeTopology() again after changing
+ the default point. If you want to change the constrained station during
+ a simulation, use setPointOnBody1() instead to override it in a State. **/
+ Ball& setDefaultPointOnBody1(const Vec3& defaultPoint_B1);
+ /** Change the default station location on body 2. This is a topological
+ change meaning you'll have to call realizeTopology() again after changing
+ the default point. If you want to change the constrained station during
+ a simulation, use setPointOnBody2() instead to override it in a State. **/
+ Ball& setDefaultPointOnBody2(const Vec3& defaultPoint_B2);
+
+ /** Return the default location for the station point on body 1, as a
+ vector in the body 1 frame. Note that
+ this is not necessarily the station point being used for any given State;
+ use getPointOnBody1() for that. **/
+ const Vec3& getDefaultPointOnBody1() const;
+ /** Return the default location for the station point on body 2, as a
+ vector in the body 2 frame. Note that
+ this is not necessarily the station point being used for any given State;
+ use getPointOnBody2() for that. **/
+ const Vec3& getDefaultPointOnBody2() const;
+
+
+ /** For visualization only, you can override the default radius used by
+ this %Constraint to draw itself. **/
+ Ball& setDefaultRadius(Real r);
+ /** Retreive the radius being used for visualization of
+ this %Constraint. **/
+ Real getDefaultRadius() const;
+
+ /** Return the MobilizedBodyIndex corresponding to body 1. **/
+ MobilizedBodyIndex getBody1MobilizedBodyIndex() const;
+ /** Return the MobilizedBodyIndex corresponding to body 2. **/
+ MobilizedBodyIndex getBody2MobilizedBodyIndex() const;
+
+
+ /** Return the current position-level constraint error for this %Constraint.
+ This is the vector between the constrained stations on body 1 and body 2,
+ which would be zero if this constraint were perfectly satisfied. The
+ returned vector is measured in the Ancestor body frame. The given
+ \a state must be realized through Position stage. **/
+ Vec3 getPositionErrors(const State& state) const;
+
+ /** Return the current velocity-level constraint error for this %Constraint.
+ This is the relative velocity between the material points of body 1 and
+ body 2 that are coincident with the constrained station point on body 2;
+ note that this is subtly different from the time derivative of the
+ position error vector. The returned vector is measured in the Ancestor
+ body frame. The given \a state must be realized through Velocity stage. **/
+ Vec3 getVelocityErrors(const State& state) const;
+
+ /** Return the current acceleration-level constraint error for this
+ %Constraint. This is the relative acceleration between the material points
+ of body 1 and body 2 that are coincident with the constrained station point
+ on body 2; this is precisely the time derivative of the
+ velocity error vector (but not exactly the second time derivative of the
+ position error). The returned vector is measured in the Ancestor
+ body frame. The given \a state must be realized through Acceleration
+ stage. **/
+ Vec3 getAccelerationErrors(const State&) const;
+
+ /** Return the force currently being applied by this %Constraint to the
+ point of body 1 that is coincident in space with the constrained point on
+ body 2. The force vector is expressed in body 1's local frame. **/
+ Vec3 getBallReactionForceOnBody1(const State&) const;
+ /** Return the force currently being applied by this %Constraint to body 2,
+ at its constrained station point. The force vector is expressed in body 2's
+ local frame. **/
+ Vec3 getBallReactionForceOnBody2(const State&) const;
+
+ /** Return the three Lagrange multipliers associated with the three
+ accleration-level constraint equations generated by this %Constraint.
+ Although these are related to reaction forces, if that's what you're
+ interested in you should use getBallReactionForcesOnBody1() or
+ getBallReactionForceOnBody2() instead; the definition of the multipliers
+ is somewhat arbitrary and will not always be easy to interpret as forces.
+ The given \a state must be realized through Acceleration stage. **/
+ Vec3 getMultipliers(const State& state) const;
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Ball, BallImpl, Constraint);
+ /** @endcond **/
+};
+
+ /////////////////////////////////////
+ // CONSTANT ORIENTATION CONSTRAINT //
+ /////////////////////////////////////
+
+/**
+ * Three constraint equations. This constraint enforces that a reference frame
+ * fixed to one body (the "follower body") must have the same orientation as another
+ * reference frame fixed on another body (the "base body"). That is, we have three
+ * constraint equations that collectively prohibit any relative rotation between
+ * the base and follower. The run time equations we use are just three "constant angle"
+ * constraints enforcing perpendicularity between follower's x,y,z axes with the base
+ * y,z,x axes respectively.
+ *
+ * This constraint is enforced by an internal (non-working) torque vector applied equal and
+ * opposite on each body.
+ *
+ * TODO: The assembly condition is not the same as the run-time constraint, because the
+ * perpendicularity conditions can be satisfied with antiparallel axes. For assembly
+ * we must have additional (redundant) constraints requiring parallel axes.
+ */
+class SimTK_SIMBODY_EXPORT Constraint::ConstantOrientation : public Constraint
+{
+public:
+ // no default constructor
+ ConstantOrientation(MobilizedBody& baseBody_B, const Rotation& defaultRB,
+ MobilizedBody& followerBody_F, const Rotation& defaultRF);
+
+ /** Default constructor creates an empty handle. **/
+ ConstantOrientation() {}
+
+ //TODO: default visualization geometry?
+
+ // Defaults for Instance variables.
+ ConstantOrientation& setDefaultBaseRotation(const Rotation&);
+ ConstantOrientation& setDefaultFollowerRotation(const Rotation&);
+
+ // Stage::Topology
+ MobilizedBodyIndex getBaseMobilizedBodyIndex() const;
+ MobilizedBodyIndex getFollowerMobilizedBodyIndex() const;
+
+ const Rotation& getDefaultBaseRotation() const;
+ const Rotation& getDefaultFollowerRotation() const;
+
+ // Stage::Instance
+ const Rotation& getBaseRotation(const State&) const;
+ const Rotation& getFollowerRotation(const State&) const;
+
+ // Stage::Position, Velocity
+ Vec3 getPositionErrors(const State&) const;
+ Vec3 getVelocityErrors(const State&) const;
+
+ // Stage::Acceleration
+ Vec3 getAccelerationErrors(const State&) const;
+ Vec3 getMultipliers(const State&) const;
+ Vec3 getTorqueOnFollowerBody(const State&) const;
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS
+ (ConstantOrientation, ConstantOrientationImpl, Constraint);
+ /** @endcond **/
+};
+
+ /////////////////////////////////////////
+ // WELD (COINCIDENT FRAMES) CONSTRAINT //
+ /////////////////////////////////////////
+
+/**
+ * Six constraint equations. This constraint enforces coincidence between
+ * a frame on one body and a frame on another body. This is a combination
+ * of a ConstantOrientation constraint and a Ball constraint. The first three
+ * equations correspond to the perpendicularity constraints associated with
+ * the orientation constraint, the last three equations are the
+ * coincident point conditions.
+ *
+ * The constraint is enforced by an internal (non-working) force applied at the
+ * spatial location of the frame origin on body 2, on material points of each body that
+ * are coincident with that spatial location. Note that this is somewhat asymmetric
+ * when the Weld is not properly assembled -- it acts as though the contact occurs
+ * at the origin of the frame on body 2, *not* at the origin of the frame on body 1.
+ * The orientation constraints on the other hand are symmetric, they are three
+ * "constant angle" constraints enforcing perpendicularity between body2's
+ * x,y,z axes with body1's y,z,x axes respectively, via an internal (non-working)
+ * torque vector applied equal and opposite on each body.
+ *
+ * TODO: Although the frame origins can be brought together by the Ball constraint, the
+ * perpendicularity conditions can be satisfied with antiparallel axes in addition
+ * to the parallel ones we want. Therefore the assembly conditions must include
+ * additional (redundant) constraints requiring parallel axes.
+ */
+class SimTK_SIMBODY_EXPORT Constraint::Weld : public Constraint {
+public:
+ // no default constructor
+
+ /// Make the body frame of one body coincident with the body frame
+ /// of the other body.
+ Weld(MobilizedBody& body1, MobilizedBody& body2);
+
+ /// Make a particular frame attached to one body coincident with
+ /// a particular frame attached to the other body. The frames are
+ /// specified by giving the transform X_BF which expresses the
+ /// position and orientation of frame F relative to the body frame B.
+ Weld(MobilizedBody& body1, const Transform& frame1,
+ MobilizedBody& body2, const Transform& frame2);
+
+ /** Default constructor creates an empty handle. **/
+ Weld() {}
+
+ // Control over generated decorative geometry.
+
+ /// This is used only for visualization. Set r <= 0 to disable
+ /// default frame drawing. Default axis length is r=1. This is a
+ /// topology-stage variable, not changeable later.
+ Weld& setAxisDisplayLength(Real r);
+
+ /// Report the length being used for display of the frames being
+ /// connected by this Weld. If this returns 0 then no geometry is
+ /// being generated for the frames.
+ Real getAxisDisplayLength() const;
+
+ // Defaults for Instance variables.
+
+ /// Explicitly set the default value for the frame on body1 which
+ /// is to be made coincident with a frame on body2. Note that this is
+ /// topology-stage value so requires non-const access to the Constraint.
+ Weld& setDefaultFrameOnBody1(const Transform&);
+
+ /// Retrieve the default transform for the frame on body 1.
+ const Transform& getDefaultFrameOnBody1() const;
+
+ /// Explicitly set the default value for the frame on body2 which
+ /// is to be made coincident with a frame on body1. Note that this is
+ /// topology-stage value so requires non-const access to the Constraint.
+ Weld& setDefaultFrameOnBody2(const Transform&);
+
+ /// Retrieve the default transform for the frame on body 2.
+ const Transform& getDefaultFrameOnBody2() const;
+
+
+ // Stage::Topology
+
+ /// Report the MobilizedBodyIndex of body 1 for this Weld constraint.
+ MobilizedBodyIndex getBody1MobilizedBodyIndex() const;
+
+ /// Report the MobilizedBodyIndex of body 2 for this Weld constraint.
+ MobilizedBodyIndex getBody2MobilizedBodyIndex() const;
+
+
+ // Stage::Instance
+ const Transform& getFrameOnBody1(const State&) const;
+ const Transform& getFrameOnBody2(const State&) const;
+
+ // Stage::Position, Velocity, Acceleration
+ Vec6 getPositionErrors(const State&) const;
+ Vec6 getVelocityErrors(const State&) const;
+
+ // Stage::Acceleration
+ Vec6 getAccelerationErrors(const State&) const;
+ Vec6 getMultipliers(const State&) const;
+
+ // Forces are reported expressed in the body frame of the indicated body.
+ const SpatialVec& getWeldReactionOnBody1(const State&) const;
+ const SpatialVec& getWeldReactionOnBody2(const State&) const;
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Weld, WeldImpl, Constraint);
+ /** @endcond **/
+};
+
+ ///////////////////////////
+ // NO SLIP 1D CONSTRAINT //
+ ///////////////////////////
+
+/**
+ * One non-holonomic constraint equation. There is a contact point P and a no-slip
+ * direction n fixed in a case body C. There are two moving bodies B0 and B1. The
+ * material point of B0 and the material point of B1 which are each coincident
+ * with the contact point P must have identical velocities in C, along the direction n.
+ * This can be used to implement simple rolling contact between disks, such as occurs
+ * in gear trains.
+ *
+ * The assembly condition is the same as the run-time constraint: the velocities must
+ * be made to match.
+ */
+class SimTK_SIMBODY_EXPORT Constraint::NoSlip1D : public Constraint {
+public:
+ // no default constructor
+
+ /** Define the up to three bodies involved in this constraint: the two
+ "moving" bodies and a Case body, and a default contact point and no-slip
+ direction in the Case body frame C. (If you are modeling gears then the
+ Case is the gearbox.) The case serves to define the
+ contact geometry but no forces are applied to it. It is OK for the Case
+ body to be the same body as one of the moving bodies. **/
+ NoSlip1D(MobilizedBody& caseBodyC, const Vec3& P_C, const UnitVec3& n_C,
+ MobilizedBody& movingBody0, MobilizedBody& movingBody1);
+
+ /** Default constructor creates an empty handle. **/
+ NoSlip1D() {}
+
+ /** Change the contact point at which this %Constraint acts.
+ Provide the station location in the Case body local frame.
+ This overrides the default point that was supplied on construction. This
+ is an Instance-stage change. **/
+ void setContactPoint(State& state, const Vec3& point_C) const;
+ /** Change the no-slip direction along which this %Constraint acts.
+ Provide the direction unit vector in the Case body local frame.
+ This overrides the default direction that was supplied on construction. This
+ is an Instance-stage change. **/
+ void setDirection(State& state, const UnitVec3& direction_C) const;
+
+ /** Return from the given \a state the contact point, in the Case body
+ frame. **/
+ const Vec3& getContactPoint(const State& state) const;
+ /** Return from the given \a state the no-slip direction, in the Case
+ body frame. **/
+ const UnitVec3& getDirection(const State& state) const;
+
+ // These affect only generated decorative geometry for visualization;
+ // the plane is really infinite in extent with zero depth and the
+ // point is really of zero radius.
+
+ /** For visualization only, set the length of the line used to show the
+ no-slip direction. **/
+ NoSlip1D& setDirectionDisplayLength(Real);
+ /** For visualization only, set the radius of the sphere used to show
+ the contact point location. **/
+ NoSlip1D& setPointDisplayRadius(Real);
+ /** Return the current value of the visualization line length for the
+ no-slip direction. **/
+ Real getDirectionDisplayLength() const;
+ /** Return the current value of the radius for visualization of the
+ contact point. **/
+ Real getPointDisplayRadius() const;
+
+ // Defaults for Instance variables.
+
+ /** Change the default contact point; this is the initial value for
+ for the actual contact point and is a topological change. **/
+ NoSlip1D& setDefaultContactPoint(const Vec3&);
+ /** Change the default no-slip direction; this is the initial value for
+ for the actual direction and is a topological change. **/
+ NoSlip1D& setDefaultDirection(const UnitVec3&);
+
+
+ // Stage::Topology
+
+ /** Get the mobilized body index of the Case body that was set during
+ construction. **/
+ MobilizedBodyIndex getCaseMobilizedBodyIndex() const;
+ /** Get the mobilized body index of moving body 0 or moving body 1 that
+ was set during construction. Set \a which to 0 or 1 accordingly. **/
+ MobilizedBodyIndex getMovingBodyMobilizedBodyIndex(int which) const;
+
+ /** Obtain the default value for the no-slip direction, expressed in the
+ Case body frame. **/
+ const UnitVec3& getDefaultDirection() const;
+ /** Obtain the default value for the contact point, in the Case body
+ frame. **/
+ const Vec3& getDefaultContactPoint() const;
+
+
+ // Stage::Position, Velocity
+ // no position error
+
+ /** Get the velocity error for this constraint equation, using configuration
+ and velocity information from the given \a state, which must already have
+ been realized through Velocity stage. **/
+ Real getVelocityError(const State& state) const;
+
+ // Stage::Acceleration
+
+ /** Get the acceleration error for this constraint equation, using
+ configuration, velocity, and acceleration information from the given
+ \a state, which must already have been realized through Acceleration
+ stage. **/
+ Real getAccelerationError(const State&) const;
+
+ /** Get the Lagrange multiplier for this constraint equation, using
+ configuration, velocity, and acceleration information from the given
+ \a state, which must already have been realized through Acceleration
+ stage. While this is linearly related to the constraint force it may have
+ arbitrary sign and scaling; if you want an actual force use
+ getForceAtContactPoint() instead. **/
+ Real getMultiplier(const State&) const;
+
+ /** Determine the constraint force currently being generated by this
+ constraint. The force is as applied to the second moving body, that is,
+ moving body 1, and is applied along the no-slip direction vector. **/
+ Real getForceAtContactPoint(const State&) const;
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(NoSlip1D, NoSlip1DImpl, Constraint);
+ /** @endcond **/
+};
+
+ //////////////////////////////////////
+ // BALL ROLLING ON PLANE CONSTRAINT //
+ //////////////////////////////////////
+
+/** This constraint enforces continuous contact and non-slip rolling between a
+spherical surface fixed on one body and a half space (flat surface) fixed on
+another. This requires one holonomic (position) constraint equation enforcing
+contact, and two nonholonomic (velocity) contraint equations enforcing the
+non-slip condition in the plane. Note that this is a bilateral
+constraint and will push or pull as necessary to keep the sphere in contact
+with the plane, and that rolling is enforced regardless of the amount of
+normal force begin generated. If you want to make this unilateral, you must
+handle switching it on and off separately; when this constraint is enabled it
+always enforces the contact and no-slip conditions.
+
+We define the contact point on the ball to be the unique point CB on the sphere
+surface at which the radius vector is antiparallel to the plane's normal
+vector, that is, the point of the sphere directly below the sphere center if
+the plane's normal is considered the "up" direction. Then the contact point CP
+on the plane is defined to be the point on the plane that is directly below the
+center; that is, the intersection of the antiparallel radius vector and the
+halfspace surface. Note that in general CB != CP; the sphere contact point
+and plane contact point will be separated along the plane normal by a small
+distance, limited to the constraint tolerance after assembly. Now we
+define \e the contact point C=(CB+CP)/2, the point in space that is half way
+between the sphere's contact point and the plane's contact point. Equal and
+opposite forces are applied to the ball body B and the plane body P, at the
+station on each body that is coincident with C.
+
+The holonomic constraint we enforce is that point C should be touching the
+plane. We enforce this with the condition that
+~C_P*n_P = h, that is, given the contact point C measured and
+expressed in the plane body's frame, the height of that point in the direction
+of the plane normal should be the height of the plane.
+
+The assembly condition is the same as the run-time constraint: the point of
+the sphere where the inward normal is the same as the halfspace normal must
+be brought into contact with the halfspace surface. **/
+class SimTK_SIMBODY_EXPORT Constraint::BallRollingOnPlane : public Constraint {
+public:
+ // no default constructor
+ /** Create a BallOnPlane constraint and define the default plane and
+ ball geometry. **/
+ BallRollingOnPlane(MobilizedBody& planeBody_P,
+ const UnitVec3& defaultPlaneNormal_P,
+ Real defaultPlaneHeight,
+ MobilizedBody& ballBody_B,
+ const Vec3& defaultBallCenter_B,
+ Real defaultBallRadius);
+
+ /** Default constructor creates an empty handle. **/
+ BallRollingOnPlane() {}
+
+ // These affect only generated decorative geometry for visualization;
+ // the plane is really infinite in extent with zero depth.
+ BallRollingOnPlane& setPlaneDisplayHalfWidth(Real);
+ Real getPlaneDisplayHalfWidth() const;
+
+ // Defaults for Instance variables.
+ BallRollingOnPlane& setDefaultPlaneNormal(const UnitVec3&);
+ BallRollingOnPlane& setDefaultPlaneHeight(Real);
+ BallRollingOnPlane& setDefaultBallCenter(const Vec3&);
+ BallRollingOnPlane& setDefaultBallRadius(Real);
+
+ // Stage::Topology
+ MobilizedBodyIndex getPlaneMobilizedBodyIndex() const;
+ MobilizedBodyIndex getBallMobilizedBodyIndex() const;
+
+ const UnitVec3& getDefaultPlaneNormal() const;
+ Real getDefaultPlaneHeight() const;
+ const Vec3& getDefaultBallCenter() const;
+ Real getDefaultBallRadius() const;
+
+ // Stage::Instance
+ const UnitVec3& getPlaneNormal(const State&) const;
+ Real getPlaneHeight(const State&) const;
+ const Vec3& getBallCenter(const State&) const;
+ Real getBallRadius(const State&) const;
+
+ // Stage::Position, Velocity
+ Real getPositionError(const State&) const;
+ Vec3 getVelocityError(const State&) const;
+
+ // Stage::Acceleration
+ Vec3 getAccelerationError(const State&) const;
+ Vec3 getMultipliers(const State&) const;
+
+ /** Return the signed magnitude of the normal force applied by the plane
+ to the ball at the contact point, in the direction of the plane normal;
+ negative indicates sticking. **/
+ Real getNormalForce(const State&) const;
+ /** Return the friction force vector being applied by the plane to the
+ ball at the contact point, expressed in the plane frame. **/
+ Vec2 getFrictionForceOnBallInPlaneFrame(const State&) const;
+
+ /** @cond **/ // Don't let doxygen see this
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(BallRollingOnPlane, BallRollingOnPlaneImpl, Constraint);
+ /** @endcond **/
+};
+
+
+ ////////////////////
+ // CONSTANT SPEED //
+ ////////////////////
+
+/** Constrain a single mobility to have a particular speed.
+
+One non-holonomic constraint equation. Some mobility u is required to be at a
+particular value s.
+
+The assembly condition is the same as the run-time constraint: u must be set
+to s. **/
+class SimTK_SIMBODY_EXPORT Constraint::ConstantSpeed : public Constraint {
+public:
+ // no default constructor
+ /** Construct a constant speed constraint on a particular mobility
+ of the given mobilizer. **/
+ ConstantSpeed(MobilizedBody& mobilizer, MobilizerUIndex whichU,
+ Real defaultSpeed);
+ /** Construct a constant speed constraint on the mobility
+ of the given mobilizer, assuming there is only one mobility. **/
+ ConstantSpeed(MobilizedBody& mobilizer, Real defaultSpeed);
+
+ /** Default constructor creates an empty handle. **/
+ ConstantSpeed() {}
+
+ /** Return the index of the mobilized body to which this constant speed
+ constraint is being applied (to \e one of its mobilities). This is set on
+ construction of the %ConstantSpeed constraint. **/
+ MobilizedBodyIndex getMobilizedBodyIndex() const;
+ /** Return the particular mobility whose generalized speed is controlled by
+ this %ConstantSpeed constraint. This is set on construction. **/
+ MobilizerUIndex getWhichU() const;
+ /** Return the default value for the speed to be enforced. This is set on
+ construction or via setDefaultSpeed(). This is used to initialize the speed
+ when a default State is created, but it can be overriden by changing the
+ value in the State using setSpeed(). **/
+ Real getDefaultSpeed() const;
+ /** Change the default value for the speed to be enforced by this
+ constraint. This is a topological change, meaning you'll have to call
+ realizeTopology() on the containing System and obtain a new State before
+ you can use it. If you just want to make a runtime change in the State,
+ see setSpeed(). **/
+ ConstantSpeed& setDefaultSpeed(Real speed);
+
+ /** Override the default speed with this one whose value is stored in the
+ given State. This invalidates the Velocity stage in the state. Don't
+ confuse this with setDefaultSpeed() -- the value set here overrides that
+ one. **/
+ void setSpeed(State& state, Real speed) const;
+ /** Get the current value of the speed set point from the indicated State.
+ This is the value currently in effect, either from the default or from a
+ previous call to setSpeed(). **/
+ Real getSpeed(const State& state) const;
+
+ // no position error
+
+ /** Return the amount by which the given State fails to satisfy this
+ %ConstantSpeed constraint. The \a state must already be realized through
+ Stage::Velocity. **/
+ Real getVelocityError(const State& state) const;
+
+ // Stage::Acceleration
+ /** Return the amount by which the accelerations in the given State fail
+ to satify the time derivative of this constraint (which must be zero).
+ The \a state must already be realized through Stage::Acceleration. **/
+ Real getAccelerationError(const State& state) const;
+ /** Get the value of the Lagrange multipler generated to satisfy this
+ constraint. For a %ConstantSpeed constraint, that is the same as the
+ generalized force although by convention constraint multipliers have the
+ opposite sign from applied forces. The \a state must already be realized
+ through Stage::Acceleration.**/
+ Real getMultiplier(const State&) const;
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS
+ (ConstantSpeed, ConstantSpeedImpl, Constraint);
+ /** @endcond **/
+};
+
+ ///////////////////////////
+ // CONSTANT ACCELERATION //
+ ///////////////////////////
+
+/** Constrain a single mobility to have a particular acceleration.
+
+One acceleration-only constraint equation. Some generalized acceleration
+udot is required to be at a particular value a.
+
+There is no assembly condition because this does not involve state
+variables q or u, just u's time derivative udot. **/
+class SimTK_SIMBODY_EXPORT Constraint::ConstantAcceleration : public Constraint
+{
+public:
+ // no default constructor
+ /** Construct a constant acceleration constraint on a particular mobility
+ of the given mobilizer. **/
+ ConstantAcceleration(MobilizedBody& mobilizer, MobilizerUIndex whichU,
+ Real defaultAcceleration);
+ /** Construct a constant acceleration constraint on the mobility
+ of the given mobilizer, assuming there is only one mobility. **/
+ ConstantAcceleration(MobilizedBody& mobilizer,
+ Real defaultAcceleration);
+
+ /** Default constructor creates an empty handle. **/
+ ConstantAcceleration() {}
+
+ /** Return the index of the mobilized body to which this constant
+ acceleration constraint is being applied (to \e one of its mobilities).
+ This is set on construction of the %ConstantAcceleration constraint. **/
+ MobilizedBodyIndex getMobilizedBodyIndex() const;
+ /** Return the particular mobility whose generalized acceleration is
+ controlled by this %ConstantAcceleration constraint. This is set on
+ construction. **/
+ MobilizerUIndex getWhichU() const;
+ /** Return the default value for the acceleration to be enforced. This is
+ set on construction or via setDefaultAcceleration(). This is used to
+ initialize the acceleration when a default State is created, but it can be
+ overriden by changing the value in the State using setAcceleration(). **/
+ Real getDefaultAcceleration() const;
+ /** Change the default value for the acceleration to be enforced by this
+ constraint. This is a topological change, meaning you'll have to call
+ realizeTopology() on the containing System and obtain a new State before
+ you can use it. If you just want to make a runtime change in the State,
+ see setAcceleration(). **/
+ ConstantAcceleration& setDefaultAcceleration(Real accel);
+
+ /** Override the default acceleration with this one whose value is stored
+ in the given State. This invalidates the Acceleration stage in the state.
+ Don't confuse this with setDefaultAcceleration() -- the value set here
+ overrides that one. **/
+ void setAcceleration(State& state, Real accel) const;
+ /** Get the current value of the acceleration set point from the indicated
+ State. This is the value currently in effect, either from the default or
+ from a previous call to setAcceleration(). **/
+ Real getAcceleration(const State& state) const;
+
+ // no position or velocity error
+
+ // Stage::Acceleration
+ /** Return the amount by which the accelerations in the given State fail
+ to satify this constraint. The \a state must already be realized through
+ Stage::Acceleration. **/
+ Real getAccelerationError(const State&) const;
+ /** Get the value of the Lagrange multipler generated to satisfy this
+ constraint. For a %ConstantAcceleration constraint, that is the same as the
+ generalized force although by convention constraint multipliers have the
+ opposite sign from applied forces. The \a state must already be realized
+ through Stage::Acceleration.**/
+ Real getMultiplier(const State&) const;
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS
+ (ConstantAcceleration, ConstantAccelerationImpl, Constraint);
+ /** @endcond **/
+};
+
+//==============================================================================
+// CUSTOM
+//==============================================================================
+/** The handle class Constraint::Custom (dataless) and its companion class
+Constraint::Custom::Implementation can be used together to define new
+Constraint types with arbitrary properties. To use it, create a class that
+extends Constraint::Custom::Implementation. You can then create an instance
+of it and pass it to the Constraint::Custom constructor:
+
+<pre>
+Constraint::Custom myConstraint(new MyConstraintImplementation( args ));
+</pre>
+
+Alternatively, you can also create a new Handle class which is a subclass of
+Constraint::Custom and which creates the Implementation itself in its
+constructors.
+
+<pre>
+class MyConstraint : public Constraint::Custom {
+public:
+ MyConstraint( args ) : Constraint::Custom(new MyForceImplementation( args )) {
+ }
+}
+</pre>
+
+This allows an end user to simply write
+
+<pre>
+MyConstraint( args );
+</pre>
+
+and not worry about implementation classes or creating objects on the heap.
+If you do this, your Constraint::Custom subclass must not have any data
+members or virtual methods. If it does, it will not work correctly. Instead,
+store all data in the Implementation subclass. **/
+class SimTK_SIMBODY_EXPORT Constraint::Custom : public Constraint {
+public:
+ class Implementation;
+ class ImplementationImpl;
+
+ /** Create a Custom Constraint.
+ *
+ * @param implementation
+ * The object which implements the custom constraint. The
+ * Constraint::Custom takes over ownership of the implementation
+ * object, and deletes it when the Constraint itself is deleted.
+ */
+ explicit Custom(Implementation* implementation);
+
+
+ /** Default constructor creates an empty handle. **/
+ Custom() {}
+
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Custom, CustomImpl, Constraint);
+protected:
+ const Implementation& getImplementation() const;
+ Implementation& updImplementation();
+};
+
+//==============================================================================
+// CUSTOM::IMPLEMENTATION
+//==============================================================================
+
+// We only want the template instantiation to occur once. This symbol is
+// defined in the SimTK core compilation unit that defines the Constraint
+// class but should not be defined any other time.
+#ifndef SimTK_SIMBODY_DEFINING_CONSTRAINT
+ extern template class PIMPLHandle<Constraint::Custom::Implementation,
+ Constraint::Custom::ImplementationImpl>;
+#endif
+
+/** This is the abstract base class for the implementation of custom
+constraints.\ See Constraint::Custom for more information. **/
+class SimTK_SIMBODY_EXPORT Constraint::Custom::Implementation
+: public PIMPLHandle<Implementation,ImplementationImpl> {
+public:
+// No default constructor because you have to supply at least the
+// SimbodyMatterSubsystem to which this Constraint belongs.
+
+/** Destructor is virtual so derived classes get a chance to clean up if
+necessary. **/
+virtual ~Implementation() { }
+
+/** This method should produce a deep copy identical to the concrete derived
+Implementation object underlying this Implementation base class object. Note
+that the result is new heap space; the caller must be sure to take ownership of
+the returned pointer and call delete on it when done. **/
+virtual Implementation* clone() const = 0;
+
+/** This Implementation base class constructor sets the topological defaults
+for the number of position level (holonomic), velocity level (nonholonomic),
+and acceleration-only constraint equations to be generated. **/
+Implementation(SimbodyMatterSubsystem&, int mp, int mv, int ma);
+
+/** The default constructor for the Implementation base class sets the number
+of generated equations to zero for this constraint, meaning the Constraint
+won't do anything by default. The actual number can be changed using
+setNumConstraintEquationsInUse() prior to realizeModel(). **/
+explicit Implementation(SimbodyMatterSubsystem&);
+
+/** Return a reference to the matter subsystem containing this constraint. **/
+const SimbodyMatterSubsystem& getMatterSubsystem() const;
+
+ // Topological information//
+
+/** Call this if you want to make sure that the next realizeTopology() call
+does something. This is done automatically when you modify the constraint in
+ways understood by Simbody, such as adding a ConstrainedBody. But if you are
+just changing some of your own topology and want to make sure you get a chance
+to recompute something in realizeTopology(), make this call at the time of
+modification. **/
+void invalidateTopologyCache() const;
+
+/** This is an alternate way to set the default number of equations to be
+generated if you didn't specify them in the base class constructor. A reference
+to this Implementation is returned so that this can be used in a sequence like
+an assignment operator. **/
+Implementation& setDefaultNumConstraintEquations(int mp, int mv, int ma);
+
+/** Normally Constraints are enabled when defined and can be disabled later. If
+you want to define this constraint but have it be off by default, use this
+method. A reference to this Implementation is returned so that this can be
+used in a sequence like an assignment operator. **/
+Implementation& setDisabledByDefault(bool shouldBeDisabled);
+
+/** Call this during construction phase to add a body to the topological
+structure of this Constraint. This body's mobilizer's mobilities are \e not
+part of the constraint; mobilizers must be added separately. Numbering starts
+from 0 for each Constraint. The supplied MobilizedBody must be in the Matter
+Subsystem of which this Constraint is a part. **/
+ConstrainedBodyIndex addConstrainedBody(const MobilizedBody&);
+
+/** Call this during construction phase to add a mobilizer to the topological
+structure of this Constraint. All the coordinates q and mobilities u for this
+mobilizer are added also, but we don't know how many of those there will be
+until Stage::Model. Numbering starts from 0 for each Constraint. The supplied
+MobilizedBody must be in the Matter Subsystem of which this Constraint is
+a part. **/
+ConstrainedMobilizerIndex addConstrainedMobilizer(const MobilizedBody&);
+
+/** Map a constrained body for this constraint to the mobilized body to which
+it corresponds in the matter subsystem. You should not use this to extract
+any information in the constraint error or forces methods; always work with
+the constrained bodies and constrained mobilities instead. **/
+MobilizedBodyIndex
+getMobilizedBodyIndexOfConstrainedBody(ConstrainedBodyIndex) const;
+
+/** Map a constrained mobilizer for this constraint to the mobilized body to
+which it corresponds in the matter subsystem. You should not use this to
+extract any information in the constraint error or forces methods; always work
+with the constrained bodies and constrained mobilities instead. **/
+MobilizedBodyIndex
+getMobilizedBodyIndexOfConstrainedMobilizer(ConstrainedMobilizerIndex) const;
+
+
+/** @name Methods for use with ConstrainedMobilizers
+When a constraint acts directly on generalized coordinates q or generalized
+speeds u (or their time derivatives), use methods in this section to access
+those values in your constraint error and force methods. The "from state"
+methods should only be used to pull information from the state that is at a
+higher level than the method being written. For example, if you are calculating
+velocity errors you can get positions from the state, but not velocities.
+Instead, the velocities will be passed as an argument. **/
+/**@{**/
+
+
+/** Use this method in your calcPositionErrors() implementation to extract the
+value of a particular generalized coordinate q selected by (mobilizer,whichQ),
+from the "constrained q" argument that is passed to the method from Simbody.
+ at param[in] state
+ Supplied state which is used only for modeling information; generalized
+ coordinates q within \a state are ignored.
+ at param[in] constrainedQ
+ This is the argument that is supplied to calcPositionErrors() from which
+ we will extract the particular q value selected by the next two arguments.
+ at param[in] mobilizer
+ The constrained mobilizer one of whose generalized coordinates is of
+ interest.
+ at param[in] whichQ
+ The particular generalized coordinate of \a mobilizer whose value we
+ want. The actual value will be selected from \a constrainedQ.
+ at return
+ The value of the generalized coordinate q of interest, extracted from
+ the \a constrainedQ argument. **/
+Real getOneQ(const State& state,
+ const Array_<Real,ConstrainedQIndex>& constrainedQ,
+ ConstrainedMobilizerIndex mobilizer,
+ MobilizerQIndex whichQ) const;
+
+/** Same as the getOneQ() method but for use in methods to which no explicit
+"constrained q" argument is supplied. The desired q value is obtained
+from \a state. You can call this from any constraint implementation method
+\e except calcPositionError(). **/
+Real getOneQFromState(const State& state,
+ ConstrainedMobilizerIndex mobilizer,
+ MobilizerQIndex whichQ) const;
+
+/** Use this method in your calcPositionDotErrors() implementation to extract
+the value of a particular generalized coordinate derivative qdot selected by
+(mobilizer,whichQ), from the "constrained qdot" argument that is passed to the
+method from Simbody.
+ at param[in] state
+ Supplied state which is used only for modeling information; qdots within
+ \a state are ignored.
+ at param[in] constrainedQDot
+ This is the argument that is supplied to calcPositionDotErrors() from which
+ we will extract the particular qdot value selected by the next two
+ arguments.
+ at param[in] mobilizer
+ The constrained mobilizer one of whose generalized coordinates is of
+ interest.
+ at param[in] whichQ
+ The particular generalized coordinate of \a mobilizer whose qdot value we
+ want. The actual value will be selected from \a constrainedQDot.
+ at return
+ The value of the generalized coordinate derivative qdot of interest,
+ extracted from the \a constrainedQDot argument. **/
+Real getOneQDot(const State& state,
+ const Array_<Real,ConstrainedQIndex>& constrainedQDot,
+ ConstrainedMobilizerIndex mobilizer,
+ MobilizerQIndex whichQ) const;
+
+/** Same as the getOneQDot() method above but for use in velocity- or
+acceleration-level methods to which no explicit "constrained qdot" argument
+is supplied. The desired qdot value is obtained from \a state. You can call
+this from calcPositionDotDotError(). State must already be realized to the
+Velocity stage. **/
+Real getOneQDotFromState(const State& state,
+ ConstrainedMobilizerIndex mobilizer,
+ MobilizerQIndex whichQ) const;
+
+
+/** Use this method in your calcPositionDotDotErrors() implementation to extract
+the value of a particular generalized coordinate second derivative qdotdot
+selected by (mobilizer,whichQ), from the "constrained qdotdot" argument that
+is passed to the method from Simbody.
+ at param[in] state
+ Supplied state which is used only for modeling information; qdotdots within
+ \a state are ignored.
+ at param[in] constrainedQDotDot
+ This is the argument that is supplied to calcPositionDotDotErrors() from
+ which we will extract the particular qdotdot value selected by the next two
+ arguments.
+ at param[in] mobilizer
+ The constrained mobilizer one of whose generalized coordinates is of
+ interest.
+ at param[in] whichQ
+ The particular generalized coordinate of \a mobilizer whose qdotdot value
+ we want. The actual value will be selected from \a constrainedQDotDot.
+ at return
+ The value of the generalized coordinate second derivative qdotdot of
+ interest, extracted from the \a constrainedQDotDot argument.
+
+There is no getOneQDotDotFromState() method because all the acceleration-
+level methods are passed qdotdot or udot as an explicit argument. **/
+Real getOneQDotDot(const State& state,
+ const Array_<Real,ConstrainedQIndex>& constrainedQDotDot,
+ ConstrainedMobilizerIndex mobilizer,
+ MobilizerQIndex whichQ) const;
+
+/** Use this method in your calcVelocityErrors() implementation to extract the
+value of a particular generalized speed u selected by (mobilizer,whichU),
+from the "constrained u" argument that is passed to the method from Simbody.
+ at param[in] state
+ Supplied state which is used only for modeling information; generalized
+ speeds u within \a state are ignored.
+ at param[in] constrainedU
+ This is the argument that is supplied to calcVelocityErrors() from which
+ we will extract the particular u value selected by the next two arguments.
+ at param[in] mobilizer
+ The constrained mobilizer one of whose generalized speeds is of interest.
+ at param[in] whichU
+ The particular generalized speed of \a mobilizer whose value we
+ want. The actual value will be selected from \a constrainedU.
+ at return
+ The value of the generalized speed u of interest, extracted from
+ the \a constrainedU argument. **/
+Real getOneU(const State& state,
+ const Array_<Real,ConstrainedUIndex>& constrainedU,
+ ConstrainedMobilizerIndex mobilizer,
+ MobilizerUIndex whichU) const;
+
+/** Same as the getOneU() method but for use in velocity- or acceleration-
+level methods to which no explicit "constrained u" argument is supplied. The
+desired u value is obtained from \a state. You can call this only from
+calcVelocityDotError(), calcAccelerationError(), and any constraint force
+method. The State needs to be realized only as high as Model stage, but don't
+use this value in calcPositionError() or addInPositionConstraintForces().
+Those must be limited to dependencies on time and configuration only. **/
+Real getOneUFromState(const State& state,
+ ConstrainedMobilizerIndex mobilizer,
+ MobilizerUIndex whichU) const;
+
+/** Use this method in your calcVelocityDotErrors() and calcAccelerationErrors()
+implementations to extract the value of a particular generalized speed
+derivative udot selected by (mobilizer,whichU), from the "constrained udot"
+argument that is passed to these two methods from Simbody.
+ at param[in] state
+ Supplied state which is used only for modeling information; udots within
+ \a state are ignored.
+ at param[in] constrainedUDot
+ This is the argument that is supplied to calcVelocityDotErrors() and
+ calcAccelerationErrros() from which we will extract the particular udot
+ value selected by the next two arguments.
+ at param[in] mobilizer
+ The constrained mobilizer one of whose generalized speeds is of interest.
+ at param[in] whichU
+ The particular generalized speed of \a mobilizer whose udot value we
+ want. The actual value will be selected from \a constrainedUDot.
+ at return
+ The value of the generalized speed derivative udot of interest,
+ extracted from the \a constrainedUDot argument.
+
+There is no getOneUDotFromState() method because all the acceleration-
+level methods are passed qdotdot or udot as an explicit argument. **/
+Real getOneUDot(const State& state,
+ const Array_<Real,ConstrainedUIndex>& constrainedUDot,
+ ConstrainedMobilizerIndex mobilizer,
+ MobilizerUIndex whichU) const;
+
+/** Apply a scalar generalized (mobility-space) force \a fu to a particular
+mobility of one of this %Constraint's Constrained Mobilizers, \e adding it in to
+the appropriate slot of the mobilityForces vector, which is of length
+getNumConstrainedU() for this %Constraint. State need only have been realized
+to Model stage, but this is intended for use in Velocity-stage calls to
+addInXXXConstraintForce() methods for nonholonomic (velocity) or acceleration-only
+constraint equations.
+ at see addInOneQForce() for use in position (holonomic) constraints **/
+void addInOneMobilityForce
+ (const State& state,
+ ConstrainedMobilizerIndex mobilizer,
+ MobilizerUIndex whichU,
+ Real fu,
+ Array_<Real,ConstrainedUIndex>& mobilityForces) const;
+
+/** For use with holonomic (position) constraints, this method allows
+generalized forces to be applied in "q-space" rather than "u-space".
+A scalar q-space generalized force \a fq is applied to a particular
+generalized coordinate (q) of one of this position (holonomic) %Constraint's
+Constrained Mobilizers, \e adding it in to the appropriate slot of the qForces
+vector, which must be of length getNumConstrainedQ() for this %Constraint.
+State need only have been realized to Model stage, but this is intended for
+Position-stage use in the addInPositionConstraintForce() method for position
+constraint equations.
+
+Simbody will convert these automatically to mobility (u) space as needed
+via fu = ~N * fq, where N is block-diagonal kinematic coupling matrix that
+appears in the equation qdot = N*u.
+ at see addInOneMobilityForces() for velocity and acceleration-only constraint
+equations **/
+void addInOneQForce
+ (const State& state,
+ ConstrainedMobilizerIndex mobilizer,
+ MobilizerQIndex whichQ,
+ Real fq,
+ Array_<Real,ConstrainedQIndex>& qForces) const;
+/**@}**/
+
+
+/** @name Methods for use with Constrained Bodies
+When a constraint is enforced (at least in part) by applying forces to bodies,
+use the methods in this section to access position, velocity, and acceleration
+information about those constrained bodies. Note that you can pull higher-level
+information from the state, but information at the current level for a method
+must be taken from the supplied arguments instead. For example, if you are
+writing an acceleration error routine, you can get time, position, and velocity
+information from the state but must get acceleration information from the
+body accelerations that are supplied by Simbody as arguments. **/
+/**@{**/
+
+
+/** Extract from the \a allX_AB argument the spatial transform X_AB giving the
+pose (orientation and location) of a Constrained Body B's body frame B in this
+constraint's Ancestor frame A. **/
+const Transform& getBodyTransform
+ (const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
+ ConstrainedBodyIndex bodyB) const;
+/** Convenient inline interface to getBodyTransform() that returns just the
+orientation as the Rotation matrix R_AB. **/
+const Rotation& getBodyRotation
+ (const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
+ ConstrainedBodyIndex bodyB) const
+{ return getBodyTransform(allX_AB,bodyB).R(); }
+/** Convenient inline interface to getBodyTransform() that returns just the
+location part of B's pose in A, that is the vector p_AB from A's origin Ao
+to B's origin Bo, expressed in A. **/
+const Vec3& getBodyOriginLocation
+ (const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
+ ConstrainedBodyIndex bodyB) const
+{ return getBodyTransform(allX_AB,bodyB).p(); }
+
+/** Extract from the State cache the spatial transform X_AB giving the
+pose (orientation and location) of a Constrained Body B's body frame B in this
+constraint's Ancestor frame A. Do not use this method in a routine that has
+an explicit argument providing the transforms X_AB; use the above
+getBodyTransform() method instead. **/
+const Transform& getBodyTransformFromState
+ (const State& state, ConstrainedBodyIndex B) const; // X_AB
+/** Convenient inline interface to getBodyTransformFromState() that returns
+just the orientation as the Rotation matrix R_AB. **/
+const Rotation& getBodyRotationFromState
+ (const State& state, ConstrainedBodyIndex bodyB) const
+{ return getBodyTransformFromState(state,bodyB).R(); }
+/** Convenient inline interface to getBodyTransformFromState() that returns
+just the location part of B's pose in A, that is the vector p_AB from A's
+origin Ao to B's origin Bo, expressed in A. **/
+const Vec3& getBodyOriginLocationFromState
+ (const State& state, ConstrainedBodyIndex bodyB) const
+{ return getBodyTransformFromState(state,bodyB).p(); }
+
+/** Extract from the \a allV_AB argument the spatial velocity V_AB giving the
+angular and linear velocity of a Constrained Body B's body frame B measured and
+expressed in this constraint's Ancestor frame A. **/
+const SpatialVec& getBodyVelocity
+ (const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
+ ConstrainedBodyIndex bodyB) const;
+/** Convenient inline interface to getBodyVelocity() that returns just the
+angular velocity vector w_AB. **/
+const Vec3& getBodyAngularVelocity
+ (const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
+ ConstrainedBodyIndex bodyB) const
+{ return getBodyVelocity(allV_AB,bodyB)[0]; }
+/** Convenient inline interface to getBodyVelocity() that returns just the
+linear velocity vector v_AB. **/
+const Vec3& getBodyOriginVelocity
+ (const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
+ ConstrainedBodyIndex bodyB) const
+{ return getBodyVelocity(allV_AB,bodyB)[1]; }
+
+/** Extract from the State cache the spatial velocity V_AB giving the angular
+and linear velocity of a Constrained Body B's body frame B measured and
+expressed in this Constraint's Ancestor frame A. Do not use this method in a
+routine that has an explicit argument providing the spatial velocities V_AB;
+use the above getBodyVelocity() method instead. **/
+const SpatialVec& getBodyVelocityFromState
+ (const State& state, ConstrainedBodyIndex bodyB) const; // V_AB
+/** Convenient inline interface to getBodyVelocityFromState() that returns just
+the angular velocity vector w_AB. **/
+const Vec3& getBodyAngularVelocityFromState
+ (const State& state, ConstrainedBodyIndex bodyB) const
+{ return getBodyVelocityFromState(state,bodyB)[0]; }
+/** Convenient inline interface to getBodyVelocityFromState() that returns just
+the linear velocity vector v_AB. **/
+const Vec3& getBodyOriginVelocityFromState
+ (const State& state, ConstrainedBodyIndex bodyB) const
+{ return getBodyVelocityFromState(state,bodyB)[1]; }
+
+/** Extract from the \a allA_AB argument the spatial acceleration A_AB giving
+the angular and linear acceleration of a Constrained Body B's body frame B
+measured and expressed in this constraint's Ancestor frame A. Note that there
+is no getBodyAccelerationFromState() method because all acceleration-level
+methods will be passed body accelerations explicitly. **/
+const SpatialVec& getBodyAcceleration
+ (const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
+ ConstrainedBodyIndex bodyB) const;
+/** Convenient inline interface to getBodyAcceleration() that returns just the
+angular acceleration vector b_AB. **/
+const Vec3& getBodyAngularAcceleration
+ (const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
+ ConstrainedBodyIndex bodyB) const
+{ return getBodyAcceleration(allA_AB,bodyB)[0]; }
+/** Convenient inline interface to getBodyAcceleration() that returns just the
+linear acceleration vector a_AB. **/
+const Vec3& getBodyOriginAcceleration
+ (const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
+ ConstrainedBodyIndex bodyB) const
+{ return getBodyAcceleration(allA_AB,bodyB)[1]; }
+
+ // Calculate location, velocity, and acceleration for a given station.
+
+/** Calculate the position p_AS in the Ancestor frame of a station S of a
+Constrained Body B, specified with the position vector p_BS (or more
+explicitly, p_BoS) from the B frame origin Bo to the point S, expressed
+in the B frame. The return value is a position vector from the Ancestor
+frame's origin Ao to the location of the point S, expressed in the
+Ancestor frame. Cost is 18 flops. **/
+Vec3 findStationLocation
+ (const Array_<Transform, ConstrainedBodyIndex>& allX_AB,
+ ConstrainedBodyIndex bodyB,
+ const Vec3& p_BS) const
+{
+ const Transform& X_AB = allX_AB[bodyB];
+ return X_AB * p_BS; // re-measure and re-express
+}
+/** Same as findStationLocation() but for when you have to get the position
+information from the \a state rather than from an explicit argument.
+Cost is 18 flops. **/
+Vec3 findStationLocationFromState
+ (const State& state,
+ ConstrainedBodyIndex bodyB,
+ const Vec3& p_BS) const
+{
+ const Transform& X_AB = getBodyTransformFromState(state,bodyB);
+ return X_AB * p_BS; // re-measure and re-express
+}
+
+/** Calculate the velocity v_AS in the Ancestor frame of a station S of a
+Constrained Body B, specified with the position vector p_BS (or more
+explicitly, p_BoS) from the B frame origin Bo to the point S, expressed
+in the B frame. The return value v_AS is a vector expressed in the Ancestor
+frame, and is the time derivative taken in A of the position vector p_AS.
+Cost is 27 flops. **/
+Vec3 findStationVelocity
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
+ ConstrainedBodyIndex bodyB,
+ const Vec3& p_BS) const
+{ // p_BS_A is p_BS rexpressed in A but not shifted to Ao
+ const Rotation& R_AB = getBodyRotationFromState(state,bodyB);
+ const Vec3 p_BS_A = R_AB * p_BS;
+ const SpatialVec& V_AB = allV_AB[bodyB];
+ return V_AB[1] + (V_AB[0] % p_BS_A); // v + w X r
+}
+/** Same as findStationVelocity() but for when you have to get the velocity
+information from the \a state rather than from an explicit argument.
+Cost is 27 flops. **/
+Vec3 findStationVelocityFromState
+ (const State& state,
+ ConstrainedBodyIndex bodyB,
+ const Vec3& p_BS) const
+{ // p_BS_A is p_BS rexpressed in A but not shifted to Ao
+ const Rotation& R_AB = getBodyRotationFromState(state,bodyB);
+ const Vec3 p_BS_A = R_AB * p_BS;
+ const SpatialVec& V_AB = getBodyVelocityFromState(state,bodyB);
+ return V_AB[1] + (V_AB[0] % p_BS_A); // v + w X r
+}
+
+/** Calculate the acceleration a_AS in the Ancestor frame of a station S of
+a Constrained Body B, specified with the position vector p_BS (or more
+explicitly, p_BoS) from the B frame origin Bo to the point S, expressed in the
+B frame. The return value a_AS is a vector expressed in the Ancestor frame,
+and is the time derivative taken in A of the velocity vector v_AS and hence the
+second derivative taken in A of the position vectory p_AS. Note that there
+is no findStationAccelerationFromState() method because all acceleration-level
+routines here are provided acceleration information in explicit arguments.
+Cost is 48 flops. **/
+Vec3 findStationAcceleration
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
+ ConstrainedBodyIndex bodyB,
+ const Vec3& p_BS) const
+{ // p_BS_A is p_BS rexpressed in A but not shifted to Ao
+ const Rotation& R_AB = getBodyRotationFromState(state,bodyB);
+ const Vec3 p_BS_A = R_AB * p_BS;
+ const Vec3& w_AB = getBodyAngularVelocityFromState(state,bodyB);
+ const SpatialVec& A_AB = allA_AB[bodyB];
+
+ // Result is a + b X r + w X (w X r).
+ // ("b" is angular acceleration; w is angular velocity).
+ const Vec3 a_AS = A_AB[1] + (A_AB[0] % p_BS_A)
+ + w_AB % (w_AB % p_BS_A); // % is not associative
+ return a_AS;
+}
+
+ // Utilities for applying constraint forces to ConstrainedBodies.
+
+/** Apply an Ancestor-frame force to a B-frame station S given by the position
+vector p_BS (or more explicitly, p_BoS) from the B frame origin Bo to the point
+S, expressed in the B frame, <em>adding to</em> the appropriate
+\p bodyForcesInA entry for this ConstrainedBody B. **/
+void addInStationForce
+ (const State& state,
+ ConstrainedBodyIndex bodyB,
+ const Vec3& p_BS,
+ const Vec3& forceInA,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA) const;
+
+/** Apply an Ancestor-frame torque to body B, <em>adding to</em> the
+appropriate \p bodyForcesInA entry for this ConstrainedBody B. **/
+void addInBodyTorque
+ (const State& state,
+ ConstrainedBodyIndex bodyB,
+ const Vec3& torqueInA,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA) const;
+/**@}**/
+
+/** @name Utility methods
+These provide access to quantities associated with this constraint,
+suitable for use in the optional realize() virtual methods. **/
+/**@{**/
+/** Given a \a state as passed to your realizeAcceleration() implementation,
+obtain the multipliers that Simbody just calculated for this %Constraint. **/
+void getMultipliers(const State& state,
+ Array_<Real>& multipliers) const;
+/**@}**/
+
+protected:
+/** @name Optional realize() virtual methods
+Provide implementations of these methods if you want to allocate State
+variables (such as modeling options or parameters) or want to pre-calculate
+some expensive quantities and store them in the State cache for your future
+use. Note that the Position, Velocity, and Acceleration-stage realize methods
+will be called <em>after</em> the constraint error calculating methods
+associated with this Constraint's constraint equations have been used by
+Simbody to perform any constraint calculations. That means, for example, you
+can access calculated multipliers from your realizeAcceleration() method. **/
+
+/**@{**/
+/** The Matter Subsystem's realizeTopology() method will call this method after
+all MobilizedBody topology has been processed. This gives the Constraint a
+chance to
+ - calculate Topology stage "cache" values (mutable values which are
+ stored in the derived Implementation class directly), and
+ - allocate Model-stage state variables for later use, and
+ - allocate Model-stage cache entries in the State.
+The indices to the Model-stage state & cache entries must be stored locally as
+part of the Topology-stage cache. **/
+virtual void realizeTopology(State&) const { }
+
+/** The Matter Subsystem's realizeModel() method will call this method after
+all MobilizedBody Model-stage processing has been done. This gives the
+Constraint a chance to
+ - calculate Model stage cache values according to the settings of the
+ Model variables,
+ - allocate any later-Stage variables that may be needed (typically these
+ will be Instance stage variables containing geometric information or
+ constraint parameters like lengths or velocities.
+The indices to any of the State entries allocated here must be stored in the
+State as part of the Model-stage cache. **/
+virtual void realizeModel(State&) const { }
+
+/** The Matter Subsystem's realizeInstance() method will call this method after
+all MobilizedBody Instance-stage processing has been done. This gives the
+Constraint a chance to
+ - calculate Instance stage cache values according to the settings of the
+ Instance variables. **/
+virtual void realizeInstance(const State&) const { }
+
+/** The Matter Subsystem's realizeTime() method will call this method after any
+MobilizedBody Time-stage processing has been done. This gives the Constraint
+a chance to
+ - calculate Time stage cache values according to the current value of
+ time found in the State. **/
+virtual void realizeTime(const State&) const { }
+
+/** The Matter Subsystem's realizePosition() method will call this method after
+any MobilizedBody Position-stage processing has been done, and \e after the
+call has been made to your calcPositionErrors() operator. This gives the
+Constraint a chance to
+ - calculate Position stage cache values according to the current values of
+ positions and position errors found in the State. **/
+virtual void realizePosition(const State&) const { }
+
+/** The Matter Subsystem's realizeVelocity() method will call this method after
+any MobilizedBody Velocity-stage processing has been done, and \e after your
+calcVelocityErrors() and calcPositionDotErrors() operators have been called.
+This gives the Constraint a chance to
+ - calculate Velocity stage cache values according to the current values of
+ velocities and velocity errors found in the State. **/
+virtual void realizeVelocity(const State&) const { }
+
+/** The Matter Subsystem's realizeDynamics() method will call this method after
+any MobilizedBody Dynamics-stage processing has been done. This gives the
+Constraint a chance to
+ - calculate Dynamics stage cache values according to the current values found
+ in the State. **/
+virtual void realizeDynamics(const State&) const { }
+
+/** The Matter Subsystem's realizeAcceleration() method will call this method
+after any MobilizedBody Acceleration-stage processing has been done, and
+\e after your calcAccelerationErrors(), calcVelocityDotErrors(), and
+calcPositionDotDotErrors() operators have been called. This gives the
+Constraint a chance to
+ - calculate Acceleration stage cache values according to the current values
+ of body and mobility accelerations, acceleration errors, and multiplier
+ values found in the state. **/
+virtual void realizeAcceleration(const State&) const { }
+
+/** The Matter Subsystem's realizeReport() method will call this method after
+any MobilizedBody Report-stage processing has been done. This gives the
+Constraint a chance to
+ - calculate Report stage cache values according to the current values found
+ in the State. **/
+virtual void realizeReport(const State&) const { }
+/**@}**/
+
+/** @name Position (Holonomic) Constraint Virtuals
+These must be defined if there are any position (holonomic) constraint
+equations generated by this %Constraint. **/
+/**@{**/
+
+/** Calculate the \e mp position-constraint errors due to the position-level
+specification of a holonomic constraint and write them to \a perr, which will
+have been allocated to length \e mp; do not reallocate it. When this is called,
+\a state will already have been realized to Stage::Time; all position
+information used in your implementation must be taken from the passed-in
+arguments \a X_AB and \a constrainedQ, not from \a state. **/
+virtual void calcPositionErrors
+ (const State& state, // Stage::Time
+ const Array_<Transform,ConstrainedBodyIndex>& X_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr) // mp of these
+ const;
+
+/** Calculate the \e mp velocity errors arising from the first time derivative
+of the position-level holonomic constraint function calcPositionErrors(), and
+write them to \a pverr, which will have been allocated to length \e mp; do not
+reallocate it. When this is called, \a state will have already been realized to
+Stage::Position; all velocity information used in your implementation must be
+taken from the passed-in arguments \a V_AB and \a constrainedQDot, not from
+\a state. However, you can obtain position information for the constrained
+bodies and constrained mobilizers from \a state using getOneQFromState(),
+getBodyTransformFromState(), and related methods. The implementation of this
+method must produce \e exactly the time derivative of the implementation of
+calcPositionErrors(). **/
+virtual void calcPositionDotErrors
+ (const State& state, // Stage::Position
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr) // mp of these
+ const;
+
+/** Calculate the \e mp errors arising from the second time derivative of the
+position-level holonomic constraint function calcPositionErrors(), and write
+them to \a paerr, which will have been allocated to length \e mp; do not
+reallocate it. When this is called, \a state will already have been realized to
+Stage::Velocity; all acceleration-level information used in your implementation
+must be taken from the passed-in arguments \a A_AB and \a constrainedQDotDot,
+\e not from \a state. However, you can obtain position and velocity information
+for the constrained bodies and constrained mobilizers from \a state using
+getOneQFromState(), getOneQDotFromState(), getBodyTransformFromState(),
+getBodyVelocityFromState(), and related methods. The implementation of this
+method must produce \e exactly the time derivative of the implementation of
+calcPositionDotErrors(). **/
+virtual void calcPositionDotDotErrors
+ (const State& state, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr) // mp of these
+ const;
+
+/** From the \e mp supplied Lagrange multipliers provided in \a multipliers,
+calculate the forces produced by this Constraint on its Constrained Bodies and
+Constrained Qs. Body spatial forces are applied at the body origin and
+expressed in the Ancestor frame and written to an array \a bodyForcesInA of
+length getNumConstrainedBodies(). Q forces are written to an array \a qForces
+of length getNumConstrainedQ(), that is, the number of constrained
+<em>generalized coordinates</em> q, not the number of constrained \e mobilizers
+or constrained \e mobilities u. When this is called, \a state will already have
+been realized to Stage::Position and all position-stage cache information is
+available including any that may have been calculated during the prior call to
+this Constraint's calcPositionErrors() method and realizePosition() method.
+Simbody will already have ensured that the force-return arrays have been
+allocated to the right size and properly initialized; you need update only
+those to which you are applying forces.
+
+ at note Don't forget that you must <em>add in</em> your force contributions;
+don't just write them or you'll wipe out all preceding constraints'
+contributions! **/
+virtual void addInPositionConstraintForces
+ (const State& state,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedQIndex>& qForces) const;
+/**@}**/
+
+/** @name Velocity (Nonholonomic) Constraint Virtuals
+These must be defined if there are any velocity (nonholonomic) constraint
+equations generated by this Constraint. **/
+/**@{**/
+
+/** Calculate the \e mv velocity-constraint errors due to the velocity-level
+specification of a nonholonomic constraint and write them to \a verr, which will
+already have been allocated to length \e mv; do not reallocate it. When this
+is called, \a state will have been realized to Stage::Position; all
+velocity-level information used in your implementation must be taken from the
+passed-in arguments \a V_AB and \a constrainedU, not from \a state. However,
+you may obtain time or any position-related information from \a state.
+A nonholonomic constraint may depend on \e any position information; you do
+not have to limit that to constrained bodies and mobilizers as you do for
+velocity-level information. **/
+virtual void calcVelocityErrors
+ (const State& state, // Stage::Position
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedU,
+ Array_<Real>& verr) // mv of these
+ const;
+
+/** Calculate the \e mv errors arising from the first time derivative
+of the velocity-level specification of a nonholonomic constraint and write them
+to \a vaerr, which will already have been allocated to length \e mv; do not
+reallocate it. When this is called, \a state will have been realized to
+Stage::Velocity; all acceleration-level information used in your implementation
+must be taken from the passed-in arguments \a A_AB and \a constrainedUDot,
+\e not from \a state. However, you can obtain from \a state time, and any
+needed position and velocity information. The implementation of this method
+must produce \e exactly the time derivative of the implementation of
+calcVelocityErrors(). **/
+virtual void calcVelocityDotErrors
+ (const State& state, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedUDot,
+ Array_<Real>& vaerr) // mv of these
+ const;
+
+/** From the \p mv supplied Lagrange multipliers provided in \a multipliers,
+calculate the forces produced by this Constraint on its Constrained Bodies and
+Constrained Mobilities due to its velocity-level (nonholonomic) constraints.
+Body spatial forces are applied at the body origin and expressed in the
+Ancestor frame and written to an array \a bodyForcesInA of length
+getNumConstrainedBodies(). Mobility (generalized) forces are written to an
+array \a mobilityForces of length getNumConstrainedU(), that is, the number of
+constrained \e mobilities, not the number of constrained \e mobilizers. The
+supplied \a state will have been realized to Stage::Velocity and all
+position- and velocity-stage cache information is available including any that
+may have been calculated during the prior call to this constraint's
+realizePosition() and realizeVelocity() methods. Simbody will already have
+ensured that the force-return arrays have been allocated to the right
+size and initialized properly; you need only update the non-zero ones.
+
+ at note Don't forget that you must <em>add in</em> your force contributions;
+don't just write them or you'll wipe out all preceding constraints'
+contributions! **/
+virtual void addInVelocityConstraintForces
+ (const State& state,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedUIndex>& mobilityForces) const;
+/**@}**/
+
+/** @name Acceleration-Only Constraint Virtuals
+These must be defined if there are any acceleration-only constraint equations
+generated by this Constraint. **/
+/**@{**/
+
+/** Calculate the \e ma acceleration-constraint errors due to the
+specification of an acceleration-only constraint and write them to \a aerr,
+which will already have been allocated to length \e ma; do not reallocate it.
+When this is called, \a state will have been realized to Stage::Velocity; all
+acceleration-level information used in your implementation must be taken from
+the passed-in arguments \a A_AB and \a constrainedUDot, \e not from \a state.
+However, an acceleration-only constraint may depend arbitrarily on time,
+position, and velocity information which you may obtain freely from \a state;
+you do not have to limit that to constrained bodies and mobilizers as you do
+for acceleration-level information.
+
+ at note This method \e must be linear in the accelerations; Simbody has no
+way to enforce that so it is up to you to do this correctly. **/
+virtual void calcAccelerationErrors
+ (const State& state, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedUDot,
+ Array_<Real>& aerr) // ma of these
+ const;
+
+/** From the \e ma supplied Lagrange multipliers provided in \a multipliers,
+calculate the forces produced by this Constraint on its Constrained Bodies and
+Constrained Mobilities due to its acceleration-only constraints. Body spatial
+forces are applied at the body origin and expressed in the Ancestor frame and
+written to an array \a bodyForcesInA of length getNumConstrainedBodies().
+Mobility forces are written to an array \a mobilityForces of length
+getNumConstrainedU(), that is, the number of constrained \e mobilities, not the
+number of constrained \e mobilizers. The \a state will have been realized to
+Stage::Velocity and all position- and velocity-stage cache information is
+available including any that may have been calculated during the prior call to
+this constraint's realizePosition() and realizeVelocity() methods. Simbody will
+already have ensured that the force-return arrays have been allocated to the
+right size and initialized properly; you need only update the non-zero ones.
+
+ at note Don't forget that you must <em>add in</em> your force contributions;
+don't just write them or you'll wipe out all preceding constraints'
+contributions! **/
+virtual void addInAccelerationConstraintForces
+ (const State& state,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedUIndex>& mobilityForces) const;
+/**@}**/
+
+/** Implement this optional method if you would like your constraint to
+generate any suggestions for geometry that could be used as default
+visualization as an aid to understanding a system containing this constraint.
+For example, if your constraint connects two points, you might want to draw a
+line between those points. You can also generate text labels, and you can
+provide methods for controlling the presence or appearance of your generated
+geometry. If you don't implement this routine no geometry will be generated. **/
+virtual void calcDecorativeGeometryAndAppend
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+{
+}
+
+friend class Constraint::CustomImpl;
+};
+
+
+
+//==============================================================================
+// COORDINATE COUPLER
+//==============================================================================
+/** This is a %Constraint that uses a Function object to define a single
+holonomic (position) constraint equation acting to relate a set of generalized
+coordinates q. You provide a Function which takes some subset of the system's
+generalized coordinates as arguments, and returns a single value. It also must
+support partial derivatives up to second order. The constraint enforces that
+the value of the function should equal 0 at all times. For example, if you
+wanted q1 and q2 to be constrained to have the same value you could define your
+function f as f=q1-q2. **/
+class SimTK_SIMBODY_EXPORT Constraint::CoordinateCoupler
+: public Constraint::Custom {
+public:
+ /** Create a CoordinateCoupler. You specify a Function and a list of
+ generalized coordinates to pass to it as arguments. Each generalized
+ coordinate is specified by a MobilizedBody and the index of the coordinate
+ within its mobilizer. For example <pre>
+ matter.getMobilizedBody(coordMobod[2]).getOneQ(state, coordQIndex[2])
+ </pre> will be passed to the Function as the value of the second argument.
+
+ @param matter
+ The matter subsystem to which this constraint will be added.
+ @param function
+ The Function whose value should be maintained at zero by this
+ constraint at all times. The constraint takes over ownership of this
+ object, and automatically deletes in when the constraint is deleted.
+ @param coordMobod
+ The MobilizedBody corresponding to each generalized coordinate that
+ should be passed as a function argument.
+ @param coordQIndex
+ The index corresponding to each generalized coordinate that should be
+ passed as a function argument.
+ **/
+ CoordinateCoupler(SimbodyMatterSubsystem& matter,
+ const Function* function,
+ const Array_<MobilizedBodyIndex>& coordMobod,
+ const Array_<MobilizerQIndex>& coordQIndex);
+
+ /** For compatibility with std::vector; no copying is done. **/
+ CoordinateCoupler(SimbodyMatterSubsystem& matter, const Function* function,
+ const std::vector<MobilizedBodyIndex>& coordBody,
+ const std::vector<MobilizerQIndex>& coordIndex)
+ { // Invoke the above constructor with converted arguments.
+ new (this) CoordinateCoupler(matter,function,
+ ArrayViewConst_<MobilizedBodyIndex>(coordBody),
+ ArrayViewConst_<MobilizerQIndex>(coordIndex));
+ }
+
+ /** Default constructor creates an empty handle. **/
+ CoordinateCoupler() {}
+};
+
+
+
+//==============================================================================
+// SPEED COUPLER
+//==============================================================================
+/** This is a %Constraint that uses a Function object to
+define a nonholonomic (velocity) constraint. You provide a Function which takes
+some subset of the system's generalized speeds as arguments, and returns a
+single value. It also must support partial derivatives up to second order. The
+constraint enforces that the value of the function should equal 0 at all times.
+
+The Function may optionally depend on coordinates (q) as well as speeds (u),
+but it only acts as a constraint on the speeds. The constraint takes the
+current values of the coordinates as constants, then tries to modify only the
+speeds so as to satisfy the constraint. **/
+class SimTK_SIMBODY_EXPORT Constraint::SpeedCoupler : public Constraint::Custom {
+public:
+ /** Create a SpeedCoupler. You specify a Function and a list of generalized
+ speeds to pass to it as arguments. Each generalized speed is specified by a
+ MobilizedBody and the index of the speeds within that body. For example
+ matter.getMobilizedBody(bodies[2]).getOneU(state, speeds[2]) will be passed
+ to the function as the value of the second argument.
+
+ @param matter
+ The matter subsystem to which this constraint will be added.
+ @param function
+ The Function whose value should equal 0 at all times. The constraint
+ takes over ownership of this object, and automatically deletes it when
+ the constraint is deleted.
+ @param speedBody
+ The MobilizedBody corresponding to each generalized speed that should be
+ passed as a function argument.
+ @param speedIndex
+ The index corresponding to each generalized speed that should be passed
+ as a function argument. **/
+ SpeedCoupler(SimbodyMatterSubsystem& matter,
+ const Function* function,
+ const Array_<MobilizedBodyIndex>& speedBody,
+ const Array_<MobilizerUIndex>& speedIndex);
+
+ /** For compatibility with std::vector; no copying is done. **/
+ SpeedCoupler(SimbodyMatterSubsystem& matter,
+ const Function* function,
+ const std::vector<MobilizedBodyIndex>& speedBody,
+ const std::vector<MobilizerUIndex>& speedIndex)
+ { // Invoke above constructor with converted arguments.
+ new (this) SpeedCoupler(matter, function,
+ ArrayViewConst_<MobilizedBodyIndex>(speedBody),
+ ArrayViewConst_<MobilizerUIndex>(speedIndex));
+ }
+
+ /** Create a SpeedCoupler. You specify a Function and a list of generalized
+ coordinates and speeds to pass to it as arguments. Each generalized speed is
+ specified by a MobilizedBody and the index of the speeds within that body.
+ For example matter.getMobilizedBody(bodies[2]).getOneU(state, speeds[2]) will
+ be passed to the function as the value of the second argument. Generalized
+ coordinates come after generalized speeds in the argument list. For example,
+ if you specify three generalized speeds and two generalized coordinates, the
+ Function must take a total of five arguments. The first three are the speeds,
+ and the last two are the coordinates.
+
+ @param matter
+ The matter subsystem to which this constraint will be added.
+ @param function
+ The Function whose value should equal 0 at all times. The constraint
+ takes over ownership of this object, and automatically deletes it when
+ the constraint is deleted.
+ @param speedBody
+ The MobilizedBody corresponding to each generalized speed that should be
+ passed as a function argument.
+ @param speedIndex
+ The index corresponding to each generalized speed that should be passed
+ as a function argument.
+ @param coordBody
+ The MobilizedBody corresponding to each generalized coordinate that
+ should be passed as a function argument.
+ @param coordIndex
+ The index corresponding to each generalized coordinate that should be
+ passed as a function argument. **/
+ SpeedCoupler(SimbodyMatterSubsystem& matter,
+ const Function* function,
+ const Array_<MobilizedBodyIndex>& speedBody,
+ const Array_<MobilizerUIndex>& speedIndex,
+ const Array_<MobilizedBodyIndex>& coordBody,
+ const Array_<MobilizerQIndex>& coordIndex);
+
+ /** For compatibility with std::vector; no copying is done. **/
+ SpeedCoupler(SimbodyMatterSubsystem& matter,
+ const Function* function,
+ const std::vector<MobilizedBodyIndex>& speedBody,
+ const std::vector<MobilizerUIndex>& speedIndex,
+ const std::vector<MobilizedBodyIndex>& coordBody,
+ const std::vector<MobilizerQIndex>& coordIndex)
+ { // Invoke above constructor with converted arguments.
+ new (this) SpeedCoupler(matter, function,
+ ArrayViewConst_<MobilizedBodyIndex>(speedBody),
+ ArrayViewConst_<MobilizerUIndex>(speedIndex),
+ ArrayViewConst_<MobilizedBodyIndex>(coordBody),
+ ArrayViewConst_<MobilizerQIndex>(coordIndex));
+ }
+
+ /** Default constructor creates an empty handle. **/
+ SpeedCoupler() {}
+};
+
+
+
+//==============================================================================
+// PRESCRIBED MOTION
+//==============================================================================
+/** This is a %Constraint that uses a Function to prescribe
+the behavior of a single generalized coordinate as a function of time. You
+provide a Function which takes the current time as its argument and returns the
+required value of the generalized coordinate. It also must support derivatives
+up to second order. **/
+class SimTK_SIMBODY_EXPORT Constraint::PrescribedMotion
+: public Constraint::Custom {
+public:
+ /** Create a PrescribedMotion constraint. You specify a Function that takes
+ time as its single argument, and returns the required value for the
+ constrained coordinate.
+
+ @param matter
+ The matter subsystem to which this constraint will be added.
+ @param function
+ The Function which specifies the value of the constrained coordinate.
+ The constraint takes over ownership of this object, and automatically
+ deletes it when the constraint is deleted.
+ @param coordBody
+ The MobilizedBody corresponding to the generalized coordinate which will
+ be constrained.
+ @param coordIndex
+ The index of the generalized coordinate which will be constrained. **/
+ PrescribedMotion(SimbodyMatterSubsystem& matter,
+ const Function* function,
+ MobilizedBodyIndex coordBody,
+ MobilizerQIndex coordIndex);
+
+
+ /** Default constructor creates an empty handle. **/
+ PrescribedMotion() {}
+};
+
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_CONSTRAINT_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/ContactSurface.h b/Simbody/include/simbody/internal/ContactSurface.h
new file mode 100644
index 0000000..9ccb195
--- /dev/null
+++ b/Simbody/include/simbody/internal/ContactSurface.h
@@ -0,0 +1,462 @@
+#ifndef SimTK_SIMBODY_CONTACT_SURFACE_H_
+#define SimTK_SIMBODY_CONTACT_SURFACE_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-13 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares ContactMaterial and ContactSurface classes. **/
+
+#include "SimTKmath.h"
+#include "simbody/internal/common.h"
+
+#include <algorithm>
+
+namespace SimTK {
+
+class ContactGeometry;
+
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(ContactCliqueId);
+
+
+
+//==============================================================================
+// CONTACT MATERIAL
+//==============================================================================
+/** Define the physical properties of the material from which a contact
+surface is made, including properties needed by a variety of contact response
+techniques that might be applied during contact. Two basic techniques are
+supported:
+ - compliant contact
+ - rigid contact
+
+Each of these techniques can respond to both impacts and continuous contact,
+and provides for both compressive and frictional effects.
+
+For compliant contact this material can be applied in different ways:
+ - the material represents an elastic solid, large in comparison to the
+ region of contact deformation
+ - the material represents a uniform thin elastic layer over an
+ otherwise-rigid body (elastic foundation model).
+
+The elastic foundation layer thickness h is \e not a property of this material;
+instead it is specified at the time this material is used to construct a
+ContactSurface; the material properties supplied here are independent of how
+the material is used.
+
+<h3>Compliant vs. rigid contact models</h3>
+
+Compliant contact models respond via forces that are generated by
+finite deformations of the contact surface, using theory of elasticity. Rigid
+contact models instead enforce zero deformation at the contact surface via
+the reaction forces of non-penetration contraints, and respond to impacts by
+generating impulsive changes to velocities rather than forces.
+
+Energy dissipation upon impact is dealt with differently in the two schemes.
+In compliant contact, energy dissipation occurs via a continuous function
+of deformation and deformation rate, based on a material property c we call
+the "dissipation coefficient" (Hunt & Crossley call this "alpha"). In rigid
+contact, energy dissipation occurs via a "coefficient of restitution" e that
+defines what fraction of (a) impact velocity (Newton's hypothesis) or (b)
+collision impulse (Poisson's hypothesis) is lost on rebound. It is common
+practice, but blatantly incorrect, to use a constant value for the coefficient
+of restitution. Empirically, coefficient of restitution e is known to be a
+linear function of impact velocity for small velocities: e=1-c*v where c is
+the dissipation coefficient mentioned above. At large velocities plasticity and
+other effects cause the coefficient of restitution to drop dramatically; we
+do not attempt to support plasticity models here. **/
+class SimTK_SIMBODY_EXPORT ContactMaterial {
+public:
+/** Default constructor creates an invalid contact material; you must
+specify at least stiffness before using this material. Defaults are provided
+for dissipation (none) and friction (none). **/
+ContactMaterial() {clear();}
+
+/** Create a contact material with a complete set of compliant contact
+material properties. This does not set the coefficient of restitution.
+There are a variety of static methods provided to aid in calculating the
+stiffness and dissipation from available data.
+ at param[in] stiffness
+ This is in units of pressure per unit strain, where pressure is force per
+ unit area, and strain is a unitless measure of deformation. There are
+ a variety of ways to compute this from Young's modulus and Poisson's
+ ratio; see for example calcPlaneStrainStiffness() and
+ calcConfinedCompressionStiffness(). Note that different compliant contact
+ models determine strain differently for a given deformation x. For example,
+ Hertz uses the relationship between x and local curvatures to determine the
+ fractional strain, while Elastic Foundation must be given a "thickness" h
+ with strain then defined as x/h. We require \a stiffness >= 0.
+ at param[in] dissipation
+ This is the Hunt and Crossley dissipation coefficient for this material
+ in units of 1/velocity. This is the fraction of stiffness force generated
+ as a dissipation force per unit deformation rate, via
+ f_dissipation = f_stiffness * (dissipation * v_deformation). You can
+ measure this as the slope of the coefficient of restitution (e) vs. impact
+ velocity (v) curve at low velocities, where e=1-c*v.
+ We require \a dissipation >= 0.
+ at param[in] staticFriction
+ This is the Coulomb "stiction" coefficient mu_s that is used at very low
+ sliding velocities, with tangential force limited to f=mu_s*N where N
+ is the contact normal force due to stiffness and dissipation effects.
+ We require \a staticFriction >= \a dynamicFriction >= 0.
+ at param[in] dynamicFriction
+ This is the Coulomb friction coefficient mu_d that is to be used at
+ significant sliding velocities, with velocity-independent tangential
+ force magnitude f=mu_d*N produced opposing the sliding direction, where
+ N is the contact normal force due to stiffness and dissipation effects.
+ We require 0 <= \a dynamicFriction <= \a staticFriction.
+ at param[in] viscousFriction
+ This is the "wet" friction coefficient mu_v >= 0 for this material
+ generating a sliding velocity-dependent tangential force of magnitude
+ f=mu_v*v*N where v is the sliding velocity and N is the contact normal
+ force due to stiffness and dissipation effects. This is not commonly
+ used and defaults to zero.
+**/
+ContactMaterial(Real stiffness, Real dissipation,
+ Real staticFriction, Real dynamicFriction,
+ Real viscousFriction = 0) {
+ setStiffness(stiffness);
+ setDissipation(dissipation);
+ setFriction(staticFriction, dynamicFriction, viscousFriction);
+}
+
+/** Return false if material properties have not yet been supplied for this
+contact material. **/
+bool isValid() const {return m_stiffness > 0;}
+
+/** Return the material stiffness k=(force/area)/%%strain. **/
+Real getStiffness() const
+{ SimTK_ERRCHK(isValid(), "ContactMaterial::getStiffness()",
+ "This is an invalid ContactMaterial.");
+ return m_stiffness; }
+/** Return precalculated 2/3 power of material stiffness k (k^(2/3)). **/
+Real getStiffness23() const
+{ SimTK_ERRCHK(isValid(), "ContactMaterial::getStiffness23()",
+ "This is an invalid ContactMaterial.");
+ return m_stiffness23; }
+/** Return the material dissipation coefficient c, in units of 1/velocity. **/
+Real getDissipation() const
+{ SimTK_ERRCHK(isValid(), "ContactMaterial::getDissipation()",
+ "This is an invalid ContactMaterial.");
+ return m_dissipation; }
+/** Return the coefficient of static friction mu_s (unitless). **/
+Real getStaticFriction() const
+{ SimTK_ERRCHK(isValid(), "ContactMaterial::getStaticFriction()",
+ "This is an invalid ContactMaterial.");
+ return m_staticFriction; }
+/** Return the coefficient of dynamic friction mu_d (unitless). **/
+Real getDynamicFriction() const
+{ SimTK_ERRCHK(isValid(), "ContactMaterial::getDynamicFriction()",
+ "This is an invalid ContactMaterial.");
+ return m_dynamicFriction; }
+/** Return the coefficient of viscous friction mu_v (1/velocity). **/
+Real getViscousFriction() const
+{ SimTK_ERRCHK(isValid(), "ContactMaterial::getViscousFriction()",
+ "This is an invalid ContactMaterial.");
+ return m_viscousFriction; }
+
+/** Supply material stiffness k=(force/area)/%%strain. A compliant
+contact model will then calculate the contact area A and strain fraction
+s to calculate the resulting force f=k*A*s. **/
+ContactMaterial& setStiffness(Real stiffness) {
+ SimTK_ERRCHK1_ALWAYS(stiffness >= 0, "ContactMaterial::setStiffness()",
+ "Stiffness %g is illegal; must be >= 0.", stiffness);
+ m_stiffness = stiffness;
+ m_stiffness23 = std::pow(m_stiffness, Real(2./3.));
+ return *this;
+}
+
+/** Supply material dissipation coefficient c, which is the slope of the
+coefficient of restitution (e) vs. impact speed (v) curve, with e=1-cv at
+low (non-yielding) impact speeds v. **/
+ContactMaterial& setDissipation(Real dissipation) {
+ SimTK_ERRCHK1_ALWAYS(dissipation >= 0, "ContactMaterial::setDissipation()",
+ "Dissipation %g (in 1/speed) is illegal; must be >= 0.", dissipation);
+ m_dissipation = dissipation;
+ return *this;
+}
+
+/** Set the friction coefficients for this material. **/
+ContactMaterial& setFriction(Real staticFriction,
+ Real dynamicFriction,
+ Real viscousFriction = 0)
+{
+ SimTK_ERRCHK1_ALWAYS(0 <= staticFriction, "ContactMaterial::setFriction()",
+ "Illegal static friction coefficient %g.", staticFriction);
+ SimTK_ERRCHK2_ALWAYS(0<=dynamicFriction && dynamicFriction<=staticFriction,
+ "ContactMaterial::setFriction()",
+ "Dynamic coefficient %g illegal; must be between 0 and static"
+ " coefficient %g.", dynamicFriction, staticFriction);
+ SimTK_ERRCHK1_ALWAYS(0 <= viscousFriction, "ContactMaterial::setFriction()",
+ "Illegal viscous friction coefficient %g.", viscousFriction);
+
+ m_staticFriction = staticFriction;
+ m_dynamicFriction = dynamicFriction;
+ m_viscousFriction = viscousFriction;
+ return *this;
+}
+
+/** Calculate the contact material stiffness k from its linear elastic
+material properties Young's modulus E and Poisson's ratio v, assuming that
+the material is free to move under pressure. In this circumstance the
+effective stiffness is given by the plane strain modulus k=E/(1-v^2), and
+the stiffness never exceeds (4/3)E when the material becomes incompressible
+as v->0.5 as is typical of rubber. If this material is to be used in a
+circumstance in which its volume must be reduced under contact pressure (because
+there is no room to move), consider using the confined compression modulus
+instead. @see calcConfinedCompressionStiffness() **/
+static Real calcPlaneStrainStiffness(Real youngsModulus,
+ Real poissonsRatio)
+{
+ SimTK_ERRCHK2_ALWAYS(youngsModulus >= 0 &&
+ -1 < poissonsRatio && poissonsRatio <= 0.5,
+ "ContactMaterial::calcStiffnessForSolid()",
+ "Illegal material properties E=%g, v=%g.",
+ youngsModulus, poissonsRatio);
+
+ return youngsModulus / (1-square(poissonsRatio));
+}
+
+
+/** Calculate the contact material stiffness k from its linear elastic
+material properties Young's modulus E and Poisson's ratio v, assuming that
+the material is confined such that its volume must be reduced under pressure.
+In this case the effective stiffness is given by the confined compression
+modulus k=E(1-v)/((1+v)(1-2v)). Note that the stiffness becomes infinite when
+the material becomes incompressible as v->0.5. That means rubber will have
+a near infinite stiffness if you calculate it this way; make sure that's what
+you want! @see calcPlaneStrainStiffness() **/
+static Real calcConfinedCompressionStiffness(Real youngsModulus,
+ Real poissonsRatio)
+{
+ SimTK_ERRCHK2_ALWAYS(youngsModulus >= 0 &&
+ -1 < poissonsRatio && poissonsRatio < 0.5,
+ "ContactMaterial::calcStiffnessForSolid()",
+ "Illegal material properties E=%g, v=%g.",
+ youngsModulus, poissonsRatio);
+
+ return youngsModulus*(1-poissonsRatio)
+ / ((1+poissonsRatio)*(1-2*poissonsRatio));
+}
+
+/** Given an observation of the coefficient of restitution e at a given small
+impact velocity v, calculate the apparent value of the dissipation coefficient
+c, which is a material property. You should get the same value for c from
+any observation, provided that v is non-zero but small. The coefficient of
+restitution as v->0 is 1 for all materials.
+
+Coefficient of restitution at low impact speed is linearly related to
+impact velocity by the dissipation coefficient c (1/velocity) by e=(1-cv) so we
+can calculate c if we have observed e at some low speed v as c=(1-e)/v. If the
+coefficient of restitution is 1 we'll return c=0 and ignore v, otherwise
+v must be greater than zero. **/
+static Real calcDissipationFromObservedRestitution
+ (Real restitution, Real speed) {
+ if (restitution==1) return 0;
+ SimTK_ERRCHK2_ALWAYS(0<=restitution && restitution<=1 && speed>0,
+ "ContactMaterial::calcDissipationFromRestitution()",
+ "Illegal coefficient of restitution or speed (%g,%g).",
+ restitution, speed);
+ return (1-restitution)/speed;
+}
+
+/** Restore this contact material to an invalid state containing no stiffness
+specification and default values for dissipation (none) and friction
+(none). **/
+void clear() {
+ m_stiffness = NaN; // unspecified
+ m_stiffness23 = NaN;
+ m_restitution = NaN; // unspecified
+ m_dissipation = 0; // default; no dissipation
+ // default; no friction
+ m_staticFriction=m_dynamicFriction=m_viscousFriction = 0;
+}
+
+//--------------------------------------------------------------------------
+private:
+
+// For compliant contact models.
+Real m_stiffness; // k: stress/%strain=(force/area)/%strain
+Real m_stiffness23; // k^(2/3) in case we need it
+Real m_dissipation; // c: %normalForce/normalVelocity
+
+// For impulsive collisions.
+Real m_restitution; // e: unitless, e=(1-cv)
+
+// Friction.
+Real m_staticFriction; // us: unitless
+Real m_dynamicFriction; // ud: unitless
+Real m_viscousFriction; // uv: %normalForce/slipVelocity
+};
+
+
+
+//==============================================================================
+// CONTACT SURFACE
+//==============================================================================
+/** This class combines a piece of ContactGeometry with a ContactMaterial to
+make an object suitable for attaching to a body which can then engage in
+contact behavior with other contact surfaces. The ContactMaterial may be
+considered as uniform throughout a large solid volume or as a thin layer of
+uniform material thickness h on top of a rigid substrate. Compliant contact
+models vary in how they deal with the thickness h: Hertz ignores it, Elastic
+Foundation uses it to define unit strain.
+
+Typically any contact surface can bump into any other contact surface. However,
+some groups of surfaces can or should never interact; those groups are called
+"contact cliques". The set of surfaces that are mounted to the same rigid body
+always constitues a clique. In addition, other cliques may be defined and
+arbitrary surfaces made members so that they won't interact. This is commonly
+used for contact surfaces on adjacent bodies when those surfaces are near the
+joint between two bodies. **/
+class SimTK_SIMBODY_EXPORT ContactSurface {
+public:
+/** Create an empty ContactSurface. **/
+ContactSurface() {}
+/** Create a ContactSurface with a given shape and material. **/
+ContactSurface(const ContactGeometry& shape,
+ const ContactMaterial& material,
+ Real thickness=0)
+: m_shape(shape), m_material(material), m_thickness(thickness) {
+ SimTK_ERRCHK1_ALWAYS(thickness >= 0, "ContactSurface::ctor()",
+ "Illegal thickness %g.", thickness);
+}
+
+/** Define a new shape for this ContactSurface. **/
+ContactSurface& setShape(const ContactGeometry& shape)
+{ m_shape = shape; return *this; }
+
+/** Define a new material for this ContactSurface, optionally providing
+the thickness of this elastic material layer over a rigid substrate. **/
+ContactSurface& setMaterial(const ContactMaterial& material,
+ Real thickness = 0)
+{ m_material = material; setThickness(thickness); return *this; }
+
+/** Set the thickness of the layer of elastic material coating this contact
+surface. This is required by the Elastic Foundation Model but is ignored
+by the Hertz model. Elastic Foundation uses this value to define "unit strain",
+that is, an element deformed by 0.1*thickness has 10% strain. **/
+ContactSurface& setThickness(Real thickness) {
+ SimTK_ERRCHK1_ALWAYS(thickness >= 0, "ContactSurface::setThickness()",
+ "Illegal thickness %g.", thickness);
+ m_thickness = thickness;
+ return *this;
+}
+
+/** Get read-only access to the shape of this contact surface. **/
+const ContactGeometry& getShape() const {return m_shape;}
+
+/** Get read-only access to the material of this contact surface. **/
+const ContactMaterial& getMaterial() const {return m_material;}
+
+/** Get the thickness of the elastic material layer on this contact surface,
+with zero indicating that the thickness has not been set. **/
+Real getThickness() const {return m_thickness;}
+
+/** Get writable access to the shape of this contact surface. **/
+ContactGeometry& updShape() {return m_shape;}
+
+/** Get writable access to the material of this contact surface. **/
+ContactMaterial& updMaterial() {return m_material;}
+
+/** Join a contact clique if not already a member. It is OK to pass an invalid
+clique id here, in which case it will be quietly ignored. **/
+ContactSurface& joinClique(ContactCliqueId clique) {
+ if (!clique.isValid()) return *this;
+ // Although this is a sorted list, we expect it to be very short so
+ // are using linear search to find where this new clique goes.
+ Array_<ContactCliqueId,short>::iterator p;
+ for (p = m_cliques.begin(); p != m_cliques.end(); ++p) {
+ if (*p==clique) return *this; // already a member
+ if (*p>clique) break;
+ }
+ // insert just before p (might be at the end)
+ m_cliques.insert(p, clique);
+ return *this;
+}
+
+/** Remove this surface from a contact clique if it is a member. It is OK to
+pass an invalid clique id here, in which case it will be quietly ignored. **/
+void leaveClique(ContactCliqueId clique) {
+ if (!clique.isValid()) return;
+ // We expect this to be a very short list so are using linear search.
+ Array_<ContactCliqueId,short>::iterator p =
+ std::find(m_cliques.begin(), m_cliques.end(), clique);
+ if (p != m_cliques.end()) m_cliques.erase(p);
+}
+
+/** Determine whether this contact surface is a member of any of the same
+cliques as some \a other contact surface, in which case they will be invisible
+to one another. **/
+bool isInSameClique(const ContactSurface& other) const
+{ if (getCliques().empty() || other.getCliques().empty()) return false;//typical
+ return cliquesIntersect(getCliques(), other.getCliques()); }
+
+/** Return a reference to the list of CliqueIds of the cliques in which this
+contact surface is a member; the list is always sorted in ascending order. **/
+const Array_<ContactCliqueId,short>& getCliques() const {return m_cliques;}
+
+/** Return true if there are any common cliques on two clique lists which
+must each be sorted in ascending order. This takes no longer than O(a+b)
+where a and b are the sizes of the two clique lists. **/
+static bool cliquesIntersect(const Array_<ContactCliqueId,short>& a,
+ const Array_<ContactCliqueId,short>& b)
+{
+ Array_<ContactCliqueId,short>::const_iterator ap = a.begin();
+ Array_<ContactCliqueId,short>::const_iterator bp = b.begin();
+ // Quick checks: empty or non-overlapping.
+ if (ap==a.end() || bp==b.end()) return false;
+ if (*ap > b.back() || *bp > a.back())
+ return false; // disjoint
+ // Both lists have elements and the elements overlap numerically.
+ while (true) {
+ if (*ap==*bp) return true;
+ // Increment the list with smaller front element.
+ if (*ap < *bp) {++ap; if (ap==a.end()) break;}
+ else {++bp; if (bp==b.end()) break;}
+ }
+ // One of the lists ran out of elements before we found a match.
+ return false;
+}
+
+/** Create a new contact clique and return its unique integer id (thread
+safe). Every contact surface is automatically a member of a clique containing
+all the surfaces that reside on the same body. **/
+static ContactCliqueId createNewContactClique()
+{ static AtomicInteger nextAvailableContactClique = 1;
+ return ContactCliqueId(nextAvailableContactClique++); }
+
+//----------------------------------------------------------------------
+ private:
+
+ContactGeometry m_shape;
+ContactMaterial m_material;
+Real m_thickness; // default=Infinity
+Array_<ContactCliqueId,short> m_cliques; // sorted
+};
+
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_CONTACT_SURFACE_H_
diff --git a/Simbody/include/simbody/internal/ContactTrackerSubsystem.h b/Simbody/include/simbody/internal/ContactTrackerSubsystem.h
new file mode 100644
index 0000000..c33a399
--- /dev/null
+++ b/Simbody/include/simbody/internal/ContactTrackerSubsystem.h
@@ -0,0 +1,375 @@
+#ifndef SimTK_SIMBODY_CONTACT_TRACKER_SUBSYSTEM_H_
+#define SimTK_SIMBODY_CONTACT_TRACKER_SUBSYSTEM_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) 2011-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/ContactSurface.h"
+
+namespace SimTK {
+
+
+class MultibodySystem;
+class MobilizedBody;
+class ContactTracker;
+class Contact;
+class ContactSnapshot;
+
+/** This subsystem identifies and tracks potential contacts between bodies of
+a multibody system, but does not generate any physical responses to those
+contacts. It operates on the \e undeformed, material-independent geometry of
+ContactSurfaces that have been associated with those bodies, identifying and
+characterizing pairwise geometric relationships that occur between surfaces,
+and tracking the evolution of particular Contacts through time. No physical
+response to contact is generated here; this subsystem provides only a general
+contact-tracking service that can be used by other parts of the MultibodySystem
+to generate forces, impulses, visualizations, messages, noises, or whatever.
+The goal here is to provide robust and extremely high performance
+characterization of geometric interactions in a way that is useful for a
+variety of contact response models.
+
+The ContactTrackerSubsystem maintains an evolving set of tracked Contacts
+throughout a simulation; for any given state of the system the subsystem can
+calculate the current value of that set, which we call the "contact status" of
+that state. The most recently known \e prior valid contact status is maintained
+as part of the system state because the correct instantaneous evaluation of
+contact status may require past information, and because we are interested in
+discrete contact events such as initiation and breaking of contact as well as
+ongoing interactions.
+
+Each Contact being tracked represents the interaction between a unique pair of
+ContactSurfaces; by definition there is at most one Contact between any such
+pair (per ContactTrackerSubsystem). The presence of a Contact in the set does
+not necessarily mean its two surfaces are touching, just that their proximity
+is interesting in some way. Each Contact is characterized as impending,
+initiated, ongoing, broken, or separating. At each evaluation, the last-known
+set of Contacts is used in conjuction with the current State to determine a
+disposition for each of the tracked Contacts. Contacts that have become boring
+are removed from the tracked set, and newly-interesting ones are assigned a
+new ContactId and added to the tracked contacts set. The ContactId persists as
+long as that Contact continues to remain in the set.
+
+Ambiguous or impossible contact situations can occur during trial steps and
+can be detected as long as we know the correct contact status in the immediate
+past. Problems are more common when a simulation contains fast-moving objects,
+small objects, or thin surfaces, and a relatively large time step size is
+being attempted by the integrator. For example, we may find that we could have
+missed a contact that may have occurred since the end of the last step
+(pass-through), or can't determine whether a small object is deeply penetrated
+through a thin surface or has simple "gone around the back". In such cases the
+subsystem reports an error condition that will cause the integrator to reduce
+the step size. At the start of a time stepping study, there is no past
+information available to help disambiguate; in that case heuristics are used
+to make a "best guess" at which surfaces are in contact; those can be
+overridden by a knowledgable human.
+
+As mentioned above, we track at most one Contact at a time between any pair
+of ContactSurfaces. However, for some surface types a single Contact may
+involve many geometric interactions; a mesh Contact, for example, may include
+a list of all the mesh faces that overlap between the two surfaces, and
+these do not have to be contiguous over the mesh surface. You may
+want to break up a concave surface into several ContactSurfaces so that you
+can separately track several Contacts between that surface and others.
+
+A ContactSurface consists of a piece of surface geometry and a contact material
+that describes the contact-related physical properties of the surface.
+A body may have many ContactSurface objects attached to it, each with its own
+associated contact material. Within an instance of the ContactTrackerSubsystem,
+each ContactSurface is assigned a unique ContactSurfaceIndex. If you have
+multiple ContactTrackerSubsystems, they will each have independent sets of
+ContactSurfaces (and know nothing about one another), so the (Subsystem,
+ContactSurfaceIndex) pair would be unique but not the index alone.
+
+Within a ContactTrackerSubsystem, all the ContactSurfaces are presumed to be
+capable of interacting with one another unless they share membership in a
+"contact clique". All surfaces attached to the same body are placed together
+in a clique, so they will never interact. It is also common to create a clique
+associated with each joint and place nearby contact surfaces on adjacent
+bodies into that clique to avoid having to build excessively precise geometry
+around joints.
+
+Each concrete type of ContactGeometry object (whether provided as part of
+SimTK or as an extension by the user) has a unique integer
+ContactGeometryTypeId. Ordered pairs of these Ids are used as a key to select
+a ContactTracker that is able to identify and manage contacts or
+potential contacts between those two kinds of geometric objects.
+You can provide a default tracking algorithm for unhandled pairs that will
+either ignore them or throw an error. Note that a ContactTracker
+is invoked only for "narrow phase" contact; the ContactTrackerSubsytem handles
+the "broad phase" and weeds out all but a few possible contacting surfaces
+that are then passed to ContactTracker for final disposition.
+All the ContactTrackers to be used with a given
+ContactTrackerSubsystem must be registered with that subsystem during
+extended construction (Topology stage). Simbody provides default
+ContactTrackers for interactions among most of its built-in
+Geometry types, such as Sphere-Sphere, Sphere-HalfSpace, Mesh-Sphere,
+Mesh-Mesh, etc. These will be pre-registered in every ContactTrackerSubsystem
+but you can replace them with something else if you want.
+
+ at note ContactGeometryTypeIds are unique and persistent within any execution of
+a program that uses them, but they are assigned at run time, possibly by
+multiple asynchronous threads, and are likely to be different in different
+programs and in separate runs of the same program.
+
+The result of a ContactTracker when applied to a pair of contact
+surfaces, is either a determination that the surfaces are not in contact,
+or a Contact object describing their contact interaction. There are different
+types of these Contact objects (for example, PointContact, LineContact,
+MeshContact) and the same algorithm may result in different kinds of Contact
+under different circumstances. At each evaluation, the subsystem passes in the
+previous Contact object, if any, that was associated with two ContactSurfaces,
+then receives an update from the algorithm. **/
+//==============================================================================
+// CONTACT TRACKER SUBSYSTEM
+//==============================================================================
+class SimTK_SIMBODY_EXPORT ContactTrackerSubsystem : public Subsystem {
+public:
+ContactTrackerSubsystem();
+explicit ContactTrackerSubsystem(MultibodySystem&);
+
+/** Get the number of surfaces being managed by this contact tracker subsystem.
+These are identified by ContactSurfaceIndex values from 0 to
+getNumSurfaces()-1. This is available after realizeTopology() and does not
+change subsequently. **/
+int getNumSurfaces() const;
+/** Get the MobilizedBody associated with a particular contact surface. **/
+const MobilizedBody& getMobilizedBody(ContactSurfaceIndex surfIx) const;
+/** Get the ContactSurface object (detailed geometry and material properties)
+that is associated with the given ContactSurfaceIndex. **/
+const ContactSurface& getContactSurface(ContactSurfaceIndex surfIx) const;
+/** Get the transform X_BS that gives the pose of the indicated contact
+surface with respect to the body frame of the body to which it is attached. **/
+const Transform& getContactSurfaceTransform(ContactSurfaceIndex surfIx) const;
+
+/** Register the contact tracking algorithm to use for a particular pair of
+ContactGeometry types, replacing the existing tracker if any. If the tracker
+takes a pair (id1,id2), we will use it both for that pair and for (id2,id1) by
+calling it with the arguments reversed. The subsystem takes over ownership of
+the supplied heap-allocated object. **/
+void adoptContactTracker(ContactTracker* tracker);
+
+/** Return true if this subsystem has a contact tracker registered that can
+deal with ineractions between surfaces using the indicated pair of geometry
+types, in either order. **/
+bool hasContactTracker(ContactGeometryTypeId surface1,
+ ContactGeometryTypeId surface2) const;
+
+/** Return the contact tracker to be used for an interaction between the
+indicated types of contact geometry. If the tracker requires the geometry
+types to be in reverse order from the (surface1,surface2) order given here,
+then the return argument \a reverseOrder will be set true, otherwise it will
+be false. If no tracker was registered, this will be the default tracker. **/
+const ContactTracker& getContactTracker(ContactGeometryTypeId surface1,
+ ContactGeometryTypeId surface2,
+ bool& reverseOrder) const;
+
+/** Obtain the value of the ContactSnapshot state variable representing the
+most recently known set of Contacts for this system. **/
+const ContactSnapshot& getPreviousActiveContacts(const State& state) const;
+
+/** Obtain the value of the ContactSnapshot state variable representing the
+most recently predicted set of \e impending Contacts for this system. **/
+const ContactSnapshot& getPreviousPredictedContacts(const State& state) const;
+
+/** Get the calculated value of the ContactSnapshot cache entry representing the
+current set of Contacts for this system, as determined by the various Tracker
+algorithms registered with this subsystem. You can call this at Position
+stage or later; computation of contact status will be initiated if needed.
+Only the past contact status and current positions are used. This
+cache entry value is precisely what will become the "previous active
+contacts" state variable at the beginning of the next time step. An error
+will be thrown if we have to calculate the contacts here but fail to do so;
+if you don't want to deal with the possibility that an error might occur here,
+you should realize contacts explicitly first.
+ at see realizeActiveContacts() **/
+const ContactSnapshot& getActiveContacts(const State& state) const;
+
+/** Get an additional set of predicted Contacts that can be anticipated from
+current velocity and acceleration information. You can call this at
+Acceleration stage; computation will be initiated if needed. This
+cache entry value is precisely what will become the "previous impending
+contacts" state variable at the beginning of the next time step. An error
+will be thrown if we have to calculate the contacts here but fail to do so;
+to avoid that you should realize them explicitly first.
+ at see realizePredictedContacts() **/
+const ContactSnapshot& getPredictedContacts(const State& state) const;
+
+/** Calculate the current ActiveContacts set at Position stage or later if
+it hasn't already been done and return true if successful. If we can't
+unambiguously determine the contact status, we'll return false and give the
+caller a hint as to the latest time at which we think we could have succeeded.
+If \a lastTry is true, then we throw an error on failure rather than
+returning false. **/
+bool realizeActiveContacts(const State& state,
+ bool lastTry,
+ Real& stepAdvice) const;
+
+/** Calculate the set of anticipated Contacts set at Acceleration
+stage if not already calculated and return true if successful. Otherwise,
+problems are handled as for realizeActiveContacts(). **/
+bool realizePredictedContacts(const State& state,
+ bool lastTry,
+ Real& stepAdvice) const;
+
+SimTK_PIMPL_DOWNCAST(ContactTrackerSubsystem, Subsystem);
+
+//--------------------------------------------------------------------------
+private:
+class ContactTrackerSubsystemImpl& updImpl();
+const ContactTrackerSubsystemImpl& getImpl() const;
+};
+
+
+
+//==============================================================================
+// CONTACT SNAPSHOT
+//==============================================================================
+/** Objects of this class represent collections of surface-pair interactions
+that are being tracked at a particular instant during a simulation. These are
+suitable for use as state variables for remembering past contact status and
+as calculated cache entries containing the current contact status. Each
+tracked surface pair has an integer ContactId that is persistent for as long
+as a particular interaction is being tracked. We maintain a map providing
+very fast access to individual Contact entries by ContactId. There is also
+a map from ContactSurfaceIndex pairs to ContactId that can be used to see
+whether we are already tracking a Contact between those surfaces; there can
+be at most one Contact between a given surface pair at any given moment. **/
+class SimTK_SIMBODY_EXPORT ContactSnapshot {
+//TODO: replace with fast hash tables
+typedef std::map<ContactId,int> ContactMap;
+// Note: we always order the key so that the first surface index is less than
+// the second (they can't be equal!).
+typedef std::map<std::pair<ContactSurfaceIndex,ContactSurfaceIndex>,
+ ContactId> SurfaceMap;
+public:
+/** Default constructor sets timestamp to NaN. **/
+ContactSnapshot() : m_time(NaN) {}
+
+/** Restore to default-constructed condition. **/
+void clear() {
+ m_time = NaN;
+ m_contacts.clear();
+ m_id2contact.clear();
+ m_surfPair2id.clear();
+}
+
+/** Set the time at which this snapshot was taken. **/
+void setTimestamp(Real time) {m_time=time;}
+/** At what simulation time was this contact snapshot taken? **/
+Real getTimestamp() const {return m_time;}
+
+/** Add this Contact object to this snapshot; this is a shallow,
+reference-counted copy so the Contact object is not duplicated. The Contact
+is assigned a slot in the array of Contact objects that can be used for
+very fast access; however, that index may change if other Contact objects
+are removed from the snapshot. **/
+void adoptContact(Contact& contact) {
+ const ContactId id = contact.getContactId();
+ ContactSurfaceIndex surf1(contact.getSurface1());
+ ContactSurfaceIndex surf2(contact.getSurface2());
+ assert(id.isValid() && surf1.isValid() && surf2.isValid());
+
+ // Surface pair must be ordered (low,high) for the map.
+ if (surf1 > surf2) std::swap(surf1,surf2);
+
+ assert(!hasContact(id));
+ assert(!hasContact(surf1,surf2));
+
+ const int indx = m_contacts.size();
+ m_contacts.push_back(contact); // shallow copy
+ m_id2contact[id] = indx;
+ m_surfPair2id[std::make_pair(surf1,surf2)] = id;
+}
+
+/** Does this snapshot contain a Contact object with the given ContactId? **/
+bool hasContact(ContactId id) const
+{ return m_id2contact.find(id) != m_id2contact.end(); }
+/** Does this snapshot contain a Contact object for the given surface pair
+(in either order)? **/
+bool hasContact(ContactSurfaceIndex surf1, ContactSurfaceIndex surf2) const
+{ if (surf1 > surf2) std::swap(surf1,surf2);
+ return m_surfPair2id.find(std::make_pair(surf1,surf2))
+ != m_surfPair2id.end(); }
+
+/** Find out how many Contacts are in this snapshot. **/
+int getNumContacts() const {return m_contacts.size();}
+/** Get a reference to the n'th Contact in this snapshot; note that the
+position of a given Contact in this array is not guaranteed to remain
+unchanged when Contacts are removed from this snapshot. When in doubt,
+look up the contact by ContactId instead. @see getContactById() **/
+const Contact& getContact(int n) const {return m_contacts[n];}
+/** If this snapshot contains a contact with the given id, return a reference
+to it; otherwise, return a reference to an empty contact handle (you can check
+with isEmpty()). **/
+const Contact& getContactById(ContactId id) const
+{ static Contact empty;
+ ContactMap::const_iterator p = m_id2contact.find(id);
+ return p==m_id2contact.end() ? empty : m_contacts[p->second]; }
+/** If this snapshot contains a contact for the given pair of contact surfaces
+(order doesn't matter), return its ContactId; otherwise, return an invalid
+ContactId (you can check with isValid()). **/
+ContactId getContactIdForSurfacePair(ContactSurfaceIndex surf1,
+ ContactSurfaceIndex surf2) const
+{ if (surf1 > surf2) std::swap(surf1,surf2);
+ SurfaceMap::const_iterator p =
+ m_surfPair2id.find(std::make_pair(surf1,surf2));
+ return p==m_surfPair2id.end() ? ContactId() : p->second; }
+
+//--------------------------------------------------------------------------
+ private:
+
+// Remove a Contact occupying a particular slot in the Contact array. This
+// will result in another Contact object being moved to occupy the now-empty
+// slot to keep the array compact.
+void removeContact(int n) {
+ assert(0 <= n && n < m_contacts.size());
+ if (n+1 == m_contacts.size()) {
+ m_contacts.pop_back(); // this was the last one
+ return;
+ }
+ // Move the last one to replace this one and update the map.
+ m_contacts[n] = m_contacts.back(); // shallow copy
+ m_contacts.pop_back(); // destruct
+ m_id2contact[m_contacts[n].getContactId()] = n;
+}
+
+
+Real m_time; // when this snapshot was taken
+Array_<Contact,int> m_contacts; // all the contact pairs
+ContactMap m_id2contact; // the contact pairs by contactId
+SurfaceMap m_surfPair2id; // surfacepair -> contactId map
+};
+
+// for debugging
+inline std::ostream& operator<<(std::ostream& o, const ContactSnapshot& cs) {
+ o << "Contact snapshot: time=" << cs.getTimestamp()
+ << " numContacts=" << cs.getNumContacts() << std::endl;
+ return o;
+}
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_CONTACT_TRACKER_SUBSYSTEM_H_
diff --git a/Simbody/include/simbody/internal/DecorationSubsystem.h b/Simbody/include/simbody/internal/DecorationSubsystem.h
new file mode 100644
index 0000000..3912ef9
--- /dev/null
+++ b/Simbody/include/simbody/internal/DecorationSubsystem.h
@@ -0,0 +1,83 @@
+#ifndef SimTK_SIMBODY_DECORATION_SUBSYSTEM_H_
+#define SimTK_SIMBODY_DECORATION_SUBSYSTEM_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) 2007-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * Define the public interface to DecorationSubsystem, a "do nothing"
+ * subsystem providing a place for a MultibodySystem Modeler to toss in some
+ * visuals which may optionally be displayed by an application which uses
+ * that System.
+ */
+
+#include "SimTKcommon.h"
+#include "simbody/internal/common.h"
+
+#include <cassert>
+
+namespace SimTK {
+
+class DecorationGenerator;
+class DecorativeGeometry;
+class DecorativeLine;
+class MultibodySystem;
+
+/**
+ * This is the client-side handle class encapsulating the hidden implementation
+ * of the DecorationSubsystem. Any Subsystem can generate decorative
+ * geometry, so the methods for extracting the geometry are at the Subsystem
+ * level and thus inherited here. So the only significant methods here are
+ * those for adding decorative geometry to the System beyond that which is
+ * generated by the other subsystems.
+ */
+class SimTK_SIMBODY_EXPORT DecorationSubsystem : public Subsystem {
+public:
+ DecorationSubsystem();
+ explicit DecorationSubsystem(MultibodySystem&);
+
+ void addBodyFixedDecoration(MobilizedBodyIndex bodyNum,
+ const Transform& X_GD,
+ const DecorativeGeometry&);
+
+ void addRubberBandLine(MobilizedBodyIndex b1, const Vec3& station1,
+ MobilizedBodyIndex b2, const Vec3& station2,
+ const DecorativeLine&);
+ /**
+ * Add a DecorationGenerator that will be invoked to add dynamically generated geometry
+ * to the scene. The DecorationSubsystem assumes ownership of the object passed to this method,
+ * and will delete it when the subsystem is deleted.
+ *
+ * @param stage the Stage the generator should be invoked at
+ * @param generator the DecorationGenerator to add
+ */
+ void addDecorationGenerator(Stage stage, DecorationGenerator* generator);
+
+ SimTK_PIMPL_DOWNCAST(DecorationSubsystem, Subsystem);
+ class DecorationSubsystemGuts& updGuts();
+ const DecorationSubsystemGuts& getGuts() const;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_DECORATION_SUBSYSTEM_H_
diff --git a/Simbody/include/simbody/internal/ElasticFoundationForce.h b/Simbody/include/simbody/internal/ElasticFoundationForce.h
new file mode 100644
index 0000000..54a390e
--- /dev/null
+++ b/Simbody/include/simbody/internal/ElasticFoundationForce.h
@@ -0,0 +1,133 @@
+#ifndef SimTK_SIMBODY_ELASTIC_FOUNDATION_FORCE_H_
+#define SimTK_SIMBODY_ELASTIC_FOUNDATION_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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 GeneralContactSubsystem;
+class ElasticFoundationForceImpl;
+
+/**
+ * This class implements an elastic foundation or "bed of springs" contact model. It places
+ * springs over the surface of a triangle mesh, each of which independently interacts with
+ * objects in contact with the mesh. The interaction includes components for the normal
+ * restoring force, dissipation in the material, and surface friction. This force
+ * is only applied to contacts involving a triangle mesh, though the object colliding with
+ * the mesh may be of any type. Contacts which do not involve a triangle mesh are ignored.
+ *
+ * This class relies on a GeneralContactSubsystem to identify contacts, then applies
+ * forces to all contacts in a single contact set. To use it, do the following:
+ *
+ * <ol>
+ * <li>Add a GeneralContactSubsystem and GeneralForceSubsystem to a MultibodySystem.</li>
+ * <li>Create a contact set within the GeneralContactSubsystem, and add ContactGeometry objects to it.</li>
+ * <li>Add an ElasticFoundationForce to the GeneralForceSubsystem, and call setBodyParameters() on it
+ * once for each TriangleMesh in the contact set.</li>
+ * </ol>
+ *
+ * A spring is placed at the center of each face of each triangle mesh. When a mesh collides with
+ * any other object, the other object is considered to be rigid and the displacement of each spring is
+ * calculated independently. A spring is considered to be displaced if its base location is inside
+ * the other object, and the contact point is taken to be the nearest point on that object's surface.
+ * When two meshes collide, the springs on each mesh are treated independently: each mesh is assumed
+ * to be rigid for purposes of calculating the force exerted by the other mesh's springs.
+ *
+ * <h1>Normal Force Components</h1>
+ *
+ * The force exerted by each spring along its displacement direction is given by
+ *
+ * f = k*a*x*(1+c*v)
+ *
+ * where k is the spring stiffness, a is the area of the face the spring belongs to, x is the displacement
+ * distance, c is the spring's dissipation coefficient, and v=dx/dt. If the springs are assumed
+ * to represent a uniform layer of elastic material over a rigid substrate, the stiffness is given by
+ *
+ * k = (1-p)*E/((1+p)(1-2p)*h)
+ *
+ * where E is the Young's modulus of the elastic layer, p is its Poisson's ratio, and h is its thickness.
+ *
+ * <h1>Friction Force</h1>
+ *
+ * The friction force exerted by each spring is based on a model by Michael Hollars:
+ *
+ * f = fn*[min(vs/vt,1)*(ud+2(us-ud)/(1+(vs/vt)^2))+uv*vs]
+ *
+ * where fn is the normal force at the contact point, vs is the slip (tangential)
+ * velocity of the two bodies at the contact point, vt is a transition velocity
+ * (see below), and us, ud, and uv are the coefficients of static, dynamic, and
+ * viscous friction respectively.
+ *
+ * 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. The default value of vt is 0.01.
+ */
+class SimTK_SIMBODY_EXPORT ElasticFoundationForce : public Force {
+public:
+ /**
+ * Create an elastic foundation contact model.
+ *
+ * @param forces the subsystem which will own this ElasticFoundationForce element
+ * @param contacts the subsystem to which this contact model should be applied
+ * @param contactSet the index of the contact set to which this contact model will be applied
+ */
+ ElasticFoundationForce(GeneralForceSubsystem& forces, GeneralContactSubsystem& contacts, ContactSetIndex contactSet);
+ /**
+ * Set the material parameters for a surface in the contact set, which
+ * must be a ContactGeometry::TriangleMesh.
+ *
+ * @param surfIndex the index of the surface within the contact set
+ * @param stiffness the stiffness constant (k) for the body
+ * @param dissipation the dissipation coefficient (c) for the body
+ * @param staticFriction the coefficient of static friction (us) for the body
+ * @param dynamicFriction the coefficient of dynamic friction (ud) for the body
+ * @param viscousFriction the coefficient of viscous friction (uv) for the body
+ */
+ void setBodyParameters
+ (ContactSurfaceIndex surfIndex, Real stiffness, Real dissipation,
+ Real staticFriction, Real dynamicFriction, Real viscousFriction);
+ /**
+ * Get the transition velocity (vt) of the friction model.
+ */
+ Real getTransitionVelocity() const;
+ /**
+ * Set the transition velocity (vt) of the friction model.
+ */
+ void setTransitionVelocity(Real v);
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(ElasticFoundationForce, ElasticFoundationForceImpl, Force);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_ELASTIC_FOUNDATION_FORCE_H_
diff --git a/Simbody/include/simbody/internal/Force.h b/Simbody/include/simbody/internal/Force.h
new file mode 100644
index 0000000..ac06428
--- /dev/null
+++ b/Simbody/include/simbody/internal/Force.h
@@ -0,0 +1,392 @@
+#ifndef SimTK_SIMBODY_FORCE_H_
+#define SimTK_SIMBODY_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-13 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/GeneralForceSubsystem.h"
+
+namespace SimTK {
+
+class SimbodyMatterSubsystem;
+class GeneralForceSubsystem;
+class MobilizedBody;
+class Force;
+class ForceImpl;
+
+// We only want the template instantiation to occur once. This symbol is defined
+// in the SimTK core compilation unit that defines the Force class but should
+// not be defined any other time.
+#ifndef SimTK_SIMBODY_DEFINING_FORCE
+ extern template class PIMPLHandle<Force, ForceImpl, true>;
+#endif
+
+/** This is the base class from which all Force element handle classes derive.
+A Force object applies forces to some or all of the bodies, particles, and
+mobilities in a System. There are subclasses for various standard types of
+forces, or you can create your own forces by deriving from Force::Custom. **/
+class SimTK_SIMBODY_EXPORT Force : public PIMPLHandle<Force, ForceImpl, true> {
+public:
+ /**@name Enabling and disabling
+ These methods determine whether this force element is active in a given
+ State. When disabled, the Force element is completely ignored and will
+ not be updated during realization. Normally force elements are enabled
+ when defined unless explicitly disabled; you can reverse that using the
+ setDisabledByDefault() method below. **/
+ /*@{*/
+ /** Disable this force element, effectively removing it from the System
+ for computational purposes (it is still using its ForceIndex, however).
+ This is an Instance-stage change. **/
+ void disable(State&) const;
+ /** Enable this force element if it was previously disabled. This is an
+ Instance-stage change. Nothing happens if the force element was already
+ enabled. **/
+ void enable(State&) const;
+ /** Test whether this force element is currently disabled in the supplied
+ State. If it is disabled you cannot depend on any computations it
+ normally performs being available. **/
+ bool isDisabled(const State&) const;
+ /** Normally force elements are enabled when defined and can be disabled
+ later. If you want to define this force element but have it be off by
+ default, use this method. Note that this is a Topology-stage (construction)
+ change; you will have to call realizeTopology() before using the containing
+ System after a change to this setting has been made. **/
+ void setDisabledByDefault(bool shouldBeDisabled);
+ /** Test whether this force element is disabled by default in which case it
+ must be explicitly enabled before it will take effect.
+ @see enable() **/
+ bool isDisabledByDefault() const;
+ /*@}*/
+
+ /**@name Advanced methods
+ Don't use these unless you're sure you know what you're doing. They aren't
+ normally necessary but can be handy sometimes, especially when debugging
+ newly-developed force elements. **/
+ /*@{*/
+ /** Calculate the force that would be applied by this force element if
+ the given \a state were realized to Dynamics stage. This sizes the given
+ arrays if necessary, zeroes them, and then calls the force element's
+ calcForce() method which adds its force contributions if any to the
+ appropriate array elements for bodies, particles, and mobilities. Note that
+ in general we have no idea what elements of the system are affected by a
+ force element, and in fact that can change based on state and time (consider
+ contact forces, for example). A disabled force element will return all
+ zeroes without invoking calcForce(), since that method may depend on
+ earlier computations which may not have been performed in that case.
+ @param[in] state
+ The State containing information to be used by the force element to
+ calculate the current force. This must have already been realized to
+ a high enough stage for the force element to get what it needs; if you
+ don't know then realize it to Stage::Velocity.
+ @param[out] bodyForces
+ This is a Vector of spatial forces, one per mobilized body in the
+ matter subsystem associated with this force element. This Vector is
+ indexed by MobilizedBodyIndex so it has a 0th entry corresponding
+ to Ground. A spatial force contains two Vec3's; index with [0] to get
+ the moment vector, with [1] to get the force vector. This argument is
+ resized if necessary to match the number of mobilized bodies and any
+ unused entry will be set to zero on return.
+ @param[out] particleForces
+ This is a Vector of force vectors, one per particle in the
+ matter subsystem associated with this force element. This vector is
+ indexed by ParticleIndex; the 0th entry is the 1st particle, not Ground.
+ This argument is resized if necessary to match the number of particles
+ and any unused entry will be set to zero on return. (As of March 2010
+ Simbody treats particles as mobilized bodies so this is unused.)
+ @param[out] mobilityForces
+ This is a Vector of scalar generalized forces, one per mobility in
+ the matter subsystem associated with this force element. This is the
+ same as the number of generalized speeds u that collectively represent
+ all the mobilities of the mobilizers. To determine the per-mobilizer
+ correspondence, you must call methods of MobilizedBody; there is no
+ hint here.
+ @note This method must zero out the passed in arrays, and in most cases
+ almost all returned entries will be zero, so this is \e not the most
+ efficent way to calculate forces; use it sparingly. **/
+ void calcForceContribution(const State& state,
+ Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces,
+ Vector& mobilityForces) const;
+ /** Calculate the potential energy contribution that is made by this
+ force element at the given \a state. This calls the force element's
+ calcPotentialEnergy() method. A disabled force element will return zero
+ without invoking calcPotentialEnergy().
+ @param[in] state
+ The State containing information to be used by the force element to
+ calculate the current potential energy. This must have already been
+ realized to a high enough stage for the force element to get what it
+ needs; if you don't know then realize it to Stage::Position.
+ @return The potential energy contribution of this force element at this
+ \a state value. **/
+ Real calcPotentialEnergyContribution(const State& state) const;
+ /*@}*/
+
+ /**@name Bookkeeping
+ These methods are not normally needed. They provide bookkeeping
+ information such as access to the parent force subsystem and the force
+ index assigned to this force element. **/
+ /*@{*/
+ /** Default constructor for Force handle base class does nothing. **/
+ Force() {}
+ /** Implicit conversion to ForceIndex when needed. This will throw an
+ exception if the force element has not yet been adopted by a force
+ subsystem. **/
+ operator ForceIndex() const {return getForceIndex();}
+ /** Get the GeneralForceSubsystem of which this Force is an element.
+ This will throw an exception if the force element has not yet been
+ adopted by a force subsystem. **/
+ const GeneralForceSubsystem& getForceSubsystem() const;
+ /** Get the index of this force element within its parent force subsystem.
+ The returned index will be invalid if the force element has not yet been
+ adopted by any subsystem (test with the index.isValid() method). **/
+ ForceIndex getForceIndex() const;
+ /*@}*/
+
+ class TwoPointLinearSpring;
+ class TwoPointLinearDamper;
+ class TwoPointConstantForce;
+ class MobilityLinearSpring;
+ class MobilityLinearDamper;
+ class MobilityConstantForce;
+ class MobilityLinearStop;
+ class MobilityDiscreteForce;
+ class DiscreteForces;
+ class LinearBushing;
+ class ConstantForce;
+ class ConstantTorque;
+ class GlobalDamper;
+ class Thermostat;
+ class UniformGravity;
+ class Gravity;
+ class Custom;
+
+ class TwoPointLinearSpringImpl;
+ class TwoPointLinearDamperImpl;
+ class TwoPointConstantForceImpl;
+ class MobilityLinearSpringImpl;
+ class MobilityLinearDamperImpl;
+ class MobilityConstantForceImpl;
+ class MobilityLinearStopImpl;
+ class MobilityDiscreteForceImpl;
+ class DiscreteForcesImpl;
+ class LinearBushingImpl;
+ class ConstantForceImpl;
+ class ConstantTorqueImpl;
+ class GlobalDamperImpl;
+ class ThermostatImpl;
+ class UniformGravityImpl;
+ class GravityImpl;
+ class CustomImpl;
+
+protected:
+ /** Use this in a derived Force handle class constructor to supply the
+ concrete implementation object to be stored in the handle base. **/
+ explicit Force(ForceImpl* r) : HandleBase(r) { }
+};
+
+
+/**
+ * A linear spring between two points, specified as a station on
+ * each of two bodies. The stiffness k and
+ * unstretched length x0 are provided. Then if d is the unit vector
+ * from point1 to point2, and x the current separation, we have
+ * f = k(x-x0) and we apply a force f*d to point1 and -f*d to point2.
+ * This contributes to potential energy: pe = 1/2 k (x-x0)^2.
+ * It is an error if the two points become coincident, since we
+ * are unable to determine a direction for the force in that case.
+ */
+
+class SimTK_SIMBODY_EXPORT Force::TwoPointLinearSpring : public Force {
+public:
+ /**
+ * Create a TwoPointLinearSpring.
+ *
+ * @param forces the subsystem to which this force should be added
+ * @param body1 the first body to which the force should be applied
+ * @param station1 the location on the first body at which the force should be applied
+ * @param body2 the second body to which the force should be applied
+ * @param station2 the location on the second body at which the force should be applied
+ * @param k the spring constant
+ * @param x0 the distance at which the force is 0
+ */
+ TwoPointLinearSpring(GeneralForceSubsystem& forces, const MobilizedBody& body1, const Vec3& station1, const MobilizedBody& body2, const Vec3& station2, Real k, Real x0);
+
+ /** Default constructor creates an empty handle. **/
+ TwoPointLinearSpring() {}
+
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(TwoPointLinearSpring, TwoPointLinearSpringImpl, Force);
+};
+
+/**
+ * A force which resists changes in the distance between
+ * two points, acting along the line between those points. The points
+ * are specified as a station on each of two bodies. A damping constant
+ * c >= 0 is given. If the relative (scalar) velocity between the points
+ * is v, then we apply a force of magnitude f=c*|v| to each point in
+ * a direction which opposes their separation. This is not a potential
+ * force and thus does not contribute to the potential energy calculation.
+ * It is an error if the two points become coincident, since we
+ * are unable to determine a direction for the force in that case.
+ */
+
+class SimTK_SIMBODY_EXPORT Force::TwoPointLinearDamper: public Force {
+public:
+ /**
+ * Create a TwoPointLinearDamper.
+ *
+ * @param forces the subsystem to which this force should be added
+ * @param body1 the first body to which the force should be applied
+ * @param station1 the location on the first body at which the force should be applied
+ * @param body2 the second body to which the force should be applied
+ * @param station2 the location on the second body at which the force should be applied
+ * @param damping the damping constant
+ */
+ TwoPointLinearDamper(GeneralForceSubsystem& forces, const MobilizedBody& body1, const Vec3& station1, const MobilizedBody& body2, const Vec3& station2, Real damping);
+
+ /** Default constructor creates an empty handle. **/
+ TwoPointLinearDamper() {}
+
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(TwoPointLinearDamper, TwoPointLinearDamperImpl, Force);
+};
+
+/**
+ * A constant force f (a signed scalar) which acts along the line between
+ * two points, specified as a station on each of two bodies. A positive
+ * force acts to separate the points; negative pulls them together. The
+ * force magnitude is independent of the separation between the points.
+ * This force does not contribute to the potential energy, so adding it to
+ * a system will cause energy not to be conserved.
+ * It is an error if the two points become coincident, since we
+ * are unable to determine a direction for the force in that case.
+ */
+
+class SimTK_SIMBODY_EXPORT Force::TwoPointConstantForce: public Force {
+public:
+ /**
+ * Create a TwoPointConstantForce.
+ *
+ * @param forces the subsystem to which this force should be added
+ * @param body1 the first body to which the force should be applied
+ * @param station1 the location on the first body at which the force should be applied
+ * @param body2 the second body to which the force should be applied
+ * @param station2 the location on the second body at which the force should be applied
+ * @param force the magnitude of the force to apply
+ */
+ TwoPointConstantForce(GeneralForceSubsystem& forces, const MobilizedBody& body1, const Vec3& station1, const MobilizedBody& body2, const Vec3& station2, Real force);
+
+ /** Default constructor creates an empty handle. **/
+ TwoPointConstantForce() {}
+
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(TwoPointConstantForce, TwoPointConstantForceImpl, Force);
+};
+
+
+/**
+ * A constant force applied to a body station. The force is a
+ * vector fixed forever in the Ground frame.
+ * This force does not contribute to the potential energy, so adding it to
+ * a system will cause energy not to be conserved.
+ */
+
+class SimTK_SIMBODY_EXPORT Force::ConstantForce: public Force {
+public:
+ ConstantForce(GeneralForceSubsystem& forces, const MobilizedBody& body, const Vec3& station, const Vec3& force);
+
+ /** Default constructor creates an empty handle. **/
+ ConstantForce() {}
+
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(ConstantForce, ConstantForceImpl, Force);
+};
+
+/**
+ * A constant torque to a body. The torque is a
+ * vector fixed forever in the Ground frame.
+ * This force does not contribute to the potential energy, so adding it to
+ * a system will cause energy not to be conserved.
+ */
+
+class SimTK_SIMBODY_EXPORT Force::ConstantTorque: public Force {
+public:
+ ConstantTorque(GeneralForceSubsystem& forces, const MobilizedBody& body, const Vec3& torque);
+
+ /** Default constructor creates an empty handle. **/
+ ConstantTorque() {}
+
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(ConstantTorque, ConstantTorqueImpl, Force);
+};
+
+/**
+ * A general energy "drain" on the system. This is
+ * done by effectively adding a damper to every generalized
+ * speed (mobility) in the system. Each generalized speed
+ * u_i feels a force -dampingFactor*u_i.
+ * This usually is not physically meaningful, but it can be useful in some
+ * circumstances just to drain energy out of the model when
+ * the specific energy-draining mechanism is not important.
+ * You can have more than one of these in which case the
+ * dampingFactors are added. No individual dampingFactor is
+ * allowed to be negative. This is not a potential force and
+ * hence does not contribute to potential energy.
+ */
+
+class SimTK_SIMBODY_EXPORT Force::GlobalDamper : public Force {
+public:
+ GlobalDamper(GeneralForceSubsystem& forces, const SimbodyMatterSubsystem& matter, Real damping);
+
+ /** Default constructor creates an empty handle. **/
+ GlobalDamper() {}
+
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(GlobalDamper, GlobalDamperImpl, Force);
+};
+
+/**
+ * A uniform gravitational force applied to every body in the system.\ See
+ * Force::Gravity for a more flexible option.
+ *
+ * The %UniformGravity force is specified by a vector in the Ground frame. You
+ * can optionally specify a height at which the gravitational potential energy
+ * is zero.
+ * @see SimTK::Force::Gravity
+ */
+
+class SimTK_SIMBODY_EXPORT Force::UniformGravity : public Force {
+public:
+ UniformGravity(GeneralForceSubsystem& forces,
+ const SimbodyMatterSubsystem& matter,
+ const Vec3& g, Real zeroHeight=0);
+
+ /** Default constructor creates an empty handle. **/
+ UniformGravity() {}
+
+ Vec3 getGravity() const;
+ void setGravity(const Vec3& g);
+ Real getZeroHeight() const;
+ void setZeroHeight(Real height);
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(UniformGravity, UniformGravityImpl, Force);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_FORCE_H_
diff --git a/Simbody/include/simbody/internal/ForceSubsystem.h b/Simbody/include/simbody/internal/ForceSubsystem.h
new file mode 100644
index 0000000..b76d24b
--- /dev/null
+++ b/Simbody/include/simbody/internal/ForceSubsystem.h
@@ -0,0 +1,53 @@
+#ifndef SimTK_SIMBODY_FORCE_SUBSYSTEM_H_
+#define SimTK_SIMBODY_FORCE_SUBSYSTEM_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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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"
+
+namespace SimTK {
+
+/**
+ * This is logically an abstract class, more specialized than "Subsystem"
+ * but not yet concrete.
+ */
+class SimTK_SIMBODY_EXPORT ForceSubsystem : public Subsystem {
+public:
+ /// forward declaration of extendable internals
+ class Guts;
+
+ ForceSubsystem() : Subsystem() { }
+
+ SimTK_PIMPL_DOWNCAST(ForceSubsystem, Subsystem);
+ Guts& updRep();
+ const Guts& getRep() const;
+
+};
+
+typedef ForceSubsystem::Guts ForceSubsystemRep;
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_FORCE_SUBSYSTEM_H_
diff --git a/Simbody/include/simbody/internal/ForceSubsystemGuts.h b/Simbody/include/simbody/internal/ForceSubsystemGuts.h
new file mode 100644
index 0000000..5f1c7f9
--- /dev/null
+++ b/Simbody/include/simbody/internal/ForceSubsystemGuts.h
@@ -0,0 +1,68 @@
+#ifndef SimTK_SIMBODY_FORCE_SUBSYSTEM_GUTS_H
+#define SimTK_SIMBODY_FORCE_SUBSYSTEM_GUTS_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * Define the extendable library-side implementation of the ForceSubsystem.
+ */
+
+#include "SimTKcommon.h"
+
+#include "simbody/internal/common.h"
+#include "simbody/internal/MultibodySystem.h"
+
+namespace SimTK {
+
+/// Public declaration of internals for ForceSubsystem extension
+class ForceSubsystem::Guts : public Subsystem::Guts {
+public:
+ Guts(const String& name, const String& version)
+ : Subsystem::Guts(name,version)
+ {
+ }
+
+ // Make sure the virtual destructor in Subsystem::Guts remains
+ // virtual in this intermediate class.
+ virtual ~Guts() { }
+
+ // All the other Subsystem::Guts virtuals remain unresolved.
+
+ // Return the MultibodySystem which owns this ForceSubsystem.
+ const MultibodySystem& getMultibodySystem() const {
+ return MultibodySystem::downcast(getSystem());
+ }
+
+ /// Get this subsystem's contribution to the potential energy. The state must
+ /// be at Dynamics stage or later.
+ virtual Real calcPotentialEnergy(const State& state) const = 0;
+
+ SimTK_DOWNCAST(ForceSubsystem::Guts, Subsystem::Guts);
+};
+
+// typedef ForceSubsystem::Guts ForceSubsystemRep;
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_FORCE_SUBSYSTEM_GUTS_H
diff --git a/Simbody/include/simbody/internal/Force_BuiltIns.h b/Simbody/include/simbody/internal/Force_BuiltIns.h
new file mode 100644
index 0000000..a9de530
--- /dev/null
+++ b/Simbody/include/simbody/internal/Force_BuiltIns.h
@@ -0,0 +1,48 @@
+#ifndef SimTK_SIMBODY_FORCE_BUILTINS_H_
+#define SimTK_SIMBODY_FORCE_BUILTINS_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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Include the header files that define each of the built-in force
+subclasses of Force. These all depend on Force.h. Note that there are
+other force elements; these are just the ones that are internal classes
+with the Force class, so logically their definitions are part of the
+Force class definition. **/
+
+#include "simbody/internal/Force_Custom.h"
+#include "simbody/internal/Force_DiscreteForces.h"
+#include "simbody/internal/Force_Gravity.h"
+#include "simbody/internal/Force_LinearBushing.h"
+#include "simbody/internal/Force_MobilityConstantForce.h"
+#include "simbody/internal/Force_MobilityDiscreteForce.h"
+#include "simbody/internal/Force_MobilityLinearDamper.h"
+#include "simbody/internal/Force_MobilityLinearSpring.h"
+#include "simbody/internal/Force_MobilityLinearStop.h"
+#include "simbody/internal/Force_Thermostat.h"
+
+#endif // SimTK_SIMBODY_FORCE_BUILTINS_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/Force_Custom.h b/Simbody/include/simbody/internal/Force_Custom.h
new file mode 100644
index 0000000..235d15d
--- /dev/null
+++ b/Simbody/include/simbody/internal/Force_Custom.h
@@ -0,0 +1,274 @@
+#ifndef SimTK_SIMBODY_FORCE_CUSTOM_H_
+#define SimTK_SIMBODY_FORCE_CUSTOM_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-13 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Force.h"
+
+/** @file
+This contains the user-visible API ("handle" class) for the SimTK::Force
+subclass Force::Custom, and its related class Force::Custom::Implemnetation,
+and is logically part of Force.h. The file assumes that
+Force.h will have included all necessary declarations. **/
+
+namespace SimTK {
+
+/**
+ * This class is used to define new force elements. To use it, you will
+ * create a class that extends Force::Custom::Implementation. Optionally, you
+ * may also create a "handle" class extending Force::Custom that hides your
+ * implementation and provides a nicer API for users of your new force element.
+ * Here we'll use as an example a force "MySpring" that we'll presume you will
+ * declare in a header file "MySpring.h". The MySpring constructor will take
+ * a reference to a MobilizedBody \a mobod and a spring constant \a k, and
+ * connect the origin OB of body \a mobod to the Ground origin O by a spring of
+ * stiffness \a k. MySpring will apply a force of magnitude \a kx to OB,
+ * directed towards O, where x=|OB-O| is the current distance from OB to O.
+ * First we'll look at how MySpring would be used in a main program (whether by
+ * you or some future user of your force element), then how you would
+ * implement it.
+ *
+ * There are two possibilities:
+ * - you supply only an %Implementation object MySpringImpl (less work
+ * for you), or
+ * - you supply both the %Implementation object and a handle MySpring
+ * (nicer for the user).
+ *
+ * If you are planning only to use this force yourself, and perhaps just once
+ * for a single application, the %Implementation-only approach is probably
+ * adequate. However, if you plan to have others use your new force object, it
+ * is well worth the (minimal) extra effort to provide a handle class also to
+ * make your new force behave identically to the built-in forces that come
+ * with Simbody. In either case the %Implementation object is the same so you
+ * can add a handle later if you want. To reiterate: the handle class is
+ * completely optional; you \e must write an %Implementation class
+ * but a handle class is an aesthetic addition whose main purpose is to
+ * make a cleaner API for users of your force element.
+ *
+ * In the case where you write only the %Implementation class, a user will
+ * create an instance of that class and pass it to the generic Force::Custom
+ * constructor (this would be in main.cpp or some other piece of application
+ * code):
+ * @code
+ * // Using your custom force in a simulation application, no handle.
+ * #include "Simbody.h"
+ * #include "MySpring.h"
+ * using namespace SimTK;
+ * GeneralForceSubsystem forces;
+ * MobilizedBody mobod1 = MobilizedBody::Pin(...); // or whatever
+ * // ...
+ * Force::Custom spring1(forces, new MySpringImpl(mobod1,100.));
+ * @endcode
+ * If you also write a handle class, then the API seen by the user is the
+ * same as for built-in force objects:
+ * @code
+ * // Using your custom force in a simulation application, with handle.
+ * // Same as above except last line is now:
+ * MySpring spring1(forces, mobod1, 100.);
+ * @endcode
+ *
+ * <h2>Writing the %Implementation class</h2>
+ *
+ * You would put the following code in MySpring.h, although you can hide it
+ * elsewhere (such as in a separate .cpp file) if you provide a handle class:
+ * @code
+ * // Sample implementation of a custom force Implementation class.
+ * class MySpringImpl: public Force::Custom::Implementation {
+ * public:
+ * MySpringImpl(MobilizedBody mobod, Real k)
+ * : m_mobod(mobod), m_k(k) {}
+ *
+ * virtual void calcForce(const State& state,
+ * Vector_<SpatialVec>& bodyForcesInG,
+ * Vector_<Vec3>& particleForcesInG,
+ * Vector& mobilityForces) const
+ * {
+ * Vec3 bodyPointInB(0,0,0); // body origin (in the body frame)
+ * Vec3 pos = m_mobod.getBodyOriginLocation(state); // in Ground frame
+ * // apply force towards origin, proportional to distance x
+ * m_mobod.applyForceToBodyPoint(state, bodyPointInB, -m_k*pos,
+ * bodyForcesInG);
+ * }
+ *
+ * virtual Real calcPotentialEnergy(const State& state) const {
+ * Vec3 pos = m_mobod.getBodyOriginLocation(state); // in Ground
+ * Real x = pos.norm(); // distance from Ground origin
+ * return m_k*x*x/2; // potential energy in the spring
+ * }
+ * private:
+ * MobilizedBody m_mobod; // the body to which to apply the force
+ * Real m_k; // spring stiffness
+ * };
+ * @endcode
+ *
+ * To write the code exactly as above, the compiler has to be told to look in
+ * the %SimTK namespace for some of the symbols. There are a variety of ways
+ * to do that; see the discussion below for details.
+ *
+ * <h2>Writing the handle class</h2>
+ *
+ * Here is how to implement the handle class, assuming you've already
+ * written the %Implementation class MySpringImpl. You would put this
+ * code (at least the declarations) in MySpring.h, following the declaration
+ * of the MySpringImpl class:
+ * @code
+ * // Sample implementation of a handle class. Note: a handle class
+ * // may *not* have any data members or virtual methods.
+ * class MySpring : public Force::Custom {
+ * public:
+ * MySpring(GeneralForceSubsystem& forces, // note the "&"
+ * MobilizedBody mobod,
+ * Real k)
+ * : Force::Custom(forces, new MySpringImpl(mobod,k)) {}
+ * };
+ * @endcode
+ * As you can see, the handle class is very simple and just hides the creation
+ * of the implementation object. Since this removes any reference to the
+ * implementation object from the user's program, it also means you can hide
+ * the implementation details completely (perhaps in a separately compiled
+ * library), which has many advantages. You can add additional methods to the
+ * handle class to provide a clean API to users of your custom force; these
+ * methods will forward to the implementation object as necessary but will not
+ * expose any aspects of the implementation that are not needed by the user.
+ *
+ * @warning A handle class <em>must not</em> have any data members or virtual
+ * methods and will not work properly if it does. Instead, store any data you
+ * need in the %Implementation object.
+ *
+ * <h2>Getting access to symbols in the %SimTK namespace</h2>
+ *
+ * The examples above glossed over the naming of symbols in the SimTK namespace.
+ * To write the code exactly as above you would need to precede it with
+ * \c using statements to tell the compiler to look there to resolve symbols,
+ * for example:
+ * @code
+ * using namespace SimTK; // search the entire namespace for symbols, or
+ * using SimTK::Real; using SimTK::Vector_; // define just particular symbols
+ * @endcode
+ * Either of those approaches will work, but note that this will have the same
+ * effect on user programs that include MySpring.h as it does within the header
+ * file. That may be unwanted behavior for some users who might prefer not to have
+ * the namespace searched automatically, perhaps to avoid conflicts with their own
+ * symbols that have the same names. If you want to avoid introducing unwanted
+ * symbols into users' compilation units, then in the header file you should
+ * refer to each symbol explicitly by its full name; that is, prepend the
+ * namespace each time the symbol is used, for example:
+ * @code
+ * class MySpringImpl: public SimTK::Force::Custom::Implementation {
+ * void calcForce(const SimTK::State& state,
+ * SimTK::Vector_<SimTK::SpatialVec>& bodyForcesInG,
+ * SimTK::Vector_<SimTK::Vec3>& particleForcesInG,
+ * SimTK::Vector& mobilityForces) const ;
+ * };
+ * @endcode
+ * That is less convenient for you (and uglier) but avoids the possibility of
+ * unwanted side effects in user code so should be considered if you expect
+ * wide distribution of your new force element.
+ *
+ * Thanks to Nur Adila Faruk Senan (a.k.a. Adila Papaya) for help in clarifying
+ * this documentation.
+ */
+class SimTK_SIMBODY_EXPORT Force::Custom : public Force {
+public:
+ class Implementation;
+ /**
+ * Create a Custom force.
+ *
+ * @param forces the subsystem to which this force should be added
+ * @param implementation the object which implements the custom force. The Force::Custom takes over
+ * ownership of the implementation object, and deletes it when the Force itself
+ * is deleted.
+ */
+ Custom(GeneralForceSubsystem& forces, Implementation* implementation);
+
+ /** Default constructor creates an empty handle. **/
+ Custom() {}
+
+ /** @cond **/ // don't let Doxygen see this
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Custom, CustomImpl, Force);
+ /** @endcond **/
+protected:
+ const Implementation& getImplementation() const;
+ Implementation& updImplementation();
+};
+
+/**
+ * Every custom force requires implementation of a class that is derived from
+ * this abstract class.\ See Force::Custom for details.
+ */
+class SimTK_SIMBODY_EXPORT Force::Custom::Implementation {
+public:
+ virtual ~Implementation() { }
+ /**
+ * Calculate the force for a given state.
+ *
+ * @param state the State for which to calculate the force
+ * @param bodyForces spatial forces on MobilizedBodies are accumulated in this. To apply a force to a body,
+ * add it to the appropriate element of this vector.
+ * @param particleForces forces on particles are accumulated in this. Since particles are not yet implemented,
+ * this is ignored.
+ * @param mobilityForces forces on individual mobilities (elements of the state's u vector) are accumulated in this.
+ * To apply a force to a mobility, add it to the appropriate element of this vector.
+ */
+ virtual void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const = 0;
+ /**
+ * Calculate this force's contribution to the potential energy of the System.
+ *
+ * @param state the State for which to calculate the potential energy
+ */
+ virtual Real calcPotentialEnergy(const State& state) const = 0;
+ /**
+ * Get whether this force depends only on the position variables (q), not on the velocies (u) or auxiliary variables (z).
+ * The default implementation returns false. If the force depends only on positions, you should override this to return
+ * true. This allows force calculations to be optimized in some cases.
+ */
+ virtual bool dependsOnlyOnPositions() const {
+ return false;
+ }
+ /** The following methods may optionally be overridden to do specialized
+ realization for a Force. **/
+ //@{
+ virtual void realizeTopology(State& state) const {}
+ virtual void realizeModel(State& state) const {}
+ virtual void realizeInstance(const State& state) const {}
+ virtual void realizeTime(const State& state) const {}
+ virtual void realizePosition(const State& state) const {}
+ virtual void realizeVelocity(const State& state) const {}
+ virtual void realizeDynamics(const State& state) const {}
+ virtual void realizeAcceleration(const State& state) const {}
+ virtual void realizeReport(const State& state) const {}
+ //@}
+
+ /** Override this if you want to generate some geometry for the visualizer
+ to display. Be sure to \e append the geometry to the list. **/
+ virtual void calcDecorativeGeometryAndAppend
+ (const State& state, Stage stage,
+ Array_<DecorativeGeometry>& geometry) const {}
+
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_FORCE_CUSTOM_H_
diff --git a/Simbody/include/simbody/internal/Force_DiscreteForces.h b/Simbody/include/simbody/internal/Force_DiscreteForces.h
new file mode 100644
index 0000000..ea72ec2
--- /dev/null
+++ b/Simbody/include/simbody/internal/Force_DiscreteForces.h
@@ -0,0 +1,182 @@
+#ifndef SimTK_SIMBODY_FORCE_DISCRETE_FORCES_H_
+#define SimTK_SIMBODY_FORCE_DISCRETE_FORCES_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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Force.h"
+
+/** @file
+This contains the user-visible API ("handle" class) for the SimTK::Force
+subclass Force::DiscreteForces and is logically part of Force.h. The file assumes that
+Force.h will have included all necessary declarations. **/
+
+namespace SimTK {
+
+/** Arbitrary discrete body forces and mobility (generalized) forces.\ Useful
+for applying external forces or forces that are updated at discrete times due
+to the occurrence of events.
+ at see Force::MobilityDiscreteForce **/
+class SimTK_SIMBODY_EXPORT Force::DiscreteForces : public Force {
+public:
+ /** Create a %DiscreteForces force element.
+
+ @param forces Subsystem to which this force element should be added.
+ @param matter Subsystem containing the mobilized bodies to which these
+ forces will be applied.
+ **/
+ DiscreteForces(GeneralForceSubsystem& forces,
+ const SimbodyMatterSubsystem& matter);
+
+
+ /** Default constructor creates an empty handle. **/
+ DiscreteForces() {}
+
+ /** Set to zero all forces stored by this force element in the given
+ \a state, including both generalized forces and body spatial forces. **/
+ void clearAllForces(State& state) const {
+ clearAllMobilityForces(state);
+ clearAllBodyForces(state);
+ }
+
+ /** Set to zero all generalized forces stored by this force element in the
+ given \a state. **/
+ void clearAllMobilityForces(State& state) const;
+ /** Set to zero all body spatial forces (force and torques) stored by this
+ force element in the given \a state. **/
+ void clearAllBodyForces(State& state) const;
+
+ /** Change the value of a generalized force to be applied in the given
+ \a state. Set this to zero if you don't want it to do anything. You can
+ call this method any time after realizeTopology() since the force just needs
+ to be saved in the \a state.
+
+ @param state The state in which the generalized force is to be saved.
+ @param mobod The mobilizer to which the force should be applied.
+ @param whichU To which of the mobilizer's mobilities (degrees of
+ freedom) should this force be applied (first is 0)?
+ @param force The scalar value of the generalized force.
+
+ @see getOneMobilityForce() **/
+ void setOneMobilityForce(State& state, const MobilizedBody& mobod,
+ MobilizerUIndex whichU, Real force) const;
+
+ /** Change the value of the discrete spatial force (force and torque) to
+ be applied to a body in the given \a state. Set this to zero if you don't
+ want any force applied to the body. You can call this method any time after
+ realizeTopology() since the force just needs to be saved in the \a state.
+
+ @param state
+ The state in which the force is to be saved.
+ @param mobod
+ The mobilized body to which the force should be applied.
+ @param spatialForceInG
+ The (torque,force) pair as a SpatialVec. These are expressed in the
+ Ground frame and the force is applied to the body origin.
+
+ @see getOneBodyForce(), addForceToBodyPoint() **/
+ void setOneBodyForce(State& state, const MobilizedBody& mobod,
+ const SpatialVec& spatialForceInG) const;
+
+ /** Convenience method to \e add in a force applied at a point (station) on
+ a particular body into the forces currently set to be applied to that body
+ in this \a state. The station location is given in the body frame and the
+ force is given in the Ground frame. Note that the \a state must have been
+ realized to Stage::Position because we need to know the body orientation
+ in order to shift the applied force to the body origin.
+
+ Be sure to call setOneBodyForce() to initialize this force to zero before
+ you start accumulating point forces using this method.
+
+ @param state The state in which the force is saved. Must already have
+ been realized to at least Stage::Position.
+ @param mobod The mobilized body to which the force will be applied.
+ @param pointInB The body station at which the force will be applied,
+ measured and expressed in the body frame.
+ @param forceInG The force vector to be applied, expressed in Ground.
+
+ @see setOneBodyForce() **/
+ void addForceToBodyPoint(State& state, const MobilizedBody& mobod,
+ const Vec3& pointInB,
+ const Vec3& forceInG) const;
+
+ /** Set all the generalized forces at once. These will be applied until
+ they are changed.
+ @param state The state in which the generalized forces are saved.
+ @param f The set of n generalized forces. This Vector must be either
+ empty (in which case it is treated as though it were
+ all zero) or the same length as the number of generalized
+ speeds (state.getNU()).
+ @see clearAllMobilityForces() **/
+ void setAllMobilityForces(State& state, const Vector& f) const;
+
+ /** Set all the body spatial forces at once. These will be applied until
+ they are changed.
+ @param state The state in which the forces are saved.
+ @param bodyForcesInG
+ The set of nb spatial forces (torque,force) pairs, expressed in the
+ Ground frame, with the forces applied at each body frame origin. This
+ Vector must be either empty (in which case it is treated as though it
+ were all zero) or the same length as the number of mobilized bodies in
+ the system (and don't forget that Ground is mobilized body 0).
+ @see clearAllBodyForces() **/
+ void setAllBodyForces(State& state,
+ const Vector_<SpatialVec>& bodyForcesInG) const;
+
+ /** Return the value for a generalized force that is stored in the
+ given \a state. If no calls to setGeneralizedForce() have been made on this
+ \a state then zero will be returned.
+ @see setOneMobilityForce() **/
+ Real getOneMobilityForce(const State& state, const MobilizedBody& mobod,
+ MobilizerUIndex whichU) const;
+
+ /** Return the value for the discrete spatial force (force and torque) on a
+ particular body that is stored in the given \a state. The result is a force
+ applied at the body origin and expressed in the Ground frame.
+ @see setOneBodyForce() **/
+ SpatialVec getOneBodyForce(const State& state,
+ const MobilizedBody& mobod) const;
+
+ /** Get a reference to the internal Vector of all the generalized forces
+ currently set to be applied by this force element. This will be length zero
+ if no forces are being applied; otherwise, its length will be the number
+ of generalized speeds u in the given \a state.
+ The returned reference is a reference into the given \a state object. **/
+ const Vector& getAllMobilityForces(const State& state) const;
+ /** Get a reference to the internal Vector of all the body spatial forces
+ currently set to be applied by this force element. This will be length zero
+ if no body forces are being applied; otherwise, its length will be the
+ number of mobilized bodies in the containing MultibodySystem.
+ The returned reference is a reference into the given \a state object. **/
+ const Vector_<SpatialVec>& getAllBodyForces(const State& state) const;
+
+ /** @cond **/
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(DiscreteForces,
+ DiscreteForcesImpl, Force);
+ /** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_FORCE_DISCRETE_FORCES_H_
diff --git a/Simbody/include/simbody/internal/Force_Gravity.h b/Simbody/include/simbody/internal/Force_Gravity.h
new file mode 100644
index 0000000..7b9cace
--- /dev/null
+++ b/Simbody/include/simbody/internal/Force_Gravity.h
@@ -0,0 +1,554 @@
+#ifndef SimTK_SIMBODY_FORCE_GRAVITY_H_
+#define SimTK_SIMBODY_FORCE_GRAVITY_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) 2010-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Force.h"
+
+/** @file
+This contains the user-visible API ("handle" class) for the SimTK::Force
+subclass Force::Gravity and is logically part of Force.h. The file assumes that
+Force.h will have included all necessary declarations. **/
+
+namespace SimTK {
+
+/** This force element represents a uniform gravitational field applied to a
+set of bodies. This can be used instead of the more limited
+Force::UniformGravity class if you want more control.
+
+Force::Gravity is given a magnitude g in units of acceleration, and a "down"
+direction (unit vector) d, expressed in the Ground frame. For example, in SI
+units "standard gravity" g at the Earth's surface is defined to be exactly
+g=9.80665 m/s^2. If gravity acts in Ground's -Y direction, then its direction
+vector is d=[0,-1,0]; together this yields a gravity vector
+v=g*d=[0,-9.80665,0]. By default gravity will be applied to each MobilizedBody
+in the given SimbodyMatterSubsystem (except for Ground). You can specify that
+some bodies are excluded; those will not have a gravity force applied. You can
+include multiple Force::Gravity elements with different exclusion lists in the
+same System if you want different bodies to feel different effects.
+
+\par Force
+Each body B that has not been explicitly excluded will experience a force
+fb = mb*g*d, applied to its center of mass, where mb is the mass of body B.
+Although this is a pure force, note that when it is measured in the body frame
+B there will also be a moment unless the body frame origin Bo is located at the
+body's mass center. You can obtain the applied forces if you need them, for
+example for gravity compensation; see getBodyForces().
+
+\par Potential Energy
+Gravitational potential energy for a body B is mb*g*hb where hb is the height of
+body B's mass center over an arbitrary "zero" height hz (default is hz=0),
+measured along the "up" direction -d. If pb is the Ground frame vector giving
+the position of body B's mass center, its height over or under hz is
+hb=pb*(-d) - hz. Note that this is a signed quantity so the potential energy is
+also signed.
+
+ at author Michael Sherman
+**/
+class SimTK_SIMBODY_EXPORT Force::Gravity : public Force {
+public:
+
+
+//------------------------------------------------------------------------------
+/** @name Construction
+Methods in this section refer both to constructors, and to methods that can
+be used to set or change contruction (Topology-stage) parameters; these
+specify the values assigned by default to the corresponding state variables.
+Note:
+ - Changing one of these default parameters invalidates the containing
+ System's topology, meaning that realizeTopology() will have to be called
+ and a new State obtained before subsequent use.
+ - The set...() methods return a reference to "this" %Gravity element (in
+ the manner of an assignment operator) so they can be chained in a single
+ expression. **/
+/*@{*/
+
+
+/** This is the most general constructor for creating a %Gravity force element
+within a particular force subsystem and affecting all the bodies of a particular
+matter subsystem, given gravity magnitude and direction separately.
+
+If you want to exclude by default some bodies from the effects of this force
+element, call setDefaultBodyIsExcluded(). You can also make changes on the
+fly with setBodyIsExcluded().
+
+ at param[in,out] forces
+ The subsystem to which this force should be added.
+ at param[in] matter
+ The subsystem containing the bodies that will be affected.
+ at param[in] down
+ The default gravity direction vector d (i.e., the "down" direction),
+ expressed in the Ground frame. %Gravity will be directed along this direction
+ unless explicitly changed within a particular State via the setDirection()
+ or setGravityVector() methods. This is a unit vector; if you provide an
+ ordinary Vec3 it will be normalized before use and its magnitude ignored.
+ It is an error to provide a zero vector here; provide a zero magnitude
+ instead or use the alternate constructor.
+ at param[in] g
+ The default magnitude g to be used for gravity, given as a nonnegative
+ scalar with units of acceleration. The gravity vector is calculated as
+ v=g*d where d is the current direction vector, typically the default
+ direction as given by the \a down argument to this constructor.
+ %Gravity will have the magnitude given here unless explicitly changed within
+ a particular State via the setMagnitude() or setGravityVector() methods.
+ at param[in] zeroHeight
+ This is an optional specification of the default value for the height
+ up the gravity vector that is considered to be "zero" for purposes of
+ calculating the gravitational potential energy. The default is
+ \a zeroHeight == 0, i.e., a body's potential energy is defined to be zero
+ when the height of its mass center is the same as the height of the Ground
+ origin. The zero height will have the value specified here unless
+ explicitly changed within a particular State use the setZeroHeight()
+ method. **/
+Gravity(GeneralForceSubsystem& forces,
+ const SimbodyMatterSubsystem& matter,
+ const UnitVec3& down,
+ Real g,
+ Real zeroHeight = 0);
+
+/** Convenience constructor to create a %Gravity force element by specifying
+only a gravity vector, that is, the product of the gravity magnitude and the
+"down" direction vector. We have to extract both a magnitude and direction from
+this since they are maintained separately by this force element. If you provide
+a zero vector here (exactly zero, that is), then the magnitude is zero but there
+is no direction. In that case we default to the negative of the containing
+System's default "up" direction.
+
+If you want to exclude by default some bodies from the effects of this force
+element, call setDefaultBodyIsExcluded(). You can also make changes on the
+fly with setBodyIsExcluded().
+
+ at param[in,out] forces
+ The subsystem to which this force should be added.
+ at param[in] matter
+ The subsystem containing the bodies that will be affected.
+ at param[in] gravity
+ The default gravity vector v, interpreted as v=g*d where g=|\a gravity| is
+ a positive scalar and d is the "down" direction unit vector d=\a gravity/g.
+ If the magnitude is exactly zero we'll set the "down" direction to the
+ opposite of the containing System's "up" direction, otherwise the direction
+ is extracted from the given vector.
+
+This constructor sets the default zero height hz to zero (for use in calculating
+gravitational potential energy). If you want a different default value, use
+the more general constructor or the setDefaultZeroHeight() method.
+ at see SimTK::System::setUpDirection() **/
+Gravity(GeneralForceSubsystem& forces,
+ const SimbodyMatterSubsystem& matter,
+ const Vec3& gravity);
+
+/** Convenience constructor to create a %Gravity force element by specifying
+only gravity's magnitude, with the direction chosen to oppose the containing
+System's "up" direction. Note that the direction is extracted from the System
+and recorded when this %Gravity element is contructed; it will not be affected
+by subsequent changes to the System's up direction.
+
+If you want to exclude by default some bodies from the effects of this force
+element, call setDefaultBodyIsExcluded(). You can also make changes on the
+fly with setBodyIsExcluded().
+
+ at param[in,out] forces
+ The subsystem to which this force should be added.
+ at param[in] matter
+ The subsystem containing the bodies that will be affected.
+ at param[in] g
+ The default magnitude g to be used for gravity, given as a nonnegative
+ scalar with units of acceleration. %Gravity will have the magnitude given
+ here unless explicitly changed within a particular State via the
+ setMagnitude() or setGravityVector() methods.
+
+This constructor sets the default zero height hz to zero (for use in calculating
+gravitational potential energy). If you want a different default value, use
+the more general constructor or the setDefaultZeroHeight() method.
+ at see SimTK::System::setUpDirection() **/
+Gravity(GeneralForceSubsystem& forces,
+ const SimbodyMatterSubsystem& matter,
+ Real g);
+
+/** Default constructor creates an empty handle. **/
+Gravity() {}
+
+/** Set how the indicated body is to be treated by default. Normally all bodies
+except Ground are affected by this force element so you only need to call this
+if you want to exclude a body. This is a topological change since the flag will
+be stored with the System; for on-the-fly changes you should call
+setBodyIsExcluded() instead. Ground is always excluded; you can call this
+method on Ground but the call will be ignored.
+
+ at param[in] mobod
+ This is a MobilizedBody or MobilizedBodyIndex for the body whose
+ default exclusion status you want to change.
+ at param[in] isExcluded
+ Set to true if the default should be that this body is \e not to be affected
+ by this %Gravity force element.
+ at return A writable reference to "this" %Gravity element which will now have
+ the indicated body excluded.
+
+ at see setBodyIsExcluded() **/
+Gravity& setDefaultBodyIsExcluded(MobilizedBodyIndex mobod, bool isExcluded);
+
+/** Set the default value for the gravity vector, that is, the direction and
+magnitude with which gravity will act. From the given vector we extract
+separately the magnitude g and "down" direction d (a unit vector), unless the
+magnitude is exactly zero in which case we'll leave the direction unchanged.
+ at param[in] gravity
+ A vector giving the magnitude and direction with which gravity will act
+ on the bodies. If exactly zero only the magnitude will be changed.
+ at return A writable reference to "this" %Gravity element which will now have
+ the new default magnitude and direction. **/
+Gravity& setDefaultGravityVector(const Vec3& gravity);
+
+/** Set the default "down" direction d, that is, the direction along which
+gravity will act.
+ at param[in] down
+ A unit vector giving the direction along which gravity will act ("down").
+ If you pass an ordinary Vec3, it will be normalized before use and its
+ length will be ignored. Do not pass a zero-length Vec3, however.
+ at return A writable reference to "this" %Gravity element which will now have
+ the new default \a direction. **/
+Gravity& setDefaultDownDirection(const UnitVec3& down);
+/** Convenience overload that takes the down direction as a Vec3 and
+normalizes it (throwing away the magnitude) to create the required unit
+vector for the down direction. It is an error if the supplied Vec3 has zero
+length. **/
+Gravity& setDefaultDownDirection(const Vec3& down)
+{ return setDefaultDownDirection(UnitVec3(down)); }
+
+/** Set the default magnitude of gravity (a nonegative scalar). This will be
+combined with the default \a direction unit vector d to calculate the gravity
+vector v = g*d that is used to generate body forces mb*v where mb is the
+mass of body b and the force is applied to b's mass center.
+ at param[in] g
+ The magnitude of the uniform gravitational field, given as a nonnegative
+ scalar in units of acceleration. If this is zero no forces will be applied
+ by this element.
+ at return A writable reference to "this" %Gravity element which will now have
+ the new default \a strength. **/
+Gravity& setDefaultMagnitude(Real g);
+
+/** Set the default zero height (a signed scalar), for use in potential energy
+calculation. See the Gravity class comments for how this is used.
+ at param[in] zeroHeight
+ The height that is considered to be "zero" for purposes of potential
+ energy calculation. An affected body whose mass center is at this height
+ will have zero gravitational potential energy.
+ at return A writable reference to "this" %Gravity element which will now have
+ the new default \a zeroHeight. **/
+Gravity& setDefaultZeroHeight(Real zeroHeight);
+
+/** Return the current setting of the "is excluded by default" flag for the
+given body. This is the status that the flag will have in the default State
+returned by System::realizeTopology().
+ at param[in] mobod
+ This is a MobilizedBody or MobilizedBodyIndex for the body whose
+ exclusion status you want to obtain.
+ at return The current value of the "is excluded by default" flag for this body;
+ this will always be \c true for Ground.
+ at see getBodyIsExcluded() **/
+bool getDefaultBodyIsExcluded(MobilizedBodyIndex mobod) const;
+/** Return the default gravity vector being used for this %Gravity force
+element, calculated from the default magnitude and direction. **/
+Vec3 getDefaultGravityVector() const;
+/** Return the default down direction (a unit vector) for this %Gravity force
+element. **/
+const UnitVec3& getDefaultDownDirection() const;
+/** Return the default gravity magnitude g (a nonnegative scalar). **/
+Real getDefaultMagnitude() const;
+/** Return the default zero height used in the calculation of graviational
+potential energy. **/
+Real getDefaultZeroHeight() const;
+
+/*@}............................ Construction ................................*/
+
+
+
+//------------------------------------------------------------------------------
+/** @name Runtime Changes
+
+These refer to Dynamics-stage discrete state variables that determine the actual
+values to be used to calculate gravitational forces and energy from a given
+State object, and which bodies are to be excluded from that calculation. If
+these are not set explicitly, the values are set to those provided in the
+constructor or via the correponding setDefault...() methods. Note:
+ - Changing one of these parameters invalidates the given State's
+ Stage::Dynamics, meaning that the State's stage will be no higher than
+ Stage::Velocity after the parameter change. That ensures that forces
+ will be recalcuated before they are used (forces are calculated when
+ Stage::Dynamics is realized).
+ - The set...() methods here return a const reference to "this" %Gravity force
+ element (in the manner of an assignment operator, except read-only) so they
+ can be chained in a single expression. **/
+/*@{*/
+
+/** Within a given State, selectively exclude (or include) a body from the
+effects of this %Gravity force. The default State will inherit the "is excluded
+by default" values set during construction, but then you can change them. Ground
+is always excluded; you can call this method on Ground but the call will
+be ignored.
+ at param[in,out] state
+ The State object that is modified by this method; Stage::Dynamics will
+ be invalidated leaving the current stage no higher than Stage::Velocity.
+ at param[in] mobod
+ This is a MobilizedBody or MobilizedBodyIndex for the body whose
+ exclusion status you want to change.
+ at param[in] isExcluded
+ Set to true if this body should \e not be affected by this force element.
+ at return A const reference to "this" %Gravity force element for convenient
+ chaining of set...() methods in a single expression.
+ at pre \a state must already be realized to Stage::Topology.
+ at post Stage::Dynamics is invalidated in \a state if anything was changed.
+ at see setDefaultBodyIsExcluded(), getBodyIsExcluded() **/
+const Gravity& setBodyIsExcluded(State& state, MobilizedBodyIndex mobod,
+ bool isExcluded) const;
+
+/** Set the gravity vector v, that is, the magnitude and direction with which
+gravitational forces will act, overriding the default magnitude and direction
+that are stored in this %Gravity force element. If the given vector is exactly
+zero, then only the magnitude will be changed here.
+ at param[in,out] state
+ The State object that is modified by this method; Stage::Dynamics will
+ be invalidated leaving the current stage no higher than Stage::Velocity.
+ at param[in] gravity
+ The gravity vector including both the magnitude and direction. If this is
+ exactly zero only the magnitude will be changed.
+ at return A const reference to "this" %Gravity element for convenient chaining of
+ set...() methods in a single expression.
+ at pre \a state must already be realized to Stage::Topology.
+ at post Stage::Dynamics is invalidated in \a state if anything was changed.
+ at see setDownDirection(), setMagnitude() **/
+const Gravity& setGravityVector(State& state, const Vec3& gravity) const;
+
+/** Set the "down" direction d (a unit vector), that is, the direction along
+which gravitational forces will act. Magnitude can be changed independently
+with setMagnitude().
+ at param[in,out] state
+ The State object that is modified by this method; Stage::Dynamics will
+ be invalidated leaving the current stage no higher than Stage::Velocity.
+ at param[in] down
+ A unit vector giving the "down" direction along which gravity will act.
+ at return A const reference to "this" %Gravity element for convenient chaining of
+ set...() methods in a single expression.
+ at pre \a state must already be realized to Stage::Topology.
+ at post Stage::Dynamics is invalidated in \a state if anything was changed.
+ at see setMagnitude() **/
+const Gravity& setDownDirection(State& state,
+ const UnitVec3& down) const;
+/** Convenience overload that takes the down direction as a Vec3 and
+normalizes it (throwing away the magnitude) to create the required unit
+vector. It is an error if the supplied Vec3 has zero length. **/
+const Gravity& setDownDirection(State& state,
+ const Vec3& down) const
+{ return setDownDirection(state, UnitVec3(down)); }
+
+/** Set the gravity magnitude g (a nonnegative scalar) in this \a state,
+overriding the default gravity magnitude that is stored in this %Gravity force
+element.
+ at param[in,out] state
+ The State object that is modified by this method; Stage::Dynamics will
+ be invalidated leaving the current stage no higher than Stage::Velocity.
+ at param[in] g
+ The magnitude of the uniform gravitational field.
+ at return A const reference to "this" %Gravity element for convenient chaining of
+ set...() methods in a single expression.
+ at pre \a state must already be realized to Stage::Topology.
+ at post Stage::Dynamics is invalidated in \a state if anything was changed.
+ at see setDownDirection() **/
+const Gravity& setMagnitude(State& state, Real g) const;
+
+/** Set the potential energy zero height hz (a scalar) in this \a state,
+overriding the default zero height that is stored in this %Gravity force
+element.
+ at param[in,out] state
+ The State object that is modified by this method; Stage::Dynamics will
+ be invalidated leaving the current stage no higher than Stage::Velocity.
+ at param[in] hz
+ The height at which potential energy is considered to be zero.
+ at return A const reference to "this" %Gravity element for convenient chaining of
+ set...() methods in a single expression.
+ at pre \a state must already be realized to Stage::Topology.
+ at post Stage::Dynamics is invalidated in \a state if anything was changed. **/
+const Gravity& setZeroHeight(State& state, Real hz) const;
+
+/** Return the current setting of the "is excluded" flag for a given body in a
+given State. This is not necessarily the same as the default value for this
+body's exclusion status.
+ at param[in] state
+ The state containing the "is excluded" flag to be obtained.
+ at param[in] mobod
+ A MobilizedBody or MobilizedBodyIndex for the body whose exclusion status
+ you want to obtain.
+ at return The current value of the "is excluded" flag for this body in this
+ \a state; this will always be \c true for Ground.
+ at pre \a state must already be realized to Stage::Topology.
+ at see getDefaultBodyIsExcluded(), setBodyIsExcluded() **/
+bool getBodyIsExcluded(const State& state, MobilizedBodyIndex mobod) const;
+/** Get the gravity vector v that will be used for computations done with this
+\a state. This is calculated as v=g*d from the current values of the magnitude
+g and direction d Instance variables in the \a state.
+ at param[in] state
+ The state containing the gravity runtime variables used to calculate the
+ gravity vector.
+ at return The current value of the gravity vector.
+ at pre \a state must have been realized to Topology stage. **/
+Vec3 getGravityVector(const State& state) const;
+/** Get the gravity "down" direction d (a unit vector) that is currently set
+in this \a state.
+ at param[in] state
+ The state containing the gravity direction runtime variable whose value is
+ returned.
+ at return The current value of the gravity direction variable.
+ at pre \a state must have been realized to Topology stage. **/
+const UnitVec3& getDownDirection(const State& state) const;
+/** Get the gravity magnitude g (a nonnegative scalar) that is currently set
+in this \a state.
+ at param[in] state
+ The state containing the gravity magnitude runtime variable whose value is
+ returned.
+ at return The current value of the gravity magnitude variable.
+ at pre \a state must have been realized to Topology stage. **/
+Real getMagnitude(const State& state) const;
+/** Get the zero height hz that is currently set in this \a state for use in
+calculating gravitational potential energy. See the class documentation for
+information about hz is used.
+ at param[in] state
+ The state containing the zero height runtime variable whose value is
+ returned.
+ at return The current value of the zero height variable.
+ at pre \a state must have been realized to Topology stage. **/
+Real getZeroHeight(const State& state) const;
+
+/*@}.......................... Runtime Changes ...............................*/
+
+
+
+//------------------------------------------------------------------------------
+/** @name Energy and Forces
+These methods return the gravitational potential energy and the forces being
+applied by this %Gravity element in the configuration contained in the supplied
+State. These are evaluated during force calculation and available at no
+computational cost afterwards, although the first call after a position or
+runtime variable change will initiate computation if forces haven't already
+been computed. **/
+/*@{*/
+
+/** Obtain the gravitational potential energy contained in this %Gravity force
+element in the given configuration. Note that gravitational potential energy
+is m*g*h for each body where the height h is measured from an arbitrary zero
+height. By default the zero height is defined to be height of the Ground
+origin but that can be changed arbitrarily.
+ at param[in] state
+ The State from whose cache the potential energy is retrieved.
+ at return The gravitational potential energy of this %Gravity force element in the
+ configuration contained in \a state (a signed scalar).
+ at pre \a state must be realized to Stage::Position **/
+Real getPotentialEnergy(const State& state) const;
+
+/** Obtain a reference to the set of gravitational forces currently being
+applied by this %Gravity force element, as a Vector of spatial forces indexed
+by MobilizedBodyIndex. The force on Ground or on any explicitly excluded body
+is zero, but there is an entry for every mobilized body (starting with Ground)
+in the returned result. The return vector of spatial vectors is in a form
+suitable for direct use with the SimbodyMatterSubsystem Jacobian operators,
+which can be used to transform these into the equivalent generalized forces.
+This can be useful for gravity compensation.
+
+ at param[in] state
+ The State from whose cache the force is retrieved.
+ at return A reference to the vector of spatial forces (meaning the gravitational
+ moment about and force at the body origin, \e not necessarily the center
+ of mass) currently being applied to each of the mobilized bodies, expressed
+ in the Ground frame.
+ at pre \a state must be realized to Stage::Position
+
+Forces are returned as though applied at the body origin, which is
+\e not necessarily the same place as the body center of mass. That means that
+in general there will be both a moment and a force returned for each body.
+
+If gravity forces have not yet been calculated for the configuration given
+in \a state, the computation will be initiated here and cached for use later.
+**/
+const Vector_<SpatialVec>& getBodyForces(const State& state) const;
+
+/** Convenience method to extract the gravitational force on just one body;
+see getBodyForces() to get the whole set at once, and for more information.
+
+ at param[in] state
+ The State from whose cache the force is retrieved.
+ at param[in] mobod
+ The MobilizedBody or MobilizedBodyIndex whose force is wanted.
+ at return The spatial force (meaning the gravitational force and moment about
+ the body origin, \e not necessarily the center of mass) currently being
+ applied to the given mobilized body, expressed in the Ground frame.
+ at pre \a state must be realized to Stage::Position **/
+const SpatialVec&
+getBodyForce(const State& state, MobilizedBodyIndex mobod) const
+{ return getBodyForces(state)[mobod]; }
+
+// Particles aren't supported yet so don't show this in Doxygen.
+/** @cond **/
+/** Obtain the gravitational forces currently being applied by this %Gravity
+force element to the particles.
+ at param[in] state The State from whose cache the force is retrieved.
+ at return The current value of the forces, expressed in the Ground frame.
+ at pre \a state must be realized to Stage::Position **/
+const Vector_<Vec3>& getParticleForces(const State& state) const;
+/** @endcond **/
+
+/*@}............................ Energy and Forces ...........................*/
+
+//------------------------------------------------------------------------------
+/** @name Debugging/Testing
+This is information for use by developers of this class for debugging and
+testing -- please ignore. **/
+/*@{*/
+/** Return a count of the number of times the set of gravitational forces
+or potential energy was calculated since this force element was constructed.
+Note that this is not a per-body count. This count is only incremented when an
+actual computation is performed, not when forces are requested but are already
+valid. Also, we don't consider it a computation if the gravity magnitude is
+zero. This is intended for use in testing that caching and invalidation is being
+done properly. **/
+long long getNumEvaluations() const;
+
+/** Return true if the gravitational forces for this configuration have already
+been calculated and are up to date. That means they can be retrieved with no
+further evaluation. **/
+bool isForceCacheValid(const State& state) const;
+
+/** Invalidate the stored gravitational forces if they have already been
+calculated at this configuration. That will force a new evaluation the next
+time they are requested (unless the gravity magnitude is currently zero). **/
+void invalidateForceCache(const State& state) const;
+/*@}*/
+
+// Don't show this in Doxygen.
+/** @cond **/
+SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Gravity, GravityImpl, Force);
+/** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_FORCE_GRAVITY_H_
diff --git a/Simbody/include/simbody/internal/Force_LinearBushing.h b/Simbody/include/simbody/internal/Force_LinearBushing.h
new file mode 100644
index 0000000..b0f9f33
--- /dev/null
+++ b/Simbody/include/simbody/internal/Force_LinearBushing.h
@@ -0,0 +1,601 @@
+#ifndef SimTK_SIMBODY_FORCE_LINEAR_BUSHING_H_
+#define SimTK_SIMBODY_FORCE_LINEAR_BUSHING_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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Force.h"
+
+/** @file
+ * This contains the user-visible API ("handle" class) for the SimTK::Force
+ * subclass Force::LinearBushing and is logically part of Force.h. The file
+ * assumes that Force.h will have included all necessary declarations.
+ */
+
+namespace SimTK {
+/**
+ * This force element represents a bushing acting to connect a frame F fixed
+ * on one body (B1) to a frame M fixed on a second body (B2), by a massless,
+ * compliant element with linear stiffness and damping properties.
+ *
+ * The relative orientation R_FM of frame M given in frame F is parametrized by
+ * an x-y-z (1-2-3) B2-fixed Euler angle sequence and their time derivatives,
+ * as well as a position vector p_FM from F's origin OF to M's origin OM
+ * expressed in F, and its time derivative (velocity) v_FM (taken in F). For
+ * small orientation displacements, the Euler angles can be considered
+ * independent rotations about x, y, and z. Stiffness and damping parameters
+ * (6 of each) are provided for each direction's rotation and translation. The
+ * 6 coordinates q are defined as the three Euler angles [qx,qy,qz] followed
+ * by the three translations [px,py,pz]=p_FM.
+ *
+ * This force element is intended for small-displacement use, but is defined
+ * nonlinearly so is physically correct for large displacements also. However,
+ * be aware that the inferred q's cannot "wrap" so you must keep the motion
+ * small enough that the set of q's inferred from the transform X_FM (M's
+ * configuration in F) is continuous during a simulation. Also, the Bushing is
+ * singular in a configuration in which the middle rotation angle is near 90
+ * degrees, because the time derivative of that angle is unbounded; you should
+ * stay far away from that configuration.
+ *
+ * If you would like a force element like this one but suited for very large
+ * rotations (e.g., multiple revolutions along one of the axes) you should
+ * arrange to have a Bushing Mobilizer connected between frames F and M so that
+ * you can apply mobility forces rather than body forces as we're doing here.
+ * In that case the q's are the defining coordinates rather than the frames,
+ * and mobilizer Euler angle q's can wrap continuously without limit. However,
+ * with Euler angles there is necessarily a singular configuration so one of
+ * the axes must be kept within a modest range even if you use mobilizer
+ * coordinates.
+ *
+ * \par Theory:
+ *
+ * The LinearBushing calculates the relative orientation X_FM between the
+ * frames, and the relative spatial velocity V_FM. From these it infers a set
+ * of q's and qdot's as though there were a gimbal-like mechanism connecting
+ * frame F to frame M. This is done using standard formulas to decompose
+ * a rotation matrix into an Euler sequence (1-2-3 body fixed in this case)
+ * and to convert angular velocity to the time derivatives of the Euler
+ * angles. (There are also translational q's but they are trivial.)
+ *
+ * The generated force is then calculated for each of the 6 inferred
+ * coordinates and their 6 time derivatives as
+ * <pre> f_i = -(k_i*q_i + c_i*qdot_i). </pre>
+ * Each contribution to potential energy is
+ * <pre> e_i = k_i*q_i^2/2. </pre>
+ * The damping terms contribute no potential energy, but dissipate power at
+ * a rate
+ * <pre> p_i = c_i*qdot_i^2. </pre>
+ * Numerically integrating the dissipated power over time gives the energy
+ * dissipated by the Bushing since some arbitrary starting time and can be used
+ * to check conservation of energy in the presence of damping. The
+ * LinearBushing force allocates a state variable for this purpose so tracks
+ * the energy it dissipates; see the getDissipatedEnergy() method.
+ *
+ * The scalar rotational moments f_0, f_1, and f_2 act about rotated axes so
+ * do not constitute a vector; they are transformed internally here to produce
+ * the appropriate moments on the bodies. The scalar translational forces
+ * f_3, f_4, f_5 on the other hand are aligned with frame F's axes so
+ * constitute a vector in F.
+ */
+class SimTK_SIMBODY_EXPORT Force::LinearBushing : public Force {
+public:
+ /// Create a LinearBushing between a pair of arbitrary frames, one fixed
+ /// to each of two bodies.
+ ///
+ /// @param[in,out] forces
+ /// The subsystem to which this force should be added.
+ /// @param[in] body1
+ /// The first body to which the force should be applied.
+ /// @param[in] frameFOnB1
+ /// A frame F fixed to body 1 given by its constant transform X_B1F
+ /// from the B1 frame.
+ /// @param[in] body2
+ /// The other body to which the force should be applied.
+ /// @param[in] frameMOnB2
+ /// A frame M fixed to body 2 given by its constant transform X_B2M
+ /// from the B2 frame.
+ /// @param[in] stiffness
+ /// The six nonnegative spring constants, torsional followed by
+ /// translational.
+ /// @param[in] damping
+ /// The six nonnegative damping coefficients, torsional followed by
+ /// translational.
+ LinearBushing(GeneralForceSubsystem& forces,
+ const MobilizedBody& body1, const Transform& frameFOnB1,
+ const MobilizedBody& body2, const Transform& frameMOnB2,
+ const Vec6& stiffness, const Vec6& damping);
+
+ /// Create a LinearBushing connecting the body frames of two bodies.
+ /// This is the same as the more general constructor except it assumes
+ /// identity transforms for the two frames, meaning they are coincident
+ /// with the body frames.
+ ///
+ /// @param[in,out] forces
+ /// The subsystem to which this force should be added.
+ /// @param[in] body1
+ /// The first body to which the force should be applied.
+ /// @param[in] body2
+ /// The other body to which the force should be applied.
+ /// @param[in] stiffness
+ /// The six nonnegative spring constants, torsional followed by
+ /// translational.
+ /// @param[in] damping
+ /// The six nonnegative damping coefficients, torsional followed by
+ /// translational.
+ LinearBushing(GeneralForceSubsystem& forces,
+ const MobilizedBody& body1,
+ const MobilizedBody& body2,
+ const Vec6& stiffness, const Vec6& damping);
+
+ /// Default constructor creates an empty handle.
+ LinearBushing() {}
+
+
+ //--------------------------------------------------------------------------
+ /// @name Default Parameters
+ ///
+ /// These refer to Topology-stage parameters normally set in the
+ /// constructor; these determine the initial values assigned to the
+ /// corresponding Instance-stage state variables.
+ ///
+ /// \par Notes
+ /// - Changing one of these parameters invalidates the containing System's
+ /// topology, meaning that realizeTopology() will have to be called
+ /// before subsequent use.
+ /// - The set...() methods return a reference to "this" LinearBushing (in
+ /// the manner of an assignment operator) so they can be chained in a
+ /// single expression.
+ //@{
+ /// Set the frame F on body B1 that will be used by default as one of the
+ /// frames connected by the bushing, by giving the transform X_B1F locating
+ /// frame F with respect to the body frame B1.
+ /// @param[in] X_B1F
+ /// The Transform giving the default placement of the first Bushing
+ /// frame F in the body 1 frame B1.
+ /// @return
+ /// A writable reference to "this" Linear Bushing which will now have
+ /// the new default F frame
+ LinearBushing& setDefaultFrameOnBody1(const Transform& X_B1F);
+ /// Set the frame M on body B2 that will be used by default as one of the
+ /// frames connected by the bushing, by giving the transform X_B2M locating
+ /// frame M with respect to the body frame B2.
+ /// @param[in] X_B2M
+ /// The Transform giving the default placement of the second Bushing
+ /// frame M in the body 2 frame B2.
+ /// @return
+ /// A writable reference to "this" Linear Bushing which will now have
+ /// the new default M frame.
+ LinearBushing& setDefaultFrameOnBody2(const Transform& X_B2M);
+ /// Set the stiffnesses (spring constants) k that will be used by default
+ /// for this Bushing; these must be nonnegative.
+ /// @param[in] stiffness
+ /// The six nonnegative spring constants, one for each axis in the
+ /// order qx,qy,qz,px,py,pz (i.e., rotations first).
+ /// @return
+ /// A writable reference to "this" Linear Bushing which will now have
+ /// the new default stiffnesses.
+ LinearBushing& setDefaultStiffness(const Vec6& stiffness);
+ /// Set the damping coefficients c that will be used by default for this
+ /// Bushing; these must be nonnegative.
+ /// @param[in] damping
+ /// The six nonnegative damping coefficients, one for each axis in the
+ /// order qx,qy,qz,px,py,pz (i.e., rotations first).
+ /// @return
+ /// A writable reference to "this" Linear Bushing which will now have
+ /// the new default damping coefficients
+ LinearBushing& setDefaultDamping(const Vec6& damping);
+
+ /// Return the frame F on body B1 that will be used by default as one of the
+ /// frames connected by the bushing, as the transform X_B1F locating frame F
+ /// with respect to the body frame B1.
+ /// @return
+ /// The Transform X_B1F giving the default placement of the first
+ /// Bushing frame F in the body 1 frame B1.
+ const Transform& getDefaultFrameOnBody1() const;
+ /// Return the frame M on body B2 that will be used by default as one of the
+ /// frames connected by the bushing, as the transform X_B2M locating frame M
+ /// with respect to the body frame B2.
+ /// @return
+ /// The Transform X_B2M giving the default placement of the second
+ /// Bushing frame M in the body 2 frame B2.
+ const Transform& getDefaultFrameOnBody2() const;
+ /// Return the stiffnesses k that will be used by default for this Bushing.
+ /// @return
+ /// The six default spring constants, one for each axis in the
+ /// order qx,qy,qz,px,py,pz (i.e., rotations first).
+ const Vec6& getDefaultStiffness() const;
+ /// Return the damping coefficients c that will be used by default for this
+ /// Bushing.
+ /// @return
+ /// The six default damping coefficients, one for each axis in the
+ /// order qx,qy,qz,px,py,pz (i.e., rotations first).
+ const Vec6& getDefaultDamping() const;
+ //@}......................... Default Parameters ...........................
+
+
+
+ //--------------------------------------------------------------------------
+ /// @name Instance Parameters
+ ///
+ /// These refer to the Instance-stage state variables that determine the
+ /// geometry and material properties that will be used in computations
+ /// involving this Bushing when performed with the given State. If these
+ /// are not set explicitly, the default values are set to those provided in
+ /// the constructor or via the correponding setDefault...() methods.
+ ///
+ /// \par Notes
+ /// - Changing one of these parameters invalidates the given State's
+ /// Instance stage, meaning that realize(Instance) will have to be called
+ /// (explicitly or implicitly by realizing a higher stage) before
+ /// subsequent use.
+ /// - The set...() methods here return a const reference to "this"
+ /// LinearBushing (in the manner of an assignment operator, except
+ /// read-only) so they can be chained in a single expression.
+ //@{
+ /// Set the frame F on body B1 that will be used as one of the frames
+ /// connected by the bushing when evaluating with this State, by giving
+ /// the transform X_B1F locating frame F with respect to the body frame B1.
+ /// @pre \a state realized to Stage::Topology
+ /// @param[in,out] state
+ /// The State object that is modified by this method.
+ /// @param[in] X_B1F
+ /// The Transform giving the placement of the first Bushing frame F
+ /// in the body 1 frame B1.
+ /// @return
+ /// A const reference to "this" Linear Bushing for convenient
+ /// chaining of set...() methods in a single expression.
+ const LinearBushing& setFrameOnBody1(State& state,
+ const Transform& X_B1F) const;
+ /// Set the frame M on body B2 that will be used as one of the frames
+ /// connected by the bushing when evaluating with this State, by giving
+ /// the transform X_B2M locating frame M with respect to the body frame B2.
+ /// @pre \a state realized to Stage::Topology
+ /// @param[in,out] state
+ /// The State object that is modified by this method.
+ /// @param[in] X_B2M
+ /// The Transform giving the placement of the second Bushing frame M
+ /// in the body 2 frame B2.
+ /// @return
+ /// A const reference to "this" Linear Bushing for convenient
+ /// chaining of set...() methods in a single expression.
+ const LinearBushing& setFrameOnBody2(State& state,
+ const Transform& X_B2M) const;
+ /// Set the stiffnesses (spring constants) k that will be used for this
+ /// Bushing when evaluated using this State.
+ /// @pre \a state realized to Stage::Topology
+ /// @param[in,out] state
+ /// The State object that is modified by this method.
+ /// @param[in] stiffness
+ /// Six nonnegative spring constants, one for each axis in the
+ /// order qx,qy,qz,px,py,pz (i.e., rotations first).
+ /// @return
+ /// A const reference to "this" Linear Bushing for convenient
+ /// chaining of set...() methods in a single expression.
+ const LinearBushing& setStiffness(State& state,
+ const Vec6& stiffness) const;
+ /// Set the damping coefficients c that will be used for this Bushing when
+ /// evaluated using this State.
+ /// @pre \a state realized to Stage::Topology
+ /// @param[in,out] state
+ /// The State object that is modified by this method.
+ /// @param[in] damping
+ /// Six nonnegative damping coefficients, one for each axis in the
+ /// order qx,qy,qz,px,py,pz (i.e., rotations first).
+ /// @return
+ /// A const reference to "this" Linear Bushing for convenient
+ /// chaining of set...() methods in a single expression.
+ const LinearBushing& setDamping(State& state,
+ const Vec6& damping) const;
+
+ /// Return the frame F on body B1 currently being used by this State as one
+ /// of the frames connected by the bushing, as the transform X_B1F locating
+ /// frame F with respect to the body frame B1.
+ /// @pre \a state realized to Stage::Topology
+ /// @param[in] state
+ /// The State object from which we obtain the frame.
+ /// @return
+ /// The Transform X_B1F giving the given state's current placement
+ /// of the first Bushing frame F in the body 1 frame B1.
+ const Transform& getFrameOnBody1(const State& state) const;
+ /// Return the frame M on body B2 currently being used by this State as one
+ /// of the frames connected by the bushing, as the transform X_B2M locating
+ /// frame M with respect to the body frame B2.
+ /// @pre \a state realized to Stage::Topology
+ /// @param[in] state
+ /// The State object from which we obtain the frame.
+ /// @return
+ /// The Transform X_B2M giving the given state's current placement
+ /// of the second Bushing frame M in the body 2 frame B2.
+ const Transform& getFrameOnBody2(const State& state) const;
+ /// Return the stiffnesses (spring constants) k currently being used for
+ /// this Bushing by this State.
+ /// @pre \a state realized to Stage::Topology
+ /// @param[in] state
+ /// The State object from which we obtain the stiffnesses.
+ /// @return
+ /// The current values in the given state of the six spring constants
+ /// k, one for each axis in the order qx,qy,qz,px,py,pz (i.e.,
+ /// rotations first).
+ const Vec6& getStiffness(const State& state) const;
+ /// Return the damping coefficients c currently being used for this Bushing
+ /// by this State.
+ /// @pre \a state realized to Stage::Topology
+ /// @param[in] state
+ /// The State object from which we obtain the damping coefficients.
+ /// @return
+ /// The current values in the given state of the six damping
+ /// coefficients c, one for each axis in the order qx,qy,qz,px,py,pz
+ /// (i.e., rotations first).
+ const Vec6& getDamping(const State& state) const;
+ //@}...................... Instance Parameters .............................
+
+
+
+ //--------------------------------------------------------------------------
+ /// @name Position-related Quantities
+ ///
+ /// These methods return position-dependent quantities that are calculated
+ /// by the Bushing as part of its force calculations and stored in the
+ /// State cache. These can be obtained at no cost, although the first call
+ /// after a position change may initiate computation if forces haven't
+ /// already been computed.
+ ///
+ /// @pre These methods may be called only after the supplied state has
+ /// been realized to Position stage.
+ //@{
+ /// Obtain the generalized coordinates last calculated by this force
+ /// element. These are the body2-fixed x-y-z Euler angles qx,qy,qz and
+ /// p_FM=[px,py,pz], the vector from the frame F origin OF to
+ /// the frame M origin OM, expressed in the F frame. The full generalized
+ /// coordinate vector is q=[qx,qy,qz,px,py,pz].
+ /// @pre \a state realized to Stage::Position
+ /// @param[in] state
+ /// The State from whose cache the coordinates are retrieved.
+ /// @return
+ /// The six generalized coordinates relating the frames in order
+ /// qx,qy,qz,px,py,pz (i.e., rotational coordinates first).
+ /// @see getQDot()
+ const Vec6& getQ(const State& state) const;
+
+ /// Obtain the spatial transform X_GF giving the location and orientation of
+ /// body 1's frame F in Ground.
+ /// @pre \a state realized to Stage::Position
+ /// @param[in] state
+ /// The State from whose cache the transform is retrieved.
+ /// @return
+ /// The transform X_GF.
+ const Transform& getX_GF(const State& state) const;
+
+ /// Obtain the spatial transform X_GM giving the location and orientation of
+ /// body 2's frame M in Ground.
+ /// @pre \a state realized to Stage::Position
+ /// @param[in] state
+ /// The State from whose cache the transform is retrieved.
+ /// @return
+ /// The transform X_GM.
+ const Transform& getX_GM(const State& state) const;
+
+ /// Obtain the spatial transform X_FM giving the location and orientation of
+ /// body 2's frame M in body 1's frame F.
+ /// @pre \a state realized to Stage::Position
+ /// @param[in] state
+ /// The State from whose cache the transform is retrieved.
+ /// @return
+ /// The transform X_FM.
+ const Transform& getX_FM(const State& state) const;
+ //@}.................. Position-related quantities .........................
+
+
+
+ //--------------------------------------------------------------------------
+ /// @name Velocity-related Quantities
+ ///
+ /// These methods return velocity-dependent quantities that are calculated
+ /// by the Bushing as part of its force calculations and stored in the
+ /// State cache. These can be obtained at no cost, although the first call
+ /// after a velocity change may initiate computation if forces haven't
+ /// already been computed.
+ ///
+ /// @pre These methods may be called only after the supplied state has
+ /// been realized to Velocity stage.
+ //@{
+ /// Obtain the generalized coordinate derivatives last calculated by this
+ /// force element. These are the body2-fixed x-y-z Euler angle derivatives
+ /// qdotx,qdoty,qdotz and v_FM=[vx,vy,vz], the velocity of point OM in
+ /// frame F, expressed in F. That is, v_FM = d/dt p_FM with the derivative
+ /// taken in the F frame. The full generalized coordinate derivative vector
+ /// qdot=[qdotx,qdoty,qdotz,vx,vy,vz].
+ /// @pre \a state realized to Stage::Velocity
+ /// @param[in] state
+ /// The State from whose cache the coordinate derivatives are retrieved.
+ /// @return
+ /// The six generalized coordinate derivatives.
+ /// @see getQ()
+ const Vec6& getQDot(const State& state) const;
+
+ /// Obtain the spatial velocity V_GF giving the velocity of body 1's frame
+ /// F in the Ground frame. Note that this is the time derivative of X_GF
+ /// <em>taken in G</em>, that is, the ordinary spatial velocity.
+ /// @pre \a state realized to Stage::Velocity
+ /// @param[in] state
+ /// The State from whose cache the velocity is retrieved.
+ /// @return
+ /// The spatial velocity V_GF as a spatial vector, that is, V_GF[0]
+ /// is the angular velocity of F in G; V_GF[1] is the linear velocity
+ /// of F's origin point OF in G. Both vectors are expressed in G.
+ /// @see getV_FM() to get the local bushing deformation velocity
+ const SpatialVec& getV_GF(const State& state) const;
+
+ /// Obtain the spatial velocity V_GM giving the velocity of body 2's frame
+ /// M in the Ground frame. Note that this is the time derivative of X_GM
+ /// <em>taken in G</em>, that is, the ordinary spatial velocity.
+ /// @pre \a state realized to Stage::Velocity
+ /// @param[in] state
+ /// The State from whose cache the velocity is retrieved.
+ /// @return
+ /// The spatial velocity V_GM as a spatial vector, that is, V_GM[0]
+ /// is the angular velocity of M in G; V_GM[1] is the linear velocity
+ /// of M's origin point OM in G. Both vectors are expressed in G.
+ /// @see getV_FM() to get the local bushing deformation velocity
+ const SpatialVec& getV_GM(const State& state) const;
+
+ /// Obtain the spatial velocity V_FM giving the velocity of body 2's frame
+ /// M in body 1's frame F, expressed in the F frame. Note that this is
+ /// the time derivative of X_FM <em>taken in F</em>, which means that V_FM
+ /// is a local relationship between F and M and is not affected by F's
+ /// motion with respect to Ground.
+ /// @pre \a state realized to Stage::Velocity
+ /// @param[in] state
+ /// The State from whose cache the velocity is retrieved.
+ /// @return
+ /// The spatial velocity V_FM as a spatial vector, that is, V_FM[0]
+ /// is the angular velocity of F in M; V_FM[1] is the linear velocity
+ /// of M's origin point OM in F. Both vectors are expressed in F.
+ const SpatialVec& getV_FM(const State& state) const;
+ //@}.................. Velocity-related quantities .........................
+
+
+
+ //--------------------------------------------------------------------------
+ /// @name Forces
+ ///
+ /// These methods return the forces being applied by this Bushing in the
+ /// configuration and velocities contained in the supplied State. These are
+ /// evaluated during force calculation and available at no computational cost
+ /// afterwards, although the first call after a velocity change may initiate
+ /// computation if forces haven't already been computed.
+ ///
+ /// @pre These methods may be called only after the supplied state has
+ /// been realized to Velocity stage.
+ //@{
+ /// Obtain the generalized forces f being applied by this Bushing force
+ /// element on each of its six axes. The sign is such that it would be
+ /// appropriate to apply to a Bushing mobilizer connecting the same two
+ /// frames; that is, these are the generalized forces acting on the
+ /// "outboard" body 2; the negative of these forces acts on the "inboard"
+ /// body 1.
+ /// @pre \a state realized to Stage::Velocity
+ /// @param[in] state
+ /// The State from whose cache the forces are retrieved.
+ /// @return
+ /// The six generalized forces as a Vec6 in the order mx,my,mz,
+ /// fx,fy,fz where the m's are moments about the rotated Euler axes
+ /// and the f's are forces along the translational axes of the F frame.
+ const Vec6& getF(const State& state) const;
+
+ /// Return the spatial force F_GM being applied by this Bushing to body 2
+ /// at its M frame, expressed in the Ground frame.
+ /// @pre \a state realized to Stage::Velocity
+ /// @param[in] state
+ /// The State from whose cache the force is retrieved.
+ /// @return
+ /// The spatial force F_GM as a spatial vector, expressed in G. That
+ /// is, F_GM[0] is the torque vector applied to body 2; F_GM[1] is
+ /// the force vector applied to body 2 at frame M's origin OM.
+ const SpatialVec& getF_GM(const State& state) const;
+
+ /// Return the spatial force F_GF being applied by this Bushing to body 1
+ /// at its F frame, expressed in the Ground frame. This is equal and
+ /// opposite to the spatial force applied on body 2.
+ /// @pre \a state realized to Stage::Velocity
+ /// @param[in] state
+ /// The State from whose cache the force is retrieved.
+ /// @return
+ /// The spatial force F_GF as a spatial vector, expressed in G. That
+ /// is, F_GF[0] is the torque vector applied to body 1; F_GF[1] is
+ /// the force vector applied to body 1 at frame F's origin OF.
+ const SpatialVec& getF_GF(const State& state) const;
+ //@}............................ Forces ....................................
+
+
+
+ //--------------------------------------------------------------------------
+ /// @name Energy, Power, and Work
+ ///
+ /// These methods return the energy, power, and work-related quantities
+ /// associated with this Bushing element for the values in the supplied
+ /// State.
+ //@{
+ /// Obtain the potential energy stored in this Bushing in the current
+ /// configuration.
+ /// @pre \a state realized to Stage::Position
+ /// @param[in] state
+ /// The State from whose cache the potential energy is retrieved.
+ /// @return
+ /// The potential energy currently contained in the Bushing in the
+ /// configuration contained in \a state (a nonnegative scalar).
+ Real getPotentialEnergy(const State& state) const;
+
+ /// Obtain the rate at which energy is being dissipated by this Bushing,
+ /// that is, the power being lost. This is in units of energy/time which
+ /// is watts in MKS.
+ /// @pre \a state realized to Stage::Velocity
+ /// @param[in] state
+ /// The State from which to obtain the current value of the power
+ /// dissipation.
+ /// @return
+ /// The dissipated power (a nonnegative scalar).
+ /// @see getDissipatedEnergy() for the time-integrated power loss
+ Real getPowerDissipation(const State& state) const;
+
+ /// Obtain the total amount of energy dissipated by this Bushing since some
+ /// arbitrary starting point. This is the time integral of the power
+ /// dissipation. For a system whose only non-conservative forces are
+ /// Bushings, the sum of potential, kinetic, and dissipated energies should
+ /// be conserved. This is a State variable so you can obtain its value any
+ /// time after it is allocated.
+ /// @pre \a state realized to Stage::Model
+ /// @param[in] state
+ /// The State from which to obtain the current value of the dissipated
+ /// energy.
+ /// @return
+ /// The total dissipated energy (a nonnegative scalar).
+ /// @see getPowerDissipation() for the instantaneous power loss
+ Real getDissipatedEnergy(const State& state) const;
+
+ /// Set the accumulated dissipated energy to an arbitrary value. Typically
+ /// this is used only to reset the dissipated energy to zero, but non-zero
+ /// values can be useful if you are trying to match some existing data or
+ /// continuing a simulation. This is a State variable so you can set its
+ /// value any time after it is allocated.
+ /// @pre \a state realized to Stage::Model
+ /// @param[in,out] state
+ /// The State whose dissipated energy variable for this Bushing is to
+ /// be modified.
+ /// @param[in] energy
+ /// The new value for the accumulated dissipated energy (must be a
+ /// nonnegative scalar).
+ void setDissipatedEnergy(State& state, Real energy) const;
+ //@}..................... Energy, Work, and Power ..........................
+
+ // Don't show this in Doxygen.
+ /// @cond
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS
+ (LinearBushing, LinearBushingImpl, Force);
+ /// @endcond
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_FORCE_LINEAR_BUSHING_H_
diff --git a/Simbody/include/simbody/internal/Force_MobilityConstantForce.h b/Simbody/include/simbody/internal/Force_MobilityConstantForce.h
new file mode 100644
index 0000000..79ca77c
--- /dev/null
+++ b/Simbody/include/simbody/internal/Force_MobilityConstantForce.h
@@ -0,0 +1,131 @@
+#ifndef SimTK_SIMBODY_FORCE_MOBILITY_CONSTANT_FORCE_H_
+#define SimTK_SIMBODY_FORCE_MOBILITY_CONSTANT_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-13 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Force.h"
+
+/** @file
+This contains the user-visible API ("handle" class) for the SimTK::Force
+subclass Force::MobilityConstantForce and is logically part of Force.h. The
+file assumes that Force.h will have included all necessary declarations. **/
+
+namespace SimTK {
+
+/** A constant generalized force f (a scalar) applied to a mobility. The
+mobility here selects a generalized speed (u), not a generalized coordinate (q),
+and the meaning depends on the definition of the generalized speed. If that
+speed is a translation then this is a force; if a rotation then this is a
+torque; if anything more general then this is in compatible force units. This
+force does not contribute to the potential energy, so adding it to a system will
+cause energy not to be conserved unless you account for the power injected or
+dissipated here. **/
+class SimTK_SIMBODY_EXPORT Force::MobilityConstantForce : public Force {
+public:
+ /** Add a %MobilityConstantForce element to the indicated Subsystem.
+ @param forces Subsystem to which this force should be added.
+ @param mobod Mobilizer to which the force should be applied.
+ @param whichU Index within \a mobod of the generalized speed u to
+ which this force should be applied (first is zero).
+ @param defaultForce Default value for the generalized force. **/
+ MobilityConstantForce(GeneralForceSubsystem& forces,
+ const MobilizedBody& mobod,
+ MobilizerUIndex whichU,
+ Real defaultForce);
+
+ /** Alternate constructor signature for when the mobilizer has only
+ a single generalized speed, in which case we'll use MobilizerUIndex(0).
+ See the other signature for documentation. **/
+ MobilityConstantForce(GeneralForceSubsystem& forces,
+ const MobilizedBody& mobod,
+ Real defaultForce)
+ { // Invoke the other constructor.
+ new(this) MobilityConstantForce(forces, mobod, MobilizerUIndex(0),
+ defaultForce);
+ }
+
+ /** Default constructor creates an empty handle. **/
+ MobilityConstantForce() {}
+
+ /** Provide a new value for the default generalied force to be applied by
+ this force element. This is a topological change because it affects the
+ value that the containing System's default state will have when
+ realizeTopology() is called. This is for use during construction, not for
+ during a simulation where you should be using setForce() to set the force
+ in a State rather than in the System.
+
+ @param defaultForce Default value for the generalized force.
+ @returns A writable reference to this modified force element.
+ @see setForce(), getDefaultForce() **/
+ MobilityConstantForce& setDefaultForce(Real defaultForce);
+
+ /** Return the default value for the generalized force. This is normally
+ set at construction but may have been changed with setDefaultForce().
+ @see setDefaultForce(), getForce() **/
+ Real getDefaultForce() const;
+
+ /** Change the value of the generalized force that is stored in the given
+ \a state; this may differ from the default value supplied at construction.
+ @param state The State in which the bounds are changed.
+ @param force The value of the generalized force to be applied when the
+ \a state is subsequently used.
+
+ Changing this force invalidates Stage::Dynamics and above in the \a state
+ since it affects force generation.
+ @see getForce(), setDefaultForce() **/
+ void setForce(State& state, Real force) const;
+
+ /** Return the value for the generalized force that is stored in the
+ given \a state. Note that this is not the same thing as the default force
+ that was supplied on construction or with setDefaultForce().
+ @see setForce(), getDefaultForce() **/
+ Real getForce(const State& state) const;
+
+ /** @name Deprecated
+ Methods here are for backwards compatibility but have been replaced with
+ better ones that you should use. **/
+ /**@{**/
+ /** Deprecated: Alternate signature for backwards compatibilty -- for
+ safety you should prefer using the other constructor signature that
+ takes a MobilizerUIndex rather than a plain int. **/
+ MobilityConstantForce(GeneralForceSubsystem& forces,
+ const MobilizedBody& mobod,
+ int whichU,
+ Real defaultForce)
+ { // Invoke the other constructor.
+ new(this) MobilityConstantForce(forces, mobod, MobilizerUIndex(whichU),
+ defaultForce);
+ }
+ /**@}**/
+
+ /** @cond **/ // Hide from Doxygen.
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(MobilityConstantForce,
+ MobilityConstantForceImpl, Force);
+ /** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_FORCE_MOBILITY_CONSTANT_FORCE_H_
diff --git a/Simbody/include/simbody/internal/Force_MobilityDiscreteForce.h b/Simbody/include/simbody/internal/Force_MobilityDiscreteForce.h
new file mode 100644
index 0000000..70a0dac
--- /dev/null
+++ b/Simbody/include/simbody/internal/Force_MobilityDiscreteForce.h
@@ -0,0 +1,130 @@
+#ifndef SimTK_SIMBODY_FORCE_MOBILITY_DISCRETE_FORCE_H_
+#define SimTK_SIMBODY_FORCE_MOBILITY_DISCRETE_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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Force.h"
+
+/** @file
+This contains the user-visible API ("handle" class) for the SimTK::Force
+subclass Force::MobilityDiscreteForce and is logically part of Force.h. The
+file assumes that Force.h will have included all necessary declarations. **/
+
+namespace SimTK {
+
+/** A discrete mobility (generalized) force f applied to a particular mobility
+that is specified at construction.\ Useful for applying
+external forces or forces that are updated at discrete times due to the
+occurrence of events. Note that a mobility is a generalized speed (u), not
+a generalized coordinate (q). The meaning of a generalized force depends on the
+definition of the generalized speed. If that speed is a translation then this is
+a force; if a rotation then this is a torque; if something else then f has a
+comparable definition (the defining condition is that f*u should always have
+physically meaningful units of power). This force does not contribute to the
+potential energy, so adding it to a system will cause potential+kinetic energy
+not to be conserved.
+
+If you want to be able to apply discrete forces to any body or mobilizer
+specifying which one in advance, see Force::DiscreteForces.
+ at see Force::DiscreteForces **/
+class SimTK_SIMBODY_EXPORT Force::MobilityDiscreteForce : public Force {
+public:
+ /** Create a %MobilityDiscreteForce.
+
+ @param forces subsystem to which this force element should be added
+ @param mobod mobilizer to which the force should be applied
+ @param whichU to which of the mobilizer's mobilities (degrees of
+ freedom) should this force be applied (first is 0)?
+ @param defaultForce initial value for the generalized force to be applied
+ (default 0)
+
+ Note that if you have an integer value for the generalized speed (u) index,
+ you have to cast it to a MobilizerUIndex here. The generalized speeds are
+ numbered starting with 0 for each mobilizer. Here is an example:
+ @code
+ GeneralForceSubsystem forces;
+ MobilizedBody::Pin pinJoint(...);
+ MobilityDiscreteForce myForce(forces, pinJoint, MobilizerUIndex(0));
+ @endcode
+ **/
+ MobilityDiscreteForce(GeneralForceSubsystem& forces,
+ const MobilizedBody& mobod,
+ MobilizerUIndex whichU,
+ Real defaultForce=0);
+
+
+ /** Alternate constructor signature for when the mobilizer has only
+ a single generalized speed, in which case we'll use MobilizerUIndex(0).
+ See the other signature for documentation. **/
+ MobilityDiscreteForce(GeneralForceSubsystem& forces,
+ const MobilizedBody& mobod,
+ Real defaultForce=0)
+ { // Invoke the other constructor.
+ new(this) MobilityDiscreteForce(forces, mobod, MobilizerUIndex(0),
+ defaultForce);
+ }
+
+ /** Default constructor creates an empty handle. **/
+ MobilityDiscreteForce() {}
+
+ /** Provide a new value for the \a defaultForce, overriding the one
+ provided in the constructor. This is a topological change because it
+ affects the value that the containing System's default state will have
+ when realizeTopology() is called. This is for use during construction, not
+ for during a simulation where you should be using setGeneralizedForce().
+
+ @param defaultForce the value this generalized force should have by
+ default
+ @returns a writable reference to this modified force element
+ @see setMobilityForce(), getDefaultMobilityForce() **/
+ MobilityDiscreteForce& setDefaultMobilityForce(Real defaultForce);
+
+ /** Return the value that this generalized force will have by default. This
+ is normally set in the constructor, or left to its default value of 0. It
+ can also be set in setDefaultMobilityForce(). Note that this is \e not
+ the same as the value that may be set in any particular State.
+ @see getMobilityForce(), setDefaultMobilityForce() **/
+ Real getDefaultMobilityForce() const;
+
+ /** Change the value of the generalized force to be applied in the given
+ \a state. Set this to zero if you don't want it to do anything.
+ @see getMobilityForce() **/
+ void setMobilityForce(State& state, Real f) const;
+ /** Return the value for this generalized force that is stored in the
+ given \a state. If no calls to setMobilityForce() have been made on this
+ \a state then it will have the \a defaultForce value that was supplied on
+ construction or via setDefaultMobilityForce().
+ @see setMobilityForce() **/
+ Real getMobilityForce(const State& state) const;
+
+ /** @cond **/
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(MobilityDiscreteForce,
+ MobilityDiscreteForceImpl, Force);
+ /** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_FORCE_MOBILITY_DISCRETE_FORCE_H_
diff --git a/Simbody/include/simbody/internal/Force_MobilityLinearDamper.h b/Simbody/include/simbody/internal/Force_MobilityLinearDamper.h
new file mode 100644
index 0000000..c520ef6
--- /dev/null
+++ b/Simbody/include/simbody/internal/Force_MobilityLinearDamper.h
@@ -0,0 +1,137 @@
+#ifndef SimTK_SIMBODY_FORCE_MOBILITY_LINEAR_DAMPER_H_
+#define SimTK_SIMBODY_FORCE_MOBILITY_LINEAR_DAMPER_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-13 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Force.h"
+
+/** @file
+This contains the user-visible API ("handle" class) for the SimTK::Force
+subclass Force::MobilityLinearDamper and is logically part of Force.h. The file
+assumes that Force.h will have included all necessary declarations. **/
+
+namespace SimTK {
+
+/** A linear damper that acts along or around a mobility coordinate to apply
+a generalized force there.
+
+The damping constant c is provided, with the generated force being -c*u where
+u is the mobility's generalized speed. This is meaningful on any mobility, since
+all our generalized speeds have physical meaning. This is not a potential force
+and hence does not contribute to potential energy.
+**/
+
+class SimTK_SIMBODY_EXPORT Force::MobilityLinearDamper : public Force {
+public:
+ /** Create a %MobilityLinearDamper force element on a particular mobility
+ (generalized speed).
+
+ @param[in,out] forces
+ The subsystem to which this force should be added.
+ @param[in] mobod
+ Mobilizer to which the force should be applied.
+ @param[in] whichU
+ To which of the mobilizer's mobilities (generalized speeds) u should
+ this force be applied (first is 0)?
+ @param[in] defaultDamping
+ The default value for the damping constant c.
+ **/
+ MobilityLinearDamper(GeneralForceSubsystem& forces,
+ const MobilizedBody& mobod,
+ MobilizerUIndex whichU,
+ Real defaultDamping);
+
+ /** Default constructor creates an empty handle that can be assigned to
+ refer to any %MobilityLinearDamper object. **/
+ MobilityLinearDamper() {}
+
+ /** Provide a new value for the default damping constant c of this damper.
+ This is a topological change because it affects the value that the
+ containing System's default state will have when realizeTopology() is
+ called. This is for use during construction, not for during a simulation
+ where you should be using setDamping() to set the damping constant in a
+ State rather than in the System.
+ @param[in] defaultDamping
+ The default value for the damping constant c.
+ @return
+ A writable reference to this modified force element for convenience in
+ chaining set methods.
+ @see getDefaultDamping(), setDamping() **/
+ MobilityLinearDamper& setDefaultDamping(Real defaultDamping);
+
+ /** Return the default value for the damper's damping constant c. This is
+ normally set at construction but can be modified with setDefaultDamping().
+ @see setDefaultDamping(), getDamping() **/
+ Real getDefaultDamping() const;
+
+ /** Change the value of the damping constant c in the given \a state; this
+ may differ from the default value supplied at construction.
+ @param[in,out] state
+ The State in which the damping constant is to be changed.
+ @param[in] damping
+ The new damping constant c (>= 0) that overrides the default.
+ @return
+ A const reference to this %MobilityLinearDamper element for convenience
+ in chaining set methods together.
+
+ Changing the damping constant invalidates Stage::Dynamics and above in the
+ \a state since it can affect force generation.
+ @see setDefaultDamping(), getDamping() **/
+ const MobilityLinearDamper& setDamping(State& state,
+ Real damping) const;
+
+ /** Return the value for the damping constant c that is stored in
+ the given \a state. Note that this is not the same thing as the default
+ damping constant that was supplied on construction or in
+ setDefaultDamping().
+ @see setDamping(), getDefaultDamping() **/
+ Real getDamping(const State& state) const;
+
+ /** @name Deprecated
+ Methods here are for backwards compatibility but have been replaced with
+ better ones that you should use. **/
+ /**@{**/
+ /** Deprecated: Alternate signature for backwards compatibilty -- for
+ safety you should prefer using the other constructor signature that
+ takes a MobilizerUIndex rather than a plain int. **/
+ MobilityLinearDamper(GeneralForceSubsystem& forces,
+ const MobilizedBody& mobod,
+ int whichU,
+ Real defaultDamping)
+ { // Invoke the other constructor.
+ new(this) MobilityLinearDamper(forces, mobod, MobilizerUIndex(whichU),
+ defaultDamping);
+ }
+ /**@}**/
+
+ /** @cond **/
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(MobilityLinearDamper,
+ MobilityLinearDamperImpl, Force);
+ /** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_FORCE_MOBILITY_LINEAR_DAMPER_H_
diff --git a/Simbody/include/simbody/internal/Force_MobilityLinearSpring.h b/Simbody/include/simbody/internal/Force_MobilityLinearSpring.h
new file mode 100644
index 0000000..a5a7efd
--- /dev/null
+++ b/Simbody/include/simbody/internal/Force_MobilityLinearSpring.h
@@ -0,0 +1,186 @@
+#ifndef SimTK_SIMBODY_FORCE_MOBILITY_LINEAR_SPRING_H_
+#define SimTK_SIMBODY_FORCE_MOBILITY_LINEAR_SPRING_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-13 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Force.h"
+
+/** @file
+This contains the user-visible API ("handle" class) for the SimTK::Force
+subclass Force::MobilityLinearSpring and is logically part of Force.h. The file
+assumes that Force.h will have included all necessary declarations. **/
+
+namespace SimTK {
+
+/** A linear spring that acts along or around a mobility coordinate to apply
+a generalized force there.
+
+The stiffness k is provided, along with an arbitrary "zero" coordinate value
+q0 at which the spring generates no force. The generated force is k*(q-q0), and
+potential energy is pe = 1/2 k (q-q0)^2.
+
+ at bug This is not meaningful unless the mobilizer coordinates are defined such
+that qdot=u. In particular, do not use this on coordinates for Ball or Free
+mobilizers. You can often replace those with Gimbal or Bushing mobilizers for
+which qdot=u holds, or split up the mobilizer into simpler components separated
+by massless bodies. **/
+class SimTK_SIMBODY_EXPORT Force::MobilityLinearSpring : public Force {
+public:
+ /** Create a %MobilityLinearSpring force element on a particular generalized
+ coordinate.
+
+ @param[in,out] forces
+ The subsystem to which this force should be added.
+ @param[in] mobod
+ Mobilizer to which the force should be applied.
+ @param[in] whichQ
+ To which of the mobilizer's generalized coordinates q should this
+ force be applied (first is 0)?
+ @param[in] defaultStiffness
+ The default value for the spring constant k.
+ @param[in] defaultQZero
+ The default for the value of the coordinate q0 at which the force is 0.
+ **/
+ MobilityLinearSpring(GeneralForceSubsystem& forces,
+ const MobilizedBody& mobod,
+ MobilizerQIndex whichQ,
+ Real defaultStiffness,
+ Real defaultQZero);
+
+ /** Default constructor creates an empty handle that can be assigned to
+ refer to any %MobilityLinearSpring object. **/
+ MobilityLinearSpring() {}
+
+ /** Provide a new value for the default stiffness k of this spring.
+ This is a topological change because it affects the value that the
+ containing System's default state will have when realizeTopology() is
+ called. This is for use during construction, not for during a simulation
+ where you should be using setStiffness() to set the stiffness in a State
+ rather than in the System.
+ @param[in] defaultStiffness
+ The default value for the spring constant k.
+ @return
+ A writable reference to this modified force element for convenience in
+ chaining set methods.
+ @see getDefaultStiffness(), setStiffness() **/
+ MobilityLinearSpring& setDefaultStiffness(Real defaultStiffness);
+
+
+ /** Provide a new value for the zero position q0 of this spring, at which
+ position the spring force will be zero.
+ This is a topological change because it affects the value that the
+ containing System's default state will have when realizeTopology() is
+ called. This is for use during construction, not for during a simulation
+ where you should be using setQZero() to set the zero position in a State
+ rather than in the System.
+ @param[in] defaultQZero
+ The default for the value of the coordinate at which the force is 0.
+ @return
+ A writable reference to this modified force element for convenience in
+ chaining set methods.
+ @see getDefaultQZero(), setQZero() **/
+ MobilityLinearSpring& setDefaultQZero(Real defaultQZero);
+
+ /** Return the default value for the spring's stiffness k. This is
+ normally set at construction but can be modified with setDefaultStiffness().
+ @see setDefaultStiffness(), getStiffness() **/
+ Real getDefaultStiffness() const;
+ /** Return the default value for the spring's zero position q0. This is
+ normally set at construction but can be modified with setDefaultQZero().
+ @see setDefaultQZero(), getQZero() **/
+ Real getDefaultQZero() const;
+
+ /** Change the value of the spring stiffness in the given \a state; this may
+ differ from the default value supplied at construction.
+ @param[in,out] state
+ The State in which the stiffness is to be changed.
+ @param[in] stiffness
+ The new stiffness k (>= 0) that overrides the default.
+ @return
+ A const reference to this %MobilityLinearSpring element for convenience
+ in chaining set methods together.
+
+ Changing the spring stiffness invalidates Stage::Dynamics and above in the
+ \a state since it can affect force generation.
+ @see setDefaultStiffness(), getStiffness() **/
+ const MobilityLinearSpring& setStiffness(State& state,
+ Real stiffness) const;
+
+ /** Change the value of the spring zero length in the given \a state; this
+ may differ from the default value supplied at construction.
+ @param[in,out] state
+ The State in which the zero length is to be changed.
+ @param[in] qZero
+ The value of the controlled coordinate q at which the generated spring
+ force should be zero. This overrides the default.
+ @return
+ A const reference to this %MobilityLinearSpring element for convenience
+ in chaining set methods together.
+
+ Changing the spring stiffness invalidates Stage::Dynamics and above in the
+ \a state since it can affect force generation.
+ @see setDefaultStiffness(), getStiffness() **/
+ const MobilityLinearSpring& setQZero(State& state,
+ Real qZero) const;
+
+ /** Return the value for the spring's stiffness k that is stored in
+ the given \a state. Note that this is not the same thing as the default
+ stiffness that was supplied on construction or in setDefaultStiffness().
+ @see setStiffness(), getDefaultStiffness() **/
+ Real getStiffness(const State& state) const;
+
+ /** Return the value for the spring zero position q0 that is stored in
+ the given \a state. Note that this is not the same thing as the default
+ q0 that was supplied on construction or in setDefaultQZero().
+ @see setQZero(), getDefaultQZero() **/
+ Real getQZero(const State& state) const;
+
+ /** @name Deprecated
+ Methods here are for backwards compatibility but have been replaced with
+ better ones that you should use. **/
+ /**@{**/
+ /** Deprecated: Alternate signature for backwards compatibilty -- for
+ safety you should prefer using the other constructor signature that
+ takes a MobilizerQIndex rather than a plain int. **/
+ MobilityLinearSpring(GeneralForceSubsystem& forces,
+ const MobilizedBody& mobod,
+ int whichQ,
+ Real defaultStiffness,
+ Real defaultQZero)
+ { // Invoke the other constructor.
+ new(this) MobilityLinearSpring(forces, mobod, MobilizerQIndex(whichQ),
+ defaultStiffness, defaultQZero);
+ }
+ /**@}**/
+
+ /** @cond **/
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(MobilityLinearSpring,
+ MobilityLinearSpringImpl, Force);
+ /** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_FORCE_MOBILITY_LINEAR_SPRING_H_
diff --git a/Simbody/include/simbody/internal/Force_MobilityLinearStop.h b/Simbody/include/simbody/internal/Force_MobilityLinearStop.h
new file mode 100644
index 0000000..2c00346
--- /dev/null
+++ b/Simbody/include/simbody/internal/Force_MobilityLinearStop.h
@@ -0,0 +1,224 @@
+#ifndef SimTK_SIMBODY_FORCE_MOBILITY_LINEAR_STOP_H_
+#define SimTK_SIMBODY_FORCE_MOBILITY_LINEAR_STOP_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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Force.h"
+
+/** @file
+This contains the user-visible API ("handle" class) for the SimTK::Force
+subclass Force::MobilityLinearStop and is logically part of Force.h. The file
+assumes that Force.h will have included all necessary declarations. **/
+
+namespace SimTK {
+
+/** Model a compliant stop element that acts to keep a mobilizer coordinate q
+within specified bounds. This force element generates no force when the
+coordinate is in bounds, but when either the lower or upper bound is exceeded
+it generates a generalized force opposing further violation of the bound. The
+generated force is composed of a stiffness force (or torque, or generalized
+force) that is linear in the violation of the bound, and dissipation that is
+linear in the rate qdot.
+
+ at bug MobilityLinearStop currently works only for coordinates q whose time
+derivatives qdot are just the corresponding generalized speed u. That is the
+case for translational mobilities, pin, universal, and gimbal mobilizers but
+not free or ball mobilizers.
+
+<h3>Theory:</h3>
+Given a mobilizer coordinate q and limits q_low and q_high, the generalized
+force generated here is:
+<pre>
+ { 0, q_low <= q <= q_high
+ f = { min(0, -k*x*(1+d*qdot)), q > q_high, x=q-q_high (x>0, f<=0)
+ { max(0, -k*x*(1-d*qdot)), q < q_low, x=q-q_low (x<0, f>=0)
+</pre>
+where k is a stiffness parameter, and d is a dissipation coefficient.
+
+Note that dissipation occurs both during compression and expansion, but we
+will never generate a "sticking" force. This is a Hunt and Crossley-like
+dissipation model. It has the nice property that the damping force is zero
+when you first touch the stop.
+**/
+class SimTK_SIMBODY_EXPORT Force::MobilityLinearStop : public Force {
+public:
+ /** Create a %MobilityLinearStop force element on a particular generalized
+ coordinate.
+
+ @param forces subsystem to which this force element should be added
+ @param mobod mobilizer to which the force should be applied
+ @param whichQ to which of the mobilizer's generalized coordinates q
+ should this force be applied (first is 0)?
+ @param defaultStiffness default stop stiffness (>= 0)
+ @param defaultDissipation default stop dissipation coefficient (>= 0)
+ @param defaultQLow default lower bound (-Infinity)
+ @param defaultQHigh default upper bound (+Infinity)
+
+ Note that if you have an integer value for the generalized coordinate (q)
+ index, you have to cast it to a MobilizerQIndex here. The generalized
+ coordinates are numbered starting with 0 for each mobilizer. Here is an
+ example: @code
+ GeneralForceSubsystem forces;
+ MobilizedBody::Pin pinJoint(...);
+ MobilityLinearStop myStop(forces, pinJoint, MobilizerQIndex(0),
+ k, d, -Pi/4, Pi/4);
+ @endcode
+ **/
+ MobilityLinearStop(GeneralForceSubsystem& forces,
+ const MobilizedBody& mobod,
+ MobilizerQIndex whichQ,
+ Real defaultStiffness,
+ Real defaultDissipation,
+ Real defaultQLow =-Infinity,
+ Real defaultQHigh= Infinity);
+
+ /** Default constructor creates an empty handle that can be assigned to
+ refer to any %MobilityLinearStop object. **/
+ MobilityLinearStop() {}
+
+ /** Provide new values for the default lower and upper bounds of this stop.
+ This is a topological change because it affects the value that the
+ containing System's default state will have when realizeTopology() is
+ called. This is for use during construction, not for during a simulation
+ where you should be using setBounds() to set the bounds in a State rather
+ than in the System.
+
+ @param defaultQLow
+ Default lower bound (generalized coordinate value); <= defaultQHigh.
+ Set to <code>-Infinity</code> to disable the lower bound by default.
+ @param defaultQHigh
+ Default upper bound (generalized coordinate value); >= defaultQLow.
+ Set to <code>Infinity</code> to disable the upper bound by default.
+
+ @returns a writable reference to this modified force element
+ @see setBounds(), getDefaultLowerBound(), getDefaultUpperBound() **/
+ MobilityLinearStop& setDefaultBounds(Real defaultQLow, Real defaultQHigh);
+
+
+ /** Provide new values for the default material properties of this stop,
+ which are assumed to be the same for the upper and lower stops. This is
+ a topological change because it affects the value that the containing
+ System's default state will have when realizeTopology() is called. This is
+ for use during construction, not for during a simulation where you should
+ be using setMaterialProperties() to set the material properties in a State
+ rather than in the System.
+
+ @param defaultStiffness default stop stiffness (>= 0)
+ @param defaultDissipation default stop dissipation coefficient (>= 0)
+
+ @returns a writable reference to this modified force element
+ @see setMaterialProperties(), getDefaultStiffness(),
+ getDefaultDissipation() **/
+ MobilityLinearStop& setDefaultMaterialProperties
+ (Real defaultStiffness, Real defaultDissipation);
+
+ /** Return the default value for the stop's lower bound (a generalized
+ coordinate value). This is normally set at construction.
+ @see getDefaultUpperBound(), getLowerBound() **/
+ Real getDefaultLowerBound() const;
+
+ /** Return the default value for the stop's upper bound (a generalized
+ coordinate value). This is normally set at construction.
+ @see getDefaultLowerBound(), getUpperBound() **/
+ Real getDefaultUpperBound() const;
+
+ /** Return the default value for the stop material's stiffness k. This is
+ normally set at construction.
+ @see getDefaultDissipation(), getStiffness() **/
+ Real getDefaultStiffness() const;
+
+ /** Return the default value for the stop's material's dissipation
+ coefficient d. This is normally set at construction.
+ @see getDefaultStiffness(), getDissipation() **/
+ Real getDefaultDissipation() const;
+
+ /** Change the values of the lower and upper bounds in the given \a state;
+ these may differ from the default values supplied at construction.
+
+ @param state
+ The State in which the bounds are changed.
+ @param qLow
+ Lower bound (generalized coordinate value); <= qHigh. Set to
+ \c -Infinity to disable the lower bound.
+ @param qHigh
+ Upper bound (generalized coordinate value); >= qLow. Set to
+ \c Infinity to disable the upper bound.
+
+ Changing these bounds invalidates Stage::Dynamics and above in the \a state
+ since it can affect force generation.
+ @see setDefaultBounds(), getLowerBound(), getUpperBound() **/
+ void setBounds(State& state, Real qLow, Real qHigh) const;
+
+ /** Change the values of this stop's material properties in the given
+ \a state; these may differ from the default values supplied at construction.
+
+ @param state
+ The State in which the material properties are changed.
+ @param stiffness
+ The stop material stiffness k (>= 0).
+ @param dissipation
+ The stop material dissipation coefficient d (>= 0).
+
+ Changing these properties invalidates Stage::Dynamics and above in the
+ \a state since it can affect force generation.
+ @see setDefaultMaterialProperties(), getStiffness(), getDissipation() **/
+ void setMaterialProperties
+ (State& state, Real stiffness, Real dissipation) const;
+
+ /** Return the value for the lower bound that is stored in the
+ given \a state. Note that this is not the same thing as the default lower
+ bound that was supplied on construction.
+ @see getUpperBound(), setBounds(), getDefaultLowerBound() **/
+ Real getLowerBound(const State& state) const;
+
+ /** Return the value for the upper bound that is stored in the
+ given \a state. Note that this is not the same thing as the default upper
+ bound that was supplied on construction.
+ @see getLowerBound(), setBounds(), getDefaultUpperBound() **/
+ Real getUpperBound(const State& state) const;
+
+ /** Return the value for the stop material's stiffness k that is stored in
+ the given \a state. Note that this is not the same thing as the default
+ stiffness that was supplied on construction.
+ @see getDissipation(), setMaterialProperties(), getDefaultStiffness() **/
+ Real getStiffness(const State& state) const;
+
+ /** Return the value for the stop material's dissipation coefficient d that
+ is stored in the given \a state. Note that this is not the same thing as the
+ default dissipation coefficient that was supplied on construction.
+ @see getStiffness(), setMaterialProperties(), getDefaultDissipation() **/
+ Real getDissipation(const State& state) const;
+
+ /** @cond **/
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(MobilityLinearStop,
+ MobilityLinearStopImpl, Force);
+ /** @endcond **/
+};
+
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_FORCE_MOBILITY_LINEAR_STOP_H_
diff --git a/Simbody/include/simbody/internal/Force_Thermostat.h b/Simbody/include/simbody/internal/Force_Thermostat.h
new file mode 100644
index 0000000..9f3d8d6
--- /dev/null
+++ b/Simbody/include/simbody/internal/Force_Thermostat.h
@@ -0,0 +1,292 @@
+#ifndef SimTK_SIMBODY_FORCE_THERMOSTAT_H_
+#define SimTK_SIMBODY_FORCE_THERMOSTAT_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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Christopher Bruns *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * This contains the user-visible API ("handle" class) for the SimTK::Force
+ * subclass Force::Thermostat and is logically part of Force.h. The file
+ * assumes that Force.h will have included all necessary declarations.
+ */
+
+#include "SimTKcommon.h"
+#include "simbody/internal/Force.h"
+
+namespace SimTK {
+
+/**
+ * This is a feedback-controlled force that uses Nose'-Hoover chains to
+ * maintain a particular temperature Tb, as though the system were immersed in
+ * an infinite heat bath at that temperature. There are two parameters, the
+ * temperature Tb and a "relaxation time" t which controls how tightly the
+ * temperature is maintained. This thermostat is particularly useful in
+ * molecular simulations but can be applied to any mechanical system also.
+ *
+ * Temperature is defined here as T = (2*KE) / (N*kB) where KE is the system
+ * kinetic energy, N is the number of coupled degrees of freedom (mobilities
+ * minus active, nonredundant constraints, minus up to 6 rigid body dofs for
+ * the system as a whole), and kB is Boltzmann's constant in appropriate units.
+ *
+ * We use a Nose'-Hoover chain to achieve excellent statistical mechanics
+ * properties with a continuous force. At equilibrium the temperature will have
+ * a Boltzmann distribution; the relaxation time controls how long it takes the
+ * system to reach equilibrium with the bath. Smaller values of relaxation time
+ * produce faster response but can make the system stiff and will normally
+ * require smaller step sizes; larger values will take longer to equilibrate
+ * but will run faster.
+ *
+ * This Force does not produce any potential energy. However, there is a "bath
+ * energy" available through a separate call which can be used in combination
+ * with the system energy to construct a conserved quantity; this is described
+ * further below.
+ *
+ * \par Theory:
+ *
+ * The current system temperature is defined
+ * <pre>
+ * T = (2*KE) / (N*kB)
+ * </pre>
+ * where KE is the kinetic energy of the moving bodies whose N degrees of
+ * freedom are being controlled (not necessarily all the bodies in the system),
+ * and kB is Boltzmann's constant. Our goal here is to control T so that it
+ * follows a Boltzmann distribution around the specified bath temperature Tb.
+ *
+ * For an m-chain Nose'-Hoover chain, we will define m auxiliary "thermostat"
+ * state variables c[i], 0<=i<m, with units of 1/time. The 0'th thermostat
+ * variable c[0] is used to generate a generalized force f applied to the
+ * system mobilities u:
+ * <pre>
+ * f = -c[0] * M * u
+ * </pre>
+ * where M is the system mass matrix and u is the vector of generalized speeds.
+ * (Note that in Simbody the M*u product is formed in O(n) time; M itself
+ * is never formed.) The c variables should be initialized to zero at the
+ * start of a simulation. Ideally, you should initialize the u's so that they
+ * are already at the right temperature, but if not you should still make them
+ * non-zero -- you can see above that if you have no velocities you will get
+ * no Nose'-Hoover forces.
+ *
+ * If m==1, we have the standard Nose'-Hoover method except with a relaxation
+ * time specified instead of the thermal mass parameter, as in reference [2]:
+ * <pre>
+ * cdot[0] = (T/Tb - 1)/t^2
+ * </pre>
+ * Otherwise, for m > 1 we have:
+ * <pre>
+ * cdot[0] = (T/Tb - 1)/t^2 - c[0]*c[1]
+ * cdot[1] = N*c[0]^2 - 1/t^2 - c[1]*c[2]
+ * cdot[i] = c[i-1]^2 - 1/t^2 - c[i]*c[i+1] (2<=i<m-1)
+ * cdot[m-1] = c[m-2]^2 - 1/t^2
+ * </pre>
+ * For comparison with the literature where thermal mass parameters Qi are
+ * used, we use Q0 = N kB Tb t^2 and Qi = kB Tb t^2, i > 0. That is, the first
+ * thermostat that controls the N thermal degrees of freedom is N times
+ * "heavier" than the subsequent ones, each of which controls only the one
+ * dof of its next-lower thermostat. See refs [1] and [2].
+ *
+ * In addition there is a set of state variables si given by sdot[i]=c[i].
+ * Together these permit us to define a "bath energy" which can be combined
+ * with system energy to produce a conserved quantity. Bath energy is KEb+PEb
+ * where
+ * <pre>
+ * KEb = 1/2 kB Tb t^2 (N c[0]^2 + sum(c[i]^2))
+ * PEb = kB Tb (N s[0] + sum(s[i]))
+ * </pre>
+ * where kB is Boltzmann's constant, Tb the bath temperature, N the number of
+ * thermal degrees of freedom in the temperature definition, and the sums run
+ * from 1 to m-1. Note that you must request the bath energy separately; we do
+ * not return any potential energy for this force otherwise.
+ *
+ * \par References:
+ *
+ * [1] Martyna, GJ; Klien, ML; Tuckerman, M. Nose'-Hoover chains:
+ * The canonical ensemble via continuous dynamics. J. Chem. Phys.
+ * 97(4):2635-2643 (1992).
+ *
+ * [2] Vaidehi, N; Jain, A; Goddard, WA. Constant Temperature
+ * Constrained Molecular Dynamics: The Newton-Euler Inverse Mass
+ * Operator Method. J. Phys. Chem. 100:10508-10517 (1996).
+ */
+class SimTK_SIMBODY_EXPORT Force::Thermostat : public Force {
+public:
+ /// Define a global thermostat (one that affects all degrees of freedom) at
+ /// a given default temperature and relaxation time. The number of
+ /// Nose'-Hoover chains is given a default value.
+ Thermostat(GeneralForceSubsystem& forces,
+ const SimbodyMatterSubsystem& matter,
+ Real boltzmannsConstant,
+ Real bathTemperature,
+ Real relaxationTime,
+ int numExcludedDofs = 6);
+
+
+ /// Default constructor creates an empty handle.
+ Thermostat() {}
+
+ /// TODO: not implemented yet. Remove a body from consideration in
+ /// the thermostat. Typically this would be the system base body so
+ /// that overall rigid body translation and orientation is not counted
+ /// as part of the temperature.
+ Thermostat& excludeMobilizedBody(MobilizedBodyIndex);
+
+ /// Set the default (state independent) number of Nose'-Hoover chains.
+ /// This is a Topology-stage change.
+ Thermostat& setDefaultNumChains(int numChains);
+
+ /// Set the default (state independent) bath temperature. This will be
+ /// interpreted using the value of Boltzmann's constant Kb provided on
+ /// construction. The units will be Kb/energy, typically Kelvins.
+ Thermostat& setDefaultBathTemperature(Real bathTemperature);
+
+ /// Set the default (state independent) relaxation time.
+ Thermostat& setDefaultRelaxationTime(Real relaxationTime);
+
+ /// Set the default number of system rigid body degrees of freedom (0-6)
+ /// to be excluded from the calculation of the number of thermal degrees
+ /// of freedom N; if you don't call this it is assumed that 6 dofs should
+ /// be excluded.
+ Thermostat& setDefaultNumExcludedDofs(int numExcludedDofs);
+
+ /// Get the initial value for the number of chains that will be used for
+ /// the "number of chains" State variable. A new value may be set in
+ /// a particular State.
+ int getDefaultNumChains() const;
+ /// Get the initial value for the bath temperature that will be use for
+ /// the "bath temperature" State variable. A new value may be set in
+ /// a particular State.
+ Real getDefaultBathTemperature() const;
+ /// Get the initial value for the bath temperature that will be use for
+ /// the "bath temperature" State variable.
+ Real getDefaultRelaxationTime() const;
+ /// Get the initial value for the number of system rigid body degrees
+ /// of freedom (0-6) to be excluded from the calculation of the number of
+ /// thermal degrees of freedom N. A new value may be set in a particular
+ /// State.
+ int getDefaultNumExcludedDofs() const;
+
+ /// Can't change the value of Boltzmann's constant after construction;
+ /// this is the value being used.
+ Real getBoltzmannsConstant() const;
+
+ /// Set the actual number of Nose'-Hoover chains to be used. This variable
+ /// controls the number of auxiliary state variables allocated by the
+ /// Thermostat so invalidates Model stage.
+ const Thermostat& setNumChains(State&, int numChains) const;
+ /// Set the bath temperature which serves as the target temperature for
+ /// the thermostat. This is given in units defined by the value of
+ /// Boltzmann's constant (which has units of energy/temperature) that was
+ /// set on construction. This sets an Instance-stage state variable so
+ /// invalidates Instance and higher stages in the given State.
+ const Thermostat& setBathTemperature(State&, Real Tb) const;
+ /// Set the relaxation time which determines how long the system will
+ /// take to equilibrate to the bath temperature. This sets an
+ /// Instance-stage state variable so invalidates Instance and higher
+ /// stages in the given State.
+ const Thermostat& setRelaxationTime(State&, Real t) const;
+ /// Set the actual number of system rigid body degrees of freedom (0-6)
+ /// to be excluded from the calculation of the number of thermal degrees
+ /// of freedom N. This sets an Instance-stage state variable so invalidates
+ /// Instance and higher stages in the given State.
+ const Thermostat& setNumExcludedDofs(State&, int numExcludedDofs) const;
+
+ /// Obtain the current number of Nose'-Hoover chains in use. This is a
+ /// state variable so can be obtained any time after realization of
+ /// the Model stage.
+ int getNumChains(const State&) const;
+ /// Obtain the current bath temperature, in units which are determined
+ /// by the value of Boltzmann's constant that was supplied on construction
+ /// of this Thermostat force element. This is a state variable so can
+ /// be obtained any time after realization of the Model stage.
+ Real getBathTemperature(const State&) const;
+ /// Obtain the current relaxation time. This is a state variable so can
+ /// be obtained any time after realization of the Model stage.
+ Real getRelaxationTime(const State&) const;
+ /// Get the current value for the number of system rigid body degrees
+ /// of freedom (0-6) to be excluded from the calculation of the number of
+ /// thermal degrees of freedom N. This is a state variable so can
+ /// be obtained any time after realization of the Model stage.
+ int getNumExcludedDofs(const State&) const;
+
+ /// Return the number of thermal degrees of freedom being used in the
+ /// definition of temperature for this thermostat. This is the net of the
+ /// total number of mobilities minus nonredundant constraints minus
+ /// the number of excluded system rigid body degrees of freedom (0-6).
+ int getNumThermalDofs(const State&) const;
+
+ /// Return the temperature of the controlled degrees of freedom via the
+ /// definition T = 2*ke / (N*Kb) where N is the number of thermal degrees
+ /// of freedom. You can call this after Stage::Velocity has been realized.
+ Real getCurrentTemperature(const State&) const;
+
+ /// This is a solver that initializes thermostat state variables to zero.
+ void initializeChainState(State&) const;
+ /// Set the thermostat state variables to particular values. The Vector's
+ /// length must be the same as twice the current number of chains called
+ /// for by the State.
+ void setChainState(State&, const Vector&) const;
+
+ /// Return the current values of the thermostat chain variables. The
+ /// returned vector will have twice the length that getNumChains() would
+ /// return if called on this same State.
+ Vector getChainState(const State&) const;
+
+ /// Calculate the total "bath energy" which, when added to the system
+ /// energy, should yield a conserved quantity (assuming all other forces
+ /// are conservative).
+ Real calcBathEnergy(const State& state) const;
+
+ /// Get the amount of power being applied by the thermostat to the
+ /// system; sign is positive when energy is coming from the bath.
+ Real getExternalPower(const State& state) const;
+
+ /// Get the amount of work that has been done by the bath on the
+ /// system since an arbitrary start time.
+ Real getExternalWork(const State& state) const;
+
+ /// Set the current value of the work done by the bath to an
+ /// arbitrary value; normally zero for initialization.
+ void setExternalWork(State& state, Real work) const;
+
+ /// Set the controlled system to a set of randomized velocities which
+ /// yields the bath temperature. This ignores the current system velocities.
+ /// TODO: not implemented yet.
+ void initializeSystemToBathTemperature(State&) const;
+
+ /// Set the controlled system to a set of randomized velocities which
+ /// yields a particular temperature. This ignores the current system
+ /// velocities. The temperature is interpreted using the value of
+ /// Boltzmann's constant that was provided on construction of this
+ /// Thermostat. TODO: not implemented yet.
+ void setSystemToTemperature(State&, Real T) const;
+
+ // Don't show this in Doxygen.
+ /// @cond
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Thermostat, ThermostatImpl, Force);
+ /// @endcond
+};
+
+} // namespace SimTK
+
+
+#endif // SimTK_SIMBODY_FORCE_THERMOSTAT_H_
diff --git a/Simbody/include/simbody/internal/GeneralContactSubsystem.h b/Simbody/include/simbody/internal/GeneralContactSubsystem.h
new file mode 100644
index 0000000..78a699e
--- /dev/null
+++ b/Simbody/include/simbody/internal/GeneralContactSubsystem.h
@@ -0,0 +1,133 @@
+#ifndef SimTK_SIMBODY_GENERAL_CONTACT_SUBSYSTEM_H_
+#define SimTK_SIMBODY_GENERAL_CONTACT_SUBSYSTEM_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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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"
+
+namespace SimTK {
+
+class MultibodySystem;
+class MobilizedBody;
+class ContactGeometry;
+
+/**
+ * This class performs collision detection for use in contact modeling. It manages sets
+ * of bodies that can potentially interact with each other. At each time step, it identifies
+ * all the contacts between them. It does not directly affect the behavior of the system
+ * in any way. Instead, it simply provides a service that can be used by other classes
+ * to implement forces, events, contraints, etc. that are based on contacts between bodies.
+ *
+ * To use this class, first create a "contact set" by calling createContactSet(). A contact
+ * set is a group of bodies which can interact with each other. If you create multiple contact
+ * sets, the bodies within each one will be checked for contacts with each other, but bodies in
+ * different sets will not interact.
+ *
+ * Next, add bodies to the contact set. Each one is represented by a ContactGeometry object
+ * that describes its shape, the MoblizedBody it is attached to, and a Transform giving the location
+ * of the geometry in the MobilizedBody's reference frame.
+ *
+ * Finally, call getContacts() to get a list of all contacts which exist between bodies in a
+ * contact set. Each Contact specifies two bodies that overlap, along with a description of the
+ * contact point, such as its location and normal vector.
+ */
+
+class SimTK_SIMBODY_EXPORT GeneralContactSubsystem : public Subsystem {
+public:
+ GeneralContactSubsystem();
+ explicit GeneralContactSubsystem(MultibodySystem&);
+ /**
+ * Create a new contact set. The return value is an index which can be used to refer to the
+ * contact set when calling other methods.
+ */
+ ContactSetIndex createContactSet();
+ /**
+ * Get the total number of contact sets that have been created.
+ */
+ int getNumContactSets() const;
+ /**
+ * Add a body to a contact set.
+ *
+ * @param index the index of the contact set the body should be added to
+ * @param body the MobilizedBody whose reference frame the body is attached to
+ * @param geom a ContactGeometry describing the shape of the body
+ * @param transform the location and orientation of the ContactGeometry in the MobilizedBody's reference frame
+ */
+ void addBody(ContactSetIndex index, const MobilizedBody& body, const ContactGeometry& geom, Transform transform);
+ /**
+ * Get the number of bodies in a contact set.
+ */
+ int getNumBodies(ContactSetIndex set) const;
+ /**
+ * Get the MobilizedBody in whose reference frame a body is defined.
+ *
+ * @param set the contact set the body belongs to
+ * @param index the index of the body within the contact set
+ */
+ const MobilizedBody& getBody(ContactSetIndex set, ContactSurfaceIndex index) const;
+ /**
+ * Get the ContactGeometry which defines the shape of a body.
+ *
+ * @param set the contact set the body belongs to
+ * @param index the index of the body within the contact set
+ */
+ const ContactGeometry& getBodyGeometry(ContactSetIndex set, ContactSurfaceIndex index) const;
+ /**
+ * Get a mutable reference to the ContactGeometry which defines the shape of a body.
+ *
+ * @param set the contact set the body belongs to
+ * @param index the index of the body within the contact set
+ */
+ ContactGeometry& updBodyGeometry(ContactSetIndex set, ContactSurfaceIndex index);
+ /**
+ * Get the location and orientation of a body.
+ *
+ * @param set the contact set the body belongs to
+ * @param index the index of the body within the contact set
+ */
+ const Transform& getBodyTransform(ContactSetIndex set, ContactSurfaceIndex index) const;
+ /**
+ * Get a mutable reference to the location and orientation of a body.
+ *
+ * @param set the contact set the body belongs to
+ * @param index the index of the body within the contact set
+ */
+ Transform& updBodyTransform(ContactSetIndex set, ContactSurfaceIndex index);
+ /**
+ * Get a list of all contacts between bodies in a contact set. Contacts are calculated at
+ * Dynamics stage, so the state must have been realized to at least Dynamics stage. This
+ * subsystem is guaranteed to be realized before all ForceSubsystems, however, so a Force
+ * may still invoke it to calculate forces based on contacts.
+ */
+ const Array_<Contact>& getContacts(const State& state, ContactSetIndex set) const;
+ SimTK_PIMPL_DOWNCAST(GeneralContactSubsystem, Subsystem);
+private:
+ class GeneralContactSubsystemImpl& updImpl();
+ const GeneralContactSubsystemImpl& getImpl() const;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_GENERAL_CONTACT_SUBSYSTEM_H_
diff --git a/Simbody/include/simbody/internal/GeneralForceSubsystem.h b/Simbody/include/simbody/internal/GeneralForceSubsystem.h
new file mode 100644
index 0000000..ef16617
--- /dev/null
+++ b/Simbody/include/simbody/internal/GeneralForceSubsystem.h
@@ -0,0 +1,108 @@
+#ifndef SimTK_SIMBODY_GENERAL_FORCE_ELEMENTS_H_
+#define SimTK_SIMBODY_GENERAL_FORCE_ELEMENTS_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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "simbody/internal/common.h"
+#include "simbody/internal/ForceSubsystem.h"
+
+#include <cassert>
+
+namespace SimTK {
+
+class MultibodySystem;
+class SimbodyMatterSubsystem;
+class Force;
+
+/** This is a concrete subsystem which can apply arbitrary forces to a
+MultibodySystem. Each force element is represented by a Force object. For
+example, to add a spring between two bodies, you would write
+ at code
+ GeneralForceSubsystem forces(system);
+ // ...
+ Force::TwoPointLinearSpring(forces, body1, station1, body2, station2, k, x0);
+ at endcode
+**/
+class SimTK_SIMBODY_EXPORT GeneralForceSubsystem : public ForceSubsystem {
+public:
+ GeneralForceSubsystem();
+ explicit GeneralForceSubsystem(MultibodySystem&);
+
+ /** Attach a new force to this subsystem. The subsystem takes over
+ ownership of the force, leaving the passed in handle as a reference to
+ it. This is normally called by the Force handle constructor.
+ @param force
+ A writable reference to a Force handle whose referenced force element
+ is not yet owned by any subsystem. We will take over ownership of the
+ ForceImpl implementation objected referenced by the handle, bumping the
+ reference count and leaving the reference in place so that the original
+ handle can continue to be used to reference and modify the force
+ element.
+ @return A unique integer index for the adopted force element that can be
+ used to retrieve this force element from the subsystem later if needed. **/
+ ForceIndex adoptForce(Force& force);
+
+ /** Get the number of force elements which have been added to this
+ Subsystem. Legal ForceIndex values range from 0 to getNumForces()-1. **/
+ int getNumForces() const;
+
+ /** Get a const reference to a force element by index. **/
+ const Force& getForce(ForceIndex index) const;
+
+ /** Get a writable reference to a force element by index. **/
+ Force& updForce(ForceIndex index);
+
+ /** Find out whether a particular force element contained in this
+ subsystem is currently disabled in the given state. **/
+ bool isForceDisabled(const State& state, ForceIndex index) const;
+
+ /** Disable or enable a particular force element contained in this
+ subsystem. This can usually be done more conveniently through the Force
+ handle's disable() and enable() methods. Note that although force elements
+ are normally enabled when created, it is possible that the force element
+ will have been constructed to be disabled by default in which case it must
+ be explicitly enabled. **/
+ void setForceIsDisabled
+ (State& state, ForceIndex index, bool shouldBeDisabled) const;
+
+ /** Every Subsystem is owned by a System; a GeneralForceSubsystem expects
+ to be owned by a MultibodySystem. This method returns a const reference
+ to the containing MultibodySystem and will throw an exception if there is
+ no containing System or it is not a MultibodySystem. **/
+ const MultibodySystem& getMultibodySystem() const;
+
+ /** @cond **/ // don't show in Doxygen docs
+ SimTK_PIMPL_DOWNCAST(GeneralForceSubsystem, ForceSubsystem);
+ /** @endcond **/
+private:
+ // OBSOLETE; use getNumForces() instead.
+ int getNForces() const {return getNumForces();}
+
+ class GeneralForceSubsystemRep& updRep();
+ const GeneralForceSubsystemRep& getRep() const;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_GENERAL_FORCE_ELEMENTS_H_
diff --git a/Simbody/include/simbody/internal/HuntCrossleyContact.h b/Simbody/include/simbody/internal/HuntCrossleyContact.h
new file mode 100644
index 0000000..23d7874
--- /dev/null
+++ b/Simbody/include/simbody/internal/HuntCrossleyContact.h
@@ -0,0 +1,112 @@
+#ifndef SimTK_SIMBODY_HUNT_CROSSLEY_CONTACT_H_
+#define SimTK_SIMBODY_HUNT_CROSSLEY_CONTACT_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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * Define the public interface to HuntCrossleyContact, a subsystem which
+ * provides some minimal contact behavior.
+ */
+
+#include "SimTKcommon.h"
+
+#include "simbody/internal/common.h"
+#include "simbody/internal/ForceSubsystem.h"
+
+#include <cassert>
+
+namespace SimTK {
+
+class MultibodySystem;
+
+/**
+ * This is a concrete subsystem that handles simple, frictionless contact situations
+ * with a model due to Hunt & Crossley: K. H. Hunt and F. R. E. Crossley,
+ * "Coefficient of Restitution Interpreted as Damping in Vibroimpact,"
+ * ASME Journal of Applied Mechanics, pp. 440-445, June 1975. This is
+ * a continuous model based on Hertz elastic contact theory,
+ * which correctly reproduces the empirically observed dependence
+ * on velocity of coefficient of restitution, where e=(1-cv) for
+ * (small) impact velocity v and a material property c with units 1/v. Note that
+ * c can be measured right off the coefficient of restitution-vs.-velocity
+ * curves: it is the absolute value of the slope at low velocities.
+ *
+ * Given a collision between two spheres, or a sphere and a plane,
+ * we can generate a contact force from this equation
+ * f = kx^n(1 + 3/2 cv)
+ * where k is a stiffness constant incorporating material properties
+ * and geometry (to be defined below), x is penetration depth and
+ * v = dx/dt is penetration rate (positive during penetration and
+ * negative during rebound). Exponent n depends on the surface
+ * geometry. For Hertz contact where the geometry can be approximated
+ * by sphere (or sphere-plane) interactions, which is all we are
+ * currently handling here, n=3/2.
+ *
+ * Stiffness k is defined in terms of the relative radius of curvature R and
+ * effective plane-strain modulus E, each of which is a combination of
+ * the description of the two individual contacting elements. TODO:
+ * derivation of the following results should be in the SimTK Engr J; you'll
+ * have to take my word for it now:
+ *
+ * R1*R2 E2^(2/3)
+ * R = -------, E = (s1 * E1^(2/3))^(3/2), s1= -------------------
+ * R1 + R2 E1^(2/3) + E2^(2/3)
+ *
+ * c = c1*s1 + c2*(1-s1)
+ * k = (4/3) sqrt(R) E
+ * f = k x^(3/2) (1 + 3/2 c xdot)
+ * pe = 2/5 k x^(5/2)
+ * Also, we can calculate the contact patch radius a as
+ * a = sqrt(R*x)
+ *
+ * In the above, E1 and E2 are the *plane strain* moduli. If you have instead
+ * Young's modulus Y1 and Poisson's ratio p1, then E1=Y1/(1-p1^2). The interface
+ * to this subsystem asks for E1 (pressure/%strain) and c1 (1/velocity), and
+ * E2,c2 only.
+ *
+ */
+class SimTK_SIMBODY_EXPORT HuntCrossleyContact : public ForceSubsystem {
+public:
+ HuntCrossleyContact();
+ explicit HuntCrossleyContact(MultibodySystem&);
+
+ int addSphere(MobilizedBodyIndex body, const Vec3& center,
+ const Real& radius,
+ const Real& stiffness, // E (plane strain)
+ const Real& dissipation); // c (1/v)
+
+ int addHalfSpace(MobilizedBodyIndex body, const UnitVec3& normal,
+ const Real& height,
+ const Real& stiffness, // E (plane strain)
+ const Real& dissipation); // c (1/v)
+
+ SimTK_PIMPL_DOWNCAST(HuntCrossleyContact, ForceSubsystem);
+private:
+ class HuntCrossleyContactRep& updRep();
+ const HuntCrossleyContactRep& getRep() const;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_HUNT_CROSSLEY_CONTACT_H_
diff --git a/Simbody/include/simbody/internal/HuntCrossleyForce.h b/Simbody/include/simbody/internal/HuntCrossleyForce.h
new file mode 100644
index 0000000..e335d2f
--- /dev/null
+++ b/Simbody/include/simbody/internal/HuntCrossleyForce.h
@@ -0,0 +1,166 @@
+#ifndef SimTK_SIMBODY_HUNT_CROSSLEY_FORCE_H_
+#define SimTK_SIMBODY_HUNT_CROSSLEY_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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 GeneralContactSubsystem;
+class HuntCrossleyForceImpl;
+
+/**
+ * This class models the forces generated by simple point contacts, such as between
+ * two spheres, or a sphere and a half space. This includes components for the normal
+ * restoring force, dissipation in the material, and surface friction. This force
+ * is only applied to point contacts. Other contacts, such as those involving triangle
+ * meshes, are ignored.
+ *
+ * This class relies on a GeneralContactSubsystem to identify contacts, then applies
+ * forces to all contacts in a single contact set. To use it, do the following:
+ *
+ * <ol>
+ * <li>Add a GeneralContactSubsystem and GeneralForceSubsystem to a MultibodySystem.</li>
+ * <li>Create a contact set within the GeneralContactSubsystem, and add ContactGeometry::Sphere
+ * and ContactGeometry::HalfSpace objects to it.</li>
+ * <li>Add a HuntCrossleyForce to the GeneralForceSubsystem, and call setBodyParameters() on it
+ * once for each body in the contact set.</li>
+ * </ol>
+ *
+ * <h1>Normal Force Components</h1>
+ *
+ * The force in the normal direction is based on a model due to Hunt & Crossley: K. H. Hunt
+ * and F. R. E. Crossley, "Coefficient of Restitution Interpreted as Damping in Vibroimpact,"
+ * ASME Journal of Applied Mechanics, pp. 440-445, June 1975. This is
+ * a continuous model based on Hertz elastic contact theory,
+ * which correctly reproduces the empirically observed dependence
+ * on velocity of coefficient of restitution, where e=(1-cv) for
+ * (small) impact velocity v and a material property c with units 1/v. Note that
+ * c can be measured right off the coefficient of restitution-vs.-velocity
+ * curves: it is the absolute value of the slope at low velocities.
+ *
+ * Given a collision between two spheres, or a sphere and a plane,
+ * we can generate a contact force from this equation
+ * f = kx^n(1 + 3/2 cv)
+ * where k is a stiffness constant incorporating material properties
+ * and geometry (to be defined below), x is penetration depth and
+ * v = dx/dt is penetration rate (positive during penetration and
+ * negative during rebound). Exponent n depends on the surface
+ * geometry. For Hertz contact where the geometry can be approximated
+ * by sphere (or sphere-plane) interactions, which is all we are
+ * currently handling here, n=3/2.
+ *
+ * Stiffness k is defined in terms of the relative radius of curvature R and
+ * effective plane-strain modulus E, each of which is a combination of
+ * the description of the two individual contacting elements:
+ *
+ * <pre>
+ * R1*R2 E2^(2/3)
+ * R = -------, E = (s1 * E1^(2/3))^(3/2), s1= -------------------
+ * R1 + R2 E1^(2/3) + E2^(2/3)
+ * </pre>
+ *
+ * c = c1*s1 + c2*(1-s1)
+ * k = (4/3) sqrt(R) E
+ * f = k x^(3/2) (1 + 3/2 c xdot)
+ * pe = 2/5 k x^(5/2)
+ * Also, we can calculate the contact patch radius a as
+ * a = sqrt(R*x)
+ *
+ * In the above, E1 and E2 are the *plane strain* moduli. If you have instead
+ * Young's modulus Y1 and Poisson's ratio p1, then E1=Y1/(1-p1^2). The interface
+ * to this subsystem asks for E1 (pressure/%strain) and c1 (1/velocity), and
+ * E2,c2 only.
+ *
+ * <h1>Friction Force</h1>
+ *
+ * The friction force is based on a model by Michael Hollars:
+ *
+ * f = fn*[min(vs/vt,1)*(ud+2(us-ud)/(1+(vs/vt)^2))+uv*vs]
+ *
+ * where fn is the normal force at the contact point, vs is the slip (tangential)
+ * velocity of the two bodies at the contact point, vt is a transition velocity
+ * (see below), and us, ud, and uv are the coefficients of static, dynamic, and
+ * viscous friction respectively. Each of the three friction coefficients is calculated
+ * based on the friction coefficients of the two bodies in contact:
+ *
+ * u = 2*u1*u2/(u1+u2)
+ *
+ * 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. The default value of vt is 0.01.
+ */
+class SimTK_SIMBODY_EXPORT HuntCrossleyForce : public Force {
+public:
+ /**
+ * Create a Hunt-Crossley contact model.
+ *
+ * @param forces the subsystem which will own this HuntCrossleyForce element
+ * @param contacts the subsystem to which this contact model should be applied
+ * @param contactSet the index of the contact set to which this contact model will be applied
+ */
+ HuntCrossleyForce(GeneralForceSubsystem& forces,
+ GeneralContactSubsystem& contacts,
+ ContactSetIndex contactSet);
+ /**
+ * Set the material parameters for a surface in the contact set.
+ *
+ * @param surfIndex the index of the surface within the contact set
+ * @param stiffness the stiffness constant (k) for the body
+ * @param dissipation the dissipation coefficient (c) for the body
+ * @param staticFriction the coefficient of static friction (us) for the body
+ * @param dynamicFriction the coefficient of dynamic friction (ud) for the body
+ * @param viscousFriction the coefficient of viscous friction (uv) for the body
+ */
+ void setBodyParameters
+ (ContactSurfaceIndex surfIndex, Real stiffness, Real dissipation,
+ Real staticFriction, Real dynamicFriction, Real viscousFriction);
+ /**
+ * Get the transition velocity (vt) of the friction model.
+ */
+ Real getTransitionVelocity() const;
+ /**
+ * Set the transition velocity (vt) of the friction model.
+ */
+ void setTransitionVelocity(Real v);
+ /**
+ * Retrieve the ContactSetIndex that was associated with this
+ * %HuntCrossleyForce on construction.
+ */
+ ContactSetIndex getContactSetIndex() const;
+
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(HuntCrossleyForce, HuntCrossleyForceImpl, Force);
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_HUNT_CROSSLEY_FORCE_H_
diff --git a/Simbody/include/simbody/internal/LocalEnergyMinimizer.h b/Simbody/include/simbody/internal/LocalEnergyMinimizer.h
new file mode 100644
index 0000000..6ed86bc
--- /dev/null
+++ b/Simbody/include/simbody/internal/LocalEnergyMinimizer.h
@@ -0,0 +1,57 @@
+#ifndef SimTK_SIMBODY_LOCAL_ENERGY_MINIMIZER_H_
+#define SimTK_SIMBODY_LOCAL_ENERGY_MINIMIZER_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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/MultibodySystem.h"
+
+namespace SimTK {
+
+/**
+ * This class performs local potential energy minimization of a MultibodySystem.
+ * Only positions (generalized coordinates q) are changed; velocities are ignored.
+ */
+
+class SimTK_SIMBODY_EXPORT LocalEnergyMinimizer {
+public:
+ /**
+ * Find the local potential energy minimum of a MultibodySystem.
+ *
+ * @param system the system whose energy should be minimized
+ * @param state on entry, this should contain the starting state from which to begin the search.
+ * On exit, it contains a (usually nearby) state with revised generalized coordinate
+ * values q at which the energy is a local minimum.
+ * @param tolerance a tolerance for the energy minimization. The search ends when no component of the
+ * energy gradient is larger than this.
+ */
+ static void minimizeEnergy(const MultibodySystem& system, State& state, Real tolerance);
+private:
+ class OptimizerFunction;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_LOCAL_ENERGY_MINIMIZER_H_
diff --git a/Simbody/include/simbody/internal/MobilizedBody.h b/Simbody/include/simbody/internal/MobilizedBody.h
new file mode 100644
index 0000000..a2ff2c3
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody.h
@@ -0,0 +1,1884 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy, 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+This defines the MobilizedBody class, which associates a new body (the
+"child", "outboard", or "successor" body) with a mobilizer and a reference
+frame on an existing body (the "parent", "inboard", or "predecessor" body)
+that is already part of a SimbodyMatterSubsystem.
+
+MobilizedBody is an abstract base class handle, with concrete classes
+defined for each kind of mobilizer. There are a set of built-in mobilizers
+and a generic "Custom" mobilizer (an actual abstract base class) from
+which advanced users may derive their own mobilizers.
+**/
+
+#include "SimTKmath.h"
+#include "simbody/internal/common.h"
+#include "simbody/internal/Body.h"
+#include "simbody/internal/Motion.h"
+
+#include <cassert>
+
+namespace SimTK {
+
+class SimbodyMatterSubsystem;
+class Motion;
+class MobilizedBody;
+class MobilizedBodyImpl;
+
+// We only want the template instantiation to occur once. This symbol is
+// defined in the SimTK core compilation unit that instantiates the mobilized
+// body class but should not be defined any other time.
+#ifndef SimTK_SIMBODY_DEFINING_MOBILIZED_BODY
+ extern template class PIMPLHandle<MobilizedBody, MobilizedBodyImpl, true>;
+#endif
+
+/** %Mobod is the approved abbreviation for MobilizedBody.\ Feel free to
+use it if you get tired of typing or seeing the full name.
+ at relates SimTK::MobilizedBody **/
+typedef MobilizedBody Mobod;
+
+
+
+//==============================================================================
+// MOBILIZED BODY
+//==============================================================================
+/** A %MobilizedBody is Simbody's fundamental body-and-joint object used to
+parameterize a system's motion by constructing a multibody tree containing
+each body and its unique "mobilizer" (internal coordinate joint). A
+%MobilizedBody connects a new body (the "child", "outboard", or "successor"
+body) with a mobilizer and a reference frame to an existing %MobilizedBody (the
+"parent", "inboard", or "predecessor" body) that is already part of a
+SimbodyMatterSubsystem. In the topology of the multibody tree, the parent
+mobilized body is always closer to Ground ("inboard") than is the child.
+
+This is the base class for all %MobilizedBody classes, which include a body
+and a particular kind of mobilizer. Each built-in %MobilizedBody type is a local
+subclass within %MobilizedBody, so the built-ins have names like
+MobilizedBody::Pin. All concrete %MobilizedBody classes, including the
+built-ins, are derived from MobilizedBody. There is a MobilizedBody::Custom
+class available for defining your own mobilizer types.
+
+Normally, a %MobilizedBody's motion is unknown and Simbody calculates it using
+forward dynamics where the motion results from external or internal forces.
+However, a %MobilizedBody may be locked or controlled by an associated Motion
+object which defines how it is to move; those are inverse dynamics conditions
+in which the motion is known but the generalized forces required to produce that
+motion are to be determined. In general a system may contain a mix of forward-
+and inverse-dynamics mobilizers, and mobilizers may be switched between forward
+and inverse dynamics during simulations.
+
+The %MobilizedBody class supports a large number of methods useful for
+obtaining and modifying body-specific and mobilizer-specific information and
+for performing useful computations. These are organized in three sets:
+ - %State Access
+ - Basic Operators
+ - High Level Operators
+
+<em>%State Access</em> methods simply extract already-calculated data from the
+State or %State Cache, or set state values. They involve no additional
+computation, have names beginning with "get" and "upd" (update) and return
+references to the requested quantities rather than calculated values. We
+divide these into routines which deal with bodies and routines which deal
+with mobilizers and mobilities.
+
+<em>Basic Operators</em> use State Access methods to compute basic quantities
+which cannot be precomputed, such as the velocity of an arbitrary point,
+using an inline combination of basic floating point operations which can be
+reliably determined at compile time. These have names beginning with "find"
+or a more specific verb, as a reminder that they do not require a great deal
+of computation.
+
+<em>High Level Operators</em> combine responses and basic operators with
+run-time tests to calculate more complex quantities, with more complicated
+implementations that can exploit special cases at run time. These begin with
+"calc" (calculate) as a reminder that they may involve substantial run time
+computation.
+
+There is also a set of methods used for construction, and miscellaneous
+utilities.
+
+<h3>Mobilizer Terminology and Notation</h3>
+
+Refer to the figure below for the terminology we use when discussing mobilizers
+and mobilized bodies.
+
+ at image html MobilizerTerminology.png "Terminology and notation for mobilized bodies"
+
+The figure shows the coordinate frames used in describing the mobility of
+%MobilizedBody B with respect to its inboard parent body P. Everything blue
+in the figure is associated with B. The origin point O of each frame is labeled.
+A new %MobilizedBody with body frame B is added to the multibody tree by
+choosing a parent body P that is already present in the tree. There are two
+frames associated with the mobilizer: the "fixed" frame F that is attached to
+the parent, and the "moving" frame M that is attached to the new body B. Frame
+F is specified by giving its transform X_PF relative to the P frame. Frame M is
+specified by giving its transform X_BM relative to the B frame. At run time
+the transform X_FM between the two mobilizer frames represents translation and
+rotation of the mobilizer. That motion is parameterized via generalized
+coordinates q and generalized speeds u, the specific meaning of which is a
+unique property of each type of mobilizer.
+
+In the API below, we'll refer to the current ("this") MobilizedBody as "body B".
+It is the "object" or "main" body with which we are concerned. Often there will
+be another body mentioned in the argument list as a target for some conversion.
+That "another" body will be called "body A". The Ground body is abbreviated "G".
+
+We use Fo to mean "the origin of frame F", Bc is "the mass center of body B".
+R_AF is the rotation matrix giving frame F's orientation in frame A, such that a
+vector v expressed in F is reexpressed in A by v_A = R_AF*v_F. X_AF is the
+spatial transform giving frame F's origin location and orientation in frame A,
+such that a point P whose location is measured from F's origin Fo and expressed
+in F by position vector p_FP (or more explicitly p_FoP) is remeasured from frame
+A's origin Ao and reexpressed in A via p_AP = X_AF*p_FP, where p_AP==p_AoP.
+
+<h3>Theory</h3>
+For the mathematical and computational theory behind Simbody's mobilizers, see
+ - the <a href="https://simtk.org/docman/view.php/47/1536/Seth-2010-ShermanEastmanDelp-MinimalJointFormulationForBiomechanisms-NonlinearDyn-v62-p291.pdf">
+ paper</a> Seth, A.; Sherman, M.A.; Eastman, P.; Delp, S.L. "Minimal formulation
+ of joint motion for biomechanisms" Nonlinear Dynamics 62:291-303 (2010), or
+ - the Simbody Theory Manual.
+**/
+
+class SimTK_SIMBODY_EXPORT MobilizedBody
+: public PIMPLHandle<MobilizedBody, MobilizedBodyImpl, true> {
+public:
+
+/** Constructors can take an argument of this type to indicate that the
+mobilizer is being defined in the reverse direction, meaning from the outboard
+(child) body to the inboard (parent) body. That means that the mobilizer
+coordinates and speeds will be defined as though the tree had been built in the
+opposite direction. It does not actually affect which body is the inboard one
+and which is the outboard; it just affects the definitions of the generalized
+coordinates q and speeds u that parameterize the motion of the outboard body
+with respect to the inboard one. This is a topological setting and can't be
+changed dynamically. **/
+enum Direction {
+ Forward = 0, ///< Use default definitions for q and u (inboard to outboard).
+ Reverse = 1 ///< Define q and u in the reverse order (outboard to inboard).
+};
+
+//------------------------------------------------------------------------------
+/**@name Mobilizer locking and unlocking
+
+Every %MobilizedBody object supports locking and unlocking of the mobilizer it
+contains. You can lock the mobilizer's position, velocity, or acceleration. In
+all cases the generalized accelerations udot of a locked mobilizer are
+prescribed. If you lock just the accelerations, then velocity and position
+remain free. If you lock velocity, then the generalized speeds u are prescribed
+to specified values, accelerations udot are prescribed to zero, and positions
+will remain free. If you lock position (the default locking level) then the
+generalized coordinates q are prescribed to specified values and the speeds u
+and accelerations udot are prescribed to zero. Prescribed values may be obtained
+from the current state, or set explicitly. The lock() and lockAt() methods when
+called at position level will modify q in the given state if necessary to
+satisfy the locked position, and will set u to zero. When called at velocity
+level they will modify u if necessary to satisfied the locked velocity, but will
+leave q unchanged.
+
+You can also specify that a mobilizer is locked by default (at acceleration,
+velocity, or position level). In that case the prescribed value is recorded
+when the state is realized to Stage::Model, using values taken from the
+state at that time.
+
+If this mobilizer is driven by a Motion object, locking overrides that while
+the lock is active; when unlocked the Motion object resumes control. **/
+/**@{**/
+/** Lock this mobilizer's position or velocity at its current value, or lock the
+acceleration to zero, depending on the \p level parameter. The prescribed
+position or velocity is taken from \p state and recorded. If the lock is at
+the position (q) level then the generalized speeds u for this mobilizer are set
+to zero in the \p state. If the mobilizer was already locked, the new lock
+specification overrides it. The \p state must already have been realized to at
+least Stage::Model and will be lowered to Stage::Model on return since a lock is
+an Instance-stage change. **/
+void lock(State& state, Motion::Level level=Motion::Position) const;
+
+/** Lock this mobilizer's q, u, or udot to the given scalar \p value, depending
+on \p level. When locking at the position level (the default), this mobilizer's
+q in \p state is set to \p value, and u in \p state is set to zero. When locking
+at velocity level, this mobilizer's u in \p state is set to \p value but the q
+is left unchanged. This signature of lockAt(), taking \p value as a scalar, is
+only permitted for mobilizers that have just a single q and u, such as a Pin or
+Slider mobilizer. See the other signatures of lockAt() for multi-coordinate
+mobilizers. **/
+void lockAt(State& state, Real value,
+ Motion::Level level=Motion::Position) const;
+
+/** Lock this mobilizer's q, u, or udot to the given Vector \p value, depending
+on \p level. When locking at the position level (the default), this mobilizer's
+q's in \p state is set to \p value, and its u's in \p state are set to zero.
+When locking at velocity level, this mobilizer's u's in \p state are set to
+\p value but its q's are left unchanged. The Vector must be the expected length,
+either nq=getNumQ() for the default Motion::Position level, or nu=getNumU() for
+Motion::Velocity or Motion::Acceleration levels.
+ at see getNumQ(), getNumU() **/
+void lockAt(State& state, const Vector& value,
+ Motion::Level level=Motion::Position) const;
+
+/** Lock this mobilizer's q, u, or udot to the given Vec\<N\> \p value,
+depending on the \p level. When locking at the position level (the default),
+this mobilizer's q's in \p state is set to \p value, and its u's in \p state are
+set to zero. When locking at velocity level, this mobilizer's u's in \p state
+are set to \p value but its q's are left unchanged. The size N of the given Vec
+must match the expected length, either nq=getNumQ() for the default
+Motion::Position level, or nu=getNumU() for Motion::Velocity or
+Motion::Acceleration levels.
+ at see getNumQ(), getNumU() **/
+template <int N> SimTK_SIMBODY_EXPORT // instantiated in library
+void lockAt(State& state, const Vec<N>& value,
+ Motion::Level level=Motion::Position) const;
+
+/** Unlock this mobilizer, returning it to its normal behavior which may be
+free motion or may be controlled by a Motion object. If the mobilizer is
+already unlocked nothing happens. **/
+void unlock(State& state) const;
+
+/** Check whether this mobilizer is currently locked in the given \p state. **/
+bool isLocked(const State& state) const
+{ return getLockLevel(state)!=Motion::NoLevel; }
+
+/** Returns the lock level if the mobilizer is locked in the given \p state,
+otherwise Motion::NoLevel. **/
+Motion::Level getLockLevel(const State& state) const;
+
+/** Return the q, u, or udot value at which this mobilizer is locked, depending
+on the lock level, as a Vector of the appropriate length. If the mobilizer is
+not currently locked, a zero-length Vector is returned. **/
+Vector getLockValueAsVector(const State& state) const;
+
+/** Change whether this mobilizer is initially locked. On construction the
+mobilizer is unlocked by default; you can change that here. To respecify that
+the motion is unlocked, use Motion::NoLevel as the \p level. This is a
+topological change; you'll have to call realizeTopology() again and get a new
+State if you call this method. If the default lock is at the position or
+velocity level, the required q or u value is recorded at the time a \p state is
+realized to Stage::Model. A default Motion::Acceleration lock always prescribes
+the accelerations udot to zero. **/
+MobilizedBody& lockByDefault(Motion::Level level=Motion::Position);
+
+/** Check whether this mobilizer is to be locked in the default state. **/
+bool isLockedByDefault() const
+{ return getLockByDefaultLevel()!=Motion::NoLevel; }
+
+/** Returns the level at which the mobilizer is locked by default, if it is
+locked by default, otherwise Motion::NoLevel. **/
+Motion::Level getLockByDefaultLevel() const;
+/**@}**/
+
+ //////////////////////////
+ // STATE ACCESS METHODS //
+ //////////////////////////
+
+//------------------------------------------------------------------------------
+/** @name State Access - Bodies
+These methods extract already-computed information from the State or State
+cache, or set values in the State. **/
+/**@{**/
+
+/** Extract from the state cache the already-calculated spatial configuration
+X_GB of body B's body frame, measured with respect to the Ground frame and
+expressed in the Ground frame. That is, we return the location of the body
+frame's origin, and the orientation of its x, y, and z axes, as the Transform
+X_GB. This notation is intended to convey unambiguously the sense of this
+transform, which is as follows: if you have a station (body fixed point) S on
+body B, represented by position vector p_BS (a.k.a. p_BoS) from the origin Bo
+of B to the point S and expressed in the B frame, then p_GS=X_GB*p_BS where
+p_GS (== p_GoS) is the position vector from the Ground origin Go to the point in
+space currently coincident with S and expressed in the Ground frame. The inverse
+transformation is obtained using the "~" operator where ~X_GB=X_BG, so that
+p_BS = ~X_GB*p_GS. This response is available at Position stage. **/
+const Transform& getBodyTransform(const State& state) const; // X_GB
+
+/** Extract from the state cache the already-calculated spatial orientation R_GB
+of body B's body frame x, y, and z axes expressed in the Ground frame, as the
+Rotation matrix R_GB. The sense of this rotation matrix is such that if you have
+a vector v fixed on body B, represented by the vector v_B expressed in the B
+frame, then v_G=R_GB*v_B where v_G is the same vector but re-expressed in the
+Ground frame. The inverse transformation is obtained using the "~" operator
+where ~R_GB=R_BG, so that v_B = ~R_GB*v_G. This response is available at
+Position stage. **/
+const Rotation& getBodyRotation(const State& state) const {
+ return getBodyTransform(state).R();
+}
+/** Extract from the state cache the already-calculated spatial location of body
+B's body frame origin Bo, measured from the Ground origin Go and expressed in
+the Ground frame, as the position vector p_GB (== p_GoBo). This response is
+available at Position stage. **/
+const Vec3& getBodyOriginLocation(const State& state) const {
+ return getBodyTransform(state).p();
+}
+
+/** At stage Position or higher, return the cross-mobilizer transform X_FM, the
+body's inboard mobilizer frame M measured and expressed in the parent body's
+corresponding outboard frame F. **/
+const Transform& getMobilizerTransform(const State& state) const; // X_FM
+
+/** Extract from the state cache the already-calculated spatial velocity V_GB of
+this body's reference frame B, measured with respect to the Ground frame and
+expressed in the Ground frame. That is, we return the linear velocity v_GB of
+the body frame's origin in G, and the body's angular velocity w_GB as the
+spatial velocity vector V_GB = {w_GB, v_GB}. This response is available at
+Velocity stage. **/
+const SpatialVec& getBodyVelocity(const State& state) const; // V_GB
+
+/** Extract from the state cache the already-calculated inertial angular
+velocity vector w_GB of this body B, measured with respect to the Ground frame
+and expressed in the Ground frame. This response is available at Velocity
+stage. **/
+const Vec3& getBodyAngularVelocity(const State& state) const { // w_GB
+ return getBodyVelocity(state)[0];
+}
+/** Extract from the state cache the already-calculated inertial linear velocity
+vector v_GB (more explicitly, v_GBo) of this body B's origin point Bo, measured
+with respect to the Ground frame and expressed in the Ground frame. This
+response is available at Velocity stage. **/
+const Vec3& getBodyOriginVelocity(const State& state) const { // v_GB
+ return getBodyVelocity(state)[1];
+}
+
+/** At stage Velocity or higher, return the cross-mobilizer velocity V_FM, the
+relative velocity of this body's "moving" mobilizer frame M in the parent body's
+corresponding "fixed" frame F, measured and expressed in F. Note that this isn't
+the usual spatial velocity since it isn't expressed in G. **/
+const SpatialVec& getMobilizerVelocity(const State& state) const; // V_FM
+
+/** Extract from the state cache the already-calculated spatial acceleration
+A_GB of this body's reference frame B, measured with respect to the Ground frame
+and expressed in the Ground frame. That is, we return the linear acceleration
+a_GB of the body frame's origin in G, and the body's angular acceleration b_GB
+as the spatial acceleration vector A_GB = {b_GB, a_GB}. This response is
+available at Acceleration stage. **/
+const SpatialVec& getBodyAcceleration(const State& state) const; // A_GB
+
+/** Extract from the state cache the already-calculated inertial angular
+acceleration vector b_GB of this body B, measured with respect to the Ground
+frame and expressed in the Ground frame. This response is available at
+Acceleration stage. **/
+const Vec3& getBodyAngularAcceleration(const State& state) const { // b_GB
+ return getBodyAcceleration(state)[0];
+}
+
+/** Extract from the state cache the already-calculated inertial linear
+acceleration vector a_GB (more explicitly, a_GBo) of this body B's origin point
+Bo, measured with respect to the Ground frame and expressed in the Ground frame.
+This response is available at Acceleration stage. **/
+const Vec3& getBodyOriginAcceleration(const State& state) const { // a_GB
+ return getBodyAcceleration(state)[1];
+}
+
+/** TODO: Not implemented yet -- any volunteers? At stage Acceleration, return
+the cross-mobilizer acceleration A_FM, the relative acceleration of body B's
+"moving" mobilizer frame M in the parent body's corresponding "fixed" frame F,
+measured and expressed in F. Note that this isn't the usual spatial acceleration
+since it isn't expressed in G. **/
+const SpatialVec& getMobilizerAcceleration(const State& state) const { // A_FM
+ SimTK_ASSERT_ALWAYS(!"unimplemented method",
+"MobilizedBody::getMobilizerAcceleration() not yet implemented -- volunteers?");
+ return *(new SpatialVec());
+}
+
+/** Return a reference to this body's mass properties in the State cache. The
+State must have been realized to Stage::Instance or higher. **/
+const MassProperties& getBodyMassProperties(const State& state) const;
+
+/** Return a reference to the already-calculated SpatialInertia of this body,
+taken about the body's origin (\e not its mass center), and expressed in the
+Ground frame. The State must have been realized to Stage::Position or higher. **/
+const SpatialInertia& getBodySpatialInertiaInGround(const State& state) const;
+
+/** Return the mass of this body. The State must have been realized to
+Stage::Instance. **/
+Real getBodyMass(const State& state) const {
+ return getBodyMassProperties(state).getMass();
+}
+
+/** Return this body's center of mass station (i.e., the vector fixed in the
+body, going from body origin to body mass center, expressed in the body frame.)
+The State must have been realized to Stage::Instance or higher. **/
+const Vec3& getBodyMassCenterStation(const State& state) const {
+ return getBodyMassProperties(state).getMassCenter();
+}
+
+/** Return a reference to this body's unit inertia matrix in the State cache,
+taken about the body origin and expressed in the body frame. The State must have
+been realized to Stage::Instance or higher. **/
+const UnitInertia& getBodyUnitInertiaAboutBodyOrigin(const State& state) const {
+ return getBodyMassProperties(state).getUnitInertia();
+}
+
+/** Return a reference to this mobilizer's frame F fixed on the parent body P,
+as the fixed Transform from P's body frame to the frame F fixed to P. If this
+frame is changeable, the result comes from the State cache, otherwise it is from
+the MobilizedBody object itself. The State must have been realized to
+Stage::Instance or higher. **/
+const Transform& getInboardFrame (const State& state) const; // X_PF
+/** Return a reference to this MobilizedBody's mobilizer frame M, as the fixed
+Transform from this body B's frame to the frame M fixed on B. If this frame is
+changeable, the result comes from the State cache, otherwise it is from the
+MobilizedBody object itself. The State must have been realized to
+Stage::Instance or higher. **/
+const Transform& getOutboardFrame(const State& state) const; // X_BM
+
+/** TODO: not implemented yet. Set the location and orientation of the inboard
+(parent) mobilizer frame F, fixed to this mobilizer's parent body P.
+ at see setDefaultInboardFrame() **/
+void setInboardFrame (State& state, const Transform& X_PF) const;
+/** TODO: not implemented yet. Set the location and orientation of the outboard
+mobilizer frame M, fixed to this body B.
+ at see setDefaultOutboardFrame() **/
+void setOutboardFrame(State& state, const Transform& X_BM) const;
+
+// End of State Access - Bodies
+/**@}**/
+
+
+//------------------------------------------------------------------------------
+/** @name State Access - Mobilizer generalized coordinates q and speeds u
+These methods extract q- or u-related information from the State or State cache,
+or set q or u values in the State. **/
+/**@{**/
+/** Return the number of generalized coordinates q currently in use by this
+mobilizer. State must have been realized to Stage::Model. **/
+int getNumQ(const State& state) const;
+/** Return the number of generalized speeds u currently in use by this
+mobilizer. State must have been realized to Stage::Model. **/
+int getNumU(const State& state) const;
+
+/** Return the global QIndex of the first q for this mobilizer; all the q's
+range from getFirstQIndex() to QIndex(getFirstQIndex()+getNumQ()-1). **/
+QIndex getFirstQIndex(const State& state) const;
+
+/** Return the global UIndex of the first u for this mobilizer; all the u's
+range from getFirstUIndex() to UIndex(getFirstUIndex()+getNumU()-1). **/
+UIndex getFirstUIndex(const State& state) const;
+
+/** Determine how generalized coordinate q values are being determined.
+ at param[in] state Must be realized to Instance stage. **/
+Motion::Method getQMotionMethod(const State& state) const;
+/** Determine how generalized speed u values are being determined.
+ at param[in] state Must be realized to Instance stage. **/
+Motion::Method getUMotionMethod(const State& state) const;
+/** Determine how generalized acceleration udot values are being determined.
+ at param[in] state Must be realized to Instance stage. **/
+Motion::Method getUDotMotionMethod(const State& state) const;
+
+/** Return one of the generalized coordinates q from this mobilizer's partition
+of the matter subsystem's full q vector in the State. The particular coordinate
+is selected using the \p which parameter, numbering from zero to
+getNumQ()-1. **/
+Real getOneQ(const State& state, int which) const;
+
+/** Return one of the generalized speeds u from this mobilizer's partition of
+the matter subsystem's full u vector in the State. The particular coordinate is
+selected using the \p which parameter, numbering from zero to getNumU()-1. **/
+Real getOneU(const State& state, int which) const;
+
+/** Return as a Vector of length getNumQ() all the generalized coordinates q
+currently in use by this mobilizer, from this mobilizer's partion in the matter
+subsystem's full q vector in the State. **/
+Vector getQAsVector(const State& state) const;
+/** Return as a Vector of length getNumU() all the generalized speeds u
+currently in use by this mobilizer, from this mobilizer's partion in the matter
+subsystem's full u vector in the State. **/
+Vector getUAsVector(const State& state) const;
+
+/** Return one of the generalized coordinate derivatives qdot from this
+mobilizer's partition of the matter subsystem's full qdot vector in the State
+cache. The particular coordinate is selected using the \p which parameter,
+numbering from zero to getNumQ()-1. **/
+Real getOneQDot (const State& state, int which) const;
+/** Return as a Vector of length getNumQ() all the generalized coordinate
+derivatives qdot currently in use by this mobilizer, from this mobilizer's
+partition in the matter subsystem's full qdot vector in the State cache. **/
+Vector getQDotAsVector(const State& state) const;
+
+/** Return one of the generalized accelerations udot from this mobilizer's
+partition of the matter subsystem's full udot vector in the State cache. The
+particular coordinate is selected using the \p which parameter, numbering from
+zero to getNumU()-1. **/
+Real getOneUDot(const State& state, int which) const;
+/** Return one of the generalized coordinate second derivatives qdotdot from
+this mobilizer's partition of the matter subsystem's full qdotdot vector in the
+State cache. The particular coordinate is selected using the \p which parameter,
+numbering from zero to getNumQ()-1. **/
+Real getOneQDotDot(const State& state, int which) const;
+/** Return as a Vector of length getNumU() all the generalized accelerations
+udot currently in use by this mobilizer, from this mobilizer's partion in the
+matter subsystem's full udot vector in the State cache. **/
+Vector getUDotAsVector(const State& state) const;
+/** Return as a Vector of length getNumQ() all the generalized coordinate
+second derivatives qdotdot currently in use by this mobilizer, from this
+mobilizer's partion in the matter subsystem's full qdotdot vector in the
+State cache. **/
+Vector getQDotDotAsVector(const State& state) const;
+
+/** Return the generalized forces tau resulting from prescribed (known)
+acceleration, corresponding to each of this mobilizer's mobilities, as a Vector
+of length nu=getNumU().
+
+If this mobilizer has prescribed accelerations udot due to an active lock or
+Motion object, the set of generalized forces tau that must be added in order to
+produce those accelerations is calculated at Acceleration stage. There is one
+scalar tau per mobility and they can be returned individually or as a Vector.
+The return value is zero if this mobilizer is currently free. **/
+Vector getTauAsVector(const State& state) const;
+
+/** Return one of the tau forces resulting from prescribed (known) acceleration,
+corresponding to one of this mobilizer's mobilities as selected here using the
+\p which parameter, numbered from zero to getNumU()-1.
+ at see getTauAsVector() for more information **/
+Real getOneTau(const State& state, MobilizerUIndex which) const;
+
+/** Set one of the generalized coordinates q to value \p v, in this mobilizer's
+partition of the matter subsystem's full q vector in the State. The particular
+coordinate is selected using the \p which parameter, numbering from zero to
+getNumQ()-1. **/
+void setOneQ(State& state, int which, Real v) const;
+/** Set one of the generalized speeds u to value \p v, in this mobilizer's
+partition of the matter subsystem's full u vector in the State. The particular
+coordinate is selected using the \p which parameter, numbering from zero to
+getNumU()-1. **/
+void setOneU(State& state, int which, Real v) const;
+
+/** Set all of the generalized coordinates q to value \p v (a Vector of length
+getNumQ()), in this mobilizer's partition of the matter subsystem's full q
+vector in the State. **/
+void setQFromVector(State& state, const Vector& v) const;
+/** Set all of the generalized speeds u to value \p v (a Vector of length
+getNumU()), in this mobilizer's partition of the matter subsystem's full u
+vector in the State. **/
+void setUFromVector(State& state, const Vector& v) const;
+
+/** Adjust this mobilizer's q's to best approximate the supplied Transform
+which requests a particular relative orientation and translation between the
+F "fixed" frame and M "moving" frame connected by this mobilizer.
+
+This set of methods sets the generalized coordinates, or speeds (state
+variables) for just the mobilizer associated with this MobilizedBody (ignoring
+all other mobilizers and constraints), without requiring knowledge of the
+meanings of the individual state variables. The idea here is to provide a
+physically-meaningful quantity relating the mobilizer's inboard and outboard
+frames, and then ask the mobilizer to set its state variables to reproduce that
+quantity to the extent it can.
+
+These methods can be called in Stage::Model, however the routines may consult
+the current values of the state variables in some cases, so you must make sure
+they have been set to reasonable, or at least innocuous values (zero will work).
+In no circumstance will any of these methods look at any state variables that
+belong to another mobilizer; they are limited to working locally with just the
+current mobilizer.
+
+Routines which specify only translation (linear velocity) may use rotational
+coordinates to help satisfy the translation requirement. An alternate "Only"
+method is available to forbid modification of purely rotational coordinates in
+that case. When a mobilizer uses state variables which have combined rotational
+and translational character (e.g. a screw joint) consult the documentation for
+the derived MobilizedBody class to find out how that mobilizer responds to these
+routines.
+
+There is no guarantee that the desired physical quantity will be achieved by
+these routines; you can check on return if you're worried. Individual mobilizers
+make specific promises about what they will do; consult the documentation. These
+routines do not throw exceptions even for absurd requests like specifying a
+rotation for a sliding mobilizer. Nothing happens if there are no mobilities
+here, i.e. Ground or a Weld mobilizer. **/
+void setQToFitTransform(State& state, const Transform& X_FM) const;
+
+/** Adjust this mobilizer's q's to best approximate the supplied Rotation matrix
+which requests a particular relative orientation between the "fixed" frame F and
+"moving" frame M connected by this mobilizer.
+ at see setQToFitTransform() **/
+void setQToFitRotation(State& state, const Rotation& R_FM) const;
+
+/** Adjust this mobilizer's q's to best approximate the supplied position vector
+which requests a particular offset between the origins of the F "fixed" frame
+and M "moving" frame connected by this mobilizer, with \e any q's (rotational
+or translational) being modified if doing so helps satisfy the request.
+ at see setQToFitTransform() **/
+void setQToFitTranslation(State& state, const Vec3& p_FM) const;
+
+/** Adjust this mobilizer's u's (generalized speeds) to best approximate the
+supplied spatial velocity \p V_FM which requests the relative angular and linear
+velocity between the "fixed" and "moving" frames connected by this mobilizer.
+Routines which affect generalized speeds u depend on the generalized coordinates
+q already having been set; they never change these coordinates.
+ at see setQToFitTransform() **/
+void setUToFitVelocity(State& state, const SpatialVec& V_FM) const;
+
+/** Adjust this mobilizer's u's (generalized speeds) to best approximate the
+supplied angular velocity \p w_FM which requests a particular relative angular
+between the "fixed" and "moving" frames connected by this mobilizer.
+ at see setQToFitTransform(), setUToFitVelocity() **/
+void setUToFitAngularVelocity(State& state, const Vec3& w_FM) const;
+
+/** Adjust <em>any</em> of this mobilizer's u's (generalized speeds) to best
+approximate the supplied linear velocity \p v_FM which requests a particular
+velocity for the "moving" frame M origin in the "fixed" frame F on the parent
+where these are the frames connected by this mobilizer.
+ at see setQToFitTransform(), setUToFitVelocity() **/
+void setUToFitLinearVelocity(State& state, const Vec3& v_FM) const;
+
+/** Expert use only: obtain a column of the hinge matrix H corresponding to one
+of this mobilizer's mobilities (actually a column of H_PB_G; what Jain calls H*
+and Schwieters calls H^T). This is the matrix that maps generalized speeds u to
+the cross-body relative spatial velocity V_PB_G via V_PB_G=H*u. Note that
+although H relates child body B to parent body B, it is expressed in the ground
+frame G so the resulting cross-body velocity of B in P is also expressed in G.
+The supplied state must have been realized through Position stage because H
+varies with this mobilizer's generalized coordinates q.
+ at see getH_FMCol() **/
+SpatialVec getHCol(const State& state, MobilizerUIndex ux) const;
+
+/** Expert use only: obtain a column of the mobilizer-local hinge matrix H_FM
+which maps generalized speeds u to cross-mobilizer spatial velocity V_FM via
+V_FM=H_FM*u. Note that H and V here are expressed in the parent body's (inboard)
+frame F. The supplied state must have been realized through Position stage
+because H varies with this mobilizer's generalized coordinates q.
+ at see getHCol() **/
+SpatialVec getH_FMCol(const State& state, MobilizerUIndex ux) const;
+
+// End of State Access Methods.
+/**@}**/
+
+ /////////////////////
+ // BASIC OPERATORS //
+ /////////////////////
+
+//------------------------------------------------------------------------------
+/** @name Basic Operators
+
+These methods use state variables and Response methods to compute basic
+quantities which cannot be precomputed, but which can be implemented with an
+inline combination of basic floating point operations which can be reliably
+determined at compile time. The method names and descriptions use the following
+terms:
+ - %Body or ThisBody: the %Body B associated with the current %MobilizedBody.
+ ThisBody is implied when no other Body is mentioned.
+ - %Ground: the "MobilizedBody" G representing the %Ground reference frame which
+ never moves.
+ - AnotherBody: the %Body A being referenced, which in general is neither
+ ThisBody nor %Ground.
+ - Station: a point S fixed on ThisBody B, located by a position vector p_BS (or
+ more explicitly, p_BoS) from the B-frame origin Bo to the point S, expressed
+ in the B-frame coordinate system.
+ - %Vector: a vector v fixed on ThisBody B, given by a vector v_B expressed in
+ the B-frame coordinate system.
+ - Direction: a unit vector u fixed on ThisBody B, given by a unit vector u_B
+ expressed in the B-frame coordinate system.
+ - Frame: an origin and coordinate axes F fixed on ThisBody B, given by a
+ transform X_BF that locates F's origin (a Station) in B and expresses each of
+ F's axes (Directions) in B.
+ - Origin: the Station located at (0,0,0) in ThisBody frame B, that is, body B's
+ origin point.
+ - MassCenter: the Station on ThisBody B which is the center of mass for B.
+ - GroundPoint, GroundVector: a Point P or Vector v on the %Ground "Body" G.
+ These are measured and expressed in the %Ground frame, as p_GP or v_G.
+ - AnotherBodyStation, AnotherBodyVector, etc.: a Station S or %Vector v on
+ AnotherBody A. These are measured and expressed in the A frame, as p_AS
+ or v_A.
+ - Mobilizer frame M: the mobilizer's outboard "moving" frame, fixed to
+ ThisBody B.
+ - Mobilizer frame F: the mobilizer's inboard "fixed" frame, fixed to the parent
+ body P. **/
+
+/**@{**/
+
+/** Return X_AB, the spatial transform giving this body B's frame in body A's
+frame. Cost is 63 flops. If you know that one of the bodies is Ground, use the
+0-cost response getBodyTransform() instead of this operators. This operator is
+available in Position stage.
+ at see getBodyTransform() **/
+Transform findBodyTransformInAnotherBody(const State& state,
+ const MobilizedBody& inBodyA) const
+{
+ const Transform& X_GA = inBodyA.getBodyTransform(state);
+ const Transform& X_GB = this->getBodyTransform(state);
+
+ return ~X_GA*X_GB; // X_AB=X_AG*X_GB
+}
+
+/** Return R_AB, the rotation matrix giving this body B's axes in body A's
+frame. Cost is 45 flops. If you know that one of the bodies is Ground, use the
+0-cost response getBodyRotation() instead of this operators. This operator is
+available in Position stage.
+ at see getBodyRotation() **/
+Rotation findBodyRotationInAnotherBody(const State& state,
+ const MobilizedBody& inBodyA) const
+{
+ const Rotation& R_GA = inBodyA.getBodyRotation(state);
+ const Rotation& R_GB = this->getBodyRotation(state);
+
+ return ~R_GA*R_GB; // R_AB=R_AG*R_GB
+}
+
+/** Return the station on another body A (that is, a point measured and
+expressed in A) that is currently coincident in space with the origin Bo of
+this body B. Cost is 18 flops. This operator is available at Position stage.
+Note: "findBodyOriginLocationInGround" doesn't exist because it would be the
+same as the no-cost response method getBodyOriginLocation().
+ at see getBodyOriginLocation() **/
+Vec3 findBodyOriginLocationInAnotherBody
+ (const State& state, const MobilizedBody& toBodyA) const {
+ return toBodyA.findStationAtGroundPoint(state,
+ getBodyOriginLocation(state));
+}
+
+/** Return the angular and linear velocity of body B's frame in body A's frame,
+expressed in body A, and arranged as a SpatialVec. Cost is 51 flops. If you know
+inBodyA is Ground, don't use this routine; use the response method
+getBodyVelocity() which is free. This operator is available in Velocity stage.
+ at see getBodyVelocity() **/
+SpatialVec findBodyVelocityInAnotherBody
+ (const State& state, const MobilizedBody& inBodyA) const
+{
+ const SpatialVec& V_GB = this->getBodyVelocity(state);
+ const SpatialVec& V_GA = inBodyA.getBodyVelocity(state);
+ // angular velocity of B in A, exp in G
+ const Vec3 w_AB_G = V_GB[0]-V_GA[0]; // 3 flops
+
+ // Angular vel. was easy, but for linear vel. we have to add in an wXr term.
+
+ const Transform& X_GB = getBodyTransform(state);
+ const Transform& X_GA = inBodyA.getBodyTransform(state);
+ // vector from Ao to Bo, exp in G
+ const Vec3 p_AB_G = X_GB.p() - X_GA.p(); // 3 flops
+ // d/dt p taken in G
+ const Vec3 p_AB_G_dot = V_GB[1] - V_GA[1]; // 3 flops
+
+ // d/dt p taken in A, exp in G
+ const Vec3 v_AB_G = p_AB_G_dot - V_GA[0] % p_AB_G; // 12 flops
+
+ // We're done, but answer is expressed in Ground. Reexpress in A and return.
+ return ~X_GA.R()*SpatialVec(w_AB_G, v_AB_G); // 30 flops
+}
+
+/** Return the angular velocity w_AB of body B's frame in body A's frame,
+expressed in body A. Cost is 18 flops. If you know inBodyA is Ground, don't use
+this routine; use the response method getBodyAngularVelocity() which is free.
+This operator is available in Velocity stage.
+ at see getBodyAngularVelocity() **/
+Vec3 findBodyAngularVelocityInAnotherBody(const State& state,
+ const MobilizedBody& inBodyA) const
+{
+ const Vec3& w_GB = getBodyAngularVelocity(state);
+ const Vec3& w_GA = inBodyA.getBodyAngularVelocity(state);
+ // angular velocity of B in A, exp in G
+ const Vec3 w_AB_G = w_GB-w_GA; // 3 flops
+
+ // Now reexpress in A.
+ return inBodyA.expressGroundVectorInBodyFrame(state, w_AB_G); // 15 flops
+}
+
+/** Return the velocity of body B's origin point in body A's frame, expressed in
+body A. Cost is 51 flops. If you know inBodyA is Ground, don't use this routine;
+use the response method getBodyOriginVelocity() which is free. This operator is
+available in Velocity stage.
+ at see getBodyOriginVelocity() **/
+Vec3 findBodyOriginVelocityInAnotherBody(const State& state,
+ const MobilizedBody& inBodyA) const
+{
+ // Doesn't save much to special case this one.
+ return findBodyVelocityInAnotherBody(state,inBodyA)[1];
+}
+
+/** Return the angular and linear acceleration of body B's frame in body A's
+frame, expressed in body A, and arranged as a SpatialVec. Cost is 105 flops. If
+you know that inBodyA is Ground, don't use this operator; instead, use the
+response method getBodyAcceleration() which is free. This operator is available
+in Acceleration stage.
+ at see getBodyAcceleration() **/
+SpatialVec findBodyAccelerationInAnotherBody(const State& state,
+ const MobilizedBody& inBodyA) const
+{
+ const Transform& X_GA = inBodyA.getBodyTransform(state);
+ const SpatialVec& V_GA = inBodyA.getBodyVelocity(state);
+ const SpatialVec& A_GA = inBodyA.getBodyAcceleration(state);
+ const Transform& X_GB = this->getBodyTransform(state);
+ const SpatialVec& V_GB = this->getBodyVelocity(state);
+ const SpatialVec& A_GB = this->getBodyAcceleration(state);
+
+ return findRelativeAcceleration(X_GA, V_GA, A_GA,
+ X_GB, V_GB, A_GB);
+}
+
+/** Return the angular acceleration of body B's frame in body A's frame,
+expressed in body A. Cost is 33 flops. If you know \p inBodyA is Ground, don't
+use this operator; instead use the response method getBodyAngularAccleration()
+which is free. This operator is available at AccelerationStage.
+ at see getBodyAngularAcceleration() **/
+Vec3 findBodyAngularAccelerationInAnotherBody
+ (const State& state, const MobilizedBody& inBodyA) const
+{
+ const Rotation& R_GA = inBodyA.getBodyRotation(state);
+ const Vec3& w_GA = inBodyA.getBodyAngularVelocity(state);
+ const Vec3& w_GB = this->getBodyAngularVelocity(state);
+ const Vec3& b_GA = inBodyA.getBodyAngularAcceleration(state);
+ const Vec3& b_GB = this->getBodyAngularAcceleration(state);
+
+ // relative ang. vel. of B in A, exp. in G
+ const Vec3 w_AB_G = w_GB - w_GA; // 3 flops
+ const Vec3 w_AB_G_dot = b_GB - b_GA; // d/dt of w_AB_G taken in G; 3 flops
+
+ // We have the derivative in G; change it to derivative in A by adding
+ // in contribution caused by motion of G in A, that is w_AG X w_AB_G.
+ // (Note that w_AG=-w_GA.)
+ const Vec3 b_AB_G = w_AB_G_dot - w_GA % w_AB_G; // ang. accel. of B in A
+ // 12 flops
+
+ return ~R_GA * b_AB_G; // taken in A, expressed in A; 15 flops
+}
+
+/** Return the acceleration of body B's origin point in body A's frame,
+expressed in body A. Cost is 105 flops. If you know that inBodyA is Ground,
+don't use this operator; instead, use the response method
+getBodyOriginAcceleration() which is free. This operator is available at
+Acceleration stage.
+ at see getBodyOriginAcceleration() **/
+Vec3 findBodyOriginAccelerationInAnotherBody(const State& state,
+ const MobilizedBody& inBodyA) const
+{
+ // Not much to be saved by trying to optimize this since the linear part
+ // is the most expensive to calculate.
+ return findBodyAccelerationInAnotherBody(state,inBodyA)[1];
+}
+
+/** Return the spatial reaction force (moment and force) applied by the
+mobilizer to body B at the location of the mobilizer frame M (fixed to body B,
+but not necessarily at the body frame origin), expressed in Ground. This
+operator is available at Acceleration stage. Cost is about 120 flops.
+ at see findMobilizerReactionOnParentAtFInGround()
+ at see findMobilizerReactionOnBodyAtOriginInGround()
+ at see SimTK::SimbodyMatterSubsystem::calcMobilizerReactionForces() **/
+SpatialVec findMobilizerReactionOnBodyAtMInGround(const State& state) const;
+
+/** Return the spatial reaction force (moment and force) applied by the
+mobilizer to body B but shifted to the B frame origin, and expressed in Ground.
+This operator is available at Acceleration stage. Cost is about 90 flops.
+ at see findMobilizerReactionOnParentAtOriginInGround()
+ at see findMobilizerReactionOnBodyAtMInGround()
+ at see SimTK::SimbodyMatterSubsystem::calcMobilizerReactionForces() **/
+SpatialVec findMobilizerReactionOnBodyAtOriginInGround
+ (const State& state) const;
+
+/** Return the spatial reaction force (moment and force) applied by the
+mobilizer to the parent (inboard) body P at the location of the inboard "fixed"
+mobilizer frame F (fixed to body P, but not necessarily at the P frame origin),
+expressed in Ground. This operator is available at Acceleration stage. Cost is
+about 140 flops.
+ at see findMobilizerReactionOnBodyAtMInGround()
+ at see findMobilizerReactionOnParentAtOriginInGround()
+ at see SimTK::SimbodyMatterSubsystem::calcMobilizerReactionForces() **/
+SpatialVec findMobilizerReactionOnParentAtFInGround(const State& state) const;
+
+/** Return the spatial reaction force (moment and force) applied by the
+mobilizer to the parent (inboard) body P at the location of the P frame origin,
+and expressed in Ground. This operator is available at Acceleration stage. Cost
+is about 110 flops.
+ at see findMobilizerReactionOnBodyAtOriginInGround()
+ at see findMobilizerReactionOnParentAtFInGround()
+ at see SimTK::SimbodyMatterSubsystem::calcMobilizerReactionForces() **/
+SpatialVec findMobilizerReactionOnParentAtOriginInGround
+ (const State& state) const;
+
+/** Return the Cartesian (ground) location that is currently coincident with a
+station (point) S fixed on body B. That is, we return
+locationInG=X_GB*stationOnB which means the result is measured from the Ground
+origin and expressed in Ground. In more precise notation, we're calculating
+p_GS = X_GB * p_BS for position vectors p_GS and p_BS. Cost is 18 flops. This
+operator is available at Position stage. **/
+Vec3 findStationLocationInGround
+ (const State& state, const Vec3& stationOnB) const {
+ return getBodyTransform(state) * stationOnB;
+}
+
+
+/** Given a station S on this body B, return the location on another body A
+which is at the same location in space. That is, we return
+locationOnA=X_AB*locationOnB, which means the result is measured from the body A
+origin and expressed in body A. In more precise notation, we're calculating
+p_AS = X_AB * p_BS, which we actually calculate as p_AS = X_AG*(X_GB*p_BS). Cost
+is 36 flops. Note: if you know that one of the bodies is Ground, use one of the
+routines above which is specialized for Ground to avoid half the work. This
+operator is available at Position stage or higher. **/
+Vec3 findStationLocationInAnotherBody
+ (const State& state, const Vec3& stationOnB, const MobilizedBody& toBodyA)
+ const
+{
+ return toBodyA.findStationAtGroundPoint(state,
+ findStationLocationInGround(state,stationOnB));
+}
+
+/** Given a station fixed on body B, return its inertial (Cartesian) velocity,
+that is, its velocity relative to the Ground frame, expressed in the Ground
+frame. Cost is 27 flops. If you know the station is the body origin (0,0,0)
+don't use this routine; use the response getBodyOriginVelocity() which is free.
+This operator is available at Velocity stage.
+ at see getBodyOriginVelocity() **/
+Vec3 findStationVelocityInGround
+ (const State& state, const Vec3& stationOnB) const {
+ const Vec3& w = getBodyAngularVelocity(state); // in G
+ const Vec3& v = getBodyOriginVelocity(state); // in G
+ const Vec3 r = expressVectorInGroundFrame(state,stationOnB); // 15 flops
+ return v + w % r; // 12 flops
+}
+
+
+/** Return the velocity of a station S fixed on body B, in body A's frame,
+expressed in body A. Cost is 93 flops. If you know \p inBodyA is Ground, don't
+use this operator; instead use the operator findStationVelocityInGround() which
+is much cheaper. This operator is available in Velocity stage.
+ at see findStationVelocityInGround() **/
+Vec3 findStationVelocityInAnotherBody(const State& state,
+ const Vec3& stationOnBodyB,//p_BS
+ const MobilizedBody& inBodyA) const
+{
+ const SpatialVec V_AB =
+ findBodyVelocityInAnotherBody(state, inBodyA); //51 flops
+ // Bo->S rexpressed in A but not shifted to Ao
+ const Vec3 p_BS_A =
+ expressVectorInAnotherBodyFrame(state, stationOnBodyB, inBodyA); //30
+ return V_AB[1] + (V_AB[0] % p_BS_A); //12 flops
+}
+
+
+/** Given a station fixed on body B, return its inertial (Cartesian)
+acceleration, that is, its acceleration relative to the ground frame, expressed
+in the ground frame. Cost is 48 flops. If you know the station is the body
+origin (0,0,0) don't use this routine; use the response
+getBodyOriginAcceleration() which is free. This operator is available at
+Acceleration stage.
+ at see getBodyOriginAcceleration() **/
+Vec3 findStationAccelerationInGround
+ (const State& state, const Vec3& stationOnB) const {
+ const Vec3& w = getBodyAngularVelocity(state); // in G
+ const Vec3& b = getBodyAngularAcceleration(state); // in G
+ const Vec3& a = getBodyOriginAcceleration(state); // in G
+
+ const Vec3 r = expressVectorInGroundFrame(state,stationOnB); // 15 flops
+ return a + b % r + w % (w % r); // 33 flops
+}
+
+/** Return the acceleration of a station S fixed on body B, in another body A's
+frame, expressed in body A. Cost is 186 flops. If you know that \p inBodyA is
+Ground, don't use this operator; instead, use the operator
+findStationAccelerationInGround() which is much cheaper. This operator is
+available in Acceleration stage. **/
+Vec3 findStationAccelerationInAnotherBody(const State& state,
+ const Vec3& stationOnBodyB,
+ const MobilizedBody& inBodyA) const
+{
+ const Vec3 w_AB =
+ findBodyAngularVelocityInAnotherBody(state,inBodyA); // 18 flops
+ const SpatialVec A_AB =
+ findBodyAccelerationInAnotherBody(state,inBodyA); // 105 flops
+ // Bo->S rexpressed in A but not shifted to Ao
+ const Vec3 p_BS_A =
+ expressVectorInAnotherBodyFrame(state, stationOnBodyB, inBodyA); // 30
+
+ return A_AB[1] + (A_AB[0] % p_BS_A) + w_AB % (w_AB % p_BS_A); // 33 flops
+}
+
+/** It is cheaper to calculate a station's ground location and velocity together
+than to do them separately. Here we can return them both in 30 flops, vs. 45 to
+do them in two calls. This operator is available at Velocity stage. **/
+void findStationLocationAndVelocityInGround
+ (const State& state, const Vec3& locationOnB,
+ Vec3& locationOnGround, Vec3& velocityInGround) const
+{
+ const Vec3& p_GB = getBodyOriginLocation(state);
+ const Vec3 p_BS_G = expressVectorInGroundFrame(state,locationOnB);//15flops
+ locationOnGround = p_GB + p_BS_G; // 3flops
+
+ const Vec3& w_GB = getBodyAngularVelocity(state);
+ const Vec3& v_GB = getBodyOriginVelocity(state);
+ velocityInGround = v_GB + w_GB % p_BS_G; // 12 flops
+}
+
+
+/** It is cheaper to calculate a station's ground location, velocity, and
+acceleration together than to do them separately. Here we can return them all in
+54 flops, vs. 93 to do them in three calls. This operator is available at
+Acceleration stage. **/
+void findStationLocationVelocityAndAccelerationInGround
+ (const State& state, const Vec3& locationOnB,
+ Vec3& locationOnGround, Vec3& velocityInGround, Vec3& accelerationInGround)
+ const
+{
+ const Rotation& R_GB = getBodyRotation(state);
+ const Vec3& p_GB = getBodyOriginLocation(state);
+
+ // re-express station vector p_BS in G
+ const Vec3 r = R_GB*locationOnB; // 15 flops
+ locationOnGround = p_GB + r; // 3 flops
+
+ const Vec3& w = getBodyAngularVelocity(state); // in G
+ const Vec3& v = getBodyOriginVelocity(state); // in G
+ const Vec3& b = getBodyAngularAcceleration(state); // in G
+ const Vec3& a = getBodyOriginAcceleration(state); // in G
+
+ const Vec3 wXr = w % r; // "whipping" velocity w X r due to ang vel; 9 flops
+ velocityInGround = v + wXr; // v + w X r (3 flops)
+ accelerationInGround = a + b % r + w % wXr; // 24 flops
+}
+
+/** Return the Cartesian (ground) location of this body B's mass center. Cost is
+18 flops. This operator is available at Position stage. **/
+Vec3 findMassCenterLocationInGround(const State& state) const {
+ return findStationLocationInGround(state,getBodyMassCenterStation(state));
+}
+
+/** Return the point of another body A that is currently coincident in space
+with the mass center CB of this body B. Cost is 36 flops. This operator is
+available at Position stage. **/
+Vec3 findMassCenterLocationInAnotherBody
+ (const State& state, const MobilizedBody& toBodyA) const {
+ return findStationLocationInAnotherBody(state,
+ getBodyMassCenterStation(state),toBodyA);
+}
+
+/** Return the station (point) S of this body B that is coincident with the
+given Ground location. That is we return locationOnB = X_BG*locationInG, which
+means the result is measured from the body origin Bo and expressed in the body
+frame. In more precise notation, we're calculating p_BS = X_BG*p_GS. Cost is 18
+flops. This operator is available at
+Position stage. **/
+Vec3 findStationAtGroundPoint
+ (const State& state, const Vec3& locationInG) const {
+ return ~getBodyTransform(state) * locationInG;
+}
+
+/** Return the station (point) on this body B that is coincident with the given
+station on another body A. That is we return stationOnB = X_BA * stationOnA,
+which means the result is measured from the body origin Bo and expressed in the
+body frame. Cost is 36 flops. This operator is available at Position stage.
+ at see findStationLocationInAnotherBody() **/
+Vec3 findStationAtAnotherBodyStation
+ (const State& state, const MobilizedBody& fromBodyA,
+ const Vec3& stationOnA) const {
+ return fromBodyA.findStationLocationInAnotherBody(state, stationOnA, *this);
+}
+
+/** Return the station S of this body that is currently coincident in space with
+the origin Ao of another body A. Cost is 18 flops. This operator is available at
+Position stage. **/
+Vec3 findStationAtAnotherBodyOrigin
+ (const State& state, const MobilizedBody& fromBodyA) const {
+ return findStationAtGroundPoint(state,
+ fromBodyA.getBodyOriginLocation(state));
+}
+
+/** Return the station S of this body that is currently coincident in space with
+the mass center Ac of another body A. Cost is 36 flops. This operator is
+available at Position stage. **/
+Vec3 findStationAtAnotherBodyMassCenter
+ (const State& state, const MobilizedBody& fromBodyA) const {
+ return fromBodyA.findStationLocationInAnotherBody(state,
+ getBodyMassCenterStation(state),*this);
+}
+
+/** Return the current Ground-frame pose (position and orientation) of a frame
+F that is fixed to body B. That is, we return X_GF=X_GB*X_BF. Cost is 63 flops.
+This operator is available at Position stage. **/
+Transform findFrameTransformInGround
+ (const State& state, const Transform& frameOnB) const {
+ return getBodyTransform(state) * frameOnB;
+}
+
+/** Return the current Ground-frame spatial velocity V_GF (that is, angular and
+linear velocity) of a frame F that is fixed to body B. The angular velocity of F
+is the same as that of B, but the linear velocity is the velocity of F's origin
+Fo rather than B's origin Bo. This operator is available at Velocity stage. Cost
+is 27 flops. **/
+SpatialVec findFrameVelocityInGround
+ (const State& state, const Transform& frameOnB) const {
+ return SpatialVec(getBodyAngularVelocity(state),
+ findStationVelocityInGround(state,frameOnB.p()));
+}
+
+/** Return the current Ground-frame spatial acceleration A_GF (that is, angular
+and linear acceleration) of a frame F that is fixed to body B. The angular
+acceleration of F is the same as that of B, but the linear acceleration is the
+acceleration of F's origin Fo rather than B's origin Bo. This operator is
+available at Acceleration stage. Cost is 48 flops. **/
+SpatialVec findFrameAccelerationInGround
+ (const State& state, const Transform& frameOnB) const {
+ return SpatialVec(getBodyAngularAcceleration(state),
+ findStationAccelerationInGround(state,frameOnB.p()));
+}
+
+/** Re-express a vector expressed in this body B's frame into the same vector in
+G, by applying only a rotation. That is, we return vectorInG = R_GB * vectorInB.
+Cost is 15 flops. This operator is available at Position stage. **/
+Vec3 expressVectorInGroundFrame
+ (const State& state, const Vec3& vectorInB) const {
+ return getBodyRotation(state)*vectorInB;
+}
+
+/** Re-express a vector expressed in Ground into the same vector expressed in
+this body B, by applying only rotation. That is, we return
+vectorInB = R_BG * vectorInG. Cost is 15 flops. This operator is available at
+Position stage. **/
+Vec3 expressGroundVectorInBodyFrame
+ (const State& state, const Vec3& vectorInG) const {
+ return ~getBodyRotation(state)*vectorInG;
+}
+
+/** Re-express a vector expressed in this body B into the same vector expressed
+in body A, by applying only a rotation. That is, we return
+vectorInA = R_AB * vectorInB. Cost is 30 flops. This operator is available at
+Position stage. Note: if you know one of the bodies is Ground, call one of the
+specialized methods above to save 15 flops. **/
+Vec3 expressVectorInAnotherBodyFrame
+ (const State& state, const Vec3& vectorInB,
+ const MobilizedBody& inBodyA) const
+{
+ return inBodyA.expressGroundVectorInBodyFrame(state,
+ expressVectorInGroundFrame(state,vectorInB));
+}
+
+/** Re-express this body B's mass properties in Ground by applying only a
+rotation, not a shift of reference point. The mass properties remain measured in
+the body B frame, taken about the body B origin Bo, but are reexpressed in
+Ground. **/
+MassProperties expressMassPropertiesInGroundFrame(const State& state) const {
+ const MassProperties& M_Bo_B = getBodyMassProperties(state);
+ const Rotation& R_GB = getBodyRotation(state);
+ return M_Bo_B.reexpress(~R_GB);
+}
+
+/** Re-express this body B's mass properties in another body A's frame by
+applying only a rotation, not a shift of reference point. The mass properties
+remain measured in the body B frame, taken about the body B origin Bo, but are
+reexpressed in A. **/
+MassProperties expressMassPropertiesInAnotherBodyFrame
+ (const State& state, const MobilizedBody& inBodyA) const {
+ const MassProperties& M_Bo_B = getBodyMassProperties(state);
+ const Rotation R_AB = findBodyRotationInAnotherBody(state,inBodyA);
+ return M_Bo_B.reexpress(~R_AB);
+}
+
+// End of Basic Operators.
+/**@}**/
+
+//------------------------------------------------------------------------------
+/** @name High-Level Operators
+High level operators combine State Access and Basic Operators with run-time
+tests to calculate more complex %MobilizedBody-specific quantities, with more
+complicated implementations that can exploit special cases at run time. **/
+
+/**@{**/
+/** Return the mass properties of body B, measured from and about the B
+origin Bo, but expressed in Ground and then returned as a Spatial Inertia
+Matrix. The mass properties are arranged in the SpatialMat like this:
+<pre>
+ M=[ I_Bo crossMat(m*Bc) ]
+ [ ~crossMat(m*Bc) diag(m) ]
+</pre>
+where I_Bo is the inertia taken about the B frame origin Bo, and Bc is the
+vector p_BoBc from B's origin to its mass center.
+
+The Spatial Inertia Matrix for Ground has infinite mass and inertia, with
+the cross terms set to zero. That is, it looks like a 6x6 diagonal matrix
+with Infinity on the diagonals.
+
+ at par Required stage
+\c Stage::Position, unless \p objectBodyB == \c GroundIndex **/
+SpatialMat calcBodySpatialInertiaMatrixInGround(const State& state) const
+{
+ if (isGround())
+ return SpatialMat(Mat33(Infinity)); // sets diagonals to Inf
+
+ const MassProperties& mp = getBodyMassProperties(state);
+ const Rotation& R_GB = getBodyRotation(state);
+ // re-express in Ground without shifting, convert to spatial mat.
+ return mp.reexpress(~R_GB).toSpatialMat();
+}
+
+/** Return the central inertia for body B, that is, the inertia taken about
+body B's mass center Bc, and expressed in B.
+///
+ at par Required stage
+ \c Stage::Instance **/
+Inertia calcBodyCentralInertia(const State& state,
+ MobilizedBodyIndex objectBodyB) const
+{
+ return getBodyMassProperties(state).calcCentralInertia();
+}
+
+/** Return the inertia of this body B, taken about an arbitrary point PA of
+body A, and expressed in body A.
+TODO: this needs testing! **/
+Inertia calcBodyInertiaAboutAnotherBodyStation
+ (const State& state, const MobilizedBody& inBodyA,
+ const Vec3& aboutLocationOnBodyA) const
+{
+ // get B's mass props MB, measured about Bo, exp. in B
+ const MassProperties& MB_Bo_B = getBodyMassProperties(state);
+
+ // Calculate the vector from the body B origin (current "about" point) to
+ // the new "about" point PA, expressed in B.
+ const Vec3 p_Bo_PA =
+ findStationAtAnotherBodyStation(state, inBodyA, aboutLocationOnBodyA);
+
+ // Now shift the "about" point for body B's inertia IB to PA, but still
+ // expressed in B.
+ const Inertia IB_PA_B = MB_Bo_B.calcShiftedInertia(p_Bo_PA);
+
+ // Finally reexpress the inertia in the A frame.
+ const Rotation R_BA =
+ inBodyA.findBodyRotationInAnotherBody(state, *this);
+ const Inertia IB_PA_A = IB_PA_B.reexpress(R_BA);
+ return IB_PA_A;
+}
+
+
+/** Calculate body B's momentum (angular, linear) measured and expressed in
+Ground, but taken about the body origin Bo. **/
+SpatialVec calcBodyMomentumAboutBodyOriginInGround(const State& state) {
+ const MassProperties M_Bo_G = expressMassPropertiesInGroundFrame(state);
+ const SpatialVec& V_GB = getBodyVelocity(state);
+ return M_Bo_G.toSpatialMat() * V_GB;
+}
+
+/** Calculate body B's momentum (angular, linear) measured and expressed in
+Ground, but taken about the body mass center Bc. **/
+SpatialVec calcBodyMomentumAboutBodyMassCenterInGround
+ (const State& state) const {
+ const MassProperties& M_Bo_B = getBodyMassProperties(state);
+ const Rotation& R_GB = getBodyRotation(state);
+
+ // Given a central inertia matrix I, angular velocity w, and mass center
+ // velocity v, the central angular momentum is Iw and linear momentum is mv.
+ const Inertia I_Bc_B = M_Bo_B.calcCentralInertia();
+ const Inertia I_Bc_G = I_Bc_B.reexpress(~R_GB);
+ const Real mb = M_Bo_B.getMass();
+ const Vec3& w_GB = getBodyAngularVelocity(state);
+ Vec3 v_GBc =
+ findStationVelocityInGround(state, M_Bo_B.getMassCenter());
+
+ return SpatialVec( I_Bc_G*w_GB, mb*v_GBc );
+}
+
+/** Calculate the distance from a station PB on body B to a station PA on
+body A. We are given the location vectors (stations) p_Bo_PB and p_Ao_PA,
+expressed in their respective frames. We return |p_PB_PA|. **/
+Real calcStationToStationDistance(const State& state,
+ const Vec3& locationOnBodyB,
+ const MobilizedBody& bodyA,
+ const Vec3& locationOnBodyA) const
+{
+ if (isSameMobilizedBody(bodyA))
+ return (locationOnBodyA-locationOnBodyB).norm();
+
+ const Vec3 r_Go_PB =
+ this->findStationLocationInGround(state,locationOnBodyB);
+ const Vec3 r_Go_PA =
+ bodyA.findStationLocationInGround(state,locationOnBodyA);
+ return (r_Go_PA - r_Go_PB).norm();
+}
+
+/** Calculate the time rate of change of distance from a fixed point PB on
+body B to a fixed point PA on body A. We are given the location vectors
+p_Bo_PB and p_Ao_PA, expressed in their respective frames. We return
+d/dt |p_BoAo|, under the assumption that the time derivatives of the two given
+vectors in their own frames is zero. **/
+Real calcStationToStationDistanceTimeDerivative
+ (const State& state,
+ const Vec3& locationOnBodyB,
+ const MobilizedBody& bodyA,
+ const Vec3& locationOnBodyA) const
+{
+ if (isSameMobilizedBody(bodyA))
+ return 0;
+
+ Vec3 rB, rA, vB, vA;
+ this->findStationLocationAndVelocityInGround(state,locationOnBodyB,rB,vB);
+ bodyA.findStationLocationAndVelocityInGround(state,locationOnBodyA,rA,vA);
+ const Vec3 r = rA-rB, v = vA-vB;
+ const Real d = r.norm();
+
+ // When the points are coincident, the rate of change of distance is just
+ // their relative speed. Otherwise, it is the speed along the direction of
+ // separation.
+ if (d==0) return v.norm();
+ else return dot(v, r/d);
+}
+
+
+/** Calculate the second time derivative of distance from a fixed point PB on
+body B to a fixed point PA on body A. We are given the position vectors
+(stations) p_Bo_PB and p_Ao_PA, expressed in their respective frames. We return
+d^2/dt^2 |p_PB_PA|, under the assumption that the time derivatives of the two
+given vectors in their own frames is zero. **/
+Real calcStationToStationDistance2ndTimeDerivative
+ (const State& state,
+ const Vec3& locationOnBodyB,
+ const MobilizedBody& bodyA,
+ const Vec3& locationOnBodyA) const
+{
+ if (isSameMobilizedBody(bodyA))
+ return 0;
+
+ Vec3 rB, rA, vB, vA, aB, aA;
+ this->findStationLocationVelocityAndAccelerationInGround
+ (state,locationOnBodyB,rB,vB,aB);
+ bodyA.findStationLocationVelocityAndAccelerationInGround
+ (state,locationOnBodyA,rA,vA,aA);
+
+ const Vec3 r = rA-rB, v = vA-vB, a = aA-aB;
+ const Real d = r.norm();
+
+ // This method is the time derivative of
+ // calcFixedPointToPointDistanceTimeDerivative(), so it must follow the same
+ // two cases. That is, when the points are coincident the change in
+ // separation rate is the time derivative of the speed |v|, otherwise it is
+ // the time derivative of the speed along the separation vector.
+
+ if (d==0) {
+ // Return d/dt |v|. This has two cases: if |v| is zero, the rate of
+ // change of speed is just the points' relative acceleration magnitude.
+ // Otherwise, it is the acceleration in the direction of the current
+ // relative velocity vector.
+ const Real s = v.norm(); // speed
+ if (s==0) return a.norm();
+ else return dot(a, v/s);
+ }
+
+ // Points are separated.
+ const Vec3 u = r/d; // u is separation direction (a unit vector from B to A)
+ const Vec3 vp = v - dot(v,u)*u; // velocity perp. to separation direction
+ return dot(a,u) + dot(vp,v)/d;
+}
+
+
+/** TODO: not implemented yet -- any volunteers? Return the velocity of a
+point P moving on body B, in body A's frame, expressed in body A. **/
+Vec3 calcBodyMovingPointVelocityInBody
+ (const State& state,
+ const Vec3& locationOnBodyB,
+ const Vec3& velocityOnBodyB,
+ const MobilizedBody& inBodyA) const
+{
+ SimTK_ASSERT_ALWAYS(!"unimplemented method",
+ "MobilizedBody::calcBodyMovingPointVelocityInBody()"
+ " is not yet implemented -- any volunteers?");
+ return Vec3::getNaN();
+}
+
+
+/** TODO: not implemented yet -- any volunteers? Return the velocity of a point
+P moving (and possibly accelerating) on body B, in body A's frame, expressed in
+body A. **/
+Vec3 calcBodyMovingPointAccelerationInBody
+ (const State& state,
+ const Vec3& locationOnBodyB,
+ const Vec3& velocityOnBodyB,
+ const Vec3& accelerationOnBodyB,
+ const MobilizedBody& inBodyA) const
+{
+ SimTK_ASSERT_ALWAYS(!"unimplemented method",
+ "MobilizedBody::calcBodyMovingPointAccelerationInBody()"
+ " is not yet implemented -- any volunteers?");
+ return Vec3::getNaN();
+}
+
+/** TODO: not implemented yet -- any volunteers? Calculate the time rate of
+change of distance from a moving point PB on body B to a moving point PA on body
+A. We are given the location vectors p_Bo_PB and p_Ao_PA, and the velocities of
+PB in B and PA in A, all expressed in their respective frames. We return
+d/dt |p_BoAo|, taking into account the (given) time derivatives of the locations
+in their local frames, as well as the relative velocities of the bodies. **/
+Real calcMovingPointToPointDistanceTimeDerivative
+ (const State& state,
+ const Vec3& locationOnBodyB,
+ const Vec3& velocityOnBodyB,
+ const MobilizedBody& bodyA,
+ const Vec3& locationOnBodyA,
+ const Vec3& velocityOnBodyA) const
+{
+ SimTK_ASSERT_ALWAYS(!"unimplemented method",
+ "MobilizedBody::calcMovingPointToPointDistanceTimeDerivative()"
+ " is not yet implemented -- any volunteers?");
+ return NaN;
+}
+
+/** TODO: not implemented yet -- any volunteers? Calculate the second time
+derivative of distance from a moving point PB on body B to a moving point PA on
+body A. We are given the location vectors p_Bo_PB and p_Ao_PA, and the
+velocities and accelerations of PB in B and PA in A, all expressed in their
+respective frames. We return d^2/dt^2 |p_AoBo|, taking into account the time
+derivatives of the locations in their local frames, as well as the relative
+velocities and accelerations of the bodies. **/
+Real calcMovingPointToPointDistance2ndTimeDerivative
+ (const State& state,
+ const Vec3& locationOnBodyB,
+ const Vec3& velocityOnBodyB,
+ const Vec3& accelerationOnBodyB,
+ const MobilizedBody& bodyA,
+ const Vec3& locationOnBodyA,
+ const Vec3& velocityOnBodyA,
+ const Vec3& accelerationOnBodyA) const
+{
+ SimTK_ASSERT_ALWAYS(!"unimplemented method",
+ "MobilizedBody::calcMovingPointToPointDistance2ndTimeDerivative()"
+ " is not yet implemented -- any volunteers?");
+ return NaN;
+}
+
+
+// End of High Level Operators.
+/**@}**/
+
+
+ //////////////////////////
+ // CONSTRUCTION METHODS //
+ //////////////////////////
+
+/** The default constructor provides an empty %MobilizedBody handle that
+can be assigned to reference any type of %MobilizedBody. **/
+MobilizedBody() {}
+
+/** Internal use only. **/
+explicit MobilizedBody(MobilizedBodyImpl* r);
+
+//------------------------------------------------------------------------------
+/** @name Construction and Misc Methods
+These methods are the base class services which are used while building a
+concrete %MobilizedBody, or to query a %MobilizedBody to find out how it was
+built. These are unlikely to be used by end users of MobilizedBodies. **/
+/**@{**/
+
+/** Return a const reference to the Body contained within this %MobilizedBody.
+This refers to an internal copy of the Body that is owned by the
+%MobilizedBody. **/
+const Body& getBody() const;
+
+/** Return a writable reference to the Body contained within this
+%MobilizedBody. This refers to an internal copy of the Body that is owned by the
+%MobilizedBody. Calling this method invalidates the %MobilizedBody's topology,
+so the containing System's realizeTopology() method must be called again. **/
+Body& updBody();
+
+/** Replace the Body contained within this %MobilizedBody with a new one.
+Calling this method invalidates the %MobilizedBody's topology, so the containing
+System's realizeTopology() method must be called again. A reference to this
+%MobilizedBody is returned so that this can be chained like an assignment
+operator. **/
+MobilizedBody& setBody(const Body&);
+
+/** Add decorative geometry specified relative to the new (outboard) body's
+reference frame B. Note that the body itself may already have had some
+decorative geometry on it when it was first put into this MobilizedBody; this
+just adds more and the returned index is larger. Use the underlying Body
+object's accessors to find this decorative geometry again. The given
+\p geometry object is \e copied here; we do not keep a reference to the
+supplied object. **/
+int addBodyDecoration(const Transform& X_BD,
+ const DecorativeGeometry& geometry) {
+ return updBody().addDecoration(X_BD, geometry);
+}
+/** Convenience method for use when the \p geometry is supplied in the body
+frame. This is the same as addBodyDecoration(Transform(),geometry). **/
+int addBodyDecoration(const DecorativeGeometry& geometry) {
+ return updBody().addDecoration(geometry);
+}
+
+/** Add decorative geometry specified relative to the outboard mobilizer frame M
+attached to body B. If body B already has decorative geometry on it, this just
+adds some more, but kept in a separate list from the body decorations and
+inboard decorations. Returns a unique index that can be used to identify this
+outboard decoration later (numbered starting from zero for outboard decorations
+only). **/
+int addOutboardDecoration(const Transform& X_MD,
+ const DecorativeGeometry& geometry);
+/** Return the count of decorations added with addOutboardDecoration(). **/
+int getNumOutboardDecorations() const;
+/** Return a const reference to the i'th outboard decoration. **/
+const DecorativeGeometry& getOutboardDecoration(int i) const;
+/** Return a writable reference to the i'th outboard decoration. **/
+DecorativeGeometry& updOutboardDecoration(int i);
+
+/** Add decorative geometry specified relative to the inboard mobilizer frame F
+attached to the parent body P. If body P already has decorative geometry on it,
+this just adds some more, but kept in a separate list from the body decorations
+and outboard decorations. Returns a unique index that can be used to identify
+this inboard decoration later (numbered starting from zero for inboard
+decorations only). **/
+int addInboardDecoration(const Transform& X_FD,
+ const DecorativeGeometry& geometry);
+/** Return the count of decorations added with addInboardDecoration(). **/
+int getNumInboardDecorations() const;
+/** Return a const reference to the i'th inboard decoration. **/
+const DecorativeGeometry& getInboardDecoration(int i) const;
+/** Return a writable reference to the i'th inboard decoration. **/
+DecorativeGeometry& updInboardDecoration(int i);
+
+/** If the contained Body can have its mass properties set to the supplied value
+\p m its mass properties are changed, otherwise the method fails. Calling this
+method invalidates the MobilizedBody's topology, so the containing matter
+subsystem's realizeTopology() method must be called again. A reference to this
+%MobilizedBody is returned so that this can be chained like an assignment
+operator. **/
+MobilizedBody& setDefaultMassProperties(const MassProperties& m) {
+ updBody().setDefaultRigidBodyMassProperties(m); // might not be allowed
+ return *this;
+}
+
+/** Return the mass properties of the Body stored within this MobilizedBody. **/
+const MassProperties& getDefaultMassProperties() const {
+ // every body type can do this
+ return getBody().getDefaultRigidBodyMassProperties();
+}
+
+/** Provide a unique Motion object for this %MobilizedBody. The %MobilizedBody
+takes over ownership of the Motion object and is responsible for cleaning up
+its heap space when the time comes. This is a Topology-changing operation and
+consequently requires write access to the %MobilizedBody which will propagate
+to invalidate the containing Subsystem and System's topology. There can only
+be one Motion object per mobilizer; this method will throw an exception if
+there is already one here. **/
+void adoptMotion(Motion& ownerHandle);
+
+/** If there is a Motion object associated with this MobilizedBody it is
+removed; otherwise, nothing happens. If a Motion is deleted, the containing
+System's topology is invalidated. **/
+void clearMotion();
+
+/** Check whether this %MobilizedBody has an associated Motion object. This does
+not tell you whether the Motion object is currently enabled or in use; just
+whether it is available. **/
+bool hasMotion() const;
+
+/** If there is a Motion object associated with this MobilizedBody, this returns
+a const reference to it. Otherwise it will throw an exception. You can check
+first using hasMotion(). Note that there is no provision to obtain a writable
+reference to the contained Motion object; if you want to change it clear the
+existing object instead and replace it with a new one.
+ at see hasMotion() **/
+const Motion& getMotion() const;
+
+/** Change this mobilizer's frame F on the parent body P. Calling this method
+invalidates the %MobilizedBody's topology, so the containing matter subsystem's
+realizeTopology() method must be called again. A reference to this
+%MobilizedBody is returned so that this can be chained like an assignment
+operator. **/
+MobilizedBody& setDefaultInboardFrame (const Transform& X_PF);
+/** Change this mobilizer's frame M fixed on this (the outboard) body B. Calling
+this method invalidates the %MobilizedBody's topology, so the containing
+matter subsystem's realizeTopology() method must be called again. A reference
+to this %MobilizedBody is returned so that this can be chained like an
+assignment operator. **/
+MobilizedBody& setDefaultOutboardFrame(const Transform& X_BM);
+
+/** Return a reference to this mobilizer's default for the frame F fixed on the
+parent (inboard) body P, as the fixed Transform from P's body frame to the frame
+F fixed to P. This default Transform is stored with the %MobilizedBody object,
+not the State. **/
+const Transform& getDefaultInboardFrame() const; // X_PF
+/** Return a reference to this %MobilizedBody's default for mobilizer frame M,
+as the fixed Transform from this body B's frame to the frame M fixed on B. This
+default Transform is stored with the %MobilizedBody object, not the State. **/
+const Transform& getDefaultOutboardFrame() const; // X_BM
+
+/** This is an implicit conversion from %MobilizedBody to MobilizedBodyIndex
+when needed. This will fail unless this %MobilizedBody is owned by some
+SimbodyMatterSubsystem. We guarantee that the MobilizedBodyIndex of a mobilized
+body is numerically larger than the MobilizedBodyIndex of its parent. **/
+operator MobilizedBodyIndex() const {return getMobilizedBodyIndex();}
+
+/** Return the MobilizedBodyIndex of this MobilizedBody within the owning
+SimbodyMatterSubsystem. This will fail unless this MobilizedBody is owned by
+some SimbodyMatterSubsystem. We guarantee that the MobilizedBodyIndex of a
+mobilized body is numerically larger than the MobilizedBodyIndex of its
+parent. **/
+MobilizedBodyIndex getMobilizedBodyIndex() const;
+
+/** Return a reference to the MobilizedBody serving as the parent body of the
+current MobilizedBody. This call will fail if the current MobilizedBody is
+Ground, since Ground has no parent. **/
+const MobilizedBody& getParentMobilizedBody() const;
+
+/** Return a reference to this %MobilizedBody's oldest ancestor other than
+Ground, or return Ground if this %MobilizedBody is Ground. That is, we return
+the "base" %MobilizedBody for this %MobilizedBody, meaning the one which
+connects this branch of the multibody tree directly to Ground. **/
+const MobilizedBody& getBaseMobilizedBody() const;
+
+/** Obtain a reference to the SimbodyMatterSubsystem which contains this
+%MobilizedBody. This will fail unless this MobilizedBody is owned by some
+SimbodyMatterSubsystem. **/
+const SimbodyMatterSubsystem& getMatterSubsystem() const;
+/** Obtain a writable reference to the SimbodyMatterSubsystem which contains
+this %MobilizedBody. This will fail unless this %MobilizedBody is owned by some
+SimbodyMatterSubsystem. **/
+SimbodyMatterSubsystem& updMatterSubsystem();
+
+/** Determine whether the current MobilizedBody object is owned by a matter
+subsystem. **/
+bool isInSubsystem() const;
+
+/** Determine whether a given MobilizedBody \p mobod is in the same matter
+subsystem as the current body. If the bodies are not in a subsystem, this
+routine will return \c false. **/
+bool isInSameSubsystem(const MobilizedBody& mobod) const;
+
+/** Determine whether a given %MobilizedBody \p mobod is the same %MobilizedBody
+as this one. For this to be true the handles must not be empty, and the
+implementation objects must be <em>the same object</em> not separate objects
+with identical contents. **/
+bool isSameMobilizedBody(const MobilizedBody& mobod) const;
+
+/** Determine whether this body is Ground, meaning that it is actually
+body 0 of some matter subsytem, not just that its body type is Ground. **/
+bool isGround() const;
+
+/** Return this body's level in the tree of bodies, starting with ground at 0,
+bodies directly connected to ground at 1, bodies directly connected to those at
+2, etc. This is callable after realizeTopology(). This is the graph distance of
+the body from Ground. **/
+int getLevelInMultibodyTree() const;
+
+/** Create a new MobilizedBody which is identical to this one, except that it
+has a different parent (and consequently might belong to a different
+MultibodySystem). **/
+MobilizedBody& cloneForNewParent(MobilizedBody& parent) const;
+
+
+ // Utility operators //
+
+/** This utility selects one of the q's (generalized coordinates) associated
+with this mobilizer from a supplied "q-like" Vector, meaning a Vector which is
+the same length as the Vector of q's for the containing matter subsystem. **/
+Real getOneFromQPartition
+ (const State& state, int which, const Vector& qlike) const;
+
+/** This utility returns a writable reference to one of the q's (generalized
+coordinates) associated with this mobilizer from a supplied "q-like" Vector,
+meaning a Vector which is the same length as the Vector of q's for the
+containing matter subsystem. **/
+Real& updOneFromQPartition
+ (const State& state, int which, Vector& qlike) const;
+
+/** This utility selects one of the u's (generalized speeds) associated with
+this mobilizer from a supplied "u-like" Vector, meaning a Vector which is
+the same length as the Vector of u's for the containing matter subsystem. **/
+Real getOneFromUPartition
+ (const State& state, int which, const Vector& ulike) const;
+
+/** This utility returns a writable reference to one of the u's (generalized
+speeds) associated with this mobilizer from a supplied "u-like" Vector, meaning
+a Vector which is the same length as the Vector of u's for the containing matter
+subsystem. **/
+Real& updOneFromUPartition(const State& state, int which, Vector& ulike) const;
+
+/** This utility adds in the supplied generalized force \p f (a scalar) to
+the appropriate slot of the supplied \p mobilityForces Vector, which is a
+"u-like" Vector. Note that we are <em>adding</em> this not <em>setting</em> it
+so it important that \p mobilityForces be initialized to zero before making a
+set of calls to applyOneMobilityForce(). **/
+void applyOneMobilityForce(const State& state, int which, Real f,
+ Vector& mobilityForces) const
+{
+ updOneFromUPartition(state,which,mobilityForces) += f;
+}
+
+/** Given a generalized force in the q-space of this mobilizer, convert it to
+the equivalent generalized mobility force (u-space force). This uses the
+kinematic coupling matrix N that appears in equation (1) qdot=N*u. Here we
+compute (2) fu = ~N*fq (that's N transpose, not inverse).
+
+Simbody deals with generalized forces in mobility (u) space, but sometimes these
+are more convenient to generate in generalized coordinate (q) space. In that
+case this utility method is useful to perform the conversion from q space to u
+space that is necessary for communicating the force to Simbody.
+
+ at param[in] state
+ A State already realized through Position stage, from which this mobilizer's
+ kinematic coupling matrix N(q) is obtained.
+ at param[in] fq
+ This is a generalized force in the space of the generalized coordinates q
+ rather than the generalized speeds u. The length of \p fq must be nq, the
+ number of q's currently being used by this mobilizer in the given \p state.
+ (This can depend on a Model-stage state variable.)
+ at param[out] fu
+ This is the generalized force in mobility space (the space of the
+ generalized speeds u) that is equivalent to \p fq. \p fu will be resized if
+ necessary to length nu, the number of u's being used by this mobilizer.
+
+<h3>Theory</h3>
+The physical quantity power (force times velocity) must not change as a result
+of a change of coordinates. Hence we must have ~fq*qdot==~fu*u which follows
+from equations (1) and (2): multiply (1) by ~fq to get
+<pre>
+ ~fq*qdot= ~fq*N*u
+ = ~(~N*fq)*u
+ = ~fu*u from equation (2).
+</pre>
+For any mobilizer where qdot==u this simply copies the input to the output.
+Otherwise a multiplication by ~N is done, but that is very fast since N has
+already been computed. Cost depends on type of mobilizer but is unlikely to
+exceed 25 flops. **/
+void convertQForceToUForce(const State& state,
+ const Array_<Real,MobilizerQIndex>& fq,
+ Array_<Real,MobilizerUIndex>& fu) const;
+
+/** This utility adds in the supplied spatial force \p spatialForceInG
+(consisting of a torque vector, and a force vector to be applied at the current
+body's origin) to the appropriate slot of the supplied \p bodyForcesInG Vector.
+Note that we are <em>adding</em> this not <em>setting</em> it so it is important
+that \p bodyForcesInG be initialized to zero before making a set of calls to
+applyBodyForce(). **/
+void applyBodyForce(const State& state, const SpatialVec& spatialForceInG,
+ Vector_<SpatialVec>& bodyForcesInG) const;
+
+/** This utility adds in the supplied pure torque \p torqueInG to the
+appropriate slot of the supplied \p bodyForcesInG Vector. Note that we are
+<em>adding</em> this not <em>setting</em> it so it is important that
+\p bodyForcesInG be initialized to zero before making a set of calls to
+applyBodyTorque(). **/
+void applyBodyTorque(const State& state, const Vec3& torqueInG,
+ Vector_<SpatialVec>& bodyForcesInG) const;
+
+/** This utility adds in the supplied force \p forceInG applied at a point
+\p pointInB to the appropriate slot of the supplied \p bodyForcesInG Vector.
+Notes:
+ - we are <em>adding</em> this not <em>setting</em> it so it is important that
+ \p bodyForcesInG be initialized to zero before making a set of calls to
+ applyForceToBodyPoint().
+ - \p pointInB represents a fixed station of B and is provided by giving the
+ vector from body B's origin to the point, expressed in the B frame, while
+ the applied force (and resulting body forces and torques) are expressed
+ in the ground frame. **/
+void applyForceToBodyPoint
+ (const State& state, const Vec3& pointInB, const Vec3& forceInG,
+ Vector_<SpatialVec>& bodyForcesInG) const;
+
+// End of Construction and Misc Methods.
+/**@}**/
+
+ /////////////////////////////////////
+ // BUILT IN MOBILIZER DECLARATIONS //
+ /////////////////////////////////////
+
+//------------------------------------------------------------------------------
+// These are the built-in MobilizedBody types. Each of these has a known
+// number of coordinates and speeds (at least a default number) so
+// can define routines which return and accept specific-size arguments, e.g.
+// Real (for 1-dof mobilizer) and Vec5 (for 5-dof mobilizer). Here is the
+// conventional interface that each built-in should provide. The base type
+// provides similar routines but using variable-sized or "one at a time"
+// arguments. (Vec<1> here will actually be a Real; assume the built-in
+// MobilizedBody class is "BuiltIn")
+//
+// BuiltIn& setDefaultQ(const Vec<nq>&);
+// const Vec<nq>& getDefaultQ() const;
+//
+// const Vec<nq>& getQ[Dot[Dot]](const State&) const;
+// const Vec<nu>& getU[Dot](const State&) const;
+//
+// void setQ(State&, const Vec<nq>&) const;
+// void setU(State&, const Vec<nu>&) const;
+//
+// const Vec<nq>& getMyPartQ(const State&, const Vector& qlike) const;
+// const Vec<nu>& getMyPartU(const State&, const Vector& ulike) const;
+//
+// Vec<nq>& updMyPartQ(const State&, Vector& qlike) const;
+// Vec<nu>& updMyPartU(const State&, Vector& ulike) const;
+//
+// Each built in mobilized body type is declared in its own header
+// file using naming convention MobilizedBody_Pin.h, for example. All the
+// built-in headers are collected in MobilizedBody_BuiltIns.h; you should
+// include new ones there also.
+
+
+class Pin;
+typedef Pin Torsion; ///< Synonym for Pin mobilizer.
+typedef Pin Revolute; ///< Synonym for Pin mobilizer.
+
+class Universal;
+class Cylinder;
+class Weld;
+
+class Slider;
+typedef Slider Prismatic; ///< Synonym for Slider mobilizer.
+
+class Translation;
+typedef Translation Cartesian; ///< Synonym for Translation mobilizer.
+typedef Translation CartesianCoords; ///< Synonym for Translation mobilizer.
+
+class BendStretch;
+typedef BendStretch PolarCoords; ///< Synonym for BendStretch mobilizer.
+
+class SphericalCoords;
+class LineOrientation;
+
+class Planar;
+class Gimbal;
+class Bushing;
+
+class Ball;
+typedef Ball Orientation; ///< Synonym for Ball mobilizer.
+typedef Ball Spherical; ///< Synonym for Ball mobilizer.
+
+class Free;
+class FreeLine;
+class Screw;
+class Ellipsoid;
+class Custom;
+class Ground;
+class FunctionBased;
+
+// Internal use only.
+class PinImpl;
+class SliderImpl;
+class UniversalImpl;
+class CylinderImpl;
+class BendStretchImpl;
+class PlanarImpl;
+class GimbalImpl;
+class BushingImpl;
+class BallImpl;
+class TranslationImpl;
+class SphericalCoordsImpl;
+class FreeImpl;
+class LineOrientationImpl;
+class FreeLineImpl;
+class WeldImpl;
+class ScrewImpl;
+class EllipsoidImpl;
+class CustomImpl;
+class GroundImpl;
+class FunctionBasedImpl;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_Ball.h b/Simbody/include/simbody/internal/MobilizedBody_Ball.h
new file mode 100644
index 0000000..2f9a055
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_Ball.h
@@ -0,0 +1,120 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_BALL_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_BALL_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy, 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::Ball class. **/
+
+#include "simbody/internal/MobilizedBody.h"
+
+namespace SimTK {
+
+
+/** Three mobilities -- unrestricted orientation modeled with a
+quaternion which is never singular. A modeling option allows the
+joint to use a 1-2-3 Euler sequence (identical to a Gimbal)
+instead. The three generalized speeds u for this mobilizer are always
+the three measure numbers of the angular velocity vector w_FM, the
+relative angular velocity of the outboard M frame in the inboard F frame,
+expressed in the F frame. That is unchanged by setting the "use Euler
+angles" modeling option, so the generalized speeds differ from those of
+a Gimbal joint. Note that qdot != u for this mobilizer. **/
+class SimTK_SIMBODY_EXPORT MobilizedBody::Ball : public MobilizedBody {
+public:
+ /** Default constructor provides an empty handle that can be assigned to
+ reference any %MobilizedBody::Ball. **/
+ Ball() {}
+
+ /** Create a %Ball mobilizer between an existing parent (inboard) body P
+ and a new child (outboard) body B created by copying the given \a bodyInfo
+ into a privately-owned Body within the constructed %MobilizedBody object.
+ Specify the mobilizer frames F fixed to parent P and M fixed to child B.
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ Ball(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM, Direction=Forward);
+
+ /** Abbreviated constructor you can use if the mobilizer frames are
+ coincident with the parent and child body frames. **/
+ Ball(MobilizedBody& parent, const Body& bodyInfo, Direction=Forward);
+
+ Ball& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
+ }
+ Ball& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
+ }
+ Ball& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
+ }
+
+ Ball& setDefaultInboardFrame(const Transform& X_PF) {
+ (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
+ }
+
+ Ball& setDefaultOutboardFrame(const Transform& X_BM) {
+ (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
+ }
+
+ // This is just a nicer name for the generalized coordinate.
+ Ball& setDefaultRotation(const Rotation& R_FM) {
+ return setDefaultQ(R_FM.convertRotationToQuaternion());
+ }
+ Rotation getDefaultRotation() const {return Rotation(getDefaultQ());}
+
+ // This is used only for visualization.
+ Ball& setDefaultRadius(Real r);
+ Real getDefaultRadius() const;
+
+ // Generic default state Topology methods.
+ const Quaternion& getDefaultQ() const;
+ Ball& setDefaultQ(const Quaternion& q);
+
+ const Vec4& getQ(const State&) const;
+ const Vec4& getQDot(const State&) const;
+ const Vec4& getQDotDot(const State&) const;
+ const Vec3& getU(const State&) const;
+ const Vec3& getUDot(const State&) const;
+
+ void setQ(State&, const Vec4&) const;
+ void setU(State&, const Vec3&) const;
+
+ const Vec4& getMyPartQ(const State&, const Vector& qlike) const;
+ const Vec3& getMyPartU(const State&, const Vector& ulike) const;
+
+ Vec4& updMyPartQ(const State&, Vector& qlike) const;
+ Vec3& updMyPartU(const State&, Vector& ulike) const;
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Ball, BallImpl, MobilizedBody);
+ /** @endcond **/
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_BALL_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_BendStretch.h b/Simbody/include/simbody/internal/MobilizedBody_BendStretch.h
new file mode 100644
index 0000000..2843ad0
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_BendStretch.h
@@ -0,0 +1,91 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_BENDSTRETCH_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_BENDSTRETCH_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::BendStretch class. **/
+
+#include "simbody/internal/MobilizedBody.h"
+
+namespace SimTK {
+
+/** Two mobilities: The z axis of the parent's F frame is
+used for rotation (and that is always aligned with the M frame z axis).
+The x axis of the *M* (outboard) frame is then used for translation;
+that is, first we rotate around z, which moves M's x with respect to F's x. Then
+we slide along the rotated x axis. The two generalized coordinates are the
+rotation and the translation, in that order.
+This can also be viewed a a 2D polar coordinate mobilizer since the coordinates
+are (theta, r) about perpendicular axes. **/
+class SimTK_SIMBODY_EXPORT MobilizedBody::BendStretch : public MobilizedBody {
+public:
+ /** Default constructor provides an empty handle that can be assigned to
+ reference any %MobilizedBody::BendStretch. **/
+ BendStretch() {}
+
+ /** Create a %BendStretch mobilizer between an existing parent (inboard)
+ body P and a new child (outboard) body B created by copying the given
+ \a bodyInfo into a privately-owned Body within the constructed
+ %MobilizedBody object. Specify the mobilizer frames F fixed to parent P
+ and M fixed to child B.
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ BendStretch(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM, Direction=Forward);
+
+ /** Abbreviated constructor you can use if the mobilizer frames are
+ coincident with the parent and child body frames. **/
+ BendStretch(MobilizedBody& parent, const Body& bodyInfo, Direction=Forward);
+
+ BendStretch& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
+ }
+ BendStretch& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
+ }
+ BendStretch& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
+ }
+
+ BendStretch& setDefaultInboardFrame(const Transform& X_PF) {
+ (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
+ }
+
+ BendStretch& setDefaultOutboardFrame(const Transform& X_BM) {
+ (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
+ }
+
+ /** @cond **/ // Don't let doxygen see this
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(BendStretch, BendStretchImpl,
+ MobilizedBody);
+ /** @endcond **/
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_BENDSTRETCH_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_BuiltIns.h b/Simbody/include/simbody/internal/MobilizedBody_BuiltIns.h
new file mode 100644
index 0000000..784d68a
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_BuiltIns.h
@@ -0,0 +1,55 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_BUILTINS_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_BUILTINS_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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Include the header files that define each of the built-in mobilizer
+subclasses of MobilizedBody. **/
+
+#include "simbody/internal/MobilizedBody_Ball.h"
+#include "simbody/internal/MobilizedBody_BendStretch.h"
+#include "simbody/internal/MobilizedBody_Bushing.h"
+#include "simbody/internal/MobilizedBody_Custom.h"
+#include "simbody/internal/MobilizedBody_Cylinder.h"
+#include "simbody/internal/MobilizedBody_Ellipsoid.h"
+#include "simbody/internal/MobilizedBody_Free.h"
+#include "simbody/internal/MobilizedBody_FreeLine.h"
+#include "simbody/internal/MobilizedBody_FunctionBased.h"
+#include "simbody/internal/MobilizedBody_Gimbal.h"
+#include "simbody/internal/MobilizedBody_Ground.h"
+#include "simbody/internal/MobilizedBody_LineOrientation.h"
+#include "simbody/internal/MobilizedBody_Pin.h"
+#include "simbody/internal/MobilizedBody_Planar.h"
+#include "simbody/internal/MobilizedBody_Screw.h"
+#include "simbody/internal/MobilizedBody_Slider.h"
+#include "simbody/internal/MobilizedBody_SphericalCoords.h"
+#include "simbody/internal/MobilizedBody_Translation.h"
+#include "simbody/internal/MobilizedBody_Universal.h"
+#include "simbody/internal/MobilizedBody_Weld.h"
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_BUILTINS_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_Bushing.h b/Simbody/include/simbody/internal/MobilizedBody_Bushing.h
new file mode 100644
index 0000000..da58db2
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_Bushing.h
@@ -0,0 +1,258 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_BUSHING_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_BUSHING_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) 2012-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::Bushing class. **/
+
+#include "simbody/internal/MobilizedBody.h"
+
+namespace SimTK {
+
+/** Six mobilities -- arbitrary relative motion modeled as x-y-z translation
+followed by an x-y-z body-fixed Euler angle sequence, though with a
+singularity when the middle angle is +/- 90 degrees. Use MobilizedBody::Free
+for a similar six-dof joint that is free of singularities.
+
+The six generalized coordinates q are defined q={qx,qy,qz,px,py,pz} where the
+first three coordinates are the rotation angles and the last three are the
+Cartesian translations. Note that we follow the usual Simbody convention of
+having the rotational coordinates precede the translational ones; that does
+not imply any particular order of operations. In fact, as a sequence this
+mobilizer should be thought of as first translating the M frame by a vector
+p_FM=[px,py,pz] (expressed in F), then reorienting M about its new origin
+location using the rotation angles. In the reference configuration where q=0,
+the F and M frames are aligned and their origins coincident (Mo=Fo). The
+convention used here exactly matches the convention used by the force element
+Force::LinearBushing which infers an identical set of coordinates to express the
+relative pose of its two connected frames (also called F and M).
+
+The first three generalized coordinates q should be interpreted as a series of
+rotation angles in radians: q[0]=qx is a rotation about the Mx (=Fx) axis, then
+q[1]=qy is a rotation about the now-rotated My axis, and then q[2]=qz is a
+rotation about the now twice-rotated Mz axis. These rotations do not affect the
+location of M's origin point Mo. The generalized speeds u for the %Bushing
+mobilizer are just the time derivatives of the generalized coordinates, that is,
+u=qdot={qxdot,qydot,qzdot,vx,vy,vz}, where qxdot=d/dt qx, etc. and
+v_FM=[vx,vy,vz]=d/dt p_FM is the velocity of Mo measured and expressed in frame
+F. Note that this is different than the choice of generalized speeds used for
+MobilizedBody::Free, where the angular velocity vector w_FM is used as the three
+rotational generalized speeds instead of Euler angle derivatives. (Euler angle
+derivatives are \e not the same as angular velocity, except when q=0.)
+
+While this mobilizer provides arbitrary orientation, the Euler angle derivatives
+are singular when q[1] (qy, the middle rotation) is near +/- Pi/2. That means
+you should not attempt to do any dynamics in that configuration; if you can't
+be sure the motion will remain away from that region, you should use a
+MobilizedBody::Free instead, which uses quaternions to ensure singularity-free
+rotation.
+
+Here is another way to describe the meaning of a %Bushing mobilizer: except
+for the ordering of the generalized coordinates, a %Bushing between frames F
+on parent body P and frame M on child body B is exactly equivalent to a
+translation (Cartesian) mobilizer between F and a massless body B0, followed by
+a series of three pin joints using two massless intermediate bodies B1 and B2.
+The first pin joint rotates about the common B0x(=Fx) and B1x axes. The second
+rotates about the common B1y and B2y axes. The last one rotates about the common
+B2z and Mz axes. Other than performance (which is somewhat better if you use the
+%Bushing since there are fewer bodies), the translation+pins+intermediate
+bodies system generates exactly the same mobility as the %Bushing, but with
+coordinates q={px,py,pz,qx,qy,qz} (flipped from the %Bushing coordinate
+ordering).
+
+ at see Force::LinearBushing, MobilizedBody::Free, MobilizedBody::Gimbal **/
+class SimTK_SIMBODY_EXPORT MobilizedBody::Bushing : public MobilizedBody {
+public:
+ /** Construct an empty handle that can be assigned to reference any
+ %MobilizedBody::Bushing. **/
+ Bushing() {}
+
+ /** Create a %Bushing mobilizer between an existing parent (inboard) body P
+ and a new child (outboard) body B created by copying the given \a bodyInfo
+ into a privately-owned Body within the constructed %MobilizedBody object.
+ Specify the mobilizer frames F fixed to parent P and M fixed to child B.
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ Bushing(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM, Direction=Forward);
+
+ /** Abbreviated constructor you can use if the mobilizer frames are
+ coincident with the parent and child body frames. **/
+ Bushing(MobilizedBody& parent, const Body& bodyInfo, Direction=Forward);
+
+ /** Change the default inboard ("fixed") frame F on the parent body of this
+ mobilizer. The supplied frame is given as a Transform from the parent
+ body's frame P and replaces the F frame specified in the constructor. This
+ is a topological change, meaning that you must call realizeTopology() again
+ if you call this method. **/
+ Bushing& setDefaultInboardFrame(const Transform& X_PF)
+ { (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this; }
+
+ /** Change the default outboard ("moving") frame M on this mobilized body
+ (that is, on this mobilizer's child body). The supplied frame is given as
+ a Transform from the child body's frame B and replaces the M frame specified
+ in the constructor. This is a topological change, meaning that you must
+ call realizeTopology() again if you use this method. **/
+ Bushing& setDefaultOutboardFrame(const Transform& X_BM)
+ { (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this; }
+
+ /** Override the default pose for this mobilizer. Normally the
+ default is q=0, meaning that the F and M frames are aligned. Here you can
+ provide a Transform giving the default pose of the mobilizer's M frame in
+ the parent body's F frame. The given orientation will be converted to a
+ body fixed x-y-z Euler sequence; the given position vector will be used
+ directly. Those six values will be used to set the default values for the
+ generalized coordinates q that will appear in a State that has just been
+ created by a realizeTopology() call. This is a topological change, meaning
+ that you must call realizeTopology() again if you use this method. **/
+ Bushing& setDefaultTransform(const Transform& X_FM)
+ { Vec6 q;
+ q.updSubVec<3>(0) = X_FM.R().convertRotationToBodyFixedXYZ();
+ q.updSubVec<3>(3) = X_FM.p();
+ return setDefaultQ(q); }
+
+ /** Return the default pose for this mobilizer as a Transform. See
+ setDefaultTransform() for more information. **/
+ Transform getDefaultTransform() const {
+ const Vec6& q = getDefaultQ();
+ return Transform(Rotation(BodyRotationSequence,
+ q[0], XAxis, q[1], YAxis, q[2], ZAxis),
+ q.getSubVec<3>(3));
+ }
+
+ /** Add DecorativeGeometry to the privately-owned Body contained in this
+ %MobilizedBody, positioned relative to the body's frame. **/
+ Bushing& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g)
+ { (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this; }
+
+ /** Add DecorativeGeometry to the privately-owned Body contained in this
+ %MobilizedBody, positioned relative to the mobilizer's M frame on that
+ body. **/
+ Bushing& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g)
+ { (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this; }
+
+ /** Add DecorativeGeometry to the parent (inboard) body of this mobilizer,
+ positioned relative to the mobilizer's F frame on the parent body. **/
+ Bushing& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g)
+ { (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this; }
+
+ /** Obtain the default value for the generalized coordinates q that has
+ been set for this mobilizer. The value is returned as a Vec6 but note
+ that the rotational part (first three elements) of the returned quantity
+ is \e not a vector; it is a body fixed x-y-z Euler angle sequence in
+ radians. The last three elements are a position vector p_FM. The default
+ is q=0 unless overridden.
+ @see getDefaultTransform() **/
+ const Vec6& getDefaultQ() const;
+ /** Set the default value for the generalized coordinates q for this
+ mobilizer. The value is given as a Vec6 but note
+ that the supplied quantity is \e not a pair of vectors; it is a body fixed x-y-z
+ Euler angle sequence in radians, followed by a position vector in the
+ last three elements. The default is q=0 unless overridden.
+ @see setDefaultTransform() **/
+ Bushing& setDefaultQ(const Vec6& q);
+
+ /** Obtain the current value for this mobilizer's generalized coordinates
+ q from the given State, which must be realized through Stage::Model (these
+ are state variables). **/
+ const Vec6& getQ(const State& state) const;
+ /** Obtain the current value for this mobilizer's generalized coordinate
+ time derivatives qdot from the given State, which must be realized through
+ Stage::Velocity. These are the three Euler angle time derivatives followed
+ by the three measure numbers of the velocity vector v_FM giving the
+ velocity of M's origin Mo taken in the F frame. For the %Bushing
+ mobilizer, qdot has the same numerical values as the generalized speeds u,
+ but unlike u, qdot is not a state variable. **/
+ const Vec6& getQDot(const State& state) const;
+ /** Obtain the current value for this mobilizer's generalized coordinate
+ second time derivatives qdotdot from the given State, which must be
+ realized through Stage::Acceleration. These are Euler angle second time
+ derivatives followed by the three measure numbers of the vector a_FM
+ giving the acceleration of M's origin Mo taken in the F frame. For the
+ %Bushing mobilizer, qdotdot=udot. **/
+ const Vec6& getQDotDot(const State& state) const;
+
+ /** Obtain the current value for this mobilizer's generalized speeds u
+ from the given State, which must be realized through Stage::Model (these are
+ state variables). For the %Bushing joint, these are the time derivatives of
+ q, that is, u=qdot. **/
+ const Vec6& getU(const State& state) const;
+ /** Obtain the current value for this mobilizer's generalized accelerations
+ udot (=d/dt u) from the given State, which must be realized through
+ Stage::Acceleration. For the %Bushing joint, these are the time derivatives
+ of qdot, that is, udot=qdotdot. **/
+ const Vec6& getUDot(const State& state) const;
+
+ /** Set new values for this mobilizer's generalized coordinates q in the
+ given \a state. This invalidates Stage::Position and above. The new
+ value is given as a Vec6, but is interpreted as a body fixed x-y-z
+ Euler angle sequence in radians, followed by the three measure numbers of
+ the position vector p_FM giving the location of the M frame origin Mo
+ measured from and expressed in the F frame.
+ @see MobilizedBody::setQToFitTransform() **/
+ void setQ(State& state, const Vec6& q) const;
+ /** Set new values for this mobilizer's generalized speeds u in the given
+ \a state. This invalidates Stage::Velocity and above. The new value is given
+ as a Vec6, but is interpreted as the time derivatives of a body fixed x-y-z
+ Euler angle sequence in radians/time (\e not an angular velocity!), followed
+ by the three measure numbers of the velocity vector v_FM giving the velocity
+ of the M frame origin Mo measured and expressed in the F frame. For the
+ %Bushing mobilizer, these are just the time derivatives of the generalized
+ coordinates q, that is, u=qdot.
+ @see MobilizedBody::setUToFitVelocity() **/
+ void setU(State& state, const Vec6& u) const;
+
+ /** @name Advanced/Obscure
+ Most users won't use these methods. **/
+ /**@{**/
+
+ /** Given a vector in the System's generalized coordinate basis, extract
+ the six q's belonging to this mobilizer. **/
+ const Vec6& getMyPartQ(const State& state, const Vector& qlike) const;
+ /** Given a vector in the System's generalized speed basis, extract
+ the six u's belonging to this mobilizer. **/
+ const Vec6& getMyPartU(const State& state, const Vector& ulike) const;
+
+ /** Given a writable vector in the System's generalized coordinate basis,
+ extract a writable reference to the six q's in that vector that belong to
+ this mobilizer. **/
+ Vec6& updMyPartQ(const State& state, Vector& qlike) const;
+ /** Given a writable vector in the System's generalized speed basis,
+ extract a writable reference to the six u's in that vector that belong to
+ this mobilizer. **/
+ Vec6& updMyPartU(const State& state, Vector& ulike) const;
+ /**@}**/
+
+ /** @cond **/ // Hide from Doxygen.
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Bushing, BushingImpl, MobilizedBody);
+ /** @endcond **/
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_BUSHING_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_Custom.h b/Simbody/include/simbody/internal/MobilizedBody_Custom.h
new file mode 100644
index 0000000..43be8b0
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_Custom.h
@@ -0,0 +1,463 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_CUSTOM_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_CUSTOM_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-13 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::Custom and MobilizedBody::Custom::Implementation
+subclasses. **/
+
+#include "simbody/internal/MobilizedBody.h"
+
+namespace SimTK {
+
+//==============================================================================
+// MOBILIZED BODY :: CUSTOM
+//==============================================================================
+/** The handle class MobilizedBody::Custom (dataless) and its companion class
+MobilizedBody::Custom::Implementation can be used together to define new
+MobilizedBody types with arbitrary properties. To use it, create a class that
+extends MobilizedBody::Custom::Implementation. You can then create an instance
+of it and pass it to the MobilizedBody::Custom constructor:
+
+ at code
+MobilizedBody::Custom myMobilizedBody(new MyMobilizedBodyImplementation(args));
+ at endcode
+("args" here and below stands for whatever arguments are needed for your
+particular mobilizer; it isn't meant literally.)
+
+Alternatively, you can also create a new Handle class which is a subclass of
+MobilizedBody::Custom and which creates the Implementation itself in its
+constructors.
+ at code
+class MyMobilizedBody : public MobilizedBody::Custom {
+public:
+ MyMobilizedBody(args)
+ : MobilizedBody::Custom(new MyMobilizedBodyImplementation(args)) {}
+};
+ at endcode
+
+This allows an end user to simply write
+ at code
+MyMobilizedBody(args);
+ at endcode
+
+and not worry about implementation classes or creating objects on the heap.
+If you do this, your MobilizedBody::Custom subclass must not have any data
+members or virtual methods. If it does, it will not work correctly. Instead,
+store all data in the Implementation subclass.
+
+ at see MobilizedBody::Custom::Implementation **/
+class SimTK_SIMBODY_EXPORT MobilizedBody::Custom : public MobilizedBody {
+public:
+ class Implementation;
+ class ImplementationImpl;
+
+ /** Default constructor provides an empty handle that can be assigned to
+ reference any %MobilizedBody::Custom. **/
+ Custom() {}
+
+ /** Create a %Custom mobilizer between an existing parent (inboard) body P
+ and a new child (outboard) body B created by copying the given \a bodyInfo
+ into a privately-owned Body within the constructed %MobilizedBody object.
+ Specify the mobilizer frames F fixed to parent P and M fixed to child B.
+ @see MobilizedBody for a diagram and explanation of terminology.
+
+ @param parent the %MobilizedBody's parent (inboard) body
+ @param implementation the object which implements the custom mobilized body.
+ The %MobilizedBody::Custom takes over ownership of the
+ implementation object, and deletes it when the
+ %MobilizedBody itself is deleted.
+ @param X_PF the %MobilizedBody's F (inboard) frame
+ @param bodyInfo describes this %MobilizedBody's physical properties
+ @param X_BM the %MobilizedBody's M (outboard) frame
+ @param direction whether you want the coordinates defined as though
+ parent & child were swapped
+ **/
+ Custom(MobilizedBody& parent, Implementation* implementation,
+ const Transform& X_PF, const Body& bodyInfo, const Transform& X_BM,
+ Direction direction=Forward);
+
+ /** Abbreviated constructor you can use if the mobilizer frames are
+ coincident with the parent and child body frames. **/
+ Custom(MobilizedBody& parent, Implementation* implementation,
+ const Body& bodyInfo, Direction direction=Forward);
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Custom, CustomImpl, MobilizedBody);
+ /** @endcond **/
+protected:
+ const Implementation& getImplementation() const;
+ Implementation& updImplementation();
+
+};
+
+// We only want the template instantiation to occur once. This symbol is
+// defined in the Simbody compilation unit that defines the MobilizedBody class
+// but should not be defined any other time.
+#ifndef SimTK_SIMBODY_DEFINING_MOBILIZED_BODY
+ extern template class PIMPLHandle<MobilizedBody::Custom::Implementation,
+ MobilizedBody::Custom::ImplementationImpl>;
+#endif
+
+
+//==============================================================================
+// MOBILIZED BODY :: CUSTOM :: IMPLEMENTATION
+//==============================================================================
+/** This is the implementation class for Custom mobilizers.
+ at see MobilizedBody::Custom **/
+class SimTK_SIMBODY_EXPORT MobilizedBody::Custom::Implementation
+ : public PIMPLHandle<Implementation,ImplementationImpl>
+{
+public:
+ // No default constructor because you have to supply at least the SimbodyMatterSubsystem
+ // to which this MobilizedBody belongs.
+
+ /// Destructor is virtual so derived classes get a chance to clean up if necessary.
+ virtual ~Implementation();
+
+ /// This method should produce a deep copy identical to the concrete derived Implementation
+ /// object underlying this Implementation base class object.
+ /// Note that the result is new heap space; the caller must be sure to take ownership
+ /// of the returned pointer and call delete on it when done.
+ virtual Implementation* clone() const = 0;
+
+ /// This Implementation base class constructor sets the topological defaults for
+ /// the number of mobilities (generalized speeds) u, the number of generalized
+ /// coordinates q, and the number of those q's that are angles. There can be up
+ /// to 3 angular coordinates (which must be measured in radians). You also can
+ /// specify 4 as the number of angles, which is interpreted to mean the the
+ /// mobilizer uses a quaternion to represent orientation. Because quaternions
+ /// are not appropriate for some calculations, however, the user may globally
+ /// disable them by calling setUseEulerAngles() on the SimbodyMatterSubsystem.
+ /// Therefore, if you specify nAngles=4, the actual number of angular state variables
+ /// may be either 3 (a set of Euler angles) or 4 (quaternion components), and the
+ /// total number of state variables could be either nq-1 or nq. Before
+ /// interpreting the state variables, you must first call getUseEulerAngles() to
+ /// determine which representation is in use.
+ ///
+ /// In any case, if there are any angular coordinates they must be the <i>first</i>
+ /// coordinates in the array of q's associated with this mobilizer. Translational
+ /// or other q's will immediately follow the angular ones. This permits Simbody
+ /// to handle quaternion normalization and conversion automatically, and to find angles which
+ /// need to have their sines and cosines calculated.
+ ///
+ /// NOTE: if you don't say there are any angles, you can manage things yourself.
+ /// However, there is no way to get quaternions normalized and converted if you don't tell
+ /// Simbody about them.
+ Implementation(SimbodyMatterSubsystem&, int nu, int nq, int nAngles=0);
+
+ /// Return a Vector containing all the generalized coordinates q currently in use by this mobilizer.
+ /// Note that if this mobilizer uses quaternions, the number of q's will depend on whether
+ /// quaternions are currently enabled. Call getUseEulerAngles() to check this.
+ Vector getQ(const State& s) const;
+
+ /// Return a Vector containing all the generalized speeds u currently in use by this mobilizer.
+ Vector getU(const State& s) const;
+
+ /// Return a Vector containing all the generalized coordinate derivatives qdot currently in use by this mobilizer.
+ /// Note that if this mobilizer uses quaternions, the number of q's will depend on whether
+ /// quaternions are currently enabled. Call getUseEulerAngles() to check this.
+ Vector getQDot(const State& s) const;
+
+ /// Return a Vector containing all the generalized accelerations udot currently in use by this mobilizer.
+ Vector getUDot(const State& s) const;
+
+ /// Return a Vector containing all the generalized coordinate second derivatives qdotdot currently in use by this mobilizer.
+ /// Note that if this mobilizer uses quaternions, the number of q's will depend on whether
+ /// quaternions are currently enabled. Call getUseEulerAngles() to check this.
+ Vector getQDotDot(const State& s) const;
+
+ /// Get the cross-mobilizer transform X_FM, the body's "moving" mobilizer frame M measured and expressed in
+ /// the parent body's corresponding "fixed" frame F. The state must have been realized to at least
+ /// Position stage. Note: this refers to F and M <em>as defined</em>, not as they are if the
+ /// mobilizer has been reversed (that is, we're really returning X_F0M0 here).
+ Transform getMobilizerTransform(const State& s) const;
+
+ /// Get the cross-mobilizer velocity V_FM, the relative velocity of this body's "moving" mobilizer
+ /// frame M in the parent body's corresponding "fixed" frame F, measured and expressed in F.
+ /// Note that this isn't the usual spatial velocity since it isn't expressed in G.
+ /// The state must have been realized to at least Velocity stage. Note: this refers to
+ /// F and M <em>as defined</em>, not as they are if the
+ /// mobilizer has been reversed (that is, we're really returning V_F0M0 here).
+ SpatialVec getMobilizerVelocity(const State& s) const;
+
+ /// Get whether rotations are being represented as quaternions or Euler angles.
+ /// This method is only relevant if the constructor was invoked with nAngles==4.
+ /// If this returns false, the first four q's should be interpreted as the
+ /// components of a (possibly not normalized) quaternion. If it returns true, the
+ /// first three q's should be interpreted as Euler angles.
+ ///
+ /// Note that the total number of state variables is one less when using Euler
+ /// angles than when using quaternions.
+ bool getUseEulerAngles(const State& s) const;
+
+ /// Call this if you want to make sure that the next realizeTopology() call does
+ /// something. This is done automatically when you modify the MobilizedBody in ways
+ /// understood by Simbody. But if you are just
+ /// changing some of your own topology and want to make sure you get a chance to
+ /// recompute something in realizeTopology(), make this call at the time of
+ /// modification.
+ void invalidateTopologyCache() const;
+
+ /// @name MobilizedBody Virtuals
+ /// These must be defined for any Custom MobilizedBody.
+ /// Note that the numbers nu, nq, and nAngles are passed in to these routines for
+ /// redundancy -- you should make sure they have the values you are expecting!
+ //@{
+
+ /// Given values for this mobilizer's nq generalized coordinates q, compute X_FM(q), that is,
+ /// the cross-mobilizer spatial Transform giving the configuration of the "moving" frame M
+ /// fixed to the outboard (child) body B in the "fixed" frame F attached to the inboard (parent)
+ /// body P. The state is guaranteed to have been realized to at least Instance stage.
+ /// Caution: if your mobilizer has a quaternion, the four q's will not necessarily be
+ /// normalized here but you \e must normalize them before converting them to a Rotation.
+ /// Casting them to a Quaternion will do that automatically.
+ virtual Transform calcMobilizerTransformFromQ(const State& s, int nq, const Real* q) const = 0;
+
+
+ /// Calculate V_FM(u) = H*u where H=H(q) is the joint transition matrix mapping the mobilities to
+ /// the relative spatial velocity between the F frame on the parent to the M frame on the child.
+ /// The state is guaranteed to have been realized to at least Position stage.
+ ///
+ /// IMPORTANT -- H should depend only on X_FM(q), not directly on q, since different sets
+ /// of q's can generate the same Transform (e.g. quaternions and Euler angles). You can
+ /// call getMobilizerTransform(s) to get the already calculated Transform.
+ ///
+ /// EVEN MORE IMPORTANT -- H here must be the same as the H^T used in multiplyByHTranspose(),
+ /// and the HDot methods must use the time derivative of H.
+ ///
+ /// Note: the "H" we're using here is the transpose of what is used in Schwieter's IVM
+ /// paper and in all of Abhi Jain's papers. That's because Jain used H^T as the joint
+ /// kinematics Jacobian, with H being the force transmission matrix which no
+ /// mobilizer-writing user is going to be thinking about.
+ /// @see multiplyByHTranspose()
+ virtual SpatialVec multiplyByHMatrix(const State& s, int nu, const Real* u) const = 0;
+
+ /// Calculate f = ~H*F where F is a spatial force (torque+force) and f is its mapping onto
+ /// the mobilities.
+ ///
+ /// IMPORTANT -- H should depend only on X_FM(q), not directly on q, since different sets
+ /// of q's can generate the same Transform (e.g. quaternions and Euler angles). You can
+ /// call getMobilizerTransform(s) to get the already calculated Transform.
+ /// H here must match H and HDot in the other methods for this mobilizer.
+ /// @see multiplyByHMatrix()
+ virtual void multiplyByHTranspose(const State& s, const SpatialVec& F, int nu, Real* f) const = 0;
+
+ /// Calculate A0_FM = HDot*u where HDot=HDot(q,u) is the time derivative of H. This calculates
+ /// the "bias acceleration" due to coriolis effects, such that the full cross-mobilizer
+ /// acceleration is A_FM=A0_FM + H*udot.
+ /// The state is guaranteed to have been realized to at least Velocity stage.
+ ///
+ /// IMPORTANT -- HDot should depend only on X_FM(q) and V_FM(q,u), not directly on q or u,
+ /// since different choices of coordinates can generate the same X and V, but all such choices
+ /// must produce the same H and HDot. You can call getMobilizerTransform(s) to get the already
+ /// calculated Transform, and getMobilizerVelocity(s) to get the already calculated velocity.
+ virtual SpatialVec multiplyByHDotMatrix(const State& s, int nu, const Real* u) const = 0;
+
+ /// Calculate f = ~HDot*F where F is a spatial vector and f is its mapping onto
+ /// the mobilities.
+ /// The state is guaranteed to have been realized to at least Velocity stage.
+ ///
+ /// IMPORTANT -- HDot should depend only on X_FM(q) and V_FM(q,u), not directly on q or u,
+ /// since different choices of coordinates can generate the same X and V, but all such choices
+ /// must produce the same H and HDot. You can call getMobilizerTransform(s) to get the already
+ /// calculated Transform, and getMobilizerVelocity(s) to get the already calculated velocity.
+ virtual void multiplyByHDotTranspose(const State& s, const SpatialVec& F, int nu, Real* f) const = 0;
+
+ /// Calculate out_q = N(q)*in_u (e.g., qdot=N*u)
+ /// or out_u = ~N*in_q. Note that one of "in" and "out" is always "q-like" while
+ /// the other is "u-like", but which is which changes if the matrix is transposed.
+ /// Note that the transposed operation here is the same as multiplying by N on
+ /// the right, with the Vectors viewed as RowVectors instead.
+ /// The default implementation assumes that N is an identity matrix, and will
+ /// only work if nq=nu=nIn=nOut and nAngles < 4 (i.e., no quaternions). If this
+ /// is true for your mobilizer, you do not need to implement this method.
+ ///
+ /// The state is guaranteed to have been realized to at least Position stage.
+ virtual void multiplyByN(const State& s, bool transposeMatrix,
+ int nIn, const Real* in, int nOut, Real* out) const;
+
+ /// Calculate out_u = NInv(q)*in_q (e.g., u=NInv*qdot)
+ /// or out_q = ~NInv*in_u. Note that one of "in" and "out" is always "q-like" while
+ /// the other is "u-like", but which is which changes if the matrix is transposed.
+ /// Note that the transposed operation here is the same as multiplying by NInv on
+ /// the right, with the Vectors viewed as RowVectors instead.
+ /// The default implementation assumes that NInv is an identity matrix, and will
+ /// only work if nq=nu=nIn=nOut and nAngles < 4 (i.e., no quaternions). If this
+ /// is true for your mobilizer, you do not need to implement this method.
+ ///
+ /// The state is guaranteed to have been realized to at least Position stage.
+ virtual void multiplyByNInv(const State& s, bool transposeMatrix,
+ int nIn, const Real* in, int nOut, Real* out) const;
+
+ /// Calculate out_q = NDot(q)*in_u
+ /// or out_u = ~NDot*in_q. Note that one of "in" and "out" is always "q-like" while
+ /// the other is "u-like", but which is which changes if the matrix is transposed.
+ /// Note that the transposed operation here is the same as multiplying by NDot on
+ /// the right, with the Vectors viewed as RowVectors instead.
+ /// The default implementation assumes that NDot is zero, and will
+ /// only work if nq=nu=nIn=nOut and nAngles < 4 (i.e., no quaternions) If this
+ /// is true for your mobilizer, you do not need to implement this method.
+ ///
+ /// The state is guaranteed to have been realized to at least Position stage.
+ virtual void multiplyByNDot(const State& s, bool transposeMatrix,
+ int nIn, const Real* in, int nOut, Real* out) const;
+
+ // Methods for setting Mobilizer initial conditions. Note -- I've stripped this
+ // down to the two basic routines but the built-ins have 8 so that you can
+ // specify only rotations or translations. I'm not sure that's needed here and
+ // I suppose you could add more routines later if needed.
+ // Eventually it might be nice to provide default implementation here that would
+ // use a root finder to attempt to solve these initial condition problems. For
+ // most joints there is a much more direct way to do it, and sometimes there
+ // are behavioral choices to make, which is why it is nice to have mobilizer-specific
+ // overrides for these.
+
+ /// Find a set of q's for this mobilizer that best approximate the supplied Transform
+ /// which requests a particular relative orientation and translation between
+ /// the "fixed" and "moving" frames connected by this mobilizer.
+ /// The state is guaranteed to have been realized to at least Instance stage.
+ ///
+ /// The default implementation uses a nonlinear optimizer to search for the best
+ /// fit. Whenever possible, subclasses should override this to provide a faster
+ /// and more robust implementation.
+ virtual void setQToFitTransform(const State&, const Transform& X_FM, int nq, Real* q) const;
+
+ /// Find a set of u's (generalized speeds) for this mobilizer that best approximate
+ /// the supplied spatial velocity \p V_FM which requests the relative angular
+ /// and linear velocity between the "fixed" and "moving" frames connected by
+ /// this mobilizer. Routines which affect generalized speeds u depend on the generalized
+ /// coordinates q already having been set; they never change these coordinates.
+ /// The state is guaranteed to have been realized to at least Position stage.
+ /// @see setQToFitTransform()
+ ///
+ /// The default implementation uses a nonlinear optimizer to search for the best
+ /// fit. Whenever possible, subclasses should override this to provide a faster
+ /// and more robust implementation.
+ virtual void setUToFitVelocity(const State&, const SpatialVec& V_FM, int nu, Real* u) const;
+
+ /// Implement this optional method if you would like your MobilizedBody to generate any suggestions
+ /// for geometry that could be used as default visualization as an aid to understanding a system
+ /// containing this MobilizedBody. For example, if your mobilizer connects two points, you might
+ /// want to draw a line between those points. You can also generate text labels, and you can
+ /// provide methods for controlling the presence or appearance of your generated geometry.
+ /// If you don't implement this routine no extra geometry will be generated here.
+ virtual void calcDecorativeGeometryAndAppend
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+ {
+ }
+ //@}
+
+
+ /// @name Optional realize() Virtual Methods
+ /// Provide implementations of these methods if you want to allocate State variables (such
+ /// as modeling options or parameters) or want to pre-calculate some expensive quantities and
+ /// store them in the State cache for your future use. Note that the Position and Velocity
+ /// realize methods will be called <em>before</em> calling the matrix operator methods
+ /// for this MobilizedBody. That way if you want to precalculate the H or HDot matrix,
+ /// for example, you can do so in realizePosition() or realizeVelocity() and then use it
+ /// in multiplyByHMatrix(), etc.
+
+ //@{
+ /// The Matter Subsystem's realizeTopology() method will call this method along with the built-in
+ /// MobilizedBodies' realizeTopology() methods. This gives the MobilizedBody a chance to
+ /// - pre-calculate Topology stage "cache" values (mutable values which are stored
+ /// in the derived Implementation class directly), and
+ /// - allocate Model-stage state variables for later use, and
+ /// - allocate Model-stage cache entries in the State.
+ /// The indices to the Model-stage state & cache entries are stored locally as part of
+ /// the Topology-stage cache.
+ virtual void realizeTopology(State&) const { }
+
+ /// The Matter Subsystem's realizeModel() method will call this method along with the built-in
+ /// MobilizedBodies' realizeModel() methods. This gives the MobilizedBody a chance to
+ /// - pre-calculate Model stage cache values according to the settings of the Model variables,
+ /// - allocate any later-Stage variables that may be needed (typically these will be
+ /// Instance stage variables containing geometric information or parameters
+ /// like lengths or pitch for a Screw.
+ /// The indices to any of the State entries allocated here are stored in the State as part
+ /// of the Model-stage cache.
+ virtual void realizeModel(State&) const { }
+
+ /// The Matter Subsystem's realizeInstance() method will call this method along with the built-in
+ /// MobilizedBodies' realizeInstance() methods. This gives the MobilizedBody a chance to
+ /// - pre-calculate Instance stage cache values according to the settings of the Instance variables.
+ virtual void realizeInstance(const State&) const { }
+
+ /// The Matter Subsystem's realizeTime() method will call this method along with the built-in
+ /// MobilizedBodies' realizeTime() methods. This gives the MobilizedBody a chance to
+ /// - pre-calculate Time stage cache values according to the current value of time found
+ /// in the State.
+ virtual void realizeTime(const State&) const { }
+
+ /// The Matter Subsystem's realizePosition() method will call this method along with the built-in
+ /// MobilizedBodies' realizePosition() methods. This gives the MobilizedBody a chance to
+ /// - pre-calculate Position stage cache values according to the current values of positions found
+ /// in the State.
+ /// Note that this is called <em>before</em> methods which implement operators involving position-dependent
+ /// matrices N and H.
+ virtual void realizePosition(const State&) const { }
+
+ /// The Matter Subsystem's realizeVelocity() method will call this method along with the built-in
+ /// MobilizedBodies' realizeVelocity() methods. This gives the MobilizedBody a chance to
+ /// - pre-calculate Velocity stage cache values according to the current values of velocities found
+ /// in the State.
+ /// Note that this is called <em>before</em> methods which implement operators involving velocity-dependent
+ /// matrices NDot and HDot.
+ virtual void realizeVelocity(const State&) const { }
+
+ /// The Matter Subsystem's realizeDynamics() method will call this method along with the built-in
+ /// MobilizedBodies' realizeDynamics() methods. This gives the MobilizedBody a chance to
+ /// - pre-calculate Dynamics stage cache values according to the current values found
+ /// in the State.
+ /// Computations at Dynamics stage cannot affect the behavior of the MobilizedBody since that
+ /// is completely determined by the Position and Velocity stage operators.
+ virtual void realizeDynamics(const State&) const { }
+
+ /// The Matter Subsystem's realizeAcceleration() method will call this method along with the built-in
+ /// MobilizedBodies' realizeAcceleration() methods. This gives the MobilizedBody a chance to
+ /// - pre-calculate Acceleration stage cache values according to the current values of body
+ /// and mobility accelerations found in the State.
+ /// Computations at Acceleration stage cannot affect the behavior of the MobilizedBody since that
+ /// is completely determined by the Position and Velocity stage operators.
+ virtual void realizeAcceleration(const State&) const { }
+
+ /// The Matter Subsystem's realizeReport() method will call this method along with the built-in
+ /// MobilizedBodies' realizeReport() methods. This gives the MobilizedBody a chance to
+ /// - calculate Report stage cache values according to the current values found
+ /// in the State.
+ /// Computations at Report stage cannot affect the progress of a simulation in any way.
+ virtual void realizeReport(const State&) const { }
+ //@}
+
+ friend class MobilizedBody::CustomImpl;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_CUSTOM_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_Cylinder.h b/Simbody/include/simbody/internal/MobilizedBody_Cylinder.h
new file mode 100644
index 0000000..5284bee
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_Cylinder.h
@@ -0,0 +1,90 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_CYLINDER_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_CYLINDER_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy, 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::Cylinder class. **/
+
+#include "simbody/internal/MobilizedBody.h"
+
+namespace SimTK {
+
+/** Two mobilities -- rotation and translation along the common z axis
+of the inboard and outboard mobilizer frames.
+
+The two generalized coordinates q are the rotation angle in radians
+and the translation in length units, in that order. The two generalized speeds
+u are the time derivatives of the generalized coordinates so qdot=u for this
+mobilizer.
+**/
+class SimTK_SIMBODY_EXPORT MobilizedBody::Cylinder : public MobilizedBody {
+public:
+ /** Default constructor provides an empty handle that can be assigned to
+ reference any %MobilizedBody::Cylinder. **/
+ Cylinder() {}
+
+ /** Create a %Cylinder mobilizer between an existing parent (inboard) body P
+ and a new child (outboard) body B created by copying the given \a bodyInfo
+ into a privately-owned Body within the constructed %MobilizedBody object.
+ Specify the mobilizer frames F fixed to parent P and M fixed to child B.
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ Cylinder(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM, Direction=Forward);
+
+ /** Abbreviated constructor you can use if the mobilizer frames are
+ coincident with the parent and child body frames. **/
+ Cylinder(MobilizedBody& parent, const Body& bodyInfo, Direction=Forward);
+
+ Cylinder& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
+ }
+ Cylinder& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
+ }
+ Cylinder& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
+ }
+
+ Cylinder& setDefaultInboardFrame(const Transform& X_PF) {
+ (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
+ }
+
+ Cylinder& setDefaultOutboardFrame(const Transform& X_BM) {
+ (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
+ }
+
+ /** @cond **/ // Don't let doxygen see this
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Cylinder, CylinderImpl,
+ MobilizedBody);
+ /** @endcond **/
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_CYLINDER_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_Ellipsoid.h b/Simbody/include/simbody/internal/MobilizedBody_Ellipsoid.h
new file mode 100644
index 0000000..abf899e
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_Ellipsoid.h
@@ -0,0 +1,149 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_ELLIPSOID_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_ELLIPSOID_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) 2009-13 Stanford University and the Authors. *
+ * Authors: Ajay Seth, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::Ellipsoid class. **/
+
+#include "simbody/internal/MobilizedBody.h"
+
+namespace SimTK {
+
+/** Three mobilities -- coordinated rotation and translation along the
+surface of an ellipsoid fixed to the parent (inboard) body.
+
+The generalized coordinates q are the same as for a Ball (Orientation)
+mobilizer, that is, a quaternion or an x-y-z body-fixed Euler sequence depending
+on the "use Euler angles" modeling option. The three generalized speeds u for
+this mobilizer are also the same as for a Ball mobilizer, that is
+the three measure numbers of the angular velocity vector w_FM, the
+relative angular velocity of the outboard M frame in the inboard F frame,
+expressed in the F frame. That is unchanged by setting the "use Euler
+angles" modeling option. Note that qdot != u for this mobilizer. **/
+class SimTK_SIMBODY_EXPORT MobilizedBody::Ellipsoid : public MobilizedBody {
+public:
+ /** Default constructor provides an empty handle that can be assigned to
+ reference any %MobilizedBody::Ellipsoid. **/
+ Ellipsoid() {}
+
+ /** Create an %Ellipsoid mobilizer between an existing parent (inboard) body
+ P and a new child (outboard) body B created by copying the given \a bodyInfo
+ into a privately-owned Body within the constructed %MobilizedBody object.
+ Specify the mobilizer frames F fixed to parent P and M fixed to child B.
+ The ellipsoid is placed on the mobilizer's inboard frame F,
+ with semi-axis dimensions given in \a radii along F's x,y,z respectively.
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ Ellipsoid(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM,
+ const Vec3& radii, Direction=Forward);
+
+ /** Abbreviated constructor you can use if the mobilizer frames are
+ coincident with the parent and child body frames. **/
+ Ellipsoid(MobilizedBody& parent, const Body& bodyInfo,
+ const Vec3& radii, Direction=Forward);
+
+ /** This constructor assumes you'll set the ellipsoid dimensions later;
+ meanwhile it uses some default dimensions. **/
+ Ellipsoid(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM,
+ Direction=Forward);
+
+ /** This constructor assumes you'll set the ellipsoid dimensions later;
+ meanwhile it uses some default dimensions. The parent and child body frames
+ are used as the mobilizer frames. **/
+ Ellipsoid(MobilizedBody& parent, const Body& bodyInfo, Direction=Forward);
+
+ /** Modify the default semi-axis dimensions of the ellipsoid, given in
+ the F frame. These are usually set on construction. **/
+ Ellipsoid& setDefaultRadii(const Vec3& radii);
+ /** Get the default semi-axis dimensions of the ellipsoid as specified
+ during construction or via setDefaultRadii(). **/
+ const Vec3& getDefaultRadii() const;
+
+
+ /** Provide a default orientation for this mobilizer if you don't want to
+ start with the identity rotation (that is, alignment of the F and M
+ frames). This is the orientation the mobilizer will have in the default
+ state for the containing System. The supplied Rotation will be converted
+ to a quaternion and used as the four generalized coordinates q. **/
+ Ellipsoid& setDefaultRotation(const Rotation& R_FM) {
+ return setDefaultQ(R_FM.convertRotationToQuaternion());
+ }
+ /** Get the value for the default orientation of this mobilizer; unless
+ changed by setDefaultRotation() it will be the identity rotation. **/
+ Rotation getDefaultRotation() const {return Rotation(getDefaultQ());}
+
+ Ellipsoid& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
+ }
+ Ellipsoid& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
+ }
+ Ellipsoid& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
+ }
+
+ Ellipsoid& setDefaultInboardFrame(const Transform& X_PF) {
+ (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
+ }
+
+ Ellipsoid& setDefaultOutboardFrame(const Transform& X_BM) {
+ (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
+ }
+
+
+
+ // Generic default state Topology methods.
+ const Quaternion& getDefaultQ() const;
+ Quaternion& updDefaultQ();
+ Ellipsoid& setDefaultQ(const Quaternion& q) {updDefaultQ()=q; return *this;}
+
+ const Vec4& getQ(const State&) const;
+ const Vec4& getQDot(const State&) const;
+ const Vec4& getQDotDot(const State&) const;
+ const Vec3& getU(const State&) const;
+ const Vec3& getUDot(const State&) const;
+
+ void setQ(State&, const Vec4&) const;
+ void setU(State&, const Vec3&) const;
+
+ const Vec4& getMyPartQ(const State&, const Vector& qlike) const;
+ const Vec3& getMyPartU(const State&, const Vector& ulike) const;
+
+ Vec4& updMyPartQ(const State&, Vector& qlike) const;
+ Vec3& updMyPartU(const State&, Vector& ulike) const;
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Ellipsoid, EllipsoidImpl,
+ MobilizedBody);
+ /** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_ELLIPSOID_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_Free.h b/Simbody/include/simbody/internal/MobilizedBody_Free.h
new file mode 100644
index 0000000..1a73cf1
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_Free.h
@@ -0,0 +1,154 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_FREE_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_FREE_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy, 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::Free class. **/
+
+#include "simbody/internal/MobilizedBody.h"
+
+namespace SimTK {
+
+/** Unrestricted motion for a rigid body (six mobilities).
+
+Orientation is modeled the same as for the Ball mobilizer, that is, using
+quaternions to avoid singularities. A modeling option exists to
+have the joint modeled with an x-y-z body fixed Euler sequence like
+a Gimbal or Bushing mobilizer. Translational generalized coordinates are
+x,y,z translations along the F (inboard) axes. There are six generalized speeds
+u for this mobilizer. The first three are always the three measure numbers of
+the angular velocity vector w_FM, the relative angular velocity of the outboard
+M frame in the inboard F frame, expressed in the F frame. The second three
+are the measure numbers of v_FM, the relative linear velocity of the M frame's
+origin Mo in the F frame, expressed in the F frame. The meaning of the
+generalized speeds is unchanged by setting the "use Euler
+angles" modeling option, so the rotational generalized speeds here differ from
+those of a Bushing joint, and qdot != u for this mobilizer.
+
+ at see MobilizedBody::Bushing for an alternative.
+**/
+class SimTK_SIMBODY_EXPORT MobilizedBody::Free : public MobilizedBody {
+public:
+ /** Default constructor provides an empty handle that can be assigned to
+ reference any %MobilizedBody::Free. **/
+ Free() {}
+
+ /** Create a %Free mobilizer between an existing parent (inboard) body P
+ and a new child (outboard) body B created by copying the given \a bodyInfo
+ into a privately-owned Body within the constructed %MobilizedBody object.
+ Specify the mobilizer frames F fixed to parent P and M fixed to child B.
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ Free(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM, Direction=Forward);
+
+ /** Abbreviated constructor you can use if the mobilizer frames are
+ coincident with the parent and child body frames. **/
+ Free(MobilizedBody& parent, const Body& bodyInfo, Direction=Forward);
+
+ Free& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
+ }
+ Free& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
+ }
+ Free& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
+ }
+
+ Free& setDefaultInboardFrame(const Transform& X_PF) {
+ (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
+ }
+
+ Free& setDefaultOutboardFrame(const Transform& X_BM) {
+ (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
+ }
+
+ // Leaves rotation unchanged.
+ Free& setDefaultTranslation(const Vec3&);
+
+ // Leaves translation unchanged. The internal representation is a quaternion
+ // so we guarantee that the stored value is numerically identical to the
+ // supplied one.
+ Free& setDefaultQuaternion(const Quaternion&);
+
+ // Leaves translation unchanged. The Rotation matrix will be converted to
+ // a quaternion for storage.
+ Free& setDefaultRotation(const Rotation&);
+ // Sets both translation and rotation. The Rotation part of the Transform
+ // will be converted to a quaternion for storage.
+ Free& setDefaultTransform(const Transform&);
+
+ // These return references to the stored default values.
+ const Vec3& getDefaultTranslation() const;
+ const Quaternion& getDefaultQuaternion() const;
+
+ // These next two are derived from the stored values.
+ Rotation getDefaultRotation() const {
+ return Rotation(getDefaultQuaternion());
+ }
+ Transform getDefaultTransform() const {
+ return Transform(Rotation(getDefaultQuaternion()), getDefaultTranslation());
+ }
+
+ // Generic default state Topology methods.
+
+ // Returns (Vec4,Vec3) where the Vec4 is a normalized quaternion.
+ const Vec7& getDefaultQ() const;
+
+ // Interprets the supplied q as (Vec4,Vec3) where the Vec4 is a possibly
+ // unnormalized quaternion. The quaternion will be normalized before it is
+ // stored here, so you may not get back exactly the value supplied here if
+ // you call getDefaultQ().
+ Free& setDefaultQ(const Vec7& q);
+
+ // Note that there is no guarantee that the quaternion part of the returned Q is normalized.
+ const Vec7& getQ(const State&) const;
+ const Vec7& getQDot(const State&) const;
+ const Vec7& getQDotDot(const State&) const;
+
+ const Vec6& getU(const State&) const;
+ const Vec6& getUDot(const State&) const;
+
+ // The Q's in the state are set exactly as supplied without normalization.
+ void setQ(State&, const Vec7&) const;
+ void setU(State&, const Vec6&) const;
+
+ const Vec7& getMyPartQ(const State&, const Vector& qlike) const;
+ const Vec6& getMyPartU(const State&, const Vector& ulike) const;
+
+ Vec7& updMyPartQ(const State&, Vector& qlike) const;
+ Vec6& updMyPartU(const State&, Vector& ulike) const;
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Free, FreeImpl, MobilizedBody);
+ /** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_FREE_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_FreeLine.h b/Simbody/include/simbody/internal/MobilizedBody_FreeLine.h
new file mode 100644
index 0000000..c01b1df
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_FreeLine.h
@@ -0,0 +1,105 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_FREELINE_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_FREELINE_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::FreeLine class. **/
+
+#include "simbody/internal/MobilizedBody.h"
+
+namespace SimTK {
+
+/** Five mobilities, representing unrestricted motion for a body which is
+inertialess along its own z axis. The rotational generalized coordinates are the
+same as for the LineOrientation mobilizer. The translational coordinates are
+the same as in a Free mobilizer, or a Cartesian (Translation) mobilizer.
+
+LineOrientation and FreeLine are special "ball" and "free" mobilizers designed
+to allow arbitrary orientations for "linear" bodies, such as a CO2 molecule
+consisting only of point masses arranged along a straight line. Such bodies have
+no inertia about the line and cause singularities in the equations of motion if
+attached to Ball (Spherical) or Free mobilizers. Instead, use the
+LineOrientation and LineFree mobilizers, making sure that the inertialess
+direction is along the outboard body's z axis (that is, Mz). These mobilizers
+introduce only two rotational mobilities (generalized speeds u), being incapable
+of representing non-zero angular velocity of M in F about Mz. The generalized
+speeds are in fact the wx and wy components of w_FM_M, that is, the x and y
+components of the angular velocity of M in F <em>expressed in M</em>. However,
+at least three generalized coordinates (q's) are required to represent the
+orientation. By default we use four quaternions for unconditional stability.
+Alternatively, you can request a 1-2-3 body fixed Euler angle sequence (that is,
+about x, then new y, then new z) which will suffer a singularity when the y
+rotation is 90 degrees since that aligns the first rotation axis (x) with the
+last (z) which is the inertialess direction.
+
+ at see MobilizedBody::LineOrientation, MobilizedBody::Free **/
+class SimTK_SIMBODY_EXPORT MobilizedBody::FreeLine : public MobilizedBody {
+public:
+ /** Default constructor provides an empty handle that can be assigned to
+ reference any %MobilizedBody::Ball. **/
+ FreeLine() {}
+
+ /** Create a %FreeLine mobilizer between an existing parent (inboard) body P
+ and a new child (outboard) body B created by copying the given \a bodyInfo
+ into a privately-owned Body within the constructed %MobilizedBody object.
+ Specify the mobilizer frames F fixed to parent P and M fixed to child B.
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ FreeLine(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM, Direction=Forward);
+
+ /** Abbreviated constructor you can use if the mobilizer frames are
+ coincident with the parent and child body frames. **/
+ FreeLine(MobilizedBody& parent, const Body& bodyInfo, Direction=Forward);
+
+ FreeLine& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
+ }
+ FreeLine& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
+ }
+ FreeLine& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
+ }
+
+ FreeLine& setDefaultInboardFrame(const Transform& X_PF) {
+ (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
+ }
+
+ FreeLine& setDefaultOutboardFrame(const Transform& X_BM) {
+ (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
+ }
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(FreeLine, FreeLineImpl,
+ MobilizedBody);
+ /** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_FREELINE_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_FunctionBased.h b/Simbody/include/simbody/internal/MobilizedBody_FunctionBased.h
new file mode 100644
index 0000000..92e1977
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_FunctionBased.h
@@ -0,0 +1,197 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_FUNCTIONBASED_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_FUNCTIONBASED_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Ajay Seth *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::FunctionBased class. **/
+
+#include "simbody/internal/MobilizedBody.h"
+#include "simbody/internal/MobilizedBody_Custom.h"
+
+namespace SimTK {
+
+/** This is a subclass of MobilizedBody::Custom which uses a set of Function
+objects to define the behavior of the %MobilizedBody. When you create it, you
+specify the number of generalized coordinates, and six Function objects which
+calculate the spatial rotations and translations based on those coordinates.
+It assumes there is a one to one correspondence between generalized coordinates
+and generalized speeds, so qdot == u.
+
+Each of the Function objects must take some subset of the generalized
+coordinates as inputs, and produce a single number as its output. It also must
+support derivatives up to second order. Taken together, the six functions
+define a SpatialVec giving the body's mobilizer transform. **/
+class SimTK_SIMBODY_EXPORT MobilizedBody::FunctionBased
+: public MobilizedBody::Custom {
+public:
+ /** Default constructor provides an empty handle that can be assigned to
+ reference any %MobilizedBody::FunctionBased. **/
+ FunctionBased() {}
+
+ /** Create a FunctionBased MobilizedBody.
+
+ @param parent the MobilizedBody's parent body
+ @param bodyInfo describes this MobilizedBody's physical properties
+ @param nmobilities the number of generalized coordinates belonging to this MobilizedBody
+ @param functions the Functions describing how the body moves based on its generalized coordinates.
+ This must be of length 6. The elements correspond to, in order, x rotation, y rotation, z rotation,
+ x translation, y translation, and z translation. The MobilizedBody takes over ownership of the functions,
+ and automatically deletes them when the MobilizedBody is deleted.
+ @param coordIndices the indices of the generalized coordinates that are inputs to each function. For example, if coordIndices[2] = {0, 1},
+ that means that functions[2] takes two input arguments, and q[0] and q[1] respectively should be passed as those arguments.
+ @param direction whether you want the coordinates defined as though parent & child were swapped
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ FunctionBased(MobilizedBody& parent, const Body& bodyInfo,
+ int nmobilities, const Array_<const Function*>& functions,
+ const Array_<Array_<int> >& coordIndices,
+ Direction direction=Forward);
+
+ /** For compatibility with std::vector. **/
+ FunctionBased(MobilizedBody& parent, const Body& bodyInfo,
+ int nmobilities, const std::vector<const Function*>& functions,
+ const std::vector<std::vector<int> >& coordIndices,
+ Direction direction=Forward)
+ {
+ Array_< Array_<int> > coordCopy(coordIndices); // sorry, must copy
+ // Use the above constructor.
+ new(this) FunctionBased(parent,bodyInfo,nmobilities,
+ ArrayViewConst_<const Function*>(functions),
+ coordCopy, direction);
+ }
+
+ /** Create a FunctionBased MobilizedBody.
+
+ @param parent the MobilizedBody's parent body
+ @param X_PF the default inboard frame
+ @param bodyInfo describes this MobilizedBody's physical properties
+ @param X_BM the default outboard frame
+ @param nmobilities the number of generalized coordinates belonging to this MobilizedBody
+ @param functions the Functions describing how the body moves based on its generalized coordinates.
+ This must be of length 6. The elements correspond to, in order, x rotation, y rotation, z rotation,
+ x translation, y translation, and z translation. The MobilizedBody takes over ownership of the functions,
+ and automatically deletes them when the MobilizedBody is deleted.
+ @param coordIndices the indices of the generalized coordinates that are inputs to each function. For example, if coordIndices[2] = {0, 1},
+ that means that functions[2] takes two input arguments, and q[0] and q[1] respectively should be passed as those arguments.
+ @param direction whether you want the coordinates defined as though parent & child were swapped
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ FunctionBased(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM,
+ int nmobilities, const Array_<const Function*>& functions,
+ const Array_<Array_<int> >& coordIndices,
+ Direction direction=Forward);
+
+ /** For compatibility with std::vector. **/
+ FunctionBased(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM,
+ int nmobilities, const std::vector<const Function*>& functions,
+ const std::vector<std::vector<int> >& coordIndices,
+ Direction direction=Forward)
+ {
+ Array_< Array_<int> > coordCopy(coordIndices); // sorry, must copy
+ // Use the above constructor.
+ new(this) FunctionBased(parent,X_PF,bodyInfo,X_BM,
+ nmobilities, ArrayViewConst_<const Function*>(functions),
+ coordCopy, direction);
+ }
+
+ /** Create a FunctionBased MobilizedBody.
+
+ @param parent the MobilizedBody's parent body
+ @param bodyInfo describes this MobilizedBody's physical properties
+ @param nmobilities the number of generalized coordinates belonging to this MobilizedBody
+ @param functions the Functions describing how the body moves based on its generalized coordinates.
+ This must be of length 6. The elements correspond to, in order, x rotation, y rotation, z rotation,
+ x translation, y translation, and z translation. The MobilizedBody takes over ownership of the functions,
+ and automatically deletes them when the MobilizedBody is deleted.
+ @param coordIndices the indices of the generalized coordinates that are inputs to each function. For example, if coordIndices[2] = {0, 1},
+ that means that functions[2] takes two input arguments, and q[0] and q[1] respectively should be passed as those arguments.
+ @param axes the axes directions (as Vec3's) for each spatial coordinate, which each function describes, and is therefore length 6.
+ First 3 and last 3 axes must be linearly independent, otherwise there will be redundant speeds for the same motion.
+ @param direction whether you want the coordinates defined as though parent & child were swapped
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ FunctionBased(MobilizedBody& parent, const Body& bodyInfo,
+ int nmobilities, const Array_<const Function*>& functions,
+ const Array_<Array_<int> >& coordIndices, const Array_<Vec3>& axes,
+ Direction direction=Forward);
+
+ /** For compatibility with std::vector. **/
+ FunctionBased(MobilizedBody& parent, const Body& bodyInfo,
+ int nmobilities, const std::vector<const Function*>& functions,
+ const std::vector<std::vector<int> >& coordIndices, const std::vector<Vec3>& axes,
+ Direction direction=Forward)
+ {
+ Array_< Array_<int> > coordCopy(coordIndices); // sorry, must copy
+ // Use the above constructor.
+ new(this) FunctionBased(parent,bodyInfo,
+ nmobilities, ArrayViewConst_<const Function*>(functions),
+ coordCopy, ArrayViewConst_<Vec3>(axes),
+ direction);
+ }
+
+ /** Create a FunctionBased MobilizedBody.
+
+ @param parent the MobilizedBody's parent body
+ @param X_PF the default inboard frame
+ @param bodyInfo describes this MobilizedBody's physical properties
+ @param X_BM the default outboard frame
+ @param nmobilities the number of generalized coordinates belonging to this MobilizedBody
+ @param functions the Functions describing how the body moves based on its generalized coordinates.
+ This must be of length 6. The elements correspond to, in order, x rotation, y rotation, z rotation,
+ x translation, y translation, and z translation. The MobilizedBody takes over ownership of the functions,
+ and automatically deletes them when the MobilizedBody is deleted.
+ @param coordIndices the indices of the generalized coordinates that are inputs to each function. For example, if coordIndices[2] = {0, 1},
+ that means that functions[2] takes two input arguments, and q[0] and q[1] respectively should be passed as those arguments.
+ @param axes the axes directions (as Vec3's) for each spatial coordinate, which each function describes, and is therefore length 6.
+ First 3 and last 3 axes must be linearly independent, otherwise there will be redundant speeds for the same motion.
+ @param direction whether you want the coordinates defined as though parent & child were swapped
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ FunctionBased(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM,
+ int nmobilities, const Array_<const Function*>& functions,
+ const Array_<Array_<int> >& coordIndices, const Array_<Vec3>& axes,
+ Direction direction=Forward);
+
+ /** For compatibility with std::vector. **/
+ FunctionBased(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM,
+ int nmobilities, const std::vector<const Function*>& functions,
+ const std::vector<std::vector<int> >& coordIndices, const std::vector<Vec3>& axes,
+ Direction direction=Forward)
+ {
+ Array_< Array_<int> > coordCopy(coordIndices); // sorry, must copy
+ // Use the above constructor.
+ new(this) FunctionBased(parent,X_PF,bodyInfo,X_BM,
+ nmobilities, ArrayViewConst_<const Function*>(functions),
+ coordCopy, ArrayViewConst_<Vec3>(axes),
+ direction);
+ }
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_FUNCTIONBASED_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_Gimbal.h b/Simbody/include/simbody/internal/MobilizedBody_Gimbal.h
new file mode 100644
index 0000000..f1a1cc4
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_Gimbal.h
@@ -0,0 +1,233 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_GIMBAL_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_GIMBAL_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy, 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::Gimbal class. **/
+
+#include "simbody/internal/MobilizedBody.h"
+
+namespace SimTK {
+
+/** Three mobilities -- unrestricted orientation modeled as a 1-2-3 body-fixed
+Euler angle sequence, though with a singularity when the middle angle is +/- 90
+degrees. Use MobilizedBody::Ball for a similar joint that is free of
+singularities.
+
+This mobilizer does not provide any translation, so the parent's F frame and
+child's M frame origins will always be coincident. (MobilizedBody::Bushing is
+defined like Gimbal but adds translation.) When q0=q1=q2=0, the F and M
+frames are aligned. Then the generalized coordinates q should be interpreted
+as a series of rotation angles in radians: q0 is a rotation about the x axis,
+then q1 is a rotation about the now-rotated y axis, and then q2 is a rotation
+about the now twice-rotated z axis. The generalized speeds u for the %Gimbal
+mobilizer are the time derivatives of
+the generalized coordinates, that is, u=qdot. Note that this is different than
+the choice of generalized speeds used for MobilizedBody::Ball, where the
+angular velocity vector is used as the three generalized speeds. (Euler angle
+derivatives are \e not the same as angular velocity, except when q=0.)
+
+While this mobilizer provides arbitrary orientation, the Euler angle
+derivatives are singular when q1 (the middle rotation) is near +/- Pi/2. That
+means you should not attempt to do any dynamics in that configuration; if you
+can't be sure the motion will remain away from that region, you should use
+a MobilizedBody::Ball instead, which uses quaternions to ensure
+singularity-free rotation.
+
+Here is another way to describe the meaning of a %Gimbal mobilizer:
+A %Gimbal between frames F and M is exactly equivalent to a series
+of three pin joints using two massless intermediate bodies B1 and B2. The
+first pin joint rotates about the common Fx and B1x axes. The second rotates
+about the common B1y and B2y axes. The last one rotates about the common B2z
+and Mz axes. Other than performance (which is somewhat better if you use the
+%Gimbal), the pins-and-intermediate bodies system generates exactly the same
+generalized coordinates and generalized speeds as the %Gimbal.
+
+ at see MobilizedBody::Ball, MobilizedBody::Gimbal **/
+class SimTK_SIMBODY_EXPORT MobilizedBody::Gimbal : public MobilizedBody {
+public:
+ /** Default constructor provides an empty handle that can be assigned to
+ reference any %MobilizedBody::Gimbal. **/
+ Gimbal() {}
+
+ /** Create a %Gimbal mobilizer between an existing parent (inboard) body P
+ and a new child (outboard) body B created by copying the given \a bodyInfo
+ into a privately-owned Body within the constructed %MobilizedBody
+ object. Specify the mobilizer frames F fixed to parent P and M fixed to
+ child B. **/
+ Gimbal(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM, Direction=Forward);
+
+ /** Abbreviated constructor you can use if the mobilizer frames are
+ coincident with the parent and child body frames. **/
+ Gimbal(MobilizedBody& parent, const Body& bodyInfo, Direction=Forward);
+
+ /** Change the default inboard ("fixed") frame F on the parent body of this
+ mobilizer. The supplied frame is given as a Transform from the parent
+ body's frame P and replaces the F frame specified in the constructor. This
+ is a topological change, meaning that you must call realizeTopology() again
+ if you call this method. **/
+ Gimbal& setDefaultInboardFrame(const Transform& X_PF)
+ { (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this; }
+
+ /** Change the default outboard ("moving") frame M on this mobilized body
+ (that is, on this mobilizer's child body). The supplied frame is given as
+ a Transform from the child body's frame B and replaces the M frame specified
+ in the constructor. This is a topological change, meaning that you must
+ call realizeTopology() again if you use this method. **/
+ Gimbal& setDefaultOutboardFrame(const Transform& X_BM)
+ { (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this; }
+
+ /** Override the default orientation for this mobilizer. Normally the
+ default is q=0. Here you can provide a Rotation giving the default
+ orientation of the mobilizer's M frame in the parent body's F frame. That
+ orientation will be converted to a body fixed 1-2-3 Euler sequence and
+ used to set the default values for the generalized coordinates q that will
+ appear in a State that has just been created by a realizeTopology()
+ call. This is a topological change, meaning that you must call
+ realizeTopology() again if you use this method. **/
+ Gimbal& setDefaultRotation(const Rotation& R_FM)
+ { return setDefaultQ(R_FM.convertRotationToBodyFixedXYZ()); }
+
+ /** Return the default orientation for this mobilizer as a Rotation
+ matrix. This is the initial orientation of the mobilizer's M frame in
+ the parent body's F frame that you will see in a State that has just been
+ created via a realizeTopology() call. This is a topological change,
+ meaning that you must call realizeTopology() again if you use this
+ method. **/
+ Rotation getDefaultRotation() const {
+ const Vec3& q = getDefaultQ();
+ return Rotation(BodyRotationSequence,
+ q[0], XAxis, q[1], YAxis, q[2], ZAxis);
+ }
+
+ /** Add DecorativeGeometry to the privately-owned Body contained in this
+ %MobilizedBody, positioned relative to the body's frame. **/
+ Gimbal& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g)
+ { (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this; }
+
+ /** Add DecorativeGeometry to the privately-owned Body contained in this
+ %MobilizedBody, positioned relative to the mobilizer's M frame on that
+ body. **/
+ Gimbal& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g)
+ { (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this; }
+
+ /** Add DecorativeGeometry to the parent (inboard) body of this mobilizer,
+ positioned relative to the mobilizer's F frame on the parent body. **/
+ Gimbal& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g)
+ { (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this; }
+
+
+ /** This affects the radius of the ball that will be drawn as default
+ visualization geometry for a %Gimbal mobilizer. It has no effect whatsoever
+ on simulation or other computations. **/
+ Gimbal& setDefaultRadius(Real r);
+ /** Get the current value of the default visualization ball radius. **/
+ Real getDefaultRadius() const;
+
+ /** Obtain the default value for the generalized coordinates q that has
+ been set for this mobilizer. The value is returned as a 3-vector but note
+ that the returned quantity is \e not a vector; it is a body fixed 1-2-3
+ Euler angle sequence in radians. The default is q=0 unless overridden.
+ @see getDefaultRotation() **/
+ const Vec3& getDefaultQ() const; // X,Y,Z body-fixed Euler angles
+ /** Set the default value for the generalized coordinates q for this
+ mobilizer. The value is given as a 3-vector but note
+ that the supplied quantity is \e not a vector; it is a body fixed 1-2-3
+ Euler angle sequence in radians. The default is q=0 unless overridden.
+ @see getDefaultRotation() **/
+ Gimbal& setDefaultQ(const Vec3& q);
+
+ /** Obtain the current value for this mobilizer's generalized coordinates
+ q from the given State, which must be realized through Stage::Model (these
+ are state variables). **/
+ const Vec3& getQ(const State& state) const;
+ /** Obtain the current value for this mobilizer's generalized coordinate
+ time derivatives qdot from the given State, which must be realized through
+ Stage::Velocity. These are Euler angle time derivatives and, for the %Gimbal
+ mobilizer, have the same numerical values as the generalized speeds u. **/
+ const Vec3& getQDot(const State& state) const;
+ /** Obtain the current value for this mobilizer's generalized coordinate
+ second time derivatives qdotdot from the given State, which must be
+ realized through Stage::Acceleration. These are Euler angle second time
+ derivatives and, for the %Gimbal mobilizer, have the same numerical value
+ as the generalized accelerations udot. **/
+ const Vec3& getQDotDot(const State& state) const;
+
+ /** Obtain the current value for this mobilizer's generalized speeds u
+ from the given State, which must be realized through Stage::Model (these are
+ state variables). For the %Gimbal joint, these are the time derivatives of
+ q, that is, qdot=u. **/
+ const Vec3& getU(const State& state) const;
+ /** Obtain the current value for this mobilizer's generalized accelerations
+ udot (d/dt u) from the given State, which must be realized through
+ Stage::Acceleration. For the %Gimbal joint, these are the time derivatives
+ of qdot, that is, qdotdot=udot. **/
+ const Vec3& getUDot(const State& state) const;
+
+ /** Set new values for this mobilizer's generalized coordinates q in the
+ given \a state. This invalidates Stage::Position and above. The new
+ value is given as a 3-vector, but is interpreted as a body fixed 1-2-3
+ Euler angle sequence in radians. **/
+ void setQ(State& state, const Vec3& q) const;
+ /** Set new values for this mobilizer's generalized speeds u in the
+ given \a state. This invalidates Stage::Velocity and above. The new
+ value is given as a 3-vector, but is interpreted as the time derivatives
+ of a body fixed 1-2-3 Euler angle sequence in radians/time. **/
+ void setU(State& state, const Vec3& u) const;
+
+ /** @name Advanced/Obscure
+ Most users won't use these methods. **/
+ /**@{**/
+
+ /** Given a vector in the System's generalized coordinate basis, extract
+ the three q's belonging to this mobilizer. **/
+ const Vec3& getMyPartQ(const State& state, const Vector& qlike) const;
+ /** Given a vector in the System's generalized speed basis, extract
+ the three u's belonging to this mobilizer. **/
+ const Vec3& getMyPartU(const State& state, const Vector& ulike) const;
+
+ /** Given a writable vector in the System's generalized coordinate basis,
+ extract a writable reference to the three q's in that vector that belong to
+ this mobilizer. **/
+ Vec3& updMyPartQ(const State& state, Vector& qlike) const;
+ /** Given a writable vector in the System's generalized speed basis,
+ extract a writable reference to the three u's in that vector that belong to
+ this mobilizer. **/
+ Vec3& updMyPartU(const State& state, Vector& ulike) const;
+ /**@}**/
+
+ /** @cond **/ // Hide from Doxygen.
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Gimbal, GimbalImpl, MobilizedBody);
+ /** @endcond **/
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_GIMBAL_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_Ground.h b/Simbody/include/simbody/internal/MobilizedBody_Ground.h
new file mode 100644
index 0000000..7445d77
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_Ground.h
@@ -0,0 +1,66 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_GROUND_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_GROUND_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy, 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::Ground class. **/
+
+#include "simbody/internal/MobilizedBody.h"
+
+namespace SimTK {
+
+/** This is a special type of "mobilized" body generated automatically by
+Simbody as a placeholder for Ground in the 0th slot for a
+SimbodyMatterSubsystem's mobilized bodies; don't create this yourself. The body
+type will also be Ground. You can think of this as a Weld-like mobilizer that
+connects the Ground body to the world, located at the Ground origin. The
+reaction force in this mobilizer represents the total torque and force
+applied by the System to Ground. This mobilizer is not available for
+users -- if you want to weld something to Ground use MobilizedBody::Weld
+instead.
+ at see MobilizedBody::Weld, SimbodyMatterSubsystem::getGround(),
+ SimbodyMatterSubsystem::updGround() **/
+class SimTK_SIMBODY_EXPORT MobilizedBody::Ground : public MobilizedBody {
+public:
+ Ground();
+
+ /** Add some artwork to Ground where the Visualizer can find it. **/
+ Ground& addBodyDecoration(const Transform& X_GD,
+ const DecorativeGeometry& artwork)
+ {
+ (void)MobilizedBody::addBodyDecoration(X_GD,artwork); return *this;
+ }
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Ground, GroundImpl, MobilizedBody);
+ /** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_GROUND_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_LineOrientation.h b/Simbody/include/simbody/internal/MobilizedBody_LineOrientation.h
new file mode 100644
index 0000000..26dedac
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_LineOrientation.h
@@ -0,0 +1,134 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_LINEORIENTATION_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_LINEORIENTATION_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::LineOrientation class. **/
+
+#include "simbody/internal/MobilizedBody.h"
+
+namespace SimTK {
+
+/** Two mobilities, representing unrestricted orientation for a body which is
+inertialess along its own z axis. The generalized coordinates are the same
+as for the general Ball (Spherical) mobilizer, but there are only
+two generalized speeds. These are the x,y components of the angular velocity
+of frame M in F, but expressed in \e M (the body or outboard frame).
+
+LineOrientation and FreeLine are special "ball" and "free" mobilizers designed
+to allow arbitrary orientations for "linear" bodies, such as a CO2 molecule
+consisting only of point masses arranged along a straight line. Such bodies have
+no inertia about the line and cause singularities in the equations of motion if
+attached to Ball (Spherical) or Free mobilizers. Instead, use the
+LineOrientation and LineFree mobilizers, making sure that the inertialess
+direction is along the outboard body's z axis (that is, Mz). These mobilizers
+introduce only two rotational mobilities (generalized speeds u), being incapable
+of representing non-zero angular velocity of M in F about Mz. The generalized
+speeds are in fact the wx and wy components of w_FM_M, that is, the x and y
+components of the angular velocity of M in F <em>expressed in M</em>. However,
+at least three generalized coordinates (q's) are required to represent the
+orientation. By default we use four quaternions for unconditional stability.
+Alternatively, you can request a 1-2-3 body fixed Euler angle sequence (that is,
+about x, then new y, then new z) which will suffer a singularity when the y
+rotation is 90 degrees since that aligns the first rotation axis (x) with the
+last (z) which is the inertialess direction.
+
+ at see MobilizedBody::FreeLine, MobilizedBody::Ball **/
+class SimTK_SIMBODY_EXPORT MobilizedBody::LineOrientation : public MobilizedBody {
+public:
+ /** Default constructor provides an empty handle that can be assigned to
+ reference any %MobilizedBody::LineOrientation. **/
+ LineOrientation() {}
+
+ /** Create a %LineOrientation mobilizer between an existing parent (inboard)
+ body P and a new child (outboard) body B created by copying the given
+ \a bodyInfo into a privately-owned Body within the constructed
+ %MobilizedBody object. Specify the mobilizer frames F fixed to parent P and
+ M fixed to child B.
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ LineOrientation(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM,
+ Direction=Forward);
+
+ /** Abbreviated constructor you can use if the mobilizer frames are
+ coincident with the parent and child body frames. **/
+ LineOrientation(MobilizedBody& parent, const Body& bodyInfo,
+ Direction=Forward);
+
+ LineOrientation& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
+ }
+ LineOrientation& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
+ }
+ LineOrientation& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
+ }
+
+ LineOrientation& setDefaultInboardFrame(const Transform& X_PF) {
+ (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
+ }
+
+ LineOrientation& setDefaultOutboardFrame(const Transform& X_BM) {
+ (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
+ }
+
+ // This is just a nicer name for the generalized coordinate.
+ LineOrientation& setDefaultRotation(const Rotation& R_FM) {
+ return setDefaultQ(R_FM.convertRotationToQuaternion());
+ }
+ Rotation getDefaultRotation() const {return Rotation(getDefaultQ());}
+
+ // Generic default state Topology methods.
+ const Quaternion& getDefaultQ() const;
+ LineOrientation& setDefaultQ(const Quaternion& q);
+
+ const Vec4& getQ(const State&) const;
+ const Vec4& getQDot(const State&) const;
+ const Vec4& getQDotDot(const State&) const;
+ const Vec2& getU(const State&) const;
+ const Vec2& getUDot(const State&) const;
+
+ void setQ(State&, const Vec4&) const;
+ void setU(State&, const Vec2&) const;
+
+ const Vec4& getMyPartQ(const State&, const Vector& qlike) const;
+ const Vec2& getMyPartU(const State&, const Vector& ulike) const;
+
+ Vec4& updMyPartQ(const State&, Vector& qlike) const;
+ Vec2& updMyPartU(const State&, Vector& ulike) const;
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(LineOrientation,
+ LineOrientationImpl, MobilizedBody);
+ /** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_LINEORIENTATION_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_Pin.h b/Simbody/include/simbody/internal/MobilizedBody_Pin.h
new file mode 100644
index 0000000..b1e152c
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_Pin.h
@@ -0,0 +1,144 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_PIN_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_PIN_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy, 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::Pin class. **/
+
+#include "simbody/internal/MobilizedBody.h"
+
+namespace SimTK {
+
+/** Provides one rotational mobility about the common z axis of the F and M
+frames of the mobilizer.
+
+If you want rotation about a different direction, rotate the F and M frames
+when you define the mobilized body, so that the z axes are in the desired
+direction.
+
+The single generalized coordinate q is the rotation angle in radians, and the
+generalized speed u is the rotation rate in radians/time unit, with qdot=u.
+
+Synonyms: Torsion, Revolute.
+**/
+class SimTK_SIMBODY_EXPORT MobilizedBody::Pin : public MobilizedBody {
+public:
+ /** Default constructor provides an empty handle that can be assigned to
+ reference any %MobilizedBody::Pin. **/
+ Pin() {}
+
+ /** Create a %Pin mobilizer between an existing parent (inboard) body P
+ and a new child (outboard) body B created by copying the given \a bodyInfo
+ into a privately-owned Body within the constructed %MobilizedBody object.
+ Specify the mobilizer frames F fixed to parent P and M fixed to child B.
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ Pin(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM, Direction=Forward);
+
+ /** Abbreviated constructor you can use if the mobilizer frames are
+ coincident with the parent and child body frames. **/
+ Pin(MobilizedBody& parent, const Body& bodyInfo, Direction=Forward);
+
+ // SPECIALIZED INTERFACE FOR PIN MOBILIZER
+
+ /** Set the value that the pin angle should have in the default state. If
+ unspecified the initial angle will be zero. **/
+ Pin& setDefaultAngle(Real angleInRad) {return setDefaultQ(angleInRad);}
+ /** Get the value that the pin angle will have in the default state. **/
+ Real getDefaultAngle() const {return getDefaultQ();}
+
+ // Friendly, mobilizer-specific access to generalized coords and speeds.
+
+ /** Set the pin joint angle (generalized coordinate q) in the given state.
+ The angle is in radians. **/
+ void setAngle(State& s, Real angleInRad) {setQ(s, angleInRad);}
+ /** Get the value that this pin joint's angular coordinate has in the given
+ state. The result is in radians. **/
+ Real getAngle(const State& s) const {return getQ(s);}
+
+ /** Set the rotation rate (generalized speed u) for this pin joint in the
+ given state. The rate is in radians/time unit. **/
+ void setRate(State& s, Real rateInRadPerTime) {setU(s, rateInRadPerTime);}
+ /** Get the current rotation rate (generalized speed u) that this pin
+ mobilizer has in the given state. The rate is in radians/time unit. **/
+ Real getRate(const State& s) const {return getU(s);}
+
+ // Mobility forces are "u-like", that is, one per dof.
+ /** Get the generalized force corresponding to this pin mobilizer in the
+ given array of mobility forces. **/
+ Real getAppliedPinTorque(const State& s, const Vector& mobilityForces) const {
+ return getMyPartU(s,mobilityForces);
+ }
+ /** Add in a torque to the generalized force element corresponding to this
+ pin mobilizer in the given array of mobility forces. **/
+ void applyPinTorque(const State& s, Real torque, Vector& mobilityForces) const {
+ updMyPartU(s,mobilityForces) += torque;
+ }
+
+ // STANDARDIZED MOBILIZED BODY INTERFACE
+
+
+ // access to generalized coordinates q and generalized speeds u
+ Pin& setDefaultQ(Real);
+ Real getDefaultQ() const;
+
+ Real getQ(const State&) const;
+ Real getQDot(const State&) const;
+ Real getQDotDot(const State&) const;
+ Real getU(const State&) const;
+ Real getUDot(const State&) const;
+
+ void setQ(State&, Real) const;
+ void setU(State&, Real) const;
+
+ Real getMyPartQ(const State&, const Vector& qlike) const;
+ Real getMyPartU(const State&, const Vector& ulike) const;
+
+ Real& updMyPartQ(const State&, Vector& qlike) const;
+ Real& updMyPartU(const State&, Vector& ulike) const;
+
+ // specialize return type for convenience
+ Pin& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g)
+ { (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this; }
+ Pin& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g)
+ { (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this; }
+ Pin& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g)
+ { (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this; }
+ Pin& setDefaultInboardFrame(const Transform& X_PF)
+ { (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this; }
+ Pin& setDefaultOutboardFrame(const Transform& X_BM)
+ { (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this; }
+
+ /** @cond **/ // Don't let doxygen see this
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Pin, PinImpl, MobilizedBody);
+ /** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_PIN_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_Planar.h b/Simbody/include/simbody/internal/MobilizedBody_Planar.h
new file mode 100644
index 0000000..c4810de
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_Planar.h
@@ -0,0 +1,122 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_PLANAR_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_PLANAR_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy, 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::Planar class. **/
+
+#include "simbody/internal/MobilizedBody.h"
+
+namespace SimTK {
+
+/** Three mobilities -- z rotation and x,y translation. The generalized
+coordinates are rotation about the shared z axis of the F and M
+frame, translation along the F frame's x axis, and translation along
+its y axis, in that order. **/
+class SimTK_SIMBODY_EXPORT MobilizedBody::Planar : public MobilizedBody {
+public:
+ /** Default constructor provides an empty handle that can be assigned to
+ reference any %MobilizedBody::Planar. **/
+ Planar() {}
+
+ /** Create a %Planar mobilizer between an existing parent (inboard) body P
+ and a new child (outboard) body B created by copying the given \a bodyInfo
+ into a privately-owned Body within the constructed %MobilizedBody object.
+ Specify the mobilizer frames F fixed to parent P and M fixed to child B.
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ Planar(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM, Direction=Forward);
+
+ /** Abbreviated constructor you can use if the mobilizer frames are
+ coincident with the parent and child body frames. **/
+ Planar(MobilizedBody& parent, const Body& bodyInfo, Direction=Forward);
+
+ Planar& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
+ }
+ Planar& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
+ }
+ Planar& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
+ }
+
+ Planar& setDefaultInboardFrame(const Transform& X_PF) {
+ (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
+ }
+
+ Planar& setDefaultOutboardFrame(const Transform& X_BM) {
+ (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
+ }
+
+ // Friendly, mobilizer-specific access to coordinates and speeds.
+ Planar& setDefaultAngle(Real a) {
+ Vec3 q = getDefaultQ(); q[0] = a; setDefaultQ(q);
+ return *this;
+ }
+ Planar& setDefaultTranslation(const Vec2& r) {
+ Vec3 q = getDefaultQ(); q.updSubVec<2>(1) = r; setDefaultQ(q);
+ return *this;
+ }
+
+ Real getDefaultAngle() const {return getDefaultQ()[0];}
+ const Vec2& getDefaultTranslation() const {return getDefaultQ().getSubVec<2>(1);}
+
+ void setAngle (State& s, Real a) {setOneQ(s,0,a);}
+ void setTranslation(State& s, const Vec2& r) {setOneQ(s,1,r[0]); setOneQ(s,2,r[1]);}
+
+ Real getAngle(const State& s) const {return getQ(s)[0];}
+ const Vec2& getTranslation(const State& s) const {return getQ(s).getSubVec<2>(1);}
+
+ // Generic default state Topology methods.
+ const Vec3& getDefaultQ() const;
+ Planar& setDefaultQ(const Vec3& q);
+
+ const Vec3& getQ(const State&) const;
+ const Vec3& getQDot(const State&) const;
+ const Vec3& getQDotDot(const State&) const;
+ const Vec3& getU(const State&) const;
+ const Vec3& getUDot(const State&) const;
+
+ void setQ(State&, const Vec3&) const;
+ void setU(State&, const Vec3&) const;
+
+ const Vec3& getMyPartQ(const State&, const Vector& qlike) const;
+ const Vec3& getMyPartU(const State&, const Vector& ulike) const;
+
+ Vec3& updMyPartQ(const State&, Vector& qlike) const;
+ Vec3& updMyPartU(const State&, Vector& ulike) const;
+
+ /** @cond **/ // Don't let doxygen see this
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Planar, PlanarImpl, MobilizedBody);
+ /** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_PLANAR_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_Screw.h b/Simbody/include/simbody/internal/MobilizedBody_Screw.h
new file mode 100644
index 0000000..0a74ac8
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_Screw.h
@@ -0,0 +1,109 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_SCREW_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_SCREW_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy, 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::Screw class. **/
+
+#include "simbody/internal/MobilizedBody.h"
+
+namespace SimTK {
+
+/** One mobility -- coordinated rotation and translation along the
+common z axis of the inboard and outboard mobilizer frames. A
+"pitch" is specified relating the two. The generalized coordinate
+q is the rotation angle in radians, the translation is always
+pitch*q. **/
+class SimTK_SIMBODY_EXPORT MobilizedBody::Screw : public MobilizedBody {
+public:
+ /** Default constructor provides an empty handle that can be assigned to
+ reference any %MobilizedBody::Screw. **/
+ Screw() {}
+
+ /** Create a %Screw mobilizer between an existing parent (inboard) body P
+ and a new child (outboard) body B created by copying the given \a bodyInfo
+ into a privately-owned Body within the constructed %MobilizedBody object.
+ Specify the mobilizer frames F fixed to parent P and M fixed to child B.
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ Screw(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM,
+ Real pitch, Direction=Forward);
+
+ /** Abbreviated constructor you can use if the mobilizer frames are
+ coincident with the parent and child body frames. **/
+ Screw(MobilizedBody& parent, const Body& bodyInfo, Real pitch,
+ Direction=Forward);
+
+
+ Screw& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
+ }
+ Screw& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
+ }
+ Screw& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
+ }
+
+ Screw& setDefaultInboardFrame(const Transform& X_PF) {
+ (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
+ }
+
+ Screw& setDefaultOutboardFrame(const Transform& X_BM) {
+ (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
+ }
+
+ Screw& setDefaultPitch(Real pitch);
+ Real getDefaultPitch() const;
+
+ Screw& setDefaultQ(Real);
+ Real getDefaultQ() const;
+
+ Real getQ(const State&) const;
+ Real getQDot(const State&) const;
+ Real getQDotDot(const State&) const;
+ Real getU(const State&) const;
+ Real getUDot(const State&) const;
+
+ void setQ(State&, Real) const;
+ void setU(State&, Real) const;
+
+ Real getMyPartQ(const State&, const Vector& qlike) const;
+ Real getMyPartU(const State&, const Vector& ulike) const;
+
+ Real& updMyPartQ(const State&, Vector& qlike) const;
+ Real& updMyPartU(const State&, Vector& ulike) const;
+
+ /** @cond **/ // Don't let doxygen see this
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Screw, ScrewImpl, MobilizedBody);
+ /** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_SCREW_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_Slider.h b/Simbody/include/simbody/internal/MobilizedBody_Slider.h
new file mode 100644
index 0000000..5d0f1b1
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_Slider.h
@@ -0,0 +1,132 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_SLIDER_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_SLIDER_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy, 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::Slider class. **/
+
+#include "simbody/internal/MobilizedBody.h"
+
+namespace SimTK {
+
+/** One mobility -- translation along the common x axis of the
+F (inboard) and M (outboard) mobilizer frames.
+
+If you want translation along a different direction, rotate the F and M frames
+when you define the mobilized body, so that the x axes are in the desired
+direction.
+
+The single generalized coordinate q is the translation in length units of M's
+origin Mo with respect to F's origin Fo, and the generalized speed u is the
+translation rate in length units/time unit, with qdot=u.
+
+Synonym: Prismatic **/
+class SimTK_SIMBODY_EXPORT MobilizedBody::Slider : public MobilizedBody {
+public:
+ /** Default constructor provides an empty handle that can be assigned to
+ reference any %MobilizedBody::Slider. **/
+ Slider() {}
+
+ /** Create a %Slider mobilizer between an existing parent (inboard) body P
+ and a new child (outboard) body B created by copying the given \a bodyInfo
+ into a privately-owned Body within the constructed %MobilizedBody object.
+ Specify the mobilizer frames F fixed to parent P and M fixed to child B.
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ Slider(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM, Direction=Forward);
+
+ /** Abbreviated constructor you can use if the mobilizer frames are
+ coincident with the parent and child body frames. **/
+ Slider(MobilizedBody& parent, const Body& bodyInfo, Direction=Forward);
+
+
+ // SPECIALIZED INTERFACE FOR SLIDER MOBILIZER
+
+ // "Length" is just a nicer name for a sliding joint's lone generalized coordinate q.
+ Slider& setDefaultLength(Real length) {return setDefaultQ(length);}
+ Real getDefaultLength() const {return getDefaultQ();}
+
+ // Friendly, mobilizer-specific access to generalized coordinates and speeds.
+
+ void setLength(State& s, Real length) {setQ(s, length);}
+ Real getLength(const State& s) const {return getQ(s);}
+
+ void setRate(State& s, Real rateInLengthPerTime) {setU(s, rateInLengthPerTime);}
+ Real getRate(const State& s) const {return getU(s);}
+
+ // Mobility forces are "u-like", that is, one per dof.
+ Real getAppliedForce(const State& s, const Vector& mobilityForces) const {
+ return getMyPartU(s,mobilityForces);
+ }
+ void applyForce(const State& s, Real force, Vector& mobilityForces) const {
+ updMyPartU(s,mobilityForces) += force;
+ }
+
+ // STANDARDIZED MOBILIZED BODY INTERFACE
+
+
+ // access to generalized coordinates q and generalized speeds u
+ Slider& setDefaultQ(Real);
+ Real getDefaultQ() const;
+
+ Real getQ(const State&) const;
+ Real getQDot(const State&) const;
+ Real getQDotDot(const State&) const;
+ Real getU(const State&) const;
+ Real getUDot(const State&) const;
+
+ void setQ(State&, Real) const;
+ void setU(State&, Real) const;
+
+ Real getMyPartQ(const State&, const Vector& qlike) const;
+ Real getMyPartU(const State&, const Vector& ulike) const;
+
+ Real& updMyPartQ(const State&, Vector& qlike) const;
+ Real& updMyPartU(const State&, Vector& ulike) const;
+
+ // specialize return type for convenience
+ Slider& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g)
+ { (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this; }
+ Slider& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g)
+ { (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this; }
+ Slider& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g)
+ { (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this; }
+ Slider& setDefaultInboardFrame(const Transform& X_PF)
+ { (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this; }
+ Slider& setDefaultOutboardFrame(const Transform& X_BM)
+ { (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this; }
+
+ /** @cond **/ // Don't let doxygen see this
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Slider, SliderImpl, MobilizedBody);
+ /** @endcond **/
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_SLIDER_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_SphericalCoords.h b/Simbody/include/simbody/internal/MobilizedBody_SphericalCoords.h
new file mode 100644
index 0000000..e6cac17
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_SphericalCoords.h
@@ -0,0 +1,185 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_SPHERICALCOORDS_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_SPHERICALCOORDS_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Christopher Bruns *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::SphericalCoords class. **/
+
+#include "simbody/internal/MobilizedBody.h"
+
+namespace SimTK {
+
+/** Three mobilities -- body fixed 3-2 (z-y) rotation followed by translation
+along body z or body x. Interpreted as spherical coordinates the first rotation
+is the azimuth angle, the second is the zenith, and the translation is
+the radius. We permit a simple mapping from generalized coordinates to
+(azimuth, zenith, radius):
+<pre>
+ azimuth = s0*q0 + az0 (about Fz==Mz)
+ zenith = s1*q1 + ze0 (about My)
+ radius = s2*q2 (along Mz or Mx; Mz is default)
+</pre>
+where s0,s1,s2 are signs (1 or -1) and az0 and ze0 are offset angles. The
+F and M frames are coincident when azimuth==zenith==radius==0. But note
+that with non-zero offsets the F and M frames will not be aligned in
+the reference configuration where q0==q1==q2==0. The F and M origins
+will always be coincident when q2==0, however.
+
+With this you can define a "geographical" coordinate system where Mx is the
+Greenwich line, a is latitude and z longitude (with north positive):
+<pre>
+ v = Mx
+ s0 = 1, az0 = 0
+ s1 = -1, ze0 = 0
+ s2 = 1
+</pre>
+If you want the translation direction to be in Mz (the default) but would like
+q1=0 to mean the equatorial position rather than the (possibly singular) north
+pole which should be -90, define
+<pre>
+ v = Mz
+ s0 = 1, az0 = 0
+ s1 = 1, ze0 = Pi/2
+ s2 = 1
+</pre>
+One common convention for atomic (torsion,bend,stretch) uses the default
+spherical coordinate system but the final stretch is along the -z direction.
+For that, take all defaults but set s2=-1.
+
+This mobilizer can be used to give unrestricted 3-d motion to inertialess
+particles (as with a Cartesian mobilizer but parameterized torsion,bend,stretch
+instead of x,y,z) but in this case you must watch for two possible
+singularities: (1) radius==0, and (2) zenith==n*Pi (or equivalently
+q1==n*Pi-s1*ze0). If your operating range steers clear of those singularities,
+you're fine. **/
+class SimTK_SIMBODY_EXPORT MobilizedBody::SphericalCoords : public MobilizedBody {
+public:
+ /** Default constructor provides an empty handle that can be assigned to
+ reference any %MobilizedBody::SphericalCoords. **/
+ SphericalCoords() {}
+
+ /** Create a %SphericalCoords mobilizer between an existing parent (inboard)
+ body P and a new child (outboard) body B created by copying the given
+ \a bodyInfo into a privately-owned Body within the constructed
+ %MobilizedBody object. Specify the mobilizer frames F fixed to parent P
+ and M fixed to child B. This constructor gives you a pure spherical
+ coordinate system in which q0=azimuth about Fz(==Mz), q1=zenith about My,
+ and q2=radius along Mz. See the longer signature for more control.
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ SphericalCoords(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM,
+ Direction=Forward);
+
+ /** Abbreviated constructor you can use if the mobilizer frames are
+ coincident with the parent and child body frames. **/
+ SphericalCoords(MobilizedBody& parent, const Body& bodyInfo,
+ Direction=Forward);
+
+
+ /** Use this constructor to specify the general case described above. **/
+ SphericalCoords(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM,
+ Real azimuthOffset, bool azimuthNegated,
+ Real zenithOffset, bool zenithNegated,
+ CoordinateAxis radialAxis, bool radialNegated,
+ Direction=Forward);
+
+ SphericalCoords& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
+ }
+ SphericalCoords& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
+ }
+ SphericalCoords& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
+ }
+
+ SphericalCoords& setDefaultInboardFrame(const Transform& X_PF) {
+ (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
+ }
+
+ SphericalCoords& setDefaultOutboardFrame(const Transform& X_BM) {
+ (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
+ }
+
+ // Friendly, mobilizer-specific access to coordinates and speeds.
+ SphericalCoords& setDefaultAngles(const Vec2& a) {
+ Vec3 q = getDefaultQ(); q.updSubVec<2>(0) = a; setDefaultQ(q);
+ return *this;
+ }
+ SphericalCoords& setDefaultRadius(Real r) {
+ Vec3 q = getDefaultQ(); q[2] = r; setDefaultQ(q);
+ return *this;
+ }
+ SphericalCoords& setRadialAxis(CoordinateAxis);
+ SphericalCoords& setNegateAzimuth(bool);
+ SphericalCoords& setNegateZenith(bool);
+ SphericalCoords& setNegateRadial(bool);
+
+ const Vec2& getDefaultAngles() const {return getDefaultQ().getSubVec<2>(0);}
+ Real getDefaultTranslation() const {return getDefaultQ()[2];}
+
+ CoordinateAxis getRadialAxis() const;
+ bool isAzimuthNegated() const;
+ bool isZenithNegated() const;
+ bool isRadialNegated() const;
+
+ void setAngles(State& s, const Vec2& a) {setOneQ(s,0,a[0]); setOneQ(s,1,a[1]);}
+ void setRadius(State& s, Real r) {setOneQ(s,2,r);}
+
+ const Vec2& getAngles(const State& s) const {return getQ(s).getSubVec<2>(0);}
+ Real getRadius(const State& s) const {return getQ(s)[2];}
+
+ // Generic default state Topology methods.
+ const Vec3& getDefaultQ() const;
+ SphericalCoords& setDefaultQ(const Vec3& q);
+
+ const Vec3& getQ(const State&) const;
+ const Vec3& getQDot(const State&) const;
+ const Vec3& getQDotDot(const State&) const;
+ const Vec3& getU(const State&) const;
+ const Vec3& getUDot(const State&) const;
+
+ void setQ(State&, const Vec3&) const;
+ void setU(State&, const Vec3&) const;
+
+ const Vec3& getMyPartQ(const State&, const Vector& qlike) const;
+ const Vec3& getMyPartU(const State&, const Vector& ulike) const;
+
+ Vec3& updMyPartQ(const State&, Vector& qlike) const;
+ Vec3& updMyPartU(const State&, Vector& ulike) const;
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(SphericalCoords,
+ SphericalCoordsImpl, MobilizedBody);
+ /** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_SPHERICALCOORDS_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_Translation.h b/Simbody/include/simbody/internal/MobilizedBody_Translation.h
new file mode 100644
index 0000000..8ab6916
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_Translation.h
@@ -0,0 +1,148 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_TRANSLATION_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_TRANSLATION_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy, 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::Translation class. **/
+
+#include "simbody/internal/MobilizedBody.h"
+
+namespace SimTK {
+
+/** Three translational mobilities describing the Cartesian motion of a point.
+The generalized coordinates q are x,y,z translations of the M (outboard) frame
+origin Mo along the parent (inboard) F frame axes. The generalized speeds u are
+the relative velocity v_FM of M's origin in F, so qdot=u for this mobilizer. **/
+class SimTK_SIMBODY_EXPORT MobilizedBody::Translation : public MobilizedBody {
+public:
+ /** Default constructor provides an empty handle that can be assigned to
+ reference any %MobilizedBody::Translation. **/
+ Translation() {}
+
+ /** Create a %Translation mobilizer between an existing parent (inboard)
+ body P and a new child (outboard) body B created by copying the given
+ \a bodyInfo into a privately-owned Body within the constructed
+ %MobilizedBody object. Specify the mobilizer frames F fixed to parent P and
+ M fixed to child B.
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ Translation(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM,
+ Direction=Forward);
+
+ /** Abbreviated constructor you can use if the mobilizer frames are
+ coincident with the parent and child body frames. **/
+ Translation(MobilizedBody& parent, const Body& bodyInfo, Direction=Forward);
+
+ Translation& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
+ }
+ Translation& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
+ }
+ Translation& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
+ }
+
+ Translation& setDefaultInboardFrame(const Transform& X_PF) {
+ (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
+ }
+
+ Translation& setDefaultOutboardFrame(const Transform& X_BM) {
+ (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
+ }
+
+ // This is just a nicer name for setting this mobilizer's generalized coordinates,
+ // which together constitute the vector from the F frame's origin to the M
+ // frame's origin, expressed in F.
+
+ // Set the topological default values for the initial q's.
+ Translation& setDefaultTranslation(const Vec3& p_FM) {
+ return setDefaultQ(p_FM);
+ }
+
+ // Get the topological default values for the initial q's.
+ const Vec3& getDefaultTranslation() const {
+ return getDefaultQ();
+ }
+
+ // Set the current value of q's in the given State. Note that this is
+ // the *cross-mobilizer* translation, not location in the Ground frame.
+ void setMobilizerTranslation(State& s, const Vec3& p_FM) const {
+ setQ(s,p_FM);
+ }
+
+ // Get the current value of the q's for this mobilizer from the given State.
+ const Vec3& getMobilizerTranslation(const State& s) const {
+ return getQ(s);
+ }
+
+
+ // Set the current value of u's in the given State. Note that this is
+ // the *cross-mobilizer* velocity v_FM, not velocity in the Ground frame.
+ void setMobilizerVelocity(State& s, const Vec3& v_FM) const {
+ setU(s,v_FM);
+ }
+
+ // Get the current value of the u's for this mobilizer from the given State.
+ const Vec3& getMobilizerVelocity(const State& s) const {
+ return getU(s);
+ }
+
+ // Get the value of the udot's for this mobilizer from the given State.
+ const Vec3& getMobilizerAcceleration(const State& s) const {
+ return getUDot(s);
+ }
+
+ // Generic default state Topology methods.
+ const Vec3& getDefaultQ() const;
+ Translation& setDefaultQ(const Vec3& q);
+
+ const Vec3& getQ(const State&) const;
+ const Vec3& getQDot(const State&) const;
+ const Vec3& getQDotDot(const State&) const;
+ const Vec3& getU(const State&) const;
+ const Vec3& getUDot(const State&) const;
+
+ void setQ(State&, const Vec3&) const;
+ void setU(State&, const Vec3&) const;
+
+ const Vec3& getMyPartQ(const State&, const Vector& qlike) const;
+ const Vec3& getMyPartU(const State&, const Vector& ulike) const;
+
+ Vec3& updMyPartQ(const State&, Vector& qlike) const;
+ Vec3& updMyPartU(const State&, Vector& ulike) const;
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Translation, TranslationImpl,
+ MobilizedBody);
+ /** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_TRANSLATION_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_Universal.h b/Simbody/include/simbody/internal/MobilizedBody_Universal.h
new file mode 100644
index 0000000..415a17d
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_Universal.h
@@ -0,0 +1,88 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_UNIVERSAL_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_UNIVERSAL_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy, 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::Universal class. **/
+
+#include "simbody/internal/MobilizedBody.h"
+
+namespace SimTK {
+
+/** Two mobilities -- rotation about the x axis, followed by a rotation
+about the new y axis. The two generalized speeds u are the time derivatives of
+these angles so qdot=u for this mobilizer.
+
+This mobilizer is badly behaved when the second rotation is near 90 degrees (no
+worse than a real U-joint, though). **/
+class SimTK_SIMBODY_EXPORT MobilizedBody::Universal : public MobilizedBody {
+public:
+ /** Default constructor provides an empty handle that can be assigned to
+ reference any %MobilizedBody::Universal. **/
+ Universal() {}
+
+ /** Create a %Universal mobilizer between an existing parent (inboard)
+ body P and a new child (outboard) body B created by copying the given
+ \a bodyInfo into a privately-owned Body within the constructed
+ %MobilizedBody object. Specify the mobilizer frames F fixed to parent P and
+ M fixed to child B.
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ Universal(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM, Direction=Forward);
+
+ /** Abbreviated constructor you can use if the mobilizer frames are
+ coincident with the parent and child body frames. **/
+ Universal(MobilizedBody& parent, const Body& bodyInfo, Direction=Forward);
+
+
+ Universal& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
+ }
+ Universal& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
+ }
+ Universal& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
+ }
+
+ Universal& setDefaultInboardFrame(const Transform& X_PF) {
+ (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
+ }
+
+ Universal& setDefaultOutboardFrame(const Transform& X_BM) {
+ (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
+ }
+ /** @cond **/ // Don't let doxygen see this
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Universal, UniversalImpl,
+ MobilizedBody);
+ /** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_UNIVERSAL_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/MobilizedBody_Weld.h b/Simbody/include/simbody/internal/MobilizedBody_Weld.h
new file mode 100644
index 0000000..f073eb2
--- /dev/null
+++ b/Simbody/include/simbody/internal/MobilizedBody_Weld.h
@@ -0,0 +1,89 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_WELD_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_WELD_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy, 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Declares the MobilizedBody::Weld class. **/
+
+#include "simbody/internal/MobilizedBody.h"
+
+namespace SimTK {
+
+/** Zero mobilities. This degenerate "mobilizer" serves only to weld together
+the M frame of a body to the F frame on its parent. It has no generalized
+coordinates or speeds. Note that there is no "reverse" weld, because "reverse"
+for a mobilizer refers to how the q's and u's are defined and there are none.
+
+You can use this (im)mobilizer to create a composite rigid body from simpler
+rigid bodies. Although the effect is as though the bodies were combined, they
+are still tracked separately so per-body information remains available. You can
+also get the reaction force at the weld in the usual manner. **/
+class SimTK_SIMBODY_EXPORT MobilizedBody::Weld : public MobilizedBody {
+public:
+ /** Default constructor provides an empty handle that can be assigned to
+ reference any %MobilizedBody::Weld. **/
+ Weld() {};
+
+ /** Create a %Weld mobilizer between an existing parent (inboard) body P
+ and a new child (outboard) body B created by copying the given \a bodyInfo
+ into a privately-owned Body within the constructed %MobilizedBody object.
+ Specify the mobilizer frames F fixed to parent P and M fixed to child B.
+ @see MobilizedBody for a diagram and explanation of terminology. **/
+ Weld(MobilizedBody& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM);
+
+ /** Abbreviated constructor you can use if the mobilizer frames are
+ coincident with the parent and child body frames. **/
+ Weld(MobilizedBody& parent, const Body& bodyInfo);
+
+ Weld& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
+ }
+ Weld& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
+ }
+ Weld& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
+ (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
+ }
+
+ Weld& setDefaultInboardFrame(const Transform& X_PF) {
+ (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
+ }
+
+ Weld& setDefaultOutboardFrame(const Transform& X_BM) {
+ (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
+ }
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Weld, WeldImpl, MobilizedBody);
+ /** @endcond **/
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_WELD_H_
+
+
+
diff --git a/Simbody/include/simbody/internal/Motion.h b/Simbody/include/simbody/internal/Motion.h
new file mode 100644
index 0000000..e51ec10
--- /dev/null
+++ b/Simbody/include/simbody/internal/Motion.h
@@ -0,0 +1,535 @@
+#ifndef SimTK_SIMBODY_MOTION_H_
+#define SimTK_SIMBODY_MOTION_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) 2009-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+This defines the Motion class, which is used to specify how the mobilities
+associated with a particular mobilizer are to be treated. **/
+
+#include "SimTKcommon.h"
+#include "simbody/internal/common.h"
+
+namespace SimTK {
+
+class SimbodyMatterSubsystem;
+class MobilizedBody;
+class Motion;
+class MotionImpl;
+
+// We only want the template instantiation to occur once. This symbol is defined
+// in the Simbody compilation unit that defines the Motion class but should not
+// be defined any other time.
+#ifndef SimTK_SIMBODY_DEFINING_MOTION
+ extern template class PIMPLHandle<Motion, MotionImpl, true>;
+#endif
+
+
+//==============================================================================
+// MOTION
+//==============================================================================
+/** A %Motion object belongs to a particular MobilizedBody and prescribes how
+the associated motion is to be calculated.
+
+%Motions can provide functionality similar to some constraints, but are much
+more efficient. There are two independent aspects of prescribed motion:
+ - at what level is the motion driven (low to high: acceleration, velocity,
+ or position), and
+ - how is the motion at that level specified.
+
+Levels lower than the driven level are also driven; higher levels are free and
+determined by integration or other solution method.
+
+Concrete %Motion objects derived from the base class provide specialized
+functionality, such as prescribing the mobilizer motion as a function of time.
+The specialized functionality may be enabled and disabled at run time, with
+motion determined dynamically if the %Motion object is disabled.
+
+For mobilizers with more than one mobility, the associated %Motion controls
+\e all the mobilities and moreover they are all driven at the same level and by
+the same method. Using MobilizedBody::lock() temporarily overrides the
+%Motion associated with that mobilizer.
+
+<h3>Details</h3>
+
+This table shows all the possible ways in which mobilizer motion can be
+determined, based on the %Motion level and method. The default is that we
+determine motion at the acceleration level from forces, with velocity and
+position calculated by integrating the acceleration.
+ at verbatim
+ | Motion Method
+ Level How driven | Acceleration Velocity Position
+ ----- ---------- | ------------ ---------- ----------
+ Acc Zero | 0 discrete free
+ " Discrete | discrete free "
+ " Prescribed | a(t,q,u) " "
+ " Free | from forces " " <-- default
+
+ Vel Zero | 0 0 discrete
+ " Discrete | 0 discrete free
+ " Prescribed | dv/dt v(t,q) "
+ " Fast | 0 relax(v) "
+
+ Pos Zero | 0 0 0 (ref.)
+ " Discrete | 0 0 discrete
+ " Prescribed | d2p/dt2 dp/dt p(t)
+ " Fast | 0 0 relax(p)
+ at endverbatim
+There are two duplicates in the above table: specifying acceleration as Zero is
+the same as specifying velocity as Discrete and specifying velocity as Zero is
+the same as specifying position as Discrete.
+
+%Motion is a PIMPL-style abstract base class, with concrete classes defined for
+each kind of %Motion. There is a set of built-in Motions and a generic "Custom"
+%Motion (an abstract base class) from which advanced users may derive their own
+%Motion objects. **/
+class SimTK_SIMBODY_EXPORT Motion : public PIMPLHandle<Motion,MotionImpl,true> {
+public:
+
+/** What is the highest level of motion that is driven? Lower levels are
+also driven; higher levels are determined by integration. **/
+enum Level {
+ NoLevel = -1, ///< invalid level
+ Acceleration = 0, ///< we know udot; integrate to get u, q
+ Velocity = 1, ///< we know u and udot; integrate to get q
+ Position = 2 ///< we know q, u, and udot
+};
+/** Returns a human-readable name corresponding to the given Level; useful
+for debugging. If the Level is unrecognized the method will return some text
+to that effect rather than crashing. **/
+static const char* nameOfLevel(Level);
+
+/** There are several ways to specify the motion at this Level, and the
+selected method also determines lower-level motions. Free is only permitted
+when Level==Acceleration, and Fast is not allowed for that Level. **/
+enum Method {
+ NoMethod = -1,///< invalid method
+ Zero = 0, ///< motion at this level and below is always zero
+ Discrete = 1, ///< motion is "slow"; lower levels are zero
+ Prescribed = 2, ///< motion is function of time and state; <level is derivative
+ Free = 3, ///< accel. calculated from forces, pos and vel integrated
+ Fast = 4 ///< motion is "fast"; lower levels are zero
+};
+/** Returns a human-readable name corresponding to the given Method; useful
+for debugging. If the Method is unrecognized the method will return
+some text to that effect rather than crashing. **/
+static const char* nameOfMethod(Method);
+
+/** Default constructor creates an empty %Motion handle that can be assigned
+to reference any kind of %Motion object. **/
+Motion() {}
+
+/** Get the highest level being driven by this %Motion. If the mobilizer is
+locked, this is the level that would be driven if the mobilizer were unlocked.
+If this %Motion is currently disabled, it returns Motion::NoLevel. **/
+Level getLevel(const State&) const;
+/** Get the method being used to control the indicated Level. If the mobilizer
+is locked, this is the method that would be used if the mobilizer were unlocked.
+If this %Motion is currently disabled, it returns Motion::NoMethod. **/
+Method getLevelMethod(const State&) const;
+
+//------------------------------------------------------------------------------
+/**@name Enabling and disabling
+
+These methods determine whether this %Motion object is active in a given State.
+When disabled, the concrete %Motion object's prescribed motion is ignored.
+Normally %Motion objects are enabled when defined unless explicitly disabled;
+you can reverse that using the setDisabledByDefault() method below. You can then
+override the default setting at run time using the enable() and disable()
+methods to modify the %Motion behavior within a particular State.
+
+Note that when a %Motion is disabled, the mobilizer to which it belongs is
+unrestricted by the concrete %Motion object, but may be restricted in other ways
+such as being locked, constrained, or acted upon by forces. Even if a %Motion
+is enabled, a lock request takes priority; the %Motion will resume its effect
+when the mobilizer is unlocked. **/
+/**@{**/
+/** Disable this %Motion, effectively removing it from the mobilizer to which
+it belongs and allowing the mobilizer to move freely (unless locked). This is
+an Stage::Instance change and affects the allocation of %Motion-related
+resources in the supplied State.
+ at see enable(), setDisabledByDefault(), SimTK::MobilizedBody::lock() **/
+void disable(State& state) const;
+
+/** Enable this %Motion, without necessarily satisfying it. This is an
+Instance-stage change and affects the allocation of %Motion-related resources
+in the supplied State. Note that merely enabling a %Motion does
+not ensure that the State's positions and velocities satisfy the %Motion's
+requirements; initial satisfaction requires use of an appropriate prescribe()
+or project() solver. Also, if the controlled mobilizer is currently locked
+enabling the %Motion will not take effect until the mobilizer is unlocked.
+ at see disable(), SimTK::System::prescribe(), SimTK::System::project()
+ at see SimTK::MobilizedBody::unlock() **/
+void enable(State& state) const;
+
+/** Test whether this %Motion is currently disabled in the supplied State. **/
+bool isDisabled(const State& state) const;
+
+/** Specify that a %Motion is to be inactive by default. This is a topological
+change, meaning you'll have to call realizeTopology() again and get a new State.
+If you just want to disable temporarily, use disable() instead.
+ at see isDisabledByDefault(), disable() **/
+void setDisabledByDefault(bool shouldBeDisabled);
+
+/** Test whether this %Motion is disabled by default in which case it must
+be explicitly enabled before it will take effect.
+ at see setDisabledByDefault(), enable() **/
+bool isDisabledByDefault() const;
+/**@}**/
+
+//------------------------------------------------------------------------------
+/**@name Advanced/Obscure/Debugging
+You probably won't need to use these. **/
+/**@{**/
+/** Get the MobilizedBody to which this %Motion belongs. **/
+const MobilizedBody& getMobilizedBody() const;
+
+
+/** (Advanced) This implements the above table. Given the (level,method)
+Motion specification, it reports the actual method to be used for each of
+the three levels. **/
+void calcAllMethods(const State& s, Method& qMethod, Method& uMethod,
+ Method& udotMethod) const;
+/**@}**/
+
+class Steady;
+class Linear;
+class Sinusoid;
+class Polynomial;
+class Custom;
+
+class SteadyImpl;
+class LinearImpl;
+class SinusoidImpl;
+class PolynomialImpl;
+class CustomImpl;
+
+protected:
+/** For internal use: construct a new %Motion handle referencing a
+particular implementation object. **/
+explicit Motion(MotionImpl* r) : HandleBase(r) { }
+};
+
+
+//==============================================================================
+// MOTION :: SINUSOID
+//==============================================================================
+/** Prescribe position, velocity, or acceleration motion as a sinusoidal
+function of time, m(t) = a * sin( w*t + p ). **/
+class SimTK_SIMBODY_EXPORT Motion::Sinusoid : public Motion {
+public:
+ /** Create a sinusoidal prescribed motion applied at position, velocity,
+ or acceleration level.
+
+ @param[in,out] mobod
+ The MobilizedBody to which this %Motion should be added.
+ @param[in] level
+ The %Motion level that is being prescribed: Motion::Position,
+ Motion::Velocity, or Motion::Acceleration.
+ @param[in] amplitude
+ Scaling factor mapping the -1..1 sin() result to your desired
+ units; output values will range between -amplitude and +amplitude.
+ @param[in] rate
+ Angular rate in radians/unit time; e.g. if time is in seconds
+ then rate=2*Pi would be 1 Hz (1 rotation per second).
+ @param[in] phase
+ Phase angle in radians.
+ **/
+ Sinusoid(MobilizedBody& mobod, Motion::Level level,
+ Real amplitude, Real rate, Real phase);
+
+ /** Default constructor creates an empty handle that can be assigned to
+ reference any Motion::Sinusoid object. **/
+ Sinusoid() {}
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Sinusoid, SinusoidImpl, Motion);
+ /** @endcond **/
+};
+
+
+//==============================================================================
+// MOTION :: STEADY
+//==============================================================================
+/** This non-holonomic Motion object imposes a constant rate on all mobilities.
+**/
+class SimTK_SIMBODY_EXPORT Motion::Steady : public Motion {
+public:
+ /** Create a Motion::Steady where all mobilities have the same velocity.
+ @param[in,out] mobod the MobilizedBody to which this Motion should be added
+ @param[in] u the rate to be applied to all mobilities **/
+ Steady(MobilizedBody& mobod, Real u);
+
+ /** Create a Motion::Steady with different velocities for each mobility
+ specified. Any unspecified mobilities will get zero velocity.
+ @param[in,out] mobod the MobilizedBody to which this Motion should be added
+ @param[in] u the rates to be applied to the first N mobilities; the
+ rest are set to zero **/
+ template <int N> SimTK_SIMBODY_EXPORT
+ Steady(MobilizedBody& mobod, const Vec<N>& u); // instantiated in library
+
+ /** Default constructor creates an empty handle than can be assigned to
+ reference any Motion::Steady object. **/
+ Steady() {}
+
+ /** Change the default rate this %Motion will prescribe unless overridden in
+ a particular State. All mobilities will use this same rate. **/
+ Steady& setDefaultRate(Real u);
+ /** Change the default rate this %Motion will prescribe for one mobility,
+ unless overridden in a particular State. Rates for the other mobilities (if
+ there is more than one) remain unchanged. **/
+ Steady& setOneDefaultRate(MobilizerUIndex, Real u);
+ /** Change the default rates this %Motion will prescribe, supplying separate
+ rates for each mobility as a Vec<N>. **/
+ template <int N> SimTK_SIMBODY_EXPORT
+ Steady& setDefaultRates(const Vec<N>& u); // instantiated in library
+
+ /** Get the default rate setting for one mobility. **/
+ Real getOneDefaultRate(MobilizerUIndex ux) const;
+
+ /** Change the rate to be prescribed by this %Motion when used with the
+ given State. All mobilities will use this same rate. **/
+ void setRate(State& state, Real u) const; // all axes set to u
+ /** Change the rate this %Motion will prescribe for one mobility when used
+ with the given State. Rates for the other mobilities (if there is more than
+ one) remain unchanged. **/
+ void setOneRate(State& state, MobilizerUIndex ux, Real u) const;
+
+ /** Get the rate setting for one mobility. **/
+ Real getOneRate(const State& state, MobilizerUIndex ux) const;
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Steady, SteadyImpl, Motion);
+ /** @endcond **/
+};
+
+
+
+//==============================================================================
+// MOTION :: CUSTOM
+//==============================================================================
+/** This class can be used to define new motions. To use it, create a class that
+extends Motion::Custom::Implementation. You can then create an instance of it
+and pass it to the Motion::Custom constructor:
+
+<pre>
+Motion::Custom myMotion(mobod, new MyMotionImplementation());
+</pre>
+
+Alternatively, you can create a subclass of Motion::Custom which creates the
+Implementation itself:
+
+<pre>
+class MyMotion : public Motion::Custom {
+public:
+ MyMotion(MobilizedBody& mobod)
+ : Motion::Custom(mobod, new MyMotionImplementation()) {}
+};
+</pre>
+
+This allows a user to simply write
+
+<pre>
+MyMotion(mobod);
+</pre>
+
+and not worry about implementation classes or creating objects on the heap. If
+you do this, your Motion::Custom handle subclass must not have any data members
+or virtual methods. If it does, it will not work correctly. Instead, store all
+data in the Implementation subclass.
+ at see SimTK::Motion::Custom::Implementation, SimTK::Motion **/
+class SimTK_SIMBODY_EXPORT Motion::Custom : public Motion {
+public:
+ class Implementation;
+
+ /** Create a Custom %Motion.
+ @param mobod the MobilizedBody to which this Motion should be added
+ @param implementation the object which implements the custom Motion. The
+ Motion::Custom takes over ownership of the
+ implementation object, and deletes it when the Motion
+ itself is deleted. **/
+ Custom(MobilizedBody& mobod, Implementation* implementation);
+
+ /** Default constructor creates an empty handle that can be assigned to
+ reference any Motion::Custom object. **/
+ Custom() {}
+
+ /** @cond **/ // hide from Doxygen
+ SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Custom, CustomImpl, Motion);
+ /** @endcond **/
+protected:
+ const Implementation& getImplementation() const;
+ Implementation& updImplementation();
+};
+
+
+//==============================================================================
+// MOTION :: CUSTOM :: IMPLEMENTATION
+//==============================================================================
+/** This is the abstract base class for Custom Motion implementations.
+ at see SimTK::Motion::Custom **/
+class SimTK_SIMBODY_EXPORT Motion::Custom::Implementation {
+public:
+ /** Destructor is virtual; be sure to provide one in you concrete class if
+ there is anything to destruct. **/
+ virtual ~Implementation() { }
+
+ /** Override this if you want your Motion objects to be copyable. **/
+ virtual Implementation* clone() const {
+ SimTK_ERRCHK_ALWAYS(!"unimplemented",
+ "Motion::Custom::Implementation::clone()",
+ "Concrete Implementation did not supply a clone() method, "
+ "but a copy operation was attempted.");
+ /*NOTREACHED*/
+ return 0;
+ }
+
+ /** A %Motion prescribes either position, velocity, or acceleration.
+ When velocity is prescribed, acceleration must also be prescribed as the
+ time derivative of the velocity. And, when position is prescribed, velocity
+ must also be prescribed as the time derivative of the position (and
+ acceleration as above). Thus acceleration is \e always prescribed. Anything
+ not prescribed will be determined by numerical integration, by relaxation,
+ or by discrete changes driven by events, depending on whether the associated
+ mobilizer is "free", "fast", or "slow", respectively. **/
+ virtual Motion::Level getLevel(const State&) const = 0;
+
+ /** Override this if the method is not Motion::Prescribed. **/
+ virtual Motion::Method getLevelMethod(const State&) const {
+ return Motion::Prescribed;
+ }
+
+ /** @name Position (Holonomic) prescribed motion virtuals
+
+ These must be defined if the motion method is "Prescribed" and the
+ motion level is "Position". In that case q=q(t), qdot, and qdotdot are
+ all required. Note that Simbody passes in the number of q's being
+ prescribed; make sure you are seeing what you expect. **/
+ //@{
+ /** This operator is called during the MatterSubsystem's realize(Time)
+ computation. This Motion's own realizeTime() method will already have
+ been called. The result must depend only on time and earlier-stage
+ state variables. **/
+ virtual void calcPrescribedPosition
+ (const State& s, int nq, Real* q) const;
+
+ /** Calculate the time derivative of the prescribed positions. The qdots
+ calculated here must be the exact time derivatives of the q's returned
+ by calcPrescribedPosition(). So the calculation must be limited to
+ the same dependencies, plus the current value of this mobilizer's q's
+ (or the cross-mobilizer transform X_FM because that depends only on
+ those q's). Note that we are return qdots, not u's; they are not always
+ the same. Simbody knows how to map from qdots to u's when necessary.
+
+ This operator is called during the MatterSubsystem's realize(Position)
+ computation. This Motion's own realizePosition() method will already
+ have been called. **/
+ virtual void calcPrescribedPositionDot
+ (const State& s, int nq, Real* qdot) const;
+
+ /** Calculate the 2nd time derivative of the prescribed positions. The
+ qdotdots calculated here must be the exact time derivatives of the qdots
+ returned by calcPrescribedPositionDot(). So the calculation must be
+ limited to the same dependencies, plus the current value of this
+ mobilizer's qdots (or the cross-mobilizer velocity V_FM because that
+ depends only on those qdots). Note that we are return qdotdots, not
+ udots; they are not always the same. Simbody knows how to map from
+ qdotdots to udots when necessary.
+
+ This operator is called during the MatterSubsystem's realize(Dynamics)
+ computation. This Motion's own realizeDynamics() method will already
+ have been called. **/
+ virtual void calcPrescribedPositionDotDot
+ (const State& s, int nq, Real* qdotdot) const;
+ //@}
+
+ /** @name Velocity (Nonholonomic) prescribed motion virtuals
+
+ These must be defined if the motion method is "Prescribed" and the
+ motion level is "Velocity". In that case u=u(t,q), and udot are
+ both required. Note that Simbody passes in the number of u's being
+ prescribed; make sure you are seeing what you expect. **/
+ //@{
+ /** This operator is called during the MatterSubsystem's realize(Position)
+ computation. The result must depend only on time and positions (of any
+ body or mobilizer), or earlier-stage state variables; it must not depend
+ on any velocities. This Motion's own realizePosition() method will
+ already have been called. This will not be called if the u's are known
+ to be zero. **/
+ virtual void calcPrescribedVelocity
+ (const State& s, int nu, Real* u) const;
+
+ /** Calculate the time derivative of the prescribed velocity. The udots
+ calculated here must be the exact time derivatives of the u's returned
+ by calcPrescribedVelocity(). So the calculation must be limited to the
+ same dependencies, plus the current value of this mobilizer's u's (or
+ the cross-mobilizer velocity V_FM because that depends only on those u's).
+
+ This operator is called during the MatterSubsystem's realize(Dynamics)
+ computation. This Motion's own realizeDynamics() method will already
+ have been called. This will not be called if the udots are known to be
+ zero. **/
+ virtual void calcPrescribedVelocityDot
+ (const State& s, int nu, Real* udot) const;
+ //@}
+
+ /** @name Acceleration-only prescribed motion virtual
+
+ This must be defined if the motion method is "Prescribed" and the
+ motion level is "Acceleration". In that case udot=udot(t,q,u) is
+ required. Note that Simbody passes in the number of u's (same as
+ number of udots) being prescribed; make sure you are seeing what you
+ expect. **/
+ //@{
+ /** This operator is called during the MatterSubsystem's realize(Dynamics)
+ computation. The result can depend on time, any positions, and any
+ velocities but must not depend on accelerations or reaction forces. This
+ Motion's own realizeDynamics() method will already have been called.
+ This will not be called if the udots are known to be zero. **/
+ virtual void calcPrescribedAcceleration
+ (const State& s, int nu, Real* udot) const;
+ //@}
+
+ /** @name Optional realize() virtual methods
+
+ The following methods may optionally be overridden to do specialized
+ realization for a Motion. These are called during the corresponding
+ realization stage of the containing MatterSubsystem. **/
+ //@{
+ virtual void realizeTopology (State& state) const {}
+ virtual void realizeModel (State& state) const {}
+ virtual void realizeInstance (const State& state) const {}
+ virtual void realizeTime (const State& state) const {}
+ virtual void realizePosition (const State& state) const {}
+ virtual void realizeVelocity (const State& state) const {}
+ virtual void realizeDynamics (const State& state) const {}
+ virtual void realizeAcceleration(const State& state) const {}
+ virtual void realizeReport (const State& state) const {}
+ //@}
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOTION_H_
diff --git a/Simbody/include/simbody/internal/MultibodySystem.h b/Simbody/include/simbody/internal/MultibodySystem.h
new file mode 100644
index 0000000..7b7954b
--- /dev/null
+++ b/Simbody/include/simbody/internal/MultibodySystem.h
@@ -0,0 +1,113 @@
+#ifndef SimTK_SIMBODY_MULTIBODY_SYSTEM_H_
+#define SimTK_SIMBODY_MULTIBODY_SYSTEM_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 <vector>
+
+namespace SimTK {
+
+class SimbodyMatterSubsystem;
+class ForceSubsystem;
+class DecorationSubsystem;
+class GeneralContactSubsystem;
+
+
+/** The job of the MultibodySystem class is to coordinate the activities of
+various subsystems which can be part of a multibody system. We insist on
+having exactly one SimbodyMatterSubsystem, and we would like also to have:
+ - one or more ForceSubsystems
+ - a DecorationSubsystem for visualization
+ - a GeneralContactSubsystem for contact geometry
+There will also be a generic System-level "subsystem" for global variables.
+**/
+class SimTK_SIMBODY_EXPORT MultibodySystem : public System {
+public:
+ MultibodySystem();
+ explicit MultibodySystem(SimbodyMatterSubsystem& m);
+
+ // Steals ownership of the source; returns subsystem ID number.
+ int addForceSubsystem(ForceSubsystem&);
+
+ int setMatterSubsystem(SimbodyMatterSubsystem&);
+ const SimbodyMatterSubsystem& getMatterSubsystem() const;
+ SimbodyMatterSubsystem& updMatterSubsystem();
+ bool hasMatterSubsystem() const;
+
+ int setDecorationSubsystem(DecorationSubsystem&);
+ const DecorationSubsystem& getDecorationSubsystem() const;
+ DecorationSubsystem& updDecorationSubsystem();
+ bool hasDecorationSubsystem() const;
+
+ int setContactSubsystem(GeneralContactSubsystem&);
+ const GeneralContactSubsystem& getContactSubsystem() const;
+ GeneralContactSubsystem& updContactSubsystem();
+ bool hasContactSubsystem() const;
+
+
+ /// Calculate the total potential energy of the system. The state must
+ /// be at Dynamics stage or later.
+ const Real calcPotentialEnergy(const State&) const;
+ /// Calculate the total kinetic energy of the system. The state must
+ /// be at Velocity stage or later.
+ const Real calcKineticEnergy(const State&) const;
+ /// Calculate the total energy of the system. The state must
+ /// be at Dynamics stage or later.
+ Real calcEnergy(const State& s) const {
+ return calcPotentialEnergy(s)+calcKineticEnergy(s);
+ }
+
+ // These methods are for use by our constituent subsystems to communicate
+ // with each other and with the MultibodySystem as a whole.
+
+ // These cache entries belong to the global subsystem, which zeroes them at
+ // the start of the corresponding stage. They are filled in by the force
+ // subsystems when they are realized to each stage. Forces are cumulative
+ // from stage to stage, so the Dynamics stage includes everything. That may
+ // then be accessed by the matter subsystem in Acceleration stage to
+ // generate the accelerations.
+ const Vector_<SpatialVec>& getRigidBodyForces(const State&, Stage) const;
+ const Vector_<Vec3>& getParticleForces (const State&, Stage) const;
+ const Vector& getMobilityForces (const State&, Stage) const;
+
+ // These routines are for use by force subsystems during Dynamics stage.
+ Vector_<SpatialVec>& updRigidBodyForces(const State&, Stage) const;
+ Vector_<Vec3>& updParticleForces (const State&, Stage) const;
+ Vector& updMobilityForces (const State&, Stage) const;
+
+ // Private implementation.
+ SimTK_PIMPL_DOWNCAST(MultibodySystem, System);
+ class MultibodySystemRep& updRep();
+ const MultibodySystemRep& getRep() const;
+protected:
+ explicit MultibodySystem(MultibodySystemRep*);
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MULTIBODY_SYSTEM_H_
diff --git a/Simbody/include/simbody/internal/ObservedPointFitter.h b/Simbody/include/simbody/internal/ObservedPointFitter.h
new file mode 100644
index 0000000..907f444
--- /dev/null
+++ b/Simbody/include/simbody/internal/ObservedPointFitter.h
@@ -0,0 +1,129 @@
+#ifndef SimTK_SIMBODY_OBSERVED_POINT_FITTER_H_
+#define SimTK_SIMBODY_OBSERVED_POINT_FITTER_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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/MultibodySystem.h"
+
+namespace SimTK {
+
+/**
+ * This class attempts to find the configuration of an internal coordinate model which best fits a set of
+ * observed data. The inputs to the algorithm are as follows:
+ *
+ * - A MultibodySystem which describes the model to fit
+ * - A set of points (called "stations") whose locations are defined relative to particular bodies
+ * - The target location for each station, defined relative to ground
+ * - (optional) A weight for each station, giving its relative importance for fitting
+ *
+ * The output is a State giving the set of internal coordinates that best fit the stations to the target locations.
+ */
+
+class SimTK_SIMBODY_EXPORT ObservedPointFitter {
+public:
+ /**
+ * Find the configuration of a MultibodySystem which best fits a set of target locations for stations. This is identical to the other form of findBestFit(), but assumes every station
+ * has a weight of 1.
+ */
+
+ static Real findBestFit
+ (const MultibodySystem& system,
+ State& state,
+ const Array_<MobilizedBodyIndex>& bodyIxs,
+ const Array_<Array_<Vec3> >& stations,
+ const Array_<Array_<Vec3> >& targetLocations,
+ Real tolerance=0.001);
+
+ /** For compatibility with std::vector; requires extra copying. **/
+ static Real findBestFit
+ (const MultibodySystem& system,
+ State& state,
+ const std::vector<MobilizedBodyIndex>& bodyIxs,
+ const std::vector<std::vector<Vec3> >& stations,
+ const std::vector<std::vector<Vec3> >& targetLocations,
+ Real tolerance=0.001)
+ {
+ Array_<Array_<Vec3> > stationCopy(stations);
+ Array_<Array_<Vec3> > targetCopy(targetLocations);
+ return findBestFit(system,state,
+ ArrayViewConst_<MobilizedBodyIndex>(bodyIxs), // no copying here
+ stationCopy, targetCopy, tolerance);
+ }
+
+ /**
+ * Find the configuration of a MultibodySystem which best fits a set of target locations for stations.
+ *
+ * @param system the MultibodySystem being analyzed
+ * @param state on exit, this State's Q vector contains the values which provide a best fit
+ * @param bodyIxs a list of MobilizedBodyIndexs corresponding to the bodies for which stations are defined
+ * @param stations the list of stations for each body. stations[i][j] is the location of the j'th station for the body given by
+ * bodyIxs[i], given in that body's reference frame.
+ * @param targetLocations the target locations for each body, given relative to ground. targetLocations[i][j] is the target for stations[i][j].
+ * @param weights weights[i][j] is the weight to use for stations[i][j] when performing the fitting
+ * @param tolerance the distance tolerance within which the best fit should be found
+ * @return the RMS distance of points in the best fit conformation from their target locations
+ */
+
+ static Real findBestFit
+ (const MultibodySystem& system,
+ State& state,
+ const Array_<MobilizedBodyIndex>& bodyIxs,
+ const Array_<Array_<Vec3> >& stations,
+ const Array_<Array_<Vec3> >& targetLocations,
+ const Array_<Array_<Real> >& weights,
+ Real tolerance=0.001);
+
+ /** For compatibility with std::vector; requires extra copying. **/
+ static Real findBestFit
+ (const MultibodySystem& system,
+ State& state,
+ const std::vector<MobilizedBodyIndex>& bodyIxs,
+ const std::vector<std::vector<Vec3> >& stations,
+ const std::vector<std::vector<Vec3> >& targetLocations,
+ const std::vector<std::vector<Real> >& weights,
+ Real tolerance=0.001)
+ {
+ Array_<Array_<Vec3> > stationCopy(stations);
+ Array_<Array_<Vec3> > targetCopy(targetLocations);
+ Array_<Array_<Real> > weightCopy(weights);
+ return findBestFit(system,state,
+ ArrayViewConst_<MobilizedBodyIndex>(bodyIxs), // no copying here
+ stationCopy, targetCopy, weightCopy,
+ tolerance);
+ }
+
+
+private:
+ static void createClonedSystem(const MultibodySystem& original, MultibodySystem& copy, const Array_<MobilizedBodyIndex>& originalBodyIxs, Array_<MobilizedBodyIndex>& copyBodyIxs, bool& hasArtificialBaseBody);
+ static void findUpstreamBodies(MobilizedBodyIndex currentBodyIx, const Array_<int> numStations, const SimbodyMatterSubsystem& matter, Array_<MobilizedBodyIndex>& bodyIxs, int requiredStations);
+ static void findDownstreamBodies(MobilizedBodyIndex currentBodyIx, const Array_<int> numStations, const Array_<Array_<MobilizedBodyIndex> > children, Array_<MobilizedBodyIndex>& bodyIxs, int& requiredStations);
+ static int findBodiesForClonedSystem(MobilizedBodyIndex primaryBodyIx, const Array_<int> numStations, const SimbodyMatterSubsystem& matter, const Array_<Array_<MobilizedBodyIndex> > children, Array_<MobilizedBodyIndex>& bodyIxs);
+ class OptimizerFunction;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_OBSERVED_POINT_FITTER_H_
diff --git a/Simbody/include/simbody/internal/SimbodyMatterSubsystem.h b/Simbody/include/simbody/internal/SimbodyMatterSubsystem.h
new file mode 100644
index 0000000..0b4d9be
--- /dev/null
+++ b/Simbody/include/simbody/internal/SimbodyMatterSubsystem.h
@@ -0,0 +1,2772 @@
+#ifndef SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
+#define SimTK_SIMBODY_MATTER_SUBSYSTEM_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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy *
+ * *
+ * 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/MobilizedBody.h"
+
+#include <cassert>
+#include <vector>
+#include <iostream>
+
+class SimbodyMatterSubsystemRep;
+
+namespace SimTK {
+
+class MobilizedBody;
+class MultibodySystem;
+class Constraint;
+
+/** This subsystem contains the bodies ("matter") in the multibody system,
+the mobilizers (joints) that define the generalized coordinates used to
+represent the motion of those bodies, and constraints that must be satisfied
+by the values of those coordinates.
+
+There are many methods in the API for this class. For whole-system information
+and calculations, the methods here are the right ones to use. For information
+associated with individual objects contained in the subsystem, such as
+MobilizedBody and Constraint objects, it is generally easier to obtain the
+information through the contained objects' APIs instead.
+
+This class is is a "handle" containing only an opaque reference to the
+underlying implementation class.
+
+<h3>Theory discussion</h3>
+The bodies, mobilizers, and constraints are represented mathematically with
+the following set of equations:
+<pre>
+ qdot = N u Kinematic differential eqns.
+ zdot = zdot(t,q,u,z) Auxiliary states
+
+ M udot + ~G mult = f(t,q,u,z) Equations of motion
+ G udot = b(t,q,u)
+
+ where
+
+ [P] [bp]
+ G=[V] b=[bv] f = T + ~J*(F-C)
+ [A] [ba]
+
+ pdotdot = P udot - bp(t,q,u) = 0 Acceleration constraints
+ vdot = V udot - bv(t,q,u) = 0
+ a(t,q,u,udot) = A udot - ba(t,q,u) = 0
+
+ pdot = P u - c(t,q) = 0 Velocity constraints
+ v(t,q,u) = 0
+
+ p(t,q) = 0 Position constraints
+ n(q) = 0 Normalization constraints
+</pre>
+where M(q) is the mass matrix, G(t,q,u) the acceleration constraint matrix,
+C(q,u) the coriolis and gyroscopic forces, T is user-applied joint mobility
+forces, F is user-applied body forces and torques and gravity. J(q) is the
+%System Jacobian (partial velocity matrix) whose transpose ~J maps spatial
+forces to joint mobility forces. p(t,q) are the holonomic (position)
+constraints, v(t,q,u) the non-holonomic (velocity) constraints, and
+a(t,q,u,udot) the acceleration-only constraints, which must be linear in udot,
+with A(t,q,u) the coefficient matrix for a(). pdot, pdotdot are obtained by
+differentiation of p(), vdot by differentiation of v(). P(t,q)=Dpdot/Du
+(yes, that's u, not q -- we can get Pq=Dp/Dq when we need it as Pq=P*N^-1) and
+V(t,q,u)=Dv/Du. (We use capital "D" to indicate partial derivative.) n(q) is
+the set of quaternion normalization constraints, which exist only at the
+position level and are uncoupled from everything else.
+
+We calculate the constraint multipliers like this:
+<pre>
+ G M^-1 ~G mult = G udot0 - b
+ where udot0 = M^-1 f
+</pre>
+using the pseudo inverse of G M^-1 ~G to give a least squares solution for
+mult: mult = pinv(G M^-1 ~G)(G M^-1 f - b). Then the real udot is
+udot = udot0 - udotC, with udotC = M^-1 ~G mult. Note: M^-1* is an
+O(n) operator that provides the desired result; it <em>does not</em> require
+forming or factoring M.
+
+NOTE: only the following constraint matrices have to be formed and factored:
+<pre>
+ [G M^-1 ~G] to calculate multipliers
+
+ [P N^-1] for projection onto position manifold (a.k.a. Pq)
+
+ [ P ] for projection onto velocity manifold
+ [ V ]
+</pre>
+
+When working in a weighted norm with weights W on the state variables and
+weights T (1/tolerance) on the constraint errors, the matrices we need are
+actually [Tp Pq Wq^-1], [Tpv [P;V] Wu^-1], etc. with T and W diagonal
+weighting matrices. These can then be used to find least squares solutions
+in the weighted norms.
+
+In many cases these matrices consist of decoupled blocks which can
+be solved independently. (TODO: take advantage of that whenever possible
+to solve a set of smaller systems rather than one large one.) Also, in the
+majority of biosimulation applications we are likely to have only holonomic
+(position) constraints, so there is no V or A and G=P is the whole story.
+**/
+class SimTK_SIMBODY_EXPORT SimbodyMatterSubsystem : public Subsystem {
+public:
+
+//==============================================================================
+/** @name Construction, Destruction, Topological information
+
+Methods in this section are used in the extended construction phase for
+a %SimbodyMatterSubsystem which we call defining the "topology" of the
+multibody system. This includes adding mobilized bodies and constraints.
+Topological information is always state-independent since it is kept in
+the %SimbodyMatterSubsystem object directly. The construction phase ends
+when realizeTopology() is called on the containing System. **/
+/**@{**/
+
+/** Create a matter subsystem containing only the Ground body (mobilized
+body 0), and add the subsystem to the indicated MultibodySystem. The
+MultibodySystem takes over ownership of the subsystem, which is not
+copied. The MultibodySystem and this subsystem handle both refer to the
+same subsystem after this call. **/
+explicit SimbodyMatterSubsystem(MultibodySystem&);
+/** Create an orphan matter subsystem containing only the Ground body
+(mobilized body 0); normally use the other constructor to place the
+subsystem in a MultibodySystem. **/
+SimbodyMatterSubsystem();
+/** The destructor destroys the subsystem implementation object only if this
+handle is the last reference. Normally, there is a MultibodySystem that holds
+a reference to the subsystem implementation, so this destruction will do
+nothing. **/
+~SimbodyMatterSubsystem() {} // invokes ~Subsystem()
+
+/** Given a MobilizedBodyIndex, return a read-only (const) reference to the
+corresponding MobilizedBody within this matter subsystem. This method will
+fail if the index is invalid or out of range. MobilizedBodyIndex(0) selects
+the Ground mobilized body. **/
+const MobilizedBody& getMobilizedBody(MobilizedBodyIndex) const;
+
+/** Given a MobilizedBodyIndex, return a writable reference to the
+corresponding MobilizedBody within this matter subsystem. This method will
+fail if the index is invalid or out of range. MobilizedBodyIndex(0) selects
+the Ground mobilized body. **/
+MobilizedBody& updMobilizedBody(MobilizedBodyIndex);
+
+/** Return a read-only (const) reference to the Ground MobilizedBody
+within this matter subsystem. **/
+const MobilizedBody::Ground& getGround() const;
+/** Return a writable reference to the Ground MobilizedBody within this
+matter subsystem; you need a writable reference if you're adding a
+mobilized body that is directly connected to Ground. **/
+MobilizedBody::Ground& updGround();
+/** This is a synonym for updGround() that makes for nicer-looking examples.
+Note: topology is not marked invalid upon returning a writable reference
+here; that will be done only if a non-const method of the returned
+MobilizedBody is called. That means it is OK to use Ground() to satisfy
+a const argument; it won't have an "invalidate topology" side effect.
+ at see updGround() **/
+MobilizedBody::Ground& Ground() {return updGround();}
+
+
+/** Given a ConstraintIndex, return a read-only (const) reference to the
+corresponding Constraint within this matter subsystem. This method will
+fail if the index is invalid or out of range. **/
+const Constraint& getConstraint(ConstraintIndex) const;
+
+/** Given a ConstraintIndex, return a writable reference to the corresponding
+Constraint within this matter subsystem. This method will
+fail if the index is invalid or out of range. **/
+Constraint& updConstraint(ConstraintIndex);
+
+/** Normally the matter subsystem will attempt to generate some decorative
+geometry as a sketch of the defined multibody system; you can disable that
+with this method. **/
+void setShowDefaultGeometry(bool show);
+/** Get whether this matter subsystem is set to generate default decorative
+geometry that can be used to visualize this multibody system. **/
+bool getShowDefaultGeometry() const;
+
+/** The number of bodies includes all mobilized bodies \e including Ground,
+which is the 0th mobilized body. (Note: if special particle handling were
+implmemented, the count here would \e not include particles.) Bodies and their
+inboard mobilizers have the same index since they are grouped together as a
+MobilizedBody. MobilizedBody numbering (using unique integer type
+MobilizedBodyIndex) starts with Ground at MobilizedBodyIndex(0) with a regular
+labeling such that children have higher indices than their parents. Ground
+does not have a mobilizer (or I suppose you could think of its mobilizer as
+the Weld joint that attaches it to the universe), but otherwise
+every mobilized body has a unique body and mobilizer. **/
+int getNumBodies() const;
+
+/** This is the total number of defined constraints, each of which may
+generate more than one constraint equation. This is the number of Constraint
+objects that were defined; in a given State some of these may be disabled. **/
+int getNumConstraints() const;
+
+/** The sum of all the mobilizer degrees of freedom. This is also the length
+of the state variable vector u and the mobility forces array. **/
+int getNumMobilities() const;
+
+/** The sum of all the q vector allocations for each joint. There may be
+some that are not in use for particular modeling options. **/
+int getTotalQAlloc() const;
+
+/** This is the sum of all the allocations for constraint multipliers, one per
+acceleration constraint equation. There may be some that are not in use due
+to the corresonding Constraint elements being disabled in a given State. **/
+int getTotalMultAlloc() const;
+
+
+/** Attach new matter by attaching it to the indicated parent body (not
+normally called by users -- see MobilizedBody). The mobilizer and mass
+properties are provided by \a child. A new MobilizedBodyIndex is assigned for
+the child; it is guaranteed to be numerically larger than the
+MobilizedBodyIndex of the parent. We take over ownership of \a child's
+implementation object from the given MobilizedBody handle, leaving that handle
+as a reference to the implementation object now owned by the matter subsystem.
+It is an error if the given MobilizedBody handle wasn't the owner of the
+implementation object to which it refers.
+ at note
+This method is usually called by concrete MobilizedBody constructors;
+it does not normally need to be called by end users. **/
+MobilizedBodyIndex adoptMobilizedBody(MobilizedBodyIndex parent,
+ MobilizedBody& child);
+
+/** Add a new Constraint object to the matter subsystem (not normally called
+by users -- see Constraint). The details of the Constraint are opaque here.
+A new ConstraintIndex is assigned. We take over ownership of the
+implementation object from the given Constraint handle, leaving that handle as
+a reference to the implementation object now owned by the matter subsystem. It
+is an error if the given Constraint handle wasn't the owner of the
+implementation object to which it refers.
+ at note
+This method is usually called by concrete Constraint constructors; it does not
+normally need to be called by end users. **/
+ConstraintIndex adoptConstraint(Constraint&);
+
+/** Copy constructor is not very useful. **/
+SimbodyMatterSubsystem(const SimbodyMatterSubsystem& ss) : Subsystem(ss) {}
+/** Copy assignment is not very useful. **/
+SimbodyMatterSubsystem& operator=(const SimbodyMatterSubsystem& ss)
+{ Subsystem::operator=(ss); return *this; }
+
+
+//==============================================================================
+/** @name Set/get modeling options
+
+Methods in this section involve setting and getting various modeling options
+that may be selected. This includes whether to use quaternions or Euler angles
+to represent rotations, and enabling/disabling constraints. **/
+
+/**@{**/
+/** For all mobilizers offering unrestricted orientation, decide what
+method we should use to model their orientations. Choices are:
+quaternions (best for dynamics), or rotation angles (1-2-3 Euler
+sequence, good for optimization). Changing this flag invalidates Model
+stage and above in the supplied \a state, leaving it realized only through
+Topology stage, so you must call realizeModel() on the containing
+MultibodySystem prior to using this \a state in further calculations. **/
+void setUseEulerAngles(State& state, bool useEulerAngles) const;
+
+/** Return the current setting of the "use Euler angles" model variable as
+set in the supplied \a state. **/
+bool getUseEulerAngles (const State& state) const;
+
+/** Return the number of quaternions in use by the mobilizers of this system,
+given the current setting of the "use Euler angles" flag in the supplied
+\a state, and the types of mobilizers in the multibody tree.
+ at see isUsingQuaternion(), getQuaternionPoolIndex() **/
+int getNumQuaternionsInUse(const State& state) const;
+
+/** Check whether a given mobilizer is currently using quaternions, based
+on the type of mobilizer and the setting of the "use Euler angles" flag in
+the supplied \a state.
+ at see getNumQuaternionsInUse(), getQuaternionPoolIndex() **/
+bool isUsingQuaternion(const State& state, MobilizedBodyIndex mobodIx) const;
+/** If the given mobilizer is currently using a quaternion to represent
+orientation, return the QuaternionPoolIndex (a small integer) assigned to that
+quaternion. This is used, for example, to find which normalization constraint
+error is associated with which quaternion.
+ at see isUsingQuaternion(), getNumQuaternionsInUse() **/
+QuaternionPoolIndex getQuaternionPoolIndex(const State& state,
+ MobilizedBodyIndex mobodIx) const;
+
+/** Disable or enable the Constraint whose ConstraintIndex is supplied within
+the supplied \a state. Whether a Constraint is disabled is an Instance-stage
+state variable so enabling or disabling invalidates Instance stage and higher
+in the given \a state, leaving the \a state realized no higher than Model
+stage.
+ at see isConstraintDisabled() **/
+void setConstraintIsDisabled(State& state,
+ ConstraintIndex constraintIx,
+ bool shouldDisableConstraint) const;
+
+/** Determine whether a particular Constraint is currently disabled in the
+given \a state.
+ at see setConstraintIsDisabled() **/
+bool isConstraintDisabled(const State&, ConstraintIndex constraint) const;
+
+/** Given a State which may be modeled using quaternions, copy it to another
+State which represents the same configuration using Euler angles instead. If
+the \a inputState already uses Euler angles, the output will just be a
+duplicate. All continuous and discrete State variables will be copied to the
+\a outputState but they will not necessarily have been realized to the same
+level as the \a inputState. **/
+void convertToEulerAngles(const State& inputState, State& outputState) const;
+
+/** Given a State which may be modeled using Euler angles, copy it to another
+State which represents the same configuration using quaternions instead. If
+the \a inputState already uses quaternions, the output will just be a
+duplicate. All continuous and discrete State variables will be copied to the
+\a outputState but they will not necessarily have been realized to the same
+level as the \a inputState. **/
+void convertToQuaternions(const State& inputState, State& outputState) const;
+
+/** (Advanced) Given a State whose generalized coordinates q have been modified
+in some manner that doesn't necessarily keep quaternions normalized, fix them.
+Note that all of Simbody's integrators and solvers take care of this
+automatically so most users will never need to make this call.
+
+Since we are modifying q's here, Stage::Position is invalidated.
+ at param[in,out] state **/
+void normalizeQuaternions(State& state) const;
+
+/**@}**/
+
+
+//==============================================================================
+/** @name Calculate whole-system properties
+
+These methods perform calculations that yield properties of the system as
+a whole. These are \e operators, meaning that they make use of the supplied
+State but do not modify the State. They simply calculate a result and return
+it to you without storing it internally. Each method requires that the
+State has already been realized to at least a particular stage which is
+documented with the method. **/
+
+/**@{**/
+/** Calculate the total system mass.
+ at par Required stage
+ \c Stage::Instance **/
+Real calcSystemMass(const State& s) const;
+
+/** Return the position vector p_GC of the system mass center C, measured from
+the Ground origin, and expressed in Ground.
+ at par Required stage
+ \c Stage::Position **/
+Vec3 calcSystemMassCenterLocationInGround(const State& s) const;
+
+/** Return total system mass, mass center location measured from the Ground
+origin, and system inertia taken about the Ground origin, expressed in Ground.
+ at par Required stage
+ \c Stage::Position **/
+MassProperties calcSystemMassPropertiesInGround(const State& s) const;
+
+/** Return the system inertia matrix taken about the system center of mass,
+expressed in Ground.
+ at par Required stage
+ \c Stage::Position **/
+Inertia calcSystemCentralInertiaInGround(const State& s) const;
+
+/** Return the velocity v_GC = d/dt p_GC of the system mass center C in the
+Ground frame G, measured from Ground origin and expressed in G.
+ at par Required stage
+ \c Stage::Velocity **/
+Vec3 calcSystemMassCenterVelocityInGround(const State& s) const;
+
+/** Return the acceleration a_GC = d/dt p_GC of the system mass center C in the
+Ground frame G, measured from Ground origin and expressed in G.
+ at par Required stage
+ \c Stage::Acceleration **/
+Vec3 calcSystemMassCenterAccelerationInGround(const State& s) const;
+
+/** Return the momentum of the system as a whole (angular, linear) measured
+in the Ground frame, taken about the Ground origin and expressed in Ground.
+(The linear component is independent of the "about" point.)
+ at see calcSystemCentralMomentum()
+ at par Required stage
+ \c Stage::Velocity **/
+SpatialVec calcSystemMomentumAboutGroundOrigin(const State& s) const;
+
+/** Return the momentum of the system as a whole (angular, linear) measured
+in the Ground frame, taken about the current system center of mass
+location C and expressed in Ground. (The linear component is independent of the
+"about" point.)
+ at see calcSystemMomentumAboutGroundOrigin()
+ at par Required stage
+ \c Stage::Velocity **/
+SpatialVec calcSystemCentralMomentum(const State& s) const;
+
+/** Calculate the total kinetic energy of all the mobilized bodies in this
+matter subsystem, given the configuration and velocities in \a state.
+ at par Required stage
+ \c Stage::Velocity **/
+Real calcKineticEnergy(const State& state) const;
+/**@}**/
+
+//==============================================================================
+/** @name System and Task Space Kinematic Jacobian Operators
+
+The system kinematic Jacobian maps between mobility space (generalized speeds
+and generalized forces) and Cartesian body space (mobilized body frame spatial
+velocities and spatial forces). A task space Jacobian maps between mobility
+space and a specified set of task points or frames fixed to a subset of the
+bodies, and generally located away from the body frame. A task space Jacobian
+J can be used to construct various task space matrices such as the task space
+compliance matrix J M^-1 ~J or its inverse, the task space (or operational
+space) inertia matrix.
+
+The system Jacobian J(q) maps n generalized speeds u to spatial velocities V of
+each of the nb mobilized bodies (including Ground), measured at the body frame
+origin relative to Ground, and expressed in the Ground frame. The transpose ~J
+of this matrix maps nb spatial forces to n generalized forces, where the spatial
+forces are applied at the body frame origin and expressed in Ground. Similarly,
+task space Jacobians map from n generalized speeds to nt task frame spatial
+velocities (expressed in Ground), and transposed task space Jacobians map
+between task frame spatial forces (or impulses), expressed in Ground, and
+generalized forces (or generalized impulses).
+
+Simbody provides fast O(n) methods ("operators") that can form matrix-vector
+products like J*u or ~J*F without forming J. The "bias" term Jdot*u (also known
+as the Coriolis acceleration) is also available; this arises when working at
+the acceleration level because d/dt J*u = J*udot+Jdot*u (where dot means time
+derivative). The computational cost of these operators is O(n+nt) so it is
+\e much more efficient to work with a group of tasks simultaneously than to
+process one at a time, which would have complexity O(n*nt). Alternatively, we
+provide methods that will return all or part of J explicitly; in general it
+is \e much more efficient computationally to work with the O(n) matrix-vector
+multiply operators rather than to form explicit matrices and then perform O(n^2)
+matrix-vector products. Performance estimates are given with each method so that
+you can determine which methods to use. If you can, you should use the O(n)
+methods -- it is a good habit to get into when using an O(n) multibody code like
+Simbody!
+
+Note that the Jacobian is associated with an expressed-in frame for the
+velocity or force vector and a designated station (point) on each body. We
+always use the Ground frame for Jacobians. For the system Jacobian, the body
+origin is always the designated station; for task Jacobians different stations
+may be specified. We provide three different sets of methods for working with
+ - the full %System Jacobian: J, nb X n 6-vectors (or 6*nb X n scalars)
+ - the Station Jacobian for a set of nt task stations (points): JS, nt rows
+ of n 3-vectors (or a 3*nt X n Matrix of scalars)
+ - the Frame Jacobian for a set of nt task frames fixed to a body: JF, nt
+ rows of n 6-vectors (or a 6*nt X n Matrix of scalars)
+
+The rotational part of a Jacobian is the same for any frame fixed to the same
+body. So for Frame Jacobians you need specify only a station on the body (the
+frame's origin point). That means if you want a 3*nt X n Orientation Jacobian,
+you can obtain it from alternate rows of a Frame Jacobian. Using the above
+terminology, the complete %System Jacobian is a Frame Jacobian for which the
+task frames are the body frames, with each MobilizedBody appearing only once
+and in order of MobilizedBodyIndex (starting with Ground).
+
+It is acceptable for the same body to appear more than once in a list of tasks;
+these are likely to conflict but that can be dealt with elsewhere. **/
+
+/**@{**/
+
+/** Calculate the product of the %System kinematic Jacobian J (also known as the
+partial velocity matrix) and a mobility-space vector u in O(n) time. If the
+vector u is a set of generalized speeds, then this produces the body spatial
+velocities that result from those generalized speeds. That is, the result is
+V_GB = J*u where V_GB[i] is the spatial velocity of the i'th body's body frame
+origin (in Ground) that results from the given set of generalized speeds.
+
+ at param[in] state
+ A State compatible with this System that has already been realized to
+ Stage::Position.
+ at param[in] u
+ A mobility-space Vector, such as a set of generalized speeds. The length
+ and order must match the mobilities of this system (that is n, the number
+ of generalized speeds u, \e not nq, the number of generalized
+ coordinates q).
+ at param[out] Ju
+ This is the product V=J*u as described above. Each element is a spatial
+ vector, one per mobilized body, to be indexed by MobilizedBodyIndex.
+ If the input vector is a set of generalized speeds u, then the results
+ are nb spatial velocities V_GBi (that is, a pair of vectors w_GBi and v_GBi
+ giving angular and linear velocity). Note that Ground is body 0 so the 0th
+ element V_GB0=V_GG=Ju[0] is always zero on return.
+
+The kinematic Jacobian (partial velocity matrix) J is defined as follows:
+<pre>
+ partial(V) T T
+ J = ----------, V = [V_GB0 V_GB1 ... V_GB nb-1] , u = [u0 u1 ... u n-1]
+ partial(u)
+</pre>
+Thus the element J(i,j)=partial(V_GBi)/partial(uj) (each element of J is a
+spatial vector). The transpose of this matrix maps spatial forces to
+generalized forces; see multiplyBySystemJacobianTranspose().
+
+Note that we're using "monogram" notation for the spatial velocities, where
+<pre>
+ G Bi
+ V_GBi = V
+</pre>
+the spatial velocity of body i's body frame Bi (at its origin), measured and
+expressed in the Ground frame G.
+
+<h3>Performance discussion</h3>
+This is a very fast operator, costing about 12*(nb+n) flops, where nb is the
+number of bodies and n the number of mobilities (degrees of freedom) u. In
+contrast, even if you have already calculated the entire nbXnX6 matrix J, the
+multiplication J*u would cost 12*nb*n flops. As an example, for a 20 body
+system with a free flying base and 19 pin joints (25 dofs altogether), this
+method takes 12*(20+25)=540 flops while the explicit matrix-vector multiply
+would take 12*20*25=6000 flops. So this method is already >10X faster for
+that small system; for larger systems the difference grows rapidly.
+
+ at see multiplyBySystemJacobianTranspose(), calcSystemJacobian() **/
+void multiplyBySystemJacobian( const State& state,
+ const Vector& u,
+ Vector_<SpatialVec>& Ju) const;
+
+/** Calculate the acceleration bias term for the %System Jacobian, that is, the
+part of the acceleration that is due only to velocities. This term is also
+known as the Coriolis acceleration, and it is returned here as a spatial
+acceleration of each body frame in Ground.
+
+ at param[in] state
+ A State that has already been realized through Velocity stage.
+ at param[out] JDotu
+ The product JDot*u where JDot = d/dt J, and u is the vector of generalized
+ speeds taken from \a state. This is a Vector of nb SpatialVec elements.
+
+<h3>Theory</h3>
+The spatial velocity V_GBi of each body i can be obtained from the generalized
+speeds u by V = {V_GBi} = J*u. Taking the time derivative in G gives
+<pre>
+ A = d/dt V = {A_GBi} = J*udot + JDot*u
+</pre>
+where JDot=JDot(q,u). This method returns JDot*u, which depends only on
+configuration q and speeds u. Note that the same u is used to calculate JDot,
+which is linear in u, so this term is quadratic in u.
+
+<h3>Implementation</h3>
+This method simply extracts the total Coriolis acceleration for each body that
+is already available in the \a state cache so there is no computation done
+here.
+ at see getTotalCoriolisAcceleration()
+**/
+void calcBiasForSystemJacobian(const State& state,
+ Vector_<SpatialVec>& JDotu) const;
+
+
+/** Alternate signature that returns the bias as a 6*nb-vector of scalars
+rather than as an nb-vector of 2x3 spatial vectors. See the other signature for
+documentation. **/
+void calcBiasForSystemJacobian(const State& state,
+ Vector& JDotu) const;
+
+/** Calculate the product of the transposed kinematic Jacobian ~J (==J^T) and
+a vector F_G of spatial force-like elements, one per body, in O(n) time to
+produce a generalized force-like result f=~J*F. If F_G is actually a set of
+spatial forces applied at the body frame origin of each body, and expressed
+in the Ground frame, then the result is the equivalent set of generalized
+forces f that would produce the same accelerations as F_G.
+
+ at param[in] state
+ A State compatible with this System that has already been realized to
+ Stage::Position.
+ at param[in] F_G
+ This is a vector of SpatialVec elements, one per mobilized body and in
+ order of MobilizedBodyIndex (with the 0th entry a force on Ground; hence
+ ignored). Each SpatialVec is a spatial force-like pair of 3-vectors
+ (torque,force) with the force applied at the body origin and the vectors
+ expressed in Ground.
+ at param[out] f
+ This is the product f=~J*F_G as described above. This result is in the
+ generalized force space, that is, it has one scalar entry for each of the
+ n system mobilities (velocity degrees of freedom). Resized if necessary.
+
+The kinematic Jacobian (partial velocity matrix) J is defined as follows:
+<pre>
+ partial(V) T T
+ J = ----------, V = [V_GB0 V_GB1 ... V_GB nb-1] , u = [u0 u1 ... u n-1]
+ partial(u)
+</pre>
+Thus the element J(i,j)=partial(V_GBi)/partial(uj) (each element of J is a
+spatial vector). J maps generalized speeds to spatial velocities (see
+multiplyBySystemJacobian()); its transpose ~J maps spatial forces
+to generalized forces.
+
+Note that we're using "monogram" notation for the spatial velocities, where
+<pre>
+ G Bi
+ V_GBi = V
+</pre>
+the spatial velocity of body i's body frame Bi (at its origin), measured and
+expressed in the Ground frame G.
+
+<h3>Performance discussion</h3>
+This is a very fast operator, costing about 18*nb+11*n flops, where nb is the
+number of bodies and n the number of mobilities (degrees of freedom) u. In
+contrast, even if you have already calculated the entire 6*nbXnu matrix J, the
+multiplication ~J*F would cost 12*nb*n flops. As an example, for a 20 body
+system with a free flying base and 19 pin joints (25 dofs altogether), this
+method takes 18*20+11*25=635 flops while the explicit matrix-vector multiply
+would take 12*20*25=6000 flops. So this method is already >9X faster for
+that small system; for larger systems the difference grows rapidly.
+
+ at see multiplyBySystemJacobian(), calcSystemJacobian() **/
+void multiplyBySystemJacobianTranspose( const State& state,
+ const Vector_<SpatialVec>& F_G,
+ Vector& f) const;
+
+
+/** Explicitly calculate and return the nb x nu whole-system kinematic
+Jacobian J_G, with each element a 2x3 spatial vector (SpatialVec). This matrix
+maps generalized speeds to the spatial velocities of all the bodies, which
+will be at the body origins, measured and expressed
+in Ground. That is, if you have a set of n generalized speeds u, you can
+find the spatial velocities of all nb bodies as V_G = J_G*u. The transpose of
+this matrix maps a set of spatial forces F_G, applied at the body frame
+origins and expressed in Ground, to the equivalent set of n generalized
+forces f: f = ~J_G*F_G.
+
+ at note The 0th row of the returned Jacobian is always zero since it represents
+the spatial velocity of Ground.
+
+<h3>Performance discussion</h3>
+Before using this method, consider whether you really need to form this
+very large matrix which necessarily will take O(n^2) space and time; it will
+almost always be \e much faster to use the multiplyBySystemJacobian() method
+that directly calculate the matrix-vector product in O(n) time without explictly
+forming the matrix. Here are the details:
+
+As currently implemented, forming the full Jacobian J costs about
+12*n*(nb+n) flops. Assuming nb ~= n, this is about 24*n^2 flops. Then
+if you want to form a product J*u explicitly, the matrix-vector multiply will
+cost about 12*n^2 flops each time you do it. In contrast the J*u product is
+calculated using multiplyBySystemJacobian() in about 24*n flops. Even for
+very small systems it is cheaper to make repeated calls to
+multiplyBySystemJacobian() than to form J explicitly and multiply by it.
+See the Performance section for multiplyBySystemJacobian() for more
+comparisons.
+
+ at see multiplyBySystemJacobian(), multiplyBySystemJacobianTranspose()
+ at see calcSystemJacobian() alternate signature using scalar elements **/
+void calcSystemJacobian(const State& state,
+ Matrix_<SpatialVec>& J_G) const; // nb X nu
+
+/** Alternate signature that returns a system Jacobian as a 6*nb X n Matrix
+of scalars rather than as an nb X n matrix of 2x3 spatial vectors. See
+the other signature for documentation and important performance
+considerations. **/
+void calcSystemJacobian(const State& state,
+ Matrix& J_G) const; // 6 nb X nu
+
+
+/** Calculate the Cartesian ground-frame velocities of a set of task stations
+(points fixed on bodies) that results from a particular set of generalized
+speeds u. The result is the station velocities measured and expressed in Ground.
+
+ at param[in] state
+ A State that has already been realized through Position stage.
+ at param[in] onBodyB
+ An array of nt mobilized bodies (one per task) to which the stations of
+ interest are fixed.
+ at param[in] stationPInB
+ The array of nt station points P of interest (one per task), each
+ corresponding to one of the bodies B from \a onBodyB, given as vectors
+ from each body B's origin Bo to its station P, expressed in frame B.
+ at param[in] u
+ A mobility-space Vector, such as a set of generalized speeds. The length
+ and order must match the mobilities of this system (that is n, the number
+ of generalized speeds u, \e not nq, the number of generalized
+ coordinates q).
+ at param[out] JSu
+ The resulting product JS*u, where JS is the station task Jacobian. Resized
+ to nt if needed.
+
+<h3>Performance discussion</h3>
+It is almost always better to use this method than to form an explicit 3*nt X n
+station task Jacobian explicitly and then multiply by it. If you have only one
+or two tasks, so that the matrix is only 3xn or 6xn, and then perform many
+multiplies with that matrix, it might be slightly cheaper to form it. For
+example, it is about 4X cheaper to use this method than to form a one-task
+Station Jacobian JS explicitly and use it once. However, because this would be
+such a skinny matrix (3 X n) explicit multiplication is cheap so if you will
+re-use this same Jacobian repeatedly before recalculating (at least 6 times)
+then it may be worth calculating and saving it. Here are the details:
+
+A call to this method costs 27*nt + 12*(nb+n) flops. If you assume that
+nb ~= n >> 1, you could say this is about 27*nt + 24*n flops. In
+contrast, assuming you already have the 3*nt X n station Jacobian JS available,
+you can compute the JS*u product in about 6*nt*n flops, 3X faster for one task,
+about even for three tasks, and slower for more than three tasks.
+However forming JS costs about 40*nt+90*n flops (see calcStationJacobian()).
+So to form a one-task Jacobian and use it once is 4X more expensive (96*n vs
+24*n), but if you use it more than 5 times it is cheaper to do it
+explicitly. Forming a one-task JS and using it 100 times costs about 690*n
+flops while calling this method 100 times would cost about 2400*n flops.
+
+ at see multiplyByStationJacobianTranspose(), calcStationJacobian() **/
+void multiplyByStationJacobian(const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB,
+ const Array_<Vec3>& stationPInB,
+ const Vector& u,
+ Vector_<Vec3>& JSu) const;
+
+/** Alternate signature for when you just have a single station task.
+ at returns JS*u, where JS is the station task Jacobian. **/
+Vec3 multiplyByStationJacobian(const State& state,
+ MobilizedBodyIndex onBodyB,
+ const Vec3& stationPInB,
+ const Vector& u) const
+{
+ ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
+ ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
+ Vector_<Vec3> JSu(1);
+ multiplyByStationJacobian(state, bodies, stations, u, JSu);
+ return JSu[0];
+}
+
+
+/** Calculate the generalized forces resulting from a single force applied
+to a set of nt station tasks (points fixed to bodies) P. The applied forces
+f_GP should be 3-vectors expressed in Ground. This is considerably faster than
+forming the Jacobian explicitly and then performing the matrix-vector multiply.
+
+<h3>Performance discussion</h3>
+Cost is about 30*nt + 18*nb + 11*n. Assuming nb ~= n, this is roughly
+30*(n+nt). In contrast, forming the complete 3*nt X n matrix would cost about
+90*(n+nt/2), and subsequent explicit matrix-vector multiplies would cost
+about 6*nt*n each.
+
+ at see multiplyByStationJacobian(), calcStationJacobian() **/
+void multiplyByStationJacobianTranspose
+ (const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB,
+ const Array_<Vec3>& stationPInB,
+ const Vector_<Vec3>& f_GP,
+ Vector& f) const;
+
+/** Alternate signature for when you just have a single station task. **/
+void multiplyByStationJacobianTranspose
+ (const State& state,
+ MobilizedBodyIndex onBodyB,
+ const Vec3& stationPInB,
+ const Vec3& f_GP,
+ Vector& f) const
+{
+ ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
+ ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
+ Vector_<Vec3> forces(1, f_GP);
+ multiplyByStationJacobianTranspose(state, bodies, stations, forces, f);
+}
+
+/** Explicitly calculate and return the 3*nt x n kinematic Jacobian JS for a
+set of nt station tasks P (a station is a point fixed on a particular mobilized
+body). This matrix maps generalized speeds to the Cartesian velocity of each
+station, measured and expressed in Ground. That is, if you have a set of n
+generalized speeds u, you can find the Cartesian velocities of stations P as
+v_GP = JS*u, where v_GP is a 3*nt column vector. The transpose of this
+matrix maps a 3*nt vector of forces f_GP (expressed in Ground and applied
+to P) to the equivalent set of n generalized forces f: f = ~JS*f_GP.
+
+ at note It is almost always far more efficient to use multiplyByStationJacobian()
+or multiplyByStationJacobianTranspose() to form matrix-vector products rather
+than to use this method to form the Jacobian explicitly. See the performance
+discussions there.
+
+Overloaded signatures of this method are available to allow you to obtain the
+Jacobian either as an nt X n Matrix with Vec3 elements, or as 3*nt X n Matrix
+with scalar elements.
+
+ at param[in] state
+ A State that has already been realized through Position stage.
+ at param[in] onBodyB
+ An array of nt mobilized bodies (one per task) to which the stations of
+ interest are fixed.
+ at param[in] stationPInB
+ The array of nt station points P of interest (one per task), each
+ corresponding to one of the bodies B from \a onBodyB, given as vectors
+ from each body B's origin Bo to its station P, expressed in frame B.
+ at param[out] JS
+ The resulting nt X n station task Jacobian. Resized if necessary.
+
+<h3>Performance discussion</h3>
+The cost of a call to this method is about 42*nt + 54*nb + 33*n flops. If we
+assume that nb ~= n >> 1, this is roughly 90*(n+nt/2) flops. Then once the
+Station Jacobian JS has been formed, each JS*u matrix-vector product costs
+6*nt*n flops to form. When nt is small enough (say one or two tasks), and you
+plan to re-use it a lot, this can be computationally efficient; but for single
+use or more than a few tasks you can do much better with
+multiplyByStationJacobian() or multiplyByStationJacobianTranspose().
+
+ at see multiplyByStationJacobian(), multiplyByStationJacobianTranspose() **/
+void calcStationJacobian(const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB,
+ const Array_<Vec3>& stationPInB,
+ Matrix_<Vec3>& JS) const;
+
+/** Alternate signature for when you just have a single station task. **/
+void calcStationJacobian(const State& state,
+ MobilizedBodyIndex onBodyB,
+ const Vec3& stationPInB,
+ RowVector_<Vec3>& JS) const
+{
+ ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
+ ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
+ calcStationJacobian(state, bodies, stations, JS);
+}
+
+/** Alternate signature that returns a station Jacobian as a 3*nt x n Matrix
+rather than as a Matrix of Vec3 elements. See the other signature for
+documentation and important performance considerations. **/
+void calcStationJacobian(const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB,
+ const Array_<Vec3>& stationPInB,
+ Matrix& JS) const;
+
+/** Alternate signature for when you just have a single station task. **/
+void calcStationJacobian(const State& state,
+ MobilizedBodyIndex onBodyB,
+ const Vec3& stationPInB,
+ Matrix& JS) const
+{
+ ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
+ ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
+ calcStationJacobian(state, bodies, stations, JS);
+}
+
+
+/** Calculate the acceleration bias term for a station Jacobian, that is, the
+part of the station's acceleration that is due only to velocities. This term
+is also known as the Coriolis acceleration, and it is returned here as a linear
+acceleration of the station in Ground.
+
+ at param[in] state
+ A State that has already been realized through Velocity stage.
+ at param[in] onBodyB
+ An array of nt mobilized bodies (one per task) to which the stations of
+ interest are fixed.
+ at param[in] stationPInB
+ The array of nt station points P of interest (one per task), each
+ corresponding to one of the bodies B from \a onBodyB, given as vectors
+ from each body B's origin Bo to its station P, expressed in frame B.
+ at param[out] JSDotu
+ The resulting product JSDot*u, where JSDot is the time derivative of JS,
+ the station task Jacobian. Resized to nt if needed.
+
+<h3>Theory</h3>
+The velocity v_GP of a station point P in the Ground frame G can be obtained
+from the generalized speeds u using the station Jacobian for P, as <pre>
+ v_GP = JS_P*u
+</pre> Taking the time derivative in G gives <pre>
+ a_GP = JS_P*udot + JSDot_P*u
+</pre>
+This method returns JSDot_P*u, which depends only on configuration and
+velocities. We allow for a set of task points P so that all their bias terms
+can be calculated in a single sweep of the multibody tree. Note that u is taken
+from the \a state and that the same u shown above is also used to calculate
+JSDot_P, which is linear in u, so the bias term is quadratic in u.
+
+<h3>Implementation</h3>
+This method just obtains body B's total Coriolis acceleration already available
+in the \a state cache and shifts it to station point P. Cost is 48*nt flops.
+ at see getTotalCoriolisAcceleration(), shiftAccelerationBy()
+**/
+void calcBiasForStationJacobian(const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB,
+ const Array_<Vec3>& stationPInB,
+ Vector_<Vec3>& JSDotu) const;
+
+/** Alternate signature that returns the bias as a 3*nt-vector of scalars
+rather than as an nt-vector of Vec3s. See the other signature for
+documentation. **/
+void calcBiasForStationJacobian(const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB,
+ const Array_<Vec3>& stationPInB,
+ Vector& JSDotu) const;
+
+/** Alternate signature for when you just have a single station task.
+ at returns JSDot*u, where JSDot is the station Jacobian time derivative. **/
+Vec3 calcBiasForStationJacobian(const State& state,
+ MobilizedBodyIndex onBodyB,
+ const Vec3& stationPInB) const
+{
+ ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
+ ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
+ Vector_<Vec3> JSDotu(1);
+ calcBiasForStationJacobian(state, bodies, stations, JSDotu);
+ return JSDotu[0];
+}
+
+
+/** Calculate the spatial velocities of a set of nt task frames A={Ai} fixed to
+nt bodies B={Bi}, that result from a particular set of n generalized speeds u.
+
+The result is each task frame's angular and linear velocity measured and
+expressed in Ground. Using this method is considerably faster than forming the
+6*nt X n Frame Jacobian explicitly and then performing the matrix-vector
+multiply. See the performance analysis below for details.
+
+There is a simplified signature of this method available if you have only a
+single frame task.
+
+ at param[in] state
+ A State that has already been realized through Position stage.
+ at param[in] onBodyB
+ An array of nt mobilized bodies (one per task) to which the task frames of
+ interest are fixed. These may be in any order and the same body may appear
+ more than once if there are multiple task frames on it.
+ at param[in] originAoInB
+ An array of nt frame origin points Ao for the task frames interest (one
+ per task), each corresponding to one of the bodies B from \a onBodyB, given
+ as vectors from each body B's origin Bo to its task frame origin Ao,
+ expressed in frame B.
+ at param[in] u
+ A mobility-space Vector, such as a set of generalized speeds. The length
+ and order must match the mobilities of this system (that is n, the number
+ of generalized speeds u, \e not nq, the number of generalized
+ coordinates q).
+ at param[out] JFu
+ The resulting product JF*u, where JF is the frame task Jacobian. Resized
+ if needed to a Vector of nt SpatialVec entries.
+
+ at note All frames A fixed to a given body B have the same angular velocity so
+we do not actually need to know the task frames' orientations here, just the
+location on B of their origin points Ao. If you have a Transform X_BA giving
+the pose of frame A in the body frame B, you can extract the position vector
+for the origin point Ao using X_BA.p() and pass that as the \a originAoInB
+parameter here.
+
+<h3>Performance discussion</h3>
+A call to this method costs 27*nt + 12*(nb+n) flops. If you assume that
+nb ~= n >> 1, you could say this is about 25*(nt+n) flops. In contrast, assuming
+you already have the 6*nt X n Frame Jacobian JF available, you can compute the
+JF*u product in about 12*nt*n flops. If you have just one task (nt==1) this
+explicit multiplication is about twice as fast; at two tasks it is about even
+and for more than two it is more expensive. However forming JF costs about
+180*(n+nt/4) flops (see calcFrameJacobian()). So to form a one-task Jacobian
+and use it once is almost 8X more expensive (192*n vs 25*n), but if you use it
+more than 16 times it is (marginally) cheaper to do it explicitly (for one
+task). For example, forming a one-task JF and using it 100 times costs 1392*n
+flops while calling this method 100 times would cost about 2500*n flops.
+
+Conclusion: in almost all practical cases you are better off using this operator
+rather than forming JF, even if you have only a single frame task and certainly
+if you have more than two tasks.
+
+ at see multiplyByFrameJacobianTranspose(), calcFrameJacobian() **/
+void multiplyByFrameJacobian
+ (const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB,
+ const Array_<Vec3>& originAoInB,
+ const Vector& u,
+ Vector_<SpatialVec>& JFu) const;
+
+/** Simplified signature for when you just have a single frame task; see the
+main signature for documentation.
+ at returns JF*u, where JF is the single frame task Jacobian. **/
+SpatialVec multiplyByFrameJacobian( const State& state,
+ MobilizedBodyIndex onBodyB,
+ const Vec3& originAoInB,
+ const Vector& u) const
+{
+ ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
+ ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
+ Vector_<SpatialVec> JFu(1);
+ multiplyByFrameJacobian(state, bodies, stations, u, JFu);
+ return JFu[0];
+}
+
+
+/** Calculate the n generalized forces f resulting from a set of spatial forces
+(torque,force pairs) F applied at nt task frames Ai fixed to nt bodies Bi. The
+applied forces are spatial vectors (pairs of 3-vectors) expressed in Ground. Use
+of this O(n) method is considerably faster than forming the 6*nt X n Jacobian
+explicitly and then performing an O(n^2) matrix-vector multiply.
+
+ at param[in] state
+ A State that has already been realized through Position stage.
+ at param[in] onBodyB
+ An array of nt mobilized bodies (one per task) to which the task frames of
+ interest are fixed. These may be in any order and the same body may appear
+ more than once if there are multiple task frames on it.
+ at param[in] originAoInB
+ An array of nt frame origin points Ao for the task frames interest (one
+ per task), each corresponding to one of the bodies B from \a onBodyB, given
+ as vectors from each body B's origin Bo to its task frame origin Ao,
+ expressed in frame B.
+ at param[in] F_GAo
+ A Vector of nt spatial forces, each applied one of the task frames. These
+ are expressed in Ground.
+ at param[out] f
+ The Vector of n generalized forces that results from applying the forces
+ \a F_GAo to the task frames. Resized if necessary.
+
+<h3>Performance discussion</h3>
+A call to this method costs 33*nt + 18*nb + 11*n flops. If you assume that
+nb ~= n >> 1, you could say this is about 30*(n+nt) flops. In contrast, assuming
+you already have the 6*nt X n Frame Jacobian JF available, you can compute the
+~JF*F product in about 12*nt*n flops. For one or two tasks that would be faster
+than applying the operator. However forming JF costs about 180*(n+nt/4) flops
+(see calcFrameJacobian()). So to form even a one-task Frame Jacobian and use
+it once is about 6X more expensive than using the operator (192*n vs 30*n),
+but if you use it more than 10 times it is (marginally) cheaper to do it
+explicitly. For example, forming a one-task JF and using it 100 times costs
+around 1392*n flops while calling this method 100 times would cost about
+3000*n flops.
+
+Conclusion: in almost all practical cases you are better off using this operator
+rather than forming JF, even if you have only a single frame task and certainly
+if you have more than two tasks.
+
+ at see multiplyByFrameJacobian(), calcFrameJacobian() **/
+void multiplyByFrameJacobianTranspose
+ (const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB,
+ const Array_<Vec3>& originAoInB,
+ const Vector_<SpatialVec>& F_GAo,
+ Vector& f) const;
+
+/** Simplified signature for when you just have a single frame task. See the
+other signature for documentation. **/
+void multiplyByFrameJacobianTranspose( const State& state,
+ MobilizedBodyIndex onBodyB,
+ const Vec3& originAoInB,
+ const SpatialVec& F_GAo,
+ Vector& f) const
+{
+ ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
+ ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
+ const Vector_<SpatialVec> forces(1, F_GAo);
+ multiplyByFrameJacobianTranspose(state, bodies, stations, forces, f);
+}
+
+
+
+/** Explicitly calculate and return the 6*nt x n frame task Jacobian JF for a
+set of nt frame tasks A={Ai} fixed to nt bodies B={Bi}. This matrix maps
+generalized speeds to the Cartesian spatial velocity (angular and linear
+velocity) of each frame, measured and expressed in Ground. That is, if you have
+a set of n generalized speeds u, you can find the Cartesian spatial velocities
+of task frames A as V_GA = JF*u, where V_GA is a 6*nt column vector. The
+transpose of this matrix maps a 6*nt vector of spatial forces F_GA (expressed
+in Ground and applied to the origins of frames A) to the equivalent set of n
+generalized forces f: f = ~JF*F_GA.
+
+ at note It is almost always far more efficient to use multiplyByFrameJacobian() or
+multiplyByFrameJacobianTranspose() to form matrix-vector products rather than to
+use this method to form the Jacobian explicitly. See the performance discussion
+there.
+
+Overloaded signatures of this method are available to allow you to obtain the
+Jacobian either as an nt X n Matrix with SpatialVec elements, or as 6*nt X n
+Matrix with scalar elements.
+
+ at param[in] state
+ A State that has already been realized through Position stage.
+ at param[in] onBodyB
+ An array of nt mobilized bodies (one per task) to which the task frames of
+ interest are fixed. These may be in any order and the same body may appear
+ more than once if there are multiple task frames on it.
+ at param[in] originAoInB
+ An array of nt frame origin points Ao for the task frames of interest (one
+ per task), each corresponding to one of the bodies B from \a onBodyB, given
+ as vectors from each body B's origin Bo to its task frame origin Ao,
+ expressed in frame B.
+ at param[out] JF
+ The resulting nt X n frame task Jacobian, with each element a SpatialVec.
+ Resized if necessary.
+
+<h3>Performance discussion</h3>
+The cost of a call to this method is about 42*nt + 108*nb + 66*n flops. If we
+assume that nb ~= n >> 1, this is roughly 180*(n+nt/4) flops. Then once the
+Frame Jacobian JF has been formed, each JF*u matrix-vector product costs about
+12*nt*n flops to form. When nt is small enough (say one or two tasks), and you
+plan to re-use it a lot, this can be computationally efficient; but for single
+use or more than a few tasks you can do much better with
+multiplyByFrameJacobian() or multiplyByFrameJacobianTranspose().
+
+ at see multiplyByFrameJacobian(), multiplyByFrameJacobianTranspose() **/
+void calcFrameJacobian(const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB,
+ const Array_<Vec3>& originAoInB,
+ Matrix_<SpatialVec>& JF) const;
+
+/** Simplified signature for when you just have a single frame task. See the
+other signature for documentation. **/
+void calcFrameJacobian(const State& state,
+ MobilizedBodyIndex onBodyB,
+ const Vec3& originAoInB,
+ RowVector_<SpatialVec>& JF) const
+{
+ ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
+ ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
+ calcFrameJacobian(state, bodies, stations, JF);
+}
+
+/** Alternate signature that returns a frame Jacobian as a 6*nt X n Matrix
+rather than as an nt X n Matrix of SpatialVecs. See the other signature for
+documentation and important performance considerations.**/
+void calcFrameJacobian(const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB,
+ const Array_<Vec3>& originAoInB,
+ Matrix& JF) const;
+
+/** Simplified signature for when you just have a single frame task. See the
+other signature for documentation. **/
+void calcFrameJacobian(const State& state,
+ MobilizedBodyIndex onBodyB,
+ const Vec3& originAoInB,
+ Matrix& JF) const
+{
+ ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
+ ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
+ calcFrameJacobian(state, bodies, stations, JF);
+}
+
+/** Calculate the acceleration bias term for a task frame Jacobian, that is, the
+parts of the frames' accelerations that are due only to velocities. This term
+is also known as the Coriolis acceleration, and it is returned here as spatial
+accelerations of the frames in Ground.
+
+There is a simplified signature of this method available if you have only a
+single frame task.
+
+ at param[in] state
+ A State that has already been realized through Velocity stage.
+ at param[in] onBodyB
+ An array of nt mobilized bodies (one per task) to which the task frames of
+ interest are fixed. These may be in any order and the same body may appear
+ more than once if there are multiple task frames on it.
+ at param[in] originAoInB
+ An array of nt frame origin points Ao for the task frames interest (one
+ per task), each corresponding to one of the bodies B from \a onBodyB, given
+ as vectors from each body B's origin Bo to its task frame origin Ao,
+ expressed in frame B.
+ at param[out] JFDotu
+ The result JFDot*u, where JF is the task frame Jacobian and JFDot its
+ time derivative, and u is the set of generalized speeds taken from the
+ the supplied \a state.
+
+<h3>Theory</h3>
+The spatial velocity V_GA of frame A can be obtained from the generalized
+speeds u using the frame Jacobian for A, as V_GA = JF*u. Taking the time
+derivative in G gives
+<pre>
+ A_GA = JF*udot + JFDot*u
+</pre>
+This method returns JFDot*u, which depends only on configuration and
+velocities. Note that the same u is used to calculate JFDot, which is linear
+in u, so the term JFDot*u is quadratic in u.
+
+<h3>Implementation</h3>
+This method just obtains body B's total Coriolis acceleration already available
+in the \a state cache and shifts it to the A frame's origin Ao, for each of the
+nt task frames. Cost is 48*nt flops.
+
+ at see getTotalCoriolisAcceleration(), shiftAccelerationBy()
+**/
+void calcBiasForFrameJacobian
+ (const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB,
+ const Array_<Vec3>& originAoInB,
+ Vector_<SpatialVec>& JFDotu) const;
+
+/** Alternate signature that returns the bias as a 6*nt-vector of scalars
+rather than as an nt-vector of SpatialVec elements. See the other signature for
+documentation. **/
+void calcBiasForFrameJacobian(const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB,
+ const Array_<Vec3>& originAoInB,
+ Vector& JFDotu) const;
+
+/** Simplified signature for when you just have a single frame task.
+ at returns JFDot*u, where JFDot is the frame task Jacobian time derivative and
+u the generalized speeds taken from \a state. **/
+SpatialVec calcBiasForFrameJacobian(const State& state,
+ MobilizedBodyIndex onBodyB,
+ const Vec3& originAoInB) const
+{
+ ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
+ ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
+ Vector_<SpatialVec> JFDotu(1);
+ calcBiasForFrameJacobian(state, bodies, stations, JFDotu);
+ return JFDotu[0];
+}
+
+/**@}**/
+
+//==============================================================================
+/** @name System matrix manipulation
+The documentation for the SimbodyMatterSubsystem describes the system equations
+in matrix notion, although internal computations are generally matrix-free.
+The operators in this section provide the ability to perform fast operations
+that can be described in terms of those matrices (e.g., multiply by the mass
+matrix) but are actually done using O(n), matrix-free algorithms. There are
+also routines here for obtaining the matrices explicitly, although working with
+explicit matrices should be avoided whenever performance is an issue.
+
+The mass matrix M and constraint matrix G are the most significant. G=[P;V;A]
+is composed of submatrices P for position (holonomic), V for velocity
+(nonholonomic), and A for acceleration-only constraints. These matrices are
+sometimes needed separately. Also, these matrices are all in mobility space
+(generalized speeds u). When qdot != u, the matrix N in the equation
+qdot = N*u becomes important and operators for working with it efficiently
+are also provided here. In that case, the position constraint matrix
+in generalized coordinate q space, Pq, can also be accessed. (In terms of
+the other matrices, Pq=P*N^-1.) **/
+/**@{**/
+
+/** This operator calculates in O(n) time the product M*v where M is the
+system mass matrix and v is a supplied mobility-space vector (that is, it has
+one entry for each of the n mobilities). If v is a set of mobility accelerations
+(generalized accelerations udot), then the result is a generalized force
+(f=M*udot). Only the supplied vector is used, and M depends only on position
+states, so the result here is not affected by velocities in the State.
+Constraints and prescribed motions are ignored.
+
+The current implementation requires about 120*n flops and does not require
+realization of composite-body or articulated-body inertias.
+ at par Required stage
+ \c Stage::Position **/
+void multiplyByM(const State& state, const Vector& a, Vector& Ma) const;
+
+/** This operator calculates in O(n) time the product M^-1*v where M is the
+system mass matrix and v is a supplied vector with one entry per u-space
+mobility. If v is a set of generalized forces f, the result is a generalized
+acceleration (udot=M^-1*f). Only the supplied vector is used, and M depends
+only on position states, so the result here is not affected by velocities in
+\a state. In particular, you'll have to obtain your own inertial forces and
+put them in f if you want them included.
+
+ at param[in] state
+ This is a State that has been realized through Position stage, from which
+ the current system configuration and articulated body inertias are
+ obtained. If necessary, the articulated body inertias will be realized in
+ the state the first time this is called. They will then be retained in the
+ \a state cache for speed.
+ at param[in] v
+ This is a generalized-force like vector in mobility space (u-space). If
+ there is any prescribed motion specified using Motion objects or mobilizer
+ locking (see below), then only the entries of v corresponding to
+ non-prescribed mobilities are examined by this method; the prescribed ones
+ are not referenced at all.
+ at param[out] MinvV
+ This is the result M^-1*v. If there is any prescribed motion specified
+ using Motion objects or mobilizer locks (see below), then only the
+ non-prescribed entries in MinvV are calculated; the prescribed ones are set
+ to zero.
+
+<h3>Behavior with prescribed motion</h3>
+If you prescribe the motion of one or more mobilizers using Motion objects or
+mobilizer locking, the behavior of this method is altered. (This does \e not
+apply if you use Constraint objects to specify the motion.) With prescribed
+motion enabled, this method works only with the free (non-prescribed)
+mobilities. Only the entries in \a v corresponding to free mobilities are
+examined, and only the entries in the result \a MinvV corresponding to free
+mobilities are calculated; the others are set to zero.
+
+<h3>Theory</h3>
+View the unconstrained, prescribed zero-velocity equations of motion
+M udot + tau = f as partitioned into "free" and "prescribed" variables like
+this:
+<pre>
+ [M_ff ~M_fp] [udot_f] [ 0 ] [f_f]
+ [ ] [ ] + [ ] = [ ]
+ [M_fp M_pp] [udot_p] [tau] [f_p]
+</pre>
+The free and prescribed variables have been grouped here for clarity but
+in general they are interspersed among the columns and rows of M.
+
+Given that decomposition, this method returns
+<pre>
+ [udot_f] [udot_f] [M_ff^-1 0 ][f_f]
+ [ ] = [ ] = [ ][ ]
+ [udot_p] [ 0 ] [ 0 0 ][f_p]
+</pre>
+When there is no prescribed motion M_ff is the entire mass matrix, and the
+result is udot_f=udot=M^-1*f. When there is prescribed motion, M_ff is a
+submatrix of M, and the result is the nf elements of udot_f, with udot_p=0.
+
+<h3>Implementation</h3>
+This is a stripped-down version of forward dynamics. It requires the hybrid
+free/prescribed articulated body inertias to have been realized and will
+initiate that calculation if necessary the first time it is called for a given
+configuration q. The M^-1*f calculation requires two sweeps of the multibody
+tree, an inward sweep to accumulate forces, followed by an outward sweep to
+propagate accelerations.
+
+<h3>Performance</h3>
+If the supplied State does not already contain realized values for the
+articulated body inertias, then they will be realized when this operator is
+first called for a new set of positions. Calculating articulated body inertias
+is O(n) but relatively expensive. Once the appropriate articulated body
+inertias are available, repeated calls to this operator are very fast, with
+worst case around 80*n flops when all mobilizers have 1 dof. If you want to
+force realization of the articulated body inertias, call the method
+realizeArticulatedBodyInertias().
+
+ at par Required stage
+ \c Stage::Position
+
+ at see multiplyByM(), calcMInv(), realizeArticulatedBodyInertias() **/
+void multiplyByMInv(const State& state,
+ const Vector& v,
+ Vector& MinvV) const;
+
+/** This operator explicitly calculates the n X n mass matrix M. Note that this
+is inherently an O(n^2) operation since the mass matrix has n^2 elements
+(although only n(n+1)/2 are unique due to symmetry). <em>DO NOT USE THIS CALL
+DURING NORMAL DYNAMICS</em>. To do so would change an O(n) operation into an
+O(n^2) one. Instead, see if you can accomplish what you need with O(n) operators
+like multiplyByM() which calculates the matrix-vector product M*v in O(n)
+without explicitly forming M. Also, don't invert this matrix numerically to get
+M^-1. Instead, call the method calcMInv() which can produce M^-1 directly.
+ at see multiplyByM(), calcMInv() **/
+void calcM(const State&, Matrix& M) const;
+
+/** This operator explicitly calculates the inverse of the part of the system
+mobility-space mass matrix corresponding to free (non-prescribed)
+mobilities. The returned matrix is always n X n, but rows and columns
+corresponding to prescribed mobilities are zero. This is an O(n^2) operation,
+which is of course within a constant factor of optimal for returning a matrix
+with n^2 elements explicitly. (There are actually only n(n+1)/2 unique elements
+since the matrix is symmetric.) <em>DO NOT USE THIS CALL DURING NORMAL
+DYNAMICS</em>. To do so would change an O(n) operation into an O(n^2) one.
+Instead, see if you can accomplish what you need with O(n) operators like
+multiplyByMInv() which calculates the matrix-vector product M^-1*v in O(n)
+without explicitly forming M or M^-1. If you need M explicitly, you can get it
+with the calcM() method.
+ at see multiplyByMInv(), calcM() **/
+void calcMInv(const State&, Matrix& MInv) const;
+
+/** This operator calculates in O(m*n) time the m X m "projected inverse mass
+matrix" or "constraint compliance matrix" W=G*M^-1*~G, where G (mXn) is the
+acceleration-level constraint Jacobian mapped to generalized coordinates,
+and M (nXn) is the unconstrained system mass matrix. In case there is prescribed
+motion specified with Motion objects or mobilizer locking, M^-1 here is really
+M_ff^-1, that is, it is restricted to the free (non-prescribed) mobilities, but
+scattered into a full n X n matrix (conceptually). See multiplyByMInv() and
+calcMInv() for more information.
+
+W is the projection of the inverse mass matrix into the constraint coordinate
+space (that is, the vector space of the multipliers lambda). It can be used to
+solve for the constraint forces that will eliminate a given constraint
+acceleration error:
+<pre>
+ (1) W * lambda = aerr
+ (2) aerr = G*udot - b(t,q,u)
+</pre>
+where udot is an unconstrained generalized acceleration. Note that you can
+view equation (1) as a dynamic system in a reduced set of m generalized
+coordinates, with the caveat that W may be singular.
+
+In general W is singular and does not uniquely determine lambda. Simbody
+normally calculates a least squares solution for lambda so that loads are
+distributed among redundant constraints.
+
+ at note If you just need to multiply W by a vector or matrix, you do not need
+to form W explicitly. Instead you can use the method described in the
+Implementation section to produce a W*v product in the O(n) time it takes to
+compute a single column of W.
+
+<h3>Implementation</h3>
+We are able to form W without forming G or M^-1 and without performing any
+matrix-matrix multiplies. Instead, W is calculated using m applications of
+O(n) operators:
+ - multiplyByGTranspose() by a unit vector to form a column of ~G
+ - multiplyByMInv() to form a column of M^-1 ~G
+ - multiplyByG() to form a column of W
+
+Even if G and M^-1 were already available, computing W by matrix multiplication
+would cost O(m^2*n + m*n^2) time and O(m*n) intermediate storage. Here we do
+it in O(m*n) time with O(n) intermediate storage, which is a \e lot better.
+
+ at see multiplyByG(), calcG(), multiplyByGTranspose(), calcGTranspose()
+ at see multiplyByMInv(), calcMInv() **/
+void calcProjectedMInv(const State& s,
+ Matrix& GMInvGt) const;
+
+/** Given a set of desired constraint-space speed changes, calculate the
+corresponding constraint-space impulses that would cause those changes. Here we
+are solving the equation
+<pre>
+ W * impulse = deltaV
+</pre>
+for \e impulse, where W=G*M^-1*~G is the "projected inverse mass matrix" as
+described for calcProjectedMInv(). In general W is singular due to constraint
+redundancies, so the solution for \e impulse is not unique. Simbody handles
+redundant constraints by finding least squares solutions, and this operator
+method duplicates the method Simbody uses for determining the rank and
+performing the factorization of W.
+
+ at param[in] state
+ The State whose generalized coordinates and speeds define the matrix W.
+ Must already be realized to Velocity stage.
+ at param[in] deltaV
+ The set of desired velocity changes to be produced by the impulse, in
+ constraint space. These will consist of observed velocity constraint
+ violations (-verr) and constraint violations that would be generated by
+ impulsive applied forces (-G*M^-1*f).
+ at param[out] impulse
+ The set of constraint multiplier-space impulses that will produce the
+ desired velocity changes without violating the constraints.
+
+To convert these constraint-space impulses into updates to the mobility-space
+generalized speeds u, use code like this:
+ at code
+ const SimbodyMatterSubsystem& matter=...;
+ Vector deltaV=...; // constraint space speed change desired; length m
+ Vector impulse; // constraint space impulses; length m
+ solveForConstraintImpulses(state, deltaV, impulse);
+ Vector f; // mobility space impulses; length n
+ Vector du; // change to generalized speeds u; length n
+ matter.multiplyByGTranspose(s,impulse,f);
+ matter.multiplyByMInv(s,f,du);
+ state.updU() += du; // update generalized speeds
+ at endcode
+
+Note that the length of the constraint-space vectors is m=mp+mv+ma, the total
+number of acceleration-level constraints including the second time derivatives
+of the position (holonomic) constraints, the first time derivatives of the
+velocity (nonholonomic) constraints, and the acceleration-only constraints.
+ at see calcProjectedMInv(), multiplyByGTranspose(), multiplyByMInv() **/
+void solveForConstraintImpulses(const State& state,
+ const Vector& deltaV,
+ Vector& impulse) const;
+
+
+/** Returns Gulike = G*ulike, the product of the mXn acceleration
+constraint Jacobian G and a "u-like" (mobility space) vector of length n.
+m is the number of active acceleration-level constraint equations, n is the
+number of mobilities. This is an O(m+n) operation.
+
+If you are going to call this method repeatedly at the same time, positions and
+velocities, you should precalculate the bias term once and supply it to the
+alternate signature of this method. See the Implementation section for more
+information.
+
+ at pre \a state realized to Velocity stage
+ at par Implementation
+This is accomplished by treating the input vector \a ulike as though it were
+a set of generalized accelerations (for nonholonomic and acceleration-only
+constraints) or generalized speeds (for holonomic constraints). These are
+mapped to body accelerations (or velocities) in O(n) time. See
+calcBodyAccelerationFromUDot() for more information (converting from
+generalized speeds to velocities is just multiplying by the System Jacobian).
+The method calcBiasForMultiplyByG() is used to determine the state-dependent
+term of the constraint error equations. Then a second call is made to
+evaluate the bias term aerr(t,q,u;0)=-b(t,q,u). We then calculate
+Gulike = aerr(t,q,u;ulike)-aerr(t,q,u;0) in O(m) time.
+ at see calcBiasForMultiplyByG() **/
+void multiplyByG(const State& state,
+ const Vector& ulike,
+ Vector& Gulike) const {
+ Vector bias;
+ calcBiasForMultiplyByG(state, bias);
+ multiplyByG(state, ulike, bias, Gulike);
+}
+
+
+/** Multiply Gulike=G*ulike using the supplied precalculated bias vector to
+improve performance (approximately 2X) over the other signature.
+ at see calcBiasForMultiplyByG() **/
+void multiplyByG(const State& state,
+ const Vector& ulike,
+ const Vector& bias,
+ Vector& Gulike) const;
+
+/** Calculate the bias vector needed for the higher-performance signature of
+the multiplyByG() method above.
+
+ at param[in] state
+ Provides time t, positions q, and speeds u; must be realized through
+ Velocity stage so that all body spatial velocities are known.
+ at param[out] bias
+ This is the bias vector for use in repeated calls to multiplyByG(). It
+ will be resized if necessary to length m=mp+mv+ma, the total number of
+ active acceleration-level constraint equations.
+
+ at pre \a state realized to Velocity stage
+ at par Implementation
+This method uses either velocity- or acceleration- level constraint error
+functions with zero input to determine the bias term for use in
+multiplyByG(). Body quantities and generalized quantities are supplied to each
+of the m active constraints' (constant time) error methods to calculate
+<pre>
+ pverr(t,q,u;ulike)=G*ulike - c(t,q) (holonomic)
+or aerr(t,q,u;ulike)=G*ulike - b(t,q,u) (nonholonomic or acceleration-only)
+</pre>
+with ulike=0, giving the bias term in O(m) time.
+
+If you want the acceleration-level bias terms b for all the constraints, even
+if they are holonomic, use calcBiasForAccelerationConstraints(). **/
+void calcBiasForMultiplyByG(const State& state,
+ Vector& bias) const;
+
+/** This O(m*n) operator explicitly calculates the m X n acceleration-level
+constraint Jacobian G which appears in the system equations of
+motion. Consider using the multiplyByG() method instead of this one,
+which forms the matrix-vector product G*v in O(m+n) time without explicitly
+forming G.
+
+ at par Implementation
+This method generates G columnwise using repeated calls to multiplyByG(),
+which makes use of the constraint error methods to perform a G*v product
+in O(m+n) time. To within numerical error, for non-working constraints
+this should be identical to the transpose of the matrix returned by calcGt()
+which uses the constraint force methods instead.
+ at see multiplyByG(), calcGt(), calcPq() **/
+void calcG(const State& state, Matrix& G) const;
+
+
+/** Calculate the acceleration constraint bias vector, that is, the terms in
+the acceleration constraints that are independent of the accelerations.
+
+ at param[in] state
+ Provides time t, positions q, and speeds u; must be realized through
+ Velocity stage so that all body spatial velocities are known.
+ at param[out] bias
+ This is the bias vector for all the acceleration constraint equations
+ together. It will be resized if necessary to length m=mp+mv+ma, the total
+ number of active acceleration-level constraint equations.
+
+ at pre \a state realized to Velocity stage
+ at par Implementation
+We have constant-time constraint acceleration error methods
+<pre>
+paerr(t,q,u;udot)=P*udot - b_p(t,q,u)
+vaerr(t,q,u;udot)=V*udot - b_v(t,q,u)
+ aerr(t,q,u;udot)=A*udot - b_a(t,q,u)
+</pre>
+that together define the acceleration constraint equation G*udot-b=0
+where G=[P;V;A] and b=[b_p b_v b_a]. There is one of these error functions
+for each %Constraint, with paerr() the twice-differentiated position (holonomic)
+constraints, vaerr() the once-differentiated velocity (nonholonomic)
+constraints, and aerr() the acceleration-only constraints. This method
+sets \c udot = 0 and invokes each of those methods to obtain
+bias = -[b_p b_v b_a].
+
+<h3>Performance note</h3>
+The actual acceleration constraint functions require both udot and body
+accelerations for the constrained bodies; even with udot==0 body accelerations
+may have a non-zero velocity-dependent component (the coriolis accelerations).
+Those are already available in the state, but only as accelerations in Ground.
+For constraints that have a non-Ground Ancestor, we have to convert the
+accelerations to A at a cost of 105 flops/constrained body. **/
+void calcBiasForAccelerationConstraints(const State& state,
+ Vector& bias) const;
+
+
+/** Returns f = ~G*lambda, the product of the n X m transpose of the
+acceleration constraint Jacobian G (=[P;V;A]) and a multiplier-like vector
+\a lambda of length m, returning a generalized-force like quantity \a f of
+length n. m=mp+mv+ma is the total number of active constraint equations,
+n (==nu) is the number of mobilities (generalized speeds u). If lambda is a set
+of constraint multipliers, then f=~G*lambda is the set of forces generated by
+the constraints, mapped into generalized forces. This is an O(m+n) operation.
+
+Because the velocity (non-holonomic) or acceleration-only constraint Jacobians
+V and A can have velocity dependence, the \a state supplied here must generally
+be realized through Velocity stage. If the system has only position (holonomic)
+constraints then the \a state need be realized only through Position stage.
+
+ at param[in] state
+ A State that has been realized through Velocity stage (or Position stage
+ if the system has only position constraints). Time, configuration,
+ and velocities if needed are taken from \a state.
+ at param[in] lambda
+ A multiplier-like vector to be multiplied by ~G. Its length must be the
+ same as the total number of active constraint equations m.
+ at param[out] f
+ This is the generalized force-like output. It will be resized if necessary
+ to length equal to the number of mobilities (generalized speeds) n (==nu).
+
+ at par Implementation
+This is accomplished by treating the input vector \a lambda as though it were
+a set of Lagrange multipliers, then calling each of the active Constraints'
+(constant time) force generation methods, providing the appropriate subset of
+the multipliers each time. That gives body forces F0 and mobility forces f0 in
+O(m) time. We then use the equivalent of multiplyBySystemJacobianTranspose()
+to convert the returned body spatial forces to generalized forces in O(n)
+time, and finally return the generalized force-like result f = ~J*F0 + f0.
+ at see multiplyByG(), multiplyBySystemJacobianTranspose() **/
+void multiplyByGTranspose(const State& state,
+ const Vector& lambda,
+ Vector& f) const;
+
+/** This O(nm) operator explicitly calculates the n X m transpose of the
+acceleration-level constraint Jacobian G = [P;V;A] which appears in the system
+equations of motion. This method generates ~G columnwise use the constraint
+force generating methods which map constraint multipliers to constraint forces.
+To within numerical error, this should be identical to the transpose of
+the matrix returned by calcG() which uses a different method. Consider using
+the multiplyByGTranspose() method instead of this one, which forms the
+matrix-vector product ~G*v in O(n) time without explicitly forming ~G.
+ at par Required stage
+ \c Stage::Velocity
+ at see calcG(), multiplyByGTranspose() **/
+void calcGTranspose(const State&, Matrix& Gt) const;
+
+
+/** Calculate in O(n) time the product Pq*qlike where Pq is the mp X nq
+position (holonomic) constraint Jacobian and \a qlike is a "q-like"
+(generalized coordinate space) vector of length nq. Here mp is the number of
+active position-level constraint equations in this system.
+
+If you are going to call this method repeatedly at the same time t and
+configuration q and want maximum efficiency, you can gain a factor of almost
+2X by precalculating a bias term once using calcBiasForMultiplyByPq() and
+supplying it to the alternate signature of this method. See the Theory
+section below for an explanation of the bias term.
+
+ at pre \a state realized to Position stage
+
+<h3>Theory</h3>
+Simbody's position (holonomic) constraints are defined by the constraint
+error equation
+<pre>
+ (1) perr(t;q) = p(t,q)
+</pre>
+where we try to maintain perr=0 at all times. We also have available time
+derivatives of equation (1); the first time derivative is relevant here:
+<pre>
+ (2) pverr(t,q;qdot) = dperr/dt = Pq * qdot + Pt
+</pre>
+where Pq=Dperr/Dq and Pt=Dperr/Dt (capital "D" means partial derivative).
+Pt=Pt(t,q) is called the "bias" term. (Note that because u=N^-1*qdot we also
+have Pq=P*N^-1, where P=Dpverr/Du is the very useful mobility-space holonomic
+constraint Jacobian.) Eq. (2) can be used to perform efficient multiplication
+by Pq, since it can be used to calculate Pq*qlike+Pt, and a second evaluation
+at qlike=0 can be used to calculate the unwanted bias term for removal:
+<pre>
+ (3) Pq*qlike = pverr(t,q;qlike) - pverr(t,q;0)
+</pre>
+Despite appearances, eq. (2) calculates its result in constant time per
+constraint equation, for a total cost that is O(n) or more strictly O(mp+nq).
+The matrix Pq is never actually formed; instead the matrix-vector product
+is calculated directly.
+
+<h3>Implementation</h3>
+We treat the input vector \a qlike as though it were a set of generalized
+coordinate derivatives qdot. These are mapped to body velocities V in O(n)
+time, using V=Jq*qdot, where Jq is the coordinate space system Jacobian
+(partial velocity matrix), with Jq=J*N^-1. Then the body velocities and qdots
+are supplied to each of the mp active position constraints' (constant time)
+velocity error methods to get pverr(t,q;qlike)=Pq*qlike-Pt in O(n) time. A
+second call is made to evaluate the bias term pverr(t,q;0)=-Pt. We then
+calculate the result \a PqXqlike = pverr(t,q;qlike)-pverr(t,q;0) in O(n) time
+using equation (3).
+
+ at see calcBiasForMultiplyByPq() **/
+void multiplyByPq(const State& state,
+ const Vector& qlike,
+ Vector& PqXqlike) const {
+ Vector biasp;
+ calcBiasForMultiplyByPq(state, biasp);
+ multiplyByPq(state, qlike, biasp, PqXqlike);
+}
+
+
+/** Multiply Pq*qlike using the supplied precalculated bias vector to
+improve performance (approximately 2X) over the other signature.
+ at see calcBiasForMultiplyByPq() **/
+void multiplyByPq(const State& state,
+ const Vector& qlike,
+ const Vector& biasp,
+ Vector& PqXqlike) const;
+
+/** Calculate the bias vector needed for the higher-performance signature of
+the multiplyByPq() method above.
+
+ at param[in] state
+ Provides time t, and positions q; must be realized through
+ Position stage so that all body spatial poses are known.
+ at param[out] biasp
+ This is the bias vector for use in repeated calls to multiplyByPq(). It
+ will be resized if necessary to length mp, the total number of
+ active position-level (holonomic) constraint equations.
+
+ at pre \a state realized to Position stage
+
+See multiplyByPq() for theory and implementation; this method is just
+performing the qlike=0 case described there for calculating the bias term Pt.
+**/
+void calcBiasForMultiplyByPq(const State& state,
+ Vector& biasp) const;
+
+/** This O(m*n) operator explicitly calculates the mp X nq position-level
+(holonomic) constraint Jacobian Pq (=P*N^-1), the partial derivative of the
+position error equations with respect to q. Consider using the multiplyByPq()
+method instead of this one, which forms the matrix-vector product Pq*v in
+O(m+n) time without explicitly forming Pq.
+
+Note that quaternion normalization constraints are \e not included in mp; we
+do not consider those holonomic constraints.
+
+ at pre \a state realized to Position stage
+
+ at param[in] state
+ A State realized through Position stage so that time and the pose
+ (configuration) of each body is known.
+ at param[out] Pq
+ The position constraint Jacobian Dperr/Dq. This will be resized to
+ mp X nq if necessary.
+
+ at par Implementation
+This method generates Pq columnwise using repeated calls to multiplyByPq(),
+which makes use of the position constraint velocity-level error methods to
+perrform a Pq*v product in O(m+n) time. See multiplyByPq() for a more
+detailed explanation. If Pq's columns are in contiguous memory we'll work
+in place, otherwise columns are generated into a contiguous temporary and
+then copied into Pq.
+
+ at see multiplyByPq() **/
+void calcPq(const State& state, Matrix& Pq) const;
+
+
+/** Returns f = ~Pq*lambdap, the product of the n X mp transpose of the
+position (holonomic) constraint Jacobian Pq (=P*N^-1) and a multiplier-like
+vector \a lambdap of length mp, returning a generalized-force like quantity
+\a f of length n. mp is the number of active position constraint equations,
+n (==nu) is the number of mobilities (generalized speeds u). If lambdap is a set
+of mp constraint multipliers, then f=~G*lambdap is the set of forces generated
+by the position constraints, mapped into generalized forces. This is an
+O(mp+n) operation.
+
+A holonomic constraint Jacobian cannot have a velocity dependence, so the
+\a state need be realized only to Position stage here.
+
+ at param[in] state
+ A State that has been realized through Position stage. Time and
+ configuration are taken from \a state.
+ at param[in] lambdap
+ A multiplier-like vector to be multiplied by ~Pq. Its length must be the
+ same as the number of active position constraint equations mp.
+ at param[out] f
+ This is the generalized force-like output. It will be resized if necessary
+ to length equal to the number of mobilities (generalized speeds) n (==nu).
+
+ at par Implementation
+This is accomplished by treating the input vector \a lambdap as though it were
+a set of Lagrange multipliers, then calling each of the active holonomic
+Constraints' (constant time) force generation methods, providing the
+appropriate subset of the multipliers each time. That gives body forces F0 and
+mobility forces f0 in O(mp) time. We then use the equivalent of
+multiplyBySystemJacobianTranspose() to convert the returned body spatial
+forces to generalized forces in O(n) time, and finally return the generalized
+force-like result f = ~J*F0 + f0.
+ at see multiplyByPq(), multiplyBySystemJacobianTranspose() **/
+void multiplyByPqTranspose(const State& state,
+ const Vector& lambdap,
+ Vector& f) const;
+
+/** This O(m*n) operator explicitly calculates the nq X mp transpose of
+the position-level (holonomic) constraint Jacobian Pq (=P*N^-1), the partial
+derivative of the position error equations with respect to q. Consider using
+the multiplyByPqTranspose() method instead of this one, which forms the
+matrix-vector product ~Pq*v in O(m+n) time without explicitly forming ~Pq.
+
+Note that quaternion normalization constraints are \e not included in mp; we
+do not consider those holonomic constraints.
+
+ at pre \a state realized to Position stage
+
+ at param[in] state
+ A State realized through Position stage so that time and the pose
+ (configuration) of each body is known.
+ at param[out] Pqt
+ The transposed position constraint Jacobian ~Pq=(Dperr/Dq)^T. This will be
+ resized to nq X mp if necessary.
+
+ at par Implementation
+This method generates \a Pqt columnwise using repeated calls to
+multiplyByPqTranspose(), which makes use of the position constraint force
+generating methods to perform a ~Pq*v product in O(m+n) time. See
+multiplyByPqTranspose() for a more detailed explanation. If Pqt's columns
+are in contiguous memory we'll work in place, otherwise columns are generated
+into a contiguous temporary and then copied into Pqt.
+
+ at see multiplyByPqTranspose() **/
+void calcPqTranspose(const State& state, Matrix& Pqt) const;
+
+/** Returns the mp X nu matrix P which is the Jacobian of the first time
+derivative of the holonomic (position) constraint errors with respect to the
+generalized speeds u; that is, P = partial( dperr/dt )/partial(u). Here mp is
+the number of holonomic constraint equations (not including quaternion
+normalization constraints) and nu is the total number of generalized speeds as
+found in the supplied State. P is resized if necessary; an error will be thrown
+if the Matrix is not the right size and not resizeable.
+
+ at pre \a state is realized to Position stage
+ at par Complexity:
+Calculates the m X n matrix in O(m*n) time, which is good if you really need
+this matrix. However, in many cases what is really needed is the product
+of this matrix with a vector which can be done in O(n) time; consider whether
+you really need the whole matrix explicitly.
+ at par Required stage
+ \c Stage::Position **/
+void calcP(const State& state, Matrix& P) const;
+
+/** Returns the nu X mp matrix ~P - see calcP() for a description.
+ at par Required stage
+ \c Stage::Position **/
+void calcPt(const State& state, Matrix& Pt) const;
+
+
+/** Calculate out_q = N(q)*in_u (like qdot=N*u) or out_u = ~N*in_q. Note that
+one of "in" and "out" is always "q-like" while the other is "u-like", but which
+is which changes if the matrix is transposed. Note that the transposed
+operation here is the same as multiplying by N on the right, with the Vectors
+viewed as RowVectors instead. This is an O(n) operator since N is block
+diagonal.
+ at par Required stage
+ \c Stage::Position **/
+void multiplyByN(const State& s, bool transpose,
+ const Vector& in, Vector& out) const;
+
+/** Calculate out_u = NInv(q)*in_q (like u=NInv*qdot) or out_q = ~NInv*in_u.
+Note that one of "in" and "out" is always "q-like" while the other is "u-like",
+but which is which changes if the matrix is transposed. Note that the
+transposed operation here is the same as multiplying by NInv on the right,
+with the Vectors viewed as RowVectors instead. This is an O(N) operator since
+NInv is block diagonal. The configuration q is taken from the supplied state.
+ at par Required stage
+ \c Stage::Position **/
+void multiplyByNInv(const State& s, bool transpose,
+ const Vector& in, Vector& out) const;
+
+/** Calculate out_q = NDot(q,u)*in_u or out_u = ~NDot(q,u)*in_q. This is used,
+for example, as part of the conversion between udot and qdotdot. Note that one
+of "in" and "out" is always "q-like" while the other is "u-like", but which is
+which changes if the matrix is transposed. Note that the transposed operation
+here is the same as multiplying by NDot on the right, with the Vectors viewed
+as RowVectors instead. This is an O(N) operator since NDot is block diagonal.
+Configuration q and generalized speeds u are taken from the supplied state.
+ at par Required stage
+ \c Stage::Velocity **/
+void multiplyByNDot(const State& s, bool transpose,
+ const Vector& in, Vector& out) const;
+
+/**@}**/
+
+
+//==============================================================================
+/** @name Miscellaneous Operators
+
+Operators make use of the State but do not write their results back
+into the State, not even into the State cache. **/
+/**@{**/
+
+/** This is the primary forward dynamics operator. It takes a state which
+has been realized to the Dynamics stage, a complete set of forces to apply,
+and returns the accelerations that result. Only the forces supplied here,
+and those calculated internally from prescribed motion, constraints, and
+centrifugal effects, affect the results. Acceleration constraints are
+always satisfied on return as long as the constraints are consistent.
+If the position and velocity constraints aren't already satisified in the
+State, results are harder to interpret physically, but they will still be
+calculated and the acceleration constraints will still be satisfied. No
+attempt will be made to satisfy position and velocity constraints, or to
+set prescribed positions and velocities, nor even to check whether these
+are satisfied; position and velocity constraint and prescribed positions
+and velocities are simply irrelevant here.
+
+Given applied forces f_applied, this operator solves this set of equations:
+<pre>
+ M udot + tau + ~G lambda + f_inertial = f_applied (1)
+ G udot = b (2)
+ udot_p = udot_p(t,q,u,z) (3)
+</pre>
+where udot={udot_f,udot_p}, tau={0,tau_p}. The unknowns are: the free
+generalized accelerations udot_f, the constraint multipliers lambda, and the
+prescribed motion generalized forces tau_p. A subset udot_p of udot may have
+been prescribed as a known function of state via Motion objects or locks
+associated with the mobilized bodies. On return all the entries in udot will
+have been set to their calculated or prescribed values, and body spatial
+accelerations A_GB (that is, measured and expressed in Ground) are also
+returned. Lambda and tau_p are necessarily calculated but are not returned here.
+
+f_applied is the set of generalized (mobility) forces equivalent to the
+\a appliedMobilityForces and \a appliedBodyForces arguments supplied here.
+That is,
+<pre>
+ f_applied = appliedMobilityForces + ~J * appliedBodyForces
+</pre>
+where J is the system Jacobian mapping between spatial and generalized
+coordinates. Typically these forces will have been calculated as a function of
+state so we will have f_applied(t,q,u,z).
+
+M(t,q), G(t,q,u), and b(t,q,u) are defined by the mobilized bodies and
+constraints present in the system. f_inertial(q,u) includes the
+velocity-dependent gyroscopic and coriolis forces due to rigid body
+rotations and is extracted internally from the already-realized state.
+
+Note that this method does not allow you to specify your own prescribed udots;
+those are calculated from the mobilizers' state-dependent Motion specifications
+(or are zero due to mobilizer locks) that are already part of the system.
+
+This is an O(n*m + m^3) operator where n is the number of generalized speeds
+and m the number of constraint equations (mobilities with prescribed motion are
+counted in n, not m).
+
+ at par Required stage
+ \c Stage::Dynamics **/
+void calcAcceleration
+ (const State& state,
+ const Vector& appliedMobilityForces,
+ const Vector_<SpatialVec>& appliedBodyForces,
+ Vector& udot, // output only; no prescribed motions
+ Vector_<SpatialVec>& A_GB) const;
+
+/** This operator is similar to calcAcceleration() but ignores the effects of
+acceleration constraints although it obeys prescribed accelerations. The
+supplied forces, prescribed motion forces, and velocity-induced centrifugal
+and gyroscopic effects are properly accounted for, but any forces that would
+have resulted from enforcing the contraints are not present. This operator
+solves the equations
+<pre>
+ M udot + tau + f_inertial = f_applied (1)
+ udot_p = udot_p(t,q,u,z) (2)
+</pre>
+where udot={udot_f,udot_p}, tau={0,tau_p}. The unknowns are the free
+generalized accelerations udot_f and the prescribed motion generalized forces
+tau_p. f_inertial contains the velocity-dependent gyroscopic and coriolis
+forces due to rigid body rotations. No constraint forces are included.
+
+On return all the entries in udot will have been set to their calculated or
+prescribed values, and body spatial accelerations A_GB (that is, measured and
+expressed in Ground) are also returned. tau_p is not returned.
+
+This is an O(n) operator.
+
+ at par Required stage
+ \c Stage::Dynamics **/
+void calcAccelerationIgnoringConstraints
+ (const State& state,
+ const Vector& appliedMobilityForces,
+ const Vector_<SpatialVec>& appliedBodyForces,
+ Vector& udot,
+ Vector_<SpatialVec>& A_GB) const;
+
+
+
+/** This is the inverse dynamics operator for the tree system; if there are
+any constraints or prescribed motion they are ignored. This method solves
+<pre>
+ f_residual = M udot + f_inertial - f_applied
+</pre>
+for f_residual in O(n) time, meaning that the mass matrix M is never formed.
+Inverse dynamics is considerably faster than forward dynamics, even though
+both are O(n) in Simbody.
+
+In the above equation we solve for the residual forces \c f_residual given
+desired accelerations and (optionally) a set of applied forces. Here
+\c f_applied is the mobility-space equivalent of all the applied forces
+(including mobility and body forces), \c f_inertial is the mobility-space
+equivalent of the velocity-dependent inertial forces due to rigid
+body rotations (coriolis and gyroscopic forces), and \c udot is the
+given set of values for the desired generalized accelerations. The returned
+\c f_residual is the additional generalized force (that is, mobility
+force) that would have to be applied at each mobility to give the desired
+\c udot. The inertial forces depend on the velocities \c u already realized
+in the State. Otherwise, only the explicitly-supplied forces affect the
+results of this operator; any forces that may be present elsewhere in
+the system are ignored.
+
+ at param[in] state
+ A State valid for the containing System, already realized to
+ Stage::Velocity.
+ at param[in] appliedMobilityForces
+ One scalar generalized force applied per mobility. Can be zero
+ length if there are no mobility forces; otherwise must have exactly
+ one entry per mobility in the matter subsystem.
+ at param[in] appliedBodyForces
+ One spatial force for each body. A spatial force is a force applied
+ to the body origin and a torque on the body, each expressed in the
+ Ground frame. Gravity, if present, is specified here as a body force.
+ The supplied Vector must be either zero length (interpreted as all-zero)
+ or have exactly one entry per body in the matter subsystem, starting with
+ Ground as body zero.
+ at param[in] knownUdot
+ These are the desired generalized accelerations, one per mobility.
+ If this is zero length it will be treated as all-zero; otherwise
+ it must have exactly one entry per mobility in the matter subsystem.
+ at param[out] residualMobilityForces
+ These are the residual generalized forces which, if added to the applied
+ forces, would produce the given \a knownUdot in forward dynamics (assuming
+ the system is unconstrained). This will be resized if necessary to have
+ length nu; that is, one scalar entry per mobility. You can view this as a
+ measure of how much the given \a knownUdot fails to satisfy the equations
+ of motion.
+
+ at par Required stage
+ \c Stage::Velocity
+
+ at see calcResidualForce(), multiplyByM()
+ at see calcAcceleration(), calcAccelerationIgnoringConstraints() **/
+void calcResidualForceIgnoringConstraints
+ (const State& state,
+ const Vector& appliedMobilityForces,
+ const Vector_<SpatialVec>& appliedBodyForces,
+ const Vector& knownUdot,
+ Vector& residualMobilityForces) const;
+
+
+/** This is the inverse dynamics operator for when you know both the
+accelerations and Lagrange multipliers for a constrained system. Prescribed
+motion is ignored. Using position and velocity from the given state, a set of
+applied forces, and a known set of generalized accelerations udot and
+constraint multipliers lambda, it calculates the additional generalized forces
+that would be required to satisfy Newton's 2nd law, f=Ma. That is, this
+operator returns
+<pre>
+ f_residual = M udot + ~G lambda + f_inertial - f_applied
+</pre>
+where f_applied is the mobility-space equivalent to all the applied forces
+(including mobility and body forces), f_inertial is the mobility-space
+equivalent of the velocity-dependent inertial forces due to rigid body
+rotations (coriolis and gyroscopic forces), and the udots and lambdas are given
+values of the generalized accelerations and constraint multipliers, resp.
+
+Note that there is no requirement that the given udots satisfy the constraint
+equations; we simply solve the above equation for \c f_residual.
+
+The inertial forces depend on the velocities \c u already realized in the State.
+Otherwise, only the explicitly-supplied forces affect the results of this
+operator; any forces that may be defined elsewhere in the system are ignored
+here.
+
+ at param[in] state
+ A State valid for the containing System, already realized to
+ \c Stage::Velocity.
+ at param[in] appliedMobilityForces
+ One scalar generalized force applied per mobility. Can be zero
+ length if there are no mobility forces; otherwise must have exactly
+ one entry per mobility in the matter subsystem.
+ at param[in] appliedBodyForces
+ One spatial force for each body. A spatial force is a force applied
+ to the body origin and a torque on the body, each expressed in the
+ Ground frame. Gravity, if present, is specified here as a body force.
+ The supplied Vector must be either zero length (interpreted as all-zero)
+ or have exactly one entry per body in the matter subsystem, starting with
+ Ground as body zero.
+ at param[in] knownUdot
+ These are the specified generalized accelerations, one per mobility so
+ the length should be nu. If this is zero length it will be treated as
+ all-zero of length nu; otherwise it must have exactly one entry per
+ mobility in the matter subsystem.
+ at param[in] knownLambda
+ These are the specified Lagrange multipliers, one per constraint
+ equation. If this is zero length it will be treated as all-zero; otherwise
+ it must have exactly m entries, where m=mp+mv+ma is the total number of
+ position, velocity, and acceleration-only constraints. There are no
+ entries here corresponding to quaternion constraints, which do not
+ generate forces.
+ at param[out] residualMobilityForces
+ These are the residual generalized forces which, if added to the applied
+ forces along with the constraint forces ~G*lambda, would produce the
+ given \a knownUdot in unconstrained forward dynamics. This will be resized
+ if necessary to have length nu; that is, one scalar entry per mobility.
+ You can view this as a measure of how much the given udot and lambda fail
+ to satisfy the equations of motion.
+
+ at par Required stage
+ \c Stage::Velocity
+
+ at see calcResidualForceIgnoringConstraints() **/
+void calcResidualForce
+ (const State& state,
+ const Vector& appliedMobilityForces,
+ const Vector_<SpatialVec>& appliedBodyForces,
+ const Vector& knownUdot,
+ const Vector& knownLambda,
+ Vector& residualMobilityForces) const;
+
+
+/** This operator calculates the composite body inertias R given a State
+realized to Position stage. Composite body inertias are the spatial mass
+properties of the rigid body formed by a particular body and all bodies
+outboard of that body as if all the outboard mobilizers were welded in their
+current orientations.
+
+This is a very fast O(n) operator.
+
+ at par Required stage
+ \c Stage::Position **/
+void calcCompositeBodyInertias(const State& state,
+ Array_<SpatialInertia,MobilizedBodyIndex>& R) const;
+
+
+
+/** Given a complete set of n generalized accelerations udot, this kinematic
+operator calculates in O(n) time the resulting body accelerations, including
+velocity-dependent terms taken from the supplied \a state.
+
+ at pre \a state must already be realized to Velocity stage
+ at param[in] state
+ The State from which position- and velocity- related terms are taken;
+ must already have been realized to Velocity stage.
+ at param[in] knownUDot
+ A complete set of generalized accelerations. Must have the same length
+ as the number of mobilities nu, or if length zero the udots will be taken
+ as all zero in which case only velocity-dependent (Coriolis) accelerations
+ will be returned in \a A_GB.
+ at param[out] A_GB
+ Spatial accelerations of all the body frames measured and expressed in
+ the Ground frame, resulting from supplied generalized accelerations
+ \a knownUDot and velocity-dependent acceleration terms taken from
+ \a state. This will be resized if necessary to the number of bodies
+ <em>including</em> Ground so that the returned array may be indexed by
+ MobilizedBodyIndex with A_GB[0]==0 always. The angular acceleration
+ vector for MobilizedBody i is A_GB[i][0]; linear acceleration of the
+ body's origin is A_GB[i][1].
+
+ at par Theory
+The generalized speeds u and spatial velocities V are related by the system
+Jacobian J as V=J*u. Thus the spatial accelerations A=Vdot=J*udot+Jdot*u.
+
+ at par Implementation
+The Coriolis accelerations Jdot*u are already available in a State realized
+to Velocity stage. The J*udot term is equivalent to an application of
+multiplyBySystemJacobian() to the \a knownUdot vector. The current
+implementation uses 12*nu + 18*nb flops to produce nb body accelerations.
+
+ at par Required stage
+ \c Stage::Velocity
+
+ at see multiplyBySystemJacobian(), getTotalCoriolisAcceleration() **/
+void calcBodyAccelerationFromUDot(const State& state,
+ const Vector& knownUDot,
+ Vector_<SpatialVec>& A_GB) const;
+
+
+/** Treating all Constraints together, given a comprehensive set of m
+Lagrange multipliers \e lambda, generate the complete set of body spatial forces
+and mobility (generalized) forces applied by all the Constraints.
+
+Spatial forces are applied at each body's origin and the moment and force
+vectors therein are expressed in the Ground frame. Watch the
+sign -- normally constraint forces have opposite sign from applied forces,
+because our equations of motion are
+ <pre> M udot + ~G lambda = f_applied </pre>
+If you want to take Simbody-calculated multipliers and use them to generate
+forces that look like applied forces, negate the multipliers in the argument
+passed to this call.
+
+State must be realized to Stage::Velocity to call this operator (although
+typically the multipliers are obtained by realizing to Stage::Acceleration).
+
+This is an O(m) operator. In particular it does \e not involve forming or
+multiplying by the constraint force matrix ~G. Instead, one constant-time call
+is made to each %Constraint's calcConstraintForce methods.
+
+ at par Required stage
+ \c Stage::Velocity **/
+void calcConstraintForcesFromMultipliers
+ (const State& state,
+ const Vector& multipliers,
+ Vector_<SpatialVec>& bodyForcesInG,
+ Vector& mobilityForces) const;
+
+/** Calculate the mobilizer reaction force generated at each MobilizedBody,
+as felt at the mobilizer's outboard frame M, and expressed in Ground.
+
+ at param[in] state
+ A State compatible with this System that has already been realized
+ to Stage::Acceleration.
+ at param[out] forcesAtMInG
+ A Vector of spatial force vectors, indexed by MobilizedBodyIndex
+ (beginning with 0 for Ground), giving the reaction moment and force
+ applied by each body's unique inboard mobilizer to that body. The
+ force is returned as though it were applied at the origin of the
+ body's mobilizer frame M. The returned force is expressed in the
+ Ground frame. Applied mobility (generalized) forces are \e included in the
+ returned reaction forces.
+
+A simple way to think of the reaction force is to think of cutting the
+mobilizer, then imagine the force required to make the system move in
+the same manner as when the mobilizer was present. This is what the
+reaction forces accomplish. With that definition, mobility forces (that is,
+generalized forces as opposed to body forces) are \e included in the reactions.
+Some conventions do not include the mobility forces in the definition of a
+reaction force. We chose to include them since this preserves Newton's
+3rd law of equal and opposite reactions between bodies. Ours is the same
+convention as used in SD/FAST.
+
+ at note You can think of the Ground body being welded to the universe at the
+Ground origin. The reactions reported for Ground are the ones that would
+occur in that Weld mobilizer if it were really present. That is, it includes
+the effects of all the base bodies on Ground.
+
+<h3>How to find the reaction felt by the parent body</h3>
+
+A mobilizer connects a frame F fixed on the parent (inboard) body P to a
+frame M fixed on the child (outboard) body B. It exerts equal and opposite
+reaction forces on the two bodies, at a given location in space. This method
+reports the force on the child body, as though it were applied at the origin
+Mo of frame M, and expressed in the Ground frame. The force on the parent body
+<em>at Mo</em> is just the negative of the returned value. However, it is
+more likely that you would want it as felt <em>at Fo</em>, the origin of the
+F frame on the parent. Here is one way to calculate that from the returned
+quantities:
+ at code
+ matter.calcMobilizerReactionForces(state,forcesAtMInG); // This method.
+ const int nb = matter.getNumBodies();
+ Vector_<SpatialVec> forcesAtFInG(nb); // to hold the result
+ forcesAtFInG[0] = -forcesAtMInG[0]; // Ground is "welded" at origin
+ for (MobilizedBodyIndex i(1); i < nb; ++i) {
+ const MobilizedBody& body = matter.getMobilizedBody(i);
+ const MobilizedBody& parent = body.getParentMobilizedBody();
+ // Want to shift reaction by p_MF, the vector from M to F across the
+ // mobilizer, and negate. Can get p_FM; must reexpress in G.
+ const Vec3& p_FM = body.getMobilizerTransform(state).p();
+ const Rotation& R_PF = body.getInboardFrame(state).R(); // In parent.
+ const Rotation& R_GP = parent.getBodyTransform(state).R();
+ Rotation R_GF = R_GP*R_PF; // F frame orientation in Ground.
+ Vec3 p_MF_G = -(R_GF*p_FM); // Re-express and negate shift vector.
+ forcesAtFInG[i] = -shiftForceBy(forcesAtMInG[i], p_MF_G);
+ }
+ at endcode
+
+<h3>Implementation</h3>
+This method combines already-calculated quantities to calculate the reactions.
+See Abhi Jain's 2011 book "Robot and Multibody Dynamics", Eq. 7.34 page 128:
+<pre> F_reaction = PPlus*APlus + zPlus </pre>
+where P is the articulated body inertia, A is the spatial acceleration,
+a the Coriolis acceleration and z the articulated body forces, and "Plus"
+indicates that we evaluate these on the inboard (parent) side of the mobilizer
+rather than on the body's side. (The alternative P(A-a)+z given there does not
+work for prescribed mobilizers unless you replace "a" with "a_underscore" from
+equation 16.14.) After calculating F_reaction at the body frame
+origin Bo, we shift it to M for reporting.
+
+<h3>Performance</h3>
+The cost of the above calculation is 114 flops/body. The code presented
+above for converting from M to F costs an additional 81 flops/body if you
+use it.
+
+ at par Required stage
+ \c Stage::Acceleration
+
+ at see SimTK::MobilizedBody::findMobilizerReactionOnBodyAtMInGround()
+ at see calcMobilizerReactionForcesUsingFreebodyMethod() **/
+void calcMobilizerReactionForces
+ (const State& state,
+ Vector_<SpatialVec>& forcesAtMInG) const;
+
+/** Return a reference to the prescribed motion multipliers tau that have
+already been calculated in the given \a state, which must have been realized
+through Acceleration stage. The result contains entries only for prescribed
+mobilities; if you want these unpacked into u-space mobility forces, use
+findMotionForces() instead. A mobilizer may follow prescribed motion either
+because of a Motion object or a call to MobilizedBody::lock(). **/
+const Vector& getMotionMultipliers(const State& state) const;
+
+/** Calculate the degree to which the supplied \a state does not satisfy the
+prescribed motion requirements at a particular Stage. For Position and Velocity
+stage, a call to the prescribe() solver using the same stage will eliminate
+the error. Accelerations should have been calculated to satisfy all prescribed
+accelerations, so the returned value should be zero always. The returned
+Vector has one element per known (prescribed) q, known u, or known udot.
+
+The \a state must be realized to Time stage to check Position errors,
+Position stage to check Velocity errors, and Acceleration stage to check
+Acceleration errors.
+
+Errors are calculated actualValue - prescribedValue so a positive error
+indicates that the value in \a state is too large. **/
+Vector calcMotionErrors(const State& state, const Stage& stage) const;
+
+/** Find the generalized mobility space forces produced by all the Motion
+objects active in this system. These are the same values as returned by
+getMotionMultipliers() but unpacked into u-space slots, with zeroes
+corresponding to any "free" mobilities, that is, those whose motion is not
+prescribed. **/
+void findMotionForces
+ (const State& state,
+ Vector& mobilityForces) const;
+
+/** Return a reference to the constraint multipliers lambda that have already
+been calculated in the given \a state, which must have been realized through
+Acceleration stage. Constraint multipliers are not directly interpretable as
+forces; if you want the actual forces use findConstraintForces() instead. If
+you want to know individual Constraint contributions to these forces, ask the
+Constraint objects rather than this SimbodyMatterSubsystem object. **/
+const Vector& getConstraintMultipliers(const State& state) const;
+
+/** Find the forces produced by all the active Constraint objects in this
+system. Constraints produce both body spatial forces and generalized
+mobility-space forces. The supplied \a state must have been realized through
+Acceleration stage. **/
+void findConstraintForces
+ (const State& state,
+ Vector_<SpatialVec>& bodyForcesInG,
+ Vector& mobilityForces) const;
+
+/** Calculate the power being generated or dissipated by all the Motion objects
+currently active in this system. The sign is chosen so that a positive value for
+power means the Motion is adding energy to the system; negative means it is
+removing energy. The \a state must already have been realized through
+Acceleration stage so that the prescribed motion forces are available.
+
+ at param[in] state
+ A State realized through Acceleration stage from which we obtain the
+ prescribed motion forces and the velocities needed to calculate power.
+
+<h3>Implementation</h3>
+We calculate power=-dot(tau, u) where tau is the set of mobility reaction
+forces generated by Motion objects and mobilizer locks (tau[i]==0 if mobility
+i is free), and u is the set of all generalized speeds.
+ at see calcConstraintPower() **/
+Real calcMotionPower(const State& state) const;
+
+/** Return the power begin generated or dissipated by all the Constraint
+objects currently active in this system. The sign is chosen so that a positive
+value for power means the Constraints (taken together) are adding energy to the
+system; negative means they are removing energy. The \a state must already have
+been realized through Acceleration stage so that the constraint forces are
+available.
+
+Note that if you want to know the power output of an individual Constraint,
+you should call that Constraint's calcPower() method; here they are all summed
+together.
+
+ at param[in] state
+ A State realized through Acceleration stage from which we obtain the
+ constraint forces and the velocities needed to calculate power.
+ at return
+ The signed sum over all the Constraint objects of the power being generated
+ or dissipated by each Constraint. A positive value means that together the
+ constraints are adding energy to the system; negative means they are
+ removing energy.
+
+<h3>Implementation</h3>
+We calculate power=-(dot(F,V)+dot(f,u)) where F is the set of body spatial
+reaction forces produced by the Constraints, V is the body spatial velocities,
+f is the set of mobility reaction forces produced by the Constraints, and u is
+the set of generalized speeds.
+ at see calcMotionPower() **/
+Real calcConstraintPower(const State& state) const;
+
+/** Accounts for applied forces and inertial forces produced by non-zero
+velocities in the State. Returns a set of mobility forces which replace both
+the applied bodyForces and the inertial forces.
+ at par Required stage
+ \c Stage::Dynamics **/
+void calcTreeEquivalentMobilityForces(const State&,
+ const Vector_<SpatialVec>& bodyForces,
+ Vector& mobilityForces) const;
+
+
+/** Calculate qdot = N(q)*u in O(n) time (very fast). Note that q is taken
+from the supplied state while u is an argument to this operator method.
+ at par Required stage
+ \c Stage::Position **/
+void calcQDot(const State& s,
+ const Vector& u,
+ Vector& qdot) const;
+
+/** Calculate qdotdot = N(q)*udot + Ndot(q,u)*u in O(n) time (very fast). Note
+that q and u are taken from the supplied state while udot is an argument to
+this operator method.
+ at par Required stage
+ \c Stage::Velocity **/
+void calcQDotDot(const State& s,
+ const Vector& udot,
+ Vector& qdotdot) const;
+
+/** Add in to the given body forces vector a force applied to a station (fixed
+point) S on a body B. The new force is added into the existing spatial force
+slot for the body. Note that this does not actually apply any forces to the
+multibody system! This is just a "helper" utility that makes it easier to
+fill in a body forces array. This has no effect on the system unless you
+later supply the body forces array for use.
+
+Provide the station in the body frame, force in the Ground frame.
+ at par Required stage
+ \c Stage::Position **/
+void addInStationForce(const State& state,
+ MobilizedBodyIndex bodyB,
+ const Vec3& stationOnB,
+ const Vec3& forceInG,
+ Vector_<SpatialVec>& bodyForcesInG) const;
+
+/** Add in to the given body forces vector a torque applied to a body B. The
+new torque is added into the existing spatial force slot for the body. Note
+that this does not actually apply any torques to the multibody system! This is
+just a "helper" utility that makes it easier to fill in a body forces array.
+This has no effect on the system unless you later supply the body forces array
+for use. Provide the torque vector in the Ground frame. **/
+void addInBodyTorque(const State& state,
+ MobilizedBodyIndex mobodIx,
+ const Vec3& torqueInG,
+ Vector_<SpatialVec>& bodyForcesInG) const;
+
+/** Add in to the given mobility forces vector a scalar generalized force, that
+is a force or torque applied to a mobilizer generalized speed. Note
+that this does not actually apply any forces to the multibody system! This is
+just a "helper" utility that makes it easier to fill in a mobility forces array.
+This has no effect on the system unless you later supply the mobility forces
+array for use. The meaning of a generalized force f is determined by the
+definition of the corresponding generalized speed u, so that f*u has physical
+units of power. **/
+void addInMobilityForce(const State& state,
+ MobilizedBodyIndex mobodIx,
+ MobilizerUIndex which,
+ Real f,
+ Vector& mobilityForces) const;
+/**@}**/
+
+
+
+//==============================================================================
+/** @name Realization and response methods
+
+Realization methods request that some calculation be performed ("realized") if
+it has not already been done since the last change to one of the state
+variables on which the result depends, with the result being placed in the
+state cache. Methods beginning with "get" are called \e responses and are used
+to extract pre-calculated information that has been realized into the cache.
+
+Realization is normally initiated at the System level. However, there are some
+"lazy" calculations in the %SimbodyMatterSubsystem whose computations are
+delayed until needed; you can cause those calculations to be performed
+explicitly here if you want. **/
+/**@{**/
+
+ // POSITION STAGE realizations //
+
+/** This method checks whether composite body inertias have already been
+computed since the last change to a Position stage state variable (q) and if so
+returns immediately at little cost; otherwise, it initiates computation of
+composite body inertias for all of the mobilized bodies. These are not otherwise
+computed unless specifically requested.
+ at par Required stage
+ \c Stage::Position
+ at see invalidateCompositeBodyInertias() **/
+void realizeCompositeBodyInertias(const State&) const;
+
+/** This method checks whether articulated body inertias have already been
+computed since the last change to a Position stage state variable (q) and if so
+returns immediately at little cost; otherwise, it initiates the relatively
+expensive computation of articulated body inertias for all of the mobilized
+bodies. These are not otherwise computed until they are needed at Dynamics
+stage.
+ at par Required stage
+ \c Stage::Position
+ at see invalidateArticulatedBodyInertias() **/
+void realizeArticulatedBodyInertias(const State&) const;
+
+
+ // INSTANCE STAGE responses //
+
+const Array_<QIndex>& getFreeQIndex(const State& state) const;
+const Array_<UIndex>& getFreeUIndex(const State& state) const;
+const Array_<UIndex>& getFreeUDotIndex(const State& state) const;
+const Array_<UIndex>& getKnownUDotIndex(const State& state) const;
+void packFreeQ
+ (const State& s, const Vector& allQ, Vector& packedFreeQ) const;
+void unpackFreeQ
+ (const State& s, const Vector& packedFreeQ, Vector& unpackedFreeQ) const;
+void packFreeU
+ (const State& s, const Vector& allU, Vector& packedFreeU) const;
+void unpackFreeU
+ (const State& s, const Vector& packedFreeU, Vector& unpackedFreeU) const;
+
+
+ // POSITION STAGE responses //
+
+/** Return the composite body inertia for a particular mobilized body. You can
+call this any time after the State has been realized to Position stage, however
+it will first trigger realization of all the composite body inertias if they
+have not already been calculated. Ground is mobilized body zero; its composite
+body inertia has infinite mass and principle moments of inertia, and zero
+center of mass.
+ at par Required stage
+ \c Stage::Position
+ at see realizeCompositeBodyInertias() **/
+const SpatialInertia&
+getCompositeBodyInertia(const State& state, MobilizedBodyIndex mbx) const;
+
+/** Return the articulated body inertia for a particular mobilized body. You
+can call this any time after the State has been realized to Position stage,
+however it will first trigger expensive realization of all the articulated body
+inertias if they have not already been calculated. Ground is mobilized body
+zero; its articulated body inertia is the same as its composite body inertia --
+an ordinary Spatial Inertia but with infinite mass and principle moments of
+inertia, and zero center of mass.
+ at par Required stage
+ \c Stage::Position
+ at see realizeArticulatedBodyInertias() **/
+const ArticulatedInertia&
+getArticulatedBodyInertia(const State& state, MobilizedBodyIndex mbx) const;
+
+
+ // VELOCITY STAGE responses //
+
+/** This is the angular velocity-dependent force on the body due to rotational
+inertia.
+ at par Required stage
+ \c Stage::Velocity **/
+const SpatialVec&
+getGyroscopicForce(const State& state, MobilizedBodyIndex mbx) const;
+
+/** This is the cross-mobilizer incremental contribution to coriolis (angular
+velocity dependent) acceleration; not too useful, see
+getTotalCoriolisAcceleration() instead.
+ at par Required stage
+ \c Stage::Velocity **/
+const SpatialVec&
+getMobilizerCoriolisAcceleration(const State& state,
+ MobilizedBodyIndex mbx) const;
+
+/** This is the total coriolis acceleration including the effect of the parent's
+angular velocity as well as the joint's. This is Jdot*u where J is the system
+kinematic Jacobian and u is the current set of generalized speeds in the
+supplied state. It is thus the remainder term in calculation of body
+accelerations from generalized accelerations udot: since V=J*u,
+A=J*udot + Jdot*u.
+ at par Required stage
+ \c Stage::Velocity **/
+const SpatialVec&
+getTotalCoriolisAcceleration(const State& state, MobilizedBodyIndex mbx) const;
+
+
+ // DYNAMICS STAGE responses //
+
+/** This is the angular velocity-dependent force accounting for gyroscopic
+forces plus coriolis forces due only to the cross-mobilizer velocity; this
+ignores the parent's velocity and is not too useful -- see
+getTotalCentrifugalForces() instead.
+ at par Required stage
+ \c Stage::Dynamics **/
+const SpatialVec&
+getMobilizerCentrifugalForces(const State& state, MobilizedBodyIndex mbx) const;
+
+/** This is the total angular velocity-dependent force acting on this body,
+including forces due to Coriolis acceleration and gyroscopic forces due to
+rotational inertia. This is F(b)=M[b]*A[b]+g[b] where M[b] is the spatial
+inertia matrix of body b (\e not the articulated inertia), A[b] is the total
+spatial Coriolis acceleration of body b, and g[b] is the (velocity-dependent)
+spatial gyroscopic force acting on body b.
+ at par Required stage
+ \c Stage::Dynamics **/
+const SpatialVec&
+getTotalCentrifugalForces(const State& state, MobilizedBodyIndex mbx) const;
+/**@}**/
+
+
+
+//==============================================================================
+/** @name Testing and debugging utilities
+
+Methods in this section provide alternate ways of calculating quantities for
+which we provide more efficient methods above. You should use the better
+methods normally, but these can be very useful for regression testing and
+Simbody development because the answers are obtained differently. Numerical
+results should agree with the faster methods to within numerical precision. **/
+/**@{**/
+
+/** This is a slower alternative to calcMobilizerReactionForces(), for use in
+regression testing and Simbody development. This method builds a freebody
+"diagram" for each body in turn to determine the unknown reaction force at
+its inboard mobilizer.
+
+The given \a state must have been realized through Acceleration stage.
+
+<h3>Implementation</h3>
+We use a tip-to-base recursion in which we assemble all applied body forces
+from force elements, constraints, and gyroscopic effects and compare that with
+the apparent rigid body force determined from F=M*A where M is a body's
+spatial inertia (in G) and A its already-calculated spatial acceleration. The
+difference is the missing force applied by the body's mobilizer, i.e. the
+reaction force. That is shifted to the M frame and reported. Then the equal
+and opposite reaction is applied to the parent body and included in its
+collection of applied forces, which can be used to determine its reaction force
+and so on.
+
+This is is about 3X slower than the method used by
+calcMobilizerReactionForces().
+ at see calcMobilizerReactionForces() **/
+void calcMobilizerReactionForcesUsingFreebodyMethod
+ (const State& state,
+ Vector_<SpatialVec>& forcesAtMInG) const;
+
+/** This is useful for timing computation time for
+realizeCompositeBodyInertias(), which otherwise will not recalculate them
+if called repeatedly. **/
+void invalidateCompositeBodyInertias(const State& state) const;
+
+/** This is useful for timing computation time for
+realizeArticulatedBodyInertias(), which otherwise will not recalculate them
+if called repeatedly. Note that this also invalidates Dynamics stage and
+above in the \a state because articulated body inertias are assumed to be
+valid after Dynamics stage, regardless of lazy evaluation status. **/
+void invalidateArticulatedBodyInertias(const State& state) const;
+/**@}**/
+
+
+//==============================================================================
+/** @name Proposed particle API
+
+(NOT IMPLEMENTED YET) These methods are a proposed API for explicit handling
+of particles. Currently a particle should be implemented as point mass with a
+Cartesian (translation) mobilizer to Ground instead. The idea here would be to
+special-case particles to make them faster; there would be no additional
+functionality. **/
+
+/**@{**/
+
+/// TODO: total number of particles.
+int getNumParticles() const;
+
+// The generalized coordinates for a particle are always the three measure numbers
+// (x,y,z) of the particle's Ground-relative Cartesian location vector. The generalized
+// speeds are always the three corresponding measure numbers of the particle's
+// Ground-relative Cartesian velocity. The generalized applied forces are
+// always the three measure numbers of a Ground-relative force vector.
+const Vector_<Vec3>& getAllParticleLocations (const State&) const;
+const Vector_<Vec3>& getAllParticleVelocities (const State&) const;
+
+const Vec3& getParticleLocation(const State& s, ParticleIndex p) const {
+ return getAllParticleLocations(s)[p];
+}
+const Vec3& getParticleVelocity(const State& s, ParticleIndex p) const {
+ return getAllParticleVelocities(s)[p];
+}
+
+Vector& updAllParticleMasses(State& s) const;
+
+void setAllParticleMasses(State& s, const Vector& masses) const {
+ updAllParticleMasses(s) = masses;
+}
+
+// Note that particle generalized coordinates, speeds, and applied forces
+// are defined to be the particle Cartesian locations, velocities, and
+// applied force vectors, so can be set directly at Stage::Model or higher.
+
+// These are the only routines that must be provided by the concrete MatterSubsystem.
+Vector_<Vec3>& updAllParticleLocations(State&) const;
+Vector_<Vec3>& updAllParticleVelocities(State&) const;
+
+// The following inline routines are provided by the generic MatterSubsystem
+// class for convenience.
+
+Vec3& updParticleLocation(State& s, ParticleIndex p) const {
+ return updAllParticleLocations(s)[p];
+}
+Vec3& updParticleVelocity(State& s, ParticleIndex p) const {
+ return updAllParticleVelocities(s)[p];
+}
+
+void setParticleLocation(State& s, ParticleIndex p, const Vec3& r) const {
+ updAllParticleLocations(s)[p] = r;
+}
+void setParticleVelocity(State& s, ParticleIndex p, const Vec3& v) const {
+ updAllParticleVelocities(s)[p] = v;
+}
+
+void setAllParticleLocations(State& s, const Vector_<Vec3>& r) const {
+ updAllParticleLocations(s) = r;
+}
+void setAllParticleVelocities(State& s, const Vector_<Vec3>& v) const {
+ updAllParticleVelocities(s) = v;
+}
+
+const Vector& getAllParticleMasses(const State&) const;
+
+const Vector_<Vec3>& getAllParticleAccelerations(const State&) const;
+
+const Vec3& getParticleAcceleration(const State& s, ParticleIndex p) const {
+ return getAllParticleAccelerations(s)[p];
+}
+/**@}**/
+
+//==============================================================================
+/** @name Obsolete methods
+
+These methods are deprecated because there is a better way now to do what they
+used to do. This may involve just a name change, calling signature, or something
+more substantial; see the documentation for the individual obsolete methods.
+**/
+
+/**@{**/
+private:
+/** Obsolete synonym for multiplyBySystemJacobian().
+ at see multiplyBySystemJacobian() **/
+void calcSpatialKinematicsFromInternal(const State& state,
+ const Vector& u,
+ Vector_<SpatialVec>& Ju) const
+{ multiplyBySystemJacobian(state,u,Ju); }
+
+/** Obsolete synonym for multiplyBySystemJacobianTranspose().
+ at see multiplyBySystemJacobianTranspose() **/
+void calcInternalGradientFromSpatial(const State& state,
+ const Vector_<SpatialVec>& F_G,
+ Vector& f) const
+{ multiplyBySystemJacobianTranspose(state, F_G, f); }
+
+/** Obsolete synonym for multiplyByM().
+ at see multiplyByM() **/
+void calcMV(const State& state, const Vector& v, Vector& MV) const
+{ multiplyByM(state,v,MV); }
+
+/** Obsolete synonym for multiplyByMInv().
+ at see multiplyByMInv() **/
+void calcMInverseV(const State& state,
+ const Vector& v,
+ Vector& MinvV) const
+{ multiplyByMInv(state,v,MinvV); }
+
+/** Obsolete slow version of calcPq.
+ at see calcPq() **/
+void calcPNInv(const State& state, Matrix& PNInv) const;
+
+/** Obsolete slow version of calcGTranspose().
+ at see calcGTranspose() **/
+void calcGt(const State&, Matrix& Gt) const;
+
+
+/**@}**/
+
+
+//==============================================================================
+// Bookkeeping methods and internal types -- hide from Doxygen
+/** @cond **/
+public:
+class Subtree; // used for working with a connected subgraph of the MobilizedBody tree
+class SubtreeResults;
+
+
+SimTK_PIMPL_DOWNCAST(SimbodyMatterSubsystem, Subsystem);
+const SimbodyMatterSubsystemRep& getRep() const;
+SimbodyMatterSubsystemRep& updRep();
+/** @endcond **/
+
+private:
+};
+
+/** Dump some debug information about the given subsystem to the given
+output stream; this is \e not for serialization.
+ at relates SimbodyMatterSubsystem **/
+SimTK_SIMBODY_EXPORT std::ostream&
+operator<<(std::ostream&, const SimbodyMatterSubsystem&);
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
diff --git a/Simbody/include/simbody/internal/SimbodyMatterSubtree.h b/Simbody/include/simbody/internal/SimbodyMatterSubtree.h
new file mode 100644
index 0000000..c10d43a
--- /dev/null
+++ b/Simbody/include/simbody/internal/SimbodyMatterSubtree.h
@@ -0,0 +1,290 @@
+#ifndef SimTK_SIMBODY_MATTER_SUBTREE_H_
+#define SimTK_SIMBODY_MATTER_SUBTREE_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) 2007-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 <cassert>
+#include <iosfwd>
+
+namespace SimTK {
+
+class SimbodyMatterSubsystem;
+class MobilizedBody;
+class SimbodyMatterSubtree;
+class SimbodyMatterSubtreeResults;
+
+/** A SimbodyMatterSubtree is a view of a connected subgraph of the tree of
+mobilized bodies in a SimbodyMatterSubsystem. It is used to perform kinematic
+operations on the subgraph to facilitate the handling of constraints, which
+typically involve only small subgraphs.
+
+A SimbodyMatterSubtree is characterized by a single ancestor body A and a set
+of terminal mobilized bodies T={Ti}, where A is the outmost body that is on
+the inboard path of each Ti. Note that a SimbodyMatterSubtree's "terminal"
+bodies do not have to be terminal in the full tree. The SimbodyMatterSubtree
+includes T and all "branch" mobilized bodies B={Bij} found on any path from a
+Ti to A, and A itself which serves as Ground. A may be one of the terminal
+bodies. A's mobilizer is *not* part of the SimbodyMatterSubtree. The path
+from Ti to A is called the ith branch of the SimbodyMatterSubtree; branches
+can overlap.
+ at verbatim
+ . .
+ . . .
+ . . .
+ T0 T1 T2 }
+ * * . * }
+ B0 * * B1 * }
+ * * * } A SimbodyMatterSubtree with
+ . * * B2 } three branches.
+ . . * * }
+ * * }
+ B0,B1 * * }
+ A }
+ .
+ .
+ ...
+ Ground
+ at endverbatim
+Each body in the SimbodyMatterSubtree is assigned an index called a
+SubtreeBodyIndex, with the Ancestor being SubtreeBodyIndex 0 and other ids
+assigned such that ids increase going outwards along a branch. Maps are kept
+in the SimbodyMatterSubtree object to track its relationship to the full tree.
+
+A SimbodyMatterSubtree can be constructed at Topology stage and needed ones
+can thus be precalculated and stored in the SimbodyMatterSubsystem Topology
+Cache (i.e., in the System not the State). Calculations done on the
+SimbodyMatterSubtree, on the other hand, require further state information
+and cannot be stored as part of the System. For those, we define a companion
+class below called SimbodyMatterSubtreeResults.
+
+A SimbodyMatterSubtreeResults object is initialized at Model stage, at which
+point we can determine the mobilities u and generalized coordinates q. These
+are assigned SubtreeUIndex's and SubtreeQIndex's in the same order that the
+SimbodyMatterSubtree bodies are numbered. Maps are kept in the
+SimbodyMatterSubtreeResults object to track the relationship between the
+SimbodyMatterSubtree mobilities and those in the full tree.
+
+Note that SimbodyMatterSubtree operations are elaborate \e operators, not
+\e responses. That means the results are not stored in the State, but rather
+in the private SimbodyMatterSubtreeResults objects.
+
+Operators here perform kinematic operations based on perturbations of the
+global System State values. The supported perturbations are:
+ General
+ 1a same as global System state (except answers are in A rather than G)
+ 1b all mobility variables set
+ 2 all mobility variables from 1a or 1b, except for one which is perturbed
+ (q,u,udot)
+
+ Linear
+ 3 all mobility variables are zero (u,udot)
+ 4 all mobility variables are zero *again*, except for one which is 1
+ (u,udot)
+Steps 1 and 2 are designed to work together, as are 3 and 4: first evaluate
+nominal kinematics; then perturb. **/
+class SimTK_SIMBODY_EXPORT SimbodyMatterSubtree {
+public:
+ SimbodyMatterSubtree();
+ SimbodyMatterSubtree(const SimbodyMatterSubtree&);
+ SimbodyMatterSubtree& operator=(const SimbodyMatterSubtree&);
+ ~SimbodyMatterSubtree();
+
+ explicit SimbodyMatterSubtree(const SimbodyMatterSubsystem&);
+ SimbodyMatterSubtree(const SimbodyMatterSubsystem&,
+ const Array_<MobilizedBodyIndex>& terminalBodies);
+
+ void setSimbodyMatterSubsystem(const SimbodyMatterSubsystem& matter);
+ const SimbodyMatterSubsystem& getSimbodyMatterSubsystem() const;
+
+ // This doesn't change the associated SimbodyMatterSubsystem if there
+ // is one, but does remove all the bodies from the SimbodyMatterSubtree.
+ void clear();
+
+ SimbodyMatterSubtree& addTerminalBody(MobilizedBodyIndex);
+
+ void realizeTopology();
+
+ int getNumSubtreeBodies() const; // includes ancestor
+ MobilizedBodyIndex getAncestorMobilizedBodyIndex() const;
+
+ // These are in the same order they were added; body[i] is the terminus
+ // of branch i.
+ const Array_<MobilizedBodyIndex>& getTerminalBodies() const;
+
+ // These are indexed by SubtreeBodyIndex starting with 0 for the ancestor
+ // body and monotonically increasing outwards along a branch.
+ const Array_<MobilizedBodyIndex>& getAllBodies() const;
+
+ // 0 returns an invalid Index
+ SubtreeBodyIndex getParentSubtreeBodyIndex(SubtreeBodyIndex) const;
+ const Array_<SubtreeBodyIndex>&
+ getChildSubtreeBodyIndices(SubtreeBodyIndex) const;
+
+ // MODEL STAGE
+
+ // State must be realized to at least Stage::Model for this call to work.
+ // The supplied SimbodyMatterSubtreeResults object is allocated and properly initialized to
+ // be able to hold computation results from this SimbodyMatterSubtree.
+ void initializeSubtreeResults(const State&, SimbodyMatterSubtreeResults&) const;
+
+ // This can be used as a sanity check that initializeSubtreeResults() was
+ // already called in this SimbodyMatterSubtree to produce these
+ // SimbodyMatterSubtreeResults. It is by no means exhaustive but will catch
+ // egregious errors.
+ bool isCompatibleSubtreeResults(const SimbodyMatterSubtreeResults&) const;
+
+ // POSITION STAGE
+
+ // State must be realized to at least Stage::Position for this to work. SimbodyMatterSubtreeResults
+ // must have already been initialized to work with this SimbodyMatterSubtree. SimbodyMatterSubtreeResults stage
+ // will be Stage::Position after this call. All body transforms will be the same as
+ // the corresponding ones in the state, except they will be measured from the ancestor
+ // frame instead of ground. SimbodyMatterSubtree q's will be identical to corresponding State q's.
+ void copyPositionsFromState(const State&, SimbodyMatterSubtreeResults&) const;
+
+ // State must be realized to Stage::Instance. subQ must be the right length for this
+ // SimbodyMatterSubtree, and SimbodyMatterSubtreeResults must have been properly initialized. SimbodyMatterSubtreeResults
+ // stage will be Stage::Position after this call.
+ void calcPositionsFromSubtreeQ(const State&, const Vector& subQ, SimbodyMatterSubtreeResults&) const;
+
+ // Calculates a perturbed position result starting with the subQ's and position results
+ // which must already be in SimbodyMatterSubtreeResults.
+ void perturbPositions(const State&, SubtreeQIndex subQIndex, Real perturbation, SimbodyMatterSubtreeResults&) const;
+
+
+ // VELOCITY STAGE
+
+ // State must be realized to at least Stage::Velocity for this to work. SimbodyMatterSubtreeResults
+ // must already be at Stage::Position. SimbodyMatterSubtreeResults stage
+ // will be Stage::Velocity after this call. All subtree body spatial velocities will be
+ // the same as in the State, except measured relative to A and expressed in A. SimbodyMatterSubtree u's
+ // will be identical to corresponding State u's.
+ void copyVelocitiesFromState(const State&, SimbodyMatterSubtreeResults&) const;
+
+ // State must be realized to Stage::Instance. subU must be the right length for this
+ // SimbodyMatterSubtree, and SimbodyMatterSubtreeResults must already be at Stage::Position. SimbodyMatterSubtreeResults
+ // stage will be Stage::Velocity after this call.
+ void calcVelocitiesFromSubtreeU(const State&, const Vector& subU, SimbodyMatterSubtreeResults&) const;
+
+ // State must be realized to Stage::Instance and SimbodyMatterSubtreeResults must already be at
+ // Stage::Position. SimbodyMatterSubtreeResults stage will be Stage::Velocity after this call, but
+ // all SimbodyMatterSubtree u's and body velocities will be zero.
+ void calcVelocitiesFromZeroU(const State&, SimbodyMatterSubtreeResults&) const;
+
+ // Calculates a perturbed velocity result starting with the subU's and velocity results
+ // which must already be in SimbodyMatterSubtreeResults.
+ void perturbVelocities(const State&, SubtreeUIndex subUIndex, Real perturbation, SimbodyMatterSubtreeResults&) const;
+
+
+ // ACCELERATION STAGE
+
+ // State must be realized to at least Stage::Acceleration for this to work. SimbodyMatterSubtreeResults
+ // must already be at Stage::Velocity. SimbodyMatterSubtreeResults stage
+ // will be Stage::Acceleration after this call. All subtree body spatial accelerations will be
+ // the same as in the State, except measured relative to A and expressed in A. SimbodyMatterSubtree udots
+ // will be identical to corresponding State udots.
+ void copyAccelerationsFromState(const State&, SimbodyMatterSubtreeResults&) const;
+
+ // State must be realized to Stage::Instance. subUDot must be the right length for this
+ // SimbodyMatterSubtree, and SimbodyMatterSubtreeResults must already be at Stage::Velocity. SimbodyMatterSubtreeResults
+ // stage will be Stage::Acceleration after this call.
+ void calcAccelerationsFromSubtreeUDot(const State&, const Vector& subUDot, SimbodyMatterSubtreeResults&) const;
+
+ // State must be realized to Stage::Instance and SimbodyMatterSubtreeResults must already be at
+ // Stage::Velocity. SimbodyMatterSubtreeResults stage will be Stage::Acceleration after this call.
+ // All SimbodyMatterSubtree udots's will be zero, body accelerations will have only their bias values
+ // (coriolis accelerations from nonzero u's).
+ void calcAccelerationsFromZeroUDot(const State&, SimbodyMatterSubtreeResults&) const;
+
+ // Calculates a perturbed velocity result starting with the subUDot's and acceleration results
+ // which must already be in SimbodyMatterSubtreeResults.
+ void perturbAccelerations(const State&, SubtreeUIndex subUDotIndex, Real perturbation, SimbodyMatterSubtreeResults&) const;
+
+ class SubtreeRep;
+private:
+ SubtreeRep* rep;
+ const SubtreeRep& getRep() const {assert(rep);return *rep;}
+ SubtreeRep& updRep() {assert(rep);return *rep;}
+};
+
+SimTK_SIMBODY_EXPORT std::ostream&
+operator<<(std::ostream&, const SimbodyMatterSubtree&);
+
+/*
+ * This is the writable "cache" for a SimbodyMatterSubtree. Once the full State has
+ * been realized to the Model stage, a SimbodyMatterSubtree can initialize one of these
+ * objects and then use it to hold operator results.
+ */
+class SimTK_SIMBODY_EXPORT SimbodyMatterSubtreeResults {
+public:
+ SimbodyMatterSubtreeResults();
+ SimbodyMatterSubtreeResults(const SimbodyMatterSubtreeResults&);
+ SimbodyMatterSubtreeResults& operator=(const SimbodyMatterSubtreeResults&);
+ ~SimbodyMatterSubtreeResults();
+
+ void clear();
+
+ void reallocateBodies(int nBodies);
+ void addMobilities(SubtreeBodyIndex, QIndex qStart, int nq, UIndex uStart, int nu);
+ void realizeModel(const Vector& stateQ, const Vector& stateU);
+
+ Stage getStage() const;
+
+ int getNumSubtreeBodies() const;
+ int getNumSubtreeQs() const;
+ int getNumSubtreeUs() const;
+
+ const Vector& getSubtreeQ() const;
+ const Transform& getSubtreeBodyTransform(SubtreeBodyIndex) const; // from ancestor frame
+
+ const Vector& getSubtreeU() const;
+ const SpatialVec& getSubtreeBodyVelocity(SubtreeBodyIndex) const; // measured & expressed in ancestor frame
+
+ const Vector& getSubtreeUDot() const;
+ const SpatialVec& getSubtreeBodyAcceleration(SubtreeBodyIndex) const; // measured & expressed in ancestor frame
+
+ // These are indexed by SubtreeQIndex and SubtreeUIndex.
+ const Array_<QIndex>& getQSubset() const; // subset of Subsystem Qs used by this SimbodyMatterSubtree
+ const Array_<UIndex>& getUSubset() const; // subset of Subsystem Us used by this SimbodyMatterSubtree
+
+ void findSubtreeBodyQ(SubtreeBodyIndex, SubtreeQIndex& qStart, int& nq) const; // indices into QSubset
+ void findSubtreeBodyU(SubtreeBodyIndex, SubtreeUIndex& uStart, int& nu) const; // indices into USubset
+
+ class SubtreeResultsRep;
+private:
+ friend class SimbodyMatterSubtree;
+ SubtreeResultsRep* rep;
+ const SubtreeResultsRep& getRep() const {assert(rep);return *rep;}
+ SubtreeResultsRep& updRep() {assert(rep);return *rep;}
+};
+
+SimTK_SIMBODY_EXPORT std::ostream&
+operator<<(std::ostream&, const SimbodyMatterSubtreeResults&);
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MATTER_SUBTREE_H_
diff --git a/Simbody/include/simbody/internal/TextDataEventReporter.h b/Simbody/include/simbody/internal/TextDataEventReporter.h
new file mode 100644
index 0000000..a9eab0c
--- /dev/null
+++ b/Simbody/include/simbody/internal/TextDataEventReporter.h
@@ -0,0 +1,83 @@
+#ifndef SimTK_SIMBODY_TEXT_DATA_EVENT_REPORTER_H_
+#define SimTK_SIMBODY_TEXT_DATA_EVENT_REPORTER_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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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"
+
+namespace SimTK {
+
+
+/** This is an EventReporter which prints out numeric data at regular intervals
+in tabular form. You provide it with a UserFunction, which calculates the
+values to be reported. At every reporting interval, it invokes the
+UserFunction, then prints out the current time along with the value or
+values returned by the function.
+
+After creating a TextDataEventReporter, add it to the System by calling the
+addEventReporter() method. **/
+class SimTK_SIMBODY_EXPORT TextDataEventReporter
+: public PeriodicEventReporter {
+public:
+
+ /** This template class defines a standard interface for objects that
+ calculate a function based on a System and State for use in a
+ TextDataEventReporter. **/
+ template <class T> class UserFunction {
+ public:
+ virtual ~UserFunction() {}
+ virtual T evaluate(const System& system, const State& state) = 0;
+ };
+
+ /** Create a TextDataEventReporter which calculates a single number at each
+ reporting interval, and displays it along with the time. Takes ownership
+ of the UserFunction object. **/
+ TextDataEventReporter(const System& system,
+ UserFunction<Real>* function,
+ Real reportInterval);
+
+ /** Create a TextDataEventReporter which calculates a vector of numbers at
+ each reporting interval, and displays them along with the time. Takes
+ ownership of the UserFunction object. **/
+ TextDataEventReporter(const System& system,
+ UserFunction<Vector>* function,
+ Real reportInterval);
+ /** The destructor will take care of deleting the UserFunction object;
+ don't delete it yourself. **/
+ ~TextDataEventReporter();
+
+ /** This is the implementation of the EventReporter virtual. **/
+ void handleEvent(const State& state) const OVERRIDE_11;
+
+ class TextDataEventReporterRep;
+protected:
+ TextDataEventReporterRep* rep;
+ const TextDataEventReporterRep& getRep() const {assert(rep); return *rep;}
+ TextDataEventReporterRep& updRep() const {assert(rep); return *rep;}
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_TEXT_DATA_EVENT_REPORTER_H_
diff --git a/Simbody/include/simbody/internal/common.h b/Simbody/include/simbody/internal/common.h
new file mode 100644
index 0000000..4aa0fe2
--- /dev/null
+++ b/Simbody/include/simbody/internal/common.h
@@ -0,0 +1,285 @@
+#ifndef SimTK_SIMBODY_COMMON_H_
+#define SimTK_SIMBODY_COMMON_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+Every Simbody header and source file should include this header before
+any other Simbody header. **/
+
+#include "SimTKcommon.h"
+
+#include <cassert>
+#include <vector>
+#include <limits>
+
+
+// Shared libraries are messy in Visual Studio. We have to distinguish three
+// cases:
+// (1) this header is being used to build the simbody shared library (dllexport)
+// (2) this header is being used by a *client* of the simbody shared
+// library (dllimport)
+// (3) we are building the simbody static library, or the client is
+// being compiled with the expectation of linking with the
+// simbody static library (nothing special needed)
+// In the CMake script for building this library, we define one of the symbols
+// SimTK_SIMBODY_BUILDING_{SHARED|STATIC}_LIBRARY
+// Client code normally has no special symbol defined, in which case we'll
+// assume it wants to use the shared library. However, if the client defines
+// the symbol SimTK_USE_STATIC_LIBRARIES we'll suppress the dllimport so
+// that the client code can be linked with static libraries. Note that
+// the client symbol is not library dependent, while the library symbols
+// affect only the simbody library, meaning that other libraries can
+// be clients of this one. However, we are assuming all-static or all-shared.
+
+#ifdef _WIN32
+ #ifdef _MSC_VER
+ #pragma warning(disable:4231) // need to use 'extern' template explicit instantiation
+ #endif
+ #if defined(SimTK_SIMBODY_BUILDING_SHARED_LIBRARY)
+ #define SimTK_SIMBODY_EXPORT __declspec(dllexport)
+ // Keep MS VC++ quiet when it tries to instantiate incomplete template classes in a DLL.
+ #ifdef _MSC_VER
+ #pragma warning(disable:4661)
+ #endif
+ #elif defined(SimTK_SIMBODY_BUILDING_STATIC_LIBRARY) || defined(SimTK_USE_STATIC_LIBRARIES)
+ #define SimTK_SIMBODY_EXPORT
+ #else
+ #define SimTK_SIMBODY_EXPORT __declspec(dllimport) // i.e., a client of a shared library
+ #endif
+#else
+ #define SimTK_SIMBODY_EXPORT // Linux, Mac
+#endif
+
+// Every SimTK library must provide these two routines, with the library
+// name appearing after the "version_" and "about_".
+extern "C" {
+ SimTK_SIMBODY_EXPORT void SimTK_version_simbody(int* major, int* minor, int* build);
+ SimTK_SIMBODY_EXPORT void SimTK_about_simbody(const char* key, int maxlen, char* value);
+}
+
+namespace SimTK {
+
+ // MATTER SUBSYSTEM-GLOBAL INDEX TYPES
+
+
+/** @class SimTK::MobilizedBodyIndex
+This is for arrays indexed by mobilized body number within a subsystem
+(typically the SimbodyMatterSubsystem).\ It is assigned when a MobilizedBody is
+added to a subsystem.\ You can abbreviate this as MobodIndex if you prefer.
+These will be used of course to index MobilizedBody objects but also to index
+many different kinds of information that may be stored on a per-MobilizedBody
+basis. @see SimTK::GroundIndex, SimTK::MobodIndex
+ at ingroup UniqueIndexTypes **/
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(MobilizedBodyIndex);
+
+/** This is the approved abbeviation for MobilizedBodyIndex.\ Feel free to
+use it if you get tired of typing or seeing the full name.
+ at ingroup UniqueIndexTypes **/
+typedef MobilizedBodyIndex MobodIndex;
+
+/** This is the MobilizedBodyIndex corresponding to the unique Ground body;
+its index is always zero.
+ at ingroup UniqueIndexTypes **/
+static const MobilizedBodyIndex GroundIndex(0);
+
+/** @class SimTK::ConstraintIndex
+This is for arrays indexed by constraint number within a subsystem (typically
+the SimbodyMatterSubsystem).\ It is assigned when a Constraint is added to the
+subsystem.
+ at ingroup UniqueIndexTypes **/
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(ConstraintIndex);
+
+// TODO: This is for arrays indexed by MatterSubsystem-global ParticleIndex,
+// as yet to be defined.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(ParticleIndex);
+
+// Constrained Bodies in constraints where the Ancestor body is not Ground (we
+// call these "Ancestor Constrained Bodies") require some additional cached
+// data, such as their orientations and velocities in the Ancestor frame, so
+// are each allocated a slot in pools of that data. Those pools are indexed by
+// this type.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(AncestorConstrainedBodyPoolIndex);
+
+// This is for "u-squared" arrays, that is, arrays which allocate space for an
+// nuXnu block for each MobilizedBody.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(USquaredIndex);
+
+// This is for "quaternion information" arrays, which have total dimension
+// equal to the number of quaternions currently in use as generalized
+// coordinates for modeling the Matter Subsystem's MobilizedBodies. Primarily
+// this is for storing the norm of quaternions so we need calculate them only
+// once.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(QuaternionPoolIndex);
+
+// This is for miscellaneous Real-valued position cache data that individual
+// mobilizers ask us to hold for generalized coordinate q precalculations
+// (e.g. sines and cosines).
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(MobodQPoolIndex);
+
+// These are for indexing the pools of prescribed q's, u's, udots, and calculated forces
+// needed to produce the udots. The arrays are allocated in order of MobilizedBodyIndex, and
+// then in q and u order within the mobilizer. A mobilier with prescribed positions q gets
+// slots in the u and udot pools also to hold derivatives, and similarly if it is the
+// velocities u that are prescribed there will be slots in the udot pools. Note that
+// the Q index can be used to index qdot and qdotdot arrays if needed. Note that a
+// prescribed force is produced whenever there is a udot that is not force driven; that
+// includes prescribed udots but also zero and discrete ones.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(PresQPoolIndex);
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(PresUPoolIndex);
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(PresUDotPoolIndex);
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(PresForcePoolIndex);
+
+ // PER-MOBILIZER INDEX TYPES
+
+/** @class SimTK::MobilizerQIndex
+The Mobilizer associated with each MobilizedBody, once modeled, has a specific number
+of generalized coordinates \e q (0-7) and generalized speeds (mobilities)
+\e u (0-6).\ This is the index type for the small array of Mobilizer-local \e q's.
+ at see MobilizerUIndex, QIndex
+ at ingroup UniqueIndexTypes **/
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(MobilizerQIndex);
+/** @class SimTK::MobilizerUIndex
+The Mobilizer associated with each MobilizedBody, once modeled, has a specific number
+of generalized coordinates \e q (0-7) and generalized speeds (mobilities)
+\e u (0-6).\ This is the index type for the small array of Mobilizer-local \e u's.
+ at see MobilizerQIndex, UIndex
+ at ingroup UniqueIndexTypes **/
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(MobilizerUIndex);
+
+ // PER-CONSTRAINT INDEX TYPES
+
+// This is the Constraint-specific index of the MobilizedBodies which are *directly* affected
+// by a constraint, through body forces or body torques on these bodies.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(ConstrainedBodyIndex);
+
+// This is the Constraint-specific index of the MobilizedBodies whose mobilizers' mobilities
+// can appear explicitly in constraint equations, and which are acted upon by the Constraint
+// through generation of generalized (mobility) forces. Note that for a multi-dof mobilizer
+// we don't select individual mobilities; it is all or nothing so we can use the MobilizedBody
+// to stand for its mobilizer.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(ConstrainedMobilizerIndex);
+
+// This is the Constraint-specific index of a coordinate q which can be *directly* affected
+// by this constraint through generation of a mobility force on a corresponding mobility. These
+// are numbered in order of ConstrainedMobilizerIndex for the mobilizers for which these are the q's.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(ConstrainedQIndex);
+
+// This is the Constraint-specific index of a mobility u which can be *directly* affected
+// by this constraint through generation of a mobility force. These are numbered in order
+// of ConstrainedMobilizerIndex for the bodies for which these are the u's.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(ConstrainedUIndex);
+
+
+// This is the Constraint-specific index of a coordinate q which can be involved in any
+// constraint equation of this constraint, either directly through ConstrainedMobilizers
+// or indirectly as a result of its effects on ConstrainedBodies (that is, this list
+// includes all the ConstraintQIndex entries above, plus possibly many more). These are in sorted
+// order by subsystem-wide QIndex, and each QIndex appears at most once.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(ParticipatingQIndex);
+
+// This is the Constraint-specific index of a coordinate u which can be involved in any
+// constraint equation of this constraint, either directly through ConstrainedMobilizers
+// or indirectly as a result of its effects on ConstrainedBodies (that is, this list
+// includes all the ConstraintUIndex entries above, plus possibly many more). These are in sorted
+// order by subsystem-wide UIndex, and each UIndex appears at most once.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(ParticipatingUIndex);
+
+ // SUBTREE INDEX TYPES
+
+// And similarly for other unique Index types.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(SubtreeBodyIndex);
+static const SubtreeBodyIndex SubtreeAncestorIndex(0);
+
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(SubtreeQIndex);
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(SubtreeUIndex);
+
+ // INDEX TYPES FOR OTHER SUBSYSTEMS
+
+/** @class SimTK::ForceIndex
+This type represents the index of a Force element within its subsystem.
+ at relates SimTK::Force
+ at ingroup UniqueIndexTypes **/
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(ForceIndex);
+
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(ContactSetIndex);
+
+
+namespace Exception {
+
+
+class APIMethodFailed : public Base {
+public:
+ APIMethodFailed(const char* fn, int ln, String method, String cause) : Base(fn,ln)
+ {
+ setMessage(method + " failed because:\n " + cause);
+ }
+};
+
+
+// This just reports rep-level bad things up to the API level with a helpful string.
+class RepLevelException : public Base {
+public:
+ RepLevelException(const char* fn, int ln, String message) : Base(fn,ln)
+ {
+ setMessage(message);
+ }
+};
+
+class MobilizerCantExactlyRepresentRequestedQuantity : public Base {
+public:
+ MobilizerCantExactlyRepresentRequestedQuantity(const char* fn, int ln,
+ String method, MobilizedBodyIndex body, String quantity) : Base(fn,ln)
+ {
+ setMessage(method + "(): the mobilizer for body " + String((int)body)
+ + " can't represent the given " + quantity + " to machine precision");
+ }
+private:
+};
+
+class NewtonRaphsonFailure : public Base {
+public:
+ NewtonRaphsonFailure(const char* fn, int ln, String msg) : Base(fn,ln)
+ {
+ setMessage("NewtonRaphson failure: " + msg);
+ }
+private:
+};
+
+class LoopConstraintConstructionFailure : public Base {
+public:
+ LoopConstraintConstructionFailure(const char* fn, int ln, String msg) : Base(fn,ln)
+ {
+ setMessage("Loop constraint construction failure: " + msg);
+ }
+private:
+};
+
+} // namespace SimTK::Exception
+
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_COMMON_H_
diff --git a/Simbody/sharedTarget/CMakeLists.txt b/Simbody/sharedTarget/CMakeLists.txt
new file mode 100644
index 0000000..771abce
--- /dev/null
+++ b/Simbody/sharedTarget/CMakeLists.txt
@@ -0,0 +1,64 @@
+## This whole directory exists just so I could define this extra
+## preprocessor value.
+
+ADD_DEFINITIONS(-DSimTK_SIMBODY_BUILDING_SHARED_LIBRARY)
+
+IF(BUILD_UNVERSIONED_LIBRARIES)
+
+ ADD_LIBRARY(${SHARED_TARGET} SHARED
+ ${SOURCE_FILES} ${SOURCE_INCLUDE_FILES}
+ ${API_ABS_INCLUDE_FILES})
+
+ TARGET_LINK_LIBRARIES(${SHARED_TARGET}
+ debug ${SimTKMATH_SHARED_LIBRARY}_d
+ optimized ${SimTKMATH_SHARED_LIBRARY}
+ debug ${SimTKCOMMON_SHARED_LIBRARY}_d
+ optimized ${SimTKCOMMON_SHARED_LIBRARY}
+ ${MATH_LIBS_TO_USE})
+
+ SET_TARGET_PROPERTIES(${SHARED_TARGET} PROPERTIES
+ PROJECT_LABEL "Code - ${SHARED_TARGET}"
+ SOVERSION ${SIMBODY_SONAME_VERSION})
+
+ # install library; on Windows the .dll goes in the bin directory.
+
+ INSTALL(TARGETS ${SHARED_TARGET}
+ PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE
+ GROUP_READ GROUP_WRITE GROUP_EXECUTE
+ WORLD_READ WORLD_EXECUTE
+ LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ RUNTIME DESTINATION ${CMAKE_INSTALL_FULL_BINDIR})
+
+
+ENDIF(BUILD_UNVERSIONED_LIBRARIES)
+
+
+IF(BUILD_VERSIONED_LIBRARIES)
+
+ ADD_LIBRARY(${SHARED_TARGET_VN} SHARED
+ ${SOURCE_FILES} ${SOURCE_INCLUDE_FILES}
+ ${API_ABS_INCLUDE_FILES})
+
+ TARGET_LINK_LIBRARIES(${SHARED_TARGET_VN}
+ debug ${SimTKMATH_SHARED_LIBRARY_VN}_d
+ optimized ${SimTKMATH_SHARED_LIBRARY_VN}
+ debug ${SimTKCOMMON_SHARED_LIBRARY_VN}_d
+ optimized ${SimTKCOMMON_SHARED_LIBRARY_VN}
+ ${MATH_LIBS_TO_USE_VN})
+
+ SET_TARGET_PROPERTIES(${SHARED_TARGET_VN} PROPERTIES
+ PROJECT_LABEL "Code - ${SHARED_TARGET_VN}"
+ SOVERSION ${SIMBODY_SONAME_VERSION})
+
+ # install library; on Windows the .dll goes in the bin directory.
+
+ INSTALL(TARGETS ${SHARED_TARGET_VN}
+ PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE
+ GROUP_READ GROUP_WRITE GROUP_EXECUTE
+ WORLD_READ WORLD_EXECUTE
+ LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ RUNTIME DESTINATION ${CMAKE_INSTALL_FULL_BINDIR})
+
+ENDIF(BUILD_VERSIONED_LIBRARIES)
diff --git a/Simbody/src/About.cpp b/Simbody/src/About.cpp
new file mode 100644
index 0000000..60c789d
--- /dev/null
+++ b/Simbody/src/About.cpp
@@ -0,0 +1,115 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/** @file
+ * Defines the standard SimTK core "version" and "about" routines.
+ */
+
+#include "SimTKcommon.h"
+#include "simbody/internal/common.h"
+
+#include <string>
+#include <cstring>
+#include <cctype>
+
+#define STR(var) #var
+#define MAKE_VERSION_STRING(maj,min,build) STR(maj.min.build)
+#define MAKE_COPYRIGHT_STRING(y,a) \
+ "Copyright (c) " STR(y) " Stanford University, " STR(a)
+#define MAKE_STRING(a) STR(a)
+
+#define GET_VERSION_STRING \
+ MAKE_VERSION_STRING(SimTK_SIMBODY_MAJOR_VERSION, \
+ SimTK_SIMBODY_MINOR_VERSION, \
+ SimTK_SIMBODY_PATCH_VERSION)
+
+#define GET_COPYRIGHT_STRING \
+ MAKE_COPYRIGHT_STRING(SimTK_SIMBODY_COPYRIGHT_YEARS, \
+ SimTK_SIMBODY_AUTHORS)
+
+#define GET_AUTHORS_STRING \
+ MAKE_STRING(SimTK_SIMBODY_AUTHORS)
+
+#define GET_LIBRARY_STRING \
+ MAKE_STRING(SimTK_SIMBODY_LIBRARY_NAME)
+
+#if defined(SimTK_SIMBODY_BUILDING_SHARED_LIBRARY)
+ #define GET_TYPE_STRING "shared"
+#elif defined(SimTK_SIMBODY_BUILDING_STATIC_LIBRARY)
+ #define GET_TYPE_STRING "static"
+#else
+ #define GET_TYPE_STRING "<unknown library type?!>"
+#endif
+
+#ifndef NDEBUG
+ #define GET_DEBUG_STRING "debug"
+#else
+ #define GET_DEBUG_STRING "release"
+#endif
+
+extern "C" {
+
+void SimTK_version_simbody(int* major, int* minor, int* patch) {
+ static const char* l = "SimTK library=" GET_LIBRARY_STRING;
+ static const char* t = "SimTK type=" GET_TYPE_STRING;
+ static const char* d = "SimTK debug=" GET_DEBUG_STRING;
+ static const char* v = "SimTK version=" GET_VERSION_STRING;
+ static const char* c = "SimTK copyright=" GET_COPYRIGHT_STRING;
+
+ if (major) *major = SimTK_SIMBODY_MAJOR_VERSION;
+ if (minor) *minor = SimTK_SIMBODY_MINOR_VERSION;
+ if (patch) *patch = SimTK_SIMBODY_PATCH_VERSION;
+
+ // Force statics to be present in the binary (Release mode otherwise
+ // optimizes them away).
+ volatile int i=0;
+ if (i) { // never true, but compiler doesn't know ...
+ *major = *l + *t + *d + *v + *c;
+ }
+}
+
+void SimTK_about_simbody(const char* key, int maxlen, char* value) {
+ if (maxlen <= 0 || value==0) return;
+ value[0] = '\0'; // in case we don't find a match
+ if (key==0) return;
+
+ // downshift the key
+ std::string skey(key);
+ for (size_t i=0; i<skey.size(); ++i)
+ skey[i] = std::tolower(skey[i]);
+
+ const char* v = 0;
+ if (skey == "version") v = GET_VERSION_STRING;
+ else if (skey == "library") v = GET_LIBRARY_STRING;
+ else if (skey == "type") v = GET_TYPE_STRING;
+ else if (skey == "copyright") v = GET_COPYRIGHT_STRING;
+ else if (skey == "authors") v = GET_AUTHORS_STRING;
+ else if (skey == "debug") v = GET_DEBUG_STRING;
+
+ if (v) {
+ std::strncpy(value,v,maxlen-1);
+ value[maxlen-1] = '\0'; // in case we ran out of room
+ }
+}
+
+}
diff --git a/Simbody/src/Assembler.cpp b/Simbody/src/Assembler.cpp
new file mode 100644
index 0000000..212a712
--- /dev/null
+++ b/Simbody/src/Assembler.cpp
@@ -0,0 +1,1087 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/MobilizedBody.h"
+#include "simbody/internal/MultibodySystem.h"
+#include "simbody/internal/Assembler.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include <map>
+#include <iostream>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+//------------------------------------------------------------------------------
+// EXCEPTIONS
+//------------------------------------------------------------------------------
+class Assembler::AssembleFailed : public Exception::Base {
+public:
+ AssembleFailed
+ (const char* fn, int ln, const char* why,
+ Real tolAchieved, Real tolRequired)
+ : Base(fn,ln)
+ {
+ setMessage(
+ "Method Assembler::assemble() failed because:\n" + String(why)
+ + "\nAssembly error tolerance achieved: "
+ + String(tolAchieved) + " required: " + String(tolRequired)
+ + ".");
+ }
+};
+class Assembler::TrackFailed : public Exception::Base {
+public:
+ TrackFailed
+ (const char* fn, int ln, const char* why,
+ Real tolAchieved, Real tolRequired)
+ : Base(fn,ln)
+ {
+ setMessage(
+ "Method Assembler::track() failed because:\n" + String(why)
+ + "\nAssembly error tolerance achieved: "
+ + String(tolAchieved) + " required: " + String(tolRequired)
+ + ".");
+ }
+};
+
+//------------------------------------------------------------------------------
+// BUILT IN CONSTRAINTS
+//------------------------------------------------------------------------------
+// This is the Assembly Condition representing the System's built-in
+// Constraints. Only Constraints that are currently enabled are included.
+// This class provides an efficient implementation for treating these
+// Constraints either as an assembly requirement or an assembly goal.
+namespace { // this class is local to this file
+class BuiltInConstraints : public AssemblyCondition {
+public:
+ BuiltInConstraints()
+ : AssemblyCondition("System Constraints") {}
+
+ // Note that we have turned off quaternions so the number of q error
+ // slots in the State includes only real holonomic constraint equations.
+ int getNumErrors(const State& s) const {return s.getNQErr();}
+
+ // Return the system holonomic constraint errors as found in the State.
+ int calcErrors(const State& state, Vector& err) const {
+ err = state.getQErr();
+ return 0;
+ }
+
+ // The Jacobian of the holonomic constraint errors is Pq=PN^-1. We can get
+ // that analytically from Simbody but we might have to strip out some
+ // of the columns if we aren't using all the q's.
+ int calcErrorJacobian(const State& state, Matrix& jacobian) const {
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+ const int np = getNumFreeQs();
+ const int nq = state.getNQ();
+
+ jacobian.resize(state.getNQErr(), np); // ok cuz no quaternions
+
+ if (np == nq) {
+ // Jacobian is already the right shape.
+ matter.calcPq(state, jacobian);
+ } else {
+ Matrix fullJac(state.getNQErr(), nq);
+ matter.calcPq(state, fullJac);
+ // Extract just the columns corresponding to free Qs
+ for (Assembler::FreeQIndex fx(0); fx < np; ++fx)
+ jacobian(fx) = fullJac(getQIndexOfFreeQ(fx));
+ }
+ return 0;
+ }
+
+ // Goal is qerr^2/2 (the /2 makes the gradient easier).
+ int calcGoal(const State& state, Real& goal) const {
+ goal = (~state.getQErr() * state.getQErr()) / 2;
+ return 0;
+ }
+
+ // Gradient is ~(d goal/dq) = ~(~qerr * dqerr/dq) = ~(~qerr*Pq)
+ // = ~Pq qerr. This can be done in O(n+m) time since we can calculate
+ // the matrix-vector product ~Pq*v in O(n+m) time, where
+ // n=#q's and m=# constraint equations.
+ int calcGoalGradient(const State& state, Vector& grad) const {
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+ const int np = getNumFreeQs();
+ const int nq = state.getNQ();
+
+ grad.resize(np);
+
+ if (np == nq) {
+ // Nothing locked; analytic gradient is the right size
+ matter.multiplyByPqTranspose(state, state.getQErr(), grad);
+ } else {
+ Vector fullGrad(nq);
+ matter.multiplyByPqTranspose(state, state.getQErr(), fullGrad);
+ // Extract just the entries corresponding to free Qs
+ for (Assembler::FreeQIndex fx(0); fx < np; ++fx)
+ grad[fx] = fullGrad[getQIndexOfFreeQ(fx)];
+ }
+
+ //cout << "built in grad=" << grad << endl;
+ return 0;
+ }
+
+private:
+};
+} // end anonymous namespace
+
+
+//------------------------------------------------------------------------------
+// ASSEMBLER SYSTEM
+//------------------------------------------------------------------------------
+
+// This class defines the objective function which is passed to the Optimizer.
+class Assembler::AssemblerSystem : public OptimizerSystem {
+public:
+ AssemblerSystem(Assembler& assembler)
+ : OptimizerSystem(assembler.getNumFreeQs()), assembler(assembler)
+ { getSystem().realize(getInternalState(), Stage::Time);
+ resetStats(); }
+
+ // Convenient interface to objective function.
+ Real calcCurrentGoal() const {
+ Real val;
+ const int status = objectiveFunc(getFreeQsFromInternalState(),true,val);
+ SimTK_ERRCHK1_ALWAYS(status==0,
+ "AssemblerSystem::calcCurrentGoal()",
+ "objectiveFunc() returned status %d.", status);
+ return val;
+ }
+
+ // Convenient interface to gradient of objective function.
+ Vector calcCurrentGradient() const {
+ Vector grad(getNumFreeQs());
+ const int status =
+ gradientFunc(getFreeQsFromInternalState(),true,grad);
+ SimTK_ERRCHK1_ALWAYS(status==0,
+ "AssemblerSystem::calcCurrentGradient()",
+ "gradientFunc() returned status %d.", status);
+ return grad;
+ }
+
+ // Convenient interface to assembly constraint error function.
+ Vector calcCurrentErrors() const {
+ Vector errs(getNumEqualityConstraints());
+ const int status =
+ constraintFunc(getFreeQsFromInternalState(), true, errs);
+ SimTK_ERRCHK1_ALWAYS(status==0,
+ "AssemblerSystem::calcCurrentErrors()",
+ "constraintFunc() returned status %d.", status);
+ return errs;
+ }
+
+ // Convenient interface to Jacobian of assembly constraint error function.
+ Matrix calcCurrentJacobian() const {
+ Matrix jac(getNumEqualityConstraints(), getNumFreeQs());
+ const int status =
+ constraintJacobian(getFreeQsFromInternalState(), true, jac);
+ SimTK_ERRCHK1_ALWAYS(status==0,
+ "AssemblerSystem::calcCurrentJacobian()",
+ "constraintJacobian() returned status %d.", status);
+ return jac;
+ }
+
+ // Return the value of the objective to be minimized when the freeQs
+ // have the values given by the parameters.
+ int objectiveFunc(const Vector& parameters,
+ bool new_parameters,
+ Real& objectiveValue) const
+ { ++nEvalObjective;
+
+ if (new_parameters)
+ setInternalStateFromFreeQs(parameters);
+
+ objectiveValue = 0;
+ for (unsigned i=0; i < assembler.goals.size(); ++i) {
+ AssemblyConditionIndex goalIx = assembler.goals[i];
+ const AssemblyCondition& cond = *assembler.conditions[goalIx];
+ Real goalValue;
+ const int stat = cond.calcGoal(getInternalState(), goalValue);
+ if (stat != 0)
+ return stat;
+ SimTK_ERRCHK2_ALWAYS(goalValue >= 0,
+ "AssemblerSystem::objectiveFunc()",
+ "calcGoal() method of assembly condition %s returned a"
+ " negative or non-finite value %g.", cond.getName(), goalValue);
+ objectiveValue += assembler.weights[goalIx] * goalValue;
+ }
+
+ //static int count = 0;
+ //cout << " " << count++ << " obj=" << objectiveValue << endl;
+ return 0;
+ }
+
+ class NumGradientFunc : public Differentiator::GradientFunction {
+ public:
+ NumGradientFunc(Assembler& assembler,
+ const Array_<AssemblyConditionIndex>& numGoals)
+ : Differentiator::GradientFunction(assembler.getNumFreeQs()),
+ assembler(assembler), numGoals(numGoals) {}
+
+ // This is the function that gets differentiated. We want it to
+ // return fy = sum( w[i] * goal[i] ) for each of the goals that needs
+ // a numerical gradient. Then we can calculate all of them at once.
+ int f(const Vector& y, Real& fy) const {
+ assembler.setInternalStateFromFreeQs(y);
+ fy = 0;
+ for (unsigned i=0; i < numGoals.size(); ++i) {
+ AssemblyConditionIndex goalIx = numGoals[i];
+ const AssemblyCondition& cond =
+ *assembler.conditions[goalIx];
+ Real goalValue;
+ const int stat = cond.calcGoal(assembler.getInternalState(),
+ goalValue);
+ if (stat != 0)
+ return stat;
+ fy += assembler.weights[goalIx] * goalValue;
+ }
+ return 0;
+ }
+ private:
+ Assembler& assembler;
+ const Array_<AssemblyConditionIndex>& numGoals;
+ };
+
+ int gradientFunc(const Vector& parameters,
+ bool new_parameters,
+ Vector& gradient) const
+ { SimTK_ASSERT2_ALWAYS(gradient.size() == getNumFreeQs(),
+ "AssemblySystem::gradientFunc(): expected gradient vector of"
+ " size %d but got %d; this is likely a problem with the optimizer"
+ " which is required to allocate the right amount of space.",
+ getNumFreeQs(), gradient.size());
+
+ ++nEvalGradient;
+
+ if (new_parameters)
+ setInternalStateFromFreeQs(parameters);
+
+ for (unsigned i=0; i < assembler.reporters.size(); ++i)
+ assembler.reporters[i]->handleEvent(getInternalState());
+
+ // This will record the indices of any goals we encounter that can't
+ // provide their own gradients; we'll handle them all together at
+ // the end.
+ Array_<AssemblyConditionIndex> needNumericalGradient;
+
+ gradient = 0;
+ Vector tmpGradient(gradient.size());
+ for (unsigned i=0; i < assembler.goals.size(); ++i) {
+ AssemblyConditionIndex goalIx = assembler.goals[i];
+ const AssemblyCondition& cond = *assembler.conditions[goalIx];
+ const int stat = (assembler.forceNumericalGradient
+ ? -1
+ : cond.calcGoalGradient(getInternalState(),
+ tmpGradient));
+ if (stat == -1) {
+ needNumericalGradient.push_back(goalIx);
+ continue;
+ }
+ if (stat != 0)
+ return stat;
+
+ gradient += assembler.weights[goalIx] * tmpGradient;
+ }
+
+ if (!needNumericalGradient.empty()) {
+ //cout << "Need numerical gradient for "
+ // << needNumericalGradient.size() << " goals." << endl;
+ NumGradientFunc numGoals(assembler, needNumericalGradient);
+ // Essential to use central difference here so that the
+ // approximate gradient is actually zero at the optimum
+ // solution, otherwise IpOpt won't converge.
+ Differentiator gradNumGoals
+ (numGoals,Differentiator::CentralDifference);
+ // weights are already included here
+ gradient += gradNumGoals.calcGradient(getFreeQsFromInternalState());
+
+ nEvalObjective += gradNumGoals.getNumCallsToUserFunction();
+ }
+
+ //cout << "Grad=" << gradient << endl;
+
+ return 0;
+ }
+
+
+ // Return the errors in the hard assembly error conditions.
+ int constraintFunc(const Vector& parameters,
+ bool new_parameters,
+ Vector& qerrs) const
+ { ++nEvalConstraints;
+
+ if (new_parameters)
+ setInternalStateFromFreeQs(parameters);
+
+ int nxtEqn = 0;
+ for (unsigned i=0; i < assembler.errors.size(); ++i) {
+ AssemblyConditionIndex consIx = assembler.errors[i];
+ const AssemblyCondition& cond = *assembler.conditions[consIx];
+ const int m = cond.getNumErrors(getInternalState());
+ int stat = cond.calcErrors(getInternalState(), qerrs(nxtEqn,m));
+ if (stat != 0)
+ return stat;
+ nxtEqn += m;
+ }
+
+ //cout << " err=" << qerrs << endl;
+
+ return 0;
+ }
+
+ class NumJacobianFunc : public Differentiator::JacobianFunction {
+ public:
+ NumJacobianFunc(Assembler& assembler,
+ const Array_<AssemblyConditionIndex>& numCons,
+ const Array_<int>& nErrorEqns,
+ int totalNEqns)
+ : Differentiator::JacobianFunction
+ (totalNEqns, assembler.getNumFreeQs()),
+ assembler(assembler), numCons(numCons), nEqns(nErrorEqns),
+ totalNEqns(totalNEqns)
+ { assert(numCons.size() == nEqns.size()); }
+
+ // This is the function that gets differentiated. We want it to
+ // return fy = [ err[i] ] for each of the assembly constraint
+ // conditions that needs a numerical gradient. Then we can calculate
+ // all their Jacobians at once.
+ int f(const Vector& y, Vector& fy) const {
+ assert(y.size() == assembler.getNumFreeQs());
+ assert(fy.size() == totalNEqns);
+
+ assembler.setInternalStateFromFreeQs(y);
+ int nxtSlot = 0;
+ for (unsigned i=0; i < numCons.size(); ++i) {
+ AssemblyConditionIndex consIx = numCons[i];
+ const AssemblyCondition& cond =
+ *assembler.conditions[consIx];
+ const int stat = cond.calcErrors
+ (assembler.getInternalState(), fy(nxtSlot, nEqns[i]));
+ if (stat != 0)
+ return stat;
+ nxtSlot += nEqns[i];
+ }
+
+ assert(nxtSlot == totalNEqns); // must use all slots
+ return 0;
+ }
+ private:
+ Assembler& assembler;
+ const Array_<AssemblyConditionIndex>& numCons;
+ const Array_<int>& nEqns;
+ const int totalNEqns;
+ };
+
+ int constraintJacobian(const Vector& parameters,
+ bool new_parameters,
+ Matrix& J) const
+ { ++nEvalJacobian;
+
+ if (new_parameters)
+ setInternalStateFromFreeQs(parameters);
+ for (unsigned i=0; i < assembler.reporters.size(); ++i)
+ assembler.reporters[i]->handleEvent(getInternalState());
+
+ assert(J.nrow() == getNumEqualityConstraints());
+ assert(J.ncol() == getNumFreeQs());
+
+ const int n = getNumFreeQs();
+
+ // This will record the indices of any constraints we encounter that
+ // can't provide their own gradients; we'll handle them all together
+ // at the end.
+ Array_<AssemblyConditionIndex> needNumericalJacobian;
+ Array_<int> firstEqn;
+ Array_<int> nEqns;
+ int needy = 0;
+
+ int nxtEqn = 0;
+ for (unsigned i=0; i < assembler.errors.size(); ++i) {
+ AssemblyConditionIndex consIx = assembler.errors[i];
+ const AssemblyCondition& cond = *assembler.conditions[consIx];
+ const int m = cond.getNumErrors(getInternalState());
+ const int stat = (assembler.forceNumericalJacobian
+ ? -1
+ : cond.calcErrorJacobian(getInternalState(),
+ J(nxtEqn,0,m,n)));
+ if (stat == -1) {
+ needNumericalJacobian.push_back(consIx);
+ firstEqn.push_back(nxtEqn);
+ nEqns.push_back(m);
+ needy += m;
+ } else if (stat != 0)
+ return stat;
+ nxtEqn += m;
+ }
+
+ if (!needNumericalJacobian.empty()) {
+ //cout << "Need numerical Jacobian for "
+ // << needNumericalJacobian.size() << " constraints." << endl;
+ NumJacobianFunc numCons(assembler, needNumericalJacobian,
+ nEqns, needy);
+ // Forward difference should be fine here, unlike for the
+ // gradient because we converge on the solution value
+ // rather than the derivative norm.
+ Differentiator jacNumCons(numCons);
+ Matrix numJ = jacNumCons.calcJacobian(getFreeQsFromInternalState());
+ nEvalConstraints += jacNumCons.getNumCallsToUserFunction();
+
+ // Fill in the missing rows.
+ int nxtInNumJ = 0;
+ for (unsigned i=0; i < needNumericalJacobian.size(); ++i) {
+ J(firstEqn[i],0,nEqns[i],n) = numJ(nxtInNumJ,0,nEqns[i],n);
+ nxtInNumJ += nEqns[i];
+ }
+
+ }
+
+ //cout << "J=" << J;
+ return 0;
+ }
+
+ int getNumObjectiveEvals() const {return nEvalObjective;}
+ int getNumConstraintEvals() const {return nEvalConstraints;}
+ int getNumGradientEvals() const {return nEvalGradient;}
+ int getNumJacobianEvals() const {return nEvalJacobian;}
+
+ void resetStats() const { // stats are mutable
+ nEvalObjective=nEvalConstraints=nEvalGradient=nEvalJacobian=0;
+ }
+private:
+ const MultibodySystem& getSystem() const
+ { return assembler.getMultibodySystem(); }
+ const State& getInternalState() const
+ { return assembler.getInternalState(); }
+ int getNumFreeQs() const {return assembler.getNumFreeQs();}
+ QIndex getQIndexOfFreeQ(FreeQIndex fx) const
+ { return assembler.getQIndexOfFreeQ(fx);}
+ FreeQIndex getFreeQIndexOfQ(QIndex qx) const
+ { return assembler.getFreeQIndexOfQ(qx); }
+ Vector getFreeQsFromInternalState() const
+ { return assembler.getFreeQsFromInternalState(); }
+ void setInternalStateFromFreeQs(const Vector& freeQs) const
+ { assembler.setInternalStateFromFreeQs(freeQs); }
+
+ Assembler& assembler;
+
+ mutable int nEvalObjective;
+ mutable int nEvalConstraints;
+ mutable int nEvalGradient;
+ mutable int nEvalJacobian;
+};
+
+
+
+//------------------------------------------------------------------------------
+// ASSEMBLER
+//------------------------------------------------------------------------------
+Assembler::Assembler(const MultibodySystem& system)
+: system(system), accuracy(0), tolerance(0), // i.e., 1e-3, 1e-4
+ forceNumericalGradient(false), forceNumericalJacobian(false),
+ useRMSErrorNorm(false), alreadyInitialized(false),
+ asmSys(0), optimizer(0), nAssemblySteps(0), nInitializations(0)
+{
+ const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
+ matter.convertToEulerAngles(system.getDefaultState(),
+ internalState);
+ system.realizeModel(internalState);
+
+ // Make sure the System's Constraints are always present; this sets the
+ // weight to Infinity which makes us treat this as an assembly error
+ // rather than merely a goal; that can be changed by the user.
+ systemConstraints = adoptAssemblyError(new BuiltInConstraints());
+}
+
+
+Assembler::~Assembler() {
+ uninitialize();
+ // To be polite, and to show off, delete in reverse order of allocation
+ // (this is easier on the heap system).
+ Array_<AssemblyCondition*,AssemblyConditionIndex>::reverse_iterator p;
+ for (p = conditions.rbegin(); p != conditions.rend(); ++p)
+ delete *p;
+}
+
+
+AssemblyConditionIndex Assembler::
+adoptAssemblyError(AssemblyCondition* p) {
+ return adoptAssemblyGoal(p, Infinity);
+}
+
+AssemblyConditionIndex Assembler::
+adoptAssemblyGoal(AssemblyCondition* p, Real weight) {
+ SimTK_ERRCHK_ALWAYS(p != 0, "Assembler::adoptAssemblyGoal()",
+ "Null assembly condition pointer.");
+ SimTK_ERRCHK1_ALWAYS(weight >= 0, "Assembler::adoptAssemblyGoal()",
+ "Illegal assembly goal weight %g.", weight);
+
+ uninitialize();
+
+ const AssemblyConditionIndex acx(conditions.size());
+ assert(conditions.size() == weights.size());
+ p->setAssembler(*this, acx);
+ conditions.push_back(p);
+ weights.push_back(weight);
+ return acx;
+}
+
+
+void Assembler::initialize() const {
+ if (alreadyInitialized)
+ return;
+
+ ++nInitializations;
+
+ Array_<QIndex> toBeLocked;
+ reinitializeWithExtraQsLocked(toBeLocked);
+ alreadyInitialized = true;
+ return;
+
+ /*NOTREACHED*/
+ // TODO: This currently unused code would allow the Assembler to lock out
+ // variables that it thinks aren't worth bothering with. Needs real-world
+ // testing and probably some override options. And should there be a
+ // desperation mode where all variables are tried if we can't assemble
+ // with some of them removed?
+ Vector grad = abs(asmSys->calcCurrentGradient());
+ Real maxGrad = 0;
+ for (FreeQIndex fx(0); fx < grad.size(); ++fx)
+ maxGrad = std::max(maxGrad, grad[fx]);
+ if (maxGrad == 0) // no q does anything; probably no objective
+ maxGrad = Infinity; // no q will be kept for gradient purposes
+
+ Matrix jac = asmSys->calcCurrentJacobian();
+ Vector colNorm(getNumFreeQs());
+ Real maxJac = 0;
+ for (FreeQIndex fx(0); fx < grad.size(); ++fx) {
+ colNorm[fx] = jac(fx).norm();
+ maxJac = std::max(maxJac, colNorm[fx]);
+ }
+ if (maxJac == 0) // no q does anything; probably no constraints
+ maxJac = Infinity; // no q will be kept for Jacobian purposes
+
+ const Real QTol = SqrtEps;
+ const Real minGradAllowed = maxGrad*QTol;
+ const Real minJacAllowed = maxJac*QTol;
+ for (FreeQIndex fx(0); fx < grad.size(); ++fx)
+ if (grad[fx] < minGradAllowed && colNorm[fx] < minJacAllowed)
+ toBeLocked.push_back(getQIndexOfFreeQ(fx));
+
+ if (toBeLocked.size()) {
+ cout << "Reinitializing with these q's locked:\n";
+ cout << toBeLocked << endl;
+ reinitializeWithExtraQsLocked(toBeLocked);
+ alreadyInitialized = true;
+ }
+}
+
+void Assembler::reinitializeWithExtraQsLocked
+ (const Array_<QIndex>& toBeLocked) const {
+ uninitialize();
+
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+
+ system.realize(internalState, Stage::Instance);
+ const int nq = internalState.getNQ();
+
+ // Initialized locked q's to all those that the user locked, plus
+ // the extras.
+ q2FreeQ.resize(nq); // no q has an associated freeQ at this point
+
+ extraQsLocked = toBeLocked;
+ lockedQs.insert(extraQsLocked.begin(), extraQsLocked.end());
+
+ // Find all the mobilizers that have prescribed positions and lock
+ // all their q's.
+ for (MobodIndex mbx(0); mbx < matter.getNumBodies(); ++mbx) {
+ const MobilizedBody& mobod = matter.getMobilizedBody(mbx);
+ if (mobod.getQMotionMethod(internalState) == Motion::Free)
+ continue;
+ const QIndex q0 = mobod.getFirstQIndex(internalState);
+ const int nq = mobod.getNumQ(internalState);
+ for (int i=0; i<nq; ++i)
+ lockedQs.insert(QIndex(q0+i));
+ }
+
+ // Lock all the q's for locked mobilizers.
+ for (LockedMobilizers::const_iterator p = userLockedMobilizers.begin();
+ p != userLockedMobilizers.end(); ++p)
+ {
+ const MobilizedBody& mobod = matter.getMobilizedBody(*p);
+ const QIndex q0 = mobod.getFirstQIndex(internalState);
+ const int nq = mobod.getNumQ(internalState);
+ for (int i=0; i<nq; ++i)
+ lockedQs.insert(QIndex(q0+i));
+ }
+
+ // Next add in all the q's that were individually locked.
+ for (LockedQs::const_iterator p = userLockedQs.begin();
+ p != userLockedQs.end(); ++p)
+ {
+ const MobilizedBodyIndex mbx = p->first;
+ const MobilizedBody& mobod = matter.getMobilizedBody(mbx);
+ const QIndex q0 = mobod.getFirstQIndex(internalState);
+ const int nq = mobod.getNumQ(internalState);
+
+ const QSet& qs = p->second;
+ for (QSet::const_iterator qp = qs.begin(); qp != qs.end(); ++qp) {
+ const MobilizerQIndex q = *qp;
+ SimTK_ERRCHK3_ALWAYS(q < nq, "Assembler::initialize()",
+ "An attempt was made to lock q[%d] (numbering from 0) of"
+ " mobilized body %d but that mobilizer has only %d q(s).",
+ (int)q, (int)mbx, nq);
+ lockedQs.insert(QIndex(q0 + q));
+ }
+ }
+
+ const int nlockedq = (int)lockedQs.size();
+ const int nfreeq = std::max(nq - nlockedq, 0);
+
+ // Find all the free q's and fill in the maps from FreeQIndex
+ // to full QIndex, and from QIndex to FreeQIndex.
+ freeQ2Q.resize(nfreeq);
+ if (nlockedq) {
+ FreeQIndex nxtFree(0);
+ for (QIndex qx(0); qx < nq; ++qx) {
+ if (lockedQs.find(qx) == lockedQs.end()) {
+ q2FreeQ[qx] = nxtFree;
+ freeQ2Q[nxtFree++] = qx;
+ }
+ }
+ } else // all q's are free
+ for (QIndex qx(0); qx < nq; ++qx) {
+ q2FreeQ[qx] = FreeQIndex(qx);
+ freeQ2Q[FreeQIndex(qx)] = qx;
+ }
+
+ // If *any* of the free q's have a restricted range, we'll provide a
+ // (lower,upper) range value for *all* of them, using (-Inf,Inf) by
+ // default. It might turn out that all the restricted q's are currently
+ // locked so we'll hold off allocating the bounds arrays until we
+ // actually see a freeQ that's restricted.
+ bool foundAnyRestricted = false;
+ for (RestrictedQs::const_iterator p = userRestrictedQs.begin();
+ p != userRestrictedQs.end(); ++p)
+ {
+ const MobilizedBodyIndex mbx = p->first;
+ const MobilizedBody& mobod = matter.getMobilizedBody(mbx);
+ const QIndex q0 = mobod.getFirstQIndex(internalState);
+ const int nq = mobod.getNumQ(internalState);
+
+ // Run through each of the q's that was restricted for this mobilizer.
+ const QRanges& qranges = p->second;
+ for (QRanges::const_iterator qr = qranges.begin();
+ qr != qranges.end(); ++qr)
+ {
+ const MobilizerQIndex q = qr->first;
+ const Vec2& r = qr->second;
+
+ SimTK_ERRCHK3_ALWAYS(q < nq, "Assembler::initialize()",
+ "An attempt was made to restrict q[%d] (numbering from 0) of"
+ " mobilized body %d but that mobilizer has only %d q(s).",
+ (int)q, (int)mbx, nq);
+
+ const QIndex qx = QIndex(q0 + q);
+ const FreeQIndex fx = q2FreeQ[qx];
+ if (!fx.isValid())
+ continue; // this q is locked; no need to restrict it
+
+ if (!foundAnyRestricted) { // this is first one
+ lower.resize(nfreeq); lower = -Infinity; // allocate and initialize
+ upper.resize(nfreeq); upper = Infinity;
+ foundAnyRestricted = true;
+ }
+ lower[fx] = r[0];
+ upper[fx] = r[1];
+ }
+ }
+
+ system.realize(internalState, Stage::Position);
+
+ // Set up the lists of errors and goals based on the weights
+ // currently assigned to assembly conditions, and initialize the
+ // conditions as they are added.
+ errors.clear(); nTermsPerError.clear(); goals.clear();
+ assert(conditions.size() == weights.size());
+
+ int nErrorTerms = 0;
+ for (AssemblyConditionIndex acx(0); acx < conditions.size(); ++acx) {
+ assert(conditions[acx] != 0 && weights[acx] >= 0);
+ if (weights[acx] == 0)
+ continue;
+ conditions[acx]->initializeCondition();
+ if (weights[acx] == Infinity) {
+ const int n = conditions[acx]->getNumErrors(internalState);
+ if (n == 0)
+ continue; // never mind; no constraint errors
+ nErrorTerms += n;
+ errors.push_back(acx);
+ nTermsPerError.push_back(n);
+ } else
+ goals.push_back(acx);
+ }
+
+ // Allocate an AssemblerSystem which is in the form of an objective
+ // function for the SimTK::Optimizer class.
+ asmSys = new AssemblerSystem(*const_cast<Assembler*>(this));
+ asmSys->setNumEqualityConstraints(nErrorTerms);
+ if (lower.size())
+ asmSys->setParameterLimits(lower, upper);
+
+
+ // Optimizer will choose LBFGS for unconstrained (or just bounds-constrained)
+ // problems, InteriorPoint for constrained problems.
+ optimizer = new Optimizer(*asmSys
+ //,InteriorPoint
+ //,LBFGS
+ //,LBFGSB
+ );
+ //optimizer->useNumericalGradient(true); // of goals
+ //optimizer->useNumericalJacobian(true); // of errors
+
+ // The size of the limited memory history affects the various optimizers
+ // differently; I found this to be a good compromise. Smaller or larger
+ // can both cause degraded performance.
+ optimizer->setLimitedMemoryHistory(50);
+ optimizer->setDiagnosticsLevel(0);
+ optimizer->setMaxIterations(3000);
+}
+
+// Clean up all the mutable stuff; don't touch any user-set members.
+void Assembler::uninitialize() const {
+ if (!alreadyInitialized)
+ return;
+
+ alreadyInitialized = false;
+ nAssemblySteps = 0;
+ delete optimizer; optimizer = 0;
+ delete asmSys; asmSys = 0;
+ // Run through conditions in reverse order when uninitializing them;
+ // watch out: negative index not allowed so it is easier to use a reverse
+ // iterators.
+ Array_<AssemblyCondition*,AssemblyConditionIndex>::const_reverse_iterator p;
+ for (p = conditions.crbegin(); p != conditions.crend(); ++p)
+ (*p)->uninitializeCondition();
+ goals.clear();
+ nTermsPerError.clear();
+ errors.clear();
+ lower.clear(); upper.clear();
+ freeQ2Q.clear();
+ q2FreeQ.clear();
+ lockedQs.clear();
+ extraQsLocked.clear();
+}
+
+Real Assembler::calcCurrentGoal() const {
+ initialize();
+ return asmSys->calcCurrentGoal();
+}
+
+// Return norm of constraint errors, using the appropriate norm.
+Real Assembler::calcCurrentErrorNorm() const {
+ initialize();
+ const int nc = asmSys->getNumEqualityConstraints();
+ if (nc == 0) return 0;
+ const Vector errs = asmSys->calcCurrentErrors();
+ return useRMSErrorNorm
+ ? std::sqrt(~errs*errs / errs.size()) // RMS
+ : max(abs(errs)); // infinity norm
+}
+
+Real Assembler::assemble() {
+ initialize();
+
+ ++nAssemblySteps;
+
+ const int nfreeq = getNumFreeQs();
+ const int nqerr = internalState.getNQErr();
+
+
+ // std::cout << "assemble(): initial tol/goal is "
+ // << calcCurrentError() << "/" << calcCurrentGoal() << std::endl;
+
+ for (unsigned i=0; i < reporters.size(); ++i)
+ reporters[i]->handleEvent(internalState);
+
+ // First step: satisfy prescribed motion (exactly).
+ system.realize(internalState, Stage::Time);
+ system.prescribeQ(internalState);
+
+ // Optimize
+ Vector freeQs = getFreeQsFromInternalState();
+ // Use tolerance if there are any error conditions, else accuracy.
+ optimizer->setConvergenceTolerance(getAccuracyInUse());
+ optimizer->setConstraintTolerance(getErrorToleranceInUse());
+ try
+ { optimizer->optimize(freeQs); }
+ catch (const std::exception& e)
+ { setInternalStateFromFreeQs(freeQs);
+ system.realize(internalState, Stage::Position);
+ SimTK_THROW3(AssembleFailed,
+ (String("Optimizer failed with message: ") + e.what()).c_str(),
+ calcCurrentErrorNorm(), getErrorToleranceInUse());
+ }
+
+ // This will ensure that the internalState has its q's set to match the
+ // parameters.
+ setInternalStateFromFreeQs(freeQs);
+ system.realize(internalState, Stage::Position);
+
+ for (unsigned i=0; i < reporters.size(); ++i)
+ reporters[i]->handleEvent(internalState);
+
+ const Real tolAchieved = calcCurrentErrorNorm();
+ if (tolAchieved > getErrorToleranceInUse())
+ SimTK_THROW3(AssembleFailed,
+ "Unabled to achieve required assembly error tolerance.",
+ tolAchieved, getErrorToleranceInUse());
+
+ //std::cout << "assemble(): final tol/goal is "
+ // << calcCurrentError() << "/" << calcCurrentGoal() << std::endl;
+
+ return calcCurrentGoal();
+}
+
+
+Real Assembler::track(Real frameTime) {
+ initialize();
+
+ ++nAssemblySteps;
+
+ if (frameTime >= 0 && internalState.getTime() != frameTime) {
+ internalState.setTime(frameTime);
+ system.realize(internalState, Stage::Time);
+ // Satisfy prescribed motion (exactly).
+ system.prescribeQ(internalState);
+ }
+
+ const int nfreeq = getNumFreeQs();
+ const int nqerr = internalState.getNQErr();
+
+
+ // std::cout << "track(): initial tol/goal is "
+ // << calcCurrentError() << "/" << calcCurrentGoal() << std::endl;
+
+ // Optimize
+ Vector freeQs = getFreeQsFromInternalState();
+ optimizer->setConvergenceTolerance(getAccuracyInUse());
+ optimizer->setConstraintTolerance(getErrorToleranceInUse());
+ try
+ { optimizer->optimize(freeQs); }
+ catch (const std::exception& e)
+ { setInternalStateFromFreeQs(freeQs);
+ system.realize(internalState, Stage::Position);
+ SimTK_THROW3(TrackFailed,
+ (String("Optimizer failed with message: ") + e.what()).c_str(),
+ calcCurrentErrorNorm(), getErrorToleranceInUse());
+ }
+
+ // This will ensure that the internalState has its q's set to match the
+ // parameters.
+ // This will ensure that the internalState has its q's set to match the
+ // parameters.
+ setInternalStateFromFreeQs(freeQs);
+ system.realize(internalState, Stage::Position);
+
+ for (unsigned i=0; i < reporters.size(); ++i)
+ reporters[i]->handleEvent(internalState);
+
+ const Real tolAchieved = calcCurrentErrorNorm();
+ if (tolAchieved > getErrorToleranceInUse())
+ SimTK_THROW3(TrackFailed,
+ "Unabled to achieve required assembly error tolerance.",
+ tolAchieved, getErrorToleranceInUse());
+
+ //std::cout << "track(): final tol/goal is "
+ // << calcCurrentError() << "/" << calcCurrentGoal() << std::endl;
+
+ return calcCurrentGoal();
+}
+
+int Assembler::getNumGoalEvals() const
+{ return asmSys ? asmSys->getNumObjectiveEvals() : 0;}
+int Assembler::getNumErrorEvals() const
+{ return asmSys ? asmSys->getNumConstraintEvals() : 0;}
+int Assembler::getNumGoalGradientEvals() const
+{ return asmSys ? asmSys->getNumGradientEvals() : 0;}
+int Assembler::getNumErrorJacobianEvals() const
+{ return asmSys ? asmSys->getNumJacobianEvals() : 0;}
+int Assembler::getNumAssemblySteps() const
+{ return nAssemblySteps; }
+int Assembler::getNumInitializations() const
+{ return nInitializations; }
+void Assembler::resetStats() const
+{ if (asmSys) asmSys->resetStats();
+ nAssemblySteps = nInitializations = 0; }
+
+
+
+//------------------------------------------------------------------------------
+// MARKERS
+//------------------------------------------------------------------------------
+
+static const bool Weighted = true;
+static const Real MinimumOffset = 0;
+
+Vec3 Markers::findCurrentMarkerLocation(MarkerIx mx) const {
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+ const Marker& marker = getMarker(mx);
+ const MobilizedBody& mobod = matter.getMobilizedBody(marker.bodyB);
+ const State& state = getAssembler().getInternalState();
+ const Transform& X_GB = mobod.getBodyTransform(state);
+ return X_GB * marker.markerInB;
+}
+
+// goal = offset + 1/2 sum( wi * ri^2 ) / sum(wi) for WRMS
+int Markers::calcGoal(const State& state, Real& goal) const {
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+ goal = 0;
+ // Loop over each body that has one or more active markers.
+ Real wtot = 0;
+ PerBodyMarkers::const_iterator bodyp = bodiesWithMarkers.begin();
+ for (; bodyp != bodiesWithMarkers.end(); ++bodyp) {
+ const MobilizedBodyIndex mobodIx = bodyp->first;
+ const Array_<MarkerIx>& bodyMarkers = bodyp->second;
+ const MobilizedBody& mobod = matter.getMobilizedBody(mobodIx);
+ const Transform& X_GB = mobod.getBodyTransform(state);
+ assert(bodyMarkers.size());
+ // Loop over each marker on this body.
+ for (unsigned m=0; m < bodyMarkers.size(); ++m) {
+ const MarkerIx mx = bodyMarkers[m];
+ const Marker& marker = markers[mx];
+ assert(marker.bodyB == mobodIx); // better be on this body!
+ const Vec3& location = observations[getObservationIxForMarker(mx)];
+ if (location.isFinite()) { // skip NaNs
+ goal += marker.weight
+ * (X_GB*marker.markerInB - location).normSqr();
+ wtot += marker.weight;
+ }
+ }
+ }
+
+ if (Weighted)
+ goal /= wtot;
+
+ goal /= 2;
+ goal += MinimumOffset;
+
+ return 0;
+}
+// dgoal/dq = sum( wi * ri * dri/dq ) / sum(wi)
+// This calculation is modeled after Peter Eastman's gradient implementation
+// in ObservedPointFitter. It treats each marker position error as a potential
+// energy function whose negative spatial gradient would be a spatial force F.
+// We can then use Simbody's spatial force-to-generalized force method (using
+// -F instead of F) to obtain the gradient in internal coordinates.
+int Markers::calcGoalGradient(const State& state, Vector& gradient) const {
+ const int np = getNumFreeQs();
+ assert(gradient.size() == np);
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+
+ Vector_<SpatialVec> dEdR(matter.getNumBodies());
+ dEdR = SpatialVec(Vec3(0), Vec3(0));
+ // Loop over each body that has one or more active markers.
+ Real wtot = 0;
+ PerBodyMarkers::const_iterator bodyp = bodiesWithMarkers.begin();
+ for (; bodyp != bodiesWithMarkers.end(); ++bodyp) {
+ const MobilizedBodyIndex mobodIx = bodyp->first;
+ const Array_<MarkerIx>& bodyMarkers = bodyp->second;
+ const MobilizedBody& mobod = matter.getMobilizedBody(mobodIx);
+ const Transform& X_GB = mobod.getBodyTransform(state);
+ assert(bodyMarkers.size());
+ // Loop over each marker on this body.
+ for (unsigned m=0; m < bodyMarkers.size(); ++m) {
+ const MarkerIx mx = bodyMarkers[m];
+ const Marker& marker = markers[mx];
+ assert(marker.bodyB == mobodIx); // better be on this body!
+ const Vec3& location = observations[getObservationIxForMarker(mx)];
+ if (location.isFinite()) { // skip NaNs
+ const Vec3 nf = marker.weight
+ * (X_GB*marker.markerInB - location);
+ mobod.applyForceToBodyPoint(state, marker.markerInB, nf, dEdR);
+ wtot += marker.weight;
+ }
+ }
+ }
+ // Convert spatial forces dEdR to generalized forces dEdU.
+ Vector dEdU;
+ matter.multiplyBySystemJacobianTranspose(state, dEdR, dEdU);
+
+ if (Weighted)
+ dEdU /= wtot;
+
+ const int nq = state.getNQ();
+ if (np == nq) // gradient is full length
+ matter.multiplyByNInv(state, true, dEdU, gradient);
+ else { // calculate full gradient; extract the relevant parts
+ Vector fullGradient(nq);
+ matter.multiplyByNInv(state, true, dEdU, fullGradient);
+ for (Assembler::FreeQIndex fx(0); fx < np; ++fx)
+ gradient[fx] = fullGradient[getQIndexOfFreeQ(fx)];
+ }
+
+
+ return 0;
+}
+
+// TODO: We want the constraint version to minimize the same goal as above. But
+// there can never be more than six independent constraints on the pose of
+// a rigid body; this method should attempt to produce a minimal set so that
+// the optimizer doesn't have to figure it out.
+int Markers::calcErrors(const State& state, Vector& err) const
+{ return AssemblyCondition::calcErrors(state,err); } //TODO
+
+
+int Markers::calcErrorJacobian(const State& state, Matrix& jacobian) const
+{ return AssemblyCondition::calcErrorJacobian(state,jacobian); } //TODO
+int Markers::getNumErrors(const State& state) const
+{ return AssemblyCondition::getNumErrors(state); } //TODO
+
+// Run through all the Markers to find all the bodies that have at least one
+// active marker. For each of those bodies, we collect all its markers so that
+// we can process them all at once. Active markers are those whose weight is
+// greater than zero. Also, if we haven't been given any observation<->marker
+// correspondence, we're going to assume them map directly, with each
+// ObservationIx the same as its MarkerIx.
+int Markers::initializeCondition() const {
+ // Fill in missing observation information if needed.
+ if (observation2marker.empty()) {
+ const Array_<MarkerIx> zeroLength; // gcc doesn't like this as a temp
+ const_cast<Markers&>(*this).defineObservationOrder(zeroLength);
+ }
+
+ bodiesWithMarkers.clear();
+ for (MarkerIx mx(0); mx < markers.size(); ++mx) {
+ const Marker& marker = markers[mx];
+ if (hasObservation(mx) && marker.weight > 0)
+ bodiesWithMarkers[marker.bodyB].push_back(mx);
+ }
+ return 0;
+}
+
+// Throw away the bodiesWithMarkers map.
+void Markers::uninitializeCondition() const {
+ bodiesWithMarkers.clear();
+}
+
diff --git a/Simbody/src/Body.cpp b/Simbody/src/Body.cpp
new file mode 100644
index 0000000..9bf28fe
--- /dev/null
+++ b/Simbody/src/Body.cpp
@@ -0,0 +1,202 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+Private implementation of Body and its built-in subclasses. **/
+
+#include "SimTKcommon.h"
+#include "simbody/internal/common.h"
+#include "simbody/internal/Body.h"
+
+#include "BodyRep.h"
+
+namespace SimTK {
+
+//==============================================================================
+// BODY
+//==============================================================================
+bool Body::isEmptyHandle() const {return rep==0;}
+bool Body::isOwnerHandle() const {return rep==0 || rep->myHandle==this;}
+
+Body::~Body() {
+ if (isOwnerHandle()) delete rep;
+ rep=0;
+}
+
+// Copy constructor creates a new deep copy of the source object.
+Body::Body(const Body& src) : rep(0) {
+ if (src.rep) {
+ rep = src.rep->clone();
+ rep->setMyHandle(*this);
+ }
+}
+
+// Assignment puts a deep copy of the src Body's rep into the current handle.
+Body& Body::operator=(const Body& src) {
+ if (&src != this) {
+ if (isOwnerHandle()) delete rep;
+ rep=0;
+ if (src.rep) {
+ rep = src.rep->clone(); // create a new object
+ rep->setMyHandle(*this);
+ }
+ }
+ return *this;
+}
+
+Body::Body(const MassProperties& m) : rep(0) {
+ rep = new Body::Rigid::RigidRep(m);
+ rep->setMyHandle(*this);
+}
+
+
+int Body::addDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
+ return updRep().addDecoration(X_BD, g);
+}
+int Body::getNumDecorations() const
+{ return (int)getRep().decorations.size(); }
+const DecorativeGeometry& Body::getDecoration(int i) const
+{ return getRep().decorations[i]; }
+// Allow writable access on const Body since just a decoration.
+DecorativeGeometry& Body::updDecoration(int i) const
+{ return const_cast<DecorativeGeometry&>(getDecoration(i)); }
+
+
+int Body::addContactSurface(const Transform& X_BS,
+ const ContactSurface& shape) {
+ BodyRep& rep = updRep();
+ const int nxt = (int)rep.surfaces.size();
+ rep.surfaces.push_back(std::make_pair(X_BS,shape));
+ return nxt;
+}
+int Body::getNumContactSurfaces() const
+{ return (int)getRep().surfaces.size(); }
+const ContactSurface& Body::getContactSurface(int i) const
+{ return getRep().surfaces[i].second; }
+const Transform& Body::getContactSurfaceTransform(int i) const
+{ return getRep().surfaces[i].first; }
+ContactSurface& Body::updContactSurface(int i)
+{ return updRep().surfaces[i].second; }
+Transform& Body::updContactSurfaceTransform(int i)
+{ return updRep().surfaces[i].first; }
+
+const MassProperties& Body::getDefaultRigidBodyMassProperties() const {
+ return getRep().getDefaultRigidBodyMassProperties();
+}
+
+Body& Body::setDefaultRigidBodyMassProperties(const MassProperties& m) {
+ updRep().setDefaultRigidBodyMassProperties(m);
+ return *this;
+}
+
+
+
+//==============================================================================
+// BODY::RIGID
+//==============================================================================
+Body::Rigid::Rigid() {
+ rep = new RigidRep();
+ rep->setMyHandle(*this);
+}
+
+Body::Rigid::Rigid(const MassProperties& m) {
+ rep = new RigidRep(m);
+ rep->setMyHandle(*this);
+}
+
+bool Body::Rigid::isInstanceOf(const Body& b) {
+ return RigidRep::isA(b.getRep());
+}
+const Body::Rigid& Body::Rigid::downcast(const Body& b) {
+ assert(isInstanceOf(b));
+ return static_cast<const Rigid&>(b);
+}
+Body::Rigid& Body::Rigid::updDowncast(Body& b) {
+ assert(isInstanceOf(b));
+ return static_cast<Rigid&>(b);
+}
+const Body::Rigid::RigidRep& Body::Rigid::getRep() const {
+ return SimTK_DYNAMIC_CAST_DEBUG<const RigidRep&>(*rep);
+}
+Body::Rigid::RigidRep& Body::Rigid::updRep() {
+ return SimTK_DYNAMIC_CAST_DEBUG<RigidRep&>(*rep);
+}
+
+
+
+//==============================================================================
+// BODY::GROUND
+//==============================================================================
+Body::Ground::Ground() {
+ rep = new GroundRep();
+ rep->setMyHandle(*this);
+}
+
+bool Body::Ground::isInstanceOf(const Body& b) {
+ return GroundRep::isA(b.getRep());
+}
+const Body::Ground& Body::Ground::downcast(const Body& b) {
+ assert(isInstanceOf(b));
+ return static_cast<const Ground&>(b);
+}
+Body::Ground& Body::Ground::updDowncast(Body& b) {
+ assert(isInstanceOf(b));
+ return static_cast<Ground&>(b);
+}
+const Body::Ground::GroundRep& Body::Ground::getRep() const {
+ return SimTK_DYNAMIC_CAST_DEBUG<const GroundRep&>(*rep);
+}
+Body::Ground::GroundRep& Body::Ground::updRep() {
+ return SimTK_DYNAMIC_CAST_DEBUG<GroundRep&>(*rep);
+}
+
+
+
+//==============================================================================
+// BODY::MASSLESS
+//==============================================================================
+Body::Massless::Massless() {
+ rep = new MasslessRep();
+ rep->setMyHandle(*this);
+}
+
+bool Body::Massless::isInstanceOf(const Body& b) {
+ return MasslessRep::isA(b.getRep());
+}
+const Body::Massless& Body::Massless::downcast(const Body& b) {
+ assert(isInstanceOf(b));
+ return static_cast<const Massless&>(b);
+}
+Body::Massless& Body::Massless::updDowncast(Body& b) {
+ assert(isInstanceOf(b));
+ return static_cast<Massless&>(b);
+}
+const Body::Massless::MasslessRep& Body::Massless::getRep() const {
+ return SimTK_DYNAMIC_CAST_DEBUG<const MasslessRep&>(*rep);
+}
+Body::Massless::MasslessRep& Body::Massless::updRep() {
+ return SimTK_DYNAMIC_CAST_DEBUG<MasslessRep&>(*rep);
+}
+
+} // namespace SimTK
+
diff --git a/Simbody/src/BodyRep.h b/Simbody/src/BodyRep.h
new file mode 100644
index 0000000..6eee4cf
--- /dev/null
+++ b/Simbody/src/BodyRep.h
@@ -0,0 +1,193 @@
+#ifndef SimTK_SIMBODY_BODY_REP_H_
+#define SimTK_SIMBODY_BODY_REP_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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ *
+ * Private implementation of Body and its built-in subclasses.
+ */
+
+#include "SimTKcommon.h"
+#include "simbody/internal/common.h"
+#include "simbody/internal/Body.h"
+
+namespace SimTK {
+
+//==============================================================================
+// BODY REP
+//==============================================================================
+class Body::BodyRep {
+public:
+ BodyRep() : myHandle(0) {
+ }
+ virtual ~BodyRep() { }
+ virtual BodyRep* clone() const = 0;
+
+ virtual const MassProperties& getDefaultRigidBodyMassProperties() const = 0;
+ virtual void setDefaultRigidBodyMassProperties(const MassProperties&) = 0;
+
+ // Add a permanent ("Topological") piece of geometry which is permanently
+ // fixed to the body. Thus the 3D polygonal representation can be
+ // precalculated once and for all at Stage::Topology, then transformed at
+ // Stage::Position for display on a screen.
+ //
+ // This will make an internal copy of the supplied DecorativeGeometry. We
+ // can't fill in the MobilizedBodyIndex yet, but we apply the transform
+ // now to the saved copy so that the geometry we return later will be
+ // relative to the body frame only.
+ //
+ // Return an index that can be used to find this decoration later; the
+ // index must be the same in all copies of this Body.
+ int addDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
+ const int nxt = (int)decorations.size();
+ decorations.push_back(g); // make a new copy
+ DecorativeGeometry& myg = decorations.back();
+ myg.setTransform(X_BD*myg.getTransform());
+ return nxt;
+ }
+
+ void appendDecorativeGeometry(MobilizedBodyIndex id,
+ Array_<DecorativeGeometry>& geom) const
+ {
+ for (int i=0; i<(int)decorations.size(); ++i) {
+ geom.push_back(decorations[i]);
+ geom.back().setBodyId(id);
+ }
+ }
+
+
+
+ void setMyHandle(Body& h) {myHandle = &h;}
+ const Body& getMyBodyHandle() const {assert(myHandle); return *myHandle;}
+ void clearMyHandle() {myHandle=0;}
+private:
+ friend class Body;
+ Body* myHandle;
+
+ // TOPOLOGY "STATE" VARIABLES
+ Array_<std::pair<Transform,ContactSurface> > surfaces;
+ Array_<DecorativeGeometry> decorations;
+};
+
+
+
+//==============================================================================
+// RIGID REP
+//==============================================================================
+class Body::Rigid::RigidRep : public Body::BodyRep {
+public:
+ RigidRep()
+ : BodyRep(), defaultMassProperties(1,Vec3(0),Inertia(1))
+ {
+ }
+ explicit RigidRep(const MassProperties& m) : BodyRep(), defaultMassProperties(m) {
+ }
+ const MassProperties& getDefaultRigidBodyMassProperties() const {
+ return defaultMassProperties;
+ }
+ void setDefaultRigidBodyMassProperties(const MassProperties& m) {
+ defaultMassProperties = m;
+ }
+ RigidRep* clone() const {
+ return new RigidRep(*this);
+ }
+ const Body::Rigid& getMyRigidBodyHandle() const {
+ return static_cast<const Body::Rigid&>(getMyBodyHandle());
+ }
+
+ SimTK_DOWNCAST(RigidRep, BodyRep);
+private:
+ MassProperties defaultMassProperties;
+};
+
+
+
+//==============================================================================
+// GROUND REP
+//==============================================================================
+class Body::Ground::GroundRep : public Body::BodyRep {
+public:
+ GroundRep()
+ : BodyRep(), infiniteMassProperties(Infinity, Vec3(0), Inertia(Infinity))
+ {
+ }
+ GroundRep* clone() const {
+ return new GroundRep(*this);
+ }
+ const MassProperties& getDefaultRigidBodyMassProperties() const {
+ return infiniteMassProperties;
+ }
+
+ void setDefaultRigidBodyMassProperties(const MassProperties&) {
+ SimTK_THROW1(Exception::Cant, "You can't change Ground's mass properties!");
+ }
+
+ const Body::Ground& getMyGroundBodyHandle() const {
+ return static_cast<const Body::Ground&>(getMyBodyHandle());
+ }
+
+ SimTK_DOWNCAST(GroundRep, BodyRep);
+private:
+ const MassProperties infiniteMassProperties;
+};
+
+
+
+//==============================================================================
+// MASSLESS REP
+//==============================================================================
+class Body::Massless::MasslessRep : public Body::BodyRep {
+public:
+ MasslessRep()
+ : BodyRep(), zeroMassProperties(0, Vec3(0), Inertia(0))
+ {
+ }
+ MasslessRep* clone() const {
+ return new MasslessRep(*this);
+ }
+ const MassProperties& getDefaultRigidBodyMassProperties() const {
+ return zeroMassProperties;
+ }
+
+ void setDefaultRigidBodyMassProperties(const MassProperties&) {
+ SimTK_THROW1(Exception::Cant, "You can't change a massless body's mass properties!");
+ }
+
+ const Body::Massless& getMyMasslessBodyHandle() const {
+ return static_cast<const Body::Massless&>(getMyBodyHandle());
+ }
+
+ SimTK_DOWNCAST(MasslessRep, BodyRep);
+private:
+ const MassProperties zeroMassProperties;
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_BODY_REP_H_
+
+
+
diff --git a/Simbody/src/CablePath.cpp b/Simbody/src/CablePath.cpp
new file mode 100644
index 0000000..0417a25
--- /dev/null
+++ b/Simbody/src/CablePath.cpp
@@ -0,0 +1,1673 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Ian Stavness *
+ * Contributors: Andreas Scholz *
+ * *
+ * 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/MultibodySystem.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "simbody/internal/CableTrackerSubsystem.h"
+#include "simbody/internal/CablePath.h"
+
+#include "CablePath_Impl.h"
+#include "CableTrackerSubsystem_Impl.h"
+
+#include <cassert>
+#include <iostream>
+using std::cout; using std::endl;
+using namespace SimTK;
+
+//==============================================================================
+// PATH INSTANCE INFO / POS ENTRY / VEL ENTRY
+//==============================================================================
+
+PathInstanceInfo::PathInstanceInfo
+ (const Array_<CableObstacle,CableObstacleIndex>& obstacles)
+: mapObstacleToSurface(obstacles.size()),
+ mapSurfaceToObstacle(0),
+ obstacleDisabled(obstacles.size(),false),
+ obstaclePose(obstacles.size())
+{
+ SurfaceObstacleIndex next(0);
+ for (CableObstacleIndex ox(0); ox < obstacles.size(); ++ox) {
+ const CableObstacle::Impl& obs = obstacles[ox].getImpl();
+ if (obs.isDisabledByDefault())
+ obstacleDisabled[ox] = true;
+
+ if (obs.getNumCoordsPerContactPoint() > 0) { // a surface
+ mapObstacleToSurface[ox] = next++;
+ mapSurfaceToObstacle.push_back(ox);
+ assert(mapSurfaceToObstacle.size() == next);
+ }
+ obstaclePose[ox]=obs.getDefaultPoseOnBody();
+ }
+}
+
+std::ostream& operator<<(std::ostream& o, const PathInstanceInfo& info) {
+ o << "PathInstanceInfo nObs=" << info.getNumObstacles()
+ << " nSurf=" << info.getNumSurfaceObstacles() << endl;
+ o << " obs2surf: " << info.mapObstacleToSurface << endl;
+ o << " surf2obs: " << info.mapSurfaceToObstacle << endl;
+ o << " disabled=";
+ for (CableObstacleIndex ox(0); ox < info.obstacleDisabled.size(); ++ox)
+ o << " " << String(info.obstacleDisabled[ox]);
+ o << "\n pose: ";
+ for (CableObstacleIndex ox(0); ox < info.obstaclePose.size(); ++ox)
+ o << " " << info.obstaclePose[ox].p();
+ return o << endl;
+}
+
+std::ostream& operator<<(std::ostream& o, const PathPosEntry& ppe) {
+ cout << "PathPosEntry: length=" << ppe.length << endl;
+ cout << "mapToActive: " << ppe.mapToActive << endl;
+ cout << "mapToActiveSurface: " << ppe.mapToActiveSurface << endl;
+ cout << "mapToCoords: " << ppe.mapToCoords << endl;
+ cout << "eIn_G: " << ppe.eIn_G << endl;
+ cout << "Fu_GB: " << ppe.Fu_GB << endl;
+ cout << "witnesses=" << ppe.witnesses << endl;
+
+ cout << "geodesics lengths=";
+ for (ActiveSurfaceIndex asx(0); asx < ppe.geodesics.size(); ++asx)
+ cout << ppe.geodesics[asx].getLength() << " ";
+ cout << endl;
+ cout << "x=" << ppe.x << endl;
+ cout << "err=" << ppe.err << endl;
+ cout << "J dims=" << ppe.J.nrow() << " x " << ppe.J.ncol() << endl;
+ return o;
+}
+
+std::ostream& operator<<(std::ostream& o, const PathVelEntry& entry) {
+ return o;
+}
+
+
+//==============================================================================
+// CABLE PATH
+//==============================================================================
+
+CablePath::CablePath
+ (CableTrackerSubsystem& cables,
+ const MobilizedBody& originBody,
+ const Vec3& defaultOriginPoint,
+ const MobilizedBody& terminationBody,
+ const Vec3& defaultTerminationPoint) : impl(0)
+{
+ impl = new Impl(cables);
+ impl->referenceCount = 1;
+ impl->index = cables.updImpl().adoptCablePath(*this);
+ CableObstacle::ViaPoint(*this, originBody, defaultOriginPoint);
+ CableObstacle::ViaPoint(*this, terminationBody, defaultTerminationPoint);
+}
+
+CablePath::CablePath
+ (CableTrackerSubsystem& cables,
+ const MobilizedBody& originBody,
+ const MobilizedBody& terminationBody) : impl(0)
+{ // Invoke above constructor.
+ new(this) CablePath(cables, originBody, Vec3(0), terminationBody, Vec3(0));
+}
+
+// Copy constructor is shallow and reference counted.
+CablePath::CablePath(const CablePath& src) : impl(src.impl)
+{ if (impl) ++impl->referenceCount; }
+
+// Copy assignment is shallow and reference counted.
+CablePath& CablePath::operator=(const CablePath& src) {
+ if (&src != this) {
+ clear();
+ if (src.impl) {
+ impl = src.impl;
+ ++impl->referenceCount;
+ }
+ }
+ return *this;
+}
+
+// This is used to implement the destructor and copy assignment.
+void CablePath::clear() {
+ if (impl) {
+ if (--impl->referenceCount == 0)
+ delete impl;
+ impl = 0;
+ }
+}
+
+void CablePath::solveForInitialCablePath(State& state) const
+{ getImpl().solveForInitialCablePath(state); }
+
+int CablePath::getNumObstacles() const
+{ return getImpl().obstacles.size(); }
+
+const CableObstacle& CablePath::
+getObstacle(CableObstacleIndex obstacleIx) const
+{ return getImpl().obstacles[obstacleIx]; }
+
+Real CablePath::getCableLength(const State& state) const
+{ return getImpl().getCableLength(state); }
+
+Real CablePath::getCableLengthDot(const State& state) const
+{ return getImpl().getCableLengthDot(state); }
+
+void CablePath::applyBodyForces(const State& state, Real tension,
+ Vector_<SpatialVec>& bodyForcesInG) const
+{ getImpl().applyBodyForces(state,tension,bodyForcesInG); }
+
+Real CablePath::calcCablePower(const State& state, Real tension) const
+{ return getImpl().calcCablePower(state,tension); }
+
+Real CablePath::getIntegratedCableLengthDot(const State& state) const
+{ return getImpl().getIntegratedLengthDot(state); }
+
+void CablePath::setIntegratedCableLengthDot(State& state, Real value) const
+{ getImpl().setIntegratedLengthDot(state, value); }
+
+
+//==============================================================================
+// CABLE PATH :: IMPL
+//==============================================================================
+
+//------------------------------------------------------------------------------
+// APPLY BODY FORCES
+//------------------------------------------------------------------------------
+void CablePath::Impl::
+applyBodyForces(const State& state, Real tension,
+ Vector_<SpatialVec>& bodyForcesInG) const
+{
+ if (tension <= 0) return;
+
+ const PathPosEntry& ppe = getPosEntry(state);
+ for (CableObstacleIndex ox(0); ox < obstacles.size(); ++ox) {
+ const ActiveObstacleIndex ax = ppe.mapToActive[ox];
+ if (!ax.isValid()) // exclude disabled and inactive obstacles
+ continue;
+ const CableObstacle::Impl& obs = obstacles[ox].getImpl();
+ const MobilizedBody& B = obs.getMobilizedBody();
+ const SpatialVec& Fu_GB = ppe.Fu_GB[ax];
+ B.applyBodyForce(state, tension*Fu_GB, bodyForcesInG);
+ }
+}
+
+
+
+//------------------------------------------------------------------------------
+// SOLVE FOR INITIAL CABLE PATH
+//------------------------------------------------------------------------------
+// TODO -- this is a stub that does nothing.
+void CablePath::Impl::
+solveForInitialCablePath(State& state) const {
+ const PathInstanceInfo& instInfo = getInstanceInfo(state);
+
+ // These state variables are presumed to be uninitialized here.
+ PathPosEntry& ppe = updPrevPosEntry(state);
+ PathVelEntry& pve = updPrevVelEntry(state);
+}
+
+//------------------------------------------------------------------------------
+// REALIZE TOPOLOGY
+//------------------------------------------------------------------------------
+void CablePath::Impl::
+realizeTopology(State& state) {
+ // Initialize instance info from defaults.
+ PathInstanceInfo instInfo(obstacles);
+
+ cout << "In realizeTopology(): " << instInfo << endl;
+
+ // Allocate and initialize instance state variable.
+ instanceInfoIx = cables->allocateDiscreteVariable(state,
+ Stage::Instance, new Value<PathInstanceInfo>(instInfo));
+
+ // Allocate continuous variable in which to integrate Ldot; this is
+ // useful as a sanity check since we should have L0+integ(Ldot)=L(t) where
+ // L0 is the initial length of the cable.
+ Vector initz(1, Real(0));
+ integratedLengthDotIx = cables->allocateZ(state, initz);
+
+ // Allocate cache entries for position and velocity calculations.
+ Value<PathPosEntry>* posEntry = new Value<PathPosEntry>();
+ posEntry->upd().setNumObstacles(instInfo.getNumObstacles(),
+ instInfo.getNumSurfaceObstacles());
+ posEntryIx = cables->allocateAutoUpdateDiscreteVariable(state,
+ Stage::Velocity, // invalidates Velocity
+ posEntry, // takes over ownership of this Value
+ Stage::Position); // update depends on positions
+
+ Value<PathVelEntry>* velEntry = new Value<PathVelEntry>();
+ velEntry->upd().setNumObstacles(instInfo.getNumObstacles(),
+ instInfo.getNumSurfaceObstacles());
+ velEntryIx = cables->allocateAutoUpdateDiscreteVariable(state,
+ Stage::Dynamics, // invalidates Dynamics (forces)
+ velEntry, // takes over ownership of this Value
+ Stage::Velocity); // update depends on velocities
+
+ // Ask the System to give us some EventIds, and ask the State for some
+ // slots for the witness functions.
+ const int nSurfaces = instInfo.getNumSurfaceObstacles();
+ mapEventIdToObstacle.clear(); // this is an std::map, not an array
+ for (SurfaceObstacleIndex sox(0); sox < nSurfaces; ++sox) {
+ const EventId id = cables->getSystem().getDefaultSubsystem()
+ .createEventId(cables->getMySubsystemIndex(), state);
+ mapEventIdToObstacle[id] = instInfo.mapSurfaceToObstacle[sox];
+ }
+
+ // Every surface obstacle gets an event witness function that we can use
+ // to watch the cable lift off of or drop down onto the obstacle.
+ eventIx.invalidate();
+ if (nSurfaces) eventIx = cables->allocateEventTriggersByStage
+ (state, Stage::Position, nSurfaces);
+}
+
+//------------------------------------------------------------------------------
+// REALIZE INSTANCE
+//------------------------------------------------------------------------------
+// We now know which of the obstacles are enabled. Initialize the state
+// variables assuming all enabled obstacles are active.
+void CablePath::Impl::
+realizeInstance(const State& state) const {
+ const PathInstanceInfo& instInfo = getInstanceInfo(state);
+ PathPosEntry& ppe = updPosEntry(state);
+ PathVelEntry& pve = updVelEntry(state);
+
+ assert(!instInfo.obstacleDisabled.front()); // origin
+ assert(!instInfo.obstacleDisabled.back()); // termination
+
+ ActiveObstacleIndex nActive(0);
+ ActiveSurfaceIndex nActiveSurface(0);
+ int nx = 0; // num unknowns
+
+ Array_<Real> xInit;
+ for (CableObstacleIndex ox(0); ox < obstacles.size(); ++ox) {
+ ppe.mapToActive[ox].invalidate();
+ ppe.mapToActiveSurface[ox].invalidate();
+ ppe.mapToCoords[ox] = -1;
+
+ if (instInfo.obstacleDisabled[ox]) {
+ continue; // skip disabled obstacles
+ }
+
+ // This obstacle is enabled.
+
+ ppe.mapToActive[ox] = nActive++;
+ const CableObstacle::Impl& obs = obstacles[ox].getImpl();
+ const int d = obs.getNumCoordsPerContactPoint();
+ if (d == 0) {
+ // This obstacle is a via point (or an end point).
+ continue; // skip via points
+ }
+
+ // This obstacle is a surface obstacle.
+
+ // Assuming here that any enabled surface is active.
+ ppe.mapToActiveSurface[ox] = nActiveSurface++;
+
+ const CableObstacle::Surface::Impl& surf =
+ SimTK_DYNAMIC_CAST_DEBUG<const CableObstacle::Surface::Impl&>(obs);
+
+ ppe.mapToCoords[ox] = nx; // first index in x slot
+ nx += 2*d; // points P and Q need coords xP and xQ
+
+ Vec3 xPhint(0), xQhint(0);
+ if (surf.hasContactPointHints())
+ surf.getContactPointHints(xPhint,xQhint);
+ xInit.insert(xInit.end(), &xPhint[0], &xPhint[0]+d);
+ xInit.insert(xInit.end(), &xQhint[0], &xQhint[0]+d);
+ }
+
+ ppe.initialize(nActive, nActiveSurface, nx);
+ pve.initialize(nActive, nActiveSurface, nx);
+
+ for (int i=0; i<nx; ++i)
+ ppe.x[i] = xInit[i];
+
+ // For disabled (TODO: inactive) obstacles, fill in the initial guess
+ // at the closest-point-to-path.
+ for (CableObstacleIndex ox(0); ox < obstacles.size(); ++ox) {
+ if (!instInfo.obstacleDisabled[ox])
+ continue; // skip enabled obstacles
+
+ //TODO: for now disabled surface is treated as inactive surface
+ //and needs its closest point initialized somehow.
+ const CableObstacle::Impl& obs = obstacles[ox].getImpl();
+ const int d = obs.getNumCoordsPerContactPoint();
+ if (d == 0) continue; // just a via point
+ // Obstacle is a surface.
+ const SurfaceObstacleIndex sox = instInfo.mapObstacleToSurface[ox];
+ const CableObstacle::Surface::Impl& surf =
+ SimTK_DYNAMIC_CAST_DEBUG<const CableObstacle::Surface::Impl&>(obs);
+ Vec3 xPhint(0), xQhint(0);
+ if (surf.hasContactPointHints())
+ surf.getContactPointHints(xPhint,xQhint);
+ ppe.closestSurfacePoint[sox] = xPhint;
+ }
+ cout << "INIT PE=" << ppe << endl;
+ cout << "INIT VE=" << pve << endl;
+}
+
+
+
+//------------------------------------------------------------------------------
+// REALIZE POSITION
+//------------------------------------------------------------------------------
+void CablePath::Impl::
+realizePosition(const State& state) const {
+ ensurePositionKinematicsCalculated(state);
+}
+
+
+
+//------------------------------------------------------------------------------
+// REALIZE VELOCITY
+//------------------------------------------------------------------------------
+void CablePath::Impl::
+realizeVelocity(const State& state) const {
+ ensureVelocityKinematicsCalculated(state);
+}
+
+
+
+//------------------------------------------------------------------------------
+// REALIZE ACCELERATION
+//------------------------------------------------------------------------------
+void CablePath::Impl::
+realizeAcceleration(const State& state) const {
+ const PathVelEntry& pve = getVelEntry(state);
+
+ cables->updZDot(state)[integratedLengthDotIx] = pve.lengthDot;
+}
+
+
+
+//------------------------------------------------------------------------------
+// CALC EVENT TRIGGER INFO
+//------------------------------------------------------------------------------
+// The integrators call this during initialization, *after* realize(Instance).
+void CablePath::Impl::
+calcEventTriggerInfo(const State& state, Array_<EventTriggerInfo>& info) const
+{
+ std::cout << "In CablePath::Impl::calcEventTriggerInfo()\n";
+
+ for (std::map<EventId,CableObstacleIndex>::const_iterator p =
+ mapEventIdToObstacle.begin();
+ p != mapEventIdToObstacle.end(); ++p)
+ {
+ EventTriggerInfo einfo;
+ einfo.setEventId(p->first);
+ einfo.setTriggerOnFallingSignTransition(true); // OK active->inactive
+ einfo.setTriggerOnRisingSignTransition(false); // TODO: debugging
+ einfo.setRequiredLocalizationTimeWindow(Real(0.1)); //10% of time scale
+ info.push_back(einfo);
+ }
+}
+
+
+
+//------------------------------------------------------------------------------
+// HANDLE EVENTS
+//------------------------------------------------------------------------------
+void CablePath::Impl::handleEvents
+ (State& state, Event::Cause cause, const Array_<EventId>& eventIds,
+ const HandleEventsOptions& options, HandleEventsResults& results) const
+{
+ std::cout << "In CablePath::Impl::handleEvents() cause="
+ << Event::getCauseName(cause) << std::endl;
+ std::cout << "EventIds: " << eventIds << std::endl;
+
+
+ const PathInstanceInfo& instInfo = getInstanceInfo(state);
+ const int nObs = instInfo.getNumObstacles();
+ PathPosEntry& currPPE = updPosEntry(state);
+
+ // Capture the current path segments before we invalidate the state.
+ // TODO: won't be a problem when we're doing this in PPE rather than
+ // by disabling.
+ std::map<CableObstacleIndex, Vec3> r_SQp;
+ std::map<CableObstacleIndex, Vec3> r_SPn;
+ for (unsigned i=0; i < eventIds.size(); ++i) {
+ std::map<EventId,CableObstacleIndex>::const_iterator p =
+ mapEventIdToObstacle.find(eventIds[i]);
+ if (p==mapEventIdToObstacle.end())
+ continue;
+ const CableObstacleIndex ox = p->second;
+ if (instInfo.obstacleDisabled[ox]) {
+ Vec3 rSQp, rSPn;
+ findPathSegmentForObstacle(state, instInfo, currPPE, ox,
+ rSQp, rSPn);
+ r_SQp[ox] = rSQp;
+ r_SPn[ox] = rSPn;
+ }
+ }
+
+ // Also capture the xP and xQ locations for currently active objects
+ // so we can restore those for the objects that don't change status.
+ // And save previous geodesics.
+ Array_<Vec3,CableObstacleIndex> xP(nObs,Vec3(NaN));
+ Array_<Vec3,CableObstacleIndex> xQ(nObs,Vec3(NaN));
+ Array_<Geodesic,CableObstacleIndex> geodesics(nObs);
+ for (CableObstacleIndex ox(0); ox < nObs; ++ox) {
+ const int xSlot = currPPE.mapToCoords[ox];
+ if (xSlot < 0) continue;
+ xP[ox] = Vec3::getAs(&currPPE.x[xSlot]);
+ xQ[ox] = Vec3::getAs(&currPPE.x[xSlot+3]);
+
+ const ActiveSurfaceIndex asx = currPPE.mapToActiveSurface[ox];
+ if (!asx.isValid()) continue;
+ geodesics[ox] = currPPE.geodesics[asx];
+ }
+
+
+ // We're going to modify the *State* variable here. The state is the
+ // one used as "previous" when evaluating the current path, which is what
+ // triggered the event. We'll modify the cache entry to change the
+ // obstacle activation, then write the modified entry onto the state.
+ PathPosEntry& prevPPE = updPrevPosEntry(state);
+ std::cout << "PrevPPE in handler=\n" << prevPPE << std::endl;
+ std::cout << "CurrPPE in handler=\n" << currPPE << std::endl;
+
+ // Look through the triggered events to see if any of the event ids
+ // belong to this cable path. If so, activate or deactive the corresponding
+ // surface obstacle.
+ for (unsigned i=0; i < eventIds.size(); ++i) {
+ std::map<EventId,CableObstacleIndex>::const_iterator p =
+ mapEventIdToObstacle.find(eventIds[i]);
+ if (p==mapEventIdToObstacle.end())
+ continue;
+ const CableObstacleIndex ox = p->second;
+ const SurfaceObstacleIndex sox = instInfo.mapObstacleToSurface[ox];
+ const CableObstacle::Surface::Impl& obs =
+ SimTK_DYNAMIC_CAST_DEBUG<const CableObstacle::Surface::Impl&>
+ (getObstacleImpl(ox));
+ if (instInfo.obstacleDisabled[ox]) {
+ std::cout << "ENABLE OBSTACLE " << ox << std::endl;
+ // Grab points before we invalidate the state.
+ Vec3 rSQp = r_SQp[ox], rSPn = r_SPn[ox];
+ updInstanceInfo(state).obstacleDisabled[ox] = false;
+ cables->getSystem().realize(state, Stage::Instance);
+ // now it's active
+ const ActiveSurfaceIndex asx = currPPE.mapToActiveSurface[ox];
+ const int xSlot = currPPE.mapToCoords[ox];
+ assert(xSlot >= 0); // Should have had coordinates assigned
+ const UnitVec3 d(rSPn-rSQp);
+ const Vec3 P = currPPE.closestSurfacePoint[sox] /*- 1e-3*d*/;
+ const Vec3 Q = currPPE.closestSurfacePoint[sox] /*+ 1e-3*d*/;
+ xP[ox] = P;
+ xQ[ox] = Q;
+ const ContactGeometry& geom = obs.getContactGeometry();
+ Geodesic zeroLength;
+ geom.makeStraightLineGeodesic(P,Q,d, GeodesicOptions(),zeroLength);
+ geodesics[ox] = zeroLength;
+ } else {
+ // Save the point of last contact as the initial guess for
+ // closest point tracking.
+ Vec3 P_S, Q_S; // these will be almost the same point
+ obs.getContactPointsOnObstacle(state,instInfo,currPPE,P_S,Q_S);
+ currPPE.closestSurfacePoint[sox] = (P_S+Q_S)/2;
+ currPPE.closestPathPoint[sox] = (P_S+Q_S)/2;
+ std::cout << "DISABLE OBSTACLE " << ox << std::endl;
+ updInstanceInfo(state).obstacleDisabled[ox] = true;
+ cables->getSystem().realize(state, Stage::Instance);
+ }
+
+ // Set coordinates xP and xQ to their former values or to their
+ // initial value if a surface was enabled.
+ for (CableObstacleIndex ox(0); ox < nObs; ++ox) {
+ const int xSlot = currPPE.mapToCoords[ox];
+ if (xSlot < 0) continue;
+ const Vec3& P = xP[ox];
+ const Vec3& Q = xQ[ox];
+ SimTK_ASSERT1_ALWAYS(!P.isNaN() && !Q.isNaN(),
+ "CablePath::Impl::handleEvents(): "
+ "saved point for obstacle %d was NaN.", (int)ox);
+ Vec3::updAs(&currPPE.x[xSlot]) = P;
+ Vec3::updAs(&currPPE.x[xSlot+3]) = Q;;
+
+ const ActiveSurfaceIndex asx = currPPE.mapToActiveSurface[ox];
+ if (!asx.isValid()) continue;
+ currPPE.geodesics[asx] = geodesics[ox];
+ }
+ currPPE.witnesses[sox] = 0;
+ prevPPE = currPPE; // TODO
+ updPrevVelEntry(state) = updVelEntry(state); // don't force computation
+ }
+
+ std::cout << "Final CurrPPE in handler=\n" << currPPE << std::endl;
+
+ results.setExitStatus(HandleEventsResults::Succeeded);
+}
+
+//------------------------------------------------------------------------------
+// FIND PATH SEGMENT FOR OBSTACLE
+//------------------------------------------------------------------------------
+// Private helper method for finding this object's path segment (the segment
+// between the previous and next obstacles).
+void CablePath::Impl::
+findPathSegmentForObstacle
+ (const State& state, const PathInstanceInfo& instInfo,
+ const PathPosEntry& ppe, CableObstacleIndex ox,
+ Vec3& Qprev_S, Vec3& Pnext_S) const
+{
+ const CableObstacleIndex prevOx = ppe.findPrevActiveObstacle(ox);
+ const CableObstacleIndex nextOx = ppe.findNextActiveObstacle(ox);
+ assert(prevOx.isValid() && nextOx.isValid());
+
+ const CableObstacle::Surface::Impl& thisObs =
+ SimTK_DYNAMIC_CAST_DEBUG<const CableObstacle::Surface::Impl&>
+ (getObstacleImpl(ox));
+ const CableObstacle::Impl& prevObs = getObstacleImpl(prevOx);
+ const CableObstacle::Impl& nextObs = getObstacleImpl(nextOx);
+
+ // We're going to work in the body frames Bprev, Bthis, Bnext and
+ // then reexpress in S when we get to the "in" and "out" vectors.
+
+ // Abbreviate Bp=Bprev, Bn=Bnext, B=Bthis.
+ const MobilizedBody& Bp = prevObs.getMobilizedBody();
+ const MobilizedBody& B = thisObs.getMobilizedBody();
+ const MobilizedBody& Bn = nextObs.getMobilizedBody();
+
+ const Rotation R_BBp = Bp.findBodyRotationInAnotherBody(state, B);
+ const Rotation R_BBn = Bn.findBodyRotationInAnotherBody(state, B);
+
+ const Rotation& R_BpSp = prevObs.getObstaclePoseOnBody(state,instInfo).R();
+ const Transform& X_BS = thisObs.getObstaclePoseOnBody(state,instInfo);
+ const Rotation& R_BS = X_BS.R();
+ const Rotation& R_BnSn = nextObs.getObstaclePoseOnBody(state,instInfo).R();
+ const Rotation R_SB = ~R_BS;
+ const Rotation R_SSp = R_SB*R_BBp*R_BpSp;
+ const Rotation R_SSn = R_SB*R_BBn*R_BnSn;
+
+ Vec3 Pp, Qp, Pn, Qn; // points in local body frames
+ prevObs.getContactStationsOnBody(state, instInfo, ppe, Pp, Qp);
+ nextObs.getContactStationsOnBody(state, instInfo, ppe, Pn, Qn);
+
+ // Find prev and next points measured from & expressed in Bthis.
+ const Vec3 r_BQp = Bp.findStationLocationInAnotherBody(state, Qp, B);
+ const Vec3 r_BPn = Bn.findStationLocationInAnotherBody(state, Pn, B);
+
+ // Now shift into S frame.
+ Qprev_S = ~X_BS*r_BQp;
+ Pnext_S = ~X_BS*r_BPn;
+}
+
+//------------------------------------------------------------------------------
+// ENSURE POSITION KINEMATICS CALCULATED
+//------------------------------------------------------------------------------
+void CablePath::Impl::
+ensurePositionKinematicsCalculated(const State& state) const {
+ if (cables->isDiscreteVarUpdateValueRealized(state, posEntryIx))
+ return;
+
+ const PathInstanceInfo& instInfo = getInstanceInfo(state);
+ const PathPosEntry& prevPPE = getPrevPosEntry(state);
+ PathPosEntry& ppe = updPosEntry(state);
+
+ solveForPathPoints(state, instInfo, ppe);
+
+
+ //TODO: combine with earlier loop
+ for (SurfaceObstacleIndex sox(0); sox < instInfo.getNumSurfaceObstacles();
+ ++sox)
+ {
+ const CableObstacleIndex ox = instInfo.mapSurfaceToObstacle[sox];
+ if (ppe.mapToActiveSurface[ox].isValid())
+ continue; // skip active surface obstacles
+
+ Vec3 r_SQp, r_SPn;
+ findPathSegmentForObstacle(state, instInfo, ppe, ox, r_SQp, r_SPn);
+
+ const Vec3 r_QpPn = (r_SPn - r_SQp);
+ const UnitVec3 d_QpPn(r_QpPn); // direction of line
+
+ const CableObstacle::Surface::Impl& thisObs =
+ SimTK_DYNAMIC_CAST_DEBUG<const CableObstacle::Surface::Impl&>
+ (getObstacleImpl(ox));
+ const ContactGeometry& geo = thisObs.getContactGeometry();
+
+ Vec3 prevClosestPt = prevPPE.closestSurfacePoint[sox];
+ if (prevClosestPt.isNaN()) {
+ Vec3 xPhint, xQhint;
+ if (thisObs.hasContactPointHints()) {
+ thisObs.getContactPointHints(xPhint, xQhint);
+ prevClosestPt = xPhint;
+ } else
+ prevClosestPt = (r_SPn+r_SQp)/2;
+ }
+ Vec3 closestPointOnSurface, closestPointOnLine; Real height;
+ bool succeeded = geo.trackSeparationFromLine
+ ((r_SPn+r_SQp)/2, d_QpPn,
+ prevClosestPt, closestPointOnSurface, closestPointOnLine, height);
+
+ ppe.witnesses[sox] = height;
+ ppe.closestSurfacePoint[sox] = closestPointOnSurface;
+ ppe.closestPathPoint[sox] = closestPointOnLine;
+ //std::cout << "HEIGHT=" << ppe.witnesses[sox] << std::endl;
+ }
+
+ Vector& eventTriggers =
+ cables->updEventTriggersByStage(state,Stage::Position);
+
+ for (SurfaceObstacleIndex i(0); i < instInfo.getNumSurfaceObstacles(); ++i)
+ {
+ eventTriggers[eventIx+i] = ppe.witnesses[i];
+ }
+
+ cables->markDiscreteVarUpdateValueRealized(state, posEntryIx);
+}
+
+
+
+//------------------------------------------------------------------------------
+// ENSURE VELOCITY KINEMATICS CALCULATED
+//------------------------------------------------------------------------------
+void CablePath::Impl::
+ensureVelocityKinematicsCalculated(const State& state) const {
+ if (cables->isDiscreteVarUpdateValueRealized(state, velEntryIx))
+ return;
+
+ const PathInstanceInfo& instInfo = getInstanceInfo(state);
+ const PathPosEntry& ppe = getPosEntry(state);
+ PathVelEntry& pve = updVelEntry(state);
+
+ pve.lengthDot = 0;
+ pve.unitPower = 0;
+
+ // Solve for xdot:
+ // Calc RHS in J xdot = Kdot.
+ // Solve xdot = J\ Kdot.
+
+ if (ppe.x.size()) {
+ findKinematicVelocityErrors(state, instInfo, ppe, pve);
+ ppe.JInv.solve(pve.nerrdotK, pve.xdot);
+ cout << "*** xdot=" << pve.xdot << endl;
+ }
+
+ //TODO: calc length dot
+ Vec3 vprev_GQ;
+ for (CableObstacleIndex ox(0); ox < obstacles.size(); ++ox) {
+ const ActiveObstacleIndex ax = ppe.mapToActive[ox];
+ if (!ax.isValid())
+ continue; // skip disabled or inactive obstacles
+
+ const CableObstacle::Impl& obs = obstacles[ox].getImpl();
+ const MobilizedBody& B = obs.getMobilizedBody();
+ Vec3 P_B, Q_B, v_GP, v_GQ;
+ obs.getContactStationsOnBody(state, instInfo, ppe, P_B, Q_B);
+
+ //TODO: this is wrong -- need to use the contact point velocities
+ // not the material point velocities.
+ v_GP = B.findStationVelocityInGround(state, P_B);
+ v_GQ = B.findStationVelocityInGround(state, Q_B);
+
+ if (ax > 0) {
+ pve.lengthDot += dot(v_GP - vprev_GQ, ppe.eIn_G[ax]);
+ }
+ vprev_GQ = v_GQ;
+
+ const SpatialVec& V_GB = obs.getBodyVelocity(state);
+ pve.unitPower += ~ppe.Fu_GB[ax] * V_GB;
+ }
+
+ cables->markDiscreteVarUpdateValueRealized(state, velEntryIx);
+}
+
+// Numerical Jacobian calculation.
+
+class PathError : public Differentiator::JacobianFunction {
+public:
+ // Caution: we're holding references.
+ PathError(int nx,
+ const CablePath::Impl& path,
+ const State& state,
+ const PathInstanceInfo& instInfo,
+ const PathPosEntry& ppe,
+ Real accuracy)
+ : Differentiator::JacobianFunction(nx, nx), path(path),
+ state(state),instInfo(instInfo),localPpe(ppe)
+ { setEstimatedAccuracy(accuracy); }
+
+ int f(const Vector& x, Vector& fx) const OVERRIDE_11 {
+ localPpe.x = x;
+ path.calcPathError(state, instInfo, localPpe);
+ fx = localPpe.err;
+ return 0;
+ }
+
+private:
+ const CablePath::Impl& path;
+ const State& state;
+ const PathInstanceInfo& instInfo;
+ mutable PathPosEntry localPpe;
+};
+
+//------------------------------------------------------------------------------
+// PROJECT ONTO SURFACE
+//------------------------------------------------------------------------------
+// Take whatever is in ppe.x and move any implicit surface points that are not
+// on the surface downhill to the nearest surface point.
+void CablePath::Impl::
+projectOntoSurface(const PathInstanceInfo& instInfo,
+ PathPosEntry& ppe) const
+{
+ // Skip origin and termination "obstacles" at beginning and end.
+ for (CableObstacleIndex ox(1); ox < obstacles.size()-1; ++ox) {
+ const ActiveSurfaceIndex asx = ppe.mapToActiveSurface[ox];
+ if (!asx.isValid())
+ continue; // skip via points and inactive surfaces
+
+ const int xSlot = ppe.mapToCoords[ox];
+ assert(xSlot >= 0); // Should have had coordinates assigned
+
+ Vec3& P = Vec3::updAs(&ppe.x[xSlot]);
+ Vec3& Q = Vec3::updAs(&ppe.x[xSlot+3]);
+
+ const CableObstacle::Surface::Impl& obs =
+ SimTK_DYNAMIC_CAST_DEBUG<const CableObstacle::Surface::Impl&>
+ (getObstacleImpl(ox));
+
+ const ContactGeometry& geom = obs.getContactGeometry();
+ P = geom.projectDownhillToNearestPoint(P);
+ Q = geom.projectDownhillToNearestPoint(Q);
+ }
+}
+
+//------------------------------------------------------------------------------
+// SOLVE FOR PATH POINTS
+//------------------------------------------------------------------------------
+// The entry unit vectors have been computed for all obstacles and the lengths
+// of all straight segments have been accumulated. Now solve for the unknown
+// path point locations on surface obstacles, and accumulate the resulting
+// geodesic lengths to complete the path length.
+void CablePath::Impl::
+solveForPathPoints(const State& state, const PathInstanceInfo& instInfo,
+ PathPosEntry& ppe) const
+{
+ const PathPosEntry& prevPPE = getPrevPosEntry(state);
+ if (prevPPE.x.size())
+ ppe.x = prevPPE.x; // start with previous solution if there is one
+
+ projectOntoSurface(instInfo,ppe); // clean up first
+
+ calcPathError(state,instInfo,ppe);
+
+ if (ppe.x.size() == 0)
+ return; // only via points; no iteration to do
+
+ const Real ftol = Real(1e-12)*1000; // TODO
+ const Real xtol = Real(1e-12)*1000;
+
+ const Real estimatedPathErrorAccuracy = ftol;
+ PathError pathErrorFnc(ppe.x.size(), *this, state, instInfo, ppe,
+ estimatedPathErrorAccuracy);
+ Differentiator diff(pathErrorFnc);
+
+ Vector dx, xold, xchg;
+
+ Real f = ppe.err.norm();
+ cout << "\n***PATH solveForPathPoints at t=" << state.getTime()
+ << " err=" << f << "\n";
+
+ Real fold, lam = 1, nextlam = 1;
+ Real dxnormPrev = Infinity;
+ int maxIter = 20;
+ for (int i = 0; i < maxIter; ++i) {
+ // We always need a Jacobian even if the path is already good enough
+ // because we use it to solve for xdot. So we might as well do one
+ // iteration.
+ if (i > 0 && f <= ftol) {
+ std::cout << "\n***PATH converged in "
+ << i << " iterations err=" << f << "\n\n";
+ break;
+ }
+ //cout << "obstacle err = " << f << ", x = " << ppe.x << endl;
+
+ //diff.calcJacobian(ppe.x, ppe.err, ppe.J,
+ // Differentiator::ForwardDifference);
+ // Matrix Jnum = ppe.J;
+ calcPathErrorJacobian(state, instInfo, ppe);
+
+ //cout << "DIFF J=" << Jnum-ppe.J;
+ //cout << "DIFF NORM=" << (Jnum-ppe.J).norm() << "\n";
+
+ ppe.JInv.factor(ppe.J);
+ //ppe.JInv.factor(Jnum);
+
+ fold = f;
+ xold = ppe.x;
+
+ ppe.JInv.solve(ppe.err, dx);
+
+ const Real dxnorm = std::sqrt(dx.normSqr()/ppe.x.size()); // rms
+ cout << "|dx| = " << dxnorm << endl;
+ if (dxnorm > Real(.99)*dxnormPrev) {
+ std::cout << "\nPATH stalled in "
+ << i << " iterations err=" << f << " |dx|=" << dxnorm << "\n\n";
+ break;
+ }
+
+ // backtracking
+ lam = nextlam;
+ while (true) {
+ xchg = lam*dx;
+ ppe.x = xold - xchg;
+ projectOntoSurface(instInfo,ppe); // clean up first
+
+ calcPathError(state,instInfo,ppe);
+ f = ppe.err.norm();
+ //cout << "step=" << lam << " obstacle err = " << f
+ // << ", x = " << ppe.x << endl;
+ if (f <= fold)
+ break;
+ lam = lam / 2;
+ }
+ //cout << "step size=" << lam << endl;
+
+ if (lam == nextlam)
+ nextlam = std::min(2*lam, Real(1));
+
+ dxnormPrev = dxnorm;
+ }
+ //cout << "obstacle error = " << ppe.err << endl;
+ //SimTK_ERRCHK3_ALWAYS(f <= ftol, "CablePath::solveForPathPoints()",
+ // "At t=%g, achieved patherr=%g but tol=%g.", state.getTime(), f, ftol);
+}
+
+
+
+//------------------------------------------------------------------------------
+// FIND KINEMATIC VELOCITY ERRORS
+//------------------------------------------------------------------------------
+// Given a patherr function err(K;x), the time derivative at x=x0 is
+// errdot(K,Kdot,x0; xdot) = Derr/Dx xdot + errdotK
+// where errdotK = Derr/DK Kdot
+// Here we calculate errdotK, which is the errdot we would get if
+// xdot==0, i.e., with the contact points frozen on their surfaces. This term
+// depends only on the ordinary material point kinematics. We will later use
+// this to solve for the value of xdot that makes errdot==0.
+//
+// We actually calculate nerrdotK = -errdotK here so we don't have to negate
+// it later.
+void CablePath::Impl::
+findKinematicVelocityErrors
+ (const State& state, const PathInstanceInfo& instInfo,
+ const PathPosEntry& ppe, PathVelEntry& pve) const
+{
+ if (ppe.x.size() == 0)
+ return; // only via points; no xdots to worry about
+
+ // Run through all the active surface obstacles calculating time
+ // derivatives of the input unit vectors eIn and eOut and passing those
+ // to the obstacles to obtain the kinematic velocity errors.
+ // TODO: accumulate the length changes of the straight-line segments
+ // here.
+
+ // The origin point is always active. Start off with that as the "previous"
+ // active obstacle.
+ CableObstacleIndex prevActiveOx = CableObstacleIndex(0);
+
+ // Now go through the active obstacles and stop to process whenever one
+ // of those is a surface obstacle.
+ CableObstacleIndex thisActiveOx = ppe.findNextActiveObstacle(prevActiveOx);
+
+ // Stop when we reach the termination point -- that's not a surface.
+ while (thisActiveOx < obstacles.size()-1) {
+ // Is this active obstacle a surface? If not keep going.
+ const ActiveSurfaceIndex asx = ppe.mapToActiveSurface[thisActiveOx];
+ if (!asx.isValid()) {
+ prevActiveOx = thisActiveOx;
+ thisActiveOx = ppe.findNextActiveObstacle(prevActiveOx);
+ continue; // skip via points and inactive surfaces
+ }
+
+ // This active obstacle is a surface. Find the next active obstacle.
+ const CableObstacleIndex nextActiveOx =
+ ppe.findNextActiveObstacle(thisActiveOx);
+
+ // Now we have prev, this, and next with prev and next active and
+ // this an active surface. We want all the points in the S frame
+ // and the velocities of Qprev and Pnext in the S frame (P and Q
+ // velocities are zero in S).
+ const CableObstacle::Impl& prevObs = getObstacleImpl(prevActiveOx);
+ const CableObstacle::Surface::Impl& thisObs =
+ SimTK_DYNAMIC_CAST_DEBUG<const CableObstacle::Surface::Impl&>
+ (getObstacleImpl(thisActiveOx));
+ const CableObstacle::Impl& nextObs = getObstacleImpl(nextActiveOx);
+
+ // We're going to work in the body frames Bprev, Bthis, Bnext and
+ // then reexpress in S when we get to the "in" and "out" vectors.
+ const Rotation& R_BS = thisObs.getObstaclePoseOnBody(state,instInfo).R();
+ const Rotation R_SB = ~R_BS;
+
+ // Abbreviate Bp=Bprev, Bn=Bnext, B=Bthis.
+ const MobilizedBody& Bp = prevObs.getMobilizedBody();
+ const MobilizedBody& B = thisObs.getMobilizedBody();
+ const MobilizedBody& Bn = nextObs.getMobilizedBody();
+ Vec3 Pp, Qp, P, Q, Pn, Qn; // points in local body frames
+ prevObs.getContactStationsOnBody(state, instInfo, ppe, Pp, Qp);
+ thisObs.getContactStationsOnBody(state, instInfo, ppe, P, Q);
+ nextObs.getContactStationsOnBody(state, instInfo, ppe, Pn, Qn);
+
+ // Find prev and next points measured from & expressed in Bthis.
+ const Vec3 r_BQp = Bp.findStationLocationInAnotherBody(state, Qp, B);
+ const Vec3 r_BPn = Bn.findStationLocationInAnotherBody(state, Pn, B);
+
+ // Find prev and next point velocities measured & expressed in Bthis.
+ const Vec3 v_BQp = Bp.findStationVelocityInAnotherBody(state, Qp, B);
+ const Vec3 v_BPn = Bn.findStationVelocityInAnotherBody(state, Pn, B);
+
+ // Get relative position vectors in B and re-express in S.
+ const Vec3 rIn = R_SB*(P - r_BQp);
+ const Vec3 rOut = R_SB*(r_BPn - Q);
+
+ // These are the time derivatives in S of rIn and rOut. P, Q, and R_SB
+ // are all constants so P and Q drop out.
+ const Vec3 vIn = -(R_SB*v_BQp);
+ const Vec3 vOut = R_SB*v_BPn;
+
+ // Now create unit vectors along rIn and rOut and their time
+ // derivatives in S.
+ const Real ooNormIn = 1/rIn.norm(); // oo == "one over"
+ const Real ooNormOut = 1/rOut.norm();
+
+ // Create unit vectors in S; "true" here means no need to normalize.
+ const UnitVec3 eIn (ooNormIn *rIn, true);
+ const UnitVec3 eOut(ooNormOut*rOut, true);
+
+ const Vec3 eInDot = ooNormIn* (vIn - (~eIn *vIn )*eIn);
+ const Vec3 eOutDot = ooNormOut*(vOut - (~eOut*vOut)*eOut);
+
+ // Now ask the obstacle to calculate the kinematic velocity errors
+ // given the in/out direction time derivatives.
+
+ const int xSlot = ppe.mapToCoords[thisActiveOx];
+ assert(xSlot >= 0); // Should have had coordinates assigned
+
+ Vec6::updAs(&pve.nerrdotK[xSlot]) =
+ thisObs.calcSurfaceNegKinematicVelocityError
+ (ppe.geodesics[asx], // solved geodesic
+ eIn, // in S frame
+ eInDot,
+ Vec3::getAs(&ppe.x[xSlot]), // xP
+ Vec3::getAs(&ppe.x[xSlot+3]), // xQ
+ eOut, // in S frame
+ eOutDot);
+
+ prevActiveOx = thisActiveOx;
+ thisActiveOx = ppe.findNextActiveObstacle(prevActiveOx);
+ }
+}
+
+//------------------------------------------------------------------------------
+// CALC PATH ERROR
+//------------------------------------------------------------------------------
+// Given a set of nx coordinates for the
+// path points, calculate the nx error conditions that result. Return the
+// new geodesics in case anyone is interested.
+void CablePath::Impl::
+calcPathError(const State& state, const PathInstanceInfo& instInfo,
+ PathPosEntry& ppe) const
+{
+ const PathPosEntry& prevPPE = getPrevPosEntry(state);
+
+ ppe.length = 0;
+
+ // First pass: run through all the enabled obstacles. Update the distance
+ // function for inactive obstacles. Precalculate entry directions and
+ // unit forces for all active obstacles, and accumulate the lengths of
+ // straight-line segments.
+ Vec3 prevQB_G, prevQ_G;
+ for (CableObstacleIndex ox(0); ox < obstacles.size(); ++ox) {
+ if (instInfo.obstacleDisabled[ox])
+ continue;
+
+ const ActiveObstacleIndex ax = ppe.mapToActive[ox];
+ if (!ax.isValid()) {
+ // TODO: update distance function
+ continue;
+ }
+
+ const CableObstacle::Impl& obs = getObstacleImpl(ox);
+ Vec3 PB_G, QB_G;
+ obs.expressContactStationsInGround(state,instInfo,ppe,
+ PB_G, QB_G);
+ const Vec3& Bo_G = obs.getBodyTransform(state).p();
+ const Vec3 P_G = Bo_G + PB_G;
+ const Vec3 Q_G = Bo_G + QB_G;
+ if (ax == 0) { // origin
+ prevQB_G = QB_G;
+ prevQ_G = Q_G;
+ // No forces applied from the "entry side" at the origin.
+ ppe.Fu_GB[ax] = SpatialVec(Vec3(0));
+ continue;
+ }
+
+ const Vec3 r_QP_G(P_G - prevQ_G);
+ const Real lineSegLen = r_QP_G.norm();
+ const UnitVec3 eIn_G(r_QP_G/lineSegLen, true);
+ ppe.eIn_G[ax] = eIn_G;
+ ppe.length += lineSegLen;
+ const SpatialVec unitForcePrevQ(prevQB_G % eIn_G, eIn_G);
+ const SpatialVec unitForceP ( eIn_G % PB_G, -eIn_G);
+ // Add in force component from the "exit side" of previous obstacle.
+ ppe.Fu_GB[ax.prev()] += unitForcePrevQ;
+ // Initialize force to the contribution from the "entry side" of
+ // this obstacle.
+ ppe.Fu_GB[ax] = unitForceP;
+
+ prevQB_G = QB_G;
+ prevQ_G = Q_G;
+ }
+
+ if (ppe.x.size() == 0) {
+ // No active surfaces -- this path is all lines between points so
+ // has no errors.
+ return;
+ }
+
+ // Pass 2 : use above info to calculate errors from active surfaces.
+
+
+ // Skip origin and termination "obstacles" at beginning and end.
+ for (CableObstacleIndex ox(1); ox < obstacles.size()-1; ++ox) {
+ const ActiveSurfaceIndex asx = ppe.mapToActiveSurface[ox];
+ if (!asx.isValid())
+ continue; // skip via points and inactive surfaces
+
+ const int xSlot = ppe.mapToCoords[ox];
+ assert(xSlot >= 0); // Should have had coordinates assigned
+
+ const CableObstacle::Surface::Impl& obs =
+ SimTK_DYNAMIC_CAST_DEBUG<const CableObstacle::Surface::Impl&>
+ (getObstacleImpl(ox));
+
+ const Rotation& R_BS = obs.getObstaclePoseOnBody(state, instInfo).R();
+ const Rotation& R_GB = obs.getBodyTransform(state).R();
+ const Rotation R_GS = R_GB*R_BS;
+
+ const ActiveObstacleIndex ax = ppe.mapToActive[ox];
+ const ActiveSurfaceIndex prevASX = prevPPE.mapToActiveSurface[ox];
+ const UnitVec3 eIn_S = ~R_GS * ppe.eIn_G[ax];
+ const UnitVec3 eOut_S = ~R_GS * ppe.eIn_G[ax.next()];
+ Vec6::updAs(&ppe.err[xSlot]) =
+ obs.calcSurfacePathError(
+ prevASX.isValid() ? prevPPE.geodesics[prevASX] : Geodesic(),
+ eIn_S,
+ Vec3::getAs(&ppe.x[xSlot]), // xP
+ Vec3::getAs(&ppe.x[xSlot+3]),// xQ
+ eOut_S,
+ ppe.geodesics[asx] );
+
+ const Geodesic& geod = ppe.geodesics[asx];
+ const Real signP = (Real)sign(dot(eIn_S,geod.getTangentP()));
+ const Real signQ = (Real)sign(dot(eOut_S,geod.getTangentQ()));
+ const bool isFlipped = (signP<0 && signQ<0);
+ const Real geoLength = isFlipped ? -geod.getLength() : geod.getLength();
+
+ ppe.length += geoLength;
+
+ const SurfaceObstacleIndex sox = instInfo.mapObstacleToSurface[ox];
+ assert(sox.isValid());
+
+ ppe.witnesses[sox] = geoLength;
+
+ //std::cout << "WITNESS=" << ppe.witnesses[sox] << std::endl;
+ }
+
+}
+
+//------------------------------------------------------------------------------
+// CALC PATH ERROR JACOBIAN
+//------------------------------------------------------------------------------
+// Assemble the nx X nx banded Jacobian J=D patherr / Dx from per-obstacle
+// blocks. The obstacles compute the Jacobian of their own path error
+// functions, which are eHat(eIn_S, xP, xQ, eOut_S), with all arguments in the
+// obstacle frame S. The blocks we need are instead the Jacobian of
+// e(xQ-1, xP, xQ, xP+1) = eHat(eIn_S(xQ-1,xP), xP, xQ, eOut_S(xQ,xP+1))
+// so we need to apply the chain rule terms
+// D eIn_S D eIn_S D eOut_S D eOut_S
+// -------- -------- -------- --------
+// D xQ-1 D xP D xQ D xP+1
+// to produce the block we need for the patherr Jacobian.
+void CablePath::Impl::
+calcPathErrorJacobian(const State& state,
+ const PathInstanceInfo& instInfo,
+ PathPosEntry& ppe) // in/out
+ const
+{
+ const PathPosEntry& prevPPE = getPrevPosEntry(state);
+
+ const int nx = ppe.x.size();
+ ppe.J.resize(nx, nx);
+ ppe.J.setToZero();
+ if (nx == 0)
+ return; // only via points; nothing to do
+
+ // Run through all the active surface obstacles calculating partial
+ // derivatives of the input unit vectors eIn and eOut and applying those
+ // to the Jacobian blocks returned by the obstacles.
+
+ // The origin point is always active. Start off with that as the "previous"
+ // active obstacle.
+ CableObstacleIndex prevActiveOx = CableObstacleIndex(0);
+
+ // Now go through the active obstacles and stop to process whenever one
+ // of those is a surface obstacle.
+ CableObstacleIndex thisActiveOx = ppe.findNextActiveObstacle(prevActiveOx);
+
+ // Stop when we reach the termination point -- that's not a surface.
+ while (thisActiveOx < obstacles.size()-1) {
+ // Is this active obstacle a surface? If not keep going.
+ const ActiveSurfaceIndex asx = ppe.mapToActiveSurface[thisActiveOx];
+ if (!asx.isValid()) {
+ prevActiveOx = thisActiveOx;
+ thisActiveOx = ppe.findNextActiveObstacle(prevActiveOx);
+ continue; // skip via points and inactive surfaces
+ }
+
+ // This active obstacle is a surface. Find the next active obstacle.
+ const CableObstacleIndex nextActiveOx =
+ ppe.findNextActiveObstacle(thisActiveOx);
+
+ // Now we have prev, this, and next with prev and next active and
+ // this an active surface. We want all the points in the S frame
+ // and the velocities of Qprev and Pnext in the S frame (P and Q
+ // velocities are zero in S).
+ const CableObstacle::Impl& prevObs = getObstacleImpl(prevActiveOx);
+ const CableObstacle::Surface::Impl& thisObs =
+ SimTK_DYNAMIC_CAST_DEBUG<const CableObstacle::Surface::Impl&>
+ (getObstacleImpl(thisActiveOx));
+ const CableObstacle::Impl& nextObs = getObstacleImpl(nextActiveOx);
+
+ // We're going to work in the body frames Bprev, Bthis, Bnext and
+ // then reexpress in S when we get to the "in" and "out" vectors.
+
+ // Abbreviate Bp=Bprev, Bn=Bnext, B=Bthis.
+ const MobilizedBody& Bp = prevObs.getMobilizedBody();
+ const MobilizedBody& B = thisObs.getMobilizedBody();
+ const MobilizedBody& Bn = nextObs.getMobilizedBody();
+
+ const Rotation R_BBp = Bp.findBodyRotationInAnotherBody(state, B);
+ const Rotation R_BBn = Bn.findBodyRotationInAnotherBody(state, B);
+
+ const Rotation& R_BpSp = prevObs.getObstaclePoseOnBody(state,instInfo).R();
+ const Rotation& R_BS = thisObs.getObstaclePoseOnBody(state,instInfo).R();
+ const Rotation& R_BnSn = nextObs.getObstaclePoseOnBody(state,instInfo).R();
+ const Rotation R_SB = ~R_BS;
+ const Rotation R_SSp = R_SB*R_BBp*R_BpSp;
+ const Rotation R_SSn = R_SB*R_BBn*R_BnSn;
+
+ Vec3 Pp, Qp, P, Q, Pn, Qn; // points in local body frames
+ prevObs.getContactStationsOnBody(state, instInfo, ppe, Pp, Qp);
+ thisObs.getContactStationsOnBody(state, instInfo, ppe, P, Q);
+ nextObs.getContactStationsOnBody(state, instInfo, ppe, Pn, Qn);
+
+ // Find prev and next points measured from & expressed in Bthis.
+ const Vec3 r_BQp = Bp.findStationLocationInAnotherBody(state, Qp, B);
+ const Vec3 r_BPn = Bn.findStationLocationInAnotherBody(state, Pn, B);
+
+ // Get relative position vectors in B and re-express in S.
+ const Vec3 rIn = R_SB*(P - r_BQp);
+ const Vec3 rOut = R_SB*(r_BPn - Q);
+
+ // Now create unit vectors along rIn and rOut, in S.
+ const Real ooNormIn = 1/rIn.norm(); // oo == "one over"
+ const Real ooNormOut = 1/rOut.norm();
+
+ // Create unit vectors in S; "true" here means no need to normalize.
+ UnitVec3 eIn (ooNormIn *rIn, true);
+ UnitVec3 eOut(ooNormOut*rOut, true);
+
+ Mat33 DeinDP = ooNormIn *(Mat33(1) - eIn*~eIn);
+ Mat33 DeoutDQ = -ooNormOut*(Mat33(1) - eOut*~eOut);
+ Mat33 DeinDQp = -DeinDP * R_SSp;
+ Mat33 DeoutDPn = -DeoutDQ * R_SSn;
+
+ // Now ask the obstacle to calculate the Jacobian Jhat of its error
+ // function ehat(e_In, xP, xQ, e_Out).
+
+ const int xSlot = ppe.mapToCoords[thisActiveOx];
+ assert(xSlot >= 0); // Should have had coordinates assigned
+ const ActiveSurfaceIndex prevASX =
+ prevPPE.mapToActiveSurface[thisActiveOx];
+
+ //Mat63 DehatDein1, DehatDxP1, DehatDxQ1, DehatDeout1;
+ //thisObs.calcSurfacePathErrorJacobianNumerically
+ // (prevASX.isValid() ? prevPPE.geodesics[prevASX] : Geodesic(),
+ // eIn, // in S frame
+ // Vec3::getAs(&ppe.x[xSlot]), // xP
+ // Vec3::getAs(&ppe.x[xSlot+3]), // xQ
+ // eOut,
+ // ppe.geodesics[asx], // solved geodesic
+ // DehatDein1, DehatDxP1, DehatDxQ1, DehatDeout1);
+
+ Mat63 DehatDein, DehatDxP, DehatDxQ, DehatDeout;
+ thisObs.calcSurfacePathErrorJacobianAnalytically
+ (eIn, // in S frame
+ Vec3::getAs(&ppe.x[xSlot]), // xP
+ Vec3::getAs(&ppe.x[xSlot+3]), // xQ
+ eOut,
+ ppe.geodesics[asx], // solved geodesic
+ DehatDein, DehatDxP, DehatDxQ, DehatDeout);
+
+ //DehatDein=DehatDein1;DehatDxP=DehatDxP1;DehatDxQ=DehatDxQ1;DehatDeout=DehatDeout1;
+
+ //cout << "err=" << ppe.err << "\n";
+
+ //cout << "DehatDxP =" << DehatDxP;
+ //cout << "DehatDxP1=" << DehatDxP1;
+ //cout << "diff=" << (DehatDxP-DehatDxP1).norm() << ": " << (DehatDxP-DehatDxP1);
+
+ //cout << "DehatDxQ =" << DehatDxQ;
+ //cout << "DehatDxQ1=" << DehatDxQ1;
+ //cout << "diff=" << (DehatDxQ-DehatDxQ1).norm() << ": " << (DehatDxQ-DehatDxQ1);
+
+ if (xSlot >= 3)
+ ppe.J(xSlot,xSlot-3,6,3) = Matrix(DehatDein*DeinDQp);
+ ppe.J(xSlot,xSlot, 6,3) = Matrix(DehatDxP + DehatDein *DeinDP);
+ ppe.J(xSlot,xSlot+3,6,3) = Matrix(DehatDxQ + DehatDeout*DeoutDQ);
+ if (xSlot+9 < nx)
+ ppe.J(xSlot,xSlot+6,6,3) = Matrix(DehatDeout*DeoutDPn);
+
+ prevActiveOx = thisActiveOx;
+ thisActiveOx = ppe.findNextActiveObstacle(prevActiveOx);
+ }
+
+}
+
+//==============================================================================
+// CABLE OBSTACLE
+//==============================================================================
+
+CableObstacle::CableObstacle(CableObstacle::Impl* guts) : impl(guts)
+{
+ if (impl) ++impl->referenceCount;
+}
+
+// Copy constructor is shallow and reference counted.
+CableObstacle::CableObstacle(const CableObstacle& src) : impl(src.impl)
+{ if (impl) ++impl->referenceCount; }
+
+// Copy assignment is shallow and reference counted.
+CableObstacle& CableObstacle::operator=(const CableObstacle& src) {
+ if (&src != this) {
+ clear();
+ if (src.impl) {
+ impl = src.impl;
+ ++impl->referenceCount;
+ }
+ }
+ return *this;
+}
+
+// This is used to implement the destructor and copy assignment.
+void CableObstacle::clear() {
+ if (impl) {
+ if (--impl->referenceCount == 0)
+ delete impl;
+ impl = 0;
+ }
+}
+
+const Transform& CableObstacle::
+getDefaultTransform() const {return impl->getDefaultPoseOnBody();}
+const MobilizedBody& CableObstacle::
+getMobilizedBody() const {return impl->getMobilizedBody();}
+const CablePath& CableObstacle::
+getCablePath() const {assert(impl->cablePath); return *impl->cablePath;}
+CableObstacleIndex CableObstacle::
+getObstacleIndex() const {return impl->index;}
+const DecorativeGeometry& CableObstacle::
+getDecorativeGeometry() const {return impl->getDecoration();}
+DecorativeGeometry& CableObstacle::
+updDecorativeGeometry() {return impl->updDecoration();}
+bool CableObstacle::
+isDisabledByDefault() const {return impl->isDisabledByDefault();}
+
+CableObstacle& CableObstacle::
+setDisabledByDefault(bool shouldBeDisabled)
+{ impl->setDisabledByDefault(shouldBeDisabled); return *this;}
+CableObstacle& CableObstacle::
+setDefaultTransform(const Transform& X_BS)
+{ impl->defaultX_BS=X_BS; return *this;}
+CableObstacle& CableObstacle::
+setDecorativeGeometry(const DecorativeGeometry& viz)
+{ impl->setDecoration(viz); return *this; }
+
+inline void CableObstacle::Impl::invalidateTopology()
+{ if (cablePath) cablePath->updImpl().invalidateTopology(); }
+
+
+//==============================================================================
+// CABLE OBSTACLE :: VIA POINT
+//==============================================================================
+
+CableObstacle::ViaPoint::
+ViaPoint(CablePath& path, const MobilizedBody& viaMobod,
+ const Vec3& defaultStation)
+: CableObstacle(new ViaPoint::Impl(path, viaMobod, defaultStation))
+{
+ impl->index = path.updImpl().adoptObstacle(*this);
+}
+
+
+
+//==============================================================================
+// CABLE OBSTACLE :: SURFACE
+//==============================================================================
+
+CableObstacle::Surface::
+Surface(CablePath& path, const MobilizedBody& mobod,
+ const Transform& X_BS, const ContactGeometry& geom)
+: CableObstacle(new Surface::Impl(path, mobod, X_BS, geom))
+{
+ impl->index = path.updImpl().adoptObstacle(*this);
+}
+
+CableObstacle::Surface& CableObstacle::Surface::
+setNearPoint(const Vec3& stationInS) {
+ Impl& surfImpl = SimTK_DYNAMIC_CAST_DEBUG<Impl&>(*impl);
+ surfImpl.nearPointInS = stationInS;
+ return *this;
+}
+
+// Points are provided in S frame but converted to coordinates.
+CableObstacle::Surface& CableObstacle::Surface::
+setContactPointHints(const Vec3& Phint_S, const Vec3& Qhint_S) {
+ Impl& surfImpl = SimTK_DYNAMIC_CAST_DEBUG<Impl&>(*impl);
+ surfImpl.xPhint = Phint_S; // TODO: only works for implicit surfaces
+ surfImpl.xQhint = Qhint_S;
+ return *this;
+}
+
+//==============================================================================
+// CABLE OBSTACLE :: SURFACE :: IMPL
+//==============================================================================
+void CableObstacle::Surface::Impl::initSurfacePath
+ (const Vec3& xP,
+ const Vec3& xQ,
+ const UnitVec3& entryHint_S, // optional (use NaN)
+ const UnitVec3& exitHint_S, // optional (use NaN)
+ Geodesic& path) const
+{
+ // TODO
+ surface.initGeodesic(xP, xQ, nearPointInS,
+ GeodesicOptions(), path);
+}
+
+//------------------------------------------------------------------------------
+// CALC SURFACE PATH ERROR
+//------------------------------------------------------------------------------
+Vec6 CableObstacle::Surface::Impl::calcSurfacePathError
+ (const Geodesic& previous,
+ const UnitVec3& eIn, // from Qi-1 to Pi, in S
+ const Vec3& xP, // coordinates of Pi
+ const Vec3& xQ, // coordinates of Qi
+ const UnitVec3& eOut, // from Qi to Pi+1, in S
+ Geodesic& current) const
+{
+ surface.continueGeodesic(xP, xQ, previous, GeodesicOptions(), current);
+
+ const UnitVec3& nP = current.getNormalP();
+ const UnitVec3& nQ = current.getNormalQ();
+ const UnitVec3& tP = current.getTangentP();
+ const UnitVec3& tQ = current.getTangentQ();
+ const UnitVec3& bP = current.getBinormalP();
+ const UnitVec3& bQ = current.getBinormalQ();
+ const Real length = current.getLength();
+
+ // Watch for backwards geodesic and flip tangent error conditions.
+ Real signP = Real(~eIn *tP < 0 ? -1 : 1);
+ Real signQ = Real(~eOut*tQ < 0 ? -1 : 1);
+ //XXX
+ //signP = signQ = 1;
+
+ // If length is very short, or geodesic is backwards, use path binormals
+ // rather than geodesic binormals.
+ const Real ShortLength = Real(1e-3);
+ if (length <= ShortLength)
+ cout << "==> Using short formulation for length=" << length << endl;
+ if (signP < 0 || signQ < 0) {
+ cout << "==> Backwards geodesic with length=" << length
+ << " eIn*tP=" << ~eIn*tP << " eOut*tQ=" << ~eOut*tQ << endl;
+ if (signP != signQ) {
+ cout <<" Mismatch eIn="<<eIn << " eOut="<<eOut<<endl;
+ cout <<" tP=" <<tP << " tQ="<<tQ<<endl;
+ }
+ }
+
+ const Vec3 bbarP = length<=ShortLength ? eOut % nP : Vec3(bP);
+ const Vec3 bbarQ = length<=ShortLength ? eIn % nQ : Vec3(bQ);
+
+ Vec6 err;
+ err[0] = ~eIn*nP; // tangent error in normal direction
+ err[1] = ~eOut*nQ;
+
+ err[2] = ~eIn*bbarP; // tangent errors in geodesic direction
+ err[3] = ~eOut*bbarQ;
+
+ // These are the implicit surface errors forcing P and Q to lie on the
+ // surface.
+ err[4] = surface.calcSurfaceValue(xP);
+ err[5] = surface.calcSurfaceValue(xQ);
+
+ //cout << " surfErr=" << err << endl;
+ return err;
+}
+
+
+
+//------------------------------------------------------------------------------
+// CALC SURFACE PATH ERROR JACOBIAN ANALYTICALLY
+//------------------------------------------------------------------------------
+void CableObstacle::Surface::Impl::
+calcSurfacePathErrorJacobianAnalytically
+ (const UnitVec3& eIn, // from Qi-1 to Pi, in S
+ const Vec3& xP, // coordinates of Pi, in S
+ const Vec3& xQ, // coordinates of Qi, in S
+ const UnitVec3& eOut, // from Qi to Pi+1, in S
+ const Geodesic& current, // Must have been computed from above params.
+ Mat63& DerrDentry, // 4x3 for parametric
+ Mat63& DerrDxP, // 4x2 "
+ Mat63& DerrDxQ, // 4x2 "
+ Mat63& DerrDexit) // 4x3 "
+ const
+{
+ const UnitVec3 nP = current.getNormalP();
+ const UnitVec3 nQ = current.getNormalQ();
+ const UnitVec3& tP = current.getTangentP();
+ const UnitVec3& tQ = current.getTangentQ();
+ const UnitVec3& bP = current.getBinormalP();
+ const UnitVec3& bQ = current.getBinormalQ();
+ // Watch for backwards geodesic and flip tangent error conditions.
+ Real signP = Real(~eIn *tP < 0 ? -1 : 1);
+ Real signQ = Real(~eOut*tQ < 0 ? -1 : 1);
+ // XXX
+ //signP = signQ = 1;
+
+ const Real jP = current.getJacobiP(),
+ jQ = current.getJacobiQ();
+ const Real jdP = current.getJacobiPDot(),
+ jdQ = current.getJacobiQDot();
+ const Real tauP = current.getTorsionP(),
+ tauQ = current.getTorsionQ();
+ const Real kappaP = current.getCurvatureP(),
+ kappaQ = current.getCurvatureQ();
+ const Real muP = current.getBinormalCurvatureP(),
+ muQ = current.getBinormalCurvatureQ();
+
+ DerrDentry = Mat63( ~nP, Row3(0), signP*~bP, Row3(0), Row3(0), Row3(0) );
+ DerrDexit = Mat63( Row3(0), ~nQ, Row3(0), signP*~bQ, Row3(0), Row3(0) );
+ const Vec3 gP = surface.calcSurfaceGradient(xP),
+ gQ = surface.calcSurfaceGradient(xQ);
+ const Mat33 HP = surface.calcSurfaceHessian(xP),
+ HQ = surface.calcSurfaceHessian(xQ);
+ const Real oojP = std::abs(jP) < SqrtEps ? Real(0) : 1/jP,
+ oojQ = std::abs(jQ) < SqrtEps ? Real(0) : 1/jQ;
+ const Mat33 DnPDxP = (Mat33(1) - nP*~nP)*HP / (~gP*nP),
+ DnQDxQ = (Mat33(1) - nQ*~nQ)*HQ / (~gQ*nQ);
+ const Mat33 DbPDxP = -tauP*nP*~tP - (oojP*jdP*tP + muP*nP)*~bP,
+ DbQDxQ = -tauQ*nQ*~tQ - (oojQ*jdQ*tQ + muQ*nQ)*~bQ;
+ const Mat33 DbPDxQ = -oojQ*tP*~bQ,
+ DbQDxP = oojP*tQ*~bP;
+
+ DerrDxP = Mat63( ~eIn * DnPDxP,
+ Row3(0),
+ signP*(~eIn * DbPDxP),
+ signQ*(~eOut * DbQDxP),
+ ~gP,
+ Row3(0) );
+
+ DerrDxQ = Mat63( Row3(0),
+ ~eOut * DnQDxQ,
+ signP*(~eIn * DbPDxQ),
+ signQ*(~eOut * DbQDxQ),
+ Row3(0),
+ ~gQ );
+}
+
+//------------------------------------------------------------------------------
+// CALC SURFACE KINEMATIC VELOCITY ERROR
+//------------------------------------------------------------------------------
+// Note: this must calculate the *negative* of the kinematic velocity term.
+Vec6 CableObstacle::Surface::Impl::calcSurfaceNegKinematicVelocityError
+ (const Geodesic& geodesic,
+ const UnitVec3& eIn, // from Qi-1 to Pi, in S
+ const Vec3& eInDot, // time derivative of eIn, taken in S
+ const Vec3& xP, // coordinates of Pi
+ const Vec3& xQ, // coordinates of Qi
+ const UnitVec3& eOut, // from Qi to Pi+1, in S
+ const Vec3& eOutDot) // time derivative of eOut, taken in S
+ const
+{
+ Vec6 errdotK;
+
+ const UnitVec3& nP = geodesic.getNormalP();
+ const UnitVec3& nQ = geodesic.getNormalQ();
+ const UnitVec3& tP = geodesic.getTangentP();
+ const UnitVec3& tQ = geodesic.getTangentQ();
+ const UnitVec3& bP = geodesic.getBinormalP();
+ const UnitVec3& bQ = geodesic.getBinormalQ();
+
+ // Watch for backwards geodesic and flip tangent error conditions.
+ Real signP = Real(~eIn *tP < 0 ? -1 : 1);
+ Real signQ = Real(~eOut*tQ < 0 ? -1 : 1);
+ //XXX
+ //signP = signQ = 1;
+
+ // These must be the kinematic part of the time derivatives of the
+ // error conditions that were reported for this obstacle during path
+ // error calculation, but negated.
+ errdotK[0] = - ~eInDot*nP;
+ errdotK[1] = - ~eOutDot*nQ;
+ errdotK[2] = -signP*(~eInDot*bP);
+ errdotK[3] = -signQ*(~eOutDot*bQ);
+ // Implicit surface error is frozen since xP and xQ are.
+ errdotK[4] = 0;
+ errdotK[5] = 0;
+
+ return errdotK;
+}
+
+// Numerical Jacobian calculation.
+
+class SurfaceError : public Differentiator::JacobianFunction {
+public:
+ // Caution: we're holding references to surface and prevGeodesic.
+ SurfaceError(const CableObstacle::Surface::Impl& surface,
+ const Geodesic& prevGeodesic,
+ Real accuracy)
+ : Differentiator::JacobianFunction(6, 12), surface(surface),
+ prevGeodesic(prevGeodesic)
+ { setEstimatedAccuracy(accuracy); }
+
+ int f(const Vector& x, Vector& fx) const {
+ UnitVec3 eIn, eOut;
+ Vec3 xP, xQ;
+ Geodesic newGeodesic; // throw away
+ mapXToVecs(x, eIn, xP, xQ, eOut);
+ Vec6 err = surface.calcSurfacePathError(prevGeodesic,
+ eIn, xP, xQ, eOut, newGeodesic);
+ assert(fx.size() == 6);
+ Vec6::updAs(&fx[0]) = err;
+ return 0;
+ }
+
+ void mapXToVecs(const Vector& x,
+ UnitVec3& eIn, Vec3& xP, Vec3& xQ, UnitVec3& eOut) const
+ {
+ assert(x.size()==12);
+ //eIn = UnitVec3(Vec3::getAs(&x[0]), true); // no need to normalize
+ eIn = UnitVec3(Vec3::getAs(&x[0]));
+ xP = Vec3::getAs(&x[3]);
+ xQ = Vec3::getAs(&x[6]);
+ //eOut = UnitVec3(Vec3::getAs(&x[9]), true);
+ eOut = UnitVec3(Vec3::getAs(&x[9]));
+ }
+
+ void mapVecsToX(const UnitVec3& eIn, const Vec3& xP,
+ const Vec3& xQ, const UnitVec3& eOut,
+ Vector& x) const
+ {
+ assert(x.size()==12);
+ Vec3::updAs(&x[0]) = eIn.asVec3();
+ Vec3::updAs(&x[3]) = xP;
+ Vec3::updAs(&x[6]) = xQ;
+ Vec3::updAs(&x[9]) = eOut.asVec3();
+ }
+
+ // Map 6x12 matrix into four 6x3 blocks. We're assuming the Matrix is
+ // compact and column-ordered.
+ void mapMatrixToMats(const Matrix& J,
+ Mat63& DerrDentry,
+ Mat63& DerrDxP,
+ Mat63& DerrDxQ,
+ Mat63& DerrDexit) const
+ {
+ DerrDentry = Mat63::getAs(&J(0,0));
+ DerrDxP = Mat63::getAs(&J(0,3));
+ DerrDxQ = Mat63::getAs(&J(0,6));
+ DerrDexit = Mat63::getAs(&J(0,9));
+ }
+private:
+ const CableObstacle::Surface::Impl& surface;
+ const Geodesic& prevGeodesic;
+};
+
+void CableObstacle::Surface::Impl::
+calcSurfacePathErrorJacobianNumerically
+ (const Geodesic& previous,
+ const UnitVec3& eIn_S,
+ const Vec3& xP,
+ const Vec3& xQ,
+ const UnitVec3& eOut_S,
+ const Geodesic& current, // Must have been computed from above params.
+ Mat63& DerrDentry, // 4x3 for parametric
+ Mat63& DerrDxP, // 4x2 "
+ Mat63& DerrDxQ, // 4x2 "
+ Mat63& DerrDexit) // 4x3 "
+ const
+{
+ SurfaceError jacFunction(*this, previous, Real(1e-8)); // TODO
+ Differentiator diff(jacFunction);
+ Vector x(12), err0(6);
+ jacFunction.mapVecsToX(eIn_S,xP,xQ,eOut_S, x);
+ jacFunction.f(x,err0);
+ Matrix J(6,12);
+ diff.calcJacobian(x, err0, J, Differentiator::CentralDifference);
+ jacFunction.mapMatrixToMats(J, DerrDentry, DerrDxP, DerrDxQ, DerrDexit);
+}
diff --git a/Simbody/src/CablePath_Impl.h b/Simbody/src/CablePath_Impl.h
new file mode 100644
index 0000000..46de87d
--- /dev/null
+++ b/Simbody/src/CablePath_Impl.h
@@ -0,0 +1,764 @@
+#ifndef SimTK_SIMBODY_CABLE_PATH_IMPL_H_
+#define SimTK_SIMBODY_CABLE_PATH_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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Ian Stavness *
+ * Contributors: Andreas Scholz *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+// This file declares the private implementation classes for CablePath and
+// CableObstacle classes. This is internal source, not part of the Simbody API.
+
+#include "SimTKmath.h"
+
+#include "simbody/internal/common.h"
+#include "simbody/internal/MultibodySystem.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "simbody/internal/CableTrackerSubsystem.h"
+#include "simbody/internal/CablePath.h"
+
+
+#include <cassert>
+#include <map>
+#include <iostream>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+// This is a unique integer type for identifying active obstacles in a
+// particular path, including both via points and surfaces.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(ActiveObstacleIndex);
+
+// This is a unique integer type for identifying those obstacles that
+// are surfaces rather than via points, regardless of whether they are
+// enabled or active.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(SurfaceObstacleIndex);
+
+// These are indices assigned to the subset of active obstacles that are
+// surfaces and thus have geodesics and contribute unknowns to the path
+// finding problem. This set is the intersection of active & surface.
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(ActiveSurfaceIndex);
+
+
+// This is a discrete state variable for holding a path's instance-level
+// information, including placement of points and surfaces on their bodies
+// and obstacle enable/disable settings.
+class PathInstanceInfo {
+public:
+ // Initialize instance info from the defaults built into the obstacles.
+ explicit PathInstanceInfo
+ (const Array_<CableObstacle,CableObstacleIndex>& obstacles);
+
+ int getNumObstacles() const {return mapObstacleToSurface.size();}
+ int getNumSurfaceObstacles() const {return mapSurfaceToObstacle.size();}
+
+ // Each surface obstacle gets one of these. Via point obstacles will
+ // have a surface obstacle index that tests !isValid(). This is filled
+ // in by the constructor.
+ Array_<SurfaceObstacleIndex,CableObstacleIndex> mapObstacleToSurface;
+ // This is indexed by surface obstacle index to give back the corresponding
+ // obstacle index.
+ Array_<CableObstacleIndex,SurfaceObstacleIndex> mapSurfaceToObstacle;
+
+ // One entry per obstacle but you can't disable the origin or termination
+ // points.
+ Array_<bool,CableObstacleIndex> obstacleDisabled;
+ Array_<Transform,CableObstacleIndex> obstaclePose; // X_BS[i]
+};
+
+
+// This is a cache entry for holding a path's calculated position-level
+// information. At the time it is created we know the total number of
+// obstacles n (including the end points), but not which ones are active. We
+// don't know how many
+// unknowns there are until we find out later based on the subset of active
+// obstacles that have unknown P's and Q's.
+// Note that disabled obstacles still get slots here but we never look at
+// them. And disabled != inactive.
+//
+// Individual obstacles can only know their CableObstacleIndex because all
+// possible obstacles for a cable are provided during construction (i.e., that
+// is topological information). Then when we determine the set of active
+// obstacles, we assign ActiveObstacleIndex and ActiveSurfaceIndex indices
+// which must be maintained in the state here. An obstacle can use its
+// CableObstacleIndex to obtain the other indices for access to related
+// quantities.
+class PathPosEntry {
+public:
+ PathPosEntry() : length(NaN) {}
+
+ // Set the number of obstacles to n. If there is any information already
+ // in this object, it is lost.
+ void setNumObstacles(int n, int nSurfaces) {
+ mapToActive.clear(); mapToActive.resize(n);
+ mapToActiveSurface.clear(); mapToActiveSurface.resize(n);
+ mapToCoords.clear(); mapToCoords.resize(n);
+ witnesses.clear(); witnesses.resize(nSurfaces, Real(NaN));
+ closestSurfacePoint.clear(); closestSurfacePoint.resize(nSurfaces, Vec3(NaN));
+ closestPathPoint.clear(); closestPathPoint.resize(nSurfaces, Vec3(NaN));
+ initialize(0,0,0); // not yet
+ }
+
+ // mapToActive, mapToActiveSurface and mapToCoords are already allocated.
+ void initialize(int na, int nas, int nx) {
+ length = NaN;
+ // Active obstacles
+ eIn_G.clear(); eIn_G.resize(na); // all NaN
+ Fu_GB.clear(); Fu_GB.resize(na, SpatialVec(Vec3(NaN)));
+ // Active surfaces
+ geodesics.clear(); geodesics.resize(nas);
+ x.clear(); x.resize(nx);
+ err.clear(); err.resize(nx);
+ J.clear(); J.resize(nx,nx);
+ }
+
+ // Return the obstacle index of the first active obstacle following the
+ // given obstacle. Returns an invalid index if there are no more.
+ CableObstacleIndex findNextActiveObstacle(CableObstacleIndex thisOx) const {
+ for (CableObstacleIndex ox(thisOx+1); ox < mapToActive.size(); ++ox)
+ if (mapToActive[ox].isValid())
+ return ox;
+ return CableObstacleIndex();
+ }
+
+ // Return the obstacle index of the first active obstacle preceding the
+ // given obstacle. Returns an invalid index if there are no more in that
+ // direction.
+ CableObstacleIndex findPrevActiveObstacle(CableObstacleIndex thisOx) const {
+ for (CableObstacleIndex ox(thisOx); ox > 0; --ox)
+ if (mapToActive[ox.prev()].isValid())
+ return ox.prev();
+ return CableObstacleIndex();
+ }
+
+ // This is the total length of the path corresponding to the current
+ // configuration and values for contact point coordinates x stored here.
+ Real length;
+
+ // ALL OBSTACLES
+
+ // Map each cable obstacle to its ActiveObstacleIndex if it is currently
+ // active, otherwise the entry tests !isValid(). Disabled obstacles are
+ // always inactive, enabled via points are always active. The origin and
+ // termination points are always active, so mapToActive[0]=0 and
+ // mapToActive[n-1]=nActive-1.
+ Array_<ActiveObstacleIndex,CableObstacleIndex> mapToActive;
+
+ // Each active, surface obstacle gets one of these. Points and inactive
+ // surfaces will have indices that test !isValid().
+ Array_<ActiveSurfaceIndex,CableObstacleIndex> mapToActiveSurface;
+
+ // Each active, surface obstacle is assigned a slot in the vector of
+ // unknowns x. The first index of that slot is saved here, with a -1
+ // for any obstacle that is not a surface or is inactive.
+ Array_<int,CableObstacleIndex> mapToCoords;
+
+ // ACTIVE OBSTACLES
+
+ // These are unit vectors along the straight segments following the Q
+ // point of each active obstacle except the termination point.
+ Array_<UnitVec3,ActiveObstacleIndex> eIn_G;
+
+ // Multiply these unit spatial forces by tension to get body spatial
+ // forces in Ground.
+ Array_<SpatialVec,ActiveObstacleIndex> Fu_GB;
+
+
+ // SURFACE OBSTACLES
+ // Each surface obstacle has an event witness (trigger) function that
+ // passes through zero when the cable lifts off or touches down on this
+ // obstacle.
+ Array_<Real,SurfaceObstacleIndex> witnesses;
+ // Each inactive surface tracks the closest point on the surface to the
+ // path line segment that it would contact if it were active. We also
+ // keep the closest point on the path for display purposes. Both points
+ // are in the surface's local frame S. Only the
+ // slots for inactive surfaces have meaningful content here.
+ Array_<Vec3,SurfaceObstacleIndex> closestSurfacePoint;
+ Array_<Vec3,SurfaceObstacleIndex> closestPathPoint;
+
+
+ // ACTIVE SURFACES
+
+ // We calculate a geodesic for each active surface obstacle.
+ Array_<Geodesic,ActiveSurfaceIndex> geodesics;
+
+ // This is a dense vector with all the active surface obstacles' unknowns
+ // packed together.
+ Vector x; // nx unknowns
+
+ // This is the patherr corresponding to x and is always the same length.
+ Vector err; // patherr (nx of these)
+
+ // This is J(x) where J=partial(patherr)/partial(x), and its LU
+ // factorization for use in solving for length dot at Velocity stage.
+ // TODO: this is banded but we're treating it as full.
+ Matrix J; // nx X nx
+ FactorLU JInv;
+};
+
+
+// This is a cache entry for holding a path's calculated velocity-level
+// information.
+class PathVelEntry {
+public:
+ PathVelEntry() : lengthDot(NaN), unitPower(NaN) {}
+
+ // Set the number of obstacles n (including end points). We don't yet know
+ // the number of active obstacles. Any information previously here is
+ // lost.
+ void setNumObstacles(int n, int nSurfaces) {
+ initialize(0,0,0); // not yet
+ }
+
+ void initialize(int na, int nas, int nx) {
+ xdot.clear(); xdot.resize(nx);
+ nerrdotK.clear(); nerrdotK.resize(nx);
+ lengthDot = NaN;
+ unitPower = NaN;
+ }
+
+ Vector nerrdotK; // negative of frozen-contact point portion of patherrdot
+ Vector xdot; // calculated time derivatives of x; xdot=-J\errdotK
+ Real lengthDot;
+ Real unitPower; // unitForces*body velocities
+};
+
+std::ostream& operator<<(std::ostream& o, const PathInstanceInfo& info);
+std::ostream& operator<<(std::ostream& o, const PathPosEntry& entry);
+std::ostream& operator<<(std::ostream& o, const PathVelEntry& entry);
+
+
+
+//==============================================================================
+// CABLE OBSTACLE :: IMPL
+//==============================================================================
+// This is the internal implementation abstract base class for CableObstacle.
+// It is reference counted so that CableObstacle objects may be multiply
+// referenced.
+class CableObstacle::Impl {
+public:
+ explicit Impl(CablePath& cablePath, const MobilizedBody& mobod,
+ const Transform& defaultPose)
+ : referenceCount(0), cablePath(&cablePath), mobod(mobod),
+ defaultX_BS(defaultPose), defaultDisabled(false) {}
+
+ virtual ~Impl() {assert(referenceCount == 0);}
+
+ // Return the number of scalar coordinates needed to represent the
+ // location of one of this obstacle's contact points. This will be zero for
+ // via points, 2 for parametric surfaces, and 3 for
+ // implicit surfaces. This is also necessarily half the number of
+ // coordinates, their time derivatives, and the number of path error terms
+ // associated with this obstacle.
+ virtual int getNumCoordsPerContactPoint() const = 0;
+
+ // Concrete CableObstacle classes must implement these.
+
+ // Given coordinates xi=(xP,xQ) for this obstacle (taken from the supplied
+ // PathPosEntry), return the Cartesian positions of P and Q measured and
+ // expressed in the obstacle's S frame. For via points these are always
+ // at (0,0,0)_S. For implicit surfaces, the returned points will *not*
+ // necessarily be on the surface; that is, we are not doing any projection
+ // of xP and xQ here.
+ virtual void getContactPointsOnObstacle(const State&,
+ const PathInstanceInfo&,
+ const PathPosEntry&,
+ Vec3& P_S, Vec3& Q_S) const = 0;
+ // After geodesics have been calculated, this will return the length
+ // of the geodesic from P' to Q' for this obstacle, where P' and Q' are
+ // the nearest points on the surface corresponding to given points P and
+ // Q which for an implicit surface may be off the surface. For via points
+ // the length will be returned zero since we consider P and Q to be in the
+ // same place.
+ virtual Real getSegmentLength(const State&,
+ const PathInstanceInfo&,
+ const PathPosEntry&) const = 0;
+
+ // Once xdots have been calculated, this will return the rate of length
+ // change for the geodesic on this obstacle, or zero for via points. Note
+ // that the geodesic connects P' to Q'; see above for what that means.
+ virtual Real getSegmentLengthDot(const State&,
+ const PathInstanceInfo&,
+ const PathPosEntry&,
+ const PathVelEntry&) const = 0;
+
+ // Return the contact point stations in the body frame, that is, vectors
+ // from body origin Bo to point P and Q, expressed in B.
+ // Cost is 36 flops.
+ // TODO: should this be projected onto the surface?
+ void getContactStationsOnBody(const State& state,
+ const PathInstanceInfo& instInfo,
+ const PathPosEntry& ppe,
+ Vec3& P_B, Vec3& Q_B) const
+ { Vec3 P_S, Q_S;
+ getContactPointsOnObstacle(state, instInfo, ppe, P_S, Q_S);
+ const Transform& X_BS = getObstaclePoseOnBody(state, instInfo);
+ P_B = X_BS*P_S; Q_B = X_BS*Q_S; // 36 flops
+ }
+
+ // Return the contact point stations measured in the body frame, but
+ // re-expressed in Ground. This is still the vector from the body origin
+ // Bo to the contact points P and G; it is not measured from the ground
+ // origin. Cost is 66 flops.
+ // TODO: should this be projected onto the surface?
+ void expressContactStationsInGround(const State& state,
+ const PathInstanceInfo& instInfo,
+ const PathPosEntry& ppe,
+ Vec3& PB_G, Vec3& QB_G) const
+ { Vec3 P_B, Q_B;
+ getContactStationsOnBody(state, instInfo, ppe, P_B, Q_B); // 36 flops
+ const Rotation& R_GB = getBodyTransform(state).R();
+ PB_G = R_GB*P_B; QB_G = R_GB*Q_B; // 30 flops
+ }
+
+ const MobilizedBody& getMobilizedBody() const {return mobod;}
+ const Transform& getDefaultPoseOnBody() const {return defaultX_BS;}
+ const Transform& getObstaclePoseOnBody(const State& state,
+ const PathInstanceInfo& instInfo) const
+ { return instInfo.obstaclePose[index]; }
+ const Transform& getBodyTransform(const State& state) const
+ { return mobod.getBodyTransform(state); }
+ const SpatialVec& getBodyVelocity(const State& state) const
+ { return mobod.getBodyVelocity(state); }
+ void setCablePath(CablePath& path) { cablePath = &path; }
+ void setCableObstacleIndex(int ix) { index = CableObstacleIndex(ix); }
+ void setDisabledByDefault(bool shouldBeDisabled)
+ { defaultDisabled=shouldBeDisabled; }
+ bool isDisabledByDefault() const {return defaultDisabled;}
+ void invalidateTopology(); // see below
+
+ const DecorativeGeometry& getDecoration() const {return decoration;}
+ DecorativeGeometry& updDecoration() {return decoration;}
+ void setDecoration(const DecorativeGeometry& newGeo)
+ { decoration = newGeo; }
+
+protected:
+friend class CableObstacle;
+
+ // CablePath to which this obstacle belongs, and the index within that path.
+ CablePath* cablePath;
+ CableObstacleIndex index;
+
+ // MobilizedBody to which this obstacle is fixed.
+ const MobilizedBody mobod;
+ // Pose of surface frame (or point) on its body.
+ Transform defaultX_BS;
+ // How to draw this obstacle in the visualizer.
+ DecorativeGeometry decoration;
+
+ // Whether this obstacle should start out disabled.
+ bool defaultDisabled;
+
+ // The number of handles that reference this implementation object.
+ mutable int referenceCount;
+};
+
+
+
+//==============================================================================
+// CABLE PATH :: IMPL
+//==============================================================================
+// This is the internal implementation class for CablePath. It is reference
+// counted so that CablePath objects can be multiply referenced.
+class CablePath::Impl {
+public:
+ Impl() : referenceCount(0), cables(0) {}
+ Impl(CableTrackerSubsystem& cables)
+ : referenceCount(0), cables(&cables) {}
+ ~Impl() {assert(referenceCount == 0);}
+
+ int getNumObstacles() const {return obstacles.size();}
+ const CableObstacle& getObstacle(CableObstacleIndex ix) const
+ { return obstacles[ix]; }
+ const CableObstacle::Impl& getObstacleImpl(CableObstacleIndex ix) const
+ { return obstacles[ix].getImpl(); }
+
+ // Insert an obstacle into the path, just before the termination point
+ // which is always the last entry. The first two are the origin point
+ // and then the termination point.
+ CableObstacleIndex adoptObstacle(CableObstacle& obstacle)
+ { if (obstacles.size() < 2) {
+ obstacles.push_back(obstacle);
+ return CableObstacleIndex(obstacles.size()-1);
+ }
+ obstacles.insert(obstacles.end()-1, obstacle);
+ // Update the relocated termination point's obstacle index.
+ obstacles.back().updImpl().setCableObstacleIndex(obstacles.size()-1);
+ return CableObstacleIndex(obstacles.size()-2);
+ }
+
+ void solveForInitialCablePath(State& state) const;
+
+ Real getCableLength(const State& state) const {
+ const PathPosEntry& posEntry = getPosEntry(state);
+ return posEntry.length;
+ }
+
+ Real getCableLengthDot(const State& state) const {
+ const PathVelEntry& velEntry = getVelEntry(state);
+ return velEntry.lengthDot;
+ }
+
+ void applyBodyForces(const State& state, Real tension,
+ Vector_<SpatialVec>& bodyForcesInG) const;
+
+ // TODO: this isn't right when the tension goes to zero during rapid
+ // shrinking due to dissipation cancelling stiffness.
+ Real calcCablePower(const State& state, Real tension) const {
+ if (tension <= 0) return 0;
+ const PathVelEntry& velEntry = getVelEntry(state);
+ return tension * velEntry.unitPower;
+ }
+
+ // Allocate state variables and cache entries.
+ void realizeTopology(State& state);
+
+ // We know which obstacles are disabled. Assume the rest are active
+ void realizeInstance(const State& state) const;
+
+ // Solve for the new path and its length L, evaluate event witnesses to
+ // monitor for liftoff or touchdown.
+ void realizePosition(const State& state) const;
+
+ // Evaluate the path length rate of change (Ldot).
+ void realizeVelocity(const State& state) const;
+
+ void realizeAcceleration(const State& state) const;
+
+ void calcEventTriggerInfo
+ (const State&, Array_<EventTriggerInfo>&) const;
+
+ void handleEvents
+ (State&, Event::Cause, const Array_<EventId>& eventIds,
+ const HandleEventsOptions& options, HandleEventsResults& results) const;
+
+ // Update the cable path and its length in the state cache. This is the
+ // expensive part of the cable path computation. State must already have
+ // been realized to position stage. This is invoked automatically when
+ // you request access to the position cache entry.
+ void ensurePositionKinematicsCalculated(const State& state) const;
+
+ // Calculate the cable's rate of length change and put the result in
+ // the state cache. State must already have been realized to Velocity stage.
+ // This is invoked automatically when you request access to the velocity
+ // cache entry.
+ void ensureVelocityKinematicsCalculated(const State& state) const;
+
+ // Methods for convenient access to state variables and cache entries.
+
+ const PathInstanceInfo& getInstanceInfo(const State& state) const
+ { return Value<PathInstanceInfo>::downcast
+ (cables->getDiscreteVariable(state, instanceInfoIx)); }
+ PathInstanceInfo& updInstanceInfo(State& state) const
+ { return Value<PathInstanceInfo>::updDowncast
+ (cables->updDiscreteVariable(state, instanceInfoIx)); }
+
+ Real getIntegratedLengthDot(const State& state) const
+ { return cables->getZ(state)[integratedLengthDotIx]; }
+
+ void setIntegratedLengthDot(State& state, Real value) const
+ { cables->updZ(state)[integratedLengthDotIx] = value; }
+
+ // Return the *previous* path pos entry.
+ const PathPosEntry& getPrevPosEntry(const State& state) const
+ { return Value<PathPosEntry>::downcast
+ (cables->getDiscreteVariable(state, posEntryIx)); }
+ PathPosEntry& updPrevPosEntry(State& state) const
+ { return Value<PathPosEntry>::updDowncast
+ (cables->updDiscreteVariable(state, posEntryIx)); }
+ // Return the *previous* path vel entry.
+ const PathVelEntry& getPrevVelEntry(const State& state) const
+ { return Value<PathVelEntry>::downcast
+ (cables->getDiscreteVariable(state, velEntryIx)); }
+ PathVelEntry& updPrevVelEntry(State& state) const
+ { return Value<PathVelEntry>::updDowncast
+ (cables->updDiscreteVariable(state, velEntryIx)); }
+
+ // Lazy-evaluate position kinematics and return the result.
+ const PathPosEntry& getPosEntry(const State& state) const
+ { ensurePositionKinematicsCalculated(state);
+ return Value<PathPosEntry>::downcast
+ (cables->getDiscreteVarUpdateValue(state, posEntryIx)); }
+
+ PathPosEntry& updPosEntry(const State& state) const
+ { return Value<PathPosEntry>::updDowncast
+ (cables->updDiscreteVarUpdateValue(state, posEntryIx)); }
+
+ // Lazy-evaluate velocity kinematics and return the result.
+ const PathVelEntry& getVelEntry(const State& state) const
+ { ensureVelocityKinematicsCalculated(state);
+ return Value<PathVelEntry>::downcast
+ (cables->getDiscreteVarUpdateValue(state, velEntryIx)); }
+
+ PathVelEntry& updVelEntry(const State& state) const
+ { return Value<PathVelEntry>::updDowncast
+ (cables->updDiscreteVarUpdateValue(state, velEntryIx)); }
+
+ // Be sure to call this whenever you make a topology-level change to
+ // the cable definition, like adding an obstacle or modifying one in
+ // a significant way.
+ void invalidateTopology()
+ { if (cables) cables->invalidateSubsystemTopologyCache(); }
+
+ // Given kinematics and a set of contact point coordinates x (already in
+ // PathPosEntry), calculate the resulting path errors and related
+ // quantities with the result going back into PathPosEntry.
+ void calcPathError
+ (const State&, const PathInstanceInfo&, PathPosEntry&) const;
+
+ // Given kinematics K and a set of contact point coordinates x (already in
+ // PathPosEntry), calculate the Jacobian D patherr(K;x)/Dx, with the
+ // result going back into PathPosEntry. This is a banded matrix assembled
+ // from individual blocks provided by the active surface obstacles.
+ void calcPathErrorJacobian
+ (const State&, const PathInstanceInfo&, PathPosEntry&) const;
+
+private:
+friend class CablePath;
+
+ // Starting with the current guess in PathPosEntry, project each contact
+ // point to the nearest point on the surface.
+ void projectOntoSurface(const PathInstanceInfo&, PathPosEntry&) const;
+
+ // Starting with an initial guess for the contact coordinates x, make
+ // repeated calls to calcPathError to drive the path errors to zero by
+ // modifying x in PathPosEntry.
+ void solveForPathPoints
+ (const State&, const PathInstanceInfo&, PathPosEntry&) const;
+
+ // Given a solved path, compute the term of the path error time derivative
+ // due only to kinematics, with path points frozen on their surfaces.
+ // Results go into the errdotK member of the velocity cache entry. This
+ // is used to solve the for xdot such that the total path error time
+ // derivative is zero.
+ void findKinematicVelocityErrors
+ (const State&, const PathInstanceInfo&, const PathPosEntry&,
+ PathVelEntry&) const;
+
+ // Call this for any obstacle except the end points to obtain the
+ // segment between the previous and next active obstacles which is the
+ // part with which this obstacle may interact. The points are returned
+ // in the local frame S of the given obstacle.
+ void findPathSegmentForObstacle
+ (const State&, const PathInstanceInfo&, const PathPosEntry&,
+ CableObstacleIndex ox, Vec3& Qprev_S, Vec3& Pnext_S) const;
+
+
+ // Subsystem to which this path belongs, and the index within that
+ // subsystem.
+ CableTrackerSubsystem* cables;
+ CablePathIndex index;
+
+ // The list of via points and surfaces, ordered by expected path
+ // coordinate. The first obstacle is the origin point; the last is the
+ // termination point.
+ Array_<CableObstacle,CableObstacleIndex> obstacles;
+
+ // TOPOLOGY CACHE (set during realizeTopology())
+ DiscreteVariableIndex instanceInfoIx;
+ ZIndex integratedLengthDotIx;
+ DiscreteVariableIndex posEntryIx; // these are auto-update
+ DiscreteVariableIndex velEntryIx;
+ EventTriggerByStageIndex eventIx; // 1st index; one for each surface
+ std::map<EventId, CableObstacleIndex> mapEventIdToObstacle;
+
+ mutable int referenceCount;
+};
+
+
+
+//==============================================================================
+// CABLE OBSTACLE :: VIA POINT :: IMPL
+//==============================================================================
+class CableObstacle::ViaPoint::Impl : public CableObstacle::Impl {
+ typedef CableObstacle::Impl Super;
+public:
+ Impl(CablePath& path, const MobilizedBody& viaMobod,
+ const Vec3& station)
+ : Super(path, viaMobod, station)
+ { decoration = DecorativePoint().setColor(Red); }
+
+ int getNumCoordsPerContactPoint() const {return 0;}
+
+ void getContactPointsOnObstacle(const State& state,
+ const PathInstanceInfo& instInfo,
+ const PathPosEntry& posEntry,
+ Vec3& P_S, Vec3& Q_S) const OVERRIDE_11
+ {
+ P_S = Q_S = Vec3(0);
+ }
+
+ Real getSegmentLength(const State& state,
+ const PathInstanceInfo& instInfo,
+ const PathPosEntry& posInfo) const OVERRIDE_11
+ { return 0; } // via point has no segment
+
+ Real getSegmentLengthDot( const State& state,
+ const PathInstanceInfo& instInfo,
+ const PathPosEntry& posInfo,
+ const PathVelEntry& velInfo) const OVERRIDE_11
+ { return 0; } // via point has no segment
+};
+
+
+
+//==============================================================================
+// CABLE OBSTACLE :: SURFACE :: IMPL
+//==============================================================================
+class CableObstacle::Surface::Impl : public CableObstacle::Impl {
+ typedef CableObstacle::Impl Super;
+public:
+ Impl(CablePath& path, const MobilizedBody& mobod,
+ const Transform& pose, const ContactGeometry& geom)
+ : Super(path, mobod, pose), surface(geom),
+ nearPointInS(NaN), xPhint(NaN), xQhint(NaN)
+ { decoration = geom.createDecorativeGeometry()
+ .setColor(Orange).setOpacity(.75).setResolution(3); }
+
+ const ContactGeometry& getContactGeometry() const {return surface;}
+
+ // Hardcoded for implicit surfaces -- would be 2 for parametric.
+ int getNumCoordsPerContactPoint() const {return 3;}
+
+ void getContactPointsOnObstacle(const State& state,
+ const PathInstanceInfo& instInfo,
+ const PathPosEntry& ppe,
+ Vec3& P_S, Vec3& Q_S) const OVERRIDE_11
+ {
+ const ActiveSurfaceIndex asx = ppe.mapToActiveSurface[index];
+ const int xSlot = ppe.mapToCoords[index];
+ assert(asx.isValid() && xSlot >= 0);
+ P_S = Vec3::getAs(&ppe.x[xSlot]); // implicit coords are point in S
+ Q_S = Vec3::getAs(&ppe.x[xSlot+3]);
+ }
+
+ Real getSegmentLength(const State& state,
+ const PathInstanceInfo& instInfo,
+ const PathPosEntry& ppe) const OVERRIDE_11
+ { const ActiveSurfaceIndex asx = ppe.mapToActiveSurface[index];
+ assert(asx.isValid());
+ const Geodesic& geod = ppe.geodesics[asx];
+ return geod.getLength();
+ }
+
+
+ Real getSegmentLengthDot( const State& state,
+ const PathInstanceInfo& instInfo,
+ const PathPosEntry& ppe,
+ const PathVelEntry& pve) const OVERRIDE_11
+ { const ActiveSurfaceIndex asx = ppe.mapToActiveSurface[index];
+ const int xSlot = ppe.mapToCoords[index];
+ assert(asx.isValid() && xSlot >= 0);
+ // contact point velocities
+ const Vec3 xdotP = Vec3::getAs(&pve.xdot[xSlot]);
+ const Vec3 xdotQ = Vec3::getAs(&pve.xdot[xSlot+3]);
+ const Geodesic& geod = ppe.geodesics[asx];
+ return geod.calcLengthDot(xdotP,xdotQ);
+ }
+
+ // During initialization, call this to calculate an appropriate geodesic
+ // connecting points P' and Q', the nearest surface points to points P and
+ // Q whose surface coordinates are supplied.
+ // There may be multiple geodesics between P' and Q'. If a near point
+ // N has been provided, we'll return the geodesic that passes closest to it,
+ // otherwise we'll return the shortest one. If entry and exit hints are
+ // provided they may be used to bias the algorithm.
+ void initSurfacePath(const Vec3& xP,
+ const Vec3& xQ,
+ const UnitVec3& entryHint_S, // optional (use NaN)
+ const UnitVec3& exitHint_S, // optional (use NaN)
+ Geodesic& path) const;
+
+ // Calculate the surface-local path error due to the given entry/exit
+ // points and directions being inconsistent with a geodesic connecting
+ // those points.
+ Vec6 calcSurfacePathError( const Geodesic& previous,
+ const UnitVec3& entryDir_S,
+ const Vec3& xP,
+ const Vec3& xQ,
+ const UnitVec3& exitDir_S,
+ Geodesic& next) const;
+
+ // Calculate the 6-vector surface-local path error due to the given
+ // entry/exit points and directions being inconsistent with a geodesic
+ // connecting those points, and the 6x12 Jacobian of the errors with respect
+ // to each of the 12 measure numbers of the 4 input vectors.
+ void calcSurfacePathErrorJacobianAnalytically
+ (const UnitVec3& entryDir_S,
+ const Vec3& xP_S,
+ const Vec3& xQ_S,
+ const UnitVec3& exitDir_S,
+ const Geodesic& current, // Must have used above parameters.
+ Mat63& DerrDentry, // 4x3 for parametric
+ Mat63& DerrDxP, // 4x2 "
+ Mat63& DerrDxQ, // 4x2 "
+ Mat63& DerrDexit) // 4x3 "
+ const;
+
+
+ // Calculate partial derivatives of the surface path error with respect to
+ // each of its four arguments.
+ void calcSurfacePathErrorJacobianNumerically
+ (const Geodesic& previous,
+ const UnitVec3& entryDir_S,
+ const Vec3& xP,
+ const Vec3& xQ,
+ const UnitVec3& exitDir_S,
+ const Geodesic& current, // Must have used above parameters.
+ Mat63& DerrDentry, // 4x3 for parametric
+ Mat63& DerrDxP, // 4x2 "
+ Mat63& DerrDxQ, // 4x2 "
+ Mat63& DerrDexit) // 4x3 "
+ const;
+
+ // Calculate the *negated* surface-local contribution to the path error time
+ // derivative due only to material point velocities, with the surface
+ // coordinates xP and xQ held constant at the given values.
+ // This is nerrdotK = -(Dpatherr(K;x)/DK)*(dK/dt).
+ Vec6 calcSurfaceNegKinematicVelocityError
+ ( const Geodesic& geodesic,
+ const UnitVec3& entryDir_S,
+ const Vec3& entryDirDot_S, // time derivative in S
+ const Vec3& xP,
+ const Vec3& xQ,
+ const UnitVec3& exitDir_S,
+ const Vec3& exitDirDot_S) const;
+
+ bool hasNearPoint() const {return !nearPointInS.isNaN(); }
+ const Vec3& getNearPoint() const {return nearPointInS;}
+
+ bool hasContactPointHints() const
+ { return !(xPhint.isNaN() || xQhint.isNaN()); }
+ void getContactPointHints(Vec3& xPhint, Vec3& xQhint) const
+ { xPhint = this->xPhint; xQhint = this->xQhint; }
+
+private:
+friend class CableObstacle::Surface;
+
+ ContactGeometry surface;
+ Vec3 nearPointInS; // Cartesian location of N, in S frame
+ Vec3 xPhint, xQhint;
+};
+
+
+#endif // SimTK_SIMBODY_CABLE_PATH_IMPL_H_
+
diff --git a/Simbody/src/CableSpring.cpp b/Simbody/src/CableSpring.cpp
new file mode 100644
index 0000000..51d0f65
--- /dev/null
+++ b/Simbody/src/CableSpring.cpp
@@ -0,0 +1,321 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2012-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/MobilizedBody.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "simbody/internal/CableSpring.h"
+
+#include "ForceImpl.h"
+
+using namespace SimTK;
+
+//==============================================================================
+// CABLE SPRING :: IMPL
+//==============================================================================
+// This is the hidden implementation class for a CableSpring force element.
+class CableSpring::Impl : public ForceImpl {
+friend class CableSpring;
+
+ // Type of the discrete state variable that holds values for this
+ // cable spring's changeable parameters in a State.
+ struct InstanceVars {
+ InstanceVars(Real defStiffness, Real defSlackLength,
+ Real defDissipationCoef)
+ : k(defStiffness), L0(defSlackLength), c(defDissipationCoef) {}
+
+ Real k, L0, c;
+ };
+
+ // Type of the velocity-stage lazy cache entry that holds the spring
+ // tension and instantaneous power if either has been requested.
+ struct ForceCache {
+ Real f; // scalar tension
+ Real powerLoss;
+ };
+
+ Impl(const CablePath& path,
+ Real stiffness,
+ Real slackLength,
+ Real dissipationCoef)
+ : path(path), defK(stiffness), defL0(slackLength), defC(dissipationCoef)
+ {
+ }
+
+ // Implementation of virtual methods from ForceImpl:
+ Impl* clone() const OVERRIDE_11 {return new Impl(*this);}
+ bool dependsOnlyOnPositions() const OVERRIDE_11 {return false;}
+
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+ OVERRIDE_11
+ {
+ const ForceCache& forceCache = ensureForceCacheValid(state);
+ path.applyBodyForces(state, forceCache.f, bodyForces);
+ }
+
+ // We're not bothering to cache P.E. -- just recalculate it when asked.
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11
+ {
+ const InstanceVars& inst = getInstanceVars(state);
+ const Real x = calcStretch(state, inst); // x >= 0
+ const Real pe = inst.k * x*x / 2;
+ return pe;
+ }
+
+ // Allocate the state variables and cache entry.
+ void realizeTopology(State& s) const OVERRIDE_11 {
+ // Allocate the discrete variable for instance parameters.
+ const InstanceVars iv(defK,defL0,defC);
+ instanceVarsIx = getForceSubsystem()
+ .allocateDiscreteVariable(s, Stage::Instance,
+ new Value<InstanceVars>(iv));
+
+ // Allocate a continuous variable to hold the integrated power loss.
+ const Vector einit(1, Real(0));
+ dissipatedEnergyIx = getForceSubsystem().allocateZ(s,einit);
+
+ // Allocate a lazy cache entry for force and power loss.
+ forceCacheIx = getForceSubsystem().allocateLazyCacheEntry(s,
+ Stage::Velocity, new Value<ForceCache>());
+ }
+
+ // We must always evaluate the power if we're going to calculate its
+ // integral.
+ void realizeAcceleration(const State& s) const {
+ const ForceCache& forceCache = ensureForceCacheValid(s);
+ updDissipatedEnergyDeriv(s) = forceCache.powerLoss;
+ }
+
+ // Return the amount x by which the cable is stretched beyond its slack
+ // length or zero if the cable is slack. Must be at stage Position.
+ Real calcStretch(const State& state, const InstanceVars& inst) const {
+ const Real x0 = path.getCableLength(state) - inst.L0; // signed
+ return std::max(Real(0), x0); // return x >= 0
+ }
+
+ // Calculate tension f=f_stretch+f_rate, and power loss f_rate*xdot. See
+ // theory discussion for CableSpring class for an explanation. Must be at
+ // stage Velocity.
+ void calcTensionAndPowerLoss(const State& state,
+ Real& f, Real& powerLoss) const
+ {
+ const InstanceVars& inst = getInstanceVars(state);
+ const Real x = calcStretch(state, inst); // >= 0
+ if (x == 0) {powerLoss = f = 0; return;} // cable spring is slack
+
+ const Real f_stretch = inst.k * x;
+ const Real xdot = path.getCableLengthDot(state);
+ const Real diss = f_stretch*inst.c*xdot;
+ const Real f_rate = std::max(-f_stretch, diss);
+ f = f_stretch + f_rate; // f=0 if dissipation too negative
+ powerLoss = f_rate * xdot; // but can still dissipate power
+ }
+
+ // If state is at stage Velocity, we can calculate and store tension
+ // in the cache if it hasn't already been calculated.
+ const ForceCache& ensureForceCacheValid(const State& state) const {
+ if (isForceCacheValid(state))
+ return getForceCache(state);
+ ForceCache& forceCache = updForceCache(state);
+ calcTensionAndPowerLoss(state, forceCache.f, forceCache.powerLoss);
+ markForceCacheValid(state);
+ return forceCache;
+ }
+
+ const InstanceVars& getInstanceVars(const State& s) const
+ { return Value<InstanceVars>::downcast
+ (getForceSubsystem().getDiscreteVariable(s,instanceVarsIx)); }
+ InstanceVars& updInstanceVars(State& s) const
+ { return Value<InstanceVars>::updDowncast
+ (getForceSubsystem().updDiscreteVariable(s,instanceVarsIx)); }
+
+ const Real& getDissipatedEnergyVar(const State& s) const
+ { return getForceSubsystem().getZ(s)[dissipatedEnergyIx]; }
+ Real& updDissipatedEnergyVar(State& s) const
+ { return getForceSubsystem().updZ(s)[dissipatedEnergyIx]; }
+ Real& updDissipatedEnergyDeriv(const State& s) const
+ { return getForceSubsystem().updZDot(s)[dissipatedEnergyIx]; }
+
+ const ForceCache& getForceCache(const State& s) const
+ { return Value<ForceCache>::downcast
+ (getForceSubsystem().getCacheEntry(s,forceCacheIx)); }
+ ForceCache& updForceCache(const State& s) const
+ { return Value<ForceCache>::updDowncast
+ (getForceSubsystem().updCacheEntry(s,forceCacheIx)); }
+
+ bool isForceCacheValid(const State& s) const
+ { return getForceSubsystem().isCacheValueRealized(s,forceCacheIx); }
+ void markForceCacheValid(const State& s) const
+ { getForceSubsystem().markCacheValueRealized(s,forceCacheIx); }
+
+
+//------------------------------------------------------------------------------
+ // TOPOLOGY STATE
+ const CablePath path; // shallow, reference counted copy
+ Real defK, defL0, defC;
+
+ // TOPOLOGY CACHE (Set only once in realizeTopology(); const thereafter.)
+ mutable DiscreteVariableIndex instanceVarsIx; // k, L0, c
+ mutable ZIndex dissipatedEnergyIx;
+ mutable CacheEntryIndex forceCacheIx;
+};
+
+
+//==============================================================================
+// CABLE SPRING DEFINITIONS
+//==============================================================================
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(CableSpring,
+ CableSpring::Impl, Force);
+
+CableSpring::CableSpring
+ (GeneralForceSubsystem& forces,
+ const CablePath& path,
+ Real defaultStiffness,
+ Real defaultSlackLength,
+ Real defaultDissipationCoef)
+: Force(new CableSpring::Impl(path, defaultStiffness, defaultSlackLength,
+ defaultDissipationCoef))
+{
+ SimTK_ERRCHK_ALWAYS(defaultStiffness >= 0,
+ "CableSpring constructor", "Spring constant must be nonnegative.");
+ SimTK_ERRCHK_ALWAYS(defaultSlackLength >= 0,
+ "CableSpring constructor", "Slack length must be nonnegative.");
+ SimTK_ERRCHK_ALWAYS(defaultDissipationCoef >= 0,
+ "CableSpring constructor", "Dissipation coefficient must be nonnegative.");
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+CableSpring& CableSpring::
+setDefaultStiffness(Real stiffness) {
+ SimTK_ERRCHK_ALWAYS(stiffness >= 0,
+ "CableSpring::setDefaultStiffness()",
+ "Spring constant must be nonnegative.");
+ getImpl().invalidateTopologyCache();
+ updImpl().defK = stiffness;
+ return *this;
+}
+CableSpring& CableSpring::
+setDefaultSlackLength(Real slackLength) {
+ SimTK_ERRCHK_ALWAYS(slackLength >= 0,
+ "CableSpring::setDefaultSlackLength()",
+ "Slack length must be nonnegative.");
+ getImpl().invalidateTopologyCache();
+ updImpl().defL0 = slackLength;
+ return *this;
+}
+CableSpring& CableSpring::
+setDefaultDissipationCoef(Real dissipationCoef) {
+ SimTK_ERRCHK_ALWAYS(dissipationCoef >= 0,
+ "CableSpring::setDefaultDissipationCoef()",
+ "Dissipation coefficient must be nonnegative.");
+ getImpl().invalidateTopologyCache();
+ updImpl().defC = dissipationCoef;
+ return *this;
+}
+
+Real CableSpring::
+getDefaultStiffness() const {return getImpl().defK;}
+Real CableSpring::
+getDefaultSlackLength() const {return getImpl().defL0;}
+Real CableSpring::
+getDefaultDissipationCoef() const {return getImpl().defC;}
+
+const CableSpring& CableSpring::
+setStiffness(State& state, Real stiffness) const {
+ SimTK_ERRCHK_ALWAYS(stiffness >= 0,
+ "CableSpring::setStiffness()",
+ "Spring constant must be nonnegative.");
+ getImpl().updInstanceVars(state).k = stiffness;
+ return *this;
+}
+const CableSpring& CableSpring::
+setSlackLength(State& state, Real slackLength) const {
+ SimTK_ERRCHK_ALWAYS(slackLength >= 0,
+ "CableSpring::setSlackLength()",
+ "Slack length must be nonnegative.");
+ getImpl().updInstanceVars(state).L0 = slackLength;
+ return *this;
+}
+const CableSpring& CableSpring::
+setDissipationCoef(State& state, Real dissipationCoef) const {
+ SimTK_ERRCHK_ALWAYS(dissipationCoef >= 0,
+ "CableSpring::setDissipationCoef()",
+ "Dissipation coefficient must be nonnegative.");
+ getImpl().updInstanceVars(state).c = dissipationCoef;
+ return *this;
+}
+
+Real CableSpring::
+getStiffness(const State& state) const
+{ return getImpl().getInstanceVars(state).k; }
+Real CableSpring::
+getSlackLength(const State& state) const
+{ return getImpl().getInstanceVars(state).L0; }
+Real CableSpring::
+getDissipationCoef(const State& state) const
+{ return getImpl().getInstanceVars(state).c; }
+
+Real CableSpring::
+getLength(const State& state) const
+{ return getImpl().path.getCableLength(state); }
+Real CableSpring::
+getLengthDot(const State& state) const
+{ return getImpl().path.getCableLengthDot(state); }
+
+
+Real CableSpring::
+getTension(const State& s) const
+{ getImpl().ensureForceCacheValid(s);
+ return getImpl().getForceCache(s).f; }
+
+Real CableSpring::
+getPotentialEnergy(const State& s) const
+{ return getImpl().calcPotentialEnergy(s); }
+
+Real CableSpring::
+getPowerDissipation(const State& s) const
+{ getImpl().ensureForceCacheValid(s);
+ return getImpl().getForceCache(s).powerLoss; }
+
+Real CableSpring::
+getDissipatedEnergy(const State& s) const
+{ return getImpl().getDissipatedEnergyVar(s); }
+
+void CableSpring::
+setDissipatedEnergy(State& s, Real energy) const {
+ SimTK_ERRCHK1_ALWAYS(energy >= 0,
+ "CableSpring::setDissipatedEnergy()",
+ "The initial value for the dissipated energy must be nonnegative"
+ " but an attempt was made to set it to %g.", energy);
+ getImpl().updDissipatedEnergyVar(s) = energy;
+}
+
+const CablePath& CableSpring::
+getCablePath() const
+{ return getImpl().path; }
+
+
diff --git a/Simbody/src/CableTrackerSubsystem.cpp b/Simbody/src/CableTrackerSubsystem.cpp
new file mode 100644
index 0000000..2596844
--- /dev/null
+++ b/Simbody/src/CableTrackerSubsystem.cpp
@@ -0,0 +1,87 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Ian Stavness, Andreas Scholz *
+ * Contributors: *
+ * *
+ * 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/MultibodySystem.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "simbody/internal/CableTrackerSubsystem.h"
+
+#include "CablePath_Impl.h"
+#include "CableTrackerSubsystem_Impl.h"
+
+#include <cassert>
+#include <iostream>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+//==============================================================================
+// CABLE TRACKER SUBSYSTEM
+//==============================================================================
+
+bool CableTrackerSubsystem::isInstanceOf(const Subsystem& s) {
+ return Impl::isA(s.getSubsystemGuts());
+}
+
+const CableTrackerSubsystem& CableTrackerSubsystem::
+downcast(const Subsystem& s) {
+ assert(isInstanceOf(s));
+ return static_cast<const CableTrackerSubsystem&>(s);
+}
+CableTrackerSubsystem& CableTrackerSubsystem::
+updDowncast(Subsystem& s) {
+ assert(isInstanceOf(s));
+ return static_cast<CableTrackerSubsystem&>(s);
+}
+
+const CableTrackerSubsystem::Impl& CableTrackerSubsystem::
+getImpl() const {
+ return SimTK_DYNAMIC_CAST_DEBUG<const Impl&>(getSubsystemGuts());
+}
+CableTrackerSubsystem::Impl& CableTrackerSubsystem::
+updImpl() {
+ return SimTK_DYNAMIC_CAST_DEBUG<Impl&>(updSubsystemGuts());
+}
+
+// Create Subsystem but don't associate it with any System. This isn't much use
+// except for making std::vectors, which require a default constructor to be
+// available.
+CableTrackerSubsystem::CableTrackerSubsystem()
+{ adoptSubsystemGuts(new Impl()); }
+
+CableTrackerSubsystem::CableTrackerSubsystem(MultibodySystem& mbs)
+{ adoptSubsystemGuts(new Impl());
+ mbs.adoptSubsystem(*this); } // steal ownership
+
+int CableTrackerSubsystem::getNumCablePaths() const
+{ return getImpl().getNumCablePaths(); }
+
+const CablePath& CableTrackerSubsystem::
+getCablePath(CablePathIndex cableIx) const
+{ return getImpl().getCablePath(cableIx); }
+
+CablePath& CableTrackerSubsystem::
+updCablePath(CablePathIndex cableIx)
+{ return updImpl().updCablePath(cableIx); }
+
diff --git a/Simbody/src/CableTrackerSubsystem_Impl.h b/Simbody/src/CableTrackerSubsystem_Impl.h
new file mode 100644
index 0000000..5147bf9
--- /dev/null
+++ b/Simbody/src/CableTrackerSubsystem_Impl.h
@@ -0,0 +1,268 @@
+#ifndef SimTK_SIMBODY_CABLE_TRACKER_SUBSYSTEM_IMPL_H_
+#define SimTK_SIMBODY_CABLE_TRACKER_SUBSYSTEM_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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Ian Stavness, Andreas Scholz *
+ * Contributors: *
+ * *
+ * 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/MultibodySystem.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "simbody/internal/CableTrackerSubsystem.h"
+#include "simbody/internal/CablePath.h"
+
+#include "CablePath_Impl.h"
+
+#include <cassert>
+#include <iostream>
+using std::cout; using std::endl;
+
+namespace SimTK {
+
+//==============================================================================
+// CABLE TRACKER SUBSYSTEM :: IMPL
+//==============================================================================
+// This is the private implementation of CableTrackerSubsystem.
+
+class CableTrackerSubsystem::Impl : public Subsystem::Guts {
+public:
+// Constructor registers a default set of Trackers to use with geometry
+// we know about. These can be overridden later.
+Impl() {}
+
+~Impl() {}
+
+Impl* cloneImpl() const OVERRIDE_11
+{ return new Impl(*this); }
+
+int getNumCablePaths() const {return cablePaths.size();}
+
+const CablePath& getCablePath(CablePathIndex index) const
+{ return cablePaths[index]; }
+
+CablePath& updCablePath(CablePathIndex index)
+{ return cablePaths[index]; }
+
+// Add a cable path to the list, bumping the reference count.
+CablePathIndex adoptCablePath(CablePath& path) {
+ cablePaths.push_back(path);
+ return CablePathIndex(cablePaths.size()-1);
+}
+
+// Return the MultibodySystem which owns this ContactTrackerSubsystem.
+const MultibodySystem& getMultibodySystem() const
+{ return MultibodySystem::downcast(getSystem()); }
+
+// Return the SimbodyMatterSubsystem from which this ContactTrackerSubsystem
+// gets the bodies to track.
+const SimbodyMatterSubsystem& getMatterSubsystem() const
+{ return getMultibodySystem().getMatterSubsystem(); }
+
+// Get access to state variables and cache entries.
+// TODO
+
+void calcEventTriggerInfoImpl
+ (const State& state, Array_<EventTriggerInfo>& info) const OVERRIDE_11
+{
+ for (CablePathIndex ix(0); ix < cablePaths.size(); ++ix) {
+ const CablePath& path = getCablePath(ix);
+ path.getImpl().calcEventTriggerInfo(state,info);
+ }
+}
+
+void handleEventsImpl
+ (State& state, Event::Cause cause, const Array_<EventId>& eventIds,
+ const HandleEventsOptions& options,
+ HandleEventsResults& results) const OVERRIDE_11
+{
+ for (CablePathIndex ix(0); ix < cablePaths.size(); ++ix) {
+ const CablePath& path = getCablePath(ix);
+ path.getImpl().handleEvents(state,cause,eventIds,options,results);
+ getSystem().realize(state, Stage::Position); // TODO
+ }
+}
+
+// Allocate state variables.
+int realizeSubsystemTopologyImpl(State& state) const OVERRIDE_11 {
+ // Briefly allow writing into the Topology cache; after this the
+ // Topology cache is const.
+ Impl* wThis = const_cast<Impl*>(this);
+
+ for (CablePathIndex ix(0); ix < cablePaths.size(); ++ix) {
+ CablePath& path = wThis->updCablePath(ix);
+ path.updImpl().realizeTopology(state);
+ }
+
+ return 0;
+}
+
+int realizeSubsystemInstanceImpl(const State& state) const OVERRIDE_11 {
+ for (CablePathIndex ix(0); ix < cablePaths.size(); ++ix) {
+ const CablePath& path = getCablePath(ix);
+ path.getImpl().realizeInstance(state);
+ }
+ return 0;
+}
+
+int realizeSubsystemPositionImpl(const State& state) const OVERRIDE_11 {
+ for (CablePathIndex ix(0); ix < cablePaths.size(); ++ix) {
+ const CablePath& path = getCablePath(ix);
+ path.getImpl().realizePosition(state);
+ }
+ return 0;
+}
+
+int realizeSubsystemVelocityImpl(const State& state) const OVERRIDE_11 {
+ for (CablePathIndex ix(0); ix < cablePaths.size(); ++ix) {
+ const CablePath& path = getCablePath(ix);
+ path.getImpl().realizeVelocity(state);
+ }
+ return 0;
+}
+
+
+int realizeSubsystemAccelerationImpl(const State& state) const OVERRIDE_11 {
+ for (CablePathIndex ix(0); ix < cablePaths.size(); ++ix) {
+ const CablePath& path = getCablePath(ix);
+ path.getImpl().realizeAcceleration(state);
+ }
+ return 0;
+}
+
+int calcDecorativeGeometryAndAppendImpl
+ (const State& state,
+ Stage stage,
+ Array_<DecorativeGeometry>& decorations) const OVERRIDE_11
+{
+ if (stage != Stage::Position)
+ return 0;
+
+ for (CablePathIndex ix(0); ix < cablePaths.size(); ++ix) {
+ const CablePath::Impl& path = getCablePath(ix).getImpl();
+ const PathInstanceInfo& instInfo = path.getInstanceInfo(state);
+ const PathPosEntry& ppe = path.getPosEntry(state);
+
+ Vec3 prevPoint;
+ for (CableObstacleIndex ox(0); ox < path.getNumObstacles(); ++ox) {
+ const CableObstacle::Impl& obs = path.getObstacleImpl(ox);
+ const Transform& X_GB = obs.getBodyTransform(state);
+ const Transform& X_BS = obs.getObstaclePoseOnBody(state,instInfo);
+ const Transform X_GS = X_GB*X_BS;
+
+ // Disabled obstacle is dimmed.
+ if (instInfo.obstacleDisabled[ox]) {
+ DecorativeGeometry geo = obs.getDecoration();
+ const Transform& X_SD = geo.getTransform();
+ decorations.push_back(
+ geo.setTransform(X_GS*X_SD).setColor(Real(0.75)*geo.getColor()));
+ continue;
+ }
+
+ // If this is a surface, draw it.
+ if (obs.getNumCoordsPerContactPoint() > 0) {
+ DecorativeGeometry geo = obs.getDecoration();
+ const Transform& X_SD = geo.getTransform();
+ decorations.push_back(geo.setTransform(X_GS*X_SD));
+ }
+
+ Vec3 P_B, Q_B;
+ obs.getContactStationsOnBody(state, instInfo, ppe, P_B, Q_B);
+ Vec3 P_G = X_GB*P_B, Q_G = X_GB*Q_B;
+
+ const Vec3 color = ox==0 ? Green
+ : ox==path.getNumObstacles()-1 ? Red : Purple;
+ // Draw point at Pi.
+ decorations.push_back(DecorativePoint(P_G).setColor(color));
+
+ if (ox!=0) // draw straight line from Qi-1 to Pi
+ decorations.push_back(DecorativeLine(prevPoint,P_G)
+ .setColor(Purple).setLineThickness(3));
+
+ // If there are two distinct points, draw the second one and a
+ // red curve connecting them.
+ if ((Q_G-P_G).normSqr() >= square(SignificantReal)) {
+ decorations.push_back(DecorativePoint(Q_G).setColor(color));
+
+ ActiveSurfaceIndex asx = ppe.mapToActiveSurface[ox];
+ assert(asx.isValid());
+ const Geodesic& g = ppe.geodesics[asx];
+ const Array_<Transform>& Kf = g.getFrenetFrames();
+ Vec3 prevP_G = X_GS*Kf.front().p();
+ for (unsigned i=0; i < Kf.size(); ++i) {
+ Vec3 Q_G = X_GS*Kf[i].p();
+ decorations.push_back(DecorativeLine(prevP_G, Q_G)
+ .setColor(Red).setLineThickness(2));
+ if (i < Kf.size()-1)
+ decorations.push_back(DecorativePoint(Q_G)
+ .setColor(Blue).setScale(.5));
+
+ prevP_G = Q_G;
+ }
+ }
+ prevPoint = Q_G;
+ }
+
+ // Draw "closest point" lines for inactive surfaces.
+ for (SurfaceObstacleIndex sox(0); sox < instInfo.getNumSurfaceObstacles();
+ ++sox)
+ {
+ const CableObstacleIndex ox = instInfo.mapSurfaceToObstacle[sox];
+ if (ppe.mapToActiveSurface[ox].isValid())
+ continue; // skip active surface obstacles
+
+ const CableObstacle::Surface::Impl& obs =
+ dynamic_cast<const CableObstacle::Surface::Impl&>
+ (path.getObstacleImpl(ox));
+
+ const Vec3 P_S = ppe.closestSurfacePoint[sox];
+ const Vec3 L_S = ppe.closestPathPoint[sox];
+
+ const Transform& X_BS = obs.getObstaclePoseOnBody(state,instInfo);
+ const MobilizedBody& B = obs.getMobilizedBody();
+ const Transform& X_GB = B.getBodyTransform(state);
+ const Transform X_GS = X_GB*X_BS;
+
+ const Vec3 P_G = X_GS*P_S, L_G = X_GS*L_S;
+ decorations.push_back(
+ DecorativePoint(P_G).setColor(Purple).setLineThickness(2));
+ decorations.push_back(
+ DecorativePoint(L_G).setColor(Purple).setLineThickness(2));
+ decorations.push_back(
+ DecorativeLine(P_G,L_G).setColor(Purple));
+ }
+ }
+ return 0;
+}
+
+
+SimTK_DOWNCAST(Impl, Subsystem::Guts);
+
+private:
+// TOPOLOGY STATE
+Array_<CablePath, CablePathIndex> cablePaths;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_CABLE_TRACKER_SUBSYSTEM_IMPL_H_
+
diff --git a/Simbody/src/CompliantContactSubsystem.cpp b/Simbody/src/CompliantContactSubsystem.cpp
new file mode 100644
index 0000000..38441dc
--- /dev/null
+++ b/Simbody/src/CompliantContactSubsystem.cpp
@@ -0,0 +1,1666 @@
+/* -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+Private implementation of CompliantContactSubsystem and the built in contact
+patch analysis methods.
+**/
+
+#include "SimTKcommon.h"
+
+#include "simbody/internal/common.h"
+#include "simbody/internal/ForceSubsystem.h"
+#include "simbody/internal/ForceSubsystemGuts.h"
+#include "simbody/internal/CompliantContactSubsystem.h"
+#include "simbody/internal/ContactTrackerSubsystem.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "simbody/internal/MultibodySystem.h"
+
+namespace SimTK {
+
+//==============================================================================
+// COMPLIANT CONTACT SUBSYSTEM IMPL
+//==============================================================================
+
+class CompliantContactSubsystemImpl : public ForceSubsystemRep {
+typedef std::map<ContactTypeId, const ContactForceGenerator*> GeneratorMap;
+public:
+
+CompliantContactSubsystemImpl(const ContactTrackerSubsystem& tracker)
+: ForceSubsystemRep("CompliantContactSubsystem", "0.0.1"),
+ m_tracker(tracker), m_transitionVelocity(Real(0.01)),
+ m_ooTransitionVelocity(1/m_transitionVelocity),
+ m_trackDissipatedEnergy(false), m_defaultGenerator(0)
+{
+}
+
+Real getTransitionVelocity() const {return m_transitionVelocity;}
+Real getOOTransitionVelocity() const {return m_ooTransitionVelocity;}
+void setTransitionVelocity(Real vt)
+{ m_transitionVelocity=vt; m_ooTransitionVelocity=1/vt;}
+
+void setTrackDissipatedEnergy(bool shouldTrack) {
+ if (m_trackDissipatedEnergy != shouldTrack) {
+ m_trackDissipatedEnergy = shouldTrack;
+ invalidateSubsystemTopologyCache();
+ }
+}
+bool getTrackDissipatedEnergy() const {return m_trackDissipatedEnergy;}
+
+int getNumContactForces(const State& s) const {
+ ensureForceCacheValid(s);
+ const Array_<ContactForce>& forces = getForceCache(s);
+ return (int)forces.size();
+}
+
+const ContactForce& getContactForce(const State& s, int n) const {
+ const int numContactForces = getNumContactForces(s); // realize if needed
+ SimTK_ERRCHK2_ALWAYS(n < numContactForces,
+ "CompliantContactSubsystem::getContactForce()",
+ "There are currently only %d contact forces but force %d"
+ " was requested (they are numbered from 0). Use getNumContactForces()"
+ " first to determine how many are available.", numContactForces, n);
+ const Array_<ContactForce>& forces = getForceCache(s);
+ return forces[n];
+}
+
+
+const ContactForce& getContactForceById(const State& s, ContactId id) const {
+ static const ContactForce invalidForce;
+ const int numContactForces = getNumContactForces(s); // realize if needed
+ const Array_<ContactForce>& forces = getForceCache(s);
+ // TODO: use a map
+ for (int i=0; i < numContactForces; ++i)
+ if (forces[i].getContactId()==id)
+ return forces[i];
+
+ return invalidForce;
+}
+
+bool calcContactPatchDetailsById(const State& state,
+ ContactId id,
+ ContactPatch& patch_G) const
+{
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(state), Stage::Velocity,
+ "CompliantContactSubystemImpl::ensureForceCacheValid()");
+
+ const ContactSnapshot& active = m_tracker.getActiveContacts(state);
+ const Contact& contact = active.getContactById(id);
+
+ if (contact.isEmpty() || contact.getCondition() == Contact::Broken) {
+ patch_G.clear();
+ return false;
+ }
+
+ const Transform& X_S1S2 = contact.getTransform();
+ const ContactSurfaceIndex surf1(contact.getSurface1());
+ const ContactSurfaceIndex surf2(contact.getSurface2());
+ const MobilizedBody& mobod1 = m_tracker.getMobilizedBody(surf1);
+ const MobilizedBody& mobod2 = m_tracker.getMobilizedBody(surf2);
+
+ const Transform X_GS1 = mobod1.findFrameTransformInGround
+ (state, m_tracker.getContactSurfaceTransform(surf1));
+ const Transform X_GS2 = mobod2.findFrameTransformInGround
+ (state, m_tracker.getContactSurfaceTransform(surf2));
+
+ const SpatialVec V_GS1 = mobod1.findFrameVelocityInGround
+ (state, m_tracker.getContactSurfaceTransform(surf1));
+ const SpatialVec V_GS2 = mobod2.findFrameVelocityInGround
+ (state, m_tracker.getContactSurfaceTransform(surf2));
+
+ // Calculate the relative velocity of S2 in S1, expressed in S1.
+ const SpatialVec V_S1S2 =
+ findRelativeVelocity(X_GS1, V_GS1, X_GS2, V_GS2);
+
+ // Get the right force generator to use for this kind of contact.
+ const ContactForceGenerator& generator =
+ getForceGenerator(contact.getTypeId());
+
+ // Calculate the contact patch measured and expressed in S1.
+ generator.calcContactPatch(state, contact, V_S1S2,
+ patch_G); // it is actually in S1 at this point
+
+ // Re-express the contact patch in Ground for later use.
+ if (patch_G.isValid()) {
+ patch_G.changeFrameInPlace(X_GS1); // switch from S1 to Ground
+ return true;
+ }
+
+ return false;
+}
+
+const ContactTrackerSubsystem& getContactTrackerSubsystem() const
+{ return m_tracker; }
+
+~CompliantContactSubsystemImpl() {
+ delete m_defaultGenerator;
+ for (GeneratorMap::iterator p = m_generators.begin();
+ p != m_generators.end(); ++p)
+ delete p->second; // free the generator
+ // The map itself gets freed by its destructor here.
+}
+
+// We're going to take over ownership of this object. If the generator map
+// already contains a generator for this type of contact, the new one replaces
+// it.
+void adoptForceGenerator(CompliantContactSubsystem* subsys,
+ ContactForceGenerator* generator) {
+ assert(generator);
+ generator->setCompliantContactSubsystem(subsys);
+ invalidateSubsystemTopologyCache();
+ GeneratorMap::iterator p=m_generators.find(generator->getContactTypeId());
+ if (p != m_generators.end()) {
+ // We're replacing an existing generator.
+ delete p->second;
+ p->second = generator; // just copying the pointer
+ } else {
+ // Insert the new generator.
+ m_generators.insert(std::make_pair(generator->getContactTypeId(),
+ generator));
+ }
+}
+
+// Set the default force generator, deleting the previous one. Null is OK.
+void adoptDefaultForceGenerator(CompliantContactSubsystem* subsys,
+ ContactForceGenerator* generator) {
+ invalidateSubsystemTopologyCache();
+ delete m_defaultGenerator;
+ m_defaultGenerator = generator; // just copying the pointer
+ if (m_defaultGenerator)
+ m_defaultGenerator->setCompliantContactSubsystem(subsys);
+}
+
+// Do we have a generator registered for this type? If not, we'll invoke our
+// default generator.
+bool hasForceGenerator(ContactTypeId type) const
+{ return m_generators.find(type) != m_generators.end(); }
+
+bool hasDefaultForceGenerator() const
+{ return m_defaultGenerator != 0; }
+
+// Get the generator registered for this type of contact or the default
+// generator if nothing is registered.
+const ContactForceGenerator& getForceGenerator(ContactTypeId type) const
+{ GeneratorMap::const_iterator p=m_generators.find(type);
+ return p != m_generators.end() ? *p->second : getDefaultForceGenerator(); }
+
+// Get the default generator.
+const ContactForceGenerator& getDefaultForceGenerator() const
+{ assert(m_defaultGenerator); return *m_defaultGenerator; }
+
+
+// These override default implementations of virtual methods in the
+// Subsystem::Guts class.
+
+CompliantContactSubsystemImpl* cloneImpl() const
+{ return new CompliantContactSubsystemImpl(*this); }
+
+// Allocate lazy evaluation cache entries to hold potential energy (calculable
+// after position stage) and forces and power (calculable after velocity stage).
+int realizeSubsystemTopologyImpl(State& s) const {
+ // Get writability briefly to fill in the Topology cache.
+ CompliantContactSubsystemImpl* wThis =
+ const_cast<CompliantContactSubsystemImpl*>(this);
+
+ // Calculating forces includes calculating PE for each force.
+ wThis->m_forceCacheIx = allocateLazyCacheEntry(s,
+ Stage::Velocity, new Value<Array_<ContactForce> >());
+ // This usually just requires summing up the forceCache PE values, but
+ // may also have to be calculated at Position stage in which case we can't
+ // calculate the forces.
+ wThis->m_potEnergyCacheIx = allocateLazyCacheEntry(s,
+ Stage::Position, new Value<Real>(NaN));
+
+ // This state variable is used to integrate power to get dissipated
+ // energy. Allocate only if requested.
+ if (m_trackDissipatedEnergy) {
+ Vector einit(1, Real(0));
+ wThis->m_dissipatedEnergyIx = allocateZ(s,einit);
+ }
+
+ return 0;
+}
+
+int realizeSubsystemModelImpl(State& s) const {
+ return 0;
+}
+
+int realizeSubsystemInstanceImpl(const State& s) const {
+ return 0;
+}
+
+int realizeSubsystemTimeImpl(const State& s) const {
+ return 0;
+}
+
+int realizeSubsystemPositionImpl(const State& s) const {
+ return 0;
+}
+
+int realizeSubsystemVelocityImpl(const State& s) const {
+ return 0;
+}
+
+int realizeSubsystemDynamicsImpl(const State& s) const {
+ ensureForceCacheValid(s);
+
+ const MultibodySystem& mbs = getMultibodySystem(); // my owner
+ const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
+
+ // Get access to System-global force cache array.
+ Vector_<SpatialVec>& rigidBodyForces =
+ mbs.updRigidBodyForces(s, Stage::Dynamics);
+
+ // Accumulate the values from the cache into the global arrays.
+ const ContactSnapshot& contacts = m_tracker.getActiveContacts(s);
+ const Array_<ContactForce>& forces = getForceCache(s);
+ for (unsigned i=0; i < forces.size(); ++i) {
+ const ContactForce& force = forces[i];
+ const Contact& contact = contacts.getContactById(force.getContactId());
+ const MobilizedBody& mobod1 = m_tracker.getMobilizedBody
+ (contact.getSurface1());
+ const MobilizedBody& mobod2 = m_tracker.getMobilizedBody
+ (contact.getSurface2());
+ const Vec3 r1 = force.getContactPoint() - mobod1.getBodyOriginLocation(s);
+ const Vec3 r2 = force.getContactPoint() - mobod2.getBodyOriginLocation(s);
+ const SpatialVec& F2cpt = force.getForceOnSurface2(); // at contact pt
+ // Shift applied force to body origins.
+ const SpatialVec F2( F2cpt[0] + r2 % F2cpt[1], F2cpt[1]);
+ const SpatialVec F1(-F2cpt[0] + r1 % -F2cpt[1], -F2cpt[1]);
+ mobod1.applyBodyForce(s, F1, rigidBodyForces);
+ mobod2.applyBodyForce(s, F2, rigidBodyForces);
+ }
+
+ return 0;
+}
+
+// Potential energy is normally a side effect of force calculation done after
+// Velocity stage. But if only positions are available, we
+// have to calculate forces at zero velocity and then throw away everything
+// but the PE.
+Real calcPotentialEnergy(const State& state) const {
+ ensurePotentialEnergyCacheValid(state);
+ return getPotentialEnergyCache(state);
+}
+
+int realizeSubsystemAccelerationImpl(const State& state) const {
+ if (!m_trackDissipatedEnergy)
+ return 0; // nothing to do here in that case
+
+ ensureForceCacheValid(state);
+ Real powerLoss = 0;
+ const Array_<ContactForce>& forces = getForceCache(state);
+ for (unsigned i=0; i < forces.size(); ++i)
+ powerLoss += forces[i].getPowerDissipation();
+ updDissipatedEnergyDeriv(state) = powerLoss;
+ return 0;
+}
+
+int realizeSubsystemReportImpl(const State&) const {
+ return 0;
+}
+
+const Real& getDissipatedEnergyVar(const State& s) const
+{ return getZ(s)[m_dissipatedEnergyIx]; }
+Real& updDissipatedEnergyVar(State& s) const
+{ return updZ(s)[m_dissipatedEnergyIx]; }
+
+//--------------------------------------------------------------------------
+ private:
+
+Real& updDissipatedEnergyDeriv(const State& s) const
+{ return updZDot(s)[m_dissipatedEnergyIx]; }
+
+const Real& getPotentialEnergyCache(const State& s) const
+{ return Value<Real>::downcast(getCacheEntry(s,m_potEnergyCacheIx)); }
+const Array_<ContactForce>& getForceCache(const State& s) const
+{ return Value<Array_<ContactForce> >::downcast
+ (getCacheEntry(s,m_forceCacheIx)); }
+
+Real& updPotentialEnergyCache(const State& s) const
+{ return Value<Real>::updDowncast(updCacheEntry(s,m_potEnergyCacheIx)); }
+Array_<ContactForce>& updForceCache(const State& s) const
+{ return Value<Array_<ContactForce> >::updDowncast
+ (updCacheEntry(s,m_forceCacheIx)); }
+
+bool isPotentialEnergyCacheValid(const State& s) const
+{ return isCacheValueRealized(s,m_potEnergyCacheIx); }
+bool isForceCacheValid(const State& s) const
+{ return isCacheValueRealized(s,m_forceCacheIx); }
+
+
+void markPotentialEnergyCacheValid(const State& s) const
+{ markCacheValueRealized(s,m_potEnergyCacheIx); }
+void markForceCacheValid(const State& s) const
+{ markCacheValueRealized(s,m_forceCacheIx); }
+
+void ensurePotentialEnergyCacheValid(const State&) const;
+void ensureForceCacheValid(const State&) const;
+
+
+
+ // TOPOLOGY "STATE"
+
+// This is the ContactTracker that we will consult to obtain Contact objects
+// to use as raw material for contact force generation.
+const ContactTrackerSubsystem& m_tracker;
+
+// This is the value that should be used by generators for the friction
+// transition velocity if they are using a Hollars-like friction model.
+// Also precalculate the inverse to avoid expensive runtime division.
+Real m_transitionVelocity;
+Real m_ooTransitionVelocity; // 1/vt
+
+// This flag determines whether we allocate a Z variable to integrate
+// dissipated power to track dissipated energy.
+bool m_trackDissipatedEnergy;
+
+// This map owns the generator objects; be sure to clean up on destruction or
+// when a generator is replaced.
+GeneratorMap m_generators;
+
+// This is the generator to use for an unrecognized ContactTypeId. Typically
+// this will either do nothing silently or throw an error.
+ContactForceGenerator* m_defaultGenerator;
+
+ // TOPOLOGY "CACHE"
+
+// These must be set during realizeTopology and treated as const thereafter.
+// Note that we don't allocate the dissipated energy variable unless the
+// flag above is set true (prior to realizeTopology()).
+ZIndex m_dissipatedEnergyIx;
+CacheEntryIndex m_potEnergyCacheIx;
+CacheEntryIndex m_forceCacheIx;
+};
+
+void CompliantContactSubsystemImpl::
+ensurePotentialEnergyCacheValid(const State& state) const {
+ if (isPotentialEnergyCacheValid(state)) return;
+
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(state), Stage::Position,
+ "CompliantContactSubystemImpl::ensurePotentialEnergyCacheValid()");
+
+ Real& pe = updPotentialEnergyCache(state);
+ pe = 0;
+
+ // If the State has been realized to Velocity stage, we can just sum up
+ // the PEs from the ContactForce objects.
+ if (getStage(state) >= Stage::Velocity) {
+ ensureForceCacheValid(state);
+ const Array_<ContactForce>& forces = getForceCache(state);
+ for (unsigned i=0; i < forces.size(); ++i)
+ pe += forces[i].getPotentialEnergy();
+ markPotentialEnergyCacheValid(state);
+ return;
+ }
+
+ // The State has been realized to Position stage, so we're going to have
+ // to calculate forces at zero velocity and then throw away all the
+ // results except for the PE.
+ const ContactSnapshot& active = m_tracker.getActiveContacts(state);
+ const int nContacts = active.getNumContacts();
+ for (int i=0; i<nContacts; ++i) {
+ const Contact& contact = active.getContact(i);
+ const ContactForceGenerator& generator =
+ getForceGenerator(contact.getTypeId());
+ ContactForce force;
+ generator.calcContactForce(state,contact,SpatialVec(Vec3(0)), force);
+ pe += force.getPotentialEnergy();
+ }
+
+ markPotentialEnergyCacheValid(state);
+}
+
+
+void CompliantContactSubsystemImpl::
+ensureForceCacheValid(const State& state) const {
+ if (isForceCacheValid(state)) return;
+
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(state), Stage::Velocity,
+ "CompliantContactSubystemImpl::ensureForceCacheValid()");
+
+ Array_<ContactForce>& forces = updForceCache(state);
+ forces.clear();
+
+
+ const ContactSnapshot& active = m_tracker.getActiveContacts(state);
+ const int nContacts = active.getNumContacts();
+ for (int i=0; i<nContacts; ++i) {
+ const Contact& contact = active.getContact(i);
+ if (contact.getCondition() == Contact::Broken) {
+ // No need to generate forces; this will be gone next time.
+ continue;
+ }
+ const Transform& X_S1S2 = contact.getTransform();
+ const ContactSurfaceIndex surf1(contact.getSurface1());
+ const ContactSurfaceIndex surf2(contact.getSurface2());
+ const MobilizedBody& mobod1 = m_tracker.getMobilizedBody(surf1);
+ const MobilizedBody& mobod2 = m_tracker.getMobilizedBody(surf2);
+
+ // TODO: These two are expensive (63 flops each) and shouldn't have
+ // to be recalculated here since we must have used them in creating
+ // the Contact and X_S1S2.
+ const Transform X_GS1 = mobod1.findFrameTransformInGround
+ (state, m_tracker.getContactSurfaceTransform(surf1));
+ const Transform X_GS2 = mobod2.findFrameTransformInGround
+ (state, m_tracker.getContactSurfaceTransform(surf2));
+
+ const SpatialVec V_GS1 = mobod1.findFrameVelocityInGround
+ (state, m_tracker.getContactSurfaceTransform(surf1));
+ const SpatialVec V_GS2 = mobod2.findFrameVelocityInGround
+ (state, m_tracker.getContactSurfaceTransform(surf2));
+
+ // Calculate the relative velocity of S2 in S1, expressed in S1.
+ const SpatialVec V_S1S2 =
+ findRelativeVelocity(X_GS1, V_GS1, X_GS2, V_GS2); // 51 flops
+
+ const ContactForceGenerator& generator =
+ getForceGenerator(contact.getTypeId());
+ forces.push_back(); // allocate a new garbage ContactForce
+ // Calculate the contact force measured and expressed in S1.
+ generator.calcContactForce(state, contact, V_S1S2, forces.back());
+ // Re-express the contact force in Ground for later use.
+ if (forces.back().isValid())
+ forces.back().changeFrameInPlace(X_GS1); // switch to Ground
+ else
+ forces.pop_back(); // never mind ...
+ }
+
+ markForceCacheValid(state);
+}
+
+
+//==============================================================================
+// COMPLIANT CONTACT SUBSYSTEM
+//==============================================================================
+
+/*static*/ bool
+CompliantContactSubsystem::isInstanceOf(const ForceSubsystem& s)
+{ return CompliantContactSubsystemImpl::isA(s.getRep()); }
+/*static*/ const CompliantContactSubsystem&
+CompliantContactSubsystem::downcast(const ForceSubsystem& s)
+{ assert(isInstanceOf(s));
+ return static_cast<const CompliantContactSubsystem&>(s); }
+/*static*/ CompliantContactSubsystem&
+CompliantContactSubsystem::updDowncast(ForceSubsystem& s)
+{ assert(isInstanceOf(s));
+ return static_cast<CompliantContactSubsystem&>(s); }
+
+const CompliantContactSubsystemImpl&
+CompliantContactSubsystem::getImpl() const
+{ return SimTK_DYNAMIC_CAST_DEBUG<const CompliantContactSubsystemImpl&>
+ (ForceSubsystem::getRep()); }
+CompliantContactSubsystemImpl&
+CompliantContactSubsystem::updImpl()
+{ return SimTK_DYNAMIC_CAST_DEBUG<CompliantContactSubsystemImpl&>
+ (ForceSubsystem::updRep()); }
+
+CompliantContactSubsystem::CompliantContactSubsystem
+ (MultibodySystem& mbs, const ContactTrackerSubsystem& tracker)
+: ForceSubsystem()
+{ adoptSubsystemGuts(new CompliantContactSubsystemImpl(tracker));
+ mbs.addForceSubsystem(*this); // mbs steals ownership
+
+ // Register default contact force generators.
+ adoptForceGenerator(new ContactForceGenerator::HertzCircular());
+ adoptForceGenerator(new ContactForceGenerator::HertzElliptical());
+ adoptForceGenerator(new ContactForceGenerator::ElasticFoundation());
+ adoptDefaultForceGenerator(new ContactForceGenerator::DoNothing());
+}
+
+Real CompliantContactSubsystem::getTransitionVelocity() const
+{ return getImpl().getTransitionVelocity(); }
+Real CompliantContactSubsystem::getOOTransitionVelocity() const
+{ return getImpl().getOOTransitionVelocity(); }
+void CompliantContactSubsystem::setTransitionVelocity(Real vt) {
+ SimTK_ERRCHK1_ALWAYS(vt > 0,
+ "CompliantContactSubsystem::setTransitionVelocity()",
+ "Friction transition velocity %g is illegal.", vt);
+ updImpl().setTransitionVelocity(vt);
+}
+
+void CompliantContactSubsystem::setTrackDissipatedEnergy(bool shouldTrack)
+{ updImpl().setTrackDissipatedEnergy(shouldTrack); }
+bool CompliantContactSubsystem::getTrackDissipatedEnergy() const
+{ return getImpl().getTrackDissipatedEnergy(); }
+
+int CompliantContactSubsystem::getNumContactForces(const State& s) const
+{ return getImpl().getNumContactForces(s); }
+
+const ContactForce& CompliantContactSubsystem::
+getContactForce(const State& s, int n) const
+{ return getImpl().getContactForce(s,n); }
+
+
+const ContactForce& CompliantContactSubsystem::
+getContactForceById(const State& s, ContactId id) const
+{ return getImpl().getContactForceById(s,id); }
+
+bool CompliantContactSubsystem::
+calcContactPatchDetailsById(const State& state,
+ ContactId id,
+ ContactPatch& patch_G) const
+{ return getImpl().calcContactPatchDetailsById(state,id,patch_G); }
+
+Real CompliantContactSubsystem::
+getDissipatedEnergy(const State& s) const {
+ SimTK_ERRCHK_ALWAYS(getTrackDissipatedEnergy(),
+ "CompliantContactSubsystem::getDissipatedEnergy()",
+ "You can't call getDissipatedEnergy() unless you have previously "
+ "enabled tracking of dissipated energy with a call to "
+ "setTrackDissipatedEnergy().");
+
+ return getImpl().getDissipatedEnergyVar(s); }
+
+void CompliantContactSubsystem::
+setDissipatedEnergy(State& s, Real energy) const {
+ SimTK_ERRCHK_ALWAYS(getTrackDissipatedEnergy(),
+ "CompliantContactSubsystem::setDissipatedEnergy()",
+ "You can't call setDissipatedEnergy() unless you have previously "
+ "enabled tracking of dissipated energy with a call to "
+ "setTrackDissipatedEnergy().");
+ SimTK_ERRCHK1_ALWAYS(energy >= 0,
+ "CompliantContactSubsystem::setDissipatedEnergy()",
+ "The initial value for the dissipated energy must be nonnegative"
+ " but an attempt was made to set it to %g.", energy);
+
+ getImpl().updDissipatedEnergyVar(s) = energy;
+}
+
+
+void CompliantContactSubsystem::adoptForceGenerator
+ (ContactForceGenerator* generator)
+{ return updImpl().adoptForceGenerator(this, generator); }
+
+void CompliantContactSubsystem::adoptDefaultForceGenerator
+ (ContactForceGenerator* generator)
+{ return updImpl().adoptDefaultForceGenerator(this, generator); }
+
+bool CompliantContactSubsystem::hasForceGenerator(ContactTypeId type) const
+{ return getImpl().hasForceGenerator(type); }
+bool CompliantContactSubsystem::hasDefaultForceGenerator() const
+{ return getImpl().hasDefaultForceGenerator(); }
+
+const ContactForceGenerator& CompliantContactSubsystem::
+getContactForceGenerator(ContactTypeId contactType) const
+{ return getImpl().getForceGenerator(contactType); }
+
+const ContactForceGenerator& CompliantContactSubsystem::
+getDefaultForceGenerator() const
+{ return getImpl().getDefaultForceGenerator(); }
+
+const ContactTrackerSubsystem& CompliantContactSubsystem::
+getContactTrackerSubsystem() const
+{ return getImpl().getContactTrackerSubsystem(); }
+
+const MultibodySystem& CompliantContactSubsystem::getMultibodySystem() const
+{ return MultibodySystem::downcast(getSystem()); }
+
+
+// Input x goes from 0 to 1; output goes 0 to 1 but smoothed with an S-shaped
+// quintic with two zero derivatives at either end. Cost is 7 flops.
+inline static Real step5(Real x) {
+ assert(0 <= x && x <= 1);
+ const Real x3=x*x*x;
+ return x3*(10+x*(6*x-15)); //10x^3-15x^4+6x^5
+}
+
+// Input x goes from 0 to 1; output goes from 0 to y. First derivative
+// is 0 at the beginning and yd at the end. Second derivatives are zero
+// at both ends:
+//
+// y - *
+// * | slope=yd
+// *------
+// *
+// 0 - * *
+// x=0 1
+//
+// Cost is 16 flops.
+inline static Real step5d(Real y, Real yd, Real x) {
+ assert(0 <= x && x <= 1);
+ const Real a=6*y-3*yd, b=-15*y+7*yd, c=10*y-4*yd, x3=x*x*x;
+ return x3*(c + x*(b + x*a));
+}
+
+
+// This is the sum of two curves:
+// (1) a wet friction term mu_wet which is a linear function of velocity:
+// mu_wet = uv*v
+// (2) a dry friction term mu_dry which is a quintic spline with 4 segments:
+// mu_dry =
+// (a) v=0..1: smooth interpolation from 0 to us
+// (b) v=1..3: smooth interp from us down to ud (Stribeck)
+// (c) v=3..Inf ud
+// CAUTION: uv and v must be dimensionless in multiples of transition velocity.
+// The function mu = mu_wet + mu_dry is zero at v=0 with 1st deriv (slope) uv
+// and 2nd deriv (curvature) 0. At large velocities v>>0 the value is
+// ud+uv*v, again with slope uv and zero curvature. We want mu(v) to be c2
+// continuous, so mu_wet(v) must have zero slope and curvature at v==0 and
+// at v==3 where it takes on a constant value ud.
+//
+// Cost: stiction 12 flops
+// stribeck 14 flops
+// sliding 3 flops
+// Curve looks like this:
+//
+// us+uv ***
+// * * *
+// * * *____| slope = uv at Inf
+// * * *
+// ud+3uv * * *
+// *
+// *
+// *
+// 0 *____| slope = uv at 0
+//
+// | | |
+// v=0 1 3
+//
+// This calculates a composite coefficient of friction that you should use
+// to scale the normal force to produce the friction force.
+inline static Real stribeck(Real us, Real ud, Real uv, Real v) {
+ const Real mu_wet = uv*v;
+ Real mu_dry;
+ if (v >= 3) mu_dry = ud; // sliding
+ else if (v >= 1) mu_dry = us - (us-ud)*step5((v-1)/2); // Stribeck
+ else mu_dry = us*step5(v); // 0 <= v < 1 (stiction)
+ return mu_dry + mu_wet;
+}
+
+// CAUTION: uv and v must be dimensionless in multiples of transition velocity.
+// Const 9 flops + 1 divide = approx 25 flops.
+// This calculates a composite coefficient of friction that you should use
+// to scale the normal force to produce the friction force.
+// Curve is similar to Stribeck above but more violent with a discontinuous
+// derivative at v==1.
+inline static Real hollars(Real us, Real ud, Real uv, Real v) {
+ const Real mu = std::min(v, Real(1))*(ud + 2*(us-ud)/(1+v*v))
+ + uv*v;
+ return mu;
+}
+
+// This function is shared between the Hertz circular and Hertz elliptical
+// contacts. We only need to pass in the effective relative radius R and the
+// elliptical correction factor e(=1 for circular); everything else is the
+// same.
+// This is around 250 flops.
+static void calcHertzContactForce
+ (const CompliantContactSubsystem& subsys,
+ const ContactTrackerSubsystem& tracker,
+ const State& state,
+ const Contact& contact, // contains X_S1S2
+ const UnitVec3& normal_S1, // away from surf1, exp. in S1
+ const Vec3& origin_S1, // half way btw undeformed surfaces
+ Real depth,
+ const SpatialVec& V_S1S2, // relative surface velocity, S2 in S1
+ Real R, // effective relative radius
+ Real e, // elliptical correction factor
+ ContactForce& contactForce_S1)
+{
+ if (depth <= 0) {
+ contactForce_S1.clear(); // no contact; invalidate return result
+ return;
+ }
+
+ const ContactSurfaceIndex surf1x = contact.getSurface1();
+ const ContactSurfaceIndex surf2x = contact.getSurface2();
+ const Transform& X_S1S2 = contact.getTransform();
+
+ // Abbreviations.
+ const Rotation& R12 = X_S1S2.R(); // orientation of S2 in S1
+ const Vec3& p12 = X_S1S2.p(); // position of O2 in S1
+ const Vec3& w12 = V_S1S2[0]; // ang. vel. of S2 in S1
+ const Vec3& v12 = V_S1S2[1]; // vel. of O2 in S1
+
+ const ContactSurface& surf1 = tracker.getContactSurface(surf1x);
+ const ContactSurface& surf2 = tracker.getContactSurface(surf2x);
+ // Note that Hertz doesn't care about the "thickness" property of a
+ // ContactSurface; it is able to estimate %strain from the relationship
+ // between the deformation and local radii of curvature.
+
+ const ContactMaterial& mat1 = surf1.getMaterial();
+ const ContactMaterial& mat2 = surf2.getMaterial();
+
+ // Use 2/3 power of stiffness for combining here. (We'll raise the combined
+ // result k to the 3/2 power below.)
+ const Real k1=mat1.getStiffness23(), k2=mat2.getStiffness23();
+ const Real c1=mat1.getDissipation(), c2=mat2.getDissipation();
+
+ // Adjust the contact location based on the relative stiffness of the
+ // two materials. s1 is the fraction of the "squishing" (deformation) that
+ // is done by surface1 -- if surface2 is very stiff then surface1 does most.
+
+ const Real s1 = k2/(k1+k2); // 0..1
+ const Real s2 = 1-s1; // 1..0
+ const Real x = depth;
+
+ // Actual contact point moves closer to stiffer surface.
+ const Vec3 contactPt_S1 = origin_S1 + (x*(Real(0.5)-s1))*normal_S1;
+
+ // Calculate the Hertz force fH, which is conservative.
+ const Real k = k1*s1; // (==k2*s2) == E^(2/3)
+ const Real c = c1*s1 + c2*s2;
+ // fH = 4/3 e R^1/2 k^3/2 x^3/2
+ const Real fH = e*Real(4./3.)*k*x*std::sqrt(R*k*x); // always >= 0
+
+ // Calculate the relative velocity of the two bodies at the contact point.
+ // We're considering S1 fixed, so we just need the velocity in S1 of
+ // the station of S2 that is coincident with the contact point.
+ const Vec3 contactPt2_S1 = contactPt_S1 - p12; // S2 station, exp. in S1
+
+ // All vectors are in S1; dropping the "_S1" notation now.
+
+ // Velocity of surf2 at contact point is opposite direction of normal
+ // when penetration is increasing.
+ const Vec3 vel = v12 + w12 % contactPt2_S1;
+ // Want xdot > 0 when x is increasing; normal points the other way
+ const Real xdot = -dot(vel, normal_S1); // penetration rate (signed)
+ const Vec3 velNormal = -xdot*normal_S1;
+ const Vec3 velTangent = vel-velNormal;
+
+ // Calculate the Hunt-Crossley force, which is dissipative, and the
+ // total normal force including elasticity and dissipation.
+ const Real fHC = fH*Real(1.5)*c*xdot; // same sign as xdot
+ const Real fNormal = fH + fHC; // < 0 means "sticking"; see below
+
+ // Start filling out the contact force.
+ contactForce_S1.setContactId(contact.getContactId());
+ contactForce_S1.setContactPoint(contactPt_S1);
+
+ // Total force can be negative under unusual circumstances ("yanking");
+ // that means no force is generated and no stored PE will be recovered.
+ // This will most often occur in to-be-rejected trial steps but can
+ // occasionally be real.
+ if (fNormal <= 0) {
+ //std::cout << "YANKING!!!\n";
+ contactForce_S1.setForceOnSurface2(SpatialVec(Vec3(0)));
+ contactForce_S1.setPotentialEnergy(0);
+ contactForce_S1.setPowerDissipation(0);
+ return; // there is contact, but no force
+ }
+
+ const Vec3 forceH = fH *normal_S1; // as applied to surf2
+ const Vec3 forceHC = fHC*normal_S1;
+ const Real potentialEnergy = Real(2./5.)*fH*x;
+ const Real powerHC = fHC*xdot; // rate of energy loss, >= 0
+
+ // Calculate the friction force.
+ Vec3 forceFriction(0);
+ Real powerFriction = 0;
+ const Real vslipSq = velTangent.normSqr();
+ if (vslipSq > square(SignificantReal)) {
+ const Real vslip = std::sqrt(vslipSq); // expensive
+ const Real us1=mat1.getStaticFriction(), us2=mat2.getStaticFriction();
+ const Real ud1=mat1.getDynamicFriction(), ud2=mat2.getDynamicFriction();
+ const Real uv1=mat1.getViscousFriction(), uv2=mat2.getViscousFriction();
+ const Real vtrans = subsys.getTransitionVelocity();
+ const Real ooVtrans = subsys.getOOTransitionVelocity(); // 1/vtrans
+
+ // Calculate effective coefficients of friction, being careful not
+ // to divide 0/0 if both are frictionless.
+ Real us = 2*us1*us2; if (us!=0) us /= (us1+us2);
+ Real ud = 2*ud1*ud2; if (ud!=0) ud /= (ud1+ud2);
+ Real uv = 2*uv1*uv2; if (uv!=0) uv /= (uv1+uv2);
+ assert(us >= ud);
+
+ // Express slip velocity as unitless multiple of transition velocity.
+ const Real v = vslip * ooVtrans;
+ // Must scale viscous coefficient to match unitless velocity.
+ const Real mu=stribeck(us,ud,uv*vtrans,v);
+ //const Real mu=hollars(us,ud,uv*vtrans,v);
+ const Real fFriction = fNormal * mu;
+ // Force direction on S2 opposes S2's velocity.
+ forceFriction = (-fFriction/vslip)*velTangent; // in S1
+ powerFriction = fFriction * vslip; // >= 0
+ }
+
+ const Vec3 forceLoss = forceHC + forceFriction;
+ const Vec3 forceTotal = forceH + forceLoss;
+
+ // Report the force (as applied at contactPt).
+ contactForce_S1.setForceOnSurface2(SpatialVec(Vec3(0),forceTotal));
+ contactForce_S1.setPotentialEnergy(potentialEnergy);
+ // Don't include dot(forceH,velNormal) power due to conservative force
+ // here. This way we don't double-count the energy on the way in as
+ // integrated power and potential energy. Although the books would balance
+ // again when the contact is broken, it makes continuous contact look as
+ // though some energy has been lost. In the "yanking" case above, without
+ // including the conservative power term we will actually lose energy
+ // because the deformed material isn't allowed to push back on us so the
+ // energy is lost to surface vibrations or some other unmodeled effect.
+ contactForce_S1.setPowerDissipation(powerHC + powerFriction);
+}
+
+
+//==============================================================================
+// HERTZ CIRCULAR GENERATOR
+//==============================================================================
+void ContactForceGenerator::HertzCircular::calcContactForce
+ (const State& state,
+ const Contact& overlap, // contains X_S1S2
+ const SpatialVec& V_S1S2, // relative surface velocity, S2 in S1
+ ContactForce& contactForce_S1) const
+{
+ SimTK_ASSERT(CircularPointContact::isInstance(overlap),
+ "ContactForceGenerator::HertzCircular::calcContactForce(): expected"
+ " CircularPointContact.");
+
+ const CircularPointContact& contact = CircularPointContact::getAs(overlap);
+ const Real depth = contact.getDepth();
+
+ const CompliantContactSubsystem& subsys = getCompliantContactSubsystem();
+ const ContactTrackerSubsystem& tracker = subsys.getContactTrackerSubsystem();
+
+ // normal points away from surf1, expressed in surf1's frame
+ const UnitVec3& normal_S1 = contact.getNormal();
+ // origin is half way between the two surfaces as though they had
+ // equal stiffness
+ const Vec3& origin_S1 = contact.getOrigin();
+
+ const Real R = contact.getEffectiveRadius();
+
+ calcHertzContactForce(subsys, tracker, state, overlap,
+ normal_S1, origin_S1, depth,
+ V_S1S2, R, 1, contactForce_S1);
+}
+
+void ContactForceGenerator::HertzCircular::calcContactPatch
+ (const State& state,
+ const Contact& overlap,
+ const SpatialVec& V_S1S2,
+ ContactPatch& patch_S1) const
+{
+ patch_S1.m_elements.clear(); // TODO no details yet
+ calcContactForce(state,overlap,V_S1S2,patch_S1.m_resultant);
+}
+
+
+
+//==============================================================================
+// HERTZ ELLIPTICAL GENERATOR
+//==============================================================================
+
+
+// Given the relative principal curvatures characterizing the undeformed
+// contact geometry, calculate the ratio k=a/b >= 1 of the semi axes of the
+// deformed contact ellipse (the actual size depends on the penetration but
+// the ratio depends only on the relative curvatures).
+//
+// Hertz theory relates them in this implicit equation for k:
+// B kmax k^2 E(m)-K(m)
+// - = ---- = -------------, m = 1 - (1/k)^2, B >= A.
+// A kmin K(m)-E(m)
+//
+// (B=kmax/2 and A=kmin/2 are semi-curvatures but their ratio
+// is the same as the ratio of curvatures.)
+//
+// This equation can be solved numerically for k (see the numerical
+// version of this routine below). Here we instead use an approximation
+// k = (B/A)^p where exponent p=p(B/A) as follows:
+// 1 + u0 q + u1 q^2 + u2 q^3 + u3 q^4
+// p = 2/3 -----------------------------------, q = log10^2(B/A)
+// 1 + v0 q + v1 q^2 + v2 q^3 + v3 q^4
+// This gives 5 digits or better of accuracy across the full range
+// of possible ratios kmax:kmin at a cost of about 160 flops.
+//
+// Antoine, Visa, Sauvey, Abba. "Approximate analytical model for
+// Hertzian Elliptical Contact Problems", ASME J. Tribology 128:660, 2006.
+
+static Real approxContactEllipseRatio(Real kmax, Real kmin) {
+ static const Real u[] =
+ { (Real).40227436, (Real)3.7491752e-2, (Real)7.4855761e-4,
+ (Real)2.1667028e-6 };
+ static const Real v[] =
+ { (Real).42678878, (Real)4.2605401e-2, (Real)9.0786922e-4,
+ (Real)2.7868927e-6 };
+
+ Real BoA = kmax/kmin; // ~20 flops
+ Real q = square(std::log10(BoA)), q2=q*q, q3=q2*q, q4=q3*q; // ~50 flops?
+ Real n = 1 + u[0]*q + u[1]*q2 + u[2]*q3 + u[3]*q4; // 8 flops
+ Real d = 1 + v[0]*q + v[1]*q2 + v[2]*q3 + v[3]*q4; // 8 flops
+ Real p = (2*n)/(3*d); // ~20 flops
+ Real k = std::pow(BoA, p); // ellipse ratio k=a/b, >50 flops?
+ return k;
+}
+
+// Given the relative principal curvatures characterizing the undeformed
+// contact geometry, calculate the ratio k=a/b >= 1 of the semiaxes of the
+// deformed contact ellipse (the actual size depends on the penetration but
+// the ratio depends only on the relative curvatures).
+//
+// Hertz theory relates them in this implicit equation for k:
+// B kmax k^2 E(m)-K(m)
+// - = ---- = -------------, m = 1 - (1/k)^2, B >= A.
+// A kmin K(m)-E(m)
+//
+// (B=kmax/2 and A=kmin/2 are semi-curvatures but their ratio
+// is the same as the ratio of curvatures.)
+//
+// Here we numerically solve for k using a Newton iteration, and
+// obtain K(m) and E(m) as a side effect useful later.
+// The cost is roughly ~100 + (300+70)*n, typically about 1600 flops
+// at n=4.
+//
+// This method is adapted from
+// Dyson, Evans, Snidle. "A simple accurate method for calculation of
+// stresses and deformations in elliptical Hertzian contacts",
+// J. Mech. Eng. Sci. Proc. IMechE part C 206:139-141, 1992.
+static Real numContactEllipseRatio
+ (Real kmax, Real kmin, std::pair<Real,Real>& KE)
+{
+ // See Dyson.
+ Real lambda = kmin/kmax; // A/B
+ Real kp = std::pow(lambda, Real(2./3.)); // note inverted k'=b/a
+ Real k2 = kp*kp;
+ Real lambda1;
+ for(;;) {
+ if (k2 > 1) k2 = 1;
+ Real m = 1-k2;
+ KE = completeEllipticIntegralsKE(m); // K,E
+ if (k2==1) return 1;
+ lambda1 = (KE.first - KE.second) / (KE.second/k2 - KE.first);
+ if (std::abs(lambda-lambda1) < 10*lambda*SignificantReal)
+ break;
+ Real den = lambda1*lambda1+2*m*lambda1-k2;
+ k2 += (2*m*k2*(lambda-lambda1))/den; // might make k2 slightly >1
+ };
+ kp = std::sqrt(k2);
+ return 1/kp; // return k
+}
+
+// Given the undeformed relative geometry semi-curvatures A and B (A<=B),
+// the deformed contact ellipse ratio k=a/b >= 1 and associated complete
+// elliptic integrals K(m) and E(m) where m = 1 - (1/k)^2, and the
+// penetration depth d, return the ellipse semimajor and semiminor
+// axes a, b resp. (a>=b), the contact force P, and the peak contact
+// pressure p0.
+// d Em
+// b = sqrt( -------- ), a = b*k
+// (A+B) Km
+//
+// 4 (Pi E*)^2 Em k^2 2 b^3 E* k Pi (A+B)
+// P = sqrt( d^3 ------------------ ) = -------------------
+// 9 (A+B) Km^3 3 Em
+//
+// p0 = 3/2 avg. pressure = 3/2 P/(Pi*a*b)
+//
+// Cost is about 100 flops.
+static void calcContactInfo
+ (Real A, Real B, Real k, const std::pair<Real,Real>& KE, Real d,
+ Real Estar, Vec2& ab, Real& P, Real& p0)
+{
+ Real Km = KE.first, Em = KE.second;
+ Real b = std::sqrt((d*Em)/((A+B)*Km)); // ~ 50 flops
+ Real a = b*k;
+ ab = Vec2(a,b);
+ // The rest is about 50 more flops
+ P = 2*cube(b)*Estar*k*Pi*(A+B)/(3*Em);
+ p0 = (3*P)/(2*Pi*a*b);
+}
+
+// Given just the undeformed relative geometry curvatures kmin (=2*A)
+// and kmax (=2*B) (kmax>=kmin>0), calculate the unitless eccentricity
+// correction factor e due to the fact that kmax != kmin. The correction
+// factor is 1 for a circular contact (kmax==kmin).
+//
+// The force due to a displacement d is given by
+//
+// P = e*[4/3 E* sqrt(R)] d^(3/2), R=1/((kmax+kmin)/2)=1/(A+B)
+//
+// Pi E(m)^(1/2) k
+// e = ---------------, k=a/b, m = 1 - (b/a)^2
+// 2 K(m)^(3/2)
+//
+// Derivation:
+// For a circular contact
+// P = 4/3 E* sqrt(R) d^(3/2)
+// For an elliptical contact
+// 2 Pi E* E(m)^(1/2) k
+// P = ----------------------- d^(3/2)
+// 3 (A+B)^(1/2) K(m)^(3/2)
+//
+// Pi E(m)^(1/2) k
+// = 2/3 E* sqrt(R) --------------- d^(3/2)
+// K(m)^(3/2)
+// For b/a=1 (circular) we have k=1, m=0, K(0)=E(0)=Pi/2. Substituting in
+// the equation for e gives e = Pi (Pi/2)^(1/2) / (2 (Pi/2)^(3/2)) = 1 as
+// expected.
+//
+// We calculate k and then K(m),E(m) using a fast approximate method that
+// gives us P as a smooth function that is accurate to about 7 digits.
+// More accurate methods are available but seem unnecessary and this is
+// expensive enough: about 320 flops unless kmax==kmin in which case it's free.
+static Real calcHertzForceEccentricityCorrection(Real kmax, Real kmin) {
+ SimTK_ASSERT(kmax >= kmin && kmin > 0,
+ "calcHertzForceEccentricityCorrection: kmax,kmin illegal.");
+ if (kmax==kmin) return 1;
+
+ Real k = approxContactEllipseRatio(kmax,kmin); // ~160 flops
+ Real m = 1 - square(1/k); // ~20 flops
+ std::pair<Real,Real> KE = approxCompleteEllipticIntegralsKE(m); // ~90 flops
+
+ Real Km = KE.first, Em = KE.second;
+ // Slightly twisted computation here to save one square root.
+ Real e = Pi * k * std::sqrt(Em*Km) / (2 * Km*Km); // ~50 flops
+
+ return e;
+}
+
+// This is about 340 flops plus the usual 250 for the Hertz contact force
+// calculation.
+void ContactForceGenerator::HertzElliptical::calcContactForce
+ (const State& state,
+ const Contact& overlap, // contains X_S1S2
+ const SpatialVec& V_S1S2, // relative surface velocity, S2 in S1
+ ContactForce& contactForce_S1) const
+{
+ SimTK_ASSERT(EllipticalPointContact::isInstance(overlap),
+ "ContactForceGenerator::HertzElliptical::calcContactForce(): expected"
+ " EllipticalPointContact.");
+
+ const EllipticalPointContact& contact =
+ EllipticalPointContact::getAs(overlap);
+ const Real depth = contact.getDepth();
+
+ const CompliantContactSubsystem& subsys = getCompliantContactSubsystem();
+ const ContactTrackerSubsystem& tracker = subsys.getContactTrackerSubsystem();
+
+ const Transform& X_S1C = contact.getContactFrame();
+ const Vec2& k = contact.getCurvatures(); // kmax,kmin
+ const Real e = calcHertzForceEccentricityCorrection(k[0],k[1]);
+
+ // normal points away from surf1, expressed in surf1's frame
+ const UnitVec3& normal_S1 = X_S1C.z();
+ // origin is half way between the two surfaces as though they had
+ // equal stiffness
+ const Vec3& origin_S1 = X_S1C.p();
+
+ const Real R = 2/(k[0]+k[1]); // 1/avg curvature (~20 flops)
+
+ calcHertzContactForce(subsys, tracker, state, overlap,
+ normal_S1, origin_S1, depth,
+ V_S1S2, R, e, contactForce_S1);
+}
+
+
+void ContactForceGenerator::HertzElliptical::calcContactPatch
+ (const State& state,
+ const Contact& overlap,
+ const SpatialVec& V_S1S2,
+ ContactPatch& patch_S1) const
+{
+ patch_S1.m_elements.clear(); // TODO no details yet
+ calcContactForce(state,overlap,V_S1S2,patch_S1.m_resultant);
+}
+
+
+
+//==============================================================================
+// ELASTIC FOUNDATION GENERATOR
+//==============================================================================
+void ContactForceGenerator::ElasticFoundation::calcContactForce
+ (const State& state,
+ const Contact& overlap, // contains X_S1S2
+ const SpatialVec& V_S1S2, // relative surface velocity
+ ContactForce& contactForce_S1) const
+{
+ calcContactForceAndDetails(state,overlap,V_S1S2,contactForce_S1,0);
+}
+
+
+
+void ContactForceGenerator::ElasticFoundation::calcContactPatch
+ (const State& state,
+ const Contact& overlap,
+ const SpatialVec& V_S1S2,
+ ContactPatch& patch_S1) const
+{
+ calcContactForceAndDetails(state,overlap,V_S1S2,
+ patch_S1.m_resultant, &patch_S1.m_elements);
+}
+
+// How to think about mesh-mesh contact:
+// There is only a single contact patch. We would like to mesh the patch
+// but we don't have that. Instead, we have two approximations of a meshed
+// patch, one on each surface after deformation, but at different resolutions.
+// Either one would be sufficient alone, since both cover the whole patch and
+// apply equal and opposite forces to the two bodies. So when we evaluate
+// forces on each mesh, we are making two approximations of the same force.
+// DON'T APPLY BOTH! That would be double-counting. Instead, we use the
+// two independent calculations to refine our result. So we scale the
+// areas of the two patch approximations so that each contributes 50% to
+// the overall patch area, forces, and center of pressure.
+
+void ContactForceGenerator::ElasticFoundation::calcContactForceAndDetails
+ (const State& state,
+ const Contact& overlap, // contains X_S1S2
+ const SpatialVec& V_S1S2, // relative surface velocity
+ ContactForce& contactForce_S1,
+ Array_<ContactDetail>* contactDetails_S1) const
+{
+ SimTK_ASSERT(TriangleMeshContact::isInstance(overlap),
+ "ContactForceGenerator::ElasticFoundation::calcContactForce(): expected"
+ " TriangleMeshContact.");
+
+ const bool wantDetails = (contactDetails_S1 != 0);
+ if (wantDetails)
+ contactDetails_S1->clear();
+
+ const TriangleMeshContact& contact = TriangleMeshContact::getAs(overlap);
+
+ const CompliantContactSubsystem& subsys = getCompliantContactSubsystem();
+ const ContactTrackerSubsystem& tracker = subsys.getContactTrackerSubsystem();
+
+ const ContactSurfaceIndex surf1x = contact.getSurface1();
+ const ContactSurfaceIndex surf2x = contact.getSurface2();
+ const Transform& X_S1S2 = contact.getTransform();
+
+ const ContactSurface& surf1 = tracker.getContactSurface(surf1x);
+ const ContactSurface& surf2 = tracker.getContactSurface(surf2x);
+
+ const ContactGeometry& shape1 = surf1.getShape();
+ const ContactGeometry& shape2 = surf2.getShape();
+
+ // One or both of these contact shapes must be a mesh.
+ const bool isMesh1 =
+ (shape1.getTypeId() == ContactGeometry::TriangleMesh::classTypeId());
+ const bool isMesh2 =
+ (shape2.getTypeId() == ContactGeometry::TriangleMesh::classTypeId());
+
+ // This is an assert because we should never have gotten here otherwise.
+ SimTK_ASSERT_ALWAYS(isMesh1 || isMesh2,
+ "ContactForceGenerator::ElasticFoundation::calcContactForceAndDetails():"
+ " At least one of the two surfaces should have been a mesh.");
+
+ // We require that any elastic foundation mesh have a non-zero thickness
+ // specified for the ContactSurface. This is an error rather than an
+ // assert because we might not find out until first contact that we need
+ // to know the thickness.
+ // For a non-mesh surface we'll *allow* a thickness but if there isn't
+ // one we'll assume it has the same thickness as the mesh.
+ // TODO: can't we do better than that?
+ Real h1=surf1.getThickness(), h2=surf2.getThickness();
+ SimTK_ERRCHK_ALWAYS(!isMesh1 || h1 > 0,
+ "ContactForceGenerator::ElasticFoundation::calcContactForceAndDetails()",
+ " Surface 1 was a mesh but no thickness was provided for the"
+ " ContactSurface.");
+ SimTK_ERRCHK_ALWAYS(!isMesh2 || h2 > 0,
+ "ContactForceGenerator::ElasticFoundation::calcContactForceAndDetails()",
+ " Surface 2 was a mesh but no thickness was provided for the"
+ " ContactSurface.");
+
+ assert(h1>0 || h2>0);
+
+ //TODO: for now assign the same thickness to the non-mesh as to the mesh.
+ //Should be able to do better than this.
+ if (h1==0) h1=h2;
+ if (h2==0) h2=h1;
+
+ // Compute combined material properties just once -- they are the same
+ // for all triangles.
+
+ const ContactMaterial& mat1 = surf1.getMaterial();
+ const ContactMaterial& mat2 = surf2.getMaterial();
+
+ // Stiffnesses combine linearly here, not as 2/3 power as for Hertz.
+ const Real k1=mat1.getStiffness(), k2=mat2.getStiffness();
+ const Real c1=mat1.getDissipation(), c2=mat2.getDissipation();
+
+ // Above stiffnesses are in pressure/unit strain; we need to work here
+ // with pressure/unit deformation using strain=x/h where x is deformation
+ // and h is thickness (that is, x==h => 100% strain).
+ const Real kh1 = k1/h1, kh2 = k2/h2;
+
+ // Adjust the contact location based on the relative stiffness of the
+ // two materials. s1 is the fraction of the "squishing" (deformation) that
+ // is done by surface1 -- if surface2 is very stiff then surface1 does most.
+ const Real s1 = kh2/(kh1+kh2); // 0..1
+ const Real s2 = 1-s1; // 1..0
+
+ // kh is the effective stiffness, c the effective dissipation
+ const Real kh = kh1*s1; // (==kh2*s2) == E/x
+ const Real c = c1*s1 + c2*s2;
+
+ // Calculate effective coefficients of friction, being careful not
+ // to divide 0/0 if both are frictionless.
+ const Real us1=mat1.getStaticFriction(), us2=mat2.getStaticFriction();
+ const Real ud1=mat1.getDynamicFriction(), ud2=mat2.getDynamicFriction();
+ const Real uv1=mat1.getViscousFriction(), uv2=mat2.getViscousFriction();
+ Real us = 2*us1*us2; if (us!=0) us /= (us1+us2);
+ Real ud = 2*ud1*ud2; if (ud!=0) ud /= (ud1+ud2);
+ Real uv = 2*uv1*uv2; if (uv!=0) uv /= (uv1+uv2);
+
+ // Now generate forces using the meshed surfaces only (one or two).
+
+
+ // We want both patches to accumulate forces at the same point in
+ // space. For numerical reasons this should be near the center of the
+ // patch.
+ Vec3 weightedPatchCentroid1_S1(0), weightedPatchCentroid2_S1(0);
+ Real patchArea1 = 0, patchArea2 = 0;
+ if (shape1.getTypeId() == ContactGeometry::TriangleMesh::classTypeId()) {
+ const ContactGeometry::TriangleMesh& mesh1 =
+ ContactGeometry::TriangleMesh::getAs(shape1);
+
+ calcWeightedPatchCentroid(mesh1, contact.getSurface1Faces(),
+ weightedPatchCentroid1_S1, patchArea1);
+ }
+ if (shape2.getTypeId() == ContactGeometry::TriangleMesh::classTypeId()) {
+ const ContactGeometry::TriangleMesh& mesh2 =
+ ContactGeometry::TriangleMesh::getAs(shape2);
+ Vec3 weightedPatchCentroid2_S2;
+
+ calcWeightedPatchCentroid(mesh2, contact.getSurface2Faces(),
+ weightedPatchCentroid2_S2, patchArea2);
+ // Remeasure patch2's weighted centroid from surface1's frame;
+ // be sure to weight the new offset also.
+ weightedPatchCentroid2_S1 = X_S1S2.R()*weightedPatchCentroid2_S2
+ + patchArea2*X_S1S2.p();
+ }
+
+ // At this point one or two area-weighted patch centroids and corresponding
+ // patch area estimates are known; if one is unused it's (0,0,0) with 0
+ // area weight. Combine them into a single composite centroid for the patch,
+ // with each contributing equally, and an averaged patch area estimate.
+ Real patchArea=0, areaScale1=0, areaScale2=0;
+ Vec3 patchCentroid_S1(0);
+ if (patchArea1 != 0) {
+ if (patchArea2 != 0) {
+ patchArea = (patchArea1 + patchArea2)/2;
+ areaScale1 = (patchArea/2) / patchArea1;
+ areaScale2 = (patchArea/2) / patchArea2;
+ patchCentroid_S1 =
+ areaScale1 * weightedPatchCentroid1_S1 / patchArea1
+ + areaScale2 * weightedPatchCentroid2_S1 / patchArea2;
+ } else { // only patchArea1
+ patchArea=patchArea1;
+ areaScale1 = 1;
+ areaScale2 = 0;
+ patchCentroid_S1 = weightedPatchCentroid1_S1 / patchArea1;
+ }
+ } else if (patchArea2 != 0) { // only patchArea2
+ patchArea = patchArea2;
+ areaScale2 = 1;
+ areaScale1 = 0;
+ patchCentroid_S1 = weightedPatchCentroid2_S1 / patchArea2;
+ }
+
+
+ // The patch centroid as calculated from one or two meshes is now
+ // in patchCentroid_S1, measured from and expressed in S1. Now we'll
+ // calculate all the patch forces and accumulate them at the patch
+ // centroid.
+
+
+ // Forces must be as applied to surface 2 at the patch centroid, but
+ // expressed in surface 1's frame.
+ SpatialVec force1_S1(Vec3(0)), force2_S1(Vec3(0));
+ Real potEnergy1=0, potEnergy2=0;
+ Real powerLoss1=0, powerLoss2=0;
+
+ // Center of pressure r_c is calculated like this:
+ // sum_i (r_i * |r_i X Fn_i|)
+ // r_c = --------------------------
+ // sum_i |r_i X Fn_i|
+ // where Fn is the normal force applied by element i at location r_i,
+ // with all locations measured from the patch centroid calculated above.
+ //
+ // We're going to calculate the weighted-points numerator for each
+ // mesh separately in weightedCOP1 and weightedCOP2, and the corresponding
+ // denominators (sum of all pressure-moment magnitudes) in weightCOP1 and
+ // weightCOP2. At the end we'll combine and divide to calculate the actual
+ // center of pressure where we'll ultimately apply the contact force.
+ Vec3 weightedCOP1_PC_S1(0), weightedCOP2_PC_S1(0); // from patch centroid
+ Real weightCOP1=0, weightCOP2=0;
+
+ if (shape1.getTypeId() == ContactGeometry::TriangleMesh::classTypeId()) {
+ const ContactGeometry::TriangleMesh& mesh =
+ ContactGeometry::TriangleMesh::getAs(shape1);
+
+ processOneMesh(state,
+ mesh, contact.getSurface1Faces(),
+ X_S1S2, V_S1S2, shape2,
+ s1, areaScale1,
+ kh, c, us, ud, uv,
+ patchCentroid_S1,
+ force1_S1, potEnergy1, powerLoss1,
+ weightedCOP1_PC_S1, weightCOP1,
+ contactDetails_S1); // details returned if this is non-null
+ }
+
+ if (shape2.getTypeId() == ContactGeometry::TriangleMesh::classTypeId()) {
+ const ContactGeometry::TriangleMesh& mesh =
+ ContactGeometry::TriangleMesh::getAs(shape2);
+
+ // Costs 120 flops to flip everything into S2 for processing and
+ // then put the results back in S1.
+
+ const Transform X_S2S1 = ~X_S1S2; // 3 flops
+ const SpatialVec V_S2S1 =
+ reverseRelativeVelocity(X_S1S2,V_S1S2); // 51 flops
+
+ const Vec3 patchCentroid_S2 = X_S2S1*patchCentroid_S1; // 18 flops
+
+ SpatialVec force2_S2;
+ Vec3 weightedCOP2_PC_S2;
+
+ const unsigned nDetailsBefore =
+ wantDetails ? contactDetails_S1->size() : 0;
+
+ processOneMesh(state,
+ mesh, contact.getSurface2Faces(),
+ X_S2S1, V_S2S1, shape1,
+ s2, areaScale2,
+ kh, c, us, ud, uv,
+ patchCentroid_S2,
+ force2_S2, potEnergy2, powerLoss2,
+ weightedCOP2_PC_S2, weightCOP2,
+ contactDetails_S1); // details returned *in S2* if this is non-null
+
+ // Switch contact details from S1 in S2 to S2 in S1.
+ if (wantDetails) {
+ for (unsigned i=nDetailsBefore; i < contactDetails_S1->size(); ++i)
+ (*contactDetails_S1)[i].changeFrameAndSwitchSurfacesInPlace(X_S1S2);
+ }
+
+ // Returned force is as applied to surface 1 (at the patch centroid);
+ // negate to make it the force applied to surface 2. Also re-express
+ // the moment and force vectors in S1.
+ force2_S1 = -(X_S1S2.R()*force2_S2); // 36 flops
+ // Returned weighted COP is measured from patch centroid but
+ // expressed in S2; reexpress in S1.
+ weightedCOP2_PC_S1 = X_S1S2.R()*weightedCOP2_PC_S2; // 15 flops
+ }
+
+ const Real weightCOP = weightCOP1+weightCOP2;
+ Vec3 centerOfPressure_PC_S1 = // offset from patch centroid
+ weightCOP > 0 ? (weightedCOP1_PC_S1+weightedCOP2_PC_S1) / weightCOP
+ : Vec3(0);
+
+ // Calculate total force applied to surface 2 at the patch centroid,
+ // expressed in S1.
+ const SpatialVec force_PC_S1 = force1_S1 + force2_S1;
+
+ // Shift to center of pressure.
+ const SpatialVec forceCOP_S1 =
+ shiftForceBy(force_PC_S1, centerOfPressure_PC_S1);
+
+ // Contact point is the center of pressure measured from the S1 origin.
+ const Vec3 contactPt_S1 = patchCentroid_S1 + centerOfPressure_PC_S1;
+
+ // Fill in contact force object.
+ contactForce_S1.setTo(contact.getContactId(),
+ contactPt_S1, // contact point in S1
+ forceCOP_S1, // force on surf2 at contact pt exp. in S1
+ potEnergy1 + potEnergy2,
+ powerLoss1 + powerLoss2);
+}
+
+
+
+// Compute the approximate geometric center of the patch represented by the
+// set of "inside faces" of this mesh, weighted by the total patch area.
+// We'll use the inside faces' centroids, weighted by face area, to calculate
+// a point that will be somewhere near the center of the patch, times the total
+// area. Dividing by the area yields the actual patch centroid point which can
+// be used as a numerically good nearby point for accumulating all the forces,
+// since the moments will all be small and thus
+// not suffer huge roundoff errors when combined. Afterwards, the numerically
+// good resultant can be shifted to whatever point is convenient, such as the
+// body frame. We return the total area represented by this patch so that
+// it can be combined with another mesh's patch if needed.
+// The weighted centroid is returned in the mesh surface's own frame.
+void ContactForceGenerator::ElasticFoundation::
+calcWeightedPatchCentroid
+ (const ContactGeometry::TriangleMesh& mesh,
+ const std::set<int>& insideFaces,
+ Vec3& weightedPatchCentroid,
+ Real& patchArea) const
+{
+ weightedPatchCentroid = Vec3(0); patchArea = 0;
+ for (std::set<int>::const_iterator iter = insideFaces.begin();
+ iter != insideFaces.end(); ++iter)
+ { const int face = *iter;
+ const Real area = mesh.getFaceArea(face);
+ weightedPatchCentroid += area*mesh.findCentroid(face);
+ patchArea += area;
+ }
+}
+
+
+
+// Private method that calculates the net contact force produced by a single
+// triangle mesh in contact with some other object (which might be another
+// mesh; we don't care). We are given the relative spatial pose and velocity of
+// the two surface frames, and for the mesh we are given a specific list of
+// the faces that are suspected of being at least partially inside the
+// other surface (in the undeformed geometry overlap). Normally this just
+// computes the resultant force but it can optionally append contact patch
+// details (one entry per element) as well, if the contactDetails argument is
+// non-null.
+// An area-scaling factor is used to scale the area represented by each face.
+// If there is only one mesh involved this should be 1. However, if this
+// is part of a pair of contact meshes each half of the pair should be scaled
+// so that both surfaces see the same overall patch area (so nominally the
+// area-scaling factor would be 0.5).
+void ContactForceGenerator::ElasticFoundation::
+processOneMesh
+ (const State& state,
+ const ContactGeometry::TriangleMesh& mesh,
+ const std::set<int>& insideFaces,
+ const Transform& X_MO,
+ const SpatialVec& V_MO,
+ const ContactGeometry& other,
+ Real meshDeformationFraction, // 0..1
+ Real areaScaleFactor,
+ Real kh, Real c, Real us, Real ud, Real uv, // composite material props
+ const Vec3& resultantPt_M, // where to apply forces
+ SpatialVec& resultantForceOnOther_M, // at resultant pt
+ Real& potentialEnergy,
+ Real& powerLoss,
+ Vec3& weightedCenterOfPressure_M,
+ Real& sumOfAllPressureMoments, // COP weight
+ Array_<ContactDetail>* contactDetails_M) const // in/out if present
+{
+ assert(!insideFaces.empty());
+ const bool wantDetails = (contactDetails_M != 0);
+
+ // Initialize all results to zero.
+ resultantForceOnOther_M = SpatialVec(Vec3(0),Vec3(0));
+ potentialEnergy = powerLoss = 0;
+ weightedCenterOfPressure_M = Vec3(0);
+ sumOfAllPressureMoments = 0;
+
+ // Don't initialize contact details; we're going to append them.
+
+ // Abbreviations.
+ const Rotation& RMO = X_MO.R(); // orientation of O in M
+ const Vec3& pMO = X_MO.p(); // position of OO (origin of O) in M
+ const Vec3& wMO = V_MO[0]; // ang. vel. of O in M
+ const Vec3& vMO = V_MO[1]; // vel. of OO in M
+
+ // Get friction model transition velocity vt and 1/vt.
+ const CompliantContactSubsystem& subsys = getCompliantContactSubsystem();
+ const Real vtrans = subsys.getTransitionVelocity();
+ const Real ooVtrans = subsys.getOOTransitionVelocity(); // 1/vtrans
+
+ // Now loop over all the faces again, evaluate the force from each
+ // spring, and apply it at the patch centroid.
+ // This costs roughly 300 flops per contacting face.
+ for (std::set<int>::const_iterator iter = insideFaces.begin();
+ iter != insideFaces.end(); ++iter)
+ { const int face = *iter;
+ const Vec3 springPos_M = mesh.findCentroid(face);
+ const Real faceArea = areaScaleFactor*mesh.getFaceArea(face);
+
+ bool inside;
+ UnitVec3 normal_O; // not used
+ const Vec3 nearestPoint_O = // 18 flops + cost of findNearestPoint
+ other.findNearestPoint(~X_MO*springPos_M, inside, normal_O);
+ if (!inside)
+ continue;
+
+ // Although the "spring" is associated with just one surface (the mesh M)
+ // it is considered here to include the compression of both surfaces
+ // together, using composite material properties for stiffness and
+ // dissipation properties of the spring. The total displacement vector
+ // for both surfaces points from the nearest point on the undeformed other
+ // surface to the undeformed spring position (face centroid) on the
+ // mesh. Since these overlap (we checked above) the nearest point is
+ // *inside* the mesh thus the vector points towards the mesh exterior;
+ // i.e., in the direction that the force will be applied to the
+ // "other" body. This is the same convention we use for the patch
+ // normal for Hertz contact.
+ const Vec3 nearestPoint_M = X_MO*nearestPoint_O; // 18 flops
+ const Vec3 overlap_M = springPos_M - nearestPoint_M; // 3 flops
+ const Real overlap = overlap_M.norm(); // ~40 flops
+
+ // If there is no overlap we can't generate forces.
+ if (overlap == 0)
+ continue;
+
+ // The surfaces are compressed by total amount "overlap".
+ const UnitVec3 normal_M(overlap_M/overlap, true); // ~15 flops
+
+ // Calculate the contact point location based on the relative
+ // squishiness of the two surfaces. The mesh deformation fraction (0-1)
+ // gives the fraction of the material squishing that is done by the
+ // mesh; the rest is done by the other surface. At 0 (rigid mesh) the
+ // contact point will be at the undeformed mesh face centroid; at 1
+ // (other body rigid) it will be at the (undeformed) nearest point on
+ // the other body.
+ const Real meshSquish = meshDeformationFraction*overlap; // mesh displacement
+ // Remember that the normal points towards the exterior of this mesh.
+ const Vec3 contactPt_M = springPos_M - meshSquish*normal_M; // 6 flops
+
+ // Calculate the relative velocity of the two bodies at the contact
+ // point. We're considering the mesh M fixed, so we just need the
+ // velocity in M of the station of O that is coincident with the
+ // contact point.
+
+ // O station, exp. in M
+ const Vec3 contactPtO_M = contactPt_M - pMO; // 3 flops
+
+ // All vectors are in M; dropping the "_M" notation now.
+
+ // Velocity of other at contact point is opposite direction of normal
+ // when penetration is increasing.
+ const Vec3 vel = vMO + wMO % contactPtO_M; // 12 flops
+
+ // Want odot > 0 when overlap is increasing; normal points the
+ // other way. odot is signed penetration (overlap) rate.
+ const Real odot = -dot(vel, normal_M); // 6 flops
+ const Vec3 velNormal = -odot*normal_M; // 4 flops
+ const Vec3 velTangent = vel-velNormal; // 3 flops
+
+ // Calculate scalar normal force (5 flops)
+ // Here kh has units of pressure/area/displacement
+ const Real fK = kh*faceArea*overlap; // normal elastic force (conservative)
+ const Real fC = fK*c*odot; // normal dissipation force (loss)
+ const Real fNormal = fK + fC; // normal force
+
+ // Total force can be negative under unusual circumstances ("yanking");
+ // that means no force is generated and no stored PE will be recovered.
+ // This will most often occur in to-be-rejected trial steps but can
+ // occasionally be real.
+ if (fNormal <= 0) {
+ SimTK_DEBUG1("YANKING!!! (face %d)\n", face);
+ //printf("YANKING!!! (face %d)\n", face);
+ continue;
+ }
+
+ // 12 flops in this series.
+ const Vec3 forceK = fK*normal_M; // as applied to other surf
+ const Vec3 forceC = fC*normal_M;
+ const Real PE = fK*overlap/2; // 1/2 kAx^2
+ const Real powerC = fC*odot; // rate of energy loss, >= 0
+ const Vec3 forceNormal = forceK + forceC;
+
+ // This is the moment r X f about the resultant point produced by
+ // applying this pure force at the contact point. Cost ~60 flops.
+ const Vec3 r = contactPt_M - resultantPt_M;
+ const Real pressureMoment = (r % forceNormal).norm();
+ weightedCenterOfPressure_M += pressureMoment*r;
+ sumOfAllPressureMoments += pressureMoment;
+
+ // Calculate the friction force. Cost is about 60 flops.
+ Vec3 forceFriction(0);
+ Real powerFriction = 0;
+ const Real vslipSq = velTangent.normSqr(); // 5 flops
+ if (vslipSq > square(SignificantReal)) {
+ const Real vslip = std::sqrt(vslipSq); // expensive: ~25 flops
+ // Express slip velocity as unitless multiple of transition velocity.
+ const Real v = vslip * ooVtrans;
+ // Must scale viscous coefficient to match unitless velocity.
+ const Real mu=stribeck(us,ud,uv*vtrans,v); // ~10 flops
+ //const Real mu=hollars(us,ud,uv*vtrans,v);
+ const Real fFriction = fNormal * mu;
+ // Force direction on O opposes O's velocity.
+ forceFriction = (-fFriction/vslip)*velTangent; // ~20 flops
+ powerFriction = fFriction * vslip; // always >= 0
+ }
+
+ const Vec3 forceLoss = forceC + forceFriction; // 3 flops
+ const Vec3 forceTotal = forceK + forceLoss; // 3 flops
+
+ // Accumulate the moment and force on the *other* surface as though
+ // applied at the point of O that is coincident with the resultant
+ // point; we'll move it later. (15 flops)
+ resultantForceOnOther_M += SpatialVec(r % forceTotal, forceTotal);
+
+ // Accumulate potential energy stored in elastic displacement.
+ potentialEnergy += PE; // 1 flop
+
+ // Don't include dot(forceK,velNormal) power due to conservative force
+ // here. This way we don't double-count the energy on the way in as
+ // integrated power and potential energy. Although the books would
+ // balance again when the contact is broken, it makes continuous
+ // contact look as though some energy has been lost. In the "yanking"
+ // case above, without including the conservative power term we will
+ // actually lose energy because the deformed material isn't allowed to
+ // push back on us so the energy is lost to surface vibrations or some
+ // other unmodeled effect.
+ const Real powerLossThisElement = powerC + powerFriction; // 1 flop
+ powerLoss += powerLossThisElement; // 1 flop
+
+ if (wantDetails) {
+ contactDetails_M->push_back();
+ ContactDetail& detail = contactDetails_M->back();
+ detail.m_contactPt = contactPt_M;
+ detail.m_patchNormal = normal_M;
+ detail.m_slipVelocity = velTangent;
+ detail.m_forceOnSurface2 = forceTotal;
+ detail.m_deformation = overlap;
+ detail.m_deformationRate = odot;
+ detail.m_patchArea = faceArea;
+ detail.m_peakPressure = (faceArea != 0 ? fNormal/faceArea
+ : Real(0));
+ detail.m_potentialEnergy = PE;
+ detail.m_powerLoss = powerLossThisElement;
+ }
+ }
+}
+
+} // namespace SimTK
+
diff --git a/Simbody/src/Constraint.cpp b/Simbody/src/Constraint.cpp
new file mode 100644
index 0000000..4169a02
--- /dev/null
+++ b/Simbody/src/Constraint.cpp
@@ -0,0 +1,4073 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ *
+ * Private implementation of Constraint, and its included subclasses which
+ * represent the built-in constraint types.
+ */
+
+#include "SimTKcommon.h"
+#include "simbody/internal/common.h"
+#include "simbody/internal/Constraint.h"
+
+#include "ConstraintImpl.h"
+#include "SimbodyMatterSubsystemRep.h"
+#include "MobilizedBodyImpl.h"
+
+#include <algorithm>
+
+namespace SimTK {
+
+
+//==============================================================================
+// CONSTRAINT
+//==============================================================================
+
+void Constraint::disable(State& s) const {
+ getImpl().setDisabled(s, true);
+}
+void Constraint::enable(State& s) const {
+ getImpl().setDisabled(s, false);
+}
+bool Constraint::isDisabled(const State& s) const {
+ return getImpl().isDisabled(s);
+}
+bool Constraint::isDisabledByDefault() const {
+ return getImpl().isDisabledByDefault();
+}
+void Constraint::setDisabledByDefault(bool shouldBeDisabled) {
+ updImpl().setDisabledByDefault(shouldBeDisabled);
+}
+
+const SimbodyMatterSubsystem& Constraint::getMatterSubsystem() const {
+ SimTK_ASSERT_ALWAYS(isInSubsystem(),
+ "getMatterSubsystem() called on a Constraint that is not part of a subsystem.");
+ return getImpl().getMyMatterSubsystemRep().getMySimbodyMatterSubsystemHandle();
+}
+
+ConstraintIndex Constraint::getConstraintIndex() const {
+ SimTK_ASSERT_ALWAYS(isInSubsystem(),
+ "getConstraintIndex() called on a Constraint that is not part of a subsystem.");
+ return getImpl().getMyConstraintIndex();
+}
+
+SimbodyMatterSubsystem& Constraint::updMatterSubsystem() {
+ SimTK_ASSERT_ALWAYS(isInSubsystem(),
+ "updMatterSubsystem() called on a Constraint that is not part of a subsystem.");
+ return updImpl().updMyMatterSubsystemRep().updMySimbodyMatterSubsystemHandle();
+}
+
+bool Constraint::isInSubsystem() const {
+ return getImpl().isInSubsystem();
+}
+
+bool Constraint::isInSameSubsystem(const MobilizedBody& body) const {
+ return getImpl().isInSameSubsystem(body);
+}
+
+int Constraint::getNumConstrainedBodies() const {
+ return getImpl().getNumConstrainedBodies();
+}
+int Constraint::getNumConstrainedMobilizers() const {
+ return getImpl().getNumConstrainedMobilizers();
+}
+
+int Constraint::getNumConstrainedQ(const State& s) const {
+ return getImpl().getNumConstrainedQ(s);
+}
+int Constraint::getNumConstrainedU(const State& s) const {
+ return getImpl().getNumConstrainedU(s);
+}
+
+QIndex Constraint::getQIndexOfConstrainedQ(const State& state,
+ ConstrainedQIndex consQIndex) const {
+ return getImpl().getQIndexOfConstrainedQ(state,consQIndex);
+}
+
+UIndex Constraint::getUIndexOfConstrainedU(const State& state,
+ ConstrainedUIndex consUIndex) const {
+ return getImpl().getUIndexOfConstrainedU(state,consUIndex);
+}
+
+int Constraint::getNumConstrainedQ(const State& s, ConstrainedMobilizerIndex M) const {
+ return getImpl().getNumConstrainedQ(s,M);
+}
+int Constraint::getNumConstrainedU(const State& s, ConstrainedMobilizerIndex M) const {
+ return getImpl().getNumConstrainedU(s,M);
+}
+
+ConstrainedQIndex Constraint::getConstrainedQIndex
+ (const State& s, ConstrainedMobilizerIndex M, MobilizerQIndex which) const {
+ return getImpl().getConstrainedQIndex(s,M,which);
+}
+ConstrainedUIndex Constraint::getConstrainedUIndex
+ (const State& s, ConstrainedMobilizerIndex M, MobilizerUIndex which) const {
+ return getImpl().getConstrainedUIndex(s,M,which);
+}
+
+const MobilizedBody& Constraint::getMobilizedBodyFromConstrainedMobilizer(ConstrainedMobilizerIndex M) const {
+ return getImpl().getMobilizedBodyFromConstrainedMobilizer(M);
+}
+
+const MobilizedBody& Constraint::getMobilizedBodyFromConstrainedBody(ConstrainedBodyIndex B) const {
+ return getImpl().getMobilizedBodyFromConstrainedBody(B);
+}
+const MobilizedBody& Constraint::getAncestorMobilizedBody() const {
+ return getImpl().getAncestorMobilizedBody();
+}
+
+const SimbodyMatterSubtree& Constraint::getSubtree() const {
+ assert(getImpl().subsystemTopologyHasBeenRealized());
+ return getImpl().mySubtree;
+}
+
+// Find out how many holonomic (position), nonholonomic (velocity),
+// and acceleration-only constraint equations are generated by this Constraint.
+void Constraint::
+getNumConstraintEquationsInUse(const State& s, int& mp, int& mv, int& ma) const
+{ getImpl().getNumConstraintEquationsInUse(s,mp,mv,ma); }
+
+void Constraint::
+getIndexOfMultipliersInUse(const State& s,
+ MultiplierIndex& px0,
+ MultiplierIndex& vx0,
+ MultiplierIndex& ax0) const
+{ getImpl().getIndexOfMultipliersInUse(s,px0,vx0,ax0); }
+
+void Constraint::setMyPartInConstraintSpaceVector(const State& state,
+ const Vector& myPart,
+ Vector& constraintSpace) const
+{ getImpl().setMyPartInConstraintSpaceVector(state,myPart,constraintSpace); }
+
+void Constraint::
+getMyPartFromConstraintSpaceVector(const State& state,
+ const Vector& constraintSpace,
+ Vector& myPart) const
+{ getImpl().getMyPartFromConstraintSpaceVector(state,constraintSpace,myPart);}
+
+Vector Constraint::getPositionErrorsAsVector(const State& s) const {
+ int mp,mv,ma;
+ getNumConstraintEquationsInUse(s, mp, mv, ma);
+
+ Vector perr(mp);
+ if (mp) getImpl().getPositionErrors(s, mp, &perr[0]);
+ return perr;
+}
+
+Vector Constraint::getVelocityErrorsAsVector(const State& s) const {
+ int mp,mv,ma;
+ getNumConstraintEquationsInUse(s, mp, mv, ma);
+
+ Vector pverr(mp+mv);
+ if (mp+mv) getImpl().getVelocityErrors(s, mp+mv, &pverr[0]);
+ return pverr;
+}
+
+Vector Constraint::getAccelerationErrorsAsVector(const State& s) const {
+ int mp,mv,ma;
+ getNumConstraintEquationsInUse(s, mp, mv, ma);
+
+ Vector pvaerr(mp+mv+ma);
+ if (mp+mv+ma) getImpl().getAccelerationErrors(s, mp+mv+ma, &pvaerr[0]);
+ return pvaerr;
+}
+
+Vector Constraint::getMultipliersAsVector(const State& s) const {
+ int mp,mv,ma;
+ getNumConstraintEquationsInUse(s, mp, mv, ma);
+
+ Vector mult(mp+mv+ma);
+ if (mp+mv+ma) getImpl().getMultipliers(s, mp+mv+ma, &mult[0]);
+ return mult;
+}
+
+void Constraint::getConstraintForcesAsVectors
+ (const State& state,
+ Vector_<SpatialVec>& bodyForcesInG,
+ Vector& mobilityForces) const
+{
+ SimTK_ERRCHK1_ALWAYS(!isDisabled(state),
+ "Constraint::getConstraintForcesAsVector()",
+ "Constraint %d is currently disabled; you can't ask for its forces."
+ " Use isDisabled() to check.", (int)getConstraintIndex());
+
+ ArrayViewConst_<SpatialVec,ConstrainedBodyIndex>
+ bodyF_G = getImpl().getConstrainedBodyForcesInGFromState(state);
+ ArrayViewConst_<Real,ConstrainedUIndex>
+ mobilityF = getImpl().getConstrainedMobilityForcesFromState(state);
+
+ const int ncb = bodyF_G.size();
+ bodyForcesInG.resize(ncb);
+ for (ConstrainedBodyIndex cbx(0); cbx < ncb; ++cbx)
+ bodyForcesInG[cbx] = bodyF_G[cbx];
+
+ const int ncu = mobilityF.size();
+ mobilityForces.resize(ncu);
+ for (ConstrainedUIndex cux(0); cux < ncu; ++cux)
+ mobilityForces[cux] = mobilityF[cux];
+}
+
+// Multiply constraint forces (negated) by velocities to get power.
+// (Remember that constraint forces are on the LHS so have the opposite
+// sign from applied forces.)
+Real Constraint::calcPower(const State& state) const {
+ const ConstraintImpl& impl = getImpl();
+ const SimbodyMatterSubsystemRep& matter = impl.getMyMatterSubsystemRep();
+
+ ArrayViewConst_<SpatialVec,ConstrainedBodyIndex>
+ bodyF_G = impl.getConstrainedBodyForcesInGFromState(state);
+ ArrayViewConst_<Real,ConstrainedUIndex>
+ mobilityF = impl.getConstrainedMobilityForcesFromState(state);
+
+ Real power = 0;
+ for (ConstrainedBodyIndex cbx(0); cbx < bodyF_G.size(); ++cbx) {
+ const MobilizedBodyIndex mbx =
+ impl.getMobilizedBodyIndexOfConstrainedBody(cbx);
+ const SpatialVec& V_GB = matter.getBodyVelocity(state, mbx);
+ power -= ~bodyF_G[cbx] * V_GB;
+ }
+ const Vector& u = matter.getU(state);
+ for (ConstrainedUIndex cux(0); cux < mobilityF.size(); ++cux) {
+ const UIndex ux = impl.getUIndexOfConstrainedU(state, cux);
+ power -= mobilityF[cux] * u[ux];
+ }
+
+ return power;
+}
+
+
+Vector Constraint::calcPositionErrorFromQ(const State&, const Vector& q) const {
+ SimTK_THROW1(Exception::UnimplementedMethod, "Constraint::calcPositionErrorFromQ");
+}
+
+Vector Constraint::calcVelocityErrorFromU(const State&, const Vector& u) const {
+ SimTK_THROW1(Exception::UnimplementedMethod, "Constraint::calcVelocityErrorFromU");
+}
+
+Vector Constraint::calcAccelerationErrorFromUDot(const State&, const Vector& udot) const {
+ SimTK_THROW1(Exception::UnimplementedMethod, "Constraint::calcAccelerationErrorFromUDot");
+}
+
+
+// TODO: change to use operator form to avoid State copying kludge.
+Matrix Constraint::calcPositionConstraintMatrixP(const State& s) const {
+ int mp,mv,ma;
+ getNumConstraintEquationsInUse(s, mp, mv, ma);
+
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+ const System& system = matter.getSystem();
+
+ const int nu = matter.getNU(s);
+
+ Matrix P(mp, nu);
+ if (mp && nu) {
+ Vector pverr0(mp), pverr(mp); // we're interested in the first mp of these
+ State tmp = s; // don't change s
+
+ matter.updU(tmp) = 0; // first calculate the bias term -c(t,q)
+ system.realize(tmp, Stage::Velocity);
+ pverr0 = getVelocityErrorsAsVector(tmp)(0,mp);
+
+ // Now calculate sensitivity of d(perr)/dt=Pu-c(t,q) to each u in turn.
+ for (int j=0; j<nu; ++j) {
+ matter.updU(tmp)[j] = 1;
+ system.realize(tmp, Stage::Velocity);
+ pverr = getVelocityErrorsAsVector(tmp)(0,mp);
+ matter.updU(tmp)[j] = 0;
+ P(j) = pverr - pverr0;
+ }
+ }
+ return P;
+}
+
+Matrix Constraint::calcPositionConstraintMatrixPt(const State& s) const {
+ int mp,mv,ma;
+ getNumConstraintEquationsInUse(s, mp, mv, ma);
+
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+ const System& system = matter.getSystem();
+
+ const int nu = matter.getNU(s);
+ const int nb = matter.getNumBodies();
+
+
+ Matrix Pt(nu, mp);
+ if (mp==0 || nu==0)
+ return Pt;
+
+ const ConstraintImpl& rep = getImpl();
+ const int ncb = rep.getNumConstrainedBodies();
+ const int ncq = rep.getNumConstrainedQ(s);
+ const int ncu = rep.getNumConstrainedU(s);
+
+ // Any of these may be zero length.
+ Array_<SpatialVec,ConstrainedBodyIndex> bodyForcesInA(ncb);
+ Array_<Real,ConstrainedQIndex> qForces(ncq);
+ Array_<Real,ConstrainedUIndex> mobilityForces(ncu);
+
+ Array_<Real> lambdap(mp, Real(0));
+
+ if (ncb == 0) {
+ // Mobility forces only
+ for (int i=0; i<mp; ++i) {
+ lambdap[i] = 1;
+ qForces.fill(Real(0));
+ rep.addInPositionConstraintForces(s, lambdap,
+ bodyForcesInA, qForces);
+ rep.convertQForcesToUForces(s, qForces, mobilityForces); // fu = ~N fq
+ lambdap[i] = 0;
+ Pt(i) = 0;
+ for (ConstrainedUIndex cux(0); cux < ncu; ++cux)
+ Pt(rep.getUIndexOfConstrainedU(s, cux), i) = mobilityForces[cux]; // unpack
+ }
+ } else {
+ // There are some body forces
+ Vector_<SpatialVec> bodyForcesInG(nb);
+ bodyForcesInG.setToZero();
+
+ // For converting those A-relative forces to G
+ const Rotation& R_GA = rep.getAncestorMobilizedBody().getBodyRotation(s);
+
+ // Calculate Pt*lambda with each lambda set to 1 in turn.
+ for (int i=0; i<mp; ++i) {
+ lambdap[i] = 1;
+ bodyForcesInA.fill(SpatialVec(Vec3(0), Vec3(0)));
+ qForces.fill(Real(0));
+ rep.addInPositionConstraintForces(s, lambdap,
+ bodyForcesInA, qForces);
+ rep.convertQForcesToUForces(s, qForces, mobilityForces); // fu = ~N fq
+ for (ConstrainedBodyIndex cb(0); cb < ncb; ++cb) {
+ bodyForcesInG[rep.getMobilizedBodyIndexOfConstrainedBody(cb)] =
+ R_GA*bodyForcesInA[cb];
+ }
+ lambdap[i] = 0;
+
+ rep.getMyMatterSubsystem().multiplyBySystemJacobianTranspose
+ (s,bodyForcesInG,Pt(i));
+ for (ConstrainedUIndex cux(0); cux < ncu; ++cux)
+ Pt(rep.getUIndexOfConstrainedU(s, cux), i) += mobilityForces[cux]; // unpack
+ }
+ }
+ return Pt;
+}
+
+// Calculate the constraint matrix V= partial(verr)/partial(u) for just
+// the nonholonomic constraints. For this we use the acceleration error
+// equations obtained from differentiation of the nonholonomic constraints:
+// verr_dot = V udot - b(t,q,u)
+//
+Matrix Constraint::calcVelocityConstraintMatrixV(const State& s) const {
+ int mp,mv,ma;
+ getNumConstraintEquationsInUse(s, mp, mv, ma);
+
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+ const System& system = matter.getSystem();
+
+ const int nu = matter.getNU(s); // same as nudot
+
+ Matrix V(mv, nu);
+ if (mv && nu) {
+ Vector vaerr0(mv), vaerr(mv); // we're interested in the middle mv of these (mp-mv-ma)
+
+ Vector udot(nu);
+ udot = 0;
+
+ // Calculate the bias term -b(t,q,u)
+ vaerr0 = calcAccelerationErrorFromUDot(s, udot)(mp, mv);
+
+ // Now calculate sensitivity of d(verr)/dt=Vudot-b(t,q,u) to each udot in turn.
+ for (int j=0; j<nu; ++j) {
+ udot[j] = 1;
+ vaerr = calcAccelerationErrorFromUDot(s, udot)(mp, mv);
+ udot[j] = 0;
+ V(j) = vaerr - vaerr0;
+ }
+ }
+ return V;
+}
+
+Matrix Constraint::calcVelocityConstraintMatrixVt(const State& s) const {
+ int mp,mv,ma;
+ getNumConstraintEquationsInUse(s, mp, mv, ma);
+
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+ const System& system = matter.getSystem();
+
+ const int nu = matter.getNU(s);
+ const int nb = matter.getNumBodies();
+
+
+ Matrix Vt(nu, mv);
+ if (mv==0 || nu==0)
+ return Vt;
+
+ const ConstraintImpl& rep = getImpl();
+ const int ncb = rep.getNumConstrainedBodies();
+ const int ncu = rep.getNumConstrainedU(s);
+
+ // Either of these may be zero length.
+ Array_<SpatialVec,ConstrainedBodyIndex> bodyForcesInA(ncb);
+ Array_<Real,ConstrainedUIndex> mobilityForces(ncu);
+ Array_<Real> lambdav(mv, Real(0));
+
+ if (ncb == 0) {
+ // Mobility forces only
+ for (int i=0; i<mv; ++i) {
+ lambdav[i] = 1;
+ mobilityForces.fill(Real(0));
+ rep.addInVelocityConstraintForces(s, lambdav,
+ bodyForcesInA, mobilityForces);
+ lambdav[i] = 0;
+ Vt(i) = 0; // set column i to zero
+ for (ConstrainedUIndex cux(0); cux < ncu; ++cux)
+ Vt(rep.getUIndexOfConstrainedU(s, cux), i) =
+ mobilityForces[cux]; // unpack
+ }
+ } else {
+ // There are some body forces
+ Vector_<SpatialVec> bodyForcesInG(nb);
+ bodyForcesInG = SpatialVec(Vec3(0), Vec3(0));
+
+ // For converting those A-relative forces to G
+ const Rotation& R_GA =
+ rep.getAncestorMobilizedBody().getBodyRotation(s);
+
+ // Calculate Vt*lambda with each lambda set to 1 in turn.
+ for (int i=0; i<mv; ++i) {
+ lambdav[i] = 1;
+ bodyForcesInA.fill(SpatialVec(Vec3(0), Vec3(0)));
+ mobilityForces.fill(Real(0));
+ rep.addInVelocityConstraintForces(s, lambdav,
+ bodyForcesInA, mobilityForces);
+ for (ConstrainedBodyIndex cb(0); cb < ncb; ++cb) {
+ bodyForcesInG[rep.getMobilizedBodyIndexOfConstrainedBody(cb)] =
+ R_GA*bodyForcesInA[cb];
+ }
+ lambdav[i] = 0;
+
+ // Convert body forces F to generalized forces f=~J*F.
+ // TODO: this should result in generalized forces only on the
+ // participating mobilities - does it?
+ rep.getMyMatterSubsystem().multiplyBySystemJacobianTranspose
+ (s,bodyForcesInG,Vt(i));
+
+ // Now add in the generalized forces produced directly by the
+ // Constraint, unpacking into the right global mobility slot.
+ for (ConstrainedUIndex cux(0); cux < ncu; ++cux)
+ Vt(rep.getUIndexOfConstrainedU(s,cux), i)
+ += mobilityForces[cux];
+ }
+ }
+
+ return Vt;
+}
+
+// Calculate the constraint matrix A= partial(aerr)/partial(udot) for just
+// the acceleration-only constraints. For this we use the acceleration error
+// equations directly because we're *requiring* that acceleration-only constraints
+// are linear in the udots.
+// aerr = A udot - b(t,q,u)
+//
+Matrix Constraint::calcAccelerationConstraintMatrixA(const State& s) const {
+ int mp,mv,ma;
+ getNumConstraintEquationsInUse(s, mp, mv, ma);
+
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+ const System& system = matter.getSystem();
+
+ const int nudot = matter.getNU(s); // same as nudot
+
+ Matrix A(ma, nudot);
+ if (ma && nudot) {
+ Vector aerr0(ma), aerr(ma); // we're interested in the final ma of these (mp-mv-ma)
+
+ Vector udot(nudot);
+ udot = 0;
+
+ // Calculate the bias term -b(t,q,u)
+ aerr0 = calcAccelerationErrorFromUDot(s, udot)(mp+mv, ma);
+
+ // Now calculate sensitivity of d(aerr)/dt=Audot-b(t,q,u) to each udot in turn.
+ for (int j=0; j<nudot; ++j) {
+ udot[j] = 1;
+ aerr = calcAccelerationErrorFromUDot(s, udot)(mp+mv, ma);
+ udot[j] = 0;
+ A(j) = aerr - aerr0;
+ }
+ }
+ return A;
+}
+
+Matrix Constraint::calcAccelerationConstraintMatrixAt(const State& s) const {
+ int mp,mv,ma;
+ getNumConstraintEquationsInUse(s, mp, mv, ma);
+
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+ const System& system = matter.getSystem();
+
+ const int nu = matter.getNU(s);
+ const int nb = matter.getNumBodies();
+
+
+ Matrix At(nu, ma);
+ if (ma==0 || nu==0)
+ return At;
+
+ const ConstraintImpl& rep = getImpl();
+ const int ncb = rep.getNumConstrainedBodies();
+ const int ncu = rep.getNumConstrainedU(s);
+
+ // Either of these may be zero length.
+ Array_<SpatialVec,ConstrainedBodyIndex> bodyForcesInA(ncb);
+ Array_<Real,ConstrainedUIndex> mobilityForces(ncu);
+ Array_<Real> lambdaa(ma, Real(0));
+
+ if (ncb == 0) {
+ // Mobility forces only
+ for (int i=0; i<ma; ++i) {
+ lambdaa[i] = 1;
+ mobilityForces.fill(Real(0));
+ rep.addInAccelerationConstraintForces(s, lambdaa,
+ bodyForcesInA, mobilityForces);
+ lambdaa[i] = 0;
+ At(i) = 0;
+ for (ConstrainedUIndex cux(0); cux < ncu; ++cux)
+ At(rep.getUIndexOfConstrainedU(s, cux), i) = mobilityForces[cux]; // unpack
+ }
+ } else {
+ // There are some body forces
+ Vector_<SpatialVec> bodyForcesInG(nb);
+ bodyForcesInG = SpatialVec(Vec3(0), Vec3(0));
+
+ // For converting those A-relative forces to G
+ const Rotation& R_GA = rep.getAncestorMobilizedBody().getBodyRotation(s);
+
+ // Calculate At*lambda with each lambda set to 1 in turn.
+ for (int i=0; i<ma; ++i) {
+ lambdaa[i] = 1;
+ bodyForcesInA.fill(SpatialVec(Vec3(0), Vec3(0)));
+ mobilityForces.fill(Real(0));
+ rep.addInAccelerationConstraintForces(s, lambdaa,
+ bodyForcesInA, mobilityForces);
+ for (ConstrainedBodyIndex cb(0); cb < ncb; ++cb) {
+ bodyForcesInG[rep.getMobilizedBodyIndexOfConstrainedBody(cb)] =
+ R_GA*bodyForcesInA[cb];
+ }
+ lambdaa[i] = 0;
+
+ rep.getMyMatterSubsystem().multiplyBySystemJacobianTranspose
+ (s,bodyForcesInG,At(i));
+ for (ConstrainedUIndex cux(0); cux < ncu; ++cux)
+ At(rep.getUIndexOfConstrainedU(s, cux), i) += mobilityForces[cux]; // unpack
+ }
+ }
+ return At;
+}
+
+Matrix Constraint::calcPositionConstraintMatrixPNInv(const State& s) const {
+ int mp,mv,ma;
+ getNumConstraintEquationsInUse(s, mp, mv, ma);
+
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+ const System& system = matter.getSystem();
+
+ const int nu = matter.getNU(s);
+ const int nq = matter.getNQ(s);
+
+ const Matrix P = calcPositionConstraintMatrixP(s);
+ assert(P.nrow()==mp && P.ncol()==nu);
+
+ Matrix PNInv(mp, nq); // = P*N^-1
+ if (mp && nq) {
+ // The routine below calculates qlikeRow = ulikeRow * N^-1 which is what
+ // we need but it actually works with Vectors (transpose of RowVectors)
+ // and at the moment they have to be contiguous so we'll have to copy.
+ Vector uin(nu);
+ Vector qout(nq);
+ for (int i=0; i < mp; ++i) {
+ uin = ~P[i];
+ matter.multiplyByNInv(s, true, uin, qout);
+ PNInv[i] = ~qout;
+ }
+ }
+ return PNInv;
+}
+
+void Constraint::calcConstraintForcesFromMultipliers(
+ const State& s,
+ const Vector& lambda,
+ Vector_<SpatialVec>& bodyForcesInA,
+ Vector& mobilityForces) const
+{
+ int mp, mv, ma;
+ getNumConstraintEquationsInUse(s, mp, mv, ma);
+
+ SimTK_APIARGCHECK2_ALWAYS(lambda.size() == mp+mv+ma,
+ "Constraint", "calcConstraintForcesFromMultipliers()",
+ "Expected %d multipliers but got %d.", mp+mv+ma, lambda.size());
+
+ // We have to convert to and from Arrays since the underlying
+ // constraint methods use those for speed.
+
+ Array_<Real> lambdap(mp), lambdav(mv), lambdaa(ma);
+ for (int i=0; i<mp; ++i) lambdap[i] = lambda[i];
+ for (int i=0; i<mv; ++i) lambdav[i] = lambda[mp+i];
+ for (int i=0; i<ma; ++i) lambdaa[i] = lambda[mp+mv+i];
+
+ Array_<SpatialVec,ConstrainedBodyIndex> tmpBodyForcesInA;
+ Array_<Real, ConstrainedUIndex> tmpMobilityForces;
+
+ getImpl().calcConstraintForcesFromMultipliers
+ (s,lambdap,lambdav,lambdaa,tmpBodyForcesInA,tmpMobilityForces);
+
+ const int ncb = tmpBodyForcesInA.size(); // # constrained bodies
+ const int ncu = tmpMobilityForces.size(); // # constrained u's
+
+ bodyForcesInA.resize(ncb); mobilityForces.resize(ncu);
+
+ for (ConstrainedBodyIndex i(0); i<ncb; ++i)
+ bodyForcesInA[i] = tmpBodyForcesInA[i];
+
+ for (ConstrainedUIndex i(0); i<ncu; ++i)
+ mobilityForces[i] = tmpMobilityForces[i];
+}
+
+
+
+//==============================================================================
+// CONSTRAINT::ROD
+//==============================================================================
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Constraint::Rod, Constraint::RodImpl, Constraint);
+
+Constraint::Rod::Rod(MobilizedBody& body1, MobilizedBody& body2, Real defaultRodLength)
+ : Constraint(new RodImpl())
+{
+ SimTK_ASSERT_ALWAYS(body1.isInSubsystem() && body2.isInSubsystem(),
+ "Constraint::Rod(): both bodies must already be in a MatterSubsystem.");
+ SimTK_ASSERT_ALWAYS(body1.isInSameSubsystem(body2),
+ "Constraint::Rod(): both bodies to be connected must be in the same MatterSubsystem.");
+ SimTK_ASSERT_ALWAYS(defaultRodLength > 0,
+ "Constraint::Rod(): Rod length must always be greater than zero");
+
+ body1.updMatterSubsystem().adoptConstraint(*this);
+
+ updImpl().B1 = updImpl().addConstrainedBody(body1);
+ updImpl().B2 = updImpl().addConstrainedBody(body2);
+
+ updImpl().defaultRodLength = defaultRodLength;
+}
+
+Constraint::Rod::Rod(MobilizedBody& body1, const Vec3& point1,
+ MobilizedBody& body2, const Vec3& point2, Real defaultRodLength)
+ : Constraint(new RodImpl())
+{
+ SimTK_ASSERT_ALWAYS(body1.isInSubsystem() && body2.isInSubsystem(),
+ "Constraint::Rod(): both bodies must already be in a MatterSubsystem.");
+ SimTK_ASSERT_ALWAYS(body1.isInSameSubsystem(body2),
+ "Constraint::Rod(): both bodies to be connected must be in the same MatterSubsystem.");
+ SimTK_ASSERT_ALWAYS(defaultRodLength > 0,
+ "Constraint::Rod(): Rod length must always be greater than zero");
+
+ body1.updMatterSubsystem().adoptConstraint(*this);
+
+ updImpl().B1 = updImpl().addConstrainedBody(body1);
+ updImpl().B2 = updImpl().addConstrainedBody(body2);
+
+ updImpl().defaultPoint1 = point1;
+ updImpl().defaultPoint2 = point2;
+ updImpl().defaultRodLength = defaultRodLength;
+}
+
+Constraint::Rod& Constraint::Rod::setDefaultPointOnBody1(const Vec3& p1) {
+ updImpl().defaultPoint1 = p1;
+ return *this;
+}
+
+Constraint::Rod& Constraint::Rod::setDefaultPointOnBody2(const Vec3& p2) {
+ updImpl().defaultPoint2 = p2;
+ return *this;
+}
+
+Constraint::Rod& Constraint::Rod::setDefaultRodLength(Real length) {
+ updImpl().defaultRodLength = length;
+ return *this;
+}
+
+
+MobilizedBodyIndex Constraint::Rod::getBody1MobilizedBodyIndex() const {
+ return getImpl().getMobilizedBodyIndexOfConstrainedBody(getImpl().B1);
+}
+MobilizedBodyIndex Constraint::Rod::getBody2MobilizedBodyIndex() const {
+ return getImpl().getMobilizedBodyIndexOfConstrainedBody(getImpl().B1);
+}
+const Vec3& Constraint::Rod::getDefaultPointOnBody1() const {
+ return getImpl().defaultPoint1;
+}
+const Vec3& Constraint::Rod::getDefaultPointOnBody2() const {
+ return getImpl().defaultPoint2;
+}
+Real Constraint::Rod::getDefaultRodLength() const {
+ return getImpl().defaultRodLength;
+}
+
+Real Constraint::Rod::getPositionError(const State& s) const {
+ Real perr;
+ getImpl().getPositionErrors(s, 1, &perr);
+ return perr;
+}
+
+Real Constraint::Rod::getVelocityError(const State& s) const {
+ Real pverr;
+ getImpl().getVelocityErrors(s, 1, &pverr);
+ return pverr;
+}
+
+Real Constraint::Rod::getAccelerationError(const State& s) const {
+ Real pvaerr;
+ getImpl().getAccelerationErrors(s, 1, &pvaerr);
+ return pvaerr;
+}
+
+Real Constraint::Rod::getMultiplier(const State& s) const {
+ Real mult;
+ getImpl().getMultipliers(s, 1, &mult);
+ return mult;
+}
+
+
+
+ // RodImpl
+
+void Constraint::Rod::RodImpl::calcDecorativeGeometryAndAppendVirtual
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+{
+ if (!getMyMatterSubsystemRep().getShowDefaultGeometry())
+ return;
+
+ // We can't generate the endpoint artwork until we know the end point stations,
+ // which could be as late as Stage::Instance.
+ if (stage == Stage::Instance && pointRadius != 0) {
+ // TODO: point stations and rod length should be instance-stage data
+ // from State rather than topological data
+ const MobilizedBodyIndex body1 = getMobilizedBodyIndexOfConstrainedBody(B1);
+ const MobilizedBodyIndex body2 = getMobilizedBodyIndexOfConstrainedBody(B2);
+
+ const Real useRadius = pointRadius > 0 ? pointRadius
+ : std::max(defaultRodLength, Real(.1)) * Real(0.02); // 2% of the length by default
+
+ // Draw a blue mesh sphere at the first point.
+ geom.push_back(DecorativeSphere(useRadius)
+ .setColor(Blue)
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setResolution(Real(0.5))
+ .setBodyId(body1)
+ .setTransform(defaultPoint1));
+
+ // On the follower body draw an purple mesh sphere at the point radius.
+ geom.push_back(DecorativeSphere(useRadius)
+ .setColor(Purple)
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setResolution(Real(0.5))
+ .setBodyId(body2)
+ .setTransform(defaultPoint2));
+ }
+
+ // We can't generate the line artwork until we know the two end point locations,
+ // which isn't until Position stage since the ends are on different bodies.
+ if (stage == Stage::Position) {
+ // TODO: point stations and rod length should be instance-stage data
+ // from State rather than topological data
+
+ const Vec3 p_GP1 = getMobilizedBodyFromConstrainedBody(B1)
+ .findStationLocationInGround(s, defaultPoint1);
+ const Vec3 p_GP2 = getMobilizedBodyFromConstrainedBody(B2)
+ .findStationLocationInGround(s, defaultPoint2);
+
+ const Vec3 p_P1P2 = p_GP2 - p_GP1;
+ const Real d = p_P1P2.norm();
+
+ if (d >= SignificantReal) {
+ const Vec3 endPoint = p_GP1 + defaultRodLength * p_P1P2/d;
+ geom.push_back(DecorativeLine(p_GP1, endPoint)
+ .setColor(Gray)
+ .setLineThickness(3)
+ .setBodyId(GroundIndex));
+ }
+ }
+
+}
+
+
+
+//==============================================================================
+// CONSTRAINT::POINT IN PLANE
+//==============================================================================
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Constraint::PointInPlane, Constraint::PointInPlaneImpl, Constraint);
+
+Constraint::PointInPlane::PointInPlane
+ (MobilizedBody& planeBody, const UnitVec3& defPlaneNormal, Real defPlaneHeight,
+ MobilizedBody& followerBody, const Vec3& defFollowerPoint)
+ : Constraint(new PointInPlaneImpl())
+{
+ SimTK_ASSERT_ALWAYS(planeBody.isInSubsystem() && followerBody.isInSubsystem(),
+ "Constraint::PointInPlane(): both bodies must already be in a SimbodyMatterSubsystem.");
+ SimTK_ASSERT_ALWAYS(planeBody.isInSameSubsystem(followerBody),
+ "Constraint::PointInPlane(): both bodies to be connected must be in the same SimbodyMatterSubsystem.");
+
+ //rep = new PointInPlaneRep(); rep->setMyHandle(*this);
+ planeBody.updMatterSubsystem().adoptConstraint(*this);
+
+ updImpl().planeBody = updImpl().addConstrainedBody(planeBody);
+ updImpl().followerBody = updImpl().addConstrainedBody(followerBody);
+ updImpl().defaultPlaneNormal = defPlaneNormal;
+ updImpl().defaultPlaneHeight = defPlaneHeight;
+ updImpl().defaultFollowerPoint = defFollowerPoint;
+}
+
+Constraint::PointInPlane& Constraint::PointInPlane::setDefaultPlaneNormal(const UnitVec3& n) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultPlaneNormal = n;
+ return *this;
+}
+
+Constraint::PointInPlane& Constraint::PointInPlane::setDefaultPlaneHeight(Real h) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultPlaneHeight = h;
+ return *this;
+}
+
+Constraint::PointInPlane& Constraint::PointInPlane::setDefaultFollowerPoint(const Vec3& p) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultFollowerPoint = p;
+ return *this;
+}
+
+MobilizedBodyIndex Constraint::PointInPlane::getPlaneMobilizedBodyIndex() const {
+ return getImpl().getMobilizedBodyIndexOfConstrainedBody(getImpl().planeBody);
+}
+MobilizedBodyIndex Constraint::PointInPlane::getFollowerMobilizedBodyIndex() const {
+ return getImpl().getMobilizedBodyIndexOfConstrainedBody(getImpl().followerBody);
+}
+const UnitVec3& Constraint::PointInPlane::getDefaultPlaneNormal() const {
+ return getImpl().defaultPlaneNormal;
+}
+Real Constraint::PointInPlane::getDefaultPlaneHeight() const {
+ return getImpl().defaultPlaneHeight;
+}
+const Vec3& Constraint::PointInPlane::getDefaultFollowerPoint() const {
+ return getImpl().defaultFollowerPoint;
+}
+
+Constraint::PointInPlane& Constraint::PointInPlane::setPlaneDisplayHalfWidth(Real h) {
+ updImpl().setPlaneDisplayHalfWidth(h);
+ return *this;
+}
+Constraint::PointInPlane& Constraint::PointInPlane::setPointDisplayRadius(Real r) {
+ updImpl().setPointDisplayRadius(r);
+ return *this;
+}
+
+Real Constraint::PointInPlane::getPlaneDisplayHalfWidth() const {
+ return getImpl().getPlaneDisplayHalfWidth();
+}
+
+Real Constraint::PointInPlane::getPointDisplayRadius() const {
+ return getImpl().getPointDisplayRadius();
+}
+
+Real Constraint::PointInPlane::getPositionError(const State& s) const {
+ Real perr;
+ getImpl().getPositionErrors(s, 1, &perr);
+ return perr;
+}
+
+Real Constraint::PointInPlane::getVelocityError(const State& s) const {
+ Real pverr;
+ getImpl().getVelocityErrors(s, 1, &pverr);
+ return pverr;
+}
+
+Real Constraint::PointInPlane::getAccelerationError(const State& s) const {
+ Real pvaerr;
+ getImpl().getAccelerationErrors(s, 1, &pvaerr);
+ return pvaerr;
+}
+
+Real Constraint::PointInPlane::getMultiplier(const State& s) const {
+ Real mult;
+ getImpl().getMultipliers(s, 1, &mult);
+ return mult;
+}
+
+ // PointInPlaneImpl
+
+void Constraint::PointInPlane::PointInPlaneImpl::calcDecorativeGeometryAndAppendVirtual
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+{
+ // We can't generate the artwork until we know the normal, height, and follower
+ // point location, which might not be until Instance stage.
+ if (stage == Stage::Instance && getMyMatterSubsystemRep().getShowDefaultGeometry()) {
+ const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
+ // TODO: should be instance-stage data from State rather than topological data
+ // This makes z axis point along plane normal
+ const Transform X_B1(Rotation(defaultPlaneNormal,ZAxis), defaultPlaneHeight*defaultPlaneNormal);
+ const Transform X_B2(Rotation(), defaultFollowerPoint);
+
+ const MobilizedBodyIndex planeMBId = getMobilizedBodyIndexOfConstrainedBody(planeBody);
+ const MobilizedBodyIndex followerMBId = getMobilizedBodyIndexOfConstrainedBody(followerBody);
+
+ if (planeHalfWidth > 0 && pointRadius > 0) {
+ // On the inboard body, draw a gray transparent rectangle, outlined in black lines.
+ geom.push_back(DecorativeBrick(Vec3(planeHalfWidth,planeHalfWidth,pointRadius/2))
+ .setColor(Gray)
+ .setRepresentation(DecorativeGeometry::DrawSurface)
+ .setOpacity(Real(0.3))
+ .setBodyId(planeMBId)
+ .setTransform(X_B1));
+ geom.push_back(DecorativeBrick(Vec3(planeHalfWidth,planeHalfWidth,pointRadius/2))
+ .setColor(Black)
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setBodyId(planeMBId)
+ .setTransform(X_B1));
+
+ // On the follower body draw an orange mesh sphere at the point radius.
+ geom.push_back(DecorativeSphere(pointRadius)
+ .setColor(Orange)
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setResolution(Real(0.5))
+ .setBodyId(followerMBId)
+ .setTransform(X_B2));
+ }
+ }
+}
+
+
+
+//==============================================================================
+// CONSTRAINT::POINT ON LINE
+//==============================================================================
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Constraint::PointOnLine, Constraint::PointOnLineImpl, Constraint);
+
+Constraint::PointOnLine::PointOnLine
+ (MobilizedBody& lineBody, const UnitVec3& defLineDirection, const Vec3& defPointOnLine,
+ MobilizedBody& followerBody, const Vec3& defFollowerPoint)
+ : Constraint(new PointOnLineImpl())
+{
+ SimTK_ASSERT_ALWAYS(lineBody.isInSubsystem() && followerBody.isInSubsystem(),
+ "Constraint::PointOnLine(): both bodies must already be in a SimbodyMatterSubsystem.");
+ SimTK_ASSERT_ALWAYS(lineBody.isInSameSubsystem(followerBody),
+ "Constraint::PointOnLine(): both bodies to be connected must be in the same SimbodyMatterSubsystem.");
+
+ //rep = new PointOnLineRep(); rep->setMyHandle(*this);
+ lineBody.updMatterSubsystem().adoptConstraint(*this);
+
+ updImpl().lineBody = updImpl().addConstrainedBody(lineBody);
+ updImpl().followerBody = updImpl().addConstrainedBody(followerBody);
+ updImpl().defaultLineDirection = defLineDirection;
+ updImpl().defaultPointOnLine = defPointOnLine;
+ updImpl().defaultFollowerPoint = defFollowerPoint;
+}
+
+Constraint::PointOnLine& Constraint::PointOnLine::setDefaultLineDirection(const UnitVec3& z) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultLineDirection = z;
+ return *this;
+}
+
+Constraint::PointOnLine& Constraint::PointOnLine::setDefaultPointOnLine(const Vec3& P) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultPointOnLine = P;
+ return *this;
+}
+
+Constraint::PointOnLine& Constraint::PointOnLine::setDefaultFollowerPoint(const Vec3& S) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultFollowerPoint = S;
+ return *this;
+}
+
+MobilizedBodyIndex Constraint::PointOnLine::getLineMobilizedBodyIndex() const {
+ return getImpl().getMobilizedBodyIndexOfConstrainedBody(getImpl().lineBody);
+}
+MobilizedBodyIndex Constraint::PointOnLine::getFollowerMobilizedBodyIndex() const {
+ return getImpl().getMobilizedBodyIndexOfConstrainedBody(getImpl().followerBody);
+}
+const UnitVec3& Constraint::PointOnLine::getDefaultLineDirection() const {
+ return getImpl().defaultLineDirection;
+}
+const Vec3& Constraint::PointOnLine::getDefaultPointOnLine() const {
+ return getImpl().defaultPointOnLine;
+}
+const Vec3& Constraint::PointOnLine::getDefaultFollowerPoint() const {
+ return getImpl().defaultFollowerPoint;
+}
+
+Constraint::PointOnLine& Constraint::PointOnLine::setLineDisplayHalfLength(Real h) {
+ updImpl().setLineDisplayHalfLength(h);
+ return *this;
+}
+Constraint::PointOnLine& Constraint::PointOnLine::setPointDisplayRadius(Real r) {
+ updImpl().setPointDisplayRadius(r);
+ return *this;
+}
+
+Real Constraint::PointOnLine::getLineDisplayHalfLength() const {
+ return getImpl().getLineDisplayHalfLength();
+}
+
+Real Constraint::PointOnLine::getPointDisplayRadius() const {
+ return getImpl().getPointDisplayRadius();
+}
+
+Vec2 Constraint::PointOnLine::getPositionErrors(const State& s) const {
+ Vec2 perr;
+ getImpl().getPositionErrors(s, 2, &perr[0]);
+ return perr;
+}
+
+Vec2 Constraint::PointOnLine::getVelocityErrors(const State& s) const {
+ Vec2 pverr;
+ getImpl().getVelocityErrors(s, 2, &pverr[0]);
+ return pverr;
+}
+
+Vec2 Constraint::PointOnLine::getAccelerationErrors(const State& s) const {
+ Vec2 pvaerr;
+ getImpl().getAccelerationErrors(s, 2, &pvaerr[0]);
+ return pvaerr;
+}
+
+Vec2 Constraint::PointOnLine::getMultipliers(const State& s) const {
+ Vec2 mult;
+ getImpl().getMultipliers(s, 2, &mult[0]);
+ return mult;
+}
+
+
+ // PointOnLineImpl
+
+void Constraint::PointOnLine::PointOnLineImpl::calcDecorativeGeometryAndAppendVirtual
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+{
+ // We can't generate the artwork until we know the direction, point on line, and follower
+ // point location, which might not be until Instance stage.
+ if (stage == Stage::Instance && getMyMatterSubsystemRep().getShowDefaultGeometry()) {
+ const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
+ // TODO: should be instance-stage data from State rather than topological data
+ // This makes z axis point along line
+ const Transform X_B1(Rotation(defaultLineDirection,ZAxis), defaultPointOnLine);
+ const Transform X_B2(Rotation(), defaultFollowerPoint);
+
+ const MobilizedBodyIndex lineMBId = getMobilizedBodyIndexOfConstrainedBody(lineBody);
+ const MobilizedBodyIndex followerMBId = getMobilizedBodyIndexOfConstrainedBody(followerBody);
+
+ if (lineHalfLength > 0 && pointRadius > 0) {
+ // On the line body, draw a black line centered at the point-on-line.
+ geom.push_back(DecorativeLine(Vec3(0,0,-lineHalfLength), Vec3(0,0,lineHalfLength))
+ .setColor(Black)
+ .setLineThickness(3)
+ .setBodyId(lineMBId)
+ .setTransform(X_B1));
+
+ // On the line body draw a blue mesh sphere at the line center.
+ geom.push_back(DecorativeSphere(pointRadius)
+ .setColor(Blue)
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setResolution(0.5)
+ .setBodyId(lineMBId)
+ .setTransform(X_B1));
+
+ // On the follower body draw an orange mesh sphere at the point radius.
+ geom.push_back(DecorativeSphere(pointRadius)
+ .setColor(Orange)
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setResolution(0.5)
+ .setBodyId(followerMBId)
+ .setTransform(X_B2));
+ }
+ }
+}
+
+
+
+//==============================================================================
+// CONSTRAINT::CONSTANT ANGLE
+//==============================================================================
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Constraint::ConstantAngle, Constraint::ConstantAngleImpl, Constraint);
+
+Constraint::ConstantAngle::ConstantAngle
+ (MobilizedBody& baseBody, const UnitVec3& defaultAxisOnB,
+ MobilizedBody& followerBody, const UnitVec3& defaultAxisOnF,
+ Real angle)
+ : Constraint(new ConstantAngleImpl())
+{
+ SimTK_ASSERT_ALWAYS(baseBody.isInSubsystem() && followerBody.isInSubsystem(),
+ "Constraint::ConstantAngle(): both bodies must already be in a SimbodyMatterSubsystem.");
+ SimTK_ASSERT_ALWAYS(baseBody.isInSameSubsystem(followerBody),
+ "Constraint::ConstantAngle(): both bodies to be connected must be in the same SimbodyMatterSubsystem.");
+
+ //rep = new ConstantAngleRep(); rep->setMyHandle(*this);
+ baseBody.updMatterSubsystem().adoptConstraint(*this);
+
+ updImpl().B = updImpl().addConstrainedBody(baseBody);
+ updImpl().F = updImpl().addConstrainedBody(followerBody);
+ updImpl().defaultAxisB = defaultAxisOnB;
+ updImpl().defaultAxisF = defaultAxisOnF;
+ updImpl().defaultAngle = angle;
+}
+
+Constraint::ConstantAngle& Constraint::ConstantAngle::setDefaultBaseAxis(const UnitVec3& a) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultAxisB = a;
+ return *this;
+}
+
+Constraint::ConstantAngle& Constraint::ConstantAngle::setDefaultFollowerAxis(const UnitVec3& a) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultAxisF = a;
+ return *this;
+}
+
+Constraint::ConstantAngle& Constraint::ConstantAngle::setDefaultAngle(Real t) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultAngle = t;
+ return *this;
+}
+
+MobilizedBodyIndex Constraint::ConstantAngle::getBaseMobilizedBodyIndex() const {
+ return getImpl().getMobilizedBodyIndexOfConstrainedBody(getImpl().B);
+}
+MobilizedBodyIndex Constraint::ConstantAngle::getFollowerMobilizedBodyIndex() const {
+ return getImpl().getMobilizedBodyIndexOfConstrainedBody(getImpl().F);
+}
+const UnitVec3& Constraint::ConstantAngle::getDefaultBaseAxis() const {
+ return getImpl().defaultAxisB;
+}
+const UnitVec3& Constraint::ConstantAngle::getDefaultFollowerAxis() const {
+ return getImpl().defaultAxisF;
+}
+Real Constraint::ConstantAngle::getDefaultAngle() const {
+ return getImpl().defaultAngle;
+}
+
+Constraint::ConstantAngle& Constraint::ConstantAngle::setAxisDisplayLength(Real l) {
+ updImpl().axisLength = l;
+ return *this;
+}
+Constraint::ConstantAngle& Constraint::ConstantAngle::setAxisDisplayWidth(Real w) {
+ updImpl().axisThickness = w;
+ return *this;
+}
+
+Real Constraint::ConstantAngle::getAxisDisplayLength() const {
+ return getImpl().axisLength;
+}
+
+Real Constraint::ConstantAngle::getAxisDisplayWidth() const {
+ return getImpl().axisThickness;
+}
+
+Real Constraint::ConstantAngle::getPositionError(const State& s) const {
+ Real perr;
+ getImpl().getPositionErrors(s, 1, &perr);
+ return perr;
+}
+
+Real Constraint::ConstantAngle::getVelocityError(const State& s) const {
+ Real pverr;
+ getImpl().getVelocityErrors(s, 1, &pverr);
+ return pverr;
+}
+
+Real Constraint::ConstantAngle::getAccelerationError(const State& s) const {
+ Real pvaerr;
+ getImpl().getAccelerationErrors(s, 1, &pvaerr);
+ return pvaerr;
+}
+
+Real Constraint::ConstantAngle::getMultiplier(const State& s) const {
+ Real mult;
+ getImpl().getMultipliers(s, 1, &mult);
+ return mult;
+}
+
+ // ConstantAngleImpl
+
+void Constraint::ConstantAngle::ConstantAngleImpl::calcDecorativeGeometryAndAppendVirtual
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+{
+ // We can't generate the artwork until we know the normal, height, and follower
+ // point location, which might not be until Instance stage.
+ if (stage == Stage::Instance && getMyMatterSubsystemRep().getShowDefaultGeometry()) {
+ const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
+ //TODO
+ }
+}
+
+
+
+//==============================================================================
+// CONSTRAINT::BALL
+//==============================================================================
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Constraint::Ball, Constraint::BallImpl, Constraint);
+
+Constraint::Ball::Ball(MobilizedBody& body1, MobilizedBody& body2)
+ : Constraint(new BallImpl())
+{
+ SimTK_ASSERT_ALWAYS(body1.isInSubsystem() && body2.isInSubsystem(),
+ "Constraint::Ball(): both bodies must already be in a MatterSubsystem.");
+ SimTK_ASSERT_ALWAYS(body1.isInSameSubsystem(body2),
+ "Constraint::Ball(): both bodies to be connected must be in the same MatterSubsystem.");
+
+ //rep = new BallRep(); rep->setMyHandle(*this);
+ body1.updMatterSubsystem().adoptConstraint(*this);
+
+ updImpl().B1 = updImpl().addConstrainedBody(body1);
+ updImpl().B2 = updImpl().addConstrainedBody(body2);
+}
+
+Constraint::Ball::Ball(MobilizedBody& body1, const Vec3& point1,
+ MobilizedBody& body2, const Vec3& point2)
+ : Constraint(new BallImpl())
+{
+ SimTK_ASSERT_ALWAYS(body1.isInSubsystem() && body2.isInSubsystem(),
+ "Constraint::Ball(): both bodies must already be in a MatterSubsystem.");
+ SimTK_ASSERT_ALWAYS(body1.isInSameSubsystem(body2),
+ "Constraint::Ball(): both bodies to be connected must be in the same MatterSubsystem.");
+
+ //rep = new BallRep(); rep->setMyHandle(*this);
+ body1.updMatterSubsystem().adoptConstraint(*this);
+
+ updImpl().B1 = updImpl().addConstrainedBody(body1);
+ updImpl().B2 = updImpl().addConstrainedBody(body2);
+
+ updImpl().defaultPoint1 = point1;
+ updImpl().defaultPoint2 = point2;
+}
+
+Constraint::Ball& Constraint::Ball::setDefaultPointOnBody1(const Vec3& p1) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultPoint1 = p1;
+ return *this;
+}
+
+Constraint::Ball& Constraint::Ball::setDefaultPointOnBody2(const Vec3& p2) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultPoint2 = p2;
+ return *this;
+}
+
+MobilizedBodyIndex Constraint::Ball::getBody1MobilizedBodyIndex() const {
+ return getImpl().getMobilizedBodyIndexOfConstrainedBody(getImpl().B1);
+}
+MobilizedBodyIndex Constraint::Ball::getBody2MobilizedBodyIndex() const {
+ return getImpl().getMobilizedBodyIndexOfConstrainedBody(getImpl().B2);
+}
+const Vec3& Constraint::Ball::getDefaultPointOnBody1() const {
+ return getImpl().defaultPoint1;
+}
+const Vec3& Constraint::Ball::getDefaultPointOnBody2() const {
+ return getImpl().defaultPoint2;
+}
+
+Constraint::Ball& Constraint::Ball::setDefaultRadius(Real r) {
+ getImpl().invalidateTopologyCache();
+ updImpl().setDefaultRadius(r);
+ return *this;
+}
+
+Real Constraint::Ball::getDefaultRadius() const {
+ return getImpl().getDefaultRadius();
+}
+
+void Constraint::Ball::
+setPointOnBody1(State& state, const Vec3& point_B1) const {
+ getImpl().updBodyStations(state).first = point_B1;
+}
+
+void Constraint::Ball::
+setPointOnBody2(State& state, const Vec3& point_B2) const {
+ getImpl().updBodyStations(state).second = point_B2;
+}
+
+const Vec3& Constraint::Ball::
+getPointOnBody1(const State& state) const {
+ return getImpl().getBodyStations(state).first;
+}
+const Vec3& Constraint::Ball::
+getPointOnBody2(const State& state) const {
+ return getImpl().getBodyStations(state).second;
+}
+
+
+Vec3 Constraint::Ball::getPositionErrors(const State& s) const {
+ Vec3 perr;
+ getImpl().getPositionErrors(s, 3, &perr[0]);
+ return perr;
+}
+
+Vec3 Constraint::Ball::getVelocityErrors(const State& s) const {
+ Vec3 pverr;
+ getImpl().getVelocityErrors(s, 3, &pverr[0]);
+ return pverr;
+}
+
+Vec3 Constraint::Ball::getAccelerationErrors(const State& s) const {
+ Vec3 pvaerr;
+ getImpl().getAccelerationErrors(s, 3, &pvaerr[0]);
+ return pvaerr;
+}
+
+Vec3 Constraint::Ball::getMultipliers(const State& s) const {
+ Vec3 mult;
+ getImpl().getMultipliers(s, 3, &mult[0]);
+ return mult;
+}
+
+// The multipliers are the A-frame force vector as applied to body 2 at
+// the user-defined station point on body 2. We want to return the
+// force applied to body 1, expressed in body 1's frame. Note that this
+// is the force applied to body 1 at the point coincident with body 2's
+// station point -- if instead we returned the force at body 1's station
+// point there would also be a small moment since that point in general
+// differs from the contact point.
+Vec3 Constraint::Ball::
+getBallReactionForceOnBody1(const State& s) const {
+ const BallImpl& impl = getImpl();
+ Vec3 force2_A;
+ impl.getMultipliers(s, 3, &force2_A[0]);
+ const Rotation& R_AB1 = impl.getBodyRotationFromState(s, impl.B1);
+ return -(~R_AB1*force2_A);
+}
+
+Vec3 Constraint::Ball::
+getBallReactionForceOnBody2(const State& s) const{
+ const BallImpl& impl = getImpl();
+ Vec3 force2_A;
+ impl.getMultipliers(s, 3, &force2_A[0]);
+ const Rotation& R_AB2 = impl.getBodyRotationFromState(s, impl.B2);
+ return ~R_AB2*force2_A;
+}
+
+ // BallImpl
+
+// The default body stations may be overridden by setting instance variables
+// in the state. We allocate the state resources here.
+void Constraint::Ball::BallImpl::
+realizeTopologyVirtual(State& state) const {
+ stationsIx = getMyMatterSubsystemRep().
+ allocateDiscreteVariable(state, Stage::Instance,
+ new Value< std::pair<Vec3,Vec3> >
+ (std::make_pair(defaultPoint1,defaultPoint2)));
+}
+
+// Return the pair of constrained station points, with the first expressed
+// in the body 1 frame and the second in the body 2 frame. Note that although
+// these are used to define the position error, only the station on body 2
+// is used to generate constraint forces; the point of body 1 that is
+// coincident with the body 2 point receives the equal and opposite force.
+const std::pair<Vec3,Vec3>& Constraint::Ball::BallImpl::
+getBodyStations(const State& state) const {
+ return Value< std::pair<Vec3,Vec3> >::downcast
+ (getMyMatterSubsystemRep().getDiscreteVariable(state,stationsIx));
+}
+
+// Return a writable reference into the Instance-stage state variable
+// containing the pair of constrained station points, with the first expressed
+// in the body 1 frame and the second in the body 2 frame. Calling this
+// method invalidates the Instance stage and above in the given state.
+std::pair<Vec3,Vec3>& Constraint::Ball::BallImpl::
+updBodyStations(State& state) const {
+ return Value< std::pair<Vec3,Vec3> >::updDowncast
+ (getMyMatterSubsystemRep().updDiscreteVariable(state,stationsIx));
+}
+
+void Constraint::Ball::BallImpl::calcDecorativeGeometryAndAppendVirtual
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+{
+ const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
+
+ // We can't generate the ball until we know the radius, and we can't place
+ // the geometry on the body until we know the body1 and body2 point
+ // placements on the bodies, which might not be until Instance stage.
+ if ( stage == Stage::Instance
+ && getDefaultRadius() > 0
+ && matterRep.getShowDefaultGeometry())
+ {
+ const std::pair<Vec3,Vec3>& pts = getBodyStations(s);
+
+ const Transform X_B1(Rotation(), pts.first); // should be point from State
+ const Transform X_B2(Rotation(), pts.second);
+
+ // On the inboard body, draw a solid sphere and a wireframe one attached to it for
+ // easier visualization of its rotation. These are at about 90% of the radius.
+ geom.push_back(DecorativeSphere(Real(0.92)*getDefaultRadius())
+ .setColor(Gray)
+ .setRepresentation(DecorativeGeometry::DrawSurface)
+ .setOpacity(Real(0.5))
+ .setResolution(Real(0.75))
+ .setBodyId(getMobilizedBodyIndexOfConstrainedBody(B1))
+ .setTransform(X_B1));
+ geom.push_back(DecorativeSphere(Real(0.90)*getDefaultRadius())
+ .setColor(White)
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setResolution(Real(0.75))
+ .setLineThickness(3)
+ .setOpacity(Real(0.1))
+ .setBodyId(getMobilizedBodyIndexOfConstrainedBody(B1))
+ .setTransform(X_B1));
+
+ // Draw connector line back to body origin.
+ if (pts.first.norm() >= SignificantReal)
+ geom.push_back(DecorativeLine(Vec3(0), pts.first)
+ .setColor(Gray)
+ .setLineThickness(2)
+ .setBodyId(getMobilizedBodyIndexOfConstrainedBody(B1)));
+
+ // On the outboard body draw an orange mesh sphere at the ball radius.
+ geom.push_back(DecorativeSphere(getDefaultRadius())
+ .setColor(Orange)
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setOpacity(Real(0.5))
+ .setResolution(Real(0.5))
+ .setBodyId(getMobilizedBodyIndexOfConstrainedBody(B2))
+ .setTransform(X_B2));
+
+ // Draw connector line back to body origin.
+ if (pts.second.norm() >= SignificantReal)
+ geom.push_back(DecorativeLine(Vec3(0), pts.second)
+ .setColor(Gray)
+ .setLineThickness(2)
+ .setBodyId(getMobilizedBodyIndexOfConstrainedBody(B2)));
+ }
+}
+
+
+
+//==============================================================================
+// CONSTRAINT::CONSTANT ORIENTATION
+//==============================================================================
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Constraint::ConstantOrientation, Constraint::ConstantOrientationImpl, Constraint);
+
+Constraint::ConstantOrientation::ConstantOrientation
+ (MobilizedBody& baseBody, const Rotation& defaultFrameOnB,
+ MobilizedBody& followerBody, const Rotation& defaultFrameOnF)
+ : Constraint(new ConstantOrientationImpl())
+{
+ SimTK_ASSERT_ALWAYS(baseBody.isInSubsystem() && followerBody.isInSubsystem(),
+ "Constraint::ConstantOrientation(): both bodies must already be in a SimbodyMatterSubsystem.");
+ SimTK_ASSERT_ALWAYS(baseBody.isInSameSubsystem(followerBody),
+ "Constraint::ConstantOrientation(): both bodies to be connected must be in the same SimbodyMatterSubsystem.");
+
+ //rep = new ConstantOrientationRep(); rep->setMyHandle(*this);
+ baseBody.updMatterSubsystem().adoptConstraint(*this);
+
+ updImpl().B = updImpl().addConstrainedBody(baseBody);
+ updImpl().F = updImpl().addConstrainedBody(followerBody);
+ updImpl().defaultRB = defaultFrameOnB;
+ updImpl().defaultRF = defaultFrameOnF;
+}
+
+Constraint::ConstantOrientation& Constraint::ConstantOrientation::setDefaultBaseRotation(const Rotation& R) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultRB = R;
+ return *this;
+}
+
+Constraint::ConstantOrientation& Constraint::ConstantOrientation::setDefaultFollowerRotation(const Rotation& R) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultRF = R;
+ return *this;
+}
+
+
+MobilizedBodyIndex Constraint::ConstantOrientation::getBaseMobilizedBodyIndex() const {
+ return getImpl().getMobilizedBodyIndexOfConstrainedBody(getImpl().B);
+}
+MobilizedBodyIndex Constraint::ConstantOrientation::getFollowerMobilizedBodyIndex() const {
+ return getImpl().getMobilizedBodyIndexOfConstrainedBody(getImpl().F);
+}
+const Rotation& Constraint::ConstantOrientation::getDefaultBaseRotation() const {
+ return getImpl().defaultRB;
+}
+const Rotation& Constraint::ConstantOrientation::getDefaultFollowerRotation() const {
+ return getImpl().defaultRF;
+}
+
+Vec3 Constraint::ConstantOrientation::getPositionErrors(const State& s) const {
+ Vec3 perr;
+ getImpl().getPositionErrors(s, 3, &perr[0]);
+ return perr;
+}
+
+Vec3 Constraint::ConstantOrientation::getVelocityErrors(const State& s) const {
+ Vec3 pverr;
+ getImpl().getVelocityErrors(s, 3, &pverr[0]);
+ return pverr;
+}
+
+Vec3 Constraint::ConstantOrientation::getAccelerationErrors(const State& s) const {
+ Vec3 pvaerr;
+ getImpl().getAccelerationErrors(s, 3, &pvaerr[0]);
+ return pvaerr;
+}
+
+Vec3 Constraint::ConstantOrientation::getMultipliers(const State& s) const {
+ Vec3 mult;
+ getImpl().getMultipliers(s, 3, &mult[0]);
+ return mult;
+}
+
+
+ // ConstantOrientationImpl
+
+ //TODO: no visualization yet
+
+
+
+//==============================================================================
+// CONSTRAINT::WELD
+//==============================================================================
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Constraint::Weld, Constraint::WeldImpl, Constraint);
+
+Constraint::Weld::Weld(MobilizedBody& body1, MobilizedBody& body2)
+ : Constraint(new WeldImpl())
+{
+ SimTK_ASSERT_ALWAYS(body1.isInSubsystem() && body2.isInSubsystem(),
+ "Constraint::Weld(): both bodies must already be in a MatterSubsystem.");
+ SimTK_ASSERT_ALWAYS(body1.isInSameSubsystem(body2),
+ "Constraint::Weld(): both bodies to be connected must be in the same MatterSubsystem.");
+
+ //rep = new WeldRep(); rep->setMyHandle(*this);
+ body1.updMatterSubsystem().adoptConstraint(*this);
+
+ updImpl().B = updImpl().addConstrainedBody(body1);
+ updImpl().F = updImpl().addConstrainedBody(body2);
+}
+
+Constraint::Weld::Weld(MobilizedBody& body1, const Transform& frame1,
+ MobilizedBody& body2, const Transform& frame2)
+ : Constraint(new WeldImpl())
+{
+ SimTK_ASSERT_ALWAYS(body1.isInSubsystem() && body2.isInSubsystem(),
+ "Constraint::Weld(): both bodies must already be in a MatterSubsystem.");
+ SimTK_ASSERT_ALWAYS(body1.isInSameSubsystem(body2),
+ "Constraint::Weld(): both bodies to be connected must be in the same MatterSubsystem.");
+
+ //rep = new WeldRep(); rep->setMyHandle(*this);
+ body1.updMatterSubsystem().adoptConstraint(*this);
+
+ updImpl().B = updImpl().addConstrainedBody(body1);
+ updImpl().F = updImpl().addConstrainedBody(body2);
+
+ updImpl().defaultFrameB = frame1;
+ updImpl().defaultFrameF = frame2;
+}
+
+Constraint::Weld& Constraint::Weld::setDefaultFrameOnBody1(const Transform& f1) {
+ updImpl().defaultFrameB = f1;
+ return *this;
+}
+
+Constraint::Weld& Constraint::Weld::setDefaultFrameOnBody2(const Transform& f2) {
+ updImpl().defaultFrameF = f2;
+ return *this;
+}
+
+MobilizedBodyIndex Constraint::Weld::getBody1MobilizedBodyIndex() const {
+ return getImpl().getMobilizedBodyIndexOfConstrainedBody(getImpl().B);
+}
+MobilizedBodyIndex Constraint::Weld::getBody2MobilizedBodyIndex() const {
+ return getImpl().getMobilizedBodyIndexOfConstrainedBody(getImpl().F);
+}
+const Transform& Constraint::Weld::getDefaultFrameOnBody1() const {
+ return getImpl().defaultFrameB;
+}
+const Transform& Constraint::Weld::getDefaultFrameOnBody2() const {
+ return getImpl().defaultFrameF;
+}
+
+Vec6 Constraint::Weld::getPositionErrors(const State& s) const {
+ Vec6 perr;
+ getImpl().getPositionErrors(s, 6, &perr[0]);
+ return perr;
+}
+
+Vec6 Constraint::Weld::getVelocityErrors(const State& s) const {
+ Vec6 pverr;
+ getImpl().getVelocityErrors(s, 6, &pverr[0]);
+ return pverr;
+}
+
+Vec6 Constraint::Weld::getAccelerationErrors(const State& s) const {
+ Vec6 pvaerr;
+ getImpl().getAccelerationErrors(s, 6, &pvaerr[0]);
+ return pvaerr;
+}
+
+Vec6 Constraint::Weld::getMultipliers(const State& s) const {
+ Vec6 mult;
+ getImpl().getMultipliers(s, 6, &mult[0]);
+ return mult;
+}
+
+ // WeldImpl
+
+void Constraint::Weld::WeldImpl::calcDecorativeGeometryAndAppendVirtual
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+{
+ // We can't generate the frames until we know the axis lengths to use, and we can't place
+ // the geometry on the bodies until we know the body1 and body2 frame
+ // placements, which might not be until Instance stage.
+ if (stage == Stage::Instance && getAxisDisplayLength() != 0 && getMyMatterSubsystemRep().getShowDefaultGeometry()) {
+
+ geom.push_back(DecorativeFrame(getAxisDisplayLength())
+ .setColor(getFrameColor(0))
+ .setLineThickness(2)
+ .setBodyId(getMobilizedBodyIndexOfConstrainedBody(B))
+ .setTransform(defaultFrameB));
+
+ // Draw connector line back to body origin.
+ if (defaultFrameB.p().norm() >= SignificantReal)
+ geom.push_back(DecorativeLine(Vec3(0), defaultFrameB.p())
+ .setColor(getFrameColor(0))
+ .setLineThickness(2)
+ .setBodyId(getMobilizedBodyIndexOfConstrainedBody(B)));
+
+
+ geom.push_back(DecorativeFrame(Real(0.67)*getAxisDisplayLength())
+ .setColor(getFrameColor(1))
+ .setLineThickness(4)
+ .setBodyId(getMobilizedBodyIndexOfConstrainedBody(F))
+ .setTransform(defaultFrameF));
+
+ if (defaultFrameF.p().norm() >= SignificantReal)
+ geom.push_back(DecorativeLine(Vec3(0), defaultFrameF.p())
+ .setColor(getFrameColor(1))
+ .setLineThickness(4)
+ .setBodyId(getMobilizedBodyIndexOfConstrainedBody(F)));
+ }
+}
+
+
+
+//==============================================================================
+// CONSTRAINT::NO SLIP 1D
+//==============================================================================
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Constraint::NoSlip1D, Constraint::NoSlip1DImpl, Constraint);
+
+Constraint::NoSlip1D::NoSlip1D
+ (MobilizedBody& caseBody, const Vec3& P_C, const UnitVec3& n_C,
+ MobilizedBody& movingBody0, MobilizedBody& movingBody1)
+ : Constraint(new NoSlip1DImpl())
+{
+ SimTK_ASSERT_ALWAYS(caseBody.isInSubsystem() && movingBody0.isInSubsystem()&& movingBody1.isInSubsystem(),
+ "Constraint::NoSlip1D(): all three bodies must already be in a SimbodyMatterSubsystem.");
+ SimTK_ASSERT_ALWAYS(caseBody.isInSameSubsystem(movingBody0) && caseBody.isInSameSubsystem(movingBody1),
+ "Constraint::NoSlip1D(): all three bodies must be in the same SimbodyMatterSubsystem.");
+
+ //rep = new NoSlip1DRep(); rep->setMyHandle(*this);
+ caseBody.updMatterSubsystem().adoptConstraint(*this);
+
+ updImpl().caseBody = updImpl().addConstrainedBody(caseBody);
+ updImpl().movingBody0 = updImpl().addConstrainedBody(movingBody0);
+ updImpl().movingBody1 = updImpl().addConstrainedBody(movingBody1);
+ updImpl().defaultNoSlipDirection = n_C;
+ updImpl().defaultContactPoint = P_C;
+}
+
+Constraint::NoSlip1D& Constraint::NoSlip1D::setDefaultDirection(const UnitVec3& n) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultNoSlipDirection = n;
+ return *this;
+}
+
+Constraint::NoSlip1D& Constraint::NoSlip1D::setDefaultContactPoint(const Vec3& p) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultContactPoint = p;
+ return *this;
+}
+
+MobilizedBodyIndex Constraint::NoSlip1D::getCaseMobilizedBodyIndex() const {
+ return getImpl().getMobilizedBodyIndexOfConstrainedBody(getImpl().caseBody);
+}
+MobilizedBodyIndex Constraint::NoSlip1D::getMovingBodyMobilizedBodyIndex(int which) const {
+ assert(which==0 || which==1);
+ return getImpl().getMobilizedBodyIndexOfConstrainedBody(
+ which==0 ? getImpl().movingBody0 : getImpl().movingBody1);
+}
+const UnitVec3& Constraint::NoSlip1D::getDefaultDirection() const {
+ return getImpl().defaultNoSlipDirection;
+}
+const Vec3& Constraint::NoSlip1D::getDefaultContactPoint() const {
+ return getImpl().defaultContactPoint;
+}
+
+Constraint::NoSlip1D& Constraint::NoSlip1D::setDirectionDisplayLength(Real l) {
+ updImpl().setDirectionDisplayLength(l);
+ return *this;
+}
+Constraint::NoSlip1D& Constraint::NoSlip1D::setPointDisplayRadius(Real r) {
+ updImpl().setPointDisplayRadius(r);
+ return *this;
+}
+
+Real Constraint::NoSlip1D::getDirectionDisplayLength() const {
+ return getImpl().getDirectionDisplayLength();
+}
+
+Real Constraint::NoSlip1D::getPointDisplayRadius() const {
+ return getImpl().getPointDisplayRadius();
+}
+
+void Constraint::NoSlip1D::
+setContactPoint(State& state, const Vec3& point_C) const {
+ getImpl().updContactInfo(state).first = point_C;
+}
+
+void Constraint::NoSlip1D::
+setDirection(State& state, const UnitVec3& direction_C) const {
+ getImpl().updContactInfo(state).second = direction_C;
+}
+
+const Vec3& Constraint::NoSlip1D::
+getContactPoint(const State& state) const {
+ return getImpl().getContactInfo(state).first;
+}
+const UnitVec3& Constraint::NoSlip1D::
+getDirection(const State& state) const {
+ return getImpl().getContactInfo(state).second;
+}
+
+
+Real Constraint::NoSlip1D::getVelocityError(const State& s) const {
+ Real pverr;
+ getImpl().getVelocityErrors(s, 1, &pverr);
+ return pverr;
+}
+
+Real Constraint::NoSlip1D::getAccelerationError(const State& s) const {
+ Real pvaerr;
+ getImpl().getAccelerationErrors(s, 1, &pvaerr);
+ return pvaerr;
+}
+
+Real Constraint::NoSlip1D::getMultiplier(const State& s) const {
+ Real mult;
+ getImpl().getMultipliers(s, 1, &mult);
+ return mult;
+}
+
+Real Constraint::NoSlip1D::getForceAtContactPoint(const State& s) const {
+ return -getMultiplier(s); // applied forces have opposite sign from lambda
+}
+
+ // NoSlip1DImpl
+
+
+// The default contact point and no-slip direction may be overridden by setting
+// an instance variable in the state. We allocate the state resources here.
+void Constraint::NoSlip1D::NoSlip1DImpl::
+realizeTopologyVirtual(State& state) const {
+ contactInfoIx = getMyMatterSubsystemRep().
+ allocateDiscreteVariable(state, Stage::Instance,
+ new Value< std::pair<Vec3,UnitVec3> >
+ (std::make_pair(defaultContactPoint,defaultNoSlipDirection)));
+}
+
+// Return the pair of constrained station points, with the first expressed
+// in the body 1 frame and the second in the body 2 frame. Note that although
+// these are used to define the position error, only the station on body 2
+// is used to generate constraint forces; the point of body 1 that is
+// coincident with the body 2 point receives the equal and opposite force.
+const std::pair<Vec3,UnitVec3>& Constraint::NoSlip1D::NoSlip1DImpl::
+getContactInfo(const State& state) const {
+ return Value< std::pair<Vec3,UnitVec3> >::downcast
+ (getMyMatterSubsystemRep().getDiscreteVariable(state,contactInfoIx));
+}
+
+// Return a writable reference into the Instance-stage state variable
+// containing the pair of constrained station points, with the first expressed
+// in the body 1 frame and the second in the body 2 frame. Calling this
+// method invalidates the Instance stage and above in the given state.
+std::pair<Vec3,UnitVec3>& Constraint::NoSlip1D::NoSlip1DImpl::
+updContactInfo(State& state) const {
+ return Value< std::pair<Vec3,UnitVec3> >::updDowncast
+ (getMyMatterSubsystemRep().updDiscreteVariable(state,contactInfoIx));
+}
+
+void Constraint::NoSlip1D::NoSlip1DImpl::calcDecorativeGeometryAndAppendVirtual
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+{
+ // We can't generate the artwork until we know the direction and contact
+ // point location, which might not be until Instance stage.
+ if (stage == Stage::Instance && getMyMatterSubsystemRep().getShowDefaultGeometry()) {
+ const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
+
+ const std::pair<Vec3,UnitVec3>& info = getContactInfo(s);
+ const Vec3& P_C = info.first;
+ const UnitVec3& n_C = info.second;
+
+ // This makes x axis point along no-slip direction, origin at contact point
+ const Transform X_CP(Rotation(n_C,XAxis), P_C);
+
+ const MobilizedBodyIndex caseMBId = getMobilizedBodyIndexOfConstrainedBody(caseBody);
+
+ if (directionLength > 0) {
+ // On the case body, draw a gray line in the no-slip direction, starting at contact point.
+ geom.push_back(DecorativeLine(Vec3(0), Vec3(directionLength,0,0))
+ .setColor(Gray)
+ .setBodyId(caseMBId)
+ .setTransform(X_CP));
+ }
+ if (pointRadius > 0) {
+ // On the follower body draw an orange mesh sphere at the point radius.
+ geom.push_back(DecorativeSphere(pointRadius)
+ .setColor(Orange)
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setResolution(0.5)
+ .setBodyId(caseMBId)
+ .setTransform(X_CP));
+ }
+ }
+}
+
+
+
+//==============================================================================
+// CONSTRAINT::CONSTANT SPEED
+//==============================================================================
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS
+ (Constraint::ConstantSpeed, Constraint::ConstantSpeedImpl, Constraint);
+
+// This picks one of the mobilities from a multiple-mobility mobilizer.
+Constraint::ConstantSpeed::ConstantSpeed
+ (MobilizedBody& mobilizer, MobilizerUIndex whichU, Real defaultSpeed)
+ : Constraint(new ConstantSpeedImpl())
+{
+ SimTK_ASSERT_ALWAYS(mobilizer.isInSubsystem(),
+ "Constraint::ConstantSpeed(): the mobilizer must already be"
+ " in a SimbodyMatterSubsystem.");
+
+ mobilizer.updMatterSubsystem().adoptConstraint(*this);
+
+ updImpl().theMobilizer = updImpl().addConstrainedMobilizer(mobilizer);
+ updImpl().whichMobility = whichU;
+ updImpl().defaultSpeed = defaultSpeed;
+}
+
+// This is for mobilizers with only 1 mobility.
+Constraint::ConstantSpeed::ConstantSpeed
+ (MobilizedBody& mobilizer, Real defaultSpeed)
+: Constraint(new ConstantSpeedImpl())
+{
+ SimTK_ASSERT_ALWAYS(mobilizer.isInSubsystem(),
+ "Constraint::ConstantSpeed(): the mobilizer must already be"
+ " in a SimbodyMatterSubsystem.");
+
+ mobilizer.updMatterSubsystem().adoptConstraint(*this);
+
+ updImpl().theMobilizer = updImpl().addConstrainedMobilizer(mobilizer);
+ updImpl().whichMobility = MobilizerUIndex(0);
+ updImpl().defaultSpeed = defaultSpeed;
+}
+
+MobilizedBodyIndex Constraint::ConstantSpeed::getMobilizedBodyIndex() const {
+ return getImpl().getMobilizedBodyIndexOfConstrainedMobilizer
+ (getImpl().theMobilizer);
+}
+MobilizerUIndex Constraint::ConstantSpeed::getWhichU() const {
+ return getImpl().whichMobility;
+}
+
+Real Constraint::ConstantSpeed::getDefaultSpeed() const {
+ return getImpl().defaultSpeed;
+}
+
+Constraint::ConstantSpeed& Constraint::ConstantSpeed::
+setDefaultSpeed(Real u) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultSpeed = u;
+ return *this;
+}
+
+void Constraint::ConstantSpeed::
+setSpeed(State& state, Real speed) const {
+ getImpl().updSpeed(state) = speed;
+}
+
+Real Constraint::ConstantSpeed::
+getSpeed(const State& state) const {
+ return getImpl().getSpeed(state);
+}
+
+Real Constraint::ConstantSpeed::getVelocityError(const State& s) const {
+ Real pverr;
+ getImpl().getVelocityErrors(s, 1, &pverr);
+ return pverr;
+}
+
+Real Constraint::ConstantSpeed::getAccelerationError(const State& s) const {
+ Real pvaerr;
+ getImpl().getAccelerationErrors(s, 1, &pvaerr);
+ return pvaerr;
+}
+
+Real Constraint::ConstantSpeed::getMultiplier(const State& s) const {
+ Real mult;
+ getImpl().getMultipliers(s, 1, &mult);
+ return mult;
+}
+
+
+ // ConstantSpeedImpl
+
+// Allocate a state variable to hold the desired speed.
+void Constraint::ConstantSpeedImpl::
+realizeTopologyVirtual(State& state) const {
+ ConstantSpeedImpl* mThis = // mutable momentarily
+ const_cast<ConstantSpeedImpl*>(this);
+ mThis->speedIx = getMyMatterSubsystemRep().
+ allocateDiscreteVariable(state, Stage::Velocity,
+ new Value<Real>(defaultSpeed));
+}
+
+Real Constraint::ConstantSpeedImpl::
+getSpeed(const State& state) const {
+ const SimbodyMatterSubsystemRep& matter = getMyMatterSubsystemRep();
+ return Value<Real>::downcast(matter.getDiscreteVariable(state,speedIx));
+}
+
+Real& Constraint::ConstantSpeedImpl::
+updSpeed(State& state) const {
+ const SimbodyMatterSubsystemRep& matter = getMyMatterSubsystemRep();
+ return Value<Real>::updDowncast(matter.updDiscreteVariable(state,speedIx));
+}
+
+
+//==============================================================================
+// CONSTRAINT::CONSTANT ACCELERATION
+//==============================================================================
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS
+ (Constraint::ConstantAcceleration, Constraint::ConstantAccelerationImpl,
+ Constraint);
+
+// This picks one of the mobilities from a multiple-mobility mobilizer.
+Constraint::ConstantAcceleration::ConstantAcceleration
+ (MobilizedBody& mobilizer, MobilizerUIndex whichU, Real defaultAcceleration)
+: Constraint(new ConstantAccelerationImpl())
+{
+ SimTK_ASSERT_ALWAYS(mobilizer.isInSubsystem(),
+ "Constraint::ConstantAcceleration(): the mobilizer must already be"
+ " in a SimbodyMatterSubsystem.");
+
+ mobilizer.updMatterSubsystem().adoptConstraint(*this);
+
+ updImpl().theMobilizer = updImpl().addConstrainedMobilizer(mobilizer);
+ updImpl().whichMobility = whichU;
+ updImpl().defaultAcceleration = defaultAcceleration;
+}
+
+// This is for mobilizers with only 1 mobility.
+Constraint::ConstantAcceleration::ConstantAcceleration
+ (MobilizedBody& mobilizer, Real defaultAcceleration)
+: Constraint(new ConstantAccelerationImpl())
+{
+ SimTK_ASSERT_ALWAYS(mobilizer.isInSubsystem(),
+ "Constraint::ConstantAcceleration(): the mobilizer must already be"
+ " in a SimbodyMatterSubsystem.");
+
+ mobilizer.updMatterSubsystem().adoptConstraint(*this);
+
+ updImpl().theMobilizer = updImpl().addConstrainedMobilizer(mobilizer);
+ updImpl().whichMobility = MobilizerUIndex(0);
+ updImpl().defaultAcceleration = defaultAcceleration;
+}
+
+MobilizedBodyIndex Constraint::ConstantAcceleration::
+getMobilizedBodyIndex() const {
+ return getImpl().getMobilizedBodyIndexOfConstrainedMobilizer
+ (getImpl().theMobilizer);
+}
+MobilizerUIndex Constraint::ConstantAcceleration::getWhichU() const {
+ return getImpl().whichMobility;
+}
+Real Constraint::ConstantAcceleration::getDefaultAcceleration() const {
+ return getImpl().defaultAcceleration;
+}
+Constraint::ConstantAcceleration& Constraint::ConstantAcceleration::
+setDefaultAcceleration(Real udot) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultAcceleration = udot;
+ return *this;
+}
+
+void Constraint::ConstantAcceleration::
+setAcceleration(State& state, Real accel) const {
+ getImpl().updAcceleration(state) = accel;
+}
+
+Real Constraint::ConstantAcceleration::
+getAcceleration(const State& state) const {
+ return getImpl().getAcceleration(state);
+}
+
+Real Constraint::ConstantAcceleration::getAccelerationError(const State& s) const {
+ Real pvaerr;
+ getImpl().getAccelerationErrors(s, 1, &pvaerr);
+ return pvaerr;
+}
+
+Real Constraint::ConstantAcceleration::getMultiplier(const State& s) const {
+ Real mult;
+ getImpl().getMultipliers(s, 1, &mult);
+ return mult;
+}
+
+ // ConstantAccelerationImpl
+
+// Allocate a state variable to hold the desired acceleration.
+void Constraint::ConstantAccelerationImpl::
+realizeTopologyVirtual(State& state) const {
+ ConstantAccelerationImpl* mThis = // mutable momentarily
+ const_cast<ConstantAccelerationImpl*>(this);
+ mThis->accelIx = getMyMatterSubsystemRep().
+ allocateDiscreteVariable(state, Stage::Acceleration,
+ new Value<Real>(defaultAcceleration));
+}
+
+Real Constraint::ConstantAccelerationImpl::
+getAcceleration(const State& state) const {
+ const SimbodyMatterSubsystemRep& matter = getMyMatterSubsystemRep();
+ return Value<Real>::downcast(matter.getDiscreteVariable(state,accelIx));
+}
+
+Real& Constraint::ConstantAccelerationImpl::
+updAcceleration(State& state) const {
+ const SimbodyMatterSubsystemRep& matter = getMyMatterSubsystemRep();
+ return Value<Real>::updDowncast(matter.updDiscreteVariable(state,accelIx));
+}
+
+
+
+//==============================================================================
+// CONSTRAINT::CUSTOM
+//==============================================================================
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS
+ (Constraint::Custom, Constraint::CustomImpl, Constraint);
+
+// We are given an Implementation object which is already holding a CustomImpl
+// object for us. We'll first take away ownership of the CustomImpl, then
+// make the CustomImpl take over ownership of the Implementation object.
+Constraint::Custom::Custom(Constraint::Custom::Implementation* implementation)
+: Constraint(implementation
+ ? implementation->updImpl().removeOwnershipOfCustomImpl()
+ : 0)
+{
+ SimTK_ASSERT_ALWAYS(implementation,
+ "Constraint::Custom::Custom(): Implementation pointer was NULL.");
+
+ // Now store the Implementation pointer in our CustomImpl. The Implementation
+ // object retains its original pointer to the CustomImpl object so it can
+ // operate as a proxy for the CustomImpl. However the Custom handle now owns the
+ // CustomImpl and the CustomImpl owns the Implementation.
+ updImpl().takeOwnershipOfImplementation(implementation);
+
+ updImpl().updMyMatterSubsystemRep().adoptConstraint(*this);
+}
+
+const Constraint::Custom::Implementation&
+Constraint::Custom::getImplementation() const {
+ return getImpl().getImplementation();
+}
+
+Constraint::Custom::Implementation&
+Constraint::Custom::updImplementation() {
+ return updImpl().updImplementation();
+}
+
+ // Constraint::CustomImpl
+
+// The Implementation object should already contain a pointer to this CustomImpl object.
+void Constraint::CustomImpl::
+takeOwnershipOfImplementation(Custom::Implementation* userImpl) {
+ assert(!implementation); // you can only do this once!
+ assert(userImpl);
+ const Custom::ImplementationImpl& impImpl = userImpl->getImpl();
+ assert(&impImpl.getCustomImpl() == this && !impImpl.isOwnerOfCustomImpl());
+ implementation = userImpl;
+}
+
+
+
+//==============================================================================
+// CONSTRAINT::CUSTOM::IMPLEMENTATION
+//==============================================================================
+
+// Default constructor allocates a CustomImpl object and saves it in the
+// ImplementationImpl object. When this gets passed to a Custom handle we'll
+// turn over ownership of the CustomImpl object to the Custom handle.
+Constraint::Custom::Implementation::Implementation
+ (SimbodyMatterSubsystem& matter)
+: PIMPLHandle<Implementation,ImplementationImpl>
+ (new ImplementationImpl(new CustomImpl()))
+{
+ // We don't know the ConstraintIndex yet since this hasn't been adopted
+ // by the MatterSubsystem.
+ updImpl().updCustomImpl().setMyMatterSubsystem(matter, ConstraintIndex());
+}
+
+Constraint::Custom::Implementation::Implementation
+ (SimbodyMatterSubsystem& matter, int mp, int mv, int ma)
+: PIMPLHandle<Implementation,ImplementationImpl>
+ (new ImplementationImpl(new CustomImpl(mp,mv,ma)))
+{
+ // We don't know the ConstraintIndex yet since this hasn't been adopted
+ // by the MatterSubsystem.
+ updImpl().updCustomImpl().setMyMatterSubsystem(matter, ConstraintIndex());
+}
+
+const SimbodyMatterSubsystem&
+Constraint::Custom::Implementation::getMatterSubsystem() const {
+ return getImpl().getCustomImpl().getMyMatterSubsystem();
+}
+
+void Constraint::Custom::Implementation::invalidateTopologyCache() const {
+ getImpl().getCustomImpl().invalidateTopologyCache();
+}
+
+Constraint::Custom::Implementation& Constraint::Custom::Implementation::
+setDefaultNumConstraintEquations(int mp, int mv, int ma) {
+ updImpl().updCustomImpl().setDefaultNumConstraintEquations(mp,mv,ma);
+ return *this;
+}
+
+Constraint::Custom::Implementation& Constraint::Custom::Implementation::
+setDisabledByDefault(bool shouldBeDisabled) {
+ updImpl().updCustomImpl().setDisabledByDefault(shouldBeDisabled);
+ return *this;
+}
+
+ConstrainedBodyIndex Constraint::Custom::Implementation::
+addConstrainedBody(const MobilizedBody& mb) {
+ return updImpl().updCustomImpl().addConstrainedBody(mb);
+}
+ConstrainedMobilizerIndex Constraint::Custom::Implementation::
+addConstrainedMobilizer(const MobilizedBody& mb) {
+ return updImpl().updCustomImpl().addConstrainedMobilizer(mb);
+}
+
+MobilizedBodyIndex Constraint::Custom::Implementation::
+getMobilizedBodyIndexOfConstrainedBody(ConstrainedBodyIndex c) const {
+ return getImpl().getCustomImpl().getMobilizedBodyIndexOfConstrainedBody(c);
+}
+MobilizedBodyIndex Constraint::Custom::Implementation::
+getMobilizedBodyIndexOfConstrainedMobilizer(ConstrainedMobilizerIndex c) const {
+ return getImpl().getCustomImpl().getMobilizedBodyIndexOfConstrainedMobilizer(c);
+}
+
+Real Constraint::Custom::Implementation::
+getOneQ(const State& state,
+ const Array_<Real,ConstrainedQIndex>& constrainedQ,
+ ConstrainedMobilizerIndex mobilizer,
+ MobilizerQIndex whichQ) const
+{ return getImpl().getCustomImpl()
+ .getOneQ(state,constrainedQ,mobilizer,whichQ); }
+
+Real Constraint::Custom::Implementation::
+getOneQDot(const State& state,
+ const Array_<Real,ConstrainedQIndex>& constrainedQDot,
+ ConstrainedMobilizerIndex mobilizer,
+ MobilizerQIndex whichQ) const
+{ return getImpl().getCustomImpl()
+ .getOneQDot(state,constrainedQDot,mobilizer,whichQ); }
+
+Real Constraint::Custom::Implementation::
+getOneQDotDot(const State& state,
+ const Array_<Real,ConstrainedQIndex>& constrainedQDotDot,
+ ConstrainedMobilizerIndex mobilizer,
+ MobilizerQIndex whichQ) const
+{ return getImpl().getCustomImpl()
+ .getOneQDotDot(state,constrainedQDotDot,mobilizer,whichQ); }
+
+
+Real Constraint::Custom::Implementation::
+getOneU(const State& state,
+ const Array_<Real,ConstrainedUIndex>& constrainedU,
+ ConstrainedMobilizerIndex mobilizer,
+ MobilizerUIndex whichU) const
+{ return getImpl().getCustomImpl()
+ .getOneU(state,constrainedU,mobilizer,whichU); }
+
+Real Constraint::Custom::Implementation::
+getOneUDot(const State& state,
+ const Array_<Real,ConstrainedUIndex>& constrainedUDot,
+ ConstrainedMobilizerIndex mobilizer,
+ MobilizerUIndex whichU) const
+{ return getImpl().getCustomImpl()
+ .getOneUDot(state,constrainedUDot,mobilizer,whichU); }
+
+Real Constraint::Custom::Implementation::
+getOneQFromState(const State& s, ConstrainedMobilizerIndex cmx, MobilizerQIndex mqx) const {
+ return getImpl().getCustomImpl().getOneQFromState(s,cmx,mqx);
+}
+
+Real Constraint::Custom::Implementation::
+getOneUFromState(const State& s, ConstrainedMobilizerIndex cmx, MobilizerUIndex mux) const {
+ return getImpl().getCustomImpl().getOneUFromState(s,cmx,mux);
+}
+
+
+Real Constraint::Custom::Implementation::
+getOneQDotFromState(const State& s,
+ ConstrainedMobilizerIndex cmx,
+ MobilizerQIndex mqx) const {
+ return getImpl().getCustomImpl().getOneQDotFromState(s,cmx,mqx);
+}
+
+
+// Apply a generalized (mobility) force to a particular mobility of the given constrained body B,
+// adding it in to the appropriate slot of the mobilityForces vector.
+void Constraint::Custom::Implementation::
+addInOneMobilityForce
+ (const State& s, ConstrainedMobilizerIndex M, MobilizerUIndex whichU,
+ Real fu, Array_<Real,ConstrainedUIndex>& mobilityForces) const
+{
+ getImpl().getCustomImpl().addInOneMobilityForce(s,M,whichU,fu,mobilityForces);
+}
+
+void Constraint::Custom::Implementation::
+addInOneQForce
+ (const State& state,
+ ConstrainedMobilizerIndex mobilizer,
+ MobilizerQIndex whichQ,
+ Real fq,
+ Array_<Real,ConstrainedQIndex>& qForces) const
+{
+ getImpl().getCustomImpl().addInOneQForce(state,mobilizer,whichQ,fq,qForces);
+}
+
+
+const Transform& Constraint::Custom::Implementation::
+getBodyTransform
+ (const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
+ ConstrainedBodyIndex bodyB) const
+{ return getImpl().getCustomImpl().getBodyTransform(allX_AB,bodyB); }
+
+const SpatialVec& Constraint::Custom::Implementation::
+getBodyVelocity
+ (const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
+ ConstrainedBodyIndex bodyB) const
+{ return getImpl().getCustomImpl().getBodyVelocity(allV_AB,bodyB); }
+
+const SpatialVec& Constraint::Custom::Implementation::
+getBodyAcceleration
+ (const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
+ ConstrainedBodyIndex bodyB) const
+{ return getImpl().getCustomImpl().getBodyAcceleration(allA_AB,bodyB); }
+
+const Transform& Constraint::Custom::Implementation::
+getBodyTransformFromState(const State& s, ConstrainedBodyIndex B) const
+{
+ return getImpl().getCustomImpl().getBodyTransformFromState(s,B);
+}
+
+const SpatialVec& Constraint::Custom::Implementation::
+getBodyVelocityFromState(const State& s, ConstrainedBodyIndex B) const
+{
+ return getImpl().getCustomImpl().getBodyVelocityFromState(s,B);
+}
+
+void Constraint::Custom::Implementation::
+addInStationForce
+ (const State& s, ConstrainedBodyIndex B, const Vec3& p_B,
+ const Vec3& forceInA,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA) const
+{
+ getImpl().getCustomImpl().addInStationForce(s,B,p_B,forceInA,bodyForcesInA);
+}
+
+void Constraint::Custom::Implementation::
+addInBodyTorque
+ (const State& s, ConstrainedBodyIndex B,
+ const Vec3& torqueInA,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA) const
+{
+ getImpl().getCustomImpl().addInBodyTorque(s,B,torqueInA,bodyForcesInA);
+}
+
+void Constraint::Custom::Implementation::
+getMultipliers(const State& s,
+ Array_<Real>& multipliers) const
+{
+ int mp, mv, ma;
+ const Constraint::CustomImpl& cimpl = getImpl().getCustomImpl();
+ cimpl.getNumConstraintEquationsInUse(s,mp,mv,ma);
+ multipliers.resize(mp+mv+ma);
+ cimpl.getMultipliers(s, multipliers.size(), &multipliers[0]);
+}
+
+
+
+// Default implementations for ConstraintImpl virtuals throw "unimplemented"
+// exceptions. These shouldn't be called unless the concrete constraint has
+// given a non-zero value for mp, mv, and/or ma which is a promise to
+// implement the associated methods.
+
+ // These must be defined if there are any position (holonomic) constraints
+ // defined.
+
+void Constraint::Custom::Implementation::
+calcPositionErrors
+ (const State& state,
+ const Array_<Transform,ConstrainedBodyIndex>& X_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr) const
+{
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "Constraint::Custom::Implementation", "calcPositionErrors");
+}
+
+void Constraint::Custom::Implementation::
+calcPositionDotErrors
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr) const
+{
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "Constraint::Custom::Implementation", "realizePositionDotErrors");
+}
+
+void Constraint::Custom::Implementation::
+calcPositionDotDotErrors
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr) const
+{
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "Constraint::Custom::Implementation", "calcPositionDotDotErrors");
+}
+
+void Constraint::Custom::Implementation::
+addInPositionConstraintForces
+ (const State& state,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real,ConstrainedQIndex>& qForces) const
+{
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "Constraint::Custom::Implementation", "addInPositionConstraintForces");
+}
+
+ // These must be defined if there are any velocity (nonholonomic)
+ // constraints defined.
+
+void Constraint::Custom::Implementation::
+calcVelocityErrors
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedU,
+ Array_<Real>& verr) const
+{
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "Constraint::Custom::Implementation", "calcVelocityErrors");
+}
+
+
+void Constraint::Custom::Implementation::
+calcVelocityDotErrors
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedUDot,
+ Array_<Real>& vaerr) const
+{
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "Constraint::Custom::Implementation", "calcVelocityDotErrors");
+}
+
+
+void Constraint::Custom::Implementation::
+addInVelocityConstraintForces
+ (const State& state,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real,ConstrainedUIndex>& mobilityForces) const
+{
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "Constraint::Custom::Implementation", "addInVelocityConstraintForces");
+}
+
+
+
+ // These must be defined if there are any acceleration-only constraints
+ // defined.
+
+void Constraint::Custom::Implementation::
+calcAccelerationErrors
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedUDot,
+ Array_<Real>& aerr) const
+{
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "Constraint::Custom::Implementation", "calcAccelerationErrors");
+}
+
+void Constraint::Custom::Implementation::
+addInAccelerationConstraintForces
+ (const State& state,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real,ConstrainedUIndex>& mobilityForces) const
+{
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "Constraint::Custom::Implementation", "addInAccelerationConstraintForces");
+}
+
+
+
+//==============================================================================
+// CONSTRAINT::COORDINATE COUPLER
+//==============================================================================
+Constraint::CoordinateCoupler::CoordinateCoupler
+ (SimbodyMatterSubsystem& matter,
+ const Function* function,
+ const Array_<MobilizedBodyIndex>& coordBody,
+ const Array_<MobilizerQIndex>& coordIndex)
+: Custom(new CoordinateCouplerImpl(matter, function, coordBody, coordIndex))
+{}
+
+Constraint::CoordinateCouplerImpl::CoordinateCouplerImpl
+ (SimbodyMatterSubsystem& matter,
+ const Function* function,
+ const Array_<MobilizedBodyIndex>& coordMobod,
+ const Array_<MobilizerQIndex>& coordQIndex)
+: Implementation(matter, 1, 0, 0), function(function),
+ coordBodies(coordMobod.size()), coordIndices(coordQIndex),
+ temp(coordBodies.size()), referenceCount(new int[1])
+{
+ assert(coordBodies.size() == coordIndices.size());
+ assert(coordIndices.size() == function->getArgumentSize());
+ assert(function->getMaxDerivativeOrder() >= 2);
+ referenceCount[0] = 1;
+ for (int i = 0; i < (int)coordBodies.size(); ++i) {
+ const MobilizedBody& mobod = matter.getMobilizedBody(coordMobod[i]);
+ coordBodies[i] = addConstrainedMobilizer(mobod);
+ }
+}
+
+void Constraint::CoordinateCouplerImpl::
+calcPositionErrors
+ (const State& s,
+ const Array_<Transform,ConstrainedBodyIndex>& X_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr) const
+{
+ for (int i = 0; i < temp.size(); ++i)
+ temp[i] = getOneQ(s, constrainedQ, coordBodies[i], coordIndices[i]);
+ perr[0] = function->calcValue(temp);
+}
+
+void Constraint::CoordinateCouplerImpl::
+calcPositionDotErrors
+ (const State& s,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr) const
+{
+ pverr[0] = 0;
+ for (int i = 0; i < temp.size(); ++i)
+ temp[i] = getOneQFromState(s, coordBodies[i], coordIndices[i]);
+ Array_<int> components(1);
+ for (int i = 0; i < temp.size(); ++i) {
+ components[0] = i;
+ pverr[0] += function->calcDerivative(components, temp)
+ * getOneQDot(s, constrainedQDot,
+ coordBodies[i], coordIndices[i]);
+ }
+}
+
+void Constraint::CoordinateCouplerImpl::
+calcPositionDotDotErrors
+ (const State& s,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr) const
+{
+ paerr[0] = 0.0;
+ for (int i = 0; i < temp.size(); ++i)
+ temp[i] = getOneQFromState(s, coordBodies[i], coordIndices[i]);
+
+ // TODO this could be made faster by using symmetry.
+ Array_<int> components(2);
+ for (int i = 0; i < temp.size(); ++i) {
+ components[0] = i;
+ Real qdoti = getOneQDotFromState(s, coordBodies[i], coordIndices[i]);
+ for (int j = 0; j < temp.size(); ++j) {
+ components[1] = j;
+ Real qdotj = getOneQDotFromState(s, coordBodies[j], coordIndices[j]);
+ paerr[0] += function->calcDerivative(components, temp)
+ * qdoti * qdotj;
+ }
+ }
+
+ Array_<int> component(1);
+ for (int i = 0; i < temp.size(); ++i) {
+ component[0] = i;
+ paerr[0] += function->calcDerivative(component, temp)
+ * getOneQDotDot(s, constrainedQDotDot,
+ coordBodies[i], coordIndices[i]);
+ }
+}
+
+void Constraint::CoordinateCouplerImpl::
+addInPositionConstraintForces
+ (const State& s,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForces,
+ Array_<Real,ConstrainedQIndex>& qForces) const
+{
+ assert(multipliers.size() == 1);
+ assert(bodyForces.size() == 0);
+
+ const Real lambda = multipliers[0];
+
+ for (int i = 0; i < temp.size(); ++i)
+ temp[i] = getOneQFromState(s, coordBodies[i], coordIndices[i]);
+
+ Array_<int> components(1);
+ for (int i = 0; i < temp.size(); ++i) {
+ components[0] = i;
+ const Real fq = lambda * function->calcDerivative(components, temp);
+ addInOneQForce(s, coordBodies[i], coordIndices[i], fq, qForces);
+ }
+}
+
+
+
+//==============================================================================
+// CONSTRAINT::SPEED COUPLER
+//==============================================================================
+Constraint::SpeedCoupler::SpeedCoupler
+ (SimbodyMatterSubsystem& matter,
+ const Function* function,
+ const Array_<MobilizedBodyIndex>& speedBody,
+ const Array_<MobilizerUIndex>& speedIndex)
+: Custom(new SpeedCouplerImpl(matter, function, speedBody, speedIndex,
+ Array_<MobilizedBodyIndex>(),
+ Array_<MobilizerQIndex>())) {}
+
+Constraint::SpeedCoupler::SpeedCoupler
+ (SimbodyMatterSubsystem& matter,
+ const Function* function,
+ const Array_<MobilizedBodyIndex>& speedBody,
+ const Array_<MobilizerUIndex>& speedIndex,
+ const Array_<MobilizedBodyIndex>& coordBody,
+ const Array_<MobilizerQIndex>& coordIndex)
+: Custom(new SpeedCouplerImpl(matter, function, speedBody, speedIndex,
+ coordBody, coordIndex)) {}
+
+Constraint::SpeedCouplerImpl::SpeedCouplerImpl
+ (SimbodyMatterSubsystem& matter,
+ const Function* function,
+ const Array_<MobilizedBodyIndex>& speedBody,
+ const Array_<MobilizerUIndex>& speedIndex,
+ const Array_<MobilizedBodyIndex>& coordBody,
+ const Array_<MobilizerQIndex>& coordIndex)
+: Implementation(matter, 0, 1, 0), function(function),
+ speedBodies(speedBody.size()), speedIndices(speedIndex),
+ coordBodies(coordBody), coordIndices(coordIndex),
+ temp(speedBody.size()+coordBody.size()), referenceCount(new int[1])
+{
+ assert(speedBodies.size() == speedIndices.size());
+ assert(coordBodies.size() == coordIndices.size());
+ assert(temp.size() == function->getArgumentSize());
+ assert(function->getMaxDerivativeOrder() >= 2);
+
+ referenceCount[0] = 1;
+ for (int i = 0; i < (int)speedBodies.size(); ++i) {
+ const MobilizedBody& mobod = matter.getMobilizedBody(speedBody[i]);
+ speedBodies[i] = addConstrainedMobilizer(mobod);
+ }
+}
+
+// Constraint is f(q,u)=0, i.e. verr=f(q,u).
+void Constraint::SpeedCouplerImpl::
+calcVelocityErrors
+ (const State& s,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedU,
+ Array_<Real>& verr) const
+{
+ for (int i = 0; i < (int) speedBodies.size(); ++i)
+ temp[i] = getOneU(s, constrainedU, speedBodies[i], speedIndices[i]);
+ for (int i = 0; i < (int) coordBodies.size(); ++i)
+ temp[i+speedBodies.size()] =
+ getMatterSubsystem().getMobilizedBody(coordBodies[i])
+ .getOneQ(s, coordIndices[i]);
+ verr[0] = function->calcValue(temp);
+}
+
+// d verr / dt = (df/du)*udot + (df/dq)*qdot.
+void Constraint::SpeedCouplerImpl::
+calcVelocityDotErrors
+ (const State& s,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedUDot,
+ Array_<Real>& vaerr) const
+{
+ for (int i = 0; i < (int)speedBodies.size(); ++i)
+ temp[i] = getOneUFromState(s, speedBodies[i], speedIndices[i]);
+ for (int i = 0; i < (int)coordBodies.size(); ++i) {
+ const Real q = getMatterSubsystem().getMobilizedBody(coordBodies[i])
+ .getOneQ(s, coordIndices[i]);
+ temp[i+speedBodies.size()] = q;
+ }
+
+ Array_<int> components(1);
+ vaerr[0] = 0;
+ // Differentiate the u-dependent terms here.
+ for (int i = 0; i < (int)speedBodies.size(); ++i) {
+ components[0] = i;
+ vaerr[0] += function->calcDerivative(components, temp)
+ * getOneUDot(s, constrainedUDot,
+ speedBodies[i], speedIndices[i]);
+ }
+ // Differentiate the q-dependent terms here.
+ for (int i = 0; i < (int)coordBodies.size(); ++i) {
+ components[0] = i + speedBodies.size();
+ const Real qdot = getMatterSubsystem().getMobilizedBody(coordBodies[i])
+ .getOneQDot(s, coordIndices[i]);
+ vaerr[0] += function->calcDerivative(components, temp) * qdot;
+ }
+}
+
+// Force is (df/du)*lambda.
+void Constraint::SpeedCouplerImpl::
+addInVelocityConstraintForces
+ (const State& s,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForces,
+ Array_<Real,ConstrainedUIndex>& mobilityForces) const
+{
+ assert(multipliers.size() == 1);
+ const Real lambda = multipliers[0];
+
+ for (int i = 0; i < (int) speedBodies.size(); ++i)
+ temp[i] = getOneUFromState(s, speedBodies[i], speedIndices[i]);
+ for (int i = 0; i < (int) coordBodies.size(); ++i)
+ temp[i+speedBodies.size()] =
+ getMatterSubsystem().getMobilizedBody(coordBodies[i])
+ .getOneQ(s, coordIndices[i]);
+
+ Array_<int> components(1);
+ // Only the u-dependent terms generate forces.
+ for (int i = 0; i < (int) speedBodies.size(); ++i) {
+ components[0] = i;
+ const Real force = function->calcDerivative(components, temp)
+ * lambda;
+ addInOneMobilityForce(s, speedBodies[i], speedIndices[i],
+ force, mobilityForces);
+ }
+}
+
+
+
+//==============================================================================
+// CONSTRAINT::PRESCRIBED MOTION
+//==============================================================================
+Constraint::PrescribedMotion::PrescribedMotion
+ (SimbodyMatterSubsystem& matter,
+ const Function* function,
+ MobilizedBodyIndex coordBody,
+ MobilizerQIndex coordIndex)
+: Custom(new PrescribedMotionImpl(matter, function, coordBody, coordIndex)) {}
+
+Constraint::PrescribedMotionImpl::PrescribedMotionImpl
+ (SimbodyMatterSubsystem& matter,
+ const Function* function,
+ MobilizedBodyIndex coordBody,
+ MobilizerQIndex coordIndex)
+: Implementation(matter, 1, 0, 0), function(function),
+ coordIndex(coordIndex), temp(1), referenceCount(new int[1])
+{
+ assert(function->getArgumentSize() == 1);
+ assert(function->getMaxDerivativeOrder() >= 2);
+
+ referenceCount[0] = 1;
+ const MobilizedBody& mobod = matter.getMobilizedBody(coordBody);
+ this->coordBody = addConstrainedMobilizer(mobod);
+}
+
+void Constraint::PrescribedMotionImpl::
+calcPositionErrors
+ (const State& s,
+ const Array_<Transform,ConstrainedBodyIndex>& X_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr) const
+{
+ temp[0] = s.getTime();
+ perr[0] = getOneQ(s, constrainedQ, coordBody, coordIndex)
+ - function->calcValue(temp);
+}
+
+void Constraint::PrescribedMotionImpl::
+calcPositionDotErrors
+ (const State& s,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr) const
+{
+ temp[0] = s.getTime();
+ Array_<int> components(1, 0); // i.e., components={0}
+ pverr[0] = getOneQDot(s, constrainedQDot, coordBody, coordIndex)
+ - function->calcDerivative(components, temp);
+}
+
+void Constraint::PrescribedMotionImpl::
+calcPositionDotDotErrors
+ (const State& s,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr) const
+{
+ temp[0] = s.getTime();
+ Array_<int> components(2, 0); // i.e., components={0,0}
+ paerr[0] = getOneQDotDot(s, constrainedQDotDot, coordBody, coordIndex)
+ - function->calcDerivative(components, temp);
+}
+
+void Constraint::PrescribedMotionImpl::
+addInPositionConstraintForces
+ (const State& s,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForces,
+ Array_<Real,ConstrainedQIndex>& qForces) const
+{
+ const Real fq = multipliers[0];
+ addInOneQForce(s, coordBody, coordIndex, fq, qForces);
+}
+
+
+
+//==============================================================================
+// CONSTRAINT IMPL
+//==============================================================================
+
+
+// =============================================================================
+// REALIZE TOPOLOGY
+// =============================================================================
+void ConstraintImpl::realizeTopology(State& s) const {
+ // Calculate the relevant Subtree. There might not be any Constrained
+ // Bodies here but we want to make sure we have a properly initialized
+ // empty Subtree in that case.
+ mySubtree.clear();
+ mySubtree.setSimbodyMatterSubsystem(getMyMatterSubsystem());
+ for (ConstrainedBodyIndex b(0); b < myConstrainedBodies.size(); ++b)
+ mySubtree.addTerminalBody(myConstrainedBodies[b]);
+ mySubtree.realizeTopology();
+
+ myAncestorBodyIsNotGround = myConstrainedBodies.size()
+ && mySubtree.getAncestorMobilizedBodyIndex() != GroundIndex;
+
+ // If the Ancestor isn't Ground, reserve slots in the State cache
+ // ancestor constrained body pools for each constrained body here
+ // except the Ancestor (which may or may not be a constrained body).
+ if (myAncestorBodyIsNotGround) {
+ myPoolIndex.resize(myConstrainedBodies.size());
+ for (ConstrainedBodyIndex b(0); b < myConstrainedBodies.size(); ++b) {
+ if (myConstrainedBodies[b] == mySubtree.getAncestorMobilizedBodyIndex())
+ myPoolIndex[b].invalidate();
+ else myPoolIndex[b] =
+ getMyMatterSubsystemRep().allocateNextAncestorConstrainedBodyPoolSlot();
+ }
+ } else
+ myPoolIndex.clear(); // ancestor is Ground; no need for pool entries
+
+ realizeTopologyVirtual(s); // delegate to concrete constraint
+}
+
+
+
+// =============================================================================
+// REALIZE MODEL
+// =============================================================================
+void ConstraintImpl::realizeModel(State& s) const
+{
+ SimTK_ASSERT(subsystemTopologyHasBeenRealized(),
+ "ConstraintImpl::realizeModel() can't be called until after realizeToplogy().");
+
+ realizeModelVirtual(s); // delegate to concrete constraint
+}
+
+
+
+// =============================================================================
+// REALIZE INSTANCE
+// =============================================================================
+// There are several tasks here that can be performed now that we have values
+// for the Instance stage state variables (including the enable/disable flag
+// for Constraints):
+// (1) Count up the number of holonomic, nonholonomic, and acceleration-only
+// constraint equations to be contributed by each Constraint, and assign
+// corresponding slots in constraint-equation ordered arrays, such as the
+// State's constraint error arrays.
+// (2) Count up the number of constrained bodies, mobilizers, and corresoponding
+// constrained mobilities to be affected by each Constraint, and assign
+// corresponding slots for use in "pools" that are indexed by these.
+// (3) Above we assigned q's and u's to each mobilizer and stored the results in
+// the Model cache, now we can determine which of those q's and u's are
+// involved in each constraint. We need to collect up both the set of
+// directly-constrained q's and u's resulting from ConstrainedMobilizers,
+// and indirectly-constrained ones arising from their effects on
+// ConstrainedBodies. Together we call those "participating q's" and
+// "participating u's" (or "participating mobilities").
+// The results of these computations goes in the Instance cache.
+
+void ConstraintImpl::realizeInstance(const State& s) const {
+ const SimbodyMatterSubsystemRep& matter = getMyMatterSubsystemRep();
+ const SBInstanceVars& instanceVars = matter.getInstanceVars(s);
+ SBInstanceCache& ic = matter.updInstanceCache(s);
+
+ const int ncb = getNumConstrainedBodies();
+ const int ncm = getNumConstrainedMobilizers();
+
+ // Note that there is a "per constraint info" object for every declared
+ // constraint, whether enabled or not.
+ SBInstancePerConstraintInfo& cInfo =
+ ic.updConstraintInstanceInfo(myConstraintIndex);
+
+ cInfo.clear();
+ cInfo.allocateConstrainedMobilizerInstanceInfo(ncm);
+
+ // We're in the process of counting up constraint equations in the
+ // totalN...Constraints variables; on entry they are set to the number
+ // we've seen so far and we'll increment them here to add in the
+ // contributions from this Constraint.
+
+ if (isDisabled(s)) {
+ // Just to be neat, we'll assign zero-width segments where our slots
+ // would have gone if this constraint were enabled.
+ cInfo.holoErrSegment =
+ Segment(0,ic.totalNHolonomicConstraintEquationsInUse);
+ cInfo.nonholoErrSegment =
+ Segment(0,ic.totalNNonholonomicConstraintEquationsInUse);
+ cInfo.accOnlyErrSegment =
+ Segment(0,ic.totalNAccelerationOnlyConstraintEquationsInUse);
+
+ cInfo.consBodySegment = Segment(0, ic.totalNConstrainedBodiesInUse);
+ cInfo.consMobilizerSegment = Segment(0, ic.totalNConstrainedMobilizersInUse);
+ cInfo.consQSegment = Segment(0, ic.totalNConstrainedQInUse);
+ cInfo.consUSegment = Segment(0, ic.totalNConstrainedUInUse);
+ return;
+ }
+
+ // This constraint is enabled.
+
+ // These count just the primary contraint equations, not their time
+ // derivatives.
+ int mHolo, mNonholo, mAccOnly;
+ calcNumConstraintEquationsInUse(s, mHolo, mNonholo, mAccOnly);
+
+ // Must allocate space for primary constraint equations & time derivatives.
+ // length offset
+ cInfo.holoErrSegment = Segment(mHolo, ic.totalNHolonomicConstraintEquationsInUse);
+ cInfo.nonholoErrSegment = Segment(mNonholo, ic.totalNNonholonomicConstraintEquationsInUse);
+ cInfo.accOnlyErrSegment = Segment(mAccOnly, ic.totalNAccelerationOnlyConstraintEquationsInUse);
+
+ ic.totalNHolonomicConstraintEquationsInUse += mHolo;
+ ic.totalNNonholonomicConstraintEquationsInUse += mNonholo;
+ ic.totalNAccelerationOnlyConstraintEquationsInUse += mAccOnly;
+
+ cInfo.consBodySegment = Segment(ncb, ic.totalNConstrainedBodiesInUse);
+ cInfo.consMobilizerSegment = Segment(ncm, ic.totalNConstrainedMobilizersInUse);
+ ic.totalNConstrainedBodiesInUse += ncb;
+ ic.totalNConstrainedMobilizersInUse += ncm;
+
+ // At this point we can find out how many q's and u's are associated with
+ // each of the constrained mobilizers. We'll create packed arrays of q's and
+ // u's ordered corresponding to the ConstrainedMobilizerIndices. We'll
+ // record these in the InstanceCache, by storing the ConstrainedQIndex and
+ // ConstrainedUIndex of the lowest-numbered coordinate and mobility
+ // associated with each of the ConstrainedMobilizers, along with the number
+ // of q's and u's.
+
+ for (ConstrainedMobilizerIndex cmx(0); cmx < ncm; ++cmx) {
+ SBInstancePerConstrainedMobilizerInfo& mInfo =
+ cInfo.updConstrainedMobilizerInstanceInfo(cmx);
+
+ const MobilizedBodyIndex mbx =
+ getMobilizedBodyIndexOfConstrainedMobilizer(cmx);
+ QIndex qix; int nq;
+ UIndex uix; int nu;
+ matter.findMobilizerQs(s,mbx,qix,nq);
+ matter.findMobilizerUs(s,mbx,uix,nu);
+ mInfo.nQInUse = nq;
+ if (nq) {
+ mInfo.firstConstrainedQIndex = cInfo.addConstrainedQ(qix);
+ for (int i=1; i<nq; ++i) cInfo.addConstrainedQ(QIndex(qix+i));
+ }
+ mInfo.nUInUse = nu;
+ if (nu) {
+ mInfo.firstConstrainedUIndex = cInfo.addConstrainedU(uix);
+ for (int i=1; i<nu; ++i) cInfo.addConstrainedU(UIndex(uix+i));
+ }
+ }
+
+ // Now we can assign slots for Qs and Us.
+ const int ncq = cInfo.getNumConstrainedQ();
+ const int ncu = cInfo.getNumConstrainedU();
+ cInfo.consQSegment = Segment(ncq, ic.totalNConstrainedQInUse);
+ cInfo.consUSegment = Segment(ncu, ic.totalNConstrainedUInUse);
+ ic.totalNConstrainedQInUse += ncq;
+ ic.totalNConstrainedUInUse += ncu;
+
+ // Now collect all the participating mobilities. This includes the
+ // constrained mobilities as well as every q and u that can affect the
+ // constraint equations which involve constrained bodies. At the end we'll
+ // sort this list by subsystem QIndex/UIndex and remove duplicates.
+ cInfo.participatingQ = cInfo.constrainedQ;
+ cInfo.participatingU = cInfo.constrainedU;
+
+ const Array_<MobilizedBodyIndex>& bodies = mySubtree.getAllBodies();
+ for (int b=1; b<(int)bodies.size(); ++b) { // skip the Ancestor body 0
+ QIndex qix; int nq;
+ UIndex uix; int nu;
+ matter.findMobilizerQs(s,bodies[b],qix,nq);
+ matter.findMobilizerUs(s,bodies[b],uix,nu);
+ for (int i=0; i<nq; ++i) cInfo.participatingQ.push_back(QIndex(qix+i));
+ for (int i=0; i<nu; ++i) cInfo.participatingU.push_back(UIndex(uix+i));
+ }
+
+ // Caution: std::unique does not automatically shorten the original list.
+ std::sort(cInfo.participatingQ.begin(), cInfo.participatingQ.end());
+ Array_<QIndex>::iterator newEnd =
+ std::unique(cInfo.participatingQ.begin(), cInfo.participatingQ.end());
+ cInfo.participatingQ.erase(newEnd, cInfo.participatingQ.end());
+
+ realizeInstanceVirtual(s); // delegate to concrete constraint
+}
+
+
+
+// =============================================================================
+// REALIZE TIME
+// =============================================================================
+void ConstraintImpl::realizeTime(const SBStateDigest& sbs) const {
+ const SBInstanceVars& instanceVars = sbs.getInstanceVars();
+ if (instanceVars.constraintIsDisabled[myConstraintIndex]) return;
+
+ realizeTimeVirtual(sbs.getState()); // nothing to do in the base class
+}
+
+
+
+// =============================================================================
+// REALIZE POSITION
+// =============================================================================
+void ConstraintImpl::realizePosition(const SBStateDigest& sbs) const {
+ const SBInstanceVars& instanceVars = sbs.getInstanceVars();
+ if (instanceVars.constraintIsDisabled[myConstraintIndex]) return;
+ realizePositionVirtual(sbs.getState()); // delegate to concrete constraint
+}
+
+
+
+// =============================================================================
+// REALIZE VELOCITY
+// =============================================================================
+void ConstraintImpl::realizeVelocity(const SBStateDigest& sbs) const {
+ const SBInstanceVars& instanceVars = sbs.getInstanceVars();
+ if (instanceVars.constraintIsDisabled[myConstraintIndex]) return;
+ realizeVelocityVirtual(sbs.getState()); // delegate to concrete constraint
+}
+
+
+
+// =============================================================================
+// REALIZE DYNAMICS
+// =============================================================================
+void ConstraintImpl::realizeDynamics(const SBStateDigest& sbs) const {
+ const SBInstanceVars& instanceVars = sbs.getInstanceVars();
+ if (instanceVars.constraintIsDisabled[myConstraintIndex]) return;
+ realizeDynamicsVirtual(sbs.getState()); // delegate to concrete constraint
+}
+
+
+
+// =============================================================================
+// REALIZE ACCELERATION
+// =============================================================================
+void ConstraintImpl::realizeAcceleration(const SBStateDigest& sbs) const {
+ const SBInstanceVars& instanceVars = sbs.getInstanceVars();
+ if (instanceVars.constraintIsDisabled[myConstraintIndex]) return;
+ realizeAccelerationVirtual(sbs.getState()); // delegate to concrete constraint
+}
+
+
+
+// =============================================================================
+// REALIZE REPORT
+// =============================================================================
+void ConstraintImpl::realizeReport(const State& s) const {
+ if (isDisabled(s)) return;
+ realizeReportVirtual(s); // nothing to do in the base class
+}
+
+
+
+void ConstraintImpl::invalidateTopologyCache() const {
+ if (myMatterSubsystemRep)
+ myMatterSubsystemRep->invalidateSubsystemTopologyCache();
+}
+
+bool ConstraintImpl::subsystemTopologyHasBeenRealized() const {
+ return myMatterSubsystemRep && myMatterSubsystemRep->subsystemTopologyHasBeenRealized();
+}
+
+void ConstraintImpl::setMyMatterSubsystem
+ (SimbodyMatterSubsystem& matter, ConstraintIndex id)
+{
+ // If this is already set it has to be to the same MatterSubsystem.
+ assert(!myMatterSubsystemRep || myMatterSubsystemRep == &matter.getRep());
+ myMatterSubsystemRep = &matter.updRep();
+ myConstraintIndex = id;
+}
+
+const SimbodyMatterSubsystem&
+ConstraintImpl::getMyMatterSubsystem() const {
+ return getMyMatterSubsystemRep().getMySimbodyMatterSubsystemHandle();
+}
+
+bool ConstraintImpl::isInSameSubsystem(const MobilizedBody& body) const {
+ return isInSubsystem() && body.isInSubsystem()
+ && getMyMatterSubsystem().isSameSubsystem(body.getMatterSubsystem());
+}
+
+const MobilizedBody&
+ConstraintImpl::getMobilizedBodyFromConstrainedMobilizer(ConstrainedMobilizerIndex M) const {
+ SimTK_ASSERT(subsystemTopologyHasBeenRealized(),
+ "Constrained mobilizers are not available until Topology stage has been realized.");
+ return getMyMatterSubsystemRep().getMobilizedBody(myConstrainedMobilizers[M]);
+}
+
+const MobilizedBody&
+ConstraintImpl::getMobilizedBodyFromConstrainedBody(ConstrainedBodyIndex B) const {
+ SimTK_ASSERT(subsystemTopologyHasBeenRealized(),
+ "Constrained bodies are not available until Topology stage has been realized.");
+ return getMyMatterSubsystemRep().getMobilizedBody(myConstrainedBodies[B]);
+}
+
+const MobilizedBody&
+ConstraintImpl::getAncestorMobilizedBody() const {
+ SimTK_ASSERT(subsystemTopologyHasBeenRealized(),
+ "The ancestor body is not available until Topology stage has been realized.");
+ return getMyMatterSubsystemRep().getMobilizedBody(mySubtree.getAncestorMobilizedBodyIndex()); ;
+}
+
+Real ConstraintImpl::getOneQFromState
+ (const State& s, ConstrainedMobilizerIndex cmx, MobilizerQIndex whichQ) const
+{
+ const QIndex qx = getQIndexOfConstrainedQ(s, getConstrainedQIndex(s, cmx, whichQ));
+ return getMyMatterSubsystemRep().getQ(s)[qx];
+}
+
+Real ConstraintImpl::getOneUFromState
+ (const State& s, ConstrainedMobilizerIndex cmx, MobilizerUIndex whichU) const
+{
+ const UIndex ux = getUIndexOfConstrainedU(s, getConstrainedUIndex(s, cmx, whichU));
+ return getMyMatterSubsystemRep().getU(s)[ux];
+}
+
+Real ConstraintImpl::getOneQDotFromState
+ (const State& s,
+ ConstrainedMobilizerIndex cmx,
+ MobilizerQIndex whichQ) const
+{
+ const QIndex qx = getQIndexOfConstrainedQ
+ (s, getConstrainedQIndex(s, cmx, whichQ));
+ const SimbodyMatterSubsystemRep& matter = getMyMatterSubsystemRep();
+ return matter.getQDot(s)[qx];
+}
+
+Real ConstraintImpl::getOneUDotFromState
+ (const State& s,
+ ConstrainedMobilizerIndex cmx,
+ MobilizerUIndex whichU) const
+{
+ const UIndex ux = getUIndexOfConstrainedU
+ (s, getConstrainedUIndex(s, cmx, whichU));
+ const SimbodyMatterSubsystemRep& matter = getMyMatterSubsystemRep();
+ return matter.getUDot(s)[ux];
+}
+
+
+Real ConstraintImpl::getOneQDotDotFromState
+ (const State& s,
+ ConstrainedMobilizerIndex cmx,
+ MobilizerQIndex whichQ) const
+{
+ const QIndex qx = getQIndexOfConstrainedQ(s, getConstrainedQIndex(s, cmx, whichQ));
+ const SimbodyMatterSubsystemRep& matter = getMyMatterSubsystemRep();
+ return matter.getQDotDot(s)[qx];
+}
+
+// These are used to retrieve the indicated values from the State cache.
+const Transform& ConstraintImpl::getBodyTransformFromState
+ (const State& s, ConstrainedBodyIndex B) const
+{
+ const SimbodyMatterSubsystemRep& matter = getMyMatterSubsystemRep();
+ const MobilizedBodyIndex bodyB = myConstrainedBodies[B];
+
+ if (!myAncestorBodyIsNotGround)
+ return matter.getBodyTransform(s,bodyB); // X_AB==X_GB
+
+ static const Transform X_AA; // identity Transform
+ const MobilizedBodyIndex ancestorA = mySubtree.getAncestorMobilizedBodyIndex();
+ if (bodyB == ancestorA)
+ return X_AA;
+
+ const AncestorConstrainedBodyPoolIndex bx = myPoolIndex[B];
+ return matter.getTreePositionCache(s)
+ .constrainedBodyConfigInAncestor[bx]; // X_AB
+}
+const SpatialVec& ConstraintImpl::getBodyVelocityFromState
+ (const State& s, ConstrainedBodyIndex B) const
+{
+ const SimbodyMatterSubsystemRep& matter = getMyMatterSubsystemRep();
+ const MobilizedBodyIndex bodyB = myConstrainedBodies[B];
+
+ if (!myAncestorBodyIsNotGround)
+ return matter.getBodyVelocity(s,bodyB); // V_AB==V_GB
+
+ static const SpatialVec V_AA(Vec3(0),Vec3(0)); // zero velocity
+ const MobilizedBodyIndex ancestorA = mySubtree.getAncestorMobilizedBodyIndex();
+ if (bodyB == ancestorA)
+ return V_AA;
+
+ const AncestorConstrainedBodyPoolIndex bx = myPoolIndex[B];
+ return matter.getTreeVelocityCache(s)
+ .constrainedBodyVelocityInAncestor[bx]; // V_AB
+}
+
+
+
+// =============================================================================
+// CALC CONSTRAINED BODY TRANSFORM IN ANCESTOR
+// =============================================================================
+// 63 flops per constrained body
+void ConstraintImpl::calcConstrainedBodyTransformInAncestor // X_AB
+ (const SBStateDigest& sbs, SBTreePositionCache& tpc) const
+{
+ const SBInstanceVars& instanceVars = sbs.getInstanceVars();
+ if (instanceVars.constraintIsDisabled[myConstraintIndex]) return;
+ if (!myAncestorBodyIsNotGround) return;
+ const MobilizedBodyIndex ancestorA = mySubtree.getAncestorMobilizedBodyIndex();
+
+ // We expect Ground-relative kinematics already to have been calculated in
+ // tpc, but we can't verify that.
+ const Transform& X_GA = tpc.getX_GB(ancestorA);
+
+ for (ConstrainedBodyIndex cbx(0); cbx < getNumConstrainedBodies(); ++cbx) {
+ const MobilizedBodyIndex bodyB = myConstrainedBodies[cbx];
+ if (bodyB == ancestorA) continue; // skip ancestor if it's a constrained body also
+ const AncestorConstrainedBodyPoolIndex px = myPoolIndex[cbx];
+ assert(px.isValid());
+
+ const Transform& X_GB = tpc.getX_GB(bodyB);
+ tpc.updX_AB(px) = ~X_GA*X_GB; // 63 flops
+ }
+}
+
+
+
+// =============================================================================
+// CALC CONSTRAINED BODY VELOCITY IN ANCESTOR
+// =============================================================================
+// 51 flops per constrained body
+void ConstraintImpl::calcConstrainedBodyVelocityInAncestor // V_AB
+ (const SBStateDigest& sbs, SBTreeVelocityCache& tvc) const
+{
+ const SBInstanceVars& instanceVars = sbs.getInstanceVars();
+ if (instanceVars.constraintIsDisabled[myConstraintIndex]) return;
+ if (!myAncestorBodyIsNotGround) return;
+ const MobilizedBodyIndex ancestorA = mySubtree.getAncestorMobilizedBodyIndex();
+
+ const SBTreePositionCache& tpc = sbs.getTreePositionCache();
+
+ // All position kinematics has been calculated, and we also expect
+ // Ground-relative velocity kinematics already to have been calculated in tvc,
+ // but we can't verify that.
+ const Transform& X_GA = tpc.getX_GB(ancestorA);
+ const SpatialVec& V_GA = tvc.getV_GB(ancestorA);
+
+ for (ConstrainedBodyIndex cbx(0); cbx < getNumConstrainedBodies(); ++cbx) {
+ const MobilizedBodyIndex bodyB = myConstrainedBodies[cbx];
+ if (bodyB == ancestorA) continue; // skip the ancestor itself if it is a constrained body also
+ const AncestorConstrainedBodyPoolIndex px = myPoolIndex[cbx];
+ assert(px.isValid());
+
+ const Transform& X_GB = tpc.getX_GB(bodyB);
+ const SpatialVec& V_GB = tvc.getV_GB(bodyB);
+
+ // 6 flops
+ const Vec3 p_AB_G = X_GB.p() - X_GA.p();
+ const Vec3 p_AB_G_dot = V_GB[1] - V_GA[1]; // d/dt p taken in G
+
+ // 3 flops
+ const Vec3 w_AB_G = V_GB[0] - V_GA[0]; // relative angular velocity of B in A, exp. in G
+
+ // To get d/dt p taken in A, get derivative in G and remove the contribution generated by
+ // A's velocity in G.
+ // 12 flops
+ const Vec3 v_AB_G = p_AB_G_dot - V_GA[0] % p_AB_G; // time deriv of p in A, exp in G
+
+ // 30 flops
+ tvc.updV_AB(px) = ~X_GA.R() * SpatialVec(w_AB_G, v_AB_G); // re-express in A
+ }
+}
+
+
+// Find out how many holonomic (position), nonholonomic (velocity),
+// and acceleration-only constraint equations are generated by this Constraint
+// as it is currently being modeled.
+void ConstraintImpl::getNumConstraintEquationsInUse
+ (const State& s, int& mHolo, int& mNonholo, int& mAccOnly) const
+{
+ const SBInstancePerConstraintInfo& cInfo =
+ getInstanceCache(s).getConstraintInstanceInfo(myConstraintIndex);
+
+ mHolo = cInfo.holoErrSegment.length;
+ mNonholo = cInfo.nonholoErrSegment.length;
+ mAccOnly = cInfo.accOnlyErrSegment.length;
+}
+
+void ConstraintImpl::
+getIndexOfMultipliersInUse(const State& s,
+ MultiplierIndex& px0,
+ MultiplierIndex& vx0,
+ MultiplierIndex& ax0) const
+{
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const SBInstancePerConstraintInfo& cInfo =
+ ic.getConstraintInstanceInfo(myConstraintIndex);
+
+ // Find the offset to our first multiplier in the ModelCache.
+ const int firstHoloErr = cInfo.holoErrSegment.offset;
+ const int mHolo = cInfo.holoErrSegment.length;
+
+ const int firstNonholoErr =
+ ic.totalNHolonomicConstraintEquationsInUse //total for whole subsystem
+ + cInfo.nonholoErrSegment.offset;
+ const int mNonholo = cInfo.nonholoErrSegment.length;
+
+ const int firstAccOnlyErr =
+ ic.totalNHolonomicConstraintEquationsInUse
+ + ic.totalNNonholonomicConstraintEquationsInUse
+ + cInfo.accOnlyErrSegment.offset;
+ const int mAccOnly = cInfo.accOnlyErrSegment.length;
+
+ px0.invalidate(); if (mHolo) px0=MultiplierIndex(firstHoloErr);
+ vx0.invalidate(); if (mNonholo) vx0=MultiplierIndex(firstNonholoErr);
+ ax0.invalidate(); if (mAccOnly) ax0=MultiplierIndex(firstAccOnlyErr);
+}
+
+void ConstraintImpl::
+setMyPartInConstraintSpaceVector(const State& s,
+ const Vector& myPart,
+ Vector& constraintSpace) const
+{
+ const SBInstanceCache& ic = getInstanceCache(s);
+ // Global problem dimensions.
+ const int mHolo = ic.totalNHolonomicConstraintEquationsInUse;
+ const int mNonholo = ic.totalNNonholonomicConstraintEquationsInUse;
+ const int mAccOnly = ic.totalNAccelerationOnlyConstraintEquationsInUse;
+
+ const int m = mHolo+mNonholo+mAccOnly;
+ if (constraintSpace.size() == 0) {
+ constraintSpace.resize(m);
+ constraintSpace.setToZero();
+ }
+
+ SimTK_ERRCHK2_ALWAYS(constraintSpace.size()==m,
+ "Constraint::setMyPartInConstraintSpaceVector()",
+ "Output vector had size %d but should have had size zero or m=%d.",
+ constraintSpace.size(), m);
+
+ int mp, mv, ma;
+ getNumConstraintEquationsInUse(s, mp, mv, ma);
+ MultiplierIndex px0, vx0, ax0;
+ getIndexOfMultipliersInUse(s, px0, vx0, ax0);
+
+ for (int i=0; i<mp; ++i)
+ constraintSpace[px0+i] = myPart[i];
+ for (int i=0; i<mv; ++i)
+ constraintSpace[vx0+i] = myPart[mp+i];
+ for (int i=0; i<ma; ++i)
+ constraintSpace[ax0+i] = myPart[mp+mv+i];
+}
+
+void ConstraintImpl::
+getMyPartFromConstraintSpaceVector(const State& s,
+ const Vector& constraintSpace,
+ Vector& myPart) const
+{
+ const SBInstanceCache& ic = getInstanceCache(s);
+ // Global problem dimensions.
+ const int mHolo = ic.totalNHolonomicConstraintEquationsInUse;
+ const int mNonholo = ic.totalNNonholonomicConstraintEquationsInUse;
+ const int mAccOnly = ic.totalNAccelerationOnlyConstraintEquationsInUse;
+
+ const int m = mHolo+mNonholo+mAccOnly;
+
+ SimTK_ERRCHK2_ALWAYS(constraintSpace.size()==m,
+ "Constraint::getMyPartFromConstraintSpaceVector()",
+ "Input vector had size %d but should have had size m=%d.",
+ constraintSpace.size(), m);
+
+ int mp, mv, ma;
+ getNumConstraintEquationsInUse(s, mp, mv, ma);
+ MultiplierIndex px0, vx0, ax0;
+ getIndexOfMultipliersInUse(s, px0, vx0, ax0);
+
+ myPart.resize(mp+mv+ma);
+
+ for (int i=0; i<mp; ++i)
+ myPart[i] = constraintSpace[px0+i];
+ for (int i=0; i<mv; ++i)
+ myPart[mp+i] = constraintSpace[vx0+i];
+ for (int i=0; i<ma; ++i)
+ myPart[mp+mv+i] = constraintSpace[ax0+i];
+}
+
+void ConstraintImpl::setDisabled(State& s, bool shouldBeDisabled) const {
+ getMyMatterSubsystemRep().setConstraintIsDisabled(s, myConstraintIndex, shouldBeDisabled);
+}
+
+bool ConstraintImpl::isDisabled(const State& s) const {
+ return getMyMatterSubsystemRep().isConstraintDisabled(s, myConstraintIndex);
+}
+
+// Call this during construction phase to add a body to the topological
+// structure of this Constraint. This body's mobilizer's mobilities are
+// *not* part of the constraint; mobilizers must be added separately. It is OK
+// to add the same body multiple times; it will only get inserted once and
+// you'll get the same index every time.
+ConstrainedBodyIndex ConstraintImpl::
+addConstrainedBody(const MobilizedBody& b) {
+ assert(isInSameSubsystem(b));
+ invalidateTopologyCache();
+
+ const ConstrainedBodyIndex nextIx((int)myConstrainedBodies.size());
+
+ // Add to the Mobilized->Constrained map and check for duplicates.
+ std::pair<MobilizedBody2ConstrainedBodyMap::iterator, bool> result;
+ result = myMobilizedBody2ConstrainedBodyMap.insert(
+ MobilizedBody2ConstrainedBodyMap::value_type(b.getMobilizedBodyIndex(),
+ nextIx));
+ if (!result.second) {
+ // It was already there.
+ return result.first->second; // the index we assigned before
+ }
+
+ // This is a new constrained body -- add it to the
+ // ConstrainedBody->MobilizedBody map too.
+ myConstrainedBodies.push_back(b.getMobilizedBodyIndex());
+ return nextIx;
+}
+
+// Call this during construction phase to add a mobilizer to the topological
+// structure of this Constraint. All the coordinates q and mobilities u for this
+// mobilizer are added also, but we don't know how many of those there will be
+// until Stage::Model. It is OK to add the same mobilizer multiple times; it will
+// only get inserted once and you'll get the same index every time.
+ConstrainedMobilizerIndex ConstraintImpl::
+addConstrainedMobilizer(const MobilizedBody& b) {
+ assert(isInSameSubsystem(b));
+ invalidateTopologyCache();
+
+ const ConstrainedMobilizerIndex nextIx((int)myConstrainedMobilizers.size());
+
+ // Add to the Mobilized->Constrained map and check for duplicates.
+ std::pair<MobilizedBody2ConstrainedMobilizerMap::iterator, bool> result;
+ result = myMobilizedBody2ConstrainedMobilizerMap.insert
+ (MobilizedBody2ConstrainedMobilizerMap::value_type
+ (b.getMobilizedBodyIndex(), nextIx));
+
+ if (!result.second) {
+ // It was already there.
+ return result.first->second; // the index we assigned before
+ }
+
+ // This is a new constrained mobilizer -- add it to the
+ // ConstrainedMobilizer->MobilizedBody map too.
+ myConstrainedMobilizers.push_back(b.getMobilizedBodyIndex());
+ return nextIx;
+}
+
+QIndex ConstraintImpl::getQIndexOfConstrainedQ(const State& s, ConstrainedQIndex cqx) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const SBInstancePerConstraintInfo&
+ cInfo = ic.getConstraintInstanceInfo(myConstraintIndex);
+ return cInfo.getQIndexFromConstrainedQ(cqx);
+}
+
+UIndex ConstraintImpl::getUIndexOfConstrainedU(const State& s, ConstrainedUIndex cux) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const SBInstancePerConstraintInfo& cInfo =
+ ic.getConstraintInstanceInfo(myConstraintIndex);
+ return cInfo.getUIndexFromConstrainedU(cux);
+}
+
+int ConstraintImpl::getNumConstrainedQ(const State& s) const {
+ return getInstanceCache(s).getConstraintInstanceInfo(myConstraintIndex)
+ .getNumConstrainedQ();
+}
+
+int ConstraintImpl::getNumConstrainedQ
+ (const State& s, ConstrainedMobilizerIndex M) const
+{
+ const SBInstancePerConstrainedMobilizerInfo& mInfo =
+ getInstanceCache(s).getConstraintInstanceInfo(myConstraintIndex)
+ .getConstrainedMobilizerInstanceInfo(M);
+ return mInfo.nQInUse; // same as corresponding Mobod, or 0 if disabled
+}
+
+ConstrainedQIndex ConstraintImpl::getConstrainedQIndex
+ (const State& s, ConstrainedMobilizerIndex M, MobilizerQIndex which) const
+{
+ const int nq = getNumConstrainedQ(s,M);
+ assert(0 <= which && which < nq);
+ const SBInstancePerConstrainedMobilizerInfo& mInfo =
+ getInstanceCache(s).getConstraintInstanceInfo(myConstraintIndex)
+ .getConstrainedMobilizerInstanceInfo(M);
+ return ConstrainedQIndex(mInfo.firstConstrainedQIndex + which);
+}
+
+int ConstraintImpl::getNumConstrainedU(const State& s) const {
+ return getInstanceCache(s).getConstraintInstanceInfo(myConstraintIndex)
+ .getNumConstrainedU();
+}
+
+int ConstraintImpl::getNumConstrainedU
+ (const State& s, ConstrainedMobilizerIndex M) const
+{
+ const SBInstancePerConstrainedMobilizerInfo& mInfo =
+ getInstanceCache(s).getConstraintInstanceInfo(myConstraintIndex)
+ .getConstrainedMobilizerInstanceInfo(M);
+ return mInfo.nUInUse; // same as corr. MobilizedBody, or 0 if disabled
+}
+
+ConstrainedUIndex ConstraintImpl::getConstrainedUIndex
+ (const State& s, ConstrainedMobilizerIndex M, MobilizerUIndex which) const
+{
+ const int nu = getNumConstrainedU(s,M);
+ assert(0 <= which && which < nu);
+ const SBInstancePerConstrainedMobilizerInfo& mInfo =
+ getInstanceCache(s).getConstraintInstanceInfo(myConstraintIndex)
+ .getConstrainedMobilizerInstanceInfo(M);
+ return ConstrainedUIndex(mInfo.firstConstrainedUIndex + which);
+}
+
+
+
+//==============================================================================
+// CONVERT Q FORCES TO U FORCES
+//==============================================================================
+// uForces = ~N * qForces
+void ConstraintImpl::
+convertQForcesToUForces(const State& s,
+ const Array_<Real,ConstrainedQIndex>& qForces,
+ Array_<Real,ConstrainedUIndex>& uForces) const
+{
+ const int ncm = getNumConstrainedMobilizers();
+ if (ncm == 0)
+ return; // very common!
+
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const SBInstancePerConstraintInfo&
+ cInfo = ic.getConstraintInstanceInfo(myConstraintIndex);
+
+ assert(cInfo.getNumConstrainedMobilizers() == ncm);
+
+ const int ncu = cInfo.getNumConstrainedU();
+ const int ncq = cInfo.getNumConstrainedQ();
+
+ assert(qForces.size() == ncq);
+ uForces.resize(ncu);
+
+ if (ncu == 0)
+ return;
+
+ for (ConstrainedMobilizerIndex cmx(0); cmx < ncm; ++cmx) {
+ const SBInstancePerConstrainedMobilizerInfo& mInfo =
+ cInfo.getConstrainedMobilizerInstanceInfo(cmx);
+ const int nq = mInfo.nQInUse, nu = mInfo.nUInUse;
+ if (nu == 0) continue;
+
+ const Real* firstQ = &qForces[mInfo.firstConstrainedQIndex];
+ Real* firstU = &uForces[mInfo.firstConstrainedUIndex];
+ const ArrayViewConst_<Real,MobilizerQIndex> fq(firstQ, firstQ+nq);
+ ArrayView_<Real,MobilizerUIndex> fu(firstU, firstU+nu);
+ const MobilizedBody& mobod =
+ getMobilizedBodyFromConstrainedMobilizer(cmx);
+ mobod.convertQForceToUForce(s, fq, fu);
+ }
+}
+
+
+
+//==============================================================================
+// CONVERT BODY ACCEL TO CONSTRAINED BODY ACCEL
+//==============================================================================
+void ConstraintImpl::convertBodyAccelToConstrainedBodyAccel
+ (const State& s,
+ const Array_<SpatialVec, MobilizedBodyIndex>& allA_GB,
+ Array_<SpatialVec, ConstrainedBodyIndex>& A_AB) const
+{
+ const SimbodyMatterSubsystemRep& matter = getMyMatterSubsystemRep();
+ assert(allA_GB.size() == matter.getNumBodies());
+
+ const int ncb = getNumConstrainedBodies();
+ A_AB.resize(ncb);
+
+ // If the Ancestor is Ground we're just reordering.
+ if (!isAncestorDifferentFromGround()) {
+ for (ConstrainedBodyIndex cbx(0); cbx < ncb; ++cbx) {
+ const MobilizedBodyIndex mbx =
+ getMobilizedBodyIndexOfConstrainedBody(cbx);
+ A_AB[cbx] = allA_GB[mbx];
+ }
+ return;
+ }
+
+ // If the Ancestor is not Ground we have to transform the accelerations
+ // from Ground to Ancestor, at a cost of 105 flops/constrained body (not
+ // just re-expressing).
+
+ const MobilizedBody& ancestor = getAncestorMobilizedBody();
+ const MobilizedBodyIndex acx = ancestor.getMobilizedBodyIndex();
+ const Transform& X_GA = ancestor.getBodyTransform(s);
+ const SpatialVec& V_GA = ancestor.getBodyVelocity(s);
+ const SpatialVec& A_GA = allA_GB[acx]; // from input argument
+
+ for (ConstrainedBodyIndex cbx(0); cbx < ncb; ++cbx) {
+ const MobilizedBodyIndex mbx =
+ getMobilizedBodyIndexOfConstrainedBody(cbx);
+ if (mbx == acx) { // common: Ancestor is a constrained body
+ A_AB[cbx] = SpatialVec(Vec3(0));
+ continue;
+ }
+ const MobilizedBody& consBody = matter.getMobilizedBody(mbx);
+ const Transform& X_GB = consBody.getBodyTransform(s);
+ const SpatialVec& V_GB = consBody.getBodyVelocity(s);
+ const SpatialVec& A_GB = allA_GB[mbx]; // from input arg
+ A_AB[cbx] = findRelativeAcceleration(X_GA, V_GA, A_GA,
+ X_GB, V_GB, A_GB);
+ }
+}
+
+
+
+//==============================================================================
+// CONVERT BODY VELOCITY TO CONSTRAINED BODY VELOCITY
+//==============================================================================
+void ConstraintImpl::convertBodyVelocityToConstrainedBodyVelocity
+ (const State& s,
+ const Array_<SpatialVec, MobilizedBodyIndex>& allV_GB,
+ Array_<SpatialVec, ConstrainedBodyIndex>& V_AB) const
+{
+ const SimbodyMatterSubsystemRep& matter = getMyMatterSubsystemRep();
+ assert(allV_GB.size() == matter.getNumBodies());
+
+ const int ncb = getNumConstrainedBodies();
+ V_AB.resize(ncb);
+
+ // If the Ancestor is Ground we're just reordering.
+ if (!isAncestorDifferentFromGround()) {
+ for (ConstrainedBodyIndex cbx(0); cbx < ncb; ++cbx) {
+ const MobilizedBodyIndex mbx =
+ getMobilizedBodyIndexOfConstrainedBody(cbx);
+ V_AB[cbx] = allV_GB[mbx];
+ }
+ return;
+ }
+
+ // If the Ancestor is not Ground we have to transform the velocities
+ // from Ground to Ancestor, at a cost of 51 flops/constrained body (not
+ // just re-expressing).
+
+ const MobilizedBody& ancestor = getAncestorMobilizedBody();
+ const MobilizedBodyIndex acx = ancestor.getMobilizedBodyIndex();
+ const Transform& X_GA = ancestor.getBodyTransform(s);
+ const SpatialVec& V_GA = allV_GB[acx]; // from input argument
+
+ for (ConstrainedBodyIndex cbx(0); cbx < ncb; ++cbx) {
+ const MobilizedBodyIndex mbx =
+ getMobilizedBodyIndexOfConstrainedBody(cbx);
+ if (mbx == acx) { // common: Ancestor is a constrained body
+ V_AB[cbx] = SpatialVec(Vec3(0));
+ continue;
+ }
+ const MobilizedBody& consBody = matter.getMobilizedBody(mbx);
+ const Transform& X_GB = consBody.getBodyTransform(s);
+ const SpatialVec& V_GB = allV_GB[mbx]; // from input arg
+ V_AB[cbx] = findRelativeVelocity(X_GA, V_GA, // 51 flops
+ X_GB, V_GB);
+ }
+}
+
+
+
+//==============================================================================
+// GET POSITION, VELOCITY, ACCELERATION ERRORS
+//==============================================================================
+
+// Given a state realized to Position stage, extract the position constraint
+// errors corresponding to this Constraint. The 'mp' argument is for sanity
+// checking -- it is an error if that isn't an exact match for the current
+// number of holonomic constraint equations generated by this Constraint. We
+// expect that perr points to an array of at least mp elements that we can
+// write on.
+void ConstraintImpl::getPositionErrors(const State& s, int mp, Real* perr) const {
+ const SBInstancePerConstraintInfo& cInfo =
+ getInstanceCache(s).getConstraintInstanceInfo(myConstraintIndex);
+
+ assert(mp == cInfo.holoErrSegment.length);
+
+ // Find the offset to our first qerr in the ModelCache.
+ const int firstQErr = cInfo.holoErrSegment.offset;
+
+ // Get all qerr's for the subsystem.
+ const Vector& qerr = getMyMatterSubsystemRep().getQErr(s);
+
+ // Copy out the qerr's belonging to this constraint.
+ for (int i=0; i < mp; ++i)
+ perr[i] = qerr[firstQErr + i];
+}
+
+// Given a State realized to Velocity stage, extract the velocity constraint errors
+// corresponding to this Constraint. This includes velocity constraints which were
+// produced by differentiation of holonomic (position) constraints, and nonholonomic
+// constraints which are introduced at the velocity level. The 'mpv' argument is
+// for sanity checking -- it is an error if that isn't an exact match for the
+// current number of holonomic+nonholonomic (mp+mv) constraint equations generated
+// by this Constraint. We expect that pverr points to an array of at least mp+mv
+// elements that we can write on.
+void ConstraintImpl::getVelocityErrors(const State& s, int mpv, Real* pverr) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const SBInstancePerConstraintInfo& cInfo =
+ ic.getConstraintInstanceInfo(myConstraintIndex);
+
+ assert(mpv == cInfo.holoErrSegment.length
+ + cInfo.nonholoErrSegment.length);
+
+ // Get reference to all uerr's for the subsystem.
+ const Vector& uerr = getMyMatterSubsystemRep().getUErr(s);
+
+ // Find the offset to our first uerr in the ModelCache.
+ const int firstHoloErr = cInfo.holoErrSegment.offset;
+ const int mHolo = cInfo.holoErrSegment.length;
+
+ for (int i=0; i < mHolo; ++i)
+ pverr[i] = uerr[firstHoloErr+i];
+
+ const int firstNonholoErr = ic.totalNHolonomicConstraintEquationsInUse // total for whole subsystem
+ + cInfo.nonholoErrSegment.offset;
+ const int mNonholo = cInfo.nonholoErrSegment.length;
+
+ for (int i=0; i < mNonholo; ++i)
+ pverr[mHolo+i] = uerr[firstNonholoErr+i];
+}
+
+// Given a State realized to Acceleration stage, extract the acceleration
+// constraint errors corresponding to this Constraint. This includes
+// acceleration constraints which were produced by twice differentiation of
+// holonomic (position) constraints, and differentiation of nonholonomic
+// (velocity) constraints, and acceleration-only constraints which are
+// first introduced at the acceleration level. The 'mpva' argument is
+// for sanity checking -- it is an error if that isn't an exact match for the
+// current number of holonomic+nonholonomic+accelerationOnly (mp+mv+ma)
+// constraint equations generated by this Constraint. We expect that pvaerr
+// points to an array of at least mp+mv+ma elements that we can write on.
+void ConstraintImpl::getAccelerationErrors(const State& s, int mpva, Real* pvaerr) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const SBInstancePerConstraintInfo& cInfo =
+ ic.getConstraintInstanceInfo(myConstraintIndex);
+
+ assert(mpva == cInfo.holoErrSegment.length
+ + cInfo.nonholoErrSegment.length
+ + cInfo.accOnlyErrSegment.length);
+
+ // Get reference to all udoterr's for the subsystem.
+ const Vector& udoterr = getMyMatterSubsystemRep().getUDotErr(s);
+
+ // Find the offset to our first uerr in the ModelCache.
+ const int firstHoloErr = cInfo.holoErrSegment.offset;
+ const int mHolo = cInfo.holoErrSegment.length;
+
+ for (int i=0; i < mHolo; ++i)
+ pvaerr[i] = udoterr[firstHoloErr+i];
+
+ const int firstNonholoErr = ic.totalNHolonomicConstraintEquationsInUse // total for whole subsystem
+ + cInfo.nonholoErrSegment.offset;
+ const int mNonholo = cInfo.nonholoErrSegment.length;
+
+ for (int i=0; i < mNonholo; ++i)
+ pvaerr[mHolo+i] = udoterr[firstNonholoErr+i];
+
+ const int firstAccOnlyErr = ic.totalNHolonomicConstraintEquationsInUse
+ + ic.totalNNonholonomicConstraintEquationsInUse // total for whole subsystem
+ + cInfo.accOnlyErrSegment.offset;
+ const int mAccOnly = cInfo.accOnlyErrSegment.length;
+
+ for (int i=0; i < mAccOnly; ++i)
+ pvaerr[mHolo+mNonholo+i] = udoterr[firstAccOnlyErr+i];
+}
+
+// Given a State realized to Acceleration stage, extract the Lagrange
+// multipliers corresponding to this Constraint. The 'mpva' argument is for
+// sanity checking -- it is an error if that isn't an exact match for the
+// current number of holonomic+nonholonomic+accelerationOnly (mp+mv+ma)
+// constraint equations generated by this Constraint. We expect that lambda
+// points to an array of at least mp+mv+ma elements that we can write on.
+void ConstraintImpl::getMultipliers(const State& s, int mpva, Real* lambda) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const SBInstancePerConstraintInfo& cInfo =
+ ic.getConstraintInstanceInfo(myConstraintIndex);
+
+ assert(mpva == cInfo.holoErrSegment.length
+ + cInfo.nonholoErrSegment.length
+ + cInfo.accOnlyErrSegment.length);
+
+ // Get reference to all multipliers for the subsystem. This will throw
+ // an error if the matter subsystem hasn't already been realized through
+ // acceleration stage.
+ const Vector& multipliers = getMyMatterSubsystemRep().getMultipliers(s);
+
+ // Find the offset to our first multiplier in the ModelCache.
+ const int firstHoloErr = cInfo.holoErrSegment.offset;
+ const int mHolo = cInfo.holoErrSegment.length;
+
+ for (int i=0; i < mHolo; ++i)
+ lambda[i] = multipliers[firstHoloErr+i];
+
+ const int firstNonholoErr = ic.totalNHolonomicConstraintEquationsInUse // total for whole subsystem
+ + cInfo.nonholoErrSegment.offset;
+ const int mNonholo = cInfo.nonholoErrSegment.length;
+
+ for (int i=0; i < mNonholo; ++i)
+ lambda[mHolo+i] = multipliers[firstNonholoErr+i];
+
+ const int firstAccOnlyErr = ic.totalNHolonomicConstraintEquationsInUse
+ + ic.totalNNonholonomicConstraintEquationsInUse // total for whole subsystem
+ + cInfo.accOnlyErrSegment.offset;
+ const int mAccOnly = cInfo.accOnlyErrSegment.length;
+
+ for (int i=0; i < mAccOnly; ++i)
+ lambda[mHolo+mNonholo+i] = multipliers[firstAccOnlyErr+i];
+}
+
+// Reference this constraint's assigned partition within the larger array.
+ArrayView_<SpatialVec,ConstrainedBodyIndex> ConstraintImpl::
+updConstrainedBodyForces(const State& state,
+ Array_<SpatialVec>& allConsBodyForces) const
+{
+ const SBInstanceCache& ic = getInstanceCache(state);
+ assert(allConsBodyForces.size() == ic.totalNConstrainedBodiesInUse);
+
+ const ConstraintIndex cx = getMyConstraintIndex();
+ const SBInstancePerConstraintInfo& cInfo = ic.getConstraintInstanceInfo(cx);
+
+ const Segment& consBodySegment = cInfo.consBodySegment;
+ const int ncb = consBodySegment.length;
+
+ // No heap allocation is being done here. The longer array can be
+ // empty so we're using begin() here rather than &array[0] which
+ // would be illegal in that case. This pointer may be null.
+ SpatialVec* firstBodySlot = allConsBodyForces.begin()
+ + consBodySegment.offset;
+
+ return ArrayView_<SpatialVec,ConstrainedBodyIndex>
+ (firstBodySlot, firstBodySlot + ncb);
+}
+
+ArrayView_<Real,ConstrainedUIndex> ConstraintImpl::
+updConstrainedMobilityForces(const State& state,
+ Array_<Real>& allConsMobForces) const
+{
+ const SBInstanceCache& ic = getInstanceCache(state);
+ assert(allConsMobForces.size() == ic.totalNConstrainedUInUse);
+
+ const ConstraintIndex cx = getMyConstraintIndex();
+ const SBInstancePerConstraintInfo& cInfo = ic.getConstraintInstanceInfo(cx);
+
+ const Segment& consUSegment = cInfo.consUSegment;
+ const int ncu = consUSegment.length;
+
+ // No heap allocation is being done here. The longer array can be
+ // empty so we're using begin() here rather than &array[0] which
+ // would be illegal in that case. This pointer may be null.
+ Real* firstMobSlot = allConsMobForces.begin()
+ + consUSegment.offset;
+
+ return ArrayView_<Real,ConstrainedUIndex>
+ (firstMobSlot, firstMobSlot + ncu);
+}
+
+const SBInstanceCache& ConstraintImpl::getInstanceCache(const State& s) const {
+ return getMyMatterSubsystemRep().getInstanceCache(s);
+}
+const SBModelCache& ConstraintImpl::getModelCache(const State& s) const {
+ return getMyMatterSubsystemRep().getModelCache(s);
+}
+const SBTreePositionCache& ConstraintImpl::getTreePositionCache(const State& s) const {
+ return getMyMatterSubsystemRep().getTreePositionCache(s);
+}
+const SBTreeVelocityCache& ConstraintImpl::getTreeVelocityCache(const State& s) const {
+ return getMyMatterSubsystemRep().getTreeVelocityCache(s);
+}
+const SBTreeAccelerationCache& ConstraintImpl::getTreeAccelerationCache(const State& s) const {
+ return getMyMatterSubsystemRep().getTreeAccelerationCache(s);
+}
+const SBConstrainedAccelerationCache& ConstraintImpl::
+getConstrainedAccelerationCache(const State& s) const {
+ return getMyMatterSubsystemRep().getConstrainedAccelerationCache(s);
+}
+SBConstrainedAccelerationCache& ConstraintImpl::
+updConstrainedAccelerationCache(const State& s) const {
+ return getMyMatterSubsystemRep().updConstrainedAccelerationCache(s);
+}
+
+//------------------------------------------------------------------------------
+// Default implementations for ConstraintImpl virtuals throw "unimplemented"
+// exceptions. These shouldn't be called unless the concrete constraint has
+// given a non-zero value for mp, mv, and/or ma which is a promise to
+// implement the associated methods.
+//------------------------------------------------------------------------------
+
+ // These four must be defined if there are any position (holonomic)
+ // constraints defined.
+
+void ConstraintImpl::calcPositionErrorsVirtual
+ (const State& state,
+ const Array_<Transform,ConstrainedBodyIndex>& X_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr)
+ const {
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "ConstraintImpl", "calcPositionErrorsVirtual");
+}
+
+void ConstraintImpl::calcPositionDotErrorsVirtual
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr)
+ const {
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "ConstraintImpl", "calcPositionDotErrorsVirtual");
+}
+
+void ConstraintImpl::calcPositionDotDotErrorsVirtual
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr)
+ const {
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "ConstraintImpl", "calcPositionDotDotErrorsVirtual");
+}
+
+void ConstraintImpl::addInPositionConstraintForcesVirtual
+ (const State& state,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForces,
+ Array_<Real, ConstrainedQIndex>& qForces)
+ const
+{
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "ConstraintImpl", "addInPositionConstraintForcesVirtual");
+}
+
+
+ // These three must be defined if there are any velocity (nonholonomic)
+ // constraints defined.
+
+void ConstraintImpl::calcVelocityErrorsVirtual
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedU,
+ Array_<Real>& verr)
+ const {
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "ConstraintImpl", "calcVelocityErrorsVirtual");
+}
+
+void ConstraintImpl::calcVelocityDotErrorsVirtual
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedUDot,
+ Array_<Real>& vaerr)
+ const {
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "ConstraintImpl", "calcVelocityDotErrorsVirtual");
+}
+
+void ConstraintImpl::addInVelocityConstraintForcesVirtual
+ (const State& state,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForces,
+ Array_<Real, ConstrainedUIndex>& mobilityForces)
+ const
+{
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "ConstraintImpl", "addInVelocityConstraintForcesVirtual");
+}
+
+
+ // These two must be defined if there are any acceleration-only constraints
+ // defined.
+
+void ConstraintImpl::calcAccelerationErrorsVirtual
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedUDot,
+ Array_<Real>& aerr)
+ const {
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "ConstraintImpl", "calcAccelerationErrorsVirtual");
+}
+
+void ConstraintImpl::addInAccelerationConstraintForcesVirtual
+ (const State& state,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForces,
+ Array_<Real, ConstrainedUIndex>& mobilityForces)
+ const
+{
+ SimTK_THROW2(Exception::UnimplementedVirtualMethod,
+ "ConstraintImpl", "addInAccelerationConstraintForcesVirtual");
+}
+
+//------------------------------------------------------------------------------
+// These are interfaces to the constraint operators which first extract
+// the operands from a given state. These are thus suitable for use when
+// realizing that state at the point where the constraint operator results
+// are about to go into the state cache. The cache is not updated here,
+// however. Instead the result is returned explicitly in an argument.
+//------------------------------------------------------------------------------
+
+// Calculate the mp position errors that would result from the configuration
+// present in the supplied state (that is, q's and body transforms). The state
+// must be realized through Time stage and part way through realization of
+// Position stage.
+void ConstraintImpl::
+calcPositionErrorsFromState(const State& s, Array_<Real>& perr) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const SBInstancePerConstraintInfo& cInfo =
+ ic.getConstraintInstanceInfo(myConstraintIndex);
+ const Vector& q = s.getQ();
+
+ const int ncb = getNumConstrainedBodies();
+ const int ncq = cInfo.getNumConstrainedQ();
+
+ Array_<Transform, ConstrainedBodyIndex> X_AB(ncb);
+ Array_<Real, ConstrainedQIndex> cq(ncq);
+
+ for (ConstrainedBodyIndex cbx(0); cbx < ncb; ++cbx)
+ X_AB[cbx] = getBodyTransformFromState(s, cbx);
+
+ for (ConstrainedQIndex cqx(0); cqx < ncq; ++cqx)
+ cq[cqx] = q[cInfo.getQIndexFromConstrainedQ(cqx)];
+
+ calcPositionErrors(s,X_AB,cq,perr);
+}
+
+// Calculate the mp velocity errors resulting from pdot equations, given a
+// configuration and velocities in the supplied state which must be realized
+// through Position stage and part way through realization of Velocity stage.
+void ConstraintImpl::
+calcPositionDotErrorsFromState(const State& s, Array_<Real>& pverr) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const SBInstancePerConstraintInfo& cInfo =
+ ic.getConstraintInstanceInfo(myConstraintIndex);
+
+ // We're not checking, but the tree velocity cache better have been
+ // marked valid by now, indicating that we finished calculating qdots.
+ // The state doesn't know about that yet, so we have to use updQDot()
+ // here rather than getQDot().
+ const Vector& qdot = s.updQDot();
+
+ const int ncb = getNumConstrainedBodies();
+ const int ncq = cInfo.getNumConstrainedQ();
+
+ Array_<SpatialVec, ConstrainedBodyIndex> V_AB(ncb);
+ Array_<Real, ConstrainedQIndex> cqdot(ncq);
+
+ for (ConstrainedBodyIndex cbx(0); cbx < ncb; ++cbx)
+ V_AB[cbx] = getBodyVelocityFromState(s, cbx);
+
+ for (ConstrainedQIndex cqx(0); cqx < ncq; ++cqx)
+ cqdot[cqx] = qdot[cInfo.getQIndexFromConstrainedQ(cqx)];
+
+ calcPositionDotErrors(s,V_AB,cqdot,pverr);
+}
+
+
+
+// Calculate the mv velocity errors resulting from the nonholonomic constraint
+// equations here, taking the configuration and velocities (u, qdot, body
+// spatial velocities) from the supplied state, which must be realized through
+// Position stage and part way through realization of Velocity stage.
+void ConstraintImpl::
+calcVelocityErrorsFromState(const State& s, Array_<Real>& verr) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const SBInstancePerConstraintInfo& cInfo =
+ ic.getConstraintInstanceInfo(myConstraintIndex);
+ const Vector& u = s.getU();
+
+ const int ncb = getNumConstrainedBodies();
+ const int ncu = cInfo.getNumConstrainedU();
+
+ Array_<SpatialVec, ConstrainedBodyIndex> V_AB(ncb);
+ Array_<Real, ConstrainedUIndex> cu(ncu);
+
+ for (ConstrainedBodyIndex cbx(0); cbx < ncb; ++cbx)
+ V_AB[cbx] = getBodyVelocityFromState(s, cbx);
+
+ for (ConstrainedUIndex cux(0); cux < ncu; ++cux)
+ cu[cux] = u[cInfo.getUIndexFromConstrainedU(cux)];
+
+ calcVelocityErrorsVirtual(s,V_AB,cu,verr);
+}
+
+
+
+} // namespace SimTK
+
diff --git a/Simbody/src/ConstraintImpl.h b/Simbody/src/ConstraintImpl.h
new file mode 100644
index 0000000..0d2dd7b
--- /dev/null
+++ b/Simbody/src/ConstraintImpl.h
@@ -0,0 +1,3244 @@
+#ifndef SimTK_SIMBODY_CONSTRAINT_IMPL_H_
+#define SimTK_SIMBODY_CONSTRAINT_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) 2007-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+Private implementation of Constraint and its included subclasses which
+represent the built-in constraint types. **/
+
+#include "SimTKmath.h"
+#include "simbody/internal/common.h"
+#include "simbody/internal/Constraint.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "simbody/internal/SimbodyMatterSubtree.h"
+
+#include "SimbodyTreeState.h"
+
+#include <map>
+#include <utility> // std::pair
+#include <iostream>
+using std::cout; using std::endl;
+
+class SimbodyMatterSubsystemRep;
+
+namespace SimTK {
+
+class SimbodyMatterSubsystem;
+class SimbodyMatterSubtree;
+class MobilizedBody;
+
+//==============================================================================
+// CONSTRAINT IMPL
+//==============================================================================
+// This is what a Constraint handle points to.
+class ConstraintImpl : public PIMPLImplementation<Constraint, ConstraintImpl> {
+public:
+
+ConstraintImpl()
+ : myMatterSubsystemRep(0),
+ defaultMp(0), defaultMv(0), defaultMa(0), defaultDisabled(false),
+ myAncestorBodyIsNotGround(false)
+{
+}
+virtual ~ConstraintImpl() { }
+virtual ConstraintImpl* clone() const = 0;
+
+ConstraintImpl(int mp, int mv, int ma)
+ : myMatterSubsystemRep(0),
+ defaultMp(mp), defaultMv(mv), defaultMa(ma), defaultDisabled(false),
+ myAncestorBodyIsNotGround(false)
+{
+}
+
+void setDefaultNumConstraintEquations(int mp, int mv, int ma) {
+ assert(mp >= 0 && mv >= 0 && ma >= 0);
+ invalidateTopologyCache();
+ defaultMp = mp;
+ defaultMv = mv;
+ defaultMa = ma;
+}
+
+void getDefaultNumConstraintEquations(int& mp, int& mv, int& ma) const {
+ mp = defaultMp;
+ mv = defaultMv;
+ ma = defaultMa;
+}
+
+void setDisabledByDefault(bool shouldBeDisabled) {
+ invalidateTopologyCache();
+ defaultDisabled = shouldBeDisabled;
+}
+
+bool isDisabledByDefault() const {
+ return defaultDisabled;
+}
+
+void setDisabled(State& s, bool shouldBeDisabled) const ;
+bool isDisabled(const State& s) const;
+
+typedef std::map<MobilizedBodyIndex,ConstrainedBodyIndex>
+ MobilizedBody2ConstrainedBodyMap;
+typedef std::map<MobilizedBodyIndex,ConstrainedMobilizerIndex>
+ MobilizedBody2ConstrainedMobilizerMap;
+
+// Call this during construction phase to add a body to the topological
+// structure of this Constraint. This body's mobilizer's mobilities are *not*
+// part of the constraint; mobilizers must be added separately. If this
+// mobilized body has been seen as a constrained body before we'll return the
+// same index as first assigned to it.
+ConstrainedBodyIndex addConstrainedBody(const MobilizedBody&);
+
+// Call this during construction phase to add a mobilizer to the topological
+// structure of this Constraint. All the coordinates q and mobilities u for
+// this mobilizer are added as "constrainable". We don't know how many
+// coordinates and speeds there are until Stage::Model. If this
+// mobilized body has been seen as a constrained mobilizer before we'll return
+// the same index as first assigned to it.
+ConstrainedMobilizerIndex addConstrainedMobilizer(const MobilizedBody&);
+
+MobilizedBodyIndex getMobilizedBodyIndexOfConstrainedBody
+ (ConstrainedBodyIndex c) const {
+ assert(0 <= c && c < (int)myConstrainedBodies.size());
+ return myConstrainedBodies[c];
+}
+MobilizedBodyIndex getMobilizedBodyIndexOfConstrainedMobilizer
+ (ConstrainedMobilizerIndex c) const {
+ assert(0 <= c && c < (int)myConstrainedMobilizers.size());
+ return myConstrainedMobilizers[c];
+}
+
+//TODO: Constraint-local State allocation
+//int allocateDiscreteVariable(State& s, Stage g, AbstractValue* v) const;
+//int allocateCacheEntry(State& s, Stage g, AbstractValue* v) const;
+
+QIndex getQIndexOfConstrainedQ(const State& s, ConstrainedQIndex cqx) const;
+UIndex getUIndexOfConstrainedU(const State& s, ConstrainedUIndex cqx) const;
+
+void convertQForcesToUForces(const State& s,
+ const Array_<Real,ConstrainedQIndex>& qForces,
+ Array_<Real,ConstrainedUIndex>& uForces) const;
+
+// Given a position-stage state and an array of body velocities for all
+// bodies (relative to Ground), select the short list of constrained bodies
+// for this Constraint and ensure that their velocities are relative to
+// the Ancestor body.
+void convertBodyVelocityToConstrainedBodyVelocity
+ (const State& state,
+ const Array_<SpatialVec, MobilizedBodyIndex>& V_GB,
+ Array_<SpatialVec, ConstrainedBodyIndex>& V_AB) const;
+
+// Given a velocity-stage state and an array of body accelerations for all
+// bodies (relative to Ground), select the short list of constrained bodies
+// for this Constraint and ensure that their accelerations are relative to
+// the Ancestor body.
+void convertBodyAccelToConstrainedBodyAccel
+ (const State& state,
+ const Array_<SpatialVec, MobilizedBodyIndex>& A_GB,
+ Array_<SpatialVec, ConstrainedBodyIndex>& A_AB) const;
+
+void realizeTopology(State&) const; // eventually calls realizeTopologyVirtual()
+void realizeModel (State&) const; // eventually calls realizeModelVirtual()
+void realizeInstance(const State&) const; // eventually calls realizeInstanceVirtual()
+
+// These are called in loops over all the Constraints from the
+// SimbodyMatterSubsystem realize() methods, which will have already
+// deconstructed the State into an SBStateDigest object.
+void realizeTime (const SBStateDigest&) const; // eventually calls realizeTimeVirtual()
+void realizePosition(const SBStateDigest&) const; // eventually calls realizePositionVirtual()
+void realizeVelocity(const SBStateDigest&) const; // eventually calls realizeVelocityVirtual()
+void realizeDynamics(const SBStateDigest&) const; // eventually calls realizeDynamicsVirtual()
+void realizeAcceleration
+ (const SBStateDigest&) const; // eventually calls realizeAccelerationVirtual()
+void realizeReport (const State&) const; // eventually calls realizeReportVirtual()
+
+// Given a state realized to Position stage, extract the position constraint
+// errors corresponding to this Constraint. The 'mp' argument is for sanity
+// checking -- it is an error if that isn't an exact match for the current
+// number of holonomic constraint equations generated by this Constraint. We
+// expect that perr points to an array of at least mp elements that we can
+// write on.
+void getPositionErrors(const State& s, int mp, Real* perr) const;
+
+// Given a State realized to Velocity stage, extract the velocity constraint
+// errors corresponding to this Constraint. This includes velocity constraints
+// which were produced by differentiation of holonomic (position) constraints,
+// and nonholonomic constraints which are introduced at the velocity level. The
+// 'mpv' argument is for sanity checking -- it is an error if that isn't an
+// exact match for the current number of holonomic+nonholonomic (mp+mv)
+// constraint equations generated by this Constraint. We expect that pverr
+// points to an array of at least mp+mv elements that we can write on.
+void getVelocityErrors(const State& s, int mpv, Real* pverr) const;
+
+// Given a State realized to Acceleration stage, extract the accleration
+// constraint errors corresponding to this Constraint. This includes
+// acceleration constraints which were produced by twice differentiation of
+// holonomic (position) constraints, and differentiation of nonholonomic
+// (velocity) constraints, and acceleration-only constraints which are first
+// introduced at the acceleration level. The 'mpva' argument is for sanity
+// checking -- it is an error if that isn't an exact match for the current
+// number of holonomic+nonholonomic+accelerationOnly (mp+mv+ma) constraint
+// equations generated by this Constraint. We expect that pvaerr points to an
+// array of at least mp+mv+ma elements that we can write on.
+void getAccelerationErrors(const State& s, int mpva, Real* pvaerr) const;
+
+// Given a State realized to Acceleration stage, extract the constraint
+// multipliers lambda corresponding to this constraint. This includes
+// multipliers for all the holonomic, nonholonomic, and acceleration-only
+// constraints (but not quaternion constraints which do not use multipliers).
+// The 'mpva' argument is for sanity checking -- it is an error if that isn't
+// an exact match for the current number (mp+mv+ma) of constraint equations
+// generated by this Constraint. We expect that lambda points to an array of at
+// least mp+mv+ma elements that we can write on.
+void getMultipliers(const State& s, int mpva, Real* lambda) const;
+
+// Return a small, writable array directly referencing the segment of the longer
+// passed-in array that belongs to this constraint. State must be realized
+// through Instance stage.
+ArrayView_<SpatialVec,ConstrainedBodyIndex>
+updConstrainedBodyForces(const State& state,
+ Array_<SpatialVec>& allConsBodyForces) const;
+// Same, but for mobility forces.
+ArrayView_<Real,ConstrainedUIndex>
+updConstrainedMobilityForces(const State& state,
+ Array_<Real>& allConsMobForces) const;
+
+// Return a const reference to our segment. Can use above methods efficiently
+// since ArrayView is derived from ArrayViewConst; this just does a
+// shallow copy to fill in the ArrayViewConst handle; no heap is involved.
+ArrayViewConst_<SpatialVec,ConstrainedBodyIndex>
+getConstrainedBodyForces(const State& state,
+ const Array_<SpatialVec>& allConsBodyForces) const
+{
+ return updConstrainedBodyForces
+ (state, const_cast<Array_<SpatialVec>&>(allConsBodyForces));
+}
+ArrayViewConst_<Real,ConstrainedUIndex>
+getConstrainedMobilityForces(const State& state,
+ const Array_<Real>& allConsMobForces) const
+{
+ return updConstrainedMobilityForces
+ (state, const_cast<Array_<Real>&>(allConsMobForces));
+}
+
+// Same as above but we use the matter subsystem's constrained acceleration
+// cache as the source of the full-sized constrained body forces array,
+// where the forces are expressed in Ground. That cache entry must have been
+// realized which occurs during Acceleration stage computations.
+ArrayViewConst_<SpatialVec,ConstrainedBodyIndex>
+getConstrainedBodyForcesInGFromState(const State& state) const
+{
+ const SBConstrainedAccelerationCache& cac =
+ getConstrainedAccelerationCache(state);
+ return getConstrainedBodyForces(state, cac.constrainedBodyForcesInG);
+}
+
+ArrayView_<SpatialVec,ConstrainedBodyIndex>
+updConstrainedBodyForcesInGFromState(const State& state) const
+{
+ SBConstrainedAccelerationCache& cac =
+ updConstrainedAccelerationCache(state);
+ return updConstrainedBodyForces(state, cac.constrainedBodyForcesInG);
+}
+
+ArrayViewConst_<Real,ConstrainedUIndex>
+getConstrainedMobilityForcesFromState(const State& state) const
+{
+ const SBConstrainedAccelerationCache& cac =
+ getConstrainedAccelerationCache(state);
+ return getConstrainedMobilityForces(state, cac.constraintMobilityForces);
+}
+
+ArrayView_<Real,ConstrainedUIndex>
+updConstrainedMobilityForcesFromState(const State& state) const
+{
+ SBConstrainedAccelerationCache& cac =
+ updConstrainedAccelerationCache(state);
+ return getConstrainedMobilityForces(state, cac.constraintMobilityForces);
+}
+
+// Given a State and a set of m multipliers lambda,
+// calculate in O(m) time the constraint forces (body forces and torques and
+// u-space mobility forces) which would be generated by those multipliers. You can
+// restrict this to P,V,A subsets setting mp, mv, or ma to zero; in any case
+// m = mp+mv+ma and any non-zero segments must match the corresponding number
+// of constraint equations of that type.
+//
+// The State must be realized to at least Position stage, and if you ask for
+// forces from holonomic (mv>0) or acceleration-only (ma>0) constraints then
+// the State will have to be realized to Velocity stage if the corresponding
+// constraint equations are velocity dependent.
+void calcConstraintForcesFromMultipliers
+ (const State& s,
+ const Array_<Real>& lambdap, // 0 or mp of these
+ const Array_<Real>& lambdav, // 0 or mv of these
+ const Array_<Real>& lambdaa, // 0 or ma of these
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedUIndex>& mobilityForces) const
+{
+ int actual_mp,actual_mv,actual_ma;
+ getNumConstraintEquationsInUse(s, actual_mp, actual_mv, actual_ma);
+
+ bodyForcesInA.resize(getNumConstrainedBodies());
+ bodyForcesInA.fill(SpatialVec(Vec3(0), Vec3(0)));
+ mobilityForces.resize(getNumConstrainedU(s));
+ mobilityForces.fill(Real(0));
+
+ if (lambdap.size()) {
+ assert(lambdap.size() == actual_mp);
+ const int ncq = getNumConstrainedQ(s);
+ const int ncu = getNumConstrainedU(s);
+ // State need only be realized to Position stage
+ Array_<Real, ConstrainedQIndex> qForces(ncq, Real(0));
+ Array_<Real, ConstrainedUIndex> uForces(ncu);
+ addInPositionConstraintForces(s, lambdap,
+ bodyForcesInA, qForces);
+ convertQForcesToUForces(s, qForces, uForces);
+ for (ConstrainedUIndex cux(0); cux < ncu; ++cux)
+ mobilityForces[cux] += uForces[cux];
+ }
+ if (lambdav.size()) {
+ assert(lambdav.size() == actual_mv);
+ // State may need to be realized to Velocity stage
+ addInVelocityConstraintForces(s, lambdav,
+ bodyForcesInA, mobilityForces);
+ }
+ if (lambdaa.size()) {
+ assert(lambdaa.size() == actual_ma);
+ // State may need to be realized to Velocity stage
+ addInAccelerationConstraintForces(s, lambdaa,
+ bodyForcesInA, mobilityForces);
+ }
+}
+
+// Given a State realized to Position stage, and a set of spatial forces applied
+// to the constrained bodies and u-space generalized forces applied to the
+// constrained mobilities, convert these to an equivalent set
+// of n generalized forces applied at each of the participating mobilities, in
+// O(n) time.
+// TODO
+void convertConstraintForcesToGeneralizedForces(const State& s,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ const Array_<Real, ConstrainedUIndex>& mobilityForces,
+ Vector& generalizedForces) const
+{
+ // TODO
+ assert(!"convertConstraintForcesToGeneralizedForces: not implemented yet");
+}
+
+// Calculate f = ~G*lambda in O(n+m) time. ~G=[~P ~V ~A] and you can work with
+// any subblock or combination by setting some of mp,mv,ma to zero. If nonzero
+// they have to match the actual number of holonomic, nonholonomic, and
+// acceleration-only constraints. Vector lambda (typically Lagrange multipliers
+// but not necessarily) is segmented lambda=[mp|mv|ma] where some of the
+// segments can be empty.
+void calcGTransposeLambda
+ (const State& s,
+ const Array_<Real>& lambdap, // 0 or mp of these
+ const Array_<Real>& lambdav, // 0 or mv of these
+ const Array_<Real>& lambdaa, // 0 or ma of these
+ Vector& f) const
+{
+ Array_<SpatialVec,ConstrainedBodyIndex> bodyForcesInA;
+ Array_<Real, ConstrainedUIndex> mobilityForces;
+
+ calcConstraintForcesFromMultipliers
+ (s, lambdap, lambdav, lambdaa, bodyForcesInA, mobilityForces);
+ convertConstraintForcesToGeneralizedForces
+ (s, bodyForcesInA, mobilityForces, f);
+}
+
+// Find the indicated cache in the passed-in State. The "get" methods require
+// that the cache entry has already been marked valid.
+
+const SBModelCache& getModelCache(const State&) const;
+const SBInstanceCache& getInstanceCache(const State&) const;
+const SBTreePositionCache& getTreePositionCache(const State&) const;
+const SBTreeVelocityCache& getTreeVelocityCache(const State&) const;
+const SBTreeAccelerationCache& getTreeAccelerationCache(const State&) const;
+const SBConstrainedAccelerationCache&
+ getConstrainedAccelerationCache(const State& s) const;
+SBConstrainedAccelerationCache&
+ updConstrainedAccelerationCache(const State& s) const;
+
+ // Methods for use with ConstrainedMobilizers.
+
+Real getOneQFromState
+ (const State&, ConstrainedMobilizerIndex, MobilizerQIndex) const;
+Real getOneQDotFromState
+ (const State&, ConstrainedMobilizerIndex, MobilizerQIndex) const;
+Real getOneQDotDotFromState
+ (const State&, ConstrainedMobilizerIndex, MobilizerQIndex) const;
+
+Real getOneUFromState
+ (const State&, ConstrainedMobilizerIndex, MobilizerUIndex) const;
+Real getOneUDotFromState
+ (const State&, ConstrainedMobilizerIndex, MobilizerUIndex) const;
+
+// Analogous methods for use when the generalized coordinate has been provided
+// as an operand. The state is still necessary for modeling information.
+Real getOneQ(const State& s,
+ const Array_<Real,ConstrainedQIndex>& cq,
+ ConstrainedMobilizerIndex M,
+ MobilizerQIndex whichQ) const
+{
+ assert(cq.size() == getNumConstrainedQ(s));
+ assert(0 <= whichQ && whichQ < getNumConstrainedQ(s, M));
+ return cq[getConstrainedQIndex(s,M,whichQ)];
+}
+Real getOneQDot(const State& s,
+ const Array_<Real,ConstrainedQIndex>& cqdot,
+ ConstrainedMobilizerIndex M,
+ MobilizerQIndex whichQ) const
+{
+ assert(cqdot.size() == getNumConstrainedQ(s));
+ assert(0 <= whichQ && whichQ < getNumConstrainedQ(s, M));
+ return cqdot[getConstrainedQIndex(s,M,whichQ)];
+}
+Real getOneQDotDot(const State& s,
+ const Array_<Real,ConstrainedQIndex>& cqdotdot,
+ ConstrainedMobilizerIndex M,
+ MobilizerQIndex whichQ) const
+{
+ assert(cqdotdot.size() == getNumConstrainedQ(s));
+ assert(0 <= whichQ && whichQ < getNumConstrainedQ(s, M));
+ return cqdotdot[getConstrainedQIndex(s,M,whichQ)];
+}
+
+Real getOneU(const State& s,
+ const Array_<Real,ConstrainedUIndex>& cu,
+ ConstrainedMobilizerIndex M,
+ MobilizerUIndex whichU) const
+{
+ assert(cu.size() == getNumConstrainedU(s));
+ assert(0 <= whichU && whichU < getNumConstrainedU(s, M));
+ return cu[getConstrainedUIndex(s,M,whichU)];
+}
+
+Real getOneUDot(const State& s,
+ const Array_<Real,ConstrainedUIndex>& cudot,
+ ConstrainedMobilizerIndex M,
+ MobilizerUIndex whichU) const
+{
+ assert(cudot.size() == getNumConstrainedU(s));
+ assert(0 <= whichU && whichU < getNumConstrainedU(s, M));
+ return cudot[getConstrainedUIndex(s,M,whichU)];
+}
+
+// Apply a u-space (mobility) generalized force fu to a particular mobility of
+// the given constrained mobilizer, adding it in to the appropriate slot of the
+// mobilityForces vector which is of length numConstrainedU for this Constraint.
+void addInOneMobilityForce
+ (const State& s,
+ ConstrainedMobilizerIndex cmx,
+ MobilizerUIndex whichU,
+ Real fu,
+ Array_<Real,ConstrainedUIndex>& mobilityForces) const
+{
+ assert(mobilityForces.size() == getNumConstrainedU(s));
+ assert(0 <= whichU && whichU < getNumConstrainedU(s, cmx));
+ mobilityForces[getConstrainedUIndex(s,cmx,whichU)] += fu;
+}
+
+// Apply a q-space generalized force fq to a particular generalized coordinate q
+// of the given constrained mobilizer, adding it in to the appropriate slot of
+// the qForces vector which is of length numConstrainedQ for this Constraint.
+void addInOneQForce
+ (const State& s,
+ ConstrainedMobilizerIndex cmx,
+ MobilizerQIndex whichQ,
+ Real fq,
+ Array_<Real,ConstrainedQIndex>& qForces) const
+{
+ assert(qForces.size() == getNumConstrainedQ(s));
+ assert(0 <= whichQ && whichQ < getNumConstrainedQ(s, cmx));
+ qForces[getConstrainedQIndex(s,cmx,whichQ)] += fq;
+}
+
+ // Methods for use with ConstrainedBodies.
+
+// These are used to retrieve the indicated values from the State cache, with all values
+// measured and expressed in the Ancestor (A) frame.
+const Transform& getBodyTransformFromState
+ (const State& s, ConstrainedBodyIndex B) const; // X_AB
+const SpatialVec& getBodyVelocityFromState
+ (const State& s, ConstrainedBodyIndex B) const; // V_AB
+
+// Extract just the rotational quantities from the spatial quantities above.
+const Rotation& getBodyRotationFromState
+ (const State& s, ConstrainedBodyIndex B) const // R_AB
+ {return getBodyTransformFromState(s,B).R();}
+const Vec3& getBodyAngularVelocityFromState
+ (const State& s, ConstrainedBodyIndex B) const // w_AB
+ {return getBodyVelocityFromState(s,B)[0];}
+
+// Extract just the translational (linear) quantities from the spatial quantities above.
+const Vec3& getBodyOriginLocationFromState
+ (const State& s, ConstrainedBodyIndex B) const // p_AB
+ {return getBodyTransformFromState(s,B).p();}
+const Vec3& getBodyOriginVelocityFromState
+ (const State& s, ConstrainedBodyIndex B) const // v_AB
+ {return getBodyVelocityFromState(s,B)[1];}
+
+// These are analogous methods for when you've been given X_AB, V_AB, or A_AB
+// explicitly as an operand. These are all inline and trivial.
+const Transform& getBodyTransform
+ (const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
+ ConstrainedBodyIndex B) const
+{ return allX_AB[B]; }
+
+const SpatialVec& getBodyVelocity
+ (const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
+ ConstrainedBodyIndex B) const
+{ return allV_AB[B]; }
+
+const SpatialVec& getBodyAcceleration
+ (const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
+ ConstrainedBodyIndex B) const
+{ return allA_AB[B]; }
+
+const Rotation& getBodyRotation
+ (const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
+ ConstrainedBodyIndex B) const
+{ return allX_AB[B].R(); }
+
+const Vec3& getBodyAngularVelocity
+ (const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
+ ConstrainedBodyIndex B) const
+{ return allV_AB[B][0]; }
+
+const Vec3& getBodyAngularAcceleration
+ (const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
+ ConstrainedBodyIndex B) const
+{ return allA_AB[B][0]; }
+
+const Vec3& getBodyOriginLocation
+ (const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
+ ConstrainedBodyIndex B) const
+{ return allX_AB[B].p(); }
+
+const Vec3& getBodyOriginVelocity
+ (const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
+ ConstrainedBodyIndex B) const
+{ return allV_AB[B][1]; }
+
+const Vec3& getBodyOriginAcceleration
+ (const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
+ ConstrainedBodyIndex B) const
+{ return allA_AB[B][1]; }
+
+
+// Given a station on body B, find its location measured and expressed in the
+// Ancestor's frame A. 18 flops.
+Vec3 findStationLocationFromState
+ (const State& s, ConstrainedBodyIndex B, const Vec3& p_B) const {
+ return getBodyTransformFromState(s,B) * p_B; // re-measure and re-express
+}
+
+// Same, but we're given the constrained body poses as an operand.
+Vec3 findStationLocation
+ (const Array_<Transform, ConstrainedBodyIndex>& allX_AB,
+ ConstrainedBodyIndex B, const Vec3& p_B) const {
+ const Transform& X_AB = allX_AB[B];
+ return X_AB * p_B; // re-measure and re-express
+}
+
+// Given a station on body B, find its velocity measured and expressed in the
+// Ancestor's frame A. 27 flops.
+Vec3 findStationVelocityFromState
+ (const State& s, ConstrainedBodyIndex B, const Vec3& p_B) const {
+ // p_B rexpressed in A but not shifted to Ao
+ const Vec3 p_A = getBodyRotationFromState(s,B) * p_B;
+ const SpatialVec& V_AB = getBodyVelocityFromState(s,B);
+ return V_AB[1] + (V_AB[0] % p_A);
+}
+
+// Same, but only configuration comes from state; velocities are an operand.
+Vec3 findStationVelocity
+ (const State& s,
+ const Array_<SpatialVec, ConstrainedBodyIndex>& allV_AB,
+ ConstrainedBodyIndex B, const Vec3& p_B) const
+{
+ // p_B rexpressed in A but not shifted to Ao
+ const Vec3 p_A = getBodyRotationFromState(s,B) * p_B;
+ const SpatialVec& V_AB = allV_AB[B];
+ return V_AB[1] + (V_AB[0] % p_A);
+}
+
+// There is no findStationAccelerationFromState().
+
+// Same, but only configuration and velocity come from state; accelerations
+// are an operand.
+Vec3 findStationAcceleration
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
+ ConstrainedBodyIndex bodyB,
+ const Vec3& p_BS) const
+{ // p_BS_A is p_BS rexpressed in A but not shifted to Ao
+ const Rotation& R_AB = getBodyRotationFromState(state,bodyB);
+ const Vec3 p_BS_A = R_AB * p_BS;
+ const Vec3& w_AB = getBodyAngularVelocityFromState(state,bodyB);
+ const SpatialVec& A_AB = allA_AB[bodyB];
+
+ // Result is a + b X r + w X (w X r).
+ // ("b" is angular acceleration; w is angular velocity).
+ const Vec3 a_AS = A_AB[1] + (A_AB[0] % p_BS_A)
+ + w_AB % (w_AB % p_BS_A); // % is not associative
+ return a_AS;
+}
+
+// Apply an Ancestor-frame force to a B-frame station, adding it to the
+// appropriate bodyForcesInA entry, where bodyForcesInA is *already* size
+// numConstrainedBodies for this Constraint. 30 flops.
+void addInStationForce(const State& s,
+ ConstrainedBodyIndex B, const Vec3& p_B,
+ const Vec3& forceInA,
+ Array_<SpatialVec, ConstrainedBodyIndex>& bodyForcesInA)
+ const
+{
+ assert(bodyForcesInA.size() == getNumConstrainedBodies());
+ const Rotation& R_AB = getBodyRotationFromState(s,B);
+ bodyForcesInA[B] += SpatialVec((R_AB*p_B) % forceInA, forceInA); // rXf, f
+}
+
+// Apply an Ancestor-frame torque to body B, updating the appropriate
+// bodyForcesInA entry, where bodyForcesInA is *already* size
+// numConstrainedBodies for this Constraint. 3 flops.
+void addInBodyTorque(const State& s, ConstrainedBodyIndex B,
+ const Vec3& torqueInA,
+ Array_<SpatialVec, ConstrainedBodyIndex>& bodyForcesInA)
+ const
+{
+ assert(bodyForcesInA.size() == getNumConstrainedBodies());
+ bodyForcesInA[B][0] += torqueInA; // no force
+}
+
+// After realizeTopology() we can look at the values of modeling variables in
+// the State. A Constraint is free to use those in determining how many
+// constraint equations of each type to generate. The default implementation
+// doesn't look at the state but instead returns the default numbers of
+// equations supplied when the Constraint was constructed.
+void calcNumConstraintEquationsInUse(const State& s, int& mp, int& mv, int& ma) const {
+ calcNumConstraintEquationsInUseVirtual(s,mp,mv,ma);
+}
+
+// Abbreviated version of the above; returns number of holonomic constraint
+// equations in use.
+int calcNumPositionEquationsInUse(const State& s) const {
+ int mp, mv, ma; calcNumConstraintEquationsInUse(s, mp, mv, ma);
+ return mp;
+}
+// Abbreviated version of the above; returns number of nonholonomic constraint
+// equations in use.
+int calcNumVelocityEquationsInUse(const State& s) const {
+ int mp, mv, ma; calcNumConstraintEquationsInUse(s, mp, mv, ma);
+ return mv;
+}
+// Abbreviated version of the above; returns number of acceleration-only
+// constraint equations in use.
+int calcNumAccelerationEquationsInUse(const State& s) const {
+ int mp, mv, ma; calcNumConstraintEquationsInUse(s, mp, mv, ma);
+ return ma;
+}
+
+ // The next three methods are just interfaces to the constraint
+ // operators like calcPositionErrors(); they extract needed operands
+ // from the supplied state and then call the operator.
+
+// Calculate the mp position errors that would result from the configuration
+// present in the supplied state (that is, q's and body transforms). The state
+// must be realized through Time stage and part way through realization of
+// Position stage.
+void calcPositionErrorsFromState(const State& s, Array_<Real>& perr) const;
+
+// Calculate the mp velocity errors resulting from pdot equations, given a
+// configuration and velocities in the supplied state which must be realized
+// through Position stage and part way through realization of Velocity stage.
+void calcPositionDotErrorsFromState(const State& s, Array_<Real>& pverr) const;
+
+// Calculate the mv velocity errors resulting from the nonholonomic constraint
+// equations here, taking the configuration and velocities (u, qdot, body
+// spatial velocities) from the supplied state, which must be realized through
+// Position stage and part way through realization of Velocity stage.
+void calcVelocityErrorsFromState(const State& s, Array_<Real>& verr) const;
+
+// Calculate position errors given pose of the constrained bodies and the
+// values of the constrained q's. Pull t from state.
+void calcPositionErrors
+ (const State& s, // Stage::Time
+ const Array_<Transform,ConstrainedBodyIndex>& X_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr) // mp of these
+ const
+{
+ assert(X_AB.size() == getNumConstrainedBodies());
+ assert(constrainedQ.size() == getNumConstrainedQ(s));
+ assert(perr.size() == calcNumPositionEquationsInUse(s));
+
+ calcPositionErrorsVirtual(s,X_AB,constrainedQ,perr);
+}
+
+// Calculate pdot errors given spatial velocity of the constrained bodies and
+// the values of the constrained qdot's. Pull t, X_AB and q from state.
+void calcPositionDotErrors
+ (const State& s, // Stage::Position
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr) // mp of these
+ const
+{
+ assert(V_AB.size() == getNumConstrainedBodies());
+ assert(constrainedQDot.size() == getNumConstrainedQ(s));
+ assert(pverr.size() == calcNumPositionEquationsInUse(s));
+
+ calcPositionDotErrorsVirtual(s,V_AB,constrainedQDot,pverr);
+}
+
+// Calculate pdotdot errors given spatial acceleration of the constrained
+// bodies and the values of the constrained qdotdot's. Pull t, X_AB, q, V_AB,
+// qdot from state.
+void calcPositionDotDotErrors
+ (const State& s, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr) // mp of these
+ const
+{
+ assert(A_AB.size() == getNumConstrainedBodies());
+ assert(constrainedQDotDot.size() == getNumConstrainedQ(s));
+ assert(paerr.size() == calcNumPositionEquationsInUse(s));
+
+ calcPositionDotDotErrorsVirtual(s,A_AB,constrainedQDotDot,paerr);
+}
+
+// Given mp position constraint multipliers, generate the corresponding
+// constraint forces acting on the constrained bodies and the generalized
+// coordinates q of the constrained mobilizers and add them in to the given
+// arrays. The state must be realized through Position stage.
+void addInPositionConstraintForces
+ (const State& s,
+ const Array_<Real>& multipliers, // mp of these
+ Array_<SpatialVec, ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedQIndex>& qForces) const
+{
+ assert(multipliers.size() == calcNumPositionEquationsInUse(s));
+ assert(bodyForcesInA.size() == getNumConstrainedBodies());
+ assert(qForces.size() == getNumConstrainedQ(s));
+
+ // Note that position constraints act on q (qdot,qdotdot) and for
+ // convenience we allow them to generate generalized forces in qdot-space
+ // rather than u-space. These will have to be mapped into u-space when
+ // they are used, since Simbody only deals in u-space forces normally.
+ // Since qdot=N*u, and we must have power ~f_qdot*qdot==~f_u*u, we have
+ // f_u=~N * f_qdot.
+ addInPositionConstraintForcesVirtual
+ (s,multipliers,bodyForcesInA,qForces);
+}
+
+// Calculate velocity errors (errors produced by nonholonomic constraint
+// equations) given spatial velocity of the constrained bodies and the
+// values of the constrained u's. Pull t, X_AB, q from state.
+void calcVelocityErrors
+ (const State& s, // Stage::Position
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedU,
+ Array_<Real>& verr) // mv of these
+ const
+{
+ assert(V_AB.size() == getNumConstrainedBodies());
+ assert(constrainedU.size() == getNumConstrainedU(s));
+ assert(verr.size() == calcNumVelocityEquationsInUse(s));
+
+ calcVelocityErrorsVirtual(s,V_AB,constrainedU,verr);
+}
+
+// Calculate vdot errors (errors produced by nonholonomic constraint
+// derivatives) given spatial acceleration of the constrained bodies and the
+// values of the constrained udot's. Pull t, X_AB, q, V_AB, u from state.
+void calcVelocityDotErrors
+ (const State& s, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedUDot,
+ Array_<Real>& vaerr) // mv of these
+ const
+{
+ assert(A_AB.size() == getNumConstrainedBodies());
+ assert(constrainedUDot.size() == getNumConstrainedU(s));
+ assert(vaerr.size() == calcNumVelocityEquationsInUse(s));
+
+ calcVelocityDotErrorsVirtual(s,A_AB,constrainedUDot,vaerr);
+}
+
+
+// Given mv velocity constraint multipliers, generate the corresponding
+// constraint forces acting on the constrained bodies and the mobilities of the
+// constrained mobilizers, and add them in to the given arrays. The state must
+// be realized through Velocity stage unless the V matrix is
+// velocity-independent in which case Position stage is enough.
+void addInVelocityConstraintForces
+ (const State& s,
+ const Array_<Real>& multipliers, // mv of these
+ Array_<SpatialVec, ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedUIndex>& mobilityForces) const
+{
+ assert(multipliers.size() == calcNumVelocityEquationsInUse(s));
+ assert(bodyForcesInA.size() == getNumConstrainedBodies());
+ assert(mobilityForces.size() == getNumConstrainedU(s));
+
+ addInVelocityConstraintForcesVirtual
+ (s,multipliers,bodyForcesInA,mobilityForces);
+}
+
+// Calculate acceleration errors (errors produced by acceleration-only
+// constraint equations) given spatial acceleration of the constrained bodies
+// and the values of the constrained udot's. Pull t, X_AB, q, V_AB, u from state.
+void calcAccelerationErrors
+ (const State& s, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedUDot,
+ Array_<Real>& aerr) // ma of these
+ const
+{
+ assert(A_AB.size() == getNumConstrainedBodies());
+ assert(constrainedUDot.size() == getNumConstrainedU(s));
+ assert(aerr.size() == calcNumAccelerationEquationsInUse(s));
+
+ calcAccelerationErrorsVirtual(s,A_AB,constrainedUDot,aerr);
+}
+
+
+// Given ma acceleration constraint multipliers, generate the corresponding
+// constraint forces acting on the constrained bodies and the mobilities of the
+// constrained mobilizers, and add them in to the given arrays. The state must
+// be realized through Velocity stage unless the A matrix is
+// velocity-independent in which case Position stage is enough.
+void addInAccelerationConstraintForces
+ (const State& s,
+ const Array_<Real>& multipliers, // ma of these
+ Array_<SpatialVec, ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedUIndex>& mobilityForces) const
+{
+ assert(multipliers.size() == calcNumAccelerationEquationsInUse(s));
+ assert(bodyForcesInA.size() == getNumConstrainedBodies());
+ assert(mobilityForces.size() == getNumConstrainedU(s));
+
+ addInAccelerationConstraintForcesVirtual
+ (s,multipliers,bodyForcesInA,mobilityForces);
+}
+
+void calcDecorativeGeometryAndAppend
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+{
+ // Let the individual constraint deal with any complicated stuff.
+ calcDecorativeGeometryAndAppendVirtual(s,stage,geom);
+}
+
+ // Don't call these virtuals directly; use the provided interface
+ // methods for safety (they evaporate in Release builds anyway).
+
+virtual void calcNumConstraintEquationsInUseVirtual
+ (const State&, int& mp, int& mv, int& ma) const
+{ mp = defaultMp; mv = defaultMv; ma = defaultMa; }
+
+virtual void realizeTopologyVirtual (State&) const {}
+virtual void realizeModelVirtual (State&) const {}
+virtual void realizeInstanceVirtual (const State&) const {}
+virtual void realizeTimeVirtual (const State&) const {}
+virtual void realizePositionVirtual (const State&) const {}
+virtual void realizeVelocityVirtual (const State&) const {}
+virtual void realizeDynamicsVirtual (const State&) const {}
+virtual void realizeAccelerationVirtual (const State&) const {}
+virtual void realizeReportVirtual (const State&) const {}
+
+ // These must be defined if there are any position (holonomic) constraints.
+
+// Pull t from state.
+virtual void calcPositionErrorsVirtual
+ (const State& state, // Stage::Time
+ const Array_<Transform,ConstrainedBodyIndex>& X_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr) // mp of these
+ const;
+
+// Pull t, X_AB and q from state.
+virtual void calcPositionDotErrorsVirtual
+ (const State& state, // Stage::Position
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr) // mp of these
+ const;
+
+// Pull t, X_AB, q, V_AB, qdot from state.
+virtual void calcPositionDotDotErrorsVirtual
+ (const State& state, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr) // mp of these
+ const;
+
+// Pull t, X_AB and q from state.
+virtual void addInPositionConstraintForcesVirtual
+ (const State& state, // Stage::Position
+ const Array_<Real>& multipliers, // mp of these
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedQIndex>& qForces)
+ const;
+
+ // These must be defined if there are velocity (nonholonomic) constraints.
+
+// Pull t, X_AB, q from state.
+virtual void calcVelocityErrorsVirtual
+ (const State& state, // Stage::Position
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedU,
+ Array_<Real>& verr) // mv of these
+ const;
+
+// Pull t, X_AB, q, V_AB, u from state.
+virtual void calcVelocityDotErrorsVirtual
+ (const State& state, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedUDot,
+ Array_<Real>& vaerr) // mv of these
+ const;
+
+// Pull t, X_AB, q, V_AB, u from state.
+virtual void addInVelocityConstraintForcesVirtual
+ (const State& state, // Stage::Velocity
+ const Array_<Real>& multipliers, // mv of these
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedUIndex>& mobilityForces)
+ const;
+
+ // These must be defined if there are any acceleration-only constraints.
+
+// Pull t, X_AB, q, V_AB, u from state.
+virtual void calcAccelerationErrorsVirtual
+ (const State& state, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedUDot,
+ Array_<Real>& aerr) // ma of these
+ const;
+
+// Pull t, X_AB, q, V_AB, u from state.
+virtual void addInAccelerationConstraintForcesVirtual
+ (const State& state, // Stage::Velocity
+ const Array_<Real>& multipliers, // ma of these
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedUIndex>& mobilityForces)
+ const;
+
+
+virtual void calcDecorativeGeometryAndAppendVirtual
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const {}
+
+
+void invalidateTopologyCache() const;
+bool subsystemTopologyHasBeenRealized() const;
+
+void setMyMatterSubsystem(SimbodyMatterSubsystem& matter,
+ ConstraintIndex id);
+
+const SimbodyMatterSubsystem& getMyMatterSubsystem() const;
+
+bool isInSubsystem() const {
+ return myMatterSubsystemRep != 0;
+}
+
+// Is the supplied body in the same subsystem as this Constraint? (Returns
+// false also if either the Constraint or the MobilizedBody is not in a
+// subsystem.)
+bool isInSameSubsystem(const MobilizedBody& body) const;
+
+int getNumConstrainedBodies() const {
+ SimTK_ASSERT(subsystemTopologyHasBeenRealized(),
+ "Number of constrained bodies is not available until Topology stage has been realized.");
+ return (int)myConstrainedBodies.size();
+}
+int getNumConstrainedMobilizers() const {
+ SimTK_ASSERT(subsystemTopologyHasBeenRealized(),
+ "Number of constrained mobilizers is not available until Topology stage has been realized.");
+ return (int)myConstrainedMobilizers.size();
+}
+
+const MobilizedBody&
+ getMobilizedBodyFromConstrainedMobilizer(ConstrainedMobilizerIndex) const;
+const MobilizedBody&
+ getMobilizedBodyFromConstrainedBody(ConstrainedBodyIndex) const;
+
+// Don't call this unless there is at least one Constrained Body.
+const MobilizedBody& getAncestorMobilizedBody() const;
+
+// After realizeTopology() we remember whether this constraint's Ancestor
+// body is different from Ground.
+bool isAncestorDifferentFromGround() const {
+ SimTK_ASSERT(subsystemTopologyHasBeenRealized(),
+ "isAncestorDifferentFromGround(): must call realizeTopology() first.");
+ return myAncestorBodyIsNotGround;
+}
+
+// Find out how many holonomic (position), nonholonomic (velocity),
+// and acceleration-only constraint equations are generated by this Constraint
+// as currently modeled. State must be realized to Stage::Model.
+void getNumConstraintEquationsInUse
+ (const State&, int& mHolo, int& mNonholo, int& mAccOnly) const;
+
+// Return the starting index within the multiplier or udot error arrays
+// for each of the acceleration-level constraints produced by this Constraint.
+// Let holo0, nonholo0, accOnly0 be the first index of the slots assigned to
+// this Constraint's holonomic, nonholonomic, and acceleration-only constraints
+// within each block for that category. Then the returns here are:
+// px = holo0
+// vx = totalNumHolo + nonholo0
+// ax = totalNumHolo+totalNumNonholo + accOnly0
+// These are returned invalid if there are no constraint equations in that
+// category.
+void getIndexOfMultipliersInUse(const State& state,
+ MultiplierIndex& px0,
+ MultiplierIndex& vx0,
+ MultiplierIndex& ax0) const;
+
+void setMyPartInConstraintSpaceVector(const State& state,
+ const Vector& myPart,
+ Vector& constraintSpace) const;
+
+void getMyPartFromConstraintSpaceVector(const State& state,
+ const Vector& constraintSpace,
+ Vector& myPart) const;
+
+int getNumConstrainedQ(const State&) const;
+int getNumConstrainedU(const State&) const;
+int getNumConstrainedQ(const State&, ConstrainedMobilizerIndex) const;
+int getNumConstrainedU(const State&, ConstrainedMobilizerIndex) const;
+ConstrainedQIndex getConstrainedQIndex
+ (const State&, ConstrainedMobilizerIndex, MobilizerQIndex which) const;
+ConstrainedUIndex getConstrainedUIndex
+ (const State&, ConstrainedMobilizerIndex, MobilizerUIndex which) const;
+
+const SimbodyMatterSubsystemRep& getMyMatterSubsystemRep() const {
+ SimTK_ASSERT(myMatterSubsystemRep,
+ "Operation illegal on a Constraint that is not in a Subsystem.");
+ return *myMatterSubsystemRep;
+}
+SimbodyMatterSubsystemRep& updMyMatterSubsystemRep() {
+ SimTK_ASSERT(myMatterSubsystemRep,
+ "Operation illegal on a Constraint that is not in a Subsystem.");
+ return *myMatterSubsystemRep;
+}
+
+ConstraintIndex getMyConstraintIndex() const {
+ SimTK_ASSERT(myMatterSubsystemRep,
+ "Operation illegal on a Constraint that is not in a Subsystem.");
+ return myConstraintIndex;
+}
+
+
+// Calculate the transform X_AB of each ConstrainedBody in its Ancestor frame,
+// provided Ancestor!=Ground and A!=B. We expect a StateDigest of a State
+// that has been realized through Time stage, and a TreePositionCache in
+// which the mobilizer- and ground-frame position kinematics results have
+// already been calculated. We then fill in the missing ancestor-frame
+// results back into that same TreePositionCache. The TreePositionCache
+// in the StateDigest is ignored; it may or may not be the same as the
+// second argument depending on whether this is part of a realization or
+// part of an operator.
+void calcConstrainedBodyTransformInAncestor(const SBStateDigest&, // in only
+ SBTreePositionCache& // in/out
+ ) const;
+
+// Similarly we calculate V_AB during realizeVelocity().
+// Here we expect a StateDigest realized through Position stage, and a
+// partly-filled-in VelocityCache where we'll put V_AB for the
+// ConstrainedBodies.
+void calcConstrainedBodyVelocityInAncestor(const SBStateDigest&, // in only
+ SBTreeVelocityCache& // in/out
+ ) const;
+
+// A_AB is not cached.
+
+private:
+friend class Constraint;
+
+ // TOPOLOGY "STATE"
+
+// These data members are filled in once the Constraint is added to
+// a MatterSubsystem.
+SimbodyMatterSubsystemRep* myMatterSubsystemRep;
+ConstraintIndex myConstraintIndex; // id within the matter subsystem
+
+// We'll keep the constrained bodies and constrained mobilizers each in two
+// maps: one maps MobilizedBodyIndex->ConstrainedBody[Mobilizer]Index (O(log n)
+// to look up), and the other maps ConstrainedBody[Mobilizer]Index->
+// MobilizedBodyIndex (randomly addressable in constant time).
+MobilizedBody2ConstrainedBodyMap myMobilizedBody2ConstrainedBodyMap;
+MobilizedBody2ConstrainedMobilizerMap myMobilizedBody2ConstrainedMobilizerMap;
+
+Array_<MobilizedBodyIndex> myConstrainedBodies; // [ConstrainedBodyIndex]
+Array_<MobilizedBodyIndex> myConstrainedMobilizers; // [ConstrainedMobilizerIndex]
+
+
+// These are the defaults for the number of position (holonomic) constraint
+// equations, the number of velocity (nonholonomic) constraint equations, and
+// the number of acceleration-only constraint equations.
+int defaultMp, defaultMv, defaultMa;
+
+// This says whether the Model-stage "disabled" flag for this Constraint should
+// be initially on or off. Most constraints are enabled by default.
+bool defaultDisabled;
+
+ // TOPOLOGY "CACHE"
+
+// When topology is realized we study the constrained bodies to identify the
+// subtree of mobilized bodies which may be kinematically involved in
+// satisfaction of this Constraint. This requires finding the outmost common
+// ancestor of the constrained bodies. All mobilized bodies on the paths inward
+// from the constrained bodies to the ancestor are included; nothing outboard
+// of the constrained bodies is included; and the ancestor is treated as ground
+// so that its mobilities are *not* included. The Ancestor may be one of the
+// Constrained Bodies or may be distinct.
+mutable SimbodyMatterSubtree mySubtree;
+
+// This is true only when (1) there are Constrained Bodies and (2) their
+// Ancestor is some MobilizedBody other than Ground.
+mutable bool myAncestorBodyIsNotGround;
+
+// When the Ancestor is not Ground, each of the Constrained Bodies (except
+// the Ancestor) needs some additional precalculated data in the State cache
+// so is assigned a MatterSubsystem-global AncestorConstrainedBodyPool slot.
+// If Ancestor isn't Ground there is an entry here for each ConstrainedBody
+// (including Ancestor if it is one), but the index is invalid for Ancestor's
+// entry. When Ancestor is Ground we don't allocate this array.
+mutable Array_<AncestorConstrainedBodyPoolIndex> myPoolIndex;
+ // index with ConstrainedBodyIndex
+};
+
+
+
+//==============================================================================
+// ROD IMPL
+//==============================================================================
+// TODO: should use distance rather than distance^2 to improve scaling.
+class Constraint::RodImpl : public ConstraintImpl {
+public:
+RodImpl()
+ : ConstraintImpl(1,0,0), defaultPoint1(0), defaultPoint2(0), defaultRodLength(1),
+ pointRadius(-1) // this means "use default point radius"
+{
+ // Rod constructor sets all the data members here directly
+}
+RodImpl* clone() const { return new RodImpl(*this); }
+
+// Draw some end points and a rubber band line.
+void calcDecorativeGeometryAndAppendVirtual
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const;
+
+void setPointDisplayRadius(Real r) {
+ // r == 0 means don't display point, r < 0 means use default which is some fraction of rod length
+ invalidateTopologyCache();
+ pointRadius= r > 0 ? r : 0;
+}
+Real getPointDisplayRadius() const {return pointRadius;}
+
+// Implementation of virtuals required for holonomic constraints.
+
+// TODO: this is badly scaled; consider using length instead of length^2
+// perr = (p^2 - d^2)/2
+void calcPositionErrorsVirtual
+ (const State& s, // Stage::Time
+ const Array_<Transform,ConstrainedBodyIndex>& X_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr) // mp of these
+ const
+{
+ assert(X_AB.size()==2 && constrainedQ.size()==0 && perr.size()==1);
+ const Vec3 p1 = findStationLocation(X_AB, B1, defaultPoint1); // meas from & expr in ancestor
+ const Vec3 p2 = findStationLocation(X_AB, B2, defaultPoint2);
+ const Vec3 p = p2 - p1;
+ //TODO: save p in state
+
+ perr[0] = (dot(p, p) - square(defaultRodLength)) / 2;
+}
+
+// pverr = d/dt perr = pdot*p = v*p, where v=v2-v1 is relative velocity
+void calcPositionDotErrorsVirtual
+ (const State& s, // Stage::Position
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr) // mp of these
+ const
+{
+ assert(V_AB.size()==2 && constrainedQDot.size()==0 && pverr.size()==1);
+ //TODO: should be able to get p from State
+ const Vec3 p1 = findStationLocationFromState(s, B1, defaultPoint1); // meas from & expr in ancestor
+ const Vec3 p2 = findStationLocationFromState(s, B2, defaultPoint2);
+ const Vec3 p = p2 - p1;
+
+ const Vec3 v1 = findStationVelocity(s, V_AB, B1, defaultPoint1); // meas & expr in ancestor
+ const Vec3 v2 = findStationVelocity(s, V_AB, B2, defaultPoint2);
+ const Vec3 v = v2 - v1;
+ pverr[0] = dot(v, p);
+}
+
+// paerr = d/dt verr = vdot*p + v*pdot =a*p+v*v, where a=a2-a1 is relative
+// acceleration
+void calcPositionDotDotErrorsVirtual
+ (const State& s, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr) // mp of these
+ const
+{
+ assert(A_AB.size()==2 && constrainedQDotDot.size()==0 && paerr.size()==1);
+ //TODO: should be able to get p and v from State
+ const Vec3 p1 = findStationLocationFromState(s, B1, defaultPoint1); // meas from & expr in ancestor
+ const Vec3 p2 = findStationLocationFromState(s, B2, defaultPoint2);
+ const Vec3 p = p2 - p1;
+ const Vec3 v1 = findStationVelocityFromState(s, B1, defaultPoint1); // meas & expr in ancestor
+ const Vec3 v2 = findStationVelocityFromState(s, B2, defaultPoint2);
+ const Vec3 v = v2 - v1;
+
+ const Vec3 a1 = findStationAcceleration(s, A_AB, B1, defaultPoint1); // meas & expr in ancestor
+ const Vec3 a2 = findStationAcceleration(s, A_AB, B2, defaultPoint2);
+ const Vec3 a = a2 - a1;
+
+ paerr[0] = dot(a, p) + dot(v, v);
+}
+
+// Write this routine by inspection of the pdot routine, looking for terms
+// involving velocity. On point2 we see v2*p, on point1 we see -v1*p, so forces
+// are m*p and -m*p, respectively.
+void addInPositionConstraintForcesVirtual
+ (const State& s, // Stage::Position
+ const Array_<Real>& multipliers, // mp of these
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedQIndex>& qForces)
+ const
+{
+ assert(multipliers.size()==1 && bodyForcesInA.size()==2
+ && qForces.size()==0);
+ const Real lambda = multipliers[0];
+ //TODO: should be able to get p from State
+ const Vec3 p1 = findStationLocationFromState(s, B1, defaultPoint1); // meas from & expr in ancestor
+ const Vec3 p2 = findStationLocationFromState(s, B2, defaultPoint2);
+ const Vec3 p = p2 - p1;
+
+ const Vec3 f2 = lambda * p;
+
+ // The forces on either point have the same line of action because they are
+ // aligned with the vector between the points. Applying the forces to any
+ // point along the line would have the same effect (e.g., same point in
+ // space on both bodies) so this is the same as an equal and opposite force
+ // applied to the same point and this constraint will do no work even if
+ // the position or velocity constraints are not satisfied.
+ addInStationForce(s, B2, defaultPoint2, f2, bodyForcesInA);
+ addInStationForce(s, B1, defaultPoint1, -f2, bodyForcesInA);
+}
+
+SimTK_DOWNCAST(RodImpl, ConstraintImpl);
+//------------------------------------------------------------------------------
+ private:
+friend class Constraint::Rod;
+
+ConstrainedBodyIndex B1; // must be 0 and 1
+ConstrainedBodyIndex B2;
+Vec3 defaultPoint1; // on body 1, exp. in B1 frame
+Vec3 defaultPoint2; // on body 2, exp. in B2 frame
+Real defaultRodLength;
+
+// This is just for visualization
+Real pointRadius;
+};
+
+
+
+//==============================================================================
+// POINT IN PLANE IMPL
+//==============================================================================
+class Constraint::PointInPlaneImpl : public ConstraintImpl {
+public:
+PointInPlaneImpl()
+ : ConstraintImpl(1,0,0), defaultPlaneNormal(), defaultPlaneHeight(0), defaultFollowerPoint(0),
+ planeHalfWidth(1), pointRadius(Real(0.05))
+{ }
+PointInPlaneImpl* clone() const { return new PointInPlaneImpl(*this); }
+
+void calcDecorativeGeometryAndAppendVirtual
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const;
+
+void setPlaneDisplayHalfWidth(Real h) {
+ // h <= 0 means don't display plane
+ invalidateTopologyCache();
+ planeHalfWidth = h > 0 ? h : 0;
+}
+Real getPlaneDisplayHalfWidth() const {return planeHalfWidth;}
+
+void setPointDisplayRadius(Real r) {
+ // r <= 0 means don't display point
+ invalidateTopologyCache();
+ pointRadius= r > 0 ? r : 0;
+}
+Real getPointDisplayRadius() const {return pointRadius;}
+
+// Implementation of virtuals required for holonomic constraints.
+
+// We have a point-in-plane connection between base body B, on which the plane
+// is fixed, and follower body F, on which the follower point S is fixed. All
+// forces will be applied at point S and the coincident material point C on B
+// which is instantaneously at the same spatial location as S. Then n is the
+// plane normal (a constant unit vector in B), h is the plane height measured
+// from the B origin along n (a scalar constant).Point C's location in B is
+// given by the vector p_BC from B's origin to the current location of S, and
+// expressed in B. That vector expressed in A is p_BC_A (= p_AS-p_AB). We will
+// express in the A frame but differentiate in the B frame.
+//
+// Derivation:
+// (1) Note that to take a derivative d/dt_B in a moving frame B, we can take
+// the derivative d/dt_A and then add in the contribution d_A/dt_B from
+// A's rotation in B (which is the angular velocity of A in B,
+// w_BA=-w_AB).
+// (2) p_CS = p_AS-p_AC = 0 by construction of C, but its derivative in A,
+// v_CS_A = d/dt_A p_CS != 0.
+//
+// perr = p_CS * n + constant
+// = constant (because p_CS==0 by construction)
+//
+// verr = d/dt_B perr = d/dt_A perr + d_A/dt_B perr
+// = [v_CS_A*n + p_CS * (w_AB X n)] + [(w_BA X p_CS) * n + p_CS * (w_BA X n)]
+// = v_CS_A*n + p_CS * (w_AB X n) (because terms in 2nd [] cancel)
+// = v_CS_A * n (because p_CS==0 by construction)
+//
+// aerr = d/dt_B verr = d/dt_A verr + d_A/dt_B verr
+// = [a_CS_A*n + v_CS_A*(w_AB X n) + v_CS_A*(w_AB X n) + p_CS*(2 w_AB X(w_AB X n))]
+// + [w_BAXv_CS_A*n + v_CS_A*w_BAXn]
+// = (a_CS_A - 2 w_AB X v_CS_A) * n (2nd bracket cancels, and p_CS==0)
+//
+// (The constant in perr is set so that S starts at the same height h as the
+// plane.)
+//
+// Then, from examination of verr noting that v_CS_A=v_AS-v_AC:
+// ~v_AS*n (body F at point S)
+// - ~v_AC*n (body B at point C)
+// so we apply a forces lambda*n to F at S, -lambda*n to B at C.
+//
+// --------------------------------
+// perr = ~p_BS*n - h
+// --------------------------------
+void calcPositionErrorsVirtual
+ (const State& s, // Stage::Time
+ const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr) // mp of these
+ const
+{
+ assert(allX_AB.size()==2 && constrainedQ.size()==0 && perr.size() == 1);
+
+ const Vec3 p_AS = findStationLocation(allX_AB, followerBody,
+ defaultFollowerPoint);
+ const Transform& X_AB = getBodyTransform(allX_AB, planeBody);
+ const Vec3 p_BC = ~X_AB * p_AS; // shift to B origin, reexpress in B;
+ // C is material pt of B coincident with S
+
+ // We'll calculate this scalar using B-frame vectors, but any frame would
+ // have done.
+ perr[0] = dot(p_BC, defaultPlaneNormal) - defaultPlaneHeight;
+}
+
+// --------------------------------
+// verr = ~v_CS_A*n
+// --------------------------------
+void calcPositionDotErrorsVirtual
+ (const State& s, // Stage::Position
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr) // mp of these
+ const
+{
+ assert(V_AB.size()==2 && constrainedQDot.size()==0 && pverr.size() == 1);
+ //TODO: should be able to get p info from State
+
+ const Vec3 p_AS = findStationLocationFromState(s, followerBody,
+ defaultFollowerPoint);
+ const Transform& X_AB = getBodyTransformFromState(s, planeBody);
+ const Vec3 p_BC = ~X_AB * p_AS; // shift to B origin, reexpress in B;
+ // C is material point of B coincident with S
+ const UnitVec3 n_A = X_AB.R() * defaultPlaneNormal;
+
+ const Vec3 v_AS = findStationVelocity(s, V_AB, followerBody,
+ defaultFollowerPoint);
+ const Vec3 v_AC = findStationVelocity(s, V_AB, planeBody, p_BC);
+
+ // Calculate this scalar using A-frame vectors.
+ pverr[0] = dot( v_AS-v_AC, n_A );
+}
+
+// -------------------------------------
+// aerr = ~(a_CS_A - 2 w_AB X v_CS_A) * n
+// -------------------------------------
+void calcPositionDotDotErrorsVirtual
+ (const State& s, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr) // mp of these
+ const
+{
+ assert(A_AB.size()==2 && constrainedQDotDot.size()==0 && paerr.size() == 1);
+ //TODO: should be able to get p and v info from State
+ const Vec3 p_AS = findStationLocationFromState(s, followerBody,
+ defaultFollowerPoint);
+ const Transform& X_AB = getBodyTransformFromState(s, planeBody);
+ const Vec3 p_BC = ~X_AB * p_AS; // shift to B origin, reexpress in B;
+ // C is material point of B coincident with S
+ const UnitVec3 n_A = X_AB.R() * defaultPlaneNormal;
+
+ const Vec3& w_AB = getBodyAngularVelocityFromState(s, planeBody);
+ const Vec3 v_AS = findStationVelocityFromState(s, followerBody,
+ defaultFollowerPoint);
+ const Vec3 v_AC = findStationVelocityFromState(s, planeBody, p_BC);
+
+ const Vec3 a_AS = findStationAcceleration(s, A_AB, followerBody,
+ defaultFollowerPoint);;
+ const Vec3 a_AC = findStationAcceleration(s, A_AB, planeBody, p_BC);
+
+ paerr[0] = dot( (a_AS-a_AC) - 2*w_AB % (v_AS-v_AC), n_A );
+}
+
+// apply f=lambda*n to the follower point S of body F,
+// -f to point C (coincident point) of body B
+void addInPositionConstraintForcesVirtual
+ (const State& s, // Stage::Position
+ const Array_<Real>& multipliers, // mp of these
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedQIndex>& qForces)
+ const
+{
+ assert(multipliers.size()==1 && bodyForcesInA.size()==2
+ && qForces.size()==0);
+ const Real lambda = multipliers[0];
+
+ //TODO: should be able to get p info from State
+ const Vec3& p_FS = defaultFollowerPoint; // measured & expressed in F
+ const Vec3 p_AS = findStationLocationFromState(s, followerBody,
+ defaultFollowerPoint);
+ const Transform& X_AB = getBodyTransformFromState(s, planeBody);
+ const Vec3 p_BC = ~X_AB * p_AS; // measured & expressed in B
+ const Vec3 force_A = X_AB.R()*(lambda*defaultPlaneNormal);
+
+ addInStationForce(s, followerBody, p_FS, force_A, bodyForcesInA);
+ addInStationForce(s, planeBody, p_BC, -force_A, bodyForcesInA);
+}
+
+SimTK_DOWNCAST(PointInPlaneImpl, ConstraintImpl);
+//------------------------------------------------------------------------------
+ private:
+friend class Constraint::PointInPlane;
+
+ConstrainedBodyIndex planeBody; // B1
+ConstrainedBodyIndex followerBody; // B2
+
+UnitVec3 defaultPlaneNormal; // on body 1, exp. in B1 frame
+Real defaultPlaneHeight;
+Vec3 defaultFollowerPoint; // on body 2, exp. in B2 frame
+
+// These are just for visualization
+Real planeHalfWidth;
+Real pointRadius;
+};
+
+
+
+//==============================================================================
+// POINT ON LINE IMPL
+//==============================================================================
+class Constraint::PointOnLineImpl : public ConstraintImpl {
+public:
+PointOnLineImpl()
+ : ConstraintImpl(2,0,0), defaultLineDirection(), defaultPointOnLine(), defaultFollowerPoint(0),
+ lineHalfLength(1), pointRadius(Real(0.05))
+{ }
+PointOnLineImpl* clone() const { return new PointOnLineImpl(*this); }
+
+void calcDecorativeGeometryAndAppendVirtual
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const;
+
+void setLineDisplayHalfLength(Real h) {
+ // h <= 0 means don't display line
+ invalidateTopologyCache();
+ lineHalfLength = h > 0 ? h : 0;
+}
+Real getLineDisplayHalfLength() const {return lineHalfLength;}
+
+void setPointDisplayRadius(Real r) {
+ // r <= 0 means don't display point
+ invalidateTopologyCache();
+ pointRadius= r > 0 ? r : 0;
+}
+Real getPointDisplayRadius() const {return pointRadius;}
+
+// Implementation of ContraintRep virtuals
+void realizeTopologyVirtual(State& s) const {
+ x = defaultLineDirection.perp(); // x and y are mutable
+ y = UnitVec3(defaultLineDirection % x);
+}
+
+// Implementation of virtuals required for holonomic constraints.
+
+// We have a point-on-line connection between base body B, on which the line is
+// fixed, and follower body F, on which the follower point S is fixed. All
+// forces will be applied at point S and the coincident material point C on B
+// which is instantaneously at the same spatial location as S. Then z is a unit
+// vector in the direction of the line, and P is a point fixed to B that the
+// line passes through. We will enforce this using two point-on-plane
+// constraints, where the intersection of the two planes is the line. For that
+// we need two plane normals perpendicular to z. We'll use an arbitrary
+// perpendicular x, then use y=z X x as the other perpendicular. This
+// establishes a right handed coordinate system where the line is along the z
+// axis, and we'll apply constraint forces in the x-y plane.
+//
+// See the point-in-plane constraint for details; here we're just picking x and
+// y as plane normals.
+
+// --------------------------------
+// perr = ~(p_BS-p_BP) * x
+// ~(p_BS-p_BP) * y
+// --------------------------------
+void calcPositionErrorsVirtual
+ (const State& s, // Stage::Time
+ const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr) // mp of these
+ const
+{
+ assert(allX_AB.size()==2 && constrainedQ.size()==0 && perr.size() == 2);
+
+ const Transform& X_AB = getBodyTransform(allX_AB, lineBody);
+ const Vec3 p_AS = findStationLocation(allX_AB, followerBody,
+ defaultFollowerPoint);
+ const Vec3 p_BC = ~X_AB * p_AS; // shift to B origin, reexpress in B;
+ // C is material point of B coincident with S
+ const Vec3 p_PC = p_BC - defaultPointOnLine;
+
+ // We'll calculate these two scalars using B-frame vectors, but any frame
+ // would have done.
+ perr[0] = ~p_PC * x;
+ perr[1] = ~p_PC * y;
+}
+
+// --------------------------------
+// verr = ~v_CS_A*n
+// --------------------------------
+void calcPositionDotErrorsVirtual
+ (const State& s, // Stage::Position
+ const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr) // mp of these
+ const
+{
+ assert(allV_AB.size()==2 && constrainedQDot.size()==0 && pverr.size() == 2);
+ //TODO: should be able to get p info from State
+ const Transform& X_AB = getBodyTransformFromState(s, lineBody);
+ const Vec3 p_AS = findStationLocationFromState(s, followerBody,
+ defaultFollowerPoint);
+ const Vec3 p_BC = ~X_AB * p_AS;
+ const Vec3 p_PC = p_BC - defaultPointOnLine;
+
+ const Vec3 v_AS = findStationVelocity(s, allV_AB, followerBody,
+ defaultFollowerPoint);
+ const Vec3 v_AC = findStationVelocity(s, allV_AB, lineBody, p_BC);
+
+ const Vec3 v_CS_B = ~X_AB.R()*(v_AS-v_AC); // reexpress in B
+
+ // Calculate these scalar using B-frame vectors, but any frame would
+ // have done.
+ pverr[0] = ~v_CS_B * x;
+ pverr[1] = ~v_CS_B * y;
+}
+
+// -------------------------------------
+// aerr = ~(a_CS_A - 2 w_AB X v_CS_A) * n
+// -------------------------------------
+void calcPositionDotDotErrorsVirtual
+ (const State& s, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr) // mp of these
+ const
+{
+ assert(allA_AB.size()==2 && constrainedQDotDot.size()==0 && paerr.size()==2);
+ //TODO: should be able to get p and v info from State
+ const Transform& X_AB = getBodyTransformFromState(s, lineBody);
+ const Vec3 p_AS = findStationLocationFromState(s, followerBody,
+ defaultFollowerPoint);
+ const Vec3 p_BC = ~X_AB * p_AS; // shift to B origin, reexpress in B;
+ // C is material point of B coincident with S
+ const Vec3 p_PC = p_BC - defaultPointOnLine;
+
+ const Vec3& w_AB = getBodyAngularVelocityFromState(s, lineBody);
+ const Vec3 v_AS = findStationVelocityFromState(s, followerBody, defaultFollowerPoint);
+ const Vec3 v_AC = findStationVelocityFromState(s, lineBody, p_BC);
+
+ const Vec3 a_AS = findStationAcceleration(s, allA_AB, followerBody,
+ defaultFollowerPoint);
+ const Vec3 a_AC = findStationAcceleration(s, allA_AB, lineBody, p_BC);
+ const Vec3 a_CS_B = ~X_AB.R()*(a_AS-a_AC - 2 * w_AB % (v_AS-v_AC));
+
+ // Calculate these scalar using B-frame vectors, but any frame would
+ // have done.
+ paerr[0] = ~a_CS_B * x;
+ paerr[1] = ~a_CS_B * y;
+}
+
+// apply f=lambda0*x + lambda1*y to the follower point S of body F,
+// -f to point C (coincident point) of body B
+void addInPositionConstraintForcesVirtual
+ (const State& s, // Stage::Position
+ const Array_<Real>& multipliers, // mp of these
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedQIndex>& qForces)
+ const
+{
+ assert(multipliers.size()==2 && bodyForcesInA.size()==2
+ && qForces.size()==0);
+ const Vec2 lambda = Vec2::getAs(&multipliers[0]);
+
+ //TODO: should be able to get p info from State
+ const Transform& X_AB = getBodyTransformFromState(s, lineBody);
+ const Vec3& p_FS = defaultFollowerPoint; // measured & expressed in F
+ const Vec3 p_AS = findStationLocationFromState(s, followerBody,
+ defaultFollowerPoint);
+ const Vec3 p_BC = ~X_AB * p_AS; // measured & expressed in B
+
+ const Vec3 force_B = lambda[0] * x + lambda[1] * y;
+ const Vec3 force_A = X_AB.R() * force_B;
+
+ addInStationForce(s, followerBody, p_FS, force_A, bodyForcesInA);
+ addInStationForce(s, lineBody, p_BC, -force_A, bodyForcesInA);
+}
+
+SimTK_DOWNCAST(PointOnLineImpl, ConstraintImpl);
+//------------------------------------------------------------------------------
+ private:
+friend class Constraint::PointOnLine;
+
+ConstrainedBodyIndex lineBody; // B
+ConstrainedBodyIndex followerBody; // F
+
+UnitVec3 defaultLineDirection; // z on B, exp. in B frame
+Vec3 defaultPointOnLine; // P on B, meas&exp in B frame
+Vec3 defaultFollowerPoint; // S on F, meas&exp in F frame
+
+// These are just for visualization
+Real lineHalfLength;
+Real pointRadius;
+
+// TOPOLOGY CACHE (that is, calculated from construction data)
+mutable UnitVec3 x, y;
+};
+
+
+
+//==============================================================================
+// CONSTANT ANGLE IMPL
+//==============================================================================
+class Constraint::ConstantAngleImpl : public ConstraintImpl {
+public:
+ConstantAngleImpl()
+ : ConstraintImpl(1,0,0), defaultAxisB(), defaultAxisF(), defaultAngle(Pi/2),
+ axisLength(1), axisThickness(1), cosineOfDefaultAngle(NaN)
+{ }
+ConstantAngleImpl* clone() const { return new ConstantAngleImpl(*this); }
+
+void calcDecorativeGeometryAndAppendVirtual
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const;
+
+void setAxisLength(Real length) {
+ // length <= 0 means don't display axis
+ invalidateTopologyCache();
+ axisLength = length > 0 ? length : 0;
+}
+Real getAxisLength() const {return axisLength;}
+
+void setAxisThickness(Real t) {
+ // t <= 0 means don't display axis
+ invalidateTopologyCache();
+ axisThickness = t > 0 ? t : 0;
+}
+Real getAxisThickness() const {return axisThickness;}
+
+// Implementation of ContraintRep virtuals
+void realizeTopologyVirtual(State& s) const {
+ cosineOfDefaultAngle = std::cos(defaultAngle);
+}
+
+
+// Implementation of virtuals required for holonomic constraints.
+
+// Let B=B1 be the "base" body onto which unit vector b is fixed, and F=B2 the
+// "follower" body onto which unit vector f is fixed. The angle theta between
+// these vectors is given by cos(theta) = dot(b, f) with the axes expressed in
+// a common basis. This can range from 1 to -1, corresponding to angles 0 to
+// 180 respectively. We would like to enforce the constraint that cos(theta) is
+// a constant. This can be done with a single constraint equation as long as
+// theta is sufficiently far away from 0 and 180, with the numerically best
+// performance at theta=90 degrees where cos(theta)==0.
+//
+// If you want to enforce that two axes are aligned with one another (that is,
+// the angle between them is 0 or 180), that takes *two* constraint equations
+// since the only remaining rotation is about the common axis.
+//
+// We will work in the A frame.
+//
+// ------------------------------
+// perr = ~b_A * f_A - cos(theta)
+// ------------------------------
+//
+// verr = d/dt perr (derivative taken in A)
+// = ~b_A * (w_AF % f_A) + ~f_A * (w_AB % b_A)
+// = ~w_AF * (f_A % b_A) - ~w_AB * (f_A % b_A) (scalar triple product identity)
+// => ------------------------------
+// verr = ~(w_AF-w_AB) * (f_A % b_A)
+// ---------------------------------
+//
+// aerr = d/dt verr (derivative taken in A)
+// = ~(b_AF-b_AB) * (f_A % b_A)
+// + (w_AF-w_AB) * ((w_AF%f_A) % b_A)
+// + (w_AF-w_AB) * (f_A % (w_AB%b_A))
+// = ~(b_AF-b_AB) * (f_A % b_A)
+// + ~(w_AF-w_AB) * ((w_AF%f_A) % b_A) - (w_AB%b_A) % f_A)
+// => -----------------------------------------------------------
+// aerr = ~(b_AF-b_AB) * (f_A % b_A)
+// + ~(w_AF-w_AB) * ((w_AF%f_A) % b_A) - (w_AB%b_A) % f_A)
+// --------------------------------------------------------------
+//
+// Constraint torque can be determined by inspection of verr:
+// lambda * (f_A % b_A) applied to body F
+// -lambda * (f_A % b_A) applied to body B
+//
+
+// ------------------------------
+// perr = ~b_A * f_A - cos(theta)
+// ------------------------------
+void calcPositionErrorsVirtual
+ (const State& s, // Stage::Time
+ const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr) // mp of these
+ const
+{
+ assert(allX_AB.size()==2 && constrainedQ.size()==0 && perr.size() == 1);
+
+ const Rotation& R_AB = getBodyRotation(allX_AB, B);
+ const Rotation& R_AF = getBodyRotation(allX_AB, F);
+ const UnitVec3 b_A = R_AB * defaultAxisB;
+ const UnitVec3 f_A = R_AF * defaultAxisF;
+
+ perr[0] = dot(b_A, f_A) - cosineOfDefaultAngle;
+}
+
+// ----------------------------------
+// pverr = ~(w_AF-w_AB) * (f_A % b_A)
+// ----------------------------------
+void calcPositionDotErrorsVirtual
+ (const State& s, // Stage::Position
+ const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr) // mp of these
+ const
+{
+ assert(allV_AB.size()==2 && constrainedQDot.size()==0 && pverr.size()==1);
+ //TODO: should be able to get p info from State
+ const Rotation& R_AB = getBodyRotationFromState(s, B);
+ const Rotation& R_AF = getBodyRotationFromState(s, F);
+ const UnitVec3 b_A = R_AB * defaultAxisB;
+ const UnitVec3 f_A = R_AF * defaultAxisF;
+
+ const Vec3& w_AB = getBodyAngularVelocity(allV_AB, B);
+ const Vec3& w_AF = getBodyAngularVelocity(allV_AB, F);
+
+ pverr[0] = dot( w_AF-w_AB, f_A % b_A );
+}
+
+// --------------------------------------------------------------
+// paerr = ~(b_AF-b_AB) * (f_A % b_A)
+// + ~(w_AF-w_AB) * ((w_AF%f_A) % b_A) - (w_AB%b_A) % f_A)
+// --------------------------------------------------------------
+void calcPositionDotDotErrorsVirtual
+ (const State& s, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr) // mp of these
+ const
+{
+ assert(allA_AB.size()==2 && constrainedQDotDot.size()==0 && paerr.size()==1);
+ //TODO: should be able to get p and v info from State
+ const Rotation& R_AB = getBodyRotationFromState(s, B);
+ const Rotation& R_AF = getBodyRotationFromState(s, F);
+ const UnitVec3 b_A = R_AB * defaultAxisB;
+ const UnitVec3 f_A = R_AF * defaultAxisF;
+ const Vec3& w_AB = getBodyAngularVelocityFromState(s, B);
+ const Vec3& w_AF = getBodyAngularVelocityFromState(s, F);
+
+ const Vec3& b_AB = getBodyAngularAcceleration(allA_AB, B);
+ const Vec3& b_AF = getBodyAngularAcceleration(allA_AB, F);
+
+ paerr[0] = dot( b_AF-b_AB, f_A % b_A )
+ + dot( w_AF-w_AB, (w_AF%f_A) % b_A - (w_AB%b_A) % f_A);
+}
+
+// lambda * (f_A % b_A) applied to body F
+// -lambda * (f_A % b_A) applied to body B
+void addInPositionConstraintForcesVirtual
+ (const State& s, // Stage::Position
+ const Array_<Real>& multipliers, // mp of these
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedQIndex>& qForces)
+ const
+{
+ assert(multipliers.size()==1 && bodyForcesInA.size()==2
+ && qForces.size()==0);
+ const Real lambda = multipliers[0];
+ //TODO: should be able to get p info from State
+ const Rotation& R_AB = getBodyRotationFromState(s, B);
+ const Rotation& R_AF = getBodyRotationFromState(s, F);
+ const UnitVec3 b_A = R_AB*defaultAxisB;
+ const UnitVec3 f_A = R_AF*defaultAxisF;
+ const Vec3 torque_F_A = lambda * (f_A % b_A); // on F, in A frame
+
+ addInBodyTorque(s, F, torque_F_A, bodyForcesInA);
+ addInBodyTorque(s, B, -torque_F_A, bodyForcesInA);
+}
+
+SimTK_DOWNCAST(ConstantAngleImpl, ConstraintImpl);
+//------------------------------------------------------------------------------
+ private:
+friend class Constraint::ConstantAngle;
+
+ConstrainedBodyIndex B; // B1 is "base" body
+ConstrainedBodyIndex F; // B2 is "follower" body
+
+UnitVec3 defaultAxisB; // fixed to B, expressed in B frame
+UnitVec3 defaultAxisF; // fixed to F, expressed in F frame
+Real defaultAngle; // required angle between axisB and axisF
+
+// These are just for visualization
+Real axisLength;
+Real axisThickness;
+
+// TOPOLOGY CACHE (that is, calculated from construction data)
+mutable Real cosineOfDefaultAngle;
+};
+
+
+
+//==============================================================================
+// BALL IMPL
+//==============================================================================
+class Constraint::BallImpl : public ConstraintImpl {
+public:
+BallImpl() : ConstraintImpl(3,0,0), defaultPoint1(0), defaultPoint2(0),
+ defaultRadius(Real(0.1)) { }
+BallImpl* clone() const { return new BallImpl(*this); }
+
+void calcDecorativeGeometryAndAppendVirtual
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const;
+
+void setDefaultRadius(Real r) {
+ // r <= 0 means don't display
+ invalidateTopologyCache();
+ defaultRadius = r > 0 ? r : 0;
+}
+Real getDefaultRadius() const {return defaultRadius;}
+
+// The default body stations may be overridden by setting instance variables
+// in the state. We allocate the state resources here.
+void realizeTopologyVirtual(State& state) const;
+
+// Return the pair of constrained station points, with the first expressed
+// in the body 1 frame and the second in the body 2 frame. Note that although
+// these are used to define the position error, only the station on body 2
+// is used to generate constraint forces; the point of body 1 that is
+// coincident with the body 2 point receives the equal and opposite force.
+const std::pair<Vec3,Vec3>& getBodyStations(const State& state) const;
+
+// Return a writable reference into the Instance-stage state variable
+// containing the pair of constrained station points, with the first expressed
+// in the body 1 frame and the second in the body 2 frame. Calling this
+// method invalidates the Instance stage and above in the given state.
+std::pair<Vec3,Vec3>& updBodyStations(State& state) const;
+
+// Implementation of virtuals required for holonomic constraints.
+
+// We have a ball joint between base body B and follower body F, located at a
+// point P fixed to B and point S fixed on F. All forces will be applied at
+// point S and the coincident material point C on B which is instantaneously at
+// the same spatial location as S. We will work in the A frame.
+//
+// First, find the material point C of B that is coincident
+// in space with point S of F: p_BC = p_AS-p_AB. This vector
+// is *constant* in the B frame because it is a material point,
+// despite the fact that its definition involves a point which
+// moves with respect to B. The velocity constraint is then
+// very simple: the spatial velocity of point C of B should be
+// the same as the spatial velocity of point S of F:
+// verr = v_AS - v_AC = v_AS - (v_AB + w_AB X p_BC) = 0
+// Integrating to get perr, we get
+// perr = p_AS - p_AC + constant = 0
+// But p_AC=p_AS by construction, so perr=constant=0.
+// The constant is defined by the fact that we want material point
+// C of B to be in the same spatial location as point P of B,
+// so constant=p_BC-p_BP=0. Writing in the A frame we have:
+// perr = p_AS-(p_AB+R_AB*p_BP) = 0 (a constant)
+// verr = v_AS - (v_AB + w_AB X R_AB*p_BC)
+// aerr = a_AS - (a_AB + b_AB X R_AB*p_BC + w_AB X (w_AB X R_AB*p_BC))
+// apply +lambda to S of F, -lambda to C of B.
+//
+//
+
+void calcPositionErrorsVirtual
+ (const State& s, // Stage::Time
+ const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr) // mp of these
+ const
+{
+ assert(allX_AB.size()==2 && constrainedQ.size()==0 && perr.size() == 3);
+
+ const std::pair<Vec3,Vec3>& pts = getBodyStations(s);
+
+ const Vec3 p_AP = findStationLocation(allX_AB, B1, pts.first);
+ const Vec3 p_AS = findStationLocation(allX_AB, B2, pts.second);
+
+ // See above comments -- this is just the constant of integration; there is a missing (p_AS-p_AC)
+ // term (always 0) here which is what we differentiate to get the verr equation.
+ Vec3::updAs(&perr[0]) = p_AS - p_AP;
+}
+
+void calcPositionDotErrorsVirtual
+ (const State& s, // Stage::Position
+ const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr) // mp of these
+ const
+{
+ assert(allV_AB.size()==2 && constrainedQDot.size()==0 && pverr.size()==3);
+
+ // Note that we're not using point1.
+ const Vec3& point2 = getBodyStations(s).second;
+
+ //TODO: should be able to get p info from State
+ const Transform& X_AB = getBodyTransformFromState(s, B1);
+ const Vec3 p_AS = findStationLocationFromState(s, B2, point2);
+ const Vec3 p_BC = ~X_AB*p_AS; // C is a material point of body B
+
+ const Vec3 v_AS = findStationVelocity(s, allV_AB, B2,
+ point2);
+ const Vec3 v_AC = findStationVelocity(s, allV_AB, B1, p_BC);
+ Vec3::updAs(&pverr[0]) = v_AS - v_AC;
+}
+
+void calcPositionDotDotErrorsVirtual
+ (const State& s, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr) // mp of these
+ const
+{
+ assert(allA_AB.size()==2 && constrainedQDotDot.size()==0 && paerr.size()==3);
+
+ // Note that we're not using point1.
+ const Vec3& point2 = getBodyStations(s).second;
+
+ //TODO: should be able to get p and v info from State
+
+ const Transform& X_AB = getBodyTransformFromState(s, B1);
+ const Vec3 p_AS = findStationLocationFromState(s, B2, point2);
+ const Vec3 p_BC = ~X_AB*p_AS; // C is a material point of body B
+
+ const Vec3 a_AS = findStationAcceleration(s, allA_AB, B2,
+ point2);
+ const Vec3 a_AC = findStationAcceleration(s, allA_AB, B1, p_BC);
+ Vec3::updAs(&paerr[0]) = a_AS - a_AC;
+}
+
+void addInPositionConstraintForcesVirtual
+ (const State& s, // Stage::Position
+ const Array_<Real>& multipliers, // mp of these
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedQIndex>& qForces)
+ const
+{
+ assert(multipliers.size()==3 && bodyForcesInA.size()==2
+ && qForces.size()==0);
+
+ // Note that we're not using point1.
+ const Vec3& point2 = getBodyStations(s).second;
+
+ //TODO: should be able to get p info from State
+ const Transform& X_AB = getBodyTransformFromState(s,B1);
+ const Vec3& p_FS = point2;
+ const Vec3 p_AS = findStationLocationFromState(s, B2, p_FS);
+ const Vec3 p_BC = ~X_AB * p_AS; // shift to B origin, reexpress in B;
+ // C is material point of B coincident with S
+
+ const Vec3 force_A = Vec3::getAs(&multipliers[0]);
+
+ // Multipliers are force to be applied to S on F, but
+ // apply the -force not to point P of B, but to the point "C" of B
+ // coincident with S, which won't be exactly the same place
+ // as P if the position-level constraint isn't met exactly.
+
+ addInStationForce(s, B2, p_FS, force_A, bodyForcesInA);
+ addInStationForce(s, B1, p_BC, -force_A, bodyForcesInA);
+}
+
+SimTK_DOWNCAST(BallImpl, ConstraintImpl);
+//------------------------------------------------------------------------------
+ private:
+friend class Constraint::Ball;
+
+ConstrainedBodyIndex B1;
+ConstrainedBodyIndex B2;
+
+Vec3 defaultPoint1; // on body 1, exp. in B1 frame
+Vec3 defaultPoint2; // on body 2, exp. in B2 frame
+Real defaultRadius; // used for visualization only
+
+// This Instance-stage variable holds the actual stations on B1 & B2.
+mutable DiscreteVariableIndex stationsIx;
+};
+
+
+
+//==============================================================================
+// CONSTANT ORIENTATION IMPL
+//==============================================================================
+class Constraint::ConstantOrientationImpl : public ConstraintImpl {
+public:
+ConstantOrientationImpl()
+ : ConstraintImpl(3,0,0), defaultRB(), defaultRF()
+{ }
+ConstantOrientationImpl* clone() const { return new ConstantOrientationImpl(*this); }
+
+//TODO: visualization?
+
+
+// Implementation of virtuals required for holonomic constraints.
+
+// Let B=B1 be the "base" body onto which rotation matrix RB is fixed, and F=B2
+// the "follower" body onto which rotation matrix RF is fixed. We would like to
+// enforce the constraint that RB==RF when both are expressed in a common basis.
+// (Remember that a rotation matrix is just three axis vectors.)
+//
+// Here the (redundant) assembly constraint is that all the axes are parallel,
+// that is RBx==RFx, RBy==RFy, and RBz==RFz. However, aligning two vectors
+// takes *two* constraints so that would be a total of 6 constraints, with only
+// 3 independent. The independent runtime constraints just enforce
+// perpendicularity, but can be satisfied in cases where some of the axes are
+// antiparallel so are not suited for the initial assembly. The runtime
+// constraints are thus three "constant angle" constraints, where the angle
+// is always 90 degrees:
+//
+// ~RFx * RBy = 0
+// ~RFy * RBz = 0
+// ~RFz * RBx = 0
+//
+// We'll work in A. See the "constant angle" constraint for details.
+//
+// -----------------
+// perr = ~RFx * RBy (with all axes expressed in A)
+// ~RFy * RBz
+// ~RFz * RBx
+// -----------------
+//
+// ---------------------------------
+// verr = ~(w_AF-w_AB) * (RFx % RBy)
+// = ~(w_AF-w_AB) * (RFy % RBz)
+// = ~(w_AF-w_AB) * (RFz % RBx)
+// ---------------------------------
+//
+// -----------------------------------------------------------------------
+// aerr = ~(b_AF-b_AB) * (RFx % RBy)
+// + ~(w_AF-w_AB) * ((w_AF%RFx) % RBy) - (w_AB%RBy) % RFx)
+// ~(b_AF-b_AB) * (RFy % RBz)
+// + ~(w_AF-w_AB) * ((w_AF%RFy) % RBz) - (w_AB%RBz) % RFy)
+// ~(b_AF-b_AB) * (RFz % RBx)
+// + ~(w_AF-w_AB) * ((w_AF%RFz) % RBx) - (w_AB%RBx) % RFz)
+// -----------------------------------------------------------------------
+//
+// Constraint torque can be determined by inspection of verr:
+// t_F = lambda_x * (RFx % RBy) (applied to body F)
+// + lambda_y * (RFy % RBz)
+// + lambda_z * (RFz % RBx)
+// t_B = -t_F (applied to body B)
+//
+
+// -----------------
+// perr = ~RFx * RBy (with all axes expressed in A)
+// ~RFy * RBz
+// ~RFz * RBx
+// -----------------
+void calcPositionErrorsVirtual
+ (const State& s, // Stage::Time
+ const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr) // mp of these
+ const
+{
+ assert(allX_AB.size()==2 && constrainedQ.size()==0 && perr.size()==3);
+
+ const Rotation& R_AB = getBodyRotation(allX_AB, B);
+ const Rotation& R_AF = getBodyRotation(allX_AB, F);
+ const Rotation RB = R_AB * defaultRB; // now expressed in A
+ const Rotation RF = R_AF * defaultRF;
+
+ Vec3::updAs(&perr[0]) = Vec3(~RF.x()*RB.y(),
+ ~RF.y()*RB.z(),
+ ~RF.z()*RB.x());
+}
+
+// ----------------------------------
+// verr = ~(w_AF-w_AB) * (RFx % RBy)
+// = ~(w_AF-w_AB) * (RFy % RBz)
+// = ~(w_AF-w_AB) * (RFz % RBx)
+// ----------------------------------
+void calcPositionDotErrorsVirtual
+ (const State& s, // Stage::Position
+ const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr) // mp of these
+ const
+{
+ assert(allV_AB.size()==2 && constrainedQDot.size()==0 && pverr.size()==3);
+ //TODO: should be able to get p info from State
+ const Rotation& R_AB = getBodyRotationFromState(s, B);
+ const Rotation& R_AF = getBodyRotationFromState(s, F);
+ const Rotation RB = R_AB * defaultRB; // now expressed in A
+ const Rotation RF = R_AF * defaultRF;
+
+ const Vec3& w_AB = getBodyAngularVelocity(allV_AB, B);
+ const Vec3& w_AF = getBodyAngularVelocity(allV_AB, F);
+ const Vec3 w_BF = w_AF-w_AB; // in A
+
+ Vec3::updAs(&pverr[0]) = Vec3( ~w_BF * (RF.x() % RB.y()),
+ ~w_BF * (RF.y() % RB.z()),
+ ~w_BF * (RF.z() % RB.x()) );
+}
+
+//------------------------------------------------------------------------
+// aerr = ~(b_AF-b_AB) * (RFx % RBy)
+// + ~(w_AF-w_AB) * ((w_AF%RFx) % RBy) - (w_AB%RBy) % RFx)
+// ~(b_AF-b_AB) * (RFy % RBz)
+// + ~(w_AF-w_AB) * ((w_AF%RFy) % RBz) - (w_AB%RBz) % RFy)
+// ~(b_AF-b_AB) * (RFz % RBx)
+// + ~(w_AF-w_AB) * ((w_AF%RFz) % RBx) - (w_AB%RBx) % RFz)
+//------------------------------------------------------------------------
+void calcPositionDotDotErrorsVirtual
+ (const State& s, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr) // mp of these
+ const
+{
+ assert(allA_AB.size()==2 && constrainedQDotDot.size()==0 && paerr.size()==3);
+ //TODO: should be able to get p and v info from State
+ const Rotation& R_AB = getBodyRotationFromState(s, B);
+ const Rotation& R_AF = getBodyRotationFromState(s, F);
+ const Rotation RB = R_AB * defaultRB; // now expressed in A
+ const Rotation RF = R_AF * defaultRF;
+
+ const Vec3& w_AB = getBodyAngularVelocityFromState(s, B);
+ const Vec3& w_AF = getBodyAngularVelocityFromState(s, F);
+ const Vec3 w_BF = w_AF-w_AB; // in A
+
+ const Vec3& b_AB = getBodyAngularAcceleration(allA_AB, B);
+ const Vec3& b_AF = getBodyAngularAcceleration(allA_AB, F);
+ const Vec3 b_BF = b_AF-b_AB; // in A
+
+ Vec3::updAs(&paerr[0]) =
+ Vec3( dot( b_BF, RF.x() % RB.y() )
+ + dot( w_BF, (w_AF%RF.x()) % RB.y() - (w_AB%RB.y()) % RF.x()),
+ dot( b_BF, RF.y() % RB.z() )
+ + dot( w_BF, (w_AF%RF.y()) % RB.z() - (w_AB%RB.z()) % RF.y()),
+ dot( b_BF, RF.z() % RB.x() )
+ + dot( w_BF, (w_AF%RF.z()) % RB.x() - (w_AB%RB.x()) % RF.z()));
+}
+
+// t_F = lambda_x * (RFx % RBy) (applied to body F)
+// + lambda_y * (RFy % RBz)
+// + lambda_z * (RFz % RBx)
+// t_B = -t_F (applied to body B)
+void addInPositionConstraintForcesVirtual
+ (const State& s, // Stage::Position
+ const Array_<Real>& multipliers, // mp of these
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedQIndex>& qForces)
+ const
+{
+ assert(multipliers.size()==3 && bodyForcesInA.size()==2
+ && qForces.size()==0);
+ const Vec3& lambda = Vec3::getAs(&multipliers[0]);
+
+ //TODO: should be able to get p info from State
+ const Rotation& R_AB = getBodyRotationFromState(s, B);
+ const Rotation& R_AF = getBodyRotationFromState(s, F);
+ const Rotation RB = R_AB * defaultRB; // now expressed in A
+ const Rotation RF = R_AF * defaultRF;
+
+ const Vec3 torque_F_A = lambda[0] * (RF.x() % RB.y())
+ + lambda[1] * (RF.y() % RB.z())
+ + lambda[2] * (RF.z() % RB.x());
+
+ addInBodyTorque(s, F, torque_F_A, bodyForcesInA);
+ addInBodyTorque(s, B, -torque_F_A, bodyForcesInA);
+}
+
+SimTK_DOWNCAST(ConstantOrientationImpl, ConstraintImpl);
+//------------------------------------------------------------------------------
+ private:
+friend class Constraint::ConstantOrientation;
+
+ConstrainedBodyIndex B; // B1 is "base" body
+ConstrainedBodyIndex F; // B2 is "follower" body
+
+Rotation defaultRB; // fixed to B, expressed in B frame; RB = R_B_RB
+Rotation defaultRF; // fixed to F, expressed in F frame; RF = R_F_RF
+};
+
+
+
+//==============================================================================
+// WELD IMPL
+//==============================================================================
+class Constraint::WeldImpl : public ConstraintImpl {
+static Real getDefaultAxisDisplayLength() {return 1;}
+static Vec3 getDefaultFrameColor(int which) {
+ return which==0 ? Blue : Purple;
+}
+public:
+WeldImpl()
+ : ConstraintImpl(6,0,0), axisDisplayLength(-1), // means "use default axis length"
+ frameBColor(-1), frameFColor(-1) // means "use default colors"
+{ // default Transforms are identity, i.e. body frames
+}
+WeldImpl* clone() const { return new WeldImpl(*this); }
+
+// Draw the two frames.
+void calcDecorativeGeometryAndAppendVirtual
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const;
+
+void setAxisDisplayLength(Real len) {
+ // len == 0 means "don't display"
+ // len < 0 means "use default"
+ invalidateTopologyCache();
+ axisDisplayLength = len >= 0 ? len : -1;
+}
+Real getAxisDisplayLength() const {
+ return axisDisplayLength < 0 ? getDefaultAxisDisplayLength() : axisDisplayLength;
+}
+
+void setFrameColor(int which, const Vec3& color) {
+ assert(which==0 || which==1);
+ // color[0] < 0 means "use default color for this frame"
+ invalidateTopologyCache();
+ if (which==0) frameBColor = color[0] < 0 ? Vec3(-1) : color;
+ else frameFColor = color[0] < 0 ? Vec3(-1) : color;
+}
+Vec3 getFrameColor(int which) const {
+ assert(which==0 || which==1);
+ if (which==0) return frameBColor[0] < 0 ? getDefaultFrameColor(0) : frameBColor;
+ else return frameFColor[0] < 0 ? getDefaultFrameColor(1) : frameFColor;
+}
+
+// Implementation of virtuals required for holonomic constraints.
+
+// For theory, look at the ConstantOrientation (1st 3 equations) and
+// Ball (last 3 equations) theory above. Otherwise just lay back and
+// enjoy the ride.
+
+void calcPositionErrorsVirtual
+ (const State& s, // Stage::Time
+ const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr) // mp of these
+ const
+{
+ assert(allX_AB.size()==2 && constrainedQ.size()==0 && perr.size()==6);
+
+ const Rotation& R_AB = getBodyRotation(allX_AB, B);
+ const Rotation& R_AF = getBodyRotation(allX_AB, F);
+ const Rotation RB = R_AB * defaultFrameB.R(); // now expressed in A
+ const Rotation RF = R_AF * defaultFrameF.R();
+
+ // Orientation error
+ Vec3::updAs(&perr[0]) = Vec3(~RF.x()*RB.y(),
+ ~RF.y()*RB.z(),
+ ~RF.z()*RB.x());
+
+ const Vec3 p_AF1 = findStationLocation(allX_AB, B, defaultFrameB.p());
+ const Vec3 p_AF2 = findStationLocation(allX_AB, F, defaultFrameF.p());
+
+ // position error
+ Vec3::updAs(&perr[3]) = p_AF2 - p_AF1;
+}
+
+void calcPositionDotErrorsVirtual
+ (const State& s, // Stage::Position
+ const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr) // mp of these
+ const
+{
+ assert(allV_AB.size()==2 && constrainedQDot.size()==0 && pverr.size()==6);
+ //TODO: should be able to get p info from State
+ const Rotation& R_AB = getBodyRotationFromState(s, B);
+ const Rotation& R_AF = getBodyRotationFromState(s, F);
+ const Rotation RB = R_AB * defaultFrameB.R(); // now expressed in A
+ const Rotation RF = R_AF * defaultFrameF.R();
+
+ const Vec3& w_AB = getBodyAngularVelocity(allV_AB, B);
+ const Vec3& w_AF = getBodyAngularVelocity(allV_AB, F);
+ const Vec3 w_BF = w_AF-w_AB; // in A
+
+ // orientation error
+ Vec3::updAs(&pverr[0]) = Vec3( ~w_BF * (RF.x() % RB.y()),
+ ~w_BF * (RF.y() % RB.z()),
+ ~w_BF * (RF.z() % RB.x()) );
+
+ //TODO: should be able to get p info from State
+ const Transform& X_AB = getBodyTransformFromState(s, B);
+ const Vec3 p_AF2 = findStationLocationFromState(s, F,
+ defaultFrameF.p());
+ const Vec3 p_BC = ~X_AB*p_AF2; // C is a material point of body B
+
+ const Vec3 v_AF2 = findStationVelocity(s, allV_AB, F,
+ defaultFrameF.p());
+ const Vec3 v_AC = findStationVelocity(s, allV_AB, B, p_BC);
+
+ // position error
+ Vec3::updAs(&pverr[3]) = v_AF2 - v_AC;
+}
+
+void calcPositionDotDotErrorsVirtual
+ (const State& s, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr) // mp of these
+ const
+{
+ assert(allA_AB.size()==2 && constrainedQDotDot.size()==0 && paerr.size()==6);
+ //TODO: should be able to get p and v info from State
+ const Rotation& R_AB = getBodyRotationFromState(s, B);
+ const Rotation& R_AF = getBodyRotationFromState(s, F);
+ const Rotation RB = R_AB * defaultFrameB.R(); // now expressed in A
+ const Rotation RF = R_AF * defaultFrameF.R();
+
+ const Vec3& w_AB = getBodyAngularVelocityFromState(s, B);
+ const Vec3& w_AF = getBodyAngularVelocityFromState(s, F);
+ const Vec3 w_BF = w_AF-w_AB; // in A
+
+ const Vec3& b_AB = getBodyAngularAcceleration(allA_AB, B);
+ const Vec3& b_AF = getBodyAngularAcceleration(allA_AB, F);
+ const Vec3 b_BF = b_AF-b_AB; // in A
+
+ // orientation error
+ Vec3::updAs(&paerr[0]) =
+ Vec3( dot( b_BF, RF.x() % RB.y() )
+ + dot( w_BF, (w_AF%RF.x()) % RB.y() - (w_AB%RB.y()) % RF.x()),
+ dot( b_BF, RF.y() % RB.z() )
+ + dot( w_BF, (w_AF%RF.y()) % RB.z() - (w_AB%RB.z()) % RF.y()),
+ dot( b_BF, RF.z() % RB.x() )
+ + dot( w_BF, (w_AF%RF.z()) % RB.x() - (w_AB%RB.x()) % RF.z()));
+
+ const Transform& X_AB = getBodyTransformFromState(s, B);
+ const Vec3 p_AF2 = findStationLocationFromState(s, F,
+ defaultFrameF.p());
+ const Vec3 p_BC = ~X_AB*p_AF2; // C is a material point of body B
+
+ const Vec3 a_AF2 = findStationAcceleration(s, allA_AB, F,
+ defaultFrameF.p());
+ const Vec3 a_AC = findStationAcceleration(s, allA_AB, B, p_BC);
+
+ // position error
+ Vec3::updAs(&paerr[3]) = a_AF2 - a_AC;
+}
+
+void addInPositionConstraintForcesVirtual
+ (const State& s, // Stage::Position
+ const Array_<Real>& multipliers, // mp of these
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedQIndex>& qForces)
+ const
+{
+ assert(multipliers.size()==6 && bodyForcesInA.size()==2
+ && qForces.size()==0);
+
+ const Vec3& torques = Vec3::getAs(&multipliers[0]);
+ const Vec3& force_A = Vec3::getAs(&multipliers[3]);
+
+ //TODO: should be able to get p info from State
+ const Rotation& R_AB = getBodyRotationFromState(s, B);
+ const Rotation& R_AF = getBodyRotationFromState(s, F);
+ const Rotation RB = R_AB * defaultFrameB.R(); // now expressed in A
+ const Rotation RF = R_AF * defaultFrameF.R();
+
+ const Vec3 torque_F_A = torques[0] * (RF.x() % RB.y())
+ + torques[1] * (RF.y() % RB.z())
+ + torques[2] * (RF.z() % RB.x());
+
+ addInBodyTorque(s, F, torque_F_A, bodyForcesInA);
+ addInBodyTorque(s, B, -torque_F_A, bodyForcesInA);
+
+ const Transform& X_AB = getBodyTransformFromState(s,B);
+ const Vec3& p_FF2 = defaultFrameF.p();
+ const Vec3 p_AF2 = findStationLocationFromState(s, F, p_FF2);
+ const Vec3 p_BC = ~X_AB * p_AF2;
+
+ addInStationForce(s, F, p_FF2, force_A, bodyForcesInA);
+ addInStationForce(s, B, p_BC, -force_A, bodyForcesInA);
+}
+
+SimTK_DOWNCAST(WeldImpl, ConstraintImpl);
+//------------------------------------------------------------------------------
+ private:
+friend class Constraint::Weld;
+
+ConstrainedBodyIndex B; // aka "body 1"
+ConstrainedBodyIndex F; // aka "body 2"
+
+Transform defaultFrameB; // on body 1, relative to B frame
+Transform defaultFrameF; // on body 2, relative to F frame};
+
+// These are for visualization control only.
+Real axisDisplayLength; // for all 6 axes; <= 0 means "don't
+Vec3 frameBColor; // display"
+Vec3 frameFColor;
+};
+
+
+
+//==============================================================================
+// NO SLIP 1D IMPL
+//==============================================================================
+class Constraint::NoSlip1DImpl : public ConstraintImpl {
+public:
+NoSlip1DImpl()
+: ConstraintImpl(0,1,0), defaultContactPoint(0), defaultNoSlipDirection(),
+ directionLength(1), pointRadius(Real(0.05))
+{ }
+NoSlip1DImpl* clone() const { return new NoSlip1DImpl(*this); }
+
+// The default contact point and no-slip direction may be overridden by
+// setting an instance variable in the state. We allocate the state
+// resources here.
+void realizeTopologyVirtual(State& state) const;
+
+// Return the contact point and no-slip direction, both expressed
+// in the Case body frame C.
+const std::pair<Vec3,UnitVec3>& getContactInfo(const State& state) const;
+
+// Return a writable reference into the Instance-stage state variable
+// containing the contact point and no-slip direction, both expressed
+// in the Case body frame C. Calling this
+// method invalidates the Instance stage and above in the given state.
+std::pair<Vec3,UnitVec3>& updContactInfo(State& state) const;
+
+void calcDecorativeGeometryAndAppendVirtual
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const;
+
+void setDirectionDisplayLength(Real l) {
+ // l <= 0 means don't display direction line
+ invalidateTopologyCache();
+ directionLength = l > 0 ? l : 0;
+}
+Real getDirectionDisplayLength() const {return directionLength;}
+
+void setPointDisplayRadius(Real r) {
+ // r <= 0 means don't display point
+ invalidateTopologyCache();
+ pointRadius= r > 0 ? r : 0;
+}
+Real getPointDisplayRadius() const {return pointRadius;}
+
+// Implementation of virtuals required for nonholonomic constraints.
+
+// One non-holonomic constraint equation. There is a contact point P and a
+// no-slip direction n fixed in a case body C. There are two moving bodies B0
+// and B1. The material point P0 of B0 and the material point P1 of B1 which
+// are each coincident with the contact point P must have identical velocities
+// in C, along the direction n. This can be used to implement simple rolling
+// contact between disks, such as occurs in gear trains.
+//
+// There is no perr equation here since this is a non-holonomic (velocity)
+// constraint. In the C frame, the constraint we want is
+// verr = ~(v_CP1 - v_CP0) * n_C
+// that is, the two contact points have no relative velocity in C along the
+// normal. We can calculate this in A instead since the velocities in C of each
+// point will differ from their velocities in A by a constant (because they are
+// both in the same place in space). So:
+// verr = ~(v_AP1 - v_AP0) * n_A
+// Differentiating material point velocities in A, we get the acceleration
+// error
+// aerr = ~(a_AP1 - a_AP0) * n_A + ~(v_AP1 - v_AP0) * (w_AC X n_A)
+// = ~(a_AP1 - a_AP0 - w_AC X (v_AP1 - v_AP0)) * n_A
+//
+void calcVelocityErrorsVirtual
+ (const State& s, // Stage::Position
+ const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedU,
+ Array_<Real>& verr) // mv of these
+ const
+{
+ // There ought to be at least 2 distinct bodies, up to 3.
+ assert((allV_AB.size()==2 || allV_AB.size() == 3)
+ && constrainedU.size()==0 && verr.size()==1);
+
+ const std::pair<Vec3,UnitVec3>& info = getContactInfo(s);
+ const Vec3& P_C = info.first;
+ const UnitVec3& n_C = info.second;
+
+ //TODO: should be able to get p info from State
+ const Transform& X_AC = getBodyTransformFromState(s, caseBody);
+ const Transform& X_AB0 = getBodyTransformFromState(s, movingBody0);
+ const Transform& X_AB1 = getBodyTransformFromState(s, movingBody1);
+ const Vec3 p_AP = X_AC * P_C; // P's location in A
+ const Vec3 p_P0 = ~X_AB0 * p_AP; // P0's station in B0
+ const Vec3 p_P1 = ~X_AB1 * p_AP; // P1's station in B1
+ const UnitVec3 n_A = X_AC.R() * n_C;
+
+ const Vec3 v_AP0 = findStationVelocity(s, allV_AB, movingBody0, p_P0);
+ const Vec3 v_AP1 = findStationVelocity(s, allV_AB, movingBody1, p_P1);
+
+ // Calculate this scalar using A-frame vectors.
+ verr[0] = ~(v_AP1-v_AP0) * n_A;
+}
+
+void calcVelocityDotErrorsVirtual
+ (const State& s, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedUDot,
+ Array_<Real>& vaerr) // mv of these
+ const
+{
+ assert((allA_AB.size()==2 || allA_AB.size() == 3)
+ && constrainedUDot.size()==0 && vaerr.size()==1);
+
+ const std::pair<Vec3,UnitVec3>& info = getContactInfo(s);
+ const Vec3& P_C = info.first;
+ const UnitVec3& n_C = info.second;
+
+ //TODO: should be able to get p and v info from State
+ const Transform& X_AC = getBodyTransformFromState(s, caseBody);
+ const Transform& X_AB0 = getBodyTransformFromState(s, movingBody0);
+ const Transform& X_AB1 = getBodyTransformFromState(s, movingBody1);
+ const Vec3 p_AP = X_AC * P_C; // P's location in A
+ const Vec3 p_P0 = ~X_AB0 * p_AP; // P0's station in B0
+ const Vec3 p_P1 = ~X_AB1 * p_AP; // P1's station in B1
+ const UnitVec3 n_A = X_AC.R() * n_C;
+
+ const Vec3 v_AP0 = findStationVelocityFromState(s, movingBody0, p_P0);
+ const Vec3 v_AP1 = findStationVelocityFromState(s, movingBody1, p_P1);
+ const Vec3& w_AC = getBodyAngularVelocityFromState(s, caseBody);
+
+ const Vec3 a_AP0 = findStationAcceleration(s, allA_AB, movingBody0, p_P0);
+ const Vec3 a_AP1 = findStationAcceleration(s, allA_AB, movingBody1, p_P1);
+
+ // Calculate this scalar using A-frame vectors.
+ vaerr[0] = ~(a_AP1-a_AP0 - w_AC % (v_AP1-v_AP0)) * n_A;
+}
+
+// apply f=lambda*n to contact point P1 of B1,
+// -f to contact point P2 of B2
+void addInVelocityConstraintForcesVirtual
+ (const State& s, // Stage::Velocity
+ const Array_<Real>& multipliers, // mv of these
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedUIndex>& mobilityForces) const
+{
+ assert(multipliers.size()==1 && mobilityForces.size()==0
+ && (bodyForcesInA.size()==2 || bodyForcesInA.size()==3));
+
+ const std::pair<Vec3,UnitVec3>& info = getContactInfo(s);
+ const Vec3& P_C = info.first;
+ const UnitVec3& n_C = info.second;
+
+ const Real lambda = multipliers[0];
+
+ //TODO: should be able to get p info from State
+ const Transform& X_AC = getBodyTransformFromState(s, caseBody);
+ const Transform& X_AB0 = getBodyTransformFromState(s, movingBody0);
+ const Transform& X_AB1 = getBodyTransformFromState(s, movingBody1);
+ const Vec3 p_AP = X_AC * P_C; // P's location in A
+ const Vec3 p_P0 = ~X_AB0 * p_AP; // P0's station in B0
+ const Vec3 p_P1 = ~X_AB1 * p_AP; // P1's station in B1
+
+ const Vec3 force_A = X_AC.R()*(lambda*n_C);
+
+ addInStationForce(s, movingBody1, p_P1, force_A, bodyForcesInA);
+ addInStationForce(s, movingBody0, p_P0, -force_A, bodyForcesInA);
+}
+
+SimTK_DOWNCAST(NoSlip1DImpl, ConstraintImpl);
+//------------------------------------------------------------------------------
+ private:
+friend class Constraint::NoSlip1D;
+
+ConstrainedBodyIndex caseBody; // C
+ConstrainedBodyIndex movingBody0; // B0
+ConstrainedBodyIndex movingBody1; // B1
+
+Vec3 defaultContactPoint; // on body C, exp. in C frame
+UnitVec3 defaultNoSlipDirection; // on body C, exp. in C frame
+
+// These are just for visualization
+Real directionLength;
+Real pointRadius;
+
+// This Instance-stage variable holds the actual contact point and no-slip
+// direction on the Case body.
+mutable DiscreteVariableIndex contactInfoIx;
+};
+
+
+
+//==============================================================================
+// BALL ROLLING ON PLANE IMPL
+//==============================================================================
+/// TODO!!
+class Constraint::BallRollingOnPlaneImpl : public ConstraintImpl {
+public:
+BallRollingOnPlaneImpl()
+: ConstraintImpl(1,2,0), defaultPlaneNormal(), defaultPlaneHeight(0),
+ defaultBallCenter(0), defaultBallRadius(NaN),
+ planeHalfWidth(1)
+{ }
+BallRollingOnPlaneImpl* clone() const { return new BallRollingOnPlaneImpl(*this); }
+
+void calcDecorativeGeometryAndAppendVirtual
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const;
+
+void setPlaneDisplayHalfWidth(Real h) {
+ // h <= 0 means don't display plane
+ invalidateTopologyCache();
+ planeHalfWidth = h > 0 ? h : 0;
+}
+Real getPlaneDisplayHalfWidth() const {return planeHalfWidth;}
+
+
+// Implementation of virtuals required for holonomic constraints.
+
+// TODO
+// We have a plane attached to base body B, and a sphere of radius r attached
+// to follower body F, with the sphere center at O. The plane normal is n, the
+// plane height over B's origin is h. We would like the bottom point S of the
+// sphere to be in contact with the plane, that is, the height of S should be
+// h, or equivalently the center height should be h+r. Now S=O-r*n, so we have
+// perr = S*n - h = (p_BO - r*n_B)*n_B - h
+// or perr = O*n - (h+r) = p_BO*n_B - (h+r)
+// If we work entirely in body B, we have
+// verr = d/dt_B perr = v_BO * n_B
+// aerr = d/dt_B verr = a_BO * n_B
+//
+// Then we apply equal and opposite forces lambda*n to stations of B and F
+// that are coincident with the contact point C.
+// TODO: C should be half way between S and the point on the plane nearest S.
+// Can I apply it there? Or do the constraint errors have to reflect
+// that to keep the force transmission matrix the transpose of G?
+//
+// TODO: need utility methods like in MobilizedBody for getting p_BO, v_BO
+// and a_BO rather than just A frame values.
+
+SimTK_DOWNCAST(BallRollingOnPlaneImpl, ConstraintImpl);
+//------------------------------------------------------------------------------
+ private:
+friend class Constraint::BallRollingOnPlane;
+
+ConstrainedBodyIndex planeBody; // B1
+ConstrainedBodyIndex ballBody; // B2
+
+UnitVec3 defaultPlaneNormal; // on body 1, exp. in B1 frame
+Real defaultPlaneHeight;
+Vec3 defaultBallCenter; // on body 2, exp. in B2 frame
+Real defaultBallRadius;
+
+// This is just for visualization
+Real planeHalfWidth;
+};
+
+
+
+//==============================================================================
+// CONSTANT SPEED IMPL
+//==============================================================================
+class Constraint::ConstantSpeedImpl : public ConstraintImpl {
+public:
+ConstantSpeedImpl()
+: ConstraintImpl(0,1,0), theMobilizer(), whichMobility(),
+ defaultSpeed(NaN){}
+
+ConstantSpeedImpl* clone() const
+{ return new ConstantSpeedImpl(*this); }
+
+// Allocate a state variable to hold the desired speed.
+void realizeTopologyVirtual(State& state) const;
+// Obtain the currently-set desired speed from the state.
+Real getSpeed(const State& state) const;
+// Get a reference to the desired speed in the state; this
+// invalidates Velocity stage in the supplied state.
+Real& updSpeed(State& state) const;
+
+// Implementation of virtuals required for nonholonomic constraints.
+
+// One non-holonomic (well, velocity-level) constraint equation.
+// verr = u - s
+// aerr = udot
+//
+void calcVelocityErrorsVirtual
+ (const State& s, // Stage::Position
+ const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedU,
+ Array_<Real>& verr) // mv of these
+ const
+{
+ // All the u's for a given constrained mobilizer are considered
+ // constrainedU's, but we're just going to grab one of them.
+ assert(allV_AB.size()==0 && constrainedU.size()>=1 && verr.size()==1);
+ const Real u = getOneU(s, constrainedU, theMobilizer, whichMobility);
+ verr[0] = u - getSpeed(s);
+}
+
+void calcVelocityDotErrorsVirtual
+ (const State& s, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedUDot,
+ Array_<Real>& vaerr) // mv of these
+ const
+{
+ // All the u's for a given constrained mobilizer are considered
+ // constrainedUDots's, but we're just going to grab one of them.
+ assert(allA_AB.size()==0 && constrainedUDot.size()>=1 && vaerr.size()==1);
+ const Real udot = getOneUDot(s, constrainedUDot,
+ theMobilizer, whichMobility);
+ vaerr[0] = udot;
+}
+
+// apply generalized force lambda to the mobility
+void addInVelocityConstraintForcesVirtual
+ (const State& s, // Stage::Velocity
+ const Array_<Real>& multipliers, // mv of these
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedUIndex>& mobilityForces) const
+{
+ // All the mobilities for a given constrained mobilizer have slots in
+ // mobilizedForces, but we're just going to update one of them.
+ assert(multipliers.size()==1 && bodyForcesInA.size()==0
+ && mobilityForces.size()>=1);
+
+ const Real lambda = multipliers[0];
+ addInOneMobilityForce(s, theMobilizer, whichMobility, lambda,
+ mobilityForces);
+}
+
+SimTK_DOWNCAST(ConstantSpeedImpl, ConstraintImpl);
+//------------------------------------------------------------------------------
+ private:
+friend class Constraint::ConstantSpeed;
+
+// TOPOLOGY STATE
+ConstrainedMobilizerIndex theMobilizer;
+MobilizerUIndex whichMobility;
+Real defaultSpeed;
+
+// TOPOLOGY CACHE
+DiscreteVariableIndex speedIx;
+};
+
+
+
+//==============================================================================
+// CONSTANT ACCELERATION IMPL
+//==============================================================================
+class Constraint::ConstantAccelerationImpl : public ConstraintImpl {
+public:
+ConstantAccelerationImpl()
+: ConstraintImpl(0,0,1), theMobilizer(), whichMobility(),
+ defaultAcceleration(NaN) {}
+
+ConstantAccelerationImpl* clone() const
+{ return new ConstantAccelerationImpl(*this); }
+
+// Allocate a state variable to hold the desired acceleration.
+void realizeTopologyVirtual(State& state) const;
+// Obtain the currently-set desired acceleration from the state.
+Real getAcceleration(const State& state) const;
+// Get a reference to the desired acceleration in the state; this
+// invalidates Acceleration stage in the supplied state.
+Real& updAcceleration(State& state) const;
+
+// Implementation of virtuals required for acceleration-only constraints.
+
+// One acceleration-only constraint equation.
+// aerr = udot - a
+//
+void calcAccelerationErrorsVirtual
+ (const State& s, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedUDot,
+ Array_<Real>& aerr) const // ma of these
+{
+ // All the u's for a given constrained mobilizer are considered
+ // constrainedUDots's, but we're just going to grab one of them.
+ assert(allA_AB.size()==0 && constrainedUDot.size()>=1 && aerr.size()==1);
+
+ const Real udot = getOneUDot(s, constrainedUDot,
+ theMobilizer, whichMobility);
+ aerr[0] = udot - getAcceleration(s);
+}
+
+// apply generalized force lambda to the mobility
+void addInAccelerationConstraintForcesVirtual
+ (const State& state, // Stage::Velocity
+ const Array_<Real>& multipliers, // ma of these
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedUIndex>& mobilityForces) const
+{
+ // All the mobilities for a given constrained mobilizer have slots in
+ // mobilizedForces, but we're just going to update one of them.
+ assert(multipliers.size()==1 && bodyForcesInA.size()==0
+ && mobilityForces.size()>=1);
+
+ const Real lambda = multipliers[0];
+ addInOneMobilityForce(state, theMobilizer, whichMobility, lambda,
+ mobilityForces);
+}
+
+SimTK_DOWNCAST(ConstantAccelerationImpl, ConstraintImpl);
+//------------------------------------------------------------------------------
+ private:
+friend class Constraint::ConstantAcceleration;
+
+// TOPOLOGY STATE
+ConstrainedMobilizerIndex theMobilizer;
+MobilizerUIndex whichMobility;
+Real defaultAcceleration;
+
+// TOPOLOGY CACHE
+DiscreteVariableIndex accelIx;
+};
+
+
+
+//==============================================================================
+// CUSTOM IMPLEMENTATION IMPL
+//==============================================================================
+// This class exists primarily to allow the Custom::Implementation class to keep
+// a pointer to its handle class's CustomImpl class which is derived from
+// ConstraintImpl which has all the goodies that are needed for defining a
+// Constraint.
+//
+// At first this class is the owner of the CustomImpl. Then when this is put in
+// a Custom handle, that handle takes over ownership of the CustomImpl and the
+// CustomImpl takes over ownership of this ImplementationImpl object.
+class Constraint::Custom::ImplementationImpl
+: public PIMPLImplementation<Implementation, ImplementationImpl>
+{
+public:
+// no default constructor
+explicit ImplementationImpl(CustomImpl* customImpl) : isOwner(true), builtInImpl(customImpl) { }
+inline ~ImplementationImpl(); // see below -- have to wait for CustomImpl's definition
+
+// Copying one of these just gives us a new one with a NULL CustomImpl pointer.
+ImplementationImpl(const ImplementationImpl& src) : isOwner(false), builtInImpl(0) { }
+
+ImplementationImpl* clone() const {return new ImplementationImpl(*this);}
+
+bool isOwnerOfCustomImpl() const {return builtInImpl && isOwner;}
+CustomImpl* removeOwnershipOfCustomImpl() {
+ assert(isOwnerOfCustomImpl());
+ isOwner=false;
+ return builtInImpl;
+}
+
+void setReferenceToCustomImpl(CustomImpl* cimpl) {
+ assert(!builtInImpl); // you can only do this once
+ isOwner=false;
+ builtInImpl = cimpl;
+}
+
+bool hasCustomImpl() const {return builtInImpl != 0;}
+
+const CustomImpl& getCustomImpl() const {
+ assert(builtInImpl);
+ return *builtInImpl;
+}
+CustomImpl& updCustomImpl() {
+ assert(builtInImpl);
+ return *builtInImpl;
+}
+
+//------------------------------------------------------------------------------
+ private:
+bool isOwner;
+CustomImpl* builtInImpl; // just a reference; not owned
+
+// suppress assignment
+ImplementationImpl& operator=(const ImplementationImpl&);
+};
+
+
+
+//==============================================================================
+// CUSTOM IMPL
+//==============================================================================
+class Constraint::CustomImpl : public ConstraintImpl {
+public:
+CustomImpl() : implementation(0) { }
+CustomImpl(int mp, int mv, int ma) : ConstraintImpl(mp,mv,ma), implementation(0) { }
+
+void takeOwnershipOfImplementation(Custom::Implementation* userImpl);
+
+explicit CustomImpl(Custom::Implementation* userImpl) : implementation(0) {
+ assert(userImpl);
+ implementation = userImpl;
+ implementation->updImpl().setReferenceToCustomImpl(this);
+}
+
+// Copy constructor
+CustomImpl(const CustomImpl& src) : implementation(0) {
+ if (src.implementation) {
+ implementation = src.implementation->clone();
+ implementation->updImpl().setReferenceToCustomImpl(this);
+ }
+}
+
+~CustomImpl() {
+ delete implementation;
+}
+
+CustomImpl* clone() const { return new CustomImpl(*this); }
+
+const Custom::Implementation& getImplementation() const {
+ assert(implementation);
+ return *implementation;
+}
+
+Custom::Implementation& updImplementation() {
+ assert(implementation);
+ return *implementation;
+}
+
+// Forward all the virtuals to the Custom::Implementation virtuals.
+void realizeTopologyVirtual(State& s) const {getImplementation().realizeTopology(s);}
+void realizeModelVirtual (State& s) const {getImplementation().realizeModel(s);}
+void realizeInstanceVirtual(const State& s) const {getImplementation().realizeInstance(s);}
+void realizeTimeVirtual (const State& s) const {getImplementation().realizeTime(s);}
+void realizePositionVirtual(const State& s) const {getImplementation().realizePosition(s);}
+void realizeVelocityVirtual(const State& s) const {getImplementation().realizeVelocity(s);}
+void realizeDynamicsVirtual(const State& s) const {getImplementation().realizeDynamics(s);}
+void realizeAccelerationVirtual(const State& s) const {getImplementation().realizeAcceleration(s);}
+void realizeReportVirtual (const State& s) const {getImplementation().realizeReport(s);}
+
+void calcPositionErrorsVirtual
+ (const State& state,
+ const Array_<Transform,ConstrainedBodyIndex>& X_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr) const
+{ getImplementation().calcPositionErrors(state,X_AB,constrainedQ,perr); }
+
+void calcPositionDotErrorsVirtual
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr) const
+{ getImplementation().calcPositionDotErrors
+ (state,V_AB,constrainedQDot,pverr); }
+
+void calcPositionDotDotErrorsVirtual
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr) const
+{ getImplementation().calcPositionDotDotErrors
+ (state,A_AB,constrainedQDotDot,paerr); }
+
+void addInPositionConstraintForcesVirtual
+ (const State& s,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real,ConstrainedQIndex>& qForces) const
+{ getImplementation().addInPositionConstraintForces
+ (s,multipliers,bodyForcesInA,qForces); }
+
+void calcVelocityErrorsVirtual
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedU,
+ Array_<Real>& verr) const
+{ getImplementation().calcVelocityErrors(state,V_AB,constrainedU,verr); }
+
+void calcVelocityDotErrorsVirtual
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedUDot,
+ Array_<Real>& vaerr) const
+{ getImplementation().calcVelocityDotErrors
+ (state,A_AB,constrainedUDot,vaerr); }
+
+void addInVelocityConstraintForcesVirtual
+ (const State& s,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real,ConstrainedUIndex>& mobilityForces) const
+{ getImplementation().addInVelocityConstraintForces
+ (s,multipliers,bodyForcesInA,mobilityForces); }
+
+void calcAccelerationErrorsVirtual
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedUDot,
+ Array_<Real>& aerr) const
+{ getImplementation().calcVelocityDotErrors
+ (state,A_AB,constrainedUDot,aerr); }
+
+void addInAccelerationConstraintForcesVirtual
+ (const State& s,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real,ConstrainedUIndex>& mobilityForces) const
+{ getImplementation().addInAccelerationConstraintForces
+ (s,multipliers,bodyForcesInA,mobilityForces); }
+
+void calcDecorativeGeometryAndAppendVirtual
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+ {getImplementation().calcDecorativeGeometryAndAppend(s,stage,geom);}
+
+SimTK_DOWNCAST(CustomImpl, ConstraintImpl);
+//------------------------------------------------------------------------------
+ private:
+friend class Constraint::Custom;
+
+Custom::Implementation* implementation;
+
+CustomImpl& operator=(const CustomImpl&); // suppress assignment
+};
+
+// Need definition for CustomImpl here in case we have to delete it.
+inline Constraint::Custom::ImplementationImpl::~ImplementationImpl() {
+ if (isOwner)
+ delete builtInImpl;
+ builtInImpl=0;
+}
+
+
+
+//==============================================================================
+// COORDINATE COUPLER IMPL
+//==============================================================================
+class Constraint::CoordinateCouplerImpl
+: public Constraint::Custom::Implementation {
+public:
+CoordinateCouplerImpl(SimbodyMatterSubsystem& matter,
+ const Function* function,
+ const Array_<MobilizedBodyIndex>& coordBody,
+ const Array_<MobilizerQIndex>& coordIndex);
+
+~CoordinateCouplerImpl() {
+ if (--referenceCount[0] == 0) {
+ delete function;
+ delete[] referenceCount;
+ }
+}
+
+Implementation* clone() const {
+ referenceCount[0]++;
+ CoordinateCouplerImpl* newCoupler = new CoordinateCouplerImpl(*this);
+ return newCoupler;
+}
+
+void calcPositionErrors
+ (const State& state,
+ const Array_<Transform,ConstrainedBodyIndex>& X_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr) const;
+
+void calcPositionDotErrors
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr) const;
+
+void calcPositionDotDotErrors
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr) const;
+
+void addInPositionConstraintForces
+ (const State& state,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForces,
+ Array_<Real,ConstrainedQIndex>& qForces) const;
+
+//------------------------------------------------------------------------------
+ private:
+friend class Constraint::CoordinateCoupler;
+
+// TOPOLOGY STATE
+const Function* function;
+Array_<ConstrainedMobilizerIndex> coordBodies;
+Array_<MobilizerQIndex> coordIndices;
+
+// TOPOLOGY CACHE
+// None.
+
+// A reusable temporary variable allocated to the correct size
+// to hold all the Function arguments.
+mutable Vector temp;
+
+// This allows copies to be made of this constraint which share
+// the function object.
+int* referenceCount;
+
+};
+
+
+
+//==============================================================================
+// SPEED COUPLER IMPL
+//==============================================================================
+class Constraint::SpeedCouplerImpl
+: public Constraint::Custom::Implementation {
+public:
+SpeedCouplerImpl(SimbodyMatterSubsystem& matter,
+ const Function* function,
+ const Array_<MobilizedBodyIndex>& speedBody,
+ const Array_<MobilizerUIndex>& speedIndex,
+ const Array_<MobilizedBodyIndex>& coordBody,
+ const Array_<MobilizerQIndex>& coordIndex);
+
+~SpeedCouplerImpl() {
+ if (--referenceCount[0] == 0) {
+ delete function;
+ delete[] referenceCount;
+ }
+}
+
+Implementation* clone() const {
+ referenceCount[0]++;
+ return new SpeedCouplerImpl(*this);
+}
+
+void calcVelocityErrors
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedU,
+ Array_<Real>& verr) const;
+
+void calcVelocityDotErrors
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedUDot,
+ Array_<Real>& vaerr) const;
+
+void addInVelocityConstraintForces
+ (const State& state,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForces,
+ Array_<Real,ConstrainedUIndex>& mobilityForces) const;
+
+//------------------------------------------------------------------------------
+ private:
+
+const Function* function;
+int* referenceCount;
+Array_<ConstrainedMobilizerIndex> speedBodies;
+Array_<MobilizedBodyIndex> coordBodies;
+Array_<MobilizerUIndex> speedIndices;
+Array_<MobilizerQIndex> coordIndices;
+mutable Vector temp;
+};
+
+
+
+//==============================================================================
+// PRESCRIBED MOTION IMPL
+//==============================================================================
+class Constraint::PrescribedMotionImpl
+: public Constraint::Custom::Implementation {
+public:
+PrescribedMotionImpl(SimbodyMatterSubsystem& matter,
+ const Function* function,
+ MobilizedBodyIndex coordBody,
+ MobilizerQIndex coordIndex);
+
+~PrescribedMotionImpl() {
+ if (--referenceCount[0] == 0) {
+ delete function;
+ delete[] referenceCount;
+ }
+}
+
+Implementation* clone() const {
+ referenceCount[0]++;
+ return new PrescribedMotionImpl(*this);
+}
+
+void calcPositionErrors
+ (const State& state,
+ const Array_<Transform,ConstrainedBodyIndex>& X_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr) const;
+
+void calcPositionDotErrors
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr) const;
+
+void calcPositionDotDotErrors
+ (const State& state,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr) const;
+
+void addInPositionConstraintForces
+ (const State& state,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForces,
+ Array_<Real,ConstrainedQIndex>& qForces) const;
+
+//------------------------------------------------------------------------------
+ private:
+const Function* function;
+int* referenceCount;
+ConstrainedMobilizerIndex coordBody;
+MobilizerQIndex coordIndex;
+mutable Vector temp;
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_CONSTRAINT_IMPL_H_
+
+
+
diff --git a/Simbody/src/ContactTrackerSubsystem.cpp b/Simbody/src/ContactTrackerSubsystem.cpp
new file mode 100644
index 0000000..ab3094d
--- /dev/null
+++ b/Simbody/src/ContactTrackerSubsystem.cpp
@@ -0,0 +1,671 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/MultibodySystem.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "simbody/internal/ContactTrackerSubsystem.h"
+
+#include <algorithm>
+using std::pair; using std::make_pair;
+#include <iostream>
+using std::cout; using std::endl;
+#include <set>
+
+
+namespace SimTK {
+
+// We keep a list of all the ContactSurfaces being tracked by this
+// subsystem, and a separate list of "bubble wrap" spheres that are
+// used in broad phase determination of which contact surfaces need
+// to be examined more closely by ContactTracker objects.
+
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(BubbleIndex);
+
+//TODO: should these be organized by body rather than surface?
+// surfaces on ground certainly should!
+
+struct Surface {
+ const MobilizedBody* mobod;
+ const ContactSurface* surface; // in its own frame S
+ Transform X_BS; // surface's pose on body
+};
+
+struct Bubble {
+ const Vec3& getCenter() const {return sphere.getSubVec<3>(0);}
+ const Real& getRadius() const {return sphere[3];}
+ Vec3& updCenter() {return sphere.updSubVec<3>(0);}
+ Real& updRadius() {return sphere[3];}
+
+ Vec4 sphere; // x,y,z,r in body frame
+ ContactSurfaceIndex surface; // associated surface
+};
+static std::ostream& operator<<(std::ostream& o, const Bubble& bb) {
+ o << bb.sphere << "->" << bb.surface;
+ return o;
+}
+
+// For spatial sorting of bubbles.
+class BubbleExtent {
+public:
+ BubbleExtent(Real start, Real end, BubbleIndex index)
+ : start(start), end(end), index(index) {}
+ BubbleExtent() {}
+ bool operator<(const BubbleExtent& e) const
+ { return start < e.start; }
+ Real start, end; // span along chosen sort axis
+ BubbleIndex index;
+};
+static std::ostream& operator<<(std::ostream& o, const BubbleExtent& bx) {
+ o << bx.index << ":(" << bx.start << "," << bx.end << ")";
+ return o;
+}
+
+typedef std::map< pair<ContactGeometryTypeId,ContactGeometryTypeId>,
+ pair<ContactTracker*,bool> > TrackerMap;
+
+// This type maps a contact surface onto a set of all the higher-numbered
+// contact surfaces it might be touching, and for each of those we keep
+// a pointer to that pair's Contact object if it is currently being tracked
+// (null if it is new). You must use the *lower* numbered surface as the key
+// when inserting a pair so that any given pair of surfaces appears just once.
+// However, the surface order in the Contact object will be determined by
+// the order required by the corresponding tracker.
+// TODO: replace with a fast data structure
+typedef std::map<ContactSurfaceIndex,const Contact*> ContactSurfaceSet;
+typedef std::map<ContactSurfaceIndex,ContactSurfaceSet> PairMap;
+
+static std::ostream& operator<<(std::ostream& o, const ContactSurfaceSet& css) {
+ ContactSurfaceSet::const_iterator p = css.begin();
+ o << "{";
+ for (; p != css.end(); ++p) {
+ if (p!=css.begin()) o << ", ";
+ o << p->first << ":0x" << p->second;
+ }
+ return o << "}";
+}
+static std::ostream& operator<<(std::ostream& o, const PairMap& pm) {
+ PairMap::const_iterator p = pm.begin();
+ for (; p != pm.end(); ++p) {
+ if (p != pm.begin()) o << endl;
+ o << p->first << "->" << p->second;
+ }
+ return o;
+}
+
+
+
+//==============================================================================
+// CONTACT TRACKER SUBSYSTEM IMPL
+//==============================================================================
+class ContactTrackerSubsystemImpl : public Subsystem::Guts {
+public:
+// Constructor registers a default set of Trackers to use with geometry
+// we know about. These can be overridden later.
+ContactTrackerSubsystemImpl() : m_defaultTracker(0) {
+ adoptContactTracker(new ContactTracker::HalfSpaceSphere());
+ adoptContactTracker(new ContactTracker::SphereSphere());
+ adoptContactTracker(new ContactTracker::HalfSpaceEllipsoid());
+ adoptContactTracker(new ContactTracker::HalfSpaceTriangleMesh());
+ adoptContactTracker(new ContactTracker::SphereTriangleMesh());
+ adoptContactTracker(new ContactTracker::TriangleMeshTriangleMesh());
+
+ // Handle sphere-ellipsoid and ellipsoid-ellipsoid by treating them as
+ // convex objects represented by their implicit functions.
+ adoptContactTracker(new ContactTracker::ConvexImplicitPair
+ (ContactGeometry::Sphere::classTypeId(),
+ ContactGeometry::Ellipsoid::classTypeId()));
+ adoptContactTracker(new ContactTracker::ConvexImplicitPair
+ (ContactGeometry::Ellipsoid::classTypeId(),
+ ContactGeometry::Ellipsoid::classTypeId()));
+}
+
+~ContactTrackerSubsystemImpl() {
+ delete m_defaultTracker;
+ TrackerMap::iterator p = m_contactTrackers.begin();
+ for (; p != m_contactTrackers.end(); ++p)
+ delete p->second.first; // the tracker
+ // The map itself gets deleted automatically.
+}
+
+ContactTrackerSubsystemImpl* cloneImpl() const
+{ return new ContactTrackerSubsystemImpl(*this); }
+
+void adoptContactTracker(ContactTracker* tracker) {
+ assert(tracker);
+ // This is the order required by the tracker's trackContact() method.
+ const pair<ContactGeometryTypeId,ContactGeometryTypeId>&
+ types = tracker->getContactGeometryTypeIds();
+ ContactGeometryTypeId low=types.first, high=types.second;
+ const bool mustReverse = (low > high);
+ if (mustReverse) std::swap(low,high);
+ m_contactTrackers[make_pair(low,high)] = make_pair(tracker, mustReverse);
+}
+
+// Return the MultibodySystem which owns this ContactTrackerSubsystem.
+const MultibodySystem& getMultibodySystem() const
+{ return MultibodySystem::downcast(getSystem()); }
+
+// Return the SimbodyMatterSubsystem from which this ContactTrackerSubsystem
+// gets the bodies to track.
+const SimbodyMatterSubsystem& getMatterSubsystem() const
+{ return getMultibodySystem().getMatterSubsystem(); }
+
+// Get access to state variables and cache entries.
+
+const ContactSnapshot& getPrevActiveContacts(const State& state) const {
+ const ContactSnapshot& contacts = Value<ContactSnapshot>::downcast
+ (getDiscreteVariable(state, m_activeContactsIx));
+ return contacts;
+}
+const ContactSnapshot& getPrevPredictedContacts(const State& state) const {
+ const ContactSnapshot& contacts = Value<ContactSnapshot>::downcast
+ (getDiscreteVariable(state, m_predictedContactsIx));
+ return contacts;
+}
+const ContactSnapshot& getNextActiveContacts(const State& state) const {
+ const ContactSnapshot& contacts = Value<ContactSnapshot>::downcast
+ (getDiscreteVarUpdateValue(state, m_activeContactsIx));
+ return contacts;
+}
+const ContactSnapshot& getNextPredictedContacts(const State& state) const {
+ const ContactSnapshot& contacts = Value<ContactSnapshot>::downcast
+ (getDiscreteVarUpdateValue(state, m_predictedContactsIx));
+ return contacts;
+}
+ContactSnapshot& updNextActiveContacts(const State& state) const {
+ ContactSnapshot& contacts = Value<ContactSnapshot>::downcast
+ (updDiscreteVarUpdateValue(state, m_activeContactsIx));
+ return contacts;
+}
+ContactSnapshot& updNextPredictedContacts(const State& state) const {
+ ContactSnapshot& contacts = Value<ContactSnapshot>::downcast
+ (updDiscreteVarUpdateValue(state, m_predictedContactsIx));
+ return contacts;
+}
+
+// Run through all the bodies to find the contact surfaces, assigning each
+// a unique ContactSurfaceIndex. Then for each surface, get its geometry
+// and create a Bubble from each of its bubble wrap spheres; each of those
+// gets a unique BubbleIndex that maps back to the associated surface.
+int realizeSubsystemTopologyImpl(State& state) const {
+ // Briefly allow writing into the Topology cache; after this the
+ // Topology cache is const.
+ ContactTrackerSubsystemImpl* wThis =
+ const_cast<ContactTrackerSubsystemImpl*>(this);
+ wThis->m_activeContactsIx = allocateAutoUpdateDiscreteVariable
+ (state, Stage::Dynamics, new Value<ContactSnapshot>(),
+ Stage::Position); // update depends on positions
+ wThis->m_predictedContactsIx = allocateAutoUpdateDiscreteVariable
+ (state, Stage::Dynamics, new Value<ContactSnapshot>(),
+ Stage::Acceleration); // update depends on accelerations
+
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+
+ const int numBodies = matter.getNumBodies();
+ wThis->m_surfaces.clear();
+ wThis->m_bubbles.clear();
+
+ for (MobilizedBodyIndex mbx(0); mbx < numBodies; ++mbx) {
+ const MobilizedBody& mobod = matter.getMobilizedBody(mbx);
+ const Body& body = mobod.getBody();
+ for (int i=0; i < body.getNumContactSurfaces(); ++i) {
+ const ContactSurfaceIndex surfx(m_surfaces.size());
+ wThis->m_surfaces.push_back(); // default construct
+ Surface& surf = wThis->m_surfaces.back();
+ surf.mobod = &mobod;
+ surf.surface = &body.getContactSurface(i);
+ surf.X_BS = body.getContactSurfaceTransform(i);
+ const ContactGeometry& geo = surf.surface->getShape();
+ // for each bubble:
+ wThis->m_bubbles.push_back(); // default construct
+ Bubble& bubble = wThis->m_bubbles.back();
+ Vec3 center_S;
+ geo.getBoundingSphere(center_S, bubble.updRadius());
+ bubble.updCenter() = surf.X_BS*center_S; // in B
+ bubble.surface = surfx;
+ }
+ }
+ //cout << "Bubbles:" << m_bubbles << "\n";
+ return 0;
+}
+
+int realizeSubsystemPositionImpl(const State& state) const {
+ return 0;
+}
+
+int realizeSubsystemVelocityImpl(const State& state) const {
+ return 0;
+}
+
+// Adds new pairs to the existing set, if not already present.
+void addInBroadPhasePairs(const State& state, PairMap& pairs) const {
+ const int numBubbles = getNumBubbles();
+
+ // Perform a sweep-and-prune on a single axis to identify potential
+ // contacts. First, find which axis has the most variation in body
+ // locations. That is the axis we will use.
+ // TODO: this one-axis method is not good enough in general
+
+ Vector_<Vec3> centers(numBubbles);
+ for (BubbleIndex bbx(0); bbx < numBubbles; ++bbx) {
+ const Bubble& bubb = m_bubbles[bbx];
+ const Surface& surf = m_surfaces[bubb.surface];
+ centers[bbx] = surf.mobod->getBodyTransform(state)
+ * bubb.getCenter();
+ }
+ Vec3 average = mean(centers);
+ Vec3 var(0);
+ for (BubbleIndex bbx(0); bbx < numBubbles; ++bbx)
+ var += abs(centers[bbx]-average);
+ int axis = (var[0] > var[1] ? 0 : 1);
+ if (var[2] > var[axis])
+ axis = 2;
+
+ // Find the extent of each bubble along the axis and sort them by
+ // starting location.
+ Array_<BubbleExtent,int> extents(numBubbles);
+ for (BubbleIndex bbx(0); bbx < numBubbles; ++bbx) {
+ const Real radius = m_bubbles[bbx].getRadius();
+ const Real center = centers[bbx][axis];
+ extents[bbx] = BubbleExtent(center-radius, center+radius, bbx);
+ }
+ //cout << "Bubble extents:" << extents << "\n";
+
+ // Expensive: O(n log n)
+ std::sort(extents.begin(), extents.end());
+
+ // Now sweep along the axis, finding potential contacts.
+
+ for (int ex1=0; ex1 < numBubbles; ++ex1) {
+ const BubbleExtent& extent1 = extents[ex1];
+ const Bubble& bubb1 = m_bubbles[extent1.index];
+ const Real radius1 = bubb1.getRadius();
+ const Vec3& center1 = centers[extent1.index];
+
+ // Loop over just the overlapping bubbles.
+ for (int ex2(ex1+1); ex2 < numBubbles; ++ex2) {
+ const BubbleExtent& extent2 = extents[ex2];
+ if (extent2.start > extent1.end)
+ break; // no more bubbles can overlap with extent1
+
+ // These bubbles do overlap along this axis. See if they are
+ // actually touching.
+ const Bubble& bubb2 = m_bubbles[extent2.index];
+ const Real radius2 = bubb2.getRadius();
+ const Vec3& center2 = centers[extent2.index];
+ if ((center1-center2).normSqr() > square(radius1+radius2))
+ continue; // nope
+
+ // The bubbles are touching. We'll add the corresponding surfaces
+ // to the narrow-phase list unless there are relevant exclusions.
+ const Surface& surf1 = m_surfaces[bubb1.surface];
+ const Surface& surf2 = m_surfaces[bubb2.surface];
+ // Ignore if on the same body.
+ if (surf1.mobod == surf2.mobod) continue;
+ assert(bubb1.surface != bubb2.surface); // duh!
+ // Ignore if surfaces are in a common clique.
+ if (surf1.surface->isInSameClique(*surf2.surface)) continue;
+ // We'll need to do a narrow phase investigation of these two
+ // surfaces; use the lower-numbered one as the index to avoid
+ // duplicates.
+ ContactSurfaceIndex low=bubb1.surface, high=bubb2.surface;
+ if (low > high) std::swap(low,high);
+ ContactSurfaceSet& surfSet = pairs[low];
+ // Insert this pair with null Contact if the pair isn't already
+ // in the PairMap.
+ surfSet.insert(make_pair(high,(Contact*)0));
+ }
+ }
+}
+
+// Call this any time after positions are known, to ensure that the active
+// contact set has been updated for those positions. We can use three
+// sources of information to compute the update:
+// - The previously-known set of active contacts
+// - The previously-known set of predicted contacts
+// - The current contact surface positions
+// Note that we cannot update the set of predicted contacts here because
+// that requires velocity and acceleration information.
+//
+// Algorithm:
+// for all "interesting" surface pairs (surf1,surf2):
+// prev = getPrevActiveContact(surf1,surf2) (might be empty)
+// trackContact(prev, boundsNow?, next)
+// updNextActiveContact(surf1,surf2) = next (might be empty)
+// "interesting" means
+// - previously active, or
+// - previously predicted, or
+// - broad phase position bounds intersect
+void ensureActiveContactsUpdated(const State& state) const {
+ if (isDiscreteVarUpdateValueRealized(state, m_activeContactsIx))
+ return; // already done
+
+ const ContactSnapshot& active = getPrevActiveContacts(state);
+ const ContactSnapshot& predicted = getPrevPredictedContacts(state);
+ ContactSnapshot& nextActive = updNextActiveContacts(state);
+
+ // TODO: Can we reuse heap space in this cache entry?
+ nextActive.clear();
+
+ PairMap interesting;
+ for (int i=0; i < active.getNumContacts(); ++i) {
+ const Contact& contact = active.getContact(i);
+ ContactSurfaceIndex low=contact.getSurface1(),
+ high=contact.getSurface2();
+ if (low > high) std::swap(low,high);
+ ContactSurfaceSet& others = interesting[low];
+ assert(others.find(high)==others.end());
+ others[high] = &contact;
+ }
+ for (int i=0; i < predicted.getNumContacts(); ++i) {
+ const Contact& contact = predicted.getContact(i);
+ ContactSurfaceIndex low=contact.getSurface1(),
+ high=contact.getSurface2();
+ if (low > high) std::swap(low,high);
+ ContactSurfaceSet& others = interesting[low];
+ assert(others.find(high)==others.end());
+ others[high] = &contact;
+ }
+ // This will ignore pairs that we already inserted above; new ones
+ // will be inserted with null Contact object pointers.
+ addInBroadPhasePairs(state, interesting);
+ //cout << "Interesting pairs:\n" << interesting << "\n";
+
+ PairMap::const_iterator p = interesting.begin();
+ for (; p != interesting.end(); ++p) {
+ const ContactSurfaceIndex index1 = p->first;
+ const Transform transform1 =
+ m_surfaces[index1].mobod->getBodyTransform(state)
+ * m_surfaces[index1].X_BS;
+ const ContactGeometry& geom1 = m_surfaces[index1].surface->getShape();
+ const ContactGeometryTypeId typeId1 = geom1.getTypeId();
+
+ const ContactSurfaceSet& others = p->second;
+ ContactSurfaceSet::const_iterator q = others.begin();
+ for (; q != others.end(); ++q) {
+ const ContactSurfaceIndex index2 = q->first;
+ const Transform transform2 =
+ m_surfaces[index2].mobod->getBodyTransform(state)
+ * m_surfaces[index2].X_BS;
+ const ContactGeometry& geom2 =
+ m_surfaces[index2].surface->getShape();
+ const ContactGeometryTypeId typeId2 = geom2.getTypeId();
+ if (!hasContactTracker(typeId1,typeId2))
+ continue; // No algorithm available for detecting collisions between these two objects.
+ bool mustReverse;
+ const ContactTracker& tracker =
+ getContactTracker(typeId1, typeId2, mustReverse);
+
+ // Put the surfaces in the order required by the tracker.
+ const ContactSurfaceIndex trackSurf1 = (mustReverse? index2:index1);
+ const ContactSurfaceIndex trackSurf2 = (mustReverse? index1:index2);
+
+ UntrackedContact untracked; // empty handle in case we need it
+ const Contact* prev = q->second;
+ if (prev && prev->getCondition() == Contact::Broken)
+ prev = 0; // that contact expired
+ if (!prev) {
+ untracked = UntrackedContact(trackSurf1, trackSurf2);
+ prev = &untracked;
+ }
+ Contact next; // empty handle
+ if (mustReverse)
+ tracker.trackContact
+ (*prev, transform2,geom2, transform1,geom1, 0/*TODO*/, next);
+ else
+ tracker.trackContact
+ (*prev, transform1,geom1, transform2,geom2, 0/*TODO*/, next);
+
+ if (!next.isEmpty()) {
+ next.setSurfaces(trackSurf1,trackSurf2);
+ next.setContactId(prev->getCondition()==Contact::Untracked
+ ? Contact::createNewContactId()
+ : prev->getContactId()); // persistent
+ if ( prev->getCondition()==Contact::Untracked
+ || prev->getCondition()==Contact::Anticipated)
+ next.setCondition(Contact::NewContact);
+ else { // was NewContact or Ongoing; now Ongoing or Broken
+ assert(prev->getCondition()==Contact::NewContact
+ || prev->getCondition()==Contact::Ongoing);
+ if (next.getTypeId() != BrokenContact::classTypeId())
+ next.setCondition(Contact::Ongoing);
+ // Condition will already by Broken for a BrokenContact
+ }
+ nextActive.adoptContact(next);
+ }
+ }
+ }
+
+ markDiscreteVarUpdateValueRealized(state, m_activeContactsIx);
+}
+
+// Call this any time after accelerations are known, to ensure that the
+// predicted contact set has been updated for new velocities and accelerations.
+// We can use three sources of information to compute the update:
+// - The current (updated) set of active contacts
+// - The previously-known set of impending contacts
+// - The current contact surface positions, velocities, and accelerations
+// The active contact update cannot be modified here although we can
+// initiate its computation if it hasn't been done yet; after that it is
+// frozen.
+//
+// Algorithm:
+// for all "interesting" surface pairs (surf1,surf2):
+// prev = getPrevPredictedContact(surf1,surf2) (might be empty)
+// predictContact(prev, projBoundsNow?, next)
+// updNextImpendingContact(surf1,surf2) = next (might be empty)
+// "interesting" means not currently active and:
+// - previously impending, or
+// - fast(surf1)||fast(surf2) and
+// fast object broad phase projected bounds intersect
+void ensurePredictedContactsUpdated(const State& state) const {
+ if (isDiscreteVarUpdateValueRealized(state, m_predictedContactsIx))
+ return; // already done
+
+ const ContactSnapshot& nextActive = getNextActiveContacts(state);
+ const ContactSnapshot& prevPredicted = getPrevPredictedContacts(state);
+ ContactSnapshot& nextPredicted = updNextPredictedContacts(state);
+ // TODO
+ markDiscreteVarUpdateValueRealized(state, m_predictedContactsIx);
+}
+
+int realizeSubsystemDynamicsImpl(const State& state) const {
+ ensureActiveContactsUpdated(state);
+ return 0;
+}
+
+int realizeSubsystemAccelerationImpl(const State& state) const {
+ ensurePredictedContactsUpdated(state);
+ return 0;
+}
+
+bool hasContactTracker(ContactGeometryTypeId id1,
+ ContactGeometryTypeId id2) const
+{ if (id1 > id2) std::swap(id1,id2); // (low,high) order for lookup
+ return m_contactTrackers.find(make_pair(id1,id2))
+ != m_contactTrackers.end();
+}
+
+const ContactTracker&
+getContactTracker(ContactGeometryTypeId id1, ContactGeometryTypeId id2,
+ bool& mustReverse) const
+{ const bool inputSwapped = id1 > id2;
+ if (inputSwapped) std::swap(id1,id2); // (low,high) order for lookup
+ TrackerMap::const_iterator p = m_contactTrackers.find(make_pair(id1,id2));
+ const bool trackerSwapped = p->second.second;
+ if (p != m_contactTrackers.end()) {
+ mustReverse = (inputSwapped != trackerSwapped); // xor
+ assert(p->second.first);
+ return *p->second.first;
+ }
+ // Couldn't find a Tracker.
+
+ SimTK_ERRCHK2_ALWAYS(m_defaultTracker,
+ "ContactTrackerSubsystem::getContactTracker()",
+ "There was no registered ContactTracker for surface geometry"
+ " type ids (%d,%d) and there was no default tracker.",
+ (int)id1, (int)id2);
+
+ return *m_defaultTracker;
+}
+
+int getNumSurfaces() const {return m_surfaces.size();}
+int getNumBubbles() const {return m_bubbles.size();}
+
+SimTK_DOWNCAST(ContactTrackerSubsystemImpl, Subsystem::Guts);
+
+private:
+friend class ContactTrackerSubsystem;
+
+ // TOPOLOGY STATE
+// Always order the key with the lower numbered geometry type first but
+// if that is the reverse from how the tracker is defined then the bool
+// should be set to true meaning "reverse arguments".
+// This map owns the heap space for the ContactTrackers; be sure to
+// delete it when replacing or destructing.
+TrackerMap m_contactTrackers;
+ContactTracker* m_defaultTracker;
+
+ // TOPOLOGY CACHE
+Array_<Surface,ContactSurfaceIndex> m_surfaces;
+Array_<Bubble,BubbleIndex> m_bubbles;
+DiscreteVariableIndex m_activeContactsIx;
+DiscreteVariableIndex m_predictedContactsIx;
+};
+
+
+
+//==============================================================================
+// CONTACT TRACKER SUBSYSTEM
+//==============================================================================
+
+bool ContactTrackerSubsystem::isInstanceOf(const Subsystem& s) {
+ return ContactTrackerSubsystemImpl::isA(s.getSubsystemGuts());
+}
+const ContactTrackerSubsystem& ContactTrackerSubsystem::
+downcast(const Subsystem& s) {
+ assert(isInstanceOf(s));
+ return static_cast<const ContactTrackerSubsystem&>(s);
+}
+ContactTrackerSubsystem& ContactTrackerSubsystem::
+updDowncast(Subsystem& s) {
+ assert(isInstanceOf(s));
+ return static_cast<ContactTrackerSubsystem&>(s);
+}
+
+const ContactTrackerSubsystemImpl& ContactTrackerSubsystem::
+getImpl() const {
+ return SimTK_DYNAMIC_CAST_DEBUG<const ContactTrackerSubsystemImpl&>
+ (getSubsystemGuts());
+}
+ContactTrackerSubsystemImpl& ContactTrackerSubsystem::
+updImpl() {
+ return SimTK_DYNAMIC_CAST_DEBUG<ContactTrackerSubsystemImpl&>
+ (updSubsystemGuts());
+}
+
+// Create Subsystem but don't associate it with any System. This isn't much use
+// except for making std::vectors, which require a default constructor to be
+// available.
+ContactTrackerSubsystem::ContactTrackerSubsystem()
+{ adoptSubsystemGuts(new ContactTrackerSubsystemImpl()); }
+
+ContactTrackerSubsystem::ContactTrackerSubsystem(MultibodySystem& mbs)
+{ adoptSubsystemGuts(new ContactTrackerSubsystemImpl());
+ mbs.adoptSubsystem(*this); } // steal ownership
+
+int ContactTrackerSubsystem::getNumSurfaces() const
+{ return getImpl().getNumSurfaces(); }
+
+const MobilizedBody& ContactTrackerSubsystem::
+getMobilizedBody(ContactSurfaceIndex surfIx) const
+{ return *getImpl().m_surfaces[surfIx].mobod; }
+
+const ContactSurface& ContactTrackerSubsystem::
+getContactSurface(ContactSurfaceIndex surfIx) const
+{ return *getImpl().m_surfaces[surfIx].surface; }
+
+const Transform& ContactTrackerSubsystem::
+getContactSurfaceTransform(ContactSurfaceIndex surfIx) const
+{ return getImpl().m_surfaces[surfIx].X_BS; }
+
+void ContactTrackerSubsystem::
+adoptContactTracker(ContactTracker* tracker)
+{ updImpl().adoptContactTracker(tracker); }
+
+bool ContactTrackerSubsystem::
+hasContactTracker(ContactGeometryTypeId surface1,
+ ContactGeometryTypeId surface2) const
+{ return getImpl().hasContactTracker(surface1,surface2); }
+
+const ContactTracker& ContactTrackerSubsystem::
+getContactTracker(ContactGeometryTypeId surface1,
+ ContactGeometryTypeId surface2,
+ bool& reverseOrder) const
+{ return getImpl().getContactTracker(surface1,surface2,reverseOrder); }
+
+const ContactSnapshot& ContactTrackerSubsystem::
+getPreviousActiveContacts(const State& state) const
+{ return getImpl().getPrevActiveContacts(state); }
+
+const ContactSnapshot& ContactTrackerSubsystem::
+getPreviousPredictedContacts(const State& state) const
+{ return getImpl().getPrevPredictedContacts(state); }
+
+const ContactSnapshot& ContactTrackerSubsystem::
+getActiveContacts(const State& state) const {
+ Real dummy;
+ realizeActiveContacts(state,true,dummy);
+ return getImpl().getNextActiveContacts(state);
+}
+
+const ContactSnapshot& ContactTrackerSubsystem::
+getPredictedContacts(const State& state) const {
+ Real dummy;
+ realizePredictedContacts(state,true,dummy);
+ return getImpl().getNextPredictedContacts(state);
+}
+
+bool ContactTrackerSubsystem::
+realizeActiveContacts(const State& state,
+ bool lastTry,
+ Real& stepAdvice) const
+{ // TODO: errors and step advice
+ getImpl().ensureActiveContactsUpdated(state);
+ return true;
+}
+
+bool ContactTrackerSubsystem::
+realizePredictedContacts(const State& state,
+ bool lastTry,
+ Real& stepAdvice) const
+{ // TODO: errors and step advice
+ getImpl().ensurePredictedContactsUpdated(state);
+ return true;
+}
+
+
+} // namespace SimTK
+
diff --git a/Simbody/src/DecorationSubsystem.cpp b/Simbody/src/DecorationSubsystem.cpp
new file mode 100644
index 0000000..2b0ea96
--- /dev/null
+++ b/Simbody/src/DecorationSubsystem.cpp
@@ -0,0 +1,143 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ *
+ * Private implementation of DecorationSubsystem.
+ */
+
+#include "SimTKcommon.h"
+#include "simbody/internal/common.h"
+#include "simbody/internal/MultibodySystem.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "simbody/internal/DecorationSubsystem.h"
+
+#include "DecorationSubsystemRep.h"
+
+namespace SimTK {
+
+
+ //////////////////////////
+ // DECORATION SUBSYSTEM //
+ //////////////////////////
+
+/*static*/ bool
+DecorationSubsystem::isInstanceOf(const Subsystem& s) {
+ return DecorationSubsystemGuts::isA(s.getSubsystemGuts());
+}
+/*static*/ const DecorationSubsystem&
+DecorationSubsystem::downcast(const Subsystem& s) {
+ assert(isInstanceOf(s));
+ return static_cast<const DecorationSubsystem&>(s);
+}
+/*static*/ DecorationSubsystem&
+DecorationSubsystem::updDowncast(Subsystem& s) {
+ assert(isInstanceOf(s));
+ return static_cast<DecorationSubsystem&>(s);
+}
+
+const DecorationSubsystemGuts&
+DecorationSubsystem::getGuts() const {
+ return SimTK_DYNAMIC_CAST_DEBUG<const DecorationSubsystemGuts&>(getSubsystemGuts());
+}
+DecorationSubsystemGuts&
+DecorationSubsystem::updGuts() {
+ return SimTK_DYNAMIC_CAST_DEBUG<DecorationSubsystemGuts&>(updSubsystemGuts());
+}
+
+// Create Subsystem but don't associate it with any System. This isn't much use except
+// for making std::vector's, which require a default constructor to be available.
+DecorationSubsystem::DecorationSubsystem()
+ : Subsystem()
+{
+ adoptSubsystemGuts(new DecorationSubsystemGuts());
+}
+
+DecorationSubsystem::DecorationSubsystem(MultibodySystem& mbs)
+ : Subsystem()
+{
+ adoptSubsystemGuts(new DecorationSubsystemGuts());
+ mbs.setDecorationSubsystem(*this);
+}
+
+void DecorationSubsystem::addBodyFixedDecoration
+ (MobilizedBodyIndex body, const Transform& X_GD, const DecorativeGeometry& g)
+{
+ updGuts().addBodyFixedDecoration(body, X_GD, g);
+}
+
+void DecorationSubsystem::addRubberBandLine
+ (MobilizedBodyIndex b1, const Vec3& station1,
+ MobilizedBodyIndex b2, const Vec3& station2,
+ const DecorativeLine& g)
+{
+ updGuts().addRubberBandLine(b1,station1,b2,station2,g);
+}
+
+void DecorationSubsystem::addDecorationGenerator(Stage stage, DecorationGenerator* generator) {
+ updGuts().addDecorationGenerator(stage, generator);
+}
+
+ ///////////////////////////////
+ // DECORATION SUBSYSTEM GUTS //
+ ///////////////////////////////
+
+// Return the MultibodySystem which owns this DecorationSubsystem.
+const MultibodySystem& DecorationSubsystemGuts::getMultibodySystem() const {
+ return MultibodySystem::downcast(getSystem());
+}
+
+int DecorationSubsystemGuts::calcDecorativeGeometryAndAppendImpl
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+{
+ switch(stage) {
+ case Stage::Topology: {
+ assert(subsystemTopologyHasBeenRealized());
+ for (int i=0; i<(int)geometry.size(); ++i)
+ geom.push_back(geometry[i]);
+ break;
+ }
+ case Stage::Position: {
+ assert(getStage(s) >= Stage::Position);
+ const MultibodySystem& mbs = getMultibodySystem(); // my owner
+ const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
+ for (int i=0; i<(int)rubberBandLines.size(); ++i) {
+ const RubberBandLine& rb = rubberBandLines[i];
+ geom.push_back(rb.line); // make a new copy
+ DecorativeLine& line = DecorativeLine::updDowncast(geom.back()); // get access to copy
+ line.setEndpoints(
+ matter.getMobilizedBody(rb.body1).findStationLocationInGround(s,rb.station1),
+ matter.getMobilizedBody(rb.body2).findStationLocationInGround(s,rb.station2));
+ }
+ }
+ default:
+ assert(getStage(s) >= stage);
+ }
+ for (int i = 0; i < (int) generators[stage].size(); i++)
+ generators[stage][i]->generateDecorations(s, geom);
+
+ return 0;
+}
+
+} // namespace SimTK
+
diff --git a/Simbody/src/DecorationSubsystemRep.h b/Simbody/src/DecorationSubsystemRep.h
new file mode 100644
index 0000000..551116b
--- /dev/null
+++ b/Simbody/src/DecorationSubsystemRep.h
@@ -0,0 +1,171 @@
+#ifndef SimTK_SIMBODY_DECORATION_SUBSYSTEM_REP_H_
+#define SimTK_SIMBODY_DECORATION_SUBSYSTEM_REP_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) 2007-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ *
+ * Private implementation of DecorationSubsystem.
+ */
+
+#include "SimTKcommon.h"
+
+#include "simbody/internal/common.h"
+#include "simbody/internal/DecorationSubsystem.h"
+
+namespace SimTK {
+
+class DecorationSubsystemGuts : public Subsystem::Guts {
+
+ struct RubberBandLine {
+ RubberBandLine(MobilizedBodyIndex b1, const Vec3& s1,
+ MobilizedBodyIndex b2, const Vec3& s2,
+ const DecorativeLine& l)
+ : body1(b1), body2(b2), station1(s1), station2(s2), line(l)
+ {
+ }
+ MobilizedBodyIndex body1, body2;
+ Vec3 station1, station2;
+ DecorativeLine line;
+ };
+
+public:
+ DecorationSubsystemGuts()
+ : Subsystem::Guts("DecorationSubsystem", "0.0.1"), generators(Stage::NValid)
+ {
+ }
+
+ ~DecorationSubsystemGuts() {
+ for (int i = 0; i < (int) generators.size(); i++)
+ for (int j = 0; j < (int) generators[i].size(); j++)
+ delete generators[i][j];
+ }
+
+ // Return the MultibodySystem which owns this DecorationSubsystem.
+ const MultibodySystem& getMultibodySystem() const;
+
+ // Add a permanent ("Topological") piece of geometry which is permanently fixed to a single body.
+ // Thus the 3D polygonal representation can be precalculated once and for all at Stage::Topology,
+ // then transformed at Stage::Position for display on a screen.
+ //
+ // This will make an internal copy of the supplied DecorativeGeometry. We'll save the
+ // body Id in and apply the transform now to the saved copy, so that the geometry we return
+ // later will be relative to the body frame only.
+ void addBodyFixedDecoration(MobilizedBodyIndex body, const Transform& X_BD, const DecorativeGeometry& g)
+ {
+ invalidateSubsystemTopologyCache(); // this is a topological change
+ geometry.push_back(g); // make a new copy
+ DecorativeGeometry& myg = geometry.back();
+ myg.setBodyId(body);
+ myg.setTransform(X_BD*myg.getTransform());
+ }
+
+ // This will make an internal copy of the supplied DecorativeGeometry.
+ void addRubberBandLine(MobilizedBodyIndex b1, const Vec3& station1, MobilizedBodyIndex b2, const Vec3& station2,
+ const DecorativeLine& g)
+ {
+ invalidateSubsystemTopologyCache(); // this is a topological change
+ rubberBandLines.push_back(RubberBandLine(b1,station1,b2,station2,g));
+ DecorativeLine& myg = rubberBandLines.back().line; // new copy
+ myg.setBodyId(GroundIndex); // make sure the generated geometry will display properly
+ myg.setTransform(Transform());
+ }
+
+ void addDecorationGenerator(Stage stage, DecorationGenerator* generator) {
+ generators[stage].push_back(generator);
+ }
+
+ DecorationSubsystemGuts* cloneImpl() const {
+ return new DecorationSubsystemGuts(*this);
+ }
+
+ // Override virtual realize methods, although at the moment there is nothing here
+ // so the default implementations would have been fine.
+
+ int realizeSubsystemTopologyImpl(State& s) const {
+ // No Topology-stage cache here
+ return 0;
+ }
+
+ int realizeSubsystemModelImpl(State& s) const {
+ // Sorry, no choices available at the moment.
+ return 0;
+ }
+
+ int realizeSubsystemInstanceImpl(const State& s) const {
+ // Nothing to compute here.
+ return 0;
+ }
+
+ int realizeSubsystemTimeImpl(const State& s) const {
+ // Nothing to compute here.
+ return 0;
+ }
+
+ int realizeSubsystemPositionImpl(const State& s) const {
+ // Nothing to compute here.
+ return 0;
+ }
+
+ int realizeSubsystemVelocityImpl(const State& s) const {
+ // Nothing to compute here.
+ return 0;
+ }
+
+ int realizeSubsystemDynamicsImpl(const State& s) const {
+ // Nothing to compute here.
+ return 0;
+ }
+
+ int realizeSubsystemAccelerationImpl(const State& s) const {
+ // Nothing to compute here.
+ return 0;
+ }
+
+ int realizeSubsystemReportImpl(const State& s) const {
+ // Nothing to compute here.
+ return 0;
+ }
+
+ int calcDecorativeGeometryAndAppendImpl
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const;
+
+ SimTK_DOWNCAST(DecorationSubsystemGuts, Subsystem::Guts);
+private:
+ // TOPOLOGY "STATE" VARIABLES
+ Array_<DecorativeGeometry> geometry;
+ Array_<RubberBandLine> rubberBandLines;
+ Array_<Array_<DecorationGenerator*> > generators;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_DECORATION_SUBSYSTEM_REP_H_
+
+
+
+
+
+
+
diff --git a/Simbody/src/ElasticFoundationForce.cpp b/Simbody/src/ElasticFoundationForce.cpp
new file mode 100644
index 0000000..715088d
--- /dev/null
+++ b/Simbody/src/ElasticFoundationForce.cpp
@@ -0,0 +1,208 @@
+/* -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/GeneralContactSubsystem.h"
+#include "simbody/internal/MobilizedBody.h"
+#include "ElasticFoundationForceImpl.h"
+#include <map>
+#include <set>
+
+namespace SimTK {
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(ElasticFoundationForce, ElasticFoundationForceImpl, Force);
+
+ElasticFoundationForce::ElasticFoundationForce(GeneralForceSubsystem& forces, GeneralContactSubsystem& contacts, ContactSetIndex set) :
+ Force(new ElasticFoundationForceImpl(contacts, set)) {
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+void ElasticFoundationForce::setBodyParameters
+ (ContactSurfaceIndex bodyIndex, Real stiffness, Real dissipation,
+ Real staticFriction, Real dynamicFriction, Real viscousFriction) {
+ updImpl().setBodyParameters(bodyIndex, stiffness, dissipation, staticFriction, dynamicFriction, viscousFriction);
+}
+
+Real ElasticFoundationForce::getTransitionVelocity() const {
+ return getImpl().transitionVelocity;
+}
+
+void ElasticFoundationForce::setTransitionVelocity(Real v) {
+ updImpl().transitionVelocity = v;
+}
+
+ElasticFoundationForceImpl::ElasticFoundationForceImpl
+ (GeneralContactSubsystem& subsystem, ContactSetIndex set) :
+ subsystem(subsystem), set(set), transitionVelocity(Real(0.01)) {
+}
+
+void ElasticFoundationForceImpl::setBodyParameters
+ (ContactSurfaceIndex bodyIndex, Real stiffness, Real dissipation,
+ Real staticFriction, Real dynamicFriction, Real viscousFriction) {
+ SimTK_APIARGCHECK1(bodyIndex >= 0 && bodyIndex < subsystem.getNumBodies(set), "ElasticFoundationForceImpl", "setBodyParameters",
+ "Illegal body index: %d", (int)bodyIndex);
+ SimTK_APIARGCHECK1(subsystem.getBodyGeometry(set, bodyIndex).getTypeId()
+ == ContactGeometry::TriangleMesh::classTypeId(),
+ "ElasticFoundationForceImpl", "setBodyParameters",
+ "Body %d is not a triangle mesh", (int)bodyIndex);
+ parameters[bodyIndex] =
+ Parameters(stiffness, dissipation, staticFriction, dynamicFriction,
+ viscousFriction);
+ const ContactGeometry::TriangleMesh& mesh =
+ ContactGeometry::TriangleMesh::getAs
+ (subsystem.getBodyGeometry(set, bodyIndex));
+ Parameters& param = parameters[bodyIndex];
+ param.springPosition.resize(mesh.getNumFaces());
+ param.springNormal.resize(mesh.getNumFaces());
+ param.springArea.resize(mesh.getNumFaces());
+ Vec2 uv(Real(1./3.), Real(1./3.));
+ for (int i = 0; i < (int) param.springPosition.size(); i++) {
+ param.springPosition[i] =
+ (mesh.getVertexPosition(mesh.getFaceVertex(i, 0))
+ +mesh.getVertexPosition(mesh.getFaceVertex(i, 1))
+ +mesh.getVertexPosition(mesh.getFaceVertex(i, 2)))/3;
+ param.springNormal[i] = -mesh.findNormalAtPoint(i, uv);
+ param.springArea[i] = mesh.getFaceArea(i);
+ }
+ subsystem.invalidateSubsystemTopologyCache();
+}
+
+void ElasticFoundationForceImpl::calcForce
+ (const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+{
+ const Array_<Contact>& contacts = subsystem.getContacts(state, set);
+ Real& pe = Value<Real>::downcast
+ (subsystem.updCacheEntry(state, energyCacheIndex));
+ pe = 0.0;
+ for (int i = 0; i < (int) contacts.size(); i++) {
+ std::map<ContactSurfaceIndex, Parameters>::const_iterator iter1 =
+ parameters.find(contacts[i].getSurface1());
+ std::map<ContactSurfaceIndex, Parameters>::const_iterator iter2 =
+ parameters.find(contacts[i].getSurface2());
+
+ // If there are two meshes, scale each one's contributions by 50%.
+ Real areaScale = (iter1==parameters.end() || iter2==parameters.end())
+ ? Real(1) : Real(0.5);
+
+ if (iter1 != parameters.end()) {
+ const TriangleMeshContact& contact =
+ static_cast<const TriangleMeshContact&>(contacts[i]);
+ processContact(state, contact.getSurface1(),
+ contact.getSurface2(), iter1->second,
+ contact.getSurface1Faces(), areaScale, bodyForces, pe);
+ }
+
+ if (iter2 != parameters.end()) {
+ const TriangleMeshContact& contact =
+ static_cast<const TriangleMeshContact&>(contacts[i]);
+ processContact(state, contact.getSurface2(),
+ contact.getSurface1(), iter2->second,
+ contact.getSurface2Faces(), areaScale, bodyForces, pe);
+ }
+ }
+}
+
+void ElasticFoundationForceImpl::processContact
+ (const State& state,
+ ContactSurfaceIndex meshIndex, ContactSurfaceIndex otherBodyIndex,
+ const Parameters& param, const std::set<int>& insideFaces,
+ Real areaScale, Vector_<SpatialVec>& bodyForces, Real& pe) const
+{
+ const ContactGeometry& otherObject = subsystem.getBodyGeometry(set, otherBodyIndex);
+ const MobilizedBody& body1 = subsystem.getBody(set, meshIndex);
+ const MobilizedBody& body2 = subsystem.getBody(set, otherBodyIndex);
+ const Transform t1g = body1.getBodyTransform(state)*subsystem.getBodyTransform(set, meshIndex); // mesh to ground
+ const Transform t2g = body2.getBodyTransform(state)*subsystem.getBodyTransform(set, otherBodyIndex); // other object to ground
+ const Transform t12 = ~t2g*t1g; // mesh to other object
+
+ // Loop over all the springs, and evaluate the force from each one.
+
+ for (std::set<int>::const_iterator iter = insideFaces.begin();
+ iter != insideFaces.end(); ++iter) {
+ int face = *iter;
+ UnitVec3 normal;
+ bool inside;
+ Vec3 nearestPoint = otherObject.findNearestPoint(t12*param.springPosition[face], inside, normal);
+ if (!inside)
+ continue;
+
+ // Find how much the spring is displaced.
+
+ nearestPoint = t2g*nearestPoint;
+ const Vec3 springPosInGround = t1g*param.springPosition[face];
+ const Vec3 displacement = nearestPoint-springPosInGround;
+ const Real distance = displacement.norm();
+ if (distance == 0.0)
+ continue;
+ const Vec3 forceDir = displacement/distance;
+
+ // Calculate the relative velocity of the two bodies at the contact point.
+
+ const Vec3 station1 = body1.findStationAtGroundPoint(state, nearestPoint);
+ const Vec3 station2 = body2.findStationAtGroundPoint(state, nearestPoint);
+ const Vec3 v1 = body1.findStationVelocityInGround(state, station1);
+ const Vec3 v2 = body2.findStationVelocityInGround(state, station2);
+ const Vec3 v = v2-v1;
+ const Real vnormal = dot(v, forceDir);
+ const Vec3 vtangent = v-vnormal*forceDir;
+
+ // Calculate the damping force.
+
+ const Real area = areaScale * param.springArea[face];
+ const Real f = param.stiffness*area*distance*(1+param.dissipation*vnormal);
+ Vec3 force = (f > 0 ? f*forceDir : Vec3(0));
+
+ // Calculate the friction force.
+
+ const Real vslip = vtangent.norm();
+ if (f > 0 && vslip != 0) {
+ const Real vrel = vslip/transitionVelocity;
+ const Real ffriction =
+ f*(std::min(vrel, Real(1))
+ *(param.dynamicFriction+2*(param.staticFriction-param.dynamicFriction)
+ /(1+vrel*vrel))+param.viscousFriction*vslip);
+ force += ffriction*vtangent/vslip;
+ }
+
+ body1.applyForceToBodyPoint(state, station1, force, bodyForces);
+ body2.applyForceToBodyPoint(state, station2, -force, bodyForces);
+ pe += param.stiffness*area*displacement.normSqr()/2;
+ }
+}
+
+Real ElasticFoundationForceImpl::calcPotentialEnergy(const State& state) const {
+ return Value<Real>::downcast
+ (subsystem.getCacheEntry(state, energyCacheIndex));
+}
+
+void ElasticFoundationForceImpl::realizeTopology(State& state) const {
+ energyCacheIndex = subsystem.allocateCacheEntry
+ (state, Stage::Dynamics, new Value<Real>());
+}
+
+
+} // namespace SimTK
+
diff --git a/Simbody/src/ElasticFoundationForceImpl.h b/Simbody/src/ElasticFoundationForceImpl.h
new file mode 100644
index 0000000..f12f599
--- /dev/null
+++ b/Simbody/src/ElasticFoundationForceImpl.h
@@ -0,0 +1,79 @@
+#ifndef SimTK_SIMBODY_HUNT_CROSSLEY_FORCE_IMPL_H_
+#define SimTK_SIMBODY_HUNT_CROSSLEY_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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/ElasticFoundationForce.h"
+#include "ForceImpl.h"
+
+namespace SimTK {
+
+class ElasticFoundationForceImpl : public ForceImpl {
+public:
+ class Parameters;
+ ElasticFoundationForceImpl(GeneralContactSubsystem& subystem,
+ ContactSetIndex set);
+ ElasticFoundationForceImpl* clone() const {
+ return new ElasticFoundationForceImpl(*this);
+ }
+ void setBodyParameters
+ (ContactSurfaceIndex bodyIndex, Real stiffness, Real dissipation,
+ Real staticFriction, Real dynamicFriction, Real viscousFriction);
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const;
+ Real calcPotentialEnergy(const State& state) const;
+ void realizeTopology(State& state) const;
+ void processContact(const State& state, ContactSurfaceIndex meshIndex,
+ ContactSurfaceIndex otherBodyIndex,
+ const Parameters& param,
+ const std::set<int>& insideFaces,
+ Real areaScale,
+ Vector_<SpatialVec>& bodyForces, Real& pe) const;
+private:
+ friend class ElasticFoundationForce;
+ const GeneralContactSubsystem& subsystem;
+ const ContactSetIndex set;
+ std::map<ContactSurfaceIndex, Parameters> parameters;
+ Real transitionVelocity;
+ mutable CacheEntryIndex energyCacheIndex;
+};
+
+class ElasticFoundationForceImpl::Parameters {
+public:
+ Parameters() : stiffness(1), dissipation(0), staticFriction(0), dynamicFriction(0), viscousFriction(0) {
+ }
+ Parameters(Real stiffness, Real dissipation, Real staticFriction, Real dynamicFriction, Real viscousFriction) :
+ stiffness(stiffness), dissipation(dissipation), staticFriction(staticFriction), dynamicFriction(dynamicFriction), viscousFriction(viscousFriction) {
+ }
+ Real stiffness, dissipation, staticFriction, dynamicFriction, viscousFriction;
+ Array_<Vec3> springPosition;
+ Array_<UnitVec3> springNormal;
+ Array_<Real> springArea;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_HUNT_CROSSLEY_FORCE_IMPL_H_
diff --git a/Simbody/src/Force.cpp b/Simbody/src/Force.cpp
new file mode 100644
index 0000000..ed9f761
--- /dev/null
+++ b/Simbody/src/Force.cpp
@@ -0,0 +1,1098 @@
+/* -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/MobilizedBody.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "simbody/internal/MultibodySystem.h"
+#include "simbody/internal/Force.h"
+#include "simbody/internal/Force_BuiltIns.h"
+
+#include "ForceImpl.h"
+
+namespace SimTK {
+
+//------------------------------------------------------------------------------
+// FORCE
+//------------------------------------------------------------------------------
+
+void Force::setDisabledByDefault(bool shouldBeDisabled)
+{ updImpl().setDisabledByDefault(shouldBeDisabled); }
+bool Force::isDisabledByDefault() const
+{ return getImpl().isDisabledByDefault(); }
+
+void Force::disable(State& state) const
+{ getForceSubsystem().setForceIsDisabled(state, getForceIndex(), true); }
+void Force::enable(State& state) const
+{ getForceSubsystem().setForceIsDisabled(state, getForceIndex(), false); }
+bool Force::isDisabled(const State& state) const
+{ return getForceSubsystem().isForceDisabled(state, getForceIndex()); }
+
+void Force::calcForceContribution(const State& state,
+ Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces,
+ Vector& mobilityForces) const
+{
+ const MultibodySystem& mbs = getForceSubsystem().getMultibodySystem();
+ const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
+
+ // Resize if necessary.
+ bodyForces.resize(matter.getNumBodies());
+ particleForces.resize(matter.getNumParticles());
+ mobilityForces.resize(matter.getNumMobilities());
+
+ // Set all forces to zero.
+ bodyForces.setToZero();
+ particleForces.setToZero();
+ mobilityForces.setToZero();
+ if (isDisabled(state)) return;
+
+ // Add in force element contributions.
+ getImpl().calcForce(state,bodyForces,particleForces,mobilityForces);
+}
+
+Real Force::calcPotentialEnergyContribution(const State& state) const {
+ if (isDisabled(state)) return 0;
+ return getImpl().calcPotentialEnergy(state);
+}
+
+const GeneralForceSubsystem& Force::getForceSubsystem() const
+{ return getImpl().getForceSubsystem(); }
+ForceIndex Force::getForceIndex() const
+{ return getImpl().getForceIndex(); }
+
+//-------------------------- TwoPointLinearSpring ------------------------------
+//------------------------------------------------------------------------------
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Force::TwoPointLinearSpring, Force::TwoPointLinearSpringImpl, Force);
+
+Force::TwoPointLinearSpring::TwoPointLinearSpring(GeneralForceSubsystem& forces, const MobilizedBody& body1, const Vec3& station1,
+ const MobilizedBody& body2, const Vec3& station2, Real k, Real x0) : Force(new TwoPointLinearSpringImpl(
+ body1, station1, body2, station2, k, x0)) {
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+Force::TwoPointLinearSpringImpl::TwoPointLinearSpringImpl(const MobilizedBody& body1, const Vec3& station1,
+ const MobilizedBody& body2, const Vec3& station2, Real k, Real x0) : matter(body1.getMatterSubsystem()),
+ body1(body1.getMobilizedBodyIndex()), station1(station1),
+ body2(body2.getMobilizedBodyIndex()), station2(station2), k(k), x0(x0) {
+}
+
+void Force::TwoPointLinearSpringImpl::calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const {
+ const Transform& X_GB1 = matter.getMobilizedBody(body1).getBodyTransform(state);
+ const Transform& X_GB2 = matter.getMobilizedBody(body2).getBodyTransform(state);
+
+ const Vec3 s1_G = X_GB1.R() * station1;
+ const Vec3 s2_G = X_GB2.R() * station2;
+
+ const Vec3 p1_G = X_GB1.p() + s1_G; // station measured from ground origin
+ const Vec3 p2_G = X_GB2.p() + s2_G;
+
+ const Vec3 r_G = p2_G - p1_G; // vector from point1 to point2
+ const Real d = r_G.norm(); // distance between the points
+ const Real stretch = d - x0; // + -> tension, - -> compression
+ const Real frcScalar = k*stretch; // k(x-x0)
+
+ const Vec3 f1_G = (frcScalar/d) * r_G;
+ bodyForces[body1] += SpatialVec(s1_G % f1_G, f1_G);
+ bodyForces[body2] -= SpatialVec(s2_G % f1_G, f1_G);
+}
+
+Real Force::TwoPointLinearSpringImpl::calcPotentialEnergy(const State& state) const {
+ const Transform& X_GB1 = matter.getMobilizedBody(body1).getBodyTransform(state);
+ const Transform& X_GB2 = matter.getMobilizedBody(body2).getBodyTransform(state);
+
+ const Vec3 s1_G = X_GB1.R() * station1;
+ const Vec3 s2_G = X_GB2.R() * station2;
+
+ const Vec3 p1_G = X_GB1.p() + s1_G; // station measured from ground origin
+ const Vec3 p2_G = X_GB2.p() + s2_G;
+
+ const Vec3 r_G = p2_G - p1_G; // vector from point1 to point2
+ const Real d = r_G.norm(); // distance between the points
+ const Real stretch = d - x0; // + -> tension, - -> compression
+
+ return k*stretch*stretch/2; // 1/2 k (x-x0)^2
+}
+
+
+//-------------------------- TwoPointLinearDamper ------------------------------
+//------------------------------------------------------------------------------
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Force::TwoPointLinearDamper, Force::TwoPointLinearDamperImpl, Force);
+
+Force::TwoPointLinearDamper::TwoPointLinearDamper(GeneralForceSubsystem& forces, const MobilizedBody& body1, const Vec3& station1,
+ const MobilizedBody& body2, const Vec3& station2, Real damping) : Force(new TwoPointLinearDamperImpl(
+ body1, station1, body2, station2, damping)) {
+ assert(damping >= 0);
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+Force::TwoPointLinearDamperImpl::TwoPointLinearDamperImpl(const MobilizedBody& body1, const Vec3& station1,
+ const MobilizedBody& body2, const Vec3& station2, Real damping) : matter(body1.getMatterSubsystem()),
+ body1(body1.getMobilizedBodyIndex()), station1(station1),
+ body2(body2.getMobilizedBodyIndex()), station2(station2), damping(damping) {
+}
+
+void Force::TwoPointLinearDamperImpl::calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const {
+ const Transform& X_GB1 = matter.getMobilizedBody(body1).getBodyTransform(state);
+ const Transform& X_GB2 = matter.getMobilizedBody(body2).getBodyTransform(state);
+
+ const Vec3 s1_G = X_GB1.R() * station1;
+ const Vec3 s2_G = X_GB2.R() * station2;
+
+ const Vec3 p1_G = X_GB1.p() + s1_G; // station measured from ground origin
+ const Vec3 p2_G = X_GB2.p() + s2_G;
+
+ const Vec3 v1_G = matter.getMobilizedBody(body1).findStationVelocityInGround(state, station1);
+ const Vec3 v2_G = matter.getMobilizedBody(body2).findStationVelocityInGround(state, station2);
+ const Vec3 vRel = v2_G - v1_G; // relative velocity
+
+ const UnitVec3 d(p2_G - p1_G); // direction from point1 to point2
+ const Real frc = damping*dot(vRel, d); // c*v
+
+ const Vec3 f1_G = frc*d;
+ bodyForces[body1] += SpatialVec(s1_G % f1_G, f1_G);
+ bodyForces[body2] -= SpatialVec(s2_G % f1_G, f1_G);
+}
+
+Real Force::TwoPointLinearDamperImpl::calcPotentialEnergy(const State& state) const {
+ return 0;
+}
+
+
+//-------------------------- TwoPointConstantForce -----------------------------
+//------------------------------------------------------------------------------
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Force::TwoPointConstantForce, Force::TwoPointConstantForceImpl, Force);
+
+Force::TwoPointConstantForce::TwoPointConstantForce(GeneralForceSubsystem& forces, const MobilizedBody& body1, const Vec3& station1,
+ const MobilizedBody& body2, const Vec3& station2, Real force) : Force(new TwoPointConstantForceImpl(
+ body1, station1, body2, station2, force)) {
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+Force::TwoPointConstantForceImpl::TwoPointConstantForceImpl(const MobilizedBody& body1, const Vec3& station1,
+ const MobilizedBody& body2, const Vec3& station2, Real force) : matter(body1.getMatterSubsystem()),
+ body1(body1.getMobilizedBodyIndex()), station1(station1),
+ body2(body2.getMobilizedBodyIndex()), station2(station2), force(force) {
+}
+
+void Force::TwoPointConstantForceImpl::calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const {
+ const Transform& X_GB1 = matter.getMobilizedBody(body1).getBodyTransform(state);
+ const Transform& X_GB2 = matter.getMobilizedBody(body2).getBodyTransform(state);
+
+ const Vec3 s1_G = X_GB1.R() * station1;
+ const Vec3 s2_G = X_GB2.R() * station2;
+
+ const Vec3 p1_G = X_GB1.p() + s1_G; // station measured from ground origin
+ const Vec3 p2_G = X_GB2.p() + s2_G;
+
+ const Vec3 r_G = p2_G - p1_G; // vector from point1 to point2
+ const Real x = r_G.norm(); // distance between the points
+ const UnitVec3 d(r_G/x, true);
+
+ const Vec3 f2_G = force * d;
+ bodyForces[body1] -= SpatialVec(s1_G % f2_G, f2_G);
+ bodyForces[body2] += SpatialVec(s2_G % f2_G, f2_G);
+}
+
+Real Force::TwoPointConstantForceImpl::calcPotentialEnergy(const State& state) const {
+ return 0;
+}
+
+
+//--------------------------- MobilityLinearSpring -----------------------------
+//------------------------------------------------------------------------------
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Force::MobilityLinearSpring,
+ Force::MobilityLinearSpringImpl,
+ Force);
+
+Force::MobilityLinearSpring::
+MobilityLinearSpring(GeneralForceSubsystem& forces,
+ const MobilizedBody& mobod,
+ MobilizerQIndex whichQ,
+ Real defaultStiffness,
+ Real defaultQZero)
+: Force(new MobilityLinearSpringImpl(mobod, whichQ,
+ defaultStiffness, defaultQZero))
+{
+ SimTK_ERRCHK1_ALWAYS(defaultStiffness >= 0,
+ "Force::MobilityLinearSpring::MobilityLinearSpring()",
+ "Stiffness coefficient must be nonnegative "
+ "(defaultStiffness=%g).", defaultStiffness);
+
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+Force::MobilityLinearSpring& Force::MobilityLinearSpring::
+setDefaultStiffness(Real defaultStiffness) {
+ SimTK_ERRCHK1_ALWAYS(defaultStiffness >= 0,
+ "Force::MobilityLinearSpring::setDefaultStiffness()",
+ "Stiffness coefficient must be nonnegative "
+ "(defaultStiffness=%g).", defaultStiffness);
+
+ MobilityLinearSpringImpl& impl = updImpl();
+ if (impl.m_defaultStiffness != defaultStiffness) {
+ impl.m_defaultStiffness = defaultStiffness;
+ impl.invalidateTopologyCache();
+ }
+ return *this;
+}
+
+
+Force::MobilityLinearSpring& Force::MobilityLinearSpring::
+setDefaultQZero(Real defaultQZero) {
+ MobilityLinearSpringImpl& impl = updImpl();
+ if (impl.m_defaultQZero != defaultQZero) {
+ impl.m_defaultQZero = defaultQZero;
+ impl.invalidateTopologyCache();
+ }
+ return *this;
+}
+
+Real Force::MobilityLinearSpring::
+getDefaultStiffness() const
+{ return getImpl().m_defaultStiffness; }
+
+Real Force::MobilityLinearSpring::
+getDefaultQZero() const
+{ return getImpl().m_defaultQZero; }
+
+const Force::MobilityLinearSpring& Force::MobilityLinearSpring::
+setStiffness(State& state, Real stiffness) const {
+ SimTK_ERRCHK1_ALWAYS(stiffness >= 0,
+ "Force::MobilityLinearSpring::setStiffness()",
+ "Stiffness coefficient must be nonnegative "
+ "(stiffness=%g).", stiffness);
+
+ getImpl().updParams(state).first = stiffness;
+ return *this;
+}
+
+const Force::MobilityLinearSpring& Force::MobilityLinearSpring::
+setQZero(State& state, Real qZero) const
+{ getImpl().updParams(state).second = qZero; return *this; }
+
+Real Force::MobilityLinearSpring::
+getStiffness(const State& state) const
+{ return getImpl().getParams(state).first; }
+
+Real Force::MobilityLinearSpring::
+getQZero(const State& state) const
+{ return getImpl().getParams(state).second; }
+
+Force::MobilityLinearSpringImpl::
+MobilityLinearSpringImpl(const MobilizedBody& mobod,
+ MobilizerQIndex whichQ,
+ Real defaultStiffness,
+ Real defaultQZero)
+: m_matter(mobod.getMatterSubsystem()),
+ m_mobodIx(mobod.getMobilizedBodyIndex()), m_whichQ(whichQ),
+ m_defaultStiffness(defaultStiffness), m_defaultQZero(defaultQZero)
+{
+}
+
+void Force::MobilityLinearSpringImpl::
+calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+{
+ const MobilizedBody& mb = m_matter.getMobilizedBody(m_mobodIx);
+ const Real q = mb.getOneQ(state, m_whichQ);
+ const std::pair<Real,Real>& params = getParams(state);
+ const Real k = params.first, q0 = params.second;
+ const Real frc = -k*(q-q0);
+ // bug: this is depending on qdot=u
+ mb.applyOneMobilityForce(state, MobilizerUIndex((int)m_whichQ),
+ frc, mobilityForces);
+}
+
+Real Force::MobilityLinearSpringImpl::
+calcPotentialEnergy(const State& state) const {
+ const MobilizedBody& mb = m_matter.getMobilizedBody(m_mobodIx);
+ const Real q = mb.getOneQ(state, m_whichQ);
+ const std::pair<Real,Real>& params = getParams(state);
+ const Real k = params.first, q0 = params.second;
+ const Real frc = -k*(q-q0);
+ return k*square(q-q0)/2;
+}
+
+
+
+//--------------------------- MobilityLinearDamper -----------------------------
+//------------------------------------------------------------------------------
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Force::MobilityLinearDamper,
+ Force::MobilityLinearDamperImpl,
+ Force);
+
+Force::MobilityLinearDamper::
+MobilityLinearDamper(GeneralForceSubsystem& forces,
+ const MobilizedBody& mobod,
+ MobilizerUIndex whichU,
+ Real defaultDamping)
+: Force(new MobilityLinearDamperImpl(mobod, whichU, defaultDamping))
+{
+ SimTK_ERRCHK1_ALWAYS(defaultDamping >= 0,
+ "Force::MobilityLinearDamper::MobilityLinearDamper()",
+ "Damping coefficient must be nonnegative "
+ "(defaultDamping=%g).", defaultDamping);
+
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+
+Force::MobilityLinearDamper& Force::MobilityLinearDamper::
+setDefaultDamping(Real defaultDamping) {
+ SimTK_ERRCHK1_ALWAYS(defaultDamping >= 0,
+ "Force::MobilityLinearDamper::setDefaultDamping()",
+ "Damping coefficient must be nonnegative "
+ "(defaultDamping=%g).", defaultDamping);
+
+ MobilityLinearDamperImpl& impl = updImpl();
+ if (impl.m_defaultDamping != defaultDamping) {
+ impl.m_defaultDamping = defaultDamping;
+ impl.invalidateTopologyCache();
+ }
+ return *this;
+}
+
+Real Force::MobilityLinearDamper::
+getDefaultDamping() const
+{ return getImpl().m_defaultDamping; }
+
+const Force::MobilityLinearDamper& Force::MobilityLinearDamper::
+setDamping(State& state, Real damping) const
+{
+ SimTK_ERRCHK1_ALWAYS(damping >= 0,
+ "Force::MobilityLinearDamper::setDamping()",
+ "Damping coefficient must be nonnegative "
+ "(damping=%g).", damping);
+
+ getImpl().updDamping(state) = damping;
+ return *this;
+}
+
+Real Force::MobilityLinearDamper::
+getDamping(const State& state) const
+{ return getImpl().getDamping(state); }
+
+
+Force::MobilityLinearDamperImpl::
+MobilityLinearDamperImpl(const MobilizedBody& mobod,
+ MobilizerUIndex whichU,
+ Real defaultDamping)
+: m_matter(mobod.getMatterSubsystem()),
+ m_mobodIx(mobod.getMobilizedBodyIndex()), m_whichU(whichU),
+ m_defaultDamping(defaultDamping)
+{
+}
+
+void Force::MobilityLinearDamperImpl::
+calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+{
+ const MobilizedBody& mb = m_matter.getMobilizedBody(m_mobodIx);
+ const Real u = mb.getOneU(state, m_whichU);
+ const Real damping = getDamping(state);
+ const Real frc = -damping*u;
+ mb.applyOneMobilityForce(state, m_whichU, frc, mobilityForces);
+}
+
+Real Force::MobilityLinearDamperImpl::
+calcPotentialEnergy(const State& state) const {
+ return 0;
+}
+
+
+
+//-------------------------- MobilityConstantForce -----------------------------
+//------------------------------------------------------------------------------
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Force::MobilityConstantForce,
+ Force::MobilityConstantForceImpl,
+ Force);
+
+Force::MobilityConstantForce::MobilityConstantForce
+ (GeneralForceSubsystem& forces,
+ const MobilizedBody& mobod,
+ MobilizerUIndex whichU,
+ Real defaultForce)
+: Force(new MobilityConstantForceImpl(mobod, whichU, defaultForce)) {
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+Force::MobilityConstantForce& Force::MobilityConstantForce::
+setDefaultForce(Real defaultForce) {
+ MobilityConstantForceImpl& impl = updImpl();
+ if (impl.m_defaultForce != defaultForce) {
+ impl.m_defaultForce = defaultForce;
+ impl.invalidateTopologyCache();
+ }
+ return *this;
+}
+
+Real Force::MobilityConstantForce::
+getDefaultForce() const
+{ return getImpl().m_defaultForce; }
+
+void Force::MobilityConstantForce::
+setForce(State& state, Real force) const {
+ getImpl().updForce(state) = force;
+}
+
+Real Force::MobilityConstantForce::
+getForce(const State& state) const
+{ return getImpl().getForce(state); }
+
+Force::MobilityConstantForceImpl::MobilityConstantForceImpl
+ (const MobilizedBody& mobod,
+ MobilizerUIndex whichU,
+ Real defaultForce)
+: m_matter(mobod.getMatterSubsystem()),
+ m_mobodIx(mobod.getMobilizedBodyIndex()), m_whichU(whichU),
+ m_defaultForce(defaultForce)
+{
+}
+
+void Force::MobilityConstantForceImpl::
+calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+{
+ const MobilizedBody& mb = m_matter.getMobilizedBody(m_mobodIx);
+ mb.applyOneMobilityForce(state, m_whichU, getForce(state), mobilityForces);
+}
+
+
+//---------------------------- MobilityLinearStop ------------------------------
+//------------------------------------------------------------------------------
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Force::MobilityLinearStop,
+ Force::MobilityLinearStopImpl,
+ Force);
+
+Force::MobilityLinearStop::MobilityLinearStop
+ (GeneralForceSubsystem& forces,
+ const MobilizedBody& mobod,
+ MobilizerQIndex whichQ,
+ Real defaultStiffness,
+ Real defaultDissipation,
+ Real defaultQLow,
+ Real defaultQHigh)
+: Force(new MobilityLinearStopImpl(mobod,whichQ,
+ defaultStiffness, defaultDissipation,
+ defaultQLow, defaultQHigh))
+{
+ SimTK_ERRCHK2_ALWAYS(defaultStiffness >= 0 && defaultDissipation >= 0,
+ "Force::MobilityLinearStop::MobilityLinearStop()",
+ "Stiffness and dissipation coefficient must be nonnegative "
+ "(defaultStiffness=%g, defaultDissipation=%g).",
+ defaultStiffness, defaultDissipation);
+ SimTK_ERRCHK2_ALWAYS(defaultQLow <= defaultQHigh,
+ "Force::MobilityLinearStop::MobilityLinearStop()",
+ "Lower bound can't be larger than upper bound "
+ "(defaultQLow=%g, defaultQHigh=%g).",
+ defaultQLow, defaultQHigh);
+
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+Force::MobilityLinearStop& Force::MobilityLinearStop::
+setDefaultBounds(Real defaultQLow, Real defaultQHigh) {
+ SimTK_ERRCHK2_ALWAYS(defaultQLow <= defaultQHigh,
+ "Force::MobilityLinearStop::setDefaultBounds()",
+ "Lower bound can't be larger than upper bound "
+ "(defaultQLow=%g, defaultQHigh=%g).",
+ defaultQLow, defaultQHigh);
+
+ MobilityLinearStopImpl& impl = updImpl();
+ if (impl.m_defQLow != defaultQLow || impl.m_defQHigh != defaultQHigh) {
+ impl.m_defQLow = defaultQLow;
+ impl.m_defQHigh = defaultQHigh;
+ impl.invalidateTopologyCache();
+ }
+ return *this;
+}
+
+Force::MobilityLinearStop& Force::MobilityLinearStop::
+setDefaultMaterialProperties(Real defaultStiffness, Real defaultDissipation) {
+ SimTK_ERRCHK2_ALWAYS(defaultStiffness >= 0 && defaultDissipation >= 0,
+ "Force::MobilityLinearStop::setDefaultMaterialProperties()",
+ "Stiffness and dissipation coefficient must be nonnegative "
+ "(defaultStiffness=%g, defaultDissipation=%g).",
+ defaultStiffness, defaultDissipation);
+
+ MobilityLinearStopImpl& impl = updImpl();
+ if ( impl.m_defStiffness != defaultStiffness
+ || impl.m_defDissipation != defaultDissipation) {
+ impl.m_defStiffness = defaultStiffness;
+ impl.m_defDissipation = defaultDissipation;
+ impl.invalidateTopologyCache();
+ }
+ return *this;
+}
+
+Real Force::MobilityLinearStop::getDefaultLowerBound() const
+{ return getImpl().m_defQLow; }
+Real Force::MobilityLinearStop::getDefaultUpperBound() const
+{ return getImpl().m_defQHigh; }
+Real Force::MobilityLinearStop::getDefaultStiffness() const
+{ return getImpl().m_defStiffness; }
+Real Force::MobilityLinearStop::getDefaultDissipation() const
+{ return getImpl().m_defDissipation; }
+
+
+void Force::MobilityLinearStop::
+setBounds(State& state, Real qLow, Real qHigh) const {
+ SimTK_ERRCHK2_ALWAYS(qLow <= qHigh,
+ "Force::MobilityLinearStop::setBounds()",
+ "Lower bound can't be larger than upper bound (qLow=%g, qHigh=%g).",
+ qLow, qHigh);
+
+ MobilityLinearStopImpl::Parameters& params =
+ getImpl().updParameters(state); // invalidates Dynamics stage
+ params.qLow = qLow; params.qHigh = qHigh;
+}
+void Force::MobilityLinearStop::
+setMaterialProperties(State& state, Real stiffness, Real dissipation) const {
+ SimTK_ERRCHK2_ALWAYS(stiffness >= 0 && dissipation >= 0,
+ "Force::MobilityLinearStop::setMaterialProperties()",
+ "Stiffness and dissipation coefficient must be nonnegative "
+ "(stiffness=%g, dissipation=%g).",
+ stiffness, dissipation);
+
+ MobilityLinearStopImpl::Parameters& params =
+ getImpl().updParameters(state); // invalidates Dynamics stage
+ params.k = stiffness; params.d = dissipation;
+}
+
+Real Force::MobilityLinearStop::getLowerBound(const State& state) const
+{ return getImpl().getParameters(state).qLow; }
+Real Force::MobilityLinearStop::getUpperBound(const State& state) const
+{ return getImpl().getParameters(state).qHigh; }
+Real Force::MobilityLinearStop::getStiffness(const State& state) const
+{ return getImpl().getParameters(state).k; }
+Real Force::MobilityLinearStop::getDissipation(const State& state) const
+{ return getImpl().getParameters(state).d; }
+
+
+Force::MobilityLinearStopImpl::MobilityLinearStopImpl
+ (const MobilizedBody& mobod,
+ MobilizerQIndex whichQ,
+ Real defaultStiffness,
+ Real defaultDissipation,
+ Real defaultQLow,
+ Real defaultQHigh)
+: m_matter(mobod.getMatterSubsystem()),
+ m_mobodIx(mobod.getMobilizedBodyIndex()), m_whichQ(whichQ),
+ m_defStiffness(defaultStiffness), m_defDissipation(defaultDissipation),
+ m_defQLow(defaultQLow), m_defQHigh(defaultQHigh)
+{
+}
+
+
+void Force::MobilityLinearStopImpl::
+calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+{
+ const Parameters& param = getParameters(state);
+ if (param.k == 0) return; // no stiffness, no force
+
+ const MobilizedBody& mb = m_matter.getMobilizedBody(m_mobodIx);
+ const Real q = mb.getOneQ(state, m_whichQ);
+
+ // Don't ask for velocity-dependent qdot if there is no dissipation.
+ const Real qdot = param.d != 0 ? mb.getOneQDot(state, m_whichQ)
+ : Real(0);
+
+ if (q > param.qHigh) {
+ const Real x = q-param.qHigh; // x > 0
+ const Real fraw = param.k*x*(1+param.d*qdot); // should be >= 0
+ mb.applyOneMobilityForce(state,
+ MobilizerUIndex(m_whichQ), // TODO: only works qdot & u match
+ std::min(Real(0), -fraw), mobilityForces);
+ } else if (q < param.qLow) {
+ const Real x = q-param.qLow; // x < 0
+ const Real fraw = param.k*x*(1-param.d*qdot); // should be <= 0
+ mb.applyOneMobilityForce(state,
+ MobilizerUIndex(m_whichQ), // TODO: only works qdot & u match
+ std::max(Real(0), -fraw), mobilityForces);
+ }
+}
+
+Real Force::MobilityLinearStopImpl::
+calcPotentialEnergy(const State& state) const {
+ const Parameters& param = getParameters(state);
+ if (param.k == 0) return 0; // no stiffness, no energy stored
+
+ const MobilizedBody& mb = m_matter.getMobilizedBody(m_mobodIx);
+ const Real q = mb.getOneQ(state, m_whichQ);
+ Real x = 0;
+ if (q > param.qHigh) x = q-param.qHigh;
+ else if (q < param.qLow) x = q-param.qLow;
+ else return 0; // neither stop is engaged
+
+ return param.k*x*x/2; // 1/2 k x^2
+}
+
+//-------------------------- MobilityDiscreteForce -----------------------------
+//------------------------------------------------------------------------------
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Force::MobilityDiscreteForce,
+ Force::MobilityDiscreteForceImpl,
+ Force);
+
+Force::MobilityDiscreteForce::MobilityDiscreteForce
+ (GeneralForceSubsystem& forces, const MobilizedBody& mobod,
+ MobilizerUIndex whichU, Real defaultForce)
+: Force(new MobilityDiscreteForceImpl(mobod, whichU, defaultForce)) {
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+Force::MobilityDiscreteForce& Force::MobilityDiscreteForce::
+setDefaultMobilityForce(Real defaultForce) {
+ updImpl().m_defaultVal = defaultForce;
+ getImpl().invalidateTopologyCache();
+ return *this;
+}
+
+Real Force::MobilityDiscreteForce::
+getDefaultMobilityForce() const {
+ return getImpl().m_defaultVal;
+}
+
+void Force::MobilityDiscreteForce::
+setMobilityForce(State& state, Real f) const {
+ getImpl().setMobilityForce(state, f);
+}
+
+Real Force::MobilityDiscreteForce::
+getMobilityForce(const State& state) const {
+ return getImpl().getMobilityForce(state);
+}
+
+Force::MobilityDiscreteForceImpl::MobilityDiscreteForceImpl
+ (const MobilizedBody& mobod, MobilizerUIndex whichU, Real defaultForce)
+: m_matter(mobod.getMatterSubsystem()),
+ m_mobodIx(mobod.getMobilizedBodyIndex()),
+ m_whichU(whichU), m_defaultVal(defaultForce) {
+}
+
+void Force::MobilityDiscreteForceImpl::
+setMobilityForce(State& state, Real f) const {
+ const GeneralForceSubsystem& forces = getForceSubsystem();
+ Real& fInState = Value<Real>::updDowncast
+ (forces.updDiscreteVariable(state, m_forceIx));
+ fInState = f;
+}
+
+// Get the value of the generalized force to be applied.
+Real Force::MobilityDiscreteForceImpl::
+getMobilityForce(const State& state) const {
+ const GeneralForceSubsystem& forces = getForceSubsystem();
+ const Real& fInState = Value<Real>::downcast
+ (forces.getDiscreteVariable(state, m_forceIx));
+ return fInState;
+}
+
+void Force::MobilityDiscreteForceImpl::
+calcForce( const State& state,
+ Vector_<SpatialVec>& /*bodyForces*/,
+ Vector_<Vec3>& /*particleForces*/,
+ Vector& mobilityForces) const
+{
+ const GeneralForceSubsystem& forces = getForceSubsystem();
+ const Real f = Value<Real>::downcast
+ (forces.getDiscreteVariable(state, m_forceIx));
+ const MobilizedBody& mobod = m_matter.getMobilizedBody(m_mobodIx);
+ mobod.applyOneMobilityForce(state, m_whichU, f, mobilityForces);
+}
+
+void Force::MobilityDiscreteForceImpl::
+realizeTopology(State& state) const {
+ const GeneralForceSubsystem& forces = getForceSubsystem();
+ m_forceIx = forces.allocateDiscreteVariable
+ (state, Stage::Dynamics, new Value<Real>(m_defaultVal));
+}
+
+
+
+
+//------------------------------ DiscreteForces --------------------------------
+//------------------------------------------------------------------------------
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Force::DiscreteForces,
+ Force::DiscreteForcesImpl,
+ Force);
+
+Force::DiscreteForces::DiscreteForces
+ (GeneralForceSubsystem& forces, const SimbodyMatterSubsystem& matter)
+: Force(new DiscreteForcesImpl(matter)) {
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+void Force::DiscreteForces::
+clearAllMobilityForces(State& state) const {
+ getImpl().updAllMobilityForces(state).clear(); // set size to zero
+}
+
+
+void Force::DiscreteForces::
+clearAllBodyForces(State& state) const {
+ getImpl().updAllBodyForces(state).clear(); // set size to zero
+}
+
+void Force::DiscreteForces::
+setOneMobilityForce(State& state, const MobilizedBody& mobod,
+ MobilizerUIndex whichU, Real f) const {
+ Vector& mobForces = getImpl().updAllMobilityForces(state);
+ if (mobForces.size() == 0) {
+ mobForces.resize(state.getNU());
+ mobForces.setToZero();
+ }
+ // Don't use "apply" here because that would add in the force.
+ mobod.updOneFromUPartition(state, whichU, mobForces) = f;
+}
+
+Real Force::DiscreteForces::
+getOneMobilityForce(const State& state, const MobilizedBody& mobod,
+ MobilizerUIndex whichU) const {
+ const Vector& mobForces = getImpl().getAllMobilityForces(state);
+ if (mobForces.size() == 0) {return 0;}
+
+ return mobod.getOneFromUPartition(state, whichU, mobForces);
+}
+
+
+void Force::DiscreteForces::
+setAllMobilityForces(State& state, const Vector& f) const {
+ if (f.size()==0) {clearAllMobilityForces(state); return;}
+ SimTK_ERRCHK2_ALWAYS(f.size() == state.getNU(),
+ "Force::DiscreteForces::setAllMobilityForces()",
+ "Mobility force vector f had wrong size %d; should have been %d.",
+ f.size(), state.getNU());
+
+ getImpl().updAllMobilityForces(state) = f;
+}
+
+const Vector& Force::DiscreteForces::
+getAllMobilityForces(const State& state) const
+{ return getImpl().getAllMobilityForces(state); }
+
+void Force::DiscreteForces::
+setOneBodyForce(State& state, const MobilizedBody& mobod,
+ const SpatialVec& spatialForceInG) const {
+ Vector_<SpatialVec>& bodyForces = getImpl().updAllBodyForces(state);
+ if (bodyForces.size() == 0) {
+ bodyForces.resize(getImpl().m_matter.getNumBodies());
+ bodyForces.setToZero();
+ }
+ bodyForces[mobod.getMobilizedBodyIndex()] = spatialForceInG;
+}
+
+
+SpatialVec Force::DiscreteForces::
+getOneBodyForce(const State& state, const MobilizedBody& mobod) const {
+ const Vector_<SpatialVec>& bodyForces = getImpl().getAllBodyForces(state);
+ if (bodyForces.size() == 0) {return SpatialVec(Vec3(0),Vec3(0));}
+
+ return bodyForces[mobod.getMobilizedBodyIndex()];
+}
+
+const Vector_<SpatialVec>& Force::DiscreteForces::
+getAllBodyForces(const State& state) const
+{ return getImpl().getAllBodyForces(state); }
+
+void Force::DiscreteForces::
+setAllBodyForces(State& state, const Vector_<SpatialVec>& bodyForcesInG) const {
+ if (bodyForcesInG.size()==0) {clearAllBodyForces(state); return;}
+ const int numBodies = getImpl().m_matter.getNumBodies();
+ SimTK_ERRCHK2_ALWAYS(bodyForcesInG.size() == numBodies,
+ "Force::DiscreteForces::setAllBodyForces()",
+ "Body force vector bodyForcesInG had wrong size %d; "
+ "should have been %d (0th entry is for Ground).",
+ bodyForcesInG.size(), numBodies);
+
+ getImpl().updAllBodyForces(state) = bodyForcesInG;
+}
+
+
+void Force::DiscreteForces::
+addForceToBodyPoint(State& state, const MobilizedBody& mobod,
+ const Vec3& pointInB, const Vec3& forceInG) const {
+ Vector_<SpatialVec>& bodyForces = getImpl().updAllBodyForces(state);
+ if (bodyForces.size() == 0) {
+ bodyForces.resize(getImpl().m_matter.getNumBodies());
+ bodyForces.setToZero();
+ }
+ mobod.applyForceToBodyPoint(state, pointInB, forceInG, bodyForces);
+}
+
+Force::DiscreteForcesImpl::DiscreteForcesImpl
+ (const SimbodyMatterSubsystem& matter) : m_matter(matter) {}
+
+const Vector& Force::DiscreteForcesImpl::
+getAllMobilityForces(const State& state) const {
+ const GeneralForceSubsystem& forces = getForceSubsystem();
+ const Vector& fInState = Value<Vector>::downcast
+ (forces.getDiscreteVariable(state, m_mobForcesIx));
+ return fInState;
+}
+
+Vector& Force::DiscreteForcesImpl::
+updAllMobilityForces(State& state) const {
+ const GeneralForceSubsystem& forces = getForceSubsystem();
+ Vector& fInState = Value<Vector>::updDowncast
+ (forces.updDiscreteVariable(state, m_mobForcesIx));
+ return fInState;
+}
+
+const Vector_<SpatialVec>& Force::DiscreteForcesImpl::
+getAllBodyForces(const State& state) const {
+ const GeneralForceSubsystem& forces = getForceSubsystem();
+ const Vector_<SpatialVec>& FInState = Value< Vector_<SpatialVec> >::downcast
+ (forces.getDiscreteVariable(state, m_bodyForcesIx));
+ return FInState;
+}
+Vector_<SpatialVec>& Force::DiscreteForcesImpl::
+updAllBodyForces(State& state) const {
+ const GeneralForceSubsystem& forces = getForceSubsystem();
+ Vector_<SpatialVec>& FInState = Value< Vector_<SpatialVec> >::updDowncast
+ (forces.updDiscreteVariable(state, m_bodyForcesIx));
+ return FInState;
+}
+
+void Force::DiscreteForcesImpl::
+calcForce( const State& state,
+ Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& /*particleForces*/,
+ Vector& mobilityForces) const
+{
+ const GeneralForceSubsystem& forces = getForceSubsystem();
+ const Vector& f = Value<Vector>::downcast
+ (forces.getDiscreteVariable(state, m_mobForcesIx));
+ const Vector_<SpatialVec>& F = Value< Vector_<SpatialVec> >::downcast
+ (forces.getDiscreteVariable(state, m_bodyForcesIx));
+ if (f.size()) mobilityForces += f;
+ if (F.size()) bodyForces += F;
+}
+
+void Force::DiscreteForcesImpl::
+realizeTopology(State& state) const {
+ const GeneralForceSubsystem& forces = getForceSubsystem();
+ m_mobForcesIx = forces.allocateDiscreteVariable
+ (state, Stage::Dynamics, new Value<Vector>());
+ m_bodyForcesIx = forces.allocateDiscreteVariable
+ (state, Stage::Dynamics, new Value< Vector_<SpatialVec> >());
+}
+
+
+
+//------------------------------ ConstantForce ---------------------------------
+//------------------------------------------------------------------------------
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Force::ConstantForce, Force::ConstantForceImpl, Force);
+
+Force::ConstantForce::ConstantForce(GeneralForceSubsystem& forces, const MobilizedBody& body, const Vec3& station, const Vec3& force) :
+ Force(new ConstantForceImpl(body, station, force)) {
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+Force::ConstantForceImpl::ConstantForceImpl(const MobilizedBody& body, const Vec3& station, const Vec3& force) :
+ matter(body.getMatterSubsystem()), body(body.getMobilizedBodyIndex()), station(station), force(force) {
+}
+
+void Force::ConstantForceImpl::calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const {
+ const Transform& X_GB = matter.getMobilizedBody(body).getBodyTransform(state);
+ const Vec3 station_G = X_GB.R() * station;
+ bodyForces[body] += SpatialVec(station_G % force, force);
+}
+
+Real Force::ConstantForceImpl::calcPotentialEnergy(const State& state) const {
+ return 0;
+}
+
+
+//------------------------------ ConstantTorque --------------------------------
+//------------------------------------------------------------------------------
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Force::ConstantTorque, Force::ConstantTorqueImpl, Force);
+
+Force::ConstantTorque::ConstantTorque(GeneralForceSubsystem& forces, const MobilizedBody& body, const Vec3& torque) :
+ Force(new ConstantTorqueImpl(body, torque)) {
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+Force::ConstantTorqueImpl::ConstantTorqueImpl(const MobilizedBody& body, const Vec3& torque) :
+ matter(body.getMatterSubsystem()), body(body.getMobilizedBodyIndex()), torque(torque) {
+}
+
+void Force::ConstantTorqueImpl::calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const {
+ bodyForces[body][0] += torque;
+}
+
+Real Force::ConstantTorqueImpl::calcPotentialEnergy(const State& state) const {
+ return 0;
+}
+
+
+//------------------------------- GlobalDamper ---------------------------------
+//------------------------------------------------------------------------------
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Force::GlobalDamper, Force::GlobalDamperImpl, Force);
+
+Force::GlobalDamper::GlobalDamper(GeneralForceSubsystem& forces, const SimbodyMatterSubsystem& matter,
+ Real damping) : Force(new GlobalDamperImpl(matter, damping)) {
+ assert(damping >= 0);
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+Force::GlobalDamperImpl::GlobalDamperImpl(const SimbodyMatterSubsystem& matter, Real damping) : matter(matter), damping(damping) {
+}
+
+void Force::GlobalDamperImpl::calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const {
+ mobilityForces -= damping*matter.getU(state);
+}
+
+Real Force::GlobalDamperImpl::calcPotentialEnergy(const State& state) const {
+ return 0;
+}
+
+
+//------------------------------ UniformGravity --------------------------------
+//------------------------------------------------------------------------------
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Force::UniformGravity, Force::UniformGravityImpl, Force);
+
+Force::UniformGravity::UniformGravity(GeneralForceSubsystem& forces, const SimbodyMatterSubsystem& matter,
+ const Vec3& g, Real zeroHeight) : Force(new UniformGravityImpl(matter, g, zeroHeight)) {
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+Vec3 Force::UniformGravity::getGravity() const {
+ return getImpl().getGravity();
+}
+
+void Force::UniformGravity::setGravity(const Vec3& g) {
+ updImpl().setGravity(g);
+}
+
+Real Force::UniformGravity::getZeroHeight() const {
+ return getImpl().getZeroHeight();
+}
+
+void Force::UniformGravity::setZeroHeight(Real height) {
+ updImpl().setZeroHeight(height);
+}
+
+Force::UniformGravityImpl::UniformGravityImpl(const SimbodyMatterSubsystem& matter, const Vec3& g, Real zeroHeight) : matter(matter), g(g), zeroHeight(zeroHeight) {
+}
+
+void Force::UniformGravityImpl::calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const {
+ const int nBodies = matter.getNumBodies();
+ const int nParticles = matter.getNumParticles();
+
+ if (nParticles) {
+ const Vector& m = matter.getAllParticleMasses(state);
+ const Vector_<Vec3>& loc_G = matter.getAllParticleLocations(state);
+ for (int i=0; i < nParticles; ++i) {
+ particleForces[i] += g * m[i];
+ }
+ }
+
+ // no need to apply gravity to Ground!
+ for (MobilizedBodyIndex i(1); i < nBodies; ++i) {
+ const MassProperties& mprops = matter.getMobilizedBody(i).getBodyMassProperties(state);
+ const Real& m = mprops.getMass();
+ const Vec3& com_B = mprops.getMassCenter();
+ const Transform& X_GB = matter.getMobilizedBody(i).getBodyTransform(state);
+ const Vec3 com_B_G = X_GB.R()*com_B;
+ const Vec3 frc_G = m*g;
+
+ bodyForces[i] += SpatialVec(com_B_G % frc_G, frc_G);
+ }
+}
+
+Real Force::UniformGravityImpl::calcPotentialEnergy(const State& state) const {
+ const int nBodies = matter.getNumBodies();
+ const int nParticles = matter.getNumParticles();
+ Real pe = 0.0;
+
+ if (nParticles) {
+ const Vector& m = matter.getAllParticleMasses(state);
+ const Vector_<Vec3>& loc_G = matter.getAllParticleLocations(state);
+ for (int i=0; i < nParticles; ++i) {
+ pe -= m[i]*(~g*loc_G[i] + zeroHeight); // odd signs because height is in -g direction
+ }
+ }
+
+ // no need to apply gravity to Ground!
+ for (MobilizedBodyIndex i(1); i < nBodies; ++i) {
+ const MassProperties& mprops = matter.getMobilizedBody(i).getBodyMassProperties(state);
+ const Real& m = mprops.getMass();
+ const Vec3& com_B = mprops.getMassCenter();
+ const Transform& X_GB = matter.getMobilizedBody(i).getBodyTransform(state);
+ const Vec3 com_B_G = X_GB.R()*com_B;
+ const Vec3 com_G = X_GB.p() + com_B_G;
+
+ pe -= m*(~g*com_G + zeroHeight); // odd signs because height is in -g direction
+ }
+ return pe;
+}
+
+
+//---------------------------------- Custom ------------------------------------
+//------------------------------------------------------------------------------
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Force::Custom, Force::CustomImpl, Force);
+
+Force::Custom::Custom(GeneralForceSubsystem& forces, Implementation* implementation) :
+ Force(new CustomImpl(implementation)) {
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+const Force::Custom::Implementation& Force::Custom::getImplementation() const {
+ return getImpl().getImplementation();
+}
+
+Force::Custom::Implementation& Force::Custom::updImplementation() {
+ return updImpl().updImplementation();
+}
+
+
+Force::CustomImpl::CustomImpl(Force::Custom::Implementation* implementation) : implementation(implementation) {
+}
+
+void Force::CustomImpl::calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const {
+ implementation->calcForce(state, bodyForces, particleForces, mobilityForces);
+}
+
+Real Force::CustomImpl::calcPotentialEnergy(const State& state) const {
+ return implementation->calcPotentialEnergy(state);
+}
+
+} // namespace SimTK
+
diff --git a/Simbody/src/ForceImpl.h b/Simbody/src/ForceImpl.h
new file mode 100644
index 0000000..9b03bbb
--- /dev/null
+++ b/Simbody/src/ForceImpl.h
@@ -0,0 +1,652 @@
+#ifndef SimTK_SIMBODY_FORCE_IMPL_H_
+#define SimTK_SIMBODY_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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Michael Sherman *
+ * Contributors: *
+ * *
+ * 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"
+#include "simbody/internal/Force_BuiltIns.h"
+
+namespace SimTK {
+
+// This is what a Force handle points to.
+class ForceImpl : public PIMPLImplementation<Force, ForceImpl> {
+public:
+ ForceImpl() : forces(0), defaultDisabled(false) {}
+ ForceImpl(const ForceImpl& clone) {*this = clone;}
+
+ void setDisabledByDefault(bool shouldBeDisabled)
+ { invalidateTopologyCache();
+ defaultDisabled = shouldBeDisabled; }
+
+ bool isDisabledByDefault() const
+ { return defaultDisabled; }
+
+ virtual ~ForceImpl() {}
+ virtual ForceImpl* clone() const = 0;
+ virtual bool dependsOnlyOnPositions() const {
+ return false;
+ }
+ ForceIndex getForceIndex() const {return index;}
+ const GeneralForceSubsystem& getForceSubsystem() const
+ { assert(forces); return *forces; }
+ void setForceSubsystem(GeneralForceSubsystem& frcsub, ForceIndex ix) {
+ forces = &frcsub;
+ index = ix;
+ }
+ void invalidateTopologyCache() const {
+ if (forces) forces->invalidateSubsystemTopologyCache();
+ }
+
+ // Every force element must provide the next two methods. Note that
+ // calcForce() must *add in* (+=) its forces to the given arrays.
+ virtual void calcForce(const State& state,
+ Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces,
+ Vector& mobilityForces) const = 0;
+ virtual Real calcPotentialEnergy(const State& state) const = 0;
+
+ virtual void realizeTopology (State& state) const {}
+ virtual void realizeModel (State& state) const {}
+ virtual void realizeInstance (const State& state) const {}
+ virtual void realizeTime (const State& state) const {}
+ virtual void realizePosition (const State& state) const {}
+ virtual void realizeVelocity (const State& state) const {}
+ virtual void realizeDynamics (const State& state) const {}
+ virtual void realizeAcceleration(const State& state) const {}
+ virtual void realizeReport (const State& state) const {}
+
+ virtual void calcDecorativeGeometryAndAppend
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const {}
+
+private:
+ // CONSTRUCTION
+ GeneralForceSubsystem* forces; // just a reference; no delete on destruction
+ ForceIndex index;
+
+ // TOPOLOGY "STATE"
+ // Changing anything here invalidates the topology of the containing
+ // force Subsystem and thus of the whole System.
+
+ // This says whether the Instance-stage "disabled" flag for this force
+ // element should be initially on or off. Most force elements are enabled
+ // by default.
+ bool defaultDisabled;
+
+ // TOPOLOGY "CACHE"
+ // Nothing in the base Impl class.
+};
+
+
+
+//------------------------------------------------------------------------------
+// TWO POINT LINEAR SPRING IMPL
+//------------------------------------------------------------------------------
+class Force::TwoPointLinearSpringImpl : public ForceImpl {
+public:
+ TwoPointLinearSpringImpl(const MobilizedBody& body1, const Vec3& station1, const MobilizedBody& body2, const Vec3& station2, Real k, Real x0);
+ TwoPointLinearSpringImpl* clone() const {
+ return new TwoPointLinearSpringImpl(*this);
+ }
+ bool dependsOnlyOnPositions() const {
+ return true;
+ }
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const;
+ Real calcPotentialEnergy(const State& state) const;
+private:
+ const SimbodyMatterSubsystem& matter;
+ const MobilizedBodyIndex body1, body2;
+ Vec3 station1, station2;
+ Real k, x0;
+};
+
+
+
+//------------------------------------------------------------------------------
+// TWO POINT LINEAR DAMPER IMPL
+//------------------------------------------------------------------------------
+class Force::TwoPointLinearDamperImpl : public ForceImpl {
+public:
+ TwoPointLinearDamperImpl(const MobilizedBody& body1, const Vec3& station1, const MobilizedBody& body2, const Vec3& station2, Real damping);
+ TwoPointLinearDamperImpl* clone() const {
+ return new TwoPointLinearDamperImpl(*this);
+ }
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const;
+ Real calcPotentialEnergy(const State& state) const;
+private:
+ const SimbodyMatterSubsystem& matter;
+ const MobilizedBodyIndex body1, body2;
+ Vec3 station1, station2;
+ Real damping;
+};
+
+
+
+//------------------------------------------------------------------------------
+// TWO POINT CONSTANT FORCE IMPL
+//------------------------------------------------------------------------------
+class Force::TwoPointConstantForceImpl : public ForceImpl {
+public:
+ TwoPointConstantForceImpl(const MobilizedBody& body1, const Vec3& station1, const MobilizedBody& body2, const Vec3& station2, Real force);
+ TwoPointConstantForceImpl* clone() const {
+ return new TwoPointConstantForceImpl(*this);
+ }
+ bool dependsOnlyOnPositions() const {
+ return true;
+ }
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const;
+ Real calcPotentialEnergy(const State& state) const;
+private:
+ const SimbodyMatterSubsystem& matter;
+ const MobilizedBodyIndex body1, body2;
+ Vec3 station1, station2;
+ Real force;
+};
+
+
+
+//------------------------------------------------------------------------------
+// MOBILITY LINEAR SPRING IMPL
+//------------------------------------------------------------------------------
+class Force::MobilityLinearSpringImpl : public ForceImpl {
+friend class MobilityLinearSpring;
+
+ MobilityLinearSpringImpl(const MobilizedBody& mobod,
+ MobilizerQIndex whichQ,
+ Real defaultStiffness,
+ Real defaultQZero);
+
+ const std::pair<Real,Real>& getParams(const State& state) const {
+ return Value< std::pair<Real,Real> >::downcast
+ (getForceSubsystem().getDiscreteVariable(state, m_paramsIx));
+ }
+ std::pair<Real,Real>& updParams(State& state) const {
+ return Value< std::pair<Real,Real> >::updDowncast
+ (getForceSubsystem().updDiscreteVariable(state, m_paramsIx));
+ }
+
+ MobilityLinearSpringImpl* clone() const OVERRIDE_11
+ { return new MobilityLinearSpringImpl(*this); }
+ bool dependsOnlyOnPositions() const OVERRIDE_11 {return true;}
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+ OVERRIDE_11;
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11;
+
+ // Allocate the discrete state variable for the parameters.
+ void realizeTopology(State& s) const OVERRIDE_11 {
+ m_paramsIx = getForceSubsystem()
+ .allocateDiscreteVariable(s, Stage::Dynamics,
+ new Value< std::pair<Real,Real> >
+ (std::make_pair(m_defaultStiffness, m_defaultQZero)));
+ }
+
+//------------------------------------------------------------------------------
+ // TOPOLOGY STATE
+ const SimbodyMatterSubsystem& m_matter;
+ const MobilizedBodyIndex m_mobodIx;
+ const MobilizerQIndex m_whichQ;
+ Real m_defaultStiffness;
+ Real m_defaultQZero;
+
+ // TOPOLOGY CACHE (Set only once in realizeTopology(); const thereafter.)
+ // The value is a pair<Real,Real> containing k,q0
+ mutable DiscreteVariableIndex m_paramsIx;
+};
+
+
+
+//------------------------------------------------------------------------------
+// MOBILITY LINEAR DAMPER IMPL
+//------------------------------------------------------------------------------
+class Force::MobilityLinearDamperImpl : public ForceImpl {
+friend class MobilityLinearDamper;
+
+ MobilityLinearDamperImpl(const MobilizedBody& mobod,
+ MobilizerUIndex whichU,
+ Real defaultDamping);
+
+ const Real& getDamping(const State& state) const {
+ return Value<Real>::downcast
+ (getForceSubsystem().getDiscreteVariable(state, m_dampingIx));
+ }
+ Real& updDamping(State& state) const {
+ return Value<Real>::updDowncast
+ (getForceSubsystem().updDiscreteVariable(state, m_dampingIx));
+ }
+
+ MobilityLinearDamperImpl* clone() const OVERRIDE_11
+ { return new MobilityLinearDamperImpl(*this); }
+
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+ OVERRIDE_11;
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11;
+
+ // Allocate the discrete state variable for the parameters.
+ void realizeTopology(State& s) const OVERRIDE_11 {
+ m_dampingIx = getForceSubsystem()
+ .allocateDiscreteVariable(s, Stage::Dynamics,
+ new Value<Real>(m_defaultDamping));
+ }
+
+//------------------------------------------------------------------------------
+ // TOPOLOGY STATE
+ const SimbodyMatterSubsystem& m_matter;
+ const MobilizedBodyIndex m_mobodIx;
+ MobilizerUIndex m_whichU;
+ Real m_defaultDamping;
+
+ // TOPOLOGY CACHE (Set only once in realizeTopology(); const thereafter.)
+ // The value is a Real containing c.
+ mutable DiscreteVariableIndex m_dampingIx;
+
+};
+
+
+
+//------------------------------------------------------------------------------
+// MOBILITY CONSTANT FORCE IMPL
+//------------------------------------------------------------------------------
+// Hidden implementation class for a MobilityConstantForce force element.
+class Force::MobilityConstantForceImpl : public ForceImpl {
+friend class MobilityConstantForce;
+
+ MobilityConstantForceImpl(const MobilizedBody& mobod,
+ MobilizerUIndex whichU,
+ Real defaultForce);
+
+ Real getForce(const State& state) const {
+ return Value<Real>::downcast
+ (getForceSubsystem().getDiscreteVariable(state, m_forceIx));
+ }
+ Real& updForce(State& state) const {
+ return Value<Real>::updDowncast
+ (getForceSubsystem().updDiscreteVariable(state, m_forceIx));
+ }
+
+ // Implementation of virtual methods from ForceImpl:
+
+ MobilityConstantForceImpl* clone() const OVERRIDE_11
+ { return new MobilityConstantForceImpl(*this); }
+
+ // Has to wait for Dynamics stage because that's all that gets invalidated
+ // if the constant force is changed.
+ bool dependsOnlyOnPositions() const OVERRIDE_11 {return false;}
+
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+ OVERRIDE_11;
+
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11 {return 0;}
+
+ // Allocate the discrete state variable for the force.
+ void realizeTopology(State& s) const OVERRIDE_11 {
+ m_forceIx = getForceSubsystem()
+ .allocateDiscreteVariable(s, Stage::Dynamics,
+ new Value<Real>(m_defaultForce));
+ }
+
+//------------------------------------------------------------------------------
+ // TOPOLOGY STATE
+ const SimbodyMatterSubsystem& m_matter;
+ const MobilizedBodyIndex m_mobodIx;
+ const MobilizerUIndex m_whichU;
+ Real m_defaultForce;
+
+ // TOPOLOGY CACHE (Set only once in realizeTopology(); const thereafter.)
+ mutable DiscreteVariableIndex m_forceIx;
+};
+
+//------------------------------------------------------------------------------
+// MOBILITY LINEAR STOP IMPL
+//------------------------------------------------------------------------------
+// Hidden implementation class for a MobilityLinearStop force element.
+class Force::MobilityLinearStopImpl : public ForceImpl {
+friend class MobilityLinearStop;
+
+ // Type of the discrete state variable that holds values for this
+ // stop's changeable parameters in a State. Since these affect only forces
+ // the variable invalidates Dynamics stage and later when it changes.
+ struct Parameters {
+ Parameters(Real defStiffness, Real defDissipation,
+ Real defQLow, Real defQHigh)
+ : k(defStiffness), d(defDissipation),
+ qLow(defQLow), qHigh(defQHigh) {}
+
+ Real k, d, qLow, qHigh;
+ };
+
+ MobilityLinearStopImpl(const MobilizedBody& mobod,
+ MobilizerQIndex whichQ,
+ Real defaultStiffness,
+ Real defaultDissipation,
+ Real defaultQLow,
+ Real defaultQHigh);
+
+ // Implementation of virtual methods from ForceImpl:
+ MobilityLinearStopImpl* clone() const OVERRIDE_11
+ { return new MobilityLinearStopImpl(*this); }
+ bool dependsOnlyOnPositions() const OVERRIDE_11 {return false;}
+
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+ OVERRIDE_11;
+
+ // We're not bothering to cache P.E. -- just recalculate it when asked.
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11;
+
+ // Allocate the state variables and cache entry.
+ void realizeTopology(State& s) const OVERRIDE_11 {
+ // Allocate the discrete variable for dynamics parameters.
+ const Parameters dv(m_defStiffness, m_defDissipation,
+ m_defQLow, m_defQHigh);
+ m_parametersIx = getForceSubsystem()
+ .allocateDiscreteVariable(s, Stage::Dynamics,
+ new Value<Parameters>(dv));
+ }
+
+ const Parameters& getParameters(const State& s) const
+ { return Value<Parameters>::downcast
+ (getForceSubsystem().getDiscreteVariable(s,m_parametersIx)); }
+ Parameters& updParameters(State& s) const
+ { return Value<Parameters>::updDowncast
+ (getForceSubsystem().updDiscreteVariable(s,m_parametersIx)); }
+
+//------------------------------------------------------------------------------
+ // TOPOLOGY STATE
+ const SimbodyMatterSubsystem& m_matter;
+ const MobilizedBodyIndex m_mobodIx;
+ const MobilizerQIndex m_whichQ;
+
+ Real m_defStiffness, m_defDissipation, m_defQLow, m_defQHigh;
+
+ // TOPOLOGY CACHE (Set only once in realizeTopology(); const thereafter.)
+ mutable DiscreteVariableIndex m_parametersIx; // k, d, qLow, qHigh
+};
+
+
+
+//------------------------------------------------------------------------------
+// MOBILITY DISCRETE FORCE IMPL
+//------------------------------------------------------------------------------
+// This Force element allocates a scalar discrete variable and applies its
+// value as a generalized force at a particular mobility. The value can be
+// set externally in an event handler or between steps.
+class Force::MobilityDiscreteForceImpl : public ForceImpl {
+public:
+ MobilityDiscreteForceImpl(const MobilizedBody& mobod,
+ MobilizerUIndex whichU,
+ Real defaultForce);
+
+ // Change the force value to be applied. The force will remain at this
+ // value until changed again.
+ void setMobilityForce(State& state, Real f) const;
+
+ // Get the value of the generalized force to be applied.
+ Real getMobilityForce(const State& state) const;
+
+ // Override five virtuals from base class:
+
+ // This is called at Simbody's realize(Dynamics) stage.
+ void calcForce( const State& state,
+ Vector_<SpatialVec>& /*bodyForces*/,
+ Vector_<Vec3>& /*particleForces*/,
+ Vector& mobilityForces) const OVERRIDE_11;
+
+ // This force element does not store potential energy.
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11 {return 0;}
+
+ // Allocate the needed state variable and record its index.
+ void realizeTopology(State& state) const OVERRIDE_11;
+
+ MobilityDiscreteForceImpl* clone() const OVERRIDE_11
+ { return new MobilityDiscreteForceImpl(*this); }
+
+ // Force this to wait for Dynamics stage before calculating, because that's
+ // all that gets invalidated when a new forces is applied.
+ bool dependsOnlyOnPositions() const OVERRIDE_11 {return false;}
+
+private:
+friend class Force::MobilityDiscreteForce;
+
+ const SimbodyMatterSubsystem& m_matter;
+ const MobilizedBodyIndex m_mobodIx;
+ const MobilizerUIndex m_whichU;
+ Real m_defaultVal;
+
+ mutable DiscreteVariableIndex m_forceIx;
+};
+
+
+
+//------------------------------------------------------------------------------
+// DISCRETE FORCES IMPL
+//------------------------------------------------------------------------------
+// This Force element allocates a Vector discrete variables and applies their
+// values as generalized and body spatial forces. The values can be
+// set externally in an event handler or between steps.
+class Force::DiscreteForcesImpl : public ForceImpl {
+public:
+ DiscreteForcesImpl(const SimbodyMatterSubsystem& matter);
+
+ const Vector& getAllMobilityForces(const State& state) const;
+ Vector& updAllMobilityForces(State& state) const;
+
+ const Vector_<SpatialVec>& getAllBodyForces(const State& state) const;
+ Vector_<SpatialVec>& updAllBodyForces(State& state) const;
+
+ // Override five virtuals from base class:
+
+ // This is called at Simbody's realize(Dynamics) stage.
+ void calcForce( const State& state,
+ Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces,
+ Vector& mobilityForces) const OVERRIDE_11;
+
+ // This force element does not store potential energy.
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11 {return 0;}
+
+ // Allocate the needed state variable and record its index.
+ void realizeTopology(State& state) const OVERRIDE_11;
+
+ DiscreteForcesImpl* clone() const OVERRIDE_11
+ { return new DiscreteForcesImpl(*this); }
+
+ // Force this to wait for Dynamics stage before calculating, because that's
+ // all that gets invalidated when a new forces is applied.
+ bool dependsOnlyOnPositions() const OVERRIDE_11 {return false;}
+
+private:
+friend class Force::DiscreteForces;
+
+ const SimbodyMatterSubsystem& m_matter;
+
+ mutable DiscreteVariableIndex m_mobForcesIx; // Vector(n)
+ mutable DiscreteVariableIndex m_bodyForcesIx; // Vector_<SpatialVec>(nb)
+};
+
+
+
+//------------------------------------------------------------------------------
+// CONSTANT FORCE IMPL
+//------------------------------------------------------------------------------
+class Force::ConstantForceImpl : public ForceImpl {
+public:
+ ConstantForceImpl(const MobilizedBody& body, const Vec3& station, const Vec3& force);
+ ConstantForceImpl* clone() const {
+ return new ConstantForceImpl(*this);
+ }
+ bool dependsOnlyOnPositions() const {
+ return true;
+ }
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const;
+ Real calcPotentialEnergy(const State& state) const;
+private:
+ const SimbodyMatterSubsystem& matter;
+ const MobilizedBodyIndex body;
+ Vec3 station, force;
+};
+
+
+
+//------------------------------------------------------------------------------
+// CONSTANT TORQUE IMPL
+//------------------------------------------------------------------------------
+class Force::ConstantTorqueImpl : public ForceImpl {
+public:
+ ConstantTorqueImpl(const MobilizedBody& body, const Vec3& torque);
+ ConstantTorqueImpl* clone() const {
+ return new ConstantTorqueImpl(*this);
+ }
+ bool dependsOnlyOnPositions() const {
+ return true;
+ }
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const;
+ Real calcPotentialEnergy(const State& state) const;
+private:
+ const SimbodyMatterSubsystem& matter;
+ const MobilizedBodyIndex body;
+ Vec3 torque;
+};
+
+
+
+//------------------------------------------------------------------------------
+// GLOBAL DAMPER IMPL
+//------------------------------------------------------------------------------
+class Force::GlobalDamperImpl : public ForceImpl {
+public:
+ GlobalDamperImpl(const SimbodyMatterSubsystem& matter, Real damping);
+ GlobalDamperImpl* clone() const {
+ return new GlobalDamperImpl(*this);
+ }
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const;
+ Real calcPotentialEnergy(const State& state) const;
+private:
+ const SimbodyMatterSubsystem& matter;
+ Real damping;
+};
+
+
+
+//------------------------------------------------------------------------------
+// UNIFORM GRAVITY IMPL
+//------------------------------------------------------------------------------
+class Force::UniformGravityImpl : public ForceImpl {
+public:
+ UniformGravityImpl(const SimbodyMatterSubsystem& matter, const Vec3& g, Real zeroHeight);
+ UniformGravityImpl* clone() const {
+ return new UniformGravityImpl(*this);
+ }
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const;
+ Real calcPotentialEnergy(const State& state) const;
+ Vec3 getGravity() const {
+ return g;
+ }
+ void setGravity(const Vec3& gravity) {
+ g = gravity;
+ invalidateTopologyCache();
+ }
+ Real getZeroHeight() const {
+ return zeroHeight;
+ }
+ void setZeroHeight(Real height) {
+ zeroHeight = height;
+ invalidateTopologyCache();
+ }
+private:
+ const SimbodyMatterSubsystem& matter;
+ Vec3 g;
+ Real zeroHeight;
+};
+
+
+
+//------------------------------------------------------------------------------
+// CUSTOM IMPL
+//------------------------------------------------------------------------------
+class Force::CustomImpl : public ForceImpl {
+public:
+ CustomImpl(Force::Custom::Implementation* implementation);
+ CustomImpl* clone() const {
+ return new CustomImpl(*this);
+ }
+ bool dependsOnlyOnPositions() const {
+ return implementation->dependsOnlyOnPositions();
+ }
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces)
+ const OVERRIDE_11;
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11;
+ ~CustomImpl() {
+ delete implementation;
+ }
+ const Force::Custom::Implementation& getImplementation() const {
+ return *implementation;
+ }
+ Force::Custom::Implementation& updImplementation() {
+ return *implementation;
+ }
+protected:
+ void realizeTopology(State& state) const OVERRIDE_11 {
+ implementation->realizeTopology(state);
+ }
+ void realizeModel(State& state) const OVERRIDE_11 {
+ implementation->realizeModel(state);
+ }
+ void realizeInstance(const State& state) const OVERRIDE_11 {
+ implementation->realizeInstance(state);
+ }
+ void realizeTime(const State& state) const OVERRIDE_11 {
+ implementation->realizeTime(state);
+ }
+ void realizePosition(const State& state) const OVERRIDE_11 {
+ implementation->realizePosition(state);
+ }
+ void realizeVelocity(const State& state) const OVERRIDE_11 {
+ implementation->realizeVelocity(state);
+ }
+ void realizeDynamics(const State& state) const OVERRIDE_11 {
+ implementation->realizeDynamics(state);
+ }
+ void realizeAcceleration(const State& state) const OVERRIDE_11 {
+ implementation->realizeAcceleration(state);
+ }
+ void realizeReport(const State& state) const OVERRIDE_11 {
+ implementation->realizeReport(state);
+ }
+ void calcDecorativeGeometryAndAppend(const State& state, Stage stage,
+ Array_<DecorativeGeometry>& geom) const OVERRIDE_11
+ {
+ implementation->calcDecorativeGeometryAndAppend(state,stage,geom);
+ }
+private:
+ Force::Custom::Implementation* implementation;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_FORCE_IMPL_H_
diff --git a/Simbody/src/ForceSubsystem.cpp b/Simbody/src/ForceSubsystem.cpp
new file mode 100644
index 0000000..5ddeec6
--- /dev/null
+++ b/Simbody/src/ForceSubsystem.cpp
@@ -0,0 +1,72 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ *
+ * Implementation of ForceSubsystem, a still-abstract Subsystem.
+ */
+
+#include "SimTKcommon.h"
+
+#include "simbody/internal/common.h"
+#include "simbody/internal/ForceSubsystem.h"
+
+#include "simbody/internal/ForceSubsystemGuts.h"
+
+namespace SimTK {
+
+ /////////////////////
+ // FORCE SUBSYSTEM //
+ /////////////////////
+
+// Default constructor is inline and creates an empty handle.
+// Default copy & assignment just copy the parent class.
+// Default destructor destructs the parent class.
+
+/*static*/ bool
+ForceSubsystem::isInstanceOf(const Subsystem& s) {
+ return ForceSubsystemRep::isA(s.getSubsystemGuts());
+}
+/*static*/ const ForceSubsystem&
+ForceSubsystem::downcast(const Subsystem& s) {
+ assert(isInstanceOf(s));
+ return static_cast<const ForceSubsystem&>(s);
+}
+/*static*/ ForceSubsystem&
+ForceSubsystem::updDowncast(Subsystem& s) {
+ assert(isInstanceOf(s));
+ return static_cast<ForceSubsystem&>(s);
+}
+
+
+const ForceSubsystemRep&
+ForceSubsystem::getRep() const {
+ return SimTK_DYNAMIC_CAST_DEBUG<const ForceSubsystemRep&>(getSubsystemGuts());
+}
+ForceSubsystemRep&
+ForceSubsystem::updRep() {
+ return SimTK_DYNAMIC_CAST_DEBUG<ForceSubsystemRep&>(updSubsystemGuts());
+}
+
+} // namespace SimTK
+
diff --git a/Simbody/src/Force_Gravity.cpp b/Simbody/src/Force_Gravity.cpp
new file mode 100644
index 0000000..de620fc
--- /dev/null
+++ b/Simbody/src/Force_Gravity.cpp
@@ -0,0 +1,589 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2010-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/MobilizedBody.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "simbody/internal/Force_Gravity.h"
+
+#include "ForceImpl.h"
+
+namespace SimTK {
+
+//==============================================================================
+// FORCE :: GRAVITY IMPL
+//==============================================================================
+// This is the hidden implementation class for Force::Gravity.
+class Force::GravityImpl : public ForceImpl {
+friend class Force::Gravity;
+
+ // These are settable parameters including gravity vector, zero height,
+ // and which if any mobilized bodies are immune to gravity. Modifying this
+ // variable invalidates Dynamics stage automatically, but every modification
+ // must explicitly invalidate the force cache because that can be filled in
+ // as early as Position stage.
+ struct Parameters {
+ Parameters(const UnitVec3& defDirection,
+ Real defMagnitude, Real defZeroHeight,
+ const Array_<bool,MobilizedBodyIndex>& defMobodIsImmune)
+ : d(defDirection), g(defMagnitude), z(defZeroHeight),
+ mobodIsImmune(defMobodIsImmune) {}
+ UnitVec3 d;
+ Real g, z;
+ Array_<bool,MobilizedBodyIndex> mobodIsImmune; // [nb]
+ };
+
+ // The cache has a SpatialVec for each mobilized body, a Vec3 for each
+ // particle [not used], and a scalar for potential energy. The SpatialVec
+ // corresponding to Ground is initialized to zero and stays that way.
+ // This is a lazy-evaluated cache entry that can be calculated any time
+ // after Position stage. It is automatically invalidated if a Position
+ // stage variable changes. But, it is also dependent on the parameter
+ // values in the discrete Parameters variable and must be invalidated
+ // explicitly if any of those change.
+ struct ForceCache {
+ ForceCache() {}
+ void allocate(int nb, int np, bool initToZero)
+ { F_GB.resize(nb); f_GP.resize(np);
+ if (initToZero) setToZero(); else setToNaN(); }
+ void setToZero() {F_GB.setToZero(); f_GP.setToZero(); pe=0;}
+ void setToNaN()
+ { F_GB.setToNaN(); F_GB[0]=SpatialVec(Vec3(0)); // Ground
+ f_GP.setToNaN(); pe=NaN;}
+ Vector_<SpatialVec> F_GB; // rigid body forces
+ Vector_<Vec3> f_GP; // particle forces
+ Real pe; // total potential energy
+ };
+
+ // Constructor from a direction and magnitude.
+ GravityImpl(const SimbodyMatterSubsystem& matter,
+ const UnitVec3& direction,
+ Real magnitude,
+ Real zeroHeight)
+ : matter(matter), defDirection(direction), defMagnitude(magnitude),
+ defZeroHeight(zeroHeight),
+ defMobodIsImmune(matter.getNumBodies(), false),
+ numEvaluations(0)
+ { defMobodIsImmune.front() = true; } // Ground is always immune
+
+ // Constructor from a gravity vector, which might have zero magnitude.
+ // In that case we'll negate the default up direction in the System as the
+ // default direction. That would only be noticeable if the magnitude were
+ // later increased without giving a new direction.
+ GravityImpl(const SimbodyMatterSubsystem& matter,
+ const Vec3& gravityVec,
+ Real zeroHeight)
+ : matter(matter)
+ { // Invoke the general constructor using system up direction.
+ new (this) GravityImpl(matter, -matter.getSystem().getUpDirection(),
+ gravityVec.norm(), zeroHeight);
+ if (defMagnitude > 0)
+ defDirection=UnitVec3(gravityVec/defMagnitude,true);
+ }
+
+ // Constructor from just gravity magnitude; use negative of System "up"
+ // direction as the down direction here.
+ GravityImpl(const SimbodyMatterSubsystem& matter,
+ Real magnitude,
+ Real zeroHeight)
+ : matter(matter)
+ { // Invoke the general constructor using system up direction.
+ new (this) GravityImpl(matter, -matter.getSystem().getUpDirection(),
+ magnitude, zeroHeight);
+ }
+
+ void setMobodIsImmuneByDefault(MobilizedBodyIndex mbx, bool isImmune) {
+ if (mbx == 0) return; // can't change Ground's innate immunity
+ invalidateTopologyCache();
+ if (defMobodIsImmune.size() < mbx+1)
+ defMobodIsImmune.resize(mbx+1, false);
+ defMobodIsImmune[mbx] = isImmune;
+ }
+
+ bool getMobodIsImmuneByDefault(MobilizedBodyIndex mbx) const {
+ if (defMobodIsImmune.size() < mbx+1)
+ return false;
+ return defMobodIsImmune[mbx];
+ }
+
+ void setMobodIsImmune(State& state, MobilizedBodyIndex mbx,
+ bool isImmune) const {
+ if (mbx == 0) return; // no messing with Ground
+ invalidateForceCache(state);
+ Parameters& p = updParameters(state);
+ p.mobodIsImmune[mbx] = isImmune;
+ }
+
+ bool getMobodIsImmune(const State& state, MobilizedBodyIndex mbx) const {
+ const Parameters& p = getParameters(state);
+ return p.mobodIsImmune[mbx];
+ }
+
+ GravityImpl* clone() const OVERRIDE_11 {
+ return new GravityImpl(*this);
+ }
+
+ // We are doing our own caching here, so don't override the
+ // dependsOnlyOnPositions() method which would cause the base class also
+ // to cache the results.
+
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+ OVERRIDE_11;
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11;
+
+ // Allocate the state variables and cache entries.
+ void realizeTopology(State& s) const OVERRIDE_11;
+
+ const Parameters& getParameters(const State& s) const
+ { return Value<Parameters>::downcast
+ (getForceSubsystem().getDiscreteVariable(s,parametersIx)); }
+ Parameters& updParameters(State& s) const
+ { return Value<Parameters>::updDowncast
+ (getForceSubsystem().updDiscreteVariable(s,parametersIx)); }
+
+ const ForceCache& getForceCache(const State& s) const
+ { return Value<ForceCache>::downcast
+ (getForceSubsystem().getCacheEntry(s,forceCacheIx)); }
+ ForceCache& updForceCache(const State& s) const
+ { return Value<ForceCache>::updDowncast
+ (getForceSubsystem().updCacheEntry(s,forceCacheIx)); }
+
+ bool isForceCacheValid(const State& s) const
+ { return getForceSubsystem().isCacheValueRealized(s,forceCacheIx); }
+ void markForceCacheValid(const State& s) const
+ { getForceSubsystem().markCacheValueRealized(s,forceCacheIx); }
+ void invalidateForceCache(const State& s) const
+ { getForceSubsystem().markCacheValueNotRealized(s,forceCacheIx); }
+
+ // This method calculates gravity forces if needed, and bumps the
+ // numEvaluations counter if it has to do any work.
+ void ensureForceCacheValid(const State&) const;
+
+ // TOPOLOGY STATE
+ const SimbodyMatterSubsystem& matter;
+ UnitVec3 defDirection;
+ Real defMagnitude;
+ Real defZeroHeight;
+ Array_<bool,MobilizedBodyIndex> defMobodIsImmune;
+
+ // TOPOLOGY CACHE
+ DiscreteVariableIndex parametersIx;
+ CacheEntryIndex forceCacheIx;
+
+ mutable long long numEvaluations;
+};
+
+
+//==============================================================================
+// FORCE :: GRAVITY
+//==============================================================================
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Force::Gravity,
+ Force::GravityImpl, Force);
+
+Force::Gravity::Gravity
+ (GeneralForceSubsystem& forces,
+ const SimbodyMatterSubsystem& matter,
+ const UnitVec3& defDirection,
+ Real defMagnitude,
+ Real defZeroHeight)
+: Force(new GravityImpl(matter,defDirection,defMagnitude,defZeroHeight))
+{
+ SimTK_ERRCHK1_ALWAYS(defMagnitude >= 0,
+ "Force::Gravity::Gravity(downDirection,magnitude)",
+ "The gravity magnitude g must be nonnegative but was specified as %g.",
+ defMagnitude);
+ SimTK_ERRCHK_ALWAYS(defDirection.isFinite(),
+ "Force::Gravity::Gravity(downDirection,magnitude)",
+ "A non-finite 'down' direction was received; did you specify a zero-"
+ "length Vec3? The direction must be non-zero.");
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+Force::Gravity::Gravity
+ (GeneralForceSubsystem& forces,
+ const SimbodyMatterSubsystem& matter,
+ const Vec3& defGravity)
+: Force(new GravityImpl(matter,defGravity,0))
+{
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+Force::Gravity::Gravity
+ (GeneralForceSubsystem& forces,
+ const SimbodyMatterSubsystem& matter,
+ Real defMagnitude)
+: Force(new GravityImpl(matter,defMagnitude,0))
+{
+ SimTK_ERRCHK1_ALWAYS(defMagnitude >= 0,
+ "Force::Gravity::Gravity(magnitude)",
+ "The gravity magnitude g must be nonnegative but was specified as %g.",
+ defMagnitude);
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+// Each of the setDefault methods must invalidate the topology cache.
+
+Force::Gravity& Force::Gravity::
+setDefaultBodyIsExcluded(MobilizedBodyIndex mobod, bool isExcluded) {
+ // Invalidates topology cache.
+ updImpl().setMobodIsImmuneByDefault(mobod, isExcluded);
+ return *this;
+}
+
+Force::Gravity& Force::Gravity::
+setDefaultGravityVector(const Vec3& gravity) {
+ const Real g = gravity.norm();
+ getImpl().invalidateTopologyCache();
+ updImpl().defMagnitude = g;
+ // Don't change the direction if the magnitude is zero.
+ if (g > 0)
+ updImpl().defDirection = UnitVec3(gravity/g, true);
+ return *this;
+}
+
+Force::Gravity& Force::Gravity::
+setDefaultDownDirection(const UnitVec3& down) {
+ SimTK_ERRCHK_ALWAYS(down.isFinite(),
+ "Force::Gravity::setDefaultDownDirection()",
+ "A non-finite 'down' direction was received; did you specify a zero-"
+ "length Vec3? The direction must be non-zero.");
+
+ getImpl().invalidateTopologyCache();
+ updImpl().defDirection = down;
+ return *this;
+}
+
+Force::Gravity& Force::Gravity::
+setDefaultMagnitude(Real g) {
+ SimTK_ERRCHK1_ALWAYS(g >= 0,
+ "Force::Gravity::setDefaultMagnitude()",
+ "The gravity magnitude g must be nonnegative but was specified as %g.",
+ g);
+
+ getImpl().invalidateTopologyCache();
+ updImpl().defMagnitude = g;
+ return *this;
+}
+
+Force::Gravity& Force::Gravity::
+setDefaultZeroHeight(Real zeroHeight) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defZeroHeight = zeroHeight;
+ return *this;
+}
+
+bool Force::Gravity::
+getDefaultBodyIsExcluded(MobilizedBodyIndex mobod) const
+{ return getImpl().getMobodIsImmuneByDefault(mobod); }
+Vec3 Force::Gravity::
+getDefaultGravityVector() const
+{ return getImpl().defDirection * getImpl().defMagnitude;}
+const UnitVec3& Force::Gravity::
+getDefaultDownDirection() const {return getImpl().defDirection;}
+Real Force::Gravity::
+getDefaultMagnitude() const {return getImpl().defMagnitude;}
+Real Force::Gravity::
+getDefaultZeroHeight() const {return getImpl().defZeroHeight;}
+
+
+// These set routines must explicitly invalidate the force cache since only
+// Dynamics stage is invalidated automatically by this state variable. Try
+// not to do anything if the new value is the same as the old one.
+
+const Force::Gravity& Force::Gravity::
+setBodyIsExcluded(State& state, MobilizedBodyIndex mobod,
+ bool isExcluded) const
+{
+ const GravityImpl& impl = getImpl();
+ SimTK_ERRCHK2_ALWAYS(mobod < impl.matter.getNumBodies(),
+ "Force::Gravity::setBodyIsExcluded()",
+ "Attemped to exclude mobilized body with index %d but only mobilized"
+ " bodies with indices between 0 and %d exist in this System.",
+ (int)mobod, impl.matter.getNumBodies()-1);
+
+ if (getBodyIsExcluded(state, mobod) != isExcluded) {
+ // Invalidates force cache.
+ impl.setMobodIsImmune(state, mobod, isExcluded);
+ // The zero must be precalculated if the body is immune to gravity.
+ SpatialVec& F = impl.updForceCache(state).F_GB[mobod];
+ if (isExcluded || getMagnitude(state) == 0)
+ F.setToZero();
+ else
+ F.setToNaN();
+ }
+ return *this;
+}
+
+const Force::Gravity& Force::Gravity::
+setGravityVector(State& state, const Vec3& gravity) const {
+ const Real newg = gravity.norm();
+ const UnitVec3 newd = newg > 0 ? UnitVec3(gravity/newg, true)
+ : getDownDirection(state);
+ if (getMagnitude(state) != newg || getDownDirection(state) != newd) {
+ getImpl().invalidateForceCache(state);
+ getImpl().updParameters(state).g = newg;
+ getImpl().updParameters(state).d = newd;
+
+ if (newg == 0)
+ getImpl().updForceCache(state).setToZero(); // must precalculate
+ }
+ return *this;
+}
+
+const Force::Gravity& Force::Gravity::
+setDownDirection(State& state, const UnitVec3& down) const {
+ SimTK_ERRCHK_ALWAYS(down.isFinite(),
+ "Force::Gravity::setDownDirection()",
+ "A non-finite 'down' direction was received; did you specify a zero-"
+ "length Vec3? The direction must be non-zero.");
+
+ if (getDownDirection(state) != down) {
+ getImpl().invalidateForceCache(state);
+ getImpl().updParameters(state).d = down;
+ }
+ return *this;
+}
+
+const Force::Gravity& Force::Gravity::
+setMagnitude(State& state, Real g) const {
+ SimTK_ERRCHK1_ALWAYS(g >= 0,
+ "Force::Gravity::setMagnitude()",
+ "The gravity magnitude g must be nonnegative but was specified as %g.",
+ g);
+
+ if (getMagnitude(state) != g) {
+ getImpl().invalidateForceCache(state);
+ getImpl().updParameters(state).g = g;
+
+ if (g == 0)
+ getImpl().updForceCache(state).setToZero(); // must precalculate
+ }
+ return *this;
+}
+
+const Force::Gravity& Force::Gravity::
+setZeroHeight(State& state, Real zeroHeight) const {
+ if (getZeroHeight(state) != zeroHeight) {
+ getImpl().invalidateForceCache(state);
+ getImpl().updParameters(state).z = zeroHeight;
+ }
+ return *this;
+}
+
+bool Force::Gravity::
+getBodyIsExcluded(const State& state, MobilizedBodyIndex mobod) const
+{ return getImpl().getMobodIsImmune(state, mobod); }
+
+
+Vec3 Force::Gravity::
+getGravityVector(const State& state) const
+{ const GravityImpl::Parameters& p = getImpl().getParameters(state);
+ return p.g*p.d; }
+
+const UnitVec3& Force::Gravity::
+getDownDirection(const State& state) const
+{ return getImpl().getParameters(state).d; }
+
+
+Real Force::Gravity::getMagnitude(const State& state) const
+{ return getImpl().getParameters(state).g; }
+
+Real Force::Gravity::getZeroHeight(const State& state) const
+{ return getImpl().getParameters(state).z; }
+
+Real Force::Gravity::
+getPotentialEnergy(const State& s) const
+{ getImpl().ensureForceCacheValid(s);
+ return getImpl().getForceCache(s).pe; }
+
+const Vector_<SpatialVec>& Force::Gravity::
+getBodyForces(const State& s) const
+{ getImpl().ensureForceCacheValid(s);
+ const GravityImpl::ForceCache& fc = getImpl().getForceCache(s);
+ return fc.F_GB; }
+
+const Vector_<Vec3>& Force::Gravity::
+getParticleForces(const State& s) const
+{ getImpl().ensureForceCacheValid(s);
+ const GravityImpl::ForceCache& fc = getImpl().getForceCache(s);
+ return fc.f_GP; }
+
+long long Force::Gravity::
+getNumEvaluations() const
+{ return getImpl().numEvaluations; }
+
+bool Force::Gravity::
+isForceCacheValid(const State& state) const
+{ return getImpl().isForceCacheValid(state); }
+
+void Force::Gravity::
+invalidateForceCache(const State& state) const
+{ getImpl().invalidateForceCache(state); }
+
+
+//==============================================================================
+// FORCE :: GRAVITY IMPL
+//==============================================================================
+
+//----------------------------- REALIZE TOPOLOGY -------------------------------
+// Allocate the state variables and cache entries. The force cache is a lazy-
+// evaluation entry - although it can be calculated any time after
+// Stage::Position, it won't be unless someone asks for it. And if it is
+// evaluated early, it should not be re-evaluated when used as a force during
+// Stage::Dynamics (via the calcForce() call).
+//
+// In addition, the force cache has a dependency on the parameter values that
+// are stored in a discrete state variable. Changes to that variable
+// automatically invalidate Dynamics stage, but must be carefully managed also
+// to invalidate the force cache here since it is only Position-dependent.
+void Force::GravityImpl::
+realizeTopology(State& s) const {
+ GravityImpl* mThis = const_cast<GravityImpl*>(this);
+ const int nb=matter.getNumBodies(), np=matter.getNumParticles();
+
+ // In case more mobilized bodies were added after this Gravity element
+ // was constructed, make room for the rest now. Earlier default immunity
+ // settings are preserved.
+ if (defMobodIsImmune.size() != nb)
+ mThis->defMobodIsImmune.resize(nb, false);
+
+ // Allocate a discrete state variable to hold parameters; see above comment.
+ const Parameters p(defDirection,defMagnitude,defZeroHeight,
+ defMobodIsImmune); // initial value
+ mThis->parametersIx = getForceSubsystem()
+ .allocateDiscreteVariable(s, Stage::Dynamics, new Value<Parameters>(p));
+
+ // Don't allocate force cache space yet since we would have to copy it.
+ // Caution -- dependence on Parameters requires manual invalidation.
+ mThis->forceCacheIx = getForceSubsystem().allocateLazyCacheEntry(s,
+ Stage::Position, new Value<ForceCache>());
+
+ // Now allocate the appropriate amount of space, and set to zero now
+ // any forces that we know will end up zero so we don't have to calculate
+ // them at run time. Precalculated zeroes must be provided for any
+ // immune elements, or for all elements if g==0, and this must be kept
+ // up to date if there are runtime changes to the parameters.
+
+ ForceCache& fc = updForceCache(s);
+ if (defMagnitude == 0) {
+ fc.allocate(nb, np, true); // initially zero since no gravity
+ } else {
+ fc.allocate(nb, np, false); // initially NaN except for Ground
+ for (MobilizedBodyIndex mbx(1); mbx < nb; ++mbx)
+ if (defMobodIsImmune[mbx])
+ fc.F_GB[mbx] = SpatialVec(Vec3(0),Vec3(0));
+ // This doesn't mean the ForceCache is valid yet.
+ }
+}
+
+
+//------------------------- ENSURE FORCE CACHE VALID ---------------------------
+// This will also calculate potential energy since we can do it on the cheap
+// simultaneously with the force. Note that if the strength of gravity was set
+// to zero then we already zeroed out the forces and pe during realizeInstance()
+// so all we have to do in that case is mark the cache valid now. Also, any
+// immune bodies have their force set to zero in realizeInstance() so we don't
+// have to do it again here.
+void Force::GravityImpl::
+ensureForceCacheValid(const State& state) const {
+ if (isForceCacheValid(state)) return;
+
+ SimTK_STAGECHECK_GE_ALWAYS(state.getSystemStage(), Stage::Position,
+ "Force::GravityImpl::ensureForceCacheValid()");
+
+ const Parameters& p = getParameters(state);
+ if (p.g == 0) { // no gravity
+ markForceCacheValid(state); // zeroes must have been precalculated
+ return;
+ }
+
+ // Gravity is non-zero and gravity forces are not up to date, so this counts
+ // as an evaluation.
+ ++numEvaluations;
+
+ const Vec3 gravity = p.g * p.d;
+ const Real zeroPEOffset = p.g * p.z;
+ ForceCache& fc = updForceCache(state);
+ fc.pe = 0;
+
+ const int nb = matter.getNumBodies();
+ // Skip Ground since we know it is immune.
+ for (MobilizedBodyIndex mbx(1); mbx < nb; ++mbx) {
+ if (p.mobodIsImmune[mbx])
+ continue; // don't apply gravity to this body; F already zero
+
+ const MobilizedBody& mobod = matter.getMobilizedBody(mbx);
+ const MassProperties& mprops = mobod.getBodyMassProperties(state);
+ const Transform& X_GB = mobod.getBodyTransform(state);
+
+ Real m = mprops.getMass();
+ const Vec3& p_CB = mprops.getMassCenter(); // in B
+ const Vec3 p_CB_G = X_GB.R()*p_CB; // exp. in G; 15 flops
+ const Vec3 p_G_CB = X_GB.p() + p_CB_G; // meas. in G; 3 flops
+
+ const Vec3 F_CB_G = m*gravity; // force at mass center; 3 flops
+ fc.F_GB[mbx] = SpatialVec(p_CB_G % F_CB_G, F_CB_G); // body frc; 9 flops
+
+ // odd signs here because height is in -gravity direction.
+ fc.pe -= m*(~gravity*p_G_CB + zeroPEOffset); // 8 flops
+ }
+
+ const int np = matter.getNumParticles();
+ if (np) {
+ const Vector& m = matter.getAllParticleMasses(state);
+ const Vector_<Vec3>& p_GP = matter.getAllParticleLocations(state);
+ for (ParticleIndex px(0); px < np; ++px) {
+ fc.f_GP[px] = m[px] * gravity; // 3 flops
+ fc.pe -= m[px]*(~gravity*p_GP[px] + zeroPEOffset); // 8 flops
+ }
+ }
+
+ markForceCacheValid(state);
+}
+
+//------------------------------- CALC FORCE -----------------------------------
+void Force::GravityImpl::
+calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+{ ensureForceCacheValid(state);
+ const ForceCache& fc = getForceCache(state);
+ bodyForces += fc.F_GB;
+ particleForces += fc.f_GP; }
+
+
+//-------------------------- CALC POTENTIAL ENERGY -----------------------------
+// If the force was calculated, then the potential energy will already
+// be valid. Otherwise we'll have to calculate it.
+Real Force::GravityImpl::
+calcPotentialEnergy(const State& state) const
+{ ensureForceCacheValid(state);
+ const ForceCache& fc = getForceCache(state);
+ return fc.pe; }
+
+
+} // namespace SimTK
+
diff --git a/Simbody/src/Force_Instantiations.cpp b/Simbody/src/Force_Instantiations.cpp
new file mode 100644
index 0000000..247ddcf
--- /dev/null
+++ b/Simbody/src/Force_Instantiations.cpp
@@ -0,0 +1,39 @@
+/* -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "SimTKcommon/internal/PrivateImplementation_Defs.h"
+
+// This suppresses the 'extern template' instantiations in Force.h so that
+// we can instantiate them for real here.
+#define SimTK_SIMBODY_DEFINING_FORCE
+#include "simbody/internal/Force.h"
+#include "ForceImpl.h"
+
+namespace SimTK {
+
+template class PIMPLHandle<Force, ForceImpl, true>;
+template class PIMPLImplementation<Force, ForceImpl>;
+} // namespace SimTK
+
diff --git a/Simbody/src/Force_LinearBushing.cpp b/Simbody/src/Force_LinearBushing.cpp
new file mode 100644
index 0000000..9efb9bf
--- /dev/null
+++ b/Simbody/src/Force_LinearBushing.cpp
@@ -0,0 +1,615 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/MobilizedBody.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "simbody/internal/Force_LinearBushing.h"
+
+#include "ForceImpl.h"
+
+namespace SimTK {
+
+class Force::LinearBushingImpl : public ForceImpl {
+ struct InstanceVars {
+ InstanceVars(const Transform& defX_B1F, const Transform& defX_B2M,
+ const Vec6& defStiffness, const Vec6& defDamping)
+ : X_B1F(defX_B1F), X_B2M(defX_B2M), k(defStiffness), c(defDamping) {}
+
+ Transform X_B1F, X_B2M;
+ Vec6 k, c;
+ };
+ struct PositionCache {
+ Transform X_GF, X_GM, X_FM;
+ Vec3 p_B1F_G, p_B2M_G, p_FM_G;
+ Vec6 q;
+ };
+ struct VelocityCache {
+ SpatialVec V_GF, V_GM, V_FM;
+ Vec6 qdot;
+ };
+ struct ForceCache {
+ SpatialVec F_GF, F_GM; // at Bushing frames
+ SpatialVec F_GB1, F_GB2; // at Body frames
+ Vec6 f; // scalar generalized forces
+ Real power;
+ };
+public:
+ LinearBushingImpl(const MobilizedBody& body1, const Transform& frameOnB1,
+ const MobilizedBody& body2, const Transform& frameOnB2,
+ const Vec6& stiffness, const Vec6& damping);
+ LinearBushingImpl* clone() const {
+ return new LinearBushingImpl(*this);
+ }
+ bool dependsOnlyOnPositions() const {
+ return false;
+ }
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const;
+ Real calcPotentialEnergy(const State& state) const;
+
+ // Allocate the position and velocity cache entries. These are all
+ // lazy-evaluation entries - be sure to check whether they have already
+ // been calculated; calculate them if not; and then mark them done.
+ // They will be invalidated when the indicated stage has changed and
+ // can be recalculated any time after that stage is realized.
+ void realizeTopology(State& s) const {
+ LinearBushingImpl* mThis = const_cast<LinearBushingImpl*>(this);
+
+ const InstanceVars iv(defX_B1F,defX_B2M,defK,defC);
+ mThis->instanceVarsIx = getForceSubsystem()
+ .allocateDiscreteVariable(s, Stage::Instance,
+ new Value<InstanceVars>(iv));
+
+ Vector einit(1, Real(0));
+ mThis->dissipatedEnergyIx = getForceSubsystem().allocateZ(s,einit);
+
+ mThis->positionCacheIx = getForceSubsystem().allocateCacheEntry(s,
+ Stage::Position, Stage::Infinity, new Value<PositionCache>());
+ mThis->potEnergyCacheIx = getForceSubsystem().allocateCacheEntry(s,
+ Stage::Position, Stage::Infinity, new Value<Real>(NaN));
+ mThis->velocityCacheIx = getForceSubsystem().allocateCacheEntry(s,
+ Stage::Velocity, Stage::Infinity, new Value<VelocityCache>());
+ mThis->forceCacheIx = getForceSubsystem().allocateCacheEntry(s,
+ Stage::Velocity, Stage::Infinity, new Value<ForceCache>());
+ }
+
+ void realizeAcceleration(const State& s) const {
+ ensureForceCacheValid(s);
+ updDissipatedEnergyDeriv(s) = getForceCache(s).power;
+ }
+
+private:
+ const InstanceVars& getInstanceVars(const State& s) const
+ { return Value<InstanceVars>::downcast
+ (getForceSubsystem().getDiscreteVariable(s,instanceVarsIx)); }
+ InstanceVars& updInstanceVars(State& s) const
+ { return Value<InstanceVars>::updDowncast
+ (getForceSubsystem().updDiscreteVariable(s,instanceVarsIx)); }
+
+ const Real& getDissipatedEnergyVar(const State& s) const
+ { return getForceSubsystem().getZ(s)[dissipatedEnergyIx]; }
+ Real& updDissipatedEnergyVar(State& s) const
+ { return getForceSubsystem().updZ(s)[dissipatedEnergyIx]; }
+ Real& updDissipatedEnergyDeriv(const State& s) const
+ { return getForceSubsystem().updZDot(s)[dissipatedEnergyIx]; }
+
+ const PositionCache& getPositionCache(const State& s) const
+ { return Value<PositionCache>::downcast
+ (getForceSubsystem().getCacheEntry(s,positionCacheIx)); }
+ const Real& getPotentialEnergyCache(const State& s) const
+ { return Value<Real>::downcast
+ (getForceSubsystem().getCacheEntry(s,potEnergyCacheIx)); }
+ const VelocityCache& getVelocityCache(const State& s) const
+ { return Value<VelocityCache>::downcast
+ (getForceSubsystem().getCacheEntry(s,velocityCacheIx)); }
+ const ForceCache& getForceCache(const State& s) const
+ { return Value<ForceCache>::downcast
+ (getForceSubsystem().getCacheEntry(s,forceCacheIx)); }
+
+ PositionCache& updPositionCache(const State& s) const
+ { return Value<PositionCache>::updDowncast
+ (getForceSubsystem().updCacheEntry(s,positionCacheIx)); }
+ Real& updPotentialEnergyCache(const State& s) const
+ { return Value<Real>::updDowncast
+ (getForceSubsystem().updCacheEntry(s,potEnergyCacheIx)); }
+ VelocityCache& updVelocityCache(const State& s) const
+ { return Value<VelocityCache>::updDowncast
+ (getForceSubsystem().updCacheEntry(s,velocityCacheIx)); }
+ ForceCache& updForceCache(const State& s) const
+ { return Value<ForceCache>::updDowncast
+ (getForceSubsystem().updCacheEntry(s,forceCacheIx)); }
+
+ bool isPositionCacheValid(const State& s) const
+ { return getForceSubsystem().isCacheValueRealized(s,positionCacheIx); }
+ bool isPotentialEnergyValid(const State& s) const
+ { return getForceSubsystem().isCacheValueRealized(s,potEnergyCacheIx); }
+ bool isVelocityCacheValid(const State& s) const
+ { return getForceSubsystem().isCacheValueRealized(s,velocityCacheIx); }
+ bool isForceCacheValid(const State& s) const
+ { return getForceSubsystem().isCacheValueRealized(s,forceCacheIx); }
+
+ void markPositionCacheValid(const State& s) const
+ { getForceSubsystem().markCacheValueRealized(s,positionCacheIx); }
+ void markPotentialEnergyValid(const State& s) const
+ { getForceSubsystem().markCacheValueRealized(s,potEnergyCacheIx); }
+ void markVelocityCacheValid(const State& s) const
+ { getForceSubsystem().markCacheValueRealized(s,velocityCacheIx); }
+ void markForceCacheValid(const State& s) const
+ { getForceSubsystem().markCacheValueRealized(s,forceCacheIx); }
+
+ void ensurePositionCacheValid(const State&) const;
+ void ensurePotentialEnergyValid(const State&) const;
+ void ensureVelocityCacheValid(const State&) const;
+ void ensureForceCacheValid(const State&) const;
+
+ // TOPOLOGY STATE
+ const SimbodyMatterSubsystem& matter;
+ MobilizedBodyIndex body1x;
+ MobilizedBodyIndex body2x;
+ Transform defX_B1F, defX_B2M;
+ Vec6 defK, defC;
+
+ // TOPOLOGY CACHE
+ DiscreteVariableIndex instanceVarsIx;
+ ZIndex dissipatedEnergyIx;
+ CacheEntryIndex positionCacheIx;
+ CacheEntryIndex potEnergyCacheIx;
+ CacheEntryIndex velocityCacheIx;
+ CacheEntryIndex forceCacheIx;
+
+friend class Force::LinearBushing;
+friend std::ostream& operator<<(std::ostream&,const InstanceVars&);
+friend std::ostream& operator<<(std::ostream&,const PositionCache&);
+friend std::ostream& operator<<(std::ostream&,const VelocityCache&);
+friend std::ostream& operator<<(std::ostream&,const ForceCache&);
+};
+
+// These are required by Value<T>.
+inline std::ostream& operator<<
+ (std::ostream& o, const Force::LinearBushingImpl::InstanceVars& iv)
+{ assert(!"implemented"); return o; }
+inline std::ostream& operator<<
+ (std::ostream& o, const Force::LinearBushingImpl::PositionCache& pc)
+{ assert(!"implemented"); return o; }
+inline std::ostream& operator<<
+ (std::ostream& o, const Force::LinearBushingImpl::VelocityCache& vc)
+{ assert(!"implemented"); return o; }
+inline std::ostream& operator<<
+ (std::ostream& o, const Force::LinearBushingImpl::ForceCache& fc)
+{ assert(!"implemented"); return o; }
+
+
+//------------------------------ LinearBushing ---------------------------------
+//------------------------------------------------------------------------------
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Force::LinearBushing,
+ Force::LinearBushingImpl, Force);
+
+Force::LinearBushing::LinearBushing
+ (GeneralForceSubsystem& forces,
+ const MobilizedBody& body1, const Transform& frameOnB1,
+ const MobilizedBody& body2, const Transform& frameOnB2,
+ const Vec6& stiffness, const Vec6& damping)
+: Force(new LinearBushingImpl(body1,frameOnB1,body2,frameOnB2,
+ stiffness,damping))
+{
+ SimTK_ERRCHK_ALWAYS(stiffness >= 0,
+ "Force::LinearBushing::ctor()",
+ "Bushing spring constants must be nonnegative.");
+ SimTK_ERRCHK_ALWAYS(damping >= 0,
+ "Force::LinearBushing::ctor()",
+ "Bushing damping coefficients must be nonnegative.");
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+
+Force::LinearBushing::LinearBushing
+ (GeneralForceSubsystem& forces,
+ const MobilizedBody& body1, // assume body frames
+ const MobilizedBody& body2,
+ const Vec6& stiffness, const Vec6& damping)
+: Force(new LinearBushingImpl(body1,Transform(),body2,Transform(),
+ stiffness,damping))
+{
+ SimTK_ERRCHK_ALWAYS(stiffness >= 0,
+ "Force::LinearBushing::ctor()",
+ "Bushing spring constants must be nonnegative.");
+ SimTK_ERRCHK_ALWAYS(damping >= 0,
+ "Force::LinearBushing::ctor()",
+ "Bushing damping coefficients must be nonnegative.");
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+Force::LinearBushing& Force::LinearBushing::
+setDefaultFrameOnBody1(const Transform& X_B1F) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defX_B1F = X_B1F;
+ return *this;
+}
+Force::LinearBushing& Force::LinearBushing::
+setDefaultFrameOnBody2(const Transform& X_B2M) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defX_B2M = X_B2M;
+ return *this;
+}
+Force::LinearBushing& Force::LinearBushing::
+setDefaultStiffness(const Vec6& stiffness) {
+ SimTK_ERRCHK_ALWAYS(stiffness >= 0,
+ "Force::LinearBushing::setDefaultStiffness()",
+ "Bushing spring constants must be nonnegative.");
+ getImpl().invalidateTopologyCache();
+ updImpl().defK = stiffness;
+ return *this;
+}
+Force::LinearBushing& Force::LinearBushing::
+setDefaultDamping(const Vec6& damping) {
+ SimTK_ERRCHK_ALWAYS(damping >= 0,
+ "Force::LinearBushing::setDefaultDamping()",
+ "Bushing damping coefficients must be nonnegative.");
+ getImpl().invalidateTopologyCache();
+ updImpl().defC = damping;
+ return *this;
+}
+const Transform& Force::LinearBushing::
+getDefaultFrameOnBody1() const {return getImpl().defX_B1F;}
+const Transform& Force::LinearBushing::
+getDefaultFrameOnBody2() const {return getImpl().defX_B2M;}
+const Vec6& Force::LinearBushing::
+getDefaultStiffness() const {return getImpl().defK;}
+const Vec6& Force::LinearBushing::
+getDefaultDamping() const {return getImpl().defC;}
+
+const Force::LinearBushing& Force::LinearBushing::
+setFrameOnBody1(State& state, const Transform& X_B1F) const {
+ getImpl().updInstanceVars(state).X_B1F = X_B1F;
+ return *this;
+}
+const Force::LinearBushing& Force::LinearBushing::
+setFrameOnBody2(State& state, const Transform& X_B2M) const {
+ getImpl().updInstanceVars(state).X_B2M = X_B2M;
+ return *this;
+}
+const Force::LinearBushing& Force::LinearBushing::
+setStiffness(State& state, const Vec6& stiffness) const {
+ SimTK_ERRCHK_ALWAYS(stiffness >= 0,
+ "Force::LinearBushing::setStiffness()",
+ "Bushing spring constants must be nonnegative.");
+ getImpl().updInstanceVars(state).k = stiffness;
+ return *this;
+}
+const Force::LinearBushing& Force::LinearBushing::
+setDamping(State& state, const Vec6& damping) const {
+ SimTK_ERRCHK_ALWAYS(damping >= 0,
+ "Force::LinearBushing::setDamping()",
+ "Bushing damping coefficients must be nonnegative.");
+ getImpl().updInstanceVars(state).c = damping;
+ return *this;
+}
+const Transform& Force::LinearBushing::
+getFrameOnBody1(const State& state) const
+{ return getImpl().getInstanceVars(state).X_B1F; }
+const Transform& Force::LinearBushing::
+getFrameOnBody2(const State& state) const
+{ return getImpl().getInstanceVars(state).X_B2M; }
+const Vec6& Force::LinearBushing::
+getStiffness(const State& state) const
+{ return getImpl().getInstanceVars(state).k; }
+const Vec6& Force::LinearBushing::
+getDamping(const State& state) const
+{ return getImpl().getInstanceVars(state).c; }
+
+const Vec6& Force::LinearBushing::
+getQ(const State& s) const
+{ getImpl().ensurePositionCacheValid(s);
+ return getImpl().getPositionCache(s).q; }
+
+const Transform& Force::LinearBushing::
+getX_GF(const State& s) const
+{ getImpl().ensurePositionCacheValid(s);
+ return getImpl().getPositionCache(s).X_GF; }
+const Transform& Force::LinearBushing::
+getX_GM(const State& s) const
+{ getImpl().ensurePositionCacheValid(s);
+ return getImpl().getPositionCache(s).X_GM; }
+const Transform& Force::LinearBushing::
+getX_FM(const State& s) const
+{ getImpl().ensurePositionCacheValid(s);
+ return getImpl().getPositionCache(s).X_FM; }
+
+const Vec6& Force::LinearBushing::
+getQDot(const State& s) const
+{ getImpl().ensureVelocityCacheValid(s);
+ return getImpl().getVelocityCache(s).qdot; }
+const SpatialVec& Force::LinearBushing::
+getV_GF(const State& s) const
+{ getImpl().ensureVelocityCacheValid(s);
+ return getImpl().getVelocityCache(s).V_GF; }
+const SpatialVec& Force::LinearBushing::
+getV_GM(const State& s) const
+{ getImpl().ensureVelocityCacheValid(s);
+ return getImpl().getVelocityCache(s).V_GM; }
+const SpatialVec& Force::LinearBushing::
+getV_FM(const State& s) const
+{ getImpl().ensureVelocityCacheValid(s);
+ return getImpl().getVelocityCache(s).V_FM; }
+
+const Vec6& Force::LinearBushing::
+getF(const State& s) const
+{ getImpl().ensureForceCacheValid(s);
+ return getImpl().getForceCache(s).f; }
+const SpatialVec& Force::LinearBushing::
+getF_GF(const State& s) const
+{ getImpl().ensureForceCacheValid(s);
+ return getImpl().getForceCache(s).F_GF; }
+const SpatialVec& Force::LinearBushing::
+getF_GM(const State& s) const
+{ getImpl().ensureForceCacheValid(s);
+ return getImpl().getForceCache(s).F_GM; }
+
+Real Force::LinearBushing::
+getPotentialEnergy(const State& s) const
+{ getImpl().ensurePotentialEnergyValid(s);
+ return getImpl().getPotentialEnergyCache(s); }
+
+Real Force::LinearBushing::
+getPowerDissipation(const State& s) const
+{ getImpl().ensureForceCacheValid(s);
+ return getImpl().getForceCache(s).power; }
+
+Real Force::LinearBushing::
+getDissipatedEnergy(const State& s) const
+{ return getImpl().getDissipatedEnergyVar(s); }
+
+void Force::LinearBushing::
+setDissipatedEnergy(State& s, Real energy) const {
+ SimTK_ERRCHK1_ALWAYS(energy >= 0,
+ "Force::LinearBushing::setDissipatedEnergy()",
+ "The initial value for the dissipated energy must be nonnegative"
+ " but an attempt was made to set it to %g.", energy);
+ getImpl().updDissipatedEnergyVar(s) = energy;
+}
+
+
+
+//---------------------------- LinearBushingImpl -------------------------------
+//------------------------------------------------------------------------------
+
+
+Force::LinearBushingImpl::LinearBushingImpl
+ (const MobilizedBody& body1, const Transform& frameOnB1,
+ const MobilizedBody& body2, const Transform& frameOnB2,
+ const Vec6& stiffness, const Vec6& damping)
+: matter(body1.getMatterSubsystem()),
+ body1x(body1.getMobilizedBodyIndex()),
+ body2x(body2.getMobilizedBodyIndex()),
+ defX_B1F(frameOnB1), defX_B2M(frameOnB2),
+ defK(stiffness), defC(damping)
+{
+}
+
+void Force::LinearBushingImpl::
+ensurePositionCacheValid(const State& state) const {
+ if (isPositionCacheValid(state)) return;
+
+ const InstanceVars& iv = getInstanceVars(state);
+ const Transform& X_B1F = iv.X_B1F;
+ const Transform& X_B2M = iv.X_B2M;
+
+ PositionCache& pc = updPositionCache(state);
+
+ const MobilizedBody& body1 = matter.getMobilizedBody(body1x);
+ const MobilizedBody& body2 = matter.getMobilizedBody(body2x);
+
+ const Transform& X_GB1 = body1.getBodyTransform(state);
+ const Transform& X_GB2 = body2.getBodyTransform(state);
+ pc.X_GF = X_GB1* X_B1F; // 63 flops
+ pc.X_GM = X_GB2* X_B2M; // 63 flops
+ pc.X_FM = ~pc.X_GF *pc.X_GM; // 63 flops
+
+ // Re-express local vectors in the Ground frame.
+ pc.p_B1F_G = X_GB1.R() * X_B1F.p(); // 15 flops
+ pc.p_B2M_G = X_GB2.R() * X_B2M.p(); // 15 flops
+ pc.p_FM_G = pc.X_GF.R() * pc.X_FM.p(); // 15 flops
+
+ // Calculate the 1-2-3 body B2-fixed Euler angles; these are the
+ // rotational coordinates.
+ pc.q.updSubVec<3>(0) = pc.X_FM.R().convertRotationToBodyFixedXYZ();
+
+ // The translation vector in X_FM contains our translational coordinates.
+ pc.q.updSubVec<3>(3) = pc.X_FM.p();
+
+ markPositionCacheValid(state);
+}
+
+void Force::LinearBushingImpl::
+ensureVelocityCacheValid(const State& state) const {
+ if (isVelocityCacheValid(state)) return;
+
+ // We'll be needing this.
+ ensurePositionCacheValid(state);
+ const PositionCache& pc = getPositionCache(state);
+ const Rotation& R_GF = pc.X_GF.R();
+ const Rotation& R_FM = pc.X_FM.R();
+ const Vec3& q = pc.q.getSubVec<3>(0);
+ const Vec3& p = pc.q.getSubVec<3>(3);
+
+ VelocityCache& vc = updVelocityCache(state);
+
+ const MobilizedBody& body1 = matter.getMobilizedBody(body1x);
+ const MobilizedBody& body2 = matter.getMobilizedBody(body2x);
+
+ // Now do velocities.
+ const SpatialVec& V_GB1 = body1.getBodyVelocity(state);
+ const SpatialVec& V_GB2 = body2.getBodyVelocity(state);
+
+ vc.V_GF = SpatialVec(V_GB1[0], V_GB1[1] + V_GB1[0] % pc.p_B1F_G);
+ vc.V_GM = SpatialVec(V_GB2[0], V_GB2[1] + V_GB2[0] % pc.p_B2M_G);
+
+ // This is the velocity of M in F, but with the time derivative
+ // taken in the Ground frame.
+ const SpatialVec V_FM_G = vc.V_GM - vc.V_GF;
+
+ // To get derivative in F, we must remove the part due to the
+ // angular velocity w_GF of F in G.
+ vc.V_FM = ~R_GF * SpatialVec(V_FM_G[0],
+ V_FM_G[1] - vc.V_GF[0] % pc.p_FM_G);
+
+ // Need angular velocity in M frame for conversion to qdot.
+ const Vec3 w_FM_M = ~R_FM*vc.V_FM[0];
+ const Mat33 N_FM = Rotation::calcNForBodyXYZInBodyFrame(q);
+ vc.qdot.updSubVec<3>(0) = N_FM * w_FM_M;
+ vc.qdot.updSubVec<3>(3) = vc.V_FM[1];
+
+ markVelocityCacheValid(state);
+}
+
+// This will also calculate potential energy since we can do it on the
+// cheap simultaneously with the force.
+void Force::LinearBushingImpl::
+ensureForceCacheValid(const State& state) const {
+ if (isForceCacheValid(state)) return;
+
+ const InstanceVars& iv = getInstanceVars(state);
+ const Transform& X_B1F = iv.X_B1F;
+ const Transform& X_B2M = iv.X_B2M;
+ const Vec6& k = iv.k;
+ const Vec6& c = iv.c;
+
+ ForceCache& fc = updForceCache(state);
+
+ ensurePositionCacheValid(state);
+ const PositionCache& pc = getPositionCache(state);
+
+ const MobilizedBody& body1 = matter.getMobilizedBody(body1x);
+ const MobilizedBody& body2 = matter.getMobilizedBody(body2x);
+
+ const Transform& X_GB1 = body1.getBodyTransform(state);
+ const Rotation& R_GB1 = X_GB1.R();
+
+ const Transform& X_GB2 = body2.getBodyTransform(state);
+ const Rotation& R_GB2 = X_GB2.R();
+
+ const Vec3& p_B1F = X_B1F.p();
+ const Vec3& p_B2M = X_B2M.p();
+
+ const Rotation& R_GF = pc.X_GF.R();
+ const Vec3& p_GF = pc.X_GF.p();
+
+ const Rotation& R_GM = pc.X_GM.R();
+ const Vec3& p_GM = pc.X_GM.p();
+
+ const Rotation& R_FM = pc.X_FM.R();
+ const Vec3& p_FM = pc.X_FM.p();
+
+ // Calculate stiffness generalized forces and potential
+ // energy (cheap to do here).
+ const Vec6& q = pc.q;
+ Vec6 fk; Real pe2=0;
+ for (int i=0; i<6; ++i)
+ pe2 += (fk[i]=k[i]*q[i])*q[i];
+ updPotentialEnergyCache(state) = pe2/2;
+ markPotentialEnergyValid(state);
+
+ ensureVelocityCacheValid(state);
+ const VelocityCache& vc = getVelocityCache(state);
+
+ const Vec6& qd = vc.qdot;
+ Vec6 fv; fc.power=0;
+ for (int i=0; i<6; ++i)
+ fc.power += (fv[i]=c[i]*qd[i])*qd[i];
+
+ fc.f = -(fk+fv); // generalized forces on body 2
+ const Vec3& fB2_q = fc.f.getSubVec<3>(0); // in q basis
+ const Vec3& fM_F = fc.f.getSubVec<3>(3); // acts at M, but exp. in F frame
+
+ // Calculate the matrix relating q-space generalized forces to a real-space
+ // moment vector. We know qforce = ~H * moment (where H is the
+ // the hinge matrix for a mobilizer using qdots as generalized speeds).
+ // In that case H would be N^-1, qforce = ~(N^-1)*moment so
+ // moment = ~N*qforce. Caution: our N wants the moment in the outboard
+ // body frame, in this case M.
+ const Mat33 N_FM = Rotation::calcNForBodyXYZInBodyFrame(q.getSubVec<3>(0));
+ const Vec3 mB2_M = ~N_FM * fB2_q; // moment acting on body 2, exp. in M
+ const Vec3 mB2_G = R_GM * mB2_M; // moment on body 2, now exp. in G
+
+ // Transform force from F frame to ground. This is the force to
+ // apply to body 2 at point OM; -f goes on body 1 at the same
+ // spatial location. Here we actually apply it at OF so we have to
+ // account for the moment produced by the shift from OM.
+ const Vec3 fM_G = R_GF*fM_F;
+
+ fc.F_GM = SpatialVec( mB2_G, fM_G);
+ fc.F_GF = SpatialVec(-(mB2_G + pc.p_FM_G % fM_G) , -fM_G);
+
+ // Shift forces to body origins.
+ fc.F_GB2 = SpatialVec(fc.F_GM[0] + pc.p_B2M_G % fc.F_GM[1], fc.F_GM[1]);
+ fc.F_GB1 = SpatialVec(fc.F_GF[0] + pc.p_B1F_G % fc.F_GF[1], fc.F_GF[1]);
+
+ markForceCacheValid(state);
+}
+
+// This calculate is only performed if the PE is requested without
+// already having calculated the force.
+void Force::LinearBushingImpl::
+ensurePotentialEnergyValid(const State& state) const {
+ if (isPotentialEnergyValid(state)) return;
+
+ const InstanceVars& iv = getInstanceVars(state);
+ const Vec6& k = iv.k;
+
+ ensurePositionCacheValid(state);
+ const PositionCache& pc = getPositionCache(state);
+ const Vec6& q = pc.q;
+
+ Real pe2=0;
+ for (int i=0; i<6; ++i)
+ pe2 += k[i]*q[i]*q[i];
+
+ updPotentialEnergyCache(state) = pe2 / 2;
+ markPotentialEnergyValid(state);
+}
+
+void Force::LinearBushingImpl::
+calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+{
+ ensureForceCacheValid(state);
+ const ForceCache& fc = getForceCache(state);
+ bodyForces[body2x] += fc.F_GB2;
+ bodyForces[body1x] += fc.F_GB1; // apply forces
+}
+
+// If the force was calculated, then the potential energy will already
+// be valid. Otherwise we'll have to calculate it.
+Real Force::LinearBushingImpl::
+calcPotentialEnergy(const State& state) const {
+ ensurePotentialEnergyValid(state);
+ return getPotentialEnergyCache(state);
+}
+
+
+} // namespace SimTK
+
diff --git a/Simbody/src/Force_Thermostat.cpp b/Simbody/src/Force_Thermostat.cpp
new file mode 100644
index 0000000..e4b6544
--- /dev/null
+++ b/Simbody/src/Force_Thermostat.cpp
@@ -0,0 +1,524 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Christopher Bruns *
+ * *
+ * 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/MobilizedBody.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "simbody/internal/Force_Thermostat.h"
+
+#include "ForceImpl.h"
+
+namespace SimTK {
+
+// Implementation class for Force::Thermostat.
+class Force::ThermostatImpl : public ForceImpl {
+public:
+ ThermostatImpl(const SimbodyMatterSubsystem& matter,
+ Real boltzmannsConstant,
+ Real defBathTemp,
+ Real defRelaxationTime,
+ int defNumExcludedDofs)
+ : matter(matter), kB(boltzmannsConstant),
+ defaultNumChains(DefaultDefaultNumChains),
+ defaultBathTemp(defBathTemp),
+ defaultRelaxationTime(defRelaxationTime),
+ defaultNumExcludedDofs(defNumExcludedDofs) {}
+
+ ThermostatImpl* clone() const {return new ThermostatImpl(*this);}
+ bool dependsOnlyOnPositions() const {return false;}
+
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const;
+
+ // Temperature does not contribute to potential energy.
+ Real calcPotentialEnergy(const State& state) const {return 0;}
+
+ void realizeTopology(State& state) const;
+ void realizeModel(State& state) const;
+ void realizeVelocity(const State& state) const;
+ void realizeDynamics(const State& state) const;
+
+ // Get/update the current number of chains.
+ int getNumChains(const State& s) const {
+ assert(dvNumChains.isValid());
+ return Value<int>::downcast(getForceSubsystem().getDiscreteVariable(s, dvNumChains));
+ }
+ int& updNumChains(State& s) const {
+ assert(dvNumChains.isValid());
+ return Value<int>::updDowncast(getForceSubsystem().updDiscreteVariable(s, dvNumChains));
+ }
+
+ // Get/update the current number of excluded dofs.
+ int getNumExcludedDofs(const State& s) const {
+ assert(dvNumExcludedDofs.isValid());
+ return Value<int>::downcast
+ (getForceSubsystem().getDiscreteVariable(s, dvNumExcludedDofs));
+ }
+ int& updNumExcludedDofs(State& s) const {
+ assert(dvNumExcludedDofs.isValid());
+ return Value<int>::updDowncast
+ (getForceSubsystem().updDiscreteVariable(s, dvNumExcludedDofs));
+ }
+
+ int getNumThermalDOFs(const State& s) const;
+
+ // Get the auxiliary continuous state index of the 0'th thermostat state variable z.
+ ZIndex getZ0Index(const State& s) const {
+ assert(cacheZ0Index.isValid());
+ return Value<ZIndex>::downcast(getForceSubsystem().getCacheEntry(s, cacheZ0Index));
+ }
+ ZIndex& updZ0Index(State& s) const {
+ assert(cacheZ0Index.isValid());
+ return Value<ZIndex>::updDowncast(getForceSubsystem().updCacheEntry(s, cacheZ0Index));
+ }
+
+ // Get the current bath temperature (i.e., the desired temperature).
+ Real getBathTemp(const State& s) const {
+ assert(dvBathTemp.isValid());
+ return Value<Real>::downcast(getForceSubsystem().getDiscreteVariable(s, dvBathTemp));
+ }
+ Real& updBathTemp(State& s) const {
+ assert(dvBathTemp.isValid());
+ return Value<Real>::updDowncast(getForceSubsystem().updDiscreteVariable(s, dvBathTemp));
+ }
+
+ // Get the current bath temperature (i.e., the desired temperature).
+ Real getRelaxationTime(const State& s) const {
+ assert(dvRelaxationTime.isValid());
+ return Value<Real>::downcast(getForceSubsystem().getDiscreteVariable(s, dvRelaxationTime));
+ }
+ Real& updRelaxationTime(State& s) const {
+ assert(dvRelaxationTime.isValid());
+ return Value<Real>::updDowncast(getForceSubsystem().updDiscreteVariable(s, dvRelaxationTime));
+ }
+
+ // Get the calculated momentum M*u (after Stage::Velocity).
+ const Vector& getMomentum(const State& s) const {
+ assert(cacheMomentumIndex.isValid());
+ return Value<Vector>::downcast(getForceSubsystem().getCacheEntry(s, cacheMomentumIndex));
+ }
+ Vector& updMomentum(const State& s) const {
+ assert(cacheMomentumIndex.isValid());
+ return Value<Vector>::updDowncast(getForceSubsystem().updCacheEntry(s, cacheMomentumIndex));
+ }
+
+ // Get the calculated system kinetic energy ~u*M*u/2 (after Stage::Velocity).
+ const Real& getKE(const State& s) const {
+ assert(cacheKEIndex.isValid());
+ return Value<Real>::downcast(getForceSubsystem().getCacheEntry(s, cacheKEIndex));
+ }
+ Real& updKE(const State& s) const {
+ assert(cacheKEIndex.isValid());
+ return Value<Real>::updDowncast(getForceSubsystem().updCacheEntry(s, cacheKEIndex));
+ }
+
+ Real getExternalWork(const State& s) const
+ { return getForceSubsystem().getZ(s)[workZIndex]; }
+ Real& updExternalWork(State& s) const
+ { return getForceSubsystem().updZ(s)[workZIndex]; }
+
+ Real& updWorkZDot(const State& s) const
+ { return getForceSubsystem().updZDot(s)[workZIndex]; }
+
+ Real calcExternalPower(const State& s) const;
+
+ // Get the current value of one of the thermostat state variables.
+ Real getZ(const State& s, int i) const {
+ assert(0 <= i && i < 2*getNumChains(s));
+ const ZIndex z0 = getZ0Index(s);
+ return getForceSubsystem().getZ(s)[z0+i];
+ }
+ Real& updZ(State& s, int i) const {
+ assert(0 <= i && i < 2*getNumChains(s));
+ const ZIndex z0 = getZ0Index(s);
+ return getForceSubsystem().updZ(s)[z0+i];
+ }
+ Real getZDot(const State& s, int i) const {
+ assert(0 <= i && i < 2*getNumChains(s));
+ const ZIndex z0 = getZ0Index(s);
+ return getForceSubsystem().getZDot(s)[z0+i];
+ }
+ // State is const here because ZDot is a cache entry.
+ Real& updZDot(const State& s, int i) const {
+ assert(0 <= i && i < 2*getNumChains(s));
+ const ZIndex z0 = getZ0Index(s);
+ return getForceSubsystem().updZDot(s)[z0+i];
+ }
+
+ static const int DefaultDefaultNumChains = 3;
+private:
+ const SimbodyMatterSubsystem& matter;
+ const Real kB; // Boltzmann's constant in compatible units
+
+ // Topology-stage "state" variables.
+ int defaultNumChains; // # chains in a new State
+ int defaultNumExcludedDofs; // # non-thermal rigid body dofs
+ Real defaultBathTemp; // bath temperature
+ Real defaultRelaxationTime; // relaxation time
+
+ // These indices are Topology-stage "cache" variables.
+ DiscreteVariableIndex dvNumChains; // integer
+ DiscreteVariableIndex dvNumExcludedDofs; // integer
+ DiscreteVariableIndex dvBathTemp; // Real
+ DiscreteVariableIndex dvRelaxationTime; // Real
+ CacheEntryIndex cacheZ0Index; // ZIndex
+ CacheEntryIndex cacheMomentumIndex; // M*u
+ CacheEntryIndex cacheKEIndex; // ~u*M*u/2
+ ZIndex workZIndex; // power integral
+
+friend class Force::Thermostat;
+};
+
+//-------------------------------- Thermostat ----------------------------------
+//------------------------------------------------------------------------------
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Force::Thermostat, Force::ThermostatImpl, Force);
+
+Force::Thermostat::Thermostat
+ (GeneralForceSubsystem& forces, const SimbodyMatterSubsystem& matter,
+ Real boltzmannsConstant, Real bathTemperature, Real relaxationTime,
+ int numExcludedDofs)
+: Force(new ThermostatImpl(matter, boltzmannsConstant, bathTemperature,
+ relaxationTime, numExcludedDofs))
+{
+ SimTK_APIARGCHECK1_ALWAYS(boltzmannsConstant > 0,
+ "Force::Thermostat","ctor", "Illegal Boltzmann's constant %g.", boltzmannsConstant);
+ SimTK_APIARGCHECK1_ALWAYS(bathTemperature > 0,
+ "Force::Thermostat","ctor", "Illegal bath temperature %g.", bathTemperature);
+ SimTK_APIARGCHECK1_ALWAYS(relaxationTime > 0,
+ "Force::Thermostat","ctor", "Illegal relaxation time %g.", relaxationTime);
+ SimTK_APIARGCHECK1_ALWAYS(0 <= numExcludedDofs && numExcludedDofs <= 6,
+ "Force::Thermostat","ctor",
+ "Illegal number of excluded rigid body dofs %d (must be 0-6).",
+ numExcludedDofs);
+
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+Force::Thermostat& Force::Thermostat::
+setDefaultNumChains(int numChains) {
+ SimTK_APIARGCHECK1_ALWAYS(numChains > 0,
+ "Force::Thermostat","setDefaultNumChains",
+ "Illegal number of chains %d.", numChains);
+
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultNumChains = numChains;
+ return *this;
+}
+
+Force::Thermostat& Force::Thermostat::
+setDefaultBathTemperature(Real bathTemperature) {
+ SimTK_APIARGCHECK1_ALWAYS(bathTemperature > 0,
+ "Force::Thermostat","setDefaultBathTemperature",
+ "Illegal bath temperature %g.", bathTemperature);
+
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultBathTemp = bathTemperature;
+ return *this;
+}
+
+Force::Thermostat& Force::Thermostat::
+setDefaultRelaxationTime(Real relaxationTime) {
+ SimTK_APIARGCHECK1_ALWAYS(relaxationTime > 0,
+ "Force::Thermostat","setDefaultRelaxationTime",
+ "Illegal bath temperature %g.", relaxationTime);
+
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultRelaxationTime = relaxationTime;
+ return *this;
+}
+
+Force::Thermostat& Force::Thermostat::
+setDefaultNumExcludedDofs(int numExcludedDofs) {
+ SimTK_APIARGCHECK1_ALWAYS(0 <= numExcludedDofs && numExcludedDofs <= 6,
+ "Force::Thermostat","setDefaultNumExcludedDofs",
+ "Illegal number of excluded rigid body dofs %d (must be 0-6).",
+ numExcludedDofs);
+
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultNumExcludedDofs = numExcludedDofs;
+ return *this;
+}
+
+int Force::Thermostat::getDefaultNumChains() const {return getImpl().defaultNumChains;}
+Real Force::Thermostat::getDefaultBathTemperature() const {return getImpl().defaultBathTemp;}
+Real Force::Thermostat::getDefaultRelaxationTime() const {return getImpl().defaultRelaxationTime;}
+int Force::Thermostat::getDefaultNumExcludedDofs() const {return getImpl().defaultNumExcludedDofs;}
+Real Force::Thermostat::getBoltzmannsConstant() const {return getImpl().kB;}
+
+const Force::Thermostat& Force::Thermostat::
+setNumChains(State& s, int numChains) const {
+ SimTK_APIARGCHECK1_ALWAYS(numChains > 0,
+ "Force::Thermostat","setNumChains",
+ "Illegal number of chains %d.", numChains);
+
+ getImpl().updNumChains(s) = numChains;
+ return *this;
+}
+
+const Force::Thermostat& Force::Thermostat::
+setBathTemperature(State& s, Real bathTemperature) const {
+ SimTK_APIARGCHECK1_ALWAYS(bathTemperature > 0,
+ "Force::Thermostat","setBathTemperature",
+ "Illegal bath temperature %g.", bathTemperature);
+
+ getImpl().updBathTemp(s) = bathTemperature;
+ return *this;
+}
+
+const Force::Thermostat& Force::Thermostat::
+setRelaxationTime(State& s, Real relaxationTime) const {
+ SimTK_APIARGCHECK1_ALWAYS(relaxationTime > 0,
+ "Force::Thermostat","setRelaxationTime",
+ "Illegal bath temperature %g.", relaxationTime);
+
+ getImpl().updRelaxationTime(s) = relaxationTime;
+ return *this;
+}
+
+const Force::Thermostat& Force::Thermostat::
+setNumExcludedDofs(State& s, int numExcludedDofs) const {
+ SimTK_APIARGCHECK1_ALWAYS(0 <= numExcludedDofs && numExcludedDofs <= 6,
+ "Force::Thermostat","setNumExcludedDofs",
+ "Illegal number of excluded rigid body dofs %d (must be 0-6).",
+ numExcludedDofs);
+
+ getImpl().updNumExcludedDofs(s) = numExcludedDofs;
+ return *this;
+}
+
+int Force::Thermostat::getNumChains(const State& s) const {return getImpl().getNumChains(s);}
+Real Force::Thermostat::getBathTemperature(const State& s) const {return getImpl().getBathTemp(s);}
+Real Force::Thermostat::getRelaxationTime(const State& s) const {return getImpl().getRelaxationTime(s);}
+int Force::Thermostat::getNumExcludedDofs(const State& s) const {return getImpl().getNumExcludedDofs(s);}
+
+void Force::Thermostat::initializeChainState(State& s) const {
+ const ThermostatImpl& impl = getImpl();
+ const int nChains = impl.getNumChains(s);
+ for (int i=0; i < 2*nChains; ++i)
+ impl.updZ(s, i) = 0;
+}
+
+void Force::Thermostat::setChainState(State& s, const Vector& z) const {
+ const ThermostatImpl& impl = getImpl();
+ const int nChains = impl.getNumChains(s);
+ SimTK_APIARGCHECK2_ALWAYS(z.size() == 2*nChains,
+ "Force::Thermostat", "setChainState",
+ "Number of values supplied (%d) didn't match twice the number of chains %d.",
+ z.size(), nChains);
+ for (int i=0; i < 2*nChains; ++i)
+ impl.updZ(s, i) = z[i];
+}
+
+Vector Force::Thermostat::getChainState(const State& s) const {
+ const ThermostatImpl& impl = getImpl();
+ const int nChains = impl.getNumChains(s);
+ Vector out(2*nChains);
+ for (int i=0; i < 2*nChains; ++i)
+ out[i] = impl.getZ(s, i);
+ return out;
+}
+
+// Cost is a divide and two flops, say about 20 flops.
+Real Force::Thermostat::getCurrentTemperature(const State& s) const {
+ const Real ke = getImpl().getKE(s); // Cached value for kinetic energy
+ const Real kB = getImpl().kB; // Boltzmann's constant
+ const int N = getImpl().getNumThermalDOFs(s);
+ return (2*ke) / (N*kB);
+}
+
+int Force::Thermostat::getNumThermalDofs(const State& s) const {
+ return getImpl().getNumThermalDOFs(s);
+}
+
+// Bath energy is KEb + PEb where
+// KEb = 1/2 kT t^2 (N z0^2 + sum(zi^2))
+// PEb = kT (N s0 + sum(si))
+// Cost is about 7 flops + 4 flops/chain, say about 25 flops.
+Real Force::Thermostat::calcBathEnergy(const State& state) const {
+ const ThermostatImpl& impl = getImpl();
+ const int nChains = impl.getNumChains(state);
+ const int N = impl.getNumThermalDOFs(state);
+ const Real kT = impl.kB * impl.getBathTemp(state);
+ const Real t = impl.getRelaxationTime(state);
+
+ Real zsqsum = N * square(impl.getZ(state,0));
+ for (int i=1; i < nChains; ++i)
+ zsqsum += square(impl.getZ(state,i));
+
+ Real ssum = N * impl.getZ(state, nChains);
+ for (int i=1; i < nChains; ++i)
+ ssum += impl.getZ(state, nChains+i);
+
+ const Real KEb = (kT/2) * t*t * zsqsum;
+ const Real PEb = kT * ssum;
+
+ return KEb + PEb;
+}
+
+Real Force::Thermostat::getExternalPower(const State& state) const {
+ return getImpl().calcExternalPower(state);
+}
+
+Real Force::Thermostat::getExternalWork(const State& state) const {
+ return getImpl().getExternalWork(state);
+}
+
+void Force::Thermostat::setExternalWork(State& state, Real w) const {
+ getImpl().updExternalWork(state) = w;
+}
+
+// This is the number of thermal dofs. TODO: we're ignoring constraint
+// redundancy but we shouldn't be! That could result in negative dofs, so we'll
+// make sure that doesn't happen. But don't expect meaningful results
+// in that case. Note that it is the acceleration-level constraints that
+// matter; they remove dofs regardless of whether there is a corresponding
+// velocity constraint.
+int Force::ThermostatImpl::getNumThermalDOFs(const State& state) const {
+ const int ndofs = state.getNU() - state.getNUDotErr()
+ - getNumExcludedDofs(state);
+ const int N = std::max(1, ndofs);
+ return N;
+}
+
+// This force produces only mobility forces, with
+// f = -z0 * M * u
+// Conveniently we already calculated the momentum M*u and cached it
+// in realizeVelocity(). Additional cost here is 2*N flops.
+void Force::ThermostatImpl::
+calcForce(const State& state, Vector_<SpatialVec>&, Vector_<Vec3>&,
+ Vector& mobilityForces) const
+{
+ const Vector& p = getMomentum(state);
+
+ // Generate momentum-weighted forces and apply to mobilities.
+ // This is 2*N flops.
+ mobilityForces -= getZ(state, 0)*p;
+}
+
+// All the power generated by this force is external (to or from the
+// thermal bath). So the power is
+// p = ~u * f = -z0 * (~u * M * u) = -z0 * (2*KE).
+// We already calculated and cached KE in realizeVelocity() so power
+// is practically free here (2 flops).
+Real Force::ThermostatImpl::
+calcExternalPower(const State& state) const {
+ return -2 * getZ(state, 0) * getKE(state);
+}
+
+// Allocate and initialize state variables.
+void Force::ThermostatImpl::realizeTopology(State& state) const {
+ // Make these writable just here where we need to fill in the Topology "cache"
+ // variables; after this they are const.
+ Force::ThermostatImpl* mutableThis = const_cast<Force::ThermostatImpl*>(this);
+ mutableThis->dvNumChains =
+ getForceSubsystem().allocateDiscreteVariable(state, Stage::Model,
+ new Value<int>(defaultNumChains));
+ mutableThis->dvBathTemp =
+ getForceSubsystem().allocateDiscreteVariable(state, Stage::Instance,
+ new Value<Real>(defaultBathTemp));
+ mutableThis->dvRelaxationTime =
+ getForceSubsystem().allocateDiscreteVariable(state, Stage::Instance,
+ new Value<Real>(defaultRelaxationTime));
+ mutableThis->dvNumExcludedDofs =
+ getForceSubsystem().allocateDiscreteVariable(state, Stage::Model,
+ new Value<int>(defaultNumExcludedDofs));
+
+ // This cache entry holds the auxiliary state index of our first
+ // thermostat state variable. It is valid after realizeModel().
+ mutableThis->cacheZ0Index =
+ getForceSubsystem().allocateCacheEntry(state, Stage::Model,
+ new Value<ZIndex>());
+
+ // This cache entry holds the generalized momentum M*u. The vector
+ // will be allocated to hold nu values.
+ mutableThis->cacheMomentumIndex =
+ getForceSubsystem().allocateCacheEntry(state, Stage::Velocity,
+ new Value<Vector>());
+
+ // This cache entry holds the kinetic energy ~u*M*u/2.
+ mutableThis->cacheKEIndex =
+ getForceSubsystem().allocateCacheEntry(state, Stage::Velocity,
+ new Value<Real>(NaN));
+
+ const Vector workZInit(1, Zero);
+ mutableThis->workZIndex =
+ getForceSubsystem().allocateZ(state, workZInit);
+}
+
+// Allocate the chain state variables and bath energy variables.
+// TODO: this should be done at Instance stage.
+void Force::ThermostatImpl::realizeModel(State& state) const {
+ const int nChains = getNumChains(state);
+ const Vector zInit(2*nChains, Zero);
+ updZ0Index(state) = getForceSubsystem().allocateZ(state, zInit);
+}
+
+// Calculate velocity-dependent terms, the internal coordinate
+// momentum and the kinetic energy. This is the expensive part
+// at about 125*N flops if all joints are 1 dof.
+void Force::ThermostatImpl::realizeVelocity(const State& state) const {
+ Vector& Mu = updMomentum(state);
+ matter.multiplyByM(state, state.getU(), Mu); // <-- expensive ~123*N flops
+ updKE(state) = (~state.getU() * Mu) / 2; // 2*N flops
+}
+
+// Calculate time derivatives of the various state variables.
+// This is just a fixed cost for the whole system, independent of
+// size: 3 divides + a few flops per chain, maybe 100 flops total.
+void Force::ThermostatImpl::realizeDynamics(const State& state) const {
+ const Real t = getRelaxationTime(state);
+ const Real oot2 = 1 / square(t);
+ const int m = getNumChains(state);
+
+ // This is the desired kinetic energy per dof.
+ const Real Eb = kB * getBathTemp(state) / 2;
+
+ const int N = getNumThermalDOFs(state);
+
+ // This is the current average kinetic energy per dof.
+ const Real E = getKE(state) / N;
+
+ updZDot(state, 0) = (E/Eb - 1) * oot2;
+
+ int Ndofs = N; // only for z0
+ for (int k=1; k < m; ++k) {
+ const Real zk1 = getZ(state, k-1);
+ const Real zk = getZ(state, k);
+ updZDot(state, k-1) -= zk1 * zk;
+ updZDot(state, k) = Ndofs * square(zk1) - oot2;
+ Ndofs = 1; // z1..m-1 control only 1 dof each
+ }
+
+ // Calculate sdot's for energy calculation.
+ for (int k=0; k < m; ++k)
+ updZDot(state, m+k) = getZ(state, k);
+
+ updWorkZDot(state) = calcExternalPower(state);
+}
+
+
+} // namespace SimTK
+
diff --git a/Simbody/src/GeneralContactSubsystem.cpp b/Simbody/src/GeneralContactSubsystem.cpp
new file mode 100644
index 0000000..0a01c42
--- /dev/null
+++ b/Simbody/src/GeneralContactSubsystem.cpp
@@ -0,0 +1,321 @@
+/* -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/GeneralContactSubsystem.h"
+#include "simbody/internal/MultibodySystem.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+
+#include <algorithm>
+
+namespace SimTK {
+
+// Useless, but required by Value<T>.
+std::ostream& operator<<(std::ostream& o, const Array_<Array_<Contact> >&) {
+ assert(false);
+ return o;
+}
+
+class ContactSet {
+public:
+ Array_<MobilizedBody,ContactSurfaceIndex> bodies;
+ Array_<ContactGeometry,ContactSurfaceIndex> geometry;
+ Array_<Transform,ContactSurfaceIndex> transforms;
+ mutable Array_<Vec3,ContactSurfaceIndex> sphereCenters;
+ mutable Array_<Real,ContactSurfaceIndex> sphereRadii;
+};
+
+class ContactBodyExtent {
+public:
+ ContactBodyExtent(Real start, Real end, ContactSurfaceIndex index)
+ : start(start), end(end), index(index) {}
+ ContactBodyExtent() {}
+ bool operator<(const ContactBodyExtent& e) const
+ { return start < e.start; }
+ Real start, end;
+ ContactSurfaceIndex index;
+};
+
+
+//==============================================================================
+// GENERAL CONTACT SUBSYSTEM IMPL
+//==============================================================================
+class GeneralContactSubsystemImpl : public Subsystem::Guts {
+public:
+ GeneralContactSubsystemImpl() {}
+
+ GeneralContactSubsystemImpl* cloneImpl() const {
+ return new GeneralContactSubsystemImpl(*this);
+ }
+
+ ContactSetIndex createContactSet() {
+ invalidateSubsystemTopologyCache();
+ int size = sets.size();
+ sets.resize(size+1);
+ return ContactSetIndex(size);
+ }
+
+ int getNumContactSets() const {
+ return sets.size();
+ }
+
+ void addBody(ContactSetIndex setIndex, const MobilizedBody& body, const ContactGeometry& geom, Transform& transform) {
+ assert(setIndex >= 0 && setIndex < sets.size());
+ invalidateSubsystemTopologyCache();
+ ContactSet& set = sets[setIndex];
+ set.bodies.push_back(body);
+ set.geometry.push_back(geom);
+ set.transforms.push_back(transform);
+ }
+
+ int getNumBodies(ContactSetIndex set) const {
+ return sets[set].bodies.size();
+ }
+
+ const MobilizedBody& getBody(ContactSetIndex set, ContactSurfaceIndex index) const {
+ assert(set >= 0 && set < sets.size());
+ assert(index >= 0 && index < (int) sets[set].bodies.size());
+ return sets[set].bodies[index];
+ }
+
+ const ContactGeometry& getBodyGeometry(ContactSetIndex set, ContactSurfaceIndex index) const {
+ assert(set >= 0 && set < sets.size());
+ assert(index >= 0 && index < (int) sets[set].geometry.size());
+ return sets[set].geometry[index];
+ }
+
+ ContactGeometry& updBodyGeometry(ContactSetIndex set, ContactSurfaceIndex index) {
+ assert(set >= 0 && set < sets.size());
+ assert(index >= 0 && index < (int) sets[set].geometry.size());
+ invalidateSubsystemTopologyCache();
+ return sets[set].geometry[index];
+ }
+
+ const Transform& getBodyTransform(ContactSetIndex set, ContactSurfaceIndex index) const {
+ assert(set >= 0 && set < sets.size());
+ assert(index >= 0 && index < (int) sets[set].transforms.size());
+ return sets[set].transforms[index];
+ }
+
+ Transform& updBodyTransform(ContactSetIndex set, ContactSurfaceIndex index) {
+ assert(set >= 0 && set < sets.size());
+ assert(index >= 0 && index < (int) sets[set].transforms.size());
+ invalidateSubsystemTopologyCache();
+ return sets[set].transforms[index];
+ }
+
+ const Array_<Contact>& getContacts(const State& state, ContactSetIndex set) const {
+ assert(set >= 0 && set < sets.size());
+ SimTK_STAGECHECK_GE_ALWAYS(state.getSubsystemStage(getMySubsystemIndex()), Stage::Dynamics, "GeneralContactSubsystemImpl::getContacts()");
+ Array_<Array_<Contact> >& contacts = Value<Array_<Array_<Contact> > >::downcast(updCacheEntry(state, contactsCacheIndex)).upd();
+ return contacts[set];
+ }
+
+ int realizeSubsystemTopologyImpl(State& state) const {
+ contactsCacheIndex = state.allocateCacheEntry(getMySubsystemIndex(), Stage::Dynamics, new Value<Array_<Array_<Contact> > >());
+ contactsValidCacheIndex = state.allocateCacheEntry(getMySubsystemIndex(), Stage::Position, new Value<bool>());
+ for (int i = 0; i < (int) sets.size(); ++i) {
+ const ContactSet& set = sets[i];
+ int numBodies = set.bodies.size();
+ set.sphereCenters.resize(numBodies);
+ set.sphereRadii.resize(numBodies);
+ for (ContactSurfaceIndex j(0); j < numBodies; j++) {
+ set.geometry[j].getBoundingSphere(set.sphereCenters[j], set.sphereRadii[j]);
+ set.sphereCenters[j] = set.transforms[j]*set.sphereCenters[j];
+ }
+ }
+ return 0;
+ }
+
+ int realizeSubsystemPositionImpl(const State& state) const {
+ Value<bool>::downcast(state.updCacheEntry(getMySubsystemIndex(), contactsValidCacheIndex)).upd() = false;
+ return 0;
+ }
+
+ int realizeSubsystemDynamicsImpl(const State& state) const {
+ bool& contactsValid = Value<bool>::downcast(state.updCacheEntry(getMySubsystemIndex(), contactsValidCacheIndex)).upd();
+ if (contactsValid)
+ return 0;
+ Array_<Array_<Contact> >& contacts = Value<Array_<Array_<Contact> > >::downcast(updCacheEntry(state, contactsCacheIndex)).upd();
+ int numSets = getNumContactSets();
+ contacts.resize(numSets);
+
+ // Loop over all contact sets.
+
+ for (int setIndex = 0; setIndex < numSets; setIndex++) {
+ contacts[setIndex].clear();
+ const ContactSet& set = sets[setIndex];
+ int numBodies = set.bodies.size();
+
+ // Perform a sweep-and-prune on a single axis to identify potential contacts. First, find which
+ // axis has the most variation in body locations. That is the axis we will use.
+
+ Vector_<Vec3> centers(numBodies);
+ for (ContactSurfaceIndex i(0); i < numBodies; i++)
+ centers[i] = set.bodies[i].getBodyTransform(state)*set.sphereCenters[i];
+ Vec3 average = mean(centers);
+ Vec3 var(0);
+ for (int i = 0; i < numBodies; i++)
+ var += abs(centers[i]-average);
+ int axis = (var[0] > var[1] ? 0 : 1);
+ if (var[2] > var[axis])
+ axis = 2;
+
+ // Find the extent of each body along the axis and sort them by starting location.
+
+ Array_<ContactBodyExtent> extents(numBodies);
+ for (ContactSurfaceIndex i(0); i < numBodies; i++)
+ extents[i] = ContactBodyExtent(centers[i][axis]-set.sphereRadii[i], centers[i][axis]+set.sphereRadii[i], i);
+ std::sort(extents.begin(), extents.end());
+
+ // Now sweep along the axis, finding potential contacts.
+
+ for (int i = 0; i < numBodies; i++) {
+ const ContactSurfaceIndex index1 = extents[i].index;
+ const Transform transform1 = set.bodies[index1].getBodyTransform(state)*set.transforms[index1];
+ const ContactGeometry& geom1 = set.geometry[index1];
+ const ContactGeometryTypeId typeId1 = geom1.getTypeId();
+ for (int j = i+1; j < numBodies && extents[j].start <= extents[i].end; j++) {
+ // They overlap along this axis. See if the bounding spheres overlap.
+
+ const ContactSurfaceIndex index2 = extents[j].index;
+ const Real sumRadius = set.sphereRadii[index1]+set.sphereRadii[index2];
+ if ((centers[index1]-centers[index2]).normSqr() <= sumRadius*sumRadius) {
+ // Do a full collision detection.
+
+ const Transform transform2 = set.bodies[index2].getBodyTransform(state)*set.transforms[index2];
+ const ContactGeometry& geom2 = set.geometry[index2];
+ const ContactGeometryTypeId typeId2 = geom2.getTypeId();
+ CollisionDetectionAlgorithm* algorithm =
+ CollisionDetectionAlgorithm::getAlgorithm
+ (typeId1, typeId2);
+ if (algorithm == NULL) {
+ algorithm = CollisionDetectionAlgorithm::
+ getAlgorithm(typeId2, typeId1);
+ if (algorithm == NULL)
+ continue; // No algorithm available for detecting collisions between these two objects.
+ algorithm->processObjects(index2, geom2, transform2,
+ index1, geom1, transform1,
+ contacts[setIndex]);
+ }
+ else {
+ algorithm->processObjects(index1, geom1, transform1,
+ index2, geom2, transform2,
+ contacts[setIndex]);
+ }
+ }
+ }
+ }
+ }
+ contactsValid = true;
+ return 0;
+ }
+
+ SimTK_DOWNCAST(GeneralContactSubsystemImpl, Subsystem::Guts);
+
+private:
+ Array_<ContactSet> sets;
+
+ mutable CacheEntryIndex contactsCacheIndex;
+ mutable CacheEntryIndex contactsValidCacheIndex;
+};
+
+
+
+//==============================================================================
+// GENERAL CONTACT SUBSYSTEM
+//==============================================================================
+ContactSetIndex GeneralContactSubsystem::createContactSet() {
+ return updImpl().createContactSet();
+}
+
+int GeneralContactSubsystem::getNumContactSets() const {
+ return getImpl().getNumContactSets();
+}
+
+void GeneralContactSubsystem::addBody(ContactSetIndex index, const MobilizedBody& body, const ContactGeometry& geom, Transform transform) {
+ return updImpl().addBody(index, body, geom, transform);
+}
+
+int GeneralContactSubsystem::getNumBodies(ContactSetIndex set) const {
+ return getImpl().getNumBodies(set);
+}
+
+const MobilizedBody& GeneralContactSubsystem::getBody(ContactSetIndex set, ContactSurfaceIndex index) const {
+ return getImpl().getBody(set, index);
+}
+
+const ContactGeometry& GeneralContactSubsystem::getBodyGeometry(ContactSetIndex set, ContactSurfaceIndex index) const {
+ return getImpl().getBodyGeometry(set, index);
+}
+
+ContactGeometry& GeneralContactSubsystem::updBodyGeometry(ContactSetIndex set, ContactSurfaceIndex index) {
+ return updImpl().updBodyGeometry(set, index);
+}
+
+const Transform& GeneralContactSubsystem::getBodyTransform(ContactSetIndex set, ContactSurfaceIndex index) const {
+ return getImpl().getBodyTransform(set, index);
+}
+
+Transform& GeneralContactSubsystem::updBodyTransform(ContactSetIndex set, ContactSurfaceIndex index) {
+ return updImpl().updBodyTransform(set, index);
+}
+
+const Array_<Contact>& GeneralContactSubsystem::getContacts(const State& state, ContactSetIndex set) const {
+ return getImpl().getContacts(state, set);
+}
+
+bool GeneralContactSubsystem::isInstanceOf(const Subsystem& s) {
+ return GeneralContactSubsystemImpl::isA(s.getSubsystemGuts());
+}
+const GeneralContactSubsystem& GeneralContactSubsystem::downcast(const Subsystem& s) {
+ assert(isInstanceOf(s));
+ return static_cast<const GeneralContactSubsystem&>(s);
+}
+GeneralContactSubsystem& GeneralContactSubsystem::updDowncast(Subsystem& s) {
+ assert(isInstanceOf(s));
+ return static_cast<GeneralContactSubsystem&>(s);
+}
+
+const GeneralContactSubsystemImpl& GeneralContactSubsystem::getImpl() const {
+ return SimTK_DYNAMIC_CAST_DEBUG<const GeneralContactSubsystemImpl&>(getSubsystemGuts());
+}
+GeneralContactSubsystemImpl& GeneralContactSubsystem::updImpl() {
+ return SimTK_DYNAMIC_CAST_DEBUG<GeneralContactSubsystemImpl&>(updSubsystemGuts());
+}
+
+// Create Subsystem but don't associate it with any System. This isn't much use except
+// for making std::Array_'s, which require a default constructor to be available.
+GeneralContactSubsystem::GeneralContactSubsystem() {
+ adoptSubsystemGuts(new GeneralContactSubsystemImpl());
+}
+
+GeneralContactSubsystem::GeneralContactSubsystem(MultibodySystem& mbs) {
+ adoptSubsystemGuts(new GeneralContactSubsystemImpl());
+ mbs.setContactSubsystem(*this); // steal ownership
+}
+
+} // namespace SimTK
+
diff --git a/Simbody/src/GeneralForceSubsystem.cpp b/Simbody/src/GeneralForceSubsystem.cpp
new file mode 100644
index 0000000..a974c6c
--- /dev/null
+++ b/Simbody/src/GeneralForceSubsystem.cpp
@@ -0,0 +1,427 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2006-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ *
+ * Private implementation of GeneralForceSubsystem.
+ */
+
+#include "SimTKcommon.h"
+
+#include "simbody/internal/common.h"
+#include "simbody/internal/ForceSubsystem.h"
+#include "simbody/internal/ForceSubsystemGuts.h"
+#include "simbody/internal/GeneralForceSubsystem.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "simbody/internal/MultibodySystem.h"
+
+#include "ForceImpl.h"
+
+
+namespace SimTK {
+
+// There is some tricky caching being done here for forces that have overridden
+// dependsOnlyOnPositions() (and returned "true"). This is probably only worth
+// doing for very expensive position-only forces like atomic force fields. We
+// try not to incur any overhead if there are no such forces in the System.
+// Note in particular that Force::Gravity does its own caching so doesn't make
+// use of this service.
+class GeneralForceSubsystemRep : public ForceSubsystem::Guts {
+public:
+ GeneralForceSubsystemRep()
+ : ForceSubsystemRep("GeneralForceSubsystem", "0.0.1")
+ {
+ }
+
+ ~GeneralForceSubsystemRep() {
+ // Delete in reverse order to be nice to heap system.
+ for (int i = (int)forces.size()-1; i >= 0; --i)
+ delete forces[i];
+ }
+
+ ForceIndex adoptForce(Force& force) {
+ invalidateSubsystemTopologyCache();
+ const ForceIndex index((int) forces.size());
+ forces.push_back(new Force()); // grow
+ Force& f = *forces.back(); // refer to the empty handle we just created
+ force.disown(f); // transfer ownership to f
+ return index;
+ }
+
+ int getNumForces() const {
+ return forces.size();
+ }
+
+ const Force& getForce(ForceIndex index) const {
+ assert(index >= 0 && index < forces.size());
+ return *forces[index];
+ }
+
+ Force& updForce(ForceIndex index) {
+ assert(index >= 0 && index < forces.size());
+ return *forces[index];
+ }
+
+ bool isForceDisabled(const State& state, ForceIndex index) const {
+ const Array_<bool>& forceEnabled = Value< Array_<bool> >::downcast
+ (getDiscreteVariable(state, forceEnabledIndex));
+ return !forceEnabled[index];
+ }
+
+ void setForceIsDisabled
+ (State& s, ForceIndex index, bool shouldDisable) const
+ {
+ Array_<bool>& forceEnabled = Value< Array_<bool> >::updDowncast
+ (updDiscreteVariable(s, forceEnabledIndex));
+
+ const bool shouldEnable = !shouldDisable; // sorry
+ if (forceEnabled[index] != shouldEnable) {
+ forceEnabled[index] = shouldEnable;
+
+ // If we're caching position-dependent forces, make sure they are
+ // marked invalid here.
+ if (cachedForcesAreValidCacheIndex.isValid()) {
+ Value<bool>::updDowncast
+ (updCacheEntry(s, cachedForcesAreValidCacheIndex)) = false;
+ }
+ }
+ }
+
+
+ // These override default implementations of virtual methods in the
+ // Subsystem::Guts class.
+
+ GeneralForceSubsystemRep* cloneImpl() const OVERRIDE_11
+ { return new GeneralForceSubsystemRep(*this); }
+
+ int realizeSubsystemTopologyImpl(State& s) const OVERRIDE_11 {
+ forceEnabledIndex.invalidate();
+ cachedForcesAreValidCacheIndex.invalidate();
+ rigidBodyForceCacheIndex.invalidate();
+ mobilityForceCacheIndex.invalidate();
+ particleForceCacheIndex.invalidate();
+
+ // Some forces are disabled by default; initialize the enabled flags
+ // accordingly. Also, see if we're going to need to do any caching
+ // on behalf of any forces that don't depend on velocities.
+ Array_<bool> forceEnabled(getNumForces());
+ bool someForceElementNeedsCaching = false;
+ for (int i = 0; i < (int)forces.size(); ++i) {
+ forceEnabled[i] = !(forces[i]->isDisabledByDefault());
+ if (!someForceElementNeedsCaching)
+ someForceElementNeedsCaching =
+ forces[i]->getImpl().dependsOnlyOnPositions();
+ }
+
+ forceEnabledIndex = allocateDiscreteVariable(s, Stage::Instance,
+ new Value<Array_<bool> >(forceEnabled));
+
+ // Note that we'll allocate these even if all the needs-caching
+ // elements are presently disabled. That way they'll be around when
+ // the force gets enabled.
+ if (someForceElementNeedsCaching) {
+ cachedForcesAreValidCacheIndex =
+ allocateCacheEntry(s, Stage::Position, new Value<bool>());
+ rigidBodyForceCacheIndex = allocateCacheEntry(s, Stage::Dynamics,
+ new Value<Vector_<SpatialVec> >());
+ mobilityForceCacheIndex = allocateCacheEntry(s, Stage::Dynamics,
+ new Value<Vector>());
+ particleForceCacheIndex = allocateCacheEntry(s, Stage::Dynamics,
+ new Value<Vector_<Vec3> >());
+ }
+
+ // We must realizeTopology() even if the force is disabled by default.
+ for (int i = 0; i < (int) forces.size(); ++i)
+ forces[i]->getImpl().realizeTopology(s);
+ return 0;
+ }
+
+ // Forces must realizeModel() even if they are currently disabled.
+ int realizeSubsystemModelImpl(State& s) const OVERRIDE_11 {
+ for (int i = 0; i < (int) forces.size(); ++i)
+ forces[i]->getImpl().realizeModel(s);
+ return 0;
+ }
+
+ // No need to realize Instance stage or later for force elements that are
+ // currently disabled.
+ int realizeSubsystemInstanceImpl(const State& s) const OVERRIDE_11 {
+ const Array_<bool>& enabled = Value<Array_<bool> >::downcast
+ (getDiscreteVariable(s, forceEnabledIndex));
+ for (int i = 0; i < (int) forces.size(); ++i)
+ if (enabled[i]) forces[i]->getImpl().realizeInstance(s);
+ return 0;
+ }
+
+ int realizeSubsystemTimeImpl(const State& s) const OVERRIDE_11 {
+ const Array_<bool>& enabled = Value<Array_<bool> >::downcast
+ (getDiscreteVariable(s, forceEnabledIndex));
+ for (int i = 0; i < (int) forces.size(); ++i)
+ if (enabled[i]) forces[i]->getImpl().realizeTime(s);
+ return 0;
+ }
+
+ int realizeSubsystemPositionImpl(const State& s) const OVERRIDE_11 {
+ const Array_<bool>& enabled = Value<Array_<bool> >::downcast
+ (getDiscreteVariable(s, forceEnabledIndex));
+ // If we're caching position-dependent forces, make sure they are
+ // marked invalid here.
+ if (cachedForcesAreValidCacheIndex.isValid()) {
+ Value<bool>::updDowncast
+ (updCacheEntry(s, cachedForcesAreValidCacheIndex)) = false;
+ }
+ for (int i = 0; i < (int) forces.size(); ++i)
+ if (enabled[i]) forces[i]->getImpl().realizePosition(s);
+ return 0;
+ }
+
+ int realizeSubsystemVelocityImpl(const State& s) const OVERRIDE_11 {
+ const Array_<bool>& enabled = Value<Array_<bool> >::downcast
+ (getDiscreteVariable(s, forceEnabledIndex));
+ for (int i = 0; i < (int) forces.size(); ++i)
+ if (enabled[i]) forces[i]->getImpl().realizeVelocity(s);
+ return 0;
+ }
+
+ int realizeSubsystemDynamicsImpl(const State& s) const OVERRIDE_11 {
+ const MultibodySystem& mbs = getMultibodySystem(); // my owner
+ const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
+
+ // Get access to the discrete state variable that records whether each
+ // force element is enabled.
+ const Array_<bool>& forceEnabled = Value< Array_<bool> >::downcast
+ (getDiscreteVariable(s, forceEnabledIndex));
+
+ // Get access to System-global force cache arrays.
+ Vector_<SpatialVec>& rigidBodyForces =
+ mbs.updRigidBodyForces(s, Stage::Dynamics);
+ Vector_<Vec3>& particleForces =
+ mbs.updParticleForces (s, Stage::Dynamics);
+ Vector& mobilityForces =
+ mbs.updMobilityForces (s, Stage::Dynamics);
+
+ // Short circuit if we're not doing any caching here. Note that we're
+ // checking whether the *index* is valid (i.e. does the cache entry
+ // exist?), not the contents.
+ if (!cachedForcesAreValidCacheIndex.isValid()) {
+ for (int i = 0; i < (int)forces.size(); ++i) {
+ if (forceEnabled[i])
+ forces[i]->getImpl().calcForce
+ (s, rigidBodyForces, particleForces, mobilityForces);
+ }
+
+ // Allow forces to do their own realization, but wait until all
+ // forces have executed calcForce(). TODO: not sure if that is
+ // necessary (sherm 20130716).
+ for (int i = 0; i < (int)forces.size(); ++i)
+ if (forceEnabled[i])
+ forces[i]->getImpl().realizeDynamics(s);
+ return 0;
+ }
+
+ // OK, we're doing some caching. This is a little messier.
+
+ // Get access to subsystem force cache entries.
+ bool& cachedForcesAreValid = Value<bool>::downcast
+ (updCacheEntry(s, cachedForcesAreValidCacheIndex));
+
+ Vector_<SpatialVec>&
+ rigidBodyForceCache = Value<Vector_<SpatialVec> >::downcast
+ (updCacheEntry(s, rigidBodyForceCacheIndex));
+ Vector_<Vec3>&
+ particleForceCache = Value<Vector_<Vec3> >::downcast
+ (updCacheEntry(s, particleForceCacheIndex));
+ Vector&
+ mobilityForceCache = Value<Vector>::downcast
+ (updCacheEntry(s, mobilityForceCacheIndex));
+
+
+ if (!cachedForcesAreValid) {
+ // We need to calculate the velocity independent forces.
+ rigidBodyForceCache.resize(matter.getNumBodies());
+ rigidBodyForceCache = SpatialVec(Vec3(0), Vec3(0));
+ particleForceCache.resize(matter.getNumParticles());
+ particleForceCache = Vec3(0);
+ mobilityForceCache.resize(matter.getNumMobilities());
+ mobilityForceCache = 0;
+
+ // Run through all the forces, accumulating directly into the
+ // force arrays or indirectly into the cache as appropriate.
+ for (int i = 0; i < (int) forces.size(); ++i) {
+ if (!forceEnabled[i]) continue;
+ const ForceImpl& impl = forces[i]->getImpl();
+ if (impl.dependsOnlyOnPositions())
+ impl.calcForce(s, rigidBodyForceCache, particleForceCache,
+ mobilityForceCache);
+ else // ordinary velocity dependent force
+ impl.calcForce(s, rigidBodyForces, particleForces,
+ mobilityForces);
+ }
+ cachedForcesAreValid = true;
+ } else {
+ // Cache already valid; just need to do the non-cached ones.
+ for (int i = 0; i < (int) forces.size(); ++i) {
+ if (!forceEnabled[i]) continue;
+ const ForceImpl& impl = forces[i]->getImpl();
+ if (!impl.dependsOnlyOnPositions())
+ impl.calcForce(s, rigidBodyForces, particleForces,
+ mobilityForces);
+ }
+ }
+
+ // Accumulate the values from the cache into the global arrays.
+ rigidBodyForces += rigidBodyForceCache;
+ particleForces += particleForceCache;
+ mobilityForces += mobilityForceCache;
+
+ // Allow forces to do their own Dynamics-stage realization. Note that
+ // this *follows* all the calcForce() calls.
+ for (int i = 0; i < (int) forces.size(); ++i)
+ if (forceEnabled[i]) forces[i]->getImpl().realizeDynamics(s);
+ return 0;
+ }
+
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11 {
+ const Array_<bool>& forceEnabled = Value<Array_<bool> >::downcast
+ (getDiscreteVariable(state, forceEnabledIndex)).get();
+ Real energy = 0;
+ for (int i = 0; i < (int) forces.size(); ++i) {
+ if (forceEnabled[i]) {
+ const Force& f = *forces[i];
+ energy += f.getImpl().calcPotentialEnergy(state);
+ }
+ }
+ return energy;
+ }
+
+ int realizeSubsystemAccelerationImpl(const State& s) const OVERRIDE_11 {
+ const Array_<bool>& enabled = Value<Array_<bool> >::downcast
+ (getDiscreteVariable(s, forceEnabledIndex));
+ for (int i = 0; i < (int) forces.size(); ++i)
+ if (enabled[i]) forces[i]->getImpl().realizeAcceleration(s);
+ return 0;
+ }
+
+ int realizeSubsystemReportImpl(const State& s) const OVERRIDE_11 {
+ const Array_<bool>& enabled = Value<Array_<bool> >::downcast
+ (getDiscreteVariable(s, forceEnabledIndex));
+ for (int i = 0; i < (int) forces.size(); ++i)
+ if (enabled[i]) forces[i]->getImpl().realizeReport(s);
+ return 0;
+ }
+
+ int calcDecorativeGeometryAndAppendImpl
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+ OVERRIDE_11
+ {
+ const Array_<bool>& enabled = Value<Array_<bool> >::downcast
+ (getDiscreteVariable(s, forceEnabledIndex));
+ for (int i = 0; i < (int) forces.size(); ++i)
+ if (enabled[i])
+ forces[i]->getImpl().calcDecorativeGeometryAndAppend(s,stage,geom);
+ return 0;
+ }
+
+private:
+ Array_<Force*> forces;
+
+ // TOPOLOGY "CACHE"
+ // These indices must be filled in during realizeTopology and treated
+ // as const thereafter.
+
+ // This instance-stage variable holds a bool for each force element.
+ mutable DiscreteVariableIndex forceEnabledIndex;
+
+ // This set of cache entries is allocated only if some force element
+ // overrode dependsOnlyOnPositions().
+ mutable CacheEntryIndex cachedForcesAreValidCacheIndex;
+ mutable CacheEntryIndex rigidBodyForceCacheIndex;
+ mutable CacheEntryIndex mobilityForceCacheIndex;
+ mutable CacheEntryIndex particleForceCacheIndex;
+};
+
+ ///////////////////////////
+ // GeneralForceSubsystem //
+ ///////////////////////////
+
+
+/*static*/ bool
+GeneralForceSubsystem::isInstanceOf(const ForceSubsystem& s) {
+ return GeneralForceSubsystemRep::isA(s.getRep());
+}
+/*static*/ const GeneralForceSubsystem&
+GeneralForceSubsystem::downcast(const ForceSubsystem& s) {
+ assert(isInstanceOf(s));
+ return static_cast<const GeneralForceSubsystem&>(s);
+}
+/*static*/ GeneralForceSubsystem&
+GeneralForceSubsystem::updDowncast(ForceSubsystem& s) {
+ assert(isInstanceOf(s));
+ return static_cast<GeneralForceSubsystem&>(s);
+}
+
+const GeneralForceSubsystemRep&
+GeneralForceSubsystem::getRep() const {
+ return SimTK_DYNAMIC_CAST_DEBUG<const GeneralForceSubsystemRep&>(ForceSubsystem::getRep());
+}
+GeneralForceSubsystemRep&
+GeneralForceSubsystem::updRep() {
+ return SimTK_DYNAMIC_CAST_DEBUG<GeneralForceSubsystemRep&>(ForceSubsystem::updRep());
+}
+
+// Create Subsystem but don't associate it with any System. This isn't much use except
+// for making std::vector's, which require a default constructor to be available.
+GeneralForceSubsystem::GeneralForceSubsystem()
+: ForceSubsystem()
+{ adoptSubsystemGuts(new GeneralForceSubsystemRep()); }
+
+GeneralForceSubsystem::GeneralForceSubsystem(MultibodySystem& mbs)
+: ForceSubsystem()
+{ adoptSubsystemGuts(new GeneralForceSubsystemRep());
+ mbs.addForceSubsystem(*this); } // steal ownership
+
+ForceIndex GeneralForceSubsystem::adoptForce(Force& force)
+{ return updRep().adoptForce(force); }
+
+int GeneralForceSubsystem::getNumForces() const
+{ return getRep().getNumForces(); }
+
+const Force& GeneralForceSubsystem::getForce(ForceIndex index) const
+{ return getRep().getForce(index); }
+
+Force& GeneralForceSubsystem::updForce(ForceIndex index)
+{ return updRep().updForce(index); }
+
+bool GeneralForceSubsystem::isForceDisabled
+ (const State& state, ForceIndex index) const
+{ return getRep().isForceDisabled(state, index); }
+
+void GeneralForceSubsystem::setForceIsDisabled
+ (State& state, ForceIndex index, bool disabled) const
+{ getRep().setForceIsDisabled(state, index, disabled); }
+
+const MultibodySystem& GeneralForceSubsystem::getMultibodySystem() const
+{ return MultibodySystem::downcast(getSystem()); }
+
+} // namespace SimTK
+
diff --git a/Simbody/src/HuntCrossleyContact.cpp b/Simbody/src/HuntCrossleyContact.cpp
new file mode 100644
index 0000000..2c5822c
--- /dev/null
+++ b/Simbody/src/HuntCrossleyContact.cpp
@@ -0,0 +1,478 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ *
+ * Private implementation of HuntCrossleyContact Subsystem.
+ */
+
+#include "SimTKcommon.h"
+#include "simbody/internal/common.h"
+#include "simbody/internal/MultibodySystem.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+
+#include "simbody/internal/ForceSubsystem.h"
+#include "simbody/internal/HuntCrossleyContact.h"
+
+#include "simbody/internal/ForceSubsystemGuts.h"
+
+namespace SimTK {
+
+class HuntCrossleyContactRep : public ForceSubsystemRep {
+
+ // Private class definitions for state entries.
+ // Note that we take the 2/3 power of the plane-strain modulus
+ // and store that as "stiffness" here; see comments on the
+ // client-side class for why.
+
+ struct SphereParameters {
+ SphereParameters() {
+ center.setToNaN();
+ radius = stiffness = dissipation = CNT<Real>::getNaN();
+ }
+
+ SphereParameters(MobilizedBodyIndex b, const Vec3& ctr,
+ const Real& r, const Real& k, const Real& c)
+ : body(b), center(ctr), radius(r), stiffness(std::pow(k,Real(2./3.))), dissipation(c) {
+ assert(b.isValid());
+ assert(radius > 0 && stiffness >= 0 && dissipation >= 0);
+ }
+
+ MobilizedBodyIndex body;
+ Vec3 center; // in body frame
+ Real radius, stiffness, dissipation; // r,k,c from H&C
+ };
+
+ struct HalfspaceParameters {
+ HalfspaceParameters() {
+ height = stiffness = dissipation = CNT<Real>::getNaN();
+ }
+
+ HalfspaceParameters(MobilizedBodyIndex b, const UnitVec3& n,
+ const Real& h, const Real& k, const Real& c)
+ : body(b), normal(n), height(h), stiffness(std::pow(k,Real(2./3.))), dissipation(c) {
+ assert(b.isValid());
+ assert(stiffness >= 0 && dissipation >= 0);
+ }
+
+ MobilizedBodyIndex body;
+ UnitVec3 normal; // in body frame
+ Real height, stiffness, dissipation;
+ };
+
+ struct Parameters {
+ Parameters() : enabled(true) { }
+ bool enabled;
+ Array_<SphereParameters> spheres;
+ Array_<HalfspaceParameters> halfSpaces;
+ };
+
+
+public:
+ HuntCrossleyContactRep()
+ : ForceSubsystemRep("HuntCrossleyContact", "0.0.1")
+ {
+ }
+ int addSphere(MobilizedBodyIndex body, const Vec3& center,
+ const Real& radius,
+ const Real& stiffness,
+ const Real& dissipation)
+ {
+ assert(body.isValid() && radius > 0 && stiffness >= 0 && dissipation >= 0);
+
+ invalidateSubsystemTopologyCache(); // this is a topological change
+
+ defaultParameters.spheres.push_back(
+ SphereParameters(body,center,radius,stiffness,dissipation));
+ return (int)defaultParameters.spheres.size() - 1;
+ }
+
+ int addHalfSpace(MobilizedBodyIndex body, const UnitVec3& normal,
+ const Real& height,
+ const Real& stiffness,
+ const Real& dissipation)
+ {
+ assert(body.isValid() && stiffness >= 0 && dissipation >= 0);
+
+ invalidateSubsystemTopologyCache(); // this is a topological change
+
+ defaultParameters.halfSpaces.push_back(
+ HalfspaceParameters(body,normal,height,stiffness,dissipation));
+ return (int)defaultParameters.halfSpaces.size() - 1;
+ }
+
+ // OVERRIDE VIRTUAL FUNCTIONS FROM Subsystem::Guts
+
+ HuntCrossleyContactRep* cloneImpl() const {return new HuntCrossleyContactRep(*this);}
+
+
+ int realizeSubsystemTopologyImpl(State& s) const {
+ instanceVarsIndex = s.allocateDiscreteVariable(getMySubsystemIndex(), Stage::Instance,
+ new Value<Parameters>(defaultParameters));
+ energyCacheIndex = s.allocateCacheEntry(getMySubsystemIndex(), Stage::Dynamics, new Value<Real>());
+ return 0;
+ }
+
+ int realizeSubsystemModelImpl(State& s) const {
+ // Sorry, no choices available at the moment.
+ return 0;
+ }
+
+ int realizeSubsystemInstanceImpl(const State& s) const {
+ // Nothing to compute here.
+ return 0;
+ }
+
+ int realizeSubsystemTimeImpl(const State& s) const {
+ // Nothing to compute here.
+ return 0;
+ }
+
+ int realizeSubsystemPositionImpl(const State& s) const {
+ // Nothing to compute here.
+ return 0;
+ }
+
+ int realizeSubsystemVelocityImpl(const State& s) const {
+ // Nothing to compute here.
+ return 0;
+ }
+
+ // Cost of contact processing here (in flops):
+ // 30*(ns*ns)/2 + 7*(ns*nhg) + 28*(ns*nhm) -- to determine whether there is contact
+ // + 156 * (number of actual contacts)
+ // where ns==# spheres, nhg==# half spaces on ground, nhm==#half spaces on moving bodies.
+ // It doesn't take many objects before that first term is very expensive.
+ // TODO: contact test can be made O(n) by calculating neighborhoods, e.g.
+
+ int realizeSubsystemDynamicsImpl(const State& s) const;
+
+ int realizeSubsystemAccelerationImpl(const State& s) const {
+ // Nothing to compute here.
+ return 0;
+ }
+
+ int realizeSubsystemReportImpl(const State& s) const {
+ // Nothing to compute here.
+ return 0;
+ }
+
+ Real calcPotentialEnergy(const State& state) const;
+
+private:
+ // TOPOLOGY "STATE" VARIABLES
+
+ Parameters defaultParameters;
+
+ // TOPOLOGY "CACHE" VARIABLES
+
+ // This must be filled in during realizeTopology and treated
+ // as const thereafter. These are garbage unless built=true.
+ mutable DiscreteVariableIndex instanceVarsIndex;
+ mutable CacheEntryIndex energyCacheIndex;
+
+ const Parameters& getParameters(const State& s) const {
+ assert(subsystemTopologyHasBeenRealized());
+ return Value<Parameters>::downcast(
+ getDiscreteVariable(s,instanceVarsIndex)).get();
+ }
+ Parameters& updParameters(State& s) const {
+ assert(subsystemTopologyHasBeenRealized());
+ return Value<Parameters>::downcast(
+ updDiscreteVariable(s,instanceVarsIndex)).upd();
+ }
+
+ // We have determined that contact is occurring. The *undeformed* contact points and the
+ // contact normal along which they lie have been determined. This method calculates and
+ // applies the force to each body at the *deformed* contact point, accounting for the
+ // different material properties. (cost ~144 flops)
+ void processContact(const Real& R, // relative radius of curvature
+ const Real& k1, const Real& c1, const Transform& X_GB1, const SpatialVec& V_GB1,
+ const Real& k2, const Real& c2, const Transform& X_GB2, const SpatialVec& V_GB2,
+ const Vec3& undefContactPt1_G, const Vec3& undefContactPt2_G,
+ const UnitVec3& contactNormal_G, // points from body2 to body1
+ Real& pe, SpatialVec& force1, SpatialVec& force2) const;
+
+ friend std::ostream& operator<<(std::ostream& o,
+ const HuntCrossleyContactRep::Parameters&);
+};
+// Useless, but required by Value<T>.
+std::ostream& operator<<(std::ostream& o,
+ const HuntCrossleyContactRep::Parameters&)
+{assert(false);return o;}
+
+
+
+ /////////////////////////
+ // HuntCrossleyContact //
+ /////////////////////////
+
+/*static*/ bool
+HuntCrossleyContact::isInstanceOf(const ForceSubsystem& s) {
+ return HuntCrossleyContactRep::isA(s.getRep());
+}
+/*static*/ const HuntCrossleyContact&
+HuntCrossleyContact::downcast(const ForceSubsystem& s) {
+ assert(isInstanceOf(s));
+ return static_cast<const HuntCrossleyContact&>(s);
+}
+/*static*/ HuntCrossleyContact&
+HuntCrossleyContact::updDowncast(ForceSubsystem& s) {
+ assert(isInstanceOf(s));
+ return static_cast<HuntCrossleyContact&>(s);
+}
+
+const HuntCrossleyContactRep&
+HuntCrossleyContact::getRep() const {
+ return SimTK_DYNAMIC_CAST_DEBUG<const HuntCrossleyContactRep&>(ForceSubsystem::getRep());
+}
+HuntCrossleyContactRep&
+HuntCrossleyContact::updRep() {
+ return SimTK_DYNAMIC_CAST_DEBUG<HuntCrossleyContactRep&>(ForceSubsystem::updRep());
+}
+
+// Create Subsystem but don't associate it with any System. This isn't much use except
+// for making std::vector's, which require a default constructor to be available.
+HuntCrossleyContact::HuntCrossleyContact()
+ : ForceSubsystem()
+{
+ adoptSubsystemGuts(new HuntCrossleyContactRep());
+}
+
+HuntCrossleyContact::HuntCrossleyContact(MultibodySystem& mbs)
+ : ForceSubsystem()
+{
+ adoptSubsystemGuts(new HuntCrossleyContactRep());
+ mbs.addForceSubsystem(*this); // steal ownership
+}
+
+int HuntCrossleyContact::addSphere(MobilizedBodyIndex body, const Vec3& center,
+ const Real& radius,
+ const Real& stiffness,
+ const Real& dissipation)
+{
+ return updRep().addSphere(body,center,radius,stiffness,dissipation);
+}
+
+int HuntCrossleyContact::addHalfSpace(MobilizedBodyIndex body, const UnitVec3& normal,
+ const Real& height,
+ const Real& stiffness,
+ const Real& dissipation)
+{
+ return updRep().addHalfSpace(body,normal,height,stiffness,dissipation);
+}
+
+
+
+ ////////////////////////////
+ // HuntCrossleyContactRep //
+ ////////////////////////////
+
+// Cost of contact processing here (in flops):
+// 30*(ns*ns)/2 + 7*(ns*nhg) + 28*(ns*nhm) -- to determine whether there is contact
+// + 210 * (#sphere/sphere contacts) + 156 * (#sphere/halfspace contacts)
+// where ns==# spheres, nhg==# half spaces on ground, nhm==#half spaces on moving bodies.
+// It doesn't take many spheres before that first term is very expensive.
+// TODO: contact test can be made O(n) by calculating neighborhoods, e.g.
+
+int HuntCrossleyContactRep::realizeSubsystemDynamicsImpl(const State& s) const
+{
+ const Parameters& p = getParameters(s);
+ if (!p.enabled) return 0;
+
+ const MultibodySystem& mbs = getMultibodySystem(); // my owner
+ const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
+ Real& pe = Value<Real>::downcast(s.updCacheEntry(getMySubsystemIndex(), energyCacheIndex)).upd();
+ pe = 0;
+
+ // Get access to system-global cache entries.
+ Vector_<SpatialVec>& rigidBodyForces = mbs.updRigidBodyForces(s, Stage::Dynamics);
+
+ for (int s1=0; s1 < (int)p.spheres.size(); ++s1) {
+ const SphereParameters& sphere1 = p.spheres[s1];
+ const Transform& X_GB1 = matter.getMobilizedBody(sphere1.body).getBodyTransform(s);
+ const SpatialVec& V_GB1 = matter.getMobilizedBody(sphere1.body).getBodyVelocity(s); // in G
+ const Real r1 = sphere1.radius;
+ const Vec3 center1_G = X_GB1*sphere1.center;
+
+ for (int s2=s1+1; s2 < (int)p.spheres.size(); ++s2) {
+ const SphereParameters& sphere2 = p.spheres[s2];
+ if (sphere2.body == sphere1.body) continue;
+ const Transform& X_GB2 = matter.getMobilizedBody(sphere2.body).getBodyTransform(s);
+ const SpatialVec& V_GB2 = matter.getMobilizedBody(sphere2.body).getBodyVelocity(s);
+ const Real r2 = sphere2.radius;
+ const Vec3 center2_G = X_GB2*sphere2.center; // 18 flops
+
+ const Vec3 c2c1_G = center1_G - center2_G; // points at sphere1 (3 flops)
+ const Real dsq = c2c1_G.normSqr(); // 5 flops
+ if (dsq >= (r1+r2)*(r1+r2)) continue; // 4 flops
+
+ // There is a collision
+ const Real d = std::sqrt(dsq); // distance between the centers (~30 flops)
+ const UnitVec3 normal_G(c2c1_G/d, true); // direction from c2 center to c1 center (~12 flops)
+
+ processContact((r1*r2)/(r1+r2), // relative curvature, ~12 flops
+ sphere1.stiffness, sphere1.dissipation, X_GB1, V_GB1,
+ sphere2.stiffness, sphere2.dissipation, X_GB2, V_GB2,
+ center1_G - r1 * normal_G, // undeformed contact point for sphere1 (6 flops)
+ center2_G + r2 * normal_G, // undeformed contact point for sphere2 (6 flops)
+ normal_G,
+ pe, rigidBodyForces[sphere1.body], rigidBodyForces[sphere2.body]);
+ }
+
+ // half spaces
+ for (int h=0; h < (int)p.halfSpaces.size(); ++h) {
+ const HalfspaceParameters& halfSpace = p.halfSpaces[h];
+ if (halfSpace.body == sphere1.body) continue;
+
+ // Quick escape in the common case where the half space is on ground (7 flops)
+ if (halfSpace.body==0) {
+ const UnitVec3& normal_G = halfSpace.normal;
+ const Real hc1_G = ~center1_G*normal_G; // ht of center over ground
+ if (hc1_G - halfSpace.height < r1) {
+ // Collision of sphere1 with a half space on ground.
+ processContact(r1, // relative curvature is just r1
+ sphere1.stiffness, sphere1.dissipation, X_GB1, V_GB1,
+ halfSpace.stiffness, halfSpace.dissipation, Transform(), SpatialVec(Vec3(0)),
+ center1_G - (r1*normal_G), // undeformed contact point for sphere (6 flops)
+ halfSpace.height*normal_G, // " for halfSpace (3 flops)
+ normal_G, pe, rigidBodyForces[sphere1.body], rigidBodyForces[halfSpace.body]);
+ }
+ continue;
+ }
+
+ // Half space is not on ground.
+
+ const Transform& X_GB2 = matter.getMobilizedBody(halfSpace.body).getBodyTransform(s);
+ const SpatialVec& V_GB2 = matter.getMobilizedBody(halfSpace.body).getBodyVelocity(s);
+ const UnitVec3 normal_G = X_GB2.R()*halfSpace.normal; // 15 flops
+
+ // Find the heights of the half space surface and sphere center measured
+ // along the contact normal from the ground origin. Then we can get the
+ // height of the sphere center over the half space surface.
+ const Real h_G = ~X_GB2.p()*normal_G + halfSpace.height; // 6 flops
+ const Real hc1_G = ~center1_G*normal_G; // 5 flops
+ const Real d = hc1_G - h_G; // 1 flop
+ if (d >= r1) continue; // 1 flop
+
+ // There is a collision
+ processContact(r1, // relative curvature is just r1
+ sphere1.stiffness, sphere1.dissipation, X_GB1, V_GB1,
+ halfSpace.stiffness, halfSpace.dissipation, X_GB2, V_GB2,
+ center1_G - r1 * normal_G, // undeformed contact point for sphere (6 flops)
+ center1_G - d * normal_G, // undeformed contact point for halfSpace (6 flops)
+ normal_G,
+ pe, rigidBodyForces[sphere1.body], rigidBodyForces[halfSpace.body]);
+ }
+ }
+
+ return 0;
+}
+
+Real HuntCrossleyContactRep::calcPotentialEnergy(const State& state) const {
+ return Value<Real>::downcast(state.getCacheEntry(getMySubsystemIndex(), energyCacheIndex)).get();
+}
+
+// We have determined that contact is occurring. The *undeformed* contact points and the
+// contact normal along which they lie have been determined. This method calculates and
+// applies the force to each body at the *deformed* contact point, accounting for the
+// different material properties. (cost ~144 flops)
+//
+// Note: k1 = E1^(2/3), E1 is plane-strain modulus of material 1, etc.
+// R = (r1*r2)/(r1+r2) (sphere-sphere)
+// R = r1 (sphere-halfspace)
+// We calculate:
+// s1 = k2/(k1+k2); s2 = 1-s1
+// k = k1*s1
+// E = k^(3/2)
+// c = c1*s1 + c2*s2
+// We need to calculate Hertz force fH
+// fH = 4/3 sqrt(R) E x^(3/2)
+// We can do this all with one square root like this:
+// fH = 4/3 sqrt(R) k sqrt(k) x sqrt(x)
+// = 4/3 k x sqrt(R*k*x)
+// Then the complete Hunt & Crossley force f is
+// fH(1 + 3/2 c v) where v = xdot.
+// We also want potential energy
+// pe = 2/5 * (4/3 sqrt(R) E)*x^(5/2) = 2/5 fH x
+// TODO: If we want the patch radius a also (not currently needed) it is
+// a = sqrt(R*x)
+// That would take another square root as coded but could be avoided
+// by precalculating the combined material properties.
+//
+// Note that we don't apply the force or count potential energy if the
+// calculated value is negative, meaning the bodies would be "sticking".
+// That situation can only occur because an outside force is yanking
+// the bodies apart.
+
+void HuntCrossleyContactRep::processContact
+ (const Real& R,
+ const Real& k1, const Real& c1, const Transform& X_GB1, const SpatialVec& V_GB1,
+ const Real& k2, const Real& c2, const Transform& X_GB2, const SpatialVec& V_GB2,
+ const Vec3& undefContactPt1_G, const Vec3& undefContactPt2_G,
+ const UnitVec3& contactNormal_G, // points from body2 to body1
+ Real& pe, SpatialVec& force1, SpatialVec& force2) const
+{
+ const Real x = ~(undefContactPt2_G-undefContactPt1_G) * contactNormal_G; // 8 flops
+ // Body 1 must be "above" body 2, meaning its undeformed contact point must be "below"
+ // body 2's so that they are interpenetrating.
+ assert(x > 0);
+
+ // Calculate the fraction of total squish x which will be undergone by body 1; body 2's
+ // squish fraction will be 1-squish1.
+ const Real squish1 = k2/(k1+k2); // ~11 flops counting divide as ~10
+ const Real squish2 = 1 - squish1; // 1 flop
+
+ // Now we can find the real contact point, which is a little up the normal from pt1
+ const Vec3 contactPt_G = undefContactPt1_G + (squish1*x)*contactNormal_G; // 7 flops
+
+ // Find the body stations coincident with the contact point so that we can calculate
+ // their velocities.
+ const Vec3 contactPt1_G = contactPt_G - X_GB1.p(); // 3 flops
+ const Vec3 contactPt2_G = contactPt_G - X_GB2.p(); // 3 flops
+ const Vec3 vContactPt1_G = V_GB1[1] + V_GB1[0] % contactPt1_G; // 12 flops
+ const Vec3 vContactPt2_G = V_GB2[1] + V_GB2[0] % contactPt2_G; // 12 flops
+
+ const Real v = ~(vContactPt2_G-vContactPt1_G)*contactNormal_G; // dx/dt (8 flops)
+
+ const Real k=k1*squish1; // = k2*squish2 1 flop
+ const Real c=c1*squish1 + c2*squish2; // 3 flops
+
+ const Real fH = Real(4./3.) * k * x * std::sqrt(R*k*x); // ~35 flops
+ const Real f = fH * (1 + Real(1.5)*c*v); // 4 flops
+
+ // If the resulting force is negative, the multibody system is "yanking"
+ // the objects apart so fast that the material can't undeform fast enough
+ // to keep up. That means it can't apply any force and that the stored
+ // potential energy will now be wasted. (I suppose it would be dissipated
+ // internally as the body's surface oscillated around its undeformed shape.)
+ if (f > 0) { // 1 flop
+ pe += Real(2./5.) * fH * x; // 3 flops
+ const Vec3 fvec = f * contactNormal_G; // points towards body1 (3 flops)
+ force1 += SpatialVec( contactPt1_G % fvec, fvec); // 15 flops
+ force2 -= SpatialVec( contactPt2_G % fvec, fvec); // 15 flops
+ }
+}
+
+} // namespace SimTK
+
diff --git a/Simbody/src/HuntCrossleyForce.cpp b/Simbody/src/HuntCrossleyForce.cpp
new file mode 100644
index 0000000..81795f3
--- /dev/null
+++ b/Simbody/src/HuntCrossleyForce.cpp
@@ -0,0 +1,180 @@
+/* -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/GeneralContactSubsystem.h"
+#include "simbody/internal/MobilizedBody.h"
+
+#include "HuntCrossleyForceImpl.h"
+
+namespace SimTK {
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(HuntCrossleyForce, HuntCrossleyForceImpl, Force);
+
+HuntCrossleyForce::HuntCrossleyForce(GeneralForceSubsystem& forces, GeneralContactSubsystem& contacts, ContactSetIndex set) :
+ Force(new HuntCrossleyForceImpl(contacts, set)) {
+ updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
+}
+
+void HuntCrossleyForce::setBodyParameters
+ (ContactSurfaceIndex surfIndex, Real stiffness, Real dissipation,
+ Real staticFriction, Real dynamicFriction, Real viscousFriction) {
+ updImpl().setBodyParameters(surfIndex, stiffness, dissipation, staticFriction, dynamicFriction, viscousFriction);
+}
+
+Real HuntCrossleyForce::getTransitionVelocity() const {
+ return getImpl().getTransitionVelocity();
+}
+
+void HuntCrossleyForce::setTransitionVelocity(Real v) {
+ updImpl().setTransitionVelocity(v);
+}
+
+ContactSetIndex HuntCrossleyForce::getContactSetIndex() const {
+ return getImpl().getContactSetIndex();
+}
+
+
+HuntCrossleyForceImpl::HuntCrossleyForceImpl(GeneralContactSubsystem& subsystem, ContactSetIndex set) :
+ subsystem(subsystem), set(set), transitionVelocity(Real(0.01)) {
+}
+
+void HuntCrossleyForceImpl::setBodyParameters
+ (ContactSurfaceIndex bodyIndex, Real stiffness, Real dissipation,
+ Real staticFriction, Real dynamicFriction, Real viscousFriction) {
+ updParameters(bodyIndex) = Parameters(stiffness, dissipation, staticFriction, dynamicFriction, viscousFriction);
+ subsystem.invalidateSubsystemTopologyCache();
+}
+
+const HuntCrossleyForceImpl::Parameters& HuntCrossleyForceImpl::
+getParameters(ContactSurfaceIndex bodyIndex) const {
+ assert(bodyIndex >= 0 && bodyIndex < subsystem.getNumBodies(set));
+ // This fills in the default values which the missing entries implicitly
+ // had already.
+ if (bodyIndex >= parameters.size())
+ const_cast<Array_<Parameters,ContactSurfaceIndex>&>(parameters)
+ .resize(bodyIndex+1);
+ return parameters[bodyIndex];
+}
+
+HuntCrossleyForceImpl::Parameters& HuntCrossleyForceImpl::
+updParameters(ContactSurfaceIndex bodyIndex) {
+ assert(bodyIndex >= 0 && bodyIndex < subsystem.getNumBodies(set));
+ subsystem.invalidateSubsystemTopologyCache();
+ if (bodyIndex >= (int) parameters.size())
+ parameters.resize(bodyIndex+1);
+ return parameters[bodyIndex];
+}
+
+
+Real HuntCrossleyForceImpl::getTransitionVelocity() const {
+ return transitionVelocity;
+}
+
+void HuntCrossleyForceImpl::setTransitionVelocity(Real v) {
+ transitionVelocity = v;
+ subsystem.invalidateSubsystemTopologyCache();
+}
+
+void HuntCrossleyForceImpl::calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const {
+ const Array_<Contact>& contacts = subsystem.getContacts(state, set);
+ Real& pe = Value<Real>::downcast(state.updCacheEntry(subsystem.getMySubsystemIndex(), energyCacheIndex)).upd();
+ pe = 0.0;
+ for (int i = 0; i < (int) contacts.size(); i++) {
+ if (!PointContact::isInstance(contacts[i]))
+ continue;
+ const PointContact& contact = static_cast<const PointContact&>(contacts[i]);
+ const Parameters& param1 = getParameters(contact.getSurface1());
+ const Parameters& param2 = getParameters(contact.getSurface2());
+
+ // Adjust the contact location based on the relative stiffness of the two materials.
+
+ const Real s1 = param2.stiffness/(param1.stiffness+param2.stiffness);
+ const Real s2 = 1-s1;
+ const Real depth = contact.getDepth();
+ const Vec3& normal = contact.getNormal();
+ const Vec3 location = contact.getLocation()+(depth*(Real(0.5)-s1))*normal;
+
+ // Calculate the Hertz force.
+
+ const Real k = param1.stiffness*s1;
+ const Real c = param1.dissipation*s1 + param2.dissipation*s2;
+ const Real radius = contact.getEffectiveRadiusOfCurvature();
+ const Real fH = Real(4./3.)*k*depth*std::sqrt(radius*k*depth);
+ pe += Real(2./5.)*fH*depth;
+
+ // Calculate the relative velocity of the two bodies at the contact point.
+
+ const MobilizedBody& body1 = subsystem.getBody(set, contact.getSurface1());
+ const MobilizedBody& body2 = subsystem.getBody(set, contact.getSurface2());
+ const Vec3 station1 = body1.findStationAtGroundPoint(state, location);
+ const Vec3 station2 = body2.findStationAtGroundPoint(state, location);
+ const Vec3 v1 = body1.findStationVelocityInGround(state, station1);
+ const Vec3 v2 = body2.findStationVelocityInGround(state, station2);
+ const Vec3 v = v1-v2;
+ const Real vnormal = dot(v, normal);
+ const Vec3 vtangent = v-vnormal*normal;
+
+ // Calculate the Hunt-Crossley force.
+
+ const Real f = fH*(1+Real(1.5)*c*vnormal);
+ if (f <= 0)
+ return;
+
+ Vec3 force = f*normal;
+
+ // Calculate the friction force.
+
+ const Real vslip = vtangent.norm();
+ if (vslip != 0) {
+ const bool hasStatic = (param1.staticFriction != 0 || param2.staticFriction != 0);
+ const bool hasDynamic= (param1.dynamicFriction != 0 || param2.dynamicFriction != 0);
+ const bool hasViscous = (param1.viscousFriction != 0 || param2.viscousFriction != 0);
+ const Real us = hasStatic ? 2*param1.staticFriction*param2.staticFriction/(param1.staticFriction+param2.staticFriction) : 0;
+ const Real ud = hasDynamic ? 2*param1.dynamicFriction*param2.dynamicFriction/(param1.dynamicFriction+param2.dynamicFriction) : 0;
+ const Real uv = hasViscous ? 2*param1.viscousFriction*param2.viscousFriction/(param1.viscousFriction+param2.viscousFriction) : 0;
+ const Real vrel = vslip/getTransitionVelocity();
+ const Real ffriction = f*(std::min(vrel, Real(1))*(ud+2*(us-ud)/(1+vrel*vrel))+uv*vslip);
+ force += ffriction*vtangent/vslip;
+ }
+
+ // Apply the force to the bodies.
+
+ body1.applyForceToBodyPoint(state, station1, -force, bodyForces);
+ body2.applyForceToBodyPoint(state, station2, force, bodyForces);
+ }
+}
+
+Real HuntCrossleyForceImpl::calcPotentialEnergy(const State& state) const {
+ return Value<Real>::downcast(state.getCacheEntry(subsystem.getMySubsystemIndex(), energyCacheIndex)).get();
+}
+
+void HuntCrossleyForceImpl::realizeTopology(State& state) const {
+ energyCacheIndex = state.allocateCacheEntry(subsystem.getMySubsystemIndex(), Stage::Dynamics, new Value<Real>());
+}
+
+} // namespace SimTK
+
diff --git a/Simbody/src/HuntCrossleyForceImpl.h b/Simbody/src/HuntCrossleyForceImpl.h
new file mode 100644
index 0000000..7f878ce
--- /dev/null
+++ b/Simbody/src/HuntCrossleyForceImpl.h
@@ -0,0 +1,75 @@
+#ifndef SimTK_SIMBODY_HUNT_CROSSLEY_FORCE_IMPL_H_
+#define SimTK_SIMBODY_HUNT_CROSSLEY_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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/HuntCrossleyForce.h"
+#include "ForceImpl.h"
+
+namespace SimTK {
+
+class HuntCrossleyForceImpl : public ForceImpl {
+public:
+ class Parameters;
+ HuntCrossleyForceImpl(GeneralContactSubsystem& subystem, ContactSetIndex set);
+ HuntCrossleyForceImpl* clone() const {
+ return new HuntCrossleyForceImpl(*this);
+ }
+ void setBodyParameters
+ (ContactSurfaceIndex surfIndex, Real stiffness, Real dissipation,
+ Real staticFriction, Real dynamicFriction, Real viscousFriction);
+ const Parameters& getParameters(ContactSurfaceIndex bodyIndex) const;
+ Parameters& updParameters(ContactSurfaceIndex bodyIndex);
+ Real getTransitionVelocity() const;
+ void setTransitionVelocity(Real v);
+ ContactSetIndex getContactSetIndex() const {return set;}
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const;
+ Real calcPotentialEnergy(const State& state) const;
+ void realizeTopology(State& state) const;
+private:
+ const GeneralContactSubsystem& subsystem;
+ const ContactSetIndex set;
+ Array_<Parameters,ContactSurfaceIndex> parameters;
+ Real transitionVelocity;
+ mutable CacheEntryIndex energyCacheIndex;
+};
+
+class HuntCrossleyForceImpl::Parameters {
+public:
+ Parameters() : stiffness(1), dissipation(0), staticFriction(0), dynamicFriction(0), viscousFriction(0) {
+ }
+ Parameters(Real stiffness, Real dissipation,
+ Real staticFriction, Real dynamicFriction, Real viscousFriction)
+ : stiffness(std::pow(stiffness, Real(2./3.))), dissipation(dissipation),
+ staticFriction(staticFriction), dynamicFriction(dynamicFriction),
+ viscousFriction(viscousFriction) {}
+ Real stiffness, dissipation, staticFriction,
+ dynamicFriction, viscousFriction;
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_HUNT_CROSSLEY_FORCE_IMPL_H_
diff --git a/Simbody/src/LocalEnergyMinimizer.cpp b/Simbody/src/LocalEnergyMinimizer.cpp
new file mode 100644
index 0000000..0cd845d
--- /dev/null
+++ b/Simbody/src/LocalEnergyMinimizer.cpp
@@ -0,0 +1,102 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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/MobilizedBody.h"
+#include "simbody/internal/MultibodySystem.h"
+#include "simbody/internal/LocalEnergyMinimizer.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include <vector>
+#include <map>
+
+using namespace SimTK;
+using namespace std;
+
+/**
+ * This class defines the objective function which is passed to the Optimizer.
+ */
+
+class LocalEnergyMinimizer::OptimizerFunction : public OptimizerSystem {
+public:
+ // stateIn must be realized to Model stage already.
+ OptimizerFunction(const MultibodySystem& system, const State& stateIn)
+ : OptimizerSystem(stateIn.getNQ()), system(system), state(stateIn) {
+ state.updU() = 0; // no velocities
+ system.realize(state, Stage::Time); // we'll only change Position stage and above
+ setNumEqualityConstraints(state.getNQErr());
+ }
+ int objectiveFunc(const Vector& parameters, bool new_parameters, Real& f) const {
+ if (new_parameters)
+ state.updQ() = parameters;
+ system.realize(state, Stage::Dynamics);
+ f = system.calcPotentialEnergy(state);
+ return 0;
+ }
+ int gradientFunc(const Vector& parameters, bool new_parameters, Vector& gradient) const {
+ if (new_parameters)
+ state.updQ() = parameters;
+ system.realize(state, Stage::Dynamics);
+ Vector_<SpatialVec> dEdR = system.getRigidBodyForces(state, Stage::Dynamics);
+ const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
+ Vector dEdU;
+ // Convert spatial forces dEdR to generalized forces dEdU.
+ matter.multiplyBySystemJacobianTranspose(state, dEdR, dEdU);
+ dEdU -= system.getMobilityForces(state, Stage::Dynamics);
+ matter.multiplyByNInv(state, true, -1*dEdU, gradient);
+ return 0;
+ }
+ int constraintFunc(const Vector& parameters, bool new_parameters, Vector& constraints) const {
+ state.updQ() = parameters;
+ system.realize(state, Stage::Position);
+ constraints = state.getQErr();
+ return 0;
+ }
+ void optimize(Vector& q, Real tolerance) {
+ Optimizer opt(*this);
+ opt.useNumericalJacobian(true);
+ opt.setConvergenceTolerance(tolerance);
+ opt.optimize(q);
+ }
+private:
+ const MultibodySystem& system;
+ mutable State state;
+};
+
+void LocalEnergyMinimizer::minimizeEnergy(const MultibodySystem& system, State& state, Real tolerance) {
+ const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
+ State tempState = state;
+ matter.setUseEulerAngles(tempState, true);
+ system.realizeModel(tempState);
+ if (!matter.getUseEulerAngles(state))
+ matter.convertToEulerAngles(state, tempState);
+ OptimizerFunction optimizer(system, tempState);
+ Vector q = tempState.getQ();
+ optimizer.optimize(q, tolerance);
+ if (matter.getUseEulerAngles(state))
+ state.updQ() = q;
+ else {
+ tempState.updQ() = q;
+ matter.convertToQuaternions(tempState, state);
+ }
+ system.realize(state, Stage::Dynamics);
+}
diff --git a/Simbody/src/Matter_Instantiations.cpp b/Simbody/src/Matter_Instantiations.cpp
new file mode 100644
index 0000000..e41f845
--- /dev/null
+++ b/Simbody/src/Matter_Instantiations.cpp
@@ -0,0 +1,67 @@
+/* -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "SimTKcommon/internal/PrivateImplementation_Defs.h"
+
+// This suppresses the 'extern template' instantiations in Motion.h so that
+// we can instantiate them for real here.
+#define SimTK_SIMBODY_DEFINING_MOTION
+#include "simbody/internal/Motion.h"
+#include "MotionImpl.h"
+
+// This suppresses the 'extern template' instantiations in MobilizedBody.h so that
+// we can instantiate them for real here.
+#define SimTK_SIMBODY_DEFINING_MOBILIZED_BODY
+#include "simbody/internal/MobilizedBody.h"
+#include "simbody/internal/MobilizedBody_Custom.h"
+#include "MobilizedBodyImpl.h"
+
+// This suppresses the 'extern template' instantiations in Constraint.h so that
+// we can instantiate them for real here.
+#define SimTK_SIMBODY_DEFINING_CONSTRAINT
+#include "simbody/internal/Constraint.h"
+#include "ConstraintImpl.h"
+
+
+namespace SimTK {
+
+ // Motion
+template class PIMPLHandle<Motion, MotionImpl, true>;
+template class PIMPLImplementation<Motion, MotionImpl>;
+
+ // MobilizedBody
+template class PIMPLHandle<MobilizedBody, MobilizedBodyImpl, true>;
+template class PIMPLImplementation<MobilizedBody, MobilizedBodyImpl>;
+template class PIMPLHandle<MobilizedBody::Custom::Implementation, MobilizedBody::Custom::ImplementationImpl>;
+template class PIMPLImplementation<MobilizedBody::Custom::Implementation, MobilizedBody::Custom::ImplementationImpl>;
+
+ // Constraint
+template class PIMPLHandle<Constraint, ConstraintImpl, true>;
+template class PIMPLImplementation<Constraint, ConstraintImpl>;
+template class PIMPLHandle<Constraint::Custom::Implementation, Constraint::Custom::ImplementationImpl>;
+template class PIMPLImplementation<Constraint::Custom::Implementation, Constraint::Custom::ImplementationImpl>;
+
+} // namespace SimTK
+
diff --git a/Simbody/src/MobilizedBody.cpp b/Simbody/src/MobilizedBody.cpp
new file mode 100644
index 0000000..ea98575
--- /dev/null
+++ b/Simbody/src/MobilizedBody.cpp
@@ -0,0 +1,3043 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2007-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ *
+ * Private implementation of MobilizedBody, and its included subclasses which
+ * represent the built-in mobilizer types.
+ */
+
+#include "SimTKcommon.h"
+#include "simbody/internal/common.h"
+#include "simbody/internal/MobilizedBody.h"
+#include "simbody/internal/MobilizedBody_BuiltIns.h"
+
+#include "MobilizedBodyImpl.h"
+#include "SimbodyMatterSubsystemRep.h"
+
+namespace SimTK {
+
+ ////////////////////
+ // MOBILIZED BODY //
+ ////////////////////
+
+MobilizedBody::MobilizedBody(MobilizedBodyImpl* r) : HandleBase(r) {
+}
+
+int MobilizedBody::
+addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g)
+{ return updImpl().addOutboardDecoration(X_MD,g); }
+int MobilizedBody::getNumOutboardDecorations() const
+{ return (int)getImpl().outboardGeometry.size(); }
+const DecorativeGeometry& MobilizedBody::getOutboardDecoration(int i) const
+{ return getImpl().outboardGeometry[i]; }
+DecorativeGeometry& MobilizedBody::updOutboardDecoration(int i)
+{ return updImpl().outboardGeometry[i]; }
+
+int MobilizedBody::
+addInboardDecoration(const Transform& X_FD, const DecorativeGeometry& g)
+{ return updImpl().addInboardDecoration(X_FD,g); }
+int MobilizedBody::getNumInboardDecorations() const
+{ return (int)getImpl().inboardGeometry.size(); }
+const DecorativeGeometry& MobilizedBody::getInboardDecoration(int i) const
+{ return getImpl().inboardGeometry[i]; }
+DecorativeGeometry& MobilizedBody::updInboardDecoration(int i)
+{ return updImpl().inboardGeometry[i]; }
+
+
+void MobilizedBody::
+lock(State& s, Motion::Level level) const
+{ getImpl().lock(s,level); }
+
+void MobilizedBody::
+lockAt(State& s, Real value, Motion::Level level) const
+{ getImpl().lockAt(s,1,&value,level); }
+
+void MobilizedBody::
+lockAt(State& s, const Vector& value, Motion::Level level) const
+{
+ const MobilizedBodyImpl& impl = getImpl();
+ if (value.hasContiguousData()) {
+ getImpl().lockAt(s,value.size(),&value[0],level);
+ return;
+ }
+
+ Vector contig(value.size()); // contiguous
+ contig.viewAssign(value); // don't reallocate
+ // This will produce an error if value.size()>7.
+ getImpl().lockAt(s,value.size(),&contig[0],level);
+}
+
+template <> SimTK_SIMBODY_EXPORT void MobilizedBody::
+lockAt(State& s, const Vec<1>& value, Motion::Level level) const
+{ getImpl().lockAt(s,1,&value[0],level); }
+template <> SimTK_SIMBODY_EXPORT void MobilizedBody::
+lockAt(State& s, const Vec<2>& value, Motion::Level level) const
+{ getImpl().lockAt(s,2,&value[0],level); }
+template <> SimTK_SIMBODY_EXPORT void MobilizedBody::
+lockAt(State& s, const Vec<3>& value, Motion::Level level) const
+{ getImpl().lockAt(s,3,&value[0],level); }
+template <> SimTK_SIMBODY_EXPORT void MobilizedBody::
+lockAt(State& s, const Vec<4>& value, Motion::Level level) const
+{ getImpl().lockAt(s,4,&value[0],level); }
+template <> SimTK_SIMBODY_EXPORT void MobilizedBody::
+lockAt(State& s, const Vec<5>& value, Motion::Level level) const
+{ getImpl().lockAt(s,5,&value[0],level); }
+template <> SimTK_SIMBODY_EXPORT void MobilizedBody::
+lockAt(State& s, const Vec<6>& value, Motion::Level level) const
+{ getImpl().lockAt(s,6,&value[0],level); }
+template <> SimTK_SIMBODY_EXPORT void MobilizedBody::
+lockAt(State& s, const Vec<7>& value, Motion::Level level) const
+{ getImpl().lockAt(s,7,&value[0],level); }
+
+void MobilizedBody::unlock(State& s) const
+{ getImpl().unlock(s); }
+
+Motion::Level MobilizedBody::getLockLevel(const State& s) const
+{ return getImpl().getLockLevel(s); }
+
+Vector MobilizedBody::getLockValueAsVector(const State& s) const
+{ return getImpl().getLockValueAsVector(s); }
+
+MobilizedBody& MobilizedBody::lockByDefault(Motion::Level level)
+{ updImpl().lockByDefault(level); return *this; }
+
+Motion::Level MobilizedBody::getLockByDefaultLevel() const
+{ return getImpl().getLockByDefaultLevel(); }
+
+
+const SimbodyMatterSubsystem& MobilizedBody::getMatterSubsystem() const {
+ SimTK_ASSERT_ALWAYS(isInSubsystem(),
+ "getMatterSubsystem() called on a MobilizedBody that is not part of a subsystem.");
+ return getImpl().getMyMatterSubsystemRep().getMySimbodyMatterSubsystemHandle();
+}
+
+
+MobilizedBodyIndex MobilizedBody::getMobilizedBodyIndex() const {
+ SimTK_ASSERT_ALWAYS(isInSubsystem(),
+ "getMobilizedBodyIndex() called on a MobilizedBody that is not part of a subsystem.");
+ return getImpl().getMyMobilizedBodyIndex();
+}
+
+const MobilizedBody& MobilizedBody::getParentMobilizedBody() const {
+ SimTK_ASSERT_ALWAYS(isInSubsystem(),
+ "getParentMobilizedBody() called on a MobilizedBody that is not part of a subsystem.");
+ return getImpl().getMyMatterSubsystemRep()
+ .getMobilizedBody(getImpl().getMyParentMobilizedBodyIndex());
+}
+
+const MobilizedBody& MobilizedBody::getBaseMobilizedBody() const {
+ SimTK_ASSERT_ALWAYS(isInSubsystem(),
+ "getBaseMobilizedBody() called on a MobilizedBody that is not part of a subsystem.");
+ return getImpl().getMyMatterSubsystemRep()
+ .getMobilizedBody(getImpl().getMyBaseBodyMobilizedBodyIndex());
+}
+
+bool MobilizedBody::isInSubsystem() const {
+ return !isEmptyHandle() && getImpl().isInSubsystem();
+}
+
+bool MobilizedBody::isInSameSubsystem(const MobilizedBody& otherBody) const {
+ return isInSubsystem() && otherBody.isInSubsystem()
+ && getMatterSubsystem().isSameSubsystem(otherBody.getMatterSubsystem());
+}
+
+bool MobilizedBody::isSameMobilizedBody(const MobilizedBody& otherBody) const {
+ return hasSameImplementation(otherBody);
+}
+
+bool MobilizedBody::isGround() const {
+ return isInSubsystem()
+ && isSameMobilizedBody(getMatterSubsystem().getGround());
+}
+
+int MobilizedBody::getLevelInMultibodyTree() const {
+ return getImpl().getMyLevel();
+}
+
+SimbodyMatterSubsystem& MobilizedBody::updMatterSubsystem() {
+ SimTK_ASSERT_ALWAYS(isInSubsystem(),
+ "updMatterSubsystem() called on a MobilizedBody that is not part of a subsystem.");
+ return updImpl().updMyMatterSubsystemRep()
+ .updMySimbodyMatterSubsystemHandle();
+}
+
+const Body& MobilizedBody::getBody() const {
+ return getImpl().theBody;
+}
+Body& MobilizedBody::updBody() {
+ getImpl().invalidateTopologyCache();
+ return updImpl().theBody;
+}
+MobilizedBody& MobilizedBody::setBody(const Body& b) {
+ updBody() = b;
+ return *this;
+}
+MobilizedBody& MobilizedBody::setDefaultInboardFrame (const Transform& X_PMb) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultInboardFrame = X_PMb;
+ return *this;
+}
+MobilizedBody& MobilizedBody::setDefaultOutboardFrame(const Transform& X_BM) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultOutboardFrame = X_BM;
+ return *this;
+}
+
+const Transform& MobilizedBody::getDefaultInboardFrame() const {
+ return getImpl().defaultInboardFrame;
+}
+const Transform& MobilizedBody::getDefaultOutboardFrame() const {
+ return getImpl().defaultOutboardFrame;
+}
+
+// Access to State
+
+const MassProperties& MobilizedBody::getBodyMassProperties(const State& s) const
+{ return getImpl().getBodyMassProperties(s); }
+
+const SpatialInertia& MobilizedBody::
+getBodySpatialInertiaInGround(const State& s) const {
+ return getImpl().getBodySpatialInertiaInGround(s);
+}
+
+const Transform& MobilizedBody::getInboardFrame (const State& s) const {
+ return getImpl().getInboardFrame(s);
+}
+const Transform& MobilizedBody::getOutboardFrame(const State& s) const {
+ return getImpl().getOutboardFrame(s);
+}
+
+void MobilizedBody::setInboardFrame (State& s, const Transform& X_PMb) const {
+ getImpl().setInboardFrame(s, X_PMb);
+}
+void MobilizedBody::setOutboardFrame(State& s, const Transform& X_BM) const {
+ getImpl().setOutboardFrame(s, X_BM);
+}
+
+const Transform& MobilizedBody::getBodyTransform(const State& s) const {
+ return getImpl().getBodyTransform(s);
+}
+const SpatialVec& MobilizedBody::getBodyVelocity(const State& s) const {
+ return getImpl().getBodyVelocity(s);
+}
+const SpatialVec& MobilizedBody::getBodyAcceleration(const State& s) const {
+ return getImpl().getBodyAcceleration(s);
+}
+
+const Transform& MobilizedBody::getMobilizerTransform(const State& s) const {
+ return getImpl().getMobilizerTransform(s);
+}
+const SpatialVec& MobilizedBody::getMobilizerVelocity(const State& s) const {
+ return getImpl().getMobilizerVelocity(s);
+}
+
+void MobilizedBody::setQToFitTransform(State& s, const Transform& X_MbM) const {
+ getImpl().setQToFitTransform(s,X_MbM);
+}
+void MobilizedBody::setQToFitRotation(State& s, const Rotation& R_MbM) const {
+ getImpl().setQToFitRotation(s,R_MbM);
+}
+void MobilizedBody::setQToFitTranslation(State& s, const Vec3& p_MbM) const {
+ getImpl().setQToFitTranslation(s,p_MbM);
+}
+void MobilizedBody::setUToFitVelocity(State& s, const SpatialVec& V_MbM) const {
+ getImpl().setUToFitVelocity(s,V_MbM);
+}
+void MobilizedBody::setUToFitAngularVelocity(State& s, const Vec3& w_MbM) const {
+ getImpl().setUToFitAngularVelocity(s,w_MbM);
+}
+void MobilizedBody::setUToFitLinearVelocity(State& s, const Vec3& v_MbM) const {
+ getImpl().setUToFitLinearVelocity(s,v_MbM);
+}
+
+SpatialVec MobilizedBody::getHCol(const State& s, MobilizerUIndex ux) const {
+ SimTK_INDEXCHECK(ux, getNumU(s), "MobilizedBody::getHCol()");
+ return getImpl().getHCol(s,ux);
+}
+
+SpatialVec MobilizedBody::getH_FMCol(const State& s, MobilizerUIndex ux) const {
+ SimTK_INDEXCHECK(ux, getNumU(s), "MobilizedBody::getH_FMCol()");
+ return getImpl().getH_FMCol(s,ux);
+}
+
+int MobilizedBody::getNumQ(const State& s) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s, qStart, nq);
+ return nq;
+}
+
+QIndex MobilizedBody::getFirstQIndex(const State& s) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s, qStart, nq);
+ return qStart;
+}
+
+int MobilizedBody::getNumU(const State& s) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s, uStart, nu);
+ return nu;
+}
+
+UIndex MobilizedBody::getFirstUIndex(const State& s) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s, uStart, nu);
+ return uStart;
+}
+
+Motion::Method MobilizedBody::getQMotionMethod(const State& s) const
+{ return getImpl().getQMotionMethod(s); }
+Motion::Method MobilizedBody::getUMotionMethod(const State& s) const
+{ return getImpl().getUMotionMethod(s); }
+Motion::Method MobilizedBody::getUDotMotionMethod(const State& s) const
+{ return getImpl().getUDotMotionMethod(s); }
+
+Real MobilizedBody::getOneFromQPartition
+ (const State& s, int which, const Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s, qStart, nq);
+ assert(0 <= which && which < nq);
+ return qlike[qStart+which];
+}
+Real& MobilizedBody::updOneFromQPartition
+ (const State& s, int which, Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s, qStart, nq);
+ assert(0 <= which && which < nq);
+ return qlike[qStart+which];
+}
+
+Real MobilizedBody::getOneFromUPartition
+ (const State& s, int which, const Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s, uStart, nu);
+ assert(0 <= which && which < nu);
+ return ulike[uStart+which];
+}
+Real& MobilizedBody::updOneFromUPartition
+ (const State& s, int which, Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s, uStart, nu);
+ assert(0 <= which && which < nu);
+ return ulike[uStart+which];
+}
+
+void MobilizedBody::
+convertQForceToUForce( const State& state,
+ const Array_<Real,MobilizerQIndex>& fq,
+ Array_<Real,MobilizerUIndex>& fu) const
+{
+ const MobilizedBodyImpl& impl = getImpl();
+ const SimbodyMatterSubsystemRep& matter = impl.getMyMatterSubsystemRep();
+
+ UIndex uStart; int nu;
+ QIndex qStart; int nq;
+ matter.findMobilizerUs(state, impl.getMyMobilizedBodyIndex(), uStart, nu);
+ matter.findMobilizerQs(state, impl.getMyMobilizedBodyIndex(), qStart, nq);
+ assert(fq.size() == nq);
+
+ fu.resize(nu);
+ const RigidBodyNode& node = impl.getMyRigidBodyNode();
+ const SBStateDigest digest(state, matter, Stage::Velocity);
+ node.multiplyByN(digest, true, fq.cbegin(), fu.begin());
+}
+
+SpatialVec MobilizedBody::
+findMobilizerReactionOnBodyAtMInGround(const State& s) const {
+ const SpatialVec FB_G = findMobilizerReactionOnBodyAtOriginInGround(s);
+ // Shift to M
+ const Rotation& R_GB = getBodyRotation(s);
+ const Vec3& p_BM = getOutboardFrame(s).p();
+ const Vec3 p_BM_G = R_GB*p_BM; // p_BM in G, 15 flops
+ const SpatialVec FM_G = shiftForceBy(FB_G, p_BM_G); // 12 flop
+ return FM_G;
+}
+
+
+SpatialVec MobilizedBody::
+findMobilizerReactionOnBodyAtOriginInGround(const State& s) const {
+ const MobilizedBodyImpl& impl = getImpl();
+ const SimbodyMatterSubsystemRep& matter = impl.getMyMatterSubsystemRep();
+
+ // This will initiate computation if necessary.
+ const Array_<ArticulatedInertia,MobilizedBodyIndex>& PPlus =
+ matter.getArticulatedBodyInertiasPlus(s);
+ const Array_<SpatialVec,MobilizedBodyIndex>& zPlus =
+ matter.getArticulatedBodyForcesPlus(s);
+
+ const Transform& X_GB = impl.getBodyTransform(s);
+ const MobilizedBodyIndex mbx = impl.getMyMobilizedBodyIndex();
+
+ SpatialVec FB_G = zPlus[mbx];
+ if (mbx != GroundIndex) {
+ const MobilizedBody& parent = getParentMobilizedBody();
+ const Transform& X_GP = parent.getBodyTransform(s);
+ const SpatialVec& A_GP = parent.getBodyAcceleration(s);
+ const Vec3& p_PB_G = X_GB.p() - X_GP.p(); // 3 flops
+ SpatialVec APlus( A_GP[0],
+ A_GP[1] + A_GP[0] % p_PB_G ); // 12 flops
+ FB_G += PPlus[mbx]*APlus; // 72 flops
+ }
+ return FB_G;
+}
+
+
+SpatialVec MobilizedBody::
+findMobilizerReactionOnParentAtFInGround(const State& s) const {
+ const SpatialVec FP_G = findMobilizerReactionOnParentAtOriginInGround(s);
+ // Shift from P to F
+ const Vec3& p_PF = getInboardFrame(s).p();
+ if (isGround())
+ return shiftForceBy(FP_G, p_PF); // P==G (12 flops)
+ const Rotation& R_GP = getParentMobilizedBody().getBodyRotation(s);
+ const Vec3 p_PF_G = R_GP*p_PF; // p_PF in G, 15 flops
+ return shiftForceBy(FP_G, p_PF_G); // 12 flops
+}
+
+SpatialVec MobilizedBody::
+findMobilizerReactionOnParentAtOriginInGround(const State& s) const {
+ const SpatialVec FB_G = // negate this
+ -findMobilizerReactionOnBodyAtOriginInGround(s); // 6 flops
+ if (isGround())
+ return FB_G; // Ground is welded to the universe at its origin
+
+ const MobilizedBody& parent = getParentMobilizedBody();
+ const Vec3& p_GB = getBodyTransform(s).p();
+ const Vec3& p_GP = parent.getBodyTransform(s).p();
+ const Vec3 p_BP_G = p_GP - p_GB; // 3 flops
+ const SpatialVec FP_G = shiftForceBy(FB_G, p_BP_G); // 12 flops
+
+ return FP_G;
+}
+
+
+void MobilizedBody::applyBodyForce
+ (const State& s, const SpatialVec& spatialForceInG,
+ Vector_<SpatialVec>& bodyForces) const
+{
+ assert(bodyForces.size() == getMatterSubsystem().getNumBodies());
+ bodyForces[getMobilizedBodyIndex()] += spatialForceInG;
+}
+
+void MobilizedBody::applyBodyTorque(const State& s, const Vec3& torqueInG,
+ Vector_<SpatialVec>& bodyForces) const
+{
+ assert(bodyForces.size() == getMatterSubsystem().getNumBodies());
+ bodyForces[getMobilizedBodyIndex()][0] += torqueInG; // don't change force
+}
+
+void MobilizedBody::applyForceToBodyPoint
+ (const State& s, const Vec3& pointInB, const Vec3& forceInG,
+ Vector_<SpatialVec>& bodyForces) const
+{
+ assert(bodyForces.size() == getMatterSubsystem().getNumBodies());
+ const Rotation& R_GB = getBodyTransform(s).R();
+ bodyForces[getMobilizedBodyIndex()] +=
+ SpatialVec((R_GB*pointInB) % forceInG, forceInG);
+}
+
+Real MobilizedBody::getOneQ(const State& s, int which) const {
+ return getOneFromQPartition
+ (s,which,getImpl().getMyMatterSubsystemRep().getQ(s));
+}
+
+void MobilizedBody::setOneQ(State& s, int which, Real value) const {
+ updOneFromQPartition
+ (s,which,getImpl().getMyMatterSubsystemRep().updQ(s)) = value;
+}
+
+Real MobilizedBody::getOneU(const State& s, int which) const {
+ return getOneFromUPartition
+ (s,which,getImpl().getMyMatterSubsystemRep().getU(s));
+}
+void MobilizedBody::setOneU(State& s, int which, Real value) const {
+ updOneFromUPartition
+ (s,which,getImpl().getMyMatterSubsystemRep().updU(s)) = value;
+}
+
+Real MobilizedBody::getOneQDot(const State& s, int which) const {
+ return getOneFromQPartition
+ (s,which,getImpl().getMyMatterSubsystemRep().getQDot(s));
+}
+Real MobilizedBody::getOneUDot(const State& s, int which) const {
+ return getOneFromUPartition
+ (s,which,getImpl().getMyMatterSubsystemRep().getUDot(s));
+}
+Real MobilizedBody::getOneQDotDot(const State& s, int which) const {
+ return getOneFromQPartition
+ (s,which,getImpl().getMyMatterSubsystemRep().getQDotDot(s));
+}
+Real MobilizedBody::getOneTau(const State& s, MobilizerUIndex which) const {
+ const MobilizedBodyImpl& mbimpl = MobilizedBody::getImpl();
+ const MobilizedBodyIndex mbx = mbimpl.getMyMobilizedBodyIndex();
+ const SimbodyMatterSubsystemRep& matter = mbimpl.getMyMatterSubsystemRep();
+ const SBModelCache& mc = matter.getModelCache(s);
+ const SBModelPerMobodInfo&
+ mobodModelInfo = mc.getMobodModelInfo(mbx);
+ const int nu = mobodModelInfo.nUInUse;
+
+ SimTK_INDEXCHECK(which, nu, "MobilizedBody::getOneTau()");
+
+ const SBInstanceCache& ic = matter.getInstanceCache(s);
+ const SBInstancePerMobodInfo&
+ mobodInstanceInfo = ic.getMobodInstanceInfo(mbx);
+ if (mobodInstanceInfo.udotMethod == Motion::Free)
+ return 0; // not prescribed
+
+ const SBTreeAccelerationCache& tac = matter.getTreeAccelerationCache(s);
+ return tac.presMotionForces[mobodInstanceInfo.firstPresForce + which];
+}
+
+Vector MobilizedBody::getTauAsVector(const State& s) const {
+ const MobilizedBodyImpl& mbimpl = MobilizedBody::getImpl();
+ const MobilizedBodyIndex mbx = mbimpl.getMyMobilizedBodyIndex();
+ const SimbodyMatterSubsystemRep& matter = mbimpl.getMyMatterSubsystemRep();
+ const SBModelCache& mc = matter.getModelCache(s);
+ const SBModelPerMobodInfo&
+ mobodModelInfo = mc.getMobodModelInfo(mbx);
+ const int nu = mobodModelInfo.nUInUse;
+
+ const SBInstanceCache& ic = matter.getInstanceCache(s);
+ const SBInstancePerMobodInfo&
+ mobodInstanceInfo = ic.getMobodInstanceInfo(mbx);
+ if (mobodInstanceInfo.udotMethod == Motion::Free)
+ return Vector(nu, Real(0)); // not prescribed
+
+ const SBTreeAccelerationCache& tac = matter.getTreeAccelerationCache(s);
+ return tac.presMotionForces(mobodInstanceInfo.firstPresForce, nu);
+}
+
+Vector MobilizedBody::getQAsVector(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s, qStart, nq);
+ return mbr.getMyMatterSubsystemRep().getQ(s)(qStart,nq);
+}
+
+void MobilizedBody::setQFromVector(State& s, const Vector& q) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s, qStart, nq);
+ assert(q.size() == nq);
+ mbr.getMyMatterSubsystemRep().updQ(s)(qStart,nq) = q;
+}
+
+Vector MobilizedBody::getUAsVector(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s, uStart, nu);
+ return mbr.getMyMatterSubsystemRep().getU(s)(uStart,nu);
+}
+
+void MobilizedBody::setUFromVector(State& s, const Vector& u) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s, uStart, nu);
+ assert(u.size() == nu);
+ mbr.getMyMatterSubsystemRep().updU(s)(uStart,nu) = u;
+}
+
+Vector MobilizedBody::getQDotAsVector(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s, qStart, nq);
+ return mbr.getMyMatterSubsystemRep().getQDot(s)(qStart,nq);
+}
+
+Vector MobilizedBody::getUDotAsVector(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s, uStart, nu);
+ return mbr.getMyMatterSubsystemRep().getUDot(s)(uStart,nu);
+}
+
+Vector MobilizedBody::getQDotDotAsVector(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s, qStart, nq);
+ return mbr.getMyMatterSubsystemRep().getQDotDot(s)(qStart,nq);
+}
+
+MobilizedBody& MobilizedBody::cloneForNewParent(MobilizedBody& parent) const {
+ MobilizedBody copyBody;
+ copyBody.copyAssign(*this);
+ copyBody.updImpl().myMatterSubsystemRep = 0;
+ copyBody.updImpl().myRBnode = 0;
+ parent.updMatterSubsystem()
+ .adoptMobilizedBody(parent.getMobilizedBodyIndex(), copyBody);
+ return parent.updMatterSubsystem()
+ .updMobilizedBody(copyBody.getMobilizedBodyIndex());
+}
+
+void MobilizedBody::adoptMotion(Motion& ownerHandle) {
+ updImpl().adoptMotion(ownerHandle);
+}
+
+void MobilizedBody::clearMotion() {
+ updImpl().clearMotion();
+}
+
+bool MobilizedBody::hasMotion() const {
+ return getImpl().hasMotion();
+}
+
+const Motion& MobilizedBody::getMotion() const {
+ return getImpl().getMotion();
+}
+
+
+
+//==============================================================================
+// MOBILIZED BODY IMPL
+//==============================================================================
+
+void MobilizedBodyImpl::findMobilizerQs
+ (const State& s, QIndex& qStart, int& nq) const {
+ getMyMatterSubsystemRep()
+ .findMobilizerQs(s, myMobilizedBodyIndex, qStart, nq);
+}
+void MobilizedBodyImpl::findMobilizerUs
+ (const State& s, UIndex& uStart, int& nu) const {
+ getMyMatterSubsystemRep()
+ .findMobilizerUs(s, myMobilizedBodyIndex, uStart, nu);
+}
+
+// Set the lock level and record the current q or u if needed so we can
+// ensure that the state is prescribed to the recorded value. When locking
+// q, set u to zero now.
+// Can't do this until *after* realize(Model) because that's when the q's and
+// u's are sized and the lock-by-default coordinates are recorded, and this
+// should override those.
+void MobilizedBodyImpl::
+lock(State& state, Motion::Level level) const {
+ SimTK_STAGECHECK_GE_ALWAYS(getMyMatterSubsystemRep().getStage(state),
+ Stage::Model, "MobilizedBody::lock()");
+
+ SBInstanceVars& iv = getMyMatterSubsystemRep().updInstanceVars(state);
+ iv.mobilizerLockLevel[getMyMobilizedBodyIndex()] = level;
+ UIndex uStart; int nu; findMobilizerUs(state, uStart, nu);
+ const UIndex uEnd(uStart+nu);
+
+ if (level == Motion::Position) {
+ Vector& u = state.updU(); // set generalized speeds to zero
+ for (UIndex ux=uStart; ux != uEnd; ++ux)
+ u[ux] = 0;
+ const Vector& q = state.getQ();
+ assert(iv.lockedQs.size() == q.size());
+ QIndex qStart; int nq; findMobilizerQs(state, qStart, nq);
+ const QIndex qEnd(qStart+nq);
+ for (QIndex qx=qStart; qx != qEnd; ++qx)
+ iv.lockedQs[qx] = q[qx];
+ }
+ else if (level == Motion::Velocity) {
+ const Vector& u = state.getU();
+ assert(iv.lockedUs.size() == u.size());
+ for (UIndex ux=uStart; ux != uEnd; ++ux)
+ iv.lockedUs[ux] = u[ux];
+ }
+ else if (level == Motion::Acceleration) {
+ for (UIndex ux=uStart; ux != uEnd; ++ux)
+ iv.lockedUs[ux] = 0;
+ }
+}
+
+void MobilizedBodyImpl::
+lockAt(State& state, int n, const Real* value, Motion::Level level) const {
+ SimTK_STAGECHECK_GE_ALWAYS(getMyMatterSubsystemRep().getStage(state),
+ Stage::Model, "MobilizedBody::lockAt()");
+
+ SBInstanceVars& iv = getMyMatterSubsystemRep().updInstanceVars(state);
+ iv.mobilizerLockLevel[getMyMobilizedBodyIndex()] = level;
+ UIndex uStart; int nu; findMobilizerUs(state, uStart, nu);
+ const UIndex uEnd(uStart+nu);
+
+ if (level == Motion::Position) {
+ Vector& u = state.updU(); // set generalized speeds to zero
+ for (UIndex ux=uStart; ux != uEnd; ++ux)
+ u[ux] = 0;
+ Vector& q = state.updQ();
+ QIndex qStart; int nq; findMobilizerQs(state, qStart, nq);
+ const QIndex qEnd(qStart+nq);
+ SimTK_ERRCHK3_ALWAYS(n==nq, "MobilizedBody::lockAt()",
+ "Supplied value had wrong length %d for locking at "
+ "%s level; should have been nq=%d.",
+ n, Motion::nameOfLevel(level), nq);
+ for (QIndex qx=qStart; qx != qEnd; ++qx)
+ q[qx] = iv.lockedQs[qx] = *value++; // set qs now & remember
+ }
+ else if (level == Motion::Velocity || level == Motion::Acceleration) {
+ SimTK_ERRCHK3_ALWAYS(n==nu, "MobilizedBody::lockAt()",
+ "Supplied value had wrong length %d for locking at "
+ "%s level; should have been nu=%d.",
+ n, Motion::nameOfLevel(level), nu);
+ for (UIndex ux=uStart; ux != uEnd; ++ux)
+ iv.lockedUs[ux] = *value++;
+ }
+}
+
+Vector MobilizedBodyImpl::
+getLockValueAsVector(const State& state) const {
+ const SBInstanceVars& iv = getMyMatterSubsystemRep().getInstanceVars(state);
+ const Motion::Level level =
+ iv.mobilizerLockLevel[getMyMobilizedBodyIndex()];
+ Vector value;
+ if (level == Motion::Position) {
+ QIndex qStart; int nq; findMobilizerQs(state, qStart, nq);
+ value.resize(nq);
+ for (int i=0; i < nq; ++i)
+ value[i] = iv.lockedQs[QIndex(qStart+i)];
+ } else if (level == Motion::Velocity || level == Motion::Acceleration) {
+ UIndex uStart; int nu; findMobilizerUs(state, uStart, nu);
+ value.resize(nu);
+ for (int i=0; i < nu; ++i)
+ value[i] = iv.lockedUs[UIndex(uStart+i)];
+ }
+ return value;
+}
+
+
+// OK to do this after realize(Topology) since we're not looking at q or u.
+// If this was a lock-by-default mobilizer, this will unlock it since the
+// lock-by-default flag is transferred to InstanceVars during realize(Topology).
+void MobilizedBodyImpl::unlock(State& state) const {
+ SimTK_STAGECHECK_GE_ALWAYS(getMyMatterSubsystemRep().getStage(state),
+ Stage::Topology, "MobilizedBody::unlock()");
+
+ SBInstanceVars& iv = getMyMatterSubsystemRep().updInstanceVars(state);
+ iv.mobilizerLockLevel[getMyMobilizedBodyIndex()] = Motion::NoLevel;
+}
+Motion::Level MobilizedBodyImpl::getLockLevel(const State& state) const {
+ const SBInstanceVars& iv = getMyMatterSubsystemRep().getInstanceVars(state);
+ return iv.mobilizerLockLevel[getMyMobilizedBodyIndex()];
+}
+
+Motion::Method MobilizedBodyImpl::getQMotionMethod(const State& s) const
+{ return getMyInstanceInfo(s).qMethod; }
+Motion::Method MobilizedBodyImpl::getUMotionMethod(const State& s) const
+{ return getMyInstanceInfo(s).uMethod; }
+Motion::Method MobilizedBodyImpl::getUDotMotionMethod(const State& s) const
+{ return getMyInstanceInfo(s).udotMethod; }
+
+void MobilizedBodyImpl::
+copyOutDefaultQ(const State& s, Vector& qDefault) const {
+ SimTK_STAGECHECK_GE_ALWAYS(getMyMatterSubsystemRep().getStage(s),
+ Stage::Topology, "MobilizedBody::copyOutDefaultQ()");
+ QIndex qStart; int nq;
+ findMobilizerQs(s, qStart, nq);
+ if (nq) // nq may vary due to modeling options, e.g. 3 or 4 for Ball
+ copyOutDefaultQImpl(nq, &qDefault[qStart]);
+}
+
+ // TODO: currently we delegate these requests to the RigidBodyNodes.
+ // Probably most of this functionality should be handled directly
+ // by the MobilizedBody objects.
+
+void MobilizedBodyImpl::setQToFitTransform(State& s, const Transform& X_MbM) const {
+ const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
+ const SBStateDigest digest(s, matterRep, Stage::Instance);
+ Vector& q = matterRep.updQ(s);
+ return getMyRigidBodyNode().setQToFitTransform(digest, X_MbM, q);
+}
+void MobilizedBodyImpl::setQToFitRotation(State& s, const Rotation& R_MbM) const {
+ const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
+ const SBStateDigest digest(s, matterRep, Stage::Instance);
+ Vector& q = matterRep.updQ(s);
+ return getMyRigidBodyNode().setQToFitRotation(digest, R_MbM, q);
+}
+void MobilizedBodyImpl::setQToFitTranslation(State& s, const Vec3& p_MbM) const
+{
+ const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
+ const SBStateDigest digest(s, matterRep, Stage::Instance);
+ Vector& q = matterRep.updQ(s);
+ return getMyRigidBodyNode().setQToFitTranslation(digest, p_MbM, q);
+}
+
+void MobilizedBodyImpl::setUToFitVelocity(State& s, const SpatialVec& V_MbM) const {
+ const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
+ const SBStateDigest digest(s, matterRep, Stage::Instance);
+ const Vector& q = matterRep.updQ(s);
+ Vector& u = matterRep.updU(s);
+ return getMyRigidBodyNode().setUToFitVelocity(digest, q, V_MbM, u);
+}
+void MobilizedBodyImpl::setUToFitAngularVelocity(State& s, const Vec3& w_MbM) const {
+ const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
+ const SBStateDigest digest(s, matterRep, Stage::Instance);
+ const Vector& q = matterRep.updQ(s);
+ Vector& u = matterRep.updU(s);
+ return getMyRigidBodyNode().setUToFitAngularVelocity(digest, q, w_MbM, u);
+}
+void MobilizedBodyImpl::setUToFitLinearVelocity(State& s, const Vec3& v_MbM) const
+{
+ const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
+ const SBStateDigest digest(s, matterRep, Stage::Instance);
+ const Vector& q = matterRep.updQ(s);
+ Vector& u = matterRep.updU(s);
+ return getMyRigidBodyNode().setUToFitLinearVelocity(digest, q, v_MbM, u);
+}
+
+ // REALIZE TOPOLOGY
+const RigidBodyNode& MobilizedBodyImpl::realizeTopology
+ (State& s, UIndex& nxtU, USquaredIndex& nxtUSq, QIndex& nxtQ) const
+{
+ delete myRBnode;
+ myRBnode = createRigidBodyNode(nxtU,nxtUSq,nxtQ);
+
+ assert(myMobilizedBodyIndex.isValid());
+ assert(myParentIndex.isValid() || myMobilizedBodyIndex == GroundIndex);
+
+ if (myParentIndex.isValid()) {
+ // not ground
+ const MobilizedBodyImpl& parent =
+ myMatterSubsystemRep->getMobilizedBody(myParentIndex).getImpl();
+ assert(myLevel == parent.myRBnode->getLevel() + 1);
+ parent.myRBnode->addChild(myRBnode);
+ myRBnode->setParent(parent.myRBnode);
+ }
+
+ myRBnode->setLevel(myLevel);
+ myRBnode->setNodeNum(myMobilizedBodyIndex);
+
+ // Realize Motion topology.
+ if (hasMotion())
+ getMotion().getImpl().realizeTopology(s);
+
+ // Realize MobilizedBody-specific topology.
+ realizeTopologyVirtual(s);
+ return *myRBnode;
+}
+
+//------------------------------------------------------------------------------
+// REALIZE MODEL
+//------------------------------------------------------------------------------
+void MobilizedBodyImpl::realizeModel(State& s) const {
+ if (hasMotion())
+ getMotion().getImpl().realizeModel(s);
+ realizeModelVirtual(s);
+}
+
+//------------------------------------------------------------------------------
+// REALIZE INSTANCE
+//------------------------------------------------------------------------------
+void MobilizedBodyImpl::realizeInstance(const SBStateDigest& sbs) const {
+ if (hasMotion())
+ getMotion().getImpl().realizeInstance(sbs.getState());
+ realizeInstanceVirtual(sbs.getState());
+}
+
+//------------------------------------------------------------------------------
+// REALIZE TIME
+//------------------------------------------------------------------------------
+void MobilizedBodyImpl::realizeTime(const SBStateDigest& sbs) const {
+ const MobilizedBodyIndex mbx = getMyMobilizedBodyIndex();
+ const SBInstanceVars& iv = sbs.getInstanceVars();
+ const SBInstanceCache& ic = sbs.getInstanceCache();
+ const SBInstancePerMobodInfo& instInfo = ic.getMobodInstanceInfo(mbx);
+
+ // Note that we only need to deal with explicitly prescribed motion;
+ // if the mobilizer is prescribed to zero it will be dealt with elsewhere.
+ // A mobilizer can be prescribed either due to a locked position or to
+ // an explicit Motion.
+ if (instInfo.qMethod==Motion::Prescribed) {
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBModelPerMobodInfo& modelInfo = mc.getMobodModelInfo(mbx);
+ const int nq = modelInfo.nQInUse;
+ const QIndex qStart = modelInfo.firstQIndex;
+ const PresQPoolIndex pqx = instInfo.firstPresQ;
+ SBTimeCache& tc = sbs.updTimeCache();
+
+ if (iv.mobilizerLockLevel[mbx] != Motion::NoLevel) {
+ // Positions are prescribed because of a lock.
+ assert(iv.mobilizerLockLevel[mbx] == Motion::Position);
+ for (int i=0; i < nq; ++i)
+ tc.presQPool[pqx+i] = iv.lockedQs[QIndex(qStart+i)];
+ } else {
+ // Positions are prescribed because of a Motion.
+ assert(hasMotion() && !iv.prescribedMotionIsDisabled[mbx]);
+ const MotionImpl& motion = getMotion().getImpl();
+ motion.calcPrescribedPosition(sbs.getState(), nq,
+ &tc.presQPool[pqx]);
+ }
+ }
+
+ realizeTimeVirtual(sbs.getState());
+}
+
+//------------------------------------------------------------------------------
+// REALIZE POSITION
+//------------------------------------------------------------------------------
+void MobilizedBodyImpl::realizePosition(const SBStateDigest& sbs) const {
+ const MobilizedBodyIndex mbx = getMyMobilizedBodyIndex();
+ const SBInstanceVars& iv = sbs.getInstanceVars();
+ const SBInstanceCache& ic = sbs.getInstanceCache();
+ const SBInstancePerMobodInfo& instInfo = ic.getMobodInstanceInfo(mbx);
+
+ // Note that we only need to deal with explicitly prescribed motion;
+ // if the u is prescribed to zero it will be dealt with elsewhere.
+ // Prescribed u may be due to nonholonomic prescribed u, or to derivative
+ // of holonomic prescribed q, or to a lock(Velocity).
+ if (instInfo.uMethod==Motion::Prescribed) {
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBModelPerMobodInfo& modelInfo = mc.getMobodModelInfo(mbx);
+ const int nu = modelInfo.nUInUse;
+ const UIndex uStart = modelInfo.firstUIndex;
+ const PresUPoolIndex pux = instInfo.firstPresU;
+ SBConstrainedPositionCache& cpc = sbs.updConstrainedPositionCache();
+
+ if (iv.mobilizerLockLevel[mbx] != Motion::NoLevel) {
+ // Velocities are prescribed because of a lock.
+ assert(iv.mobilizerLockLevel[mbx] == Motion::Velocity);
+ for (int i=0; i < nu; ++i)
+ cpc.presUPool[pux+i] = iv.lockedUs[UIndex(uStart+i)];
+ } else {
+ // Velocities are prescribed because of a Motion.
+ assert(hasMotion() && !iv.prescribedMotionIsDisabled[mbx]);
+ const MotionImpl& motion = getMotion().getImpl();
+
+ if (instInfo.qMethod==Motion::Prescribed) {
+ // Holonomic
+ const int nq = modelInfo.nQInUse;
+ const RigidBodyNode& rbn = getMyRigidBodyNode();
+
+ if (rbn.isQDotAlwaysTheSameAsU()) {
+ assert(nq==nu);
+ motion.calcPrescribedPositionDot(sbs.getState(), nu,
+ &cpc.presUPool[pux]);
+ } else {
+ Real qdot[8]; // we won't use all of these -- max is 7
+ motion.calcPrescribedPositionDot(sbs.getState(), nq, qdot);
+ // u = N^-1 qdot
+ rbn.multiplyByNInv(sbs, false, qdot, &cpc.presUPool[pux]);
+ }
+ } else { // Non-holonomic
+ motion.calcPrescribedVelocity(sbs.getState(), nu,
+ &cpc.presUPool[pux]);
+ }
+ }
+ }
+
+ realizePositionVirtual(sbs.getState());
+}
+
+//------------------------------------------------------------------------------
+// REALIZE VELOCITY
+//------------------------------------------------------------------------------
+void MobilizedBodyImpl::realizeVelocity(const SBStateDigest& sbs) const {
+ // We don't deal with prescribed accelerations until Dynamics stage.
+ realizeVelocityVirtual(sbs.getState());
+}
+
+//------------------------------------------------------------------------------
+// REALIZE DYNAMICS
+//------------------------------------------------------------------------------
+void MobilizedBodyImpl::realizeDynamics(const SBStateDigest& sbs) const {
+ const MobilizedBodyIndex mbx = getMyMobilizedBodyIndex();
+ const SBInstanceVars& iv = sbs.getInstanceVars();
+ const SBInstanceCache& ic = sbs.getInstanceCache();
+ const SBInstancePerMobodInfo& instInfo = ic.getMobodInstanceInfo(mbx);
+
+ // Note that we only need to deal with explicitly prescribed motion;
+ // if the udot is prescribed to zero it will be dealt with elsewhere.
+ // Prescribed udot may be due to acceleration-only prescribed udot,
+ // derivative of nonholonomic prescribed u, or to 2nd derivative
+ // of holonomic prescribed q, or to a lock(Acceleration).
+ if (instInfo.udotMethod==Motion::Prescribed) {
+
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBModelPerMobodInfo& modelInfo = mc.getMobodModelInfo(mbx);
+ const int nu = modelInfo.nUInUse;
+ const UIndex uStart = modelInfo.firstUIndex;
+ const PresUDotPoolIndex pudx = instInfo.firstPresUDot;
+ SBDynamicsCache& dc = sbs.updDynamicsCache();
+ Real* presUDotp = &dc.presUDotPool[pudx];
+
+ if (iv.mobilizerLockLevel[mbx] != Motion::NoLevel) {
+ // Accelerations are prescribed because of a lock.
+ assert(iv.mobilizerLockLevel[mbx] == Motion::Acceleration);
+ for (int i=0; i < nu; ++i)
+ presUDotp[i] = iv.lockedUs[UIndex(uStart+i)];
+ } else {
+ // Accelerations are prescribed because of a Motion.
+ assert(hasMotion() && !iv.prescribedMotionIsDisabled[mbx]);
+ const MotionImpl& motion = getMotion().getImpl();
+
+ if (instInfo.qMethod==Motion::Prescribed) {
+ // Holonomic
+ const int nq = modelInfo.nQInUse;
+ const RigidBodyNode& rbn = getMyRigidBodyNode();
+
+ if (rbn.isQDotAlwaysTheSameAsU()) {
+ assert(nq==nu);
+ motion.calcPrescribedPositionDotDot(sbs.getState(), nu,
+ presUDotp);
+ } else {
+ Real ndotU[8]; // remainder term NDot*u (nq; max is 7)
+ const Vector& u = sbs.getU();
+ rbn.multiplyByNDot(sbs, false, &u[uStart], ndotU);
+
+ Real qdotdot[8]; // nq of these -- max is 7
+ motion.calcPrescribedPositionDotDot(sbs.getState(), nq,
+ qdotdot);
+
+ for (int i=0; i < nq; ++i)
+ qdotdot[i] -= ndotU[i]; // velocity correction
+
+ // udot = N^-1 (qdotdot - NDot*u)
+ rbn.multiplyByNInv(sbs, false, qdotdot, presUDotp);
+ }
+ } else if (instInfo.uMethod==Motion::Prescribed) {
+ // Non-holonomic
+ motion.calcPrescribedVelocityDot(sbs.getState(), nu, presUDotp);
+ } else {
+ // Acceleration-only
+ motion.calcPrescribedAcceleration(sbs.getState(), nu, presUDotp);
+ }
+ }
+ }
+
+ realizeDynamicsVirtual(sbs.getState());
+}
+
+//------------------------------------------------------------------------------
+// REALIZE ACCELERATION
+//------------------------------------------------------------------------------
+void MobilizedBodyImpl::realizeAcceleration(const SBStateDigest& sbs) const {
+ realizeAccelerationVirtual(sbs.getState());
+}
+
+//------------------------------------------------------------------------------
+// REALIZE REPORT
+//------------------------------------------------------------------------------
+void MobilizedBodyImpl::realizeReport(const SBStateDigest& sbs) const {
+ realizeReportVirtual(sbs.getState());
+}
+
+ /////////////////////////
+ // MOBILIZED BODY::PIN //
+ /////////////////////////
+
+MobilizedBody::Pin::Pin(MobilizedBody& parent, const Body& body, Direction d)
+: MobilizedBody(new PinImpl(d)) {
+ // inb & outb frames are just the parent body's frame and new body's frame
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Pin::Pin(MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame, Direction d)
+: MobilizedBody(new PinImpl(d)) {
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+
+Real MobilizedBody::Pin::getDefaultQ() const {
+ return getImpl().defaultQ;
+}
+
+MobilizedBody::Pin& MobilizedBody::Pin::setDefaultQ(Real q) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultQ = q;
+ return *this;
+}
+
+Real MobilizedBody::Pin::getQ(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 1);
+ return mbr.getMyMatterSubsystemRep().getQ(s)[qStart];
+}
+void MobilizedBody::Pin::setQ(State& s, Real q) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 1);
+ mbr.getMyMatterSubsystemRep().updQ(s)[qStart] = q;
+}
+Real MobilizedBody::Pin::getQDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 1);
+ return mbr.getMyMatterSubsystemRep().getQDot(s)[qStart];
+}
+Real MobilizedBody::Pin::getQDotDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 1);
+ return mbr.getMyMatterSubsystemRep().getQDotDot(s)[qStart];
+}
+
+
+Real MobilizedBody::Pin::getU(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 1);
+ return mbr.getMyMatterSubsystemRep().getU(s)[uStart];
+}
+void MobilizedBody::Pin::setU(State& s, Real u) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 1);
+ mbr.getMyMatterSubsystemRep().updU(s)[uStart] = u;
+}
+Real MobilizedBody::Pin::getUDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 1);
+ return mbr.getMyMatterSubsystemRep().getUDot(s)[uStart];
+}
+
+Real MobilizedBody::Pin::getMyPartQ(const State& s, const Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq == 1);
+ return qlike[qStart];
+}
+
+Real MobilizedBody::Pin::getMyPartU(const State& s, const Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu == 1);
+ return ulike[uStart];
+}
+
+Real& MobilizedBody::Pin::updMyPartQ(const State& s, Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq == 1);
+ return qlike[qStart];
+}
+
+Real& MobilizedBody::Pin::updMyPartU(const State& s, Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu == 1);
+ return ulike[uStart];
+}
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(MobilizedBody::Pin,
+ MobilizedBody::PinImpl, MobilizedBody);
+
+ ////////////////////////////
+ // MOBILIZED BODY::SLIDER //
+ ////////////////////////////
+
+MobilizedBody::Slider::Slider(MobilizedBody& parent, const Body& body, Direction d)
+: MobilizedBody(new SliderImpl(d)) {
+ // inb & outb frames are just the parent body's frame and new body's frame
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Slider::Slider(MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame, Direction d)
+: MobilizedBody(new SliderImpl(d)) {
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+Real MobilizedBody::Slider::getDefaultQ() const {
+ return getImpl().defaultQ;
+}
+
+MobilizedBody::Slider& MobilizedBody::Slider::setDefaultQ(Real q) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultQ = q;
+ return *this;
+}
+
+Real MobilizedBody::Slider::getQ(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 1);
+ return mbr.getMyMatterSubsystemRep().getQ(s)[qStart];
+}
+void MobilizedBody::Slider::setQ(State& s, Real q) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 1);
+ mbr.getMyMatterSubsystemRep().updQ(s)[qStart] = q;
+}
+Real MobilizedBody::Slider::getQDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 1);
+ return mbr.getMyMatterSubsystemRep().getQDot(s)[qStart];
+}
+Real MobilizedBody::Slider::getQDotDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 1);
+ return mbr.getMyMatterSubsystemRep().getQDotDot(s)[qStart];
+}
+
+
+Real MobilizedBody::Slider::getU(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 1);
+ return mbr.getMyMatterSubsystemRep().getU(s)[uStart];
+}
+void MobilizedBody::Slider::setU(State& s, Real u) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 1);
+ mbr.getMyMatterSubsystemRep().updU(s)[uStart] = u;
+}
+Real MobilizedBody::Slider::getUDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 1);
+ return mbr.getMyMatterSubsystemRep().getUDot(s)[uStart];
+}
+
+Real MobilizedBody::Slider::getMyPartQ(const State& s, const Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq == 1);
+ return qlike[qStart];
+}
+
+Real MobilizedBody::Slider::getMyPartU(const State& s, const Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu == 1);
+ return ulike[uStart];
+}
+
+Real& MobilizedBody::Slider::updMyPartQ(const State& s, Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq == 1);
+ return qlike[qStart];
+}
+
+Real& MobilizedBody::Slider::updMyPartU(const State& s, Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu == 1);
+ return ulike[uStart];
+}
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(MobilizedBody::Slider,
+ MobilizedBody::SliderImpl, MobilizedBody);
+
+ ///////////////////////////////
+ // MOBILIZED BODY::UNIVERSAL //
+ ///////////////////////////////
+
+MobilizedBody::Universal::Universal
+ (MobilizedBody& parent, const Body& body, Direction d)
+: MobilizedBody(new UniversalImpl(d)) {
+ // inb & outb frames are just the parent body's frame and new body's frame
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Universal::Universal
+ (MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame, Direction d)
+: MobilizedBody(new UniversalImpl(d)) {
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(MobilizedBody::Universal,
+ MobilizedBody::UniversalImpl, MobilizedBody);
+
+ //////////////////////////////
+ // MOBILIZED BODY::CYLINDER //
+ //////////////////////////////
+
+MobilizedBody::Cylinder::Cylinder
+ (MobilizedBody& parent, const Body& body, Direction d)
+: MobilizedBody(new CylinderImpl(d)) {
+ // inb & outb frames are just the parent body's frame and new body's frame
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Cylinder::Cylinder
+ (MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame, Direction d)
+: MobilizedBody(new CylinderImpl(d)) {
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(MobilizedBody::Cylinder,
+ MobilizedBody::CylinderImpl, MobilizedBody);
+
+ //////////////////////////////////
+ // MOBILIZED BODY::BEND STRETCH //
+ //////////////////////////////////
+
+MobilizedBody::BendStretch::BendStretch
+ (MobilizedBody& parent, const Body& body, Direction d)
+: MobilizedBody(new BendStretchImpl(d)) {
+ // inb & outb frames are just the parent body's frame and new body's frame
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::BendStretch::BendStretch
+ (MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame, Direction d)
+: MobilizedBody(new BendStretchImpl(d)) {
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(MobilizedBody::BendStretch,
+ MobilizedBody::BendStretchImpl, MobilizedBody);
+
+ ////////////////////////////
+ // MOBILIZED BODY::PLANAR //
+ ////////////////////////////
+
+MobilizedBody::Planar::Planar
+ (MobilizedBody& parent, const Body& body, Direction d)
+: MobilizedBody(new PlanarImpl(d)) {
+ // inb & outb frames are just the parent body's frame and new body's frame
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Planar::Planar(MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame,
+ Direction d)
+: MobilizedBody(new PlanarImpl(d)) {
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+const Vec3& MobilizedBody::Planar::getDefaultQ() const {
+ return getImpl().defaultQ;
+}
+MobilizedBody::Planar& MobilizedBody::Planar::setDefaultQ(const Vec3& q) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultQ = q;
+ return *this;
+}
+
+const Vec3& MobilizedBody::Planar::getQ(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getQ(s)[qStart]);
+}
+void MobilizedBody::Planar::setQ(State& s, const Vec3& q) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ Vec3::updAs(&mbr.getMyMatterSubsystemRep().updQ(s)[qStart]) = q;
+}
+const Vec3& MobilizedBody::Planar::getQDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getQDot(s)[qStart]);
+}
+const Vec3& MobilizedBody::Planar::getQDotDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getQDotDot(s)[qStart]);
+}
+
+
+const Vec3& MobilizedBody::Planar::getU(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getU(s)[uStart]);
+}
+void MobilizedBody::Planar::setU(State& s, const Vec3& u) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ Vec3::updAs(&mbr.getMyMatterSubsystemRep().updU(s)[uStart]) = u;
+}
+const Vec3& MobilizedBody::Planar::getUDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getUDot(s)[uStart]);
+}
+
+const Vec3& MobilizedBody::Planar::getMyPartQ(const State& s, const Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ return Vec3::getAs(&qlike[qStart]);
+}
+
+const Vec3& MobilizedBody::Planar::getMyPartU(const State& s, const Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec3::getAs(&ulike[uStart]);
+}
+
+Vec3& MobilizedBody::Planar::updMyPartQ(const State& s, Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ return Vec3::updAs(&qlike[qStart]);
+}
+
+Vec3& MobilizedBody::Planar::updMyPartU(const State& s, Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec3::updAs(&ulike[uStart]);
+}
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(MobilizedBody::Planar,
+ MobilizedBody::PlanarImpl, MobilizedBody);
+
+
+ //////////////////////////////////////
+ // MOBILIZED BODY::SPHERICAL COORDS //
+ //////////////////////////////////////
+
+MobilizedBody::SphericalCoords::SphericalCoords
+ (MobilizedBody& parent, const Body& body, Direction d)
+: MobilizedBody(new SphericalCoordsImpl(d)) {
+ // inb & outb frames are just the parent body's frame and new body's frame
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::SphericalCoords::SphericalCoords
+ (MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame,
+ Direction d)
+: MobilizedBody(new SphericalCoordsImpl(d)) {
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::SphericalCoords::SphericalCoords
+ (MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame,
+ Real azimuthOffset, bool azimuthNegated,
+ Real zenithOffset, bool zenithNegated,
+ CoordinateAxis radialAxis, bool radialNegated,
+ Direction d)
+: MobilizedBody(new SphericalCoordsImpl(azimuthOffset, azimuthNegated,
+ zenithOffset, zenithNegated,
+ radialAxis, radialNegated,
+ d))
+{
+ SimTK_APIARGCHECK_ALWAYS(radialAxis != YAxis, "MobilizedBody::SphericalCoords", "ctor",
+ "SphericalCoords translation (radial) axis must be X or Z; Y is not allowed.");
+
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::SphericalCoords&
+MobilizedBody::SphericalCoords::setRadialAxis(CoordinateAxis axis) {
+ SimTK_APIARGCHECK_ALWAYS(axis != YAxis,
+ "MobilizedBody::SphericalCoords", "setRadialAxis",
+ "SphericalCoords translation (radial) axis must be X or Z; Y is not allowed.");
+ getImpl().invalidateTopologyCache();
+ updImpl().axisT = axis;
+ return *this;
+}
+
+MobilizedBody::SphericalCoords&
+MobilizedBody::SphericalCoords::setNegateAzimuth(bool shouldNegate) {
+ getImpl().invalidateTopologyCache();
+ updImpl().negAz = shouldNegate;
+ return *this;
+}
+MobilizedBody::SphericalCoords&
+MobilizedBody::SphericalCoords::setNegateZenith(bool shouldNegate) {
+ getImpl().invalidateTopologyCache();
+ updImpl().negZe = shouldNegate;
+ return *this;
+}
+MobilizedBody::SphericalCoords&
+MobilizedBody::SphericalCoords::setNegateRadial(bool shouldNegate) {
+ getImpl().invalidateTopologyCache();
+ updImpl().negT = shouldNegate;
+ return *this;
+}
+
+CoordinateAxis MobilizedBody::SphericalCoords::getRadialAxis() const
+{ return getImpl().axisT; }
+bool MobilizedBody::SphericalCoords::isAzimuthNegated() const
+{ return getImpl().negAz; }
+bool MobilizedBody::SphericalCoords::isZenithNegated() const
+{ return getImpl().negZe; }
+bool MobilizedBody::SphericalCoords::isRadialNegated() const
+{ return getImpl().negT; }
+
+const Vec3& MobilizedBody::SphericalCoords::getDefaultQ() const {
+ return getImpl().defaultQ;
+}
+MobilizedBody::SphericalCoords& MobilizedBody::SphericalCoords::
+setDefaultQ(const Vec3& q) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultQ = q;
+ return *this;
+}
+
+const Vec3& MobilizedBody::SphericalCoords::getQ(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getQ(s)[qStart]);
+}
+void MobilizedBody::SphericalCoords::setQ(State& s, const Vec3& q) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ Vec3::updAs(&mbr.getMyMatterSubsystemRep().updQ(s)[qStart]) = q;
+}
+const Vec3& MobilizedBody::SphericalCoords::getQDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getQDot(s)[qStart]);
+}
+const Vec3& MobilizedBody::SphericalCoords::getQDotDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getQDotDot(s)[qStart]);
+}
+
+
+const Vec3& MobilizedBody::SphericalCoords::getU(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getU(s)[uStart]);
+}
+void MobilizedBody::SphericalCoords::setU(State& s, const Vec3& u) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ Vec3::updAs(&mbr.getMyMatterSubsystemRep().updU(s)[uStart]) = u;
+}
+const Vec3& MobilizedBody::SphericalCoords::getUDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getUDot(s)[uStart]);
+}
+
+const Vec3& MobilizedBody::SphericalCoords::
+getMyPartQ(const State& s, const Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq==3);
+ return Vec3::getAs(&qlike[qStart]);
+}
+
+const Vec3& MobilizedBody::SphericalCoords::
+getMyPartU(const State& s, const Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu==3);
+ return Vec3::getAs(&ulike[uStart]);
+}
+
+Vec3& MobilizedBody::SphericalCoords::
+updMyPartQ(const State& s, Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq==3);
+ return Vec3::updAs(&qlike[qStart]);
+}
+
+Vec3& MobilizedBody::SphericalCoords::
+updMyPartU(const State& s, Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu==3);
+ return Vec3::updAs(&ulike[uStart]);
+}
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(MobilizedBody::SphericalCoords,
+ MobilizedBody::SphericalCoordsImpl, MobilizedBody);
+
+ ////////////////////////////
+ // MOBILIZED BODY::GIMBAL //
+ ////////////////////////////
+
+MobilizedBody::Gimbal::Gimbal
+ (MobilizedBody& parent, const Body& body, Direction d)
+: MobilizedBody(new GimbalImpl(d)) {
+ // inb & outb frames are just the parent body's frame and new body's frame
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Gimbal::Gimbal
+ (MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame, Direction d)
+: MobilizedBody(new GimbalImpl(d)) {
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Gimbal& MobilizedBody::Gimbal::setDefaultRadius(Real r) {
+ getImpl().invalidateTopologyCache();
+ updImpl().setDefaultRadius(r);
+ return *this;
+}
+
+Real MobilizedBody::Gimbal::getDefaultRadius() const {
+ return getImpl().getDefaultRadius();
+}
+
+const Vec3& MobilizedBody::Gimbal::getDefaultQ() const {
+ return getImpl().defaultQ;
+}
+MobilizedBody::Gimbal& MobilizedBody::Gimbal::setDefaultQ(const Vec3& q) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultQ = q;
+ return *this;
+}
+
+const Vec3& MobilizedBody::Gimbal::getQ(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getQ(s)[qStart]);
+}
+void MobilizedBody::Gimbal::setQ(State& s, const Vec3& q) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ Vec3::updAs(&mbr.getMyMatterSubsystemRep().updQ(s)[qStart]) = q;
+}
+const Vec3& MobilizedBody::Gimbal::getQDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getQDot(s)[qStart]);
+}
+const Vec3& MobilizedBody::Gimbal::getQDotDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getQDotDot(s)[qStart]);
+}
+
+
+const Vec3& MobilizedBody::Gimbal::getU(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getU(s)[uStart]);
+}
+void MobilizedBody::Gimbal::setU(State& s, const Vec3& u) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ Vec3::updAs(&mbr.getMyMatterSubsystemRep().updU(s)[uStart]) = u;
+}
+const Vec3& MobilizedBody::Gimbal::getUDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getUDot(s)[uStart]);
+}
+
+const Vec3& MobilizedBody::Gimbal::
+getMyPartQ(const State& s, const Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq==3);
+ return Vec3::getAs(&qlike[qStart]);
+}
+
+const Vec3& MobilizedBody::Gimbal::
+getMyPartU(const State& s, const Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu==3);
+ return Vec3::getAs(&ulike[uStart]);
+}
+
+Vec3& MobilizedBody::Gimbal::
+updMyPartQ(const State& s, Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq==3);
+ return Vec3::updAs(&qlike[qStart]);
+}
+
+Vec3& MobilizedBody::Gimbal::
+updMyPartU(const State& s, Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu==3);
+ return Vec3::updAs(&ulike[uStart]);
+}
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(MobilizedBody::Gimbal,
+ MobilizedBody::GimbalImpl, MobilizedBody);
+
+ // GimbalImpl
+
+void MobilizedBody::GimbalImpl::calcDecorativeGeometryAndAppendImpl
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+{
+ // Call the superclass implementation to get standard default geometry.
+
+ MobilizedBodyImpl::calcDecorativeGeometryAndAppendImpl(s, stage, geom);
+
+ // We can't generate the ball until we know the radius, and we can't place
+ // the geometry on the body until we know the parent and child mobilizer frame
+ // placement on the body, which might not be until Instance stage.
+ if (stage == Stage::Instance && getMyMatterSubsystemRep().getShowDefaultGeometry())
+ {
+ const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
+ const Transform& X_PMb = getInboardFrame(s);
+ const Transform& X_BM = getOutboardFrame(s);
+
+ // On the inboard body, draw a solid sphere and a wireframe one attached to it for
+ // easier visualization of its rotation. These are at about 90% of the radius.
+ geom.push_back(DecorativeSphere(Real(0.92)*getDefaultRadius())
+ .setColor(Gray)
+ .setRepresentation(DecorativeGeometry::DrawSurface)
+ .setOpacity(Real(0.5))
+ .setResolution(Real(0.75))
+ .setBodyId(getMyParentMobilizedBodyIndex())
+ .setTransform(X_PMb));
+ geom.push_back(DecorativeSphere(Real(0.90)*getDefaultRadius())
+ .setColor(White)
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setResolution(Real(0.75))
+ .setLineThickness(3)
+ .setOpacity(Real(0.1))
+ .setBodyId(getMyParentMobilizedBodyIndex())
+ .setTransform(X_PMb));
+
+ // On the outboard body draw an orange mesh sphere at the ball radius.
+ geom.push_back(DecorativeSphere(getDefaultRadius())
+ .setColor(Orange)
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setOpacity(0.5)
+ .setResolution(0.5)
+ .setBodyId(getMyMobilizedBodyIndex())
+ .setTransform(X_BM));
+ }
+}
+
+ /////////////////////////////
+ // MOBILIZED BODY::BUSHING //
+ /////////////////////////////
+
+MobilizedBody::Bushing::Bushing(MobilizedBody& parent, const Body& body,
+ Direction d)
+: MobilizedBody(new BushingImpl(d)) {
+ // inb & outb frames are just the parent body's frame and new body's frame
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Bushing::Bushing(MobilizedBody& parent, const Transform& inbFrame,
+ const Body& bodyInfo, const Transform& outbFrame,
+ Direction d)
+: MobilizedBody(new BushingImpl(d)) {
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+ setBody(bodyInfo);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+const Vec6& MobilizedBody::Bushing::getDefaultQ() const
+{ return getImpl().defaultQ; }
+
+MobilizedBody::Bushing& MobilizedBody::Bushing::setDefaultQ(const Vec6& q) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultQ = q;
+ return *this;
+}
+
+const Vec6& MobilizedBody::Bushing::getQ(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 6);
+ return Vec6::getAs(&mbr.getMyMatterSubsystemRep().getQ(s)[qStart]);
+}
+void MobilizedBody::Bushing::setQ(State& s, const Vec6& q) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 6);
+ Vec6::updAs(&mbr.getMyMatterSubsystemRep().updQ(s)[qStart]) = q;
+}
+const Vec6& MobilizedBody::Bushing::getQDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 6);
+ return Vec6::getAs(&mbr.getMyMatterSubsystemRep().getQDot(s)[qStart]);
+}
+const Vec6& MobilizedBody::Bushing::getQDotDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 6);
+ return Vec6::getAs(&mbr.getMyMatterSubsystemRep().getQDotDot(s)[qStart]);
+}
+
+
+const Vec6& MobilizedBody::Bushing::getU(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 6);
+ return Vec6::getAs(&mbr.getMyMatterSubsystemRep().getU(s)[uStart]);
+}
+void MobilizedBody::Bushing::setU(State& s, const Vec6& u) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 6);
+ Vec6::updAs(&mbr.getMyMatterSubsystemRep().updU(s)[uStart]) = u;
+}
+const Vec6& MobilizedBody::Bushing::getUDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 6);
+ return Vec6::getAs(&mbr.getMyMatterSubsystemRep().getUDot(s)[uStart]);
+}
+
+const Vec6& MobilizedBody::Bushing::
+getMyPartQ(const State& s, const Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq==6);
+ return Vec6::getAs(&qlike[qStart]);
+}
+
+const Vec6& MobilizedBody::Bushing::
+getMyPartU(const State& s, const Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu==6);
+ return Vec6::getAs(&ulike[uStart]);
+}
+
+Vec6& MobilizedBody::Bushing::
+updMyPartQ(const State& s, Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq==6);
+ return Vec6::updAs(&qlike[qStart]);
+}
+
+Vec6& MobilizedBody::Bushing::
+updMyPartU(const State& s, Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu==6);
+ return Vec6::updAs(&ulike[uStart]);
+}
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(MobilizedBody::Bushing,
+ MobilizedBody::BushingImpl, MobilizedBody);
+
+
+ ///////////////////////////////////////////////////
+ // MOBILIZED BODY::BALL (ORIENTATION, SPHERICAL) //
+ ///////////////////////////////////////////////////
+
+MobilizedBody::Ball::Ball
+ (MobilizedBody& parent, const Body& body, Direction d)
+: MobilizedBody(new BallImpl(d)) {
+ // inb & outb frames are just the parent body's frame and new body's frame
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Ball::Ball
+ (MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame, Direction d)
+: MobilizedBody(new BallImpl(d)) {
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Ball& MobilizedBody::Ball::setDefaultRadius(Real r) {
+ getImpl().invalidateTopologyCache();
+ updImpl().setDefaultRadius(r);
+ return *this;
+}
+
+Real MobilizedBody::Ball::getDefaultRadius() const {
+ return getImpl().getDefaultRadius();
+}
+
+const Quaternion& MobilizedBody::Ball::getDefaultQ() const {
+ return getImpl().defaultQ;
+}
+MobilizedBody::Ball& MobilizedBody::Ball::setDefaultQ(const Quaternion& quat) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultQ = quat;
+ return *this;
+}
+
+const Vec4& MobilizedBody::Ball::getQ(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 4);
+ return Vec4::getAs(&mbr.getMyMatterSubsystemRep().getQ(s)[qStart]);
+}
+void MobilizedBody::Ball::setQ(State& s, const Vec4& q) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 4);
+ Vec4::updAs(&mbr.getMyMatterSubsystemRep().updQ(s)[qStart]) = q;
+}
+const Vec4& MobilizedBody::Ball::getQDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 4);
+ return Vec4::getAs(&mbr.getMyMatterSubsystemRep().getQDot(s)[qStart]);
+}
+const Vec4& MobilizedBody::Ball::getQDotDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 4);
+ return Vec4::getAs(&mbr.getMyMatterSubsystemRep().getQDotDot(s)[qStart]);
+}
+
+const Vec3& MobilizedBody::Ball::getU(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getU(s)[uStart]);
+}
+void MobilizedBody::Ball::setU(State& s, const Vec3& u) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ Vec3::updAs(&mbr.getMyMatterSubsystemRep().updU(s)[uStart]) = u;
+}
+const Vec3& MobilizedBody::Ball::getUDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getUDot(s)[uStart]);
+}
+
+const Vec4& MobilizedBody::Ball::
+getMyPartQ(const State& s, const Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq == 4);
+ return Vec4::getAs(&qlike[qStart]);
+}
+
+const Vec3& MobilizedBody::Ball::
+getMyPartU(const State& s, const Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec3::getAs(&ulike[uStart]);
+}
+
+Vec4& MobilizedBody::Ball::
+updMyPartQ(const State& s, Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq == 4);
+ return Vec4::updAs(&qlike[qStart]);
+}
+
+Vec3& MobilizedBody::Ball::
+updMyPartU(const State& s, Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec3::updAs(&ulike[uStart]);
+}
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(MobilizedBody::Ball,
+ MobilizedBody::BallImpl, MobilizedBody);
+
+ // BallImpl
+
+void MobilizedBody::BallImpl::calcDecorativeGeometryAndAppendImpl
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+{
+ // Call the superclass implementation to get standard default geometry.
+
+ MobilizedBodyImpl::calcDecorativeGeometryAndAppendImpl(s, stage, geom);
+
+ // We can't generate the ball until we know the radius, and we can't place
+ // the geometry on the body until we know the parent and child mobilizer frame
+ // placement on the body, which might not be until Instance stage.
+ if (stage == Stage::Instance && getMyMatterSubsystemRep().getShowDefaultGeometry()) {
+ const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
+ const Transform& X_PMb = getInboardFrame(s);
+ const Transform& X_BM = getOutboardFrame(s);
+
+ // On the inboard body, draw a solid sphere and a wireframe one attached to it for
+ // easier visualization of its rotation. These are at about 90% of the radius.
+ geom.push_back(DecorativeSphere(Real(0.92)*getDefaultRadius())
+ .setColor(Gray)
+ .setRepresentation(DecorativeGeometry::DrawSurface)
+ .setOpacity(Real(0.5))
+ .setResolution(Real(0.75))
+ .setBodyId(getMyParentMobilizedBodyIndex())
+ .setTransform(X_PMb));
+ geom.push_back(DecorativeSphere(Real(0.90)*getDefaultRadius())
+ .setColor(White)
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setResolution(Real(0.75))
+ .setLineThickness(3)
+ .setOpacity(Real(0.1))
+ .setBodyId(getMyParentMobilizedBodyIndex())
+ .setTransform(X_PMb));
+
+ // On the outboard body draw an orange mesh sphere at the ball radius.
+ geom.push_back(DecorativeSphere(getDefaultRadius())
+ .setColor(Orange)
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setOpacity(Real(0.5))
+ .setResolution(Real(0.5))
+ .setBodyId(getMyMobilizedBodyIndex())
+ .setTransform(X_BM));
+ }
+}
+
+ ///////////////////////////////
+ // MOBILIZED BODY::ELLIPSOID //
+ ///////////////////////////////
+
+MobilizedBody::Ellipsoid::Ellipsoid
+ (MobilizedBody& parent, const Body& body, Direction d)
+: MobilizedBody(new EllipsoidImpl(d)) {
+ // inb & outb frames are just the parent body's frame and new body's frame
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Ellipsoid::Ellipsoid
+ (MobilizedBody& parent, const Body& body, const Vec3& radii, Direction d)
+: MobilizedBody(new EllipsoidImpl(d)) {
+ // inb & outb frames are just the parent body's frame and new body's frame
+ setBody(body);
+ setDefaultRadii(radii);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Ellipsoid::Ellipsoid
+ (MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame, Direction d)
+: MobilizedBody(new EllipsoidImpl(d)) {
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Ellipsoid::Ellipsoid
+ (MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame,
+ const Vec3& radii, Direction d)
+: MobilizedBody(new EllipsoidImpl(d)) {
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+ setBody(body);
+ setDefaultRadii(radii);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Ellipsoid& MobilizedBody::Ellipsoid::
+setDefaultRadii(const Vec3& r) {
+ updImpl().setDefaultRadii(r);
+ return *this;
+}
+
+const Vec3& MobilizedBody::Ellipsoid::getDefaultRadii() const {
+ return getImpl().getDefaultRadii();
+}
+
+const Quaternion& MobilizedBody::Ellipsoid::getDefaultQ() const {
+ return getImpl().defaultQ;
+}
+Quaternion& MobilizedBody::Ellipsoid::updDefaultQ() {
+ return updImpl().defaultQ;
+}
+
+const Vec4& MobilizedBody::Ellipsoid::getQ(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 4);
+ return Vec4::getAs(&mbr.getMyMatterSubsystemRep().getQ(s)[qStart]);
+}
+void MobilizedBody::Ellipsoid::setQ(State& s, const Vec4& q) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 4);
+ Vec4::updAs(&mbr.getMyMatterSubsystemRep().updQ(s)[qStart]) = q;
+}
+const Vec4& MobilizedBody::Ellipsoid::getQDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 4);
+ return Vec4::getAs(&mbr.getMyMatterSubsystemRep().getQDot(s)[qStart]);
+}
+const Vec4& MobilizedBody::Ellipsoid::getQDotDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 4);
+ return Vec4::getAs(&mbr.getMyMatterSubsystemRep().getQDotDot(s)[qStart]);
+}
+
+const Vec3& MobilizedBody::Ellipsoid::getU(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getU(s)[uStart]);
+}
+void MobilizedBody::Ellipsoid::setU(State& s, const Vec3& u) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ Vec3::updAs(&mbr.getMyMatterSubsystemRep().updU(s)[uStart]) = u;
+}
+const Vec3& MobilizedBody::Ellipsoid::getUDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getUDot(s)[uStart]);
+}
+
+const Vec4& MobilizedBody::Ellipsoid::
+getMyPartQ(const State& s, const Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq==4);
+ return Vec4::getAs(&qlike[qStart]);
+}
+
+const Vec3& MobilizedBody::Ellipsoid::
+getMyPartU(const State& s, const Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu==3);
+ return Vec3::getAs(&ulike[uStart]);
+}
+
+Vec4& MobilizedBody::Ellipsoid::
+updMyPartQ(const State& s, Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq==4);
+ return Vec4::updAs(&qlike[qStart]);
+}
+
+Vec3& MobilizedBody::Ellipsoid::
+updMyPartU(const State& s, Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu==3);
+ return Vec3::updAs(&ulike[uStart]);
+}
+
+void MobilizedBody::EllipsoidImpl::calcDecorativeGeometryAndAppendImpl
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+{
+ // Call the superclass implementation to get standard default geometry.
+
+ MobilizedBodyImpl::calcDecorativeGeometryAndAppendImpl(s, stage, geom);
+
+ // We can't generate the ellipsoid until we know the radius, and we can't
+ // place either piece of geometry on the bodies until we know the parent
+ // and child mobilizer frame placements, which might not be until Instance
+ // stage.
+ if (stage == Stage::Instance && getMyMatterSubsystemRep().getShowDefaultGeometry())
+ {
+ const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
+ const Transform& X_PMb = getInboardFrame(s);
+ const Transform& X_BM = getOutboardFrame(s);
+
+ //TODO: this should come from the State.
+ const Vec3 radii = getDefaultRadii();
+
+ // Put an ellipsoid on the parent, and some wires to make it easier to track.
+ geom.push_back(DecorativeEllipsoid(radii)
+ .setColor(Gray)
+ .setRepresentation(DecorativeGeometry::DrawSurface)
+ .setOpacity(Real(0.5))
+ .setResolution(Real(1.25))
+ .setBodyId(getMyParentMobilizedBodyIndex())
+ .setTransform(X_PMb));
+ geom.push_back(DecorativeEllipsoid(radii*Real(.99))
+ .setColor(White)
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setResolution(Real(0.75))
+ .setLineThickness(3)
+ .setOpacity(Real(0.1))
+ .setBodyId(getMyParentMobilizedBodyIndex())
+ .setTransform(X_PMb));
+
+ // Calculate the follower plate dimensions from the ellipsoid dimensions.
+ const Real minr = std::min(radii[0],std::min(radii[1],radii[2]));
+ const Real hw = minr/3; // half width of follower plate in x
+ const Real hh = minr/30; // half height of follower plate
+
+ // raise up so bottom is on xy plane
+ const Transform X_BFollower(X_BM.R(), X_BM.p() + Vec3(0,0,hh));
+ geom.push_back(DecorativeBrick(Vec3(hw,Real(2*hw/3.),hh))
+ .setColor(Orange)
+ .setBodyId(getMyMobilizedBodyIndex())
+ .setTransform(X_BFollower));
+ }
+}
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(MobilizedBody::Ellipsoid,
+ MobilizedBody::EllipsoidImpl, MobilizedBody);
+
+ /////////////////////////////////
+ // MOBILIZED BODY::TRANSLATION //
+ /////////////////////////////////
+
+MobilizedBody::Translation::Translation
+ (MobilizedBody& parent, const Body& body, Direction d)
+: MobilizedBody(new TranslationImpl(d)) {
+ // inb & outb frames are just the parent body's frame and new body's frame
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Translation::Translation
+ (MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame, Direction d)
+: MobilizedBody(new TranslationImpl(d)) {
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+const Vec3& MobilizedBody::Translation::getDefaultQ() const {
+ return getImpl().defaultQ;
+}
+MobilizedBody::Translation& MobilizedBody::Translation::
+setDefaultQ(const Vec3& q) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultQ = q;
+ return *this;
+}
+
+const Vec3& MobilizedBody::Translation::getQ(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getQ(s)[qStart]);
+}
+void MobilizedBody::Translation::setQ(State& s, const Vec3& q) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ Vec3::updAs(&mbr.getMyMatterSubsystemRep().updQ(s)[qStart]) = q;
+}
+const Vec3& MobilizedBody::Translation::getQDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getQDot(s)[qStart]);
+}
+const Vec3& MobilizedBody::Translation::getQDotDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getQDotDot(s)[qStart]);
+}
+
+
+const Vec3& MobilizedBody::Translation::getU(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getU(s)[uStart]);
+}
+void MobilizedBody::Translation::setU(State& s, const Vec3& u) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ Vec3::updAs(&mbr.getMyMatterSubsystemRep().updU(s)[uStart]) = u;
+}
+const Vec3& MobilizedBody::Translation::getUDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec3::getAs(&mbr.getMyMatterSubsystemRep().getUDot(s)[uStart]);
+}
+
+const Vec3& MobilizedBody::Translation::
+getMyPartQ(const State& s, const Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ return Vec3::getAs(&qlike[qStart]);
+}
+
+const Vec3& MobilizedBody::Translation::
+getMyPartU(const State& s, const Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec3::getAs(&ulike[uStart]);
+}
+
+Vec3& MobilizedBody::Translation::
+updMyPartQ(const State& s, Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq == 3);
+ return Vec3::updAs(&qlike[qStart]);
+}
+
+Vec3& MobilizedBody::Translation::
+updMyPartU(const State& s, Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec3::updAs(&ulike[uStart]);
+}
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(MobilizedBody::Translation,
+ MobilizedBody::TranslationImpl, MobilizedBody);
+
+ //////////////////////////
+ // MOBILIZED BODY::FREE //
+ //////////////////////////
+
+MobilizedBody::Free::Free(MobilizedBody& parent, const Body& body, Direction d)
+: MobilizedBody(new FreeImpl(d)) {
+ // inb & outb frames are just the parent body's frame and new body's frame
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Free::Free(MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame, Direction d)
+: MobilizedBody(new FreeImpl(d)) {
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Free& MobilizedBody::Free::
+setDefaultTranslation(const Vec3& p_FM) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultQTranslation = p_FM;
+ return *this;
+}
+
+
+MobilizedBody::Free& MobilizedBody::Free::
+setDefaultQuaternion(const Quaternion& R_FM) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultQOrientation = R_FM;
+ return *this;
+}
+
+MobilizedBody::Free& MobilizedBody::Free::
+setDefaultRotation(const Rotation& R_FM) {
+ setDefaultQuaternion(R_FM.convertRotationToQuaternion());
+ return *this;
+}
+
+MobilizedBody::Free& MobilizedBody::Free::
+setDefaultTransform(const Transform& X_FM) {
+ setDefaultTranslation(X_FM.p());
+ setDefaultQuaternion(X_FM.R().convertRotationToQuaternion());
+ return *this;
+}
+
+const Vec3& MobilizedBody::Free::getDefaultTranslation() const {
+ return getImpl().defaultQTranslation;
+}
+
+const Quaternion& MobilizedBody::Free::getDefaultQuaternion() const {
+ return getImpl().defaultQOrientation;
+}
+
+const Vec7& MobilizedBody::Free::getDefaultQ() const {
+ // assuming struct is packed so (Orientation,Translation) are contiguous
+ return Vec7::getAs((const Real*)&getImpl().defaultQOrientation);
+}
+MobilizedBody::Free& MobilizedBody::Free::setDefaultQ(const Vec7& q) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultQOrientation = Quaternion(q.getSubVec<4>(0));
+ updImpl().defaultQTranslation = q.getSubVec<3>(4);
+ return *this;
+}
+
+const Vec7& MobilizedBody::Free::getQ(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq==7);
+ return Vec7::getAs(&mbr.getMyMatterSubsystemRep().getQ(s)[qStart]);
+}
+void MobilizedBody::Free::setQ(State& s, const Vec7& q) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq==7);
+ Vec7::updAs(&mbr.getMyMatterSubsystemRep().updQ(s)[qStart]) = q;
+}
+const Vec7& MobilizedBody::Free::getQDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq==7);
+ return Vec7::getAs(&mbr.getMyMatterSubsystemRep().getQDot(s)[qStart]);
+}
+const Vec7& MobilizedBody::Free::getQDotDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq==7);
+ return Vec7::getAs(&mbr.getMyMatterSubsystemRep().getQDotDot(s)[qStart]);
+}
+
+
+const Vec6& MobilizedBody::Free::getU(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 6);
+ return Vec6::getAs(&mbr.getMyMatterSubsystemRep().getU(s)[uStart]);
+}
+void MobilizedBody::Free::setU(State& s, const Vec6& u) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 6);
+ Vec6::updAs(&mbr.getMyMatterSubsystemRep().updU(s)[uStart]) = u;
+}
+const Vec6& MobilizedBody::Free::getUDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 6);
+ return Vec6::getAs(&mbr.getMyMatterSubsystemRep().getUDot(s)[uStart]);
+}
+
+const Vec7& MobilizedBody::Free::
+getMyPartQ(const State& s, const Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq == 7);
+ return Vec7::getAs(&qlike[qStart]);
+}
+
+const Vec6& MobilizedBody::Free::
+getMyPartU(const State& s, const Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu == 6);
+ return Vec6::getAs(&ulike[uStart]);
+}
+
+Vec7& MobilizedBody::Free::
+updMyPartQ(const State& s, Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq == 7);
+ return Vec7::updAs(&qlike[qStart]);
+}
+
+Vec6& MobilizedBody::Free::
+updMyPartU(const State& s, Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu == 6);
+ return Vec6::updAs(&ulike[uStart]);
+}
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(MobilizedBody::Free,
+ MobilizedBody::FreeImpl, MobilizedBody);
+
+ //////////////////////////////////////
+ // MOBILIZED BODY::LINE ORIENTATION //
+ //////////////////////////////////////
+
+MobilizedBody::LineOrientation::LineOrientation
+ (MobilizedBody& parent, const Body& body, Direction d)
+: MobilizedBody(new LineOrientationImpl(d)) {
+ // inb & outb frames are just the parent body's frame and new body's frame
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::LineOrientation::LineOrientation
+ (MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame, Direction d)
+: MobilizedBody(new LineOrientationImpl(d)) {
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+const Quaternion& MobilizedBody::LineOrientation::getDefaultQ() const {
+ return getImpl().defaultQ;
+}
+MobilizedBody::LineOrientation& MobilizedBody::LineOrientation::
+setDefaultQ(const Quaternion& quat) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultQ = quat;
+ return *this;
+}
+
+const Vec4& MobilizedBody::LineOrientation::getQ(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 4);
+ return Vec4::getAs(&mbr.getMyMatterSubsystemRep().getQ(s)[qStart]);
+}
+void MobilizedBody::LineOrientation::setQ(State& s, const Vec4& q) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 4);
+ Vec4::updAs(&mbr.getMyMatterSubsystemRep().updQ(s)[qStart]) = q;
+}
+const Vec4& MobilizedBody::LineOrientation::getQDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 4);
+ return Vec4::getAs(&mbr.getMyMatterSubsystemRep().getQDot(s)[qStart]);
+}
+const Vec4& MobilizedBody::LineOrientation::getQDotDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 4);
+ return Vec4::getAs(&mbr.getMyMatterSubsystemRep().getQDotDot(s)[qStart]);
+}
+
+const Vec2& MobilizedBody::LineOrientation::getU(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec2::getAs(&mbr.getMyMatterSubsystemRep().getU(s)[uStart]);
+}
+void MobilizedBody::LineOrientation::setU(State& s, const Vec2& u) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ Vec2::updAs(&mbr.getMyMatterSubsystemRep().updU(s)[uStart]) = u;
+}
+const Vec2& MobilizedBody::LineOrientation::getUDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec2::getAs(&mbr.getMyMatterSubsystemRep().getUDot(s)[uStart]);
+}
+
+const Vec4& MobilizedBody::LineOrientation::
+getMyPartQ(const State& s, const Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq == 4);
+ return Vec4::getAs(&qlike[qStart]);
+}
+
+const Vec2& MobilizedBody::LineOrientation::
+getMyPartU(const State& s, const Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec2::getAs(&ulike[uStart]);
+}
+
+Vec4& MobilizedBody::LineOrientation::
+updMyPartQ(const State& s, Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq == 4);
+ return Vec4::updAs(&qlike[qStart]);
+}
+
+Vec2& MobilizedBody::LineOrientation::
+updMyPartU(const State& s, Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu == 3);
+ return Vec2::updAs(&ulike[uStart]);
+}
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(MobilizedBody::LineOrientation,
+ MobilizedBody::LineOrientationImpl, MobilizedBody);
+
+ ///////////////////////////////
+ // MOBILIZED BODY::FREE LINE //
+ ///////////////////////////////
+
+MobilizedBody::FreeLine::FreeLine
+ (MobilizedBody& parent, const Body& body, Direction d)
+: MobilizedBody(new FreeLineImpl(d)) {
+ // inb & outb frames are just the parent body's frame and new body's frame
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::FreeLine::FreeLine
+ (MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame, Direction d)
+: MobilizedBody(new FreeLineImpl(d)) {
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(MobilizedBody::FreeLine,
+ MobilizedBody::FreeLineImpl, MobilizedBody);
+
+ //////////////////////////
+ // MOBILIZED BODY::WELD //
+ //////////////////////////
+
+MobilizedBody::Weld::Weld(MobilizedBody& parent, const Body& body)
+: MobilizedBody(new WeldImpl(MobilizedBody::Forward)) {
+ // inb & outb frames are just the parent body's frame and new body's frame
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Weld::Weld(MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame)
+: MobilizedBody(new WeldImpl(MobilizedBody::Forward)) {
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(MobilizedBody::Weld,
+ MobilizedBody::WeldImpl, MobilizedBody);
+
+ ////////////////////////////////
+ // (IM)MOBILIZED BODY::GROUND //
+ ////////////////////////////////
+
+MobilizedBody::Ground::Ground() : MobilizedBody(new GroundImpl()) {
+ setBody(Body::Ground());
+}
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(MobilizedBody::Ground,
+ MobilizedBody::GroundImpl, MobilizedBody);
+
+ ///////////////////////////
+ // MOBILIZED BODY::SCREW //
+ ///////////////////////////
+
+MobilizedBody::Screw::Screw
+ (MobilizedBody& parent, const Body& body, Real pitch, Direction d)
+: MobilizedBody(new ScrewImpl(pitch,d)) {
+ // inb & outb frames are just the parent body's frame and new body's frame
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Screw::Screw(MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame,
+ Real pitch, Direction d)
+: MobilizedBody(new ScrewImpl(pitch,d)) {
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+ setBody(body);
+
+ parent.updMatterSubsystem().adoptMobilizedBody(parent.getMobilizedBodyIndex(),
+ *this);
+}
+
+MobilizedBody::Screw& MobilizedBody::Screw::setDefaultPitch(Real pitch) {
+ updImpl().setDefaultPitch(pitch);
+ return *this;
+}
+
+Real MobilizedBody::Screw::getDefaultPitch() const {
+ return getImpl().getDefaultPitch();
+}
+
+Real MobilizedBody::Screw::getDefaultQ() const {
+ return getImpl().defaultQ;
+}
+
+MobilizedBody::Screw& MobilizedBody::Screw::setDefaultQ(Real q) {
+ getImpl().invalidateTopologyCache();
+ updImpl().defaultQ = q;
+ return *this;
+}
+
+Real MobilizedBody::Screw::getQ(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 1);
+ return mbr.getMyMatterSubsystemRep().getQ(s)[qStart];
+}
+void MobilizedBody::Screw::setQ(State& s, Real q) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 1);
+ mbr.getMyMatterSubsystemRep().updQ(s)[qStart] = q;
+}
+Real MobilizedBody::Screw::getQDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 1);
+ return mbr.getMyMatterSubsystemRep().getQDot(s)[qStart];
+}
+Real MobilizedBody::Screw::getQDotDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ QIndex qStart; int nq; mbr.findMobilizerQs(s,qStart,nq); assert(nq == 1);
+ return mbr.getMyMatterSubsystemRep().getQDotDot(s)[qStart];
+}
+
+
+Real MobilizedBody::Screw::getU(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 1);
+ return mbr.getMyMatterSubsystemRep().getU(s)[uStart];
+}
+void MobilizedBody::Screw::setU(State& s, Real u) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 1);
+ mbr.getMyMatterSubsystemRep().updU(s)[uStart] = u;
+}
+Real MobilizedBody::Screw::getUDot(const State& s) const {
+ const MobilizedBodyImpl& mbr = MobilizedBody::getImpl();
+ UIndex uStart; int nu; mbr.findMobilizerUs(s,uStart,nu); assert(nu == 1);
+ return mbr.getMyMatterSubsystemRep().getUDot(s)[uStart];
+}
+
+Real MobilizedBody::Screw::getMyPartQ(const State& s, const Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq==1);
+ return qlike[qStart];
+}
+
+Real MobilizedBody::Screw::getMyPartU(const State& s, const Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu==1);
+ return ulike[uStart];
+}
+
+Real& MobilizedBody::Screw::updMyPartQ(const State& s, Vector& qlike) const {
+ QIndex qStart; int nq; getImpl().findMobilizerQs(s,qStart,nq); assert(nq==1);
+ return qlike[qStart];
+}
+
+Real& MobilizedBody::Screw::updMyPartU(const State& s, Vector& ulike) const {
+ UIndex uStart; int nu; getImpl().findMobilizerUs(s,uStart,nu); assert(nu==1);
+ return ulike[uStart];
+}
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(MobilizedBody::Screw,
+ MobilizedBody::ScrewImpl, MobilizedBody);
+
+ ////////////////////////////
+ // MOBILIZED BODY::CUSTOM //
+ ////////////////////////////
+
+// We are given an Implementation object which is already holding a CustomImpl
+// object for us. We'll first take away ownership of the CustomImpl, then
+// make the CustomImpl take over ownership of the Implementation object.
+MobilizedBody::Custom::Custom
+ (MobilizedBody& parent, MobilizedBody::Custom::Implementation* implementation,
+ const Body& body, Direction d)
+: MobilizedBody(implementation
+ ? implementation->updImpl().removeOwnershipOfCustomImpl()
+ : 0)
+{
+ SimTK_ASSERT_ALWAYS(implementation,
+ "MobilizedBody::Custom::Custom(): Implementation pointer was NULL.");
+ setBody(body);
+
+ updImpl().setDirection(d);
+
+ // Now store the Implementation pointer in our CustomImpl. The Implementation
+ // object retains its original pointer to the CustomImpl object so it can
+ // operate as a proxy for the CustomImpl. However the Custom handle now owns
+ // the CustomImpl and the CustomImpl owns the Implementation.
+ updImpl().takeOwnershipOfImplementation(implementation);
+
+ updImpl().updMyMatterSubsystemRep()
+ .adoptMobilizedBody(parent.getMobilizedBodyIndex(), *this);
+}
+
+MobilizedBody::Custom::Custom
+ (MobilizedBody& parent, MobilizedBody::Custom::Implementation* implementation,
+ const Transform& inbFrame, const Body& body, const Transform& outbFrame,
+ Direction d)
+: MobilizedBody(implementation
+ ? implementation->updImpl().removeOwnershipOfCustomImpl()
+ : 0)
+{
+ SimTK_ASSERT_ALWAYS(implementation,
+ "MobilizedBody::Custom::Custom(): Implementation pointer was NULL.");
+ setBody(body);
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+
+ updImpl().setDirection(d);
+
+ // Now store the Implementation pointer in our CustomImpl. The Implementation
+ // object retains its original pointer to the CustomImpl object so it can
+ // operate as a proxy for the CustomImpl. However the Custom handle now owns
+ // the CustomImpl and the CustomImpl owns the Implementation.
+ updImpl().takeOwnershipOfImplementation(implementation);
+
+ updImpl().updMyMatterSubsystemRep()
+ .adoptMobilizedBody(parent.getMobilizedBodyIndex(), *this);
+}
+
+const MobilizedBody::Custom::Implementation& MobilizedBody::Custom::
+getImplementation() const {
+ return getImpl().getImplementation();
+}
+
+MobilizedBody::Custom::Implementation& MobilizedBody::Custom::
+updImplementation() {
+ return updImpl().updImplementation();
+}
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(MobilizedBody::Custom,
+ MobilizedBody::CustomImpl, MobilizedBody);
+
+// MobilizedBody::CustomImpl
+
+// The Implementation object should already contain a pointer to this CustomImpl object.
+void MobilizedBody::CustomImpl::
+takeOwnershipOfImplementation(Custom::Implementation* userImpl) {
+ assert(!implementation); // you can only do this once!
+ assert(userImpl);
+ const Custom::ImplementationImpl& impImpl = userImpl->getImpl();
+ assert(&impImpl.getCustomImpl() == this && !impImpl.isOwnerOfCustomImpl());
+ implementation = userImpl;
+}
+
+////////////////////////////////////////////
+// MOBILIZED BODY::CUSTOM::IMPLEMENTATION //
+////////////////////////////////////////////
+
+// We create the initial CustomImpl as though it the mobilizer will be in the
+// forward direction. However, that may be changed later when this
+// implementation is put to use.
+MobilizedBody::Custom::Implementation::Implementation
+ (SimbodyMatterSubsystem& matter, int nu, int nq, int nAngles)
+: PIMPLHandle<Implementation,ImplementationImpl>
+ (new ImplementationImpl(new CustomImpl(MobilizedBody::Forward),
+ nu, nq, nAngles))
+{
+ assert(nu > 0);
+ assert(nq > 0);
+ assert(nAngles >= 0);
+ assert(nu <= 6);
+ assert(nq <= 7);
+ assert(nAngles <= 4);
+ assert(nu <= nq);
+ assert(nAngles <= nq);
+ // We don't know the MobilizedBodyIndex yet since this hasn't been adopted
+ // by the MatterSubsystem.
+ updImpl().updCustomImpl().setMyMatterSubsystem
+ (matter, MobilizedBodyIndex(0), MobilizedBodyIndex(0));
+}
+
+MobilizedBody::Custom::Implementation::~Implementation() {
+}
+
+void MobilizedBody::Custom::Implementation::invalidateTopologyCache() const {
+ getImpl().getCustomImpl().invalidateTopologyCache();
+}
+
+bool MobilizedBody::Custom::Implementation::
+getUseEulerAngles(const State& state) const {
+ return getImpl().getCustomImpl().getMyMatterSubsystemRep().getUseEulerAngles(state);
+}
+
+Vector MobilizedBody::Custom::Implementation::getQ(const State& state) const {
+ const SBModelVars& mv = getImpl().getCustomImpl().getMyMatterSubsystemRep()
+ .getModelVars(state);
+ const RigidBodyNode& body = getImpl().getCustomImpl().getMyRigidBodyNode();
+ const int indexBase = body.getQIndex();
+ Vector q(body.getNQInUse(mv));
+ for (int i = 0; i < q.size(); ++i) {
+ int index = indexBase+i;
+ q[i] = state.getQ()[index];
+ }
+ return q;
+}
+
+Vector MobilizedBody::Custom::Implementation::getU(const State& state) const {
+ const SBModelVars& mv = getImpl().getCustomImpl().getMyMatterSubsystemRep()
+ .getModelVars(state);
+ const RigidBodyNode& body = getImpl().getCustomImpl().getMyRigidBodyNode();
+ const int indexBase = body.getUIndex();
+ Vector u(body.getNUInUse(mv));
+ for (int i = 0; i < u.size(); ++i) {
+ int index = indexBase+i;
+ u[i] = state.getU()[index];
+ }
+ return u;
+}
+
+Vector MobilizedBody::Custom::Implementation::getQDot(const State& state) const {
+ const SBModelVars& mv = getImpl().getCustomImpl().getMyMatterSubsystemRep()
+ .getModelVars(state);
+ const RigidBodyNode& body = getImpl().getCustomImpl().getMyRigidBodyNode();
+ const int indexBase = body.getQIndex();
+ Vector qdot(body.getNQInUse(mv));
+ for (int i = 0; i < qdot.size(); ++i) {
+ int index = indexBase+i;
+ qdot[i] = state.getQDot()[index];
+ }
+ return qdot;
+}
+
+Vector MobilizedBody::Custom::Implementation::getUDot(const State& state) const {
+ const SBModelVars& mv = getImpl().getCustomImpl().getMyMatterSubsystemRep()
+ .getModelVars(state);
+ const RigidBodyNode& body = getImpl().getCustomImpl().getMyRigidBodyNode();
+ const int indexBase = body.getUIndex();
+ Vector udot(body.getNUInUse(mv));
+ for (int i = 0; i < udot.size(); ++i) {
+ int index = indexBase+i;
+ udot[i] = state.getUDot()[index];
+ }
+ return udot;
+}
+
+Vector MobilizedBody::Custom::Implementation::getQDotDot(const State& state) const {
+ const SBModelVars& mv = getImpl().getCustomImpl().getMyMatterSubsystemRep()
+ .getModelVars(state);
+ const RigidBodyNode& body = getImpl().getCustomImpl().getMyRigidBodyNode();
+ const int indexBase = body.getQIndex();
+ Vector qdotdot(body.getNQInUse(mv));
+ for (int i = 0; i < qdotdot.size(); ++i) {
+ int index = indexBase+i;
+ qdotdot[i] = state.getQDotDot()[index];
+ }
+ return qdotdot;
+}
+
+// Careful: must return the transform X_F0M0 using the "as defined" frames,
+// rather than X_FM which might be reversed due to mobilizer reversal.
+Transform MobilizedBody::Custom::Implementation::
+getMobilizerTransform(const State& s) const {
+ // Use "upd" instead of "get" here because the custom mobilizer definition
+ // needs access to this local information during realizePosition() so get
+ // would throw a stage violation.
+ const SBTreePositionCache& pc = getImpl().getCustomImpl()
+ .getMyMatterSubsystemRep().updTreePositionCache(s);
+ const RigidBodyNode& node = getImpl().getCustomImpl().getMyRigidBodyNode();
+ return node.findX_F0M0(pc);
+}
+
+// Careful: must return the velocity V_F0M0 using the "as defined" frames,
+// rather than V_FM which might be reversed due to mobilizer reversal.
+SpatialVec MobilizedBody::Custom::Implementation::
+getMobilizerVelocity(const State& s) const {
+ const SBTreePositionCache& pc = getImpl().getCustomImpl()
+ .getMyMatterSubsystemRep().getTreePositionCache(s);
+ // Use "upd" instead of "get" here because the custom mobilizer definition
+ // needs access to this local information during realizeVelocity() so get
+ // would throw a stage violation.
+ const SBTreeVelocityCache& vc = getImpl().getCustomImpl()
+ .getMyMatterSubsystemRep().updTreeVelocityCache(s);
+ const RigidBodyNode& node = getImpl().getCustomImpl().getMyRigidBodyNode();
+ return node.findV_F0M0(pc,vc);
+}
+
+void MobilizedBody::Custom::Implementation::
+multiplyByN(const State& s, bool transposeMatrix,
+ int nIn, const Real* in, int nOut, Real* out) const {
+ // Default implementation
+ assert((nIn==0 || in) && (nOut==0 || out));
+ int nu = getImpl().getNU(), nq = getImpl().getNQ(),
+ nAngles = getImpl().getNumAngles();
+ assert(nq==nu && nIn==nu && nOut==nu && nAngles < 4);
+ for (int i=0; i<nu; ++i) out[i] = in[i];
+}
+
+void MobilizedBody::Custom::Implementation::
+multiplyByNInv(const State& s, bool transposeMatrix,
+ int nIn, const Real* in, int nOut, Real* out) const {
+ // Default implementation
+ assert((nIn==0 || in) && (nOut==0 || out));
+ int nu = getImpl().getNU(), nq = getImpl().getNQ(),
+ nAngles = getImpl().getNumAngles();
+ assert(nq==nu && nIn==nu && nOut==nu && nAngles < 4);
+ for (int i=0; i<nu; ++i) out[i] = in[i];
+}
+
+void MobilizedBody::Custom::Implementation::
+multiplyByNDot(const State& s, bool transposeMatrix,
+ int nIn, const Real* in, int nOut, Real* out) const {
+ // Default implementation
+ assert((nIn==0 || in) && (nOut==0 || out));
+ int nu = getImpl().getNU(), nq = getImpl().getNQ(),
+ nAngles = getImpl().getNumAngles();
+ assert(nq==nu && nIn==nu && nOut==nu && nAngles < 4);
+ for (int i=0; i<nu; ++i) out[i] = 0;
+}
+
+void MobilizedBody::Custom::Implementation::
+setQToFitTransform(const State& state, const Transform& X_FM,
+ int nq, Real* q) const
+{
+ class OptimizerFunction : public OptimizerSystem {
+ public:
+ OptimizerFunction(const MobilizedBody::Custom::Implementation& impl,
+ const State& state, int nq, const Transform& X_FM)
+ : OptimizerSystem(nq), impl(impl), state(state), X_FM(X_FM) {}
+
+ int objectiveFunc(const Vector& params, bool new_params, Real& f) const {
+ Transform transform = impl.calcMobilizerTransformFromQ
+ (state, params.size(), ¶ms[0]);
+ f = (transform.p()-X_FM.p()).norm();
+ f += std::abs((~transform.R()*X_FM.R()).convertRotationToAngleAxis()[0]);
+ return 0;
+ }
+ private:
+ const MobilizedBody::Custom::Implementation& impl;
+ const State& state;
+ const Transform& X_FM;
+ };
+ OptimizerFunction function(*this, state, nq, X_FM);
+ Optimizer opt(function);
+ opt.useNumericalJacobian(true);
+ opt.useNumericalGradient(true);
+ opt.setLimitedMemoryHistory(100);
+ Vector qvec(nq);
+
+ // Pick initial values which are 1) deterministic and 2) unlikely to
+ // correspond to a local maximum or inflection point, which could cause the
+ // optimizer to fail.
+
+ for (int i = 0; i < nq; i++)
+ qvec[i] = i+Real(0.12354);
+ opt.optimize(qvec);
+ for (int i = 0; i < nq; i++)
+ q[i] = qvec[i];
+}
+
+void MobilizedBody::Custom::Implementation::
+setUToFitVelocity(const State& state, const SpatialVec& V_FM,
+ int nu, Real* u) const
+{
+ class OptimizerFunction : public OptimizerSystem {
+ public:
+ OptimizerFunction(const MobilizedBody::Custom::Implementation& impl,
+ const State& state, int nu, const SpatialVec& V_FM)
+ : OptimizerSystem(nu), impl(impl), state(state), V_FM(V_FM) {}
+
+ int objectiveFunc(const Vector& params, bool new_params, Real& f) const {
+ SpatialVec v = impl.multiplyByHMatrix
+ (state, params.size(), ¶ms[0]);
+ f = (v[0]-V_FM[0]).norm();
+ f += (v[1]-V_FM[1]).norm();
+ return 0;
+ }
+ private:
+ const MobilizedBody::Custom::Implementation& impl;
+ const State& state;
+ const SpatialVec& V_FM;
+ };
+ OptimizerFunction function(*this, state, nu, V_FM);
+ Optimizer opt(function);
+ opt.useNumericalJacobian(true);
+ opt.useNumericalGradient(true);
+ opt.setLimitedMemoryHistory(100);
+ Vector uvec(nu);
+
+ // Pick initiial values which are 1) deterministic and 2) unlikely to
+ // correspond to a local maximum or inflection point, which could cause the
+ // optimizer to fail.
+
+ for (int i = 0; i < nu; i++)
+ uvec[i] = i+Real(0.12354);
+ opt.optimize(uvec);
+ for (int i = 0; i < nu; i++)
+ u[i] = uvec[i];
+}
+
+// Constructors without user-specified axes for function-based mobilized body
+MobilizedBody::FunctionBased::FunctionBased
+ (MobilizedBody& parent, const Body& body,
+ int nmobilities, const Array_<const Function*>& functions,
+ const Array_<Array_<int> >& coordIndices,
+ Direction direction)
+: Custom(parent, new FunctionBasedImpl(parent.updMatterSubsystem(),
+ nmobilities, functions, coordIndices),
+ body, direction)
+{
+}
+
+MobilizedBody::FunctionBased::FunctionBased
+ (MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame,
+ int nmobilities, const Array_<const Function*>& functions,
+ const Array_<Array_<int> >& coordIndices,
+ Direction direction)
+: Custom(parent, new FunctionBasedImpl(parent.updMatterSubsystem(),
+ nmobilities, functions, coordIndices),
+ body, direction)
+{
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+}
+
+// Constructors that allow user-specified axes for function-based mobilized body
+MobilizedBody::FunctionBased::FunctionBased
+ (MobilizedBody& parent, const Body& body,
+ int nmobilities, const Array_<const Function*>& functions,
+ const Array_<Array_<int> >& coordIndices, const Array_<Vec3>& axes,
+ Direction direction)
+: Custom(parent, new FunctionBasedImpl(parent.updMatterSubsystem(),
+ nmobilities, functions,
+ coordIndices, axes),
+ body, direction)
+{
+}
+
+MobilizedBody::FunctionBased::FunctionBased
+ (MobilizedBody& parent, const Transform& inbFrame,
+ const Body& body, const Transform& outbFrame,
+ int nmobilities, const Array_<const Function*>& functions,
+ const Array_<Array_<int> >& coordIndices, const Array_<Vec3>& axes,
+ Direction direction)
+: Custom(parent, new FunctionBasedImpl(parent.updMatterSubsystem(),
+ nmobilities, functions,
+ coordIndices, axes),
+ body, direction)
+{
+ setDefaultInboardFrame(inbFrame);
+ setDefaultOutboardFrame(outbFrame);
+}
+
+} // namespace SimTK
+
diff --git a/Simbody/src/MobilizedBodyImpl.h b/Simbody/src/MobilizedBodyImpl.h
new file mode 100644
index 0000000..99d6acf
--- /dev/null
+++ b/Simbody/src/MobilizedBodyImpl.h
@@ -0,0 +1,1842 @@
+#ifndef SimTK_SIMBODY_MOBILIZED_BODY_IMPL_H_
+#define SimTK_SIMBODY_MOBILIZED_BODY_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) 2007-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ *
+ * Private implementation of Body and MobilizedBody, and their included
+ * subclasses which represent the built-in body and mobilizer types.
+ */
+
+#include "SimTKcommon.h"
+#include "simbody/internal/common.h"
+#include "simbody/internal/Body.h"
+#include "simbody/internal/MobilizedBody.h"
+#include "simbody/internal/MobilizedBody_BuiltIns.h"
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "BodyRep.h"
+#include "MotionImpl.h"
+
+class RigidBodyNode;
+
+namespace SimTK {
+
+ /////////////////////////
+ // MOBILIZED BODY REPS //
+ /////////////////////////
+
+// This is what a MobilizedBody handle points to.
+class MobilizedBodyImpl : public PIMPLImplementation<MobilizedBody,MobilizedBodyImpl> {
+public:
+ explicit MobilizedBodyImpl(MobilizedBody::Direction d)
+ : defaultLockLevel(Motion::NoLevel),
+ reversed(d==MobilizedBody::Reverse),
+ myMatterSubsystemRep(0), myLevel(-1), myRBnode(0), hasChildren(false)
+ {
+ }
+
+ void setDirection(MobilizedBody::Direction d) {
+ const bool wantReversed = (d==MobilizedBody::Reverse);
+ if (wantReversed != reversed) {
+ invalidateTopologyCache();
+ reversed = wantReversed;
+ }
+ }
+
+ MobilizedBodyImpl(const MobilizedBodyImpl& src) {
+ *this = src;
+ myRBnode = 0;
+ }
+
+
+ void lock(State& state, Motion::Level level) const;
+ void lockAt(State& state, int n, const Real* value, Motion::Level level) const;
+ void unlock(State& state) const;
+ Motion::Level getLockLevel(const State& state) const;
+ Vector getLockValueAsVector(const State& state) const;
+ void lockByDefault(Motion::Level level)
+ { invalidateTopologyCache(); defaultLockLevel=level; }
+ Motion::Level getLockByDefaultLevel() const
+ { return defaultLockLevel; }
+
+ // The matter subsystem must issue these MobilizedBody realize() calls in base-to-tip
+ // order, because these methods are allowed to assume that their parent (and
+ // ancestors) have already been realized.
+
+ // eventually calls realizeTopologyVirtual()
+ const RigidBodyNode& realizeTopology(State& s, UIndex& nxtU, USquaredIndex& nxtUSq, QIndex& nxtQ) const;
+
+ void realizeModel (State&) const; // eventually calls realizeModelVirtual()
+ void realizeInstance(const SBStateDigest&) const; // eventually calls realizeInstanceVirtual()
+ void realizeTime (const SBStateDigest&) const; // eventually calls realizeTimeVirtual()
+ void realizePosition(const SBStateDigest&) const; // eventually calls realizePositionVirtual()
+ void realizeVelocity(const SBStateDigest&) const; // eventually calls realizeVelocityVirtual()
+ void realizeDynamics(const SBStateDigest&) const; // eventually calls realizeDynamicsVirtual()
+ void realizeAcceleration
+ (const SBStateDigest&) const; // eventually calls realizeAccelerationVirtual()
+ void realizeReport (const SBStateDigest&) const; // eventually calls realizeReportVirtual()
+
+ virtual ~MobilizedBodyImpl() {
+ delete myRBnode;
+ }
+ virtual MobilizedBodyImpl* clone() const = 0;
+
+ // This creates a rigid body node using the appropriate mobilizer type.
+ // Called during realizeTopology().
+ virtual RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const = 0;
+
+ virtual void realizeTopologyVirtual (State&) const {}
+ virtual void realizeModelVirtual (State&) const {}
+ virtual void realizeInstanceVirtual (const State&) const {}
+ virtual void realizeTimeVirtual (const State&) const {}
+ virtual void realizePositionVirtual (const State&) const {}
+ virtual void realizeVelocityVirtual (const State&) const {}
+ virtual void realizeDynamicsVirtual (const State&) const {}
+ virtual void realizeAccelerationVirtual (const State&) const {}
+ virtual void realizeReportVirtual (const State&) const {}
+
+ // Copy out nq default values for q, beginning at the indicated address.
+ // The concrete class should assert if nq is not a reasonable
+ // number for the kind of mobilizer; there is a bug somewhere in that case.
+ // This routine shouldn't be called directly -- call copyOutDefaultQ() below
+ // instead which has a nicer interface and does some error checking.
+ virtual void copyOutDefaultQImpl(int nq, Real* q) const = 0;
+
+ virtual void calcDecorativeGeometryAndAppendImpl
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+ {
+ if (stage != Stage::Instance || !getMyMatterSubsystemRep().getShowDefaultGeometry())
+ return;
+ const Real scale = 1;
+ DecorativeFrame axes(scale/2);
+ axes.setLineThickness(2);
+ axes.setBodyId(myMobilizedBodyIndex);
+ geom.push_back(axes); // the body frame
+
+ // Display the inboard joint frame (at half size), unless it is the
+ // same as the body frame. Then find the corresponding frame on the
+ // parent and display that in this body's color.
+ if (myMobilizedBodyIndex != 0) {
+ const Real pscale = 1;
+ const Transform& X_BM = getDefaultOutboardFrame(); // TODO: get from state
+ if (X_BM.p() != Vec3(0) || X_BM.R() != Mat33(1)) {
+ DecorativeFrame frameOnChild(scale/4);
+ frameOnChild.setBodyId(myMobilizedBodyIndex);
+ frameOnChild.setColor(Red);
+ frameOnChild.setTransform(X_BM);
+ geom.push_back(frameOnChild);
+ if (X_BM.p() != Vec3(0))
+ geom.push_back(DecorativeLine(Vec3(0), X_BM.p()).setBodyId(myMobilizedBodyIndex));
+ }
+ const Transform& X_PF = getDefaultInboardFrame(); // TODO: from state
+ DecorativeFrame frameOnParent(pscale*Real(0.3)); // slightly larger than child
+ frameOnParent.setBodyId(myParentIndex);
+ frameOnParent.setColor(Blue);
+ frameOnParent.setTransform(X_PF);
+ geom.push_back(frameOnParent);
+ if (X_PF.p() != Vec3(0))
+ geom.push_back(DecorativeLine(Vec3(0),X_PF.p()).setBodyId(myParentIndex));
+ }
+
+ // Put a little purple wireframe sphere at the COM, and add a line from
+ // body origin to the com.
+
+ DecorativeSphere com(scale*Real(.05));
+ com.setBodyId(myMobilizedBodyIndex);
+ com.setColor(Purple).setRepresentation(DecorativeGeometry::DrawPoints);
+ const Vec3& comPos_B = theBody.getDefaultRigidBodyMassProperties().getMassCenter(); // TODO: from state
+ com.setTransform(comPos_B);
+ geom.push_back(com);
+ if (comPos_B != Vec3(0))
+ geom.push_back(DecorativeLine(Vec3(0), comPos_B).setBodyId(myMobilizedBodyIndex));
+ }
+
+ void calcDecorativeGeometryAndAppend
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+ {
+ // We know how to deal with the topological (construction) geometry
+ // here. For bodies we can just draw it at topology stage. For mobilizers,
+ // we might not know the placement of the mobilizer frames on the parent
+ // and child bodies until Instance stage, at which point we can transform
+ // and then draw the topological geometry.
+ if (stage == Stage::Topology) {
+ appendTopologicalBodyGeometry(geom);
+ } else if (stage == Stage::Instance) {
+ const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
+ const Transform& X_PF = getInboardFrame(s);
+ const Transform& X_BM = getOutboardFrame(s);
+ appendTopologicalMobilizerGeometry(X_BM, X_PF, geom);
+ }
+
+ // Let the individual mobilizer deal with any complicated stuff.
+ calcDecorativeGeometryAndAppendImpl(s,stage,geom);
+ }
+
+ int addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
+ const int nxt = (int)outboardGeometry.size();
+ outboardGeometry.push_back(g); // make a new copy
+ // Combine the placement frame and the transform already in the geometry
+ // so we end up with geometry expressed directly in the M frame.
+ outboardGeometry.back().setTransform(X_MD*g.getTransform());
+ return nxt;
+ }
+ int addInboardDecoration(const Transform& X_FD, const DecorativeGeometry& g) {
+ const int nxt = (int)inboardGeometry.size();
+ inboardGeometry.push_back(g); // make a new copy
+ // Combine the placement frame and the transform already in the geometry
+ // so we end up with geometry expressed directly in the F frame.
+ inboardGeometry.back().setTransform(X_FD*g.getTransform());
+ return nxt;
+ }
+
+
+ void findMobilizerQs(const State&, QIndex& qStart, int& nq) const;
+ void findMobilizerUs(const State&, UIndex& uStart, int& nu) const;
+
+
+ Motion::Method getQMotionMethod(const State&) const;
+ Motion::Method getUMotionMethod(const State&) const;
+ Motion::Method getUDotMotionMethod(const State&) const;
+
+ // Given the Model stage state variables (if any are relevant), each
+ // concrete mobilizer should return the number of q's and u's it expects
+ // to be allotted.
+ //virtual void getStateNeeds(const State& s, int& nq, int& nu) const = 0;
+
+ // Given the Model stage variable values in the passed-in State (if
+ // any of them are relevant) copy out the appropriate default values
+ // to the appropriate slot in qDefault.
+ void copyOutDefaultQ(const State& s, Vector& qDefault) const;
+
+ // Generic State-access routines (that is, those which can be handled in
+ // the MobilizedBody base class).
+
+
+ // Invalidate Stage::Position.
+ void setQToFitTransform(State& s, const Transform& X_FM) const;
+ void setQToFitRotation(State& s, const Rotation& R_FM) const;
+ void setQToFitTranslation(State& s, const Vec3& p_FM) const;
+
+ // Invalidate Stage::Velocity.
+ void setUToFitVelocity(State& s, const SpatialVec& V_FM) const;
+ void setUToFitAngularVelocity(State& s, const Vec3& w_FM) const;
+ void setUToFitLinearVelocity(State& s, const Vec3& v_FM) const;
+
+ const MassProperties& getBodyMassProperties(const State& s) const {
+ // TODO: these should come from the state if the body has variable mass props
+ const SBInstanceVars& iv = getMyMatterSubsystemRep().getInstanceVars(s);
+ return getMyRigidBodyNode().getMassProperties_OB_B();
+ }
+
+ const SpatialInertia& getBodySpatialInertiaInGround(const State& s) const {
+ const SBTreePositionCache& tpc = getMyMatterSubsystemRep().getTreePositionCache(s);
+ return getMyRigidBodyNode().getMk_G(tpc);
+ }
+
+ const Transform& getInboardFrame (const State& s) const {
+ // TODO: these should come from the state if the mobilizer has variable frames
+ const SBInstanceVars& iv = getMyMatterSubsystemRep().getInstanceVars(s);
+ return getMyRigidBodyNode().getX_PF();
+ }
+ const Transform& getOutboardFrame(const State& s) const {
+ // TODO: these should come from the state if the mobilizer has variable frames
+ const SBInstanceVars& iv = getMyMatterSubsystemRep().getInstanceVars(s);
+ return getMyRigidBodyNode().getX_BM();
+ }
+
+ void setInboardFrame (State& s, const Transform& X_PF) const {
+ assert(!"setInboardFrame(s) not implemented yet");
+ }
+ void setOutboardFrame(State& s, const Transform& X_BM) const {
+ assert(!"setOutboardFrame(s) not implemented yet");
+ }
+
+ const Transform& getBodyTransform(const State& s) const {
+ const SBTreePositionCache& pc = getMyMatterSubsystemRep().getTreePositionCache(s);
+ return getMyRigidBodyNode().getX_GB(pc);
+ }
+ const SpatialVec& getBodyVelocity(const State& s) const {
+ const SBTreeVelocityCache& vc = getMyMatterSubsystemRep().getTreeVelocityCache(s);
+ return getMyRigidBodyNode().getV_GB(vc);
+ }
+ const SpatialVec& getBodyAcceleration(const State& s) const {
+ const SBTreeAccelerationCache& ac = getMyMatterSubsystemRep().getTreeAccelerationCache(s);
+ return getMyRigidBodyNode().getA_GB(ac);
+ }
+
+ const Transform& getMobilizerTransform(const State& s) const {
+ const SBTreePositionCache& pc = getMyMatterSubsystemRep().getTreePositionCache(s);
+ return getMyRigidBodyNode().getX_FM(pc);
+ }
+ const SpatialVec& getMobilizerVelocity(const State& s) const {
+ const SBTreeVelocityCache& vc = getMyMatterSubsystemRep().getTreeVelocityCache(s);
+ return getMyRigidBodyNode().getV_FM(vc);
+ }
+
+ SpatialVec getHCol(const State& s, MobilizerUIndex ux) const {
+ const SBTreePositionCache& pc = getMyMatterSubsystemRep().getTreePositionCache(s);
+ return getMyRigidBodyNode().getHCol(pc,ux);
+ }
+
+ SpatialVec getH_FMCol(const State& s, MobilizerUIndex ux) const {
+ const SBTreePositionCache& pc = getMyMatterSubsystemRep().getTreePositionCache(s);
+ return getMyRigidBodyNode().getH_FMCol(pc,ux);
+ }
+
+ void invalidateTopologyCache() const {
+ delete myRBnode; myRBnode=0;
+ if (myMatterSubsystemRep)
+ myMatterSubsystemRep->invalidateSubsystemTopologyCache();
+ }
+
+ // This might get called *during* realizeTopology() so just make sure there is
+ // a node here without checking whether we're done with realizeTopology().
+ const RigidBodyNode& getMyRigidBodyNode() const {
+ SimTK_ASSERT(myRBnode && myMatterSubsystemRep,
+ "An operation on a MobilizedBody was illegal because realizeTopology() has "
+ "not been performed on the containing Subsystem since the last topological change."
+ );
+ return *myRBnode;
+ }
+
+ const Body& getBody() const {return theBody;}
+ const Transform& getDefaultInboardFrame() const {return defaultInboardFrame;}
+ const Transform& getDefaultOutboardFrame() const {return defaultOutboardFrame;}
+ const MassProperties& getDefaultRigidBodyMassProperties() const {
+ return theBody.getDefaultRigidBodyMassProperties();
+ }
+
+ bool isReversed() const {return reversed;}
+
+ void adoptMotion(Motion& ownerHandle) {
+ SimTK_ERRCHK(!hasMotion(), "MobilizedBody::adoptMotion()",
+ "This MobilizedBody already has a Motion object associated with it.\n"
+ "Use clearMotion() first to replace an existing Motion object.");
+ ownerHandle.disown(motion); // transfer ownership to handle "motion"
+ invalidateTopologyCache();
+
+ // Tell the Motion that it belongs to this MobilizedBody.
+ motion.updImpl().setMobilizedBodyImpl(this);
+ }
+
+ void clearMotion() {
+ motion.clearHandle();
+ invalidateTopologyCache();
+ }
+
+ bool hasMotion() const {return !motion.isEmptyHandle();}
+
+ const Motion& getMotion() const {
+ SimTK_ERRCHK(!motion.isEmptyHandle(),
+ "MobilizedBody::getMotion()",
+ "There is no Motion object associated with this MobilizedBody.");
+ return motion;
+ }
+
+ const SimbodyMatterSubsystem& getMySimbodyMatterSubsystem() const {
+ return getMyMatterSubsystemRep().getMySimbodyMatterSubsystemHandle();
+ }
+
+ const SimbodyMatterSubsystemRep& getMyMatterSubsystemRep() const {
+ SimTK_ASSERT(myMatterSubsystemRep,
+ "An operation was illegal because a MobilizedBody was not in a Subsystem.");
+ return *myMatterSubsystemRep;
+ }
+ SimbodyMatterSubsystemRep& updMyMatterSubsystemRep() {
+ SimTK_ASSERT(myMatterSubsystemRep,
+ "An operation was illegal because a MobilizedBody was not in a Subsystem.");
+ return *myMatterSubsystemRep;
+ }
+
+ const SBModelPerMobodInfo& getMyModelInfo(const State& s) const {
+ const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
+ const SBModelCache& mc = matterRep.getModelCache(s);
+ const SBModelPerMobodInfo& info =
+ mc.getMobodModelInfo(myMobilizedBodyIndex);
+ return info;
+ }
+
+ const SBInstancePerMobodInfo& getMyInstanceInfo(const State& s) const {
+ const SimbodyMatterSubsystemRep& matterRep = getMyMatterSubsystemRep();
+ const SBInstanceCache& ic = matterRep.getInstanceCache(s);
+ const SBInstancePerMobodInfo& info =
+ ic.getMobodInstanceInfo(myMobilizedBodyIndex);
+ return info;
+ }
+
+ const MobilizedBody& getMyHandle() const {
+ const MobilizedBody& mobod =
+ getMyMatterSubsystemRep().getMobilizedBody(getMyMobilizedBodyIndex());
+ SimTK_ASSERT(&mobod.getImpl() == this,
+ "A MobilizedBodyImpl's handle didn't refer back to the same Impl!");
+ return mobod;
+ }
+
+ MobilizedBody& updMyHandle() {
+ MobilizedBody& mobod =
+ updMyMatterSubsystemRep().updMobilizedBody(getMyMobilizedBodyIndex());
+ SimTK_ASSERT(&mobod.getImpl() == this,
+ "A MobilizedBodyImpl's handle didn't refer back to the same Impl!");
+ return mobod;
+ }
+
+ MobilizedBodyIndex getMyMobilizedBodyIndex() const {
+ assert(myMobilizedBodyIndex.isValid());
+ return myMobilizedBodyIndex;
+ }
+
+ MobilizedBodyIndex getMyParentMobilizedBodyIndex() const {
+ assert(myParentIndex.isValid());
+ return myParentIndex;
+ }
+
+ MobilizedBodyIndex getMyBaseBodyMobilizedBodyIndex() const {
+ assert(myBaseBodyIndex.isValid());
+ return myBaseBodyIndex;
+ }
+
+ int getMyLevel() const {
+ assert(myLevel >= 0);
+ return myLevel;
+ }
+
+ bool isInSubsystem() const {return myMatterSubsystemRep != 0;}
+
+ void setMyMatterSubsystem(SimbodyMatterSubsystem& matter,
+ MobilizedBodyIndex parentIndex,
+ MobilizedBodyIndex index)
+ {
+ // If the subsystem is already set it must be the same one.
+ assert(!myMatterSubsystemRep || myMatterSubsystemRep==&matter.getRep());
+ myMatterSubsystemRep = &matter.updRep();
+
+ assert(index.isValid());
+ assert(parentIndex.isValid() || index==GroundIndex);
+
+ myParentIndex = parentIndex; // invalid if this is Ground
+ myMobilizedBodyIndex = index;
+
+ if (index != GroundIndex) {
+ MobilizedBody& parent = matter.updMobilizedBody(parentIndex);
+ myLevel = parent.getLevelInMultibodyTree() + 1;
+ myBaseBodyIndex = (myLevel == 1 ? myMobilizedBodyIndex
+ : parent.getBaseMobilizedBody().getMobilizedBodyIndex());
+ parent.updImpl().hasChildren = true;
+ } else {
+ myLevel = 0;
+ myBaseBodyIndex = GroundIndex;
+ }
+ }
+
+private:
+ // Body topological geometry is defined with respect to the body frame so we
+ // can draw it right away.
+ void appendTopologicalBodyGeometry(Array_<DecorativeGeometry>& geom) const {
+ getBody().getRep().appendDecorativeGeometry(getMyMobilizedBodyIndex(), geom);
+ }
+
+ // Mobilizer topological geometry is defined with respect to the M (outboard, child)
+ // frame and the F (inboard, parent) frame. The placement of those frames with
+ // respect to the body frame can be Instance variables, so we can't draw this geometry
+ // until Instance stage. At that point we can find M and F, so they are passed in
+ // here.
+ void appendTopologicalMobilizerGeometry(const Transform& X_BM, const Transform& X_PF,
+ Array_<DecorativeGeometry>& geom) const
+ {
+ for (int i=0; i<(int)outboardGeometry.size(); ++i) {
+ geom.push_back(outboardGeometry[i]);
+ geom.back().setBodyId(getMyMobilizedBodyIndex())
+ .setTransform(X_BM*outboardGeometry[i].getTransform());
+ }
+ for (int i=0; i<(int)inboardGeometry.size(); ++i) {
+ geom.push_back(inboardGeometry[i]);
+ geom.back().setBodyId(getMyParentMobilizedBodyIndex())
+ .setTransform(X_PF*inboardGeometry[i].getTransform());
+ }
+ }
+
+private:
+ friend class MobilizedBody;
+
+ // TOPOLOGY "STATE"
+
+ // Base class topological properties. Derived MobilizedBodies may
+ // have further topological properties. Whenever these change, be
+ // sure to set topologyRealized=false!
+
+ // Body represents the mass structure, mass properties, and possibly some
+ // decorative geometry for the body associated with this (body,mobilizer)
+ // pair.
+ Body theBody;
+ Transform defaultInboardFrame; // default for F (in Parent frame)
+ Transform defaultOutboardFrame; // default for M (in Body frame)
+ Motion::Level defaultLockLevel;
+
+ // A Motion object, if present, defines how this mobilizer's motion is
+ // to be calculated. Otherwise, the motion is determined dynamically
+ // as a result of forces and constraints. A Motion always prescribes
+ // \e all of the mobilizer's generalized accelerations udot (index 1);
+ // may also some of the prescribed generalized speeds u (index 2); and
+ // for speeds that are prescribed it may also prescribe the corresponding
+ // generalized coordinates q (index 3).
+ Motion motion;
+
+ bool reversed; // is the mobilizer defined from M to F?
+
+ Array_<DecorativeGeometry> outboardGeometry;
+ Array_<DecorativeGeometry> inboardGeometry;
+
+ // These data members are filled in once the MobilizedBody is added to
+ // a MatterSubsystem. Note that this pointer is just a reference to
+ // the Subsystem which owns this MobilizedBody -- we don't own the
+ // heap space here so don't delete it!
+ SimbodyMatterSubsystemRep* myMatterSubsystemRep;
+ MobilizedBodyIndex myMobilizedBodyIndex; // index within the subsystem
+ MobilizedBodyIndex myParentIndex; // Invalid if this body is Ground, otherwise the parent's index
+ MobilizedBodyIndex myBaseBodyIndex; // GroundIndex if this is ground, otherwise a level-1 BodyIndex
+ int myLevel; // Distance from ground in multibody graph (0 if this is ground,
+ // 1 if a base body, 2 if parent is a base body, etc.)
+
+ // TOPOLOGY "CACHE"
+
+ // A RigidBodyNode is created for each MobilizedBody during realizeTopology().
+ // Think of it as the *computational* form of the MobilizedBody; the MobilizedBody
+ // itself exists to make a nice API for the programmer. In fact is is possible
+ // for several different MobilizedBody classes to use the same underlying
+ // computational form while providing a different API.
+ //
+ // This is a pointer to an abstract object whose heap space is *owned* by
+ // this MobilizedBody. Be sure to delete it upon destruction and whenever
+ // topology is re-realized.
+ mutable RigidBodyNode* myRBnode;
+
+protected:
+ // Keep track of whether this body has any children.
+ bool hasChildren;
+};
+
+class MobilizedBody::PinImpl : public MobilizedBodyImpl {
+public:
+ explicit PinImpl(Direction d) : MobilizedBodyImpl(d), defaultQ(0) { }
+ PinImpl* clone() const { return new PinImpl(*this); }
+
+ RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const;
+
+ void copyOutDefaultQImpl(int nq, Real* q) const {
+ SimTK_ASSERT(nq==1,
+ "MobilizedBody::PinImpl::copyOutDefaultQImpl(): wrong number of q's");
+ *q = defaultQ;
+ }
+
+ SimTK_DOWNCAST(PinImpl, MobilizedBodyImpl);
+private:
+ friend class MobilizedBody::Pin;
+ Real defaultQ; // the angle in radians
+};
+
+
+class MobilizedBody::SliderImpl : public MobilizedBodyImpl {
+public:
+ explicit SliderImpl(Direction d) : MobilizedBodyImpl(d), defaultQ(0) { }
+ SliderImpl* clone() const { return new SliderImpl(*this); }
+
+ RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const;
+
+ void copyOutDefaultQImpl(int nq, Real* q) const {
+ SimTK_ASSERT(nq==1,
+ "MobilizedBody::SliderImpl::copyOutDefaultQImpl(): wrong number of q's");
+ *q = defaultQ;
+ }
+
+ SimTK_DOWNCAST(SliderImpl, MobilizedBodyImpl);
+private:
+ friend class MobilizedBody::Slider;
+ Real defaultQ; // the displacement
+};
+
+class MobilizedBody::UniversalImpl : public MobilizedBodyImpl {
+public:
+ explicit UniversalImpl(Direction d) : MobilizedBodyImpl(d), defaultQ(0) { }
+ UniversalImpl* clone() const { return new UniversalImpl(*this); }
+
+ RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const;
+
+ void copyOutDefaultQImpl(int nq, Real* q) const {
+ SimTK_ASSERT(nq==2,
+ "MobilizedBody::UniversalImpl::copyOutDefaultQImpl(): wrong number of q's");
+ Vec2::updAs(q) = defaultQ;
+ }
+
+ SimTK_DOWNCAST(UniversalImpl, MobilizedBodyImpl);
+private:
+ friend class MobilizedBody::Universal;
+ Vec2 defaultQ; // the two angles
+};
+
+class MobilizedBody::CylinderImpl : public MobilizedBodyImpl {
+public:
+ explicit CylinderImpl(Direction d) : MobilizedBodyImpl(d), defaultQ(0) { }
+ CylinderImpl* clone() const { return new CylinderImpl(*this); }
+
+ RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const;
+
+ void copyOutDefaultQImpl(int nq, Real* q) const {
+ SimTK_ASSERT(nq==2,
+ "MobilizedBody::CylinderImpl::copyOutDefaultQImpl(): wrong number of q's");
+ Vec2::updAs(q) = defaultQ;
+ }
+
+ SimTK_DOWNCAST(CylinderImpl, MobilizedBodyImpl);
+private:
+ friend class MobilizedBody::Cylinder;
+ Vec2 defaultQ; // the angle in radians followed by the displacement
+};
+
+class MobilizedBody::BendStretchImpl : public MobilizedBodyImpl {
+public:
+ explicit BendStretchImpl(Direction d) : MobilizedBodyImpl(d), defaultQ(0) { }
+ BendStretchImpl* clone() const { return new BendStretchImpl(*this); }
+
+ RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const;
+
+ void copyOutDefaultQImpl(int nq, Real* q) const {
+ SimTK_ASSERT(nq==2,
+ "MobilizedBody::BendStretchImpl::copyOutDefaultQImpl(): wrong number of q's");
+ Vec2::updAs(q) = defaultQ;
+ }
+
+ SimTK_DOWNCAST(BendStretchImpl, MobilizedBodyImpl);
+private:
+ friend class MobilizedBody::BendStretch;
+ Vec2 defaultQ; // the angle in radians followed by the displacement
+};
+
+class MobilizedBody::PlanarImpl : public MobilizedBodyImpl {
+public:
+ explicit PlanarImpl(Direction d) : MobilizedBodyImpl(d), defaultQ(0) { }
+ PlanarImpl* clone() const { return new PlanarImpl(*this); }
+
+ RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const;
+
+ void copyOutDefaultQImpl(int nq, Real* q) const {
+ SimTK_ASSERT(nq==3,
+ "MobilizedBody::PlanarImpl::copyOutDefaultQImpl(): wrong number of q's");
+ Vec3::updAs(q) = defaultQ;
+ }
+
+ SimTK_DOWNCAST(PlanarImpl, MobilizedBodyImpl);
+private:
+ friend class MobilizedBody::Planar;
+ Vec3 defaultQ; // the angle in radians followed by the two displacements
+};
+
+class MobilizedBody::SphericalCoordsImpl : public MobilizedBodyImpl {
+public:
+ explicit SphericalCoordsImpl(Direction d)
+ : MobilizedBodyImpl(d), az0(0), ze0(0), axisT(ZAxis),
+ negAz(false), negZe(false), negT(false),
+ defaultQ(0) {}
+
+ SphericalCoordsImpl(Real az0, bool negAz,
+ Real ze0, bool negZe,
+ CoordinateAxis axisT, bool negT,
+ Direction d)
+ : MobilizedBodyImpl(d), az0(az0), ze0(ze0), axisT(axisT),
+ negAz(negAz), negZe(negZe), negT(negT),
+ defaultQ(0) {}
+
+ SphericalCoordsImpl* clone() const { return new SphericalCoordsImpl(*this); }
+
+ RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const;
+
+ void copyOutDefaultQImpl(int nq, Real* q) const {
+ SimTK_ASSERT(nq==3,
+ "MobilizedBody::SphericalCoordsImpl::copyOutDefaultQImpl(): wrong number of q's");
+ Vec3::updAs(q) = defaultQ;
+ }
+
+ SimTK_DOWNCAST(SphericalCoordsImpl, MobilizedBodyImpl);
+private:
+ friend class MobilizedBody::SphericalCoords;
+
+ Real az0, ze0; // angle offsets
+ CoordinateAxis axisT; // translation axis (X or Z)
+ bool negAz, negZe, negT; // true if we should negate the corresponding coordinate
+
+ Vec3 defaultQ; // two angles in radians followed by a displacement
+};
+
+class MobilizedBody::GimbalImpl : public MobilizedBodyImpl {
+public:
+ explicit GimbalImpl(Direction d)
+ : MobilizedBodyImpl(d), defaultRadius(Real(0.1)), defaultQ(0) { }
+ GimbalImpl* clone() const { return new GimbalImpl(*this); }
+
+ RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const;
+
+ void copyOutDefaultQImpl(int nq, Real* q) const {
+ SimTK_ASSERT(nq==3,
+ "MobilizedBody::GimbalImpl::copyOutDefaultQImpl(): wrong number of q's");
+ Vec3::updAs(q) = defaultQ;
+ }
+
+ void calcDecorativeGeometryAndAppendImpl
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const;
+
+ void setDefaultRadius(Real r) {
+ assert(r>0);
+ invalidateTopologyCache();
+ defaultRadius=r;
+ }
+ Real getDefaultRadius() const {return defaultRadius;}
+
+ SimTK_DOWNCAST(GimbalImpl, MobilizedBodyImpl);
+private:
+ friend class MobilizedBody::Gimbal;
+ Real defaultRadius; // used for visualization only
+ Vec3 defaultQ; // the three angles in radians
+};
+
+class MobilizedBody::BushingImpl : public MobilizedBodyImpl {
+public:
+ explicit BushingImpl(Direction d)
+ : MobilizedBodyImpl(d), defaultQ(0) { }
+ BushingImpl* clone() const { return new BushingImpl(*this); }
+
+ RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const;
+
+ void copyOutDefaultQImpl(int nq, Real* q) const {
+ SimTK_ASSERT(nq==6,
+ "MobilizedBody::BushingImpl::copyOutDefaultQImpl(): wrong number of q's");
+ Vec6::updAs(q) = defaultQ;
+ }
+
+ SimTK_DOWNCAST(BushingImpl, MobilizedBodyImpl);
+private:
+ friend class MobilizedBody::Bushing;
+ Vec6 defaultQ; // 3 angles in radians, then p_FM
+};
+
+class MobilizedBody::BallImpl : public MobilizedBodyImpl {
+public:
+ explicit BallImpl(Direction d)
+ : MobilizedBodyImpl(d), defaultRadius(Real(0.1)), defaultQ() {} // (1,0,0,0), the identity rotation
+ BallImpl* clone() const { return new BallImpl(*this); }
+
+ RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const;
+
+ void copyOutDefaultQImpl(int nq, Real* q) const {
+ SimTK_ASSERT(nq==4||nq==3,
+ "MobilizedBody::BallImpl::copyOutDefaultQImpl(): wrong number of q's");
+ if (nq==4)
+ Vec4::updAs(q) = defaultQ.asVec4();
+ else
+ Vec3::updAs(q) = Rotation(defaultQ).convertRotationToBodyFixedXYZ();
+ }
+
+ void calcDecorativeGeometryAndAppendImpl
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const;
+
+ void setDefaultRadius(Real r) {
+ assert(r>0);
+ invalidateTopologyCache();
+ defaultRadius=r;
+ }
+ Real getDefaultRadius() const {return defaultRadius;}
+
+ SimTK_DOWNCAST(BallImpl, MobilizedBodyImpl);
+private:
+ friend class MobilizedBody::Ball;
+ Real defaultRadius; // used for visualization only
+ Quaternion defaultQ; // the default orientation
+};
+
+class MobilizedBody::EllipsoidImpl : public MobilizedBodyImpl {
+public:
+ explicit EllipsoidImpl(Direction d)
+ : MobilizedBodyImpl(d), defaultRadii(Real(0.5),Real(1./3.),Real(0.25)),
+ defaultQ() { } // default is (1,0,0,0), the identity rotation
+ EllipsoidImpl* clone() const { return new EllipsoidImpl(*this); }
+
+ RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const;
+
+ void copyOutDefaultQImpl(int nq, Real* q) const {
+ SimTK_ASSERT(nq==4||nq==3,
+ "MobilizedBody::EllipsoidImpl::copyOutDefaultQImpl(): wrong number of q's");
+ if (nq==4)
+ Vec4::updAs(q) = defaultQ.asVec4();
+ else
+ Vec3::updAs(q) = Rotation(defaultQ).convertRotationToBodyFixedXYZ();
+ }
+
+ void calcDecorativeGeometryAndAppendImpl
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const;
+
+ void setDefaultRadii(const Vec3& r) {
+ assert(r[0]>0 && r[1]>0 && r[2]>0);
+ invalidateTopologyCache();
+ defaultRadii=r;
+ }
+ const Vec3& getDefaultRadii() const {return defaultRadii;}
+
+ SimTK_DOWNCAST(EllipsoidImpl, MobilizedBodyImpl);
+private:
+ friend class MobilizedBody::Ellipsoid;
+ Vec3 defaultRadii; // used for visualization only
+ Quaternion defaultQ; // the default orientation
+};
+
+class MobilizedBody::TranslationImpl : public MobilizedBodyImpl {
+public:
+ explicit TranslationImpl(Direction d) : MobilizedBodyImpl(d), defaultQ(0) { }
+ TranslationImpl* clone() const { return new TranslationImpl(*this); }
+
+ RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const;
+
+ void copyOutDefaultQImpl(int nq, Real* q) const {
+ SimTK_ASSERT(nq==3,
+ "MobilizedBody::TranslationImpl::copyOutDefaultQImpl(): wrong number of q's");
+ Vec3::updAs(q) = defaultQ;
+ }
+
+ SimTK_DOWNCAST(TranslationImpl, MobilizedBodyImpl);
+private:
+ friend class MobilizedBody::Translation;
+ Vec3 defaultQ; // the three displacements
+};
+
+class MobilizedBody::FreeImpl : public MobilizedBodyImpl {
+public:
+ explicit FreeImpl(Direction d) : MobilizedBodyImpl(d), defaultQOrientation(), defaultQTranslation(0) { }
+ FreeImpl* clone() const { return new FreeImpl(*this); }
+
+ RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const;
+
+ void copyOutDefaultQImpl(int nq, Real* q) const {
+ SimTK_ASSERT(nq==7||nq==6,
+ "MobilizedBody::FreeImpl::copyOutDefaultQImpl(): wrong number of q's");
+ if (nq==7) {
+ Vec4::updAs(q) = defaultQOrientation.asVec4();
+ Vec3::updAs(q+4) = defaultQTranslation;
+ } else {
+ Vec3::updAs(q) = Rotation(defaultQOrientation).convertRotationToBodyFixedXYZ();
+ Vec3::updAs(q+3) = defaultQTranslation;
+ }
+ }
+
+ SimTK_DOWNCAST(FreeImpl, MobilizedBodyImpl);
+private:
+ friend class MobilizedBody::Free;
+ Quaternion defaultQOrientation;
+ Vec3 defaultQTranslation;
+};
+
+class MobilizedBody::LineOrientationImpl : public MobilizedBodyImpl {
+public:
+ explicit LineOrientationImpl(Direction d) : MobilizedBodyImpl(d), defaultQ() { } // 1,0,0,0
+ LineOrientationImpl* clone() const { return new LineOrientationImpl(*this); }
+
+ RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const;
+
+ void copyOutDefaultQImpl(int nq, Real* q) const {
+ SimTK_ASSERT(nq==4||nq==3,
+ "MobilizedBody::LineOrientationImpl::copyOutDefaultQImpl(): wrong number of q's");
+ if (nq==4)
+ Vec4::updAs(q) = defaultQ.asVec4();
+ else
+ Vec3::updAs(q) = Rotation(defaultQ).convertRotationToBodyFixedXYZ();
+ }
+
+ SimTK_DOWNCAST(LineOrientationImpl, MobilizedBodyImpl);
+private:
+ friend class MobilizedBody::LineOrientation;
+ Quaternion defaultQ;
+};
+
+class MobilizedBody::FreeLineImpl : public MobilizedBodyImpl {
+public:
+ explicit FreeLineImpl(Direction d) : MobilizedBodyImpl(d), defaultQOrientation(), defaultQTranslation(0) { }
+ FreeLineImpl* clone() const { return new FreeLineImpl(*this); }
+
+ RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const;
+
+ void copyOutDefaultQImpl(int nq, Real* q) const {
+ SimTK_ASSERT(nq==7||nq==6,
+ "MobilizedBody::FreeLineImpl::copyOutDefaultQImpl(): wrong number of q's");
+ if (nq==7) {
+ Vec4::updAs(q) = defaultQOrientation.asVec4();
+ Vec3::updAs(q+4) = defaultQTranslation;
+ } else {
+ Vec3::updAs(q) = Rotation(defaultQOrientation).convertRotationToBodyFixedXYZ();
+ Vec3::updAs(q+3) = defaultQTranslation;
+ }
+ }
+
+ SimTK_DOWNCAST(FreeLineImpl, MobilizedBodyImpl);
+private:
+ friend class MobilizedBody::FreeLine;
+ Quaternion defaultQOrientation;
+ Vec3 defaultQTranslation;
+};
+
+class MobilizedBody::WeldImpl : public MobilizedBodyImpl {
+public:
+ explicit WeldImpl(Direction d) : MobilizedBodyImpl(d) { }
+ WeldImpl* clone() const { return new WeldImpl(*this); }
+
+ RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const;
+
+ void copyOutDefaultQImpl(int nq, Real* q) const {
+ SimTK_ASSERT(nq==0,
+ "MobilizedBody::WeldImpl::copyOutDefaultQImpl(): wrong number of q's");
+ }
+
+ SimTK_DOWNCAST(WeldImpl, MobilizedBodyImpl);
+private:
+ friend class MobilizedBody::Weld;
+ // no q's
+};
+
+class MobilizedBody::GroundImpl : public MobilizedBodyImpl {
+public:
+ GroundImpl() : MobilizedBodyImpl(MobilizedBody::Forward) { }
+ GroundImpl* clone() const { return new GroundImpl(*this); }
+
+ RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const;
+
+ void copyOutDefaultQImpl(int nq, Real* q) const {
+ SimTK_ASSERT(nq==0,
+ "MobilizedBody::GroundImpl::copyOutDefaultQImpl(): wrong number of q's");
+ }
+
+ SimTK_DOWNCAST(GroundImpl, MobilizedBodyImpl);
+private:
+ friend class MobilizedBody::Ground;
+ // no q's
+};
+
+class MobilizedBody::ScrewImpl : public MobilizedBodyImpl {
+public:
+ ScrewImpl(Real p, Direction d) : MobilizedBodyImpl(d), defaultPitch(p), defaultQ(0) { }
+
+ Real getDefaultPitch() const {return defaultPitch;}
+ void setDefaultPitch(Real p) {
+ invalidateTopologyCache();
+ defaultPitch=p;
+ }
+
+ ScrewImpl* clone() const { return new ScrewImpl(*this); }
+
+ RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const;
+
+ void copyOutDefaultQImpl(int nq, Real* q) const {
+ SimTK_ASSERT(nq==1,
+ "MobilizedBody::ScrewImpl::copyOutDefaultQImpl(): wrong number of q's");
+ *q = defaultQ;
+ }
+
+ SimTK_DOWNCAST(ScrewImpl, MobilizedBodyImpl);
+private:
+ friend class MobilizedBody::Screw;
+ Real defaultPitch;
+ Real defaultQ; // the angle in radians
+};
+
+/////////////////////////////////////////////////
+// MOBILIZED BODY::CUSTOM::IMPLEMENTATION IMPL //
+/////////////////////////////////////////////////
+
+// This class exists primarily to allow the Custom::Implementation class to keep
+// a pointer to its handle class's CustomImpl class which is derived from MobilizedBodyImpl
+// which has all the goodies that are needed for defining a MobilizedBody.
+//
+// At first this class is the owner of the CustomImpl. Then when this is put in a
+// Custom handle, that handle takes over ownership of the CustomImpl and the
+// CustomImpl takes over ownership of this ImplementationImpl object.
+class MobilizedBody::Custom::ImplementationImpl : public PIMPLImplementation<Implementation, ImplementationImpl> {
+public:
+ // no default constructor
+ explicit ImplementationImpl(CustomImpl* customImpl, int nu, int nq, int nAngles) : isOwner(true), builtInImpl(customImpl), nu(nu), nq(nq), nAngles(nAngles) { }
+ inline ~ImplementationImpl(); // see below -- have to wait for CustomImpl's definition
+
+ // Copying one of these just gives us a new one with a NULL CustomImpl pointer.
+ ImplementationImpl(const ImplementationImpl& src) : isOwner(false), builtInImpl(0), nu(src.nu), nq(src.nq), nAngles(src.nAngles) { }
+
+ ImplementationImpl* clone() const {return new ImplementationImpl(*this);}
+
+ bool isOwnerOfCustomImpl() const {return builtInImpl && isOwner;}
+ CustomImpl* removeOwnershipOfCustomImpl() {
+ assert(isOwnerOfCustomImpl());
+ isOwner=false;
+ return builtInImpl;
+ }
+
+ void setReferenceToCustomImpl(CustomImpl* cimpl) {
+ assert(!builtInImpl); // you can only do this once
+ isOwner=false;
+ builtInImpl = cimpl;
+ }
+
+ bool hasCustomImpl() const {return builtInImpl != 0;}
+
+ const CustomImpl& getCustomImpl() const {
+ assert(builtInImpl);
+ return *builtInImpl;
+ }
+ CustomImpl& updCustomImpl() {
+ assert(builtInImpl);
+ return *builtInImpl;
+ }
+
+ int getNU() const {
+ return nu;
+ }
+
+ int getNQ() const {
+ return nq;
+ }
+
+ int getNumAngles() const {
+ return nAngles;
+ }
+
+private:
+ bool isOwner;
+ CustomImpl* builtInImpl; // just a reference; not owned
+ int nu, nq, nAngles;
+
+ // suppress assignment
+ ImplementationImpl& operator=(const ImplementationImpl&);
+};
+
+/////////////////////////////////
+// MOBILIZED BODY::CUSTOM IMPL //
+/////////////////////////////////
+
+class MobilizedBody::CustomImpl : public MobilizedBodyImpl {
+public:
+ explicit CustomImpl(Direction d) : MobilizedBodyImpl(d), implementation(0) { }
+
+ void takeOwnershipOfImplementation(Custom::Implementation* userImpl);
+
+ explicit CustomImpl(Custom::Implementation* userImpl, Direction d)
+ : MobilizedBodyImpl(d), implementation(0) {
+ assert(userImpl);
+ implementation = userImpl;
+ implementation->updImpl().setReferenceToCustomImpl(this);
+ }
+
+ // Copy constructor
+ CustomImpl(const CustomImpl& src)
+ : MobilizedBodyImpl(src), implementation(0) {
+ if (src.implementation) {
+ implementation = src.implementation->clone();
+ implementation->updImpl().setReferenceToCustomImpl(this);
+ }
+ }
+
+ ~CustomImpl() {
+ if (implementation)
+ delete implementation;
+ }
+
+ CustomImpl* clone() const { return new CustomImpl(*this); }
+
+ const Custom::Implementation& getImplementation() const {
+ assert(implementation);
+ return *implementation;
+ }
+
+ Custom::Implementation& updImplementation() {
+ assert(implementation);
+ return *implementation;
+ }
+
+ RigidBodyNode* createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const;
+
+ void copyOutDefaultQImpl(int nq, Real* q) const {
+ SimTK_ASSERT(nq==getImplementation().getImpl().getNQ() || nq==getImplementation().getImpl().getNQ()-1,
+ "MobilizedBody::CustomImpl::copyOutDefaultQImpl(): wrong number of q's");
+ for (int i = 0; i < nq; ++i)
+ q[i] = 0.0;
+ if (implementation->getImpl().getNumAngles() == 4)
+ q[0] = 1.0;
+ }
+
+ // Forward all the virtuals to the Custom::Implementation virtuals.
+ void realizeTopologyVirtual(State& s) const {getImplementation().realizeTopology(s);}
+ void realizeModelVirtual (State& s) const {getImplementation().realizeModel(s);}
+ void realizeInstanceVirtual(const State& s) const {getImplementation().realizeInstance(s);}
+ void realizeTimeVirtual (const State& s) const {getImplementation().realizeTime(s);}
+ void realizePositionVirtual(const State& s) const {getImplementation().realizePosition(s);}
+ void realizeVelocityVirtual(const State& s) const {getImplementation().realizeVelocity(s);}
+ void realizeDynamicsVirtual(const State& s) const {getImplementation().realizeDynamics(s);}
+ void realizeAccelerationVirtual
+ (const State& s) const {getImplementation().realizeAcceleration(s);}
+ void realizeReportVirtual (const State& s) const {getImplementation().realizeReport(s);}
+
+ void calcDecorativeGeometryAndAppend(const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+ {getImplementation().calcDecorativeGeometryAndAppend(s,stage,geom);}
+
+ SimTK_DOWNCAST(CustomImpl, MobilizedBodyImpl);
+private:
+ friend class MobilizedBody::Custom;
+
+ Custom::Implementation* implementation;
+
+ CustomImpl& operator=(const CustomImpl&); // suppress assignment
+};
+
+// Need definition for CustomImpl here in case we have to delete it.
+inline MobilizedBody::Custom::ImplementationImpl::~ImplementationImpl() {
+ if (isOwner)
+ delete builtInImpl;
+ builtInImpl=0;
+}
+
+////////////////////////////////////////
+// MOBILIZED BODY::FUNCTIONBASED IMPL //
+////////////////////////////////////////
+
+
+class MobilizedBody::FunctionBasedImpl : public MobilizedBody::Custom::Implementation {
+public:
+ //Constructor that uses default axes
+ FunctionBasedImpl(SimbodyMatterSubsystem& matter, int nmobilities, const Array_<const Function*>& functions, const Array_<Array_<int> >& coordIndices)
+ : Implementation(matter, nmobilities, nmobilities, 0), subsystem(matter.getMySubsystemIndex()), nu(nmobilities), cacheIndex(0), functions(functions), coordIndices(coordIndices), referenceCount(new int[1]) {
+ assert(functions.size() == 6);
+ assert(coordIndices.size() == 6);
+ referenceCount[0] = 1;
+ for (int i = 0; i < (int)functions.size(); ++i) {
+ assert(functions[i]->getArgumentSize() == coordIndices[i].size());
+ assert(functions[i]->getMaxDerivativeOrder() >= 2);
+ }
+ Arot = Mat33(1);
+ Atrans = Mat33(1);
+ }
+ FunctionBasedImpl(SimbodyMatterSubsystem& matter, int nmobilities, const Array_<const Function*>& functions, const Array_<Array_<int> >& coordIndices, const Array_<Vec3>& axes)
+ : Implementation(matter, nmobilities, nmobilities, 0), subsystem(matter.getMySubsystemIndex()), nu(nmobilities), cacheIndex(0), functions(functions), coordIndices(coordIndices), referenceCount(new int[1]) {
+ assert(functions.size() == 6);
+ assert(coordIndices.size() == 6);
+ assert(axes.size() == 6);
+ referenceCount[0] = 1;
+ for (int i = 0; i < (int)functions.size(); ++i) {
+ assert(functions[i]->getArgumentSize() == coordIndices[i].size());
+ assert(functions[i]->getMaxDerivativeOrder() >= 2);
+ }
+ double tol = 1e-5;
+ // Verify that none of the rotation axes are colinear
+ assert((axes[0]%axes[1]).norm() > tol);
+ assert((axes[0]%axes[2]).norm() > tol);
+ assert((axes[1]%axes[2]).norm() > tol);
+ // Verify that none of the translational axes are colinear
+ assert((axes[3]%axes[4]).norm() > tol);
+ assert((axes[3]%axes[5]).norm() > tol);
+ assert((axes[4]%axes[5]).norm() > tol);
+
+ Arot = Mat33(axes[0].normalize(), axes[1].normalize(), axes[2].normalize());
+ Atrans = Mat33(axes[3].normalize(), axes[4].normalize(), axes[5].normalize());
+ }
+
+ ~FunctionBasedImpl() {
+ if (--referenceCount[0] == 0) {
+ for (int i = 0; i < (int) functions.size(); i++)
+ delete functions[i];
+ delete[] referenceCount;
+ }
+ }
+
+ MobilizedBody::Custom::Implementation* clone() const {
+ referenceCount[0]++;
+ return new FunctionBasedImpl(*this);
+ }
+
+ Transform calcMobilizerTransformFromQ(const State& s, int nq, const Real* q) const {
+ // Initialize the tranformation to be returned
+ Transform X(Vec3(0));
+ Vec6 spatialCoords;
+
+ // Get the spatial cooridinates as a function of the q's
+ for(int i=0; i < 6; i++){
+ //Coordinates for this function
+ int nc = coordIndices[i].size();
+ Vector fcoords(nc);
+
+ for(int j=0; j < nc; j++)
+ fcoords(j) = q[coordIndices[i][j]];
+
+ //default behavior of constant function should take a Vector of length 0
+ spatialCoords(i) = functions[i]->calcValue(fcoords);
+ }
+
+/*
+ UnitVec3 axis1;
+ axis1.getAs(&Arot(0,0));
+ UnitVec3 axis2;
+ axis2.getAs(&Arot(0,1));
+ UnitVec3 axis3;
+ axis3.getAs(&Arot(0,2));
+*/
+
+ //X.updR().setRotationToBodyFixedXYZ(spatialCoords.getSubVec<3>(0));
+ X.updR().setRotationFromMat33TrustMe(Rotation(spatialCoords(0), UnitVec3::getAs(&Arot(0,0)))*
+ Rotation(spatialCoords(1), UnitVec3::getAs(&Arot(0,1)))*Rotation(spatialCoords(2), UnitVec3::getAs(&Arot(0,2))));
+ X.updP() = spatialCoords(3)*UnitVec3::getAs(&Atrans(0,0))+spatialCoords(4)*UnitVec3::getAs(&Atrans(0,1))
+ +spatialCoords(5)*UnitVec3::getAs(&Atrans(0,2));
+
+ return X;
+ }
+
+ SpatialVec multiplyByHMatrix(const State& s, int nu, const Real* u) const {
+
+ switch (nu) {
+ case 1: {
+ // Check that the H matrices in the cache are valid
+ if (!(Value<CacheInfo<1> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidH))
+ updateH(s);
+
+ Mat<2,1,Vec3> h = Value<CacheInfo<1> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().h;
+ return h*Vec1::getAs(u);
+ }
+ case 2: {
+ // Check that the H matrices in the cache are valid
+ if (!(Value<CacheInfo<2> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidH))
+ updateH(s);
+
+ Mat<2,2,Vec3> h = Value<CacheInfo<2> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().h;
+ return h*Vec2::getAs(u);
+ }
+ case 3: {
+ // Check that the H matrices in the cache are valid
+ if (!(Value<CacheInfo<3> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidH))
+ updateH(s);
+
+ Mat<2,3,Vec3> h = Value<CacheInfo<3> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().h;
+ return h*Vec3::getAs(u);
+ }
+ case 4: {
+ // Check that the H matrices in the cache are valid
+ if (!(Value<CacheInfo<4> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidH))
+ updateH(s);
+
+ Mat<2,4,Vec3> h = Value<CacheInfo<4> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().h;
+ return h*Vec4::getAs(u);
+ }
+ case 5: {
+ // Check that the H matrices in the cache are valid
+ if (!(Value<CacheInfo<5> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidH))
+ updateH(s);
+
+ Mat<2,5,Vec3> h = Value<CacheInfo<5> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().h;
+ return h*Vec5::getAs(u);
+ }
+ case 6: {
+ // Check that the H matrices in the cache are valid
+ if (!(Value<CacheInfo<6> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidH))
+ updateH(s);
+
+ Mat<2,6,Vec3> h = Value<CacheInfo<6> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().h;
+ return h*Vec6::getAs(u);
+ }
+ }
+ SimTK_THROW5(SimTK::Exception::ValueOutOfRange, "nu", 1, nu, 6, "MobilizedBody::FunctionBasedImpl::multiplyByHMatrix");
+ }
+
+ void multiplyByHTranspose(const State& s, const SpatialVec& F, int nu, Real* f) const {
+
+ switch (nu) {
+ case 1: {
+ // Check that the H and Hdot matrices in the cache are valid
+ if (!(Value<CacheInfo<1> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidH))
+ updateH(s);
+
+ Mat<2,1,Vec3> h = Value<CacheInfo<1> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().h;
+ Vec1::updAs(f) = ~h*F;
+ return;
+ }
+ case 2: {
+ // Check that the H and Hdot matrices in the cache are valid
+ if (!(Value<CacheInfo<2> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidH))
+ updateH(s);
+
+ Mat<2,2,Vec3> h = Value<CacheInfo<2> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().h;
+ Vec2::updAs(f) = ~h*F;
+ return;
+ }
+ case 3: {
+ // Check that the H and Hdot matrices in the cache are valid
+ if (!(Value<CacheInfo<3> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidH))
+ updateH(s);
+
+ Mat<2,3,Vec3> h = Value<CacheInfo<3> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().h;
+ Vec3::updAs(f) = ~h*F;
+ return;
+ }
+ case 4: {
+ // Check that the H and Hdot matrices in the cache are valid
+ if (!(Value<CacheInfo<4> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidH))
+ updateH(s);
+
+ Mat<2,4,Vec3> h = Value<CacheInfo<4> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().h;
+ Vec4::updAs(f) = ~h*F;
+ return;
+ }
+ case 5: {
+ // Check that the H and Hdot matrices in the cache are valid
+ if (!(Value<CacheInfo<5> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidH))
+ updateH(s);
+
+ Mat<2,5,Vec3> h = Value<CacheInfo<5> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().h;
+ Vec5::updAs(f) = ~h*F;
+ return;
+ }
+ case 6: {
+ // Check that the H and Hdot matrices in the cache are valid
+ if (!(Value<CacheInfo<6> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidH))
+ updateH(s);
+
+ Mat<2,6,Vec3> h = Value<CacheInfo<6> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().h;
+ Vec6::updAs(f) = ~h*F;
+ return;
+ }
+ }
+ SimTK_THROW5(SimTK::Exception::ValueOutOfRange, "nu", 1, nu, 6, "MobilizedBody::FunctionBasedImpl::multiplyByHTranspose");
+ }
+
+ SpatialVec multiplyByHDotMatrix(const State& s, int nu, const Real* u) const {
+
+ switch (nu) {
+ case 1: {
+ // Check that the H and Hdot matrices in the cache are valid
+ if (!(Value<CacheInfo<1> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidHdot))
+ updateHdot(s);
+
+ Mat<2,1,Vec3> hdot = Value<CacheInfo<1> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().hdot;
+ return hdot*Vec1::getAs(u);
+ }
+ case 2: {
+ // Check that the H and Hdot matrices in the cache are valid
+ if (!(Value<CacheInfo<2> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidHdot))
+ updateHdot(s);
+
+ Mat<2,2,Vec3> hdot = Value<CacheInfo<2> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().hdot;
+ return hdot*Vec2::getAs(u);
+ }
+ case 3: {
+ // Check that the H and Hdot matrices in the cache are valid
+ if (!(Value<CacheInfo<3> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidHdot))
+ updateHdot(s);
+
+ Mat<2,3,Vec3> hdot = Value<CacheInfo<3> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().hdot;
+ return hdot*Vec3::getAs(u);
+ }
+ case 4: {
+ // Check that the H and Hdot matrices in the cache are valid
+ if (!(Value<CacheInfo<4> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidHdot))
+ updateHdot(s);
+
+ Mat<2,4,Vec3> hdot = Value<CacheInfo<4> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().hdot;
+ return hdot*Vec4::getAs(u);
+ }
+ case 5: {
+ // Check that the H and Hdot matrices in the cache are valid
+ if (!(Value<CacheInfo<5> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidHdot))
+ updateHdot(s);
+
+ Mat<2,5,Vec3> hdot = Value<CacheInfo<5> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().hdot;
+ return hdot*Vec5::getAs(u);
+ }
+ case 6: {
+ // Check that the H and Hdot matrices in the cache are valid
+ if (!(Value<CacheInfo<6> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidHdot))
+ updateHdot(s);
+
+ Mat<2,6,Vec3> hdot = Value<CacheInfo<6> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().hdot;
+ return hdot*Vec6::getAs(u);
+ }
+ }
+ SimTK_THROW5(SimTK::Exception::ValueOutOfRange, "nu", 1, nu, 6, "MobilizedBody::FunctionBasedImpl::multiplyByHDotMatrix");
+ }
+
+ void multiplyByHDotTranspose(const State& s, const SpatialVec& F, int nu, Real* f) const {
+
+ switch (nu) {
+ case 1: {
+ // Check that the H and Hdot matrices in the cache are valid
+ if (!(Value<CacheInfo<1> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidHdot))
+ updateHdot(s);
+
+ Mat<2,1,Vec3> hdot = Value<CacheInfo<1> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().hdot;
+ Vec1::updAs(f) = ~hdot*F;
+ return;
+ }
+ case 2: {
+ // Check that the H and Hdot matrices in the cache are valid
+ if (!(Value<CacheInfo<2> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidHdot))
+ updateHdot(s);
+
+ Mat<2,2,Vec3> hdot = Value<CacheInfo<2> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().hdot;
+ Vec2::updAs(f) = ~hdot*F;
+ return;
+ }
+ case 3: {
+ // Check that the H and Hdot matrices in the cache are valid
+ if (!(Value<CacheInfo<3> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidHdot))
+ updateHdot(s);
+
+ Mat<2,3,Vec3> hdot = Value<CacheInfo<3> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().hdot;
+ Vec3::updAs(f) = ~hdot*F;
+ return;
+ }
+ case 4: {
+ // Check that the H and Hdot matrices in the cache are valid
+ if (!(Value<CacheInfo<4> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidHdot))
+ updateHdot(s);
+
+ Mat<2,4,Vec3> hdot = Value<CacheInfo<4> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().hdot;
+ Vec4::updAs(f) = ~hdot*F;
+ return;
+ }
+ case 5: {
+ // Check that the H and Hdot matrices in the cache are valid
+ if (!(Value<CacheInfo<5> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidHdot))
+ updateHdot(s);
+
+ Mat<2,5,Vec3> hdot = Value<CacheInfo<5> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().hdot;
+ Vec5::updAs(f) = ~hdot*F;
+ return;
+ }
+ case 6: {
+ // Check that the H and Hdot matrices in the cache are valid
+ if (!(Value<CacheInfo<6> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidHdot))
+ updateHdot(s);
+
+ Mat<2,6,Vec3> hdot = Value<CacheInfo<6> >::downcast(s.getCacheEntry(subsystem, cacheIndex)).get().hdot;
+ Vec6::updAs(f) = ~hdot*F;
+ return;
+ }
+ }
+ SimTK_THROW5(SimTK::Exception::ValueOutOfRange, "nu", 1, nu, 6, "MobilizedBody::FunctionBasedImpl::multiplyByHDotTranspose");
+ }
+
+ void realizeTopology(State& s) const {
+ switch (nu) {
+ case 1:
+ cacheIndex = s.allocateCacheEntry(subsystem, Stage::Topology, new Value<CacheInfo<1> >());
+ break;
+ case 2:
+ cacheIndex = s.allocateCacheEntry(subsystem, Stage::Topology, new Value<CacheInfo<2> >());
+ break;
+ case 3:
+ cacheIndex = s.allocateCacheEntry(subsystem, Stage::Topology, new Value<CacheInfo<3> >());
+ break;
+ case 4:
+ cacheIndex = s.allocateCacheEntry(subsystem, Stage::Topology, new Value<CacheInfo<4> >());
+ break;
+ case 5:
+ cacheIndex = s.allocateCacheEntry(subsystem, Stage::Topology, new Value<CacheInfo<5> >());
+ break;
+ case 6:
+ cacheIndex = s.allocateCacheEntry(subsystem, Stage::Topology, new Value<CacheInfo<6> >());
+ break;
+ }
+ }
+
+ void realizePosition(const State& s) const {
+ switch (nu) {
+ case 1: {
+ // invalidate H and Hdot matrices
+ Value<CacheInfo<1> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidH = false;
+ break;
+ }
+ case 2: {
+ // invalidate H and Hdot matrices
+ Value<CacheInfo<2> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidH = false;
+ break;
+ }
+ case 3: {
+ // invalidate H and Hdot matrices
+ Value<CacheInfo<3> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidH = false;
+ break;
+ }
+ case 4: {
+ // invalidate H and Hdot matrices
+ Value<CacheInfo<4> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidH = false;
+ break;
+ }
+ case 5: {
+ // invalidate H and Hdot matrices
+ Value<CacheInfo<5> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidH = false;
+ break;
+ }
+ case 6: {
+ // invalidate H and Hdot matrices
+ Value<CacheInfo<6> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidH = false;
+ break;
+ }
+ }
+ }
+
+ void realizeVelocity(const State& s) const {
+ switch (nu) {
+ case 1: {
+ // invalidate H and Hdot matrices
+ Value<CacheInfo<1> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidHdot = false;
+ break;
+ }
+ case 2: {
+ // invalidate H and Hdot matrices
+ Value<CacheInfo<2> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidHdot = false;
+ break;
+ }
+ case 3: {
+ // invalidate H and Hdot matrices
+ Value<CacheInfo<3> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidHdot = false;
+ break;
+ }
+ case 4: {
+ // invalidate H and Hdot matrices
+ Value<CacheInfo<4> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidHdot = false;
+ break;
+ }
+ case 5: {
+ // invalidate H and Hdot matrices
+ Value<CacheInfo<5> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidHdot = false;
+ break;
+ }
+ case 6: {
+ // invalidate H and Hdot matrices
+ Value<CacheInfo<6> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd().isValidHdot = false;
+ break;
+ }
+ }
+ }
+
+ void updateH(const State& s) const {
+ // Get mobilizer kinematics
+ Vector q = getQ(s);
+ Vector u = getU(s);
+ switch (nu) {
+ case 1: {
+ CacheInfo<1>& cache = Value<CacheInfo<1> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd();
+ cache.buildH(q, u, getMobilizerTransform(s), functions, coordIndices, Arot, Atrans);
+ // H matrix is now valid
+ cache.isValidH = true;
+ break;
+ }
+ case 2: {
+ CacheInfo<2>& cache = Value<CacheInfo<2> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd();
+ cache.buildH(q, u, getMobilizerTransform(s), functions, coordIndices, Arot, Atrans);
+ // H matrix is now valid
+ cache.isValidH = true;
+ break;
+ }
+ case 3: {
+ CacheInfo<3>& cache = Value<CacheInfo<3> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd();
+ cache.buildH(q, u, getMobilizerTransform(s), functions, coordIndices, Arot, Atrans);
+ // H matrix is now valid
+ cache.isValidH = true;
+ break;
+ }
+ case 4: {
+ CacheInfo<4>& cache = Value<CacheInfo<4> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd();
+ cache.buildH(q, u, getMobilizerTransform(s), functions, coordIndices, Arot, Atrans);
+ // H matrix is now valid
+ cache.isValidH = true;
+ break;
+ }
+ case 5: {
+ CacheInfo<5>& cache = Value<CacheInfo<5> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd();
+ cache.buildH(q, u, getMobilizerTransform(s), functions, coordIndices, Arot, Atrans);
+ // H matrix is now valid
+ cache.isValidH = true;
+ break;
+ }
+ case 6: {
+ CacheInfo<6>& cache = Value<CacheInfo<6> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd();
+ cache.buildH(q, u, getMobilizerTransform(s), functions, coordIndices, Arot, Atrans);
+ // H matrix is now valid
+ cache.isValidH = true;
+ break;
+ }
+ }
+ }
+
+ void updateHdot(const State& s) const {
+ // Get mobilizer kinematics
+ Vector q = getQ(s);
+ Vector u = getU(s);
+ switch (nu) {
+ case 1: {
+ CacheInfo<1>& cache = Value<CacheInfo<1> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd();
+ cache.buildHdot(q, u, getMobilizerTransform(s), functions, coordIndices, Arot, Atrans);
+ // Hdot matrix is now valid
+ cache.isValidHdot = true;
+ break;
+ }
+ case 2: {
+ CacheInfo<2>& cache = Value<CacheInfo<2> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd();
+ cache.buildHdot(q, u, getMobilizerTransform(s), functions, coordIndices, Arot, Atrans);
+ // Hdot matrix is now valid
+ cache.isValidHdot = true;
+ break;
+ }
+ case 3: {
+ CacheInfo<3>& cache = Value<CacheInfo<3> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd();
+ cache.buildHdot(q, u, getMobilizerTransform(s), functions, coordIndices, Arot, Atrans);
+ // Hdot matrix is now valid
+ cache.isValidHdot = true;
+ break;
+ }
+ case 4: {
+ CacheInfo<4>& cache = Value<CacheInfo<4> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd();
+ cache.buildHdot(q, u, getMobilizerTransform(s), functions, coordIndices, Arot, Atrans);
+ // Hdot matrix is now valid
+ cache.isValidHdot = true;
+ break;
+ }
+ case 5: {
+ CacheInfo<5>& cache = Value<CacheInfo<5> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd();
+ cache.buildHdot(q, u, getMobilizerTransform(s), functions, coordIndices, Arot, Atrans);
+ // Hdot matrix is now valid
+ cache.isValidHdot = true;
+ break;
+ }
+ case 6: {
+ CacheInfo<6>& cache = Value<CacheInfo<6> >::downcast(s.updCacheEntry(subsystem, cacheIndex)).upd();
+ cache.buildHdot(q, u, getMobilizerTransform(s), functions, coordIndices, Arot, Atrans);
+ // Hdot matrix is now valid
+ cache.isValidHdot = true;
+ break;
+ }
+ }
+ }
+
+private:
+ const SubsystemIndex subsystem;
+ const int nu;
+ mutable CacheEntryIndex cacheIndex;
+ const Array_<const Function*> functions;
+ const Array_<Array_<int> > coordIndices;
+ int* referenceCount;
+ //const Array_<Vec3> axes;
+ Mat33 Arot, Atrans;
+ template <int N> class CacheInfo {
+ public:
+ CacheInfo() : isValidH(false), isValidHdot(false) { }
+
+ void buildH(Vector& q, Vector& u, const Transform& X_FM, const Array_<const Function*>& functions, const Array_<Array_<int> >& coordIndices, const Mat33 Arot, const Mat33 Atrans)
+ {
+ // Build the Fq and Fqq matrices of partials of the spatial functions with respect to the gen coordinates, q
+ // Cycle through each row (function describing spatial coordinate)
+ Fq = Mat<6,N>(0);
+ Vec6 spatialCoords(0);
+ Array_<int> deriv(1);
+ Vector fcoords(coordIndices[0].size());
+
+ for(int i=0; i < 6; i++){
+ // Determine the number of coordinates for this function
+ int nc = coordIndices[i].size();
+ if (fcoords.size() != nc)
+ fcoords.resize(nc);
+
+ if (nc > 0) {
+ // Get coordinate values to evaluate the function
+ for(int k = 0; k < nc; k++)
+ fcoords(k) = q(coordIndices[i][k]);
+
+ for (int j = 0; j < nc; j++) {
+ deriv[0] = j;
+ Fq(i, coordIndices[i][j]) = functions[i]->calcDerivative(deriv, fcoords);
+ }
+ //default behavior of constant function should take a Vector of length 0
+ spatialCoords(i) = functions[i]->calcValue(fcoords);
+ }
+
+ }
+ // Note rotations are body fixed sequences, and so taking the derivative yields a body-fixed angular velocity
+ // and therefore, must be transformed to the parent frame.
+
+ // omega = [a1, R_F1*a2, R_F2*a3]*{Theta_dot(q)}, where Theta_dot(q) is described by the first three functions
+ // = [W*[Fq_theta]*qdot
+ // vel = [A]*{X_dot(q)}, where X_dot(q) is described by the last three functions
+ // = [A]*[Fq_x]*qdot
+
+ Rotation R_F1 = Rotation(spatialCoords(0), UnitVec3::getAs(&Arot(0,0)));
+ Rotation R_F2 = R_F1*Rotation(spatialCoords(1), UnitVec3::getAs(&Arot(0,1)));
+
+ Mat31 temp;
+ Mat33 W(UnitVec3::getAs(&Arot(0,0)), R_F1*(UnitVec3::getAs(&Arot(0,1))), R_F2*(UnitVec3::getAs(&Arot(0,2))));
+
+ for(int i=0; i < N; i++){
+ temp = W*(Fq.template getSubMat<3,1>(0,i));
+ h(0,i) = Vec3::getAs(&temp(0,0));
+ temp = Atrans*(Fq.template getSubMat<3,1>(3,i));
+ h(1,i) = Vec3::getAs(&temp(0,0));
+ }
+ }
+
+ void buildHdot(Vector& q, Vector& u, const Transform& X_FM, const Array_<const Function*>& functions, const Array_<Array_<int> >& coordIndices, const Mat33 Arot, const Mat33 Atrans)
+ {
+ Mat<6,N> Fqdot(0);
+ Vec6 spatialCoords;
+ Array_<int> derivs(2);
+ Vector fcoords(coordIndices[0].size());
+
+ for(int i=0; i < 6; i++){
+ // Determine the number of coordinates for this function
+ int nc = coordIndices[i].size();
+ if (fcoords.size() != nc)
+ fcoords.resize(nc);
+
+ if (nc > 0) {
+ // Get coordinate values to evaluate the function
+ for(int k = 0; k < nc; k++)
+ fcoords(k) = q(coordIndices[i][k]);
+
+ // function is dependent on a mobility if its index is in the list of function coordIndices
+ // cycle through the mobilities
+ for (int j = 0; j < nc; j++) {
+ derivs[0] = j;
+ for (int k = 0; k < nc; k++) {
+ derivs[1] = k;
+ Fqdot(i, coordIndices[i][j]) += functions[i]->calcDerivative(derivs, fcoords)*u[coordIndices[i][k]];
+ }
+ }
+ }
+ //default behavior of constant function should take a Vector of length 0
+ spatialCoords(i) = functions[i]->calcValue(fcoords);
+ }
+
+ Rotation R_F1 = Rotation(spatialCoords(0), UnitVec3::getAs(&Arot(0,0)));
+ Rotation R_F2 = R_F1*Rotation(spatialCoords(1), UnitVec3::getAs(&Arot(0,1)));
+
+ Mat33 W(UnitVec3::getAs(&Arot(0,0)), R_F1*(UnitVec3::getAs(&Arot(0,1))), R_F2*(UnitVec3::getAs(&Arot(0,2))));
+
+ // Hdot_theta = [Wdot]*[Fq] + [W][Fqdot]
+ // Hdot_x = [A]*[Fqdot]
+ Real *up = &u[0];
+ Vec<N> uv = Vec<N>::getAs(up);
+ Vec<N> uv1 = uv;
+ Vec<N> uv2 = uv;
+
+ for(int i=1; i < N; i++){
+ uv1(i) = 0;
+ }
+ if(N > 2)
+ uv2(2) = 0;
+
+ // Spatial velocity
+ SpatialVec V = h*uv;
+ SpatialVec V1 = h*uv1;
+ SpatialVec V2 = h*uv2;
+ // First V[0] is angular velocity, omega
+ //Mat33 Wdot(Vec3(0), Vec3(V[0](0),0,0)%(Vec3::getAs(&W(0,1))), Vec3(V[0](0),V[0](1),0)%(Vec3::getAs(&W(0,2))));
+ Mat33 Wdot(Vec3(0), V1[0]%(Vec3::getAs(&W(0,1))), V2[0]%(Vec3::getAs(&W(0,2))));
+
+ //Sanity check Omega == V[0]
+ Mat31 Omega = W*(Fq.template getSubMat<3,N>(0,0))*(Mat<N,1>::getAs(up));
+
+ Mat31 temp;
+ for(int i=0; i < N; i++){
+ temp = Wdot*(Fq.template getSubMat<3,1>(0,i))+W*(Fqdot.template getSubMat<3,1>(0,i));
+ hdot(0,i) = Vec3::getAs(&temp(0,0));
+ temp = Atrans*(Fqdot.template getSubMat<3,1>(3,i));
+ hdot(1,i) = Vec3::getAs(&temp(0,0));
+ }
+ }
+
+ Mat<2,N,Vec3> h, hdot;
+ Mat<6,N> Fq;
+ bool isValidH;
+ bool isValidHdot;
+ };
+
+};
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOBILIZED_BODY_IMPL_H_
diff --git a/Simbody/src/Motion.cpp b/Simbody/src/Motion.cpp
new file mode 100644
index 0000000..7fd2dc9
--- /dev/null
+++ b/Simbody/src/Motion.cpp
@@ -0,0 +1,404 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Motion.h"
+#include "simbody/internal/MobilizedBody.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+
+#include "MotionImpl.h"
+#include "MobilizedBodyImpl.h"
+
+namespace SimTK {
+
+//==============================================================================
+// MOTION
+//==============================================================================
+const MobilizedBody& Motion::getMobilizedBody() const
+{ return getImpl().getMobilizedBodyImpl().getMyHandle(); }
+
+Motion::Level Motion::getLevel(const State& s) const
+{ return getImpl().getLevel(s); }
+
+Motion::Method Motion::getLevelMethod(const State& s) const
+{ return getImpl().getLevelMethod(s); }
+
+
+void Motion::disable(State& s) const
+{ getImpl().disable(s); }
+
+void Motion::enable(State& s) const
+{ getImpl().enable(s); }
+
+bool Motion::isDisabled(const State& s) const
+{ return getImpl().isDisabled(s); }
+
+void Motion::setDisabledByDefault(bool shouldBeDisabled)
+{ updImpl().setDisabledByDefault(shouldBeDisabled); }
+
+bool Motion::isDisabledByDefault() const
+{ return getImpl().isDisabledByDefault(); }
+
+void Motion::calcAllMethods(const State& s, Method& qMethod, Method& uMethod,
+ Method& udotMethod) const
+{
+ const Level level = getLevel(s);
+ const Method levelMethod = getLevelMethod(s);
+ Method method[3]; // acc, vel, pos
+ method[level] = levelMethod;
+
+ switch (level) {
+ case Position:
+ method[Velocity]=method[Acceleration]=
+ (levelMethod==Prescribed ? Prescribed : Zero);
+ break;
+ case Velocity:
+ method[Acceleration] = (levelMethod==Prescribed ? Prescribed : Zero);
+ method[Position] = (levelMethod==Zero ? Discrete : Free);
+ break;
+ case Acceleration:
+ method[Velocity] = (levelMethod==Zero ? Discrete : Free);
+ method[Position] = Free;
+ break;
+ default:
+ assert(!"unrecognized level");
+ }
+
+ qMethod = method[Position];
+ uMethod = method[Velocity];
+ udotMethod = method[Acceleration];
+}
+
+/*static*/ const char*
+Motion::nameOfLevel(Level l) {
+ switch (l) {
+ case NoLevel: return "NoLevel";
+ case Acceleration: return "Acceleration";
+ case Velocity: return "Velocity";
+ case Position: return "Position";
+ default:
+ return "*** UNRECOGNIZED Motion::Level ***";
+ }
+}
+
+/*static*/ const char*
+Motion::nameOfMethod(Method m) {
+ switch (m) {
+ case NoMethod: return "NoMethod";
+ case Zero: return "Zero";
+ case Discrete: return "Discrete";
+ case Prescribed: return "Prescribed";
+ case Free: return "Free";
+ case Fast: return "Fast";
+ default:
+ return "*** UNRECOGNIZED Motion::Method ***";
+ }
+}
+
+//==============================================================================
+// MOTION IMPL
+//==============================================================================
+
+void MotionImpl::disable(State& s) const {
+ const MobilizedBodyImpl& mbi = getMobilizedBodyImpl();
+ SBInstanceVars& iv = mbi.getMyMatterSubsystemRep().updInstanceVars(s);
+ iv.prescribedMotionIsDisabled[mbi.getMyMobilizedBodyIndex()] = true;
+}
+void MotionImpl::enable(State& s) const {
+ const MobilizedBodyImpl& mbi = getMobilizedBodyImpl();
+ SBInstanceVars& iv = mbi.getMyMatterSubsystemRep().updInstanceVars(s);
+ iv.prescribedMotionIsDisabled[mbi.getMyMobilizedBodyIndex()] = false;
+}
+bool MotionImpl::isDisabled(const State& s) const {
+ const MobilizedBodyImpl& mbi = getMobilizedBodyImpl();
+ const SBInstanceVars& iv = mbi.getMyMatterSubsystemRep().getInstanceVars(s);
+ return iv.prescribedMotionIsDisabled[mbi.getMyMobilizedBodyIndex()];
+}
+
+const SimbodyMatterSubsystem&
+MotionImpl::getMatterSubsystem() const {
+ return getMobilizedBodyImpl().getMySimbodyMatterSubsystem();
+}
+
+const AbstractValue&
+MotionImpl::getDiscreteVariable(const State& s, DiscreteVariableIndex vx) const {
+ return getMatterSubsystem().getDiscreteVariable(s, vx);
+}
+
+AbstractValue&
+MotionImpl::updDiscreteVariable(State& s, DiscreteVariableIndex vx) const {
+ return getMatterSubsystem().updDiscreteVariable(s, vx);
+}
+
+DiscreteVariableIndex
+MotionImpl::allocateDiscreteVariable(State& s, Stage g, AbstractValue* v) const {
+ return getMatterSubsystem().allocateDiscreteVariable(s, g, v);
+}
+
+
+void MotionImpl::invalidateTopologyCache() const {
+ if (hasMobilizedBody())
+ getMatterSubsystem().invalidateSubsystemTopologyCache();
+}
+
+// These are the default implementations for these virtual methods. They
+// will throw exceptions if called.
+
+void MotionImpl::
+calcPrescribedPositionVirtual(const State& s, int nq, Real* q) const {
+ SimTK_ERRCHK_ALWAYS(!"unimplemented",
+ "MotionImpl::calcPrescribedPositionVirtual()",
+ "A built-in Motion class did not supply a calcPrescribedPositionVirtual() method "
+ "but was apparently expected to do so.");
+ /*NOTREACHED*/
+}
+
+void MotionImpl::
+calcPrescribedPositionDotVirtual(const State& s, int nq, Real* qdot) const {
+ SimTK_ERRCHK_ALWAYS(!"unimplemented",
+ "MotionImpl::calcPrescribedPositionDotVirtual()",
+ "A built-in Motion class did not supply a calcPrescribedPositionDotVirtual() method "
+ "but was apparently expected to do so.");
+ /*NOTREACHED*/
+}
+
+void MotionImpl::
+calcPrescribedPositionDotDotVirtual(const State& s, int nq, Real* qdotdot) const {
+ SimTK_ERRCHK_ALWAYS(!"unimplemented",
+ "MotionImpl::calcPrescribedPositionDotDotVirtual()",
+ "A built-in Motion class did not supply a calcPrescribedPositionDotDotVirtual() method "
+ "but was apparently expected to do so.");
+ /*NOTREACHED*/
+}
+
+void MotionImpl::
+calcPrescribedVelocityVirtual(const State& s, int nu, Real* u) const {
+ SimTK_ERRCHK_ALWAYS(!"unimplemented",
+ "MotionImpl::calcPrescribedVelocityVirtual()",
+ "A built-in Motion class did not supply a calcPrescribedVelocityVirtual() method "
+ "but was apparently expected to do so.");
+ /*NOTREACHED*/
+}
+
+void MotionImpl::
+calcPrescribedVelocityDotVirtual(const State& s, int nu, Real* udot) const {
+ SimTK_ERRCHK_ALWAYS(!"unimplemented",
+ "MotionImpl::calcPrescribedVelocityDotVirtual()",
+ "A built-in Motion class did not supply a calcPrescribedVelocityDotVirtual() method "
+ "but was apparently expected to do so.");
+ /*NOTREACHED*/
+}
+
+void MotionImpl::
+calcPrescribedAccelerationVirtual(const State& s, int nu, Real* udot) const {
+ SimTK_ERRCHK_ALWAYS(!"unimplemented",
+ "MotionImpl::calcPrescribedAccelerationVirtual()",
+ "A built-in Motion class did not supply a calcPrescribedAccelerationVirtual() method "
+ "but was apparently expected to do so.");
+ /*NOTREACHED*/
+}
+
+
+//-------------------------------- Sinusoid ------------------------------------
+//------------------------------------------------------------------------------
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS
+ (Motion::Sinusoid, Motion::SinusoidImpl, Motion);
+
+Motion::Sinusoid::Sinusoid(MobilizedBody& mobod, Motion::Level level,
+ Real amplitude, Real rate, Real phase)
+: Motion(new SinusoidImpl(level, amplitude, rate, phase)) {
+ mobod.adoptMotion(*this);
+}
+
+//--------------------------------- Steady -------------------------------------
+//------------------------------------------------------------------------------
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Motion::Steady, Motion::SteadyImpl, Motion);
+
+
+Motion::Steady::Steady(MobilizedBody& mobod, Real u)
+ : Motion(new SteadyImpl(Vec<6>(u))) {
+ mobod.adoptMotion(*this);
+}
+
+// These are specializations for each of the six possible instantiations
+// of the templatized constructor. Anything else will be unresolved.
+
+template <> SimTK_SIMBODY_EXPORT
+Motion::Steady::Steady(MobilizedBody& mobod, const Vec<1>& u)
+ : Motion(new SteadyImpl(Vec<6>(u[0],0,0,0,0,0))) {
+ mobod.adoptMotion(*this);
+}
+
+template <> SimTK_SIMBODY_EXPORT
+Motion::Steady::Steady(MobilizedBody& mobod, const Vec<2>& u)
+ : Motion(new SteadyImpl(Vec<6>(u[0],u[1],0,0,0,0))) {
+ mobod.adoptMotion(*this);
+}
+
+template <> SimTK_SIMBODY_EXPORT
+Motion::Steady::Steady(MobilizedBody& mobod, const Vec<3>& u)
+ : Motion(new SteadyImpl(Vec<6>(u[0],u[1],u[2],0,0,0))) {
+ mobod.adoptMotion(*this);
+}
+
+template <> SimTK_SIMBODY_EXPORT
+Motion::Steady::Steady(MobilizedBody& mobod, const Vec<4>& u)
+ : Motion(new SteadyImpl(Vec<6>(u[0],u[1],u[2],u[3],0,0))) {
+ mobod.adoptMotion(*this);
+}
+
+template <> SimTK_SIMBODY_EXPORT
+Motion::Steady::Steady(MobilizedBody& mobod, const Vec<5>& u)
+ : Motion(new SteadyImpl(Vec<6>(u[0],u[1],u[2],u[3],u[4],0))) {
+ mobod.adoptMotion(*this);
+}
+
+template <> SimTK_SIMBODY_EXPORT
+Motion::Steady::Steady(MobilizedBody& mobod, const Vec<6>& u)
+ : Motion(new SteadyImpl(u)) {
+ mobod.adoptMotion(*this);
+}
+
+Motion::Steady&
+Motion::Steady::setDefaultRate(Real u)
+{ updImpl().setDefaultRates(Vec6(u)); return *this; }
+Motion::Steady&
+Motion::Steady::setOneDefaultRate(MobilizerUIndex ux, Real u)
+{ updImpl().setOneDefaultRate(ux,u); return *this; }
+Real Motion::Steady::getOneDefaultRate(MobilizerUIndex ux) const
+{ return getImpl().getOneDefaultRate(ux); }
+
+void Motion::Steady::setRate(State& s, Real u) const
+{ getImpl().setRates(s, Vec6(u)); }
+void Motion::Steady::setOneRate(State& s, MobilizerUIndex ux, Real u) const
+{ getImpl().setOneRate(s,ux,u); }
+Real Motion::Steady::getOneRate(const State& s, MobilizerUIndex ux) const
+{ return getImpl().getOneRate(s,ux); }
+
+template <> SimTK_SIMBODY_EXPORT Motion::Steady&
+Motion::Steady::setDefaultRates(const Vec<1>& u)
+{ updImpl().setDefaultRates(Vec6(u[0],0,0,0,0,0)); return *this; }
+template <> SimTK_SIMBODY_EXPORT Motion::Steady&
+Motion::Steady::setDefaultRates(const Vec<2>& u)
+{ updImpl().setDefaultRates(Vec6(u[0],u[1],0,0,0,0)); return *this; }
+template <> SimTK_SIMBODY_EXPORT Motion::Steady&
+Motion::Steady::setDefaultRates(const Vec<3>& u)
+{ updImpl().setDefaultRates(Vec6(u[0],u[1],u[2],0,0,0)); return *this; }
+template <> SimTK_SIMBODY_EXPORT Motion::Steady&
+Motion::Steady::setDefaultRates(const Vec<4>& u)
+{ updImpl().setDefaultRates(Vec6(u[0],u[1],u[2],u[3],0,0)); return *this; }
+template <> SimTK_SIMBODY_EXPORT Motion::Steady&
+Motion::Steady::setDefaultRates(const Vec<5>& u)
+{ updImpl().setDefaultRates(Vec6(u[0],u[1],u[2],u[3],u[4],0)); return *this; }
+template <> SimTK_SIMBODY_EXPORT Motion::Steady&
+Motion::Steady::setDefaultRates(const Vec<6>& u)
+{ updImpl().setDefaultRates(u); return *this; }
+
+
+//---------------------------------- Custom ------------------------------------
+//------------------------------------------------------------------------------
+
+SimTK_INSERT_DERIVED_HANDLE_DEFINITIONS(Motion::Custom, Motion::CustomImpl, Motion);
+
+Motion::Custom::Custom(MobilizedBody& mobod, Implementation* implementation)
+ : Motion(new CustomImpl(implementation)) {
+ mobod.adoptMotion(*this);
+}
+
+const Motion::Custom::Implementation& Motion::Custom::getImplementation() const {
+ return getImpl().getImplementation();
+}
+
+Motion::Custom::Implementation& Motion::Custom::updImplementation() {
+ return updImpl().updImplementation();
+}
+
+
+Motion::CustomImpl::CustomImpl(Motion::Custom::Implementation* implementation)
+ : implementation(implementation) {}
+
+
+// These are the default implementations for these virtual methods. They
+// will throw exceptions if called.
+
+void Motion::Custom::Implementation::
+calcPrescribedPosition(const State& s, int nq, Real* q) const {
+ SimTK_ERRCHK_ALWAYS(!"unimplemented",
+ "Motion::Custom::Implementation::calcPrescribedPosition()",
+ "Concrete Implementation did not supply a calcPrescribedPosition() method "
+ "but was apparently expected to do so.");
+ /*NOTREACHED*/
+}
+
+void Motion::Custom::Implementation::
+calcPrescribedPositionDot(const State& s, int nq, Real* qdot) const {
+ SimTK_ERRCHK_ALWAYS(!"unimplemented",
+ "Motion::Custom::Implementation::calcPrescribedPositionDot()",
+ "Concrete Implementation did not supply a calcPrescribedPositionDot() method "
+ "but was apparently expected to do so.");
+ /*NOTREACHED*/
+}
+
+void Motion::Custom::Implementation::
+calcPrescribedPositionDotDot(const State& s, int nq, Real* qdotdot) const {
+ SimTK_ERRCHK_ALWAYS(!"unimplemented",
+ "Motion::Custom::Implementation::calcPrescribedPositionDotDot()",
+ "Concrete Implementation did not supply a calcPrescribedPositionDotDot() method "
+ "but was apparently expected to do so.");
+ /*NOTREACHED*/
+}
+
+void Motion::Custom::Implementation::
+calcPrescribedVelocity(const State& s, int nu, Real* u) const {
+ SimTK_ERRCHK_ALWAYS(!"unimplemented",
+ "Motion::Custom::Implementation::calcPrescribedVelocity()",
+ "Concrete Implementation did not supply a calcPrescribedVelocity() method "
+ "but was apparently expected to do so.");
+ /*NOTREACHED*/
+}
+
+void Motion::Custom::Implementation::
+calcPrescribedVelocityDot(const State& s, int nu, Real* udot) const {
+ SimTK_ERRCHK_ALWAYS(!"unimplemented",
+ "Motion::Custom::Implementation::calcPrescribedVelocityDot()",
+ "Concrete Implementation did not supply a calcPrescribedVelocityDot() method "
+ "but was apparently expected to do so.");
+ /*NOTREACHED*/
+}
+
+void Motion::Custom::Implementation::
+calcPrescribedAcceleration(const State& s, int nu, Real* udot) const {
+ SimTK_ERRCHK_ALWAYS(!"unimplemented",
+ "Motion::Custom::Implementation::calcPrescribedAcceleration()",
+ "Concrete Implementation did not supply a calcPrescribedAcceleration() method "
+ "but was apparently expected to do so.");
+ /*NOTREACHED*/
+}
+
+
+} // namespace SimTK
+
diff --git a/Simbody/src/MotionImpl.h b/Simbody/src/MotionImpl.h
new file mode 100644
index 0000000..661c736
--- /dev/null
+++ b/Simbody/src/MotionImpl.h
@@ -0,0 +1,442 @@
+#ifndef SimTK_SIMBODY_MOTION_IMPL_H_
+#define SimTK_SIMBODY_MOTION_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) 2009-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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/Motion.h"
+
+#include "SimbodyTreeState.h"
+
+namespace SimTK {
+
+class MobilizedBodyImpl;
+
+
+//==============================================================================
+// MOTION IMPL
+//==============================================================================
+// This is the hidden implementation class for Motion objects. This is the
+// abstract base class to which every Motion handle points.
+class MotionImpl : public PIMPLImplementation<Motion, MotionImpl> {
+public:
+ MotionImpl()
+ : m_isDisabledByDefault(false) {}
+
+ // Default copy constructor, copy assignment, and destructor; note that the
+ // pointer to the mobilized body is not copied or deleted.
+
+ bool hasMobilizedBody() const {return m_mobodImpl != 0;}
+ const MobilizedBodyImpl& getMobilizedBodyImpl() const
+ { assert(m_mobodImpl); return *m_mobodImpl; }
+ MobilizedBodyIndex getMobilizedBodyIndex() const;
+
+ void setMobilizedBodyImpl(MobilizedBodyImpl* mbi)
+ { assert(!m_mobodImpl); m_mobodImpl = mbi; }
+ void invalidateTopologyCache() const;
+
+ const SimbodyMatterSubsystem& getMatterSubsystem() const;
+
+ const AbstractValue&
+ getDiscreteVariable(const State& s, DiscreteVariableIndex vx) const;
+
+ AbstractValue&
+ updDiscreteVariable(State& s, DiscreteVariableIndex vx) const;
+
+ DiscreteVariableIndex
+ allocateDiscreteVariable(State& s, Stage g, AbstractValue* v) const;
+
+ template <class T> const T&
+ getVar(const State& s, DiscreteVariableIndex vx) const {
+ return Value<T>::downcast(getDiscreteVariable(s, vx));
+ }
+
+ template <class T> T&
+ updVar(State& s, DiscreteVariableIndex vx) const {
+ return Value<T>::updDowncast(updDiscreteVariable(s, vx));
+ }
+
+ template <class T> DiscreteVariableIndex
+ allocVar(State& state, const T& initVal,
+ const Stage& stage=Stage::Instance) const
+ {
+ return allocateDiscreteVariable(state, stage, new Value<T>(initVal));
+ }
+
+
+ virtual ~MotionImpl() {}
+ virtual MotionImpl* clone() const = 0;
+
+ // This reports whether this Motion is holonomic (Level::Position),
+ // nonholonomic (Level::Velocity), or acceleration (Level::Acceleration).
+ Motion::Level getLevel(const State& s) const {
+ if (isDisabled(s)) return Motion::NoLevel;
+ return getLevelVirtual(s); // ask concrete class
+ }
+
+ Motion::Method getLevelMethod(const State& s) const {
+ if (isDisabled(s)) return Motion::NoMethod;
+ return getLevelMethodVirtual(s);
+ }
+
+
+ void disable(State& s) const;
+ void enable(State& s) const;
+ bool isDisabled(const State&) const;
+
+ void setDisabledByDefault(bool shouldBeDisabled)
+ { invalidateTopologyCache(); m_isDisabledByDefault=shouldBeDisabled; }
+ bool isDisabledByDefault() const {return m_isDisabledByDefault;}
+
+ // These operators calculate prescribed positions, velocities, or
+ // accelerations given a State realized to the previous Stage.
+ void calcPrescribedPosition (const State& s, int nq, Real* q) const
+ { calcPrescribedPositionVirtual(s,nq,q); }
+ void calcPrescribedPositionDot (const State& s, int nq, Real* qdot) const
+ { calcPrescribedPositionDotVirtual(s,nq,qdot); }
+ void calcPrescribedPositionDotDot(const State& s, int nq, Real* qdotdot)const
+ { calcPrescribedPositionDotDotVirtual(s,nq,qdotdot); }
+ void calcPrescribedVelocity (const State& s, int nu, Real* u) const
+ { calcPrescribedVelocityVirtual(s,nu,u); }
+ void calcPrescribedVelocityDot (const State& s, int nu, Real* udot) const
+ { calcPrescribedVelocityDotVirtual(s,nu,udot); }
+ void calcPrescribedAcceleration (const State& s, int nu, Real* udot) const
+ { calcPrescribedAccelerationVirtual(s,nu,udot); }
+
+ void realizeTopology(State& state) const
+ { realizeTopologyVirtual(state); }
+ void realizeModel(State& state) const
+ { realizeModelVirtual(state); }
+ void realizeInstance(const State& state) const
+ { realizeInstanceVirtual(state); }
+ void realizeTime(const State& state) const
+ { realizeTimeVirtual(state); }
+ void realizePosition(const State& state) const
+ { realizePositionVirtual(state); }
+ void realizeVelocity(const State& state) const
+ { realizeVelocityVirtual(state); }
+ void realizeDynamics(const State& state) const
+ { realizeDynamicsVirtual(state); }
+ void realizeAcceleration(const State& state) const
+ { realizeAccelerationVirtual(state); }
+ void realizeReport(const State& state) const
+ { realizeReportVirtual(state); }
+
+ virtual Motion::Level getLevelVirtual(const State&) const = 0;
+ virtual Motion::Method getLevelMethodVirtual(const State&) const
+ { return Motion::Prescribed; }
+
+ virtual void calcPrescribedPositionVirtual
+ (const State&, int nq, Real* q) const;
+ virtual void calcPrescribedPositionDotVirtual
+ (const State&, int nq, Real* qdot) const;
+ virtual void calcPrescribedPositionDotDotVirtual
+ (const State&, int nq, Real* qdotdot) const;
+ virtual void calcPrescribedVelocityVirtual
+ (const State&, int nu, Real* u) const;
+ virtual void calcPrescribedVelocityDotVirtual
+ (const State&, int nu, Real* udot) const;
+ virtual void calcPrescribedAccelerationVirtual
+ (const State&, int nu, Real* udot) const;
+
+ virtual void realizeTopologyVirtual (State&) const {}
+ virtual void realizeModelVirtual (State&) const {}
+ virtual void realizeInstanceVirtual (const State&) const {}
+ virtual void realizeTimeVirtual (const State&) const {}
+ virtual void realizePositionVirtual (const State&) const {}
+ virtual void realizeVelocityVirtual (const State&) const {}
+ virtual void realizeDynamicsVirtual (const State&) const {}
+ virtual void realizeAccelerationVirtual(const State&) const {}
+ virtual void realizeReportVirtual (const State&) const {}
+private:
+ ReferencePtr<MobilizedBodyImpl> m_mobodImpl;
+ bool m_isDisabledByDefault;
+};
+
+
+//------------------------------------------------------------------------------
+// SINUSOID IMPL
+//------------------------------------------------------------------------------
+class Motion::SinusoidImpl : public MotionImpl {
+public:
+ // no default constructor
+ SinusoidImpl(Motion::Level level,
+ Real amplitude, Real rate, Real phase)
+ : level(level), defAmplitude(amplitude), defRate(rate),
+ defPhase(phase)
+ {
+ }
+
+ SinusoidImpl* clone() const OVERRIDE_11 {
+ SinusoidImpl* copy = new SinusoidImpl(*this);
+ return copy;
+ }
+
+ Motion::Level getLevelVirtual (const State&) const OVERRIDE_11
+ { return level; }
+ Motion::Method getLevelMethodVirtual(const State&) const OVERRIDE_11
+ { return Motion::Prescribed; }
+
+ // Allocate variables if needed.
+ void realizeTopologyVirtual(State& state) const OVERRIDE_11 {
+ // None yet.
+ }
+
+
+ void calcPrescribedPositionVirtual
+ (const State& state, int nq, Real* q) const OVERRIDE_11 {
+ assert(level==Motion::Position); assert(nq==0 || q);
+ const Real t = state.getTime();
+ const Real out = defAmplitude*std::sin(defRate*t + defPhase);
+ for (int i=0; i<nq; ++i)
+ q[i] = out;
+ }
+
+ void calcPrescribedPositionDotVirtual
+ (const State& state, int nq, Real* qdot) const OVERRIDE_11 {
+ assert(level==Motion::Position); assert(nq==0 || qdot);
+ const Real t = state.getTime();
+ const Real outd = defAmplitude*defRate*std::cos(defRate*t + defPhase);
+ for (int i=0; i<nq; ++i)
+ qdot[i] = outd;
+ }
+
+ void calcPrescribedPositionDotDotVirtual
+ (const State& state, int nq, Real* qdotdot) const OVERRIDE_11 {
+ assert(level==Motion::Position); assert(nq==0 || qdotdot);
+ const Real t = state.getTime();
+ const Real outdd =
+ -defAmplitude*defRate*defRate*std::sin(defRate*t + defPhase);
+ for (int i=0; i<nq; ++i)
+ qdotdot[i] = outdd;
+ }
+
+ void calcPrescribedVelocityVirtual
+ (const State& state, int nu, Real* u) const OVERRIDE_11 {
+ assert(level==Motion::Velocity);
+ assert(nu==0 || u);
+ const Real t = state.getTime();
+ const Real out = defAmplitude*std::sin(defRate*t + defPhase);
+ for (int i=0; i<nu; ++i)
+ u[i] = out;
+ }
+
+ void calcPrescribedVelocityDotVirtual
+ (const State& state, int nu, Real* udot) const OVERRIDE_11 {
+ assert(level==Motion::Velocity);
+ assert(nu==0 || udot);
+ const Real t = state.getTime();
+ const Real outd = defAmplitude*defRate*std::cos(defRate*t + defPhase);
+ for (int i=0; i<nu; ++i)
+ udot[i] = outd;
+ }
+
+ void calcPrescribedAccelerationVirtual
+ (const State& state, int nu, Real* udot) const OVERRIDE_11 {
+ assert(level==Motion::Acceleration); assert(nu==0 || udot);
+ const Real t = state.getTime();
+ const Real out = defAmplitude*std::sin(defRate*t + defPhase);
+ for (int i=0; i<nu; ++i)
+ udot[i] = out;
+ }
+private:
+ // TOPOLOGY "STATE"
+ Motion::Level level;
+ Real defAmplitude, defRate, defPhase;
+
+ // TOPOLOGY "CACHE"
+ // None yet.
+};
+
+
+//------------------------------------------------------------------------------
+// STEADY IMPL
+//------------------------------------------------------------------------------
+class Motion::SteadyImpl : public MotionImpl {
+public:
+ // no default constructor
+ explicit SteadyImpl(const Vec6& u) : defaultU(u) {}
+
+ SteadyImpl* clone() const OVERRIDE_11 {
+ SteadyImpl* copy = new SteadyImpl(*this);
+ copy->currentU.invalidate(); // no sharing state variables
+ return copy;
+ }
+
+ void setDefaultRates(const Vec6& u) {
+ invalidateTopologyCache();
+ defaultU = u;
+ }
+ void setOneDefaultRate(MobilizerUIndex ux, Real u) {
+ invalidateTopologyCache();
+ defaultU[ux] = u;
+ }
+ Real getOneDefaultRate(MobilizerUIndex ux) const {
+ return defaultU[ux];
+ }
+
+ const Vec6& getDefaultRates() const {return defaultU;}
+
+ void setRates(State& s, const Vec6& u) const {
+ updVar<Vec6>(s, currentU) = u;
+ }
+ void setOneRate(State& s, MobilizerUIndex ux, Real u) const {
+ updVar<Vec6>(s, currentU)[ux] = u;
+ }
+
+ Real getOneRate(const State& s, MobilizerUIndex ux) const {
+ return getVar<Vec6>(s, currentU)[ux];
+ }
+
+ Motion::Level getLevelVirtual (const State&) const OVERRIDE_11
+ { return Motion::Velocity; }
+ Motion::Method getLevelMethodVirtual(const State&) const OVERRIDE_11
+ { return Motion::Prescribed; }
+
+ // Allocate a discrete variable to hold the constant rates.
+ void realizeTopologyVirtual(State& state) const OVERRIDE_11 {
+ // This is in the Topology-stage "cache" so we can write to it,
+ // but only here.
+ const_cast<DiscreteVariableIndex&>(currentU) =
+ allocVar(state, defaultU);
+ }
+
+ void calcPrescribedVelocityVirtual
+ (const State& state, int nu, Real* u) const OVERRIDE_11
+ {
+ assert(0 <= nu && nu <= 6);
+ assert(nu==0 || u);
+ const Vec6& uval = getVar<Vec6>(state, currentU);
+ for (int i=0; i<nu; ++i)
+ u[i] = uval[i];
+ }
+
+ void calcPrescribedVelocityDotVirtual
+ (const State& state, int nu, Real* udot) const OVERRIDE_11
+ {
+ assert(0 <= nu && nu <= 6);
+ assert(nu==0 || udot);
+ for (int i=0; i<nu; ++i)
+ udot[i] = 0;
+ }
+
+private:
+ // TOPOLOGY "STATE"
+ Vec6 defaultU;
+
+ // TOPOLOGY "CACHE"
+ DiscreteVariableIndex currentU;
+};
+
+
+//------------------------------------------------------------------------------
+// CUSTOM IMPL
+//------------------------------------------------------------------------------
+class Motion::CustomImpl : public MotionImpl {
+public:
+ // Take over ownership of the supplied heap-allocated object.
+ explicit CustomImpl(Motion::Custom::Implementation* implementation);
+
+ CustomImpl(const CustomImpl& src) : implementation(0) {
+ if (src.implementation)
+ implementation = src.implementation->clone();
+ }
+
+ CustomImpl* clone() const OVERRIDE_11 { return new CustomImpl(*this); }
+
+ ~CustomImpl() {
+ delete implementation;
+ }
+
+ Motion::Level getLevelVirtual(const State& s) const OVERRIDE_11 {
+ return getImplementation().getLevel(s);
+ }
+ Motion::Method getLevelMethodVirtual(const State& s) const OVERRIDE_11 {
+ return getImplementation().getLevelMethod(s);
+ }
+
+ const Motion::Custom::Implementation& getImplementation() const {
+ assert(implementation); return *implementation;
+ }
+ Motion::Custom::Implementation& updImplementation() {
+ assert(implementation); return *implementation;
+ }
+
+ void calcPrescribedPositionVirtual
+ (const State& s, int nq, Real* q) const OVERRIDE_11
+ { getImplementation().calcPrescribedPosition(s,nq,q); }
+ void calcPrescribedPositionDotVirtual
+ (const State& s, int nq, Real* qdot) const OVERRIDE_11
+ { getImplementation().calcPrescribedPositionDot(s,nq,qdot); }
+ void calcPrescribedPositionDotDotVirtual
+ (const State& s, int nq, Real* qdotdot) const OVERRIDE_11
+ { getImplementation().calcPrescribedPositionDotDot(s,nq,qdotdot); }
+
+ void calcPrescribedVelocityVirtual
+ (const State& s, int nu, Real* u) const OVERRIDE_11
+ { getImplementation().calcPrescribedVelocity(s,nu,u); }
+ void calcPrescribedVelocityDotVirtual
+ (const State& s, int nu, Real* udot) const OVERRIDE_11
+ { getImplementation().calcPrescribedVelocityDot(s,nu,udot); }
+
+ void calcPrescribedAccelerationVirtual
+ (const State& s, int nu, Real* udot) const OVERRIDE_11
+ { getImplementation().calcPrescribedAcceleration(s,nu,udot); }
+
+ void realizeTopologyVirtual(State& state) const OVERRIDE_11 {
+ getImplementation().realizeTopology(state);
+ }
+ void realizeModelVirtual(State& state) const OVERRIDE_11 {
+ getImplementation().realizeModel(state);
+ }
+ void realizeInstanceVirtual(const State& state) const OVERRIDE_11 {
+ getImplementation().realizeInstance(state);
+ }
+ void realizeTimeVirtual(const State& state) const OVERRIDE_11 {
+ getImplementation().realizeTime(state);
+ }
+ void realizePositionVirtual(const State& state) const OVERRIDE_11 {
+ getImplementation().realizePosition(state);
+ }
+ void realizeVelocityVirtual(const State& state) const OVERRIDE_11 {
+ getImplementation().realizeVelocity(state);
+ }
+ void realizeDynamicsVirtual(const State& state) const OVERRIDE_11 {
+ getImplementation().realizeDynamics(state);
+ }
+ void realizeAccelerationVirtual(const State& state) const OVERRIDE_11 {
+ getImplementation().realizeAcceleration(state);
+ }
+ void realizeReportVirtual(const State& state) const OVERRIDE_11 {
+ getImplementation().realizeReport(state);
+ }
+private:
+ Motion::Custom::Implementation* implementation;
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MOTION_IMPL_H_
diff --git a/Simbody/src/MultibodySystem.cpp b/Simbody/src/MultibodySystem.cpp
new file mode 100644
index 0000000..6693099
--- /dev/null
+++ b/Simbody/src/MultibodySystem.cpp
@@ -0,0 +1,335 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ *
+ * Implementation of MultibodySystem, a concrete System.
+ */
+
+#include "simbody/internal/common.h"
+#include "simbody/internal/MultibodySystem.h"
+
+#include "MultibodySystemRep.h"
+#include "DecorationSubsystemRep.h"
+
+namespace SimTK {
+
+
+ //////////////////////
+ // MULTIBODY SYSTEM //
+ //////////////////////
+
+/*static*/ bool
+MultibodySystem::isInstanceOf(const System& s) {
+ return MultibodySystemRep::isA(s.getSystemGuts());
+}
+/*static*/ const MultibodySystem&
+MultibodySystem::downcast(const System& s) {
+ assert(isInstanceOf(s));
+ return static_cast<const MultibodySystem&>(s);
+}
+/*static*/ MultibodySystem&
+MultibodySystem::updDowncast(System& s) {
+ assert(isInstanceOf(s));
+ return static_cast<MultibodySystem&>(s);
+}
+
+const MultibodySystemRep&
+MultibodySystem::getRep() const {
+ return SimTK_DYNAMIC_CAST_DEBUG<const MultibodySystemRep&>(getSystemGuts());
+}
+MultibodySystemRep&
+MultibodySystem::updRep() {
+ return SimTK_DYNAMIC_CAST_DEBUG<MultibodySystemRep&>(updSystemGuts());
+}
+
+// Create generic multibody system by default.
+MultibodySystem::MultibodySystem() {
+ adoptSystemGuts(new MultibodySystemRep());
+ DefaultSystemSubsystem defsub(*this); // This invokes adoptSubsystem().
+ updRep().setGlobalSubsystem();
+}
+
+MultibodySystem::MultibodySystem(SimbodyMatterSubsystem& m)
+{
+ adoptSystemGuts(new MultibodySystemRep());
+ DefaultSystemSubsystem defsub(*this); // This invokes adoptSubsystem().
+ updRep().setGlobalSubsystem();
+ setMatterSubsystem(m);
+}
+
+// This is a protected constructor for use by derived classes which
+// allocate a more specialized MultibodySystemRep.
+MultibodySystem::MultibodySystem(MultibodySystemRep* rp) {
+ adoptSystemGuts(rp);
+ DefaultSystemSubsystem defsub(*this); // This invokes adoptSubsystem().
+ updRep().setGlobalSubsystem();
+}
+
+int MultibodySystem::setMatterSubsystem(SimbodyMatterSubsystem& m) {
+ return updRep().setMatterSubsystem(m);
+}
+int MultibodySystem::addForceSubsystem(ForceSubsystem& f) {
+ return updRep().addForceSubsystem(f);
+}
+int MultibodySystem::setDecorationSubsystem(DecorationSubsystem& m) {
+ return updRep().setDecorationSubsystem(m);
+}
+int MultibodySystem::setContactSubsystem(GeneralContactSubsystem& m) {
+ return updRep().setContactSubsystem(m);
+}
+
+const SimbodyMatterSubsystem&
+MultibodySystem::getMatterSubsystem() const {
+ return getRep().getMatterSubsystem();
+}
+SimbodyMatterSubsystem&
+MultibodySystem::updMatterSubsystem() {
+ return updRep().updMatterSubsystem();
+}
+bool MultibodySystem::hasMatterSubsystem() const {
+ return getRep().hasMatterSubsystem();
+}
+
+const DecorationSubsystem&
+MultibodySystem::getDecorationSubsystem() const {
+ return getRep().getDecorationSubsystem();
+}
+DecorationSubsystem&
+MultibodySystem::updDecorationSubsystem() {
+ return updRep().updDecorationSubsystem();
+}
+bool MultibodySystem::hasDecorationSubsystem() const {
+ return getRep().hasDecorationSubsystem();
+}
+
+const GeneralContactSubsystem&
+MultibodySystem::getContactSubsystem() const {
+ return getRep().getContactSubsystem();
+}
+GeneralContactSubsystem&
+MultibodySystem::updContactSubsystem() {
+ return updRep().updContactSubsystem();
+}
+bool MultibodySystem::hasContactSubsystem() const {
+ return getRep().hasContactSubsystem();
+}
+
+const Real
+MultibodySystem::calcPotentialEnergy(const State& s) const {
+ return getRep().calcPotentialEnergy(s);
+}
+const Real
+MultibodySystem::calcKineticEnergy(const State& s) const {
+ return getMatterSubsystem().getRep().calcKineticEnergy(s);
+}
+
+const Vector_<SpatialVec>&
+MultibodySystem::getRigidBodyForces(const State& s, Stage g) const {
+ return getRep().getRigidBodyForces(s,g);
+}
+const Vector_<Vec3>&
+MultibodySystem::getParticleForces(const State& s, Stage g) const {
+ return getRep().getParticleForces(s,g);
+}
+const Vector&
+MultibodySystem::getMobilityForces(const State& s, Stage g) const {
+ return getRep().getMobilityForces(s,g);
+}
+
+Vector_<SpatialVec>&
+MultibodySystem::updRigidBodyForces(const State& s, Stage g) const {
+ return getRep().updRigidBodyForces(s,g);
+}
+Vector_<Vec3>&
+MultibodySystem::updParticleForces(const State& s, Stage g) const {
+ return getRep().updParticleForces(s,g);
+}
+Vector&
+MultibodySystem::updMobilityForces(const State& s, Stage g) const {
+ return getRep().updMobilityForces(s,g);
+}
+
+
+ //////////////////////////
+ // MULTIBODY SYSTEM REP //
+ //////////////////////////
+
+int MultibodySystemRep::realizeTopologyImpl(State& s) const {
+ assert(globalSub.isValid());
+ assert(matterSub.isValid());
+
+ // We do Matter subsystem first here in case any of the GlobalSubsystem
+ // topology depends on Matter topology. That's unlikely though since
+ // we don't know sizes until Model stage.
+ getMatterSubsystem().getRep().realizeSubsystemTopology(s);
+ getGlobalSubsystem().getRep().realizeSubsystemTopology(s);
+ for (int i=0; i < (int)forceSubs.size(); ++i)
+ getForceSubsystem(forceSubs[i]).getRep().realizeSubsystemTopology(s);
+
+ if (hasDecorationSubsystem())
+ getDecorationSubsystem().getGuts().realizeSubsystemTopology(s);
+
+ return 0;
+}
+int MultibodySystemRep::realizeModelImpl(State& s) const {
+
+ // Here it is essential to do the Matter subsystem first because the
+ // force accumulation arrays in the Global subsystem depend on the
+ // Stage::Model dimensions of the Matter subsystem.
+ getMatterSubsystem().getRep().realizeSubsystemModel(s);
+ getGlobalSubsystem().getRep().realizeSubsystemModel(s);
+ for (int i=0; i < (int)forceSubs.size(); ++i)
+ getForceSubsystem(forceSubs[i]).getRep().realizeSubsystemModel(s);
+
+ if (hasDecorationSubsystem())
+ getDecorationSubsystem().getGuts().realizeSubsystemModel(s);
+
+ return 0;
+}
+int MultibodySystemRep::realizeInstanceImpl(const State& s) const {
+ getGlobalSubsystem().getRep().realizeSubsystemInstance(s);
+ getMatterSubsystem().getRep().realizeSubsystemInstance(s);
+ for (int i=0; i < (int)forceSubs.size(); ++i)
+ getForceSubsystem(forceSubs[i]).getRep().realizeSubsystemInstance(s);
+
+ if (hasDecorationSubsystem())
+ getDecorationSubsystem().getGuts().realizeSubsystemInstance(s);
+
+ return 0;
+}
+int MultibodySystemRep::realizeTimeImpl(const State& s) const {
+ getGlobalSubsystem().getRep().realizeSubsystemTime(s);
+ getMatterSubsystem().getRep().realizeSubsystemTime(s);
+ for (int i=0; i < (int)forceSubs.size(); ++i)
+ getForceSubsystem(forceSubs[i]).getRep().realizeSubsystemTime(s);
+
+ if (hasDecorationSubsystem())
+ getDecorationSubsystem().getGuts().realizeSubsystemTime(s);
+
+ return 0;
+}
+int MultibodySystemRep::realizePositionImpl(const State& s) const {
+ getGlobalSubsystem().getRep().realizeSubsystemPosition(s);
+ getMatterSubsystem().getRep().realizeSubsystemPosition(s);
+ for (int i=0; i < (int)forceSubs.size(); ++i)
+ getForceSubsystem(forceSubs[i]).getRep().realizeSubsystemPosition(s);
+
+ if (hasDecorationSubsystem())
+ getDecorationSubsystem().getGuts().realizeSubsystemPosition(s);
+
+ return 0;
+}
+int MultibodySystemRep::realizeVelocityImpl(const State& s) const {
+ getGlobalSubsystem().getRep().realizeSubsystemVelocity(s);
+ getMatterSubsystem().getRep().realizeSubsystemVelocity(s);
+ for (int i=0; i < (int)forceSubs.size(); ++i)
+ getForceSubsystem(forceSubs[i]).getRep().realizeSubsystemVelocity(s);
+
+ if (hasDecorationSubsystem())
+ getDecorationSubsystem().getGuts().realizeSubsystemVelocity(s);
+
+ return 0;
+}
+int MultibodySystemRep::realizeDynamicsImpl(const State& s) const {
+ getGlobalSubsystem().getRep().realizeSubsystemDynamics(s);
+ if (hasContactSubsystem())
+ getContactSubsystem().getSubsystemGuts().realizeSubsystemDynamics(s);
+
+ // This realizes the matter subsystem's dynamic operators; not yet accelerations.
+ getMatterSubsystem().getRep().realizeSubsystemDynamics(s);
+
+ // Now do forces in case any of them need dynamics-stage operators.
+ for (int i=0; i < (int)forceSubs.size(); ++i)
+ getForceSubsystem(forceSubs[i]).getRep().realizeSubsystemDynamics(s);
+
+ if (hasDecorationSubsystem())
+ getDecorationSubsystem().getGuts().realizeSubsystemDynamics(s);
+
+ return 0;
+}
+int MultibodySystemRep::realizeAccelerationImpl(const State& s) const {
+ getGlobalSubsystem().getRep().realizeSubsystemAcceleration(s);
+
+ // Realize matter subsystem's accelerations and multipliers next; they
+ // can depend only on force calculations at Dynamics stage.
+ getMatterSubsystem().getRep().realizeSubsystemAcceleration(s);
+
+ // Force elements' realizeAcceleration() methods might depend on
+ // accelerations or multipliers we just calculated. For example, a friction
+ // force might record normal forces to use as an initial guess in the
+ // next time step.
+ for (int i=0; i < (int)forceSubs.size(); ++i)
+ getForceSubsystem(forceSubs[i]).getRep().realizeSubsystemAcceleration(s);
+
+ if (hasDecorationSubsystem())
+ getDecorationSubsystem().getGuts().realizeSubsystemAcceleration(s);
+
+ return 0;
+}
+int MultibodySystemRep::realizeReportImpl(const State& s) const {
+ getGlobalSubsystem().getRep().realizeSubsystemReport(s);
+
+ getMatterSubsystem().getRep().realizeSubsystemReport(s);
+ for (int i=0; i < (int)forceSubs.size(); ++i)
+ getForceSubsystem(forceSubs[i]).getRep().realizeSubsystemReport(s);
+
+ if (hasDecorationSubsystem())
+ getDecorationSubsystem().getGuts().realizeSubsystemReport(s);
+
+ return 0;
+}
+
+
+ ///////////////////////////////////////
+ // MULTIBODY SYSTEM GLOBAL SUBSYSTEM //
+ ///////////////////////////////////////
+
+
+/*static*/ bool
+MultibodySystemGlobalSubsystem::isInstanceOf(const Subsystem& s) {
+ return MultibodySystemGlobalSubsystemRep::isA(s.getSubsystemGuts());
+}
+/*static*/ const MultibodySystemGlobalSubsystem&
+MultibodySystemGlobalSubsystem::downcast(const Subsystem& s) {
+ assert(isInstanceOf(s));
+ return static_cast<const MultibodySystemGlobalSubsystem&>(s);
+}
+/*static*/ MultibodySystemGlobalSubsystem&
+MultibodySystemGlobalSubsystem::updDowncast(Subsystem& s) {
+ assert(isInstanceOf(s));
+ return static_cast<MultibodySystemGlobalSubsystem&>(s);
+}
+
+const MultibodySystemGlobalSubsystemRep&
+MultibodySystemGlobalSubsystem::getRep() const {
+ return SimTK_DYNAMIC_CAST_DEBUG<const MultibodySystemGlobalSubsystemRep&>(getSubsystemGuts());
+}
+MultibodySystemGlobalSubsystemRep&
+MultibodySystemGlobalSubsystem::updRep() {
+ return SimTK_DYNAMIC_CAST_DEBUG<MultibodySystemGlobalSubsystemRep&>(updSubsystemGuts());
+}
+
+
+} // namespace SimTK
+
diff --git a/Simbody/src/MultibodySystemRep.h b/Simbody/src/MultibodySystemRep.h
new file mode 100644
index 0000000..2a1066d
--- /dev/null
+++ b/Simbody/src/MultibodySystemRep.h
@@ -0,0 +1,512 @@
+#ifndef SimTK_SIMBODY_MULTIBODY_SYSTEM_REP_H_
+#define SimTK_SIMBODY_MULTIBODY_SYSTEM_REP_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* Here we define the private implementation of the MultibodySystem class
+(a kind of System). This is not part of the Simbody API. */
+
+#include "SimTKcommon.h"
+#include "SimTKcommon/internal/SystemGuts.h"
+
+#include "simbody/internal/common.h"
+#include "simbody/internal/MultibodySystem.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "simbody/internal/ForceSubsystem.h"
+#include "simbody/internal/DecorationSubsystem.h"
+#include "simbody/internal/GeneralContactSubsystem.h"
+
+#include "simbody/internal/ForceSubsystemGuts.h"
+#include "SimbodyMatterSubsystemRep.h"
+
+#include <vector>
+
+namespace SimTK {
+
+class AnalyticGeometry;
+class DecorativeGeometry;
+
+
+//==============================================================================
+// FORCE CACHE ENTRY
+//==============================================================================
+/* This is a set of global variables for the MultibodySystem, stored as a cache
+entry in the system's State, one at each Stage. Technically it is owned by the
+MultibodySystemGlobalSubsystem, but it is manipulated directly by
+the MultibodySystem.
+
+The entry at a given Stage includes all the contributions from the previous
+Stages. For example, if some forces are generated at Stage::Model, those
+are used to initialize the ones at Stage::Instance. That way when we get
+to the final set at Stage::Dynamics we can use it directly to produce
+accelerations. This structure allows us to invalidate a higher Stage without
+having to recalculate forces that were known at a lower Stage. */
+struct ForceCacheEntry {
+ ForceCacheEntry()
+ { }
+ // default copy constructor, copy assignment, destructor
+
+ Vector_<SpatialVec> rigidBodyForces;
+ Vector_<Vec3> particleForces;
+ Vector mobilityForces;
+
+ void ensureAllocatedTo(int nRigidBodies, int nParticles, int nMobilities) {
+ rigidBodyForces.resize(nRigidBodies);
+ particleForces.resize(nParticles);
+ mobilityForces.resize(nMobilities);
+ }
+
+ void setAllForcesToZero() {
+ rigidBodyForces.setToZero();
+ particleForces.setToZero();
+ mobilityForces.setToZero();
+ }
+
+ // This is just an assignment but allows for some bugcatchers. All the
+ // ForceCacheEntries at every stage are supposed to have the same dimensions.
+ void initializeFromSimilarForceEntry(const ForceCacheEntry& src) {
+ assert(src.rigidBodyForces.size() == rigidBodyForces.size());
+ assert(src.particleForces.size() == particleForces.size());
+ assert(src.mobilityForces.size() == mobilityForces.size());
+ *this = src;
+ }
+};
+
+// Useless, but required by Value<T>.
+inline std::ostream& operator<<(std::ostream& o, const ForceCacheEntry&)
+{assert(false);return o;}
+
+
+
+//==============================================================================
+// MULTIBODY SYSTEM GLOBAL SUBSYSTEM REP
+//==============================================================================
+/* This is the subsystem used by a MultibodySystem to manage global state
+calculations like forces and potential energy. */
+class MultibodySystemGlobalSubsystemRep : public Subsystem::Guts {
+ // Topological variables
+
+ static const int NumForceCacheEntries = (Stage::Dynamics-Stage::Model+1);
+ mutable CacheEntryIndex forceCacheIndices[NumForceCacheEntries]; // where in state to find our stuff
+
+ const ForceCacheEntry& getForceCacheEntry(const State& s, Stage g) const {
+ assert(subsystemTopologyHasBeenRealized());
+ SimTK_STAGECHECK_RANGE(Stage::Model, g, Stage::Dynamics,
+ "MultibodySystem::getForceCacheEntry()");
+
+ return Value<ForceCacheEntry>::downcast(
+ getCacheEntry(s,forceCacheIndices[g-Stage::Model])).get();
+ }
+ ForceCacheEntry& updForceCacheEntry(const State& s, Stage g) const {
+ assert(subsystemTopologyHasBeenRealized());
+ SimTK_STAGECHECK_RANGE(Stage::Model, g, Stage::Dynamics,
+ "MultibodySystem::getForceCacheEntry()");
+
+ return Value<ForceCacheEntry>::downcast(
+ updCacheEntry(s,forceCacheIndices[g-Stage::Model])).upd();
+ }
+public:
+ MultibodySystemGlobalSubsystemRep()
+ : Subsystem::Guts("MultibodySystemGlobalSubsystem", "0.0.2")
+ {
+ invalidateSubsystemTopologyCache();
+ }
+
+ const MultibodySystem& getMultibodySystem() const {
+ return MultibodySystem::downcast(getSystem());
+ }
+
+ const Vector_<SpatialVec>& getRigidBodyForces(const State& s, Stage g) const {
+ return getForceCacheEntry(s,g).rigidBodyForces;
+ }
+ const Vector_<Vec3>& getParticleForces(const State& s, Stage g) const {
+ return getForceCacheEntry(s,g).particleForces;
+ }
+ const Vector& getMobilityForces(const State& s, Stage g) const {
+ return getForceCacheEntry(s,g).mobilityForces;
+ }
+
+ Vector_<SpatialVec>& updRigidBodyForces(const State& s, Stage g) const {
+ return updForceCacheEntry(s,g).rigidBodyForces;
+ }
+ Vector_<Vec3>& updParticleForces(const State& s, Stage g) const {
+ return updForceCacheEntry(s,g).particleForces;
+ }
+ Vector& updMobilityForces(const State& s, Stage g) const {
+ return updForceCacheEntry(s,g).mobilityForces;
+ }
+
+ // These override virtual methods from Subsystem::Guts.
+
+ // Use default copy constructor, but then clear out the cache indices
+ // and invalidate topology.
+ MultibodySystemGlobalSubsystemRep* cloneImpl() const {
+ MultibodySystemGlobalSubsystemRep* p =
+ new MultibodySystemGlobalSubsystemRep(*this);
+ for (int i=0; i<NumForceCacheEntries; ++i)
+ p->forceCacheIndices[i].invalidate();
+ p->invalidateSubsystemTopologyCache();
+ return p;
+ }
+
+ // At Topology stage we just allocate some slots in the State to hold
+ // the forces. We can't initialize the force arrays because we don't yet
+ // know the problem size.
+ int realizeSubsystemTopologyImpl(State& s) const {
+ const MultibodySystem& mbs = getMultibodySystem();
+
+ for (Stage g(Stage::Model); g<=Stage::Dynamics; ++g)
+ forceCacheIndices[g-Stage::Model] =
+ allocateCacheEntry(s, g, new Value<ForceCacheEntry>());
+
+ return 0;
+ }
+
+ // At Model stage we know the problem size, so we can allocate the
+ // model stage forces (if necessary) and initialize them (to zero).
+ int realizeSubsystemModelImpl(State& s) const {
+ const MultibodySystem& mbs = getMultibodySystem();
+ const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
+
+ ForceCacheEntry& modelForces = updForceCacheEntry(s, Stage::Model);
+ modelForces.ensureAllocatedTo(matter.getNumBodies(),
+ matter.getNumParticles(),
+ matter.getNumMobilities());
+ modelForces.setAllForcesToZero();
+
+ return 0;
+ }
+
+ // We treat the other stages like Model except that we use the
+ // previous Stage's ForceCacheEntry to initialize this one.
+ int realizeSubsystemInstanceImpl(const State& s) const {
+ const MultibodySystem& mbs = getMultibodySystem();
+ const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
+
+ const ForceCacheEntry& modelForces = getForceCacheEntry(s, Stage::Model);
+ ForceCacheEntry& instanceForces = updForceCacheEntry(s, Stage::Instance);
+ instanceForces.ensureAllocatedTo(matter.getNumBodies(),
+ matter.getNumParticles(),
+ matter.getNumMobilities());
+ instanceForces.initializeFromSimilarForceEntry(modelForces);
+
+ return 0;
+ }
+
+ int realizeSubsystemTimeImpl(const State& s) const {
+ const MultibodySystem& mbs = getMultibodySystem();
+ const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
+
+ const ForceCacheEntry& instanceForces = getForceCacheEntry(s, Stage::Instance);
+ ForceCacheEntry& timeForces = updForceCacheEntry(s, Stage::Time);
+ timeForces.ensureAllocatedTo(matter.getNumBodies(),
+ matter.getNumParticles(),
+ matter.getNumMobilities());
+ timeForces.initializeFromSimilarForceEntry(instanceForces);
+
+ return 0;
+ }
+
+ int realizeSubsystemPositionImpl(const State& s) const {
+ const MultibodySystem& mbs = getMultibodySystem();
+ const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
+
+ const ForceCacheEntry& timeForces = getForceCacheEntry(s, Stage::Time);
+ ForceCacheEntry& positionForces = updForceCacheEntry(s, Stage::Position);
+ positionForces.ensureAllocatedTo(matter.getNumBodies(),
+ matter.getNumParticles(),
+ matter.getNumMobilities());
+ positionForces.initializeFromSimilarForceEntry(timeForces);
+
+ return 0;
+ }
+
+ int realizeSubsystemVelocityImpl(const State& s) const {
+ const MultibodySystem& mbs = getMultibodySystem();
+ const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
+
+ const ForceCacheEntry& positionForces = getForceCacheEntry(s, Stage::Position);
+ ForceCacheEntry& velocityForces = updForceCacheEntry(s, Stage::Velocity);
+ velocityForces.ensureAllocatedTo(matter.getNumBodies(),
+ matter.getNumParticles(),
+ matter.getNumMobilities());
+ velocityForces.initializeFromSimilarForceEntry(positionForces);
+
+ return 0;
+ }
+
+ int realizeSubsystemDynamicsImpl(const State& s) const {
+ const MultibodySystem& mbs = getMultibodySystem();
+ const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
+
+ const ForceCacheEntry& velocityForces = getForceCacheEntry(s, Stage::Velocity);
+ ForceCacheEntry& dynamicsForces = updForceCacheEntry(s, Stage::Dynamics);
+ dynamicsForces.ensureAllocatedTo(matter.getNumBodies(),
+ matter.getNumParticles(),
+ matter.getNumMobilities());
+ dynamicsForces.initializeFromSimilarForceEntry(velocityForces);
+
+ return 0;
+ }
+
+ // no need for other realize() methods
+ SimTK_DOWNCAST(MultibodySystemGlobalSubsystemRep, Subsystem::Guts);
+};
+
+class MultibodySystemGlobalSubsystem : public Subsystem {
+public:
+ MultibodySystemGlobalSubsystem() : Subsystem() {
+ adoptSubsystemGuts(new MultibodySystemGlobalSubsystemRep());
+ }
+
+ SimTK_PIMPL_DOWNCAST(MultibodySystemGlobalSubsystem, Subsystem);
+ const MultibodySystemGlobalSubsystemRep& getRep() const;
+ MultibodySystemGlobalSubsystemRep& updRep();
+};
+
+
+
+//==============================================================================
+// MULTIBODY SYSTEM REP
+//==============================================================================
+/* The job of the MultibodySystem class is to coordinate the activities of a
+MatterSubsystem and a set of ForceSubsystems. */
+class MultibodySystemRep : public System::Guts {
+public:
+ MultibodySystemRep()
+ : System::Guts("MultibodySystem", "0.0.1")
+ {
+ }
+ ~MultibodySystemRep() {
+ }
+
+ SubsystemIndex setGlobalSubsystem() {
+ assert(!globalSub.isValid());
+ MultibodySystemGlobalSubsystem glo;
+ globalSub = adoptSubsystem(glo);
+ return globalSub;
+ }
+ SubsystemIndex setMatterSubsystem(SimbodyMatterSubsystem& m) {
+ assert(!matterSub.isValid());
+ matterSub = adoptSubsystem(m);
+ return matterSub;
+ }
+ SubsystemIndex addForceSubsystem(ForceSubsystem& f) {
+ forceSubs.push_back(adoptSubsystem(f));
+ return forceSubs.back();
+ }
+ SubsystemIndex setDecorationSubsystem(DecorationSubsystem& d) {
+ assert(!decorationSub.isValid());
+ decorationSub = adoptSubsystem(d);
+ return decorationSub;
+ }
+ SubsystemIndex setContactSubsystem(GeneralContactSubsystem& c) {
+ assert(!contactSub.isValid());
+ contactSub = adoptSubsystem(c);
+ return contactSub;
+ }
+
+ const SimbodyMatterSubsystem& getMatterSubsystem() const {
+ assert(matterSub.isValid());
+ return SimbodyMatterSubsystem::downcast(getSubsystem(matterSub));
+ }
+ const ForceSubsystem& getForceSubsystem(SubsystemIndex id) const {
+ return ForceSubsystem::downcast(getSubsystem(id));
+ }
+ const MultibodySystemGlobalSubsystem& getGlobalSubsystem() const {
+ assert(globalSub.isValid());
+ return MultibodySystemGlobalSubsystem::downcast(getSubsystem(globalSub));
+ }
+
+ bool hasDecorationSubsystem() const {return decorationSub.isValid();}
+ bool hasContactSubsystem() const {return contactSub.isValid();}
+ bool hasMatterSubsystem() const {return matterSub.isValid();}
+ bool hasGlobalSubsystem() const {return globalSub.isValid();}
+
+ const DecorationSubsystem& getDecorationSubsystem() const {
+ assert(decorationSub.isValid());
+ return DecorationSubsystem::downcast(getSubsystem(decorationSub));
+ }
+
+ const GeneralContactSubsystem& getContactSubsystem() const {
+ assert(contactSub.isValid());
+ return GeneralContactSubsystem::downcast(getSubsystem(contactSub));
+ }
+
+ SimbodyMatterSubsystem& updMatterSubsystem() {
+ assert(matterSub.isValid());
+ return SimbodyMatterSubsystem::updDowncast(updSubsystem(matterSub));
+ }
+ ForceSubsystem& updForceSubsystem(SubsystemIndex id) {
+ return ForceSubsystem::updDowncast(updSubsystem(id));
+ }
+ MultibodySystemGlobalSubsystem& updGlobalSubsystem() {
+ assert(globalSub.isValid());
+ return MultibodySystemGlobalSubsystem::updDowncast(updSubsystem(globalSub));
+ }
+ DecorationSubsystem& updDecorationSubsystem() {
+ assert(decorationSub.isValid());
+ return DecorationSubsystem::updDowncast(updSubsystem(decorationSub));
+ }
+ GeneralContactSubsystem& updContactSubsystem() {
+ assert(contactSub.isValid());
+ return GeneralContactSubsystem::updDowncast(updSubsystem(contactSub));
+ }
+
+ // Global state cache entries dealing with interaction between forces &
+ // matter.
+
+ // Responses available when the global subsystem is advanced to the
+ // indicated stage or higher.
+ const Vector_<SpatialVec>& getRigidBodyForces(const State& s, Stage g) const {
+ return getGlobalSubsystem().getRep().getRigidBodyForces(s,g);
+ }
+ const Vector_<Vec3>& getParticleForces(const State& s, Stage g) const {
+ return getGlobalSubsystem().getRep().getParticleForces(s,g);
+ }
+ const Vector& getMobilityForces(const State& s, Stage g) const {
+ return getGlobalSubsystem().getRep().getMobilityForces(s,g);
+ }
+ const Real calcPotentialEnergy(const State& s) const {
+ Real pe = 0;
+ for (int i = 0; i < (int) forceSubs.size(); ++i)
+ pe += getForceSubsystem(forceSubs[i]).getRep().calcPotentialEnergy(s);
+ return pe;
+ }
+
+ // These are the global subsystem cache entries at the indicated Stage.
+ // This will reduce the stage of s to the previous stage.
+ Vector_<SpatialVec>& updRigidBodyForces(const State& s, Stage g) const {
+ return getGlobalSubsystem().getRep().updRigidBodyForces(s,g);
+ }
+ Vector_<Vec3>& updParticleForces(const State& s, Stage g) const {
+ return getGlobalSubsystem().getRep().updParticleForces(s,g);
+ }
+ Vector& updMobilityForces(const State& s, Stage g) const {
+ return getGlobalSubsystem().getRep().updMobilityForces(s,g);
+ }
+
+ // pure virtual
+ MultibodySystemRep* cloneImpl() const OVERRIDE_11
+ { return new MultibodySystemRep(*this); }
+
+ // Override the SystemRep default implementations for these virtual methods.
+ int realizeTopologyImpl (State&) const OVERRIDE_11;
+ int realizeModelImpl (State&) const OVERRIDE_11;
+ int realizeInstanceImpl (const State&) const OVERRIDE_11;
+ int realizeTimeImpl (const State&) const OVERRIDE_11;
+ int realizePositionImpl (const State&) const OVERRIDE_11;
+ int realizeVelocityImpl (const State&) const OVERRIDE_11;
+ int realizeDynamicsImpl (const State&) const OVERRIDE_11;
+ int realizeAccelerationImpl(const State&) const OVERRIDE_11;
+ int realizeReportImpl (const State&) const OVERRIDE_11;
+
+
+ void multiplyByNImpl(const State& s, const Vector& u,
+ Vector& dq) const OVERRIDE_11 {
+ const SimbodyMatterSubsystem& mech = getMatterSubsystem();
+ mech.getRep().multiplyByN(s,false,u,dq);
+ }
+ void multiplyByNTransposeImpl(const State& s, const Vector& fq,
+ Vector& fu) const OVERRIDE_11 {
+ const SimbodyMatterSubsystem& mech = getMatterSubsystem();
+ mech.getRep().multiplyByN(s,true,fq,fu);
+ }
+ void multiplyByNPInvImpl(const State& s, const Vector& dq,
+ Vector& u) const OVERRIDE_11 {
+ const SimbodyMatterSubsystem& mech = getMatterSubsystem();
+ mech.getRep().multiplyByNInv(s,false,dq,u);
+ }
+ void multiplyByNPInvTransposeImpl(const State& s, const Vector& fu,
+ Vector& fq) const OVERRIDE_11 {
+ const SimbodyMatterSubsystem& mech = getMatterSubsystem();
+ mech.getRep().multiplyByNInv(s,true,fu,fq);
+ }
+
+ // Currently prescribe() and project() affect only the Matter subsystem.
+ bool prescribeQImpl(State& state) const OVERRIDE_11 {
+ const SimbodyMatterSubsystem& mech = getMatterSubsystem();
+ return mech.getRep().prescribeQ(state);
+ }
+ bool prescribeUImpl(State& state) const OVERRIDE_11 {
+ const SimbodyMatterSubsystem& mech = getMatterSubsystem();
+ return mech.getRep().prescribeU(state);
+ }
+
+ void projectQImpl(State& state, Vector& qErrEst,
+ const ProjectOptions& options,
+ ProjectResults& results) const OVERRIDE_11 {
+ const SimbodyMatterSubsystem& mech = getMatterSubsystem();
+ mech.getRep().projectQ(state, qErrEst, options, results);
+ realize(state, Stage::Position); // realize the whole system now
+ }
+ void projectUImpl(State& state, Vector& uErrEst,
+ const ProjectOptions& options,
+ ProjectResults& results) const OVERRIDE_11 {
+ const SimbodyMatterSubsystem& mech = getMatterSubsystem();
+ mech.getRep().projectU(state, uErrEst, options, results);
+ realize(state, Stage::Velocity); // realize the whole system now
+ }
+
+ void getFreeQIndexImpl
+ (const State& s, Array_<SystemQIndex>& freeQs) const OVERRIDE_11 {
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+ const SystemQIndex qStart = matter.getQStart(s);
+ const Array_<QIndex>& matterFreeQs = matter.getFreeQIndex(s);
+ freeQs.resize(matterFreeQs.size());
+ for (unsigned i=0; i < matterFreeQs.size(); ++i)
+ freeQs[i] = SystemQIndex(qStart + matterFreeQs[i]);
+ }
+ void getFreeUIndexImpl
+ (const State& s, Array_<SystemUIndex>& freeUs) const OVERRIDE_11 {
+ const SimbodyMatterSubsystem& matter = getMatterSubsystem();
+ const SystemUIndex uStart = matter.getUStart(s);
+ const Array_<UIndex>& matterFreeUs = matter.getFreeUIndex(s);
+ freeUs.resize(matterFreeUs.size());
+ for (unsigned i=0; i < matterFreeUs.size(); ++i)
+ freeUs[i] = SystemUIndex(uStart + matterFreeUs[i]);
+ }
+ /* TODO: not yet
+ virtual void handleEventsImpl
+ (State&, EventCause, const Array<EventId>& eventIds,
+ Real accuracy, const Vector& yWeights, const Vector& ooConstraintTols,
+ Stage& lowestModified, bool& shouldTerminate) const;
+ virtual int calcEventTriggerInfoImpl(const State&, Array<EventTriggerInfo>&) const;
+ virtual int calcTimeOfNextScheduledEventImpl
+ (const State&, Real& tNextEvent, Array<EventId>& eventIds) const;
+ */
+
+ SimTK_DOWNCAST(MultibodySystemRep, System::Guts);
+private:
+ SubsystemIndex globalSub; // index of global subsystem
+ SubsystemIndex matterSub; // index of matter subsystems
+ Array_<SubsystemIndex> forceSubs; // indices of force subsystems
+ SubsystemIndex decorationSub; // index of DecorationSubsystem if any, else -1
+ SubsystemIndex contactSub; // index of contact subsystem if any, else -1
+};
+
+
+} // namespace SimTK
+
+#endif // SimTK_SIMBODY_MULTIBODY_SYSTEM_REP_H_
diff --git a/Simbody/src/OBSOLETE_LengthConstraints.cpp b/Simbody/src/OBSOLETE_LengthConstraints.cpp
new file mode 100644
index 0000000..db357e8
--- /dev/null
+++ b/Simbody/src/OBSOLETE_LengthConstraints.cpp
@@ -0,0 +1,1387 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Derived from IVM code written by Charles Schwieters *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+namespace SimTK {
+void dummySymbolToAvoidWarningInLengthConstraints() {}
+};
+
+#ifdef NOTDEF
+
+
+/**@file
+ *
+ * Module for bond-length constraints use a shake-like algorithm.
+ * Requires integrator to specify time derivatives to constrain.
+ *
+ * setup:
+ * 1) determine couplings
+ * 2) determine which time derivatives to be applied
+ *
+ * execution:
+ * 1) need all time derivs of theta passed in
+ * 2) iteratively apply constraints from lowest to highest time deriv
+ *
+ * Deal with loop bond-length constraints.
+ */
+
+#include "simbody/internal/common.h"
+using namespace SimTK;
+
+#include "LengthConstraints.h"
+#include "RigidBodyNode.h"
+#include "SimbodyMatterSubsystemRep.h"
+
+#include "newtonRaphson.h"
+
+#include <iostream>
+using std::ostream;
+using std::cout;
+using std::endl;
+using std::cerr;
+using std::setw;
+
+#include <algorithm>
+
+class LoopWNodes;
+static int compareLevel(const LoopWNodes& l1, //forward declarations
+ const LoopWNodes& l2);
+
+//static bool sameBranch(const RigidBodyNode* tip,
+// const LoopWNodes& l );
+
+class BadNodeDef {}; //exception
+
+
+LoopWNodes::LoopWNodes(const SimbodyMatterSubsystemRep& t, const RBDistanceConstraint& dc)
+ : tree(&t), rbDistCons(&dc), flipStations(false), outmostCommonBody(0)
+{
+ const RigidBodyNode* dcNode1 = &dc.getStation(1).getNode();
+ const RigidBodyNode* dcNode2 = &dc.getStation(2).getNode();
+
+ if (dcNode1==dcNode2) {
+ cout << "LoopWNodes::LoopWNodes: bad topology:\n\t"
+ << "loop stations " << dc.getStation(1)
+ << " and " << dc.getStation(2)
+ << " are now in the same node. Deleting loop.\n";
+ SimTK_THROW1(SimTK::Exception::LoopConstraintConstructionFailure, "bad topology");
+ }
+
+ // Ensure that tips(2) is on the body which is farther from Ground.
+ flipStations = (dcNode1->getLevel() > dcNode2->getLevel());
+
+ // OK to use tips() at this point.
+
+ // Collect up the node path from tips(2) down to the last node on its
+ // side of the loop which is at a higher level than tips(1) (may be none).
+ const RigidBodyNode* node1 = &tips(1).getNode();
+ const RigidBodyNode* node2 = &tips(2).getNode();
+ while ( node2->getLevel() > node1->getLevel() ) {
+ nodes[1].push_back(node2);
+ node2 = node2->getParent();
+ }
+
+ // We're at the same level on both sides of the loop. Run down both
+ // simultaneously until we hit the first common ancestor, collecting
+ // up the nodes along the two paths (but not the common ancestor).
+ while ( node1 != node2 ) {
+ if ( node1->isGroundNode() ) {
+ cerr << "LoopWNodes::LoopWNodes: could not find base node.\n\t"
+ << "loop between stations " << tips(1) << " and "
+ << tips(2) << "\n";
+ SimTK_THROW1(SimTK::Exception::LoopConstraintConstructionFailure,
+ "could not find base node");
+ }
+ nodes[0].push_back(node1);
+ nodes[1].push_back(node2);
+ node1 = node1->getParent();
+ node2 = node2->getParent();
+ }
+
+ outmostCommonBody = node1; // that's the common ancestor; might be Ground
+
+ // We want these in base-to-tip order.
+ std::reverse(nodes[0].begin(), nodes[0].end());
+ std::reverse(nodes[1].begin(), nodes[1].end());
+
+ // Make the list of ancestors, starting with the outmostCommonBody unless it
+ // is ground. Put in base-to-tip order.
+ const RigidBodyNode* next = outmostCommonBody;
+ while (next->getLevel() != 0) {
+ ancestors.push_back(next);
+ next = next->getParent();
+ }
+ std::reverse(ancestors.begin(), ancestors.end());
+}
+
+
+ostream&
+operator<<(ostream& o, const LengthSet& s)
+{
+ o << "LengthSet --------------->\n";
+ for (int i=0 ; i<(int)s.loops.size() ; i++) {
+ o << "Loop " << i << ":\n" << s.loops[i];
+ }
+ o << "\nNodemap: ";
+ for (int i=0; i<(int)s.nodeMap.size(); ++i)
+ o << " " << s.nodeMap[i]->getNodeNum()
+ << "[" << s.nodeMap[i]->getLevel() << "]";
+ o << "\n<-------- end of LengthSet\n";
+ return o;
+}
+
+
+ostream&
+operator<<(ostream& o, const LoopWNodes& w)
+{
+ o << "tip1=" << w.tips(1) << " tip2=" << w.tips(2)
+ << " distance=" << w.getDistance() << endl;
+ o << "nodes[0]:";
+ for (int i=0; i<(int)w.nodes[0].size(); ++i)
+ o << " " << w.nodes[0][i]->getNodeNum()
+ << "[" << w.nodes[0][i]->getLevel() << "]";
+ o << "\nnodes[1]:";
+ for (int i=0; i<(int)w.nodes[1].size(); ++i)
+ o << " " << w.nodes[1][i]->getNodeNum()
+ << "[" << w.nodes[1][i]->getLevel() << "]";
+ o << "\nOutmost common body: " << w.getOutmostCommonBody()->getNodeNum()
+ << "[" << w.getOutmostCommonBody()->getLevel() << "]";
+ o << "\nAncestors: ";
+ for (int i=0; i<(int)w.ancestors.size(); ++i)
+ o << " " << w.ancestors[i]->getNodeNum()
+ << "[" << w.ancestors[i]->getLevel() << "]";
+ o << endl;
+ return o;
+}
+
+LengthConstraints::LengthConstraints
+ (const SimbodyMatterSubsystemRep& rbt, int vbose)
+ : maxIters( 20 ), maxMin( 20 ),
+ rbTree(rbt), verbose(vbose), posMin("posMin", cout), velMin("velMin", cout)
+{
+ posMin.maxIters = maxIters;
+ posMin.maxMin = maxMin;
+ velMin.maxIters = maxIters;
+ velMin.maxMin = maxMin;
+}
+
+//
+// compare LoopWNodes by outmost common node level value
+//
+static int
+compareLevel(const LoopWNodes& l1,
+ const LoopWNodes& l2)
+{
+ if ( l1.getOutmostCommonBody()->getLevel() > l2.getOutmostCommonBody()->getLevel() )
+ return 1;
+ else if ( l1.getOutmostCommonBody()->getLevel() < l2.getOutmostCommonBody()->getLevel() )
+ return -1;
+ else
+ return 0;
+}
+
+// Define this operator so std::sort will work.
+static inline bool operator<(const LoopWNodes& l1, const LoopWNodes& l2) {
+ return compareLevel(l1,l2) == -1;
+}
+
+//1) construct: given list of loops
+// a) if appropriate issue warning and exit.
+// b) sort by base node of each loop.
+// c) find loops which intersect: combine loops and increment
+// number of length constraints
+//
+// Sherm's constraint coupling hypothesis (060616):
+//
+// For a constraint i:
+// Kinematic participants K[i]
+// = the set of bodies whose mobilities can affect verr[i]
+// Dynamic participants D[i]
+// = the set of bodies whose effective inertias can change
+// as a result of enforcement of constraint i
+// TODO: A thought on dynamic coupling -- is this the same as computing
+// effective joint forces? Then only mobilities that can feel a
+// test constraint force can be considered dynamic participants of
+// that constraint.
+//
+// Constraints i & j are directly kinematically coupled if
+// K[i] intersect K[j] <> { }
+// Constraints i & j are directly dynamically coupled if
+// (a) they are directly kinematically coupled, or
+// (b) D[i] intersect D[j] <> { }
+// TODO: I *think* dynamic coupling is one-way, from outboard to inboard.
+// That means it defines an evaluation *order*, where modified
+// inertias from outboard loops become the articulated body
+// inertias to use in the inboard loops, rather than requiring simultaneous
+// solutions for the multipliers.
+// Kinematic coupling, on the other hand, is symmetric and implies
+// simultaneous solution.
+//
+// Compute transitive closure:
+// Constraints i & j are kinematically coupled if
+// (a) they are directly kinematically coupled, or
+// (b) constraint i is kinematically coupled to any constraint
+// k to which j is kinematically coupled
+// That is, build kinematic clusters with completely disjoint constraints.
+//
+// For now, do the same thing with acceleration constraints but this
+// is too strict. TODO
+//
+//
+void
+LengthConstraints::construct(const Array_<RBDistanceConstraint*>& iloops)
+{
+ //clean up
+ pvConstraints.resize(0);
+ accConstraints.resize(0);
+
+ // if ( !useLengthConstraint ) FIX:!!!
+ // //issue error message?
+ // return;
+
+ LoopList loops;
+ for (int i=0 ; i < (int)iloops.size() ; i++) {
+ try {
+ LoopWNodes loop(rbTree, *iloops[i] );
+ loops.push_back( loop );
+ }
+ catch ( BadNodeDef ) {}
+ }
+
+ //cout << "RAW LOOPS:\n";
+ //for (int i=0; i < (int)loops.size(); ++i)
+ // cout << "Loop " << i << ":\n " << loops[i] << endl;
+
+ // sort loops by outmostCommonBody->level
+ std::sort(loops.begin(), loops.end()); // uses "<" operator by default; see above
+ LoopList accLoops = loops; //version for acceleration
+
+ // Sherm 060616: transitive closure calculation. Remember that
+ // the constraints are sorted by the level of their outmost common body,
+ // starting at ground, so that more-outboard constraints cannot cause
+ // us to need to revisit an earlier cluster. However, constraints at
+ // the same level can bring together earlier ones at that level.
+ // TODO: currently just restarting completely when we add a
+ // constraint to a cluster.
+
+ for (int i=0 ; i<(int)loops.size() ; i++) {
+ pvConstraints.push_back(LengthSet(this));
+ pvConstraints.back().addKinematicConstraint(loops[i]);
+ bool addedAConstraint;
+ do {
+ addedAConstraint = false;
+ for (int j=i+1 ; j<(int)loops.size() ; j++)
+ if ( (loops[j].nodes[0].size()
+ && pvConstraints[i].contains(loops[j].nodes[0][0]))
+ || (loops[j].nodes[1].size()
+ && pvConstraints[i].contains(loops[j].nodes[1][0])))
+ {
+ //add constraint equation j to cluster i
+ pvConstraints[i].addKinematicConstraint(loops[j]);
+ loops.erase(loops.begin() + j); // STL for &loops[j]
+ addedAConstraint = true;
+ break;
+ }
+ } while (addedAConstraint);
+ }
+
+ for (int i=0 ; i<(int)accLoops.size() ; i++) {
+ accConstraints.push_back(LengthSet(this));
+ accConstraints.back().addDynamicConstraint(accLoops[i]);
+ bool addedAConstraint;
+ do {
+ addedAConstraint = false;
+ for (int j=i+1 ; j<(int)accLoops.size() ; j++)
+ if ( (accLoops[j].nodes[0].size()
+ && accConstraints[i].contains(accLoops[j].nodes[0][0]))
+ || (accLoops[j].nodes[1].size()
+ && accConstraints[i].contains(accLoops[j].nodes[1][0]))
+ || (accLoops[j].ancestors.size()
+ && accConstraints[i].contains(accLoops[j].ancestors[0])))
+ {
+ //add constraint equation j to cluster i
+ accConstraints[i].addDynamicConstraint(accLoops[j]);
+ accLoops.erase(accLoops.begin() + j); // STL for &accLoops[j]
+ addedAConstraint = true;
+ break;
+ }
+ } while (addedAConstraint);
+ }
+
+ if (false && pvConstraints.size()>0) {
+ cout << "LengthConstraints::construct: pos/vel length sets found:\n";
+ for (int i=0 ; i<(int)pvConstraints.size() ; i++)
+ cout << pvConstraints[i] << "\n";
+ cout << "LengthConstraints::construct: accel length sets found:\n";
+ for (int i=0 ; i<(int)accConstraints.size() ; i++)
+ cout << accConstraints[i] << "\n";
+ }
+}
+
+void LengthSet::addKinematicConstraint(const LoopWNodes& loop) {
+ loops.push_back( LoopWNodes(loop) );
+ for (int b=0 ; b<2 ; b++)
+ for (int i=0; i<(int)loop.nodes[b].size(); i++)
+ if (std::find(nodeMap.begin(),nodeMap.end(),loop.nodes[b][i])
+ ==nodeMap.end())
+ {
+ // not found
+ ndofThisSet += loop.nodes[b][i]->getDOF();
+ nodeMap.push_back( loop.nodes[b][i] );
+ }
+}
+
+void LengthSet::addDynamicConstraint(const LoopWNodes& loop) {
+ loops.push_back( LoopWNodes(loop) );
+ for (int b=0 ; b<2 ; b++)
+ for (int i=0; i<(int)loop.nodes[b].size(); i++)
+ if (std::find(nodeMap.begin(),nodeMap.end(),loop.nodes[b][i])
+ ==nodeMap.end())
+ {
+ // not found
+ ndofThisSet += loop.nodes[b][i]->getDOF();
+ nodeMap.push_back( loop.nodes[b][i] );
+ }
+
+ for (int i=0; i<(int)loop.ancestors.size(); i++)
+ if (std::find(nodeMap.begin(),nodeMap.end(),loop.ancestors[i])
+ ==nodeMap.end())
+ {
+ // not found
+ ndofThisSet += loop.ancestors[i]->getDOF();
+ nodeMap.push_back( loop.ancestors[i] );
+ }
+}
+
+bool LengthSet::contains(const RigidBodyNode* node) {
+ return std::find(nodeMap.begin(),nodeMap.end(),node) != nodeMap.end();
+}
+
+class CalcPosB {
+ State& s;
+ const LengthSet* lengthSet;
+public:
+ CalcPosB(State& ss, const LengthSet* lset)
+ : s(ss), lengthSet(lset) {}
+ Vector operator()(const Vector& pos) const
+ { return lengthSet->calcPosB(s,pos); }
+};
+
+class CalcPosZ {
+ const State& s;
+ const LengthSet* lengthSet;
+public:
+ CalcPosZ(const State& ss, const LengthSet* constraint)
+ : s(ss), lengthSet(constraint) {}
+ Vector operator()(const Vector& b) const
+ { return lengthSet->calcPosZ(s, b); }
+};
+
+class CalcVelB {
+ State& s;
+ const LengthSet* lengthSet;
+public:
+ CalcVelB(State& ss, const LengthSet* constraint)
+ : s(ss), lengthSet(constraint) {}
+ Vector operator()(const Vector& vel)
+ { return lengthSet->calcVelB(s,vel); }
+};
+
+//
+// Calculate the position constraint violation (zero when constraint met).
+//
+Vector
+LengthSet::calcPosB(State& s, const Vector& pos) const
+{
+ setPos(s, pos);
+
+ // Although we're not changing the qErr cache entry here, we access
+ // it with "upd" because setPos() will have modified the q's and
+ // thus invalidated stage Position, so a "get" would fail.
+ const Vector& qErr = getRBTree().updQErr(s);
+ Vector b((int)loops.size());
+ for (int i=0; i<(int)loops.size(); ++i)
+ b[i] = loops[i].rbDistCons->getPosErr(qErr);
+ return b;
+}
+
+//
+// Calculate the velocity constraint violation (zero when constraint met).
+//
+Vector
+LengthSet::calcVelB(State& s, const Vector& vel) const
+{
+ setVel(s, vel);
+
+ // Although we're not changing the uErr cache entry here, we access
+ // it with "upd" because setVel() will have modified the u's and
+ // thus invalidated stage Velocity, so a "get" would fail.
+ const Vector& uErr = getRBTree().updUErr(s);
+
+ Vector b((int)loops.size());
+ for (int i=0; i<(int)loops.size(); ++i)
+ b[i] = loops[i].rbDistCons->getVelErr(uErr);
+ return b;
+}
+
+//
+// Given a vector containing violations of position constraints, calculate
+// a state update which should drive those violations to zero (if they
+// were linear).
+//
+// This is a little tricky since the gradient we have is actually the
+// gradient of the *velocity* errors with respect to the generalized speeds,
+// but we want the gradient of the *position* errors with respect to
+// the generalized coordinates. The theory goes something like this:
+//
+// d perr d perr_dot d verr d u
+// ------ = ---------- = ------ * -----
+// d q d qdot d u d qdot
+//
+// Let P = d perr/dq, A = d verr/du, Q = d qdot/du.
+//
+// We want to find a change deltaq that will elimate the current error b:
+// P deltaq = b. [WRONG:]Instead we solve A * x = b, where x = inv(Q) * deltaq,
+// and then solve deltaq = Q * x. Conveniently Q is our friendly invertible
+// relation between qdot's and u's: qdot = Q*u.
+//
+// TODO: Sherm 080101: the above is incorrect. We have to factor (PQ^-1) and
+// solve (PQ^-1)*deltaq = b for LS deltaq, which is not the same as
+// solving for LS x in P*x=b and then deltaq=Q*x.
+//
+// TODO: I have oversimplified the above since we are really looking for
+// a least squares solution to an underdetermined system. With the
+// pseudoinverse A+ available we can write x = A+ * b.
+//
+Vector
+LengthSet::calcPosZ(const State& s, const Vector& b) const
+{
+ const Matrix Gt = calcGrad(s);
+ const Vector x = calcPseudoInverseA(Gt) * b;
+
+ const SBStateDigest digest(s, getRBTree(), Stage::Position);
+
+ Vector zu(getRBTree().getTotalDOF(),0.);
+ Vector zq(getRBTree().getTotalQAlloc(),0.);
+
+ // map the vector dir back to the appropriate elements of z
+ int indx=0; // sherm 060222: I added this
+ for (int i=0 ; i<(int)nodeMap.size() ; i++) {
+ const int d = nodeMap[i]->getDOF();
+ const int offs = nodeMap[i]->getUIndex();
+ zu(offs,d) = x(indx,d);
+ indx += d;
+
+ // Make qdot = Q*u.
+ nodeMap[i]->calcQDot(digest,zu,zq); // change u's to qdot's
+ }
+ assert(indx == ndofThisSet);
+
+ return zq;
+}
+
+//
+// Given a vector containing violations of velocity constraints, calculate
+// a state update which would drive those violations to zero (if they
+// are linear and well conditioned).
+//
+// This is simpler than CalcPosZ because we have the right gradient here (it's
+// the same one in both places). TODO: and shouldn't be recalculated!
+//
+class CalcVelZ {
+ const State& s;
+ const LengthSet* lengthSet;
+ const Matrix Gt;
+ const Matrix GInverse;
+public:
+ CalcVelZ(const State& ss, const LengthSet* lset)
+ : s(ss), lengthSet(lset), Gt(lset->calcGrad(s)),
+ GInverse(LengthSet::calcPseudoInverseA(Gt))
+ {
+ }
+
+ Vector operator()(const Vector& b) {
+ const Vector dir = GInverse * b;
+ Vector z(lengthSet->getRBTree().getTotalDOF(),0.0);
+
+ // map the vector dir back to the appropriate elements of z
+ int indx=0; // sherm 060222: I added this
+ for (int i=0 ; i<(int)lengthSet->nodeMap.size() ; i++) {
+ const RigidBodyNode* n = lengthSet->nodeMap[i];
+ const int d = n->getDOF();
+ const int offs = n->getUIndex();
+ z(offs,d) = dir(indx,d);
+ indx += d;
+ }
+ assert(indx == lengthSet->ndofThisSet);
+ return z;
+ }
+};
+
+// Project out the position constraint errors from the given state.
+bool
+LengthConstraints::enforcePositionConstraints(State& s, const Real& requiredTol, const Real& desiredTol) const
+{
+ assert(rbTree.getStage(s) >= Stage::Position-1);
+ Vector& pos = rbTree.updQ(s);
+
+ bool anyChanges = false;
+
+ try {
+ for (int i=0 ; i<(int)pvConstraints.size() ; i++) {
+ anyChanges = true; // TODO: assuming for now
+ posMin.calc(requiredTol, desiredTol, pos,
+ CalcPosB(s, &pvConstraints[i]),
+ CalcPosZ(s, &pvConstraints[i]));
+ }
+ }
+ catch ( SimTK::Exception::NewtonRaphsonFailure cptn ) {
+ cout << "LengthConstraints::enforcePositionConstraints: exception: "
+ << cptn.getMessage() << '\n';
+ }
+
+ return anyChanges;
+}
+
+// Project out the velocity constraint errors from the given state.
+bool
+LengthConstraints::enforceVelocityConstraints(State& s, const Real& requiredTol, const Real& desiredTol) const
+{
+ assert(rbTree.getStage(s) >= Stage(Stage::Velocity).prev());
+ Vector& vel = rbTree.updU(s);
+
+ bool anyChanges = false;
+
+ try {
+ for (int i=0 ; i<(int)pvConstraints.size() ; i++) {
+ anyChanges = true; // TODO: assuming for now
+ velMin.calc(requiredTol, desiredTol, vel,
+ CalcVelB(s, &pvConstraints[i]),
+ CalcVelZ(s, &pvConstraints[i]));
+ }
+ }
+ catch ( SimTK::Exception::NewtonRaphsonFailure cptn ) {
+ cout << "LengthConstraints::enforceVelocityConstraints: exception: "
+ << cptn.getMessage() << '\n';
+ }
+
+ return anyChanges;
+}
+
+//
+//// on each iteration
+//// for each loop
+//// -first calc all usual properties
+//// -recursively compute phi_ni for each length constraint
+//// from tip to outmost common body of
+//// -compute gradient
+//// -update theta using quasi-Newton-Raphson
+//// -compute Cartesian coords
+//// -check for convergence
+//// -repeat
+//
+
+
+void LengthSet::setPos(State& s, const Vector& pos) const
+{
+ const SBStateDigest digest(s, getRBTree(), Stage::Position);
+ Vector& q = getRBTree().updQ(s);
+ SBPositionCache& pc = getRBTree().updPositionCache(s);
+ Vector& qErr = getRBTree().updQErr(s);
+
+ for (int i=0 ; i<(int)nodeMap.size() ; i++)
+ nodeMap[i]->copyQ(digest, pos, q);
+
+ // TODO: sherm this is the wrong place for the stage update!
+ s.invalidateAll(Stage::Position);
+
+ // sherm TODO: this now computes kinematics for the whole system,
+ // but it should only have to update the loop we are interested in.
+ // Schwieters had this right before because his equivalent of 'setQ'
+ // above also performed the kinematics, while ours just saves the
+ // new state variable values and calculates here:
+ getRBTree().realizeSubsystemPosition(s);
+
+ // TODO: This is redundant after realizePosition(), but I'm leaving
+ // it here because this is actually all that need be recalculated for
+ // the loop closure iterations.
+ for (int i=0; i<(int)loops.size(); ++i)
+ loops[i].calcPosInfo(qErr,pc);
+}
+
+// Must have called LengthSet::setPos() already.
+void LengthSet::setVel(State& s, const Vector& vel) const
+{
+ const SBStateDigest digest(s, getRBTree(), Stage::Position);
+ const SBPositionCache& pc = getRBTree().getPositionCache(s);
+ Vector& u = getRBTree().updU(s);
+ SBVelocityCache& vc = getRBTree().updVelocityCache(s);
+ Vector& uErr = getRBTree().updUErr(s);
+
+ for (int i=0 ; i<(int)nodeMap.size() ; i++)
+ nodeMap[i]->copyU(digest, vel, u);
+
+ // TODO: sherm this is the wrong place for the stage update!
+ s.invalidateAll(Stage::Velocity);
+
+ getRBTree().realizeSubsystemVelocity(s);
+
+ // TODO: see comment above in setPos
+ for (int i=0; i<(int)loops.size(); ++i)
+ loops[i].calcVelInfo(pc,uErr,vc);
+}
+
+//
+////A = df / dtheta_i
+//// = [ -(q_ni-qn0)x , 1 ] [ phi_na^T Ha , phi_nb^T Hb , ... ]^T
+////
+//
+
+// Calculate gradient by central difference for testing the analytic
+// version. Presumes that calcEnergy has been called previously with current
+// value of ipos.
+void
+LengthSet::fdgradf(State& s,
+ const Vector& pos,
+ Matrix& grad) const
+{
+ const SBModelVars& mv = getRBTree().getModelVars(s);
+
+ // Gradf gradf(tree);
+ // gradf(x,grad); return;
+ const double eps = 1e-6;
+ const CalcPosB calcB(s, this);
+ const Vector b = calcB(pos);
+ int grad_indx=0;
+ for (int i=0 ; i<(int)nodeMap.size() ; i++) {
+ int pos_indx=nodeMap[i]->getQIndex();
+ for (int j=0 ; j<nodeMap[i]->getNQInUse(mv) ; j++,pos_indx++,grad_indx++) {
+ Vector posp = pos;
+ posp(pos_indx) += eps;
+ const Vector bp = calcB(posp);
+ posp(pos_indx) -= 2.*eps;
+ const Vector bpm = calcB(posp);
+ for (int k=0 ; k<b.size() ; k++)
+ grad(grad_indx,k) = -(bp(k)-bpm(k)) / (2.*eps);
+ }
+ }
+}
+
+void
+LengthSet::testGrad(State& s, const Vector& pos, const Matrix& grad) const
+{
+ double tol = 1e-4;
+
+ Matrix fdgrad(ndofThisSet,loops.size());
+ fdgradf(s,pos,fdgrad);
+
+ for (int i=0 ; i<grad.nrow() ; i++)
+ for (int j=0 ; j<grad.ncol() ; j++)
+ if (fabs(grad(i,j)-fdgrad(i,j)) > fabs(tol))
+ cout << "testGrad: error in gradient: "
+ << setw(2) << i << ' '
+ << setw(2) << j << ": "
+ << grad(i,j) << ' ' << fdgrad(i,j) << '\n';
+ cout.flush();
+}
+
+//
+// unitVec(p+ - p-) * d (p+ - p-) / d (theta_i)
+// d g / d theta for all hingenodes in nodemap
+//
+// sherm: this appears to calculate the transpose of G
+//
+// sherm 060222: OK, this routine doesn't really do what it says
+// when the constraint includes a ball or free joint. It
+// does not use the actual q's, which are quaternions. Instead
+// it is something like d(v+ - v-)/du. If the u's are the
+// angular velocity across the joint then this is
+// d(p+ - p-)/d qbar where qbar is a 1-2-3 Euler sequence
+// which is 0,0,0 at the current orientation. ("instant
+// coordinates").
+//
+// sherm: 060303 This routine uses the joint transition matrices ~H,
+// which can be thought of as Jacobians ~H = d V_PB_G / d uB, that is
+// partial derivative of the cross-joint relative *spatial* velocity
+// with respect to that joint's generalized speeds uB. This allows
+// analytic computation of d verr / d u where verr is the set of
+// distance constraint velocity errors for the current set of
+// coupled loops, and u are the generalized speeds for all joints
+// which contribute to any of those loops. Some times this is the
+// gradient we want, but we also want to use this routine to
+// calculate d perr / d q, which we can't get directly. But it
+// is easily finagled into the right gradient; see CalcPosZ above.
+// The trickiest part is that we use nice physical variables for
+// generalized speeds, like angular velocities for ball joints,
+// but we use awkward mathematical constructs like quaternions and
+// Euler angles for q's.
+//
+Matrix
+LengthSet::calcGrad(const State& s) const
+{
+ // We're not updating, but need to use upd here because Position stage
+ // was invalidated by change to state.
+ const SBStateDigest digest(s, getRBTree(), Stage::Position);
+ const SBPositionCache& pc = getRBTree().updPositionCache(s);
+
+ Matrix grad(ndofThisSet,loops.size(),0.0);
+ const Mat33 one(1); //FIX: should be done once
+
+ for (int i=0 ; i<(int)loops.size() ; i++) {
+ const LoopWNodes& l = loops[i];
+ Array_<SpatialMat> phiT[2];
+ for (int b=0 ; b<2 ; b++) {
+ phiT[b].resize( l.nodes[b].size() );
+ if ( l.nodes[b].size() ) {
+ phiT[b][phiT[b].size()-1] = 1; // identity
+ for (int j=l.nodes[b].size()-2 ; j>=0 ; j-- ) {
+ const RigidBodyNode* n = l.nodes[b][j+1];
+ phiT[b][j] = phiT[b][j+1] * ~n->getPhi(pc);
+ }
+ }
+ }
+
+ // compute gradient
+ //Vec3 uBond = unitVec(l.tipPos(pc,2) - l.tipPos(pc,1));
+ const Vec3 uBond = l.tipPos(pc,2) - l.tipPos(pc,1); // p
+ Row<2,Mat33> J[2];
+ for (int b=1 ; b<=2 ; b++)
+ // TODO: get rid of this b-1; make tips 0-based
+ J[b-1] = Row<2,Mat33>(-crossMat(l.tipPos(pc,b) -
+ l.tips(b).getNode().getX_GB(pc).p()), one);
+ int g_indx=0;
+ for (int j=0 ; j<(int)nodeMap.size() ; j++) {
+ Real elem=0.0;
+
+ // We just want to get the index at which nodeMap[j] is found in the
+ // Array_ (or -1 if not found) but that's not so easy!
+ const Array_<const RigidBodyNode*>& n0 = l.nodes[0];
+ const Array_<const RigidBodyNode*>& n1 = l.nodes[1];
+ Array_<const RigidBodyNode*>::const_iterator found0 =
+ std::find(n0.begin(),n0.end(),nodeMap[j]);
+ Array_<const RigidBodyNode*>::const_iterator found1 =
+ std::find(n1.begin(),n1.end(),nodeMap[j]);
+
+ const int l1_indx = (found0==n0.end() ? -1 : found0-n0.begin());
+ const int l2_indx = (found1==n1.end() ? -1 : found1-n1.begin());
+
+ for (int k=0 ; k < nodeMap[j]->getDOF() ; k++) {
+ const SpatialVec& HtCol = ~nodeMap[j]->getHRow(digest, k);
+ if ( l1_indx >= 0 ) {
+ elem = -dot(uBond , Vec3(J[0] * phiT[0][l1_indx]*HtCol));
+ } else if ( l2_indx >= 0 ) {
+ elem = dot(uBond , Vec3(J[1] * phiT[1][l2_indx]*HtCol));
+ }
+ grad(g_indx++,i) = elem;
+ }
+ }
+ // added by sherm 060222:
+ assert(g_indx == ndofThisSet); // ??
+ }
+ return grad;
+}
+
+//
+// Calculate generalized inverse which minimizes changes in soln vector.
+// TODO (sherm) This is trying to create a pseudoinverse
+// using normal equations which is numerically bad and can't deal with
+// redundant constraints. Should use an SVD or (faster) QTZ factorization
+// instead.
+//
+// sherm 060314:
+// n
+// --------------------
+// | |
+// A (mXn) = m | | rank(A) <= m.
+// | |
+// --------------------
+//
+// The nXm pseudo-inverse A+ of a matrix A is A+ = V S+ ~U, where A=U*S*~V
+// is the singular value decomposition (SVD) of A, and S+ is the inverse
+// of the diagonal matrix S with some zeroes thrown it at the end after
+// we run out of rank(A). For an underdetermined system with full row
+// rank, that is, assuming m < n and rank(A)=m, you can do a poor man's
+// calculation of this as A+ = ~A*inv(A*~A). In the overdetermined
+// case we have m > n and rank(A)=n, in which case A+ = inv(~A*A)*~A.
+//
+// We are given gradient G (nuXnc). We're assuming nu > nc and rank(G)=nc, i.e.,
+// no redundant constraints (not a good assumption in general!). We would like
+// to get a least squares solution to ~G x = b where x has dimension nu and
+// b has dimension nc. So if we set A=~G we have the underdetermined system
+// depicted above and want to return A+ = ~A*inv(A*~A) = G*inv(~G*G). Ergo ...
+/*static*/ Matrix
+LengthSet::calcPseudoInverseA(const Matrix& transposeOfA)
+{
+ const Matrix A = ~transposeOfA; // now A is dg/dtheta
+ const int m = A.nrow(); // see picture above
+ const int n = A.ncol();
+
+ // Now calculate the pseudo inverse of A.
+ Matrix pinvA(n,m,0.);
+ if ( A.normSqr() > 1e-10 ) {
+ if (m < n)
+ pinvA = ~A * (A * ~A).invert(); // normal underdetermined case
+ else if (m > n)
+ pinvA = (A * ~A).invert() * ~A; // wow, that's a lot of constraints!
+ else
+ pinvA = A.invert(); // not likely
+ }
+ return pinvA;
+}
+
+
+//
+// Given a vector in mobility space, project it along the motion constraints
+// of this LengthSet, by removing its component normal to the constraint
+// manifold. Here we want to obtain a least squares solution x to
+// A x = A v, A=[ncXnu] is the constraint Jacobian.
+// We solve with pseudo inverse (see calcPseudoInverseA):
+// Least squares x = A+ * Av.
+// That is the component of v which is normal to the constraint manifold.
+// So we then set v = v - x and return.
+//
+// TODO: projections for integration errors should be done in the
+// error norm. I'm not sure whether that has to be done in this routine
+// or whether caller can scale on the way in and out.
+//
+void
+LengthSet::projectUVecOntoMotionConstraints(const State& s, Vector& v)
+{
+ const Matrix transposeOfA = calcGrad(s);
+ const Matrix pinvA = calcPseudoInverseA(transposeOfA); // TODO: this should already be available
+
+ const Vector rhs = packedMatTransposeTimesVec(transposeOfA,v); // i.e., rhs = Av
+
+ //FIX: using inverse is inefficient
+ const Vector x = pinvA * rhs;
+
+ // subtract forces due to these constraints
+ subtractPackedVecFromVec(v, x);
+}
+
+Matrix
+LengthSet::calcPseudoInverseAFD(const State& s) const
+{
+ Matrix grad(ndofThisSet,loops.size()); // <-- transpose of the actual dg/dtheta
+ State sTmp = s;
+ Vector pos = getRBTree().getQ(s); // a copy
+ fdgradf(sTmp,pos,grad);
+
+ const Matrix A = ~grad; // now A is dg/dtheta
+ const int m = A.nrow(); // see picture above
+ const int n = A.ncol();
+
+ // Now calculate the pseudo inverse of A.
+ Matrix pinvA(n,m,0.);
+ if ( A.normSqr() > 1e-10 ) {
+ if (m < n)
+ pinvA = ~A * (A * ~A).invert(); // normal underdetermined case
+ else if (m > n)
+ pinvA = (A * ~A).invert() * ~A; // wow, that's a lot of constraints!
+ else
+ pinvA = A.invert(); // not likely
+ }
+ return pinvA;
+}
+
+//acceleration:
+// 0) after initial acceleration calculation:
+// 1) calculate Y (block diagonal matrix) for all nodes (common body to tip)
+// 2) for each LengthSet, calculate Lagrange multiplier(s) and add in
+// resulting force- update accelerations
+// Do this step from tip to base.
+//
+
+bool LengthConstraints::calcConstraintForces(const State& s, const Vector& udotErr,
+ Vector& multipliers,
+ SBAccelerationCache& ac) const
+{
+ if ( accConstraints.size() == 0 )
+ return false;
+
+ rbTree.calcY(s); // TODO <-- this doesn't belong here!
+
+ for (int i=accConstraints.size()-1 ; i>=0 ; i--)
+ accConstraints[i].calcConstraintForces(s,udotErr,multipliers,ac);
+
+ return true;
+}
+
+void LengthConstraints::addInCorrectionForces(const State& s, const SBAccelerationCache& ac,
+ SpatialVecList& spatialForces) const
+{
+ for (int i=accConstraints.size()-1 ; i>=0 ; i--)
+ accConstraints[i].addInCorrectionForces(s, ac, spatialForces);
+}
+
+void
+LengthConstraints::projectUVecOntoMotionConstraints(const State& s, Vector& vec)
+{
+ if ( pvConstraints.size() == 0 )
+ return;
+
+ for (int i=pvConstraints.size()-1 ; i>=0 ; i--)
+ pvConstraints[i].projectUVecOntoMotionConstraints(s, vec);
+
+ for (int i=pvConstraints.size()-1 ; i>=0 ; i--)
+ pvConstraints[i].testProjectedVec(s, vec);
+}
+
+//
+// Calculate the acceleration of atom assuming that the spatial acceleration
+// of the body (node) it's on is available.
+//
+//static Vec3
+//getAccel(const IVMAtom* a)
+//{
+// const RigidBodyNode* n = a->node;
+// Vec3 ret( SubVec6(n->getSpatialAcc(),3,3).vector() );
+// ret += cross( SubVec6(n->getSpatialAcc(),0,3).vector() , a->pos - n->getAtom(0)->pos );
+// ret += cross( SubVec6(n->getSpatialVel(),0,3).vector() , a->vel - n->getAtom(0)->vel );
+// return ret;
+//}
+
+// T -1 T
+// Compute v1 *(J M J )_mn * v2 where the indices mn are given by
+// the nodes associated with stations s1 & s2.
+//
+static Real
+computeA(const SBPositionCache& cc,
+ const SBDynamicsCache& dc,
+ const Vec3& v1,
+ const LoopWNodes& loop1, int s1,
+ const LoopWNodes& loop2, int s2,
+ const Vec3& v2)
+{
+ const RigidBodyNode* n1 = &loop1.tips(s1).getNode();
+ const RigidBodyNode* n2 = &loop2.tips(s2).getNode();
+
+ const Mat33 one(1);
+
+ SpatialRow t1 = ~v1 * Row<2,Mat33>(crossMat(n1->getX_GB(cc).p() - loop1.tipPos(cc,s1)), one);
+ SpatialVec t2 = Vec<2,Mat33>(crossMat(loop2.tipPos(cc,s2) - n2->getX_GB(cc).p()), one) * v2;
+
+ while ( n1->getLevel() > n2->getLevel() ) {
+ t1 = t1 * ~n1->getPsi(dc);
+ n1 = n1->getParent();
+ }
+
+ while ( n2->getLevel() > n1->getLevel() ) {
+ t2 = n2->getPsi(dc) * t2;
+ n2 = n2->getParent();
+ }
+
+ while ( n1 != n2 ) {
+ if (n1->isGroundNode() || n2->isGroundNode() ) {
+ t1 = 0.; //not in same branch (or same tree -- sherm)
+ t2 = 0.;
+ cout << "computeA: cycles wasted calculating missed branch: "
+ << loop1.tips(s1) << " <-> " << loop2.tips(s2) << '\n';
+ return 0.;
+ }
+ t1 = t1 * ~n1->getPsi(dc);
+ t2 = n2->getPsi(dc) * t2;
+ n1 = n1->getParent();
+ n2 = n2->getParent();
+ }
+
+ // here n1==n2
+
+ Real ret = t1 * n1->getY(dc) * t2;
+ return ret;
+}
+
+//
+// To be called for LengthSets consecutively from tip to base.
+// Given acceleration errors for each loop contained in this LengthSet,
+// this will calculate a force for every station, and store that force
+// in the runtime block associated with that loop in the AccelerationCache
+// output argument. It is up to the caller to do something with these forces.
+//
+// See Section 2.6 on p. 294 of Schwieters & Clore,
+// J. Magnetic Resonance 152:288-302. Equation reference below
+// are to that paper. (sherm)
+//
+void
+LengthSet::calcConstraintForces(const State& s, const Vector& udotErr,
+ Vector& multipliers, SBAccelerationCache& ac) const
+{
+ const SBPositionCache& pc = getRBTree().getPositionCache(s);
+ const SBVelocityCache& vc = getRBTree().getVelocityCache(s);
+ const SBDynamicsCache& dc = getRBTree().getDynamicsCache(s);
+
+ // This is the acceleration error for each loop constraint in this
+ // LengthSet. We get a single scalar error per loop, since each
+ // contains one distance constraint.
+ // See Eq. [53] and the last term of Eq. [66].
+ Vector rhs(loops.size());
+ for (int i=0; i<(int)loops.size(); ++i)
+ rhs[i] = loops[i].rbDistCons->getAccErr(udotErr);
+
+ // Here A = Q*(J inv(M) J')*Q' where J is the kinematic Jacobian for
+ // the constrained points and Q is the constraint Jacobian. See first
+ // term of Eq. [66].
+ Matrix A(loops.size(),loops.size(),0.);
+ for (int i=0 ; i<(int)loops.size() ; i++) {
+ const Vec3 v1 = loops[i].tipPos(pc,2) - loops[i].tipPos(pc,1);
+ for (int bi=1 ; bi<=2 ; bi++)
+ for (int bj=1 ; bj<=2 ; bj++) {
+ for (int j=i ; j<(int)loops.size() ; j++) {
+ const Vec3 v2 = loops[j].tipPos(pc,2) - loops[j].tipPos(pc,1);
+ Real contrib = computeA(pc, dc, v1, loops[i], bi,
+ loops[j], bj, v2);
+ A(i,j) += contrib * (bi==bj ? 1 : -1);
+ }
+ }
+ }
+
+ // (sherm) Ouch -- this part is very crude. If this is known to be well
+ // conditioned it should be factored with a symmetric method
+ // like Cholesky, otherwise use an SVD (or better, complete orthogonal
+ // factorization QTZ) to get appropriate least squares solution.
+
+ for (int i=0 ; i<(int)loops.size() ; i++) //fill lower triangle
+ for (int j=0 ; j<i ; j++)
+ A(i,j) = A(j,i);
+
+ //cout << "Solve A lambda=rhs; A=" << A;
+ //cout << " rhs = " << rhs << endl;
+
+ //FIX: using inverse is inefficient
+ const Vector multipliersForThisSet = A.invert() * rhs;
+
+ //TODO: need to copy out the right multipliers to the full multipliers array
+
+ cout << " OLD lambda = " << multipliersForThisSet << endl;
+
+ // add forces due to these constraints
+ for (int i=0 ; i<(int)loops.size() ; i++) {
+ const Vec3 frc = multipliersForThisSet[i] * (loops[i].tipPos(pc,2) - loops[i].tipPos(pc,1));
+ loops[i].setTipForce(ac, 2, -frc);
+ loops[i].setTipForce(ac, 1, frc);
+ }
+}
+
+void LengthSet::addInCorrectionForces(const State& s, const SBAccelerationCache& ac,
+ SpatialVecList& spatialForces) const
+{
+ const SBPositionCache& pc = getRBTree().getPositionCache(s);
+
+ for (int i=0; i<(int)loops.size(); ++i) {
+ for (int t=1; t<=2; ++t) {
+ const RigidBodyNode& node = loops[i].tipNode(t);
+ const Vec3 force = loops[i].tipForce(ac,t);
+ const Vec3 moment = cross(loops[i].tipPos(pc,t) - node.getX_GB(pc).p(), force);
+ spatialForces[node.getNodeNum()] += SpatialVec(moment, force);
+ }
+ }
+}
+
+void LengthSet::testAccel(const State& s) const
+{
+ const Vector& udotErr = getRBTree().getUDotErr(s);
+
+ double testTol=1e-8;
+ for (int i=0 ; i<(int)loops.size() ; i++) {
+ const Real aerr = fabs(loops[i].rbDistCons->getAccErr(udotErr));
+ if (aerr > testTol)
+ cout << "LengthSet::testAccel: constraint condition between atoms "
+ << loops[i].tips(1) << " and " << loops[i].tips(2) << " violated.\n"
+ << "\tnorm of violation: " << aerr << '\n';
+ }
+ cout.flush();
+}
+
+
+// This just computes ~mat*vec but first plucks out the relevant entries
+// in vec to squash it down to the same size as mat.
+Vector
+LengthSet::packedMatTransposeTimesVec(const Matrix& packedMat, const Vector& vec)
+{
+ // sherm 060222:
+ assert(vec.size() == getRBTree().getTotalDOF());
+ assert(packedMat.nrow() == ndofThisSet && packedMat.ncol() == ndofThisSet);
+
+ Vector packedVec(ndofThisSet); //build vector same size as mat
+ int indx=0;
+ for (int j=0 ; j<(int)nodeMap.size() ; j++) {
+ const int d = nodeMap[j]->getDOF();
+ const int offs = nodeMap[j]->getUIndex();
+ packedVec(indx,d) = vec(offs,d);
+ indx += d;
+ }
+ // sherm 060222:
+ assert(indx == ndofThisSet);
+
+ return ~packedMat * packedVec;
+}
+
+// vec is a full vector in mobility space; packedVec consists of just
+// the mobilities used in this LengthSet.
+void
+LengthSet::subtractPackedVecFromVec(Vector& vec,
+ const Vector& packedVec)
+{
+ // sherm 060222:
+ assert(vec.size() == getRBTree().getTotalDOF());
+ assert(packedVec.size() == ndofThisSet);
+
+ int indx=0;
+ for (int j=0 ; j<(int)nodeMap.size() ; j++) {
+ const int d = nodeMap[j]->getDOF();
+ const int offs = nodeMap[j]->getUIndex();
+ vec(offs,d) -= packedVec(indx,d);
+ indx += d;
+ }
+
+ assert(indx == ndofThisSet);
+}
+
+void
+LengthSet::testProjectedVec(const State& s,
+ const Vector& vec) const
+{
+ // sherm 060222:
+ assert(vec.size() == getRBTree().getTotalDOF());
+
+ Vector packedVec(ndofThisSet);
+ int indx=0;
+ for (int j=0 ; j<(int)nodeMap.size() ; j++) {
+ const int d = nodeMap[j]->getDOF();
+ const int offs = nodeMap[j]->getUIndex();
+ packedVec(indx,d) = vec(offs,d);
+ indx += d;
+ }
+ assert(indx == ndofThisSet);
+
+ const Matrix grad = calcGrad(s);
+ const Vector test = ~grad * packedVec;
+
+ double testTol=1e-8;
+ for (int i=0 ; i<(int)loops.size() ; i++) {
+ if ( test(i) > testTol )
+ cout << "LengthSet::Gradient: constraint condition between atoms "
+ << loops[i].tips(1) << " and " << loops[i].tips(2) << " violated.\n"
+ << "\tnorm of violation: " << test(i) << '\n';
+ }
+ cout.flush();
+}
+
+void
+LengthConstraints::fixVel0(State& s, Vector& iVel)
+{
+ // use molecule grouping
+ for (int i=0 ; i<(int)accConstraints.size() ; i++)
+ accConstraints[i].fixVel0(s, iVel);
+}
+
+//
+// Correct internal velocities such that the constraint conditions are
+// met under the condition that the disturbance to station velocites is
+// small as possible.
+//
+void
+LengthSet::fixVel0(State& s, Vector& iVel)
+{
+ assert(iVel.size() == getRBTree().getTotalDOF());
+
+ const SBPositionCache& pc = getRBTree().getPositionCache(s);
+ const Vector& uErr = getRBTree().getUErr(s);
+
+ // store internal velocities
+ Vector iVel0 = iVel;
+
+ // Allocate the vector of body impulses so we don't have to do it
+ // inside the loop below. No need to initialize them here, though.
+ Vector_<SpatialVec> bodyImpulses(getRBTree().getNumBodies());
+
+ //TODO
+ // Currently we do not have any constraints involving mobilities (u's)
+ // directly, so we don't generate mobility impulses here. But we'll need
+ // a set of zero mobility forces to apply below.
+ Vector zeroMobilityImpulses(getRBTree().getTotalDOF());
+ zeroMobilityImpulses.setToZero();
+
+ // verr stores the current velocity errors, which we're assuming are valid.
+ Vector verr((int)loops.size());
+ for (int i=0; i<(int)loops.size(); ++i)
+ verr[i] = loops[i].rbDistCons->getVelErr(uErr);
+
+ Matrix mat(loops.size(),loops.size());
+ Array_<Vector> deltaIVel(loops.size());
+ for (int m=0 ; m<(int)loops.size() ; m++) {
+ deltaIVel[m].resize(iVel.size());
+
+ // Set all velocities to zero. TODO: this should just be an "ignore velocity"
+ // option to realizeVelocity(); it shouldn't actually require putting zeroes everywhere.
+ iVel = 0.;
+ getRBTree().setU(s, iVel );
+ getRBTree().realizeSubsystemVelocity(s);
+
+ // sherm: I think the following is a unit "probe" velocity, projected
+ // along the separation vector.
+ // That would explain the fact that there are no velocities here!
+ const Vec3 probeImpulse = loops[m].tipPos(pc,2)-loops[m].tipPos(pc,1);
+ const Vec3 force1 = -probeImpulse;
+ const Vec3 force2 = probeImpulse;
+
+ const RigidBodyNode& node1 = loops[m].tipNode(1);
+ const RigidBodyNode& node2 = loops[m].tipNode(2);
+ const Vec3 moment1 = cross(loops[m].tipPos(pc,1)-node1.getX_GB(pc).p(), force1);
+ const Vec3 moment2 = cross(loops[m].tipPos(pc,2)-node2.getX_GB(pc).p(), force2);
+
+ // Convert the probe impulses at the stations to spatial impulses at
+ // the body origin.
+ bodyImpulses.setToZero();
+ bodyImpulses[node1.getNodeNum()] += SpatialVec(moment1, force1);
+ bodyImpulses[node2.getNodeNum()] += SpatialVec(moment2, force2);
+
+ // Apply probe impulse as though it were a force; the resulting "acceleration"
+ // is actually the deltaV produced by this impulse, that is, a deltaV which
+ // results in a change in velocity along the line between the stations.
+
+ // calc deltaVa from k-th constraint condition
+ //FIX: make sure that nodeMap is ordered by level
+ // get all nodes in given molecule, ordered by level
+ getRBTree().realizeZ(s, zeroMobilityImpulses, bodyImpulses);
+ getRBTree().realizeTreeAccel(s);
+ deltaIVel[m] = getRBTree().getUDot(s);
+
+ // Set the new velocities.
+ getRBTree().setU(s, deltaIVel[m] );
+ getRBTree().realizeSubsystemVelocity(s);
+
+ // Calculating partial(velocityError[n])/partial(deltav[m]). Any velocity
+ // we see here is due to the deltav, since we started out at zero (uErr
+ // was modified by realizeVelocity(), but our reference is still valid).
+ for (int n=0; n<(int)loops.size(); ++n)
+ mat(n,m) = loops[n].rbDistCons->getVelErr(uErr);
+
+ //store results of m-th constraint on deltaVa-n
+ }
+
+ // Calculate the fraction of each deltaV by which we should change V to simultaneously
+ // drive all the velocity errors to zero.
+ // TODO: deal with a badly conditioned Jacobian to produce a least squares lambda.
+ const Vector lambda = mat.invert() * verr;
+
+ iVel = iVel0;
+ for (int m=0 ; m<(int)loops.size() ; m++)
+ iVel -= lambda[m] * deltaIVel[m];
+ getRBTree().setU(s, iVel);
+ getRBTree().realizeSubsystemVelocity(s);
+}
+
+
+std::ostream& operator<<(std::ostream& o, const RBStation& s) {
+ o << "station " << s.getPoint() << " on node " << s.getNode().getNodeNum();
+ return o;
+}
+
+
+std::ostream& operator<<(std::ostream& o, const RBDirection& d) {
+ o << "normal " << d.getUnitVec() << " on node " << d.getNode().getNodeNum();
+ return o;
+}
+
+ ////////////////////////////
+ // RB DISTANCE CONSTRAINT //
+ ////////////////////////////
+
+void RBDistanceConstraint::calcStationPosInfo(int i,
+ SBPositionCache& pc) const
+{
+ updStation_G(pc,i) = getNode(i).getX_GB(pc).R() * getPoint(i);
+ updPos_G(pc,i) = getNode(i).getX_GB(pc).p() + getStation_G(pc,i);
+}
+
+void RBDistanceConstraint::calcStationVelInfo(int i,
+ const SBPositionCache& pc,
+ SBVelocityCache& vc) const
+{
+ const Vec3& w_G = getNode(i).getSpatialAngVel(vc);
+ const Vec3& v_G = getNode(i).getSpatialLinVel(vc);
+ updStationVel_G(vc,i) = cross(w_G, getStation_G(pc,i));
+ updVel_G(vc,i) = v_G + getStationVel_G(vc,i);
+}
+
+void RBDistanceConstraint::calcStationAccInfo(int i,
+ const SBPositionCache& pc,
+ const SBVelocityCache& vc,
+ SBAccelerationCache& ac) const
+{
+ const Vec3& w_G = getNode(i).getSpatialAngVel(vc);
+ const Vec3& v_G = getNode(i).getSpatialLinVel(vc);
+ const Vec3& aa_G = getNode(i).getSpatialAngAcc(ac);
+ const Vec3& a_G = getNode(i).getSpatialLinAcc(ac);
+ updAcc_G(ac,i) = a_G + cross(aa_G, getStation_G(pc,i))
+ + cross(w_G, getStationVel_G(vc,i)); // i.e., w X (wXr)
+}
+
+
+void RBDistanceConstraint::calcPosInfo(Vector& qErr, SBPositionCache& pc) const
+{
+ assert(isValid() && distConstNum >= 0);
+ for (int i=1; i<=2; ++i) calcStationPosInfo(i,pc);
+
+ const Vec3 p = getPos_G(pc,2) - getPos_G(pc,1);
+ updFromTip1ToTip2_G(pc) = p;
+ const Real separation = getFromTip1ToTip2_G(pc).norm();
+ updUnitDirection_G(pc) = getFromTip1ToTip2_G(pc) / separation;
+ //TODO: |p|-d (should be 0.5(p^2-d^2)
+ //updPosErr(cc) = separation - distance;
+ updPosErr(qErr) = 0.5*(p.normSqr() - distance*distance);
+
+}
+
+void RBDistanceConstraint::calcVelInfo(
+ const SBPositionCache& pc,
+ Vector& uErr,
+ SBVelocityCache& vc) const
+{
+ assert(isValid() && distConstNum >= 0);
+ for (int i=1; i<=2; ++i) calcStationVelInfo(i,pc,vc);
+
+ updRelVel_G(vc) = getVel_G(vc,2) - getVel_G(vc,1);
+ //TODO: u.v, u=p/|p| (should be p.v)
+ //updVelErr(mc) = ~getUnitDirection_G(pc) * getRelVel_G(vc);
+ updVelErr(uErr) = ~getFromTip1ToTip2_G(pc) * getRelVel_G(vc);
+}
+
+void RBDistanceConstraint::calcAccInfo(
+ const SBPositionCache& pc,
+ const SBVelocityCache& vc,
+ Vector& udotErr,
+ SBAccelerationCache& ac) const
+{
+ assert(isValid() && distConstNum >= 0);
+ for (int i=1; i<=2; ++i) calcStationAccInfo(i,pc,vc,ac);
+
+ // TODO: v.v + a.p (good), but would have to be
+ // u.a + (v-(v.u).u).v/|p| to be compatible with above
+ const Vec3 relAcc_G = getAcc_G(ac,2) - getAcc_G(ac,1);
+ updAccErr(udotErr) = getRelVel_G(vc).normSqr() + (~relAcc_G * getFromTip1ToTip2_G(pc));
+}
+
+
+#endif
+
diff --git a/Simbody/src/OBSOLETE_LengthConstraints.h b/Simbody/src/OBSOLETE_LengthConstraints.h
new file mode 100644
index 0000000..09e0912
--- /dev/null
+++ b/Simbody/src/OBSOLETE_LengthConstraints.h
@@ -0,0 +1,464 @@
+#ifndef SimTK_SIMBODY_LENGTH_CONSTRAINTS_H_
+#define SimTK_SIMBODY_LENGTH_CONSTRAINTS_H_
+
+#ifdef NOTDEF
+/* -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "simbody/internal/common.h"
+using namespace SimTK;
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "SimbodyTreeState.h"
+
+#include "newtonRaphson.h"
+
+#include <iostream>
+using std::ostream;
+
+/**
+ * A station is a point located on a particular rigid body. A station is
+ * measured from the body frame origin and expressed in the body frame.
+ */
+class RBStation {
+public:
+ RBStation() : rbNode(0) { } // so we can have arrays of these
+ RBStation(const RigidBodyNode& n, const Vec3& pos)
+ : rbNode(&n), station_B(pos) { }
+ // default copy, assignment, destructor
+
+ const RigidBodyNode& getNode() const { assert(isValid()); return *rbNode; }
+ const Vec3& getPoint() const { assert(isValid()); return station_B; }
+ bool isValid() const { return rbNode != 0; }
+private:
+ const RigidBodyNode* rbNode;
+ Vec3 station_B;
+};
+std::ostream& operator<<(std::ostream&, const RBStation&);
+
+/**
+ * A direction is a unit vector fixed in a particular rigid body, typically
+ * one of its coordinate axes. The direction is expressed in the body frame.
+ */
+class RBDirection {
+public:
+ RBDirection() : rbNode(0) { } // so we can have arrays of these
+ RBDirection(const RigidBodyNode& n, const UnitVec3& dir)
+ : rbNode(&n), direction_B(dir) { }
+ // default copy, assignment, destructor
+
+ const RigidBodyNode& getNode() const { assert(isValid()); return *rbNode; }
+ const UnitVec3& getUnitVec() const { assert(isValid()); return direction_B; }
+ bool isValid() const { return rbNode != 0; }
+private:
+ const RigidBodyNode* rbNode;
+ UnitVec3 direction_B;
+};
+std::ostream& operator<<(std::ostream&, const RBDirection&);
+
+/**
+ * This class requests that two stations, one on each of two rigid bodies,
+ * be maintained at a certain separation distance at all times. This is
+ * an internal service provided by SimbodyMatterSubsystems and not something
+ * built directly by users. User-requested Constraints may allocate one
+ * or more distance constraints in the performance of their duties.
+ *
+ * Each distance constraint adds one constraint equation and is thus
+ * associated with a particular constraint multiplier and matching
+ * acceleration constraint error. However, not all multipliers come
+ * from distance constraints, so we store both the distance constraint
+ * number here and the multiplier index, for use in accessing the
+ * appropriate quantities in the state cache.
+ */
+class RBDistanceConstraint {
+public:
+ RBDistanceConstraint() : distance(-1.), distConstNum(-1),
+ qerrIndex(-1), uerrIndex(-1), multIndex(-1) {}
+ RBDistanceConstraint(const RBStation& s1, const RBStation& s2, const Real& d)
+ {
+ assert(s1.isValid() && s2.isValid() && d >= 0.);
+ stations[0] = s1; stations[1] = s2; distance = d;
+ distConstNum = qerrIndex = uerrIndex = multIndex = -1;
+ }
+
+ void calcPosInfo(
+ Vector& qErr,
+ SBPositionCache& pc) const;
+ void calcVelInfo(
+ const SBPositionCache& pc,
+ Vector& uErr,
+ SBVelocityCache& vc) const;
+ void calcAccInfo(
+ const SBPositionCache& pc,
+ const SBVelocityCache& vc,
+ Vector& udotErr,
+ SBAccelerationCache& ac) const;
+
+ void setDistanceConstraintNum(int ix) {assert(ix>=0); distConstNum=ix;}
+ int getDistanceConstraintNum() const {assert(isValid()&&distConstNum>=0); return distConstNum;}
+
+ void setQErrIndex(int ix) {assert(ix>=0); qerrIndex=ix;}
+ int getQErrIndex() const {assert(isValid()&&qerrIndex>=0); return qerrIndex;}
+ void setUErrIndex(int ix) {assert(ix>=0); uerrIndex=ix;}
+ int getUErrIndex() const {assert(isValid()&&uerrIndex>=0); return uerrIndex;}
+ void setMultIndex(int ix) {assert(ix>=0); multIndex=ix;}
+ int getMultIndex() const {assert(isValid()&&multIndex>=0); return multIndex;}
+
+ // Currently these are not parameterizable so they have no state references.
+ const Real& getDistance() const {return distance;}
+ const RBStation& getStation(int i) const {assert(isValid() && (i==1||i==2)); return stations[i-1];}
+ const RigidBodyNode& getNode(int i) const {return getStation(i).getNode();}
+ const Vec3& getPoint(int i) const {return getStation(i).getPoint();}
+ bool isValid() const {return distance >= 0.;}
+
+ // State access routines
+
+ // POSITION STAGE
+ const Real& getPosErr(const Vector& qErr) const {
+ assert(qerrIndex >= 0);
+ return qErr[qerrIndex];
+ }
+ Real& updPosErr(Vector& qErr) const {
+ assert(qerrIndex >= 0);
+ return qErr[qerrIndex];
+ }
+
+ const Vec3& getStation_G(const SBPositionCache& pc, int i) const {
+ assert(1 <= i && i <= 2);
+ return pc.station_G[i-1][distConstNum];
+ }
+ Vec3& updStation_G(SBPositionCache& pc, int i) const {
+ assert(1 <= i && i <= 2);
+ return pc.station_G[i-1][distConstNum];
+ }
+
+ const Vec3& getPos_G(const SBPositionCache& pc, int i) const {
+ assert(1 <= i && i <= 2);
+ return pc.pos_G[i-1][distConstNum];
+ }
+ Vec3& updPos_G(SBPositionCache& pc, int i) const {
+ assert(1 <= i && i <= 2);
+ return pc.pos_G[i-1][distConstNum];
+ }
+
+ const Vec3& getFromTip1ToTip2_G(const SBPositionCache& pc) const {
+ return pc.fromTip1ToTip2_G[distConstNum];
+ }
+ Vec3& updFromTip1ToTip2_G(SBPositionCache& pc) const {
+ return pc.fromTip1ToTip2_G[distConstNum];
+ }
+
+ const Vec3& getUnitDirection_G(const SBPositionCache& pc) const {
+ return pc.unitDirection_G[distConstNum];
+ }
+ Vec3& updUnitDirection_G(SBPositionCache& pc) const {
+ return pc.unitDirection_G[distConstNum];
+ }
+
+ // VELOCITY STAGE
+ const Real& getVelErr(const Vector& uErr) const {
+ return uErr[uerrIndex];
+ }
+ Real& updVelErr(Vector& uErr) const {
+ return uErr[uerrIndex];
+ }
+
+ const Vec3& getStationVel_G(const SBVelocityCache& vc, int i) const {
+ assert(1 <= i && i <= 2);
+ return vc.stationVel_G[i-1][distConstNum];
+ }
+ Vec3& updStationVel_G(SBVelocityCache& vc, int i) const {
+ assert(1 <= i && i <= 2);
+ return vc.stationVel_G[i-1][distConstNum];
+ }
+
+ const Vec3& getVel_G(const SBVelocityCache& vc, int i) const {
+ assert(1 <= i && i <= 2);
+ return vc.vel_G[i-1][distConstNum];
+ }
+ Vec3& updVel_G(SBVelocityCache& vc, int i) const {
+ assert(1 <= i && i <= 2);
+ return vc.vel_G[i-1][distConstNum];
+ }
+
+ const Vec3& getRelVel_G(const SBVelocityCache& vc) const {
+ return vc.relVel_G[distConstNum];
+ }
+ Vec3& updRelVel_G(SBVelocityCache& vc) const {
+ return vc.relVel_G[distConstNum];
+ }
+
+
+ // ACCELERATION STAGE
+ const Real& getAccErr(const Vector& udotErr) const {
+ return udotErr[multIndex];
+ }
+ Real& updAccErr(Vector& udotErr) const {
+ return udotErr[multIndex];
+ }
+
+ const Vec3& getAcc_G(const SBAccelerationCache& ac, int i) const {
+ assert(1 <= i && i <= 2);
+ return ac.acc_G[i-1][distConstNum];
+ }
+ Vec3& updAcc_G(SBAccelerationCache& ac, int i) const {
+ assert(1 <= i && i <= 2);
+ return ac.acc_G[i-1][distConstNum];
+ }
+
+ const Vec3& getForce_G(const SBAccelerationCache& ac, int i) const {
+ assert(1 <= i && i <= 2);
+ return ac.force_G[i-1][distConstNum];
+ }
+ Vec3& updForce_G(SBAccelerationCache& ac, int i) const {
+ assert(1 <= i && i <= 2);
+ return ac.force_G[i-1][distConstNum];
+ }
+
+protected:
+ Real distance;
+ RBStation stations[2];
+ int distConstNum;
+ int qerrIndex, uerrIndex, multIndex;
+
+private:
+ // Per-station calculations
+ void calcStationPosInfo(int i,
+ SBPositionCache& pc) const;
+ void calcStationVelInfo(int i,
+ const SBPositionCache& pc,
+ SBVelocityCache& vc) const;
+ void calcStationAccInfo(int i,
+ const SBPositionCache& pc,
+ const SBVelocityCache& vc,
+ SBAccelerationCache& ac) const;
+};
+
+class SimbodyMatterSubsystemRep;
+class LengthConstraints;
+class LengthSet;
+
+/*
+ * Collect up useful information about a loop.
+ * This includes the two connected stations, ordered by level, and the
+ * paths from each of the associated nodes back to the common ancestor.
+ * We also identify the molecule base node for the molecule which
+ * contains both ends of the loop.
+ * We will throw an exception if the loop ends are both on the same
+ * node or if they are on different molecules.
+ */
+class LoopWNodes {
+public:
+ LoopWNodes()
+ : tree(0), rbDistCons(0), flipStations(false), outmostCommonBody(0)
+ {
+ }
+ LoopWNodes(const SimbodyMatterSubsystemRep&, const RBDistanceConstraint&);
+
+ void calcPosInfo(
+ Vector& qErr,
+ SBPositionCache& pc) const
+ { rbDistCons->calcPosInfo(qErr,pc); }
+
+ void calcVelInfo(
+ const SBPositionCache& pc,
+ Vector& uErr,
+ SBVelocityCache& vc) const
+ { rbDistCons->calcVelInfo(pc,uErr,vc); }
+
+ void calcAccInfo(
+ const SBPositionCache& pc,
+ const SBVelocityCache& vc,
+ Vector& udotErr,
+ SBAccelerationCache& ac) const
+ { rbDistCons->calcAccInfo(pc,vc,udotErr,ac); }
+
+ // TODO: State isn't currently in use but will be necessary when the distances
+ // are parameters.
+ const Real& getDistance() const { return rbDistCons->getDistance(); }
+
+ // Return one of the stations, ordered such that tips(1).level <= tips(2).level.
+ const RBStation& tips(int i) const {return rbDistCons->getStation(ix(i));}
+ const RigidBodyNode& tipNode(int i) const {return tips(i).getNode();}
+
+ const Vec3& tipPos(const SBPositionCache& pc, int i) const {
+ assert(1 <= i && i <= 2);
+ const int dc = rbDistCons->getDistanceConstraintNum();
+ return pc.pos_G[ix(i)-1][dc];
+ }
+ const Vec3& tipVel(const SBVelocityCache& vc, int i) const {
+ assert(1 <= i && i <= 2);
+ const int dc = rbDistCons->getDistanceConstraintNum();
+ return vc.vel_G[ix(i)-1][dc];
+ }
+ const Vec3& tipAcc(const SBAccelerationCache& ac, int i) const {
+ assert(1 <= i && i <= 2);
+ const int dc = rbDistCons->getDistanceConstraintNum();
+ return ac.acc_G[ix(i)-1][dc];
+ }
+ const Vec3& tipForce(const SBAccelerationCache& ac, int i) const {
+ assert(1 <= i && i <= 2);
+ const int dc = rbDistCons->getDistanceConstraintNum();
+ return ac.force_G[ix(i)-1][dc];
+ }
+
+ // Use this for both forces and impulses.
+ void setTipForce(SBAccelerationCache& ac, int i, const Vec3& f) const {
+ assert(1 <= i && i <= 2);
+
+ const int dc = rbDistCons->getDistanceConstraintNum();
+ ac.force_G[ix(i)-1][dc] = f;
+ }
+
+ const RigidBodyNode* getOutmostCommonBody() const {return outmostCommonBody;}
+
+private:
+ int ix(int i) const { assert(i==1||i==2); return flipStations ? 3-i : i; }
+
+ const SimbodyMatterSubsystemRep* tree; // a reference to the tree we're part of
+ const RBDistanceConstraint* rbDistCons; // reference to the constraint
+
+ // calculated construction-time (topological) info about the constraint
+ bool flipStations; // make sure station(1).level
+ // <= station(2).level
+ Array_<const RigidBodyNode*> nodes[2]; // the two paths: base..tip1, base..tip2,
+ // incl. tip nodes but not base
+ const RigidBodyNode* outmostCommonBody; // highest-level common ancestor of tips
+
+ // Ancestors includes everything from outmostCommonBody (inclusive) down to ground
+ // (exclusive).
+ Array_<const RigidBodyNode*> ancestors;
+
+ friend class LengthSet;
+ friend class LengthConstraints;
+ friend ostream& operator<<(ostream&, const LengthSet&);
+ friend ostream& operator<<(ostream&, const LoopWNodes&);
+};
+
+typedef Array_<LoopWNodes> LoopList;
+
+class LengthSet {
+ static void construct(const LoopList& loops);
+ const LengthConstraints* lConstraints;
+ LoopList loops;
+ int ndofThisSet;
+ Array_<const RigidBodyNode*> nodeMap; //unique nodes (union of loops->nodes)
+public:
+ LengthSet() : lConstraints(0), ndofThisSet(0) { }
+ LengthSet(const LengthConstraints* lConstraints)
+ : lConstraints(lConstraints), ndofThisSet(0)
+ {
+ }
+
+ inline const SimbodyMatterSubsystemRep& getRBTree() const;
+ inline int getVerbose() const;
+
+ void addKinematicConstraint(const LoopWNodes& loop);
+ void addDynamicConstraint(const LoopWNodes& loop);
+ bool contains(const RigidBodyNode* node);
+
+ void setPos(State&, const Vector& pos) const;
+ void setVel(State&, const Vector& vel) const;
+ Vector getPos();
+ Vector calcPosB(State&, const Vector& pos) const;
+ Vector calcVelB(State&, const Vector& vel) const;
+ Vector calcPosZ(const State&, const Vector& b) const;
+ Matrix calcGrad(const State&) const;
+ static Matrix calcPseudoInverseA(const Matrix& AT);
+ Matrix calcPseudoInverseAFD(const State&) const;
+
+ void calcConstraintForces(const State&, const Vector& udotErr,
+ Vector& multipliers, SBAccelerationCache&) const;
+ void addInCorrectionForces(const State&, const SBAccelerationCache&,
+ SpatialVecList& spatialForces) const; // spatialForces+=correction
+
+ void fixVel0(State&, Vector&);
+
+ void projectQVecOntoConfigurationConstraints(const State&, Vector& q);
+ void projectUVecOntoMotionConstraints(const State&, Vector& u);
+
+ Vector packedMatTransposeTimesVec(const Matrix&, const Vector&);
+ void subtractPackedVecFromVec(Vector& vec, const Vector& packedVec);
+
+ void testAccel(const State&) const;
+ void testProjectedVec(const State&, const Vector&) const;
+ friend ostream& operator<<(ostream& os, const LengthSet& s);
+ friend class CalcPosB;
+ friend class CalcVelB;
+ friend class CalcPosZ;
+ friend class CalcVelZ;
+
+ void fdgradf(State&, const Vector& pos, Matrix& grad) const;
+ void testGrad(State&, const Vector& pos, const Matrix& grad) const;
+};
+
+class LengthConstraints {
+public:
+ LengthConstraints(const SimbodyMatterSubsystemRep&, int verbose);
+
+ void construct(const Array_<RBDistanceConstraint*>&);
+
+ // Returns true if any change was made in the state.
+ bool enforcePositionConstraints(State&, const Real& requiredTol, const Real& desiredTol) const;
+ bool enforceVelocityConstraints(State&, const Real& requiredTol, const Real& desiredTol) const;
+
+ // After constraints have been enforced, call these to project out the components
+ // of the passed-in q- and u-basis vectors which are normal to the constraint manifold.
+ // These project in the Euclidean norm; if you want to project in the error norm
+ // you have to scale q and u on the way in and out yourself.
+ void projectQVecOntoConfigurationConstraints(const State&, Vector& q);
+ void projectUVecOntoMotionConstraints(const State&, Vector& u);
+
+ bool calcConstraintForces(const State&, const Vector& udotErr,
+ Vector& multipliers, SBAccelerationCache&) const;
+ void addInCorrectionForces(const State&, const SBAccelerationCache&,
+ SpatialVecList& spatialForces) const;
+
+ void fixVel0(State&, Vector&);
+
+
+private:
+ int maxIters;
+ int maxMin;
+
+ const SimbodyMatterSubsystemRep& rbTree;
+ const int verbose;
+
+ Array_<LengthSet> pvConstraints; // used for pos, vel
+ Array_<LengthSet> accConstraints; // used for acc
+ NewtonRaphson posMin, velMin;
+
+ friend class LengthSet;
+};
+
+inline const SimbodyMatterSubsystemRep&
+LengthSet::getRBTree() const {return lConstraints->rbTree;}
+inline int
+LengthSet::getVerbose() const {return lConstraints->verbose;}
+
+#endif
+
+
+#endif // SimTK_SIMBODY_LENGTH_CONSTRAINTS_H_
+
+
diff --git a/Simbody/src/OBSOLETE_NewtonRaphson.h b/Simbody/src/OBSOLETE_NewtonRaphson.h
new file mode 100644
index 0000000..2aa21ef
--- /dev/null
+++ b/Simbody/src/OBSOLETE_NewtonRaphson.h
@@ -0,0 +1,147 @@
+#ifndef SimTK_SIMBODY_NEWTON_RAPHSON_H_
+#define SimTK_SIMBODY_NEWTON_RAPHSON_H_
+
+#ifdef NOTDEF
+/* -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Derived from IVM code written by Charles Schwieters *
+ * *
+ * 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 "simbody/internal/common.h"
+
+#include <iostream>
+
+class NewtonRaphson {
+public:
+ int maxMin;
+ int maxIters;
+ bool verbose;
+ Real zTol; // minimization will quit if gradient norm is smaller
+ std::ostream& errStream;
+ String name;
+
+ NewtonRaphson(const String& id, std::ostream& errStream=std::cerr)
+ : maxMin( 20 ) , maxIters( 20 ), verbose(false),
+ zTol( 1e-8 ), errStream(errStream), name(id)
+ { }
+
+ // This will attempt to modify x to drive error b to below desiredTol, but will be
+ // happy if it achieves only requiredTol (>= desiredTol). That is, as long as were
+ // having an easy time of it we'll continue down to desiredTol, but we won't do
+ // anything desperate once we're better than requiredTol.
+ template<class CalcB,class CalcZ,class VecType>
+ void calc(const Real& requiredTol, const Real& desiredTol,
+ VecType& x, CalcB calcB, CalcZ calcZ) const
+ {
+ assert(requiredTol >= desiredTol);
+ if ( verbose )
+ errStream << "NewtonRaphson '" << name << "': start. Req="
+ << requiredTol << " desired=" << desiredTol << "\n";
+ VecType b = calcB(x);
+ Real norm = std::sqrt(b.normSqr() / b.size());
+ const Real zTolz = zTol*x.size();
+ const Real zTol2 = zTolz*zTolz;
+ Real onorm=norm;
+ int iters=0;
+ if ( verbose )
+ errStream << "NewtonRaphson '" << name << "': iter: "
+ << iters << " norm: " << norm << '\n';
+ bool finished=(norm < desiredTol);
+ while (!finished) {
+ VecType ox = x;
+ //std::cout << "NR: vars=" << x << std::endl;
+ //std::cout << "NR: errs=" << b << std::endl;
+
+ VecType z = calcZ(b);
+ x -= z;
+
+ iters++;
+
+ b = calcB(x);
+ norm = std::sqrt(b.normSqr() / b.size());
+
+ // If that made the norm worse, we're going to need to back off and feel
+ // our way around slowly here. But we won't bother if we've already met
+ // requiredTol.
+ if (norm > onorm) {
+ if (onorm <= requiredTol) {
+ if (verbose) {
+ errStream << "NewtonRaphson '" << name
+ << "': got required but not desired, giving up rather than gradient search\n";
+ }
+ x = ox; norm = onorm; // back up to last good
+ finished = true;
+ continue;
+ }
+
+ if (verbose)
+ errStream << "NewtonRaphson '" << name << "': newton-Raphson failed."
+ << " Trying gradient search.\n";
+
+ // Now we'll try a gradient search using up to maxMin iterations, halving
+ // the step along the gradient each time. We'll stop at the first norm
+ // which is an improvement over onorm.
+
+ int mincnt = maxMin;
+ do {
+ if ( mincnt < 1 )
+ SimTK_THROW1(Exception::NewtonRaphsonFailure,
+ "Too many gradient search steps taken");
+ z *= 0.5;
+ if ( z.normSqr() < zTol2 ) // TODO should be weighted norm of z
+ SimTK_THROW1(Exception::NewtonRaphsonFailure, "step along gradient too small");
+ x = ox - z;
+ b = calcB(x);
+ norm = std::sqrt(b.normSqr() / b.size());
+ if ( verbose )
+ errStream << "NewtonRaphson '" << name << "': gradient search in iter: "
+ << iters << " norm: " << norm << '\n';
+ mincnt--;
+ } while (norm >= onorm);
+ }
+
+ // At this point we know that norm is better than onorm.
+
+ onorm = norm;
+
+ if ( verbose )
+ errStream << "NewtonRaphson '" << name << "': iter: "
+ << iters << " norm: " << norm << '\n';
+
+ if (norm <= desiredTol)
+ finished = true;
+ else if (iters >= maxIters) {
+ if (norm > requiredTol)
+ SimTK_THROW1(Exception::NewtonRaphsonFailure, "maxIters exceeded");
+ if (verbose) {
+ errStream << "NewtonRaphson '" << name
+ << "': got required but not desired, giving up after iter "
+ << iters << "\n";
+ }
+ finished = true;
+ }
+ }
+ }
+};
+
+#endif
+
+#endif // SimTK_SIMBODY_NEWTON_RAPHSON_H_
diff --git a/Simbody/src/ObservedPointFitter.cpp b/Simbody/src/ObservedPointFitter.cpp
new file mode 100644
index 0000000..5adf863
--- /dev/null
+++ b/Simbody/src/ObservedPointFitter.cpp
@@ -0,0 +1,377 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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/MobilizedBody.h"
+#include "simbody/internal/MobilizedBody_Ground.h"
+#include "simbody/internal/MobilizedBody_Free.h"
+#include "simbody/internal/MultibodySystem.h"
+#include "simbody/internal/ObservedPointFitter.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include <map>
+
+using namespace SimTK;
+
+/**
+ * This class defines the objective function which is passed to the Optimizer.
+ */
+
+static const bool UseWeighted = true;
+static const Real MinimumShift = 1; // add to objective to get minimum away from zero
+
+class ObservedPointFitter::OptimizerFunction : public OptimizerSystem {
+public:
+ OptimizerFunction(const MultibodySystem& system, const State& state, Array_<MobilizedBodyIndex> bodyIxs, Array_<Array_<Vec3> > stations, Array_<Array_<Vec3> > targetLocations, Array_<Array_<Real> > weights) :
+ OptimizerSystem(state.getNQ()), system(system), state(state), bodyIxs(bodyIxs), stations(stations), targetLocations(targetLocations), weights(weights) {
+ system.realize(state, Stage::Instance);
+ setNumEqualityConstraints(state.getNQErr());
+ }
+ int objectiveFunc(const Vector& parameters, bool new_parameters, Real& f) const {
+ if (new_parameters)
+ state.updQ() = parameters;
+ system.realize(state, Stage::Position);
+ Real wtot = 0;
+ f = 0;
+ for (int i = 0; i < (int)bodyIxs.size(); ++i) {
+ const MobilizedBodyIndex id = bodyIxs[i];
+ const MobilizedBody& body = system.getMatterSubsystem().getMobilizedBody(id);
+ for (int j = 0; j < (int)stations[i].size(); ++j) {
+ f += weights[i][j]*(targetLocations[i][j]-body.getBodyTransform(state)*stations[i][j]).normSqr();
+ wtot += weights[i][j];
+ }
+ }
+ if (UseWeighted && wtot > 0) f /= wtot;
+
+ f += MinimumShift; // so minimum won't be at zero where scaling is tricky
+ return 0;
+ }
+ int gradientFunc(const Vector ¶meters, bool new_parameters, Vector &gradient) const {
+ if (new_parameters)
+ state.updQ() = parameters;
+ system.realize(state, Stage::Position);
+ const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
+ Vector_<SpatialVec> dEdR(matter.getNumBodies());
+ dEdR = SpatialVec(Vec3(0), Vec3(0));
+ Real wtot = 0;
+ for (int i = 0; i < (int)bodyIxs.size(); ++i) {
+ const MobilizedBodyIndex id = bodyIxs[i];
+ const MobilizedBody& body = matter.getMobilizedBody(id);
+ for (int j = 0; j < (int)stations[i].size(); ++j) {
+ Vec3 f = 2*weights[i][j]*(body.getBodyTransform(state)*stations[i][j]-targetLocations[i][j]);
+ body.applyForceToBodyPoint(state, stations[i][j], f, dEdR);
+ wtot += weights[i][j];
+ }
+ }
+ Vector dEdU;
+ // Convert spatial forces dEdR to generalized forces dEdU.
+ matter.multiplyBySystemJacobianTranspose(state, dEdR, dEdU);
+ if (UseWeighted && wtot > 0) dEdU /= wtot;
+ matter.multiplyByNInv(state, true, dEdU, gradient);
+ return 0;
+ }
+ int constraintFunc(const Vector& parameters, bool new_parameters, Vector& constraints) const {
+ state.updQ() = parameters;
+ system.realize(state, Stage::Velocity);
+ constraints = state.getQErr();
+ return 0;
+ }
+ void optimize(Vector& q, Real tolerance) {
+ Optimizer opt(*this
+ //, LBFGSB // XXX
+ //, InteriorPoint // XXX
+ );
+ //opt.useNumericalGradient(true); //XXX
+ opt.useNumericalJacobian(true);
+ opt.setConvergenceTolerance(tolerance);
+ opt.setMaxIterations(3000);
+ opt.setLimitedMemoryHistory(40);
+ //opt.setDiagnosticsLevel(5);
+ opt.optimize(q);
+ }
+private:
+ const MultibodySystem& system;
+ const Array_<MobilizedBodyIndex> bodyIxs;
+ const Array_<Array_<Vec3> > stations;
+ const Array_<Array_<Vec3> > targetLocations;
+ const Array_<Array_<Real> > weights;
+ mutable State state;
+};
+
+/**
+ * Create a new MultibodySystem which is identical to a subset of the original MultibodySystem. This is called once for each MobilizedBody
+ * in the original system, and is used to find an initial estimate of that MobilizedBody's conformation.
+ */
+
+void ObservedPointFitter::
+createClonedSystem(const MultibodySystem& original, MultibodySystem& copy,
+ const Array_<MobilizedBodyIndex>& originalBodyIxs,
+ Array_<MobilizedBodyIndex>& copyBodyIxs,
+ bool& hasArtificialBaseBody)
+{
+ const SimbodyMatterSubsystem& originalMatter = original.getMatterSubsystem();
+ SimbodyMatterSubsystem copyMatter(copy);
+ Body::Rigid body = Body::Rigid(MassProperties(1, Vec3(0), Inertia(1)));
+ body.addDecoration(Transform(), DecorativeSphere(Real(.1)));
+ std::map<MobilizedBodyIndex, MobilizedBodyIndex> idMap;
+ hasArtificialBaseBody = false;
+ for (int i = 0; i < (int)originalBodyIxs.size(); ++i) {
+ const MobilizedBody& originalBody = originalMatter.getMobilizedBody(originalBodyIxs[i]);
+ MobilizedBody* copyBody;
+ if (i == 0) {
+ if (originalBody.isGround())
+ copyBody = ©Matter.Ground();
+ else {
+ hasArtificialBaseBody = true; // not using the original joint here
+ MobilizedBody::Free free(copyMatter.Ground(), body);
+ copyBody = ©Matter.updMobilizedBody(free.getMobilizedBodyIndex());
+ }
+ }
+ else {
+ MobilizedBody& parent = copyMatter.updMobilizedBody(idMap[originalBody.getParentMobilizedBody().getMobilizedBodyIndex()]);
+ copyBody = &originalBody.cloneForNewParent(parent);
+ }
+ copyBodyIxs.push_back(copyBody->getMobilizedBodyIndex());
+ idMap[originalBodyIxs[i]] = copyBody->getMobilizedBodyIndex();
+ }
+ copy.realizeTopology();
+ State& s = copy.updDefaultState();
+ copyMatter.setUseEulerAngles(s, true);
+ copy.realizeModel(s);
+}
+
+/**
+ * This is invoked by findBodiesForClonedSystem(). It traces the tree upstream, adding bodies until there are sufficient stations
+ * to reasonably perform a fit.
+ */
+
+void ObservedPointFitter::findUpstreamBodies(MobilizedBodyIndex currentBodyIx, const Array_<int> numStations, const SimbodyMatterSubsystem& matter, Array_<MobilizedBodyIndex>& bodyIxs, int requiredStations) {
+ const MobilizedBody& currentBody = matter.getMobilizedBody(currentBodyIx);
+ if (currentBody.isGround())
+ return;
+ MobilizedBodyIndex parentIx = currentBody.getParentMobilizedBody().getMobilizedBodyIndex();
+ requiredStations -= numStations[parentIx];
+ if (requiredStations > 0)
+ findUpstreamBodies(parentIx, numStations, matter, bodyIxs, requiredStations);
+ bodyIxs.push_back(parentIx);
+}
+
+/**
+ * This is invoked by findBodiesForClonedSystem(). It traces the tree downstream, adding bodies until there are sufficient stations
+ * to reasonably perform a fit.
+ */
+
+void ObservedPointFitter::findDownstreamBodies(MobilizedBodyIndex currentBodyIx, const Array_<int> numStations, const Array_<Array_<MobilizedBodyIndex> > children, Array_<MobilizedBodyIndex>& bodyIxs, int& requiredStations) {
+ if (numStations[currentBodyIx] == 0 && children[currentBodyIx].empty())
+ return; // There's no benefit from including this body.
+ bodyIxs.push_back(currentBodyIx);
+ requiredStations -= numStations[currentBodyIx];
+ for (int i = 0; i < (int)children[currentBodyIx].size() && requiredStations > 0; ++i) {
+ findDownstreamBodies(children[currentBodyIx][i], numStations, children, bodyIxs, requiredStations);
+ }
+}
+
+/**
+ * Find the set of bodies that will be included in the system to be created by createClonedSystem(). The goal is to have sufficient
+ * stations both upstream and downstream of the MobilizedBody currently being analyzed.
+ */
+
+int ObservedPointFitter::findBodiesForClonedSystem(MobilizedBodyIndex primaryBodyIx, const Array_<int> numStations, const SimbodyMatterSubsystem& matter, const Array_<Array_<MobilizedBodyIndex> > children, Array_<MobilizedBodyIndex>& bodyIxs) {
+ findUpstreamBodies(primaryBodyIx, numStations, matter, bodyIxs, 5);
+ int primaryBodyIndex = bodyIxs.size();
+ int requiredStations = 5;
+ findDownstreamBodies(primaryBodyIx, numStations, children, bodyIxs, requiredStations);
+ return primaryBodyIndex;
+}
+
+Real ObservedPointFitter::findBestFit
+ (const MultibodySystem& system, State& state,
+ const Array_<MobilizedBodyIndex>& bodyIxs,
+ const Array_<Array_<Vec3> >& stations,
+ const Array_<Array_<Vec3> >& targetLocations,
+ Real tolerance)
+{
+ Array_<Array_<Real> > weights(stations.size());
+ for (int i = 0; i < (int)stations.size(); ++i)
+ for (int j = 0; j < (int)stations[i].size(); ++j)
+ weights[i].push_back(1.0);
+ return findBestFit(system, state, bodyIxs, stations, targetLocations, weights, tolerance);
+}
+
+Real ObservedPointFitter::findBestFit
+ (const MultibodySystem& system, State& state,
+ const Array_<MobilizedBodyIndex>& bodyIxs,
+ const Array_<Array_<Vec3> >& stations,
+ const Array_<Array_<Vec3> >& targetLocations,
+ const Array_<Array_<Real> >& weights,
+ Real tolerance)
+{
+ // Verify the inputs.
+
+ const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
+ SimTK_APIARGCHECK(bodyIxs.size() == stations.size() && stations.size() == targetLocations.size(), "ObservedPointFitter", "findBestFit", "bodyIxs, stations, and targetLocations must all be the same length");
+ int numBodies = matter.getNumBodies();
+ for (int i = 0; i < (int)stations.size(); ++i) {
+ SimTK_APIARGCHECK(bodyIxs[i] >= 0 && bodyIxs[i] < numBodies, "ObservedPointFitter", "findBestFit", "Illegal body ID");
+ SimTK_APIARGCHECK(stations[i].size() == targetLocations[i].size(), "ObservedPointFitter", "findBestFit", "Different number of stations and target locations for body");
+ }
+
+ // Build a list of children for each body.
+
+ Array_<Array_<MobilizedBodyIndex> > children(matter.getNumBodies());
+ for (int i = 0; i < matter.getNumBodies(); ++i) {
+ const MobilizedBody& body = matter.getMobilizedBody(MobilizedBodyIndex(i));
+ if (!body.isGround())
+ children[body.getParentMobilizedBody().getMobilizedBodyIndex()].push_back(body.getMobilizedBodyIndex());
+ }
+
+ // Build a mapping of body IDs to indices.
+
+ Array_<int> bodyIndex(matter.getNumBodies());
+ for (int i = 0; i < (int) bodyIndex.size(); ++i)
+ bodyIndex[i] = -1;
+ for (int i = 0; i < (int)bodyIxs.size(); ++i)
+ bodyIndex[bodyIxs[i]] = i;
+
+ // Find the number of stations on each body with a nonzero weight.
+
+ Array_<int> numStations(matter.getNumBodies());
+ for (int i = 0; i < (int) numStations.size(); ++i)
+ numStations[i] = 0;
+ for (int i = 0; i < (int)weights.size(); ++i) {
+ for (int j = 0; j < (int)weights[i].size(); ++j)
+ if (weights[i][j] != 0)
+ numStations[bodyIxs[i]]++;
+ }
+
+ // Perform the initial estimation of Q for each mobilizer.
+ // Our first guess is the passed-in q's, with quaternions converted
+ // to Euler angles if necessary. As we solve a subproblem for each
+ // of the bodies in ascending order, we'll update tempState's q's
+ // for that body to their solved values.
+ State tempState;
+ if (!matter.getUseEulerAngles(state))
+ matter.convertToEulerAngles(state, tempState);
+ else tempState = state;
+ system.realizeModel(tempState);
+ system.realize(tempState, Stage::Position);
+
+ // This will accumulate best-guess spatial poses for the bodies as
+ // they are processed. This is useful for when a body is used as
+ // an artificial base body; our first guess will to be to place it
+ // wherever it was the last time it was used in a subproblem.
+ Array_<Transform> guessX_GB(matter.getNumBodies());
+ for (MobilizedBodyIndex mbx(1); mbx < guessX_GB.size(); ++mbx)
+ guessX_GB[mbx] = matter.getMobilizedBody(mbx).getBodyTransform(tempState);
+
+ for (int i = 0; i < matter.getNumBodies(); ++i) {
+ MobilizedBodyIndex id(i);
+ const MobilizedBody& body = matter.getMobilizedBody(id);
+ if (body.getNumQ(tempState) == 0)
+ continue; // No degrees of freedom to determine.
+ if (children[id].size() == 0 && numStations[id] == 0)
+ continue; // There are no stations whose positions are affected by this.
+ Array_<MobilizedBodyIndex> originalBodyIxs;
+ int currentBodyIndex = findBodiesForClonedSystem(body.getMobilizedBodyIndex(), numStations, matter, children, originalBodyIxs);
+ if (currentBodyIndex == (int)originalBodyIxs.size()-1
+ && (bodyIndex[id] == -1 || stations[bodyIndex[id]].size() == 0))
+ continue; // There are no stations whose positions are affected by this.
+ MultibodySystem copy;
+ Array_<MobilizedBodyIndex> copyBodyIxs;
+ bool hasArtificialBaseBody;
+ createClonedSystem(system, copy, originalBodyIxs, copyBodyIxs, hasArtificialBaseBody);
+ const SimbodyMatterSubsystem& copyMatter = copy.getMatterSubsystem();
+ // Construct an initial state.
+ State copyState = copy.getDefaultState();
+ assert(copyBodyIxs.size() == originalBodyIxs.size());
+ for (int ob=0; ob < (int)originalBodyIxs.size(); ++ob) {
+ const MobilizedBody& copyMobod = copyMatter.getMobilizedBody(copyBodyIxs[ob]);
+ const MobilizedBody& origMobod = matter.getMobilizedBody(originalBodyIxs[ob]);
+ if (ob==0 && hasArtificialBaseBody)
+ copyMobod.setQToFitTransform(copyState, guessX_GB[origMobod.getMobilizedBodyIndex()]);
+ else
+ copyMobod.setQFromVector(copyState, origMobod.getQAsVector(tempState));
+ }
+
+ Array_<Array_<Vec3> > copyStations(copyMatter.getNumBodies());
+ Array_<Array_<Vec3> > copyTargetLocations(copyMatter.getNumBodies());
+ Array_<Array_<Real> > copyWeights(copyMatter.getNumBodies());
+ for (int j = 0; j < (int)originalBodyIxs.size(); ++j) {
+ int index = bodyIndex[originalBodyIxs[j]];
+ if (index != -1) {
+ copyStations[copyBodyIxs[j]] = stations[index];
+ copyTargetLocations[copyBodyIxs[j]] = targetLocations[index];
+ copyWeights[copyBodyIxs[j]] = weights[index];
+ }
+ }
+ try {
+ OptimizerFunction optimizer(copy, copyState, copyBodyIxs, copyStations, copyTargetLocations, copyWeights);
+ Vector q(copyState.getQ());
+ //std::cout << "BODY " << i << " q0=" << q << std::endl;
+ optimizer.optimize(q, tolerance);
+ //std::cout << " qf=" << q << std::endl;
+ copyState.updQ() = q;
+ copy.realize(copyState, Stage::Position);
+ // Transfer updated state back to tempState as improved initial guesses.
+ // However, all but the currentBody will get overwritten later.
+ for (int ob=0; ob < (int)originalBodyIxs.size(); ++ob) {
+ const MobilizedBody& copyMobod = copyMatter.getMobilizedBody(copyBodyIxs[ob]);
+ guessX_GB[originalBodyIxs[ob]] = copyMobod.getBodyTransform(copyState);
+
+ if (ob==0 && hasArtificialBaseBody) continue; // leave default state
+ const MobilizedBody& origMobod = matter.getMobilizedBody(originalBodyIxs[ob]);
+ origMobod.setQFromVector(tempState, copyMobod.getQAsVector(copyState));
+ }
+ //body.setQFromVector(tempState, copyMatter.getMobilizedBody(copyBodyIxs[currentBodyIndex]).getQAsVector(copyState));
+ }
+ catch (Exception::OptimizerFailed ex) {
+ std::cout << "Optimization failure for body "<<i<<": "<<ex.getMessage() << std::endl;
+ // Just leave this body's state variables set to 0, and rely on the final optimization to fix them.
+ }
+ }
+
+ // Now do the final optimization of the whole system.
+
+ OptimizerFunction optimizer(system, tempState, bodyIxs, stations, targetLocations, weights);
+ Vector q = tempState.getQ();
+ optimizer.optimize(q, tolerance);
+ if (matter.getUseEulerAngles(state))
+ state.updQ() = q;
+ else {
+ tempState.updQ() = q;
+ matter.convertToQuaternions(tempState, state);
+ }
+
+ // Return the RMS error in the optimized system.
+
+ Real error;
+ optimizer.objectiveFunc(q, true, error);
+ if (UseWeighted)
+ return std::sqrt(error - MinimumShift); // already weighted; this makes WRMS
+
+ Real totalWeight = 0;
+ for (int i = 0; i < (int)weights.size(); ++i)
+ for (int j = 0; j < (int)weights[i].size(); ++j)
+ totalWeight += weights[i][j];
+
+ return std::sqrt((error-MinimumShift)/totalWeight);
+}
diff --git a/Simbody/src/RigidBodyNode.cpp b/Simbody/src/RigidBodyNode.cpp
new file mode 100644
index 0000000..4887500
--- /dev/null
+++ b/Simbody/src/RigidBodyNode.cpp
@@ -0,0 +1,253 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Derived from IVM code written by Charles Schwieters *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This file contains implementations for non-inline methods of the
+ * base RigidBodyNode class. The methods here are those for which we do
+ * not need to know the number of mobilities associated with a node in order
+ * to compute efficiently.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "RigidBodyNodeSpec.h"
+
+
+//////////////////////////////////////////////
+// Implementation of RigidBodyNode methods. //
+//////////////////////////////////////////////
+
+void RigidBodyNode::addChild(RigidBodyNode* child) {
+ children.push_back( child );
+}
+
+
+
+//==============================================================================
+// CALC JOINT INDEPENDENT KINEMATICS POS
+//==============================================================================
+// Calc posCM, mass, Mk
+// phi, inertia
+// Should be calc'd from base to tip.
+// We depend on transforms X_PB and X_GB being available.
+// Cost is 90 flops.
+void RigidBodyNode::calcJointIndependentKinematicsPos(
+ SBTreePositionCache& pc) const
+{
+ // Re-express parent-to-child shift vector (Bo-Po) into the ground frame.
+ const Vec3 p_PB_G = getX_GP(pc).R() * getX_PB(pc).p(); // 15 flops
+
+ // The Phi matrix conveniently performs child-to-parent (inward) shifting
+ // on spatial quantities (forces); its transpose does parent-to-child
+ // (outward) shifting for velocities and accelerations.
+ updPhi(pc) = PhiMatrix(p_PB_G);
+
+ // Calculate spatial mass properties. That means we need to transform
+ // the local mass moments into the Ground frame and reconstruct the
+ // spatial inertia matrix Mk.
+
+ const Rotation& R_GB = getX_GB(pc).R();
+ const Vec3& p_GB = getX_GB(pc).p();
+
+ // reexpress inertia in ground (57 flops)
+ const UnitInertia G_Bo_G = getUnitInertia_OB_B().reexpress(~R_GB);
+ const Vec3 p_BBc_G = R_GB*getCOM_B(); // 15 flops
+
+ updCOM_G(pc) = p_GB + p_BBc_G; // 3 flops
+
+ // Calc Mk: the spatial inertia matrix about the body origin.
+ // Note: we need to calculate this now so that we'll be able to calculate
+ // kinetic energy without going past the Velocity stage.
+ updMk_G(pc) = SpatialInertia(getMass(), p_BBc_G, G_Bo_G);
+}
+
+
+
+//==============================================================================
+// CALC JOINT INDEPENDENT KINEMATICS VEL
+//==============================================================================
+// Calculate velocity-related quantities: spatial velocity (V_GB),
+// and coriolis acceleration. This must be
+// called base to tip: depends on parent's spatial velocity, and
+// the just-calculated cross-joint spatial velocity V_PB_G and
+// velocity-dependent acceleration remainder term VD_PB_G.
+// Cost is about 100 flops.
+void
+RigidBodyNode::calcJointIndependentKinematicsVel(
+ const SBTreePositionCache& pc,
+ SBTreeVelocityCache& vc) const
+{
+ if (nodeNum == 0) { // ground, just in case
+ updV_GB(vc) = SpatialVec(Vec3(0), Vec3(0));
+ updGyroscopicForce(vc) = SpatialVec(Vec3(0), Vec3(0));
+ updMobilizerCoriolisAcceleration(vc) = SpatialVec(Vec3(0), Vec3(0));
+ updTotalCoriolisAcceleration(vc) = SpatialVec(Vec3(0), Vec3(0));
+ return;
+ }
+
+ const SpatialVec& V_GP = parent->getV_GB(vc); // parent P's velocity in G
+ const SpatialVec& V_PB_G = getV_PB_G(vc); // child B's vel in P, exp. in G
+ const PhiMatrixTranspose PhiT = ~getPhi(pc); // shift outwards
+
+ // calc spatial velocity of B's origin Bo in G (angular,linear)
+ const SpatialVec V_GB = PhiT*V_GP + V_PB_G; // 18 flops
+ updV_GB(vc) = V_GB;
+
+ const Vec3& w_GB = V_GB[0]; // for convenience
+ const Vec3& v_GB = V_GB[1];
+
+ // Calculate gyroscopic moment and force (48 flops). Although this is
+ // really a dynamic quantity (requires spatial inertia and is itself
+ // a force), we calculate this here rather than in stage Dynamics because
+ // it is needed in inverse dynamics but does not require articulated body
+ // inertias to be calculated. This could be deferred until needed but
+ // probably isn't worth the trouble.
+ updGyroscopicForce(vc) = getMass() * // 6 flops
+ SpatialVec( w_GB % (getUnitInertia_OB_G(pc)*w_GB), // moment (24 flops)
+ (w_GB % (w_GB % getCB_G(pc)))); // force (18 flops)
+
+
+ // Parent velocity.
+ const Vec3& w_GP = V_GP[0]; // for convenience
+ const Vec3& v_GP = V_GP[1];
+
+ // Calculate this mobilizer's incremental contribution to coriolis
+ // acceleration, and this body's total coriolis acceleration (it parent's
+ // coriolis acceleration plus the incremental contribution).
+ //
+ // We just calculated
+ // (1) V_GB = J*u = ~Phi * V_GP + H*u
+ // above. Eventually we will want to compute
+ // (2) A_GB = d/dt V_GB
+ // (3) = J*udot + Jdot*u
+ // (4) = (~Phi*A_GP + H*udot) + (~Phidot*V_GP+Hdot*u).
+ // (Don't be tempted to match the "J" terms in (3) with the two terms in (4)
+ // because A_GP already includes coriolis terms up to the parent.)
+ // That second term in (4) is just velocity dependent so we can calculate
+ // it here. That is what we're calling the "incremental contribution to
+ // coriolis acceleration" of mobilizer B.
+ // CAUTION: our definition of H is transposed from Jain's and Schwieters'.
+ //
+ // So the incremental contribution to the coriolis acceleration is
+ // Amob = ~PhiDot * V_GP + HDot * u
+ // As correctly calculated in Schwieters' paper, Eq [16], the first term
+ // above simplifies to SpatialVec( 0, w_GP % (v_GB-v_GP) ). However,
+ // Schwieters' second term in [16] is correct only if H is constant in P,
+ // in which case the derivative just accounts for the rotation of P in G.
+ // In general H is not constant in P, so we don't try to calculate the
+ // derivative here but assume that HDot*u has already been calculated for
+ // us and stored in VD_PB_G. (That is, V_PB_G = H*u, VD_PB_G = HDot*u.)
+ //
+ // Note: despite all the ground-relative velocities here, this is just
+ // the contribution of the cross-joint velocity, but reexpressed in G.
+ const SpatialVec& VD_PB_G = getVD_PB_G(vc);
+ const SpatialVec Amob(VD_PB_G[0],
+ VD_PB_G[1] + w_GP % (v_GB-v_GP)); // 15 flops
+
+ updMobilizerCoriolisAcceleration(vc) = Amob;
+
+ // Finally, the total coriolis acceleration (normally just called "coriolis
+ // acceleration"!) of body B is the total coriolis acceleration of its
+ // parent shifted outward, plus B's local contribution that we just
+ // calculated.
+ updTotalCoriolisAcceleration(vc) =
+ PhiT * parent->getTotalCoriolisAcceleration(vc) + Amob; // 18 flops
+}
+
+
+
+//==============================================================================
+// CALC KINETIC ENERGY
+//==============================================================================
+// Cost is 57 flops.
+Real RigidBodyNode::calcKineticEnergy(
+ const SBTreePositionCache& pc,
+ const SBTreeVelocityCache& vc) const
+{
+ const Real ret = dot(getV_GB(vc) , getMk_G(pc)*getV_GB(vc));
+ return ret/2;
+}
+
+
+
+//==============================================================================
+// CALC JOINT INDEPENDENT DYNAMICS VEL
+//==============================================================================
+// Calculate mass and velocity-related quantities that are needed for building
+// our dynamics operators, namely the gyroscopic force and centrifugal
+// forces due to coriolis acceleration.
+// This routine expects that coriolis accelerations, gyroscopic forces, and
+// articulated body inertias are already available.
+// As written, the calling order doesn't matter.
+// Cost is 117 flops.
+void
+RigidBodyNode::calcJointIndependentDynamicsVel(
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache& abc,
+ const SBTreeVelocityCache& vc,
+ SBDynamicsCache& dc) const
+{
+ if (nodeNum == 0) { // ground, just in case
+ updMobilizerCentrifugalForces(dc) = SpatialVec(Vec3(0), Vec3(0));
+ updTotalCentrifugalForces(dc) = SpatialVec(Vec3(0), Vec3(0));
+ return;
+ }
+
+ // 72 flops
+ updMobilizerCentrifugalForces(dc) =
+ getP(abc) * getMobilizerCoriolisAcceleration(vc) + getGyroscopicForce(vc);
+
+ // 45 flops
+ updTotalCentrifugalForces(dc) =
+ getMk_G(pc) * getTotalCoriolisAcceleration(vc) + getGyroscopicForce(vc);
+}
+
+
+
+//==============================================================================
+// CALC COMPOSITE BODY INERTIAS
+//==============================================================================
+// Given only position-related quantities from the State
+// Mk_G (this body's spatial inertia matrix, exp. in Ground)
+// Phi (composite body child-to-parent shift matrix)
+// calculate the inverse dynamics quantity
+// R (composite body inertia)
+// This must be called tip-to-base (inward).
+//
+// Note that this method does not depend on the mobilities of
+// the joint so can be implemented here rather than in RigidBodyNodeSpec.
+// Ground should override this implementation though.
+// Cost is about 80 flops.
+void
+RigidBodyNode::calcCompositeBodyInertiasInward(
+ const SBTreePositionCache& pc,
+ Array_<SpatialInertia,MobilizedBodyIndex>& allR) const
+{
+ SpatialInertia& R = toB(allR);
+ R = getMk_G(pc);
+ for (unsigned i=0; i<children.size(); ++i) {
+ const SpatialInertia& RChild = children[i]->fromB(allR);
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+ R += RChild.shift(-phiChild.l()); // ~80 flops
+ }
+}
diff --git a/Simbody/src/RigidBodyNode.h b/Simbody/src/RigidBodyNode.h
new file mode 100644
index 0000000..7839ecc
--- /dev/null
+++ b/Simbody/src/RigidBodyNode.h
@@ -0,0 +1,1093 @@
+#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_H_
+#define SimTK_SIMBODY_RIGID_BODY_NODE_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Derived from IVM code written by Charles Schwieters *
+ * *
+ * 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 "simbody/internal/common.h"
+#include "SimbodyTreeState.h"
+
+#include <cassert>
+
+using SimTK::Vector;
+using SimTK::Vector_;
+using SimTK::Vec3;
+using SimTK::SpatialVec;
+using SimTK::Transform;
+using SimTK::Rotation;
+using SimTK::Inertia;
+using SimTK::MassProperties;
+using SimTK::Array_;
+
+/* This is an abstract class representing the *computational* form of a body
+and its (generic) mobilizer, that is, the joint connecting it to its parent.
+Concrete classes are derived from this one to represent each specific type of
+mobilizer, with the emphasis being on fast calculation since node traversals
+are inner loops of all O(N) multibody algorithms. In particular, while the
+generic RigidBodyNode has a variable number of mobilities, each concrete node
+has a compile-time known size so can perform inline floating point operations
+using stack-allocated variables.
+
+Every body has a body frame B, and a unique inboard mobilizer frame M fixed to
+that body. For convenience, we refer to the body frame of a body's unique
+parent body P as the P frame. There is a frame F fixed to P which is where
+B's inboard mobilizer attaches. The transform X_FM(q) tracks the across-
+mobilizer change in configuration induced by the generalized coordinates q.
+When all the mobilizer coordinates are 0 (=1000 for quaternions), M and F take
+on their "reference configuration" relationship. Usually M==F in the reference
+configuration, but sometimes only the axes are aligned (X_FM(0).R()=Identity)
+while the origins are separated (X_FM(0).p() != 0); the ellipsoid joint is an
+example.
+
+The mobilizer frame M is fixed with respect to B, and F is fixed with
+respect to P. In some cases M and B or F and P will be the same, but not always.
+The constant (TODO: Instance stage) transforms X_BM and X_PF provide the
+configuration of the mobilizer frames with respect to the body and parent
+frames. With these definitions we can easily calculate X_PB as
+X_PB = X_PF*X_FM*X_MB, where X_FM is the q-dependent cross-mobilizer transform
+calculated at Position stage.
+
+RigidBodyNodes know how to extract and deposit their own information from and
+to the Simbody state variables and cache entries, but they don't know anything
+about the State class, stages, etc. Instead they depend on being given
+appropriate access by the caller, whose job it is to mine the State.
+
+The RigidBodyNode abstract base class defines the interface a concrete
+mobilized body node must implement. The nodes must implement two kinds of
+interface methods:
+ - mobilizer-specific local computations
+ - single-node contributions to multibody tree-sweeping algorithms
+The latter methods are called in a well-defined sequence, sweeping either
+inwards or outwards so that a node can depend on either its child or parent
+computations (respectively) having already been completed.
+
+Here is the spec (TODO):
+
+Mobilizer-specific
+
+ Mobility-dependent local kinematics calculations
+ ------------------------------------------------
+ calcQDot
+ calcQDotDot
+ calcMobilizerTransformFromQ
+ calcMobilizerVelocityFromU
+ calcMobilizerAccelerationFromUDot
+
+
+ Intialization of state variables
+ -------------------------------------
+ setMobilizerDefaultModelValues
+ setMobilizerDefaultInstanceValues
+ setMobilizerDefaultTimeValues
+ setMobilizerDefaultPositionValues
+ setMobilizerDefaultVelocityValues
+ setMobilizerDefaultDynamicsValues
+ setMobilizerDefaultAccelerationValues
+
+ Local initial condition setting (solvers)
+ -----------------------------------------
+ setQToFitTransform
+ setQToFitRotation
+ setQToFitTranslation
+ setUToFitVelocity
+ setUToFitAngularVelocity
+ setUToFitLinearVelocity
+ enforceQuaternionConstraints
+
+ Bookkeeping
+ -----------
+ getName
+ getNumU
+ getNumQ
+ isUsingQuaternion
+*/
+class RigidBodyNode {
+public:
+
+// Every concrete RigidBodyNode sets this property on construction so we know
+// whether it can use simple default implementations for kinematic equations
+// (those involving the N matrix), which rely on qdot=u.
+enum QDotHandling {
+ QDotIsAlwaysTheSameAsU, // can use default implementations
+ QDotMayDifferFromU // must supply custom implementation
+};
+
+// This is a fixed property of a given concrete RigidBodyNode.
+bool isQDotAlwaysTheSameAsU() const
+{ return qdotHandling==QDotIsAlwaysTheSameAsU; }
+
+// This property tells us whether a RigidBodyNode may use a quaternion for
+// some set of modeling options. If so, we know that nq > nu, and it must
+// also be the case that QDotMayDifferFromU. (But note that there may be
+// mobilizers for which qdot != u but there is no quaternion.)
+enum QuaternionUse {
+ QuaternionIsNeverUsed,
+ QuaternionMayBeUsed
+};
+
+// This is a fixed property of a given concrete RigidBodyNode.
+bool isQuaternionEverUsed() const
+{ return quaternionUse==QuaternionMayBeUsed; }
+
+virtual ~RigidBodyNode() {}
+
+ // MOBILIZER-SPECIFIC VIRTUAL METHODS //
+
+virtual const char* type() const {return "unknown";}
+virtual int getDOF() const=0; //number of independent dofs
+virtual int getMaxNQ() const=0; //dofs plus extra quaternion coordinate if any
+
+virtual int getNQInUse(const SBModelVars&) const=0; //actual number of q's
+virtual int getNUInUse(const SBModelVars&) const=0; // actual number of u's
+
+// This depends on the mobilizer type and modeling options. If it returns
+// true it also returns the first generalized coordinate of the quaternion
+// (numbering locally), which is assumed to occupy that generalized coordinate
+// and the next three contiguous ones. Quaternion-using mobilizers should be
+// assigned a slot in the quaternion pool.
+virtual bool isUsingQuaternion(const SBStateDigest&,
+ MobilizerQIndex& startOfQuaternion) const=0;
+
+// This depends on the mobilizer type and modeling options. This is the amount
+// of position-cache storage this mobilizer wants us to set aside for
+// precalculations involving its q's, in units of number of Reals. The meaning
+// of the entries in this pool is known only to the node.
+virtual int calcQPoolSize(const SBModelVars&) const = 0;
+
+// This operator is the first step in realizePosition() for this RBNode. Taking
+// modeling information from the supplied state digest, perform calculations
+// that can be done knowing only the current modeling parameters and the
+// values of the q's. The results go into the posCache and qErr arrays which
+// have already been set to the first entry belonging exclusively to this
+// RBNode. DO NOT write directly into the provided State. (Typically
+// the output arrays will be referring to cache entries in the same State, but
+// they don't have to.) Either of those output arrays may be null, and the
+// expected lengths are passed in for consistency checking -- at least in Debug
+// mode you should make sure they are what you're expecting.
+//
+// The only mandatory calculation here is that this routine *must* calculate
+// constraint errors if there are any mobilizer-local constraints among
+// the q's -- typically that is the quaternion normalization error where
+// qerr=|q|-1. For many mobilizers, it is a good idea to precalculate
+// expensive operations like sin(q), cos(q), or 1/norm(q) so that these don't
+// need to be recalculated when forming X_FM, N, NInv, and NDot later.
+// Note that if you want to save such precalculations, you must have asked
+// for space to be set aside during realizeModel().
+//
+// The default implementation here checks the parameters and then does
+// nothing.
+virtual void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* posCache, int nPosCache,
+ Real* qErr, int nQErr) const=0;
+
+// This mandatory routine calculates the across-joint transform X_FM generated
+// by the supplied q values. This may depend on sines & cosines or normalized
+// quaternions already having been calculated and placed into the supplied
+// posCache. If you just have q's, call the calcAcrossJointTransform() method
+// instead because it knows how to fill in the posCache if need be.
+//
+// This method constitutes the *definition* of the generalized coordinates for
+// a particular mobilizer.
+//
+// Note: this calculates the transform between the *as defined* frames; we
+// might reverse the frames in use. So we call this X_F0M0 while the possibly
+// reversed version is X_FM.
+virtual void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* posCache, int nPos,
+ Transform& X_F0M0) const = 0;
+
+// This is a pure operator form of calcX_FM(). The StateDigest must have been
+// realized to model stage. The result depends only on the passed-in q,
+// not anything in the State beyond model stage.
+void calcAcrossJointTransform(
+ const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Transform& X_F0M0) const
+{
+ const int poolz = getQPoolSize(sbs.getModelCache());
+ Real* qPool = poolz ? new Real[poolz] : 0;
+ Real qErr;
+ const int nQErr = isQuaternionInUse(sbs.getModelCache()) ? 1 : 0;
+ performQPrecalculations(sbs, q, nq, qPool, poolz, &qErr, nQErr);
+ calcX_FM(sbs, q, nq, qPool, poolz, X_F0M0);
+ delete[] qPool;
+}
+
+// An alternate signature for when you have all the Q's together; this
+// method will find just ours and call the above method.
+void calcAcrossJointTransform(
+ const SBStateDigest& sbs,
+ const Vector& q,
+ Transform& X_F0M0) const
+{
+ const SBModelCache mc = sbs.getModelCache();
+ const int nq = getNumQInUse(mc);
+ const Real* qp = nq ? &q[getFirstQIndex(mc)] : (Real*)0;
+ calcAcrossJointTransform(sbs, qp, nq, X_F0M0);
+}
+
+
+// This operator pulls N(q) from the StateDigest if necessary and calculates
+// qdot=N(q)*u from the supplied argument. For many mobilizers it
+// can simply copy u to qdot without referencing the state at all.
+virtual void calcQDot
+ (const SBStateDigest&, const Real* u, Real* qdot) const
+{ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode", "calcQDot");}
+// This operator pulls N(q) and NDot(q,u) from the StateDigest if necessary
+// and calculates qdotdot=N*udot + NDot*u from the supplied argument.
+// For many mobilizers it can simply copy udot to qdotdot without referencing
+// the state at all.
+virtual void calcQDotDot
+ (const SBStateDigest&, const Real* udot, Real* qdotdot) const
+{ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode", "calcQDotDot");}
+virtual Transform calcMobilizerTransformFromQ
+ (const SBStateDigest&, const Real* q) const
+{ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode", "calcMobilizerTransformFromQ");}
+virtual SpatialVec calcMobilizerVelocityFromU
+ (const SBStateDigest&, const Real* u) const
+{ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode", "calcMobilizerVelocityFromU");}
+virtual SpatialVec calcMobilizerAccelerationFromUDot
+ (const SBStateDigest&, const Real* udot) const
+{ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode", "calcMobilizerAccelerationFromUDot");}
+virtual Transform calcParentToChildTransformFromQ
+ (const SBStateDigest&, const Real* q) const
+{ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode", "calcParentToChildTransformFromQ");}
+virtual SpatialVec calcParentToChildVelocityFromU
+ (const SBStateDigest&, const Real* u) const
+{ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode", "calcParentToChildVelocityFromU");}
+virtual SpatialVec calcParentToChildAccelerationFromUDot
+ (const SBStateDigest&, const Real* udot) const
+{ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode", "calcParentToChildAccelerationFromUDot");}
+
+// Operators involving kinematics matrix N and related matrices NInv and
+// NDot. These methods perform operations involving just the block on the
+// diagonal of the matrix that corresponds to this mobilizer. These blocks
+// can be rectangular, in which case the u-dimension is always the number
+// of mobilizers dofs (generalized speeds) but the q-dimension may depend on
+// modeling options (specifically, whether the mobilizer orientation is
+// modeled with 4 quaternions or 3 Euler angles). These normally treat the
+// input as a column vector and multiply with the matrix on the left.
+// Optionally they will treat the input as a row vector and multiply with
+// the matrix on the right. (The latter is equivalent to multiplication on
+// the left by the transpose of the matrix.)
+//
+// multiplyByN
+// Left out_q = N * in_u
+// Right out_u = in_q * N
+//
+// multiplyByNInv
+// Left out_u = inv(N) * in_q
+// Right out_q = in_u * inv(N)
+//
+// multiplyByNDot
+// Left out_q = NDot * in_u
+// Right out_u = in_q * NDot
+// (not invertible but inverse isn't needed; see below)
+//
+// where 'in_q' and 'in_u' etc. indicate q-like and u-like vectors or row
+// vectors, not the actual state variables of the same names. Note that the
+// interpretation of the in and out arrays is different depending on whether
+// we're multiplying on the left or right; which is q-like and which is
+// u-like reverses.
+//
+// Note that N=N(q), NInv=Ninv(q), and NDot=NDot(q,u) where now I am talking
+// about the real set of generalized coordinates q and generalized speeds u
+// but *just for this mobilizer*, whose values are taken from the supplied
+// StateDigest.
+//
+// In typical usage these are used to calculate qdot=N*u,
+// qdotdot=N*udot + NDot*u. These can be inverted to calculate u=inv(N)*qdot,
+// and udot=inv(N)*(qdotdot - NDot*u); note that inv(NDot) is not needed (a
+// good thing since it is likely to be singular!).
+//
+// WARNING: these routines do not normalize quaternions before using them,
+// so the resulting matrices are scaled by the quaternion norm (if it
+// isn't 1). That's a feature not a bug! This is important for correct
+// calculation of qdots because otherwise they would not be the correct
+// derivatives for unnormalized q's.
+
+virtual void multiplyByN (const SBStateDigest&, bool matrixOnRight,
+ const Real* in, Real* out) const = 0;
+virtual void multiplyByNInv(const SBStateDigest&, bool matrixOnRight,
+ const Real* in, Real* out) const = 0;
+virtual void multiplyByNDot(const SBStateDigest&, bool matrixOnRight,
+ const Real* in, Real* out) const = 0;
+
+// This will do nothing unless the mobilizer is using a quaternion. Otherwise it
+// will normalize its quaternion in q, and if qErrest has non-zero length then
+// it will remove the component of the error estimate which was along the
+// direction of the quaternion, since that error will now be zero. That is,
+// we'll set
+// q = q_fixed = q/|q|
+// and qErrest -= dot(qErrest,q_fixed)*q_fixed
+virtual bool enforceQuaternionConstraints(
+ const SBStateDigest& sbs,
+ Vector& q,
+ Vector& qErrest) const=0;
+
+// Convert from quaternion to Euler angle representations.
+virtual void convertToEulerAngles(const Vector& inputQ, Vector& outputQ) const
+{ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode", "convertToEulerAngles");};
+// Convert from Euler angle to quaternion representations.
+virtual void convertToQuaternions(const Vector& inputQ, Vector& outputQ) const
+{ SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode", "convertToQuaternions");};
+
+// Called after Model variables are allocated by realizeTopology()
+virtual void setMobilizerDefaultModelValues
+ (const SBTopologyCache&, SBModelVars&) const {}
+
+// All the rest are called right after realizeModel() since that's when all the
+// remaining state variables are allocated.
+virtual void setMobilizerDefaultInstanceValues
+ (const SBModelVars&, SBInstanceVars&) const {}
+virtual void setMobilizerDefaultTimeValues
+ (const SBModelVars&, SBTimeVars&) const {}
+virtual void setMobilizerDefaultPositionValues
+ (const SBModelVars&, Vector& q) const {}
+virtual void setMobilizerDefaultVelocityValues
+ (const SBModelVars&, Vector& u) const {}
+virtual void setMobilizerDefaultDynamicsValues
+ (const SBModelVars&, SBDynamicsVars&) const {}
+virtual void setMobilizerDefaultAccelerationValues
+ (const SBModelVars&, SBAccelerationVars&) const {}
+
+// These attempt to set the mobilizer's internal configuration or velocity
+// to a specified value. This is intended to be a fast, local calculation that
+// produces an answer to machine precision *if* the mobilizer can represent the
+// given quantity exactly. The answer is returned in the appropriate slots of a
+// caller-provided "q-like" or "u-like" Vector; the implementation must not
+// look at or change any other slots.
+//
+// It is OK for the implementation to use the current values of the coordinates
+// or speeds, and it is required to preserve any of these that are not needed
+// to satisfy the request. If the mobilizer can't satisfy the request to
+// machine precision it should just do nothing or the best it can, with the
+// only general rule being that it shouldn't make things worse. In particular,
+// it does not need to work hard on an approximate solution.
+//
+// Note: these are non-virtual wrappers which arrange to reverse the request
+// for reversed mobilizers, so that the mobilizers themselves do not need to
+// know they have been reversed. The corresponding pure virtuals are protected.
+
+void setQToFitTransform
+ (const SBStateDigest& sbs, const Transform& X_FM, Vector& q) const
+{ setQToFitTransformImpl(sbs, isReversed() ? Transform(~X_FM) : X_FM, q); }
+
+void setQToFitRotation
+ (const SBStateDigest& sbs, const Rotation& R_FM, Vector& q) const
+{ setQToFitRotationImpl(sbs, isReversed() ? Rotation(~R_FM) : R_FM, q); }
+
+// Reversing a translation requires that we obtain the current orientation,
+// which we'll do assuming there is something meaningful in the rotational
+// part of the q's.
+void setQToFitTranslation
+ (const SBStateDigest& sbs, const Vec3& p_FM, Vector& q) const
+{
+ if (!isReversed()) {setQToFitTranslationImpl(sbs, p_FM, q); return;}
+
+ Transform X_MF; // note reversal of frames
+ calcAcrossJointTransform(sbs, q, X_MF);
+ setQToFitTranslationImpl(sbs, X_MF.R()*(-p_FM), q);
+}
+
+void setUToFitVelocity
+ (const SBStateDigest& sbs, const Vector& q, const SpatialVec& V_FM, Vector& u) const
+{ if (!isReversed()) {setUToFitVelocityImpl(sbs, q, V_FM, u); return;}
+ Transform X_MF; // note reversal of frames
+ calcAcrossJointTransform(sbs, q, X_MF);
+ setUToFitVelocityImpl(sbs, q, reverseSpatialVelocity(~X_MF,V_FM), u);
+}
+
+void setUToFitAngularVelocity
+ (const SBStateDigest& sbs, const Vector& q, const Vec3& w_FM, Vector& u) const
+{ if (!isReversed()) {setUToFitAngularVelocityImpl(sbs, q, w_FM, u); return;}
+ Transform X_MF; // note reversal of frames
+ calcAcrossJointTransform(sbs, q, X_MF);
+ setUToFitAngularVelocityImpl(sbs, q, reverseAngularVelocity(~X_MF.R(),w_FM), u);
+}
+
+void setUToFitLinearVelocity
+ (const SBStateDigest& sbs, const Vector& q, const Vec3& v_FM, Vector& u) const
+{ if (!isReversed()) {setUToFitLinearVelocityImpl(sbs, q, v_FM, u); return;}
+ Transform X_MF; // note reversal of frames
+ calcAcrossJointTransform(sbs, q, X_MF);
+ //TODO: we have to assume angular velocity is zero here. Should be some
+ // way to get the joint to compute w_FM.
+ const SpatialVec V_FM( Vec3(0), v_FM );
+ setUToFitLinearVelocityImpl(sbs, q, reverseSpatialVelocity(~X_MF,V_FM)[1], u);
+}
+
+
+ // VIRTUAL METHODS FOR SINGLE-NODE OPERATOR CONTRIBUTIONS //
+
+virtual void realizeModel(
+ SBStateDigest& sbs) const=0;
+
+virtual void realizeInstance(
+ const SBStateDigest& sbs) const=0;
+
+// The multibody tree cannot have any time dependence; hence no
+// realizeTime().
+
+// Introduce new values for generalized coordinates and calculate
+// all the position-dependent kinematic terms, including position
+// constraint errors. Must be called base to tip.
+virtual void realizePosition(
+ const SBStateDigest& sbs) const=0;
+
+// Introduce new values for generalized speeds and calculate
+// all the velocity-dependent kinematic terms. Assumes realizePosition()
+// has already been called on all nodes. Must be called base to tip.
+virtual void realizeVelocity(
+ const SBStateDigest& sbs) const=0;
+
+// Calculate base-to-tip velocity-dependent terms which will be used
+// in Dynamics stage operators. Assumes realizeVelocity()
+// has already been called on all nodes, as well as any Dynamics
+// stage calculations which must go tip-to-base (e.g. articulated
+// body inertias).
+virtual void realizeDynamics(
+ const SBArticulatedBodyInertiaCache& abc,
+ const SBStateDigest& sbs) const=0;
+
+// There is no realizeAcceleration().
+
+virtual void realizeReport(
+ const SBStateDigest& sbs) const=0;
+
+virtual void realizeArticulatedBodyInertiasInward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ SBArticulatedBodyInertiaCache& abc) const=0;
+
+virtual void realizeYOutward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache& abc,
+ SBDynamicsCache& dc) const=0;
+
+// This has a default implementation that is good for everything
+// but Ground.
+virtual void calcCompositeBodyInertiasInward(
+ const SBTreePositionCache& pc,
+ Array_<SpatialInertia,MobilizedBodyIndex>& R) const;
+
+virtual void multiplyBySystemJacobian(
+ const SBTreePositionCache& pc,
+ const Real* v,
+ SpatialVec* Jv) const
+ { SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode",
+ "multiplyBySystemJacobian"); }
+
+virtual void multiplyBySystemJacobianTranspose(
+ const SBTreePositionCache& pc,
+ SpatialVec* zTmp,
+ const SpatialVec* X,
+ Real* JtX) const
+ { SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode",
+ "multiplyBySystemJacobianTranspose"); }
+
+virtual void calcEquivalentJointForces(
+ const SBTreePositionCache& pc,
+ const SBDynamicsCache& dc,
+ const SpatialVec* bodyForces,
+ SpatialVec* allZ,
+ Real* jointForces) const
+ { SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode", "calcEquivalentJointForces"); }
+
+virtual void calcUDotPass1Inward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache& abc,
+ const SBDynamicsCache& dc,
+ const Real* jointForces,
+ const SpatialVec* bodyForces,
+ const Real* allUDot,
+ SpatialVec* allZ,
+ SpatialVec* allGepsilon,
+ Real* allEpsilon) const=0;
+virtual void calcUDotPass2Outward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache& abc,
+ const SBTreeVelocityCache& vc,
+ const SBDynamicsCache& dc,
+ const Real* epsilonTmp,
+ SpatialVec* allA_GB,
+ Real* allUDot,
+ Real* allTau) const=0;
+
+virtual void multiplyByMInvPass1Inward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache& abc,
+ const Real* f,
+ SpatialVec* allZ,
+ SpatialVec* allGepsilon,
+ Real* allEpsilon) const=0;
+virtual void multiplyByMInvPass2Outward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache& abc,
+ const Real* epsilonTmp,
+ SpatialVec* allA_GB,
+ Real* allUDot) const=0;
+
+// Also serves as pass 1 for inverse dynamics.
+virtual void calcBodyAccelerationsFromUdotOutward(
+ const SBTreePositionCache& pc,
+ const SBTreeVelocityCache& vc,
+ const Real* allUDot,
+ SpatialVec* allA_GB) const
+ { SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode", "calcBodyAccelerationsFromUdotOutward"); }
+virtual void calcInverseDynamicsPass2Inward(
+ const SBTreePositionCache& pc,
+ const SBTreeVelocityCache& vc,
+ const SpatialVec* allA_GB,
+ const Real* jointForces,
+ const SpatialVec* bodyForces,
+ SpatialVec* allFTmp,
+ Real* allTau) const
+ { SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode", "calcInverseDynamicsPass2Inward"); }
+
+virtual void multiplyByMPass1Outward(
+ const SBTreePositionCache& pc,
+ const Real* allUDot,
+ SpatialVec* allA_GB) const
+ { SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode", "multiplyByMPass1Outward"); }
+virtual void multiplyByMPass2Inward(
+ const SBTreePositionCache& pc,
+ const SpatialVec* allA_GB,
+ SpatialVec* allFTmp,
+ Real* allTau) const
+ { SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode", "multiplyByMPass2Inward"); }
+
+
+virtual void setVelFromSVel(const SBStateDigest&,
+ const SpatialVec&, Vector& u) const {SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode", "setVelFromSVel");}
+
+// Note that this requires columns of H to be packed like SpatialVec.
+virtual const SpatialVec& getHCol(const SBTreePositionCache&, int j) const
+{SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode", "getHCol");}
+
+virtual const SpatialVec& getH_FMCol(const SBTreePositionCache&, int j) const
+{SimTK_THROW2(Exception::UnimplementedVirtualMethod, "RigidBodeNode", "getH_FMCol");}
+
+//TODO (does this even belong here?)
+virtual void velFromCartesian() {}
+
+
+ // BASE CLASS METHODS //
+
+RigidBodyNode& operator=(const RigidBodyNode&);
+
+static RigidBodyNode* createGroundNode();
+
+// Register the passed-in node as a child of this one.
+void addChild(RigidBodyNode* child);
+void setParent(RigidBodyNode* p) {parent=p;}
+void setNodeNum(MobilizedBodyIndex n) {nodeNum=n;}
+void setLevel(int i) {level=i;}
+
+ // TOPOLOGICAL INFO: no State needed
+
+RigidBodyNode* getParent() const {return parent;}
+int getNumChildren() const {return (int)children.size();}
+RigidBodyNode* getChild(int i) const {return (i<(int)children.size()?children[i]:0);}
+
+bool isReversed() const {return reversed;}
+
+// Return this node's level, that is, how many ancestors separate it from
+// the Ground node at level 0. Level 1 nodes (directly connected to the
+// Ground node) are called 'base' nodes.
+int getLevel() const {return level;}
+
+// This is the unique "body index" for this (body,mobilizer) node. It is used to index
+// arrays of body quantities.
+MobilizedBodyIndex getNodeNum() const {return nodeNum;}
+
+bool isGroundNode() const { return level==0; }
+bool isBaseNode() const { return level==1; }
+
+// TODO: these should come from the model cache.
+UIndex getUIndex() const {return uIndex;}
+QIndex getQIndex() const {return qIndex;}
+
+
+// Access routines for plucking the right per-body data from the pool in the State.
+const Transform& fromB(const Array_<Transform,MobilizedBodyIndex>& x) const {return x[nodeNum];}
+const PhiMatrix& fromB(const Array_<PhiMatrix,MobilizedBodyIndex>& p) const {return p[nodeNum];}
+const MassProperties& fromB(const Array_<MassProperties,MobilizedBodyIndex>& m) const {return m[nodeNum];}
+const Inertia& fromB(const Array_<Inertia,MobilizedBodyIndex>& i) const {return i[nodeNum];}
+const UnitInertia& fromB(const Array_<UnitInertia,MobilizedBodyIndex>& i) const {return i[nodeNum];}
+int fromB(const Array_<int,MobilizedBodyIndex>& i) const {return i[nodeNum];}
+const SpatialVec& fromB(const Array_<SpatialVec,MobilizedBodyIndex>& v) const {return v[nodeNum];}
+const SpatialMat& fromB(const Array_<SpatialMat,MobilizedBodyIndex>& m) const {return m[nodeNum];}
+const SpatialInertia& fromB(const Array_<SpatialInertia,MobilizedBodyIndex>& m) const {return m[nodeNum];}
+const ArticulatedInertia& fromB(const Array_<ArticulatedInertia,MobilizedBodyIndex>& m) const {return m[nodeNum];}
+const Vec3& fromB(const Array_<Vec3,MobilizedBodyIndex>& v) const {return v[nodeNum];}
+
+
+Transform& toB(Array_<Transform,MobilizedBodyIndex>& x) const {return x[nodeNum];}
+PhiMatrix& toB(Array_<PhiMatrix,MobilizedBodyIndex>& p) const {return p[nodeNum];}
+MassProperties& toB(Array_<MassProperties,MobilizedBodyIndex>& m) const {return m[nodeNum];}
+Inertia& toB(Array_<Inertia,MobilizedBodyIndex>& i) const {return i[nodeNum];}
+UnitInertia& toB(Array_<UnitInertia,MobilizedBodyIndex>& i) const {return i[nodeNum];}
+int& toB(Array_<int,MobilizedBodyIndex>& i) const {return i[nodeNum];}
+SpatialVec& toB(Array_<SpatialVec,MobilizedBodyIndex>& v) const {return v[nodeNum];}
+SpatialMat& toB(Array_<SpatialMat,MobilizedBodyIndex>& m) const {return m[nodeNum];}
+SpatialInertia& toB(Array_<SpatialInertia,MobilizedBodyIndex>& m) const {return m[nodeNum];}
+ArticulatedInertia& toB(Array_<ArticulatedInertia,MobilizedBodyIndex>& m) const {return m[nodeNum];}
+Vec3& toB(Array_<Vec3,MobilizedBodyIndex>& v) const {return v[nodeNum];}
+
+// Elementwise access to Vectors is relatively expensive; use sparingly.
+const SpatialVec& fromB(const Vector_<SpatialVec>& v) const {return v[nodeNum];}
+const SpatialMat& fromB(const Vector_<SpatialMat>& m) const {return m[nodeNum];}
+const Vec3& fromB(const Vector_<Vec3>& v) const {return v[nodeNum];}
+SpatialMat& toB(Vector_<SpatialMat>& m) const {return m[nodeNum];}
+Vec3& toB(Vector_<Vec3>& v) const {return v[nodeNum];}
+SpatialVec& toB(Vector_<SpatialVec>& v) const {return v[nodeNum];}
+
+ // MODELING INFO
+bool getUseEulerAngles(const SBModelVars& mv) const {return mv.useEulerAngles;}
+
+// Find cache resources allocated to this RigidBodyNode.
+const SBModelPerMobodInfo&
+getModelInfo(const SBModelCache& mc) const
+{ return mc.getMobodModelInfo(nodeNum); }
+
+QIndex getFirstQIndex(const SBModelCache& mc) const
+{ return getModelInfo(mc).firstQIndex; }
+int getNumQInUse(const SBModelCache& mc) const
+{ return getModelInfo(mc).nQInUse; }
+
+UIndex getFirstUIndex(const SBModelCache& mc) const
+{ return getModelInfo(mc).firstUIndex; }
+int getNumUInUse(const SBModelCache& mc) const
+{ return getModelInfo(mc).nUInUse; }
+
+// Return value will be invalid if there are no quaternions currently in use here.
+QuaternionPoolIndex getQuaternionPoolIndex(const SBModelCache& mc) const
+{ return getModelInfo(mc).quaternionPoolIndex; }
+bool isQuaternionInUse(const SBModelCache& mc) const
+{ return getModelInfo(mc).hasQuaternionInUse; }
+
+// Return value will be invalid if this node is not currently using any space
+// in the q-pool position cache entry.
+MobodQPoolIndex getQPoolIndex(const SBModelCache& mc) const
+{ return getModelInfo(mc).startInQPool; }
+int getQPoolSize(const SBModelCache& mc) const
+{ return getModelInfo(mc).nQPoolInUse; }
+
+// Return a pointer to the first slot in the q cache pool that is
+// assigned to this node, or null if there are none.
+const Real* getQPool(const SBModelCache& mc,
+ const SBTreePositionCache& pc) const
+{ const MobodQPoolIndex ix = getQPoolIndex(mc);
+ return ix.isValid() ? &pc.mobilizerQCache[ix] : (Real*)0; }
+
+ // INSTANCE INFO
+
+// TODO: These ignore State currently since they aren't parametrizable.
+const MassProperties& getMassProperties_OB_B() const {return massProps_B;}
+const Real& getMass () const {return massProps_B.getMass();}
+const Vec3& getCOM_B () const {return massProps_B.getMassCenter();}
+const UnitInertia& getUnitInertia_OB_B() const {return massProps_B.getUnitInertia();}
+const Transform& getX_BM () const {return X_BM;}
+const Transform& getX_PF () const {return X_PF;}
+
+// These are calculated on construction.
+// TODO: principal axes
+const Inertia& getInertia_CB_B () const {return inertia_CB_B;}
+const Transform& getX_MB () const {return X_MB;}
+
+// This says whether this mobilizer has prescribed *acceleration*, and if so
+// whether the acceleration is known to be zero.
+bool isUDotKnown(const SBInstanceCache& ic) const
+{ return ic.getMobodInstanceInfo(nodeNum).udotMethod!=Motion::Free; }
+bool isUDotKnownToBeZero(const SBInstanceCache& ic) const
+{ return ic.getMobodInstanceInfo(nodeNum).udotMethod==Motion::Zero; }
+
+ // POSITION INFO
+
+// Extract from the cache X_FM, the cross-mobilizer transformation matrix giving the configuration
+// of this body's mobilizer frame M, measured from and expressed in the corresponding outboard
+// mobilizer frame F attached to the parent. This transformation is defined to be zero (that is, F=M)
+// in the reference configuration where the joint coordinates are all 0 (or 1,0,0,0 for quaternions).
+// This is NOT a spatial (ground frame) transformation.
+const Transform& getX_FM(const SBTreePositionCache& pc) const {return fromB(pc.bodyJointInParentJointFrame);}
+Transform& updX_FM(SBTreePositionCache& pc) const {return toB (pc.bodyJointInParentJointFrame);}
+
+// Return X_F0M0, the cross-joint mobilizer transform *as it was defined* in the mobilizer
+// specification. If the mobilizer has been reversed, then X_F0M0=~X_FM, otherwise it is
+// just X_FM.
+Transform findX_F0M0(const SBTreePositionCache& pc) const
+{ return isReversed() ? Transform(~getX_FM(pc)) // 18 flops
+ : getX_FM(pc); }
+
+// Extract from the cache X_PB, the cross-joint transformation matrix giving the configuration
+// of this body's frame B measured from and expressed in its *parent* frame P. Thus this is NOT
+// a spatial (ground frame) transformation.
+const Transform& getX_PB(const SBTreePositionCache& pc) const {return fromB(pc.bodyConfigInParent);}
+Transform& updX_PB(SBTreePositionCache& pc) const {return toB (pc.bodyConfigInParent);}
+
+// Extract from the cache X_GB, the transformation matrix giving the spatial configuration of this
+// body's frame B measured from and expressed in ground. This consists of a rotation matrix
+// R_GB, and a ground-frame vector r_OG_OB from ground's origin to the origin point of frame B.
+const Transform& getX_GB(const SBTreePositionCache& pc) const {
+ return fromB(pc.bodyConfigInGround);
+}
+Transform& updX_GB(SBTreePositionCache& pc) const {
+ return toB(pc.bodyConfigInGround);
+}
+
+// Extract from the cache the body-to-parent shift matrix "phi".
+const PhiMatrix& getPhi(const SBTreePositionCache& pc) const {return fromB(pc.bodyToParentShift);}
+PhiMatrix& updPhi(SBTreePositionCache& pc) const {return toB (pc.bodyToParentShift);}
+
+// Extract this body's spatial inertia matrix from the cache. This contains
+// the mass properties measured from (and about) the body frame origin, but
+// expressed in the Ground frame.
+const SpatialInertia& getMk_G(const SBTreePositionCache& pc) const {return fromB(pc.bodySpatialInertiaInGround);}
+SpatialInertia& updMk_G(SBTreePositionCache& pc) const {return toB (pc.bodySpatialInertiaInGround);}
+
+// Extract from the cache the location of the body's center of mass, measured from the ground
+// origin and expressed in Ground.
+const Vec3& getCOM_G(const SBTreePositionCache& pc) const {return fromB(pc.bodyCOMInGround);}
+Vec3& updCOM_G(SBTreePositionCache& pc) const {return toB (pc.bodyCOMInGround);}
+
+// Extract from the cache the vector from body B's origin to its center of
+// mass, reexpressed in Ground.
+const Vec3& getCB_G(const SBTreePositionCache& pc) const
+{ return getMk_G(pc).getMassCenter(); }
+
+// Extract from the cache the body's inertia about the body origin OB, but
+// reexpressed in Ground.
+const UnitInertia& getUnitInertia_OB_G(const SBTreePositionCache& pc) const
+{ return getMk_G(pc).getUnitInertia(); }
+
+// Extract from the cache the spatial (ground-relative) location and orientation of this body's
+// *parent's* body frame P.
+const Transform& getX_GP(const SBTreePositionCache& pc) const {assert(parent); return parent->getX_GB(pc);}
+
+ // VELOCITY INFO
+
+const SpatialVec& getV_FM(const SBTreeVelocityCache& vc) const {return fromB(vc.mobilizerRelativeVelocity);}
+SpatialVec& updV_FM(SBTreeVelocityCache& vc) const {return toB (vc.mobilizerRelativeVelocity);}
+
+// Given V_AB, the spatial velocity of frame B in A expressed in A, return V_BA, the spatial
+// velocity of frame A in B, expressed in B. The reversal also requires knowing X_AB, the spatial
+// position and orientation of B in A. Theory:
+// V_BA = -R_BA * [ w_AB ]
+// [ v_AB + p_AB x w_AB ]
+// This costs 45 flops. See reverseAngularVelocity() if that's all you need; it's a lot cheaper.
+static SpatialVec reverseSpatialVelocity(const Transform& X_AB, const SpatialVec& V_AB) {
+ const Rotation& R_AB = X_AB.R();
+ const Vec3& p_AB = X_AB.p();
+ const Vec3& w_AB = V_AB[0];
+ const Vec3& v_AB = V_AB[1];
+
+ return ~R_AB * SpatialVec( -w_AB, (w_AB % p_AB) - v_AB );
+}
+
+// Given w_AB, the angular velocity of frame B in A expressed in A, return w_BA, the angular
+// velocity of frame A in B, expressed in B. The reversal also requires knowing R_AB, the
+// orientation of B in A. Theory:
+// w_BA = -R_BA*w_AB
+// This is just a subset of what reverseSpatialVelocity does, but it is cheap and
+// often all you need is the angular velocity.
+// This costs 18 flops.
+static Vec3 reverseAngularVelocity(const Rotation& R_AB, const Vec3& w_AB) {
+ return ~R_AB * (-w_AB);
+}
+
+// Return V_F0M0, the cross-joint mobilizer velocity *as it was defined* in the mobilizer
+// specification. If the mobilizer has not been reversed, this is just V_FM.
+// 45 flops if reversed.
+SpatialVec findV_F0M0(const SBTreePositionCache& pc, const SBTreeVelocityCache& vc) const {
+ return isReversed() ? reverseSpatialVelocity(getX_FM(pc), getV_FM(vc))
+ : getV_FM(vc);
+}
+
+// Return w_F0M0, the cross-joint mobilizer angular velocity *as it was defined*
+// in the mobilizer specification. If the mobilizer has not been reversed, this is just w_FM.
+// This is a useful and much cheaper subset of findV_F0M0.
+// 18 flops if reversed.
+Vec3 find_w_F0M0(const SBTreePositionCache& pc, const SBTreeVelocityCache& vc) const {
+ return isReversed() ? reverseAngularVelocity(getX_FM(pc).R(), getV_FM(vc)[0])
+ : getV_FM(vc)[0];
+}
+
+// Extract from the cache V_GB, the spatial velocity of this body's frame B measured in and
+// expressed in ground. This contains the angular velocity of B in G, and the linear velocity
+// of B's origin point OB in G, with both vectors expressed in G.
+const SpatialVec& getV_GB (const SBTreeVelocityCache& vc) const {return fromB(vc.bodyVelocityInGround);}
+SpatialVec& updV_GB (SBTreeVelocityCache& vc) const {return toB (vc.bodyVelocityInGround);}
+
+// Extract from the cache V_PB_G, the *spatial* velocity of this body's frame B, that is the
+// cross-joint velocity measured with respect to the parent frame, but then expressed in the
+// *ground* frame. This contains the angular velocity of B in P, and the linear velocity
+// of B's origin point OB in P, with both vectors expressed in *G*.
+const SpatialVec& getV_PB_G (const SBTreeVelocityCache& vc) const {return fromB(vc.bodyVelocityInParent);}
+SpatialVec& updV_PB_G (SBTreeVelocityCache& vc) const {return toB (vc.bodyVelocityInParent);}
+
+const SpatialVec& getV_GP(const SBTreeVelocityCache& vc) const {assert(parent); return parent->getV_GB(vc);}
+
+const SpatialVec& getSpatialVel (const SBTreeVelocityCache& vc) const {return getV_GB(vc);}
+const Vec3& getSpatialAngVel(const SBTreeVelocityCache& vc) const {return getV_GB(vc)[0];}
+const Vec3& getSpatialLinVel(const SBTreeVelocityCache& vc) const {return getV_GB(vc)[1];}
+
+// Extract from the cache VD_PB_G, the *spatial* velocity derivative remainder term
+// generated by H_PB_G_Dot*u, where H_PB_G_Dot = d/dt H_PB_G with the derivative taken
+// in the Ground frame. This is used in calculation of coriolis acceleration.
+// CAUTION: our definition for the H matrix is transposed from those used by Jain and Schwieters.
+const SpatialVec& getVD_PB_G (const SBTreeVelocityCache& vc) const
+ {return fromB(vc.bodyVelocityInParentDerivRemainder);}
+SpatialVec& updVD_PB_G (SBTreeVelocityCache& vc) const
+ {return toB (vc.bodyVelocityInParentDerivRemainder);}
+
+const SpatialVec& getGyroscopicForce(const SBTreeVelocityCache& vc) const {return fromB(vc.gyroscopicForces);}
+SpatialVec& updGyroscopicForce(SBTreeVelocityCache& vc) const {return toB (vc.gyroscopicForces);}
+
+const SpatialVec& getMobilizerCoriolisAcceleration(const SBTreeVelocityCache& vc) const {return fromB(vc.mobilizerCoriolisAcceleration);}
+SpatialVec& updMobilizerCoriolisAcceleration(SBTreeVelocityCache& vc) const {return toB (vc.mobilizerCoriolisAcceleration);}
+
+const SpatialVec& getTotalCoriolisAcceleration(const SBTreeVelocityCache& vc) const {return fromB(vc.totalCoriolisAcceleration);}
+SpatialVec& updTotalCoriolisAcceleration(SBTreeVelocityCache& vc) const {return toB (vc.totalCoriolisAcceleration);}
+
+ // DYNAMICS INFO
+
+// Composite body inertias.
+const SpatialInertia& getR(const SBCompositeBodyInertiaCache& cbc) const {return fromB(cbc.compositeBodyInertia);}
+SpatialInertia& updR(SBCompositeBodyInertiaCache& cbc) const {return toB (cbc.compositeBodyInertia);}
+
+// Articulated body inertias and related calculations
+const ArticulatedInertia& getP(const SBArticulatedBodyInertiaCache& abc) const {return fromB(abc.articulatedBodyInertia);}
+ArticulatedInertia& updP(SBArticulatedBodyInertiaCache& abc) const {return toB (abc.articulatedBodyInertia);}
+
+const ArticulatedInertia& getPPlus(const SBArticulatedBodyInertiaCache& abc) const {return fromB(abc.pPlus);}
+ArticulatedInertia& updPPlus(SBArticulatedBodyInertiaCache& abc) const {return toB (abc.pPlus);}
+
+// Others
+
+const SpatialVec& getMobilizerCentrifugalForces(const SBDynamicsCache& dc) const {return fromB(dc.mobilizerCentrifugalForces);}
+SpatialVec& updMobilizerCentrifugalForces(SBDynamicsCache& dc) const {return toB (dc.mobilizerCentrifugalForces);}
+
+const SpatialVec& getTotalCentrifugalForces(const SBDynamicsCache& dc) const {return fromB(dc.totalCentrifugalForces);}
+SpatialVec& updTotalCentrifugalForces(SBDynamicsCache& dc) const {return toB (dc.totalCentrifugalForces);}
+
+const SpatialVec& getZ(const SBTreeAccelerationCache& tac) const {return fromB(tac.z);}
+SpatialVec& updZ(SBTreeAccelerationCache& tac) const {return toB (tac.z);}
+
+const SpatialVec& getZPlus(const SBTreeAccelerationCache& tac) const {return fromB(tac.zPlus);}
+SpatialVec& updZPlus(SBTreeAccelerationCache& tac) const {return toB (tac.zPlus);}
+
+
+const SpatialMat& getY(const SBDynamicsCache& dc) const {return fromB(dc.Y);}
+SpatialMat& updY(SBDynamicsCache& dc) const {return toB (dc.Y);}
+
+ // ACCELERATION INFO
+
+// Extract from the cache A_GB, the spatial acceleration of this body's frame B measured in and
+// expressed in ground. This contains the inertial angular acceleration of B in G, and the
+// linear acceleration of B's origin point OB in G, with both vectors expressed in G.
+const SpatialVec& getA_GB (const SBTreeAccelerationCache& ac) const {return fromB(ac.bodyAccelerationInGround);}
+SpatialVec& updA_GB (SBTreeAccelerationCache& ac) const {return toB (ac.bodyAccelerationInGround);}
+
+const SpatialVec& getSpatialAcc (const SBTreeAccelerationCache& ac) const {return getA_GB(ac);}
+const Vec3& getSpatialAngAcc(const SBTreeAccelerationCache& ac) const {return getA_GB(ac)[0];}
+const Vec3& getSpatialLinAcc(const SBTreeAccelerationCache& ac) const {return getA_GB(ac)[1];}
+
+
+
+// These are called just after new state variables are allocated,
+// in case there are any node-specific default values.
+// We can handle the "body" variables (like mass) here, but we have to forward the
+// request to the mobilizers to handle their own variables. At the Position
+// stage, for example, mobilizers which use quaternions will set the default ball
+// joint q's to 1,0,0,0.
+// Most of these will use the default implementations here, i.e. do nothing.
+
+// Called after Model variables are allocated by realizeTopology()
+void setNodeDefaultModelValues(const SBTopologyCache& tc, SBModelVars& mv) const {
+ // no body model variables at the moment; TODO: should forward to Body type
+ setMobilizerDefaultModelValues(tc, mv);
+}
+
+// All the rest are called right after realizeModel() since that's when all the
+// remaining state variables are allocated.
+void setNodeDefaultInstanceValues(const SBModelVars& mv, SBInstanceVars& iv) const {
+ // mass properties, inb and outb frame are handled here
+ toB(iv.bodyMassProperties) = getMassProperties_OB_B();
+ toB(iv.outboardMobilizerFrames) = getX_BM();
+ toB(iv.inboardMobilizerFrames) = getX_PF();
+ setMobilizerDefaultInstanceValues(mv,iv);
+}
+void setNodeDefaultTimeValues(const SBModelVars& mv, SBTimeVars& tv) const {
+ // no body model variables at the moment; TODO: should forward to Body type
+ setMobilizerDefaultTimeValues(mv, tv);
+}
+void setNodeDefaultPositionValues(const SBModelVars& mv, Vector& q) const {
+ // no body model variables at the moment; TODO: should forward to Body type
+ setMobilizerDefaultPositionValues(mv, q);
+}
+void setNodeDefaultVelocityValues(const SBModelVars& mv, Vector& u) const {
+ // no body model variables at the moment; TODO: should forward to Body type
+ setMobilizerDefaultVelocityValues(mv, u);
+}
+void setNodeDefaultDynamicsValues(const SBModelVars& mv, SBDynamicsVars& dv) const {
+ // no body model variables at the moment; TODO: should forward to Body type
+ setMobilizerDefaultDynamicsValues(mv, dv);
+}
+void setNodeDefaultAccelerationValues(const SBModelVars& mv, SBAccelerationVars& av) const {
+ // no body model variables at the moment; TODO: should forward to Body type
+ setMobilizerDefaultAccelerationValues(mv, av);
+}
+
+
+// Calculate kinetic energy (from spatial quantities only).
+Real calcKineticEnergy(
+ const SBTreePositionCache& pc,
+ const SBTreeVelocityCache& vc) const;
+
+// Calculate all spatial configuration quantities, assuming availability of
+// joint-specific relative quantities.
+void calcJointIndependentKinematicsPos(
+ SBTreePositionCache& pc) const;
+
+// Calcluate all spatial velocity quantities, assuming availability of
+// joint-specific relative quantities and all position kinematics.
+void calcJointIndependentKinematicsVel(
+ const SBTreePositionCache& pc,
+ SBTreeVelocityCache& vc) const;
+
+// Calculate velocity-dependent quantities which will be needed for
+// computing accelerations.
+void calcJointIndependentDynamicsVel(
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache& abc,
+ const SBTreeVelocityCache& vc,
+ SBDynamicsCache& dc) const;
+
+
+protected:
+// This is the constructor for the abstract base type for use by the derived
+// concrete types in their constructors.
+RigidBodyNode(const MassProperties& mProps_B,
+ const Transform& xform_PF,
+ const Transform& xform_BM,
+ QDotHandling qdotType,
+ QuaternionUse quatUse,
+ bool reverse=false)
+ : parent(0), children(), level(-1),
+ massProps_B(mProps_B),
+ inertia_CB_B(mProps_B.isFinite()
+ ? mProps_B.calcCentralInertia()
+ : (mProps_B.isInf() ? Inertia(Infinity) : Inertia())),
+ X_BM(xform_BM), X_PF(xform_PF), X_MB(~xform_BM),
+ qdotHandling(qdotType), quaternionUse(quatUse), reversed(reverse)
+{
+ // If a quaternion might be used, it can't possibly be true that qdot is
+ // always the same as u.
+ assert(!(quatUse==QuaternionMayBeUsed && qdotType==QDotIsAlwaysTheSameAsU));
+}
+
+// These have wrappers above which call them and should not be
+// called directly from outside this class and its descendents. Note that
+// the requests are couched in terms of the way the mobilizer was
+// *defined*, not as it is after reversal (hence F0 instead of F and
+// M0 instead of M). The q's and u's have identical meanings for both
+// forward and reverse mobilizers.
+virtual void setQToFitTransformImpl
+ (const SBStateDigest&, const Transform& X_F0M0, Vector& q) const = 0;
+virtual void setQToFitRotationImpl
+ (const SBStateDigest&, const Rotation& R_F0M0, Vector& q) const = 0;
+virtual void setQToFitTranslationImpl
+ (const SBStateDigest&, const Vec3& p_F0M0, Vector& q) const = 0;
+
+virtual void setUToFitVelocityImpl
+ (const SBStateDigest&, const Vector& q, const SpatialVec& V_F0M0, Vector& u) const = 0;
+virtual void setUToFitAngularVelocityImpl
+ (const SBStateDigest&, const Vector& q, const Vec3& w_F0M0, Vector& u) const = 0;
+virtual void setUToFitLinearVelocityImpl
+ (const SBStateDigest&, const Vector& q, const Vec3& v_F0M0, Vector& u) const = 0;
+
+typedef Array_<RigidBodyNode*> RigidBodyNodeList;
+
+QIndex qIndex; // index into internal coord pos array
+UIndex uIndex; // index into internal coord vel,acc arrays
+USquaredIndex uSqIndex; // index into array of DOF^2 objects
+QuaternionPoolIndex quaternionIndex; // if this mobilizer has a quaternion, this is our slot
+
+RigidBodyNode* parent;
+RigidBodyNodeList children;
+int level; //how far from base
+MobilizedBodyIndex nodeNum; //unique ID number in SimbodyMatterSubsystemRep
+
+// These are the default body properties, all supplied or calculated on
+// construction. TODO: they should be
+// (optionally?) overrideable by Instance-level state variable entries.
+
+// This is the mass, center of mass, and inertia as supplied at construction.
+// Here the inertia is taken about the B origin OB.
+const MassProperties massProps_B;
+
+// This is the supplied inertia, shifted to the center of mass. It is still
+// a constant expressed in B, but is taken about the COM.
+const Inertia inertia_CB_B;
+
+// Orientation and location of inboard mobilizer frame M, measured
+// and expressed in body frame B.
+const Transform X_BM;
+const Transform X_MB; // inverse of X_BM, calculated on construction
+
+// This is set when we attach this node to its parent in the tree. This is the
+// configuration of the parent's outboard mobilizer attachment frame corresponding
+// to body B (F) measured from and expressed in the parent frame P. It is
+// a constant in frame P. TODO: make it parameterizable.
+const Transform X_PF;
+
+// Concrete RigidBodyNodes should set this flag on construction to indicate whether they can guarantee
+// that their mobilizer's qdots are just the generalized speeds u, for all possible
+// modeling options. That is the same as saying nq=nu and qdot=u, or that the block of the kinematic
+// matrix N corresponding to this node is an nuXnu identity matrix; NInv is the same; and
+// NDot is nuXnu zero. These conditions allows the use of default implementations of many
+// methods which would otherwise have to be overridden. The default implementations assert
+// that qdotHandling==QDotIsAlwaysTheSameAsU.
+const QDotHandling qdotHandling;
+
+// Concrete RigidBodyNodes should set this flag on construction to indicate whether they
+// can guarantee never to use a quaternion in their generailized coordinates for any set
+// of modeling options.
+const QuaternionUse quaternionUse;
+
+// This is true if the mobilizer is specified in the reverse direction,
+// that is from child to parent.
+const bool reversed;
+
+
+};
+
+#endif // SimTK_SIMBODY_RIGID_BODY_NODE_H_
diff --git a/Simbody/src/RigidBodyNodeSpec.cpp b/Simbody/src/RigidBodyNodeSpec.cpp
new file mode 100644
index 0000000..583efbe
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec.cpp
@@ -0,0 +1,880 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Derived from IVM code written by Charles Schwieters *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This file contains implementations of the base class methods for the
+ * templatized class RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>, and instantiations of the
+ * class for all possible values of the arguments.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "RigidBodyNodeSpec.h"
+
+#include "RigidBodyNodeSpec_Pin.h"
+#include "RigidBodyNodeSpec_Slider.h"
+#include "RigidBodyNodeSpec_Ball.h"
+#include "RigidBodyNodeSpec_Free.h"
+#include "RigidBodyNodeSpec_Custom.h"
+
+//==============================================================================
+// CALC H_PB_G
+//==============================================================================
+// Same for all mobilizers.
+// CAUTION: our H matrix definition is transposed from Jain and Schwieters.
+// Cost: 60 + 45*dof flops
+template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
+RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::calcParentToChildVelocityJacobianInGround(
+ const SBModelVars& mv,
+ const SBTreePositionCache& pc,
+ HType& H_PB_G) const
+{
+ const HType& H_FM = getH_FM(pc);
+
+ // We want R_GF so we can reexpress the cross-joint velocity V_FB (==V_PB)
+ // in the ground frame, to get V_PB_G.
+
+ const Rotation& R_PF = getX_PF().R(); // fixed config of F in P
+
+ // Calculated already since we're going base to tip.
+ const Rotation& R_GP = getX_GP(pc).R(); // parent orientation in ground
+ const Rotation R_GF = (noR_PF ? R_GP : R_GP * R_PF); // 45 flops
+
+ if (noX_MB || noR_FM)
+ H_PB_G = R_GF * H_FM; // 3*dof flops
+ else {
+ // want r_MB_F, that is, the vector from Mo to Bo, expressed in F
+ const Vec3& r_MB = getX_MB().p(); // fixed
+ const Rotation& R_FM = getX_FM(pc).R(); // just calculated
+ const Vec3 r_MB_F = (noR_FM ? r_MB : R_FM*r_MB); // 15 flops
+ HType H_MB_F;
+ H_MB_F[0] = Vec3(0); // fills top row with zero
+ H_MB_F[1] = -r_MB_F % H_FM[0]; // 9*dof (negation not actually done)
+ H_PB_G = R_GF * (H_FM + H_MB_F); // 36*dof flops
+ }
+}
+
+//==============================================================================
+// CALC H_PB_G_DOT
+//==============================================================================
+// Same for all mobilizers.
+// CAUTION: our H matrix definition is transposed from Jain and Schwieters.
+// Cost is 69 + 65*dof flops
+template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
+RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::
+calcParentToChildVelocityJacobianInGroundDot(
+ const SBModelVars& mv,
+ const SBTreePositionCache& pc,
+ const SBTreeVelocityCache& vc,
+ HType& HDot_PB_G) const
+{
+ const HType& H_FM = getH_FM(pc);
+ const HType& HDot_FM = getHDot_FM(vc);
+
+ // We want R_GF so we can reexpress the cross-joint velocity V_FB (==V_PB)
+ // in the ground frame, to get V_PB_G.
+
+ const Rotation& R_PF = getX_PF().R(); // fixed config of F in P
+
+ // Calculated already since we're going base to tip.
+ const Rotation& R_GP = getX_GP(pc).R(); // parent orientation in ground
+ const Rotation R_GF = (noR_PF ? R_GP : R_GP * R_PF); // 45 flops (TODO: again??)
+
+ const Vec3& w_GF = getV_GP(vc)[0]; // F and P have same angular velocity
+
+ // Note: time derivative of R_GF is crossMat(w_GF)*R_GF.
+ // H = H_PB_G = R_GF * (H_FM + H_MB_F) (see above method)
+ const HType& H_PB_G = getH(pc);
+ if (noX_MB || noR_FM)
+ HDot_PB_G = R_GF * HDot_FM // 48*dof
+ + HType(w_GF % H_PB_G[0],
+ w_GF % H_PB_G[1]);
+ else {
+ // want r_MB_F, that is, the vector from OM to OB, expressed in F
+ const Vec3& r_MB = getX_MB().p(); // fixed
+ const Rotation& R_FM = getX_FM(pc).R(); // just calculated
+ const Vec3 r_MB_F = (noR_FM ? r_MB : R_FM*r_MB); // 15 flops
+
+ const Vec3& w_FM = getV_FM(vc)[0]; // local angular velocity
+
+ HType HDot_MB_F;
+ HDot_MB_F[0] = Vec3(0);
+ HDot_MB_F[1] = -r_MB_F % HDot_FM[0] // 21*dof + 9 flops
+ - (w_FM % r_MB_F) % H_FM[0];
+
+
+ HDot_PB_G = R_GF * (HDot_FM + HDot_MB_F) // 54*dof
+ + HType(w_GF % H_PB_G[0],
+ w_GF % H_PB_G[1]);
+ }
+}
+
+//==============================================================================
+// CALC REVERSE MOBILIZER H_FM
+//==============================================================================
+// This is the default implementation for turning H_MF into H_FM.
+// A mobilizer can override this to do it faster.
+// From the Simbody theory manual,
+// H_FM_w = - R_FM*H_MF_w
+// H_FM_v = -(R_FM*H_MF_v + p_FM x H_FM_w)
+//
+template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
+RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::calcReverseMobilizerH_FM(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+{
+ // Must use "upd" here rather than "get" because this is
+ // called during realize(Position).
+ const SBTreePositionCache& pc = sbs.updTreePositionCache();
+
+ HType H_MF;
+ calcAcrossJointVelocityJacobian(sbs, H_MF);
+
+ const Rotation& R_FM = getX_FM(pc).R();
+ const Vec3& p_FM = getX_FM(pc).p();
+
+ // Make cross product matrix (3 flops). Saves a few flops (3*dof) to
+ // transpose this and get the negative of the cross product matrix.
+ const Mat33 np_FM_x = ~crossMat(p_FM);
+
+ if (noR_FM) {
+ H_FM[0] = -H_MF[0];
+ H_FM[1] = np_FM_x * H_FM[0] - H_MF[1]; // 15*dof flops
+ }
+ else {
+ H_FM[0] = -(R_FM * H_MF[0]); // 18*dof flops
+ H_FM[1] = np_FM_x * H_FM[0] - R_FM*H_MF[1]; // 33*dof flops
+ }
+}
+
+//==============================================================================
+// CALC REVERSE MOBILIZER HDOT_FM
+//==============================================================================
+// This is the default implementation for turning HDot_MF into HDot_FM.
+// A mobilizer can override this to do it faster.
+// We depend on H_FM having already been calculated.
+//
+// From the Simbody theory manual,
+// HDot_FM_w = -R_FM * HDot_MF_w + w_FM_x H_FM_w
+//
+// HDot_FM_v = -R_FM * HDot_MF_v + w_FM_x H_FM_v
+// - (p_FM_x HDot_FM_w
+// + (v_FM_x - w_FM_x p_FM_x)H_FM_w)
+//
+// where "a_x" indicates the cross product matrix of vector a.
+//
+template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
+RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::calcReverseMobilizerHDot_FM(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+{
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ // Must use "upd" here rather than "get" because this is
+ // called during realize(Velocity).
+ const SBTreeVelocityCache& vc = sbs.updTreeVelocityCache();
+
+ HType HDot_MF;
+ calcAcrossJointVelocityJacobianDot(sbs, HDot_MF);
+
+ const Rotation& R_FM = getX_FM(pc).R();
+ const Vec3& p_FM = getX_FM(pc).p();
+ const HType& H_FM = getH_FM(pc);
+
+ const Vec3& w_FM = getV_FM(vc)[0];
+ const Vec3& v_FM = getV_FM(vc)[1];
+
+ // Make cross product matrices.
+ const Mat33 p_FM_x = crossMat(p_FM); // 3 flops
+ const Mat33 w_FM_x = crossMat(w_FM); // 3 flops
+ const Mat33 v_FM_x = crossMat(v_FM); // 3 flops
+ const Mat33 vwp = v_FM_x - w_FM_x*p_FM_x; // 54 flops
+
+ // Initialize both rows with the first two terms above.
+ HDot_FM = w_FM_x*H_FM - (noR_FM ? HDot_MF : R_FM*HDot_MF); // 66*dof flops
+
+ // Add in the additional terms in the second row.
+ HDot_FM[1] -= p_FM_x * HDot_FM[0] + vwp * H_FM[0]; // 36*dof flops
+}
+
+
+//==============================================================================
+// REALIZE ARTICULATED BODY INERTIAS
+//==============================================================================
+// Compute articulated body inertia and related quantities for this body B.
+// This must be called tip-to-base (inward).
+//
+// Given only position-related quantities from the State
+// Mk (this body's spatial inertia matrix)
+// Phi (composite body child-to-parent shift matrix)
+// H (joint transition matrix; sense is transposed from Jain & Schwieters)
+// we calculate dynamic quantities
+// P (articulated body inertia)
+// PPlus (articulated body inertia as seen through the mobilizer)
+// For a prescribed mobilizer, we have PPlus==P. Otherwise we also compute
+// D (factored mass matrix LDL' diagonal part D=~H*P*H)
+// DI (inverse of D)
+// G (P * H * DI)
+// PPlus (P - P * H * DI * ~H * P = P - G * ~H * P)
+// and put them in the state cache.
+//
+// This is Algorithm 6.1 on page 106 of Jain's 2011 book modified to accommodate
+// prescribed motion as described on page 323. Note that although P+ is "as
+// felt" on the inboard side of the mobilizer it is still calculated about
+// Bo just as is P.
+//
+// Cost is 93 flops per child plus
+// n^3 + 23*n^2 + 115*n + 12
+// e.g. pin=143, ball=591 (197/dof), free=1746 (291/dof)
+// Note that per-child cost is paid just once for each non-base body in
+// the whole tree; that is, each body is touched just once.
+template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
+RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::
+realizeArticulatedBodyInertiasInward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ SBArticulatedBodyInertiaCache& abc) const
+{
+ ArticulatedInertia& P = updP(abc);
+
+ // Start with the spatial inertia of the current body (in Ground frame).
+ P = ArticulatedInertia(getMk_G(pc)); // 12 flops
+
+ // For each child, we previously took its articulated body inertia P and
+ // removed the portion of that inertia that can't be felt from the parent
+ // because of the joint mobilities. That is, we calculated
+ // P+ = P - P H DI ~H P, where the second term is the projection of P into
+ // the mobility space of the child's inboard mobilizer. Note that if the
+ // child's mobilizer is prescribed, then the entire inertia will be felt
+ // by the parent so P+ = P in that case. Now we're going to shift P+
+ // from child to parent: Pparent += Phi*P+*~Phi.
+ // TODO: can this be optimized for the common case where the
+ // child is a terminal body and hence its P is an ordinary
+ // spatial inertia? (Spatial inertia shift is 37 flops vs. 72 here; not
+ // really much help.)
+ for (unsigned i=0; i<children.size(); ++i) {
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+ const ArticulatedInertia& PPlusChild = children[i]->getPPlus(abc);
+
+ // Apply the articulated body shift.
+ // This takes 93 flops (72 for the shift and 21 to add it in).
+ // (Note that PPlusChild==PChild if child's mobilizer is prescribed.)
+ P += PPlusChild.shift(phiChild.l());
+ }
+
+ // Now compute PPlus. P+ = P for a prescribed mobilizer. Otherwise
+ // it is P+ = P - P H DI ~H P = P - G*~PH. In the prescribed case
+ // we leave G, D, DI untouched -- they should have been set to NaN at
+ // Instance stage.
+ ArticulatedInertia& PPlus = updPPlus(abc);
+
+ if (isUDotKnown(ic)) {
+ PPlus = P; // prescribed
+ return;
+ }
+
+ // This is a non-prescribed mobilizer. Compute D, DI, G then P+.
+
+ const HType& H = getH(pc);
+ HType& G = updG(abc);
+ Mat<dof,dof>& D = updD(abc);
+ Mat<dof,dof>& DI = updDI(abc);
+
+ const HType PH = P*H; // 66*dof flops
+ D = ~H * PH; // 11*dof^2 flops (symmetric result)
+
+ // this will throw an exception if the matrix is ill conditioned
+ DI = D.invert(); // ~dof^3 flops (symmetric)
+ G = PH * DI; // 12*dof^2-6*dof flops
+
+ // Want P+ = P - G*~PH. We can do this in about 55*dof flops.
+ // These require 9 dot products of length dof. The symmetric ones could
+ // be done with 6 dot products instead for a small savings but this gives
+ // us a chance to symmetrize and hopefully clean up some numerical errors.
+ // The full price for all three is 54*dof-27 flops.
+ Mat33 massMoment = G.row(0)*~PH.row(1); // (full) 9*(2*dof-1) flops
+ Mat33 mass = G.row(1)*~PH.row(1); // symmetric result 9*(2*dof-1) flops
+ Mat33 inertia = G.row(0)*~PH.row(0); // symmetric result 9*(2*dof-1) flops
+ // These must be symmetrized due to numerical errors for 12 more flops.
+ SymMat33 symMass( mass(0,0),
+ (mass(1,0)+mass(0,1))/2, mass(1,1),
+ (mass(2,0)+mass(0,2))/2, (mass(2,1)+mass(1,2))/2, mass(2,2));
+ SymMat33 symInertia(
+ inertia(0,0),
+ (inertia(1,0)+inertia(0,1))/2, inertia(1,1),
+ (inertia(2,0)+inertia(0,2))/2, (inertia(2,1)+inertia(1,2))/2, inertia(2,2));
+ PPlus = P - ArticulatedInertia(symMass, massMoment, symInertia); // 21 flops
+}
+
+
+
+//==============================================================================
+// CALC UDOT
+//==============================================================================
+// To be called from tip to base.
+// Temps do not need to be initialized.
+//
+// First pass
+// ----------
+// Given previously-calculated quantities from the State
+// P this body's articulated body inertia
+// Phi composite body child-to-parent shift matrix
+// H joint transition matrix; sense is transposed from Jain
+// G P * H * DI
+// and supplied arguments
+// F body force applied to B
+// f generalize forces applied to B's mobilities
+// udot_p known udots (if mobilizer is prescribed)
+// calculate
+// z articulated body residual force on B
+// zPlus AB residual force as felt on inboard side of mobilizer
+// eps f - ~H*z gen force plus gen equiv of body forces
+// For a prescribed mobilizer we have zPlus=z. Otherwise, zPlus = z + G*eps.
+//
+// This is the first (inward) loop of Algorithm 16.2 on page 323 of Jain's
+// 2011 book, modified to include the applied body force and not including
+// the auxiliary quantity "nu=DI*eps".
+template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
+RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::calcUDotPass1Inward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache& abc,
+ const SBDynamicsCache& dc,
+ const Real* jointForces,
+ const SpatialVec* bodyForces,
+ const Real* allUDot,
+ SpatialVec* allZ,
+ SpatialVec* allZPlus,
+ Real* allEpsilon) const
+{
+ const Vec<dof>& f = fromU(jointForces);
+ const SpatialVec& F = bodyForces[nodeNum];
+ SpatialVec& z = allZ[nodeNum];
+ SpatialVec& zPlus = allZPlus[nodeNum];
+ Vec<dof>& eps = toU(allEpsilon);
+
+ const bool isPrescribed = isUDotKnown(ic);
+ const HType& H = getH(pc);
+ const ArticulatedInertia& P = getP(abc);
+ const HType& G = getG(abc);
+
+ // z = Pa+b - F
+ z = getMobilizerCentrifugalForces(dc) - F; // 6 flops
+
+ // z += P H udot_p if prescribed
+ if (isPrescribed && !isUDotKnownToBeZero(ic)) {
+ const Vec<dof>& udot = fromU(allUDot);
+ z += P*(H*udot); // 66+12*dof flops
+ }
+
+ // z += sum(Phi(child) * zPlus(child)) for all children
+ for (unsigned i=0; i<children.size(); ++i) {
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+ const SpatialVec& zPlusChild = allZPlus[children[i]->getNodeNum()];
+ z += phiChild * zPlusChild; // 18 flops
+ }
+
+ eps = f - ~H*z; // 12*dof flops
+
+ zPlus = z;
+ if (!isPrescribed)
+ zPlus += G*eps; // 12*dof flops
+}
+
+//
+// Calculate acceleration in internal coordinates, based on the last set
+// of forces that were reduced into epsilon (e.g., see above).
+// Base to tip: temp allA_GB does not need to be initialized before
+// beginning the iteration.
+//
+template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
+RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::calcUDotPass2Outward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache& abc,
+ const SBTreeVelocityCache& vc,
+ const SBDynamicsCache& dc,
+ const Real* allEpsilon,
+ SpatialVec* allA_GB,
+ Real* allUDot,
+ Real* allTau) const
+{
+ const Vec<dof>& eps = fromU(allEpsilon);
+ SpatialVec& A_GB = allA_GB[nodeNum];
+ Vec<dof>& udot = toU(allUDot); // pull out this node's udot
+
+ const bool isPrescribed = isUDotKnown(ic);
+ const HType& H = getH(pc);
+ const PhiMatrix& phi = getPhi(pc);
+ const ArticulatedInertia& P = getP(abc);
+ const SpatialVec& a = getMobilizerCoriolisAcceleration(vc);
+ const Mat<dof,dof>& DI = getDI(abc);
+ const HType& G = getG(abc);
+
+ // Shift parent's acceleration outward (Ground==0). 12 flops
+ const SpatialVec& A_GP = allA_GB[parent->getNodeNum()];
+ const SpatialVec APlus = ~phi * A_GP;
+
+ if (isPrescribed) {
+ Vec<dof>& tau = updTau(ic,allTau); // pull out this node's tau
+ // This is f - ~H(P*APlus + z); compare Jain 16.17b. Note sign
+ // change since our tau is on the LHS while his is on the RHS.
+ tau = eps - ~H*(P*APlus); // 66 + 12*dof flops
+ } else {
+ udot = DI*eps - ~G*APlus; // 2*dof^2 + 11*dof
+ }
+
+ A_GB = APlus + H*udot + a;
+}
+
+
+//==============================================================================
+// CALC M INVERSE F
+//==============================================================================
+// Temps do not need to be initialized.
+//
+// This calculates udot = M^-1 f in two O(n) passes. Note that we are ignoring
+// velocities; if there are any velocity-dependent forces you care about, they
+// should already be in f.
+//
+// When there is prescribed motion, it is already reflected in the articulated
+// body inertias, which were formed with rigid body shifts across the prescribed
+// mobilizers. In that case you can think of the udots and generalized forces f
+// as partitioned into two sets each: udot={udot_r, udot_p}, f={f_r, f_p}. We
+// can discuss them as though the partitions were contiguous although they are
+// not and the code deals with that properly. So now you can view the system as
+// [ Mrr Mrp ] [udot_r] [ 0 ] [f_r]
+// [~Mrp Mpp ] [udot_p] + [tau_p] = [f_p]
+// where udot_p, f_r, and f_p are given and the unknowns are udot_r (the free
+// accelerations) and tau_p (the unknown forces that enforce the prescribed
+// motion). This produces two equations when multiplied out:
+// (1) Mrr udot_r = f_r - Mrp udot_p
+// (2) tau_p = f_p - ~Mrp udot_r - Mpp udot_p
+//
+// Now we can define what the present method does: it returns
+// [udot_r] [udot_r] [Mrr^-1 0] [f_r]
+// [udot_p] = [ 0 ] = [ 0 0] [f_p]
+// f_p is ignored and won't be examined; udot_p will be set to zero on return.
+//
+// Cost per body is
+// 30 + 47*ndof_r + 2*ndof_r^2
+// where ndof_r is the number of u's for a regular (non-prescribed) mobilizer;
+// 0 for a prescribed one.
+
+// Pass 1, to be called from tip to base.
+template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
+RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::multiplyByMInvPass1Inward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache& abc,
+ const Real* jointForces,
+ SpatialVec* allZ,
+ SpatialVec* allZPlus,
+ Real* allEpsilon) const
+{
+ const Vec<dof>& f = fromU(jointForces);
+ SpatialVec& z = allZ[nodeNum];
+ SpatialVec& zPlus = allZPlus[nodeNum];
+ Vec<dof>& eps = toU(allEpsilon);
+
+ const bool isPrescribed = isUDotKnown(ic);
+ const HType& H = getH(pc);
+ const HType& G = getG(abc);
+
+ z = 0;
+
+ for (unsigned i=0; i<children.size(); i++) {
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+ const SpatialVec& zPlusChild = allZPlus[children[i]->getNodeNum()];
+ z += phiChild * zPlusChild; // 18 flops
+ }
+
+ zPlus = z;
+ if (!isPrescribed) {
+ eps = f - ~H*z; // 12*dof flops
+ zPlus += G*eps; // 12*dof flops
+ }
+}
+
+
+// Pass 2 of multiplyByMInv.
+// Base to tip: temp allA_GB does not need to be initialized before
+// beginning the iteration.
+template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
+RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::multiplyByMInvPass2Outward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache& abc,
+ const Real* allEpsilon,
+ SpatialVec* allA_GB,
+ Real* allUDot) const
+{
+ const Vec<dof>& eps = fromU(allEpsilon);
+ SpatialVec& A_GB = allA_GB[nodeNum];
+ Vec<dof>& udot = toU(allUDot); // pull out this node's udot
+
+ const bool isPrescribed = isUDotKnown(ic);
+ const HType& H = getH(pc);
+ const PhiMatrix& phi = getPhi(pc);
+ const Mat<dof,dof>& DI = getDI(abc);
+ const HType& G = getG(abc);
+
+ // Shift parent's acceleration outward (Ground==0). 12 flops
+ const SpatialVec& A_GP = allA_GB[parent->getNodeNum()];
+ const SpatialVec APlus = ~phi * A_GP;
+
+ // For a prescribed mobilizer, set udot==0.
+ if (isPrescribed) {
+ udot = 0;
+ A_GB = APlus;
+ } else {
+ udot = DI*eps - ~G*APlus; // 2dof^2 + 11 dof flops
+ A_GB = APlus + H*udot; // 12 dof flops
+ }
+}
+
+
+
+//==============================================================================
+// CALC BODY ACCELERATIONS FROM UDOT
+//==============================================================================
+// This method calculates:
+// A_GB = J*udot + Jdot*u
+// in O(N) time, where udot is supplied as an argument and Jdot*u is the
+// coriolis acceleration which we get from the velocity cache. This also serves
+// as pass 1 for inverse dynamics.
+//
+// This must be called base to tip. The cost is 12*dof + 18 flops.
+template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
+RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::
+calcBodyAccelerationsFromUdotOutward(
+ const SBTreePositionCache& pc,
+ const SBTreeVelocityCache& vc,
+ const Real* allUDot,
+ SpatialVec* allA_GB) const
+{
+ const Vec<dof>& udot = fromU(allUDot);
+ SpatialVec& A_GB = allA_GB[nodeNum];
+
+ // Shift parent's A_GB outward. (Ground A_GB is zero.) 12 flops.
+ const SpatialVec A_GP = ~getPhi(pc) * allA_GB[parent->getNodeNum()];
+
+ // 12*dof+6 flops.
+ A_GB = A_GP + getH(pc)*udot + getMobilizerCoriolisAcceleration(vc);
+}
+
+
+
+//==============================================================================
+// CALC INVERSE DYNAMICS
+//==============================================================================
+// This algorithm calculates
+// f = M*udot + C(u) - f_applied
+// in O(N) time, where C(u) are the velocity-dependent coriolis, centrifugal and
+// gyroscopic forces, f_applied are the applied body and joint forces, and udot
+// is given.
+//
+// Pass 1 is base to tip and is just a calculation of body accelerations arising
+// from udot and the coriolis accelerations. It is embodied in the reusable
+// calcBodyAccelerationsFromUdotOutward() method above.
+//
+// Pass 2 is tip to base. It takes body accelerations from pass 1 (including coriolis
+// accelerations as well as hinge accelerations), and the applied forces,
+// and calculates the additional hinge forces that would be necessary to produce
+// the observed accelerations.
+//
+// Costs for inverse dynamics include both passes:
+// pass1: 12*dof + 18 flops
+// pass2: 12*dof + 75 flops
+// -----------------
+// total: 24*dof + 93 flops
+template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
+RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::
+calcInverseDynamicsPass2Inward(
+ const SBTreePositionCache& pc,
+ const SBTreeVelocityCache& vc,
+ const SpatialVec* allA_GB,
+ const Real* jointForces,
+ const SpatialVec* bodyForces,
+ SpatialVec* allF, // temp
+ Real* allTau) const
+{
+ const Vec<dof>& myJointForce = fromU(jointForces);
+ const SpatialVec& myBodyForce = bodyForces[nodeNum];
+ const SpatialVec& A_GB = allA_GB[nodeNum];
+ SpatialVec& F = allF[nodeNum];
+ Vec<dof>& tau = toU(allTau);
+
+ // Start with rigid body force from desired body acceleration and
+ // gyroscopic forces due to angular velocity, minus external forces
+ // applied directly to this body.
+ F = getMk_G(pc)*A_GB + getGyroscopicForce(vc) - myBodyForce; // 57 flops
+
+ // Add in forces on children, shifted to this body.
+ for (unsigned i=0; i<children.size(); ++i) {
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+ const SpatialVec& FChild = allF[children[i]->getNodeNum()];
+ F += phiChild * FChild; // 18 flops
+ }
+
+ // Project body forces into hinge space and subtract any hinge forces already
+ // being applied to get the remaining hinge forces needed.
+ tau = ~getH(pc)*F - myJointForce; // 12*dof flops
+}
+
+
+
+//==============================================================================
+// CALC M V
+//==============================================================================
+// The next two methods calculate x=M*v (or f=M*a) in O(N) time, given v.
+// The first one is called base to tip.
+// Cost is: pass1 12*dof+12 flops
+// pass2 11*dof+63 flops
+// total 23*dof + 75
+// TODO: Possibly this could be much faster if composite body inertias R were
+// precalculated along with D = ~H*R*H. Then I *think* this could be a single
+// pass with tau = D*udot. Whether this is worth it would depend on how much
+// this gets re-used after R and D are calculated.
+template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
+RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::multiplyByMPass1Outward(
+ const SBTreePositionCache& pc,
+ const Real* allUDot,
+ SpatialVec* allA_GB) const
+{
+ const Vec<dof>& udot = fromU(allUDot);
+ SpatialVec& A_GB = allA_GB[nodeNum];
+
+ // Shift parent's A_GB outward. (Ground A_GB is zero.)
+ const SpatialVec A_GP = ~getPhi(pc) * allA_GB[parent->getNodeNum()]; // 12 flops
+
+ A_GB = A_GP + getH(pc)*udot; // 12*dof flops
+}
+
+// Call tip to base after calling multiplyByMPass1Outward() for each
+// rigid body.
+template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
+RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::multiplyByMPass2Inward(
+ const SBTreePositionCache& pc,
+ const SpatialVec* allA_GB,
+ SpatialVec* allF, // temp
+ Real* allTau) const
+{
+ const SpatialVec& A_GB = allA_GB[nodeNum];
+ SpatialVec& F = allF[nodeNum];
+ Vec<dof>& tau = toU(allTau);
+
+ // 45 flops because Mk has a nice structure
+ F = getMk_G(pc)*A_GB;
+
+ for (unsigned i=0; i<children.size(); ++i) {
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+ const SpatialVec& FChild = allF[children[i]->getNodeNum()];
+ F += phiChild * FChild; // 18 flops
+ }
+
+ tau = ~getH(pc)*F; // 11*dof flops
+}
+
+
+
+//==============================================================================
+// REALIZE Y
+//==============================================================================
+// To be called base to tip.
+// This is calculating what Abhi Jain calls the operational space compliance
+// kernel in his 2011 book. This is the inverse of the operational space inertia
+// for each body at its body frame. Also, see Equation 20 in Rodriguez,Jain,
+// & Kreutz-Delgado: A spatial operator algebra
+// for manipulator modeling and control. Intl. J. Robotics Research
+// 10(4):371-381 (1991).
+template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
+RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::realizeYOutward
+ (const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache& abc,
+ SBDynamicsCache& dc) const
+{
+ if (isUDotKnown(ic)) {
+ //TODO: (sherm 090810) is this right?
+ assert(false);
+ //updY(dc) = (~getPhi(pc) * parent->getY(dc)) * getPhi(pc); // rigid shift
+ return;
+ }
+
+ // Compute psi. Jain has TauBar=I-G*~H but we're negating that to G*~H-I
+ // because we can save 30 flops by just subtracting 1 from the diagonals
+ // rather than having to negate all the off-diagonals. Then Psi ends up
+ // with the wrong sign here also, which doesn't matter because we multiply
+ // by it twice.
+
+ SpatialMat tauBar = getG(abc)*~getH(pc);// 11*dof^2 flops
+ tauBar(0,0) -= 1; // subtract identity matrix (only touches diags: 3 flops)
+ tauBar(1,1) -= 1; // " (3 flops)
+ SpatialMat psi = getPhi(pc)*tauBar; // ~100 flops
+
+ // TODO: this is very expensive (~1000 flops?) Could cut be at least half
+ // by exploiting symmetry. Also, does Psi have special structure?
+ // And does this need to be computed for every body or only those
+ // which are loop "base" bodies or some such?
+
+
+ // Psi here has the opposite sign from Jain's, but we're multiplying twice
+ // by it here so it doesn't matter.
+ updY(dc) = (getH(pc) * getDI(abc) * ~getH(pc))
+ + (~psi * parent->getY(dc) * psi);
+}
+
+
+
+//==============================================================================
+// MULTIPLY BY SYSTEM JACOBIAN
+//==============================================================================
+// Calculate product of kinematic Jacobian J=~Phi*H and a mobility-space vector. Requires
+// that Phi and H are available, so this should only be called in Stage::Position or higher.
+// This does not change the cache at all.
+//
+// Note that if the vector v==u (generalized speeds) then the result is V_GB=J*u, the
+// body spatial velocities generated by those speeds.
+//
+// Call base to tip (outward).
+//
+template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
+RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::
+multiplyBySystemJacobian(
+ const SBTreePositionCache& pc,
+ const Real* v,
+ SpatialVec* Jv) const
+{
+ const Vec<dof>& in = fromU(v);
+ SpatialVec& out = Jv[nodeNum];
+
+ // Shift parent's result outward (ground result is 0).
+ const SpatialVec outP = ~getPhi(pc) * Jv[parent->getNodeNum()]; // 12 flops
+
+ out = outP + getH(pc)*in; // 12*dof flops
+}
+
+//==============================================================================
+// MULTIPLY BY SYSTEM JACOBIAN TRANSPOSE
+//==============================================================================
+// Calculate product of kinematic Jacobian transpose ~J=~H*Phi and a spatial
+// forces vector on each of the outboard bodies. Requires that Phi and H are
+// available, so this should only be called in Stage::Position or higher. This
+// does not change the cache at all.
+// NOTE (sherm 060214): I reworked this from the original. This one no longer
+// incorporates applied hinge gradients if there are any; just add those in at
+// the end if you want them.
+//
+// (sherm 060727) In spatial operators, this calculates ~H*Phi*F where F are the
+// spatial forces applied to each body. See Schwieters Eq. 41. (But with sense
+// of H transposed.)
+//
+// Call tip to base.
+//
+template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
+RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::
+multiplyBySystemJacobianTranspose(
+ const SBTreePositionCache& pc,
+ SpatialVec* zTmp,
+ const SpatialVec* X,
+ Real* JtX) const
+{
+ const SpatialVec& in = X[getNodeNum()];
+ Vec<dof>& out = Vec<dof>::updAs(&JtX[getUIndex()]);
+ SpatialVec& z = zTmp[getNodeNum()];
+
+ z = in;
+
+ for (unsigned i=0; i<children.size(); ++i) {
+ const SpatialVec& zChild = zTmp[children[i]->getNodeNum()];
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+
+ z += phiChild * zChild; // 18 flops
+ }
+
+ out = ~getH(pc) * z; // 11*dof flops
+}
+
+//==============================================================================
+// CALC EQUIVALENT JOINT FORCES
+//==============================================================================
+// To be called from tip to base.
+// Temps do not need to be initialized.
+// (sherm 060727) In spatial operators, this calculates ~H*Phi*(F-(Pa+b))
+template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
+RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::calcEquivalentJointForces(
+ const SBTreePositionCache& pc,
+ const SBDynamicsCache& dc,
+ const SpatialVec* bodyForces,
+ SpatialVec* allZ,
+ Real* jointForces) const
+{
+ const SpatialVec& myBodyForce = bodyForces[nodeNum];
+ SpatialVec& z = allZ[nodeNum];
+ Vec<dof>& eps = toU(jointForces);
+
+ // Centrifugal forces are PA+b where P is articulated body inertia,
+ // A is total coriolis acceleration, and b is gyroscopic force.
+ z = myBodyForce - getTotalCentrifugalForces(dc);
+
+ for (unsigned i=0; i<children.size(); ++i) {
+ const SpatialVec& zChild = allZ[children[i]->getNodeNum()];
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+
+ z += phiChild * zChild;
+ }
+
+ eps = ~getH(pc) * z;
+}
+
+//
+// to be called from base to tip.
+//
+template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
+RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::setVelFromSVel(
+ const SBTreePositionCache& pc,
+ const SBTreeVelocityCache& mc,
+ const SpatialVec& sVel,
+ Vector& u) const
+{
+ toU(u) = ~getH(pc) * (sVel - (~getPhi(pc) * parent->getV_GB(mc)));
+}
+
+ ////////////////////
+ // INSTANTIATIONS //
+ ////////////////////
+
+#define INSTANTIATE(dof, noR_FM) \
+template class RigidBodyNodeSpec<dof, noR_FM, false, false>; \
+template class RigidBodyNodeSpec<dof, noR_FM, false, true>; \
+template class RigidBodyNodeSpec<dof, noR_FM, true, false>; \
+template class RigidBodyNodeSpec<dof, noR_FM, true, true>;
+
+INSTANTIATE(1, false)
+INSTANTIATE(2, false)
+INSTANTIATE(3, false)
+INSTANTIATE(4, false)
+INSTANTIATE(5, false)
+INSTANTIATE(6, false)
+INSTANTIATE(1, true)
+INSTANTIATE(3, true)
diff --git a/Simbody/src/RigidBodyNodeSpec.h b/Simbody/src/RigidBodyNodeSpec.h
new file mode 100644
index 0000000..cd911e5
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec.h
@@ -0,0 +1,759 @@
+#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_H_
+#define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * Charles Schwieters (NIH): wrote the public domain IVM code from which *
+ * this was derived. *
+ * Peter Eastman: wrote the Euler Angle<->Quaternion conversion *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This file contains just the templatized class RigidBodyNodeSpec<dof> which
+ * is used as the base class for most of the RigidBodyNodes (that is, the
+ * implementations of Mobilizers). The only exceptions are nodes whose
+ * mobilizers provide no degrees of freedom -- Ground and Weld.
+ *
+ * This file contains all the multibody mechanics method declarations that
+ * involve a single body and its mobilizer (inboard joint), that is, one node
+ * in the multibody tree. These methods constitute the inner loops of the
+ * multibody calculations, and much suffering is undergone here to make them
+ * run fast. In particular most calculations are templatized by the number of
+ * mobilities, so that compile-time sizes are known for everything.
+ *
+ * Most methods here expect to be called in a particular order during traversal
+ * of the tree -- either base to tip or tip to base.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+
+/**
+ * This still-abstract class is a skeleton implementation of a built-in
+ * mobilizer, with the number of mobilities (dofs, u's) specified as a template
+ * parameter. That way all the code that is common except for the dimensionality
+ * of the mobilizer can be written once, and the compiler generates specific
+ * implementations for each of the six possible dimensionalities (1-6
+ * mobilities). Each implementation works only on fixed size Vec<> and Mat<>
+ * types, so can use very high speed inline operators.
+ */
+template<int dof, bool noR_FM=false, bool noX_MB=false, bool noR_PF=false>
+class RigidBodyNodeSpec : public RigidBodyNode {
+public:
+
+RigidBodyNodeSpec(const MassProperties& mProps_B,
+ const Transform& X_PF,
+ const Transform& X_BM,
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot,
+ QDotHandling qdotHandling,
+ QuaternionUse quaternionUse,
+ bool isReversed)
+: RigidBodyNode(mProps_B, X_PF, X_BM,
+ qdotHandling, quaternionUse, isReversed)
+{
+ // don't call any virtual methods in here!
+ uIndex = nextUSlot;
+ uSqIndex = nextUSqSlot;
+ qIndex = nextQSlot;
+}
+
+void updateSlots(UIndex& nextUSlot, USquaredIndex& nextUSqSlot, QIndex& nextQSlot) {
+ // OK to call virtual method here.
+ nextUSlot += getDOF();
+ nextUSqSlot += getDOF()*getDOF();
+ nextQSlot += getMaxNQ();
+}
+
+virtual ~RigidBodyNodeSpec() {}
+
+// This is the type of the joint transition matrix H, but our definition
+// of H is transposed from Jain's or Schwieters'. That is, what we're
+// calling H here is what Jain calls H* and Schwieters calls H^T. So
+// our H matrix is 6 x nu, or more usefully it is 2 rows of Vec3's.
+// The first row define how u's contribute to angular velocities; the
+// second defines how u's contribute to linear velocities.
+typedef Mat<2,dof,Vec3> HType;
+
+// Provide default implementations for setQToFitTransformImpl() and setQToFitVelocityImpl()
+// which are implemented using the rotational and translational quantity routines. These assume
+// that the rotational and translational coordinates are independent, with rotation handled
+// first and then left alone. If a mobilizer type needs to deal with rotation and
+// translation simultaneously, it should provide a specific implementation for these two routines.
+// *Each* mobilizer must implement setQToFit{Rotation,Translation} and
+// setUToFit{AngularVelocity,LinearVelocity}; there are no defaults.
+
+virtual void setQToFitTransformImpl(const SBStateDigest& sbs, const Transform& X_FM, Vector& q) const {
+ setQToFitRotationImpl (sbs,X_FM.R(),q);
+ setQToFitTranslationImpl(sbs,X_FM.p(),q);
+}
+
+virtual void setUToFitVelocityImpl(const SBStateDigest& sbs, const Vector& q, const SpatialVec& V_FM, Vector& u) const {
+ setUToFitAngularVelocityImpl(sbs,q,V_FM[0],u);
+ setUToFitLinearVelocityImpl (sbs,q,V_FM[1],u);
+}
+
+// The following routines calculate joint-specific position kinematic
+// quantities. They may assume that *all* position kinematics (not just
+// joint-specific) has been done for the parent, and that the position
+// state variables q are available. Each routine may also assume that the
+// previous routines have been called, in the order below.
+// The routines are structured as operators -- they use the State but
+// do not change anything in it, including the cache. Instead they are
+// passed arguments to write their results into. In practice, these
+// arguments will typically be in the State cache (see below).
+
+
+// This mandatory routine calculates the joint transition matrix H_FM, giving
+// the change of velocity induced by the generalized speeds u for this
+// mobilizer, expressed in the mobilizer's inboard "fixed" frame F (attached to
+// this body's parent).
+// This method constitutes the *definition* of the generalized speeds for
+// a particular joint.
+// This routine can depend on X_FM having already
+// been calculated and available in the PositionCache but must NOT depend
+// on any quantities involving Ground or other bodies.
+// Note: this calculates the H matrix *as defined*; we might reverse
+// the frames in use. So we call this H_F0M0 while the possibly reversed
+// version is H_FM. Be sure to use X_F0M0 in your calculation for H_F0M0
+// if you need access to the cross-mobilizer transform.
+// CAUTION: our definition of H is transposed from Jain and Schwieters.
+virtual void calcAcrossJointVelocityJacobian(
+ const SBStateDigest& sbs,
+ HType& H_F0M0) const=0;
+
+// This mandatory routine calculates the time derivative taken in F of
+// the matrix H_FM (call it HDot_FM). This is zero if the generalized
+// speeds are all defined in terms of the F frame, which is often the case.
+// This routine can depend on X_FM and H_FM being available already in
+// the state PositionCache, and V_FM being already in the state VelocityCache.
+// However, it must NOT depend on any quantities involving Ground or other bodies.
+// Note: this calculates the HDot matrix *as defined*; we might reverse
+// the frames in use. So we call this HDot_F0M0 while the possibly reversed
+// version is HDot_FM. Be sure to use X_F0M0 and V_F0M0 in your calculation
+// of HDot_F0M0 if you need access to the cross-mobilizer transform and/or velocity.
+// CAUTION: our definition of H is transposed from Jain and Schwieters.
+virtual void calcAcrossJointVelocityJacobianDot(
+ const SBStateDigest& sbs,
+ HType& HDot_F0M0) const = 0;
+
+// We allow a mobilizer to be defined in reverse when that is more
+// convenient. That is, the H matrix can be defined by giving H_F0M0=H_MF and
+// HDot_F0M0=HDot_MF instead of H_FM and HDot_FM. In that case the
+// following methods are called instead of the above; the default
+// implementation postprocesses the output from the above methods, but a
+// mobilizer can override it if it knows how to get the job done faster.
+virtual void calcReverseMobilizerH_FM(
+ const SBStateDigest& sbs,
+ HType& H_FM) const;
+
+virtual void calcReverseMobilizerHDot_FM(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const;
+
+// This routine is NOT joint specific, but cannot be called until the across-joint
+// transform X_FM has been calculated and is available in the State cache.
+void calcBodyTransforms(
+ const SBTreePositionCache& pc,
+ Transform& X_PB,
+ Transform& X_GB) const
+{
+ const Transform& X_MB = getX_MB(); // fixed
+ const Transform& X_PF = getX_PF(); // fixed
+ const Transform& X_FM = getX_FM(pc); // just calculated
+ const Transform& X_GP = getX_GP(pc); // already calculated
+
+ const Transform X_FB = (noX_MB ? X_FM : X_FM*X_MB); // 63 flops
+ X_PB = (noR_PF ? Transform(X_FB.R(), X_PF.p()+X_FB.p()) : X_PF*X_FB); // 63 flops
+ X_GB = X_GP * X_PB; // 63 flops
+}
+
+// Same for all mobilizers. The return matrix here is precisely the
+// one used by Jain and Schwieters, but transposed.
+void calcParentToChildVelocityJacobianInGround(
+ const SBModelVars& mv,
+ const SBTreePositionCache& pc,
+ HType& H_PB_G) const;
+
+// Same for all mobilizers. This is the time derivative of
+// the matrix H_PB_G above, with the derivative taken in the
+// Ground frame.
+void calcParentToChildVelocityJacobianInGroundDot(
+ const SBModelVars& mv,
+ const SBTreePositionCache& pc,
+ const SBTreeVelocityCache& vc,
+ HType& HDot_PB_G) const;
+
+void realizeModel(SBStateDigest& sbs) const
+{
+}
+
+void realizeInstance(const SBStateDigest& sbs) const
+{
+}
+
+// Set a new configuration and calculate the consequent kinematics.
+// Must call base-to-tip.
+void realizePosition(const SBStateDigest& sbs) const
+{
+ const SBModelVars& mv = sbs.getModelVars();
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBInstanceCache& ic = sbs.getInstanceCache();
+ const Vector& allQ = sbs.getQ();
+ SBTreePositionCache& pc = sbs.updTreePositionCache();
+ Vector& allQErr = sbs.updQErr();
+
+ // Mobilizer specific.
+
+ const SBModelPerMobodInfo& mbInfo = getModelInfo(mc);
+
+ // First perform precalculations on these new q's, such as stashing away
+ // sines and cosines of angles. We'll put all the results into the State's
+ // position cache for later use. If there are local constraints on the q's
+ // (currently that means quaternion normalization constraints), then we
+ // calculate those here and put the result in the appropriate slot of qerr.
+
+ const int nq=mbInfo.nQInUse, nqpool=mbInfo.nQPoolInUse,
+ nqerr=(mbInfo.hasQuaternionInUse ? 1 : 0);
+ const Real* q0 = nq ? &allQ[mbInfo.firstQIndex] : 0;
+ Real* qpool0= nqpool ? &pc.mobilizerQCache[mbInfo.startInQPool] : 0;
+ Real* qerr0 = nqerr ? &allQErr[ic.firstQuaternionQErrSlot
+ + mbInfo.quaternionPoolIndex]
+ : 0;
+ performQPrecalculations(sbs, q0, nq, qpool0, nqpool, qerr0, nqerr);
+
+ // Now that we've done the necessary precalculations, calculate the cross-
+ // mobilizer transform X_FM without recalculating anything. For reversed
+ // mobilizers we have to do our own reversal here because mobilizers don't
+ // handle that themselves.
+
+ if (isReversed()) {
+ Transform X_MF;
+ calcX_FM(sbs, q0, nq, qpool0, nqpool, X_MF);
+ updX_FM(pc) = ~X_MF;
+ } else
+ calcX_FM(sbs, q0, nq, qpool0, nqpool, updX_FM(pc));
+
+ // With X_FM in the cache, and X_GP for the parent already calculated (we're doing
+ // an outward pass), we can calculate X_PB and X_GB now.
+ calcBodyTransforms(pc, updX_PB(pc), updX_GB(pc));
+
+ // Here we do allow the mobilizer to calculate the reversed H matrix, but the
+ // default implementation of the reversed method just calls the forward method
+ // and then reverses it. For some mobilizers that is unreasonably expensive.
+ // REMINDER: our H matrix definition is transposed from Jain and Schwieters.
+ if (isReversed()) calcReverseMobilizerH_FM (sbs, updH_FM(pc));
+ else calcAcrossJointVelocityJacobian(sbs, updH_FM(pc));
+
+ // Here we're using the cross-mobilizer hinge matrix H_FM that we just
+ // calculated to compute H(==H_PB_G), the equivalent hinge matrix between
+ // the parent's body frame P and child's body frame B, but expressed in
+ // Ground. (F is fixed on P and M is fixed on B.)
+ calcParentToChildVelocityJacobianInGround(mv,pc, updH(pc));
+
+ // Mobilizer independent.
+ calcJointIndependentKinematicsPos(pc);
+}
+
+// Set new velocities for the current configuration, and calculate
+// all the velocity-dependent terms. Must call base-to-tip.
+// This routine may assume that *all* position
+// kinematics (not just joint-specific) has been done for this node,
+// that all velocity kinematics has been done for the parent, and
+// that the velocity state variables (u) are available. The
+// quantities that must be computed are:
+// V_FM relative velocity of B's M frame in P's F frame,
+// expressed in F (note: this is also V_PM_F since
+// F is fixed on P). (This is H_FM*u.)
+// V_PB_G relative velocity of B in P (==H*u), expr. in G
+// HDot_FM time derivative of hinge matrix H_FM
+// HDot time derivative of H (==H_PB_G) hinge matrix relating body frames
+// VD_PB_G acceleration remainder term HDot*u, expr. in G
+// The code is the same for all joints, although parametrized by ndof.
+void realizeVelocity(const SBStateDigest& sbs) const
+{
+ const SBModelVars& mv = sbs.getModelVars();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ SBTreeVelocityCache& vc = sbs.updTreeVelocityCache();
+ const Vector& allU = sbs.getU();
+ const Vec<dof>& u = fromU(allU);
+
+ // Mobilizer specific.
+ calcQDot(sbs, &allU[uIndex], &sbs.updQDot()[qIndex]);
+
+ updV_FM(vc) = getH_FM(pc) * u; // 6*dof flops
+ updV_PB_G(vc) = getH(pc) * u; // 6*dof flops
+
+ // REMINDER: our H matrix definition is transposed from Jain and Schwieters.
+ if (isReversed()) calcReverseMobilizerHDot_FM (sbs, updHDot_FM(vc));
+ else calcAcrossJointVelocityJacobianDot(sbs, updHDot_FM(vc));
+
+ // Here we're using the cross-mobilizer hinge matrix derivative HDot_FM
+ // that we just calculated to compute HDot(==HDot_PB_G), the derivative
+ // of H (==H_PB_G) between the parent's body frame P and child's body
+ // frame B. The derivative is taken in the Ground frame and the result
+ // is expressed in Ground. (F is fixed on P and M is fixed on B.)
+ calcParentToChildVelocityJacobianInGroundDot(mv,pc,vc, updHDot(vc));
+ updVD_PB_G(vc) = getHDot(vc) * u; // 6*dof flops
+
+ // Mobilizer independent.
+ calcJointIndependentKinematicsVel(pc,vc);
+}
+
+// Articulated body inertias have been calculated; here we're
+void realizeDynamics(const SBArticulatedBodyInertiaCache& abc,
+ const SBStateDigest& sbs) const
+{
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const SBTreeVelocityCache& vc = sbs.getTreeVelocityCache();
+ SBDynamicsCache& dc = sbs.updDynamicsCache();
+
+ // Mobilizer-specific.
+ // None.
+
+ // Mobilizer independent.
+ calcJointIndependentDynamicsVel(pc,abc,vc,dc);
+}
+
+// There is no realizeAcceleration().
+
+void realizeReport(const SBStateDigest& sbs) const
+{
+}
+
+// Use base class implementation of calcCompositeBodyInertiasInward() since
+// that is independent of mobilities.
+
+// This is a dynamics-stage calculation and must be called tip-to-base (inward).
+void realizeArticulatedBodyInertiasInward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ SBArticulatedBodyInertiaCache& abc) const;
+
+// calcJointIndependentDynamicsVel() must be called after ArticulatedBodyInertias.
+
+// This dynamics-stage calculation is needed for handling constraints. It
+// must be called base-to-tip (outward);
+void realizeYOutward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache& abc,
+ SBDynamicsCache& dc) const;
+
+// These routines give each node a chance to set appropriate defaults in a piece
+// of the state corresponding to a particular stage. Default implementations here
+// assume non-ball joint; override if necessary.
+virtual void setMobilizerDefaultModelValues (const SBTopologyCache&, SBModelVars&) const {}
+virtual void setMobilizerDefaultInstanceValues(const SBModelVars&, SBInstanceVars&) const {}
+virtual void setMobilizerDefaultTimeValues (const SBModelVars&, SBTimeVars&) const {}
+
+virtual void setMobilizerDefaultPositionValues(const SBModelVars& s, Vector& q) const
+{
+ toQ(q) = 0;
+}
+virtual void setMobilizerDefaultVelocityValues(const SBModelVars&, Vector& u) const
+{
+ toU(u) = 0;
+}
+virtual void setMobilizerDefaultDynamicsValues(const SBModelVars&, SBDynamicsVars&) const {}
+virtual void setMobilizerDefaultAccelerationValues(const SBModelVars&,
+ SBDynamicsVars& v) const {}
+
+int getDOF() const {return dof;}
+virtual int getMaxNQ() const {
+ assert(quaternionUse == QuaternionIsNeverUsed);
+ return dof; // maxNQ can be larger than dof if there's a quaternion
+}
+
+// Default method must be overridden if there is a chance that NQ
+// might not be the same as NU.
+virtual int getNQInUse(const SBModelVars&) const {
+ assert(quaternionUse == QuaternionIsNeverUsed);
+ return dof; // DOF <= NQ <= maxNQ
+}
+
+// Currently NU is always just the Mobilizer's DOFs (the template argument)
+// Later we may want to offer modeling options to lock joints, or perhaps
+// break them.
+virtual int getNUInUse(const SBModelVars&) const {
+ return dof;
+}
+
+// Default method must be overridden if this mobilizer might use a
+// quaternion under some conditions.
+virtual bool isUsingQuaternion(const SBStateDigest&, MobilizerQIndex& startOfQuaternion) const {
+ assert(quaternionUse == QuaternionIsNeverUsed);
+ startOfQuaternion.invalidate();
+ return false;
+}
+
+// You must override the next few methods if qdot might not be the same
+// as u under *any* circumstance.
+
+// This method should calculate qdot=N*u, where N=N(q) is the kinematic
+// coupling matrix. State digest should be at Stage::Position.
+virtual void calcQDot(const SBStateDigest&,
+ const Real* u, Real* qdot) const
+{
+ assert(qdotHandling == QDotIsAlwaysTheSameAsU);
+ Vec<dof>::updAs(qdot) = Vec<dof>::getAs(u); // default says qdot=u
+}
+
+// This method should calculate u=N^-1*qdot, where N=N(q).
+// State digest should be at Stage::Position.
+virtual void calcLocalUFromLocalQDot(const SBStateDigest&,
+ const Real* qdot, Real* u) const
+{
+ assert(qdotHandling == QDotIsAlwaysTheSameAsU);
+ Vec<dof>::updAs(u) = Vec<dof>::getAs(qdot); // default says u=qdot
+}
+
+// This method should calculate qdotdot=N*udot + NDot*u, where N=N(q),
+// NDot=NDot(q,u). State digest should be at Stage::Velocity.
+virtual void calcQDotDot(const SBStateDigest&,
+ const Real* udot, Real* qdotdot) const
+{
+ assert(qdotHandling == QDotIsAlwaysTheSameAsU);
+ Vec<dof>::updAs(qdotdot) = Vec<dof>::getAs(udot); // default: qdotdot=udot
+}
+
+// This method should calculate udot=N^-1*(qdotdot-NDot*u), where N=N(q),
+// NDot=NDot(q,u). State digest should be at Stage::Velocity.
+virtual void calcLocalUDotFromLocalQDotDot(const SBStateDigest&,
+ const Real* qdotdot, Real* udot) const
+{
+ assert(qdotHandling == QDotIsAlwaysTheSameAsU);
+ Vec<dof>::updAs(udot) = Vec<dof>::getAs(qdotdot); // default: udot=qdotdot
+}
+
+
+
+
+// State digest should be at Stage::Position for calculating N (the matrix that
+// maps generalized speeds u to coordinate derivatives qdot, qdot=Nu). The
+// default implementation assumes nq==nu and the nqXnu block of N corresponding
+// to this mobilizer is identity. Then either operation (regardless of side)
+// just copies nu numbers from in to out.
+//
+// THIS MUST BE OVERRIDDEN by any mobilizer for which nq != nu, or qdot != u.
+virtual void multiplyByN(const SBStateDigest&, bool matrixOnRight,
+ const Real* in, Real* out) const
+{
+ assert(qdotHandling == QDotIsAlwaysTheSameAsU);
+ Vec<dof>::updAs(out) = Vec<dof>::getAs(in);
+}
+
+virtual void multiplyByNInv(const SBStateDigest&, bool matrixOnRight,
+ const Real* in, Real* out) const
+{
+ assert(qdotHandling == QDotIsAlwaysTheSameAsU);
+ Vec<dof>::updAs(out) = Vec<dof>::getAs(in);
+}
+
+// State digest should be at Stage::Velocity for calculating NDot (the matrix
+// that is used in mapping generalized accelerations udot to coordinate 2nd
+// derivatives qdotdot, qdotdot=N udot + NDot u. The default implementation
+// assumes nq==nu and the nqXnu block of N corresponding to this mobilizer is
+// identity, so NDot is an nuXnu block of zeroes. Then either operation
+// (regardless of side) just copies nu zeros to out.
+//
+// THIS MUST BE OVERRIDDEN by any mobilizer for which nq != nu, or qdot != u.
+virtual void multiplyByNDot(const SBStateDigest&, bool matrixOnRight,
+ const Real* in, Real* out) const
+{
+ assert(qdotHandling == QDotIsAlwaysTheSameAsU);
+ Vec<dof>::updAs(out) = 0;
+}
+
+
+// No default implementations here for:
+// calcMobilizerTransformFromQ
+// calcMobilizerVelocityFromU
+// calcMobilizerAccelerationFromUDot
+// calcParentToChildTransformFromQ
+// calcParentToChildVelocityFromU
+// calcParentToChildAccelerationFromUDot
+
+
+virtual void setVelFromSVel(
+ const SBTreePositionCache& pc,
+ const SBTreeVelocityCache& vc,
+ const SpatialVec& sVel,
+ Vector& u) const;
+
+// Return true if any change is made to the q vector.
+virtual bool enforceQuaternionConstraints(
+ const SBStateDigest& sbs,
+ Vector& q,
+ Vector& qErrest) const
+{
+ assert(quaternionUse == QuaternionIsNeverUsed);
+ return false;
+}
+
+void convertToEulerAngles(const Vector& inputQ, Vector& outputQ) const {
+ // The default implementation just copies Q. Subclasses may override this.
+ assert(quaternionUse == QuaternionIsNeverUsed);
+ toQ(outputQ) = fromQ(inputQ);
+}
+
+void convertToQuaternions(const Vector& inputQ, Vector& outputQ) const {
+ // The default implementation just copies Q. Subclasses may override this.
+ assert(quaternionUse == QuaternionIsNeverUsed);
+ toQ(outputQ) = fromQ(inputQ);
+}
+
+// Get a column of H_PB_G, which is what Jain calls H* and Schwieters calls H^T.
+const SpatialVec& getHCol(const SBTreePositionCache& pc, int j) const {
+ return getH(pc)(j);
+}
+
+// Get a column of H_FM the local cross-mobilizer hinge matrix expressed in the
+// parent (inboard) mobilizer frame F.
+const SpatialVec& getH_FMCol(const SBTreePositionCache& pc, int j) const {
+ return getH_FM(pc)(j);
+}
+
+// Access to body-oriented state and cache entries is the same for all nodes,
+// and joint oriented access is almost the same but parametrized by dof. There is a special
+// case for quaternions because they use an extra state variable, and although we don't
+// have to we make special scalar routines available for 1-dof joints. Note that all State access
+// routines are inline, not virtual, so the cost is just an indirection and an index.
+//
+// TODO: these inner-loop methods probably shouldn't be indexing a Vector, which requires
+// several indirections. Instead, the top-level caller should find the Real* data contained in the
+// Vector and then pass that to the RigidBodyNode routines which will call these ones.
+
+// General joint-dependent select-my-goodies-from-the-pool routines.
+const Vec<dof>& fromQ (const Real* q) const {return Vec<dof>::getAs(&q[qIndex]);}
+Vec<dof>& toQ ( Real* q) const {return Vec<dof>::updAs(&q[qIndex]);}
+const Vec<dof>& fromU (const Real* u) const {return Vec<dof>::getAs(&u[uIndex]);}
+Vec<dof>& toU ( Real* u) const {return Vec<dof>::updAs(&u[uIndex]);}
+const Mat<dof,dof>& fromUSq(const Real* uSq) const {return Mat<dof,dof>::getAs(&uSq[uSqIndex]);}
+Mat<dof,dof>& toUSq ( Real* uSq) const {return Mat<dof,dof>::updAs(&uSq[uSqIndex]);}
+
+// Same, but specialized for the common case where dof=1 so everything is scalar.
+const Real& from1Q (const Real* q) const {return q[qIndex];}
+Real& to1Q ( Real* q) const {return q[qIndex];}
+const Real& from1U (const Real* u) const {return u[uIndex];}
+Real& to1U ( Real* u) const {return u[uIndex];}
+const Real& from1USq(const Real* uSq) const {return uSq[uSqIndex];}
+Real& to1USq ( Real* uSq) const {return uSq[uSqIndex];}
+
+// Same, specialized for quaternions. We're assuming that the quaternion is stored
+// in the *first* four coordinates, if there are more than four altogether.
+const Vec4& fromQuat(const Real* q) const {return Vec4::getAs(&q[qIndex]);}
+Vec4& toQuat ( Real* q) const {return Vec4::updAs(&q[qIndex]);}
+
+// Extract a Vec3 from a Q-like or U-like object, beginning at an offset from the qIndex or uIndex.
+const Vec3& fromQVec3(const Real* q, int offs) const {return Vec3::getAs(&q[qIndex+offs]);}
+Vec3& toQVec3 ( Real* q, int offs) const {return Vec3::updAs(&q[qIndex+offs]);}
+const Vec3& fromUVec3(const Real* u, int offs) const {return Vec3::getAs(&u[uIndex+offs]);}
+Vec3& toUVec3 ( Real* u, int offs) const {return Vec3::updAs(&u[uIndex+offs]);}
+
+// These provide an identical interface for when q,u are given as Vectors rather than Reals.
+
+const Vec<dof>& fromQ (const Vector& q) const {return fromQ(&q[0]);} // convert to array of Real
+Vec<dof>& toQ ( Vector& q) const {return toQ (&q[0]);}
+const Vec<dof>& fromU (const Vector& u) const {return fromU(&u[0]);}
+Vec<dof>& toU ( Vector& u) const {return toU (&u[0]);}
+const Mat<dof,dof>& fromUSq(const Vector& uSq) const {return fromUSq(&uSq[0]);}
+Mat<dof,dof>& toUSq ( Vector& uSq) const {return toUSq (&uSq[0]);}
+
+const Real& from1Q (const Vector& q) const {return from1Q (&q[0]);}
+Real& to1Q ( Vector& q) const {return to1Q (&q[0]);}
+const Real& from1U (const Vector& u) const {return from1U (&u[0]);}
+Real& to1U ( Vector& u) const {return to1U (&u[0]);}
+const Real& from1USq(const Vector& uSq) const {return from1USq(&uSq[0]);}
+Real& to1USq ( Vector& uSq) const {return to1USq (&uSq[0]);}
+
+// Same, specialized for quaternions. We're assuming that the quaternion comes first in the coordinates.
+const Vec4& fromQuat(const Vector& q) const {return fromQuat(&q[0]);}
+Vec4& toQuat ( Vector& q) const {return toQuat (&q[0]);}
+
+// Extract a Vec3 from a Q-like or U-like object, beginning at an offset from the qIndex or uIndex.
+const Vec3& fromQVec3(const Vector& q, int offs) const {return fromQVec3(&q[0], offs);}
+Vec3& toQVec3 ( Vector& q, int offs) const {return toQVec3 (&q[0], offs);}
+const Vec3& fromUVec3(const Vector& u, int offs) const {return fromUVec3(&u[0], offs);}
+Vec3& toUVec3 ( Vector& u, int offs) const {return toUVec3 (&u[0], offs);}
+
+// Applications of the above extraction routines to particular interesting items in the State. Note
+// that you can't use these for quaternions since they extract "dof" items.
+
+// Cache entries (cache is mutable in a const State)
+
+ // Position
+
+
+// CAUTION: our H definition is transposed from Jain and Schwieters.
+const HType& getH_FM(const SBTreePositionCache& pc) const
+ { return HType::getAs(&pc.storageForH_FM[2*uIndex]); }
+HType& updH_FM(SBTreePositionCache& pc) const
+ { return HType::updAs(&pc.storageForH_FM[2*uIndex]); }
+
+// "H" here should really be H_PB_G, that is, cross joint transition
+// matrix relating parent and body frames, but expressed in Ground.
+// CAUTION: our H definition is transposed from Jain and Schwieters.
+const HType& getH(const SBTreePositionCache& pc) const
+ { return HType::getAs(&pc.storageForH[2*uIndex]); }
+HType& updH(SBTreePositionCache& pc) const
+ { return HType::updAs(&pc.storageForH[2*uIndex]); }
+
+ // Velocity
+
+// CAUTION: our H definition is transposed from Jain and Schwieters.
+const HType& getHDot_FM(const SBTreeVelocityCache& vc) const
+ { return HType::getAs(&vc.storageForHDot_FM[2*uIndex]); }
+HType& updHDot_FM(SBTreeVelocityCache& vc) const
+ { return HType::updAs(&vc.storageForHDot_FM[2*uIndex]); }
+
+// CAUTION: our H definition is transposed from Jain and Schwieters.
+const HType& getHDot(const SBTreeVelocityCache& vc) const
+ { return HType::getAs(&vc.storageForHDot[2*uIndex]); }
+HType& updHDot(SBTreeVelocityCache& vc) const
+ { return HType::updAs(&vc.storageForHDot[2*uIndex]); }
+
+ // Dynamics
+
+// These are calculated with articulated body inertias.
+const Mat<dof,dof>& getD(const SBArticulatedBodyInertiaCache& abc) const
+{return fromUSq(abc.storageForD);}
+Mat<dof,dof>& updD(SBArticulatedBodyInertiaCache& abc) const
+{return toUSq (abc.storageForD);}
+
+const Mat<dof,dof>& getDI(const SBArticulatedBodyInertiaCache& abc) const
+{return fromUSq(abc.storageForDI);}
+Mat<dof,dof>& updDI(SBArticulatedBodyInertiaCache& abc) const
+{return toUSq (abc.storageForDI);}
+
+const Mat<2,dof,Vec3>& getG(const SBArticulatedBodyInertiaCache& abc) const
+ { return Mat<2,dof,Vec3>::getAs(&abc.storageForG[2*uIndex]); }
+Mat<2,dof,Vec3>& updG(SBArticulatedBodyInertiaCache& abc) const
+ { return Mat<2,dof,Vec3>::updAs(&abc.storageForG[2*uIndex]); }
+
+const Vec<dof>& getTau(const SBInstanceCache& ic, const Real* tau) const {
+ const PresForcePoolIndex tauIx = ic.getMobodInstanceInfo(nodeNum).firstPresForce;
+ assert(tauIx.isValid());
+ return Vec<dof>::getAs(&tau[tauIx]);
+}
+Vec<dof>& updTau(const SBInstanceCache& ic, Real* tau) const {
+ const PresForcePoolIndex tauIx = ic.getMobodInstanceInfo(nodeNum).firstPresForce;
+ assert(tauIx.isValid());
+ return Vec<dof>::updAs(&tau[tauIx]);
+}
+
+ // Acceleration
+
+const Vec<dof>& getEpsilon (const SBTreeAccelerationCache& ac) const {return fromU (ac.epsilon);}
+Vec<dof>& updEpsilon (SBTreeAccelerationCache& ac) const {return toU (ac.epsilon);}
+const Real& get1Epsilon(const SBTreeAccelerationCache& ac) const {return from1U(ac.epsilon);}
+Real& upd1Epsilon(SBTreeAccelerationCache& ac) const {return to1U (ac.epsilon);}
+
+
+void multiplyBySystemJacobian(
+ const SBTreePositionCache& pc,
+ const Real* v,
+ SpatialVec* Jv) const;
+
+void multiplyBySystemJacobianTranspose(
+ const SBTreePositionCache& pc,
+ SpatialVec* zTmp,
+ const SpatialVec* X,
+ Real* JtX) const;
+
+void calcEquivalentJointForces(
+ const SBTreePositionCache& pc,
+ const SBDynamicsCache& dc,
+ const SpatialVec* bodyForces,
+ SpatialVec* allZ,
+ Real* jointForces) const;
+
+void calcUDotPass1Inward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache&,
+ const SBDynamicsCache& dc,
+ const Real* jointForces,
+ const SpatialVec* bodyForces,
+ const Real* allUDot,
+ SpatialVec* allZ,
+ SpatialVec* allGepsilon,
+ Real* allEpsilon) const;
+
+void calcUDotPass2Outward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache&,
+ const SBTreeVelocityCache& vc,
+ const SBDynamicsCache& dc,
+ const Real* epsilonTmp,
+ SpatialVec* allA_GB,
+ Real* allUDot,
+ Real* allTau) const;
+
+void multiplyByMInvPass1Inward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache&,
+ const Real* f,
+ SpatialVec* allZ,
+ SpatialVec* allGepsilon,
+ Real* allEpsilon) const OVERRIDE_11;
+
+void multiplyByMInvPass2Outward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache&,
+ const Real* epsilonTmp,
+ SpatialVec* allA_GB,
+ Real* allUDot) const OVERRIDE_11;
+
+// Also serves as pass 1 for inverse dynamics.
+void calcBodyAccelerationsFromUdotOutward(
+ const SBTreePositionCache& pc,
+ const SBTreeVelocityCache& vc,
+ const Real* allUDot,
+ SpatialVec* allA_GB) const;
+
+void calcInverseDynamicsPass2Inward(
+ const SBTreePositionCache& pc,
+ const SBTreeVelocityCache& vc,
+ const SpatialVec* allA_GB,
+ const Real* jointForces,
+ const SpatialVec* bodyForces,
+ SpatialVec* allFTmp,
+ Real* allTau) const;
+
+void multiplyByMPass1Outward(
+ const SBTreePositionCache& pc,
+ const Real* allUDot,
+ SpatialVec* allA_GB) const;
+void multiplyByMPass2Inward(
+ const SBTreePositionCache& pc,
+ const SpatialVec* allA_GB,
+ SpatialVec* allFTmp,
+ Real* allTau) const;
+
+};
+
+#endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_H_
diff --git a/Simbody/src/RigidBodyNodeSpec_Ball.h b/Simbody/src/RigidBodyNodeSpec_Ball.h
new file mode 100644
index 0000000..70ea5f8
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec_Ball.h
@@ -0,0 +1,452 @@
+#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_BALL_H_
+#define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_BALL_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Derived from IVM code written by Charles Schwieters *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+
+/**@file
+ * Define the RigidBodyNode that implements a Ball mobilizer, also known as an
+ * Orientation or Spherical joint.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "RigidBodyNodeSpec.h"
+
+// ORIENTATION (BALL) //
+
+// Ball joint. This provides three degrees of rotational freedom, i.e.,
+// unrestricted orientation of the body's M frame in the parent's F frame.
+// The generalized coordinates are:
+// * 4 quaternions or 3 X-Y-Z (1-2-3) body fixed Euler angles (that is,
+// fixed in M)
+// and generalized speeds are:
+// * angular velocity w_FM as a vector expressed in the F (parent) frame.
+// Thus rotational qdots have to be derived from the generalized speeds to
+// be turned into either 4 quaternion derivatives or 3 Euler angle derivatives.
+//
+// NOTE: An XYZ Euler angle sequence has a singularity when the middle angle
+// is at 90 or 270 degrees; quaternions are never singular.
+template<bool noX_MB, bool noR_PF>
+class RBNodeBall : public RigidBodyNodeSpec<3, false, noX_MB, noR_PF> {
+public:
+
+typedef typename RigidBodyNodeSpec<3, false, noX_MB, noR_PF>::HType HType;
+virtual const char* type() { return "ball"; }
+
+RBNodeBall(const MassProperties& mProps_B,
+ const Transform& X_PF,
+ const Transform& X_BM,
+ bool isReversed,
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot)
+ : RigidBodyNodeSpec<3, false, noX_MB, noR_PF>(mProps_B,X_PF,X_BM,nextUSlot,nextUSqSlot,nextQSlot,
+ RigidBodyNode::QDotMayDifferFromU, RigidBodyNode::QuaternionMayBeUsed, isReversed)
+{
+ this->updateSlots(nextUSlot,nextUSqSlot,nextQSlot);
+}
+
+void setQToFitRotationImpl(const SBStateDigest& sbs, const Rotation& R_FM,
+ Vector& q) const
+{
+ if (this->getUseEulerAngles(sbs.getModelVars()))
+ this->toQ(q) = R_FM.convertRotationToBodyFixedXYZ();
+ else
+ this->toQuat(q) = R_FM.convertRotationToQuaternion().asVec4();
+}
+
+void setQToFitTranslationImpl(const SBStateDigest& sbs, const Vec3& p_FM, Vector& q) const {
+ // M and F frame origins are always coincident for this mobilizer so there is no
+ // way to create a translation by rotating. So the only translation we can represent is 0.
+}
+
+void setUToFitAngularVelocityImpl(const SBStateDigest& sbs, const Vector&, const Vec3& w_FM,
+ Vector& u) const
+{
+ this->toU(u) = w_FM; // relative angular velocity always used as generalized speeds
+}
+
+void setUToFitLinearVelocityImpl
+ (const SBStateDigest& sbs, const Vector&, const Vec3& v_FM, Vector& u) const
+{
+ // M and F frame origins are always coincident for this mobilizer so there is no
+ // way to create a linear velocity by rotating. So the only linear velocity
+ // we can represent is 0.
+}
+
+// When we're using Euler angles, we're going to want to cache cos and sin for
+// each angle, and also 1/cos of the middle angle will be handy to have around.
+// When we're using quaternions, we'll cache 1/|q| for convenient normalizing
+// of quaternions.
+enum {AnglePoolSize=7, QuatPoolSize=1};
+// These are indices into the pool: cos x,y,z sin x,y,z 1/cos(y)
+enum {AngleCosQ=0, AngleSinQ=3, AngleOOCosQy=6};
+enum {QuatOONorm=0};
+int calcQPoolSize(const SBModelVars& mv) const
+{ return this->getUseEulerAngles(mv) ? AnglePoolSize : QuatPoolSize; }
+
+// This is expensive in Euler angle mode due to the three sin/cos computations
+// required (about 150 flops). In quaternion mode it is much cheaper (about
+// 25 flops).
+void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* qCache, int nQCache,
+ Real* qErr, int nQErr) const
+{
+ if (this->getUseEulerAngles(sbs.getModelVars())) {
+ assert(q && nq==3 && qCache && nQCache==AnglePoolSize && nQErr==0);
+
+ const Real cy = std::cos(q[1]);
+ Vec3::updAs(&qCache[AngleCosQ]) =
+ Vec3(std::cos(q[0]), cy, std::cos(q[2]));
+ Vec3::updAs(&qCache[AngleSinQ]) =
+ Vec3(std::sin(q[0]), std::sin(q[1]), std::sin(q[2]));
+ qCache[AngleOOCosQy] = 1/cy; // trouble at 90 or 270 degrees
+ } else {
+ assert(q && nq==4 && qCache && nQCache==QuatPoolSize &&
+ qErr && nQErr==1);
+
+ const Real quatLen = Vec4::getAs(q).norm();
+ qErr[0] = quatLen - Real(1); // normalization error
+ qCache[QuatOONorm] = 1/quatLen; // save for later
+ }
+}
+
+// Because of the precalculations above we can calculate the cross-mobilizer
+// transform in 18 flops (Euler angles) or 33 flops (quaternions).
+void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* qCache, int nQCache,
+ Transform& X_F0M0) const
+{
+ // Rotation.
+ if (this->getUseEulerAngles(sbs.getModelVars())) {
+ assert(q && nq==3 && qCache && nQCache==AnglePoolSize);
+ X_F0M0.updR().setRotationToBodyFixedXYZ // 18 flops
+ (Vec3::getAs(&qCache[AngleCosQ]), Vec3::getAs(&qCache[AngleSinQ]));
+ } else {
+ assert(q && nq==4 && qCache && nQCache==QuatPoolSize);
+ // Must use a normalized quaternion to generate the rotation matrix.
+ // Here we normalize with just 4 flops using precalculated 1/norm(q).
+ const Quaternion quat(Vec4::getAs(q)*qCache[QuatOONorm], true);
+ X_F0M0.updR().setRotationFromQuaternion(quat); // 29 flops
+ }
+
+ // Translation.
+ X_F0M0.updP() = 0; // This joint can't translate.
+}
+
+// Generalized speeds are the angular velocity expressed in F, so they
+// cause rotations around F x,y,z axes respectively.
+void calcAcrossJointVelocityJacobian(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+{
+ H_FM(0) = SpatialVec( Vec3(1,0,0), Vec3(0) );
+ H_FM(1) = SpatialVec( Vec3(0,1,0), Vec3(0) );
+ H_FM(2) = SpatialVec( Vec3(0,0,1), Vec3(0) );
+}
+
+// The derivative of constant matrix H_FM is not too interesting.
+void calcAcrossJointVelocityJacobianDot(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+{
+ HDot_FM(0) = SpatialVec( Vec3(0), Vec3(0) );
+ HDot_FM(1) = SpatialVec( Vec3(0), Vec3(0) );
+ HDot_FM(2) = SpatialVec( Vec3(0), Vec3(0) );
+}
+
+
+// Calculate qdot=N(q)*u. Precalculations make this fast in Euler angle
+// mode (10 flops); quaternion mode is 27 flops.
+// TODO: we're expecting that there are 4 qdots even in Euler angle mode
+// so we have to zero out the last one in that case.
+void calcQDot(const SBStateDigest& sbs, const Real* u,
+ Real* qdot) const {
+ assert(sbs.getStage() >= Stage::Position);
+ assert(u && qdot);
+
+ const SBModelVars& mv = sbs.getModelVars();
+
+ // Generalized speeds are the same in either mode -- angular velocity
+ // of F in M (or B in P), expressed in F (fixed on parent) frame.
+ const Vec3& w_FM = Vec3::getAs(u);
+
+ if (this->getUseEulerAngles(mv)) {
+ // Euler angle mode (10 flops)
+ qdot[3] = 0; // TODO: kludge, clear unused element
+
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Real* pool = this->getQPool(mc, pc);
+
+ Vec3::updAs(qdot) = Rotation::convertAngVelInParentToBodyXYZDot(
+ Vec2::getAs(&pool[AngleCosQ]), Vec2::getAs(&pool[AngleSinQ]), //x,y
+ pool[AngleOOCosQy], w_FM);
+ } else {
+ // Quaternion mode (27 flops)
+ const Vec4& quat = this->fromQuat(sbs.getQ()); // unnormalized
+ Vec4::updAs(qdot) = Rotation::convertAngVelToQuaternionDot(quat, w_FM);
+ }
+}
+
+// CAUTION: we do not zero the unused 4th element of q for Euler angles; it
+// is up to the caller to do that if it is necessary.
+// Compute out_q = N * in_u
+// or out_u = in_q * N
+// The u quantity is a vector v_F in the parent frame (e.g. angular
+// velocity of child in parent), while the q quantity is the derivative
+// of a quaternion representing R_FM, i.e. orientation of child in parent.
+// Cost: Euler angle mode - 10 flops, Quaternion mode - 27 flops.
+void multiplyByN(const SBStateDigest& sbs, bool matrixOnRight,
+ const Real* in, Real* out) const
+{
+ assert(sbs.getStage() >= Stage::Position);
+ assert(in && out);
+
+ const SBModelVars& mv = sbs.getModelVars();
+
+ if (this->getUseEulerAngles(mv)) {
+ // Euler angle mode (10 flops).
+
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Real* pool = this->getQPool(mc, pc);
+
+ // Aliases.
+ const Vec2& cosxy = Vec2::getAs(&pool[AngleCosQ]); // only need x,y
+ const Vec2& sinxy = Vec2::getAs(&pool[AngleSinQ]);
+ const Real& oocosy = pool[AngleOOCosQy];
+ const Vec3& in_v = Vec3::getAs(in);
+ Vec3& out_v = Vec3::updAs(out);
+
+ if (matrixOnRight) { // out_u = in_q * N = ~N * in_q (9 flops)
+ // Transpose on left is same as untransposed on right.
+ out_v = Rotation::multiplyByBodyXYZ_NT_P(cosxy, sinxy, oocosy, in_v);
+ } else { // out_q = N*in_u (10 flops)
+ out_v = Rotation::multiplyByBodyXYZ_N_P(cosxy, sinxy, oocosy, in_v);
+ }
+ } else {
+ // Quaternion mode (27 flops).
+ const Vec4& quat = this->fromQuat(sbs.getQ()); // unnormalized
+ const Mat43 N_F = // This method returns N for the parent frame
+ Rotation::calcUnnormalizedNForQuaternion(quat); // 7 flops
+ if (matrixOnRight)
+ Row3::updAs(out) = Row4::getAs(in) * N_F; // 20 flops
+ else Vec4::updAs(out) = N_F * Vec3::getAs(in);
+ }
+}
+
+// Compute out_u = inv(N) * in_q
+// or out_q = in_u * inv(N)
+// Cost: Euler angle mode ~ 10 flops, Quaternion mode ~ 27 flops.
+void multiplyByNInv(const SBStateDigest& sbs, bool matrixOnRight,
+ const Real* in, Real* out) const
+{
+ assert(sbs.getStage() >= Stage::Position);
+ assert(in && out);
+
+ const SBModelVars& mv = sbs.getModelVars();
+
+ if (this->getUseEulerAngles(mv)) {
+ // Euler angle mode (10 flops).
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Real* pool = this->getQPool(mc, pc);
+
+ // Aliases.
+ const Vec2& cosxy = Vec2::getAs(&pool[AngleCosQ]); // only need x,y
+ const Vec2& sinxy = Vec2::getAs(&pool[AngleSinQ]);
+ const Vec3& in_v = Vec3::getAs(in);
+ Vec3& out_v = Vec3::updAs(out);
+
+ if (matrixOnRight) { // out_q = in_u * inv(N) = ~inv(N) * in_u (10 flops)
+ // transpose on left is same as untransposed on right
+ out_v = Rotation::multiplyByBodyXYZ_NInvT_P(cosxy, sinxy, in_v);
+ } else { // out_u = inv(N) * in_q (9 flops)
+ out_v = Rotation::multiplyByBodyXYZ_NInv_P(cosxy, sinxy, in_v);
+ }
+ } else {
+ // Quaternion mode (27 flops).
+ const Vec4& quat = this->fromQuat(sbs.getQ()); // unnormalized
+ const Mat34 NInv_F = // Returns NInv for parent frame.
+ Rotation::calcUnnormalizedNInvForQuaternion(quat); // 7 flops
+ if (matrixOnRight)
+ Row4::updAs(out) = Row3::getAs(in) * NInv_F; // 20 flops
+ else Vec3::updAs(out) = NInv_F * Vec4::getAs(in); // 21 flops
+ }
+}
+
+// Compute out_q = NDot * in_u
+// or out_u = in_q * NDot
+// Note that it is much cheaper to calculate
+// qdotdot directly than to do it using N and NDot; see below.
+// Cost: Euler angle mode - 36 flops, Quaternion mode - 27 flops.
+void multiplyByNDot(const SBStateDigest& sbs, bool matrixOnRight,
+ const Real* in, Real* out) const
+{
+ assert(sbs.getStage() > Stage::Velocity);
+ assert(in && out);
+
+ const SBModelVars& mv = sbs.getModelVars();
+
+ if (this->getUseEulerAngles(mv)) {
+ // Euler angle mode (36 flops).
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Real* pool = this->getQPool(mc, pc);
+
+ // Aliases.
+ const Vec2& cosxy = Vec2::getAs(&pool[AngleCosQ]); // only need x,y
+ const Vec2& sinxy = Vec2::getAs(&pool[AngleSinQ]);
+ const Real& oocosy = pool[AngleOOCosQy];
+ const Vec3& qdot = this->fromQ(sbs.getQDot());
+
+ // We don't have a nice multiply-by routine here so just get the NDot
+ // matrix and use it (21 flops).
+ const Mat33 NDot_F =
+ Rotation::calcNDotForBodyXYZInParentFrame(cosxy, sinxy, oocosy, qdot);
+
+ if (matrixOnRight) { // out_u = in_q * NDot (15 flops)
+ Row3::updAs(out) = Row3::getAs(in) * NDot_F;
+ } else { // out_q = NDot * in_u (15 flops)
+ Vec3::updAs(out) = NDot_F * Vec3::getAs(in);
+ }
+ } else {
+ // Quaternion mode (27 flops).
+ const Vec4& qdot = this->fromQuat(sbs.getQDot()); // unnormalized
+ const Mat43 NDot_F = // This method returns NDot for the parent frame
+ Rotation::calcUnnormalizedNDotForQuaternion(qdot); // 7 flops
+ if (matrixOnRight)
+ Row3::updAs(out) = Row4::getAs(in) * NDot_F; // 21 flops
+ else Vec4::updAs(out) = NDot_F * Vec3::getAs(in); // 20 flops
+ }
+}
+
+// Calculate qdotdot from udot as qdotdot = N(q)*udot + NDot(q,qdot)*u, but
+// faster. Here we assume that qdot=N*u has already been calculated, which
+// is very helpful in Euler angle mode. In either mode it is a very bad
+// idea to calculate this using explicit N and NDot matrices; we can save
+// a lot of time by using more specialized methods.
+// Cost: Euler angle mode - 22 flops, Quaternion mode - 41 flops.
+void calcQDotDot(const SBStateDigest& sbs, const Real* udot,
+ Real* qdotdot) const {
+ assert(sbs.getStage() > Stage::Velocity);
+ assert(udot && qdotdot);
+
+ const SBModelVars& mv = sbs.getModelVars();
+
+ // Angular acceleration is the same in either mode.
+ const Vec3& b_FM = Vec3::getAs(udot); // a.k.a. w_FM_dot
+
+ if (this->getUseEulerAngles(mv)) {
+ // Euler angle mode (22 flops).
+ Vec4::updAs(qdotdot) = 0; // TODO: kludge, clear unused element
+
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Real* pool = this->getQPool(mc, pc);
+
+ // Aliases.
+ const Vec2& cosxy = Vec2::getAs(&pool[AngleCosQ]); // only need x,y
+ const Vec2& sinxy = Vec2::getAs(&pool[AngleSinQ]);
+ const Real& oocosy = pool[AngleOOCosQy];
+ const Vec3& qdot = this->fromQ(sbs.getQDot());
+ Vec3& qdotdot_v = Vec3::updAs(qdotdot);
+
+ // 22 flops.
+ qdotdot_v = Rotation::convertAngAccInParentToBodyXYZDotDot
+ (cosxy, sinxy, oocosy, qdot, b_FM);
+ } else {
+ // Quaternion mode (41 flops).
+ const Vec4& quat = this->fromQuat(sbs.getQ()); // unnormalized
+ const Vec3& w_FM = this->fromU(sbs.getU());
+ Vec4::updAs(qdotdot) =
+ Rotation::convertAngVelDotToQuaternionDotDot(quat,w_FM,b_FM);
+ }
+}
+
+int getMaxNQ() const {return 4;}
+int getNQInUse(const SBModelVars& mv) const {
+ return this->getUseEulerAngles(mv) ? 3 : 4;
+}
+bool isUsingQuaternion(const SBStateDigest& sbs, MobilizerQIndex& startOfQuaternion) const {
+ if (this->getUseEulerAngles(sbs.getModelVars())) {
+ startOfQuaternion.invalidate();
+ return false;
+ }
+ startOfQuaternion = MobilizerQIndex(0); // quaternion comes first
+ return true;
+}
+
+void setMobilizerDefaultPositionValues(
+ const SBModelVars& mv,
+ Vector& q) const
+{
+ if (this->getUseEulerAngles(mv)) {
+ //TODO: kludge
+ this->toQuat(q) = Vec4(0); // clear unused element
+ this->toQ(q) = 0;
+ }
+ else this->toQuat(q) = Vec4(1,0,0,0);
+}
+
+bool enforceQuaternionConstraints(
+ const SBStateDigest& sbs,
+ Vector& q,
+ Vector& qErrest) const
+{
+ if (this->getUseEulerAngles(sbs.getModelVars()))
+ return false; // no change
+
+ Vec4& quat = this->toQuat(q);
+ quat = quat / quat.norm();
+
+ if (qErrest.size()) {
+ Vec4& qerr = this->toQuat(qErrest);
+ qerr -= dot(qerr,quat) * quat;
+ }
+
+ return true;
+}
+
+void convertToEulerAngles(const Vector& inputQ, Vector& outputQ) const {
+ this->toQuat(outputQ) = Vec4(0); // clear unused element
+ this->toQ(outputQ) = Rotation(Quaternion(this->fromQuat(inputQ))).convertRotationToBodyFixedXYZ();
+}
+
+void convertToQuaternions(const Vector& inputQ, Vector& outputQ) const {
+ Rotation rot;
+ rot.setRotationToBodyFixedXYZ(this->fromQ(inputQ));
+ this->toQuat(outputQ) = rot.convertRotationToQuaternion().asVec4();
+}
+
+
+};
+
+
+#endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_BALL_H_
+
diff --git a/Simbody/src/RigidBodyNodeSpec_Bushing.h b/Simbody/src/RigidBodyNodeSpec_Bushing.h
new file mode 100644
index 0000000..cc129aa
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec_Bushing.h
@@ -0,0 +1,228 @@
+#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_BUSHING_H_
+#define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_BUSHING_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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Define the RigidBodyNode that implements a Bushing mobilizer.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "RigidBodyNodeSpec.h"
+
+
+ // BUSHING //
+
+// Bushing joint. This provides a full six degrees of freedom but with
+// orientation represented using Euler angles and thus not suited to
+// arbitrarily large rotations.
+// The generalized coordinates q are (in this order):
+// * 3 x-y-z (1-2-3) body fixed Euler angles (that is, fixed in M)
+// * 3 measure numbers of position vector p_FM
+// and generalized speeds u are:
+// * u = qdot, that is, the x-y-z body fixed Euler angle derivatives
+// followed by the velocity vector v_FM
+//
+// NOTE: This joint has a singularity when the middle angle (q[1]) is near
+// +/-90 degrees. In most cases you should use a Free joint instead, which by
+// default uses a quaternion as its generalized coordinates to avoid this
+// singularity. A modeling option allows the Free to be switched to use Euler
+// angles when necessary.
+
+template<bool noX_MB, bool noR_PF>
+class RBNodeBushing : public RigidBodyNodeSpec<6, false, noX_MB, noR_PF> {
+public:
+
+typedef typename RigidBodyNodeSpec<6, false, noX_MB, noR_PF>::HType HType;
+virtual const char* type() { return "bushing"; }
+
+RBNodeBushing(const MassProperties& mProps_B,
+ const Transform& X_PF,
+ const Transform& X_BM,
+ bool isReversed,
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot)
+: RigidBodyNodeSpec<6, false, noX_MB, noR_PF>
+ (mProps_B,X_PF,X_BM,
+ nextUSlot,nextUSqSlot,nextQSlot,
+ RigidBodyNode::QDotIsAlwaysTheSameAsU,
+ RigidBodyNode::QuaternionIsNeverUsed,
+ isReversed)
+{
+ this->updateSlots(nextUSlot,nextUSqSlot,nextQSlot);
+}
+
+ // Implementations of virtual methods.
+
+// This has a default implementation but it rotates first then translates,
+// which works fine for the normal bushing but produces wrong behavior when
+// the mobilizer is reversed.
+void setQToFitTransformImpl(const SBStateDigest& sbs, const Transform& X_FM,
+ Vector& q) const OVERRIDE_11
+{
+ setQToFitTranslationImpl(sbs, X_FM.p(), q); // see below
+ setQToFitRotationImpl(sbs, X_FM.R(), q);
+}
+
+void setQToFitRotationImpl(const SBStateDigest& sbs, const Rotation& R_FM,
+ Vector& q) const OVERRIDE_11 {
+ this->toQVec3(q,0) = R_FM.convertRotationToBodyFixedXYZ();
+}
+
+void setQToFitTranslationImpl(const SBStateDigest& sbs, const Vec3& p_FM,
+ Vector& q) const OVERRIDE_11 {
+ this->toQVec3(q,3) = p_FM; // skip the 3 Euler angles
+}
+
+// Given the angular velocity of M in F, expressed in F, compute the Euler
+// angle derivatives qdot that would produce that angular velocity, and
+// return u=qdot.
+void setUToFitAngularVelocityImpl
+ (const SBStateDigest& sbs, const Vector& q, const Vec3& w_FM,
+ Vector& u) const OVERRIDE_11
+{
+ const Vec2 cosxy(std::cos(q[0]), std::cos(q[1]));
+ const Vec2 sinxy(std::sin(q[0]), std::sin(q[1]));
+ const Real oocosy = 1 / cosxy[1];
+ const Vec3 qdot =
+ Rotation::convertAngVelInParentToBodyXYZDot(cosxy,sinxy,oocosy,w_FM);
+ this->toUVec3(u,0) = qdot;
+}
+
+void setUToFitLinearVelocityImpl
+ (const SBStateDigest& sbs, const Vector&, const Vec3& v_FM,
+ Vector& u) const OVERRIDE_11
+{
+ this->toUVec3(u,3) = v_FM;
+}
+
+// We want to cache cos and sin for each angle, and also 1/cos of the middle
+// angle will be handy to have around.
+enum {PoolSize=7};
+// cos x,y,z sin x,y,z 1/cos(y)
+enum {CosQ=0, SinQ=3, OOCosQy=6};
+int calcQPoolSize(const SBModelVars& mv) const OVERRIDE_11
+{ return PoolSize; }
+
+// This is expensive since we have three sin/cos computations and a divide
+// to do, approx. 150 flops. But we hope to re-use these calculations several
+// times before we're done with a complete realization.
+void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* qCache, int nQCache,
+ Real* qErr, int nQErr) const OVERRIDE_11
+{
+ assert(q && nq==6 && qCache && nQCache==PoolSize && nQErr==0);
+
+ const Real cy = std::cos(q[1]);
+ Vec3::updAs(&qCache[CosQ]) =
+ Vec3(std::cos(q[0]), cy, std::cos(q[2]));
+ Vec3::updAs(&qCache[SinQ]) =
+ Vec3(std::sin(q[0]), std::sin(q[1]), std::sin(q[2]));
+ qCache[OOCosQy] = 1/cy; // trouble at 90 or 270 (-90) degrees
+}
+
+// Because of the precalculations we can calculate the cross-mobilizer
+// transform in only 18 flops.
+void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* qCache, int nQCache,
+ Transform& X_F0M0) const OVERRIDE_11
+{
+ assert(q && nq==6 && qCache && nQCache==PoolSize);
+
+ X_F0M0.updR().setRotationToBodyFixedXYZ // 18 flops
+ (Vec3::getAs(&qCache[CosQ]), Vec3::getAs(&qCache[SinQ]));
+ X_F0M0.updP() = Vec3::getAs(&q[3]); // a012 x y z
+}
+
+// Generalized speeds are the Euler angle derivatives and the velocity
+// vector v_FM. The H_FM matrix maps those into the angular velocity of
+// M in F, expressed in F, and the linear velocity
+// [w_FM] [Hw_FM]
+// [ ] = [ ] * qdot (where v_FM=qdot(3,3))
+// [v_FM] [ 0 I ]
+// Using the precalculations this requires only 3 flops, although there is
+// a fair bit of poking around in memory required.
+void calcAcrossJointVelocityJacobian(
+ const SBStateDigest& sbs,
+ HType& H_FM) const OVERRIDE_11
+{
+ const SBModelCache& mc = sbs.getModelCache();
+ // Use "upd" here because we're realizing positions now.
+ const SBTreePositionCache& pc = sbs.updTreePositionCache();
+ const Real* pool = this->getQPool(mc, pc);
+
+ const Real c0 = pool[CosQ], c1 = pool[CosQ+1];
+ const Real s0 = pool[SinQ], s1 = pool[SinQ+1];
+
+ // Fill in columns of H_FM. See Rotation::calcNInvForBodyXYZInParentFrame().
+ H_FM(0) = SpatialVec(Vec3(1, 0, 0), Vec3(0) ); // rotations
+ H_FM(1) = SpatialVec(Vec3(0, c0, s0), Vec3(0) );
+ H_FM(2) = SpatialVec(Vec3(s1, -s0*c1, c0*c1), Vec3(0) );
+ H_FM(3) = SpatialVec( Vec3(0), Vec3(1,0,0)); // translations
+ H_FM(4) = SpatialVec( Vec3(0), Vec3(0,1,0));
+ H_FM(5) = SpatialVec( Vec3(0), Vec3(0,0,1));
+}
+
+// Differentiate H_FM to get HDot_FM. Note that this depends on qdot:
+// d/dt cos(q0) = -sin(q0)*qdot0, etc.
+void calcAcrossJointVelocityJacobianDot(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const OVERRIDE_11
+{
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+
+ const Real* pool = this->getQPool(mc, pc);
+ const Real c0 = pool[CosQ], c1 = pool[CosQ+1];
+ const Real s0 = pool[SinQ], s1 = pool[SinQ+1];
+
+ // Use "upd" here because we're realizing velocities now.
+ const Vec6& qdot = this->fromQ(sbs.updQDot());
+ const Real qd0 = qdot[0], qd1 = qdot[1];
+
+ const Real dc0 = -s0*qd0, dc1 = -s1*qd1; // derivatives of c0,c1,s0,s1
+ const Real ds0 = c0*qd0, ds1 = c1*qd1;
+
+ // Compare with H_FM above.
+ HDot_FM(0) = SpatialVec(Vec3(0, 0, 0), Vec3(0));
+ HDot_FM(1) = SpatialVec(Vec3(0, dc0, ds0), Vec3(0));
+ HDot_FM(2) = SpatialVec(Vec3(ds1, -ds0*c1-s0*dc1, dc0*c1+c0*dc1), Vec3(0));
+ HDot_FM(3) = SpatialVec( Vec3(0), Vec3(0));
+ HDot_FM(4) = SpatialVec( Vec3(0), Vec3(0));
+ HDot_FM(5) = SpatialVec( Vec3(0), Vec3(0));
+}
+
+// Can use default for calcQDot, multiplyByN, etc., since qdot==u for Bushing
+// mobilizer.
+
+};
+
+
+
+#endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_BUSHING_H_
+
diff --git a/Simbody/src/RigidBodyNodeSpec_Custom.h b/Simbody/src/RigidBodyNodeSpec_Custom.h
new file mode 100644
index 0000000..a639cdf
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec_Custom.h
@@ -0,0 +1,269 @@
+#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_CUSTOM_H_
+#define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_CUSTOM_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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Define the RigidBodyNode that implements Custom mobilizers.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "RigidBodyNodeSpec.h"
+#include "MobilizedBodyImpl.h" // need Custom::ImplementationImpl
+
+
+/**
+ * RigidBodyNodeSpec for Custom Mobilizers. This is still templatized
+ * by the number of u's (mobilities) in the user-defined Mobilizer.
+ */
+template <int nu, bool noX_MB, bool noR_PF>
+class RBNodeCustom : public RigidBodyNodeSpec<nu, false, noX_MB, noR_PF> {
+ typedef typename RigidBodyNodeSpec<nu, false, noX_MB, noR_PF>::HType HType;
+public:
+ RBNodeCustom(const MobilizedBody::Custom::Implementation& impl,
+ const MassProperties& mProps_B,
+ const Transform& X_PF,
+ const Transform& X_BM,
+ bool isReversed,
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot)
+ : RigidBodyNodeSpec<nu, false, noX_MB, noR_PF>(mProps_B, X_PF, X_BM, nextUSlot, nextUSqSlot, nextQSlot,
+ RigidBodyNode::QDotMayDifferFromU,
+ impl.getImpl().getNumAngles() == 4 ? RigidBodyNode::QuaternionMayBeUsed
+ : RigidBodyNode::QuaternionIsNeverUsed,
+ isReversed),
+ impl(impl), nq(impl.getImpl().getNQ()), nAngles(impl.getImpl().getNumAngles())
+ {
+ this->updateSlots(nextUSlot,nextUSqSlot,nextQSlot);
+ }
+ const char* type() const {
+ return "custom";
+ }
+ int getMaxNQ() const {
+ return nq;
+ }
+ int getNQInUse(const SBModelVars& mv) const {
+ return (nAngles == 4 && this->getUseEulerAngles(mv) ? nq-1 : nq);
+ }
+ virtual int getNUInUse(const SBModelVars& mv) const {
+ return nu;
+ }
+ bool isUsingQuaternion(const SBStateDigest& sbs, MobilizerQIndex& startOfQuaternion) const {
+ if (nAngles < 4 || this->getUseEulerAngles(sbs.getModelVars())) {
+ startOfQuaternion.invalidate();
+ return false;
+ }
+ startOfQuaternion = MobilizerQIndex(0); // quaternion comes first
+ return true;
+ }
+ void calcQDot(const SBStateDigest& sbs,
+ const Real* u, Real* qdot) const
+ {
+ const int nqInUse = getNQInUse(sbs.getModelVars());
+ impl.multiplyByN(sbs.getState(), false, nu, u, getNQInUse(sbs.getModelVars()), qdot);
+ for (int i = nqInUse; i < nq; ++i)
+ qdot[i] = 0.0;
+ }
+ void calcQDotDot(const SBStateDigest& sbs,
+ const Real* udot, Real* qdotdot) const
+ {
+ const SBModelVars& mv = sbs.getModelVars();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const int nqInUse = getNQInUse(sbs.getModelVars());
+ const Real* u = &sbs.getU()[this->getUIndex()];
+ impl.multiplyByN(sbs.getState(), false, nu, udot, nqInUse, qdotdot);
+ Real temp[7];
+ impl.multiplyByNDot(sbs.getState(), false, nu, u, nqInUse, temp);
+ for (int i = 0; i < nqInUse; ++i)
+ qdotdot[i] += temp[i];
+ for (int i = nqInUse; i < nq; ++i)
+ qdotdot[i] = 0.0;
+ }
+ void multiplyByN(const SBStateDigest& sbs, bool matrixOnRight,
+ const Real* in, Real* out) const
+ {
+ const SBModelVars& mv = sbs.getModelVars();
+ int nIn, nOut;
+ if (matrixOnRight) {
+ nIn = getNQInUse(mv);
+ nOut = getNUInUse(mv);
+ }
+ else {
+ nIn = getNUInUse(mv);
+ nOut = getNQInUse(mv);
+ }
+ impl.multiplyByN(sbs.getState(), matrixOnRight, nIn, in, nOut, out);
+ }
+ void multiplyByNInv(const SBStateDigest& sbs, bool matrixOnRight,
+ const Real* in, Real* out) const
+ {
+ const SBModelVars& mv = sbs.getModelVars();
+ int nIn, nOut;
+ if (matrixOnRight) {
+ nIn = getNUInUse(mv);
+ nOut = getNQInUse(mv);
+ }
+ else {
+ nIn = getNQInUse(mv);
+ nOut = getNUInUse(mv);
+ }
+ impl.multiplyByNInv(sbs.getState(), matrixOnRight, nIn, in, nOut, out);
+ }
+ void multiplyByNDot(const SBStateDigest& sbs, bool matrixOnRight,
+ const Real* in, Real* out) const
+ {
+ const SBModelVars& mv = sbs.getModelVars();
+ int nIn, nOut;
+ if (matrixOnRight) {
+ nIn = getNQInUse(mv);
+ nOut = getNUInUse(mv);
+ }
+ else {
+ nIn = getNUInUse(mv);
+ nOut = getNQInUse(mv);
+ }
+ impl.multiplyByNDot(sbs.getState(), matrixOnRight, nIn, in, nOut, out);
+ }
+
+ bool enforceQuaternionConstraints(const SBStateDigest& sbs, Vector& q, Vector& qErrest) const {
+ if (nAngles != 4 || this->getUseEulerAngles(sbs.getModelVars()))
+ return false;
+ Vec4& quat = this->toQuat(q);
+ quat = quat / quat.norm();
+ if (qErrest.size()) {
+ Vec4& qerr = this->toQuat(qErrest);
+ qerr -= dot(qerr,quat) * quat;
+ }
+ return true;
+ }
+
+ // Convert from quaternion to Euler angle representations.
+ void convertToEulerAngles(const Vector& inputQ, Vector& outputQ) const {
+ int indexBase = this->getQIndex();
+ if (nAngles != 4) {
+ for (int i = 0; i < nq; ++i)
+ outputQ[indexBase+i] = inputQ[indexBase+i];
+ }
+ else {
+ this->toQVec3(outputQ, 0) = Rotation(Quaternion(this->fromQuat(inputQ))).convertRotationToBodyFixedXYZ();
+ for (int i = 3; i < nq-1; ++i)
+ outputQ[indexBase+i] = inputQ[indexBase+i+1];
+ outputQ[indexBase+nq-1] = 0.0;
+ }
+ }
+ // Convert from Euler angle to quaternion representations.
+ void convertToQuaternions(const Vector& inputQ, Vector& outputQ) const {
+ int indexBase = this->getQIndex();
+ if (nAngles != 4) {
+ for (int i = 0; i < nq; ++i)
+ outputQ[indexBase+i] = inputQ[indexBase+i];
+ }
+ else {
+ Rotation rot;
+ rot.setRotationToBodyFixedXYZ(Vec3(inputQ[indexBase], inputQ[indexBase+1], inputQ[indexBase+2]));
+ this->toQuat(outputQ) = rot.convertRotationToQuaternion().asVec4();
+ for (int i = 4; i < nq; ++i)
+ outputQ[indexBase+i] = inputQ[indexBase+i-1];
+ }
+ };
+
+ void setQToFitTransformImpl(const SBStateDigest& sbs, const Transform& X_FM, Vector& q) const {
+ impl.setQToFitTransform(sbs.getState(), X_FM, this->getNQInUse(sbs.getModelVars()), &q[this->getQIndex()]);
+ }
+ void setQToFitRotationImpl(const SBStateDigest& sbs, const Rotation& R_FM, Vector& q) const {
+ setQToFitTransformImpl(sbs, Transform(R_FM), q);
+ }
+ void setQToFitTranslationImpl(const SBStateDigest& sbs, const Vec3& p_FM, Vector& q) const {
+ setQToFitTransformImpl(sbs, Transform(p_FM), q);
+ }
+
+ void setUToFitVelocityImpl(const SBStateDigest& sbs, const Vector& q, const SpatialVec& V_FM, Vector& u) const {
+ impl.setUToFitVelocity(sbs.getState(), V_FM, nu, &u[this->getUIndex()]);
+ }
+ void setUToFitAngularVelocityImpl(const SBStateDigest& sbs, const Vector& q, const Vec3& w_FM, Vector& u) const {
+ setUToFitVelocityImpl(sbs, q, SpatialVec(w_FM, Vec3(0)), u);
+ }
+ void setUToFitLinearVelocityImpl(const SBStateDigest& sbs, const Vector& q, const Vec3& v_FM, Vector& u) const {
+ setUToFitVelocityImpl(sbs, q, SpatialVec(Vec3(0), v_FM), u);
+ }
+
+ // VIRTUAL METHODS FOR SINGLE-NODE OPERATOR CONTRIBUTIONS //
+
+ // We're not going to attempt to cache any q precalculations.
+ int calcQPoolSize(const SBModelVars&) const {return 0;}
+
+ void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* qCache, int nQCache,
+ Real* qErr, int nQErr) const
+ {
+ assert(nq==getNQInUse(sbs.getModelVars()) && nQCache==0);
+ if (nAngles == 4 && !this->getUseEulerAngles(sbs.getModelVars())) {
+ // Need to calculate qerr
+ assert(nQErr==1);
+ const Real quatLen = Vec4::getAs(&q[0]).norm();
+ qErr[0] = quatLen - Real(1); // normalization error
+ }
+ }
+
+ void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* qCache, int nQCache,
+ Transform& X_F0M0) const
+ {
+ assert(nq==getNQInUse(sbs.getModelVars()) && nQCache==0);
+ // Note: quaternion will be unnormalized.
+ X_F0M0 = impl.calcMobilizerTransformFromQ(sbs.getState(), nq, q);
+ }
+
+ void calcAcrossJointVelocityJacobian(
+ const SBStateDigest& sbs,
+ HType& H_F0M0) const {
+ for (int i = 0; i < nu; ++i) {
+ Vec<nu> u(0);
+ u[i] = 1;
+ H_F0M0(i) = impl.multiplyByHMatrix(sbs.getState(), nu, &u[0]);
+ }
+ }
+
+ void calcAcrossJointVelocityJacobianDot(
+ const SBStateDigest& sbs,
+ HType& HDot_F0M0) const {
+ for (int i = 0; i < nu; ++i) {
+ Vec<nu> u(0);
+ u[i] = 1;
+ HDot_F0M0(i) = impl.multiplyByHDotMatrix(sbs.getState(), nu, &u[0]);
+ }
+ }
+
+private:
+ const MobilizedBody::Custom::Implementation& impl;
+ const int nq, nAngles;
+};
+
+
+#endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_CUSTOM_H_
+
diff --git a/Simbody/src/RigidBodyNodeSpec_Cylinder.h b/Simbody/src/RigidBodyNodeSpec_Cylinder.h
new file mode 100644
index 0000000..20e515a
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec_Cylinder.h
@@ -0,0 +1,163 @@
+#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_CYLINDER_H_
+#define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_CYLINDER_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Derived from IVM code written by Charles Schwieters *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+
+/**@file
+ * Define the RigidBodyNode that implements a Cylinder mobilizer.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "RigidBodyNodeSpec.h"
+
+
+ // CYLINDER //
+
+// This is a "cylinder" joint, meaning one degree of rotational freedom
+// about a particular axis, and one degree of translational freedom
+// along the same axis. For molecules you can think of this as a combination
+// of torsion and bond stretch. The axis used is the z axis of the parent's
+// F frame, which is aligned forever with the z axis of the body's M frame.
+// In addition, the origin points of M and F are separated only along the
+// z axis; i.e., they have the same x & y coords in the F frame. The two
+// generalized coordinates are the rotation and the translation, in that order.
+template<bool noX_MB, bool noR_PF>
+class RBNodeCylinder : public RigidBodyNodeSpec<2, false, noX_MB, noR_PF> {
+public:
+ typedef typename RigidBodyNodeSpec<2, false, noX_MB, noR_PF>::HType HType;
+ virtual const char* type() { return "cylinder"; }
+
+ RBNodeCylinder(const MassProperties& mProps_B,
+ const Transform& X_PF,
+ const Transform& X_BM,
+ bool isReversed,
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot)
+ : RigidBodyNodeSpec<2, false, noX_MB, noR_PF>(mProps_B,X_PF,X_BM,nextUSlot,nextUSqSlot,nextQSlot,
+ RigidBodyNode::QDotIsAlwaysTheSameAsU, RigidBodyNode::QuaternionIsNeverUsed, isReversed)
+ {
+ this->updateSlots(nextUSlot,nextUSqSlot,nextQSlot);
+ }
+
+
+ void setQToFitRotationImpl(const SBStateDigest& sbs, const Rotation& R_FM, Vector& q) const {
+ // The only rotation our cylinder joint can handle is about z.
+ // TODO: this code is bad -- see comments for Torsion joint above.
+ const Vec3 angles123 = R_FM.convertRotationToBodyFixedXYZ();
+ this->toQ(q)[0] = angles123[2];
+ }
+
+ void setQToFitTranslationImpl(const SBStateDigest& sbs, const Vec3& p_FM, Vector& q) const {
+ // Because the M and F origins must lie along their shared z axis, there is no way to
+ // create a translation by rotating around z. So the only translation we can represent
+ // is that component which is along z.
+ this->toQ(q)[1] = p_FM[2];
+ }
+
+ void setUToFitAngularVelocityImpl(const SBStateDigest& sbs, const Vector&, const Vec3& w_FM, Vector& u) const {
+ // We can only represent an angular velocity along z with this joint.
+ this->toU(u)[0] = w_FM[2];
+ }
+
+ void setUToFitLinearVelocityImpl
+ (const SBStateDigest& sbs, const Vector&, const Vec3& v_FM, Vector& u) const
+ {
+ // Because the M and F origins must lie along their shared z axis, there is no way to
+ // create a linear velocity by rotating around z. So the only linear velocity we can represent
+ // is that component which is along z.
+ this->toU(u)[1] = v_FM[2];
+ }
+
+ enum {CosQ=0, SinQ=1};
+ // We want space for cos(q0) and sin(q0).
+ int calcQPoolSize(const SBModelVars&) const
+ { return 2; }
+
+ void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* qCache, int nQCache,
+ Real* qErr, int nQErr) const
+ {
+ assert(q && nq==2 && qCache && nQCache==2 && nQErr==0);
+ qCache[CosQ] = std::cos(q[0]);
+ qCache[SinQ] = std::sin(q[0]);
+ }
+
+ void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* qCache, int nQCache,
+ Transform& X_FM) const
+ {
+ assert(q && nq==2 && qCache && nQCache==2);
+ X_FM.updR().setRotationFromAngleAboutZ(qCache[CosQ], qCache[SinQ]);
+ X_FM.updP() = Vec3(0,0,q[1]);
+ }
+
+
+ // The generalized speeds are (1) the angular velocity of M in the F frame,
+ // about F's z axis, expressed in F, and (2) the velocity of M's origin
+ // in F, along F's z axis. (The z axis is also constant in M for this joint.)
+ void calcAcrossJointVelocityJacobian(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+ {
+ H_FM(0) = SpatialVec( Vec3(0,0,1), Vec3(0) );
+ H_FM(1) = SpatialVec( Vec3(0), Vec3(0,0,1) );
+ }
+
+ // Since the Jacobian above is constant in F, its time derivative in F is zero.
+ void calcAcrossJointVelocityJacobianDot(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+ {
+ HDot_FM(0) = SpatialVec( Vec3(0), Vec3(0) );
+ HDot_FM(1) = SpatialVec( Vec3(0), Vec3(0) );
+ }
+
+ // Override the computation of reverse-H for this simple mobilizer.
+ void calcReverseMobilizerH_FM(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+ {
+ H_FM(0) = SpatialVec( Vec3(0,0,-1), Vec3(0) );
+ H_FM(1) = SpatialVec( Vec3(0), Vec3(0,0,-1) );
+ }
+
+ // Override the computation of reverse-HDot for this simple mobilizer.
+ void calcReverseMobilizerHDot_FM(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+ {
+ HDot_FM(0) = SpatialVec( Vec3(0), Vec3(0) );
+ HDot_FM(1) = SpatialVec( Vec3(0), Vec3(0) );
+ }
+};
+
+
+
+#endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_CYLINDER_H_
+
diff --git a/Simbody/src/RigidBodyNodeSpec_Derived.cpp b/Simbody/src/RigidBodyNodeSpec_Derived.cpp
new file mode 100644
index 0000000..67fbf56
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec_Derived.cpp
@@ -0,0 +1,302 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Peter Eastman, Charles Schwieters *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This file contains implementations for any of the classes derived from
+ * RigidBodyNodeSpec that aren't defined in the headers.
+ */
+
+#include "RigidBodyNodeSpec_Pin.h"
+#include "RigidBodyNodeSpec_Slider.h"
+#include "RigidBodyNodeSpec_Cylinder.h"
+#include "RigidBodyNodeSpec_SphericalCoords.h"
+#include "RigidBodyNodeSpec_Ball.h"
+#include "RigidBodyNodeSpec_Ellipsoid.h"
+#include "RigidBodyNodeSpec_Free.h"
+#include "RigidBodyNodeSpec_Screw.h"
+#include "RigidBodyNodeSpec_Universal.h"
+#include "RigidBodyNodeSpec_PolarCoords.h"
+#include "RigidBodyNodeSpec_Planar.h"
+#include "RigidBodyNodeSpec_Gimbal.h"
+#include "RigidBodyNodeSpec_Bushing.h"
+#include "RigidBodyNodeSpec_FreeLine.h"
+#include "RigidBodyNodeSpec_LineOrientation.h"
+#include "RigidBodyNodeSpec_Custom.h"
+// Note: _Translation is handled separately so we can special case
+// a lone particle for speed if we find one.
+
+// A macro for instantiating rigid body nodes.
+#define INSTANTIATE(CLASS, ...) \
+ bool noX_MB = (getDefaultOutboardFrame().p() == 0 && getDefaultOutboardFrame().R() == Mat33(1)); \
+ bool noR_PF = (getDefaultInboardFrame().R() == Mat33(1)); \
+ if (noX_MB) { \
+ if (noR_PF) \
+ return new CLASS<true, true> (__VA_ARGS__); \
+ else \
+ return new CLASS<true, false> (__VA_ARGS__); \
+ } \
+ else { \
+ if (noR_PF) \
+ return new CLASS<false, true> (__VA_ARGS__); \
+ else \
+ return new CLASS<false, false> (__VA_ARGS__); \
+ }
+
+ /////////////////////////////////////////////////////////
+ // MoblizedBodyImpl::createRigidBodyNode() definitions //
+ /////////////////////////////////////////////////////////
+
+
+// These probably don't belong here.
+
+#include "MobilizedBodyImpl.h"
+
+RigidBodyNode* MobilizedBody::PinImpl::createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const
+{
+ INSTANTIATE(RBNodeTorsion,
+ getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ isReversed(),
+ nextUSlot,nextUSqSlot,nextQSlot)
+}
+
+
+RigidBodyNode* MobilizedBody::SliderImpl::createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const
+{
+ INSTANTIATE(RBNodeSlider,
+ getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ isReversed(),
+ nextUSlot,nextUSqSlot,nextQSlot)
+}
+
+
+RigidBodyNode* MobilizedBody::BallImpl::createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const
+{
+ INSTANTIATE(RBNodeBall,
+ getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ isReversed(),
+ nextUSlot,nextUSqSlot,nextQSlot)
+}
+
+
+RigidBodyNode* MobilizedBody::FreeImpl::createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const
+{
+ INSTANTIATE(RBNodeFree,
+ getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ isReversed(),
+ nextUSlot,nextUSqSlot,nextQSlot)
+}
+
+
+
+RigidBodyNode* MobilizedBody::ScrewImpl::createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const
+{
+ INSTANTIATE(RBNodeScrew,
+ getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ getDefaultPitch(),
+ isReversed(),
+ nextUSlot,nextUSqSlot,nextQSlot)
+}
+
+
+RigidBodyNode* MobilizedBody::UniversalImpl::createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const
+{
+ INSTANTIATE(RBNodeUJoint,
+ getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ isReversed(),
+ nextUSlot,nextUSqSlot,nextQSlot)
+}
+
+RigidBodyNode* MobilizedBody::CylinderImpl::createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const
+{
+ INSTANTIATE(RBNodeCylinder, getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ isReversed(),
+ nextUSlot,nextUSqSlot,nextQSlot)
+}
+
+RigidBodyNode* MobilizedBody::BendStretchImpl::createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const
+{
+ INSTANTIATE(RBNodeBendStretch, getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ isReversed(),
+ nextUSlot,nextUSqSlot,nextQSlot)
+}
+
+RigidBodyNode* MobilizedBody::PlanarImpl::createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const
+{
+ INSTANTIATE(RBNodePlanar, getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ isReversed(),
+ nextUSlot,nextUSqSlot,nextQSlot)
+}
+
+RigidBodyNode* MobilizedBody::SphericalCoordsImpl::createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const
+{
+ INSTANTIATE(RBNodeSphericalCoords, getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ az0, negAz, ze0, negZe, axisT, negT,
+ isReversed(),
+ nextUSlot,nextUSqSlot,nextQSlot)
+}
+
+RigidBodyNode* MobilizedBody::GimbalImpl::createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const
+{
+ INSTANTIATE(RBNodeGimbal, getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ isReversed(),
+ nextUSlot,nextUSqSlot,nextQSlot)
+}
+
+RigidBodyNode* MobilizedBody::BushingImpl::createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const
+{
+ INSTANTIATE(RBNodeBushing, getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ isReversed(),
+ nextUSlot,nextUSqSlot,nextQSlot)
+}
+
+RigidBodyNode* MobilizedBody::EllipsoidImpl::createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const
+{
+ INSTANTIATE(RBNodeEllipsoid,
+ getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ getDefaultRadii(),
+ isReversed(),
+ nextUSlot,nextUSqSlot,nextQSlot)
+}
+
+RigidBodyNode* MobilizedBody::LineOrientationImpl::createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const
+{
+ INSTANTIATE(RBNodeLineOrientation,
+ getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ isReversed(),
+ nextUSlot,nextUSqSlot,nextQSlot)
+}
+
+RigidBodyNode* MobilizedBody::FreeLineImpl::createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const
+{
+ INSTANTIATE(RBNodeFreeLine,
+ getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ isReversed(),
+ nextUSlot,nextUSqSlot,nextQSlot)
+}
+
+#define INSTANTIATE_CUSTOM(DOF, ...) \
+ if (noX_MB) { \
+ if (noR_PF) \
+ return new RBNodeCustom<DOF, true, true> (__VA_ARGS__); \
+ else \
+ return new RBNodeCustom<DOF, true, false> (__VA_ARGS__); \
+ } \
+ else { \
+ if (noR_PF) \
+ return new RBNodeCustom<DOF, false, true> (__VA_ARGS__); \
+ else \
+ return new RBNodeCustom<DOF, false, false> (__VA_ARGS__); \
+ }
+
+RigidBodyNode* MobilizedBody::CustomImpl::createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const
+{
+ bool noX_MB = (getDefaultOutboardFrame().p() == 0 && getDefaultOutboardFrame().R() == Mat33(1));
+ bool noR_PF = (getDefaultInboardFrame().R() == Mat33(1));
+ switch (getImplementation().getImpl().getNU()) {
+ case 1:
+ INSTANTIATE_CUSTOM(1, getImplementation(), getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(), getDefaultOutboardFrame(), isReversed(), nextUSlot, nextUSqSlot, nextQSlot)
+ case 2:
+ INSTANTIATE_CUSTOM(2, getImplementation(), getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(), getDefaultOutboardFrame(), isReversed(), nextUSlot, nextUSqSlot, nextQSlot)
+ case 3:
+ INSTANTIATE_CUSTOM(3, getImplementation(), getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(), getDefaultOutboardFrame(), isReversed(), nextUSlot, nextUSqSlot, nextQSlot)
+ case 4:
+ INSTANTIATE_CUSTOM(4, getImplementation(), getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(), getDefaultOutboardFrame(), isReversed(), nextUSlot, nextUSqSlot, nextQSlot)
+ case 5:
+ INSTANTIATE_CUSTOM(5, getImplementation(), getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(), getDefaultOutboardFrame(), isReversed(), nextUSlot, nextUSqSlot, nextQSlot)
+ case 6:
+ INSTANTIATE_CUSTOM(6, getImplementation(), getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(), getDefaultOutboardFrame(), isReversed(), nextUSlot, nextUSqSlot, nextQSlot)
+ default:
+ assert(!"Illegal number of degrees of freedom for custom MobilizedBody");
+ return 0;
+ }
+}
+
diff --git a/Simbody/src/RigidBodyNodeSpec_Ellipsoid.h b/Simbody/src/RigidBodyNodeSpec_Ellipsoid.h
new file mode 100644
index 0000000..8df5a41
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec_Ellipsoid.h
@@ -0,0 +1,552 @@
+#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_ELLIPSOID_H_
+#define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_ELLIPSOID_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * Charles Schwieters (NIH): wrote the public domain IVM code from which *
+ * this was derived. *
+ * Ajay Seth: wrote the Ellipsoid joint as a Custom mobilizer; Sherm *
+ * derived this implementation from Ajay's *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Define the RigidBodyNode that implements an Ellipsoid mobilizer.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "RigidBodyNodeSpec.h"
+
+
+ // ELLIPSOID //
+
+// ELLIPSOID mobilizer. This provides three degrees of rotational freedom, i.e.,
+// unrestricted orientation, of the body's M frame in the parent's F frame,
+// along with coordinated translation that keeps the M frame origin on
+// the surface of an ellipsoid fixed in F and centered on the F origin.
+// The surface point is chosen for a given orientation of M in F as the
+// unique point on the ellipsoid surface where the surface normal is aligned
+// with Mz. That is, the z axis of M is assumed to be normal to the ellipsoid
+// at all times, and the translation is chosen to make that true.
+//
+// Unlike most joints, the reference configuration (i.e., X_FM when q=0 is
+// not the identity transform. Instead, although the frames are aligned the
+// M frame origin is offset from F along their shared +z axis, so that it lies
+// on the ellipsoid surface at the point (0,0,rz) where rz is the z-radius
+// (semiaxis) of the ellipsoid.
+//
+// The generalized coordinates are:
+// * 4 quaternions or 3 1-2-3 body fixed Euler angles (that is, fixed in M)
+// In Euler angles, axis 3 is just the spin of the outboard body about
+// its Mz axis, which is always normal to the ellipse. For small 1,2 angles
+// you can think of angle 1 (about x) as latitude, and angle 2 (about y)
+// as longitude when viewing the ellipsoid by looking down the z axis.
+// (That would be true for large angles too if we were using space fixed
+// angles, that is, angles defined about F rather than M axes.)
+//
+// Generalized speeds are:
+// * angular velocity w_FM as a vector expressed in the F frame.
+//
+// Thus rotational qdots have to be derived from the generalized speeds to
+// be turned into either 4 quaternion derivatives or 3 Euler angle derivatives.
+//
+// This mobilizer was written by Ajay Seth and hacked somewhat by Sherm.
+
+template<bool noX_MB, bool noR_PF>
+class RBNodeEllipsoid : public RigidBodyNodeSpec<3, false, noX_MB, noR_PF> {
+ Vec3 semi; // semi axis dimensions in x,y,z resp.
+public:
+
+typedef typename RigidBodyNodeSpec<3, false, noX_MB, noR_PF>::HType HType;
+virtual const char* type() { return "ellipsoid"; }
+
+RBNodeEllipsoid(const MassProperties& mProps_B,
+ const Transform& X_PF,
+ const Transform& X_BM,
+ const Vec3& radii, // x,y,z
+ bool isReversed,
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot)
+ : RigidBodyNodeSpec<3, false, noX_MB, noR_PF>(mProps_B,X_PF,X_BM,nextUSlot,nextUSqSlot,nextQSlot,
+ RigidBodyNode::QDotMayDifferFromU, RigidBodyNode::QuaternionMayBeUsed, isReversed),
+ semi(radii)
+{
+ this->updateSlots(nextUSlot,nextUSqSlot,nextQSlot);
+}
+
+void setQToFitRotationImpl(const SBStateDigest& sbs, const Rotation& R_FM,
+ Vector& q) const
+{
+ if (this->getUseEulerAngles(sbs.getModelVars()))
+ this->toQ(q) = R_FM.convertRotationToBodyFixedXYZ();
+ else
+ this->toQuat(q) = R_FM.convertRotationToQuaternion().asVec4();
+}
+
+// We can't hope to represent arbitrary translations with a joint that has
+// only rotational coordinates! However, since F is at the center of the
+// ellipsoid and M on its surface, we can at least obtain a translation in
+// the *direction* of the requested translation. The magnitude must of
+// course be set to end up with the M origin right on the surface of the
+// ellipsoid, and Mz will be the normal at that point.
+//
+// Expressed as an x-y-z body fixed Euler sequence, the z rotation is just
+// the spin around the Mz (surface normal) and could be anything, so we'll
+// just leave it at its current value. The x and y rotations act like polar
+// coordinates to get the M origin point on the direction indicated by the
+// requested translation.
+//
+// If the requested translation is near zero we can't do anything since we
+// can't find a direction to align with. And of course we can't do anything
+// if "only" is true here -- that means we aren't allowed to touch the
+// rotations, and for this joint that's all there is.
+void setQToFitTranslationImpl(const SBStateDigest& sbs, const Vec3& p_FM, Vector& q) const {
+ if (p_FM.norm() < Eps) return;
+
+ const UnitVec3 e(p_FM); // direction from F origin towards desired M origin
+ const Real latitude = std::atan2(-e[1],e[2]); // project onto F's yz plane
+ const Real longitude = std::atan2( e[0],e[2]); // project onto F's xz plane
+
+ // Calculate the current value of the spin coordinate (3rd Euler angle).
+ Real spin;
+ if (this->getUseEulerAngles(sbs.getModelVars())){
+ spin = this->fromQ(q)[2];
+ } else {
+ const Rotation R_FM_now(Quaternion(this->fromQuat(q)));
+ spin = R_FM_now.convertRotationToBodyFixedXYZ()[2];
+ }
+
+ // Calculate the desired rotation, which is a space-fixed 1-2 sequence for
+ // latitude and longitude, followed by a body fixed rotation for spin.
+ const Rotation R_FM = Rotation( SpaceRotationSequence, latitude, XAxis, longitude, YAxis ) * Rotation(spin,ZAxis);
+
+ if (this->getUseEulerAngles(sbs.getModelVars())) {
+ const Vec3 q123 = R_FM.convertRotationToBodyFixedXYZ();
+ this->toQ(q) = q123;
+ } else {
+ const Quaternion quat = R_FM.convertRotationToQuaternion();
+ this->toQuat(q) = quat.asVec4();
+ }
+}
+
+void setUToFitAngularVelocityImpl(const SBStateDigest& sbs, const Vector& q, const Vec3& w_FM,
+ Vector& u) const
+{
+ this->toU(u) = w_FM; // relative ang. vel. always used as generalized speeds
+}
+
+// We can't do general linear velocity with this rotation-only mobilizer, but
+// we can express any velocity which is tangent to the ellipsoid surface. So
+// we'll find the current surface normal (Mz) and ignore any component of the
+// requested velocity which is along that direction. (The resulting vz won't
+// be zero, though, but it is completely determined by vx,vy.)
+void setUToFitLinearVelocityImpl
+ (const SBStateDigest& sbs, const Vector& q, const Vec3& v_FM, Vector& u) const
+{
+ Transform X_FM;
+ this->calcAcrossJointTransform(sbs,q,X_FM);
+
+ const Vec3 v_FM_M = ~X_FM.R()*v_FM; // we can only do vx and vy in this frame
+ const Vec3 r_FM_M = ~X_FM.R()*X_FM.p();
+ const Vec3 wnow_FM_M = ~X_FM.R()*this->fromU(u); // preserve z component
+
+ // Now vx can only result from angular velocity about y, vy from x.
+ // TODO: THIS IS ONLY RIGHT FOR A SPHERE!!!
+ const Real wx = -v_FM_M[1]/r_FM_M[2];
+ const Real wy = v_FM_M[0]/r_FM_M[2];
+ const Vec3 w_FM_M(wx, wy, wnow_FM_M[2]);
+ const Vec3 w_FM = X_FM.R()*w_FM_M;
+
+ this->toU(u) = w_FM;
+}
+
+// When we're using Euler angles, we're going to want to cache cos and sin for
+// each angle, and also 1/cos of the middle angle will be handy to have around.
+// When we're using quaternions, we'll cache 1/|q| for convenient normalizing
+// of quaternions.
+enum {AnglePoolSize=7, QuatPoolSize=1};
+// cos x,y,z sin x,y,z 1/cos(y)
+enum {AngleCosQ=0, AngleSinQ=3, AngleOOCosQy=6};
+enum {QuatOONorm=0};
+int calcQPoolSize(const SBModelVars& mv) const
+{ return this->getUseEulerAngles(mv) ? AnglePoolSize : QuatPoolSize; }
+
+// This is expensive in Euler angle mode due to the three sin/cos computations
+// required (about 150 flops). In quaternion mode it is much cheaper (about
+// 25 flops).
+void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* qCache, int nQCache,
+ Real* qErr, int nQErr) const
+{
+ if (this->getUseEulerAngles(sbs.getModelVars())) {
+ assert(q && nq==3 && qCache && nQCache==AnglePoolSize && nQErr==0);
+
+ const Real cy = std::cos(q[1]);
+ Vec3::updAs(&qCache[AngleCosQ]) =
+ Vec3(std::cos(q[0]), cy, std::cos(q[2]));
+ Vec3::updAs(&qCache[AngleSinQ]) =
+ Vec3(std::sin(q[0]), std::sin(q[1]), std::sin(q[2]));
+ qCache[AngleOOCosQy] = 1/cy; // trouble at 90 degrees
+ } else {
+ assert(q && nq==4 && qCache && nQCache==QuatPoolSize &&
+ qErr && nQErr==1);
+ const Real quatLen = Vec4::getAs(q).norm();
+ qErr[0] = quatLen - Real(1); // normalization error
+ qCache[QuatOONorm] = 1/quatLen; // save for later
+ }
+}
+
+// Because of the precalculations above we can calculate the cross-mobilizer
+// transform in 21 flops (Euler angles) or 36 flops (quaternions).
+void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* qCache, int nQCache,
+ Transform& X_F0M0) const
+{
+ // Rotation
+ if (this->getUseEulerAngles(sbs.getModelVars())) {
+ assert(q && nq==3 && qCache && nQCache==AnglePoolSize);
+ X_F0M0.updR().setRotationToBodyFixedXYZ // 18 flops
+ (Vec3::getAs(&qCache[AngleCosQ]), Vec3::getAs(&qCache[AngleSinQ]));
+ } else {
+ assert(q && nq==4 && qCache && nQCache==QuatPoolSize);
+ // Must use a normalized quaternion to generate the rotation matrix.
+ // Here we normalize with just 4 flops using precalculated 1/norm(q).
+ const Quaternion quat(Vec4::getAs(q)*qCache[QuatOONorm], true);
+ X_F0M0.updR().setRotationFromQuaternion(quat); // 29 flops
+ }
+
+ // Translation.
+ const Vec3& n = X_F0M0.z(); // just calculated above
+ X_F0M0.updP() = Vec3(semi[0]*n[0], semi[1]*n[1], semi[2]*n[2]);
+}
+
+// Generalized speeds are the angular velocity expressed in F, so they
+// cause rotations around F x,y,z axes respectively. (9 flops)
+void calcAcrossJointVelocityJacobian(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+{
+ const SBTreePositionCache& pc = sbs.updTreePositionCache(); // "upd" because we're realizing positions now
+
+ // The normal is M's z axis, expressed in F but only in the frames we
+ // used to *define* this mobilizer, not necessarily the ones used after
+ // handling mobilizer reversal.
+ const Vec3 n = this->findX_F0M0(pc).z();
+
+ H_FM(0) = SpatialVec( Vec3(1,0,0), Vec3( 0, -n[2]*semi[1], n[1]*semi[2]) );
+ H_FM(1) = SpatialVec( Vec3(0,1,0), Vec3( n[2]*semi[0], 0, -n[0]*semi[2]) );
+ H_FM(2) = SpatialVec( Vec3(0,0,1), Vec3(-n[1]*semi[0], n[0]*semi[1], 0 ) );
+}
+
+// Calculate time derivative of H_FM, which is *not* constant. (18 flops)
+void calcAcrossJointVelocityJacobianDot(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+{
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const SBTreeVelocityCache& vc = sbs.updTreeVelocityCache(); // "upd" because we're realizing velocities now
+
+ // We need the normal and cross-joint velocity in the frames we're
+ // using to *define* the mobilizer, not necessarily the frames we're
+ // using to compute it it (if it has been reversed).
+ const Vec3 n = this->findX_F0M0(pc).z();
+ const Vec3 w_F0M0 = this->find_w_F0M0(pc, vc);
+ const Vec3 ndot = w_F0M0 % n; // w_FM x n (9 flops)
+
+ HDot_FM(0) = SpatialVec( Vec3(0), Vec3( 0, -ndot[2]*semi[1], ndot[1]*semi[2]) );
+ HDot_FM(1) = SpatialVec( Vec3(0), Vec3( ndot[2]*semi[0], 0, -ndot[0]*semi[2]) );
+ HDot_FM(2) = SpatialVec( Vec3(0), Vec3(-ndot[1]*semi[0], ndot[0]*semi[1], 0 ) );
+}
+
+// Calculate qdot=N(q)*u. Precalculations make this fast in Euler angle
+// mode (10 flops); quaternion mode is 27 flops.
+// TODO: we're expecting that there are 4 qdots even in Euler angle mode
+// so we have to zero out the last one in that case.
+void calcQDot(const SBStateDigest& sbs, const Real* u,
+ Real* qdot) const {
+ assert(sbs.getStage() >= Stage::Position);
+ assert(u && qdot);
+
+ const SBModelVars& mv = sbs.getModelVars();
+
+ // Generalized speeds are the same in either mode -- angular velocity
+ // of F in M (or B in P), expressed in F (fixed on parent) frame.
+ const Vec3& w_FM = Vec3::getAs(u);
+
+ if (this->getUseEulerAngles(mv)) {
+ // Euler angle mode (10 flops)
+ qdot[3] = 0; // TODO: kludge, clear unused element
+
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Real* pool = this->getQPool(mc, pc);
+
+ Vec3::updAs(qdot) = Rotation::convertAngVelInParentToBodyXYZDot(
+ Vec2::getAs(&pool[AngleCosQ]), Vec2::getAs(&pool[AngleSinQ]), //x,y
+ pool[AngleOOCosQy], w_FM);
+ } else {
+ // Quaternion mode (27 flops)
+ const Vec4& quat = this->fromQuat(sbs.getQ()); // unnormalized
+ Vec4::updAs(qdot) = Rotation::convertAngVelToQuaternionDot(quat, w_FM);
+ }
+}
+
+// CAUTION: we do not zero the unused 4th element of q for Euler angles; it
+// is up to the caller to do that if it is necessary.
+// Compute out_q = N * in_u
+// or out_u = in_q * N
+// The u quantity is a vector v_F in the parent frame (e.g. angular
+// velocity of child in parent), while the q quantity is the derivative
+// of a quaternion representing R_FM, i.e. orientation of child in parent.
+// Cost: Euler angle mode - 10 flops, Quaternion mode - 27 flops.
+void multiplyByN(const SBStateDigest& sbs, bool matrixOnRight,
+ const Real* in, Real* out) const
+{
+ assert(sbs.getStage() >= Stage::Position);
+ assert(in && out);
+
+ const SBModelVars& mv = sbs.getModelVars();
+
+ if (this->getUseEulerAngles(mv)) {
+ // Euler angle mode (10 flops).
+
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Real* pool = this->getQPool(mc, pc);
+
+ // Aliases.
+ const Vec2& cosxy = Vec2::getAs(&pool[AngleCosQ]); // only need x,y
+ const Vec2& sinxy = Vec2::getAs(&pool[AngleSinQ]);
+ const Real& oocosy = pool[AngleOOCosQy];
+ const Vec3& in_v = Vec3::getAs(in);
+ Vec3& out_v = Vec3::updAs(out);
+
+ if (matrixOnRight) { // out_u = in_q * N = ~N * in_q (9 flops)
+ // Transpose on left is same as untransposed on right.
+ out_v = Rotation::multiplyByBodyXYZ_NT_P(cosxy, sinxy, oocosy, in_v);
+ } else { // out_q = N*in_u (10 flops)
+ out_v = Rotation::multiplyByBodyXYZ_N_P(cosxy, sinxy, oocosy, in_v);
+ }
+ } else {
+ // Quaternion mode (27 flops).
+ const Vec4& quat = this->fromQuat(sbs.getQ()); // unnormalized
+ const Mat43 N_F = // This method returns N for the parent frame
+ Rotation::calcUnnormalizedNForQuaternion(quat); // 7 flops
+ if (matrixOnRight)
+ Row3::updAs(out) = Row4::getAs(in) * N_F; // 20 flops
+ else Vec4::updAs(out) = N_F * Vec3::getAs(in);
+ }
+}
+
+// Compute out_u = inv(N) * in_q
+// or out_q = in_u * inv(N)
+// Cost: Euler angle mode ~ 10 flops, Quaternion mode ~ 27 flops.
+void multiplyByNInv(const SBStateDigest& sbs, bool matrixOnRight,
+ const Real* in, Real* out) const
+{
+ assert(sbs.getStage() >= Stage::Position);
+ assert(in && out);
+
+ const SBModelVars& mv = sbs.getModelVars();
+
+ if (this->getUseEulerAngles(mv)) {
+ // Euler angle mode (10 flops).
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Real* pool = this->getQPool(mc, pc);
+
+ // Aliases.
+ const Vec2& cosxy = Vec2::getAs(&pool[AngleCosQ]); // only need x,y
+ const Vec2& sinxy = Vec2::getAs(&pool[AngleSinQ]);
+ const Vec3& in_v = Vec3::getAs(in);
+ Vec3& out_v = Vec3::updAs(out);
+
+ if (matrixOnRight) { // out_q = in_u * inv(N) = ~inv(N) * in_u (10 flops)
+ // transpose on left is same as untransposed on right
+ out_v = Rotation::multiplyByBodyXYZ_NInvT_P(cosxy, sinxy, in_v);
+ } else { // out_u = inv(N) * in_q (9 flops)
+ out_v = Rotation::multiplyByBodyXYZ_NInv_P(cosxy, sinxy, in_v);
+ }
+ } else {
+ // Quaternion mode (27 flops).
+ const Vec4& quat = this->fromQuat(sbs.getQ()); // unnormalized
+ const Mat34 NInv_F = // Returns NInv for parent frame.
+ Rotation::calcUnnormalizedNInvForQuaternion(quat); // 7 flops
+ if (matrixOnRight)
+ Row4::updAs(out) = Row3::getAs(in) * NInv_F; // 20 flops
+ else Vec3::updAs(out) = NInv_F * Vec4::getAs(in); // 21 flops
+ }
+}
+
+// Compute out_q = NDot * in_u
+// or out_u = in_q * NDot
+// Note that it is much cheaper to calculate
+// qdotdot directly than to do it using N and NDot; see below.
+// Cost: Euler angle mode - 36 flops, Quaternion mode - 27 flops.
+void multiplyByNDot(const SBStateDigest& sbs, bool matrixOnRight,
+ const Real* in, Real* out) const
+{
+ assert(sbs.getStage() > Stage::Velocity);
+ assert(in && out);
+
+ const SBModelVars& mv = sbs.getModelVars();
+
+ if (this->getUseEulerAngles(mv)) {
+ // Euler angle mode (36 flops).
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Real* pool = this->getQPool(mc, pc);
+
+ // Aliases.
+ const Vec2& cosxy = Vec2::getAs(&pool[AngleCosQ]); // only need x,y
+ const Vec2& sinxy = Vec2::getAs(&pool[AngleSinQ]);
+ const Real& oocosy = pool[AngleOOCosQy];
+ const Vec3& qdot = this->fromQ(sbs.getQDot());
+
+ // We don't have a nice multiply-by routine here so just get the NDot
+ // matrix and use it (21 flops).
+ const Mat33 NDot_F =
+ Rotation::calcNDotForBodyXYZInParentFrame(cosxy, sinxy, oocosy, qdot);
+
+ if (matrixOnRight) { // out_u = in_q * NDot (15 flops)
+ Row3::updAs(out) = Row3::getAs(in) * NDot_F;
+ } else { // out_q = NDot * in_u (15 flops)
+ Vec3::updAs(out) = NDot_F * Vec3::getAs(in);
+ }
+ } else {
+ // Quaternion mode (27 flops).
+ const Vec4& qdot = this->fromQuat(sbs.getQDot()); // unnormalized
+ const Mat43 NDot_F = // This method returns NDot for the parent frame
+ Rotation::calcUnnormalizedNDotForQuaternion(qdot); // 7 flops
+ if (matrixOnRight)
+ Row3::updAs(out) = Row4::getAs(in) * NDot_F; // 21 flops
+ else Vec4::updAs(out) = NDot_F * Vec3::getAs(in); // 20 flops
+ }
+}
+
+// Calculate qdotdot from udot as qdotdot = N(q)*udot + NDot(q,qdot)*u, but
+// faster. Here we assume that qdot=N*u has already been calculated, which
+// is very helpful in Euler angle mode. In either mode it is a very bad
+// idea to calculate this using explicit N and NDot matrices; we can save
+// a lot of time by using more specialized methods.
+// Cost: Euler angle mode - 22 flops, Quaternion mode - 41 flops.
+void calcQDotDot(const SBStateDigest& sbs, const Real* udot,
+ Real* qdotdot) const {
+ assert(sbs.getStage() > Stage::Velocity);
+ assert(udot && qdotdot);
+
+ const SBModelVars& mv = sbs.getModelVars();
+
+ // Angular acceleration is the same in either mode.
+ const Vec3& b_FM = Vec3::getAs(udot); // a.k.a. w_FM_dot
+
+ if (this->getUseEulerAngles(mv)) {
+ // Euler angle mode (22 flops).
+ Vec4::updAs(qdotdot) = 0; // TODO: kludge, clear unused element
+
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Real* pool = this->getQPool(mc, pc);
+
+ // Aliases.
+ const Vec2& cosxy = Vec2::getAs(&pool[AngleCosQ]); // only need x,y
+ const Vec2& sinxy = Vec2::getAs(&pool[AngleSinQ]);
+ const Real& oocosy = pool[AngleOOCosQy];
+ const Vec3& qdot = this->fromQ(sbs.getQDot());
+ Vec3& qdotdot_v = Vec3::updAs(qdotdot);
+
+ // 22 flops.
+ qdotdot_v = Rotation::convertAngAccInParentToBodyXYZDotDot
+ (cosxy, sinxy, oocosy, qdot, b_FM);
+ } else {
+ // Quaternion mode (41 flops).
+ const Vec4& quat = this->fromQuat(sbs.getQ()); // unnormalized
+ const Vec3& w_FM = this->fromU(sbs.getU());
+ Vec4::updAs(qdotdot) =
+ Rotation::convertAngVelDotToQuaternionDotDot(quat,w_FM,b_FM);
+ }
+}
+
+int getMaxNQ() const {return 4;}
+int getNQInUse(const SBModelVars& mv) const {
+ return this->getUseEulerAngles(mv) ? 3 : 4;
+}
+bool isUsingQuaternion(const SBStateDigest& sbs,
+ MobilizerQIndex& startOfQuaternion) const {
+ if (this->getUseEulerAngles(sbs.getModelVars())) {
+ startOfQuaternion.invalidate();
+ return false;
+ }
+ startOfQuaternion = MobilizerQIndex(0); // quaternion comes first
+ return true;
+}
+
+void setMobilizerDefaultPositionValues(
+ const SBModelVars& mv,
+ Vector& q) const
+{
+ if (this->getUseEulerAngles(mv)) {
+ //TODO: kludge
+ this->toQuat(q) = Vec4(0); // clear unused element
+ this->toQ(q) = 0;
+ }
+ else this->toQuat(q) = Vec4(1,0,0,0);
+}
+
+bool enforceQuaternionConstraints(
+ const SBStateDigest& sbs,
+ Vector& q,
+ Vector& qErrest) const
+{
+ if (this->getUseEulerAngles(sbs.getModelVars()))
+ return false; // no change
+
+ Vec4& quat = this->toQuat(q);
+ quat = quat / quat.norm();
+
+ if (qErrest.size()) {
+ Vec4& qerr = this->toQuat(qErrest);
+ qerr -= dot(qerr,quat) * quat;
+ }
+
+ return true;
+}
+
+void convertToEulerAngles(const Vector& inputQ, Vector& outputQ) const {
+ this->toQuat(outputQ) = Vec4(0); // clear unused element
+ this->toQ(outputQ) = Rotation(Quaternion(this->fromQuat(inputQ))).convertRotationToBodyFixedXYZ();
+}
+
+void convertToQuaternions(const Vector& inputQ, Vector& outputQ) const {
+ Rotation rot;
+ rot.setRotationToBodyFixedXYZ(this->fromQ(inputQ));
+ this->toQuat(outputQ) = rot.convertRotationToQuaternion().asVec4();
+}
+
+};
+
+
+#endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_ELLIPSOID_H_
+
diff --git a/Simbody/src/RigidBodyNodeSpec_Free.h b/Simbody/src/RigidBodyNodeSpec_Free.h
new file mode 100644
index 0000000..f4f54a4
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec_Free.h
@@ -0,0 +1,510 @@
+#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_FREE_H_
+#define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_FREE_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Derived from IVM code written by Charles Schwieters *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Define the RigidBodyNode that implements a Free mobilizer.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "RigidBodyNodeSpec.h"
+
+
+ // FREE //
+
+// Free joint. This provides six degrees of freedom, three rotational and
+// three translational. The rotation is like the ball joint; the
+// translation is like the Cartesian joint.
+// The generalized coordinates are:
+// * 4 quaternions or 3 X-Y-Z (1-2-3) body fixed Euler angles (that is,
+// about M's axes fixed on the child)
+// * translation from Fo to Mo as a 3-vector expressed in frame F fixed to
+// the parent
+// and generalized speeds are:
+// * angular velocity w_FM as a vector expressed in the F frame
+// * linear velocity of the M origin Mo in F (v_FM), expressed in F
+// Thus translational qdots are just generalized speeds, but rotational
+// qdots have to be derived from the generalized speeds to be turned into
+// either 4 quaternion derivatives or 3 Euler angle derivatives.
+//
+// NOTE: An XYZ Euler angle sequence has a singularity when the middle angle
+// is at 90 or 270 degrees; quaternions are never singular.
+template<bool noX_MB, bool noR_PF>
+class RBNodeFree : public RigidBodyNodeSpec<6, false, noX_MB, noR_PF> {
+public:
+
+typedef typename RigidBodyNodeSpec<6, false, noX_MB, noR_PF>::HType HType;
+virtual const char* type() { return "free"; }
+
+RBNodeFree(const MassProperties& mProps_B,
+ const Transform& X_PF,
+ const Transform& X_BM,
+ bool isReversed,
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot)
+ : RigidBodyNodeSpec<6, false, noX_MB, noR_PF>(mProps_B,X_PF,X_BM,nextUSlot,nextUSqSlot,nextQSlot,
+ RigidBodyNode::QDotMayDifferFromU, RigidBodyNode::QuaternionMayBeUsed, isReversed)
+{
+ this->updateSlots(nextUSlot,nextUSqSlot,nextQSlot);
+}
+
+ // Implementations of virtual methods.
+
+// This has a default implementation but it rotates first then translates,
+// which works fine for the normal Free joint but produces wrong behavior when
+// the mobilizer is reversed.
+void setQToFitTransformImpl(const SBStateDigest& sbs, const Transform& X_FM,
+ Vector& q) const OVERRIDE_11
+{
+ setQToFitTranslationImpl(sbs, X_FM.p(), q); // see below
+ setQToFitRotationImpl(sbs, X_FM.R(), q);
+}
+
+void setQToFitRotationImpl(const SBStateDigest& sbs, const Rotation& R_FM,
+ Vector& q) const
+{
+ if (this->getUseEulerAngles(sbs.getModelVars()))
+ this->toQVec3(q,0) = R_FM.convertRotationToBodyFixedXYZ();
+ else
+ this->toQuat(q) = R_FM.convertRotationToQuaternion().asVec4();
+}
+
+// The user gives us the translation vector from OF to OM as a vector expressed
+// in F, which is what we use as translational generalized coordinates. Also,
+// with a free joint we never have to change orientation coordinates in order
+// to achieve a translation.
+void setQToFitTranslationImpl(const SBStateDigest& sbs, const Vec3& p_FM, Vector& q) const {
+ if (this->getUseEulerAngles(sbs.getModelVars()))
+ this->toQVec3(q,3) = p_FM; // skip the 3 Euler angles
+ else
+ this->toQVec3(q,4) = p_FM; // skip the 4 quaternions
+}
+
+// Our 3 rotational generalized speeds are just the angular velocity vector of
+// M in F, expressed in F, which is exactly what the user provides here.
+void setUToFitAngularVelocityImpl(const SBStateDigest& sbs,
+ const Vector& q, const Vec3& w_FM,
+ Vector& u) const
+{
+ this->toUVec3(u,0) = w_FM; // relative ang. vel. always used as generalized speeds
+}
+
+// Our 3 translational generalized speeds are the linear velocity of M's origin
+// in F, expressed in F, which is just what the user gives us.
+void setUToFitLinearVelocityImpl
+ (const SBStateDigest& sbs, const Vector& q, const Vec3& v_FM, Vector& u) const
+{
+ this->toUVec3(u,3) = v_FM;
+}
+
+// When we're using Euler angles, we're going to want to cache cos and sin for
+// each angle, and also 1/cos of the middle angle will be handy to have around.
+// When we're using quaternions, we'll cache 1/|q| for convenient normalizing
+// of quaternions.
+enum {AnglePoolSize=7, QuatPoolSize=1};
+// cos x,y,z sin x,y,z 1/cos(y)
+enum {AngleCosQ=0, AngleSinQ=3, AngleOOCosQy=6};
+enum {QuatOONorm=0};
+int calcQPoolSize(const SBModelVars& mv) const
+{ return this->getUseEulerAngles(mv) ? AnglePoolSize : QuatPoolSize; }
+
+// This is expensive in Euler angle mode due to the three sin/cos computations
+// required (about 150 flops). In quaternion mode it is much cheaper (about
+// 25 flops).
+void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* qCache, int nQCache,
+ Real* qErr, int nQErr) const
+{
+ if (this->getUseEulerAngles(sbs.getModelVars())) {
+ assert(q && nq==6 && qCache && nQCache==AnglePoolSize && nQErr==0);
+
+ const Real cy = std::cos(q[1]);
+ Vec3::updAs(&qCache[AngleCosQ]) =
+ Vec3(std::cos(q[0]), cy, std::cos(q[2]));
+ Vec3::updAs(&qCache[AngleSinQ]) =
+ Vec3(std::sin(q[0]), std::sin(q[1]), std::sin(q[2]));
+ qCache[AngleOOCosQy] = 1/cy; // trouble at 90 or 270 degrees
+ } else {
+ assert(q && nq==7 && qCache && nQCache==QuatPoolSize &&
+ qErr && nQErr==1);
+
+ const Real quatLen = Vec4::getAs(q).norm();
+ qErr[0] = quatLen - Real(1); // normalization error
+ qCache[QuatOONorm] = 1/quatLen; // save for later
+ }
+}
+
+// Because of the precalculations above we can calculate the cross-mobilizer
+// transform in 18 flops (Euler angles) or 33 flops (quaternions).
+void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* qCache, int nQCache,
+ Transform& X_F0M0) const
+{
+ if (this->getUseEulerAngles(sbs.getModelVars())) {
+ assert(q && nq==6 && qCache && nQCache==AnglePoolSize);
+
+ X_F0M0.updR().setRotationToBodyFixedXYZ // 18 flops
+ (Vec3::getAs(&qCache[AngleCosQ]), Vec3::getAs(&qCache[AngleSinQ]));
+ X_F0M0.updP() = Vec3::getAs(&q[3]); // a012 x y z
+ } else {
+ assert(q && nq==7 && qCache && nQCache==QuatPoolSize);
+
+ // Must use a normalized quaternion to generate the rotation matrix.
+ // Here we normalize with just 4 flops using precalculated 1/norm(q).
+ const Quaternion quat(Vec4::getAs(q)*qCache[QuatOONorm], true);
+ X_F0M0.updR().setRotationFromQuaternion(quat); // 29 flops
+ X_F0M0.updP() = Vec3::getAs(&q[4]); // q0123 x y z
+ }
+}
+
+
+// The generalized speeds for this 6-dof ("free") joint are
+// (1) the angular velocity of M in the F frame, expressed in F, and
+// (2) the (linear) velocity of M's origin in F, expressed in F.
+void calcAcrossJointVelocityJacobian(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+{
+ H_FM(0) = SpatialVec( Vec3(1,0,0), Vec3(0) ); // rotations
+ H_FM(1) = SpatialVec( Vec3(0,1,0), Vec3(0) );
+ H_FM(2) = SpatialVec( Vec3(0,0,1), Vec3(0) );
+
+ H_FM(3) = SpatialVec( Vec3(0), Vec3(1,0,0) ); // translations
+ H_FM(4) = SpatialVec( Vec3(0), Vec3(0,1,0) );
+ H_FM(5) = SpatialVec( Vec3(0), Vec3(0,0,1) );
+}
+
+// Since the Jacobian above is constant in F, its derivative in F is 0.
+void calcAcrossJointVelocityJacobianDot(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+{
+ HDot_FM(0) = SpatialVec( Vec3(0), Vec3(0) );
+ HDot_FM(1) = SpatialVec( Vec3(0), Vec3(0) );
+ HDot_FM(2) = SpatialVec( Vec3(0), Vec3(0) );
+
+ HDot_FM(3) = SpatialVec( Vec3(0), Vec3(0) );
+ HDot_FM(4) = SpatialVec( Vec3(0), Vec3(0) );
+ HDot_FM(5) = SpatialVec( Vec3(0), Vec3(0) );
+}
+
+
+// Calculate qdot=N(q)*u. Precalculations make this fast in Euler angle
+// mode (10 flops); quaternion mode is 27 flops.
+// TODO: we're expecting that there are 7 qdots even in Euler angle mode
+// so we have to zero out the last one in that case.
+void calcQDot(const SBStateDigest& sbs, const Real* u,
+ Real* qdot) const {
+ assert(sbs.getStage() >= Stage::Position);
+ assert(u && qdot);
+
+ const SBModelVars& mv = sbs.getModelVars();
+
+ // Generalized speeds are the same in either mode -- velocity
+ // of F in M (or B in P), expressed in F (fixed on parent) frame.
+ const Vec3& w_FM = Vec3::getAs(&u[0]); // Angular velocity in F
+ const Vec3& v_FM = Vec3::getAs(&u[3]); // Linear velocity in F
+
+ if (this->getUseEulerAngles(mv)) {
+ // Euler angle mode (10 flops)
+ qdot[6] = 0; // TODO: kludge, clear unused element
+
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Real* pool = this->getQPool(mc, pc);
+
+ Vec3::updAs(&qdot[0]) = Rotation::convertAngVelInParentToBodyXYZDot(
+ Vec2::getAs(&pool[AngleCosQ]), Vec2::getAs(&pool[AngleSinQ]), //x,y
+ pool[AngleOOCosQy], w_FM);
+ Vec3::updAs(&qdot[3]) = v_FM;
+ } else {
+ // Quaternion mode (27 flops)
+ const Vec4& quat = this->fromQuat(sbs.getQ()); // unnormalized
+ Vec4::updAs(&qdot[0]) =
+ Rotation::convertAngVelToQuaternionDot(quat, w_FM);
+ Vec3::updAs(&qdot[4]) = v_FM;
+ }
+}
+
+// CAUTION: we do not zero the unused 4th element of q for Euler angles; it
+// is up to the caller to do that if it is necessary.
+// Compute out_q = N * in_u
+// or out_u = in_q * N
+// The u quantity is a vector v_F in the parent frame (e.g. angular
+// velocity of child in parent), while the q quantity is the derivative
+// of a quaternion or Euler sequence representing R_FM, i.e. orientation
+// of child in parent.
+void multiplyByN(const SBStateDigest& sbs, bool matrixOnRight,
+ const Real* in, Real* out) const
+{
+ assert(sbs.getStage() >= Stage::Position);
+ assert(in && out);
+
+ const SBModelVars& mv = sbs.getModelVars();
+
+ if (this->getUseEulerAngles(mv)) {
+ // Euler angle mode (10 flops).
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Real* pool = this->getQPool(mc, pc);
+
+ // Aliases.
+ const Vec2& cosxy = Vec2::getAs(&pool[AngleCosQ]); // only need x,y
+ const Vec2& sinxy = Vec2::getAs(&pool[AngleSinQ]);
+ const Real& oocosy = pool[AngleOOCosQy];
+ const Vec3& in_v = Vec3::getAs(in);
+ Vec3& out_v = Vec3::updAs(out);
+
+ if (matrixOnRight) { // out_u = in_q * N = ~N * in_q (9 flops)
+ // Transpose on left is same as untransposed on right.
+ out_v = Rotation::multiplyByBodyXYZ_NT_P(cosxy, sinxy, oocosy, in_v);
+ } else { // out_q = N*in_u (10 flops)
+ out_v = Rotation::multiplyByBodyXYZ_N_P(cosxy, sinxy, oocosy, in_v);
+ }
+ // Translational part of N block is identity.
+ Vec3::updAs(out+3) = Vec3::getAs(in+3);
+ } else {
+ // Quaternion mode (27 flops).
+ const Vec4& quat = this->fromQuat(sbs.getQ()); // unnormalized
+ const Mat43 N_F = // This method returns N in the parent frame
+ Rotation::calcUnnormalizedNForQuaternion(quat);
+ // Translational part of N is identity so just copy in to out.
+ if (matrixOnRight) {
+ Row3::updAs(out) = Row4::getAs(in) * N_F;
+ Row3::updAs(out+3) = Row3::getAs(in+4); // translation
+ } else {
+ Vec4::updAs(out) = N_F * Vec3::getAs(in);
+ Vec3::updAs(out+4) = Vec3::getAs(in+3); // translation
+ }
+ }
+}
+
+// Compute out_u = inv(N) * in_q
+// or out_q = in_u * inv(N)
+// Cost: Euler angle mode ~ 10 flops, Quaternion mode ~ 27 flops.
+void multiplyByNInv(const SBStateDigest& sbs, bool matrixOnRight,
+ const Real* in, Real* out) const
+{
+ assert(sbs.getStage() >= Stage::Position);
+ assert(in && out);
+
+ const SBModelVars& mv = sbs.getModelVars();
+
+ if (this->getUseEulerAngles(mv)) {
+ // Euler angle mode (10 flops).
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Real* pool = this->getQPool(mc, pc);
+
+ // Aliases.
+ const Vec2& cosxy = Vec2::getAs(&pool[AngleCosQ]); // only need x,y
+ const Vec2& sinxy = Vec2::getAs(&pool[AngleSinQ]);
+ const Vec3& in_v = Vec3::getAs(in);
+ Vec3& out_v = Vec3::updAs(out);
+
+ if (matrixOnRight) { // out_q = in_u * inv(N) = ~inv(N) * in_u (10 flops)
+ // transpose on left is same as untransposed on right
+ out_v = Rotation::multiplyByBodyXYZ_NInvT_P(cosxy, sinxy, in_v);
+ } else { // out_u = inv(N) * in_q (9 flops)
+ out_v = Rotation::multiplyByBodyXYZ_NInv_P(cosxy, sinxy, in_v);
+ }
+ // Translational part of NInv block is identity.
+ Vec3::updAs(out+3) = Vec3::getAs(in+3);
+ } else {
+ // Quaternion mode (27 flops).
+ const Vec4& quat = this->fromQuat(sbs.getQ()); // unnormalized
+ const Mat34 NInv_F = // This method returns NInv in parent frame
+ Rotation::calcUnnormalizedNInvForQuaternion(quat); // 7 flops
+ // Translational part of NInv is identity so just copy in to out.
+ if (matrixOnRight) {
+ Row4::updAs(out) = Row3::getAs(in) * NInv_F; // 20 flops
+ Row3::updAs(out+4) = Row3::getAs(in+3); // translation
+ } else {
+ Vec3::updAs(out) = NInv_F * Vec4::getAs(in); // 21 flops
+ Vec3::updAs(out+3) = Vec3::getAs(in+4); // translation
+ }
+ }
+}
+
+// Compute out_q = NDot * in_u
+// or out_u = in_q * NDot
+// Note that it is much cheaper to calculate
+// qdotdot directly than to do it using N and NDot; see below.
+// Cost: Euler angle mode - 36 flops, Quaternion mode - 27 flops.
+void multiplyByNDot(const SBStateDigest& sbs, bool matrixOnRight,
+ const Real* in, Real* out) const
+{
+ assert(sbs.getStage() > Stage::Velocity);
+ assert(in && out);
+
+ const SBModelVars& mv = sbs.getModelVars();
+
+ if (this->getUseEulerAngles(mv)) {
+ // Euler angle mode (36 flops).
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Real* pool = this->getQPool(mc, pc);
+
+ // Aliases.
+ const Vec2& cosxy = Vec2::getAs(&pool[AngleCosQ]); // only need x,y
+ const Vec2& sinxy = Vec2::getAs(&pool[AngleSinQ]);
+ const Real& oocosy = pool[AngleOOCosQy];
+ const Vec3& qdot = this->fromQVec3(sbs.getQDot(),0);
+
+ // We don't have a nice multiply-by routine here so just get the NDot
+ // matrix and use it (21 flops).
+ const Mat33 NDot_F =
+ Rotation::calcNDotForBodyXYZInParentFrame(cosxy, sinxy, oocosy, qdot);
+
+ if (matrixOnRight) { // out_u = in_q * NDot (15 flops)
+ Row3::updAs(out) = Row3::getAs(in) * NDot_F;
+ } else { // out_q = NDot * in_u (15 flops)
+ Vec3::updAs(out) = NDot_F * Vec3::getAs(in);
+ }
+ // Translational part of NDot block is zero.
+ Vec3::updAs(out+3) = 0;
+ } else {
+ // Quaternion mode (27 flops).
+ const Vec4& qdot = this->fromQuat(sbs.getQDot()); // unnormalized
+ const Mat43 NDot_F = // This method returns NDot for the parent frame
+ Rotation::calcUnnormalizedNDotForQuaternion(qdot); // 7 flops
+ // Translational part of NDot is zero so set out to zero.
+ if (matrixOnRight) {
+ Row3::updAs(out) = Row4::getAs(in) * NDot_F; // 21 flops
+ Row3::updAs(out+3) = 0; // translation
+ } else {
+ Vec4::updAs(out) = NDot_F * Vec3::getAs(in); // 20 flops
+ Vec3::updAs(out+4) = 0; // translation
+ }
+ }
+}
+
+// Calculate qdotdot from udot as qdotdot = N(q)*udot + NDot(q,qdot)*u, but
+// faster. Here we assume that qdot=N*u has already been calculated, which
+// is very helpful in Euler angle mode. In either mode it is a very bad
+// idea to calculate this using explicit N and NDot matrices; we can save
+// a lot of time by using more specialized methods.
+// Cost: Euler angle mode - 22 flops, Quaternion mode - 41 flops.
+void calcQDotDot(const SBStateDigest& sbs, const Real* udot,
+ Real* qdotdot) const {
+ assert(sbs.getStage() > Stage::Velocity);
+ assert(udot && qdotdot);
+
+ const SBModelVars& mv = sbs.getModelVars();
+
+ // Angular acceleration is the same in either mode.
+ const Vec3& b_FM = Vec3::getAs(udot); // a.k.a. w_FM_dot
+ const Vec3& a_FM = Vec3::getAs(udot+3); // a.k.a. v_FM_dot
+
+ if (this->getUseEulerAngles(mv)) {
+ // Euler angle mode (22 flops).
+ qdotdot[6] = 0; // TODO: kludge, clear unused element
+
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Real* pool = this->getQPool(mc, pc);
+
+ // Aliases.
+ const Vec2& cosxy = Vec2::getAs(&pool[AngleCosQ]); // only need x,y
+ const Vec2& sinxy = Vec2::getAs(&pool[AngleSinQ]);
+ const Real& oocosy = pool[AngleOOCosQy];
+ const Vec3& qdot = this->fromQVec3(sbs.getQDot(),0);
+
+ // 22 flops.
+ Vec3::updAs(qdotdot+0) = Rotation::convertAngAccInParentToBodyXYZDotDot
+ (cosxy, sinxy, oocosy, qdot, b_FM);
+ Vec3::updAs(qdotdot+3) = a_FM;
+ } else {
+ // Quaternion mode (41 flops).
+ const Vec4& quat = this->fromQuat(sbs.getQ()); // unnormalized
+ const Vec3& w_FM = this->fromUVec3(sbs.getU(),0);
+ Vec4::updAs(qdotdot) =
+ Rotation::convertAngVelDotToQuaternionDotDot(quat,w_FM,b_FM);
+ Vec3::updAs(qdotdot+4) = a_FM;
+ }
+}
+
+int getMaxNQ() const {return 7;}
+int getNQInUse(const SBModelVars& mv) const {return this->getUseEulerAngles(mv) ? 6 : 7;}
+bool isUsingQuaternion(const SBStateDigest& sbs, MobilizerQIndex& startOfQuaternion) const {
+ if (this->getUseEulerAngles(sbs.getModelVars())) {startOfQuaternion.invalidate(); return false;}
+ startOfQuaternion = MobilizerQIndex(0); // quaternion comes first
+ return true;
+}
+
+void setMobilizerDefaultPositionValues(const SBModelVars& mv, Vector& q) const
+{
+ if (this->getUseEulerAngles(mv)) {
+ this->toQVec3(q,4) = Vec3(0); // TODO: kludge, clear unused element
+ this->toQ(q) = 0;
+ } else {
+ this->toQuat(q) = Vec4(1.,0.,0.,0.);
+ this->toQVec3(q,4) = 0;
+ }
+}
+
+bool enforceQuaternionConstraints(
+ const SBStateDigest& sbs,
+ Vector& q,
+ Vector& qErrest) const
+{
+ if (this->getUseEulerAngles(sbs.getModelVars()))
+ return false; // no change
+
+ Vec4& quat = this->toQuat(q);
+ quat = quat / quat.norm();
+
+ if (qErrest.size()) {
+ Vec4& qerr = this->toQuat(qErrest);
+ qerr -= dot(qerr,quat) * quat;
+ }
+
+ return true;
+}
+
+void convertToEulerAngles(const Vector& inputQ, Vector& outputQ) const {
+ this->toQVec3(outputQ, 4) = Vec3(0); // clear unused element
+ this->toQVec3(outputQ, 3) = this->fromQVec3(inputQ, 4);
+ this->toQVec3(outputQ, 0) = Rotation(Quaternion(this->fromQuat(inputQ))).convertRotationToBodyFixedXYZ();
+}
+
+void convertToQuaternions(const Vector& inputQ, Vector& outputQ) const {
+ this->toQVec3(outputQ, 4) = this->fromQVec3(inputQ, 3);
+ Rotation rot;
+ rot.setRotationToBodyFixedXYZ(this->fromQVec3(inputQ, 0));
+ this->toQuat(outputQ) = rot.convertRotationToQuaternion().asVec4();
+}
+
+
+};
+
+
+
+#endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_FREE_H_
+
diff --git a/Simbody/src/RigidBodyNodeSpec_FreeLine.h b/Simbody/src/RigidBodyNodeSpec_FreeLine.h
new file mode 100644
index 0000000..a3539a2
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec_FreeLine.h
@@ -0,0 +1,510 @@
+#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_FREELINE_H_
+#define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_FREELINE_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Define the RigidBodyNode that implements a Translation mobilizer, also
+ * known as a Cartesian joint.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "RigidBodyNodeSpec.h"
+
+
+ // FREE LINE //
+
+// FreeLine joint. Like a Free joint, this provides full rotational and
+// translational freedom, but for a degenerate body which is thin (inertialess)
+// along its own z axis. These arise in molecular modeling for linear molecules
+// formed by pairs of atoms, or by multiple atoms in a linear arrangement like
+// carbon dioxide (CO2) whose structure is O=C=O in a straight line. We are
+// assuming that there is no meaning to a rotation about the linear axis,
+// so free orientation requires just *two* degrees of freedom, not *three*
+// as is required for general rigid bodies. And in fact we can get away with
+// just two rotational generalized speeds so this joint provides only 5
+// mobilities.
+//
+// But so far, no one has been able to come up with a way to manage with only
+// two rotational generalized *coordinates*, so this joint has the same q's as
+// a regular Free joint: either a quaternion for unconditional stability, or a
+// three-angle (body fixed 1-2-3) Euler sequence which will be dynamically
+// singular when the middle (y) axis is 90 degrees. Use the Euler sequence only
+// for small motions or for kinematics problems (and note that only the first
+// two are meaningful). Translations here are treated exactly as for a Free
+// joint (or for a Cartesian joint for that matter).
+//
+// To summarize, the generalized coordinates are:
+// * 4 quaternions or 3 1-2-3 body fixed Euler angles (that is, fixed in M)
+// * 3 components of the translation vector p_FM (that is, vector from origin
+// of F to origin of M, expressed in F)
+// and generalized speeds are:
+// * the x,y components of the angular velocity w_FM_M, that is, the angular
+// velocity of M in F expressed in *M* (where we want wz=0).
+// * 3 components of the linear velocity of origin of M in F, expressed in F.
+// NOTE: THAT IS NOT THE SAME FRAME AS FOR A FREE JOINT
+// Thus the qdots have to be derived from the generalized speeds to
+// be turned into either 4 quaternion derivatives or 3 Euler angle derivatives.
+template<bool noX_MB, bool noR_PF>
+class RBNodeFreeLine : public RigidBodyNodeSpec<5, false, noX_MB, noR_PF> {
+public:
+
+typedef typename RigidBodyNodeSpec<5, false, noX_MB, noR_PF>::HType HType;
+virtual const char* type() { return "full"; }
+
+RBNodeFreeLine(const MassProperties& mProps_B,
+ const Transform& X_PF,
+ const Transform& X_BM,
+ bool isReversed,
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot)
+ : RigidBodyNodeSpec<5, false, noX_MB, noR_PF>(mProps_B,X_PF,X_BM,nextUSlot,nextUSqSlot,nextQSlot,
+ RigidBodyNode::QDotMayDifferFromU, RigidBodyNode::QuaternionMayBeUsed, isReversed)
+{
+ this->updateSlots(nextUSlot,nextUSqSlot,nextQSlot);
+}
+
+ // Implementations of virtual methods.
+
+// This has a default implementation but it rotates first then translates,
+// which works fine for the normal FreeLine joint but produces wrong behavior
+// when the mobilizer is reversed.
+void setQToFitTransformImpl(const SBStateDigest& sbs, const Transform& X_FM,
+ Vector& q) const OVERRIDE_11
+{
+ setQToFitTranslationImpl(sbs, X_FM.p(), q); // see below
+ setQToFitRotationImpl(sbs, X_FM.R(), q);
+}
+
+void setQToFitRotationImpl(const SBStateDigest& sbs, const Rotation& R_FM,
+ Vector& q) const
+{
+ if (this->getUseEulerAngles(sbs.getModelVars()))
+ this->toQVec3(q,0) = R_FM.convertRotationToBodyFixedXYZ();
+ else
+ this->toQuat(q) = R_FM.convertRotationToQuaternion().asVec4();
+}
+
+// The user gives us the translation vector from OF to OM as a vector expressed
+// in F. With a free joint we never have to *change* orientation coordinates in
+// order to achieve a translation.
+void setQToFitTranslationImpl(const SBStateDigest& sbs, const Vec3& p_FM, Vector& q) const {
+ if (this->getUseEulerAngles(sbs.getModelVars()))
+ this->toQVec3(q,3) = p_FM; // skip the 3 Euler angles
+ else
+ this->toQVec3(q,4) = p_FM; // skip the 4 quaternions
+}
+
+// Our 2 rotational generalized speeds are just the (x,y) components of the
+// angular velocity vector of M in F, expressed in *M*.
+// Note: a quaternion from a state is not necessarily normalized so can't be
+// used directly as though it were a set of Euler parameters; it must be
+// normalized first.
+void setUToFitAngularVelocityImpl(const SBStateDigest& sbs,
+ const Vector& q, const Vec3& w_FM,
+ Vector& u) const
+{
+ Rotation R_FM;
+ if (this->getUseEulerAngles(sbs.getModelVars()))
+ R_FM.setRotationToBodyFixedXYZ( this->fromQVec3(q,0) );
+ else {
+ // can't use qnorm pool here since state hasn't been
+ // realized to position stage yet; q's can be anything
+ R_FM.setRotationFromQuaternion( Quaternion(this->fromQuat(q)) ); // normalize
+ }
+ const Vec3 w_FM_M = ~R_FM*w_FM;
+ // (x,y) of relative angular velocity always used as generalized speeds
+ Vec2::updAs(&this->toU(u)[0]) = Vec2(w_FM_M[0], w_FM_M[1]);
+}
+
+// Our 3 translational generalized speeds are the linear velocity of M's origin
+// in F, expressed in F. The user gives us that same vector.
+void setUToFitLinearVelocityImpl
+ (const SBStateDigest& sbs, const Vector& q, const Vec3& v_FM, Vector& u) const
+{
+ this->toUVec3(u,2) = v_FM;
+}
+
+// When we're using Euler angles, we're going to want to cache cos and sin for
+// each angle, and also 1/cos of the middle angle will be handy to have around.
+// When we're using quaternions, we'll cache 1/|q| for convenient normalizing
+// of quaternions.
+enum {AnglePoolSize=7, QuatPoolSize=1};
+// cos x,y,z sin x,y,z 1/cos(y)
+enum {AngleCosQ=0, AngleSinQ=3, AngleOOCosQy=6};
+enum {QuatOONorm=0};
+int calcQPoolSize(const SBModelVars& mv) const
+{ return this->getUseEulerAngles(mv) ? AnglePoolSize : QuatPoolSize; }
+
+void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* qCache, int nQCache,
+ Real* qErr, int nQErr) const
+{
+ if (this->getUseEulerAngles(sbs.getModelVars())) {
+ assert(q && nq==6 && qCache && nQCache==AnglePoolSize && nQErr==0);
+ const Real cy = std::cos(q[1]);
+ Vec3::updAs(&qCache[AngleCosQ]) =
+ Vec3(std::cos(q[0]), cy, std::cos(q[2]));
+ Vec3::updAs(&qCache[AngleSinQ]) =
+ Vec3(std::sin(q[0]), std::sin(q[1]), std::sin(q[2]));
+ qCache[AngleOOCosQy] = 1/cy; // trouble at 90 degrees
+ } else {
+ assert(q && nq==7 && qCache && nQCache==QuatPoolSize &&
+ qErr && nQErr==1);
+ const Real quatLen = Vec4::getAs(q).norm();
+ qErr[0] = quatLen - Real(1); // normalization error
+ qCache[QuatOONorm] = 1/quatLen; // save for later
+ }
+}
+
+void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* qCache, int nQCache,
+ Transform& X_F0M0) const
+{
+ if (this->getUseEulerAngles(sbs.getModelVars())) {
+ assert(q && nq==6 && qCache && nQCache==AnglePoolSize);
+ X_F0M0.updR().setRotationToBodyFixedXYZ // 18 flops
+ (Vec3::getAs(&qCache[AngleCosQ]), Vec3::getAs(&qCache[AngleSinQ]));
+ X_F0M0.updP() = Vec3::getAs(&q[3]); // a012 x y z
+ } else {
+ assert(q && nq==7 && qCache && nQCache==QuatPoolSize);
+ // Must use a normalized quaternion to generate the rotation matrix.
+ // Here we normalize with just 4 flops using precalculated 1/norm(q).
+ const Quaternion quat(Vec4::getAs(q)*qCache[QuatOONorm], true);
+ X_F0M0.updR().setRotationFromQuaternion(quat); // 29 flops
+ X_F0M0.updP() = Vec3::getAs(&q[4]); // q0123 x y z
+ }
+}
+
+
+// The generalized speeds for this 5-dof ("free line") joint are
+// (1) the (x,y) components of angular velocity of M in the F frame,
+// expressed in *M*, and
+// (2) the (linear) velocity of M's origin in F, expressed in F.
+void calcAcrossJointVelocityJacobian(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+{
+ const SBTreePositionCache& pc = sbs.updTreePositionCache(); // "upd" because we're realizing positions now
+ const Transform X_F0M0 = this->findX_F0M0(pc);
+
+ // Dropping the 0's here.
+ const Rotation& R_FM = X_F0M0.R();
+ const Vec3& Mx_F = R_FM.x(); // M's x axis, expressed in F
+ const Vec3& My_F = R_FM.y(); // M's y axis, expressed in F
+
+ H_FM(0) = SpatialVec( Mx_F, Vec3(0) ); // x,y ang. vel. in M, exp. in F
+ H_FM(1) = SpatialVec( My_F, Vec3(0) );
+
+ H_FM(2) = SpatialVec( Vec3(0), Vec3(1,0,0) ); // translations in F
+ H_FM(3) = SpatialVec( Vec3(0), Vec3(0,1,0) );
+ H_FM(4) = SpatialVec( Vec3(0), Vec3(0,0,1) );
+}
+
+// Since the first two rows of the Jacobian above are not constant in F,
+// its time derivative is non zero. Here we use the fact that for
+// a vector r_B_A fixed in a moving frame B but expressed in another frame A,
+// its time derivative in A is the angular velocity of B in A crossed with
+// the vector, i.e., d_A/dt r_B_A = w_AB % r_B_A.
+void calcAcrossJointVelocityJacobianDot(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+{
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const SBTreeVelocityCache& vc = sbs.updTreeVelocityCache(); // "upd" because we're realizing velocities now
+ const Transform X_F0M0 = this->findX_F0M0(pc);
+
+ // Dropping the 0's here.
+ const Rotation& R_FM = X_F0M0.R();
+ const Vec3& Mx_F = R_FM.x(); // M's x axis, expressed in F
+ const Vec3& My_F = R_FM.y(); // M's y axis, expressed in F
+
+ const Vec3 w_FM = this->find_w_F0M0(pc,vc); // angular velocity of M in F
+
+ HDot_FM(0) = SpatialVec( w_FM % Mx_F, Vec3(0) );
+ HDot_FM(1) = SpatialVec( w_FM % My_F, Vec3(0) );
+
+ // For translation in F.
+ HDot_FM(2) = SpatialVec( Vec3(0), Vec3(0) );
+ HDot_FM(3) = SpatialVec( Vec3(0), Vec3(0) );
+ HDot_FM(4) = SpatialVec( Vec3(0), Vec3(0) );
+}
+
+// CAUTION: we do not zero the unused 4th element of q for Euler angles; it
+// is up to the caller to do that if it is necessary.
+// Compute out_q = N * in_u
+// or out_u = in_q * N
+// The u quantity is a vector v_M in the child body frame (e.g. angular
+// velocity of child in parent, but expressed in child), while the q
+// quantity is the derivative of a quaternion or Euler sequence representing
+// R_FM, i.e. orientation of child in parent.
+void multiplyByN(const SBStateDigest& sbs, bool matrixOnRight,
+ const Real* in, Real* out) const
+{
+ assert(sbs.getStage() >= Stage::Position);
+ assert(in && out);
+
+ const SBModelVars& mv = sbs.getModelVars();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Vector& allQ = sbs.getQ();
+
+ if (this->getUseEulerAngles(mv)) {
+ const Vec3& q = this->fromQVec3(allQ,0);
+ const Mat32 N_FM = // N here mixes parent and body frames (convenient)
+ Rotation::calcNForBodyXYZInBodyFrame(q)
+ .getSubMat<3,2>(0,0); // drop 3rd column
+ // Translational part of N is identity so just copy in to out.
+ if (matrixOnRight) {
+ Row2::updAs(out) = Row3::getAs(in) * N_FM;
+ Row3::updAs(out+2) = Row3::getAs(in+3);// translation
+ } else {
+ Vec3::updAs(out) = N_FM * Vec2::getAs(in);
+ Vec3::updAs(out+3) = Vec3::getAs(in+2);// translation
+ }
+ } else {
+ // Quaternion: N block is only available expecting angular velocity
+ // in the parent frame F, but we have it in M for this joint so we
+ // have to calculate N_FM = N_FF*R_FM.
+ const Rotation& R_FM = this->getX_FM(pc).R();
+ const Vec4& q = this->fromQuat(allQ);
+ const Mat42 N_FM = (Rotation::calcUnnormalizedNForQuaternion(q)*R_FM)
+ .getSubMat<4,2>(0,0); // drop 3rd column
+ // Translational part of N is identity so just copy in to out.
+ if (matrixOnRight) {
+ Row2::updAs(out) = Row4::getAs(in) * N_FM;
+ Row3::updAs(out+2) = Row3::getAs(in+4); // translation
+ } else { // matrix on left
+ Vec4::updAs(out) = N_FM * Vec2::getAs(in);
+ Vec3::updAs(out+4) = Vec3::getAs(in+2); // translation
+ }
+ }
+}
+
+
+// Compute out_u = inv(N) * in_q
+// or out_q = in_u * inv(N)
+void multiplyByNInv(const SBStateDigest& sbs, bool matrixOnRight,
+ const Real* in, Real* out) const
+{
+ assert(sbs.getStage() >= Stage::Position);
+ assert(in && out);
+
+ const SBModelVars& mv = sbs.getModelVars();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Vector& allQ = sbs.getQ();
+
+ if (this->getUseEulerAngles(mv)) {
+ const Vec3& q = this->fromQVec3(allQ,0);
+ const Mat23 NInv_MF = Rotation::calcNInvForBodyXYZInBodyFrame(q)
+ .getSubMat<2,3>(0,0); // drop 3rd row
+ // Translational part of NInv block is identity.
+ if (matrixOnRight) {
+ Row3::updAs(out) = Row2::getAs(in) * NInv_MF;
+ Row3::updAs(out+3) = Row3::getAs(in+2); // translation
+ } else {
+ Vec2::updAs(out) = NInv_MF * Vec3::getAs(in);
+ Vec3::updAs(out+2) = Vec3::getAs(in+3); // translation
+ }
+ } else {
+ // Quaternion: NInv block is only available expecting angular
+ // velocity in the parent frame F, but we have it in M for
+ // this joint so we have to calculate NInv_MF = R_MF*NInv_FF.
+ const Rotation& R_FM = this->getX_FM(pc).R();
+ const Vec4& q = this->fromQuat(allQ);
+ const Mat24 NInv_MF = (~R_FM*Rotation::calcUnnormalizedNInvForQuaternion(q))
+ .getSubMat<2,4>(0,0); // drop 3rd row
+ // Translational part of NInv block is identity.
+ if (matrixOnRight) {
+ Row4::updAs(out) = Row2::getAs(in) * NInv_MF;
+ Row3::updAs(out+4) = Row3::getAs(in+2); // translation
+ } else { // matrix on left
+ Vec2::updAs(out) = NInv_MF * Vec4::getAs(in);
+ Vec3::updAs(out+2) = Vec3::getAs(in+4); // translation
+ }
+ }
+}
+
+// Compute out_q = NDot * in_u
+// or out_u = in_q * NDot
+void multiplyByNDot(const SBStateDigest& sbs, bool matrixOnRight,
+ const Real* in, Real* out) const
+{
+ assert(sbs.getStage() >= Stage::Velocity);
+ assert(in && out);
+
+ const SBModelVars& mv = sbs.getModelVars();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Vector& allQ = sbs.getQ();
+ const Vector& allQDot = sbs.getQDot();
+
+ if (this->getUseEulerAngles(mv)) {
+ const Vec3& q = this->fromQVec3(allQ,0);
+ const Vec3& qdot = this->fromQVec3(allQDot,0);
+ const Mat32 NDot_FM = // NDot here mixes parent and body frames (convenient)
+ Rotation::calcNDotForBodyXYZInBodyFrame(q, qdot)
+ .getSubMat<3,2>(0,0); // drop 3rd column
+ // Translational part of NDot is zero so set out to zero.
+ if (matrixOnRight) {
+ Row2::updAs(out) = Row3::getAs(in) * NDot_FM;
+ Row3::updAs(out+2) = 0; // translation
+ } else {
+ Vec3::updAs(out) = NDot_FM * Vec2::getAs(in);
+ Vec3::updAs(out+3) = 0; // translation
+ }
+ } else {
+ // Quaternion: NDot block is only available expecting angular velocity
+ // in the parent frame F, but we have it in M for this joint so we
+ // have to calculate NDot_FM = NDot_FF*R_FM.
+ const Rotation& R_FM = this->getX_FM(pc).R();
+ const Vec4& qdot = this->fromQuat(allQDot);
+ const Mat42 NDot_FM =
+ (Rotation::calcUnnormalizedNDotForQuaternion(qdot)*R_FM)
+ .getSubMat<4,2>(0,0); // drop 3rd column
+ // Translational part of N is identity so just copy in to out.
+ if (matrixOnRight) {
+ Row2::updAs(out) = Row4::getAs(in) * NDot_FM;
+ Row3::updAs(out+2) = 0; // translation
+ } else { // matrix on left
+ Vec4::updAs(out) = NDot_FM * Vec2::getAs(in);
+ Vec3::updAs(out+4) = 0; // translation
+ }
+ }
+}
+
+
+void calcQDot(
+ const SBStateDigest& sbs,
+ const Real* u,
+ Real* qdot) const
+{
+ const SBModelVars& mv = sbs.getModelVars();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Vec3 w_FM_M = Vec3(u[0], u[1], 0); // Angular velocity in M
+ const Vec3& v_FM = Vec3::getAs(&u[2]); // Linear velocity in F
+
+ if (this->getUseEulerAngles(mv)) {
+ const Vec3& theta = this->fromQVec3(sbs.getQ(),0); // Euler angles
+ Vec3::updAs(qdot) = Rotation::convertAngVelInBodyFrameToBodyXYZDot(theta,
+ w_FM_M); // need w in *body*, not parent
+ Vec3::updAs(&qdot[3]) = v_FM;
+ qdot[6] = 0;
+ } else {
+ const Rotation& R_FM = this->getX_FM(pc).R();
+ const Vec4& quat = this->fromQuat(sbs.getQ());
+ Vec4::updAs(qdot) = Rotation::convertAngVelToQuaternionDot(quat,
+ R_FM*w_FM_M); // need w in *parent* frame here
+ Vec3::updAs(&qdot[4]) = v_FM;
+ }
+}
+
+void calcQDotDot(
+ const SBStateDigest& sbs,
+ const Real* udot,
+ Real* qdotdot) const
+{
+ const SBModelVars& mv = sbs.getModelVars();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Vec3 w_FM_M = Vec3(this->fromU(sbs.getU())[0], this->fromU(sbs.getU())[1], 0); // Angular velocity of M in F, exp. in M
+ const Vec3& v_FM = this->fromUVec3(sbs.getU(),2); // linear velocity of M in F, expressed in M
+ const Vec3 w_FM_M_dot = Vec3(udot[0], udot[1], 0);
+ const Vec3& v_FM_dot = Vec3::getAs(&udot[2]);
+
+ if (this->getUseEulerAngles(mv)) {
+ const Vec3& theta = this->fromQVec3(sbs.getQ(),0); // Euler angles
+ Vec3::updAs(qdotdot) = Rotation::convertAngVelDotInBodyFrameToBodyXYZDotDot
+ (theta, w_FM_M, w_FM_M_dot); // needed in body frame here
+ Vec3::updAs(&qdotdot[3]) = v_FM_dot;
+ qdotdot[6] = 0;
+ } else {
+ const Rotation& R_FM = this->getX_FM(pc).R();
+ const Vec4& quat = this->fromQuat(sbs.getQ());
+ Vec4::updAs(qdotdot) = Rotation::convertAngVelDotToQuaternionDotDot
+ (quat,R_FM*w_FM_M,R_FM*w_FM_M_dot); // needed in parent frame
+ Vec3::updAs(&qdotdot[4]) = v_FM_dot;
+ }
+}
+
+int getMaxNQ() const {return 7;}
+int getNQInUse(const SBModelVars& mv) const {return this->getUseEulerAngles(mv) ? 6 : 7;}
+bool isUsingQuaternion(const SBStateDigest& sbs, MobilizerQIndex& startOfQuaternion) const {
+ if (this->getUseEulerAngles(sbs.getModelVars())) {startOfQuaternion.invalidate(); return false;}
+ startOfQuaternion = MobilizerQIndex(0); // quaternion comes first
+ return true;
+}
+
+void setMobilizerDefaultPositionValues(const SBModelVars& mv, Vector& q) const
+{
+ if (this->getUseEulerAngles(mv)) {
+ this->toQVec3(q,4) = Vec3(0); // TODO: kludge, clear unused element
+ this->toQ(q) = 0;
+ } else {
+ this->toQuat(q) = Vec4(1,0,0,0);
+ this->toQVec3(q,4) = 0;
+ }
+}
+
+bool enforceQuaternionConstraints(
+ const SBStateDigest& sbs,
+ Vector& q,
+ Vector& qErrest) const
+{
+ if (this->getUseEulerAngles(sbs.getModelVars()))
+ return false; // no change
+
+ Vec4& quat = this->toQuat(q);
+ quat = quat / quat.norm();
+
+ if (qErrest.size()) {
+ Vec4& qerr = this->toQuat(qErrest);
+ qerr -= dot(qerr,quat) * quat;
+ }
+
+ return true;
+}
+
+void convertToEulerAngles(const Vector& inputQ, Vector& outputQ) const {
+ this->toQVec3(outputQ, 4) = Vec3(0); // clear unused element
+ this->toQVec3(outputQ, 3) = this->fromQVec3(inputQ, 4);
+ this->toQVec3(outputQ, 0) = Rotation(Quaternion(this->fromQuat(inputQ))).convertRotationToBodyFixedXYZ();
+}
+
+void convertToQuaternions(const Vector& inputQ, Vector& outputQ) const {
+ this->toQVec3(outputQ, 4) = this->fromQVec3(inputQ, 3);
+ Rotation rot;
+ rot.setRotationToBodyFixedXYZ(this->fromQVec3(inputQ, 0));
+ this->toQuat(outputQ) = rot.convertRotationToQuaternion().asVec4();
+}
+
+
+};
+
+
+
+
+#endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_FREELINE_H_
+
diff --git a/Simbody/src/RigidBodyNodeSpec_Gimbal.h b/Simbody/src/RigidBodyNodeSpec_Gimbal.h
new file mode 100644
index 0000000..e40ee7d
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec_Gimbal.h
@@ -0,0 +1,210 @@
+#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_GIMBAL_H_
+#define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_GIMBAL_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Derived from IVM code written by Charles Schwieters *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Define the RigidBodyNode that implements a Gimbal mobilizer.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "RigidBodyNodeSpec.h"
+
+
+ // GIMBAL //
+
+// Gimbal joint. This provides three degrees of rotational freedom, i.e.,
+// unrestricted orientation of the body's M frame in the parent's F frame.
+// The generalized coordinates q are:
+// * 3 X-Y-Z (1-2-3) body fixed Euler angles (that is, fixed in M)
+// and generalized speeds u are:
+// * u = qdot, that is, the 1-2-3 body fixed Euler angle derivatives
+//
+// NOTE: This joint has a singularity when the middle angle is near 90 degrees.
+// In most cases you should use a Ball joint instead, which by default uses
+// a quaternion as its generalized coordinates to avoid this singularity. A
+// modeling option allows the Ball to be switched to use Euler angles when
+// convenient.
+
+template<bool noX_MB, bool noR_PF>
+class RBNodeGimbal : public RigidBodyNodeSpec<3, false, noX_MB, noR_PF> {
+public:
+
+typedef typename RigidBodyNodeSpec<3, false, noX_MB, noR_PF>::HType HType;
+virtual const char* type() { return "gimbal"; }
+
+RBNodeGimbal( const MassProperties& mProps_B,
+ const Transform& X_PF,
+ const Transform& X_BM,
+ bool isReversed,
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot)
+: RigidBodyNodeSpec<3, false, noX_MB, noR_PF>
+ (mProps_B,X_PF,X_BM,
+ nextUSlot,nextUSqSlot,nextQSlot,
+ RigidBodyNode::QDotIsAlwaysTheSameAsU,
+ RigidBodyNode::QuaternionIsNeverUsed,
+ isReversed)
+{
+ this->updateSlots(nextUSlot,nextUSqSlot,nextQSlot);
+}
+
+void setQToFitRotationImpl(const SBStateDigest& sbs, const Rotation& R_FM,
+ Vector& q) const {
+ this->toQ(q) = R_FM.convertRotationToBodyFixedXYZ();
+}
+
+void setQToFitTranslationImpl(const SBStateDigest& sbs, const Vec3& p_FM,
+ Vector& q) const {
+ // M and F frame origins are always coincident for this mobilizer so
+ // there is no way to create a translation by rotating. So the only
+ // translation we can represent is 0.
+}
+
+// Given the angular velocity of M in F, expressed in F, compute the Euler
+// angle derivatives qdot that would produce that angular velocity, and
+// return u=qdot.
+void setUToFitAngularVelocityImpl
+ (const SBStateDigest& sbs, const Vector& q, const Vec3& w_FM,
+ Vector& u) const
+{
+ const Vec2 cosxy(std::cos(q[0]), std::cos(q[1]));
+ const Vec2 sinxy(std::sin(q[0]), std::sin(q[1]));
+ const Real oocosy = 1 / cosxy[1];
+ const Vec3 qdot =
+ Rotation::convertAngVelInParentToBodyXYZDot(cosxy,sinxy,oocosy,w_FM);
+ this->toU(u) = qdot;
+}
+
+void setUToFitLinearVelocityImpl
+ (const SBStateDigest& sbs, const Vector&, const Vec3& v_FM,
+ Vector& u) const
+{
+ // M and F frame origins are always coincident for this mobilizer so
+ // there is no way to create a linear velocity by rotating. So the
+ // only linear velocity we can represent is 0.
+}
+
+// We want to cache cos and sin for each angle, and also 1/cos of the middle
+// angle will be handy to have around.
+enum {PoolSize=7};
+// cos x,y,z sin x,y,z 1/cos(y)
+enum {CosQ=0, SinQ=3, OOCosQy=6};
+int calcQPoolSize(const SBModelVars& mv) const
+{ return PoolSize; }
+
+// This is expensive since we have three sin/cos computations and a divide
+// to do, approx. 150 flops. But we hope to re-use these calculations several
+// times before we're done with a complete realization.
+void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* qCache, int nQCache,
+ Real* qErr, int nQErr) const
+{
+ assert(q && nq==3 && qCache && nQCache==PoolSize && nQErr==0);
+
+ const Real cy = std::cos(q[1]);
+ Vec3::updAs(&qCache[CosQ]) =
+ Vec3(std::cos(q[0]), cy, std::cos(q[2]));
+ Vec3::updAs(&qCache[SinQ]) =
+ Vec3(std::sin(q[0]), std::sin(q[1]), std::sin(q[2]));
+ qCache[OOCosQy] = 1/cy; // trouble at 90 or 270 (-90) degrees
+}
+
+// Because of the precalculations we can calculate the cross-mobilizer
+// transform in only 18 flops.
+void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* qCache, int nQCache,
+ Transform& X_F0M0) const
+{
+ assert(q && nq==3 && qCache && nQCache==PoolSize);
+
+ X_F0M0.updR().setRotationToBodyFixedXYZ // 18 flops
+ (Vec3::getAs(&qCache[CosQ]), Vec3::getAs(&qCache[SinQ]));
+ X_F0M0.updP() = 0; // This joint can't translate.
+}
+
+// Generalized speeds are the Euler angle derivatives. The H_FM matrix maps
+// those into the angular velocity of M in F, expressed in F, so
+// [w_FM] [Hw_FM]
+// [ ] = [ ] * qdot (where v_FM is always zero)
+// [v_FM] [ 0 ]
+// Using the precalculations this requires only 3 flops, although there is
+// a fair bit of poking around in memory required.
+void calcAcrossJointVelocityJacobian(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+{
+ const SBModelCache& mc = sbs.getModelCache();
+ // Use "upd" here because we're realizing positions now.
+ const SBTreePositionCache& pc = sbs.updTreePositionCache();
+ const Real* pool = this->getQPool(mc, pc);
+
+ const Real c0 = pool[CosQ], c1 = pool[CosQ+1];
+ const Real s0 = pool[SinQ], s1 = pool[SinQ+1];
+
+ // Fill in columns of H_FM. See Rotation::calcNInvForBodyXYZInParentFrame().
+ H_FM(0) = SpatialVec( Vec3(1, 0, 0), Vec3(0) );
+ H_FM(1) = SpatialVec( Vec3(0, c0, s0), Vec3(0) );
+ H_FM(2) = SpatialVec( Vec3(s1, -s0*c1, c0*c1), Vec3(0) );
+}
+
+// Differentiate H_FM to get HDot_FM. Note that this depends on qdot:
+// d/dt cos(q0) = -sin(q0)*qdot0, etc.
+void calcAcrossJointVelocityJacobianDot(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+{
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+
+ const Real* pool = this->getQPool(mc, pc);
+ const Real c0 = pool[CosQ], c1 = pool[CosQ+1];
+ const Real s0 = pool[SinQ], s1 = pool[SinQ+1];
+
+ // Use "upd" here because we're realizing velocities now.
+ const Vec3& qdot = this->fromQ(sbs.updQDot());
+ const Real qd0 = qdot[0], qd1 = qdot[1];
+
+ const Real dc0 = -s0*qd0, dc1 = -s1*qd1; // derivatives of c0,c1,s0,s1
+ const Real ds0 = c0*qd0, ds1 = c1*qd1;
+
+ // Compare with H_FM above.
+ HDot_FM(0) = SpatialVec(Vec3(0, 0, 0), Vec3(0));
+ HDot_FM(1) = SpatialVec(Vec3(0, dc0, ds0), Vec3(0));
+ HDot_FM(2) = SpatialVec(Vec3(ds1, -ds0*c1-s0*dc1, dc0*c1+c0*dc1), Vec3(0));
+}
+
+// Can use default for calcQDot, multiplyByN, etc., since qdot==u for Gimbal
+// mobilizer.
+
+};
+
+
+
+#endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_GIMBAL_H_
+
diff --git a/Simbody/src/RigidBodyNodeSpec_LineOrientation.h b/Simbody/src/RigidBodyNodeSpec_LineOrientation.h
new file mode 100644
index 0000000..d379134
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec_LineOrientation.h
@@ -0,0 +1,419 @@
+#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_LINEORIENTATION_H_
+#define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_LINEORIENTATION_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Paul Mitiguy *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Define the RigidBodyNode that implements a LineOrientation mobilizer.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "RigidBodyNodeSpec.h"
+
+
+ // LINE ORIENTATION //
+
+// LineOrientation joint. Like a Ball joint, this provides full rotational
+// freedom, but for a degenerate body which is thin (inertialess) along its
+// own z axis. These arise in molecular modeling for linear molecules formed
+// by pairs of atoms, or by multiple atoms in a linear arrangement like
+// carbon dioxide (CO2) whose structure is O=C=O in a straight line. We are
+// assuming that there is no meaning to a rotation about the linear axis,
+// so free orientation requires just *two* degrees of freedom, not *three*
+// as is required for general rigid bodies. And in fact we can get away with
+// just two generalized speeds. But so far, no one has been able to come up
+// with a way to manage with only two generalized coordinates, so this joint
+// has the same q's as a regular Orientation (Ball) joint: either a quaternion
+// for unconditional stability, or a three-angle (body fixed 1-2-3)
+// Euler sequence which will be dynamically singular when the middle (y) axis
+// is 90 degrees. Use the Euler sequence only for small motions or for
+// kinematics problems (and note that only the first two are meaningful).
+//
+// To summarize, the generalized coordinates are:
+// * 4 quaternions or 3 1-2-3 body fixed Euler angles (that is, fixed in M)
+// and generalized speeds are:
+// * the x,y components of the angular velocity w_FM_M, that is, the angular
+// velocity of M in F expressed in M (where we want wz=0).
+// NOTE: THAT IS A DIFFERENT FRAME THAN IS USED FOR BALL AND GIMBAL
+// Thus the qdots have to be derived from the generalized speeds to
+// be turned into either 4 quaternion derivatives or 3 Euler angle derivatives.
+template<bool noX_MB, bool noR_PF>
+class RBNodeLineOrientation : public RigidBodyNodeSpec<2, false, noX_MB, noR_PF> {
+public:
+
+typedef typename RigidBodyNodeSpec<2, false, noX_MB, noR_PF>::HType HType;
+virtual const char* type() { return "lineOrientation"; }
+
+RBNodeLineOrientation(const MassProperties& mProps_B,
+ const Transform& X_PF,
+ const Transform& X_BM,
+ bool isReversed,
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot)
+ : RigidBodyNodeSpec<2, false, noX_MB, noR_PF>(mProps_B,X_PF,X_BM,nextUSlot,nextUSqSlot,nextQSlot,
+ RigidBodyNode::QDotMayDifferFromU, RigidBodyNode::QuaternionMayBeUsed, isReversed)
+{
+ this->updateSlots(nextUSlot,nextUSqSlot,nextQSlot);
+}
+
+void setQToFitRotationImpl(const SBStateDigest& sbs, const Rotation& R_FM,
+ Vector& q) const
+{
+ if (this->getUseEulerAngles(sbs.getModelVars()))
+ this->toQVec3(q,0) = R_FM.convertRotationToBodyFixedXYZ();
+ else
+ this->toQuat(q) = R_FM.convertRotationToQuaternion().asVec4();
+}
+
+void setQToFitTranslationImpl(const SBStateDigest& sbs, const Vec3& p_FM,
+ Vector& q) const {
+ // M and F frame origins are always coincident for this mobilizer so there is no
+ // way to create a translation by rotating. So the only translation we can represent is 0.
+}
+
+void setUToFitAngularVelocityImpl(const SBStateDigest& sbs,
+ const Vector& q, const Vec3& w_FM,
+ Vector& u) const
+{
+ Rotation R_FM;
+ if (this->getUseEulerAngles(sbs.getModelVars()))
+ R_FM.setRotationToBodyFixedXYZ( this->fromQVec3(q,0) );
+ else {
+ // TODO: should use qnorm pool
+ R_FM.setRotationFromQuaternion( Quaternion(this->fromQuat(q)) ); // normalize
+ }
+ const Vec3 w_FM_M = ~R_FM*w_FM;
+ this->toU(u) = Vec2(w_FM_M[0], w_FM_M[1]); // (x,y) of relative angular velocity always used as generalized speeds
+}
+
+void setUToFitLinearVelocityImpl
+ (const SBStateDigest& sbs, const Vector&, const Vec3& v_FM, Vector& u) const
+{
+ // M and F frame origins are always coincident for this mobilizer so there is no
+ // way to create a linear velocity by rotating. So the only linear velocity
+ // we can represent is 0.
+}
+
+// When we're using Euler angles, we're going to want to cache cos and sin for
+// each angle, and also 1/cos of the middle angle will be handy to have around.
+// When we're using quaternions, we'll cache 1/|q| for convenient normalizing
+// of quaternions.
+enum {AnglePoolSize=7, QuatPoolSize=1};
+// cos x,y,z sin x,y,z 1/cos(y)
+enum {AngleCosQ=0, AngleSinQ=3, AngleOOCosQy=6};
+enum {QuatOONorm=0};
+int calcQPoolSize(const SBModelVars& mv) const
+{ return this->getUseEulerAngles(mv) ? AnglePoolSize : QuatPoolSize; }
+
+void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* qCache, int nQCache,
+ Real* qErr, int nQErr) const
+{
+ if (this->getUseEulerAngles(sbs.getModelVars())) {
+ assert(q && nq==3 && qCache && nQCache==AnglePoolSize && nQErr==0);
+ const Real cy = std::cos(q[1]);
+ Vec3::updAs(&qCache[AngleCosQ]) =
+ Vec3(std::cos(q[0]), cy, std::cos(q[2]));
+ Vec3::updAs(&qCache[AngleSinQ]) =
+ Vec3(std::sin(q[0]), std::sin(q[1]), std::sin(q[2]));
+ qCache[AngleOOCosQy] = 1/cy; // trouble at 90 degrees
+ } else {
+ assert(q && nq==4 && qCache && nQCache==QuatPoolSize &&
+ qErr && nQErr==1);
+ const Real quatLen = Vec4::getAs(q).norm();
+ qErr[0] = quatLen - Real(1); // normalization error
+ qCache[QuatOONorm] = 1/quatLen; // save for later
+ }
+}
+
+void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* qCache, int nQCache,
+ Transform& X_F0M0) const
+{
+ X_F0M0.updP() = 0; // This joint can't translate.
+ if (this->getUseEulerAngles(sbs.getModelVars())) {
+ assert(q && nq==3 && qCache && nQCache==AnglePoolSize);
+ X_F0M0.updR().setRotationToBodyFixedXYZ // 18 flops
+ (Vec3::getAs(&qCache[AngleCosQ]), Vec3::getAs(&qCache[AngleSinQ]));
+ } else {
+ assert(q && nq==4 && qCache && nQCache==QuatPoolSize);
+ // Must use a normalized quaternion to generate the rotation matrix.
+ // Here we normalize with just 4 flops using precalculated 1/norm(q).
+ const Quaternion quat(Vec4::getAs(q)*qCache[QuatOONorm], true);
+ X_F0M0.updR().setRotationFromQuaternion(quat); // 29 flops
+ }
+}
+
+// The generalized speeds for this 2-dof rotational joint are the x and y
+// components of the angular velocity of M in the F frame, expressed in the *M*
+// frame.
+void calcAcrossJointVelocityJacobian(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+{
+ const SBTreePositionCache& pc = sbs.updTreePositionCache(); // "upd" because we're realizing positions now
+ const Transform X_F0M0 = this->findX_F0M0(pc);
+
+ // Dropping the 0's here.
+ const Rotation& R_FM = X_F0M0.R();
+ const Vec3& Mx_F = R_FM.x(); // M's x axis, expressed in F
+ const Vec3& My_F = R_FM.y(); // M's y axis, expressed in F
+
+ H_FM(0) = SpatialVec( Mx_F, Vec3(0) );
+ H_FM(1) = SpatialVec( My_F, Vec3(0) );
+}
+
+// Since the Jacobian above is not constant in F,
+// its time derivative is non zero. Here we use the fact that for
+// a vector r_B_A fixed in a moving frame B but expressed in another frame A,
+// its time derivative in A is the angular velocity of B in A crossed with
+// the vector, i.e., d_A/dt r_B_A = w_AB % r_B_A.
+void calcAcrossJointVelocityJacobianDot(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+{
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const SBTreeVelocityCache& vc = sbs.updTreeVelocityCache(); // "upd" because we're realizing velocities now
+ const Transform X_F0M0 = this->findX_F0M0(pc);
+
+ // Dropping the 0's here.
+ const Rotation& R_FM = X_F0M0.R();
+ const Vec3& Mx_F = R_FM.x(); // M's x axis, expressed in F
+ const Vec3& My_F = R_FM.y(); // M's y axis, expressed in F
+
+ const Vec3 w_FM = this->find_w_F0M0(pc,vc); // angular velocity of M in F
+
+ HDot_FM(0) = SpatialVec( w_FM % Mx_F, Vec3(0) );
+ HDot_FM(1) = SpatialVec( w_FM % My_F, Vec3(0) );
+}
+
+// CAUTION: we do not zero the unused 4th element of q for Euler angles; it
+// is up to the caller to do that if it is necessary.
+// Compute out_q = N * in_u
+// or out_u = in_q * N
+// The u quantity is a vector v_M in the child body frame (e.g. angular
+// velocity of child in parent, but expressed in child), while the q
+// quantity is the derivative of a quaternion or Euler sequence representing
+// R_FM, i.e. orientation of child in parent.
+void multiplyByN(const SBStateDigest& sbs, bool matrixOnRight,
+ const Real* in, Real* out) const
+{
+ assert(sbs.getStage() >= Stage::Position);
+ assert(in && out);
+
+ const SBModelVars& mv = sbs.getModelVars();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Vector& allQ = sbs.getQ();
+
+ if (this->getUseEulerAngles(mv)) {
+ const Vec3& q = this->fromQVec3(allQ,0);
+ const Mat32 N_FM = // N here mixes parent and body frames (convenient)
+ Rotation::calcNForBodyXYZInBodyFrame(q)
+ .getSubMat<3,2>(0,0); // drop 3rd column
+ if (matrixOnRight) Row2::updAs(out) = Row3::getAs(in) * N_FM;
+ else Vec3::updAs(out) = N_FM * Vec2::getAs(in);
+ } else {
+ // Quaternion: N block is only available expecting angular velocity
+ // in the parent frame F, but we have it in M for this joint so we
+ // have to calculate N_FM = N_FF*R_FM.
+ const Rotation& R_FM = this->getX_FM(pc).R();
+ const Vec4& q = this->fromQuat(allQ);
+ const Mat42 N_FM = (Rotation::calcUnnormalizedNForQuaternion(q)*R_FM)
+ .getSubMat<4,2>(0,0); // drop 3rd column
+ if (matrixOnRight) Row2::updAs(out) = Row4::getAs(in) * N_FM;
+ else Vec4::updAs(out) = N_FM * Vec2::getAs(in);
+ }
+}
+
+// Compute out_u = inv(N) * in_q
+// or out_q = in_u * inv(N)
+void multiplyByNInv(const SBStateDigest& sbs, bool matrixOnRight,
+ const Real* in, Real* out) const
+{
+ assert(sbs.getStage() >= Stage::Position);
+ assert(in && out);
+
+ const SBModelVars& mv = sbs.getModelVars();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Vector& allQ = sbs.getQ();
+
+ if (this->getUseEulerAngles(mv)) {
+ const Vec3& q = this->fromQVec3(allQ,0);
+ const Mat23 NInv_MF = Rotation::calcNInvForBodyXYZInBodyFrame(q)
+ .getSubMat<2,3>(0,0); // drop 3rd row
+ if (matrixOnRight) Row3::updAs(out) = Row2::getAs(in) * NInv_MF;
+ else Vec2::updAs(out) = NInv_MF * Vec3::getAs(in);
+ } else {
+ // Quaternion: NInv block is only available expecting angular
+ // velocity in the parent frame F, but we have it in M for
+ // this joint so we have to calculate NInv_MF = R_MF*NInv_FF.
+ const Rotation& R_FM = this->getX_FM(pc).R();
+ const Vec4& q = this->fromQuat(allQ);
+ const Mat24 NInv_MF = (~R_FM*Rotation::calcUnnormalizedNInvForQuaternion(q))
+ .getSubMat<2,4>(0,0); // drop 3rd row
+ if (matrixOnRight) Row4::updAs(out) = Row2::getAs(in) * NInv_MF;
+ else Vec2::updAs(out) = NInv_MF * Vec4::getAs(in);
+ }
+}
+
+// Compute out_q = NDot * in_u
+// or out_u = in_q * NDot
+void multiplyByNDot(const SBStateDigest& sbs, bool matrixOnRight,
+ const Real* in, Real* out) const
+{
+ assert(sbs.getStage() >= Stage::Velocity);
+ assert(in && out);
+
+ const SBModelVars& mv = sbs.getModelVars();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Vector& allQ = sbs.getQ();
+ const Vector& allQDot = sbs.getQDot();
+
+ if (this->getUseEulerAngles(mv)) {
+ const Vec3& q = this->fromQVec3(allQ,0);
+ const Vec3& qdot = this->fromQVec3(allQDot,0);
+ const Mat32 NDot_FM = // NDot here mixes parent and body frames (convenient)
+ Rotation::calcNDotForBodyXYZInBodyFrame(q, qdot)
+ .getSubMat<3,2>(0,0); // drop 3rd column
+ if (matrixOnRight) Row2::updAs(out) = Row3::getAs(in) * NDot_FM;
+ else Vec3::updAs(out) = NDot_FM * Vec2::getAs(in);
+ } else {
+ // Quaternion: NDot block is only available expecting angular velocity
+ // in the parent frame F, but we have it in M for this joint so we
+ // have to calculate NDot_FM = NDot_FF*R_FM.
+ const Rotation& R_FM = this->getX_FM(pc).R();
+ const Vec4& qdot = this->fromQuat(allQDot);
+ const Mat42 NDot_FM =
+ (Rotation::calcUnnormalizedNDotForQuaternion(qdot)*R_FM)
+ .getSubMat<4,2>(0,0); // drop 3rd column
+ if (matrixOnRight) Row2::updAs(out) = Row4::getAs(in) * NDot_FM;
+ else Vec4::updAs(out) = NDot_FM * Vec2::getAs(in);
+ }
+}
+
+void calcQDot(
+ const SBStateDigest& sbs,
+ const Real* u,
+ Real* qdot) const
+{
+ const SBModelVars& mv = sbs.getModelVars();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Vec3 w_FM_M = Vec2::getAs(u).append1(Real(0)); // angular velocity of M in F, exp in M (with wz=0)
+ if (this->getUseEulerAngles(mv)) {
+ Vec3::updAs(qdot) = Rotation::convertAngVelInBodyFrameToBodyXYZDot(this->fromQVec3(sbs.getQ(),0),
+ w_FM_M); // need w in *body*, not parent
+ qdot[3] = 0;
+ } else {
+ const Rotation& R_FM = this->getX_FM(pc).R();
+ Vec4::updAs(qdot) = Rotation::convertAngVelToQuaternionDot(this->fromQuat(sbs.getQ()),
+ R_FM*w_FM_M); // need w in *parent* frame
+ }
+}
+
+void calcQDotDot(
+ const SBStateDigest& sbs,
+ const Real* udot,
+ Real* qdotdot) const
+{
+ const SBModelVars& mv = sbs.getModelVars();
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const Vec3 w_FM_M = this->fromU(sbs.getU()).append1(Real(0)); // angular velocity of M in F, exp in M (with wz=0)
+ const Vec3 w_FM_M_dot = Vec2::getAs(udot).append1(Real(0));
+
+ if (this->getUseEulerAngles(mv)) {
+ Vec3::updAs(qdotdot) = Rotation::convertAngVelDotInBodyFrameToBodyXYZDotDot
+ (this->fromQVec3(sbs.getQ(),0), w_FM_M, w_FM_M_dot); // body frame
+ qdotdot[3] = 0;
+ } else {
+ const Rotation& R_FM = this->getX_FM(pc).R();
+ Vec4::updAs(qdotdot) = Rotation::convertAngVelDotToQuaternionDotDot
+ (this->fromQuat(sbs.getQ()),R_FM*w_FM_M,R_FM*w_FM_M_dot); // parent frame
+ }
+}
+
+int getMaxNQ() const {return 4;}
+int getNQInUse(const SBModelVars& mv) const {
+ return this->getUseEulerAngles(mv) ? 3 : 4;
+}
+bool isUsingQuaternion(const SBStateDigest& sbs, MobilizerQIndex& startOfQuaternion) const {
+ if (this->getUseEulerAngles(sbs.getModelVars())) {startOfQuaternion.invalidate(); return false;}
+ startOfQuaternion = MobilizerQIndex(0); // quaternion comes first
+ return true;
+}
+
+void setMobilizerDefaultPositionValues(
+ const SBModelVars& mv,
+ Vector& q) const
+{
+ if (this->getUseEulerAngles(mv)) {
+ //TODO: kludge
+ this->toQuat(q) = Vec4(0); // clear unused element
+ this->toQ(q) = 0;
+ }
+ else this->toQuat(q) = Vec4(1,0,0,0);
+}
+
+bool enforceQuaternionConstraints(
+ const SBStateDigest& sbs,
+ Vector& q,
+ Vector& qErrest) const
+{
+ if (this->getUseEulerAngles(sbs.getModelVars()))
+ return false; // no change
+
+ Vec4& quat = this->toQuat(q);
+ quat = quat / quat.norm();
+
+ if (qErrest.size()) {
+ Vec4& qerr = this->toQuat(qErrest);
+ qerr -= dot(qerr,quat) * quat;
+ }
+
+ return true;
+}
+
+void convertToEulerAngles(const Vector& inputQ, Vector& outputQ) const {
+ this->toQVec3(outputQ, 4) = Vec3(0); // clear unused element
+ this->toQVec3(outputQ, 2) = this->fromQVec3(inputQ, 3);
+ this->toQVec3(outputQ, 0) = Rotation(Quaternion(this->fromQuat(inputQ))).convertRotationToBodyFixedXYZ();
+}
+
+void convertToQuaternions(const Vector& inputQ, Vector& outputQ) const {
+ this->toQVec3(outputQ, 4) = this->fromQVec3(inputQ, 3);
+ Rotation rot;
+ rot.setRotationToBodyFixedXYZ(this->fromQVec3(inputQ, 0));
+ this->toQuat(outputQ) = rot.convertRotationToQuaternion().asVec4();
+}
+
+
+};
+
+
+
+#endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_LINEORIENTATION_H_
+
diff --git a/Simbody/src/RigidBodyNodeSpec_Pin.h b/Simbody/src/RigidBodyNodeSpec_Pin.h
new file mode 100644
index 0000000..3f7331b
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec_Pin.h
@@ -0,0 +1,163 @@
+#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_PIN_H_
+#define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_PIN_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Derived from IVM code written by Charles Schwieters *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Define the RigidBodyNode that implements a Pin mobilizer, also known as a
+ * Torsion or Revolute joint.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "RigidBodyNodeSpec.h"
+
+
+ // PIN (TORSION) //
+
+// This is a "pin" or "torsion" or "revolute" joint, meaning one degree of
+// rotational freedom about a particular axis, the z axis of the parent's F
+// frame, which is aligned forever with the z axis of the body's M frame. In
+// addition, the origin points Mo of M and Fo of F are identical forever.
+template<bool noX_MB, bool noR_PF>
+class RBNodeTorsion : public RigidBodyNodeSpec<1, false, noX_MB, noR_PF> {
+public:
+virtual const char* type() { return "torsion"; }
+typedef typename RigidBodyNodeSpec<1, false, noX_MB, noR_PF>::HType HType;
+
+RBNodeTorsion(const MassProperties& mProps_B,
+ const Transform& X_PF,
+ const Transform& X_BM,
+ bool isReversed,
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot)
+: RigidBodyNodeSpec<1, false, noX_MB, noR_PF>(mProps_B,X_PF,X_BM,nextUSlot,nextUSqSlot,nextQSlot,
+ RigidBodyNode::QDotIsAlwaysTheSameAsU, RigidBodyNode::QuaternionIsNeverUsed,
+ isReversed)
+{
+ this->updateSlots(nextUSlot,nextUSqSlot,nextQSlot);
+}
+
+void setQToFitRotationImpl(const SBStateDigest& sbs, const Rotation& R_FM,
+ Vector& q) const {
+ // The only rotation our pin joint can handle is about z.
+ // TODO: should use 321 to deal with singular configuration (angle2==pi/2)
+ // better; in that case 1 and 3 are aligned and the conversion routine
+ // allocates all the rotation to whichever comes first.
+ // TODO: isn't there a better way to come up with "the rotation around z
+ // that best approximates a rotation R"?
+ const Vec3 angles123 = R_FM.convertRotationToBodyFixedXYZ();
+ this->to1Q(q) = angles123[2];
+}
+
+void setQToFitTranslationImpl(const SBStateDigest& sbs, const Vec3& p_FM,
+ Vector& q) const {
+ // M and F frame origins are always coincident for this mobilizer so there
+ // is no way to create a translation by rotating. So the only translation
+ // we can represent is 0.
+}
+
+void setUToFitAngularVelocityImpl(const SBStateDigest& sbs, const Vector&,
+ const Vec3& w_FM, Vector& u) const {
+ // We can only represent an angular velocity along z with this joint.
+ this->to1U(u) = w_FM[2]; // project angular velocity onto z axis
+}
+
+void setUToFitLinearVelocityImpl
+ (const SBStateDigest& sbs, const Vector&, const Vec3& v_FM, Vector& u) const
+{
+ // M and F frame origins are always coincident for this mobilizer so there
+ // is no way to create a linear velocity by rotating. So the only linear
+ // velocity we can represent is 0.
+}
+
+enum {PoolSize=2}; // number of Reals
+enum {CosQ=0, SinQ=1};
+// We want space for cos(q) and sin(q).
+int calcQPoolSize(const SBModelVars&) const
+{ return PoolSize; }
+
+// Precalculation of sin/cos costs around 50 flops.
+void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* qCache, int nQCache,
+ Real* qErr, int nQErr) const
+{
+ assert(q && nq==1 && qCache && nQCache==PoolSize && nQErr==0);
+ qCache[CosQ] = std::cos(q[0]);
+ qCache[SinQ] = std::sin(q[0]);
+}
+
+// This is nearly free since we already calculated sin/cos.
+void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* qCache, int nQCache,
+ Transform& X_FM) const
+{
+ assert(q && nq==1 && qCache && nQCache==PoolSize);
+ X_FM.updR().setRotationFromAngleAboutZ(qCache[CosQ], qCache[SinQ]);
+ X_FM.updP() = 0; // can't translate
+}
+
+// The generalized speed is the angular velocity of M in the F frame,
+// about F's z axis, expressed in F. (This axis is also constant in M.)
+void calcAcrossJointVelocityJacobian(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+{
+ H_FM(0) = SpatialVec( Vec3(0,0,1), Vec3(0) );
+}
+
+
+// Since H_FM above is constant in F, its time derivative in F is zero.
+void calcAcrossJointVelocityJacobianDot(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+{
+ HDot_FM(0) = SpatialVec( Vec3(0), Vec3(0) );
+}
+
+// Override the computation of reverse-H for this simple mobilizer.
+void calcReverseMobilizerH_FM(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+{
+ H_FM(0) = SpatialVec( Vec3(0,0,-1), Vec3(0) );
+}
+
+// Override the computation of reverse-HDot for this simple mobilizer.
+void calcReverseMobilizerHDot_FM(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+{
+ // doesn't get better than this!
+ HDot_FM(0) = SpatialVec( Vec3(0), Vec3(0) );
+}
+
+};
+
+
+#endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_PIN_H_
+
diff --git a/Simbody/src/RigidBodyNodeSpec_Planar.h b/Simbody/src/RigidBodyNodeSpec_Planar.h
new file mode 100644
index 0000000..8b98f86
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec_Planar.h
@@ -0,0 +1,158 @@
+#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_PLANAR_H_
+#define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_PLANAR_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Derived from IVM code written by Charles Schwieters *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Define the RigidBodyNode that implements a Planar mobilizer.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "RigidBodyNodeSpec.h"
+
+
+ // PLANAR //
+
+// This provides free motion (translation and rotation) in a plane. We use
+// the 2d coordinate system formed by the x,y axes of F as the translations,
+// and the common z axis of F and M as the rotational axis. The generalized
+// coordinates are theta,x,y interpreted as rotation around z and translation
+// along the (space fixed) Fx and Fy axes.
+template<bool noX_MB, bool noR_PF>
+class RBNodePlanar : public RigidBodyNodeSpec<3, false, noX_MB, noR_PF> {
+public:
+typedef typename RigidBodyNodeSpec<3, false, noX_MB, noR_PF>::HType HType;
+virtual const char* type() { return "planar"; }
+
+RBNodePlanar(const MassProperties& mProps_B,
+ const Transform& X_PF,
+ const Transform& X_BM,
+ bool isReversed,
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot)
+: RigidBodyNodeSpec<3, false, noX_MB, noR_PF>(mProps_B,X_PF,X_BM,nextUSlot,nextUSqSlot,nextQSlot,
+ RigidBodyNode::QDotIsAlwaysTheSameAsU, RigidBodyNode::QuaternionIsNeverUsed,
+ isReversed)
+{
+ this->updateSlots(nextUSlot,nextUSqSlot,nextQSlot);
+}
+
+ // Implementations of virtual methods.
+
+// This has a default implementation but it rotates first then translates,
+// which works fine for the normal Planar joint but produces wrong behavior
+// when the mobilizer is reversed.
+void setQToFitTransformImpl(const SBStateDigest& sbs, const Transform& X_FM,
+ Vector& q) const OVERRIDE_11
+{
+ setQToFitTranslationImpl(sbs, X_FM.p(), q); // see below
+ setQToFitRotationImpl(sbs, X_FM.R(), q);
+}
+
+void setQToFitRotationImpl(const SBStateDigest& sbs, const Rotation& R_FM, Vector& q) const {
+ // The only rotation our planar joint can handle is about z.
+ // TODO: should use 321 to deal with singular configuration (angle2==pi/2) better;
+ // in that case 1 and 3 are aligned and the conversion routine allocates all the
+ // rotation to whichever comes first.
+ // TODO: isn't there a better way to come up with "the rotation around z that
+ // best approximates a rotation R"?
+ const Vec3 angles123 = R_FM.convertRotationToBodyFixedXYZ();
+ this->toQ(q)[0] = angles123[2];
+}
+void setQToFitTranslationImpl(const SBStateDigest& sbs, const Vec3& p_FM, Vector& q) const {
+ // Ignore translation in the z direction.
+ this->toQ(q)[1] = p_FM[0]; // x
+ this->toQ(q)[2] = p_FM[1]; // y
+}
+
+void setUToFitAngularVelocityImpl(const SBStateDigest& sbs, const Vector&, const Vec3& w_FM, Vector& u) const {
+ // We can represent the z angular velocity exactly, but nothing else.
+ this->toU(u)[0] = w_FM[2];
+}
+void setUToFitLinearVelocityImpl
+ (const SBStateDigest& sbs, const Vector&, const Vec3& v_FM, Vector& u) const
+{
+ // Ignore translational velocity in the z direction.
+ this->toU(u)[1] = v_FM[0]; // x
+ this->toU(u)[2] = v_FM[1]; // y
+}
+
+enum {PoolSize=2};
+enum {CosQ=0, SinQ=1};
+// We want space for cos(q) and sin(q).
+int calcQPoolSize(const SBModelVars&) const
+{ return PoolSize; }
+
+// Precalculation of sin/cos costs around 50 flops.
+void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* qCache, int nQCache,
+ Real* qErr, int nQErr) const
+{
+ assert(q && nq==3 && qCache && nQCache==PoolSize && nQErr==0);
+ qCache[CosQ] = std::cos(q[0]);
+ qCache[SinQ] = std::sin(q[0]);
+}
+
+// This is nearly free since we already calculated sin/cos.
+void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* qCache, int nQCache,
+ Transform& X_FM) const
+{
+ assert(q && nq==3 && qCache && nQCache==PoolSize);
+ // Rotational q is about common z axis, translational q's along
+ // Fx and Fy.
+ X_FM.updR().setRotationFromAngleAboutZ(qCache[CosQ], qCache[SinQ]);
+ X_FM.updP() = Vec3(q[1], q[2], 0);
+}
+
+// The rotational generalized speed is about the common z axis; translations
+// are along Fx and Fy so all axes are constant in F.
+void calcAcrossJointVelocityJacobian(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+{
+ H_FM(0) = SpatialVec( Vec3(0,0,1), Vec3(0) );
+ H_FM(1) = SpatialVec( Vec3(0), Vec3(1,0,0) );
+ H_FM(2) = SpatialVec( Vec3(0), Vec3(0,1,0) );
+}
+
+// Since the Jacobian above is constant in F, its time derivative is zero.
+void calcAcrossJointVelocityJacobianDot(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+{
+ HDot_FM(0) = SpatialVec( Vec3(0), Vec3(0) );
+ HDot_FM(1) = SpatialVec( Vec3(0), Vec3(0) );
+ HDot_FM(2) = SpatialVec( Vec3(0), Vec3(0) );
+}
+
+};
+
+
+#endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_PLANAR_H_
+
diff --git a/Simbody/src/RigidBodyNodeSpec_PolarCoords.h b/Simbody/src/RigidBodyNodeSpec_PolarCoords.h
new file mode 100644
index 0000000..e512593
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec_PolarCoords.h
@@ -0,0 +1,205 @@
+#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_POLARCOORDS_H_
+#define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_POLARCOORDS_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Derived from IVM code written by Charles Schwieters *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Define the RigidBodyNode that implements a PolarCoords mobilizer, also known
+ * as a BendStretch joint.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "RigidBodyNodeSpec.h"
+
+
+ // BEND-STRETCH //
+
+// This is a "bend-stretch" joint, meaning one degree of rotational freedom
+// about a particular axis, and one degree of translational freedom
+// along a perpendicular axis. The z axis of the parent's F frame is
+// used for rotation (and that is always aligned with the M frame z axis).
+// The x axis of the *M* frame is used for translation; that is, first
+// we rotate around z, which moves M's x with respect to F's x. Then
+// we slide along the rotated x axis. The two generalized coordinates are the
+// rotation and the translation, in that order.
+template<bool noX_MB, bool noR_PF>
+class RBNodeBendStretch : public RigidBodyNodeSpec<2, false, noX_MB, noR_PF> {
+public:
+typedef typename RigidBodyNodeSpec<2, false, noX_MB, noR_PF>::HType HType;
+virtual const char* type() { return "bendstretch"; }
+
+RBNodeBendStretch(const MassProperties& mProps_B,
+ const Transform& X_PF,
+ const Transform& X_BM,
+ bool isReversed,
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot)
+: RigidBodyNodeSpec<2, false, noX_MB, noR_PF>(mProps_B,X_PF,X_BM,nextUSlot,nextUSqSlot,nextQSlot,
+ RigidBodyNode::QDotIsAlwaysTheSameAsU, RigidBodyNode::QuaternionIsNeverUsed,
+ isReversed)
+{
+ this->updateSlots(nextUSlot,nextUSqSlot,nextQSlot);
+}
+
+
+void setQToFitRotationImpl(const SBStateDigest& sbs, const Rotation& R_FM,
+ Vector& q) const {
+ // The only rotation our bend-stretch joint can handle is about z.
+ // TODO: this code is bad -- see comments for Torsion joint above.
+ const Vec3 angles123 = R_FM.convertRotationToBodyFixedXYZ();
+ this->toQ(q)[0] = angles123[2];
+}
+
+void setQToFitTranslationImpl(const SBStateDigest& sbs, const Vec3& p_FM,
+ Vector& q) const {
+ // We can represent any translation that puts the M origin in the x-y plane
+ // of F, by a suitable rotation around z followed by translation along x.
+ const Vec2 r = p_FM.getSubVec<2>(0); // (rx, ry)
+ const Real d = r.norm();
+
+ // If there is no translation worth mentioning, we'll leave the rotational
+ // coordinate alone, otherwise rotate so M's x axis is aligned with r.
+ if (d >= 4*Eps) {
+ const Real angle = std::atan2(r[1],r[0]);
+ this->toQ(q)[0] = angle;
+ this->toQ(q)[1] = d;
+ } else
+ this->toQ(q)[1] = 0;
+}
+
+void setUToFitAngularVelocityImpl(const SBStateDigest& sbs, const Vector& q,
+ const Vec3& w_FM, Vector& u) const {
+ // We can only represent an angular velocity along z with this joint.
+ this->toU(u)[0] = w_FM[2];
+}
+
+// If the translational coordinate is zero, we can only represent a linear
+// velocity of Mo in F which is along M's current x axis direction. Otherwise,
+// we can represent any velocity in the x-y plane by introducing angular
+// velocity about z. We can never represent a linear velocity along z.
+void setUToFitLinearVelocityImpl
+ (const SBStateDigest& sbs, const Vector& q, const Vec3& v_FM, Vector& u) const
+{
+ // Decompose the requested v into "along Mx" and "along My" components.
+ const Rotation R_FM = Rotation( this->fromQ(q)[0], ZAxis ); // =[ Mx My Mz ] in F
+ const Vec3 v_FM_M = ~R_FM*v_FM; // re-express in M frame
+
+ this->toU(u)[1] = v_FM_M[0]; // velocity along Mx we can represent directly
+
+ const Real x = this->fromQ(q)[1]; // translation along Mx (signed)
+ if (std::abs(x) < SignificantReal) {
+ // No translation worth mentioning; we can only do x velocity, which
+ // we just set above.
+ return;
+ }
+
+ // significant translation
+ this->toU(u)[0] = v_FM_M[1] / x; // set angular velocity about z to produce vy
+}
+
+enum {PoolSize=2}; // number of Reals
+enum {CosQ=0, SinQ=1};
+// We want space for cos(q0) and sin(q0).
+int calcQPoolSize(const SBModelVars&) const
+{ return PoolSize; }
+
+// Precalculation of sin/cos costs around 50 flops.
+void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* qCache, int nQCache,
+ Real* qErr, int nQErr) const
+{
+ assert(q && nq==2 && qCache && nQCache==PoolSize && nQErr==0);
+ qCache[CosQ] = std::cos(q[0]);
+ qCache[SinQ] = std::sin(q[0]);
+}
+
+// This is about 15 flops.
+void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* qCache, int nQCache,
+ Transform& X_FM) const
+{
+ assert(q && nq==2 && qCache && nQCache==PoolSize);
+ X_FM.updR().setRotationFromAngleAboutZ(qCache[CosQ], qCache[SinQ]);
+ // Translational coordinate is in M frame so must be reexpressed in F.
+ X_FM.updP() = X_FM.R()*Vec3(q[1],0,0); // 15 flops; could be cheaper
+}
+
+// The generalized speeds for this bend-stretch joint are (1) the angular
+// velocity of M in the F frame, about F's z axis, expressed in F, and
+// (2) the (linear) velocity of M's origin in F, along *M's* current x axis
+// (that is, after rotation about z). (The z axis is also constant in M for
+// this joint.)
+// 9 flops (could be cheaper but who cares?)
+void calcAcrossJointVelocityJacobian(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+{
+ // use "upd" because we're realizing positions now
+ const SBTreePositionCache& pc = sbs.updTreePositionCache();
+ const Transform X_F0M0 = this->findX_F0M0(pc);
+ const Rotation& R_F0M0 = X_F0M0.R();
+
+ // Dropping the 0's here.
+ const Vec3& p_FM = X_F0M0.p();
+ const Vec3& Mx_F = R_F0M0.x(); // M's x axis, expressed in F
+
+ H_FM(0) = SpatialVec( Vec3(0,0,1), Vec3(0,0,1) % p_FM );
+ H_FM(1) = SpatialVec( Vec3(0), Mx_F );
+}
+
+// Since the the Jacobian above is not constant in F,
+// its time derivative is non zero. Here we use the fact that for
+// a vector r_B_A fixed in a moving frame B but expressed in another frame A,
+// its time derivative in A is the angular velocity of B in A crossed with
+// the vector, i.e., d_A/dt r_B_A = w_AB % r_B_A.
+// 18 flops (could be cheaper).
+void calcAcrossJointVelocityJacobianDot(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+{
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const SBTreeVelocityCache& vc = sbs.updTreeVelocityCache(); // use "upd" because we're realizing velocities now
+
+ const Transform X_F0M0 = this->findX_F0M0(pc);
+ const Rotation& R_F0M0 = X_F0M0.R();
+ const SpatialVec V_F0M0 = this->findV_F0M0(pc,vc);
+
+ // Dropping the 0's here.
+ const Vec3& Mx_F = R_F0M0.x(); // M's x axis, expressed in F
+ const Vec3& w_FM = V_F0M0[0]; // angular velocity of M in F
+ const Vec3& v_FM = V_F0M0[1]; // linear velocity of OM in F
+
+ HDot_FM(0) = SpatialVec( Vec3(0), Vec3(0,0,1) % v_FM );
+ HDot_FM(1) = SpatialVec( Vec3(0), w_FM % Mx_F );
+}
+
+};
+
+
+#endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_POLARCOORDS_H_
+
diff --git a/Simbody/src/RigidBodyNodeSpec_Screw.h b/Simbody/src/RigidBodyNodeSpec_Screw.h
new file mode 100644
index 0000000..a76cd3d
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec_Screw.h
@@ -0,0 +1,171 @@
+#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_SCREW_H_
+#define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_SCREW_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Derived from IVM code written by Charles Schwieters *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Define the RigidBodyNode that implements a Screw mobilizer.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "RigidBodyNodeSpec.h"
+
+
+ // SCREW //
+
+// This is a one-dof "screw" joint, meaning one degree of rotational freedom
+// about a particular axis, coupled to translation along that same axis.
+// Here we use the common z axis of the F and M frames, which remains
+// aligned forever. For the generalized coordinate q, we use the rotation
+// angle. For the generalized speed u we use the rotation rate, which is also
+// the angular velocity of M in F (about the z axis). We compute the
+// translational position as pitch*q, and the translation rate as pitch*u.
+template<bool noX_MB, bool noR_PF>
+class RBNodeScrew : public RigidBodyNodeSpec<1, false, noX_MB, noR_PF> {
+ Real pitch;
+public:
+typedef typename RigidBodyNodeSpec<1, false, noX_MB, noR_PF>::HType HType;
+virtual const char* type() { return "screw"; }
+
+RBNodeScrew(const MassProperties& mProps_B,
+ const Transform& X_PF,
+ const Transform& X_BM,
+ Real p, // the pitch
+ bool isReversed,
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot)
+: RigidBodyNodeSpec<1, false, noX_MB, noR_PF>(mProps_B,X_PF,X_BM,nextUSlot,nextUSqSlot,nextQSlot,
+ RigidBodyNode::QDotIsAlwaysTheSameAsU, RigidBodyNode::QuaternionIsNeverUsed,
+ isReversed),
+ pitch(p)
+{
+ this->updateSlots(nextUSlot,nextUSqSlot,nextQSlot);
+}
+
+void setQToFitRotationImpl(const SBStateDigest& sbs, const Rotation& R_FM,
+ Vector& q) const {
+ // The only rotation our screw joint can handle is about z.
+ // TODO: should use 321 to deal with singular configuration (angle2==pi/2)
+ // better; in that case 1 and 3 are aligned and the conversion routine
+ // allocates all the rotation to whichever comes first.
+ // TODO: isn't there a better way to come up with "the rotation around z
+ // that best approximates a rotation R"?
+ const Vec3 angles123 = R_FM.convertRotationToBodyFixedXYZ();
+ this->to1Q(q) = angles123[2];
+}
+
+void setQToFitTranslationImpl(const SBStateDigest& sbs, const Vec3& p_FM,
+ Vector& q) const {
+ this->to1Q(q) = p_FM[2]/pitch;
+}
+
+void setUToFitAngularVelocityImpl(const SBStateDigest& sbs, const Vector&,
+ const Vec3& w_FM, Vector& u) const {
+ // We can only represent an angular velocity along z with this joint.
+ this->to1U(u) = w_FM[2]; // project angular velocity onto z axis
+}
+
+void setUToFitLinearVelocityImpl
+ (const SBStateDigest& sbs, const Vector&, const Vec3& v_FM, Vector& u) const
+{
+ this->to1U(u) = v_FM[2]/pitch;
+}
+
+// We're currently using an angle as the generalized coordinate for the
+// screw joint but could just as easily have used translation or some
+// non-physical coordinate. It might make sense to offer a Model stage
+// option to set the coordinate meaning.
+
+enum {PoolSize=2}; // number of Reals
+enum {CosQ=0, SinQ=1};
+// We want space for cos(q) and sin(q).
+int calcQPoolSize(const SBModelVars&) const
+{ return PoolSize; }
+
+// Precalculation of sin/cos costs around 50 flops.
+void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* qCache, int nQCache,
+ Real* qErr, int nQErr) const
+{
+ assert(q && nq==1 && qCache && nQCache==PoolSize && nQErr==0);
+ qCache[CosQ] = std::cos(q[0]);
+ qCache[SinQ] = std::sin(q[0]);
+}
+
+// This is nearly free since we already calculated sin/cos.
+void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* qCache, int nQCache,
+ Transform& X_FM) const
+{
+ assert(q && nq==1 && qCache && nQCache==PoolSize);
+ X_FM.updR().setRotationFromAngleAboutZ(qCache[CosQ], qCache[SinQ]);
+ // Note that we're using the same coordinate to control
+ // translation, using "pitch" as a conversion from radians to
+ // length units.
+ X_FM.updP() = Vec3(0,0,q[0]*pitch);
+}
+
+// The generalized speed is the angular velocity of M in the F frame,
+// about F's z axis, expressed in F. (This axis is also constant in M.)
+void calcAcrossJointVelocityJacobian(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+{
+ H_FM(0) = SpatialVec( Vec3(0,0,1), Vec3(0,0,pitch) );
+}
+
+// Since the Jacobian above is constant in F, its time derivative in F is zero.
+void calcAcrossJointVelocityJacobianDot(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+{
+ HDot_FM(0) = SpatialVec( Vec3(0), Vec3(0) );
+}
+
+// Override the computation of reverse-H for this simple mobilizer.
+void calcReverseMobilizerH_FM(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+{
+ H_FM(0) = SpatialVec( Vec3(0,0,-1), Vec3(0,0,-pitch) );
+}
+
+// Override the computation of reverse-HDot for this simple mobilizer.
+void calcReverseMobilizerHDot_FM(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+{
+ HDot_FM(0) = SpatialVec( Vec3(0), Vec3(0) );
+}
+
+};
+
+
+
+#endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_SCREW_H_
+
diff --git a/Simbody/src/RigidBodyNodeSpec_Slider.h b/Simbody/src/RigidBodyNodeSpec_Slider.h
new file mode 100644
index 0000000..ecad06a
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec_Slider.h
@@ -0,0 +1,149 @@
+#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_SLIDER_H_
+#define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_SLIDER_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Derived from IVM code written by Charles Schwieters *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Define the RigidBodyNode that implements a Slider mobilizer, also known as a
+ * Sliding or Prismatic joint.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "RigidBodyNodeSpec.h"
+
+ // SLIDING (PRISMATIC) //
+
+// Sliding joint (1 dof translation). The translation is along the x
+// axis of the parent body's F frame, with M=F when the coordinate
+// is zero and the orientation of M in F frozen at 0 forever.
+template<bool noX_MB, bool noR_PF>
+class RBNodeSlider : public RigidBodyNodeSpec<1, true, noX_MB, noR_PF> {
+public:
+typedef typename RigidBodyNodeSpec<1, false, noX_MB, noR_PF>::HType HType;
+virtual const char* type() { return "slider"; }
+
+RBNodeSlider(const MassProperties& mProps_B,
+ const Transform& X_PF,
+ const Transform& X_BM,
+ bool isReversed,
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot)
+: RigidBodyNodeSpec<1, true, noX_MB, noR_PF>(mProps_B,X_PF,X_BM,nextUSlot,nextUSqSlot,nextQSlot,
+ RigidBodyNode::QDotIsAlwaysTheSameAsU, RigidBodyNode::QuaternionIsNeverUsed,
+ isReversed)
+{
+ this->updateSlots(nextUSlot,nextUSqSlot,nextQSlot);
+}
+
+ // Implementations of virtual methods.
+
+void setQToFitRotationImpl(const SBStateDigest& sbs, const Rotation& R_FM,
+ Vector& q) const {
+ // The only rotation a slider can represent is identity.
+}
+
+void setQToFitTranslationImpl(const SBStateDigest& sbs, const Vec3& p_FM,
+ Vector& q) const {
+ // We can only represent the x coordinate with this joint.
+ this->to1Q(q) = p_FM[0];
+}
+
+void setUToFitAngularVelocityImpl(const SBStateDigest& sbs, const Vector&,
+ const Vec3& w_FM, Vector& u) const {
+ // The only angular velocity a slider can represent is zero.
+}
+
+void setUToFitLinearVelocityImpl(const SBStateDigest& sbs, const Vector&,
+ const Vec3& v_FM, Vector& u) const
+{
+ // We can only represent a velocity along x with this joint.
+ this->to1U(u) = v_FM[0];
+}
+
+// A sliding joint doesn't need to cache any q calculations.
+int calcQPoolSize(const SBModelVars&) const
+{ return 0; }
+
+// Nothing to precalculate.
+void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* qCache, int nQCache,
+ Real* qErr, int nQErr) const
+{
+ assert(q && nq==1 && nQCache==0 && nQErr==0);
+}
+
+// This is free.
+void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* qCache, int nQCache,
+ Transform& X_FM) const
+{
+ assert(q && nq==1 && nQCache==0);
+ // Translation vector q is expressed in F (and M since they have same
+ // orientation). A sliding joint can't change orientation, and only
+ // translates along x.
+ X_FM = Transform(Rotation(), Vec3(q[0],0,0));
+}
+
+// The generalized speed is the velocity of M's origin in the F frame,
+// along F's x axis, expressed in F.
+void calcAcrossJointVelocityJacobian(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+{
+ H_FM(0) = SpatialVec( Vec3(0), Vec3(1,0,0) );
+}
+
+// Since the Jacobian above is constant in F, its time derivative is zero.
+void calcAcrossJointVelocityJacobianDot(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+{
+ HDot_FM(0) = SpatialVec( Vec3(0), Vec3(0) );
+}
+
+// Override the computation of reverse-H for this simple mobilizer.
+void calcReverseMobilizerH_FM(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+{
+ H_FM(0) = SpatialVec( Vec3(0), Vec3(-1,0,0) );
+}
+
+// Override the computation of reverse-HDot for this simple mobilizer.
+void calcReverseMobilizerHDot_FM(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+{
+ HDot_FM(0) = SpatialVec( Vec3(0), Vec3(0) );
+}
+
+};
+
+
+#endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_SLIDER_H_
+
diff --git a/Simbody/src/RigidBodyNodeSpec_SphericalCoords.h b/Simbody/src/RigidBodyNodeSpec_SphericalCoords.h
new file mode 100644
index 0000000..52f0c47
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec_SphericalCoords.h
@@ -0,0 +1,313 @@
+#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_SPHERICALCOORDS_H_
+#define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_SPHERICALCOORDS_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Derived from IVM code written by Charles Schwieters *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Define the RigidBodyNode that implements a SphericalCoods mobilizer.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "RigidBodyNodeSpec.h"
+
+/* This is a mobilizer whose generalized coordinates are directly interpretable
+as a spherical coordinate system in this order: azimuth (longitude), zenith
+(latitude-90), radius. In the reference configuration (all coordinates zero)
+the parent's F frame and child's M frame are aligned, with the z axes pointing
+to the north pole. The equator is in the common x-y plane. Then the azimuth is
+a rotation of M about the common z axis. Next, the zenith angle is a rotation
+about My, the *new* child body y axis. This angle measures the deviation of Mz
+from Fz, that is, 0 when Mz is pointing to F's north pole; Pi (180 degrees)
+when Mz points to the south pole. The final coordinate is a translation along
+a selected axis (Mx or Mz; default is Mz). Whenever the translation is zero,
+the F and M origins are coincident.
+
+This mobilizer can be used to connect an inertialess particle to its parent in
+an unrestricted way that corresponds directly to (torsion, bend, stretch)
+coordinates, with two caveats: (1) the joint is singular whenever the stretch
+is zero, and (2) the joint is singular when the bend angle is 0 or Pi, because
+then the torsion rotates the particle about its center. The joint is never
+singular if the child body has a full inertia matrix.
+
+Because there are an assortment of useful conventions which are minor
+variations on the spherical coordinate theme, we allow some flexibility in
+defining the coordinates here. Although the rotation axes are fixed in a 3-2
+body-fixed sequence, the third (sliding) axis can be selected as Mx or Mz.
+Any of the three coordinates can be defined to use a reversed sign convention,
+and you can "bake in" fixed offsets for the angles. So we define the mobilizer
+in terms of aximuth angle a, zenith angle z, and stretch length d and define
+these in terms of the generalized coordinates like this:
+<pre>
+ a = q0*s0 + a0 (about Mz==Fz)
+ z = q1*s1 + z0 (about My)
+ d = q2*s2 (along v from {Mx,Mz})
+</pre>
+In all cases we have qi_dot = ui.
+
+With this you can define a "geographical" coordinate system where Mx is the
+Greenwich line, a is latitude and z longitude (with north positive):
+<pre>
+ v = Mx
+ s0 = 1, a0 = 0
+ s1 = -1, z0 = 0
+ s2 = 1
+</pre>
+If you want the translation direction to be in Mz (the default) but would like
+q1=0 to mean the equatorial position rather than the (possibly singular) north
+pole which should be -90, define
+<pre>
+ v = Mz
+ s0 = 1, a0 = 0
+ s1 = 1, z0 = Pi/2
+ s2 = 1
+</pre>
+One common convention for atomic (torsion,bend,stretch) uses the default
+spherical coordinate system but the final stretch is along the -z direction.
+For that, take all defaults but set s2=-1. */
+
+template<bool noX_MB, bool noR_PF>
+class RBNodeSphericalCoords : public RigidBodyNodeSpec<3, false, noX_MB, noR_PF> {
+public:
+typedef typename RigidBodyNodeSpec<3, false, noX_MB, noR_PF>::HType HType;
+virtual const char* type() { return "spherical coords"; }
+
+RBNodeSphericalCoords(const MassProperties& mProps_B,
+ const Transform& X_PF,
+ const Transform& X_BM,
+ Real azimuthOffset, // radians
+ bool azimuthNegated,
+ Real zenithOffset,
+ bool zenithNegated,
+ CoordinateAxis translationAxis, // only x or z
+ bool translationNegated,
+ bool isReversed,
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot)
+: RigidBodyNodeSpec<3, false, noX_MB, noR_PF>(mProps_B,X_PF,X_BM,nextUSlot,nextUSqSlot,nextQSlot,
+ RigidBodyNode::QDotIsAlwaysTheSameAsU, RigidBodyNode::QuaternionIsNeverUsed,
+ isReversed),
+ az0(azimuthOffset), ze0(zenithOffset), axisT(translationAxis),
+ signAz(Real(azimuthNegated ? -1 : 1)), signZe(Real(zenithNegated ? -1 : 1)),
+ signT(Real(translationNegated ? -1 : 1))
+{
+ SimTK_ASSERT_ALWAYS( translationAxis != YAxis,
+ "RBNodeSphericalCoords: translation axis must be x or z; not y.");
+ this->updateSlots(nextUSlot,nextUSqSlot,nextQSlot);
+}
+
+void setQToFitRotationImpl(const SBStateDigest& sbs, const Rotation& R_FM,
+ Vector& q) const {
+ // The only rotations this joint can handle are about Mz and My.
+ // TODO: isn't there a better way to come up with "the rotation around z&y
+ // that best approximates a rotation R"? Here we're just hoping that the
+ // supplied rotation matrix can be decomposed into (z,y) rotations.
+ const Vec2 angles = R_FM.convertTwoAxesRotationToTwoAngles
+ (BodyRotationSequence, ZAxis, YAxis);
+ this->toQ(q)[0] = signAz * (angles[0] - az0);
+ this->toQ(q)[1] = signZe * (angles[1] - ze0);
+}
+
+void setQToFitTranslationImpl(const SBStateDigest& sbs, const Vec3& p_FM,
+ Vector& q) const {
+ // We're not going to attempt any rotations to fit the translation. We'll
+ // find out which way the translation axis is currently pointing and move
+ // along it.
+ const Rotation R_FM = calcR_FM(q);
+ // note: rows of R_FM are columns of R_MF.
+ this->toQ(q)[2] = signT * dot(p_FM, R_FM[axisT]);
+}
+
+// We can only express angular velocity that can be produced with our
+// generalized speeds which are Fz(=Mz) and My rotation rates. So we'll take
+// the supplied angular velocity expressed in F, project it on Fz and use that
+// as the first generalized speed. Then take whatever angular velocity is
+// unaccounted for, express it in M, and project onto My and use that as the
+// second generalized speed.
+void setUToFitAngularVelocityImpl(const SBStateDigest& sbs, const Vector& q,
+ const Vec3& w_FM, Vector& u) const {
+ const Rotation R_FM = calcR_FM(q);
+ this->toU(u)[0] = w_FM[2];
+ // Calculate the remaining angular velocity w-wz and re-express in M.
+ const Vec3 wxy_FM_M = ~R_FM*Vec3(w_FM[0],w_FM[1],0);
+ this->toU(u)[1] = wxy_FM_M[1]; // can only deal with the y angular velocity now
+}
+
+// Although we could try to use angular velocity to affect linear velocity,
+// we're going to restrain ourselves here and only change the translational
+// velocity along the final axis.
+void setUToFitLinearVelocityImpl
+ (const SBStateDigest& sbs, const Vector& q, const Vec3& v_FM, Vector& u) const
+{
+ const Rotation R_FM = calcR_FM(q);
+ this->toU(u)[2] = dot(v_FM, R_FM[axisT]); // i.e., v_FM*Mx_F or v_FM*Mz_F.
+}
+
+enum {PoolSize=4}; // number of Reals
+enum {CosQ=0, SinQ=2};
+// We want space for cos(q01) and sin(q01).
+int calcQPoolSize(const SBModelVars&) const
+{ return PoolSize; }
+
+// Precalculate sines and cosines. Note: these are sines and cosines of q,
+// not az and el.
+// az = s*q + az0, so
+// sin(az) = sin(s*q + az0)
+// = sin(s*q)cos(az0) + cos(s*q)sin(az0)
+// = s*sin(q)cos(az0) + cos(q)sin(az0)
+// cos(az) = cos(s*q + az0)
+// = cos(s*q)cos(az0) - sin(s*q)sin(az0)
+// = cos(q)cos(az0) - s*sin(q)sin(az0)
+// and similarly for the zenith angle. We can precalculate sine and cosine of
+// the offsets so we need only 4 flops to get sin or cos of the actual angles
+// given sin and cos of the generalized coordinates.
+// Cost here is about 100 flops.
+void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* qCache, int nQCache,
+ Real* qErr, int nQErr) const
+{
+ assert(q && nq==3 && qCache && nQCache==PoolSize && nQErr==0);
+ Vec2::updAs(&qCache[CosQ]) = Vec2(std::cos(q[0]),std::cos(q[1]));
+ Vec2::updAs(&qCache[SinQ]) = Vec2(std::sin(q[0]),std::sin(q[1]));
+}
+
+// TODO: should be using sin/cos cache but isn't.
+void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* qCache, int nQCache,
+ Transform& X_FM) const
+{
+ assert(q && nq==3 && qCache && nQCache==PoolSize);
+ // Calculate azimuth and zenith angles from the first two qs.
+ const Vec2 azZe = calcAzZe(q[0], q[1]);
+
+ // Calculate R_FM from q's. This is always a body fixed Z-Y (3-2)
+ // sequence although the angles may be negated and shifted from the
+ // generalized coordinates.
+ // TODO: use qCache for speed.
+ X_FM.updR() = Rotation( BodyRotationSequence,
+ azZe[0], ZAxis, azZe[1], YAxis );
+
+ const Real t = signT * q[2];
+ X_FM.updP() = t * X_FM.R()(axisT); // i.e., t*Mx or t*Mz, expressed in F
+}
+
+// The generalized speeds for this joint are the time derivatives of the
+// body-fixed 3-2 rotation sequence defining the orientation, and the
+// translation velocity. That is, the first speed is just a rotation rate about
+// Fx. The second is a rotation rate about the current My, so we have to
+// transform it into F to make H_FM uniformly expressed in F. The third axis is
+// either Mx or My and is likewise transformed.
+// 18 flops normally; 36 if reversed.
+void calcAcrossJointVelocityJacobian(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+{
+ // "upd" because we're realizing positions now
+ const SBTreePositionCache& pc = sbs.updTreePositionCache();
+ const Transform X_F0M0 = this->findX_F0M0(pc); // 18 flops if reversed
+
+ // Dropping the 0's here.
+ const Rotation& R_FM = X_F0M0.R();
+ const Vec3& p_FM = X_F0M0.p();
+
+ const Vec3 sFz = Vec3(0,0,signAz); // signed Fz for 1st rotation
+ const Vec3 sMy = signZe*R_FM.y(); // signed My for 2nd rotation (3 flops)
+ const Vec3 sMt = signT*R_FM(axisT); // signed Mz or Mx for trans. (3 flops)
+
+ // We need sFz % p_FM but sFz is the z or -z axis so this is a very
+ // cheap calculation.
+ const Vec3 sFzXp_FM = Vec3( -sFz[2]*p_FM[1], sFz[2]*p_FM[0], 0); // 3 flops
+
+ H_FM(0) = SpatialVec( sFz , sFzXp_FM );
+ H_FM(1) = SpatialVec( sMy , sMy % p_FM ); // 9 flops
+ H_FM(2) = SpatialVec( Vec3(0) , sMt );
+}
+
+// Since the last two rows of H_FM above are not constant in F, their time
+// derivatives are non zero. Here we use the fact that for a vector r_B_A
+// fixed in a moving frame B but expressed in another frame A, its time
+// derivative in A is the angular velocity of B in A crossed with the vector,
+// i.e., d_A/dt r_B_A = w_AB % r_B_A. 48 flops normally; 111 if reversed.
+void calcAcrossJointVelocityJacobianDot(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+{
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ // "upd" because we're realizing velocities now
+ const SBTreeVelocityCache& vc = sbs.updTreeVelocityCache();
+
+ const Transform X_F0M0 = this->findX_F0M0(pc); // 18 flops if reversed
+ const SpatialVec V_F0M0 = this->findV_F0M0(pc,vc); // 45 flops if reversed
+
+ // Dropping the 0's here.
+ const Rotation& R_FM = X_F0M0.R();
+ const Vec3& p_FM = X_F0M0.p();
+
+ const Vec3 sFz = Vec3(0,0,signAz); // signed Fz for 1st rotation
+ const Vec3 sMy = signZe*R_FM.y(); // signed My for 2nd rotation (3 flops)
+ const Vec3 sMt = signT*R_FM(axisT); // signed Mz or Mx for trans. (3 flops)
+
+ const Vec3 w_FM = V_F0M0[0]; // angular velocity of M in F
+ const Vec3& v_FM = V_F0M0[1]; // linear velocity of OM in F
+
+ const Vec3 dsMy = w_FM % sMy; // time derivative of sMy (9 flops)
+ const Vec3 dsMt = w_FM % sMt; // time derivative of sMz or sMx (9 flops)
+
+ // We need d/dt (sFz % p_FM) = (sFz % v_FM). But since sFz is the
+ // z or -z axis this is a very cheap calculation.
+ const Vec3 sFzXv_FM = Vec3( -sFz[2]*v_FM[1], sFz[2]*v_FM[0], 0); // 3 flops
+
+ HDot_FM(0) = SpatialVec( Vec3(0) , sFzXv_FM );
+ HDot_FM(1) = SpatialVec( dsMy , dsMy % p_FM + sMy % v_FM ); // 21 flops
+ HDot_FM(2) = SpatialVec( Vec3(0) , dsMt );
+}
+
+private:
+const Real az0, ze0; // angle offsets
+const CoordinateAxis axisT; // translation axis (X or Z)
+const Real signAz, signZe, signT; // 1 or -1
+
+// Calculate azimuth and zenith angles from the first two qs.
+Vec2 calcAzZe(Real q0, Real q1) const {
+ const Real az = signAz * q0 + az0;
+ const Real ze = signZe * q1 + ze0;
+ return Vec2(az,ze);
+}
+
+// Calculate R_FM from q's. This is always a body fixed 3-2 sequence
+// although the angles may be negated and shifted from the generalized
+// coordinates.
+Rotation calcR_FM(const Vector& q) const {
+ const Vec2 azZe = calcAzZe(this->fromQ(q)[0], this->fromQ(q)[1]);
+ return Rotation( BodyRotationSequence, azZe[0], ZAxis, azZe[1], YAxis );
+}
+
+};
+
+
+#endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_SPHERICALCOORDS_H_
+
diff --git a/Simbody/src/RigidBodyNodeSpec_Translation.h b/Simbody/src/RigidBodyNodeSpec_Translation.h
new file mode 100644
index 0000000..6bc32e4
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec_Translation.h
@@ -0,0 +1,157 @@
+#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_TRANSLATION_H_
+#define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_TRANSLATION_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Derived from IVM code written by Charles Schwieters *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Define the RigidBodyNode that implements a Translation mobilizer, also
+ * known as a Cartesian joint.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "RigidBodyNodeSpec.h"
+
+ // TRANSLATION (CARTESIAN) //
+
+// Translate (Cartesian) joint. This provides three degrees of translational
+// freedom which is suitable (e.g.) for connecting a free atom to ground. The
+// Cartesian directions are the axes of the parent body's F frame, with M=F
+// when all 3 coords are 0, and the orientation of M in F is 0 (identity)
+// forever.
+template<bool noX_MB, bool noR_PF>
+class RBNodeTranslate : public RigidBodyNodeSpec<3, true, noX_MB, noR_PF> {
+public:
+typedef typename RigidBodyNodeSpec<3, true, noX_MB, noR_PF>::HType HType;
+virtual const char* type() { return "translate"; }
+
+RBNodeTranslate(const MassProperties& mProps_B,
+ const Transform& X_PF,
+ const Transform& X_BM,
+ bool isReversed,
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot)
+: RigidBodyNodeSpec<3, true, noX_MB, noR_PF>(mProps_B,X_PF,X_BM,nextUSlot,nextUSqSlot,nextQSlot,
+ RigidBodyNode::QDotIsAlwaysTheSameAsU, RigidBodyNode::QuaternionIsNeverUsed,
+ isReversed)
+{
+ this->updateSlots(nextUSlot,nextUSqSlot,nextQSlot);
+}
+
+ // Implementations of virtual methods.
+
+void setQToFitRotationImpl(const SBStateDigest& sbs, const Rotation& R_FM,
+ Vector& q) const {
+ // the only rotation this mobilizer can represent is identity
+}
+void setQToFitTranslationImpl(const SBStateDigest& sbs, const Vec3& p_FM,
+ Vector& q) const {
+ // here's what this joint is really good at!
+ this->toQ(q) = p_FM;
+}
+
+void setUToFitAngularVelocityImpl(const SBStateDigest& sbs, const Vector&,
+ const Vec3& w_FM, Vector& u) const {
+ // The only angular velocity this can represent is zero.
+}
+void setUToFitLinearVelocityImpl
+ (const SBStateDigest& sbs, const Vector&, const Vec3& v_FM, Vector& u) const
+{
+ // linear velocity is in a Cartesian joint's sweet spot
+ this->toU(u) = v_FM;
+}
+
+// A translation joint doesn't need to cache any q calculations.
+int calcQPoolSize(const SBModelVars&) const
+{ return 0; }
+
+// Nothing to precalculate.
+void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* qCache, int nQCache,
+ Real* qErr, int nQErr) const
+{
+ assert(q && nq==3 && nQCache==0 && nQErr==0);
+}
+
+// This is free.
+void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* qCache, int nQCache,
+ Transform& X_FM) const
+{
+ assert(q && nq==3 && nQCache==0);
+ // Translation vector q is expressed in F (and M since they have same
+ // orientation). A translation (Cartesian) joint can't change orientation.
+ X_FM = Transform(Rotation(), Vec3::getAs(&q[0]));
+}
+
+// Generalized speeds together are the velocity of M's origin in the F frame,
+// expressed in F. So individually they produce velocity along F's x,y,z
+// axes respectively.
+void calcAcrossJointVelocityJacobian(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+{
+ H_FM(0) = SpatialVec( Vec3(0), Vec3(1,0,0) );
+ H_FM(1) = SpatialVec( Vec3(0), Vec3(0,1,0) );
+ H_FM(2) = SpatialVec( Vec3(0), Vec3(0,0,1) );
+}
+
+// Since the Jacobian above is constant in F, its time derivative is zero.
+void calcAcrossJointVelocityJacobianDot(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+{
+ HDot_FM(0) = SpatialVec( Vec3(0), Vec3(0) );
+ HDot_FM(1) = SpatialVec( Vec3(0), Vec3(0) );
+ HDot_FM(2) = SpatialVec( Vec3(0), Vec3(0) );
+}
+
+// Override the computation of reverse-H for this simple mobilizer.
+void calcReverseMobilizerH_FM(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+{
+ H_FM(0) = SpatialVec( Vec3(0), Vec3(-1, 0, 0) );
+ H_FM(1) = SpatialVec( Vec3(0), Vec3( 0,-1, 0) );
+ H_FM(2) = SpatialVec( Vec3(0), Vec3( 0, 0,-1) );
+}
+
+// Override the computation of reverse-HDot for this simple mobilizer.
+void calcReverseMobilizerHDot_FM(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+{
+ HDot_FM(0) = SpatialVec( Vec3(0), Vec3(0) );
+ HDot_FM(1) = SpatialVec( Vec3(0), Vec3(0) );
+ HDot_FM(2) = SpatialVec( Vec3(0), Vec3(0) );
+}
+
+};
+
+
+#endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_TRANSLATION_H_
+
diff --git a/Simbody/src/RigidBodyNodeSpec_Universal.h b/Simbody/src/RigidBodyNodeSpec_Universal.h
new file mode 100644
index 0000000..dcdeea1
--- /dev/null
+++ b/Simbody/src/RigidBodyNodeSpec_Universal.h
@@ -0,0 +1,197 @@
+#ifndef SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_UNIVERSAL_H_
+#define SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_UNIVERSAL_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Derived from IVM code written by Charles Schwieters *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Define the RigidBodyNode that implements a Universal mobilizer, also known
+ * as a UJoint or Hooke's joint.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "RigidBodyNodeSpec.h"
+
+ // UNIVERSAL (U-JOINT, HOOKE'S JOINT) //
+
+// This is a Universal Joint (U-Joint), also known as a Hooke's joint. This is
+// identical to the joint that connects pieces of a driveshaft together under
+// a car. Physically, you can think of this as a parent body P, hinged to a
+// massless cross-shaped coupler, which is then hinged to the child body B. The
+// massless coupler doesn't actually appear in the model. Instead, we use a
+// body-fixed 1-2 Euler rotation sequence for orientation, which has the same
+// effect: starting with frames B and P aligned (when q0=q1=0), rotate frame
+// B about the Px(=Bx) axis by q0; then, rotate frame B further about the new
+// By(!=Py) by q1. For generalized speeds u we use the Euler angle derivatives
+// qdot, which are *not* the same as angular velocity components because u0 is
+// a rotation rate around Px(!=Bx any more) while u1 is a rotation rate
+// about By.
+//
+// To summarize,
+// q's: a two-angle body-fixed rotation sequence about x, then new y
+// u's: time derivatives of the q's
+//
+// Note that the U-Joint degrees of freedom relating the parent's F frame to
+// the child's M frame are about x and y, with the "long" axis of the
+// driveshaft along z.
+template<bool noX_MB, bool noR_PF>
+class RBNodeUJoint : public RigidBodyNodeSpec<2, false, noX_MB, noR_PF> {
+public:
+typedef typename RigidBodyNodeSpec<2, false, noX_MB, noR_PF>::HType HType;
+virtual const char* type() { return "ujoint"; }
+
+RBNodeUJoint(const MassProperties& mProps_B,
+ const Transform& X_PF,
+ const Transform& X_BM,
+ bool isReversed,
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot)
+: RigidBodyNodeSpec<2, false, noX_MB, noR_PF>(mProps_B,X_PF,X_BM,nextUSlot,nextUSqSlot,nextQSlot,
+ RigidBodyNode::QDotIsAlwaysTheSameAsU, RigidBodyNode::QuaternionIsNeverUsed,
+ isReversed)
+{
+ this->updateSlots(nextUSlot,nextUSqSlot,nextQSlot);
+}
+
+void setQToFitRotationImpl(const SBStateDigest& sbs, const Rotation& R_FM,
+ Vector& q) const {
+ // The only rotations this joint can handle are about Mx and My.
+ // TODO: isn't there a better way to come up with "the rotation around x&y
+ // that best approximates a rotation R"? Here we're just hoping that the
+ // supplied rotation matrix can be decomposed into (x,y) rotations.
+ const Vec2 angles12 = R_FM.convertRotationToBodyFixedXY();
+ this->toQ(q) = angles12;
+}
+
+void setQToFitTranslationImpl(const SBStateDigest& sbs, const Vec3& p_FM,
+ Vector& q) const {
+ // M and F frame origins are always coincident for this mobilizer so there
+ // is no way to create a translation by rotating. So the only translation
+ // we can represent is 0.
+}
+
+// We can only express angular velocity that can be produced with our
+// generalized speeds which are Fx and My rotations rates. So we'll take the
+// supplied angular velocity expressed in F, project it on Fx and use that as
+// the first generalized speed. Then take whatever angular velocity is
+// unaccounted for, express it in M, and project onto My and use that as the
+// second generalized speed.
+void setUToFitAngularVelocityImpl(const SBStateDigest& sbs, const Vector& q,
+ const Vec3& w_FM, Vector& u) const {
+ const Rotation R_FM =
+ Rotation(BodyRotationSequence, this->fromQ(q)[0], XAxis, this->fromQ(q)[1], YAxis); // body fixed 1-2 sequence
+ const Vec3 wyz_FM_M = ~R_FM*Vec3(0,w_FM[1],w_FM[2]);
+ this->toU(u) = Vec2(w_FM[0], wyz_FM_M[1]);
+}
+
+void setUToFitLinearVelocityImpl
+ (const SBStateDigest& sbs, const Vector&, const Vec3& v_FM, Vector& u) const
+{
+ // M and F frame origins are always coincident for this mobilizer so there
+ // is no way to create a linear velocity by rotating. So the only linear
+ // velocity we can represent is 0.
+}
+
+enum {PoolSize=4}; // number of Reals
+enum {CosQ=0, SinQ=2};
+// We want space for cos(q01) and sin(q01).
+int calcQPoolSize(const SBModelVars&) const
+{ return PoolSize; }
+
+// Precalculation of sin/cos costs around 100 flops.
+void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* qCache, int nQCache,
+ Real* qErr, int nQErr) const
+{
+ assert(q && nq==2 && qCache && nQCache==PoolSize && nQErr==0);
+ Vec2::updAs(&qCache[CosQ]) = Vec2(std::cos(q[0]),std::cos(q[1]));
+ Vec2::updAs(&qCache[SinQ]) = Vec2(std::sin(q[0]),std::sin(q[1]));
+}
+
+// TODO: should use precalculated sin/cos but doesn't.
+void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* qCache, int nQCache,
+ Transform& X_FM) const
+{
+ assert(q && nq==2 && qCache && nQCache==PoolSize);
+ // TODO: use qCache
+ // We're only updating the orientation here because a U-joint
+ // can't translate. This is a body fixed X-Y sequence.
+ X_FM.updR() = Rotation(BodyRotationSequence, q[0], XAxis, q[1], YAxis);
+ X_FM.updP() = 0;
+}
+
+// The generalized speeds for this 2-dof rotational joint are the time
+// derivatives of the body-fixed 1-2 rotation sequence defining the
+// orientation. That is, the first speed is just a rotation rate about Fx. The
+// second is a rotation rate about the current My, so we have to transform it
+// into F to make H_FM uniformly expressed in F.
+void calcAcrossJointVelocityJacobian(
+ const SBStateDigest& sbs,
+ HType& H_FM) const
+{
+ // "upd" because we're realizing positions now
+ const SBTreePositionCache& pc = sbs.updTreePositionCache();
+ const Transform X_F0M0 = this->findX_F0M0(pc);
+
+ // Dropping the 0's here.
+ const Rotation& R_FM = X_F0M0.R();
+
+ H_FM(0) = SpatialVec( Vec3(1,0,0) , Vec3(0) );
+ H_FM(1) = SpatialVec( R_FM.y() , Vec3(0) );
+}
+
+// Since the second row of the Jacobian H_FM above is not constant in F,
+// its time derivative is non zero. Here we use the fact that for
+// a vector r_B_A fixed in a moving frame B but expressed in another frame A,
+// its time derivative in A is the angular velocity of B in A crossed with
+// the vector, i.e., d_A/dt r_B_A = w_AB % r_B_A.
+void calcAcrossJointVelocityJacobianDot(
+ const SBStateDigest& sbs,
+ HType& HDot_FM) const
+{
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ // "upd" because we're realizing velocities now
+ const SBTreeVelocityCache& vc = sbs.updTreeVelocityCache();
+
+ const Transform X_F0M0 = this->findX_F0M0(pc);
+
+ // Dropping the 0's here.
+ const Rotation& R_FM = X_F0M0.R();
+ const Vec3 w_FM = this->find_w_F0M0(pc,vc); // angular velocity of M in F
+
+ HDot_FM(0) = SpatialVec( Vec3(0) , Vec3(0) );
+ HDot_FM(1) = SpatialVec( w_FM % R_FM.y() , Vec3(0) );
+}
+
+};
+
+
+
+
+#endif // SimTK_SIMBODY_RIGID_BODY_NODE_SPEC_UNIVERSAL_H_
+
diff --git a/Simbody/src/RigidBodyNode_LoneParticle.cpp b/Simbody/src/RigidBodyNode_LoneParticle.cpp
new file mode 100644
index 0000000..1f92384
--- /dev/null
+++ b/Simbody/src/RigidBodyNode_LoneParticle.cpp
@@ -0,0 +1,489 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "MobilizedBodyImpl.h"
+#include "RigidBodyNodeSpec_Translation.h"
+
+/**
+ * This is a specialized class used for MobilizedBody::Translation objects that satisfy
+ * all of the following requirements:
+ *
+ * <ul>
+ * <li>The body has no children.</li>
+ * <li>The body's parent is ground.</li>
+ * <li>The inboard and outboard transforms are both identities.</li>
+ * <li>The body is not reversed.</li>
+ * </ul>
+ *
+ * These assumptions allow lots of routines to be implemented in simpler, faster ways.
+ */
+class RBNodeLoneParticle : public RigidBodyNode {
+public:
+RBNodeLoneParticle(const MassProperties& mProps_B,
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot)
+ : RigidBodyNode(mProps_B, Vec3(0), Vec3(0), QDotIsAlwaysTheSameAsU, QuaternionIsNeverUsed, false) {
+ uIndex = nextUSlot;
+ uSqIndex = nextUSqSlot;
+ qIndex = nextQSlot;
+ nextUSlot += 3;
+ nextUSqSlot += 9;
+ nextQSlot += 3;
+}
+
+const char* type() const {return "loneparticle";}
+int getDOF() const {return 3;}
+int getMaxNQ() const {return 3;}
+
+int getNQInUse(const SBModelVars&) const {return 3;}
+int getNUInUse(const SBModelVars&) const {return 3;}
+
+bool isUsingQuaternion(const SBStateDigest&, MobilizerQIndex& startOfQuaternion) const {
+ return false;
+}
+
+bool isUsingAngles(const SBStateDigest&, MobilizerQIndex& startOfAngles, int& nAngles) const {
+ return false;
+}
+
+int calcQPoolSize(const SBModelVars&) const {
+ return 0;
+}
+
+void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* qCache, int nQCache,
+ Real* qErr, int nQErr) const {
+ assert(q && nq==3 && nQCache==0 && nQErr==0);
+}
+
+void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* qCache, int nQCache,
+ Transform& X_FM) const {
+ assert(q && nq==3 && nQCache==0);
+ X_FM = Transform(Rotation(), Vec3::getAs(&q[0]));
+}
+
+void calcQDot(const SBStateDigest&, const Real* u, Real* qdot) const {
+ Vec3::updAs(qdot) = Vec3::getAs(u);
+}
+void calcQDotDot(const SBStateDigest&, const Real* udot, Real* qdotdot) const {
+ Vec3::updAs(qdotdot) = Vec3::getAs(udot);
+}
+
+void multiplyByN(const SBStateDigest&, bool matrixOnRight, const Real* in, Real* out) const {
+ Vec3::updAs(out) = Vec3::getAs(in);
+}
+void multiplyByNInv(const SBStateDigest&, bool matrixOnRight, const Real* in, Real* out) const {
+ Vec3::updAs(out) = Vec3::getAs(in);
+}
+void multiplyByNDot(const SBStateDigest&, bool matrixOnRight, const Real* in, Real* out) const {
+ Vec3::updAs(out) = 0;
+}
+
+bool enforceQuaternionConstraints(
+ const SBStateDigest& sbs,
+ Vector& q,
+ Vector& qErrest) const {
+ return false;
+}
+
+void convertToEulerAngles(const Vector& inputQ, Vector& outputQ) const {
+ Vec3::updAs(&outputQ[qIndex]) = Vec3::getAs(&inputQ[qIndex]);
+}
+
+void convertToQuaternions(const Vector& inputQ, Vector& outputQ) const {
+ Vec3::updAs(&outputQ[qIndex]) = Vec3::getAs(&inputQ[qIndex]);
+}
+
+void setMobilizerDefaultModelValues
+ (const SBTopologyCache&, SBModelVars&) const {}
+
+void setMobilizerDefaultInstanceValues
+ (const SBModelVars&, SBInstanceVars&) const {}
+void setMobilizerDefaultTimeValues
+ (const SBModelVars&, SBTimeVars&) const {}
+void setMobilizerDefaultPositionValues
+ (const SBModelVars&, Vector& q) const {q = 0;}
+void setMobilizerDefaultVelocityValues
+ (const SBModelVars&, Vector& u) const {u = 0;}
+void setMobilizerDefaultDynamicsValues
+ (const SBModelVars&, SBDynamicsVars&) const {}
+void setMobilizerDefaultAccelerationValues
+ (const SBModelVars&, SBAccelerationVars&) const {}
+
+void realizeModel(SBStateDigest& sbs) const {
+}
+
+void realizeInstance(const SBStateDigest& sbs) const {
+ // Initialize cache entries that will never be changed at later stages.
+
+ SBTreePositionCache& pc = sbs.updTreePositionCache();
+ SBTreeVelocityCache& vc = sbs.updTreeVelocityCache();
+ SBDynamicsCache& dc = sbs.updDynamicsCache();
+ SBTreeAccelerationCache& ac = sbs.updTreeAccelerationCache();
+ Transform& X_FM = toB(pc.bodyJointInParentJointFrame);
+ X_FM.updR().setRotationToIdentityMatrix();
+ updV_FM(vc)[0] = Vec3(0);
+ updV_PB_G(vc)[0] = Vec3(0);
+ updVD_PB_G(vc)[0] = Vec3(0);
+ updV_GB(vc)[0] = Vec3(0);
+ updGyroscopicForce(vc) = SpatialVec(Vec3(0), Vec3(0));
+ updMobilizerCoriolisAcceleration(vc) = SpatialVec(Vec3(0), Vec3(0));
+ updTotalCoriolisAcceleration(vc) = SpatialVec(Vec3(0), Vec3(0));
+ updMobilizerCentrifugalForces(dc) = SpatialVec(Vec3(0), Vec3(0));
+ updTotalCentrifugalForces(dc) = SpatialVec(Vec3(0), Vec3(0));
+ updY(dc) = SpatialMat(Mat33(0), Mat33(0), Mat33(0), Mat33(1/getMass()));
+ updA_GB(ac)[0] = Vec3(0);
+}
+
+void realizePosition(const SBStateDigest& sbs) const {
+ SBTreePositionCache& pc = sbs.updTreePositionCache();
+ Transform& X_FM = toB(pc.bodyJointInParentJointFrame);
+ const Vec3& q = Vec3::getAs(&sbs.getQ()[qIndex]);
+ X_FM.updP() = q;
+ updX_PB(pc) = X_FM;
+ updX_GB(pc) = X_FM;
+ updPhi(pc) = PhiMatrix(q);
+ updCOM_G(pc) = q + getCOM_B(); // 3 flops
+ updMk_G(pc) = SpatialInertia(getMass(), getCOM_B(), getUnitInertia_OB_B());
+}
+
+void realizeVelocity(const SBStateDigest& sbs) const {
+ SBTreeVelocityCache& vc = sbs.updTreeVelocityCache();
+ const Vector& allU = sbs.getU();
+ const Vec3& u = Vec3::getAs(&allU[uIndex]);
+
+ Vec3::updAs(&sbs.updQDot()[qIndex]) = Vec3::getAs(&allU[uIndex]);
+ updV_FM(vc)[1] = u;
+ updV_PB_G(vc)[1] = u;
+ updV_GB(vc)[1] = u;
+}
+
+void realizeDynamics(const SBArticulatedBodyInertiaCache& abc, const SBStateDigest& sbs) const {
+}
+
+// There is no realizeAcceleration().
+
+void realizeReport(const SBStateDigest& sbs) const {
+}
+
+void realizeArticulatedBodyInertiasInward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ SBArticulatedBodyInertiaCache& abc) const {
+ ArticulatedInertia& P = updP(abc);
+ ArticulatedInertia& PPlus = updPPlus(abc);
+
+ PPlus = P = ArticulatedInertia(getMk_G(pc));
+}
+
+void realizeYOutward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache& abc,
+ SBDynamicsCache& dc) const {
+}
+
+void calcCompositeBodyInertiasInward
+ (const SBTreePositionCache& pc,
+ Array_<SpatialInertia,MobilizedBodyIndex>& R) const {
+ toB(R) = getMk_G(pc);
+}
+
+void multiplyBySystemJacobian(
+ const SBTreePositionCache& pc,
+ const Real* v,
+ SpatialVec* Jv) const {
+ const Vec3& in = Vec3::getAs(&v[uIndex]);
+ SpatialVec& out = Jv[nodeNum];
+ out = ~getPhi(pc) * Jv[parent->getNodeNum()];
+ out[1] += in;
+}
+
+void multiplyBySystemJacobianTranspose(
+ const SBTreePositionCache& pc,
+ SpatialVec* zTmp,
+ const SpatialVec* X,
+ Real* JtX) const {
+ const SpatialVec& in = X[getNodeNum()];
+ Vec3& out = Vec3::updAs(&JtX[getUIndex()]);
+ SpatialVec& z = zTmp[getNodeNum()];
+ z = in;
+ out = z[1];
+}
+
+void calcEquivalentJointForces(
+ const SBTreePositionCache& pc,
+ const SBDynamicsCache& dc,
+ const SpatialVec* bodyForces,
+ SpatialVec* allZ,
+ Real* jointForces) const {
+ const SpatialVec& myBodyForce = bodyForces[nodeNum];
+ SpatialVec& z = allZ[nodeNum];
+ Vec3& eps = Vec3::updAs(&jointForces[uIndex]);
+ z = myBodyForce;
+ eps = z[1];
+}
+
+void calcUDotPass1Inward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache& abc,
+ const SBDynamicsCache& dc,
+ const Real* jointForces,
+ const SpatialVec* bodyForces,
+ const Real* allUDot,
+ SpatialVec* allZ,
+ SpatialVec* allZPlus,
+ Real* allEpsilon) const
+{
+ const Vec3& f = Vec3::getAs(&jointForces[uIndex]);
+ const SpatialVec& F = bodyForces[nodeNum];
+ SpatialVec& z = allZ[nodeNum];
+ SpatialVec& zPlus = allZPlus[nodeNum];
+ Vec3& eps = Vec3::updAs(&allEpsilon[uIndex]);
+
+ const bool isPrescribed = isUDotKnown(ic);
+
+ z = -F;
+
+ if (isPrescribed && !isUDotKnownToBeZero(ic)) {
+ const Vec3& udot = Vec3::getAs(&allUDot[uIndex]);
+ z[1] += getMass()*udot; // == P*H*udot
+ }
+
+ // Lone particle has no children.
+
+ eps = f - z[1];
+
+ zPlus = z;
+ if (!isPrescribed)
+ zPlus[1] += eps;
+}
+
+void calcUDotPass2Outward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache& abc,
+ const SBTreeVelocityCache& vc,
+ const SBDynamicsCache& dc,
+ const Real* allEpsilon,
+ SpatialVec* allA_GB,
+ Real* allUDot,
+ Real* allTau) const {
+ const Vec3& eps = Vec3::getAs(&allEpsilon[uIndex]);
+ SpatialVec& A_GB = allA_GB[nodeNum];
+ Vec3& udot = Vec3::updAs(&allUDot[uIndex]);
+
+ const bool isPrescribed = isUDotKnown(ic);
+
+ if (isPrescribed) {
+ const PresForcePoolIndex tauIx =
+ ic.getMobodInstanceInfo(nodeNum).firstPresForce;
+ assert(tauIx.isValid());
+ Vec3& tau = Vec3::updAs(&allTau[tauIx]);
+ tau = eps; // our sign convention
+ } else
+ udot = eps/getMass();
+
+ A_GB = SpatialVec(Vec3(0), udot);
+}
+
+// Note that we're not setting z temporaries here; you can't count on that as
+// a side effect the M^-1*f kernel.
+void multiplyByMInvPass1Inward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache& abc,
+ const Real* jointForces,
+ SpatialVec* allZ,
+ SpatialVec* allZPlus,
+ Real* allEpsilon) const OVERRIDE_11
+{
+ if (isUDotKnown(ic)) // prescribed
+ return;
+
+ // We promised not to look at f if it is part of f_p (prescribed).
+ const Vec3& f = Vec3::getAs(&jointForces[uIndex]);
+ Vec3& eps = Vec3::updAs(&allEpsilon[uIndex]);
+
+ eps = f;
+}
+
+// Outward pass must set A_GB properly so it can be propagated
+// to children.
+void multiplyByMInvPass2Outward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache& abc,
+ const Real* allEpsilon,
+ SpatialVec* allA_GB,
+ Real* allUDot) const OVERRIDE_11
+{
+ const bool isPrescribed = isUDotKnown(ic);
+ const Vec3& eps = Vec3::getAs(&allEpsilon[uIndex]);
+ SpatialVec& A_GB = allA_GB[nodeNum];
+ Vec3& udot = Vec3::updAs(&allUDot[uIndex]);
+
+ if (isPrescribed) {
+ udot = 0;
+ A_GB = SpatialVec(Vec3(0), Vec3(0));
+ } else {
+ udot = eps/getMass();
+ A_GB = SpatialVec(Vec3(0), udot);
+ }
+}
+
+// Also serves as pass 1 for inverse dynamics.
+void calcBodyAccelerationsFromUdotOutward(
+ const SBTreePositionCache& pc,
+ const SBTreeVelocityCache& vc,
+ const Real* allUDot,
+ SpatialVec* allA_GB) const {
+ const Vec3& udot = Vec3::getAs(&allUDot[uIndex]);
+ SpatialVec& A_GB = allA_GB[nodeNum];
+ A_GB = SpatialVec(Vec3(0), udot);
+}
+void calcInverseDynamicsPass2Inward(
+ const SBTreePositionCache& pc,
+ const SBTreeVelocityCache& vc,
+ const SpatialVec* allA_GB,
+ const Real* jointForces,
+ const SpatialVec* bodyForces,
+ SpatialVec* allF,
+ Real* allTau) const {
+ const Vec3& myJointForce = Vec3::getAs(&jointForces[uIndex]);
+ const SpatialVec& myBodyForce = bodyForces[nodeNum];
+ const SpatialVec& A_GB = allA_GB[nodeNum];
+ SpatialVec& F = allF[nodeNum];
+ Vec3& tau = Vec3::updAs(&allTau[uIndex]);
+ F = getMk_G(pc)*A_GB - myBodyForce;
+ tau = F[1] - myJointForce;
+}
+
+void multiplyByMPass1Outward(
+ const SBTreePositionCache& pc,
+ const Real* allUDot,
+ SpatialVec* allA_GB) const {
+ const Vec3& udot = Vec3::getAs(&allUDot[uIndex]);
+ SpatialVec& A_GB = allA_GB[nodeNum];
+ A_GB = SpatialVec(Vec3(0), udot);
+}
+void multiplyByMPass2Inward(
+ const SBTreePositionCache& pc,
+ const SpatialVec* allA_GB,
+ SpatialVec* allF,
+ Real* allTau) const {
+ const SpatialVec& A_GB = allA_GB[nodeNum];
+ SpatialVec& F = allF[nodeNum];
+ Vec3& tau = Vec3::updAs(&allTau[uIndex]);
+ F = getMk_G(pc)*A_GB;
+ tau = F[1];
+}
+
+const SpatialVec& getHCol(const SBTreePositionCache& pc, int j) const {
+ Mat<2,3,Vec3> H = Mat<2,3,Vec3>::getAs(&pc.storageForH[2*uIndex]);
+ SpatialVec& col = H(j);
+ col = SpatialVec(Vec3(0), Vec3(0));
+ col[1][j] = 1;
+ return col;
+}
+
+const SpatialVec& getH_FMCol(const SBTreePositionCache& pc, int j) const {
+ Mat<2,3,Vec3> H = Mat<2,3,Vec3>::getAs(&pc.storageForH_FM[2*uIndex]);
+ SpatialVec& col = H(j);
+ col = SpatialVec(Vec3(0), Vec3(0));
+ col[1][j] = 1;
+ return col;
+}
+
+void setQToFitTransformImpl(const SBStateDigest&, const Transform& X_F0M0, Vector& q) const {
+ Vec3::updAs(&q[qIndex]) = X_F0M0.p();
+}
+void setQToFitRotationImpl(const SBStateDigest&, const Rotation& R_F0M0, Vector& q) const {
+}
+void setQToFitTranslationImpl(const SBStateDigest&, const Vec3& p_F0M0, Vector& q) const {
+ Vec3::updAs(&q[qIndex]) = p_F0M0;
+}
+
+void setUToFitVelocityImpl(const SBStateDigest&, const Vector& q, const SpatialVec& V_F0M0, Vector& u) const {
+ Vec3::updAs(&u[uIndex]) = V_F0M0[1];
+}
+void setUToFitAngularVelocityImpl(const SBStateDigest&, const Vector& q, const Vec3& w_F0M0, Vector& u) const {
+}
+void setUToFitLinearVelocityImpl(const SBStateDigest&, const Vector& q, const Vec3& v_F0M0, Vector& u) const {
+ Vec3::updAs(&u[uIndex]) = v_F0M0;
+}
+
+};
+
+RigidBodyNode* MobilizedBody::TranslationImpl::createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const {
+ if (!hasChildren && getMyParentMobilizedBodyIndex() == 0 && !isReversed() &&
+ getDefaultInboardFrame().p() == 0 && getDefaultInboardFrame().R() == Mat33(1) &&
+ getDefaultOutboardFrame().p() == 0 && getDefaultOutboardFrame().R() == Mat33(1)) {
+ // This satisfies all the requirements to use RBNodeLoneParticle.
+
+ return new RBNodeLoneParticle(getDefaultRigidBodyMassProperties(), nextUSlot,nextUSqSlot,nextQSlot);
+ }
+
+ // Use RBNodeTranslate for the general case.
+
+ bool noX_MB = (getDefaultOutboardFrame().p() == 0 && getDefaultOutboardFrame().R() == Mat33(1));
+ bool noR_PF = (getDefaultInboardFrame().R() == Mat33(1));
+ if (noX_MB) {
+ if (noR_PF)
+ return new RBNodeTranslate<true, true> (
+ getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ isReversed(),
+ nextUSlot,nextUSqSlot,nextQSlot);
+ else
+ return new RBNodeTranslate<true, false> (
+ getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ isReversed(),
+ nextUSlot,nextUSqSlot,nextQSlot);
+ }
+ else {
+ if (noR_PF)
+ return new RBNodeTranslate<false, true> (
+ getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ isReversed(),
+ nextUSlot,nextUSqSlot,nextQSlot);
+ else
+ return new RBNodeTranslate<false, false> (
+ getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ isReversed(),
+ nextUSlot,nextUSqSlot,nextQSlot);
+ }
+}
diff --git a/Simbody/src/RigidBodyNode_Weld.cpp b/Simbody/src/RigidBodyNode_Weld.cpp
new file mode 100644
index 0000000..7d1f418
--- /dev/null
+++ b/Simbody/src/RigidBodyNode_Weld.cpp
@@ -0,0 +1,723 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * Charles Schwieters (NIH): wrote the public domain IVM code from which *
+ * this was derived. *
+ * Peter Eastman wrote the Weld joint. *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This file contains the implementations for RigidBodyNodes which have
+ * no degrees of freedom -- Ground and Weld mobilizers. These cannot be
+ * derived from the usual RigidBodyNodeSpec<dof> class because dof==0
+ * is problematic there. Also, these can have very efficient
+ * implementations here since they know they have no dofs.
+ */
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "RigidBodyNode.h"
+#include "MobilizedBodyImpl.h"
+
+
+////////////////////////////////////////////////
+// Define classes derived from RigidBodyNode. //
+////////////////////////////////////////////////
+
+/**
+ * This still-abstract class is the common base for any MobilizedBody which
+ * has no mobilities. Currently that is only the unique Ground body and
+ * MobilizedBody::Weld, but it is conceivable that others could crop up.
+ *
+ * The base class overrides all the virtual methods which deal in q's and u's
+ * so have common, trivial implementations if there are no q's and no u's.
+ */
+class ImmobileRigidBodyNode : public RigidBodyNode {
+public:
+ ImmobileRigidBodyNode(const MassProperties& mProps_B, const Transform& X_PF, const Transform& X_BM,
+ const UIndex& uIx, const USquaredIndex& usqIx, const QIndex& qIx)
+ : RigidBodyNode(mProps_B, X_PF, X_BM, QDotIsAlwaysTheSameAsU, QuaternionIsNeverUsed)
+ {
+ uIndex = uIx;
+ uSqIndex = usqIx;
+ qIndex = qIx;
+ }
+ ~ImmobileRigidBodyNode() {}
+
+ int getDOF() const {return 0;}
+ int getMaxNQ() const {return 0;}
+ int getNUInUse(const SBModelVars&) const {return 0;}
+ int getNQInUse(const SBModelVars&) const {return 0;}
+ bool isUsingQuaternion(const SBStateDigest&, MobilizerQIndex& ix) const
+ { ix.invalidate(); return false; }
+ int getPosPoolSize(const SBStateDigest&) const {return 0;}
+ int getVelPoolSize(const SBStateDigest&) const {return 0;}
+
+ int calcQPoolSize(const SBModelVars&) const {return 0;}
+
+ void performQPrecalculations(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ Real* qCache, int nQCache,
+ Real* qErr, int nQErr) const
+ { assert(nq==0 && nQCache==0 && nQErr==0); }
+
+
+ // An immobile mobilizer holds the mobilized body's M frame coincident
+ // with the parent body's F frame forever.
+ void calcX_FM(const SBStateDigest& sbs,
+ const Real* q, int nq,
+ const Real* qCache, int nQCache,
+ Transform& X_FM) const
+ { assert(nq==0 && nQCache==0);
+ X_FM.setToZero(); }
+
+ void setQToFitTransformImpl
+ (const SBStateDigest& sbs, const Transform& X_FM, Vector& q) const {}
+ void setQToFitRotationImpl
+ (const SBStateDigest& sbs, const Rotation& R_FM, Vector& q) const {}
+ void setQToFitTranslationImpl
+ (const SBStateDigest& sbs, const Vec3& p_FM, Vector& q) const {}
+
+ void setUToFitVelocityImpl
+ (const SBStateDigest& sbs, const Vector& q, const SpatialVec& V_FM, Vector& u) const {}
+ void setUToFitAngularVelocityImpl
+ (const SBStateDigest& sbs, const Vector& q, const Vec3& w_FM, Vector& u) const {}
+ void setUToFitLinearVelocityImpl
+ (const SBStateDigest& sbs, const Vector& q, const Vec3& v_FM, Vector& u) const {}
+
+
+ void multiplyByN(const SBStateDigest&, bool matrixOnRight,
+ const Real* in, Real* out) const {}
+ void multiplyByNInv(const SBStateDigest&, bool matrixOnRight,
+ const Real* in, Real* out) const {}
+ void multiplyByNDot(const SBStateDigest&, bool matrixOnRight,
+ const Real* in, Real* out) const {}
+
+ void calcQDot(const SBStateDigest&, const Real* udot, Real* qdotdot) const {}
+ void calcQDotDot(const SBStateDigest&, const Real* udot, Real* qdotdot) const {}
+
+ bool enforceQuaternionConstraints(
+ const SBStateDigest& sbs,
+ Vector& q,
+ Vector& qErrest) const {return false;}
+
+ void convertToEulerAngles(const Vector& inputQ, Vector& outputQ) const {}
+ void convertToQuaternions(const Vector& inputQ, Vector& outputQ) const {}
+
+ void setVelFromSVel(
+ const SBTreePositionCache& pc,
+ const SBTreeVelocityCache& vc,
+ const SpatialVec& sVel,
+ Vector& u) const {}
+};
+
+/**
+ * This is the distinguished body representing the immobile ground frame. Other bodies may
+ * be fixed to this one, but only this is the actual Ground.
+ */
+class RBGroundBody : public ImmobileRigidBodyNode {
+public:
+ RBGroundBody()
+ : ImmobileRigidBodyNode(MassProperties(Infinity, Vec3(0), Inertia(Infinity)),
+ Transform(), Transform(),
+ UIndex(0), USquaredIndex(0), QIndex(0)) {}
+
+ const char* type() const { return "ground"; }
+
+ // TODO: should ground set the various cache entries here?
+ void realizeModel (SBStateDigest&) const {}
+ void realizeInstance(const SBStateDigest& sbs) const {
+ // Initialize cache entries that will never be changed at later stages.
+
+ SBTreeVelocityCache& vc = sbs.updTreeVelocityCache();
+ SBDynamicsCache& dc = sbs.updDynamicsCache();
+ SBTreeAccelerationCache& ac = sbs.updTreeAccelerationCache();
+ updY(dc) = SpatialMat(Mat33(0));
+ updA_GB(ac) = 0;
+ }
+ void realizePosition(const SBStateDigest&) const {}
+ void realizeVelocity(const SBStateDigest&) const {}
+ void realizeDynamics(const SBArticulatedBodyInertiaCache&, const SBStateDigest&) const {}
+ // There is no realizeAcceleration().
+ void realizeReport (const SBStateDigest&) const {}
+
+ // Ground's "composite" body inertia is still the infinite mass
+ // and inertia it started with; no need to look at the children.
+ // This overrides the base class default implementation.
+ void calcCompositeBodyInertiasInward(
+ const SBTreePositionCache& pc,
+ Array_<SpatialInertia,MobilizedBodyIndex>& R) const
+ { R[GroundIndex] = SpatialInertia(Infinity, Vec3(0), UnitInertia(1)); }
+
+ // Ground's "articulated" body inertia is still the infinite mass and
+ // inertia it started with; no need to look at the children.
+ void realizeArticulatedBodyInertiasInward(
+ const SBInstanceCache&,
+ const SBTreePositionCache&,
+ SBArticulatedBodyInertiaCache& abc) const
+ { ArticulatedInertia& P = updP(abc);
+ P = ArticulatedInertia(SymMat33(Infinity), Mat33(Infinity),
+ SymMat33(0));
+ updPPlus(abc) = P;
+ }
+
+ void realizeYOutward(
+ const SBInstanceCache&,
+ const SBTreePositionCache&,
+ const SBArticulatedBodyInertiaCache&,
+ SBDynamicsCache& dc) const
+ {
+ }
+
+
+ // Treat Ground as though welded to the universe at the ground
+ // origin. The reaction there collects the effects of all the
+ // base bodies and of any forces applied directly to Ground.
+ void calcUDotPass1Inward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache&,
+ const SBDynamicsCache&,
+ const Real* jointForces,
+ const SpatialVec* bodyForces,
+ const Real* allUDot,
+ SpatialVec* allZ,
+ SpatialVec* allZPlus,
+ Real* allEpsilon) const
+ {
+ const SpatialVec& F = bodyForces[0];
+ SpatialVec& z = allZ[0];
+ SpatialVec& zPlus = allZPlus[0];
+
+ z = -F;
+
+ for (unsigned i=0; i<children.size(); ++i) {
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+ const SpatialVec& zPlusChild = allZPlus[children[i]->getNodeNum()];
+ z += phiChild * zPlusChild; // 18 flops
+ }
+
+ zPlus = z;
+ }
+
+ void calcUDotPass2Outward(
+ const SBInstanceCache&,
+ const SBTreePositionCache&,
+ const SBArticulatedBodyInertiaCache&,
+ const SBTreeVelocityCache&,
+ const SBDynamicsCache&,
+ const Real* epsilonTmp,
+ SpatialVec* allA_GB,
+ Real* allUDot,
+ Real* allTau) const
+ {
+ allA_GB[0] = 0;
+ }
+
+ // Ground doesn't contribute to M^-1*f. Inward pass does nothing since
+ // Ground can't be the child of any body.
+ void multiplyByMInvPass1Inward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache&,
+ const Real* f,
+ SpatialVec* allZ,
+ SpatialVec* allZPlus,
+ Real* allEpsilon) const OVERRIDE_11
+ {
+ }
+
+ // Outward pass must make sure A_GB[0] is zero so it can be propagated
+ // outwards properly.
+ void multiplyByMInvPass2Outward(
+ const SBInstanceCache&,
+ const SBTreePositionCache&,
+ const SBArticulatedBodyInertiaCache&,
+ const Real* epsilonTmp,
+ SpatialVec* allA_GB,
+ Real* allUDot) const OVERRIDE_11
+ {
+ allA_GB[0] = 0;
+ }
+
+ // Also serves as pass 1 for inverse dynamics.
+ void calcBodyAccelerationsFromUdotOutward(
+ const SBTreePositionCache& pc,
+ const SBTreeVelocityCache& vc,
+ const Real* allUDot,
+ SpatialVec* allA_GB) const
+ {
+ allA_GB[0] = 0;
+ }
+
+ // Here Ground is the last body processed. Although it has no mobility forces
+ // we can still collect up all the forces from the base bodies to Ground
+ // in case anyone cares.
+ void calcInverseDynamicsPass2Inward(
+ const SBTreePositionCache& pc,
+ const SBTreeVelocityCache& vc,
+ const SpatialVec* allA_GB,
+ const Real* jointForces,
+ const SpatialVec* bodyForces,
+ SpatialVec* allF,
+ Real* allTau) const
+ {
+ allF[0] = -bodyForces[0];
+
+ // Add in forces on base bodies, shifted to Ground.
+ for (unsigned i=0; i<children.size(); ++i) {
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+ const SpatialVec& FChild = allF[children[i]->getNodeNum()];
+ allF[0] += phiChild * FChild;
+ }
+
+ // no taus
+ }
+
+ void multiplyByMPass1Outward(
+ const SBTreePositionCache& pc,
+ const Real* allUDot,
+ SpatialVec* allA_GB) const
+ {
+ allA_GB[0] = 0;
+ }
+
+ void multiplyByMPass2Inward(
+ const SBTreePositionCache& pc,
+ const SpatialVec* allA_GB,
+ SpatialVec* allF,
+ Real* allTau) const
+ {
+ allF[0] = 0;
+
+ // Add in forces on base bodies, shifted to Ground.
+ for (unsigned i=0; i<children.size(); ++i) {
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+ const SpatialVec& FChild = allF[children[i]->getNodeNum()];
+ allF[0] += phiChild * FChild;
+ }
+
+ // no taus
+ }
+
+
+ void multiplyBySystemJacobian(
+ const SBTreePositionCache& pc,
+ const Real* v,
+ SpatialVec* Jv) const
+ {
+ Jv[0] = SpatialVec(Vec3(0));
+ }
+
+ void multiplyBySystemJacobianTranspose(
+ const SBTreePositionCache& pc,
+ SpatialVec* zTmp,
+ const SpatialVec* X,
+ Real* JtX) const
+ {
+ zTmp[0] = X[0];
+ for (unsigned i=0; i<children.size(); ++i) {
+ const SpatialVec& zChild = zTmp[children[i]->getNodeNum()];
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+ zTmp[0] += phiChild * zChild;
+ }
+ // No generalized speeds so no contribution to JtX.
+ }
+
+ void calcEquivalentJointForces(
+ const SBTreePositionCache& pc,
+ const SBDynamicsCache&,
+ const SpatialVec* bodyForces,
+ SpatialVec* allZ,
+ Real* jointForces) const
+ {
+ allZ[0] = bodyForces[0];
+ for (unsigned i=0; i<children.size(); ++i) {
+ const SpatialVec& zChild = allZ[children[i]->getNodeNum()];
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+ allZ[0] += phiChild * zChild;
+ }
+ }
+};
+
+ // WELD //
+
+// This is a "joint" with no degrees of freedom, that simply forces
+// the two reference frames to be identical. A Weld node always has a parent
+// but has no q's and no u's.
+class RBNodeWeld : public ImmobileRigidBodyNode {
+public:
+ RBNodeWeld(const MassProperties& mProps_B, const Transform& X_PF, const Transform& X_BM,
+ const UIndex& uIx, const USquaredIndex& usqIx, const QIndex& qIx)
+ : ImmobileRigidBodyNode(mProps_B, X_PF, X_BM, uIx, usqIx, qIx) {}
+
+ const char* type() { return "weld"; }
+
+ void realizeModel(SBStateDigest& sbs) const {}
+ void realizeInstance(const SBStateDigest& sbs) const {
+ // Initialize cache entries that will never be changed at later stages.
+
+ SBTreeVelocityCache& vc = sbs.updTreeVelocityCache();
+ SBTreeAccelerationCache& ac = sbs.updTreeAccelerationCache();
+ updV_FM(vc) = 0;
+ updV_PB_G(vc) = 0;
+ updVD_PB_G(vc) = 0;
+ }
+
+ void realizePosition(const SBStateDigest& sbs) const {
+ SBTreePositionCache& pc = sbs.updTreePositionCache();
+
+ const Transform& X_MB = getX_MB(); // fixed
+ const Transform& X_PF = getX_PF(); // fixed
+ const Transform& X_GP = getX_GP(pc); // already calculated
+
+ updX_FM(pc).setToZero();
+ updX_PB(pc) = X_PF * X_MB;
+ updX_GB(pc) = X_GP * getX_PB(pc);
+ const Vec3 p_PB_G = getX_GP(pc).R() * getX_PB(pc).p();
+
+ // The Phi matrix conveniently performs child-to-parent (inward) shifting
+ // on spatial quantities (forces); its transpose does parent-to-child
+ // (outward) shifting for velocities.
+ updPhi(pc) = PhiMatrix(p_PB_G);
+
+ // Calculate spatial mass properties. That means we need to transform
+ // the local mass moments into the Ground frame and reconstruct the
+ // spatial inertia matrix Mk.
+
+ const Rotation& R_GB = getX_GB(pc).R();
+ const Vec3& p_GB = getX_GB(pc).p();
+
+ // reexpress inertia in ground (57 flops)
+ const UnitInertia G_Bo_G = getUnitInertia_OB_B().reexpress(~R_GB);
+ const Vec3 p_BBc_G = R_GB*getCOM_B(); // 15 flops
+
+ updCOM_G(pc) = p_GB + p_BBc_G; // 3 flops
+
+ // Calc Mk: the spatial inertia matrix about the body origin.
+ // Note: we need to calculate this now so that we'll be able to calculate
+ // kinetic energy without going past the Velocity stage.
+ updMk_G(pc) = SpatialInertia(getMass(), p_BBc_G, G_Bo_G);
+ }
+
+ void realizeVelocity(const SBStateDigest& sbs) const {
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ SBTreeVelocityCache& vc = sbs.updTreeVelocityCache();
+ calcJointIndependentKinematicsVel(pc,vc);
+ }
+
+ void realizeDynamics(const SBArticulatedBodyInertiaCache& abc,
+ const SBStateDigest& sbs) const {
+ // Mobilizer-specific.
+ const SBTreePositionCache& pc = sbs.getTreePositionCache();
+ const SBTreeVelocityCache& vc = sbs.getTreeVelocityCache();
+ SBDynamicsCache& dc = sbs.updDynamicsCache();
+
+ // Mobilizer independent.
+ calcJointIndependentDynamicsVel(pc,abc,vc,dc);
+ }
+
+ // There is no realizeAcceleration().
+
+ void realizeReport(const SBStateDigest& sbs) const {}
+
+ // Weld uses base class implementation of calcCompositeBodyInertiasInward() since
+ // that is independent of mobilities.
+
+ void realizeArticulatedBodyInertiasInward
+ (const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ SBArticulatedBodyInertiaCache& abc) const
+ {
+ ArticulatedInertia& P = updP(abc);
+ P = ArticulatedInertia(getMk_G(pc));
+ for (unsigned i=0 ; i<children.size() ; i++) {
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+ const ArticulatedInertia& PPlusChild = children[i]->getPPlus(abc);
+
+ P += PPlusChild.shift(phiChild.l());
+ }
+ updPPlus(abc) = P;
+ }
+
+
+ void realizeYOutward(
+ const SBInstanceCache&,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache& abc,
+ SBDynamicsCache& dc) const
+ {
+ // This psi actually has the wrong sign, but it doesn't matter since we multiply
+ // by it twice.
+
+ SpatialMat psi = getPhi(pc).toSpatialMat();
+ updY(dc) = ~psi * parent->getY(dc) * psi;
+ }
+
+
+ void calcUDotPass1Inward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache&,
+ const SBDynamicsCache& dc,
+ const Real* jointForces,
+ const SpatialVec* bodyForces,
+ const Real* allUDot,
+ SpatialVec* allZ,
+ SpatialVec* allZPlus,
+ Real* allEpsilon) const
+ {
+ const SpatialVec& myBodyForce = bodyForces[nodeNum];
+ SpatialVec& z = allZ[nodeNum];
+ SpatialVec& zPlus = allZPlus[nodeNum];
+
+ z = getMobilizerCentrifugalForces(dc) - myBodyForce;
+
+ for (unsigned i=0; i<children.size(); ++i) {
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+ const SpatialVec& zPlusChild = allZPlus[children[i]->getNodeNum()];
+
+ z += phiChild * zPlusChild; // 18 flops
+ }
+
+ zPlus = z;
+ }
+
+ void calcUDotPass2Outward(
+ const SBInstanceCache&,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache&,
+ const SBTreeVelocityCache& vc,
+ const SBDynamicsCache& dc,
+ const Real* allEpsilon,
+ SpatialVec* allA_GB,
+ Real* allUDot,
+ Real* allTau) const
+ {
+ SpatialVec& A_GB = allA_GB[nodeNum];
+
+ const PhiMatrix& phi = getPhi(pc);
+ const SpatialVec& a = getMobilizerCoriolisAcceleration(vc);
+
+ // Shift parent's acceleration outward (Ground==0). 12 flops
+ const SpatialVec& A_GP = allA_GB[parent->getNodeNum()];
+ const SpatialVec APlus = ~phi * A_GP;
+
+ A_GB = APlus + a; // no udot for weld
+ }
+
+ // A weld doesn't have udots but we still have to calculate z, zPlus,
+ // for use by the parent of this body.
+ void multiplyByMInvPass1Inward(
+ const SBInstanceCache& ic,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache&,
+ const Real* f,
+ SpatialVec* allZ,
+ SpatialVec* allZPlus,
+ Real* allEpsilon) const OVERRIDE_11
+ {
+ SpatialVec& z = allZ[nodeNum];
+ SpatialVec& zPlus = allZPlus[nodeNum];
+
+ z = 0;
+
+ for (unsigned i=0; i<children.size(); i++) {
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+ const SpatialVec& zPlusChild = allZPlus[children[i]->getNodeNum()];
+ z += phiChild * zPlusChild; // 18 flops
+ }
+
+ zPlus = z;
+ }
+
+ // Must set A_GB properly for propagation to children.
+ void multiplyByMInvPass2Outward(
+ const SBInstanceCache&,
+ const SBTreePositionCache& pc,
+ const SBArticulatedBodyInertiaCache&,
+ const Real* allEpsilon,
+ SpatialVec* allA_GB,
+ Real* allUDot) const OVERRIDE_11
+ {
+ SpatialVec& A_GB = allA_GB[nodeNum];
+ const PhiMatrix& phi = getPhi(pc);
+
+ // Shift parent's acceleration outward (Ground==0). 12 flops
+ const SpatialVec& A_GP = allA_GB[parent->getNodeNum()];
+ const SpatialVec APlus = ~phi * A_GP;
+
+ A_GB = APlus;
+ }
+
+ // Also serves as pass 1 for inverse dynamics.
+ void calcBodyAccelerationsFromUdotOutward(
+ const SBTreePositionCache& pc,
+ const SBTreeVelocityCache& vc,
+ const Real* allUDot,
+ SpatialVec* allA_GB) const
+ {
+ SpatialVec& A_GB = allA_GB[nodeNum];
+
+ // Shift parent's A_GB outward. (Ground A_GB is zero.)
+ const SpatialVec A_GP = ~getPhi(pc) * allA_GB[parent->getNodeNum()];
+
+ A_GB = A_GP + getMobilizerCoriolisAcceleration(vc); // no udot for weld
+ }
+
+ void calcInverseDynamicsPass2Inward(
+ const SBTreePositionCache& pc,
+ const SBTreeVelocityCache& vc,
+ const SpatialVec* allA_GB,
+ const Real* jointForces,
+ const SpatialVec* bodyForces,
+ SpatialVec* allF,
+ Real* allTau) const
+ {
+ const SpatialVec& myBodyForce = bodyForces[nodeNum];
+ const SpatialVec& A_GB = allA_GB[nodeNum];
+ SpatialVec& F = allF[nodeNum];
+
+ // Start with rigid body force from desired body acceleration and
+ // gyroscopic forces due to angular velocity, minus external forces
+ // applied directly to this body.
+ F = getMk_G(pc)*A_GB + getGyroscopicForce(vc) - myBodyForce;
+
+ // Add in forces on children, shifted to this body.
+ for (unsigned i=0; i<children.size(); ++i) {
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+ const SpatialVec& FChild = allF[children[i]->getNodeNum()];
+ F += phiChild * FChild;
+ }
+
+ // no taus.
+ }
+
+ void multiplyByMPass1Outward(
+ const SBTreePositionCache& pc,
+ const Real* allUDot,
+ SpatialVec* allA_GB) const
+ {
+ SpatialVec& A_GB = allA_GB[nodeNum];
+
+ // Shift parent's A_GB outward. (Ground A_GB is zero.)
+ const SpatialVec A_GP = ~getPhi(pc) * allA_GB[parent->getNodeNum()];
+
+ A_GB = A_GP;
+ }
+
+ void multiplyByMPass2Inward(
+ const SBTreePositionCache& pc,
+ const SpatialVec* allA_GB,
+ SpatialVec* allF, // temp
+ Real* allTau) const
+ {
+ const SpatialVec& A_GB = allA_GB[nodeNum];
+ SpatialVec& F = allF[nodeNum];
+
+ F = getMk_G(pc)*A_GB;
+
+ for (int i=0 ; i<(int)children.size() ; i++) {
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+ const SpatialVec& FChild = allF[children[i]->getNodeNum()];
+ F += phiChild * FChild;
+ }
+ }
+
+ void multiplyBySystemJacobian(
+ const SBTreePositionCache& pc,
+ const Real* v,
+ SpatialVec* Jv) const
+ {
+ SpatialVec& out = Jv[nodeNum];
+
+ // Shift parent's result outward (ground result is 0).
+ const SpatialVec outP = ~getPhi(pc) * Jv[parent->getNodeNum()];
+
+ out = outP;
+ }
+
+ void multiplyBySystemJacobianTranspose(
+ const SBTreePositionCache& pc,
+ SpatialVec* zTmp,
+ const SpatialVec* X,
+ Real* JtX) const
+ {
+ const SpatialVec& in = X[getNodeNum()];
+ SpatialVec& z = zTmp[getNodeNum()];
+
+ z = in;
+
+ for (unsigned i=0; i<children.size(); ++i) {
+ const SpatialVec& zChild = zTmp[children[i]->getNodeNum()];
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+ z += phiChild * zChild;
+ }
+ // No generalized speeds so no contribution to JtX.
+ }
+
+ void calcEquivalentJointForces(
+ const SBTreePositionCache& pc,
+ const SBDynamicsCache& dc,
+ const SpatialVec* bodyForces,
+ SpatialVec* allZ,
+ Real* jointForces) const
+ {
+ const SpatialVec& myBodyForce = bodyForces[nodeNum];
+ SpatialVec& z = allZ[nodeNum];
+
+ // Centrifugal forces are PA+b where P is articulated body inertia,
+ // A is total coriolis acceleration, and b is gyroscopic force.
+ z = myBodyForce - getTotalCentrifugalForces(dc);
+
+ for (int i=0 ; i<(int)children.size() ; i++) {
+ const SpatialVec& zChild = allZ[children[i]->getNodeNum()];
+ const PhiMatrix& phiChild = children[i]->getPhi(pc);
+ z += phiChild * zChild;
+ }
+ }
+};
+
+
+// The Ground node is special because it doesn't need a mobilizer.
+/*static*/ RigidBodyNode*
+RigidBodyNode::createGroundNode() {
+ return new RBGroundBody();
+}
+
+RigidBodyNode* MobilizedBody::WeldImpl::createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const
+{
+ return new RBNodeWeld(
+ getDefaultRigidBodyMassProperties(),
+ getDefaultInboardFrame(),getDefaultOutboardFrame(),
+ nextUSlot, nextUSqSlot, nextQSlot);
+}
+
+RigidBodyNode* MobilizedBody::GroundImpl::createRigidBodyNode(
+ UIndex& nextUSlot,
+ USquaredIndex& nextUSqSlot,
+ QIndex& nextQSlot) const
+{
+ return RigidBodyNode::createGroundNode();
+}
+
diff --git a/Simbody/src/SimbodyMatterSubsystem.cpp b/Simbody/src/SimbodyMatterSubsystem.cpp
new file mode 100644
index 0000000..03c7306
--- /dev/null
+++ b/Simbody/src/SimbodyMatterSubsystem.cpp
@@ -0,0 +1,2208 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ *
+ * Implementation of SimbodyMatterSubsystem, a concrete Subsystem.
+ */
+
+#include "SimTKcommon.h"
+#include "simbody/internal/MobilizedBody.h"
+
+#include "MobilizedBodyImpl.h"
+#include "SimbodyMatterSubsystemRep.h"
+class RigidBodyNode;
+
+#include <string>
+#include <iostream>
+using std::cout;
+using std::endl;
+
+namespace SimTK {
+
+
+/*static*/ bool
+SimbodyMatterSubsystem::isInstanceOf(const Subsystem& s) {
+ return SimbodyMatterSubsystemRep::isA(s.getSubsystemGuts());
+}
+/*static*/ const SimbodyMatterSubsystem&
+SimbodyMatterSubsystem::downcast(const Subsystem& s) {
+ assert(isInstanceOf(s));
+ return static_cast<const SimbodyMatterSubsystem&>(s);
+}
+/*static*/ SimbodyMatterSubsystem&
+SimbodyMatterSubsystem::updDowncast(Subsystem& s) {
+ assert(isInstanceOf(s));
+ return static_cast<SimbodyMatterSubsystem&>(s);
+}
+
+const SimbodyMatterSubsystemRep&
+SimbodyMatterSubsystem::getRep() const {
+ return SimTK_DYNAMIC_CAST_DEBUG<const SimbodyMatterSubsystemRep&>(getSubsystemGuts());
+}
+SimbodyMatterSubsystemRep&
+SimbodyMatterSubsystem::updRep() {
+ return SimTK_DYNAMIC_CAST_DEBUG<SimbodyMatterSubsystemRep&>(updSubsystemGuts());
+}
+
+// Create Subsystem but don't associate it with any System. This isn't much
+// use except for making std::vector's, which require a default constructor
+// to be available.
+SimbodyMatterSubsystem::SimbodyMatterSubsystem()
+ : Subsystem()
+{
+ adoptSubsystemGuts(new SimbodyMatterSubsystemRep());
+ updRep().createGroundBody(); //TODO: handle this differently
+}
+
+SimbodyMatterSubsystem::SimbodyMatterSubsystem(MultibodySystem& mbs)
+ : Subsystem()
+{
+ adoptSubsystemGuts(new SimbodyMatterSubsystemRep());
+ updRep().createGroundBody(); //TODO: handle this differently
+ mbs.setMatterSubsystem(*this);
+}
+
+MobilizedBodyIndex SimbodyMatterSubsystem::adoptMobilizedBody(MobilizedBodyIndex parent, MobilizedBody& child) {
+ return updRep().adoptMobilizedBody(parent,child);
+}
+const MobilizedBody& SimbodyMatterSubsystem::getMobilizedBody(MobilizedBodyIndex id) const {
+ return getRep().getMobilizedBody(id);
+}
+MobilizedBody& SimbodyMatterSubsystem::updMobilizedBody(MobilizedBodyIndex id) {
+ return updRep().updMobilizedBody(id);
+}
+const MobilizedBody::Ground& SimbodyMatterSubsystem::getGround() const {
+ return getRep().getGround();
+}
+MobilizedBody::Ground& SimbodyMatterSubsystem::updGround() {
+ return updRep().updGround();
+}
+
+bool SimbodyMatterSubsystem::getShowDefaultGeometry() const {
+ return getRep().getShowDefaultGeometry();
+}
+
+void SimbodyMatterSubsystem::setShowDefaultGeometry(bool show) {
+ updRep().setShowDefaultGeometry(show);
+}
+
+
+ConstraintIndex SimbodyMatterSubsystem::adoptConstraint(Constraint& child) {
+ return updRep().adoptConstraint(child);
+}
+const Constraint& SimbodyMatterSubsystem::getConstraint(ConstraintIndex id) const {
+ return getRep().getConstraint(id);
+}
+Constraint& SimbodyMatterSubsystem::updConstraint(ConstraintIndex id) {
+ return updRep().updConstraint(id);
+}
+
+
+
+//==============================================================================
+// CALC ACCELERATION
+//==============================================================================
+//TODO: should allow zero-length force arrays to stand for zeroes.
+void SimbodyMatterSubsystem::calcAcceleration
+ (const State& state,
+ const Vector& appliedMobilityForces,
+ const Vector_<SpatialVec>& appliedBodyForces,
+ Vector& udot,
+ Vector_<SpatialVec>& A_GB) const
+{
+ SimTK_APIARGCHECK2_ALWAYS(
+ appliedMobilityForces.size()==getNumMobilities(),
+ "SimbodyMatterSubsystem", "calcAcceleration",
+ "Got %d appliedMobilityForces but there are %d mobilities.",
+ appliedMobilityForces.size(), getNumMobilities());
+ SimTK_APIARGCHECK2_ALWAYS(
+ appliedBodyForces.size()==getNumBodies(),
+ "SimbodyMatterSubsystem", "calcAcceleration",
+ "Got %d appliedBodyForces but there are %d bodies (including Ground).",
+ appliedBodyForces.size(), getNumBodies());
+
+ Vector_<Vec3> appliedParticleForces; // TODO
+
+ // Create a dummy acceleration cache to hold the result.
+ const SBModelCache& mc = getRep().getModelCache(state);
+ const SBInstanceCache& ic = getRep().getInstanceCache(state);
+ SBTreeAccelerationCache tac;
+ SBConstrainedAccelerationCache cac;
+ tac.allocate(getRep().topologyCache, mc, ic);
+ cac.allocate(getRep().topologyCache, mc, ic);
+
+ Vector qdotdot; // unwanted return value
+ Vector multipliers(getNMultipliers(state)); // unwanted return value
+ Vector udotErr(getNUDotErr(state)); // unwanted return value
+
+ getRep().calcLoopForwardDynamicsOperator(state,
+ appliedMobilityForces, appliedParticleForces, appliedBodyForces,
+ tac, cac, udot, qdotdot, multipliers, udotErr);
+
+ A_GB = tac.bodyAccelerationInGround;
+}
+
+
+
+//==============================================================================
+// CALC ACCELERATION IGNORING CONSTRAINTS
+//==============================================================================
+//TODO: should allow zero-length force arrays to stand for zeroes.
+void SimbodyMatterSubsystem::calcAccelerationIgnoringConstraints
+ (const State& state,
+ const Vector& appliedMobilityForces,
+ const Vector_<SpatialVec>& appliedBodyForces,
+ Vector& udot, // output only; returns pres. accels
+ Vector_<SpatialVec>& A_GB) const
+{
+ SimTK_APIARGCHECK2_ALWAYS(
+ appliedMobilityForces.size()==getNumMobilities(),
+ "SimbodyMatterSubsystem", "calcAccelerationIgnoringConstraints",
+ "Got %d appliedMobilityForces but there are %d mobilities.",
+ appliedMobilityForces.size(), getNumMobilities());
+ SimTK_APIARGCHECK2_ALWAYS(
+ appliedBodyForces.size()==getNumBodies(),
+ "SimbodyMatterSubsystem", "calcAccelerationIgnoringConstraints",
+ "Got %d appliedBodyForces but there are %d bodies (including Ground).",
+ appliedBodyForces.size(), getNumBodies());
+
+ Vector netHingeForces(getNumMobilities()); // unwanted side effects
+ Array_<SpatialVec,MobilizedBodyIndex> abForcesZ(getNumBodies());
+ Array_<SpatialVec,MobilizedBodyIndex> abForcesZPlus(getNumBodies());
+ Vector tau;
+ Vector qdotdot;
+
+ const SBDynamicsCache& dc = getRep().getDynamicsCache(state);
+
+ getRep().calcTreeAccelerations(state,
+ appliedMobilityForces, appliedBodyForces, dc.presUDotPool,
+ netHingeForces, abForcesZ, abForcesZPlus,
+ A_GB, udot, qdotdot, tau);
+}
+
+
+
+//==============================================================================
+// CALC RESIDUAL FORCE IGNORING CONSTRAINTS
+//==============================================================================
+// This is inverse dynamics.
+// This just checks the arguments, arranges for contiguous vectors to work
+// with if necessary, and then calls the implementation method.
+void SimbodyMatterSubsystem::calcResidualForceIgnoringConstraints
+ (const State& state,
+ const Vector& appliedMobilityForces,
+ const Vector_<SpatialVec>& appliedBodyForcesInG,
+ const Vector& knownUdot,
+ Vector& residualMobilityForces) const
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nb = rep.getNumBodies();
+ const int nu = rep.getNU(state);
+
+ SimTK_APIARGCHECK2_ALWAYS(
+ appliedMobilityForces.size()==0 || appliedMobilityForces.size()==nu,
+ "SimbodyMatterSubsystem", "calcResidualForceIgnoringConstraints",
+ "Got %d appliedMobilityForces but there are %d mobilities.",
+ appliedMobilityForces.size(), nu);
+ SimTK_APIARGCHECK2_ALWAYS(
+ appliedBodyForcesInG.size()==0 || appliedBodyForcesInG.size()==nb,
+ "SimbodyMatterSubsystem", "calcResidualForceIgnoringConstraints",
+ "Got %d appliedBodyForces but there are %d bodies (including Ground).",
+ appliedBodyForcesInG.size(), nb);
+ SimTK_APIARGCHECK2_ALWAYS(
+ knownUdot.size()==0 || knownUdot.size()==nu,
+ "SimbodyMatterSubsystem", "calcResidualForceIgnoringConstraints",
+ "Got %d knownUdots but there are %d mobilities.",
+ knownUdot.size(), nu);
+
+ residualMobilityForces.resize(nu);
+
+ // Assume at first that all Vectors are contiguous.
+ const Vector* cmobForces = &appliedMobilityForces;
+ const Vector_<SpatialVec>* cbodyForces = &appliedBodyForcesInG;
+ const Vector* cudot = &knownUdot;
+ Vector* cresid = &residualMobilityForces;
+ bool needToCopyBack = false;
+
+ // We'll allocate these or not as needed.
+ Vector contig_mobForces, contig_udot, contig_resid;
+ Vector_<SpatialVec> contig_bodyForces;
+
+ if (!appliedMobilityForces.hasContiguousData()) {
+ contig_mobForces.resize(nu); // contiguous memory
+ contig_mobForces(0, nu) = appliedMobilityForces; // copy, no reallocation
+ cmobForces = (const Vector*)&contig_mobForces;
+ }
+ if (!appliedBodyForcesInG.hasContiguousData()) {
+ contig_bodyForces.resize(nb); // contiguous memory
+ contig_bodyForces(0, nb) = appliedBodyForcesInG; // copy, no reallocation
+ cbodyForces = (const Vector_<SpatialVec>*)&contig_bodyForces;
+ }
+ if (!knownUdot.hasContiguousData()) {
+ contig_udot.resize(nu); // contiguous memory
+ contig_udot(0, nu) = knownUdot; // copy, no reallocation
+ cudot = (const Vector*)&contig_udot;
+ }
+ if (!residualMobilityForces.hasContiguousData()) {
+ contig_resid.resize(nu); // contiguous memory
+ cresid = (Vector*)&contig_resid;
+ needToCopyBack = true;
+ }
+
+ Vector_<SpatialVec> A_GB(nb); // temp for unwanted result
+ rep.calcTreeResidualForces(state,
+ *cmobForces, *cbodyForces, *cudot,
+ A_GB, *cresid);
+
+ if (needToCopyBack)
+ residualMobilityForces = *cresid;
+}
+
+
+
+//==============================================================================
+// CALC RESIDUAL FORCE
+//==============================================================================
+// This is inverse dynamics with constraints.
+void SimbodyMatterSubsystem::calcResidualForce
+ (const State& state,
+ const Vector& appliedMobilityForces,
+ const Vector_<SpatialVec>& appliedBodyForcesInG,
+ const Vector& knownUdot,
+ const Vector& knownLambda,
+ Vector& residualMobilityForces) const
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nb = rep.getNumBodies();
+ const int nu = rep.getNU(state);
+ const int m = rep.getNMultipliers(state);
+
+ SimTK_APIARGCHECK2_ALWAYS(
+ appliedMobilityForces.size()==0 || appliedMobilityForces.size()==nu,
+ "SimbodyMatterSubsystem", "calcResidualForce",
+ "Got %d appliedMobilityForces but there are %d mobilities.",
+ appliedMobilityForces.size(), nu);
+ SimTK_APIARGCHECK2_ALWAYS(
+ appliedBodyForcesInG.size()==0 || appliedBodyForcesInG.size()==nb,
+ "SimbodyMatterSubsystem", "calcResidualForce",
+ "Got %d appliedBodyForces but there are %d bodies (including Ground).",
+ appliedBodyForcesInG.size(), nb);
+ SimTK_APIARGCHECK2_ALWAYS(
+ knownUdot.size()==0 || knownUdot.size()==nu,
+ "SimbodyMatterSubsystem", "calcResidualForce",
+ "Got %d knownUdots but there are %d mobilities.",
+ knownUdot.size(), nu);
+ SimTK_APIARGCHECK2_ALWAYS(
+ knownLambda.size()==0 || knownLambda.size()==m,
+ "SimbodyMatterSubsystem", "calcResidualForce",
+ "Got %d knownLambdas but there are %d constraint equations.",
+ knownUdot.size(), m);
+
+ if (knownLambda.size() == 0) { // no constraint forces
+ // Call above method instead.
+ calcResidualForceIgnoringConstraints(state,
+ appliedMobilityForces, appliedBodyForcesInG, knownUdot,
+ residualMobilityForces);
+ return;
+ }
+
+ // There are some lambdas, so calculate the forces they produce, with
+ // the result going in newly-allocated contiguous storage. We have to
+ // negate lambda to make the constraint forces have the sign of applied
+ // forces; we'll do that into a contiguous Vector also.
+ Vector_<SpatialVec> bodyForcesInG(nb);
+ Vector mobilityForces(nu);
+ Vector negLambda = -knownLambda;
+ rep.calcConstraintForcesFromMultipliers(state, negLambda,
+ bodyForcesInG, mobilityForces);
+
+ // Now add in the applied forces, and call the unconstrained routine.
+ if (appliedBodyForcesInG.size())
+ bodyForcesInG += appliedBodyForcesInG;
+ if (appliedMobilityForces.size())
+ mobilityForces += appliedMobilityForces;
+
+ calcResidualForceIgnoringConstraints(state,
+ mobilityForces, bodyForcesInG, knownUdot,
+ residualMobilityForces);
+}
+
+
+
+//==============================================================================
+// MULTIPLY BY M
+//==============================================================================
+// Check arguments, copy in/out of contiguous Vectors if necessary, call the
+// implementation method to calculate f = M*a.
+void SimbodyMatterSubsystem::multiplyByM(const State& state,
+ const Vector& a,
+ Vector& Ma) const
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nu = rep.getNU(state);
+
+ SimTK_ERRCHK2_ALWAYS(a.size() == nu,
+ "SimbodyMatterSubsystem::multiplyByMInv()",
+ "Argument 'a' had length %d but should have the same length"
+ " as the number of mobilities (generalized speeds u) %d.",
+ a.size(), nu);
+
+ Ma.resize(nu);
+ if (nu==0) return;
+
+ // Assume at first that both Vectors are contiguous.
+ const Vector* ca = &a;
+ Vector* cMa = &Ma;
+ bool needToCopyBack = false;
+
+ // We'll allocate these or not as needed.
+ Vector contig_a, contig_Ma;
+
+ if (!a.hasContiguousData()) {
+ contig_a.resize(nu); // contiguous memory
+ contig_a(0, nu) = a; // copy, prevent reallocation
+ ca = (const Vector*)&contig_a;
+ }
+
+ if (!Ma.hasContiguousData()) {
+ contig_Ma.resize(nu); // contiguous memory
+ cMa = (Vector*)&contig_Ma;
+ needToCopyBack = true;
+ }
+
+ rep.multiplyByM(state, *ca, *cMa);
+
+ if (needToCopyBack)
+ Ma = *cMa;
+}
+
+
+
+//==============================================================================
+// MULTIPLY BY M INV
+//==============================================================================
+// Check arguments, copy in/out of contiguous Vectors if necessary, call the
+// implementation method to calculate a = M^-1*v.
+void SimbodyMatterSubsystem::multiplyByMInv(const State& state,
+ const Vector& v,
+ Vector& MInvV) const
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nu = rep.getNU(state);
+
+ SimTK_ERRCHK2_ALWAYS(v.size() == nu,
+ "SimbodyMatterSubsystem::multiplyByMInv()",
+ "Argument 'v' had length %d but should have the same length"
+ " as the number of mobilities (generalized speeds u) %d.",
+ v.size(), nu);
+
+ MInvV.resize(nu);
+ if (nu==0) return;
+
+ // Assume at first that both Vectors are contiguous.
+ const Vector* cv = &v;
+ Vector* cMInvV = &MInvV;
+ bool needToCopyBack = false;
+
+ // We'll allocate these or not as needed.
+ Vector contig_v, contig_MInvV;
+
+ if (!v.hasContiguousData()) {
+ contig_v.resize(nu); // contiguous memory
+ contig_v(0, nu) = v; // copy, prevent reallocation
+ cv = (const Vector*)&contig_v;
+ }
+
+ if (!MInvV.hasContiguousData()) {
+ contig_MInvV.resize(nu); // contiguous memory
+ cMInvV = (Vector*)&contig_MInvV;
+ needToCopyBack = true;
+ }
+
+ rep.multiplyByMInv(state, *cv, *cMInvV);
+
+ if (needToCopyBack)
+ MInvV = *cMInvV;
+}
+
+
+
+void SimbodyMatterSubsystem::calcM(const State& s, Matrix& M) const
+{ getRep().calcM(s, M); }
+
+void SimbodyMatterSubsystem::calcMInv(const State& s, Matrix& MInv) const
+{ getRep().calcMInv(s, MInv); }
+
+
+// Note: the implementation methods that generate matrices do *not* require
+// contiguous storage, so we can just forward to them with no preliminaries.
+void SimbodyMatterSubsystem::calcProjectedMInv(const State& s,
+ Matrix& GMInvGt) const
+{ getRep().calcGMInvGt(s, GMInvGt); }
+
+void SimbodyMatterSubsystem::
+solveForConstraintImpulses(const State& state,
+ const Vector& deltaV,
+ Vector& impulse) const
+{ getRep().solveForConstraintImpulses(state,deltaV,impulse); }
+
+
+void SimbodyMatterSubsystem::calcG(const State& s, Matrix& G) const
+{ getRep().calcPVA(s, true, true, true, G); }
+void SimbodyMatterSubsystem::calcGTranspose(const State& s, Matrix& Gt) const
+{ getRep().calcPVATranspose(s, true, true, true, Gt); }
+void SimbodyMatterSubsystem::calcPq(const State& s, Matrix& Pq) const
+{ getRep().calcPq(s,Pq); }
+void SimbodyMatterSubsystem::calcPqTranspose(const State& s, Matrix& Pqt) const
+{ getRep().calcPqTranspose(s,Pqt); }
+
+// OBSOLETE
+void SimbodyMatterSubsystem::
+calcPNInv(const State& s, Matrix& PNInv) const {
+ return getRep().calcHolonomicConstraintMatrixPNInv(s,PNInv);
+}
+
+void SimbodyMatterSubsystem::
+calcP(const State& s, Matrix& P) const {
+ return getRep().calcHolonomicVelocityConstraintMatrixP(s,P);
+}
+
+void SimbodyMatterSubsystem::
+calcPt(const State& s, Matrix& Pt) const {
+ return getRep().calcHolonomicVelocityConstraintMatrixPt(s,Pt);
+}
+
+
+
+//==============================================================================
+// MULTIPLY BY G TRANSPOSE
+//==============================================================================
+// Check arguments, copy in/out of contiguous Vectors if necessary, call the
+// implementation method to calculate f = ~G*lambda.
+void SimbodyMatterSubsystem::
+multiplyByGTranspose(const State& s,
+ const Vector& lambda,
+ Vector& f) const
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const SBInstanceCache& ic = rep.getInstanceCache(s);
+
+ // Global problem dimensions.
+ const int mHolo = ic.totalNHolonomicConstraintEquationsInUse;
+ const int mNonholo = ic.totalNNonholonomicConstraintEquationsInUse;
+ const int mAccOnly = ic.totalNAccelerationOnlyConstraintEquationsInUse;
+ const int m = mHolo+mNonholo+mAccOnly;
+ const int nu = rep.getNU(s);
+
+ SimTK_ERRCHK2_ALWAYS(lambda.size() == m,
+ "SimbodyMatterSubsystem::multiplyByGTranspose()",
+ "Argument 'lambda' had length %d but should have the same length"
+ " as the total number of active constraint equations m=%d.",
+ lambda.size(), m);
+
+ f.resize(nu);
+ if (nu==0) return;
+ if (m==0) {f.setToZero(); return;}
+
+ // Assume at first that both Vectors are contiguous.
+ const Vector* clambda = λ
+ Vector* cf = &f;
+ bool needToCopyBack = false;
+
+ // We'll allocate these or not as needed.
+ Vector contig_lambda, contig_f;
+
+ if (!lambda.hasContiguousData()) {
+ contig_lambda.resize(m); // contiguous memory
+ contig_lambda(0, m) = lambda; // copy, prevent reallocation
+ clambda = (const Vector*)&contig_lambda;
+ }
+
+ if (!f.hasContiguousData()) {
+ contig_f.resize(nu); // contiguous memory
+ cf = (Vector*)&contig_f;
+ needToCopyBack = true;
+ }
+
+ rep.multiplyByPVATranspose(s, true, true, true, *clambda, *cf);
+ if (needToCopyBack)
+ f = *cf;
+}
+
+
+
+
+//==============================================================================
+// MULTIPLY BY Pq TRANSPOSE
+//==============================================================================
+// Check arguments, copy in/out of contiguous Vectors if necessary, call the
+// implementation method to calculate fq = ~Pq*lambdap.
+void SimbodyMatterSubsystem::
+multiplyByPqTranspose(const State& s,
+ const Vector& lambdap,
+ Vector& fq) const
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const SBInstanceCache& ic = rep.getInstanceCache(s);
+
+ // Global problem dimensions.
+ const int mp = ic.totalNHolonomicConstraintEquationsInUse;
+ const int nq = rep.getNQ(s);
+
+ SimTK_ERRCHK2_ALWAYS(lambdap.size() == mp,
+ "SimbodyMatterSubsystem::multiplyByPqTranspose()",
+ "Argument 'lambdap' had length %d but should have had the same length"
+ " as the number of active position (holonomic) constraint equations"
+ " mp=%d.", lambdap.size(), mp);
+
+ fq.resize(nq);
+ if (nq==0) return;
+ if (mp==0) {fq.setToZero(); return;}
+
+ // Assume at first that both Vectors are contiguous.
+ const Vector* clambdap = &lambdap;
+ Vector* cfq = &fq;
+ bool needToCopyBack = false;
+
+ // We'll allocate these or not as needed.
+ Vector contig_lambdap, contig_fq;
+
+ if (!lambdap.hasContiguousData()) {
+ contig_lambdap.resize(mp); // contiguous memory
+ contig_lambdap(0, mp) = lambdap; // copy, prevent reallocation
+ clambdap = (const Vector*)&contig_lambdap;
+ }
+
+ if (!fq.hasContiguousData()) {
+ contig_fq.resize(nq); // contiguous memory
+ cfq = (Vector*)&contig_fq;
+ needToCopyBack = true;
+ }
+
+ rep.multiplyByPqTranspose(s, *clambdap, *cfq);
+ if (needToCopyBack)
+ fq = *cfq;
+}
+
+
+
+//==============================================================================
+// MULTIPLY BY G
+//==============================================================================
+// Check arguments, copy in/out of contiguous Vectors if necessary, call the
+// implementation method.
+void SimbodyMatterSubsystem::
+multiplyByG(const State& s,
+ const Vector& ulike,
+ const Vector& bias,
+ Vector& Gulike) const
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const SBInstanceCache& ic = rep.getInstanceCache(s);
+
+ // Global problem dimensions.
+ const int mHolo = ic.totalNHolonomicConstraintEquationsInUse;
+ const int mNonholo = ic.totalNNonholonomicConstraintEquationsInUse;
+ const int mAccOnly = ic.totalNAccelerationOnlyConstraintEquationsInUse;
+ const int m = mHolo+mNonholo+mAccOnly;
+ const int nu = rep.getNU(s);
+
+ SimTK_ERRCHK2_ALWAYS(ulike.size() == nu,
+ "SimbodyMatterSubsystem::multiplyByG()",
+ "Argument 'ulike' had length %d but should have the same length"
+ " as the total number of mobilities nu=%d.", ulike.size(), nu);
+
+ SimTK_ERRCHK2_ALWAYS(bias.size() == m,
+ "SimbodyMatterSubsystem::multiplyByG()",
+ "Argument 'bias' had length %d but should have the same length"
+ " as the total number of constraint equations m=%d.", bias.size(), m);
+
+ Gulike.resize(m);
+ if (m==0) return;
+
+ // Assume at first that all Vectors are contiguous.
+ const Vector* culike = &ulike;
+ const Vector* cbias = &bias;
+ Vector* cGulike = &Gulike;
+ bool needToCopyBack = false;
+
+ // We'll allocate these or not as needed.
+ Vector contig_ulike, contig_bias, contig_Gulike;
+
+ if (!ulike.hasContiguousData()) {
+ contig_ulike.resize(nu); // contiguous memory
+ contig_ulike(0, nu) = ulike; // copy, prevent reallocation
+ culike = (const Vector*)&contig_ulike;
+ }
+ if (!bias.hasContiguousData()) {
+ contig_bias.resize(m); // contiguous memory
+ contig_bias(0, m) = bias; // copy, prevent reallocation
+ cbias = (const Vector*)&contig_bias;
+ }
+ if (!Gulike.hasContiguousData()) {
+ contig_Gulike.resize(m); // contiguous memory
+ cGulike = (Vector*)&contig_Gulike;
+ needToCopyBack = true;
+ }
+
+ rep.multiplyByPVA(s, true, true, true, *cbias, *culike, *cGulike);
+ if (needToCopyBack)
+ Gulike = *cGulike;
+}
+
+
+
+//==============================================================================
+// CALC BIAS FOR MULTIPLY BY G
+//==============================================================================
+// Here we just make sure that we have a contiguous array for the result and
+// then call the implementation method.
+void SimbodyMatterSubsystem::
+calcBiasForMultiplyByG(const State& state,
+ Vector& bias) const
+{
+ const SBInstanceCache& ic = getRep().getInstanceCache(state);
+
+ // Global problem dimensions.
+ const int mHolo = ic.totalNHolonomicConstraintEquationsInUse;
+ const int mNonholo = ic.totalNNonholonomicConstraintEquationsInUse;
+ const int mAccOnly = ic.totalNAccelerationOnlyConstraintEquationsInUse;
+ const int m = mHolo+mNonholo+mAccOnly;
+
+ bias.resize(m);
+ if (m==0) return;
+
+ if (bias.hasContiguousData()) {
+ getRep().calcBiasForMultiplyByPVA(state, true, true, true, bias);
+ } else {
+ Vector tmpbias(m); // contiguous
+ getRep().calcBiasForMultiplyByPVA(state, true, true, true, tmpbias);
+ bias = tmpbias;
+ }
+}
+
+
+
+
+
+//==============================================================================
+// CALC BIAS FOR ACCELERATION CONSTRAINTS
+//==============================================================================
+// Here we just make sure that we have a contiguous array for the result and
+// then call the implementation method.
+void SimbodyMatterSubsystem::
+calcBiasForAccelerationConstraints(const State& state,
+ Vector& bias) const
+{
+ const SBInstanceCache& ic = getRep().getInstanceCache(state);
+
+ // Global problem dimensions.
+ const int mHolo = ic.totalNHolonomicConstraintEquationsInUse;
+ const int mNonholo = ic.totalNNonholonomicConstraintEquationsInUse;
+ const int mAccOnly = ic.totalNAccelerationOnlyConstraintEquationsInUse;
+ const int m = mHolo+mNonholo+mAccOnly;
+
+ bias.resize(m);
+ if (m==0) return;
+
+ if (bias.hasContiguousData()) {
+ getRep().calcBiasForAccelerationConstraints(state,true,true,true,bias);
+ } else {
+ Vector tmpbias(m); // contiguous
+ getRep().calcBiasForAccelerationConstraints(state,true,true,true,tmpbias);
+ bias = tmpbias;
+ }
+}
+
+
+
+
+
+//==============================================================================
+// MULTIPLY BY Pq
+//==============================================================================
+// Check arguments, copy in/out of contiguous Vectors if necssary, call the
+// implementation method.
+void SimbodyMatterSubsystem::
+multiplyByPq(const State& s,
+ const Vector& qlike,
+ const Vector& biasp,
+ Vector& PqXqlike) const
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const SBInstanceCache& ic = rep.getInstanceCache(s);
+
+ // Problem dimensions.
+ const int mp = ic.totalNHolonomicConstraintEquationsInUse;
+ const int nq = rep.getNQ(s);
+
+ SimTK_ERRCHK2_ALWAYS(qlike.size() == nq,
+ "SimbodyMatterSubsystem::multiplyByPq()",
+ "Argument 'qlike' had length %d but should have been the same length"
+ " as the total number of generalized coordinates nq=%d.",
+ qlike.size(), nq);
+
+ SimTK_ERRCHK2_ALWAYS(biasp.size() == mp,
+ "SimbodyMatterSubsystem::multiplyByPq()",
+ "Argument 'biasp' had length %d but should have been the same length"
+ " as the total number of position (holonomic) constraint"
+ " equations mp=%d.", biasp.size(), mp);
+
+ PqXqlike.resize(mp);
+ if (mp==0) return;
+
+ // Assume at first that all Vectors are contiguous.
+ const Vector* cqlike = &qlike;
+ const Vector* cbiasp = &biasp;
+ Vector* cPqXqlike = &PqXqlike;
+ bool needToCopyBack = false;
+
+ // We'll allocate these or not as needed.
+ Vector contig_qlike, contig_biasp, contig_PqXqlike;
+
+ if (!qlike.hasContiguousData()) {
+ contig_qlike.resize(nq); // contiguous memory
+ contig_qlike(0, nq) = qlike; // copy, prevent reallocation
+ cqlike = (const Vector*)&contig_qlike;
+ }
+ if (!biasp.hasContiguousData()) {
+ contig_biasp.resize(mp); // contiguous memory
+ contig_biasp(0, mp) = biasp; // copy, prevent reallocation
+ cbiasp = (const Vector*)&contig_biasp;
+ }
+ if (!PqXqlike.hasContiguousData()) {
+ contig_PqXqlike.resize(mp); // contiguous memory
+ cPqXqlike = (Vector*)&contig_PqXqlike;
+ needToCopyBack = true;
+ }
+
+ rep.multiplyByPq(s, *cbiasp, *cqlike, *cPqXqlike);
+ if (needToCopyBack)
+ PqXqlike = *cPqXqlike;
+}
+
+
+
+//==============================================================================
+// CALC BIAS FOR MULTIPLY BY Pq
+//==============================================================================
+// The bias term is the same for P as for Pq because you can view the
+// position error first derivative as either
+// pverr = Pq * qdot + Pt
+// or pverr = P * u + Pt
+// since Pq = P*N^-1. Either way the bias term is just Pt (or c(t,q)).
+void SimbodyMatterSubsystem::
+calcBiasForMultiplyByPq(const State& state,
+ Vector& biasp) const
+{
+ const SBInstanceCache& ic = getRep().getInstanceCache(state);
+
+ // Problem dimension.
+ const int mp = ic.totalNHolonomicConstraintEquationsInUse;
+
+ biasp.resize(mp);
+ if (mp==0) return;
+
+ if (biasp.hasContiguousData()) {
+ // Just ask for P's bias term.
+ getRep().calcBiasForMultiplyByPVA(state, true, false, false, biasp);
+ } else {
+ Vector tmpbias(mp); // contiguous
+ getRep().calcBiasForMultiplyByPVA(state, true, false, false, tmpbias);
+ biasp = tmpbias;
+ }
+}
+
+
+
+
+//==============================================================================
+// CALC Gt -- OBSOLETE, use calcGTranspose()
+//==============================================================================
+void SimbodyMatterSubsystem::
+calcGt(const State& s, Matrix& Gt) const {
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int mHolo = rep.getNumHolonomicConstraintEquationsInUse(s);
+ const int mNonholo = rep.getNumNonholonomicConstraintEquationsInUse(s);
+ const int mAccOnly = rep.getNumAccelerationOnlyConstraintEquationsInUse(s);
+ const int m = mHolo+mNonholo+mAccOnly;
+ const int nu = rep.getNU(s);
+
+ Gt.resize(nu,m);
+
+ if (m==0 || nu==0)
+ return;
+
+ // Fill in all the columns of Gt
+ rep.calcHolonomicVelocityConstraintMatrixPt(s, Gt(0, 0, nu, mHolo));
+ rep.calcNonholonomicConstraintMatrixVt (s, Gt(0, mHolo, nu, mNonholo));
+ rep.calcAccelerationOnlyConstraintMatrixAt (s, Gt(0, mHolo+mNonholo, nu, mAccOnly));
+}
+
+
+
+
+//==============================================================================
+// CALC BODY ACCELERATION FROM UDOT
+//==============================================================================
+// Here we implement the zero-length udot, which is interpreted as an all-zero
+// udot meaning that only coriolis accelerations contribute. Otherwise, we
+// arrange to have contiguous input and output vectors to work with if the
+// supplied arguments won't do, then invoke the implementation method.
+void SimbodyMatterSubsystem::
+calcBodyAccelerationFromUDot(const State& state,
+ const Vector& knownUDot,
+ Vector_<SpatialVec>& A_GB) const
+{
+ // Interpret 0-length knownUDot as nu all-zero udots.
+ if (knownUDot.size() == 0) {
+ // Acceleration is just the coriolis acceleration.
+ const SBTreeVelocityCache& vc = getRep().getTreeVelocityCache(state);
+ const Array_<SpatialVec>& tca = vc.totalCoriolisAcceleration;
+ const Vector_<SpatialVec>
+ AC_GB(tca.size(), (const Real*)tca.begin(), true); // shallow ref
+ A_GB = AC_GB;
+ return;
+ }
+
+ const int nu = getNumMobilities();
+
+ SimTK_ERRCHK2_ALWAYS(knownUDot.size() == nu,
+ "SimbodyMatterSubsystem::calcBodyAccelerationFromUDot()",
+ "Length of knownUDot argument was %d but should have been either"
+ " zero or the same as the number of mobilities nu=%d.\n",
+ knownUDot.size(), nu);
+
+ const int nb = getNumBodies();
+ A_GB.resize(nb);
+
+ // If the arguments use contiguous memory we'll work in place, otherwise
+ // we'll work in contiguous temporaries and copy back.
+
+ Vector udotspace; // allocate only if we need to
+ Vector_<SpatialVec> Aspace;
+
+ const Vector* udotp;
+ Vector_<SpatialVec>* Ap;
+
+ if (knownUDot.hasContiguousData()) {
+ udotp = &knownUDot;
+ } else {
+ udotspace.resize(nu); // contiguous memory
+ udotspace(0, nu) = knownUDot; // prevent reallocation
+ udotp = (const Vector*)&udotspace;
+ }
+
+ bool needToCopyBack = false;
+ if (A_GB.hasContiguousData()) {
+ Ap = &A_GB;
+ } else {
+ Aspace.resize(nb); // contiguous memory
+ Ap = &Aspace;
+ needToCopyBack = true;
+ }
+
+ getRep().calcBodyAccelerationFromUDot(state, *udotp, *Ap);
+
+ if (needToCopyBack)
+ A_GB = *Ap;
+}
+
+
+
+//==============================================================================
+// MULTIPLY BY N, NInv, NDot
+//==============================================================================
+// These methods arrange for contiguous Vectors if necessary, then call the
+// implementation method.
+void SimbodyMatterSubsystem::multiplyByN
+ (const State& s, bool matrixOnRight, const Vector& in, Vector& out) const
+{
+ const bool inIsContig=in.hasContiguousData();
+ const bool outIsContig=out.hasContiguousData();
+
+ if (inIsContig && outIsContig) {
+ getRep().multiplyByN(s,matrixOnRight,in,out);
+ return;
+ }
+
+ Vector inSpace, outSpace; // allocate if needed
+ const Vector* inp = inIsContig ? &in : (const Vector*)&inSpace;
+ Vector* outp = outIsContig ? &out : &outSpace;
+ if (!inIsContig) {
+ inSpace.resize(in.size());
+ inSpace(0, in.size()) = in; // prevent reallocation
+ }
+
+ getRep().multiplyByN(s,matrixOnRight,*inp,*outp);
+
+ if (!outIsContig)
+ out = *outp;
+}
+
+void SimbodyMatterSubsystem::multiplyByNInv
+ (const State& s, bool matrixOnRight, const Vector& in, Vector& out) const
+{
+ const bool inIsContig=in.hasContiguousData();
+ const bool outIsContig=out.hasContiguousData();
+
+ if (inIsContig && outIsContig) {
+ getRep().multiplyByNInv(s,matrixOnRight,in,out);
+ return;
+ }
+
+ Vector inSpace, outSpace; // allocate if needed
+ const Vector* inp = inIsContig ? &in : (const Vector*)&inSpace;
+ Vector* outp = outIsContig ? &out : &outSpace;
+ if (!inIsContig) {
+ inSpace.resize(in.size());
+ inSpace(0, in.size()) = in; // prevent reallocation
+ }
+
+ getRep().multiplyByNInv(s,matrixOnRight,*inp,*outp);
+
+ if (!outIsContig)
+ out = *outp;
+}
+
+void SimbodyMatterSubsystem::multiplyByNDot
+ (const State& s, bool matrixOnRight, const Vector& in, Vector& out) const
+{
+ const bool inIsContig=in.hasContiguousData();
+ const bool outIsContig=out.hasContiguousData();
+
+ if (inIsContig && outIsContig) {
+ getRep().multiplyByNDot(s,matrixOnRight,in,out);
+ return;
+ }
+
+ Vector inSpace, outSpace; // allocate if needed
+ const Vector* inp = inIsContig ? &in : (const Vector*)&inSpace;
+ Vector* outp = outIsContig ? &out : &outSpace;
+ if (!inIsContig) {
+ inSpace.resize(in.size());
+ inSpace(0, in.size()) = in; // prevent reallocation
+ }
+
+ getRep().multiplyByNDot(s,matrixOnRight,*inp,*outp);
+
+ if (!outIsContig)
+ out = *outp;
+}
+
+
+
+
+//==============================================================================
+// JACOBIAN METHODS
+//==============================================================================
+
+
+//------------------------------------------------------------------------------
+// MULTIPLY BY SYSTEM JACOBIAN
+//------------------------------------------------------------------------------
+// Ensure that we have contiguous storage and then call the underlying method.
+void SimbodyMatterSubsystem::multiplyBySystemJacobian
+ (const State& s, const Vector& u, Vector_<SpatialVec>& Ju) const
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nb = rep.getNumBodies(), nu = rep.getNumMobilities();
+
+ SimTK_ERRCHK2_ALWAYS(u.size() == nu,
+ "SimbodyMatterSubsystem::multiplyBySystemJacobian()",
+ "The supplied u-space Vector had length %d; expected %d.",u.size(),nu);
+
+ const bool uIsContig = u.hasContiguousData();
+ const bool JuIsContig = Ju.hasContiguousData();
+
+ Vector u_contig; Vector_<SpatialVec> Ju_contig; // allocate only if needed
+ const Vector* up = uIsContig ? &u : (const Vector*)&u_contig;
+ Vector_<SpatialVec>* Jup = JuIsContig ? &Ju : &Ju_contig;
+ if (!uIsContig) {
+ u_contig.resize(nu);
+ u_contig(0, nu) = u; // prevent reallocation
+ }
+
+ rep.multiplyBySystemJacobian(s, *up, *Jup);
+
+ if (!JuIsContig)
+ Ju = Ju_contig;
+}
+
+
+//------------------------------------------------------------------------------
+// MULTIPLY BY SYSTEM JACOBIAN TRANSPOSE
+//------------------------------------------------------------------------------
+void SimbodyMatterSubsystem::multiplyBySystemJacobianTranspose
+ (const State& s, const Vector_<SpatialVec>& F_G, Vector& f) const
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nb = rep.getNumBodies(), nu = rep.getNumMobilities();
+
+ SimTK_ERRCHK2_ALWAYS(F_G.size() == nb,
+ "SimbodyMatterSubsystem::multiplyBySystemJacobianTranspose()",
+ "The supplied spatial forces vector had length %d; expected %d.",
+ F_G.size(),nb);
+
+ const bool F_GIsContig = F_G.hasContiguousData();
+ const bool fIsContig = f.hasContiguousData();
+
+ Vector_<SpatialVec> F_G_contig; Vector f_contig; // allocate only if needed
+ const Vector_<SpatialVec>* F_Gp = F_GIsContig ?
+ &F_G : (const Vector_<SpatialVec>*)&F_G_contig;
+ Vector* fp = fIsContig ? &f : &f_contig;
+ if (!F_GIsContig) {
+ F_G_contig.resize(nb);
+ F_G_contig(0, nb) = F_G; // prevent reallocation
+ }
+
+ rep.multiplyBySystemJacobianTranspose(s, *F_Gp, *fp);
+
+ if (!fIsContig)
+ f = f_contig;
+}
+
+
+//------------------------------------------------------------------------------
+// CALC SYSTEM JACOBIAN (spatial)
+//------------------------------------------------------------------------------
+// Calculate J as an nb X n matrix of SpatialVecs, by repeated calls
+// to J*u with u=0 except one u[i]=1. Cost is 12*n*(nb+n).
+// If the output matrix J_G isn't contiguous we have to allocate an nb-length
+// temporary and perform an extra copy from there into J_G.
+void SimbodyMatterSubsystem::calcSystemJacobian
+ (const State& state,
+ Matrix_<SpatialVec>& J_G) const
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nb = rep.getNumBodies(), nu = rep.getNumMobilities();
+ J_G.resize(nb,nu);
+ Vector u(nu, Real(0));
+
+ // If J_G is contiguous we can generate results directly into its columns.
+ if (J_G.hasContiguousData()) {
+ for (int j=0; j<nu; ++j) {
+ VectorView_<SpatialVec> col = J_G(j);
+ u[j] = 1; rep.multiplyBySystemJacobian(state,u,col); u[j] = 0;
+ }
+ return;
+ }
+
+ // J_G is non-contiguous so generate results into a temporary column and
+ // copy back.
+ Vector_<SpatialVec> Ju(nb);
+ for (int j=0; j<nu; ++j) {
+ u[j] = 1; rep.multiplyBySystemJacobian(state,u,Ju); u[j] = 0;
+ VectorView_<SpatialVec> col = J_G(j);
+ col = Ju;
+ }
+}
+
+
+//------------------------------------------------------------------------------
+// CALC SYSTEM JACOBIAN (scalars)
+//------------------------------------------------------------------------------
+// Alternate signature that returns a system Jacobian as a 6*nb X n Matrix
+// rather than as an nb X n matrix of spatial vectors. Note that we
+// don't know whether the output matrix has contiguous rows, columns or
+// neither; we'll always work with a contiguous temporary here and copy back.
+void SimbodyMatterSubsystem::calcSystemJacobian
+ (const State& state,
+ Matrix& J_G) const
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nb = rep.getNumBodies(), nu = rep.getNumMobilities();
+ J_G.resize(6*nb,nu); // we don't know how this is stored
+ Vector u(nu, Real(0));
+ Vector_<SpatialVec> Ju(nb); // temp Ju=J_G*u
+ for (int j=0; j<nu; ++j) {
+ u[j] = 1; rep.multiplyBySystemJacobian(state,u,Ju); u[j] = 0;
+ VectorView col = J_G(j); // 6*nb long; maybe not contiguous!
+ int nxt = 0; // index into col
+ for (MobilizedBodyIndex mbx(0); mbx < nb; ++mbx) {
+ const SpatialVec& V = Ju[mbx];
+ for (int k=0; k<3; ++k) col[nxt++] = V[0][k]; // w
+ for (int k=0; k<3; ++k) col[nxt++] = V[1][k]; // v
+ }
+ }
+}
+
+
+//------------------------------------------------------------------------------
+// CALC BIAS FOR SYSTEM JACOBIAN (spatial)
+//------------------------------------------------------------------------------
+// This is just a synonym for getTotalCoriolisAcceleration(). No flops since
+// we already computed this.
+void SimbodyMatterSubsystem::calcBiasForSystemJacobian
+ (const State& state,
+ Vector_<SpatialVec>& JDotu) const
+{
+ // Just return the coriolis acceleration.
+ const SBTreeVelocityCache& vc = getRep().getTreeVelocityCache(state);
+ const Array_<SpatialVec,MobilizedBodyIndex>& tca =
+ vc.totalCoriolisAcceleration;
+ const Vector_<SpatialVec>
+ AC_GB(tca.size(), (const Real*)tca.begin(), true); // shallow ref
+ JDotu = AC_GB;
+}
+
+
+//------------------------------------------------------------------------------
+// CALC BIAS FOR SYSTEM JACOBIAN (scalar)
+//------------------------------------------------------------------------------
+// Same as above but unpack into 6*nb vector rather nb spatial vecs.
+void SimbodyMatterSubsystem::calcBiasForSystemJacobian
+ (const State& state,
+ Vector& JDotu) const
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nb = rep.getNumBodies(), nu = rep.getNumMobilities();
+
+ const SBTreeVelocityCache& vc = rep.getTreeVelocityCache(state);
+ const Array_<SpatialVec,MobilizedBodyIndex>& tca =
+ vc.totalCoriolisAcceleration;
+
+ JDotu.resize(6*nb); // Might not be contiguous
+ int nxt = 0; // index into JDotu
+ for (MobilizedBodyIndex mbx(0); mbx < nb; ++mbx) {
+ const SpatialVec& A = tca[mbx];
+ for (int k=0; k<3; ++k) JDotu[nxt++] = A[0][k]; // b (angular accel)
+ for (int k=0; k<3; ++k) JDotu[nxt++] = A[1][k]; // a
+ }
+}
+
+
+//------------------------------------------------------------------------------
+// MULTIPLY BY STATION JACOBIAN
+//------------------------------------------------------------------------------
+// We want v_GS = J_GS*u, the linear velocity of nt station tasks Si in Ground
+// induced by the given generalized speeds. Station Si is on body Bi and is
+// given by the Bi-frame vector p_BiS. We can easily calculate V_GB = J_GB*u,
+// the spatial velocity of *all* the mobilized bodies. Then for each station
+// task,
+// v_GSi = v_GBi + w_GBi X p_BiS_G
+// where p_BiS_G is p_BiS re-expressed in Ground.
+//
+// Cost is 27*nt + 12*(nb+nu) flops.
+//
+// It is OK for the input u and output JSu vectors to be non-contiguous.
+void SimbodyMatterSubsystem::multiplyByStationJacobian
+ (const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB, // task body
+ const Array_<Vec3>& p_BS, // task station
+ const Vector& u,
+ Vector_<Vec3>& JSu) const
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nb = rep.getNumBodies(), nu = rep.getNumMobilities();
+
+ SimTK_ERRCHK2_ALWAYS(u.size() == nu,
+ "SimbodyMatterSubsystem::multiplyByStationJacobian()",
+ "The supplied u-space Vector had length %d; expected %d.",u.size(),nu);
+
+ const int nt = (int)onBodyB.size(); // number of tasks
+
+ SimTK_ERRCHK2_ALWAYS(p_BS.size() == nt,
+ "SimbodyMatterSubsystem::multiplyByStationJacobian()",
+ "The given number of task bodies (%d) and station tasks (%d) must "
+ "be the same.", nt, (int)p_BS.size());
+
+ // First use the System Jacobian to obtain spatial velocities for *all*
+ // mobilized body frames, at a cost of 12*(nb+nu) flops.
+ Vector_<SpatialVec> Ju(nb); // temp Ju=J_G*u (contiguous)
+ if (u.hasContiguousData())
+ rep.multiplyBySystemJacobian(state,u,Ju);
+ else {
+ Vector contig_u(nu); // contiguous data
+ contig_u(0,nu) = u; // no reallocation
+ rep.multiplyBySystemJacobian(state,contig_u,Ju);
+ }
+
+ // Then for each station task, determine its linear velocity at a cost of
+ // 27 flops per task.
+ JSu.resize(nt);
+ for (int task=0; task < nt; ++task) {
+ const MobilizedBodyIndex mobodx = onBodyB[task];
+ SimTK_INDEXCHECK(mobodx, nb,
+ "SimbodyMatterSubsystem::multiplyByStationJacobian()");
+
+ const MobilizedBody& mobod = rep.getMobilizedBody(mobodx);
+ const Vec3 p_BS_G =
+ mobod.expressVectorInGroundFrame(state, p_BS[task]); // 15 flops
+ const SpatialVec& V_GB = Ju[mobodx];
+ const SpatialVec V_GS = shiftVelocityBy(V_GB, p_BS_G); // 12 flops
+ JSu[task] = V_GS[1]; // return linear velocity only
+ }
+}
+
+
+//------------------------------------------------------------------------------
+// MULTIPLY BY STATION JACOBIAN TRANSPOSE
+//------------------------------------------------------------------------------
+// We want f = ~J_GS*f_GS, the generalized forces produced by applying
+// translational task force vectors f_GSi to stations Si. Each station Si is on
+// body Bi and is given by the Bi-frame vector p_BiSi. We can easily calculate
+// f = ~J_GB*F_GB for task body origins, so we shift the task forces there
+// with F_GBi=[p_BiSi_G X f_GSi, f_GSi].
+// It is OK if f_GS and/or f are not contiguous.
+// Cost is 30nt + 18nb + 11nu.
+void SimbodyMatterSubsystem::multiplyByStationJacobianTranspose
+ (const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB,
+ const Array_<Vec3>& p_BS,
+ const Vector_<Vec3>& f_GS,
+ Vector& f) const
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nb = rep.getNumBodies(), nu = rep.getNumMobilities();
+ const int nt = (int)onBodyB.size(); // number of tasks
+
+ SimTK_ERRCHK3_ALWAYS(p_BS.size() == nt && f_GS.size() == nt,
+ "SimbodyMatterSubsystem::multiplyByStationJacobianTranspose()",
+ "The given number of task bodies (%d), task stations (%d), and "
+ "applied task forces (%d) must all be the same.",
+ nt, (int)p_BS.size(), (int)f_GS.size());
+
+ f.resize(nu); // might not be contiguous
+ const bool fIsContig = f.hasContiguousData();
+ Vector f_contig; // will get allocated only if used below
+ Vector* fp = fIsContig ? &f : &f_contig;
+
+ // Need an array putting a spatial force on *every* body.
+ Vector_<SpatialVec> F_G(nb); F_G.setToZero();
+
+ // Collect the applied task forces into F_G.
+ for (int task=0; task < nt; ++task) {
+ const MobilizedBodyIndex mobodx = onBodyB[task];
+ SimTK_INDEXCHECK(mobodx, nb,
+ "SimbodyMatterSubsystem::multiplyByStationJacobianTranspose()");
+ const MobilizedBody& mobod = rep.getMobilizedBody(mobodx);
+ const Vec3 p_BS_G =
+ mobod.expressVectorInGroundFrame(state, p_BS[task]); // 15 flops
+ F_G[mobodx] += SpatialVec(p_BS_G % f_GS[task], f_GS[task]); // 15 flops
+ }
+
+ rep.multiplyBySystemJacobianTranspose(state,F_G,*fp); // 18nb+11nu flops
+
+ if (!fIsContig)
+ f = f_contig; // copy result out
+}
+
+
+//------------------------------------------------------------------------------
+// CALC STATION JACOBIAN (spatial)
+//------------------------------------------------------------------------------
+// Cost is 3*nt*(14 + 18nb + 11n) flops.
+// Each subsequent multiply by JS_G*u would be 3*nt*(2n-1)~=6*nt*n flops.
+void SimbodyMatterSubsystem::calcStationJacobian
+ (const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB,
+ const Array_<Vec3>& p_BS,
+ Matrix_<Vec3>& JS_G) const // nt X nu Vec3s
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nb = rep.getNumBodies(), nu = rep.getNumMobilities();
+ const int nt = (int)onBodyB.size(); // number of tasks
+
+ SimTK_ERRCHK2_ALWAYS(p_BS.size() == nt,
+ "SimbodyMatterSubsystem::calcStationJacobian()",
+ "The given number of task bodies (%d) and station tasks (%d) must "
+ "be the same.", nt, (int)p_BS.size());
+
+ // Calculate J=dvdu where v is linear velocity of task stations p_BS.
+ // (This is nt half-rows of J.)
+ JS_G.resize(nt,nu);
+
+ // We're assuming that 3*nt << nu so that it is cheaper to calculate ~JS
+ // than JS, using ~J*F rather than J*u.
+ // TODO: check dimensions and use whichever method is cheaper.
+ Vector_<SpatialVec> F_G(nb); F_G.setToZero();
+ Vector col(nu); // temporary to hold column of ~J_G
+ for (int task=0; task < nt; ++task) {
+ const MobilizedBodyIndex mobodx = onBodyB[task];
+ SimTK_INDEXCHECK(mobodx, nb,
+ "SimbodyMatterSubsystem::calcStationJacobian()");
+ const MobilizedBody& mobod = rep.getMobilizedBody(mobodx);
+ const Vec3 p_BS_G =
+ mobod.expressVectorInGroundFrame(state, p_BS[task]); // 15 flops
+
+ // Calculate the 3 rows of JS corresponding to this task.
+ RowVectorView_<Vec3> row = JS_G[task];
+ SpatialVec& Fb = F_G[mobodx]; // the only one we'll change
+ for (int i=0; i < 3; ++i) {
+ Fb[1][i] = 1;
+ Fb[0] = p_BS_G % Fb[1]; // r X F (9 flops)
+ rep.multiplyBySystemJacobianTranspose(state,F_G,col);// 18nb+11nu flops
+ for (int r=0; r < nu; ++r) row[r][i] = col[r];
+ Fb[1][i] = 0;
+ Fb[0] = 0;
+ }
+ }
+}
+
+
+//------------------------------------------------------------------------------
+// CALC STATION JACOBIAN (scalar)
+//------------------------------------------------------------------------------
+// Alternate signature that returns a station Jacobian as a 3*nt X nu Matrix
+// rather than as an nt X nu Matrix of Vec3s.
+void SimbodyMatterSubsystem::calcStationJacobian
+ (const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB,
+ const Array_<Vec3>& p_BS,
+ Matrix& JS_G) const // 3*nt X nu Vec3s
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nb = rep.getNumBodies(), nu = rep.getNumMobilities();
+ const int nt = (int)onBodyB.size(); // number of tasks
+
+ SimTK_ERRCHK2_ALWAYS(p_BS.size() == nt,
+ "SimbodyMatterSubsystem::calcStationJacobian()",
+ "The specified number of task bodies (%d) and station tasks (%d) must "
+ "be the same.", nt, (int)p_BS.size());
+
+ // Calculate J=dvdu where v is linear velocity of p_BS.
+ // (This is nt rows of J.)
+ JS_G.resize(3*nt,nu);
+
+ // We're assuming that 3*nt << nu so that it is cheaper to calculate ~JS
+ // than JS, using ~J*F rather than J*u.
+ // TODO: check dimensions and use whichever method is cheaper.
+ Vector_<SpatialVec> F_G(nb); F_G.setToZero();
+ Vector col(nu); // contiguous temporary to hold column of ~J_G
+ for (int task=0; task < nt; ++task) {
+ const MobilizedBodyIndex mobodx = onBodyB[task];
+ SimTK_INDEXCHECK(mobodx, nb,
+ "SimbodyMatterSubsystem::calcStationJacobian()");
+ const MobilizedBody& mobod = rep.getMobilizedBody(mobodx);
+ const Vec3 p_BS_G =
+ mobod.expressVectorInGroundFrame(state, p_BS[task]); // 15 flops
+
+ // Calculate the 3 rows of JS corresponding to this task.
+ SpatialVec& Fb = F_G[mobodx]; // the only one we'll change
+ for (int i=0; i < 3; ++i) {
+ Fb[1][i] = 1;
+ Fb[0] = p_BS_G % Fb[1]; // r X F (9 flops)
+ rep.multiplyBySystemJacobianTranspose(state,F_G,col);// 18nb+11nu flops
+ JS_G[3*task + i] = ~col;
+ Fb[1][i] = 0;
+ Fb[0] = 0;
+ }
+ }
+}
+
+
+//------------------------------------------------------------------------------
+// CALC BIAS FOR STATION JACOBIAN (spatial)
+//------------------------------------------------------------------------------
+// Just get the total Coriolis acceleration for each task body and shift it to
+// the task station S. Cost is 48*nt flops.
+void SimbodyMatterSubsystem::calcBiasForStationJacobian
+ (const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB, // nt task bodies
+ const Array_<Vec3>& p_BS, // nt task stations
+ Vector_<Vec3>& JSDotu) const // nt of these
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nb = rep.getNumBodies();
+ const int nt = (int)onBodyB.size(); // number of tasks
+
+ SimTK_ERRCHK2_ALWAYS(p_BS.size() == nt,
+ "SimbodyMatterSubsystem::calcBiasForStationJacobian()",
+ "The specified number of task bodies (%d) and station tasks (%d) must "
+ "be the same.", nt, (int)p_BS.size());
+
+ const SBTreeVelocityCache& vc = rep.getTreeVelocityCache(state);
+
+ JSDotu.resize(nt);
+ for (int task=0; task < nt; ++task) {
+ const MobilizedBodyIndex mobodx = onBodyB[task];
+ SimTK_INDEXCHECK(mobodx, nb,
+ "SimbodyMatterSubsystem::calcBiasForStationJacobian()");
+ const MobilizedBody& mobod = rep.getMobilizedBody(mobodx);
+ const Vec3 p_BS_G =
+ mobod.expressVectorInGroundFrame(state, p_BS[task]); // 15 flops
+ const SpatialVec& A_GB = vc.totalCoriolisAcceleration[mobodx];
+ const Vec3& w_GB = mobod.getBodyAngularVelocity(state);
+ const SpatialVec A_GS = shiftAccelerationBy(A_GB, w_GB, p_BS_G);
+ // 33 flops
+ JSDotu[task] = A_GS[1]; // linear acceleration only
+ }
+}
+
+//------------------------------------------------------------------------------
+// CALC BIAS FOR STATION JACOBIAN (scalar)
+//------------------------------------------------------------------------------
+void SimbodyMatterSubsystem::calcBiasForStationJacobian
+ (const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB, // nt task bodies
+ const Array_<Vec3>& p_BS, // nt task stations
+ Vector& JSDotu) const // 3*nt
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nb = rep.getNumBodies();
+ const int nt = (int)onBodyB.size(); // number of tasks
+
+ SimTK_ERRCHK2_ALWAYS(p_BS.size() == nt,
+ "SimbodyMatterSubsystem::calcBiasForStationJacobian()",
+ "The given number of task bodies (%d) and station tasks (%d) must "
+ "be the same.", nt, (int)p_BS.size());
+
+ const SBTreeVelocityCache& vc = rep.getTreeVelocityCache(state);
+
+ JSDotu.resize(3*nt); // might not be contiguous
+ for (int task=0; task < nt; ++task) {
+ const MobilizedBodyIndex mobodx = onBodyB[task];
+ SimTK_INDEXCHECK(mobodx, nb,
+ "SimbodyMatterSubsystem::calcBiasForStationJacobian()");
+ const MobilizedBody& mobod = rep.getMobilizedBody(mobodx);
+ const Vec3 p_BS_G =
+ mobod.expressVectorInGroundFrame(state, p_BS[task]); // 15 flops
+ const SpatialVec& A_GB = vc.totalCoriolisAcceleration[mobodx];
+ const Vec3& w_GB = mobod.getBodyAngularVelocity(state);
+ const SpatialVec A_GS = shiftAccelerationBy(A_GB, w_GB, p_BS_G);
+ // 33 flops
+ const Vec3& a_GS = A_GS[1]; // linear acceleration only
+ for (int k=0; k<3; ++k) JSDotu[3*task+k] = a_GS[k];
+ }
+}
+
+
+//------------------------------------------------------------------------------
+// MULTIPLY BY FRAME JACOBIAN
+//------------------------------------------------------------------------------
+// We want V_GA = J_GA*u, the spatial velocity of nt task frames Ai in
+// Ground induced by the given generalized speeds. Frames A are fixed on bodies
+// B and would be given by the transform X_BA, except the result depends only
+// on A's origin position p_BA (==p_BoAo) because angular velocity is the same
+// for all frames fixed to the same body. We can easily calculate V_GB = J_GB*u,
+// the spatial velocities at each body B's origin Bo. Then
+// V_GAi = [w_GAi, v_GAi] = [w_GBi, v_GBi + w_GBi X p_BiAi_G]
+// where p_BiAi_G is p_BiAi re-expressed in Ground.
+//
+// Cost is 27*nt + 12*(nb+nu) flops.
+void SimbodyMatterSubsystem::multiplyByFrameJacobian
+ (const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB,
+ const Array_<Vec3>& p_BA,
+ const Vector& u,
+ Vector_<SpatialVec>& JFu) const
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nb = rep.getNumBodies(), nu = rep.getNumMobilities();
+ const int nt = (int)onBodyB.size(); // number of tasks
+
+ SimTK_ERRCHK2_ALWAYS(u.size() == nu,
+ "SimbodyMatterSubsystem::multiplyByFrameJacobian()",
+ "The supplied u-space Vector had length %d; expected %d.",u.size(),nu);
+
+ SimTK_ERRCHK2_ALWAYS(p_BA.size() == nt,
+ "SimbodyMatterSubsystem::multiplyByFrameJacobian()",
+ "The given number of task bodies (%d) and frame tasks (%d) must "
+ "be the same.", nt, (int)p_BA.size());
+
+ // First use the System Jacobian to obtain spatial velocities for *all*
+ // mobilized body frames, at a cost of 12*(nb+nu) flops.
+ Vector_<SpatialVec> Ju(nb); // temp Ju=J_G*u (contiguous)
+ if (u.hasContiguousData())
+ rep.multiplyBySystemJacobian(state,u,Ju);
+ else {
+ Vector contig_u(nu); // contiguous data
+ contig_u(0,nu) = u; // no reallocation
+ rep.multiplyBySystemJacobian(state,contig_u,Ju);
+ }
+
+ // Then for each frame task, determine its linear velocity at a cost of
+ // 27 flops per task.
+ JFu.resize(nt); // OK if not contiguous
+ for (int task=0; task < nt; ++task) {
+ const MobilizedBodyIndex mobodx = onBodyB[task];
+ SimTK_INDEXCHECK(mobodx, nb,
+ "SimbodyMatterSubsystem::multiplyByFrameJacobian()");
+
+ const MobilizedBody& mobod = rep.getMobilizedBody(mobodx);
+ const Vec3 p_BA_G =
+ mobod.expressVectorInGroundFrame(state, p_BA[task]); // 15 flops
+ const SpatialVec& V_GB = Ju[mobodx];
+ JFu[task] = shiftVelocityBy(V_GB, p_BA_G); // 12 flops
+ }
+}
+
+//------------------------------------------------------------------------------
+// MULTIPLY BY FRAME JACOBIAN TRANSPOSE
+//------------------------------------------------------------------------------
+// We want f = ~J_GA*F_GA, the generalized forces produced by applying spatial
+// force vectors F_GAi=[t_G,f_GAi] at Aio, the origin of task frame Ai, which
+// is fixed to some body Bi. Frame Ai would be given by transform X_BAi, but the
+// result depends only on Ai's origin location p_BiAi (==p_BioAio) since a
+// torque is the same wherever it is applied. We can easily calculate
+// fb = ~J_GB*F_GB so we shift each F_GAi to Bi via
+// F_GBi = [t_G + p_BiAi_G X f_GAi, f_GAi].
+// Cost is 33nt + 18nb + 11nu.
+void SimbodyMatterSubsystem::multiplyByFrameJacobianTranspose
+ (const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB,
+ const Array_<Vec3>& p_BA,
+ const Vector_<SpatialVec>& F_GA,
+ Vector& f) const
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nb = rep.getNumBodies(), nu = rep.getNumMobilities();
+ const int nt = (int)onBodyB.size(); // number of tasks
+
+ SimTK_ERRCHK3_ALWAYS(p_BA.size() == nt && F_GA.size() == nt,
+ "SimbodyMatterSubsystem::multiplyByFrameJacobianTranspose()",
+ "The given number of task bodies (%d), task stations (%d), and "
+ "applied task forces (%d) must all be the same.",
+ nt, (int)p_BA.size(), (int)F_GA.size());
+
+ f.resize(nu); // might not be contiguous
+ const bool fIsContig = f.hasContiguousData();
+ Vector f_contig; // will get allocated only if used below
+ Vector* fp = fIsContig ? &f : &f_contig;
+
+ // Need an array putting a spatial force on each body.
+ Vector_<SpatialVec> F_G(nb); F_G.setToZero();
+
+ // Collect the applied task forces into F_G.
+ for (int task=0; task < nt; ++task) {
+ const MobilizedBodyIndex mobodx = onBodyB[task];
+ SimTK_INDEXCHECK(mobodx, nb,
+ "SimbodyMatterSubsystem::multiplyByFrameJacobianTranspose()");
+ const MobilizedBody& mobod = rep.getMobilizedBody(mobodx);
+ const Vec3 p_BA_G =
+ mobod.expressVectorInGroundFrame(state, p_BA[task]); // 15 flops
+ const SpatialVec& F_GAi = F_GA[task];
+ F_G[mobodx] += SpatialVec(F_GAi[0] + p_BA_G % F_GAi[1], // 18 flops
+ F_GAi[1]);
+ }
+
+ rep.multiplyBySystemJacobianTranspose(state,F_G,*fp); // 18nb+11nu flops
+
+ if (!fIsContig)
+ f = f_contig; // copy result out
+}
+
+
+//------------------------------------------------------------------------------
+// CALC FRAME JACOBIAN (spatial)
+//------------------------------------------------------------------------------
+// Cost is 42*nt + 6*(18nb + 11nu) flops.
+// Each subsequent multiply by JF_G*u would be 12*nu-6 flops.
+void SimbodyMatterSubsystem::calcFrameJacobian
+ (const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB,
+ const Array_<Vec3>& p_BA,
+ Matrix_<SpatialVec>& JF_G) const
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nb = rep.getNumBodies(); // includes ground
+ const int nu = rep.getNumMobilities();
+ const int nt = (int)onBodyB.size(); // number of tasks
+
+ SimTK_ERRCHK2_ALWAYS(p_BA.size() == nt,
+ "SimbodyMatterSubsystem::calcFrameJacobian()",
+ "The given number of task bodies (%d) and frame tasks (%d) must "
+ "be the same.", nt, (int)p_BA.size());
+
+ // Calculate J=dVdu where V is spatial velocity of task frames A.
+ // (This is nt rows of J.)
+ JF_G.resize(nt,nu);
+
+ // We're assuming that 6*nt << nu so that it is cheaper to calculate ~JF
+ // than JF, using ~J*F rather than J*u.
+ // TODO: check dimensions and use whichever method is cheaper.
+ Vector_<SpatialVec> F_G(nb); F_G.setToZero();
+ Vector col(nu); // temporary to hold column of ~J_G
+ for (int task=0; task < nt; ++task) {
+ const MobilizedBodyIndex mobodx = onBodyB[task];
+ SimTK_INDEXCHECK(mobodx, nb,
+ "SimbodyMatterSubsystem::calcFrameJacobian()");
+ const MobilizedBody& mobod = rep.getMobilizedBody(mobodx);
+ const Vec3 p_BA_G =
+ mobod.expressVectorInGroundFrame(state, p_BA[task]); // 15 flops
+
+ // Calculate the 6 rows of JS corresponding to this task.
+ RowVectorView_<SpatialVec> row = JF_G[task];
+ SpatialVec& Fb = F_G[mobodx]; // the only one we'll change
+
+ // Rotational part.
+ for (int i=0; i < 3; ++i) {
+ Fb[0][i] = 1;
+ rep.multiplyBySystemJacobianTranspose(state,F_G,col);// 18nb+11nu flops
+ for (int r=0; r < nu; ++r) row[r][0][i] = col[r];
+ Fb[0][i] = 0;
+ }
+
+ // Translational part.
+ for (int i=0; i < 3; ++i) {
+ Fb[1][i] = 1;
+ Fb[0] = p_BA_G % Fb[1]; // r X F (9 flops)
+ rep.multiplyBySystemJacobianTranspose(state,F_G,col);// 18nb+11nu flops
+ for (int r=0; r < nu; ++r) row[r][1][i] = col[r];
+ Fb[1][i] = 0;
+ Fb[0] = 0;
+ }
+ }
+}
+
+
+//------------------------------------------------------------------------------
+// CALC FRAME JACOBIAN (scalar)
+//------------------------------------------------------------------------------
+// Alternate signature that returns a frame Jacobian as a 6*nt x n Matrix
+// rather than as a Matrix of SpatialVecs.
+// Cost is 42*nt + 6*(18*nb + 11*n)
+void SimbodyMatterSubsystem::calcFrameJacobian
+ (const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB,
+ const Array_<Vec3>& p_BA,
+ Matrix& JF_G) const // 6*nt X n
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nb = rep.getNumBodies(); // includes ground
+ const int nu = rep.getNumMobilities();
+ const int nt = (int)onBodyB.size(); // number of tasks
+
+ SimTK_ERRCHK2_ALWAYS(p_BA.size() == nt,
+ "SimbodyMatterSubsystem::calcFrameJacobian()",
+ "The given number of task bodies (%d) and frame tasks (%d) must "
+ "be the same.", nt, (int)p_BA.size());
+
+ // Calculate J=dVdu where V is spatial velocity of task frames A.
+ // (This is 6*nt rows of the scalar matrix form of J.)
+ JF_G.resize(6*nt,nu);
+
+ // We're assuming that 6*nt << nu so that it is cheaper to calculate ~JF
+ // than JF, using ~J*F rather than J*u.
+ // TODO: check dimensions and use whichever method is cheaper.
+ Vector_<SpatialVec> F_G(nb); F_G.setToZero();
+ Vector col(nu); // temporary to hold column of ~J_G
+ for (int task=0; task < nt; ++task) {
+ const MobilizedBodyIndex mobodx = onBodyB[task];
+ SimTK_INDEXCHECK(mobodx, nb,
+ "SimbodyMatterSubsystem::calcFrameJacobian()");
+ const MobilizedBody& mobod = rep.getMobilizedBody(mobodx);
+ const Vec3 p_BA_G =
+ mobod.expressVectorInGroundFrame(state, p_BA[task]); // 15 flops
+
+ // Calculate the 6 rows of JS corresponding to this task.
+ SpatialVec& Fb = F_G[mobodx]; // the only one we'll change
+
+ // Rotational part.
+ for (int i=0; i < 3; ++i) {
+ Fb[0][i] = 1;
+ rep.multiplyBySystemJacobianTranspose(state,F_G,col);// 18nb+11nu flops
+ JF_G[6*task + i] = ~col;
+ Fb[0][i] = 0;
+ }
+
+ // Translational part.
+ for (int i=0; i < 3; ++i) {
+ Fb[1][i] = 1;
+ Fb[0] = p_BA_G % Fb[1]; // r X F (9 flops)
+ rep.multiplyBySystemJacobianTranspose(state,F_G,col);// 18nb+11nu flops
+ JF_G[6*task + 3 + i] = ~col;
+ Fb[1][i] = 0;
+ Fb[0] = 0;
+ }
+ }
+}
+
+
+//------------------------------------------------------------------------------
+// CALC BIAS FOR FRAME JACOBIAN (spatial)
+//------------------------------------------------------------------------------
+// Just get the total Coriolis acceleration for this body and shift it to
+// the A frame origin Ao.
+// Cost is 48*nt.
+void SimbodyMatterSubsystem::calcBiasForFrameJacobian
+ (const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB, // nt task bodies
+ const Array_<Vec3>& p_BA, // nt task frames
+ Vector_<SpatialVec>& JFDotu) const // nt of these
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nb = rep.getNumBodies();
+ const int nt = (int)onBodyB.size(); // number of tasks
+
+ SimTK_ERRCHK2_ALWAYS(p_BA.size() == nt,
+ "SimbodyMatterSubsystem::calcBiasForFrameJacobian()",
+ "The given number of task bodies (%d) and frame tasks (%d) must "
+ "be the same.", nt, (int)p_BA.size());
+
+ const SBTreeVelocityCache& vc = rep.getTreeVelocityCache(state);
+
+ JFDotu.resize(nt);
+ for (int task=0; task < nt; ++task) {
+ const MobilizedBodyIndex mobodx = onBodyB[task];
+ SimTK_INDEXCHECK(mobodx, nb,
+ "SimbodyMatterSubsystem::calcBiasForFrameJacobian()");
+ const MobilizedBody& mobod = rep.getMobilizedBody(mobodx);
+ const Vec3 p_BA_G =
+ mobod.expressVectorInGroundFrame(state, p_BA[task]); // 15 flops
+ const SpatialVec& A_GB = vc.totalCoriolisAcceleration[mobodx];
+ const Vec3& w_GB = mobod.getBodyAngularVelocity(state);
+ const SpatialVec A_GA = shiftAccelerationBy(A_GB, w_GB, p_BA_G);
+ // 33 flops
+ JFDotu[task] = A_GA; // linear acceleration only
+ }
+}
+
+
+//------------------------------------------------------------------------------
+// CALC BIAS FOR FRAME JACOBIAN (scalar)
+//------------------------------------------------------------------------------
+// Just get the total Coriolis acceleration for this body and shift it to
+// the A frame origin Ao.
+// Cost is 48*nt.
+void SimbodyMatterSubsystem::calcBiasForFrameJacobian
+ (const State& state,
+ const Array_<MobilizedBodyIndex>& onBodyB, // nt task bodies
+ const Array_<Vec3>& p_BA, // nt task frames
+ Vector& JFDotu) const // 6*nt
+{
+ const SimbodyMatterSubsystemRep& rep = getRep();
+ const int nb = rep.getNumBodies();
+ const int nt = (int)onBodyB.size(); // number of tasks
+
+ SimTK_ERRCHK2_ALWAYS(p_BA.size() == nt,
+ "SimbodyMatterSubsystem::calcBiasForFrameJacobian()",
+ "The given number of task bodies (%d) and frame tasks (%d) must "
+ "be the same.", nt, (int)p_BA.size());
+
+ const SBTreeVelocityCache& vc = rep.getTreeVelocityCache(state);
+
+ JFDotu.resize(6*nt); // might not be contiguous
+ for (int task=0; task < nt; ++task) {
+ const MobilizedBodyIndex mobodx = onBodyB[task];
+ SimTK_INDEXCHECK(mobodx, nb,
+ "SimbodyMatterSubsystem::calcBiasForFrameJacobian()");
+ const MobilizedBody& mobod = rep.getMobilizedBody(mobodx);
+ const Vec3 p_BA_G =
+ mobod.expressVectorInGroundFrame(state, p_BA[task]); // 15 flops
+ const SpatialVec& A_GB = vc.totalCoriolisAcceleration[mobodx];
+ const Vec3& w_GB = mobod.getBodyAngularVelocity(state);
+ const SpatialVec A_GA = shiftAccelerationBy(A_GB, w_GB, p_BA_G);
+ // 33 flops
+ for (int k=0; k<3; ++k) JFDotu[6*task+k] = A_GA[0][k];
+ for (int k=0; k<3; ++k) JFDotu[6*task+3+k] = A_GA[1][k];
+ }
+}
+
+
+
+//==============================================================================
+// MISC OPERATORS
+//==============================================================================
+
+void SimbodyMatterSubsystem::calcCompositeBodyInertias
+ (const State& s, Array_<SpatialInertia,MobilizedBodyIndex>& R) const
+{ getRep().calcCompositeBodyInertias(s,R); }
+
+void SimbodyMatterSubsystem::calcTreeEquivalentMobilityForces
+ (const State& s, const Vector_<SpatialVec>& bodyForces,
+ Vector& mobForces) const
+{ getRep().calcTreeEquivalentMobilityForces(s,bodyForces,mobForces); }
+
+Real SimbodyMatterSubsystem::calcKineticEnergy(const State& s) const
+{ return getRep().calcKineticEnergy(s); }
+
+void SimbodyMatterSubsystem::calcMobilizerReactionForces
+ (const State& s, Vector_<SpatialVec>& forces) const
+{ getRep().calcMobilizerReactionForces(s, forces); }
+
+const Vector& SimbodyMatterSubsystem::
+getMotionMultipliers(const State& s) const
+{ return getRep().getMotionMultipliers(s); }
+
+Vector SimbodyMatterSubsystem::
+calcMotionErrors(const State& s, const Stage& stage) const
+{ return getRep().calcMotionErrors(s,stage); }
+
+void SimbodyMatterSubsystem::
+findMotionForces(const State& s,
+ Vector& mobilityForces) const
+{ getRep().findMotionForces(s, mobilityForces); }
+
+const Vector& SimbodyMatterSubsystem::
+getConstraintMultipliers(const State& s) const
+{ return getRep().getConstraintMultipliers(s); }
+
+void SimbodyMatterSubsystem::
+findConstraintForces(const State& s,
+ Vector_<SpatialVec>& bodyForcesInG,
+ Vector& mobilityForces) const
+{ getRep().findConstraintForces(s, bodyForcesInG, mobilityForces); }
+
+Real SimbodyMatterSubsystem::
+calcMotionPower(const State& s) const
+{ return getRep().calcMotionPower(s); }
+
+Real SimbodyMatterSubsystem::
+calcConstraintPower(const State& s) const
+{ return getRep().calcConstraintPower(s); }
+
+void SimbodyMatterSubsystem::
+calcConstraintForcesFromMultipliers(const State& s,
+ const Vector& lambda,
+ Vector_<SpatialVec>& bodyForcesInG,
+ Vector& mobilityForces) const
+{ getRep().calcConstraintForcesFromMultipliers
+ (s,lambda,bodyForcesInG,mobilityForces); }
+
+void SimbodyMatterSubsystem::
+calcMobilizerReactionForcesUsingFreebodyMethod
+ (const State& s, Vector_<SpatialVec>& forces) const
+{ getRep().calcMobilizerReactionForcesUsingFreebodyMethod(s, forces); }
+
+
+void SimbodyMatterSubsystem::calcQDot(const State& s,
+ const Vector& u,
+ Vector& qdot) const
+{
+ getRep().calcQDot(s, u, qdot);
+}
+
+void SimbodyMatterSubsystem::calcQDotDot(const State& s,
+ const Vector& udot,
+ Vector& qdotdot) const
+{
+ getRep().calcQDotDot(s, udot, qdotdot);
+}
+
+// Topological info. Note the lack of a State argument.
+int SimbodyMatterSubsystem::getNumBodies() const {return getRep().getNumBodies();}
+int SimbodyMatterSubsystem::getNumMobilities() const {return getRep().getNumMobilities();}
+int SimbodyMatterSubsystem::getNumConstraints() const {return getRep().getNumConstraints();}
+int SimbodyMatterSubsystem::getNumParticles() const {return getRep().getNumParticles();}
+
+int SimbodyMatterSubsystem::getTotalQAlloc() const {return getRep().getTotalQAlloc();}
+
+// Modeling info.
+void SimbodyMatterSubsystem::setUseEulerAngles(State& s, bool useAngles) const
+ { getRep().setUseEulerAngles(s,useAngles); }
+void SimbodyMatterSubsystem::setConstraintIsDisabled(State& s, ConstraintIndex constraint, bool disabled) const
+ { getRep().setConstraintIsDisabled(s,constraint,disabled); }
+bool SimbodyMatterSubsystem::getUseEulerAngles(const State& s) const
+ { return getRep().getUseEulerAngles(s); }
+bool SimbodyMatterSubsystem::isConstraintDisabled(const State& s, ConstraintIndex constraint) const
+ { return getRep().isConstraintDisabled(s,constraint); }
+void SimbodyMatterSubsystem::convertToEulerAngles(const State& inputState, State& outputState) const
+ { return getRep().convertToEulerAngles(inputState, outputState); }
+void SimbodyMatterSubsystem::convertToQuaternions(const State& inputState, State& outputState) const
+ { return getRep().convertToQuaternions(inputState, outputState); }
+
+void SimbodyMatterSubsystem::normalizeQuaternions(State& state) const {
+ Vector dummy; // no error estimate to correct
+ getRep().normalizeQuaternions(state, dummy);
+}
+
+int SimbodyMatterSubsystem::getNumQuaternionsInUse(const State& s) const {
+ return getRep().getNumQuaternionsInUse(s);
+}
+bool SimbodyMatterSubsystem::isUsingQuaternion(const State& s, MobilizedBodyIndex body) const {
+ return getRep().isUsingQuaternion(s, body);
+}
+QuaternionPoolIndex SimbodyMatterSubsystem::getQuaternionPoolIndex(const State& s, MobilizedBodyIndex body) const {
+ return getRep().getQuaternionPoolIndex(s, body);
+}
+const SpatialVec&
+SimbodyMatterSubsystem::getMobilizerCoriolisAcceleration(const State& s, MobilizedBodyIndex body) const {
+ return getRep().getMobilizerCoriolisAcceleration(s,body);
+}
+const SpatialVec&
+SimbodyMatterSubsystem::getTotalCoriolisAcceleration(const State& s, MobilizedBodyIndex body) const {
+ return getRep().getTotalCoriolisAcceleration(s,body);
+}
+const SpatialVec&
+SimbodyMatterSubsystem::getGyroscopicForce(const State& s, MobilizedBodyIndex body) const {
+ return getRep().getGyroscopicForce(s,body);
+}
+const SpatialVec&
+SimbodyMatterSubsystem::getMobilizerCentrifugalForces(const State& s, MobilizedBodyIndex body) const {
+ return getRep().getMobilizerCentrifugalForces(s,body);
+}
+const SpatialVec&
+SimbodyMatterSubsystem::getTotalCentrifugalForces(const State& s, MobilizedBodyIndex body) const {
+ return getRep().getTotalCentrifugalForces(s,body);
+}
+const Vector&
+SimbodyMatterSubsystem::getAllParticleMasses(const State& s) const {
+ return getRep().getAllParticleMasses(s);
+}
+Vector& SimbodyMatterSubsystem::updAllParticleMasses(State& s) const {
+ return getRep().updAllParticleMasses(s);
+}
+
+const Vector_<Vec3>&
+SimbodyMatterSubsystem::getAllParticleLocations(const State& s) const {
+ return getRep().getAllParticleLocations(s);
+}
+
+const Vector_<Vec3>&
+SimbodyMatterSubsystem::getAllParticleVelocities(const State& s) const {
+ return getRep().getAllParticleVelocities(s);
+}
+
+const Vector_<Vec3>&
+SimbodyMatterSubsystem::getAllParticleAccelerations(const State& s) const {
+ return getRep().getAllParticleAccelerations(s);
+}
+
+void SimbodyMatterSubsystem::addInStationForce(const State& s, MobilizedBodyIndex body, const Vec3& stationInB,
+ const Vec3& forceInG, Vector_<SpatialVec>& bodyForces) const
+{
+ assert(bodyForces.size() == getRep().getNumBodies());
+ const Rotation& R_GB = getRep().getBodyTransform(s,body).R();
+ bodyForces[body] += SpatialVec((R_GB*stationInB) % forceInG, forceInG);
+}
+
+void SimbodyMatterSubsystem::realizeCompositeBodyInertias(const State& s) const {
+ getRep().realizeCompositeBodyInertias(s);
+}
+
+void SimbodyMatterSubsystem::realizeArticulatedBodyInertias(const State& s) const {
+ getRep().realizeArticulatedBodyInertias(s);
+}
+
+
+void SimbodyMatterSubsystem::invalidateCompositeBodyInertias(const State& s) const {
+ getRep().invalidateCompositeBodyInertias(s);
+}
+
+void SimbodyMatterSubsystem::invalidateArticulatedBodyInertias(const State& s) const {
+ getRep().invalidateArticulatedBodyInertias(s);
+}
+
+
+const Array_<QIndex>& SimbodyMatterSubsystem::
+getFreeQIndex(const State& state) const
+{ return getRep().getFreeQIndex(state); }
+const Array_<UIndex>& SimbodyMatterSubsystem::
+getFreeUIndex(const State& state) const
+{ return getRep().getFreeUIndex(state); }
+const Array_<UIndex>& SimbodyMatterSubsystem::
+getFreeUDotIndex(const State& state) const
+{ return getRep().getFreeUDotIndex(state); }
+const Array_<UIndex>& SimbodyMatterSubsystem::
+getKnownUDotIndex(const State& state) const
+{ return getRep().getKnownUDotIndex(state); }
+void SimbodyMatterSubsystem::
+packFreeQ(const State& s, const Vector& allQ, Vector& packedFreeQ) const
+{ getRep().packFreeQ(s,allQ,packedFreeQ); }
+void SimbodyMatterSubsystem::
+unpackFreeQ(const State& s, const Vector& packedFreeQ, Vector& unpackedFreeQ) const
+{ getRep().unpackFreeQ(s,packedFreeQ,unpackedFreeQ); }
+void SimbodyMatterSubsystem::
+packFreeU(const State& s, const Vector& allU, Vector& packedFreeU) const
+{ getRep().packFreeU(s,allU,packedFreeU); }
+void SimbodyMatterSubsystem::
+unpackFreeU(const State& s, const Vector& packedFreeU, Vector& unpackedFreeU) const
+{ getRep().unpackFreeU(s,packedFreeU,unpackedFreeU); }
+
+const SpatialInertia&
+SimbodyMatterSubsystem::getCompositeBodyInertia(const State& s, MobilizedBodyIndex mbx) const {
+ return getRep().getCompositeBodyInertias(s)[mbx]; // will lazy-evaluate if necessary
+}
+
+const ArticulatedInertia&
+SimbodyMatterSubsystem::getArticulatedBodyInertia(const State& s, MobilizedBodyIndex mbx) const {
+ return getRep().getArticulatedBodyInertias(s)[mbx]; // will lazy-evaluate if necessary
+}
+
+void SimbodyMatterSubsystem::addInBodyTorque(const State& s, MobilizedBodyIndex body, const Vec3& torqueInG,
+ Vector_<SpatialVec>& bodyForces) const
+{
+ assert(bodyForces.size() == getRep().getNumBodies());
+ bodyForces[body][0] += torqueInG; // no force
+}
+void SimbodyMatterSubsystem::addInMobilityForce(const State& s, MobilizedBodyIndex body, MobilizerUIndex which, Real d,
+ Vector& mobilityForces) const
+{
+ assert(mobilityForces.size() == getRep().getNumMobilities());
+ UIndex uStart; int nu; getRep().findMobilizerUs(s,body,uStart,nu);
+ assert(0 <= which && which < nu);
+ mobilityForces[uStart+which] += d;
+}
+
+Vector_<Vec3>& SimbodyMatterSubsystem::updAllParticleLocations(State& s) const {
+ return getRep().updAllParticleLocations(s);
+}
+Vector_<Vec3>& SimbodyMatterSubsystem::updAllParticleVelocities(State& s) const {
+ return getRep().updAllParticleVelocities(s);
+}
+
+/// Calculate the total system mass.
+/// TODO: this should be precalculated.
+Real SimbodyMatterSubsystem::calcSystemMass(const State& s) const {
+ Real mass = 0;
+ for (MobilizedBodyIndex b(1); b < getNumBodies(); ++b)
+ mass += getMobilizedBody(b).getBodyMassProperties(s).getMass();
+ return mass;
+}
+
+
+// Return the location r_OG_C of the system mass center C, measured from the ground
+// origin OG, and expressed in Ground.
+Vec3 SimbodyMatterSubsystem::calcSystemMassCenterLocationInGround(const State& s) const {
+ Real mass = 0;
+ Vec3 com = Vec3(0);
+
+ for (MobilizedBodyIndex b(1); b < getNumBodies(); ++b) {
+ const MassProperties& MB_OB_B = getMobilizedBody(b).getBodyMassProperties(s);
+ const Transform& X_GB = getMobilizedBody(b).getBodyTransform(s);
+ const Real mb = MB_OB_B.getMass();
+ const Vec3 r_OG_CB = X_GB * MB_OB_B.getMassCenter();
+ mass += mb;
+ com += mb * r_OG_CB; // weighted by mass
+ }
+
+ if (mass != 0)
+ com /= mass;
+
+ return com;
+}
+
+
+// Return total system mass, mass center location measured from the Ground origin,
+// and system inertia taken about the Ground origin, expressed in Ground.
+MassProperties SimbodyMatterSubsystem::calcSystemMassPropertiesInGround(const State& s) const {
+ Real mass = 0;
+ Vec3 com = Vec3(0);
+ Inertia I = Inertia(0);
+
+ for (MobilizedBodyIndex b(1); b < getNumBodies(); ++b) {
+ const MassProperties& MB_OB_B = getMobilizedBody(b).getBodyMassProperties(s);
+ const Transform& X_GB = getMobilizedBody(b).getBodyTransform(s);
+ const MassProperties MB_OG_G = MB_OB_B.calcTransformedMassProps(~X_GB);
+ const Real mb = MB_OG_G.getMass();
+ mass += mb;
+ com += mb * MB_OG_G.getMassCenter();
+ I += mb * MB_OG_G.getUnitInertia();
+ }
+
+ if (mass != 0)
+ com /= mass;
+
+ return MassProperties(mass, com, I);
+}
+
+// Return the system inertia matrix taken about the system center of mass,
+// expressed in Ground.
+Inertia SimbodyMatterSubsystem::calcSystemCentralInertiaInGround(const State& s) const {
+ const MassProperties M_OG_G = calcSystemMassPropertiesInGround(s);
+ return M_OG_G.calcCentralInertia();
+}
+
+
+// Return the velocity V_G_C = d/dt r_OG_C of the system mass center C in the Ground frame G,
+// expressed in G.
+Vec3 SimbodyMatterSubsystem::calcSystemMassCenterVelocityInGround(const State& s) const {
+ Real mass = 0;
+ Vec3 comv = Vec3(0);
+
+ for (MobilizedBodyIndex b(1); b < getNumBodies(); ++b) {
+ const MassProperties& MB_OB_B = getMobilizedBody(b).getBodyMassProperties(s);
+ const Vec3 v_G_CB = getMobilizedBody(b).findStationVelocityInGround(s, MB_OB_B.getMassCenter());
+ const Real mb = MB_OB_B.getMass();
+
+ mass += mb;
+ comv += mb * v_G_CB; // weighted by mass
+ }
+
+ if (mass != 0)
+ comv /= mass;
+
+ return comv;
+}
+
+// Return the acceleration A_G_C = d^2/dt^2 r_OG_C of the system mass center C in
+// the Ground frame G, expressed in G.
+Vec3 SimbodyMatterSubsystem::calcSystemMassCenterAccelerationInGround(const State& s) const {
+ Real mass = 0;
+ Vec3 coma = Vec3(0);
+
+ for (MobilizedBodyIndex b(1); b < getNumBodies(); ++b) {
+ const MassProperties& MB_OB_B = getMobilizedBody(b).getBodyMassProperties(s);
+ const Vec3 a_G_CB = getMobilizedBody(b).findStationAccelerationInGround(s, MB_OB_B.getMassCenter());
+ const Real mb = MB_OB_B.getMass();
+
+ mass += mb;
+ coma += mb * a_G_CB; // weighted by mass
+ }
+
+ if (mass != 0)
+ coma /= mass;
+
+ return coma;
+}
+
+// Return the momentum of the system as a whole (angular, linear) measured
+// in the ground frame, taken about the ground origin and expressed in ground.
+// (The linear component is independent of the "about" point.)
+SpatialVec SimbodyMatterSubsystem::calcSystemMomentumAboutGroundOrigin(const State& s) const {
+ SpatialVec mom(Vec3(0), Vec3(0));
+ for (MobilizedBodyIndex b(1); b < getNumBodies(); ++b) {
+ const SpatialVec mom_CB_G = getMobilizedBody(b).calcBodyMomentumAboutBodyMassCenterInGround(s);
+ const Vec3& Iw = mom_CB_G[0];
+ const Vec3& mv = mom_CB_G[1];
+ const Vec3 r = getMobilizedBody(b).findMassCenterLocationInGround(s);
+ mom[0] += (Iw + r % mv); // add central angular momentum plus contribution from mass center location
+ mom[1] += mv; // just add up central linear momenta
+ }
+ return mom;
+}
+
+// Return the momentum of the system as a whole (angular, linear) measured
+// in the ground frame, taken about the current system center of mass
+// location and expressed in ground.
+// (The linear component is independent of the "about" point.)
+SpatialVec SimbodyMatterSubsystem::calcSystemCentralMomentum(const State& s) const {
+ SpatialVec mom(Vec3(0), Vec3(0));
+ Real mtot = 0; // system mass
+ Vec3 com(0); // system mass center
+
+ for (MobilizedBodyIndex b(1); b < getNumBodies(); ++b) {
+ const MobilizedBody& mobod = getMobilizedBody(b);
+ const Real m = mobod.getBodyMass(s);
+ const Vec3 CB_G = mobod.findMassCenterLocationInGround(s);
+ mtot += m;
+ com += m * CB_G;
+ const SpatialVec mom_CB_G = mobod.calcBodyMomentumAboutBodyMassCenterInGround(s);
+ const Vec3& Iw = mom_CB_G[0];
+ const Vec3& mv = mom_CB_G[1];
+ mom[0] += (Iw + CB_G % mv); // add central angular momentum plus contribution from mass center location
+ mom[1] += mv; // just add up central linear momenta
+ }
+ if (mtot != 0)
+ com /= mtot;
+
+ // Shift momentum from ground origin to system COM (only angular affected).
+ mom[0] -= com % mom[1];
+ return mom;
+}
+
+} // namespace SimTK
+
diff --git a/Simbody/src/SimbodyMatterSubsystemRep.cpp b/Simbody/src/SimbodyMatterSubsystemRep.cpp
new file mode 100644
index 0000000..86217c9
--- /dev/null
+++ b/Simbody/src/SimbodyMatterSubsystemRep.cpp
@@ -0,0 +1,6216 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Derived from IVM code written by Charles Schwieters *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ *
+ * Implementation of SimbodyMatterSubsystemRep.
+ */
+
+#include "SimTKcommon.h"
+#include "SimTKmath.h"
+#include "simbody/internal/common.h"
+
+#include "SimbodyMatterSubsystemRep.h"
+#include "SimbodyTreeState.h"
+#include "RigidBodyNode.h"
+#include "MultibodySystemRep.h"
+#include "MobilizedBodyImpl.h"
+#include "ConstraintImpl.h"
+
+#include <string>
+#include <iostream>
+using std::cout; using std::endl;
+
+SimbodyMatterSubsystemRep::SimbodyMatterSubsystemRep(const SimbodyMatterSubsystemRep& src)
+ : SimTK::Subsystem::Guts("SimbodyMatterSubsystemRep", "X.X.X")
+{
+ assert(!"SimbodyMatterSubsystemRep copy constructor ... TODO!");
+}
+
+
+void SimbodyMatterSubsystemRep::clearTopologyState() {
+ // Constraints are independent from one another, so any deletion order
+ // is fine. However, they depend on bodies and not vice versa so we'll
+ // delete them first just to be neat.
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx)
+ delete constraints[cx];
+ constraints.clear();
+
+ // These are the owner handles, so this deletes the MobilizedBodyImpls also.
+ // We'll delete from the terminal nodes inward just to be neat.
+ for (MobilizedBodyIndex bx(mobilizedBodies.size()-1); bx >= 0; --bx)
+ delete mobilizedBodies[bx];
+ mobilizedBodies.clear();
+}
+
+void SimbodyMatterSubsystemRep::clearTopologyCache() {
+ invalidateSubsystemTopologyCache();
+
+ // TODO: state indices really shouldn't be dealt out until Stage::Model.
+ // At the moment they are part of the topology.
+ nextUSlot = UIndex(0);
+ nextUSqSlot = USquaredIndex(0);
+ nextQSlot = QIndex(0);
+
+ nextAncestorConstrainedBodyPoolSlot = AncestorConstrainedBodyPoolIndex(0);
+
+ DOFTotal=SqDOFTotal=maxNQTotal = -1;
+ nextQErrSlot = nextUErrSlot = nextMultSlot = 0; // TODO: are these being used?
+
+ topologyCache.clear();
+ topologyCacheIndex.invalidate();
+
+ // New constraint fields (TODO not used yet)
+ branches.clear();
+ positionCoupledConstraints.clear();
+ velocityCoupledConstraints.clear();
+ accelerationCoupledConstraints.clear();
+ dynamicallyCoupledConstraints.clear();
+
+ // RigidBodyNodes themselves are owned by the MobilizedBodyImpls and will
+ // be deleted when the MobilizedBodyImpl objects are.
+ rbNodeLevels.clear();
+ nodeNum2NodeMap.clear();
+
+ showDefaultGeometry = true;
+}
+
+MobilizedBodyIndex SimbodyMatterSubsystemRep::adoptMobilizedBody
+ (MobilizedBodyIndex parentIx, MobilizedBody& child)
+{
+ invalidateSubsystemTopologyCache();
+
+ const MobilizedBodyIndex ix(mobilizedBodies.size());
+ assert(parentIx < ix);
+
+ mobilizedBodies.push_back(new MobilizedBody()); // grow
+ MobilizedBody& m = *mobilizedBodies.back(); // refer to the empty handle we just created
+
+ child.disown(m); // transfer ownership to m
+
+ // Now tell the MobilizedBody object its owning MatterSubsystem, id within
+ // that Subsystem, and parent MobilizedBody object.
+ m.updImpl().setMyMatterSubsystem(updMySimbodyMatterSubsystemHandle(), parentIx, ix);
+ return ix;
+}
+
+ConstraintIndex SimbodyMatterSubsystemRep::adoptConstraint(Constraint& child) {
+ invalidateSubsystemTopologyCache();
+
+ const ConstraintIndex ix(constraints.size());
+
+ constraints.push_back(new Constraint()); // grow
+ Constraint& c = *constraints.back(); // refer to the empty handle we just created
+
+ child.disown(c); // transfer ownership to c
+
+ // Now tell the Constraint object its owning MatterSubsystem and id within
+ // that Subsystem.
+ c.updImpl().setMyMatterSubsystem(updMySimbodyMatterSubsystemHandle(), ix);
+ return ix;
+}
+
+
+void SimbodyMatterSubsystemRep::createGroundBody() {
+ assert(mobilizedBodies.empty());
+ invalidateSubsystemTopologyCache();
+
+ mobilizedBodies.push_back(new MobilizedBody::Ground());
+ mobilizedBodies[GroundIndex]->updImpl()
+ .setMyMatterSubsystem(updMySimbodyMatterSubsystemHandle(),
+ MobilizedBodyIndex(), // no parent
+ GroundIndex); //== MobilizedBodyIndex(0)
+}
+
+MobilizedBodyIndex SimbodyMatterSubsystemRep::getParent(MobilizedBodyIndex body) const {
+ return getRigidBodyNode(body).getParent()->getNodeNum();
+}
+
+Array_<MobilizedBodyIndex>
+SimbodyMatterSubsystemRep::getChildren(MobilizedBodyIndex body) const {
+ const RigidBodyNode& node = getRigidBodyNode(body);
+ Array_<MobilizedBodyIndex> children;
+ for (MobilizedBodyIndex bx(0); bx < node.getNumChildren(); ++bx)
+ children.push_back(node.getChild(bx)->getNodeNum());
+ return children;
+}
+
+const MassProperties&
+SimbodyMatterSubsystemRep::getDefaultBodyMassProperties(MobilizedBodyIndex body) const
+ { return getRigidBodyNode(body).getMassProperties_OB_B(); }
+
+const Transform&
+SimbodyMatterSubsystemRep::getDefaultMobilizerFrame(MobilizedBodyIndex body) const
+ { return getRigidBodyNode(body).getX_BM(); }
+
+const Transform&
+SimbodyMatterSubsystemRep::getDefaultMobilizerFrameOnParent(MobilizedBodyIndex body) const
+ { return getRigidBodyNode(body).getX_PF(); }
+
+const Transform&
+SimbodyMatterSubsystemRep::getBodyTransform(const State& s, MobilizedBodyIndex body) const {
+ return getRigidBodyNode(body).getX_GB(getTreePositionCache(s));
+}
+
+const SpatialVec&
+SimbodyMatterSubsystemRep::getBodyVelocity(const State& s, MobilizedBodyIndex body) const {
+ return getRigidBodyNode(body).getV_GB(getTreeVelocityCache(s));
+}
+
+const SpatialVec&
+SimbodyMatterSubsystemRep::getBodyAcceleration(const State& s, MobilizedBodyIndex body) const {
+ return getRigidBodyNode(body).getA_GB(getTreeAccelerationCache(s));
+}
+
+const SpatialVec&
+SimbodyMatterSubsystemRep::getMobilizerCoriolisAcceleration(const State& s, MobilizedBodyIndex body) const {
+ return getRigidBodyNode(body).getMobilizerCoriolisAcceleration(getTreeVelocityCache(s));
+}
+const SpatialVec&
+SimbodyMatterSubsystemRep::getTotalCoriolisAcceleration(const State& s, MobilizedBodyIndex body) const {
+ return getRigidBodyNode(body).getTotalCoriolisAcceleration(getTreeVelocityCache(s));
+}
+const SpatialVec&
+SimbodyMatterSubsystemRep::getGyroscopicForce(const State& s, MobilizedBodyIndex body) const {
+ return getRigidBodyNode(body).getGyroscopicForce(getTreeVelocityCache(s));
+}
+const SpatialVec&
+SimbodyMatterSubsystemRep::getMobilizerCentrifugalForces(const State& s, MobilizedBodyIndex body) const {
+ return getRigidBodyNode(body).getMobilizerCentrifugalForces(getDynamicsCache(s));
+}
+const SpatialVec&
+SimbodyMatterSubsystemRep::getTotalCentrifugalForces(const State& s, MobilizedBodyIndex body) const {
+ return getRigidBodyNode(body).getTotalCentrifugalForces(getDynamicsCache(s));
+}
+
+
+
+//==============================================================================
+// REALIZE TOPOLOGY
+//==============================================================================
+
+// Here we lock in the topological structure of the multibody system,
+// and compute allocation sizes we're going to need later for state
+// variables and cache entries. We allocate and initialize all the
+// Modeling variables here.
+void SimbodyMatterSubsystemRep::endConstruction(State& s) {
+ if (subsystemTopologyHasBeenRealized())
+ return; // already done
+
+ // This creates a RigidBodyNode owned by the the Topology cache of each
+ // MobilizedBody. Each RigidBodyNode lists as its parent the RigidBodyNode
+ // contained in the MobilizedBody's parent. We simultaneously build up the
+ // computational version of the multibody tree, based on RigidBodyNode
+ // objects rather than on MobilizedBody objects.
+ nodeNum2NodeMap.clear();
+ rbNodeLevels.clear();
+ DOFTotal = SqDOFTotal = maxNQTotal = 0;
+
+ // state allocation
+ nextUSlot = UIndex(0);
+ nextUSqSlot = USquaredIndex(0);
+ nextQSlot = QIndex(0);
+
+ //Must do these in order from lowest number (ground) to highest.
+ for (MobilizedBodyIndex mbx(0); mbx<getNumMobilizedBodies(); ++mbx) {
+ // Create the RigidBodyNode properly linked to its parent.
+ const MobilizedBodyImpl& mbr = getMobilizedBody(mbx).getImpl();
+ const RigidBodyNode& n = mbr.realizeTopology(s,nextUSlot,nextUSqSlot,nextQSlot);
+
+ // Create the computational multibody tree data structures, organized
+ // by level.
+ const int level = n.getLevel();
+ if ((int)rbNodeLevels.size() <= level)
+ rbNodeLevels.resize(level+1); // make room for the new level
+ const int nodeIndexWithinLevel = rbNodeLevels[level].size();
+ rbNodeLevels[level].push_back(&n);
+ nodeNum2NodeMap.push_back(RigidBodyNodeIndex(level, nodeIndexWithinLevel));
+
+ // Count up multibody tree totals.
+ const int ndof = n.getDOF();
+ DOFTotal += ndof; SqDOFTotal += ndof*ndof;
+ maxNQTotal += n.getMaxNQ();
+ }
+
+ // Order doesn't matter for constraints as long as the bodies are already
+ // there. Quaternion normalization constraints exist only at the
+ // position level, however they are not topological since modeling
+ // choices affect whether we use them. See realizeModel() below.
+
+// groundAncestorConstraint.clear();
+// branches.clear();
+// if (rbNodeLevels.size() > 1)
+// branches.resize(rbNodeLevels[1].size()); // each level 1 body is a branch
+
+ nextAncestorConstrainedBodyPoolSlot = AncestorConstrainedBodyPoolIndex(0);
+
+ for (ConstraintIndex cx(0); cx<getNumConstraints(); ++cx) {
+ // Note: currently there is no such thing as a disabled constraint at
+ // the Topology stage. Even a constraint which is disabled by default
+ // is not actually disabled until Model stage, and can be re-enabled
+ // at Model stage.
+ const ConstraintImpl& crep = getConstraint(cx).getImpl();
+ crep.realizeTopology(s);
+
+ // Create computational constraint data structure. This is organized by
+ // the ancestor body's "branch" (ground or level 1 base body), then by
+ // ancestor level along that branch.
+ /*
+ const int level = crep.getAncestorLevel();
+ if (crep.getAncestorLevel() == 0)
+ groundAncestorConstraints.push_back(i);
+ else {
+ const int branch = crep.getAncestorBranch();
+ if (branches[branch].size() <= level)
+ branches[branch].resize(level+1);
+ branches[branch][level].push_back(i);
+ }
+ */
+ }
+}
+
+int SimbodyMatterSubsystemRep::realizeSubsystemTopologyImpl(State& s) const {
+ SimTK_STAGECHECK_EQ_ALWAYS(getStage(s), Stage::Empty,
+ "SimbodyMatterSubsystem::realizeTopology()");
+
+ // Some of our 'const' values must be treated as mutable *just for this
+ // call*. Afterwards they are truly const so we don't declare them mutable,
+ // but cheat here instead.
+ SimbodyMatterSubsystemRep* mutableThis =
+ const_cast<SimbodyMatterSubsystemRep*>(this);
+
+ if (!subsystemTopologyHasBeenRealized())
+ mutableThis->endConstruction(s); // no more bodies after this!
+
+ // Fill in the local copy of the topologyCache from the information
+ // calculated in endConstruction(). Also ask the State for some room to
+ // put Modeling variables & cache and remember the indices in our
+ // construction cache.
+
+ mutableThis->topologyCache.nBodies = nodeNum2NodeMap.size();
+ mutableThis->topologyCache.nConstraints = constraints.size();
+ mutableThis->topologyCache.nParticles = 0; // TODO
+ mutableThis->topologyCache.nAncestorConstrainedBodies =
+ (int)nextAncestorConstrainedBodyPoolSlot;
+
+ mutableThis->topologyCache.nDOFs = DOFTotal;
+ mutableThis->topologyCache.maxNQs = maxNQTotal;
+ mutableThis->topologyCache.sumSqDOFs = SqDOFTotal;
+
+ SBModelVars mvars;
+ mvars.allocate(topologyCache);
+ setDefaultModelValues(topologyCache, mvars);
+ mutableThis->topologyCache.modelingVarsIndex =
+ allocateDiscreteVariable(s,Stage::Model, new Value<SBModelVars>(mvars));
+
+ mutableThis->topologyCache.modelingCacheIndex =
+ allocateCacheEntry(s,Stage::Model, new Value<SBModelCache>());
+
+ SBInstanceVars iv;
+ iv.allocate(topologyCache);
+ setDefaultInstanceValues(mvars, iv); // sets lock-by-default, but not q or u
+ mutableThis->topologyCache.topoInstanceVarsIndex =
+ allocateDiscreteVariable(s, Stage::Instance,
+ new Value<SBInstanceVars>(iv));
+
+ mutableThis->topologyCache.valid = true;
+
+ // Allocate a cache entry for the topologyCache, and save a copy there.
+ mutableThis->topologyCacheIndex =
+ allocateCacheEntry(s,Stage::Topology,
+ new Value<SBTopologyCache>(topologyCache));
+
+ return 0;
+}
+
+
+
+//==============================================================================
+// REALIZE MODEL
+//==============================================================================
+// Here we lock in modeling choices as conveyed by the values of Model-stage
+// state variables which now all have values. These choices determine the number
+// and types of state variables we're going to use to represent the changeable
+// properties of this matter subsystem. This is the last realization stage at
+// which we are given a writable State. That means all the state variables
+// we'll ever need must be allocated here (although cache entries can be added
+// later since they are mutable.)
+//
+// Issues we'll settle here include:
+// - whether to use 4 quaternions or 3 Euler angles to represent orientation
+// - the default values of all the state variables
+// - the locked q or u values for lock-by-default mobilizers
+//
+// We allocate and fill in the Model-stage cache with information that can be
+// calculated now that we have values for the Model-stage state variables.
+
+int SimbodyMatterSubsystemRep::realizeSubsystemModelImpl(State& s) const {
+ SimTK_STAGECHECK_EQ_ALWAYS(getStage(s), Stage::Topology,
+ "SimbodyMatterSubsystem::realizeModel()");
+
+ SBStateDigest sbs(s, *this, Stage::Model);
+ const SBModelVars& mv = sbs.getModelVars();
+
+ // We're going to finish initializing InstanceVars below.
+ SBInstanceVars& iv = updInstanceVars(s);
+
+ // Get the Model-stage cache and make sure it has been allocated and
+ // initialized if needed. It is OK to hold a reference here because the
+ // discrete variables (and cache entries) in the State are stable, that is,
+ // they don't change location even if more variables are added.
+ SBModelCache& mc = updModelCache(s);
+ mc.clear(); // forget any previous modeling information
+ mc.allocate(topologyCache);
+
+
+ // MOBILIZED BODY MODELING
+
+ // Count quaternions, and assign a "quaternion pool" index to each
+ // MobilizedBody that needs one, and allow mobilizers to reserve some
+ // position- and velocity-cache space for their own purposes. We
+ // can't do this until Model stage because it is a Model stage variable
+ // which decides whether ball-like joints get quaternions or Euler angles.
+ mc.totalNQInUse = mc.totalNUInUse = mc.totalNQuaternionsInUse = 0;
+ mc.totalNQPoolInUse = 0;
+ for (int i=0 ; i<(int)rbNodeLevels.size() ; ++i)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; ++j) {
+ const RigidBodyNode& node = *rbNodeLevels[i][j];
+ SBModelPerMobodInfo& mbInfo =
+ mc.updMobodModelInfo(node.getNodeNum());
+
+ // Assign q's.
+ mbInfo.nQInUse = node.getNQInUse(mv);
+ //KLUDGE: currently the Q slots are assigned at topology stage.
+ //mbInfo.firstQIndex = QIndex(mc.totalNQInUse);
+ mbInfo.firstQIndex = node.getQIndex(); // TODO
+ mc.totalNQInUse += mbInfo.nQInUse;
+
+ // Assign u's.
+ mbInfo.nUInUse = node.getNUInUse(mv);
+ //KLUDGE: currently the U slots are assigned at topology stage.
+ //mbInfo.firstUIndex = UIndex(mc.totalNUInUse);
+ mbInfo.firstUIndex = node.getUIndex(); // TODO
+ mc.totalNUInUse += mbInfo.nUInUse;
+
+ // Assign quaternion pool slot.
+ if (node.isUsingQuaternion(sbs, mbInfo.startOfQuaternion)) {
+ mbInfo.hasQuaternionInUse = true;
+ mbInfo.quaternionPoolIndex =
+ QuaternionPoolIndex(mc.totalNQuaternionsInUse);
+ mc.totalNQuaternionsInUse++;
+ }
+
+ // Assign misc. cache data slots for q's of this mobilizer.
+ if ((mbInfo.nQPoolInUse=node.calcQPoolSize(mv)) != 0) {
+ mbInfo.startInQPool =
+ MobodQPoolIndex(mc.totalNQPoolInUse);
+ mc.totalNQPoolInUse += mbInfo.nQPoolInUse;
+ } else mbInfo.startInQPool.invalidate();
+ }
+
+
+ // Give the bodies a chance to put something in the cache if they need to.
+ for (int i=0 ; i<(int)rbNodeLevels.size() ; ++i)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; ++j)
+ rbNodeLevels[i][j]->realizeModel(sbs);
+
+ // CONSTRAINT MODELING
+
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx)
+ getConstraint(cx).getImpl().realizeModel(s);
+
+ // STATE RESOURCE ALLOCATION
+
+ // Now allocate all remaining variables and cache entries and record the
+ // state resource index numbers in the ModelCache. Although we allocate
+ // all the resources now, we can only initialize those that depend only on
+ // Model-stage variables; initialization of the rest will be performed at
+ // Instance stage.
+
+ // SBInstanceVars were mostly allocated at topology stage but the lockedQs
+ // and lockedUs values can't be initialized until now. We're setting all
+ // of them although only the ones that are lock-by-default will actually
+ // get used. Note that lockedUs does double duty since it holds both u
+ // and udot values; the udot ones must be initialized to zero.
+
+ iv.lockedQs.resize(maxNQTotal);
+ setDefaultPositionValues(mv, iv.lockedQs); // set locked q's to init values
+
+ // MobilizedBodies provide default values for their q's. The number and
+ // values of these can depend on modeling variables, which are already
+ // set in the State. Don't do this for Ground, which has no q's.
+ for (MobilizedBodyIndex mbx(1); mbx < mobilizedBodies.size(); ++mbx) {
+ const MobilizedBodyImpl& mb = mobilizedBodies[mbx]->getImpl();
+ mb.copyOutDefaultQ(s, iv.lockedQs);
+ }
+
+ // Velocity variables are just the generalized speeds u, which the State
+ // knows how to deal with. Zero is always a reasonable value for velocity,
+ // so we'll initialize it here.
+ iv.lockedUs.resize(DOFTotal); // also holds locked udots
+ setDefaultVelocityValues(mv, iv.lockedUs); // set locked Us to initial value
+ // We'll overwrite the locked udots to zero below after we've used the
+ // current setting to initialize the u's.
+
+ mc.instanceCacheIndex =
+ allocateCacheEntry(s, Stage::Instance, new Value<SBInstanceCache>());
+
+ mc.timeVarsIndex =
+ allocateDiscreteVariable(s, Stage::Time, new Value<SBTimeVars>());
+ mc.timeCacheIndex =
+ allocateCacheEntry(s, Stage::Time, new Value<SBTimeCache>());
+
+ // Position variables are just q's, which the State knows how to deal with.
+
+ // Initialize state's q values to the same values we put into lockedQs.
+ mc.qIndex = allocateQ(s, iv.lockedQs);
+ mc.qVarsIndex.invalidate(); // no position-stage vars other than q
+
+ // Basic tree position kinematics can be calculated any time after Time
+ // stage and should be filled in first during realizePosition() and then
+ // marked valid so later computations during the same realization can
+ // access these quantities.
+ mc.treePositionCacheIndex =
+ allocateLazyCacheEntry(s, Stage::Time,
+ new Value<SBTreePositionCache>());
+
+ // Here is where later computations during realizePosition() go; these
+ // will assume that the TreePositionCache is available. So you can
+ // calculate these prior to Position stage's completion but not until
+ // the TreePositionCache has been marked valid.
+ mc.constrainedPositionCacheIndex =
+ allocateLazyCacheEntry(s, Stage::Time,
+ new Value<SBConstrainedPositionCache>());
+
+ // Composite body inertias *can* be calculated any time after Position
+ // stage but we want to put them off as long as possible since
+ // they may never be needed. These will only be valid if they are
+ // explicitly realized at some point.
+ mc.compositeBodyInertiaCacheIndex =
+ allocateLazyCacheEntry(s, Stage::Position,
+ new Value<SBCompositeBodyInertiaCache>());
+
+ // Articulated body inertias *can* be calculated any time after Position
+ // stage but we want to put them off until Dynamics stage if possible.
+ mc.articulatedBodyInertiaCacheIndex =
+ allocateCacheEntry(s, Stage::Position, Stage::Dynamics,
+ new Value<SBArticulatedBodyInertiaCache>());
+
+ // Initialize state's u values to the same values we put into lockedUs.
+ mc.uIndex = allocateU(s, iv.lockedUs); // set state to same initial value
+
+ // Done with the default u's stored in iv.lockedUs; now overwrite any
+ // that were locked by default at Motion::Acceleration; those must be zero.
+ for (MobilizedBodyIndex mbx(1); mbx < mobilizedBodies.size(); ++mbx) {
+ if (iv.mobilizerLockLevel[mbx] != Motion::Acceleration)
+ continue;
+
+ const SBModelPerMobodInfo& mbInfo = mc.updMobodModelInfo(mbx);
+ const int nu = mbInfo.nUInUse;
+ const UIndex uStart = mbInfo.firstUIndex;
+ for (int i=0; i < nu; ++i)
+ iv.lockedUs[UIndex(uStart+i)] = 0;
+ }
+
+ mc.uVarsIndex.invalidate(); // no velocity-stage vars other than u
+
+ // Basic tree velocity kinematics can be calculated any time after Position
+ // stage and should be filled in first during realizeVelocity() and then
+ // marked valid so later computations during the same realization can
+ // access these quantities. Note that qdots are automatically allocated in
+ // the State's Velocity-stage cache.
+ mc.treeVelocityCacheIndex =
+ allocateLazyCacheEntry(s, Stage::Position,
+ new Value<SBTreeVelocityCache>());
+
+ // Here is where later computations during realizeVelocity() go; these
+ // will assume that the TreeVelocityCache is available. So you can
+ // calculate these prior to Velocity stage's completion but not until
+ // the TreeVelocityCache has been marked valid.
+ mc.constrainedVelocityCacheIndex =
+ allocateLazyCacheEntry(s, Stage::Position,
+ new Value<SBConstrainedVelocityCache>());
+
+ // no z's
+ // Probably no dynamics-stage variables but we'll allocate anyway.
+ SBDynamicsVars dvars;
+ dvars.allocate(topologyCache);
+ setDefaultDynamicsValues(mv, dvars);
+
+ mc.dynamicsVarsIndex =
+ allocateDiscreteVariable(s, Stage::Dynamics,
+ new Value<SBDynamicsVars>(dvars));
+ mc.dynamicsCacheIndex =
+ allocateCacheEntry(s, Stage::Dynamics,
+ new Value<SBDynamicsCache>());
+
+ // No Acceleration variables that I know of. But we can go through the
+ // charade here anyway.
+ SBAccelerationVars rvars;
+ rvars.allocate(topologyCache);
+ setDefaultAccelerationValues(mv, rvars);
+ mc.accelerationVarsIndex =
+ allocateDiscreteVariable(s, Stage::Acceleration,
+ new Value<SBAccelerationVars>(rvars));
+
+ // Tree acceleration kinematics can be calculated any time after Dynamics
+ // stage and should be filled in first during realizeAcceleration() and then
+ // marked valid so later computations during the same realization can
+ // access these quantities.
+ // Note that qdotdots, udots, zdots are automatically allocated by
+ // the State when we advance the stage past Model.
+ mc.treeAccelerationCacheIndex =
+ allocateLazyCacheEntry(s, Stage::Dynamics,
+ new Value<SBTreeAccelerationCache>());
+
+ // Here is where later computations during realizeAcceleration() go; these
+ // will assume that the TreeAccelerationCache is available. So you can
+ // calculate these prior to Acceleration stage's completion but not until
+ // the TreeAccelerationCache has been marked valid.
+ mc.constrainedAccelerationCacheIndex =
+ allocateLazyCacheEntry(s, Stage::Dynamics,
+ new Value<SBConstrainedAccelerationCache>());
+
+ return 0;
+}
+
+
+
+//==============================================================================
+// REALIZE INSTANCE
+//==============================================================================
+// Here we lock in parameterization of ("instantiate") the model, including
+// - how the motion of each mobilizer is to be treated
+// - the total number of constraint equations
+// - physical parameters like mass and geometry.
+// All cache entries should be allocated at this stage although they can be
+// written into at any stage.
+//
+// This is the last stage that doesn't change during time stepping, so it is
+// important to calculate as much as possible now to avoid unnecessary work
+// later. The Instance-stage cache is fully calculated and filled in here. Any
+// values in higher-level caches that can be calculated now should be.
+
+int SimbodyMatterSubsystemRep::
+realizeSubsystemInstanceImpl(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(s), Stage(Stage::Instance).prev(),
+ "SimbodyMatterSubsystem::realizeInstance()");
+
+ const SBModelCache& mc = getModelCache(s);
+ const SBInstanceVars& iv = getInstanceVars(s);
+
+ // Get the Instance-stage cache and make sure it has been allocated and
+ // initialized if needed. We'll fill it in here and then allocate all
+ // the rest of the cache entries after that.
+ SBInstanceCache& ic = updInstanceCache(s);
+ ic.allocate(topologyCache, mc);
+
+
+ // MOBILIZED BODY INSTANCE
+ // Here we need to instantiate the Body, Mobilizer, Motion, and the
+ // implementing RigidBodyNode.
+
+ // Body mass properties are now available in the InstanceVars.
+ ic.totalMass = iv.particleMasses.sum();
+ for (MobilizedBodyIndex i(1); i<getNumBodies(); ++i) // not Ground!
+ ic.totalMass += iv.bodyMassProperties[i].getMass();
+ // TODO: central and principal inertias
+
+ // Mobilizer geometry is now available in the InstanceVars.
+ // TODO: reference configuration
+
+ // Count position-, velocity-, and acceleration- prescribed motions
+ // generated by Motion objects associated with MobilizedBodies and allocate
+ // pools to hold the associated values in the state cache. When position is
+ // prescribed (by specifying q(t)), the corresponding qdot and qdotdot are
+ // also prescribed and we use them to set u and udot (via u=N^-1 qdot and
+ // udot = N^-1(qdotdot-NDot*u)). Each prescribed udot will have a
+ // corresponding force calculated, and other known udots (zero or discrete;
+ // anything but free) will also need force slots although they don't get
+ // UDotPool slots.
+ //
+ // There is no built-in support in the State for these pools, so we
+ // allocate them in Simbody's cache entries at the appropriate stages. The
+ // prescribed q pool is in the TimeCache, prescribed u (dependent on the
+ // TreePositionCache) is written into the ConstrainedPositionCache,
+ // prescribed udots are written to the DynamicsCache,
+ // and the prescribed forces tau are calculated at the same time as the
+ // free (non-prescribed) udots and are thus in the TreeAccelerationCache.
+ //
+ // NOTE: despite appearances here, each pool is in MobilizedBodyIndex order,
+ // meaning that the prescribed position, velocity, and acceleration, and the
+ // other known udot entries, will be intermingled in the ForcePool rather than
+ // neatly lined up as I've drawn them.
+ //
+ // --------------------
+ // QPool | nPresQ | NOTE: not really ordered like this
+ // \------------------/
+ // \----------------/-------------
+ // UPool | nPresU |
+ // |----------------|-------------|
+ // |------------------------------|--------------
+ // UDotPool | nPresUDot |
+ // |------------------------------|--------------|
+ // |---------------------------------------------|----------
+ // ForcePool | nPresForces |
+ // ---------------------------------------------|----------
+ //
+ // Note that there are no slots allocated for q's, u's, or udots that are
+ // known to be zero; only the explicitly prescribed ones get a slot. And
+ // udots known for any reason get a slot in the ForcePool.
+
+ // Motion options for all mobilizers are now available in the InstanceVars.
+ // We need to figure out what cache resources are required, including
+ // slots for the generalized forces that implement prescribed motion.
+ ic.presQ.clear(); ic.zeroQ.clear(); ic.freeQ.clear();
+ ic.presU.clear(); ic.zeroU.clear(); ic.freeU.clear();
+ ic.presUDot.clear(); ic.zeroUDot.clear(); ic.freeUDot.clear();
+ ic.presForce.clear();
+ for (MobilizedBodyIndex mbx(0); mbx < mobilizedBodies.size(); ++mbx) {
+ const MobilizedBody& mobod = getMobilizedBody(mbx);
+ const SBModelPerMobodInfo& modelInfo = mc.getMobodModelInfo(mbx);
+ SBInstancePerMobodInfo& instanceInfo = ic.updMobodInstanceInfo(mbx);
+
+ const int nq = modelInfo.nQInUse;
+ const int nu = modelInfo.nUInUse;
+
+ const QIndex qx = modelInfo.firstQIndex;
+ const UIndex ux = modelInfo.firstUIndex;
+
+ instanceInfo.clear(); // all motion is tentatively free
+
+ // Treat Ground or Weld as prescribed to zero.
+ if (mbx == GroundIndex || nq==0) {
+ instanceInfo.qMethod = instanceInfo.uMethod =
+ instanceInfo.udotMethod = Motion::Zero;
+ continue;
+ }
+
+ // Not Ground or a Weld. If locked, use the lock level to determine
+ // how motion is calculated. Otherwise, if this mobilizer has a
+ // non-disabled Motion, let the Motion decide.
+
+ const Motion::Level lockLevel = iv.mobilizerLockLevel[mbx];
+ if (lockLevel != Motion::NoLevel) { // locked
+ if (lockLevel == Motion::Acceleration) {
+ // If all the udots are zero, the method is Motion::Zero;
+ // otherwise, it is Motion::Prescribed.
+ instanceInfo.udotMethod = Motion::Zero;
+ for (int i=0; i<nu; ++i)
+ if (iv.lockedUs[UIndex(ux+i)] != 0) {
+ instanceInfo.udotMethod = Motion::Prescribed;
+ break;
+ }
+ } else if (lockLevel == Motion::Velocity) {
+ // If all the u's are zero, the method is Motion::Zero;
+ // otherwise, it is Motion::Prescribed.
+ instanceInfo.uMethod = Motion::Zero;
+ for (int i=0; i<nu; ++i)
+ if (iv.lockedUs[UIndex(ux+i)] != 0) {
+ instanceInfo.uMethod = Motion::Prescribed;
+ break;
+ }
+ instanceInfo.udotMethod = Motion::Zero;
+ } else if (lockLevel == Motion::Position) {
+ //TODO: Motion::Zero should mean identity transform; not the
+ //same as q==0. Always use Prescribed for positions for now.
+ instanceInfo.qMethod = Motion::Prescribed;
+ instanceInfo.uMethod = Motion::Zero;
+ instanceInfo.udotMethod = Motion::Zero;
+ }
+ } else if (mobod.hasMotion() && !iv.prescribedMotionIsDisabled[mbx]) {
+ // Not locked, but has an active Motion.
+ const Motion& motion = mobod.getMotion();
+ motion.calcAllMethods(s, instanceInfo.qMethod,
+ instanceInfo.uMethod,
+ instanceInfo.udotMethod);
+ }
+
+ // Assign q's to appropriate index vectors for convenient
+ // manipulation. Prescribed q's also need pool allocation.
+ switch(instanceInfo.qMethod) {
+ case Motion::Prescribed:
+ instanceInfo.firstPresQ = PresQPoolIndex(ic.presQ.size());
+ for (int i=0; i < nq; ++i)
+ ic.presQ.push_back(QIndex(qx+i));
+ break;
+ case Motion::Zero:
+ for (int i=0; i < nq; ++i)
+ ic.zeroQ.push_back(QIndex(qx+i));
+ break;
+ case Motion::Free:
+ for (int i=0; i < nq; ++i)
+ ic.freeQ.push_back(QIndex(qx+i));
+ break;
+ case Motion::Discrete:
+ case Motion::Fast:
+ break; // nothing to do for these here
+ default:
+ SimTK_ASSERT1_ALWAYS(!"qMethod",
+ "SimbodyMatterSubsystemRep::realizeSubsystemInstanceImpl(): "
+ "Unrecognized qMethod %d.", (int)instanceInfo.qMethod);
+ }
+
+ // Assign u's to appropriate index vectors for convenient
+ // manipulation. Prescribed u's also need pool allocation.
+ switch(instanceInfo.uMethod) {
+ case Motion::Prescribed:
+ instanceInfo.firstPresU = PresUPoolIndex(ic.presU.size());
+ for (int i=0; i < nu; ++i)
+ ic.presU.push_back(UIndex(ux+i));
+ break;
+ case Motion::Zero:
+ for (int i=0; i < nu; ++i)
+ ic.zeroU.push_back(UIndex(ux+i));
+ break;
+ case Motion::Free:
+ for (int i=0; i < nu; ++i)
+ ic.freeU.push_back(UIndex(ux+i));
+ break;
+ case Motion::Discrete:
+ case Motion::Fast:
+ break; // nothing to do for these here
+ default:
+ SimTK_ASSERT1_ALWAYS(!"uMethod",
+ "SimbodyMatterSubsystemRep::realizeSubsystemInstanceImpl(): "
+ "Unrecognized uMethod %d.", (int)instanceInfo.uMethod);
+ }
+
+ // Assign udots to appropriate index vectors for convenient
+ // manipulation. Prescribed udots also need pool allocation.
+
+ switch(instanceInfo.udotMethod) {
+ case Motion::Prescribed:
+ instanceInfo.firstPresUDot = PresUDotPoolIndex(ic.presUDot.size());
+ for (int i=0; i < nu; ++i)
+ ic.presUDot.push_back(UIndex(ux+i));
+ break;
+ case Motion::Zero:
+ for (int i=0; i < nu; ++i)
+ ic.zeroUDot.push_back(UIndex(ux+i));
+ break;
+ case Motion::Free:
+ for (int i=0; i < nu; ++i)
+ ic.freeUDot.push_back(UIndex(ux+i));
+ break;
+ case Motion::Discrete:
+ case Motion::Fast:
+ break; // nothing to do for these here
+ default:
+ SimTK_ASSERT1_ALWAYS(!"udotMethod",
+ "SimbodyMatterSubsystemRep::realizeSubsystemInstanceImpl(): "
+ "Unrecognized udotMethod %d.", (int)instanceInfo.udotMethod);
+ }
+
+ // Any non-Free udot needs a prescribed force (tau) slot.
+ // Count mobilities that need a slot to hold the calculated force due
+ // to a known udot, whether prescribed or known for some other reason.
+ if (instanceInfo.udotMethod != Motion::Free) {
+ instanceInfo.firstPresForce =
+ PresForcePoolIndex(ic.presForce.size());
+ for (int i=0; i < nu; ++i)
+ ic.presForce.push_back(UIndex(ux+i));
+ }
+ }
+
+
+ // CONSTRAINT INSTANCE
+
+
+ // Count position, velocity, and acceleration constraint equations
+ // generated by each Constraint that has not been disabled. The State's
+ // QErr, UErr, UDotErr/Multiplier arrays are laid out like this:
+ //
+ // ------------------- -------------
+ // QErr | mHolonomic | mQuaternion |
+ // ------------------- -------------
+ // ------------------- -------------------
+ // UErr | " | mNonholonomic |
+ // ------------------- -------------------
+ // ------------------- ------------------- ---------------------
+ // UDotErr | " | " | mAccelerationOnly |
+ // ------------------- ------------------- ---------------------
+ //
+ // Multipliers are allocated exactly as for UDotErr.
+ //
+ // Note that a Constraint with both holonomic and nonholonomic constraint
+ // equations will get two disjoint segments in UErr (and UDotErr).
+
+ // Each Constraint's realizeInstance() method will add its contribution to
+ // these. "InUse" here means we only add up contributions from enabled
+ // constraints and ignore disabled ones.
+ ic.totalNHolonomicConstraintEquationsInUse = 0;
+ ic.totalNNonholonomicConstraintEquationsInUse = 0;
+ ic.totalNAccelerationOnlyConstraintEquationsInUse = 0;
+
+ ic.totalNConstrainedBodiesInUse = 0;
+ ic.totalNConstrainedMobilizersInUse = 0;
+ ic.totalNConstrainedQInUse = 0; // q,u from the constrained mobilizers
+ ic.totalNConstrainedUInUse = 0;
+
+
+ // Build sets of kinematically coupled constraints. Kinematic coupling can
+ // be different at position, velocity, and acceleration levels, with only
+ // holonomic constraints included at the position level,
+ // holonomic+nonholonic at the velocity level, and
+ // holonomic+nonholonomic+accelerationOnly coupled at the acceleration
+ // level.
+
+ /*
+ positionCoupledConstraints.clear();
+ velocityCoupledConstraints.clear();
+ accelerationCoupledConstraints.clear();
+
+ for (int i=0; i < (int)constraints.size(); ++i) {
+ if (!mc.mHolonomicEquationsInUse[i]) continue;
+
+ positionCoupledConstraints.push_back(CoupledConstraintSet(*constraints[i]));
+ CoupledConstraintSet& cset = positionCoupledConstraints.back();
+
+ }
+ */
+
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx)
+ getConstraint(cx).getImpl().realizeInstance(s);
+
+
+ // Quaternion errors are located after last holonomic constraint error;
+ // see diagram above.
+ ic.firstQuaternionQErrSlot = ic.totalNHolonomicConstraintEquationsInUse;
+
+ // We'll store the the physical constraint errors, followed by the
+ // quaternion constraints.
+ ic.qErrIndex = allocateQErr(s, ic.totalNHolonomicConstraintEquationsInUse
+ + mc.totalNQuaternionsInUse);
+
+ // Only physical constraints exist at the velocity and acceleration levels;
+ // the quaternion normalization constraints are gone.
+ ic.uErrIndex = allocateUErr (s, ic.totalNHolonomicConstraintEquationsInUse
+ + ic.totalNNonholonomicConstraintEquationsInUse);
+ ic.udotErrIndex = allocateUDotErr(s, ic.totalNHolonomicConstraintEquationsInUse
+ + ic.totalNNonholonomicConstraintEquationsInUse
+ + ic.totalNAccelerationOnlyConstraintEquationsInUse);
+
+ // ALLOCATE REMAINING CACHE ENTRIES
+ // Now that we know all the Instance-stage info, we can allocate (or
+ // reallocate) the rest of the cache entries.
+ updTimeCache(s).allocate(topologyCache, mc, ic);
+ updTreePositionCache(s).allocate(topologyCache, mc, ic);
+ updConstrainedPositionCache(s).allocate(topologyCache, mc, ic);
+ updCompositeBodyInertiaCache(s).allocate(topologyCache, mc, ic);
+ updArticulatedBodyInertiaCache(s).allocate(topologyCache, mc, ic);
+ updTreeVelocityCache(s).allocate(topologyCache, mc, ic);
+ updConstrainedVelocityCache(s).allocate(topologyCache, mc, ic);
+ updDynamicsCache(s).allocate(topologyCache, mc, ic);
+ updTreeAccelerationCache(s).allocate(topologyCache, mc, ic);
+ updConstrainedAccelerationCache(s).allocate(topologyCache, mc, ic);
+
+ // Now let the implementing RigidBodyNodes do their realization.
+ SBStateDigest stateDigest(s, *this, Stage::Instance);
+ for (int i=0 ; i<(int)rbNodeLevels.size() ; i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++)
+ rbNodeLevels[i][j]->realizeInstance(stateDigest);
+
+ return 0;
+}
+
+
+
+//==============================================================================
+// REALIZE TIME
+//==============================================================================
+int SimbodyMatterSubsystemRep::realizeSubsystemTimeImpl(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(s), Stage(Stage::Time).prev(),
+ "SimbodyMatterSubsystem::realizeTime()");
+
+ const SBStateDigest stateDigest(s, *this, Stage::Time);
+ const SBModelCache& mc = stateDigest.getModelCache();
+ const SBInstanceCache& ic = stateDigest.getInstanceCache();
+ SBTimeCache& tc = stateDigest.updTimeCache();
+
+ // the multibody tree cannot have time dependence
+
+ // Now realize the MobilizedBodies, which will realize prescribed positions
+ // if there are any; those will go into the TimeCache.
+ for (MobilizedBodyIndex mbx(0); mbx < mobilizedBodies.size(); ++mbx)
+ getMobilizedBody(mbx).getImpl().realizeTime(stateDigest);
+
+ // Constraints
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx)
+ getConstraint(cx).getImpl().realizeTime(stateDigest);
+
+ // We're done with the TimeCache now.
+ markCacheValueRealized(s, mc.timeCacheIndex);
+ return 0;
+}
+
+
+
+//==============================================================================
+// REALIZE POSITION
+//==============================================================================
+// The goals here are:
+// (1) fill in the TreePositionCache and mark it valid
+// (2) realize the remaining position dependencies (which may depend on
+// step (1)) with the result going into ConstrainedPositionCache & QErr
+//
+// In step (1) we take the q's from the State and sweep outward from Ground
+// through the multibody tree, calculating all position kinematics. Note that
+// we *do not* look at the prescribed q's in the TimeCache except to calculate
+// errors; unless someone has invoked a solver to update the State q's from
+// the prescribed values they will not be the same.
+//
+// In step (2) we calculate the matter subsystem's other position dependencies
+// which are:
+// - prescribed velocities (u's) --> PresUPool
+// - position constraint errors --> State's QErr array
+// These calculations, which may be user-written, depend on values from step (1)
+// being valid already, even though the stage won't yet have been advanced to
+// stage Position. So we explicitly mark the TreePositionCache valid as soon as
+// it is known.
+
+int SimbodyMatterSubsystemRep::realizeSubsystemPositionImpl(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(s), Stage(Stage::Position).prev(),
+ "SimbodyMatterSubsystem::realizePosition()");
+
+ // Set up StateDigest for calculating position information.
+ const SBStateDigest stateDigest(s, *this, Stage::Position);
+ const SBModelCache& mc = stateDigest.getModelCache();
+ const SBInstanceCache& ic = stateDigest.getInstanceCache();
+ SBTreePositionCache& tpc = stateDigest.updTreePositionCache();
+
+ // realize tree positions (kinematics)
+ // This includes all local cross-mobilizer kinematics (M in F, B in P)
+ // and all global kinematics relative to Ground (G).
+
+ // Any body which is using quaternions should calculate the quaternion
+ // constraint here and put it in the appropriate slot of qErr.
+ // Set generalized coordinates: sweep from base to tips.
+ for (int i=0 ; i<(int)rbNodeLevels.size() ; i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++)
+ rbNodeLevels[i][j]->realizePosition(stateDigest);
+
+ // Ask the constraints to calculate ancestor-relative kinematics (still
+ // goes in TreePositionCache).
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx)
+ getConstraint(cx).getImpl()
+ .calcConstrainedBodyTransformInAncestor(stateDigest, tpc);
+
+ // Now we're done with the TreePositionCache.
+ markCacheValueRealized(s, mc.treePositionCacheIndex);
+
+ // MobilizedBodies
+ // This will include writing the prescribed velocities (u's) into
+ // the PresUPool in the ConstrainedPositionCache.
+ for (MobilizedBodyIndex mbx(0); mbx < mobilizedBodies.size(); ++mbx)
+ getMobilizedBody(mbx).getImpl().realizePosition(stateDigest);
+
+
+ // Put position constraint equation errors in qErr
+ Vector& qErr = stateDigest.updQErr();
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx) {
+ if (isConstraintDisabled(s,cx))
+ continue;
+ const SBInstancePerConstraintInfo&
+ cInfo = ic.getConstraintInstanceInfo(cx);
+ const Segment& pseg = cInfo.holoErrSegment;
+ if (pseg.length) {
+ Real* perrp = &qErr[pseg.offset];
+ ArrayView_<Real> perr(perrp, perrp+pseg.length);
+ constraints[cx]->getImpl().calcPositionErrorsFromState(s, perr);
+ }
+ }
+
+ // Now we're done with the ConstrainedPositionCache.
+ markCacheValueRealized(s, mc.constrainedPositionCacheIndex);
+
+ // Constraints
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx)
+ getConstraint(cx).getImpl().realizePosition(stateDigest);
+ return 0;
+}
+
+
+
+//==============================================================================
+// REALIZE COMPOSITE BODY INERTIAS
+//==============================================================================
+void SimbodyMatterSubsystemRep::realizeCompositeBodyInertias(const State& state) const {
+ const CacheEntryIndex cbx = getModelCache(state).compositeBodyInertiaCacheIndex;
+
+ if (isCacheValueRealized(state, cbx))
+ return; // already realized
+
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(state), Stage::Position,
+ "SimbodyMatterSubsystem::realizeCompositeBodyInertias()");
+
+ SBCompositeBodyInertiaCache& cbc =
+ Value<SBCompositeBodyInertiaCache>::updDowncast(updCacheEntry(state, cbx));
+
+ calcCompositeBodyInertias(state, cbc.compositeBodyInertia);
+ markCacheValueRealized(state, cbx);
+}
+
+void SimbodyMatterSubsystemRep::
+invalidateCompositeBodyInertias(const State& state) const {
+ const CacheEntryIndex cbx =
+ getModelCache(state).compositeBodyInertiaCacheIndex;
+ markCacheValueNotRealized(state, cbx);
+}
+
+
+
+//==============================================================================
+// REALIZE ARTICULATED BODY INERTIAS
+//==============================================================================
+void SimbodyMatterSubsystemRep::
+realizeArticulatedBodyInertias(const State& state) const {
+ const CacheEntryIndex abx =
+ getModelCache(state).articulatedBodyInertiaCacheIndex;
+
+ if (isCacheValueRealized(state, abx))
+ return; // already realized
+
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(state), Stage::Position,
+ "SimbodyMatterSubsystem::realizeArticulatedBodyInertias()");
+
+ const SBInstanceCache& ic = getInstanceCache(state);
+ const SBTreePositionCache& tpc = getTreePositionCache(state);
+ SBArticulatedBodyInertiaCache& abc = updArticulatedBodyInertiaCache(state);
+
+ // tip-to-base sweep
+ for (int i=rbNodeLevels.size()-1 ; i>=0 ; --i)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; ++j)
+ rbNodeLevels[i][j]->realizeArticulatedBodyInertiasInward(ic,tpc,abc);
+
+ markCacheValueRealized(state, abx);
+}
+
+void SimbodyMatterSubsystemRep::
+invalidateArticulatedBodyInertias(const State& state) const {
+ // ABIs are assumed calculated at Dynamics stage, regardless of the
+ // flag in the cache entry.
+ state.invalidateAllCacheAtOrAbove(Stage::Dynamics);
+ const CacheEntryIndex abx =
+ getModelCache(state).articulatedBodyInertiaCacheIndex;
+ markCacheValueNotRealized(state, abx);
+}
+
+
+
+//==============================================================================
+// REALIZE VELOCITY
+//==============================================================================
+// The goals here are:
+// (1) fill in the TreeVelocityCache and mark it valid
+// (2) realize the remaining velocity dependencies (which may depend on
+// step (1)) with the result going into ConstrainedVelocityCache & UErr
+//
+// In step (1) we take the u's from the State and sweep outward from Ground
+// through the multibody tree, calculating all velocity kinematics. Note that
+// we *do not* look at the prescribed u's in the ConstrainedPositionCache except
+// to calculate errors; unless someone has invoked a solver to update the State
+// u's from the prescribed values they will not be the same.
+//
+// In step (2) we calculate the matter subsystem's other velocity dependencies
+// which are:
+// - (no need to do prescribed accelerations yet)
+// - velocity constraint errors --> State's UErr array
+// These calculations, which may be user-written, depend on values from step (1)
+// being valid already, even though the stage won't yet have been advanced to
+// stage Velocity. So we explicitly mark the TreeVelocityCache valid as soon as
+// it is known.
+int SimbodyMatterSubsystemRep::realizeSubsystemVelocityImpl(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(s), Stage(Stage::Velocity).prev(),
+ "SimbodyMatterSubsystem::realizeVelocity()");
+
+ const SBStateDigest stateDigest(s, *this, Stage::Velocity);
+ const SBModelCache& mc = stateDigest.getModelCache();
+ const SBInstanceCache& ic = stateDigest.getInstanceCache();
+ SBTreeVelocityCache& tvc = stateDigest.updTreeVelocityCache();
+
+ // realize tree velocity kinematics
+ // This includes all local cross-mobilizer velocities (M in F, B in P)
+ // and all global velocities relative to Ground (G).
+
+ // Set generalized speeds: sweep from base to tips.
+ for (int i=0 ; i<(int)rbNodeLevels.size() ; ++i)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; ++j)
+ rbNodeLevels[i][j]->realizeVelocity(stateDigest);
+
+ // Ask the constraints to calculate ancestor-relative velocity kinematics
+ // (still goes in TreePositionCache).
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx)
+ getConstraint(cx).getImpl()
+ .calcConstrainedBodyVelocityInAncestor(stateDigest, tvc);
+
+ // Now we're done with the TreeVelocityCache.
+ markCacheValueRealized(s, mc.treeVelocityCacheIndex);
+
+ // MobilizedBodies
+ // probably not much to do here
+ for (MobilizedBodyIndex mbx(0); mbx < mobilizedBodies.size(); ++mbx)
+ getMobilizedBody(mbx).getImpl().realizeVelocity(stateDigest);
+
+
+ // Put velocity constraint equation errors in uErr
+ Vector& uErr = stateDigest.updUErr();
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx) {
+ if (isConstraintDisabled(s,cx))
+ continue;
+ const SBInstancePerConstraintInfo&
+ cInfo = ic.getConstraintInstanceInfo(cx);
+
+ const Segment& holoseg = cInfo.holoErrSegment; // for derivs of holo constraints
+ const Segment& nonholoseg = cInfo.nonholoErrSegment; // includes holo+nonholo
+ const int mHolo = holoseg.length, mNonholo = nonholoseg.length;
+ if (mHolo) {
+ Real* pverrp = &uErr[holoseg.offset];
+ ArrayView_<Real> pverr(pverrp, pverrp+mHolo);
+ constraints[cx]->getImpl().calcPositionDotErrorsFromState(s, pverr);
+ }
+ if (mNonholo) {
+ Real* verrp = &uErr[ic.totalNHolonomicConstraintEquationsInUse
+ + nonholoseg.offset];
+ ArrayView_<Real> verr(verrp, verrp+mNonholo);
+ constraints[cx]->getImpl().calcVelocityErrorsFromState(s, verr);
+ }
+ }
+
+ // Now we're done with the ConstrainedVelocityCache.
+ markCacheValueRealized(s, mc.constrainedVelocityCacheIndex);
+
+ // Constraints
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx)
+ getConstraint(cx).getImpl().realizeVelocity(stateDigest);
+ return 0;
+}
+
+
+
+//==============================================================================
+// REALIZE DYNAMICS
+//==============================================================================
+// Prepare for dynamics by calculating position-dependent quantities like the
+// articulated body inertias P, and velocity-dependent quantities like the
+// Coriolis acceleration.
+
+int SimbodyMatterSubsystemRep::realizeSubsystemDynamicsImpl(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(s), Stage(Stage::Dynamics).prev(),
+ "SimbodyMatterSubsystem::realizeDynamics()");
+
+ // tip-to-base calculation
+ realizeArticulatedBodyInertias(s); // (may already have been realized)
+ const SBArticulatedBodyInertiaCache& abc = getArticulatedBodyInertiaCache(s);
+
+ SBStateDigest stateDigest(s, *this, Stage::Dynamics);
+
+ // Get the Dynamics-stage cache; it was already allocated at Instance stage.
+ SBDynamicsCache& dc = stateDigest.updDynamicsCache();
+
+ // Realize velocity-dependent articulated body quantities needed for
+ // dynamics: base-to-tip.
+ for (int i=0; i < (int)rbNodeLevels.size(); ++i)
+ for (int j=0; j < (int)rbNodeLevels[i].size(); ++j)
+ rbNodeLevels[i][j]->realizeDynamics(abc, stateDigest);
+
+ // MobilizedBodies
+ // This will include writing the prescribed accelerations into
+ // the udot array in the State.
+ for (MobilizedBodyIndex mbx(0); mbx < mobilizedBodies.size(); ++mbx)
+ getMobilizedBody(mbx).getImpl().realizeDynamics(stateDigest);
+
+ // Realize Constraint dynamics.
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx)
+ getConstraint(cx).getImpl().realizeDynamics(stateDigest);
+
+ return 0;
+}
+
+
+
+//==============================================================================
+// REALIZE ACCELERATION
+//==============================================================================
+int SimbodyMatterSubsystemRep::realizeSubsystemAccelerationImpl(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(s), Stage(Stage::Acceleration).prev(),
+ "SimbodyMatterSubsystem::realizeAcceleration()");
+
+ SBStateDigest stateDigest(s, *this, Stage::Acceleration);
+
+ // Get the Acceleration-stage cache entries. They were all allocated by
+ // the end of Instance stage.
+ Vector& udot = stateDigest.updUDot();
+ Vector& qdotdot = stateDigest.updQDotDot();
+ SBTreeAccelerationCache& tac = stateDigest.updTreeAccelerationCache();
+ SBConstrainedAccelerationCache& cac = stateDigest.updConstrainedAccelerationCache();
+
+ // We ask our containing MultibodySystem for a reference to the cached
+ // forces accumulated from all the force subsystems. We use these to
+ // compute accelerations, with all results going into the AccelerationCache.
+ const MultibodySystem& mbs = getMultibodySystem(); // owner of this subsystem
+ realizeLoopForwardDynamics(s,
+ mbs.getMobilityForces(s, Stage::Dynamics),
+ mbs.getParticleForces(s, Stage::Dynamics),
+ mbs.getRigidBodyForces(s, Stage::Dynamics));
+
+ // MobilizedBodies
+ for (MobilizedBodyIndex mbx(0); mbx < mobilizedBodies.size(); ++mbx)
+ getMobilizedBody(mbx).getImpl().realizeAcceleration(stateDigest);
+
+ // Constraints
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx)
+ getConstraint(cx).getImpl().realizeAcceleration(stateDigest);
+
+ return 0;
+}
+
+
+
+//==============================================================================
+// REALIZE REPORT
+//==============================================================================
+int SimbodyMatterSubsystemRep::realizeSubsystemReportImpl(const State& s) const {
+ SimTK_STAGECHECK_GE_ALWAYS(getStage(s), Stage(Stage::Report).prev(),
+ "SimbodyMatterSubsystem::realizeReport()");
+
+ // realize RB nodes report
+ SBStateDigest stateDigest(s, *this, Stage::Report);
+ for (int i=0 ; i<(int)rbNodeLevels.size() ; ++i)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; ++j)
+ rbNodeLevels[i][j]->realizeReport(stateDigest);
+
+ // MobilizedBodies
+ for (MobilizedBodyIndex mbx(0); mbx < mobilizedBodies.size(); ++mbx)
+ getMobilizedBody(mbx).getImpl().realizeReport(stateDigest);
+
+ // realize Constraint report
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx)
+ getConstraint(cx).getImpl().realizeReport(s);
+
+ return 0;
+}
+
+
+
+//==============================================================================
+// CALC DECORATIVE GEOMETRY AND APPEND
+//==============================================================================
+int SimbodyMatterSubsystemRep::calcDecorativeGeometryAndAppendImpl
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+{
+ // Let the bodies and mobilizers have a chance to generate some geometry.
+ for (MobilizedBodyIndex bx(0); bx<mobilizedBodies.size(); ++bx)
+ mobilizedBodies[bx]->getImpl().calcDecorativeGeometryAndAppend(s,stage,geom);
+
+ // Likewise for the constraints
+ for (ConstraintIndex cx(0); cx<constraints.size(); ++cx)
+ constraints[cx]->getImpl().calcDecorativeGeometryAndAppend(s,stage,geom);
+
+ // Now add in any subsystem-level geometry.
+ switch(stage) {
+ case Stage::Topology: {
+ assert(subsystemTopologyHasBeenRealized());
+ // none yet
+ break;
+ }
+ case Stage::Position: {
+ assert(getStage(s) >= Stage::Position);
+ //TODO: just to check control flow, put a ball at system COM
+ //const Vec3 com = getMyMatterSubsystemHandle().calcSystemMassCenterLocationInGround(s);
+ //geom.push_back(DecorativeSphere(0.02).setBodyId(GroundIndex).setTransform(com)
+ // .setColor(Green).setRepresentation(DecorativeGeometry::DrawPoints)
+ // .setResolution(1));
+ }
+ default:
+ assert(getStage(s) >= stage);
+ }
+
+ return 0;
+}
+
+// TODO: the weight for u_i should be something like the largest Dv_j/Du_i in
+// the system Jacobian, where v_j is body j's origin speed, with a lower limit
+// given by length scale (e.g. 1 unit).
+// Then the q_i weight should be obtained via dq = N*du.
+int SimbodyMatterSubsystemRep::calcQUnitWeightsImpl(const State& s, Vector& weights) const {
+ weights.resize(getNQ(s));
+ weights = 1;
+ return 0;
+}
+int SimbodyMatterSubsystemRep::calcUUnitWeightsImpl(const State& s, Vector& weights) const {
+ weights.resize(getNU(s));
+ weights = 1;
+ return 0;
+}
+
+int SimbodyMatterSubsystemRep::getQIndex(MobilizedBodyIndex body) const {
+ assert(subsystemTopologyHasBeenRealized());
+ return getRigidBodyNode(body).getQIndex();
+}
+int SimbodyMatterSubsystemRep::getQAlloc(MobilizedBodyIndex body) const {
+ assert(subsystemTopologyHasBeenRealized());
+ return getRigidBodyNode(body).getMaxNQ();
+}
+int SimbodyMatterSubsystemRep::getUIndex(MobilizedBodyIndex body) const {
+ assert(subsystemTopologyHasBeenRealized());
+ return getRigidBodyNode(body).getUIndex();
+}
+int SimbodyMatterSubsystemRep::getDOF(MobilizedBodyIndex body) const {
+ assert(subsystemTopologyHasBeenRealized());
+ return getRigidBodyNode(body).getDOF();
+}
+
+// We are in the process of realizeTopology() when we need to make this call.
+// We pass in the partially-completed Topology-stage cache, which must have all
+// the dimensions properly filled in at this point.
+void SimbodyMatterSubsystemRep::setDefaultModelValues(const SBTopologyCache& topologyCache,
+ SBModelVars& modelVars) const
+{
+ // Tree-level defaults
+ modelVars.useEulerAngles = false;
+
+ // Node/joint-level defaults
+ for (int i=0 ; i<(int)rbNodeLevels.size() ; i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++)
+ rbNodeLevels[i][j]->setNodeDefaultModelValues(topologyCache, modelVars);
+
+}
+
+void SimbodyMatterSubsystemRep::setDefaultInstanceValues(const SBModelVars& mv,
+ SBInstanceVars& iv) const
+{
+ // Node/joint-level defaults
+ for (int i=0 ; i<(int)rbNodeLevels.size() ; i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++)
+ rbNodeLevels[i][j]->setNodeDefaultInstanceValues(mv, iv);
+
+ assert((int)iv.mobilizerLockLevel.size() == getNumBodies());
+ assert((int)iv.prescribedMotionIsDisabled.size() == getNumBodies());
+ for (MobilizedBodyIndex mbx(0); mbx < getNumBodies(); ++mbx) {
+ const MobilizedBody& mobod = getMobilizedBody(mbx);
+ iv.mobilizerLockLevel[mbx] = mobod.getLockByDefaultLevel();
+ iv.prescribedMotionIsDisabled[mbx] =
+ mobod.hasMotion() ? mobod.getMotion().isDisabledByDefault()
+ : false;
+ }
+
+ assert((int)iv.constraintIsDisabled.size() == getNumConstraints());
+ for (ConstraintIndex cx(0); cx < getNumConstraints(); ++cx)
+ iv.constraintIsDisabled[cx] = getConstraint(cx).isDisabledByDefault();
+
+ // TODO: constraint defaults
+}
+
+void SimbodyMatterSubsystemRep::setDefaultTimeValues(const SBModelVars& mv,
+ SBTimeVars& timeVars) const
+{
+ // Tree-level defaults (none)
+
+ // Node/joint-level defaults
+ for (int i=0 ; i<(int)rbNodeLevels.size() ; i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++)
+ rbNodeLevels[i][j]->setNodeDefaultTimeValues(mv, timeVars);
+
+ // TODO: constraint defaults
+}
+
+void SimbodyMatterSubsystemRep::setDefaultPositionValues(const SBModelVars& mv, Vector& q) const
+{
+ // Tree-level defaults (none)
+
+
+ // Node/joint-level defaults
+ for (int i=0 ; i<(int)rbNodeLevels.size() ; i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++)
+ rbNodeLevels[i][j]->setNodeDefaultPositionValues(mv, q);
+
+ // TODO: constraint defaults
+}
+
+void SimbodyMatterSubsystemRep::setDefaultVelocityValues(const SBModelVars& mv, Vector& u) const
+{
+ // Tree-level defaults (none)
+
+ // Node/joint-level defaults
+ for (int i=0 ; i<(int)rbNodeLevels.size() ; i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++)
+ rbNodeLevels[i][j]->setNodeDefaultVelocityValues(mv, u);
+
+ // TODO: constraint defaults
+}
+
+void SimbodyMatterSubsystemRep::setDefaultDynamicsValues(const SBModelVars& mv,
+ SBDynamicsVars& dynamicsVars) const
+{
+ // Tree-level defaults (none)
+
+ // Node/joint-level defaults
+ for (int i=0 ; i<(int)rbNodeLevels.size() ; i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++)
+ rbNodeLevels[i][j]->setNodeDefaultDynamicsValues(mv, dynamicsVars);
+
+ // TODO: constraint defaults
+}
+
+void SimbodyMatterSubsystemRep::setDefaultAccelerationValues(const SBModelVars& mv,
+ SBAccelerationVars& accVars) const
+{
+ // Tree-level defaults (none)
+
+ // Node/joint-level defaults
+ for (int i=0 ; i<(int)rbNodeLevels.size() ; i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++)
+ rbNodeLevels[i][j]->setNodeDefaultAccelerationValues(mv, accVars);
+
+ // TODO: constraint defaults
+}
+
+void SimbodyMatterSubsystemRep::
+setUseEulerAngles(State& s, bool useAngles) const {
+ SBModelVars& modelVars = updModelVars(s); // check/adjust stage
+ modelVars.useEulerAngles = useAngles;
+}
+
+void SimbodyMatterSubsystemRep::
+setConstraintIsDisabled(State& s, ConstraintIndex constraint, bool disable) const {
+ SBInstanceVars& instanceVars = updInstanceVars(s); // check/adjust stage
+ instanceVars.constraintIsDisabled[constraint] = disable;
+}
+
+bool SimbodyMatterSubsystemRep::getUseEulerAngles(const State& s) const {
+ const SBModelVars& modelVars = getModelVars(s); // check stage
+ return modelVars.useEulerAngles;
+}
+
+bool SimbodyMatterSubsystemRep::isConstraintDisabled(const State& s, ConstraintIndex constraint) const {
+ const SBInstanceVars& instanceVars = getInstanceVars(s); // check stage
+ return instanceVars.constraintIsDisabled[constraint];
+}
+
+void SimbodyMatterSubsystemRep::
+convertToEulerAngles(const State& inputState, State& outputState) const {
+ outputState = inputState;
+ if (!getUseEulerAngles(inputState)) {
+ setUseEulerAngles(outputState, true);
+ getMultibodySystem().realizeModel(outputState);
+ const Vector& inputQ = inputState.getQ();
+ Vector& outputQ = outputState.updQ();
+ for (MobilizedBodyIndex i(0); i < getNumMobilizedBodies(); ++i) {
+ const RigidBodyNode& node = getRigidBodyNode(i);
+ node.convertToEulerAngles(inputQ, outputQ);
+ }
+ }
+}
+
+void SimbodyMatterSubsystemRep::
+convertToQuaternions(const State& inputState, State& outputState) const {
+ outputState = inputState;
+ if (getUseEulerAngles(inputState)) {
+ setUseEulerAngles(outputState, false);
+ getMultibodySystem().realizeModel(outputState);
+ const Vector& inputQ = inputState.getQ();
+ Vector& outputQ = outputState.updQ();
+ for (MobilizedBodyIndex i(0); i < getNumMobilizedBodies(); ++i) {
+ const RigidBodyNode& node = getRigidBodyNode(i);
+ node.convertToQuaternions(inputQ, outputQ);
+ }
+ }
+}
+
+
+bool SimbodyMatterSubsystemRep::
+isUsingQuaternion(const State& s, MobilizedBodyIndex body) const {
+ const RigidBodyNode& n = getRigidBodyNode(body);
+ MobilizerQIndex startOfQuaternion; // we don't need this information here
+ SBStateDigest sbs(s, *this, Stage::Model);
+ return n.isUsingQuaternion(sbs, startOfQuaternion);
+}
+
+int SimbodyMatterSubsystemRep::getNumQuaternionsInUse(const State& s) const {
+ const SBModelCache& mc = getModelCache(s); // must be >=Model stage
+ return mc.totalNQuaternionsInUse;
+}
+
+QuaternionPoolIndex SimbodyMatterSubsystemRep::getQuaternionPoolIndex
+ (const State& s, MobilizedBodyIndex body) const
+{
+ const SBModelCache& mc = getModelCache(s); // must be >=Model stage
+ return mc.getMobodModelInfo(body).quaternionPoolIndex;
+}
+
+int SimbodyMatterSubsystemRep::getNumHolonomicConstraintEquationsInUse(const State& s) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ return ic.totalNHolonomicConstraintEquationsInUse;
+}
+int SimbodyMatterSubsystemRep::getNumNonholonomicConstraintEquationsInUse(const State& s) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ return ic.totalNNonholonomicConstraintEquationsInUse;
+}
+int SimbodyMatterSubsystemRep::getNumAccelerationOnlyConstraintEquationsInUse(const State& s) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ return ic.totalNAccelerationOnlyConstraintEquationsInUse;
+}
+
+
+const Array_<QIndex>& SimbodyMatterSubsystemRep::
+getFreeQIndex(const State& state) const {
+ const SBInstanceCache& ic = getInstanceCache(state);
+ return ic.freeQ;
+}
+
+const Array_<QIndex>& SimbodyMatterSubsystemRep::
+getPresQIndex(const State& state) const {
+ const SBInstanceCache& ic = getInstanceCache(state);
+ return ic.presQ;
+}
+const Array_<QIndex>& SimbodyMatterSubsystemRep::
+getZeroQIndex(const State& state) const {
+ const SBInstanceCache& ic = getInstanceCache(state);
+ return ic.zeroQ;
+}
+
+const Array_<UIndex>& SimbodyMatterSubsystemRep::
+getFreeUIndex(const State& state) const {
+ const SBInstanceCache& ic = getInstanceCache(state);
+ return ic.freeU;
+}
+
+const Array_<UIndex>& SimbodyMatterSubsystemRep::
+getPresUIndex(const State& state) const {
+ const SBInstanceCache& ic = getInstanceCache(state);
+ return ic.presU;
+}
+const Array_<UIndex>& SimbodyMatterSubsystemRep::
+getZeroUIndex(const State& state) const {
+ const SBInstanceCache& ic = getInstanceCache(state);
+ return ic.zeroU;
+}
+
+const Array_<UIndex>& SimbodyMatterSubsystemRep::
+getFreeUDotIndex(const State& state) const {
+ const SBInstanceCache& ic = getInstanceCache(state);
+ return ic.freeUDot;
+}
+
+const Array_<UIndex>& SimbodyMatterSubsystemRep::
+getKnownUDotIndex(const State& state) const {
+ const SBInstanceCache& ic = getInstanceCache(state);
+ return ic.presForce;
+}
+
+
+
+//==============================================================================
+// PACK/UNPACK FREE Q/U, ZERO KNOWN Q/U
+//==============================================================================
+void SimbodyMatterSubsystemRep::
+packFreeQ(const State& s, const Vector& allQ,
+ Vector& packedFreeQ) const
+{
+ const Array_<QIndex>& freeQX = getFreeQIndex(s);
+ const int nq = getNQ(s);
+ const int nfq = freeQX.size();
+ assert(allQ.size() == nq);
+ assert(packedFreeQ.size() == nfq);
+
+ if (nfq==0) return; // all prescribed
+ if (nfq==nq) {packedFreeQ=allQ; return;} // none prescribed
+
+ if (allQ.hasContiguousData() && packedFreeQ.hasContiguousData()) {
+ const Real* allQp = &allQ[0];
+ Real* packedFreeQp = &packedFreeQ[0];
+ for (int i=0; i < nfq; ++i)
+ packedFreeQp[i] = allQp[freeQX[i]];
+ return;
+ }
+
+ // Slower copy for noncontiguous data.
+ for (int i=0; i < nfq; ++i)
+ packedFreeQ[i] = allQ[freeQX[i]];
+}
+
+void SimbodyMatterSubsystemRep::
+unpackFreeQ(const State& s, const Vector& packedFreeQ,
+ Vector& unpackedFreeQ) const
+{
+ const Array_<QIndex>& freeQX = getFreeQIndex(s);
+ const int nq = getNQ(s);
+ const int nfq = freeQX.size();
+ assert(packedFreeQ.size() == nfq);
+ assert(unpackedFreeQ.size() == nq);
+
+ if (nfq==0) return; // all prescribed
+ if (nfq==nq) {unpackedFreeQ=packedFreeQ; return;} // none prescribed
+
+ if (packedFreeQ.hasContiguousData() && unpackedFreeQ.hasContiguousData()) {
+ const Real* packedFreeQp = &packedFreeQ[0];
+ Real* unpackedFreeQp = &unpackedFreeQ[0];
+ for (int i=0; i < nfq; ++i)
+ unpackedFreeQp[freeQX[i]] = packedFreeQp[i];
+ return;
+ }
+
+ // Slower copy for noncontiguous data.
+ for (int i=0; i < nfq; ++i)
+ unpackedFreeQ[freeQX[i]] = packedFreeQ[i];
+}
+
+
+void SimbodyMatterSubsystemRep::
+zeroKnownQ(const State& s, Vector& qlike) const
+{ // known = prescribed + zero
+ const int nq = getNQ(s);
+ assert(qlike.size() == nq);
+
+ const Array_<QIndex>& presQX = getPresQIndex(s);
+ const Array_<QIndex>& zeroQX = getZeroQIndex(s);
+ const int npq = presQX.size();
+ const int nzq = zeroQX.size();
+
+ if (npq==0 && nzq==0) return; // all q's are free
+
+ if (qlike.hasContiguousData()) {
+ Real* qp = &qlike[0];
+ for (int i=0; i < npq; ++i)
+ qp[presQX[i]] = 0;
+ for (int i=0; i < nzq; ++i)
+ qp[zeroQX[i]] = 0;
+ return;
+ }
+
+ // Slower zeroing for noncontiguous data.
+ for (int i=0; i < npq; ++i)
+ qlike[presQX[i]] = 0;
+ for (int i=0; i < nzq; ++i)
+ qlike[zeroQX[i]] = 0;
+}
+
+void SimbodyMatterSubsystemRep::
+packFreeU(const State& s, const Vector& allU,
+ Vector& packedFreeU) const
+{
+ const Array_<UIndex>& freeUX = getFreeUIndex(s);
+ const int nu = getNU(s);
+ const int nfu = freeUX.size();
+ assert(allU.size() == nu);
+ assert(packedFreeU.size() == nfu);
+
+ if (nfu==0) return; // all prescribed
+ if (nfu==nu) {packedFreeU=allU; return;} // none prescribed
+
+ if (allU.hasContiguousData() && packedFreeU.hasContiguousData()) {
+ const Real* allUp = &allU[0];
+ Real* packedFreeUp = &packedFreeU[0];
+ for (int i=0; i < nfu; ++i)
+ packedFreeUp[i] = allUp[freeUX[i]];
+ return;
+ }
+
+ // Slower copy for noncontiguous data.
+ for (int i=0; i < nfu; ++i)
+ packedFreeU[i] = allU[freeUX[i]];
+}
+
+void SimbodyMatterSubsystemRep::
+unpackFreeU(const State& s, const Vector& packedFreeU,
+ Vector& unpackedFreeU) const
+{
+ const Array_<UIndex>& freeUX = getFreeUIndex(s);
+ const int nu = getNU(s);
+ const int nfu = freeUX.size();
+ assert(packedFreeU.size() == nfu);
+ assert(unpackedFreeU.size() == nu);
+
+ if (nfu==0) return; // all prescribed
+ if (nfu==nu) {unpackedFreeU=packedFreeU; return;} // none prescribed
+
+ if (packedFreeU.hasContiguousData() && unpackedFreeU.hasContiguousData()) {
+ const Real* packedFreeUp = &packedFreeU[0];
+ Real* unpackedFreeUp = &unpackedFreeU[0];
+ for (int i=0; i < nfu; ++i)
+ unpackedFreeUp[freeUX[i]] = packedFreeUp[i];
+ return;
+ }
+
+ // Slower copy for noncontiguous data.
+ for (int i=0; i < nfu; ++i)
+ unpackedFreeU[freeUX[i]] = packedFreeU[i];
+}
+
+
+void SimbodyMatterSubsystemRep::
+zeroKnownU(const State& s, Vector& ulike) const
+{ // known = prescribed + zero
+ const int nu = getNU(s);
+ assert(ulike.size() == nu);
+
+ const Array_<UIndex>& presUX = getPresUIndex(s);
+ const Array_<UIndex>& zeroUX = getZeroUIndex(s);
+ const int npu = presUX.size();
+ const int nzu = zeroUX.size();
+
+ if (npu==0 && nzu==0) return; // all u's are free
+
+ if (ulike.hasContiguousData()) {
+ Real* up = &ulike[0];
+ for (int i=0; i < npu; ++i)
+ up[presUX[i]] = 0;
+ for (int i=0; i < nzu; ++i)
+ up[zeroUX[i]] = 0;
+ return;
+ }
+
+ // Slower zeroing for noncontiguous data.
+ for (int i=0; i < npu; ++i)
+ ulike[presUX[i]] = 0;
+ for (int i=0; i < nzu; ++i)
+ ulike[zeroUX[i]] = 0;
+}
+
+//==============================================================================
+// OBSOLETE -- see calcPq()
+//==============================================================================
+void SimbodyMatterSubsystemRep::calcHolonomicConstraintMatrixPNInv(const State& s, Matrix& PNInv) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const int mp = ic.totalNHolonomicConstraintEquationsInUse;
+ const int nq = getNQ(s);
+
+ PNInv.resize(mp,nq);
+
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx) {
+ if (isConstraintDisabled(s,cx))
+ continue;
+ const SBInstancePerConstraintInfo&
+ cInfo = ic.getConstraintInstanceInfo(cx);
+ const Segment& holoSeg = cInfo.holoErrSegment; // offset into qErr and mHolo (mp)
+
+ PNInv(holoSeg.offset, 0, holoSeg.length, nq) =
+ constraints[cx]->calcPositionConstraintMatrixPNInv(s);
+ }
+}
+
+//==============================================================================
+// CALC HOLONOMIC VELOCITY CONSTRAINT MATRIX P
+//==============================================================================
+void SimbodyMatterSubsystemRep::calcHolonomicVelocityConstraintMatrixP(const State& s, Matrix& P) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const int mHolo = ic.totalNHolonomicConstraintEquationsInUse;
+ const int nu = getNU(s);
+
+ P.resize(mHolo,nu);
+
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx) {
+ if (isConstraintDisabled(s,cx))
+ continue;
+ const SBInstancePerConstraintInfo&
+ cInfo = ic.getConstraintInstanceInfo(cx);
+ const Segment& holoSeg = cInfo.holoErrSegment; // offset into uErr and mHolo (mp)
+
+ P(holoSeg.offset, 0, holoSeg.length, nu) =
+ constraints[cx]->calcPositionConstraintMatrixP(s);
+ }
+}
+//==============================================================================
+// CALC HOLONOMIC VELOCITY CONSTRAINT MATRIX P^T
+//==============================================================================
+void SimbodyMatterSubsystemRep::calcHolonomicVelocityConstraintMatrixPt(const State& s, Matrix& Pt) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const int mHolo = ic.totalNHolonomicConstraintEquationsInUse;
+ const int nu = getNU(s);
+
+ Pt.resize(nu,mHolo);
+
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx) {
+ if (isConstraintDisabled(s,cx))
+ continue;
+ const SBInstancePerConstraintInfo&
+ cInfo = ic.getConstraintInstanceInfo(cx);
+ const Segment& holoSeg = cInfo.holoErrSegment; // offset into uErr and mHolo (mp)
+
+ // Fill in columns of Pt
+ Pt(0, holoSeg.offset, nu, holoSeg.length) =
+ constraints[cx]->calcPositionConstraintMatrixPt(s);
+ }
+}
+
+//==============================================================================
+// CALC NONHOLONOMIC CONSTRAINT MATRIX V
+//==============================================================================
+void SimbodyMatterSubsystemRep::calcNonholonomicConstraintMatrixV(const State& s, Matrix& V) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const int mNonholo = ic.totalNNonholonomicConstraintEquationsInUse;
+ const int nu = getNU(s);
+
+ V.resize(mNonholo,nu);
+
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx) {
+ if (isConstraintDisabled(s,cx))
+ continue;
+ const SBInstancePerConstraintInfo&
+ cInfo = ic.getConstraintInstanceInfo(cx);
+ const Segment& nonholoSeg = cInfo.nonholoErrSegment; // after holo derivs, offset into uerr
+
+ V(nonholoSeg.offset, 0, nonholoSeg.length, nu) =
+ constraints[cx]->calcVelocityConstraintMatrixV(s);
+ }
+}
+
+//==============================================================================
+// CALC NONHOLONOMIC CONSTRAINT MATRIX V^T
+//==============================================================================
+void SimbodyMatterSubsystemRep::calcNonholonomicConstraintMatrixVt(const State& s, Matrix& Vt) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const int mNonholo = ic.totalNNonholonomicConstraintEquationsInUse;
+ const int nu = getNU(s);
+
+ Vt.resize(nu,mNonholo);
+
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx) {
+ if (isConstraintDisabled(s,cx))
+ continue;
+ const SBInstancePerConstraintInfo&
+ cInfo = ic.getConstraintInstanceInfo(cx);
+ const Segment& nonholoSeg = cInfo.nonholoErrSegment; // after holo derivs, offset into uerr
+
+ // Fill in columns of Vt
+ Vt(0, nonholoSeg.offset, nu, nonholoSeg.length) =
+ constraints[cx]->calcVelocityConstraintMatrixVt(s);
+ }
+}
+//==============================================================================
+// CALC ACCELERATION ONLY CONSTRAINT MATRIX A
+//==============================================================================
+void SimbodyMatterSubsystemRep::calcAccelerationOnlyConstraintMatrixA (const State& s, Matrix& A) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const int mAccOnly = ic.totalNAccelerationOnlyConstraintEquationsInUse;
+ const int nu = getNU(s); // also nudot
+
+ A.resize(mAccOnly,nu);
+
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx) {
+ if (isConstraintDisabled(s,cx))
+ continue;
+ const SBInstancePerConstraintInfo&
+ cInfo = ic.getConstraintInstanceInfo(cx);
+ const Segment& accOnlySeg = cInfo.accOnlyErrSegment; // after holo&nonholo derivs, offset into udoterr
+
+ A(accOnlySeg.offset, 0, accOnlySeg.length, nu) =
+ constraints[cx]->calcAccelerationConstraintMatrixA(s);
+ }
+}
+//==============================================================================
+// CALC ACCELERATION ONLY CONSTRAINT MATRIX A^T
+//==============================================================================
+void SimbodyMatterSubsystemRep::calcAccelerationOnlyConstraintMatrixAt(const State& s, Matrix& At) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const int mAccOnly = ic.totalNAccelerationOnlyConstraintEquationsInUse;
+ const int nu = getNU(s); // also nudot
+
+ At.resize(nu,mAccOnly);
+
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx) {
+ if (isConstraintDisabled(s,cx))
+ continue;
+ const SBInstancePerConstraintInfo&
+ cInfo = ic.getConstraintInstanceInfo(cx);
+ const Segment& accOnlySeg = cInfo.accOnlyErrSegment; // after holo&nonholo derivs, offset into udoterr
+
+ At(0, accOnlySeg.offset, nu, accOnlySeg.length) =
+ constraints[cx]->calcAccelerationConstraintMatrixAt(s);
+ }
+}
+
+
+
+//==============================================================================
+// CALC CONSTRAINT FORCES FROM MULTIPLIERS
+//==============================================================================
+// Must be realized to Stage::Velocity.
+// Note that constraint forces have the opposite sign from applied forces,
+// because we calculate multipliers from
+// M udot + ~G lambda = f_applied
+// If you want to view the constraint generated forces as though they
+// were applied forces, negate lambda before the call here. This method
+// returns the individual constraint force contributions as well as returning
+// the combined forces.
+void SimbodyMatterSubsystemRep::
+calcConstraintForcesFromMultipliers
+ (const State& s,
+ const Vector& lambda,
+ Vector_<SpatialVec>& bodyForcesInG,
+ Vector& mobilityForces,
+ Array_<SpatialVec>& consBodyForcesInG,
+ Array_<Real>& consMobilityForces) const
+{
+ const SBInstanceCache& ic = getInstanceCache(s);
+
+ // Global problem dimensions.
+ const int mHolo = ic.totalNHolonomicConstraintEquationsInUse;
+ const int mNonholo = ic.totalNNonholonomicConstraintEquationsInUse;
+ const int mAccOnly = ic.totalNAccelerationOnlyConstraintEquationsInUse;
+ const int m = mHolo+mNonholo+mAccOnly;
+
+ assert(lambda.size() == m);
+
+ // These must be zeroed because we'll be adding in constraint force
+ // contributions and multiple constraints may generate forces on the
+ // same body or mobility.
+ bodyForcesInG.resize(getNumBodies()); bodyForcesInG.setToZero();
+ mobilityForces.resize(getNU(s)); mobilityForces.setToZero();
+
+ // These Arrays are for one constraint at a time.
+ Array_<Real> lambdap, lambdav, lambdaa; // multipliers
+
+ // Loop over all enabled constraints, ask them to generate forces, and
+ // accumulate the results in the global problem return vectors.
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx) {
+ if (isConstraintDisabled(s,cx))
+ continue;
+
+ const ConstraintImpl& crep = constraints[cx]->getImpl();
+
+ // No heap allocation is being done here. These are views directly
+ // into the proper segment of the longer array.
+ ArrayView_<SpatialVec,ConstrainedBodyIndex> bodyF1_G =
+ crep.updConstrainedBodyForces(s, consBodyForcesInG);
+ ArrayView_<Real,ConstrainedUIndex> mobilityF1 =
+ crep.updConstrainedMobilityForces(s, consMobilityForces);
+
+ const int ncb = bodyF1_G.size();
+ const int ncu = mobilityF1.size();
+
+ // These have to be zeroed because a Constraint force method is not
+ // *required* to apply forces to all its bodies and mobilities.
+ bodyF1_G.fill(SpatialVec(Vec3(0), Vec3(0)));
+ mobilityF1.fill(Real(0));
+
+ const SBInstancePerConstraintInfo&
+ cInfo = ic.getConstraintInstanceInfo(cx);
+
+ // Find this Constraint's multipliers within the global array.
+ const Segment& holoSeg = cInfo.holoErrSegment;
+ const Segment& nonholoSeg = cInfo.nonholoErrSegment;
+ const Segment& accOnlySeg = cInfo.accOnlyErrSegment;
+ const int mp=holoSeg.length, mv=nonholoSeg.length,
+ ma=accOnlySeg.length;
+
+ // Pack the multipliers into small arrays lambdap for holonomic 2nd
+ // derivs, labmdav for nonholonomic 1st derivs, and lambda for
+ // acceleration-only.
+ // Note: these lengths are *very* small integers!
+ lambdap.resize(mp); lambdav.resize(mv); lambdaa.resize(ma);
+ for (int i=0; i<mp; ++i)
+ lambdap[i] = lambda[ holoSeg.offset + i];
+ for (int i=0; i<mv; ++i)
+ lambdav[i] = lambda[mHolo + nonholoSeg.offset + i];
+ for (int i=0; i<ma; ++i)
+ lambdaa[i] = lambda[mHolo+mNonholo + accOnlySeg.offset + i];
+
+ // Generate forces for this Constraint. Body forces will come back
+ // in the A frame; if that's not Ground then we have to re-express
+ // them in Ground before moving on.
+ crep.calcConstraintForcesFromMultipliers
+ (s, lambdap, lambdav, lambdaa, bodyF1_G, mobilityF1);
+ if (crep.isAncestorDifferentFromGround()) {
+ const Rotation& R_GA =
+ crep.getAncestorMobilizedBody().getBodyRotation(s);
+ for (ConstrainedBodyIndex cbx(0); cbx < ncb; ++cbx)
+ bodyF1_G[cbx] = R_GA*bodyF1_G[cbx]; // 30 flops
+ }
+
+ // Unpack constrained body forces and add them to the proper slots
+ // in the global body forces array. They are already expressed in
+ // the Ground frame.
+ for (ConstrainedBodyIndex cbx(0); cbx < ncb; ++cbx)
+ bodyForcesInG[crep.getMobilizedBodyIndexOfConstrainedBody(cbx)]
+ += bodyF1_G[cbx]; // 6 flops
+
+ // Unpack constrained mobility forces and add them into global array.
+ for (ConstrainedUIndex cux(0); cux < ncu; ++cux)
+ mobilityForces[cInfo.getUIndexFromConstrainedU(cux)]
+ += mobilityF1[cux]; // 1 flop
+ }
+}
+
+
+
+//==============================================================================
+// MULTIPLY BY PVA TRANSPOSE
+//==============================================================================
+// We have these constraint equations available:
+// (1) [Fp,fp] = pforces(t,q; lambdap) holonomic (position)
+// (2) [Fv,fv] = vforces(t,q,u; lambdav) non-holonomic (velocity)
+// (3) [Fa,fa] = aforces(t,q,u; lambdaa) acceleration-only
+// where F denotes body spatial forces and f denotes u-space generalized forces.
+//
+// such that ~J*Fp+fp = ~P*lambdap
+// ~J*Fv+fv = ~V*lambdav
+// and ~J*Fa+fa = ~A*lambdaa
+//
+// with P=P(t,q), V=V(t,q,u), A=A(t,q,u). Note that P is the u-space matrix
+// P=Dperrdot/Du, *not* the q-space matrix Pq=Dperr/Dq=P*N^-1.
+// (See multiplyByPqTranspose() to work conveniently with Pq.)
+//
+// Here we will use those equations to perform the multiplications by the
+// matrices ~P,~V, and/or ~A times a multiplier-like vector:
+// [lambdap]
+// (4) fu = ~G*lambda = [~P ~V ~A] * [lambdav] = ~J*(Fp+Fv+Fa) + (fp+fv+fa).
+// [lambdaa]
+//
+// Individual constraint force equations are calculated in constant time, so
+// the whole multiplication can be done in O(m) time where m=mp+mv+ma is the
+// total number of active constraint equations.
+//
+// In general the state must be realized through Velocity stage, but if the
+// system contains only holonomic constraints, or if only ~P is included, then
+// the result is only time- and position-dependent since we just need to use
+// equation (1). In that case we require only that the state be realized to
+// stage Position.
+//
+// All of the Vector arguments must use contiguous storage.
+// Complexity is O(m+n).
+void SimbodyMatterSubsystemRep::
+multiplyByPVATranspose( const State& s,
+ bool includeP,
+ bool includeV,
+ bool includeA,
+ const Vector& lambda,
+ Vector& allfuVector) const
+{
+ const SBInstanceCache& ic = getInstanceCache(s);
+
+ // Global problem dimensions.
+ const int mHolo = includeP ?
+ ic.totalNHolonomicConstraintEquationsInUse : 0;
+ const int mNonholo = includeV ?
+ ic.totalNNonholonomicConstraintEquationsInUse : 0;
+ const int mAccOnly = includeA ?
+ ic.totalNAccelerationOnlyConstraintEquationsInUse : 0;
+ const int m = mHolo+mNonholo+mAccOnly;
+
+ const int nu = getNU(s);
+ const int nb = getNumBodies();
+
+ assert(lambda.size() == m);
+ assert(lambda.hasContiguousData());
+
+ allfuVector.resize(nu);
+ assert(allfuVector.hasContiguousData());
+ if (nu==0) return;
+ if (m==0) {allfuVector.setToZero(); return;}
+
+ // Allocate a temporary body forces vector here. We'll map these to
+ // generalized forces as the penultimate step, then add those into
+ // the output argument allfuVector which will have already accumulated
+ // all directly-generated mobility forces.
+ Vector_<SpatialVec> allF_GVector(nb);
+
+ // We'll be accumulating constraint forces into these Vectors so zero
+ // them now. Multiple constraints may contribute to forces on the same
+ // body or mobility.
+ allF_GVector.setToZero();
+ allfuVector.setToZero();
+
+ // Overlay arrays on the contiguous Vector memory for faster elementwise
+ // access. Any of the lambda segments may be empty.
+ const Real* first = &lambda[0];
+ ArrayViewConst_<Real> allLambdap(first, first+mHolo);
+ ArrayViewConst_<Real> allLambdav(first+mHolo,
+ first+mHolo+mNonholo);
+ ArrayViewConst_<Real> allLambdaa(first+mHolo+mNonholo,
+ first+mHolo+mNonholo+mAccOnly);
+
+ ArrayView_<SpatialVec> allF_G(&allF_GVector[0], &allF_GVector[0] + nb);
+ ArrayView_<Real> allfu (&allfuVector[0], &allfuVector[0] + nu);
+
+ // These Arrays are for one constraint at a time. We need separate
+ // memory for these because constrained bodies and constrained u's are
+ // not ordered the same as the global ones, nor are they necessarily
+ // contiguous in the global arrays. We're declaring these arrays
+ // outside the loop to avoid heap allocation -- they will grow to the
+ // max size needed by any constraint, then get resized as needed without
+ // further heap allocation.
+ Array_<SpatialVec,ConstrainedBodyIndex> oneF_G; // body spatial forces
+ Array_<Real, ConstrainedUIndex> onefu; // u-space generalized forces
+ Array_<Real, ConstrainedQIndex> onefq; // q-space generalized forces
+
+ // Loop over all enabled constraints, ask them to generate forces, and
+ // accumulate the results in the global problem arrays (allF_G,allfu).
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx) {
+ if (isConstraintDisabled(s,cx))
+ continue;
+
+ const ConstraintImpl& crep = constraints[cx]->getImpl();
+ const SBInstancePerConstraintInfo&
+ cInfo = ic.getConstraintInstanceInfo(cx);
+ const int ncb = crep.getNumConstrainedBodies();
+ const int ncu = cInfo.getNumConstrainedU();
+
+ // These have to be zeroed because we're accumulating forces from
+ // each of the three possible constraint levels below.
+ oneF_G.resize(ncb); onefu.resize(ncu);
+ oneF_G.fill(SpatialVec(Vec3(0))); onefu.fill(Real(0));
+
+ // Find this Constraint's multiplier segments within the global array.
+ // (These match the acceleration error segments.)
+ const Segment& holoSeg = cInfo.holoErrSegment;
+ const Segment& nonholoSeg = cInfo.nonholoErrSegment;
+ const Segment& accOnlySeg = cInfo.accOnlyErrSegment;
+ const int mp = includeP ? holoSeg.length : 0;
+ const int mv = includeV ? nonholoSeg.length : 0;
+ const int ma = includeA ? accOnlySeg.length : 0;
+
+ // Now generate forces. Body forces will come back in the A frame;
+ // if that's not Ground then we have to re-express them in Ground
+ // before moving on.
+ if (mp) {
+ const int ncq = cInfo.getNumConstrainedQ();
+ onefq.resize(ncq); onefq.fill(Real(0));
+ ArrayViewConst_<Real> lambdap(&allLambdap[holoSeg.offset],
+ &allLambdap[holoSeg.offset]+mp);
+ crep.addInPositionConstraintForces(s, lambdap, oneF_G, onefq);
+ // OK just to write on onefu here because it is zero.
+ crep.convertQForcesToUForces(s, onefq, onefu);
+ }
+ if (mv) {
+ ArrayViewConst_<Real> lambdav(&allLambdav[nonholoSeg.offset],
+ &allLambdav[nonholoSeg.offset]+mv);
+ crep.addInVelocityConstraintForces(s, lambdav, oneF_G, onefu);
+ }
+ if (ma) {
+ ArrayViewConst_<Real> lambdaa(&allLambdaa[accOnlySeg.offset],
+ &allLambdaa[accOnlySeg.offset]+ma);
+ crep.addInAccelerationConstraintForces(s, lambdaa, oneF_G, onefu);
+ }
+
+ // Fix expressed-in frame for body forces if necessary.
+ if (crep.isAncestorDifferentFromGround()) {
+ const Rotation& R_GA =
+ crep.getAncestorMobilizedBody().getBodyRotation(s);
+ for (ConstrainedBodyIndex cbx(0); cbx < ncb; ++cbx)
+ oneF_G[cbx] = R_GA*oneF_G[cbx]; // 30 flops
+ }
+
+ // Unpack constrained body forces and add them to the proper slots
+ // in the global body forces array.
+ for (ConstrainedBodyIndex cbx(0); cbx < ncb; ++cbx)
+ allF_G[crep.getMobilizedBodyIndexOfConstrainedBody(cbx)]
+ += oneF_G[cbx]; // 6 flops per constrained body
+
+ // Unpack constrained mobility forces and add them into global array.
+ // (1 flop per constrained mobility).
+ for (ConstrainedUIndex cux(0); cux < ncu; ++cux)
+ allfu[cInfo.getUIndexFromConstrainedU(cux)] += onefu[cux];
+ }
+
+
+ // Map the body forces into u-space generalized forces.
+ // 12*nu + 18*nb flops.
+ Vector ftmp(nu);
+ multiplyBySystemJacobianTranspose(s, allF_GVector, ftmp);
+ allfuVector += ftmp;
+}
+
+
+
+//==============================================================================
+// CALC PVA TRANSPOSE
+//==============================================================================
+// Arranges for contiguous workspace if necessary, then makes repeated calls
+// to multiplyByPVATranspose() to compute one column at a time of ~G.
+// Complexity is O(m^2 + m*n) = O(m*n).
+void SimbodyMatterSubsystemRep::
+calcPVATranspose( const State& s,
+ bool includeP,
+ bool includeV,
+ bool includeA,
+ Matrix& PVAt) const
+ {
+ const SBInstanceCache& ic = getInstanceCache(s);
+
+ // Global problem dimensions.
+ const int mHolo = includeP ?
+ ic.totalNHolonomicConstraintEquationsInUse : 0;
+ const int mNonholo = includeV ?
+ ic.totalNNonholonomicConstraintEquationsInUse : 0;
+ const int mAccOnly = includeA ?
+ ic.totalNAccelerationOnlyConstraintEquationsInUse : 0;
+ const int m = mHolo+mNonholo+mAccOnly;
+
+ const int nu = getNU(s);
+
+ PVAt.resize(nu,m);
+ if (m==0 || nu==0)
+ return;
+
+ Vector lambda(m, Real(0));
+
+ // If PVAt's columns are contiguous we can avoid copying.
+ const bool isContiguous = PVAt(0).hasContiguousData();
+ Vector contig_col(isContiguous ? 0 : m);
+ for (int j=0; j < m; ++j) {
+ lambda[j] = 1; // column we're working on
+ if (isContiguous) {
+ multiplyByPVATranspose(s, includeP, includeV, includeA, lambda,
+ PVAt(j));
+ } else {
+ multiplyByPVATranspose(s, includeP, includeV, includeA, lambda,
+ contig_col);
+ PVAt(j) = contig_col;
+ }
+ lambda[j] = 0;
+ }
+}
+
+
+
+//==============================================================================
+// MULTIPLY BY Pq TRANSPOSE
+//==============================================================================
+// First we calculate the u-space result fu=~P*lambdap, then we map that
+// result into q-space via fq=N^-T*fu.
+// See multiplyByPVATranspose() above for an explanation.
+// Vectors lambdap and fq must be using contiguous storage.
+// Complexity is O(mp + n).
+void SimbodyMatterSubsystemRep::
+multiplyByPqTranspose( const State& state,
+ const Vector& lambdap,
+ Vector& fq) const
+{
+ Vector fu;
+ // Calculate fu = ~P*lambdap.
+ multiplyByPVATranspose(state, true, false, false, lambdap, fu);
+ // Calculate fq = ~(N^-1) * fu = ~(~fu * N^-1)
+ multiplyByNInv(state, true/*transpose*/,fu,fq);
+}
+
+
+
+//==============================================================================
+// CALC Pq TRANSPOSE
+//==============================================================================
+// Arranges for contiguous workspace if necessary, then makes repeated calls
+// to multiplyByPqTranspose() to compute one column at a time of ~Pq.
+// Complexity is O(mp*mp + mp*n) = O(mp*n).
+void SimbodyMatterSubsystemRep::
+calcPqTranspose(const State& s, Matrix& Pqt) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+
+ // Global problem dimensions.
+ const int mp = ic.totalNHolonomicConstraintEquationsInUse;
+ const int nq = getNQ(s);
+
+ Pqt.resize(nq, mp);
+ if (mp==0 || nq==0)
+ return;
+
+ Vector lambdap(mp, Real(0));
+
+ // If Pqt's columns are contiguous we can avoid copying.
+ const bool isContiguous = Pqt(0).hasContiguousData();
+ Vector contig_col(isContiguous ? 0 : nq);
+
+ for (int i=0; i < mp; ++i) {
+ lambdap[i] = 1; // column we're working on
+ if (isContiguous) {
+ multiplyByPqTranspose(s, lambdap, Pqt(i));
+ } else {
+ multiplyByPqTranspose(s, lambdap, contig_col);
+ Pqt(i) = contig_col;
+ }
+ lambdap[i] = 0;
+ }
+}
+
+
+
+//==============================================================================
+// CALC WEIGHTED Pq_r TRANSPOSE
+//==============================================================================
+// The full Pq matrix is mp X nq. We want the mp X nfq submatrix that retains
+// only the columns that correspond to free (not prescribed) q's; call that
+// Pq_r. Also, we want the rows scaled by 1/constraint tolerances and retained
+// columns scaled by 1/q weights; call that Pqw_r. And we're actually going to
+// compute the nfq X mp transpose ~Pqw_r, which we'll call Pqw_rt.
+//
+// Pq = P N^+
+// Pqw = Tp Pq Wq^+
+// = Tp Pq (N Wu^-1 N^+)
+// = (Tp P Wu^-1) N^+ (because N^+ N = I)
+// = Pw N^+
+// Pqwt = ~Pqw = ~N^+ Wu^-1 ~P Tp (weights are symmetric)
+// (= ~N^+ ~Pw)
+//
+// Pqw_rt = Pqwt submatrix with rows removed if they correspond to q_p.
+//
+// We calculate one column at a time to avoid any matrix ops. We can do the
+// column scaling of ~P by Tp for free, but the row scaling requires nq*mp flops.
+// Return matrix Pqw_rt must have contiguous-data columns.
+void SimbodyMatterSubsystemRep::
+calcWeightedPqrTranspose(
+ const State& s,
+ const Vector& Tp, // 1/perr tols (mp)
+ const Vector& ooWu, // 1/u weights (nu)
+ Matrix& Pqw_rt) const // nfq X mp
+{
+ const SBInstanceCache& ic = getInstanceCache(s);
+
+ // Global problem dimensions.
+ const int mp = ic.totalNHolonomicConstraintEquationsInUse;
+ const int nu = getNU(s);
+ const int nq = getNQ(s);
+ const int nfq = ic.getTotalNumFreeQ();
+ assert(nfq <= nq);
+ const bool mustPack = (nfq != nq);
+
+ assert(Tp.size() == mp);
+ assert(ooWu.size() == nu);
+
+ Pqw_rt.resize(nfq, mp);
+ if (mp==0 || nfq==0)
+ return;
+
+ assert(Pqw_rt(0).hasContiguousData());
+
+ Vector Ptcol(nu), PNInvtcol;
+ Vector lambdap(mp, Real(0));
+
+ for (int j=0; j < mp; ++j) {
+ lambdap[j] = Tp[j]; // this gives column j of ~P scaled by Tp[j]
+ multiplyByPVATranspose(s, true, false, false, lambdap, Ptcol);
+ lambdap[j] = 0;
+ Ptcol.rowScaleInPlace(ooWu); // now (Wu^-1 ~P Tp)_i
+ // Calculate (~Pqw)(j) = ~(N^+) * (~Pw)(j)
+ if (mustPack) {
+ multiplyByNInv(s, true/*transpose*/,Ptcol,PNInvtcol);
+ packFreeQ(s, PNInvtcol, Pqw_rt(j));
+ } else
+ multiplyByNInv(s, true/*transpose*/,Ptcol,Pqw_rt(j));
+ }
+}
+
+
+
+//==============================================================================
+// CALC WEIGHTED PV_r TRANSPOSE
+//==============================================================================
+// The full P;V matrix is mpv X nu, where mpv=(mp+mv). Here we want the
+// mpv X nfu submatrix
+// that retains only the columns that correspond to free (not prescribed) u's;
+// call that PV_r. Also, we want the rows scaled by 1/constraint tolerances and
+// retained columns scaled by 1/u weights; call that PVw_r. And we're actually
+// going to compute the nfu X mpv transpose ~PVw_r, which we'll call PVw_rt.
+//
+// PV = [P]
+// [V]
+// PVw = Tpv PV Wu^-1
+// PVwt = ~PVw = Wu^-1 ~PV Tpv (weights are symmetric)
+//
+// PVw_rt = PVwt submatrix with rows removed if they correspond to u_p.
+//
+// We calculate one column at a time to avoid any matrix ops. We can do the
+// column scaling of ~PV by Tpv for free, but the row scaling requires nu*mpv
+// flops. Return matrix PVw_rt must have contiguous-data columns.
+void SimbodyMatterSubsystemRep::
+calcWeightedPVrTranspose(
+ const State& s,
+ const Vector& Tpv, // 1/verr tols (mp+mv)
+ const Vector& ooWu, // 1/u weights (nu)
+ Matrix& PVw_rt) const
+{
+ const SBInstanceCache& ic = getInstanceCache(s);
+
+ // Global problem dimensions.
+ const int mp = ic.totalNHolonomicConstraintEquationsInUse;
+ const int mv = ic.totalNNonholonomicConstraintEquationsInUse;
+ const int mpv = mp+mv;
+
+ const int nu = getNU(s);
+ const int nfu = ic.getTotalNumFreeU();
+ assert(nfu <= nu);
+ const bool mustPack = (nfu != nu);
+
+ assert(Tpv.size() == mpv);
+ assert(ooWu.size() == nu);
+
+ PVw_rt.resize(nfu,mpv);
+ if (mpv==0 || nfu==0)
+ return;
+
+ assert(PVw_rt.hasContiguousData());
+
+ Vector lambdapv(mpv, Real(0));
+ if (mustPack) {
+ Vector PVtcol(nu);
+ for (int j=0; j < mpv; ++j) {
+ lambdapv[j] = Tpv[j]; // this gives column j of ~PV scaled by Tpv[i]
+ multiplyByPVATranspose(s, true, true, false, lambdapv, PVtcol);
+ lambdapv[j] = 0;
+ PVtcol.rowScaleInPlace(ooWu); // now (Wu^-1 ~PV Tpv)_j
+ packFreeU(s, PVtcol, PVw_rt(j));
+ }
+ } else { // No prescribed motion so we can work in place.
+ for (int j=0; j < mpv; ++j) {
+ VectorView PVw_rt_j = PVw_rt(j);
+ lambdapv[j] = Tpv[j]; // this gives column j of ~PV scaled by Tpv[i]
+ multiplyByPVATranspose(s, true, true, false, lambdapv, PVw_rt_j);
+ lambdapv[j] = 0;
+ PVw_rt_j.rowScaleInPlace(ooWu); // now (Wu^-1 ~PV Tpv)_j
+ }
+ }
+}
+
+
+
+//==============================================================================
+// CALC BIAS FOR MULTIPLY BY PVA
+//==============================================================================
+// We have these constraint equations available:
+// (1) pverr = Pq*qdot - Pt (Pq==P*N^+, Pt==c(t,q))
+// (2) vaerr = V*udot - b_v(t,q,u)
+// (3) aerr = A*udot - b_a(t,q,u)
+// with P=P(t,q), N=N(q), V=V(t,q,u), A=A(t,q,u). Individual constraint
+// error equations are calculated in constant time, so the whole set can be
+// evaluated in O(m) time where m is the total number of constraint equations.
+//
+// Our plan is to use those equations to perform the multiplications by the
+// matrices P,V, and/or A times a u-like vector. But first we need to calculate
+// those extra terms that don't involve the matrices so we can subtract them
+// off later. So we're going to compute:
+// (4) bias=[ bias_p, bias_v, bias_a ]
+// =[ -Pt, -b_v(t,q,u), -b_a(t,q,u) ].
+// which we can get by using equations (1)-(3) with zero qdot or udot.
+//
+// In general the state must be realized through Velocity stage, but if the
+// system contains only holonomic constraints, or if only P is requested, then
+// bias is just bias_p and only time- and position-dependent since we just need
+// to use eq. (1). In that case we require only stage Position.
+//
+// Note that bias_p is correct for use when multiplying a u-like vector by P
+// or a q-like vector by Pq (==P*N^-1).
+//
+// The output vector must use contiguous storage.
+// Complexity is O(m).
+void SimbodyMatterSubsystemRep::
+calcBiasForMultiplyByPVA(const State& s,
+ bool includeP,
+ bool includeV,
+ bool includeA,
+ Vector& bias) const
+{
+ const SBInstanceCache& ic = getInstanceCache(s);
+
+ // Global problem dimensions.
+ const int mHolo = includeP ?
+ ic.totalNHolonomicConstraintEquationsInUse : 0;
+ const int mNonholo = includeV ?
+ ic.totalNNonholonomicConstraintEquationsInUse : 0;
+ const int mAccOnly = includeA ?
+ ic.totalNAccelerationOnlyConstraintEquationsInUse : 0;
+ const int m = mHolo+mNonholo+mAccOnly;
+
+ bias.resize(m);
+ if (m == 0) return;
+
+ assert(bias.hasContiguousData());
+
+ // Overlay this Array on the bias Vector's data so that we can manipulate
+ // small chunks of it repeatedly with no heap activity or virtual method
+ // calls.
+ ArrayView_<Real> biasArray(&bias[0], &bias[0] + m);
+
+ // Except for holonomic constraint equations where we can work at the
+ // velocity level, we'll need to supply body accelerations to the constraint
+ // acceleration error routines. Because the udots are zero, the body
+ // accelerations include velocity-dependent terms only, i.e. the coriolis
+ // accelerations. Those have already been calculated in the state, but they
+ // are AC_GB, the coriolis accelerations in Ground. The constraint methods
+ // want those relative to their Ancestor frames, which might not be Ground.
+ const Array_<SpatialVec,MobilizedBodyIndex>* allAC_GB = 0;
+ if (mNonholo || mAccOnly)
+ allAC_GB = &getTreeVelocityCache(s).totalCoriolisAcceleration;
+
+ // This array will be resized and filled with the Ancestor-relative
+ // coriolis accelerations for the constrained bodies of each velocity
+ // or acceleration-only Constraint in turn; we're declaring it outside the
+ // loop to minimize heap allocation (resizing down doesn't normally free
+ // heap space). This won't be used if we have only holonomic constraints.
+ Array_<SpatialVec,ConstrainedBodyIndex> AC_AB;
+
+ // Subarrays of these all-zero arrays will be used to supply zero body
+ // velocities and qdots (holonomic) or zero udots (nonholonomic and
+ // acceleration-only) for each Constraint in turn; we're declaring
+ // them outside the loop to minimize heap allocation. They'll grow until
+ // they hit the maximum size needed by any Constraint.
+ Array_<SpatialVec,ConstrainedBodyIndex> zeroV_AB;
+ Array_<Real, ConstrainedQIndex> zeroQDot;
+ Array_<Real, ConstrainedUIndex> zeroUDot;
+
+ // Loop over all enabled constraints, ask them to generate constraint
+ // errors, and collect those in the output bias vector.
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx) {
+ if (isConstraintDisabled(s,cx))
+ continue;
+
+ const SBInstancePerConstraintInfo&
+ cInfo = ic.getConstraintInstanceInfo(cx);
+ // Find this Constraint's err segments within the global array.
+ const Segment& holoSeg = cInfo.holoErrSegment;
+ const Segment& nonholoSeg = cInfo.nonholoErrSegment;
+ const Segment& accOnlySeg = cInfo.accOnlyErrSegment;
+ const int mp = includeP ? holoSeg.length : 0;
+ const int mv = includeV ? nonholoSeg.length : 0;
+ const int ma = includeA ? accOnlySeg.length : 0;
+
+ const ConstraintImpl& crep = constraints[cx]->getImpl();
+ const int ncb = crep.getNumConstrainedBodies();
+
+ if (mp) { // holonomic -- use velocity equations
+ const int ncq = cInfo.getNumConstrainedQ();
+ // Make sure we have enough zeroes.
+ if (zeroV_AB.size() < ncb) zeroV_AB.resize(ncb, SpatialVec(Vec3(0)));
+ if (zeroQDot.size() < ncq) zeroQDot.resize(ncq, Real(0));
+
+ // Make subarrays; this does not require heap allocation.
+ const ArrayViewConst_<SpatialVec,ConstrainedBodyIndex>
+ V0_AB = zeroV_AB(ConstrainedBodyIndex(0), ncb);
+ const ArrayViewConst_<Real,ConstrainedQIndex>
+ qdot0 = zeroQDot(ConstrainedQIndex(0), ncq);
+
+ // The holonomic error slots start at beginning of bias array.
+ ArrayView_<Real> pverr = biasArray(holoSeg.offset, mp);
+
+ // Write errors into pverr, which is a segment of the bias argument.
+ crep.calcPositionDotErrors(s, V0_AB, qdot0, pverr);
+ }
+
+ if (!(mv || ma))
+ continue; // nothing else to do here
+
+ const int ncu = cInfo.getNumConstrainedU();
+ // Make sure we have enough zeroes for udots.
+ if (zeroUDot.size() < ncu) zeroUDot.resize(ncu, Real(0));
+ // Make a subarray of the right size.
+ const ArrayViewConst_<Real,ConstrainedUIndex>
+ udot0 = zeroUDot(ConstrainedUIndex(0), ncu);
+
+ // Now fill in coriolis accelerations. If the Ancestor is Ground
+ // we're just reordering. If it isn't Ground we have to transform
+ // the coriolis accelerations from Ground to Ancestor, at a cost
+ // of 105 flops/constrained body (not just re-expressing).
+ crep.convertBodyAccelToConstrainedBodyAccel(s, *allAC_GB, AC_AB);
+
+ // At this point AC_AB holds the coriolis accelerations of each
+ // constrained body in A.
+
+ if (mv) { // non-holonomic constraints
+ // The error slots begin after skipping the holonomic part of
+ // the bias array. (That could be empty if P wasn't included.)
+ const int start = mHolo+nonholoSeg.offset;
+ ArrayView_<Real> vaerr = biasArray(start, mv);
+ crep.calcVelocityDotErrors(s, AC_AB, udot0, vaerr);
+ }
+ if (ma) { // acceleration-only constraints
+ // The error slots begin after skipping the holonomic and
+ // non-holonomic parts of the bias array (those could be empty
+ // if P or V weren't included).
+ const int start = mHolo+mNonholo+accOnlySeg.offset;
+ ArrayView_<Real> aerr = biasArray(start, ma);
+ crep.calcAccelerationErrors(s, AC_AB, udot0, aerr);
+ }
+ }
+}
+
+
+
+
+
+//==============================================================================
+// CALC BIAS FOR ACCELERATION CONSTRAINTS
+//==============================================================================
+// We have these constraint equations available:
+// (1) paerr = Pq*qdotdot - b_p(t,q,u) (Pq==P*N^+, Pt==c(t,q))
+// (2) vaerr = V*udot - b_v(t,q,u)
+// (3) aerr = A*udot - b_a(t,q,u)
+// with P=P(t,q), N=N(q), V=V(t,q,u), A=A(t,q,u). Individual constraint
+// error equations are calculated in constant time, so the whole set can be
+// evaluated in O(m) time where m is the total number of constraint equations.
+//
+// We want to calculate those extra terms that don't involve the matrices so
+// we can subtract them off later. So we're going to compute:
+// (4) bias=[ bias_p, bias_v, bias_a ]
+// =[-b_p(t,q,u), -b_v(t,q,u), -b_a(t,q,u) ].
+// which we can get by using equations (1)-(3) with zero qdotdot or udot.
+//
+// The state must be realized through Velocity stage.
+//
+// Note that bias_p is correct for use when multiplying a u-like vector by P
+// or a q-like vector by Pq (==P*N^-1).
+//
+// The output vector must use contiguous storage.
+// Complexity is O(m).
+void SimbodyMatterSubsystemRep::
+calcBiasForAccelerationConstraints(const State& s,
+ bool includeP,
+ bool includeV,
+ bool includeA,
+ Vector& bias) const
+{
+ const SBInstanceCache& ic = getInstanceCache(s);
+
+ // Global problem dimensions.
+ const int mHolo = includeP ?
+ ic.totalNHolonomicConstraintEquationsInUse : 0;
+ const int mNonholo = includeV ?
+ ic.totalNNonholonomicConstraintEquationsInUse : 0;
+ const int mAccOnly = includeA ?
+ ic.totalNAccelerationOnlyConstraintEquationsInUse : 0;
+ const int m = mHolo+mNonholo+mAccOnly;
+
+ bias.resize(m);
+ if (m == 0) return;
+
+ assert(bias.hasContiguousData());
+
+ // Overlay this Array on the bias Vector's data so that we can manipulate
+ // small chunks of it repeatedly with no heap activity or virtual method
+ // calls.
+ ArrayView_<Real> biasArray(&bias[0], &bias[0] + m);
+
+ // We'll need to supply body accelerations to the constraint
+ // acceleration error routines. Because the udots are zero, the body
+ // accelerations include velocity-dependent terms only, i.e. the coriolis
+ // accelerations. Those have already been calculated in the state, but they
+ // are AC_GB, the coriolis accelerations in Ground. The constraint methods
+ // want those relative to their Ancestor frames, which might not be Ground.
+ const Array_<SpatialVec,MobilizedBodyIndex>&
+ allAC_GB = getTreeVelocityCache(s).totalCoriolisAcceleration;
+
+ // This array will be resized and filled with the Ancestor-relative
+ // coriolis accelerations for the constrained bodies of each Constraint in
+ // turn; we're declaring it outside the
+ // loop to minimize heap allocation (resizing down doesn't normally free
+ // heap space).
+ Array_<SpatialVec,ConstrainedBodyIndex> AC_AB;
+
+ // Subarrays of these all-zero arrays will be used to supply zero qdotdots
+ // (holonomic) or zero udots (nonholonomic and
+ // acceleration-only) for each Constraint in turn; we're declaring
+ // them outside the loop to minimize heap allocation. They'll grow until
+ // they hit the maximum size needed by any Constraint.
+ Array_<Real, ConstrainedQIndex> zeroQDotDot;
+ Array_<Real, ConstrainedUIndex> zeroUDot;
+
+ // Loop over all enabled constraints, ask them to generate constraint
+ // errors, and collect those in the output bias vector.
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx) {
+ if (isConstraintDisabled(s,cx))
+ continue;
+
+ const SBInstancePerConstraintInfo&
+ cInfo = ic.getConstraintInstanceInfo(cx);
+ // Find this Constraint's err segments within the global array.
+ const Segment& holoSeg = cInfo.holoErrSegment;
+ const Segment& nonholoSeg = cInfo.nonholoErrSegment;
+ const Segment& accOnlySeg = cInfo.accOnlyErrSegment;
+ const int mp = includeP ? holoSeg.length : 0;
+ const int mv = includeV ? nonholoSeg.length : 0;
+ const int ma = includeA ? accOnlySeg.length : 0;
+
+ const ConstraintImpl& crep = constraints[cx]->getImpl();
+ const int ncb = crep.getNumConstrainedBodies();
+
+ // Now fill in coriolis accelerations. If the Ancestor is Ground
+ // we're just reordering. If it isn't Ground we have to transform
+ // the coriolis accelerations from Ground to Ancestor, at a cost
+ // of 105 flops/constrained body (not just re-expressing).
+ crep.convertBodyAccelToConstrainedBodyAccel(s, allAC_GB, AC_AB);
+
+ // At this point AC_AB holds the coriolis accelerations of each
+ // constrained body in A.
+
+ if (mp) { // holonomic -- use velocity equations
+ const int ncq = cInfo.getNumConstrainedQ();
+ // Make sure we have enough zeroes.
+ if (zeroQDotDot.size() < ncq) zeroQDotDot.resize(ncq, Real(0));
+
+ // Make subarray; this does not require heap allocation.
+ const ArrayViewConst_<Real,ConstrainedQIndex>
+ qdotdot0 = zeroQDotDot(ConstrainedQIndex(0), ncq);
+
+ // The holonomic error slots start at beginning of bias array.
+ ArrayView_<Real> paerr = biasArray(holoSeg.offset, mp);
+
+ // Write errors into paerr, which is a segment of the bias argument.
+ crep.calcPositionDotDotErrors(s, AC_AB, qdotdot0, paerr);
+ }
+
+ if (!(mv || ma))
+ continue; // nothing else to do here
+
+ const int ncu = cInfo.getNumConstrainedU();
+ // Make sure we have enough zeroes for udots.
+ if (zeroUDot.size() < ncu) zeroUDot.resize(ncu, Real(0));
+ // Make a subarray of the right size.
+ const ArrayViewConst_<Real,ConstrainedUIndex>
+ udot0 = zeroUDot(ConstrainedUIndex(0), ncu);
+
+ if (mv) { // non-holonomic constraints
+ // The error slots begin after skipping the holonomic part of
+ // the bias array. (That could be empty if P wasn't included.)
+ const int start = mHolo+nonholoSeg.offset;
+ ArrayView_<Real> vaerr = biasArray(start, mv);
+ crep.calcVelocityDotErrors(s, AC_AB, udot0, vaerr);
+ }
+ if (ma) { // acceleration-only constraints
+ // The error slots begin after skipping the holonomic and
+ // non-holonomic parts of the bias array (those could be empty
+ // if P or V weren't included).
+ const int start = mHolo+mNonholo+accOnlySeg.offset;
+ ArrayView_<Real> aerr = biasArray(start, ma);
+ crep.calcAccelerationErrors(s, AC_AB, udot0, aerr);
+ }
+ }
+}
+
+
+
+//==============================================================================
+// MULTIPLY BY Pq
+//==============================================================================
+// We have these mp constraint equations available:
+// (1) pverr(t,q;qdot) = Pq*qdot - Pt (where Pq=P*N^+, Pt=c(t,q))
+// = P *u - Pt
+// with P=P(t,q), N=N(q), and Pt=Pt(t,q). Individual constraint
+// error equations are calculated in constant time, so the whole set can be
+// evaluated in O(nq+mp) time where mp is the total number of holonomic
+// constraint equations. We expect to be given bias_p=-Pt as a
+// precalculated argument. (See calcBiasForMultiplyByPVA().)
+//
+// Given a q-like vector we can calculate
+// PqXqlike = Pq*qlike = P*N^+*qlike = pverr(t,q;qlike) - bias_p.
+//
+// The state must be realized to stage Position.
+// All vectors must be using contiguous storage.
+// Complexity is O(mp + n).
+void SimbodyMatterSubsystemRep::
+multiplyByPq(const State& s,
+ const Vector& bias_p,
+ const Vector& qlike,
+ Vector& PqXqlike) const
+{
+ const SBInstanceCache& ic = getInstanceCache(s);
+
+ // Global problem dimensions.
+ const int mHolo = ic.totalNHolonomicConstraintEquationsInUse;
+ const int m = mHolo;
+ const int nq = getNQ(s);
+ const int nu = getNU(s);
+ const int nb = getNumBodies();
+
+ assert(bias_p.size() == m);
+ assert(qlike.size() == nq);
+
+ PqXqlike.resize(m);
+ if (m == 0) return;
+
+ if (nq == 0) { // not likely!
+ PqXqlike.setToZero();
+ return;
+ }
+
+ assert(bias_p.hasContiguousData() && qlike.hasContiguousData());
+ assert(PqXqlike.hasContiguousData());
+
+ // Generate a u-like Vector via ulike = N^-1 * qlike. Then use that to
+ // calculate spatial velocities V_GB = J * ulike.
+ Vector ulike(nu);
+ Vector_<SpatialVec> V_GB(nb);
+ multiplyByNInv(s, false, qlike, ulike); // cheap
+ multiplyBySystemJacobian(s, ulike, V_GB); // 12*(nu+nb) flops
+
+ // Overlay Arrays on the Vectors' data so that we can manipulate small
+ // chunks of them repeatedly with no heap activity or virtual method calls.
+ const ArrayViewConst_<SpatialVec,MobilizedBodyIndex>
+ V_GBArray (&V_GB[0], &V_GB[0] +nb);
+ const ArrayViewConst_<Real,QIndex> qArray (&qlike[0], &qlike[0] +nq);
+ const ArrayViewConst_<Real> biasArray (&bias_p[0], &bias_p[0] +m);
+ ArrayView_<Real> PNInvqArray(&PqXqlike[0],&PqXqlike[0]+m);
+
+ // This array will be resized and filled with the Ancestor-relative
+ // velocities for the constrained bodies of each Constraint in turn;
+ // we're declaring it outside the loop to minimize heap allocation
+ // (resizing down doesn't normally free heap space).
+ Array_<SpatialVec,ConstrainedBodyIndex> V_AB;
+ // Same, but for each constraint's qdot subset.
+ Array_<Real,ConstrainedQIndex> qdot;
+
+ // Loop over all enabled constraints, ask them to generate constraint
+ // errors, and collect those in the output vector, subtracting off the bias
+ // as we go so we can go through the memory just once.
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx) {
+ if (isConstraintDisabled(s,cx))
+ continue;
+
+ const SBInstancePerConstraintInfo&
+ cInfo = ic.getConstraintInstanceInfo(cx);
+ // Find this Constraint's perr segment within the global array.
+ const Segment& holoSeg = cInfo.holoErrSegment;
+ const int mp = holoSeg.length;
+
+ if (!mp)
+ continue;
+
+ const ConstraintImpl& crep = constraints[cx]->getImpl();
+ const int ncq = cInfo.getNumConstrainedQ();
+
+ // Need body velocities relative to Ancestor body.
+ crep.convertBodyVelocityToConstrainedBodyVelocity(s, V_GBArray, V_AB);
+
+ qdot.resize(ncq);
+ for (ConstrainedQIndex cqx(0); cqx < ncq; ++cqx)
+ qdot[cqx] = qArray[cInfo.getQIndexFromConstrainedQ(cqx)];
+
+ const int start = holoSeg.offset;
+ const ArrayViewConst_<Real> bias = biasArray(start, mp);
+ ArrayView_<Real> pverr = PNInvqArray(start, mp);
+ crep.calcPositionDotErrors(s, V_AB, qdot, pverr);
+ for (int i=0; i < mp; ++i)
+ pverr[i] -= bias[i];
+ }
+}
+
+
+
+//==============================================================================
+// CALC Pq
+//==============================================================================
+// Arranges for contiguous workspace if necessary, then makes repeated calls
+// to multiplyByPq() to compute one column at a time of Pq (=P*N^-1).
+// Complexity is O(n*mp + n*n) = O(n^2) <-- EXPENSIVE! Use transpose instead.
+void SimbodyMatterSubsystemRep::
+calcPq(const State& s, Matrix& Pq) const
+{
+ const SBInstanceCache& ic = getInstanceCache(s);
+
+ // Global problem dimensions.
+ const int mp = ic.totalNHolonomicConstraintEquationsInUse;
+ const int nq = getNQ(s);
+
+ Pq.resize(mp,nq);
+ if (mp==0 || nq==0)
+ return;
+
+ Vector biasp(mp);
+ calcBiasForMultiplyByPVA(s, true, false, false, biasp);
+ Vector qlike(nq, Real(0));
+
+ // If Pq's columns are contiguous we can avoid copying.
+ const bool isContiguous = Pq(0).hasContiguousData();
+ Vector contig_col(isContiguous ? 0 : mp);
+ for (int j=0; j < nq; ++j) {
+ qlike[j] = 1; // column we're working on
+ if (isContiguous)
+ multiplyByPq(s, biasp, qlike, Pq(j));
+ else {
+ multiplyByPq(s, biasp, qlike, contig_col);
+ Pq(j) = contig_col;
+ }
+ qlike[j] = 0;
+ }
+}
+
+
+
+//==============================================================================
+// MULTIPLY BY PVA
+//==============================================================================
+// We have these constraint equations available:
+// (1) pverr = Pq*qdot - Pt (Pq==P*N^+, Pt==c(t,q))
+// (2) vaerr = V*udot - b_v(t,q,u)
+// (3) aerr = A*udot - b_a(t,q,u)
+// with P=P(t,q), N=N(q), V=V(t,q,u), A=A(t,q,u). Individual constraint
+// error equations are calculated in constant time, so the whole set can be
+// evaluated in O(m) time where m is the total number of constraint equations.
+//
+// Here we will use those equations to perform the multiplications by the
+// matrices P,V, and/or A times a u-like vector:
+// [P] [ pverr(N*ulike) ]
+// (4) PVAu = [V] * ulike = [ vaerr(ulike) ] - bias.
+// [A] [ aerr(ulike) ]
+//
+// We expect to be supplied as a precalculated argument "bias" the terms in
+// equations (1)-(3) that don't involve P,V, or A:
+// (5) bias=[ bias_p, bias_v, bias_a ]
+// =[ -Pt, -b_v(t,q,u), -b_a(t,q,u) ].
+// See calcBiasForMultiplyByPVA() for how to get the bias terms.
+//
+// In general the state must be realized through Velocity stage, but if the
+// system contains only holonomic constraints, or if only P is included, then
+// bias is just bias_p and the result is only time- and position-dependent
+// since we just need to use eq. (1). In that case we require only that the
+// state be realized to stage Position.
+//
+// All of the Vector arguments must use contiguous storage.
+// Complexity is O(m+n).
+void SimbodyMatterSubsystemRep::
+multiplyByPVA( const State& s,
+ bool includeP,
+ bool includeV,
+ bool includeA,
+ const Vector& bias,
+ const Vector& ulike,
+ Vector& PVAu) const
+{
+ const SBInstanceCache& ic = getInstanceCache(s);
+
+ // Global problem dimensions.
+ const int mHolo = includeP ?
+ ic.totalNHolonomicConstraintEquationsInUse : 0;
+ const int mNonholo = includeV ?
+ ic.totalNNonholonomicConstraintEquationsInUse : 0;
+ const int mAccOnly = includeA ?
+ ic.totalNAccelerationOnlyConstraintEquationsInUse : 0;
+
+ const int m = mHolo+mNonholo+mAccOnly;
+ const int nq = getNQ(s);
+ const int nu = getNU(s);
+ const int nb = getNumBodies();
+
+ assert(bias.size() == m);
+ assert(ulike.size() == nu);
+
+ PVAu.resize(m);
+ if (m == 0) return;
+
+ if (nu == 0) { // not likely!
+ PVAu.setToZero();
+ return;
+ }
+
+ assert(bias.hasContiguousData() && ulike.hasContiguousData());
+ assert(PVAu.hasContiguousData());
+
+ // Generate body spatial velocities V=J*u or first term of the
+ // body spatial accelerations A=J*udot + Jdot*u, depending on how we're
+ // interpreting the ulike argument (as a u for holonomic constraints,
+ // and as udot for everything else).
+ Vector_<SpatialVec> Julike(nb);
+ multiplyBySystemJacobian(s, ulike, Julike); // 12*(nu+nb) flops
+
+ // Julike serves as V_GB when we're interpreting ulike as u.
+ const ArrayViewConst_<SpatialVec,MobilizedBodyIndex>
+ allV_GB(&Julike[0], &Julike[0] + nb);
+
+ // If we're doing any nonholonomic or acceleration-only constraints, we'll
+ // finish calculating body spatial accelerations and put them here.
+ Array_<SpatialVec,MobilizedBodyIndex> allA_GB;
+ if (mNonholo || mAccOnly) {
+ allA_GB.resize(nb);
+ const Array_<SpatialVec>&
+ allAC_GB = getTreeVelocityCache(s).totalCoriolisAcceleration;
+ for (MobilizedBodyIndex b(0); b < nb; ++b)
+ allA_GB[b] = allV_GB[b] + allAC_GB[b]; // i.e., J*udot + Jdot*u
+ }
+
+ // If we're going to be dealing with holonomic (position) constraints,
+ // generate a q-like Vector via qlike = N * ulike since the position
+ // error derivative routine wants qdots.
+ Vector qlike(nq);
+ if (mHolo)
+ multiplyByN(s, false, ulike, qlike); // cheap
+
+ // Overlay Arrays on the Vectors' data so that we can manipulate small
+ // chunks of them repeatedly with no heap activity or virtual method calls.
+ const ArrayViewConst_<Real,UIndex> uArray (&ulike[0], &ulike[0] + nu);
+ const ArrayViewConst_<Real,QIndex> qArray (&qlike[0], &qlike[0] + nq);
+ const ArrayViewConst_<Real> biasArray(&bias[0], &bias[0] + m );
+ ArrayView_<Real> PVAuArray(&PVAu[0], &PVAu[0] + m );
+
+ // This array will be resized and filled with the Ancestor-relative
+ // velocities for the constrained bodies of each holonomic Constraint in
+ // turn; we're declaring it outside the loop to minimize heap allocation
+ // (resizing down doesn't normally free heap space). This won't be used
+ // if we aren't processing holonomic constraints.
+ Array_<SpatialVec,ConstrainedBodyIndex> V_AB;
+ // Same, but for each holonomic constraint's qdot subset.
+ Array_<Real,ConstrainedQIndex> qdot;
+
+ // This array will be resized and filled with the Ancestor-relative
+ // accelerations for the constrained bodies of each velocity
+ // or acceleration-only Constraint in turn. This won't be used if we have
+ // only holonomic constraints.
+ Array_<SpatialVec,ConstrainedBodyIndex> A_AB;
+ // Same, but for each nonholonomic/acconly constraint's udot subset.
+ Array_<Real,ConstrainedUIndex> udot;
+
+ // Loop over all enabled constraints, ask them to generate constraint
+ // errors, and collect those in the output argument PVAu. Remove bias
+ // as we go so we only have to touch the memory once.
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx) {
+ if (isConstraintDisabled(s,cx))
+ continue;
+
+ const SBInstancePerConstraintInfo&
+ cInfo = ic.getConstraintInstanceInfo(cx);
+ // Find this Constraint's err segments within the global array.
+ const Segment& holoSeg = cInfo.holoErrSegment;
+ const Segment& nonholoSeg = cInfo.nonholoErrSegment;
+ const Segment& accOnlySeg = cInfo.accOnlyErrSegment;
+ const int mp = includeP ? holoSeg.length : 0;
+ const int mv = includeV ? nonholoSeg.length : 0;
+ const int ma = includeA ? accOnlySeg.length : 0;
+
+ const ConstraintImpl& crep = constraints[cx]->getImpl();
+
+ if (mp) { // holonomic -- use velocity equations
+ const int ncq = cInfo.getNumConstrainedQ();
+ // Need body velocities in ancestor frame.
+ crep.convertBodyVelocityToConstrainedBodyVelocity(s, allV_GB, V_AB);
+ qdot.resize(ncq);
+ for (ConstrainedQIndex cqx(0); cqx < ncq; ++cqx)
+ qdot[cqx] = qArray[cInfo.getQIndexFromConstrainedQ(cqx)];
+
+ // The error slots start at the beginning of the bias array.
+ const int start = holoSeg.offset;
+ const ArrayViewConst_<Real> bias = biasArray(start, mp);
+ ArrayView_<Real> pverr = PVAuArray(start, mp);
+ crep.calcPositionDotErrors(s, V_AB, qdot, pverr);
+ for (int i=0; i < mp; ++i)
+ pverr[i] -= bias[i];
+ }
+
+ if (!(mv || ma))
+ continue; // nothing else to do here
+
+ const int ncu = cInfo.getNumConstrainedU();
+
+ // Now fill in accelerations. If the Ancestor is Ground
+ // we're just reordering. If it isn't Ground we have to transform
+ // the accelerations from Ground to Ancestor, at a cost
+ // of 105 flops/constrained body (not just re-expressing).
+ crep.convertBodyAccelToConstrainedBodyAccel(s, allA_GB, A_AB);
+ // At this point A_AB holds the accelerations of each
+ // constrained body in A.
+
+ // Now pack together the appropriate udots.
+ udot.resize(ncu);
+ for (ConstrainedUIndex cux(0); cux < ncu; ++cux)
+ udot[cux] = uArray[cInfo.getUIndexFromConstrainedU(cux)];
+
+ if (mv) { // non-holonomic constraints
+ // The error slots begin after skipping the holonomic part of
+ // the arrays. (Those could be empty if P wasn't included.)
+ const int start = mHolo + nonholoSeg.offset;
+ const ArrayViewConst_<Real> bias = biasArray(start, mv);
+ ArrayView_<Real> vaerr = PVAuArray(start, mv);
+ crep.calcVelocityDotErrors(s, A_AB, udot, vaerr);
+ for (int i=0; i < mv; ++i)
+ vaerr[i] -= bias[i];
+ }
+
+ if (ma) { // acceleration-only constraints
+ // The error slots begin after skipping the holonomic and
+ // non-holonomic parts of the arrays (those could be empty
+ // if P or V weren't included).
+ const int start = mHolo+mNonholo+accOnlySeg.offset;
+ const ArrayViewConst_<Real> bias = biasArray(start, ma);
+ ArrayView_<Real> aerr = PVAuArray(start, ma);
+ crep.calcAccelerationErrors(s, A_AB, udot, aerr);
+ for (int i=0; i < ma; ++i)
+ aerr[i] -= bias[i];
+ }
+ }
+}
+
+
+
+//==============================================================================
+// CALC PVA
+//==============================================================================
+// Arranges for contiguous workspace if necessary, then makes repeated calls
+// to multiplyByPVA() to compute one column at a time of G=PVA or a submatrix.
+// This is particularly useful for computing [P;V] which is the velocity-level
+// constraint projection matrix.
+// Complexity is O(n*m + n*n) = O(n^2) <-- EXPENSIVE! Use transpose instead.
+void SimbodyMatterSubsystemRep::
+calcPVA(const State& s,
+ bool includeP,
+ bool includeV,
+ bool includeA,
+ Matrix& PVA) const
+{
+ const SBInstanceCache& ic = getInstanceCache(s);
+
+ // Global problem dimensions.
+ const int mHolo = includeP ?
+ ic.totalNHolonomicConstraintEquationsInUse : 0;
+ const int mNonholo = includeV ?
+ ic.totalNNonholonomicConstraintEquationsInUse : 0;
+ const int mAccOnly = includeA ?
+ ic.totalNAccelerationOnlyConstraintEquationsInUse : 0;
+
+ const int m = mHolo+mNonholo+mAccOnly;
+ const int nu = getNU(s);
+
+ PVA.resize(m,nu);
+ if (m==0 || nu==0)
+ return;
+
+ Vector bias(m);
+ calcBiasForMultiplyByPVA(s, includeP, includeV, includeA, bias);
+ Vector ulike(nu, Real(0));
+
+ // If PVA's columns are contiguous we can avoid copying.
+ const bool isContiguous = PVA(0).hasContiguousData();
+ Vector contig_col(isContiguous ? 0 : m); // temp space if needed
+ for (int j=0; j < nu; ++j) {
+ ulike[j] = 1; // column we're working on
+ if (isContiguous) {
+ multiplyByPVA(s, includeP, includeV, includeA, bias, ulike,
+ PVA(j));
+ } else {
+ multiplyByPVA(s, includeP, includeV, includeA, bias, ulike,
+ contig_col);
+ PVA(j) = contig_col;
+ }
+ ulike[j] = 0;
+ }
+}
+
+
+
+// =============================================================================
+// CALC G MInv G^T
+// =============================================================================
+// We will calculate multipliers lambda as
+// (G M^-1 ~G) lambda = aerr
+// and need a fast way to explicitly calculate this mXm matrix.
+// Optimally, we would calculate it in O(m^2) time. I don't know
+// how to calculate it that fast, but using m calls to operator sequence:
+// Gt_j = Gt* lambda_j O(n)
+// MInvGt_j = M^-1* Gt_j O(n)
+// GMInvGt(j) = G* MInvGt_j O(n)
+// we can calculate it in O(mn) time. As long as m << n, and
+// especially if m is a small constant independent of n, and even better
+// if we've partitioned it into little subblocks, this is all very
+// reasonable. One slip up and you'll toss in a factor of mn^2 or m^2n and
+// screw this up -- be careful!
+//
+// When there is prescribed motion in the system the matrix we want is
+// Gr Mrr^-1 ~Gr. That is still an mXm matrix and we are able to produce it
+// with no visible effort due to the definition of our a=M^-1*f operator. It
+// ignores the prescribed part fp of f (here a column of ~Gp), and returns
+// zeroes in the prescribed part ap of a. Those zeroes have the effect of
+// removing the Gp columns of G in the final operation. Note: the resulting
+// matrix is *not* a submatrix of G*M^-1*~G!
+//
+// Note that we do not require contiguous storage for GMInvGt's columns,
+// although we'll take advantage of it if they are. If not, we'll work in
+// a contiguous temp and then copy back. This is because we want to allow
+// any matrix at the Simbody API level and we don't want to force the API
+// method to have to allocate a whole new mXm matrix when all we need for
+// a temporary here is an m-length temporary.
+//
+// Complexity is O(m^2 + m*n) = O(m*n).
+//
+// TODO: as long as the force transmission matrix for all constraints is G^T
+// the resulting matrix is symmetric. But (a) I don't know how to take
+// advantage of that in forming the matrix, and (b) some constraints may
+// result in the force transmission matrix != G (this occurs for example for
+// some kinds of "working" constraints like sliding friction).
+void SimbodyMatterSubsystemRep::
+calcGMInvGt(const State& s,
+ Matrix& GMInvGt) const
+{
+ const SBInstanceCache& ic = getInstanceCache(s);
+
+ // Global problem dimensions.
+ const int mHolo = ic.totalNHolonomicConstraintEquationsInUse;
+ const int mNonholo = ic.totalNNonholonomicConstraintEquationsInUse;
+ const int mAccOnly = ic.totalNAccelerationOnlyConstraintEquationsInUse;
+ const int m = mHolo+mNonholo+mAccOnly;
+ const int nu = getNU(s);
+
+ GMInvGt.resize(m,m);
+ if (m==0) return;
+
+ // If the output matrix doesn't have columns in contiguous memory, we'll
+ // allocate a contiguous-memory temp that we can use to hold one column
+ // at a time as we compute them.
+ const bool columnsAreContiguous = GMInvGt(0).hasContiguousData();
+ Vector GMInvGt_j(columnsAreContiguous ? 0 : m);
+
+ // These two temporaries are always needed to hold one column of Gt,
+ // then one column of M^-1 * Gt.
+ Vector Gtcol(nu), MInvGtcol(nu);
+
+ // Precalculate bias so we can perform multiplication by G efficiently.
+ Vector bias(m);
+ calcBiasForMultiplyByPVA(s,true,true,true,bias);
+
+ // Lambda is used to pluck out one column at a time of Gt. Exactly one
+ // element at a time of lambda will be 1, the rest are 0.
+ Vector lambda(m, Real(0));
+
+ for (int j=0; j < m; ++j) {
+ lambda[j] = 1;
+ multiplyByPVATranspose(s, true, true, true, lambda, Gtcol);
+ lambda[j] = 0;
+ multiplyByMInv(s, Gtcol, MInvGtcol);
+ if (columnsAreContiguous)
+ multiplyByPVA(s, true, true, true, bias, MInvGtcol, GMInvGt(j));
+ else {
+ multiplyByPVA(s, true, true, true, bias, MInvGtcol, GMInvGt_j);
+ GMInvGt(j) = GMInvGt_j;
+ }
+ }
+}
+
+
+
+// =============================================================================
+// SOLVE FOR CONSTRAINT IMPULSES
+// =============================================================================
+// Current implementation computes G*M^-1*~G, factors it, and does a single
+// solve all at great expense.
+// TODO: should realize factored matrix if needed and reuse if possible.
+void SimbodyMatterSubsystemRep::
+solveForConstraintImpulses(const State& state,
+ const Vector& deltaV,
+ Vector& impulse) const
+{
+ Matrix GMInvGt;
+ calcGMInvGt(state, GMInvGt);
+ // MUST DUPLICATE SIMBODY'S METHOD HERE:
+ const Real conditioningTol = GMInvGt.nrow()
+ * SqrtEps*std::sqrt(SqrtEps); // Eps^(3/4)
+ FactorQTZ qtz(GMInvGt, conditioningTol);
+ qtz.solve(deltaV, impulse);
+}
+
+
+
+// =============================================================================
+// CALC BODY ACCELERATION FROM UDOT
+// =============================================================================
+// Input and output vectors must use contiguous storage.
+// The knownUDot argument must be of length nu; the output argument will be
+// resized if necessary to length nb.
+// Cost is 12*nu + 18*nb flops.
+void SimbodyMatterSubsystemRep::
+calcBodyAccelerationFromUDot(const State& s,
+ const Vector& knownUDot,
+ Vector_<SpatialVec>& A_GB) const {
+ const int nu = getNU(s);
+ const int nb = getNumBodies();
+ A_GB.resize(nb); // always at least Ground
+
+ // Should have been checked before we got here.
+ assert(knownUDot.size() == nu);
+ if (nu == 0) {
+ A_GB.setToZero();
+ return;
+ }
+
+ assert(knownUDot.hasContiguousData() && A_GB.hasContiguousData());
+ const Real* knownUdotPtr = &knownUDot[0];
+ SpatialVec* aPtr = &A_GB[0];
+
+ const SBTreePositionCache& tpc = getTreePositionCache(s);
+ const SBTreeVelocityCache& tvc = getTreeVelocityCache(s);
+
+ // Note: this is equivalent to calculating J*u with
+ // multiplyBySystemJacobian() and adding in the totalCoriolisAcceleration
+ // for each body (which is Jdot*u). (sherm 110829: I tried it both ways)
+
+ // Sweep outward and delegate to RB nodes.
+ for (int i=0; i<(int)rbNodeLevels.size(); i++)
+ for (int j=0; j<(int)rbNodeLevels[i].size(); j++) {
+ const RigidBodyNode& node = *rbNodeLevels[i][j];
+ node.calcBodyAccelerationsFromUdotOutward
+ (tpc,tvc,knownUdotPtr,aPtr);
+ }
+}
+
+
+
+//==============================================================================
+// CALC CONSTRAINT ACCELERATION ERRORS
+//==============================================================================
+// Note: similar to multiplyByPVA() except:
+// - we're using the holonomic *second* derivative here
+// - we don't want to get rid of the bias terms
+// - state must be realized through Velocity stage even if there are only
+// holonomic constraints
+// - no option to work with only a subset of the constraints
+//
+// We have these constraint equations available:
+// (1) paerr = Pq*qdotdot - b_p(t,q,u) (Pq = P*N^-1)
+// (2) vaerr = V*udot - b_v(t,q,u)
+// (3) aerr = A*udot - b_a(t,q,u)
+// with P=P(t,q), N=N(q), V=V(t,q,u), A=A(t,q,u). Individual constraint
+// error equations are calculated in constant time, so the whole set can be
+// evaluated in O(m) time where m is the total number of constraint equations.
+//
+// All of the Vector arguments must use contiguous storage. Output is resized
+// to m=mp+mv+ma. Complexity is O(m+n).
+void SimbodyMatterSubsystemRep::
+calcConstraintAccelerationErrors
+ (const State& s,
+ const Vector_<SpatialVec>& A_GB,
+ const Vector_<Real>& udot,
+ const Vector_<Real>& qdotdot,
+ Vector& pvaerr) const
+{
+ const SBInstanceCache& ic = getInstanceCache(s);
+
+ // Global problem dimensions.
+ const int mHolo = ic.totalNHolonomicConstraintEquationsInUse;
+ const int mNonholo = ic.totalNNonholonomicConstraintEquationsInUse;
+ const int mAccOnly = ic.totalNAccelerationOnlyConstraintEquationsInUse;
+
+ const int m = mHolo+mNonholo+mAccOnly;
+ const int nq = getNQ(s);
+ const int nu = getNU(s);
+ const int nb = getNumBodies();
+
+ assert(A_GB.size() == nb);
+ assert(udot.size() == nu);
+ assert(qdotdot.size() == nq);
+
+ pvaerr.resize(m);
+ if (m == 0) return;
+
+ if (nu == 0) { // not likely!
+ pvaerr.setToZero();
+ return;
+ }
+
+ assert(udot.hasContiguousData() && qdotdot.hasContiguousData());
+ assert(A_GB.hasContiguousData() && pvaerr.hasContiguousData());
+
+ // Overlay Arrays on the Vectors' data so that we can manipulate small
+ // chunks of them repeatedly with no heap activity or virtual method calls.
+ const ArrayViewConst_<SpatialVec, MobilizedBodyIndex>
+ allA_GB (&A_GB[0], &A_GB[0] + nb);
+ const ArrayViewConst_<Real,UIndex> udArray (&udot[0], &udot[0] + nu);
+ const ArrayViewConst_<Real,QIndex> qddArray(&qdotdot[0], &qdotdot[0] + nq);
+ ArrayView_<Real> allAerr (&pvaerr[0], &pvaerr[0] + m );
+
+ // These arrays will be resized and filled with the input needs of each
+ // Constraint in turn. We're declaring them outside the loop to minimize
+ // heap allocation (resizing down doesn't normally free heap space).
+ Array_<SpatialVec,ConstrainedBodyIndex> A_AB;
+ Array_<Real,ConstrainedQIndex> qdd; // holonomic only
+ Array_<Real,ConstrainedUIndex> ud; // nonholonomic or acc-only
+
+ // Loop over all enabled constraints, ask them to generate constraint
+ // errors, and collect those in the output argument pvaerr.
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx) {
+ if (isConstraintDisabled(s,cx))
+ continue;
+
+ const SBInstancePerConstraintInfo&
+ cInfo = ic.getConstraintInstanceInfo(cx);
+ // Find this Constraint's err segments within the global array.
+ const Segment& holoSeg = cInfo.holoErrSegment;
+ const Segment& nonholoSeg = cInfo.nonholoErrSegment;
+ const Segment& accOnlySeg = cInfo.accOnlyErrSegment;
+ const int mp = holoSeg.length;
+ const int mv = nonholoSeg.length;
+ const int ma = accOnlySeg.length;
+
+ const ConstraintImpl& crep = constraints[cx]->getImpl();
+
+ // Now fill in accelerations. If the Ancestor is Ground
+ // we're just reordering. If it isn't Ground we have to transform
+ // the accelerations from Ground to Ancestor, at a cost
+ // of 105 flops/constrained body (not just re-expressing).
+ crep.convertBodyAccelToConstrainedBodyAccel(s, allA_GB, A_AB);
+
+ // At this point A_AB holds the accelerations of each
+ // constrained body in A.
+
+
+ if (mp) { // holonomic
+ // Now pack together the appropriate qdotdots.
+ const int ncq = cInfo.getNumConstrainedQ();
+ qdd.resize(ncq);
+ for (ConstrainedQIndex cqx(0); cqx < ncq; ++cqx)
+ qdd[cqx] = qddArray[cInfo.getQIndexFromConstrainedQ(cqx)];
+
+ // The error slots start at the beginning of the pvaerr array.
+ const int start = holoSeg.offset;
+ ArrayView_<Real> paerr = allAerr(start, mp);
+ crep.calcPositionDotDotErrors(s, A_AB, qdd, paerr);
+ }
+
+ if (!(mv || ma))
+ continue; // nothing else to do here
+
+ // Now pack together the appropriate udots.
+ const int ncu = cInfo.getNumConstrainedU();
+ ud.resize(ncu);
+ for (ConstrainedUIndex cux(0); cux < ncu; ++cux)
+ ud[cux] = udArray[cInfo.getUIndexFromConstrainedU(cux)];
+
+ if (mv) { // non-holonomic constraints
+ // The error slots begin after skipping the holonomic part of
+ // the arrays.
+ const int start = mHolo + nonholoSeg.offset;
+ ArrayView_<Real> vaerr = allAerr(start, mv);
+ crep.calcVelocityDotErrors(s, A_AB, ud, vaerr);
+ }
+
+ if (ma) { // acceleration-only constraints
+ // The error slots begin after skipping the holonomic and
+ // non-holonomic parts of the arrays.
+ const int start = mHolo+mNonholo+accOnlySeg.offset;
+ ArrayView_<Real> aerr = allAerr(start, ma);
+ crep.calcAccelerationErrors(s, A_AB, ud, aerr);
+ }
+ }
+}
+
+
+
+
+// =============================================================================
+// PRESCRIBE Q, PRESCRIBE U
+// =============================================================================
+// These are solvers that set continuous state variables q or u to their
+// prescribed values q(t) or u(t,q) that will already have been computed.
+// Note that prescribed udot=udot(t,q,u) is not dealt with here because it does
+// not involve a state change.
+bool SimbodyMatterSubsystemRep::prescribeQ(State& s) const {
+ const SBModelCache& mc = getModelCache(s);
+ const SBInstanceCache& ic = getInstanceCache(s);
+
+ const int npq = ic.getTotalNumPresQ();
+ const int nzq = ic.getTotalNumZeroQ();
+ if (npq==0 && nzq==0) return false; // don't invalidate positions
+
+ // copy prescribed q's from cache to state
+ // set known-zero q's to zero (or reference configuration)
+ const SBTimeCache& tc = getTimeCache(s);
+ Vector& q = updQ(s); // this Subsystem's q's, now invalidated
+
+ for (int i=0; i < npq; ++i)
+ q[ic.presQ[i]] = tc.presQPool[i];
+
+ //TODO: this isn't right -- need to use reference config
+ //q's which will be 1000 for quaternion.
+ for (int i=0; i < nzq; ++i)
+ q[ic.zeroQ[i]] = 0;
+
+ return true;
+}
+
+bool SimbodyMatterSubsystemRep::prescribeU(State& s) const {
+ const SBModelCache& mc = getModelCache(s);
+ const SBInstanceCache& ic = getInstanceCache(s);
+
+ const int npu = ic.getTotalNumPresU();
+ const int nzu = ic.getTotalNumZeroU();
+ if (npu==0 && nzu==0) return false; // don't invalidate velocities
+
+ // copy prescribed u's from cache to state
+ // set known-zero u's to zero
+ const SBConstrainedPositionCache& cpc = getConstrainedPositionCache(s);
+ Vector& u = updU(s); // this Subsystem's u's, now invalidated
+
+ for (int i=0; i < npu; ++i)
+ u[ic.presU[i]] = cpc.presUPool[i];
+
+ for (int i=0; i < nzu; ++i)
+ u[ic.zeroU[i]] = 0;
+
+ return true;
+}
+//............................. PRESCRIBE Q, U .................................
+
+
+
+//==============================================================================
+// ENFORCE POSITION CONSTRAINTS
+//==============================================================================
+// A note on state variable weights:
+// - q and u weights are not independent
+// - we consider u weights Wu primary and want the weighted variables to be
+// related like the unweighted ones: qdot=N*u so qdotw=N*uw, where
+// uw = Wu*u. So qdotw should be Wq*qdot=N*uw=N*Wu*u=N*Wu*N^+*qdot ==>
+// Wq = N*Wu*N^+. (and Wq^+=N*Wu^-1*N^+)
+// - Wu is diagonal, but Wq is block diagonal. We have fast operators for
+// multiplying these matrices by columns, but not for producing Wq so we
+// just create it operationally as we go.
+
+// These statics are for debugging use only.
+static Real calcQErrestWeightedNormU(const SimbodyMatterSubsystemRep& matter,
+ const State& s, const Vector& qErrest, const Vector& uWeights) {
+ Vector qhatErrest(uWeights.size());
+ matter.multiplyByNInv(s, false, qErrest, qhatErrest); // qhatErrest = N+ qErrest
+ qhatErrest.rowScaleInPlace(uWeights); // qhatErrest = Wu N+ qErrest
+ return qhatErrest.normRMS();
+}
+static Real calcQErrestWeightedNormQ(const SimbodyMatterSubsystemRep& matter,
+ const State& s, const Vector& qErrest, const Vector& qWeights) {
+ Vector Wq_qErrest(qErrest.size());
+ Wq_qErrest = qErrest.rowScale(qWeights); // Wq*qErrest
+ return Wq_qErrest.normRMS();
+}
+
+void SimbodyMatterSubsystemRep::enforcePositionConstraints
+ (State& s, Real consAccuracy, const Vector& yWeights,
+ const Vector& ooTols, Vector& yErrest, ProjectOptions opts) const
+{
+ assert(getStage(s) >= Stage::Position-1);
+
+ const SBInstanceCache& ic = getInstanceCache(s);
+
+ realizeSubsystemPosition(s);
+
+ // First work only with the holonomic (position) constraints, which appear
+ // first in the QErr array. Don't work on the quaternion constraints in
+ // this first section.
+ const int mHolo = getNumHolonomicConstraintEquationsInUse(s);
+ const int mQuats = getNumQuaternionsInUse(s);
+ const int nq = getNQ(s);
+ const int nfq = ic.getTotalNumFreeQ();
+ const int nu = getNU(s);
+ bool hasPrescribedMotion = (nfq != nq);
+
+ // Wq = N * Wu * N^+
+ // Wq^+ = N * Wu^-1 * N^+
+ const VectorView uWeights = yWeights(nq,nu);
+ const Vector ooUWeights = uWeights.elementwiseInvert(); //TODO: precalc
+ const VectorView ooPTols = ooTols(0,mHolo);
+
+ VectorView qErrest = yErrest.size() ? yErrest(0,nq) : yErrest(0,0);
+
+ // This is a const view into the State; the contents it refers to will
+ // change though.
+ const VectorView pErrs = getQErr(s)(0,mHolo); // just leave off quaternions
+ bool anyChange = false;
+
+ // Check whether we should stop if we see the solution diverging
+ // which should not happen when we're in the neighborhood of a solution
+ // on entry. This is always set while integrating, except during
+ // initialization.
+ const bool localOnly = opts.isOptionSet(ProjectOptions::LocalOnly);
+
+ // Solve
+ // (Tp Pq Wq^+) dq_WLS = Tp perr
+ // dq = Wq^+ dq_WLS
+ // q -= dq
+ // until RMS(Tp perr) <= 0.1*accuracy.
+ //
+ // But Pq=P*N^+, Wq^+=N*Wu^-1*N^+ so Pq Wq^+=P*Wu^-1*N^+. Since N^+ N=I,
+ // we can rewrite the above:
+ //
+ // (Tp P Wu^-1 N^+) dq_WLS = Tp perr
+ // dq = N Wu^-1 N^+ dq_WLS
+ // q -= dq
+ //
+ // We define Pqwt = ~Pqw = ~(Tp P Wu^-1 N^+) = ~N^+ Wu^-1 ~P Tp
+ // (diagonal weights are symmetric). We only retain rows that
+ // correspond to free (non prescribed) q's.
+ //
+ // This is a nonlinear least squares problem. Below is a full Newton
+ // iteration since we recalculate the iteration matrix each time around the
+ // loop. TODO: a solution could be found using the same iteration matrix,
+ // since we are projecting from (presumably) not too far away. Q1: will it
+ // be the same solution? Q2: if not, does it matter?
+ Vector scaledPerrs = pErrs.rowScale(ooPTols);
+ Real normAchievedTRMS = scaledPerrs.normRMS();
+
+ Real lastChangeMadeWRMS = 0; // size of last change in weighted dq
+ int nItsUsed = 0;
+
+ // Set how far past the required tolerance we'll attempt to go.
+ // We only fail if we can't achieve consAccuracy, but while we're
+ // solving we'll see if we can get consAccuracyToTryFor.
+ const Real consAccuracyToTryFor =
+ std::max(Real(0.1)*consAccuracy, SignificantReal);
+
+ // Conditioning tolerance. This determines when we'll drop a
+ // constraint.
+ // TODO: this is sloppy; should depend on constraint tolerance
+ // and rank should be saved and reused in velocity and acceleration
+ // constraint methods (or should be calculated elsewhere and passed in).
+ const Real conditioningTol = mHolo
+ //* SignificantReal; -- too tight
+ * SqrtEps;
+
+ if (normAchievedTRMS > consAccuracyToTryFor) {
+ Vector saveQ = getQ(s);
+ Matrix Pqwrt(nfq,mHolo);
+ Vector dfq_WLS(nfq), du(nu), dq(nq); // = Wq^+ dq_WLS
+ Vector udfq_WLS(hasPrescribedMotion ? nq : 0); // unpacked if needed
+ udfq_WLS.setToZero(); // must initialize unwritten elements
+ FactorQTZ Pqwr_qtz;
+ Real prevNormAchievedTRMS = normAchievedTRMS; // watch for divergence
+ const int MaxIterations = 20;
+ do {
+ calcWeightedPqrTranspose(s, ooPTols, ooUWeights, Pqwrt);//nfq X mp
+
+ // This factorization acts like a pseudoinverse.
+ Pqwr_qtz.factor<Real>(~Pqwrt, conditioningTol);
+
+ //printf("enforcePositionConstraints %d: condTol=%g rank=%d rcond=%g\n",
+ // nItsUsed, conditioningTol, Pqwr_qtz.getRank(),
+ // Pqwr_qtz.getRCondEstimate());
+
+ Pqwr_qtz.solve(scaledPerrs, dfq_WLS); // this is weighted dq_WLS=Wq*dq
+ lastChangeMadeWRMS = dfq_WLS.normRMS(); // change in weighted norm
+
+ // switch back to unweighted dq=Wq^+*dq_WLS
+ // = N * Wu^-1 * N^+ * dq_WLS
+ if (hasPrescribedMotion) {
+ unpackFreeQ(s, dfq_WLS, udfq_WLS); // zeroes in q_p slots
+ multiplyByNInv(s,false,udfq_WLS,du);
+ } else {
+ multiplyByNInv(s,false,dfq_WLS,du);
+ }
+
+ du.rowScaleInPlace(ooUWeights); // in place to save memory
+ multiplyByN(s,false,du,dq);
+
+ // This causes quaternions to become unnormalized, but it doesn't
+ // matter because N is calculated from the unnormalized q's so
+ // scales dq to match.
+ updQ(s) -= dq; // this is unweighted dq
+ anyChange = true;
+
+ // Now recalculate the position constraint errors at the new q.
+ realizeSubsystemPosition(s); // pErrs changes here
+
+ scaledPerrs = pErrs.rowScale(ooPTols); // Tp * pErrs
+ normAchievedTRMS = scaledPerrs.normRMS();
+ ++nItsUsed;
+
+ if (localOnly && nItsUsed >= 2
+ && normAchievedTRMS > prevNormAchievedTRMS) {
+ // perr norm got worse; restore to end of previous iteration
+ updQ(s) += dq;
+ realizeSubsystemPosition(s); // pErrs changes here
+ scaledPerrs = pErrs.rowScale(ooPTols);
+ normAchievedTRMS = scaledPerrs.normRMS();
+ break; // diverging -- quit now to prevent a bad solution
+ }
+
+ prevNormAchievedTRMS = normAchievedTRMS;
+
+ } while (normAchievedTRMS > consAccuracyToTryFor
+ && nItsUsed < MaxIterations);
+
+ // Make sure we achieved at least the required constraint accuracy.
+ if (normAchievedTRMS > consAccuracy) {
+ updQ(s) = saveQ; // revert
+ realizeSubsystemPosition(s);
+ SimTK_THROW1(Exception::NewtonRaphsonFailure,
+ "Failed to converge in position projection");
+ }
+
+
+ // Next, if we projected out the position constraint errors, remove the
+ // corresponding error from the integrator's error estimate.
+ //
+ // (Tp Pq Wq^+)_r dqr_WLS = (Tp Pq Wq^+)_r (Wq*qErrest)_r
+ // dq_WLS = unpack(dqr_WLS) (with zero fill)
+ // dq = Wq^+ dq_WLS
+ // = N Wu^-1 N^+ dq_WLS
+ // qErrest -= dq
+ // No iteration is required.
+ //
+ // We can simplify the RHS of the first equation above:
+ // (Tp Pq Wq^+)_r (Wq qErrest)_r = Tp Pq unpack(qErrest_r)
+ // for which we have an O(n) operator to use for the matrix-vector
+ // product. (Proof: expand Pq, Wq^+, and Wq and cancel Wu^-1*Wu and
+ // N^+*N.)
+ if (qErrest.size()) {
+ // Work in Wq-norm
+ Vector Tp_Pq_qErrest, bias_p;
+ calcBiasForMultiplyByPq(s, bias_p);
+
+ // Switch back to unweighted dq=Wq^+*dq_WLS
+ // = N * Wu^-1 * N^+ * dq_WLS
+ if (hasPrescribedMotion) {
+ Vector qErrest_0(qErrest);
+ zeroKnownQ(s, qErrest_0); // zero out prescribed entries
+ multiplyByPq(s, bias_p, qErrest_0, Tp_Pq_qErrest); // (Pq*qErrest)_r
+ Tp_Pq_qErrest.rowScaleInPlace(ooPTols); // now Tp*(Pq*qErrest)_r
+ Pqwr_qtz.solve(Tp_Pq_qErrest, dfq_WLS); // weighted
+ unpackFreeQ(s, dfq_WLS, udfq_WLS); // zeroes in q_p slots
+ multiplyByNInv(s,false,udfq_WLS,du);
+ } else {
+ multiplyByPq(s, bias_p, qErrest, Tp_Pq_qErrest); // Pq*qErrest
+ Tp_Pq_qErrest.rowScaleInPlace(ooPTols); // now Tp*Pq*qErrest
+ Pqwr_qtz.solve(Tp_Pq_qErrest, dfq_WLS); // weighted
+ multiplyByNInv(s,false,dfq_WLS,du);
+ }
+
+ du.rowScaleInPlace(ooUWeights); // in place to save memory
+ multiplyByN(s,false,du,dq);
+ qErrest -= dq; // unweighted
+ }
+ }
+
+ //cout << "!!!! perr TRMS achieved " << normAchievedTRMS << " in "
+ // << nItsUsed << " iterations" << endl;
+
+ // By design, normalization of quaternions can't have any effect on the
+ // length constraints we just fixed (because we normalize internally for
+ // calculations). So now we can simply normalize the quaternions.
+ // Don't touch any q's that are prescribed, though.
+ if (mQuats) {
+ SBStateDigest sbs(s, *this, Stage::Model);
+ Vector& q = updQ(s); // invalidates q's. TODO: see below.
+
+ for (int i=0 ; i<(int)rbNodeLevels.size() ; i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++) {
+ const RigidBodyNode& node = *rbNodeLevels[i][j];
+ const SBInstancePerMobodInfo& mobodInfo =
+ ic.mobodInstanceInfo[node.getNodeNum()];
+ if (mobodInfo.qMethod != Motion::Free)
+ continue;
+
+ if (node.enforceQuaternionConstraints(sbs,q,qErrest))
+ anyChange = true;
+ }
+
+ // This will recalculate the qnorms (all 1), qerrs (all 0). The only
+ // other quaternion dependency is the N matrix (and NInv, NDot).
+ // TODO: better if these updates could be made without invalidating
+ // Position stage in general. I *think* N is always calculated on they fly.
+ realizeSubsystemPosition(s);
+ }
+}
+//........................ ENFORCE POSITION CONSTRAINTS ........................
+
+
+
+//==============================================================================
+// PROJECT Q
+//==============================================================================
+// A note on state variable weights:
+// - q and u weights are not independent
+// - we consider u weights Wu primary and want the weighted variables to be
+// related like the unweighted ones: qdot=N*u so qdotw=N*uw, where
+// uw = Wu*u. So qdotw should be Wq*qdot=N*uw=N*Wu*u=N*Wu*N^+*qdot ==>
+// Wq = N*Wu*N^+. (and Wq^+ = N*Wu^-1*N^+)
+// - Wu is diagonal, but Wq is block diagonal. We have fast operators for
+// multiplying these matrices by columns, but not for producing Wq so we
+// just create it operationally as we go.
+
+int SimbodyMatterSubsystemRep::projectQ
+ (State& s,
+ Vector& qErrest, // q error estimate or empty
+ const ProjectOptions& opts,
+ ProjectResults& results) const
+{
+ SimTK_STAGECHECK_GE(getStage(s), Stage::Position,
+ "SimbodyMatterSubsystemRep::projectQ()");
+
+ results.clear();
+ const Real consAccuracy = opts.getRequiredAccuracy();
+ // Normally we'll use an RMS norm for the perrs.
+ const bool useNormInf = opts.isOptionSet(ProjectOptions::UseInfinityNorm);
+ // Force projection even if accuracy ok on entry.
+ const bool forceOneIter = opts.isOptionSet(ProjectOptions::ForceProjection);
+ // Normally we'll throw an exception with a helpful message. If this is
+ // set we'll quietly return status instead.
+ const bool dontThrow = opts.isOptionSet(ProjectOptions::DontThrow);
+
+ const int mHolo = getNumHolonomicConstraintEquationsInUse(s);
+ const int mQuats = getNumQuaternionsInUse(s);
+
+ // This is a const view into the State; the contents it refers to will
+ // change though.
+ const VectorView pErrs = getQErr(s)(0,mHolo); // just leave off quaternions
+ const VectorView quatErrs = getQErr(s)(mHolo,mQuats); // quaternions
+
+ // We don't weight the quaternion errors.
+ const VectorView perrWeights = getQErrWeights(s)(0,mHolo); // 1/unit error (Tp)
+
+ // Determine norms on entry.
+ int worstPerr, worstQuatErr;
+ Vector scaledPerrs = pErrs.rowScale(perrWeights);
+ const Real perrNormOnEntry = useNormInf ? scaledPerrs.normInf(&worstPerr)
+ : scaledPerrs.normRMS(&worstPerr);
+ const Real quatNormOnEntry = useNormInf ? quatErrs.normInf(&worstQuatErr)
+ : quatErrs.normRMS(&worstQuatErr);
+
+ Real normOnEntry;
+ if (perrNormOnEntry >= quatNormOnEntry) {
+ results.setNormOnEntrance(perrNormOnEntry, worstPerr);
+ normOnEntry = perrNormOnEntry;
+ }
+ else {
+ results.setNormOnEntrance(quatNormOnEntry, mHolo + worstQuatErr);
+ normOnEntry = quatNormOnEntry;
+ }
+
+ if (normOnEntry > opts.getProjectionLimit()) {
+ results.setProjectionLimitExceeded(true);
+ results.setExitStatus(ProjectResults::FailedToConverge);
+ return 1;
+ }
+
+
+ // Return quickly if (a) constraint norm is zero (probably because there
+ // aren't any), or (b) constraints are already satisfied and we're
+ // being forced to go ahead anyway.
+ if ( perrNormOnEntry == 0
+ || (perrNormOnEntry <= consAccuracy && !forceOneIter)) {
+ // Perrs are good enough already. Might still need to project
+ // quaternions, but that doesn't take long. The only way this can
+ // fail is if some of the quaternions are prescribed but prescribedQ()
+ // wasn't called earlier as it should have been..
+ if (quatNormOnEntry > consAccuracy || forceOneIter) {
+ const bool anyQuatChange = normalizeQuaternions(s,qErrest);
+ results.setAnyChangeMade(anyQuatChange);
+ const Real quatNorm = useNormInf ? quatErrs.normInf(&worstQuatErr)
+ : quatErrs.normRMS(&worstQuatErr);
+ results.setNormOnExit(quatNorm);
+ if (quatNorm > consAccuracy) {
+ results.setExitStatus(ProjectResults::FailedToAchieveAccuracy);
+ if (!dontThrow) {
+ SimTK_ERRCHK2_ALWAYS(quatNorm <= consAccuracy,
+ "SimbodyMatterSubsystem::projectQ()",
+ "Failed to normalize quaternions. Norm achieved=%g"
+ " but required norm=%g. Did you forget to call"
+ " prescribeQ()?", quatNorm, consAccuracy);
+ }
+ return 1;
+ }
+ } else { // both perrs and quatErrs were good on entry
+ // numIterations==0.
+ results.setAnyChangeMade(false);
+ results.setNormOnExit(normOnEntry);
+ }
+ results.setExitStatus(ProjectResults::Succeeded);
+ return 0;
+ }
+
+
+ // We're going to have to project constraints. Get the remaining options.
+
+
+ // This is the factor by which we try to achieve a tighter accuracy
+ // than requested. E.g. if overshootFactor=0.1 then we attempt 10X
+ // tighter accuracy if we can get it. But we won't fail as long as
+ // we manage to reach consAccuracy.
+ const Real overshootFactor = opts.getOvershootFactor();
+ const Real consAccuracyToTryFor =
+ std::max(overshootFactor*consAccuracy, SignificantReal);
+
+ // Check whether we should stop if we see the solution diverging
+ // which should not happen when we're in the neighborhood of a solution
+ // on entry. This is always set while integrating, except during
+ // initialization.
+ const bool localOnly = opts.isOptionSet(ProjectOptions::LocalOnly);
+ // We are permitted to use an out-of-date Jacobian for projection unless
+ // this is set. TODO: always using full Newton at the moment.
+ const bool forceFullNewton =
+ opts.isOptionSet(ProjectOptions::ForceFullNewton);
+
+ // Get problem dimensions.
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const int nq = getNQ(s);
+ const int nfq = ic.getTotalNumFreeQ();
+ const int nu = getNU(s);
+ const bool hasPrescribedMotion = (nfq != nq);
+
+ // Solve
+ // (Tp Pq Wq^+) dq_WLS = Tp perr
+ // dq = Wq^+ dq_WLS
+ // q -= dq
+ // until RMS(Tp perr) <= 0.1*accuracy.
+ //
+ // But Pq=P*N^+, Wq^+=N*Wu^-1*N^+ so Pq Wq^+=P*Wu^-1*N^+. Since N^+ N=I,
+ // we can rewrite the above:
+ //
+ // (Tp P Wu^-1 N^+) dq_WLS = Tp perr
+ // dq = N Wu^-1 N^+ dq_WLS
+ // q -= dq
+ //
+ // We define Pqwt = ~Pqw = ~(Tp P Wu^-1 N^+) = ~N^+ Wu^-1 ~P Tp
+ // (diagonal weights are symmetric). We only retain rows that
+ // correspond to free (non prescribed) q's.
+ //
+ // This is a nonlinear least squares problem. Below is a full Newton
+ // iteration since we recalculate the iteration matrix each time around the
+ // loop. TODO: a solution could be found using the same iteration matrix,
+ // since we are projecting from (presumably) not too far away. Q1: will it
+ // be the same solution? Q2: if not, does it matter?
+
+ // These will be updated as we go.
+ Real perrNormAchieved = perrNormOnEntry;
+ Real quatNormAchieved = quatNormOnEntry;
+
+ // We always use absolute scaling for q's, derived from the absolute
+ // scaling of u's.
+ const Vector& uWeights = getUWeights(s); // 1/unit change (Wu)
+ const Vector uAbsScale = uWeights.elementwiseInvert(); // Wu^-1
+
+ Real lastChangeMadeWRMS = 0; // size of last change in weighted dq
+ int nItsUsed = 0;
+
+
+ // Conditioning tolerance. This determines when we'll drop a
+ // constraint.
+ // TODO: this is sloppy; should depend on constraint tolerance
+ // and rank should be saved and reused in velocity and acceleration
+ // constraint methods (or should be calculated elsewhere and passed in).
+ const Real conditioningTol = mHolo
+ //* SignificantReal; -- too tight
+ * SqrtEps;
+
+ // Keep the starting q in case we have to restore it, which we'll do
+ // if the attempts here make the constraint norm worse.
+ const Vector saveQ = getQ(s);
+
+ Matrix Pqwrt(nfq,mHolo);
+ Vector dfq_WLS(nfq), du(nu), dq(nq); // = Wq^+ dq_WLS
+ Vector udfq_WLS(hasPrescribedMotion ? nq : 0); // unpacked if needed
+ udfq_WLS.setToZero(); // must initialize unwritten elements
+ FactorQTZ Pqwr_qtz;
+ Real prevPerrNormAchieved = perrNormAchieved; // watch for divergence
+ bool diverged = false;
+ const int MaxIterations = 20;
+ do {
+ calcWeightedPqrTranspose(s, perrWeights, uAbsScale, Pqwrt);//nfq X mp
+
+ // This factorization acts like a pseudoinverse.
+ Pqwr_qtz.factor<Real>(~Pqwrt, conditioningTol);
+
+ //printf("projectQ %d: m=%d condTol=%g rank=%d rcond=%g\n",
+ // nItsUsed, Pqwrt.ncol(), conditioningTol, Pqwr_qtz.getRank(),
+ // Pqwr_qtz.getRCondEstimate());
+
+ Pqwr_qtz.solve(scaledPerrs, dfq_WLS); // this is weighted dq_WLS=Wq*dq
+ lastChangeMadeWRMS = dfq_WLS.normRMS(); // change in weighted norm
+
+ // switch back to unweighted dq=Wq^+*dq_WLS
+ // = N * Wu^-1 * N^+ * dq_WLS
+ if (hasPrescribedMotion) {
+ unpackFreeQ(s, dfq_WLS, udfq_WLS); // zeroes in q_p slots
+ multiplyByNInv(s,false,udfq_WLS,du);
+ } else {
+ multiplyByNInv(s,false,dfq_WLS,du);
+ }
+ // Here du = du_WLS = N^+ * dq_WLS
+ du.rowScaleInPlace(uAbsScale); // Now du = Wu^-1 * du_WLS.
+ multiplyByN(s,false,du,dq); // dq = N*du
+
+ // This causes quaternions to become unnormalized, but it doesn't
+ // matter because N is calculated from the unnormalized q's so
+ // scales dq to match.
+ updQ(s) -= dq; // this is unweighted dq
+ results.setAnyChangeMade(true);
+
+ // Now recalculate the position constraint errors at the new q.
+ realizeSubsystemPosition(s); // pErrs changes here
+
+ scaledPerrs = pErrs.rowScale(perrWeights); // Tp * pErrs
+ perrNormAchieved = useNormInf ? scaledPerrs.normInf()
+ : scaledPerrs.normRMS();
+ ++nItsUsed;
+
+ if (localOnly && nItsUsed >= 2
+ && perrNormAchieved > prevPerrNormAchieved) {
+ // perr norm got worse; restore to end of previous iteration
+ updQ(s) += dq;
+ realizeSubsystemPosition(s); // pErrs changes here
+ scaledPerrs = pErrs.rowScale(perrWeights);
+ perrNormAchieved = useNormInf ? scaledPerrs.normInf()
+ : scaledPerrs.normRMS();
+ diverged = true;
+ break; // diverging -- quit now to prevent a bad solution
+ }
+
+ prevPerrNormAchieved = perrNormAchieved;
+
+ } while (perrNormAchieved > consAccuracyToTryFor
+ && nItsUsed < MaxIterations);
+
+ results.setNumIterations(nItsUsed);
+
+ //printf(" perrNormAchieved=%g in %d its\n",perrNormAchieved, nItsUsed);
+
+ // Make sure we achieved at least the required constraint accuracy. If not
+ // we'll return with an error. If we see that the norm has been made worse
+ // than it was on entry, we'll restore the state to what it was on entry.
+ // Otherwise we'll return with the improved-but-not-good-enough result.
+ if (perrNormAchieved > consAccuracy) {
+ if (perrNormAchieved >= perrNormOnEntry) { // made it worse
+ updQ(s) = saveQ; // revert
+ realizeSubsystemPosition(s);
+ perrNormAchieved = perrNormOnEntry;
+ }
+
+ results.setNormOnExit(perrNormAchieved);
+
+ if (diverged) {
+ results.setExitStatus(ProjectResults::FailedToConverge);
+ if (!dontThrow) {
+ SimTK_ERRCHK_ALWAYS(!diverged,
+ "SimbodyMatterSubsystem::projectQ()",
+ "Attempt to project constraints locally diverged.");
+ }
+ } else {
+ results.setExitStatus(ProjectResults::FailedToAchieveAccuracy);
+ if (!dontThrow) {
+ SimTK_ERRCHK3_ALWAYS(perrNormAchieved <= consAccuracy,
+ "SimbodyMatterSubsystem::projectQ()",
+ "Failed to achieve required accuracy %g. Norm on entry "
+ " was %g; norm on exit %g. You might need a better"
+ " starting configuration, or if there are prescribed or "
+ " locked q's you might have to free some of them.",
+ consAccuracy, perrNormOnEntry, perrNormAchieved);
+ }
+ }
+
+ return 1;
+ }
+
+ // Position constraint errors were successfully driven to consAccuracy.
+
+ // Next, remove the corresponding error from the integrator's error
+ // estimate.
+ //
+ // (Tp Pq Wq^+)_r dqr_WLS = (Tp Pq Wq^+)_r (Wq*qErrest)_r
+ // dq_WLS = unpack(dqr_WLS) (with zero fill)
+ // dq = Wq^+ dq_WLS
+ // = N Wu^-1 N^+ dq_WLS
+ // qErrest -= dq
+ // No iteration is required.
+ //
+ // We can simplify the RHS of the first equation above:
+ // (Tp Pq Wq^+)_r (Wq qErrest)_r = Tp Pq unpack(qErrest_r)
+ // for which we have an O(n) operator to use for the matrix-vector
+ // product. (Proof: expand Pq, Wq^+, and Wq and cancel Wu^-1*Wu and
+ // N^+*N.)
+ if (qErrest.size()) {
+ // Work in Wq-norm
+ Vector Tp_Pq_qErrest, bias_p;
+ calcBiasForMultiplyByPq(s, bias_p);
+
+ // Switch back to unweighted dq = Wq^+ * dq_WLS
+ // = N * Wu^-1 * N^+ * dq_WLS
+ if (hasPrescribedMotion) {
+ Vector qErrest_0(qErrest);
+ zeroKnownQ(s, qErrest_0); // zero out prescribed entries
+ multiplyByPq(s, bias_p, qErrest_0, Tp_Pq_qErrest); // (Pq*qErrest)_r
+ Tp_Pq_qErrest.rowScaleInPlace(perrWeights); // now Tp*(Pq*qErrest)_r
+ Pqwr_qtz.solve(Tp_Pq_qErrest, dfq_WLS); // weighted
+ unpackFreeQ(s, dfq_WLS, udfq_WLS); // zeroes in q_p slots
+ multiplyByNInv(s,false,udfq_WLS,du);
+ } else {
+ multiplyByPq(s, bias_p, qErrest, Tp_Pq_qErrest); // Pq*qErrest
+ Tp_Pq_qErrest.rowScaleInPlace(perrWeights); // now Tp*Pq*qErrest
+ Pqwr_qtz.solve(Tp_Pq_qErrest, dfq_WLS); // weighted
+ multiplyByNInv(s,false,dfq_WLS,du);
+ }
+ // Here du = du_WLS = N^+ * dq_WLS
+ du.rowScaleInPlace(uAbsScale); // now du = Wu^-1 * du_WLS
+ multiplyByN(s,false,du,dq); // dq = N*du
+ qErrest -= dq; // unweighted
+ }
+
+ //cout << "!!!! perr TRMS achieved " << normAchievedTRMS << " in "
+ // << nItsUsed << " iterations" << endl;
+
+ // By design, normalization of quaternions can't have any effect on the
+ // constraints we just fixed (because we normalize internally for
+ // calculations). So now we can simply normalize the quaternions.
+ // We can't touch any q's that are prescribed, though, so it is
+ // possible that we'll fail to achieve the required tolerance if some
+ // quaterion is prescribed but not up to date.
+ if (mQuats) {
+ const bool anyQuatChange = normalizeQuaternions(s,qErrest);
+ if (anyQuatChange) results.setAnyChangeMade(true);
+ quatNormAchieved = useNormInf ? quatErrs.normInf(&worstQuatErr)
+ : quatErrs.normRMS(&worstQuatErr);
+ if (quatNormAchieved > consAccuracy) {
+ results.setNormOnExit(quatNormAchieved);
+ results.setExitStatus(ProjectResults::FailedToAchieveAccuracy);
+ if (!dontThrow) {
+ SimTK_ERRCHK2_ALWAYS(quatNormAchieved <= consAccuracy,
+ "SimbodyMatterSubsystem::projectQ()",
+ "Failed to normalize quaternions. Norm achieved=%g"
+ " but required norm=%g. Did you forget to call"
+ " prescribeQ()?", quatNormAchieved, consAccuracy);
+ }
+ return 1;
+ }
+ }
+ results.setNormOnExit(std::max(perrNormAchieved, quatNormAchieved));
+ results.setExitStatus(ProjectResults::Succeeded);
+ return 0;
+}
+//................................. PROJECT Q ..................................
+
+// Project quaternions onto their constraint manifold by normalizing
+// them. Also removes any error component along the length of the
+// quaternions if given a qErrest. Returns true if any change was made.
+bool SimbodyMatterSubsystemRep::normalizeQuaternions
+ (State& s, Vector& qErrest) const
+{
+ SBStateDigest sbs(s, *this, Stage::Model);
+ const SBInstanceCache& ic = getInstanceCache(s);
+
+ Vector& q = updQ(s); // invalidates q's. TODO: see below.
+
+ bool anyChangeMade = false;
+ for (int i=0; i<(int)rbNodeLevels.size(); ++i)
+ for (int j=0; j<(int)rbNodeLevels[i].size(); ++j) {
+ const RigidBodyNode& node = *rbNodeLevels[i][j];
+ const SBInstancePerMobodInfo& mobodInfo =
+ ic.mobodInstanceInfo[node.getNodeNum()];
+ if (mobodInfo.qMethod != Motion::Free)
+ continue;
+
+ if (node.enforceQuaternionConstraints(sbs,q,qErrest))
+ anyChangeMade = true;
+ }
+
+ // This will recalculate the qnorms (all 1), qerrs (all 0). The only
+ // other quaternion dependency is the N matrix (and NInv, NDot).
+ // TODO: better if these updates could be made without invalidating
+ // Position stage in general. I *think* N is always calculated on the fly.
+ realizeSubsystemPosition(s);
+ return anyChangeMade;
+}
+
+
+
+//==============================================================================
+// ENFORCE VELOCITY CONSTRAINTS
+//==============================================================================
+void SimbodyMatterSubsystemRep::enforceVelocityConstraints
+ (State& s, Real consAccuracy, const Vector& yWeights,
+ const Vector& ooTols, Vector& yErrest, ProjectOptions opts) const
+{
+ assert(getStage(s) >= Stage::Velocity-1);
+
+ const SBInstanceCache& ic = getInstanceCache(s);
+
+ realizeSubsystemVelocity(s);
+
+ // Here we deal with the nonholonomic (velocity) constraints and the
+ // derivatives of the holonomic constraints.
+ const int mHolo = getNumHolonomicConstraintEquationsInUse(s);
+ const int mNonholo = getNumNonholonomicConstraintEquationsInUse(s);
+ const int nq = getNQ(s);
+ const int nu = getNU(s);
+ const int nfu = ic.getTotalNumFreeU();
+ bool hasPrescribedMotion = (nfu != nu);
+
+ const VectorView uWeights = yWeights(nq,nu); // skip the first nq weights
+ const Vector ooUWeights = uWeights.elementwiseInvert();
+ const VectorView ooPVTols = ooTols(0,mHolo+mNonholo);
+ //TODO: scale holo part by time scale
+
+ VectorView uErrest = yErrest.size() ? yErrest(nq,nu) : yErrest(0,0);
+
+ // This is a const view into the State; the contents it refers to will
+ // change though.
+ const Vector& vErrs = getUErr(s); // all velocity constraint errors (mHolo+mNonholo)
+
+ bool anyChange = false;
+
+ // Solve
+ // (Tpv [P;V] Wu^-1) du_WLS = Tpv uerr
+ // du = Wu^-1*du_WLS
+ // u -= du
+ // Note that although this is a nonlinear least squares problem since uerr
+ // is a function of u, we do not need to refactor the matrix since it does
+ // not depend on u.
+ // TODO: I don't think that's true -- V can depend on u (rarely). That
+ // doesn't mean we need to refactor it, but then this is a modified Newton
+ // iteration (rather than full) if we're not updating V when we could be.
+ //
+ // This is a nonlinear least squares problem, but we only need to factor
+ // once since only the RHS is dependent on u (TODO: see above).
+ Vector scaledVerrs = vErrs.rowScale(ooPVTols);
+ Real normAchievedTRMS = scaledVerrs.normRMS();
+
+ //cout << "!!!! initially @" << s.getTime() << ", verr TRMS="
+ // << normAchievedTRMS << " consAcc=" << consAccuracy;
+ //if (uErrest.size())
+ // cout << " uErrest WRMS=" << uErrest.rowScale(uWeights).normRMS();
+ //else cout << " NO U ERROR ESTIMATE";
+ //cout << endl;
+
+
+ Real lastChangeMadeWRMS = 0;
+ int nItsUsed = 0;
+
+ // Check whether we should stop if we see the solution diverging
+ // which should not happen when we're in the neighborhood of a solution
+ // on entry.
+ const bool localOnly = opts.isOptionSet(ProjectOptions::LocalOnly);
+
+ // Set how far past the required tolerance we'll attempt to go.
+ // We only fail if we can't achieve consAccuracy, but while we're
+ // solving we'll see if we can get consAccuracyToTryFor.
+ const Real consAccuracyToTryFor =
+ std::max(Real(0.1)*consAccuracy, SignificantReal);
+
+ // Conditioning tolerance. This determines when we'll drop a
+ // constraint.
+ // TODO: this is much too tight; should depend on constraint tolerance
+ // and should be consistent with the holonomic rank.
+ const Real conditioningTol = (mHolo+mNonholo)
+ //* SignificantReal;
+ * SqrtEps;
+
+ if (normAchievedTRMS > consAccuracyToTryFor) {
+ const Vector saveU = getU(s);
+ Matrix PVwrt(nfu, mHolo+mNonholo);
+ Vector dfu_WLS(nfu);
+ Vector du(nu); // unpacked into here if necessary
+ if (hasPrescribedMotion)
+ du.setToZero(); // must initialize unwritten elements
+
+ calcWeightedPVrTranspose(s, ooPVTols, ooUWeights, PVwrt);
+ // PVwrt is now Wu^-1 (Pt Vt) Tpv
+
+ // Calculate pseudoinverse (just once)
+ FactorQTZ PVwr_qtz;
+ PVwr_qtz.factor<Real>(~PVwrt, conditioningTol);
+
+ Real prevNormAchievedTRMS = normAchievedTRMS; // watch for divergence
+ const int MaxIterations = 7;
+ do {
+ PVwr_qtz.solve(scaledVerrs, dfu_WLS);
+ lastChangeMadeWRMS = dfu_WLS.normRMS(); // change in weighted norm
+ if (hasPrescribedMotion) {
+ unpackFreeU(s, dfu_WLS, du); // zeroes in u_p slots
+ du.rowScaleInPlace(ooUWeights); // du=Wu^-1*unpack(dfu_WLS)
+ } else {
+ du = dfu_WLS.rowScale(ooUWeights); // unscale: du=Wu^-1*du_WLS
+ }
+ updU(s) -= du;
+ anyChange = true;
+
+ // Recalculate the constraint errors for the new u's.
+ realizeSubsystemVelocity(s);
+ scaledVerrs = vErrs.rowScale(ooPVTols);
+ normAchievedTRMS=scaledVerrs.normRMS();
+ ++nItsUsed;
+
+ if (localOnly && nItsUsed >= 2
+ && normAchievedTRMS > prevNormAchievedTRMS) {
+ // Velocity norm worse -- restore to end of previous iteration.
+ updU(s) += du;
+ realizeSubsystemVelocity(s);
+ scaledVerrs = vErrs.rowScale(ooPVTols);
+ normAchievedTRMS=scaledVerrs.normRMS();
+ break; // diverging -- quit now to prevent a bad solution
+ }
+
+ prevNormAchievedTRMS = normAchievedTRMS;
+
+ } while (normAchievedTRMS > consAccuracyToTryFor
+ && nItsUsed < MaxIterations);
+
+ // Make sure we achieved at least the required constraint accuracy.
+ if (normAchievedTRMS > consAccuracy) {
+ updU(s) = saveU;
+ realizeSubsystemVelocity(s);
+ SimTK_THROW1(Exception::NewtonRaphsonFailure,
+ "Failed to converge in velocity projection");
+ }
+
+ // Next, if we projected out the velocity constraint errors, remove the
+ // corresponding error from the integrator's error estimate.
+ //
+ // (Tpv [P;V] Wu^-1)_f dfu_WLS = (Tpv [P;V] Wu^-1)_f (Wu uErrest)_f
+ // du_WLS = unpack(dfu_WLS) (with zero fill)
+ // du = Wu^-1 du_WLS
+ // uErrest -= du
+ // No iteration is required.
+ //
+ // We can simplify the RHS of the first equation above:
+ // (Tpv [P;V] Wu^-1)_f (Wu uErrest)_f = Tpv [P;V] unpack(uErrest_f)
+ // for which we have an O(n) operator to compute the matrix-vector
+ // product.
+
+ if (uErrest.size()) {
+ // Work in Wu-norm
+ Vector Tpv_PV_uErrest(mHolo+mNonholo);
+ Vector bias_pv(mHolo+mNonholo);
+ calcBiasForMultiplyByPVA(s,true,true,false,bias_pv); // just P,V
+ if (hasPrescribedMotion) {
+ Vector uErrest_0(uErrest);
+ zeroKnownU(s, uErrest_0); // zero out prescribed entries
+ multiplyByPVA(s,true,true,false,bias_pv,
+ uErrest_0,Tpv_PV_uErrest);
+ Tpv_PV_uErrest.rowScaleInPlace(ooPVTols); // = Tpv*PV*uErrest_0
+ PVwr_qtz.solve(Tpv_PV_uErrest, dfu_WLS);
+ unpackFreeU(s, dfu_WLS, du); // still weighted
+ } else {
+ multiplyByPVA(s,true,true,false,bias_pv,uErrest,Tpv_PV_uErrest);
+ Tpv_PV_uErrest.rowScaleInPlace(ooPVTols); // = Tpv PV uErrEst
+ PVwr_qtz.solve(Tpv_PV_uErrest, du);
+ }
+ du.rowScaleInPlace(ooUWeights); // now du=Wu^-1*unpack(dfu_WLS)
+ uErrest -= du; // this is unweighted now
+ }
+ }
+
+ //cout << "!!!! verr achieved " << normAchievedTRMS << " in "
+ // << nItsUsed << " iterations" << endl;
+ //if (uErrest.size())
+ // cout << " uErrest WRMS=" << uErrest.rowScale(uWeights).normRMS() << endl;
+}
+//........................ ENFORCE VELOCITY CONSTRAINTS ........................
+
+
+//==============================================================================
+// PROJECT U
+//==============================================================================
+int SimbodyMatterSubsystemRep::projectU
+ (State& s,
+ Vector& uErrest, // u error estimate or empty
+ const ProjectOptions& opts,
+ ProjectResults& results) const
+{
+ SimTK_STAGECHECK_GE(getStage(s), Stage::Velocity,
+ "SimbodyMatterSubsystemRep::projectU()");
+
+ results.clear();
+ const Real consAccuracy = opts.getRequiredAccuracy();
+ // Normally we'll use an RMS norm for the perrs.
+ const bool useNormInf = opts.isOptionSet(ProjectOptions::UseInfinityNorm);
+ // Force projection even if accuracy ok on entry.
+ const bool forceOneIter = opts.isOptionSet(ProjectOptions::ForceProjection);
+ // Normally we'll throw an exception with a helpful message. If this is
+ // set we'll quietly return status instead.
+ const bool dontThrow = opts.isOptionSet(ProjectOptions::DontThrow);
+
+ // Here we deal with the nonholonomic (velocity) constraints and the
+ // derivatives of the holonomic constraints.
+ const int mHolo = getNumHolonomicConstraintEquationsInUse(s);
+ const int mNonholo = getNumNonholonomicConstraintEquationsInUse(s);
+
+ // This is a const view into the State; the contents it refers to will
+ // change though. These are all the velocity-level constraint errors,
+ // mHolo from differentiating holonomic constraints and mNonholo directly
+ // from the nonholonomic constraints.
+ const Vector& pvErrs = getUErr(s); // mHolo+mNonholo of these
+ const Vector& pverrWeights = getUErrWeights(s); // 1/unit err (Tpv)
+
+ // Determine norm on entry.
+ int worstPVerr;
+ Vector scaledPVerrs = pvErrs.rowScale(pverrWeights);
+ const Real pverrNormOnEntry = useNormInf ? scaledPVerrs.normInf(&worstPVerr)
+ : scaledPVerrs.normRMS(&worstPVerr);
+
+ results.setNormOnEntrance(pverrNormOnEntry, worstPVerr);
+
+ if (pverrNormOnEntry > opts.getProjectionLimit()) {
+ results.setProjectionLimitExceeded(true);
+ results.setExitStatus(ProjectResults::FailedToConverge);
+ return 1;
+ }
+
+ //cout << "!!!! initially @" << s.getTime() << ", verr TRMS="
+ // << normAchievedTRMS << " consAcc=" << consAccuracy;
+ //if (uErrest.size())
+ // cout << " uErrest WRMS=" << uErrest.rowScale(uWeights).normRMS();
+ //else cout << " NO U ERROR ESTIMATE";
+ //cout << endl;
+
+
+ // Return quickly if (a) constraint norm is zero (probably because there
+ // aren't any), or (b) constraints are already satisfied and we're
+ // being forced to go ahead anyway.
+ if ( pverrNormOnEntry == 0
+ || (pverrNormOnEntry <= consAccuracy && !forceOneIter)) {
+ // numIterations==0.
+ results.setAnyChangeMade(false);
+ results.setNormOnExit(pverrNormOnEntry);
+ results.setExitStatus(ProjectResults::Succeeded);
+ return 0;
+ }
+
+ // We're going to have to project constraints. Get the remaining options.
+
+ // This is the factor by which we try to achieve a tighter accuracy
+ // than requested. E.g. if overshootFactor=0.1 then we attempt 10X
+ // tighter accuracy if we can get it. But we won't fail as long as
+ // we manage to reach consAccuracy.
+ const Real overshootFactor = opts.getOvershootFactor();
+ const Real consAccuracyToTryFor =
+ std::max(overshootFactor*consAccuracy, SignificantReal);
+
+ // Check whether we should stop if we see the solution diverging
+ // which should not happen when we're in the neighborhood of a solution
+ // on entry. This is always set while integrating, except during
+ // initialization.
+ const bool localOnly = opts.isOptionSet(ProjectOptions::LocalOnly);
+ // We are permitted to use an out-of-date Jacobian for projection unless
+ // this is set. TODO: always using modified Newton at the moment.
+ const bool forceFullNewton =
+ opts.isOptionSet(ProjectOptions::ForceFullNewton);
+
+ // Get problem dimensions.
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const int nq = getNQ(s);
+ const int nu = getNU(s);
+ const int nfu = ic.getTotalNumFreeU();
+ bool hasPrescribedMotion = (nfu != nu);
+
+ // Solve
+ // (Tpv [P;V] Wu^-1) du_WLS = Tpv uerr
+ // du = Wu^-1*du_WLS
+ // u -= du
+ // Note that although this is a nonlinear least squares problem since uerr
+ // is a function of u, we do not need to refactor the matrix since it does
+ // not depend on u.
+ // TODO: I don't think that's true -- V can depend on u (rarely). That
+ // doesn't mean we need to refactor it, but then this is a modified Newton
+ // iteration (rather than full) if we're not updating V when we could be.
+ //
+ // This is a nonlinear least squares problem, but we only need to factor
+ // once since only the RHS is dependent on u (TODO: see above).
+
+ // This will be updated as we go.
+ Real pverrNormAchieved = pverrNormOnEntry;
+
+
+ // Calculate relative scaling for changes to u.
+ const Vector& u = getU(s);
+ const Vector& uWeights = getUWeights(s); // 1/unit change (Wu)
+ Vector uRelScale(nu);
+ for (int i=0; i<nu; ++i) {
+ const Real ui = std::abs(u[i]);
+ const Real wi = uWeights[i];
+ uRelScale[i] = ui*wi > 1 ? ui : 1/wi; // max(unit error, u) (1/Eu)
+ }
+
+ Real lastChangeMadeWRMS = 0;
+ int nItsUsed = 0;
+
+ // Conditioning tolerance. This determines when we'll drop a
+ // constraint.
+ // TODO: this is much too tight; should depend on constraint tolerance
+ // and should be consistent with the holonomic rank.
+ const Real conditioningTol = (mHolo+mNonholo)
+ //* SignificantReal;
+ * SqrtEps;
+
+ // Keep the starting u in case we have to restore it, which we'll do
+ // if the attempts here make the constraint norm worse.
+ const Vector saveU = getU(s);
+
+ Matrix PVwrt(nfu, mHolo+mNonholo);
+ Vector dfu_WLS(nfu);
+ Vector du(nu); // unpacked into here if necessary
+ if (hasPrescribedMotion)
+ du.setToZero(); // must initialize unwritten elements
+
+ calcWeightedPVrTranspose(s, pverrWeights, uRelScale, PVwrt);
+ // PVwrt is now Eu^-1 (Pt Vt) Tpv
+
+ // Calculate pseudoinverse (just once)
+ FactorQTZ PVwr_qtz;
+ PVwr_qtz.factor<Real>(~PVwrt, conditioningTol);
+
+ //printf("projectU m=%d condTol=%g rank=%d rcond=%g\n",
+ // PVwrt.ncol(), conditioningTol, PVwr_qtz.getRank(),
+ // PVwr_qtz.getRCondEstimate());
+
+ Real prevPVerrNormAchieved = pverrNormAchieved; // watch for divergence
+ bool diverged = false;
+ const int MaxIterations = 7;
+ do {
+ PVwr_qtz.solve(scaledPVerrs, dfu_WLS);
+ lastChangeMadeWRMS = dfu_WLS.normRMS(); // change in weighted norm
+
+ // switch back to unweighted du=Eu^-1*du_WLS
+ if (hasPrescribedMotion) {
+ unpackFreeU(s, dfu_WLS, du); // zeroes in u_p slots
+ du.rowScaleInPlace(uRelScale); // du=Eu^-1*unpack(dfu_WLS)
+ } else {
+ du = dfu_WLS.rowScale(uRelScale); // unscale: du=Eu^-1*du_WLS
+ }
+ updU(s) -= du;
+ results.setAnyChangeMade(true);
+
+ // Recalculate the constraint errors for the new u's.
+ realizeSubsystemVelocity(s);
+ scaledPVerrs = pvErrs.rowScale(pverrWeights); // Tpv * pvErrs
+ pverrNormAchieved = useNormInf ? scaledPVerrs.normInf()
+ : scaledPVerrs.normRMS();
+ ++nItsUsed;
+
+ if (localOnly && nItsUsed >= 2
+ && pverrNormAchieved > prevPVerrNormAchieved) {
+ // Velocity norm worse -- restore to end of previous iteration.
+ updU(s) += du;
+ realizeSubsystemVelocity(s); // pvErrs changes here
+ scaledPVerrs = pvErrs.rowScale(pverrWeights);
+ pverrNormAchieved = useNormInf ? scaledPVerrs.normInf()
+ : scaledPVerrs.normRMS();
+ diverged = true;
+ break; // diverging -- quit now to prevent a bad solution
+ }
+
+ prevPVerrNormAchieved = pverrNormAchieved;
+
+ } while (pverrNormAchieved > consAccuracyToTryFor
+ && nItsUsed < MaxIterations);
+
+ results.setNumIterations(nItsUsed);
+
+ // Make sure we achieved at least the required constraint accuracy. If not
+ // we'll return with an error. If we see that the norm has been made worse
+ // than it was on entry, we'll restore the state to what it was on entry.
+ // Otherwise we'll return with the improved-but-not-good-enough result.
+ if (pverrNormAchieved > consAccuracy) {
+ if (pverrNormAchieved >= pverrNormOnEntry) { // made it worse
+ updU(s) = saveU; // revert
+ realizeSubsystemVelocity(s);
+ pverrNormAchieved = pverrNormOnEntry;
+ }
+
+ results.setNormOnExit(pverrNormAchieved);
+
+ if (diverged) {
+ results.setExitStatus(ProjectResults::FailedToConverge);
+ if (!dontThrow) {
+ SimTK_ERRCHK_ALWAYS(!diverged,
+ "SimbodyMatterSubsystem::projectU()",
+ "Attempt to project constraints locally diverged.");
+ }
+ } else {
+ results.setExitStatus(ProjectResults::FailedToAchieveAccuracy);
+ if (!dontThrow) {
+ SimTK_ERRCHK3_ALWAYS(pverrNormAchieved <= consAccuracy,
+ "SimbodyMatterSubsystem::projectU()",
+ "Failed to achieve required accuracy %g. Norm on entry "
+ " was %g; norm on exit %g. You might need a better"
+ " starting configuration, or if there are prescribed or "
+ " locked u's you might have to free some of them.",
+ consAccuracy, pverrNormOnEntry, pverrNormAchieved);
+ }
+ }
+
+ return 1;
+ }
+
+ // Velocity constraint errors were successfully driven to consAccuracy.
+
+ // Next, if we projected out the velocity constraint errors, remove the
+ // corresponding error from the integrator's error estimate.
+ //
+ // (Tpv [P;V] Wu^-1)_f dfu_WLS = (Tpv [P;V] Wu^-1)_f (Wu uErrest)_f
+ // du_WLS = unpack(dfu_WLS) (with zero fill)
+ // du = Wu^-1 du_WLS
+ // uErrest -= du
+ // No iteration is required.
+ //
+ // We can simplify the RHS of the first equation above:
+ // (Tpv [P;V] Wu^-1)_f (Wu uErrest)_f = Tpv [P;V] unpack(uErrest_f)
+ // for which we have an O(n) operator to compute the matrix-vector
+ // product.
+
+ if (uErrest.size()) {
+ // Work in Wu-norm
+ Vector Tpv_PV_uErrest(mHolo+mNonholo);
+ Vector bias_pv(mHolo+mNonholo);
+ calcBiasForMultiplyByPVA(s,true,true,false,bias_pv); // just P,V
+ if (hasPrescribedMotion) {
+ Vector uErrest_0(uErrest);
+ zeroKnownU(s, uErrest_0); // zero out prescribed entries
+ multiplyByPVA(s,true,true,false,bias_pv,
+ uErrest_0,Tpv_PV_uErrest);
+ Tpv_PV_uErrest.rowScaleInPlace(pverrWeights); // = Tpv*PV*uErrest_0
+ PVwr_qtz.solve(Tpv_PV_uErrest, dfu_WLS);
+ unpackFreeU(s, dfu_WLS, du); // still weighted
+ } else {
+ multiplyByPVA(s,true,true,false,bias_pv,uErrest,Tpv_PV_uErrest);
+ Tpv_PV_uErrest.rowScaleInPlace(pverrWeights); // = Tpv PV uErrEst
+ PVwr_qtz.solve(Tpv_PV_uErrest, du);
+ }
+ du.rowScaleInPlace(uRelScale); // now du=Eu^-1*unpack(dfu_WLS)
+ uErrest -= du; // this is unweighted now
+ }
+
+ //cout << "!!!! verr achieved " << pverrNormAchieved << " in "
+ // << nItsUsed << " iterations" << endl;
+ //if (uErrest.size())
+ // cout << " uErrest WRMS=" << uErrest.rowScale(uWeights).normRMS() << endl;
+
+ results.setNormOnExit(pverrNormAchieved);
+ results.setExitStatus(ProjectResults::Succeeded);
+ return 0;
+}
+//................................ PROJECT U ...................................
+
+
+
+//==============================================================================
+// CALC TREE FORWARD DYNAMICS OPERATOR
+//==============================================================================
+// Given a State realized through Stage::Dynamics, and a complete set of applied
+// forces, calculate all acceleration results into the return arguments here.
+// This routine *does not* affect the State cache -- it is an operator. In
+// typical usage, the output arguments actually will be part of the state cache
+// to effect a response, but this method can also be used to effect an operator.
+//
+// Note that although acceleration constraint errors will be calculated, the
+// returned accelerations will not obey the constraints, unless the supplied
+// forces already account for constraints. The argument list allows for some
+// extra forces to be supplied, with the intent that these will be used to deal
+// with internal forces generated by constraints. Note that the extra forces
+// here are treated with opposite sign from the applied forces, as is
+// appropriate for constraint forces.
+void SimbodyMatterSubsystemRep::calcTreeForwardDynamicsOperator(
+ const State& s,
+ const Vector& mobilityForces,
+ const Vector_<Vec3>& particleForces,
+ const Vector_<SpatialVec>& bodyForces,
+ const Vector* extraMobilityForces,
+ const Vector_<SpatialVec>* extraBodyForces,
+ SBTreeAccelerationCache& tac, // accels, prescribed forces go here
+ Vector& udot, // in/out (in for prescribed udot)
+ Vector& qdotdot,
+ Vector& udotErr) const
+{
+ SBStateDigest sbs(s, *this, Stage::Acceleration);
+
+ const SBModelCache& mc = sbs.getModelCache();
+ const SBInstanceCache& ic = sbs.getInstanceCache();
+ const SBTreePositionCache& tpc = sbs.getTreePositionCache();
+ const SBTreeVelocityCache& tvc = sbs.getTreeVelocityCache();
+ const SBDynamicsCache& dc = sbs.getDynamicsCache();
+
+ // Ensure that output arguments have been allocated properly.
+ tac.allocate(topologyCache, mc, ic);
+ udot.resize(topologyCache.nDOFs);
+ qdotdot.resize(topologyCache.maxNQs);
+
+ Vector totalMobilityForces;
+ Vector_<SpatialVec> totalBodyForces;
+
+ // inputs
+
+ // First assume we'll use the input forces as-is.
+ const Vector* mobilityForcesToUse = &mobilityForces;
+ const Vector_<SpatialVec>* bodyForcesToUse = &bodyForces;
+
+ if (extraMobilityForces) {
+ totalMobilityForces = mobilityForces - *extraMobilityForces; // note sign
+ mobilityForcesToUse = &totalMobilityForces;
+ }
+
+ if (extraBodyForces) {
+ totalBodyForces = bodyForces - *extraBodyForces; // note sign
+ bodyForcesToUse = &totalBodyForces;
+ }
+
+ // outputs
+ Vector& netHingeForces = tac.epsilon;
+ Array_<SpatialVec,MobilizedBodyIndex>&
+ abForcesZ = tac.z;
+ Array_<SpatialVec,MobilizedBodyIndex>&
+ abForcesZPlus = tac.zPlus;
+ Vector_<SpatialVec>& A_GB = tac.bodyAccelerationInGround;
+ Vector& tau = tac.presMotionForces;
+
+ // Calculate accelerations produced by these forces in three forms:
+ // body accelerations A_GB, u-space generalized accelerations udot,
+ // and q-space generalized accelerations qdotdot.
+ calcTreeAccelerations
+ (s, *mobilityForcesToUse, *bodyForcesToUse, dc.presUDotPool,
+ netHingeForces, abForcesZ, abForcesZPlus,
+ A_GB, udot, qdotdot, tau);
+
+ // Feed the accelerations into the constraint error methods to determine
+ // the acceleration constraint errors they generate.
+ calcConstraintAccelerationErrors(s, A_GB, udot, qdotdot, udotErr);
+}
+//......................CALC TREE FORWARD DYNAMICS OPERATOR ....................
+
+
+
+//==============================================================================
+// REALIZE TREE FORWARD DYNAMICS
+//==============================================================================
+// This is the response version of the above operator; that is, it uses
+// the operator but puts the results in the State cache. Note that this
+// only makes sense if the force arguments also come from the State
+// somewhere else in the System that includes this Subsystem.
+void SimbodyMatterSubsystemRep::realizeTreeForwardDynamics(
+ const State& s,
+ const Vector& mobilityForces,
+ const Vector_<Vec3>& particleForces,
+ const Vector_<SpatialVec>& bodyForces,
+ const Vector* extraMobilityForces,
+ const Vector_<SpatialVec>* extraBodyForces) const
+{
+ const SBModelCache& mc = getModelCache(s);
+
+ // Output goes into State's global cache and our AccelerationCache.
+ SBTreeAccelerationCache& tac = updTreeAccelerationCache(s);
+ Vector& udot = updUDot(s);
+ Vector& qdotdot = updQDotDot(s);
+ Vector& udotErr = updUDotErr(s);
+
+ calcTreeForwardDynamicsOperator
+ (s, mobilityForces, particleForces, bodyForces,
+ extraMobilityForces, extraBodyForces,
+ tac, udot, qdotdot, udotErr);
+
+ // Since we're realizing, mark the resulting cache entry valid.
+ markCacheValueRealized(s, mc.treeAccelerationCacheIndex);
+}
+//....................... REALIZE TREE FORWARD DYNAMICS ........................
+
+
+
+//==============================================================================
+// CALC LOOP FORWARD DYNAMICS OPERATOR
+//==============================================================================
+// Given a State realized through Stage::Dynamics, and a complete set of applied
+// forces, calculate all acceleration results resulting from those forces AND
+// enforcement of the acceleration constraints. The results go into the return
+// arguments here. This routine *does not* affect the State cache -- it is an
+// operator. In typical usage, the output arguments actually will be part of
+// the state cache to effect a response, but this method can also be used to
+// effect an operator.
+void SimbodyMatterSubsystemRep::calcLoopForwardDynamicsOperator
+ (const State& s,
+ const Vector& mobilityForces,
+ const Vector_<Vec3>& particleForces,
+ const Vector_<SpatialVec>& bodyForces,
+ SBTreeAccelerationCache& tac,
+ SBConstrainedAccelerationCache& cac,
+ Vector& udot,
+ Vector& qdotdot,
+ Vector& multipliers,
+ Vector& udotErr) const
+{
+ assert(getStage(s) >= Stage::Acceleration-1);
+
+ // Calculate acceleration results ignoring Constraints, except to have
+ // them calculate the resulting constraint errors.
+ calcTreeForwardDynamicsOperator
+ (s, mobilityForces, particleForces, bodyForces,
+ 0, 0, tac, udot, qdotdot, udotErr);
+
+ // Next, determine how many acceleration-level constraint equations
+ // need to be obeyed.
+
+ const int mHolo = getNumHolonomicConstraintEquationsInUse(s);
+ const int mNonholo = getNumNonholonomicConstraintEquationsInUse(s);
+ const int mAccOnly = getNumAccelerationOnlyConstraintEquationsInUse(s);
+ const int m = mHolo+mNonholo+mAccOnly;
+ const int nq = getNQ(s);
+ const int nu = getNU(s);
+
+ multipliers.resize(m);
+ if (m==0) return;
+ if (nu==0) {multipliers.setToZero(); return;}
+
+ // Conditioning tolerance. This determines when we'll drop a
+ // constraint.
+ // TODO: this is probably too tight; should depend on constraint tolerance
+ // and should be consistent with position and velocity projection ranks.
+ // Tricky here because conditioning depends on mass matrix as well as
+ // constraints.
+ const Real conditioningTol = m
+ //* SignificantReal;
+ * SqrtEps*std::sqrt(SqrtEps); // Eps^(3/4)
+
+ // Calculate multipliers lambda as
+ // (G M^-1 ~G) lambda = aerr
+ // The method here calculates the mXm matrix G*M^-1*G^T as fast as
+ // I know how to do, O(m*n) with O(n) temporary memory, using a series
+ // of O(n) operators. Then we'll factor it here in O(m^3) time.
+ Matrix GMInvGt(m,m);
+ calcGMInvGt(s, GMInvGt);
+
+ // specify 1/cond at which we declare rank deficiency
+ FactorQTZ qtz(GMInvGt, conditioningTol);
+
+ //printf("fwdDynamics: m=%d condTol=%g rank=%d rcond=%g\n",
+ // GMInvGt.nrow(), conditioningTol, qtz.getRank(),
+ // qtz.getRCondEstimate());
+
+ qtz.solve(udotErr, multipliers);
+
+ // We have the multipliers, now turn them into forces.
+
+ Vector_<SpatialVec> bodyForcesInG;
+ Vector mobilityF;
+ calcConstraintForcesFromMultipliers(s,multipliers,bodyForcesInG,mobilityF,
+ cac.constrainedBodyForcesInG, cac.constraintMobilityForces);
+ // Note that constraint forces have the opposite sign from applied forces
+ // so must be subtracted to calculate the total forces.
+
+ // Recalculate the accelerations applying the constraint forces in addition
+ // to the applied forces that were passed in. The constraint errors
+ // calculated now should be within numerical noise of zero.
+ calcTreeForwardDynamicsOperator
+ (s, mobilityForces, particleForces, bodyForces,
+ &mobilityF, &bodyForcesInG, tac, udot, qdotdot, udotErr);
+}
+//................... CALC LOOP FORWARD DYNAMICS OPERATOR ......................
+
+
+
+//==============================================================================
+// REALIZE LOOP FORWARD DYNAMICS
+//==============================================================================
+// Given the set of forces in the state, calculate accelerations resulting from
+// those forces and enforcement of acceleration constraints.
+void SimbodyMatterSubsystemRep::realizeLoopForwardDynamics(const State& s,
+ const Vector& mobilityForces,
+ const Vector_<Vec3>& particleForces,
+ const Vector_<SpatialVec>& bodyForces) const
+{
+ const SBModelCache& mc = getModelCache(s);
+
+ // Because we are realizing, we want to direct the output of the operator
+ // back into the State cache.
+ SBTreeAccelerationCache& tac = updTreeAccelerationCache(s);
+ SBConstrainedAccelerationCache& cac = updConstrainedAccelerationCache(s);
+ Vector& udot = updUDot(s);
+ Vector& qdotdot = updQDotDot(s);
+ Vector& udotErr = updUDotErr(s);
+ Vector& multipliers = updMultipliers(s);
+
+ calcLoopForwardDynamicsOperator
+ (s, mobilityForces, particleForces, bodyForces,
+ tac, cac, udot, qdotdot, multipliers, udotErr);
+
+ // Since we're realizing, note that we're done with these cache entries.
+ markCacheValueRealized(s, mc.treeAccelerationCacheIndex);
+ markCacheValueRealized(s, mc.constrainedAccelerationCacheIndex);
+}
+//....................... REALIZE LOOP FORWARD DYNAMICS ........................
+
+
+
+// =============================================================================
+// CALC COMPOSITE BODY INERTIAS
+// =============================================================================
+// Given a State realized to Position stage, calculate the composite
+// body inertias seen by each mobilizer. A composite body inertia is
+// the inertia of the rigid body created by locking all joints outboard
+// of a particular mobilized body. (Constraints have no effect on the result.)
+//
+void SimbodyMatterSubsystemRep::calcCompositeBodyInertias(const State& s,
+ Array_<SpatialInertia,MobilizedBodyIndex>& R) const
+{
+ const SBTreePositionCache& tpc = getTreePositionCache(s);
+ R.resize(getNumBodies());
+
+ for (int i=rbNodeLevels.size()-1 ; i>=0 ; i--)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++)
+ rbNodeLevels[i][j]->calcCompositeBodyInertiasInward(tpc,R);
+}
+//....................... CALC COMPOSITE BODY INERTIAS .........................
+
+
+
+// =============================================================================
+// REALIZE Y
+// =============================================================================
+// Y is used for length constraints: sweep from base to tip. You can call this
+// after Position stage but it may have to realize articulated bodies first.
+void SimbodyMatterSubsystemRep::realizeY(const State& s) const {
+ realizeArticulatedBodyInertias(s);
+
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const SBTreePositionCache& tpc = getTreePositionCache(s);
+ const SBArticulatedBodyInertiaCache& abc = getArticulatedBodyInertiaCache(s);
+ SBDynamicsCache& dc = updDynamicsCache(s);
+
+ for (int i=0; i < (int)rbNodeLevels.size(); i++)
+ for (int j=0; j < (int)rbNodeLevels[i].size(); j++)
+ rbNodeLevels[i][j]->realizeYOutward(ic,tpc,abc,dc);
+}
+//.................................. REALIZE Y .................................
+
+
+
+//==============================================================================
+// CALC KINETIC ENERGY
+//==============================================================================
+Real SimbodyMatterSubsystemRep::calcKineticEnergy(const State& s) const {
+ const SBTreePositionCache& tpc = getTreePositionCache(s);
+ const SBTreeVelocityCache& tvc = getTreeVelocityCache(s);
+
+ Real ke = 0;
+
+ // Skip ground level 0!
+ for (int i=1 ; i<(int)rbNodeLevels.size() ; i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++)
+ ke += rbNodeLevels[i][j]->calcKineticEnergy(tpc,tvc);
+
+ return ke;
+}
+
+
+
+//==============================================================================
+// CALC TREE ACCELERATIONS
+//==============================================================================
+// Operator for open-loop forward dynamics.
+// This Subsystem must have already been realized to Dynamics stage so that
+// coriolis terms are available, and articulated body inertias should have been
+// realized. All vectors must use contiguous storage.
+void SimbodyMatterSubsystemRep::calcTreeAccelerations(const State& s,
+ const Vector& mobilityForces,
+ const Vector_<SpatialVec>& bodyForces,
+ const Array_<Real>& presUDots, // packed
+ Vector& netHingeForces,
+ Array_<SpatialVec,MobilizedBodyIndex>& allZ,
+ Array_<SpatialVec,MobilizedBodyIndex>& allZPlus,
+ Vector_<SpatialVec>& A_GB,
+ Vector& udot, // in/out (in for prescribed udots)
+ Vector& qdotdot,
+ Vector& tau) const
+{
+ SBStateDigest sbs(s, *this, Stage::Acceleration);
+ const SBArticulatedBodyInertiaCache& abc = getArticulatedBodyInertiaCache(s);
+
+ const SBInstanceCache& ic = sbs.getInstanceCache();
+ const SBTreePositionCache& tpc = sbs.getTreePositionCache();
+ const SBTreeVelocityCache& tvc = sbs.getTreeVelocityCache();
+ const SBDynamicsCache& dc = sbs.getDynamicsCache();
+
+ assert(mobilityForces.size() == getTotalDOF());
+ assert(bodyForces.size() == getNumBodies());
+
+ netHingeForces.resize(getTotalDOF());
+ allZ.resize(getNumBodies());
+ allZPlus.resize(getNumBodies());
+ A_GB.resize(getNumBodies());
+ udot.resize(getTotalDOF());
+ qdotdot.resize(getTotalQAlloc());
+ tau.resize(ic.getTotalNumPresForces());
+
+ assert(mobilityForces.hasContiguousData());
+ assert(bodyForces.hasContiguousData());
+ assert(netHingeForces.hasContiguousData());
+ assert(A_GB.hasContiguousData());
+ assert(udot.hasContiguousData());
+ assert(qdotdot.hasContiguousData());
+ assert(tau.hasContiguousData());
+
+ const Real* mobilityForcePtr = mobilityForces.size()
+ ? &mobilityForces[0] : NULL;
+ const SpatialVec* bodyForcePtr = bodyForces.size()
+ ? &bodyForces[0] : NULL;
+ Real* hingeForcePtr = netHingeForces.size()
+ ? &netHingeForces[0] : NULL;
+ SpatialVec* aPtr = A_GB.size() ? &A_GB[0] : NULL;
+ Real* udotPtr = udot.size() ? &udot[0] : NULL;
+ Real* qdotdotPtr = qdotdot.size() ? &qdotdot[0] : NULL;
+ Real* tauPtr = tau.size() ? &tau[0] : NULL;
+ SpatialVec* zPtr = allZ.begin();
+ SpatialVec* zPlusPtr = allZPlus.begin();
+
+ // If there are any prescribed udots, scatter them into the appropriate
+ // udot entries now. We must also set known-zero udots to zero here.
+ assert(presUDots.size() == ic.getTotalNumPresUDot());
+ for (PresUDotPoolIndex i(0); i < presUDots.size(); ++i)
+ udotPtr[ic.presUDot[i]] = presUDots[i];
+ for (int i=0; i < (int)ic.zeroUDot.size(); ++i)
+ udotPtr[ic.zeroUDot[i]] = 0;
+
+ for (int i=rbNodeLevels.size()-1 ; i>=0 ; i--)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++) {
+ const RigidBodyNode& node = *rbNodeLevels[i][j];
+ node.calcUDotPass1Inward(ic,tpc,abc,dc,
+ mobilityForcePtr, bodyForcePtr, udotPtr, zPtr, zPlusPtr,
+ hingeForcePtr);
+ }
+
+ for (int i=0 ; i<(int)rbNodeLevels.size() ; i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++) {
+ const RigidBodyNode& node = *rbNodeLevels[i][j];
+ node.calcUDotPass2Outward(ic,tpc,abc,tvc,dc,
+ hingeForcePtr, aPtr, udotPtr, tauPtr);
+ node.calcQDotDot(sbs, &udotPtr[node.getUIndex()],
+ &qdotdotPtr[node.getQIndex()]);
+ }
+}
+//......................... CALC TREE ACCELERATIONS ............................
+
+
+
+//==============================================================================
+// MULTIPLY BY M INV
+//==============================================================================
+// Calculate udot = M^-1 f. We also get spatial accelerations A_GB for
+// each body as a side effect.
+// This Subsystem must already be realized through Position stage; we'll
+// realize articulated body inertias if necessary.
+// All vectors must use contiguous storage.
+void SimbodyMatterSubsystemRep::multiplyByMInv(const State& s,
+ const Vector& f,
+ Vector& MInvf) const
+{
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const SBTreePositionCache& tpc = getTreePositionCache(s);
+
+ realizeArticulatedBodyInertias(s); // (may already have been realized)
+ const SBArticulatedBodyInertiaCache& abc = getArticulatedBodyInertiaCache(s);
+
+ const int nb = getNumBodies();
+ const int nu = getNU(s);
+
+ assert(f.size() == nu);
+
+ MInvf.resize(nu);
+ if (nu==0)
+ return;
+
+ assert(f.hasContiguousData());
+ assert(MInvf.hasContiguousData());
+
+ // Temporaries
+ Array_<Real> eps(nu);
+ Array_<SpatialVec> z(nb), zPlus(nb), A_GB(nb);
+
+ // Point to raw data of input arguments.
+ const Real* fPtr = &f[0];
+ Real* MInvfPtr = &MInvf[0];
+
+ for (int i=rbNodeLevels.size()-1 ; i>=0 ; i--)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++) {
+ const RigidBodyNode& node = *rbNodeLevels[i][j];
+ node.multiplyByMInvPass1Inward(ic,tpc,abc,
+ fPtr, z.begin(), zPlus.begin(), eps.begin());
+ }
+
+ for (int i=0 ; i<(int)rbNodeLevels.size() ; i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++) {
+ const RigidBodyNode& node = *rbNodeLevels[i][j];
+ node.multiplyByMInvPass2Outward(ic,tpc,abc,
+ eps.cbegin(), A_GB.begin(), MInvfPtr);
+ }
+}
+//............................. CALC M INVERSE F ...............................
+
+
+
+//==============================================================================
+// MULTIPLY BY M
+//==============================================================================
+// Calculate f = M a.
+// This Subsystem must already have been realized to Position stage.
+// All vectors must use contiguous storage.
+void SimbodyMatterSubsystemRep::multiplyByM(const State& s,
+ const Vector& a,
+ Vector& Ma) const
+{
+
+ const SBTreePositionCache& tpc = getTreePositionCache(s);
+ const int nb = getNumBodies();
+ const int nu = getNU(s);
+
+ assert(a.size() == nu);
+ Ma.resize(nu);
+
+ if (nu == 0)
+ return;
+
+ assert(a.hasContiguousData());
+ assert(Ma.hasContiguousData());
+
+ // Temporaries
+ Array_<SpatialVec> fTmp(nb), A_GB(nb);
+
+ // Point to raw data of input arguments.
+ const Real* aPtr = &a[0];
+ Real* MaPtr = &Ma[0];
+
+ for (int i=0 ; i<(int)rbNodeLevels.size() ; i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++) {
+ const RigidBodyNode& node = *rbNodeLevels[i][j];
+ node.multiplyByMPass1Outward(tpc, aPtr, A_GB.begin());
+ }
+
+ for (int i=rbNodeLevels.size()-1 ; i>=0 ; i--)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++) {
+ const RigidBodyNode& node = *rbNodeLevels[i][j];
+ node.multiplyByMPass2Inward(tpc,A_GB.cbegin(),fTmp.begin(),MaPtr);
+ }
+}
+
+
+
+//==============================================================================
+// CALC M
+//==============================================================================
+// Calculate the mass matrix M in O(n^2) time. This Subsystem must already have
+// been realized to Position stage.
+// It is OK if M's data is not contiguous.
+void SimbodyMatterSubsystemRep::calcM(const State& s, Matrix& M) const {
+ const int nu = getTotalDOF();
+ M.resize(nu,nu);
+ if (nu==0) return;
+
+ // This could be calculated much faster by doing it directly and calculating
+ // only half of it. As a placeholder, however, we're doing this with
+ // repeated O(n) calls to multiplyByM() to get M one column at a time.
+
+ // If M's columns are contiguous we can avoid copying.
+ const bool isContiguous = M(0).hasContiguousData();
+ Vector contig_col(isContiguous ? 0 : nu);
+
+ Vector v(nu); v.setToZero();
+ for (int i=0; i < nu; ++i) {
+ v[i] = 1;
+ if (isContiguous) {
+ multiplyByM(s, v, M(i));
+ } else {
+ multiplyByM(s, v, contig_col);
+ M(i) = contig_col;
+ }
+ v[i] = 0;
+ }
+}
+
+
+
+//==============================================================================
+// CALC MInv
+//==============================================================================
+// Calculate the mass matrix inverse MInv(=M^-1) in O(n^2) time. This Subsystem
+// must already have been realized to Position stage.
+// It is OK if MInv's data is not contiguous.
+void SimbodyMatterSubsystemRep::calcMInv(const State& s, Matrix& MInv) const {
+ const int nu = getTotalDOF();
+ MInv.resize(nu,nu);
+ if (nu==0) return;
+
+ // This could probably be calculated faster by doing it directly and
+ // filling in only half. For now we're doing it with repeated calls to
+ // the O(n) operator multiplyByMInv().
+
+ // If M's columns are contiguous we can avoid copying.
+ const bool isContiguous = MInv(0).hasContiguousData();
+ Vector contig_col(isContiguous ? 0 : nu);
+
+ Vector f(nu); f.setToZero();
+ for (int i=0; i < nu; ++i) {
+ f[i] = 1;
+ if (isContiguous) {
+ multiplyByMInv(s, f, MInv(i));
+ } else {
+ multiplyByMInv(s, f, contig_col);
+ MInv(i) = contig_col;
+ }
+ f[i] = 0;
+ }
+}
+
+
+
+//==============================================================================
+// CALC TREE RESIDUAL FORCES
+//==============================================================================
+// Operator for tree system inverse dynamics.
+// Note that this includes the effects of inertial forces.
+// This calculates
+// f_resid = M(q) udot + f_inertial(q,u) - f_applied
+// given udot and f_applied as arguments, with the rest from the state.
+// No constraint forces are included unless the caller has included them in
+// f_applied.
+//
+// This Subsystem must already have been realized to Velocity stage in the
+// supplied state. All vectors must use contiguous storage.
+void SimbodyMatterSubsystemRep::calcTreeResidualForces(const State& s,
+ const Vector& appliedMobilityForces,
+ const Vector_<SpatialVec>& appliedBodyForces,
+ const Vector& knownUdot,
+ Vector_<SpatialVec>& A_GB,
+ Vector& residualMobilityForces) const
+{
+ const SBTreePositionCache& tpc = getTreePositionCache(s);
+ const SBTreeVelocityCache& tvc = getTreeVelocityCache(s);
+
+ // We allow the input Vectors to be zero length, meaning they are to be
+ // considered as though they were full length but all zero. For now we have
+ // to make explicit Vectors of zero for them in that case; better would
+ // be to have a special operator.
+ const Vector* pAppliedMobForces = &appliedMobilityForces;
+ const Vector_<SpatialVec>* pAppliedBodyForces = &appliedBodyForces;
+ const Vector* pKnownUdot = &knownUdot;
+
+ Vector zeroPerMobility;
+ Vector_<SpatialVec> zeroPerBody;
+ if (appliedMobilityForces.size()==0 || knownUdot.size()==0) {
+ zeroPerMobility.resize(getNumMobilities());
+ zeroPerMobility = 0;
+ if (appliedMobilityForces.size()==0)
+ pAppliedMobForces = &zeroPerMobility;
+ if (knownUdot.size()==0)
+ pKnownUdot = &zeroPerMobility;
+ }
+ if (appliedBodyForces.size()==0) {
+ zeroPerBody.resize(getNumBodies());
+ zeroPerBody = SpatialVec(Vec3(0),Vec3(0));
+ pAppliedBodyForces = &zeroPerBody;
+ }
+
+ // At this point the three pointers point either to the original arguments
+ // or to appropriate-sized arrays of zero. Any non-zero length original
+ // arguments should already have been verified by the caller (a method
+ // in the SimTK API) to be the correct length.
+
+ // Check input sizes.
+ assert(pAppliedMobForces->size() == getNumMobilities());
+ assert(pAppliedBodyForces->size() == getNumBodies());
+ assert(pKnownUdot->size() == getNumMobilities());
+
+ // Resize outputs if necessary.
+ A_GB.resize(getNumBodies());
+ residualMobilityForces.resize(getNumMobilities());
+
+ assert(pAppliedMobForces->hasContiguousData());
+ assert(pAppliedBodyForces->hasContiguousData());
+ assert(pKnownUdot->hasContiguousData());
+ assert(A_GB.hasContiguousData());
+ assert(residualMobilityForces.hasContiguousData());
+
+
+ // Allocate temporary.
+ Vector_<SpatialVec> allFTmp(getNumBodies());
+
+ // Make pointers to (contiguous) Vector data for fast access.
+ const Real* knownUdotPtr = &(*pKnownUdot)[0];
+ SpatialVec* aPtr = A_GB.size() ? &A_GB[0] : NULL;
+ const Real* mobilityForcePtr = pAppliedMobForces->size()
+ ? &(*pAppliedMobForces)[0] : NULL;
+ const SpatialVec* bodyForcePtr = pAppliedBodyForces->size()
+ ? &(*pAppliedBodyForces)[0] : NULL;
+ Real *residualPtr = residualMobilityForces.size()
+ ? &residualMobilityForces[0] : NULL;
+ SpatialVec* tempPtr = allFTmp.size() ? &allFTmp[0] : NULL;
+
+ for (int i=0 ; i<(int)rbNodeLevels.size() ; i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++) {
+ const RigidBodyNode& node = *rbNodeLevels[i][j];
+ node.calcBodyAccelerationsFromUdotOutward
+ (tpc,tvc,knownUdotPtr,aPtr);
+ }
+
+ for (int i=rbNodeLevels.size()-1 ; i>=0 ; i--)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++) {
+ const RigidBodyNode& node = *rbNodeLevels[i][j];
+ node.calcInverseDynamicsPass2Inward(
+ tpc,tvc,aPtr,
+ mobilityForcePtr,bodyForcePtr,
+ tempPtr,residualPtr);
+ }
+}
+//........................ CALC TREE RESIDUAL FORCES ...........................
+
+
+
+//==============================================================================
+// MULTIPLY BY N
+//==============================================================================
+// q=Nu or u=~Nq
+// Both vectors must use contiguous storage.
+void SimbodyMatterSubsystemRep::multiplyByN
+ (const State& s, bool transpose, const Vector& in, Vector& out) const
+{
+ // i.e., we must be *done* with Stage::Position
+ const SBStateDigest sbState(s, *this, Stage(Stage::Position).next());
+
+ assert(in.size() == (transpose?getTotalQAlloc():getTotalDOF()));
+ out.resize(transpose?getTotalDOF():getTotalQAlloc());
+
+ assert(in.hasContiguousData());
+ assert(out.hasContiguousData());
+
+ const Real* inp = in.size() ? &in[0] : 0;
+ Real* outp = out.size() ? &out[0] : 0;
+
+ // Skip ground; it doesn't have qdots!
+ for (int i=1; i<(int)rbNodeLevels.size(); i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++) {
+ const RigidBodyNode& rbn = *rbNodeLevels[i][j];
+ const int maxNQ = rbn.getMaxNQ();
+
+ // Skip weld joints: no q's, no work to do here.
+ if (maxNQ == 0)
+ continue;
+
+ // Find the right piece of the vectors to work with.
+ const int qx = rbn.getQIndex();
+ const int ux = rbn.getUIndex();
+ const int inpx = transpose ? qx : ux;
+ const int outpx = transpose ? ux : qx;
+
+ // TODO: kludge: for now q-like output may have an unused element
+ // because we always allocate the max space. Set the last element
+ // to zero in case it doesn't get written.
+ if (!transpose) outp[outpx + maxNQ-1] = 0;
+
+ rbn.multiplyByN(sbState, transpose, &inp[inpx], &outp[outpx]);
+ }
+}
+
+
+
+//==============================================================================
+// MULTIPLY BY NDOT
+//==============================================================================
+// q=NDot*u or u=~NDot*q
+// Both vectors must use contiguous storage.
+void SimbodyMatterSubsystemRep::multiplyByNDot
+ (const State& s, bool transpose, const Vector& in, Vector& out) const
+{
+ // i.e., we must be *done* with Stage::Velocity
+ const SBStateDigest sbState(s, *this, Stage(Stage::Velocity).next());
+
+ assert(in.size() == (transpose?getTotalQAlloc():getTotalDOF()));
+ out.resize(transpose?getTotalDOF():getTotalQAlloc());
+
+ assert(in.hasContiguousData());
+ assert(out.hasContiguousData());
+
+ const Real* inp = in.size() ? &in[0] : 0;
+ Real* outp = out.size() ? &out[0] : 0;
+
+ // Skip ground; it doesn't have q's or u's!
+ for (int i=1; i<(int)rbNodeLevels.size(); i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++) {
+ const RigidBodyNode& rbn = *rbNodeLevels[i][j];
+ const int maxNQ = rbn.getMaxNQ();
+
+ // Skip weld joints: no q's, no work to do here.
+ if (maxNQ == 0)
+ continue;
+
+ // Find the right piece of the vectors to work with.
+ const int qx = rbn.getQIndex();
+ const int ux = rbn.getUIndex();
+ const int inpx = transpose ? qx : ux;
+ const int outpx = transpose ? ux : qx;
+
+ // TODO: kludge: for now q-like output may have an unused element
+ // because we always allocate the max space. Set the last element
+ // to zero in case it doesn't get written.
+ if (!transpose) outp[outpx + maxNQ-1] = 0;
+
+ rbn.multiplyByNDot(sbState, transpose, &inp[inpx], &outp[outpx]);
+ }
+}
+
+
+
+//==============================================================================
+// MULTIPLY BY NINV
+//==============================================================================
+// u= NInv * q or q = ~NInv * u
+// Both vectors must use contiguous storage.
+void SimbodyMatterSubsystemRep::multiplyByNInv
+ (const State& s, bool transpose, const Vector& in, Vector& out) const
+{
+ // i.e., we must be *done* with Stage::Position
+ const SBStateDigest sbState(s, *this, Stage(Stage::Position).next());
+
+ assert(in.size() == (transpose?getTotalDOF():getTotalQAlloc()));
+ out.resize(transpose?getTotalQAlloc():getTotalDOF());
+
+ assert(in.hasContiguousData());
+ assert(out.hasContiguousData());
+
+ const Real* inp = in.size() ? &in[0] : 0;
+ Real* outp = out.size() ? &out[0] : 0;
+
+ // Skip ground; it doesn't have qdots!
+ for (int i=1; i<(int)rbNodeLevels.size(); i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++) {
+ const RigidBodyNode& rbn = *rbNodeLevels[i][j];
+ const int maxNQ = rbn.getMaxNQ();
+
+ // Skip weld joints: no q's, no work to do here.
+ if (maxNQ == 0)
+ continue;
+
+ // Find the right piece of the vectors to work with.
+ const int qx = rbn.getQIndex();
+ const int ux = rbn.getUIndex();
+ const int inpx = transpose ? ux : qx;
+ const int outpx = transpose ? qx : ux;
+
+ // TODO: kludge: for now q-like output may have an unused element
+ // because we always allocate the max space. Set the last element
+ // to zero in case it doesn't get written.
+ if (transpose) outp[outpx + maxNQ-1] = 0;
+
+ rbn.multiplyByNInv(sbState, transpose, &inp[inpx], &outp[outpx]);
+ }
+}
+
+
+
+// =============================================================================
+// CALC MOBILIZER REACTION FORCES
+// =============================================================================
+// This method calculates mobilizer reaction forces using repeated application
+// of the equation
+// F_reaction = PPlus*APlus + zPlus
+// where P is an articulated body inertia, A is the spatial acceleration of
+// that body, and z is the articulated body residual force. The "Plus"
+// indicates that the quantity is as seen on the *inboard* side of the
+// mobilizer, although still measured about Bo and expressed in G.
+// All of these quantities are already available at Stage::Acceleration except
+// APlus= ~phi * A_GP, the parent body's acceleration shifted to the child.
+//
+// See Abhi Jain's 2011 book "Robot and Multibody Dynamics", Eq. 7.34 on
+// page 128: reaction = P(A-a)+z = PPlus*APlus + zPlus. The first equation
+// is not correct if there is prescribed motion (you'd have to remove H*udot
+// also), but the "Plus" version works regardless.
+//
+// After calculating the reaction at the body frame origin Bo, we shift it to
+// the mobilizer's outboard frame M and report it there, though expressed in G.
+// Note that any generalized forces applied at mobilities end up included in
+// the reaction forces.
+//
+// Cost is 114 flops/body plus lots of memory access to dredge up the
+// already-calculated goodies. If you don't need all the reactions, you can
+// calculate them one at a time as needed just as efficiently.
+void SimbodyMatterSubsystemRep::calcMobilizerReactionForces
+ (const State& s, Vector_<SpatialVec>& FM_G) const
+{
+ const int nb = getNumBodies();
+ // We're going to work with forces in Ground, applied at the body frame
+ // of each body. Then at the end we'll shift to the M frame as promised
+ // (though still expressed in Ground).
+ FM_G.resize(nb);
+
+ const Array_<ArticulatedInertia,MobilizedBodyIndex>& PPlus =
+ getArticulatedBodyInertiasPlus(s);
+ const Array_<SpatialVec,MobilizedBodyIndex>& zPlus =
+ getArticulatedBodyForcesPlus(s);
+
+ for (MobodIndex mbx(0); mbx < nb; ++mbx) {
+ const MobilizedBody& body = getMobilizedBody(mbx);
+ const Transform& X_GB = body.getBodyTransform(s);
+
+ SpatialVec FB_G = zPlus[mbx];
+ if (mbx != GroundIndex) {
+ const MobilizedBody& parent = body.getParentMobilizedBody();
+ const Transform& X_GP = parent.getBodyTransform(s);
+ const SpatialVec& A_GP = parent.getBodyAcceleration(s);
+ const Vec3& p_PB_G = X_GB.p() - X_GP.p(); // 3 flops
+ SpatialVec APlus( A_GP[0],
+ A_GP[1] + A_GP[0] % p_PB_G ); // 12 flops
+ FB_G += PPlus[mbx]*APlus; // 72 flops
+ }
+ // Shift to M
+ const Vec3& p_BM = body.getOutboardFrame(s).p();
+ const Vec3 p_BM_G = X_GB.R()*p_BM; // p_BM in G, 15 flops
+ FM_G[mbx] = shiftForceBy(FB_G, p_BM_G); // 12 flops
+ }
+}
+//....................... CALC MOBILIZER REACTION FORCES .......................
+
+
+
+// =============================================================================
+// CALC MOBILIZER REACTION FORCES USING FREEBODY METHOD
+// =============================================================================
+// This method provides an alternative way to calculate mobilizer reaction
+// forces. It is about 3X slower than calcMobilizerReactionForces() so should
+// not be used except for Simbody debugging and regression testing purposes.
+void SimbodyMatterSubsystemRep::calcMobilizerReactionForcesUsingFreebodyMethod
+ (const State& s, Vector_<SpatialVec>& FM_G) const
+{
+ const int nb = getNumBodies();
+ // We're going to work with forces in Ground, applied at the body frame
+ // of each body. Then at the end we'll shift to the M frame as promised
+ // (though still expressed in Ground).
+ FM_G.resize(nb);
+
+ // Find the body forces on every body from all sources *other* than
+ // mobilizer reaction forces; we accumulate them in otherForces_G.
+
+ // First, get the applied body forces (at Bo).
+ Vector_<SpatialVec> otherFB_G =
+ getMultibodySystem().getRigidBodyForces(s, Stage::Dynamics);
+
+ // Plus body forces applied by constraints (watch the sign).
+ Vector_<SpatialVec> constrainedBodyForces_G(getNumBodies());
+ Vector constrainedMobilizerForces(s.getNU());
+ calcConstraintForcesFromMultipliers(s, s.getMultipliers(),
+ constrainedBodyForces_G, constrainedMobilizerForces);
+ otherFB_G -= constrainedBodyForces_G;
+
+ // We'll account below for gyroscopic forces due to angular velocity.
+
+ // Starting from the leaf nodes and working back toward ground, take the
+ // difference between the total force and the other forces we can account
+ // for to find the reaction force, then apply that to the parent as a
+ // known force.
+ for (int i = (int)rbNodeLevels.size()-1; i >= 0; --i)
+ for (int j = 0; j < (int)rbNodeLevels[i].size(); ++j) {
+ const MobilizedBodyIndex mbx = rbNodeLevels[i][j]->getNodeNum();
+ const MobilizedBody& body = getMobilizedBody(mbx);
+
+ SpatialVec totalFB_G(Vec3(0)); // Force from freebody f=ma
+ if (mbx != GroundIndex) {
+ const SpatialVec& A_GB = body.getBodyAcceleration(s);
+ const SpatialInertia& MB_G = body.getBodySpatialInertiaInGround(s);
+ totalFB_G = MB_G * A_GB; // f = ma (45 flops)
+ }
+
+ // Body B's reaction force, but applied at Bo
+ const SpatialVec FB_G = totalFB_G
+ - (otherFB_G[mbx]-getGyroscopicForce(s, mbx));
+ // Shift to M
+ const Transform& X_GB = body.getBodyTransform(s);
+ const Vec3& p_BM = body.getOutboardFrame(s).p();
+ const Vec3 p_BM_G = X_GB.R()*p_BM; // p_BM, but expressed in G
+ FM_G[mbx] = shiftForceBy(FB_G, p_BM_G);
+ if (i==0) continue; // no parent
+
+ // Now apply reaction to parent as an "otherForce". Apply equal and
+ // opposite force & torque to parent at Po, by shifting the forces
+ // at Bo.
+ const MobilizedBody& parent = body.getParentMobilizedBody();
+ const MobilizedBodyIndex px = parent.getMobilizedBodyIndex();
+ const Transform& X_GP = parent.getBodyTransform(s);
+ const Vec3 p_BP_G = X_GP.p() - X_GB.p();
+ otherFB_G[px] -= shiftForceBy(FB_G, p_BP_G);
+ }
+}
+//....................... CALC MOBILIZER REACTION FORCES .......................
+
+
+
+//==============================================================================
+// CALC MOTION ERRORS
+//==============================================================================
+// Return one error per known value, in order of mobilized bodies.
+Vector SimbodyMatterSubsystemRep::
+calcMotionErrors(const State& s, const Stage& stage) const
+{
+ const SBInstanceCache& ic = getInstanceCache(s);
+ Vector errs;
+
+ if (stage == Stage::Position) {
+ const SBTimeCache& tc = getTimeCache(s);
+ assert(tc.presQPool.size() == ic.getTotalNumPresQ());
+ errs.resize(ic.getTotalNumPresQ()+ic.getTotalNumZeroQ());
+ int nxt = 0;
+ for (MobodIndex mbx(1); mbx < getNumBodies(); ++mbx) {
+ const Mobod& mobod = getMobilizedBody(mbx);
+ const int nq = mobod.getNumQ(s);
+ const MobilizedBodyImpl& mbimpl = mobod.getImpl();
+ const SBInstancePerMobodInfo& mbinfo = mbimpl.getMyInstanceInfo(s);
+ if (mbinfo.qMethod == Motion::Zero) {
+ errs(nxt, nq) = mobod.getQAsVector(s); //TODO not right for quats
+ nxt += nq;
+ } else if (mbinfo.qMethod == Motion::Prescribed) {
+ Vector pres(nq, &tc.presQPool[mbinfo.firstPresQ], true);
+ errs(nxt, nq) = mobod.getQAsVector(s) - pres;
+ nxt += nq;
+ }
+ }
+ return errs;
+ }
+
+ if (stage == Stage::Velocity) {
+ const SBConstrainedPositionCache& cpc = getConstrainedPositionCache(s);
+ assert(cpc.presUPool.size() == ic.getTotalNumPresU());
+ errs.resize(ic.getTotalNumPresU()+ic.getTotalNumZeroU());
+ int nxt = 0;
+ for (MobodIndex mbx(1); mbx < getNumBodies(); ++mbx) {
+ const Mobod& mobod = getMobilizedBody(mbx);
+ const int nu = mobod.getNumU(s);
+ const MobilizedBodyImpl& mbimpl = mobod.getImpl();
+ const SBInstancePerMobodInfo& mbinfo = mbimpl.getMyInstanceInfo(s);
+ if (mbinfo.uMethod == Motion::Zero) {
+ errs(nxt, nu) = mobod.getUAsVector(s);
+ nxt += nu;
+ } else if (mbinfo.uMethod == Motion::Prescribed) {
+ Vector pres(nu, &cpc.presUPool[mbinfo.firstPresU], true);
+ errs(nxt, nu) = mobod.getUAsVector(s) - pres;
+ nxt += nu;
+ }
+ }
+ return errs;
+ }
+
+ if (stage == Stage::Acceleration) {
+ const SBDynamicsCache& dc = getDynamicsCache(s);
+ assert(dc.presUDotPool.size() == ic.getTotalNumPresUDot());
+ errs.resize(ic.getTotalNumPresUDot()+ic.getTotalNumZeroUDot());
+ int nxt = 0;
+ for (MobodIndex mbx(1); mbx < getNumBodies(); ++mbx) {
+ const Mobod& mobod = getMobilizedBody(mbx);
+ const int nu = mobod.getNumU(s);
+ const MobilizedBodyImpl& mbimpl = mobod.getImpl();
+ const SBInstancePerMobodInfo& mbinfo = mbimpl.getMyInstanceInfo(s);
+ if (mbinfo.udotMethod == Motion::Zero) {
+ errs(nxt, nu) = mobod.getUDotAsVector(s);
+ nxt += nu;
+ } else if (mbinfo.udotMethod == Motion::Prescribed) {
+ Vector pres(nu, &dc.presUDotPool[mbinfo.firstPresUDot], true);
+ errs(nxt, nu) = mobod.getUDotAsVector(s) - pres;
+ nxt += nu;
+ }
+ }
+ return errs;
+ }
+
+ SimTK_APIARGCHECK1_ALWAYS
+ (Stage::Position <= stage && stage <= Stage::Acceleration,
+ "SimbodyMatterSubsystem", "calcMotionErrors()",
+ "Requested stage must be Position, Velocity, or Acceleration but"
+ " was %s.", stage.getName().c_str());
+
+ /*NOTREACHED*/
+ return errs;
+}
+
+
+
+//==============================================================================
+// FIND MOTION FORCES, CALC MOTION POWER
+//==============================================================================
+void SimbodyMatterSubsystemRep::
+findMotionForces(const State& s,
+ Vector& mobilityForces) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const int nu = getNU(s);
+ const int nu_p = ic.getTotalNumPresForces(); // num prescribed udots
+
+ mobilityForces.resize(nu);
+ mobilityForces.setToZero(); // assume nothing is prescribed
+ if (nu_p == 0)
+ return; // nothing more to do
+
+ const Vector& tau = getMotionMultipliers(s); // nu_p of these, contiguous
+ assert(tau.size() == nu_p);
+
+ const Real* taup = &tau[0];
+ for (PresForcePoolIndex i(0); i < nu_p; ++i)
+ mobilityForces[ic.presForce[i]] = taup[i];
+}
+
+Real SimbodyMatterSubsystemRep::
+calcMotionPower(const State& s) const {
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const int nu_p = ic.getTotalNumPresForces(); // num prescribed udots
+ if (nu_p == 0)
+ return 0;
+
+ const Vector& tau = getMotionMultipliers(s); // nu_p of these, contiguous
+ assert(tau.size() == nu_p);
+ assert(tau.hasContiguousData());
+
+ const Vector& u = getU(s);
+ assert(u.hasContiguousData());
+
+ const Real* taup = &tau[0];
+ const Real* up = &u[0];
+ Real power = 0;
+
+ // Note that we're negating tau to get +power to mean adding energy
+ // and -power to mean removing energy. That's required because tau's
+ // appear on the LHS of the equations of motion so have the opposite
+ // sign from applied forces.
+ for (PresForcePoolIndex i(0); i < nu_p; ++i)
+ power -= taup[i] * up[ic.presForce[i]];
+
+ return power;
+}
+//................... FIND MOTION FORCES, CALC MOTION POWER ....................
+
+
+
+//==============================================================================
+// FIND CONSTRAINT FORCES, CALC CONSTRAINT POWER
+//==============================================================================
+void SimbodyMatterSubsystemRep::
+findConstraintForces(const State& s,
+ Vector_<SpatialVec>& bodyForcesInG,
+ Vector& mobilityForces) const
+{
+ const SBInstanceCache& ic = getInstanceCache(s);
+ const int nb = getNumBodies();
+ const int nu = getNU(s);
+
+ // Set all forces to zero here; we'll only update the ones that are
+ // affected by some active constraint.
+ bodyForcesInG.resize(getNumBodies()); bodyForcesInG.setToZero();
+ mobilityForces.resize(getNU(s)); mobilityForces.setToZero();
+
+ // Loop over all enabled constraints, get their forces, and
+ // accumulate the results in the global problem return vectors.
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx) {
+ if (isConstraintDisabled(s,cx))
+ continue;
+
+ const ConstraintImpl& crep = constraints[cx]->getImpl();
+ const SBInstancePerConstraintInfo&
+ cInfo = ic.getConstraintInstanceInfo(cx);
+
+ // No heap allocation is being done here. These are views directly
+ // into the proper segment of the longer array.
+ const ArrayViewConst_<SpatialVec,ConstrainedBodyIndex> bodyF1_G =
+ crep.getConstrainedBodyForcesInGFromState(s);
+ const ArrayViewConst_<Real,ConstrainedUIndex> mobilityF1 =
+ crep.getConstrainedMobilityForcesFromState(s);
+
+ const int ncb = bodyF1_G.size();
+ const int ncu = mobilityF1.size();
+
+ // Unpack constrained body forces and add them to the proper slots
+ // in the global body forces array. They are already expressed in
+ // the Ground frame.
+ for (ConstrainedBodyIndex cbx(0); cbx < ncb; ++cbx)
+ bodyForcesInG[crep.getMobilizedBodyIndexOfConstrainedBody(cbx)]
+ += bodyF1_G[cbx]; // 6 flops
+
+ // Unpack constrained mobility forces and add them into global array.
+ for (ConstrainedUIndex cux(0); cux < ncu; ++cux)
+ mobilityForces[cInfo.getUIndexFromConstrainedU(cux)]
+ += mobilityF1[cux]; // 1 flop
+ }
+}
+
+Real SimbodyMatterSubsystemRep::
+calcConstraintPower(const State& s) const {
+ Real power = 0;
+
+ // Loop over all enabled constraints, get their forces, and
+ // accumulate the results in the global problem return vectors.
+ for (ConstraintIndex cx(0); cx < constraints.size(); ++cx) {
+ if (isConstraintDisabled(s,cx))
+ continue;
+
+ const Constraint& constraint = getConstraint(cx);
+ power += constraint.calcPower(s); // sign already correct
+ }
+
+ return power;
+}
+//............... FIND CONSTRAINT FORCES, CALC CONSTRAINT POWER ................
+
+
+
+//==============================================================================
+// CALC QDOT
+//==============================================================================
+// Must be done with Position stage to calculate qdot = N*u.
+// Both vectors must use contiguous storage.
+void SimbodyMatterSubsystemRep::calcQDot
+ (const State& s, const Vector& u, Vector& qdot) const
+{
+ SBStateDigest sbs(s, *this, Stage::Velocity);
+
+ assert(u.size() == getTotalDOF());
+ qdot.resize(getTotalQAlloc());
+
+ assert(u.hasContiguousData());
+ assert(qdot.hasContiguousData());
+
+ const Real* uPtr = u.size() ? &u[0] : NULL;
+ Real* qdotPtr = qdot.size() ? &qdot[0] : NULL;
+
+ // Skip ground; it doesn't have qdots!
+ for (int i=1; i<(int)rbNodeLevels.size(); i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++) {
+ const RigidBodyNode& node = *rbNodeLevels[i][j];
+ node.calcQDot(sbs, &uPtr[node.getUIndex()], &qdotPtr[node.getQIndex()]);
+ }
+}
+//............................... CALC QDOT ....................................
+
+
+
+// =============================================================================
+// CALC QDOTDOT
+// =============================================================================
+// Must be done with Velocity stage to calculate qdotdot = Ndot*u + N*udot.
+// Both vectors must use contiguous storage.
+void SimbodyMatterSubsystemRep::calcQDotDot
+ (const State& s, const Vector& udot, Vector& qdotdot) const
+{
+ SBStateDigest sbs(s, *this, Stage::Dynamics);
+
+ assert(udot.size() == getTotalDOF());
+ qdotdot.resize(getTotalQAlloc());
+
+ assert(udot.hasContiguousData());
+ assert(qdotdot.hasContiguousData());
+
+ const Real* udotPtr = udot.size() ? &udot[0] : NULL;
+ Real* qdotdotPtr = qdotdot.size() ? &qdotdot[0] : NULL;
+
+ // Skip ground; it doesn't have qdots!
+ for (int i=1; i<(int)rbNodeLevels.size(); i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++) {
+ const RigidBodyNode& node = *rbNodeLevels[i][j];
+ node.calcQDotDot(sbs, &udotPtr[node.getUIndex()],
+ &qdotdotPtr[node.getQIndex()]);
+ }
+}
+//............................. CALC QDOTDOT ...................................
+
+
+
+
+// State must be in Stage::Position.
+void SimbodyMatterSubsystemRep::
+calcMobilizerQDotFromU(const State& s, MobilizedBodyIndex mb, int nu, const Real* u,
+ int nq, Real* qdot) const
+{
+ const SBStateDigest sbState(s, *this, Stage::Position);
+ const RigidBodyNode& n = getRigidBodyNode(mb);
+
+ assert(nu == n.getDOF());
+ assert(nq == n.getNQInUse(sbState.getModelVars()));
+
+ n.calcQDot(sbState, u, qdot);
+}
+
+// State must be realized to Stage::Velocity, so that we can extract N(q),
+// NDot(q,u), and u from it to calculate qdotdot=N(q)*udot + NDot(q,u)*u for
+// this mobilizer.
+void SimbodyMatterSubsystemRep::
+calcMobilizerQDotDotFromUDot(const State& s, MobilizedBodyIndex mb,
+ int nu, const Real* udot,
+ int nq, Real* qdotdot) const
+{
+ const SBStateDigest sbState(s, *this, Stage::Velocity);
+ const RigidBodyNode& n = getRigidBodyNode(mb);
+
+ assert(nu == n.getDOF());
+ assert(nq == n.getNQInUse(sbState.getModelVars()));
+
+ n.calcQDotDot(sbState, udot, qdotdot);
+}
+
+// State must be realized through Stage::Instance. Neither the State nor its
+// cache are modified by this method, since it is an operator.
+// The number of q's is passed in as a sanity check, to make sure the caller
+// and the called mobilizer agree on the generalized coordinates.
+// Returns X_FM(q).
+Transform SimbodyMatterSubsystemRep::
+calcMobilizerTransformFromQ(const State& s, MobilizedBodyIndex mb,
+ int nq, const Real* q) const {
+ const SBStateDigest sbState(s, *this, Stage::Instance);
+ const RigidBodyNode& n = getRigidBodyNode(mb);
+
+ assert(nq == n.getNQInUse(sbState.getModelVars()));
+
+ return n.calcMobilizerTransformFromQ(sbState, q);
+}
+
+// State must be realized through Stage::Position. Neither the State nor its
+// cache are modified by this method, since it is an operator.
+// The number of u's is passed in as a sanity check, to make sure the caller
+// and the called mobilizer agree on the generalized speeds.
+// Returns V_FM(q,u)=H_FM(q)*u, where the q dependency is extracted from the State via
+// the hinge transition matrix H_FM(q).
+SpatialVec SimbodyMatterSubsystemRep::
+calcMobilizerVelocityFromU(const State& s, MobilizedBodyIndex mb,
+ int nu, const Real* u) const {
+ const SBStateDigest sbState(s, *this, Stage::Position);
+ const RigidBodyNode& n = getRigidBodyNode(mb);
+
+ assert(nu == n.getDOF());
+
+ assert(!"not implemented yet");
+ return SpatialVec(Vec3(0),Vec3(0));
+}
+
+// State must be realized through Stage::Velocity. Neither the State nor its
+// cache are modified by this method, since it is an operator.
+// The number of u's (and udot's) is passed in as a sanity check, to make sure the caller
+// and the called mobilizer agree on the generalized accelerations.
+// Returns A_FM(q,u,udot)=H_FM(q)*udot + HDot_FM(q,u)*u where the q and u dependencies
+// are extracted from the State via H_FM(q), and HDot_FM(q,u).
+SpatialVec SimbodyMatterSubsystemRep::
+calcMobilizerAccelerationFromUDot(const State& s, MobilizedBodyIndex mb,
+ int nu, const Real* udot) const{
+ const SBStateDigest sbState(s, *this, Stage::Velocity);
+ const RigidBodyNode& n = getRigidBodyNode(mb);
+
+ assert(nu == n.getDOF());
+
+ assert(!"not implemented yet");
+ return SpatialVec(Vec3(0),Vec3(0));
+}
+
+// These perform the same computations as above but then transform the results
+// so that they relate the child body's frame B to its parent body's frame P,
+// rather than the M and F frames which are attached to B and P respectively
+// but differ by a constant transform.
+Transform SimbodyMatterSubsystemRep::
+calcParentToChildTransformFromQ(const State& s, MobilizedBodyIndex mb,
+ int nq, const Real* q) const {
+ const Transform& X_PF = getMobilizerFrameOnParent(s,mb);
+ const Transform& X_BM = getMobilizerFrame(s,mb);
+
+ const Transform X_FM = calcMobilizerTransformFromQ(s,mb,nq,q);
+ return X_PF * X_FM * ~X_BM; // X_PB
+}
+
+SpatialVec SimbodyMatterSubsystemRep::
+calcParentToChildVelocityFromU(const State& s, MobilizedBodyIndex mb,
+ int nu, const Real* u) const {
+ assert(!"not implemented yet");
+ return SpatialVec(Vec3(0),Vec3(0));
+}
+SpatialVec SimbodyMatterSubsystemRep::
+calcParentToChildAccelerationFromUDot(const State& s, MobilizedBodyIndex mb,
+ int nu, const Real* udot) const {
+ assert(!"not implemented yet");
+ return SpatialVec(Vec3(0),Vec3(0));
+}
+
+
+
+
+// =============================================================================
+// MULTIPLY BY SYSTEM JACOBIAN
+// =============================================================================
+// We have V_GB = J u where J=~Phi*~H is the kinematic Jacobian (partial
+// velocity matrix) that maps generalized speeds to spatial velocities.
+// This method performs the multiplication J*u in O(n) time (i.e., without
+// actually forming J).
+// The input and output vectors must use contiguous storage.
+void SimbodyMatterSubsystemRep::multiplyBySystemJacobian(const State& s,
+ const Vector& v,
+ Vector_<SpatialVec>& Jv) const
+{
+ Jv.resize(getNumBodies());
+
+ assert(v.size() == getNU(s));
+ assert(v.hasContiguousData() && Jv.hasContiguousData());
+
+ const SBTreePositionCache& tpc = getTreePositionCache(s);
+
+ const Real* vPtr = v.size() ? &v[0] : NULL;
+ SpatialVec* jvPtr = Jv.size() ? &Jv[0] : NULL;
+
+ for (int i=0 ; i<(int)rbNodeLevels.size() ; i++)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++) {
+ const RigidBodyNode& node = *rbNodeLevels[i][j];
+ node.multiplyBySystemJacobian(tpc, vPtr, jvPtr);
+ }
+}
+//......................... MULTIPLY BY SYSTEM JACOBIAN ........................
+
+
+
+// =============================================================================
+// MULTIPLY BY SYSTEM JACOBIAN TRANSPOSE
+// =============================================================================
+// The system Jacobian J (a.k.a. partial velocity matrix) is the kinematic
+// mapping between generalized speeds u and body spatial velocities V. Its
+// transpose ~J maps body spatial forces to generalized forces. This method
+// calculates in O(n) time the product of ~J and a "spatial force-like"
+// vector X.
+// The input and output vectors must use contiguous storage.
+void SimbodyMatterSubsystemRep::multiplyBySystemJacobianTranspose
+ (const State& s,
+ const Vector_<SpatialVec>& X,
+ Vector& JtX) const
+{
+ assert(X.size() == getNumBodies());
+ JtX.resize(getNU(s));
+
+ assert(X.hasContiguousData() && JtX.hasContiguousData());
+
+ const SBTreePositionCache& tpc = getTreePositionCache(s);
+
+ Vector_<SpatialVec> zTemp(getNumBodies()); zTemp.setToZero();
+ const SpatialVec* xPtr = X.size() ? &X[0] : NULL;
+ Real* jtxPtr = JtX.size() ? &JtX[0] : NULL;
+ SpatialVec* zPtr = zTemp.size() ? &zTemp[0] : NULL;
+
+ for (int i=rbNodeLevels.size()-1 ; i>=0 ; i--)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++) {
+ const RigidBodyNode& node = *rbNodeLevels[i][j];
+ node.multiplyBySystemJacobianTranspose(tpc, zPtr, xPtr, jtxPtr);
+ }
+}
+//................... MULTIPLY BY SYSTEM JACOBIAN TRANSPOSE ....................
+
+
+
+// =============================================================================
+// CALC TREE EQUIVALENT MOBILITY FORCES
+// =============================================================================
+// This routine does the same thing as the above but accounts for centrifugal
+// forces induced by velocities. The equivalent joint forces returned include
+// both the applied forces and the centrifugal ones. Constraints are ignored.
+// Both vectors must use contiguous storage.
+// TODO: is this useful for anything?
+void SimbodyMatterSubsystemRep::calcTreeEquivalentMobilityForces(const State& s,
+ const Vector_<SpatialVec>& bodyForces,
+ Vector& mobilityForces) const
+{
+ const SBTreePositionCache& tpc = getTreePositionCache(s);
+ const SBDynamicsCache& dc = getDynamicsCache(s);
+
+ assert(bodyForces.size() == getNumBodies());
+ mobilityForces.resize(getTotalDOF());
+
+ assert(bodyForces.hasContiguousData());
+ assert(mobilityForces.hasContiguousData());
+
+ Vector_<SpatialVec> allZ(getNumBodies());
+ const SpatialVec* bodyForcePtr = bodyForces.size() ? &bodyForces[0] : NULL;
+ Real* mobilityForcePtr = mobilityForces.size() ? &mobilityForces[0] : NULL;
+ SpatialVec* zPtr = allZ.size() ? &allZ[0] : NULL;
+
+ // Don't do ground's level since ground has no inboard joint.
+ for (int i=rbNodeLevels.size()-1 ; i>0 ; i--)
+ for (int j=0 ; j<(int)rbNodeLevels[i].size() ; j++) {
+ const RigidBodyNode& node = *rbNodeLevels[i][j];
+ node.calcEquivalentJointForces(tpc,dc,
+ bodyForcePtr, zPtr,
+ mobilityForcePtr);
+ }
+}
+//.................... CALC TREE EQUIVALENT MOBILITY FORCES ....................
+
+
+
+bool SimbodyMatterSubsystemRep::getShowDefaultGeometry() const {
+ return showDefaultGeometry;
+}
+
+void SimbodyMatterSubsystemRep::setShowDefaultGeometry(bool show) {
+ showDefaultGeometry = show;
+}
+
+std::ostream& operator<<(std::ostream& o, const SimbodyMatterSubsystemRep& tree) {
+ o << "SimbodyMatterSubsystemRep has " << tree.getNumBodies() << " bodies (incl. G) in "
+ << tree.rbNodeLevels.size() << " levels." << std::endl;
+ o << "NodeNum->level,offset;stored nodeNum,level (stateOffset:dim)" << std::endl;
+ for (MobilizedBodyIndex i(0); i < tree.getNumBodies(); ++i) {
+ o << i << "->" << tree.nodeNum2NodeMap[i].level << ","
+ << tree.nodeNum2NodeMap[i].offset << ";";
+ const RigidBodyNode& n = tree.getRigidBodyNode(i);
+ o << n.getNodeNum() << "," << n.getLevel()
+ <<"(u"<< n.getUIndex()<<":"<<n.getDOF()
+ <<",q"<< n.getQIndex()<<":"<<n.getMaxNQ()<<")"<< std::endl;
+ }
+
+ return o;
+}
+
+ /////////////////////
+ // SB STATE DIGEST //
+ /////////////////////
+
+void SBStateDigest::fillThroughStage(const SimbodyMatterSubsystemRep& matter, Stage g) {
+ assert((int)g <= (int)matter.getStage(state) + 1);
+ clear();
+
+ if (state.getSystemStage() >= Stage::Model) {
+ q = &matter.getQ(state);
+ u = &matter.getU(state);
+ }
+ if (g >= Stage::Model) {
+ mv = &matter.getModelVars(state);
+ mc = &matter.updModelCache(state);
+ iv = &matter.getInstanceVars(state);
+ }
+ if (g >= Stage::Instance) {
+ if (mc->instanceCacheIndex.isValid())
+ ic = &matter.updInstanceCache(state);
+
+ // All cache entries, for any stage, can be modified at instance stage
+ // or later.
+
+ if (mc->timeCacheIndex.isValid())
+ tc = &matter.updTimeCache(state);
+ if (mc->treePositionCacheIndex.isValid())
+ tpc = &matter.updTreePositionCache(state);
+ if (mc->constrainedPositionCacheIndex.isValid())
+ cpc = &matter.updConstrainedPositionCache(state);
+ if (mc->treeVelocityCacheIndex.isValid())
+ tvc = &matter.updTreeVelocityCache(state);
+ if (mc->constrainedVelocityCacheIndex.isValid())
+ cvc = &matter.updConstrainedVelocityCache(state);
+ if (mc->dynamicsCacheIndex.isValid())
+ dc = &matter.updDynamicsCache(state);
+ if (mc->treeAccelerationCacheIndex.isValid())
+ tac = &matter.updTreeAccelerationCache(state);
+ if (mc->constrainedAccelerationCacheIndex.isValid())
+ cac = &matter.updConstrainedAccelerationCache(state);
+ }
+ if (g >= Stage::Time) {
+ if (mc->timeVarsIndex.isValid())
+ tv = &matter.getTimeVars(state);
+ }
+ if (g >= Stage::Position) {
+ if (mc->qVarsIndex.isValid())
+ pv = &matter.getPositionVars(state);
+
+ qErr = &matter.updQErr(state);
+ }
+ if (g >= Stage::Velocity) {
+ if (mc->uVarsIndex.isValid())
+ vv = &matter.getVelocityVars(state);
+
+ qdot = &matter.updQDot(state);
+ uErr = &matter.updUErr(state);
+ }
+ if (g >= Stage::Dynamics) {
+ if (mc->dynamicsVarsIndex.isValid())
+ dv = &matter.getDynamicsVars(state);
+
+ // Prescribed accelerations are filled in at Dynamics
+ // stage so we may need these now.
+ udot = &matter.updUDot(state);
+ qdotdot = &matter.updQDotDot(state);
+ }
+ if (g >= Stage::Acceleration) {
+ if (mc->accelerationVarsIndex.isValid())
+ av = &matter.getAccelerationVars(state);
+
+ udotErr = &matter.updUDotErr(state);
+ }
+
+ stage = g;
+}
+
diff --git a/Simbody/src/SimbodyMatterSubsystemRep.h b/Simbody/src/SimbodyMatterSubsystemRep.h
new file mode 100644
index 0000000..622e734
--- /dev/null
+++ b/Simbody/src/SimbodyMatterSubsystemRep.h
@@ -0,0 +1,1328 @@
+#ifndef SimTK_SIMBODY_MATTER_SUBSYSTEM_REP_H_
+#define SimTK_SIMBODY_MATTER_SUBSYSTEM_REP_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: Derived from IVM code written by Charles Schwieters *
+ * *
+ * 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/MultibodySystem.h"
+#include "simbody/internal/SimbodyMatterSubsystem.h"
+#include "simbody/internal/SimbodyMatterSubtree.h"
+#include "simbody/internal/MobilizedBody.h"
+#include "simbody/internal/MobilizedBody_Ground.h"
+
+#include "SimbodyTreeState.h"
+#include "RigidBodyNode.h"
+
+#include <set>
+#include <map>
+#include <utility> // std::pair
+using std::pair;
+
+
+#include <cassert>
+#include <iostream>
+#include <vector>
+#include <map>
+#include <set>
+#include <algorithm>
+
+class RigidBodyNode;
+class RBDistanceConstraint;
+class RBStation;
+class RBDirection;
+
+using namespace SimTK;
+
+typedef Array_<const RigidBodyNode*> RBNodePtrList;
+typedef Vector_<SpatialVec> SpatialVecList;
+
+/*
+ * A CoupledConstraintSet is a set of Simbody Constraints which must be
+ * handled simultaneously for purposes of a particular computation. We maintain a
+ * Subtree here which encompasses all the included Constraints. This
+ * Subtree has as its terminal bodies the set of all constrained bodies
+ * from any of the included Constraints. The ancestor body will be
+ * the outmost common ancestor of all those constrained bodies, or
+ * equivalently the outmost common ancestor of all the Constraints'
+ * ancestor bodies.
+ *
+ * Operators and responses here set up an environment in which the
+ * individual constraint error functions are evaluated. The environments
+ * are:
+ * - same as global System state (i.e., this is just a response)
+ * - all mobility variables from State, except for one which is perturbed (q,u,udot)
+ * - all mobility variables are zero (u,udot)
+ * - all mobility variables are zero except for one (u,udot)
+ */
+
+class CoupledConstraintSet {
+public:
+ CoupledConstraintSet() : topologyRealized(false) { }
+ void addConstraint(ConstraintIndex cid) {
+ topologyRealized = false;
+ constraints.insert(cid);
+ }
+
+ void mergeInConstraintSet(const CoupledConstraintSet& src) {
+ topologyRealized = false;
+ for (int i=0; i < (int)src.getCoupledConstraints().size(); ++i)
+ constraints.insert(src.getCoupledConstraints()[i]);
+ }
+
+ void realizeTopology(const SimbodyMatterSubsystem& matter) {
+ coupledConstraints.resize((unsigned)constraints.size());
+ std::set<ConstraintIndex>::const_iterator i = constraints.begin();
+ for (int nxt=0; i != constraints.end(); ++i, ++nxt)
+ coupledConstraints[nxt] = *i;
+ topologyRealized = true;
+ }
+
+ const Array_<ConstraintIndex>& getCoupledConstraints() const {
+ assert(topologyRealized);
+ return coupledConstraints;
+ }
+
+private:
+ // TOPOLOGY STATE
+ std::set<ConstraintIndex> constraints;
+
+ // TOPOLOGY CACHE
+ bool topologyRealized;
+
+ // Sorted in nondecreasing order of ancestor MobilizedBodyIndex.
+ Array_<ConstraintIndex> coupledConstraints;
+ SimbodyMatterSubtree coupledSubtree; // with the new ancestor
+};
+
+ //////////////////////////////////
+ // SIMBODY MATTER SUBSYSTEM REP //
+ //////////////////////////////////
+
+/*
+ * The SimbodyMatterSubsystemRep class owns the tree of MobilizedBodies and their
+ * associated computational form inherited from IVM, called RigidBodyNodes.
+ * Here we store references to the RigidBodyNodes in a tree structure organized
+ * in "levels" with level 0 being Ground, level 1
+ * being bodies which are connected to Ground (base bodies), level 2 connected to
+ * level 1 and so on. Nodes at the same level are stored together in an array,
+ * but the order does not reflect the logical tree structure; that is maintained
+ * via parent & children pointers kept in the nodes.
+ *
+ * Access to mobilized body information is requested via MobilizedBodyIndex's here,
+ * which are small integer indices, rather than via MobilizedBody objects. These
+ * integers are used both to index MobilizedBodies and their corresponding
+ * RigidBodyNodes. You can get the abstract MobilizedBody corresponding to a
+ * MobilizedBodyIndex if you want one, but you should prefer MobilizedBodyIndex's when
+ * working at the "Rep" level here.
+ */
+class SimbodyMatterSubsystemRep : public SimTK::Subsystem::Guts {
+public:
+ SimbodyMatterSubsystemRep()
+ : Subsystem::Guts("SimbodyMatterSubsystem", "0.7.1")
+ {
+ clearTopologyCache();
+ }
+
+ SimbodyMatterSubsystemRep(const SimbodyMatterSubsystemRep&);
+ SimbodyMatterSubsystemRep& operator=(const SimbodyMatterSubsystemRep&);
+
+
+ // IMPLEMENTATIONS OF SUBSYSTEM::GUTS VIRTUAL METHODS
+
+ // Subsystem::Guts destructor is virtual
+ ~SimbodyMatterSubsystemRep() {
+ invalidateSubsystemTopologyCache();
+ clearTopologyCache(); // should do cache before state
+ clearTopologyState();
+ }
+
+ SimbodyMatterSubsystemRep* cloneImpl() const {
+ return new SimbodyMatterSubsystemRep(*this);
+ }
+
+ int realizeSubsystemTopologyImpl (State&) const;
+ int realizeSubsystemModelImpl (State&) const;
+ int realizeSubsystemInstanceImpl (const State&) const;
+ int realizeSubsystemTimeImpl (const State&) const;
+ int realizeSubsystemPositionImpl (const State&) const;
+ int realizeSubsystemVelocityImpl (const State&) const;
+ int realizeSubsystemDynamicsImpl (const State&) const;
+ int realizeSubsystemAccelerationImpl(const State&) const;
+ int realizeSubsystemReportImpl (const State&) const;
+
+ int calcDecorativeGeometryAndAppendImpl
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const;
+
+ // TODO: these are just unit weights and tolerances. They should be calculated
+ // to be something more reasonable.
+
+ int calcQUnitWeightsImpl(const State& s, Vector& weights) const;
+ int calcUUnitWeightsImpl(const State& s, Vector& weights) const;
+ int calcZUnitWeightsImpl(const State& s, Vector& weights) const {
+ weights.resize(getNZ(s));
+ weights = 1;
+ return 0;
+ }
+ int calcQErrUnitTolerancesImpl(const State& s, Vector& tolerances) const {
+ tolerances.resize(getNQErr(s));
+ tolerances = 1;
+ return 0;
+ }
+ int calcUErrUnitTolerancesImpl(const State& s, Vector& tolerances) const {
+ tolerances.resize(getNUErr(s));
+ tolerances = 1;
+ return 0;
+ }
+
+ // END OF SUBSYSTEM::GUTS VIRTUALS.
+
+ // Return the MultibodySystem which owns this MatterSubsystem.
+ const MultibodySystem& getMultibodySystem() const {
+ return MultibodySystem::downcast(getSystem());
+ }
+
+
+ // CONSTRUCTION STAGE //
+
+ // The SimbodyMatterSubsystemRep takes over ownership of the child
+ // MobilizedBody handle (leaving child as a non-owner reference), and
+ // makes it a child (outboard body) of the indicated parent. The new
+ // child body's id is returned, and will be greater than the parent's id.
+
+ MobilizedBodyIndex adoptMobilizedBody(MobilizedBodyIndex parentIndex, MobilizedBody& child);
+ int getNumMobilizedBodies() const {return (int)mobilizedBodies.size();}
+
+ const MobilizedBody& getMobilizedBody(MobilizedBodyIndex ix) const {
+ assert(ix < (int)mobilizedBodies.size());
+ assert(mobilizedBodies[ix]);
+ return *mobilizedBodies[ix];
+ }
+
+ // Note that we do not invalidate the subsystem topology cache yet, even
+ // though we're handing out a writable reference to a MobilizedBody here.
+ // Otherwise every time someone references Ground, e.g., by calling
+ // matterSubsys.Ground() the subsystem would have its topology marked
+ // invalid. For this reason, and also because the main program normally
+ // retains a writable reference to MobilizedBodies, it is essential that
+ // every non-const method of MobilizedBody and its many descendants mark
+ // the subsystem topology invalid when called.
+ MobilizedBody& updMobilizedBody(MobilizedBodyIndex ix) {
+ assert(ix < (int)mobilizedBodies.size());
+ assert(mobilizedBodies[ix]);
+ return *mobilizedBodies[ix]; // topology not marked invalid yet
+ }
+
+ void createGroundBody();
+
+ const MobilizedBody::Ground& getGround() const {
+ return MobilizedBody::Ground::downcast(getMobilizedBody(MobilizedBodyIndex(0)));
+ }
+ MobilizedBody::Ground& updGround() {
+ return MobilizedBody::Ground::updDowncast(updMobilizedBody(MobilizedBodyIndex(0)));
+ }
+
+
+ // Constraints are treated similarly to MobilizedBodies here.
+
+ ConstraintIndex adoptConstraint(Constraint& child);
+
+ const Constraint& getConstraint(ConstraintIndex id) const {
+ assert(id < (int)constraints.size());
+ assert(constraints[id]);
+ return *constraints[id];
+ }
+ Constraint& updConstraint(ConstraintIndex id) {
+ assert(id < (int)constraints.size());
+ assert(constraints[id]);
+ return *constraints[id];
+ }
+
+ // Topology stage cache entry
+ AncestorConstrainedBodyPoolIndex
+ allocateNextAncestorConstrainedBodyPoolSlot() const {
+ const AncestorConstrainedBodyPoolIndex nxt = nextAncestorConstrainedBodyPoolSlot;
+ // Make this mutable briefly.
+ ++ const_cast<SimbodyMatterSubsystemRep*>(this)->nextAncestorConstrainedBodyPoolSlot;
+ return nxt;
+ }
+
+
+ // These counts can be obtained even during construction, where they
+ // just return the current counts.
+ // NumBodies includes ground.
+ int getNumBodies() const {return mobilizedBodies.size();}
+ int getNumParticles() const {return 0;} // TODO
+ int getNumMobilities() const {return getTotalDOF();}
+ int getNumConstraints() const {return constraints.size();}
+ MobilizedBodyIndex getParent(MobilizedBodyIndex) const;
+ Array_<MobilizedBodyIndex> getChildren(MobilizedBodyIndex) const;
+
+ const MassProperties& getDefaultBodyMassProperties (MobilizedBodyIndex b) const;
+ const Transform& getDefaultMobilizerFrame (MobilizedBodyIndex b) const;
+ const Transform& getDefaultMobilizerFrameOnParent(MobilizedBodyIndex b) const;
+
+ void findMobilizerQs(const State& s, MobilizedBodyIndex body, QIndex& qStart, int& nq) const {
+ const RigidBodyNode& n = getRigidBodyNode(body);
+ qStart = n.getQIndex();
+ nq = n.getNQInUse(getModelVars(s));
+ }
+
+ void findMobilizerUs(const State& s, MobilizedBodyIndex body, UIndex& uStart, int& nu) const {
+ const RigidBodyNode& n = getRigidBodyNode(body);
+ uStart = n.getUIndex();
+ nu = n.getDOF();
+ }
+
+ // Access to Instance variables. //
+
+ const MassProperties& getBodyMassProperties(const State& s, MobilizedBodyIndex b) const {
+ return getInstanceVars(s).bodyMassProperties[b];
+ }
+ const Transform& getMobilizerFrame(const State& s, MobilizedBodyIndex b) const {
+ return getInstanceVars(s).outboardMobilizerFrames[b];
+ }
+ const Transform& getMobilizerFrameOnParent(const State& s, MobilizedBodyIndex b) const {
+ return getInstanceVars(s).inboardMobilizerFrames[b];
+ }
+
+ MassProperties& updBodyMassProperties(State& s, MobilizedBodyIndex b) const {
+ return updInstanceVars(s).bodyMassProperties[b];
+ }
+ Transform& updMobilizerFrame(State& s, MobilizedBodyIndex b) const {
+ return updInstanceVars(s).outboardMobilizerFrames[b];
+ }
+ Transform& updMobilizerFrameOnParent(State& s, MobilizedBodyIndex b) const {
+ return updInstanceVars(s).inboardMobilizerFrames[b];
+ }
+
+ const Vector& getAllParticleMasses(const State& s) const {
+ return getInstanceVars(s).particleMasses;
+ }
+ Vector& updAllParticleMasses(State& s) const {
+ return updInstanceVars(s).particleMasses;
+ }
+
+ Real getTotalMass(const State& s) const {
+ return getInstanceCache(s).totalMass;
+ }
+
+ // Extract position, velocity, and acceleration information for
+ // MobilizedBodies out of the State cache, accessing only the Tree cache
+ // entries at each level, which should already have been marked valid.
+ const Transform& getBodyTransform (const State&, MobilizedBodyIndex) const;
+ const SpatialVec& getBodyVelocity (const State&, MobilizedBodyIndex) const;
+ const SpatialVec& getBodyAcceleration(const State&, MobilizedBodyIndex) const;
+
+ // Call at Position stage or later. If necessary, composite body inertias
+ // will be realized first.
+ const Array_<SpatialInertia,MobilizedBodyIndex>&
+ getCompositeBodyInertias(const State& s) const {
+ realizeCompositeBodyInertias(s);
+ return getCompositeBodyInertiaCache(s).compositeBodyInertia;
+ }
+
+ // Call at Position stage or later. If necessary, articulated body
+ // inertias will be realized first.
+ const Array_<ArticulatedInertia,MobilizedBodyIndex>&
+ getArticulatedBodyInertias(const State& s) const {
+ realizeArticulatedBodyInertias(s);
+ return getArticulatedBodyInertiaCache(s).articulatedBodyInertia;
+ }
+
+ const Array_<ArticulatedInertia,MobilizedBodyIndex>&
+ getArticulatedBodyInertiasPlus(const State& s) const {
+ realizeArticulatedBodyInertias(s);
+ return getArticulatedBodyInertiaCache(s).pPlus;
+ }
+
+ // Call at Acceleration stage only.
+ const Array_<SpatialVec,MobilizedBodyIndex>&
+ getArticulatedBodyForces(const State& s) const {
+ return getTreeAccelerationCache(s).z;
+ }
+ const Array_<SpatialVec,MobilizedBodyIndex>&
+ getArticulatedBodyForcesPlus(const State& s) const {
+ return getTreeAccelerationCache(s).zPlus;
+ }
+
+ // velocity dependent
+ const SpatialVec& getMobilizerCoriolisAcceleration(const State&, MobilizedBodyIndex) const;
+ const SpatialVec& getTotalCoriolisAcceleration (const State&, MobilizedBodyIndex) const;
+ const SpatialVec& getGyroscopicForce (const State&, MobilizedBodyIndex) const;
+ const SpatialVec& getMobilizerCentrifugalForces (const State&, MobilizedBodyIndex) const;
+ const SpatialVec& getTotalCentrifugalForces (const State&, MobilizedBodyIndex) const;
+
+ // PARTICLES TODO
+
+ const Vector_<Vec3>& getAllParticleLocations (const State&) const {
+ static const Vector_<Vec3> v;
+ return v;
+ }
+ const Vector_<Vec3>& getAllParticleVelocities(const State&) const {
+ static const Vector_<Vec3> v;
+ return v;
+ }
+ // Invalidate Stage::Position.
+ Vector_<Vec3>& updAllParticleLocations (State&) const {
+ static Vector_<Vec3> v;
+ return v;
+ }
+ // Invalidate Stage::Velocity.
+ Vector_<Vec3>& updAllParticleVelocities(State&) const {
+ static Vector_<Vec3> v;
+ return v;
+ }
+ const Vector_<Vec3>& getAllParticleAccelerations(const State&) const {
+ static const Vector_<Vec3> v;
+ return v;
+ }
+
+ // This is a solver that sets continuous state variables q or u
+ // (depending on stage) to their prescribed values that will already
+ // have been computed. Note that prescribed udot=udot(t,q,u) is not
+ // dealt with here because it does not involve a state change.
+ // Returns true if it makes any changes.
+ bool prescribeQ(State& s) const;
+ bool prescribeU(State& s) const;
+
+ // Project quaternions onto their constraint manifold by normalizing
+ // them. Also removes any error component along the length of the
+ // quaternions if given a qErrest. Returns true if any change was made.
+ // This is used by projectQ().
+ bool normalizeQuaternions(State& s, Vector& qErrest) const;
+
+ int projectQ(State& s, Vector& qErrest,
+ const ProjectOptions& opts,
+ ProjectResults& results) const;
+ int projectU(State& s, Vector& uErrest,
+ const ProjectOptions& opts,
+ ProjectResults& results) const;
+
+ // REALIZATIONS //
+
+ // Call at Position Stage or later.
+ void realizeCompositeBodyInertias(const State&) const;
+
+ // Call at Position Stage or later.
+ void realizeArticulatedBodyInertias(const State&) const;
+
+ // These are just used in timing tests.
+ void invalidateCompositeBodyInertias(const State&) const;
+ void invalidateArticulatedBodyInertias(const State&) const;
+
+ // OPERATORS //
+
+ Real calcKineticEnergy(const State&) const;
+
+ void calcCompositeBodyInertias(const State&,
+ Array_<SpatialInertia,MobilizedBodyIndex>& R) const;
+
+ // Calculate the product J*v where J is the kinematic Jacobian
+ // dV/du=~Phi*~H (Schwieters' and Jain's terminology; our H is transposed
+ // from theirs), and v is a vector in mobility space (internal
+ // coordinates). If v==u, that is, it is a vector of generalized speeds,
+ // then the result will be V_GB, the spatial velocity of each body.
+ // This is an O(N) operator which can be called once the State is realized
+ // to Stage::Position or higher. Because this is an operator, there is
+ // no effect on the State cache.
+ void multiplyBySystemJacobian(const State&,
+ const Vector& v,
+ Vector_<SpatialVec>& Jv) const;
+
+ // Calculate the product ~J*X where J is the partial velocity Jacobian
+ // dV/du (~J=H*Phi)and X is a vector of force-space SpatialVec's, one per
+ // body. See Eq. 76&77 in Schwieters' paper, and see 81a & b for a use of
+ // this routine to compute energy gradient in internal coordinates. In that
+ // case X=dE/dR, that is the gradient of the energy w.r.t. atomic positions,
+ // summed and shifted to body origins. There we are pretending dR/dq is the
+ // same as dV/du, which will be true if dq/dt = u. In general, we have
+ // dR/dq = (dV/du)*N^-1, where dq/dt = N*u (i.e., N= d qdot/du). But note
+ // that this method works in terms of u, not q, so it produces a meaningful
+ // result in all cases, just not one that can be mapped directly back to
+ // generalized coordinates q. This is an O(n) operator which can be called
+ // after realizePosition(). Because this is an operator, there is no effect
+ // on the State cache.
+ void multiplyBySystemJacobianTranspose(const State&,
+ const Vector_<SpatialVec>& X,
+ Vector& JtX) const;
+
+ // Given a set of body forces, return the equivalent set of mobilizer torques
+ // IGNORING CONSTRAINTS.
+ // Must be in DynamicsStage so that articulated body inertias are available,
+ // however, velocities are ignored. This operator has NO effect on the state
+ // cache. It makes a single O(N) pass.
+ void calcTreeEquivalentMobilityForces(const State&,
+ const Vector_<SpatialVec>& bodyForces,
+ Vector& mobilityForces) const;
+
+ void calcTreeAccelerations(const State& s,
+ const Vector& mobilityForces,
+ const Vector_<SpatialVec>& bodyForces,
+ const Array_<Real>& presUDots, // packed
+ Vector& netHingeForces,
+ Array_<SpatialVec,MobilizedBodyIndex>& abForcesZ,
+ Array_<SpatialVec,MobilizedBodyIndex>& abForcesZPlus,
+ Vector_<SpatialVec>& A_GB,
+ Vector& udot, // in/out (in for prescribed udots)
+ Vector& qdotdot,
+ Vector& tau) const;
+
+ // Multiply by the mass matrix in O(n) time.
+ void multiplyByM(const State& s,
+ const Vector& a,
+ Vector& Ma) const;
+
+ // Multiply by the mass matrix inverse in O(n) time. Works only with the
+ // non-prescribed submatrix Mrr of M; entries f_p in f are not accessed,
+ // and entries MInvf_p in MInvf are not written.
+ void multiplyByMInv(const State& s,
+ const Vector& f,
+ Vector& MInvf) const;
+
+ // Calculate the mass matrix in O(n^2) time. State must have already
+ // been realized to Position stage. M must be resizeable or already the
+ // right size (nXn). The result is symmetric but the entire matrix is
+ // filled in.
+ void calcM(const State& s, Matrix& M) const;
+
+ // Calculate the mass matrix inverse in O(n^2) time. State must have already
+ // been realized to Position stage. MInv must be resizeable or already the
+ // right size (nXn). The result is symmetric but the entire matrix is
+ // filled in. Only the non-prescribed block Mrr is inverted; other elements
+ // are not written.
+ void calcMInv(const State& s, Matrix& MInv) const;
+
+ void calcTreeResidualForces(const State&,
+ const Vector& appliedMobilityForces,
+ const Vector_<SpatialVec>& appliedBodyForces,
+ const Vector& knownUdot,
+ Vector_<SpatialVec>& A_GB,
+ Vector& residualMobilityForces) const;
+
+
+
+ // Must be in Stage::Position to calculate out_q = N(q)*in_u (e.g., qdot=N*u)
+ // or out_u = in_q * N(q). Note that one of "in" and "out" is always "q-like" while
+ // the other is "u-like", but which is which changes if the matrix is on the right.
+ // This is an O(n) operator since N is block diagonal.
+ void multiplyByN(const State& s, bool matrixOnRight, const Vector& in, Vector& out) const;
+
+ // Must be in Stage::Position to calculate out_u = NInv(q)*in_q (e.g., u=NInv*qdot)
+ // or out_q = in_u * NInv(q). Note that one of "in" and "out" is always "q-like" while
+ // the other is "u-like", but which is which changes if the matrix is on the right.
+ // This is an O(n) operator since NInv is block diagonal.
+ void multiplyByNInv(const State& s, bool matrixOnRight, const Vector& in, Vector& out) const;
+
+ // Must be in Stage::Velocity to calculate out_q = NDot(q,u)*in_u
+ // or out_u = in_q * NDot(q,u). Note that one of "in" and "out" is always
+ // "q-like" while the other is "u-like", but which is which changes if the
+ // matrix is on the right. This is an O(n) operator since NDot is block diagonal.
+ void multiplyByNDot(const State& s, bool matrixOnRight, const Vector& in, Vector& out) const;
+
+ // Must be in Stage::Position to calculate qdot = N*u.
+ void calcQDot(const State& s,
+ const Vector& u,
+ Vector& qdot) const;
+
+ // Must be in Stage::Velocity to calculate qdotdot = Ndot*u + N*udot.
+ void calcQDotDot(const State& s,
+ const Vector& udot,
+ Vector& qdotdot) const;
+
+ // MOBILIZER OPERATORS //
+
+ // These operators deal with an isolated mobilizer and are thus independent of
+ // any other generalized coordinates or speeds.
+
+ // State must be realized to Stage::Position, so that we can extract N(q) from it to calculate
+ // qdot=N(q)*u for this mobilizer.
+ void calcMobilizerQDotFromU(const State&, MobilizedBodyIndex, int nu, const Real* u,
+ int nq, Real* qdot) const;
+
+ // State must be realized to Stage::Velocity, so that we can extract N(q), NDot(q,u), and u from it to calculate
+ // qdotdot=N(q)*udot + NDot(q,u)*u for this mobilizer.
+ void calcMobilizerQDotDotFromUDot(const State&, MobilizedBodyIndex, int nu, const Real* udot,
+ int nq, Real* qdotdot) const;
+
+ // State must be realized through Stage::Instance. Neither the State nor its
+ // cache are modified by this method, since it is an operator.
+ // The number of q's is passed in as a sanity check, to make sure the caller
+ // and the called mobilizer agree on the generalized coordinates.
+ // Returns X_FM(q).
+ Transform calcMobilizerTransformFromQ(const State&, MobilizedBodyIndex, int nq, const Real* q) const;
+
+ // State must be realized through Stage::Position. Neither the State nor its
+ // cache are modified by this method, since it is an operator.
+ // The number of u's is passed in as a sanity check, to make sure the caller
+ // and the called mobilizer agree on the generalized speeds.
+ // Returns V_FM(q,u)=H_FM(q)*u, where the q dependency is extracted from the State via
+ // the hinge transition matrix H_FM(q).
+ SpatialVec calcMobilizerVelocityFromU(const State&, MobilizedBodyIndex, int nu, const Real* u) const;
+
+ // State must be realized through Stage::Velocity. Neither the State nor its
+ // cache are modified by this method, since it is an operator.
+ // The number of u's (and udot's) is passed in as a sanity check, to make sure the caller
+ // and the called mobilizer agree on the generalized accelerations.
+ // Returns A_FM(q,u,udot)=H_FM(q)*udot + HDot_FM(q,u)*u where the q and u dependencies
+ // are extracted from the State via H_FM(q), and HDot_FM(q,u).
+ SpatialVec calcMobilizerAccelerationFromUDot(const State&, MobilizedBodyIndex, int nu, const Real* udot) const;
+
+ // These perform the same computations as above but then transform the results so that they
+ // relate the child body's frame B to its parent body's frame P, rather than the M and F frames
+ // which are attached to B and P respectively but differ by a constant transform.
+ Transform calcParentToChildTransformFromQ(const State& s, MobilizedBodyIndex mb, int nq, const Real* q) const;
+ SpatialVec calcParentToChildVelocityFromU (const State& s, MobilizedBodyIndex mb, int nu, const Real* u) const;
+ SpatialVec calcParentToChildAccelerationFromUDot(const State& s, MobilizedBodyIndex mb, int nu, const Real* udot) const;
+
+
+ void setDefaultModelValues (const SBTopologyCache&, SBModelVars&) const;
+ void setDefaultInstanceValues (const SBModelVars&, SBInstanceVars&) const;
+ void setDefaultTimeValues (const SBModelVars&, SBTimeVars&) const;
+ void setDefaultPositionValues (const SBModelVars&, Vector& q) const;
+ void setDefaultVelocityValues (const SBModelVars&, Vector& u) const;
+ void setDefaultDynamicsValues (const SBModelVars&, SBDynamicsVars&) const;
+ void setDefaultAccelerationValues(const SBModelVars&, SBAccelerationVars&) const;
+
+ // CALLABLE AFTER realizeTopology()
+
+ int getTotalDOF() const {assert(subsystemTopologyHasBeenRealized()); return DOFTotal;}
+ int getTotalSqDOF() const {assert(subsystemTopologyHasBeenRealized()); return SqDOFTotal;}
+ int getTotalQAlloc() const {assert(subsystemTopologyHasBeenRealized()); return maxNQTotal;}
+
+ int getNumTopologicalPositionConstraintEquations() const {
+ assert(subsystemTopologyHasBeenRealized()); return nextQErrSlot;
+ }
+ int getNumTopologicalVelocityConstraintEquations() const {
+ assert(subsystemTopologyHasBeenRealized()); return nextUErrSlot;
+ }
+ int getNumTopologicalAccelerationConstraintEquations() const {
+ assert(subsystemTopologyHasBeenRealized()); return nextMultSlot;
+ }
+
+ int getQIndex(MobilizedBodyIndex) const;
+ int getQAlloc(MobilizedBodyIndex) const;
+ int getUIndex(MobilizedBodyIndex) const;
+ int getDOF (MobilizedBodyIndex) const;
+
+ // Modeling info.
+
+ void setUseEulerAngles(State& s, bool useAngles) const;
+ void setConstraintIsDisabled(State& s, ConstraintIndex constraint, bool disabled) const;
+ bool getUseEulerAngles(const State& s) const;
+ bool isConstraintDisabled(const State& s, ConstraintIndex constraint) const;
+ void convertToEulerAngles(const State& inputState, State& outputState) const;
+ void convertToQuaternions(const State& inputState, State& outputState) const;
+
+ // CALLABLE AFTER realizeModel()
+
+ int getNumQuaternionsInUse(const State&) const; // mquat
+ bool isUsingQuaternion(const State&, MobilizedBodyIndex) const;
+ QuaternionPoolIndex getQuaternionPoolIndex(const State&, MobilizedBodyIndex) const; // Invalid if none
+
+ // Note that although holonomic constraints are position-level constraints, they
+ // do *not* include quaternion constraints (although the state's QErr vector does
+ // include both). The total number of position-level constraints is thus
+ // getNumHolonomicConstraintEquationsInUse()+getNumQuaternionsInUse()==mp+mquat.
+
+ int getNumHolonomicConstraintEquationsInUse (const State&) const; // mh
+ int getNumNonholonomicConstraintEquationsInUse (const State&) const; // mn
+ int getNumAccelerationOnlyConstraintEquationsInUse(const State&) const; // ma
+
+ void calcHolonomicConstraintMatrixPNInv (const State&, Matrix&) const; // mh X nq
+ void calcHolonomicVelocityConstraintMatrixP(const State&, Matrix&) const; // mh X nu
+ void calcHolonomicVelocityConstraintMatrixPt(const State&, Matrix&) const; // nu X mh
+ void calcNonholonomicConstraintMatrixV (const State&, Matrix&) const; // mn X nu
+ void calcNonholonomicConstraintMatrixVt (const State&, Matrix&) const; // nu X mn
+ void calcAccelerationOnlyConstraintMatrixA (const State&, Matrix&) const; // ma X nu
+ void calcAccelerationOnlyConstraintMatrixAt(const State&, Matrix&) const; // nu X ma
+
+ void calcMobilizerReactionForces(const State& s, Vector_<SpatialVec>& forces) const;
+ // This alternative is for debugging and testing; it is slow but should
+ // produce the same answers as calcMobilizerReactionForces().
+ void calcMobilizerReactionForcesUsingFreebodyMethod(const State& s, Vector_<SpatialVec>& forces) const;
+
+
+
+ // Constraint multipliers are known to the State object.
+ const Vector& getConstraintMultipliers(const State& s) const
+ { return this->getMultipliers(s); }
+
+ // But taus (prescribed motion "multipliers") are internal.
+ const Vector& getMotionMultipliers(const State& s) const {
+ const SBTreeAccelerationCache& tac = getTreeAccelerationCache(s);
+ return tac.presMotionForces;
+ }
+
+ Vector calcMotionErrors(const State& state, const Stage& stage) const;
+
+ void findMotionForces(const State& s,
+ Vector& mobilityForces) const;
+ void findConstraintForces(const State& s,
+ Vector_<SpatialVec>& bodyForcesInG,
+ Vector& mobilityForces) const;
+
+ Real calcMotionPower(const State& s) const;
+ Real calcConstraintPower(const State& s) const;
+
+ // Treating all constraints together, given a comprehensive set of
+ // multipliers lambda, generate the complete set of body and mobility
+ // forces applied by all the constraints. Return the
+ // individual Constraints' force contributions in the final two arguments.
+ void calcConstraintForcesFromMultipliers
+ (const State& state,
+ const Vector& lambda,
+ Vector_<SpatialVec>& bodyForcesInG,
+ Vector& mobilityForces,
+ Array_<SpatialVec>& constrainedBodyForcesInG,
+ Array_<Real>& contraintMobilityForces) const;
+
+ // Call this signature if you don't care about the individual constraint
+ // contributions.
+ void calcConstraintForcesFromMultipliers
+ (const State& state,
+ const Vector& lambda,
+ Vector_<SpatialVec>& bodyForcesInG,
+ Vector& mobilityForces) const
+ {
+ const SBInstanceCache& ic = getInstanceCache(state);
+ const int ncb = ic.totalNConstrainedBodiesInUse;
+ const int ncu = ic.totalNConstrainedUInUse;
+ Array_<SpatialVec> constrainedBodyForcesInG(ncb);
+ Array_<Real> constraintMobilityForces(ncu);
+ calcConstraintForcesFromMultipliers(state,lambda,bodyForcesInG,
+ mobilityForces,constrainedBodyForcesInG,constraintMobilityForces);
+ }
+
+ // Form the product
+ // fu = [ ~P ~V ~A ] * lambda
+ // with all or a subset of P,V,A included. The multiplier-like vector
+ // must have length m=mp+mv+ma always. This is an O(n+m) method.
+ void multiplyByPVATranspose(const State& state,
+ bool includeP,
+ bool includeV,
+ bool includeA,
+ const Vector& lambda,
+ Vector& fu) const;
+
+ // Explicitly form the u-space constraint Jacobian transpose
+ // ~G=[~P ~V ~A] or selected submatrices of it. Performance is best if the
+ // output matrix has columns stored contiguously in memory, but this method
+ // will work anyway, in that case using a contiguous temporary for column
+ // calculations and then copying out into the result. The matrix will be
+ // resized as necessary to nu X (mp+mv+ma) where the constraint dimensions
+ // can be zero if that submatrix is not selected.
+ void calcPVATranspose( const State& state,
+ bool includeP,
+ bool includeV,
+ bool includeA,
+ Matrix& PVAt) const;
+
+ // Form the product
+ // fq = [ ~Pq ] * lambdap
+ // The multiplier-like vector must have length m=mp always.
+ // This is equivalent to ~(P*N^-1)*lambdap = ~N^-1 * (~P * lambdap).
+ // This is an O(n+mp) method.
+ void multiplyByPqTranspose(const State& state,
+ const Vector& lambdap,
+ Vector& fq) const;
+
+ // Explicitly form the q-space holonomic constraint Jacobian transpose
+ // Pqt (= ~(N^-1) * ~P). Performance is best if the output matrix has
+ // columns stored contiguously in memory, but this method will work anyway,
+ // in that case using a contiguous temporary for column calculations and
+ // then copying out into the result. The matrix will be resized as
+ // necessary to nq X mp.
+ void calcPqTranspose( const State& state,
+ Matrix& Pqt) const;
+
+ // Calculate the bias vector from the constraint error
+ // equations used in multiplyByPVA. Here bias is what you would get
+ // when ulike==0. The output Vector must use contiguous storage. It will
+ // be resized if necessary to length m=mp+mv+ma.
+ void calcBiasForMultiplyByPVA(const State& state,
+ bool includeP,
+ bool includeV,
+ bool includeA,
+ Vector& bias) const;
+
+ // Calculate the bias vector from the acceleration constraint error
+ // equations used. Here bias is what you would get from paerr, vaerr,
+ // and aerr when udot==0. This is different than calcBiasForMultiplyByPVA()
+ // because that method uses pverr for holonomic constraints rather than
+ // paerr. The output Vector must use contiguous storage. It will
+ // be resized if necessary to length m=mp+mv+ma.
+ void calcBiasForAccelerationConstraints(const State& state,
+ bool includeP,
+ bool includeV,
+ bool includeA,
+ Vector& bias) const;
+
+ void calcBiasForMultiplyByPq(const State& state,
+ Vector& bias) const
+ { calcBiasForMultiplyByPVA(state,true,false,false,bias); }
+
+ // Given a bias calculated by the above method using the same settings
+ // for the "include" flags, form the product
+ // [ P ]
+ // PVAu = [ V ] * ulike
+ // [ A ]
+ // with all or a subset of P,V,A included. The u-like vector must have
+ // length nu always. This is an O(n+m) method.
+ void multiplyByPVA(const State& state,
+ bool includeP,
+ bool includeV,
+ bool includeA,
+ const Vector& bias,
+ const Vector& ulike,
+ Vector& PVAu) const;
+
+ // Explicitly form the u-space constraint Jacobian G=[P;V;A] or
+ // selected submatrices of it. Performance is best if the output matrix
+ // has columns stored contiguously in memory, but this method will work
+ // anyway, in that case using a contiguous temporary for column
+ // calculations and then copying out into the result. The matrix will be
+ // resized as necessary to (mp+mv+ma) X nu where the constraint dimensions
+ // can be zero if that submatrix is not selected.
+ void calcPVA( const State& state,
+ bool includeP,
+ bool includeV,
+ bool includeA,
+ Matrix& PVA) const;
+
+ // Given a bias calculated by the above method using just includeP=true
+ // (or the leading bias_p segment of a complete bias vector), form the
+ // product PqXqlike = Pq*qlike (= P*N^-1*qlike). The q-like vector must
+ // have length nq always and all Vectors must use contiguous storage.
+ // This is an O(nq+mp) method.
+ void multiplyByPq( const State& state,
+ const Vector& bias_p,
+ const Vector& qlike,
+ Vector& PqXqlike) const;
+
+ // Explicitly form the q-space holonomic constraint Jacobian Pq (= P*N^-1).
+ // Performance is best if the output matrix has columns stored contiguously
+ // in memory, but this method will work anyway, in that case using a
+ // contiguous temporary for column calculations and then copying out into
+ // the result. The matrix will be resized as necessary to mp X nq.
+ void calcPq( const State& state,
+ Matrix& Pq) const;
+
+ // Calculate the mXm "projected mass matrix" G * M^-1 * G^T. By using
+ // a combination of O(n) operators we can calculate this in O(m*n) time.
+ // The method requires only O(n) memory also, except for the mXm result.
+ // State must be realized through Velocity stage unless all constraints
+ // are holonomic in which case Position will do. This matrix is used
+ // when solving for Lagrange multipliers: (G M^-1 G^T) lambda = aerr
+ // gives values for lambda that elimnate aerr.
+ // Performance is best if the output matrix has columns stored
+ // contiguously in memory, but this method will work anyway, in that case
+ // using a contiguous temporary for column calculations and then copying
+ // out into the result.
+ void calcGMInvGt(const State& state,
+ Matrix& GMInvGt) const;
+
+ // Use factored GMInvGt to solve GMinvGt*impulse=deltaV. The main benefit
+ // of this method is that it promises to use the same method Simbody does
+ // to deal with constraint redundancies.
+ void solveForConstraintImpulses(const State& state,
+ const Vector& deltaV,
+ Vector& impulse) const;
+
+ // Given an array of nu udots, return nb body accelerations in G (including
+ // Ground as the 0th body with A_GB[0]=0). The returned accelerations are
+ // A = J*udot + Jdot*u, with the Jdot*u (coriolis acceleration) term
+ // extracted from the supplied state, which must have been realized to
+ // Velocity stage.
+ // The input and output Vectors must use contiguous storage.
+ void calcBodyAccelerationFromUDot(const State& state,
+ const Vector_<Real>& knownUDot,
+ Vector_<SpatialVec>& A_GB) const;
+
+ // Given an array of nu udots and already-calculated corresponding
+ // qdotdot=N*udot + NDot*u, and the set of corresponding
+ // Ground-relative body accelerations, compute the constraint
+ // acceleration errors that result due to the constraints currently active
+ // in the given state. All acceleration-level constraints are included:
+ // holonomic second derivatives, nonholonomic first derivatives, and
+ // acceleratin-only constraints. This is a pure operator and does not
+ // affect the state or state cache. Vectors must use contiguous data.
+ // State must have been realized through Velocity stage.
+ void calcConstraintAccelerationErrors
+ (const State& state,
+ const Vector_<SpatialVec>& A_GB,
+ const Vector_<Real>& udot,
+ const Vector_<Real>& qdotdot,
+ Vector& pvaerr) const;
+
+ // This is a solver which generates internal velocities from spatial ones.
+ void velFromCartesian(const Vector& pos, Vector& vel) {assert(false);/*TODO*/}
+
+ void enforcePositionConstraints(State& s, Real consAccuracy, const Vector& yWeights,
+ const Vector& ooTols, Vector& yErrest, ProjectOptions) const;
+ void enforceVelocityConstraints(State& s, Real consAccuracy, const Vector& yWeights,
+ const Vector& ooTols, Vector& yErrest, ProjectOptions) const;
+
+ // Unconstrained (tree) dynamics methods for use during realization.
+
+ // Part of OLD constrained dynamics; TODO: may be useful in op space inertia calcs.
+ void realizeY(const State&) const;
+
+ const RigidBodyNode& getRigidBodyNode(MobilizedBodyIndex nodeNum) const {
+ const RigidBodyNodeIndex& ix = nodeNum2NodeMap[nodeNum];
+ return *rbNodeLevels[ix.level][ix.offset];
+ }
+
+ //
+ // STATE ARCHEOLOGY
+ // These routines know where the bodies are buried (no pun intended).
+ //
+
+ // The TopologyCache in the State should be a copy of the one
+ // we keep locally here. We always use our local copy rather than
+ // this one except for checking that the State looks reasonable.
+ const SBTopologyCache& getTopologyCache(const State& s) const {
+ assert(subsystemTopologyHasBeenRealized() && topologyCacheIndex >= 0);
+ return Value<SBTopologyCache>::downcast
+ (s.getCacheEntry(getMySubsystemIndex(),topologyCacheIndex)).get();
+ }
+ SBTopologyCache& updTopologyCache(const State& s) const { //mutable
+ assert(subsystemTopologyHasBeenRealized() && topologyCacheIndex >= 0);
+ return Value<SBTopologyCache>::downcast
+ (s.updCacheEntry(getMySubsystemIndex(),topologyCacheIndex)).upd();
+ }
+
+ const SBModelCache& getModelCache(const State& s) const {
+ return Value<SBModelCache>::downcast
+ (s.getCacheEntry(getMySubsystemIndex(),topologyCache.modelingCacheIndex)).get();
+ }
+ SBModelCache& updModelCache(const State& s) const { //mutable
+ return Value<SBModelCache>::downcast
+ (s.updCacheEntry(getMySubsystemIndex(),topologyCache.modelingCacheIndex)).upd();
+ }
+
+ const SBInstanceCache& getInstanceCache(const State& s) const {
+ return Value<SBInstanceCache>::downcast
+ (s.getCacheEntry(getMySubsystemIndex(),getModelCache(s).instanceCacheIndex)).get();
+ }
+ SBInstanceCache& updInstanceCache(const State& s) const { //mutable
+ return Value<SBInstanceCache>::downcast
+ (s.updCacheEntry(getMySubsystemIndex(),getModelCache(s).instanceCacheIndex)).upd();
+ }
+
+ const SBTimeCache& getTimeCache(const State& s) const {
+ return Value<SBTimeCache>::downcast
+ (s.getCacheEntry(getMySubsystemIndex(),getModelCache(s).timeCacheIndex)).get();
+ }
+ SBTimeCache& updTimeCache(const State& s) const { //mutable
+ return Value<SBTimeCache>::downcast
+ (s.updCacheEntry(getMySubsystemIndex(),getModelCache(s).timeCacheIndex)).upd();
+ }
+
+ const SBTreePositionCache& getTreePositionCache(const State& s) const {
+ return Value<SBTreePositionCache>::downcast
+ (s.getCacheEntry(getMySubsystemIndex(),getModelCache(s).treePositionCacheIndex)).get();
+ }
+ SBTreePositionCache& updTreePositionCache(const State& s) const { //mutable
+ return Value<SBTreePositionCache>::downcast
+ (s.updCacheEntry(getMySubsystemIndex(),getModelCache(s).treePositionCacheIndex)).upd();
+ }
+
+ const SBConstrainedPositionCache& getConstrainedPositionCache(const State& s) const {
+ return Value<SBConstrainedPositionCache>::downcast
+ (s.getCacheEntry(getMySubsystemIndex(),getModelCache(s).constrainedPositionCacheIndex)).get();
+ }
+ SBConstrainedPositionCache& updConstrainedPositionCache(const State& s) const { //mutable
+ return Value<SBConstrainedPositionCache>::downcast
+ (s.updCacheEntry(getMySubsystemIndex(),getModelCache(s).constrainedPositionCacheIndex)).upd();
+ }
+
+ const SBCompositeBodyInertiaCache& getCompositeBodyInertiaCache(const State& s) const {
+ return Value<SBCompositeBodyInertiaCache>::downcast
+ (getCacheEntry(s,getModelCache(s).compositeBodyInertiaCacheIndex));
+ }
+ SBCompositeBodyInertiaCache& updCompositeBodyInertiaCache(const State& s) const { //mutable
+ return Value<SBCompositeBodyInertiaCache>::updDowncast
+ (updCacheEntry(s,getModelCache(s).compositeBodyInertiaCacheIndex));
+ }
+
+ const SBArticulatedBodyInertiaCache& getArticulatedBodyInertiaCache(const State& s) const {
+ return Value<SBArticulatedBodyInertiaCache>::downcast
+ (getCacheEntry(s,getModelCache(s).articulatedBodyInertiaCacheIndex));
+ }
+ SBArticulatedBodyInertiaCache& updArticulatedBodyInertiaCache(const State& s) const { //mutable
+ return Value<SBArticulatedBodyInertiaCache>::updDowncast
+ (updCacheEntry(s,getModelCache(s).articulatedBodyInertiaCacheIndex));
+ }
+
+ const SBTreeVelocityCache& getTreeVelocityCache(const State& s) const {
+ return Value<SBTreeVelocityCache>::downcast
+ (s.getCacheEntry(getMySubsystemIndex(),getModelCache(s).treeVelocityCacheIndex)).get();
+ }
+ SBTreeVelocityCache& updTreeVelocityCache(const State& s) const { //mutable
+ return Value<SBTreeVelocityCache>::downcast
+ (s.updCacheEntry(getMySubsystemIndex(),getModelCache(s).treeVelocityCacheIndex)).upd();
+ }
+
+ const SBConstrainedVelocityCache& getConstrainedVelocityCache(const State& s) const {
+ return Value<SBConstrainedVelocityCache>::downcast
+ (s.getCacheEntry(getMySubsystemIndex(),getModelCache(s).constrainedVelocityCacheIndex)).get();
+ }
+ SBConstrainedVelocityCache& updConstrainedVelocityCache(const State& s) const { //mutable
+ return Value<SBConstrainedVelocityCache>::downcast
+ (s.updCacheEntry(getMySubsystemIndex(),getModelCache(s).constrainedVelocityCacheIndex)).upd();
+ }
+
+ const SBDynamicsCache& getDynamicsCache(const State& s, bool realizingDynamics=false) const {
+ const AbstractValue& cacheEntry =
+ realizingDynamics ? (const AbstractValue&)s.updCacheEntry(getMySubsystemIndex(),getModelCache(s).dynamicsCacheIndex)
+ : s.getCacheEntry(getMySubsystemIndex(),getModelCache(s).dynamicsCacheIndex);
+ return Value<SBDynamicsCache>::downcast(cacheEntry).get();
+ }
+ SBDynamicsCache& updDynamicsCache(const State& s) const { //mutable
+ return Value<SBDynamicsCache>::downcast
+ (s.updCacheEntry(getMySubsystemIndex(),getModelCache(s).dynamicsCacheIndex)).upd();
+ }
+
+ const SBTreeAccelerationCache& getTreeAccelerationCache(const State& s) const {
+ return Value<SBTreeAccelerationCache>::downcast
+ (s.getCacheEntry(getMySubsystemIndex(),getModelCache(s).treeAccelerationCacheIndex)).get();
+ }
+ SBTreeAccelerationCache& updTreeAccelerationCache(const State& s) const { //mutable
+ return Value<SBTreeAccelerationCache>::downcast
+ (s.updCacheEntry(getMySubsystemIndex(),getModelCache(s).treeAccelerationCacheIndex)).upd();
+ }
+
+ const SBConstrainedAccelerationCache& getConstrainedAccelerationCache(const State& s) const {
+ return Value<SBConstrainedAccelerationCache>::downcast
+ (s.getCacheEntry(getMySubsystemIndex(),getModelCache(s).constrainedAccelerationCacheIndex)).get();
+ }
+ SBConstrainedAccelerationCache& updConstrainedAccelerationCache(const State& s) const { //mutable
+ return Value<SBConstrainedAccelerationCache>::downcast
+ (s.updCacheEntry(getMySubsystemIndex(),getModelCache(s).constrainedAccelerationCacheIndex)).upd();
+ }
+
+
+ const SBModelVars& getModelVars(const State& s) const {
+ return Value<SBModelVars>::downcast
+ (s.getDiscreteVariable(getMySubsystemIndex(),topologyCache.modelingVarsIndex)).get();
+ }
+ SBModelVars& updModelVars(State& s) const {
+ return Value<SBModelVars>::downcast
+ (s.updDiscreteVariable(getMySubsystemIndex(),topologyCache.modelingVarsIndex)).upd();
+ }
+
+ const SBInstanceVars& getInstanceVars(const State& s) const {
+ return Value<SBInstanceVars>::downcast
+ (s.getDiscreteVariable(getMySubsystemIndex(),topologyCache.topoInstanceVarsIndex)).get();
+ }
+ SBInstanceVars& updInstanceVars(State& s) const {
+ return Value<SBInstanceVars>::downcast
+ (s.updDiscreteVariable(getMySubsystemIndex(),topologyCache.topoInstanceVarsIndex)).upd();
+ }
+
+ const SBTimeVars& getTimeVars(const State& s) const {
+ return Value<SBTimeVars>::downcast
+ (s.getDiscreteVariable(getMySubsystemIndex(),getModelCache(s).timeVarsIndex)).get();
+ }
+ SBTimeVars& updTimeVars(State& s) const {
+ return Value<SBTimeVars>::downcast
+ (s.updDiscreteVariable(getMySubsystemIndex(),getModelCache(s).timeVarsIndex)).upd();
+ }
+
+ const SBPositionVars& getPositionVars(const State& s) const {
+ return Value<SBPositionVars>::downcast
+ (s.getDiscreteVariable(getMySubsystemIndex(),getModelCache(s).qVarsIndex)).get();
+ }
+ SBPositionVars& updPositionVars(State& s) const {
+ return Value<SBPositionVars>::downcast
+ (s.updDiscreteVariable(getMySubsystemIndex(),getModelCache(s).qVarsIndex)).upd();
+ }
+
+ const SBVelocityVars& getVelocityVars(const State& s) const {
+ return Value<SBVelocityVars>::downcast
+ (s.getDiscreteVariable(getMySubsystemIndex(),getModelCache(s).uVarsIndex)).get();
+ }
+ SBVelocityVars& updVelocityVars(State& s) const {
+ return Value<SBVelocityVars>::downcast
+ (s.updDiscreteVariable(getMySubsystemIndex(),getModelCache(s).uVarsIndex)).upd();
+ }
+
+ const SBDynamicsVars& getDynamicsVars(const State& s) const {
+ return Value<SBDynamicsVars>::downcast
+ (s.getDiscreteVariable(getMySubsystemIndex(),getModelCache(s).dynamicsVarsIndex)).get();
+ }
+ SBDynamicsVars& updDynamicsVars(State& s) const {
+ return Value<SBDynamicsVars>::downcast
+ (s.updDiscreteVariable(getMySubsystemIndex(),getModelCache(s).dynamicsVarsIndex)).upd();
+ }
+
+ const SBAccelerationVars& getAccelerationVars(const State& s) const {
+ return Value<SBAccelerationVars>::downcast
+ (s.getDiscreteVariable(getMySubsystemIndex(),getModelCache(s).accelerationVarsIndex)).get();
+ }
+ SBAccelerationVars& updAccelerationVars(State& s) const {
+ return Value<SBAccelerationVars>::downcast
+ (s.updDiscreteVariable(getMySubsystemIndex(),getModelCache(s).accelerationVarsIndex)).upd();
+ }
+
+ const SimbodyMatterSubsystem& getMySimbodyMatterSubsystemHandle() const
+ { return SimbodyMatterSubsystem::downcast(getOwnerSubsystemHandle()); }
+ SimbodyMatterSubsystem& updMySimbodyMatterSubsystemHandle()
+ { return SimbodyMatterSubsystem::updDowncast(updOwnerSubsystemHandle()); }
+ bool getShowDefaultGeometry() const;
+ void setShowDefaultGeometry(bool show);
+
+private:
+ void calcTreeForwardDynamicsOperator(const State&,
+ const Vector& mobilityForces,
+ const Vector_<Vec3>& particleForces,
+ const Vector_<SpatialVec>& bodyForces,
+ const Vector* extraMobilityForces,
+ const Vector_<SpatialVec>* extraBodyForces,
+ SBTreeAccelerationCache& tac, // kinematics & prescribed forces into here
+ Vector& udot, // in/out (in for prescribed udot)
+ Vector& qdotdot,
+ Vector& udotErr) const;
+
+ void calcLoopForwardDynamicsOperator(const State&,
+ const Vector& mobilityForces,
+ const Vector_<Vec3>& particleForces,
+ const Vector_<SpatialVec>& bodyForces,
+ SBTreeAccelerationCache& tac, // kinematics & prescribed forces into here
+ SBConstrainedAccelerationCache& cac, // constraint forces go here
+ Vector& udot, // in/out (in for prescribed udot)
+ Vector& qdotdot,
+ Vector& multipliers,
+ Vector& udotErr) const;
+
+ // Given a set of forces, calculate accelerations ignoring
+ // constraints, and leave the results in the state cache.
+ // Must have already called realizeDynamics().
+ // We also allow some extra forces to be supplied, with the intent
+ // that these will be used to deal with internal forces generated
+ // by constraints; set the pointers to zero if you don't have any
+ // extras to pass in.
+ void realizeTreeForwardDynamics (const State& s,
+ const Vector& mobilityForces,
+ const Vector_<Vec3>& particleForces,
+ const Vector_<SpatialVec>& bodyForces,
+ const Vector* extraMobilityForces,
+ const Vector_<SpatialVec>* extraBodyForces) const;
+
+ // Given a set of forces, calculate acclerations resulting from
+ // those forces and enforcement of acceleration constraints, and update
+ // the state cache with the results.
+ void realizeLoopForwardDynamics(const State&,
+ const Vector& mobilityForces,
+ const Vector_<Vec3>& particleForces,
+ const Vector_<SpatialVec>& bodyForces) const;
+
+ // calc ~(Tp Pq Wq^-1)_r (nfq X mp)
+ void calcWeightedPqrTranspose(
+ const State& state,
+ const Vector& Tp, // 1/perr tols
+ const Vector& Wqinv, // 1/q weights
+ Matrix& Pqrt) const;
+
+ // calc ~(Tp P Wu^-1)
+ // (Tv V Wu^-1)_r (nfu X (mp+mv))
+ void calcWeightedPVrTranspose(
+ const State& s,
+ const Vector& Tpv, // 1/verr tols
+ const Vector& Wuinv, // 1/u weights
+ Matrix& PVrt) const;
+
+ const Array_<QIndex>& getFreeQIndex(const State& state) const;
+ const Array_<QIndex>& getPresQIndex(const State& state) const;
+ const Array_<QIndex>& getZeroQIndex(const State& state) const;
+
+ const Array_<UIndex>& getFreeUIndex(const State& state) const;
+ const Array_<UIndex>& getPresUIndex(const State& state) const;
+ const Array_<UIndex>& getZeroUIndex(const State& state) const;
+
+ const Array_<UIndex>& getFreeUDotIndex(const State& state) const;
+ const Array_<UIndex>& getKnownUDotIndex(const State& state) const;
+
+ // Output must already be sized for number of free q's nfq.
+ // Input must be size nq.
+ void packFreeQ(const State& s, const Vector& allQ,
+ Vector& packedFreeQ) const;
+
+ // For efficiency, you must provide an output array of the right size nq.
+ // This method *will not* touch the prescribed slots in the output so
+ // if you want them zero make sure you do it yourself.
+ void unpackFreeQ(const State& s, const Vector& packedFreeQ,
+ Vector& unpackedFreeQ) const;
+
+ // Given a q-like array with nq entries, write zeroes onto the entries
+ // corresponding to known (prescribed) q's. The result looks like
+ // a properly-zeroed unpackedFreeQ.
+ void zeroKnownQ(const State& s, Vector& qlike) const;
+
+ // Output must already be sized for number of free u's nfu.
+ // Input must be size nu.
+ void packFreeU(const State& s, const Vector& allU,
+ Vector& packedFreeU) const;
+
+ // For efficiency, you must provide an output array of the right size nu.
+ // This method *will not* touch the prescribed slots in the output so
+ // if you want them zero make sure you do it yourself.
+ void unpackFreeU(const State& s, const Vector& packedFreeU,
+ Vector& unpackedFreeU) const;
+
+ // Given a u-like array with nu entries, write zeroes onto the entries
+ // corresponding to known (prescribed) u's. The result looks like
+ // a properly-zeroed unpackedFreeU.
+ void zeroKnownU(const State& s, Vector& ulike) const;
+
+ friend std::ostream& operator<<(std::ostream&, const SimbodyMatterSubsystemRep&);
+ friend class SimTK::SimbodyMatterSubsystem;
+
+ struct RigidBodyNodeIndex {
+ RigidBodyNodeIndex(int l, int o) : level(l), offset(o) { }
+ int level, offset;
+ };
+
+ SimTK_DOWNCAST(SimbodyMatterSubsystemRep, Subsystem::Guts);
+
+private:
+ // TOPOLOGY "STATE VARIABLES"
+
+ void clearTopologyState(); // note that this requires non-const access
+
+ // The handles in this array are the owners of the MobilizedBodies after they
+ // are adopted. The MobilizedBodyIndex (converted to int) is the index of a
+ // MobilizedBody in this array.
+ Array_<MobilizedBody*,MobilizedBodyIndex> mobilizedBodies;
+
+ // Constraints are treated similarly.
+ Array_<Constraint*,ConstraintIndex> constraints;
+
+ // Our realizeTopology method calls this after all bodies & constraints have been added,
+ // to construct part of the topology cache below.
+ void endConstruction(State&);
+
+ // TOPOLOGY CACHE
+
+ // The data members here are filled in when realizeTopology() is called.
+ // The flag which remembers whether we have realized topology is in
+ // the Subsystem::Guts base class.
+ // Note that a cache is treated as mutable, so the methods that manipulate
+ // it are const.
+
+ void clearTopologyCache() const {
+ SimbodyMatterSubsystemRep& mthis = const_cast<SimbodyMatterSubsystemRep&>(*this);
+ mthis.clearTopologyCache(); // call the non-const version
+ }
+
+ // This method should clear out all the data members below.
+ void clearTopologyCache();
+
+ // Mobilized bodies and their rigid body nodes
+
+ // This holds pointers to nodes and serves to map (level,offset) to nodeNum.
+ Array_<RBNodePtrList> rbNodeLevels;
+ // Map nodeNum (a.k.a. MobilizedBodyIndex) to (level,offset).
+ Array_<RigidBodyNodeIndex,MobilizedBodyIndex> nodeNum2NodeMap;
+
+ // Constraints
+
+ // Here we sort the above constraints by branch (ancestor's base body), then by
+ // level within that branch. That is, each constraint is addressed
+ // by three indices [branch][levelOfAncestor][offset]
+ // where offset is an arbitrary unique integer assigned to all
+ // the constraints on the same branch with the same level of ancestor.
+ Array_< Array_< Array_<ConstraintIndex> > > branches;
+
+ // Partition the constraints into groups which are coupled by constraints at
+ // the indicated level. Only Constraints which generate holonomic constraint
+ // equations (mp>0) are coupled at the position level. Constraints with
+ // *either* holonomic or nonholonomic (mv>0) constraint equations can be
+ // coupled at the velocity level because the derivatives of the holonomic
+ // constraints are velocity constraints. And any Constraints can be coupled
+ // at the accleration level.
+ //
+ // Each ConstraintSet contains a Subtree comprising all the relevant
+ // mobilized bodies for that set. Note that this is *kinematic* coupling;
+ // for dynamics there is additional coupling brought in by the mass
+ // matrix in the computation of (G M^-1 G^T).
+
+ // sorted in nondecreasing order of ancestor MobilizedBodyIndex
+ Array_<CoupledConstraintSet> positionCoupledConstraints; // for P
+ Array_<CoupledConstraintSet> velocityCoupledConstraints; // for PV
+ Array_<CoupledConstraintSet> accelerationCoupledConstraints; // for G=PVA
+
+ // This further partitions the accelerationCoupledConstraints into
+ // larger groups comprised of accelerationCoupledConstraints whose ancestor
+ // bodies' inboard paths share a body other than ground. That is, we couple
+ // the constraints unless they involve completely disjoint grounded subtrees.
+ // These groups correspond to disjoint blocks in M (as
+ // well as G, of course), so we can decouple them for the (G M^-1 G^T)
+ // calculation.
+ // TODO: acceleration constraints should instead be dealt with recursively,
+ // based on *kinematic* coupling; where the kinematically coupled groups are
+ // used to modify the articulated body inertias.
+ Array_<CoupledConstraintSet> dynamicallyCoupledConstraints;
+
+
+ // TODO: these state indices and counters should be deferred to realizeModel()
+ // so we can have Model stage variables which change the number of state
+ // slots needed.
+
+ // Initialize to 0 at beginning of construction. These are for doling
+ // out Q & U state variables to the nodes.
+ UIndex nextUSlot;
+ USquaredIndex nextUSqSlot;
+ QIndex nextQSlot;
+
+ // These are similarly for doling out slots for *topological* constraint
+ // equations for position errors, velocity errors, and multipliers.
+ // Note that quaternion normalization constraints are *not* topological
+ // so aren't counted here (although we begin assigning them slots at
+ // the end of the qErr's used up here).
+ int nextQErrSlot;
+ int nextUErrSlot;
+ int nextMultSlot;
+
+ // Dole out slots for all Constrained Bodies which belong to Constraints
+ // whose Ancestor is not Ground (except for the Ancestor bodies themselves).
+ AncestorConstrainedBodyPoolIndex nextAncestorConstrainedBodyPoolSlot;
+
+ int DOFTotal; // summed over all nodes
+ int SqDOFTotal; // sum of squares of ndofs per node
+ int maxNQTotal; // sum of dofs with room for quaternions
+
+ SBTopologyCache topologyCache;
+ CacheEntryIndex topologyCacheIndex; // topologyCache is copied here in the State
+
+ // Specifies whether default decorative geometry should be shown.
+ bool showDefaultGeometry;
+};
+
+std::ostream& operator<<(std::ostream&, const SimbodyMatterSubsystemRep&);
+
+
+#endif // SimTK_SIMBODY_MATTER_SUBSYSTEM_REP_H_
diff --git a/Simbody/src/SimbodyMatterSubtree.cpp b/Simbody/src/SimbodyMatterSubtree.cpp
new file mode 100644
index 0000000..cca4211
--- /dev/null
+++ b/Simbody/src/SimbodyMatterSubtree.cpp
@@ -0,0 +1,1313 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ *
+ * Implementation of SimbodyMatterSubtree and SimbodyMatterSubtreeResults.
+ */
+
+#include "SimTKcommon.h"
+#include "simbody/internal/SimbodyMatterSubtree.h"
+
+#include "MobilizedBodyImpl.h"
+#include "SimbodyMatterSubsystemRep.h"
+class RigidBodyNode;
+
+#include <string>
+#include <iostream>
+using std::cout;
+using std::endl;
+
+namespace SimTK {
+
+ /////////////////
+ // SUBTREE REP //
+ /////////////////
+
+class SimbodyMatterSubtree::SubtreeRep {
+public:
+ SubtreeRep(const SimbodyMatterSubtree& handle, const SimbodyMatterSubsystem& sms)
+ : myHandle(handle), matter(&sms), stage(Stage::Empty)
+ {
+ }
+
+ // Here we don't know the matter subsystem yet.
+ explicit SubtreeRep(const SimbodyMatterSubtree& handle)
+ : myHandle(handle), matter(0), stage(Stage::Empty)
+ {
+ }
+
+ void setSimbodyMatterSubsystem(const SimbodyMatterSubsystem& sms) {
+ clear();
+ matter = &sms; // just a reference
+ }
+
+ const SimbodyMatterSubsystem& getSimbodyMatterSubsystem() const {
+ assert(matter != 0);
+ return *matter;
+ }
+
+ void invalidate(Stage g) {
+ if (stage >= g)
+ stage = g.prev();
+ }
+
+ // Note that this retains the current handle and matter subsystem (if any).
+ void clear() {
+ invalidate(Stage::Topology);
+ terminalBodies.clear();
+ ancestor = InvalidMobilizedBodyIndex;
+ }
+
+
+ void setTerminalBodies(const Array_<MobilizedBodyIndex>& bids) {
+ clear();
+ for (int i=0; i < (int)bids.size(); ++i)
+ addTerminalBody(bids[i]);
+ }
+
+ void addTerminalBody(MobilizedBodyIndex bid) {
+ assert(!isTerminalBody(bid)); // can only appear once
+ invalidate(Stage::Topology);
+ terminalBodies.push_back(bid);
+ }
+
+ MobilizedBodyIndex getAncestorMobilizedBodyIndex() const {
+ assert(stage >= Stage::Topology);
+ return ancestor;
+ }
+
+ MobilizedBodyIndex getSubtreeBodyMobilizedBodyIndex(SubtreeBodyIndex b) const {
+ assert(stage >= Stage::Topology);
+ return allBodies[b];
+ }
+
+ SubtreeBodyIndex getParentSubtreeBodyIndex(SubtreeBodyIndex b) const {
+ assert(b >= 1); // ancestor has no subtree parent
+ assert(stage >= Stage::Topology);
+ return parentSubtreeBodies[b];
+ }
+
+ // State must be realized to at least Stage::Model for this call to work.
+ // The supplied SubtreeResults object is allocated and properly initialized to
+ // be able to hold computation results from this Subtree.
+ void initializeSubtreeResults(const State&, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
+
+ // This can be used as a sanity check that initializeSubtreeResults() was already called
+ // in this Subtree to produce these SubtreeResults. It is by no means exhaustive but
+ // will catch egregious errors.
+ bool isCompatibleSubtreeResults(const SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
+
+ // POSITION STAGE
+
+ // State must be realized to at least Stage::Position for this to work. SubtreeResults
+ // must have already been initialized to work with this Subtree. SubtreeResults stage
+ // will be Stage::Position after this call. All body transforms will be the same as
+ // the corresponding ones in the state, except they will be measured from the ancestor
+ // frame instead of ground. Subtree q's will be identical to corresponding State q's.
+ void copyPositionsFromState(const State&, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
+
+ // State must be realized to Stage::Instance. subQ must be the right length for this
+ // Subtree, and SubtreeResults must have been properly initialized. SubtreeResults
+ // stage will be Stage::Position after this call.
+ void calcPositionsFromSubtreeQ(const State&, const Vector& subQ, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
+
+ // Calculates a perturbed position result starting with the subQ's and position results
+ // which must already be in SubtreeResults.
+ void perturbPositions(const State&, SubtreeQIndex subQIndex, Real perturbation, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
+
+
+ // VELOCITY STAGE
+
+ // State must be realized to at least Stage::Velocity for this to work. SubtreeResults
+ // must already be at Stage::Position. SubtreeResults stage
+ // will be Stage::Velocity after this call. All subtree body spatial velocities will be
+ // the same as in the State, except measured relative to A and expressed in A. Subtree u's
+ // will be identical to corresponding State u's.
+ void copyVelocitiesFromState(const State&, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
+
+ // State must be realized to Stage::Instance. subU must be the right length for this
+ // Subtree, and SubtreeResults must already be at Stage::Position. SubtreeResults
+ // stage will be Stage::Velocity after this call.
+ void calcVelocitiesFromSubtreeU(const State&, const Vector& subU, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
+
+ // State must be realized to Stage::Instance and SubtreeResults must already be at
+ // Stage::Position. SubtreeResults stage will be Stage::Velocity after this call, but
+ // all Subtree u's and body velocities will be zero.
+ void calcVelocitiesFromZeroU(const State&, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
+
+ // Calculates a perturbed velocity result starting with the subU's and velocity results
+ // which must already be in SubtreeResults.
+ void perturbVelocities(const State&, SubtreeUIndex subUIndex, Real perturbation, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
+
+
+ // ACCELERATION STAGE
+
+ // State must be realized to at least Stage::Acceleration for this to work. SubtreeResults
+ // must already be at Stage::Velocity. SubtreeResults stage
+ // will be Stage::Acceleration after this call. All subtree body spatial accelerations will be
+ // the same as in the State, except measured relative to A and expressed in A. Subtree udots
+ // will be identical to corresponding State udots.
+ void copyAccelerationsFromState(const State&, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
+
+ // State must be realized to Stage::Instance. subUDot must be the right length for this
+ // Subtree, and SubtreeResults must already be at Stage::Velocity. SubtreeResults
+ // stage will be Stage::Acceleration after this call.
+ void calcAccelerationsFromSubtreeUDot(const State&, const Vector& subUDot, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
+
+ // State must be realized to Stage::Instance and SubtreeResults must already be at
+ // Stage::Velocity. SubtreeResults stage will be Stage::Acceleration after this call.
+ // All Subtree udots's will be zero, body accelerations will have only their bias values
+ // (coriolis accelerations from nonzero u's).
+ void calcAccelerationsFromZeroUDot(const State&, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
+
+ // Calculates a perturbed velocity result starting with the subUDot's and acceleration results
+ // which must already be in SubtreeResults.
+ void perturbAccelerations(const State&, SubtreeUIndex subUDotIndex, Real perturbation, SimbodyMatterSubtreeResults::SubtreeResultsRep&) const;
+
+ MobilizedBodyIndex getParentMobilizedBodyIndex(MobilizedBodyIndex childIx) const {
+ return getSimbodyMatterSubsystem().getMobilizedBody(childIx)
+ .getParentMobilizedBody().getMobilizedBodyIndex();
+ }
+
+ int getLevel(MobilizedBodyIndex mbid) const {
+ return getSimbodyMatterSubsystem().getMobilizedBody(mbid).getLevelInMultibodyTree();
+ }
+
+ bool hasPathToAncestor(MobilizedBodyIndex bid) const {
+ assert(ancestor.isValid());
+ while (bid!=ancestor && bid!=GroundIndex)
+ bid = getParentMobilizedBodyIndex(bid);
+ return bid == ancestor; // works if ancestor is Ground also
+ }
+
+ bool isTerminalBody(MobilizedBodyIndex bid) const {
+ for (int i=0; i < (int)terminalBodies.size(); ++i)
+ if (bid == terminalBodies[i])
+ return true;
+ return false;
+ }
+
+
+ void realizeTopology() {
+ if (stage >= Stage::Topology)
+ return;
+ allBodies.clear(); parentSubtreeBodies.clear(); childSubtreeBodies.clear();
+
+ if (terminalBodies.empty()) {
+ stage = Stage::Topology; // this is the "empty subtree"
+ return;
+ }
+
+ ancestor = findAncestorBody();
+
+ // We'll collect all the Subtree bodies in a MobilizedBodyIndex->SubtreeBodyIndex
+ // map. We'll do this in two passes through the map -- the first to eliminate
+ // duplicates and put the bodies in MobilizedBodyIndex order, the second to assign
+ // SubtreeBodyIndexs which are just the resulting ordinals.
+ // Complexity of this first pass is O(N log N) where N is the number
+ // of unique bodies in the Subtree.
+ typedef std::map<MobilizedBodyIndex, SubtreeBodyIndex> MapType;
+ typedef MapType::value_type MapEntry;
+ MapType subtreeBodyIndexMap;
+ // Pre-load the map with the ancestor body and its subtree body id 0.
+ subtreeBodyIndexMap.insert(MapEntry(ancestor, SubtreeBodyIndex(0)));
+ for (int i=0; i < (int)terminalBodies.size(); ++i) {
+ // Run down this branch adding any new bodies we encounter
+ // until we hit one that is already in the map. If we get to
+ // Ground without hitting the ancestor (OK if Ground *is* the
+ // ancestor) then we have been given a bad terminal body which
+ // should have been caught earlier.
+ MobilizedBodyIndex mbid = terminalBodies[i];
+ // ".second" will be true if the entry was actually inserted; otherwise
+ // it was already there.
+ while (subtreeBodyIndexMap.insert(MapEntry(mbid,SubtreeBodyIndex())).second) {
+ assert(mbid != GroundIndex);
+ mbid = getParentMobilizedBodyIndex(mbid);
+ }
+ }
+
+ // Now assign the SubtreeBodyIndexs in order of MobilizedBodyIndex, and fill
+ // in allBodies which serves as the SubtreeBodyIndex->MobilizedBodyIndex map,
+ // and parentSubtreeBodies which maps a subtree body to its unique subtree
+ // parent, and childSubtreeBodies which goes from a subtree body to the
+ // list of all bodies for which it is the parent.
+ // This pass is also O(N log N) because we have to look up the parent
+ // mobilized body id in the map to get its assigned subtree body id.
+
+ allBodies.resize((unsigned)subtreeBodyIndexMap.size());
+ parentSubtreeBodies.resize((unsigned)subtreeBodyIndexMap.size());
+ childSubtreeBodies.resize((unsigned)subtreeBodyIndexMap.size());
+ allBodies[0] = ancestor;
+ parentSubtreeBodies[0] = InvalidSubtreeBodyIndex;
+
+ SubtreeBodyIndex nextFree(1); // ancestor was already assigned 0
+ MapType::iterator body = subtreeBodyIndexMap.begin();
+ assert(body->first == ancestor && body->second == 0);
+ ++body; // skip the ancestor
+ for (; body != subtreeBodyIndexMap.end(); ++body)
+ {
+ const MobilizedBodyIndex mbid = body->first;
+ const SubtreeBodyIndex sbid = nextFree++;
+ body->second = sbid;
+ allBodies[sbid] = mbid;
+
+ // Look up the parent, which *must* be (a) present, and (b)
+ // one of the earlier Subtree bodies.
+ const MobilizedBodyIndex pmbid = getParentMobilizedBodyIndex(mbid);
+ MapType::const_iterator parent = subtreeBodyIndexMap.find(pmbid);
+ assert(parent != subtreeBodyIndexMap.end());
+
+ const SubtreeBodyIndex psbid = parent->second;
+ assert(psbid < sbid && allBodies[psbid]==pmbid);
+
+ parentSubtreeBodies[sbid] = psbid;
+ childSubtreeBodies[psbid].push_back(sbid);
+ }
+
+ stage = Stage::Topology;
+ }
+
+ int getNumSubtreeBodies() const {
+ assert(stage >= Stage::Topology);
+ return (int)allBodies.size();
+ }
+
+ void realizeModel(const State& s) {
+ assert(getSimbodyMatterSubsystem().getStage(s) >= Stage::Model);
+ assert(stage >= Stage::Topology);
+ if (stage >= Stage::Model)
+ return;
+
+ stage = Stage::Model;
+ }
+
+ void realizePosition(const State& s, const Vector& subQ) {
+ assert(getSimbodyMatterSubsystem().getStage(s) >= Stage::Instance);
+ assert(stage >= Stage::Model);
+
+ stage = Stage::Position;
+ }
+
+ void realizeVelocity(const State& s, const Vector& subU) {
+ assert(getSimbodyMatterSubsystem().getStage(s) >= Stage::Position);
+ assert(stage >= Stage::Model);
+
+ stage = Stage::Velocity;
+ }
+
+ void realizeAcceleration(const State& s, const Vector& subUDot) {
+ assert(getSimbodyMatterSubsystem().getStage(s) >= Stage::Velocity);
+ assert(stage >= Stage::Model);
+
+ stage = Stage::Acceleration;
+ }
+
+ const SimbodyMatterSubtree& getMySubtreeOwnerHandle() const {return myHandle;}
+
+private:
+ friend class SimbodyMatterSubtree;
+ const SimbodyMatterSubtree& myHandle; // owner handle
+ const SimbodyMatterSubsystem* matter; // a reference to the full tree of which this is a subset
+
+ Stage stage; // initially invalid
+
+ // TOPOLOGY STATE VARIABLES
+
+ Array_<MobilizedBodyIndex> terminalBodies;
+
+ // TOPOLOGY CACHE VARIABLES
+
+ // Maps SubtreeBodyIndex to MobilizedBodyIndex
+ MobilizedBodyIndex ancestor;
+ Array_<MobilizedBodyIndex> allBodies; // ancestor body is 0; ids are in increasing order
+
+ // Maps each subtree body (by SubtreeBodyIndex) to its unique parent within the subtree
+ // the base body (SubtreeBodyIndex==SubtreeBaseBodyIndex==0) returns an InvalidSubtreeBodyIndex
+ // as its parent.
+ Array_<SubtreeBodyIndex> parentSubtreeBodies;
+
+ // Maps each subtree body to its children within the subtree. Note that a subtree terminal
+ // body may have children in the full matter subsystem, but which are not included in
+ // the subtree.
+ Array_< Array_<SubtreeBodyIndex> > childSubtreeBodies;
+
+private:
+
+ // This routine finds the terminal body closest to Ground in the
+ // MobilizedBody tree's graph, then moves down all the other branches
+ // to find a body in each branch at that same lowest level. That is,
+ // we "trim" all the branches to be the same height. Then we move
+ // all the branches in sync one level closer to ground until they
+ // all hit the same body. That's the outmost common ancestor.
+ MobilizedBodyIndex findAncestorBody() {
+ assert(terminalBodies.size());
+
+ // Copy the terminal bodies, which are the current branch tips.
+ Array_<MobilizedBodyIndex> tips(&terminalBodies[0], (&terminalBodies[0])+terminalBodies.size());
+
+ // Find the level of the lowest-level tip.
+ int minTip = 0;
+ int minLevel = getLevel(tips[minTip]);
+ for (int i=1; i < (int)tips.size(); ++i)
+ if (getLevel(tips[i]) < minLevel)
+ {minTip = i; minLevel = getLevel(tips[minTip]);}
+
+ // Trim all the other branches back to the lowest level.
+ for (int i=0; i < (int)tips.size(); ++i)
+ while (getLevel(tips[i]) > minLevel)
+ tips[i] = getParentMobilizedBodyIndex(tips[i]);
+
+ // All tips are at the same level now. March them in lockstep down
+ // to their common ancestor or Ground.
+ while (!allElementsMatch(tips))
+ pruneTipsOneLevel(tips);
+
+ return tips[0]; // all branches led here
+ }
+
+ static bool allElementsMatch(const Array_<MobilizedBodyIndex>& ids) {
+ for (int i=1; i < (int)ids.size(); ++i)
+ if (ids[i] != ids[0]) return false;
+ return true;
+ }
+
+ void pruneTipsOneLevel(Array_<MobilizedBodyIndex>& tips) const {
+ for (int i=0; i < (int)tips.size(); ++i) {
+ assert(tips[i] != GroundIndex); // can't happen: they should have matched!
+ tips[i] = getParentMobilizedBodyIndex(tips[i]);
+ }
+ }
+};
+
+ /////////////////////////
+ // SUBTREE RESULTS REP //
+ /////////////////////////
+
+class SimbodyMatterSubtreeResults::SubtreeResultsRep {
+public:
+ explicit SubtreeResultsRep(const SimbodyMatterSubtreeResults& handle)
+ : myHandle(&handle), stage(Stage::Empty)
+ {
+ clear();
+ }
+
+ void clear() {
+ qSubset.clear(); uSubset.clear();
+ qSeg.clear(); uSeg.clear();
+ subQ.clear(); subU.clear(); subUDot.clear();
+ bodyTransforms.clear(); perturbedBodyTransforms.clear();
+ bodyVelocities.clear(); perturbedBodyVelocities.clear();
+ bodyAccelerations.clear(); perturbedBodyAccelerations.clear();
+ perturbedQ = InvalidSubtreeQIndex;
+ perturbedU = perturbedUDot = InvalidSubtreeUIndex;
+ stage = Stage::Empty;
+ }
+
+ // Set or change the number of Subtree bodies to be accommodated here. A
+ // "change" is cheap if the number of bodies hasn't actually changed.
+ void reallocateBodies(int nb) {
+ assert(nb >= 1); // must be at least the ancestor body
+ qSeg.resize(nb); uSeg.resize(nb);
+ bodyTransforms.resize(nb); perturbedBodyTransforms.resize(nb);
+ bodyVelocities.resize(nb); perturbedBodyVelocities.resize(nb);
+ bodyAccelerations.resize(nb); perturbedBodyAccelerations.resize(nb);
+
+ // Set the unchanging results for the ancestor body, which is treated as Ground.
+ qSeg[0] = pair<SubtreeQIndex,int>(SubtreeQIndex(0), 0); // no q's
+ uSeg[0] = pair<SubtreeUIndex,int>(SubtreeUIndex(0), 0); // no u's
+
+ bodyTransforms[0] = perturbedBodyTransforms[0] = Transform();
+ bodyVelocities[0] = perturbedBodyVelocities[0] = SpatialVec(Vec3(0), Vec3(0));
+ bodyAccelerations[0] = perturbedBodyAccelerations[0] = SpatialVec(Vec3(0), Vec3(0));
+
+ // Clear q and u mapping information -- that can't be known until Stage::Model.
+ qSubset.clear();
+ uSubset.clear();
+
+ perturbedQ = InvalidSubtreeQIndex;
+ perturbedU = perturbedUDot = InvalidSubtreeUIndex;
+ stage = Stage::Topology;
+ }
+
+ // Assign the next available Subtree q and u slots to the indicated body, and
+ // remember them. We are given the MatterSubsystem q's and u's associated with
+ // the corresponding mobilized body so we can keep a mapping.
+ void addMobilities(SubtreeBodyIndex sb, QIndex qStart, int nq, UIndex uStart, int nu) {
+ assert(stage >= Stage::Topology);
+ assert(nq >= 0 && nu >= 0 && nq >= nu);
+ assert(1 <= sb && sb < getNumSubtreeBodies());
+ stage = Stage::Topology; // back up if necessary
+
+ qSeg[sb] = pair<SubtreeQIndex,int>(SubtreeQIndex(qSubset.size()), nq);
+ uSeg[sb] = pair<SubtreeUIndex,int>(SubtreeUIndex(uSubset.size()), nu);
+
+ for (int i=0; i<nq; ++i)
+ qSubset.push_back(QIndex(qStart+i));
+ for (int i=0; i<nu; ++i)
+ uSubset.push_back(UIndex(uStart+i));
+ }
+
+ void packStateQIntoSubtreeQ(const Vector& stateQ, Vector& subtreeQ) const {
+ assert(stage >= Stage::Model);
+ assert(stateQ.size() >= getNumSubtreeQs());
+
+ subtreeQ.resize(getNumSubtreeQs());
+ for (SubtreeQIndex i(0); i<getNumSubtreeQs(); ++i)
+ subtreeQ[i] = stateQ[qSubset[i]];
+ }
+
+ void packStateUIntoSubtreeU(const Vector& stateU, Vector& subtreeU) const {
+ assert(stage >= Stage::Model);
+ assert(stateU.size() >= getNumSubtreeUs());
+
+ subtreeU.resize(getNumSubtreeUs());
+ for (SubtreeUIndex i(0); i<getNumSubtreeUs(); ++i)
+ subtreeU[i] = stateU[uSubset[i]];
+ }
+
+
+ void unpackSubtreeQIntoStateQ(const Vector& subtreeQ, Vector& stateQ) const {
+ assert(stage >= Stage::Model);
+ assert(subtreeQ.size() == getNumSubtreeQs());
+
+ for (SubtreeQIndex i(0); i<getNumSubtreeQs(); ++i)
+ stateQ[qSubset[i]] = subtreeQ[i];
+ }
+
+ // Call this when done adding mobilities.
+ void realizeModel(const Vector& allQ, const Vector& allU) {
+ stage = Stage::Model; // enable routines used below
+ assert(allQ.size() >= getNumSubtreeQs() && allU.size() >= getNumSubtreeUs());
+
+ subQ.resize(getNumSubtreeQs());
+ subU.resize(getNumSubtreeUs());
+ subUDot.resize(getNumSubtreeUs());
+
+ packStateQIntoSubtreeQ(allQ,subQ); // set initial values
+ packStateUIntoSubtreeU(allU,subU);
+
+ subUDot = NaN;
+ }
+
+ QIndex mapSubtreeQToSubsystemQ(SubtreeQIndex sq) const {
+ assert(stage >= Stage::Model);
+ assert(0 <= sq && sq < (int)qSubset.size());
+ return qSubset[sq]; // range checked indexing
+ }
+ UIndex mapSubtreeUToSubsystemU(SubtreeUIndex su) const {
+ assert(stage >= Stage::Model);
+ assert(0 <= su && su < (int)uSubset.size());
+ return uSubset[su];
+ }
+
+ int getNumSubtreeBodies() const {
+ assert(stage >= Stage::Topology);
+ return (int)bodyTransforms.size();
+ }
+
+ int getNumSubtreeQs() const {
+ assert(stage >= Stage::Model);
+ return (int)qSubset.size();
+ }
+
+ int getNumSubtreeUs() const {
+ assert(stage >= Stage::Model);
+ return (int)uSubset.size();
+ }
+
+ void setMySubtreeResultsOwnerHandle(const SimbodyMatterSubtreeResults& owner) {
+ myHandle = &owner;
+ }
+
+ const SimbodyMatterSubtreeResults& getMySubtreeResultsOwnerHandle() const {
+ assert(myHandle);
+ return *myHandle;
+ }
+
+ Stage getStage() const {return stage;}
+ void setStage(Stage g) {stage=g;}
+
+ const Vector& getSubQ() const {
+ assert(stage >= Stage::Position);
+ return subQ;
+ }
+ Vector& updSubQ() {
+ assert(stage >= Stage::Model);
+ if (stage >= Stage::Position) stage=Stage::Model;
+ return subQ;
+ }
+ const Vector& getSubU() const {
+ assert(stage >= Stage::Velocity);
+ return subU;
+ }
+ Vector& updSubU() {
+ assert(stage >= Stage::Model);
+ if (stage >= Stage::Velocity) stage=Stage::Position;
+ return subU;
+ }
+ const Vector& getSubUDot() const {
+ assert(stage >= Stage::Acceleration);
+ return subUDot;
+ }
+ Vector& updSubUDot() {
+ assert(stage >= Stage::Model);
+ if (stage >= Stage::Acceleration) stage=Stage::Velocity;
+ return subUDot;
+ }
+
+ const Transform& getSubtreeBodyTransform(SubtreeBodyIndex sb) { // X_AB
+ assert(stage >= Stage::Position);
+ assert(0 <= sb && sb < (int)bodyTransforms.size());
+ return bodyTransforms[sb];
+ }
+
+ const SpatialVec& getSubtreeBodyVelocity(SubtreeBodyIndex sb) {
+ assert(stage >= Stage::Velocity);
+ assert(0 <= sb && sb < (int)bodyVelocities.size());
+ return bodyVelocities[sb];
+ }
+
+ const SpatialVec& getSubtreeBodyAcceleration(SubtreeBodyIndex sb) {
+ assert(stage >= Stage::Acceleration);
+ assert(0 <= sb && sb < (int)bodyAccelerations.size());
+ return bodyAccelerations[sb];
+ }
+
+ void setSubtreeBodyTransform(SubtreeBodyIndex sb, const Transform& X_AB) {
+ assert(stage >= Stage::Model);
+ assert(1 <= sb && sb < getNumSubtreeBodies()); // can't set Ancestor transform
+ bodyTransforms[sb] = X_AB;
+ }
+
+ void setSubtreeBodyVelocity(SubtreeBodyIndex sb, const SpatialVec& V_AB) {
+ assert(stage >= Stage::Position);
+ assert(1 <= sb && sb < getNumSubtreeBodies()); // can't set Ancestor velocity
+ bodyVelocities[sb] = V_AB;
+ }
+
+ void setSubtreeBodyAcceleration(SubtreeBodyIndex sb, const SpatialVec& A_AB) {
+ assert(stage >= Stage::Velocity);
+ assert(1 <= sb && sb < getNumSubtreeBodies()); // can't set Ancestor velocity
+ bodyAccelerations[sb] = A_AB;
+ }
+
+ void findSubtreeBodyQ(SubtreeBodyIndex sb, SubtreeQIndex& q, int& nq) const {
+ assert(stage >= Stage::Model);
+ q = qSeg[sb].first;
+ nq = qSeg[sb].second;
+ }
+
+ void findSubtreeBodyU(SubtreeBodyIndex sb, SubtreeUIndex& u, int& nu) const {
+ assert(stage >= Stage::Model);
+ u = uSeg[sb].first;
+ nu = uSeg[sb].second;
+ }
+private:
+ friend class SimbodyMatterSubtreeResults;
+ const SimbodyMatterSubtreeResults* myHandle; // owner handle
+
+ Stage stage; // initially invalid
+
+ // Model stage information
+ Array_< QIndex > qSubset; // map from SubtreeQIndex to MatterSubsystem q
+ Array_< UIndex > uSubset; // map from SubtreeUIndex to MatterSubsystem u (also udot)
+
+ // These identify which mobilities go with which bodies.
+ Array_< pair<SubtreeQIndex,int> > qSeg; // map from SubtreeBodyIndex to qSubset offset, length
+ Array_< pair<SubtreeUIndex,int> > uSeg; // map from SubtreeBodyIndex to uSubset offset, length
+
+ //TODO: make PIMPL
+ Vector subQ; // generalized coords for Subtree bodies
+ Array_<Transform> bodyTransforms; // X_AB, index by SubtreeBodyIndex (unperturbed)
+
+ SubtreeQIndex perturbedQ; // which Q was perturbed? InvalidSubtreeQIndex if none
+ Array_<Transform> perturbedBodyTransforms; // X_AB, after perturbation
+
+ Vector subU; // generalized speeds for Subtree bodies
+ Vector_<SpatialVec> bodyVelocities; // V_AB, index by SubtreeBodyIndex
+
+ SubtreeUIndex perturbedU; // which u was perturbed? InvalidSubtreeUIndex if none
+ Vector_<SpatialVec> perturbedBodyVelocities; // V_AB, after perturbation
+
+ Vector subUDot; // generalized accelerations for Subtree bodies
+ Vector_<SpatialVec> bodyAccelerations; // A_AB, index by SubtreeBodyIndex
+
+ SubtreeUIndex perturbedUDot; // which udot was perturbed? InvalidSubtreeUIndex if none
+ Vector_<SpatialVec> perturbedBodyAccelerations; // A_AB, after perturbation
+};
+
+
+ ////////////////////////////
+ // SIMBODY MATTER SUBTREE //
+ ////////////////////////////
+
+// Default constructor -- we don't know the SimbodyMatterSubsystem yet.
+SimbodyMatterSubtree::SimbodyMatterSubtree()
+ : rep(0)
+{
+ rep = new SubtreeRep(*this);
+}
+
+SimbodyMatterSubtree::SimbodyMatterSubtree(const SimbodyMatterSubsystem& matter)
+ : rep(0)
+{
+ rep = new SubtreeRep(*this, matter);
+}
+
+
+SimbodyMatterSubtree::SimbodyMatterSubtree(const SimbodyMatterSubsystem& matter,
+ const Array_<MobilizedBodyIndex>& terminalBodies)
+ : rep(0)
+{
+ rep = new SubtreeRep(*this, matter);
+ rep->setTerminalBodies(terminalBodies);
+}
+
+// Copy constructor
+SimbodyMatterSubtree::SimbodyMatterSubtree(const SimbodyMatterSubtree& src)
+ : rep(0)
+{
+ if (src.rep)
+ rep = new SubtreeRep(*src.rep);
+}
+
+// Copy assignment
+SimbodyMatterSubtree&
+SimbodyMatterSubtree::operator=(const SimbodyMatterSubtree& src)
+{
+ if (&src != this) {
+ if (rep && (this == &rep->getMySubtreeOwnerHandle()))
+ delete rep;
+ rep = 0;
+ if (src.rep)
+ rep = new SubtreeRep(*src.rep);
+ }
+ return *this;
+}
+
+// Destructor
+SimbodyMatterSubtree::~SimbodyMatterSubtree() {
+ if (rep && (this == &rep->getMySubtreeOwnerHandle()))
+ delete rep;
+ rep=0;
+}
+
+const SimbodyMatterSubsystem&
+SimbodyMatterSubtree::getSimbodyMatterSubsystem() const {
+ return getRep().getSimbodyMatterSubsystem();
+}
+
+
+void SimbodyMatterSubtree::setSimbodyMatterSubsystem(const SimbodyMatterSubsystem& matter) {
+ return updRep().setSimbodyMatterSubsystem(matter);
+}
+
+void SimbodyMatterSubtree::clear() {
+ return updRep().clear();
+}
+
+
+SimbodyMatterSubtree&
+SimbodyMatterSubtree::addTerminalBody(MobilizedBodyIndex i) {
+ updRep().addTerminalBody(i);
+ return *this;
+}
+
+void SimbodyMatterSubtree::realizeTopology() {
+ updRep().realizeTopology();
+}
+
+MobilizedBodyIndex SimbodyMatterSubtree::getAncestorMobilizedBodyIndex() const {
+ return getRep().ancestor;
+}
+
+const Array_<MobilizedBodyIndex>&
+SimbodyMatterSubtree::getTerminalBodies() const {
+ return getRep().terminalBodies;
+}
+
+int SimbodyMatterSubtree::getNumSubtreeBodies() const {
+ return (int)getRep().allBodies.size();
+}
+
+const Array_<MobilizedBodyIndex>&
+SimbodyMatterSubtree::getAllBodies() const {
+ assert(getRep().stage >= Stage::Topology);
+ return getRep().allBodies;
+}
+
+SubtreeBodyIndex
+SimbodyMatterSubtree::getParentSubtreeBodyIndex(SubtreeBodyIndex sbid) const {
+ assert(getRep().stage >= Stage::Topology);
+ return getRep().parentSubtreeBodies[sbid];
+}
+const Array_<SubtreeBodyIndex>&
+SimbodyMatterSubtree::getChildSubtreeBodyIndices(SubtreeBodyIndex sbid) const {
+ assert(getRep().stage >= Stage::Topology);
+ return getRep().childSubtreeBodies[sbid];
+}
+
+bool SimbodyMatterSubtree::
+isCompatibleSubtreeResults(const SimbodyMatterSubtreeResults& sr) const {
+ return getRep().isCompatibleSubtreeResults(sr.getRep());
+}
+
+void SimbodyMatterSubtree::initializeSubtreeResults(const State& s, SimbodyMatterSubtreeResults& sr) const {
+ getRep().initializeSubtreeResults(s,sr.updRep());
+}
+
+
+void SimbodyMatterSubtree::
+copyPositionsFromState(const State& s, SimbodyMatterSubtreeResults& sr) const {
+ getRep().copyPositionsFromState(s,sr.updRep());
+}
+
+void SimbodyMatterSubtree::
+calcPositionsFromSubtreeQ(const State& s, const Vector& subQ, SimbodyMatterSubtreeResults& sr) const {
+ getRep().calcPositionsFromSubtreeQ(s,subQ,sr.updRep());
+}
+
+void SimbodyMatterSubtree::
+perturbPositions(const State& s, SubtreeQIndex subQIndex, Real perturbation, SimbodyMatterSubtreeResults& sr) const {
+ getRep().perturbPositions(s,subQIndex,perturbation,sr.updRep());
+}
+
+void SimbodyMatterSubtree::
+copyVelocitiesFromState(const State& s, SimbodyMatterSubtreeResults& sr) const {
+ getRep().copyVelocitiesFromState(s,sr.updRep());
+}
+
+void SimbodyMatterSubtree::
+calcVelocitiesFromSubtreeU(const State& s, const Vector& subU, SimbodyMatterSubtreeResults& sr) const {
+ getRep().calcVelocitiesFromSubtreeU(s,subU,sr.updRep());
+}
+
+void SimbodyMatterSubtree::
+calcVelocitiesFromZeroU(const State& s, SimbodyMatterSubtreeResults& sr) const {
+ getRep().calcVelocitiesFromZeroU(s,sr.updRep());
+}
+
+void SimbodyMatterSubtree::
+perturbVelocities(const State& s, SubtreeUIndex subUIndex, Real perturbation, SimbodyMatterSubtreeResults& sr) const {
+ getRep().perturbVelocities(s,subUIndex,perturbation,sr.updRep());
+}
+
+void SimbodyMatterSubtree::
+copyAccelerationsFromState(const State& s, SimbodyMatterSubtreeResults& sr) const {
+ getRep().copyAccelerationsFromState(s,sr.updRep());
+}
+
+void SimbodyMatterSubtree::
+calcAccelerationsFromSubtreeUDot(const State& s, const Vector& subUDot, SimbodyMatterSubtreeResults& sr) const {
+ getRep().calcAccelerationsFromSubtreeUDot(s,subUDot,sr.updRep());
+}
+
+void SimbodyMatterSubtree::
+calcAccelerationsFromZeroUDot(const State& s, SimbodyMatterSubtreeResults& sr) const {
+ getRep().calcAccelerationsFromZeroUDot(s,sr.updRep());
+}
+
+void SimbodyMatterSubtree::
+perturbAccelerations(const State& s, SubtreeUIndex subUDotIndex, Real perturbation, SimbodyMatterSubtreeResults& sr) const {
+ getRep().perturbAccelerations(s,subUDotIndex,perturbation,sr.updRep());
+}
+
+
+
+ ////////////////////////////////////
+ // SIMBODY MATTER SUBTREE RESULTS //
+ ////////////////////////////////////
+
+SimbodyMatterSubtreeResults::SimbodyMatterSubtreeResults() : rep(0) {
+ rep = new SubtreeResultsRep(*this);
+}
+
+SimbodyMatterSubtreeResults::~SimbodyMatterSubtreeResults() {
+ if (rep && this == rep->myHandle)
+ delete rep;
+ rep = 0;
+}
+
+SimbodyMatterSubtreeResults::SimbodyMatterSubtreeResults(const SimbodyMatterSubtreeResults& src) : rep(0) {
+ if (src.rep) {
+ rep = new SubtreeResultsRep(*src.rep);
+ rep->setMySubtreeResultsOwnerHandle(*this);
+ }
+}
+
+SimbodyMatterSubtreeResults&
+SimbodyMatterSubtreeResults::operator=(const SimbodyMatterSubtreeResults& src) {
+ if (&src != this) {
+ if (rep && this == rep->myHandle)
+ delete rep;
+ rep = 0;
+ if (src.rep) {
+ rep = new SubtreeResultsRep(*src.rep);
+ rep->setMySubtreeResultsOwnerHandle(*this);
+ }
+ }
+ return *this;
+}
+
+int SimbodyMatterSubtreeResults::getNumSubtreeBodies() const {
+ return getRep().getNumSubtreeBodies();
+}
+
+int SimbodyMatterSubtreeResults::getNumSubtreeQs() const {
+ return getRep().getNumSubtreeQs();
+}
+
+int SimbodyMatterSubtreeResults::getNumSubtreeUs() const {
+ return getRep().getNumSubtreeUs();
+}
+
+void SimbodyMatterSubtreeResults::reallocateBodies(int nb) {
+ updRep().reallocateBodies(nb);
+}
+
+void SimbodyMatterSubtreeResults::addMobilities
+ (SubtreeBodyIndex sb, QIndex qStart, int nq, UIndex uStart, int nu)
+{
+ updRep().addMobilities(sb, qStart, nq, uStart, nu);
+}
+
+void SimbodyMatterSubtreeResults::realizeModel(const Vector& stateQ, const Vector& stateU) {
+ updRep().realizeModel(stateQ, stateU);
+}
+
+Stage SimbodyMatterSubtreeResults::getStage() const {
+ return getRep().stage;
+}
+
+const Array_<QIndex>& SimbodyMatterSubtreeResults::getQSubset() const {
+ assert(getRep().stage >= Stage::Model);
+ return getRep().qSubset;
+}
+
+const Array_<UIndex>& SimbodyMatterSubtreeResults::getUSubset() const {
+ assert(getRep().stage >= Stage::Model);
+ return getRep().uSubset;
+}
+
+void SimbodyMatterSubtreeResults::findSubtreeBodyQ(SubtreeBodyIndex sbid, SubtreeQIndex& qStart, int& nq) const {
+ assert(getStage() >= Stage::Model);
+ const pair<SubtreeQIndex,int>& seg = getRep().qSeg[sbid];
+ qStart = seg.first;
+ nq = seg.second;
+}
+
+void SimbodyMatterSubtreeResults::findSubtreeBodyU(SubtreeBodyIndex sbid, SubtreeUIndex& uStart, int& nu) const {
+ assert(getStage() >= Stage::Model);
+ const pair<SubtreeUIndex,int>& seg = getRep().uSeg[sbid];
+ uStart = seg.first;
+ nu = seg.second;
+}
+
+const Vector& SimbodyMatterSubtreeResults::getSubtreeQ() const {
+ assert(getStage() >= Stage::Position);
+ return getRep().subQ;
+}
+const Transform& SimbodyMatterSubtreeResults::getSubtreeBodyTransform(SubtreeBodyIndex sbid) const {
+ assert(getStage() >= Stage::Position);
+ return getRep().bodyTransforms[sbid];
+}
+
+const Vector& SimbodyMatterSubtreeResults::getSubtreeU() const {
+ assert(getStage() >= Stage::Velocity);
+ return getRep().subU;
+}
+const SpatialVec& SimbodyMatterSubtreeResults::getSubtreeBodyVelocity(SubtreeBodyIndex sbid) const {
+ assert(getStage() >= Stage::Velocity);
+ return getRep().bodyVelocities[sbid];
+}
+
+const Vector& SimbodyMatterSubtreeResults::getSubtreeUDot() const {
+ assert(getStage() >= Stage::Acceleration);
+ return getRep().subUDot;
+}
+const SpatialVec& SimbodyMatterSubtreeResults::getSubtreeBodyAcceleration(SubtreeBodyIndex sbid) const {
+ assert(getStage() >= Stage::Acceleration);
+ return getRep().bodyAccelerations[sbid];
+}
+
+std::ostream& operator<<(std::ostream& o, const SimbodyMatterSubtree& sub) {
+ o << "SUBTREE:" << endl;
+ o << " ancestor=" << sub.getAncestorMobilizedBodyIndex();
+
+ o << " terminalBodies=";
+ for (int i=0; i < (int)sub.getTerminalBodies().size(); ++i)
+ o << sub.getTerminalBodies()[i] << " ";
+ o << endl;
+
+ o << " allBodies=";
+ for (int i=0; i < (int)sub.getAllBodies().size(); ++i)
+ o << sub.getAllBodies()[i] << " ";
+ o << endl;
+
+ for (SubtreeBodyIndex b(0); b < (int)sub.getAllBodies().size(); ++b) {
+ o << " parent[" << b << "]=" << sub.getParentSubtreeBodyIndex(b);
+
+ o << " children[" << b << "]=";
+ for (int i=0; i < (int)sub.getChildSubtreeBodyIndices(b).size(); ++i)
+ o << sub.getChildSubtreeBodyIndices(b)[i] << " ";
+ o << endl;
+ }
+
+ return o;
+}
+
+static std::ostream& operator<<(std::ostream& o, const Array_<QIndex>& q) {
+ for (int i=0; i<(int)q.size(); ++i)
+ o << q[i] << " ";
+ return o;
+}
+
+static std::ostream& operator<<(std::ostream& o, const Array_<UIndex>& u) {
+ for (int i=0; i<(int)u.size(); ++i)
+ o << u[i] << " ";
+ return o;
+}
+
+std::ostream& operator<<(std::ostream& o, const SimbodyMatterSubtreeResults& sr) {
+ o << "SUBTREE RESULTS (stage=" << sr.getStage() << "):" << endl;
+
+ if (sr.getStage() >= Stage::Topology)
+ o << " " << sr.getNumSubtreeBodies() << " subtree bodies" << endl;
+
+ if (sr.getStage() >= Stage::Model) {
+ o << " nq=" << sr.getNumSubtreeQs() << ", nu=" << sr.getNumSubtreeUs() << endl;
+ o << " QSubset: " << sr.getQSubset() << endl;
+ o << " USubset: " << sr.getUSubset() << endl;
+
+ for (SubtreeBodyIndex sb(1); sb < sr.getNumSubtreeBodies(); ++sb) {
+ SubtreeQIndex qstart; int nq;
+ SubtreeUIndex ustart; int nu;
+ sr.findSubtreeBodyQ(sb,qstart,nq);
+ sr.findSubtreeBodyU(sb,ustart,nu);
+ o << " body " << sb << " q=" << qstart << ".." << qstart+nq-1
+ << " u=" << ustart << ".." << ustart+nu-1 << endl;
+ }
+ }
+
+ if (sr.getStage() >= Stage::Position) {
+ o << " POSITION RESULTS AVAILABLE:\n";
+ o << " q=" << sr.getSubtreeQ() << endl;
+ for (SubtreeBodyIndex sb(0); sb < sr.getNumSubtreeBodies(); ++sb)
+ o << " X_AB" << sb << "=" << sr.getSubtreeBodyTransform(sb);
+ }
+
+ if (sr.getStage() >= Stage::Velocity) {
+ o << " VELOCITY RESULTS AVAILABLE\n";
+ o << " u=" << sr.getSubtreeU() << endl;
+ for (SubtreeBodyIndex sb(0); sb < sr.getNumSubtreeBodies(); ++sb)
+ o << " V_AB" << sb << "=" << sr.getSubtreeBodyVelocity(sb) << endl;
+ }
+
+ if (sr.getStage() >= Stage::Acceleration) {
+ o << " ACCELERATION RESULTS AVAILABLE\n";
+ o << " udot=" << sr.getSubtreeUDot() << endl;
+ for (SubtreeBodyIndex sb(0); sb < sr.getNumSubtreeBodies(); ++sb)
+ o << " A_AB" << sb << "=" << sr.getSubtreeBodyAcceleration(sb) << endl;
+ }
+
+ o << "END SUBTREE RESULTS." << endl;
+
+ return o;
+}
+
+
+////////////////////////////////////////////////////////
+// SIMBODY MATTER SUBSYSTEM :: SUBTREE :: SUBTREE REP //
+////////////////////////////////////////////////////////
+
+void SimbodyMatterSubtree::SubtreeRep::
+initializeSubtreeResults(const State& s, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
+ const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
+ SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Model,
+ "Subtree::initializeSubtreeResults()");
+
+ const int nSubtreeBodies = getNumSubtreeBodies();
+ sr.reallocateBodies(nSubtreeBodies);
+
+ int nSubtreeQ=0, nSubtreeU=0;
+ // start at 1 because ancestor has no relevant mobilities
+ for (SubtreeBodyIndex sb(1); sb < nSubtreeBodies; ++sb) {
+ const MobilizedBodyIndex mb = allBodies[sb];
+
+ QIndex qStart; int nq;
+ UIndex uStart; int nu;
+ matter.findMobilizerQs(s, mb, qStart, nq);
+ matter.findMobilizerUs(s, mb, uStart, nu);
+ nSubtreeQ += nq; nSubtreeU += nu;
+
+ sr.addMobilities(sb, qStart, nq, uStart, nu);
+ }
+
+ sr.realizeModel(matter.getQ(s), matter.getU(s));
+ assert(nSubtreeQ == sr.getNumSubtreeQs() && nSubtreeU == sr.getNumSubtreeUs());
+}
+
+bool SimbodyMatterSubtree::SubtreeRep::
+isCompatibleSubtreeResults(const SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
+ // hardly exhaustive but better than nothing
+ return sr.getStage() >= Stage::Model
+ && sr.getNumSubtreeBodies() == getNumSubtreeBodies();
+}
+
+void SimbodyMatterSubtree::SubtreeRep::
+copyPositionsFromState(const State& s, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
+ const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
+ SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Position,
+ "Subtree::calcPositionsFromState()");
+
+ assert(isCompatibleSubtreeResults(sr));
+
+ // Copy the q's; adjust the body transforms to be relative to the ancestor
+ // body instead of ground.
+ sr.packStateQIntoSubtreeQ(matter.getQ(s), sr.updSubQ());
+
+ if (getAncestorMobilizedBodyIndex() == GroundIndex) {
+ for (SubtreeBodyIndex sb(1); sb < getNumSubtreeBodies(); ++sb) {
+ const MobilizedBodyIndex mb = getSubtreeBodyMobilizedBodyIndex(sb);
+ const Transform& X_GB = matter.getBodyTransform(s,mb);
+ sr.setSubtreeBodyTransform(sb, X_GB); // =X_AB
+ }
+ } else {
+ // Ancestor A differs from Ground G so we have to adjust all the
+ // Subtree body transforms to measure from A instead of G.
+ const Transform& X_GA = matter.getBodyTransform(s,getAncestorMobilizedBodyIndex());
+ for (SubtreeBodyIndex sb(1); sb < getNumSubtreeBodies(); ++sb) {
+ const MobilizedBodyIndex mb = getSubtreeBodyMobilizedBodyIndex(sb);
+ const Transform& X_GB = matter.getBodyTransform(s,mb);
+ sr.setSubtreeBodyTransform(sb, ~X_GA * X_GB); // X_AB
+ }
+ }
+
+ sr.setStage(Stage::Position);
+}
+
+// Here we are given a new set of Subtree q's, and we want to calculate the resulting A-relative
+// transforms for all the Subtree bodies. This requires calculating the cross-mobilizer transforms
+// X_FM for each Subtree mobilizer and propagating them outwards towards the terminal bodies.
+void SimbodyMatterSubtree::SubtreeRep::
+calcPositionsFromSubtreeQ(const State& state, const Vector& subQ,
+ SimbodyMatterSubtreeResults::SubtreeResultsRep& results) const
+{
+ const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
+ SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(state), Stage::Instance,
+ "calcPositionsFromSubtreeQ()");
+
+ assert(isCompatibleSubtreeResults(results));
+ assert(subQ.size() == results.getNumSubtreeQs());
+
+ results.updSubQ() = subQ; // copy in the q's
+
+ // For high speed, find memory address for the first subQ; they are sequential after this.
+ const Real* allSubQ = &results.getSubQ()[0];
+
+ // Iterate from the ancestor outward to propagate the transforms to the terminal bodies.
+ for (SubtreeBodyIndex sb(1); sb < getNumSubtreeBodies(); ++sb) {
+ const SubtreeBodyIndex sp = getParentSubtreeBodyIndex(sb);
+ const MobilizedBodyIndex mb = getSubtreeBodyMobilizedBodyIndex(sb);
+
+ SubtreeQIndex firstSubQ; int nq;
+ results.findSubtreeBodyQ(sb, firstSubQ, nq);
+
+ const Transform X_PB = matter.calcMobilizerTransformFromQ(state, mb, nq, &allSubQ[firstSubQ]);
+ const Transform& X_AP = results.getSubtreeBodyTransform(sp);
+ results.setSubtreeBodyTransform(sb, X_AP*X_PB);
+ }
+
+ results.setStage(Stage::Position);
+}
+
+void SimbodyMatterSubtree::SubtreeRep::
+perturbPositions(const State& s, SubtreeQIndex subQIndex, Real perturbation, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
+ const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
+ SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Instance,
+ "perturbPositions()");
+
+ assert(isCompatibleSubtreeResults(sr));
+ assert(sr.getStage() >= Stage::Position);
+
+ assert(!"not implemented yet");
+}
+
+void SimbodyMatterSubtree::SubtreeRep::
+copyVelocitiesFromState(const State& s, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
+ const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
+ SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Velocity,
+ "calcVelocitiesFromState()");
+
+ assert(isCompatibleSubtreeResults(sr));
+
+ // Copy the u's; adjust the body velocities to be measured relative to,
+ // and expressed in, the ancestor body instead of ground.
+ sr.packStateUIntoSubtreeU(matter.getU(s), sr.updSubU());
+
+ if (getAncestorMobilizedBodyIndex() == GroundIndex) {
+ for (SubtreeBodyIndex sb(1); sb < getNumSubtreeBodies(); ++sb) {
+ const MobilizedBodyIndex mb = getSubtreeBodyMobilizedBodyIndex(sb);
+ const SpatialVec& V_GB = matter.getBodyVelocity(s,mb);
+ sr.setSubtreeBodyVelocity(sb, V_GB); // =V_AB
+ }
+ } else {
+ // Ancestor A differs from Ground G so we have to adjust all the
+ // Subtree body velocities to measure from A instead of G.
+ const Transform& X_GA = matter.getBodyTransform(s,getAncestorMobilizedBodyIndex());
+ const SpatialVec& V_GA = matter.getBodyVelocity(s,getAncestorMobilizedBodyIndex());
+ for (SubtreeBodyIndex sb(1); sb < getNumSubtreeBodies(); ++sb) {
+ const MobilizedBodyIndex mb = getSubtreeBodyMobilizedBodyIndex(sb);
+ const Transform& X_GB = matter.getBodyTransform(s,mb);
+ const SpatialVec& V_GB = matter.getBodyVelocity(s,mb);
+
+ const Vec3 p_AB_G = X_GB.p() - X_GA.p();
+ const Vec3 p_AB_G_dot = V_GB[1] - V_GA[1]; // time deriv of p taken in G
+
+ const Vec3 w_AB_G = V_GB[0] - V_GA[0]; // relative angular velocity
+ const Vec3 v_AB_G = p_AB_G_dot - V_GA[0] % p_AB_G; // time deriv of p in A, exp in G
+ const SpatialVec V_AB = ~X_GA.R() * SpatialVec(w_AB_G, v_AB_G); // re-express in A
+ sr.setSubtreeBodyVelocity(sb, V_AB);
+ }
+ }
+
+ sr.setStage(Stage::Velocity);
+}
+
+void SimbodyMatterSubtree::SubtreeRep::
+calcVelocitiesFromSubtreeU(const State& s, const Vector& subU, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
+ const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
+ SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Instance,
+ "calcVelocitiesFromSubtreeU()");
+
+ assert(isCompatibleSubtreeResults(sr));
+
+ assert(!"not implemented yet");
+}
+
+void SimbodyMatterSubtree::SubtreeRep::
+calcVelocitiesFromZeroU(const State& s, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
+ const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
+ SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Instance,
+ "calcVelocitiesFromZeroU()");
+
+ assert(isCompatibleSubtreeResults(sr));
+
+ assert(!"not implemented yet");
+}
+
+void SimbodyMatterSubtree::SubtreeRep::
+perturbVelocities(const State& s, SubtreeUIndex subUIndex, Real perturbation, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
+ const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
+ SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Instance,
+ "perturbVelocities()");
+
+ assert(isCompatibleSubtreeResults(sr));
+ assert(sr.getStage() >= Stage::Velocity);
+
+ assert(!"not implemented yet");
+}
+
+void SimbodyMatterSubtree::SubtreeRep::
+copyAccelerationsFromState(const State& s, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
+ const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
+ SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Acceleration,
+ "calcAccelerationsFromState()");
+
+ assert(isCompatibleSubtreeResults(sr));
+
+ // Copy the udot's; adjust the body accelerations to be measured relative to,
+ // and expressed in, the ancestor body instead of ground.
+ sr.packStateUIntoSubtreeU(matter.getUDot(s), sr.updSubUDot());
+
+ if (getAncestorMobilizedBodyIndex() == GroundIndex) {
+ for (SubtreeBodyIndex sb(1); sb < getNumSubtreeBodies(); ++sb) {
+ const MobilizedBodyIndex mb = getSubtreeBodyMobilizedBodyIndex(sb);
+ const SpatialVec& A_GB = matter.getBodyAcceleration(s,mb);
+ sr.setSubtreeBodyAcceleration(sb, A_GB); // =A_AB
+ }
+ } else {
+ // Ancestor A differs from Ground G so we have to adjust all the
+ // Subtree body accelerations to measure from A instead of G.
+ const Transform& X_GA = matter.getBodyTransform(s,getAncestorMobilizedBodyIndex());
+ const SpatialVec& V_GA = matter.getBodyVelocity(s,getAncestorMobilizedBodyIndex());
+ const SpatialVec& A_GA = matter.getBodyAcceleration(s,getAncestorMobilizedBodyIndex());
+ for (SubtreeBodyIndex sb(1); sb < getNumSubtreeBodies(); ++sb) {
+ const MobilizedBodyIndex mb = getSubtreeBodyMobilizedBodyIndex(sb);
+ const Transform& X_GB = matter.getBodyTransform(s,mb);
+ const SpatialVec& V_GB = matter.getBodyVelocity(s,mb);
+ const SpatialVec& A_GB = matter.getBodyAcceleration(s,mb);
+
+ const Vec3 p_AB_G = X_GB.p() - X_GA.p();
+ const Vec3 p_AB_G_dot = V_GB[1] - V_GA[1]; // taken in G
+ const Vec3 p_AB_G_dotdot = A_GB[1] - A_GA[1]; // taken in G
+
+ const Vec3 v_AB_G = p_AB_G_dot - V_GA[0] % p_AB_G; // taken in A, exp. in G
+ const Vec3 b_AB_G = A_GB[0] - A_GA[0]; // relative angular acceleration
+ const Vec3 a_AB_G = p_AB_G_dotdot - (A_GA[0] % p_AB_G + V_GA[0] % p_AB_G_dot); // taken in A, exp. in G
+ const SpatialVec A_AB = ~X_GA.R() * SpatialVec(b_AB_G, a_AB_G); // re-express in A
+ sr.setSubtreeBodyAcceleration(sb, A_AB);
+ }
+ }
+
+ sr.setStage(Stage::Acceleration);
+}
+
+void SimbodyMatterSubtree::SubtreeRep::
+calcAccelerationsFromSubtreeUDot(const State& s, const Vector& subUDot, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
+ const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
+ SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Instance,
+ "calcAccelerationsFromSubtreeUDot()");
+
+ assert(isCompatibleSubtreeResults(sr));
+
+ assert(!"not implemented yet");
+}
+
+void SimbodyMatterSubtree::SubtreeRep::
+calcAccelerationsFromZeroUDot(const State& s, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
+ const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
+ SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Instance,
+ "calcAccelerationsFromZeroUDot()");
+
+ assert(isCompatibleSubtreeResults(sr));
+
+ assert(!"not implemented yet");
+}
+
+void SimbodyMatterSubtree::SubtreeRep::
+perturbAccelerations(const State& s, SubtreeUIndex subUDotIndex, Real perturbation, SimbodyMatterSubtreeResults::SubtreeResultsRep& sr) const {
+ const SimbodyMatterSubsystemRep& matter = getSimbodyMatterSubsystem().getRep();
+ SimTK_STAGECHECK_GE_ALWAYS(matter.getStage(s), Stage::Instance,
+ "perturbAccelerations()");
+
+ assert(isCompatibleSubtreeResults(sr));
+ assert(sr.getStage() >= Stage::Acceleration);
+
+ assert(!"not implemented yet");
+}
+
+
+} // namespace SimTK
+
diff --git a/Simbody/src/SimbodyTreeState.h b/Simbody/src/SimbodyTreeState.h
new file mode 100644
index 0000000..3f89144
--- /dev/null
+++ b/Simbody/src/SimbodyTreeState.h
@@ -0,0 +1,1696 @@
+#ifndef SimTK_SIMBODY_TREE_STATE_H_
+#define SimTK_SIMBODY_TREE_STATE_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) 2005-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* This file contains the classes which define the SimbodyMatterSubsystem State,
+that is, everything that can be changed in a SimbodyMatterSubsystem after
+construction.
+
+State variables and computation results are organized into stages:
+ Stage::Empty virginal state just allocated
+ Stage::Topology Stored in SimbodyMatterSubsystem object (construction)
+ ---------------------------------------------------------
+ Stage::Model Stored in the State object
+ Stage::Instance
+ Stage::Time
+ Stage::Position
+ Stage::Velocity
+ Stage::Dynamics dynamic operators available
+ Stage::Acceleration response to forces in the state
+ ---------------------------------------------------------
+ Stage::Report only used when outputting something
+
+Construction proceeds until all the bodies and constraints have been specified.
+After that, realizeTopology() is called. Construction-related calculations are
+performed leading to values which are stored in the SimbodyMatterSubsystem
+object, NOT in the State (e.g., total number of bodies). At the same time, an
+initial state is built, with space allocated for the state variables that will
+be needed by the next stage (Stage::Model),and these are assigned default
+values. Then the stage in the SimbodyMatterSubsystem and in the initial state
+is set to "Topology".
+
+After that, values for Model stage variables can be set in the State.
+When that's done we call realizeModel(), which evaluates the Model states
+putting the values into state cache entries allocated for the purpose. Then
+all remaining state variables are allocated, and set to their default values.
+All defaults must be computable knowing only the Model stage values.
+Then the stage is advanced to Stage::Model.
+
+This continues through all the stages, with realizeWhatever() expecting to
+receive a state evaluated to stage Whatever-1 equipped with values for stage
+Whatever so that it can calculate results and put them in the cache (which is
+allocated if necessary), and then advance to stage Whatever. */
+
+#include "simbody/internal/common.h"
+
+#include <cassert>
+#include <iostream>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+class SimbodyMatterSubsystemRep;
+class RigidBodyNode;
+template <int dof, bool noR_FM, bool noX_MB, bool noR_PF>
+ class RigidBodyNodeSpec;
+
+// defined below
+
+class SBTopologyCache;
+class SBModelCache;
+class SBInstanceCache;
+class SBTimeCache;
+class SBTreePositionCache;
+class SBConstrainedPositionCache;
+class SBCompositeBodyInertiaCache;
+class SBArticulatedBodyInertiaCache;
+class SBTreeVelocityCache;
+class SBConstrainedVelocityCache;
+class SBDynamicsCache;
+class SBTreeAccelerationCache;
+class SBConstrainedAccelerationCache;
+
+class SBModelVars;
+class SBInstanceVars;
+class SBTimeVars;
+class SBPositionVars;
+class SBVelocityVars;
+class SBDynamicsVars;
+class SBAccelerationVars;
+
+
+
+// =============================================================================
+// TOPOLOGY CACHE
+// =============================================================================
+// An object of this type is stored in the SimbodyMatterSubsystem after extended
+// construction is complete, then copied into a slot in the State upon
+// realizeTopology(). It should contain enough information to size the Model
+// stage, and State resource index numbers for the Model-stage state variables
+// and Model-stage cache entry. This topology cache entry can also contain
+// whatever arbitrary data you would like to have in a State to verify that it
+// is a match for the SimbodyMatterSubsystem.
+//
+// Note that this does not include all possible topological information in
+// a SimbodyMatterSubsystem -- any subobjects are free to hold their own
+// as long as they don't change it after realizeTopology().
+class SBTopologyCache {
+public:
+ SBTopologyCache() {clear();}
+
+ void clear() {
+ nBodies = nParticles = nConstraints = nAncestorConstrainedBodies =
+ nDOFs = maxNQs = sumSqDOFs = -1;
+ modelingVarsIndex.invalidate();
+ modelingCacheIndex.invalidate();
+ topoInstanceVarsIndex.invalidate();
+ valid = false;
+ }
+
+ // These are topological objects.
+ int nBodies;
+ int nParticles;
+ int nConstraints;
+
+ // This is the total number of Constrained Bodies appearing in all
+ // Constraints where the Ancestor body is not Ground, excluding the
+ // Ancestor bodies themselves even if they are also Constrained Bodies
+ // (which is common). This is used for sizing pool entries in various
+ // caches to hold precalculated Ancestor-frame data about these bodies.
+ int nAncestorConstrainedBodies;
+
+ // TODO: these should be moved to Model stage.
+ int nDOFs;
+ int maxNQs;
+ int sumSqDOFs;
+
+ DiscreteVariableIndex modelingVarsIndex;
+ CacheEntryIndex modelingCacheIndex;
+
+ // These are instance variables that exist regardless of modeling
+ // settings; they are instance variables corresponding to topological
+ // elements of the matter subsystem (e.g. mobilized bodies and constraints).
+ DiscreteVariableIndex topoInstanceVarsIndex;
+
+ bool valid;
+};
+//.............................. TOPOLOGY CACHE ................................
+
+
+// =============================================================================
+// MODEL CACHE
+// =============================================================================
+// This cache entry contains counts of various things resulting from the
+// settings of Model-stage state variables. It also contains the resource index
+// numbers for all state variable and state cache resources allocated during
+// realizeModel().
+//
+// Model stage is when the mobilizers settle on the meaning of the q's and u's
+// they will employ, so here is where we count up the total number of q's and
+// u's and assign particular slots in those arrays to each mobilizer. We also
+// determine the sizes of related "pools", including the number of q's which
+// are angles (for sincos calculations), and the number of quaternions in use
+// (for normalization calculations), and partition the entries in those
+// pools among the mobilizers.
+//
+// Important things we still don't know at this stage:
+// - what constraints are enabled
+// - how motion will be driven for each coordinate
+
+
+// -----------------------------------------------------------------------------
+// PER MOBILIZED BODY MODEL INFO
+class SBModelPerMobodInfo {
+public:
+ SBModelPerMobodInfo()
+ : nQInUse(-1), nUInUse(-1), hasQuaternionInUse(false),
+ nQPoolInUse(-1) {}
+
+ int nQInUse, nUInUse;
+ QIndex firstQIndex; // Count from 0 for this SimbodyMatterSubsystem
+ UIndex firstUIndex;
+
+ // In case there is a quaternion in use by this Mobilizer: The index
+ // here can be used to find precalculated data associated with this
+ // quaternion, such as its current length.
+ bool hasQuaternionInUse;
+ // 0..nQInUse-1: which local coordinate starts the quaternion if any?
+ MobilizerQIndex startOfQuaternion;
+ // assigned slot # for this MB's quat, -1 if none
+ QuaternionPoolIndex quaternionPoolIndex;
+
+ // Each mobilizer can request some position-cache storage space for
+ // precalculations involving its generalized coordinates q.
+ // Here we keep track of our chunk.
+ int nQPoolInUse; // reserved space in sizeof(Real) units
+ MobodQPoolIndex startInQPool; // offset into pool
+};
+
+
+// -----------------------------------------------------------------------------
+// MODEL CACHE
+class SBModelCache {
+public:
+ SBModelCache() {clear();}
+
+ // Restore this cache entry to its just-constructed condition.
+ void clear() {
+ totalNQInUse=totalNUInUse=totalNQuaternionsInUse= -1;
+ totalNQPoolInUse= -1;
+
+ mobodModelInfo.clear();
+ }
+
+ // Allocate the entries in this ModelCache based on information provided in
+ // the TopologyCache.
+ void allocate(const SBTopologyCache& tc) {
+ mobodModelInfo.resize(tc.nBodies);
+ }
+
+ // Use these accessors so that you get type checking on the index types.
+ int getNumMobilizedBodies() const {return (int)mobodModelInfo.size();}
+ SBModelPerMobodInfo& updMobodModelInfo(MobilizedBodyIndex mbx)
+ { return mobodModelInfo[mbx]; }
+ const SBModelPerMobodInfo& getMobodModelInfo(MobilizedBodyIndex mbx) const
+ { return mobodModelInfo[mbx]; }
+
+
+ // These are sums over the per-MobilizedBody counts above.
+ int totalNQInUse, totalNUInUse, totalNQuaternionsInUse;
+ int totalNQPoolInUse;
+
+ // STATE ALLOCATION FOR THIS SUBSYSTEM
+
+ // Note that a MatterSubsystem is only one of potentially many users of a
+ // System's State, so only a subset of State variables and State Cache
+ // entries belong to it. Here we record the indices we were given when
+ // we asked the State for some resources. All indices are private to this
+ // Subsystem -- they'll start from zero regardless of whether there are
+ // other State resource consumers.
+
+ QIndex qIndex; // NOTE: local, currently always zero
+ UIndex uIndex;
+ DiscreteVariableIndex timeVarsIndex, qVarsIndex, uVarsIndex,
+ dynamicsVarsIndex, accelerationVarsIndex;
+ CacheEntryIndex instanceCacheIndex, timeCacheIndex,
+ treePositionCacheIndex, constrainedPositionCacheIndex,
+ compositeBodyInertiaCacheIndex, articulatedBodyInertiaCacheIndex,
+ treeVelocityCacheIndex, constrainedVelocityCacheIndex,
+ dynamicsCacheIndex,
+ treeAccelerationCacheIndex, constrainedAccelerationCacheIndex;
+
+private:
+ // MobilizedBody 0 is Ground.
+ Array_<SBModelPerMobodInfo,MobilizedBodyIndex> mobodModelInfo;
+};
+
+inline std::ostream& operator<<(std::ostream& o, const SBModelCache& c) {
+ o << "SBModelCache:\n";
+ o << " " << c.getNumMobilizedBodies() << " Mobilized Bodies:\n";
+ for (MobilizedBodyIndex mbx(0); mbx < c.getNumMobilizedBodies(); ++mbx) {
+ const SBModelPerMobodInfo& mInfo = c.getMobodModelInfo(mbx);
+ o << " " << mbx << ": nq,nu=" << mInfo.nQInUse << "," << mInfo.nUInUse
+ << " qix,uix=" << mInfo.firstQIndex << "," << mInfo.firstUIndex << endl;
+ if (mInfo.hasQuaternionInUse)
+ o << " firstQuat,quatPoolIx=" << mInfo.startOfQuaternion << "," << mInfo.quaternionPoolIndex << endl;
+ else o << " no quaternion in use\n";
+ if (mInfo.nQPoolInUse)
+ o << " nQPool,qPoolIx=" << mInfo.nQPoolInUse << "," << mInfo.startInQPool << endl;
+ else o << " no angles in use\n";
+ }
+ return o;
+}
+//............................... MODEL CACHE ..................................
+
+
+
+// =============================================================================
+// INSTANCE CACHE
+// =============================================================================
+// This is SimbodyMatterSubsystem information calculated during
+// realizeInstance(), possibly based on the settings of Instance-stage state
+// variables. At this point we will have determined the following information:
+// - final mass properties for all bodies (can calculate total mass)
+// - final locations for all mobilizer frames
+// - which Constraints are enabled
+// - how many and what types of constraint equations are to be included
+// - how motion is to be calculated for each mobilizer
+//
+// We allocate entries in the constraint error and multiplier pools among the
+// Constraints, and allocate entries in the prescribed motion and prescribed
+// force pools among mobilizers whose motion is prescribed.
+//
+// At this point we can classify all the mobilizers based on the kind of Motion
+// they will undergo. We determine the scope of every Constraint, and classify
+// them based on the kinds of mobilizers they affect.
+
+
+// -----------------------------------------------------------------------------
+// PER MOBILIZED BODY INSTANCE INFO
+// This is information calculated once we have seen all the Instance-stage
+// State variable values that can affect bodies, mobilizers, and motions.
+// Notes:
+// - all mobilities of a mobilizer must be treated identically
+// - if any motion level is fast, then the whole mobilizer is fast
+// - if a mobilizer is fast, so are all its outboard mobilizers
+class SBInstancePerMobodInfo {
+public:
+ SBInstancePerMobodInfo() {clear();}
+
+ void clear() {
+ qMethod=uMethod=udotMethod=Motion::Free;
+ firstPresQ.invalidate(); firstPresU.invalidate();
+ firstPresUDot.invalidate(); firstPresForce.invalidate();
+ }
+
+ Motion::Method qMethod; // how are positions calculated?
+ Motion::Method uMethod; // how are velocities calculated?
+ Motion::Method udotMethod; // how are accelerations calculated?
+
+ PresQPoolIndex firstPresQ; // if qMethod==Prescribed
+ PresUPoolIndex firstPresU; // if uMethod==Prescribed
+ PresUDotPoolIndex firstPresUDot; // if udotMethod==Prescribed
+ PresForcePoolIndex firstPresForce; // if udotMethod!=Free
+};
+
+
+// -----------------------------------------------------------------------------
+// PER CONSTRAINT INSTANCE INFO
+// Store some Instance-stage information about each Constraint. Most
+// important, we don't know how many constraint equations (if any) the
+// Constraint will generate until Instance stage. In particular, a disabled
+// Constraint won't generate any equations (it will have an Info entry
+// here, however). Also, although we know the Constrained Mobilizers at
+// Topology stage, we don't know the specific number or types of internal
+// coordinates involved until Instance stage.
+
+// Helper class for per-constrained mobilizer information.
+class SBInstancePerConstrainedMobilizerInfo {
+public:
+ SBInstancePerConstrainedMobilizerInfo()
+ : nQInUse(0), nUInUse(0) { } // assume disabled
+ // The correspondence between Constrained Mobilizers and Mobilized
+ // Bodies is Topological information you can pull from the
+ // TopologyCache. See the MobilizedBody for counts of its q's and u's,
+ // which define the allocated number of slots for the
+ // ConstrainedMobilizer as well.
+ int nQInUse, nUInUse; // same as corr. MobilizedBody unless disabled
+ ConstrainedQIndex firstConstrainedQIndex; // these count from 0 for
+ ConstrainedUIndex firstConstrainedUIndex; // each Constraint
+};
+
+
+class SBInstancePerConstraintInfo {
+public:
+ SBInstancePerConstraintInfo() { }
+ void clear() {
+ constrainedMobilizerInstanceInfo.clear();
+ constrainedQ.clear(); constrainedU.clear();
+ participatingQ.clear(); participatingU.clear();
+ }
+
+ void allocateConstrainedMobilizerInstanceInfo(int nConstrainedMobilizers) {
+ assert(nConstrainedMobilizers >= 0);
+ constrainedMobilizerInstanceInfo.resize(nConstrainedMobilizers);
+ constrainedQ.clear(); // build by appending
+ constrainedU.clear();
+ }
+
+ int getNumConstrainedMobilizers() const
+ { return (int)constrainedMobilizerInstanceInfo.size(); }
+
+ const SBInstancePerConstrainedMobilizerInfo&
+ getConstrainedMobilizerInstanceInfo(ConstrainedMobilizerIndex M) const
+ { return constrainedMobilizerInstanceInfo[M]; }
+
+ SBInstancePerConstrainedMobilizerInfo&
+ updConstrainedMobilizerInstanceInfo(ConstrainedMobilizerIndex M)
+ { return constrainedMobilizerInstanceInfo[M]; }
+
+ int getNumConstrainedQ() const {return (int)constrainedQ.size();}
+ int getNumConstrainedU() const {return (int)constrainedU.size();}
+ ConstrainedQIndex addConstrainedQ(QIndex qx) {
+ constrainedQ.push_back(qx);
+ return ConstrainedQIndex(constrainedQ.size()-1);
+ }
+ ConstrainedUIndex addConstrainedU(UIndex ux) {
+ constrainedU.push_back(ux);
+ return ConstrainedUIndex(constrainedU.size()-1);
+ }
+ QIndex getQIndexFromConstrainedQ(ConstrainedQIndex i) const
+ { return constrainedQ[i]; }
+ UIndex getUIndexFromConstrainedU(ConstrainedUIndex i) const
+ { return constrainedU[i]; }
+
+ int getNumParticipatingQ() const {return (int)participatingQ.size();}
+ int getNumParticipatingU() const {return (int)participatingU.size();}
+ ParticipatingQIndex addParticipatingQ(QIndex qx) {
+ participatingQ.push_back(qx);
+ return ParticipatingQIndex(participatingQ.size()-1);
+ }
+ ParticipatingUIndex addParticipatingU(UIndex ux) {
+ participatingU.push_back(ux);
+ return ParticipatingUIndex(participatingU.size()-1);
+ }
+ QIndex getQIndexFromParticipatingQ(ParticipatingQIndex i) const
+ { return participatingQ[i]; }
+ UIndex getUIndexFromParticipatingU(ParticipatingUIndex i) const
+ { return participatingU[i]; }
+
+ Segment holoErrSegment; // (offset,mHolo) for each Constraint, within subsystem qErr
+ Segment nonholoErrSegment; // (offset,mNonholo) same, but for uErr slots (after holo derivs)
+ Segment accOnlyErrSegment; // (offset,mAccOnly) same, but for udotErr slots (after holo/nonholo derivs)
+
+ Segment consBodySegment;
+ Segment consMobilizerSegment; // mobilizers, not *mobilities*
+ Segment consQSegment;
+ Segment consUSegment; // these (u) are *mobilities*
+public:
+ Array_<SBInstancePerConstrainedMobilizerInfo,
+ ConstrainedMobilizerIndex> constrainedMobilizerInstanceInfo;
+
+ // The ConstrainedBodies and ConstrainedMobilizers are set at Topology
+ // stage, but the particular generalized coordinates q and generalized
+ // speeds u which are involved can't be determined until Model stage,
+ // since the associated mobilizers have Model stage options which can
+ // affect the number and meanings of these variables. These are sorted
+ // in order of their associated ConstrainedMobilizer, not necessarily
+ // in order of QIndex or UIndex. Each value appears only once.
+ Array_<QIndex,ConstrainedQIndex> constrainedQ; // -> subsystem QIndex
+ Array_<UIndex,ConstrainedUIndex> constrainedU; // -> subsystem UIndex
+
+ // Participating mobilities include ALL the mobilities which may be
+ // involved in any of this Constraint's constraint equations, whether
+ // from being directly constrained or indirectly as a result of their
+ // effects on ConstrainedBodies. These are sorted in order of
+ // increasing QIndex and UIndex, and each QIndex or UIndex appears
+ // only once.
+ Array_<QIndex,ParticipatingQIndex> participatingQ; // -> subsystem QIndex
+ Array_<UIndex,ParticipatingUIndex> participatingU; // -> subsystem UIndex
+};
+
+
+// -----------------------------------------------------------------------------
+// INSTANCE CACHE
+class SBInstanceCache {
+public:
+ // Instance variables are:
+ // body mass props; particle masses
+ // X_BM, X_PF mobilizer transforms
+ //
+ // Calculations stored here derive from those states:
+ // total mass
+ // central inertia of each rigid body
+ // principal axes and corresponding principal moments of inertia of
+ // each rigid body
+ // reference configuration X_PB when q==0 (usually that means M==F),
+ // for each rigid body
+
+ Real totalMass; // sum of all rigid body and particles masses
+ Array_<Inertia,MobilizedBodyIndex> centralInertias; // nb
+ Array_<Vec3,MobilizedBodyIndex> principalMoments; // nb
+ Array_<Rotation,MobilizedBodyIndex> principalAxes; // nb
+ Array_<Transform,MobilizedBodyIndex> referenceConfiguration; // nb
+
+ int getNumMobilizedBodies() const {return (int)mobodInstanceInfo.size();}
+ SBInstancePerMobodInfo& updMobodInstanceInfo(MobilizedBodyIndex mbx)
+ { return mobodInstanceInfo[mbx]; }
+ const SBInstancePerMobodInfo& getMobodInstanceInfo(MobilizedBodyIndex mbx) const
+ { return mobodInstanceInfo[mbx]; }
+ Array_<SBInstancePerMobodInfo,MobilizedBodyIndex> mobodInstanceInfo;
+
+ int getNumConstraints() const {return (int)constraintInstanceInfo.size();}
+ SBInstancePerConstraintInfo& updConstraintInstanceInfo(ConstraintIndex cx)
+ { return constraintInstanceInfo[cx]; }
+ const SBInstancePerConstraintInfo& getConstraintInstanceInfo(ConstraintIndex cx) const
+ { return constraintInstanceInfo[cx]; }
+ Array_<SBInstancePerConstraintInfo,ConstraintIndex> constraintInstanceInfo;
+
+ // This is a sum over all the mobilizers whose q's are currently prescribed,
+ // adding the number of q's (generalized coordinates) nq currently being
+ // used for each of those. An array of size totalNPresQ is allocated in the
+ // TimeCache to hold the calculated q's (which will be different from the
+ // actual q's until they are applied). Motions will also provide this many
+ // prescribed qdots and qdotdots, but we will map those to u's and udots
+ // before recording them, with nu entries being allocated in each. These
+ // nq- and nu-sized slots are allocated in order of MobilizedBodyIndex.
+ int getTotalNumPresQ() const {return (int)presQ.size();}
+ int getTotalNumZeroQ() const {return (int)zeroQ.size();}
+ int getTotalNumFreeQ() const {return (int)freeQ.size();}
+ Array_<QIndex> presQ;
+ Array_<QIndex> zeroQ;
+ Array_<QIndex> freeQ; // must be integrated
+
+ // This is a sum over all the mobilizers whose u's are current prescribed,
+ // whether because of non-holonomic (velocity) prescribed motion u=u(t,q),
+ // or because the q's are prescribed via holonomic (position) prescribed
+ // motion and the u's are calculated from the qdots. We add the number u's
+ // (generalized speeds) nu currently being used for each holonomic- or
+ // nonholonomic-prescribed mobilizer. An array of this size is allocated
+ // in the PositionCache to hold the calculated u's (which will be
+ // different from the actual u's until they are applied). Nu-sized slots
+ // are allocated in order of MobilizedBodyIndex.
+ int getTotalNumPresU() const {return (int)presU.size();}
+ int getTotalNumZeroU() const {return (int)zeroU.size();}
+ int getTotalNumFreeU() const {return (int)freeU.size();}
+ Array_<UIndex> presU;
+ Array_<UIndex> zeroU;
+ Array_<UIndex> freeU; // must be integrated
+
+ // This is a sum over all the mobilizers whose udots are currently
+ // prescribed, adding the number of udots (mobilities) nu from each
+ // holonomic-, nonholonomic-, or acceleration-prescribed mobilizer. An
+ // array of this size is allocated in the DynamicsCache, and an entry is
+ // needed in the prescribed force array in the AccelerationCache as well.
+ // These nu-sized slots are allocated in order of MobilizedBodyIndex.
+ int getTotalNumPresUDot() const {return (int)presUDot.size();}
+ int getTotalNumZeroUDot() const {return (int)zeroUDot.size();}
+ int getTotalNumFreeUDot() const {return (int)freeUDot.size();}
+ Array_<UIndex> presUDot;
+ Array_<UIndex> zeroUDot;
+ Array_<UIndex> freeUDot; // calculated from forces
+
+ // This includes all the mobilizers whose udots are known for any
+ // reason: Prescribed, Zero, Discrete, or Fast (anything but Free).
+ // These need slots in the array of calculated prescribed motion
+ // forces (taus). This maps those tau entries to the mobility at
+ // which they are generalized forces.
+ int getTotalNumPresForces() const {return (int)presForce.size();}
+ Array_<UIndex> presForce;
+
+ // Quaternion errors go in qErr also, but after all the physical contraint
+ // errors. That is, they start at index
+ // totalNHolonomicConstraintEquationsInUse.
+ int firstQuaternionQErrSlot;
+
+ // These record where in the full System's State our Subsystem's qErr, uErr,
+ // and udotErr entries begin. That is, this subsystem's segments can be
+ // found at
+ // qErr (qErrIndex, nPositionConstraintEquationsInUse
+ // + nQuaternionsInUse)
+ // uErr (uErrIndex, nVelocityConstraintEquationsInUse)
+ // udotErr(udotErrIndex, nAccelerationConstraintEquationsInUse)
+ int qErrIndex, uErrIndex, udotErrIndex;
+
+ // These are the sums over the per-Constraint data above. The number of
+ // position constraint equations (not counting quaternion normalization
+ // constraints) is the same as the number of holonomic constraints mHolo.
+ // The number of velocity constraint equations is mHolo+mNonholo. The
+ // number of acceleration constraint equations, and thus the number of
+ // udotErrs and multipliers, is mHolo+mNonholo+mAccOnly.
+ int totalNHolonomicConstraintEquationsInUse; // sum(mHolo) (#position equations = mHolo)
+ int totalNNonholonomicConstraintEquationsInUse; // sum(mNonholo) (#velocity equations = mHolo+mNonholo)
+ int totalNAccelerationOnlyConstraintEquationsInUse; // sum(mAccOnly) (#acceleration eqns = mHolo+mNonholo+mAccOnly)
+
+ int totalNConstrainedBodiesInUse;
+ int totalNConstrainedMobilizersInUse;
+ int totalNConstrainedQInUse; // q,u from the constrained mobilizers
+ int totalNConstrainedUInUse;
+public:
+ void allocate(const SBTopologyCache& topo,
+ const SBModelCache& model)
+ {
+ totalMass = SimTK::NaN;
+ centralInertias.resize(topo.nBodies); // I_CB
+ principalMoments.resize(topo.nBodies); // (Ixx,Iyy,Izz)
+ principalAxes.resize(topo.nBodies); // [axx ayy azz]
+ referenceConfiguration.resize(topo.nBodies); // X0_PB
+
+ mobodInstanceInfo.resize(topo.nBodies);
+
+ constraintInstanceInfo.resize(topo.nConstraints);
+ firstQuaternionQErrSlot = qErrIndex = uErrIndex = udotErrIndex = -1;
+
+ totalNHolonomicConstraintEquationsInUse = 0;
+ totalNNonholonomicConstraintEquationsInUse = 0;
+ totalNAccelerationOnlyConstraintEquationsInUse = 0;
+
+ totalNConstrainedBodiesInUse = 0;
+ totalNConstrainedMobilizersInUse = 0;
+ totalNConstrainedQInUse = 0;
+ totalNConstrainedUInUse = 0;
+ }
+
+};
+//.............................. INSTANCE CACHE ................................
+
+
+
+// =============================================================================
+// TIME CACHE
+// =============================================================================
+// Here we hold information that is calculated in the SimbodyMatterSubsystem's
+// realizeTime() method. Currently that consists only of prescribed q's, which
+// must always be defined as functions of time.
+
+class SBTimeCache {
+public:
+ // This holds values from Motion prescribed position (holonomic) calculations.
+ Array_<Real> presQPool; // Index with PresQPoolIndex
+
+public:
+ void allocate(const SBTopologyCache& topo,
+ const SBModelCache& model,
+ const SBInstanceCache& instance)
+ {
+ presQPool.resize(instance.getTotalNumPresQ());
+ }
+};
+//............................... TIME CACHE ...................................
+
+
+
+// =============================================================================
+// TREE POSITION CACHE
+// =============================================================================
+// Here we hold information that is calculated early in the matter subsystem's
+// realizePosition() method. This includes
+//
+// - mobilizer matrices X_FM, H_FM, X_PB, H_PB_G
+// - basic kinematic information X_GB, Phi_PB_G
+// - mass properties expressed in Ground (TODO: these should probably be in
+// their own cache since they aren't needed for kinematics)
+//
+// - for constrained bodies, position X_AB of each body in its ancestor A
+//
+// This cache entry can be calculated after Stage::Time and is guaranteed to
+// have been calculated by the end of Stage::Position. The
+// SimbodyMatterSubsystem's realizePosition() method will mark this done as
+// soon as possible, so that later calculations (constraint position errors,
+// prescribed velocities) can access these without a stage violation.
+
+class SBTreePositionCache {
+public:
+ const Transform& getX_FM(MobilizedBodyIndex mbx) const {return bodyJointInParentJointFrame[mbx];}
+ Transform& updX_FM(MobilizedBodyIndex mbx) {return bodyJointInParentJointFrame[mbx];}
+ const Transform& getX_PB(MobilizedBodyIndex mbx) const {return bodyConfigInParent[mbx];}
+ Transform& updX_PB(MobilizedBodyIndex mbx) {return bodyConfigInParent[mbx];}
+ const Transform& getX_GB(MobilizedBodyIndex mbx) const {return bodyConfigInGround[mbx];}
+ Transform& updX_GB(MobilizedBodyIndex mbx) {return bodyConfigInGround[mbx];}
+
+ const Transform& getX_AB(AncestorConstrainedBodyPoolIndex cbpx) const
+ { return constrainedBodyConfigInAncestor[cbpx]; }
+ Transform& updX_AB(AncestorConstrainedBodyPoolIndex cbpx)
+ { return constrainedBodyConfigInAncestor[cbpx]; }
+public:
+ // qerr cache space is provided directly by the State
+
+ // At model stage, each mobilizer (RBNode) is given a chance to grab
+ // a segment of this cache entry for its own private use. This includes
+ // pre-calculated sincos(q) for mobilizers with angular coordinates,
+ // and all or part of N and NInv for mobilizers for which qdot != u.
+ // Everything must be filled in by the end of realizePosition() but the
+ // mobilizer is free to fill in different parts at different times during
+ // its realizePosition() calculations.
+ Array_<Real, MobodQPoolIndex> mobilizerQCache;
+
+ // CAUTION: our definition of the H matrix is transposed from those used
+ // by Jain and by Schwieters. Jain would call these H* and Schwieters
+ // would call them H^T, but we call them H.
+ Array_<Vec3> storageForH_FM; // 2 x ndof (H_FM)
+ Array_<Vec3> storageForH; // 2 x ndof (H_PB_G)
+
+ Array_<Transform,MobilizedBodyIndex> bodyJointInParentJointFrame; // nb (X_FM)
+ Array_<Transform,MobilizedBodyIndex> bodyConfigInParent; // nb (X_PB)
+ Array_<Transform,MobilizedBodyIndex> bodyConfigInGround; // nb (X_GB)
+ Array_<PhiMatrix,MobilizedBodyIndex> bodyToParentShift; // nb (phi)
+
+ // This contains mass m, p_BBc_G (center of mass location measured from
+ // B origin, expressed in Ground), and G_Bo_G (unit inertia [gyration]
+ // matrix about B's origin, expressed in Ground). Note that this body's
+ // inertia is I_Bo_G = m*G_Bo_G.
+ Array_<SpatialInertia,MobilizedBodyIndex> bodySpatialInertiaInGround; // nb (Mk_G)
+
+ // This is the body center of mass location measured from the ground
+ // origin and expressed in ground, p_GBc = p_GB + p_BBc_G (above).
+ Array_<Vec3,MobilizedBodyIndex> bodyCOMInGround; // nb (p_GBc)
+
+
+ // Constrained Body Pool
+
+ // For Constraints whose Ancestor body A is not Ground G, we assign pool
+ // entries for each of their Constrained Bodies (call the total number
+ // 'nacb') to store the above information but measured and expressed in
+ // the Ancestor frame rather than Ground.
+ Array_<Transform> constrainedBodyConfigInAncestor; // nacb (X_AB)
+
+public:
+ void allocate(const SBTopologyCache& tree,
+ const SBModelCache& model,
+ const SBInstanceCache& instance)
+ {
+ // Pull out construction-stage information from the tree.
+ const int nBodies = tree.nBodies;
+ const int nDofs = tree.nDOFs; // this is the number of u's (nu)
+ const int maxNQs = tree.maxNQs; // allocate the max # q's we'll ever need
+ const int nacb = tree.nAncestorConstrainedBodies;
+
+ // These contain uninitialized junk. Body-indexed entries get their
+ // ground elements set appropriately now and forever.
+
+ mobilizerQCache.resize(model.totalNQPoolInUse);
+
+ storageForH_FM.resize(2*nDofs);
+ storageForH.resize(2*nDofs);
+
+ bodyJointInParentJointFrame.resize(nBodies);
+ bodyJointInParentJointFrame[GroundIndex].setToZero();
+
+ bodyConfigInParent.resize(nBodies);
+ bodyConfigInParent[GroundIndex].setToZero();
+
+ bodyConfigInGround.resize(nBodies);
+ bodyConfigInGround[GroundIndex].setToZero();
+
+ bodyToParentShift.resize(nBodies);
+ bodyToParentShift[GroundIndex].setToZero();
+
+ bodySpatialInertiaInGround.resize(nBodies);
+ bodySpatialInertiaInGround[GroundIndex].setMass(Infinity);
+ bodySpatialInertiaInGround[GroundIndex].setMassCenter(Vec3(0));
+ bodySpatialInertiaInGround[GroundIndex].setUnitInertia(UnitInertia(Infinity));
+
+ bodyCOMInGround.resize(nBodies);
+ bodyCOMInGround[GroundIndex] = Vec3(0);
+
+ constrainedBodyConfigInAncestor.resize(nacb);
+ }
+};
+//.......................... TREE POSITION CACHE ...............................
+
+
+
+// =============================================================================
+// CONSTRAINED POSITION CACHE
+// =============================================================================
+// Here we hold information that is part of the matter subsystem's
+// realizePosition() calculation but depends on the TreePositionCache having
+// already been calculated. This includes:
+//
+// - desired values of prescribed u's (since those are functions of at most
+// time and position)
+// - logically, position constraint errors (qerrs), although in fact that
+// array is provided as a built-in by the State
+//
+// This cache entry can be calculated after Stage::Time provided that
+// the SBTreePositionCache entry has already been marked valid. We guarantee
+// this will have been calculated by Stage::Position.
+
+class SBConstrainedPositionCache {
+public:
+ // qerr cache space is provided directly by the State
+
+ // This holds values from all the Motion prescribed velocity (nonholonomic)
+ // calculations, and those resulting from diffentiating prescribed positions.
+ Array_<Real> presUPool; // Index with PresUPoolIndex
+
+public:
+ void allocate(const SBTopologyCache& tree,
+ const SBModelCache& model,
+ const SBInstanceCache& instance)
+ {
+ presUPool.resize(instance.getTotalNumPresU());
+ }
+};
+//........................ CONSTRAINED POSITION CACHE ..........................
+
+
+
+// =============================================================================
+// COMPOSITE BODY INERTIA CACHE
+// =============================================================================
+// Composite body inertias R are those that would be felt if all the mobilizers
+// had prescribed motion (or were welded in their current configurations). These
+// are convenient for inverse dynamics computations and for scaling of
+// generalized coordinates and speeds.
+//
+// Each spatial inertia here is expressed in the Ground frame but measured about
+// its body's origin.
+//
+// Note that each composite body inertia is a rigid-body spatial inertia, not
+// the more complicated articulated-body spatial inertia. That means these have
+// a scalar mass and well-defined mass center, and a very simple structure which
+// can be exploited for speed. There are at most 10 unique elements in a rigid
+// body spatial inertia matrix.
+//
+// Composite body inertias depend only on positions but are often not needed at
+// all. So we give them their own cache entry and expect explicit realization
+// some time after Position stage, if at all.
+
+class SBCompositeBodyInertiaCache {
+public:
+ Array_<SpatialInertia,MobilizedBodyIndex> compositeBodyInertia; // nb (R)
+
+public:
+ void allocate(const SBTopologyCache& tree,
+ const SBModelCache& model,
+ const SBInstanceCache& instance)
+ {
+ // Pull out construction-stage information from the tree.
+ const int nBodies = tree.nBodies;
+
+ compositeBodyInertia.resize(nBodies); // TODO: ground initialization
+ }
+};
+//....................... COMPOSITE BODY INERTIA CACHE .........................
+
+
+
+// =============================================================================
+// ARTICULATED BODY INERTIA CACHE
+// =============================================================================
+// These articulated body inertias take into account prescribed motion,
+// meaning that they are produced by a combination of articulated and rigid
+// shift operations depending on each mobilizer's current stats as "regular"
+// or "prescribed". That means that the articulated inertias here are suited
+// only for "mixed" dynamics; you can't use them to calculate M^-1*f unless
+// there is no prescribed motion in the system.
+//
+// Each articulated body inertia here is expressed in the Ground frame but
+// measured about its body's origin.
+//
+// Articulated body inertia matrices, though symmetric and positive
+// definite, do not have the same simple structure as rigid-body (or composite-
+// body) spatial inertias. For example, the apparent mass depends on direction.
+// All 21 elements of this symmetric 6x6 matrix are unique, while there are only
+// 10 unique elements in a rigid body spatial inertia.
+//
+// Note that although we use some *rigid* body shift operations here, the
+// results in general are all *articulated* body inertias, because a rigid shift
+// of an articulated body inertia is still an articulated body inertia. Only if
+// all mobilizers are prescribed will these be rigid body spatial inertias. For a
+// discussion of the properties of articulated body inertias, see Section 7.1
+// (pp. 119-123) of Roy Featherstone's excellent 2008 book, Rigid Body Dynamics
+// Algorithms.
+//
+// Intermediate quantities PPlus, D, DI, and G are calculated here
+// which are separately useful when dealing with "regular" mobilized bodies.
+// These quantities are not calculated for prescribed mobilizers; they will
+// remain NaN in that case. In
+// particular, this means that the prescribed-mobilizer mass properties do not
+// have to be invertible, so you can have terminal massless bodies as long as
+// their motion is always prescribed.
+// TODO: should D still be calculated? It doesn't require inversion.
+//
+// Articulated body inertias depend only on positions but are not usually needed
+// until Acceleration stage. Thus this cache entry should have dependsOn stage
+// Position, and computedBy stage Dynamics. However, it can be realized any
+// time after Position.
+
+class SBArticulatedBodyInertiaCache {
+public:
+ Array_<ArticulatedInertia,MobilizedBodyIndex> articulatedBodyInertia; // nb (P)
+
+ Array_<ArticulatedInertia,MobilizedBodyIndex> pPlus; // nb
+
+ Vector_<Real> storageForD; // sum(nu[j]^2)
+ Vector_<Real> storageForDI; // sum(nu[j]^2)
+ Array_<Vec3> storageForG; // 2 X ndof
+
+public:
+ void allocate(const SBTopologyCache& tree,
+ const SBModelCache& model,
+ const SBInstanceCache& instance)
+ {
+ // Pull out construction-stage information from the tree.
+ const int nBodies = tree.nBodies;
+ const int nDofs = tree.nDOFs; // this is the number of u's (nu)
+ const int nSqDofs = tree.sumSqDOFs; // sum(ndof^2) for each joint
+ const int maxNQs = tree.maxNQs; // allocate the max # q's we'll ever need
+
+ articulatedBodyInertia.resize(nBodies); // TODO: ground initialization
+
+ pPlus.resize(nBodies); // TODO: ground initialization
+
+ storageForD.resize(nSqDofs);
+ storageForDI.resize(nSqDofs);
+ storageForG.resize(2*nDofs);
+ }
+};
+//....................... ARTICULATED BODY INERTIA CACHE .......................
+
+
+
+// =============================================================================
+// TREE VELOCITY CACHE
+// =============================================================================
+// Here we hold information that is calculated early in the matter subsystem's
+// realizeVelocity() method. This includes
+//
+// - mobilizer matrices HDot_FM and HDot_PB_G
+// - cross mobilizer velocities V_FM, V_PB
+// - basic kinematics V_GB giving body velocities in Ground
+// - logically, qdot, but that is provided as a built-in cache entry in State
+// - for constrained bodies, V_AB giving body velocities in their ancestor A
+// - velocity-dependent dynamics remainder terms: coriolis acceleration and
+// gyroscopic forces
+//
+// This cache entry can be calculated after Stage::Position and is guaranteed to
+// have been calculated by the end of Stage::Velocity. The matter subsystem's
+// realizeVelocity() method will mark this done as soon as possible, so that
+// later calculations (constraint velocity errors) can access these without a
+// stage violation.
+
+class SBTreeVelocityCache {
+public:
+ const SpatialVec& getV_FM(MobilizedBodyIndex mbx) const {return mobilizerRelativeVelocity[mbx];}
+ SpatialVec& updV_FM(MobilizedBodyIndex mbx) {return mobilizerRelativeVelocity[mbx];}
+ const SpatialVec& getV_PB(MobilizedBodyIndex mbx) const {return bodyVelocityInParent[mbx];}
+ SpatialVec& updV_PB(MobilizedBodyIndex mbx) {return bodyVelocityInParent[mbx];}
+ const SpatialVec& getV_GB(MobilizedBodyIndex mbx) const {return bodyVelocityInGround[mbx];}
+ SpatialVec& updV_GB(MobilizedBodyIndex mbx) {return bodyVelocityInGround[mbx];}
+
+ const SpatialVec& getV_AB(AncestorConstrainedBodyPoolIndex cbpx) const
+ { return constrainedBodyVelocityInAncestor[cbpx]; }
+ SpatialVec& updV_AB(AncestorConstrainedBodyPoolIndex cbpx)
+ { return constrainedBodyVelocityInAncestor[cbpx]; }
+
+public:
+ // qdot cache space is supplied directly by the State
+
+ Array_<SpatialVec,MobilizedBodyIndex> mobilizerRelativeVelocity; // nb (V_FM) cross-mobilizer velocity
+ Array_<SpatialVec,MobilizedBodyIndex> bodyVelocityInParent; // nb (V_PB)
+ Array_<SpatialVec,MobilizedBodyIndex> bodyVelocityInGround; // nb (V_GB)
+
+ // CAUTION: our definition of the H matrix is transposed from those used
+ // by Jain and by Schwieters.
+ Array_<Vec3> storageForHDot_FM; // 2 x ndof (HDot_FM)
+ Array_<Vec3> storageForHDot; // 2 x ndof (HDot_PB_G)
+
+ // nb (VB_PB_G=HDot_PB_G*u)
+ Array_<SpatialVec,MobilizedBodyIndex> bodyVelocityInParentDerivRemainder;
+
+ Array_<SpatialVec,MobilizedBodyIndex> gyroscopicForces; // nb (b)
+ Array_<SpatialVec,MobilizedBodyIndex> mobilizerCoriolisAcceleration; // nb (a)
+ Array_<SpatialVec,MobilizedBodyIndex> totalCoriolisAcceleration; // nb (A)
+
+ // Ancestor Constrained Body Pool
+
+ // For Constraints whose Ancestor body A is not Ground G, we assign pool entries
+ // for each of their Constrained Bodies (call the total number 'nacb')
+ // to store the above information but measured and expressed in the Ancestor frame
+ // rather than Ground.
+ Array_<SpatialVec> constrainedBodyVelocityInAncestor; // nacb (V_AB)
+
+public:
+ void allocate(const SBTopologyCache& tree,
+ const SBModelCache& model,
+ const SBInstanceCache& instance)
+ {
+ // Pull out construction-stage information from the tree.
+ const int nBodies = tree.nBodies;
+ const int nDofs = tree.nDOFs; // this is the number of u's (nu)
+ const int maxNQs = tree.maxNQs; // allocate the max # q's we'll ever need
+ const int nacb = tree.nAncestorConstrainedBodies;
+
+ mobilizerRelativeVelocity.resize(nBodies);
+ mobilizerRelativeVelocity[GroundIndex] = SpatialVec(Vec3(0),Vec3(0));
+
+ bodyVelocityInParent.resize(nBodies);
+ bodyVelocityInParent[GroundIndex] = SpatialVec(Vec3(0),Vec3(0));
+
+ bodyVelocityInGround.resize(nBodies);
+ bodyVelocityInGround[GroundIndex] = SpatialVec(Vec3(0),Vec3(0));
+
+ storageForHDot_FM.resize(2*nDofs);
+ storageForHDot.resize(2*nDofs);
+
+ bodyVelocityInParentDerivRemainder.resize(nBodies);
+ bodyVelocityInParentDerivRemainder[GroundIndex] = SpatialVec(Vec3(0),Vec3(0));
+
+ gyroscopicForces.resize(nBodies);
+ gyroscopicForces[GroundIndex] = SpatialVec(Vec3(0),Vec3(0));
+
+ mobilizerCoriolisAcceleration.resize(nBodies);
+ mobilizerCoriolisAcceleration[GroundIndex] = SpatialVec(Vec3(0),Vec3(0));
+
+ totalCoriolisAcceleration.resize(nBodies);
+ totalCoriolisAcceleration[GroundIndex] = SpatialVec(Vec3(0),Vec3(0));
+
+ constrainedBodyVelocityInAncestor.resize(nacb);
+ }
+};
+//............................ TREE VELOCITY CACHE .............................
+
+
+
+// =============================================================================
+// CONSTRAINED VELOCITY CACHE
+// =============================================================================
+// Here we hold information that is part of the matter subsystem's
+// realizeVelocity() calculation but depends on the TreeVelocityCache having
+// already been calculated. This includes:
+//
+// - (not prescribed udots because we delay those until Dynamics)
+// - logically, velocity constraint errors (uerrs), although in fact that
+// array is provided as a built-in by the State
+//
+// This cache entry can be calculated after Stage::Position provided that
+// the SBTreeVelocityCache entry has already been marked valid. We guarantee
+// this will have been calculated by the end of Stage::Velocity.
+//
+// TODO: currently there is nothing here
+class SBConstrainedVelocityCache {
+public:
+ // uerr cache space is provided directly by the State
+
+public:
+ void allocate(const SBTopologyCache& tree,
+ const SBModelCache& model,
+ const SBInstanceCache& instance)
+ {
+ // nothing yet
+ }
+};
+//........................ CONSTRAINED VELOCITY CACHE ..........................
+
+
+
+// =============================================================================
+// DYNAMICS CACHE
+// =============================================================================
+class SBDynamicsCache {
+public:
+ // This holds the values from all the Motion prescribed acceleration
+ // calculations, and those which result from diffentiating prescribed velocities,
+ // or differentiating twice prescribed positions.
+ Array_<Real> presUDotPool; // Index with PresUDotPoolIndex
+
+ // Dynamics
+ // Here a=body's incremental contribution to coriolis acceleration
+ // A=total coriolis acceleration for this body
+ // b=gyroscopic force
+ Array_<SpatialVec,MobilizedBodyIndex> mobilizerCentrifugalForces; // nb (P*a+b)
+ Array_<SpatialVec,MobilizedBodyIndex> totalCentrifugalForces; // nb (P*A+b)
+
+ Array_<SpatialMat,MobilizedBodyIndex> Y; // nb
+
+public:
+ void allocate(const SBTopologyCache& tree,
+ const SBModelCache& model,
+ const SBInstanceCache& instance)
+ {
+ // Pull out construction-stage information from the tree.
+ const int nBodies = tree.nBodies;
+ const int nDofs = tree.nDOFs; // this is the number of u's (nu)
+ const int nSqDofs = tree.sumSqDOFs; // sum(ndof^2) for each joint
+ const int maxNQs = tree.maxNQs; // allocate the max # q's we'll ever need
+
+ presUDotPool.resize(instance.getTotalNumPresUDot());
+
+ mobilizerCentrifugalForces.resize(nBodies);
+ mobilizerCentrifugalForces[GroundIndex] = SpatialVec(Vec3(0),Vec3(0));
+
+ totalCentrifugalForces.resize(nBodies);
+ totalCentrifugalForces[GroundIndex] = SpatialVec(Vec3(0),Vec3(0));
+
+ Y.resize(nBodies); // TODO: op space compliance kernel (see Jain 2011)
+ Y[GroundIndex] = SpatialMat(Mat33(0));
+ }
+};
+//............................... DYNAMICS CACHE ...............................
+
+
+
+// =============================================================================
+// TREE ACCELERATION CACHE
+// =============================================================================
+// Here we hold information that is calculated early in the matter subsystem's
+// realizeAcceleration() method. This includes
+//
+// - basic kinematics A_GB giving body accelerations in Ground
+// - prescribed motion forces tau
+// - logically, udot and qdotdot, but those arrays are provided as built-in
+// cache entries in State
+//
+// - mobilizer reaction forces (TODO)
+//
+// This cache entry can be calculated after Stage::Dynamics and is guaranteed
+// to have been calculated by the end of Stage::Acceleration. The matter
+// subsystem's realizeAcceleration() method will mark this done as soon as
+// possible, so that later calculations (constraint acceleration errors) can
+// access these without a stage violation.
+
+class SBTreeAccelerationCache {
+public:
+ const SpatialVec& getA_GB(MobilizedBodyIndex mbx) const
+ { return bodyAccelerationInGround[mbx]; }
+ SpatialVec& updA_GB(MobilizedBodyIndex mbx)
+ { return bodyAccelerationInGround[mbx]; }
+
+public:
+ // udot, qdotdot cache space is provided directly by the State.
+
+
+ Vector_<SpatialVec> bodyAccelerationInGround; // nb (A_GB)
+
+ // This is where the calculated prescribed motion "taus" go. (That is,
+ // generalized forces needed to implement prescribed generalized
+ // accelerations.) Slots here are doled out only for mobilizers that have
+ // known accelerations; there is one scalar here per mobility in those
+ // mobilizers. Look in the InstanceCache to see which slots are allocated
+ // to which mobilizers.
+ Vector presMotionForces; // Index with PresForcePoolIndex
+
+ // Temps used in calculating accelerations and prescribed forces.
+ Vector epsilon; // nu
+ Array_<SpatialVec,MobilizedBodyIndex> z; // nb
+ Array_<SpatialVec,MobilizedBodyIndex> zPlus; // nb
+
+public:
+ void allocate(const SBTopologyCache& topo,
+ const SBModelCache& model,
+ const SBInstanceCache& instance)
+ {
+ // Pull out topology-stage information from the tree.
+ const int nBodies = topo.nBodies;
+ const int nDofs = topo.nDOFs; // this is the number of u's (nu)
+
+ bodyAccelerationInGround.resize(nBodies);
+ bodyAccelerationInGround[0] = SpatialVec(Vec3(0),Vec3(0));;
+
+ presMotionForces.resize(instance.getTotalNumPresForces());
+
+ epsilon.resize(nDofs);
+ z.resize(nBodies);
+ zPlus.resize(nBodies); // TODO: ground initialization
+ }
+};
+//.......................... TREE ACCELERATION CACHE ...........................
+
+
+
+// =============================================================================
+// CONSTRAINED ACCELERATION CACHE
+// =============================================================================
+// Here we hold information that is part of the matter subsystem's
+// realizeAcceleration() calculation but depends on the TreeAccelerationCache
+// having already been calculated. This includes:
+//
+// - logically, acceleration constraint errors (udoterrs), and constraint
+// multipliers, although in fact those arrays are provided as built-ins by
+// the State
+//
+// - constraint-generated body and mobility forces
+//
+// This cache entry can be calculated after Stage::Dynamics provided that
+// the SBTreeAccelerationCache entry has already been marked valid. We guarantee
+// this will have been calculated by the end of Stage::Acceleration.
+
+class SBConstrainedAccelerationCache {
+public:
+ // udoterr and multiplier cache space is provided directly by the State.
+
+ // These are ordered by ConstraintIndex, and then by ConstrainedBodyIndex
+ // within the constraint. Note that they have been re-expressed in G if
+ // necessary, although they still act at the constrained body origin.
+ // The same system mobilized body may appear more than once in this list
+ // if it is affected by multiple Constraints.
+ Array_<SpatialVec> constrainedBodyForcesInG; // [ncb]
+ // Ordered by ConstraintIndex, and then by ConstrainedUIndex within
+ // the constraint (and those are grouped in order of ConstrainedMobilizer
+ // for that Constraint). The same system mobility may appear more than once
+ // in this list if it is involved in multiple constraints.
+ Array_<Real> constraintMobilityForces; // [ncu]
+
+public:
+ void allocate(const SBTopologyCache&,
+ const SBModelCache&,
+ const SBInstanceCache& instance)
+ {
+ const int ncb = instance.totalNConstrainedBodiesInUse;
+ const int ncu = instance.totalNConstrainedUInUse;
+
+ constrainedBodyForcesInG.resize(ncb);
+ constraintMobilityForces.resize(ncu);
+ }
+};
+//...................... CONSTRAINED ACCELERATION CACHE ........................
+
+
+
+
+/*
+ * Generalized state variable collection for a SimbodyMatterSubsystem.
+ * Variables are divided into Stages, according to when their values
+ * are needed during a calculation. The Stages are:
+ * (Topology: not part of the state. These are the bodies, mobilizers,
+ * and topological constraints.)
+ * Model: choice of coordinates, knowns & unknowns, methods, etc.
+ * Instance: setting of physical parameters, e.g. mass
+ * (Time: currently there are no time-dependent states or computations)
+ * Position: position and orientation values q (2nd order continuous)
+ * Velocity: generalized speeds u
+ * Dynamics: dynamic quantities & operators available
+ * Acceleration: applied forces and prescribed accelerations
+ * Report: used by study for end-user reporting only; no effect on
+ * results
+ *
+ */
+
+
+// =============================================================================
+// MODEL VARS
+// =============================================================================
+// This state variable is allocated during realizeTopology(). Any change made
+// to it after that invalidates Stage::Model, requiring realizeModel() to be
+// performed.
+class SBModelVars {
+public:
+ bool useEulerAngles;
+public:
+
+ // We have to allocate these without looking at any other
+ // state variable or cache entries. We can only depend on topological
+ // information.
+ void allocate(const SBTopologyCache& tree) {
+ useEulerAngles = false;
+ }
+
+};
+
+
+
+// =============================================================================
+// INSTANCE VARS
+// =============================================================================
+// This state variable is allocated during realizeTopology(), because its
+// contents refer only to elements that form part of the fixed topology of the
+// matter subsystem -- mobilized bodies, particles, and constraints that are
+// specified as permanent parts of this matter subsystem.
+//
+// Any change to this variable invalidates Stage::Instance (not Stage::Model),
+// requiring realize(Instance) to be performed.
+//
+// Note: we may at some point have instance variables whose allocation is
+// deferred until realizeModel() but those would be wiped out whenever a change
+// to a Model-stage variable is made (most notably useEulerAngles).
+class SBInstanceVars {
+public:
+ Array_<MassProperties,MobilizedBodyIndex> bodyMassProperties;
+ Array_<Transform, MobilizedBodyIndex> outboardMobilizerFrames;
+ Array_<Transform, MobilizedBodyIndex> inboardMobilizerFrames;
+
+ Array_<Motion::Level, MobilizedBodyIndex> mobilizerLockLevel;
+ Vector lockedQs;
+ Vector lockedUs; // also used for udot
+
+ Array_<bool, MobilizedBodyIndex> prescribedMotionIsDisabled;
+
+ Vector particleMasses;
+
+ Array_<bool,ConstraintIndex> constraintIsDisabled;
+
+public:
+
+ void allocate(const SBTopologyCache& topology) {
+ const int nb = topology.nBodies;
+ const int np = topology.nParticles;
+ const int nc = topology.nConstraints;
+
+ // Clear first to make sure all entries are reset to default values.
+ bodyMassProperties.clear();
+ bodyMassProperties.resize(nb, MassProperties(1,Vec3(0),Inertia(1)));
+
+ outboardMobilizerFrames.clear();
+ outboardMobilizerFrames.resize(nb, Transform());
+
+ inboardMobilizerFrames.clear();
+ inboardMobilizerFrames.resize(nb, Transform());
+
+ mobilizerLockLevel.clear();
+ mobilizerLockLevel.resize(nb, Motion::NoLevel);
+
+ lockedQs.clear(); // must wait until realize(Model) to size these
+ lockedUs.clear();
+
+ prescribedMotionIsDisabled.clear();
+ prescribedMotionIsDisabled.resize(nb, false);
+
+ particleMasses.resize(np);
+ particleMasses = 1;
+
+ constraintIsDisabled.clear();
+ constraintIsDisabled.resize(nc, false);
+ }
+
+};
+
+
+
+// =============================================================================
+// TIME VARS
+// =============================================================================
+class SBTimeVars {
+public:
+ // none
+public:
+ void allocate(const SBTopologyCache&) {
+ }
+};
+
+
+// =============================================================================
+// POSITION VARS
+// =============================================================================
+class SBPositionVars {
+public:
+ // none here -- q is supplied directly by the State
+public:
+ void allocate(const SBTopologyCache& tree) {
+ }
+};
+
+
+// =============================================================================
+// VELOCITY VARS
+// =============================================================================
+class SBVelocityVars {
+public:
+ // none here -- u is supplied directly by the State
+public:
+ void allocate(const SBTopologyCache&) {
+ }
+};
+
+
+// =============================================================================
+// DYNAMICS VARS
+// =============================================================================
+class SBDynamicsVars {
+public:
+ // none here -- z is supplied directly by the State, but not
+ // used by the SimbodyMatterSubsystem anyway
+public:
+ void allocate(const SBTopologyCache&) {
+ }
+};
+
+
+
+// =============================================================================
+// ACCELERATION VARS
+// =============================================================================
+class SBAccelerationVars {
+public:
+ // none here
+public:
+ void allocate(const SBTopologyCache& topology) {
+ }
+};
+
+
+ /////////////////////
+ // SB STATE DIGEST //
+ /////////////////////
+
+/*
+ * Objects of this class are constructed for a particular State, and then used
+ * briefly for a related series of computations. Depending on the stage to which the
+ * State has been advanced, and the computations to be performed, some or all
+ * of the pointers here will be set to refer to State and State cache data
+ * for Simbody, of the types defined above.
+ *
+ * The idea is to do all the time consuming work of digging through the State
+ * just once, then use the results repeatedly for computations which are typically
+ * performed over all the nodes in the system. The low-level rigid body node computations
+ * assume already-digested States.
+ */
+class SBStateDigest {
+public:
+ explicit SBStateDigest(const State& s) : state(s), modifiableState(0), stage(Stage::Empty)
+ {
+ }
+ SBStateDigest(const State& s, const SimbodyMatterSubsystemRep& matter, Stage g)
+ : state(s), modifiableState(0), stage(Stage::Empty)
+ {
+ fillThroughStage(matter,g);
+ }
+ SBStateDigest(State& s, const SimbodyMatterSubsystemRep& matter, Stage g)
+ : state(s), modifiableState(&s), stage(Stage::Empty)
+ {
+ fillThroughStage(matter,g);
+ }
+
+ // Stage g here is the stage we are about to compute. So we expect the referenced
+ // State to have been realized to at least stage g-1.
+ void fillThroughStage(const SimbodyMatterSubsystemRep& matter, Stage g);
+
+ // The State is read only, for cache entries you have a choice.
+
+ const State& getState() const {return state;}
+ State& updState() {
+ assert(modifiableState);
+ return *modifiableState;
+ }
+ Stage getStage() const {return stage;}
+
+ const SBModelVars& getModelVars() const {
+ assert(stage >= Stage::Model);
+ assert(mv);
+ return *mv;
+ }
+
+ const SBInstanceVars& getInstanceVars() const {
+ assert(stage >= Stage::Model);
+ assert(iv);
+ return *iv;
+ }
+
+ const SBTimeVars& getTimeVars() const {
+ assert(stage >= Stage::Time);
+ assert(tv);
+ return *tv;
+ }
+
+ const Vector& getQ() const {
+ assert(stage >= Stage::Position);
+ assert(q);
+ return *q;
+ }
+
+ const SBPositionVars& getPositionVars() const {
+ assert(stage >= Stage::Position);
+ assert(pv);
+ return *pv;
+ }
+
+ const Vector& getU() const {
+ assert(stage >= Stage::Velocity);
+ assert(u);
+ return *u;
+ }
+
+ const SBVelocityVars& getVelocityVars() const {
+ assert(stage >= Stage::Velocity);
+ assert(vv);
+ return *vv;
+ }
+ const SBDynamicsVars& getDynamicsVars() const {
+ assert(stage >= Stage::Dynamics);
+ assert(dv);
+ return *dv;
+ }
+ const SBAccelerationVars& getAccelerationVars() const {
+ assert(stage >= Stage::Acceleration);
+ assert(av);
+ return *av;
+ }
+
+ // You can access the cache for update only at the stage being computed.
+ // You can access the cache read-only for any stage already completed.
+ // Either way you only need const access to the SBStateDigest object.
+
+ // Model
+ SBModelCache& updModelCache() const {
+ assert(stage == Stage::Model);
+ assert(mc);
+ return *mc;
+ }
+ const SBModelCache& getModelCache() const {
+ assert(stage > Stage::Model);
+ assert(mc);
+ return *mc;
+ }
+
+ // Instance
+ SBInstanceCache& updInstanceCache() const {
+ assert(stage == Stage::Instance);
+ assert(ic);
+ return *ic;
+ }
+ const SBInstanceCache& getInstanceCache() const {
+ assert(stage > Stage::Instance);
+ assert(ic);
+ return *ic;
+ }
+
+ // Time
+ SBTimeCache& updTimeCache() const {
+ assert(stage >= Stage::Instance && stage <= Stage::Time);
+ assert(tc);
+ return *tc;
+ }
+ const SBTimeCache& getTimeCache() const {
+ assert(stage > Stage::Time);
+ assert(tc);
+ return *tc;
+ }
+
+ // Position
+ Vector& updQErr() const {
+ assert(stage == Stage::Position);
+ assert(qErr);
+ return *qErr;
+ }
+ const Vector& getQErr() const {
+ assert(stage > Stage::Position);
+ assert(qErr);
+ return *qErr;
+ }
+ SBTreePositionCache& updTreePositionCache() const {
+ assert(stage >= Stage::Instance && stage <= Stage::Position);
+ assert(tpc);
+ return *tpc;
+ }
+ const SBTreePositionCache& getTreePositionCache() const {
+ assert(stage > Stage::Position);
+ assert(tpc);
+ return *tpc;
+ }
+ SBConstrainedPositionCache& updConstrainedPositionCache() const {
+ assert(stage >= Stage::Instance && stage <= Stage::Position);
+ assert(cpc);
+ return *cpc;
+ }
+ const SBConstrainedPositionCache& getConstrainedPositionCache() const {
+ assert(stage > Stage::Position);
+ assert(cpc);
+ return *cpc;
+ }
+
+ // Velocity
+ Vector& updQDot() const {
+ assert(stage == Stage::Velocity);
+ assert(qdot);
+ return *qdot;
+ }
+ const Vector& getQDot() const {
+ assert(stage > Stage::Velocity);
+ assert(qdot);
+ return *qdot;
+ }
+ Vector& updUErr() const {
+ assert(stage == Stage::Velocity);
+ assert(uErr);
+ return *uErr;
+ }
+ const Vector& getUErr() const {
+ assert(stage > Stage::Velocity);
+ assert(uErr);
+ return *uErr;
+ }
+ SBTreeVelocityCache& updTreeVelocityCache() const {
+ assert(stage >= Stage::Instance && stage <= Stage::Velocity);
+ assert(tvc);
+ return *tvc;
+ }
+ const SBTreeVelocityCache& getTreeVelocityCache() const {
+ assert(stage > Stage::Velocity);
+ assert(tvc);
+ return *tvc;
+ }
+ SBConstrainedVelocityCache& updConstrainedVelocityCache() const {
+ assert(stage >= Stage::Instance && stage <= Stage::Velocity);
+ assert(cvc);
+ return *cvc;
+ }
+ const SBConstrainedVelocityCache& getConstrainedVelocityCache() const {
+ assert(stage > Stage::Velocity);
+ assert(cvc);
+ return *cvc;
+ }
+
+ // Dynamics
+ SBDynamicsCache& updDynamicsCache() const {
+ assert(stage >= Stage::Instance && stage <= Stage::Dynamics);
+ assert(dc);
+ return *dc;
+ }
+ const SBDynamicsCache& getDynamicsCache() const {
+ assert(stage > Stage::Dynamics);
+ assert(dc);
+ return *dc;
+ }
+ Vector& updUDot() const {
+ assert(stage >= Stage::Dynamics);
+ assert(udot);
+ return *udot;
+ }
+ const Vector& getUDot() const {
+ assert(stage > Stage::Dynamics);
+ assert(udot);
+ return *udot;
+ }
+ Vector& updQDotDot() const {
+ assert(stage >= Stage::Dynamics);
+ assert(qdotdot);
+ return *qdotdot;
+ }
+ const Vector& getQDotDot() const {
+ assert(stage > Stage::Dynamics);
+ assert(qdotdot);
+ return *qdotdot;
+ }
+
+ // Accelerations
+
+ Vector& updUDotErr() const {
+ assert(stage == Stage::Acceleration);
+ assert(udotErr);
+ return *udotErr;
+ }
+ const Vector& getUDotErr() const {
+ assert(stage > Stage::Acceleration);
+ assert(udotErr);
+ return *udotErr;
+ }
+ SBTreeAccelerationCache& updTreeAccelerationCache() const {
+ assert(stage >= Stage::Instance && stage <= Stage::Acceleration);
+ assert(tac);
+ return *tac;
+ }
+ const SBTreeAccelerationCache& getTreeAccelerationCache() const {
+ assert(stage > Stage::Acceleration);
+ assert(tac);
+ return *tac;
+ }
+ SBConstrainedAccelerationCache& updConstrainedAccelerationCache() const {
+ assert(stage >= Stage::Instance && stage <= Stage::Acceleration);
+ assert(cac);
+ return *cac;
+ }
+ const SBConstrainedAccelerationCache& getConstrainedAccelerationCache() const {
+ assert(stage > Stage::Acceleration);
+ assert(cac);
+ return *cac;
+ }
+
+ void clear() {
+ // state
+ mv=0; iv=0; tv=0;
+ q=0; pv=0;
+ u=0; vv=0;
+ dv=0;
+ av=0;
+
+ // cache
+ mc=0; ic=0; tc=0;
+ qErr=0; tpc=0; cpc=0;
+ qdot=uErr=0; tvc=0; cvc=0;
+ dc=0;
+ udot=qdotdot=udotErr=0; tac=0; cac=0;
+ }
+
+private:
+ const State& state;
+ State* modifiableState;
+ Stage stage; // the stage to be computed
+
+ const SBModelVars* mv;
+ const SBInstanceVars* iv;
+ const SBTimeVars* tv;
+
+ const Vector* q;
+ const SBPositionVars* pv;
+
+ const Vector* u;
+ const SBVelocityVars* vv;
+ const SBDynamicsVars* dv;
+ const SBAccelerationVars* av;
+
+ SBModelCache* mc;
+ SBInstanceCache* ic;
+ SBTimeCache* tc;
+
+ Vector* qErr;
+ SBTreePositionCache* tpc;
+ SBConstrainedPositionCache* cpc;
+
+ Vector* qdot;
+ Vector* uErr;
+ SBTreeVelocityCache* tvc;
+ SBConstrainedVelocityCache* cvc;
+
+ SBDynamicsCache* dc;
+
+ Vector* udot;
+ Vector* qdotdot;
+ Vector* udotErr;
+ SBTreeAccelerationCache* tac;
+ SBConstrainedAccelerationCache* cac;
+};
+
+#endif // SimTK_SIMBODY_TREE_STATE_H_
diff --git a/Simbody/src/TextDataEventReporter.cpp b/Simbody/src/TextDataEventReporter.cpp
new file mode 100644
index 0000000..ae917f6
--- /dev/null
+++ b/Simbody/src/TextDataEventReporter.cpp
@@ -0,0 +1,99 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "simbody/internal/TextDataEventReporter.h"
+
+using std::cout;
+using std::endl;
+using namespace SimTK;
+
+/**
+ * This class is the internal implementation for TextDataEventReporter. It has two subclasses,
+ * one for UserFunctions that return a Real and one for UserFunctions that return a Vector.
+ */
+
+class TextDataEventReporter::TextDataEventReporterRep {
+public:
+ TextDataEventReporterRep(const System& system) : system(system) {
+ }
+ virtual ~TextDataEventReporterRep() { }
+ virtual void printValues(const State& state) const = 0;
+ void handleEvent(const State& state) const {
+ cout << state.getTime();
+ printValues(state);
+ cout << endl;
+ }
+ TextDataEventReporter* handle;
+ const System& system;
+ Real reportInterval;
+ class RealFunction;
+ class VectorFunction;
+};
+
+class TextDataEventReporter::TextDataEventReporterRep::RealFunction : public TextDataEventReporter::TextDataEventReporterRep {
+public:
+ RealFunction(const System& system, UserFunction<Real>* function) : TextDataEventReporterRep(system), function(function) {
+ }
+ ~RealFunction() {
+ delete function;
+ }
+ void printValues(const State& state) const {
+ Real value = function->evaluate(system, state);
+ cout << "\t" << value;
+ }
+ UserFunction<Real>* function;
+};
+
+class TextDataEventReporter::TextDataEventReporterRep::VectorFunction : public TextDataEventReporter::TextDataEventReporterRep {
+public:
+ VectorFunction(const System& system, UserFunction<Vector>* function) : TextDataEventReporterRep(system), function(function) {
+ }
+ ~VectorFunction() {
+ delete function;
+ }
+ void printValues(const State& state) const {
+ Vector values = function->evaluate(system, state);
+ for (int i = 0; i < values.size(); ++i)
+ cout << "\t" << values[i];
+ }
+ UserFunction<Vector>* function;
+};
+
+TextDataEventReporter::TextDataEventReporter(const System& system, UserFunction<Real>* function, Real reportInterval) : PeriodicEventReporter(reportInterval) {
+ rep = new TextDataEventReporterRep::RealFunction(system, function);
+ updRep().handle = this;
+}
+
+TextDataEventReporter::TextDataEventReporter(const System& system, UserFunction<Vector>* function, Real reportInterval) : PeriodicEventReporter(reportInterval) {
+ rep = new TextDataEventReporterRep::VectorFunction(system, function);
+ updRep().handle = this;
+}
+
+TextDataEventReporter::~TextDataEventReporter() {
+ if (rep->handle == this)
+ delete rep;
+}
+
+void TextDataEventReporter::handleEvent(const State& state) const {
+ updRep().handleEvent(state);
+}
diff --git a/Simbody/staticTarget/CMakeLists.txt b/Simbody/staticTarget/CMakeLists.txt
new file mode 100644
index 0000000..9aacb37
--- /dev/null
+++ b/Simbody/staticTarget/CMakeLists.txt
@@ -0,0 +1,63 @@
+## This whole directory exists just so I could define these extra
+## preprocessor values.
+
+ADD_DEFINITIONS(-DSimTK_SIMBODY_BUILDING_STATIC_LIBRARY
+ -DSimTK_USE_STATIC_LIBRARIES)
+
+IF(BUILD_UNVERSIONED_LIBRARIES)
+
+ ADD_LIBRARY(${STATIC_TARGET} STATIC
+ ${SOURCE_FILES} ${SOURCE_INCLUDE_FILES}
+ ${API_ABS_INCLUDE_FILES})
+
+ TARGET_LINK_LIBRARIES(${STATIC_TARGET}
+ debug ${SimTKMATH_STATIC_LIBRARY}_d
+ optimized ${SimTKMATH_STATIC_LIBRARY}
+ debug ${SimTKCOMMON_STATIC_LIBRARY}_d
+ optimized ${SimTKCOMMON_STATIC_LIBRARY}
+ ${MATH_LIBS_TO_USE})
+
+ SET_TARGET_PROPERTIES(${STATIC_TARGET} PROPERTIES
+ PROJECT_LABEL "Code - ${STATIC_TARGET}")
+
+ # install library
+
+ INSTALL(TARGETS ${STATIC_TARGET}
+ PERMISSIONS OWNER_READ OWNER_WRITE
+ GROUP_READ GROUP_WRITE
+ WORLD_READ
+ ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ RUNTIME DESTINATION ${CMAKE_INSTALL_FULL_BINDIR})
+
+ENDIF(BUILD_UNVERSIONED_LIBRARIES)
+
+
+
+IF(BUILD_VERSIONED_LIBRARIES)
+
+ ADD_LIBRARY(${STATIC_TARGET_VN} STATIC
+ ${SOURCE_FILES} ${SOURCE_INCLUDE_FILES}
+ ${API_ABS_INCLUDE_FILES})
+
+ TARGET_LINK_LIBRARIES(${STATIC_TARGET_VN}
+ debug ${SimTKMATH_STATIC_LIBRARY_VN}_d
+ optimized ${SimTKMATH_STATIC_LIBRARY_VN}
+ debug ${SimTKCOMMON_STATIC_LIBRARY_VN}_d
+ optimized ${SimTKCOMMON_STATIC_LIBRARY_VN}
+ ${MATH_LIBS_TO_USE_VN})
+
+ SET_TARGET_PROPERTIES(${STATIC_TARGET_VN} PROPERTIES
+ PROJECT_LABEL "Code - ${STATIC_TARGET_VN}")
+
+ # install library
+
+ INSTALL(TARGETS ${STATIC_TARGET_VN}
+ PERMISSIONS OWNER_READ OWNER_WRITE
+ GROUP_READ GROUP_WRITE
+ WORLD_READ
+ ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}
+ RUNTIME DESTINATION ${CMAKE_INSTALL_FULL_BINDIR})
+
+ENDIF(BUILD_VERSIONED_LIBRARIES)
diff --git a/Simbody/tests/CMakeLists.txt b/Simbody/tests/CMakeLists.txt
new file mode 100644
index 0000000..f8c738e
--- /dev/null
+++ b/Simbody/tests/CMakeLists.txt
@@ -0,0 +1,56 @@
+# Adhoc tests are those test or demo programs which are not intended,
+# or not ready, to be part of the regression suite.
+ADD_SUBDIRECTORY(adhoc)
+
+# Generate regression tests.
+#
+# This is boilerplate code for generating a set of executables, one per
+# .cpp file in a "tests" directory. These are for test cases which
+# have been engineered to be suitable as part of the nightly regression
+# test suite. Ideally, they know their correct answers and return
+# a simple thumbs-up/thumbs-down result, although some regressions
+# may be present and useful just to determine if they compile and
+# run to completion.
+#
+# For IDEs that can deal with PROJECT_LABEL properties (at least
+# Visual Studio) the projects for building each of these adhoc
+# executables will be labeled "Regr - TheTestName" if a file
+# TheTestName.cpp is found in this directory.
+#
+# We check the BUILD_TESTS_AND_EXAMPLES_{SHARED,STATIC} variables to determine
+# whether to build dynamically linked, statically linked, or both
+# versions of the executable.
+
+FILE(GLOB REGR_TESTS "*.cpp")
+FOREACH(TEST_PROG ${REGR_TESTS})
+ GET_FILENAME_COMPONENT(TEST_ROOT ${TEST_PROG} NAME_WE)
+
+ IF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+ # Link with shared library
+ ADD_EXECUTABLE(${TEST_ROOT} ${TEST_PROG})
+ SET_TARGET_PROPERTIES(${TEST_ROOT}
+ PROPERTIES
+ PROJECT_LABEL "Test_Regr - ${TEST_ROOT}")
+ TARGET_LINK_LIBRARIES(${TEST_ROOT}
+ ${TEST_SHARED_TARGET})
+ ADD_TEST(${TEST_ROOT} ${EXECUTABLE_OUTPUT_PATH}/${TEST_ROOT})
+ ENDIF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+
+ IF (BUILD_STATIC_LIBRARIES AND BUILD_TESTS_AND_EXAMPLES_STATIC)
+ # Link with static library
+ SET(TEST_STATIC ${TEST_ROOT}Static)
+ ADD_EXECUTABLE(${TEST_STATIC} ${TEST_PROG})
+ SET_TARGET_PROPERTIES(${TEST_STATIC}
+ PROPERTIES
+ COMPILE_FLAGS "-DSimTK_USE_STATIC_LIBRARIES"
+ PROJECT_LABEL "Test_Regr - ${TEST_STATIC}")
+ TARGET_LINK_LIBRARIES(${TEST_STATIC}
+ ${TEST_STATIC_TARGET})
+ ADD_TEST(${TEST_STATIC} ${EXECUTABLE_OUTPUT_PATH}/${TEST_STATIC})
+ ENDIF()
+
+ENDFOREACH(TEST_PROG ${REGR_TESTS})
+
+
+
+
diff --git a/Simbody/tests/GazeboInelasticCollision.cpp b/Simbody/tests/GazeboInelasticCollision.cpp
new file mode 100644
index 0000000..5d367eb
--- /dev/null
+++ b/Simbody/tests/GazeboInelasticCollision.cpp
@@ -0,0 +1,319 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): Gazebo Inelastic Collision *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* This test is drawn from the Open Source Robotics Foundation Gazebo physics
+regression test "CollisionTest". In the original test there is a cube and
+a sphere, both of unit mass and same half-dimension 0.5. Initially they are 1
+unit apart in x, with the block to the left of the sphere. The block is
+accelerated to the right with a discrete applied force of 1000 applied for 1ms
+for a total impulse of 1, which should produce a velocity of +1 that is steady
+until the block hits the stationary sphere. The collision is supposed to be
+fully inelastic (coefficient of restitution=0), so there should be no rebound
+and the two objects should move off to the right together at half the impact
+velocity.
+
+The test runs with fixed 1ms steps and calculates the expected result by
+integrating manually. The position and velocity changes during the first step
+with the 1000 unit force active are
+ x(.001) = 1/2 a t^2 = 1/2 1e3 1e-6 = .0005 length units
+ v(.001) = a t = 1e3 1e-3 = 1 velocity unit
+After that first step, all subsequent steps prior to the collision have
+ deltaX = v t = 1 1e-3 = .001 length units
+ deltaV = 0
+We would like the collision to occur exactly at 1s, that is, between steps 1000
+and 1001. The position x(1)=.0005 + 999*.001=.9995. So if we were going to apply
+the entire impulse during the first step we would place the sphere initially so
+that the block surface and sphere surface are separated by .9995. But read on.
+
+NOTE: to avoid problems with first-order integrators (explanation below), we're
+modifying the test here to apply 1/10th of the impulse over the first 10 steps
+(.01 seconds) rather than all in the first step. Then
+ x(.01) = 1/2 a t^2 = 1/2 1e2 1e-4 = .005 length units
+ v(.01) = a t = 1e2 1e-2 = 1 velocity unit
+At 1s we'll have x(1)=.005 + 990*.001=.995. So we'll set the initial separation
+to .995 to get a collision at 1s (between steps 1000 and 1001).
+
+Why first order integrators cause trouble
+-----------------------------------------
+While any integrator can get the velocity correct in one step, no first order
+integrator will be able to calculate x(.001) correctly in a single step.
+ Explicit Euler: v(.001)=v(0) + h*a(0)=1
+ x(.001)=x(0) + h*v(0)=0 <--
+
+ Semi-explicit Euler: v(.001)=v(0) + h*a(0)=1
+ x(.001)=x(0) + h*v(.001)=.001. <--
+
+ Semi-explicit Euler 2: v'(.0005)=v(0)+h/2 a(0)=.5
+ x'(.0005)=x(0)+h/2 v'(.0005)=.00025
+ v(.001)=v'(.0005)+h/2 a'(.0005)=1
+ x(.001)=x'(.0005)+h/2 v(.001)=.00075 <--
+
+But any 2nd order integrator would give the correct result, for example
+Explicit trapezoid rule: v'(.001)=v(0)+h*a(0)=1
+ x(.001)=x(0)+h*(v(0)+v'(.001))/2 =.0005 <--
+
+Explicit midpoint rule: v'(.0005)=v(0)+h/2*a(0)=1/2
+ x(.001)=x(0)+h*v'(.0005)=.0005 <--
+
+By spreading the impulse over several steps we substantially reduce the overall
+error. For example, after ten steps we get
+ ExplicitEuler: x(.01)=.0045
+ Semi-explicit Euler: x(.01)=.0055
+ Semi-explicit Euler 2: x(.01)=.00525
+any of which we'll deem close enough to the right answer of .005.
+*/
+
+#include "Simbody.h"
+
+#include <cassert>
+#include <iostream>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+//#define USE_VISUALIZER
+
+// Write interesting integrator info to stdout.
+static void dumpIntegratorStats(const Integrator& integ);
+
+const Real Radius = 0.5;
+const Real Mass = 1;
+// Define an extremely stiff, lossy material.
+const Real Stiffness = 1e8;
+const Real Dissipation = 1000;
+
+const Real MaxStepSize = Real(1/1000.); // 1 ms (1000 Hz)
+const int DrawEveryN = 33; // 33 ms frame update (30.3 Hz)
+const Real SimTime = 2;
+const int NSteps = // make this a whole number of viz frames
+ DrawEveryN*(int(SimTime/MaxStepSize/DrawEveryN+0.5));
+
+const Real TotalImpulse = 1; // Applied only on the first step
+const Real StepForce = TotalImpulse/MaxStepSize;
+
+const Real IntegAccuracy = 1e-3;
+const Real CheckAccuracy = 1e-3;
+
+struct MyMultibodySystem {
+ MyMultibodySystem()
+ : m_system(), m_matter(m_system),
+ m_tracker(m_system), m_contact(m_system,m_tracker),
+ m_forces(m_system), m_discrete(m_forces,m_matter)
+ #ifdef USE_VISUALIZER
+ , m_viz(m_system)
+ #endif
+ {
+ ContactMaterial lossyMaterial(Stiffness,
+ Dissipation,
+ 0, // mu_static
+ 0, // mu_dynamic
+ 0); // mu_viscous
+
+ // no gravity
+ Body::Rigid sphereBody
+ (MassProperties(Mass, Vec3(0), UnitInertia::sphere(Radius)));
+ sphereBody.addDecoration(Transform(), DecorativeSphere(Radius));
+ sphereBody.addContactSurface(Transform(),
+ ContactSurface(ContactGeometry::Sphere(Radius),
+ lossyMaterial));
+
+ // TODO: using inscribed sphere as contact shape within the block.
+ Body::Rigid cubeBody
+ (MassProperties(Mass, Vec3(0), UnitInertia::sphere(Radius)));
+ cubeBody.addDecoration(Transform(), DecorativeBrick(Vec3(Radius)));
+ cubeBody.addContactSurface(Transform(),
+ ContactSurface(ContactGeometry::Sphere(Radius), // TODO!
+ lossyMaterial));
+
+ MobilizedBody Ground = m_matter.Ground();
+
+ m_cube = MobilizedBody::Slider(Ground, Transform(Vec3(0,2,0)),
+ cubeBody, Transform(Vec3(0)));
+ m_sphere = MobilizedBody::Slider(Ground, Transform(Vec3(2-.005,2,0)),
+ sphereBody, Transform(Vec3(0)));
+
+ m_system.realizeTopology();
+
+ #ifdef USE_VISUALIZER
+ m_viz.setShowFrameNumber(true);
+ m_viz.setShowSimTime(true);
+ #endif
+ }
+
+ MultibodySystem m_system;
+ SimbodyMatterSubsystem m_matter;
+ ContactTrackerSubsystem m_tracker;
+ CompliantContactSubsystem m_contact;
+ GeneralForceSubsystem m_forces;
+ Force::DiscreteForces m_discrete;
+ #ifdef USE_VISUALIZER
+ Visualizer m_viz;
+ #endif
+
+ MobilizedBody m_cube;
+ MobilizedBody m_sphere;
+};
+
+void runOnce(const MyMultibodySystem& mbs, Integrator& integ, int nsteps)
+{
+ integ.setAllowInterpolation(false);
+ integ.setAccuracy(IntegAccuracy);
+ integ.initialize(mbs.m_system.getDefaultState());
+
+ printf(
+ "\n--------------------------------------------------------------------\n");
+ printf(
+ "Test with order %d integator %s, Accuracy=%g, MaxStepSize=%g #steps=%d\n",
+ integ.getMethodMinOrder(), integ.getMethodName(),
+ integ.getAccuracyInUse(), MaxStepSize, nsteps);
+
+ // These variables are the manually calculated values for the cube's
+ // x coordinate and x velocity in Ground. We'll calculate these assuming
+ // a perfect inelastic collision occurring at t=1 and then compare with
+ // the approximate solution produced by the integrator.
+ Real x=0, v=0;
+
+ unsigned stepNum = 0;
+ while (true) {
+ // Get access to State being advanced by the integrator. Interpolation
+ // must be off so that we're modifying the actual trajectory.
+ State& state = integ.updAdvancedState();
+
+ #ifdef USE_VISUALIZER
+ // Output a frame to the Visualizer if it is time.
+ if (stepNum % DrawEveryN == 0)
+ mbs.m_viz.report(state);
+ #endif
+
+ if (stepNum == nsteps)
+ break;
+
+ ++stepNum;
+
+ mbs.m_discrete.clearAllBodyForces(state);
+ if (stepNum <= 10)
+ mbs.m_discrete.setOneBodyForce(state, mbs.m_cube,
+ SpatialVec(Vec3(0), Vec3(StepForce/10,0,0)));
+
+ // Advance time by MaxStepSize. Might take multiple internal steps to
+ // get there, depending on difficulty and required accuracy.
+ const Real tNext = stepNum * MaxStepSize;
+ do {integ.stepTo(tNext,tNext);} while (integ.getTime() < tNext);
+
+ // From Gazebo test code in physics.cc:
+ // integrate here to see when the collision should happen
+ if (stepNum <= 10) {
+ const Real a = StepForce/10/Mass; // a is acceleration
+ x += v*MaxStepSize + a * square(MaxStepSize)/2; // dx = 1/2 a t^2
+ v += a * MaxStepSize; // dv = a t
+ } else {
+ const Real impulse = StepForce*MaxStepSize;
+ if (stepNum >= 1000)
+ v = impulse / (2*Mass); //inelastic col. w/equal mass
+ x += v * MaxStepSize;
+ }
+
+ mbs.m_system.realize(state);
+ //printf("after step %d t=%g (h=%g): px=%g,%g vx=%g,%g ax=%g,%g\n",
+ // stepNum, state.getTime(),integ.getPreviousStepSizeTaken(),
+ // mbs.m_cube.getBodyOriginLocation(state)[0],
+ // mbs.m_sphere.getBodyOriginLocation(state)[0],
+ // mbs.m_cube.getBodyOriginVelocity(state)[0],
+ // mbs.m_sphere.getBodyOriginVelocity(state)[0],
+ // mbs.m_cube.getBodyOriginAcceleration(state)[0],
+ // mbs.m_sphere.getBodyOriginAcceleration(state)[0]);
+ //printf(" x=%g v=%g\n", x, v);
+
+ // Allow some uncertainty between step 1000 and 1001.
+ if (stepNum != 1000) {
+ SimTK_TEST_EQ_TOL(mbs.m_cube.getBodyOriginLocation(state)[0], x,
+ CheckAccuracy);
+ SimTK_TEST_EQ_TOL(mbs.m_cube.getBodyOriginVelocity(state)[0], v,
+ CheckAccuracy);
+ }
+
+
+ }
+
+ dumpIntegratorStats(integ);
+}
+
+int main() {
+ SimTK_START_TEST("GazeboInelasticCollision");
+ // Create the system.
+ MyMultibodySystem mbs;
+
+ RungeKuttaMersonIntegrator rkm(mbs.m_system);
+ RungeKutta3Integrator rk3(mbs.m_system);
+ RungeKutta2Integrator rk2(mbs.m_system);
+ SemiExplicitEuler2Integrator sexpeul2(mbs.m_system);
+ ExplicitEulerIntegrator expeul(mbs.m_system);
+
+ // Fixed step integrators can't adjust to the compliant impact needs.
+ SemiExplicitEulerIntegrator sexpeul1(mbs.m_system, MaxStepSize);
+ ExplicitEulerIntegrator expeulFixed(mbs.m_system, MaxStepSize);
+
+
+ SimTK_SUBTEST3(runOnce, mbs, rkm, NSteps);
+ SimTK_SUBTEST3(runOnce, mbs, rk3, NSteps);
+ SimTK_SUBTEST3(runOnce, mbs, rk2, NSteps);
+ SimTK_SUBTEST3(runOnce, mbs, sexpeul2, NSteps);
+ SimTK_SUBTEST3(runOnce, mbs, expeul, NSteps);
+
+ // These are fixed-step integrators so can't handle the
+ // compliant impact which demands a few smaller steps for stability
+ SimTK_SUBTEST3(runOnce, mbs, sexpeul1, 990); // stop before impact
+ SimTK_SUBTEST3(runOnce, mbs, expeulFixed, 990);
+
+ //sherm 130617: this isn't accurate enough; I think it has a serious
+ // bug in its error estimator
+ //VerletIntegrator verlet(mbs.m_system);
+ //SimTK_SUBTEST2(runOnce, mbs, verlet);
+
+ //this has poor accuracy control also
+ //RungeKuttaFeldbergIntegrator rkf(mbs.m_system);
+ //SimTK_SUBTEST3(runOnce, mbs, rkf, NSteps);
+
+
+ SimTK_END_TEST();
+}
+
+
+//==============================================================================
+// DUMP INTEGRATOR STATS
+//==============================================================================
+static void dumpIntegratorStats(const Integrator& integ) {
+ const int evals = integ.getNumRealizations();
+ std::cout << "\nDone -- simulated " << integ.getTime() << "s with "
+ << integ.getNumStepsTaken() << " steps, avg step="
+ << (1000*integ.getTime())/integ.getNumStepsTaken() << "ms "
+ << (1000*integ.getTime())/evals << "ms/eval\n";
+
+ printf("Used Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(),
+ integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(),
+ integ.getNumProjections());
+}
diff --git a/Simbody/tests/GazeboReactionForce.cpp b/Simbody/tests/GazeboReactionForce.cpp
new file mode 100644
index 0000000..95ee675
--- /dev/null
+++ b/Simbody/tests/GazeboReactionForce.cpp
@@ -0,0 +1,290 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): Gazebo Reaction Force *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman, John Hsu *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* This test is drawn from the Open Source Robotics Foundation Gazebo physics
+regression test "Joint_TEST::ForceTorque".
+
+It is a small sphere pinned at its center to ground, rotating about X, then
+an outboard brick pinned to the sphere and rotating about mutual Z. Oblique
+gravity cause the brick to fall, hit the ground and come to rest. Then we check
+against John Hsu's hand coded reaction force values.
+*/
+
+#include "Simbody.h"
+
+#include <cassert>
+#include <iostream>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+//#define USE_VISUALIZER
+
+const Real Radius=.1; // for sphere attached to link1
+const Vec3 Box(.05,.1,.2); // half-dimensions of box
+const Real Mass1=10, Mass2=10;
+const Vec3 COM1(0,0,.5), COM2(0,0,.5);
+const Inertia Central(Vec3(.1,.1,.1)); // supposedly mass weighted already
+// Simbody requires inertias to be expressed about body origin rather than COM.
+const Inertia Inertia1=Central.shiftFromMassCenter(-COM1, Mass1);
+const Inertia Inertia2=Central.shiftFromMassCenter(-COM2, Mass2);
+
+// Define a stiff, lossy material.
+const Real Stiffness = 1e8;
+const Real Dissipation = 10;
+const Real Mu_s = 0.15, Mu_d = 0.1, Mu_v = 0;
+const ContactMaterial lossyMaterial(Stiffness,
+ Dissipation,
+ Mu_s,
+ Mu_d,
+ Mu_v);
+
+const Real MaxStepSize = Real(1/1000.); // 1 ms (1000 Hz)
+const int DrawEveryN = 33; // 33 ms frame update (30.3 Hz)
+const Real SimTime = 2;
+const int NSteps = // make this a whole number of viz frames
+ DrawEveryN*(int(SimTime/MaxStepSize/DrawEveryN+0.5));
+
+// Use this class to hold references into the Simbody system.
+struct MyMultibodySystem {
+ MyMultibodySystem(); // see below
+
+ MultibodySystem m_system;
+ SimbodyMatterSubsystem m_matter;
+ ContactTrackerSubsystem m_tracker;
+ CompliantContactSubsystem m_contact;
+ GeneralForceSubsystem m_forces;
+ Force::Gravity m_gravity;
+
+ #ifdef USE_VISUALIZER
+ Visualizer m_viz;
+ #endif
+
+ MobilizedBody::Pin m_link1, m_link2;
+};
+
+// Execute the simulation with a given integrator and accuracy, and verify that
+// it produces the correct answers.
+static void runOnce(const MyMultibodySystem& mbs, Integrator& integ,
+ Real accuracy);
+
+
+//==============================================================================
+// MAIN
+//==============================================================================
+int main() {
+ SimTK_START_TEST("GazeboReactionForce");
+ // Create the system.
+ MyMultibodySystem mbs;
+
+ printf("LOW ACCURACY\n");
+ SemiExplicitEuler2Integrator sexpeul2(mbs.m_system);
+ SimTK_SUBTEST3(runOnce, mbs, sexpeul2, 1e-2);
+
+ printf("\nHIGH ACCURACY\n");
+ RungeKuttaMersonIntegrator rkm(mbs.m_system);
+ SimTK_SUBTEST3(runOnce, mbs, rkm, 1e-6);
+
+ SimTK_END_TEST();
+}
+
+
+//==============================================================================
+// MY MULTIBODY SYSTEM
+//==============================================================================
+// Construct the multibody system.
+MyMultibodySystem::MyMultibodySystem()
+: m_system(), m_matter(m_system),
+ m_tracker(m_system), m_contact(m_system,m_tracker),
+ m_forces(m_system), m_gravity(m_forces,m_matter,Vec3(-30,10,-50))
+ #ifdef USE_VISUALIZER
+ , m_viz(m_system)
+ #endif
+{
+ #ifdef USE_VISUALIZER
+ m_viz.setSystemUpDirection(ZAxis);
+ m_viz.setCameraTransform(
+ Transform(Rotation(BodyRotationSequence,Pi/2,XAxis,Pi/8,YAxis),
+ Vec3(5,-8,2)));
+ m_viz.setShowFrameNumber(true);
+ m_viz.setShowSimTime(true);
+ #endif
+
+ Body::Rigid link1Info(MassProperties(Mass1, COM1, Inertia1));
+ link1Info.addDecoration(COM1, DecorativeSphere(Radius).setOpacity(.3));
+
+ Body::Rigid link2Info(MassProperties(Mass2, COM2, Inertia2));
+ link2Info.addDecoration(COM2, DecorativeBrick(Box).setOpacity(.3));
+
+ ContactGeometry::TriangleMesh cubeMesh
+ (PolygonalMesh::createBrickMesh(Box, 3));
+ DecorativeMesh showMesh(cubeMesh.createPolygonalMesh());
+ showMesh.setRepresentation(DecorativeGeometry::DrawWireframe);
+ link2Info.addContactSurface(COM2,
+ ContactSurface(cubeMesh, lossyMaterial, 1));
+ link2Info.addDecoration(COM2, showMesh);
+
+ MobilizedBody& Ground = m_matter.updGround(); // Nicer name for Ground.
+ Ground.addBodyDecoration(Vec3(0,0,.01),
+ DecorativeFrame(2).setColor(Green));
+
+ // Add the Ground contact geometry. Contact half space has -XAxis normal
+ // (right hand wall) so we have to rotate.
+ const Rotation NegXToZ(Pi/2, YAxis);
+ Ground.updBody().addContactSurface(Transform(NegXToZ,Vec3(0)),
+ ContactSurface(ContactGeometry::HalfSpace(),lossyMaterial));
+
+ const Rotation ZtoX(Pi/2, YAxis);
+ m_link1 = MobilizedBody::Pin(Ground,Transform(ZtoX,COM1),
+ link1Info, Transform(ZtoX,COM1));
+
+ m_link2 = MobilizedBody::Pin(m_link1, Vec3(0,0,1.5),
+ link2Info, Vec3(0));
+
+ Force::MobilityLinearStop
+ (m_forces, m_link1, MobilizerQIndex(0),1000000.,1.,-Pi/2,Pi/2);
+ Force::MobilityLinearStop
+ (m_forces, m_link2, MobilizerQIndex(0), 1000000.,1.,-Infinity,Infinity);
+
+ m_system.realizeTopology();
+
+}
+
+//==============================================================================
+// RUN ONCE
+//==============================================================================
+
+// Reaction force information: results are the joint reaction, at the F frame
+// on the parent and M frame on the child, expressed in the parent or child
+// frame, resp. Note that Gazebo's GetForceTorque() method uses the negation of
+// the joint reaction, and Gazebo's results are ordered (force,torque) rather
+// than (torque,force) as in a Simbody SpatialVec.
+struct ReactionPair {
+ SpatialVec reactionOnParentInParent;
+ SpatialVec reactionOnChildInChild;
+};
+static ReactionPair getReactionPair(const State& state,
+ const MobilizedBody& mobod);
+
+// Write interesting integrator info to stdout.
+static void dumpIntegratorStats(const Integrator& integ);
+
+// Run the system until it settles down, then check the answers.
+void runOnce(const MyMultibodySystem& mbs, Integrator& integ, Real accuracy)
+{
+ integ.setAllowInterpolation(false);
+ integ.setAccuracy(accuracy);
+ integ.initialize(mbs.m_system.getDefaultState());
+
+ printf("Test with order %d integator %s, Accuracy=%g, MaxStepSize=%g\n",
+ integ.getMethodMinOrder(), integ.getMethodName(),
+ integ.getAccuracyInUse(), MaxStepSize);
+
+ #ifdef USE_VISUALIZER
+ mbs.m_viz.report(integ.getState());
+ printf("Hit ENTER to simulate:");
+ getchar();
+ #endif
+
+ unsigned stepNum = 0;
+ while (true) {
+ // Get access to State being advanced by the integrator. Interpolation
+ // must be off so that we're modifying the actual trajectory.
+ State& state = integ.updAdvancedState();
+
+ #ifdef USE_VISUALIZER
+ // Output a frame to the Visualizer if it is time.
+ if (stepNum % DrawEveryN == 0)
+ mbs.m_viz.report(state);
+ #endif
+
+ if (stepNum++ == NSteps)
+ break;
+
+ // Advance time by MaxStepSize. Might take multiple internal steps to
+ // get there, depending on difficulty and required accuracy.
+ const Real tNext = stepNum * MaxStepSize;
+ do {integ.stepTo(tNext,tNext);} while (integ.getTime() < tNext);
+ }
+
+ const State& state = integ.getAdvancedState();
+ mbs.m_system.realize(state);
+
+ ReactionPair reaction1 = getReactionPair(state, mbs.m_link1);
+ ReactionPair reaction2 = getReactionPair(state, mbs.m_link2);
+ cout << "t=" << state.getTime() << endl;
+ cout << " Reaction 1p=" << reaction1.reactionOnParentInParent << "\n";
+ cout << " Reaction 1c=" << reaction1.reactionOnChildInChild << "\n";
+ cout << " Reaction 2p=" << reaction2.reactionOnParentInParent << "\n";
+ cout << " Reaction 2c=" << reaction2.reactionOnChildInChild << "\n";
+
+ // Check the answers. Note (torque,force) ordering.
+ SimTK_TEST_EQ_TOL(reaction1.reactionOnParentInParent,
+ SpatialVec(Vec3(-750, 0, 450), Vec3(-600,200,-1000)), 0.5);
+ SimTK_TEST_EQ_TOL(reaction1.reactionOnChildInChild,
+ SpatialVec(Vec3(750,450,0), Vec3(600,-1000,-200)), 0.5);
+ SimTK_TEST_EQ_TOL(reaction2.reactionOnParentInParent,
+ SpatialVec(Vec3(-250,-150,0), Vec3(-300,500,100)), 0.5);
+ SimTK_TEST_EQ_TOL(reaction2.reactionOnChildInChild,
+ SpatialVec(Vec3(250,150,0), Vec3(300,-500,-100)), 0.5);
+
+ dumpIntegratorStats(integ);
+}
+
+
+
+//==============================================================================
+// GET REACTION PAIR
+//==============================================================================
+static ReactionPair getReactionPair(const State& state,
+ const MobilizedBody& mobod)
+{
+ SpatialVec p = mobod.findMobilizerReactionOnParentAtFInGround(state);
+ SpatialVec c = mobod.findMobilizerReactionOnBodyAtMInGround(state);
+ const Rotation& R_GC = mobod.getBodyRotation(state);
+ const Rotation& R_GP = mobod.getParentMobilizedBody().getBodyRotation(state);
+ ReactionPair pair;
+ pair.reactionOnChildInChild = ~R_GC*c; // from Ground to Child
+ pair.reactionOnParentInParent = ~R_GP*p; // from Ground to Parent
+ return pair;
+}
+
+
+//==============================================================================
+// DUMP INTEGRATOR STATS
+//==============================================================================
+static void dumpIntegratorStats(const Integrator& integ) {
+ const int evals = integ.getNumRealizations();
+ std::cout << "\nDone -- simulated " << integ.getTime() << "s with "
+ << integ.getNumStepsTaken() << " steps, avg step="
+ << (1000*integ.getTime())/integ.getNumStepsTaken() << "ms "
+ << (1000*integ.getTime())/evals << "ms/eval\n";
+
+ printf("Used Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(),
+ integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(),
+ integ.getNumProjections());
+}
diff --git a/Simbody/tests/GazeboReactionForceWithAppliedForce.cpp b/Simbody/tests/GazeboReactionForceWithAppliedForce.cpp
new file mode 100644
index 0000000..00316c2
--- /dev/null
+++ b/Simbody/tests/GazeboReactionForceWithAppliedForce.cpp
@@ -0,0 +1,362 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): Gazebo Reaction Force With Applied Force *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman, John Hsu *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* This test is drawn from the Open Source Robotics Foundation Gazebo physics
+regression test "Joint_TEST::GetForceTorqueWithAppliedForce".
+
+It is a stack of three cubes hinged together at their edges. The bottom block
+is heavy and rests on the ground, the other two are light and have their
+positions maintained by a pair of PD controllers like this:
+
+ /
+ link3 / \ * = pin joint
+ / \
+ \ 1 /
+ \ / z
+ ------* 45 degrees ^ g = 0 0 -50
+ link2 | | | y |
+ | 6 | | / |
+ -----*------ | / v
+ | | 0 degrees ---------> x
+ | 100 |
+ ------- link1
+ contact
+ GROUND
+
+All the cube edges are of length 1. Masses are 100,6,1 as shown. The top block
+has a COM that is offset into the +y direction by 0.5.
+
+Expected reaction force results:
+ pin1 on inboard: tx=-25, ty= 175, fz=-300
+ pin1 on outboard: tx= 25, ty=-175, fx=1, fz= 300,
+ pin2 on inboard: tx=-25, fz=-50
+ pin2 in outboard: tx=25*pi/4, tz=-25*pi/4, fx=fz=50*pi/4
+
+The reason for fx=1 in the second line is that with a gain of only 50000, the
+first pin joint must be at an angle of 0.0035 radians to generate a torque of
+175. That tips link2's frame in which (0,0,300)_G is reexpressed.
+*/
+
+#include "Simbody.h"
+
+#include <cassert>
+#include <iostream>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+//#define USE_VISUALIZER
+
+
+// Control gains
+const Real Kp1 = 50000; // link1-2 joint stiffness
+const Real Kp2 = 10000; // link2-3 joint stiffness
+const Real Cd1 = 30; // link1-2 joint damping
+const Real Cd2 = 30; // link2-3 joint damping
+
+// Target angles
+const Real Target1 = 0;
+const Real Target2 = -Pi/4;
+
+const Vec3 Cube(.5,.5,.5); // half-dimensions of cube
+const Real Mass1=100, Mass2=5, Mass3=1;
+const Vec3 Centroid(.5,0,.5);
+const Vec3 COM1=Centroid, COM2=Centroid, COM3=Centroid+Vec3(0,.5,0);
+const UnitInertia Central(Vec3(.1), Vec3(.05));
+// Simbody requires inertias to be expressed about body origin rather than COM.
+const Inertia Inertia1=Mass1*Central.shiftFromCentroid(-COM1);
+const Inertia Inertia2=Mass2*Central.shiftFromCentroid(-COM2);
+const Inertia Inertia3=Mass3*Central.shiftFromCentroid(-COM3); // weird
+
+// Define a stiff, lossy material.
+const Real Stiffness = 1e8;
+const Real Dissipation = 10;
+const Real Mu_s = 0.15, Mu_d = 0.1, Mu_v = 0;
+const ContactMaterial lossyMaterial(Stiffness,
+ Dissipation,
+ Mu_s,
+ Mu_d,
+ Mu_v);
+
+const Real MaxStepSize = Real(1/1000.); // 1 ms (1000 Hz)
+const int DrawEveryN = 33; // 33 ms frame update (30.3 Hz)
+const Real SimTime = 5;
+const int NSteps = // make this a whole number of viz frames
+ DrawEveryN*(int(SimTime/MaxStepSize/DrawEveryN+0.5));
+
+// Use this class to hold references into the Simbody system.
+struct MyMultibodySystem {
+ MyMultibodySystem(); // see below
+
+ MultibodySystem m_system;
+ SimbodyMatterSubsystem m_matter;
+ ContactTrackerSubsystem m_tracker;
+ CompliantContactSubsystem m_contact;
+ GeneralForceSubsystem m_forces;
+ Force::DiscreteForces m_discrete;
+ #ifdef USE_VISUALIZER
+ Visualizer m_viz;
+ #endif
+
+ MobilizedBody m_link1;
+ MobilizedBody::Pin m_link2, m_link3;
+};
+
+// Execute the simulation with a given integrator and accuracy, and verify that
+// it produces the correct answers.
+static void runOnce(const MyMultibodySystem& mbs, Integrator& integ,
+ Real accuracy);
+
+
+//==============================================================================
+// MAIN
+//==============================================================================
+int main() {
+ SimTK_START_TEST("GazeboReactionForce");
+ // Create the system.
+ MyMultibodySystem mbs;
+
+ printf("LOW ACCURACY\n");
+ SemiExplicitEuler2Integrator sexpeul2(mbs.m_system);
+ SimTK_SUBTEST3(runOnce, mbs, sexpeul2, 1e-2);
+
+ printf("\nHIGH ACCURACY\n");
+ RungeKuttaMersonIntegrator rkm(mbs.m_system);
+ SimTK_SUBTEST3(runOnce, mbs, rkm, 1e-6);
+
+ SimTK_END_TEST();
+}
+
+
+//==============================================================================
+// MY MULTIBODY SYSTEM
+//==============================================================================
+// Construct the multibody system. The dampers are built in here but the springs
+// are applied during execution.
+MyMultibodySystem::MyMultibodySystem()
+: m_system(), m_matter(m_system),
+ m_tracker(m_system), m_contact(m_system,m_tracker),
+ m_forces(m_system), m_discrete(m_forces,m_matter)
+ #ifdef USE_VISUALIZER
+ , m_viz(m_system)
+ #endif
+{
+ #ifdef USE_VISUALIZER
+ m_viz.setSystemUpDirection(ZAxis);
+ m_viz.setCameraTransform(
+ Transform(Rotation(BodyRotationSequence,Pi/2,XAxis,Pi/8,YAxis),
+ Vec3(5,-8,2)));
+ m_viz.setShowFrameNumber(true);
+ m_viz.setShowSimTime(true);
+ #endif
+
+ Force::Gravity(m_forces, m_matter, -ZAxis, 50);
+
+ DecorativeBrick drawCube(Cube); drawCube.setOpacity(0.5).setColor(Gray);
+
+ Body::Rigid link1Info(MassProperties(Mass1, COM1, Inertia1));
+ ContactGeometry::TriangleMesh cubeMesh
+ (PolygonalMesh::createBrickMesh(Cube, 3));
+ DecorativeMesh showMesh(cubeMesh.createPolygonalMesh());
+ showMesh.setRepresentation(DecorativeGeometry::DrawWireframe);
+ link1Info.addDecoration(Centroid, drawCube);
+ link1Info.addContactSurface(Centroid,
+ ContactSurface(cubeMesh, lossyMaterial, 1));
+ link1Info.addDecoration(Centroid, showMesh);
+
+ Body::Rigid link2Info(MassProperties(Mass2, COM2, Inertia2));
+ link2Info.addDecoration(Centroid, drawCube);
+
+ Body::Rigid link3Info(MassProperties(Mass3, COM3, Inertia3));
+ link3Info.addDecoration(Centroid, drawCube);
+
+ MobilizedBody& Ground = m_matter.updGround(); // Nicer name for Ground.
+ Ground.addBodyDecoration(Vec3(0,0,.05),
+ DecorativeFrame(2).setColor(Green));
+
+ // Add the Ground contact geometry. Contact half space has -XAxis normal
+ // (right hand wall) so we have to rotate.
+ const Rotation NegXToZ(Pi/2, YAxis);
+ Ground.updBody().addContactSurface(Transform(NegXToZ,Vec3(0)),
+ ContactSurface(ContactGeometry::HalfSpace(),lossyMaterial));
+ m_link1 = MobilizedBody::Free(Ground,Vec3(0),
+ link1Info, Vec3(0));
+
+ // Use this instead of the free joint to remove contact.
+ //m_link1 = MobilizedBody::Weld(Ground,Vec3(0),
+ // link1Info, Vec3(0));
+
+ const Rotation ZtoY(-Pi/2, XAxis);
+ m_link2 = MobilizedBody::Pin(m_link1, Transform(ZtoY,2*Centroid),
+ link2Info, Transform(ZtoY,Vec3(0)));
+ m_link3 = MobilizedBody::Pin(m_link2, Transform(ZtoY,2*Centroid),
+ link3Info, Transform(ZtoY,Vec3(0)));
+
+ // It is more stable to build the springs into the mechanism like
+ // this rather than apply them discretely.
+ //Force::MobilityLinearSpring
+ // (m_forces, m_link2, MobilizerQIndex(0), Kp1, Target1);
+ //Force::MobilityLinearSpring
+ // (m_forces, m_link3, MobilizerQIndex(0), Kp2, Target2);
+ Force::MobilityLinearDamper
+ (m_forces, m_link2, MobilizerUIndex(0), Cd1);
+ Force::MobilityLinearDamper
+ (m_forces, m_link3, MobilizerUIndex(0), Cd2);
+
+ m_system.realizeTopology();
+
+}
+
+//==============================================================================
+// RUN ONCE
+//==============================================================================
+
+// Reaction force information: results are the joint reaction, at the F frame
+// on the parent and M frame on the child, expressed in the parent or child
+// frame, resp. Note that Gazebo's GetForceTorque() method uses the negation of
+// the joint reaction, and Gazebo's results are ordered (force,torque) rather
+// than (torque,force) as in a Simbody SpatialVec.
+struct ReactionPair {
+ SpatialVec reactionOnParentInParent;
+ SpatialVec reactionOnChildInChild;
+};
+static ReactionPair getReactionPair(const State& state,
+ const MobilizedBody& mobod);
+
+// Write interesting integrator info to stdout.
+static void dumpIntegratorStats(const Integrator& integ);
+
+// Run the system until it settles down, then check the answers.
+void runOnce(const MyMultibodySystem& mbs, Integrator& integ, Real accuracy)
+{
+ integ.setAllowInterpolation(false);
+ integ.setAccuracy(accuracy);
+ integ.initialize(mbs.m_system.getDefaultState());
+
+ printf("Test with order %d integator %s, Accuracy=%g, MaxStepSize=%g\n",
+ integ.getMethodMinOrder(), integ.getMethodName(),
+ integ.getAccuracyInUse(), MaxStepSize);
+
+ #ifdef USE_VISUALIZER
+ mbs.m_viz.report(integ.getState());
+ printf("Hit ENTER to simulate:");
+ getchar();
+ #endif
+
+ unsigned stepNum = 0;
+ while (true) {
+ // Get access to State being advanced by the integrator. Interpolation
+ // must be off so that we're modifying the actual trajectory.
+ State& state = integ.updAdvancedState();
+
+ #ifdef USE_VISUALIZER
+ // Output a frame to the Visualizer if it is time.
+ if (stepNum % DrawEveryN == 0)
+ mbs.m_viz.report(state);
+ #endif
+
+ if (stepNum++ == NSteps)
+ break;
+
+ // Apply discrete spring forces.
+ const Real a1err = mbs.m_link2.getAngle(state)-Target1;
+ const Real a2err = mbs.m_link3.getAngle(state)-Target2;
+ mbs.m_discrete.setOneMobilityForce(state, mbs.m_link2,
+ MobilizerUIndex(0), -Kp1*a1err);
+ mbs.m_discrete.setOneMobilityForce(state, mbs.m_link3,
+ MobilizerUIndex(0), -Kp2*a2err);
+
+ // Advance time by MaxStepSize. Might take multiple internal steps to
+ // get there, depending on difficulty and required accuracy.
+ const Real tNext = stepNum * MaxStepSize;
+ do {integ.stepTo(tNext,tNext);} while (integ.getTime() < tNext);
+ }
+
+ const State& state = integ.getAdvancedState();
+ mbs.m_system.realize(state);
+ Rotation R_G1 = mbs.m_link1.getBodyRotation(state);
+ Vec3 a = R_G1.convertRotationToBodyFixedXYZ();
+ Vec3 w = mbs.m_link1.getBodyAngularVelocity(state);
+ ReactionPair reaction2 = getReactionPair(state, mbs.m_link2);
+ ReactionPair reaction3 = getReactionPair(state, mbs.m_link3);
+ cout << "t=" << state.getTime() << endl;
+ cout << " joint1 a=" << a << " w=" << w << endl;
+ cout << " joint2 qerr=" << mbs.m_link2.getAngle(state)-Target1
+ << " u=" << mbs.m_link2.getRate(state) << endl;
+ cout << " joint3 qerr=" << mbs.m_link3.getAngle(state)-Target2
+ << " u=" << mbs.m_link3.getRate(state) << endl;
+ cout << " Reaction 2p=" << reaction2.reactionOnParentInParent << "\n";
+ cout << " Reaction 2c=" << reaction2.reactionOnChildInChild << "\n";
+ cout << " Reaction 3p=" << reaction3.reactionOnParentInParent << "\n";
+ cout << " Reaction 3c=" << reaction3.reactionOnChildInChild << "\n";
+
+ // Check the answers. Note (torque,force) ordering.
+ SimTK_TEST_EQ_TOL(reaction2.reactionOnParentInParent,
+ SpatialVec(Vec3(-25, 175,0), Vec3(0,0,-300)), 0.5);
+ SimTK_TEST_EQ_TOL(reaction2.reactionOnChildInChild,
+ SpatialVec(Vec3( 25,-175,0), Vec3(-1,0, 300)), 0.5);
+ SimTK_TEST_EQ_TOL(reaction3.reactionOnParentInParent,
+ SpatialVec(Vec3(-25,0,0), Vec3(0,0,-50)), 0.5);
+ SimTK_TEST_EQ_TOL(reaction3.reactionOnChildInChild,
+ (Pi/4)*SpatialVec(Vec3(25,0,-25), Vec3(50,0,50)), 0.5);
+
+ dumpIntegratorStats(integ);
+}
+
+
+
+//==============================================================================
+// GET REACTION PAIR
+//==============================================================================
+static ReactionPair getReactionPair(const State& state,
+ const MobilizedBody& mobod)
+{
+ SpatialVec p = mobod.findMobilizerReactionOnParentAtFInGround(state);
+ SpatialVec c = mobod.findMobilizerReactionOnBodyAtMInGround(state);
+ const Rotation& R_GC = mobod.getBodyRotation(state);
+ const Rotation& R_GP = mobod.getParentMobilizedBody().getBodyRotation(state);
+ ReactionPair pair;
+ pair.reactionOnChildInChild = ~R_GC*c; // from Ground to Child
+ pair.reactionOnParentInParent = ~R_GP*p; // from Ground to Parent
+ return pair;
+}
+
+
+//==============================================================================
+// DUMP INTEGRATOR STATS
+//==============================================================================
+static void dumpIntegratorStats(const Integrator& integ) {
+ const int evals = integ.getNumRealizations();
+ std::cout << "\nDone -- simulated " << integ.getTime() << "s with "
+ << integ.getNumStepsTaken() << " steps, avg step="
+ << (1000*integ.getTime())/integ.getNumStepsTaken() << "ms "
+ << (1000*integ.getTime())/evals << "ms/eval\n";
+
+ printf("Used Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(),
+ integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(),
+ integ.getNumProjections());
+}
diff --git a/Simbody/tests/TestAngleConversions.cpp b/Simbody/tests/TestAngleConversions.cpp
new file mode 100644
index 0000000..dd97f9a
--- /dev/null
+++ b/Simbody/tests/TestAngleConversions.cpp
@@ -0,0 +1,102 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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;
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+/**
+ * Test converting a SimbodyMatterSubsystem between quaternion and Euler angle representations.
+ */
+
+int main() {
+
+ // Build a system consisting of a chain of bodies with every possible mobilizer.
+
+ MultibodySystem mbs;
+ SimbodyMatterSubsystem matter(mbs);
+ Body::Rigid body = Body::Rigid(MassProperties(1, Vec3(0), Inertia(1)));
+ body.addDecoration(DecorativeSphere(.1));
+ Random::Uniform random(0.0, 2.0);
+ MobilizedBody lastBody = MobilizedBody::Pin(matter.Ground(), Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
+ lastBody = MobilizedBody::Slider(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
+ lastBody = MobilizedBody::Universal(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
+ lastBody = MobilizedBody::Cylinder(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
+ lastBody = MobilizedBody::BendStretch(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
+ lastBody = MobilizedBody::Planar(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
+ lastBody = MobilizedBody::Gimbal(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
+ lastBody = MobilizedBody::Ball(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
+ lastBody = MobilizedBody::Translation(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
+ lastBody = MobilizedBody::Free(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
+ lastBody = MobilizedBody::LineOrientation(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
+ lastBody = MobilizedBody::FreeLine(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
+ lastBody = MobilizedBody::Weld(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
+ lastBody = MobilizedBody::Screw(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())), 0.5);
+ lastBody = MobilizedBody::Ellipsoid(lastBody, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(random.getValue(), random.getValue(), random.getValue())));
+ mbs.realizeTopology();
+ State& s = mbs.updDefaultState();
+ mbs.realizeModel(s);
+
+ // Choose a random initial conformation.
+
+ for (int i = 0; i < s.getNQ(); ++i)
+ s.updQ()[i] = random.getValue();
+ mbs.realize(s, Stage::Instance);
+ // The only constraints are the quaternions -- normalize them.
+ Vector temp;
+ mbs.project(s, 0.01);
+ mbs.realize(s, Stage::Position);
+
+ // Convert to Euler angles and make sure the positions are all the same.
+
+ State euler = s;
+ matter.convertToEulerAngles(s, euler);
+ mbs.realize(euler, Stage::Position);
+ for (int i = 0; i < matter.getNumBodies(); ++i) {
+ const MobilizedBody& body = matter.getMobilizedBody(MobilizedBodyIndex(i));
+ Real dist = (body.getBodyOriginLocation(euler)-body.getBodyOriginLocation(s)).norm();
+ ASSERT(dist < 1e-5);
+ }
+
+ // Now convert back to quaternions and make sure the positions are still the same.
+
+ State quaternions = s;
+ matter.convertToQuaternions(euler, quaternions);
+ mbs.realize(quaternions, Stage::Position);
+ for (int i = 0; i < matter.getNumBodies(); ++i) {
+ const MobilizedBody& body = matter.getMobilizedBody(MobilizedBodyIndex(i));
+ Real dist = (body.getBodyOriginLocation(quaternions)-body.getBodyOriginLocation(s)).norm();
+ ASSERT(dist < 1e-5);
+ }
+
+ // Compare the state variables to see if they have been accurately reproduced.
+
+ mbs.project(s, 0.01); // Normalize the quaternions
+ Real diff = std::sqrt((s.getQ()-quaternions.getQ()).normSqr()/s.getNQ());
+ ASSERT(diff < 1e-5);
+
+ std::cout << "Done" << std::endl;
+}
diff --git a/Simbody/tests/TestCollisionDetectionAlgorithm.cpp b/Simbody/tests/TestCollisionDetectionAlgorithm.cpp
new file mode 100644
index 0000000..e832c7a
--- /dev/null
+++ b/Simbody/tests/TestCollisionDetectionAlgorithm.cpp
@@ -0,0 +1,551 @@
+/* -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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"
+
+#include <set>
+
+using namespace SimTK;
+using namespace std;
+
+const Real TOL = 1e-10;
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+template <class T>
+void assertEqual(T val1, T val2) {
+ ASSERT(abs(val1-val2) < TOL);
+}
+
+template <int N>
+void assertEqual(Vec<N> val1, Vec<N> val2) {
+ for (int i = 0; i < N; ++i)
+ ASSERT(abs(val1[i]-val2[i]) < TOL);
+}
+
+void verifyPointContact(const Array_<Contact>& contacts, int surface1, int surface2, const Vec3& normal, const Vec3& location, Real depth, Real r1, Real r2) {
+ ASSERT(contacts.size() == 1);
+ ASSERT(PointContact::isInstance(contacts[0]));
+ const PointContact& c = static_cast<const PointContact&>(contacts[0]);
+ assertEqual((int) c.getSurface1(), surface1);
+ assertEqual((int) c.getSurface2(), surface2);
+ assertEqual(c.getNormal(), normal);
+ assertEqual(c.getDepth(), depth);
+ assertEqual(min(c.getRadiusOfCurvature1(), c.getRadiusOfCurvature2()), min(r1, r2));
+ assertEqual(max(c.getRadiusOfCurvature1(), c.getRadiusOfCurvature2()), max(r1, r2));
+ assertEqual(c.getEffectiveRadiusOfCurvature(), sqrt(r1*r2));
+ assertEqual(c.getLocation(), location);
+}
+
+void testHalfSpaceSphere() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralContactSubsystem contacts(system);
+ Real radius = 0.8;
+ Vec3 center(0.1, -0.3, 0.3);
+ Random::Uniform random(0.0, 1.0);
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ ContactSetIndex setIndex = contacts.createContactSet();
+ MobilizedBody::Free sphere(matter.updGround(), Transform(), body, Transform());
+ contacts.addBody(setIndex, sphere, ContactGeometry::Sphere(radius), center);
+ contacts.addBody(setIndex, matter.updGround(), ContactGeometry::HalfSpace(), Transform(Rotation(-0.5*Pi, ZAxis), Vec3(0, 1, 0))); // y < 1
+ State state = system.realizeTopology();
+ Vec3 centerInGround;
+ for (int iteration = 0; iteration < 100; ++iteration) {
+ // Pick a random positions for the sphere.
+
+ for (int i = 0; i < state.getNY(); i++)
+ state.updY()[i] = 5*random.getValue();
+ system.realize(state, Stage::Dynamics);
+ centerInGround = sphere.findStationLocationInGround(state, center);
+
+ // Check the results of collision detection.
+
+ const Array_<Contact>& contact = contacts.getContacts(state, setIndex);
+ if (centerInGround[1] > radius+1) {
+ ASSERT(contact.size() == 0);
+ }
+ else {
+ Real depth = radius-centerInGround[1]+1;
+ verifyPointContact(contact, 1, 0, Vec3(0, 1, 0), Vec3(centerInGround[0], 1-0.5*depth, centerInGround[2]), depth, radius, radius);
+ }
+ }
+}
+
+void testSphereSphere() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralContactSubsystem contacts(system);
+ const int numBodies = 10;
+ Real radius[numBodies];
+ Vec3 center[numBodies];
+ Random::Uniform random(0.0, 1.0);
+ for (int i = 0; i < numBodies; i++) {
+ radius[i] = random.getValue();
+ center[i] = Vec3(random.getValue(), random.getValue(), random.getValue());
+ }
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ ContactSetIndex setIndex = contacts.createContactSet();
+ for (int i = 0; i < numBodies; ++i) {
+ MobilizedBody::Free b(matter.updGround(), Transform(), body, Transform());
+ contacts.addBody(setIndex, b, ContactGeometry::Sphere(radius[i]), center[i]);
+ }
+ State state = system.realizeTopology();
+ Vec3 centerInGround[numBodies];
+ for (int iteration = 0; iteration < 100; ++iteration) {
+ // Pick random positions for all the bodies.
+
+ for (int i = 0; i < state.getNY(); i++)
+ state.updY()[i] = 5*random.getValue();
+ system.realize(state, Stage::Dynamics);
+ for (MobilizedBodyIndex index(1); index <= numBodies; ++index)
+ centerInGround[index-1] = matter.getMobilizedBody(index).findStationLocationInGround(state, center[index-1]);
+
+ // Make sure all contacts are accurate.
+
+ const Array_<Contact>& contact = contacts.getContacts(state, setIndex);
+ for (int i = 0; i < (int) contact.size(); i++) {
+ ASSERT(PointContact::isInstance(contact[i]));
+ const PointContact& c = static_cast<const PointContact&>(contact[i]);
+ int body1 = c.getSurface1();
+ int body2 = c.getSurface2();
+ Vec3 delta = centerInGround[body2]-centerInGround[body1];
+ assertEqual(delta.normalize(), c.getNormal());
+ assertEqual(delta.norm(), radius[body1]+radius[body2]-c.getDepth());
+ double r = radius[body1]*radius[body2]/(radius[body1]+radius[body2]);
+ assertEqual(r, c.getRadiusOfCurvature1());
+ assertEqual(r, c.getRadiusOfCurvature2());
+ }
+
+ // Make sure no contacts were missed.
+
+ int expectedContacts = 0;
+ for (int i = 0; i < numBodies; i++)
+ for (int j = 0; j < i; j++)
+ if ((centerInGround[i]-centerInGround[j]).norm() < radius[i]+radius[j])
+ expectedContacts++;
+ ASSERT(contact.size() == expectedContacts);
+ }
+}
+
+void testHalfSpaceEllipsoid() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralContactSubsystem contacts(system);
+ Vec3 radii(0.8, 1.5, 2.1);
+ Vec3 center(0.1, -0.3, 0.3); // Major axes span the ranges [-0.7, 0.9], [-1.8, 1.2], [-1.8, 2.4]
+ Random::Uniform random(0.0, 1.0);
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ ContactSetIndex setIndex = contacts.createContactSet();
+ MobilizedBody::Free ellipsoid(matter.updGround(), Transform(), body, Transform());
+ contacts.addBody(setIndex, ellipsoid, ContactGeometry::Ellipsoid(radii), center);
+ contacts.addBody(setIndex, matter.updGround(), ContactGeometry::HalfSpace(), Transform(Rotation(-0.5*Pi, ZAxis), Vec3(0, 1, 0))); // y < 1
+ State state = system.realizeTopology();
+
+ // Test a variety of positions.
+
+ ellipsoid.setQToFitTransform(state, Transform(Rotation(), Vec3(0, 2.9, 0))); // [-0.7, 0.9], [1.1, 4.1], [-1.8, 2.4]
+ system.realize(state, Stage::Dynamics);
+ ASSERT(contacts.getContacts(state, setIndex).size() == 0);
+ ellipsoid.setQToFitTransform(state, Transform(Rotation(), Vec3(0, 2.6, 0))); // [-0.7, 0.9], [0.8, 3.8], [-1.8, 2.4]
+ system.realize(state, Stage::Dynamics);
+ verifyPointContact(contacts.getContacts(state, setIndex), 1, 0, Vec3(0, 1, 0), Vec3(0.1, 0.9, 0.3), 0.2, 0.8, 2.1);
+ ellipsoid.setQToFitTransform(state, Transform(Rotation(SimTK_PI/2, ZAxis), Vec3(0, 1.6, 0))); // [-1.2, 1.8], [0.9, 2.5], [-1.8, 2.4]
+ system.realize(state, Stage::Dynamics);
+ verifyPointContact(contacts.getContacts(state, setIndex), 1, 0, Vec3(0, 1, 0), Vec3(0.3, 0.95, 0.3), 0.1, 1.5, 2.1);
+ ellipsoid.setQToFitTransform(state, Transform(Rotation(SimTK_PI/4, XAxis), Vec3(0, 3.1, 0)));
+ system.realize(state, Stage::Dynamics);
+ ASSERT(contacts.getContacts(state, setIndex).size() == 1);
+ const PointContact& c = static_cast<const PointContact&>(contacts.getContacts(state, setIndex)[0]);
+ assertEqual(c.getNormal(), Vec3(0, 1, 0));
+ ASSERT(c.getDepth() < 0.3);
+ assertEqual(min(c.getRadiusOfCurvature1(), c.getRadiusOfCurvature2()), 0.8);
+ ASSERT(max(c.getRadiusOfCurvature1(), c.getRadiusOfCurvature2()) > 1.5 && max(c.getRadiusOfCurvature1(), c.getRadiusOfCurvature2()) < 2.1);
+ assertEqual(c.getLocation()[0], 0.1);
+ assertEqual(c.getLocation()[1], 1-c.getDepth()/2);
+ ASSERT(c.getLocation()[2] > 0);
+}
+
+bool verifyEllipsoidContact(const Contact& contact, const Vec3& radii1, const Vec3& radii2, const Vec3& center1, const Vec3& center2, const Transform& t1, const Transform& t2) {
+ ASSERT(PointContact::isInstance(contact));
+ const PointContact& c = static_cast<const PointContact&>(contact);
+
+ // The "contact point" should be midway between the two surfaces along the normal direction. Verify that.
+
+ Vec3 loc1 = ~t1*(c.getLocation()+0.5*c.getDepth()*c.getNormal())-center1;
+ assertEqual(loc1[0]*loc1[0]/(radii1[0]*radii1[0])+loc1[1]*loc1[1]/(radii1[1]*radii1[1])+loc1[2]*loc1[2]/(radii1[2]*radii1[2]), 1.0);
+ Vec3 loc2 = ~t2*(c.getLocation()-0.5*c.getDepth()*c.getNormal())-center2;
+ assertEqual(loc2[0]*loc2[0]/(radii2[0]*radii2[0])+loc2[1]*loc2[1]/(radii2[1]*radii2[1])+loc2[2]*loc2[2]/(radii2[2]*radii2[2]), 1.0);
+
+ // Check that the normals are correct. This test may occassionally fail (when points of very high
+ // curvate cause the Newton iteration not to converge), so instead of an assertion, which just return
+ // whether the normals were correct.
+
+ UnitVec3 norm1(loc1[0]/(radii1[0]*radii1[0]), loc1[1]/(radii1[1]*radii1[1]), loc1[2]/(radii1[2]*radii1[2]));
+ if (~norm1*(~t1.R()*c.getNormal()) < 0.999)
+ return false;
+ UnitVec3 norm2(loc2[0]/(radii2[0]*radii2[0]), loc2[1]/(radii2[1]*radii2[1]), loc2[2]/(radii2[2]*radii2[2]));
+ if (-~norm2*(~t2.R()*c.getNormal()) < 0.999)
+ return false;
+ return true;
+}
+
+void testEllipsoidEllipsoid() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralContactSubsystem contacts(system);
+ Vec3 radii1(0.8, 1.5, 2.1);
+ Vec3 radii2(1.0, 1.2, 1.4);
+ Vec3 center1(0, -0.2, 0.5); // Major axes span the ranges [-0.8, 0.8], [-1.7, 1.3], [-1.6, 2.6]
+ Vec3 center2(0.1, 0, 0.3); // Major axes span the ranges [-0.9, 1.1], [-1.2, 1.2], [-1.1, 1.7]
+ Random::Uniform random(0.0, 1.0);
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ Body::Rigid body2(MassProperties(1.0, Vec3(0), Inertia(1)));
+ ContactSetIndex setIndex = contacts.createContactSet();
+ MobilizedBody::Free ellipsoid1(matter.updGround(), Transform(), body, Transform());
+ MobilizedBody::Free ellipsoid2(matter.updGround(), Transform(), body2, Transform());
+ contacts.addBody(setIndex, ellipsoid1, ContactGeometry::Ellipsoid(radii1), center1);
+ contacts.addBody(setIndex, ellipsoid2, ContactGeometry::Ellipsoid(radii2), center2);
+ State state = system.realizeTopology();
+
+ // Test a variety of positions.
+
+ ellipsoid1.setQToFitTransform(state, Transform(Rotation(), Vec3(0))); // [-0.8, 0.8], [-1.7, 1.3], [-1.6, 2.6]
+ ellipsoid2.setQToFitTransform(state, Transform(Rotation(), Vec3(2, 0, 0))); // [1.1, 3.1], [-1.2, 1.2], [-1.1, 1.7]
+ system.realize(state, Stage::Dynamics);
+ ASSERT(contacts.getContacts(state, setIndex).size() == 0);
+ ellipsoid1.setQToFitTransform(state, Transform(Rotation(), Vec3(0))); // [-0.8, 0.8], [-1.7, 1.3], [-1.6, 2.6]
+ ellipsoid2.setQToFitTransform(state, Transform(Rotation(), Vec3(1.5, 0, 0))); // [0.6, 2.6], [-1.2, 1.2], [-1.1, 1.7]
+ system.realize(state, Stage::Dynamics);
+ ASSERT(contacts.getContacts(state, setIndex).size() == 1);
+ ASSERT(verifyEllipsoidContact(contacts.getContacts(state, setIndex)[0], radii1, radii2, center1, center2, ellipsoid1.getBodyTransform(state), ellipsoid2.getBodyTransform(state)));
+
+ // Create a cloud of ellipsoids and find all contacts between them.
+
+ MultibodySystem system2;
+ SimbodyMatterSubsystem matter2(system2);
+ GeneralContactSubsystem contacts2(system2);
+ ContactSetIndex setIndex2 = contacts2.createContactSet();
+ const int numEllipsoids = 100;
+ for (int i = 0; i < numEllipsoids; i++) {
+ MobilizedBody::Free ellipsoid(matter2.updGround(), Transform(), body, Transform());
+ contacts2.addBody(setIndex2, ellipsoid, ContactGeometry::Ellipsoid(Vec3(0.1+random.getValue(), 0.1+random.getValue(), 0.1+random.getValue())), Vec3(0));
+ }
+ State state2 = system2.realizeTopology();
+ for (MobilizedBodyIndex i(1); i <= numEllipsoids; i++) {
+ Rotation rot;
+ rot.setRotationToBodyFixedXYZ(Vec3(random.getValue()*SimTK_PI, random.getValue()*SimTK_PI, random.getValue()*SimTK_PI));
+ Vec3 pos = 5*Vec3(random.getValue(), random.getValue(), random.getValue());
+ matter2.getMobilizedBody(i).setQToFitTransform(state2, Transform(rot, pos));
+ }
+ system2.realize(state2, Stage::Dynamics);
+
+ // Verify each contact that was found.
+
+ const Array_<Contact>& contact = contacts2.getContacts(state2, setIndex2);
+ int errorCount = 0;
+ for (int i = 0; i < (int) contact.size(); i++) {
+ ASSERT(PointContact::isInstance(contact[i]));
+ const PointContact& c = static_cast<const PointContact&>(contact[i]);
+ const ContactGeometry::Ellipsoid& ellipsoid1 = static_cast<const ContactGeometry::Ellipsoid&>(contacts2.getBodyGeometry(setIndex2, c.getSurface1()));
+ const ContactGeometry::Ellipsoid& ellipsoid2 = static_cast<const ContactGeometry::Ellipsoid&>(contacts2.getBodyGeometry(setIndex2, c.getSurface2()));
+ const MobilizedBody& body1 = contacts2.getBody(setIndex2, c.getSurface1());
+ const MobilizedBody& body2 = contacts2.getBody(setIndex2, c.getSurface2());
+ if (!verifyEllipsoidContact(c, ellipsoid1.getRadii(), ellipsoid2.getRadii(), Vec3(0), Vec3(0), body1.getBodyTransform(state2), body2.getBodyTransform(state2)))
+ errorCount++;
+ }
+ ASSERT(errorCount < (int)contact.size()/10);
+}
+
+/**
+ * Check the set of faces in a contact.
+ */
+
+void verifyContactFaces(int* expected, int numExpected, const set<int>& found) {
+ ASSERT(numExpected == found.size());
+ for (int i = 0; i < numExpected; i++) {
+ ASSERT(found.find(expected[i]) != found.end());
+ }
+}
+
+void testHalfSpaceTriangleMesh() {
+ // Create a triangle mesh consisting of two pyramids: one right side up and one upside down.
+
+ vector<Vec3> vertices;
+ vertices.push_back(Vec3(0, 0, 0));
+ vertices.push_back(Vec3(1, 0, 0));
+ vertices.push_back(Vec3(0, 0, 1));
+ vertices.push_back(Vec3(1, 0, 1));
+ vertices.push_back(Vec3(0.5, 1, 0.5));
+ vertices.push_back(Vec3(2, 1, 0));
+ vertices.push_back(Vec3(2, 1, 1));
+ vertices.push_back(Vec3(3, 1, 1));
+ vertices.push_back(Vec3(3, 1, 0));
+ vertices.push_back(Vec3(2.5, 0, 0.5));
+ vector<int> faceIndices;
+ int faces[6][3] = {{0, 1, 2}, {0, 2, 3}, {1, 0, 4}, {0, 3, 4}, {3, 2, 4}, {2, 1, 4}};
+ for (int i = 0; i < 6; i++)
+ for (int j = 0; j < 3; j++)
+ faceIndices.push_back(faces[i][j]);
+ for (int i = 0; i < 6; i++)
+ for (int j = 0; j < 3; j++)
+ faceIndices.push_back(faces[i][j]+5);
+ ContactGeometry::TriangleMesh mesh(vertices, faceIndices);
+
+ // Create the system.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralContactSubsystem contacts(system);
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ ContactSetIndex setIndex = contacts.createContactSet();
+ MobilizedBody::Free b(matter.updGround(), Transform(), body, Transform());
+ contacts.addBody(setIndex, b, mesh, Transform());
+ contacts.addBody(setIndex, matter.updGround(), ContactGeometry::HalfSpace(), Transform(Rotation(-0.5*Pi, ZAxis), Vec3(0, 1, 0))); // y < 1
+ State state = system.realizeTopology();
+ int bottomFaces[10] = {0, 1, 2, 3, 4, 5, 8, 9, 10, 11};
+ for (Real depth = -0.25; depth < 1; depth += 0.1) {
+ Vec3 center(0.1, 1-depth, 2.0);
+ b.setQToFitTranslation(state, center);
+ system.realize(state, Stage::Dynamics);
+ const Array_<Contact>& contact = contacts.getContacts(state, setIndex);
+ if (depth < 0.0) {
+ ASSERT(contact.size() == 0);
+ }
+ else {
+ ASSERT(contact.size() == 1);
+ ASSERT(TriangleMeshContact::isInstance(contact[0]));
+ const TriangleMeshContact& c = static_cast<const TriangleMeshContact&>(contact[0]);
+ ASSERT(c.getSurface1Faces().size() == 0);
+ verifyContactFaces(bottomFaces, 10, c.getSurface2Faces());
+ }
+ }
+}
+
+void testSphereTriangleMesh() {
+ // Create a triangle mesh consisting of two pyramids: one right side up and one upside down.
+
+ vector<Vec3> vertices;
+ vertices.push_back(Vec3(0, 0, 0));
+ vertices.push_back(Vec3(0, 0, 1));
+ vertices.push_back(Vec3(1, 0, 1));
+ vertices.push_back(Vec3(1, 0, 0));
+ vertices.push_back(Vec3(0.5, 1, 0.5));
+ vertices.push_back(Vec3(2, 1, 0));
+ vertices.push_back(Vec3(2, 1, 1));
+ vertices.push_back(Vec3(3, 1, 1));
+ vertices.push_back(Vec3(3, 1, 0));
+ vertices.push_back(Vec3(2.5, 0, 0.5));
+ vector<int> faceIndices;
+ int faces[6][3] = {{0, 1, 2}, {0, 2, 3}, {1, 0, 4}, {0, 3, 4}, {3, 2, 4}, {2, 1, 4}};
+ for (int i = 0; i < 6; i++)
+ for (int j = 0; j < 3; j++)
+ faceIndices.push_back(faces[i][j]);
+ for (int i = 0; i < 6; i++)
+ for (int j = 0; j < 3; j++)
+ faceIndices.push_back(faces[i][j]+5);
+ ContactGeometry::TriangleMesh mesh(vertices, faceIndices);
+
+ // Create the system.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralContactSubsystem contacts(system);
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ ContactSetIndex setIndex = contacts.createContactSet();
+ MobilizedBody::Free b(matter.updGround(), Transform(), body, Transform());
+ contacts.addBody(setIndex, b, mesh, Transform());
+ contacts.addBody(setIndex, matter.updGround(), ContactGeometry::Sphere(0.5), Transform(Vec3(0, 1, 0)));
+ State state = system.realizeTopology();
+
+ // Try various positions and make sure the results are correct.
+
+ b.setQToFitTranslation(state, Vec3(0, -2, 0));
+ system.realize(state, Stage::Dynamics);
+ ASSERT(contacts.getContacts(state, setIndex).size() == 0);
+ b.setQToFitTranslation(state, Vec3(0, 1.51, 0));
+ system.realize(state, Stage::Dynamics);
+ ASSERT(contacts.getContacts(state, setIndex).size() == 0);
+ {
+ b.setQToFitTranslation(state, Vec3(-0.5, 1.49, -0.5));
+ system.realize(state, Stage::Dynamics);
+ ASSERT(contacts.getContacts(state, setIndex).size() == 1);
+ const TriangleMeshContact& c = static_cast<const TriangleMeshContact&>(contacts.getContacts(state, setIndex)[0]);
+ int faces[] = {0, 1};
+ verifyContactFaces(faces, 2, c.getSurface2Faces());
+ }
+ b.setQToFitTranslation(state, Vec3(-0.5, -0.51, -0.5));
+ system.realize(state, Stage::Dynamics);
+ ASSERT(contacts.getContacts(state, setIndex).size() == 0);
+ {
+ b.setQToFitTranslation(state, Vec3(-0.5, -0.49, -0.5));
+ system.realize(state, Stage::Dynamics);
+ ASSERT(contacts.getContacts(state, setIndex).size() == 1);
+ const TriangleMeshContact& c = static_cast<const TriangleMeshContact&>(contacts.getContacts(state, setIndex)[0]);
+ int faces[] = {2, 3, 4, 5};
+ verifyContactFaces(faces, 4, c.getSurface2Faces());
+ }
+ b.setQToFitTranslation(state, Vec3(-2.5, 1.51, -0.5));
+ system.realize(state, Stage::Dynamics);
+ ASSERT(contacts.getContacts(state, setIndex).size() == 0);
+ {
+ b.setQToFitTranslation(state, Vec3(-2.5, 1.49, -0.5));
+ system.realize(state, Stage::Dynamics);
+ ASSERT(contacts.getContacts(state, setIndex).size() == 1);
+ const TriangleMeshContact& c = static_cast<const TriangleMeshContact&>(contacts.getContacts(state, setIndex)[0]);
+ int faces[] = {8, 9, 10, 11};
+ verifyContactFaces(faces, 4, c.getSurface2Faces());
+ }
+}
+
+void testTriangleMeshTriangleMesh() {
+ // Create two triangle meshes, each consisting of a pyramid.
+
+ vector<Vec3> vertices;
+ vertices.push_back(Vec3(0, 0, 0));
+ vertices.push_back(Vec3(1, 0, 0));
+ vertices.push_back(Vec3(1, 0, 1));
+ vertices.push_back(Vec3(0, 0, 1));
+ vertices.push_back(Vec3(0.5, 1, 0.5));
+ vector<int> faceIndices;
+ int faces[6][3] = {{0, 1, 2}, {0, 2, 3}, {1, 0, 4}, {2, 1, 4}, {3, 2, 4}, {0, 3, 4}};
+ for (int i = 0; i < 6; i++)
+ for (int j = 0; j < 3; j++)
+ faceIndices.push_back(faces[i][j]);
+ ContactGeometry::TriangleMesh mesh1(vertices, faceIndices);
+ ContactGeometry::TriangleMesh mesh2(vertices, faceIndices);
+
+ // Create the system.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralContactSubsystem contacts(system);
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ ContactSetIndex setIndex = contacts.createContactSet();
+ MobilizedBody::Free b1(matter.updGround(), Transform(), body, Transform());
+ MobilizedBody::Free b2(matter.updGround(), Transform(), body, Transform());
+ contacts.addBody(setIndex, b1, mesh1, Transform());
+ contacts.addBody(setIndex, b2, mesh2, Transform());
+ State state = system.realizeTopology();
+
+ // Try some configurations that should not intersect.
+
+ b1.setQToFitTranslation(state, Vec3(0));
+ b2.setQToFitTranslation(state, Vec3(2));
+ system.realize(state, Stage::Dynamics);
+ ASSERT(contacts.getContacts(state, setIndex).size() == 0);
+ b1.setQToFitTranslation(state, Vec3(0));
+ b2.setQToFitTranslation(state, Vec3(1.01, 0, 0));
+ system.realize(state, Stage::Dynamics);
+ ASSERT(contacts.getContacts(state, setIndex).size() == 0);
+ b1.setQToFitTranslation(state, Vec3(0));
+ b2.setQToFitTranslation(state, Vec3(0, 1.01, 0));
+ system.realize(state, Stage::Dynamics);
+ ASSERT(contacts.getContacts(state, setIndex).size() == 0);
+ b1.setQToFitTranslation(state, Vec3(0));
+ b2.setQToFitTranslation(state, Vec3(0, -1.01, 0));
+ system.realize(state, Stage::Dynamics);
+ ASSERT(contacts.getContacts(state, setIndex).size() == 0);
+
+ // Now try ones that should intersect.
+
+ int baseFaces[2] = {0, 1};
+ int pointFaces[4] = {2, 3, 4, 5};
+ {
+ b1.setQToFitTranslation(state, Vec3(0));
+ b2.setQToFitTranslation(state, Vec3(0, -0.99, 0));
+ system.realize(state, Stage::Dynamics);
+ Array_<Contact> contact = contacts.getContacts(state, setIndex);;
+ ASSERT(contact.size() == 1);
+ ASSERT(TriangleMeshContact::isInstance(contact[0]));
+ const TriangleMeshContact& c = static_cast<const TriangleMeshContact&>(contact[0]);
+ if (contact[0].getSurface1() == 0) {
+ verifyContactFaces(baseFaces, 2, c.getSurface1Faces());
+ verifyContactFaces(pointFaces, 4, c.getSurface2Faces());
+ }
+ else {
+ verifyContactFaces(pointFaces, 4, c.getSurface1Faces());
+ verifyContactFaces(baseFaces, 2, c.getSurface2Faces());
+ }
+ }
+ {
+ b1.setQToFitTranslation(state, Vec3(0, -0.5, 0));
+ b2.setQToFitTranslation(state, Vec3(0, 0.49, 0));
+ system.realize(state, Stage::Dynamics);
+ Array_<Contact> contact = contacts.getContacts(state, setIndex);;
+ ASSERT(contact.size() == 1);
+ ASSERT(TriangleMeshContact::isInstance(contact[0]));
+ const TriangleMeshContact& c = static_cast<const TriangleMeshContact&>(contact[0]);
+ if (contact[0].getSurface1() == 0) {
+ verifyContactFaces(pointFaces, 4, c.getSurface1Faces());
+ verifyContactFaces(baseFaces, 2, c.getSurface2Faces());
+ }
+ else {
+ verifyContactFaces(baseFaces, 2, c.getSurface1Faces());
+ verifyContactFaces(pointFaces, 4, c.getSurface2Faces());
+ }
+ }
+ {
+ b1.setQToFitTranslation(state, Vec3(0.1, -0.5, 0));
+ b2.setQToFitTranslation(state, Vec3(0, 0.49, 0.1));
+ system.realize(state, Stage::Dynamics);
+ Array_<Contact> contact = contacts.getContacts(state, setIndex);;
+ ASSERT(contact.size() == 1);
+ ASSERT(TriangleMeshContact::isInstance(contact[0]));
+ }
+ {
+ b1.setQToFitTransform(state, Transform(Rotation(-0.5*Pi, ZAxis), Vec3(0, 0.5, 0)));
+ b2.setQToFitTransform(state, Transform(Rotation(0.5*Pi, ZAxis), Vec3(1.9, -0.5, 0)));
+ system.realize(state, Stage::Dynamics);
+ Array_<Contact> contact = contacts.getContacts(state, setIndex);;
+ ASSERT(contact.size() == 1);
+ ASSERT(TriangleMeshContact::isInstance(contact[0]));
+ const TriangleMeshContact& c = static_cast<const TriangleMeshContact&>(contact[0]);
+ if (contact[0].getSurface1() == 0) {
+ verifyContactFaces(pointFaces, 4, c.getSurface1Faces());
+ verifyContactFaces(pointFaces, 4, c.getSurface2Faces());
+ }
+ else {
+ verifyContactFaces(pointFaces, 4, c.getSurface1Faces());
+ verifyContactFaces(pointFaces, 4, c.getSurface2Faces());
+ }
+ }
+}
+
+int main() {
+ try {
+ testHalfSpaceSphere();
+ testSphereSphere();
+ testHalfSpaceEllipsoid();
+ testEllipsoidEllipsoid();
+ testHalfSpaceTriangleMesh();
+ testSphereTriangleMesh();
+ testTriangleMeshTriangleMesh();
+ }
+ catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ cout << "Done" << endl;
+ return 0;
+}
diff --git a/Simbody/tests/TestConstraints.cpp b/Simbody/tests/TestConstraints.cpp
new file mode 100644
index 0000000..5bb00be
--- /dev/null
+++ b/Simbody/tests/TestConstraints.cpp
@@ -0,0 +1,601 @@
+/* -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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"
+#include "SimTKcommon/Testing.h"
+
+#include "../src/ConstraintImpl.h"
+
+using namespace SimTK;
+using namespace std;
+
+const int NUM_BODIES = 10;
+const Real BOND_LENGTH = 0.5;
+
+// Keep constraints satisfied to this tolerance during testing.
+static const Real ConstraintTol = 1e-10;
+
+// Compare two quantities that are expected to agree to constraint tolerance.
+#define CONSTRAINT_TEST(a,b) SimTK_TEST_EQ_TOL(a,b, ConstraintTol)
+
+// Compare two quantities that should have been calculated to machine tolerance
+// given the problem size, which we'll characterize by the number of mobilities.
+// (times 10 after I mangled the numbers -- sherm 110831).
+#define MACHINE_TEST(a,b) SimTK_TEST_EQ_SIZE(a,b, 10*state.getNU())
+
+
+/**
+ * Create a system consisting of a chain of bodies.
+ */
+
+MultibodySystem& createSystem() {
+ MultibodySystem* system = new MultibodySystem();
+ SimbodyMatterSubsystem matter(*system);
+ GeneralForceSubsystem forces(*system);
+ const Real mass = 1.23;
+ Body::Rigid body(MassProperties(mass, Vec3(.1,.2,-.03),
+ mass*UnitInertia(1.1, 1.2, 1.3, .01, -.02, .07)));
+
+ Rotation R_PF(Pi/20, UnitVec3(1,2,3));
+ Rotation R_BM(-Pi/17, UnitVec3(2,.2,3));
+ for (int i = 0; i < NUM_BODIES; ++i) {
+ MobilizedBody& parent = matter.updMobilizedBody(MobilizedBodyIndex(matter.getNumBodies()-1));
+
+ if (i == NUM_BODIES-5) {
+ MobilizedBody::Universal b(
+ parent, Transform(R_PF, Vec3(-.1,.3,.2)),
+ body, Transform(R_BM, Vec3(BOND_LENGTH, 0, 0)));
+ } else if (i == NUM_BODIES-3) {
+ MobilizedBody::Ball b(
+ parent, Transform(R_PF, Vec3(-.1,.3,.2)),
+ body, Transform(R_BM, Vec3(BOND_LENGTH, 0, 0)));
+ } else {
+ MobilizedBody::Gimbal b(
+ parent, Transform(R_PF, Vec3(-.1,.3,.2)),
+ body, Transform(R_BM, Vec3(BOND_LENGTH, 0, 0)));
+ }
+ }
+ return *system;
+}
+
+/**
+ * Create a random state for the system.
+ */
+
+void createState(MultibodySystem& system, State& state, const Vector& qOverride=Vector()) {
+ system.realizeTopology();
+ state = system.getDefaultState();
+ Random::Uniform random;
+ for (int i = 0; i < state.getNY(); ++i)
+ state.updY()[i] = random.getValue();
+ if (qOverride.size())
+ state.updQ() = qOverride;
+ system.realize(state, Stage::Velocity);
+
+ Vector dummy;
+ system.project(state, ConstraintTol);
+ system.realize(state, Stage::Acceleration);
+}
+
+void testBallConstraint() {
+
+ State state;
+ MultibodySystem& system = createSystem();
+ SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
+ MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
+ MobilizedBody& last = matter.updMobilizedBody(MobilizedBodyIndex(NUM_BODIES));
+ Constraint::Ball constraint(first, last);
+ createState(system, state);
+ Vec3 r1 = first.getBodyOriginLocation(state);
+ Vec3 r2 = last.getBodyOriginLocation(state);
+ Vec3 dr = r1-r2;
+ Vec3 v1 = first.getBodyOriginVelocity(state);
+ Vec3 v2 = last.getBodyOriginVelocity(state);
+ Vec3 dv = v1-v2;
+ Vec3 a1 = first.getBodyOriginAcceleration(state);
+ Vec3 a2 = last.getBodyOriginAcceleration(state);
+ Vec3 da = a1-a2;
+ CONSTRAINT_TEST(dr.norm(), 0.0);
+ CONSTRAINT_TEST(dr, constraint.getPositionErrors(state));
+ CONSTRAINT_TEST(dv.norm(), 0.0);
+ CONSTRAINT_TEST(dv, constraint.getVelocityErrors(state));
+ // Accelerations should be satisfied to machine tolerance times the
+ // size of the problem.
+ MACHINE_TEST(da.norm(), 0);
+ MACHINE_TEST(da, constraint.getAccelerationErrors(state));
+ delete &system;
+}
+
+void testConstantAngleConstraint() {
+
+ State state;
+ MultibodySystem& system = createSystem();
+ SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
+ MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
+ MobilizedBody& last = matter.updMobilizedBody(MobilizedBodyIndex(NUM_BODIES));
+ Constraint::ConstantAngle constraint(first, UnitVec3(1, 0, 0), last, UnitVec3(0, 1, 0), 1.1);
+ createState(system, state);
+ Vec3 dir1 = first.getBodyRotation(state)*Vec3(1, 0, 0);
+ Vec3 dir2 = last.getBodyRotation(state)*Vec3(0, 1, 0);
+ Vec3 perpDir = dir1%dir2;
+ Vec3 v1 = first.getBodyAngularVelocity(state);
+ Vec3 v2 = last.getBodyAngularVelocity(state);
+ CONSTRAINT_TEST(dot(dir1, dir2), cos(1.1));
+ CONSTRAINT_TEST(dot(dir1, dir2)-cos(1.1), constraint.getPositionError(state));
+ CONSTRAINT_TEST(dot(v1-v2, perpDir), 0.0);
+ CONSTRAINT_TEST(dot(v1-v2, perpDir), constraint.getVelocityError(state));
+ delete &system;
+}
+
+void testConstantOrientationConstraint() {
+
+ State state;
+ MultibodySystem& system = createSystem();
+ SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
+ MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
+ MobilizedBody& last = matter.updMobilizedBody(MobilizedBodyIndex(NUM_BODIES));
+ Rotation r1(0.2, CoordinateAxis::XCoordinateAxis());
+ Rotation r2(Pi/2, CoordinateAxis::YCoordinateAxis());
+ Constraint::ConstantOrientation constraint(first, r1, last, r2);
+ createState(system, state);
+ Rotation R_G1 = first.getBodyRotation(state);
+ Rotation R_G2 = last.getBodyRotation(state);
+ Vec3 v1 = first.getBodyAngularVelocity(state);
+ Vec3 v2 = last.getBodyAngularVelocity(state);
+ Vec3 a1 = first.getBodyAngularAcceleration(state);
+ Vec3 a2 = last.getBodyAngularAcceleration(state);
+
+ // Extra constraints are required for assembly. Without them, this constraint only guarantees
+ // that the second body's X/Y/Z axis is perpendicular to the first body's Y/Z/X axis.
+
+ // Careful: constraint is x2 perp y1, y2 perp z1, z2 perp x1; this isn't the
+ // same if bodies are interchanged.
+ CONSTRAINT_TEST(dot(R_G2*r2*Vec3(1, 0, 0), R_G1*r1*Vec3(0, 1, 0)), 0.0);
+ CONSTRAINT_TEST(dot(R_G2*r2*Vec3(0, 1, 0), R_G1*r1*Vec3(0, 0, 1)), 0.0);
+ CONSTRAINT_TEST(dot(R_G2*r2*Vec3(0, 0, 1), R_G1*r1*Vec3(1, 0, 0)), 0.0);
+ CONSTRAINT_TEST(v1, v2);
+ // Should match to machine precision for this size problem.
+ MACHINE_TEST(a1, a2);
+ delete &system;
+}
+
+void testConstantSpeedConstraint() {
+
+ State state;
+ MultibodySystem& system = createSystem();
+ SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
+ MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
+ Constraint::ConstantSpeed constraint(first, MobilizerUIndex(1), 0.8);
+ createState(system, state);
+ CONSTRAINT_TEST(first.getOneU(state, 1), 0.8);
+ CONSTRAINT_TEST(first.getOneU(state, 1)-0.8, constraint.getVelocityError(state));
+ MACHINE_TEST(first.getOneUDot(state, 1), 0.0);
+ MACHINE_TEST(first.getOneUDot(state, 1), constraint.getAccelerationError(state));
+ delete &system;
+}
+
+void testNoSlip1DConstraint() {
+
+ State state;
+ MultibodySystem& system = createSystem();
+ SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
+ MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
+ MobilizedBody& last = matter.updMobilizedBody(MobilizedBodyIndex(NUM_BODIES));
+ Vec3 p(1, 0.5, -2.0);
+ UnitVec3 n(0, 1, 0);
+ Constraint::NoSlip1D constraint(matter.Ground(), p, n, first, last);
+ createState(system, state);
+ Vec3 p1 = first.findStationAtGroundPoint(state, p);
+ Vec3 p2 = last.findStationAtGroundPoint(state, p);
+ Vec3 v1 = first.findStationVelocityInGround(state, p1);
+ Vec3 v2 = last.findStationVelocityInGround(state, p2);
+ Vec3 a1 = first.findStationAccelerationInGround(state, p1);
+ Vec3 a2 = last.findStationAccelerationInGround(state, p2);
+ CONSTRAINT_TEST(dot(v1, n), dot(v2, n));
+ CONSTRAINT_TEST(dot(v1-v2, n), constraint.getVelocityError(state));
+ MACHINE_TEST(dot(a1-a2, n), 0.0);
+ MACHINE_TEST(dot(a1-a2, n), constraint.getAccelerationError(state));
+ delete &system;
+}
+
+void testPointInPlaneConstraint() {
+
+ State state;
+ MultibodySystem& system = createSystem();
+ SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
+ MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
+ MobilizedBody& last = matter.updMobilizedBody(MobilizedBodyIndex(NUM_BODIES));
+ UnitVec3 normal(1, 0.5, 0);
+ Real height = 2.0;
+ Vec3 p(1.0, 2.5, -3.0);
+ Constraint::PointInPlane constraint(first, normal, height, last, p);
+ createState(system, state);
+ Vec3 p1 = last.findStationLocationInAnotherBody(state, p, first);
+ Vec3 v1 = last.findStationVelocityInAnotherBody(state, p, first);
+ CONSTRAINT_TEST(dot(p1, normal), height);
+ CONSTRAINT_TEST(dot(p1, normal)-height, constraint.getPositionError(state));
+ CONSTRAINT_TEST(dot(v1, normal), 0.0);
+ CONSTRAINT_TEST(dot(v1, normal), constraint.getVelocityError(state));
+ delete &system;
+}
+
+void testPointOnLineConstraint() {
+
+ State state;
+ MultibodySystem& system = createSystem();
+ SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
+ MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
+ MobilizedBody& last = matter.updMobilizedBody(MobilizedBodyIndex(NUM_BODIES));
+ UnitVec3 dir(1, 0.5, 0);
+ Vec3 base(0.5, -0.5, 2.0);
+ Vec3 p(1.0, 2.5, -3.0);
+ Constraint::PointOnLine constraint(first, dir, base, last, p);
+ createState(system, state);
+ Vec3 p1 = last.findStationLocationInAnotherBody(state, p, first);
+ Vec3 v1 = last.findStationVelocityInAnotherBody(state, p, first);
+ CONSTRAINT_TEST(cross(p1-base, dir).norm(), 0.0);
+ CONSTRAINT_TEST(cross(v1, dir).norm(), 0.0);
+ delete &system;
+}
+
+void testRodConstraint() {
+
+ State state;
+ MultibodySystem& system = createSystem();
+ SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
+ MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
+ MobilizedBody& last = matter.updMobilizedBody(MobilizedBodyIndex(NUM_BODIES));
+ Constraint::Rod constraint(first, last, 3.0);
+ createState(system, state);
+ Vec3 r1 = first.getBodyOriginLocation(state);
+ Vec3 r2 = last.getBodyOriginLocation(state);
+ Vec3 dr = r1-r2;
+ Vec3 v1 = first.getBodyOriginVelocity(state);
+ Vec3 v2 = last.getBodyOriginVelocity(state);
+ Vec3 dv = v1-v2;
+ Vec3 a1 = first.getBodyOriginAcceleration(state);
+ Vec3 a2 = last.getBodyOriginAcceleration(state);
+ Vec3 da = a1-a2;
+ CONSTRAINT_TEST(dr.norm(), 3.0);
+ CONSTRAINT_TEST(dr.norm()-3.0, constraint.getPositionError(state));
+ CONSTRAINT_TEST(dot(dv, dr), 0.0);
+ CONSTRAINT_TEST(dot(dv, dr), constraint.getVelocityError(state));
+ MACHINE_TEST(dot(da, dr), -dv.normSqr());
+ MACHINE_TEST(dot(da, dr)+dv.normSqr(), constraint.getAccelerationError(state));
+ delete &system;
+}
+
+void testWeldConstraint() {
+
+ State state;
+ MultibodySystem& system = createSystem();
+ SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
+ MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
+ MobilizedBody& last = matter.updMobilizedBody(MobilizedBodyIndex(NUM_BODIES));
+ Constraint::Weld constraint(first, last);
+ createState(system, state);
+ CONSTRAINT_TEST(first.getBodyOriginLocation(state), last.getBodyOriginLocation(state));
+ CONSTRAINT_TEST(first.getBodyVelocity(state), last.getBodyVelocity(state));
+ MACHINE_TEST(first.getBodyAcceleration(state), last.getBodyAcceleration(state));
+
+ const Rotation& R_G1 = first.getBodyRotation(state);
+ const Rotation& R_G2 = last.getBodyRotation(state);
+
+ // Extra constraints are required for assembly. Without them, this constraint only guarantees
+ // that the second body's X/Y/Z axis is perpendicular to the first body's Y/Z/X axis.
+
+ // Careful: constraint is x2 perp y1, y2 perp z1, z2 perp x1; this isn't the
+ // same if bodies are interchanged.
+ CONSTRAINT_TEST(dot(R_G2*Vec3(1, 0, 0), R_G1*Vec3(0, 1, 0)), 0.0);
+ CONSTRAINT_TEST(dot(R_G2*Vec3(0, 1, 0), R_G1*Vec3(0, 0, 1)), 0.0);
+ CONSTRAINT_TEST(dot(R_G2*Vec3(0, 0, 1), R_G1*Vec3(1, 0, 0)), 0.0);
+ delete &system;
+}
+
+void testWeldConstraintWithPreAssembly() {
+
+ // Different constraints are required for assembly. Without them, this constraint only guarantees
+ // that the second body's X/Y/Z axis is perpendicular to the first body's Y/Z/X axis. Here we'll
+ // attempt to point all the axes in roughly the right direction prior to Weld-ing them.
+
+ State assemblyState;
+ MultibodySystem& assemblySystem = createSystem();
+ SimbodyMatterSubsystem& assemblyMatter = assemblySystem.updMatterSubsystem();
+ MobilizedBody& afirst = assemblyMatter.updMobilizedBody(MobilizedBodyIndex(1));
+ MobilizedBody& alast = assemblyMatter.updMobilizedBody(MobilizedBodyIndex(NUM_BODIES));
+ Constraint::ConstantAngle(afirst, UnitVec3(1,0,0), alast, UnitVec3(1,0,0), 0.5); // 30 degrees
+ Constraint::ConstantAngle(afirst, UnitVec3(0,1,0), alast, UnitVec3(0,1,0), 0.5);
+ Constraint::ConstantAngle(afirst, UnitVec3(0,0,1), alast, UnitVec3(0,0,1), 0.5);
+ Constraint::Ball(afirst, alast); // take care of translation
+ createState(assemblySystem, assemblyState);
+ delete &assemblySystem;
+
+ // Now rebuild the system using a Weld instead of the three angle constraints, but
+ // transfer the q's from the State we calculated above to use as a starting guess.
+
+ State state;
+ MultibodySystem& system = createSystem();
+ SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
+ MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
+ MobilizedBody& last = matter.updMobilizedBody(MobilizedBodyIndex(NUM_BODIES));
+ Constraint::Weld constraint(first, last);
+ createState(system, state, assemblyState.getQ());
+ CONSTRAINT_TEST(first.getBodyOriginLocation(state), last.getBodyOriginLocation(state));
+ CONSTRAINT_TEST(first.getBodyVelocity(state), last.getBodyVelocity(state));
+ MACHINE_TEST(first.getBodyAcceleration(state), last.getBodyAcceleration(state));
+
+ const Rotation& R_G1 = first.getBodyRotation(state);
+ const Rotation& R_G2 = last.getBodyRotation(state);
+
+ // This is a much more stringent requirement than the one we can ask for without
+ // the preassembly step. Here we expect the frames to be perfectly aligned.
+ CONSTRAINT_TEST(~R_G1.x()*R_G2.x(), 1.);
+ CONSTRAINT_TEST(~R_G1.y()*R_G2.y(), 1.);
+ CONSTRAINT_TEST(~R_G1.z()*R_G2.z(), 1.);
+
+ // Just for fun -- compare Rotation matrices using pointing error.
+ // ASSERT(R_G1.isSameRotationToWithinAngle(R_G2, TOL));
+ CONSTRAINT_TEST(R_G1, R_G2);
+
+ delete &system;
+}
+
+void testDisablingConstraints() {
+
+ State state;
+ MultibodySystem& system = createSystem();
+ SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
+ MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
+ MobilizedBody& last = matter.updMobilizedBody(MobilizedBodyIndex(NUM_BODIES));
+ Constraint::Rod constraint(first, last, 3.0);
+
+ class DisableHandler : public ScheduledEventHandler {
+ public:
+ DisableHandler(Constraint& constraint) : constraint(constraint) {
+ }
+ void handleEvent(State& state, Real accuracy, bool& shouldTerminate) const {
+ constraint.disable(state);
+ }
+ Real getNextEventTime(const State&, bool includeCurrentTime) const {
+ return 4.9;
+ }
+ Constraint& constraint;
+ };
+ system.addEventHandler(new DisableHandler(constraint));
+ createState(system, state);
+ const int numQuaternionsInUse = matter.getNumQuaternionsInUse(state);
+
+ // This is slow if we do it at very tight tolerance.
+ const Real LooserConstraintTol = 1e-6;
+
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setAccuracy(10*LooserConstraintTol);
+ integ.setConstraintTolerance(LooserConstraintTol);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ for (int i = 0; i < 10; i++) {
+ ts.stepTo(i+1);
+ if (i < 4) {
+ CONSTRAINT_TEST(ts.getState().getNQErr(), numQuaternionsInUse+1);
+ CONSTRAINT_TEST(ts.getState().getNUErr(), 1);
+ MACHINE_TEST(ts.getState().getNUDotErr(), 1);
+ Vec3 r1 = first.getBodyOriginLocation(ts.getState());
+ Vec3 r2 = last.getBodyOriginLocation(ts.getState());
+ Vec3 dr = r1-r2;
+ // Verify that the constraint is enforced while enabled.
+ SimTK_TEST_EQ_TOL(dr.norm(), Real(3), LooserConstraintTol);
+ }
+ else {
+ CONSTRAINT_TEST(ts.getState().getNQErr(), numQuaternionsInUse);
+ CONSTRAINT_TEST(ts.getState().getNUErr(), 0);
+ MACHINE_TEST(ts.getState().getNUDotErr(), 0);
+ Vec3 r1 = first.getBodyOriginLocation(ts.getState());
+ Vec3 r2 = last.getBodyOriginLocation(ts.getState());
+ Vec3 dr = r1-r2;
+ // Verify that the constraint is *not* being enforced any more.
+ SimTK_TEST_NOTEQ_TOL(dr.norm(), Real(3), LooserConstraintTol);
+ }
+ }
+ delete &system;
+}
+
+void testConstraintForces() {
+ // Weld one body to ground, push on it, verify that it reacts to match the load.
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+
+ MobilizedBody::Weld welded(matter.Ground(), Transform(),
+ body, Transform());
+
+ MobilizedBody::Free loose(matter.Ground(), Transform(),
+ body, Transform());
+ Constraint::Weld glue(matter.Ground(), Transform(),
+ loose, Transform());
+
+ // Apply forces to the body welded straight to ground.
+ Force::ConstantForce(forces, welded, Vec3(0,0,0), Vec3(1,2,3));
+ Force::ConstantTorque(forces, welded, Vec3(20,30,40));
+
+ // Apply the same forces to the "free" body which is welded by constraint.
+ Force::ConstantForce(forces, loose, Vec3(0,0,0), Vec3(1,2,3));
+ Force::ConstantTorque(forces, loose, Vec3(20,30,40));
+
+ State state = system.realizeTopology();
+ system.realize(state, Stage::Acceleration);
+
+ Vector_<SpatialVec> mobilizerReactions;
+ matter.calcMobilizerReactionForces(state, mobilizerReactions);
+
+ //NOT IMPLEMENTED YET:
+ //cout << "Weld constraint reaction on Ground: " << glue.getWeldReactionOnBody1(state) << endl;
+ //cout << "Weld constraint reaction on Body: " << glue.getWeldReactionOnBody2(state) << endl;
+
+
+ // Note that constraint forces have opposite sign to applied forces, because
+ // we calculate the multiplier lambda from M udot + ~G lambda = f_applied. We'll negate
+ // the calculated multipliers to turn these into applied forces.
+ const Vector mults = -state.getMultipliers();
+ Vector_<SpatialVec> constraintForces;
+ Vector mobilityForces;
+ matter.calcConstraintForcesFromMultipliers(state, mults,
+ constraintForces, mobilityForces);
+
+ MACHINE_TEST(constraintForces[loose.getMobilizedBodyIndex()],
+ mobilizerReactions[welded.getMobilizedBodyIndex()]);
+
+ // This returns just the forces on the weld's two bodies: in this
+ // case Ground and "loose", in that order.
+ Vector_<SpatialVec> glueForces =
+ glue.getConstrainedBodyForcesAsVector(state);
+
+ MACHINE_TEST(-glueForces[1], // watch the sign!
+ mobilizerReactions[welded.getMobilizedBodyIndex()]);
+}
+
+// Test methods for multiplication by the various constraint matrices, and
+// for forming the whole matrices. The equations of motion are:
+// M udot + ~G lambda + f_inertial = f_applied
+// G udot = b
+// qdot = N * u
+// [P]
+// where G = [V], and we are also interested in Pq = P*N^-1 which is the
+// [A]
+// q-space Jacobian Pq = Dperr/Dq. Routines involving these matrices use the
+// constraint error routines; routines involving their transposes use the
+// constraint force routines -- that is, they use a completely different
+// algorithm so need separate tests.
+//
+// The code for working with G and ~G has flags allowing selection of P,V,A
+// submatrices or combinations; that needs testing too. Extracting the
+// right constraint-specific segments in the holonomic, nonholonomic, and
+// acceleration-only arrays is tricky.
+void testConstraintMatrices() {
+ // Create a chain of bodies
+ State state;
+ MultibodySystem& system = createSystem();
+ SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
+ MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
+ MobilizedBody& second = matter.updMobilizedBody(MobilizedBodyIndex(2));
+ MobilizedBody& fifth = matter.updMobilizedBody(MobilizedBodyIndex(5));
+ MobilizedBody& last = matter.updMobilizedBody(MobilizedBodyIndex(NUM_BODIES));
+
+ // Add a body on a free joint to body 5 then weld it.
+ MobilizedBody::Free extra(fifth, Transform(),
+ MassProperties(1, Vec3(.01,.02,.03), Inertia(1,1.1,1.2)), Transform());
+ Constraint::Weld weld(extra, fifth);
+
+ Constraint::Ball ball(first, last);
+ Constraint::ConstantAcceleration accel2(second, MobilizerUIndex(1), .01);
+ Constraint::ConstantSpeed speed5(fifth, MobilizerUIndex(1), .1);
+ createState(system, state);
+
+ const int nu = matter.getNU(state);
+ const int nq = matter.getNQ(state);
+ const int nquat = matter.getNumQuaternionsInUse(state);
+ const int mp = matter.getNQErr(state) - nquat;
+ const int mv = matter.getNUErr(state) - mp;
+ const int ma = matter.getNUDotErr(state) - (mp+mv);
+ const int m = mp+mv+ma;
+
+
+ Matrix G, Gt;
+ matter.calcG(state, G); // use error routines
+ matter.calcGTranspose(state, Gt); // use force routines
+ SimTK_TEST_EQ(G, ~Gt); // match to numerical precision
+
+ // Repeat using matrix views that have non-contiguous columns by
+ // generating them into transposed matrices.
+ Matrix Gx(G.nrow(),G.ncol()), Gtx(Gt.nrow(),Gt.ncol());
+ matter.calcG(state, ~Gtx);
+ matter.calcGTranspose(state, ~Gx);
+ SimTK_TEST_EQ_TOL(G, ~Gtx, 1e-16); // should be identical
+ SimTK_TEST_EQ_TOL(Gt, ~Gx, 1e-16);
+
+ MatrixView P=G(0,0,mp,nu);
+ MatrixView V=G(mp,0,mv,nu);
+ MatrixView A=G(mp+mv,0,ma,nu);
+
+ // Calculate Pqx=P*N^-1 here and compare with Pq.
+ Matrix Pqx(mp, nq);
+ for (int i=0; i < mp; ++i) {
+ matter.multiplyByNInv(state, true, ~P[i], ~Pqx[i]);
+ }
+ Matrix Pq;
+ matter.calcPq(state, Pq);
+ SimTK_TEST_EQ(Pq, Pqx); // to numerical precision
+
+ Matrix Pqt;
+ matter.calcPqTranspose(state, Pqt);
+
+ // Check that multiplication method works like explicit multiplication.
+ Vector lambda = Test::randVector(m);
+ Vector lambdap = Test::randVector(mp);
+ Vector uin = Test::randVector(nu);
+ Vector qin = Test::randVector(nq);
+ Vector aerrOut, perrOut, fuout, fqout;
+
+ matter.multiplyByG(state, uin, aerrOut);
+ SimTK_TEST(aerrOut.size() == m);
+ SimTK_TEST_EQ_SIZE(aerrOut, G*uin, nu);
+
+ matter.multiplyByGTranspose(state, lambda, fuout);
+ SimTK_TEST(fuout.size() == nu);
+ SimTK_TEST_EQ_SIZE(fuout, Gt*lambda, nu);
+
+ matter.multiplyByPq(state, qin, perrOut);
+ SimTK_TEST(perrOut.size() == mp);
+ SimTK_TEST_EQ_SIZE(perrOut, Pq*qin, nq);
+
+ matter.multiplyByPqTranspose(state, lambdap, fqout);
+ SimTK_TEST(fqout.size() == nq);
+ SimTK_TEST_EQ_SIZE(fqout, Pqt*lambdap, nq);
+
+ Matrix MInv;
+ matter.calcMInv(state, MInv);
+ Matrix numGMInvGt = G*MInv*Gt; // O(m^2*n + m*n^2)
+ Matrix GMInvGt;
+ matter.calcProjectedMInv(state, GMInvGt); // O(m*n)
+ SimTK_TEST_EQ(GMInvGt, numGMInvGt);
+
+
+}
+
+int main() {
+ SimTK_START_TEST("TestConstraints");
+ SimTK_SUBTEST(testBallConstraint);
+ SimTK_SUBTEST(testConstantAngleConstraint);
+ SimTK_SUBTEST(testConstantOrientationConstraint);
+ SimTK_SUBTEST(testConstantSpeedConstraint);
+ SimTK_SUBTEST(testNoSlip1DConstraint);
+ SimTK_SUBTEST(testPointInPlaneConstraint);
+ SimTK_SUBTEST(testPointOnLineConstraint);
+ SimTK_SUBTEST(testRodConstraint);
+ SimTK_SUBTEST(testWeldConstraint);
+ SimTK_SUBTEST(testWeldConstraintWithPreAssembly);
+ SimTK_SUBTEST(testConstraintForces);
+ SimTK_SUBTEST(testConstraintMatrices);
+ SimTK_SUBTEST(testDisablingConstraints);
+ SimTK_END_TEST();
+}
diff --git a/Simbody/tests/TestCustomConstraints.cpp b/Simbody/tests/TestCustomConstraints.cpp
new file mode 100644
index 0000000..9e60d9e
--- /dev/null
+++ b/Simbody/tests/TestCustomConstraints.cpp
@@ -0,0 +1,675 @@
+/* -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "Simbody.h"
+
+using namespace SimTK;
+using namespace std;
+
+const int NUM_BODIES = 10;
+const Real BOND_LENGTH = 0.5;
+
+/* This Measure returns the instantaneous power being generated by the
+indicated Constraint */
+template <class T>
+class PowerMeasure : public Measure_<T> {
+public:
+ SimTK_MEASURE_HANDLE_PREAMBLE(PowerMeasure, Measure_<T>);
+
+ PowerMeasure(Subsystem& sub,
+ const Constraint& constraint)
+ : Measure_<T>(sub, new Implementation(constraint), AbstractMeasure::SetHandle()) {}
+ SimTK_MEASURE_HANDLE_POSTSCRIPT(PowerMeasure, Measure_<T>);
+};
+
+
+template <class T>
+class PowerMeasure<T>::Implementation : public Measure_<T>::Implementation {
+public:
+ Implementation(const Constraint& constraint)
+ : Measure_<T>::Implementation(1), m_constraint(constraint) {}
+
+ // Default copy constructor, destructor, copy assignment are fine.
+
+ // Implementations of virtual methods.
+ Implementation* cloneVirtual() const {return new Implementation(*this);}
+ int getNumTimeDerivativesVirtual() const {return 0;}
+ Stage getDependsOnStageVirtual(int order) const
+ { return Stage::Acceleration; }
+
+ void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
+ {
+ SimTK_ASSERT1_ALWAYS(derivOrder==0,
+ "PowerMeasure::Implementation::calcCachedValueVirtual():"
+ " derivOrder %d seen but only 0 allowed.", derivOrder);
+
+ value = m_constraint.calcPower(s);
+ }
+private:
+ const Constraint m_constraint;
+};
+
+/**
+ * A Function that takes a single argument and returns it.
+ */
+
+class LinearFunction : public Function {
+public:
+ Real calcValue(const Vector& x) const {
+ return x[0];
+ }
+ Real calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
+ if (derivComponents.size() == 1)
+ return 1;
+ return 0;
+ }
+ int getArgumentSize() const {
+ return 1;
+ }
+ int getMaxDerivativeOrder() const {
+ return 100;
+ }
+};
+
+/**
+ * A Function that relates three different arguments.
+ */
+
+class CompoundFunction : public Function {
+public:
+ Real calcValue(const Vector& x) const {
+ return 1*x[0]+2*x[1]+3*x[2];
+ }
+ Real calcDerivative(const Array_<int>& derivComponents, const Vector& x) const {
+ if (derivComponents.size() == 1) {
+ return derivComponents[0]+1; // i.e. coef. 1, 2, or 3
+ }
+ return 0;
+ }
+ int getArgumentSize() const {
+ return 3;
+ }
+ int getMaxDerivativeOrder() const {
+ return 2;
+ }
+};
+
+/**
+ * Create a system consisting of a chain of Gimbal joints.
+ */
+
+void createGimbalSystem(MultibodySystem& system) {
+ SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -1, 0), 0);
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ for (int i = 0; i < NUM_BODIES; ++i) {
+ MobilizedBody& parent = matter.updMobilizedBody(MobilizedBodyIndex(matter.getNumBodies()-1));
+ MobilizedBody::Gimbal b(parent, Transform(Vec3(0)), body, Transform(Vec3(BOND_LENGTH, 0, 0)));
+ }
+}
+
+/**
+ * Create a system consisting of a chain of Ball joints.
+ */
+
+void createBallSystem(MultibodySystem& system) {
+ SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -1, 0), 0);
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ for (int i = 0; i < NUM_BODIES; ++i) {
+ MobilizedBody& parent = matter.updMobilizedBody(MobilizedBodyIndex(matter.getNumBodies()-1));
+ MobilizedBody::Ball b(parent, Transform(Vec3(0)), body, Transform(Vec3(BOND_LENGTH, 0, 0)));
+ }
+}
+
+/**
+ * Create a system consisting of a chain of Planar joints.
+ */
+
+void createPlanarSystem(MultibodySystem& system) {
+ SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -1, 0), 0);
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ for (int i = 0; i < NUM_BODIES; ++i) {
+ MobilizedBody& parent = matter.updMobilizedBody(MobilizedBodyIndex(matter.getNumBodies()-1));
+ MobilizedBody::Planar b(parent, Transform(Vec3(0)), body, Transform(Vec3(BOND_LENGTH, 0, 0)));
+ }
+}
+
+/**
+ * Create a system consisting of a chain of Cylinder joints.
+ */
+
+void createCylinderSystem(MultibodySystem& system) {
+ SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
+ GeneralForceSubsystem forces(system);
+ // Skew gravity so moving takes work.
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -2, -3));
+ for (int i = 0; i < NUM_BODIES; ++i) {
+ MobilizedBody& parent =
+ matter.updMobilizedBody(MobilizedBodyIndex(matter.getNumBodies()-1));
+ const Real mass = 1 + 0.1*i;
+ Body::Rigid body(MassProperties(mass, Vec3(0), mass*UnitInertia(1)));
+ MobilizedBody::Cylinder b(parent, Transform(Vec3(.1,.2,.3)),
+ body, Transform(Vec3(BOND_LENGTH, 0, 0)));
+ }
+}
+
+/**
+ * Create a random state for the system.
+ */
+
+void createState(MultibodySystem& system, State& state, const Vector& y=Vector()) {
+ system.realizeTopology();
+ state = system.getDefaultState();
+ if (y.size() > 0)
+ state.updY() = y;
+ else {
+ Random::Uniform random;
+ for (int i = 0; i < state.getNY(); ++i)
+ state.updY()[i] = random.getValue();
+ }
+ system.realize(state, Stage::Velocity);
+
+ // Solve to tight tolerance here
+ system.project(state, 1e-12);
+ system.realize(state, Stage::Acceleration);
+}
+
+void testCoordinateCoupler1() {
+
+ // Create a system using three CoordinateCouplers to fix the orientation
+ // of one body.
+
+ MultibodySystem system1;
+ SimbodyMatterSubsystem matter1(system1);
+ createGimbalSystem(system1);
+ MobilizedBody& first = matter1.updMobilizedBody(MobilizedBodyIndex(1));
+ std::vector<MobilizedBodyIndex> bodies(1);
+ std::vector<MobilizerQIndex> coordinates(1);
+ bodies[0] = MobilizedBodyIndex(1);
+ coordinates[0] = MobilizerQIndex(0);
+ Constraint::CoordinateCoupler coupler1(matter1, new LinearFunction(), bodies, coordinates);
+ coordinates[0] = MobilizerQIndex(1);
+ Constraint::CoordinateCoupler coupler2(matter1, new LinearFunction(), bodies, coordinates);
+ coordinates[0] = MobilizerQIndex(2);
+ Constraint::CoordinateCoupler coupler3(matter1, new LinearFunction(), bodies, coordinates);
+ State state1;
+ createState(system1, state1);
+
+ // Create a system using a ConstantOrientation constraint to do the
+ // same thing.
+
+ MultibodySystem system2;
+ SimbodyMatterSubsystem matter2(system2);
+ createGimbalSystem(system2);
+ Constraint::ConstantOrientation orient(matter2.updGround(), Rotation(),
+ matter2.updMobilizedBody(MobilizedBodyIndex(1)), Rotation());
+ State state2;
+ createState(system2, state2, state1.getY());
+
+ // Compare the results.
+
+ SimTK_TEST_EQ(state1.getQ(), state2.getQ());
+ SimTK_TEST_EQ(state1.getQDot(), state2.getQDot());
+ SimTK_TEST_EQ(state1.getQDotDot(), state2.getQDotDot());
+ SimTK_TEST_EQ(state1.getU(), state2.getU());
+ SimTK_TEST_EQ(state1.getUDot(), state2.getUDot());
+}
+
+void testCoordinateCoupler2() {
+
+ // Create a system involving a constraint that affects multiple mobilizers.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ createCylinderSystem(system);
+ MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
+ std::vector<MobilizedBodyIndex> mobilizers(3);
+ std::vector<MobilizerQIndex> coordinates(3);
+ mobilizers[0] = MobilizedBodyIndex(1);
+ mobilizers[1] = MobilizedBodyIndex(1);
+ mobilizers[2] = MobilizedBodyIndex(5);
+ coordinates[0] = MobilizerQIndex(0);
+ coordinates[1] = MobilizerQIndex(1);
+ coordinates[2] = MobilizerQIndex(1);
+ Function* function = new CompoundFunction();
+ Constraint::CoordinateCoupler coupler(matter, function,
+ mobilizers, coordinates);
+ State state;
+ createState(system, state);
+
+ // Make sure the constraint is satisfied.
+
+ Vector cq(function->getArgumentSize());
+ for (int i = 0; i < cq.size(); ++i)
+ cq[i] = matter.getMobilizedBody(mobilizers[i])
+ .getOneQ(state, coordinates[i]);
+ SimTK_TEST_EQ(0.0, function->calcValue(cq));
+
+ // Simulate it and make sure the constraint is working correctly and
+ // energy is being conserved. This is a workless constraint so the
+ // power should be zer
+ system.realize(state, Stage::Acceleration);
+ Real energy0 = system.calcEnergy(state);
+
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setReturnEveryInternalStep(true);
+ integ.initialize(state);
+ while (integ.getTime() < 10.0) {
+ integ.stepTo(10.0);
+ const State& istate = integ.getState();
+ system.realize(istate, Stage::Acceleration);
+ const Vector& u = istate.getU();
+ const Real energy = system.calcEnergy(istate);
+ const Real power = coupler.calcPower(istate);
+
+
+ for (int i = 0; i < cq.size(); ++i)
+ cq[i] = matter.getMobilizedBody(mobilizers[i])
+ .getOneQ(istate, coordinates[i]);
+ SimTK_TEST_EQ_TOL(0.0, function->calcValue(cq),
+ integ.getConstraintToleranceInUse());
+
+ // Power output should always be zero to machine precision
+ // with some slop for calculation of multipliers.
+ SimTK_TEST_EQ_SIZE(0.0, power, istate.getNU());
+
+ // Energy conservation depends on global integration accuracy;
+ // accuracy returned here is local so we'll fudge at 10X.
+ const Real etol = 10*integ.getAccuracyInUse()
+ *std::max(std::abs(energy), std::abs(energy0));
+ SimTK_TEST_EQ_TOL(energy0, energy, etol);
+ }
+}
+
+void testCoordinateCoupler3() {
+
+ // Create a system involving a constrained body for which qdot != u.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ createBallSystem(system);
+ MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
+ std::vector<MobilizedBodyIndex> bodies(3);
+ std::vector<MobilizerQIndex> coordinates(3);
+ bodies[0] = MobilizedBodyIndex(1);
+ bodies[1] = MobilizedBodyIndex(1);
+ bodies[2] = MobilizedBodyIndex(1);
+ coordinates[0] = MobilizerQIndex(0);
+ coordinates[1] = MobilizerQIndex(1);
+ coordinates[2] = MobilizerQIndex(2);
+ Function* function = new CompoundFunction();
+ Constraint::CoordinateCoupler coupler(matter, function, bodies, coordinates);
+ State state;
+ createState(system, state);
+
+ // Make sure the constraint is satisfied.
+
+ Vector args(function->getArgumentSize());
+ for (int i = 0; i < args.size(); ++i)
+ args[i] = matter.getMobilizedBody(bodies[i]).getOneQ(state, coordinates[i]);
+ SimTK_TEST_EQ(0.0, function->calcValue(args));
+
+ // Simulate it and make sure the constraint is working correctly and
+ // energy is being conserved.
+
+ const Real energy0 = system.calcEnergy(state);
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setReturnEveryInternalStep(true);
+ integ.initialize(state);
+ while (integ.getTime() < 10.0) {
+ integ.stepTo(10.0);
+ const State& istate = integ.getState();
+ const Real energy = system.calcEnergy(istate);
+
+ for (int i = 0; i < args.size(); ++i)
+ args[i] = matter.getMobilizedBody(bodies[i])
+ .getOneQ(integ.getState(), coordinates[i]);
+ // Constraints are applied to unnormalized quaternions. When they are
+ // normalized, that can increase the constraint error. That is why we
+ // need the factor of 3 in the next line.
+ // TODO: Huh? (sherm)
+ SimTK_TEST_EQ_TOL(0.0, function->calcValue(args),
+ 3*integ.getConstraintToleranceInUse());
+
+ // Energy conservation depends on global integration accuracy;
+ // accuracy returned here is local so we'll fudge at 10X.
+ const Real etol = 10*integ.getAccuracyInUse()
+ *std::max(std::abs(energy), std::abs(energy0));
+ SimTK_TEST_EQ_TOL(energy0, energy, etol);
+ }
+}
+
+void testSpeedCoupler1() {
+
+ // Create a system using a SpeedCoupler to fix one speed.
+
+ MultibodySystem system1;
+ SimbodyMatterSubsystem matter1(system1);
+ createGimbalSystem(system1);
+ MobilizedBody& first = matter1.updMobilizedBody(MobilizedBodyIndex(1));
+ std::vector<MobilizedBodyIndex> bodies(1);
+ std::vector<MobilizerUIndex> speeds(1);
+ bodies[0] = MobilizedBodyIndex(1);
+ speeds[0] = MobilizerUIndex(2);
+ Constraint::SpeedCoupler coupler1(matter1, new LinearFunction(), bodies, speeds);
+ State state1;
+ createState(system1, state1);
+
+ // Create a system using a ConstantSpeed constraint to do the same thing.
+
+ MultibodySystem system2;
+ SimbodyMatterSubsystem matter2(system2);
+ createGimbalSystem(system2);
+ Constraint::ConstantSpeed orient(matter2.updMobilizedBody(MobilizedBodyIndex(1)), MobilizerUIndex(2), 0);
+ State state2;
+ createState(system2, state2, state1.getY());
+
+ // Compare the results.
+
+ SimTK_TEST_EQ(state1.getQ(), state2.getQ());
+ SimTK_TEST_EQ(state1.getQDot(), state2.getQDot());
+ SimTK_TEST_EQ(state1.getQDotDot(), state2.getQDotDot());
+ SimTK_TEST_EQ(state1.getU(), state2.getU());
+ SimTK_TEST_EQ(state1.getUDot(), state2.getUDot());
+}
+
+void testSpeedCoupler2() {
+
+ // Create a system involving a constraint that affects three different
+ // bodies.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ createGimbalSystem(system);
+ MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
+ std::vector<MobilizedBodyIndex> bodies(3);
+ std::vector<MobilizerUIndex> speeds(3);
+ bodies[0] = MobilizedBodyIndex(1);
+ bodies[1] = MobilizedBodyIndex(3);
+ bodies[2] = MobilizedBodyIndex(5);
+ speeds[0] = MobilizerUIndex(0);
+ speeds[1] = MobilizerUIndex(0);
+ speeds[2] = MobilizerUIndex(1);
+ Function* function = new CompoundFunction();
+ Constraint::SpeedCoupler coupler(matter, function, bodies, speeds);
+ State state;
+ createState(system, state);
+
+ // Make sure the constraint is satisfied.
+
+ Vector args(function->getArgumentSize());
+ for (int i = 0; i < args.size(); ++i)
+ args[i] = matter.getMobilizedBody(bodies[i]).getOneU(state, speeds[i]);
+ SimTK_TEST_EQ(0.0, function->calcValue(args));
+
+ // Simulate it and make sure the constraint is working correctly and
+ // energy is being conserved. This should be workless and power should
+ // always be zero (to the extent that the constraint is satisfied).
+
+ Real energy0 = system.calcEnergy(state);
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setAccuracy(1e-6);
+ integ.setReturnEveryInternalStep(true);
+ integ.initialize(state);
+ while (integ.getTime() < 10.0) {
+ integ.stepTo(10.0);
+ const State& istate = integ.getState();
+ system.realize(istate, Stage::Acceleration);
+ const Real energy = system.calcEnergy(istate);
+ const Real power = coupler.calcPower(istate);
+
+ for (int i = 0; i < args.size(); ++i)
+ args[i] = matter.getMobilizedBody(bodies[i]).getOneU(istate, speeds[i]);
+ SimTK_TEST_EQ_TOL(0.0, function->calcValue(args),
+ integ.getConstraintToleranceInUse());
+
+ SimTK_TEST_EQ_TOL(0.0, power, 10*integ.getConstraintToleranceInUse());
+
+ // Energy conservation depends on global integration accuracy;
+ // accuracy returned here is local so we'll fudge at 10X.
+ const Real etol = 10*integ.getAccuracyInUse()
+ *std::max(std::abs(energy), std::abs(energy0));
+ SimTK_TEST_EQ_TOL(energy0, energy, etol);
+ }
+}
+
+void testSpeedCoupler3() {
+
+ // Create a system with a constraint that uses both u's and q's.
+ // This will not be workless in general.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ createCylinderSystem(system);
+ MobilizedBody& first = matter.updMobilizedBody(MobilizedBodyIndex(1));
+ std::vector<MobilizedBodyIndex> ubody(2), qbody(1);
+ std::vector<MobilizerUIndex> uindex(2);
+ std::vector<MobilizerQIndex> qindex(1);
+ ubody[0] = MobilizedBodyIndex(1);
+ ubody[1] = MobilizedBodyIndex(3);
+ qbody[0] = MobilizedBodyIndex(5);
+ uindex[0] = MobilizerUIndex(0);
+ uindex[1] = MobilizerUIndex(1);
+ qindex[0] = MobilizerQIndex(1);
+ Function* function = new CompoundFunction();
+ Constraint::SpeedCoupler coupler(matter, function, ubody, uindex,
+ qbody, qindex);
+ PowerMeasure<Real> powMeas(matter, coupler);
+ Measure::Zero zeroMeas(matter);
+ Measure::Integrate workMeas(matter, powMeas, zeroMeas);
+
+ State state;
+ createState(system, state);
+ workMeas.setValue(state, 0); // override createState
+
+ // Make sure the constraint is satisfied.
+
+ Vector args(function->getArgumentSize());
+ args[0] = matter.getMobilizedBody(ubody[0]).getOneU(state, uindex[0]);
+ args[1] = matter.getMobilizedBody(ubody[1]).getOneU(state, uindex[1]);
+ args[2] = matter.getMobilizedBody(qbody[0]).getOneQ(state, qindex[0]);
+ SimTK_TEST_EQ(0.0, function->calcValue(args));
+
+ // Simulate it and make sure the constraint is working correctly.
+ // We don't expect energy to be conserved here but energy minus the
+ // work done by the constraint should be conserved.
+ Real energy0 = system.calcEnergy(state);
+
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setAccuracy(1e-6);
+ integ.setReturnEveryInternalStep(true);
+ integ.initialize(state);
+
+ while (integ.getTime() < 10.0) {
+ integ.stepTo(10.0);
+ const State& istate = integ.getState();
+ system.realize(istate, Stage::Acceleration);
+ const Real energy = system.calcEnergy(istate);
+ const Real power = powMeas.getValue(istate);
+ const Real work = workMeas.getValue(istate);
+
+ args[0] = matter.getMobilizedBody(ubody[0]).getOneU(state, uindex[0]);
+ args[1] = matter.getMobilizedBody(ubody[1]).getOneU(state, uindex[1]);
+ args[2] = matter.getMobilizedBody(qbody[0]).getOneQ(state, qindex[0]);
+ SimTK_TEST_EQ_TOL(0.0, function->calcValue(args),
+ integ.getConstraintToleranceInUse());
+
+ // Energy conservation depends on global integration accuracy;
+ // accuracy returned here is local so we'll fudge at 10X.
+ const Real etol = 10*integ.getAccuracyInUse()
+ *std::max(std::abs(energy-work), std::abs(energy0));
+ SimTK_TEST_EQ_TOL(energy0, energy-work, etol)
+
+ }
+}
+
+void testPrescribedMotion1() {
+
+ // Create a system requiring simple linear motion of one Q. This
+ // may require that the constraint do work.
+ // (The way the cylinder system is structured it only takes work to
+ // keep body one at a uniform velocity; the rest are in free fall.)
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ createCylinderSystem(system);
+ MobilizedBodyIndex body = MobilizedBodyIndex(1);
+ MobilizerQIndex coordinate = MobilizerQIndex(1);
+ Vector coefficients(2);
+ coefficients[0] = 0.1;
+ coefficients[1] = 0.0;
+ Function* function = new Function::Linear(coefficients);
+ Constraint::PrescribedMotion constraint(matter, function, body, coordinate);
+ PowerMeasure<Real> powMeas(matter, constraint);
+ Measure::Zero zeroMeas(matter);
+ Measure::Integrate workMeas(matter, powMeas, zeroMeas);
+
+ State state;
+ createState(system, state);
+ workMeas.setValue(state, 0); // override createState
+
+ // Make sure the constraint is satisfied.
+
+ Vector args(1, state.getTime());
+ SimTK_TEST_EQ(function->calcValue(args),
+ matter.getMobilizedBody(body).getOneQ(state, coordinate));
+
+ // Simulate it and make sure the constraint is working correctly.
+ const Real energy0 = system.calcEnergy(state);
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setReturnEveryInternalStep(true);
+ integ.initialize(state);
+ while (integ.getTime() < 10.0) {
+ integ.stepTo(10.0);
+ const State& istate = integ.getState();
+ system.realize(istate, Stage::Acceleration);
+ const Real energy = system.calcEnergy(istate);
+ const Real power = powMeas.getValue(istate);
+ const Real work = workMeas.getValue(istate);
+
+ Vector args(1, istate.getTime());
+ const Real q = matter.getMobilizedBody(body).getOneQ(istate, coordinate);
+ SimTK_TEST_EQ_TOL(function->calcValue(args), q,
+ integ.getConstraintToleranceInUse());
+
+ // Energy conservation depends on global integration accuracy;
+ // accuracy returned here is local so we'll fudge at 10X.
+ const Real etol = 10*integ.getAccuracyInUse()
+ *std::max(std::abs(energy-work), std::abs(energy0));
+ SimTK_TEST_EQ_TOL(energy0, energy-work, etol)
+ }
+}
+
+void testPrescribedMotion2() {
+
+ // Create a system prescribing the motion of two Qs.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ createCylinderSystem(system);
+ MobilizedBodyIndex body1 = MobilizedBodyIndex(2);
+ MobilizerQIndex coordinate1 = MobilizerQIndex(1);
+ Vector coefficients1(2);
+ coefficients1[0] = 0.1;
+ coefficients1[1] = 0.0;
+ Function* function1 = new Function::Linear(coefficients1);
+ Constraint::PrescribedMotion constraint1(matter, function1, body1, coordinate1);
+ MobilizedBodyIndex body2 = MobilizedBodyIndex(2);
+ MobilizerQIndex coordinate2 = MobilizerQIndex(0);
+ Vector coefficients2(3);
+ coefficients2[0] = 0.5;
+ coefficients2[1] = -0.2;
+ coefficients2[2] = 1.1;
+ Function* function2 = new Function::Polynomial(coefficients2);
+ Constraint::PrescribedMotion constraint2(matter, function2, body2, coordinate2);
+
+ // Must track work done by the constraints in order to check that
+ // energy is conserved.
+ Measure::Zero zeroMeas(matter);
+ PowerMeasure<Real> powMeas1(matter, constraint1);
+ Measure::Integrate workMeas1(matter, powMeas1, zeroMeas);
+ PowerMeasure<Real> powMeas2(matter, constraint2);
+ Measure::Integrate workMeas2(matter, powMeas2, zeroMeas);
+
+ State state;
+ createState(system, state);
+ workMeas1.setValue(state, 0); // override createState
+ workMeas2.setValue(state, 0); // override createState
+
+ // Make sure the constraint is satisfied.
+
+ Vector args(1, state.getTime());
+ SimTK_TEST_EQ(function1->calcValue(args),
+ matter.getMobilizedBody(body1).getOneQ(state, coordinate1));
+ SimTK_TEST_EQ(function2->calcValue(args),
+ matter.getMobilizedBody(body2).getOneQ(state, coordinate2));
+
+ // Simulate it and make sure the constraint is working correctly and energy is being conserved.
+ const Real energy0 = system.calcEnergy(state);
+
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setReturnEveryInternalStep(true);
+ integ.initialize(state);
+ while (integ.getTime() < 10.0) {
+ integ.stepTo(10.0);
+ const State& istate = integ.getState();
+ system.realize(istate, Stage::Acceleration);
+ const Real energy = system.calcEnergy(istate);
+ const Real power1 = powMeas1.getValue(istate);
+ const Real work1 = workMeas1.getValue(istate);
+ const Real power2 = powMeas2.getValue(istate);
+ const Real work2 = workMeas2.getValue(istate);
+
+ Vector args(1, istate.getTime());
+ SimTK_TEST_EQ_TOL(function1->calcValue(args),
+ matter.getMobilizedBody(body1).getOneQ(istate, coordinate1),
+ integ.getConstraintToleranceInUse());
+ SimTK_TEST_EQ_TOL(function2->calcValue(args),
+ matter.getMobilizedBody(body2).getOneQ(istate, coordinate2),
+ integ.getConstraintToleranceInUse());
+
+ // Energy conservation depends on global integration accuracy;
+ // accuracy returned here is local so we'll fudge at 10X.
+ const Real etol = 10*integ.getAccuracyInUse()
+ *std::max(std::abs(energy-(work1+work2)), std::abs(energy0));
+ SimTK_TEST_EQ_TOL(energy0, energy-(work1+work2), etol)
+ }
+}
+
+int main() {
+ SimTK_START_TEST("TestCustomConstraints");
+ SimTK_SUBTEST(testCoordinateCoupler1);
+ SimTK_SUBTEST(testCoordinateCoupler2);
+ SimTK_SUBTEST(testCoordinateCoupler3);
+ SimTK_SUBTEST(testSpeedCoupler1);
+ SimTK_SUBTEST(testSpeedCoupler2);
+ SimTK_SUBTEST(testSpeedCoupler3);
+ SimTK_SUBTEST(testPrescribedMotion1);
+ SimTK_SUBTEST(testPrescribedMotion2);
+ SimTK_END_TEST();
+}
diff --git a/Simbody/tests/TestCustomMobilizedBodies.cpp b/Simbody/tests/TestCustomMobilizedBodies.cpp
new file mode 100644
index 0000000..6a7741f
--- /dev/null
+++ b/Simbody/tests/TestCustomMobilizedBodies.cpp
@@ -0,0 +1,478 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 <class T>
+void assertEqual(T val1, T val2) {
+ ASSERT(abs(val1-val2) < TOL);
+}
+
+template <int N>
+void assertEqual(Vec<N> val1, Vec<N> val2) {
+ for (int i = 0; i < N; ++i)
+ ASSERT(abs(val1[i]-val2[i]) < TOL);
+}
+
+template<>
+void assertEqual(Vector val1, Vector val2) {
+ ASSERT(val1.size() == val2.size());
+ for (int i = 0; i < val1.size(); ++i)
+ assertEqual(val1[i], val2[i]);
+}
+
+template<>
+void assertEqual(SpatialVec val1, SpatialVec val2) {
+ assertEqual(val1[0], val2[0]);
+ assertEqual(val1[1], val2[1]);
+}
+
+template<>
+void assertEqual(Transform val1, Transform val2) {
+ assertEqual(val1.p(), val2.p());
+ ASSERT(val1.R().isSameRotationToWithinAngle(val2.R(), TOL));
+}
+
+/**
+ * This is a custom MobilizedBody that is identical to MobilizedBody::Translation.
+ */
+
+class CustomTranslation : public MobilizedBody::Custom::Implementation {
+public:
+ CustomTranslation(SimbodyMatterSubsystem& matter) : Implementation(matter, 3, 3, 0) {
+ }
+ Implementation* clone() const {
+ return new CustomTranslation(*this);
+ }
+ Transform calcMobilizerTransformFromQ(const State& s, int nq, const Real* q) const {
+ ASSERT(nq == 3);
+ return Transform(Vec3(q[0], q[1], q[2]));
+ }
+ SpatialVec multiplyByHMatrix(const State& s, int nu, const Real* u) const {
+ ASSERT(nu == 3);
+ return SpatialVec(Vec3(0), Vec3(u[0], u[1], u[2]));
+ }
+ void multiplyByHTranspose(const State& s, const SpatialVec& F, int nu, Real* f) const {
+ ASSERT(nu == 3);
+ Vec3::updAs(f) = F[1];
+ }
+ SpatialVec multiplyByHDotMatrix(const State& s, int nu, const Real* u) const {
+ ASSERT(nu == 3);
+ return SpatialVec(Vec3(0), Vec3(0));
+ }
+ void multiplyByHDotTranspose(const State& s, const SpatialVec& F, int nu, Real* f) const {
+ ASSERT(nu == 3);
+ Vec3::updAs(f) = Vec3(0);
+ }
+ void setQToFitTransform(const State&, const Transform& X_FM, int nq, Real* q) const {
+ ASSERT(nq == 3);
+ Vec3::updAs(q) = X_FM.p();
+ }
+ void setUToFitVelocity(const State&, const SpatialVec& V_FM, int nu, Real* u) const {
+ ASSERT(nu == 3);
+ Vec3::updAs(u) = V_FM[1];
+ }
+};
+
+/**
+ * This is a custom MobilizedBody that is identical to MobilizedBody::Ball.
+ */
+
+class CustomBall : public MobilizedBody::Custom::Implementation {
+public:
+ CustomBall(SimbodyMatterSubsystem& matter) : Implementation(matter, 3, 4, 4) {
+ }
+ Implementation* clone() const {
+ return new CustomBall(*this);
+ }
+ Transform calcMobilizerTransformFromQ(const State& s, int nq, const Real* q) const {
+ Transform t(Vec3(0));
+ if (getUseEulerAngles(s)) {
+ ASSERT(nq == 3);
+ t.updR().setRotationToBodyFixedXYZ(Vec3::getAs(q));
+ }
+ else {
+ ASSERT(nq == 4);
+ t.updR().setRotationFromQuaternion(Quaternion(Vec4::getAs(q)));
+ }
+ return t;
+
+ }
+ SpatialVec multiplyByHMatrix(const State& s, int nu, const Real* u) const {
+ ASSERT(nu == 3);
+ return SpatialVec(Vec3(u[0], u[1], u[2]), Vec3(0));
+ }
+ void multiplyByHTranspose(const State& s, const SpatialVec& F, int nu, Real* f) const {
+ ASSERT(nu == 3);
+ Vec3::updAs(f) = F[0];
+ }
+ SpatialVec multiplyByHDotMatrix(const State& s, int nu, const Real* u) const {
+ ASSERT(nu == 3);
+ return SpatialVec(Vec3(0), Vec3(0));
+ }
+ void multiplyByHDotTranspose(const State& s, const SpatialVec& F, int nu, Real* f) const {
+ ASSERT(nu == 3);
+ Vec3::updAs(f) = Vec3(0);
+ }
+ void multiplyByN(const State& s, bool transposeMatrix, int nIn, const Real* in, int nOut, Real* out) const {
+ const Vector q = getQ(s);
+ if (getUseEulerAngles(s)) {
+ ASSERT(nIn == 3 && nOut == 3);
+ Rotation R_FM;
+ R_FM.setRotationToBodyFixedXYZ(Vec3::getAs(&q[0]));
+ const Mat33 N = Rotation::calcNForBodyXYZInBodyFrame(Vec3::getAs(&q[0])) * ~R_FM;
+ if (transposeMatrix) Row3::updAs(out) = Row3::getAs(in) * N;
+ else Vec3::updAs(out) = N * Vec3::getAs(in);
+ }
+ else {
+ if (transposeMatrix)
+ ASSERT(nIn == 4 && nOut == 3)
+ else
+ ASSERT(nIn == 3 && nOut == 4)
+ const Mat43 N = Rotation::calcUnnormalizedNForQuaternion(Vec4::getAs(&q[0]));
+ if (transposeMatrix) Row3::updAs(out) = Row4::getAs(in) * N;
+ else Vec4::updAs(out) = N * Vec3::getAs(in);
+ }
+ }
+ void multiplyByNInv(const State& s, bool transposeMatrix, int nIn, const Real* in, int nOut, Real* out) const {
+ const Vector q = getQ(s);
+ if (getUseEulerAngles(s)) {
+ ASSERT(nIn == 3 && nOut == 3);
+ Rotation R_FM;
+ R_FM.setRotationToBodyFixedXYZ(Vec3::getAs(&q[0]));
+ const Mat33 NInv = R_FM*Rotation::calcNInvForBodyXYZInBodyFrame(Vec3::getAs(&q[0]));
+ if (transposeMatrix) Row3::updAs(out) = Row3::getAs(in) * NInv;
+ else Vec3::updAs(out) = NInv * Vec3::getAs(in);
+ }
+ else {
+ if (transposeMatrix)
+ ASSERT(nIn == 3 && nOut == 4)
+ else
+ ASSERT(nIn == 4 && nOut == 3)
+ const Mat34 NInv = Rotation::calcUnnormalizedNInvForQuaternion(Vec4::getAs(&q[0]));
+ if (transposeMatrix) Row4::updAs(out) = Row3::getAs(in) * NInv;
+ else Vec3::updAs(out) = NInv * Vec4::getAs(in);
+ }
+ }
+ void multiplyByNDot(const State& s, bool transposeMatrix, int nIn, const Real* in, int nOut, Real* out) const {
+ const Vector q = getQ(s);
+ if (getUseEulerAngles(s)) {
+ ASSERT(nIn == 3 && nOut == 3);
+ const Rotation& R_FM = getMobilizerTransform(s).R();
+ Vec3::updAs(out) = Rotation::convertAngVelDotInBodyFrameToBodyXYZDotDot(Vec3::getAs(&q[0]), ~R_FM*Vec3::getAs(in), Vec3(0));
+ }
+ else if (transposeMatrix) {
+ ASSERT(nIn == 4 && nOut == 3)
+ ASSERT(false); // I didn't bother to implement this case, since it currently is never used.
+ }
+ else {
+ ASSERT(nIn == 3 && nOut == 4)
+ Vec4::updAs(out) = Rotation::convertAngVelDotToQuaternionDotDot(Vec4::getAs(&q[0]), Vec3::getAs(in), Vec3(0));
+ }
+ }
+ void setQToFitTransform(const State& s, const Transform& X_FM, int nq, Real* q) const {
+ if (getUseEulerAngles(s)) {
+ ASSERT(nq == 3);
+ Vec3::updAs(q) = X_FM.R().convertRotationToBodyFixedXYZ();
+ }
+ else {
+ ASSERT(nq == 4);
+ Vec4::updAs(q) = X_FM.R().convertRotationToQuaternion().asVec4();
+ }
+ }
+ void setUToFitVelocity(const State& s, const SpatialVec& V_FM, int nu, Real* u) const {
+ ASSERT(nu == 3);
+ Vec3::updAs(u) = V_FM[0];
+ }
+};
+
+/**
+ * This is a custom MobilizedBody that is identical to MobilizedBody::Free.
+ */
+
+class CustomFree : public MobilizedBody::Custom::Implementation {
+public:
+ CustomFree(SimbodyMatterSubsystem& matter) : Implementation(matter, 6, 7, 4) {
+ }
+ Implementation* clone() const {
+ return new CustomFree(*this);
+ }
+ Transform calcMobilizerTransformFromQ(const State& s, int nq, const Real* q) const {
+ Transform t(Vec3::getAs(&q[nq-3]));
+ if (getUseEulerAngles(s)) {
+ ASSERT(nq == 6);
+ t.updR().setRotationToBodyFixedXYZ(Vec3::getAs(q));
+ }
+ else {
+ ASSERT(nq == 7);
+ t.updR().setRotationFromQuaternion(Quaternion(Vec4::getAs(q)));
+ }
+ return t;
+
+ }
+ SpatialVec multiplyByHMatrix(const State& s, int nu, const Real* u) const {
+ ASSERT(nu == 6);
+ return SpatialVec(Vec3(u[0], u[1], u[2]), Vec3(u[3], u[4], u[5]));
+ }
+ void multiplyByHTranspose(const State& s, const SpatialVec& F, int nu, Real* f) const {
+ ASSERT(nu == 6);
+ SpatialVec::updAs(reinterpret_cast<Vec3*>(f)) = F;
+ }
+ SpatialVec multiplyByHDotMatrix(const State& s, int nu, const Real* u) const {
+ ASSERT(nu == 6);
+ return SpatialVec(Vec3(0), Vec3(0));
+ }
+ void multiplyByHDotTranspose(const State& s, const SpatialVec& F, int nu, Real* f) const {
+ ASSERT(nu == 6);
+ SpatialVec::updAs(reinterpret_cast<Vec3*>(f)) = SpatialVec(Vec3(0), Vec3(0));
+ }
+ void multiplyByN(const State& s, bool transposeMatrix, int nIn, const Real* in, int nOut, Real* out) const {
+ const Vector q = getQ(s);
+ if (getUseEulerAngles(s)) {
+ ASSERT(nIn == 6 && nOut == 6);
+ Rotation R_FM;
+ R_FM.setRotationToBodyFixedXYZ(Vec3::getAs(&q[0]));
+ const Mat33 N = Rotation::calcNForBodyXYZInBodyFrame(Vec3::getAs(&q[0])) * ~R_FM;
+ if (transposeMatrix) Row3::updAs(out) = Row3::getAs(in) * N;
+ else Vec3::updAs(out) = N * Vec3::getAs(in);
+ }
+ else {
+ if (transposeMatrix)
+ ASSERT(nIn == 7 && nOut == 6)
+ else
+ ASSERT(nIn == 6 && nOut == 7)
+ const Mat43 N = Rotation::calcUnnormalizedNForQuaternion(Vec4::getAs(&q[0]));
+ if (transposeMatrix) Row3::updAs(out) = Row4::getAs(in) * N;
+ else Vec4::updAs(out) = N * Vec3::getAs(in);
+ }
+ Vec3::updAs(&out[nOut-3]) = Vec3::getAs(&in[nIn-3]);
+ }
+ void multiplyByNInv(const State& s, bool transposeMatrix, int nIn, const Real* in, int nOut, Real* out) const {
+ const Vector q = getQ(s);
+ if (getUseEulerAngles(s)) {
+ ASSERT(nIn == 6 && nOut == 6);
+ Rotation R_FM;
+ R_FM.setRotationToBodyFixedXYZ(Vec3::getAs(&q[0]));
+ const Mat33 NInv = R_FM*Rotation::calcNInvForBodyXYZInBodyFrame(Vec3::getAs(&q[0]));
+ if (transposeMatrix) Row3::updAs(out) = Row3::getAs(in) * NInv;
+ else Vec3::updAs(out) = NInv * Vec3::getAs(in);
+ }
+ else {
+ if (transposeMatrix)
+ ASSERT(nIn == 6 && nOut == 7)
+ else
+ ASSERT(nIn == 7 && nOut == 6)
+ const Mat34 NInv = Rotation::calcUnnormalizedNInvForQuaternion(Vec4::getAs(&q[0]));
+ if (transposeMatrix) Row4::updAs(out) = Row3::getAs(in) * NInv;
+ else Vec3::updAs(out) = NInv * Vec4::getAs(in);
+ }
+ Vec3::updAs(&out[nOut-3]) = Vec3::getAs(&in[nIn-3]);
+ }
+ void multiplyByNDot(const State& s, bool transposeMatrix, int nIn, const Real* in, int nOut, Real* out) const {
+ const Vector q = getQ(s);
+ if (getUseEulerAngles(s)) {
+ ASSERT(nIn == 6 && nOut == 6);
+ const Rotation& R_FM = getMobilizerTransform(s).R();
+ Vec3::updAs(out) = Rotation::convertAngVelDotInBodyFrameToBodyXYZDotDot(Vec3::getAs(&q[0]), ~R_FM*Vec3::getAs(in), Vec3(0));
+ }
+ else if (transposeMatrix) {
+ ASSERT(nIn == 7 && nOut == 6)
+ ASSERT(false); // I didn't bother to implement this case, since it currently is never used.
+ }
+ else {
+ ASSERT(nIn == 6 && nOut == 7)
+ Vec4::updAs(out) = Rotation::convertAngVelDotToQuaternionDotDot(Vec4::getAs(&q[0]), Vec3::getAs(in), Vec3(0));
+ }
+ Vec3::updAs(&out[nOut-3]) = Vec3(0);
+ }
+ void setQToFitTransform(const State& s, const Transform& X_FM, int nq, Real* q) const {
+ if (getUseEulerAngles(s)) {
+ ASSERT(nq == 6);
+ Vec3::updAs(q) = X_FM.R().convertRotationToBodyFixedXYZ();
+ }
+ else {
+ ASSERT(nq == 7);
+ Vec4::updAs(q) = X_FM.R().convertRotationToQuaternion().asVec4();
+ }
+ Vec3::updAs(&q[nq-3]) = X_FM.p();
+ }
+ void setUToFitVelocity(const State& s, const SpatialVec& V_FM, int nu, Real* u) const {
+ ASSERT(nu == 6);
+ SpatialVec::updAs(reinterpret_cast<Vec3*>(u)) = V_FM;
+ }
+};
+
+void compareMobilizedBodies(const MobilizedBody& b1, const MobilizedBody& b2, bool eulerAngles, int expectedQ, int expectedU) {
+ const SimbodyMatterSubsystem& matter = b1.getMatterSubsystem();
+ const System& system = matter.getSystem();
+
+ // Set whether to use Euler angles.
+
+ State state = system.getDefaultState();
+ matter.setUseEulerAngles(state, eulerAngles);
+ system.realizeModel(state);
+
+ // Make sure the number of state variables is correct.
+
+ assertEqual(b1.getNumQ(state), expectedQ);
+ assertEqual(b1.getNumU(state), expectedU);
+ assertEqual(b2.getNumQ(state), expectedQ);
+ assertEqual(b2.getNumU(state), expectedU);
+
+ // Set all the state variables to random values.
+
+ Random::Gaussian random;
+ int nq = state.getNQ()/2;
+ for (int i = 0; i < nq; ++i)
+ state.updQ()[i] = state.updQ()[i+nq] = random.getValue();
+ int nu = state.getNU()/2;
+ for (int i = 0; i < nu; ++i)
+ state.updU()[i] = state.updU()[i+nu] = random.getValue();
+ system.realize(state, Stage::Acceleration);
+
+ // Compare state variables and their derivatives.
+
+ for (int i = 0; i < b1.getNumQ(state); ++i) {
+ assertEqual(b1.getOneQ(state, i), b2.getOneQ(state, i));
+ assertEqual(b1.getOneQDot(state, i), b2.getOneQDot(state, i));
+ assertEqual(b1.getOneQDotDot(state, i), b2.getOneQDotDot(state, i));
+ }
+ for (int i = 0; i < b1.getNumU(state); ++i) {
+ assertEqual(b1.getOneU(state, i), b2.getOneU(state, i));
+ assertEqual(b1.getOneUDot(state, i), b2.getOneUDot(state, i));
+ }
+
+ // Compare lots of properties of the two bodies.
+
+ assertEqual(b1.getBodyTransform(state), b2.getBodyTransform(state));
+ assertEqual(b1.getBodyVelocity(state), b2.getBodyVelocity(state));
+ assertEqual(b1.getBodyAcceleration(state), b2.getBodyAcceleration(state));
+ assertEqual(b1.getBodyOriginLocation(state), b2.getBodyOriginLocation(state));
+ assertEqual(b1.getBodyOriginVelocity(state), b2.getBodyOriginVelocity(state));
+ assertEqual(b1.getBodyOriginAcceleration(state), b2.getBodyOriginAcceleration(state));
+ assertEqual(b1.getMobilizerTransform(state), b2.getMobilizerTransform(state));
+ assertEqual(b1.getMobilizerVelocity(state), b2.getMobilizerVelocity(state));
+
+ // Test methods that multiply by various matrices.
+
+ Vector tempq(state.getNQ());
+ Vector tempu(state.getNU());
+ matter.multiplyByN(state, false, state.getU(), tempq);
+ for (int i = 0; i < b1.getNumQ(state); ++i)
+ assertEqual(b1.getOneFromQPartition(state, i, tempq), b2.getOneFromQPartition(state, i, tempq));
+ matter.multiplyByN(state, true, state.getQ(), tempu);
+ for (int i = 0; i < b1.getNumU(state); ++i)
+ assertEqual(b1.getOneFromUPartition(state, i, tempu), b2.getOneFromUPartition(state, i, tempu));
+ matter.multiplyByNInv(state, false, state.getQ(), tempu);
+ for (int i = 0; i < b1.getNumU(state); ++i)
+ assertEqual(b1.getOneFromUPartition(state, i, tempu), b2.getOneFromUPartition(state, i, tempu));
+ matter.multiplyByNInv(state, true, state.getU(), tempq);
+ for (int i = 0; i < b1.getNumQ(state); ++i)
+ assertEqual(b1.getOneFromQPartition(state, i, tempq), b2.getOneFromQPartition(state, i, tempq));
+
+ // Have them calculate q and u, and see if they agree.
+
+ Transform t(Rotation(random.getValue(), Vec3(random.getValue(), random.getValue(), random.getValue())), Vec3(random.getValue(), random.getValue(), random.getValue()));
+ b1.setQToFitTransform(state, t);
+ b2.setQToFitTransform(state, t);
+ assertEqual(b1.getQAsVector(state), b2.getQAsVector(state));
+ SpatialVec v(Vec3(random.getValue(), random.getValue(), random.getValue()), Vec3(random.getValue(), random.getValue(), random.getValue()));
+ b1.setUToFitVelocity(state, v);
+ b2.setUToFitVelocity(state, v);
+ assertEqual(b1.getUAsVector(state), b2.getUAsVector(state));
+
+ // Simulate the system, and see if the two bodies remain identical.
+
+ VerletIntegrator integ(system);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(1.0);
+ assertEqual(b1.getQAsVector(integ.getState()), b2.getQAsVector(integ.getState()));
+ assertEqual(b1.getUAsVector(integ.getState()), b2.getUAsVector(integ.getState()));
+}
+
+void testCustomTranslation() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ MobilizedBody::Translation t1(matter.Ground(), body);
+ MobilizedBody::Translation t2(t1, body);
+ MobilizedBody::Custom c1(matter.Ground(), new CustomTranslation(matter), body);
+ MobilizedBody::Custom c2(c1, new CustomTranslation(matter), body);
+ system.realizeTopology();
+ compareMobilizedBodies(t2, c2, false, 3, 3);
+ compareMobilizedBodies(t2, c2, true, 3, 3);
+}
+
+void testCustomBall() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ MobilizedBody::Ball t1(matter.Ground(), body);
+ MobilizedBody::Ball t2(t1, Vec3(1, 0, 0), body, Vec3(0, 2, 0));
+ MobilizedBody::Custom c1(matter.Ground(), new CustomBall(matter), body);
+ MobilizedBody::Custom c2(c1, new CustomBall(matter), Vec3(1, 0, 0), body, Vec3(0, 2, 0));
+ system.realizeTopology();
+ compareMobilizedBodies(t2, c2, false, 4, 3);
+ compareMobilizedBodies(t2, c2, true, 3, 3);
+}
+
+void testCustomFree() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ MobilizedBody::Free t1(matter.Ground(), body);
+ MobilizedBody::Free t2(t1, body);
+ MobilizedBody::Custom c1(matter.Ground(), new CustomFree(matter), body);
+ MobilizedBody::Custom c2(c1, new CustomFree(matter), body);
+ system.realizeTopology();
+ compareMobilizedBodies(t2, c2, false, 7, 6);
+ compareMobilizedBodies(t2, c2, true, 6, 6);
+}
+
+int main() {
+ try {
+ testCustomTranslation();
+ testCustomBall();
+ testCustomFree();
+ }
+ catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ cout << "Done" << endl;
+ return 0;
+}
diff --git a/Simbody/tests/TestElasticFoundationForce.cpp b/Simbody/tests/TestElasticFoundationForce.cpp
new file mode 100644
index 0000000..b496fb4
--- /dev/null
+++ b/Simbody/tests/TestElasticFoundationForce.cpp
@@ -0,0 +1,143 @@
+/* -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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-5;
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+template <class T>
+void assertEqual(T val1, T val2) {
+ ASSERT(abs(val1-val2) < TOL || abs(val1-val2)/max(abs(val1), abs(val2)) < TOL);
+}
+
+template <int N>
+void assertEqual(Vec<N> val1, Vec<N> val2) {
+ for (int i = 0; i < N; ++i)
+ assertEqual(val1[i], val2[i]);
+}
+
+void testForces() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralContactSubsystem contacts(system);
+ GeneralForceSubsystem forces(system);
+
+ // Create a triangle mesh in the shape of a pyramid, with the
+ // square base having area 1 (split into two triangles).
+
+ vector<Vec3> vertices;
+ vertices.push_back(Vec3(0, 0, 0));
+ vertices.push_back(Vec3(1, 0, 0));
+ vertices.push_back(Vec3(1, 0, 1));
+ vertices.push_back(Vec3(0, 0, 1));
+ vertices.push_back(Vec3(0.5, 1, 0.5));
+ vector<int> faceIndices;
+ int faces[6][3] = {{0, 1, 2}, {0, 2, 3}, {1, 0, 4},
+ {2, 1, 4}, {3, 2, 4}, {0, 3, 4}};
+ for (int i = 0; i < 6; i++)
+ for (int j = 0; j < 3; j++)
+ faceIndices.push_back(faces[i][j]);
+
+ // Create the mobilized bodies and configure the contact model.
+
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ ContactSetIndex setIndex = contacts.createContactSet();
+ MobilizedBody::Translation mesh(matter.updGround(), Transform(), body, Transform());
+ contacts.addBody(setIndex, mesh, ContactGeometry::TriangleMesh(vertices, faceIndices), Transform());
+ contacts.addBody(setIndex, matter.updGround(), ContactGeometry::HalfSpace(), Transform(Rotation(-0.5*Pi, ZAxis), Vec3(0))); // y < 0
+ ElasticFoundationForce ef(forces, contacts, setIndex);
+ Real stiffness = 1e9, dissipation = 0.01, us = 0.1, ud = 0.05, uv = 0.01, vt = 0.01;
+ ef.setBodyParameters(ContactSurfaceIndex(0), stiffness, dissipation, us, ud, uv);
+ ef.setTransitionVelocity(vt);
+ ASSERT(ef.getTransitionVelocity() == vt);
+ State state = system.realizeTopology();
+
+ // Position the pyramid at a variety of positions and check the normal
+ // force.
+
+ for (Real depth = -0.1; depth < 0.1; depth += 0.01) {
+ mesh.setQToFitTranslation(state, Vec3(0, -depth, 0));
+ system.realize(state, Stage::Dynamics);
+ Real f = 0;
+ if (depth > 0)
+ f = stiffness*depth;
+ assertEqual(system.getRigidBodyForces(state, Stage::Dynamics)[mesh.getMobilizedBodyIndex()][1], Vec3(0, f, 0));
+ assertEqual(system.getRigidBodyForces(state, Stage::Dynamics)[matter.getGround().getMobilizedBodyIndex()][1], Vec3(0, -f, 0));
+ }
+
+ // Now do it with a vertical velocity and see if the dissipation force is correct.
+
+ for (Real depth = -0.105; depth < 0.1; depth += 0.01) {
+ mesh.setQToFitTranslation(state, Vec3(0, -depth, 0));
+ for (Real v = -1.0; v <= 1.0; v += 0.1) {
+ mesh.setUToFitLinearVelocity(state, Vec3(0, -v, 0));
+ system.realize(state, Stage::Dynamics);
+ Real f = (depth > 0 ? stiffness*depth*(1+dissipation*v) : 0);
+ if (f < 0)
+ f = 0;
+ assertEqual(system.getRigidBodyForces(state, Stage::Dynamics)[mesh.getMobilizedBodyIndex()][1], Vec3(0, f, 0));
+ }
+ }
+
+ // Do it with a horizontal velocity and see if the friction force is correct.
+
+ Vector_<SpatialVec> expectedForce(matter.getNumBodies());
+ for (Real depth = -0.105; depth < 0.1; depth += 0.01) {
+ mesh.setQToFitTranslation(state, Vec3(0, -depth, 0));
+ Real fh = 0;
+ if (depth > 0)
+ fh = stiffness*depth;
+ for (Real v = -1.0; v <= 1.0; v += 0.1) {
+ mesh.setUToFitLinearVelocity(state, Vec3(v, 0, 0));
+ system.realize(state, Stage::Dynamics);
+ const Real vrel = std::abs(v/vt);
+ Real ff = (v < 0 ? 1 : -1)*fh*(std::min(vrel, 1.0)*(ud+2*(us-ud)/(1+vrel*vrel))+uv*std::fabs(v));
+ const Vec3 totalForce = Vec3(ff, fh, 0);
+ expectedForce = SpatialVec(Vec3(0), Vec3(0));
+ Vec3 contactPoint1 = mesh.findStationAtGroundPoint(state, Vec3(2.0/3.0, 0, 1.0/3.0));
+ mesh.applyForceToBodyPoint(state, contactPoint1, 0.5*totalForce, expectedForce);
+ Vec3 contactPoint2 = mesh.findStationAtGroundPoint(state, Vec3(1.0/3.0, 0, 2.0/3.0));
+ mesh.applyForceToBodyPoint(state, contactPoint2, 0.5*totalForce, expectedForce);
+ SpatialVec actualForce = system.getRigidBodyForces(state, Stage::Dynamics)[mesh.getMobilizedBodyIndex()];
+ assertEqual(actualForce[0], expectedForce[mesh.getMobilizedBodyIndex()][0]);
+ assertEqual(actualForce[1], expectedForce[mesh.getMobilizedBodyIndex()][1]);
+ }
+ }
+}
+
+int main() {
+ try {
+ testForces();
+ }
+ catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ cout << "Done" << endl;
+ return 0;
+}
diff --git a/Simbody/tests/TestForces.cpp b/Simbody/tests/TestForces.cpp
new file mode 100644
index 0000000..250fc93
--- /dev/null
+++ b/Simbody/tests/TestForces.cpp
@@ -0,0 +1,372 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 int NUM_BODIES = 10;
+const Real BOND_LENGTH = 0.5;
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+#define ASSERT_EQUAL(val1, val2) {ASSERT(std::abs(val1-val2) < 1e-10);}
+
+void verifyForces(const Force& force, const State& state, Vector_<SpatialVec> bodyForces, Vector_<Vec3> particleForces, Vector mobilityForces) {
+ Vector_<SpatialVec> actualBodyForces(bodyForces.size());
+ Vector_<Vec3> actualParticleForces(particleForces.size());
+ Vector actualMobilityForces(mobilityForces.size());
+ force.calcForceContribution(state, actualBodyForces, actualParticleForces,
+ actualMobilityForces);
+
+ for (int i = 0; i < bodyForces.size(); ++i)
+ ASSERT((bodyForces[i]-actualBodyForces[i]).norm() < 1e-10);
+ for (int i = 0; i < particleForces.size(); ++i)
+ ASSERT((particleForces[i]-actualParticleForces[i]).norm() < 1e-10);
+ for (int i = 0; i < mobilityForces.size(); ++i)
+ ASSERT(std::abs(mobilityForces[i]-actualMobilityForces[i]) < 1e-10);
+}
+
+class MyForceImpl : public Force::Custom::Implementation {
+public:
+ mutable bool hasRealized[Stage::Report+1];
+ MyForceImpl() {
+ for (int i = 0; i < Stage::NValid; i++)
+ hasRealized[i] = false;
+ }
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, Vector& mobilityForces) const {
+ for (int i = 0; i < mobilityForces.size(); ++i)
+ mobilityForces[i] += i;
+ }
+ Real calcPotentialEnergy(const State& state) const {
+ return 0.0;
+ }
+ void realizeTopology(State& state) const {
+ hasRealized[Stage::Topology] = true;
+ }
+ void realizeModel(State& state) const {
+ hasRealized[Stage::Model] = true;
+ }
+ void realizeInstance(const State& state) const {
+ hasRealized[Stage::Instance] = true;
+ }
+ void realizeTime(const State& state) const {
+ hasRealized[Stage::Time] = true;
+ }
+ void realizePosition(const State& state) const {
+ hasRealized[Stage::Position] = true;
+ }
+ void realizeVelocity(const State& state) const {
+ hasRealized[Stage::Velocity] = true;
+ }
+ void realizeDynamics(const State& state) const {
+ hasRealized[Stage::Dynamics] = true;
+ }
+ void realizeAcceleration(const State& state) const {
+ hasRealized[Stage::Acceleration] = true;
+ }
+ void realizeReport(const State& state) const {
+ hasRealized[Stage::Report] = true;
+ }
+};
+
+/**
+ * Test all of the standard Force subclasses, and make sure they generate correct forces.
+ */
+
+void testStandardForces() {
+
+ // Create a system consisting of a chain of bodies.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ for (int i = 0; i < NUM_BODIES; ++i) {
+ MobilizedBody& parent = matter.updMobilizedBody(MobilizedBodyIndex(matter.getNumBodies()-1));
+ MobilizedBody::Gimbal b(parent, Transform(Vec3(0)), body, Transform(Vec3(BOND_LENGTH, 0, 0)));
+ }
+
+ // Add one of each type of force.
+
+ MobilizedBody& body1 = matter.updMobilizedBody(MobilizedBodyIndex(1));
+ MobilizedBody& body9 = matter.updMobilizedBody(MobilizedBodyIndex(9));
+ Force::ConstantForce constantForce(forces, body1, Vec3(0), Vec3(1, 2, 3));
+ Force::ConstantTorque constantTorque(forces, body1, Vec3(1, 2, 3));
+ Force::GlobalDamper globalDamper(forces, matter, 2.0);
+ Force::MobilityConstantForce mobilityConstantForce(forces, body1, 1, 2.0);
+ Force::MobilityLinearDamper mobilityLinearDamper(forces, body1, 1, 2.0);
+ Force::MobilityLinearSpring mobilityLinearSpring(forces, body1, 1, 2.0, 1.0);
+ Force::TwoPointConstantForce twoPointConstantForce(forces, body1, Vec3(0), body9, Vec3(0), 2.0);
+ Force::TwoPointLinearDamper twoPointLinearDamper(forces, body1, Vec3(0), body9, Vec3(0), 2.0);
+ Force::TwoPointLinearSpring twoPointLinearSpring(forces, body1, Vec3(0), body9, Vec3(0), 2.0, 0.5);
+ Force::UniformGravity uniformGravity(forces, matter, Vec3(0, -2.0, 0));
+ Force::Custom custom(forces, new MyForceImpl());
+
+ // Create a random state for it.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ Random::Uniform random;
+ for (int i = 0; i < state.getNY(); ++i)
+ state.updY()[i] = random.getValue();
+ system.realize(state, Stage::Velocity);
+ Vec3 pos1 = body1.getBodyOriginLocation(state);
+ Vec3 pos9 = body9.getBodyOriginLocation(state);
+ Vec3 delta19 = pos9-pos1;
+
+ // Calculate each force component and see if it is correct.
+
+ Vector_<SpatialVec> bodyForces(matter.getNumBodies());
+ Vector_<Vec3> particleForces(0);
+ Vector mobilityForces(state.getNU());
+ Real pe = 0;
+
+ // Check ConstantForce
+
+ bodyForces = SpatialVec(Vec3(0), Vec3(0));
+ particleForces = Vec3(0);
+ mobilityForces = 0;
+ bodyForces[1][1] = Vec3(1, 2, 3);
+ verifyForces(constantForce, state, bodyForces, particleForces, mobilityForces);
+
+ // Check ConstantTorque
+
+ bodyForces = SpatialVec(Vec3(0), Vec3(0));
+ particleForces = Vec3(0);
+ mobilityForces = 0;
+ bodyForces[1][0] = Vec3(1, 2, 3);
+ verifyForces(constantTorque, state, bodyForces, particleForces, mobilityForces);
+
+ // Check GlobalDamper
+
+ bodyForces = SpatialVec(Vec3(0), Vec3(0));
+ particleForces = Vec3(0);
+ mobilityForces = -2.0*state.getU();
+ verifyForces(globalDamper, state, bodyForces, particleForces, mobilityForces);
+
+ // Check MobilityConstantForce
+
+ bodyForces = SpatialVec(Vec3(0), Vec3(0));
+ particleForces = Vec3(0);
+ mobilityForces = 0;
+ body1.updOneFromUPartition(state, 1, mobilityForces) = 2.0;
+ verifyForces(mobilityConstantForce, state, bodyForces, particleForces, mobilityForces);
+
+ // Check MobilityLinearDamper
+
+ bodyForces = SpatialVec(Vec3(0), Vec3(0));
+ particleForces = Vec3(0);
+ mobilityForces = 0;
+ body1.updOneFromUPartition(state, 1, mobilityForces) = -2.0*body1.getOneU(state, 1);
+ verifyForces(mobilityLinearDamper, state, bodyForces, particleForces, mobilityForces);
+
+ // Check MobilityLinearSpring
+
+ bodyForces = SpatialVec(Vec3(0), Vec3(0));
+ particleForces = Vec3(0);
+ mobilityForces = 0;
+ body1.updOneFromUPartition(state, 1, mobilityForces) = -2.0*(body1.getOneQ(state, 1)-1.0);
+ verifyForces(mobilityLinearSpring, state, bodyForces, particleForces, mobilityForces);
+
+ // Check TwoPointConstantForce
+
+ bodyForces = SpatialVec(Vec3(0), Vec3(0));
+ particleForces = Vec3(0);
+ mobilityForces = 0;
+ bodyForces[1][1] = -2.0*delta19.normalize();
+ bodyForces[9][1] = 2.0*delta19.normalize();
+ verifyForces(twoPointConstantForce, state, bodyForces, particleForces, mobilityForces);
+
+ // Check TwoPointLinearDamper
+
+ bodyForces = SpatialVec(Vec3(0), Vec3(0));
+ particleForces = Vec3(0);
+ mobilityForces = 0;
+ Vec3 v19 = body9.getBodyOriginVelocity(state)-body1.getBodyOriginVelocity(state);
+ Real twoPointLinearDamperForce = 2.0*dot(v19, delta19.normalize());
+ bodyForces[1][1] = twoPointLinearDamperForce*delta19.normalize();
+ bodyForces[9][1] = -twoPointLinearDamperForce*delta19.normalize();
+ verifyForces(twoPointLinearDamper, state, bodyForces, particleForces, mobilityForces);
+
+ // Check TwoPointLinearSpring
+
+ bodyForces = SpatialVec(Vec3(0), Vec3(0));
+ particleForces = Vec3(0);
+ mobilityForces = 0;
+ Real twoPointLinearSpringForce = 2.0*(delta19.norm()-0.5);
+ bodyForces[1][1] = twoPointLinearSpringForce*delta19.normalize();
+ bodyForces[9][1] = -twoPointLinearSpringForce*delta19.normalize();
+ verifyForces(twoPointLinearSpring, state, bodyForces, particleForces, mobilityForces);
+
+ // Check UniformGravity
+
+ bodyForces = SpatialVec(Vec3(0), Vec3(0, -2.0, 0));
+ bodyForces[0] = SpatialVec(Vec3(0), Vec3(0));
+ particleForces = Vec3(0);
+ mobilityForces = 0;
+ verifyForces(uniformGravity, state, bodyForces, particleForces, mobilityForces);
+
+ // Check Custom
+
+ bodyForces = SpatialVec(Vec3(0), Vec3(0));
+ particleForces = Vec3(0);
+ for (int i = 0; i < mobilityForces.size(); ++i)
+ mobilityForces[i] = i;
+ verifyForces(custom, state, bodyForces, particleForces, mobilityForces);
+}
+
+/**
+ * Test the standard conservative forces to make sure they really conserve energy.
+ */
+
+void testEnergyConservation() {
+
+ // Create a system consisting of a chain of bodies.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ for (int i = 0; i < NUM_BODIES; ++i) {
+ MobilizedBody& parent = matter.updMobilizedBody(MobilizedBodyIndex(matter.getNumBodies()-1));
+ MobilizedBody::Gimbal b(parent, Transform(Vec3(0)), body, Transform(Vec3(BOND_LENGTH, 0, 0)));
+ }
+
+ // Add one of each type of conservative force.
+
+ MobilizedBody& body1 = matter.updMobilizedBody(MobilizedBodyIndex(1));
+ MobilizedBody& body9 = matter.updMobilizedBody(MobilizedBodyIndex(9));
+ Force::MobilityLinearSpring mobilityLinearSpring(forces, body1, 1, 0.1, 1.0);
+ Force::TwoPointLinearSpring twoPointLinearSpring(forces, body1, Vec3(0), body9, Vec3(0), 1.0, 4.0);
+ Force::UniformGravity uniformGravity(forces, matter, Vec3(0, -1.0, 0));
+
+ // Create a random initial state for it.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ Random::Uniform random;
+ for (int i = 0; i < state.getNY(); ++i)
+ state.updY()[i] = random.getValue();
+
+ // Simulate it for a while and see if the energy changes.
+
+ system.realize(state, Stage::Dynamics);
+ Real initialEnergy = system.calcEnergy(state);
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setAccuracy(1e-4);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(10.0);
+ system.realize(state, Stage::Dynamics);
+ Real finalEnergy = system.calcEnergy(ts.getState());
+ ASSERT(std::abs(initialEnergy/finalEnergy-1.0) < 0.005);
+}
+
+/**
+ * Make sure that all the "realize" methods on a custom force actually get called.
+ */
+
+void testCustomRealization() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ MyForceImpl* impl = new MyForceImpl();
+ Force::Custom custom(forces, impl);
+ State state = system.realizeTopology();
+ for (Stage j = Stage::Model; j <= Stage::Report; j++) {
+ system.realize(state, j);
+ for (Stage i = Stage::Topology; i <= Stage::Report; i++)
+ ASSERT(impl->hasRealized[i] == (i <= j));
+ }
+}
+
+/**
+ * Test enabling and disabling forces.
+ */
+
+void testDisabling() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ MobilizedBody::Free body1(matter.updGround(), Vec3(0), body, Vec3(0));
+ MobilizedBody::Free body2(matter.updGround(), Vec3(0), body, Vec3(0));
+ Force::TwoPointLinearSpring spring(forces, body1, Vec3(0), body2, Vec3(0), 2.0, 0.5);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -2.0, 0));
+
+ // Create an initial state.
+
+ State state = system.realizeTopology();
+ body1.setQToFitTranslation(state, Vec3(0, 1, 0));
+ body2.setQToFitTranslation(state, Vec3(1, 1, 0));
+
+ // These are the contribution of each force to the energy and to the force on body1.
+
+ Real springEnergy = 0.5*2.0*0.5*0.5;
+ SpatialVec springForce(Vec3(0), Vec3(2.0*0.5, 0, 0));
+ Real gravityEnergy = 2*2.0;
+ SpatialVec gravityForce(Vec3(0), Vec3(0, -2.0, 0));
+
+ // Verify the force and energy for each combination of the forces being enabled or disabled.
+
+ system.realize(state, Stage::Dynamics);
+ ASSERT_EQUAL(springEnergy+gravityEnergy, system.calcEnergy(state));
+ ASSERT((springForce+gravityForce-system.getRigidBodyForces(state, Stage::Dynamics)[1]).norm() < 1e-10);
+ ASSERT(!forces.isForceDisabled(state, gravity.getForceIndex()));
+ ASSERT(!forces.isForceDisabled(state, spring.getForceIndex()));
+ forces.setForceIsDisabled(state, spring.getForceIndex(), true);
+ system.realize(state, Stage::Dynamics);
+ ASSERT_EQUAL(gravityEnergy, system.calcEnergy(state));
+ ASSERT((gravityForce-system.getRigidBodyForces(state, Stage::Dynamics)[1]).norm() < 1e-10);
+ ASSERT(!forces.isForceDisabled(state, gravity.getForceIndex()));
+ ASSERT(forces.isForceDisabled(state, spring.getForceIndex()));
+ forces.setForceIsDisabled(state, gravity.getForceIndex(), true);
+ system.realize(state, Stage::Dynamics);
+ ASSERT_EQUAL(0, system.calcEnergy(state));
+ ASSERT((system.getRigidBodyForces(state, Stage::Dynamics)[1]).norm() < 1e-10);
+ ASSERT(forces.isForceDisabled(state, gravity.getForceIndex()));
+ ASSERT(forces.isForceDisabled(state, spring.getForceIndex()));
+ forces.setForceIsDisabled(state, spring.getForceIndex(), false);
+ system.realize(state, Stage::Dynamics);
+ ASSERT_EQUAL(springEnergy, system.calcEnergy(state));
+ ASSERT((springForce-system.getRigidBodyForces(state, Stage::Dynamics)[1]).norm() < 1e-10);
+ ASSERT(forces.isForceDisabled(state, gravity.getForceIndex()));
+ ASSERT(!forces.isForceDisabled(state, spring.getForceIndex()));
+}
+
+int main() {
+ try {
+ testStandardForces();
+ testEnergyConservation();
+ testCustomRealization();
+ testDisabling();
+ }
+ catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ cout << "Done" << endl;
+ return 0;
+}
diff --git a/Simbody/tests/TestFunctionBasedMobilizedBodies.cpp b/Simbody/tests/TestFunctionBasedMobilizedBodies.cpp
new file mode 100644
index 0000000..566deb4
--- /dev/null
+++ b/Simbody/tests/TestFunctionBasedMobilizedBodies.cpp
@@ -0,0 +1,1097 @@
+/* -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman, Ajay Seth *
+ * Contributors: *
+ * *
+ * 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"
+
+#include <vector>
+
+using namespace SimTK;
+using namespace std;
+
+const Real TOL = 1e-6;
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+template <class T>
+void assertEqual(T val1, T val2, Real tol = TOL) {
+ ASSERT(abs(val1-val2) < tol);
+}
+
+template <int N>
+void assertEqual(Vec<N> val1, Vec<N> val2, Real tol = TOL) {
+ for (int i = 0; i < N; ++i)
+ ASSERT(abs(val1[i]-val2[i]) < tol);
+}
+
+template<>
+void assertEqual(Vector val1, Vector val2, Real tol) {
+ ASSERT(val1.size() == val2.size());
+ for (int i = 0; i < val1.size(); ++i)
+ assertEqual(val1[i], val2[i], tol);
+}
+
+template<>
+void assertEqual(SpatialVec val1, SpatialVec val2, Real tol) {
+ assertEqual(val1[0], val2[0], tol);
+ assertEqual(val1[1], val2[1], tol);
+}
+
+template<>
+void assertEqual(Transform val1, Transform val2, Real tol) {
+ assertEqual(val1.p(), val2.p(), tol);
+ ASSERT(val1.R().isSameRotationToWithinAngle(val2.R(), tol));
+}
+
+void compareMobilizedBodies(const MobilizedBody& b1, const MobilizedBody& b2, bool eulerAngles, int expectedQ, int expectedU) {
+ const SimbodyMatterSubsystem& matter = b1.getMatterSubsystem();
+ const System& system = matter.getSystem();
+
+ // Set whether to use Euler angles.
+
+ State state = system.getDefaultState();
+ matter.setUseEulerAngles(state, eulerAngles);
+ system.realizeModel(state);
+
+ // Make sure the number of state variables is correct.
+
+ assertEqual(b1.getNumQ(state), expectedQ);
+ assertEqual(b1.getNumU(state), expectedU);
+ assertEqual(b2.getNumQ(state), expectedQ);
+ assertEqual(b2.getNumU(state), expectedU);
+
+ // Set all the state variables to random values.
+
+ Random::Gaussian random;
+ int nq = state.getNQ()/2;
+ for (int i = 0; i < nq; ++i)
+ state.updQ()[i] = state.updQ()[i+nq] = random.getValue();
+ int nu = state.getNU()/2;
+ for (int i = 0; i < nu; ++i)
+ state.updU()[i] = state.updU()[i+nu] = (eulerAngles ? 0.0 : random.getValue());
+ system.realize(state, Stage::Acceleration);
+
+ // Compare state variables and their derivatives.
+
+ for (int i = 0; i < b1.getNumQ(state); ++i) {
+ assertEqual(b1.getOneQ(state, i), b2.getOneQ(state, i));
+ assertEqual(b1.getOneQDot(state, i), b2.getOneQDot(state, i));
+ assertEqual(b1.getOneQDotDot(state, i), b2.getOneQDotDot(state, i));
+ }
+ /*
+ for (int i = 0; i < b1.getNumU(state); ++i) {
+ assertEqual(b1.getOneU(state, i), b2.getOneU(state, i));
+ assertEqual(b1.getOneUDot(state, i), b2.getOneUDot(state, i));
+ }
+ */
+ // Compare lots of properties of the two bodies.
+
+ assertEqual(b1.getBodyTransform(state), b2.getBodyTransform(state));
+ assertEqual(b1.getBodyVelocity(state), b2.getBodyVelocity(state));
+ assertEqual(b1.getBodyAcceleration(state), b2.getBodyAcceleration(state));
+ assertEqual(b1.getBodyOriginLocation(state), b2.getBodyOriginLocation(state));
+ assertEqual(b1.getBodyOriginVelocity(state), b2.getBodyOriginVelocity(state));
+ assertEqual(b1.getBodyOriginAcceleration(state), b2.getBodyOriginAcceleration(state));
+ assertEqual(b1.getMobilizerTransform(state), b2.getMobilizerTransform(state));
+ assertEqual(b1.getMobilizerVelocity(state), b2.getMobilizerVelocity(state));
+
+ // Test methods that multiply by various matrices.
+
+ Vector tempq(state.getNQ());
+ Vector tempu(state.getNU());
+ /*
+ matter.multiplyByN(state, false, state.getU(), tempq);
+ for (int i = 0; i < b1.getNumQ(state); ++i)
+ assertEqual(b1.getOneFromQPartition(state, i, tempq), b2.getOneFromQPartition(state, i, tempq));
+ matter.multiplyByN(state, true, state.getQ(), tempu);
+ for (int i = 0; i < b1.getNumU(state); ++i)
+ assertEqual(b1.getOneFromUPartition(state, i, tempu), b2.getOneFromUPartition(state, i, tempu));
+ matter.multiplyByNInv(state, false, state.getQ(), tempu);
+ for (int i = 0; i < b1.getNumU(state); ++i)
+ assertEqual(b1.getOneFromUPartition(state, i, tempu), b2.getOneFromUPartition(state, i, tempu));
+ matter.multiplyByNInv(state, true, state.getU(), tempq);
+ for (int i = 0; i < b1.getNumQ(state); ++i)
+ assertEqual(b1.getOneFromQPartition(state, i, tempq), b2.getOneFromQPartition(state, i, tempq));
+ */
+
+ // Have them calculate q and u, and see if they agree.
+
+ if (!eulerAngles) { // The optimizer does not work reliably for Euler angles, since it can hit a singularity
+ Transform t = b1.getBodyTransform(state);
+ b1.setQFromVector(state, Vector(b1.getNumQ(state), 0.0));
+ b2.setQFromVector(state, Vector(b2.getNumQ(state), 0.0));
+ b1.setQToFitTransform(state, t);
+ b2.setQToFitTransform(state, t);
+ system.realize(state, Stage::Velocity);
+ assertEqual(b1.getBodyOriginLocation(state), b2.getBodyOriginLocation(state), 1e-2);
+ assertEqual((~b1.getBodyRotation(state)*b2.getBodyRotation(state)).convertRotationToAngleAxis()[0], 0.0, 1e-2);
+ SpatialVec v = b1.getBodyVelocity(state);
+ b1.setUFromVector(state, Vector(b1.getNumU(state), 0.0));
+ b2.setUFromVector(state, Vector(b2.getNumU(state), 0.0));
+ b1.setUToFitVelocity(state, v);
+ b2.setUToFitVelocity(state, v);
+ assertEqual(b1.getUAsVector(state), b2.getUAsVector(state), 1e-2);
+ }
+
+ // Simulate the system, and see if the two bodies remain identical.
+ b2.setQFromVector(state, b1.getQAsVector(state));
+ b2.setUFromVector(state, b1.getUAsVector(state));
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setAccuracy(1e-8);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(1.0);
+
+ assertEqual(b1.getQAsVector(integ.getState()), b2.getQAsVector(integ.getState()));
+ assertEqual(b1.getQDotAsVector(integ.getState()), b2.getQDotAsVector(integ.getState()));
+}
+
+class ConstantFunction : public Function {
+// Implements a simple constant function, y = C
+private:
+ Real C;
+public:
+
+ //Default constructor
+ ConstantFunction(){
+ C = 0.0;
+ }
+
+ //Convenience constructor to specify constant value
+ ConstantFunction(Real constant){
+ C = constant;
+ }
+
+ Real calcValue(const Vector& x) const{
+ return C;
+ }
+
+ // This is the pure virtual signature.
+ Real calcDerivative(const Array_<int>& derivComponents, const Vector& x) const{
+ return 0;
+ }
+ // This is just a local method providing std::vector compatibility without copying.
+ Real calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const{
+ return calcDerivative(ArrayViewConst_<int>(derivComponents), x);
+ }
+
+ int getArgumentSize() const{
+ // constant has no arguments
+ return 0;
+ }
+
+ int getMaxDerivativeOrder() const{
+ return 10;
+ }
+};
+
+class LinearFunction : public Function {
+// Implements a simple linear functional relationship, y = m*x + b
+private:
+ Real m;
+ Real b;
+public:
+
+ //Default constructor
+ LinearFunction(){
+ m = 1.0;
+ b = 0.0;
+ }
+
+ //Convenience constructor to specify the slope and Y-intercept of the linear r
+ LinearFunction(Real slope, Real intercept){
+ m = slope;
+ b = intercept;
+ }
+
+ Real calcValue(const Vector& x) const{
+ return m*x[0]+b;
+ }
+
+ Real calcDerivative(const Array_<int>& derivComponents, const Vector& x) const{
+ if (derivComponents.size() == 1)
+ return m;
+ return 0;
+ }
+ // This is just a local method providing std::vector compatibility without copying.
+ Real calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const{
+ return calcDerivative(ArrayViewConst_<int>(derivComponents), x);
+ }
+
+ int getArgumentSize() const{
+ return 1;
+ }
+
+ int getMaxDerivativeOrder() const{
+ return 10;
+ }
+};
+
+class NonlinearFunction : public Function {
+public:
+ NonlinearFunction(){
+ }
+ Real calcValue(const Vector& x) const{
+ return x[0]+x[1]*x[1];
+ }
+ Real calcDerivative(const Array_<int>& derivComponents, const Vector& x) const{
+ switch (derivComponents.size()) {
+ case 1:
+ return (derivComponents[0] == 0 ? 1.0 : x[1]);
+ case 2:
+ return (derivComponents[0] == 1 && derivComponents[1] == 1 ? 1.0 : 0.0);
+ }
+ return 0.0;
+ }
+ // This is just a local method providing std::vector compatibility without copying.
+ Real calcDerivative(const std::vector<int>& derivComponents, const Vector& x) const{
+ return calcDerivative(ArrayViewConst_<int>(derivComponents), x);
+ }
+
+ int getArgumentSize() const{
+ return 2;
+ }
+ int getMaxDerivativeOrder() const{
+ return std::numeric_limits<int>::max();
+ }
+};
+
+int defineMobilizerFunctions(std::vector<bool> &isdof, std::vector<std::vector<int> > &coordIndices, std::vector<const Function*> &functions1, std::vector<const Function*> &functions2)
+{
+ int nm = 0;
+ for(int i=0; i<6; i++){
+ if(isdof[i]) {
+ std::vector<int> findex(1);
+ findex[0] = nm++;
+ functions1.push_back(new LinearFunction());
+ functions2.push_back(new LinearFunction());
+ coordIndices.push_back(findex);
+ }
+ else{
+ std::vector<int> findex(0);
+ functions1.push_back(new ConstantFunction());
+ functions2.push_back(new ConstantFunction());
+ coordIndices.push_back(findex);
+ }
+ }
+ return nm;
+}
+
+void testFunctionBasedPin() {
+ // Define the functions that specify the FunctionBased Mobilized Body.
+ std::vector<std::vector<int> > coordIndices;
+ std::vector<const Function*> functions1, functions2;
+ std::vector<bool> isdof(6,false);
+
+ // Set the 1 spatial rotation about Z to be mobility
+ isdof[2] = true; //rot Z
+ int nm = defineMobilizerFunctions(isdof, coordIndices, functions1, functions2);
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ Body::Rigid body(MassProperties(1.0, Vec3(0.5), Inertia(1)));
+ MobilizedBody::Pin p1(matter.Ground(), body);
+ MobilizedBody::Pin p2(p1, body);
+ MobilizedBody::FunctionBased fb1(matter.Ground(), body, nm, functions1, coordIndices);
+ MobilizedBody::FunctionBased fb2(fb1, body, nm, functions2, coordIndices);
+ system.realizeTopology();
+ compareMobilizedBodies(p2, fb2, false, nm, nm);
+}
+
+void testFunctionBasedSkewedPin() {
+ // Define the functions that specify the FunctionBased Mobilized Body.
+ std::vector<std::vector<int> > coordIndices;
+ std::vector<const Function*> functions1, functions2;
+ std::vector<bool> isdof(6,false);
+ std::vector<Vec3> axes(6);
+
+ // Set the 1 spatial rotation about first axis
+ isdof[0] = true; //rot 1
+ int nm = defineMobilizerFunctions(isdof, coordIndices, functions1, functions2);
+
+ double angle = 0;
+
+ axes[0] = Vec3(0,0,1);
+ axes[1] = Vec3(0,1,0);
+ axes[2] = Vec3(1,0,0);
+ axes[3] = Vec3(1,0,0);
+ axes[4] = Vec3(0,1,0);
+ axes[5] = Vec3(0,0,1);
+
+ Transform inParentPin = Transform(Rotation(angle, YAxis), Vec3(0));
+ Transform inChildPin = Transform(Rotation(angle, YAxis), Vec3(0,1,0));
+
+ Transform inParentFB = Transform(Vec3(0));
+ Transform inChildFB = Transform(Vec3(0,1,0));
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+
+ //Built-in
+ MobilizedBody::Pin p1(matter.Ground(), inParentPin, body, inChildPin);
+ MobilizedBody::Pin p2(p1, inParentPin, body, inChildPin);
+ //Function-based
+ MobilizedBody::FunctionBased fb1(matter.Ground(), inParentFB, body, inChildFB, nm, functions1, coordIndices, axes);
+ MobilizedBody::FunctionBased fb2(fb1, inParentFB, body, inChildFB, nm, functions2, coordIndices, axes);
+
+ system.realizeTopology();
+ compareMobilizedBodies(p2, fb2, false, nm, nm);
+}
+
+void testFunctionBasedSlider() {
+ // Define the functions that specify the FunctionBased Mobilized Body.
+ std::vector<std::vector<int> > coordIndices;
+ std::vector<const Function*> functions1, functions2;
+ std::vector<bool> isdof(6,false);
+
+ // Set the 1 spatial translation along X to be mobility
+ isdof[3] = true; //trans X
+ int nm = defineMobilizerFunctions(isdof, coordIndices, functions1, functions2);
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ MobilizedBody::Slider s1(matter.Ground(), body);
+ MobilizedBody::Slider s2(s1, body);
+ MobilizedBody::FunctionBased fb1(matter.Ground(), body, nm, functions1, coordIndices);
+ MobilizedBody::FunctionBased fb2(fb1, body, nm, functions2, coordIndices);
+ system.realizeTopology();
+ compareMobilizedBodies(s2, fb2, false, nm, nm);
+}
+
+
+void testFunctionBasedSkewedSlider() {
+ // Define the functions that specify the FunctionBased Mobilized Body.
+ std::vector<std::vector<int> > coordIndices;
+ std::vector<const Function*> functions1, functions2;
+ std::vector<bool> isdof(6,false);
+ std::vector<Vec3> axes(6);
+ //axes[0] = Vec3(1/sqrt(2.0),0, -1/sqrt(2.0));
+ axes[0] = Vec3(1,0,0);
+ axes[1] = Vec3(0,1,0);
+ axes[2] = Vec3(0,0,1);
+ axes[3] = Vec3(0,0,1);
+ axes[4] = Vec3(0,1,0);
+ axes[5] = Vec3(1,0,0);
+ // Set the 1 spatial translation along X to be mobility
+ isdof[5] = true; //trans X
+ int nm = defineMobilizerFunctions(isdof, coordIndices, functions1, functions2);
+
+ Transform inParent = Transform(Vec3(0)); //Transform(Rotation(-Pi/2, YAxis));
+ Transform inChild = Transform(Vec3(0));
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ MobilizedBody::Slider s1(matter.Ground(), inParent, body, inChild);
+ MobilizedBody::Slider s2(s1, inParent, body, inChild);
+ MobilizedBody::FunctionBased fb1(matter.Ground(), body, nm, functions1, coordIndices, axes);
+ MobilizedBody::FunctionBased fb2(fb1, body, nm, functions2, coordIndices, axes);
+ system.realizeTopology();
+ compareMobilizedBodies(s2, fb2, false, nm, nm);
+}
+
+void testFunctionBasedCylinder() {
+ // Define the functions that specify the FunctionBased Mobilized Body.
+ std::vector<std::vector<int> > coordIndices;
+ std::vector<const Function*> functions1, functions2;
+ std::vector<bool> isdof(6,false);
+
+ // Set 2 mobilities: rotation about and translation along Z
+ isdof[2] = true; //rot Z
+ isdof[5] = true; //trans Z
+ int nm = defineMobilizerFunctions(isdof, coordIndices, functions1, functions2);
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ MobilizedBody::Cylinder c1(matter.Ground(), body);
+ MobilizedBody::Cylinder c2(c1, body);
+ MobilizedBody::FunctionBased fb1(matter.Ground(), body, nm, functions1, coordIndices);
+ MobilizedBody::FunctionBased fb2(fb1, body, nm, functions2, coordIndices);
+ system.realizeTopology();
+ compareMobilizedBodies(c2, fb2, false, nm, nm);
+}
+
+void testFunctionBasedUniversal() {
+ // Define the functions that specify the FunctionBased Mobilized Body.
+ std::vector<std::vector<int> > coordIndices;
+ std::vector<const Function*> functions1, functions2;
+ std::vector<bool> isdof(6,false);
+
+ // Set 2 rotation mobilities about body's X then Y
+ isdof[0] = true; //rot X
+ isdof[1] = true; //rot Y
+ int nm = defineMobilizerFunctions(isdof, coordIndices, functions1, functions2);
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ MobilizedBody::Universal u1(matter.Ground(), body);
+ MobilizedBody::Universal u2(u1, body);
+ MobilizedBody::FunctionBased fb1(matter.Ground(), body, nm, functions1, coordIndices);
+ MobilizedBody::FunctionBased fb2(u1, body, nm, functions2, coordIndices);
+ system.realizeTopology();
+ compareMobilizedBodies(u2, fb2, true, nm, nm);
+}
+
+void testFunctionBasedPlanar() {
+ // Define the functions that specify the FunctionBased Mobilized Body.
+ std::vector<std::vector<int> > coordIndices;
+ std::vector<const Function*> functions1, functions2;
+ std::vector<bool> isdof(6,false);
+
+ // Set 3 mobilities: Z rotation and translation along body's X then Y
+ isdof[2] = true; //rot Z
+ isdof[3] = true; //trans X
+ isdof[4] = true; //trans Y
+ int nm = defineMobilizerFunctions(isdof, coordIndices, functions1, functions2);
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ //Vec3(1/sqrt(2.0000000000000), 1/sqrt(2.0000000000000), 0)
+ Body::Rigid body(MassProperties(1.0, Vec3(0, 0, 0), Inertia(1)));
+ MobilizedBody::Planar u1(matter.Ground(), body);
+ MobilizedBody::Planar u2(u1, body);
+ MobilizedBody::FunctionBased fb1(matter.Ground(), body, nm, functions1, coordIndices);
+ MobilizedBody::FunctionBased fb2(fb1, body, nm, functions2, coordIndices);
+ system.realizeTopology();
+ compareMobilizedBodies(u2, fb2, false, nm, nm);
+}
+
+void testFunctionBasedGimbal() {
+ // Define the functions that specify the FunctionBased Mobilized Body.
+ std::vector<std::vector<int> > coordIndices;
+ std::vector<const Function*> functions1, functions2;
+ std::vector<bool> isdof(6,false);
+
+ // Set 3 mobilities: Z rotation and translation along body's X then Y
+ isdof[0] = true; //rot X
+ isdof[1] = true; //rot Y
+ isdof[2] = true; //rot Z
+ int nm = defineMobilizerFunctions(isdof, coordIndices, functions1, functions2);
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ Body::Rigid body(MassProperties(1.0, Vec3(0, -0.5, 0), Inertia(0.5)));
+ MobilizedBody::Gimbal b1(matter.Ground(), body);
+ MobilizedBody::Gimbal b2(b1, body);
+ MobilizedBody::FunctionBased fb1(matter.Ground(), body, nm, functions1, coordIndices);
+ MobilizedBody::FunctionBased fb2(fb1, body, nm, functions2, coordIndices);
+ system.realizeTopology();
+ compareMobilizedBodies(b2, fb2, true, nm, nm);
+}
+
+void testFunctionBasedGimbalUserAxes() {
+ // Define the functions that specify the FunctionBased Mobilized Body.
+ std::vector<std::vector<int> > coordIndices;
+ std::vector<const Function*> functions1, functions2;
+ std::vector<Vec3> axes(6);
+ std::vector<bool> isdof(6,false);
+
+ isdof[0] = true; //rot 1
+ isdof[1] = true; //rot 2
+ isdof[2] = true; //rot 3
+ int nm = defineMobilizerFunctions(isdof, coordIndices, functions1, functions2);
+
+ // Sherm 20130213: I replaced the random number generator with some
+ // firm numbers to prevent singularities from occurring on some platforms
+ // based on different random number output.
+
+ axes[0] = Vec3(0.05, 1.4, 0);
+ axes[1] = Vec3(0.6, 0, -1.2);
+ axes[2] = Vec3(0, 2, -0.055);
+ axes[3] = Vec3(1,0,0);
+ axes[4] = Vec3(0,1,0);
+ axes[5] = Vec3(0,0,1);
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ Body::Rigid body(MassProperties(1.0, Vec3(0, -0.5, 0), Inertia(0.5)));
+ //Use massless bodies for generationg skewed-axes
+ Body::Massless massLessBody;
+
+ Transform inParent = Transform(Vec3(0));
+ Transform inChild = Transform(Vec3(0,1,0));
+
+ // Compared to standard built-in pin mobilizers with skewed axes
+ // Pin rotates about Z-axis and need to align with first axis
+ Transform parentPinAxis0 = Transform(Rotation(UnitVec3(axes[0]), ZAxis), Vec3(0,0,0));
+ Transform childPinAxis0 = Transform(Rotation(UnitVec3(axes[0]), ZAxis), Vec3(0,0,0));
+ Transform parentPinAxis1 = Transform(Rotation(UnitVec3(axes[1]), ZAxis), Vec3(0,0,0));
+ Transform childPinAxis1 = Transform(Rotation(UnitVec3(axes[1]), ZAxis), Vec3(0,0,0));
+ Transform parentPinAxis2 = Transform(Rotation(UnitVec3(axes[2]), ZAxis), Vec3(0,0,0));
+ Transform childPinAxis2 = Transform(Rotation(UnitVec3(axes[2]), ZAxis), Vec3(0,1,0));
+
+ MobilizedBody::Pin masslessPin0(matter.Ground(), parentPinAxis0, massLessBody, childPinAxis0);
+ MobilizedBody::Pin masslessPin1(masslessPin0, parentPinAxis1, massLessBody, childPinAxis1);
+ MobilizedBody::Pin b1(masslessPin1, parentPinAxis2, body, childPinAxis2);
+
+ MobilizedBody::Pin masslessPin00(b1, parentPinAxis0, massLessBody, childPinAxis0);
+ MobilizedBody::Pin masslessPin01(masslessPin00, parentPinAxis1, massLessBody, childPinAxis1);
+ MobilizedBody::Pin b2(masslessPin01, parentPinAxis2, body, childPinAxis2);
+
+ MobilizedBody::FunctionBased fb1(matter.Ground(), inParent, body, inChild, nm, functions1, coordIndices, axes);
+ MobilizedBody::FunctionBased fb2(fb1, inParent, body, inChild, nm, functions2, coordIndices, axes);
+ system.realizeTopology();
+
+ State state = system.getDefaultState();
+ matter.setUseEulerAngles(state, true);
+ system.realizeModel(state);
+
+ // These were generated randomly but we want repeatability across
+ // machines so we'll use the same numbers every time. Note that we'll
+ // re-use each of these twice, once for the pin joint system and once
+ // for the function based mobilizers.
+ Real initq[] = {1.41292, 0.048025,-1.19474, 0.618909,-0.0552235, 2.043930};
+ Real initu[] = {1.53485, 0.546119,-1.55779,-1.872230, 0.0982929, 0.118798};
+ int nq = state.getNQ()/2;
+ assert(nq <= 6); // make sure we have enough random numbers!
+ for (int i = 0; i < nq; ++i)
+ state.updQ()[i] = state.updQ()[i+nq] = initq[i];
+ int nu = state.getNU()/2;
+ for (int i = 0; i < nu; ++i)
+ state.updU()[i] = state.updU()[i+nu] = initu[i];
+
+ system.realize(state, Stage::Acceleration);
+
+ Transform Xb2 = b2.getBodyTransform(state);
+ Transform Xfb2 = fb2.getBodyTransform(state);
+
+ assertEqual(Xb2, Xfb2);
+
+ SpatialVec A_b2 = b2.getBodyAcceleration(state);
+ SpatialVec A_fb2 = fb2.getBodyAcceleration(state);
+
+ assertEqual(A_b2, A_fb2);
+
+ // Simulate it.
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setAccuracy(1e-8);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(1.0);
+ const State& result = ts.getState();
+
+ Vec3 com_bin = b2.getBodyOriginLocation(result);
+ Vec3 com_fb = fb2.getBodyOriginLocation(result);
+
+ assertEqual(com_bin, com_fb);
+ assertEqual(b2.getBodyVelocity(result), fb2.getBodyVelocity(result));
+
+ // stepTo() only guarantees realization through velocity stage.
+ system.realize(result, Stage::Acceleration);
+ assertEqual(b2.getBodyAcceleration(result), fb2.getBodyAcceleration(result));
+}
+
+void testFunctionBasedTranslation() {
+ // Test against built-in Translation mobilizer
+ // for a total of 3 coordinates and 3 mobilities
+
+ // Define the functions that specify the FunctionBased Mobilized Body.
+ std::vector<std::vector<int> > coordIndices;
+ std::vector<const Function*> functions1, functions2;
+
+ // Set 6 mobilities: rotation and translation about body's X, Y, and then Z axes
+ std::vector<bool> isdof(6,true);
+ //No rotations
+ isdof[0] = false; //rot X
+ isdof[1] = false; //rot Y
+ isdof[2] = false; //rot Z
+
+ int nm = defineMobilizerFunctions(isdof, coordIndices, functions1, functions2);
+
+ //Use massless body for translation
+ Body::Massless massLessBody;
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ Body::Rigid body(MassProperties(1.0, Vec3(0, -0.5, 0), Inertia(0.5)));
+
+ //Built-in mobilized bodies
+ MobilizedBody::Translation b1(matter.Ground(), body);
+ MobilizedBody::Translation b2(b1, body);
+
+ // Function-based
+ MobilizedBody::FunctionBased fb1(matter.Ground(), body, nm, functions1, coordIndices);
+ MobilizedBody::FunctionBased fb2(fb1, body, nm, functions2, coordIndices);
+ system.realizeTopology();
+
+ State state = system.getDefaultState();
+ matter.setUseEulerAngles(state, true);
+ system.realizeModel(state);
+
+ Random::Gaussian random;
+
+ int nq = state.getNQ()/2;
+ for (int i = 0; i < nq; ++i)
+ state.updQ()[i] = state.updQ()[i+nq] = random.getValue();
+ int nu = state.getNU()/2;
+ for (int i = 0; i < nu; ++i)
+ state.updU()[i] = state.updU()[i+nu] = random.getValue(); //0.0; //
+
+ system.realize(state, Stage::Acceleration);
+
+ Transform Xb2 = b2.getBodyTransform(state);
+ Transform Xfb2 = fb2.getBodyTransform(state);
+
+ SpatialVec A_b2 = b2.getBodyAcceleration(state);
+ SpatialVec A_fb2 = fb2.getBodyAcceleration(state);
+
+ assertEqual(A_b2, A_fb2);
+
+ // Simulate it.
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setAccuracy(1e-8);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(1.0);
+ const State& result = ts.getState();
+
+ Vec3 com_bin = b2.getBodyOriginLocation(result);
+ Vec3 com_fb = fb2.getBodyOriginLocation(result);
+
+ assertEqual(com_bin, com_fb);
+ assertEqual(b2.getBodyVelocity(result), fb2.getBodyVelocity(result));
+
+ // stepTo() only guarantees realization through velocity stage.
+ system.realize(result, Stage::Acceleration);
+ assertEqual(b2.getBodyAcceleration(result), fb2.getBodyAcceleration(result));
+}
+
+
+void testFunctionBasedFree() {
+ // Test against free joint using Euler angles for orientation (q)
+ // for a total of 6 coordinates and 6 mobilities
+
+ // Define the functions that specify the FunctionBased Mobilized Body.
+ std::vector<std::vector<int> > coordIndices;
+ std::vector<const Function*> functions1, functions2;
+
+ // Set 6 mobilities: rotation and translation about body's X, Y, and then Z axes
+ std::vector<bool> isdof(6,true);
+
+ int nm = defineMobilizerFunctions(isdof, coordIndices, functions1, functions2);
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ Body::Rigid body(MassProperties(1.0, Vec3(0.2, -0.5, 0.1), Inertia(1.2)));
+
+ //Built-in free
+ MobilizedBody::Free b1(matter.Ground(), body);
+
+ //Function-based equivalent?
+ MobilizedBody::FunctionBased fb1(matter.Ground(), body, nm, functions1, coordIndices);
+
+ system.realizeTopology();
+
+ State state = system.getDefaultState();
+ matter.setUseEulerAngles(state, true);
+ system.realizeModel(state);
+
+ int nq = state.getNQ();
+ nq = nq-nm;
+
+ assert(nm == state.getNU()/2);
+
+ // Get random q's and u's and set equivalent on both bodies.
+ // (Not really random so we can get repeatability on all platforms.)
+ Real initq[] = {0.455189,-0.383271,1.21353,-0.510623,-1.71438,0.968387};
+ assert(nm <= 6);
+ for (int i = 0; i < nm; ++i){
+ // Free has slots for 4 rot q's and fb only has 3
+ state.updQ()[i] = state.updQ()[i+nq] = initq[i]; //
+ }
+
+ system.realize(state, Stage::Position);
+ SpatialVec inputVelocity(Vec3(-0.962157,0.523767,1.94993),
+ Vec3(-1.15752,0.436991,-0.787116));
+
+ b1.setUToFitVelocity(state, inputVelocity);
+ fb1.setUToFitVelocity(state, inputVelocity);
+
+ system.realize(state, Stage::Acceleration);
+
+ //cout << system.getRigidBodyForces(state, Stage::Dynamics)[b1.getMobilizedBodyIndex()] << endl;
+ //cout << system.getRigidBodyForces(state, Stage::Dynamics)[fb1.getMobilizedBodyIndex()] << endl;
+
+ Transform Xb1 = b1.getBodyTransform(state);
+ Transform Xfb1 = fb1.getBodyTransform(state);
+
+ assertEqual(Xb1, Xfb1);
+
+ SpatialVec A_b1 = b1.getBodyAcceleration(state);
+ SpatialVec A_fb1 = fb1.getBodyAcceleration(state);
+
+ assertEqual(A_b1, A_fb1);
+
+ // Simulate it.
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setAccuracy(1e-8);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(1.0);
+ const State &result = ts.getState();
+
+ //cout << "Free and Function-based Us" << endl;
+ //cout << result.getU()(0, 6) << endl;
+ //cout << result.getU()(6, 6) << endl;
+
+ Xb1 = b1.getBodyTransform(result);
+ Xfb1 = fb1.getBodyTransform(result);
+
+ Vec3 com_bin = b1.getBodyOriginLocation(result);
+ Vec3 com_fb = fb1.getBodyOriginLocation(result);
+
+ assertEqual(com_bin, com_fb);
+ assertEqual(b1.getBodyVelocity(result), fb1.getBodyVelocity(result));
+
+ // stepTo() only guarantees realization through velocity stage.
+ system.realize(result, Stage::Acceleration);
+ assertEqual(b1.getBodyAcceleration(result), fb1.getBodyAcceleration(result));
+}
+
+void testFunctionBasedFreeVsTranslationGimbal() {
+ // Test function-based free against a combination of Translation and Gimbal mobilizer
+ // for a total of 6 coordinates and 6 mobilities
+
+ // Define the functions that specify the FunctionBased Mobilized Body.
+ std::vector<std::vector<int> > coordIndices1, coordIndices2a, coordIndices2b;
+ std::vector<const Function*> functions1, temp;
+
+ // Set 6 mobilities: rotation and translation about body's X, Y, and then Z axes
+ std::vector<bool> isdof1(6,true), isdof2a(6,true), isdof2b(6,true);
+
+
+ int nm1 = defineMobilizerFunctions(isdof1, coordIndices1, functions1, temp);
+ int nm2a = 3;
+ int nm2b = 3;
+
+ // Check that we constructed the correct number of functions
+ assert(nm1 == nm2a+nm2b);
+
+ //Use massless body for translation
+ Body::Massless massLessBody;
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ Body::Rigid body(MassProperties(0.1, Vec3(0.25, -0.5, 0.1), Inertia(0.5)));
+
+ // One Free-like function-based mmobilizer
+ MobilizedBody::FunctionBased fb1(matter.Ground(), body, nm1, functions1, coordIndices1);
+
+ // Two function-based mmobilizers: 2a for translation and 2b for rotation
+ MobilizedBody::Translation massLessTrans(matter.Ground(), massLessBody);
+ MobilizedBody::Gimbal b1(massLessTrans, body);
+
+ system.realizeTopology();
+
+ State state = system.getDefaultState();
+ matter.setUseEulerAngles(state, true);
+ system.realizeModel(state);
+
+ Random::Gaussian random;
+
+ int nq = state.getNQ();
+ nq = nq/2;
+ assert(nq == nm1);
+
+ // Set rotation states first
+ for (int i = 0; i < nm2b; ++i){
+ state.updQ()[i] = state.updQ()[i+nq+nm2a] = 0.0; //random.getValue();
+ state.updU()[i] = state.updU()[i+nq+nm2a] = random.getValue();
+ }
+
+ // Set translations states second
+ for (int i = 0; i < nm2a; ++i){
+ state.updQ()[i+nm2a] = state.updQ()[i+nq] = random.getValue();
+ state.updU()[i+nm2a] = state.updU()[i+nq] = random.getValue();
+ }
+
+ system.realize(state, Stage::Acceleration);
+
+ Transform Xfb1 = fb1.getBodyTransform(state);
+ Transform Xb1 = b1.getBodyTransform(state);
+
+ assertEqual(Xfb1, Xb1);
+
+ SpatialVec A_fb1 = fb1.getBodyAcceleration(state);
+ SpatialVec A_b1 = b1.getBodyAcceleration(state);
+
+ assertEqual(A_fb1, A_b1);
+
+ // Simulate it.
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setAccuracy(1e-8);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(1.0);
+
+ const State& result = ts.getState();
+
+ Xfb1 = fb1.getBodyTransform(result);
+ Xb1 = b1.getBodyTransform(result);
+
+ assertEqual(Xfb1, Xb1);
+ assertEqual(fb1.getBodyVelocity(result), b1.getBodyVelocity(result));
+
+ // stepTo() only guarantees realization through velocity stage.
+ system.realize(result, Stage::Acceleration);
+ assertEqual(fb1.getBodyAcceleration(result), b1.getBodyAcceleration(result));
+}
+
+
+void testFunctionBasedFreeVs2FunctionBased() {
+ // Test against free joint that is a combination of Translation and Gimbal mobilizer
+ // for a total of 6 coordinates and 6 mobilities
+
+ // Define the functions that specify the FunctionBased Mobilized Body.
+ std::vector<std::vector<int> > coordIndices1, coordIndices2a, coordIndices2b;
+ std::vector<const Function*> functions1, temp;
+ std::vector<const Function*> functions2a, functions2b;
+
+ // Set 6 mobilities: rotation and translation about body's X, Y, and then Z axes
+ std::vector<bool> isdof1(6,true), isdof2a(6,true), isdof2b(6,true);
+
+ // Just translation
+ isdof2a[0] = false; //rot X
+ isdof2a[1] = false; //rot Y
+ isdof2a[2] = false; //rot Z
+
+ // Just rotation
+ isdof2b[3] = false; //trans X
+ isdof2b[4] = false; //trans Y
+ isdof2b[5] = false; //trans Z
+
+ int nm1 = defineMobilizerFunctions(isdof1, coordIndices1, functions1, temp);
+ int nm2a = defineMobilizerFunctions(isdof2a, coordIndices2a, functions2a, temp);
+ int nm2b = defineMobilizerFunctions(isdof2b, coordIndices2b, functions2b, temp);
+
+ // Check that we constructed the correct number of functions
+ assert(nm1 == nm2a+nm2b);
+
+ //Use massless body for translation
+ Body::Massless massLessBody;
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ Body::Rigid body(MassProperties(0.1, Vec3(0.25, -0.5, 0.1), Inertia(0.5)));
+
+ // One Free-like function-based mmobilizer
+ MobilizedBody::FunctionBased fb1(matter.Ground(), body, nm1, functions1, coordIndices1);
+
+ // Two function-based mmobilizers: 2a for translation and 2b for rotation
+ MobilizedBody::FunctionBased massLessfb(matter.Ground(), massLessBody, nm2a, functions2a, coordIndices2a);
+ MobilizedBody::FunctionBased fb2(massLessfb, body, nm2b, functions2b, coordIndices2b);
+
+ system.realizeTopology();
+
+ State state = system.getDefaultState();
+ matter.setUseEulerAngles(state, true);
+ system.realizeModel(state);
+
+ Random::Gaussian random;
+
+ int nq = state.getNQ();
+ nq = nq/2;
+ assert(nq == nm1);
+
+ // Set rotation states first
+ for (int i = 0; i < nm2b; ++i){
+ state.updQ()[i] = state.updQ()[i+nq+nm2a] = random.getValue();
+ state.updU()[i] = state.updU()[i+nq+nm2a] = random.getValue();
+ }
+
+ // Set translations states second
+ for (int i = 0; i < nm2a; ++i){
+ state.updQ()[i+nm2a] = state.updQ()[i+nq] = random.getValue();
+ state.updU()[i+nm2a] = state.updU()[i+nq] = random.getValue();
+ }
+
+ system.realize(state, Stage::Acceleration);
+
+ Transform Xfb1 = fb1.getBodyTransform(state);
+ Transform Xfb2 = fb2.getBodyTransform(state);
+
+ SpatialVec A_fb1 = fb1.getBodyAcceleration(state);
+ SpatialVec A_fb2 = fb2.getBodyAcceleration(state);
+
+ assertEqual(A_fb1, A_fb2);
+
+ // Simulate it.
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setAccuracy(1e-8);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(1.0);
+
+ const State& result = ts.getState();
+
+ Vec3 com_fb1 = fb1.getBodyOriginLocation(result);
+ Vec3 com_fb2 = fb2.getBodyOriginLocation(result);
+
+ assertEqual(com_fb1, com_fb2);
+ assertEqual(fb1.getBodyVelocity(result), fb2.getBodyVelocity(result));
+
+ // stepTo() only guarantees realization through velocity stage.
+ system.realize(result, Stage::Acceleration);
+ assertEqual(fb1.getBodyAcceleration(result), fb2.getBodyAcceleration(result));
+}
+
+
+
+/**
+ * Test a mobilized body based on functions that take multiple arguments.
+ */
+void testMultipleArguments() {
+ // Define the functions that specify the FunctionBased Mobilized Body.
+
+ std::vector<std::vector<int> > coordIndices(6);
+ std::vector<const Function*> functions(6);
+ Vector coeff(3);
+ coeff[0] = 0.5;
+ coeff[1] = -0.5;
+ coeff[2] = 1.0;
+ functions[0] = new Function::Constant(0.0, 0);
+ functions[1] = new Function::Constant(0.0, 0);
+ functions[2] = new Function::Constant(0.0, 0);
+ functions[3] = new NonlinearFunction();
+ functions[4] = new Function::Linear(coeff);
+ functions[5] = new Function::Constant(0.0, 0);
+ coordIndices[3].push_back(0);
+ coordIndices[3].push_back(1);
+ coordIndices[4].push_back(0);
+ coordIndices[4].push_back(1);
+
+ // Create the system.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ Body::Rigid body(MassProperties(1.0, Vec3(0, -0.5, 0), Inertia(0.5)));
+ MobilizedBody::FunctionBased fb(matter.Ground(), body, 2, functions, coordIndices);
+ State state = system.realizeTopology();
+
+ // See if coordinates and velocities are calculated correctly.
+
+ ASSERT(state.getNQ() == 2);
+ ASSERT(state.getNU() == 2);
+ state.updQ()[0] = 2.0;
+ state.updQ()[1] = -3.0;
+ state.updU()[0] = 0.1;
+ state.updU()[1] = -0.4;
+ system.realize(state, Stage::Acceleration);
+ assertEqual(fb.getBodyTransform(state), Transform(Vec3(11.0, 3.5, 0.0)));
+ assertEqual(fb.getBodyVelocity(state), SpatialVec(Vec3(0.0), Vec3(0.1+3.0*0.4, 0.25, 0.0)));
+
+ // Simulate it.
+
+ Real energy = system.calcEnergy(state);
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setAccuracy(1e-8);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(5.0);
+ assertEqual(energy, system.calcEnergy(ts.getState()));
+}
+
+int main() {
+ try {
+ cout << "FunctionBased MobilizedBodies vs. Built-in Types: " << endl;
+
+ testFunctionBasedPin();
+ cout << "Pin: Passed" << endl;
+
+ testFunctionBasedSkewedPin();
+ cout << "Skewed Pin: Passed" << endl;
+
+ testFunctionBasedSlider();
+ cout << "Slider: Passed" << endl;
+
+ testFunctionBasedSkewedSlider();
+ cout << "Skewed Slider: Passed" << endl;
+
+ testFunctionBasedCylinder();
+ cout << "Cylinder: Passed" << endl;
+
+ testFunctionBasedUniversal();
+ cout << "Universal: Passed" << endl;
+
+ testFunctionBasedPlanar();
+ cout << "Planar: Passed" << endl;
+
+ testFunctionBasedGimbal();
+ cout << "Gimbal: Passed" << endl;
+
+ testMultipleArguments();
+ cout << "Functions with Multiple Arguments: Passed" << endl;
+
+ testFunctionBasedGimbalUserAxes();
+ cout << "Gimbal with User Axes: Passed" << endl;
+
+ testFunctionBasedTranslation();
+ cout << "Translation: Passed" << endl;
+
+ testFunctionBasedFree();
+ cout << "Free: Passed" << endl;
+
+ testFunctionBasedFreeVsTranslationGimbal();
+ cout << "F-B Free vs. Translation and Gimbal: Passed" << endl;
+
+ testFunctionBasedFreeVs2FunctionBased();
+ cout << "F-B Free vs. Combination of Two Function-Based: Passed" << endl;
+ }
+ catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ cout << "Done" << endl;
+ return 0;
+}
diff --git a/Simbody/tests/TestGravity.cpp b/Simbody/tests/TestGravity.cpp
new file mode 100644
index 0000000..0970514
--- /dev/null
+++ b/Simbody/tests/TestGravity.cpp
@@ -0,0 +1,380 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): Gazebo Inelastic Collision *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* Test the Force::Gravity element.
+*/
+
+#include "Simbody.h"
+
+#include <cassert>
+#include <iostream>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+//#define USE_VISUALIZER
+
+const Vec3 Cube(.5,.5,.5); // half-dimensions of cube
+const Real Mass1=100, Mass2=5, Mass3=1;
+const Vec3 Centroid(.5,0,.5);
+const Vec3 COM1=Centroid, COM2=Centroid, COM3=Centroid+Vec3(0,.5,0);
+const UnitInertia Central(Vec3(.1), Vec3(.05));
+// Simbody requires inertias to be expressed about body origin rather than COM.
+const Inertia Inertia1=Mass1*Central.shiftFromCentroid(-COM1);
+const Inertia Inertia2=Mass2*Central.shiftFromCentroid(-COM2);
+const Inertia Inertia3=Mass3*Central.shiftFromCentroid(-COM3); // weird
+
+Body::Rigid body1Info(MassProperties(Mass1, COM1, Inertia1));
+Body::Rigid body2Info(MassProperties(Mass2, COM2, Inertia2));
+Body::Rigid body3Info(MassProperties(Mass3, COM3, Inertia3));
+
+// Make sure the constructors and default setters and getters work.
+static void testConstruction() {
+ MultibodySystem mbs;
+ SimbodyMatterSubsystem matter(mbs);
+ GeneralForceSubsystem forces(mbs);
+
+ Force::Gravity gravity1(forces, matter, -ZAxis, 50);
+ SimTK_TEST(gravity1.getDefaultMagnitude()==50);
+ SimTK_TEST(gravity1.getDefaultDownDirection()==UnitVec3(0,0,-1));
+ SimTK_TEST(gravity1.getDefaultZeroHeight()==0);
+ SimTK_TEST(gravity1.getDefaultGravityVector()==Vec3(0,0,-50));
+ SimTK_TEST(gravity1.getDefaultBodyIsExcluded(MobodIndex(0)));
+ SimTK_TEST(!gravity1.getDefaultBodyIsExcluded(MobodIndex(1)));
+
+ const Vec3 grav2(1,2,3);
+ Force::Gravity gravity2(forces, matter, grav2);
+ SimTK_TEST_EQ(gravity2.getDefaultMagnitude(),grav2.norm());
+ SimTK_TEST_EQ(gravity2.getDefaultDownDirection(),UnitVec3(grav2));
+ SimTK_TEST(gravity2.getDefaultZeroHeight()==0);
+ SimTK_TEST(gravity2.getDefaultGravityVector()==grav2);
+
+ mbs.setUpDirection(XAxis); const Real mag = 16.75;
+ Force::Gravity gravity3(forces, matter, mag);
+
+ SimTK_TEST(gravity3.getDefaultMagnitude()==mag);
+ SimTK_TEST(gravity3.getDefaultDownDirection()==UnitVec3(-XAxis));
+ SimTK_TEST(gravity3.getDefaultZeroHeight()==0);
+ SimTK_TEST(gravity3.getDefaultGravityVector()==Vec3(-mag,0,0));
+
+ // Using the vector constructor with a zero vector should pluck the
+ // direction out of the System.
+ Force::Gravity gravity4(forces, matter, Vec3(0));
+ SimTK_TEST(gravity4.getDefaultMagnitude()==0);
+ SimTK_TEST(gravity4.getDefaultDownDirection()==UnitVec3(-XAxis));
+ SimTK_TEST(gravity4.getDefaultZeroHeight()==0);
+ SimTK_TEST(gravity4.getDefaultGravityVector()==Vec3(0));
+
+ // Make sure Ground can't be included.
+ gravity1.setDefaultBodyIsExcluded(MobodIndex(0), false);
+ SimTK_TEST(gravity1.getDefaultBodyIsExcluded(MobodIndex(0)));
+
+ // Make sure we can exclude a body and that it doesn't leak over into
+ // another body. Note that for defaults we don't yet know how may bodies
+ // there might be so we can use arbitrary body numbers.
+ SimTK_TEST(!gravity1.getDefaultBodyIsExcluded(MobodIndex(13)));
+ gravity1.setDefaultBodyIsExcluded(MobodIndex(13), true);
+ SimTK_TEST(gravity1.getDefaultBodyIsExcluded(MobodIndex(13)));
+ SimTK_TEST(!gravity1.getDefaultBodyIsExcluded(MobodIndex(5)));
+
+ gravity1.setDefaultGravityVector(Vec3(19,20,21));
+ SimTK_TEST_EQ(gravity1.getDefaultGravityVector(), Vec3(19,20,21));
+
+ gravity1.setDefaultMagnitude(3).setDefaultDownDirection(grav2);
+ SimTK_TEST(gravity1.getDefaultMagnitude()==3);
+ SimTK_TEST_EQ(gravity1.getDefaultDownDirection(), UnitVec3(grav2));
+ SimTK_TEST_EQ(gravity1.getDefaultGravityVector(), Real(3)*UnitVec3(grav2));
+
+ gravity1.setDefaultZeroHeight(5.25);
+ SimTK_TEST(gravity1.getDefaultZeroHeight() == 5.25);
+}
+
+// Make sure we can override default values in the State.
+void testParameters() {
+ MultibodySystem mbs;
+ SimbodyMatterSubsystem matter(mbs);
+ GeneralForceSubsystem forces(mbs);
+
+ Force::Gravity gravity(forces, matter, -ZAxis, 49, -5);
+ SimTK_TEST(gravity.getDefaultZeroHeight()==-5);
+
+ MobilizedBody::Weld mobod1(matter.Ground(),Vec3(0), body1Info, Vec3(0));
+ MobilizedBody::Pin mobod2(mobod1, Vec3(0), body2Info, Vec3(0));
+ MobilizedBody::Pin mobod3(mobod2, Vec3(0), body3Info,Vec3(0));
+ MobilizedBody::Pin mobod4(mobod3, Vec3(0), body3Info,Vec3(0));
+ MobilizedBody::Free mobod5(matter.Ground(), Vec3(0), body3Info,Vec3(0));
+
+ State s = mbs.realizeTopology();
+
+ // Make sure defaults made it to the state.
+ SimTK_TEST(gravity.getBodyIsExcluded(s,MobodIndex(0)));
+ for (MobodIndex i(1); i < matter.getNumBodies(); ++i)
+ SimTK_TEST(!gravity.getBodyIsExcluded(s,i));
+ SimTK_TEST(gravity.getGravityVector(s)==Vec3(0,0,-49));
+ SimTK_TEST(gravity.getDownDirection(s)==UnitVec3(0,0,-1));
+ SimTK_TEST(gravity.getMagnitude(s)==49);
+ SimTK_TEST(gravity.getZeroHeight(s)==-5);
+
+ // Now make some changes in the state.
+
+ // Shouldn't be able to include ground.
+ gravity.setBodyIsExcluded(s, MobodIndex(0), false);
+ SimTK_TEST(gravity.getBodyIsExcluded(s,MobodIndex(0)));
+
+ gravity.setBodyIsExcluded(s, mobod3, true);
+ SimTK_TEST(gravity.getBodyIsExcluded(s,mobod3));
+ SimTK_TEST(!gravity.getBodyIsExcluded(s,mobod2));
+ SimTK_TEST(!gravity.getBodyIsExcluded(s,mobod4));
+
+ // That shouldn't have changed the default.
+ SimTK_TEST(!gravity.getDefaultBodyIsExcluded(mobod3));
+
+ // When making changes in the state we are restricted to bodies that
+ // actually exist.
+ SimTK_TEST_MUST_THROW(gravity.setBodyIsExcluded(s, MobodIndex(13), true));
+
+ gravity.setGravityVector(s, Vec3(1,2,3));
+ SimTK_TEST_EQ(gravity.getGravityVector(s), Vec3(1,2,3));
+ SimTK_TEST_EQ(gravity.getDownDirection(s), UnitVec3(Vec3(1,2,3)));
+ SimTK_TEST_EQ(gravity.getMagnitude(s), Vec3(1,2,3).norm());
+
+ gravity.setDownDirection(s, UnitVec3(9,10,11));
+ SimTK_TEST_EQ(gravity.getDownDirection(s), UnitVec3(9,10,11));
+ SimTK_TEST_EQ(gravity.getMagnitude(s), Vec3(1,2,3).norm()); // no change
+
+ SimTK_TEST_MUST_THROW(gravity.setMagnitude(s, -5)); // must be >= 0
+ gravity.setMagnitude(s, 5);
+ SimTK_TEST_EQ(gravity.getDownDirection(s), UnitVec3(9,10,11)); // no change
+ SimTK_TEST(gravity.getMagnitude(s)==5);
+
+ // Changing gravity vector to zero should leave direction unchanged.
+ gravity.setGravityVector(s, Vec3(0));
+ SimTK_TEST(gravity.getGravityVector(s)==Vec3(0));
+ SimTK_TEST_EQ(gravity.getDownDirection(s), UnitVec3(9,10,11));
+ SimTK_TEST(gravity.getMagnitude(s)==0);
+
+ gravity.setZeroHeight(s, 1.25);
+ SimTK_TEST(gravity.getZeroHeight(s)==1.25);
+
+}
+
+static void testForces() {
+ MultibodySystem mbs;
+ SimbodyMatterSubsystem matter(mbs);
+ GeneralForceSubsystem forces(mbs);
+ Force::DiscreteForces discrete(forces, matter);
+ #ifdef USE_VISUALIZER
+ Visualizer viz(mbs);
+ viz.setSystemUpDirection(ZAxis);
+ viz.setCameraTransform(
+ Transform(Rotation(BodyRotationSequence,Pi/2,XAxis,Pi/8,YAxis),
+ Vec3(5,-8,2)));
+ #endif
+
+ Force::Gravity gravity(forces, matter, -ZAxis, 50, 17);
+
+ matter.Ground().addBodyDecoration(Vec3(0,0,.05),
+ DecorativeFrame(2).setColor(Green));
+
+ MobilizedBody::Weld mobod1(matter.Ground(),Vec3(0),
+ body1Info, Vec3(0));
+
+ const Rotation ZtoY(-Pi/2, XAxis);
+ MobilizedBody::Pin mobod2(mobod1, Transform(ZtoY,2*Centroid),
+ body2Info, Transform(ZtoY,Vec3(0)));
+ MobilizedBody::Pin mobod3(mobod2, Transform(ZtoY,2*Centroid),
+ body3Info, Transform(ZtoY,Vec3(0)));
+
+ State state = mbs.realizeTopology();
+ state.updQ() = Test::randVector(state.getNQ());
+ state.updU() = Test::randVector(state.getNU());
+
+ mbs.realize(state, Stage::Dynamics);
+ #ifdef USE_VISUALIZER
+ viz.report(state);
+ #endif
+
+
+ const Vector_<SpatialVec>& bodyForces =
+ mbs.getRigidBodyForces(state, Stage::Dynamics);
+
+ // Mobility forces should all be zero.
+ const Vector& mobilityForces =
+ mbs.getMobilityForces(state, Stage::Dynamics);
+ for (int i=0; i < state.getNU(); ++i)
+ SimTK_TEST(mobilityForces[i] == 0);
+
+ // Calculate body forces and torques and verify that they are correct.
+ // (These are about the body origin, not the COM.)
+ SimTK_TEST(bodyForces[0] == SpatialVec(Vec3(0))); // Ground
+ Real g = gravity.getMagnitude(state);
+ UnitVec3 d = gravity.getDownDirection(state);
+ Real h0 = gravity.getZeroHeight(state);
+ Real pe = 0;
+ for (MobodIndex i(1); i < matter.getNumBodies(); ++i) {
+ const Mobod& mobod = matter.getMobilizedBody(i);
+ const Real m = mobod.getBodyMass(state);
+ const Vec3 p_BC = mobod.getBodyMassCenterStation(state);
+ const Vec3 p_BC_G = mobod.expressVectorInGroundFrame(state,p_BC);
+ const Vec3 F = m*g*d;
+ const Vec3 M = p_BC_G % F;
+ SimTK_TEST_EQ(bodyForces[i], SpatialVec(M,F));
+
+ SimTK_TEST_EQ(gravity.getBodyForce(state, i), SpatialVec(M,F));
+
+ const Real h =
+ -dot(mobod.findStationLocationInGround(state,p_BC),d)-h0;
+ pe += m*g*h;
+ }
+
+ SimTK_TEST_EQ(gravity.getPotentialEnergy(state), pe);
+ SimTK_TEST_EQ(mbs.calcPotentialEnergy(state), pe);
+
+ // Change zero height and verify that the potential energy changes.
+ h0 = 10.3;
+ gravity.setZeroHeight(state, h0);
+ mbs.realize(state, Stage::Dynamics);
+ pe = 0;
+ for (MobodIndex i(1); i < matter.getNumBodies(); ++i) {
+ const Mobod& mobod = matter.getMobilizedBody(i);
+ const Real m = mobod.getBodyMass(state);
+ const Vec3 p_BC = mobod.getBodyMassCenterStation(state);
+ const Real h =
+ -dot(mobod.findStationLocationInGround(state,p_BC),d)-h0;
+ pe += m*g*h;
+ }
+ SimTK_TEST_EQ(gravity.getPotentialEnergy(state), pe);
+ SimTK_TEST_EQ(mbs.calcPotentialEnergy(state), pe);
+
+ // Turn off a body and make sure it doesn't see gravity after that, and
+ // that the other bodies are unchanged.
+ gravity.setBodyIsExcluded(state, mobod2, true);
+ mbs.realize(state, Stage::Dynamics);
+
+ SimTK_TEST(bodyForces[0] == SpatialVec(Vec3(0))); // Ground
+ pe = 0;
+ for (MobodIndex i(1); i < matter.getNumBodies(); ++i) {
+ const Mobod& mobod = matter.getMobilizedBody(i);
+ const Real m = mobod.getBodyMass(state);
+ const Vec3 p_BC = mobod.getBodyMassCenterStation(state);
+ const Vec3 p_BC_G = mobod.expressVectorInGroundFrame(state,p_BC);
+ const Vec3 F = m*g*d;
+ const Vec3 M = p_BC_G % F;
+
+ if (i != mobod2.getMobilizedBodyIndex()) {
+ SimTK_TEST_EQ(bodyForces[i], SpatialVec(M,F));
+ SimTK_TEST_EQ(gravity.getBodyForce(state, i), SpatialVec(M,F));
+ const Real h =
+ -dot(mobod.findStationLocationInGround(state,p_BC),d)-h0;
+ pe += m*g*h;
+ } else {
+ SimTK_TEST(bodyForces[i]==SpatialVec(Vec3(0)));
+ SimTK_TEST(gravity.getBodyForce(state, i)==SpatialVec(Vec3(0)));
+ }
+ }
+ SimTK_TEST_EQ(gravity.getPotentialEnergy(state), pe);
+ SimTK_TEST_EQ(mbs.calcPotentialEnergy(state), pe);
+
+ // Test caching.
+ const long long nevals1=gravity.getNumEvaluations();
+ SimTK_TEST(gravity.isForceCacheValid(state));
+ // This should not require re-evaluation.
+ SimTK_TEST(gravity.getBodyForces(state)[0] == SpatialVec(Vec3(0)));
+ SimTK_TEST(gravity.getNumEvaluations()==nevals1);
+
+ gravity.invalidateForceCache(state);
+ // Force re-evaluation.
+ SimTK_TEST(gravity.getBodyForces(state)[0] == SpatialVec(Vec3(0)));
+ const long long nevals2=gravity.getNumEvaluations();
+ SimTK_TEST(nevals2==nevals1+1);
+
+ state.invalidateAllCacheAtOrAbove(Stage::Velocity); // shouldn't invalidate
+ SimTK_TEST(gravity.getBodyForces(state)[0] == SpatialVec(Vec3(0)));
+ SimTK_TEST(gravity.getNumEvaluations()==nevals2);
+ mbs.realize(state, Stage::Dynamics); // shouldn't reevaluate
+ SimTK_TEST(gravity.getNumEvaluations()==nevals2);
+
+ // Bring mobod2 back in. This should only invalidate Dynamics stage, but
+ // should nevertheless force recomputation of gravity.
+ gravity.setBodyIsExcluded(state, mobod2, false);
+ SimTK_TEST(state.getSystemStage() == Stage(Stage::Dynamics-1));
+ SimTK_TEST(gravity.getBodyForces(state)[0] == SpatialVec(Vec3(0)));
+ const long long nevals3=gravity.getNumEvaluations();
+ SimTK_TEST(nevals3==nevals2+1);
+
+ // Make sure that setting gravity to zero works properly -- it is a
+ // special case since the zeroes are precalculated. This should not
+ // require a gravity evaluation.
+ gravity.setMagnitude(state, 0);
+ SimTK_TEST(!gravity.isForceCacheValid(state));
+ for (int i=0; i < matter.getNumBodies(); ++i)
+ SimTK_TEST(gravity.getBodyForces(state)[i] == SpatialVec(Vec3(0)));
+ SimTK_TEST(gravity.getNumEvaluations()==nevals3);
+
+ // Setting to non-zero should invalidate, and then require just a single
+ // evaluation to respond to multiple calls.
+ gravity.setMagnitude(state, 9.8);
+ SimTK_TEST(!gravity.isForceCacheValid(state));
+ for (int i=1; i < matter.getNumBodies(); ++i)
+ SimTK_TEST(gravity.getBodyForces(state)[i] != SpatialVec(Vec3(0)));
+ const long long nevals4=gravity.getNumEvaluations();
+ SimTK_TEST(nevals4==nevals3+1);
+
+ // Turn off velocities for gravity compensation test to eliminate Coriolis
+ // foces (shouldn't invalidate gravity forces).
+ state.updU() = 0;
+ mbs.realize(state, Stage::Acceleration);
+ const Vector_<SpatialVec>& gfrc = gravity.getBodyForces(state);
+ Vector f;
+ matter.multiplyBySystemJacobianTranspose(state, gfrc, f);
+ discrete.setAllMobilityForces(state, -f);
+ mbs.realize(state, Stage::Acceleration);
+ SimTK_TEST_EQ(state.getUDot(), Vector(state.getNU(), Real(0)));
+ SimTK_TEST(gravity.getNumEvaluations()==nevals4); // all for free?
+
+ // Sneaking a zero in by vector should behave just like setting the
+ // magnitude to zero.
+ gravity.setGravityVector(state, Vec3(0));
+ SimTK_TEST(!gravity.isForceCacheValid(state));
+ for (int i=0; i < matter.getNumBodies(); ++i)
+ SimTK_TEST(gravity.getBodyForces(state)[i] == SpatialVec(Vec3(0)));
+ SimTK_TEST(gravity.getNumEvaluations()==nevals4); // no eval needed
+}
+
+
+//==============================================================================
+// MAIN
+//==============================================================================
+int main() {
+ // Add artwork.
+ DecorativeBrick drawCube(Cube); drawCube.setOpacity(0.5).setColor(Gray);
+ body1Info.addDecoration(Centroid, drawCube);
+ body2Info.addDecoration(Centroid, drawCube);
+ body3Info.addDecoration(Centroid, drawCube);
+
+ SimTK_START_TEST("TestGravity");
+ SimTK_SUBTEST(testConstruction);
+ SimTK_SUBTEST(testParameters);
+ SimTK_SUBTEST(testForces);
+ SimTK_END_TEST();
+}
diff --git a/Simbody/tests/TestHuntCrossleyForce.cpp b/Simbody/tests/TestHuntCrossleyForce.cpp
new file mode 100644
index 0000000..ab723c1
--- /dev/null
+++ b/Simbody/tests/TestHuntCrossleyForce.cpp
@@ -0,0 +1,148 @@
+/* -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 <class T>
+void assertEqual(T val1, T val2) {
+ ASSERT(abs(val1-val2) < TOL);
+}
+
+template <int N>
+void assertEqual(Vec<N> val1, Vec<N> val2) {
+ for (int i = 0; i < N; ++i)
+ ASSERT(abs(val1[i]-val2[i]) < TOL);
+}
+
+void testForces() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralContactSubsystem contacts(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 k1 = 1.0;
+ const Real k2 = 2.0;
+ const Real stiffness1 = std::pow(k1, 2.0/3.0);
+ const Real stiffness2 = std::pow(k2, 2.0/3.0);
+ const Real dissipation1 = 0.5;
+ const Real dissipation2 = 1.0;
+ const Real us1 = 1.0;
+ const Real us2 = 0.7;
+ const Real ud1 = 0.5;
+ const Real ud2 = 0.2;
+ const Real uv1 = 0.1;
+ const Real uv2 = 0.05;
+ Random::Uniform random(0.0, 1.0);
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ ContactSetIndex setIndex = contacts.createContactSet();
+ MobilizedBody::Translation sphere(matter.updGround(), Transform(), body, Transform());
+ contacts.addBody(setIndex, sphere, ContactGeometry::Sphere(radius), Transform());
+ contacts.addBody(setIndex, matter.updGround(), ContactGeometry::HalfSpace(), Transform(Rotation(-0.5*Pi, ZAxis), Vec3(0))); // y < 0
+ HuntCrossleyForce hc(forces, contacts, setIndex);
+ hc.setBodyParameters(ContactSurfaceIndex(0), k1, dissipation1, us1, ud1, uv1);
+ hc.setBodyParameters(ContactSurfaceIndex(1), k2, dissipation2, us2, ud2, uv2);
+ const Real vt = 0.001;
+ hc.setTransitionVelocity(vt);
+ assertEqual(vt, hc.getTransitionVelocity());
+ State state = system.realizeTopology();
+
+ // Position the sphere at a variety of positions and check the normal force.
+
+ const Real stiffness = stiffness1*stiffness2/(stiffness1+stiffness2);
+ 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 = 0;
+ if (depth > 0)
+ f = (4.0/3.0)*stiffness*depth*std::sqrt(radius*stiffness*depth);
+ assertEqual(system.getRigidBodyForces(state, Stage::Dynamics)[sphere.getMobilizedBodyIndex()][1], gravity+Vec3(0, f, 0));
+ }
+
+ // Now do it with a vertical velocity and see if the dissipation force is correct.
+
+ const Real dissipation = (dissipation1*stiffness2+dissipation2*stiffness1)/(stiffness1+stiffness2);
+ 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 = 0;
+ if (depth > 0)
+ fh = (4.0/3.0)*stiffness*depth*std::sqrt(radius*stiffness*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*(1.0+1.5*dissipation*v);
+ if (f < 0)
+ f = 0;
+ assertEqual(system.getRigidBodyForces(state, Stage::Dynamics)[sphere.getMobilizedBodyIndex()][1], gravity+Vec3(0, f, 0));
+ }
+ }
+
+ // Do it with a horizontal velocity and see if the friction force is correct.
+
+ const Real us = 2.0*us1*us2/(us1+us2);
+ const Real ud = 2.0*ud1*ud2/(ud1+ud2);
+ const Real uv = 2.0*uv1*uv2/(uv1+uv2);
+ Vector_<SpatialVec> 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 = 0;
+ if (depth > 0)
+ fh = (4.0/3.0)*stiffness*depth*std::sqrt(radius*stiffness*depth);
+ for (Real v = -1.0; v <= 1.0; v += 0.1) {
+ sphere.setUToFitLinearVelocity(state, Vec3(v, 0, 0));
+ system.realize(state, Stage::Dynamics);
+ const Real vrel = std::abs(v/vt);
+ Real ff = (v < 0 ? 1 : -1)*fh*(std::min(vrel, 1.0)*(ud+2*(us-ud)/(1+vrel*vrel))+uv*std::fabs(v));
+ const Vec3 totalForce = gravity+Vec3(ff, fh, 0);
+ expectedForce = SpatialVec(Vec3(0), Vec3(0));
+ Vec3 contactPointInSphere = sphere.findStationAtGroundPoint(state, Vec3(0, -stiffness1*depth/(stiffness1+stiffness2), 0));
+ sphere.applyForceToBodyPoint(state, contactPointInSphere, totalForce, 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 --git a/Simbody/tests/TestLinearBushing.cpp b/Simbody/tests/TestLinearBushing.cpp
new file mode 100644
index 0000000..99c69c7
--- /dev/null
+++ b/Simbody/tests/TestLinearBushing.cpp
@@ -0,0 +1,754 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Test the Force::LinearBushing force element.
+ */
+
+#include "SimTKsimbody.h"
+#include "SimTKcommon/Testing.h"
+
+#include <cstdio>
+#include <exception>
+#include <iostream>
+using std::cout; using std::endl;
+
+//#define VISUALIZE
+#ifdef VISUALIZE
+ #define WAIT_FOR_INPUT(str) \
+ do {printf(str); getchar();} while(false)
+ #define REPORT(state) \
+ do {viz.report(state);viz.zoomCameraToShowAllGeometry();} while (false)
+#else
+ #define WAIT_FOR_INPUT(str)
+ #define REPORT(state)
+#endif
+
+
+using namespace SimTK;
+
+void testParameterSetting() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+
+ // This is the frame on body1.
+ const Transform X_B1F(Test::randRotation(), Test::randVec3());
+ // This is the frame on body 2.
+ const Transform X_B2M(Test::randRotation(), Test::randVec3());
+ // Material properties.
+ const Vec6 k = 10*Vec6(1,2,1,1,5,1);
+ const Vec6 c = 1.3*Vec6(1,.1,1,.11,1,11);
+
+ const Real Mass = 1.234;
+ Body::Rigid aBody(MassProperties(Mass, Vec3(.1,.2,.3),
+ Mass*Inertia(1,1.1,1.2,0.01,0.02,0.03)));
+
+ MobilizedBody::Free body1(matter.Ground(), Transform(),
+ aBody, Transform());
+ MobilizedBody::Free body2(matter.Ground(), Transform(),
+ aBody, Transform());
+
+ Force::LinearBushing bushing
+ (forces, body1, Vec3(1,2,3),
+ body2, Vec3(-2,-3,-4),
+ Vec6(5), Vec6(7));
+ SimTK_TEST_EQ(bushing.getDefaultFrameOnBody1(),Transform(Vec3(1,2,3)));
+ SimTK_TEST_EQ(bushing.getDefaultFrameOnBody2(),Transform(Vec3(-2,-3,-4)));
+ SimTK_TEST_EQ(bushing.getDefaultStiffness(),Vec6(5,5,5,5,5,5));
+ SimTK_TEST_EQ(bushing.getDefaultDamping(),Vec6(7,7,7,7,7,7));
+
+ bushing.setDefaultFrameOnBody1(Vec3(-1,-2,-3))
+ .setDefaultFrameOnBody2(Vec3(2,3,4))
+ .setDefaultStiffness(Vec6(9))
+ .setDefaultDamping(Vec6(11));
+ SimTK_TEST_EQ(bushing.getDefaultFrameOnBody1(),Transform(Vec3(-1,-2,-3)));
+ SimTK_TEST_EQ(bushing.getDefaultFrameOnBody2(),Transform(Vec3(2,3,4)));
+ SimTK_TEST_EQ(bushing.getDefaultStiffness(),Vec6(9));
+ SimTK_TEST_EQ(bushing.getDefaultDamping(),Vec6(11));
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ system.realizeModel(state);
+
+ // See if defaults transfer to default state properly.
+ SimTK_TEST_EQ(bushing.getFrameOnBody1(state),Transform(Vec3(-1,-2,-3)));
+ SimTK_TEST_EQ(bushing.getFrameOnBody2(state),Transform(Vec3(2,3,4)));
+ SimTK_TEST_EQ(bushing.getStiffness(state),Vec6(9));
+ SimTK_TEST_EQ(bushing.getDamping(state),Vec6(11));
+
+ bushing.setFrameOnBody1(state, X_B1F)
+ .setFrameOnBody2(state, X_B2M)
+ .setStiffness(state, k)
+ .setDamping(state, c);
+
+ SimTK_TEST_EQ(bushing.getFrameOnBody1(state),X_B1F);
+ SimTK_TEST_EQ(bushing.getFrameOnBody2(state),X_B2M);
+ SimTK_TEST_EQ(bushing.getStiffness(state),k);
+ SimTK_TEST_EQ(bushing.getDamping(state),c);
+
+
+}
+
+// Here we're going to build a chain like this:
+//
+// Ground --> body1 ==> body2
+//
+// where body1 is Free, and body2 is connected to body1 by
+// a series of mobilizers designed to have the same kinematics
+// as a LinearBushing element connected between them.
+//
+// We'll verify that the LinearBushing calculates kinematics
+// that exactly matches the composite joint. Then we'll run
+// for a while and see that energy+dissipation is conserved.
+void testKinematicsAndEnergyConservation() {
+ // Build system.
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+
+ // This is the frame on body1.
+ const Transform X_B1F(Test::randRotation(), Test::randVec3());
+ // This is the frame on body 2.
+ const Transform X_B2M(Test::randRotation(), Test::randVec3());
+ // Material properties.
+ const Vec6 k = 10*Vec6(1,1,1,1,1,1);
+ const Vec6 c = 1*Vec6(1,1,1,1,1,1);
+
+ // Use ugly mass properties to make sure we test all the terms.
+ const Real Mass = 1.234;
+ const Vec3 HalfShape = Vec3(1,.5,.25)/2;
+
+ Body::Rigid aBody(MassProperties(Mass, Vec3(.1,.2,.3),
+ Mass*Inertia(1,1.1,1.2,0.01,0.02,0.03)));
+ aBody.addDecoration(Transform(), DecorativeEllipsoid(HalfShape)
+ .setOpacity(0.25)
+ .setColor(Blue));
+ aBody.addDecoration(X_B1F,
+ DecorativeFrame(0.5).setColor(Red));
+ aBody.addDecoration(X_B2M,
+ DecorativeFrame(0.5).setColor(Green));
+
+ // Ground attachement frame.
+ const Transform X_GA(
+ Test::randRotation(), Test::randVec3());
+
+ MobilizedBody::Free body1(matter.Ground(), X_GA,
+ aBody, X_B2M);
+
+ // This is to keep the system from flying away.
+ Force::TwoPointLinearSpring(forces,matter.Ground(),Vec3(0),body1,Vec3(0),
+ 10,0);
+
+ Body::Rigid massless(MassProperties(0, Vec3(0), Inertia(0)));
+ const Rotation ZtoX(Pi/2, YAxis);
+ const Rotation ZtoY(-Pi/2, XAxis);
+ MobilizedBody::Cartesian dummy1(body1, X_B1F,
+ massless, Transform());
+ MobilizedBody::Pin dummy2(dummy1, ZtoX,
+ massless, ZtoX);
+ MobilizedBody::Pin dummy3(dummy2, ZtoY,
+ massless, ZtoY);
+ MobilizedBody::Pin body2 (dummy3, Transform(), // about Z
+ aBody, X_B2M);
+
+ // Set the actual parameters in the State below.
+ Force::LinearBushing bushing
+ (forces, body1, body2, Vec6(0), Vec6(0));
+
+#ifdef VISUALIZE
+ Visualizer viz(system);
+ viz.setBackgroundType(Visualizer::SolidColor);
+ system.addEventReporter(new Visualizer::Reporter(viz, 0.01));
+#endif
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+
+ bushing.setFrameOnBody1(state, X_B1F)
+ .setFrameOnBody2(state, X_B2M)
+ .setStiffness(state, k)
+ .setDamping(state, c);
+
+ REPORT(state);
+ WAIT_FOR_INPUT("\nDefault state -- hit ENTER\n");
+
+ state.updQ() = Test::randVector(state.getNQ());
+ state.updU() = Test::randVector(state.getNU());
+ state.updU()(3,3) = 0; // kill translational velocity
+
+ const Real Accuracy = 1e-6;
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setAccuracy(Accuracy);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ State istate = integ.getState();
+ system.realize(istate, Stage::Velocity);
+ Vector xyz = istate.getQ()(7,3); Vector ang = istate.getQ()(10,3);
+ Vector xyzd = istate.getQDot()(7,3); Vector angd = istate.getQDot()(10,3);
+ Vec6 mq = Vec6(ang[0],ang[1],ang[2],xyz[0],xyz[1],xyz[2]);
+ Vec6 mqd = Vec6(angd[0],angd[1],angd[2],xyzd[0],xyzd[1],xyzd[2]);
+
+ SimTK_TEST_EQ(bushing.getQ(istate), mq);
+ SimTK_TEST_EQ(bushing.getQDot(istate), mqd);
+
+ const Real initialEnergy = system.calcEnergy(istate);
+
+ SimTK_TEST( bushing.getDissipatedEnergy(istate) == 0 );
+
+ REPORT(integ.getState());
+
+ cout << "t=" << integ.getTime()
+ << "\nE=" << initialEnergy
+ << "\nmobilizer q=" << mq
+ << "\nbushing q=" << bushing.getQ(istate)
+ << "\nmobilizer qd=" << mqd
+ << "\nbushing qd=" << bushing.getQDot(istate)
+ << endl;
+ WAIT_FOR_INPUT("After initialize -- hit ENTER\n");
+
+ // Simulate it.
+ ts.stepTo(5.0);
+ istate = integ.getState();
+ system.realize(istate, Stage::Velocity);
+ xyz = istate.getQ()(7,3); ang = istate.getQ()(10,3);
+ xyzd = istate.getQDot()(7,3); angd = istate.getQDot()(10,3);
+ mq = Vec6(ang[0],ang[1],ang[2],xyz[0],xyz[1],xyz[2]);
+ mqd = Vec6(angd[0],angd[1],angd[2],xyzd[0],xyzd[1],xyzd[2]);
+
+ SimTK_TEST_EQ(bushing.getQ(istate), mq);
+ SimTK_TEST_EQ(bushing.getQDot(istate), mqd);
+
+ // This should account for all the energy.
+ const Real finalEnergy = system.calcEnergy(istate)
+ + bushing.getDissipatedEnergy(istate);
+
+ SimTK_TEST_EQ_TOL(initialEnergy, finalEnergy, 10*Accuracy);
+
+ // Let's find everything and see if the bushing agrees.
+ const Transform& X_GB1 = body1.getBodyTransform(istate);
+ const Transform& X_GB2 = body2.getBodyTransform(istate);
+ const Transform X_GF = X_GB1*X_B1F;
+ const Transform X_GM = X_GB2*X_B2M;
+ SimTK_TEST_EQ(X_GF, bushing.getX_GF(istate));
+ SimTK_TEST_EQ(X_GM, bushing.getX_GM(istate));
+ SimTK_TEST_EQ(~X_GF*X_GM, bushing.getX_FM(istate));
+
+ // Calculate velocities and ask the bushing for same.
+ const SpatialVec& V_GB1 = body1.getBodyVelocity(istate);
+ const SpatialVec& V_GB2 = body2.getBodyVelocity(istate);
+ const SpatialVec
+ V_GF(V_GB1[0], body1.findStationVelocityInGround(istate,X_B1F.p()));
+ const SpatialVec
+ V_GM(V_GB2[0], body2.findStationVelocityInGround(istate,X_B2M.p()));
+
+ SimTK_TEST_EQ(V_GF, bushing.getV_GF(istate));
+ SimTK_TEST_EQ(V_GM, bushing.getV_GM(istate));
+
+ const SpatialVec
+ V_FM(~X_B1F.R()*body2.findBodyAngularVelocityInAnotherBody(istate,body1),
+ ~X_B1F.R()*body2.findStationVelocityInAnotherBody(istate,X_B2M.p(),body1));
+
+ SimTK_TEST_EQ(V_FM, bushing.getV_FM(istate));
+
+ cout << "t=" << integ.getTime()
+ << "\nE=" << system.calcEnergy(istate)
+ << "\nE-XW=" << finalEnergy << " final-init=" << finalEnergy-initialEnergy
+ << "\nmobilizer q=" << mq
+ << "\nbushing q=" << bushing.getQ(istate)
+ << "\nmobilizer qd=" << mqd
+ << "\nbushing qd=" << bushing.getQDot(istate)
+ << endl;
+
+ WAIT_FOR_INPUT("After simulation -- hit ENTER\n");
+}
+
+
+// Here we're going to build a chain like this:
+//
+// Ground --> body1 ==> body2
+//
+// where body1 is Free, and body2 is connected to body1 by
+// a Bushing mobilizer, which should have the same kinematics
+// as a LinearBushing element connected between them.
+//
+// We'll verify that the LinearBushing calculates kinematics
+// that exactly matches the Bushing mobilizer. Then we'll run
+// for a while and see that energy+dissipation is conserved.
+void testKinematicsAndEnergyConservationUsingBushingMobilizer() {
+ // Build system.
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+
+ // This is the frame on body1.
+ const Transform X_B1F(Test::randRotation(), Test::randVec3());
+ // This is the frame on body 2.
+ const Transform X_B2M(Test::randRotation(), Test::randVec3());
+ // Material properties.
+ const Vec6 k = 10*Vec6(1,1,1,1,1,1);
+ const Vec6 c = 1*Vec6(1,1,1,1,1,1);
+
+ // Use ugly mass properties to make sure we test all the terms.
+ const Real Mass = 1.234;
+ const Vec3 HalfShape = Vec3(1,.5,.25)/2;
+
+ Body::Rigid aBody(MassProperties(Mass, Vec3(.1,.2,.3),
+ Mass*Inertia(1,1.1,1.2,0.01,0.02,0.03)));
+ aBody.addDecoration(Transform(), DecorativeEllipsoid(HalfShape)
+ .setOpacity(0.25)
+ .setColor(Blue));
+ aBody.addDecoration(X_B1F,
+ DecorativeFrame(0.5).setColor(Red));
+ aBody.addDecoration(X_B2M,
+ DecorativeFrame(0.5).setColor(Green));
+
+ // Ground attachement frame.
+ const Transform X_GA(
+ Test::randRotation(), Test::randVec3());
+
+ MobilizedBody::Free body1(matter.Ground(), X_GA,
+ aBody, X_B2M);
+
+ // This is to keep the system from flying away.
+ Force::TwoPointLinearSpring(forces,matter.Ground(),Vec3(0),body1,Vec3(0),
+ 10,0);
+
+ MobilizedBody::Bushing body2(body1, X_B1F,
+ aBody, X_B2M);
+
+ // Set the actual parameters in the State below.
+ Force::LinearBushing bushing
+ (forces, body1, body2, Vec6(0), Vec6(0));
+
+#ifdef VISUALIZE
+ Visualizer viz(system);
+ viz.setBackgroundType(Visualizer::SolidColor);
+ system.addEventReporter(new Visualizer::Reporter(viz, 0.01));
+#endif
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+
+ bushing.setFrameOnBody1(state, X_B1F)
+ .setFrameOnBody2(state, X_B2M)
+ .setStiffness(state, k)
+ .setDamping(state, c);
+
+ REPORT(state);
+ WAIT_FOR_INPUT("\nDefault state -- hit ENTER\n");
+
+ state.updQ() = Test::randVector(state.getNQ());
+ state.updU() = Test::randVector(state.getNU());
+ state.updU()(3,3) = 0; // kill translational velocity
+
+ const Real Accuracy = 1e-6;
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setAccuracy(Accuracy);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ State istate = integ.getState();
+ system.realize(istate, Stage::Velocity);
+
+ // Get the q's and qdot's from the Bushing mobilizer.
+ Vec6 mq = body2.getQ(istate);
+ Vec6 mqd = body2.getQDot(istate);
+
+ SimTK_TEST_EQ(bushing.getQ(istate), mq);
+ SimTK_TEST_EQ(bushing.getQDot(istate), mqd);
+
+ const Real initialEnergy = system.calcEnergy(istate);
+
+ SimTK_TEST( bushing.getDissipatedEnergy(istate) == 0 );
+
+ REPORT(integ.getState());
+
+ cout << "t=" << integ.getTime()
+ << "\nE=" << initialEnergy
+ << "\nmobilizer q=" << mq
+ << "\nbushing q=" << bushing.getQ(istate)
+ << "\nmobilizer qd=" << mqd
+ << "\nbushing qd=" << bushing.getQDot(istate)
+ << endl;
+ WAIT_FOR_INPUT("After initialize -- hit ENTER\n");
+
+ // Simulate it.
+ ts.stepTo(5.0);
+ istate = integ.getState();
+ system.realize(istate, Stage::Velocity);
+ mq = body2.getQ(istate);
+ mqd = body2.getQDot(istate);
+
+ SimTK_TEST_EQ(bushing.getQ(istate), mq);
+ SimTK_TEST_EQ(bushing.getQDot(istate), mqd);
+
+ // This should account for all the energy.
+ const Real finalEnergy = system.calcEnergy(istate)
+ + bushing.getDissipatedEnergy(istate);
+
+ SimTK_TEST_EQ_TOL(initialEnergy, finalEnergy, 10*Accuracy);
+
+ // Let's find everything and see if the bushing agrees.
+ const Transform& X_GB1 = body1.getBodyTransform(istate);
+ const Transform& X_GB2 = body2.getBodyTransform(istate);
+ const Transform X_GF = X_GB1*X_B1F;
+ const Transform X_GM = X_GB2*X_B2M;
+ SimTK_TEST_EQ(X_GF, bushing.getX_GF(istate));
+ SimTK_TEST_EQ(X_GM, bushing.getX_GM(istate));
+ SimTK_TEST_EQ(~X_GF*X_GM, bushing.getX_FM(istate));
+
+ // Calculate velocities and ask the bushing for same.
+ const SpatialVec& V_GB1 = body1.getBodyVelocity(istate);
+ const SpatialVec& V_GB2 = body2.getBodyVelocity(istate);
+ const SpatialVec
+ V_GF(V_GB1[0], body1.findStationVelocityInGround(istate,X_B1F.p()));
+ const SpatialVec
+ V_GM(V_GB2[0], body2.findStationVelocityInGround(istate,X_B2M.p()));
+
+ SimTK_TEST_EQ(V_GF, bushing.getV_GF(istate));
+ SimTK_TEST_EQ(V_GM, bushing.getV_GM(istate));
+
+ const SpatialVec
+ V_FM(~X_B1F.R()*body2.findBodyAngularVelocityInAnotherBody(istate,body1),
+ ~X_B1F.R()*body2.findStationVelocityInAnotherBody(istate,X_B2M.p(),body1));
+
+ SimTK_TEST_EQ(V_FM, bushing.getV_FM(istate));
+
+ cout << "t=" << integ.getTime()
+ << "\nE=" << system.calcEnergy(istate)
+ << "\nE-XW=" << finalEnergy << " final-init=" << finalEnergy-initialEnergy
+ << "\nmobilizer q=" << mq
+ << "\nbushing q=" << bushing.getQ(istate)
+ << "\nmobilizer qd=" << mqd
+ << "\nbushing qd=" << bushing.getQDot(istate)
+ << endl;
+
+ WAIT_FOR_INPUT("After simulation -- hit ENTER\n");
+}
+
+
+// Here we're going to build a system containing two parallel multibody
+// trees, each consisting of a single body connected to ground. For the
+// first system the body is on a Free mobilizer and a LinearBushing
+// connects the body and Ground. In the second, a series of mobilizers
+// exactly mimics the kinematics, and a set of mobility springs and
+// dampers are used to mimic the forces that should be produced by the
+// bushing. We'll then evaluate in some arbitrary configuration and
+// make sure the mobilizer reaction forces match the corresponding
+// LinearBushing force.
+void testForces() {
+ // Create the system.
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, 0*Vec3(0, -9.8, 0));
+
+ const Real Mass = 1;
+ const Vec3 HalfShape = Vec3(1,.5,.25)/2;
+ const Transform BodyAttach(Rotation(), Vec3(HalfShape[0],0,0));
+ Body::Rigid brickBody(MassProperties(Mass, Vec3(.1,.2,.3),
+ Mass*Inertia(1,1.1,1.2,0.01,0.02,0.03)));
+ //Body::Rigid brickBody(MassProperties(Mass, Vec3(0),
+ // Mass*UnitInertia::ellipsoid(HalfShape)));
+ brickBody.addDecoration(Transform(), DecorativeEllipsoid(HalfShape)
+ .setOpacity(0.25)
+ .setColor(Blue));
+ brickBody.addDecoration(BodyAttach,
+ DecorativeFrame(0.5).setColor(Red));
+
+ MobilizedBody::Free brick1(matter.Ground(), Transform(),
+ brickBody, BodyAttach);
+
+ Body::Rigid massless(MassProperties(0, Vec3(0), Inertia(0)));
+ const Rotation ZtoX(Pi/2, YAxis);
+ const Rotation ZtoY(-Pi/2, XAxis);
+
+ // This sequence is used to match the kinematics of a "forward" direction
+ // bushing where Ground is body 1.
+ //MobilizedBody::Cartesian dummy1(matter.Ground(), Vec3(1,1,1)+Vec3(2,0,0),
+ // massless, Transform());
+ //MobilizedBody::Pin dummy2(dummy1, ZtoX,
+ // massless, ZtoX);
+ //MobilizedBody::Pin dummy3(dummy2, ZtoY,
+ // massless, ZtoY);
+ //MobilizedBody::Pin brick2(dummy3, Transform(), // about Z
+ // brickBody, BodyAttach);
+
+ // This sequence is used to match the kinematics of a "reverse" direction
+ // bushing where Ground is body 2 (this is trickier).
+ MobilizedBody::Pin dummy1(matter.Ground(), Vec3(1,1,1)+Vec3(2,0,0),
+ massless, Transform(), MobilizedBody::Reverse);
+ MobilizedBody::Pin dummy2(dummy1, ZtoY,
+ massless, ZtoY, MobilizedBody::Reverse);
+ MobilizedBody::Pin dummy3(dummy2, ZtoX,
+ massless, ZtoX, MobilizedBody::Reverse);
+ MobilizedBody::Cartesian brick2(dummy3, Transform(),
+ brickBody, BodyAttach, MobilizedBody::Reverse);
+
+ const Vec6 k = Test::randVec<6>().abs(); // must be > 0
+ const Vec6 c = Test::randVec<6>().abs(); // "
+ Transform GroundAttach(Rotation(), Vec3(1,1,1));
+ matter.Ground().updBody().addDecoration(GroundAttach,
+ DecorativeFrame(0.5).setColor(Green));
+
+ const Vec6 k1 = k;
+ const Vec6 c1 = c;
+ // This is the Forward version
+ //Force::LinearBushing bushing
+ // (forces, matter.Ground(), GroundAttach,
+ // brick1, BodyAttach, k1, c1);
+
+ // This is the reverse version -- the moving body is the first body
+ // for the bushing; Ground is second. This is a harder test because
+ // the F frame is moving.
+ Force::LinearBushing bushing
+ (forces, brick1, BodyAttach,
+ matter.Ground(), GroundAttach,
+ k1, c1);
+
+ Force::LinearBushing bushing2
+ (forces, brick1, brick2, k, c);
+
+ Transform GroundAttach2(Rotation(), Vec3(1,1,1) + Vec3(2,0,0));
+ matter.Ground().updBody().addDecoration(GroundAttach2,
+ DecorativeFrame(0.5).setColor(Green));
+ const Vec6 k2 = k;
+ const Vec6 c2 = c;
+
+ // This is what you would do for a forward-direction set of
+ // mobilizers to match the forward bushing.
+ //Force::MobilityLinearSpring kqx(forces, dummy2, 0, k2[0], 0);
+ //Force::MobilityLinearDamper cqx(forces, dummy2, 0, c2[0]);
+ //Force::MobilityLinearSpring kqy(forces, dummy3, 0, k2[1], 0);
+ //Force::MobilityLinearDamper cqy(forces, dummy3, 0, c2[1]);
+ //Force::MobilityLinearSpring kqz(forces, brick2, 0, k2[2], 0);
+ //Force::MobilityLinearDamper cqz(forces, brick2, 0, c2[2]);
+ //Force::MobilityLinearSpring kpx(forces, dummy1, 0, k2[3], 0);
+ //Force::MobilityLinearDamper cpx(forces, dummy1, 0, c2[3]);
+ //Force::MobilityLinearSpring kpy(forces, dummy1, 1, k2[4], 0);
+ //Force::MobilityLinearDamper cpy(forces, dummy1, 1, c2[4]);
+ //Force::MobilityLinearSpring kpz(forces, dummy1, 2, k2[5], 0);
+ //Force::MobilityLinearDamper cpz(forces, dummy1, 2, c2[5]);
+
+ // This is the set of force elements that mimics the bushing hooked
+ // up in the reverse direction.
+ Force::MobilityLinearSpring kqx(forces, dummy3, 0, k2[0], 0);
+ Force::MobilityLinearDamper cqx(forces, dummy3, 0, c2[0]);
+ Force::MobilityLinearSpring kqy(forces, dummy2, 0, k2[1], 0);
+ Force::MobilityLinearDamper cqy(forces, dummy2, 0, c2[1]);
+ Force::MobilityLinearSpring kqz(forces, dummy1, 0, k2[2], 0);
+ Force::MobilityLinearDamper cqz(forces, dummy1, 0, c2[2]);
+ Force::MobilityLinearSpring kpx(forces, brick2, 0, k2[3], 0);
+ Force::MobilityLinearDamper cpx(forces, brick2, 0, c2[3]);
+ Force::MobilityLinearSpring kpy(forces, brick2, 1, k2[4], 0);
+ Force::MobilityLinearDamper cpy(forces, brick2, 1, c2[4]);
+ Force::MobilityLinearSpring kpz(forces, brick2, 2, k2[5], 0);
+ Force::MobilityLinearDamper cpz(forces, brick2, 2, c2[5]);
+
+#ifdef VISUALIZE
+ Visualizer viz(system);
+ viz.setBackgroundType(Visualizer::SolidColor);
+ system.addEventReporter(new Visualizer::Reporter(viz, 0.01));
+#endif
+
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+
+ REPORT(state);
+
+ Rotation RR = Test::randRotation();
+ brick1.setQToFitTransform(state, RR);
+
+ Vec3 qRRinv;
+ qRRinv = Rotation(~RR).convertRotationToBodyFixedXYZ();
+
+ brick2.setQToFitTransform(state, ~RR*Vec3(-1,-1,-1));
+ dummy3.setOneQ(state, 0, qRRinv[0]);
+ dummy2.setOneQ(state, 0, qRRinv[1]);
+ dummy1.setOneQ(state, 0, qRRinv[2]);
+
+
+ REPORT(state);
+ WAIT_FOR_INPUT("testForces() trial pose -- hit ENTER\n");
+
+ system.realize(state,Stage::Acceleration);
+
+ //cout << "\nf=" << bushing.getF(state) << endl;
+ //cout << "F_GM=" << bushing.getF_GM(state) << endl;
+ //cout << "F_GF=" << bushing.getF_GF(state) << endl;
+
+ Vector_<SpatialVec> reactions;
+ matter.calcMobilizerReactionForces(state, reactions);
+ //cout << "Reaction force on brick2="
+ // << reactions[brick2.getMobilizedBodyIndex()] << endl;
+
+ SimTK_TEST_EQ(bushing.getF_GF(state),
+ reactions[brick2.getMobilizedBodyIndex()]);
+}
+
+// This test is identical to testForces() except we use a reverse Bushing
+// mobilizer rather than a sequence of Translation and Pins.
+//
+// Here we're going to build a system containing two parallel multibody
+// trees, each consisting of a single body connected to ground. For the
+// first system the body is on a Free mobilizer and a LinearBushing
+// connects the body and Ground, although with Ground serving as "body2" for
+// the bushing (i.e., the M frame is on Ground). In the second, a reversed
+// Bushing mobilizer exactly mimics the kinematics, and a set of mobility
+// springs and dampers are used to mimic the forces that should be produced by
+// the bushing. We'll then evaluate in some arbitrary configuration and
+// make sure the mobilizer reaction forces match the corresponding
+// LinearBushing force.
+void testForcesUsingReverseBushingMobilizer() {
+ // Create the system.
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, 0*Vec3(0, -9.8, 0));
+
+ matter.Ground().addBodyDecoration(Transform(),
+ DecorativeFrame(.25).setColor(Purple));
+
+ const Real Mass = 1;
+ const Vec3 HalfShape = Vec3(1,.5,.25)/2;
+ const Transform BodyAttach(Rotation(), Vec3(HalfShape[0],0,0));
+ Body::Rigid brickBody(MassProperties(Mass, Vec3(.1,.2,.3),
+ Mass*Inertia(1,1.1,1.2,0.01,0.02,0.03)));
+ brickBody.addDecoration(Transform(), DecorativeEllipsoid(HalfShape)
+ .setOpacity(0.25)
+ .setColor(Blue));
+ brickBody.addDecoration(BodyAttach,
+ DecorativeFrame(0.5).setColor(Red));
+
+ MobilizedBody::Free brick1(matter.Ground(), Transform(),
+ brickBody, BodyAttach);
+ brick1.addBodyDecoration(Transform(), DecorativeText("1").setScale(.25));
+
+ // This mobilizer is used to match the kinematics of a "reverse" direction
+ // bushing where Ground is body 2.
+ MobilizedBody::Bushing brick2(matter.Ground(), Vec3(1,1,1)+Vec3(2,0,0),
+ brickBody, BodyAttach, MobilizedBody::Reverse);
+ brick2.addBodyDecoration(Transform(), DecorativeText("2").setScale(.25));
+
+ const Vec6 k = Test::randVec<6>().abs(); // must be > 0
+ const Vec6 c = Test::randVec<6>().abs(); // "
+ Transform GroundAttach(Rotation(), Vec3(1,1,1));
+ matter.Ground().updBody().addDecoration(GroundAttach,
+ DecorativeFrame(0.5).setColor(Green));
+
+ const Vec6 k1 = k;
+ const Vec6 c1 = c;
+
+ // This is reversed -- the moving body is the first body
+ // for the bushing; Ground is second. This is a hard test because
+ // the F frame is moving.
+ Force::LinearBushing bushing
+ (forces, brick1, BodyAttach, // .5,0,0
+ matter.Ground(), GroundAttach, // 1,1,1
+ k1, c1);
+
+ Force::LinearBushing bushing2
+ (forces, brick1, brick2, k, c);
+
+ Transform GroundAttach2(Rotation(), Vec3(1,1,1) + Vec3(2,0,0));
+ matter.Ground().updBody().addDecoration(GroundAttach2,
+ DecorativeFrame(0.5).setColor(Green));
+ const Vec6 k2 = k;
+ const Vec6 c2 = c;
+
+ // This is the set of force elements that mimics the bushing hooked
+ // up in the reverse direction.
+ Force::MobilityLinearSpring kqx(forces, brick2, 0, k2[0], 0);
+ Force::MobilityLinearDamper cqx(forces, brick2, 0, c2[0]);
+ Force::MobilityLinearSpring kqy(forces, brick2, 1, k2[1], 0);
+ Force::MobilityLinearDamper cqy(forces, brick2, 1, c2[1]);
+ Force::MobilityLinearSpring kqz(forces, brick2, 2, k2[2], 0);
+ Force::MobilityLinearDamper cqz(forces, brick2, 2, c2[2]);
+ Force::MobilityLinearSpring kpx(forces, brick2, 3, k2[3], 0);
+ Force::MobilityLinearDamper cpx(forces, brick2, 3, c2[3]);
+ Force::MobilityLinearSpring kpy(forces, brick2, 4, k2[4], 0);
+ Force::MobilityLinearDamper cpy(forces, brick2, 4, c2[4]);
+ Force::MobilityLinearSpring kpz(forces, brick2, 5, k2[5], 0);
+ Force::MobilityLinearDamper cpz(forces, brick2, 5, c2[5]);
+
+#ifdef VISUALIZE
+ Visualizer viz(system);
+ viz.setBackgroundType(Visualizer::SolidColor);
+ system.addEventReporter(new Visualizer::Reporter(viz, 0.01));
+#endif
+
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+
+ REPORT(state);
+
+ Rotation RR = Test::randRotation();
+ brick1.setQToFitTransform(state, RR);
+ system.realize(state, Stage::Position);
+
+ cout << "\nbrick1 .q=" << brick1.getQ(state) << endl;
+ cout << "bushing.q=" << bushing.getQ(state) << endl;
+
+ brick2.setQToFitTransform(state, Transform(RR, -Vec3(1,1,1)));
+
+ cout << "brick2 .q=" << brick2.getQ(state) << endl;
+
+
+ REPORT(state);
+ WAIT_FOR_INPUT("testForcesUsingBushing() trial pose -- hit ENTER\n");
+
+ system.realize(state,Stage::Acceleration);
+
+ //cout << "\nf=" << bushing.getF(state) << endl;
+ //cout << "F_GM=" << bushing.getF_GM(state) << endl;
+ cout << "F_GF=" << bushing.getF_GF(state) << endl;
+
+ Vector_<SpatialVec> reactions;
+ matter.calcMobilizerReactionForces(state, reactions);
+ cout << "Reaction force on brick2="
+ << reactions[brick2.getMobilizedBodyIndex()] << endl;
+
+ SimTK_TEST_EQ(bushing.getF_GF(state),
+ reactions[brick2.getMobilizedBodyIndex()]);
+}
+
+int main() {
+ SimTK_START_TEST("TestLinearBushing");
+ SimTK_SUBTEST(testParameterSetting);
+ SimTK_SUBTEST(testKinematicsAndEnergyConservation);
+ SimTK_SUBTEST(testKinematicsAndEnergyConservationUsingBushingMobilizer);
+ SimTK_SUBTEST(testForces);
+ SimTK_SUBTEST(testForcesUsingReverseBushingMobilizer);
+ SimTK_END_TEST();
+}
+
diff --git a/Simbody/tests/TestLoneParticle.cpp b/Simbody/tests/TestLoneParticle.cpp
new file mode 100644
index 0000000..d95993d
--- /dev/null
+++ b/Simbody/tests/TestLoneParticle.cpp
@@ -0,0 +1,169 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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"
+#include "SimTKcommon/Testing.h"
+
+#include <vector>
+#include <map>
+
+using namespace SimTK;
+using namespace std;
+
+const Real TOL = 1e-10;
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+template <class T>
+void assertEqual(T val1, T val2) {
+ ASSERT(abs(val1-val2) < TOL);
+}
+
+template <int N>
+void assertEqual(Vec<N> val1, Vec<N> val2) {
+ for (int i = 0; i < N; ++i)
+ ASSERT(abs(val1[i]-val2[i]) < TOL);
+}
+
+static void compareToTranslate(bool prescribe, Motion::Level level) {
+ // Create a system of pairs of identical bodies, where half will be implemented with RBNodeLoneParticle
+ // and half with RBNodeTranslate.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem force(system);
+ Force::UniformGravity gravity(force, matter, Vec3(1.1, 1.2, 1.3));
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ Random::Gaussian random(0.0, 3.0);
+ const int numBodies = 10;
+ for (int i = 0; i < numBodies; i++) {
+ MobilizedBody::Translation body1(matter.updGround(), body);
+ MobilizedBody::Translation body2(matter.updGround(), Vec3(0), body, Vec3(1e-100));
+ Vec3 station1(random.getValue(), random.getValue(), random.getValue());
+ Vec3 station2(random.getValue(), random.getValue(), random.getValue());
+ Real length = random.getValue();
+ Force::TwoPointLinearSpring(force, matter.updGround(), station1, body1, station2, 1.0, length);
+ Force::TwoPointLinearSpring(force, matter.updGround(), station1, body2, station2, 1.0, length);
+ if (prescribe) {
+ Real phase = random.getValue();
+ Motion::Sinusoid(body1, level, 1.5, 1.1, phase);
+ Motion::Sinusoid(body2, level, 1.5, 1.1, phase);
+ }
+ }
+
+ // Initialize the state.
+
+ State state = system.realizeTopology();
+ for (int i = 0; i < numBodies; i++) {
+ Vec3 pos(random.getValue(), random.getValue(), random.getValue());
+ Vec3 vel(random.getValue(), random.getValue(), random.getValue());
+ const MobilizedBody& body1 = matter.getMobilizedBody(MobilizedBodyIndex(2*i+1));
+ const MobilizedBody& body2 = matter.getMobilizedBody(MobilizedBodyIndex(2*i+2));
+ body1.setQToFitTranslation(state, pos);
+ body2.setQToFitTranslation(state, pos);
+ body1.setUToFitLinearVelocity(state, vel);
+ body2.setUToFitLinearVelocity(state, vel);
+ }
+
+ // Calculate lots of quantities from the MobilizedBodies.
+
+ system.realize(state, Stage::Acceleration);
+ Vector_<SpatialVec> reactionForces;
+ matter.calcMobilizerReactionForces(state, reactionForces);
+ Vector_<SpatialVec> reactionForcesFreebody;
+ matter.calcMobilizerReactionForcesUsingFreebodyMethod(state, reactionForcesFreebody);
+
+ // Both methods should produce the same results.
+ SimTK_TEST_EQ(reactionForces, reactionForcesFreebody);
+
+
+ Vector mv, minvv;
+ matter.multiplyByM(state, state.getU(), mv);
+ matter.multiplyByMInv(state, state.getU(), minvv);
+ Vector appliedMobilityForces(matter.getNumMobilities());
+ Vector_<SpatialVec> appliedBodyForces(matter.getNumBodies());
+ for (int i = 0; i < numBodies; i++) {
+ Vec3 mobilityForce;
+ random.fillArray((Real*) &mobilityForce, 3);
+ Vec3::updAs(&appliedMobilityForces[6*i]) = mobilityForce;
+ Vec3::updAs(&appliedMobilityForces[6*i+3]) = mobilityForce;
+ SpatialVec bodyForce;
+ random.fillArray((Real*) &bodyForce, 6);
+ appliedBodyForces[2*i+1] = bodyForce;
+ appliedBodyForces[2*i+2] = bodyForce;
+ }
+ Vector knownUdot, residualMobilityForces;
+ matter.calcResidualForceIgnoringConstraints(state, appliedMobilityForces, appliedBodyForces, knownUdot, residualMobilityForces);
+ Vector dEdQ;
+ matter.multiplyBySystemJacobianTranspose(state, appliedBodyForces, dEdQ);
+ Array_<SpatialInertia,MobilizedBodyIndex> compositeInertias;
+ matter.calcCompositeBodyInertias(state, compositeInertias);
+
+ // See whether the RBNodeLoneParticles and the RBNodeTranslates produced identical results.
+
+ for (int i = 0; i < numBodies; i++) {
+ MobilizedBodyIndex index1(2*i+1);
+ MobilizedBodyIndex index2(2*i+2);
+ const MobilizedBody& body1 = matter.getMobilizedBody(index1);
+ const MobilizedBody& body2 = matter.getMobilizedBody(index2);
+ assertEqual(body1.getBodyOriginLocation(state), body2.getBodyOriginLocation(state));
+ assertEqual(body1.getBodyOriginVelocity(state), body2.getBodyOriginVelocity(state));
+ assertEqual(body1.getBodyOriginAcceleration(state), body2.getBodyOriginAcceleration(state));
+ assertEqual(reactionForces[index1][0], reactionForces[index2][0]);
+ assertEqual(reactionForces[index1][1], reactionForces[index2][1]);
+ assertEqual(Vec3::getAs(&mv[6*i]), Vec3::getAs(&mv[6*i+3]));
+ if (!prescribe)
+ assertEqual(Vec3::getAs(&minvv[6*i]), Vec3::getAs(&minvv[6*i+3]));
+ assertEqual(Vec3::getAs(&residualMobilityForces[6*i]), Vec3::getAs(&residualMobilityForces[6*i+3]));
+ assertEqual(Vec3::getAs(&dEdQ[6*i]), Vec3::getAs(&dEdQ[6*i+3]));
+ assertEqual(compositeInertias[index1].getMass(), compositeInertias[index2].getMass());
+ assertEqual(compositeInertias[index1].getMassCenter(), compositeInertias[index2].getMassCenter());
+ assertEqual(compositeInertias[index1].getUnitInertia().getMoments(), compositeInertias[index2].getUnitInertia().getMoments());
+ assertEqual(compositeInertias[index1].getUnitInertia().getProducts(), compositeInertias[index2].getUnitInertia().getProducts());
+ }
+}
+
+static void testFree() {
+ compareToTranslate(false, Motion::Position);
+}
+
+static void testPrescribePosition() {
+ compareToTranslate(true, Motion::Position);
+}
+
+static void testPrescribeVelocity() {
+ compareToTranslate(true, Motion::Velocity);
+}
+
+static void testPrescribeAcceleration() {
+ compareToTranslate(true, Motion::Acceleration);
+}
+
+int main() {
+ SimTK_START_TEST("TestLoneParticle");
+ SimTK_SUBTEST(testFree);
+ SimTK_SUBTEST(testPrescribePosition);
+ SimTK_SUBTEST(testPrescribeVelocity);
+ SimTK_SUBTEST(testPrescribeAcceleration);
+ SimTK_END_TEST();
+}
diff --git a/Simbody/tests/TestMassMatrix.cpp b/Simbody/tests/TestMassMatrix.cpp
new file mode 100644
index 0000000..13a999d
--- /dev/null
+++ b/Simbody/tests/TestMassMatrix.cpp
@@ -0,0 +1,1248 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+
+// Test the functioning of Simbody operators which involve the mass matrix,
+// and other system matrices like the Jacobian (partial velocity matrix) that
+// maps between generalized and spatial coordinates.
+// The O(N) operators like multiplyByM() and multiplyByMInv() are supposed to
+// behave *as though* they used the mass matrix, without actually forming it.
+
+#include "SimTKsimbody.h"
+#include "SimTKcommon/Testing.h"
+
+#include <iostream>
+
+using namespace SimTK;
+using std::cout; using std::endl;
+
+// This will apply a constant set of mobility forces that can be set
+// externally.
+class MyForceImpl : public Force::Custom::Implementation {
+public:
+ MyForceImpl() {}
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces,
+ Vector& mobilityForces) const
+ {
+ SimTK_TEST( f.size() == 0 || f.size() == mobilityForces.size() );
+ SimTK_TEST( F.size() == 0 || F.size() == bodyForces.size() );
+ if (f.size())
+ mobilityForces += f;
+ if (F.size())
+ bodyForces += F;
+ }
+ Real calcPotentialEnergy(const State&) const {return 0;}
+
+ void setMobilityForces(const Vector& mobFrc) {
+ f = mobFrc;
+ }
+ void setBodyForces(const Vector_<SpatialVec>& bodFrc) {
+ F = bodFrc;
+ }
+private:
+ Vector f;
+ Vector_<SpatialVec> F;
+};
+
+// This is an imitation of SD/FAST's sdrel2cart() subroutine. We
+// are given a station point S fixed to a body B. S is given by
+// the constant vector p_BS from B's origin Bo to point S, expressed
+// in B's frame. Denote the position of S in the ground frame G
+// p_GS = p_GB + p_BS_G, where p_BS_G=R_GB*p_BS is the vector p_BS
+// reexpressed in G. The velocity of S in G is v_GS = d/dt p_GS, taken
+// in G. So v_GS = v_GB + w_GB X p_BS_G = v_GB - p_BS_G % w_GB.
+//
+// We would like to obtain the partial velocity of S with respect to each of
+// the generalized speeds u, taken in the Ground frame, that is,
+// JS=d v_GS / du. (JS is a 3xnu matrix, or a single row of Vec3s.) We have
+// a method that can calculate J=d V_GB / du where V_GB=[w_GB;v_GB] is the
+// spatial velocity of B at its origin. So we need to calculate
+// d v_GS d v_GS d V_GB
+// JS = ------ = ------ * ------ =
+// du d V_GB du
+//
+// = [ -px | eye(3) ] * J
+// where px is the cross product matrix of p_BS_G and eye(3) is a 3x3
+// identity matrix.
+//
+// This function should produce the same result as the SimbodyMatterSubsystem
+// method calcStationJacobian().
+void sbrel2cart(const State& state,
+ const SimbodyMatterSubsystem& matter,
+ MobilizedBodyIndex bodyIx,
+ const Vec3& p_BS, // point in body frame
+ RowVector_<Vec3>& dvdu) // v is dS/dt in G
+{
+ const int nu = state.getNU();
+
+ const MobilizedBody& mobod = matter.getMobilizedBody(bodyIx);
+ const Vec3 p_BS_G = mobod.expressVectorInGroundFrame(state, p_BS);
+
+ // Calculate J=dVdu where V is spatial velocity of body origin.
+ Vector_<SpatialVec> J(nu);
+ J = SpatialVec(Vec3(0), Vec3(0)); // or J.setToZero();
+
+ Vector u(nu); u = 0;
+ Vector_<SpatialVec> Ju(nu); // d allV / d ui
+ for (int i=0; i < nu; ++i) {
+ u[i] = 1;
+ matter.multiplyBySystemJacobian(state,u,Ju);
+ u[i] = 0;
+ J[i] = Ju[bodyIx]; // pick out the body of interest
+ }
+
+ Row<2,Mat33> dvdV( -crossMat(p_BS_G), Mat33(1) );
+ dvdu.resize(nu);
+ for (int i=0; i < nu; ++i)
+ dvdu[i] = dvdV * J[i]; // or J[i][0] % p_BS_G + J[i][1]
+}
+
+// Another way to calculate exactly what sbrel2cart() does -- can we do it
+// faster using f=J^T*F rather than V=J*u?
+void sbrel2cart2(const State& state,
+ const SimbodyMatterSubsystem& matter,
+ MobilizedBodyIndex bodyIx,
+ const Vec3& p_BS, // point in body frame
+ RowVector_<Vec3>& dvdu) // v is dS/dt in G
+{
+ const int nu = state.getNU();
+ const int nb = matter.getNumBodies(); // includes ground
+
+ const MobilizedBody& mobod = matter.getMobilizedBody(bodyIx);
+ const Vec3 p_BS_G = mobod.expressVectorInGroundFrame(state, p_BS);
+
+ // Calculate J=dVdu where V is spatial velocity of body origin.
+ // (This is one row of J.)
+ Matrix Jt(nu, 6); // a column of Jt but with scalar elements
+
+ Vector_<SpatialVec> F(nb, SpatialVec(Vec3(0)));
+ SpatialVec& Fb = F[bodyIx]; // the only one we'll change
+ for (int which=0; which < 2; ++which) { // moment, force
+ for (int i=0; i < 3; ++i) {
+ Fb[which][i] = 1;
+ VectorView col = Jt(3*which + i);
+ matter.multiplyBySystemJacobianTranspose(state,F,col);
+ Fb[which][i] = 0;
+ }
+ }
+
+ Row<2,Mat33> dvdV( -crossMat(p_BS_G), Mat33(1) );
+ dvdu.resize(nu);
+ for (int i=0; i < nu; ++i) {
+ const RowVectorView r = Jt[i];
+ SpatialVec V(Vec3::getAs(&r[0]), Vec3::getAs(&r[3]));
+ dvdu[i] = dvdV * V; // or J[i][0] % p_BS_G + J[i][1]
+ }
+}
+
+// This is a further refinement that still calculates exactly what sbrel2cart()
+// does. But now try it without the intermediate storage for a row of J^T and
+// using only 3 J*v multiplies since only the translational (station) Jacobian
+// is wanted.
+void sbrel2cart3(const State& state,
+ const SimbodyMatterSubsystem& matter,
+ MobilizedBodyIndex bodyIx,
+ const Vec3& p_BS, // point in body frame
+ RowVector_<Vec3>& dvdu) // v is dS/dt in G
+{
+ const int nu = state.getNU();
+ const int nb = matter.getNumBodies(); // includes ground
+
+ const MobilizedBody& mobod = matter.getMobilizedBody(bodyIx);
+ const Vec3 p_BS_G = mobod.expressVectorInGroundFrame(state, p_BS);
+
+ // Calculate J=dvdu where v is linear velocity of p_BS.
+ // (This is three rows of J.)
+ dvdu.resize(nu);
+
+ Vector_<SpatialVec> F(nb, SpatialVec(Vec3(0)));
+ SpatialVec& Fb = F[bodyIx]; // the only one we'll change
+ Vector col(nu); // temporary to hold column of J^T
+ for (int i=0; i < 3; ++i) {
+ Fb[1][i] = 1;
+ Fb[0] = p_BS_G % Fb[1]; // r X F
+ matter.multiplyBySystemJacobianTranspose(state,F,col);
+ for (int r=0; r < nu; ++r) dvdu[r][i] = col[r];
+ Fb[1][i] = 0;
+ }
+}
+
+// Using the method of sbrel2cart3() but with 6 J*v multiplies, this gives
+// the full 6xnu "Frame Jacobian" for one body for a specified frame on that
+// body (only the origin, not the orientation, matters).
+// This should produce the same result as the built-in calcFrameJacobian()
+// method.
+void sbrel2cart4(const State& state,
+ const SimbodyMatterSubsystem& matter,
+ MobilizedBodyIndex bodyIx,
+ const Vec3& p_BS, // point in body frame
+ RowVector_<SpatialVec>& dVdu) // V is [w,v] in G
+{
+ const int nu = state.getNU();
+ const int nb = matter.getNumBodies(); // includes ground
+
+ const MobilizedBody& mobod = matter.getMobilizedBody(bodyIx);
+ const Vec3 p_BS_G = mobod.expressVectorInGroundFrame(state, p_BS);
+
+ // Calculate J=dVdu where V is spatial velocity of p_BS.
+ // (This is six rows of J.)
+ dVdu.resize(nu);
+
+ Vector_<SpatialVec> F(nb, SpatialVec(Vec3(0)));
+ SpatialVec& Fb = F[bodyIx]; // the only one we'll change
+ Vector col(nu); // temporary to hold column of J^T
+ // Rotational part.
+ for (int i=0; i < 3; ++i) {
+ Fb[0][i] = 1;
+ matter.multiplyBySystemJacobianTranspose(state,F,col);
+ for (int r=0; r < nu; ++r) dVdu[r][0][i] = col[r];
+ Fb[0][i] = 0;
+ }
+ // Translational part.
+ for (int i=0; i < 3; ++i) {
+ Fb[1][i] = 1;
+ Fb[0] = p_BS_G % Fb[1]; // r X F
+ matter.multiplyBySystemJacobianTranspose(state,F,col);
+ for (int r=0; r < nu; ++r) dVdu[r][1][i] = col[r];
+ Fb[1][i] = 0;
+ }
+}
+
+// Compare two representations of the same matrix: one as an mXn matrix
+// of SpatialVecs, the other as a 6mXn matrix of scalars. Note that this
+// will also work if the first actual parameter is a Vector_<SpatialVec>
+// or RowVector_<SpatialVec> since those have implicit conversions to mX1
+// or 1Xn Matrix_<SpatialVec>, resp.
+static void compareElementwise(const Matrix_<SpatialVec>& J,
+ const Matrix& Jf)
+{
+ const int m = J.nrow(), n = J.ncol();
+ SimTK_TEST(Jf.nrow()==6*m && Jf.ncol()==n);
+
+ for (int b=0; b<m; ++b) {
+ const int r = 6*b; // row start for Jf
+ for (int i=0; i<6; ++i)
+ for (int j=0; j<n; ++j) {
+ SimTK_TEST_EQ(J (b, j)[i/3][i%3],
+ Jf(r+i,j));
+ }
+ }
+}
+
+// Same thing but for comparing matrices where one has Vec3 elements.
+static void compareElementwise(const Matrix_<Vec3>& JS,
+ const Matrix& JSf)
+{
+ const int m = JS.nrow(), n = JS.ncol();
+ SimTK_TEST(JSf.nrow()==3*m && JSf.ncol()==n);
+
+ for (int b=0; b<m; ++b) {
+ const int r = 3*b; // row start for JSf
+ for (int i=0; i<3; ++i)
+ for (int j=0; j<n; ++j) {
+ SimTK_TEST_EQ(JS (b, j)[i],
+ JSf(r+i,j));
+ }
+ }
+}
+
+void testRel2Cart() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+
+ // Pendulum of length 1, initially along -x like this:
+ // B ----------- * O
+ // -1,0,0 0,0,0
+ // At q=0, partial(B)/partial(u) = 0,-1,0.
+ // At q=pi/2, partial(B)/partial(u) = 1,0,0.
+ // Then try this with station S=(0,1,0)_B:
+ // S
+ // |
+ // |
+ // B ------ * O
+ // Now |OS| = sqrt(2). At q=Pi/4, S will be horizontal
+ // so partial(S)/partial(u) = (0, -sqrt(2)/2, 0).
+ //
+ // In all cases the partial angular velocity is (0,0,1).
+ //
+ MobilizedBody::Pin pinBody
+ (matter.Ground(), Transform(),
+ MassProperties(1,Vec3(0),Inertia(1)), Vec3(1,0,0));
+ State s = system.realizeTopology();
+
+ RowVector_<Vec3> dvdu, dvdu2, dvdu3;
+ RowVector_<Vec3> JS;
+ RowVector_<SpatialVec> dvdu4, JF;
+ Matrix_<SpatialVec> J, Jn;
+ Matrix JSf, JFf, Jf; // flat
+
+ // We'll compute Jacobians for the body origin Bo in 2 configurations,
+ // q==0,pi/2 then for station S (0,1,0) at q==pi/4. Not
+ // much of a test, I know, but at least we know the right answer.
+
+ // q == 0; answer is JB == (0,-1,0)
+
+ pinBody.setQ(s, 0);
+ system.realize(s, Stage::Position);
+ sbrel2cart(s, matter, pinBody, Vec3(0), dvdu);
+ SimTK_TEST_EQ(dvdu[0], Vec3(0,-1,0));
+
+ sbrel2cart2(s, matter, pinBody, Vec3(0), dvdu2);
+ SimTK_TEST_EQ(dvdu2[0], Vec3(0,-1,0));
+
+ sbrel2cart3(s, matter, pinBody, Vec3(0), dvdu3);
+ SimTK_TEST_EQ(dvdu3[0], Vec3(0,-1,0));
+
+ matter.calcStationJacobian(s, pinBody, Vec3(0), JS);
+ matter.calcStationJacobian(s, pinBody, Vec3(0), JSf);
+ SimTK_TEST_EQ(JS, dvdu3); // == dvdu2 == dvdu
+ compareElementwise(JS, JSf);
+
+ sbrel2cart4(s, matter, pinBody, Vec3(0), dvdu4);
+ SimTK_TEST_EQ(dvdu4[0][0], Vec3(0,0,1));
+ SimTK_TEST_EQ(dvdu4[0][1], Vec3(0,-1,0));
+
+ matter.calcFrameJacobian(s, pinBody, Vec3(0), JF);
+ matter.calcFrameJacobian(s, pinBody, Vec3(0), JFf);
+ SimTK_TEST_EQ(dvdu4, JF);
+ compareElementwise(JF, JFf);
+
+ // Calculate the whole system Jacobian at q==0 in two different
+ // representations and make sure they are the same.
+ matter.calcSystemJacobian(s, J);
+ matter.calcSystemJacobian(s, Jf);
+ compareElementwise(J, Jf);
+
+ // q == 90 degrees; answer is JB == (1,0,0)
+
+ pinBody.setQ(s, Pi/2);
+ system.realize(s, Stage::Position);
+ sbrel2cart(s, matter, pinBody, Vec3(0), dvdu);
+ SimTK_TEST_EQ(dvdu[0], Vec3(1,0,0));
+
+ sbrel2cart2(s, matter, pinBody, Vec3(0), dvdu2);
+ SimTK_TEST_EQ(dvdu2[0], Vec3(1,0,0));
+
+ sbrel2cart3(s, matter, pinBody, Vec3(0), dvdu3);
+ SimTK_TEST_EQ(dvdu3[0], Vec3(1,0,0));
+
+ matter.calcStationJacobian(s, pinBody, Vec3(0), JS);
+ matter.calcStationJacobian(s, pinBody, Vec3(0), JSf);
+ SimTK_TEST_EQ(JS, dvdu3); // == dvdu2 == dvdu
+ compareElementwise(JS, JSf);
+
+ sbrel2cart4(s, matter, pinBody, Vec3(0), dvdu4);
+ SimTK_TEST_EQ(dvdu4[0][0], Vec3(0,0,1));
+ SimTK_TEST_EQ(dvdu4[0][1], Vec3(1,0,0));
+
+ matter.calcFrameJacobian(s, pinBody, Vec3(0), JF);
+ matter.calcFrameJacobian(s, pinBody, Vec3(0), JFf);
+ SimTK_TEST_EQ(dvdu4, JF);
+ compareElementwise(JF, JFf);
+
+ // Calculate the whole system Jacobian at q==pi/2 in two different
+ // representations and make sure they are the same.
+ matter.calcSystemJacobian(s, J);
+ matter.calcSystemJacobian(s, Jf);
+ compareElementwise(J, Jf);
+
+ // now station S, q == 45 degrees; answer is JS == (0,-sqrt(2),0)
+
+ pinBody.setQ(s, Pi/4);
+ system.realize(s, Stage::Position);
+ sbrel2cart(s, matter, pinBody, Vec3(0,1,0), dvdu);
+ SimTK_TEST_EQ(dvdu[0], Vec3(0,-Sqrt2,0));
+
+ sbrel2cart2(s, matter, pinBody, Vec3(0,1,0), dvdu2);
+ SimTK_TEST_EQ(dvdu2[0], Vec3(0,-Sqrt2,0));
+
+ sbrel2cart3(s, matter, pinBody, Vec3(0,1,0), dvdu3);
+ SimTK_TEST_EQ(dvdu3[0], Vec3(0,-Sqrt2,0));
+
+ matter.calcStationJacobian(s, pinBody, Vec3(0,1,0), JS);
+ matter.calcStationJacobian(s, pinBody, Vec3(0,1,0), JSf);
+ SimTK_TEST_EQ(JS, dvdu3); // == dvdu2 == dvdu
+ compareElementwise(JS, JSf);
+
+ // Calculate station Jacobian JS by multiplication to test that the
+ // multiplyByStationJacobian[Transpose] methods are working.
+ Vec3 JSn; // 3xnu
+ Vector u(1); u[0] = 1.;
+ JSn = matter.multiplyByStationJacobian(s, pinBody, Vec3(0,1,0), u);
+ SimTK_TEST_EQ(JSn, dvdu3[0]);
+
+ Vec3 FS(0);
+ Row3 JSnt;
+ for (int i=0; i<3; ++i) {
+ FS[i] = 1;
+ matter.multiplyByStationJacobianTranspose(s,pinBody,Vec3(0,1,0),
+ FS, u);
+ FS[i] = 0;
+ JSnt[i] = u[0];
+ }
+ SimTK_TEST_EQ(JSnt, ~dvdu3[0]);
+
+ sbrel2cart4(s, matter, pinBody, Vec3(0,1,0), dvdu4);
+ SimTK_TEST_EQ(dvdu4[0][0], Vec3(0,0,1));
+ SimTK_TEST_EQ(dvdu4[0][1], Vec3(0,-Sqrt2,0));
+
+ matter.calcFrameJacobian(s, pinBody, Vec3(0,1,0), JF);
+ matter.calcFrameJacobian(s, pinBody, Vec3(0,1,0), JFf);
+ SimTK_TEST_EQ(dvdu4, JF);
+ compareElementwise(JF, JFf);
+
+ // Calculate frame Jacobian JF by multiplication to test that the
+ // multiplyByFrameJacobian[Transpose] methods are working.
+ SpatialVec JFn; // 6xnu
+ u[0] = 1.;
+ JFn = matter.multiplyByFrameJacobian(s, pinBody, Vec3(0,1,0), u);
+ SimTK_TEST_EQ(JFn, dvdu4[0]);
+
+ SpatialVec FF(Vec3(0));
+ SpatialRow JFnt;
+ for (int i=0; i<6; ++i) {
+ FF[i/3][i%3] = 1;
+ matter.multiplyByFrameJacobianTranspose(s,pinBody,Vec3(0,1,0),
+ FF, u);
+ FF[i/3][i%3] = 0;
+ JFnt[i/3][i%3] = u[0];
+ }
+ SimTK_TEST_EQ(JFnt, ~dvdu4[0]);
+
+ // Calculate the whole system Jacobian at q==pi/4 in two different
+ // representations and make sure they are the same.
+ matter.calcSystemJacobian(s, J);
+ matter.calcSystemJacobian(s, Jf);
+ compareElementwise(J, Jf);
+
+ // Generate the whole system Jacobian one body at a time by multiplication.
+ Jn.resize(matter.getNumBodies(), matter.getNumMobilities());
+ u[0] = 1;
+ for (MobodIndex i(0); i < matter.getNumBodies(); ++i)
+ Jn[i] = matter.multiplyByFrameJacobian(s,i,Vec3(0),u);
+ SimTK_TEST_EQ(Jn, J);
+}
+
+void makeSystem(bool constrained, MultibodySystem& mbs, MyForceImpl*& frcp) {
+ SimbodyMatterSubsystem pend(mbs);
+ GeneralForceSubsystem forces(mbs);
+ frcp = new MyForceImpl();
+ Force::Custom(forces, frcp);
+
+ const Real randomAngle1 = (Pi/2)*Test::randReal();
+ const Real randomAngle2 = (Pi/2)*Test::randReal();
+ Vector_<Vec3> randomVecs(10);
+ for (int i=0; i<10; ++i)
+ randomVecs[i] = Test::randVec3();
+
+ const Real mass = 2.3;
+ const Vec3 com = randomVecs[5];
+ const Inertia inertia = Inertia(3,4,5,.01,-.02,.04).shiftFromMassCenter(com, mass);
+ Body::Rigid pendulumBody = Body::Rigid(
+ MassProperties(mass, com, inertia));
+
+
+ MobilizedBody::Ball
+ pendBody1( pend.Ground(),
+ Transform(Rotation(randomAngle1, randomVecs[0]),
+ randomVecs[1]),
+ pendulumBody,
+ Transform(Rotation(randomAngle2, randomVecs[2]),
+ randomVecs[3]));
+ MobilizedBody::Weld
+ pendBody2( pendBody1,
+ Transform(Rotation(randomAngle1, randomVecs[4]),
+ randomVecs[5]),
+ pendulumBody,
+ Transform(Rotation(randomAngle2, randomVecs[6]),
+ randomVecs[7]));
+
+ MobilizedBody::Pin
+ pendBody3( pendBody2,
+ Transform(Rotation(randomAngle1, randomVecs[8]),
+ randomVecs[9]),
+ pendulumBody,
+ Transform(Rotation(randomAngle2, randomVecs[8]),
+ randomVecs[7]));
+ MobilizedBody::Screw
+ pendBody4( pendBody3,
+ Transform(Rotation(randomAngle2, randomVecs[6]),
+ randomVecs[5]),
+ pendulumBody,
+ Transform(Rotation(randomAngle1, randomVecs[4]),
+ randomVecs[3]),
+ 3); // pitch
+ MobilizedBody::Translation
+ pendBody5( pendBody4,
+ Transform(Rotation(randomAngle2, randomVecs[2]),
+ randomVecs[1]),
+ pendulumBody,
+ Transform(Rotation(randomAngle1, randomVecs[0]),
+ randomVecs[1]));
+
+ // Now add some side branches.
+ MobilizedBody::BendStretch
+ pendBody1a( pendBody1,
+ Transform(Rotation(randomAngle2, randomVecs[2]),
+ randomVecs[3]),
+ pendulumBody,
+ Transform(Rotation(randomAngle1, randomVecs[4]),
+ randomVecs[5]));
+
+ MobilizedBody::Slider
+ pendBody2a( pendBody2,
+ Transform(Rotation(randomAngle1, randomVecs[6]),
+ randomVecs[7]),
+ pendulumBody,
+ Transform(Rotation(randomAngle2, randomVecs[8]),
+ randomVecs[9]));
+
+ MobilizedBody::Universal
+ pendBody2b( pendBody2a,
+ Transform(Rotation(randomAngle1, randomVecs[8]),
+ randomVecs[7]),
+ pendulumBody,
+ Transform(Rotation(randomAngle2, randomVecs[6]),
+ randomVecs[5]));
+ MobilizedBody::Slider
+ pendBody2x( pendBody2b,
+ Transform(Rotation(randomAngle1, randomVecs[6]),
+ randomVecs[7]),
+ pendulumBody,
+ Transform(Rotation(randomAngle2, randomVecs[8]),
+ randomVecs[9]));
+
+ MobilizedBody::Planar
+ pendBody4a( pendBody4,
+ Transform(Rotation(randomAngle1, randomVecs[4]),
+ randomVecs[3]),
+ pendulumBody,
+ Transform(Rotation(randomAngle2, randomVecs[2]),
+ randomVecs[1]));
+
+
+ if (constrained) { // probably can't be satisfied, but doesn't matter
+ Constraint::Rod(pendBody4, pendBody2b, 1.); // holonomic
+ Constraint::ConstantSpeed
+ (pendBody2a, MobilizerUIndex(0), -3.); // nonholo
+ Constraint::ConstantAcceleration
+ (pendBody5, MobilizerUIndex(2), 0.01); // acc only
+ Constraint::Weld(pendBody4a, Test::randTransform(),
+ pendBody4, Test::randTransform());
+ }
+}
+
+
+// Test calculations of Jacobian "bias" terms, where bias=JDot*u.
+// We can estimate JDot using a numerical directional derivative
+// since JDot = (DJ/Dq)*qdot ~= (J(q+h*qdot)-J(q-h*qdot))/2h.
+// Then we multiply JDot*u and compare with the bias calculations.
+// Or, we can estimate JDot*u directly with
+// JDotu ~= (J(q+h*qdot)*u - J(q-h*qdot)*u)/2h
+// using the fast "multiply by Jacobian" methods.
+// We use both methods below.
+void testJacobianBiasTerms() {
+ MultibodySystem system;
+ MyForceImpl* frcp;
+ makeSystem(false, system, frcp);
+ const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
+
+ State state = system.realizeTopology();
+ const int nq = state.getNQ();
+ const int nu = state.getNU();
+ const int nb = matter.getNumBodies();
+
+ system.realizeModel(state);
+ // Randomize state.
+ state.updQ() = Test::randVector(nq);
+ state.updU() = Test::randVector(nu);
+
+
+ const MobilizedBodyIndex whichBod(8);
+ const Vec3 whichPt(1,2,3);
+ system.realize(state, Stage::Velocity);
+ const Vector& q = state.getQ();
+ const Vector& u = state.getU();
+ const Vector& qdot = state.getQDot();
+
+ // sbias, fbias, sysbias are the JDot*u quantities we want to check.
+ const Vec3 sbias =
+ matter.calcBiasForStationJacobian(state, whichBod, whichPt);
+ const SpatialVec fbias =
+ matter.calcBiasForFrameJacobian(state, whichBod, whichPt);
+ Vector_<SpatialVec> sysbias;
+ matter.calcBiasForSystemJacobian(state, sysbias);
+
+ // These are for computing JDot first.
+ RowVector_<Vec3> JS_P, JS1_P, JS2_P, JSDot_P;
+ RowVector_<SpatialVec> JF_P, JF1_P, JF2_P, JFDot_P;
+ Matrix_<SpatialVec> J, J1, J2, JDot;
+
+ // These are for computing JDot*u directly.
+ Vec3 JS_Pu, JS1_Pu, JS2_Pu, JSDot_Pu;
+ SpatialVec JF_Pu, JF1_Pu, JF2_Pu, JFDot_Pu;
+ Vector_<SpatialVec> Ju, J1u, J2u, JDotu;
+
+ // Unperturbed:
+ matter.calcStationJacobian(state,whichBod,whichPt, JS_P);
+ matter.calcFrameJacobian(state,whichBod,whichPt, JF_P);
+ matter.calcSystemJacobian(state, J);
+
+ JS_Pu = matter.multiplyByStationJacobian(state,whichBod,whichPt,u);
+ JF_Pu = matter.multiplyByFrameJacobian(state,whichBod,whichPt,u);
+ matter.multiplyBySystemJacobian(state, u, Ju);
+
+ const Real Delta = 5e-6; // we'll use central difference
+ State perturbq = state;
+ // Perturbed +:
+ perturbq.updQ() = q + Delta*qdot;
+ system.realize(perturbq, Stage::Position);
+ matter.calcStationJacobian(perturbq,whichBod,whichPt, JS2_P);
+ matter.calcFrameJacobian(perturbq,whichBod,whichPt, JF2_P);
+ matter.calcSystemJacobian(perturbq, J2);
+ JS2_Pu = matter.multiplyByStationJacobian(perturbq,whichBod,whichPt,u);
+ JF2_Pu = matter.multiplyByFrameJacobian(perturbq,whichBod,whichPt,u);
+ matter.multiplyBySystemJacobian(perturbq,u, J2u);
+
+ // Perturbed -:
+ perturbq.updQ() = q - Delta*qdot;
+ system.realize(perturbq, Stage::Position);
+ matter.calcStationJacobian(perturbq,whichBod,whichPt, JS1_P);
+ matter.calcFrameJacobian(perturbq,whichBod,whichPt, JF1_P);
+ matter.calcSystemJacobian(perturbq, J1);
+ JS1_Pu = matter.multiplyByStationJacobian(perturbq,whichBod,whichPt,u);
+ JF1_Pu = matter.multiplyByFrameJacobian(perturbq,whichBod,whichPt,u);
+ matter.multiplyBySystemJacobian(perturbq,u, J1u);
+
+ // Estimate JDots:
+ JSDot_P = (JS2_P-JS1_P)/Delta/2;
+ JFDot_P = (JF2_P-JF1_P)/Delta/2;
+ JDot = (J2-J1)/Delta/2;
+
+ // Estimate JDotus:
+ JSDot_Pu = (JS2_Pu-JS1_Pu)/Delta/2;
+ JFDot_Pu = (JF2_Pu-JF1_Pu)/Delta/2;
+ JDotu = (J2u-J1u)/Delta/2;
+
+ // Calculate errors in JDot*u:
+ SimTK_TEST_EQ_TOL((JSDot_P*u-sbias).norm(), 0, SqrtEps);
+ SimTK_TEST_EQ_TOL((JFDot_P*u-fbias).norm(), 0, SqrtEps);
+ SimTK_TEST_EQ_TOL((JDot*u-sysbias).norm(), 0, SqrtEps);
+
+ // Calculate errors in JDotu:
+ SimTK_TEST_EQ_TOL((JSDot_Pu-sbias).norm(), 0, SqrtEps);
+ SimTK_TEST_EQ_TOL((JFDot_Pu-fbias).norm(), 0, SqrtEps);
+ SimTK_TEST_EQ_TOL((JDotu-sysbias).norm(), 0, SqrtEps);
+}
+
+void testUnconstrainedSystem() {
+ MultibodySystem system;
+ MyForceImpl* frcp;
+ makeSystem(false, system, frcp);
+ const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
+
+ State state = system.realizeTopology();
+ const int nq = state.getNQ();
+ const int nu = state.getNU();
+ const int nb = matter.getNumBodies();
+
+ // Attainable accuracy drops with problem size.
+ const Real Slop = nu*SignificantReal;
+
+ system.realizeModel(state);
+ // Randomize state.
+ state.updQ() = Test::randVector(nq);
+ state.updU() = Test::randVector(nu);
+
+ Vector randVec = 100*Test::randVector(nu);
+ Vector result1, result2;
+
+ // result1 = M*v
+ system.realize(state, Stage::Position);
+ matter.multiplyByM(state, randVec, result1);
+ SimTK_TEST_EQ(result1.size(), nu);
+
+ // result2 = M^-1 * result1 == M^-1 * M * v == v
+ system.realize(state, Stage::Dynamics);
+ matter.multiplyByMInv(state, result1, result2);
+ SimTK_TEST_EQ(result2.size(), nu);
+
+ SimTK_TEST_EQ_TOL(result2, randVec, Slop);
+
+ Matrix M(nu,nu), MInv(nu,nu);
+
+ Vector v(nu, Real(0));
+ for (int j=0; j < nu; ++j) {
+ v[j] = 1;
+ matter.multiplyByM(state, v, M(j));
+ matter.multiplyByMInv(state, v, MInv(j));
+ v[j] = 0;
+ }
+
+ Matrix MInvCalc(M);
+ MInvCalc.invertInPlace();
+ SimTK_TEST_EQ_SIZE(MInv, MInvCalc, nu);
+
+ Matrix identity(nu,nu); identity=1;
+ SimTK_TEST_EQ_SIZE(M*MInv, identity, nu);
+ SimTK_TEST_EQ_SIZE(MInv*M, identity, nu);
+
+ // Compare above-calculated values with values returned by the
+ // calcM() and calcMInv() methods.
+ Matrix MM, MMInv;
+ matter.calcM(state,MM); matter.calcMInv(state,MMInv);
+ SimTK_TEST_EQ_SIZE(MM, M, nu);
+ SimTK_TEST_EQ_SIZE(MMInv, MInv, nu);
+
+ //assertIsIdentity(eye);
+ //assertIsIdentity(MInv*M);
+
+ frcp->setMobilityForces(randVec);
+ //cout << "f=" << randVec << endl;
+ system.realize(state, Stage::Acceleration);
+ Vector accel = state.getUDot();
+ //cout << "v!=0, accel=" << accel << endl;
+
+ matter.multiplyByMInv(state, randVec, result1);
+ //cout << "With velocities, |a - M^-1*f|=" << (accel-result1).norm() << endl;
+
+ SimTK_TEST_NOTEQ(accel, result1); // because of the velocities
+ //SimTK_TEST((accel-result1).norm() > SignificantReal); // because of velocities
+
+ // With no velocities M^-1*f should match calculated acceleration.
+ state.updU() = 0;
+ system.realize(state, Stage::Acceleration);
+ accel = state.getUDot();
+ //cout << "v=0, accel=" << accel << endl;
+
+ //cout << "With v=0, |a - M^-1*f|=" << (accel-result1).norm() << endl;
+
+ SimTK_TEST_EQ(accel, result1); // because no velocities
+
+ // And then M*a should = f.
+ matter.multiplyByM(state, accel, result2);
+ //cout << "v=0, M*accel=" << result2 << endl;
+ //cout << "v=0, |M*accel-f|=" << (result2-randVec).norm() << endl;
+
+
+ // Test forward and inverse dynamics operators.
+ // Apply random forces and a random prescribed acceleration to
+ // get back the residual generalized forces. Then applying those
+ // should result in zero residual, and applying them.
+
+ // Randomize state.
+ state.updQ() = Test::randVector(nq);
+ state.updU() = Test::randVector(nu);
+
+
+ // Inverse dynamics should require realization only to Velocity stage.
+ system.realize(state, Stage::Velocity);
+
+ // Randomize body forces.
+ Vector_<SpatialVec> bodyForces(nb);
+ for (int i=0; i < nb; ++i)
+ bodyForces[i] = Test::randSpatialVec();
+
+ // Random mobility forces and known udots.
+ Vector mobilityForces = Test::randVector(nu);
+ Vector knownUdots = Test::randVector(nu);
+
+ // Check self consistency: compute residual, apply it, should be no remaining residual.
+ Vector residualForces, shouldBeZeroResidualForces;
+ matter.calcResidualForceIgnoringConstraints(state,
+ mobilityForces, bodyForces, knownUdots, residualForces);
+ matter.calcResidualForceIgnoringConstraints(state,
+ mobilityForces+residualForces, bodyForces, knownUdots, shouldBeZeroResidualForces);
+
+ SimTK_TEST(shouldBeZeroResidualForces.norm() <= Slop);
+
+ // Now apply these forces in forward dynamics and see if we get the desired
+ // acceleration. State must be realized to Dynamics stage.
+ system.realize(state, Stage::Dynamics);
+ Vector udots;
+ Vector_<SpatialVec> bodyAccels;
+ matter.calcAccelerationIgnoringConstraints(state,
+ mobilityForces+residualForces, bodyForces, udots, bodyAccels);
+
+ SimTK_TEST_EQ_TOL(udots, knownUdots, Slop);
+
+ // See if we get back the same body accelerations by feeding in
+ // these udots.
+ Vector_<SpatialVec> A_GB, AC_GB;
+ matter.calcBodyAccelerationFromUDot(state, udots, A_GB);
+ SimTK_TEST_EQ_TOL(A_GB, bodyAccels, Slop);
+
+ // Collect coriolis accelerations.
+ AC_GB.resize(matter.getNumBodies());
+ for (MobodIndex i(0); i<nb; ++i)
+ AC_GB[i] = matter.getTotalCoriolisAcceleration(state, i);
+
+ // Verify that either a zero-length or all-zero udot gives just
+ // coriolis accelerations.
+ matter.calcBodyAccelerationFromUDot(state, Vector(), A_GB);
+ SimTK_TEST_EQ_TOL(A_GB, AC_GB, Slop);
+
+ Vector allZeroUdot(matter.getNumMobilities(), Real(0));
+ matter.calcBodyAccelerationFromUDot(state, allZeroUdot, A_GB);
+ SimTK_TEST_EQ_TOL(A_GB, AC_GB, Slop);
+
+ // Now let's test noncontiguous input and output vectors.
+ Matrix MatUdot(3, nu); // use middle row
+ MatUdot.setToNaN();
+ MatUdot[1] = ~udots;
+ Matrix_<SpatialRow> MatA_GB(3, nb); // use middle row
+ MatA_GB.setToNaN();
+ matter.calcBodyAccelerationFromUDot(state, ~MatUdot[1], ~MatA_GB[1]);
+ SimTK_TEST_EQ_TOL(MatA_GB[1], ~bodyAccels, Slop);
+
+ // Verify that leaving out arguments makes them act like zeroes.
+ Vector residualForces1, residualForces2;
+ matter.calcResidualForceIgnoringConstraints(state,
+ 0*mobilityForces, 0*bodyForces, 0*knownUdots, residualForces1);
+ // no, the residual is not zero here because of the angular velocities
+ matter.calcResidualForceIgnoringConstraints(state,
+ Vector(), Vector_<SpatialVec>(), Vector(), residualForces2);
+
+ SimTK_TEST_EQ_TOL(residualForces2, residualForces1, Slop);
+
+ // We just calculated f_residual = M udot + f_inertial - f_applied, with
+ // both udot and f_applied zero, i.e. f_residual=f_inertial. That should
+ // be the same as what is returned by getTotalCentrifugalForces().
+ Vector_<SpatialVec> F_inertial(nb);
+ Vector f_inertial;
+ for (MobodIndex i(0); i<nb; ++i)
+ F_inertial[i] = matter.getTotalCentrifugalForces(state, i);
+ matter.multiplyBySystemJacobianTranspose(state, F_inertial, f_inertial);
+ SimTK_TEST_EQ_TOL(f_inertial, residualForces1, Slop);
+
+ // This should also match total Mass*Coriolis acceleration + gyro force.
+ Vector_<SpatialVec> F_coriolis(nb), F_gyro(nb), F_total(nb);
+ Vector f_total;
+ for (MobodIndex i(0); i<nb; ++i) {
+ if (i==0) F_coriolis[i] = SpatialVec(Vec3(0),Vec3(0));
+ else
+ F_coriolis[i] = matter.getMobilizedBody(i)
+ .getBodySpatialInertiaInGround(state) * AC_GB[i];
+ F_gyro[i] = matter.getGyroscopicForce(state, i);
+ }
+
+ F_total = F_coriolis + F_gyro;
+ SimTK_TEST_EQ_TOL(F_inertial, F_total, Slop);
+
+ // Same, but leave out combinations of arguments.
+ matter.calcResidualForceIgnoringConstraints(state,
+ 0*mobilityForces, bodyForces, knownUdots, residualForces1);
+ matter.calcResidualForceIgnoringConstraints(state,
+ Vector(), bodyForces, knownUdots, residualForces2);
+ SimTK_TEST_EQ_TOL(residualForces2, residualForces1, Slop);
+ matter.calcResidualForceIgnoringConstraints(state,
+ mobilityForces, 0*bodyForces, knownUdots, residualForces1);
+ matter.calcResidualForceIgnoringConstraints(state,
+ mobilityForces, Vector_<SpatialVec>(), knownUdots, residualForces2);
+ SimTK_TEST_EQ_TOL(residualForces2, residualForces1, Slop);
+ matter.calcResidualForceIgnoringConstraints(state,
+ mobilityForces, bodyForces, 0*knownUdots, residualForces1);
+ matter.calcResidualForceIgnoringConstraints(state,
+ mobilityForces, bodyForces, Vector(), residualForces2);
+ SimTK_TEST_EQ_TOL(residualForces2, residualForces1, Slop);
+ matter.calcResidualForceIgnoringConstraints(state,
+ 0*mobilityForces, bodyForces, 0*knownUdots, residualForces1);
+ matter.calcResidualForceIgnoringConstraints(state,
+ Vector(), bodyForces, Vector(), residualForces2);
+ SimTK_TEST_EQ_TOL(residualForces2, residualForces1, Slop);
+ matter.calcResidualForceIgnoringConstraints(state,
+ mobilityForces, 0*bodyForces, 0*knownUdots, residualForces1);
+ matter.calcResidualForceIgnoringConstraints(state,
+ mobilityForces, Vector_<SpatialVec>(), Vector(), residualForces2);
+ SimTK_TEST_EQ_TOL(residualForces2, residualForces1, Slop);
+
+ // Check that we object to wrong-length arguments.
+ SimTK_TEST_MUST_THROW(matter.calcResidualForceIgnoringConstraints(state,
+ Vector(3,Zero), bodyForces, knownUdots, residualForces2));
+ SimTK_TEST_MUST_THROW(matter.calcResidualForceIgnoringConstraints(state,
+ mobilityForces, Vector_<SpatialVec>(5), knownUdots, residualForces2));
+ SimTK_TEST_MUST_THROW(matter.calcResidualForceIgnoringConstraints(state,
+ mobilityForces, bodyForces, Vector(2), residualForces2));
+
+}
+
+
+
+void testConstrainedSystem() {
+ MultibodySystem mbs;
+ MyForceImpl* frcp;
+ makeSystem(true, mbs, frcp);
+ const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
+
+ State state = mbs.realizeTopology();
+ mbs.realize(state, Stage::Instance); // allocate multipliers, etc.
+
+ const int nq = state.getNQ();
+ const int nu = state.getNU();
+ const int m = state.getNMultipliers();
+ const int nb = matter.getNumBodies();
+
+ // Attainable accuracy drops with problem size.
+ const Real Slop = nu*SignificantReal;
+
+ mbs.realizeModel(state);
+ // Randomize state.
+ state.updQ() = Test::randVector(nq);
+ state.updU() = Test::randVector(nu);
+
+ Vector randMobFrc = 100*Test::randVector(nu);
+ Vector_<SpatialVec> randBodyFrc(nb);
+ for (int i=0; i < nb; ++i)
+ randBodyFrc[i] = Test::randSpatialVec();
+
+ // Apply random mobility forces
+ frcp->setMobilityForces(randMobFrc);
+
+ mbs.realize(state); // calculate accelerations and multipliers
+ Vector udot = state.getUDot();
+ Vector lambda = state.getMultipliers();
+ Vector residual;
+ matter.calcResidualForce(state,randMobFrc,Vector_<SpatialVec>(),
+ udot, lambda, residual);
+
+ // Residual should be zero since we accounted for everything.
+ SimTK_TEST_EQ_TOL(residual, 0*randMobFrc, Slop);
+
+ Vector abias, mgbias;
+ // These are the acceleration error bias terms.
+ matter.calcBiasForAccelerationConstraints(state, abias);
+ // These use pverr (velocity-level errors) for holonomic constraints.
+ matter.calcBiasForMultiplyByG(state, mgbias);
+
+ Vector mgGudot; matter.multiplyByG(state, udot, mgbias, mgGudot);
+ Matrix G; matter.calcG(state, G);
+ Vector Gudot = G*udot;
+ SimTK_TEST_EQ_TOL(mgGudot, Gudot, Slop);
+ Vector aerr = state.getUDotErr(); // won't be zero because bad constraints
+ Vector GudotPlusBias = Gudot + abias;
+ SimTK_TEST_EQ_TOL(GudotPlusBias, aerr, Slop);
+
+ // Add in some body forces
+ state.invalidateAllCacheAtOrAbove(Stage::Dynamics);
+ frcp->setBodyForces(randBodyFrc);
+ mbs.realize(state);
+ udot = state.getUDot();
+ lambda = state.getMultipliers();
+ matter.calcResidualForce(state,randMobFrc,randBodyFrc,
+ udot, lambda, residual);
+ SimTK_TEST_EQ_TOL(residual, 0*randMobFrc, Slop);
+
+ // Try body forces only.
+ state.invalidateAllCacheAtOrAbove(Stage::Dynamics);
+ frcp->setMobilityForces(0*randMobFrc);
+ mbs.realize(state);
+ udot = state.getUDot();
+ lambda = state.getMultipliers();
+ matter.calcResidualForce(state,Vector(),randBodyFrc,
+ udot, lambda, residual);
+ SimTK_TEST_EQ_TOL(residual, 0*randMobFrc, Slop);
+
+ // Put vectors in noncontiguous storage.
+ Matrix udotmat(3,nu); // rows are noncontig
+ Matrix mobFrcMat(11,nu);
+ Matrix lambdamat(5,m);
+ Matrix_<SpatialRow> bodyFrcMat(3,nb);
+ udotmat[2] = ~udot;
+ lambdamat[3] = ~lambda;
+ mobFrcMat[8] = ~randMobFrc;
+ bodyFrcMat[2] = ~randBodyFrc;
+ Matrix residmat(4,nu);
+
+ // We last computed udot,lambda with no mobility forces. This time
+ // will throw some in and then make sure the residual tries to cancel them.
+ matter.calcResidualForce(state,~mobFrcMat[8],~bodyFrcMat[2],
+ ~udotmat[2],~lambdamat[3],~residmat[2]);
+ SimTK_TEST_EQ_TOL(residmat[2], -1*mobFrcMat[8], Slop);
+}
+
+
+
+void testCompositeInertia() {
+ MultibodySystem mbs;
+ SimbodyMatterSubsystem pend(mbs);
+
+ Body::Rigid pointMass(MassProperties(3, Vec3(0), Inertia(0)));
+
+ // Point mass at x=1.5 rotating about (0,0,0).
+ MobilizedBody::Pin
+ body1( pend.Ground(), Transform(),
+ pointMass, Vec3(1.5,0,0));
+ const MobilizedBodyIndex body1x = body1.getMobilizedBodyIndex();
+
+ // A second body 2 units further along x, rotating about the
+ // first point mass origin.
+ MobilizedBody::Pin
+ body2( body1, Transform(),
+ pointMass, Vec3(2,0,0));
+ const MobilizedBodyIndex body2x = body2.getMobilizedBodyIndex();
+
+ State state = mbs.realizeTopology();
+ mbs.realize(state, Stage::Position);
+
+ Array_<SpatialInertia, MobilizedBodyIndex> R(pend.getNumBodies());
+ pend.calcCompositeBodyInertias(state, R);
+
+ // Calculate expected inertias about the joint axes.
+ Real expInertia2 = body2.getBodyMassProperties(state).getMass()*square(2);
+ Real expInertia1 = body1.getBodyMassProperties(state).getMass()*square(1.5)
+ + body2.getBodyMassProperties(state).getMass()*square(3.5);
+
+ // Should be able to recover these inertias by projecting the composite
+ // body inertias onto the joint axes using H matrices.
+ const SpatialVec H1 = body1.getHCol(state, MobilizerUIndex(0));
+ const SpatialVec H2 = body2.getHCol(state, MobilizerUIndex(0));
+ SimTK_TEST_EQ(~H2*(R[body2x]*H2), expInertia2);
+ SimTK_TEST_EQ(~H1*(R[body1x]*H1), expInertia1);
+
+ // This should force realization of the composite body inertias.
+ SpatialInertia cbi = pend.getCompositeBodyInertia(state, body1);
+
+ body2.setAngle(state, Pi/4);
+ // This is not allowed until Position stage.
+ SimTK_TEST_MUST_THROW(pend.getCompositeBodyInertia(state, body1));
+ mbs.realize(state, Stage::Position);
+ // Now it should be OK.
+ cbi = pend.getCompositeBodyInertia(state, body1);
+
+ mbs.realize(state, Stage::Acceleration);
+ //cout << "udots=" << state.getUDot() << endl;
+
+ body1.setRate(state, 27);
+ mbs.realize(state, Stage::Acceleration);
+ //cout << "udots=" << state.getUDot() << endl;
+}
+
+void testTaskJacobians() {
+ MultibodySystem system;
+ MyForceImpl* frcp;
+ makeSystem(false, system, frcp);
+ const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
+
+ State state = system.realizeTopology();
+ const int nq = state.getNQ();
+ const int nu = state.getNU();
+ const int nb = matter.getNumBodies();
+
+ // Attainable accuracy drops with problem size.
+ const Real Slop = nu*SignificantReal;
+
+ system.realizeModel(state);
+ // Randomize state.
+ state.updQ() = Test::randVector(nq);
+ state.updU() = Test::randVector(nu);
+
+ system.realize(state, Stage::Position);
+
+ Matrix_<SpatialVec> J;
+ Matrix Jmat, Jmat2;
+ matter.calcSystemJacobian(state, J);
+ SimTK_TEST_EQ(J.nrow(), nb); SimTK_TEST_EQ(J.ncol(), nu);
+ matter.calcSystemJacobian(state, Jmat);
+ SimTK_TEST_EQ(Jmat.nrow(), 6*nb); SimTK_TEST_EQ(Jmat.ncol(), nu);
+
+ // Unpack J into Jmat2 and compare with Jmat.
+ Jmat2.resize(6*nb, nu);
+ for (int row=0; row < nb; ++row) {
+ const int nxtr = 6*row; // row index into scalar matrix
+ for (int col=0; col < nu; ++col) {
+ for (int k=0; k<3; ++k) {
+ Jmat2(nxtr+k, col) = J(row,col)[0][k];
+ Jmat2(nxtr+3+k, col) = J(row,col)[1][k];
+ }
+ }
+ }
+ // These should be exactly the same.
+ SimTK_TEST_EQ_TOL(Jmat2, Jmat, SignificantReal);
+
+ Vector randU = 100.*Test::randVector(nu), resultU1, resultU2;
+ Vector_<SpatialVec> randF(nb), resultF1, resultF2;
+ for (int i=0; i<nb; ++i) randF[i] = 100.*Test::randSpatialVec();
+
+ matter.multiplyBySystemJacobian(state, randU, resultF1);
+ resultF2 = J*randU;
+ SimTK_TEST_EQ_TOL(resultF1, resultF2, Slop);
+
+ matter.multiplyBySystemJacobianTranspose(state, randF, resultU1);
+ resultU2 = ~J*randF;
+ SimTK_TEST_EQ_TOL(resultU1, resultU2, Slop);
+
+ // See if Station Jacobian can be used to duplicate the translation
+ // rows of the System Jacobian, and if Frame Jacobian can be used to
+ // duplicate the whole thing.
+ Array_<MobilizedBodyIndex> allBodies(nb);
+ for (int i=0; i<nb; ++i) allBodies[i]=MobilizedBodyIndex(i);
+ Array_<Vec3> allOrigins(nb, Vec3(0));
+
+ Matrix_<Vec3> JS, JS2, JSbyrow;
+ Matrix_<SpatialVec> JF, JF2, JFbyrow;
+
+ matter.calcStationJacobian(state, allBodies, allOrigins, JS);
+ matter.calcFrameJacobian(state, allBodies, allOrigins, JF);
+ for (int i=0; i<nb; ++i) {
+ for (int j=0; j<nu; ++j) {
+ SimTK_TEST_EQ(JS(i,j), J(i,j)[1]);
+ SimTK_TEST_EQ(JF(i,j), J(i,j));
+ }
+ }
+
+ // Now use random stations to calculate JS & JF.
+ Array_<Vec3> randS(nb);
+ for (int i=0; i<nb; ++i) randS[i] = 10.*Test::randVec3();
+ matter.calcStationJacobian(state, allBodies, randS, JS);
+ matter.calcFrameJacobian(state, allBodies, randS, JF);
+
+ // Recalculate one row at a time to test non-contiguous memory handling.
+ // Do it backwards just to show off.
+ JSbyrow.resize(nb, nu); JFbyrow.resize(nb, nu);
+ for (int i=nb-1; i >= 0; --i) {
+ matter.calcStationJacobian(state, allBodies[i], randS[i], JSbyrow[i]);
+ matter.calcFrameJacobian(state, allBodies[i], randS[i], JFbyrow[i]);
+ }
+ SimTK_TEST_EQ(JS, JSbyrow);
+ SimTK_TEST_EQ(JF, JFbyrow);
+
+ // Calculate JS2=JS and JF2=JF again using multiplication by mobility-space
+ // unit vectors.
+ JS2.resize(nb, nu); JF2.resize(nb, nu);
+ Vector zeroU(nu, 0.);
+ for (int i=0; i < nu; ++i) {
+ zeroU[i] = 1;
+ matter.multiplyByStationJacobian(state, allBodies, randS, zeroU, JS2(i));
+ matter.multiplyByFrameJacobian(state, allBodies, randS, zeroU, JF2(i));
+ zeroU[i] = 0;
+ }
+ SimTK_TEST_EQ_TOL(JS2, JS, Slop);
+ SimTK_TEST_EQ_TOL(JF2, JF, Slop);
+
+ // Calculate JS2t=~JS using multiplication by force-space unit vectors.
+ Matrix_<Row3> JS2t(nu,nb);
+ Vector_<Vec3> zeroF(nb, Vec3(0));
+ // While we're at it, let's test non-contiguous vectors by filling in
+ // this scalar version and using its non-contig rows as column temps.
+ Matrix JS3mat(3*nb,nu);
+ for (int b=0; b < nb; ++b) {
+ for (int k=0; k<3; ++k) {
+ zeroF[b][k] = 1;
+ RowVectorView JS3matr = JS3mat[3*b+k];
+ matter.multiplyByStationJacobianTranspose(state, allBodies, randS,
+ zeroF, ~JS3matr);
+ zeroF[b][k] = 0;
+ for (int u=0; u < nu; ++u)
+ JS2t(u,b)[k] = JS3matr[u];
+ }
+ }
+ SimTK_TEST_EQ_TOL(JS2, ~JS2t, Slop); // we'll check JS3mat below
+
+ // Calculate JF2t=~JF using multiplication by force-space unit vectors.
+ Matrix_<SpatialRow> JF2t(nu,nb);
+ Vector_<SpatialVec> zeroSF(nb, SpatialVec(Vec3(0)));
+ // While we're at it, let's test non-contiguous vectors by filling in
+ // this scalar version and using its non-contig rows as column temps.
+ Matrix JF3mat(6*nb,nu);
+ for (int b=0; b < nb; ++b) {
+ for (int k=0; k<6; ++k) {
+ zeroSF[b][k/3][k%3] = 1;
+ RowVectorView JF3matr = JF3mat[6*b+k];
+ matter.multiplyByFrameJacobianTranspose(state, allBodies, randS,
+ zeroSF, ~JF3matr);
+ zeroSF[b][k/3][k%3] = 0;
+ for (int u=0; u < nu; ++u)
+ JF2t(u,b)[k/3][k%3] = JF3matr[u];
+ }
+ }
+ SimTK_TEST_EQ_TOL(JF2, ~JF2t, Slop); // we'll check JS3mat below
+
+
+ // All three methods match. Now let's see if they are right by shifting
+ // the System Jacobian to the new stations.
+
+ for (int i=0; i<nb; ++i) {
+ const MobilizedBody& mobod = matter.getMobilizedBody(allBodies[i]);
+ const Rotation& R_GB = mobod.getBodyRotation(state);
+ const Vec3 S_G = R_GB*randS[i];
+ for (int j=0; j<nu; ++j) {
+ const Vec3 w = J(i,j)[0];
+ const Vec3 v = J(i,j)[1];
+ const Vec3 vJ = v + w % S_G; // Shift
+ const Vec3 vS = JS2(i,j);
+ const SpatialVec vF = JF2(i,j);
+ SimTK_TEST_EQ(vS, vJ);
+ SimTK_TEST_EQ(vF, SpatialVec(w, vJ));
+ }
+ }
+
+
+ // Now create a scalar version of JS and make sure it matches the Vec3 one.
+ Matrix JSmat, JSmat2, JFmat, JFmat2;
+
+ matter.calcStationJacobian(state, allBodies, randS, JSmat);
+ matter.calcFrameJacobian(state, allBodies, randS, JFmat);
+ SimTK_TEST_EQ(JSmat.nrow(), 3*nb); SimTK_TEST_EQ(JSmat.ncol(), nu);
+ SimTK_TEST_EQ(JFmat.nrow(), 6*nb); SimTK_TEST_EQ(JFmat.ncol(), nu);
+
+ SimTK_TEST_EQ_TOL(JSmat, JS3mat, Slop); // same as above?
+ SimTK_TEST_EQ_TOL(JFmat, JF3mat, Slop); // same as above?
+
+ // Unpack JS into JSmat2 and compare with JSmat.
+ JSmat2.resize(3*nb, nu);
+ for (int row=0; row < nb; ++row) {
+ const int nxtr = 3*row; // row index into scalar matrix
+ for (int col=0; col < nu; ++col) {
+ for (int k=0; k<3; ++k) {
+ JSmat2(nxtr+k, col) = JS(row,col)[k];
+ }
+ }
+ }
+ // These should be exactly the same.
+ SimTK_TEST_EQ_TOL(JSmat2, JSmat, SignificantReal);
+
+ // Unpack JF into JFmat2 and compare with JFmat.
+ JFmat2.resize(6*nb, nu);
+ for (int row=0; row < nb; ++row) {
+ const int nxtr = 6*row; // row index into scalar matrix
+ for (int col=0; col < nu; ++col) {
+ for (int k=0; k<6; ++k) {
+ JFmat2(nxtr+k, col) = JF(row,col)[k/3][k%3];
+ }
+ }
+ }
+ // These should be exactly the same.
+ SimTK_TEST_EQ_TOL(JFmat2, JFmat, SignificantReal);
+}
+
+int main() {
+ SimTK_START_TEST("TestMassMatrix");
+ SimTK_SUBTEST(testRel2Cart);
+ SimTK_SUBTEST(testJacobianBiasTerms);
+ SimTK_SUBTEST(testCompositeInertia);
+ SimTK_SUBTEST(testUnconstrainedSystem);
+ SimTK_SUBTEST(testConstrainedSystem);
+ SimTK_SUBTEST(testTaskJacobians);
+ SimTK_END_TEST();
+}
+
diff --git a/Simbody/tests/TestMobilizedBody.cpp b/Simbody/tests/TestMobilizedBody.cpp
new file mode 100644
index 0000000..537fafb
--- /dev/null
+++ b/Simbody/tests/TestMobilizedBody.cpp
@@ -0,0 +1,280 @@
+/* -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 "Simbody.h"
+
+using namespace SimTK;
+using namespace std;
+
+void testCalculationMethods() {
+
+ // Create a system with two bodies.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+ MobilizedBody::Free b1(matter.Ground(), body);
+ MobilizedBody::Free b2(matter.Ground(), body);
+
+ // Set all the state variables to random values.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ Random::Gaussian random;
+
+ for (int i = 0; i < state.getNY(); ++i)
+ state.updY()[i] = random.getValue();
+
+ system.realize(state, Stage::Acceleration);
+
+ // Test the low level methods for transforming points and vectors.
+
+ const Vec3 point(0.5, 1, -1.5);
+ SimTK_TEST_EQ(b1.findStationLocationInGround(state, Vec3(0)), b1.getBodyOriginLocation(state));
+ SimTK_TEST_EQ(b1.findStationAtGroundPoint(state, b1.findStationLocationInGround(state, point)), point);
+ SimTK_TEST_EQ(b2.findStationAtGroundPoint(state, b1.findStationLocationInGround(state, point)), b1.findStationLocationInAnotherBody(state, point, b2));
+ SimTK_TEST_EQ(b2.findStationAtGroundPoint(state, b1.findStationLocationInGround(state, Vec3(0))).norm(), (b1.getBodyOriginLocation(state)-b2.getBodyOriginLocation(state)).norm());
+ SimTK_TEST_EQ(b2.findMassCenterLocationInGround(state), b2.findStationLocationInGround(state, b2.getBodyMassCenterStation(state)));
+ SimTK_TEST_EQ(b1.expressVectorInGroundFrame(state, Vec3(0)), Vec3(0));
+ SimTK_TEST_EQ(b1.expressVectorInGroundFrame(state, point), b1.getBodyRotation(state)*point);
+ SimTK_TEST_EQ(b1.expressGroundVectorInBodyFrame(state, b1.expressVectorInGroundFrame(state, point)), point);
+ SimTK_TEST_EQ(b2.expressGroundVectorInBodyFrame(state, b1.expressVectorInGroundFrame(state, point)), b1.expressVectorInAnotherBodyFrame(state, point, b2));
+
+ // Test the routines for mapping locations, velocities, and accelerations.
+
+ Vec3 r, v, a;
+ b1.findStationLocationVelocityAndAccelerationInGround(state, point, r, v, a);
+ SimTK_TEST_EQ(v, b1.findStationVelocityInGround(state, point));
+ SimTK_TEST_EQ(a, b1.findStationAccelerationInGround(state, point));
+ {
+ Vec3 r2, v2;
+ b1.findStationLocationAndVelocityInGround(state, point, r2, v2);
+ SimTK_TEST_EQ(r, r2);
+ SimTK_TEST_EQ(v, v2);
+ }
+ SimTK_TEST_EQ(b1.findStationVelocityInGround(state, Vec3(0)), b1.getBodyOriginVelocity(state));
+ SimTK_TEST_EQ(b1.findStationAccelerationInGround(state, Vec3(0)), b1.getBodyOriginAcceleration(state));
+ SimTK_TEST_EQ(b1.findStationVelocityInGround(state, point), b1.findStationVelocityInAnotherBody(state, point, matter.Ground()));
+}
+
+void testWeld() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -1, 0));
+ Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
+
+ // Create two pendulums, each with two welded bodies. One uses a Weld MobilizedBody,
+ // and the other uses a Weld constraint.
+
+ Transform inboard(Vec3(0.1, 0.5, -1));
+ Transform outboard(Vec3(0.2, -0.2, 0));
+ MobilizedBody::Ball p1(matter.updGround(), Vec3(0), body, Vec3(0, 1, 0));
+ MobilizedBody::Ball p2(matter.updGround(), Vec3(0), body, Vec3(0, 1, 0));
+ MobilizedBody::Weld c1(p1, inboard, body, outboard);
+ MobilizedBody::Free c2(p2, inboard, body, outboard);
+ Constraint::Weld constraint(p2, inboard, c2, outboard);
+
+ // It is not a general test unless the Weld mobilizer has children!
+ MobilizedBody::Pin wchild1(c1, inboard, body, outboard);
+ MobilizedBody::Pin wchild2(c2, inboard, body, outboard);
+ Force::MobilityLinearSpring(forces, wchild1, 0, 1000, 0);
+ Force::MobilityLinearSpring(forces, wchild2, 0, 1000, 0);
+
+ State state = system.realizeTopology();
+ p1.setU(state, Vec3(1, 2, 3));
+ p2.setU(state, Vec3(1, 2, 3));
+ system.realize(state, Stage::Velocity);
+ system.project(state, 1e-10);
+
+ SimTK_TEST_EQ(c1.getBodyTransform(state), c2.getBodyTransform(state));
+ SimTK_TEST_EQ(c1.getBodyVelocity(state), c2.getBodyVelocity(state));
+
+ // Simulate it and see if both pendulums behave identically.
+
+ RungeKuttaMersonIntegrator integ(system);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(5);
+ system.realize(integ.getState(), Stage::Velocity);
+ SimTK_TEST_EQ_TOL(c1.getBodyTransform(integ.getState()),
+ c2.getBodyTransform(integ.getState()), 1e-10);
+ SimTK_TEST_EQ_TOL(c1.getBodyVelocity(integ.getState()),
+ c2.getBodyVelocity(integ.getState()), 1e-10);
+}
+
+
+// Create two pendulums with gimbal joints, one where the gimbals are faked
+// with a series of pin joints that should provide identical generalized
+// coordinates and speeds (we're expecting the new gimbal definition in which
+// the generalized speeds are the Euler angle derivatives).
+
+void testGimbal() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.81, 0));
+ Body::Rigid lumpy(MassProperties(3.1, Vec3(.1, .2, .3),
+ UnitInertia(1.2,1.1,1.3,.01,.02,.03)));
+ Body::Rigid massless(MassProperties(0,Vec3(0),UnitInertia(0)));
+
+ Vec3 inboard(0.1, 0.5, -1);
+ Vec3 outboard(0.2, -0.2, 0);
+ MobilizedBody::Gimbal p1(matter.updGround(), inboard, lumpy, outboard);
+
+ Rotation axisX(Pi/2, YAxis); // rotate z into x
+ Rotation axisY(-Pi/2, XAxis); // rotate z into y
+ Rotation axisZ; // leave z where it is
+ MobilizedBody::Pin dummy1(matter.updGround(), Transform(axisX,inboard),
+ massless, Transform(axisX, Vec3(0)));
+ MobilizedBody::Pin dummy2(dummy1, Transform(axisY, Vec3(0)),
+ massless, Transform(axisY, Vec3(0)));
+ MobilizedBody::Pin p2(dummy2, Transform(axisZ, Vec3(0)),
+ lumpy, Transform(axisZ, outboard));
+
+ MobilizedBody::Weld c1(p1, inboard, lumpy, outboard);
+ MobilizedBody::Weld c2(p2, inboard, lumpy, outboard);
+
+ c1.addBodyDecoration(Transform(), DecorativeBrick(.1*Vec3(1,2,3))
+ .setColor(Cyan).setOpacity(0.2));
+ c2.addBodyDecoration(Transform(), DecorativeBrick(.1*Vec3(1,2,3))
+ .setColor(Red).setRepresentation(DecorativeGeometry::DrawWireframe));
+
+ //Visualizer viz(system); viz.setBackgroundType(Visualizer::SolidColor);
+ //system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
+
+ State state = system.realizeTopology();
+ p1.setQ(state, Vec3(.1,.2,.3));
+ dummy1.setAngle(state, .1);
+ dummy2.setAngle(state, .2);
+ p2.setAngle(state, .3);
+
+ p1.setU(state, Vec3(1, 2, 3));
+ dummy1.setRate(state, 1);
+ dummy2.setRate(state, 2);
+ p2.setRate(state, 3);
+
+ system.realize(state, Stage::Velocity);
+ system.project(state, 1e-10);
+
+ SimTK_TEST_EQ(c1.getBodyTransform(state), c2.getBodyTransform(state));
+ SimTK_TEST_EQ(c1.getBodyVelocity(state), c2.getBodyVelocity(state));
+
+ // Simulate it and see if both pendulums behave identically.
+
+ RungeKuttaMersonIntegrator integ(system);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(5);
+ system.realize(integ.getState(), Stage::Velocity);
+ SimTK_TEST_EQ_TOL(c1.getBodyTransform(integ.getState()),
+ c2.getBodyTransform(integ.getState()), 1e-10);
+ SimTK_TEST_EQ_TOL(c1.getBodyVelocity(integ.getState()),
+ c2.getBodyVelocity(integ.getState()), 1e-10);
+}
+
+
+// Create two pendulums with bushing joints, one where the bushings are faked
+// with a Cartesian (3 sliders) and a series of pin joints that should provide
+// identical generalized coordinates and speeds, except that the translational
+// coordinates are the last three q's for the bushing but are interpreted as
+// translating first, then rotating.
+
+void testBushing() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.81, 0));
+ Body::Rigid lumpy(MassProperties(3.1, Vec3(.1, .2, .3),
+ UnitInertia(1.2,1.1,1.3,.01,.02,.03)));
+ Body::Rigid massless(MassProperties(0,Vec3(0),UnitInertia(0)));
+
+ Vec3 inboard(0.1, 0.5, -1);
+ Vec3 outboard(0.2, -0.2, 0);
+ MobilizedBody::Bushing p1(matter.updGround(), inboard, lumpy, outboard);
+
+ Rotation axisX(Pi/2, YAxis); // rotate z into x
+ Rotation axisY(-Pi/2, XAxis); // rotate z into y
+ Rotation axisZ; // leave z where it is
+ MobilizedBody::Translation dummy0(matter.updGround(), inboard,
+ massless, Transform());
+ MobilizedBody::Pin dummy1(dummy0, Transform(axisX,Vec3(0)),
+ massless, Transform(axisX, Vec3(0)));
+ MobilizedBody::Pin dummy2(dummy1, Transform(axisY, Vec3(0)),
+ massless, Transform(axisY, Vec3(0)));
+ MobilizedBody::Pin p2(dummy2, Transform(axisZ, Vec3(0)),
+ lumpy, Transform(axisZ, outboard));
+
+ MobilizedBody::Weld c1(p1, inboard, lumpy, outboard);
+ MobilizedBody::Weld c2(p2, inboard, lumpy, outboard);
+
+ c1.addBodyDecoration(Transform(), DecorativeBrick(.1*Vec3(1,2,3))
+ .setColor(Cyan).setOpacity(0.2));
+ c2.addBodyDecoration(Transform(), DecorativeBrick(.1*Vec3(1,2,3))
+ .setColor(Red).setRepresentation(DecorativeGeometry::DrawWireframe));
+
+ //Visualizer viz(system); viz.setBackgroundType(Visualizer::SolidColor);
+ //system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
+
+ State state = system.realizeTopology();
+ p1.setQ(state, Vec6(.1,.2,.3,1,2,3));
+ dummy0.setMobilizerTranslation(state, Vec3(1,2,3));
+ dummy1.setAngle(state, .1);
+ dummy2.setAngle(state, .2);
+ p2.setAngle(state, .3);
+
+ p1.setU(state, Vec6(1, 2, 3, -.1, -.2, -.3));
+ dummy0.setMobilizerVelocity(state, Vec3(-.1, -.2, -.3));
+ dummy1.setRate(state, 1);
+ dummy2.setRate(state, 2);
+ p2.setRate(state, 3);
+
+ system.realize(state, Stage::Velocity);
+ system.project(state, 1e-10);
+
+ SimTK_TEST_EQ(c1.getBodyTransform(state), c2.getBodyTransform(state));
+ SimTK_TEST_EQ(c1.getBodyVelocity(state), c2.getBodyVelocity(state));
+
+ // Simulate it and see if both pendulums behave identically.
+
+ RungeKuttaMersonIntegrator integ(system);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(5);
+ system.realize(integ.getState(), Stage::Velocity);
+ SimTK_TEST_EQ_TOL(c1.getBodyTransform(integ.getState()),
+ c2.getBodyTransform(integ.getState()), 1e-10);
+ SimTK_TEST_EQ_TOL(c1.getBodyVelocity(integ.getState()),
+ c2.getBodyVelocity(integ.getState()), 1e-10);
+}
+
+int main() {
+ SimTK_START_TEST("TestMobilizedBody");
+ SimTK_SUBTEST(testCalculationMethods);
+ SimTK_SUBTEST(testWeld);
+ SimTK_SUBTEST(testGimbal);
+ SimTK_SUBTEST(testBushing);
+ SimTK_END_TEST();
+}
+
diff --git a/Simbody/tests/TestMobilizerReactionForces.cpp b/Simbody/tests/TestMobilizerReactionForces.cpp
new file mode 100644
index 0000000..5e4c13f
--- /dev/null
+++ b/Simbody/tests/TestMobilizerReactionForces.cpp
@@ -0,0 +1,547 @@
+/* -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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"
+#include "SimTKcommon/Testing.h"
+
+using namespace SimTK;
+using namespace std;
+
+const Real TOL = 1e-10;
+const Real BOND_LENGTH = 0.5;
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+template <class T>
+void assertEqual(T val1, T val2, double tol=TOL) {
+ ASSERT(abs(val1-val2) < tol);
+}
+
+template <int N>
+void assertEqual(Vec<N> val1, Vec<N> val2, double tol) {
+ double norm = max(val1.norm(), 1.0);
+ for (int i = 0; i < N; ++i)
+ ASSERT(abs(val1[i]-val2[i]) < tol*norm);
+}
+
+template<>
+void assertEqual(Vector val1, Vector val2, double tol) {
+ ASSERT(val1.size() == val2.size());
+ for (int i = 0; i < val1.size(); ++i)
+ assertEqual(val1[i], val2[i], tol);
+}
+
+template<>
+void assertEqual(SpatialVec val1, SpatialVec val2, double tol) {
+ assertEqual(val1[0], val2[0], tol);
+ assertEqual(val1[1], val2[1], tol);
+}
+
+template<>
+void assertEqual(Transform val1, Transform val2, double tol) {
+ assertEqual(val1.p(), val2.p(), tol);
+ ASSERT(val1.R().isSameRotationToWithinAngle(val2.R(), tol));
+}
+
+void compareReactionToConstraint(SpatialVec reactionForce, const Constraint& constraint, const State& state) {
+ Vector_<SpatialVec> constraintForce(constraint.getNumConstrainedBodies());
+ Vector mobilityForce(constraint.getNumConstrainedU(state));
+ constraint.calcConstraintForcesFromMultipliers(state, constraint.getMultipliersAsVector(state), constraintForce, mobilityForce);
+
+ // Transform the reaction force from the joint location to the body location.
+
+ const MobilizedBody& body = constraint.getMobilizedBodyFromConstrainedBody(ConstrainedBodyIndex(1));
+ Vec3 localForce = ~body.getBodyTransform(state).R()*reactionForce[1];
+ reactionForce[0] += body.getBodyTransform(state).R()*(body.getOutboardFrame(state).p()%localForce);
+ assertEqual(reactionForce, -1*constraint.getAncestorMobilizedBody().getBodyRotation(state)*constraintForce[1]);
+}
+
+/**
+ * Compare the forces generated by equivalent mobilizers and constraints.
+ */
+
+void testByComparingToConstraints() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity(forces, matter, Vec3(0, -9.8, 0));
+
+ // Create two free joints (which should produce no reaction forces).
+
+ Body::Rigid body = Body::Rigid(MassProperties(1.3, Vec3(0), Inertia(1.3)));
+ MobilizedBody::Free f1(matter.updGround(), Transform(Vec3(0)), body, Transform(Vec3(BOND_LENGTH, 0, 0)));
+ MobilizedBody::Free f2(f1, Transform(Vec3(0)), body, Transform(Vec3(BOND_LENGTH, 0, 0)));
+
+ // Two ball joints, and two free joints constrained to act like ball joints.
+
+ MobilizedBody::Free fb1(matter.updGround(), Transform(Vec3(0)), body, Transform(Vec3(BOND_LENGTH, 0, 0)));
+ MobilizedBody::Free fb2(fb1, Transform(Vec3(0, 0, BOND_LENGTH)), body, Transform(Vec3(BOND_LENGTH, 0, 0)));
+ Constraint::Ball fb1constraint(matter.updGround(), Vec3(0, 0, 0), fb1, Vec3(BOND_LENGTH, 0, 0));
+ Constraint::Ball fb2constraint(fb1, Vec3(0, 0, BOND_LENGTH), fb2, Vec3(BOND_LENGTH, 0, 0));
+ MobilizedBody::Ball b1(matter.updGround(), Transform(Vec3(0)), body, Transform(Vec3(BOND_LENGTH, 0, 0)));
+ MobilizedBody::Ball b2(b1, Transform(Vec3(0, 0, BOND_LENGTH)), body, Transform(Vec3(BOND_LENGTH, 0, 0)));
+ Force::ConstantTorque(forces, fb2, Vec3(0.1, 0.1, 1.0));
+ Force::ConstantTorque(forces, b2, Vec3(0.1, 0.1, 1.0));
+
+ // Two translation joints, and two free joints constrained to act like translation joints.
+
+ MobilizedBody::Free ft1(matter.updGround(), Transform(Vec3(0)), body, Transform(Vec3(BOND_LENGTH, 0, 0)));
+ MobilizedBody::Free ft2(ft1, Transform(Vec3(0)), body, Transform(Vec3(0, BOND_LENGTH, 0)));
+ Constraint::ConstantOrientation ft1constraint(matter.updGround(), Rotation(0, Vec3(1)), ft1, Rotation(0, Vec3(1)));
+ Constraint::ConstantOrientation ft2constraint(ft1, Rotation(0, Vec3(1)), ft2, Rotation(0, Vec3(1)));
+ MobilizedBody::Translation t1(matter.updGround(), Transform(Vec3(0)), body, Transform(Vec3(BOND_LENGTH, 0, 0)));
+ MobilizedBody::Translation t2(t1, Transform(Vec3(0)), body, Transform(Vec3(0, BOND_LENGTH, 0)));
+ Force::ConstantTorque(forces, ft2, Vec3(0.1, 0.1, 1.0));
+ Force::ConstantTorque(forces, t2, Vec3(0.1, 0.1, 1.0));
+
+ // Create the state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ Random::Gaussian random;
+ int nq = state.getNQ()/2;
+ for (int i = 0; i < state.getNY(); ++i)
+ state.updY()[i] = random.getValue();
+ system.realize(state, Stage::Velocity);
+ Transform b1transform = b1.getMobilizerTransform(state);
+ Transform b2transform = b2.getMobilizerTransform(state);
+ SpatialVec b1velocity = b1.getMobilizerVelocity(state);
+ SpatialVec b2velocity = b2.getMobilizerVelocity(state);
+ Transform t1transform = t1.getMobilizerTransform(state);
+ Transform t2transform = t2.getMobilizerTransform(state);
+ SpatialVec t1velocity = t1.MobilizedBody::getMobilizerVelocity(state);
+ SpatialVec t2velocity = t2.MobilizedBody::getMobilizerVelocity(state);
+ fb1.setQToFitTransform(state, b1transform);
+ fb2.setQToFitTransform(state, b2transform);
+ fb1.setUToFitVelocity(state, b1velocity);
+ fb2.setUToFitVelocity(state, b2velocity);
+ ft1.setQToFitTransform(state, t1transform);
+ ft2.setQToFitTransform(state, t2transform);
+ ft1.setUToFitVelocity(state, t1velocity);
+ ft2.setUToFitVelocity(state, t2velocity);
+
+ system.project(state, TOL);
+ system.realize(state, Stage::Acceleration);
+
+ // Make sure the free and constrained bodies really are identical.
+
+ assertEqual(b1.getBodyTransform(state), fb1.getBodyTransform(state));
+ assertEqual(b2.getBodyTransform(state), fb2.getBodyTransform(state));
+ assertEqual(b1.getBodyVelocity(state), fb1.getBodyVelocity(state));
+ assertEqual(b2.getBodyVelocity(state), fb2.getBodyVelocity(state));
+ assertEqual(t1.getBodyTransform(state), ft1.getBodyTransform(state));
+ assertEqual(t2.getBodyTransform(state), ft2.getBodyTransform(state));
+ assertEqual(t1.getBodyVelocity(state), ft1.getBodyVelocity(state));
+ assertEqual(t2.getBodyVelocity(state), ft2.getBodyVelocity(state));
+
+ // Calculate the mobility reaction forces.
+
+ Vector_<SpatialVec> forcesAtMInG(matter.getNumBodies());
+ matter.calcMobilizerReactionForces(state, forcesAtMInG);
+
+
+
+ // Check that the bulk calculation matches the body-by-body calculation.
+ for (MobilizedBodyIndex bx(0); bx < matter.getNumBodies(); ++bx) {
+ assertEqual(forcesAtMInG[bx],
+ matter.getMobilizedBody(bx)
+ .findMobilizerReactionOnBodyAtMInGround(state));
+ }
+
+ // Make sure all free bodies have no reaction force on them.
+
+ assertEqual((forcesAtMInG[f1.getMobilizedBodyIndex()]), SpatialVec(Vec3(0), Vec3(0)));
+ assertEqual((forcesAtMInG[f2.getMobilizedBodyIndex()]), SpatialVec(Vec3(0), Vec3(0)));
+ assertEqual((forcesAtMInG[fb1.getMobilizedBodyIndex()]), SpatialVec(Vec3(0), Vec3(0)));
+ assertEqual((forcesAtMInG[fb2.getMobilizedBodyIndex()]), SpatialVec(Vec3(0), Vec3(0)));
+ assertEqual((forcesAtMInG[ft1.getMobilizedBodyIndex()]), SpatialVec(Vec3(0), Vec3(0)));
+ assertEqual((forcesAtMInG[ft2.getMobilizedBodyIndex()]), SpatialVec(Vec3(0), Vec3(0)));
+
+ // The reaction forces should match the corresponding constraint forces.
+
+ compareReactionToConstraint(forcesAtMInG[b1.getMobilizedBodyIndex()], fb1constraint, state);
+ compareReactionToConstraint(forcesAtMInG[b2.getMobilizedBodyIndex()], fb2constraint, state);
+ compareReactionToConstraint(forcesAtMInG[t1.getMobilizedBodyIndex()], ft1constraint, state);
+ compareReactionToConstraint(forcesAtMInG[t2.getMobilizedBodyIndex()], ft2constraint, state);
+}
+
+/*
+ * (sherm 110919) None of the existing tests caught the problem reported
+ * in bug #1535 -- incorrect reaction torques sometimes.
+ * This is a pair of identical two-body pendulums, one done with pin joints
+ * and one done with equivalent constraints.
+ */
+void testByComparingToConstraints2() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(10, -9.8, 3));
+
+ Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1).setColor(Red));
+
+ // First double pendulum, using Pin joints.
+ Rotation x45(Pi/4, XAxis);
+ MobilizedBody::Pin pendulum1(matter.updGround(),
+ Transform(x45,Vec3(0,-1,0)),
+ pendulumBody,
+ Transform(Vec3(0, 1, 0)));
+ MobilizedBody::Pin pendulum1b(pendulum1,
+ Transform(x45,Vec3(0,-1,0)),
+ pendulumBody,
+ Transform(Vec3(0, 1, 0)));
+
+ // Second double pendulum, using Free joints plus 5 constraints.
+ MobilizedBody::Free pendulum2(matter.updGround(),
+ Transform(x45,Vec3(2,-1,0)),
+ pendulumBody,
+ Transform(Vec3(0,1,0)));
+ Constraint::Ball ballcons2(matter.updGround(), Vec3(2,-1,0),
+ pendulum2, Vec3(0,1,0));
+ const Transform& X_GF2 = pendulum2.getDefaultInboardFrame();
+ const Transform& X_P2M = pendulum2.getDefaultOutboardFrame();
+ Constraint::ConstantAngle angx2(matter.Ground(), X_GF2.x(),
+ pendulum2, X_P2M.z());
+ Constraint::ConstantAngle angy2(matter.Ground(), X_GF2.y(),
+ pendulum2, X_P2M.z());
+
+ MobilizedBody::Free pendulum2b(pendulum2,
+ Transform(x45,Vec3(0,-1,0)),
+ pendulumBody,
+ Transform(Vec3(0,1,0)));
+ Constraint::Ball ballcons2b(pendulum2, Vec3(0,-1,0),
+ pendulum2b, Vec3(0,1,0));
+ const Transform& X_GF2b = pendulum2b.getDefaultInboardFrame();
+ const Transform& X_P2Mb = pendulum2b.getDefaultOutboardFrame();
+ Constraint::ConstantAngle angx2b(pendulum2, X_GF2b.x(),
+ pendulum2b, X_P2Mb.z());
+ Constraint::ConstantAngle angy2b(pendulum2, X_GF2b.y(),
+ pendulum2b, X_P2Mb.z());
+
+ // Uncomment if you want to see this.
+ //Visualizer viz(system);
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ pendulum1.setOneQ(state, 0, Pi/4);
+ pendulum1.setOneU(state, 0, 1.0); // initial velocity 1 rad/sec
+
+ pendulum1b.setOneU(state, 0, 1.0); // initial velocity 1 rad/sec
+ pendulum1b.setOneQ(state, 0, Pi/4);
+
+ pendulum2.setQToFitRotation(state, Rotation(Pi/4, ZAxis));
+ pendulum2.setUToFitAngularVelocity(state, Vec3(0,0,1));
+ pendulum2b.setQToFitRotation(state, Rotation(Pi/4, ZAxis));
+ pendulum2b.setUToFitAngularVelocity(state, Vec3(0,0,1));
+
+ system.realize(state);
+ //viz.report(state);
+
+ const MobodIndex p2x = pendulum2.getMobilizedBodyIndex();
+ const MobodIndex p2bx = pendulum2b.getMobilizedBodyIndex();
+
+
+ Vector_<SpatialVec> forcesAtMInG, forcesAtBInG, forcesAtFInG;
+ matter.calcMobilizerReactionForces(state, forcesAtMInG);
+
+ // Check that the bulk results match the individual ones, and fill
+ // up the Vector of reaction on the parent bodies.
+ forcesAtFInG.resize(forcesAtMInG.size());
+ for (MobilizedBodyIndex mbx(0); mbx < matter.getNumBodies(); ++mbx) {
+ SimTK_TEST_EQ(forcesAtMInG[mbx], matter.getMobilizedBody(mbx)
+ .findMobilizerReactionOnBodyAtMInGround(state));
+
+ forcesAtFInG[mbx] = matter.getMobilizedBody(mbx)
+ .findMobilizerReactionOnParentAtFInGround(state);
+ }
+
+ // Now we'll convert forces on B at M to forces on P at F manually, and
+ // compare with the ones we got by asking the mobilized body.
+ Vector_<SpatialVec> forcesAtFInG_byhand(forcesAtMInG.size());
+ forcesAtFInG_byhand[0] = -forcesAtMInG[0]; // Ground is "welded" at origin
+ for (MobilizedBodyIndex i(1); i < matter.getNumBodies(); ++i) {
+ const MobilizedBody& body = matter.getMobilizedBody(i);
+ const MobilizedBody& parent = body.getParentMobilizedBody();
+ // Want to shift reaction by p_MF, the vector from M to F across the
+ // mobilizer, and negate. Can get p_FM; must reexpress in G.
+ const Vec3& p_FM = body.getMobilizerTransform(state).p();
+ const Rotation& R_PF = body.getInboardFrame(state).R(); // In parent.
+ const Rotation& R_GP = parent.getBodyTransform(state).R();
+ Rotation R_GF = R_GP*R_PF; // F frame orientation in Ground.
+ Vec3 p_MF_G = -(R_GF*p_FM); // Re-express and negate shift vector.
+ forcesAtFInG_byhand[i] = -shiftForceBy(forcesAtMInG[i], p_MF_G);
+ }
+
+ SimTK_TEST_EQ(forcesAtFInG, forcesAtFInG_byhand);
+
+ // Shift the reaction forces to body origins for easy comparison with
+ // the reported constraint forces.
+ forcesAtBInG.resize(forcesAtMInG.size());
+ const MobodIndex p1x = pendulum1.getMobilizedBodyIndex();
+ const MobodIndex p1bx = pendulum1b.getMobilizedBodyIndex();
+ const Rotation& R_G1 = pendulum1.getBodyTransform(state).R();
+ const Rotation& R_G1b = pendulum1b.getBodyTransform(state).R();
+ forcesAtBInG[p1x] = shiftForceFromTo(forcesAtMInG[p1x],
+ R_G1*Vec3(0,1,0), Vec3(0));
+ forcesAtBInG[p1bx] = shiftForceFromTo(forcesAtMInG[p1bx],
+ R_G1b*Vec3(0,1,0), Vec3(0));
+
+ // Compare those manually-shifted quantities to the ones we can get
+ // direction from the MobilizedBody.
+ SpatialVec forcesAtBInG_p1 =
+ pendulum1.findMobilizerReactionOnBodyAtOriginInGround(state);
+ SpatialVec forcesAtBInG_p1b =
+ pendulum1b.findMobilizerReactionOnBodyAtOriginInGround(state);
+
+ SimTK_TEST_EQ(forcesAtBInG[p1x], forcesAtBInG_p1);
+ SimTK_TEST_EQ(forcesAtBInG[p1bx], forcesAtBInG_p1b);
+
+ // The constraints apply forces to parent and body; we want to compare
+ // forces on the body, which will be the second entry here. We're assuming
+ // the ball and constant angle constraints are ordered the same way; if
+ // that ever changes the constraints can be queried to find the mobilized
+ // body index corresponding to the constrained body index.
+ Vector_<SpatialVec> cons2Forces =
+ -(ballcons2.getConstrainedBodyForcesAsVector(state)
+ + angx2.getConstrainedBodyForcesAsVector(state)
+ + angy2.getConstrainedBodyForcesAsVector(state));
+ Vector_<SpatialVec> cons2bForces =
+ -(ballcons2b.getConstrainedBodyForcesAsVector(state)
+ + angx2b.getConstrainedBodyForcesAsVector(state)
+ + angy2b.getConstrainedBodyForcesAsVector(state));
+
+ // Couldn't quite make default tolerance on some platforms. This uses
+ // 10X default.
+ SimTK_TEST_EQ_SIZE(cons2Forces[1], forcesAtBInG[p1x], 10);
+ SimTK_TEST_EQ_SIZE(cons2bForces[1], forcesAtBInG[p1bx], 10);
+}
+
+/**
+ * Construct a system of several bodies, and compare the reaction forces to those calculated by SD/FAST.
+ */
+
+void testByComparingToSDFAST() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity(forces, matter, Vec3(0, -9.8, 0));
+
+ // Construct the set of bodies.
+
+ Inertia inertia = Inertia(Mat33(0.1, 0.01, 0.01,
+ 0.01, 0.1, 0.01,
+ 0.01, 0.01, 0.1));
+ MobilizedBody::Slider body1(matter.updGround(), MassProperties(10.0, Vec3(0), inertia));
+ MobilizedBody::Pin body2(body1, Vec3(0.1, 0.1, 0), MassProperties(20.0, Vec3(0), inertia), Vec3(0, -0.2, 0));
+ MobilizedBody::Gimbal body3(body2, Vec3(0, 0.2, 0), MassProperties(20.0, Vec3(0), inertia), Vec3(0, -0.2, 0));
+ MobilizedBody::Pin body4(body3, Vec3(0, 0.2, 0), MassProperties(30.0, Vec3(0), inertia), Vec3(0, -0.2, 0));
+ State state = system.realizeTopology();
+ system.realize(state, Stage::Acceleration);
+
+ // Calculate reaction forces, and compare to the values that were generated by SD/FAST.
+
+ Vector_<SpatialVec> reaction(matter.getNumBodies());
+ matter.calcMobilizerReactionForces(state, reaction);
+ assertEqual(~body1.getBodyTransform(state).R()*reaction[body1.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 68.6), Vec3(0, 784.0, 0)));
+ assertEqual(~body2.getBodyTransform(state).R()*reaction[body2.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 0), Vec3(0, 686.0, 0)));
+ assertEqual(~body3.getBodyTransform(state).R()*reaction[body3.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 0), Vec3(0, 490.0, 0)));
+ assertEqual(~body4.getBodyTransform(state).R()*reaction[body4.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 0), Vec3(0, 294.0, 0)));
+
+ // Now set it to a different configuration and try again.
+
+ body1.setLength(state, 1.0);
+ body2.setAngle(state, 0.5);
+ Rotation r;
+ r.setRotationFromThreeAnglesThreeAxes(BodyRotationSequence, 0.2, ZAxis, -0.1, XAxis, 2.0, YAxis);
+ body3.setQToFitRotation(state, r);
+ body4.setAngle(state, -0.5);
+ system.realize(state, Stage::Acceleration);
+ matter.calcMobilizerReactionForces(state, reaction);
+ assertEqual(~body1.getBodyTransform(state).R()*reaction[body1.getMobilizedBodyIndex()], SpatialVec(Vec3(1.647327, 0.783211, 34.088183), Vec3(0, 359.274099, 3.342380)), 1e-5);
+ assertEqual(~body2.getBodyTransform(state).R()*reaction[body2.getMobilizedBodyIndex()], SpatialVec(Vec3(1.688077, 0.351125, 0), Vec3(55.399123, 267.455570, 3.342380)), 1e-5);
+ assertEqual(~body3.getBodyTransform(state).R()*reaction[body3.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 0), Vec3(-17.757553, 174.663042, -11.383057)), 1e-5);
+ assertEqual(~body4.getBodyTransform(state).R()*reaction[body4.getMobilizedBodyIndex()], SpatialVec(Vec3(0.910890, 0.082353, 0), Vec3(-13.977214, 74.444715, 4.943682)), 1e-5);
+
+ // Try giving it momentum.
+
+ state.updQ() = 0.0;
+ body2.setOneU(state, 0, 1);
+ body3.setUToFitAngularVelocity(state, Vec3(3, 4, 2));
+ body4.setOneU(state, 0, 5);
+ system.realize(state, Stage::Acceleration);
+ matter.calcMobilizerReactionForces(state, reaction);
+ assertEqual(~body1.getBodyTransform(state).R()*reaction[body1.getMobilizedBodyIndex()], SpatialVec(Vec3(-13.549253, 2.723897, -6.355912), Vec3(0, 34.0, -27.088584)), 1e-5);
+ assertEqual(~body2.getBodyTransform(state).R()*reaction[body2.getMobilizedBodyIndex()], SpatialVec(Vec3(-10.840395, 0.015039, 0), Vec3(-0.440882, -64.0, -27.088584)), 1e-5);
+ assertEqual(~body3.getBodyTransform(state).R()*reaction[body3.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 0), Vec3(0.692814, -256.000000, -27.088584)), 1e-5);
+ assertEqual(~body4.getBodyTransform(state).R()*reaction[body4.getMobilizedBodyIndex()], SpatialVec(Vec3(3.276930, -0.281928, 0), Vec3(3.796164, -372.0, 21.472977)), 1e-5);
+}
+
+/**
+ * Construct a system of several bodies, and compare the reaction forces to those calculated by SD/FAST.
+ */
+
+void testByComparingToSDFAST2() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity(forces, matter, Vec3(0, -9.8065, 0));
+
+ // Construct the set of bodies.
+
+ Body::Rigid femur(MassProperties(8.806, Vec3(0), Inertia(Vec3(0.1268, 0.0332, 0.1337))));
+ Body::Rigid tibia(MassProperties(3.510, Vec3(0), Inertia(Vec3(0.0477, 0.0048, 0.0484))));
+ MobilizedBody::Pin p1(matter.Ground(), Transform(Vec3(0.0000, -0.0700, 0.0935)), femur, Transform(Vec3(0.0020, 0.1715, 0)));
+ MobilizedBody::Slider p2(p1, Transform(Vec3(0.0033, -0.2294, 0)), tibia, Transform(Vec3(0.0, 0.1862, 0.0)));
+ State state = system.realizeTopology();
+ system.realize(state, Stage::Acceleration);
+
+ // Calculate reaction forces, and compare to the values that were generated by SD/FAST.
+
+ Vector_<SpatialVec> reaction(matter.getNumBodies());
+ matter.calcMobilizerReactionForces(state, reaction);
+ assertEqual(~p1.getBodyTransform(state).R()*reaction[p1.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 0), Vec3(0.438079, 120.773069, 0)), 1e-5);
+ assertEqual(~p2.getBodyTransform(state).R()*reaction[p2.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 0.014040), Vec3(0, 34.422139, 0)), 1e-5);
+
+ // Now set it to a different configuration and try again.
+
+ p1.setOneQ(state, 0, -90*NTraits<Real>::getPi()/180);
+ p2.setOneQ(state, 0, 0.1);
+ system.realize(state, Stage::Acceleration);
+ matter.calcMobilizerReactionForces(state, reaction);
+ assertEqual(~p1.getBodyTransform(state).R()*reaction[p1.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 0), Vec3(-39.481457, 10.489344, 0)), 1e-5);
+ assertEqual(~p2.getBodyTransform(state).R()*reaction[p2.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 1.502242), Vec3(0, 11.035987, 0)), 1e-5);
+}
+
+/**
+ * Construct a system of several bodies and a constraint, and compare the reaction forces to those calculated by SD/FAST.
+ */
+
+void testByComparingToSDFASTWithConstraint() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity(forces, matter, Vec3(0, -9.8, 0));
+
+ // Construct the set of bodies.
+
+ Inertia inertia = Inertia(Mat33(0.1, 0.01, 0.01,
+ 0.01, 0.1, 0.01,
+ 0.01, 0.01, 0.1));
+ MobilizedBody::Gimbal body1(matter.updGround(), MassProperties(10.0, Vec3(0), inertia));
+ MobilizedBody::Gimbal body2(body1, Vec3(0, -0.1, 0.2), MassProperties(20.0, Vec3(0), inertia), Vec3(0, 0.2, 0));
+ MobilizedBody::Gimbal body3(body1, Vec3(0, -0.1, -0.2), MassProperties(20.0, Vec3(0), inertia), Vec3(0, 0.2, 0));
+ MobilizedBody::Gimbal body4(body2, Vec3(0, -0.2, 0), MassProperties(30.0, Vec3(0), inertia), Vec3(0, 0.2, 0));
+ MobilizedBody::Gimbal body5(body3, Vec3(0, -0.2, 0), MassProperties(30.0, Vec3(0), inertia), Vec3(0, 0.2, 0));
+ Constraint::Rod constraint(body4, body5, 0.15);
+ State state = system.realizeTopology();
+ system.realize(state, Stage::Velocity);
+ system.project(state, 1e-10);
+ system.realize(state, Stage::Acceleration);
+
+ // Calculate reaction forces, and compare to the values that were generated by SD/FAST.
+
+ Vector_<SpatialVec> reaction(matter.getNumBodies());
+ matter.calcMobilizerReactionForces(state, reaction);
+ assertEqual(~body1.getBodyTransform(state).R()*reaction[body1.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 0), Vec3(-0.000626, 1077.988912, 0.000030)), 1e-5);
+ assertEqual(~body2.getBodyTransform(state).R()*reaction[body2.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 0), Vec3(-0.005038, 495.288692, -18.767467)), 1e-5);
+ assertEqual(~body3.getBodyTransform(state).R()*reaction[body3.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 0), Vec3(0.004236, 495.287857, 18.767535)), 1e-5);
+ assertEqual(~body4.getBodyTransform(state).R()*reaction[body4.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 0), Vec3(0.006251, 303.365940, -0.202330)), 1e-5);
+ assertEqual(~body5.getBodyTransform(state).R()*reaction[body5.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 0), Vec3(-0.005933, 303.365472, 0.202301)), 1e-5);
+
+ // Now set it to a different configuration and try again.
+
+ Rotation r;
+ r.setRotationFromThreeAnglesThreeAxes(BodyRotationSequence, 1.0, ZAxis, 1.0, XAxis, 1.0, YAxis);
+ body1.setQToFitRotation(state, r);
+ r.setRotationFromThreeAnglesThreeAxes(BodyRotationSequence, 0.433843, ZAxis, 0.647441, XAxis, 0.500057, YAxis);
+ body2.setQToFitRotation(state, r);
+ r.setRotationFromThreeAnglesThreeAxes(BodyRotationSequence, 0.066156, ZAxis, -0.117266, XAxis, -0.047605, YAxis);
+ body3.setQToFitRotation(state, r);
+ r.setRotationFromThreeAnglesThreeAxes(BodyRotationSequence, 0.000997, ZAxis, 0.055206, XAxis, 0.0, YAxis);
+ body4.setQToFitRotation(state, r);
+ r.setRotationFromThreeAnglesThreeAxes(BodyRotationSequence, 1.008746, ZAxis, 0.951972, XAxis, 1.0, YAxis);
+ body5.setQToFitRotation(state, r);
+ system.realize(state, Stage::Acceleration);
+ matter.calcMobilizerReactionForces(state, reaction);
+ assertEqual(~body1.getBodyTransform(state).R()*reaction[body1.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 0), Vec3(99.121319, 139.500095, 95.065409)), 1e-5);
+ assertEqual(~body2.getBodyTransform(state).R()*reaction[body2.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 0), Vec3(15.359115, 55.876994, 22.508078)), 1e-5);
+ assertEqual(~body3.getBodyTransform(state).R()*reaction[body3.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 0), Vec3(15.696393, 65.002065, 13.133021)), 1e-5);
+ assertEqual(~body4.getBodyTransform(state).R()*reaction[body4.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 0), Vec3(-6.262023, 32.714510, -9.770708)), 1e-5);
+ assertEqual(~body5.getBodyTransform(state).R()*reaction[body5.getMobilizedBodyIndex()], SpatialVec(Vec3(0, 0, 0), Vec3(10.471620, 0.963822, -4.640161)), 1e-5);
+}
+
+// Create a free body in space and apply some forces to it.
+// As long as we don't apply mobility forces, the reaction force
+// in the mobilizer should be zero.
+// It is important to do this with a full inertia, offset com,
+// non-unit mass, twisted frames, non-zero velocities, etc.
+
+const Real d = 1.5; // length (m)
+const Real mass = 2; // kg
+const Transform X_GF(Rotation(Pi/3, Vec3(.1,-.3,.3)), Vec3(-4,-5,-1));
+const Transform X_BM(Rotation(-Pi/10, Vec3(7,5,3)), Vec3(0,d,0));
+
+void testFreeMobilizer() {
+ MultibodySystem forward;
+ SimbodyMatterSubsystem fwdMatter(forward);
+ GeneralForceSubsystem fwdForces(forward);
+ Force::UniformGravity(fwdForces, fwdMatter, Vec3(0, -1, 0));
+
+ const Vec3 com(1,2,3);
+ const UnitInertia centralGyration(1, 1.5, 2, .1, .2, .3);
+ Body::Rigid body(MassProperties(mass, com, mass*centralGyration.shiftFromMassCenter(com, 1)));
+
+ MobilizedBody::Free fwdA (fwdMatter.Ground(), X_GF, body, X_BM);
+
+ Force::ConstantForce(fwdForces, fwdA, Vec3(-1,.27,4), Vec3(5,.6,-1));
+ Force::ConstantTorque(fwdForces, fwdA, Vec3(-5.5,1.6,-1.1));
+
+ State fwdState = forward.realizeTopology();
+ fwdA.setQToFitTransform(fwdState, Transform(Rotation(Pi/9,Vec3(-1.8,4,2.2)), Vec3(.1,.2,.7)));
+
+ forward.realize (fwdState, Stage::Position);
+
+ fwdA.setUToFitVelocity(fwdState, SpatialVec(Vec3(.99,2,4), Vec3(-1.2,4,.000333)));
+ forward.realize (fwdState, Stage::Velocity);
+ forward.realize (fwdState, Stage::Acceleration);
+
+ Vector_<SpatialVec> fwdReac;
+ fwdMatter.calcMobilizerReactionForces(fwdState, fwdReac);
+
+ // We expect no reaction from a Free joint.
+ assertEqual(fwdReac[0], SpatialVec(Vec3(0)));
+ assertEqual(fwdReac[1], SpatialVec(Vec3(0)));
+}
+
+int main() {
+ SimTK_START_TEST("TestMobilizerReactionForces");
+ SimTK_SUBTEST(testByComparingToConstraints);
+ SimTK_SUBTEST(testByComparingToConstraints2);
+ SimTK_SUBTEST(testByComparingToSDFAST);
+ SimTK_SUBTEST(testByComparingToSDFAST2);
+ SimTK_SUBTEST(testByComparingToSDFASTWithConstraint);
+ SimTK_SUBTEST(testFreeMobilizer);
+ SimTK_END_TEST();
+}
diff --git a/Simbody/tests/TestNoseHooverThermostat.cpp b/Simbody/tests/TestNoseHooverThermostat.cpp
new file mode 100644
index 0000000..895e9fa
--- /dev/null
+++ b/Simbody/tests/TestNoseHooverThermostat.cpp
@@ -0,0 +1,288 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Christopher Bruns *
+ * Contributors: Michael Sherman *
+ * *
+ * 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"
+
+// define VISUALIZE for visualisation (for debugging)
+// #define VISUALIZE 1
+
+#include <fstream>
+
+using namespace SimTK;
+using namespace std;
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+
+class HarmonicOscillator
+{
+public:
+
+ class OscillatorReporter : public PeriodicEventReporter
+ {
+ public:
+ mutable int eventCount;
+ mutable Real sumEnergy;
+ mutable Real sumEnergySquared;
+ mutable Real sumVelocity;
+ mutable Real sumAbsVelocity;
+ mutable Real sumVelocitySquared;
+ mutable Real sumPosition;
+ mutable Real sumRMSVelPos;
+
+ OscillatorReporter(HarmonicOscillator& oscillator, Real reportInterval)
+ : PeriodicEventReporter(reportInterval), oscillator(oscillator),
+ eventCount(0), sumEnergy(0.0), sumEnergySquared(0.0),
+ sumVelocity(0.0), sumAbsVelocity(0.0), sumVelocitySquared(0.0),
+ sumPosition(0.0), sumRMSVelPos(0.0)
+ // for Matlab
+ //, phaseOut("phaseOut.txt")
+ {}
+
+ void handleEvent(const State& state) const
+ {
+ // Equilibrate a bit before collecting data
+ if (state.getTime() <= 1)
+ return;
+
+ ++eventCount;
+ oscillator.updSystem().realize(state, Stage::Dynamics);
+
+ //std::ostream& po = phaseOut;
+ //po << state.getTime() << " " << state.getQ()[0] << " " << state.getU()[0] << std::endl;
+
+ Real energy = oscillator.getSystem().calcKineticEnergy(state);
+ sumEnergy += energy;
+ sumEnergySquared += energy*energy;
+
+ Real position = oscillator.getPosition(state);
+ sumPosition += position;
+
+ Real velocity = oscillator.getVelocity(state);
+ sumVelocity += velocity;
+ sumAbsVelocity += std::abs(velocity);
+ sumVelocitySquared += velocity*velocity;
+
+ sumRMSVelPos += std::sqrt(position*position * velocity*velocity);
+
+ // oscillator.savedPositions.push_back(oscillator.getPosition(state));
+ // oscillator.savedVelocities.push_back(oscillator.getVelocity(state));
+ }
+ private:
+ HarmonicOscillator& oscillator;
+
+ //mutable std::ofstream phaseOut;
+ };
+
+ HarmonicOscillator() :
+ system(), matter(system), forces(system), mass(1.0)
+ {
+ Vec3 station(0.0);
+
+ // Use a slider to constrain the oscillator to one dimension of motion
+ MobilizedBody::Slider body (
+ matter.updGround(),
+ Body::Rigid(MassProperties(mass, station, Inertia(1))) );
+ body.setDefaultLength(-2.0); // initial position at -2
+ sliderIndex = body.getMobilizedBodyIndex();
+
+ // Unit spring constant to match example in Frenkel and Smit
+ Force::TwoPointLinearSpring(forces,
+ matter.getGround(), Vec3(0),
+ body, Vec3(0),
+ 1.0, 0.0);
+ }
+
+ void simulate() {
+ // View in Visualizer - for test development only
+#ifdef VISUALIZE
+ Visualizer viz(system);
+ viz.setBackgroundType(Visualizer::SolidColor);
+ Visualizer::Reporter* vizrep = new Visualizer::Reporter(viz, 0.2);
+ system.addEventReporter(vizrep);
+#endif
+
+ reporter = new OscillatorReporter(*this, 0.1);
+ system.addEventReporter(reporter);
+
+ State state = system.realizeTopology();
+ Random::Uniform rand(-1,1);
+ //state.updQ()[0] = rand.getValue();
+ //state.updU()[0] = rand.getValue();
+
+ // Simulate it.
+
+ VerletIntegrator integ(system);
+ //RungeKuttaMersonIntegrator integ(system);
+ //integ.setAccuracy(0.01);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(150.0);
+ }
+
+ void assertTemperature(Real temperature) const
+ {
+ // ensure we collected some data
+ ASSERT(reporter->eventCount > 100);
+
+ int degreesOfFreedom = 1;
+
+ // Sanity checks
+
+ // Mean position should be zero
+ Real expectedMeanPosition = 0.0;
+ Real measuredMeanPosition = reporter->sumPosition / reporter->eventCount;
+ ASSERT(std::abs(expectedMeanPosition - measuredMeanPosition) < 0.2);
+
+ // Mean velocity should be zero
+ Real expectedMeanVelocity = 0.0;
+ Real measuredMeanVelocity = reporter->sumVelocity / reporter->eventCount;
+ ASSERT(std::abs(expectedMeanVelocity - measuredMeanVelocity) < 0.2);
+
+ // Check temperature
+ Real measuredMeanEnergy = reporter->sumEnergy/reporter->eventCount;
+ Real expectedMeanEnergy = degreesOfFreedom * 0.5 * SimTK_BOLTZMANN_CONSTANT_MD * temperature; // kT/2 per degree of freedom
+ ASSERT(std::abs(1.0 - measuredMeanEnergy/expectedMeanEnergy) < 0.2);
+
+ // Boltzmann distribution stuff
+
+ // Mean squared velocity should be dof*kT/mass
+ // Boltzmann distribution
+ Real expectedMeanVelocitySquared =
+ degreesOfFreedom * SimTK_BOLTZMANN_CONSTANT_MD * temperature / mass;
+ Real measuredMeanVelocitySquared =
+ reporter->sumVelocitySquared / reporter->eventCount;
+ ASSERT(std::abs(1.0 - measuredMeanVelocitySquared/expectedMeanVelocitySquared) < 0.2);
+
+ // TODO: check this formula
+ // Mean absolute velocity should be (8*v2bar/3PI)^1/2
+ Real expectedMeanAbsVelocity = std::sqrt(
+ 8.0 * expectedMeanVelocitySquared / (degreesOfFreedom * SimTK_PI) );
+ Real measuredMeanAbsVelocity =
+ reporter->sumAbsVelocity / reporter->eventCount;
+ // ASSERT(std::abs(1.0 - measuredMeanAbsVelocity/expectedMeanAbsVelocity) < 0.2);
+ }
+
+ MultibodySystem& updSystem() {return system;}
+ const MultibodySystem& getSystem() const {return system;}
+
+ SimbodyMatterSubsystem& updMatterSubsystem() {return matter;}
+ const SimbodyMatterSubsystem& getMatterSubsystem() const {return matter;}
+
+ GeneralForceSubsystem& updForceSubsystem() {return forces;}
+ const GeneralForceSubsystem& getForceSubsystem() const {return forces;}
+
+ // slider coordinate
+ Real getPosition(const State& state) const {
+ const MobilizedBody::Slider& slider = MobilizedBody::Slider::downcast( matter.getMobilizedBody(sliderIndex) );
+
+ return slider.getLength(state);
+ }
+
+ Real getVelocity(const State& state) const {
+ const MobilizedBody::Slider& slider = MobilizedBody::Slider::downcast( matter.getMobilizedBody(sliderIndex) );
+
+ return slider.getRate(state);
+ }
+
+ Real getTime(const State& state) const {
+ return state.getTime();
+ }
+
+ // std::vector<Real> savedVelocities;
+ // std::vector<Real> savedPositions;
+
+private:
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter;
+ GeneralForceSubsystem forces;
+
+ Real mass;
+ MobilizedBodyIndex sliderIndex;
+
+ OscillatorReporter* reporter;
+};
+
+
+// Case study 12, page 155 in
+// Understanding Molecular Simulation: From Algorithms to Applications
+// Frenkel and Smit
+void testHarmonicOscillatorNoThermostat()
+{
+ HarmonicOscillator oscillator;
+ oscillator.simulate();
+}
+
+void testNoseHooverConstructorSmoke()
+{
+ HarmonicOscillator oscillator;
+ GeneralForceSubsystem& forces = oscillator.updForceSubsystem();
+ Force::Thermostat(forces, oscillator.getMatterSubsystem(),
+ SimTK_BOLTZMANN_CONSTANT_MD, 300, .1);
+ oscillator.simulate();
+}
+
+void testOscillatorTemperature(Real temperature, int nChains=-1)
+{
+ HarmonicOscillator oscillator;
+ GeneralForceSubsystem& forces = oscillator.updForceSubsystem();
+ Force::Thermostat nhc(forces, oscillator.getMatterSubsystem(),
+ SimTK_BOLTZMANN_CONSTANT_MD, temperature, 0.1);
+ if (nChains > 0)
+ nhc.setDefaultNumChains(nChains);
+ oscillator.simulate();
+ oscillator.assertTemperature(temperature);
+}
+
+
+int main()
+{
+
+ // Several tests commented out to lessen burden on nightly build tests
+
+ //cout << "testHarmonicOscillatorNoThermostat" << endl;
+ //testHarmonicOscillatorNoThermostat();
+
+ //cout << "testNoseHooverConstructorSmoke" << endl;
+ //testNoseHooverConstructorSmoke();
+
+ cout << "oscillator 100K" << endl;
+ testOscillatorTemperature(100); // use default #chains
+
+ //cout << "oscillator 300K" << endl;
+ //testOscillatorTemperature(300.0);
+
+ //cout << "oscillator 5000K" << endl;
+ //testOscillatorTemperature(5000.0);
+
+ //cout << "argon 10K" << endl;
+ //testArgonTemperature(10.0);
+
+ //cout << "argon 300K" << endl;
+ //testArgonTemperature(300.0);
+
+ //cout << "argon 5000K" << endl;
+ //testArgonTemperature(5000.0);
+
+ return 0;
+}
diff --git a/Simbody/tests/TestObservedPointFitter.cpp b/Simbody/tests/TestObservedPointFitter.cpp
new file mode 100644
index 0000000..4aed10d
--- /dev/null
+++ b/Simbody/tests/TestObservedPointFitter.cpp
@@ -0,0 +1,208 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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"
+#include "SimTKcommon/Testing.h"
+
+#include <vector>
+#include <map>
+
+using namespace SimTK;
+using namespace std;
+
+#define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
+
+static const int NUM_BODIES = 10;
+static const Real BOND_LENGTH = 0.5;
+static const int ITERATIONS = 4;
+static const Real TOL = 1e-4;
+
+bool testFitting
+ (const MultibodySystem& mbs, State& state,
+ const vector<MobilizedBodyIndex>& bodyIxs,
+ const vector<vector<Vec3> >& stations,
+ const vector<vector<Vec3> >& targetLocations,
+ Real minError, Real maxError, Real endDistance)
+{
+ // Find the best fit.
+
+ Real reportedError = ObservedPointFitter::findBestFit(mbs, state, bodyIxs, stations, targetLocations, TOL);
+ cout << "[min,max]=" << minError << "," << maxError << " actual=" << reportedError << endl;
+ bool result = (reportedError <= maxError && reportedError >= minError);
+
+ // Verify that the error was calculated correctly.
+
+ Real error = 0.0;
+ int numStations = 0;
+ mbs.realize(state, Stage::Position);
+ const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
+ for (int i = 0; i < (int) bodyIxs.size(); ++i) {
+ MobilizedBodyIndex id = bodyIxs[i];
+ numStations += (int)stations[i].size();
+ for (int j = 0; j < (int) stations[i].size(); ++j)
+ error += (targetLocations[i][j]-matter.getMobilizedBody(id).getBodyTransform(state)*stations[i][j]).normSqr();
+ }
+ error = std::sqrt(error/numStations);
+ cout << "calc wrms=" << error << endl;
+ ASSERT(std::abs(1.0-error/reportedError) < 0.0001); // should match to machine precision
+
+ if (endDistance >= 0) {
+ // Verify that the ends are the correct distance apart.
+ Real distance = (matter.getMobilizedBody(bodyIxs[0]).getBodyOriginLocation(state)-matter.getMobilizedBody(bodyIxs[bodyIxs.size()-1]).getBodyOriginLocation(state)).norm();
+ cout << "required dist=" << endDistance << ", actual=" << distance << endl;
+ ASSERT(std::abs(1.0-endDistance/distance) < TOL);
+ }
+
+ return result;
+}
+
+
+static void testObservedPointFitter(bool useConstraint) {
+ int failures = 0;
+ for (int iter = 0; iter < ITERATIONS; ++iter) {
+
+ // Build a system consisting of a chain of bodies with occasional side chains, and
+ // a variety of mobilizers.
+
+ MultibodySystem mbs;
+ SimbodyMatterSubsystem matter(mbs);
+ Body::Rigid body = Body::Rigid(MassProperties(1, Vec3(0), Inertia(1)));
+ body.addDecoration(Transform(), DecorativeSphere(.1));
+ MobilizedBody* lastBody = &matter.Ground();
+ MobilizedBody* lastMainChainBody = &matter.Ground();
+ vector<MobilizedBody*> bodies;
+ Random::Uniform random(0.0, 1.0);
+ random.setSeed(iter);
+
+ for (int i = 0; i < NUM_BODIES; ++i) {
+ bool mainChain = random.getValue() < 0.5;
+ MobilizedBody* parent = (mainChain ? lastMainChainBody : lastBody);
+ int type = (int) (random.getValue()*4);
+ MobilizedBody* nextBody;
+ if (type == 0) {
+ MobilizedBody::Cylinder cylinder(*parent, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(0, BOND_LENGTH, 0)));
+ nextBody = &matter.updMobilizedBody(cylinder.getMobilizedBodyIndex());
+ }
+ else if (type == 1) {
+ MobilizedBody::Slider slider(*parent, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(0, BOND_LENGTH, 0)));
+ nextBody = &matter.updMobilizedBody(slider.getMobilizedBodyIndex());
+ }
+ else if (type == 2) {
+ MobilizedBody::Ball ball(*parent, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(0, BOND_LENGTH, 0)));
+ nextBody = &matter.updMobilizedBody(ball.getMobilizedBodyIndex());
+ }
+ else {
+ MobilizedBody::Pin pin(*parent, Transform(Vec3(0, 0, 0)), body, Transform(Vec3(0, BOND_LENGTH, 0)));
+ nextBody = &matter.updMobilizedBody(pin.getMobilizedBodyIndex());
+ }
+ bodies.push_back(nextBody);
+ if (mainChain)
+ lastMainChainBody = nextBody;
+ lastBody = nextBody;
+ }
+ mbs.realizeTopology();
+ State s = mbs.getDefaultState();
+ matter.setUseEulerAngles(s, true);
+ mbs.realizeModel(s);
+
+ // Choose a random initial conformation.
+
+ vector<Real> targetQ(s.getNQ(), Real(0));
+ for (MobilizedBodyIndex mbx(1); mbx < matter.getNumBodies(); ++mbx) {
+ const MobilizedBody& mobod = matter.getMobilizedBody(mbx);
+ for (int i = 0; i < mobod.getNumQ(s); ++i) {
+ const QIndex qx0 = mobod.getFirstQIndex(s);
+ s.updQ()[qx0+i] = targetQ[qx0+i] = 2.0*random.getValue();
+ }
+ }
+ //cout << "q0=" << s.getQ() << endl;
+ mbs.realize(s, Stage::Position);
+
+ // Select some random stations on each body.
+
+ vector<vector<Vec3> > stations(NUM_BODIES);
+ vector<vector<Vec3> > targetLocations(NUM_BODIES);
+ vector<MobilizedBodyIndex> bodyIxs;
+ for (int i = 0; i < NUM_BODIES; ++i) {
+ MobilizedBodyIndex id = bodies[i]->getMobilizedBodyIndex();
+ bodyIxs.push_back(id);
+ int numStations = 1 + (int) (random.getValue()*4);
+ for (int j = 0; j < numStations; ++j) {
+ Vec3 pos(2.0*random.getValue()-1.0, 2.0*random.getValue()-1.0, 2.0*random.getValue()-1.0);
+ stations[i].push_back(pos);
+ targetLocations[i].push_back(bodies[i]->getBodyTransform(s)*pos);
+ }
+ }
+
+ Real distance = -1;
+ if (useConstraint) {
+ // Add a constraint fixing the distance between the first and last bodies.
+ Real distance = (bodies[0]->getBodyOriginLocation(s)-bodies[NUM_BODIES-1]->getBodyOriginLocation(s)).norm();
+ Constraint::Rod(*bodies[0], Vec3(0), *bodies[NUM_BODIES-1], Vec3(0), distance);
+ }
+ s = mbs.realizeTopology();
+ matter.setUseEulerAngles(s, true);
+ mbs.realizeModel(s);
+
+ // Try fitting it.
+ State initState = s;
+ if (!testFitting(mbs, s, bodyIxs, stations, targetLocations, 0.0, 0.02, distance))
+ failures++;
+
+ //cout << "q1=" << s.getQ() << endl;
+
+ // Now add random noise to the target locations, and see if it can still fit decently.
+
+ Random::Gaussian gaussian(0.0, 0.15);
+ for (int i = 0; i < (int) targetLocations.size(); ++i) {
+ for (int j = 0; j < (int) targetLocations[i].size(); ++j) {
+ targetLocations[i][j] += Vec3(gaussian.getValue(), gaussian.getValue(), gaussian.getValue());
+ }
+ }
+
+ s = initState; // start from same config as before
+ if (!testFitting(mbs, s, bodyIxs, stations, targetLocations, 0.1, 0.5, distance))
+ failures++;
+
+ //cout << "q2=" << s.getQ() << endl;
+
+ }
+
+ ASSERT(failures == 0); // It found a reasonable fit every time.
+ std::cout << "Done" << std::endl;
+}
+
+static void testUnconstrained() {
+ testObservedPointFitter(false);
+}
+
+static void testConstrained() {
+ testObservedPointFitter(true);
+}
+
+int main() {
+ SimTK_START_TEST("TestObservedPointFitter");
+ SimTK_SUBTEST(testUnconstrained);
+ SimTK_SUBTEST(testConstrained);
+ SimTK_END_TEST();
+}
diff --git a/Simbody/tests/TestOrientedBoundingBox.cpp b/Simbody/tests/TestOrientedBoundingBox.cpp
new file mode 100644
index 0000000..ff5685e
--- /dev/null
+++ b/Simbody/tests/TestOrientedBoundingBox.cpp
@@ -0,0 +1,257 @@
+/* -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: *
+ * *
+ * 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 <class T>
+void assertEqual(T val1, T val2) {
+ ASSERT(abs(val1-val2) < TOL);
+}
+
+template <int N>
+void assertEqual(Vec<N> val1, Vec<N> val2) {
+ for (int i = 0; i < N; ++i)
+ ASSERT(abs(val1[i]-val2[i]) < TOL);
+}
+
+void testContainsPoint() {
+ OrientedBoundingBox box(Vec3(0), Vec3(1, 2, 3));
+ ASSERT(box.containsPoint(Vec3(0)));
+ ASSERT(box.containsPoint(Vec3(1, 2, 3)));
+ ASSERT(box.containsPoint(Vec3(0.5, 1.0, 1.5)));
+ ASSERT(!box.containsPoint(Vec3(-0.5, 1.0, 1.5)));
+ ASSERT(!box.containsPoint(Vec3(0.5, -1.0, 1.5)));
+ ASSERT(!box.containsPoint(Vec3(0.5, 1.0, -1.5)));
+ ASSERT(!box.containsPoint(Vec3(1.01, 1.0, 1.5)));
+ ASSERT(!box.containsPoint(Vec3(0.5, 2.01, 1.5)));
+ ASSERT(!box.containsPoint(Vec3(0.5, 1.0, 3.01)));
+ box = OrientedBoundingBox(Vec3(3, 2, 1), Vec3(1, 2, 3));
+ ASSERT(box.containsPoint(Vec3(3, 2, 1)));
+ ASSERT(box.containsPoint(Vec3(4, 4, 4)));
+ ASSERT(box.containsPoint(Vec3(3.5, 3.0, 2.5)));
+ ASSERT(!box.containsPoint(Vec3(2.5, 3.0, 2.5)));
+ ASSERT(!box.containsPoint(Vec3(4.5, 1.0, 2.5)));
+ ASSERT(!box.containsPoint(Vec3(4.5, 3.0, -0.5)));
+ ASSERT(!box.containsPoint(Vec3(5.01, 3.0, 2.5)));
+ ASSERT(!box.containsPoint(Vec3(3.5, 4.01, 2.5)));
+ ASSERT(!box.containsPoint(Vec3(3.5, 3.0, 4.01)));
+ box = OrientedBoundingBox(Rotation(0.25*Pi, ZAxis), Vec3(1, 1, 1));
+ ASSERT(box.containsPoint(Vec3(0)));
+ ASSERT(box.containsPoint(Vec3(0, Sqrt2-1e-10, 0)));
+ ASSERT(!box.containsPoint(Vec3(0, Sqrt2+1e-10, 0)));
+ ASSERT(box.containsPoint(Vec3(0, Sqrt2-1e-10, 1e-10)));
+ ASSERT(!box.containsPoint(Vec3(0, Sqrt2-1e-10, -1e-10)));
+}
+
+void verifyCorners(Vec3 expected[8], Vec3 found[8]) {
+ for (int i = 0; i < 8; i++) {
+ bool match = false;
+ for (int j = 0; j < 8 && !match; j++) {
+ if (abs(expected[i][0]-found[j][0]) < TOL && abs(expected[i][1]-found[j][1]) < TOL && abs(expected[i][2]-found[j][2]) < TOL) {
+ match = true;
+ break;
+ }
+ }
+ ASSERT(match);
+ }
+}
+
+void testGetCorners() {
+ Vec3 corners[8];
+ OrientedBoundingBox(Vec3(0), Vec3(1, 2, 3)).getCorners(corners);
+ {
+ Vec3 expected[] = {Vec3(0), Vec3(1, 0, 0), Vec3(0, 2, 0), Vec3(1, 2, 0), Vec3(0, 0, 3), Vec3(1, 0, 3), Vec3(0, 2, 3), Vec3(1, 2, 3)};
+ verifyCorners(expected, corners);
+ }
+ OrientedBoundingBox(Vec3(3, 2, 1), Vec3(1, 2, 3)).getCorners(corners);
+ {
+ Vec3 expected[] = {Vec3(3, 2, 1), Vec3(4, 2, 1), Vec3(3, 4, 1), Vec3(4, 4, 1), Vec3(3, 2, 4), Vec3(4, 2, 4), Vec3(3, 4, 4), Vec3(4, 4, 4)};
+ verifyCorners(expected, corners);
+ }
+ OrientedBoundingBox(Rotation(0.25*Pi, ZAxis), Vec3(1, 1, 1)).getCorners(corners);
+ {
+ Real d = 0.5*Sqrt2;
+ Vec3 expected[] = {Vec3(0), Vec3(-d, d, 0), Vec3(d, d, 0), Vec3(0, Sqrt2, 0), Vec3(0, 0, 1), Vec3(-d, d, 1), Vec3(d, d, 1), Vec3(0, Sqrt2, 1)};
+ verifyCorners(expected, corners);
+ }
+}
+
+void verifyBoxIntersection(bool shouldIntersect, OrientedBoundingBox box1, OrientedBoundingBox box2) {
+ ASSERT(box1.intersectsBox(box2) == shouldIntersect);
+ ASSERT(box2.intersectsBox(box1) == shouldIntersect);
+}
+
+void testIntersectsBox() {
+ // Try boxes with identical orientations.
+
+ verifyBoxIntersection(true, OrientedBoundingBox(Vec3(0), Vec3(1, 2, 3)), OrientedBoundingBox(Vec3(0), Vec3(3, 2, 1)));
+ verifyBoxIntersection(true, OrientedBoundingBox(Vec3(0), Vec3(1, 2, 3)), OrientedBoundingBox(Vec3(1-1e-10, 2-1e-10, 3-1e-10), Vec3(3, 2, 1)));
+ verifyBoxIntersection(false, OrientedBoundingBox(Vec3(0), Vec3(1, 2, 3)), OrientedBoundingBox(Vec3(1+1e-10, 2-1e-10, 3-1e-10), Vec3(3, 2, 1)));
+ verifyBoxIntersection(false, OrientedBoundingBox(Vec3(0), Vec3(1, 2, 3)), OrientedBoundingBox(Vec3(1-1e-10, 2+1e-10, 3-1e-10), Vec3(3, 2, 1)));
+ verifyBoxIntersection(false, OrientedBoundingBox(Vec3(0), Vec3(1, 2, 3)), OrientedBoundingBox(Vec3(1-1e-10, 2-1e-10, 3+1e-10), Vec3(3, 2, 1)));
+ verifyBoxIntersection(true, OrientedBoundingBox(Vec3(0), Vec3(1, 2, 3)), OrientedBoundingBox(Vec3(-3+1e-10, -2+1e-10, -1+1e-10), Vec3(3, 2, 1)));
+ verifyBoxIntersection(false, OrientedBoundingBox(Vec3(0), Vec3(1, 2, 3)), OrientedBoundingBox(Vec3(-3-1e-10, -2+1e-10, -1+1e-10), Vec3(3, 2, 1)));
+ verifyBoxIntersection(false, OrientedBoundingBox(Vec3(0), Vec3(1, 2, 3)), OrientedBoundingBox(Vec3(-3+1e-10, -2-1e-10, -1+1e-10), Vec3(3, 2, 1)));
+ verifyBoxIntersection(false, OrientedBoundingBox(Vec3(0), Vec3(1, 2, 3)), OrientedBoundingBox(Vec3(-3+1e-10, -2+1e-10, -1-1e-10), Vec3(3, 2, 1)));
+
+ // Try some rotations by 90 degrees.
+
+ verifyBoxIntersection(true, OrientedBoundingBox(Vec3(-0.5, 0, 0), Vec3(1, 2, 3)), OrientedBoundingBox(Rotation(0.5*Pi, ZAxis), Vec3(3, 2, 1)));
+ verifyBoxIntersection(true, OrientedBoundingBox(Vec3(-0.5, -2+1e-10, 0), Vec3(1, 2, 3)), OrientedBoundingBox(Rotation(0.5*Pi, ZAxis), Vec3(3, 2, 1)));
+ verifyBoxIntersection(false, OrientedBoundingBox(Vec3(-0.5, -2-1e-10, 0), Vec3(1, 2, 3)), OrientedBoundingBox(Rotation(0.5*Pi, ZAxis), Vec3(3, 2, 1)));
+ verifyBoxIntersection(true, OrientedBoundingBox(Vec3(-0.5, 3-1e-10, 0), Vec3(1, 2, 3)), OrientedBoundingBox(Rotation(0.5*Pi, ZAxis), Vec3(3, 2, 1)));
+ verifyBoxIntersection(false, OrientedBoundingBox(Vec3(-0.5, 3+1e-10, 0), Vec3(1, 2, 3)), OrientedBoundingBox(Rotation(0.5*Pi, ZAxis), Vec3(3, 2, 1)));
+ verifyBoxIntersection(true, OrientedBoundingBox(Rotation(0.5*Pi, ZAxis), Vec3(1, 2, 3)), OrientedBoundingBox(Rotation(0.5*Pi, ZAxis), Vec3(3, 2, 1)));
+
+ // Try rotations by 45 degrees.
+
+ const Real d = 0.5*Sqrt2;
+ verifyBoxIntersection(true, OrientedBoundingBox(Vec3(-0.5, 0, 0), Vec3(1, 1, 1)), OrientedBoundingBox(Rotation(0.25*Pi, ZAxis), Vec3(1, 1, 1)));
+ verifyBoxIntersection(true, OrientedBoundingBox(Vec3(-0.5, -1+1e-10, 0), Vec3(1, 1, 1)), OrientedBoundingBox(Rotation(0.25*Pi, ZAxis), Vec3(1, 1, 1)));
+ verifyBoxIntersection(false, OrientedBoundingBox(Vec3(-0.5, -1-1e-10, 0), Vec3(1, 1, 1)), OrientedBoundingBox(Rotation(0.25*Pi, ZAxis), Vec3(1, 1, 1)));
+ verifyBoxIntersection(true, OrientedBoundingBox(Vec3(-0.5, Sqrt2-1e-10, 0), Vec3(1, 1, 1)), OrientedBoundingBox(Rotation(0.25*Pi, ZAxis), Vec3(1, 1, 1)));
+ verifyBoxIntersection(false, OrientedBoundingBox(Vec3(-0.5, Sqrt2+1e-10, 0), Vec3(1, 1, 1)), OrientedBoundingBox(Rotation(0.25*Pi, ZAxis), Vec3(1, 1, 1)));
+ verifyBoxIntersection(true, OrientedBoundingBox(Vec3(d-1e-10, 0, 0), Vec3(1, 1, 1)), OrientedBoundingBox(Rotation(0.25*Pi, ZAxis), Vec3(1, 1, 1)));
+ verifyBoxIntersection(false, OrientedBoundingBox(Vec3(d+1e-10, 0, 0), Vec3(1, 1, 1)), OrientedBoundingBox(Rotation(0.25*Pi, ZAxis), Vec3(1, 1, 1)));
+ verifyBoxIntersection(true, OrientedBoundingBox(Vec3(-1-d+1e-10, 0, 0), Vec3(1, 1, 1)), OrientedBoundingBox(Rotation(0.25*Pi, ZAxis), Vec3(1, 1, 1)));
+ verifyBoxIntersection(false, OrientedBoundingBox(Vec3(-1-d-1e-10, 0, 0), Vec3(1, 1, 1)), OrientedBoundingBox(Rotation(0.25*Pi, ZAxis), Vec3(1, 1, 1)));
+ verifyBoxIntersection(true, OrientedBoundingBox(Vec3(0, 0, d-1e-10), Vec3(1, 1, 1)), OrientedBoundingBox(Rotation(-0.25*Pi, XAxis), Vec3(1, 1, 1)));
+ verifyBoxIntersection(false, OrientedBoundingBox(Vec3(0, 0, d+1e-10), Vec3(1, 1, 1)), OrientedBoundingBox(Rotation(-0.25*Pi, XAxis), Vec3(1, 1, 1)));
+ verifyBoxIntersection(true, OrientedBoundingBox(Vec3(0, 0, -1-d+1e-10), Vec3(1, 1, 1)), OrientedBoundingBox(Rotation(-0.25*Pi, XAxis), Vec3(1, 1, 1)));
+ verifyBoxIntersection(false, OrientedBoundingBox(Vec3(0, 0, -1-d-1e-10), Vec3(1, 1, 1)), OrientedBoundingBox(Rotation(-0.25*Pi, XAxis), Vec3(1, 1, 1)));
+
+ // The following case detects a bug that came up in a different test case.
+
+ Rotation r;
+ r.setRotationToBodyFixedXYZ(Vec3(Pi/2, 0, -1.1));;
+ verifyBoxIntersection(true, OrientedBoundingBox(Transform(r, Vec3(-0.95, 1.1, 2.5)), Vec3(2e-10, 1.118, 1)), OrientedBoundingBox(Vec3(0, -50, -50), Vec3(100, 100, 100)));
+}
+
+void verifyRayIntersection(const OrientedBoundingBox& box, const Vec3& origin, const UnitVec3& direction, bool shouldIntersect, Real expectedDistance) {
+ Real distance;
+ ASSERT(shouldIntersect == box.intersectsRay(origin, direction, distance));
+ assertEqual(expectedDistance, distance);
+}
+
+void testIntersectsRay() {
+ // Try rays starting inside the box.
+
+ verifyRayIntersection(OrientedBoundingBox(Transform(), Vec3(1, 2, 3)), Vec3(0.5, 0.5, 0.5), UnitVec3(1, 0, 0), true, 0);
+ verifyRayIntersection(OrientedBoundingBox(Transform(), Vec3(1, 2, 3)), Vec3(0.5, 0.5, 0.5), UnitVec3(0, 1, 0), true, 0);
+ verifyRayIntersection(OrientedBoundingBox(Transform(), Vec3(1, 2, 3)), Vec3(0.5, 0.5, 0.5), UnitVec3(0, 0, 1), true, 0);
+
+ // Try rays that hit it on various sides.
+
+ verifyRayIntersection(OrientedBoundingBox(Transform(), Vec3(1, 2, 3)), Vec3(-1.5, 0.5, 0.5), UnitVec3(1, 0, 0), true, 1.5);
+ verifyRayIntersection(OrientedBoundingBox(Transform(), Vec3(1, 2, 3)), Vec3(2.5, 0.5, 0.5), UnitVec3(-1, 0, 0), true, 1.5);
+ verifyRayIntersection(OrientedBoundingBox(Transform(), Vec3(1, 2, 3)), Vec3(0.5, -0.5, 0.5), UnitVec3(0, 1, 0), true, 0.5);
+ verifyRayIntersection(OrientedBoundingBox(Transform(), Vec3(1, 2, 3)), Vec3(0.5, 2.5, 0.5), UnitVec3(0, -1, 0), true, 0.5);
+ verifyRayIntersection(OrientedBoundingBox(Transform(), Vec3(1, 2, 3)), Vec3(0.5, 0.5, -1.0), UnitVec3(0, 0, 1), true, 1.0);
+ verifyRayIntersection(OrientedBoundingBox(Transform(), Vec3(1, 2, 3)), Vec3(0.5, 0.5, 4.0), UnitVec3(0, 0, -1), true, 1.0);
+
+ // Try a box at an angle.
+
+ verifyRayIntersection(OrientedBoundingBox(Rotation(-0.25*Pi, ZAxis), Vec3(2, 2, 2)), Vec3(0, 0, 0.5), UnitVec3(1, 0, 0), true, 0);
+ verifyRayIntersection(OrientedBoundingBox(Rotation(-0.25*Pi, ZAxis), Vec3(2, 2, 2)), Vec3(-1, 0, 0.5), UnitVec3(1, 0, 0), true, 1.0);
+}
+
+void testCreateFromPoints() {
+ Random::Uniform random(0, 1);
+ for (int trial = 0; trial < 100; trial++) {
+ // Select a volume in which to generate points.
+
+ Vec3 size(10*random.getValue(), 10*random.getValue(), 10*random.getValue());
+ Rotation rotation;
+ rotation.setRotationToBodyFixedXYZ(Vec3(random.getValue(), random.getValue(), random.getValue()));
+ Transform transform(rotation, Vec3(10*random.getValue(), 10*random.getValue(), 10*random.getValue()));
+
+ // Create a set of points inside it.
+
+ int numPoints = (int)(50*random.getValue()+1);
+ Vector_<Vec3> points(numPoints);
+ for (int i = 0; i < numPoints; i++)
+ points[i] = transform*Vec3(size[0]*random.getValue(), size[1]*random.getValue(), size[2]*random.getValue());
+
+ // Create a bounding box from them.
+
+ OrientedBoundingBox box(points);
+
+ // Verify that it contains all the points.
+
+ for (int i = 0; i < numPoints; i++) {
+ ASSERT(box.containsPoint(points[i]));
+ }
+
+ // Verify that it gives a reasonably tight fit to them.
+
+ Real expectedVolume = size[0]*size[1]*size[2];
+ Real volume = box.getSize()[0]*box.getSize()[1]*box.getSize()[2];
+ ASSERT(volume < 1.5*expectedVolume);
+ }
+}
+
+void testFindNearestPoint() {
+ Vec3 size(1, 1.5, 3);
+ Transform trans(Rotation(0.3, XAxis), Vec3(1, 2, 0.5));
+ OrientedBoundingBox box(trans, size);
+
+ // First test some points inside the box.
+
+ Random::Uniform random(0, 1);
+ for (int i = 0; i < 100; i++) {
+ Vec3 p(random.getValue()*size[0], random.getValue()*size[1], random.getValue()*size[2]);
+ p = trans*p;
+ assertEqual(p, box.findNearestPoint(p));
+ }
+
+ // Try some points outside the box.
+
+ assertEqual(size, ~trans*box.findNearestPoint(trans*(size+Vec3(1, 2, 3))));
+ assertEqual(Vec3(1, 1.5, 0.25), ~trans*box.findNearestPoint(trans*Vec3(2, 3, 0.25)));
+ assertEqual(Vec3(0, 0, 0), ~trans*box.findNearestPoint(trans*Vec3(-1, -1, -2)));
+ assertEqual(Vec3(0, 0, 0.5), ~trans*box.findNearestPoint(trans*Vec3(-1, -1, 0.5)));
+}
+
+int main() {
+ try {
+ testContainsPoint();
+ testGetCorners();
+ testIntersectsBox();
+ testIntersectsRay();
+ testCreateFromPoints();
+ testFindNearestPoint();
+ }
+ catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ cout << "Done" << endl;
+ return 0;
+}
diff --git a/Simbody/tests/TestPimpl1.cpp b/Simbody/tests/TestPimpl1.cpp
new file mode 100755
index 0000000..546fcc3
--- /dev/null
+++ b/Simbody/tests/TestPimpl1.cpp
@@ -0,0 +1,35 @@
+
+#include "SimTKsimbody.h"
+#pragma warning(disable:4661)
+class Element;
+class ElementRep;
+
+//extern template class /*__declspec(dllimport)*/ SimTK::PIMPLHandle<Element, ElementRep>;
+
+
+class /*__declspec(dllexport)*/ Element : public SimTK::PIMPLHandle<Element,ElementRep> {
+public:
+};
+
+
+
+#include "SimTKcommon/internal/PrivateImplementation_Defs.h"
+
+#include <iostream>
+
+int main()
+{
+ std::cout << "passed" << std::endl;
+
+ Element e;
+
+ return 0;
+}
+
+
+class ElementRep : public SimTK::PIMPLImplementation<Element,ElementRep> {
+public:
+ ElementRep* clone() const {return new ElementRep();}
+};
+
+template class SimTK::PIMPLHandle<Element,ElementRep>;
diff --git a/Simbody/tests/TestReverseMobilizers.cpp b/Simbody/tests/TestReverseMobilizers.cpp
new file mode 100644
index 0000000..dc8c6c6
--- /dev/null
+++ b/Simbody/tests/TestReverseMobilizers.cpp
@@ -0,0 +1,676 @@
+/* -------------------------------------------------------------------------- *
+ * 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-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 <class T>
+void assertEqual(T val1, T val2, Real tol) {
+ ASSERT(abs(val1-val2) < tol);
+}
+
+template <int N>
+void assertEqual(Vec<N> val1, Vec<N> val2, Real tol) {
+ for (int i = 0; i < N; ++i)
+ ASSERT(abs(val1[i]-val2[i]) < tol);
+}
+
+template<>
+void assertEqual(SpatialVec val1, SpatialVec val2, Real tol) {
+ assertEqual(val1[0], val2[0], tol);
+ assertEqual(val1[1], val2[1], tol);
+}
+
+template<>
+void assertEqual(Transform val1, Transform val2, Real tol) {
+ assertEqual(val1.p(), val2.p(), tol);
+ assertEqual(val1.R().convertRotationToBodyFixedXYZ(),
+ val2.R().convertRotationToBodyFixedXYZ(), tol);
+}
+
+
+template<class T>
+void assertEqual(Vector_<T> val1, Vector_<T> val2, Real tol) {
+ for (int i=0; i < val1.size(); ++i)
+ assertEqual(val1[i], val2[i], tol);
+}
+
+template <class T>
+void assertEqual(T val1, T val2) {
+ assertEqual(val1, val2, TOL);
+}
+
+
+// System is two free bodies A and B pinned together.
+// The system is Ground (6dof) A (pin) B (6dof) Ground.
+// The forward system is Ground (6dof)-> A (pin)-> B.
+// The reverse system1 is A <-(pin) B <-(6dof) Ground.
+// The reverse system2 is A <-(rpin) B <-(6dof) Ground.
+// Reverse system2's pin joint q and u should have the
+// same meaning as the forward system's.
+//
+// Note: because HDot is zero for both forward & reverse
+// pin joints, this test uses only H reversal and says
+// nothing about HDot reversal!
+
+const Real d = 1.5; // length (m)
+const Real mass = 200000; // kg
+// Mobilizer frame on A (used for both inboard and outboard)
+const Transform X_AM(Rotation(Pi/3, Vec3(.1,-.3,.3)), Vec3(-4,-5,-1));
+// Mobilizer frame on B (used for both inboard and outboard)
+const Transform X_BM(Rotation(-Pi/10, Vec3(7,5,3)), Vec3(0,d,0));
+
+void testPin() {
+ MultibodySystem forward;
+ SimbodyMatterSubsystem fwdMatter(forward);
+ GeneralForceSubsystem fwdForces(forward);
+ Force::UniformGravity(fwdForces, fwdMatter, Vec3(0, -1, 0));
+
+ MultibodySystem reverse1;
+ SimbodyMatterSubsystem rev1Matter(reverse1);
+ GeneralForceSubsystem rev1Forces(reverse1);
+ Force::UniformGravity(rev1Forces, rev1Matter, Vec3(0, -1, 0));
+
+ MultibodySystem reverse2;
+ SimbodyMatterSubsystem rev2Matter(reverse2);
+ GeneralForceSubsystem rev2Forces(reverse2);
+ Force::UniformGravity(rev2Forces, rev2Matter, Vec3(0, -1, 0));
+
+ const Vec3 com(1,2,3);
+ const UnitInertia centralGyration(1, 1.5, 2, .1, .2, .3);
+ Body::Rigid body(MassProperties(mass, com, mass*centralGyration.shiftFromMassCenter(com, 1)));
+
+ MobilizedBody::Free fwdA (fwdMatter.Ground(), Vec3(0), body, X_AM);
+ MobilizedBody::Free rev1B(rev1Matter.Ground(), Vec3(0), body, X_BM);
+ MobilizedBody::Free rev2B(rev2Matter.Ground(), Vec3(0), body, X_BM);
+
+ MobilizedBody::Pin fwdB (fwdA, X_AM, body, X_BM);
+ MobilizedBody::Pin rev1A(rev1B, X_BM, body, X_AM);
+ MobilizedBody::Pin rev2A(rev2B, X_BM, body, X_AM, MobilizedBody::Reverse);
+
+ Force::MobilityConstantForce(fwdForces, fwdB, 0, -34.5);
+ Force::MobilityConstantForce(rev1Forces, rev1A, 0, 34.5);
+ Force::MobilityConstantForce(rev2Forces, rev2A, 0, -34.5); // reversed
+
+
+ State fwdState = forward.realizeTopology();
+ State rev1State = reverse1.realizeTopology();
+ State rev2State = reverse2.realizeTopology();
+
+ // Put body A somewhere arbitrary.
+ fwdA.setQToFitTransform(fwdState, Transform(Rotation(-1.9, Vec3(-3,2,4)),
+ Vec3(-.33, .66, -.99)));
+
+ fwdB.setAngle (fwdState, Pi/4); // 45 degrees
+ rev1A.setAngle(rev1State, -Pi/4);
+ rev2A.setAngle(rev2State, Pi/4);
+
+ // Calculate where body B ended up in the forward system and then
+ // set the reverse Free joint to match so that the whole system
+ // is identically configured.
+
+ forward.realize (fwdState, Stage::Position);
+ const Transform& X_GB = fwdB.getBodyTransform(fwdState);
+ rev1B.setQToFitTransform(rev1State, X_GB*X_BM);
+ rev2B.setQToFitTransform(rev2State, X_GB*X_BM);
+ reverse1.realize(rev1State, Stage::Position);
+ reverse2.realize(rev2State, Stage::Position);
+
+ assertEqual(fwdB.getBodyTransform(fwdState), rev1B.getBodyTransform(rev1State));
+ assertEqual(fwdB.getBodyTransform(fwdState), rev2B.getBodyTransform(rev2State));
+ assertEqual(fwdA.getBodyTransform(fwdState), rev1A.getBodyTransform(rev1State));
+ assertEqual(fwdA.getBodyTransform(fwdState), rev2A.getBodyTransform(rev2State));
+
+ fwdB.setRate (fwdState, 3);
+ forward.realize (fwdState, Stage::Velocity);
+
+ const SpatialVec V_G_BM( fwdB.getBodyAngularVelocity(fwdState),
+ fwdB.findStationVelocityInGround(fwdState, X_BM.p()));
+ rev1B.setUToFitVelocity(rev1State, V_G_BM);
+ rev2B.setUToFitVelocity(rev2State, V_G_BM);
+
+ // Need to construct velocity of A in B from B in A.
+ const Transform X_AB = fwdB.getMobilizerTransform(fwdState);
+ const Transform X_BA = ~X_AB;
+ const SpatialVec V_AB = fwdB.getMobilizerVelocity(fwdState);
+ const SpatialVec V_BA( -X_BA.R()* V_AB[0],
+ -X_BA.R()*(V_AB[1] + X_AB.p() % V_AB[0]) );
+
+ rev1A.setUToFitVelocity(rev1State, V_BA);
+ rev2A.setUToFitVelocity(rev2State, V_BA); // KLUDGE; should be V_BA
+
+ assertEqual(fwdB.getU(fwdState), rev2A.getU(rev2State));
+
+ reverse1.realize(rev1State, Stage::Velocity);
+ reverse2.realize(rev2State, Stage::Velocity);
+
+ cout << "Vels B:\n";
+ cout << " Fwd: " << fwdB.getBodyVelocity(fwdState) << endl;
+ cout << " Rev1: " << rev1B.getBodyVelocity(rev1State)<< endl;
+ cout << " Rev2: " << rev2B.getBodyVelocity(rev2State) << endl;
+
+ assertEqual(fwdB.getBodyVelocity(fwdState), rev1B.getBodyVelocity(rev1State));
+ assertEqual(fwdB.getBodyVelocity(fwdState), rev2B.getBodyVelocity(rev2State));
+
+ cout << "Vels A:\n";
+ cout << " Fwd: " << fwdA.getBodyVelocity(fwdState) << endl;
+ cout << " Rev1: " << rev1A.getBodyVelocity(rev1State)<< endl;
+ cout << " Rev2: " << rev2A.getBodyVelocity(rev2State) << endl;
+ assertEqual(fwdA.getBodyVelocity(fwdState), rev1A.getBodyVelocity(rev1State));
+ assertEqual(fwdA.getBodyVelocity(fwdState), rev2A.getBodyVelocity(rev2State));
+
+ forward.realize (fwdState, Stage::Acceleration);
+ reverse1.realize(rev1State, Stage::Acceleration);
+ reverse2.realize(rev2State, Stage::Acceleration);
+
+ cout << "Accels B:\n";
+ cout << " Fwd: " << fwdB.getBodyAcceleration(fwdState) << endl;
+ cout << " -Rev1: " << (fwdB.getBodyAcceleration(fwdState)-rev1B.getBodyAcceleration(rev1State)).norm() << endl;
+ cout << " -Rev2: " << (fwdB.getBodyAcceleration(fwdState)-rev2B.getBodyAcceleration(rev2State)).norm() << endl;
+
+ assertEqual(fwdB.getBodyAcceleration(fwdState), rev1B.getBodyAcceleration(rev1State));
+ assertEqual(fwdB.getBodyAcceleration(fwdState), rev2B.getBodyAcceleration(rev2State));
+ assertEqual(fwdA.getBodyAcceleration(fwdState), rev1A.getBodyAcceleration(rev1State));
+ assertEqual(fwdA.getBodyAcceleration(fwdState), rev2A.getBodyAcceleration(rev2State));
+
+ Vector_<SpatialVec> fwdReac, rev1Reac, rev2Reac;
+ fwdMatter.calcMobilizerReactionForces(fwdState, fwdReac);
+ rev1Matter.calcMobilizerReactionForces(rev1State, rev1Reac);
+ rev2Matter.calcMobilizerReactionForces(rev2State, rev2Reac);
+
+ // We expect the AB reaction to be -BA reaction for a pin joint because
+ // there is no translation (that is, the pin joint holds the origins
+ // of the mobilizer frames coincident). This is not true for all joint
+ // types!
+ const SpatialVec reacBA = -fwdReac[fwdB.getMobilizedBodyIndex()];
+ const SpatialVec reacBA1 = rev1Reac[rev1A.getMobilizedBodyIndex()];
+ const SpatialVec reacBA2 = rev1Reac[rev1A.getMobilizedBodyIndex()];
+
+ cout << "Reacs BA:\n";
+ cout << " Fwd: " << reacBA << endl;
+ cout << " Rev1: " << reacBA1 << endl;
+ cout << " Rev2: " << reacBA2 << endl;
+
+
+ assertEqual(reacBA, reacBA1, 1e-7);
+ assertEqual(reacBA, reacBA2, 1e-7);
+}
+
+
+// System is two free bodies A and B connected by a planar joint.
+// The system is Ground (6dof) A (planar) B (6dof) Ground.
+// The forward system is Ground (6dof)-> A (planar)-> B.
+// The reverse system2 is A <-(rplanar) B <-(6dof) Ground.
+// Reverse system2's planar joint q's and u's should have the
+// same meaning as the forward system's.
+//
+// Note: in this test, reversing the planar joint introduces
+// sines and cosines into H where there were none before and
+// hence produces a non-zero HDot where the forward direction
+// has HDot==0. So this tests both H reversal and HDot reversal.
+// HOWEVER: the rotational axis doesn't change so some of
+// the HDot terms are not used.
+
+void testPlanar() {
+ MultibodySystem forward;
+ SimbodyMatterSubsystem fwdMatter(forward);
+ GeneralForceSubsystem fwdForces(forward);
+ Force::UniformGravity(fwdForces, fwdMatter, Vec3(0, -1, 0));
+
+ MultibodySystem reverse2;
+ SimbodyMatterSubsystem rev2Matter(reverse2);
+ GeneralForceSubsystem rev2Forces(reverse2);
+ Force::UniformGravity(rev2Forces, rev2Matter, Vec3(0, -1, 0));
+
+ const Vec3 com(1,2,3);
+ const UnitInertia centralGyration(1, 1.5, 2, .1, .2, .3);
+ Body::Rigid body(MassProperties(mass, com, mass*centralGyration.shiftFromMassCenter(com, 1)));
+
+ MobilizedBody::Free fwdA (fwdMatter.Ground(), Vec3(0), body, X_AM);
+ MobilizedBody::Free rev2B(rev2Matter.Ground(), Vec3(0), body, X_BM);
+
+ MobilizedBody::Planar fwdB (fwdA, X_AM, body, X_BM);
+ MobilizedBody::Planar rev2A(rev2B, X_BM, body, X_AM, MobilizedBody::Reverse);
+
+ // The generalized speeds should mean the same things, so the generalized
+ // forces should too.
+ Force::MobilityConstantForce(fwdForces, fwdB, 0, -3.45);
+ Force::MobilityConstantForce(fwdForces, fwdB, 1, 2);
+ Force::MobilityConstantForce(fwdForces, fwdB, 2, -3);
+ Force::MobilityConstantForce(rev2Forces, rev2A, 0, -3.45); // reversed
+ Force::MobilityConstantForce(rev2Forces, rev2A, 1, 2); // reversed
+ Force::MobilityConstantForce(rev2Forces, rev2A, 2, -3); // reversed
+
+
+ State fwdState = forward.realizeTopology();
+ State rev2State = reverse2.realizeTopology();
+
+ // Put body A somewhere arbitrary.
+ fwdA.setQToFitTransform(fwdState, Transform(Rotation(-1.9, Vec3(-3,2,4)),
+ Vec3(-.33, .66, -.99)));
+
+ // Set all the q's; meanings should be identical.
+ fwdB.setQ(fwdState, Vec3(Pi/4, -7, 4));
+ rev2A.setQ(rev2State, Vec3(Pi/4, -7, 4));
+
+ // Calculate where body B ended up in the forward system and then
+ // set the reverse Free joint to match so that the whole system
+ // is identically configured.
+
+ forward.realize (fwdState, Stage::Position);
+ const Transform& X_GB = fwdB.getBodyTransform(fwdState);
+ const Transform X_GMb = X_GB*X_BM;
+ rev2B.setQToFitTransform(rev2State, X_GMb);
+ reverse2.realize(rev2State, Stage::Position);
+
+ assertEqual(fwdB.getBodyTransform(fwdState), rev2B.getBodyTransform(rev2State));
+ assertEqual(fwdA.getBodyTransform(fwdState), rev2A.getBodyTransform(rev2State));
+
+ // Handle velocities similarly.
+
+ fwdB.setU(fwdState, Vec3(3, .3, .4));
+
+ forward.realize (fwdState, Stage::Velocity);
+
+ const SpatialVec V_G_BM( fwdB.getBodyAngularVelocity(fwdState),
+ fwdB.findStationVelocityInGround(fwdState, X_BM.p()));
+ rev2B.setUToFitVelocity(rev2State, V_G_BM);
+
+ // Need to construct velocity of A in B from B in A.
+ const Transform X_AB = fwdB.getMobilizerTransform(fwdState);
+ const Transform X_BA = ~X_AB;
+ const SpatialVec V_AB = fwdB.getMobilizerVelocity(fwdState);
+ const SpatialVec V_BA( -X_BA.R()* V_AB[0],
+ -X_BA.R()*(V_AB[1] + X_AB.p() % V_AB[0]) );
+
+ rev2A.setUToFitVelocity(rev2State, V_BA);
+ cout << "rev2A.getU()=" << rev2A.getU(rev2State) << endl;
+
+ assertEqual(fwdB.getU(fwdState), rev2A.getU(rev2State));
+
+ reverse2.realize(rev2State, Stage::Velocity);
+
+ cout << "Vels B:\n";
+ cout << " Fwd: " << fwdB.getBodyVelocity(fwdState) << endl;
+ cout << " Rev2: " << rev2B.getBodyVelocity(rev2State) << endl;
+
+ assertEqual(fwdB.getBodyVelocity(fwdState), rev2B.getBodyVelocity(rev2State));
+
+ cout << "Vels A:\n";
+ cout << " Fwd: " << fwdA.getBodyVelocity(fwdState) << endl;
+ cout << " Rev2: " << rev2A.getBodyVelocity(rev2State) << endl;
+ assertEqual(fwdA.getBodyVelocity(fwdState), rev2A.getBodyVelocity(rev2State));
+
+ forward.realize (fwdState, Stage::Acceleration);
+ reverse2.realize(rev2State, Stage::Acceleration);
+
+ cout << "Accels B:\n";
+ cout << " Fwd: " << fwdB.getBodyAcceleration(fwdState) << endl;
+ cout << " Rev2: " << rev2B.getBodyAcceleration(rev2State) << endl;
+
+ assertEqual(fwdB.getBodyAcceleration(fwdState), rev2B.getBodyAcceleration(rev2State));
+
+ cout << "Accels A:\n";
+ cout << " Fwd: " << fwdA.getBodyAcceleration(fwdState) << endl;
+ cout << " Rev2: " << rev2A.getBodyAcceleration(rev2State) << endl;
+ assertEqual(fwdA.getBodyAcceleration(fwdState), rev2A.getBodyAcceleration(rev2State));
+
+ Vector_<SpatialVec> fwdReac, rev2Reac;
+ fwdMatter.calcMobilizerReactionForces(fwdState, fwdReac);
+ rev2Matter.calcMobilizerReactionForces(rev2State, rev2Reac);
+
+ // Reaction AB != -BA for a planar joint unless the translation is 0.
+ // Instead, we have to shift the reaction F_AB at B's mobilizer frame
+ // over to A's mobilizer frame, then negate. So
+ // F_BA = -(F_AB + [f_AB x p_BA_G; 0])
+ const SpatialVec F_AB = fwdReac[fwdB.getMobilizedBodyIndex()];
+ const SpatialVec reacBA = -(F_AB + SpatialVec(F_AB[1] % (X_GMb.R()*X_BA.p()), Vec3(0)));
+ const SpatialVec reacBA2 = rev2Reac[rev2A.getMobilizedBodyIndex()];
+
+ cout << "Reacs BA:\n";
+ cout << " Fwd AB:" << F_AB << " p_BA: " << X_BA.p() << endl;
+ cout << " Fwd: " << reacBA << endl;
+ cout << " Rev2: " << reacBA2 << endl;
+ cout << " Fwd-Rev2:" << reacBA-reacBA2 << endl;
+
+ assertEqual(reacBA, reacBA2, 5e-6);
+}
+
+
+// System is two free bodies A and B connected by an ellipsoid joint.
+// The system is Ground (6dof) A (ellipsoid) B (6dof) Ground.
+// The forward system is Ground (6dof)-> A (ellipsoid)-> B.
+// The reverse system2 is A <-(rellipsoid) B <-(6dof) Ground.
+// Reverse system2's ellipsoid joint q's and u's should have the
+// same meaning as the forward system's.
+//
+// H and HDot are both populated for the ellipsoid, so this
+// tests the reverse HDot terms which involve the forward
+// HDot terms (those don't matter if forward HDot==0).
+
+void testEllipsoid() {
+ MultibodySystem forward;
+ SimbodyMatterSubsystem fwdMatter(forward);
+ GeneralForceSubsystem fwdForces(forward);
+ Force::UniformGravity(fwdForces, fwdMatter, Vec3(0, -1, 0));
+
+ MultibodySystem reverse2;
+ SimbodyMatterSubsystem rev2Matter(reverse2);
+ GeneralForceSubsystem rev2Forces(reverse2);
+ Force::UniformGravity(rev2Forces, rev2Matter, Vec3(0, -1, 0));
+
+ const Vec3 com(1,2,3);
+ const UnitInertia centralGyration(1, 1.5, 2, .1, .2, .3);
+ Body::Rigid body(MassProperties(mass, com, mass*centralGyration.shiftFromMassCenter(com, 1)));
+
+ //Transform X_AM, X_BM; // identity for now
+ MobilizedBody::Free fwdA (fwdMatter.Ground(), Vec3(0), body, X_AM);
+ MobilizedBody::Free rev2B(rev2Matter.Ground(), Vec3(0), body, X_BM);
+
+ MobilizedBody::Ellipsoid fwdB (fwdA, X_AM, body, X_BM, Vec3(1,2,3));
+ MobilizedBody::Ellipsoid rev2A(rev2B, X_BM, body, X_AM, Vec3(1,2,3), MobilizedBody::Reverse);
+
+ // The generalized speeds should mean the same things, so the generalized
+ // forces should too.
+ Force::MobilityConstantForce(fwdForces, fwdB, 0, -3.45);
+ Force::MobilityConstantForce(fwdForces, fwdB, 1, 2);
+ Force::MobilityConstantForce(fwdForces, fwdB, 2, -3);
+ Force::MobilityConstantForce(rev2Forces, rev2A, 0, -3.45); // reversed
+ Force::MobilityConstantForce(rev2Forces, rev2A, 1, 2); // reversed
+ Force::MobilityConstantForce(rev2Forces, rev2A, 2, -3); // reversed
+
+
+ State fwdState = forward.realizeTopology();
+ State rev2State = reverse2.realizeTopology();
+
+ // Put body A somewhere arbitrary.
+ fwdA.setQToFitTransform(fwdState, Transform(Rotation(-1.9, Vec3(-3,2,4)),
+ Vec3(-.33, .66, -.99)));
+
+ // Set all the q's; meanings should be identical.
+ const Quaternion quat(Rotation(Pi/7, Vec3(1.1,-2.2,3.4)));
+ fwdB.setQ(fwdState, quat);
+ rev2A.setQ(rev2State, quat);
+
+ // Calculate where body B ended up in the forward system and then
+ // set the reverse Free joint to match so that the whole system
+ // is identically configured.
+
+ forward.realize (fwdState, Stage::Position);
+ const Transform& X_GB = fwdB.getBodyTransform(fwdState);
+ const Transform X_GMb = X_GB*X_BM;
+ rev2B.setQToFitTransform(rev2State, X_GMb);
+ reverse2.realize(rev2State, Stage::Position);
+
+ assertEqual(fwdB.getBodyTransform(fwdState), rev2B.getBodyTransform(rev2State));
+ assertEqual(fwdA.getBodyTransform(fwdState), rev2A.getBodyTransform(rev2State));
+
+ // Handle velocities similarly.
+
+ fwdB.setU(fwdState, Vec3(3, .3, .4));
+
+ forward.realize (fwdState, Stage::Velocity);
+
+ const SpatialVec V_G_BM( fwdB.getBodyAngularVelocity(fwdState),
+ fwdB.findStationVelocityInGround(fwdState, X_BM.p()));
+ rev2B.setUToFitVelocity(rev2State, V_G_BM);
+
+ // Need to construct velocity of A in B from B in A.
+ const Transform X_AB = fwdB.getMobilizerTransform(fwdState);
+ const Transform X_BA = ~X_AB;
+ const SpatialVec V_AB = fwdB.getMobilizerVelocity(fwdState);
+ const SpatialVec V_BA( -X_BA.R()* V_AB[0],
+ -X_BA.R()*(V_AB[1] + X_AB.p() % V_AB[0]) );
+
+ rev2A.setUToFitAngularVelocity(rev2State, V_BA[0]);
+ cout << "rev2A.getU()=" << rev2A.getU(rev2State) << endl;
+
+ assertEqual(fwdB.getU(fwdState), rev2A.getU(rev2State));
+
+ reverse2.realize(rev2State, Stage::Velocity);
+
+ cout << "Vels B:\n";
+ cout << " Fwd: " << fwdB.getBodyVelocity(fwdState) << endl;
+ cout << " Rev2: " << rev2B.getBodyVelocity(rev2State) << endl;
+
+ assertEqual(fwdB.getBodyVelocity(fwdState), rev2B.getBodyVelocity(rev2State));
+
+ cout << "Vels A:\n";
+ cout << " Fwd: " << fwdA.getBodyVelocity(fwdState) << endl;
+ cout << " Rev2: " << rev2A.getBodyVelocity(rev2State) << endl;
+ assertEqual(fwdA.getBodyVelocity(fwdState), rev2A.getBodyVelocity(rev2State));
+
+ forward.realize (fwdState, Stage::Acceleration);
+ reverse2.realize(rev2State, Stage::Acceleration);
+
+ cout << "Accels B:\n";
+ cout << " Fwd: " << fwdB.getBodyAcceleration(fwdState) << endl;
+ cout << " Rev2: " << rev2B.getBodyAcceleration(rev2State) << endl;
+
+ assertEqual(fwdB.getBodyAcceleration(fwdState), rev2B.getBodyAcceleration(rev2State));
+
+ cout << "Accels A:\n";
+ cout << " Fwd: " << fwdA.getBodyAcceleration(fwdState) << endl;
+ cout << " Rev2: " << rev2A.getBodyAcceleration(rev2State) << endl;
+ assertEqual(fwdA.getBodyAcceleration(fwdState), rev2A.getBodyAcceleration(rev2State));
+
+ Vector_<SpatialVec> fwdReac, rev2Reac;
+ fwdMatter.calcMobilizerReactionForces(fwdState, fwdReac);
+ rev2Matter.calcMobilizerReactionForces(rev2State, rev2Reac);
+
+ // Reaction AB != -BA for a planar joint unless the translation is 0.
+ // Instead, we have to shift the reaction F_AB at B's mobilizer frame
+ // over to A's mobilizer frame, then negate. So
+ // F_BA = -(F_AB + [f_AB x p_BA_G; 0])
+ const SpatialVec F_AB = fwdReac[fwdB.getMobilizedBodyIndex()];
+ const SpatialVec reacBA = -(F_AB + SpatialVec(F_AB[1] % (X_GMb.R()*X_BA.p()), Vec3(0)));
+ const SpatialVec reacBA2 = rev2Reac[rev2A.getMobilizedBodyIndex()];
+
+ cout << "Reacs BA:\n";
+ cout << " Fwd AB:" << F_AB << " p_BA: " << X_BA.p() << endl;
+ cout << " Fwd: " << reacBA << endl;
+ cout << " Rev2: " << reacBA2 << endl;
+ cout << " Fwd-Rev2:" << reacBA-reacBA2 << endl;
+
+ assertEqual(reacBA, reacBA2, reacBA.norm()*1e-8);
+}
+
+// This one should test all the mobilizer reversal equations.
+// System is two free bodies A and B connected by a Free joint.
+// The system is Ground (6dof) A (free) B (6dof) Ground.
+// The forward system is Ground (6dof)-> A (free)-> B.
+// The reverse system2 is A <-(rfree) B <-(6dof) Ground.
+// Reverse system2's free joint q's and u's should have the
+// same meaning as the forward system's.
+// NOTE: 6dof and free are the same thing above, but the one named
+// "free" and "rfree" is the mobilizer under test.
+//
+// Note: in this test, reversing the free joint introduces
+// sines and cosines into H where there were none before and
+// hence produces a non-zero HDot where the forward direction
+// has HDot==0. So this tests both H reversal and HDot reversal.
+
+void testFree() {
+ MultibodySystem forward;
+ SimbodyMatterSubsystem fwdMatter(forward);
+ GeneralForceSubsystem fwdForces(forward);
+ Force::UniformGravity(fwdForces, fwdMatter, Vec3(0, -1, 0));
+
+ MultibodySystem reverse2;
+ SimbodyMatterSubsystem rev2Matter(reverse2);
+ GeneralForceSubsystem rev2Forces(reverse2);
+ Force::UniformGravity(rev2Forces, rev2Matter, Vec3(0, -1, 0));
+
+ const Vec3 com(1,2,3);
+ const UnitInertia centralGyration(1, 1.5, 2, .1, .2, .3);
+ Body::Rigid body(MassProperties(mass, com, mass*centralGyration.shiftFromMassCenter(com, 1)));
+
+ MobilizedBody::Free fwdA (fwdMatter.Ground(), Vec3(0), body, X_AM);
+ MobilizedBody::Free rev2B(rev2Matter.Ground(), Vec3(0), body, X_BM);
+
+ // These are the mobilizers under test.
+ MobilizedBody::Free fwdB (fwdA, X_AM, body, X_BM);
+ MobilizedBody::Free rev2A(rev2B, X_BM, body, X_AM, MobilizedBody::Reverse);
+
+ // The generalized speeds should mean the same things, so the generalized
+ // forces should too.
+ const Real genFrc[6] = {-3.45, 2, -3, .0232, 4.33, -2.4};
+ for (int i=0; i<6; ++i) {
+ // We're creating force elements here and adding them to the
+ // force subsystems.
+ Force::MobilityConstantForce(fwdForces, fwdB, i, 1000*genFrc[i]);
+ Force::MobilityConstantForce(rev2Forces, rev2A, i, 1000*genFrc[i]);
+ }
+
+ State fwdState = forward.realizeTopology();
+ State rev2State = reverse2.realizeTopology();
+
+ // Put body A somewhere arbitrary.
+ fwdA.setQToFitTransform(fwdState, Transform(Rotation(-1.9, Vec3(-3,2,4)),
+ Vec3(-.33, .66, -.99)));
+
+ // Set all the q's; meanings should be identical.
+ const Quaternion quat(Rotation(Pi/7, Vec3(1,2,-3)));
+ const Vec3 trans(1.23, 2.34, -3.45);
+ const Vec7 init(quat[0],quat[1],quat[2],quat[3],trans[0],trans[1],trans[2]);
+ fwdB.setQ (fwdState, init);
+ rev2A.setQ(rev2State, init);
+
+ // Calculate where body B ended up in the forward system and then
+ // set the reverse Free joint to match so that the whole system
+ // is identically configured.
+
+ forward.realize (fwdState, Stage::Position);
+ const Transform& X_GB = fwdB.getBodyTransform(fwdState);
+ const Transform X_GMb = X_GB*X_BM;
+ rev2B.setQToFitTransform(rev2State, X_GMb);
+ reverse2.realize(rev2State, Stage::Position);
+
+ cout << "Xforms B\n";
+ cout << " Fwd: " << fwdB.getBodyTransform(fwdState);
+ cout << " Rev2: " << rev2B.getBodyTransform(rev2State);
+ assertEqual(fwdB.getBodyTransform(fwdState), rev2B.getBodyTransform(rev2State));
+
+ cout << "Xforms A\n";
+ cout << " Fwd: " << fwdA.getBodyTransform(fwdState);
+ cout << " Rev2: " << rev2A.getBodyTransform(rev2State);
+ assertEqual(fwdA.getBodyTransform(fwdState), rev2A.getBodyTransform(rev2State));
+
+ // Handle velocities similarly.
+
+ // Want high velocity so cross terms involving HDot will be big enough
+ // to notice if they are wrong.
+ fwdB.setU(fwdState, 1000*Vec6(.1, .2, -.33, 3, .3, .4));
+
+ forward.realize (fwdState, Stage::Velocity);
+
+ const SpatialVec V_G_BM( fwdB.getBodyAngularVelocity(fwdState),
+ fwdB.findStationVelocityInGround(fwdState, X_BM.p()));
+ rev2B.setUToFitVelocity(rev2State, V_G_BM);
+
+ // Need to construct velocity of A in B from B in A.
+ const Transform X_AB = fwdB.getMobilizerTransform(fwdState);
+ const Transform X_BA = ~X_AB;
+ const SpatialVec V_AB = fwdB.getMobilizerVelocity(fwdState);
+ const SpatialVec V_BA( -X_BA.R()* V_AB[0],
+ -X_BA.R()*(V_AB[1] + X_AB.p() % V_AB[0]) );
+
+ rev2A.setUToFitVelocity(rev2State, V_BA);
+ cout << "rev2A.getU()=" << rev2A.getU(rev2State) << endl;
+
+ assertEqual(fwdB.getU(fwdState), rev2A.getU(rev2State));
+
+ reverse2.realize(rev2State, Stage::Velocity);
+
+ cout << "Vels B:\n";
+ cout << " Fwd: " << fwdB.getBodyVelocity(fwdState) << endl;
+ cout << " Rev2: " << rev2B.getBodyVelocity(rev2State) << endl;
+
+ assertEqual(fwdB.getBodyVelocity(fwdState), rev2B.getBodyVelocity(rev2State));
+
+ cout << "Vels A:\n";
+ cout << " Fwd: " << fwdA.getBodyVelocity(fwdState) << endl;
+ cout << " Rev2: " << rev2A.getBodyVelocity(rev2State) << endl;
+ assertEqual(fwdA.getBodyVelocity(fwdState), rev2A.getBodyVelocity(rev2State));
+
+ forward.realize (fwdState, Stage::Acceleration);
+ reverse2.realize(rev2State, Stage::Acceleration);
+
+ cout << "Accels B:\n";
+ cout << " Fwd: " << fwdB.getBodyAcceleration(fwdState) << endl;
+ cout << " Rev2: " << rev2B.getBodyAcceleration(rev2State) << endl;
+ cout << " Fwd-Rev2: " << fwdB.getBodyAcceleration(fwdState)-rev2B.getBodyAcceleration(rev2State) << endl;
+
+ assertEqual(fwdB.getBodyAcceleration(fwdState), rev2B.getBodyAcceleration(rev2State), 1e-7);
+
+ cout << "Accels A:\n";
+ cout << " Fwd: " << fwdA.getBodyAcceleration(fwdState) << endl;
+ cout << " Rev2: " << rev2A.getBodyAcceleration(rev2State) << endl;
+ cout << " Fwd-Rev2: " << fwdA.getBodyAcceleration(fwdState)-rev2A.getBodyAcceleration(rev2State) << endl;
+ assertEqual(fwdA.getBodyAcceleration(fwdState), rev2A.getBodyAcceleration(rev2State), 1e-7);
+
+ Vector_<SpatialVec> fwdReac, rev2Reac;
+ fwdMatter.calcMobilizerReactionForces(fwdState, fwdReac);
+ rev2Matter.calcMobilizerReactionForces(rev2State, rev2Reac);
+
+ // Reaction AB != -BA for a free joint unless the translation is 0.
+ // Instead, we have to shift the reaction F_AB at B's mobilizer frame
+ // over to A's mobilizer frame, then negate. So
+ // F_BA = -(F_AB + [f_AB x p_BA_G; 0])
+ const SpatialVec F_AB = fwdReac[fwdB.getMobilizedBodyIndex()];
+ const SpatialVec reacBA = -(F_AB + SpatialVec(F_AB[1] % (X_GMb.R()*X_BA.p()), Vec3(0)));
+ const SpatialVec reacBA2 = rev2Reac[rev2A.getMobilizedBodyIndex()];
+
+ cout << "Reacs BA:\n";
+ cout << " Fwd AB:" << F_AB << " p_BA: " << X_BA.p() << endl;
+ cout << " Fwd: " << reacBA << endl;
+ cout << " Rev2: " << reacBA2 << endl;
+ cout << " Fwd-Rev2:" << reacBA-reacBA2 << endl;
+
+ cout << " Fwd: " << fwdB.findMobilizerReactionOnParentAtFInGround(fwdState) << endl;
+ cout << " Rev: " << rev2A.findMobilizerReactionOnBodyAtMInGround(rev2State) << endl;
+
+ assertEqual(reacBA, reacBA2, reacBA.norm()*1e-5);
+}
+
+int main() {
+ try {
+ cout << "*** TEST PIN ***\n\n"; testPin();
+ cout << "\n\n*** TEST PLANAR ***\n\n"; testPlanar();
+ cout << "\n\n*** TEST ELLIPSOID ***\n\n"; testEllipsoid();
+ cout << "\n\n*** TEST FREE ***\n\n"; testFree();
+ }
+ catch(const std::exception& e) {
+ cout << "exception: " << e.what() << endl;
+ return 1;
+ }
+ cout << "Done" << endl;
+ return 0;
+}
+
diff --git a/Simbody/tests/adhoc/CMakeLists.txt b/Simbody/tests/adhoc/CMakeLists.txt
new file mode 100644
index 0000000..e1ddcf6
--- /dev/null
+++ b/Simbody/tests/adhoc/CMakeLists.txt
@@ -0,0 +1,59 @@
+# Generate adhoc tests.
+#
+# This is boilerplate code for generating a set of executables, one per
+# .cpp file in a "tests/adhoc" subdirectory. These are for "loose" test
+# cases, not suitable as regression tests but perhaps useful to the
+# developer or as demos. Unlike the similar boilerplate code in the "tests"
+# directory, this does not generate CMake ADD_TEST commands so these
+# will never run automatically.
+#
+# For IDEs that can deal with PROJECT_LABEL properties (at least
+# Visual Studio) the projects for building each of these adhoc
+# executables will be labeled "Adhoc - TheTestName" if a file
+# TheTestName.cpp is found in this directory.
+#
+# We check the BUILD_TESTS_AND_EXAMPLES_{SHARED,STATIC} variables to determine
+# whether to build dynamically linked, statically linked, or both
+# versions of the executable.
+
+
+FILE(GLOB ADHOC_TESTS "*.cpp")
+FOREACH(TEST_PROG ${ADHOC_TESTS})
+ GET_FILENAME_COMPONENT(TEST_ROOT ${TEST_PROG} NAME_WE)
+
+ IF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+ # Link with shared library
+ ADD_EXECUTABLE(${TEST_ROOT} ${TEST_PROG})
+ IF(GUI_NAME)
+ ADD_DEPENDENCIES(${TEST_ROOT} ${GUI_NAME})
+ ENDIF()
+ SET_TARGET_PROPERTIES(${TEST_ROOT}
+ PROPERTIES
+ PROJECT_LABEL "Test_Adhoc - ${TEST_ROOT}")
+ TARGET_LINK_LIBRARIES(${TEST_ROOT}
+ ${TEST_SHARED_TARGET})
+ ENDIF (BUILD_TESTS_AND_EXAMPLES_SHARED)
+
+ IF (BUILD_STATIC_LIBRARIES AND BUILD_TESTS_AND_EXAMPLES_STATIC)
+ # Link with static library
+ SET(TEST_STATIC ${TEST_ROOT}Static)
+ ADD_EXECUTABLE(${TEST_STATIC} ${TEST_PROG})
+ IF(GUI_NAME)
+ ADD_DEPENDENCIES(${TEST_STATIC} ${GUI_NAME})
+ ENDIF()
+ SET_TARGET_PROPERTIES(${TEST_STATIC}
+ PROPERTIES
+ COMPILE_FLAGS "-DSimTK_USE_STATIC_LIBRARIES"
+ PROJECT_LABEL "Test_Adhoc - ${TEST_STATIC}")
+ TARGET_LINK_LIBRARIES(${TEST_STATIC}
+ ${TEST_STATIC_TARGET})
+ ENDIF()
+
+ENDFOREACH(TEST_PROG ${ADHOC_TESTS})
+
+FILE(GLOB ADHOC_OBJS "*.obj" "*.vtp")
+FOREACH(TEST_OBJ ${ADHOC_OBJS})
+ GET_FILENAME_COMPONENT(OBJ_ROOT ${TEST_OBJ} NAME)
+ CONFIGURE_FILE(${TEST_OBJ}
+ ${CMAKE_CURRENT_BINARY_DIR}/${OBJ_ROOT} COPYONLY)
+ENDFOREACH(TEST_OBJ ${ADHOC_OBJS})
diff --git a/Simbody/tests/adhoc/CableOverBicubicSurfaces-femur.vtp b/Simbody/tests/adhoc/CableOverBicubicSurfaces-femur.vtp
new file mode 100644
index 0000000..c8b0762
--- /dev/null
+++ b/Simbody/tests/adhoc/CableOverBicubicSurfaces-femur.vtp
@@ -0,0 +1,1842 @@
+<?xml version="1.0"?>
+<VTKFile type="PolyData" version="0.1" byte_order="LittleEndian" compressor="vtkZLibDataCompressor">
+ <PolyData>
+ <Piece NumberOfPoints="456" NumberOfVerts="0" NumberOfLines="0" NumberOfStrips="0" NumberOfPolys="908">
+ <PointData Normals="Normals">
+ <DataArray type="Float32" Name="Normals" NumberOfComponents="3" format="ascii">
+ 0.001802 0.770265 -0.637722
+ 0.080536 0.341305 -0.936496
+ -0.261491 0.594688 -0.760242
+ 0.294673 0.734484 -0.611311
+ -0.021499 0.850627 -0.525330
+ 0.345921 0.819241 -0.457365
+ -0.363485 0.290920 -0.885011
+ -0.281156 0.020009 -0.959454
+ -0.049834 -0.126800 -0.990676
+ 0.294208 0.012871 -0.955655
+ 0.503436 0.221009 -0.835288
+ 0.485169 0.482857 -0.729013
+ -0.345770 0.791739 -0.503580
+ -0.589916 0.520509 -0.617308
+ 0.661767 0.557970 -0.500733
+ -0.060030 0.949096 -0.309212
+ 0.286543 0.909996 -0.299666
+ 0.603183 0.752306 -0.264964
+ -0.626172 0.086950 -0.774822
+ -0.393325 -0.236702 -0.888407
+ -0.019136 -0.320673 -0.946997
+ 0.384727 -0.209864 -0.898856
+ 0.600386 -0.073036 -0.796368
+ 0.740571 0.202020 -0.640892
+ -0.390880 0.848657 -0.356363
+ -0.641616 0.662844 -0.385962
+ -0.807183 0.285337 -0.516758
+ 0.843082 0.448514 -0.296730
+ -0.178313 0.981457 -0.070327
+ 0.129993 0.990705 -0.040078
+ 0.407588 0.913118 -0.009411
+ 0.686181 0.726973 0.025805
+ -0.692318 -0.167745 -0.701824
+ -0.374273 -0.430478 -0.821345
+ 0.018275 -0.517214 -0.855661
+ 0.451235 -0.446506 -0.772670
+ 0.741746 -0.213542 -0.635777
+ 0.881678 0.085763 -0.463992
+ -0.480568 0.867476 -0.128603
+ -0.752529 0.622662 -0.214456
+ -0.917293 0.266887 -0.295541
+ -0.877500 -0.126968 -0.462464
+ 0.983050 0.084345 -0.162783
+ 0.897923 0.438573 0.037247
+ -0.493977 0.854538 0.160471
+ -0.194235 0.959109 0.205868
+ 0.093216 0.965056 0.244904
+ 0.401454 0.864656 0.302002
+ 0.700688 0.623031 0.347662
+ -0.634975 -0.473762 -0.610210
+ -0.275622 -0.644378 -0.713308
+ 0.179437 -0.686019 -0.705110
+ 0.587405 -0.562527 -0.581824
+ 0.862532 -0.269802 -0.428071
+ -0.782310 0.619270 0.067043
+ -0.969261 0.243692 -0.033870
+ -0.961837 -0.196754 -0.190150
+ -0.724241 -0.498459 -0.476459
+ 0.992782 -0.104265 0.059262
+ 0.921349 0.265987 0.283490
+ -0.497417 0.792744 0.352326
+ -0.804969 0.552242 0.216918
+ -0.187639 0.881658 0.432979
+ 0.124675 0.847234 0.516382
+ 0.422327 0.684092 0.594692
+ 0.658318 0.411783 0.630121
+ -0.341981 -0.745995 -0.571437
+ 0.156828 -0.773496 -0.614092
+ 0.566156 -0.711792 -0.415715
+ 0.864394 -0.466003 -0.188852
+ -0.976730 0.213611 0.019218
+ -0.968529 -0.219517 -0.117318
+ -0.739264 -0.618277 -0.266876
+ -0.327919 -0.831465 -0.448482
+ 0.911563 -0.270542 0.309613
+ 0.855486 0.077182 0.512041
+ -0.327510 0.722976 0.608312
+ -0.648468 0.568321 0.506460
+ -0.857704 0.289576 0.424841
+ -0.064896 0.716154 0.694919
+ 0.224291 0.584408 0.779847
+ 0.429830 0.345194 0.834318
+ 0.597854 0.009791 0.801545
+ 0.101313 -0.868247 -0.485678
+ 0.621683 -0.703553 -0.344272
+ 0.823470 -0.566855 -0.023919
+ -0.982313 0.013515 0.186759
+ -0.858319 -0.502964 0.101563
+ -0.529784 -0.848132 0.000860
+ -0.287800 -0.952777 -0.096886
+ 0.706708 -0.609876 0.358630
+ 0.725456 -0.309117 0.614947
+ -0.574177 0.356622 0.736981
+ -0.330886 0.300884 0.894418
+ -0.756459 0.131916 0.640599
+ -0.880762 -0.226682 0.415780
+ -0.179531 0.242025 0.953516
+ 0.145543 0.213665 0.966004
+ 0.434603 -0.012704 0.900533
+ 0.578957 -0.346069 0.738272
+ -0.147174 -0.982971 -0.110037
+ 0.135287 -0.989377 -0.053198
+ 0.461124 -0.885561 0.056096
+ -0.723015 -0.690752 0.010505
+ -0.374884 -0.911240 -0.170597
+ -0.192345 -0.976353 -0.098681
+ 0.487159 -0.738418 0.466278
+ -0.663087 0.202080 0.720749
+ -0.421341 0.361998 0.831522
+ -0.947664 -0.069673 0.311575
+ -0.896955 -0.401038 -0.186122
+ -0.091893 0.464532 0.880776
+ 0.490090 0.209878 0.846028
+ 0.817550 -0.189365 0.543831
+ 0.057652 -0.997899 -0.029576
+ 0.277703 -0.944416 0.175954
+ -0.430022 -0.655687 -0.620609
+ 0.149734 -0.840468 -0.520761
+ 0.805859 -0.513081 0.295533
+ -0.720678 0.643725 0.257376
+ -0.969378 0.234888 0.071655
+ -0.043201 0.926581 0.373606
+ -0.961265 -0.056251 -0.269825
+ -0.700259 -0.195003 -0.686739
+ 0.572210 0.719875 0.392881
+ 0.961667 0.193115 0.194685
+ 0.974209 -0.193007 0.116898
+ 0.654379 -0.743833 -0.136018
+ 0.000231 -0.451309 -0.892368
+ 0.628216 -0.481259 -0.611338
+ 0.949493 -0.305467 -0.071780
+ -0.145851 0.987155 -0.065211
+ -0.728869 0.673207 -0.124668
+ -0.951319 0.295108 -0.088898
+ -0.961381 0.179371 -0.208742
+ 0.519013 0.854664 0.013260
+ -0.835227 0.069999 -0.545432
+ -0.404560 -0.090469 -0.910025
+ 0.870007 0.487822 0.071541
+ 0.975375 -0.121857 0.183832
+ 0.982112 -0.169571 0.081867
+ 0.945732 -0.284516 0.156977
+ 0.439865 -0.374278 -0.816354
+ 0.874965 -0.264979 -0.405243
+ -0.060893 0.949572 -0.307580
+ 0.537617 0.841090 -0.059459
+ -0.380183 0.515798 -0.767732
+ -0.730638 0.302884 -0.611906
+ -0.632632 0.548764 -0.546475
+ -0.776521 0.314961 -0.545724
+ 0.806706 0.528515 0.264381
+ -0.763865 0.243333 -0.597746
+ -0.255549 0.193753 -0.947182
+ 0.912706 -0.033668 0.407228
+ 0.946079 -0.197739 0.256581
+ 0.921488 -0.250648 0.296708
+ 0.860115 -0.175777 -0.478858
+ 0.990518 -0.073659 0.115971
+ 0.547978 -0.281675 -0.787641
+ 0.164688 0.905633 -0.390778
+ 0.432818 0.876742 0.209741
+ 0.756380 0.345204 0.555629
+ 0.428279 0.337594 -0.838217
+ 0.196508 -0.127384 -0.972193
+ 0.142483 0.327131 -0.934175
+ -0.264765 0.414281 -0.870787
+ -0.647359 0.318294 -0.692542
+ 0.855553 -0.054539 0.514835
+ -0.539535 0.551969 -0.635792
+ -0.467636 0.645749 -0.603593
+ 0.196620 0.296361 -0.934618
+ 0.946419 -0.090577 0.309978
+ 0.894906 -0.176264 0.409970
+ 0.913786 -0.171753 0.368098
+ 0.933599 -0.126200 0.335361
+ 0.613672 -0.244079 -0.750888
+ 0.521684 -0.163010 -0.837421
+ 0.926475 -0.009818 -0.376229
+ 0.237521 0.955442 0.175256
+ 0.418938 0.358342 0.834315
+ 0.664550 0.023498 0.746874
+ 0.137739 0.899362 -0.414942
+ 0.072919 0.129094 -0.988948
+ -0.194807 0.103127 -0.975405
+ -0.550872 0.087722 -0.829967
+ -0.719332 0.067204 -0.691408
+ -0.836378 0.378026 -0.396949
+ 0.875704 -0.240341 0.418783
+ 0.835206 -0.096955 0.541323
+ -0.621842 0.558521 -0.548969
+ -0.222881 0.249142 -0.942471
+ 0.627229 -0.204259 -0.751573
+ 0.982376 0.135850 0.128386
+ 0.911286 0.040918 0.409737
+ 0.860480 -0.099890 0.499596
+ 0.985605 -0.003056 -0.169039
+ 0.543883 -0.314258 -0.778096
+ 0.345102 -0.140170 -0.928039
+ 0.557485 -0.000953 -0.830186
+ -0.016684 0.672416 0.739985
+ -0.346658 0.904563 0.248179
+ 0.250623 0.241522 0.937473
+ 0.582226 0.190640 0.790361
+ -0.794510 0.576462 -0.190905
+ -0.878558 0.016084 -0.477366
+ -0.938318 -0.037333 -0.343752
+ -0.929345 -0.219192 -0.297106
+ -0.961480 -0.230717 -0.149418
+ -0.997214 0.021301 -0.071484
+ 0.640973 -0.349335 0.683461
+ 0.741145 -0.158922 0.652264
+ -0.689605 -0.208449 -0.693537
+ 0.296046 -0.488756 -0.820655
+ 0.552984 -0.436744 -0.709551
+ 0.980419 0.168221 0.102373
+ 0.894099 0.157254 0.419354
+ 0.728210 -0.018430 0.685106
+ 0.862087 0.069851 -0.501923
+ 0.202459 -0.264249 -0.942965
+ 0.412245 -0.058501 -0.909193
+ -0.082868 0.428138 0.899906
+ -0.522019 0.567756 0.636514
+ -0.871786 0.364987 0.326761
+ 0.251348 0.206846 0.945536
+ -0.990566 0.077776 0.112823
+ -0.966066 -0.076525 0.246699
+ -0.913221 -0.267192 0.307630
+ -0.927941 -0.355117 0.113215
+ -0.933521 -0.313732 0.173524
+ -0.932024 -0.354171 -0.076771
+ -0.900257 -0.433552 -0.039627
+ 0.259064 -0.403444 0.877564
+ 0.337104 -0.336443 0.879299
+ -0.330357 -0.773500 -0.540890
+ 0.121825 -0.699179 -0.704490
+ 0.951214 0.176415 0.253119
+ 0.904180 0.142975 -0.402512
+ 0.780418 0.071036 0.621210
+ 0.223175 -0.129637 0.966120
+ 0.596928 0.005425 0.802276
+ 0.387810 -0.031418 -0.921204
+ -0.144283 -0.309167 -0.939999
+ -0.322818 0.186343 0.927936
+ -0.677547 0.146675 0.720706
+ -0.829877 0.053923 0.555335
+ -0.164727 -0.298401 0.940118
+ -0.817085 -0.207138 0.538020
+ -0.869764 -0.437579 0.228113
+ -0.749019 -0.307494 0.586871
+ -0.679926 -0.330564 0.654544
+ -0.618373 -0.740612 -0.262885
+ -0.907803 -0.417885 -0.035583
+ -0.216310 -0.386230 0.896681
+ 0.893157 0.168960 0.416801
+ 0.896556 0.210096 -0.389932
+ 0.361847 0.081016 -0.928710
+ -0.340820 -0.213472 0.915572
+ 0.020722 -0.098689 0.994903
+ 0.561636 0.058371 0.825322
+ -0.408990 -0.147235 -0.900583
+ -0.672134 -0.354247 -0.650189
+ -0.531189 -0.213584 0.819890
+ -0.631357 -0.285644 0.720969
+ -0.896757 -0.245071 -0.368466
+ -0.822669 -0.253891 0.508680
+ 0.766772 0.149214 0.624336
+ 0.958777 0.206553 -0.195145
+ 0.107671 0.067321 -0.991905
+ -0.102720 -0.084595 0.991107
+ 0.037034 -0.060121 0.997504
+ -0.827573 -0.182006 -0.531033
+ -0.866966 -0.260179 0.425061
+ 0.663038 0.081306 0.744157
+ 0.980104 0.190551 -0.055562
+ 0.200263 0.114725 -0.973002
+ -0.813169 -0.112362 -0.571079
+ -0.165647 -0.149629 0.974768
+ -0.917077 -0.251846 0.309100
+ 0.651453 0.008028 0.758647
+ 0.985762 0.132194 -0.103915
+ 0.230590 0.099625 -0.967938
+ -0.831552 -0.070904 -0.550902
+ -0.947629 -0.181463 0.262811
+ -0.293201 -0.156677 0.943125
+ 0.674489 -0.015846 0.738115
+ 0.960117 0.122492 -0.251338
+ 0.103919 0.126513 -0.986507
+ -0.858622 -0.011463 -0.512481
+ -0.943966 -0.126919 0.304663
+ -0.323975 -0.142214 0.935316
+ 0.694622 -0.041640 0.718168
+ 0.964716 0.094431 -0.245777
+ 0.133284 0.126939 -0.982915
+ -0.847722 0.019813 -0.530071
+ -0.930009 -0.088763 0.356659
+ -0.273328 -0.143887 0.951098
+ 0.568713 -0.092715 0.817294
+ 0.989318 0.037496 -0.140871
+ 0.203006 0.148306 -0.967881
+ -0.786299 0.078151 -0.612884
+ -0.952218 -0.018988 0.304829
+ -0.307225 -0.101487 0.946210
+ 0.029685 -0.087777 0.995698
+ 0.813884 -0.065576 0.577315
+ 0.970754 0.016702 -0.239496
+ 0.323498 0.148454 -0.934511
+ -0.620309 0.120937 -0.774978
+ -0.987900 0.047387 -0.147672
+ -0.693128 -0.033588 0.720032
+ -0.079809 -0.053645 0.995366
+ 0.730497 -0.080354 0.678172
+ 0.991238 -0.011776 -0.131564
+ 0.440450 0.163378 -0.882786
+ -0.638088 0.191800 -0.745692
+ -0.957516 0.088180 -0.274568
+ -0.826526 0.046376 0.560984
+ -0.105123 0.088624 0.990502
+ 0.668166 0.003549 0.744004
+ 0.984896 -0.031217 -0.170307
+ 0.655447 0.165326 -0.736924
+ -0.526182 0.384298 -0.758583
+ -0.962664 0.168434 -0.211916
+ -0.940786 0.193792 0.278150
+ -0.603696 0.536008 0.590124
+ 0.197926 0.241797 0.949926
+ 0.874792 0.244276 0.418412
+ 0.965569 0.140500 -0.218945
+ 0.800542 0.161667 -0.577059
+ -0.029579 0.540891 -0.840572
+ -0.906211 0.367711 -0.208734
+ -0.926382 0.355536 -0.124140
+ -0.432913 0.790988 0.432347
+ 0.128323 0.407512 0.904139
+ -0.632234 0.738795 -0.233372
+ 0.412952 0.199877 0.888549
+ 0.819577 0.179284 0.544197
+ 0.980842 0.104924 -0.164133
+ 0.979576 0.142384 -0.141977
+ 0.822034 0.302923 -0.482180
+ 0.720475 0.424904 -0.548063
+ 0.142688 0.695410 -0.704305
+ -0.852438 0.514110 -0.095083
+ -0.798028 0.599380 -0.062415
+ -0.763295 0.632872 -0.129823
+ -0.393313 0.684931 0.613331
+ -0.517511 0.807998 -0.281642
+ 0.042985 0.344261 0.937890
+ 0.319129 0.040561 0.946843
+ -0.641808 0.600348 -0.477143
+ 0.417315 -0.050677 0.907348
+ 0.909884 -0.132893 0.393002
+ 0.919299 -0.317031 -0.233198
+ 0.955967 -0.275303 0.101667
+ 0.996729 0.066583 -0.045813
+ 0.824255 0.191117 -0.532990
+ 0.734795 0.230154 -0.638048
+ 0.704022 0.016597 -0.709985
+ 0.363191 0.150699 -0.919447
+ -0.255564 0.696544 -0.670458
+ -0.608865 0.776567 -0.161949
+ -0.586503 0.798479 0.135814
+ -0.874673 0.483055 -0.040062
+ -0.680934 0.522324 0.513329
+ -0.361709 0.129398 0.923267
+ -0.779029 0.626735 0.017786
+ -0.561192 0.200691 -0.802986
+ -0.715720 0.437285 -0.544543
+ -0.058361 -0.212455 0.975427
+ 0.229220 -0.276080 0.933402
+ -0.955854 -0.238729 -0.171322
+ 0.267904 -0.217028 0.938683
+ 0.405175 -0.183231 0.895689
+ 0.824168 -0.528380 0.203867
+ 0.736000 -0.619969 -0.271924
+ 0.732861 -0.656691 0.177967
+ 0.748786 -0.379931 0.543113
+ 0.963385 -0.202814 -0.175372
+ 0.784645 -0.022985 -0.619519
+ 0.763106 0.031070 -0.645527
+ 0.610860 -0.202051 -0.765523
+ 0.256043 -0.121217 -0.959035
+ -0.274930 0.194311 -0.941624
+ -0.464580 0.744362 -0.479677
+ -0.257614 0.962470 -0.085358
+ -0.304962 0.869490 0.388569
+ -0.637838 0.389982 0.664137
+ -0.974788 0.213043 0.066334
+ -0.674651 0.042659 0.736903
+ -0.309275 -0.139096 0.940745
+ -0.917624 0.039633 -0.395469
+ -0.514602 -0.249746 -0.820251
+ -0.541898 -0.151019 -0.826764
+ -0.086289 -0.157910 0.983676
+ 0.149837 -0.219660 0.964001
+ -0.563968 -0.074166 0.822459
+ -0.911206 -0.401939 0.090271
+ 0.236271 -0.242893 0.940840
+ 0.429267 -0.672291 0.603120
+ 0.488615 -0.860537 -0.143988
+ 0.364479 -0.906345 -0.213761
+ 0.315225 -0.835102 0.450819
+ 0.667571 -0.696629 0.262787
+ 0.748279 -0.099234 -0.655921
+ 0.682002 -0.136847 -0.718433
+ 0.525115 -0.270636 -0.806852
+ 0.242453 -0.255318 -0.935964
+ -0.180093 -0.022591 -0.983390
+ -0.695472 0.160330 -0.700437
+ -0.634308 0.762058 0.130080
+ -0.505313 0.342637 0.791997
+ -0.862147 -0.442858 -0.246129
+ -0.847730 -0.319270 0.423580
+ -0.557615 -0.405069 0.724558
+ -0.637459 -0.598751 -0.484916
+ -0.361615 -0.535886 -0.762929
+ -0.133630 -0.652197 0.746178
+ 0.171069 -0.674443 0.718235
+ -0.629390 -0.236008 0.740384
+ -0.486903 -0.234281 0.841450
+ -0.445025 -0.492660 0.747822
+ -0.569909 -0.775635 0.271282
+ -0.388133 -0.792595 -0.470262
+ 0.165249 -0.971204 0.171626
+ 0.170678 -0.967746 -0.185303
+ -0.042601 -0.972351 0.229604
+ -0.069554 -0.922806 -0.378933
+ 0.071385 -0.838464 0.540262
+ 0.275390 -0.890944 0.361080
+ 0.623353 -0.657996 -0.422459
+ 0.560971 -0.126073 -0.818179
+ 0.432441 -0.282113 -0.856392
+ 0.238002 -0.273281 -0.932026
+ -0.131094 -0.181175 -0.974674
+ -0.769722 -0.385874 -0.508556
+ -0.981272 0.074660 0.177568
+ -0.418906 -0.783823 -0.458409
+ -0.605701 -0.767033 -0.211628
+ -0.426410 -0.904520 -0.004301
+ -0.164714 -0.827014 -0.537510
+ -0.092421 -0.994825 0.042214
+ -0.860731 -0.470662 0.193955
+ -0.553066 -0.561434 0.615556
+ -0.373460 -0.687907 0.622344
+ -0.284494 -0.697075 0.658141
+ -0.051092 -0.975455 -0.214192
+ -0.114000 -0.863752 0.490853
+ 0.148059 -0.985985 0.076890
+ 0.409762 -0.459286 -0.788132
+ 0.031087 -0.471463 -0.881338
+ -0.519429 -0.630680 -0.576572
+ -0.710903 -0.702470 -0.033951
+ -0.249898 -0.920025 -0.301837
+ -0.519729 -0.807177 0.279903
+ -0.217718 -0.969229 0.114864
+ -0.024340 -0.900412 -0.434357
+ -0.382915 -0.901852 -0.200099
+ </DataArray>
+ </PointData>
+ <Points>
+ <DataArray type="Float32" NumberOfComponents="3" format="ascii">
+ -0.002485 0.016259 -0.015792
+ 0.002267 0.007737 -0.020323
+ -0.008353 0.013258 -0.017409
+ 0.005013 0.016179 -0.015421
+ -0.001938 0.018885 -0.012364
+ 0.010107 0.016044 -0.012061
+ -0.011733 0.006253 -0.019245
+ -0.008749 -0.002138 -0.021890
+ 0.000010 -0.006513 -0.022550
+ 0.008418 -0.000804 -0.022012
+ 0.012835 0.004267 -0.017871
+ 0.011158 0.012518 -0.015341
+ -0.008158 0.016469 -0.013682
+ -0.014097 0.010306 -0.015535
+ 0.016484 0.010525 -0.011627
+ -0.002308 0.021305 -0.006500
+ 0.006930 0.019753 -0.006998
+ 0.014310 0.016141 -0.006253
+ -0.015400 0.001654 -0.017961
+ -0.010470 -0.005477 -0.020269
+ -0.000161 -0.009490 -0.021591
+ 0.008717 -0.006668 -0.021171
+ 0.014470 -0.002996 -0.018068
+ 0.017318 0.002055 -0.015207
+ -0.009431 0.019150 -0.007913
+ -0.015531 0.013876 -0.009880
+ -0.018798 0.005641 -0.012959
+ 0.019750 0.009812 -0.005479
+ -0.003142 0.022331 -0.000786
+ 0.002942 0.022217 0.000367
+ 0.009514 0.020136 0.000860
+ 0.016752 0.015302 0.001298
+ -0.016558 -0.004469 -0.016487
+ -0.009495 -0.010572 -0.019026
+ 0.000788 -0.012375 -0.020179
+ 0.009865 -0.010466 -0.018919
+ 0.017219 -0.005776 -0.014684
+ 0.021712 0.001481 -0.008411
+ -0.010106 0.020215 -0.002173
+ -0.016733 0.015443 -0.004041
+ -0.021371 0.006567 -0.007219
+ -0.020732 -0.002328 -0.011201
+ 0.023186 -0.000307 -0.005230
+ 0.021279 0.009285 0.000272
+ -0.011544 0.019532 0.004240
+ -0.004899 0.021668 0.004890
+ 0.001573 0.021757 0.006411
+ 0.007870 0.019973 0.007768
+ 0.016054 0.013591 0.009179
+ -0.015423 -0.010163 -0.014790
+ -0.007474 -0.014195 -0.017158
+ 0.002291 -0.016348 -0.017070
+ 0.013029 -0.012810 -0.014730
+ 0.019412 -0.008592 -0.010445
+ -0.017672 0.014790 0.002346
+ -0.022373 0.006494 -0.001388
+ -0.022460 -0.003854 -0.005599
+ -0.017314 -0.011841 -0.010588
+ 0.023141 -0.004235 0.001017
+ 0.021058 0.005807 0.007548
+ -0.013445 0.016710 0.008832
+ -0.019913 0.011076 0.005111
+ -0.005972 0.019438 0.010707
+ 0.001980 0.018852 0.012902
+ 0.009327 0.015115 0.014814
+ 0.014706 0.009681 0.015469
+ -0.008936 -0.016038 -0.014580
+ 0.002108 -0.018234 -0.015043
+ 0.013393 -0.015687 -0.011030
+ 0.020966 -0.010112 -0.005375
+ -0.023301 0.001183 0.001627
+ -0.021882 -0.007930 -0.003050
+ -0.015668 -0.014510 -0.011009
+ -0.006174 -0.017913 -0.013796
+ 0.020997 -0.004771 0.009481
+ 0.019198 0.001915 0.013645
+ -0.006443 0.016768 0.014819
+ -0.013328 0.014668 0.012641
+ -0.019447 0.008811 0.009583
+ -0.000481 0.015534 0.017741
+ 0.004317 0.013490 0.018908
+ 0.008703 0.008311 0.020050
+ 0.014092 -0.000326 0.019032
+ 0.005619 -0.019266 -0.012331
+ 0.014739 -0.017320 -0.004273
+ 0.019991 -0.012422 0.002254
+ -0.023245 0.000619 0.005996
+ -0.022616 -0.006988 0.001382
+ -0.016372 -0.014129 -0.004684
+ -0.007745 -0.017630 -0.008860
+ 0.017246 -0.015463 0.005797
+ 0.017002 -0.007284 0.014690
+ -0.014142 0.010676 0.015528
+ -0.006853 0.012157 0.018589
+ -0.018719 0.003918 0.012125
+ -0.020368 -0.003759 0.009983
+ -0.000651 0.009952 0.021520
+ 0.005654 0.005692 0.022054
+ 0.008514 -0.001739 0.022061
+ 0.011673 -0.009218 0.018047
+ 0.000618 -0.019771 -0.008724
+ 0.006686 -0.020452 -0.006604
+ 0.013604 -0.018660 -0.000595
+ -0.017821 -0.012360 0.002455
+ -0.010661 -0.016631 -0.001888
+ -0.003240 -0.019143 -0.002967
+ 0.012267 -0.015560 0.012292
+ -0.013258 -0.001421 0.016467
+ -0.006526 0.003737 0.018110
+ -0.017241 -0.009529 0.013278
+ -0.015091 -0.016822 0.007396
+ -0.000963 0.001430 0.020986
+ 0.005191 -0.002065 0.023396
+ 0.008210 -0.009288 0.020870
+ 0.003401 -0.019521 -0.001047
+ 0.009445 -0.018801 0.004231
+ -0.009402 -0.020722 0.004148
+ -0.000243 -0.021797 0.005431
+ 0.008817 -0.015097 0.016013
+ -0.012344 -0.007434 0.025266
+ -0.016792 -0.015301 0.021105
+ -0.004982 -0.003153 0.027602
+ -0.015739 -0.021299 0.014938
+ -0.013535 -0.027330 0.011626
+ 0.001984 -0.006429 0.030073
+ 0.005033 -0.014067 0.029609
+ 0.004745 -0.021320 0.027106
+ 0.007000 -0.019473 0.010573
+ -0.008431 -0.030151 0.009985
+ -0.001414 -0.030849 0.013004
+ 0.003590 -0.028037 0.022023
+ -0.007116 -0.003903 0.034035
+ -0.013007 -0.008966 0.030110
+ -0.016950 -0.017191 0.025608
+ -0.017874 -0.025184 0.021559
+ -0.001184 -0.006004 0.037300
+ -0.016132 -0.033301 0.014557
+ -0.013513 -0.035736 0.011876
+ 0.003743 -0.010372 0.038220
+ 0.006122 -0.016497 0.038226
+ 0.002471 -0.032059 0.030706
+ 0.003644 -0.022498 0.035590
+ -0.003690 -0.039036 0.015654
+ 0.002085 -0.037969 0.021989
+ -0.007985 -0.002618 0.040552
+ -0.002429 -0.004001 0.042826
+ -0.015659 -0.007174 0.037285
+ -0.017491 -0.015230 0.036998
+ -0.019361 -0.023764 0.031468
+ -0.020722 -0.033184 0.023048
+ 0.001913 -0.007581 0.045013
+ -0.018872 -0.040204 0.017521
+ -0.016848 -0.044566 0.012662
+ 0.003070 -0.012640 0.045930
+ 0.000569 -0.028499 0.038959
+ 0.001374 -0.019469 0.043887
+ 0.000989 -0.045425 0.023072
+ 0.001274 -0.037578 0.032132
+ -0.007933 -0.046195 0.016363
+ -0.011426 -0.000723 0.045378
+ -0.005572 -0.000975 0.049710
+ -0.001700 -0.007206 0.051340
+ -0.023433 -0.001858 0.034786
+ -0.024874 -0.011090 0.038803
+ -0.025779 -0.020496 0.037377
+ -0.027161 -0.027566 0.031359
+ -0.025479 -0.035304 0.027566
+ -0.001146 -0.016672 0.050909
+ -0.022926 -0.043275 0.020912
+ -0.021497 -0.047650 0.016181
+ -0.019536 -0.050507 0.009915
+ -0.001981 -0.033257 0.044266
+ -0.002531 -0.026081 0.047954
+ 0.001424 -0.047581 0.032985
+ -0.000447 -0.040271 0.038440
+ -0.014325 -0.052510 0.015286
+ -0.003440 -0.053634 0.020853
+ 0.002122 -0.051316 0.026469
+ -0.011591 0.000239 0.049971
+ -0.004739 -0.009025 0.054484
+ -0.003580 -0.019134 0.053367
+ -0.023620 0.001334 0.036881
+ -0.030479 -0.002425 0.031672
+ -0.032360 -0.016638 0.034891
+ -0.032541 -0.027432 0.032240
+ -0.030797 -0.037747 0.030535
+ -0.028090 -0.045295 0.023459
+ -0.004648 -0.036635 0.048922
+ -0.003984 -0.028974 0.053373
+ -0.028187 -0.050334 0.013586
+ -0.025042 -0.053680 0.008329
+ -0.019335 -0.055888 0.009401
+ 0.002156 -0.053877 0.031958
+ -0.000140 -0.049972 0.038798
+ -0.002646 -0.043430 0.044675
+ 0.002109 -0.056661 0.025747
+ -0.016134 -0.057091 0.015675
+ -0.009042 -0.058151 0.019710
+ -0.003205 -0.058627 0.021478
+ -0.015167 -0.001662 0.053526
+ -0.026389 0.000910 0.043069
+ -0.010708 -0.010851 0.057218
+ -0.008875 -0.020097 0.058543
+ -0.034061 -0.001313 0.035545
+ -0.036018 -0.017569 0.036803
+ -0.036939 -0.026980 0.037029
+ -0.035567 -0.036397 0.036969
+ -0.028678 -0.047363 0.031117
+ -0.030119 -0.053812 0.016796
+ -0.008726 -0.038108 0.053770
+ -0.007360 -0.030235 0.058452
+ -0.026958 -0.057008 0.010057
+ -0.020557 -0.059633 0.010487
+ -0.015958 -0.061216 0.018147
+ 0.002924 -0.059957 0.033868
+ -0.000767 -0.054513 0.042745
+ -0.006313 -0.048837 0.048947
+ 0.003685 -0.062765 0.027737
+ -0.008757 -0.063822 0.020503
+ -0.000899 -0.064418 0.022435
+ -0.018208 -0.006175 0.055385
+ -0.031239 -0.003890 0.048469
+ -0.035469 -0.007487 0.041888
+ -0.013727 -0.018260 0.061969
+ -0.036412 -0.015589 0.043232
+ -0.036057 -0.026457 0.045119
+ -0.033834 -0.035118 0.044942
+ -0.029490 -0.051900 0.039298
+ -0.025523 -0.058950 0.028030
+ -0.024148 -0.062041 0.035564
+ -0.029251 -0.060613 0.017851
+ -0.013466 -0.039589 0.055445
+ -0.011038 -0.029542 0.061917
+ -0.025443 -0.062217 0.011928
+ -0.016938 -0.066165 0.020558
+ 0.003558 -0.067240 0.037632
+ 0.004376 -0.069235 0.027684
+ -0.001875 -0.065544 0.045241
+ -0.011372 -0.047986 0.053040
+ -0.006027 -0.064421 0.049750
+ -0.002154 -0.070349 0.022214
+ -0.010523 -0.068838 0.021205
+ -0.020262 -0.019043 0.060748
+ -0.029105 -0.017457 0.056437
+ -0.033262 -0.019472 0.051211
+ -0.018167 -0.032425 0.060316
+ -0.031920 -0.028846 0.052585
+ -0.024391 -0.063286 0.043106
+ -0.026538 -0.055161 0.046097
+ -0.029181 -0.041940 0.049375
+ -0.022525 -0.066173 0.025092
+ -0.020076 -0.073363 0.037020
+ -0.019857 -0.043035 0.053856
+ 0.007852 -0.083425 0.035657
+ 0.007774 -0.083415 0.028230
+ 0.001670 -0.084601 0.023175
+ -0.018056 -0.053199 0.051492
+ -0.012102 -0.065376 0.051444
+ -0.000264 -0.083380 0.045736
+ -0.007277 -0.085267 0.023971
+ -0.010418 -0.076516 0.023952
+ -0.025473 -0.031527 0.057772
+ -0.019742 -0.064448 0.048286
+ -0.013514 -0.088480 0.031602
+ -0.014593 -0.086316 0.046468
+ 0.010395 -0.114751 0.042287
+ 0.013557 -0.107318 0.030510
+ 0.007304 -0.113000 0.021120
+ -0.008417 -0.085464 0.050573
+ -0.001975 -0.115539 0.048600
+ -0.005871 -0.114810 0.028131
+ -0.007908 -0.117051 0.045137
+ 0.016288 -0.145738 0.042213
+ 0.021585 -0.143279 0.030989
+ 0.013413 -0.143481 0.018024
+ 0.002221 -0.146605 0.024984
+ 0.006723 -0.147830 0.044937
+ -0.000053 -0.149090 0.039202
+ 0.022357 -0.178337 0.039752
+ 0.026764 -0.175448 0.029277
+ 0.018745 -0.175082 0.016061
+ 0.007044 -0.175149 0.022582
+ 0.005728 -0.177552 0.034434
+ 0.011849 -0.176914 0.041937
+ 0.025319 -0.206289 0.036959
+ 0.029398 -0.204312 0.022907
+ 0.018726 -0.204504 0.014052
+ 0.010333 -0.206775 0.019873
+ 0.009502 -0.208074 0.031534
+ 0.015473 -0.207935 0.038652
+ 0.028845 -0.241638 0.032493
+ 0.032097 -0.239811 0.019746
+ 0.022526 -0.238842 0.010251
+ 0.012784 -0.239322 0.015362
+ 0.011708 -0.242395 0.027188
+ 0.019081 -0.242527 0.034715
+ 0.031172 -0.274507 0.027139
+ 0.033931 -0.272860 0.016311
+ 0.024257 -0.270705 0.006213
+ 0.013304 -0.272426 0.011678
+ 0.012871 -0.275030 0.024870
+ 0.020614 -0.276345 0.030349
+ 0.020902 -0.307010 0.026991
+ 0.028519 -0.306666 0.025291
+ 0.033406 -0.305003 0.010980
+ 0.026736 -0.303639 0.001798
+ 0.017225 -0.303517 0.004273
+ 0.011122 -0.304828 0.012867
+ 0.013097 -0.306874 0.023604
+ 0.018972 -0.340119 0.024427
+ 0.029801 -0.340453 0.021958
+ 0.033036 -0.341126 0.007879
+ 0.025037 -0.338380 -0.004078
+ 0.015455 -0.336998 -0.001503
+ 0.010596 -0.337751 0.008556
+ 0.010032 -0.339151 0.020233
+ 0.016284 -0.367009 0.024252
+ 0.030513 -0.369852 0.018576
+ 0.029980 -0.368150 0.002282
+ 0.025348 -0.365105 -0.008219
+ 0.014551 -0.362330 -0.010076
+ 0.008473 -0.367519 0.006193
+ 0.007022 -0.366640 0.020488
+ 0.002655 -0.379981 0.023009
+ 0.015523 -0.381394 0.025617
+ 0.029925 -0.386391 0.020785
+ 0.029547 -0.384766 -0.000447
+ 0.022030 -0.377881 -0.013768
+ 0.012115 -0.375266 -0.018196
+ 0.008116 -0.377884 -0.009247
+ 0.006616 -0.380674 0.003266
+ -0.002897 -0.386977 0.028552
+ 0.011980 -0.394159 0.031420
+ -0.000454 -0.386982 0.014488
+ 0.022959 -0.397253 0.028606
+ 0.035306 -0.397072 0.021105
+ 0.032968 -0.395633 0.008448
+ 0.031145 -0.395746 -0.001877
+ 0.027072 -0.390989 -0.013137
+ 0.020784 -0.387953 -0.020689
+ 0.006791 -0.387902 -0.033406
+ 0.011160 -0.382117 -0.023459
+ 0.004383 -0.386023 -0.013469
+ 0.003587 -0.387849 0.000222
+ -0.009434 -0.391673 0.029127
+ -0.009795 -0.390173 0.014384
+ 0.002353 -0.398568 0.036067
+ 0.010398 -0.402182 0.034107
+ -0.006731 -0.392187 0.009106
+ 0.021525 -0.404413 0.029318
+ 0.035446 -0.405787 0.021325
+ 0.032663 -0.405177 0.008789
+ 0.030062 -0.406264 -0.000232
+ 0.032818 -0.405157 -0.007829
+ 0.025956 -0.399818 -0.018001
+ 0.019136 -0.394624 -0.027519
+ 0.017446 -0.402156 -0.030315
+ 0.011730 -0.397800 -0.036877
+ 0.002937 -0.393959 -0.037526
+ -0.000852 -0.391272 -0.025995
+ -0.001801 -0.392515 -0.016571
+ -0.002039 -0.394480 -0.001938
+ -0.016769 -0.397147 0.027677
+ -0.009767 -0.402873 0.035207
+ -0.017997 -0.395692 0.022044
+ -0.011068 -0.400730 0.006348
+ -0.016787 -0.397566 0.012051
+ -0.002143 -0.406302 0.037033
+ 0.007871 -0.408459 0.034121
+ -0.002281 -0.400084 -0.001555
+ 0.017481 -0.411851 0.029677
+ 0.024589 -0.413262 0.027714
+ 0.031195 -0.414066 0.022521
+ 0.029754 -0.413497 0.010670
+ 0.025460 -0.412837 0.000033
+ 0.029709 -0.413635 -0.006108
+ 0.033345 -0.414184 -0.010227
+ 0.029794 -0.410377 -0.016316
+ 0.024650 -0.407569 -0.021375
+ 0.015140 -0.409293 -0.030919
+ 0.006830 -0.402541 -0.038298
+ -0.007303 -0.397547 -0.036054
+ -0.015521 -0.394175 -0.027941
+ -0.003647 -0.393526 -0.028425
+ -0.010118 -0.392991 -0.018940
+ -0.008083 -0.398395 -0.013016
+ -0.022408 -0.404588 0.023297
+ -0.021165 -0.406397 0.029793
+ -0.012553 -0.410420 0.030597
+ -0.020512 -0.405620 0.013998
+ -0.002882 -0.408422 0.006231
+ -0.016366 -0.408881 0.008787
+ -0.006449 -0.413673 0.032330
+ 0.003243 -0.416680 0.031912
+ -0.001044 -0.408861 -0.009405
+ 0.004058 -0.410535 -0.001843
+ 0.016771 -0.417675 0.028481
+ 0.023161 -0.421217 0.025633
+ 0.022884 -0.421138 0.014379
+ 0.019261 -0.417790 0.004385
+ 0.020794 -0.418748 -0.004095
+ 0.029071 -0.420928 -0.011581
+ 0.027465 -0.417050 -0.018789
+ 0.021598 -0.413145 -0.022962
+ 0.012713 -0.414202 -0.031252
+ 0.002472 -0.411800 -0.036440
+ -0.007632 -0.409205 -0.036730
+ -0.020658 -0.408592 -0.033736
+ -0.017667 -0.395899 -0.020933
+ -0.016816 -0.403420 -0.013198
+ -0.021820 -0.410716 0.019655
+ -0.021244 -0.413129 0.028080
+ -0.015779 -0.417373 0.031837
+ -0.014973 -0.415510 0.012463
+ -0.005442 -0.416331 0.007583
+ -0.007523 -0.422437 0.032296
+ 0.005837 -0.423079 0.029917
+ -0.018287 -0.409922 -0.014403
+ -0.013221 -0.415616 -0.014291
+ -0.004487 -0.422012 -0.013927
+ 0.007255 -0.416320 -0.001125
+ 0.007211 -0.417681 0.005644
+ 0.013777 -0.423537 0.025725
+ 0.014433 -0.424549 0.017703
+ 0.013143 -0.418346 -0.000209
+ 0.012486 -0.421127 0.009177
+ 0.014155 -0.421520 -0.005391
+ 0.019347 -0.425500 -0.012975
+ 0.022154 -0.425019 -0.022274
+ 0.017173 -0.418753 -0.025937
+ 0.009566 -0.420713 -0.029919
+ 0.003233 -0.417783 -0.034554
+ -0.007621 -0.415450 -0.036092
+ -0.018097 -0.414535 -0.033884
+ -0.022322 -0.403616 -0.021101
+ -0.009423 -0.419038 0.012840
+ -0.014324 -0.419420 0.018578
+ -0.011563 -0.421864 0.024560
+ 0.001318 -0.420799 0.010887
+ -0.000728 -0.424582 0.023307
+ -0.020977 -0.415899 -0.023124
+ -0.014416 -0.420856 -0.018401
+ -0.003730 -0.426432 -0.016147
+ 0.006343 -0.421611 -0.007606
+ 0.006280 -0.424557 0.017171
+ 0.004537 -0.429129 -0.015807
+ 0.009081 -0.430285 -0.021312
+ 0.011900 -0.427962 -0.028185
+ -0.004435 -0.425101 -0.034302
+ -0.013769 -0.422733 -0.031571
+ -0.015841 -0.422242 -0.024399
+ -0.004460 -0.422900 0.017036
+ -0.008972 -0.426934 -0.021198
+ -0.001631 -0.429478 -0.020925
+ 0.001302 -0.428160 -0.031677
+ -0.008172 -0.427189 -0.027455
+ </DataArray>
+ </Points>
+ <Polys>
+ <DataArray type="Int32" Name="connectivity" format="ascii">
+ 0 1 2
+ 3 1 0
+ 2 4 0
+ 0 5 3
+ 0 4 5
+ 2 1 6
+ 6 1 7
+ 7 1 8
+ 8 1 9
+ 9 1 10
+ 10 1 11
+ 11 1 3
+ 2 12 4
+ 2 13 12
+ 6 13 2
+ 3 14 11
+ 3 5 14
+ 12 15 4
+ 4 16 5
+ 4 15 16
+ 5 17 14
+ 5 16 17
+ 6 18 13
+ 7 18 6
+ 7 19 18
+ 8 19 7
+ 8 20 19
+ 9 20 8
+ 9 21 20
+ 10 21 9
+ 10 22 21
+ 10 23 22
+ 11 23 10
+ 11 14 23
+ 12 24 15
+ 13 24 12
+ 13 25 24
+ 13 26 25
+ 18 26 13
+ 14 27 23
+ 14 17 27
+ 24 28 15
+ 15 29 16
+ 15 28 29
+ 16 30 17
+ 16 29 30
+ 17 31 27
+ 17 30 31
+ 18 32 26
+ 19 32 18
+ 19 33 32
+ 20 33 19
+ 20 34 33
+ 21 34 20
+ 21 35 34
+ 22 35 21
+ 22 36 35
+ 23 36 22
+ 23 37 36
+ 23 27 37
+ 24 38 28
+ 25 38 24
+ 25 39 38
+ 26 39 25
+ 26 40 39
+ 26 41 40
+ 32 41 26
+ 27 42 37
+ 27 43 42
+ 27 31 43
+ 44 45 28
+ 38 44 28
+ 28 45 29
+ 29 46 30
+ 45 46 29
+ 30 47 31
+ 46 47 30
+ 31 48 43
+ 47 48 31
+ 32 49 41
+ 33 49 32
+ 33 50 49
+ 34 50 33
+ 34 51 50
+ 35 51 34
+ 35 52 51
+ 36 52 35
+ 36 53 52
+ 37 53 36
+ 37 42 53
+ 54 44 38
+ 39 54 38
+ 55 54 39
+ 40 55 39
+ 56 55 40
+ 41 56 40
+ 57 56 41
+ 49 57 41
+ 42 58 53
+ 59 58 42
+ 43 59 42
+ 48 59 43
+ 44 60 45
+ 61 60 44
+ 54 61 44
+ 60 62 45
+ 45 62 46
+ 46 63 47
+ 62 63 46
+ 47 64 48
+ 63 64 47
+ 48 65 59
+ 64 65 48
+ 66 57 49
+ 50 66 49
+ 67 66 50
+ 51 67 50
+ 68 67 51
+ 52 68 51
+ 69 68 52
+ 53 69 52
+ 58 69 53
+ 70 61 54
+ 55 70 54
+ 71 70 55
+ 56 71 55
+ 72 71 56
+ 57 72 56
+ 73 72 57
+ 66 73 57
+ 58 74 69
+ 75 74 58
+ 59 75 58
+ 65 75 59
+ 60 76 62
+ 60 77 76
+ 61 77 60
+ 61 78 77
+ 70 78 61
+ 62 79 63
+ 62 76 79
+ 63 80 64
+ 63 79 80
+ 64 81 65
+ 64 80 81
+ 65 82 75
+ 65 81 82
+ 83 73 66
+ 67 83 66
+ 84 83 67
+ 68 84 67
+ 85 84 68
+ 69 85 68
+ 74 85 69
+ 70 86 78
+ 71 86 70
+ 71 87 86
+ 72 87 71
+ 72 88 87
+ 73 88 72
+ 73 89 88
+ 83 89 73
+ 74 90 85
+ 74 91 90
+ 75 91 74
+ 75 82 91
+ 92 93 76
+ 77 92 76
+ 76 93 79
+ 94 92 77
+ 78 94 77
+ 95 94 78
+ 86 95 78
+ 79 96 80
+ 93 96 79
+ 80 97 81
+ 96 97 80
+ 81 98 82
+ 97 98 81
+ 82 99 91
+ 98 99 82
+ 83 100 89
+ 83 101 100
+ 84 101 83
+ 84 102 101
+ 85 102 84
+ 85 90 102
+ 87 95 86
+ 103 95 87
+ 88 103 87
+ 104 103 88
+ 89 104 88
+ 105 104 89
+ 100 105 89
+ 90 106 102
+ 99 106 90
+ 91 99 90
+ 92 107 93
+ 94 107 92
+ 107 108 93
+ 93 108 96
+ 109 107 94
+ 95 109 94
+ 110 109 95
+ 103 110 95
+ 96 111 97
+ 108 111 96
+ 97 112 98
+ 111 112 97
+ 98 112 99
+ 99 113 106
+ 112 113 99
+ 114 105 100
+ 101 114 100
+ 115 114 101
+ 102 115 101
+ 106 115 102
+ 116 110 103
+ 104 116 103
+ 117 116 104
+ 105 117 104
+ 114 117 105
+ 106 118 115
+ 113 118 106
+ 107 119 108
+ 120 119 107
+ 109 120 107
+ 119 121 108
+ 108 121 111
+ 122 120 109
+ 110 122 109
+ 123 122 110
+ 116 123 110
+ 111 124 112
+ 121 124 111
+ 112 125 113
+ 124 125 112
+ 113 126 118
+ 125 126 113
+ 127 117 114
+ 115 127 114
+ 118 127 115
+ 128 123 116
+ 117 128 116
+ 129 128 117
+ 127 129 117
+ 118 130 127
+ 126 130 118
+ 119 131 121
+ 119 132 131
+ 120 132 119
+ 120 133 132
+ 120 134 133
+ 122 134 120
+ 121 135 124
+ 121 131 135
+ 122 136 134
+ 123 136 122
+ 123 137 136
+ 128 137 123
+ 124 138 125
+ 124 135 138
+ 125 139 126
+ 125 138 139
+ 126 140 130
+ 126 141 140
+ 126 139 141
+ 130 129 127
+ 128 142 137
+ 129 142 128
+ 129 143 142
+ 130 143 129
+ 130 140 143
+ 132 144 131
+ 131 145 135
+ 131 144 145
+ 132 146 144
+ 133 146 132
+ 133 147 146
+ 134 147 133
+ 134 148 147
+ 134 149 148
+ 136 149 134
+ 135 150 138
+ 135 145 150
+ 136 151 149
+ 137 151 136
+ 137 152 151
+ 142 152 137
+ 138 153 139
+ 138 150 153
+ 139 154 141
+ 139 155 154
+ 139 153 155
+ 140 156 143
+ 140 157 156
+ 141 157 140
+ 141 154 157
+ 142 158 152
+ 143 158 142
+ 143 156 158
+ 146 159 144
+ 144 160 145
+ 144 159 160
+ 145 161 150
+ 145 160 161
+ 146 162 159
+ 146 163 162
+ 147 163 146
+ 147 164 163
+ 148 164 147
+ 148 165 164
+ 149 165 148
+ 149 166 165
+ 151 166 149
+ 150 167 153
+ 150 161 167
+ 151 168 166
+ 151 169 168
+ 152 169 151
+ 152 170 169
+ 158 170 152
+ 153 171 155
+ 153 172 171
+ 167 172 153
+ 154 173 157
+ 154 174 173
+ 155 174 154
+ 155 171 174
+ 156 175 158
+ 156 176 175
+ 177 156 157
+ 177 176 156
+ 157 173 177
+ 158 175 170
+ 162 178 159
+ 159 178 160
+ 160 179 161
+ 160 178 179
+ 161 180 167
+ 161 179 180
+ 162 181 178
+ 163 182 162
+ 162 182 181
+ 164 182 163
+ 164 183 182
+ 165 183 164
+ 165 184 183
+ 165 185 184
+ 166 185 165
+ 166 186 185
+ 168 186 166
+ 167 187 172
+ 167 188 187
+ 167 180 188
+ 168 189 186
+ 169 189 168
+ 169 190 189
+ 170 190 169
+ 170 191 190
+ 175 191 170
+ 171 192 174
+ 171 193 192
+ 171 194 193
+ 172 194 171
+ 172 187 194
+ 173 195 177
+ 174 195 173
+ 174 192 195
+ 175 196 191
+ 176 196 175
+ 176 197 196
+ 198 197 176
+ 177 198 176
+ 195 198 177
+ 178 199 179
+ 200 199 178
+ 181 200 178
+ 179 201 180
+ 199 201 179
+ 180 202 188
+ 201 202 180
+ 203 200 181
+ 182 203 181
+ 204 203 182
+ 183 204 182
+ 205 204 183
+ 184 205 183
+ 206 205 184
+ 185 206 184
+ 207 206 185
+ 186 207 185
+ 208 207 186
+ 189 208 186
+ 187 209 194
+ 210 209 187
+ 188 210 187
+ 202 210 188
+ 211 208 189
+ 190 211 189
+ 212 211 190
+ 191 212 190
+ 213 212 191
+ 196 213 191
+ 192 214 195
+ 215 214 192
+ 193 215 192
+ 216 215 193
+ 194 216 193
+ 209 216 194
+ 195 217 198
+ 214 217 195
+ 218 213 196
+ 197 218 196
+ 219 218 197
+ 198 219 197
+ 217 219 198
+ 199 220 201
+ 221 220 199
+ 200 221 199
+ 222 221 200
+ 203 222 200
+ 201 220 202
+ 202 223 210
+ 220 223 202
+ 224 222 203
+ 204 224 203
+ 225 224 204
+ 205 225 204
+ 226 225 205
+ 206 226 205
+ 227 226 206
+ 207 227 206
+ 207 228 229
+ 207 229 227
+ 228 207 208
+ 228 208 230
+ 230 208 211
+ 209 231 216
+ 232 231 209
+ 210 232 209
+ 223 232 210
+ 230 211 233
+ 233 211 212
+ 233 212 234
+ 234 212 213
+ 218 234 213
+ 215 235 214
+ 214 235 236
+ 214 236 217
+ 216 237 215
+ 215 237 235
+ 238 239 216
+ 216 239 237
+ 231 238 216
+ 217 236 240
+ 217 240 219
+ 219 241 218
+ 218 241 234
+ 219 240 241
+ 220 242 223
+ 221 242 220
+ 243 242 221
+ 222 243 221
+ 244 243 222
+ 224 244 222
+ 223 245 232
+ 242 245 223
+ 225 244 224
+ 246 244 225
+ 226 246 225
+ 227 246 226
+ 227 229 247
+ 227 247 248
+ 249 246 227
+ 248 249 227
+ 250 228 230
+ 250 251 228
+ 228 251 229
+ 229 251 247
+ 250 230 233
+ 231 252 238
+ 245 252 231
+ 232 245 231
+ 250 233 234
+ 241 250 234
+ 237 253 235
+ 235 253 254
+ 235 254 236
+ 236 254 255
+ 236 255 240
+ 239 253 237
+ 256 257 238
+ 238 257 239
+ 238 252 256
+ 257 258 239
+ 239 258 253
+ 240 255 259
+ 240 259 241
+ 259 260 241
+ 260 250 241
+ 242 261 245
+ 243 261 242
+ 244 261 243
+ 246 261 244
+ 245 261 252
+ 249 261 246
+ 248 247 262
+ 247 251 262
+ 248 262 256
+ 256 249 248
+ 252 261 249
+ 252 249 256
+ 260 251 250
+ 263 251 260
+ 264 251 263
+ 262 251 264
+ 258 265 253
+ 253 265 266
+ 253 266 254
+ 254 266 267
+ 254 267 255
+ 255 267 259
+ 256 262 257
+ 262 268 257
+ 257 268 258
+ 268 269 258
+ 258 269 265
+ 259 267 270
+ 259 270 263
+ 259 263 260
+ 262 264 268
+ 263 270 271
+ 263 271 264
+ 264 271 269
+ 264 269 268
+ 269 272 265
+ 265 272 273
+ 265 273 266
+ 266 273 274
+ 266 274 267
+ 267 274 275
+ 267 275 270
+ 271 276 269
+ 269 276 272
+ 270 275 277
+ 270 277 271
+ 271 277 276
+ 276 278 272
+ 272 278 279
+ 272 279 273
+ 273 279 280
+ 273 280 274
+ 274 280 281
+ 274 281 275
+ 275 281 282
+ 275 282 277
+ 277 283 276
+ 276 283 278
+ 277 282 283
+ 283 284 278
+ 278 284 285
+ 278 285 279
+ 279 285 286
+ 279 286 280
+ 280 286 287
+ 280 287 281
+ 281 287 288
+ 281 288 282
+ 282 288 289
+ 282 289 283
+ 283 289 284
+ 289 290 284
+ 284 290 291
+ 284 291 285
+ 285 291 292
+ 285 292 286
+ 286 292 293
+ 286 293 287
+ 287 293 294
+ 287 294 288
+ 288 294 295
+ 288 295 289
+ 289 295 290
+ 295 296 290
+ 290 296 297
+ 290 297 291
+ 291 297 298
+ 291 298 292
+ 292 298 299
+ 292 299 293
+ 293 299 300
+ 293 300 294
+ 294 300 301
+ 294 301 295
+ 295 301 296
+ 301 302 296
+ 302 303 296
+ 296 303 297
+ 303 304 297
+ 304 305 297
+ 297 305 298
+ 305 306 298
+ 298 306 299
+ 306 307 299
+ 299 307 300
+ 300 308 301
+ 307 308 300
+ 308 302 301
+ 308 309 302
+ 302 309 310
+ 302 310 303
+ 303 310 311
+ 303 311 304
+ 304 311 312
+ 304 312 305
+ 305 312 313
+ 305 313 306
+ 306 313 314
+ 306 314 307
+ 307 314 315
+ 307 315 308
+ 308 315 309
+ 315 316 309
+ 309 316 317
+ 309 317 310
+ 310 317 318
+ 310 318 311
+ 311 318 319
+ 311 319 312
+ 312 319 320
+ 312 320 313
+ 313 320 321
+ 313 321 314
+ 314 321 322
+ 314 322 315
+ 315 322 316
+ 322 323 316
+ 323 324 316
+ 316 324 317
+ 324 325 317
+ 317 325 318
+ 325 326 318
+ 326 327 318
+ 318 327 319
+ 327 328 319
+ 319 328 320
+ 328 329 320
+ 320 329 321
+ 329 330 321
+ 321 330 322
+ 330 323 322
+ 331 332 323
+ 323 332 324
+ 330 333 323
+ 333 331 323
+ 332 334 324
+ 324 334 325
+ 334 335 325
+ 335 336 325
+ 325 336 326
+ 336 337 326
+ 326 337 327
+ 337 338 327
+ 327 338 328
+ 339 340 328
+ 338 339 328
+ 340 341 328
+ 328 341 329
+ 341 342 329
+ 329 342 330
+ 342 343 330
+ 343 333 330
+ 331 344 332
+ 333 345 331
+ 345 344 331
+ 344 346 332
+ 346 347 332
+ 332 347 334
+ 343 348 333
+ 348 345 333
+ 347 349 334
+ 334 349 335
+ 349 350 335
+ 335 350 336
+ 350 351 336
+ 351 352 336
+ 336 352 337
+ 352 353 337
+ 337 353 338
+ 353 354 338
+ 338 354 339
+ 354 355 339
+ 355 340 339
+ 355 356 340
+ 356 357 340
+ 357 358 340
+ 340 358 359
+ 340 359 341
+ 341 359 342
+ 359 360 342
+ 342 360 343
+ 360 361 343
+ 361 348 343
+ 362 363 344
+ 344 363 346
+ 345 364 344
+ 364 362 344
+ 348 365 345
+ 365 366 345
+ 366 364 345
+ 363 367 346
+ 346 367 347
+ 367 368 347
+ 347 368 349
+ 361 369 348
+ 369 365 348
+ 368 370 349
+ 370 371 349
+ 349 371 350
+ 371 372 350
+ 350 372 351
+ 372 373 351
+ 373 374 351
+ 351 374 352
+ 374 375 352
+ 352 375 353
+ 375 376 353
+ 376 377 353
+ 353 377 354
+ 377 378 354
+ 354 378 355
+ 378 356 355
+ 378 379 356
+ 356 379 380
+ 356 380 357
+ 357 380 381
+ 357 381 358
+ 358 381 382
+ 358 382 383
+ 358 383 359
+ 359 383 360
+ 383 384 360
+ 384 385 360
+ 360 385 361
+ 385 369 361
+ 364 386 362
+ 362 386 387
+ 362 387 363
+ 363 387 388
+ 363 388 367
+ 366 389 364
+ 364 389 386
+ 369 390 365
+ 365 390 391
+ 365 391 366
+ 366 391 389
+ 367 388 392
+ 367 392 368
+ 368 392 393
+ 368 393 370
+ 394 395 369
+ 385 394 369
+ 369 395 390
+ 370 393 396
+ 370 396 371
+ 371 396 397
+ 371 397 372
+ 372 397 398
+ 372 398 373
+ 373 398 399
+ 373 399 374
+ 400 374 399
+ 374 400 401
+ 374 401 375
+ 375 401 376
+ 376 401 402
+ 376 402 403
+ 376 403 377
+ 377 403 379
+ 377 379 378
+ 403 404 379
+ 379 404 405
+ 379 405 380
+ 380 405 406
+ 380 406 381
+ 381 406 382
+ 382 406 407
+ 382 407 408
+ 383 382 408
+ 383 408 384
+ 384 408 409
+ 384 409 385
+ 385 409 394
+ 389 410 386
+ 386 410 411
+ 386 411 387
+ 387 411 388
+ 388 411 412
+ 388 412 392
+ 391 413 389
+ 389 413 410
+ 395 414 390
+ 390 414 391
+ 391 414 413
+ 392 412 415
+ 392 415 393
+ 393 415 416
+ 393 416 396
+ 409 417 394
+ 394 417 418
+ 394 418 395
+ 395 418 419
+ 395 419 420
+ 395 421 414
+ 420 421 395
+ 396 416 397
+ 397 416 422
+ 397 422 398
+ 398 422 423
+ 398 423 399
+ 400 399 424
+ 399 423 425
+ 399 425 424
+ 400 426 427
+ 400 427 401
+ 424 426 400
+ 401 427 428
+ 401 428 402
+ 402 428 429
+ 402 429 403
+ 403 429 404
+ 429 430 404
+ 404 430 431
+ 404 431 405
+ 405 431 406
+ 406 431 432
+ 406 432 407
+ 407 432 433
+ 407 433 434
+ 408 407 434
+ 408 434 409
+ 409 434 417
+ 413 435 410
+ 410 435 436
+ 410 436 411
+ 411 436 412
+ 412 436 437
+ 412 437 415
+ 414 435 413
+ 421 438 414
+ 414 438 435
+ 415 437 439
+ 415 439 416
+ 416 439 422
+ 434 440 417
+ 417 440 441
+ 417 441 418
+ 418 441 419
+ 419 441 442
+ 419 442 443
+ 420 419 443
+ 420 443 424
+ 424 425 420
+ 420 425 421
+ 425 438 421
+ 422 439 444
+ 422 444 423
+ 423 444 425
+ 424 443 426
+ 425 444 438
+ 426 445 446
+ 426 446 427
+ 443 445 426
+ 427 446 428
+ 428 446 447
+ 428 447 429
+ 429 447 430
+ 447 448 430
+ 430 448 431
+ 432 431 448
+ 432 449 433
+ 432 448 449
+ 433 449 450
+ 433 450 440
+ 434 433 440
+ 438 451 435
+ 435 451 436
+ 436 451 437
+ 437 451 439
+ 444 451 438
+ 439 451 444
+ 440 450 452
+ 440 452 441
+ 441 452 442
+ 442 452 453
+ 442 453 445
+ 443 442 445
+ 445 453 446
+ 446 453 454
+ 446 454 447
+ 447 454 448
+ 454 455 448
+ 448 455 449
+ 449 455 450
+ 450 455 452
+ 452 455 453
+ 453 455 454
+ </DataArray>
+ <DataArray type="Int32" Name="offsets" format="ascii">
+ 3 6 9 12 15 18 21 24 27 30 33 36 39 42 45 48 51 54 57 60 63 66 69 72 75 78 81 84 87 90 93 96 99 102 105 108 111 114 117 120 123 126 129 132 135 138 141 144 147 150 153 156 159 162 165 168 171 174 177 180 183 186 189 192 195 198 201 204 207 210 213 216 219 222 225 228 231 234 237 240 243 246 249 252 255 258 261 264 267 270 273 276 279 282 285 288 291 294 297 300 303 306 309 312 315 318 321 324 327 330 333 336 339 342 345 348 351 354 357 360 363 366 369 372 375 378 381 384 387 390 393 [...]
+ </DataArray>
+ </Polys>
+ </Piece>
+ </PolyData>
+</VTKFile>
diff --git a/Simbody/tests/adhoc/CableOverBicubicSurfaces-tibia.vtp b/Simbody/tests/adhoc/CableOverBicubicSurfaces-tibia.vtp
new file mode 100644
index 0000000..bac4db0
--- /dev/null
+++ b/Simbody/tests/adhoc/CableOverBicubicSurfaces-tibia.vtp
@@ -0,0 +1,464 @@
+<?xml version="1.0"?>
+<VTKFile type="PolyData" version="0.1" byte_order="LittleEndian" compressor="vtkZLibDataCompressor">
+ <PolyData>
+ <Piece NumberOfPoints="142" NumberOfVerts="0" NumberOfLines="0" NumberOfStrips="0" NumberOfPolys="158">
+ <PointData Normals="Normals">
+ <DataArray type="Float32" Name="Normals" NumberOfComponents="3" format="ascii">
+ 0.026104 0.982554 -0.184135
+ -0.014960 0.979010 0.203264
+ 0.049215 0.452881 0.890212
+ 0.012574 -0.207697 0.978112
+ -0.153481 -0.500705 0.851903
+ -0.192885 -0.344881 0.918615
+ -0.150209 -0.050627 0.987357
+ -0.187121 -0.009864 0.982287
+ -0.151046 0.032579 0.987990
+ -0.178578 0.181105 0.967115
+ -0.108962 0.258460 0.959857
+ 0.112415 -0.151640 0.982023
+ 0.277233 -0.946951 0.162560
+ 0.077445 -0.915073 -0.395781
+ 0.063460 -0.964583 0.256034
+ 0.238699 -0.942708 0.233077
+ 0.365907 0.927305 0.078848
+ 0.675838 0.510886 0.531261
+ 0.628821 -0.009172 0.777496
+ 0.417362 -0.248889 0.873993
+ 0.214861 -0.271632 0.938110
+ 0.105107 -0.084581 0.990858
+ 0.207089 -0.032410 0.977785
+ 0.390661 0.009946 0.920481
+ 0.462914 0.174273 0.869103
+ 0.583372 0.220021 0.781836
+ 0.644387 -0.064219 0.761998
+ 0.313435 -0.932997 0.176847
+ -0.462263 -0.804806 -0.372289
+ -0.516029 -0.846722 0.129520
+ 0.521506 0.852955 0.022333
+ 0.937141 0.313706 0.152828
+ 0.939477 0.185358 0.288142
+ 0.937654 0.155695 0.310748
+ 0.890760 -0.059736 0.450532
+ 0.799145 -0.102859 0.592273
+ 0.894351 -0.110492 0.433506
+ 0.940846 -0.063695 0.332794
+ 0.964331 0.020173 0.263931
+ 0.968665 0.089455 0.231701
+ 0.934285 0.176601 0.309714
+ 0.906698 -0.369647 0.203128
+ 0.091665 -0.995258 0.032549
+ -0.313761 -0.932870 0.176941
+ 0.543565 0.795953 -0.266448
+ 0.943921 0.189193 -0.270591
+ 0.935949 0.305374 -0.175346
+ 0.915407 0.298178 -0.270408
+ 0.933568 -0.081727 -0.348958
+ 0.920782 -0.093179 -0.378785
+ 0.828105 -0.143275 -0.541954
+ 0.861807 -0.094471 -0.498361
+ 0.911553 -0.021317 -0.410630
+ 0.933407 0.065280 -0.352831
+ 0.912974 0.301395 -0.275025
+ 0.947972 0.039131 -0.315938
+ 0.559121 -0.800855 -0.214511
+ -0.118470 -0.992615 0.026074
+ 0.272825 0.879168 -0.390679
+ 0.723004 0.532571 -0.440038
+ 0.739502 0.407353 -0.535910
+ 0.646465 0.337415 -0.684276
+ 0.594710 -0.063748 -0.801409
+ 0.272292 -0.243645 -0.930857
+ 0.079818 -0.110277 -0.990691
+ 0.229387 -0.061858 -0.971368
+ 0.421766 -0.012026 -0.906625
+ 0.513819 0.057182 -0.855991
+ 0.593585 0.284311 -0.752877
+ 0.575583 0.055560 -0.815854
+ 0.351954 -0.715507 -0.603471
+ -0.043423 -0.999056 -0.001157
+ -0.031614 0.990925 -0.130644
+ -0.037095 0.992768 -0.114173
+ 0.004335 0.596621 -0.802511
+ -0.101517 0.207897 -0.972868
+ 0.137235 -0.110776 -0.984325
+ 0.379109 -0.354815 -0.854624
+ 0.229113 -0.163328 -0.959600
+ 0.105145 -0.051551 -0.993120
+ 0.059631 -0.002644 -0.998217
+ 0.083805 0.048183 -0.995317
+ 0.041418 0.246142 -0.968348
+ -0.073799 0.051354 -0.995950
+ 0.049545 -0.559132 -0.827597
+ 0.239518 -0.954554 -0.177364
+ -0.288741 0.935561 -0.203359
+ -0.615503 0.772240 -0.157486
+ -0.744439 0.144751 -0.651811
+ -0.770644 -0.105344 -0.628498
+ -0.548356 -0.268001 -0.792137
+ 0.252230 -0.359319 -0.898482
+ 0.199139 -0.163063 -0.966309
+ -0.142571 -0.039059 -0.989014
+ -0.378921 0.010928 -0.925365
+ -0.389121 0.073280 -0.918267
+ -0.542606 0.208467 -0.813708
+ -0.561209 -0.140268 -0.815701
+ -0.237039 -0.580417 -0.779057
+ 0.381323 -0.831336 -0.404318
+ -0.508221 0.831637 -0.223811
+ -0.907647 0.133050 -0.398088
+ -0.729089 -0.545060 -0.413931
+ -0.847523 -0.496855 -0.186655
+ -0.932095 -0.246601 -0.265306
+ -0.861820 -0.116328 -0.493695
+ -0.808043 -0.080738 -0.583566
+ -0.876999 -0.001908 -0.480489
+ -0.915677 0.037598 -0.400152
+ -0.902519 0.147905 -0.404455
+ -0.936276 0.056938 -0.346621
+ -0.647045 -0.669653 -0.364550
+ -0.449493 -0.807215 -0.382571
+ 0.146867 -0.937235 -0.316260
+ -0.612523 0.770857 0.174916
+ -0.910045 -0.198379 0.363956
+ -0.583723 -0.789956 0.187716
+ -0.652539 -0.733596 0.189814
+ -0.888528 -0.246416 0.387036
+ -0.887087 -0.024943 0.460928
+ -0.921601 -0.026930 0.387203
+ -0.935863 0.014696 0.352059
+ -0.955866 0.049836 0.289547
+ -0.950639 0.182353 0.251065
+ -0.927045 -0.086946 0.364729
+ -0.409521 -0.853025 0.323482
+ -0.361870 -0.841058 0.402083
+ 0.141917 -0.849843 0.507569
+ -0.450041 0.837202 0.310735
+ -0.560299 0.053002 0.826593
+ -0.444284 -0.575854 0.686298
+ -0.441880 -0.633114 0.635538
+ -0.415833 -0.266469 0.869527
+ -0.301151 -0.030609 0.953085
+ -0.430750 -0.021867 0.902207
+ -0.506645 0.017868 0.861969
+ -0.582024 0.090835 0.808082
+ -0.627343 0.210026 0.749886
+ -0.497161 -0.098025 0.862103
+ -0.046693 -0.853293 0.519337
+ 0.213005 -0.820823 0.529980
+ 0.479420 -0.618750 0.622338
+ </DataArray>
+ </PointData>
+ <Points>
+ <DataArray type="Float32" NumberOfComponents="3" format="ascii">
+ 0.006000 -0.031255 -0.003181
+ 0.006000 -0.028520 0.008335
+ 0.006000 -0.038777 0.028074
+ 0.006000 -0.048694 0.029719
+ 0.006000 -0.068186 0.023687
+ 0.006000 -0.079129 0.013818
+ 0.006000 -0.133499 0.007785
+ 0.006000 -0.235059 0.005866
+ 0.006000 -0.304476 0.006689
+ 0.006000 -0.368081 0.011350
+ 0.006000 -0.390306 0.020945
+ 0.006000 -0.405352 0.023414
+ 0.006000 -0.415269 0.017380
+ 0.006000 -0.403302 0.013269
+ 0.006000 -0.400565 0.008882
+ 0.006000 -0.403984 -0.003181
+ 0.016460 -0.031255 0.009683
+ 0.023854 -0.040317 0.018777
+ 0.024485 -0.052627 0.019554
+ 0.024937 -0.068357 0.020108
+ 0.022682 -0.080667 0.017336
+ 0.019616 -0.117256 0.013565
+ 0.017092 -0.186161 0.010460
+ 0.014837 -0.254550 0.007688
+ 0.014927 -0.320036 0.007798
+ 0.018624 -0.365173 0.012346
+ 0.020879 -0.396121 0.015118
+ 0.020788 -0.406891 0.015007
+ 0.018263 -0.405010 0.011902
+ 0.013214 -0.404839 0.005692
+ 0.027593 -0.033990 0.003088
+ 0.030512 -0.041855 0.003937
+ 0.030804 -0.056559 0.004021
+ 0.038683 -0.068529 0.006308
+ 0.039000 -0.082205 0.007241
+ 0.038389 -0.101014 0.006224
+ 0.032262 -0.137261 0.004445
+ 0.024092 -0.204625 0.002072
+ 0.019423 -0.271990 0.000716
+ 0.021174 -0.340039 0.001225
+ 0.025842 -0.386886 0.002580
+ 0.031970 -0.398514 0.004361
+ 0.028176 -0.406719 0.003259
+ 0.016505 -0.409115 -0.000131
+ 0.027593 -0.033990 -0.009449
+ 0.030512 -0.041855 -0.010298
+ 0.030804 -0.056559 -0.010382
+ 0.038683 -0.068529 -0.012669
+ 0.039000 -0.082205 -0.013603
+ 0.038389 -0.101014 -0.012585
+ 0.032262 -0.137261 -0.010806
+ 0.024092 -0.204625 -0.008433
+ 0.019423 -0.271990 -0.007078
+ 0.021174 -0.340039 -0.007586
+ 0.025842 -0.386886 -0.008942
+ 0.031970 -0.398514 -0.010722
+ 0.028176 -0.406719 -0.009620
+ 0.016505 -0.409115 -0.006230
+ 0.016009 -0.034675 -0.015491
+ 0.019165 -0.036555 -0.019372
+ 0.024576 -0.045959 -0.026026
+ 0.027371 -0.062202 -0.029463
+ 0.028633 -0.070580 -0.031017
+ 0.024304 -0.084087 -0.025692
+ 0.019797 -0.110418 -0.020148
+ 0.015558 -0.169404 -0.014935
+ 0.013754 -0.253356 -0.012718
+ 0.014566 -0.320720 -0.013717
+ 0.017812 -0.378509 -0.017707
+ 0.022051 -0.392871 -0.022920
+ 0.019616 -0.403472 -0.019927
+ 0.013574 -0.409115 -0.012495
+ 0.006000 -0.035359 -0.013324
+ 0.006000 -0.031255 -0.020178
+ 0.006000 -0.035359 -0.036354
+ 0.006000 -0.055875 -0.037450
+ 0.006000 -0.058953 -0.038273
+ 0.006000 -0.067159 -0.028403
+ 0.006000 -0.083575 -0.020452
+ 0.006000 -0.134183 -0.015243
+ 0.006000 -0.234718 -0.014147
+ 0.006000 -0.301399 -0.014970
+ 0.006000 -0.370131 -0.020452
+ 0.006000 -0.387229 -0.027580
+ 0.006000 -0.400223 -0.023742
+ 0.006000 -0.409115 -0.016339
+ -0.007525 -0.034503 -0.019815
+ -0.011132 -0.042881 -0.024252
+ -0.015371 -0.047497 -0.029463
+ -0.013477 -0.059294 -0.027134
+ -0.011312 -0.069554 -0.024473
+ -0.007256 -0.101014 -0.019482
+ -0.004641 -0.126660 -0.016267
+ -0.001394 -0.202231 -0.012274
+ -0.002747 -0.286523 -0.013938
+ -0.003198 -0.336963 -0.014492
+ -0.008879 -0.388938 -0.021478
+ -0.010321 -0.400052 -0.023253
+ -0.005542 -0.406550 -0.017376
+ -0.001935 -0.414585 -0.012939
+ -0.026973 -0.033649 -0.012754
+ -0.031351 -0.054507 -0.014025
+ -0.027850 -0.059636 -0.013008
+ -0.020554 -0.062715 -0.010890
+ -0.012676 -0.080155 -0.008602
+ -0.010049 -0.134867 -0.007841
+ -0.010049 -0.169746 -0.007841
+ -0.005088 -0.270283 -0.006401
+ -0.010632 -0.338330 -0.008009
+ -0.011216 -0.372526 -0.008178
+ -0.023764 -0.407746 -0.011821
+ -0.020846 -0.412876 -0.010974
+ -0.009466 -0.412876 -0.007671
+ -0.005672 -0.420056 -0.006569
+ -0.026973 -0.033649 0.006393
+ -0.031351 -0.054507 0.007664
+ -0.027850 -0.059636 0.006647
+ -0.020554 -0.062715 0.004528
+ -0.012676 -0.080155 0.002242
+ -0.010049 -0.134867 0.001480
+ -0.010049 -0.169746 0.001480
+ -0.005088 -0.270283 0.000040
+ -0.010632 -0.338330 0.001648
+ -0.011216 -0.372526 0.001817
+ -0.023764 -0.407746 0.005459
+ -0.020846 -0.412876 0.004614
+ -0.009466 -0.412876 0.001310
+ -0.005672 -0.420056 0.000207
+ -0.007977 -0.031083 0.014009
+ -0.015821 -0.046642 0.023657
+ -0.015280 -0.054165 0.022992
+ -0.011042 -0.065450 0.017780
+ -0.005362 -0.079642 0.010793
+ -0.002566 -0.134183 0.007355
+ -0.001935 -0.202403 0.006578
+ -0.000673 -0.287377 0.005027
+ -0.003919 -0.353205 0.009019
+ -0.007256 -0.381415 0.013121
+ -0.011944 -0.406550 0.018889
+ -0.009058 -0.414073 0.015340
+ -0.004190 -0.408089 0.009352
+ -0.001574 -0.410312 0.006135
+ </DataArray>
+ </Points>
+ <Polys>
+ <DataArray type="Int32" Name="connectivity" format="ascii">
+ 0 1 16
+ 1 2 17 16
+ 2 3 18 17
+ 3 4 19 18
+ 4 5 20 19
+ 5 6 21 20
+ 6 7 22 21
+ 7 8 23 22
+ 8 9 24 23
+ 9 10 25 24
+ 10 11 26 25
+ 11 12 27 26
+ 12 13 28 27
+ 13 14 29 28
+ 14 15 29
+ 0 16 30
+ 16 17 31 30
+ 17 18 32 31
+ 18 19 33 32
+ 20 34 19
+ 19 34 33
+ 21 35 20
+ 20 35 34
+ 21 22 36 35
+ 22 23 37 36
+ 23 24 38 37
+ 24 25 39 38
+ 25 26 40 39
+ 26 27 41 40
+ 27 28 42 41
+ 28 29 43 42
+ 29 15 43
+ 0 30 44
+ 30 31 45 44
+ 31 32 46 45
+ 32 33 47 46
+ 34 48 33
+ 33 48 47
+ 35 49 34
+ 34 49 48
+ 35 36 50 49
+ 36 37 51 50
+ 37 38 52 51
+ 38 39 53 52
+ 39 40 54 53
+ 40 41 55 54
+ 41 42 56 55
+ 42 43 57 56
+ 43 15 57
+ 0 44 58
+ 44 45 59 58
+ 45 46 60 59
+ 46 47 61 60
+ 48 62 47
+ 47 62 61
+ 49 63 48
+ 48 63 62
+ 49 50 64 63
+ 50 51 65 64
+ 51 52 66 65
+ 52 53 67 66
+ 53 54 68 67
+ 54 55 69 68
+ 55 56 70 69
+ 56 57 71 70
+ 57 15 71
+ 0 58 72
+ 58 59 73 72
+ 59 60 74 73
+ 60 61 75 74
+ 61 62 76 75
+ 62 63 77 76
+ 63 64 78 77
+ 64 65 79 78
+ 65 66 80 79
+ 66 67 81 80
+ 67 68 82 81
+ 68 69 83 82
+ 69 70 84 83
+ 70 71 85 84
+ 71 15 85
+ 0 72 86
+ 72 73 87 86
+ 73 74 88 87
+ 74 75 89 88
+ 75 76 90 89
+ 76 77 91 90
+ 77 78 92 91
+ 78 79 93 92
+ 79 80 94 93
+ 80 81 95 94
+ 81 82 96 95
+ 82 83 97 96
+ 83 84 98 97
+ 84 85 99 98
+ 85 15 99
+ 0 86 100
+ 86 87 101 100
+ 88 102 101
+ 87 88 101
+ 88 89 103 102
+ 89 90 104 103
+ 90 91 105 104
+ 91 92 106 105
+ 92 93 107 106
+ 93 94 108 107
+ 94 95 109 108
+ 95 96 110 109
+ 96 97 111 110
+ 97 98 112 111
+ 98 99 113 112
+ 99 15 113
+ 0 100 114
+ 100 101 115 114
+ 101 102 116 115
+ 102 103 117 116
+ 103 104 118 117
+ 104 105 119 118
+ 105 106 120 119
+ 106 107 121 120
+ 107 108 122 121
+ 108 109 123 122
+ 109 110 124 123
+ 110 111 125 124
+ 111 112 126 125
+ 112 113 127 126
+ 113 15 127
+ 0 114 128
+ 114 115 129 128
+ 115 116 130 129
+ 116 117 131 130
+ 117 118 132 131
+ 118 119 133 132
+ 119 120 134 133
+ 120 121 135 134
+ 121 122 136 135
+ 122 123 137 136
+ 123 124 138 137
+ 124 125 139 138
+ 125 126 140 139
+ 126 127 141 140
+ 127 15 141
+ 0 128 1
+ 128 129 2 1
+ 129 130 3 2
+ 130 131 4 3
+ 131 132 5 4
+ 132 133 6 5
+ 133 134 7 6
+ 134 135 8 7
+ 135 136 9 8
+ 136 137 10 9
+ 137 138 11 10
+ 138 139 12 11
+ 139 140 13 12
+ 140 141 14 13
+ 141 15 14
+ 88 89 102
+ </DataArray>
+ <DataArray type="Int32" Name="offsets" format="ascii">
+ 3 7 11 15 19 23 27 31 35 39 43 47 51 55 58 61 65 69 73 76 79 82 85 89 93 97 101 105 109 113 117 120 123 127 131 135 138 141 144 147 151 155 159 163 167 171 175 179 182 185 189 193 197 200 203 206 209 213 217 221 225 229 233 237 241 244 247 251 255 259 263 267 271 275 279 283 287 291 295 299 302 305 309 313 317 321 325 329 333 337 341 345 349 353 357 360 363 367 370 373 377 381 385 389 393 397 401 405 409 413 417 420 423 427 431 435 439 443 447 451 455 459 463 467 471 475 478 481 485 [...]
+ </DataArray>
+ </Polys>
+ </Piece>
+ </PolyData>
+</VTKFile>
diff --git a/Simbody/tests/adhoc/CableOverBicubicSurfaces.cpp b/Simbody/tests/adhoc/CableOverBicubicSurfaces.cpp
new file mode 100644
index 0000000..c7866a9
--- /dev/null
+++ b/Simbody/tests/adhoc/CableOverBicubicSurfaces.cpp
@@ -0,0 +1,245 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Adhoc Test: Cable Over Bicubic Surfaces *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Andreas Scholz *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* Simbody CableOverBicubicSurfaces
+This example shows how to use a CableTrackerSubsystem to follow the motion of
+a cable that crosses bicubic surfaces. We'll then
+create a force element that generates spring forces that result from the
+stretching and stretching rate of the cable. */
+
+#include "Simbody.h"
+
+#include <cassert>
+#include <iostream>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+// This gets called periodically to dump out interesting things about
+// the cables and the system as a whole. It also saves states so that we
+// can play back at the end.
+static Array_<State> saveStates;
+class ShowStuff : public PeriodicEventReporter {
+public:
+ ShowStuff(const MultibodySystem& mbs,
+ const CableSpring& cable1, Real interval)
+ : PeriodicEventReporter(interval),
+ mbs(mbs), cable1(cable1) {}
+
+ static void showHeading(std::ostream& o) {
+ printf("%8s %10s %10s %10s %10s %10s %10s %10s %10s %12s\n",
+ "time", "length", "rate", "integ-rate", "unitpow", "tension", "disswork",
+ "KE", "PE", "KE+PE-W");
+ }
+
+ /** This is the implementation of the EventReporter virtual. **/
+ void handleEvent(const State& state) const OVERRIDE_11 {
+ const CablePath& path1 = cable1.getCablePath();
+ printf("%8g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %12.6g CPU=%g\n",
+ state.getTime(),
+ path1.getCableLength(state),
+ path1.getCableLengthDot(state),
+ path1.getIntegratedCableLengthDot(state),
+ path1.calcCablePower(state, 1), // unit power
+ cable1.getTension(state),
+ cable1.getDissipatedEnergy(state),
+ mbs.calcKineticEnergy(state),
+ mbs.calcPotentialEnergy(state),
+ mbs.calcEnergy(state)
+ + cable1.getDissipatedEnergy(state),
+ cpuTime());
+ saveStates.push_back(state);
+ }
+private:
+ const MultibodySystem& mbs;
+ CableSpring cable1;
+};
+
+int main() {
+ try {
+ // Create the system.
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ CableTrackerSubsystem cables(system);
+ GeneralForceSubsystem forces(system);
+
+ Force::Gravity gravity(forces, matter, -YAxis, 9.81);
+ //Force::GlobalDamper(forces, matter, 5);
+
+ system.setUseUniformBackground(true); // no ground plane in display
+ MobilizedBody Ground = matter.Ground(); // convenient abbreviation
+
+ // Read in some bones.
+ PolygonalMesh femur, tibia;
+ femur.loadVtpFile("CableOverBicubicSurfaces-femur.vtp");
+ tibia.loadVtpFile("CableOverBicubicSurfaces-tibia.vtp");
+ femur.scaleMesh(20);
+ tibia.scaleMesh(20);
+
+ // Create some bicubic surfaces.
+ const int Nx = 4, Ny = 5;
+ const Real xData[Nx] = { .1, 1, 2, 4 };
+ const Real yData[Ny] = { -3, -2, 0, 1, 3 };
+ const Real fData[Nx*Ny] = { 1, 2, 3, 3, 2,
+ 1.1, 2.1, 3.1, 3.1, 2.1,
+ 1, 2, 7, 3, 2,
+ 1.2, 2.2, 3.2, 3.2, 2.2 };
+ const Vector x(Nx, xData);
+ const Vector y(Ny, yData);
+ const Matrix f(Nx,Ny, fData);
+ BicubicSurface rough(x, y, f, 0); // raw
+ BicubicSurface smooth(x, y, f, 1); // smoothed
+
+ Vector xp(Vec2(.25,3.25)), yp(Vec2(.75,5.75));
+ // Nice patch:
+ //Matrix fp(Mat22(1, 1,
+ // 1, 1));
+ //Matrix fxp(.5*Mat22(-1, 0,
+ // 0, -1));
+ //Matrix fyp(.5*Mat22(-1, 0,
+ // 0, -1));
+ //Matrix fxyp(0*Mat22(1, -1,
+ // -1, 1));
+ // One-hump patch:
+ Matrix fp(Mat22(1, 1,
+ 1, 1));
+ Matrix fxp(Mat22(1, 1,
+ -1, -1));
+ Matrix fyp(Mat22(1, -1,
+ 1, -1));
+ Matrix fxyp(0.5*Mat22(1, 3,
+ -3, 4));
+ BicubicSurface patch(xp, yp, fp, fxp, fyp, fxyp);
+ Rotation xm90(-Pi/2, XAxis);
+ Transform patchPose(xm90, Vec3(4,2,0));
+
+ // Ask the bicubic surfaces for some meshes we can use for display.
+ Real resolution = 31;
+ PolygonalMesh patchMesh = patch.createPolygonalMesh(resolution);
+ PolygonalMesh roughMesh = rough.createPolygonalMesh(resolution);
+ PolygonalMesh smoothMesh = smooth.createPolygonalMesh(resolution);
+
+ const Vec3 SmoothOrigin(-3,-3,-3);
+ Ground.addBodyDecoration(SmoothOrigin,
+ DecorativeMesh(smooth.createPolygonalMesh(0))
+ .setRepresentation(DecorativeGeometry::DrawWireframe));
+
+ // Not using these yet:
+ Ground.addBodyDecoration(Vec3(5,-5,0),
+ DecorativeMesh(patchMesh).setColor(Gray));
+ Ground.addBodyDecoration(Vec3(5,0,0),
+ DecorativeMesh(roughMesh).setColor(Red));
+ Ground.addBodyDecoration(Vec3(5,0,0),
+ DecorativeMesh(femur).setColor(Vec3(.8,.8,.8)));
+ Ground.addBodyDecoration(Vec3(5,-4,0),
+ DecorativeMesh(tibia).setColor(Vec3(.8,.8,.8)));
+
+
+ Body::Rigid someBody(MassProperties(2.0, Vec3(0,-4,0),
+ UnitInertia::cylinderAlongY(1,4).shiftFromCentroid(Vec3(0,4,0))));
+
+ someBody.addDecoration(Transform(Rotation(Pi,ZAxis),Vec3(0,-4,0)),
+ DecorativeCylinder(1,4).setColor(Yellow)
+ .setOpacity(.5).setResolution(4));
+ someBody.addDecoration(Transform(),
+ DecorativeMesh(femur).setColor(Vec3(.8,.8,.8)));
+
+ MobilizedBody::Free body1(Ground, Transform(Vec3(0)),
+ someBody, Transform(Vec3(0,0,0)));
+
+ CablePath path1(cables, Ground, Vec3(.5,-.5,0), // origin
+ body1, Vec3(0,0,0)); // termination
+
+ CableObstacle::Surface obstacle1(path1, Ground, SmoothOrigin,
+ ContactGeometry::SmoothHeightMap(smooth));
+ // Provide an initial guess for P and Q (in frame of "smooth").
+ Vec3 P1(1.5,1,3.75), Q1(1.5,-1,3.75);
+ obstacle1.setContactPointHints(P1, Q1);
+
+ Ground.addBodyDecoration(SmoothOrigin,
+ DecorativePoint(P1).setColor(Green).setScale(2));
+ Ground.addBodyDecoration(SmoothOrigin,
+ DecorativePoint(Q1).setColor(Red).setScale(2));
+
+ CableSpring cable1(forces, path1, 50., 8., 0.1);
+
+ //Force::TwoPointLinearSpring spring1(forces, body1, Vec3(0,-8,0),
+ // Ground, Vec3(0,0,0), 30., 12.);
+
+ Visualizer viz(system);
+ viz.setShowFrameNumber(true);
+ system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
+ system.addEventReporter(new ShowStuff(system, cable1, 0.02));
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ //Random::Gaussian random;
+ //for (int i = 0; i < state.getNQ(); ++i)
+ // state.updQ()[i] = random.getValue();
+ //for (int i = 0; i < state.getNU(); ++i)
+ // state.updU()[i] = 0.1*random.getValue();
+ body1.setQToFitTranslation(state, Vec3(4,-10,-3));
+ body1.setQToFitRotation(state, Rotation(-Pi, ZAxis));
+
+ system.realize(state, Stage::Position);
+ viz.report(state);
+ cout << "path1 init length=" << path1.getCableLength(state) << endl;
+ cout << "Hit ENTER ...";
+ getchar();
+
+ path1.setIntegratedCableLengthDot(state, path1.getCableLength(state));
+
+
+ // Simulate it.
+ saveStates.clear(); saveStates.reserve(2000);
+
+ //RungeKutta3Integrator integ(system);
+ RungeKuttaMersonIntegrator integ(system);
+ //CPodesIntegrator integ(system);
+ //integ.setAllowInterpolation(false);
+ integ.setAccuracy(1e-3);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ShowStuff::showHeading(cout);
+
+ const Real finalTime = 10;
+ const double startTime = realTime(), startCPU = cpuTime();
+ ts.stepTo(finalTime);
+ cout << "DONE with " << finalTime
+ << "s simulated in " << realTime()-startTime
+ << "s elapsed, " << cpuTime()-startCPU << " s CPU.\n";
+
+
+ while (true) {
+ cout << "Hit ENTER FOR REPLAY, Q to quit ...";
+ const char ch = getchar();
+ if (ch=='q' || ch=='Q') break;
+ for (unsigned i=0; i < saveStates.size(); ++i)
+ viz.report(saveStates[i]);
+ }
+
+ } catch (const std::exception& e) {
+ cout << "EXCEPTION: " << e.what() << "\n";
+ }
+}
diff --git a/Simbody/tests/adhoc/CoarseRNA.cpp b/Simbody/tests/adhoc/CoarseRNA.cpp
new file mode 100644
index 0000000..7f056c9
--- /dev/null
+++ b/Simbody/tests/adhoc/CoarseRNA.cpp
@@ -0,0 +1,412 @@
+/* -------------------------------------------------------------------------- *
+ * SimTK 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * A big, chunky fake RNA built of cylinders and ball joints.
+ */
+
+#include "SimTKsimbody.h"
+
+#include <cmath>
+#include <cstdio>
+#include <exception>
+#include <vector>
+#include <ctime>
+
+using namespace std;
+using namespace SimTK;
+
+static const Real Deg2Rad = (Real)SimTK_DEGREE_TO_RADIAN;
+static const int GroundBodyNum = 0; // ground is always body 0
+
+static const Real g = 9.8; // meters/s^2; apply in �y direction
+
+static const Real DuplexRadius = 3; // A
+static const Real HalfHeight = 10; // A
+static const Real CylinderSlop = 1; // A
+
+static const int NAtoms = 20;
+static const Real AtomMass = 12; // Daltons
+static const Real AtomRadius = 1; // A
+
+static const Real ConnectorRadius = 1; // A
+static const Real ConnectorHalfHeight = 3; // A
+static const Real ConnectorEndSlop = 0.2;// A
+static const Real ConnectorDensity = 10; // Dalton/A^3
+
+static int NSegments = 3;
+
+class MyRNAExample : public SimbodyMatterSubsystem {
+ struct PerBodyInfo {
+ PerBodyInfo(MobilizedBodyIndex b, bool d) : bnum(b), isDuplex(d) { }
+ MobilizedBodyIndex bnum;
+ bool isDuplex;
+ };
+ std::vector<PerBodyInfo> bodyInfo;
+ MobilizedBodyIndex end1, end2;
+public:
+ MyRNAExample(MultibodySystem& mbs, int nsegs, bool shouldFlop) : SimbodyMatterSubsystem(mbs)
+ {
+ bodyInfo.push_back(PerBodyInfo(GroundIndex, false)); // placeholder for ground
+ end1 = makeChain(GroundIndex, Vec3(0), nsegs, shouldFlop);
+ end2 = makeChain(GroundIndex, Vec3(20,0,0), nsegs, shouldFlop);
+
+ if (true) {
+ Constraint::Rod theConstraint2(updMobilizedBody(end1), Vec3(0, -HalfHeight,0),
+ updMobilizedBody(end2), Vec3(0, -HalfHeight,0), 10);
+ }
+
+ }
+
+ void decorateBody(MobilizedBodyIndex bodyNum, Visualizer& display) const {
+ assert(bodyInfo[bodyNum].bnum == bodyNum);
+ if (bodyInfo[bodyNum].isDuplex)
+ addDuplexDecorations(bodyNum, DuplexRadius, HalfHeight, CylinderSlop,
+ NAtoms, AtomRadius, display);
+ else
+ addConnectorDecorations(bodyNum, ConnectorRadius, ConnectorHalfHeight,
+ ConnectorEndSlop, display);
+ }
+
+ void decorateGlobal(Visualizer& display) const {
+ // nothing
+ }
+
+private:
+
+ MobilizedBodyIndex makeChain(MobilizedBodyIndex startBodyId, const Vec3& startOrigin, int nSegs, bool shouldFlop) {
+ //MobilizedBodyIndex baseBodyIx = startBody;
+ MobilizedBody baseBody = updMobilizedBody(startBodyId);
+ Vec3 origin = startOrigin;
+ MobilizedBody lastDup;
+ for (int seg=0; seg < nSegs; ++seg) {
+
+ MobilizedBody::Ball left1(
+ baseBody, Transform(origin + Vec3(-DuplexRadius,-HalfHeight,0)),
+ Body::Rigid(calcConnectorMassProps(ConnectorRadius, ConnectorHalfHeight, ConnectorDensity)),
+ Transform(Vec3(0, ConnectorHalfHeight, 0)));
+ left1.setDefaultRadius(1.5);
+ bodyInfo.push_back(PerBodyInfo(left1, false));
+
+ MobilizedBody::Ball left2(
+ left1, Transform(Vec3(0, -ConnectorHalfHeight, 0)),
+ Body::Rigid(calcConnectorMassProps(ConnectorRadius, ConnectorHalfHeight, ConnectorDensity)),
+ Transform(Vec3(0, ConnectorHalfHeight, 0)));
+ left2.setDefaultRadius(1.5);
+ bodyInfo.push_back(PerBodyInfo(left2, false));
+
+ MobilizedBody::Ball rt1(
+ baseBody, Transform(origin + Vec3(DuplexRadius,-HalfHeight,0)),
+ Body::Rigid(calcConnectorMassProps(ConnectorRadius, ConnectorHalfHeight, ConnectorDensity)),
+ Transform(Vec3(0, ConnectorHalfHeight, 0)));
+ rt1.setDefaultRadius(1.5);
+ bodyInfo.push_back(PerBodyInfo(rt1, false));
+
+ MobilizedBody::Ball rt2(
+ rt1, Transform(Vec3(0, -ConnectorHalfHeight, 0)),
+ Body::Rigid(calcConnectorMassProps(ConnectorRadius, ConnectorHalfHeight, ConnectorDensity)),
+ Transform(Vec3(0, ConnectorHalfHeight, 0)));
+ rt2.setDefaultRadius(1.5);
+ bodyInfo.push_back(PerBodyInfo(rt2, false));
+
+ MobilizedBody::Ball dup(
+ rt2, Transform(Vec3(0, -ConnectorHalfHeight, 0)),
+ Body::Rigid(calcDuplexMassProps(DuplexRadius, HalfHeight, NAtoms, AtomMass)),
+ Transform(Vec3(-DuplexRadius, HalfHeight, 0)));
+ dup.setDefaultRadius(1.5);
+ bodyInfo.push_back(PerBodyInfo(dup, true));
+
+ if (!shouldFlop) {
+ Constraint::Ball theConstraint(left2, Vec3(0, -ConnectorHalfHeight, 0),
+ dup, Vec3(DuplexRadius, HalfHeight, 0));
+ theConstraint.setDefaultRadius(1.5);
+ }
+
+ baseBody = lastDup = dup;
+ origin = Vec3(0);
+ }
+ return lastDup;
+ }
+
+ MassProperties calcDuplexMassProps(
+ Real halfHeight, Real r, int nAtoms, Real atomMass)
+ {
+ const Real pitch = 2*Pi/halfHeight;
+ const Real trans = (2*halfHeight)/(nAtoms-1);
+ const Real rot = pitch*trans;
+ Inertia iner(0);
+ Vec3 com(0);
+ Real mass = 0;
+ for (int i=0; i<nAtoms; ++i) {
+ const Real h = halfHeight - i*trans;
+ const Real th = i*rot;
+ const Vec3 p1(-r*cos(th),h,r*sin(th)), p2(r*cos(th),h,-r*sin(th));
+ mass += 2*atomMass;
+ iner += Inertia(p1, atomMass) + Inertia(p2, atomMass);
+ com += atomMass*p1 + atomMass*p2;
+ }
+ return MassProperties(mass,com/mass,iner);
+ }
+
+ MassProperties calcConnectorMassProps(Real r, Real halfHeight, Real density)
+ {
+ const Real volume = Pi*r*r*halfHeight;
+ const Real mass = volume*density;
+ const Vec3 com = Vec3(0);
+ const Inertia iner = mass*UnitInertia::cylinderAlongY(r, halfHeight);
+
+ return MassProperties(mass,com,iner);
+ }
+
+ void addDuplexDecorations(MobilizedBodyIndex bodyNum, Real r, Real halfHeight, Real slop, int nAtoms,
+ Real atomRadius, Visualizer& display) const
+ {
+ display.addDecoration(bodyNum, Transform(),
+ DecorativeCylinder(r+atomRadius+slop, halfHeight).setColor(Cyan).setOpacity(0.4));
+
+ const Real pitch = 2*Pi/halfHeight;
+ const Real trans = (2*halfHeight)/(nAtoms-1);
+ const Real rot = pitch*trans;
+ for (int i=0; i<nAtoms; ++i) {
+ const Real h = halfHeight - i*trans;
+ const Real th = i*rot;
+ const Vec3 p1(-r*cos(th),h,r*sin(th)), p2(r*cos(th),h,-r*sin(th));
+ display.addDecoration(bodyNum, Transform(Vec3(p1)),
+ DecorativeSphere(atomRadius).setColor(Red).setResolution(0.5));
+ display.addDecoration(bodyNum, Transform(Vec3(p2)),
+ DecorativeSphere(atomRadius).setColor(Green).setResolution(0.5));
+ }
+ }
+
+ void addConnectorDecorations(MobilizedBodyIndex bodyNum, Real r, Real halfHeight, Real endSlop,
+ Visualizer& display) const
+ {
+ display.addDecoration(bodyNum, Transform(),
+ DecorativeCylinder(r, halfHeight-endSlop).setColor(Blue));
+ }
+};
+
+
+int main(int argc, char** argv) {
+ std::vector<State> saveEm;
+ saveEm.reserve(1000);
+
+try // If anything goes wrong, an exception will be thrown.
+ { int nseg = NSegments;
+ int shouldFlop = 0;
+ if (argc > 1) sscanf(argv[1], "%d", &nseg);
+ if (argc > 2) sscanf(argv[2], "%d", &shouldFlop);
+
+ // Create a multibody system using Simbody.
+ MultibodySystem mbs;
+ MyRNAExample myRNA(mbs, nseg, shouldFlop != 0);
+ GeneralForceSubsystem forces(mbs);
+ Force::UniformGravity ugs(forces, myRNA, Vec3(0, -g, 0), -0.8);
+
+ const Vec3 attachPt(150, -40, -50);
+
+ Force::TwoPointLinearSpring(forces, myRNA.Ground(), attachPt,
+ myRNA.getMobilizedBody(MobilizedBodyIndex(myRNA.getNumBodies()-1)), Vec3(0),
+ 1000., // stiffness
+ 1.); // natural length
+
+ Force::GlobalDamper(forces, myRNA, 1000);
+
+
+ State s = mbs.realizeTopology();
+
+
+ //myRNA.getConstraint(ConstraintIndex(myRNA.getNConstraints()-4)).disable(s);
+
+
+ //myRNA.setUseEulerAngles(s,true);
+ mbs.realizeModel(s);
+ mbs.realize(s, Stage::Position);
+
+ for (ConstraintIndex cid(0); cid < myRNA.getNumConstraints(); ++cid) {
+ const Constraint& c = myRNA.getConstraint(cid);
+ int mp,mv,ma;
+ c.getNumConstraintEquationsInUse(s, mp,mv,ma);
+
+ cout << "CONSTRAINT " << cid
+ << " constrained bodies=" << c.getNumConstrainedBodies()
+ << " ancestor=" << c.getAncestorMobilizedBody().getMobilizedBodyIndex()
+ << " constrained mobilizers/nq/nu=" << c.getNumConstrainedMobilizers()
+ << "/" << c.getNumConstrainedQ(s) << "/" << c.getNumConstrainedU(s)
+ << " mp,mv,ma=" << mp << "," << mv << "," << ma
+ << endl;
+ for (ConstrainedBodyIndex cid(0); cid < c.getNumConstrainedBodies(); ++cid) {
+ cout << " constrained body: " << c.getMobilizedBodyFromConstrainedBody(cid).getMobilizedBodyIndex();
+ cout << endl;
+ }
+ for (ConstrainedMobilizerIndex cmx(0); cmx < c.getNumConstrainedMobilizers(); ++cmx) {
+ cout << " constrained mobilizer " << c.getMobilizedBodyFromConstrainedMobilizer(cmx).getMobilizedBodyIndex()
+ << ", q(" << c.getNumConstrainedQ(s, cmx) << ")=";
+ for (MobilizerQIndex i(0); i < c.getNumConstrainedQ(s, cmx); ++i)
+ cout << " " << c.getConstrainedQIndex(s, cmx, i);
+ cout << ", u(" << c.getNumConstrainedU(s, cmx) << ")=";
+ for (MobilizerUIndex i(0); i < c.getNumConstrainedU(s, cmx); ++i)
+ cout << " " << c.getConstrainedUIndex(s, cmx, i);
+ cout << endl;
+ }
+ cout << c.getSubtree();
+
+ cout << " d(perrdot)/du=" << c.calcPositionConstraintMatrixP(s);
+ cout << " d(perrdot)/du=" << ~c.calcPositionConstraintMatrixPt(s);
+
+ cout << " d(perr)/dq=" << c.calcPositionConstraintMatrixPNInv(s);
+ }
+
+
+
+
+
+ SimbodyMatterSubtree sub(myRNA);
+ sub.addTerminalBody(myRNA.getMobilizedBody(MobilizedBodyIndex(7)));
+ sub.addTerminalBody(myRNA.getMobilizedBody(MobilizedBodyIndex(10)));
+ //sub.addTerminalBody(myRNA.getMobilizedBody(MobilizedBodyIndex(20)));
+ sub.realizeTopology();
+ cout << "sub.ancestor=" << sub.getAncestorMobilizedBodyIndex();
+// cout << " sub.terminalBodies=" << sub.getTerminalBodies() << endl;
+// cout << "sub.allBodies=" << sub.getAllBodies() << endl;
+ for (SubtreeBodyIndex i(0); i < (int)sub.getAllBodies().size(); ++i) {
+ cout << "sub.parent[" << i << "]=" << sub.getParentSubtreeBodyIndex(i);
+// cout << " sub.children[" << i << "]=" << sub.getChildSubtreeBodyIndexs(i) << endl;
+ }
+
+
+ printf("# quaternions in use = %d\n", myRNA.getNumQuaternionsInUse(s));
+ for (MobilizedBodyIndex i(0); i<myRNA.getNumBodies(); ++i) {
+ printf("body %2d: using quat? %s; quat index=%d\n",
+ (int)i, myRNA.isUsingQuaternion(s,i) ? "true":"false",
+ (int)myRNA.getQuaternionPoolIndex(s,i));
+ }
+
+ // And a study using the Runge Kutta Merson integrator
+ bool suppressProject = false;
+
+ RungeKuttaMersonIntegrator myStudy(mbs);
+ //RungeKuttaFeldbergIntegrator myStudy(mbs);
+ //RungeKutta3Integrator myStudy(mbs);
+ //CPodesIntegrator myStudy(mbs);
+ //VerletIntegrator myStudy(mbs);
+ //ExplicitEulerIntegrator myStudy(mbs);
+
+ myStudy.setAccuracy(1e-2);
+ myStudy.setConstraintTolerance(1e-3);
+ myStudy.setProjectEveryStep(false);
+
+ Visualizer display(mbs);
+ display.setBackgroundColor(White);
+ display.setBackgroundType(Visualizer::SolidColor);
+ display.setMode(Visualizer::RealTime);
+
+ for (MobilizedBodyIndex i(1); i<myRNA.getNumBodies(); ++i)
+ myRNA.decorateBody(i, display);
+ myRNA.decorateGlobal(display);
+
+ DecorativeLine rbProto; rbProto.setColor(Orange).setLineThickness(3);
+ display.addRubberBandLine(GroundIndex, attachPt,MobilizedBodyIndex(myRNA.getNumBodies()-1),Vec3(0), rbProto);
+ //display.addRubberBandLine(GroundIndex, -attachPt,myRNA.getNumBodies()-1,Vec3(0), rbProto);
+
+ const Real dt = 1./30; // output intervals
+
+ printf("time nextStepSize\n");
+
+ s.updTime() = 0;
+ for (int i=0; i<50; ++i)
+ saveEm.push_back(s); // delay
+ display.report(s);
+
+ myStudy.initialize(s);
+ cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
+ cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
+ cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
+ cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
+ cout << "U WEIGHTS=" << s.getUWeights() << endl;
+ cout << "Z WEIGHTS=" << s.getZWeights() << endl;
+ cout << "1/QTOLS=" << s.getQErrWeights() << endl;
+ cout << "1/UTOLS=" << s.getUErrWeights() << endl;
+
+ saveEm.push_back(myStudy.getState());
+ for (int i=0; i<50; ++i)
+ saveEm.push_back(myStudy.getState()); // delay
+ display.report(myStudy.getState());
+
+
+ const double startReal = realTime(), startCPU = cpuTime();
+ int stepNum = 0;
+ for (;;) {
+ const State& ss = myStudy.getState();
+
+ mbs.realize(ss);
+
+ if ((stepNum++%100)==0) {
+ printf("%5g qerr=%10.4g uerr=%10.4g hNext=%g\n", ss.getTime(),
+ myRNA.getQErr(ss).normRMS(), myRNA.getUErr(ss).normRMS(),
+ myStudy.getPredictedNextStepSize());
+ printf(" E=%14.8g (pe=%10.4g ke=%10.4g)\n",
+ mbs.calcEnergy(ss), mbs.calcPotentialEnergy(ss), mbs.calcKineticEnergy(ss));
+
+ cout << "QERR=" << ss.getQErr() << endl;
+ cout << "UERR=" << ss.getUErr() << endl;
+ }
+
+ //if (s.getTime() - std::floor(s.getTime()) < 0.2)
+ // display.addEphemeralDecoration( DecorativeSphere(10).setColor(Green) );
+
+ display.report(ss);
+ saveEm.push_back(ss);
+
+ if (ss.getTime() >= 10)
+ break;
+
+ // TODO: should check for errors or have or teach RKM to throw.
+ myStudy.stepTo(ss.getTime() + dt, Infinity);
+ }
+
+ printf("CPU time=%gs, REAL time=%gs\n", cpuTime()-startCPU, realTime()-startReal);
+ printf("Using Integrator %s:\n", myStudy.getMethodName());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
+ printf("# CONVERGENCE FAILS = %d\n", myStudy.getNumConvergenceTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());
+ printf("# PROJECTION FAILS = %d\n", myStudy.getNumProjectionFailures());
+
+ display.dumpStats(std::cout);
+
+ while(true) {
+ for (int i=0; i < (int)saveEm.size(); ++i) {
+ display.report(saveEm[i]);
+ //display.report(saveEm[i]); // half speed
+ }
+ getchar();
+ }
+ }
+catch (const exception& e)
+ {
+ printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+ }
+}
+
diff --git a/Simbody/tests/adhoc/ContactBigMeshes.cpp b/Simbody/tests/adhoc/ContactBigMeshes.cpp
new file mode 100644
index 0000000..4053f03
--- /dev/null
+++ b/Simbody/tests/adhoc/ContactBigMeshes.cpp
@@ -0,0 +1,294 @@
+/* -------------------------------------------------------------------------- *
+ * SimTK 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Adhoc main program for testing contact behavior between two unreasonably
+ * dense meshes.
+ */
+
+#include "SimTKsimbody.h"
+
+#include <cstdio>
+#include <exception>
+#include <algorithm>
+#include <iostream>
+#include <fstream>
+#include <ctime>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+Array_<State> saveEm;
+
+static const Real ReportInterval = 0.01;
+static const Real ForceScale = 10;
+static const Real MomentScale = 20;
+
+class ForceArrowGenerator : public DecorationGenerator {
+public:
+ ForceArrowGenerator(const MultibodySystem& system,
+ const CompliantContactSubsystem& complCont)
+ : m_system(system), m_compliant(complCont) {}
+
+ virtual void generateDecorations(const State& state, Array_<DecorativeGeometry>& geometry) {
+ const Vec3 frcColors[] = {Red,Orange,Cyan};
+ const Vec3 momColors[] = {Blue,Green,Purple};
+ m_system.realize(state, Stage::Velocity);
+
+ const int ncont = m_compliant.getNumContactForces(state);
+ for (int i=0; i < ncont; ++i) {
+ const ContactForce& force = m_compliant.getContactForce(state,i);
+ const ContactId id = force.getContactId();
+ printf("viz contact %d: id=%d\n", i, (int)id);
+ const Vec3& frc = force.getForceOnSurface2()[1];
+ const Vec3& mom = force.getForceOnSurface2()[0];
+ Real frcMag = frc.norm(), momMag=mom.norm();
+ int frcThickness = 1, momThickness = 1;
+ Real frcScale = ForceScale, momScale = ForceScale;
+ while (frcMag > 10)
+ frcThickness++, frcScale /= 10, frcMag /= 10;
+ while (momMag > 10)
+ momThickness++, momScale /= 10, momMag /= 10;
+ DecorativeLine frcLine(force.getContactPoint(),
+ force.getContactPoint() + frcScale*frc);
+ DecorativeLine momLine(force.getContactPoint(),
+ force.getContactPoint() + momScale*mom);
+ frcLine.setColor(frcColors[id%3]);
+ momLine.setColor(momColors[id%3]);
+ frcLine.setLineThickness(2*frcThickness);
+ momLine.setLineThickness(2*momThickness);
+ geometry.push_back(frcLine);
+ geometry.push_back(momLine);
+ }
+ }
+private:
+ const MultibodySystem& m_system;
+ const CompliantContactSubsystem& m_compliant;
+};
+
+class MyReporter : public PeriodicEventReporter {
+public:
+ MyReporter(const MultibodySystem& system,
+ const CompliantContactSubsystem& complCont,
+ Real reportInterval)
+ : PeriodicEventReporter(reportInterval), m_system(system),
+ m_compliant(complCont)
+ {}
+
+ ~MyReporter() {}
+
+ void handleEvent(const State& state) const {
+ m_system.realize(state, Stage::Dynamics);
+ cout << state.getTime() << ": E = " << m_system.calcEnergy(state)
+ << " Ediss=" << m_compliant.getDissipatedEnergy(state)
+ << " E+Ediss=" << m_system.calcEnergy(state)
+ +m_compliant.getDissipatedEnergy(state)
+ << endl;
+ const int ncont = m_compliant.getNumContactForces(state);
+ cout << "Num contacts: " << m_compliant.getNumContactForces(state) << endl;
+ for (int i=0; i < ncont; ++i) {
+ const ContactForce& force = m_compliant.getContactForce(state,i);
+ //cout << force;
+ }
+
+ saveEm.push_back(state);
+ }
+private:
+ const MultibodySystem& m_system;
+ const CompliantContactSubsystem& m_compliant;
+};
+
+int main() {
+ try
+ { std::cout << "Current working directory: "
+ << Pathname::getCurrentWorkingDirectory() << std::endl;
+
+ // Create the system.
+
+ MultibodySystem system; system.setUseUniformBackground(true);
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, 0*Vec3(2, -9.8, 0));
+
+ ContactTrackerSubsystem tracker(system);
+ CompliantContactSubsystem contactForces(system, tracker);
+ contactForces.setTrackDissipatedEnergy(true);
+
+ GeneralContactSubsystem OLDcontact(system);
+ const ContactSetIndex OLDcontactSet = OLDcontact.createContactSet();
+
+ contactForces.setTransitionVelocity(1e-3);
+
+ std::ifstream meshFile1, meshFile2;
+ PolygonalMesh femurMesh;
+ meshFile1.open("ContactBigMeshes_Femur.obj");
+ femurMesh.loadObjFile(meshFile1); meshFile1.close();
+ PolygonalMesh patellaMesh;
+ meshFile2.open("ContactBigMeshes_Patella.obj");
+ patellaMesh.loadObjFile(meshFile2); meshFile2.close();
+
+ ContactGeometry::TriangleMesh femurTri(femurMesh);
+ ContactGeometry::TriangleMesh patellaTri(patellaMesh);
+
+ DecorativeMesh showFemur(femurTri.createPolygonalMesh());
+ Array_<DecorativeLine> femurNormals;
+ const Real NormalLength = .02;
+ //for (int fx=0; fx < femurTri.getNumFaces(); ++fx)
+ // femurNormals.push_back(
+ // DecorativeLine(femurTri.findCentroid(fx),
+ // femurTri.findCentroid(fx)
+ // + NormalLength*femurTri.getFaceNormal(fx)));
+
+ DecorativeMesh showPatella(patellaTri.createPolygonalMesh());
+ Array_<DecorativeLine> patellaNormals;
+ //for (int fx=0; fx < patellaTri.getNumFaces(); ++fx)
+ // patellaNormals.push_back(
+ // DecorativeLine(patellaTri.findCentroid(fx),
+ // patellaTri.findCentroid(fx)
+ // + NormalLength*patellaTri.getFaceNormal(fx)));
+
+ // This transform has the meshes close enough that their OBBs overlap
+ // but in the end none of the faces are touching.
+ const Transform X_FP(
+ Rotation(Mat33( 0.97107625831404454, 0.23876955530133021, 0,
+ -0.23876955530133021, 0.97107625831404454, 0,
+ 0, 0, 1), true),
+ Vec3(0.057400580865008571, 0.43859170879135373,
+ -0.00016506240185135300)
+ );
+
+
+ const Real fFac =1; // to turn off friction
+ const Real fDis = .5*0.2; // to turn off dissipation
+ const Real fVis = .1*.1; // to turn off viscous friction
+ const Real fK = 100*1e6; // pascals
+
+ // Put femur on ground at origin
+ matter.Ground().updBody().addDecoration(Vec3(0,0,0),
+ showFemur.setColor(Cyan).setOpacity(.2));
+ matter.Ground().updBody().addContactSurface(Vec3(0,0,0),
+ ContactSurface(femurTri,
+ ContactMaterial(fK*.01,fDis*.9,fFac*.8,fFac*.7,fVis*10),
+ .01 /*thickness*/));
+
+
+ Body::Rigid patellaBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ patellaBody.addDecoration(Transform(),
+ showPatella.setColor(Red).setOpacity(.2));
+ patellaBody.addContactSurface(Transform(),
+ ContactSurface(patellaTri,
+ ContactMaterial(fK*.001,fDis*.9,fFac*.8,fFac*.7,fVis*10),
+ .01 /*thickness*/));
+
+ MobilizedBody::Free patella(matter.Ground(), Transform(Vec3(0)),
+ patellaBody, Transform(Vec3(0)));
+
+
+ //// The old way ...
+ //OLDcontact.addBody(OLDcontactSet, ball,
+ // pyramid, Transform());
+
+ //OLDcontact.addBody(OLDcontactSet, matter.updGround(),
+ // ContactGeometry::HalfSpace(), Transform(R_xdown, Vec3(0,-3,0)));
+ //ElasticFoundationForce ef(forces, OLDcontact, OLDcontactSet);
+ //Real stiffness = 1e6, dissipation = 0.01, us = 0.1,
+ // ud = 0.05, uv = 0.01, vt = 0.01;
+ ////Real stiffness = 1e6, dissipation = 0.1, us = 0.8,
+ //// ud = 0.7, uv = 0.01, vt = 0.01;
+
+ //ef.setBodyParameters(ContactSurfaceIndex(0),
+ // stiffness, dissipation, us, ud, uv);
+ //ef.setTransitionVelocity(vt);
+ //// end of old way.
+
+ Visualizer viz(system);
+ Visualizer::Reporter& reporter = *new Visualizer::Reporter(viz, ReportInterval);
+ viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
+ MyReporter& myRep = *new MyReporter(system,contactForces,ReportInterval);
+
+ system.addEventReporter(&myRep);
+ system.addEventReporter(&reporter);
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+
+ viz.report(state);
+ printf("Reference state -- hit ENTER\n");
+ cout << "t=" << state.getTime()
+ << " q=" << patella.getQAsVector(state)
+ << " u=" << patella.getUAsVector(state)
+ << endl;
+ char c=getchar();
+
+ patella.setQToFitTransform(state, ~X_FP);
+ viz.report(state);
+ printf("Initial state -- hit ENTER\n");
+ cout << "t=" << state.getTime()
+ << " q=" << patella.getQAsVector(state)
+ << " u=" << patella.getUAsVector(state)
+ << endl;
+ c=getchar();
+
+ // Simulate it.
+ const clock_t start = clock();
+
+ RungeKutta3Integrator integ(system);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(2.0);
+
+ const double timeInSec = (double)(clock()-start)/CLOCKS_PER_SEC;
+ const int evals = integ.getNumRealizations();
+ cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
+ timeInSec << "s for " << ts.getTime() << "s sim (avg step="
+ << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
+ << (1000*ts.getTime())/evals << "ms/eval\n";
+
+ printf("Using Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
+
+
+ while(true) {
+ for (int i=0; i < (int)saveEm.size(); ++i) {
+ viz.report(saveEm[i]);
+ }
+ getchar();
+ }
+
+ } catch (const std::exception& e) {
+ std::printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+
+ } catch (...) {
+ std::printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+ return 0;
+}
+
diff --git a/Simbody/tests/adhoc/ContactBigMeshes_Femur.obj b/Simbody/tests/adhoc/ContactBigMeshes_Femur.obj
new file mode 100644
index 0000000..31488f9
--- /dev/null
+++ b/Simbody/tests/adhoc/ContactBigMeshes_Femur.obj
@@ -0,0 +1,19612 @@
+####
+#
+# OBJ File Generated by Meshlab
+#
+####
+# Object Fem_w_Cart_m_OS.obj
+#
+# Vertices: 4900
+# Faces: 9796
+#
+####
+vn -0.662912 0.539896 -0.518711
+v -0.008521 -0.396764 -0.033436
+vn -0.622868 0.701698 -0.345914
+v -0.009735 -0.397259 -0.032282
+vn -0.630930 0.440663 -0.638548
+v -0.006886 -0.395779 -0.034457
+vn -0.524126 0.212165 -0.824790
+v -0.008381 -0.398208 -0.034318
+vn -0.451621 0.091747 -0.887480
+v -0.005856 -0.397375 -0.035681
+vn -0.596742 0.357502 -0.718395
+v -0.008073 -0.397194 -0.034181
+vn -0.530122 0.205171 -0.822725
+v -0.006901 -0.397236 -0.035046
+vn -0.273074 0.556774 -0.784495
+v -0.011328 -0.398816 -0.032841
+vn -0.466565 0.614247 -0.636410
+v -0.010482 -0.398305 -0.032855
+vn -0.194822 0.816782 -0.543058
+v -0.011441 -0.398062 -0.031945
+vn -0.357393 -0.039469 -0.933120
+v -0.007232 -0.401041 -0.035089
+vn -0.383978 0.034838 -0.922685
+v -0.008321 -0.400804 -0.034644
+vn -0.366358 0.115805 -0.923240
+v -0.009465 -0.400506 -0.034151
+vn -0.468243 0.318945 -0.824028
+v -0.009825 -0.398781 -0.033588
+vn -0.604935 0.565028 -0.561067
+v -0.009627 -0.397722 -0.033086
+vn -0.301855 -0.090027 -0.949094
+v -0.004673 -0.399591 -0.036121
+vn -0.469383 0.087608 -0.878638
+v -0.007206 -0.398436 -0.035061
+vn -0.380129 -0.010141 -0.924878
+v -0.005794 -0.398711 -0.035755
+vn -0.562570 0.383192 -0.732584
+v -0.009357 -0.398118 -0.033610
+vn -0.414435 0.015658 -0.909944
+v -0.007322 -0.399803 -0.035072
+vn -0.447988 0.127017 -0.884971
+v -0.008740 -0.399443 -0.034343
+vn -0.044471 0.579248 -0.813937
+v -0.012549 -0.398877 -0.032674
+vn 0.121359 0.793226 -0.596711
+v -0.013088 -0.397782 -0.031586
+vn -0.015603 0.765096 -0.643727
+v -0.012364 -0.398231 -0.032065
+vn -0.364507 0.265367 -0.892589
+v -0.010327 -0.399674 -0.033640
+vn -0.250551 0.197531 -0.947737
+v -0.010461 -0.400841 -0.033864
+vn 0.021984 0.440188 -0.897637
+v -0.012756 -0.399650 -0.033142
+vn -0.173697 0.377616 -0.909525
+v -0.011645 -0.399753 -0.033263
+vn -0.338407 -0.069272 -0.938447
+v -0.006138 -0.400378 -0.035541
+vn -0.304443 0.064199 -0.950365
+v -0.009041 -0.401715 -0.034414
+vn -0.707787 -0.526483 -0.471013
+v -0.019696 -0.415796 0.013795
+vn -0.077774 0.289558 -0.953995
+v -0.011663 -0.400942 -0.033682
+vn -0.041704 -0.001588 -0.999129
+v -0.009404 -0.410612 -0.034740
+vn -0.270374 -0.069351 -0.960254
+v -0.006560 -0.402953 -0.035179
+vn -0.061291 0.030593 -0.997651
+v -0.010277 -0.406819 -0.034639
+vn -0.039347 0.018935 -0.999046
+v -0.010807 -0.408562 -0.034647
+vn -0.044762 0.193649 -0.980049
+v -0.011358 -0.402621 -0.034153
+vn -0.160359 0.044964 -0.986034
+v -0.008334 -0.404357 -0.034738
+vn -0.254652 0.014284 -0.966927
+v -0.007923 -0.403113 -0.034789
+vn -0.167373 -0.053088 -0.984463
+v -0.005393 -0.405712 -0.035261
+vn 0.039295 0.150482 -0.987831
+v -0.012475 -0.403401 -0.034314
+vn -0.293482 -0.090109 -0.951708
+v -0.006285 -0.401829 -0.035356
+vn -0.320665 -0.027091 -0.946805
+v -0.007626 -0.402056 -0.034900
+vn -0.179685 0.143431 -0.973212
+v -0.009885 -0.402363 -0.034258
+vn 0.101396 0.968505 -0.227414
+v -0.014285 -0.395439 -0.025667
+vn 0.096625 0.925985 -0.364988
+v -0.013207 -0.394919 -0.023752
+vn 0.045746 0.964727 -0.259249
+v -0.011523 -0.395644 -0.025572
+vn 0.089850 0.940679 -0.327186
+v -0.013684 -0.395123 -0.024423
+vn 0.075097 0.928554 -0.363522
+v -0.012907 -0.394909 -0.023661
+vn 0.065918 0.925820 -0.372173
+v -0.012599 -0.394918 -0.023620
+vn 0.082159 0.946216 -0.312929
+v -0.013078 -0.395187 -0.024440
+vn 0.056252 0.917801 -0.393035
+v -0.012677 -0.394762 -0.023268
+vn 0.218490 0.951758 -0.215452
+v -0.013236 -0.395988 -0.027958
+vn 0.079580 0.943905 -0.320484
+v -0.012218 -0.395199 -0.024263
+vn 0.088316 0.961933 -0.258623
+v -0.013711 -0.395444 -0.025470
+vn 0.117261 0.976440 -0.181146
+v -0.013993 -0.395575 -0.026194
+vn 0.130374 0.915930 -0.379571
+v -0.011181 -0.394823 -0.023016
+vn -0.700251 0.713838 0.009221
+v -0.008050 -0.395169 -0.028326
+vn -0.562976 0.825283 -0.044341
+v -0.008624 -0.395647 -0.027737
+vn -0.799249 0.598751 0.051941
+v -0.006727 -0.393800 -0.026741
+vn -0.786051 0.614901 -0.063402
+v -0.006534 -0.393527 -0.030171
+vn -0.089436 0.953578 -0.287560
+v -0.010553 -0.396452 -0.028430
+vn -0.882391 0.466718 0.059671
+v -0.005599 -0.392194 -0.025216
+vn 0.066542 0.956046 -0.285566
+v -0.011588 -0.396249 -0.027844
+vn -0.586224 0.810148 -0.001216
+v -0.007584 -0.394892 -0.025222
+vn -0.447504 0.787437 -0.423889
+v -0.010617 -0.397777 -0.032005
+vn -0.642409 0.745117 -0.179194
+v -0.009356 -0.396590 -0.031291
+vn -0.739653 0.653909 -0.159114
+v -0.007162 -0.394532 -0.031946
+vn 0.037275 0.863500 -0.502969
+v -0.012111 -0.397588 -0.031120
+vn 0.167892 0.856255 -0.488508
+v -0.012627 -0.397178 -0.030547
+vn -0.352511 0.912094 -0.209334
+v -0.009930 -0.396532 -0.029190
+vn -0.705581 0.690902 -0.157509
+v -0.008234 -0.395613 -0.031639
+vn -0.721854 0.633118 -0.279444
+v -0.007297 -0.394968 -0.032835
+vn 0.083209 0.944995 -0.316323
+v -0.011303 -0.395179 -0.023951
+vn 0.087269 0.936255 -0.340311
+v -0.010763 -0.395025 -0.023382
+vn 0.034933 0.963976 -0.263687
+v -0.010234 -0.395237 -0.023920
+vn 0.078803 0.963527 -0.255745
+v -0.012828 -0.395571 -0.025634
+vn 0.196338 0.938610 -0.283659
+v -0.012649 -0.396251 -0.028437
+vn 0.197743 0.905304 -0.375928
+v -0.012397 -0.396657 -0.029393
+vn -0.690896 0.656055 -0.303734
+v -0.008598 -0.396292 -0.032598
+vn -0.474441 0.842040 -0.256661
+v -0.010230 -0.397132 -0.030946
+vn -0.744996 0.663776 0.066200
+v -0.007253 -0.394515 -0.026094
+vn -0.821496 0.560353 0.105586
+v -0.006280 -0.393710 -0.023434
+vn 0.122993 0.960587 -0.249288
+v -0.009461 -0.394826 -0.022362
+vn -0.083545 0.981133 -0.174353
+v -0.008754 -0.395020 -0.022937
+vn -0.355588 0.930257 -0.090443
+v -0.007784 -0.394899 -0.023509
+vn -0.078053 0.971266 -0.224832
+v -0.009869 -0.395673 -0.025695
+vn -0.842312 0.538446 0.024204
+v -0.006028 -0.392756 -0.027302
+vn -0.866191 0.490057 0.097763
+v -0.005955 -0.392991 -0.024261
+vn -0.163233 0.912000 -0.376311
+v -0.011061 -0.397286 -0.030576
+vn -0.691607 0.719110 -0.067535
+v -0.008452 -0.395604 -0.030153
+vn -0.587025 0.803038 -0.102622
+v -0.009316 -0.396290 -0.029725
+vn -0.764709 0.644328 -0.007860
+v -0.007119 -0.394166 -0.028937
+vn -0.736579 0.671558 -0.080377
+v -0.007423 -0.394618 -0.030735
+vn -0.304136 0.939659 -0.156660
+v -0.009058 -0.395760 -0.026804
+vn 0.066796 0.920467 -0.385070
+v -0.011731 -0.396917 -0.029724
+vn 0.222195 0.899393 -0.376459
+v -0.013067 -0.396476 -0.029369
+vn -0.792618 0.536220 -0.290213
+v -0.004297 -0.390805 -0.032177
+vn -0.753068 0.611693 -0.242320
+v -0.006136 -0.393559 -0.032708
+vn -0.911077 0.409744 -0.045259
+v -0.004587 -0.389911 -0.027856
+vn -0.738066 0.574024 -0.354619
+v -0.004915 -0.392284 -0.033276
+vn -0.889902 0.444238 -0.103577
+v -0.003968 -0.388579 -0.028134
+vn -0.829375 0.529378 -0.178596
+v -0.003265 -0.387338 -0.028311
+vn -0.885671 0.446680 -0.126741
+v -0.004451 -0.389949 -0.029439
+vn -0.703584 0.577839 -0.413607
+v -0.005911 -0.393834 -0.033753
+vn -0.800567 0.564652 -0.200648
+v -0.005285 -0.392205 -0.032025
+vn -0.828508 0.550083 -0.104801
+v -0.005629 -0.392336 -0.030579
+vn -0.850381 0.498735 -0.167677
+v -0.004732 -0.390926 -0.030826
+vn -0.835987 0.501642 -0.222443
+v -0.003898 -0.389229 -0.030298
+vn -0.564812 0.706060 -0.427161
+v -0.002038 -0.386786 -0.030199
+vn -0.328339 0.788902 -0.519449
+v -0.001062 -0.386006 -0.029869
+vn -0.630261 0.693308 -0.349421
+v -0.002107 -0.386190 -0.029070
+vn -0.775775 0.617690 -0.128964
+v -0.006363 -0.393490 -0.031447
+vn -0.833324 0.551636 -0.035603
+v -0.005959 -0.392643 -0.029034
+vn -0.876975 0.473874 -0.079735
+v -0.005086 -0.391203 -0.029343
+vn -0.891994 0.451883 -0.012197
+v -0.005277 -0.391405 -0.027659
+vn -0.901679 0.431161 0.032801
+v -0.005280 -0.391472 -0.026121
+vn -0.921659 0.387169 0.025397
+v -0.004637 -0.389977 -0.025975
+vn -0.769243 0.622554 -0.143848
+v -0.002362 -0.385530 -0.026374
+vn -0.733359 0.638943 -0.232242
+v -0.002401 -0.386077 -0.028086
+vn -0.636791 0.732655 -0.240237
+v -0.001302 -0.384435 -0.026354
+vn -0.563083 0.759653 -0.325368
+v -0.001415 -0.385006 -0.027687
+vn -0.757409 0.580040 -0.299807
+v -0.002924 -0.387633 -0.030068
+vn -0.626443 0.598731 -0.499090
+v -0.003001 -0.389397 -0.032672
+vn -0.649226 0.633664 -0.420683
+v -0.002662 -0.388210 -0.031543
+vn -0.762337 0.550431 -0.340396
+v -0.003452 -0.389397 -0.031893
+vn 0.159698 0.688190 -0.707737
+v 0.002158 -0.388055 -0.032282
+vn 0.113433 0.619983 -0.776373
+v 0.001549 -0.389709 -0.033872
+vn 0.042963 0.647799 -0.760599
+v 0.000306 -0.389031 -0.033445
+vn 0.204754 0.664960 -0.718264
+v 0.003375 -0.388856 -0.032732
+vn -0.157931 0.599469 -0.784662
+v -0.001358 -0.390067 -0.034153
+vn -0.241441 0.821512 -0.516551
+v 0.000064 -0.384486 -0.028083
+vn -0.428619 0.788515 -0.441056
+v -0.001195 -0.385382 -0.028731
+vn -0.449419 0.725235 -0.521590
+v -0.001897 -0.387301 -0.031148
+vn -0.442702 0.680804 -0.583541
+v -0.002241 -0.388395 -0.032236
+vn -0.267626 0.757927 -0.594914
+v -0.001226 -0.387018 -0.031177
+vn 0.229648 0.675436 -0.700748
+v 0.005814 -0.388770 -0.031847
+vn 0.182487 0.630363 -0.754547
+v 0.003435 -0.390229 -0.033902
+vn -0.029078 0.673131 -0.738951
+v -0.000460 -0.388441 -0.032930
+vn 0.207310 0.706368 -0.676806
+v 0.005248 -0.385435 -0.028583
+vn -0.153283 0.803324 -0.575477
+v 0.000060 -0.385385 -0.029439
+vn -0.137616 0.770835 -0.621993
+v -0.000384 -0.386739 -0.031118
+vn -0.001651 0.783834 -0.620968
+v 0.001170 -0.385133 -0.029239
+vn 0.084290 0.739613 -0.667733
+v 0.003184 -0.383593 -0.027174
+vn 0.184171 0.704175 -0.685725
+v 0.004188 -0.384333 -0.027748
+vn 0.188983 0.716308 -0.671706
+v 0.003052 -0.386254 -0.030166
+vn 0.125221 0.742494 -0.658045
+v 0.002103 -0.385462 -0.029533
+vn -0.460738 0.610299 -0.644403
+v -0.002612 -0.389663 -0.033295
+vn -0.302878 0.703528 -0.642895
+v -0.001589 -0.388269 -0.032480
+vn -0.140009 0.714169 -0.685828
+v -0.000844 -0.387965 -0.032438
+vn 0.019828 0.744639 -0.667172
+v 0.000381 -0.386795 -0.031265
+vn 0.107135 0.710620 -0.695372
+v 0.001139 -0.387288 -0.031716
+vn 0.218427 0.700123 -0.679792
+v 0.004300 -0.387335 -0.030919
+vn 0.497025 0.416104 -0.761462
+v 0.011562 -0.392388 -0.032352
+vn 0.414917 0.527807 -0.741123
+v 0.011702 -0.390337 -0.030957
+vn 0.251890 0.654733 -0.712654
+v 0.008248 -0.389529 -0.031753
+vn 0.383171 0.575685 -0.722334
+v 0.012518 -0.388219 -0.028824
+vn 0.312039 0.075055 -0.947100
+v 0.007567 -0.399203 -0.036730
+vn 0.330051 0.351703 -0.875997
+v 0.007504 -0.395048 -0.035654
+vn 0.440446 0.356668 -0.823890
+v 0.010258 -0.394562 -0.034228
+vn 0.307411 0.241049 -0.920540
+v 0.007481 -0.397298 -0.036431
+vn 0.364728 0.475248 -0.800695
+v 0.009259 -0.392458 -0.033619
+vn 0.280903 0.441995 -0.851900
+v 0.006087 -0.393272 -0.035347
+vn 0.143686 0.410040 -0.900678
+v 0.003143 -0.393514 -0.036174
+vn 0.287261 0.569651 -0.770051
+v 0.007438 -0.391476 -0.033719
+vn 0.215499 0.529198 -0.820676
+v 0.004640 -0.392243 -0.035162
+vn 0.065796 -0.036204 -0.997176
+v 0.004461 -0.400040 -0.037371
+vn 0.229432 0.631829 -0.740374
+v 0.005574 -0.390740 -0.033747
+vn 0.153951 0.588748 -0.793521
+v 0.003345 -0.391496 -0.034946
+vn 0.568131 0.216406 -0.793975
+v 0.011350 -0.396290 -0.034175
+vn 0.673568 0.077261 -0.735076
+v 0.015570 -0.399214 -0.030824
+vn 0.634307 -0.120690 -0.763603
+v 0.013261 -0.403937 -0.032759
+vn 0.663378 -0.031839 -0.747607
+v 0.014843 -0.401879 -0.031565
+vn 0.639621 -0.143427 -0.755191
+v 0.015185 -0.405726 -0.030824
+vn 0.470648 -0.236262 -0.850100
+v 0.009608 -0.406015 -0.034851
+vn 0.686853 -0.019588 -0.726533
+v 0.018506 -0.402067 -0.028251
+vn 0.672886 0.019075 -0.739500
+v 0.016655 -0.400945 -0.029955
+vn 0.400640 -0.173473 -0.899664
+v 0.008572 -0.403462 -0.035975
+vn 0.443508 -0.058813 -0.894339
+v 0.009047 -0.400710 -0.036117
+vn 0.555726 0.018230 -0.831166
+v 0.010427 -0.399795 -0.035325
+vn 0.523315 -0.134912 -0.841392
+v 0.010257 -0.402763 -0.035196
+vn 0.667396 0.007417 -0.744666
+v 0.013104 -0.400297 -0.033176
+vn 0.610776 0.074971 -0.788246
+v 0.011646 -0.398634 -0.034359
+vn 0.311233 -0.101375 -0.944911
+v 0.007460 -0.401352 -0.036722
+vn 0.612272 -0.071759 -0.787384
+v 0.011788 -0.401758 -0.034248
+vn 0.569589 -0.191404 -0.799333
+v 0.011532 -0.405231 -0.033838
+vn 0.681626 0.096273 -0.725339
+v 0.014218 -0.398364 -0.031974
+vn 0.443978 0.117636 -0.888282
+v 0.009043 -0.398643 -0.036086
+vn 0.647088 0.306431 -0.698124
+v 0.015486 -0.391735 -0.028835
+vn 0.670338 0.236394 -0.703395
+v 0.015449 -0.393810 -0.029664
+vn 0.591790 -0.226843 -0.773516
+v 0.013443 -0.407880 -0.031683
+vn 0.202265 -0.000845 -0.979331
+v 0.006215 -0.399963 -0.037152
+vn 0.464335 0.227975 -0.855816
+v 0.009800 -0.397060 -0.035314
+vn 0.666434 0.144302 -0.731466
+v 0.012715 -0.397152 -0.033211
+vn 0.138045 -0.143100 -0.980034
+v 0.005748 -0.401765 -0.037074
+vn 0.665424 0.213535 -0.715272
+v 0.013819 -0.395335 -0.031696
+vn 0.601062 0.290213 -0.744648
+v 0.012115 -0.394578 -0.032971
+vn 0.665295 -0.148236 -0.731716
+v 0.017660 -0.406643 -0.028458
+vn 0.665669 -0.079739 -0.741975
+v 0.016794 -0.403808 -0.029707
+vn 0.687616 -0.092144 -0.720204
+v 0.019325 -0.404470 -0.027272
+vn 0.682147 0.170429 -0.711076
+v 0.015585 -0.396610 -0.030349
+vn -0.023952 -0.144176 -0.989262
+v -0.001355 -0.410345 -0.035211
+vn -0.082367 -0.121078 -0.989220
+v -0.002065 -0.408091 -0.035470
+vn -0.003120 -0.173099 -0.984899
+v 0.003904 -0.402001 -0.037149
+vn -0.066640 -0.167584 -0.983603
+v 0.000030 -0.407520 -0.035703
+vn 0.051527 -0.259453 -0.964380
+v 0.002818 -0.410084 -0.035223
+vn 0.232875 -0.196507 -0.952446
+v 0.006817 -0.403920 -0.036491
+vn 0.009671 -0.247428 -0.968858
+v 0.003993 -0.406986 -0.036030
+vn -0.105477 -0.196730 -0.974767
+v 0.001229 -0.404706 -0.036344
+vn 0.265787 -0.313768 -0.911541
+v 0.006796 -0.408745 -0.035218
+vn 0.341498 -0.251609 -0.905578
+v 0.007732 -0.406429 -0.035615
+vn -0.046288 -0.213542 -0.975837
+v 0.001976 -0.407190 -0.035897
+vn -0.100310 -0.200079 -0.974631
+v 0.002083 -0.402280 -0.036972
+vn 0.056109 -0.213379 -0.975357
+v 0.004987 -0.404205 -0.036686
+vn -0.056845 -0.215827 -0.974776
+v 0.003099 -0.404414 -0.036600
+vn 0.141816 -0.254110 -0.956722
+v 0.005921 -0.406673 -0.036011
+vn -0.188813 -0.115706 -0.975173
+v -0.003819 -0.403997 -0.035711
+vn -0.105619 -0.061707 -0.992490
+v -0.004976 -0.408333 -0.035170
+vn -0.136844 -0.111838 -0.984259
+v -0.002841 -0.405831 -0.035658
+vn -0.218440 -0.059120 -0.974058
+v -0.006041 -0.404016 -0.035235
+vn 0.522755 -0.270353 -0.808478
+v 0.010946 -0.408510 -0.033262
+vn -0.146450 -0.191488 -0.970508
+v 0.000260 -0.402665 -0.036629
+vn 0.180770 -0.338777 -0.923338
+v 0.004788 -0.411974 -0.034403
+vn 0.112964 -0.310786 -0.943743
+v 0.005126 -0.409392 -0.035286
+vn -0.117534 -0.160528 -0.980008
+v -0.000667 -0.405243 -0.036015
+vn -0.163373 -0.159346 -0.973611
+v -0.001703 -0.403319 -0.036189
+vn -0.001616 -0.189574 -0.981865
+v 0.000617 -0.409888 -0.035308
+vn -0.261888 -0.130387 -0.956250
+v -0.004816 -0.401135 -0.035870
+vn 0.036438 -0.186022 -0.981870
+v -0.000588 -0.412327 -0.034887
+vn 0.151749 -0.287032 -0.945825
+v 0.002693 -0.412948 -0.034436
+vn 0.068342 -0.226192 -0.971682
+v 0.001119 -0.411869 -0.034894
+vn 0.368192 -0.354253 -0.859616
+v 0.007525 -0.415148 -0.032189
+vn 0.171860 -0.361295 -0.916477
+v 0.001059 -0.419907 -0.032347
+vn 0.019891 -0.203071 -0.978962
+v -0.004035 -0.414952 -0.034505
+vn 0.200972 -0.326645 -0.923533
+v 0.001911 -0.421775 -0.031445
+vn 0.411321 -0.315155 -0.855273
+v 0.008667 -0.409286 -0.034262
+vn 0.403639 -0.376716 -0.833763
+v 0.008914 -0.412049 -0.032980
+vn 0.235668 -0.331714 -0.913470
+v 0.002915 -0.417161 -0.033007
+vn 0.324185 -0.317899 -0.890980
+v 0.006454 -0.420305 -0.030696
+vn 0.322496 -0.327664 -0.888050
+v 0.005787 -0.418154 -0.031734
+vn 0.247744 -0.348778 -0.903868
+v 0.003443 -0.419168 -0.032099
+vn 0.256085 -0.334964 -0.906763
+v 0.004198 -0.421210 -0.031092
+vn 0.269856 -0.291787 -0.917626
+v 0.004957 -0.423106 -0.030210
+vn 0.114587 -0.249939 -0.961457
+v 0.000683 -0.413898 -0.034451
+vn 0.141847 -0.294461 -0.945078
+v 0.000437 -0.416015 -0.033902
+vn 0.322951 -0.383369 -0.865292
+v 0.007021 -0.413239 -0.033239
+vn 0.392132 -0.316135 -0.863881
+v 0.008141 -0.417063 -0.031169
+vn 0.456271 -0.316677 -0.831584
+v 0.010438 -0.415936 -0.030436
+vn 0.063514 -0.225863 -0.972086
+v -0.001445 -0.414600 -0.034481
+vn 0.285989 -0.368004 -0.884751
+v 0.006799 -0.411010 -0.034317
+vn 0.258942 -0.344195 -0.902485
+v 0.004888 -0.414207 -0.033525
+vn 0.304471 -0.333875 -0.892090
+v 0.005240 -0.416167 -0.032669
+vn 0.395233 -0.292714 -0.870695
+v 0.008743 -0.419222 -0.030138
+vn 0.461894 -0.245269 -0.852348
+v 0.011373 -0.420586 -0.028408
+vn 0.206422 -0.306316 -0.929280
+v 0.002677 -0.415143 -0.033756
+vn 0.154531 -0.345615 -0.925565
+v 0.000530 -0.417995 -0.033206
+vn 0.070519 -0.293718 -0.953287
+v -0.002081 -0.416604 -0.033992
+vn 0.004316 -0.152399 -0.988310
+v -0.002710 -0.412889 -0.034845
+vn -0.050843 -0.098208 -0.993866
+v -0.003680 -0.410846 -0.035073
+vn 0.074514 -0.212828 -0.974244
+v -0.008537 -0.417161 -0.034161
+vn 0.114732 -0.169125 -0.978894
+v -0.008714 -0.419463 -0.033751
+vn 0.080634 -0.154677 -0.984669
+v -0.011568 -0.416359 -0.034563
+vn 0.087358 -0.337344 -0.937319
+v -0.002126 -0.418465 -0.033343
+vn 0.029438 -0.044066 -0.998595
+v -0.010052 -0.413643 -0.034715
+vn -0.021058 -0.110009 -0.993707
+v -0.005314 -0.413093 -0.034792
+vn 0.130316 -0.279072 -0.951387
+v -0.003411 -0.420329 -0.032810
+vn 0.083722 -0.285870 -0.954604
+v -0.004988 -0.418745 -0.033465
+vn 0.043435 -0.252207 -0.966698
+v -0.005540 -0.416651 -0.034124
+vn 0.011513 0.015755 -0.999810
+v -0.010849 -0.411845 -0.034731
+vn 0.126452 -0.236843 -0.963284
+v -0.005501 -0.419681 -0.033258
+vn 0.113281 -0.201464 -0.972923
+v -0.007576 -0.418730 -0.033731
+vn 0.050681 -0.142531 -0.988492
+v -0.009432 -0.415380 -0.034544
+vn 0.082707 -0.159132 -0.983787
+v -0.010548 -0.416120 -0.034512
+vn 0.156672 -0.160933 -0.974451
+v -0.006599 -0.420054 -0.033348
+vn 0.101930 -0.178804 -0.978590
+v -0.009694 -0.418118 -0.034091
+vn 0.010469 -0.141658 -0.989860
+v -0.007129 -0.414583 -0.034589
+vn -0.020233 -0.048371 -0.998624
+v -0.008224 -0.412676 -0.034746
+vn -0.061297 -0.051256 -0.996803
+v -0.006507 -0.410986 -0.034900
+vn -0.084130 -0.009028 -0.996414
+v -0.008179 -0.408815 -0.034828
+vn 0.128849 -0.335341 -0.933244
+v -0.001408 -0.420269 -0.032577
+vn 0.173773 -0.195567 -0.965172
+v -0.004017 -0.420969 -0.032742
+vn -0.060843 -0.061251 0.996266
+v -0.016639 -0.409082 -0.011949
+vn 0.090506 -0.009139 0.995854
+v -0.014714 -0.403597 -0.011791
+vn -0.315726 0.827208 0.464804
+v -0.007584 -0.395777 -0.013339
+vn -0.331217 0.520951 0.786706
+v -0.008193 -0.396813 -0.012414
+vn -0.151196 0.780463 0.606644
+v -0.008662 -0.396108 -0.013223
+vn -0.055071 0.985491 0.160546
+v -0.008812 -0.394979 -0.016973
+vn 0.086606 0.981769 0.169199
+v -0.009696 -0.395013 -0.016564
+vn 0.051814 0.939403 0.338876
+v -0.009640 -0.395435 -0.014678
+vn -0.284860 0.057637 0.956835
+v -0.004670 -0.415244 -0.009994
+vn -0.295638 -0.012440 0.955219
+v -0.007392 -0.407824 -0.011049
+vn -0.144272 0.026812 0.989175
+v -0.010683 -0.407750 -0.011757
+vn -0.380622 0.035920 0.924033
+v -0.008297 -0.402142 -0.011240
+vn -0.232497 0.055256 0.971026
+v -0.007111 -0.410962 -0.010911
+vn -0.060396 0.015559 0.998053
+v -0.011910 -0.405761 -0.011901
+vn -0.167022 0.117610 0.978913
+v -0.008490 -0.413029 -0.010993
+vn -0.250456 0.000150 0.968128
+v -0.008816 -0.405786 -0.011431
+vn -0.608810 0.092551 0.787899
+v -0.006917 -0.400051 -0.010511
+vn -0.201449 0.045478 0.978443
+v -0.008819 -0.409260 -0.011382
+vn -0.539320 -0.054434 0.840340
+v -0.006781 -0.402348 -0.010406
+vn -0.522913 -0.121864 0.843630
+v -0.005898 -0.404570 -0.010105
+vn -0.190222 0.029774 0.981290
+v -0.010211 -0.403940 -0.011757
+vn -0.614034 -0.194064 0.765050
+v -0.004752 -0.404275 -0.009227
+vn 0.085369 0.089477 0.992324
+v -0.012523 -0.399713 -0.012159
+vn -0.415471 -0.024774 0.909269
+v -0.003316 -0.412575 -0.009550
+vn -0.597169 -0.214683 0.772852
+v -0.003457 -0.406370 -0.008805
+vn -0.506802 -0.116816 0.854111
+v -0.003201 -0.409160 -0.009229
+vn -0.550435 -0.105151 0.828230
+v -0.001771 -0.410923 -0.008593
+vn -0.072543 0.239802 0.968108
+v -0.011077 -0.398147 -0.012493
+vn -0.225510 0.118189 0.967045
+v -0.010011 -0.400342 -0.011940
+vn -0.510968 -0.138136 0.848428
+v -0.004622 -0.406857 -0.009707
+vn -0.241875 0.304548 0.921275
+v -0.009528 -0.397836 -0.012339
+vn -0.453952 0.240344 0.857999
+v -0.007907 -0.398659 -0.011477
+vn -0.066285 0.050885 0.996502
+v -0.011553 -0.402587 -0.011972
+vn -0.426185 -0.055856 0.902910
+v -0.004700 -0.409626 -0.010066
+vn -0.319172 0.028698 0.947262
+v -0.005366 -0.412110 -0.010361
+vn -0.337613 -0.011626 0.941213
+v -0.006103 -0.409544 -0.010652
+vn -0.420982 -0.077658 0.903738
+v -0.006007 -0.407181 -0.010483
+vn -0.693620 0.303215 0.653415
+v -0.006252 -0.397853 -0.010438
+vn 0.050451 0.050667 0.997441
+v -0.012620 -0.401598 -0.012020
+vn -0.273118 -0.837950 -0.472489
+v -0.009937 -0.422377 0.014371
+vn -0.403727 -0.051876 0.913408
+v -0.007260 -0.404855 -0.010865
+vn -0.691121 -0.297303 0.658759
+v -0.002658 -0.405821 -0.007883
+vn -0.615303 -0.211470 0.759396
+v -0.002118 -0.408244 -0.008303
+vn -0.642086 -0.127017 0.756037
+v -0.005731 -0.402304 -0.009590
+vn -0.729567 -0.179646 0.659893
+v -0.004875 -0.401996 -0.008684
+vn -0.678416 -0.017550 0.734469
+v -0.006226 -0.400756 -0.009866
+vn -0.700453 -0.266742 0.661978
+v -0.003869 -0.403916 -0.008288
+vn -0.856013 -0.417969 0.304208
+v -0.000862 -0.406454 -0.005271
+vn -0.858549 0.308410 0.409606
+v -0.004686 -0.396607 -0.008808
+vn -0.840840 -0.064053 0.537481
+v -0.004857 -0.399935 -0.008195
+vn -0.755467 -0.038998 0.654025
+v -0.005521 -0.400348 -0.009120
+vn -0.812935 -0.233300 0.533581
+v -0.004210 -0.401722 -0.007722
+vn -0.750467 0.122896 0.649381
+v -0.005937 -0.399095 -0.009679
+vn -0.832292 0.123828 0.540330
+v -0.005196 -0.398481 -0.008819
+vn -0.795162 0.267266 0.544322
+v -0.005417 -0.397520 -0.009511
+vn -0.780126 -0.366046 0.507360
+v -0.002251 -0.405336 -0.007094
+vn -0.879020 -0.295839 0.373902
+v -0.003693 -0.401659 -0.006730
+vn -0.919029 -0.108199 0.379049
+v -0.004347 -0.399715 -0.007176
+vn -0.913571 0.121991 0.387953
+v -0.004607 -0.397987 -0.007848
+vn -0.781306 -0.334018 0.527251
+v -0.003235 -0.403660 -0.007362
+vn -0.715419 -0.205993 0.667640
+v -0.001082 -0.409465 -0.007743
+vn -0.756604 -0.306314 0.577687
+v -0.001528 -0.407340 -0.007353
+vn -0.836690 -0.387721 0.386811
+v -0.001542 -0.405987 -0.006365
+vn -0.870089 -0.490864 -0.044699
+v -0.002154 -0.403581 -0.002716
+vn -0.998117 -0.045165 0.041499
+v -0.003919 -0.398674 -0.003540
+vn -0.846830 -0.392982 0.358390
+v -0.002681 -0.403729 -0.006430
+vn -0.870124 -0.441519 0.218964
+v -0.002134 -0.404005 -0.005221
+vn -0.864695 -0.489832 0.111211
+v -0.001472 -0.404807 -0.004045
+vn -0.909289 -0.361608 0.205996
+v -0.003272 -0.401814 -0.005619
+vn -0.967698 -0.239714 0.078084
+v -0.003735 -0.400104 -0.004669
+vn -0.915105 -0.397240 0.069155
+v -0.003036 -0.401940 -0.004313
+vn -0.962151 -0.177202 0.207038
+v -0.003984 -0.399802 -0.006027
+vn -0.995002 0.012144 0.099114
+v -0.004023 -0.398214 -0.005208
+vn -0.956315 0.256731 0.139827
+v -0.003826 -0.396292 -0.006054
+vn -0.971434 0.079057 0.223753
+v -0.004220 -0.397895 -0.006653
+vn -0.879815 -0.471737 0.058226
+v -0.002324 -0.403294 -0.003936
+vn -0.866731 -0.474854 0.152616
+v -0.000503 -0.406456 -0.003934
+vn -0.898464 -0.413125 0.148626
+v 0.000233 -0.407757 -0.003699
+vn -0.805830 0.404316 -0.432627
+v -0.004682 -0.396605 0.002645
+vn -0.910137 -0.412652 -0.037004
+v -0.002936 -0.402106 -0.002838
+vn -0.854567 -0.518362 -0.031890
+v -0.001324 -0.404960 -0.002612
+vn -0.853520 -0.493991 -0.165758
+v -0.002231 -0.403755 -0.001153
+vn -0.835076 -0.513165 -0.198266
+v -0.001558 -0.404927 -0.000961
+vn -0.858543 -0.511948 -0.028521
+v -0.000497 -0.406289 -0.002464
+vn -0.822183 -0.538911 -0.183275
+v -0.000715 -0.406251 -0.000862
+vn -0.982424 0.174945 0.065093
+v -0.003778 -0.397239 -0.002544
+vn -0.963528 -0.267606 0.000874
+v -0.003581 -0.400456 -0.003088
+vn -0.864416 -0.016604 -0.502502
+v -0.004710 -0.399647 0.001701
+vn -0.848710 0.333289 -0.410621
+v -0.004135 -0.396525 0.001693
+vn -0.928658 0.361139 0.084690
+v -0.003341 -0.395564 -0.003586
+vn -0.771452 -0.485740 -0.410997
+v -0.002397 -0.404774 0.001051
+vn -0.702300 -0.465005 -0.539022
+v -0.002860 -0.405452 0.002522
+vn -0.852802 -0.397179 -0.339083
+v -0.003264 -0.402678 0.000409
+vn -0.914886 -0.238672 -0.325606
+v -0.003840 -0.401147 0.000345
+vn -0.823333 -0.463863 -0.327037
+v -0.002578 -0.403839 0.000283
+vn -0.999479 -0.030615 -0.010254
+v -0.003852 -0.398980 -0.001777
+vn -0.959392 -0.257525 -0.115098
+v -0.003561 -0.400800 -0.001375
+vn -0.974701 0.006928 -0.223407
+v -0.004006 -0.399238 -0.000026
+vn -0.895826 -0.413589 -0.162603
+v -0.002953 -0.402408 -0.001222
+vn -0.611111 -0.325688 -0.721437
+v -0.004737 -0.404724 0.003978
+vn -0.753873 0.017839 -0.656778
+v -0.006254 -0.399763 0.003710
+vn -0.811700 -0.227165 -0.538089
+v -0.004601 -0.401456 0.002028
+vn -0.771260 -0.360128 -0.524849
+v -0.003849 -0.403043 0.001879
+vn -0.714280 -0.198654 -0.671075
+v -0.005899 -0.401682 0.003719
+vn -0.818647 0.176430 -0.546525
+v -0.005166 -0.398121 0.002564
+vn -0.594941 -0.268830 -0.757480
+v -0.003261 -0.406298 0.003432
+vn -0.289688 0.509576 -0.810193
+v -0.012118 -0.396675 0.009548
+vn -0.325573 0.631657 -0.703571
+v -0.010672 -0.396405 0.009216
+vn -0.477575 -0.059778 -0.876555
+v -0.008845 -0.403663 0.006401
+vn -0.524673 -0.025124 -0.850933
+v -0.009636 -0.401796 0.006739
+vn -0.556035 0.097613 -0.825407
+v -0.010069 -0.400067 0.007073
+vn -0.437699 0.280841 -0.854136
+v -0.011994 -0.398606 0.008560
+vn -0.662980 0.326119 -0.673873
+v -0.007999 -0.398398 0.005778
+vn -0.523257 0.464379 -0.714530
+v -0.009514 -0.397739 0.007476
+vn -0.480565 0.365203 -0.797298
+v -0.010593 -0.398230 0.007915
+vn -0.622556 -0.121382 -0.773104
+v -0.007651 -0.401764 0.005345
+vn -0.444609 -0.045352 -0.894576
+v -0.007399 -0.405538 0.005791
+vn -0.520161 -0.165521 -0.837875
+v -0.005890 -0.405127 0.004915
+vn -0.686630 -0.344477 -0.640215
+v -0.004859 -0.403549 0.003440
+vn -0.573086 -0.198124 -0.795185
+v -0.006974 -0.403403 0.005202
+vn -0.653403 0.071006 -0.753672
+v -0.008381 -0.399815 0.005810
+vn -0.738797 0.373166 -0.561183
+v -0.006292 -0.397739 0.004287
+vn -0.483350 0.112623 -0.868152
+v -0.011559 -0.400282 0.007959
+vn -0.035039 -0.904806 0.424380
+v 0.014003 -0.422698 -0.003278
+vn -0.794281 0.521740 -0.311294
+v -0.003045 -0.387705 0.013757
+vn -0.518884 0.848050 -0.107568
+v -0.008334 -0.391196 0.018345
+vn -0.410073 0.866156 -0.285681
+v -0.009406 -0.392667 0.014316
+vn -0.285547 0.857498 -0.427972
+v -0.010632 -0.393952 0.012344
+vn -0.563487 0.807707 -0.173469
+v -0.006910 -0.390475 0.017021
+vn -0.508198 0.778168 -0.369038
+v -0.006860 -0.391732 0.013116
+vn -0.599388 0.758586 -0.255502
+v -0.005822 -0.390165 0.015193
+vn -0.483022 0.855424 -0.186922
+v -0.008616 -0.391708 0.016351
+vn -0.405291 0.819106 -0.405961
+v -0.008668 -0.392935 0.012759
+vn -0.632372 0.647504 -0.425258
+v -0.004410 -0.391390 0.010315
+vn -0.577526 0.660404 -0.479927
+v -0.005395 -0.393441 0.008636
+vn -0.731947 0.562417 -0.384630
+v -0.002690 -0.389317 0.010555
+vn -0.292871 0.913951 -0.280926
+v -0.011251 -0.393532 0.013949
+vn -0.000479 0.929590 -0.368594
+v -0.013448 -0.394449 0.012242
+vn -0.055071 0.954569 -0.292856
+v -0.013140 -0.394166 0.013025
+vn -0.503256 0.820054 -0.272479
+v -0.007471 -0.391423 0.014863
+vn -0.595256 0.690092 -0.411636
+v -0.005205 -0.391420 0.011407
+vn -0.700590 0.618757 -0.355406
+v -0.003758 -0.389472 0.012336
+vn -0.610584 0.714413 -0.341763
+v -0.005270 -0.390552 0.013114
+vn -0.750671 0.553516 -0.360712
+v -0.002326 -0.390794 0.007758
+vn -0.679776 0.601606 -0.419494
+v -0.003399 -0.391235 0.009021
+vn -0.661007 0.610263 -0.436633
+v -0.005893 -0.396227 0.005378
+vn -0.418708 0.758207 -0.499806
+v -0.008297 -0.393731 0.011070
+vn -0.397804 0.681951 -0.613755
+v -0.009399 -0.395792 0.009168
+vn -0.591037 0.584610 -0.555793
+v -0.007368 -0.396933 0.006290
+vn -0.577802 0.654700 -0.487354
+v -0.006069 -0.395213 0.007030
+vn -0.511148 0.669988 -0.538371
+v -0.007237 -0.395318 0.008052
+vn -0.649179 0.625002 -0.433518
+v -0.004217 -0.393110 0.007596
+vn -0.063451 0.808255 -0.585404
+v -0.012276 -0.395021 0.011013
+vn -0.177940 0.743944 -0.644115
+v -0.011469 -0.395200 0.010704
+vn -0.661492 0.637554 -0.394910
+v -0.004636 -0.394485 0.006172
+vn -0.761228 0.555401 -0.334756
+v -0.004818 -0.396025 0.003673
+vn -0.507084 0.607860 -0.611042
+v -0.008584 -0.396744 0.007580
+vn -0.459785 0.704468 -0.540668
+v -0.007958 -0.394724 0.009415
+vn -0.508910 0.735109 -0.447913
+v -0.006636 -0.392557 0.011323
+vn -0.531209 0.695487 -0.483855
+v -0.006256 -0.393270 0.009817
+vn -0.307579 0.767624 -0.562271
+v -0.009939 -0.394666 0.010828
+vn -0.726007 0.683527 -0.075528
+v -0.005170 -0.388687 0.018924
+vn -0.572068 0.817275 -0.069284
+v -0.007122 -0.390306 0.019199
+vn -0.666391 0.729062 -0.156181
+v -0.005548 -0.389300 0.017506
+vn -0.620843 0.779359 0.084578
+v -0.006139 -0.389605 0.021640
+vn -0.537246 0.817526 0.207408
+v -0.006768 -0.390533 0.024214
+vn -0.629872 0.759638 0.161898
+v -0.005711 -0.389487 0.022959
+vn -0.527056 0.772913 0.353295
+v -0.005274 -0.390441 0.026779
+vn -0.646449 0.727729 0.229161
+v -0.005124 -0.389314 0.024163
+vn -0.372438 0.764799 0.525711
+v -0.003605 -0.390398 0.028529
+vn -0.426811 0.688844 0.585940
+v -0.002402 -0.387816 0.025802
+vn -0.609882 0.792470 -0.005897
+v -0.006500 -0.389811 0.020331
+vn -0.539662 0.693412 0.477436
+v -0.003230 -0.388348 0.025821
+vn -0.635579 0.590871 0.496900
+v -0.002298 -0.385917 0.023703
+vn -0.831909 0.532574 -0.155863
+v -0.003900 -0.387340 0.017313
+vn -0.743841 0.668190 0.014912
+v -0.004953 -0.388406 0.020145
+vn -0.762605 0.640623 0.089648
+v -0.004621 -0.388143 0.021377
+vn -0.662607 0.689135 0.293334
+v -0.004418 -0.388928 0.024872
+vn -0.513594 0.745321 0.425109
+v -0.004293 -0.390052 0.027334
+vn -0.706134 0.626790 0.329407
+v -0.003616 -0.387862 0.024428
+vn -0.540052 0.831477 0.130346
+v -0.007362 -0.390612 0.022750
+vn -0.870704 0.491804 0.001562
+v -0.003548 -0.386437 0.019672
+vn -0.882237 0.466095 0.066433
+v -0.003260 -0.385994 0.020783
+vn -0.941685 0.332125 -0.054056
+v -0.001628 -0.381955 0.018200
+vn -0.926196 0.376981 -0.006766
+v -0.002361 -0.383878 0.019276
+vn -0.863502 0.476730 0.164599
+v -0.003020 -0.385825 0.021875
+vn -0.902450 0.411861 0.126310
+v -0.002207 -0.383644 0.020391
+vn -0.869989 0.409450 0.274719
+v -0.001024 -0.381045 0.020369
+vn -0.775504 0.540489 0.326290
+v -0.002756 -0.385904 0.022940
+vn -0.920367 0.384236 -0.072712
+v -0.002511 -0.384375 0.018061
+vn -0.889928 0.304667 -0.339421
+v -0.001239 -0.384654 0.012665
+vn -0.782778 0.507678 -0.359891
+v -0.002372 -0.387900 0.011931
+vn -0.852465 0.517729 -0.072531
+v -0.003773 -0.386893 0.018460
+vn -0.910187 0.304129 -0.281185
+v -0.001485 -0.383636 0.014292
+vn -0.889070 0.391261 -0.237633
+v -0.002639 -0.385669 0.015624
+vn -0.907853 0.391697 -0.149587
+v -0.002643 -0.385047 0.016876
+vn -0.924032 0.329587 -0.193747
+v -0.001723 -0.383202 0.015739
+vn -0.539574 0.840480 0.049528
+v -0.007898 -0.390798 0.021248
+vn -0.527607 0.849027 -0.028014
+v -0.008411 -0.391111 0.019981
+vn -0.799880 0.565200 0.201843
+v -0.003726 -0.387381 0.023016
+vn -0.784598 0.601459 0.150511
+v -0.004166 -0.387798 0.022442
+vn -0.743382 0.476118 0.469782
+v -0.001420 -0.383443 0.022264
+vn -0.710755 0.649336 -0.270537
+v -0.004393 -0.388977 0.014795
+vn -0.785828 0.578489 -0.218687
+v -0.004046 -0.387955 0.016210
+vn -0.857990 0.423975 -0.289999
+v -0.002725 -0.386530 0.014633
+vn -0.849111 0.449655 0.277165
+v -0.001829 -0.383217 0.021140
+vn -0.537949 0.575287 0.616162
+v -0.001572 -0.385636 0.024124
+vn -0.641496 0.458170 0.615275
+v -0.000643 -0.382899 0.022764
+vn -0.698489 0.391534 0.599011
+v 0.000309 -0.379797 0.021709
+vn -0.457300 0.526986 0.716353
+v -0.000554 -0.384927 0.024294
+vn -0.548514 0.408947 0.729311
+v 0.000496 -0.381496 0.022881
+vn -0.360751 0.661919 0.657055
+v -0.001418 -0.387341 0.025902
+vn -0.067232 0.278616 0.958046
+v 0.010095 -0.381166 0.025487
+vn -0.025956 0.343454 0.938811
+v 0.009249 -0.384962 0.026676
+vn -0.095756 0.390101 0.915779
+v 0.007291 -0.385066 0.026570
+vn -0.172989 0.357108 0.917904
+v 0.006524 -0.383077 0.025603
+vn -0.136746 0.435149 0.889913
+v 0.005772 -0.385386 0.026493
+vn -0.135559 0.310700 0.940792
+v 0.008023 -0.381967 0.025489
+vn -0.194683 0.360643 0.912160
+v 0.005307 -0.383150 0.025383
+vn -0.237147 0.470910 0.849709
+v 0.002546 -0.384957 0.025543
+vn -0.370967 0.472242 0.799607
+v 0.000624 -0.384092 0.024364
+vn -0.217274 0.399556 0.890588
+v 0.003926 -0.383889 0.025378
+vn -0.172538 0.455024 0.873604
+v 0.004594 -0.385421 0.026272
+vn -0.143670 0.515925 0.844500
+v 0.003461 -0.386535 0.026696
+vn 0.001428 0.613439 0.789741
+v 0.001616 -0.389813 0.028889
+vn -0.137062 0.648615 0.748674
+v 0.000547 -0.388193 0.027417
+vn -0.185440 0.704596 0.684950
+v -0.000947 -0.388828 0.027696
+vn -0.269233 0.739480 0.616995
+v -0.002214 -0.389216 0.027666
+vn -0.132417 0.582608 0.801894
+v 0.002035 -0.387456 0.027072
+vn -0.247312 0.547634 0.799333
+v 0.001173 -0.385892 0.025716
+vn -0.285906 0.614360 0.735404
+v -0.000177 -0.386677 0.025860
+vn 0.072463 0.361703 0.929473
+v 0.009991 -0.388536 0.028044
+vn -0.183539 0.278449 0.942751
+v 0.006594 -0.380633 0.024799
+vn -0.130175 0.242449 0.961391
+v 0.008835 -0.378408 0.024600
+vn -0.153286 0.218909 0.963630
+v 0.008539 -0.375770 0.023974
+vn -0.351582 0.755301 0.553093
+v -0.003008 -0.389384 0.027449
+vn -0.105283 0.249512 0.962631
+v 0.009821 -0.374838 0.023939
+vn -0.453396 0.359216 0.815718
+v 0.001517 -0.381072 0.023317
+vn -0.332808 0.406241 0.851004
+v 0.001864 -0.383208 0.024417
+vn -0.228222 0.292659 0.928582
+v 0.004851 -0.380948 0.024517
+vn -0.382922 0.303539 0.872488
+v 0.002698 -0.380113 0.023550
+vn -0.398580 0.257892 0.880128
+v 0.003659 -0.376920 0.023012
+vn -0.287354 0.340829 0.895133
+v 0.003240 -0.382065 0.024438
+vn -0.285715 0.258756 0.922720
+v 0.004211 -0.378886 0.023750
+vn -0.693084 0.357489 0.625968
+v 0.002113 -0.375836 0.021391
+vn -0.677842 0.367705 0.636650
+v 0.001242 -0.377914 0.021612
+vn -0.631497 0.367482 0.682765
+v 0.003224 -0.374472 0.021806
+vn -0.459957 0.306194 0.833477
+v 0.003996 -0.374928 0.022575
+vn -0.198806 0.256683 0.945828
+v 0.006884 -0.375143 0.023522
+vn -0.166649 0.538035 0.826285
+v 0.007209 -0.373938 0.023135
+vn -0.508792 0.302312 0.806063
+v 0.002449 -0.378068 0.022706
+vn -0.586011 0.343336 0.733970
+v 0.001648 -0.378436 0.022267
+vn -0.629637 0.329742 0.703440
+v 0.002533 -0.376042 0.021936
+vn -0.809822 0.404985 0.424472
+v -0.000422 -0.380488 0.021170
+vn -0.543263 0.296356 0.785518
+v 0.003214 -0.375706 0.022357
+vn -0.332719 0.259174 0.906712
+v 0.004570 -0.375184 0.022937
+vn 0.241539 0.299088 0.923149
+v 0.019873 -0.378560 0.024041
+vn 0.456351 0.246359 0.855015
+v 0.024415 -0.380314 0.023084
+vn 0.675284 0.323691 0.662733
+v 0.028072 -0.381355 0.020634
+vn 0.918142 0.279308 -0.281074
+v 0.028278 -0.377697 -0.006352
+vn 0.558974 0.268237 -0.784600
+v 0.019074 -0.375643 -0.019147
+vn 0.042679 0.646403 0.761801
+v 0.010684 -0.374153 0.023656
+vn 0.100376 0.912044 0.397618
+v 0.010075 -0.373688 0.023128
+vn 0.169476 0.849035 0.500418
+v 0.011614 -0.374093 0.023469
+vn 0.244695 0.837819 0.488040
+v 0.013390 -0.374518 0.023448
+vn 0.254097 0.466826 0.847058
+v 0.017890 -0.376627 0.023788
+vn 0.493963 0.581025 0.646847
+v 0.024355 -0.378578 0.022288
+vn 0.545161 0.350158 0.761701
+v 0.024907 -0.379581 0.022503
+vn 0.637827 0.429450 0.639335
+v 0.027148 -0.380136 0.020813
+vn 0.834416 0.357040 0.419849
+v 0.029200 -0.381077 0.018844
+vn 0.845526 0.417514 0.332817
+v 0.030201 -0.380343 0.015951
+vn 0.939053 0.281442 0.197410
+v 0.030682 -0.380652 0.014748
+vn 0.804033 0.546199 0.234941
+v 0.030228 -0.379539 0.014532
+vn 0.832380 0.545663 0.096927
+v 0.030329 -0.379116 0.012717
+vn 0.956025 0.287525 0.057847
+v 0.030739 -0.380192 0.013395
+vn 0.981474 0.169263 -0.089773
+v 0.030664 -0.380021 0.011740
+vn 0.743858 0.667230 0.038465
+v 0.029809 -0.378165 0.009420
+vn 0.571827 0.816684 0.077725
+v 0.029333 -0.377580 0.007498
+vn 0.483789 0.873018 0.061536
+v 0.029050 -0.377199 0.005413
+vn 0.858081 0.513504 0.003328
+v 0.029606 -0.377724 0.005248
+vn 0.713551 0.700112 -0.026252
+v 0.029256 -0.377299 0.003233
+vn 0.952942 0.298577 -0.052475
+v 0.029733 -0.378174 0.002800
+vn 0.633319 0.772842 -0.040270
+v 0.028782 -0.376989 0.001068
+vn 0.909485 0.401658 -0.107276
+v 0.029478 -0.377953 0.000746
+vn 0.749279 0.661191 -0.037506
+v 0.028191 -0.376685 -0.003477
+vn 0.817586 0.557465 -0.144175
+v 0.028162 -0.376780 -0.005120
+vn 0.850151 0.396964 -0.345924
+v 0.026620 -0.376571 -0.009588
+vn 0.470983 0.879260 -0.071259
+v 0.025103 -0.375075 -0.010157
+vn 0.410994 0.909723 -0.059058
+v 0.024245 -0.374750 -0.011828
+vn 0.589038 0.544821 -0.596829
+v 0.022191 -0.375058 -0.016134
+vn 0.623524 0.292289 -0.725110
+v 0.021025 -0.375979 -0.017716
+vn 0.582359 0.609576 -0.537843
+v 0.021274 -0.374722 -0.016731
+vn 0.372233 0.920249 -0.120769
+v 0.021050 -0.373997 -0.015413
+vn 0.625944 0.393046 -0.673580
+v 0.019798 -0.374497 -0.018084
+vn 0.611342 0.662244 -0.433237
+v 0.020452 -0.374217 -0.017121
+vn 0.215998 0.961092 -0.172186
+v 0.018209 -0.373210 -0.017708
+vn 0.282225 0.505876 -0.815131
+v 0.018093 -0.374157 -0.019190
+vn 0.148875 0.768726 -0.622010
+v 0.013372 -0.372813 -0.019427
+vn 0.040847 0.692186 -0.720563
+v 0.012296 -0.373037 -0.019783
+vn 0.033871 0.482265 -0.875370
+v 0.012017 -0.374000 -0.020504
+vn -0.068980 0.762386 -0.643436
+v 0.011147 -0.372739 -0.019415
+vn -0.113278 0.756893 -0.643646
+v 0.009736 -0.372770 -0.019240
+vn 0.032344 0.852701 -0.521397
+v 0.012157 -0.372281 -0.018819
+vn -0.897548 0.418493 0.138821
+v 0.001286 -0.374382 0.018001
+vn -0.741786 0.383610 0.550088
+v 0.001972 -0.375198 0.020801
+vn -0.765872 0.454717 0.454613
+v 0.002632 -0.373476 0.020402
+vn -0.748602 0.661689 0.041988
+v 0.002233 -0.372172 0.014815
+vn -0.919249 0.368185 0.139361
+v 0.000555 -0.376354 0.018663
+vn -0.792023 0.381308 0.476765
+v 0.000453 -0.378280 0.020860
+vn -0.881016 0.403627 0.246771
+v 0.001112 -0.375385 0.019324
+vn -0.442637 0.824899 -0.351588
+v 0.006252 -0.372284 -0.016794
+vn -0.467324 0.784274 -0.408071
+v 0.006089 -0.372841 -0.017906
+vn -0.668635 0.668240 -0.326163
+v 0.004995 -0.371916 -0.013882
+vn -0.424520 0.852507 -0.304984
+v 0.006015 -0.371420 -0.014483
+vn -0.308744 0.931496 -0.192335
+v 0.005305 -0.370927 -0.011608
+vn -0.629385 0.721665 -0.288226
+v 0.004645 -0.371535 -0.012340
+vn -0.682623 0.381755 0.623128
+v 0.002815 -0.374348 0.021306
+vn -0.482437 0.873933 -0.059123
+v 0.004616 -0.370972 -0.010227
+vn -0.745181 0.382806 0.546044
+v 0.001173 -0.376865 0.020873
+vn -0.878852 0.374914 0.295058
+v 0.000355 -0.377685 0.020081
+vn -0.155027 0.986846 -0.045845
+v 0.006071 -0.370508 -0.007725
+vn -0.859409 0.509906 0.037573
+v 0.004577 -0.371442 -0.004620
+vn -0.530729 0.847532 -0.003935
+v 0.005164 -0.370784 -0.006011
+vn -0.481368 0.875167 -0.048663
+v 0.005392 -0.370597 -0.005005
+vn -0.582550 0.811957 -0.036885
+v 0.004983 -0.371128 0.000903
+vn -0.537682 0.843138 -0.004036
+v 0.005180 -0.371035 -0.000623
+vn 0.065874 0.997464 0.026952
+v 0.006091 -0.370877 0.002498
+vn -0.903288 0.246529 -0.351133
+v 0.000284 -0.383247 0.009949
+vn -0.925606 0.166828 -0.339738
+v 0.001297 -0.380407 0.008979
+vn -0.931419 0.362735 0.029690
+v 0.001029 -0.374679 0.016658
+vn -0.903701 0.428084 -0.008314
+v 0.001681 -0.373035 0.014513
+vn -0.949176 0.304364 -0.080175
+v 0.001209 -0.374362 0.014176
+vn -0.912338 0.402062 -0.077362
+v 0.002072 -0.372406 0.012024
+vn -0.949016 0.267601 -0.166607
+v 0.002111 -0.372862 0.010638
+vn -0.957288 0.225881 -0.180493
+v 0.001324 -0.374953 0.012225
+vn -0.959822 0.151086 -0.236464
+v 0.002570 -0.374502 0.007177
+vn -0.966689 0.086485 -0.240899
+v 0.002813 -0.375588 0.005720
+vn -0.857048 0.454656 -0.242396
+v 0.003588 -0.371505 0.006357
+vn -0.951106 0.215684 -0.221082
+v 0.002491 -0.373233 0.008505
+vn -0.957552 0.213061 -0.194164
+v 0.003369 -0.373136 0.004570
+vn -0.944660 0.298695 -0.135639
+v -0.000738 -0.379899 0.016326
+vn -0.942592 0.240446 -0.231747
+v 0.000013 -0.379189 0.013641
+vn -0.953138 0.178572 -0.244212
+v 0.001391 -0.376381 0.010668
+vn -0.934829 0.187026 -0.301855
+v 0.000710 -0.379370 0.011149
+vn -0.948817 0.077828 -0.306088
+v 0.002371 -0.378196 0.006726
+vn -0.930111 0.361789 0.063259
+v -0.000447 -0.378812 0.018285
+vn -0.936370 0.145862 -0.319272
+v 0.002162 -0.380187 0.006766
+vn -0.944831 0.321550 -0.062445
+v -0.000518 -0.378991 0.017086
+vn -0.949926 0.119845 -0.288580
+v 0.001909 -0.377351 0.008405
+vn -0.932737 0.359111 0.032250
+v -0.001379 -0.381284 0.018749
+vn -0.921688 0.373398 0.105191
+v -0.001115 -0.380667 0.019205
+vn -0.960083 0.071135 -0.270519
+v 0.002738 -0.378617 0.005503
+vn -0.939527 0.341597 0.024512
+v 0.000327 -0.376635 0.017291
+vn -0.906373 0.413638 -0.085973
+v 0.004307 -0.371959 0.000817
+vn -0.912061 0.238037 -0.333890
+v -0.000126 -0.382074 0.011759
+vn -0.923379 0.327639 -0.200058
+v 0.003390 -0.372284 0.005695
+vn -0.852445 0.384814 -0.353913
+v -0.001775 -0.386457 0.012309
+vn -0.915536 0.334781 -0.222969
+v 0.003012 -0.371711 0.008036
+vn -0.926241 0.267317 -0.265743
+v -0.000620 -0.381438 0.013719
+vn -0.950767 0.297538 -0.086673
+v 0.000348 -0.376733 0.015583
+vn -0.951309 0.252011 -0.177485
+v 0.000341 -0.377454 0.014199
+vn -0.907362 0.373131 0.193567
+v -0.000219 -0.378671 0.019475
+vn -0.937326 0.310701 -0.157750
+v -0.001358 -0.381841 0.016343
+vn -0.206250 0.973481 -0.098968
+v 0.004467 -0.370902 0.006048
+vn -0.948024 0.313347 -0.055357
+v 0.001814 -0.385170 0.000892
+vn -0.940969 0.336912 -0.032683
+v 0.001318 -0.386722 0.000115
+vn -0.946525 0.313902 0.074545
+v 0.001767 -0.385273 -0.002056
+vn -0.918710 0.393847 0.029266
+v 0.000805 -0.388084 -0.000342
+vn -0.907746 0.405788 -0.106458
+v 0.000196 -0.389245 0.001629
+vn -0.928006 0.332874 -0.167330
+v 0.001369 -0.385561 0.003486
+vn -0.719021 0.631929 -0.289265
+v -0.004266 -0.394909 0.004625
+vn -0.906615 0.304109 -0.292518
+v 0.000523 -0.384721 0.008112
+vn -0.957412 0.280155 -0.069829
+v 0.002313 -0.383520 0.001505
+vn -0.928711 0.300703 -0.216964
+v 0.001623 -0.383536 0.005591
+vn -0.963255 0.220997 -0.152642
+v 0.002641 -0.380956 0.004149
+vn -0.956767 0.259460 -0.131442
+v 0.002434 -0.382501 0.002933
+vn -0.970315 0.238346 0.040979
+v 0.002599 -0.382492 -0.000573
+vn -0.821318 0.500370 -0.273982
+v -0.001675 -0.391320 0.005215
+vn -0.739483 0.600555 -0.304135
+v -0.003141 -0.393353 0.005246
+vn -0.775888 0.604521 -0.180421
+v -0.002610 -0.393281 0.003764
+vn -0.845253 0.448585 -0.290379
+v -0.000951 -0.388718 0.007638
+vn -0.740085 0.574002 -0.350421
+v -0.002922 -0.392349 0.006519
+vn -0.783545 0.501099 -0.367366
+v -0.001713 -0.389022 0.009026
+vn -0.827921 0.431579 -0.358170
+v -0.001425 -0.387389 0.010475
+vn -0.872324 0.375246 -0.313436
+v -0.000485 -0.386971 0.008640
+vn -0.881035 0.318393 -0.349863
+v -0.000564 -0.385474 0.010350
+vn -0.877012 0.478702 -0.041163
+v -0.000260 -0.390286 0.001223
+vn -0.920368 0.366510 -0.136357
+v 0.000717 -0.387767 0.002377
+vn -0.821423 0.566838 -0.062921
+v -0.001445 -0.392076 0.001985
+vn -0.811378 0.538567 -0.227180
+v -0.001898 -0.392166 0.004261
+vn -0.876138 0.441916 -0.192593
+v -0.000634 -0.390288 0.003556
+vn -0.847214 0.512096 -0.141373
+v -0.001116 -0.391425 0.002821
+vn -0.890460 0.402691 -0.211947
+v -0.000137 -0.388746 0.004602
+vn -0.838608 0.473933 -0.268560
+v -0.001221 -0.389899 0.006402
+vn -0.902458 0.362546 -0.232660
+v 0.000225 -0.386910 0.006357
+vn -0.985303 0.165694 0.041520
+v 0.003922 -0.374543 -0.001983
+vn -0.917940 0.239995 -0.315893
+v 0.001401 -0.382298 0.007659
+vn -0.941351 0.235434 -0.241722
+v 0.002168 -0.381406 0.005961
+vn -0.967204 0.153952 -0.202026
+v 0.002737 -0.379672 0.005006
+vn -0.980990 0.068214 -0.181676
+v 0.003122 -0.377904 0.004060
+vn -0.973313 0.046786 -0.224662
+v 0.003056 -0.376696 0.004525
+vn -0.987630 0.079914 -0.134910
+v 0.003549 -0.375786 0.002091
+vn -0.980370 0.179052 0.082553
+v 0.003165 -0.379394 -0.001843
+vn -0.979756 0.166604 0.110998
+v 0.003592 -0.375867 -0.003209
+vn -0.991745 0.116651 -0.053238
+v 0.003625 -0.376740 0.000612
+vn -0.985172 0.124079 -0.118490
+v 0.003222 -0.378500 0.002997
+vn -0.982541 0.172887 -0.068732
+v 0.003158 -0.379710 0.001839
+vn -0.978794 0.204492 -0.012086
+v 0.002948 -0.381090 0.000600
+vn -0.987555 0.154591 0.028916
+v 0.003478 -0.377983 -0.000627
+vn -0.976940 0.188757 0.099798
+v 0.002777 -0.380688 -0.003176
+vn -0.975658 0.137341 -0.170963
+v 0.003557 -0.373929 0.002907
+vn -0.977161 0.180511 -0.112128
+v 0.004037 -0.373045 0.000378
+vn -0.388381 0.921455 0.009048
+v 0.003399 -0.371093 0.009217
+vn -0.644337 0.758187 -0.099911
+v 0.003406 -0.371125 0.007932
+vn -0.857286 0.499567 -0.124471
+v 0.002747 -0.371632 0.009581
+vn -0.132132 0.991227 0.003272
+v 0.004098 -0.370877 0.007559
+vn -0.013432 0.997840 0.064298
+v 0.004245 -0.370961 0.009586
+vn -0.690570 0.723103 -0.015372
+v 0.002719 -0.371508 0.010819
+vn -0.718790 0.694712 0.026747
+v 0.002426 -0.371846 0.012766
+vn -0.309750 0.948750 0.062674
+v 0.003285 -0.371215 0.011253
+vn -0.840373 0.493376 -0.224397
+v 0.003842 -0.372136 -0.011519
+vn -0.599772 0.799195 0.039504
+v 0.004727 -0.371058 -0.006898
+vn -0.970817 0.231367 0.063120
+v 0.003144 -0.373719 -0.009577
+vn -0.911688 0.410677 -0.013047
+v 0.003601 -0.372201 -0.010099
+vn -0.400652 0.910796 0.099643
+v 0.002835 -0.371737 0.015546
+vn -0.956185 0.244877 0.160455
+v 0.003775 -0.373435 -0.004982
+vn -0.978117 0.161562 0.131089
+v 0.003230 -0.377282 -0.004226
+vn -0.975397 0.170471 0.139787
+v 0.003481 -0.374889 -0.005029
+vn -0.986968 0.150648 -0.056564
+v 0.004004 -0.373929 -0.000591
+vn -0.952815 0.293389 0.077884
+v 0.004285 -0.372326 -0.003424
+vn -0.633777 0.736381 0.236793
+v 0.002598 -0.372736 0.019098
+vn -0.720767 0.600357 0.346506
+v 0.002534 -0.373058 0.019742
+vn -0.828376 0.462154 0.316556
+v 0.001847 -0.374061 0.019572
+vn -0.863567 0.488714 0.124140
+v 0.004280 -0.371837 -0.005410
+vn -0.784352 0.616701 0.066865
+v 0.004197 -0.371559 -0.007459
+vn -0.957589 0.265845 0.111131
+v 0.003393 -0.373107 -0.008528
+vn -0.973699 0.185126 0.132809
+v 0.003309 -0.374745 -0.006479
+vn -0.959123 0.261927 0.107131
+v 0.003664 -0.373077 -0.006314
+vn -0.921565 0.372425 0.109625
+v 0.003729 -0.372352 -0.007857
+vn -0.972944 0.183576 0.140284
+v 0.002962 -0.375662 -0.007727
+vn -0.528181 0.761964 0.374748
+v 0.003173 -0.372792 0.020453
+vn -0.856049 0.512839 -0.064630
+v 0.004611 -0.371591 -0.000773
+vn -0.896446 0.420398 -0.140180
+v 0.004109 -0.371952 0.002511
+vn -0.455936 0.886714 0.076549
+v 0.005405 -0.370882 -0.002257
+vn -0.922297 0.385757 -0.023654
+v 0.004521 -0.371923 -0.001605
+vn -0.962893 0.245518 0.112059
+v 0.004021 -0.373144 -0.003931
+vn -0.889294 0.440555 0.122749
+v 0.004015 -0.372064 -0.006178
+vn -0.567961 0.504893 0.650003
+v 0.003408 -0.373501 0.021347
+vn -0.241712 0.751678 0.613641
+v 0.004244 -0.373131 0.021572
+vn -0.362736 0.525960 0.769278
+v 0.004307 -0.373864 0.022236
+vn -0.315179 0.425974 0.848061
+v 0.004766 -0.374247 0.022673
+vn -0.169283 0.699958 0.693831
+v 0.005583 -0.373528 0.022444
+vn -0.070721 0.693534 0.716945
+v 0.009035 -0.373855 0.023369
+vn 0.018875 0.914878 0.403288
+v 0.007850 -0.373309 0.022529
+vn -0.066500 0.452233 0.889417
+v 0.009919 -0.374297 0.023753
+vn -0.970667 0.169840 0.170177
+v 0.002728 -0.378273 -0.006500
+vn -0.836342 0.545676 0.052626
+v 0.003919 -0.371637 -0.009408
+vn -0.947526 0.306625 -0.090423
+v 0.003371 -0.372903 -0.010816
+vn -0.966362 0.254970 -0.033688
+v 0.002946 -0.374564 -0.010929
+vn -0.701749 0.589271 -0.400385
+v 0.005342 -0.373491 -0.017886
+vn -0.951372 0.244547 0.187317
+v 0.002002 -0.379030 -0.009301
+vn -0.963076 0.194379 0.186283
+v 0.002505 -0.378125 -0.007785
+vn -0.970600 0.215930 0.106346
+v 0.002653 -0.376080 -0.009430
+vn -0.972669 0.183582 0.142170
+v 0.002614 -0.380455 -0.004909
+vn -0.959056 0.208381 0.191804
+v 0.002384 -0.380560 -0.006091
+vn -0.944376 0.233118 0.231968
+v 0.001978 -0.381065 -0.007375
+vn -0.932600 0.306202 0.191045
+v 0.001234 -0.380564 -0.010854
+vn -0.922433 0.276728 0.269330
+v 0.001165 -0.382377 -0.009005
+vn -0.954263 0.293576 0.056530
+v 0.002138 -0.377562 -0.011281
+vn -0.923945 0.332107 -0.189815
+v 0.003409 -0.373479 -0.012067
+vn -0.942951 0.318372 -0.097378
+v 0.002801 -0.375623 -0.012542
+vn -0.822161 0.498430 -0.274990
+v 0.004256 -0.373474 -0.014833
+vn -0.750112 0.601080 -0.275746
+v 0.004971 -0.373190 -0.016332
+vn -0.847247 0.484857 -0.216995
+v 0.003850 -0.374979 -0.016348
+vn 0.570462 0.797137 0.197853
+v 0.029418 -0.378525 0.014134
+vn -0.924684 0.371146 0.084912
+v 0.001031 -0.380383 -0.012619
+vn -0.912220 0.315118 0.261832
+v 0.000484 -0.382718 -0.010849
+vn -0.899157 0.382217 -0.213136
+v 0.003410 -0.374469 -0.013619
+vn -0.914409 0.390680 -0.105950
+v 0.002645 -0.376570 -0.014032
+vn -0.884138 0.437894 -0.162939
+v 0.003096 -0.375943 -0.015170
+vn -0.923259 0.383851 -0.015820
+v 0.001875 -0.378301 -0.013410
+vn -0.841176 0.538937 -0.044379
+v -0.001533 -0.392286 0.000907
+vn -0.889255 0.413096 -0.196410
+v -0.002879 -0.394778 0.000420
+vn -0.894293 0.445402 0.043100
+v 0.000028 -0.389551 -0.001429
+vn -0.900259 0.430634 0.063946
+v -0.000019 -0.389508 -0.002673
+vn -0.888144 0.459550 -0.003701
+v -0.001369 -0.392195 -0.001476
+vn -0.874253 0.479495 -0.075939
+v -0.001677 -0.392619 -0.000075
+vn -0.852590 0.432380 -0.293493
+v -0.003383 -0.395169 0.001405
+vn -0.905685 0.421073 -0.049311
+v -0.002670 -0.394659 -0.000737
+vn -0.886956 0.441936 0.134175
+v -0.000299 -0.389813 -0.003826
+vn -0.876025 0.474549 0.085929
+v -0.001504 -0.392321 -0.002566
+vn -0.871158 0.458307 0.176180
+v -0.000951 -0.390749 -0.004705
+vn -0.866363 0.482936 0.127234
+v -0.001835 -0.392724 -0.003390
+vn -0.930950 0.343712 0.123265
+v 0.001088 -0.386593 -0.003994
+vn -0.930116 0.283819 0.233090
+v 0.001393 -0.384166 -0.006495
+vn -0.885542 0.399120 0.237738
+v -0.000072 -0.388222 -0.006333
+vn -0.904037 0.322025 0.281099
+v 0.000597 -0.385472 -0.007751
+vn -0.928627 0.356800 0.101718
+v 0.001246 -0.386552 -0.002551
+vn -0.877032 0.477902 0.049250
+v -0.000073 -0.389887 -0.000052
+vn -0.948539 0.267033 0.170198
+v 0.001851 -0.383526 -0.005207
+vn -0.951437 0.266751 0.153661
+v 0.002138 -0.383132 -0.004149
+vn -0.911107 0.365110 0.191256
+v 0.000691 -0.387088 -0.005232
+vn -0.784826 0.596380 -0.168457
+v -0.003106 -0.394139 0.002890
+vn -0.823201 0.526752 -0.211830
+v -0.003272 -0.394671 0.001953
+vn -0.920214 0.239021 -0.309959
+v -0.003983 -0.397238 0.000856
+vn -0.886858 0.445474 0.122618
+v -0.002696 -0.393949 -0.004646
+vn -0.918526 0.377203 0.118445
+v -0.003250 -0.394929 -0.005333
+vn -0.964956 0.260184 0.034134
+v 0.002169 -0.383994 -0.001426
+vn -0.969404 0.226498 0.094629
+v 0.002359 -0.382764 -0.003271
+vn -0.976963 0.160786 0.140323
+v 0.002958 -0.377750 -0.005566
+vn -0.880766 0.459732 0.113569
+v -0.002637 -0.394162 -0.003307
+vn -0.869274 0.466241 0.164265
+v -0.001865 -0.392332 -0.004914
+vn -0.890174 0.446159 0.092373
+v -0.002622 -0.394452 -0.001972
+vn -0.966694 0.254063 -0.030895
+v -0.003581 -0.396906 -0.000953
+vn -0.920634 0.376951 0.101691
+v -0.003311 -0.395780 -0.002228
+vn -0.975504 0.207735 0.072379
+v -0.003801 -0.396849 -0.004308
+vn -0.903618 0.312838 0.292586
+v 0.000036 -0.384742 -0.010111
+vn -0.919645 0.295552 0.258655
+v -0.004074 -0.396086 -0.007622
+vn -0.896881 0.394363 0.200207
+v -0.003268 -0.394278 -0.007201
+vn -0.852710 0.429412 0.297476
+v -0.003862 -0.394341 -0.009241
+vn -0.777325 0.427382 0.461639
+v -0.005152 -0.396045 -0.010115
+vn -0.857870 0.485572 0.168164
+v -0.003899 -0.391912 -0.014330
+vn -0.876759 0.422154 0.230390
+v -0.001276 -0.390239 -0.007167
+vn -0.881787 0.437707 0.175686
+v -0.002393 -0.392832 -0.006332
+vn -0.856557 0.457965 0.237863
+v -0.003319 -0.392085 -0.011330
+vn -0.886520 0.386952 0.253675
+v -0.001797 -0.389590 -0.010044
+vn -0.874782 0.424203 0.234112
+v -0.002510 -0.392037 -0.008547
+vn -0.614437 0.560983 0.554766
+v -0.006483 -0.396262 -0.011777
+vn -0.889281 0.360812 0.281058
+v -0.000514 -0.387362 -0.008919
+vn -0.905216 0.337969 0.257606
+v -0.001309 -0.387046 -0.011759
+vn -0.900828 0.374894 0.219003
+v -0.002638 -0.389875 -0.012697
+vn -0.912527 0.335284 0.234265
+v -0.000374 -0.384177 -0.012082
+vn -0.823287 0.557820 -0.105055
+v 0.001808 -0.379040 -0.018573
+vn -0.752766 0.594519 -0.282649
+v 0.003391 -0.377101 -0.019344
+vn -0.882074 0.466702 0.064296
+v -0.000304 -0.382535 -0.016864
+vn -0.901834 0.413626 0.124939
+v -0.001315 -0.384709 -0.015990
+vn -0.871638 0.484618 0.073437
+v -0.001238 -0.383852 -0.019172
+vn -0.859226 0.511586 -0.003295
+v 0.000681 -0.380744 -0.017716
+vn -0.837561 0.546301 0.006914
+v -0.000268 -0.382178 -0.019855
+vn -0.779778 0.613206 -0.126192
+v 0.000989 -0.380482 -0.020508
+vn -0.889505 0.455934 -0.030098
+v 0.001630 -0.378986 -0.015478
+vn -0.862344 0.493953 -0.111239
+v 0.002473 -0.377625 -0.016783
+vn -0.900442 0.431364 0.055939
+v 0.000490 -0.381285 -0.014630
+vn -0.910514 0.390377 0.136269
+v -0.000216 -0.383026 -0.013563
+vn -0.917498 0.354808 0.179747
+v 0.000418 -0.382113 -0.012013
+vn -0.822156 0.524287 -0.221774
+v 0.003526 -0.376161 -0.017736
+vn -0.619947 0.622339 -0.477869
+v 0.004519 -0.376433 -0.020293
+vn -0.914526 0.355197 0.193590
+v -0.001456 -0.385817 -0.014048
+vn -0.894266 0.434461 0.107383
+v -0.002051 -0.385553 -0.018490
+vn -0.753085 0.546057 -0.366994
+v 0.004708 -0.374701 -0.018332
+vn -0.660294 0.577376 -0.480259
+v 0.005203 -0.374825 -0.019262
+vn -0.520428 0.590644 -0.616681
+v 0.005803 -0.374815 -0.019902
+vn 0.574268 0.811253 0.109935
+v 0.029646 -0.378231 0.011772
+vn -0.417207 0.597774 -0.684547
+v 0.005561 -0.375982 -0.020750
+vn -0.220533 0.553041 -0.803437
+v 0.006585 -0.375707 -0.020960
+vn -0.143447 0.547811 -0.824212
+v 0.006156 -0.377462 -0.022000
+vn -0.108184 0.518023 -0.848498
+v 0.007742 -0.375543 -0.021067
+vn -0.164352 0.599813 -0.783079
+v 0.007613 -0.374141 -0.020125
+vn -0.310831 0.595201 -0.741026
+v 0.006618 -0.374403 -0.020040
+vn -0.027255 0.456285 -0.889416
+v 0.010807 -0.374768 -0.020883
+vn -0.058588 0.484361 -0.872904
+v 0.009184 -0.375322 -0.021074
+vn 0.019919 0.441362 -0.897108
+v 0.009449 -0.377382 -0.022109
+vn -0.479652 0.623517 -0.617382
+v 0.006267 -0.373775 -0.019280
+vn -0.295550 0.602961 -0.741005
+v 0.005054 -0.377625 -0.021836
+vn -0.504607 0.663678 -0.552181
+v 0.003786 -0.378180 -0.021622
+vn -0.040662 0.499443 -0.865392
+v 0.007473 -0.377459 -0.022123
+vn -0.112250 0.598058 -0.793553
+v 0.008859 -0.373915 -0.020160
+vn -0.098436 0.593214 -0.799004
+v 0.010246 -0.373669 -0.020163
+vn -0.620013 0.602949 -0.502033
+v 0.005741 -0.373590 -0.018561
+vn 0.225547 0.575481 -0.786098
+v 0.014147 -0.373678 -0.020079
+vn 0.450941 0.289599 -0.844266
+v 0.017635 -0.377171 -0.020540
+vn 0.393822 0.333779 -0.856444
+v 0.016053 -0.380035 -0.022337
+vn 0.092067 0.390570 -0.915958
+v 0.013104 -0.377547 -0.021987
+vn 0.239125 0.387844 -0.890166
+v 0.014849 -0.379472 -0.022527
+vn 0.229622 0.452384 -0.861755
+v 0.017323 -0.374935 -0.019827
+vn 0.193353 0.385198 -0.902351
+v 0.014612 -0.376948 -0.021538
+vn 0.719506 0.567434 0.400413
+v 0.028570 -0.379616 0.018425
+vn 0.677218 0.669319 0.305593
+v 0.029193 -0.379170 0.016406
+vn 0.423965 0.887920 0.178467
+v 0.027696 -0.377967 0.016035
+vn 0.340958 0.933330 0.112439
+v 0.027961 -0.377593 0.012997
+vn 0.357302 0.928636 0.099855
+v 0.028650 -0.377498 0.009901
+vn 0.286574 0.954013 0.087951
+v 0.028070 -0.376979 0.006821
+vn 0.575575 0.229283 -0.784948
+v 0.019320 -0.377448 -0.019526
+vn 0.538354 0.430755 -0.724310
+v 0.018625 -0.374131 -0.018846
+vn 0.466688 0.743948 -0.478272
+v 0.018936 -0.373709 -0.018213
+vn 0.625912 0.624714 0.466869
+v 0.027252 -0.379105 0.019728
+vn 0.544926 0.784363 0.296360
+v 0.027496 -0.378465 0.018203
+vn 0.261677 0.962221 0.075207
+v 0.025457 -0.376510 0.009475
+vn 0.631398 0.387018 -0.671977
+v 0.017316 -0.389074 -0.025684
+vn 0.673648 0.327275 -0.662638
+v 0.019545 -0.389928 -0.023965
+vn 0.656121 0.337004 -0.675228
+v 0.017165 -0.390656 -0.026714
+vn 0.662513 0.156424 -0.732536
+v 0.020008 -0.380930 -0.019768
+vn 0.686789 0.301256 -0.661487
+v 0.022530 -0.386810 -0.019417
+vn 0.645803 0.325246 -0.690763
+v 0.019984 -0.386130 -0.021570
+vn 0.688364 0.261456 -0.676606
+v 0.022402 -0.384122 -0.018403
+vn 0.654239 0.348727 -0.671089
+v 0.019838 -0.388078 -0.022706
+vn 0.689163 0.302556 -0.658418
+v 0.022039 -0.389265 -0.021084
+vn 0.696395 0.166992 -0.697960
+v 0.021216 -0.381386 -0.018727
+vn 0.728672 0.116136 -0.674944
+v 0.021888 -0.379212 -0.017633
+vn 0.726603 0.135595 -0.673544
+v 0.022126 -0.377444 -0.017082
+vn 0.643027 0.216306 -0.734662
+v 0.019244 -0.382795 -0.020898
+vn 0.651993 0.265585 -0.710189
+v 0.020051 -0.383914 -0.020558
+vn 0.603273 0.364675 -0.709277
+v 0.017583 -0.386020 -0.023640
+vn 0.566768 0.446492 -0.692401
+v 0.015781 -0.387426 -0.025994
+vn 0.613952 0.396613 -0.682467
+v 0.017569 -0.387530 -0.024520
+vn 0.684424 0.235204 -0.690104
+v 0.022121 -0.376090 -0.016755
+vn 0.675657 0.138235 -0.724140
+v 0.020810 -0.379069 -0.018691
+vn 0.613576 0.185411 -0.767560
+v 0.019314 -0.379755 -0.020115
+vn 0.605609 0.222471 -0.764032
+v 0.018093 -0.382169 -0.021691
+vn 0.779285 0.189592 -0.597302
+v 0.024659 -0.379186 -0.014284
+vn 0.752323 0.205968 -0.625769
+v 0.024405 -0.381591 -0.015340
+vn 0.774861 0.416826 -0.475232
+v 0.023561 -0.375111 -0.014575
+vn 0.746841 0.242846 -0.619075
+v 0.024412 -0.384177 -0.016246
+vn 0.642453 0.188413 -0.742802
+v 0.020782 -0.377633 -0.018414
+vn 0.716803 0.195274 -0.669374
+v 0.022766 -0.381586 -0.017180
+vn 0.750829 0.269030 -0.603223
+v 0.024816 -0.386788 -0.016874
+vn 0.719891 0.285802 -0.632514
+v 0.023872 -0.388584 -0.018810
+vn 0.780502 0.175366 -0.600053
+v 0.023177 -0.375951 -0.015516
+vn 0.765577 0.141928 -0.627494
+v 0.023284 -0.379162 -0.016014
+vn 0.826944 0.218765 -0.517983
+v 0.024765 -0.376890 -0.013348
+vn 0.797228 0.120953 -0.591438
+v 0.023404 -0.377225 -0.015501
+vn 0.842526 0.248811 -0.477747
+v 0.024104 -0.375679 -0.013989
+vn 0.685386 0.134869 -0.715581
+v 0.017129 -0.398329 -0.029226
+vn 0.691694 0.201263 -0.693579
+v 0.017977 -0.395471 -0.027698
+vn 0.710100 -0.076963 -0.699882
+v 0.022335 -0.404429 -0.024321
+vn 0.727555 0.218601 -0.650291
+v 0.023138 -0.394573 -0.022014
+vn 0.702420 0.233587 -0.672341
+v 0.020131 -0.393571 -0.024901
+vn 0.721763 0.148108 -0.676108
+v 0.022087 -0.398910 -0.024394
+vn 0.752573 0.106426 -0.649852
+v 0.024403 -0.400203 -0.022093
+vn 0.737051 -0.001117 -0.675836
+v 0.023731 -0.402288 -0.023007
+vn 0.713277 0.194297 -0.673413
+v 0.021254 -0.396326 -0.024606
+vn 0.748265 0.173163 -0.640402
+v 0.024447 -0.397572 -0.021429
+vn 0.845615 0.098042 -0.524713
+v 0.028371 -0.401459 -0.017005
+vn 0.799288 0.036769 -0.599822
+v 0.027650 -0.403661 -0.018379
+vn 0.791232 0.120323 -0.599562
+v 0.026596 -0.399692 -0.019308
+vn 0.805401 0.167725 -0.568504
+v 0.026982 -0.397705 -0.018275
+vn 0.771921 0.197483 -0.604267
+v 0.025758 -0.395452 -0.019225
+vn 0.813844 0.223747 -0.536279
+v 0.026860 -0.393864 -0.017138
+vn 0.782238 0.052627 -0.620753
+v 0.026222 -0.401865 -0.020106
+vn 0.805567 0.242830 -0.540458
+v 0.026159 -0.391490 -0.017129
+vn 0.749383 0.237270 -0.618165
+v 0.024771 -0.392927 -0.019544
+vn 0.708854 0.265399 -0.653520
+v 0.021903 -0.391699 -0.022291
+vn 0.705209 -0.027217 -0.708477
+v 0.021018 -0.402425 -0.025807
+vn 0.688194 0.285103 -0.667163
+v 0.019162 -0.391520 -0.025120
+vn 0.735930 0.259288 -0.625442
+v 0.024135 -0.390535 -0.019343
+vn 0.722863 0.060134 -0.688370
+v 0.022349 -0.400836 -0.024437
+vn 0.698970 0.155486 -0.698043
+v 0.019440 -0.398171 -0.026948
+vn 0.682149 0.253930 -0.685706
+v 0.017403 -0.392574 -0.027318
+vn 0.687761 0.076908 -0.721852
+v 0.018237 -0.399972 -0.028431
+vn 0.745610 -0.034472 -0.665491
+v 0.025271 -0.404069 -0.021254
+vn 0.702791 0.070182 -0.707926
+v 0.020204 -0.400388 -0.026555
+vn 0.717687 -0.109509 -0.687701
+v 0.025263 -0.408458 -0.020740
+vn 0.712885 -0.117800 -0.691316
+v 0.026580 -0.410592 -0.019030
+vn 0.681656 -0.164911 -0.712846
+v 0.023337 -0.410966 -0.022171
+vn 0.712692 -0.101354 -0.694116
+v 0.023825 -0.406447 -0.022537
+vn 0.652893 -0.206731 -0.728693
+v 0.020129 -0.411103 -0.025084
+vn 0.674211 -0.288070 -0.680040
+v 0.025384 -0.414780 -0.019046
+vn 0.643421 -0.233689 -0.728971
+v 0.021248 -0.413241 -0.023469
+vn 0.757756 -0.034570 -0.651621
+v 0.028386 -0.407761 -0.017373
+vn 0.728100 -0.056167 -0.683166
+v 0.029776 -0.410393 -0.015704
+vn 0.649467 -0.286681 -0.704277
+v 0.022635 -0.415083 -0.021576
+vn 0.665740 -0.310617 -0.678459
+v 0.027620 -0.414988 -0.016707
+vn 0.679550 -0.204065 -0.704676
+v 0.024500 -0.412984 -0.020554
+vn 0.774752 0.077260 -0.627527
+v 0.030764 -0.408049 -0.014504
+vn 0.687662 -0.131971 -0.713936
+v 0.020640 -0.406767 -0.025629
+vn 0.685185 -0.150137 -0.712728
+v 0.022059 -0.408897 -0.023835
+vn 0.784315 -0.196371 -0.588463
+v 0.022011 -0.421510 -0.018673
+vn 0.716029 -0.365340 -0.594836
+v 0.022423 -0.419981 -0.018907
+vn 0.744995 -0.332969 -0.578026
+v 0.022789 -0.420853 -0.017917
+vn 0.751608 -0.054906 -0.657321
+v 0.026693 -0.405918 -0.019518
+vn 0.658140 -0.287363 -0.695898
+v 0.019481 -0.418475 -0.022952
+vn 0.687694 -0.360678 -0.630070
+v 0.021814 -0.418712 -0.020404
+vn 0.575269 -0.279550 -0.768711
+v 0.015654 -0.415184 -0.027337
+vn 0.550409 -0.300092 -0.779098
+v 0.012522 -0.410773 -0.031345
+vn 0.558052 -0.307920 -0.770560
+v 0.014159 -0.412920 -0.029294
+vn 0.607687 -0.267698 -0.747700
+v 0.017611 -0.412934 -0.026639
+vn 0.589142 -0.273697 -0.760264
+v 0.015195 -0.410829 -0.029343
+vn 0.648808 -0.201858 -0.733691
+v 0.017484 -0.420614 -0.024085
+vn 0.531774 -0.270745 -0.802442
+v 0.013476 -0.416869 -0.028274
+vn 0.600460 -0.256373 -0.757444
+v 0.016605 -0.417816 -0.025701
+vn 0.620175 -0.279842 -0.732852
+v 0.019268 -0.415757 -0.024259
+vn 0.663367 -0.181234 -0.726015
+v 0.018963 -0.409001 -0.026707
+vn 0.629278 -0.208924 -0.748572
+v 0.016198 -0.408711 -0.029232
+vn 0.480725 -0.333985 -0.810776
+v 0.010401 -0.410852 -0.032718
+vn 0.496786 -0.353075 -0.792807
+v 0.011343 -0.412664 -0.031313
+vn 0.432996 -0.367820 -0.822936
+v 0.009624 -0.414028 -0.031676
+vn 0.511814 -0.319163 -0.797610
+v 0.012504 -0.414616 -0.029727
+vn 0.630519 -0.237249 -0.739025
+v 0.017566 -0.410706 -0.027457
+vn 0.554334 -0.222749 -0.801933
+v 0.014144 -0.419788 -0.026933
+vn 0.470325 -0.272938 -0.839225
+v 0.011121 -0.418145 -0.029288
+vn -0.038617 0.199369 0.979163
+v -0.008310 -0.417900 -0.010103
+vn -0.067179 0.172016 0.982801
+v -0.006927 -0.418290 -0.009990
+vn -0.153939 0.124363 0.980223
+v -0.011869 -0.412857 -0.011600
+vn -0.106769 0.049231 0.993064
+v -0.012177 -0.410590 -0.011835
+vn -0.103046 0.066334 0.992462
+v -0.013542 -0.412576 -0.011851
+vn 0.010395 -0.008244 0.999912
+v -0.013528 -0.407371 -0.011915
+vn -0.055451 0.011632 0.998394
+v -0.012478 -0.408474 -0.011910
+vn 0.056209 -0.018174 0.998254
+v -0.014528 -0.405632 -0.011846
+vn 0.013544 -0.028966 0.999489
+v -0.014985 -0.408610 -0.011911
+vn 0.034004 -0.040961 0.998582
+v -0.015723 -0.407119 -0.011832
+vn -0.160553 0.166683 0.972851
+v -0.010271 -0.413846 -0.011164
+vn -0.165251 0.084090 0.982660
+v -0.010368 -0.411124 -0.011545
+vn 0.026409 0.012580 0.999572
+v -0.013000 -0.404336 -0.011923
+vn -0.106705 0.191902 0.975596
+v -0.009037 -0.415744 -0.010607
+vn -0.052373 -0.057516 0.996970
+v -0.016354 -0.410259 -0.012006
+vn -0.036761 0.003964 0.999316
+v -0.013998 -0.410354 -0.011946
+vn -0.128020 0.235044 0.963517
+v -0.010411 -0.415758 -0.010755
+vn 0.098752 -0.956921 0.273039
+v 0.015592 -0.422013 -0.001413
+vn -0.584710 -0.199171 0.786413
+v -0.019670 -0.414553 0.029930
+vn -0.532211 -0.054871 0.844832
+v -0.001386 -0.413812 -0.008613
+vn -0.380101 0.024863 0.924611
+v -0.002660 -0.416126 -0.009281
+vn -0.103563 0.156612 0.982216
+v -0.007522 -0.416230 -0.010386
+vn -0.164425 0.124450 0.978507
+v -0.005948 -0.417145 -0.010075
+vn -0.198347 0.096010 0.975418
+v -0.006514 -0.414220 -0.010517
+vn -0.382093 0.045712 0.922993
+v -0.001637 -0.418899 -0.008718
+vn -0.533450 -0.033363 0.845173
+v -0.000680 -0.416492 -0.008296
+vn -0.257276 0.098673 0.961287
+v -0.004108 -0.418133 -0.009592
+vn -0.244216 0.087935 0.965726
+v -0.003525 -0.420343 -0.009181
+vn -0.549042 -0.056114 0.833909
+v 0.000489 -0.418259 -0.007625
+vn -0.702363 -0.148665 0.696122
+v 0.000953 -0.415987 -0.006945
+vn -0.613328 -0.133431 0.778476
+v 0.001928 -0.418782 -0.006624
+vn -0.637673 -0.318243 0.701494
+v 0.004092 -0.418284 -0.004656
+vn -0.688139 -0.418265 0.592891
+v 0.004660 -0.417535 -0.003659
+vn -0.787569 -0.410788 0.459334
+v 0.003344 -0.415223 -0.003550
+vn -0.875698 -0.481302 0.038736
+v 0.003071 -0.413202 -0.001250
+vn -0.701901 -0.286346 0.652181
+v 0.002882 -0.417186 -0.005306
+vn -0.895181 -0.323753 0.306325
+v 0.000946 -0.409933 -0.004255
+vn -0.872431 -0.343978 0.347194
+v 0.000143 -0.408455 -0.005066
+vn -0.826418 -0.291964 0.481446
+v -0.000509 -0.408722 -0.006580
+vn -0.720057 -0.149003 0.677729
+v -0.000518 -0.411493 -0.007633
+vn -0.683279 -0.388957 0.617934
+v 0.003897 -0.417127 -0.004225
+vn -0.760685 -0.508197 0.403849
+v 0.004241 -0.415789 -0.002676
+vn -0.906449 -0.408093 0.108673
+v 0.001815 -0.410622 -0.002023
+vn -0.879171 -0.324308 0.349116
+v 0.001840 -0.411823 -0.003736
+vn -0.708510 -0.124107 0.694702
+v 0.000124 -0.413911 -0.007412
+vn -0.679327 -0.192967 0.708010
+v 0.001793 -0.417348 -0.006457
+vn -0.824856 -0.229617 0.516612
+v 0.001252 -0.413119 -0.005734
+vn -0.795910 -0.281826 0.535818
+v 0.002247 -0.414951 -0.005025
+vn -0.836889 -0.227850 0.497696
+v 0.000431 -0.411082 -0.006222
+vn -0.651073 -0.414595 0.635779
+v 0.005414 -0.418642 -0.003534
+vn -0.493698 -0.117622 -0.861642
+v -0.004081 -0.407419 0.004217
+vn -0.584706 -0.206233 -0.784594
+v -0.000608 -0.410063 0.002550
+vn -0.432700 -0.062992 -0.899335
+v -0.003742 -0.409490 0.004235
+vn -0.718370 -0.659107 -0.222536
+v 0.005012 -0.416078 0.000341
+vn -0.575171 -0.621906 -0.531424
+v 0.003416 -0.415271 0.001818
+vn -0.509563 -0.681948 -0.524684
+v 0.003898 -0.416063 0.002353
+vn -0.714920 -0.486602 0.502103
+v 0.005753 -0.417745 -0.002437
+vn -0.371582 -0.565851 -0.736030
+v 0.002704 -0.416078 0.003217
+vn -0.749950 -0.590275 0.298579
+v 0.005314 -0.416543 -0.001699
+vn -0.894270 -0.427063 0.133782
+v 0.001150 -0.409355 -0.002376
+vn -0.864582 -0.479899 -0.148977
+v 0.000868 -0.408829 -0.000717
+vn -0.883440 -0.377345 0.277748
+v 0.002557 -0.412758 -0.002718
+vn -0.721464 -0.521529 0.455519
+v 0.005062 -0.416820 -0.002502
+vn -0.802383 -0.544100 0.245229
+v 0.006139 -0.417634 -0.001507
+vn -0.897038 -0.424261 -0.123794
+v 0.001828 -0.410689 -0.000758
+vn -0.805716 -0.435916 -0.400997
+v 0.000249 -0.408606 0.000849
+vn -0.534341 -0.213271 -0.817921
+v -0.000122 -0.411869 0.002644
+vn -0.646250 -0.273521 -0.712424
+v -0.001110 -0.408303 0.002402
+vn -0.803793 -0.371699 -0.464497
+v 0.001226 -0.410544 0.000838
+vn -0.688007 -0.508138 -0.518113
+v 0.003178 -0.414343 0.000982
+vn -0.759264 -0.649403 -0.042354
+v 0.005099 -0.416042 -0.000697
+vn -0.770690 -0.412776 -0.485441
+v 0.002206 -0.412527 0.000818
+vn -0.818483 -0.574490 -0.006786
+v 0.004277 -0.415022 -0.000995
+vn -0.715507 -0.433082 -0.548170
+v -0.001452 -0.407010 0.002070
+vn -0.887316 -0.458292 0.051359
+v 0.000340 -0.407779 -0.002478
+vn -0.785095 -0.522353 -0.332826
+v -0.000798 -0.406808 0.000633
+vn -0.853375 -0.509228 -0.111529
+v 0.000251 -0.407658 -0.001113
+vn -0.334908 0.080845 -0.938776
+v -0.011293 -0.406797 0.007350
+vn -0.337348 0.014611 -0.941266
+v -0.006952 -0.410976 0.005484
+vn -0.324277 0.070838 -0.943306
+v -0.010292 -0.409565 0.006747
+vn -0.329225 -0.105310 -0.938361
+v -0.005728 -0.412375 0.005090
+vn -0.319144 -0.018370 -0.947528
+v -0.008464 -0.411774 0.006008
+vn -0.357548 0.035364 -0.933225
+v -0.007440 -0.409141 0.005754
+vn -0.394882 0.010161 -0.918676
+v -0.007462 -0.407130 0.005834
+vn -0.217986 -0.306903 -0.926441
+v -0.006604 -0.416201 0.005863
+vn -0.355362 -0.110689 -0.928152
+v -0.003907 -0.412243 0.004442
+vn -0.350128 -0.287879 -0.891367
+v -0.002397 -0.413615 0.004149
+vn -0.415642 -0.477013 -0.774403
+v 0.001389 -0.414973 0.003098
+vn -0.389269 -0.066912 -0.918691
+v -0.003682 -0.411172 0.004290
+vn -0.473091 -0.327060 -0.818057
+v -0.000088 -0.413360 0.003046
+vn -0.165087 -0.277416 -0.946460
+v -0.005053 -0.416540 0.005653
+vn -0.299719 -0.193186 -0.934263
+v -0.005131 -0.413646 0.005094
+vn -0.192123 -0.112260 -0.974929
+v -0.005900 -0.415569 0.005618
+vn -0.283408 0.070798 -0.956383
+v -0.011504 -0.410920 0.007033
+vn -0.393246 0.172792 -0.903051
+v -0.013234 -0.399641 0.008910
+vn -0.334746 0.135829 -0.932467
+v -0.014275 -0.401266 0.009050
+vn -0.334091 0.104140 -0.936770
+v -0.013280 -0.403771 0.008376
+vn -0.226995 0.143369 -0.963285
+v -0.015254 -0.402786 0.009108
+vn -0.393183 0.040180 -0.918582
+v -0.010282 -0.404700 0.007108
+vn -0.423679 0.066051 -0.903401
+v -0.011873 -0.402012 0.007957
+vn -0.254565 0.211125 0.943728
+v -0.018986 -0.407160 0.029907
+vn -0.268885 0.251097 0.929866
+v -0.018396 -0.405217 0.029584
+vn -0.253485 0.180358 0.950377
+v -0.019581 -0.408495 0.030035
+vn -0.770786 -0.406125 -0.490869
+v -0.020418 -0.413392 0.012501
+vn -0.302754 0.329701 0.894225
+v -0.018339 -0.402078 0.028578
+vn -0.321060 0.267281 0.908560
+v -0.017456 -0.403955 0.029508
+vn -0.248770 0.299471 0.921103
+v -0.019175 -0.403543 0.028870
+vn -0.416404 0.294792 0.860061
+v -0.017119 -0.401408 0.028810
+vn -0.545598 0.247797 0.800574
+v -0.016043 -0.400124 0.029066
+vn -0.571567 0.165200 0.803753
+v -0.014727 -0.401176 0.030270
+vn -0.448451 0.306510 0.839609
+v -0.017465 -0.400140 0.028184
+vn -0.481314 0.347010 0.804935
+v -0.017289 -0.399161 0.027908
+vn -0.328716 0.364883 0.871095
+v -0.018548 -0.400490 0.027845
+vn -0.563836 0.150677 0.812025
+v -0.012934 -0.401130 0.031565
+vn -0.574680 0.222459 0.787563
+v -0.014311 -0.400293 0.030378
+vn -0.524672 0.180279 0.831997
+v -0.015335 -0.402150 0.030047
+vn -0.557290 0.102996 0.823905
+v -0.013222 -0.402374 0.031536
+vn -0.520495 0.228838 0.822629
+v -0.016222 -0.400958 0.029174
+vn -0.470122 0.453995 0.756884
+v -0.016801 -0.398208 0.027730
+vn -0.498918 0.527771 0.687415
+v -0.013850 -0.397703 0.029413
+vn -0.519974 0.433326 0.736108
+v -0.014760 -0.398540 0.029315
+vn -0.549996 0.320228 0.771336
+v -0.013657 -0.399552 0.030609
+vn -0.421013 0.239516 0.874860
+v -0.016351 -0.403033 0.029693
+vn -0.617793 -0.237281 0.749686
+v -0.020893 -0.412998 0.029396
+vn -0.541902 0.329482 0.773165
+v -0.015566 -0.399300 0.029113
+vn -0.488800 0.253224 0.834837
+v -0.007676 -0.400676 0.034700
+vn -0.489018 0.332424 0.806447
+v -0.008134 -0.399083 0.033867
+vn -0.421918 0.240315 0.874204
+v -0.005248 -0.399270 0.035642
+vn -0.489742 0.278350 0.826241
+v -0.006774 -0.399598 0.034883
+vn -0.452955 0.250946 0.855487
+v -0.006011 -0.400201 0.035508
+vn -0.512037 0.224911 0.828995
+v -0.011028 -0.400641 0.032698
+vn -0.519599 0.098532 0.848710
+v -0.009795 -0.401983 0.033748
+vn -0.505620 0.212329 0.836221
+v -0.009543 -0.401163 0.033735
+vn -0.493421 0.383476 0.780693
+v -0.011000 -0.398734 0.032030
+vn -0.490141 0.274790 0.827195
+v -0.008819 -0.400245 0.033885
+vn -0.484112 0.306723 0.819486
+v -0.009944 -0.399591 0.033006
+vn -0.471323 0.394557 0.788784
+v -0.009379 -0.398356 0.032814
+vn -0.285134 0.241956 0.927446
+v -0.004669 -0.399778 0.036005
+vn -0.163548 0.229504 0.959469
+v -0.004156 -0.400395 0.036309
+vn -0.423831 0.176823 0.888313
+v -0.005664 -0.401357 0.035993
+vn -0.220438 0.195752 0.955557
+v -0.004527 -0.401171 0.036390
+vn -0.381697 0.231831 0.894741
+v -0.005265 -0.400620 0.035998
+vn -0.522970 0.405164 0.749896
+v -0.012817 -0.398758 0.030837
+vn -0.547664 0.227429 0.805196
+v -0.012734 -0.400338 0.031535
+vn -0.513318 0.305071 0.802145
+v -0.011806 -0.399707 0.031933
+vn -0.470379 0.218838 0.854900
+v -0.006631 -0.400999 0.035399
+vn -0.498025 0.173736 0.849580
+v -0.008189 -0.401458 0.034616
+vn -0.501964 0.036061 0.864136
+v -0.008319 -0.402313 0.034656
+vn -0.323251 -0.144475 0.935220
+v -0.004730 -0.405040 0.036265
+vn -0.238461 -0.069087 0.968692
+v -0.004265 -0.403411 0.036598
+vn -0.415098 -0.085985 0.905704
+v -0.005659 -0.403624 0.036057
+vn -0.106113 -0.352005 0.929964
+v -0.003310 -0.409831 0.035401
+vn -0.214039 -0.202058 0.955698
+v -0.003876 -0.406478 0.036252
+vn -0.430433 -0.155670 0.889098
+v -0.006118 -0.405446 0.035601
+vn -0.446637 0.029210 0.894238
+v -0.005798 -0.402227 0.036042
+vn -0.339670 -0.020637 0.940318
+v -0.004912 -0.402549 0.036450
+vn -0.480898 0.107077 0.870213
+v -0.006912 -0.401780 0.035407
+vn -0.478524 -0.026330 0.877679
+v -0.006982 -0.402859 0.035424
+vn -0.512496 -0.023225 0.858376
+v -0.009732 -0.402970 0.033826
+vn -0.501105 -0.080714 0.861614
+v -0.008381 -0.403574 0.034584
+vn -0.134260 0.090772 0.986780
+v -0.004107 -0.402097 0.036629
+vn -0.336724 0.125496 0.933203
+v -0.004967 -0.401793 0.036384
+vn -0.024030 0.036682 0.999038
+v -0.003029 -0.402905 0.036766
+vn 0.085812 -0.132281 0.987491
+v -0.001035 -0.405561 0.036544
+vn 0.128072 -0.217596 0.967600
+v -0.000236 -0.406772 0.036243
+vn 0.029386 0.193157 0.980728
+v -0.003099 -0.401175 0.036546
+vn 0.202588 -0.201614 0.958285
+v 0.001112 -0.406835 0.035996
+vn 0.195303 -0.083131 0.977213
+v 0.000477 -0.405333 0.036358
+vn 0.291848 0.074351 0.953571
+v 0.001409 -0.403793 0.036149
+vn 0.173626 0.026740 0.984449
+v -0.000337 -0.403825 0.036568
+vn 0.260476 0.195733 0.945431
+v 0.000460 -0.402121 0.036195
+vn 0.127355 0.138052 0.982203
+v -0.001500 -0.402348 0.036631
+vn 0.141686 -0.311734 0.939546
+v 0.000107 -0.408486 0.035701
+vn 0.048638 -0.257189 0.965136
+v -0.001497 -0.407443 0.036183
+vn -0.035705 -0.180491 0.982928
+v -0.002490 -0.406022 0.036514
+vn -0.080726 -0.268972 0.959759
+v -0.002972 -0.407996 0.036023
+vn -0.129438 -0.113544 0.985065
+v -0.003377 -0.404619 0.036655
+vn 0.044135 -0.044582 0.998030
+v -0.001954 -0.404152 0.036745
+vn 0.427391 -0.337167 0.838842
+v 0.005510 -0.408740 0.033882
+vn 0.454126 -0.304477 0.837296
+v 0.006885 -0.408786 0.033107
+vn 0.483572 -0.173481 0.857941
+v 0.006557 -0.407751 0.033617
+vn 0.316971 -0.355692 0.879211
+v 0.002978 -0.413807 0.032688
+vn 0.281700 -0.372430 0.884274
+v 0.001849 -0.412675 0.033550
+vn 0.332618 -0.393483 0.857051
+v 0.003461 -0.411209 0.033613
+vn 0.394179 -0.364595 0.843619
+v 0.006037 -0.410262 0.032909
+vn 0.382575 -0.402580 0.831604
+v 0.004840 -0.409786 0.033711
+vn 0.412109 -0.257725 0.873925
+v 0.004717 -0.408212 0.034458
+vn 0.342018 -0.396560 0.851918
+v 0.003801 -0.409290 0.034423
+vn 0.217592 -0.387899 0.895650
+v 0.000784 -0.411700 0.034274
+vn 0.285146 -0.410296 0.866227
+v 0.002451 -0.410461 0.034348
+vn 0.467014 -0.097718 0.878834
+v 0.005410 -0.407013 0.034361
+vn 0.445131 -0.046934 0.894235
+v 0.004259 -0.406194 0.035023
+vn 0.310795 -0.327719 0.892192
+v 0.003170 -0.408621 0.034964
+vn 0.162379 -0.377989 0.911459
+v 0.000125 -0.410385 0.034987
+vn 0.315571 -0.317550 0.894190
+v 0.007056 -0.414784 0.030749
+vn 0.353792 -0.361416 0.862676
+v 0.004554 -0.412012 0.032805
+vn 0.229210 -0.277627 0.932945
+v 0.002012 -0.408053 0.035485
+vn 0.311248 -0.151438 0.938185
+v 0.002673 -0.406638 0.035637
+vn 0.367064 -0.194252 0.909687
+v 0.003653 -0.407555 0.035099
+vn 0.226102 -0.375420 0.898854
+v 0.001861 -0.409398 0.035021
+vn 0.053338 -0.334161 0.941006
+v -0.001726 -0.409434 0.035578
+vn 0.477715 -0.114650 0.871002
+v 0.008736 -0.407574 0.032376
+vn 0.410342 -0.273012 0.870106
+v 0.008270 -0.409360 0.032177
+vn 0.346144 -0.334112 0.876672
+v 0.005633 -0.413275 0.031846
+vn 0.367132 -0.327761 0.870510
+v 0.007070 -0.411188 0.032049
+vn 0.330242 -0.259946 0.907396
+v 0.009333 -0.410707 0.031307
+vn 0.200497 -0.223202 0.953930
+v 0.014152 -0.412841 0.029488
+vn 0.124543 -0.249111 0.960434
+v 0.020063 -0.416045 0.027648
+vn 0.103090 -0.324047 0.940407
+v 0.017707 -0.417353 0.027513
+vn 0.208583 -0.302105 0.930175
+v 0.011522 -0.414714 0.029512
+vn 0.257323 -0.327796 0.909029
+v 0.009199 -0.415942 0.029662
+vn 0.166778 -0.147461 0.974905
+v 0.021786 -0.411392 0.028338
+vn 0.307065 0.000982 0.951688
+v 0.013274 -0.407677 0.030264
+vn 0.244042 -0.099368 0.964660
+v 0.013945 -0.410156 0.029969
+vn 0.282321 -0.179594 0.942359
+v 0.011675 -0.410660 0.030552
+vn 0.339055 -0.075024 0.937770
+v 0.012128 -0.408886 0.030636
+vn 0.158930 -0.106933 0.981482
+v 0.021026 -0.409483 0.028732
+vn 0.183092 -0.131909 0.974206
+v 0.017207 -0.411294 0.029150
+vn 0.160152 -0.209426 0.964620
+v 0.019601 -0.413862 0.028253
+vn 0.144615 -0.374389 0.915926
+v 0.014540 -0.417652 0.027790
+vn 0.317954 -0.344406 0.883340
+v 0.004285 -0.415658 0.031464
+vn 0.316409 -0.305701 0.898016
+v 0.008008 -0.412647 0.031152
+vn 0.250359 -0.264294 0.931380
+v 0.010451 -0.412569 0.030433
+vn 0.263558 -0.302955 0.915836
+v 0.009029 -0.414115 0.030332
+vn 0.197976 -0.359922 0.911735
+v 0.012002 -0.416742 0.028661
+vn 0.131443 -0.158490 0.978572
+v 0.025753 -0.414732 0.027150
+vn 0.124607 -0.137453 0.982639
+v 0.024638 -0.415889 0.027123
+vn 0.090465 -0.091469 0.991690
+v 0.023762 -0.416785 0.027140
+vn 0.070916 -0.232830 0.969928
+v 0.020533 -0.417327 0.027287
+vn 0.178771 -0.105812 0.978184
+v 0.026775 -0.411113 0.027479
+vn 0.143360 -0.081493 0.986310
+v 0.028002 -0.412261 0.027175
+vn 0.154269 -0.154158 0.975928
+v 0.024829 -0.413288 0.027519
+vn 0.137783 -0.188234 0.972411
+v 0.023148 -0.415159 0.027435
+vn 0.159104 -0.302012 0.939934
+v 0.015613 -0.415698 0.028386
+vn 0.093132 -0.354210 0.930517
+v 0.016041 -0.418701 0.027169
+vn 0.022654 -0.213390 0.976704
+v 0.020370 -0.418260 0.027099
+vn 0.100977 -0.164729 0.981156
+v 0.022661 -0.416456 0.027275
+vn 0.191731 -0.372044 0.908197
+v 0.012585 -0.419971 0.027164
+vn 0.047551 -0.283829 0.957695
+v 0.018473 -0.418515 0.027080
+vn 0.114728 -0.347154 0.930764
+v 0.014680 -0.419663 0.026934
+vn -0.444726 -0.086505 0.891480
+v -0.020625 -0.411009 0.029942
+vn -0.340886 -0.095901 0.935200
+v -0.017547 -0.414426 0.031023
+vn -0.255140 -0.094456 0.962279
+v -0.016544 -0.414861 0.031298
+vn -0.158038 -0.104818 0.981854
+v -0.015628 -0.415298 0.031449
+vn -0.189498 -0.182072 0.964853
+v -0.014981 -0.418027 0.031085
+vn -0.282667 -0.161838 0.945467
+v -0.016679 -0.415595 0.031158
+vn -0.415355 -0.155201 0.896322
+v -0.018172 -0.415140 0.030688
+vn -0.196948 -0.018665 0.980236
+v -0.016286 -0.414217 0.031390
+vn -0.072838 0.001508 0.997343
+v -0.014820 -0.413094 0.031610
+vn -0.052095 -0.078725 0.995534
+v -0.014520 -0.415290 0.031559
+vn -0.162576 -0.167314 0.972407
+v -0.015308 -0.416535 0.031307
+vn -0.055150 -0.105881 0.992848
+v -0.013470 -0.417327 0.031381
+vn 0.027156 -0.053035 0.998223
+v -0.011897 -0.417283 0.031400
+vn 0.031670 0.022353 0.999248
+v -0.009414 -0.418506 0.031205
+vn 0.092158 -0.004439 0.995735
+v -0.010311 -0.416824 0.031250
+vn 0.025083 0.020516 0.999475
+v -0.008937 -0.417221 0.031164
+vn -0.215899 0.043748 0.975435
+v -0.016610 -0.412893 0.031304
+vn -0.137906 -0.009808 0.990397
+v -0.015484 -0.413978 0.031534
+vn 0.010006 0.034379 0.999359
+v -0.009596 -0.419503 0.031245
+vn 0.078367 -0.011404 0.996859
+v -0.010790 -0.418430 0.031286
+vn 0.075542 -0.111836 0.990851
+v -0.010408 -0.412803 0.031339
+vn -0.483379 -0.231164 0.844339
+v -0.018473 -0.416311 0.030295
+vn -0.090520 -0.155098 0.983743
+v -0.011806 -0.409932 0.031823
+vn -0.009462 -0.188910 0.981949
+v -0.010839 -0.411428 0.031583
+vn -0.124535 -0.016693 0.992075
+v -0.013711 -0.409699 0.031685
+vn -0.034662 -0.074641 0.996608
+v -0.012650 -0.410285 0.031735
+vn 0.119680 -0.040215 0.991998
+v -0.011663 -0.412868 0.031506
+vn 0.125549 -0.030951 0.991605
+v -0.010441 -0.413699 0.031288
+vn 0.059941 -0.105628 0.992598
+v -0.011419 -0.411832 0.031549
+vn -0.042389 -0.013767 0.999006
+v -0.013652 -0.411775 0.031660
+vn 0.002535 -0.042066 0.999112
+v -0.013523 -0.414429 0.031600
+vn 0.065788 -0.032059 0.997318
+v -0.012422 -0.414496 0.031554
+vn 0.118046 -0.003891 0.993001
+v -0.010386 -0.415105 0.031266
+vn 0.103946 -0.021642 0.994347
+v -0.011185 -0.416768 0.031352
+vn 0.044113 -0.033520 0.998464
+v -0.012735 -0.412149 0.031646
+vn 0.127493 -0.020484 0.991628
+v -0.011530 -0.414653 0.031440
+vn -0.201923 -0.088275 0.975415
+v -0.012635 -0.408232 0.031923
+vn -0.344935 -0.241102 0.907133
+v -0.010560 -0.408180 0.032498
+vn -0.503064 -0.244096 0.829062
+v -0.007899 -0.407177 0.034196
+vn -0.484122 -0.309742 0.818343
+v -0.007056 -0.408267 0.034348
+vn -0.491549 -0.177159 0.852640
+v -0.007320 -0.405878 0.034874
+vn -0.427364 -0.388405 0.816396
+v -0.008757 -0.409428 0.032787
+vn -0.475010 -0.440338 0.761884
+v -0.007447 -0.410164 0.033148
+vn -0.450045 -0.270298 0.851116
+v -0.009212 -0.407729 0.033243
+vn -0.333310 -0.343473 0.878027
+v -0.009757 -0.409305 0.032407
+vn -0.202124 -0.338787 0.918896
+v -0.009645 -0.410643 0.031902
+vn -0.205680 -0.301897 0.930889
+v -0.010346 -0.409829 0.032046
+vn -0.450216 -0.187053 0.873108
+v -0.010049 -0.406114 0.033229
+vn -0.390348 -0.150418 0.908297
+v -0.011200 -0.406447 0.032610
+vn -0.493196 -0.113917 0.862427
+v -0.009722 -0.404347 0.033712
+vn -0.483486 -0.107787 0.868691
+v -0.007153 -0.404385 0.035209
+vn -0.499086 -0.168806 0.849952
+v -0.008577 -0.405506 0.034184
+vn -0.157596 -0.234769 0.959191
+v -0.010993 -0.409813 0.031941
+vn -0.336987 -0.353537 0.872612
+v -0.004524 -0.409795 0.035119
+vn -0.281452 -0.268863 0.921139
+v -0.004329 -0.408339 0.035666
+vn -0.435095 -0.305199 0.847081
+v -0.005703 -0.408677 0.034952
+vn -0.377338 -0.219276 0.899741
+v -0.005242 -0.406858 0.035708
+vn -0.359612 -0.367130 0.857843
+v -0.006772 -0.413423 0.031655
+vn -0.195932 -0.173964 0.965063
+v -0.007232 -0.414112 0.031303
+vn -0.465776 -0.470078 0.749719
+v -0.006674 -0.411607 0.032729
+vn 0.038215 0.005386 0.999255
+v -0.009029 -0.415413 0.031126
+vn -0.391504 -0.487824 0.780226
+v -0.004380 -0.411907 0.034026
+vn 0.022747 -0.060654 0.997900
+v -0.009304 -0.413973 0.031160
+vn -0.093674 -0.034839 0.994993
+v -0.007650 -0.415173 0.031154
+vn -0.250528 -0.092668 0.963664
+v -0.006217 -0.415209 0.031406
+vn -0.333528 -0.232703 0.913569
+v -0.006285 -0.414353 0.031514
+vn -0.488097 -0.353334 0.798071
+v -0.007966 -0.408709 0.033591
+vn -0.328767 -0.401339 0.854891
+v -0.009123 -0.410241 0.032220
+vn -0.084935 -0.259272 0.962062
+v -0.010196 -0.411122 0.031671
+vn -0.455772 -0.234707 0.858594
+v -0.006614 -0.407067 0.034982
+vn -0.393789 -0.426635 0.814195
+v -0.007382 -0.412148 0.032000
+vn -0.412544 -0.440879 0.797141
+v -0.008113 -0.410767 0.032398
+vn -0.286983 -0.380910 0.878947
+v -0.008647 -0.411395 0.031855
+vn -0.241276 -0.314710 0.918011
+v -0.007974 -0.412778 0.031497
+vn -0.015823 -0.183052 0.982976
+v -0.009760 -0.412592 0.031342
+vn -0.088508 -0.159050 0.983295
+v -0.008506 -0.413523 0.031224
+vn -0.133734 -0.278015 0.951222
+v -0.009160 -0.412046 0.031507
+vn -0.483075 -0.470459 0.738449
+v -0.005934 -0.411062 0.033593
+vn -0.484484 -0.396695 0.779685
+v -0.006624 -0.409579 0.034017
+vn -0.444431 -0.421485 0.790463
+v -0.005194 -0.410444 0.034454
+vn -0.025759 -0.170514 0.985019
+v -0.003062 -0.418885 0.031697
+vn 0.036332 -0.270939 0.961911
+v -0.001889 -0.418407 0.031807
+vn -0.027917 -0.394481 0.918480
+v -0.001285 -0.416277 0.032589
+vn 0.008864 -0.113364 0.993514
+v -0.003825 -0.419863 0.031550
+vn -0.109683 -0.073353 0.991256
+v -0.004225 -0.418648 0.031640
+vn -0.207887 -0.090683 0.973940
+v -0.004525 -0.417271 0.031700
+vn -0.132595 0.011002 0.991109
+v -0.005897 -0.418245 0.031418
+vn 0.143127 -0.395430 0.907276
+v -0.000098 -0.415996 0.032641
+vn -0.103687 -0.408443 0.906875
+v -0.001715 -0.415279 0.032991
+vn 0.074522 -0.408290 0.909805
+v -0.000727 -0.414734 0.033271
+vn -0.124841 -0.191541 0.973513
+v -0.003116 -0.417745 0.031897
+vn -0.075796 -0.320457 0.944226
+v -0.002023 -0.416998 0.032229
+vn 0.139353 -0.364190 0.920840
+v -0.000299 -0.417587 0.031968
+vn -0.078264 0.053325 0.995506
+v -0.006742 -0.419645 0.031397
+vn -0.057906 -0.007757 0.998292
+v -0.005257 -0.419896 0.031512
+vn 0.129071 -0.179115 0.975325
+v -0.003367 -0.420945 0.031367
+vn 0.098245 -0.228121 0.968663
+v -0.002467 -0.419830 0.031490
+vn -0.317264 -0.493980 0.809523
+v -0.003428 -0.413182 0.033655
+vn -0.456350 -0.490646 0.742302
+v -0.005062 -0.412480 0.033207
+vn -0.448854 -0.445294 0.774754
+v -0.005805 -0.413119 0.032330
+vn -0.390076 -0.237026 0.889752
+v -0.005319 -0.414902 0.031800
+vn -0.423993 -0.377346 0.823310
+v -0.004712 -0.414347 0.032312
+vn -0.430058 -0.361933 0.827076
+v -0.005801 -0.414039 0.031850
+vn 0.177361 -0.315276 0.932279
+v -0.000718 -0.419500 0.031346
+vn 0.257841 -0.372965 0.891300
+v 0.001422 -0.415212 0.032634
+vn -0.071840 0.049462 0.996189
+v -0.007523 -0.418500 0.031251
+vn -0.075757 0.022542 0.996871
+v -0.007455 -0.416919 0.031181
+vn 0.258581 -0.373333 0.890931
+v 0.001899 -0.416983 0.031740
+vn 0.042439 -0.390075 0.919805
+v -0.001779 -0.411352 0.034822
+vn -0.217735 -0.432687 0.874856
+v -0.003745 -0.411136 0.034746
+vn 0.126722 -0.387459 0.913136
+v -0.000661 -0.412817 0.034106
+vn -0.030235 -0.436108 0.899386
+v -0.001677 -0.413666 0.033789
+vn 0.207648 -0.379320 0.901665
+v 0.000433 -0.413896 0.033448
+vn -0.405505 -0.460333 0.789721
+v -0.004115 -0.413735 0.032958
+vn -0.125447 -0.445094 0.886654
+v -0.002713 -0.412513 0.034264
+vn -0.321959 -0.400213 0.858005
+v -0.003244 -0.414801 0.032801
+vn -0.222796 -0.353992 0.908324
+v -0.002568 -0.415822 0.032581
+vn -0.242280 -0.237211 0.940761
+v -0.003451 -0.416498 0.032109
+vn -0.193069 -0.026040 0.980840
+v -0.006041 -0.416514 0.031380
+vn -0.306823 -0.148973 0.940035
+v -0.004883 -0.415976 0.031755
+vn -0.214605 -0.455620 0.863919
+v -0.002491 -0.414260 0.033341
+vn -0.351139 -0.295576 0.888446
+v -0.004000 -0.415385 0.032248
+vn 0.163561 -0.380635 0.910145
+v 0.013574 -0.418941 0.027395
+vn 0.203001 -0.384617 0.900478
+v 0.012144 -0.418129 0.028031
+vn 0.249071 -0.410477 0.877196
+v 0.009761 -0.420309 0.027725
+vn 0.300787 -0.534873 0.789581
+v 0.004758 -0.422605 0.028038
+vn 0.258652 -0.358442 0.897005
+v 0.009714 -0.417799 0.028809
+vn 0.302230 -0.339838 0.890599
+v 0.006937 -0.417027 0.029988
+vn 0.287355 -0.384113 0.877431
+v 0.007393 -0.419188 0.028970
+vn 0.241811 -0.377052 0.894069
+v 0.010917 -0.419126 0.027922
+vn 0.274731 -0.349694 0.895677
+v -0.000621 -0.422060 0.030426
+vn 0.253669 -0.366069 0.895346
+v 0.001591 -0.418953 0.031002
+vn 0.298560 -0.364541 0.882027
+v 0.004311 -0.418129 0.030476
+vn -0.741005 -0.383431 0.551264
+v -0.022023 -0.414232 0.027626
+vn 0.318179 -0.464514 0.826432
+v 0.003466 -0.421863 0.029024
+vn 0.297813 -0.398605 0.867422
+v 0.003591 -0.420293 0.029778
+vn 0.308543 -0.403991 0.861158
+v 0.001419 -0.421758 0.029865
+vn 0.262523 -0.352784 0.898123
+v 0.000524 -0.420710 0.030606
+vn -0.539582 0.038678 0.841044
+v -0.011280 -0.402696 0.032868
+vn -0.506637 -0.049209 0.860754
+v -0.010926 -0.403843 0.033068
+vn -0.519970 0.028550 0.853707
+v -0.012411 -0.403809 0.032171
+vn -0.456597 -0.058943 0.887719
+v -0.011751 -0.405068 0.032513
+vn -0.447151 0.038928 0.893611
+v -0.013052 -0.405294 0.031848
+vn -0.496193 0.129144 0.858554
+v -0.014303 -0.403923 0.031019
+vn -0.540773 0.133545 0.830500
+v -0.011370 -0.401680 0.032711
+vn -0.260824 -0.166117 0.950987
+v -0.011651 -0.408160 0.032159
+vn -0.349391 -0.070153 0.934347
+v -0.012294 -0.406586 0.032155
+vn -0.411190 0.121320 0.903440
+v -0.014333 -0.405880 0.031298
+vn -0.326812 0.020284 0.944872
+v -0.013383 -0.406859 0.031774
+vn -0.465905 -0.119307 0.876754
+v -0.010725 -0.405031 0.033059
+vn 0.903799 0.349166 -0.247449
+v 0.032075 -0.396695 0.011263
+vn 0.911230 0.372341 -0.176133
+v 0.032846 -0.397392 0.013294
+vn 0.847480 0.446927 -0.286415
+v 0.032331 -0.398463 0.009405
+vn 0.849796 0.476875 -0.224581
+v 0.033092 -0.399028 0.010851
+vn 0.778817 0.546880 -0.307191
+v 0.032574 -0.399605 0.008117
+vn 0.814966 0.462779 -0.348805
+v 0.031786 -0.398600 0.007804
+vn 0.754904 0.523754 -0.394717
+v 0.030803 -0.399031 0.005235
+vn 0.863850 0.355473 -0.356935
+v 0.030417 -0.398154 0.005482
+vn 0.893773 0.322236 -0.311982
+v 0.031238 -0.396301 0.009077
+vn 0.752249 0.547019 -0.367276
+v 0.031490 -0.399424 0.005992
+vn 0.739818 0.585492 -0.331465
+v 0.032386 -0.400123 0.006721
+vn 0.847633 0.374053 -0.376300
+v 0.030850 -0.397654 0.006845
+vn 0.393644 0.293532 0.871139
+v 0.025073 -0.391249 0.025559
+vn 0.439628 0.240523 0.865376
+v 0.025650 -0.387666 0.024120
+vn 0.424471 0.269659 0.864354
+v 0.025900 -0.393217 0.025871
+vn 0.396194 0.199968 0.896127
+v 0.025712 -0.395035 0.026425
+vn 0.746908 0.219104 0.627791
+v 0.029205 -0.388291 0.021701
+vn 0.509466 0.135668 0.849729
+v 0.028208 -0.400690 0.026152
+vn 0.386762 0.159147 0.908343
+v 0.025768 -0.397396 0.026851
+vn 0.531454 0.279476 0.799656
+v 0.027466 -0.392031 0.024559
+vn 0.542699 0.178773 0.820681
+v 0.027878 -0.396578 0.025582
+vn 0.526808 0.162611 0.834285
+v 0.028053 -0.398892 0.025923
+vn 0.631612 0.194019 0.750615
+v 0.030080 -0.400053 0.024682
+vn 0.559731 0.174267 0.810143
+v 0.029654 -0.401766 0.025383
+vn 0.445852 0.198557 0.872807
+v 0.025552 -0.385289 0.023581
+vn 0.375555 0.217694 0.900871
+v 0.024139 -0.386109 0.024409
+vn 0.474918 0.275192 0.835896
+v 0.026427 -0.389708 0.024351
+vn 0.553700 0.223944 0.802038
+v 0.027694 -0.394330 0.025170
+vn 0.689871 0.205798 0.694064
+v 0.029754 -0.395654 0.023838
+vn 0.929933 0.353897 -0.099909
+v 0.032943 -0.396709 0.015644
+vn 0.958467 0.271229 -0.088181
+v 0.032208 -0.394461 0.015647
+vn 0.937519 0.344937 0.045577
+v 0.032766 -0.395891 0.017663
+vn 0.911991 0.207670 0.353760
+v 0.030780 -0.390115 0.019776
+vn 0.960873 0.266288 0.076246
+v 0.032069 -0.393731 0.017721
+vn 0.912013 0.405101 0.064228
+v 0.033489 -0.397612 0.017594
+vn 0.892674 0.431304 0.130805
+v 0.035660 -0.402012 0.017775
+vn 0.877410 0.453762 -0.155728
+v 0.033600 -0.398741 0.014032
+vn 0.884886 0.451689 -0.113816
+v 0.033690 -0.398331 0.015636
+vn 0.979088 0.193485 -0.062852
+v 0.031589 -0.391721 0.015898
+vn 0.894047 0.425887 0.138927
+v 0.034196 -0.399140 0.017807
+vn 0.845373 0.345351 0.407526
+v 0.033412 -0.398899 0.020099
+vn 0.776510 0.376868 0.504978
+v 0.033693 -0.400569 0.020945
+vn 0.870730 0.485938 -0.075462
+v 0.034497 -0.399731 0.015655
+vn 0.684893 0.395440 0.612004
+v 0.033646 -0.402104 0.022128
+vn 0.743793 0.262423 0.614741
+v 0.032006 -0.399264 0.022501
+vn 0.792200 0.247606 0.557773
+v 0.031719 -0.397201 0.022009
+vn 0.668213 0.194799 0.718014
+v 0.030031 -0.397924 0.024185
+vn 0.818812 0.245281 0.519023
+v 0.031239 -0.395113 0.021801
+vn 0.688944 0.233108 0.686307
+v 0.029421 -0.393526 0.023508
+vn 0.624732 0.269507 0.732854
+v 0.028394 -0.390055 0.023157
+vn 0.693365 0.261831 0.671334
+v 0.029296 -0.391828 0.023045
+vn 0.464803 0.269624 0.843363
+v 0.030424 -0.404314 0.025555
+vn 0.667707 0.281521 0.689139
+v 0.031949 -0.401122 0.023330
+vn 0.910690 0.302336 0.281489
+v 0.032227 -0.395226 0.019708
+vn 0.886346 0.324914 0.329881
+v 0.032870 -0.397102 0.019738
+vn 0.835617 0.250248 0.488999
+v 0.030739 -0.393116 0.021673
+vn 0.922484 0.267033 0.278778
+v 0.031566 -0.393124 0.019750
+vn 0.818404 0.257930 0.513504
+v 0.030112 -0.390987 0.021607
+vn 0.227763 0.212614 0.950220
+v 0.017610 -0.394345 0.028556
+vn 0.137138 0.324732 0.935811
+v 0.012920 -0.389177 0.027932
+vn 0.209246 0.261035 0.942378
+v 0.017700 -0.391501 0.027781
+vn 0.128220 0.308093 0.942676
+v 0.011415 -0.391240 0.028901
+vn 0.276926 0.143403 0.950130
+v 0.023415 -0.397847 0.027731
+vn 0.286707 0.093971 0.953398
+v 0.024154 -0.400533 0.027825
+vn 0.177530 0.120624 0.976695
+v 0.014213 -0.396278 0.029592
+vn 0.162202 0.303150 0.939037
+v 0.013191 -0.391484 0.028709
+vn 0.164181 0.213764 0.962990
+v 0.013596 -0.393659 0.029254
+vn 0.228480 0.123998 0.965620
+v 0.020375 -0.397970 0.028517
+vn 0.279804 0.203769 0.938183
+v 0.023337 -0.395093 0.027261
+vn 0.238437 0.204533 0.949376
+v 0.020563 -0.394877 0.027946
+vn 0.304319 0.256426 0.917407
+v 0.023844 -0.392815 0.026546
+vn 0.379899 0.265998 0.885958
+v 0.024404 -0.388748 0.025016
+vn 0.380588 0.122849 0.916548
+v 0.026116 -0.399785 0.027065
+vn 0.149911 0.288528 0.945663
+v 0.014896 -0.388650 0.027461
+vn 0.178033 0.253579 0.950790
+v 0.017364 -0.387318 0.026671
+vn 0.239332 0.250944 0.937948
+v 0.020774 -0.391375 0.027016
+vn 0.306352 0.268567 0.913247
+v 0.022725 -0.389817 0.026003
+vn 0.267682 0.095998 0.958713
+v 0.011251 -0.398932 0.030466
+vn 0.197809 0.150273 0.968654
+v 0.011643 -0.396067 0.030036
+vn 0.070947 0.337186 0.938761
+v 0.011037 -0.387077 0.027387
+vn 0.202381 0.138828 0.969417
+v 0.016488 -0.395935 0.029118
+vn 0.169894 -0.048870 0.984250
+v 0.021752 -0.408051 0.028726
+vn 0.138921 -0.047490 0.989164
+v 0.019366 -0.407905 0.029102
+vn 0.184828 -0.104468 0.977203
+v 0.024004 -0.409761 0.028170
+vn 0.193806 0.017694 0.980880
+v 0.022654 -0.406484 0.028605
+vn 0.280195 0.036185 0.959261
+v 0.012718 -0.403581 0.030238
+vn 0.154561 0.003320 0.987978
+v 0.019584 -0.405867 0.029118
+vn 0.201312 -0.033632 0.978950
+v 0.015593 -0.408581 0.029660
+vn 0.153942 -0.011587 0.988012
+v 0.016793 -0.406147 0.029498
+vn 0.156581 -0.078661 0.984528
+v 0.018539 -0.409362 0.029111
+vn 0.161270 0.022091 0.986663
+v 0.017169 -0.402775 0.029443
+vn 0.328282 0.057180 0.942848
+v 0.011427 -0.401629 0.030578
+vn 0.219319 0.016928 0.975506
+v 0.014149 -0.405652 0.029957
+vn 0.197061 0.047141 0.979257
+v 0.021103 -0.403728 0.028801
+vn 0.234997 0.074631 0.969127
+v 0.022268 -0.401570 0.028417
+vn 0.191992 0.016846 0.981252
+v 0.013979 -0.402452 0.029949
+vn 0.189670 0.069156 0.979409
+v 0.017943 -0.399629 0.029190
+vn 0.357355 0.062167 0.931898
+v 0.012354 -0.405324 0.030486
+vn 0.427393 -0.002966 0.904061
+v 0.011150 -0.406945 0.031128
+vn 0.289114 -0.045633 0.956207
+v 0.002009 -0.405341 0.035981
+vn 0.411110 0.098151 0.906286
+v 0.003115 -0.404012 0.035521
+vn 0.511705 -0.011370 0.859086
+v 0.007357 -0.406405 0.033275
+vn 0.410065 -0.002911 0.912051
+v 0.003344 -0.405378 0.035498
+vn 0.497446 0.198871 0.844392
+v 0.005912 -0.401346 0.033442
+vn 0.517737 0.149560 0.842366
+v 0.006247 -0.402918 0.033553
+vn 0.491602 0.154371 0.857028
+v 0.007768 -0.401552 0.032356
+vn 0.520120 0.118951 0.845769
+v 0.006529 -0.404228 0.033597
+vn 0.327485 0.288666 0.899681
+v 0.001531 -0.400345 0.035353
+vn 0.491860 0.088979 0.866116
+v 0.004825 -0.404478 0.034659
+vn 0.399802 0.321365 0.858419
+v 0.003389 -0.399480 0.034256
+vn 0.481445 0.164661 0.860870
+v 0.004472 -0.402720 0.034607
+vn 0.381099 0.199062 0.902850
+v 0.002552 -0.402351 0.035507
+vn 0.191580 0.261425 0.946020
+v -0.001206 -0.400257 0.036124
+vn 0.438224 0.246106 0.864518
+v 0.003962 -0.401087 0.034497
+vn 0.365992 0.416740 0.832092
+v 0.002846 -0.398033 0.033881
+vn 0.503213 0.037802 0.863335
+v 0.005992 -0.405588 0.034069
+vn 0.269512 0.372906 0.887865
+v -0.000522 -0.398373 0.035328
+vn 0.509653 0.138632 0.849138
+v 0.007924 -0.403213 0.032553
+vn 0.391454 -0.154380 0.907155
+v 0.010340 -0.408890 0.031350
+vn 0.451785 0.113488 0.884879
+v 0.009599 -0.402186 0.031435
+vn 0.453541 0.085361 0.887138
+v 0.010855 -0.404971 0.031139
+vn 0.407887 0.085888 0.908984
+v 0.011196 -0.403453 0.030802
+vn 0.485527 0.116873 0.866374
+v 0.009527 -0.403950 0.031729
+vn 0.516255 0.098598 0.850740
+v 0.007791 -0.404835 0.032893
+vn 0.403720 0.124249 0.906406
+v 0.009372 -0.400240 0.031302
+vn 0.495888 0.049432 0.866979
+v 0.009346 -0.405708 0.032053
+vn 0.450346 0.211177 0.867521
+v 0.007283 -0.399811 0.032274
+vn 0.247118 0.545253 0.801019
+v 0.001303 -0.394529 0.032404
+vn 0.117159 0.644969 0.755174
+v -0.000364 -0.392595 0.031367
+vn 0.024797 0.462749 0.886143
+v -0.003309 -0.396662 0.035107
+vn 0.210814 0.493289 0.843933
+v -0.002186 -0.396666 0.034950
+vn 0.105993 0.341899 0.933740
+v 0.009590 -0.390788 0.028968
+vn 0.083391 0.385467 0.918946
+v 0.007817 -0.390579 0.029070
+vn 0.064295 0.439480 0.895948
+v 0.006140 -0.390701 0.029256
+vn 0.077686 0.490438 0.868006
+v 0.004541 -0.391141 0.029618
+vn 0.178972 0.404176 0.897001
+v 0.005957 -0.393397 0.030555
+vn 0.136277 0.587615 0.797582
+v 0.001359 -0.392207 0.030750
+vn 0.236542 0.497190 0.834776
+v 0.002965 -0.394247 0.031703
+vn 0.122914 0.615210 0.778723
+v -0.002974 -0.395000 0.033914
+vn 0.256283 0.515127 0.817902
+v -0.000780 -0.396316 0.034298
+vn 0.256034 0.218111 0.941740
+v 0.009290 -0.395817 0.030535
+vn 0.166625 0.280548 0.945266
+v 0.009485 -0.393269 0.029828
+vn 0.219202 0.587237 0.779168
+v -0.000498 -0.394593 0.033021
+vn 0.302645 0.493123 0.815619
+v 0.001300 -0.396534 0.033698
+vn 0.147771 0.635489 0.757838
+v -0.002276 -0.394299 0.033237
+vn 0.070225 0.698105 0.712543
+v -0.002037 -0.392878 0.031874
+vn -0.027593 0.701712 0.711926
+v -0.003560 -0.393802 0.032924
+vn 0.178272 0.342014 0.922630
+v 0.007608 -0.393230 0.030160
+vn 0.285525 0.301809 0.909608
+v 0.007277 -0.395686 0.031101
+vn 0.271051 0.369055 0.889005
+v 0.005772 -0.395503 0.031504
+vn 0.196620 0.453119 0.869497
+v 0.004474 -0.393789 0.031060
+vn 0.326604 0.462649 0.824188
+v 0.002997 -0.396486 0.032975
+vn -0.024582 0.555731 0.830999
+v 0.003183 -0.389116 0.028393
+vn 0.113495 0.537636 0.835503
+v 0.002978 -0.391721 0.030150
+vn 0.452899 0.276548 0.847587
+v 0.005326 -0.399765 0.033344
+vn 0.392167 0.372258 0.841207
+v 0.004381 -0.398188 0.033220
+vn -0.058159 0.493926 0.867557
+v 0.004822 -0.388031 0.027799
+vn 0.312108 0.410829 0.856626
+v 0.004569 -0.396184 0.032198
+vn 0.012569 0.391992 0.919883
+v 0.008262 -0.387892 0.027868
+vn 0.323863 0.401022 0.856910
+v 0.001414 -0.398324 0.034621
+vn 0.357136 0.204221 0.911454
+v 0.008523 -0.398030 0.031289
+vn -0.221128 0.771837 0.596129
+v -0.002919 -0.390415 0.028936
+vn -0.012382 0.669809 0.742431
+v -0.000021 -0.390392 0.029385
+vn -0.066398 0.727460 0.682930
+v -0.001803 -0.390892 0.029777
+vn 0.378767 0.302779 0.874563
+v 0.006271 -0.397749 0.032168
+vn 0.120673 0.344282 0.931079
+v 0.011529 -0.389517 0.028262
+vn 0.019716 0.259587 0.965518
+v -0.003306 -0.399443 0.036137
+vn 0.145772 0.342040 0.928310
+v -0.002326 -0.398168 0.035678
+vn -0.033706 0.436887 0.898885
+v 0.006572 -0.387891 0.027844
+vn -0.492275 0.869388 0.042778
+v -0.009384 -0.391708 0.021204
+vn -0.484536 0.565794 0.667160
+v -0.008819 -0.395918 0.031557
+vn -0.455627 0.644743 0.613768
+v -0.009977 -0.395627 0.030464
+vn -0.532624 0.498295 0.684115
+v -0.007619 -0.396132 0.032635
+vn -0.424250 0.361409 0.830298
+v -0.005068 -0.397555 0.035166
+vn -0.485525 0.438458 0.756320
+v -0.005619 -0.396480 0.034331
+vn -0.504456 0.358018 0.785715
+v -0.005872 -0.397495 0.034672
+vn -0.222424 0.386027 0.895271
+v -0.004415 -0.397599 0.035438
+vn -0.470185 0.695211 0.543698
+v -0.009713 -0.394618 0.029514
+vn -0.527854 0.465467 0.710430
+v -0.006522 -0.396295 0.033590
+vn -0.460601 0.732320 0.501551
+v -0.010869 -0.394583 0.028431
+vn -0.339253 0.471274 0.814131
+v -0.004919 -0.396533 0.034737
+vn -0.476017 0.675809 0.562752
+v -0.011170 -0.395526 0.029405
+vn -0.337426 0.581617 0.740179
+v -0.004999 -0.395450 0.033968
+vn -0.458025 0.802557 0.382250
+v -0.006073 -0.391532 0.028109
+vn -0.024453 0.358358 0.933264
+v -0.003616 -0.397824 0.035633
+vn -0.463920 0.696663 0.547211
+v -0.005998 -0.393345 0.031229
+vn -0.454949 0.645987 0.612962
+v -0.006051 -0.394402 0.032381
+vn -0.492330 0.857515 0.149266
+v -0.008828 -0.391621 0.023385
+vn -0.534408 0.797886 0.278901
+v -0.006086 -0.390511 0.025607
+vn -0.485358 0.816690 0.312160
+v -0.006932 -0.391415 0.026680
+vn -0.471449 0.774985 0.420873
+v -0.008063 -0.392928 0.028581
+vn -0.474690 0.731151 0.489988
+v -0.006993 -0.393126 0.030011
+vn -0.494828 0.589898 0.638096
+v -0.011553 -0.396609 0.030226
+vn -0.371927 0.639416 0.672917
+v -0.005112 -0.394412 0.033018
+vn -0.150588 0.666632 0.730017
+v -0.004199 -0.394591 0.033547
+vn -0.380828 0.887632 0.258997
+v -0.011955 -0.393512 0.024963
+vn -0.440647 0.801552 0.404158
+v -0.010717 -0.393789 0.027234
+vn -0.447811 0.869070 0.210194
+v -0.009929 -0.392480 0.024688
+vn -0.469996 0.759685 0.449425
+v -0.009529 -0.393694 0.028343
+vn -0.461192 0.813812 0.353570
+v -0.009208 -0.392910 0.027172
+vn -0.504677 0.633561 0.586431
+v -0.008581 -0.394769 0.030664
+vn -0.430434 0.765301 0.478582
+v -0.005750 -0.392563 0.030344
+vn -0.325884 0.767176 0.552486
+v -0.005024 -0.392911 0.031424
+vn -0.470503 0.837941 0.276552
+v -0.008622 -0.392176 0.026086
+vn -0.500000 0.833323 0.235740
+v -0.007767 -0.391399 0.025155
+vn -0.483165 0.474572 0.735754
+v -0.010532 -0.397556 0.031682
+vn -0.178948 0.695486 0.695900
+v -0.004282 -0.393777 0.032753
+vn -0.366318 0.285031 0.885759
+v -0.004795 -0.398390 0.035608
+vn -0.183883 0.260716 0.947742
+v -0.004277 -0.398726 0.035856
+vn -0.053707 0.751542 0.657495
+v -0.003403 -0.392935 0.031968
+vn -0.225891 0.780153 0.583382
+v -0.004382 -0.392865 0.031661
+vn 0.043669 0.588049 0.807646
+v -0.003372 -0.395625 0.034483
+vn -0.170157 0.487952 0.856124
+v -0.004224 -0.396550 0.034957
+vn -0.192867 0.576386 0.794091
+v -0.004203 -0.395485 0.034278
+vn -0.471514 0.581980 0.662551
+v -0.010416 -0.396464 0.030966
+vn -0.504495 0.496881 0.706112
+v -0.012076 -0.397750 0.030750
+vn -0.481657 0.285807 0.828445
+v -0.005574 -0.398542 0.035252
+vn -0.428454 0.555538 0.712604
+v -0.005553 -0.395603 0.033812
+vn -0.404180 0.791693 0.458106
+v -0.005107 -0.391783 0.029561
+vn -0.488870 0.713756 0.501556
+v -0.008379 -0.393770 0.029622
+vn -0.483667 0.678119 0.553372
+v -0.007247 -0.394003 0.030973
+vn -0.474441 0.804871 0.356494
+v -0.007589 -0.392173 0.027605
+vn -0.515196 0.325162 0.792996
+v -0.006677 -0.398368 0.034501
+vn -0.533327 0.395192 0.747921
+v -0.006784 -0.397243 0.033938
+vn -0.521533 0.585345 0.620785
+v -0.007445 -0.394989 0.031856
+vn -0.139709 0.765055 0.628627
+v -0.003154 -0.391635 0.030438
+vn -0.503968 0.560427 0.657220
+v -0.006374 -0.395268 0.032967
+vn -0.304790 0.785278 0.538926
+v -0.004143 -0.391714 0.030139
+vn -0.455294 0.513839 0.727101
+v -0.009411 -0.396955 0.031980
+vn -0.500923 0.424203 0.754406
+v -0.007987 -0.397522 0.033227
+vn -0.432800 0.852192 0.294030
+v -0.010511 -0.393127 0.025911
+vn -0.466225 0.777499 0.422053
+v -0.006647 -0.392327 0.029046
+vn -0.375279 0.690348 0.618535
+v -0.005116 -0.393559 0.032149
+vn -0.012048 0.883464 -0.468344
+v -0.013672 -0.394659 0.011787
+vn -0.060326 0.869960 -0.489418
+v -0.014425 -0.394646 0.011854
+vn -0.005106 0.927418 -0.373992
+v -0.014208 -0.394371 0.012385
+vn -0.052278 0.830597 -0.554414
+v -0.013581 -0.394939 0.011329
+vn -0.296364 -0.953227 0.059392
+v -0.010708 -0.424792 0.025507
+vn -0.365494 0.929549 -0.048493
+v -0.011518 -0.392816 0.019008
+vn -0.384746 0.910067 0.154105
+v -0.011552 -0.393016 0.023489
+vn -0.442764 0.636651 0.631376
+v -0.014730 -0.396659 0.027835
+vn -0.374845 0.656869 0.654228
+v -0.016164 -0.396467 0.026709
+vn -0.326282 0.891458 0.314393
+v -0.013453 -0.394054 0.024727
+vn -0.475551 0.878434 -0.046950
+v -0.009743 -0.391930 0.019472
+vn -0.440505 0.768482 0.464102
+v -0.012242 -0.394659 0.027213
+vn -0.438078 0.891599 -0.114624
+v -0.010098 -0.392287 0.017652
+vn -0.299317 0.948200 -0.106424
+v -0.012043 -0.393162 0.017109
+vn -0.414729 0.717145 0.560093
+v -0.014159 -0.395746 0.027198
+vn -0.478256 0.616633 0.625328
+v -0.013114 -0.396688 0.029076
+vn -0.425642 0.817059 0.388900
+v -0.011828 -0.394000 0.026446
+vn -0.423934 0.905691 0.002165
+v -0.010809 -0.392453 0.020561
+vn -0.435855 0.894753 0.097194
+v -0.010566 -0.392412 0.022343
+vn 0.216141 -0.049136 0.975125
+v 0.027445 -0.409417 0.027490
+vn 0.209217 -0.017407 0.977714
+v 0.029826 -0.409987 0.026940
+vn 0.255524 0.084368 0.963115
+v 0.030146 -0.408875 0.026832
+vn 0.413050 0.095928 0.905642
+v 0.027081 -0.401726 0.026851
+vn 0.335005 0.087571 0.938138
+v 0.025694 -0.402741 0.027503
+vn -0.321649 -0.946830 -0.007470
+v -0.011328 -0.424619 0.024223
+vn 0.412699 0.269334 0.870137
+v 0.032614 -0.408150 0.025852
+vn 0.392692 0.270584 0.878964
+v 0.030499 -0.405941 0.026056
+vn 0.280812 0.079294 0.956481
+v 0.024691 -0.403971 0.027936
+vn 0.389774 0.126613 0.912165
+v 0.027431 -0.404059 0.026965
+vn 0.570953 0.280604 0.771540
+v 0.031468 -0.402736 0.024364
+vn 0.265127 0.058047 0.962465
+v 0.027777 -0.408050 0.027415
+vn -0.263874 0.055350 0.962968
+v -0.017712 -0.412260 0.031001
+vn 0.206895 -0.040629 0.977519
+v 0.024788 -0.408236 0.028142
+vn 0.511760 0.366459 0.777052
+v 0.032920 -0.405096 0.024454
+vn 0.354891 0.167327 0.919812
+v 0.027567 -0.405209 0.027079
+vn 0.314373 0.152844 0.936914
+v 0.027743 -0.406660 0.027274
+vn 0.467475 0.161963 0.869043
+v 0.028691 -0.403104 0.026214
+vn 0.262518 0.088372 0.960872
+v 0.024680 -0.405615 0.028085
+vn 0.248024 0.048548 0.967537
+v 0.025385 -0.407096 0.028018
+vn 0.242049 0.967555 0.072457
+v 0.026291 -0.376275 0.003745
+vn 0.102601 0.246095 0.963800
+v 0.015727 -0.378340 0.024750
+vn -0.418776 -0.908056 0.007838
+v -0.013015 -0.423961 0.024658
+vn 0.902207 0.409373 -0.135781
+v 0.029155 -0.377896 -0.001331
+vn 0.672958 0.738723 -0.037625
+v 0.028379 -0.376784 -0.001263
+vn 0.181480 0.271297 0.945232
+v 0.017741 -0.378241 0.024430
+vn 0.212655 0.222922 0.951359
+v 0.019304 -0.380759 0.024735
+vn 0.025943 0.236334 0.971326
+v 0.013792 -0.379242 0.025084
+vn 0.269500 0.209535 0.939928
+v 0.021747 -0.381300 0.024235
+vn 0.344802 0.314648 0.884369
+v 0.023243 -0.379271 0.023321
+vn 0.264040 0.306427 0.914541
+v 0.021882 -0.378985 0.023643
+vn 0.328848 0.194546 0.924127
+v 0.023450 -0.380832 0.023620
+vn 0.441533 0.464065 0.767914
+v 0.023789 -0.378844 0.022841
+vn 0.356234 0.184861 0.915928
+v 0.023845 -0.383349 0.023942
+vn 0.158071 0.357179 0.920563
+v 0.015969 -0.376493 0.024180
+vn -0.076633 0.214962 0.973611
+v 0.010495 -0.375966 0.024247
+vn -0.041293 0.236560 0.970739
+v 0.011730 -0.378047 0.024779
+vn 0.013528 0.262599 0.964810
+v 0.012399 -0.375700 0.024246
+vn -0.004279 0.251120 0.967947
+v 0.012096 -0.381194 0.025572
+vn 0.017266 0.294103 0.955618
+v 0.011105 -0.383994 0.026352
+vn 0.087329 0.300228 0.949861
+v 0.012621 -0.386374 0.027011
+vn 0.103013 0.260802 0.959881
+v 0.014366 -0.385282 0.026537
+vn 0.242023 0.242511 0.939475
+v 0.020318 -0.387126 0.025977
+vn 0.256350 0.215745 0.942199
+v 0.020777 -0.383811 0.025049
+vn 0.189358 0.225455 0.955674
+v 0.018386 -0.383664 0.025578
+vn 0.317828 0.237863 0.917827
+v 0.022454 -0.386677 0.025199
+vn 0.317707 0.203629 0.926066
+v 0.022617 -0.384185 0.024558
+vn 0.148038 0.223043 0.963502
+v 0.017057 -0.380540 0.025097
+vn 0.083631 0.225549 0.970636
+v 0.015235 -0.380476 0.025296
+vn 0.120327 0.231517 0.965361
+v 0.015928 -0.383162 0.025842
+vn 0.045081 0.264638 0.963294
+v 0.012614 -0.383764 0.026239
+vn 0.053380 0.238392 0.969701
+v 0.013869 -0.382092 0.025757
+vn 0.602225 0.587470 0.540559
+v 0.026008 -0.378932 0.021007
+vn 0.363131 0.915119 0.175193
+v 0.025440 -0.377434 0.018333
+vn 0.296164 0.949953 0.099378
+v 0.021901 -0.376264 0.018613
+vn 0.335410 0.940590 0.052831
+v 0.028303 -0.376786 0.003636
+vn 0.320864 0.945867 0.048794
+v 0.027530 -0.376382 0.000810
+vn 0.836505 0.472475 -0.277539
+v 0.027797 -0.376923 -0.006682
+vn 0.773458 0.599655 -0.205370
+v 0.027082 -0.376161 -0.007397
+vn 0.589100 0.518161 0.620056
+v 0.025158 -0.378934 0.021901
+vn 0.482802 0.806981 0.340123
+v 0.025809 -0.378105 0.019987
+vn 0.289221 0.952757 0.092766
+v 0.024845 -0.376824 0.015237
+vn 0.373310 0.926650 0.044254
+v 0.027252 -0.376121 -0.002241
+vn 0.268826 0.960725 0.068850
+v 0.026071 -0.375609 -0.004488
+vn 0.547046 0.836520 -0.031229
+v 0.027378 -0.376098 -0.004986
+vn 0.436785 0.899563 -0.002216
+v 0.026490 -0.375600 -0.006523
+vn 0.480537 0.759238 0.438909
+v 0.024687 -0.378158 0.021455
+vn 0.613281 0.776021 -0.147231
+v 0.026182 -0.375555 -0.008580
+vn 0.419476 0.748591 0.513470
+v 0.023370 -0.377869 0.022225
+vn 0.376306 0.891582 0.251944
+v 0.023391 -0.377195 0.020763
+vn 0.269822 0.959913 0.075915
+v 0.017764 -0.375143 0.020119
+vn 0.228881 0.971004 0.069029
+v 0.024051 -0.375109 -0.005009
+vn 0.264548 0.963167 0.048197
+v 0.025334 -0.375199 -0.007508
+vn 0.231083 0.971573 0.051447
+v 0.023701 -0.374668 -0.009988
+vn 0.651205 0.707902 -0.273507
+v 0.023944 -0.374851 -0.013387
+vn 0.310358 0.557037 0.770316
+v 0.020109 -0.377082 0.023269
+vn 0.385567 0.804509 0.451778
+v 0.021518 -0.376951 0.022306
+vn 0.240439 0.968218 0.068865
+v 0.025576 -0.375770 -0.000868
+vn 0.243593 0.967930 0.061436
+v 0.021540 -0.374305 -0.007995
+vn 0.293330 0.937618 0.186628
+v 0.017936 -0.375398 0.022083
+vn 0.357096 0.827345 0.433569
+v 0.019437 -0.376094 0.022525
+vn 0.337152 0.925453 0.172817
+v 0.020322 -0.376088 0.021293
+vn 0.289424 0.955634 -0.054742
+v 0.023261 -0.374487 -0.013103
+vn 0.136358 0.438760 0.888198
+v 0.014041 -0.375361 0.024003
+vn 0.379373 0.897045 -0.226686
+v 0.022216 -0.374405 -0.014997
+vn 0.258978 0.949264 0.178405
+v 0.015173 -0.374683 0.022401
+vn 0.298842 0.788325 0.537808
+v 0.017619 -0.375673 0.023043
+vn 0.880313 0.187837 -0.435622
+v 0.027287 -0.386278 -0.012821
+vn 0.822867 0.226316 -0.521220
+v 0.026212 -0.387293 -0.015191
+vn 0.999597 -0.028015 -0.004540
+v 0.029807 -0.382968 -0.000787
+vn 0.996723 0.018398 -0.078767
+v 0.029711 -0.383821 -0.002992
+vn 0.985157 0.144099 -0.093277
+v 0.029787 -0.379384 0.000185
+vn 0.935605 0.168775 -0.310093
+v 0.028371 -0.379617 -0.007331
+vn 0.938114 0.143818 -0.315052
+v 0.028482 -0.382519 -0.008381
+vn 0.892788 0.178111 -0.413770
+v 0.027636 -0.382317 -0.010410
+vn 0.949033 0.266124 -0.168860
+v 0.028573 -0.377809 -0.005060
+vn 0.902127 0.217682 -0.372534
+v 0.027833 -0.378813 -0.008344
+vn 0.876449 0.185247 -0.444432
+v 0.026970 -0.380251 -0.010943
+vn 0.887718 0.287469 -0.359608
+v 0.027281 -0.377592 -0.008866
+vn 0.259382 0.963803 0.061681
+v 0.017462 -0.373322 -0.006617
+vn 0.251014 0.965795 0.065060
+v 0.021731 -0.374997 0.001588
+vn 0.815771 0.191398 -0.545788
+v 0.025639 -0.379405 -0.013010
+vn 0.970916 0.185599 -0.151246
+v 0.029557 -0.379569 -0.001877
+vn 0.958573 0.167620 -0.230307
+v 0.028853 -0.379855 -0.005724
+vn 0.990471 0.068689 -0.119373
+v 0.029694 -0.381579 -0.002479
+vn 0.872033 0.209024 -0.442569
+v 0.026530 -0.378499 -0.011103
+vn 0.828257 0.213241 -0.518188
+v 0.026187 -0.384076 -0.013806
+vn 0.819263 0.190127 -0.540981
+v 0.025797 -0.381246 -0.013358
+vn 0.249724 0.968219 0.013818
+v 0.022009 -0.374166 -0.012766
+vn 0.919225 0.150318 -0.363910
+v 0.028092 -0.385176 -0.010576
+vn 0.977750 0.100627 -0.184064
+v 0.029426 -0.382138 -0.004506
+vn 0.963632 0.116369 -0.240566
+v 0.029032 -0.382574 -0.006513
+vn 0.987658 0.053767 -0.147109
+v 0.029530 -0.384083 -0.004679
+vn 0.999020 0.016213 -0.041188
+v 0.029736 -0.386005 -0.003037
+vn 0.346905 0.936563 -0.050074
+v 0.019869 -0.373558 -0.016115
+vn 0.261773 0.964307 0.039840
+v 0.020038 -0.373637 -0.012808
+vn 0.997567 0.063144 -0.029537
+v 0.029903 -0.379452 0.002097
+vn 0.999172 -0.040025 0.007324
+v 0.029888 -0.380883 0.001236
+vn 0.992738 -0.111861 0.044257
+v 0.029035 -0.385938 0.004753
+vn 0.992833 -0.114305 -0.034893
+v 0.028972 -0.386354 0.006156
+vn 0.990347 -0.097923 0.098097
+v 0.029227 -0.385269 0.003328
+vn 0.981697 -0.158386 -0.105755
+v 0.029271 -0.384799 0.006910
+vn -0.904836 -0.265760 0.332633
+v -0.023799 -0.411981 0.026242
+vn 0.998240 0.021147 -0.055412
+v 0.029850 -0.381147 -0.000460
+vn 0.998723 -0.049505 0.010130
+v 0.029891 -0.380033 0.003450
+vn 0.966150 -0.129819 -0.222939
+v 0.030244 -0.382838 0.010138
+vn 0.974292 -0.105647 -0.198981
+v 0.030147 -0.381609 0.009047
+vn 0.984818 -0.170896 -0.030458
+v 0.029677 -0.381641 0.005854
+vn 0.989785 -0.108472 0.092513
+v 0.029615 -0.383578 0.001583
+vn 0.995381 -0.083912 0.046637
+v 0.029788 -0.382019 0.001542
+vn 0.964090 -0.158783 -0.212883
+v 0.029747 -0.384263 0.009041
+vn 0.981631 -0.143108 -0.126173
+v 0.029674 -0.382251 0.006963
+vn 0.973605 -0.148570 -0.173263
+v 0.029717 -0.383163 0.008027
+vn 0.989394 -0.138088 -0.045066
+v 0.029306 -0.383970 0.005763
+vn 0.989949 -0.141425 0.000336
+v 0.029393 -0.383115 0.005041
+vn 0.993559 -0.074233 -0.085617
+v 0.029998 -0.380193 0.007158
+vn 0.982445 0.167978 -0.081153
+v 0.030124 -0.379019 0.008502
+vn 0.531068 0.644797 -0.549730
+v 0.019501 -0.373900 -0.017915
+vn 0.567726 0.749120 -0.341332
+v 0.019887 -0.373815 -0.017380
+vn 0.991942 -0.124949 0.020971
+v 0.029755 -0.381226 0.004256
+vn 0.982404 -0.121077 -0.142204
+v 0.029989 -0.380967 0.007832
+vn 0.990673 -0.048436 0.127358
+v 0.029085 -0.387984 0.002749
+vn 0.986387 -0.143467 0.080363
+v 0.029462 -0.383212 0.003530
+vn 0.989052 -0.076063 0.126456
+v 0.029458 -0.385552 0.001304
+vn 0.992353 -0.025034 0.120866
+v 0.029629 -0.388224 -0.000833
+vn 0.993598 -0.048139 0.102204
+v 0.029643 -0.385915 -0.000301
+vn 0.258417 0.714117 0.650583
+v 0.015663 -0.375331 0.023553
+vn 0.987700 -0.047030 0.149121
+v 0.029360 -0.388092 0.000955
+vn 0.998133 -0.039138 0.046901
+v 0.029733 -0.384674 -0.000665
+vn 0.972335 -0.150495 -0.178651
+v 0.029293 -0.385637 0.008026
+vn 0.987816 -0.071840 -0.138051
+v 0.029021 -0.386722 0.007310
+vn 0.981426 -0.047635 -0.185834
+v 0.030602 -0.381362 0.011014
+vn 0.994323 0.010514 -0.105886
+v 0.030873 -0.381340 0.012727
+vn 0.988113 0.027546 -0.151242
+v 0.030347 -0.380115 0.009560
+vn 0.922554 0.384153 -0.036335
+v 0.030293 -0.378961 0.010712
+vn 0.999121 0.004360 -0.041686
+v 0.029952 -0.379377 0.006040
+vn 0.977661 -0.095721 -0.187125
+v 0.030643 -0.383152 0.012032
+vn 0.998973 0.003947 -0.045135
+v 0.028885 -0.387730 0.006057
+vn 0.967757 -0.059360 -0.244792
+v 0.029344 -0.386650 0.008812
+vn 0.972363 0.150941 -0.178119
+v 0.029198 -0.390234 0.006806
+vn 0.989825 0.088817 -0.111167
+v 0.028984 -0.389016 0.006414
+vn 0.972243 0.182642 -0.146237
+v 0.031408 -0.391503 0.014706
+vn 0.956585 0.189965 -0.221041
+v 0.031156 -0.391720 0.013220
+vn 0.944027 0.268231 -0.192004
+v 0.031809 -0.394182 0.013600
+vn 0.982887 0.040632 -0.179675
+v 0.029067 -0.387737 0.007636
+vn 0.953344 0.183450 -0.239753
+v 0.029651 -0.391181 0.008078
+vn 0.949695 0.182047 -0.254831
+v 0.029802 -0.393412 0.006997
+vn 0.969202 0.115650 -0.217421
+v 0.030814 -0.389302 0.013403
+vn 0.959700 -0.136184 -0.245825
+v 0.030195 -0.384148 0.010711
+vn 0.962647 -0.097119 -0.252742
+v 0.030283 -0.385220 0.011558
+vn 0.969970 0.142784 -0.196902
+v 0.029490 -0.393765 0.005447
+vn 0.963537 0.055020 -0.261857
+v 0.029455 -0.387960 0.009236
+vn 0.946560 0.149280 -0.285900
+v 0.030145 -0.389825 0.010852
+vn 0.954211 0.150274 -0.258649
+v 0.029716 -0.389573 0.009505
+vn 0.968229 0.129624 -0.213848
+v 0.029261 -0.389127 0.007960
+vn 0.942324 0.202147 -0.266764
+v 0.030226 -0.391427 0.010053
+vn 0.932679 0.251880 -0.258198
+v 0.031068 -0.393653 0.011155
+vn 0.958121 -0.123962 -0.258141
+v 0.029777 -0.385432 0.009891
+vn 0.956963 -0.038838 -0.287599
+v 0.029829 -0.386758 0.010490
+vn 0.965421 -0.016351 -0.260183
+v 0.030314 -0.386828 0.012078
+vn 0.953897 0.064636 -0.293093
+v 0.029938 -0.388313 0.010761
+vn 0.957384 0.088914 -0.274793
+v 0.030409 -0.388736 0.012137
+vn 0.947236 0.179066 -0.265855
+v 0.030626 -0.390891 0.011797
+vn 0.923785 0.302125 -0.235246
+v 0.030099 -0.398333 0.004294
+vn 0.916777 0.240164 -0.319126
+v 0.030336 -0.395442 0.007348
+vn 0.996728 -0.053607 -0.060487
+v 0.030902 -0.382706 0.013688
+vn 0.987019 0.016498 -0.159755
+v 0.030696 -0.386678 0.013735
+vn 0.984657 -0.069314 -0.160142
+v 0.030677 -0.384790 0.013116
+vn 0.925621 0.183844 0.330799
+v 0.030355 -0.381773 0.016697
+vn 0.978812 0.086002 0.185826
+v 0.030616 -0.384038 0.016646
+vn 0.999489 -0.031938 -0.001108
+v 0.030834 -0.383996 0.014586
+vn 0.995404 0.084734 0.044625
+v 0.030918 -0.381277 0.013942
+vn 0.784997 0.289151 0.547879
+v 0.028927 -0.381918 0.019894
+vn 0.888859 0.228478 0.397149
+v 0.029644 -0.382726 0.019111
+vn 0.949598 0.171256 0.262556
+v 0.030621 -0.381360 0.015700
+vn 0.982224 0.152883 0.108915
+v 0.031026 -0.388409 0.017501
+vn 0.990574 0.075941 -0.114000
+v 0.030872 -0.387539 0.014703
+vn 0.981082 0.133278 -0.140413
+v 0.031077 -0.389472 0.014699
+vn 0.989470 0.141419 -0.030819
+v 0.031161 -0.389084 0.016059
+vn 0.995739 0.092199 0.001700
+v 0.030894 -0.386836 0.015898
+vn 0.981985 0.127000 0.139915
+v 0.030707 -0.385903 0.017232
+vn 0.547389 0.210199 0.810050
+v 0.026640 -0.383989 0.022664
+vn 0.975006 0.191955 0.111875
+v 0.031473 -0.391124 0.017655
+vn 0.553157 0.247255 0.795539
+v 0.025768 -0.381360 0.022548
+vn 0.445292 0.189161 0.875176
+v 0.025042 -0.382516 0.023262
+vn 0.992283 0.033764 0.119307
+v 0.030856 -0.382027 0.014855
+vn 0.724366 0.218983 0.653713
+v 0.028688 -0.383319 0.020758
+vn 0.658356 0.207135 0.723645
+v 0.028235 -0.385168 0.021758
+vn 0.948679 0.158403 0.273709
+v 0.030307 -0.385042 0.018553
+vn 0.859521 0.184929 0.476471
+v 0.029660 -0.384223 0.019742
+vn 0.818995 0.178870 0.545209
+v 0.029619 -0.386296 0.020521
+vn 0.618274 0.262030 0.740998
+v 0.027279 -0.382083 0.021621
+vn 0.999568 0.027293 0.010946
+v 0.030799 -0.385240 0.015373
+vn 0.936524 0.162099 0.310879
+v 0.030555 -0.387442 0.019045
+vn 0.561801 0.228514 0.795085
+v 0.027286 -0.387298 0.023081
+vn 0.260254 0.963259 0.066337
+v 0.013991 -0.373223 0.005805
+vn 0.305137 0.943229 -0.131188
+v 0.019191 -0.373419 -0.017214
+vn 0.939571 0.341918 -0.017282
+v 0.029865 -0.378285 0.007084
+vn 0.257677 0.964016 0.065386
+v 0.018685 -0.374994 0.013859
+vn 0.986493 -0.022681 0.162223
+v 0.029546 -0.394061 -0.001405
+vn 0.996640 0.010132 0.081279
+v 0.029032 -0.390141 0.002967
+vn 0.998059 0.011484 0.061215
+v 0.029157 -0.392902 0.001899
+vn 0.991183 -0.004897 0.132409
+v 0.029856 -0.393829 -0.003201
+vn 0.996851 0.008931 0.078799
+v 0.029842 -0.391066 -0.002975
+vn 0.987224 0.026447 0.157126
+v 0.029528 -0.396895 -0.001574
+vn 0.986339 0.017733 0.163771
+v 0.029855 -0.396632 -0.003383
+vn 0.998019 0.061257 -0.014321
+v 0.029072 -0.391738 0.003667
+vn 0.989184 0.111002 -0.095879
+v 0.029078 -0.390780 0.005369
+vn 0.970128 0.152942 -0.188309
+v 0.029368 -0.391934 0.006205
+vn 0.987486 0.106478 -0.116339
+v 0.029237 -0.392971 0.004517
+vn 0.919934 0.240918 -0.309321
+v 0.030155 -0.396960 0.005769
+vn 0.988355 -0.025941 0.149936
+v 0.029599 -0.391039 -0.001130
+vn 0.999515 0.011017 0.029139
+v 0.029776 -0.388068 -0.002573
+vn 0.998653 0.051659 -0.004768
+v 0.028927 -0.389446 0.004756
+vn 0.992838 0.098210 -0.068021
+v 0.029269 -0.396503 0.001703
+vn 0.997769 -0.031405 0.058909
+v 0.028908 -0.387841 0.004449
+vn 0.990210 -0.022029 0.137835
+v 0.029279 -0.391216 0.000820
+vn 0.998464 0.049821 0.024229
+v 0.030031 -0.393586 -0.005142
+vn 0.996598 0.063352 -0.052724
+v 0.029192 -0.394226 0.002777
+vn 0.999245 0.022348 0.031792
+v 0.029201 -0.395180 0.001292
+vn 0.992590 -0.010868 0.121028
+v 0.029295 -0.394610 0.000152
+vn 0.956682 0.206367 -0.205359
+v 0.029739 -0.397197 0.004044
+vn 0.953376 0.174477 -0.246235
+v 0.029783 -0.395320 0.005651
+vn 0.979538 0.130534 -0.153184
+v 0.029408 -0.395470 0.003749
+vn 0.995179 0.051909 0.083217
+v 0.029309 -0.396929 0.000083
+vn 0.979799 0.150175 -0.132064
+v 0.030123 -0.396390 -0.009507
+vn 0.939361 0.194105 -0.282709
+v 0.030333 -0.401203 -0.012671
+vn 0.874119 0.210120 -0.437911
+v 0.027215 -0.389713 -0.014488
+vn 0.881788 0.227420 -0.413195
+v 0.027858 -0.392746 -0.014807
+vn 0.964427 0.156632 -0.212950
+v 0.030023 -0.397601 -0.011260
+vn 0.924374 0.163493 -0.344678
+v 0.029064 -0.396699 -0.014021
+vn 0.942960 0.187973 -0.274758
+v 0.029239 -0.394854 -0.012406
+vn 0.901206 0.161105 -0.402335
+v 0.030023 -0.402261 -0.014051
+vn 0.946940 0.172861 -0.270970
+v 0.029931 -0.399288 -0.012685
+vn 0.970251 0.158911 -0.182647
+v 0.029702 -0.393943 -0.009704
+vn 0.851100 0.132952 -0.507890
+v 0.028112 -0.399378 -0.017035
+vn 0.969118 0.107362 -0.221997
+v 0.029156 -0.388387 -0.008376
+vn 0.985107 0.098338 -0.141045
+v 0.029642 -0.390319 -0.006872
+vn 0.988303 0.112333 -0.103139
+v 0.029886 -0.392893 -0.007265
+vn 0.965337 0.146946 -0.215710
+v 0.029325 -0.391321 -0.009397
+vn 0.988201 0.147047 0.042842
+v 0.030634 -0.399971 -0.007556
+vn 0.989110 0.130542 -0.067972
+v 0.030457 -0.398443 -0.009069
+vn 0.979783 0.167878 -0.108819
+v 0.030584 -0.399938 -0.009857
+vn 0.930763 0.192443 -0.310880
+v 0.028722 -0.391963 -0.012113
+vn 0.987610 0.094693 0.125138
+v 0.030283 -0.398775 -0.005419
+vn 0.994959 0.042458 0.090854
+v 0.030133 -0.396138 -0.005272
+vn 0.994749 0.098988 0.026014
+v 0.030377 -0.397686 -0.007327
+vn 0.988761 0.061655 -0.136202
+v 0.029545 -0.386978 -0.005582
+vn 0.789720 0.244401 -0.562682
+v 0.025468 -0.389141 -0.017079
+vn 0.904085 0.151119 -0.399741
+v 0.029346 -0.400180 -0.014827
+vn 0.847615 0.177137 -0.500172
+v 0.027684 -0.396049 -0.016655
+vn 0.890927 0.163796 -0.423580
+v 0.028615 -0.398139 -0.015646
+vn 0.890421 0.203334 -0.407192
+v 0.028254 -0.394762 -0.015085
+vn 0.931631 0.155029 -0.328678
+v 0.028382 -0.388572 -0.011190
+vn 0.993786 0.102926 -0.042383
+v 0.030149 -0.395294 -0.007341
+vn 0.995942 0.056057 -0.070403
+v 0.029751 -0.388810 -0.004744
+vn 0.997515 0.061324 -0.034684
+v 0.029885 -0.391083 -0.004967
+vn 0.975063 0.081341 -0.206485
+v 0.029234 -0.385225 -0.006705
+vn 0.954103 0.112564 -0.277517
+v 0.028724 -0.385636 -0.008899
+vn 0.827374 0.219448 -0.517005
+v 0.031540 -0.406711 -0.013036
+vn 0.885637 0.220275 -0.408811
+v 0.030956 -0.404611 -0.013019
+vn 0.928049 0.371860 0.021100
+v 0.031583 -0.403734 -0.007013
+vn 0.961712 0.239227 -0.133718
+v 0.030851 -0.401629 -0.010628
+vn 0.902667 0.361853 -0.232927
+v 0.031900 -0.404986 -0.010717
+vn 0.857749 0.110100 -0.502140
+v 0.029425 -0.403761 -0.015709
+vn 0.826388 0.099111 -0.554310
+v 0.030058 -0.405933 -0.015082
+vn 0.966996 0.254598 0.009930
+v 0.031064 -0.402027 -0.008265
+vn 0.935878 0.250606 -0.247649
+v 0.031033 -0.403067 -0.011609
+vn -0.779399 -0.623797 0.058423
+v -0.021503 -0.417674 0.023299
+vn 0.788383 0.188132 -0.585712
+v 0.032214 -0.408319 -0.012710
+vn 0.805598 0.015339 -0.592264
+v 0.028614 -0.405324 -0.017158
+vn -0.716324 -0.604690 -0.348180
+v 0.005410 -0.417008 0.001684
+vn -0.785790 -0.615922 0.056345
+v 0.005795 -0.416953 -0.000875
+vn -0.740813 -0.655183 -0.148092
+v 0.005939 -0.417223 0.000613
+vn -0.666445 -0.691445 -0.278848
+v 0.005137 -0.416525 0.001225
+vn -0.475008 -0.663918 -0.577564
+v 0.004199 -0.416521 0.002699
+vn 0.980231 0.128308 0.150611
+v 0.029991 -0.399250 -0.003405
+vn 0.979084 0.182646 0.089641
+v 0.029650 -0.398988 -0.001304
+vn 0.978882 0.202360 -0.029007
+v 0.029428 -0.398273 0.000435
+vn 0.919852 0.384816 -0.076081
+v 0.029952 -0.399963 -0.000110
+vn 0.963876 0.226771 -0.139709
+v 0.029520 -0.397890 0.002019
+vn 0.845984 0.442371 -0.297690
+v 0.030417 -0.398988 0.004474
+vn 0.965857 0.243102 0.089560
+v 0.030736 -0.401494 -0.005627
+vn 0.923344 0.347490 -0.163361
+v 0.029930 -0.399050 0.002370
+vn 0.089284 0.823339 -0.560484
+v 0.017526 -0.373573 -0.018804
+vn 0.198561 0.969763 -0.141894
+v 0.016477 -0.372770 -0.017003
+vn 0.243400 0.756649 -0.606827
+v 0.015991 -0.373744 -0.019218
+vn 0.227512 0.958856 0.169805
+v 0.012013 -0.373920 0.022709
+vn 0.271329 0.960305 0.064774
+v 0.012366 -0.372141 -0.003171
+vn 0.258410 0.965481 0.032706
+v 0.014246 -0.372117 -0.011984
+vn 0.250818 0.967797 0.021445
+v 0.018023 -0.373014 -0.014737
+vn 0.272624 0.956858 -0.100495
+v 0.013963 -0.372134 -0.016836
+vn 0.375050 0.812946 -0.445484
+v 0.014468 -0.372968 -0.019152
+vn 0.175323 0.973789 0.144902
+v 0.010026 -0.373414 0.022181
+vn 0.240154 0.968158 0.070687
+v 0.012991 -0.373900 0.020205
+vn 0.178857 0.977363 0.113016
+v 0.008484 -0.372969 0.020758
+vn 0.265879 0.960614 0.080809
+v 0.008455 -0.372121 0.011807
+vn 0.132026 0.399405 -0.907218
+v 0.013858 -0.379053 -0.022521
+vn 0.152047 0.464554 -0.872394
+v 0.013468 -0.381372 -0.023673
+vn 0.539448 0.248821 -0.804415
+v 0.017590 -0.380195 -0.021513
+vn 0.099998 0.443450 -0.890703
+v 0.012411 -0.380171 -0.023208
+vn 0.319046 0.597051 -0.736030
+v 0.010110 -0.390066 -0.031514
+vn 0.266392 0.646078 -0.715275
+v 0.010826 -0.387935 -0.029317
+vn 0.216083 0.696718 -0.684026
+v 0.006778 -0.386588 -0.029310
+vn 0.225777 0.681773 -0.695852
+v 0.008768 -0.387493 -0.029590
+vn 0.302232 0.440524 -0.845337
+v 0.014434 -0.382487 -0.024044
+vn 0.611821 0.324454 -0.721391
+v 0.013596 -0.392473 -0.030822
+vn 0.603658 0.392146 -0.694131
+v 0.015124 -0.390333 -0.028430
+vn 0.576603 0.443186 -0.686378
+v 0.015328 -0.388775 -0.027275
+vn 0.538936 0.439779 -0.718431
+v 0.013313 -0.390375 -0.029926
+vn 0.492123 0.507023 -0.707632
+v 0.013845 -0.388559 -0.028280
+vn 0.341742 0.539965 -0.769188
+v 0.013293 -0.385628 -0.026407
+vn 0.513391 0.444363 -0.734147
+v 0.015134 -0.386330 -0.025791
+vn 0.602653 0.308949 -0.735772
+v 0.017525 -0.384319 -0.022882
+vn 0.129231 0.532904 -0.836249
+v 0.011750 -0.382595 -0.024643
+vn 0.182932 0.559001 -0.808736
+v 0.012630 -0.383909 -0.025366
+vn 0.087036 0.521867 -0.848575
+v 0.010499 -0.381581 -0.024170
+vn 0.214704 0.648516 -0.730294
+v 0.011081 -0.385961 -0.027401
+vn 0.167715 0.618322 -0.767821
+v 0.010986 -0.384442 -0.026107
+vn 0.196514 0.673214 -0.712857
+v 0.009199 -0.385544 -0.027539
+vn 0.145845 0.617910 -0.772604
+v 0.009780 -0.383656 -0.025739
+vn 0.179687 0.681820 -0.709108
+v 0.007577 -0.384632 -0.027087
+vn 0.498852 0.349876 -0.792927
+v 0.015927 -0.383283 -0.023679
+vn -0.250523 0.156382 0.955397
+v -0.018629 -0.409348 0.030431
+vn -0.271360 0.091800 0.958090
+v -0.018532 -0.410943 0.030672
+vn 0.187569 0.668671 -0.719511
+v 0.006391 -0.383556 -0.026337
+vn 0.142083 0.625888 -0.766862
+v 0.005791 -0.382128 -0.025270
+vn -0.153810 0.759012 -0.632648
+v 0.002659 -0.381896 -0.025128
+vn -0.227203 0.820791 -0.524100
+v 0.001054 -0.383227 -0.026484
+vn 0.010926 0.688394 -0.725255
+v 0.004220 -0.381687 -0.025065
+vn -0.074423 0.794327 -0.602915
+v 0.001790 -0.383501 -0.027092
+vn -0.205512 0.622314 -0.755308
+v 0.004476 -0.379110 -0.022813
+vn -0.090003 0.619589 -0.779749
+v 0.004991 -0.379726 -0.023407
+vn 0.061529 0.426778 -0.902261
+v 0.010842 -0.379047 -0.022811
+vn 0.101592 0.477482 -0.872748
+v 0.008333 -0.380227 -0.023586
+vn 0.118070 0.625743 -0.771042
+v 0.008492 -0.382797 -0.025241
+vn 0.072510 0.510652 -0.856724
+v 0.009207 -0.381145 -0.023982
+vn 0.158637 0.569811 -0.806318
+v 0.007624 -0.381997 -0.024739
+vn 0.032922 0.544025 -0.838423
+v 0.006766 -0.379733 -0.023449
+vn -0.372067 0.698361 -0.611439
+v 0.003072 -0.379839 -0.022973
+vn -0.578921 0.718374 -0.385732
+v 0.001491 -0.380826 -0.022910
+vn -0.664559 0.711552 -0.228154
+v -0.000751 -0.383395 -0.024556
+vn -0.374701 0.818708 -0.435105
+v -0.000217 -0.384048 -0.027126
+vn -0.875839 0.478043 0.066188
+v -0.002162 -0.385117 -0.021555
+vn -0.920810 0.384862 0.063166
+v -0.004707 -0.390355 -0.024296
+vn -0.477628 0.800868 -0.361223
+v -0.000383 -0.383743 -0.026311
+vn -0.725814 0.665266 -0.174975
+v 0.000044 -0.382031 -0.022545
+vn -0.795894 0.600999 -0.073163
+v -0.001934 -0.384642 -0.024297
+vn -0.878961 0.475944 0.030071
+v -0.002866 -0.386110 -0.023964
+vn -0.901131 0.422945 0.095289
+v -0.002868 -0.386673 -0.020870
+vn -0.919646 0.374221 0.119206
+v -0.003436 -0.388188 -0.020218
+vn -0.819572 0.572703 -0.017676
+v -0.001190 -0.383493 -0.022072
+vn -0.675173 0.662169 -0.325074
+v 0.002409 -0.379083 -0.021146
+vn -0.320501 0.808097 -0.494225
+v 0.001038 -0.382687 -0.025568
+vn -0.252790 0.728126 -0.637126
+v 0.002812 -0.380945 -0.024037
+vn -0.475338 0.770066 -0.425502
+v 0.000730 -0.382333 -0.024685
+vn -0.913081 0.407761 0.003641
+v -0.003938 -0.388259 -0.026015
+vn -0.913657 0.401228 0.065167
+v -0.003706 -0.387992 -0.023587
+vn -0.864919 0.498506 -0.058380
+v -0.003226 -0.386789 -0.026235
+vn -0.922713 0.374179 0.092682
+v -0.003971 -0.388911 -0.022288
+vn -0.011015 0.975469 0.219862
+v -0.009063 -0.395243 -0.015518
+vn -0.239858 0.939137 0.245948
+v -0.007932 -0.395213 -0.015156
+vn -0.670782 0.741062 0.029651
+v -0.006555 -0.394153 -0.022660
+vn -0.662724 0.748114 0.033503
+v -0.006043 -0.393765 -0.020163
+vn -0.862031 0.494136 0.112839
+v -0.005360 -0.392868 -0.020412
+vn -0.567186 0.773635 0.282470
+v -0.006392 -0.394848 -0.014107
+vn -0.396181 0.918102 -0.011419
+v -0.007171 -0.394439 -0.020186
+vn -0.917706 0.382926 0.105750
+v -0.004692 -0.390743 -0.022418
+vn -0.861409 0.490249 0.132778
+v -0.004589 -0.392261 -0.017287
+vn -0.909279 0.375733 0.178987
+v -0.003464 -0.390641 -0.015107
+vn -0.917849 0.363338 0.159808
+v -0.003928 -0.390581 -0.017677
+vn -0.751643 0.631288 0.191071
+v -0.004865 -0.393432 -0.013987
+vn -0.766111 0.556043 0.322319
+v -0.004835 -0.394392 -0.011578
+vn -0.032686 0.994580 0.098706
+v -0.008785 -0.394742 -0.018796
+vn -0.919873 0.354160 0.168536
+v -0.002687 -0.387736 -0.016534
+vn -0.914035 0.381100 0.138932
+v -0.002730 -0.387174 -0.018061
+vn -0.926057 0.350064 0.140971
+v -0.003804 -0.389498 -0.019317
+vn -0.918243 0.339043 0.204646
+v -0.002583 -0.388577 -0.014528
+vn -0.905895 0.403015 0.130125
+v -0.005018 -0.391901 -0.021263
+vn -0.714228 0.692508 0.101542
+v -0.005367 -0.393381 -0.016987
+vn -0.536984 0.834587 0.122936
+v -0.006452 -0.394273 -0.016865
+vn -0.212547 0.976018 0.047040
+v -0.007897 -0.394629 -0.019220
+vn -0.285516 0.946290 0.151711
+v -0.007669 -0.394801 -0.016947
+vn -0.025794 0.994856 -0.097960
+v -0.008527 -0.394714 -0.021200
+vn -0.627694 0.702228 -0.335971
+v 0.005502 -0.372155 -0.015333
+vn 0.167561 0.980402 -0.103613
+v 0.011540 -0.371462 -0.016484
+vn 0.242327 0.929408 -0.278350
+v 0.013051 -0.372073 -0.018180
+vn -0.346993 0.005801 0.937850
+v -0.018691 -0.412189 0.030689
+vn -0.251570 0.101889 0.962461
+v -0.017369 -0.410929 0.030977
+vn 0.274788 0.958951 0.070033
+v 0.009237 -0.371835 0.005057
+vn 0.274064 0.958517 0.078321
+v 0.006971 -0.371541 0.010096
+vn 0.256614 0.965604 0.041921
+v 0.009971 -0.371132 -0.008035
+vn 0.033989 0.944369 -0.327126
+v 0.011600 -0.371750 -0.017755
+vn -0.072846 0.874961 -0.478682
+v 0.010656 -0.372070 -0.018353
+vn 0.243092 0.967129 0.074624
+v 0.010338 -0.372977 0.016105
+vn 0.201807 0.971257 0.126230
+v 0.005672 -0.371878 0.016368
+vn 0.200539 0.977578 0.064225
+v 0.007233 -0.370935 0.000295
+vn 0.244668 0.967469 0.064357
+v 0.007389 -0.371161 0.003131
+vn 0.275185 0.959023 0.067435
+v 0.008913 -0.371246 -0.001596
+vn -0.802844 0.526174 -0.280324
+v 0.004654 -0.374110 -0.017313
+vn -0.131977 0.871769 -0.471806
+v 0.008973 -0.372128 -0.018113
+vn 0.024282 0.941255 0.336822
+v 0.005764 -0.372810 0.021437
+vn -0.204246 0.945343 0.254185
+v 0.003455 -0.372390 0.019701
+vn 0.150674 0.977631 0.146748
+v 0.005674 -0.372341 0.019455
+vn -0.061695 0.979160 0.193493
+v 0.003745 -0.372051 0.018487
+vn 0.024701 0.991321 0.129119
+v 0.004003 -0.371431 0.014880
+vn 0.222161 0.969097 0.107220
+v 0.005623 -0.371460 0.013175
+vn 0.130858 0.990525 0.041664
+v 0.005121 -0.370908 0.007520
+vn 0.193157 0.978035 0.078345
+v 0.005418 -0.371144 0.010260
+vn 0.248960 0.966452 0.063164
+v 0.006643 -0.371224 0.007106
+vn 0.079097 0.995182 0.057933
+v 0.006677 -0.370778 -0.000657
+vn 0.203532 0.976562 0.070013
+v 0.007419 -0.370796 -0.002191
+vn -0.026278 0.999571 -0.012947
+v 0.006390 -0.370433 -0.006231
+vn 0.035796 0.281695 -0.958836
+v 0.001444 -0.394705 -0.036765
+vn -0.000830 0.373956 -0.927446
+v 0.000558 -0.393642 -0.036413
+vn 0.046127 0.209078 -0.976810
+v 0.002149 -0.395971 -0.037050
+vn -0.092994 0.247866 -0.964321
+v -0.000871 -0.394795 -0.036697
+vn -0.205494 -0.154206 -0.966433
+v -0.003198 -0.401994 -0.036125
+vn -0.179818 -0.171194 -0.968689
+v -0.001079 -0.400943 -0.036740
+vn -0.126633 -0.092257 -0.987650
+v 0.000630 -0.399591 -0.037252
+vn -0.050879 -0.115919 -0.991955
+v 0.002503 -0.400399 -0.037355
+vn -0.141586 -0.177072 -0.973960
+v 0.000942 -0.400920 -0.037105
+vn 0.002079 0.034995 -0.999385
+v 0.002650 -0.398853 -0.037422
+vn 0.037057 0.139099 -0.989585
+v 0.002573 -0.397389 -0.037282
+vn 0.186611 0.250901 -0.949855
+v 0.004929 -0.396365 -0.036835
+vn 0.155785 0.138031 -0.978099
+v 0.005222 -0.398299 -0.037171
+vn 0.188054 0.327331 -0.926008
+v 0.004232 -0.394763 -0.036482
+vn -0.215729 0.236669 -0.947338
+v -0.002693 -0.395062 -0.036458
+vn -0.170122 0.149627 -0.973997
+v -0.002265 -0.395962 -0.036724
+vn -0.280086 0.150425 -0.948116
+v -0.003734 -0.396186 -0.036410
+vn -0.177437 0.002639 -0.984129
+v -0.001870 -0.398123 -0.036937
+vn -0.243319 0.054171 -0.968432
+v -0.003440 -0.396937 -0.036576
+vn -0.278876 -0.018544 -0.960148
+v -0.003856 -0.398121 -0.036465
+vn -0.237656 -0.086838 -0.967460
+v -0.003219 -0.399032 -0.036605
+vn -0.177489 -0.109629 -0.977998
+v -0.001515 -0.399261 -0.036936
+vn -0.097014 0.022841 -0.995021
+v 0.000351 -0.398297 -0.037265
+vn -0.079374 0.102101 -0.991602
+v 0.000061 -0.397088 -0.037156
+vn -0.074382 0.164850 -0.983510
+v -0.000321 -0.395891 -0.036969
+vn -0.168063 0.061402 -0.983862
+v -0.002074 -0.397033 -0.036863
+vn -0.347962 0.032422 -0.936948
+v -0.004681 -0.397303 -0.036200
+vn -0.222192 -0.155350 -0.962547
+v -0.003187 -0.400236 -0.036431
+vn -0.393256 0.149319 -0.907223
+v -0.004815 -0.396491 -0.036071
+vn -0.235451 -0.115155 -0.965040
+v -0.005042 -0.402755 -0.035596
+vn -0.480865 0.226158 -0.847126
+v -0.005476 -0.396253 -0.035678
+vn -0.353749 0.243837 -0.902998
+v -0.004151 -0.395523 -0.036126
+vn -0.106996 0.506204 -0.855751
+v -0.001326 -0.392062 -0.035485
+vn -0.255656 0.544328 -0.798966
+v -0.002240 -0.391588 -0.034994
+vn -0.029869 0.570213 -0.820954
+v -0.000445 -0.390645 -0.034677
+vn -0.205912 0.412291 -0.887478
+v -0.002328 -0.393269 -0.035927
+vn -0.047525 0.469336 -0.881740
+v -0.000358 -0.392749 -0.035964
+vn 0.054829 0.563213 -0.824491
+v 0.001087 -0.391359 -0.035118
+vn 0.082742 0.495099 -0.864888
+v 0.002012 -0.392513 -0.035801
+vn -0.260609 0.628955 -0.732460
+v -0.001733 -0.389341 -0.033472
+vn -0.309596 0.475576 -0.823394
+v -0.003060 -0.392863 -0.035499
+vn -0.485000 0.532616 -0.693610
+v -0.003792 -0.392666 -0.034978
+vn -0.621549 0.556872 -0.550972
+v -0.004450 -0.392498 -0.034254
+vn -0.663002 0.564640 -0.491539
+v -0.003686 -0.390798 -0.033353
+vn -0.436379 0.572069 -0.694486
+v -0.002911 -0.391034 -0.034272
+vn -0.380328 0.371013 -0.847172
+v -0.003899 -0.393976 -0.035699
+vn -0.550727 0.390544 -0.737683
+v -0.005313 -0.394780 -0.035232
+vn -0.290128 0.313203 -0.904284
+v -0.003279 -0.394400 -0.036102
+vn -0.627042 0.509714 -0.589076
+v -0.005451 -0.394122 -0.034660
+vn -0.501985 0.462782 -0.730646
+v -0.004349 -0.393680 -0.035311
+vn -0.036856 -0.909353 -0.414389
+v -0.000956 -0.427720 -0.028530
+vn -0.139010 0.340070 -0.930069
+v -0.001569 -0.393907 -0.036337
+vn -0.687050 0.562800 -0.459585
+v -0.007209 -0.395394 -0.033724
+vn -0.448588 0.303838 -0.840507
+v -0.004761 -0.395113 -0.035714
+vn -0.555602 0.307651 -0.772436
+v -0.006273 -0.396054 -0.035104
+vn 0.608535 -0.486478 -0.626917
+v 0.025598 -0.419959 -0.015300
+vn 0.681598 -0.394821 -0.616069
+v 0.024527 -0.420710 -0.015846
+vn 0.562636 -0.480693 -0.672588
+v 0.026392 -0.420147 -0.014469
+vn 0.833195 -0.479587 -0.275284
+v 0.031841 -0.416370 -0.011360
+vn 0.790095 -0.563340 -0.241658
+v 0.030599 -0.418148 -0.011684
+vn 0.738929 -0.531685 -0.413878
+v 0.029672 -0.418898 -0.012539
+vn 0.805229 -0.590391 -0.055184
+v 0.030516 -0.418486 -0.010641
+vn 0.673915 -0.726742 -0.132986
+v 0.028119 -0.421137 -0.011069
+vn 0.649336 -0.696459 -0.305462
+v 0.026602 -0.422106 -0.012275
+vn 0.588909 -0.796573 -0.136594
+v 0.025199 -0.423231 -0.012445
+vn 0.044389 -0.825025 -0.563350
+v 0.002285 -0.427316 -0.029327
+vn -0.550636 -0.583834 -0.596606
+v -0.014310 -0.420668 -0.032201
+vn 0.023613 -0.804550 -0.593415
+v -0.000180 -0.426905 -0.030005
+vn 0.785789 -0.242488 -0.568978
+v 0.032714 -0.413662 -0.011896
+vn 0.730942 -0.197449 -0.653252
+v 0.031710 -0.413503 -0.013160
+vn -0.326274 -0.021295 0.945035
+v -0.017854 -0.413257 0.030978
+vn 0.324777 -0.458421 -0.827266
+v 0.006034 -0.426498 -0.028851
+vn 0.263080 -0.192811 -0.945311
+v 0.001802 -0.424707 -0.030696
+vn 0.110992 0.037396 -0.993117
+v -0.013805 -0.407355 -0.034697
+vn 0.253952 -0.363058 -0.896492
+v 0.002415 -0.426243 -0.030192
+vn 0.249235 -0.224226 -0.942128
+v 0.000312 -0.425465 -0.030981
+vn 0.249790 -0.151163 -0.956428
+v -0.001692 -0.424413 -0.031691
+vn 0.272681 -0.189837 -0.943190
+v 0.001888 -0.425308 -0.030541
+vn 0.242912 -0.198659 -0.949488
+v 0.001057 -0.423698 -0.031095
+vn 0.248240 -0.168375 -0.953953
+v -0.000836 -0.424175 -0.031505
+vn 0.244180 -0.160497 -0.956356
+v -0.000565 -0.423731 -0.031514
+vn 0.125139 -0.633832 -0.763281
+v 0.001868 -0.426660 -0.030106
+vn 0.208989 -0.231735 -0.950064
+v -0.002819 -0.424561 -0.031952
+vn 0.157073 -0.207972 -0.965441
+v -0.005098 -0.423647 -0.032599
+vn -0.278956 -0.040497 0.959450
+v -0.017048 -0.414054 0.031208
+vn 0.203623 -0.142244 -0.968661
+v -0.004464 -0.423352 -0.032515
+vn -0.020968 -0.311454 -0.950030
+v -0.008013 -0.422771 -0.033032
+vn 0.087807 -0.167278 -0.981992
+v -0.010666 -0.417059 -0.034369
+vn 0.056768 -0.187096 -0.980700
+v -0.011495 -0.417475 -0.034367
+vn -0.013892 -0.250966 -0.967896
+v -0.011910 -0.418075 -0.034247
+vn 0.033007 -0.165689 -0.985626
+v -0.012825 -0.416220 -0.034659
+vn 0.083420 -0.964146 -0.251920
+v 0.004617 -0.428423 -0.026398
+vn 0.049857 -0.984145 -0.170216
+v 0.003332 -0.428743 -0.025184
+vn -0.086888 -0.281784 -0.955536
+v -0.013187 -0.417616 -0.034332
+vn -0.135970 -0.253846 -0.957640
+v -0.014105 -0.416376 -0.034582
+vn -0.009644 -0.230504 -0.973024
+v -0.012717 -0.417287 -0.034437
+vn 0.045789 -0.186360 -0.981414
+v -0.011951 -0.416967 -0.034490
+vn 0.054214 -0.144592 -0.988005
+v -0.012239 -0.416074 -0.034657
+vn -0.049649 -0.288681 -0.956137
+v -0.012454 -0.418210 -0.034199
+vn -0.443191 -0.372228 -0.815492
+v -0.016076 -0.416113 -0.034038
+vn -0.216394 -0.328501 -0.919381
+v -0.013980 -0.417364 -0.034307
+vn 0.170327 -0.894196 -0.414008
+v 0.005931 -0.427956 -0.027200
+vn -0.363846 -0.288166 -0.885763
+v -0.016112 -0.414786 -0.034510
+vn -0.450100 -0.453475 -0.769266
+v -0.015247 -0.418026 -0.033497
+vn -0.298164 -0.435987 -0.849125
+v -0.013614 -0.419226 -0.033582
+vn -0.421095 -0.504823 -0.753546
+v -0.014103 -0.419952 -0.032930
+vn 0.021754 -0.035711 -0.999125
+v -0.015019 -0.412921 -0.035073
+vn -0.055854 -0.056426 -0.996843
+v -0.015914 -0.412074 -0.035098
+vn -0.390044 -0.216665 -0.894942
+v -0.017204 -0.412162 -0.034773
+vn -0.292659 -0.221935 -0.930105
+v -0.016059 -0.413577 -0.034860
+vn 0.069622 -0.042283 -0.996677
+v -0.014019 -0.413848 -0.034993
+vn 0.070716 -0.040068 -0.996691
+v -0.011715 -0.414177 -0.034794
+vn 0.073885 -0.105148 -0.991708
+v -0.011543 -0.415383 -0.034697
+vn 0.312638 -0.637148 -0.704486
+v 0.007692 -0.427029 -0.027792
+vn 0.092447 0.006193 -0.995698
+v -0.013323 -0.413184 -0.034940
+vn 0.069055 -0.078223 -0.994541
+v -0.012833 -0.414864 -0.034839
+vn 0.027131 -0.136072 -0.990327
+v -0.013630 -0.415229 -0.034845
+vn -0.007541 -0.107452 -0.994182
+v -0.014441 -0.414266 -0.034979
+vn -0.077808 -0.192260 -0.978255
+v -0.014291 -0.415229 -0.034830
+vn -0.181974 -0.218358 -0.958752
+v -0.015104 -0.414663 -0.034843
+vn -0.080821 -0.125673 -0.988774
+v -0.015149 -0.413711 -0.035005
+vn -0.227387 -0.127768 -0.965386
+v -0.016627 -0.411680 -0.035043
+vn 0.118860 0.052653 -0.991514
+v -0.013902 -0.411071 -0.034916
+vn 0.071145 0.054018 -0.996002
+v -0.012606 -0.411005 -0.034763
+vn 0.633406 -0.389774 -0.668485
+v 0.027688 -0.416551 -0.015788
+vn 0.016336 0.345074 -0.938433
+v -0.015708 -0.401398 -0.034372
+vn 0.141482 0.279313 -0.949719
+v -0.015018 -0.401612 -0.034365
+vn 0.070167 0.244276 -0.967164
+v -0.015646 -0.402153 -0.034593
+vn 0.100800 0.365513 -0.925332
+v -0.015116 -0.400992 -0.034177
+vn 0.094275 0.156570 -0.983157
+v -0.015559 -0.402933 -0.034747
+vn 0.127360 0.063444 -0.989825
+v -0.015455 -0.403710 -0.034827
+vn 0.106115 0.258600 -0.960138
+v -0.013099 -0.401854 -0.034048
+vn 0.194261 0.347213 -0.917445
+v -0.013760 -0.400831 -0.033842
+vn 0.153102 0.179876 -0.971702
+v -0.013600 -0.402591 -0.034296
+vn 0.128282 0.644640 -0.753646
+v -0.013695 -0.398546 -0.032534
+vn 0.087764 0.703354 -0.705401
+v -0.013193 -0.398348 -0.032260
+vn 0.149336 0.471561 -0.869097
+v -0.013476 -0.399475 -0.033142
+vn 0.093494 0.352214 -0.931238
+v -0.012981 -0.400635 -0.033613
+vn -0.147697 -0.981934 -0.118282
+v -0.001277 -0.428617 -0.025240
+vn -0.102536 -0.993942 -0.039575
+v -0.000137 -0.428850 -0.023694
+vn 0.180839 0.442576 -0.878307
+v -0.013859 -0.399868 -0.033435
+vn 0.020577 0.698950 -0.714875
+v -0.014433 -0.398427 -0.032523
+vn 0.108941 0.621053 -0.776160
+v -0.014130 -0.398725 -0.032752
+vn 0.108263 0.572351 -0.812831
+v -0.013350 -0.398874 -0.032734
+vn -0.129049 0.644407 -0.753715
+v -0.015282 -0.398983 -0.032990
+vn 0.087833 0.758749 -0.645435
+v -0.013971 -0.397832 -0.031836
+vn 0.114267 0.720991 -0.683458
+v -0.013758 -0.398212 -0.032219
+vn 0.109062 0.441631 -0.890543
+v -0.014835 -0.400351 -0.033858
+vn 0.165362 0.208432 -0.963956
+v -0.014910 -0.402332 -0.034531
+vn -0.042352 0.467814 -0.882812
+v -0.015591 -0.400592 -0.034024
+vn 0.382848 -0.634665 -0.671288
+v 0.010633 -0.426941 -0.026434
+vn 0.202839 0.319331 -0.925680
+v -0.014166 -0.401201 -0.034068
+vn 0.187948 0.264222 -0.945971
+v -0.014474 -0.401733 -0.034295
+vn 0.206697 0.251292 -0.945584
+v -0.014140 -0.401827 -0.034253
+vn 0.145025 0.545522 -0.825454
+v -0.013868 -0.399054 -0.032957
+vn 0.034927 0.600409 -0.798930
+v -0.014656 -0.399027 -0.033057
+vn 0.128362 0.504288 -0.853942
+v -0.014396 -0.399526 -0.033348
+vn 0.177499 0.337875 -0.924302
+v -0.014607 -0.401032 -0.034100
+vn 0.180746 0.405448 -0.896071
+v -0.014275 -0.400401 -0.033780
+vn 0.208945 0.185619 -0.960150
+v -0.014378 -0.402381 -0.034432
+vn 0.203512 0.261953 -0.943379
+v -0.013828 -0.401680 -0.034138
+vn -0.848909 0.345813 -0.399708
+v -0.020138 -0.402171 -0.032097
+vn -0.770674 0.495597 -0.400556
+v -0.019674 -0.400259 -0.030923
+vn -0.980205 0.033164 -0.195189
+v -0.022168 -0.406847 -0.028511
+vn -0.967557 -0.043012 -0.248967
+v -0.021931 -0.408314 -0.029527
+vn -0.977581 -0.104168 -0.182986
+v -0.022047 -0.409278 -0.028728
+vn -0.696782 0.543877 -0.467646
+v -0.019133 -0.400438 -0.032139
+vn -0.366972 0.590646 -0.718658
+v -0.017587 -0.400239 -0.033283
+vn -0.508769 0.467755 -0.722744
+v -0.018452 -0.401473 -0.033674
+vn -0.303338 0.512834 -0.803111
+v -0.017472 -0.401087 -0.033948
+vn -0.568284 0.528960 -0.630282
+v -0.018754 -0.400865 -0.032993
+vn -0.973240 -0.193503 -0.123936
+v -0.022020 -0.410571 -0.027278
+vn -0.861561 0.372113 -0.345318
+v -0.020728 -0.401404 -0.029952
+vn -0.966397 0.103220 -0.235421
+v -0.021894 -0.405519 -0.029358
+vn -0.928152 0.259024 -0.267284
+v -0.021444 -0.402616 -0.029050
+vn -0.982077 0.081315 -0.170034
+v -0.022295 -0.405323 -0.027387
+vn -0.979189 -0.147827 -0.139053
+v -0.022280 -0.410054 -0.026149
+vn -0.938668 0.197374 -0.282746
+v -0.021463 -0.404148 -0.030258
+vn -0.988996 -0.069619 -0.130540
+v -0.022246 -0.408422 -0.027834
+vn -0.962846 -0.230799 -0.140209
+v -0.021782 -0.411114 -0.028322
+vn -0.460183 0.637739 -0.617674
+v -0.017847 -0.399443 -0.032402
+vn -0.581961 0.647713 -0.491721
+v -0.018178 -0.398777 -0.031322
+vn -0.880613 0.297712 -0.368630
+v -0.020832 -0.402979 -0.031181
+vn -0.085201 0.726573 -0.681786
+v -0.014924 -0.398129 -0.032210
+vn -0.145892 0.758019 -0.635706
+v -0.015367 -0.397660 -0.031618
+vn -0.256978 0.666857 -0.699474
+v -0.016308 -0.398858 -0.032616
+vn -0.030579 0.536510 -0.843340
+v -0.015158 -0.399722 -0.033538
+vn -0.201367 0.572460 -0.794821
+v -0.016291 -0.400049 -0.033604
+vn -0.923945 0.130707 -0.359501
+v -0.021360 -0.405290 -0.031204
+vn -0.750744 0.402360 -0.523917
+v -0.019593 -0.401734 -0.032767
+vn -0.791280 0.219133 -0.570839
+v -0.020017 -0.403300 -0.033011
+vn -0.959529 0.028353 -0.280179
+v -0.021705 -0.406825 -0.030459
+vn -0.762704 -0.489247 -0.422989
+v -0.018839 -0.416266 -0.030612
+vn -0.745771 -0.376675 -0.549493
+v -0.019225 -0.413319 -0.032575
+vn -0.816706 -0.330660 -0.472921
+v -0.020073 -0.412510 -0.031837
+vn -0.859416 -0.255480 -0.442871
+v -0.020619 -0.411005 -0.031821
+vn -0.842577 -0.117947 -0.525502
+v -0.020547 -0.408631 -0.032876
+vn -0.774080 -0.397421 -0.492805
+v -0.019452 -0.414090 -0.031676
+vn -0.795291 -0.219947 -0.564921
+v -0.020200 -0.410041 -0.032949
+vn -0.439296 -0.784983 -0.436830
+v -0.010505 -0.424113 -0.031208
+vn -0.236180 0.107518 0.965743
+v -0.015526 -0.409316 0.031295
+vn -0.089682 0.025236 0.995651
+v -0.014590 -0.411397 0.031592
+vn -0.782173 -0.538756 -0.312966
+v -0.018428 -0.417570 -0.029608
+vn -0.815128 -0.432308 -0.385584
+v -0.019491 -0.415079 -0.030655
+vn -0.826827 -0.525469 -0.200598
+v -0.019149 -0.417075 -0.028559
+vn -0.811143 -0.486460 -0.324659
+v -0.019309 -0.416262 -0.029525
+vn -0.848460 -0.370456 -0.377992
+v -0.020215 -0.413641 -0.030626
+vn -0.863305 -0.411743 -0.291842
+v -0.020189 -0.414798 -0.029351
+vn -0.702820 -0.465189 -0.538185
+v -0.018421 -0.415164 -0.032220
+vn -0.700250 -0.490798 -0.518427
+v -0.018582 -0.415996 -0.031264
+vn -0.898682 -0.147275 -0.413135
+v -0.021066 -0.409294 -0.031725
+vn -0.934610 -0.130006 -0.331062
+v -0.021539 -0.409301 -0.030557
+vn -0.940070 -0.211465 -0.267488
+v -0.021637 -0.410770 -0.029416
+vn -0.910192 -0.344031 -0.230634
+v -0.021031 -0.413184 -0.028897
+vn -0.893928 -0.283111 -0.347479
+v -0.020990 -0.411888 -0.030380
+vn -0.141364 0.451721 -0.880889
+v -0.016517 -0.401136 -0.034219
+vn -0.064622 0.314695 -0.946991
+v -0.016505 -0.401942 -0.034549
+vn -0.236251 0.379860 -0.894367
+v -0.017391 -0.401855 -0.034375
+vn -0.590842 0.141400 -0.794300
+v -0.019261 -0.403976 -0.033939
+vn -0.660941 0.311452 -0.682755
+v -0.019284 -0.402281 -0.033426
+vn -0.321667 0.218241 0.921358
+v -0.016834 -0.405835 0.030235
+vn -0.917414 -0.005372 -0.397897
+v -0.021324 -0.407235 -0.031547
+vn -0.875962 0.154814 -0.456862
+v -0.020793 -0.404756 -0.032224
+vn -0.712732 -0.242729 -0.658100
+v -0.019331 -0.410997 -0.033645
+vn -0.770380 -0.290190 -0.567718
+v -0.019769 -0.411573 -0.032852
+vn -0.698054 -0.179401 -0.693207
+v -0.019641 -0.409370 -0.033813
+vn -0.870530 0.014100 -0.491913
+v -0.020775 -0.407005 -0.032657
+vn 0.686938 -0.410770 -0.599487
+v 0.029751 -0.418275 -0.012975
+vn 0.119769 0.104344 -0.987303
+v -0.013578 -0.403879 -0.034480
+vn -0.415737 0.387608 -0.822753
+v -0.018140 -0.401927 -0.034131
+vn -0.734891 -0.099314 -0.670874
+v -0.019938 -0.408054 -0.033781
+vn -0.588203 0.052726 -0.806992
+v -0.019458 -0.406929 -0.034267
+vn 0.610560 -0.473048 -0.635171
+v 0.026025 -0.418894 -0.015745
+vn -0.638101 -0.674675 -0.371000
+v -0.014626 -0.421365 -0.030781
+vn -0.649880 -0.590788 -0.478148
+v -0.015388 -0.419928 -0.031782
+vn 0.171400 -0.922032 -0.347102
+v 0.008064 -0.428093 -0.025970
+vn -0.696194 -0.350904 -0.626242
+v -0.018981 -0.412680 -0.033299
+vn -0.313478 0.058770 0.947775
+v -0.014242 -0.407668 0.031532
+vn -0.194022 -0.030271 0.980530
+v -0.013465 -0.408337 0.031759
+vn -0.581181 -0.513518 -0.631291
+v -0.015613 -0.418746 -0.032721
+vn -0.405347 0.192818 0.893597
+v -0.015719 -0.404898 0.030458
+vn 0.147099 -0.974224 -0.171025
+v 0.008480 -0.428362 -0.024732
+vn -0.151382 0.031207 -0.987983
+v -0.017173 -0.404453 -0.034843
+vn 0.054292 0.020615 -0.998312
+v -0.015910 -0.404726 -0.034903
+vn -0.671199 -0.391202 -0.629645
+v -0.018373 -0.413689 -0.033334
+vn 0.105026 0.050198 -0.993202
+v -0.013724 -0.405486 -0.034618
+vn -0.695504 -0.560766 -0.449238
+v -0.017118 -0.418574 -0.030528
+vn -0.522768 -0.319039 -0.790523
+v -0.017094 -0.414254 -0.034207
+vn -0.704833 -0.484457 -0.518182
+v -0.016679 -0.417653 -0.032468
+vn -0.688401 -0.397558 -0.606673
+v -0.017569 -0.415013 -0.033399
+vn -0.599225 -0.429570 -0.675574
+v -0.016589 -0.416484 -0.033468
+vn -0.716855 -0.607466 -0.342205
+v -0.015751 -0.420322 -0.030447
+vn -0.743477 -0.560278 -0.365144
+v -0.016124 -0.419333 -0.031406
+vn -0.744077 -0.487449 -0.456883
+v -0.017323 -0.417071 -0.031976
+vn 0.021180 -0.959500 -0.280911
+v 0.002809 -0.428319 -0.027163
+vn -0.014331 -0.912912 -0.407905
+v 0.001131 -0.427908 -0.028232
+vn 0.072791 -0.996657 0.037093
+v 0.005314 -0.428887 -0.019179
+vn 0.074935 -0.987488 -0.138750
+v 0.005554 -0.428669 -0.024817
+vn -0.680956 -0.631856 -0.370212
+v -0.016380 -0.420400 -0.029167
+vn -0.659760 -0.677411 -0.325318
+v -0.015162 -0.421630 -0.029160
+vn -0.743692 -0.623576 -0.240989
+v -0.016914 -0.419988 -0.028713
+vn -0.666953 -0.716059 -0.205993
+v -0.015108 -0.422119 -0.028110
+vn -0.571324 -0.793436 -0.209877
+v -0.013014 -0.423839 -0.028087
+vn -0.624276 -0.776935 -0.081552
+v -0.013660 -0.423592 -0.026906
+vn -0.829015 -0.543850 0.130234
+v -0.022701 -0.415948 0.023882
+vn -0.832183 -0.554493 -0.003154
+v -0.022661 -0.416146 0.022195
+vn -0.927053 -0.309947 0.210964
+v -0.023945 -0.413203 0.024496
+vn -0.033683 -0.927314 0.372765
+v 0.004849 -0.427374 -0.012125
+vn -0.280467 -0.503404 -0.817265
+v -0.011197 -0.422460 -0.032615
+vn -0.117478 -0.954029 0.275731
+v 0.001018 -0.427968 -0.014754
+vn -0.139530 -0.919266 0.368079
+v 0.001569 -0.427632 -0.013557
+vn -0.108862 -0.968994 0.221809
+v 0.000857 -0.428303 -0.016087
+vn -0.028375 -0.945104 0.325536
+v 0.004798 -0.427798 -0.013317
+vn -0.218158 -0.958849 0.181703
+v -0.003612 -0.427894 -0.017551
+vn -0.307725 -0.943047 0.126363
+v -0.006330 -0.427430 -0.019267
+vn -0.069388 -0.954058 0.291477
+v 0.002957 -0.427931 -0.014014
+vn -0.087823 -0.933604 0.347376
+v 0.003097 -0.427624 -0.013041
+vn -0.160905 -0.950504 0.265803
+v -0.000949 -0.427911 -0.015513
+vn -0.156982 -0.968813 0.191725
+v -0.001409 -0.428187 -0.016948
+vn -0.214204 -0.970328 0.112162
+v -0.003748 -0.428135 -0.019415
+vn -0.185538 -0.836916 0.514924
+v 0.001741 -0.426216 -0.010838
+vn -0.266051 -0.788661 0.554284
+v -0.002677 -0.425867 -0.012132
+vn -0.221246 -0.853896 0.471075
+v -0.001903 -0.426411 -0.012652
+vn -0.336291 0.044814 0.940691
+v -0.019433 -0.410834 0.030380
+vn -0.293018 0.136130 0.946366
+v -0.020026 -0.409151 0.030013
+vn -0.184618 -0.868233 0.460530
+v 0.001414 -0.427271 -0.012849
+vn -0.118548 -0.884038 0.452133
+v 0.003824 -0.427177 -0.011860
+vn -0.295762 -0.932132 0.208938
+v -0.005562 -0.427416 -0.017773
+vn -0.366848 -0.896478 0.248496
+v -0.007073 -0.426876 -0.017775
+vn -0.346248 -0.894446 0.282981
+v -0.006702 -0.426715 -0.016808
+vn -0.282371 -0.904958 0.318304
+v -0.004623 -0.427316 -0.016425
+vn -0.255626 -0.893083 0.370214
+v -0.003854 -0.427058 -0.015155
+vn -0.314444 -0.890389 0.329139
+v -0.006029 -0.426633 -0.015792
+vn -0.344523 -0.895017 0.283281
+v -0.007427 -0.425940 -0.015356
+vn -0.231048 -0.885973 0.402080
+v -0.001640 -0.427421 -0.014607
+vn -0.246693 -0.886980 0.390397
+v -0.003960 -0.426697 -0.014369
+vn -0.340881 -0.881969 0.325471
+v -0.006855 -0.426228 -0.015581
+vn -0.429034 -0.863640 0.264679
+v -0.009661 -0.425358 -0.016678
+vn -0.404752 -0.858513 0.314852
+v -0.008703 -0.425487 -0.015636
+vn -0.405477 -0.900555 0.156806
+v -0.008318 -0.426613 -0.019031
+vn -0.435738 -0.824916 0.360064
+v -0.009690 -0.425048 -0.015733
+vn -0.470802 -0.848657 0.241094
+v -0.011218 -0.424589 -0.016799
+vn -0.492687 -0.813325 0.309455
+v -0.011726 -0.424197 -0.016487
+vn -0.396343 -0.879546 0.263269
+v -0.008477 -0.426017 -0.016933
+vn -0.465690 -0.860762 0.205481
+v -0.010915 -0.425087 -0.017961
+vn -0.146772 -0.854384 0.498483
+v 0.003679 -0.426532 -0.010762
+vn -0.265530 -0.887320 0.377037
+v -0.005149 -0.426329 -0.014314
+vn -0.189432 -0.909216 0.370732
+v -0.000337 -0.427663 -0.014438
+vn -0.229493 -0.928056 0.293335
+v -0.002798 -0.427709 -0.016081
+vn -0.201067 -0.871467 0.447345
+v -0.000424 -0.426898 -0.012936
+vn -0.634690 -0.597623 0.489914
+v -0.014978 -0.420269 -0.014010
+vn 0.016339 0.032858 -0.999326
+v -0.012340 -0.406847 -0.034581
+vn -0.514480 -0.767410 0.382612
+v -0.012368 -0.423392 -0.015539
+vn -0.554902 -0.709973 0.433614
+v -0.013463 -0.422002 -0.014511
+vn -0.334721 -0.802791 0.493445
+v -0.006184 -0.425454 -0.013484
+vn -0.421093 -0.795590 0.435566
+v -0.009331 -0.424915 -0.015108
+vn -0.291255 -0.821228 0.490668
+v -0.004750 -0.426096 -0.013580
+vn -0.270690 -0.766793 0.582027
+v -0.004125 -0.425146 -0.011836
+vn -0.494841 -0.789702 0.362635
+v -0.011903 -0.423874 -0.015971
+vn -0.338186 -0.848033 0.408008
+v -0.006586 -0.425995 -0.014650
+vn -0.376842 -0.798946 0.468696
+v -0.007398 -0.425631 -0.014647
+vn -0.446295 -0.458799 0.768326
+v 0.005162 -0.421765 -0.005438
+vn -0.276655 -0.655833 0.702386
+v 0.001417 -0.424866 -0.009330
+vn -0.262071 -0.704335 0.659720
+v -0.001044 -0.425206 -0.010622
+vn -0.317077 -0.755760 -0.572965
+v 0.005532 -0.420189 0.005443
+vn -0.233458 -0.796341 -0.557977
+v 0.004529 -0.421211 0.007296
+vn -0.178666 -0.788277 -0.588810
+v -0.001546 -0.420828 0.008864
+vn -0.186517 -0.827463 -0.529638
+v 0.002337 -0.421912 0.009148
+vn -0.202449 -0.819881 -0.535546
+v -0.006634 -0.421454 0.011550
+vn -0.219266 -0.834932 -0.504789
+v -0.007895 -0.420737 0.010895
+vn -0.183489 -0.831096 -0.524987
+v -0.002572 -0.421628 0.010360
+vn -0.187697 -0.819263 -0.541829
+v -0.005903 -0.420454 0.009645
+vn -0.184748 -0.827387 -0.530376
+v -0.004395 -0.421050 0.010065
+vn -0.175998 -0.838280 -0.516054
+v -0.000830 -0.422245 0.010757
+vn -0.198113 -0.854110 -0.480882
+v 0.006154 -0.421852 0.007621
+vn -0.190604 -0.799615 -0.569462
+v 0.000537 -0.421363 0.008945
+vn 0.205326 -0.886772 0.414098
+v 0.001100 -0.425852 0.026606
+vn 0.186271 -0.395603 0.899334
+v 0.011812 -0.420951 0.026920
+vn 0.106771 -0.351534 0.930066
+v 0.013534 -0.421397 0.026465
+vn 0.079382 -0.391931 0.916564
+v 0.013511 -0.422191 0.026165
+vn 0.040563 -0.305533 0.951317
+v 0.015162 -0.421490 0.026319
+vn -0.468060 -0.825087 -0.316465
+v -0.015043 -0.421838 0.017811
+vn 0.172969 -0.516384 0.838707
+v 0.009705 -0.423102 0.026262
+vn -0.438624 -0.854704 -0.277652
+v -0.014242 -0.422464 0.018370
+vn 0.241707 -0.524295 0.816512
+v 0.007062 -0.422528 0.027302
+vn -0.228206 0.105194 -0.967913
+v -0.015609 -0.408058 0.008483
+vn -0.389427 -0.067481 -0.918582
+v -0.018416 -0.411177 0.009133
+vn -0.421456 -0.006927 -0.906823
+v -0.019086 -0.409648 0.009388
+vn -0.281913 0.052401 -0.958008
+v -0.017555 -0.408976 0.008861
+vn 0.120416 -0.710649 0.693166
+v 0.008654 -0.424965 0.025068
+vn 0.116292 -0.628663 0.768934
+v 0.010002 -0.424577 0.025196
+vn 0.188736 -0.595437 0.780918
+v 0.007735 -0.423899 0.026169
+vn -0.257078 -0.303252 -0.917578
+v -0.010599 -0.415762 0.006757
+vn 0.090092 -0.566851 0.818880
+v 0.011552 -0.424298 0.025220
+vn -0.362403 -0.254090 -0.896718
+v -0.013041 -0.414972 0.007314
+vn -0.208924 0.008513 -0.977895
+v -0.008863 -0.414634 0.006152
+vn -0.823983 -0.502112 -0.262555
+v -0.022577 -0.415569 0.018791
+vn -0.300617 -0.398578 -0.866467
+v -0.011387 -0.415917 0.007038
+vn -0.429661 -0.183765 -0.884094
+v -0.016474 -0.413310 0.008575
+vn -0.543661 -0.127650 -0.829541
+v -0.019205 -0.411375 0.009556
+vn -0.039535 -0.951040 -0.306528
+v -0.000072 -0.428342 -0.026999
+vn 0.119133 -0.852606 0.508793
+v 0.006390 -0.425689 0.024635
+vn 0.173066 -0.709721 0.682894
+v 0.006712 -0.424834 0.025616
+vn 0.233057 -0.730186 0.642271
+v 0.004746 -0.424959 0.026076
+vn -0.694249 -0.111856 -0.710990
+v -0.019727 -0.410548 0.009900
+vn -0.264960 0.086460 -0.960375
+v -0.016021 -0.409899 0.008362
+vn 0.064552 -0.520052 0.851692
+v 0.013127 -0.424099 0.025216
+vn 0.088461 -0.631537 0.770283
+v 0.014820 -0.424540 0.024790
+vn -0.662414 -0.717597 -0.215085
+v -0.019943 -0.419007 0.019647
+vn -0.657153 -0.703867 -0.269669
+v -0.020143 -0.418544 0.018672
+vn -0.710275 -0.617733 -0.337515
+v -0.020974 -0.417388 0.018092
+vn -0.561518 -0.777240 -0.283893
+v -0.017974 -0.420172 0.018356
+vn -0.538112 -0.792248 -0.287713
+v -0.017232 -0.420916 0.018999
+vn -0.954637 -0.224556 -0.195555
+v -0.024465 -0.411205 0.019212
+vn -0.688235 -0.717515 -0.107260
+v -0.020018 -0.419223 0.020701
+vn -0.275785 0.124527 0.953119
+v -0.019300 -0.409799 0.030326
+vn -0.932326 -0.002415 0.361612
+v -0.024073 -0.408679 0.026851
+vn -0.957235 0.099100 0.271811
+v -0.024609 -0.405579 0.024755
+vn -0.997185 -0.055154 0.050785
+v -0.025147 -0.408184 0.021974
+vn -0.964364 0.005652 0.264520
+v -0.024601 -0.407769 0.025225
+vn -0.982723 -0.077067 0.168273
+v -0.024879 -0.409105 0.023780
+vn -0.922855 0.090522 0.374358
+v -0.024126 -0.406378 0.026432
+vn -0.702336 0.251280 0.666020
+v -0.022886 -0.404006 0.027681
+vn -0.839329 0.173298 0.515261
+v -0.023530 -0.405093 0.027245
+vn -0.969909 -0.112494 -0.215923
+v -0.024781 -0.408630 0.018685
+vn -0.978437 0.113151 0.172792
+v -0.024915 -0.405182 0.023261
+vn -0.984923 0.016167 0.172238
+v -0.024995 -0.406931 0.023422
+vn -0.913936 -0.402860 -0.049243
+v -0.023886 -0.413901 0.021598
+vn -0.857723 -0.501732 -0.112145
+v -0.022990 -0.415506 0.020498
+vn -0.900695 -0.430651 0.057349
+v -0.023656 -0.414388 0.023230
+vn -0.960974 -0.265006 -0.079379
+v -0.024576 -0.411796 0.021034
+vn -0.909147 -0.376534 -0.177971
+v -0.023798 -0.413505 0.019590
+vn -0.956974 -0.284258 0.058290
+v -0.024405 -0.412479 0.022800
+vn -0.959438 -0.112682 0.258420
+v -0.024381 -0.410004 0.025654
+vn -0.846639 -0.040926 0.530591
+v -0.023592 -0.409491 0.027830
+vn -0.398470 -0.841418 0.365018
+v -0.013125 -0.423228 0.028575
+vn -0.985566 -0.161397 0.051095
+v -0.024897 -0.410381 0.022348
+vn 0.069899 -0.997501 -0.010264
+v 0.014421 -0.421529 0.002495
+vn 0.074411 -0.977605 -0.196854
+v 0.013484 -0.421934 0.005613
+vn -0.844105 0.077693 0.530519
+v -0.023584 -0.407523 0.027729
+vn -0.963401 -0.194026 0.184967
+v -0.024513 -0.411271 0.024210
+vn -0.996250 -0.024699 -0.082926
+v -0.025166 -0.407216 0.020398
+vn -0.985740 -0.138773 -0.095179
+v -0.024986 -0.409560 0.020567
+vn -0.996736 0.056446 0.057716
+v -0.025148 -0.406049 0.021910
+vn -0.935369 -0.136880 -0.326112
+v -0.024242 -0.408685 0.016815
+vn -0.977480 0.185038 0.101461
+v -0.024853 -0.404067 0.022422
+vn -0.597880 -0.760896 -0.252142
+v -0.018727 -0.420172 0.019924
+vn -0.912938 0.187661 0.362391
+v -0.024035 -0.404106 0.025810
+vn -0.825567 0.285082 0.486998
+v -0.023326 -0.402709 0.026505
+vn -0.820926 0.382196 0.424271
+v -0.022886 -0.400237 0.025473
+vn -0.764927 -0.614576 -0.192832
+v -0.021706 -0.417126 0.019492
+vn -0.901994 0.286185 0.323272
+v -0.023793 -0.402038 0.025018
+vn -0.771199 -0.630920 -0.084809
+v -0.021650 -0.417469 0.020821
+vn -0.966648 -0.020292 -0.255303
+v -0.024682 -0.406413 0.017579
+vn -0.939107 0.108020 -0.326205
+v -0.024295 -0.404377 0.016602
+vn -0.987672 0.040034 -0.151330
+v -0.024998 -0.405844 0.019070
+vn -0.950398 0.199800 0.238376
+v -0.024482 -0.403719 0.024136
+vn -0.947091 0.272782 0.169143
+v -0.024266 -0.402164 0.023335
+vn -0.699188 -0.685180 0.204119
+v -0.019414 -0.419504 0.025644
+vn -0.738562 -0.657367 0.149647
+v -0.020459 -0.418722 0.024364
+vn -0.699942 0.436926 0.564958
+v -0.022101 -0.399398 0.025971
+vn -0.572061 -0.657135 0.490836
+v -0.016676 -0.420470 0.029359
+vn -0.991473 0.127451 -0.027148
+v -0.025064 -0.404915 0.020708
+vn -0.975086 0.204234 -0.086582
+v -0.024718 -0.403344 0.019424
+vn -0.968347 0.247180 0.034716
+v -0.024637 -0.402799 0.021489
+vn -0.528930 -0.806611 0.263842
+v -0.015033 -0.422451 0.027913
+vn -0.494894 -0.757515 0.425736
+v -0.015047 -0.421875 0.029119
+vn -0.455441 -0.375195 0.807343
+v -0.017184 -0.419107 0.030043
+vn -0.880600 0.473698 0.012412
+v -0.022926 -0.398409 0.021641
+vn -0.879939 0.470058 -0.068934
+v -0.022961 -0.398603 0.020140
+vn -0.506845 0.595048 0.623720
+v -0.020916 -0.397053 0.025127
+vn -0.611897 0.528731 0.588240
+v -0.021484 -0.397922 0.025407
+vn -0.839367 0.532562 0.108817
+v -0.022402 -0.397613 0.022512
+vn -0.782524 0.583537 0.217120
+v -0.021680 -0.396835 0.023426
+vn -0.588827 -0.806130 0.058632
+v -0.017904 -0.421151 0.023644
+vn -0.908270 0.411958 -0.073049
+v -0.023637 -0.400075 0.019528
+vn -0.936797 0.338454 0.088654
+v -0.023969 -0.400854 0.022395
+vn -0.892159 0.430135 -0.137970
+v -0.023322 -0.399793 0.017964
+vn -0.896297 0.425922 0.123461
+v -0.023191 -0.399105 0.022835
+vn -0.868398 0.481339 -0.119154
+v -0.022768 -0.398591 0.018456
+vn -0.899755 0.362779 0.242555
+v -0.023485 -0.400310 0.024025
+vn -0.840977 0.470482 0.267215
+v -0.022633 -0.398531 0.024020
+vn -0.916927 -0.149662 0.369926
+v -0.023874 -0.410451 0.027075
+vn -0.646643 -0.762746 0.008449
+v -0.018857 -0.420440 0.022966
+vn -0.670384 -0.733477 0.112232
+v -0.019123 -0.420113 0.024094
+vn -0.770385 0.499855 0.395791
+v -0.022046 -0.398163 0.024888
+vn -0.841299 0.529165 -0.110455
+v -0.022057 -0.397250 0.018998
+vn -0.844825 0.534063 -0.032362
+v -0.022269 -0.397325 0.020802
+vn -0.405683 0.528990 0.745379
+v -0.020893 -0.397780 0.025709
+vn -0.792894 0.589554 -0.154094
+v -0.020944 -0.395864 0.017949
+vn -0.915638 0.402003 0.000920
+v -0.023574 -0.399754 0.021298
+vn -0.691188 0.600605 0.401911
+v -0.021272 -0.396897 0.024545
+vn -0.820139 0.545081 -0.173952
+v -0.021652 -0.397055 0.017373
+vn -0.362490 0.592687 0.719252
+v -0.020306 -0.396878 0.025336
+vn -0.498833 0.682140 0.534649
+v -0.020523 -0.396257 0.024590
+vn -0.353493 0.684282 0.637810
+v -0.019758 -0.396027 0.024836
+vn -0.731005 0.669419 0.132326
+v -0.020853 -0.395499 0.022237
+vn -0.966406 0.136718 -0.217643
+v -0.024612 -0.404071 0.017826
+vn -0.904935 0.371713 -0.207177
+v -0.023610 -0.400599 0.017483
+vn -0.559020 0.424562 0.712210
+v -0.021883 -0.400209 0.026710
+vn -0.488485 0.494264 0.719087
+v -0.021474 -0.398860 0.026120
+vn -0.795347 -0.219256 0.565111
+v -0.023309 -0.410947 0.027878
+vn -0.405592 -0.885189 -0.227895
+v -0.013712 -0.423081 0.019731
+vn -0.615947 0.772255 -0.155663
+v -0.019095 -0.394064 0.016960
+vn -0.742850 0.662923 -0.093312
+v -0.020404 -0.395008 0.018854
+vn -0.728934 0.662295 -0.173264
+v -0.019854 -0.394724 0.017238
+vn -0.745134 0.666907 -0.003224
+v -0.020725 -0.395225 0.020578
+vn -0.659791 0.684803 0.309386
+v -0.020768 -0.395846 0.023608
+vn -0.572889 0.780776 0.249373
+v -0.020086 -0.394914 0.022674
+vn -0.946788 0.321257 -0.019643
+v -0.024249 -0.401467 0.020534
+vn -0.939189 0.329206 -0.097707
+v -0.024120 -0.401409 0.018752
+vn -0.806050 0.587664 -0.070238
+v -0.021378 -0.396102 0.019752
+vn -0.642524 0.766021 -0.019341
+v -0.019843 -0.394388 0.019723
+vn -0.802354 0.594930 0.047828
+v -0.021588 -0.396321 0.021592
+vn -0.261399 0.927512 0.267194
+v -0.018697 -0.394058 0.022073
+vn 0.119053 -0.958554 -0.258847
+v 0.006575 -0.428406 -0.025691
+vn -0.063816 0.978058 0.198318
+v -0.018015 -0.393746 0.021221
+vn -0.069120 0.990892 0.115567
+v -0.017821 -0.393529 0.019907
+vn -0.452382 0.101144 0.886070
+v -0.021462 -0.408427 0.029441
+vn -0.529111 -0.007314 0.848521
+v -0.021924 -0.409534 0.029262
+vn -0.157216 0.846210 0.509129
+v -0.018426 -0.394900 0.024012
+vn -0.212211 0.878718 0.427577
+v -0.018655 -0.394460 0.023138
+vn -0.286077 0.956623 0.055066
+v -0.018422 -0.393595 0.019502
+vn -0.643826 -0.137350 0.752744
+v -0.022584 -0.410576 0.028730
+vn -0.269920 0.952681 0.139792
+v -0.018584 -0.393770 0.020840
+vn -0.300511 0.953689 -0.013031
+v -0.018254 -0.393514 0.018139
+vn -0.479357 0.875450 0.061681
+v -0.019218 -0.393974 0.020385
+vn -0.490314 0.871436 -0.013836
+v -0.019004 -0.393828 0.018956
+vn -0.448234 0.873948 0.187887
+v -0.019343 -0.394220 0.021752
+vn -0.629420 0.772516 0.083956
+v -0.020042 -0.394595 0.021265
+vn -0.376439 0.848601 0.371712
+v -0.019339 -0.394618 0.022999
+vn -0.453320 0.765068 0.457354
+v -0.019967 -0.395387 0.023868
+vn -0.640217 0.763478 -0.084987
+v -0.019501 -0.394214 0.018194
+vn 0.004410 0.970402 -0.241454
+v -0.016228 -0.393703 0.014465
+vn -0.477438 0.858746 -0.186031
+v -0.018439 -0.393785 0.016238
+vn -0.211942 0.879322 -0.426467
+v -0.017161 -0.394208 0.013126
+vn -0.485577 0.870585 -0.079353
+v -0.018732 -0.393747 0.017570
+vn -0.140338 0.988021 -0.064184
+v -0.017598 -0.393404 0.017092
+vn -0.320448 0.941146 -0.107504
+v -0.018124 -0.393536 0.016928
+vn -0.103404 0.957748 -0.268376
+v -0.017017 -0.393786 0.014255
+vn -0.074629 0.902893 -0.423337
+v -0.016426 -0.394271 0.012855
+vn -0.355730 0.880088 -0.314486
+v -0.017592 -0.394006 0.013963
+vn -0.666079 0.708451 -0.233314
+v -0.019057 -0.394319 0.015915
+vn -0.509212 0.820538 -0.259655
+v -0.018221 -0.394006 0.014900
+vn -0.258993 0.947072 -0.189678
+v -0.017675 -0.393620 0.015559
+vn -0.507046 0.717225 -0.478009
+v -0.017721 -0.394601 0.012898
+vn -0.086546 0.846772 -0.524869
+v -0.015465 -0.394742 0.011825
+vn -0.168492 0.779799 -0.602930
+v -0.016452 -0.395074 0.011553
+vn -0.320591 0.766783 -0.556117
+v -0.017165 -0.394801 0.012188
+vn -0.185738 -0.956944 -0.223070
+v -0.003466 -0.427904 -0.027146
+vn -0.197509 -0.923806 -0.327981
+v -0.004919 -0.427027 -0.029158
+vn -0.755900 0.443832 -0.481278
+v -0.020192 -0.398041 0.012432
+vn -0.576295 0.562460 -0.592894
+v -0.017910 -0.396092 0.011294
+vn 0.133874 -0.990974 -0.007010
+v 0.007442 -0.428647 -0.021858
+vn -0.875597 0.364080 -0.317451
+v -0.023124 -0.400160 0.016499
+vn -0.857184 0.446275 -0.257050
+v -0.022638 -0.398965 0.016747
+vn -0.851630 0.389393 -0.350855
+v -0.022352 -0.399615 0.015156
+vn -0.903874 0.223022 -0.365065
+v -0.023525 -0.402448 0.015492
+vn -0.808274 0.442668 -0.388249
+v -0.021118 -0.398466 0.013644
+vn -0.762772 0.605224 -0.227778
+v -0.020222 -0.395504 0.016203
+vn -0.680369 0.503700 -0.532339
+v -0.018821 -0.396712 0.011749
+vn -0.574395 0.439727 -0.690443
+v -0.018137 -0.396796 0.010971
+vn -0.306503 -0.204376 0.929670
+v -0.016898 -0.416614 0.030893
+vn -0.688676 0.669073 -0.279406
+v -0.019004 -0.394677 0.014793
+vn -0.580604 0.723621 -0.373191
+v -0.018225 -0.394447 0.013776
+vn -0.716261 0.585676 -0.379412
+v -0.019092 -0.395158 0.014008
+vn -0.770588 0.556793 -0.310121
+v -0.020145 -0.395925 0.015014
+vn -0.679751 0.580947 -0.447705
+v -0.018679 -0.395313 0.013138
+vn -0.730119 0.518442 -0.445134
+v -0.019480 -0.396621 0.012786
+vn -0.331819 0.418906 -0.845230
+v -0.017258 -0.396690 0.010510
+vn -0.601929 0.608443 -0.517184
+v -0.018083 -0.395553 0.012080
+vn -0.781181 0.496324 -0.378706
+v -0.020275 -0.396741 0.014106
+vn -0.819710 0.460290 -0.340894
+v -0.021631 -0.397895 0.015555
+vn -0.384528 0.681462 -0.622694
+v -0.017265 -0.395549 0.011344
+vn -0.405462 0.526774 -0.747067
+v -0.017426 -0.396170 0.010871
+vn -0.902724 -0.042217 -0.428143
+v -0.023465 -0.406733 0.014555
+vn -0.689219 0.221570 -0.689843
+v -0.020135 -0.400468 0.010909
+vn -0.699400 0.136065 -0.701659
+v -0.020455 -0.402544 0.010668
+vn -0.913858 0.042746 -0.403779
+v -0.023930 -0.405191 0.015551
+vn -0.867438 0.303943 -0.393916
+v -0.022566 -0.400927 0.014303
+vn -0.905815 -0.110534 -0.409001
+v -0.023791 -0.408039 0.015498
+vn -0.933895 -0.032403 -0.356077
+v -0.024310 -0.406543 0.016421
+vn -0.866604 0.027994 -0.498210
+v -0.023067 -0.405452 0.013732
+vn -0.896605 0.119726 -0.426338
+v -0.023365 -0.403705 0.014584
+vn -0.811909 0.210178 -0.544637
+v -0.021744 -0.401432 0.012530
+vn -0.942852 0.258168 -0.210663
+v -0.024162 -0.402050 0.017727
+vn -0.911914 0.256428 -0.320402
+v -0.023814 -0.402054 0.016552
+vn -0.858284 0.177803 -0.481387
+v -0.022646 -0.402501 0.013579
+vn -0.568605 0.294440 -0.768111
+v -0.018915 -0.399070 0.010371
+vn -0.820988 0.337912 -0.460212
+v -0.021605 -0.400003 0.013118
+vn -0.765059 0.359672 -0.534155
+v -0.020631 -0.399344 0.012034
+vn -0.383319 0.307715 -0.870849
+v -0.017947 -0.398383 0.010057
+vn -0.750066 0.243778 -0.614795
+v -0.020805 -0.400414 0.011656
+vn -0.851814 0.490845 -0.182986
+v -0.022408 -0.398314 0.017238
+vn -0.809498 0.529793 -0.253048
+v -0.021412 -0.397130 0.016230
+vn -0.678670 0.423015 -0.600388
+v -0.019294 -0.397819 0.011415
+vn -0.532626 0.390588 -0.750833
+v -0.018294 -0.397729 0.010565
+vn -0.614611 -0.774475 0.149804
+v -0.017973 -0.420887 0.025282
+vn -0.670563 0.348837 -0.654720
+v -0.019683 -0.398949 0.011107
+vn -0.580153 0.200702 -0.789393
+v -0.019370 -0.400792 0.010152
+vn -0.380334 0.219602 -0.898399
+v -0.018350 -0.399765 0.009840
+vn -0.407648 0.156246 -0.899672
+v -0.018787 -0.401486 0.009653
+vn -0.197894 0.151274 -0.968480
+v -0.017941 -0.401009 0.009480
+vn -0.203180 0.598302 -0.775082
+v -0.016674 -0.395872 0.010814
+vn -0.170174 0.253966 -0.952125
+v -0.014813 -0.397896 0.009843
+vn -0.317774 0.311818 -0.895427
+v -0.013514 -0.397751 0.009523
+vn -0.051685 0.337110 -0.940045
+v -0.015106 -0.396506 0.010242
+vn -0.105159 0.338351 -0.935126
+v -0.014332 -0.396383 0.010245
+vn 0.036292 0.127288 -0.991202
+v -0.015953 -0.399922 0.009571
+vn -0.012323 0.195044 -0.980717
+v -0.015710 -0.398276 0.009818
+vn -0.108868 0.150541 -0.982591
+v -0.015552 -0.399699 0.009616
+vn -0.170963 0.212835 -0.962015
+v -0.017385 -0.398936 0.009759
+vn -0.243115 0.341589 -0.907861
+v -0.017153 -0.397518 0.010113
+vn -0.513575 -0.652720 -0.556953
+v -0.013060 -0.421938 -0.032000
+vn -0.064584 0.249296 -0.966271
+v -0.016332 -0.397778 0.009920
+vn -0.111403 0.103652 -0.988355
+v -0.017584 -0.402467 0.009240
+vn -0.048265 0.157258 -0.986377
+v -0.016917 -0.399905 0.009541
+vn -0.334997 -0.920942 0.199104
+v -0.011966 -0.424158 0.027188
+vn -0.315978 0.085217 -0.944932
+v -0.018616 -0.403126 0.009380
+vn -0.270608 -0.100836 -0.957394
+v -0.011435 -0.414675 0.006699
+vn -0.298914 -0.230748 -0.925962
+v -0.011516 -0.414991 0.006800
+vn -0.243306 -0.362636 -0.899609
+v -0.008984 -0.416002 0.006423
+vn -0.491416 -0.737580 -0.463127
+v -0.012024 -0.422922 -0.031643
+vn -0.408064 -0.633921 -0.656984
+v -0.011957 -0.422546 -0.032195
+vn -0.375389 -0.688999 -0.619971
+v -0.010619 -0.423454 -0.032065
+vn -0.217035 0.053782 -0.974681
+v -0.010945 -0.414374 0.006592
+vn -0.235905 -0.320057 -0.917558
+v -0.008002 -0.415921 0.006122
+vn -0.209680 -0.105036 -0.972112
+v -0.008568 -0.415419 0.006127
+vn -0.186773 -0.117305 -0.975374
+v -0.006831 -0.415808 0.005811
+vn -0.217525 -0.437157 -0.872683
+v -0.005903 -0.416645 0.005892
+vn -0.242011 -0.410250 -0.879276
+v -0.007641 -0.416244 0.006162
+vn -0.260680 -0.319687 -0.910959
+v -0.009779 -0.415468 0.006436
+vn -0.219920 -0.428816 -0.876215
+v -0.007795 -0.416740 0.006435
+vn -0.220860 -0.128045 -0.966864
+v -0.009972 -0.415160 0.006402
+vn -0.605806 -0.514476 -0.606888
+v 0.004704 -0.416900 0.002590
+vn -0.105913 -0.958328 -0.265312
+v -0.001483 -0.428269 -0.026868
+vn -0.245301 -0.953822 -0.173355
+v -0.009376 -0.424882 0.020679
+vn -0.283070 0.188157 -0.940462
+v -0.014694 -0.399274 0.009517
+vn -0.104367 0.590818 -0.800026
+v -0.014736 -0.395669 0.010650
+vn -0.092476 0.756370 -0.647574
+v -0.013642 -0.395244 0.010919
+vn -0.134049 0.439622 -0.888123
+v -0.016078 -0.396422 0.010368
+vn -0.104434 0.553605 -0.826205
+v -0.013915 -0.395611 0.010589
+vn -0.211542 0.427675 -0.878831
+v -0.013450 -0.396294 0.010125
+vn -0.119287 0.542926 -0.831265
+v -0.013361 -0.395554 0.010538
+vn -0.090103 0.626189 -0.774448
+v -0.012569 -0.395480 0.010518
+vn -0.092317 0.759643 -0.643755
+v -0.012983 -0.395225 0.010819
+vn -0.123356 0.683806 -0.719161
+v -0.015635 -0.395459 0.010960
+vn -0.084773 0.775192 -0.626012
+v -0.014521 -0.395113 0.011178
+vn -0.081477 0.843802 -0.530434
+v -0.012849 -0.395020 0.011127
+vn -0.105532 0.871034 0.479752
+v -0.017651 -0.394799 0.024001
+vn -0.099797 0.869582 -0.483598
+v -0.012412 -0.394828 0.011371
+vn 0.049513 -0.146243 0.988009
+v 0.022123 -0.417451 0.027187
+vn 0.235167 0.017984 0.971789
+v 0.031286 -0.410089 0.026614
+vn 0.249304 0.072610 0.965699
+v 0.031427 -0.409390 0.026549
+vn 0.176678 -0.017741 0.984109
+v 0.029661 -0.411008 0.026964
+vn 0.333139 0.217559 0.917435
+v 0.030681 -0.407587 0.026445
+vn 0.371082 -0.025728 0.928243
+v 0.031950 -0.410110 0.026406
+vn 0.847872 0.027493 0.529487
+v 0.034733 -0.408211 0.023981
+vn 0.931299 0.000918 0.364256
+v 0.035390 -0.407931 0.022678
+vn 0.474174 0.092174 0.875593
+v 0.032550 -0.409225 0.026107
+vn 0.318577 0.192926 0.928056
+v 0.031881 -0.408737 0.026315
+vn 0.342690 0.067588 0.937014
+v 0.032121 -0.409451 0.026344
+vn 0.233945 -0.819554 -0.523068
+v 0.007889 -0.427614 -0.027005
+vn 0.566843 0.228275 0.791568
+v 0.033513 -0.408021 0.025288
+vn 0.166530 -0.984137 -0.061172
+v 0.010489 -0.428187 -0.023510
+vn 0.144475 -0.988937 -0.033622
+v 0.009131 -0.428405 -0.023297
+vn 0.647981 0.051010 0.759946
+v 0.033594 -0.408984 0.025381
+vn 0.731666 0.342925 0.589125
+v 0.034697 -0.404579 0.022620
+vn 0.676128 0.329309 0.659096
+v 0.034363 -0.405984 0.023771
+vn 0.842000 0.165946 0.513321
+v 0.034943 -0.406987 0.023481
+vn 0.669033 0.242390 0.702596
+v 0.034188 -0.407286 0.024497
+vn 0.537805 0.087478 0.838518
+v 0.033093 -0.409026 0.025748
+vn 0.588187 -0.028121 0.808236
+v 0.033169 -0.409903 0.025709
+vn 0.878891 -0.383691 0.283429
+v 0.034584 -0.412380 0.022375
+vn 0.857707 -0.201664 0.472939
+v 0.034415 -0.410653 0.024151
+vn 0.824839 -0.067615 0.561310
+v 0.034364 -0.409505 0.024494
+vn 0.759427 -0.317113 0.568076
+v 0.033710 -0.411556 0.024828
+vn 0.234518 -0.959069 -0.158708
+v 0.011125 -0.428029 -0.023895
+vn 0.783701 -0.447882 0.430365
+v 0.033453 -0.413567 0.023630
+vn 0.765463 -0.528375 0.367270
+v 0.032589 -0.415369 0.023129
+vn 0.821956 -0.361507 0.440115
+v 0.034101 -0.411974 0.023914
+vn 0.083612 -0.993674 -0.074980
+v 0.005285 -0.428823 -0.023421
+vn 0.650941 -0.293084 0.700270
+v 0.032968 -0.411714 0.025551
+vn 0.906930 -0.102000 0.408747
+v 0.034994 -0.409422 0.023399
+vn 0.643327 -0.139640 0.752749
+v 0.033143 -0.410747 0.025652
+vn 0.720642 -0.055271 0.691101
+v 0.033753 -0.409699 0.025214
+vn 0.559253 -0.363511 0.745048
+v 0.031728 -0.412849 0.026069
+vn 0.676624 -0.414464 0.608604
+v 0.032777 -0.412953 0.025055
+vn 0.752999 -0.157998 0.638772
+v 0.033800 -0.410636 0.025031
+vn 0.748049 0.065666 0.660387
+v 0.034087 -0.408543 0.024857
+vn 0.911428 -0.245988 0.329830
+v 0.034963 -0.410833 0.022846
+vn 0.986512 -0.102875 0.127325
+v 0.036444 -0.407587 0.018497
+vn 0.940893 -0.279118 0.191869
+v 0.035431 -0.411007 0.021017
+vn 0.905607 -0.376097 -0.196031
+v 0.035641 -0.410404 0.014381
+vn 0.895925 -0.426149 0.125366
+v 0.034907 -0.412681 0.020403
+vn 0.967761 -0.033291 0.249659
+v 0.035993 -0.407707 0.020817
+vn 0.968376 0.054465 0.243478
+v 0.036240 -0.405946 0.019795
+vn 0.902821 0.152032 0.402245
+v 0.035493 -0.406115 0.022052
+vn 0.945853 -0.319764 0.055789
+v 0.035696 -0.411130 0.018752
+vn 0.974422 -0.179971 0.134584
+v 0.036041 -0.409388 0.019644
+vn 0.979773 -0.194219 -0.048198
+v 0.036778 -0.406865 0.014175
+vn 0.987150 -0.155434 0.037077
+v 0.036685 -0.407318 0.016119
+vn 0.997554 0.056359 -0.041358
+v 0.036881 -0.405703 0.013959
+vn 0.962996 0.267241 -0.034941
+v 0.036676 -0.404534 0.014504
+vn 0.997833 0.031378 0.057835
+v 0.036830 -0.405738 0.015496
+vn 0.953581 -0.137369 0.267981
+v 0.035583 -0.409402 0.021799
+vn 0.991524 0.023684 0.127744
+v 0.036636 -0.405848 0.017582
+vn 0.967526 -0.252720 -0.004995
+v 0.036230 -0.409391 0.016833
+vn 0.863901 0.437828 0.248962
+v 0.034727 -0.400523 0.018602
+vn 0.943357 0.316551 0.099367
+v 0.036208 -0.403250 0.017595
+vn 0.812249 0.442369 0.380212
+v 0.034969 -0.401809 0.019878
+vn 0.891688 0.353696 0.282474
+v 0.035805 -0.403057 0.019297
+vn 0.899108 0.232712 0.370742
+v 0.035757 -0.404415 0.020668
+vn 0.763469 0.400700 0.506512
+v 0.034938 -0.403135 0.021291
+vn 0.965969 0.170045 0.194908
+v 0.036322 -0.404391 0.018661
+vn 0.976794 0.203447 0.066957
+v 0.036573 -0.404370 0.016610
+vn 0.812888 0.543623 -0.209013
+v 0.034744 -0.401903 0.009372
+vn 0.889668 0.456551 0.007211
+v 0.035232 -0.400987 0.016354
+vn 0.834305 0.528775 -0.155987
+v 0.034372 -0.400087 0.013712
+vn 0.841402 0.510685 -0.176758
+v 0.033844 -0.399753 0.012199
+vn 0.819091 0.550803 -0.160332
+v 0.034748 -0.401217 0.011606
+vn 0.796328 0.569283 -0.204398
+v 0.034007 -0.400670 0.010120
+vn 0.789395 0.558070 -0.255760
+v 0.033213 -0.400034 0.008990
+vn 0.875002 0.472693 -0.104559
+v 0.035359 -0.401630 0.013697
+vn 0.755072 0.577259 -0.310867
+v 0.030688 -0.399759 0.003872
+vn 0.744903 0.598362 -0.295097
+v 0.033106 -0.400565 0.007550
+vn 0.764932 0.589846 -0.258767
+v 0.033876 -0.401248 0.008137
+vn 0.737888 0.597848 -0.313209
+v 0.033253 -0.401490 0.006066
+vn 0.744306 0.591570 -0.309925
+v 0.032745 -0.402156 0.003619
+vn 0.708915 0.641376 -0.293387
+v 0.031726 -0.401491 0.002632
+vn 0.710116 0.622744 -0.328521
+v 0.032117 -0.400864 0.004787
+vn 0.954222 -0.078408 -0.288640
+v 0.034171 -0.405613 0.004669
+vn 0.796511 0.533814 -0.283924
+v 0.034371 -0.402324 0.007225
+vn 0.809879 0.487065 -0.326899
+v 0.033817 -0.402782 0.005083
+vn 0.929704 0.173659 -0.324796
+v 0.034157 -0.403967 0.004758
+vn 0.858891 0.419976 -0.293132
+v 0.033247 -0.403378 0.002785
+vn 0.981510 0.136856 -0.133821
+v 0.033178 -0.405140 0.000515
+vn 0.769102 0.582014 -0.264087
+v 0.032276 -0.402739 0.001225
+vn 0.996924 0.078185 -0.005391
+v 0.033166 -0.405673 -0.000738
+vn 0.993096 0.105103 0.052098
+v 0.033273 -0.406242 -0.002492
+vn 0.992276 -0.098622 0.075245
+v 0.033178 -0.406825 -0.001116
+vn 0.829654 0.538734 -0.146422
+v 0.032611 -0.404645 -0.002772
+vn 0.957684 0.130878 -0.256346
+v 0.033523 -0.404647 0.002479
+vn 0.942054 0.326743 -0.075985
+v 0.033018 -0.405104 -0.001516
+vn 0.881037 0.429057 -0.199208
+v 0.032811 -0.404072 0.000129
+vn 0.995094 0.054063 0.082852
+v 0.033913 -0.408963 -0.006960
+vn 0.972056 -0.116964 0.203534
+v 0.033728 -0.409292 -0.005723
+vn 0.990407 0.111041 0.082249
+v 0.033575 -0.407359 -0.004738
+vn 0.923716 0.378753 -0.057405
+v 0.033156 -0.405857 -0.003891
+vn 0.946612 0.321544 -0.023143
+v 0.033517 -0.407048 -0.006163
+vn 0.974289 -0.127151 0.185992
+v 0.033373 -0.408096 -0.003447
+vn 0.929296 0.348803 -0.121429
+v 0.033653 -0.407885 -0.009001
+vn 0.895020 0.439413 -0.076527
+v 0.033359 -0.406904 -0.007639
+vn 0.839877 0.532127 -0.106995
+v 0.032831 -0.405652 -0.005571
+vn 0.878923 0.458629 -0.130970
+v 0.033125 -0.406762 -0.008987
+vn 0.849062 0.522573 -0.077529
+v 0.032773 -0.405927 -0.007660
+vn 0.849387 0.524130 -0.061880
+v 0.032174 -0.404849 -0.006227
+vn 0.967943 -0.244374 -0.058028
+v 0.033804 -0.412225 -0.010252
+vn 0.998507 -0.007512 0.054112
+v 0.034062 -0.409967 -0.008613
+vn 0.985088 0.156982 -0.070415
+v 0.033995 -0.409221 -0.009225
+vn 0.973741 0.227425 -0.010348
+v 0.033849 -0.408239 -0.007967
+vn 0.998224 -0.055405 -0.021899
+v 0.034050 -0.410745 -0.009666
+vn 0.912836 -0.309073 0.266840
+v 0.033503 -0.411560 -0.006938
+vn 0.918216 -0.344410 0.195607
+v 0.033484 -0.412657 -0.008339
+vn 0.928240 -0.355398 0.109833
+v 0.033422 -0.413317 -0.009467
+vn 0.975804 -0.181108 0.122501
+v 0.033908 -0.411202 -0.008357
+vn 0.973317 -0.219765 0.066013
+v 0.033885 -0.411850 -0.009463
+vn 0.935878 -0.350490 -0.035909
+v 0.033374 -0.413559 -0.010310
+vn 0.887803 -0.305249 -0.344427
+v 0.033265 -0.413487 -0.010999
+vn 0.888955 -0.457974 -0.004352
+v 0.032657 -0.415142 -0.010472
+vn 0.885249 -0.399725 -0.237812
+v 0.032745 -0.414847 -0.010998
+vn 0.938248 -0.224769 -0.263001
+v 0.033748 -0.412234 -0.010714
+vn 0.789686 0.121614 -0.601336
+v 0.032809 -0.409401 -0.012247
+vn 0.941112 0.266197 -0.208442
+v 0.033793 -0.408813 -0.009901
+vn 0.892983 0.289097 -0.344969
+v 0.033540 -0.408673 -0.010551
+vn -0.139767 -0.972090 -0.188429
+v 0.012444 -0.421549 0.003849
+vn -0.050383 -0.997788 -0.043366
+v 0.001173 -0.428963 -0.023921
+vn -0.062876 -0.981596 -0.180321
+v 0.000118 -0.428734 -0.025459
+vn 0.985660 -0.068554 -0.154189
+v 0.033957 -0.411268 -0.010349
+vn 0.828615 0.230717 -0.510066
+v 0.032958 -0.408758 -0.011806
+vn 0.892941 0.382755 -0.236969
+v 0.033360 -0.407658 -0.009950
+vn 0.887969 0.148885 -0.435137
+v 0.033646 -0.409764 -0.010921
+vn -0.247427 -0.825904 -0.506620
+v -0.007530 -0.425370 -0.031307
+vn -0.093853 -0.679027 -0.728089
+v -0.006736 -0.425048 -0.031897
+vn 0.867331 0.355908 -0.347947
+v 0.032995 -0.407533 -0.010852
+vn 0.931179 0.180557 -0.316708
+v 0.033833 -0.409595 -0.010425
+vn 0.868116 0.262944 -0.420994
+v 0.033259 -0.408695 -0.011197
+vn 0.834807 0.297769 -0.463066
+v 0.032600 -0.407663 -0.011787
+vn 0.981602 0.092139 -0.167239
+v 0.033992 -0.410036 -0.010047
+vn 0.928292 0.058516 -0.367219
+v 0.033874 -0.410591 -0.010636
+vn 0.865705 0.339201 -0.368100
+v 0.032172 -0.406260 -0.011540
+vn 0.886461 0.450298 -0.106858
+v 0.032479 -0.405615 -0.009123
+vn 0.935531 0.345970 -0.071323
+v 0.031666 -0.403890 -0.009255
+vn 0.886314 0.462625 -0.020638
+v 0.032221 -0.405035 -0.007890
+vn 0.877008 0.421336 -0.230940
+v 0.032726 -0.406459 -0.010190
+vn 0.897792 0.440171 -0.014812
+v 0.031310 -0.403232 -0.004688
+vn 0.939994 0.339449 0.034428
+v 0.030367 -0.401156 -0.002910
+vn 0.839552 0.529325 -0.122341
+v 0.031009 -0.402246 -0.001789
+vn 0.806177 0.575682 -0.136638
+v 0.031985 -0.404128 -0.003883
+vn 0.838308 0.518532 -0.168418
+v 0.030502 -0.400708 0.000965
+vn 0.783891 0.589093 -0.196176
+v 0.031970 -0.403332 -0.001276
+vn 0.856522 0.468970 -0.215493
+v 0.030351 -0.399644 0.003057
+vn 0.751394 0.619694 -0.226683
+v 0.031342 -0.401944 0.000512
+vn 0.705586 0.623486 -0.336769
+v 0.031237 -0.400250 0.004129
+vn 0.744109 0.615537 -0.259646
+v 0.030910 -0.400700 0.002426
+vn -0.767596 -0.501349 0.399306
+v 0.006317 -0.418224 -0.001999
+vn -0.780200 -0.564101 0.270329
+v 0.005787 -0.417196 -0.001644
+vn 0.241560 -0.966794 -0.083420
+v 0.012547 -0.427835 -0.022527
+vn -0.622814 -0.319520 0.714150
+v 0.004511 -0.418949 -0.004554
+vn -0.408088 -0.260523 0.874981
+v 0.001178 -0.421807 -0.007564
+vn -0.158392 -0.019967 0.987174
+v -0.004667 -0.421536 -0.009302
+vn -0.199852 -0.168127 0.965294
+v -0.003879 -0.421941 -0.009195
+vn -0.732632 -0.327576 0.596610
+v -0.020843 -0.415318 0.028492
+vn -0.109417 0.072896 0.991319
+v -0.005931 -0.420914 -0.009488
+vn -0.122271 0.000701 0.992497
+v -0.007588 -0.420277 -0.009687
+vn -0.187255 -0.079066 0.979124
+v -0.008973 -0.419633 -0.009863
+vn -0.169770 -0.091668 0.981211
+v -0.006418 -0.420960 -0.009545
+vn -0.118002 0.132356 0.984153
+v -0.005637 -0.420482 -0.009511
+vn -0.093507 0.073351 0.992913
+v -0.008676 -0.419510 -0.009823
+vn -0.056827 0.132064 0.989611
+v -0.007243 -0.420111 -0.009671
+vn -0.133430 0.246923 0.959805
+v -0.011081 -0.417244 -0.010444
+vn -0.059426 0.219231 0.973862
+v -0.009688 -0.417873 -0.010152
+vn -0.157351 0.257466 0.953390
+v -0.011481 -0.415871 -0.010879
+vn -0.175608 0.255856 0.950631
+v -0.011976 -0.416312 -0.010844
+vn -0.214504 0.231534 0.948884
+v -0.012545 -0.416476 -0.010916
+vn -0.185431 0.238487 0.953278
+v -0.012608 -0.415336 -0.011239
+vn -0.120721 0.097169 0.987919
+v -0.009853 -0.418655 -0.010011
+vn -0.081047 0.212992 0.973687
+v -0.010219 -0.418094 -0.010139
+vn -0.277943 0.112603 0.953975
+v -0.012066 -0.417375 -0.010601
+vn -0.290295 0.152230 0.944751
+v -0.013058 -0.416598 -0.011022
+vn -0.198274 0.096932 0.975342
+v -0.014810 -0.414696 -0.011804
+vn -0.208911 0.218303 0.953258
+v -0.013361 -0.415627 -0.011318
+vn -0.342627 0.007227 0.939444
+v -0.012527 -0.417645 -0.010731
+vn -0.313201 -0.087270 0.945669
+v -0.011474 -0.418442 -0.010390
+vn -0.180531 0.230074 0.956282
+v -0.011593 -0.417293 -0.010509
+vn -0.253404 0.024271 0.967056
+v -0.011103 -0.418142 -0.010281
+vn -0.177318 0.143244 0.973673
+v -0.010892 -0.417923 -0.010247
+vn -0.764108 -0.581825 0.278600
+v -0.020633 -0.417976 0.025990
+vn -0.162440 0.160021 0.973656
+v -0.013058 -0.414164 -0.011592
+vn -0.129089 0.019683 0.991438
+v -0.015721 -0.413319 -0.012037
+vn -0.176105 0.192222 0.965421
+v -0.013739 -0.414878 -0.011578
+vn -0.489380 -0.027939 0.871623
+v 0.000502 -0.419840 -0.007664
+vn -0.563308 -0.132706 0.815520
+v 0.002200 -0.419786 -0.006567
+vn -0.627038 -0.246632 0.738915
+v 0.003216 -0.418542 -0.005502
+vn -0.587429 -0.231247 0.775533
+v 0.003515 -0.419369 -0.005514
+vn -0.776847 -0.454612 0.435703
+v -0.020447 -0.417116 0.027733
+vn -0.472007 -0.095589 0.876397
+v 0.000970 -0.420749 -0.007443
+vn -0.387426 -0.213049 0.896945
+v 0.000446 -0.421745 -0.007886
+vn -0.326268 -0.184188 0.927159
+v -0.000567 -0.421905 -0.008315
+vn -0.382077 -0.122065 0.916034
+v -0.000084 -0.421456 -0.008040
+vn -0.280492 -0.036318 0.959169
+v -0.001957 -0.421583 -0.008719
+vn -0.364116 0.007909 0.931320
+v -0.000988 -0.420736 -0.008371
+vn -0.181245 0.047373 0.982296
+v -0.004123 -0.421315 -0.009220
+vn -0.139584 0.152201 0.978443
+v -0.005626 -0.419279 -0.009708
+vn -0.042575 0.179494 0.982837
+v -0.007080 -0.419451 -0.009776
+vn -0.033841 0.177440 0.983550
+v -0.008660 -0.418983 -0.009891
+vn -0.069366 0.232559 0.970106
+v -0.009562 -0.417015 -0.010347
+vn -0.118115 0.249392 0.961172
+v -0.010672 -0.416828 -0.010498
+vn -0.168562 0.209745 0.963117
+v -0.011643 -0.414680 -0.011223
+vn 0.114477 0.205615 0.971914
+v -0.012333 -0.398335 -0.012377
+vn 0.070195 0.197885 0.977709
+v -0.011941 -0.398337 -0.012429
+vn -0.035911 0.118471 0.992308
+v -0.011675 -0.399849 -0.012197
+vn 0.112098 0.267374 0.957050
+v -0.012369 -0.397896 -0.012484
+vn 0.014931 -0.040267 0.999077
+v -0.016178 -0.406093 -0.011772
+vn -0.028524 -0.032341 0.999070
+v -0.015568 -0.410631 -0.011991
+vn -0.089721 -0.061378 0.994074
+v -0.016765 -0.410112 -0.012029
+vn -0.094029 -0.033756 0.994997
+v -0.016437 -0.411696 -0.012089
+vn -0.026681 -0.053303 0.998222
+v -0.016476 -0.407598 -0.011852
+vn -0.010323 -0.050170 0.998687
+v -0.016058 -0.409078 -0.011925
+vn -0.068045 -0.039744 0.996890
+v -0.016188 -0.411283 -0.012052
+vn -0.163357 -0.059082 0.984796
+v -0.016784 -0.412135 -0.012154
+vn -0.215736 -0.033033 0.975893
+v -0.016168 -0.413726 -0.012121
+vn -0.238203 -0.088902 0.967138
+v -0.017615 -0.410733 -0.012202
+vn -0.087919 -0.061434 0.994231
+v -0.017377 -0.406702 -0.011858
+vn -0.037524 -0.040469 0.998476
+v -0.016585 -0.405960 -0.011775
+vn -0.320062 -0.059007 0.945557
+v -0.018334 -0.406573 -0.012024
+vn -0.076806 -0.058720 0.995315
+v -0.016970 -0.407802 -0.011897
+vn -0.210598 -0.078919 0.974382
+v -0.017900 -0.408056 -0.012026
+vn -0.128302 0.091824 0.987475
+v -0.014444 -0.413892 -0.011845
+vn -0.076286 0.014779 0.996976
+v -0.015232 -0.412290 -0.012006
+vn -0.132877 -0.070342 0.988633
+v -0.017246 -0.409672 -0.012051
+vn -0.500101 -0.058667 0.863977
+v -0.018832 -0.407140 -0.012280
+vn -0.386155 -0.104145 0.916536
+v -0.018408 -0.409273 -0.012300
+vn -0.238395 -0.085742 0.967376
+v -0.017056 -0.412645 -0.012246
+vn -0.295475 -0.098292 0.950281
+v -0.016800 -0.413732 -0.012282
+vn -0.125446 -0.989358 0.073710
+v 0.013792 -0.421521 0.000966
+vn -0.451908 -0.195485 0.870382
+v -0.017016 -0.414853 -0.012533
+vn -0.465065 -0.154455 0.871699
+v -0.016019 -0.415957 -0.012261
+vn -0.570758 -0.354273 0.740760
+v -0.015341 -0.417668 -0.012279
+vn -0.577574 -0.285783 0.764680
+v -0.017623 -0.415218 -0.013010
+vn -0.572848 -0.297015 0.763955
+v -0.016612 -0.416506 -0.012762
+vn -0.363322 -0.060437 0.929701
+v -0.015710 -0.415371 -0.012062
+vn -0.422065 -0.157922 0.892705
+v -0.017536 -0.413581 -0.012525
+vn -0.429516 -0.035740 0.902351
+v -0.014717 -0.416597 -0.011668
+vn -0.407525 -0.127542 0.904244
+v -0.012952 -0.418048 -0.010916
+vn -0.344355 -0.116383 0.931598
+v -0.016567 -0.414591 -0.012301
+vn -0.434303 -0.063142 0.898551
+v -0.013948 -0.417274 -0.011325
+vn -0.511443 -0.239935 0.825141
+v -0.014229 -0.417816 -0.011555
+vn -0.510871 -0.179891 0.840625
+v -0.015278 -0.416941 -0.012007
+vn -0.371428 -0.278310 0.885768
+v -0.011657 -0.418911 -0.010537
+vn -0.358283 0.059766 0.931698
+v -0.013482 -0.416882 -0.011129
+vn -0.629713 -0.194019 0.752209
+v -0.019266 -0.412165 -0.013290
+vn -0.712051 -0.119243 0.691928
+v -0.019887 -0.410630 -0.013534
+vn -0.288736 0.104254 0.951715
+v -0.014252 -0.415680 -0.011540
+vn -0.370299 0.034535 0.928270
+v -0.014557 -0.416143 -0.011610
+vn -0.791564 -0.391004 0.469619
+v -0.019605 -0.414552 -0.014921
+vn -0.284637 -0.016484 0.958494
+v -0.015715 -0.414744 -0.012037
+vn -0.357622 -0.120281 0.926088
+v -0.017725 -0.412059 -0.012376
+vn -0.627981 -0.496206 0.599515
+v -0.015614 -0.418750 -0.013282
+vn -0.710644 -0.318377 0.627393
+v -0.019109 -0.413922 -0.013787
+vn -0.558064 -0.226367 0.798325
+v -0.018268 -0.413713 -0.012962
+vn -0.500484 -0.142366 0.853960
+v -0.018606 -0.411469 -0.012701
+vn -0.689646 -0.391383 0.609268
+v -0.018008 -0.415790 -0.013633
+vn -0.713824 -0.469249 0.519866
+v -0.017775 -0.416770 -0.014119
+vn -0.632105 -0.102363 0.768092
+v -0.019451 -0.409887 -0.013028
+vn -0.578579 -0.087134 0.810959
+v -0.019101 -0.408565 -0.012598
+vn -0.786123 -0.467629 0.404146
+v -0.021487 -0.416293 0.026658
+vn -0.634729 -0.438962 0.635949
+v -0.016469 -0.417395 -0.013099
+vn -0.333993 -0.400760 0.853136
+v -0.008795 -0.420835 -0.010165
+vn -0.323101 -0.329237 0.887248
+v -0.009713 -0.419925 -0.010136
+vn -0.333913 -0.516893 0.788241
+v -0.007873 -0.421728 -0.010269
+vn -0.478131 -0.511315 0.714106
+v -0.012517 -0.419976 -0.011596
+vn -0.408439 -0.524404 0.747113
+v -0.010079 -0.420996 -0.010843
+vn -0.234896 -0.204876 0.950184
+v -0.008355 -0.420170 -0.009809
+vn -0.284385 -0.189547 0.939786
+v -0.010134 -0.419284 -0.010094
+vn -0.212148 -0.029187 0.976802
+v -0.010029 -0.418936 -0.010034
+vn -0.515951 -0.431541 0.739978
+v -0.013750 -0.418699 -0.011618
+vn -0.460954 -0.339434 0.819943
+v -0.013040 -0.418557 -0.011078
+vn -0.417977 -0.432777 0.798749
+v -0.011544 -0.419643 -0.010806
+vn -0.570218 -0.477762 0.668278
+v -0.014637 -0.418787 -0.012373
+vn -0.199592 -0.176531 0.963846
+v -0.005189 -0.421543 -0.009388
+vn -0.257263 -0.296798 0.919634
+v -0.007016 -0.421037 -0.009687
+vn -0.271201 -0.432645 0.859807
+v -0.006231 -0.421837 -0.009762
+vn -0.714255 -0.508522 0.480879
+v -0.017161 -0.417958 -0.014432
+vn -0.265920 -0.394042 0.879783
+v -0.001329 -0.422563 -0.008728
+vn -0.253425 -0.498275 0.829155
+v -0.003605 -0.422664 -0.009426
+vn -0.232008 -0.970629 0.063645
+v -0.004469 -0.428185 -0.021925
+vn -0.323911 -0.376464 0.867961
+v 0.000338 -0.422480 -0.008138
+vn -0.335679 -0.240033 0.910881
+v -0.000110 -0.422135 -0.008196
+vn -0.238671 -0.353705 0.904394
+v -0.004617 -0.421949 -0.009363
+vn -0.220712 -0.102115 0.969979
+v -0.003035 -0.421931 -0.009020
+vn -0.240017 -0.315534 0.918058
+v -0.002802 -0.422293 -0.009034
+vn -0.273329 -0.182771 0.944397
+v -0.001504 -0.422164 -0.008665
+vn -0.281782 -0.594162 0.753372
+v 0.000089 -0.424168 -0.009251
+vn -0.448007 -0.208241 0.869439
+v 0.001363 -0.421273 -0.007333
+vn -0.435314 -0.528970 -0.728487
+v 0.003966 -0.417228 0.003409
+vn -0.629403 -0.553011 -0.545921
+v 0.005220 -0.417370 0.002420
+vn -0.605197 -0.697166 -0.384313
+v 0.004859 -0.416652 0.002115
+vn -0.716046 -0.623393 -0.314101
+v 0.005950 -0.417453 0.001288
+vn 0.282874 -0.905978 -0.314939
+v 0.010758 -0.427920 -0.024736
+vn -0.228573 -0.612472 -0.756725
+v -0.007762 -0.417910 0.007181
+vn -0.278039 -0.584618 -0.762179
+v -0.010657 -0.417336 0.007680
+vn -0.266395 -0.535305 -0.801550
+v -0.009929 -0.416845 0.007083
+vn -0.163958 -0.794073 -0.585291
+v -0.003145 -0.420348 0.008659
+vn -0.192391 -0.790887 -0.580933
+v -0.006435 -0.419865 0.008958
+vn -0.228561 -0.695553 -0.681150
+v -0.008488 -0.418416 0.007871
+vn -0.231977 -0.755394 -0.612835
+v -0.009190 -0.418931 0.008704
+vn -0.189369 -0.700616 -0.687951
+v -0.005853 -0.418676 0.007320
+vn -0.239174 -0.436058 -0.867553
+v -0.009451 -0.416516 0.006739
+vn -0.014211 -0.254476 -0.966975
+v -0.011340 -0.418771 -0.034083
+vn -0.503755 -0.617944 -0.603636
+v -0.016441 -0.416620 0.010405
+vn -0.487401 -0.446088 -0.750630
+v -0.016744 -0.415037 0.009404
+vn -0.548934 -0.457220 -0.699730
+v -0.017918 -0.414660 0.010009
+vn -0.618783 -0.606160 -0.499678
+v -0.017717 -0.415944 0.010945
+vn -0.716367 -0.697224 0.026394
+v -0.020031 -0.419340 0.023035
+vn -0.730351 -0.262595 -0.630581
+v -0.019365 -0.412184 0.009938
+vn -0.526418 -0.291938 -0.798534
+v -0.018723 -0.412787 0.009624
+vn -0.053050 0.084188 -0.995037
+v -0.017062 -0.403561 0.009113
+vn -0.238397 -0.828690 -0.506399
+v -0.009063 -0.420105 0.010335
+vn -0.345605 -0.762200 -0.547366
+v -0.012390 -0.418633 0.009860
+vn -0.403433 -0.741179 -0.536558
+v -0.014405 -0.418465 0.010975
+vn -0.315790 -0.786560 -0.530659
+v -0.012074 -0.419300 0.010656
+vn -0.434312 -0.616391 -0.656838
+v -0.014102 -0.416944 0.009009
+vn -0.438122 -0.646584 -0.624482
+v -0.014877 -0.417231 0.009869
+vn -0.245952 -0.796109 -0.552918
+v -0.009577 -0.419480 0.009591
+vn -0.340141 -0.474611 -0.811818
+v -0.012415 -0.416118 0.007543
+vn -0.362149 -0.577471 -0.731693
+v -0.012849 -0.416933 0.008276
+vn -0.283294 -0.671764 -0.684454
+v -0.011007 -0.417955 0.008325
+vn -0.324189 -0.721722 -0.611570
+v -0.011975 -0.418173 0.009027
+vn -0.181854 -0.753702 -0.631556
+v -0.005611 -0.419277 0.007958
+vn -0.801817 -0.020724 -0.597210
+v -0.021948 -0.406279 0.012091
+vn -0.819142 0.068422 -0.569496
+v -0.022422 -0.404244 0.012820
+vn -0.774017 0.032777 -0.632315
+v -0.021259 -0.405158 0.011197
+vn -0.329227 -0.927123 -0.179031
+v -0.007775 -0.426812 -0.026627
+vn -0.552880 0.109850 -0.825988
+v -0.019572 -0.402888 0.009876
+vn 0.337776 -0.846542 -0.411429
+v 0.011115 -0.427613 -0.025131
+vn -0.775454 0.119941 -0.619907
+v -0.021405 -0.402731 0.011720
+vn -0.803294 -0.118099 -0.583756
+v -0.021297 -0.408488 0.011474
+vn -0.799071 -0.201180 -0.566580
+v -0.021571 -0.409876 0.012232
+vn -0.814252 -0.077752 -0.575281
+v -0.022500 -0.407537 0.012947
+vn -0.698165 -0.484336 -0.527241
+v -0.018612 -0.414630 0.010664
+vn -0.708277 -0.384297 -0.592165
+v -0.019013 -0.413445 0.010251
+vn -0.808843 -0.240912 -0.536408
+v -0.019823 -0.411689 0.010400
+vn -0.804286 -0.346252 -0.482943
+v -0.019593 -0.413064 0.010866
+vn 0.228454 -0.462102 0.856896
+v 0.009280 -0.421568 0.027230
+vn 0.156416 -0.433436 0.887506
+v 0.011509 -0.422023 0.026491
+vn -0.377905 -0.852112 -0.362067
+v -0.012229 -0.422983 0.017172
+vn 0.762203 -0.361862 -0.536752
+v 0.020373 -0.424208 -0.020186
+vn 0.590351 -0.611303 -0.527062
+v 0.017849 -0.425475 -0.022279
+vn -0.285141 0.101829 -0.953061
+v -0.013282 -0.408377 0.007846
+vn -0.755712 -0.048924 -0.653074
+v -0.020856 -0.407366 0.010745
+vn -0.592608 -0.001781 -0.805489
+v -0.020100 -0.406926 0.010005
+vn -0.809897 -0.192104 -0.554223
+v -0.020527 -0.410395 0.010929
+vn -0.751879 -0.100141 -0.651652
+v -0.020304 -0.409241 0.010328
+vn -0.845224 0.043249 0.532659
+v -0.020822 -0.407158 -0.014600
+vn -0.868388 0.003108 0.495875
+v -0.021111 -0.408204 -0.015050
+vn -0.762956 0.189291 0.618116
+v -0.019247 -0.402803 -0.013121
+vn -0.750130 0.299312 0.589676
+v -0.018898 -0.400980 -0.013424
+vn -0.526561 0.370874 0.764974
+v -0.017048 -0.399453 -0.012391
+vn -0.849963 0.108999 0.515443
+v -0.020625 -0.405220 -0.014564
+vn -0.807547 0.055229 0.587211
+v -0.020033 -0.406125 -0.013531
+vn -0.548778 0.275426 0.789293
+v -0.017638 -0.401051 -0.012146
+vn -0.370921 0.164625 0.913956
+v -0.017446 -0.401921 -0.011824
+vn -0.689116 0.236976 0.684808
+v -0.018464 -0.401896 -0.012548
+vn -0.883637 -0.272644 0.380592
+v -0.021136 -0.412368 -0.016182
+vn -0.938598 -0.141476 0.314670
+v -0.022050 -0.410115 -0.017289
+vn -0.895225 -0.160074 0.415871
+v -0.021318 -0.410921 -0.015815
+vn -0.916532 -0.050988 0.396698
+v -0.021939 -0.408804 -0.016674
+vn -0.375266 0.361472 0.853530
+v -0.016083 -0.398808 -0.012153
+vn -0.786811 0.364287 0.498220
+v -0.019380 -0.400064 -0.014665
+vn -0.734925 0.392449 0.553054
+v -0.018458 -0.399322 -0.013857
+vn -0.921139 0.051026 0.385875
+v -0.022268 -0.406628 -0.017429
+vn -0.661623 0.339180 0.668739
+v -0.018010 -0.400224 -0.012807
+vn -0.873932 0.085036 0.478552
+v -0.021266 -0.406132 -0.015483
+vn -0.892537 0.027246 0.450151
+v -0.021683 -0.407440 -0.016121
+vn -0.875074 0.167367 0.454130
+v -0.021212 -0.403887 -0.015979
+vn -0.801149 -0.109260 0.588407
+v -0.020373 -0.410554 -0.014106
+vn -0.827812 -0.289685 0.480427
+v -0.020379 -0.412695 -0.014859
+vn -0.759167 -0.221711 0.611972
+v -0.019981 -0.412106 -0.013998
+vn -0.783528 -0.045870 0.619661
+v -0.020240 -0.409354 -0.013782
+vn -0.829264 -0.023058 0.558381
+v -0.020626 -0.408873 -0.014291
+vn -0.747555 -0.028888 0.663571
+v -0.019951 -0.408520 -0.013393
+vn -0.632421 -0.249916 0.733202
+v -0.021912 -0.411993 0.028925
+vn -0.835971 -0.385493 0.390574
+v -0.023173 -0.413859 0.026060
+vn -0.832567 -0.442566 -0.333117
+v -0.022872 -0.414267 0.017581
+vn -0.850641 -0.159070 0.501105
+v -0.020715 -0.411162 -0.014758
+vn -0.850826 -0.064511 0.521473
+v -0.020829 -0.409834 -0.014673
+vn -0.884793 -0.060394 0.462054
+v -0.021300 -0.409516 -0.015462
+vn -0.813967 0.027077 0.580279
+v -0.020391 -0.407529 -0.013949
+vn -0.825808 0.143164 0.545477
+v -0.019960 -0.403800 -0.013814
+vn -0.522100 0.024448 0.852534
+v -0.018604 -0.405243 -0.012109
+vn -0.724678 -0.012819 0.688969
+v -0.019592 -0.407327 -0.012951
+vn -0.677827 0.119213 0.725492
+v -0.018785 -0.403828 -0.012387
+vn -0.762895 0.088741 0.640403
+v -0.019508 -0.404914 -0.012978
+vn -0.675004 0.019108 0.737567
+v -0.019155 -0.406131 -0.012507
+vn -0.581285 0.176469 0.794334
+v -0.018102 -0.402459 -0.012068
+vn -0.482343 0.082480 0.872091
+v -0.018088 -0.403469 -0.011916
+vn -0.822067 0.382505 0.421776
+v -0.019996 -0.399594 -0.016140
+vn -0.768871 -0.325158 0.550554
+v -0.022834 -0.412534 0.027730
+vn -0.842306 0.290313 0.454135
+v -0.020142 -0.401010 -0.015297
+vn -0.892858 0.348696 0.284983
+v -0.021375 -0.400160 -0.018779
+vn -0.849347 0.372405 0.374064
+v -0.020709 -0.399645 -0.017598
+vn -0.437731 0.797661 0.414884
+v -0.015959 -0.395152 -0.015597
+vn -0.931155 0.236006 0.277940
+v -0.022034 -0.402294 -0.018710
+vn -0.596757 0.699978 0.392317
+v -0.017103 -0.395685 -0.016060
+vn -0.419233 0.759495 0.497403
+v -0.015509 -0.395638 -0.014336
+vn -0.269083 0.876221 0.399789
+v -0.014823 -0.394836 -0.015290
+vn -0.785504 0.483628 0.386118
+v -0.019632 -0.397975 -0.017127
+vn -0.669490 0.587321 0.454795
+v -0.017673 -0.396883 -0.015077
+vn -0.759562 0.470766 0.448826
+v -0.018830 -0.398234 -0.015370
+vn -0.551820 0.675621 0.488908
+v -0.016550 -0.396113 -0.014669
+vn -0.859905 0.190901 0.473413
+v -0.020470 -0.402731 -0.015025
+vn -0.698700 0.485205 0.525733
+v -0.017941 -0.398019 -0.014192
+vn -0.621339 0.434538 0.652008
+v -0.017494 -0.398666 -0.013204
+vn -0.592452 0.553313 0.585531
+v -0.017031 -0.397297 -0.013828
+vn -0.810660 0.245178 0.531712
+v -0.019711 -0.401794 -0.014153
+vn -0.897189 0.118293 0.425509
+v -0.021841 -0.405252 -0.016789
+vn -0.732839 -0.602654 0.315842
+v -0.019577 -0.418679 0.027303
+vn -0.896302 0.201847 0.394842
+v -0.021741 -0.403079 -0.017427
+vn -0.869046 0.265545 0.417426
+v -0.020900 -0.401489 -0.016539
+vn -0.887412 0.284796 0.362479
+v -0.021463 -0.401266 -0.017917
+vn -0.930261 0.155636 0.332254
+v -0.022315 -0.404492 -0.018165
+vn -0.763097 -0.646220 0.009122
+v -0.018803 -0.419164 -0.021643
+vn -0.676878 -0.704968 0.211794
+v -0.015886 -0.421732 -0.018493
+vn -0.688939 -0.721881 0.065202
+v -0.016919 -0.421130 -0.020455
+vn -0.845692 -0.521408 0.113749
+v -0.020325 -0.416974 -0.021067
+vn -0.986159 -0.069912 -0.150344
+v -0.022901 -0.408118 -0.023653
+vn -0.894735 -0.444055 -0.047599
+v -0.021110 -0.415735 -0.022619
+vn -0.932469 -0.359524 0.035261
+v -0.021717 -0.414294 -0.021903
+vn -0.973165 -0.215547 -0.080559
+v -0.022517 -0.411487 -0.022896
+vn -0.989437 -0.144804 -0.006728
+v -0.022899 -0.409586 -0.021946
+vn -0.968659 -0.207699 0.136235
+v -0.022465 -0.411089 -0.019798
+vn -0.956920 -0.278763 0.081212
+v -0.022228 -0.412604 -0.020985
+vn -0.915825 -0.323978 0.237284
+v -0.021602 -0.413086 -0.018085
+vn -0.990916 0.005658 0.134366
+v -0.023092 -0.406667 -0.020579
+vn -0.997317 0.071108 -0.017394
+v -0.023120 -0.405766 -0.021620
+vn -0.993573 -0.098480 0.055804
+v -0.023061 -0.408036 -0.021069
+vn -0.823044 -0.492825 0.282351
+v -0.019905 -0.416407 -0.017926
+vn -0.979519 0.048108 0.195521
+v -0.022894 -0.406249 -0.019577
+vn -0.710195 -0.642378 0.288050
+v -0.016715 -0.420470 -0.017501
+vn -0.974312 -0.111003 0.195945
+v -0.022729 -0.408812 -0.019229
+vn -0.957241 -0.040499 0.286443
+v -0.022544 -0.407784 -0.018143
+vn -0.958183 0.087989 0.272293
+v -0.022654 -0.405819 -0.018629
+vn -0.964522 0.188191 0.185154
+v -0.022572 -0.403745 -0.019613
+vn -0.980442 0.168906 0.101013
+v -0.022917 -0.404552 -0.020890
+vn -0.781720 -0.471154 0.408568
+v -0.019127 -0.415966 -0.015469
+vn -0.704435 -0.556958 0.439965
+v -0.016416 -0.419048 -0.014543
+vn -0.704697 -0.588643 0.396108
+v -0.016291 -0.419661 -0.015168
+vn -0.761317 -0.624793 0.173292
+v -0.018344 -0.419443 -0.019719
+vn -0.799093 -0.553286 0.235213
+v -0.019280 -0.417861 -0.018846
+vn -0.884873 -0.435748 0.164694
+v -0.021048 -0.415319 -0.019974
+vn -0.834150 -0.457161 0.308541
+v -0.020372 -0.415101 -0.017135
+vn -0.908473 -0.371875 0.190752
+v -0.021475 -0.413918 -0.019003
+vn -0.620772 -0.715171 0.321204
+v -0.014313 -0.422514 -0.016825
+vn -0.607266 -0.706328 0.363770
+v -0.014117 -0.422183 -0.015798
+vn -0.576874 -0.768332 0.277276
+v -0.013744 -0.423131 -0.017203
+vn -0.857607 -0.361933 0.365398
+v -0.020797 -0.413791 -0.016586
+vn -0.939704 -0.214381 0.266454
+v -0.021956 -0.411575 -0.017860
+vn -0.749896 -0.576153 0.325122
+v -0.017823 -0.418808 -0.016815
+vn -0.764871 -0.526425 0.371281
+v -0.018254 -0.417577 -0.015866
+vn -0.947002 0.320942 -0.013542
+v -0.021915 -0.400452 -0.022192
+vn -0.905151 0.414091 -0.096074
+v -0.021373 -0.399225 -0.023017
+vn -0.959742 0.266437 -0.088918
+v -0.022208 -0.401550 -0.023452
+vn -0.827555 0.541588 -0.147767
+v -0.020554 -0.397971 -0.023679
+vn -0.918386 0.374272 -0.128407
+v -0.021583 -0.400046 -0.024259
+vn -0.767682 0.623557 -0.147787
+v -0.019880 -0.397325 -0.024731
+vn -0.750350 0.619365 -0.231000
+v -0.019676 -0.397581 -0.026720
+vn -0.819075 0.548214 -0.169053
+v -0.020238 -0.398103 -0.025953
+vn -0.728562 0.598549 -0.333071
+v -0.019610 -0.398033 -0.027860
+vn -0.843660 0.477094 -0.246210
+v -0.020474 -0.398961 -0.027226
+vn -0.868212 0.473806 -0.147359
+v -0.020902 -0.398906 -0.025066
+vn -0.940100 0.303490 -0.155262
+v -0.021775 -0.401077 -0.025585
+vn -0.979530 0.197551 -0.038653
+v -0.022734 -0.403450 -0.022325
+vn -0.608198 0.665982 -0.431929
+v -0.018327 -0.397953 -0.029963
+vn -0.739544 0.553811 -0.382580
+v -0.019620 -0.398946 -0.029320
+vn -0.914854 0.325014 -0.239598
+v -0.021334 -0.401127 -0.027739
+vn -0.981808 0.126988 -0.141162
+v -0.022354 -0.403969 -0.026126
+vn -0.897502 0.397693 -0.190604
+v -0.021153 -0.399895 -0.026400
+vn -0.988488 -0.002495 -0.151281
+v -0.022658 -0.406573 -0.025221
+vn -0.971847 0.187827 -0.142251
+v -0.022332 -0.402712 -0.024788
+vn -0.984631 0.106744 -0.138231
+v -0.022790 -0.404601 -0.023783
+vn -0.190832 0.839870 -0.508135
+v -0.015724 -0.396447 -0.029793
+vn -0.617894 0.738266 -0.270502
+v -0.018782 -0.396738 -0.026819
+vn -0.585499 0.724376 -0.363964
+v -0.018292 -0.396959 -0.028251
+vn 0.001843 0.833974 -0.551800
+v -0.014474 -0.396988 -0.030782
+vn -0.174397 0.806649 -0.564715
+v -0.015609 -0.397050 -0.030758
+vn -0.429753 0.797584 -0.423288
+v -0.016870 -0.396452 -0.029102
+vn -0.842157 -0.347991 -0.411915
+v -0.022979 -0.412867 0.016321
+vn -0.756991 -0.476277 -0.447353
+v -0.021566 -0.414880 0.015679
+vn 0.040799 0.945018 -0.324464
+v -0.015057 -0.395786 -0.028526
+vn -0.164221 0.886393 -0.432827
+v -0.015683 -0.395990 -0.029003
+vn -0.968946 -0.184618 -0.164502
+v -0.022354 -0.410894 -0.024706
+vn -0.952801 0.235732 -0.191316
+v -0.021898 -0.402338 -0.026902
+vn -0.990759 -0.027965 -0.132717
+v -0.022457 -0.407316 -0.026707
+vn -0.960597 0.170497 -0.219510
+v -0.021945 -0.403875 -0.028171
+vn -0.995870 0.012926 -0.089869
+v -0.023077 -0.406478 -0.022547
+vn 0.111337 -0.992998 -0.039486
+v 0.007266 -0.428639 -0.023335
+vn -0.406124 0.758009 -0.510379
+v -0.016933 -0.397309 -0.030481
+vn -0.340166 0.719290 -0.605729
+v -0.016731 -0.398063 -0.031606
+vn -0.849593 0.427147 -0.309414
+v -0.020615 -0.400033 -0.028576
+vn -0.962669 0.262350 0.066639
+v -0.022266 -0.401673 -0.020981
+vn -0.232819 0.917982 -0.321098
+v -0.016228 -0.394498 -0.022772
+vn -0.319537 0.922852 -0.215035
+v -0.016644 -0.394299 -0.021723
+vn -0.377906 0.889119 -0.258176
+v -0.017286 -0.394762 -0.022505
+vn 0.057908 0.990303 -0.126284
+v -0.015411 -0.395331 -0.025849
+vn -0.514167 0.853437 0.085304
+v -0.017454 -0.394540 -0.020261
+vn 0.069841 0.977053 -0.201219
+v -0.015097 -0.395292 -0.025401
+vn 0.064883 0.955239 -0.288632
+v -0.014727 -0.395225 -0.025020
+vn -0.029058 0.955148 -0.294701
+v -0.015433 -0.395067 -0.024575
+vn -0.136133 0.927274 -0.348755
+v -0.015824 -0.394778 -0.023686
+vn -0.616852 0.786528 -0.029447
+v -0.018295 -0.395092 -0.021066
+vn -0.682177 0.691784 0.236790
+v -0.018767 -0.395780 -0.019175
+vn -0.543684 0.813924 -0.204783
+v -0.018341 -0.395696 -0.023732
+vn -0.741771 0.654188 -0.147691
+v -0.019801 -0.396743 -0.022729
+vn -0.834953 0.549213 0.034919
+v -0.020441 -0.397421 -0.021080
+vn -0.019965 0.992668 -0.119214
+v -0.015734 -0.395378 -0.026476
+vn -0.112555 0.983723 -0.140072
+v -0.016222 -0.395301 -0.025647
+vn -0.258419 0.949164 -0.179741
+v -0.016614 -0.395481 -0.026537
+vn -0.439782 0.880429 -0.177301
+v -0.017747 -0.395558 -0.024621
+vn -0.341253 0.924922 -0.167530
+v -0.017175 -0.395491 -0.025586
+vn -0.350107 0.905330 -0.240421
+v -0.017288 -0.395198 -0.023878
+vn -0.283861 0.908349 -0.307123
+v -0.016792 -0.394852 -0.023314
+vn -0.547966 0.814119 -0.192209
+v -0.018208 -0.395234 -0.022427
+vn -0.674929 0.730407 -0.104770
+v -0.018978 -0.395769 -0.021953
+vn -0.512689 0.843626 -0.159512
+v -0.017842 -0.394872 -0.021795
+vn -0.729657 0.683800 0.004121
+v -0.019378 -0.396079 -0.021049
+vn -0.766634 0.629452 0.126736
+v -0.019679 -0.396494 -0.020208
+vn -0.650188 0.751068 0.114684
+v -0.018531 -0.395328 -0.020177
+vn -0.798175 0.555111 0.234024
+v -0.020013 -0.397183 -0.019369
+vn -0.828724 0.464348 0.312405
+v -0.020392 -0.398251 -0.018514
+vn -0.225873 0.956078 -0.186807
+v -0.016731 -0.395234 -0.024747
+vn -0.042033 0.979456 -0.197226
+v -0.015827 -0.395189 -0.025029
+vn -0.167674 0.946842 -0.274545
+v -0.016291 -0.395015 -0.024162
+vn -0.455114 0.851847 -0.259284
+v -0.017826 -0.395204 -0.023085
+vn -0.459396 0.883875 -0.087865
+v -0.017374 -0.394485 -0.021097
+vn -0.496578 0.803527 0.328260
+v -0.016487 -0.394879 -0.016821
+vn -0.732700 0.600064 0.321051
+v -0.019163 -0.396648 -0.018148
+vn -0.701960 0.598939 0.385388
+v -0.018371 -0.396618 -0.016609
+vn 0.016935 0.973659 -0.227380
+v -0.015271 -0.395581 -0.027821
+vn -0.145542 0.968796 -0.200627
+v -0.015981 -0.395500 -0.027308
+vn -0.693125 0.700989 -0.167905
+v -0.019259 -0.396863 -0.025711
+vn -0.625160 0.763229 -0.163269
+v -0.018811 -0.396237 -0.024619
+vn -0.537986 0.822956 -0.182525
+v -0.018219 -0.396012 -0.025630
+vn -0.476349 0.842114 -0.252855
+v -0.017664 -0.395945 -0.026733
+vn -0.940942 0.284280 0.183885
+v -0.021940 -0.401055 -0.019758
+vn -0.879473 0.418610 0.226478
+v -0.021091 -0.398956 -0.019582
+vn -0.217817 0.918668 -0.329552
+v -0.015992 -0.395747 -0.028256
+vn 0.153776 0.959065 -0.237795
+v -0.014523 -0.395736 -0.028097
+vn -0.408518 0.859457 -0.307323
+v -0.016928 -0.395869 -0.027702
+vn -0.925475 0.362556 0.109770
+v -0.021586 -0.399711 -0.020404
+vn -0.630731 0.752356 -0.190100
+v -0.018817 -0.395818 -0.022892
+vn -0.706420 0.687297 -0.169093
+v -0.019407 -0.396559 -0.023672
+vn -0.218173 0.965070 -0.145050
+v -0.015629 -0.393872 -0.021068
+vn -0.093832 0.920472 -0.379376
+v -0.015327 -0.394522 -0.023210
+vn -0.012738 0.931671 -0.363079
+v -0.015052 -0.394892 -0.024111
+vn -0.036527 0.922113 -0.385192
+v -0.014803 -0.394290 -0.022744
+vn 0.026355 0.914905 -0.402807
+v -0.014689 -0.394684 -0.023607
+vn -0.456111 0.853251 0.252834
+v -0.016472 -0.394472 -0.018021
+vn -0.859036 0.496763 0.123625
+v -0.020761 -0.398041 -0.020318
+vn -0.916750 0.398176 0.032025
+v -0.021407 -0.399188 -0.021172
+vn -0.886158 0.462970 -0.019572
+v -0.021030 -0.398397 -0.021969
+vn -0.833328 0.541878 -0.109234
+v -0.020620 -0.397768 -0.022607
+vn -0.784511 0.618003 -0.051132
+v -0.020094 -0.396952 -0.021861
+vn -0.401593 0.903292 0.150952
+v -0.016471 -0.394159 -0.019347
+vn -0.554933 0.804276 0.212579
+v -0.017533 -0.394786 -0.019156
+vn -0.614145 0.727189 0.306630
+v -0.017671 -0.395366 -0.017740
+vn -0.347108 0.937535 -0.023317
+v -0.016518 -0.394080 -0.020556
+vn 0.179596 0.978765 -0.098811
+v -0.014435 -0.395558 -0.026704
+vn 0.122746 0.973610 -0.192401
+v -0.013484 -0.395692 -0.026486
+vn 0.180730 0.971412 -0.153933
+v -0.013512 -0.395810 -0.027283
+vn 0.199144 0.972693 -0.119206
+v -0.014052 -0.395694 -0.027262
+vn 0.172613 0.977138 -0.124122
+v -0.013973 -0.395653 -0.026751
+vn 0.067025 0.989520 -0.127898
+v -0.015227 -0.395476 -0.027142
+vn 0.210437 0.949252 -0.233744
+v -0.013813 -0.395899 -0.028175
+vn 0.124241 0.987496 -0.097031
+v -0.015007 -0.395448 -0.026565
+vn 0.119128 0.974272 -0.191318
+v -0.014625 -0.395412 -0.025734
+vn 0.122976 0.965810 -0.228228
+v -0.012676 -0.395922 -0.027106
+vn 0.172561 0.978914 -0.109319
+v -0.014597 -0.395574 -0.027168
+vn 0.143420 0.982494 -0.118899
+v -0.014785 -0.395438 -0.026100
+vn 0.155309 0.978954 -0.132393
+v -0.014342 -0.395529 -0.026240
+vn 0.215338 0.962923 -0.162506
+v -0.013725 -0.395829 -0.027727
+vn 0.145736 0.976458 -0.159033
+v -0.014761 -0.395596 -0.027589
+vn 0.196773 0.967206 -0.160599
+v -0.014215 -0.395719 -0.027696
+vn 0.222619 0.926632 -0.302974
+v -0.013367 -0.396146 -0.028685
+vn -0.387466 0.571261 0.723554
+v -0.017118 -0.397133 0.026789
+vn -0.178715 0.918438 -0.352892
+v -0.012002 -0.394150 0.012675
+vn -0.086311 0.909052 -0.407645
+v -0.012592 -0.394541 0.011961
+vn -0.217830 0.960288 -0.174345
+v -0.012403 -0.393521 0.015271
+vn -0.151228 0.954501 -0.257017
+v -0.012571 -0.393873 0.013833
+vn -0.105392 0.991030 -0.082177
+v -0.013765 -0.393558 0.016670
+vn -0.025694 0.985834 -0.165747
+v -0.013857 -0.393784 0.014697
+vn 0.025343 0.965638 -0.258652
+v -0.014232 -0.394070 0.013302
+vn 0.772951 -0.142034 -0.618364
+v 0.020534 -0.423469 -0.020245
+vn 0.767457 -0.127619 -0.628270
+v 0.020516 -0.422273 -0.020471
+vn 0.074393 0.993981 -0.080421
+v -0.016155 -0.393455 0.016103
+vn 0.060858 0.996545 -0.056520
+v -0.015120 -0.393559 0.016345
+vn 0.502593 -0.367694 -0.782433
+v 0.014417 -0.425200 -0.025338
+vn 0.634975 -0.417728 -0.649854
+v 0.017908 -0.424910 -0.022729
+vn 0.384145 -0.278243 -0.880349
+v 0.009147 -0.421549 -0.029194
+vn 0.521686 -0.565024 -0.639211
+v 0.015786 -0.425718 -0.024016
+vn 0.238849 -0.259456 -0.935753
+v 0.002964 -0.423470 -0.030652
+vn 0.273998 -0.216108 -0.937135
+v 0.003891 -0.424734 -0.030084
+vn 0.213808 -0.169745 -0.962015
+v -0.002078 -0.422518 -0.032074
+vn 0.187089 -0.262502 -0.946621
+v -0.000688 -0.421956 -0.031902
+vn 0.196047 -0.130725 -0.971842
+v -0.004940 -0.421506 -0.032839
+vn 0.227011 -0.130112 -0.965162
+v -0.002986 -0.422931 -0.032229
+vn 0.141026 -0.142142 -0.979749
+v -0.007240 -0.420895 -0.033341
+vn 0.230344 -0.134314 -0.963795
+v -0.003275 -0.423400 -0.032240
+vn 0.027825 -0.232231 -0.972262
+v -0.009123 -0.420883 -0.033525
+vn 0.109293 -0.179289 -0.977707
+v -0.006811 -0.422340 -0.033073
+vn 0.192290 -0.122339 -0.973683
+v -0.005266 -0.422272 -0.032822
+vn 0.052997 -0.212687 -0.975682
+v -0.010312 -0.419172 -0.033959
+vn 0.074160 -0.184356 -0.980058
+v -0.010881 -0.418029 -0.034224
+vn 0.026578 0.999629 -0.006004
+v -0.016939 -0.393361 0.017389
+vn 0.032908 0.996036 0.082643
+v -0.016193 -0.393559 0.019407
+vn -0.503135 -0.791316 -0.347381
+v -0.016107 -0.420934 0.017134
+vn 0.794460 -0.121556 -0.595027
+v 0.022604 -0.422008 -0.017780
+vn 0.791414 -0.112166 -0.600902
+v 0.021760 -0.421981 -0.018908
+vn 0.732160 -0.350062 -0.584292
+v 0.023665 -0.420871 -0.016759
+vn 0.781248 -0.194993 -0.592984
+v 0.022809 -0.421450 -0.017624
+vn 0.740625 -0.276084 -0.612578
+v 0.024348 -0.421481 -0.015665
+vn 0.763152 -0.247033 -0.597138
+v 0.021485 -0.421129 -0.019510
+vn 0.763890 -0.216566 -0.607924
+v 0.023651 -0.421405 -0.016549
+vn 0.853202 0.113072 -0.509177
+v 0.033316 -0.409735 -0.011548
+vn 0.057831 0.908386 -0.414115
+v -0.012645 -0.394631 -0.022963
+vn 0.102561 0.906316 -0.409966
+v -0.012985 -0.394690 -0.023156
+vn 0.083532 0.887075 -0.454006
+v -0.012669 -0.394507 -0.022706
+vn 0.094449 0.910159 -0.403348
+v -0.012187 -0.394730 -0.023126
+vn 0.043710 0.908816 -0.414901
+v -0.012434 -0.394651 -0.022990
+vn 0.077033 0.917880 -0.389311
+v -0.012879 -0.394769 -0.023317
+vn 0.117382 0.910258 -0.397053
+v -0.013503 -0.394794 -0.023537
+vn 0.387445 0.913216 0.126187
+v -0.010994 -0.394087 -0.019441
+vn 0.380918 0.918786 0.103604
+v -0.010650 -0.394219 -0.019626
+vn 0.244181 0.943270 -0.224985
+v -0.010639 -0.394324 -0.021517
+vn 0.227144 0.944078 -0.239004
+v -0.010332 -0.394441 -0.021645
+vn 0.212657 0.952725 -0.217007
+v -0.010924 -0.394210 -0.021313
+vn 0.256015 0.960759 -0.106761
+v -0.010013 -0.394423 -0.021165
+vn 0.352701 0.918720 0.177642
+v -0.010547 -0.394416 -0.018763
+vn 0.304002 0.950286 -0.067381
+v -0.010476 -0.394255 -0.020818
+vn 0.318887 0.946215 -0.054672
+v -0.010904 -0.394079 -0.020371
+vn 0.275489 0.961103 0.019682
+v -0.009766 -0.394467 -0.020613
+vn 0.331724 0.941015 0.066703
+v -0.010218 -0.394346 -0.020057
+vn 0.279478 0.951045 0.131930
+v -0.010051 -0.394508 -0.019278
+vn 0.158355 0.984589 0.074222
+v -0.009458 -0.394592 -0.019892
+vn 0.201357 0.919445 -0.337753
+v -0.010656 -0.394463 -0.021942
+vn 0.180909 0.980061 -0.082169
+v -0.009273 -0.394612 -0.021158
+vn 0.125462 0.981986 0.141292
+v -0.009711 -0.394758 -0.018147
+vn 0.121486 0.935409 -0.332042
+v -0.010252 -0.394884 -0.022872
+vn 0.159436 0.925439 -0.343719
+v -0.010475 -0.394638 -0.022306
+vn 0.175594 0.941875 -0.286422
+v -0.009974 -0.394629 -0.022043
+vn 0.127500 0.919468 -0.371917
+v -0.010749 -0.394805 -0.022828
+vn 0.040874 0.999097 0.011615
+v -0.008828 -0.394650 -0.020289
+vn 0.242646 0.955315 -0.168809
+v -0.009644 -0.394570 -0.021567
+vn 0.297791 0.921039 0.251013
+v -0.011735 -0.394153 -0.017993
+vn 0.348706 0.916500 0.196040
+v -0.011375 -0.394027 -0.018958
+vn 0.308479 0.945537 0.103926
+v -0.011617 -0.393836 -0.019476
+vn 0.280061 0.933359 0.224516
+v -0.011365 -0.394674 -0.016426
+vn 0.355490 0.909240 0.216586
+v -0.011069 -0.394277 -0.018441
+vn 0.291109 0.926938 0.236729
+v -0.011555 -0.394415 -0.017238
+vn 0.246062 0.943350 0.222587
+v -0.011079 -0.394884 -0.015884
+vn 0.191282 0.965851 0.174766
+v -0.010305 -0.394864 -0.016896
+vn 0.263828 0.943231 0.201766
+v -0.010811 -0.394775 -0.016715
+vn 0.265440 0.946712 0.182420
+v -0.010429 -0.394645 -0.017871
+vn 0.316577 0.923597 0.216214
+v -0.010949 -0.394551 -0.017507
+vn 0.136491 0.867313 0.478684
+v -0.011046 -0.395436 -0.014321
+vn 0.189386 0.856858 0.479508
+v -0.011586 -0.395328 -0.014326
+vn 0.093668 0.889430 0.447370
+v -0.010379 -0.395492 -0.014371
+vn 0.185682 0.908506 0.374351
+v -0.011221 -0.395172 -0.014809
+vn 0.137016 0.926618 0.350152
+v -0.010730 -0.395239 -0.014850
+vn 0.232920 0.927292 0.293050
+v -0.011336 -0.394970 -0.015306
+vn 0.262916 0.933145 0.245184
+v -0.011449 -0.394817 -0.015739
+vn 0.252789 0.936378 0.243502
+v -0.011869 -0.394663 -0.015873
+vn 0.146957 0.795546 0.587801
+v -0.011440 -0.395645 -0.013890
+vn 0.070984 0.803325 0.591295
+v -0.010778 -0.395775 -0.013845
+vn 0.147992 0.947002 0.285106
+v -0.012849 -0.394476 -0.015723
+vn 0.227560 0.971039 0.072801
+v -0.012078 -0.393704 -0.019555
+vn 0.271463 0.937566 0.217435
+v -0.011900 -0.393900 -0.018727
+vn 0.200527 0.959631 0.197223
+v -0.012476 -0.393750 -0.018738
+vn -0.027939 0.830170 0.556810
+v -0.009737 -0.395879 -0.013750
+vn -0.084993 0.928466 0.361561
+v -0.008680 -0.395602 -0.014195
+vn 0.169703 0.952449 0.253063
+v -0.013006 -0.394160 -0.016833
+vn 0.205886 0.931365 0.300284
+v -0.012336 -0.394645 -0.015505
+vn 0.194941 0.949318 0.246563
+v -0.012733 -0.394315 -0.016438
+vn 0.235118 0.938318 0.253534
+v -0.012351 -0.394224 -0.017107
+vn 0.095359 0.710583 0.697121
+v -0.011267 -0.395960 -0.013556
+vn 0.163877 0.797257 0.580969
+v -0.011910 -0.395487 -0.013957
+vn 0.215608 0.896261 0.387594
+v -0.011706 -0.395049 -0.014831
+vn 0.240254 0.921428 0.305365
+v -0.011789 -0.394833 -0.015352
+vn 0.064464 0.919442 0.387907
+v -0.012930 -0.394608 -0.015314
+vn 0.167791 0.903069 0.395365
+v -0.012285 -0.394857 -0.014957
+vn 0.210128 0.947122 0.242501
+v -0.012476 -0.394486 -0.015974
+vn 0.140358 0.956954 0.254043
+v -0.013089 -0.394334 -0.016131
+vn 0.248805 0.939279 0.236329
+v -0.012039 -0.394475 -0.016446
+vn 0.099783 0.773064 0.626431
+v -0.012426 -0.395459 -0.013874
+vn 0.152147 0.860289 0.486574
+v -0.012143 -0.395174 -0.014371
+vn 0.185950 0.947314 0.260805
+v -0.010920 -0.395050 -0.015354
+vn 0.200450 0.960092 0.195046
+v -0.010661 -0.394952 -0.016020
+vn 0.156847 0.971292 0.178861
+v -0.010237 -0.395021 -0.016071
+vn 0.232063 0.937608 0.258918
+v -0.012378 -0.393981 -0.017942
+vn 0.152894 0.961259 0.229355
+v -0.010467 -0.395107 -0.015432
+vn 0.114374 0.969875 0.215083
+v -0.009893 -0.395176 -0.015521
+vn 0.131559 0.957561 0.256454
+v -0.013243 -0.394206 -0.016532
+vn 0.100744 0.962001 0.253780
+v -0.013409 -0.394053 -0.017036
+vn 0.128222 0.943421 0.305802
+v -0.010235 -0.395272 -0.014945
+vn 0.042125 0.879185 0.474615
+v -0.012952 -0.394967 -0.014543
+vn 0.012686 0.945510 0.325346
+v -0.013396 -0.394359 -0.015920
+vn -0.118079 0.916830 0.381419
+v -0.013796 -0.394532 -0.015514
+vn -0.138324 0.985103 -0.102164
+v -0.013952 -0.393519 -0.020468
+vn 0.080078 0.992599 0.091295
+v -0.013233 -0.393497 -0.019626
+vn -0.122113 0.967810 0.220073
+v -0.014181 -0.393823 -0.017925
+vn -0.094941 0.958755 -0.267908
+v -0.014173 -0.393749 -0.021409
+vn 0.164996 0.982589 -0.085414
+v -0.012918 -0.393528 -0.020458
+vn 0.010030 0.995012 -0.099248
+v -0.013417 -0.393481 -0.020478
+vn -0.055511 0.996148 0.067881
+v -0.013689 -0.393471 -0.019735
+vn -0.151985 0.983457 0.098554
+v -0.014167 -0.393568 -0.019323
+vn 0.008095 0.918453 -0.395447
+v -0.014265 -0.394116 -0.022344
+vn 0.087967 0.959333 -0.268219
+v -0.013181 -0.393606 -0.021068
+vn -0.327450 0.889158 0.319647
+v -0.015573 -0.394472 -0.016781
+vn -0.250607 0.966075 0.062419
+v -0.015575 -0.393807 -0.019729
+vn -0.292069 0.931499 0.216804
+v -0.015521 -0.394028 -0.018249
+vn -0.195337 0.968777 0.152688
+v -0.014776 -0.393752 -0.018746
+vn -0.105273 0.958572 -0.264684
+v -0.014840 -0.393900 -0.021722
+vn 0.032081 0.908085 -0.417555
+v -0.013733 -0.393982 -0.022012
+vn -0.175772 0.936637 -0.303010
+v -0.015559 -0.394157 -0.022220
+vn -0.103169 0.949372 0.296729
+v -0.013933 -0.394138 -0.016654
+vn 0.086533 0.903701 -0.419329
+v -0.013243 -0.393852 -0.021680
+vn -0.036740 0.954740 -0.295165
+v -0.013605 -0.393658 -0.021245
+vn 0.074691 0.900795 -0.427773
+v -0.014291 -0.394505 -0.023157
+vn -0.169583 0.983923 -0.056007
+v -0.014715 -0.393641 -0.020372
+vn 0.102525 0.885109 -0.453950
+v -0.013829 -0.394351 -0.022740
+vn 0.076786 0.931331 -0.355986
+v -0.014502 -0.395006 -0.024331
+vn 0.135352 0.878223 -0.458698
+v -0.013440 -0.394501 -0.022908
+vn 0.130879 0.859794 -0.493584
+v -0.012978 -0.394309 -0.022434
+vn 0.122977 0.897014 -0.424551
+v -0.013917 -0.394683 -0.023421
+vn 0.156525 0.956014 0.248066
+v -0.012872 -0.393837 -0.018086
+vn 0.165961 0.950520 0.262619
+v -0.012951 -0.394014 -0.017410
+vn 0.053102 0.962491 0.266067
+v -0.013558 -0.394169 -0.016551
+vn 0.102229 0.924838 -0.366365
+v -0.014051 -0.394975 -0.024129
+vn 0.104326 0.976112 0.190582
+v -0.013061 -0.393656 -0.018739
+vn 0.078580 0.967345 0.240976
+v -0.013354 -0.393844 -0.017846
+vn 0.129586 0.883228 -0.450684
+v -0.012874 -0.394016 -0.021902
+vn 0.181448 0.980808 0.071362
+v -0.012662 -0.393575 -0.019610
+vn -0.008984 0.981579 0.190847
+v -0.013669 -0.393644 -0.018647
+vn 0.007112 0.970183 0.242271
+v -0.013763 -0.393935 -0.017399
+vn 0.112625 0.871509 -0.477272
+v -0.013341 -0.394186 -0.022319
+vn 0.131632 0.881415 -0.453631
+v -0.013038 -0.394539 -0.022857
+vn 0.086270 0.949826 -0.300646
+v -0.014197 -0.395242 -0.024916
+vn 0.101253 0.909543 -0.403087
+v -0.014295 -0.394802 -0.023790
+vn -0.208802 0.932186 0.295689
+v -0.014631 -0.394155 -0.016970
+vn 0.262826 0.935714 -0.235290
+v -0.011403 -0.394045 -0.021090
+vn 0.181312 0.922164 -0.341672
+v -0.012431 -0.393901 -0.021514
+vn 0.182169 0.935468 -0.302842
+v -0.012807 -0.393723 -0.021229
+vn 0.222020 0.966557 -0.128358
+v -0.012337 -0.393669 -0.020571
+vn 0.118550 0.873854 -0.471514
+v -0.012572 -0.394170 -0.022103
+vn 0.176355 0.891289 -0.417736
+v -0.012216 -0.394152 -0.021982
+vn 0.256041 0.924228 -0.283276
+v -0.011969 -0.393923 -0.021278
+vn 0.076124 0.873203 -0.481375
+v -0.012426 -0.394342 -0.022375
+vn 0.035170 0.894109 -0.446466
+v -0.012433 -0.394498 -0.022663
+vn 0.110130 0.861882 -0.495005
+v -0.012704 -0.394374 -0.022473
+vn 0.200141 0.900362 -0.386383
+v -0.011862 -0.394333 -0.022191
+vn 0.105329 0.888659 -0.446309
+v -0.012229 -0.394434 -0.022526
+vn 0.343504 0.939088 -0.010942
+v -0.011282 -0.393934 -0.019935
+vn 0.153782 0.932275 -0.327435
+v -0.010906 -0.394380 -0.021865
+vn 0.285196 0.953562 -0.096863
+v -0.011736 -0.393789 -0.020317
+vn 0.160470 0.918729 -0.360814
+v -0.011305 -0.394433 -0.022160
+vn 0.153210 0.912154 -0.380134
+v -0.010918 -0.394585 -0.022368
+vn 0.130213 0.911315 -0.390576
+v -0.011720 -0.394803 -0.023152
+vn 0.123823 0.176393 0.976500
+v -0.013379 -0.398519 -0.012234
+vn 0.115734 0.310251 0.943583
+v -0.012834 -0.397493 -0.012558
+vn -0.154576 0.576479 0.802358
+v -0.009536 -0.396597 -0.012949
+vn 0.114441 0.691298 0.713449
+v -0.011851 -0.395814 -0.013597
+vn -0.066384 0.751713 0.656141
+v -0.013902 -0.395632 -0.013555
+vn 0.098775 0.591868 0.799960
+v -0.012387 -0.396042 -0.013332
+vn 0.049260 0.770393 0.635663
+v -0.013162 -0.395458 -0.013768
+vn -0.111148 0.870625 0.479227
+v -0.013887 -0.395063 -0.014377
+vn 0.115096 0.441923 0.889639
+v -0.012610 -0.396698 -0.012906
+vn 0.050701 0.408088 0.911533
+v -0.011729 -0.396871 -0.012921
+vn 0.088647 0.601185 0.794178
+v -0.013235 -0.396046 -0.013207
+vn 0.125566 0.434980 0.891642
+v -0.013309 -0.396713 -0.012795
+vn 0.084933 0.326747 0.941288
+v -0.012089 -0.397487 -0.012644
+vn -0.073104 0.428141 0.900750
+v -0.010660 -0.396945 -0.012884
+vn 0.054474 0.555676 0.829613
+v -0.011452 -0.396292 -0.013255
+vn -0.002235 0.663322 0.748331
+v -0.010597 -0.396231 -0.013353
+vn 0.032023 -0.000643 0.999487
+v -0.016351 -0.402005 -0.011670
+vn 0.058690 -0.013593 0.998184
+v -0.016120 -0.402808 -0.011707
+vn 0.135461 0.086824 0.986971
+v -0.014682 -0.399392 -0.011924
+vn -0.058163 0.516621 0.854236
+v -0.014320 -0.396625 -0.012777
+vn -0.166719 0.477592 0.862619
+v -0.014797 -0.397085 -0.012565
+vn -0.073639 0.366838 0.927366
+v -0.014785 -0.397688 -0.012266
+vn 0.085349 0.215096 0.972856
+v -0.014597 -0.398296 -0.012091
+vn 0.043848 0.136887 0.989616
+v -0.015207 -0.399308 -0.011869
+vn -0.284650 -0.009880 0.958581
+v -0.017943 -0.404452 -0.011822
+vn 0.112819 0.019681 0.993421
+v -0.015491 -0.400670 -0.011754
+vn 0.065707 0.347809 0.935260
+v -0.014204 -0.397393 -0.012406
+vn 0.110289 0.081537 0.990549
+v -0.013771 -0.400025 -0.011993
+vn 0.051357 0.006867 0.998657
+v -0.015628 -0.402774 -0.011730
+vn -0.001683 -0.016217 0.999867
+v -0.016324 -0.404276 -0.011720
+vn 0.026804 -0.018711 0.999466
+v -0.016010 -0.404539 -0.011729
+vn 0.099662 0.027583 0.994639
+v -0.015107 -0.400975 -0.011797
+vn 0.129575 0.087798 0.987675
+v -0.014210 -0.399621 -0.011975
+vn 0.143943 0.187590 0.971643
+v -0.014012 -0.398338 -0.012168
+vn 0.136730 0.290734 0.946984
+v -0.013538 -0.397463 -0.012463
+vn 0.066393 0.001734 0.997792
+v -0.015202 -0.403078 -0.011750
+vn 0.013371 0.589264 0.807830
+v -0.013858 -0.396236 -0.013019
+vn 0.073972 0.430604 0.899505
+v -0.013821 -0.396816 -0.012675
+vn 0.058470 -0.027650 0.997906
+v -0.015495 -0.405015 -0.011761
+vn -0.181149 0.210662 0.960628
+v -0.015905 -0.399639 -0.011846
+vn -0.147008 0.101483 0.983916
+v -0.016669 -0.401399 -0.011704
+vn -0.357133 0.250473 0.899844
+v -0.016766 -0.400315 -0.011918
+vn -0.236431 0.056025 0.970032
+v -0.017464 -0.402876 -0.011721
+vn -0.505924 0.610821 0.609047
+v -0.016010 -0.396604 -0.013567
+vn -0.385223 0.682380 0.621257
+v -0.015152 -0.396146 -0.013418
+vn -0.506190 0.477226 0.718350
+v -0.016353 -0.397868 -0.012817
+vn -0.003328 -0.040399 0.999178
+v -0.016969 -0.404179 -0.011709
+vn -0.141732 -0.052010 0.988538
+v -0.017695 -0.405475 -0.011812
+vn -0.075942 -0.010115 0.997061
+v -0.017187 -0.403311 -0.011683
+vn -0.026603 -0.039945 0.998848
+v -0.016819 -0.405268 -0.011759
+vn -0.411705 0.539645 0.734358
+v -0.015419 -0.396936 -0.012877
+vn -0.144141 0.290017 0.946104
+v -0.015256 -0.398440 -0.012057
+vn -0.333946 0.436553 0.835405
+v -0.015410 -0.397718 -0.012393
+vn 0.102562 0.026272 0.994380
+v -0.014259 -0.401773 -0.011848
+vn 0.089342 0.037584 0.995292
+v -0.014696 -0.401251 -0.011824
+vn -0.179940 0.686730 0.704290
+v -0.014467 -0.396042 -0.013237
+vn -0.253277 0.587484 0.768579
+v -0.014833 -0.396520 -0.012928
+vn -0.005276 0.077898 0.996947
+v -0.015852 -0.400435 -0.011726
+vn -0.271509 0.805093 0.527359
+v -0.014630 -0.395478 -0.013971
+vn -0.809047 -0.559902 -0.178755
+v -0.019473 -0.417581 -0.024968
+vn -0.606986 -0.794710 0.001734
+v -0.013865 -0.423712 -0.022511
+vn -0.667782 -0.740091 -0.079578
+v -0.015452 -0.422341 -0.023710
+vn -0.750472 -0.648018 -0.129862
+v -0.018476 -0.419291 -0.023489
+vn -0.815262 -0.573976 -0.076812
+v -0.020098 -0.417445 -0.022565
+vn -0.932655 -0.325530 -0.155518
+v -0.021781 -0.413812 -0.023550
+vn -0.681875 -0.730505 -0.037527
+v -0.016633 -0.421395 -0.022208
+vn -0.524066 -0.844443 0.110771
+v -0.012659 -0.424292 -0.019209
+vn -0.926267 -0.353630 -0.130289
+v -0.021132 -0.413455 -0.027750
+vn -0.499763 -0.858327 0.116243
+v -0.011243 -0.425242 -0.019994
+vn -0.868049 -0.463632 -0.177587
+v -0.020502 -0.415499 -0.026008
+vn -0.440822 -0.877516 0.188791
+v -0.009582 -0.425849 -0.018351
+vn -0.930285 -0.329500 -0.161243
+v -0.021414 -0.413087 -0.026726
+vn -0.516086 -0.834080 0.194850
+v -0.012552 -0.424148 -0.018073
+vn -0.939103 -0.300972 -0.165832
+v -0.021732 -0.412724 -0.025792
+vn -0.902794 -0.387658 -0.186238
+v -0.021240 -0.414440 -0.024897
+vn -0.564915 -0.798626 0.207527
+v -0.013680 -0.423350 -0.017823
+vn -0.607230 -0.786572 0.112141
+v -0.014819 -0.422817 -0.019271
+vn -0.842617 -0.501952 -0.195041
+v -0.020555 -0.416359 -0.023759
+vn -0.526022 -0.846586 0.081196
+v -0.011745 -0.425109 -0.021464
+vn -0.596930 -0.800968 0.046099
+v -0.014370 -0.423266 -0.020841
+vn -0.716849 -0.565020 -0.408510
+v -0.017754 -0.418297 -0.029923
+vn -0.626977 -0.774337 -0.085445
+v -0.013757 -0.423657 -0.024817
+vn -0.171643 -0.348891 -0.921311
+v -0.003315 -0.416755 0.005442
+vn -0.869511 -0.469609 -0.153032
+v -0.019987 -0.415781 -0.028161
+vn -0.734009 -0.672510 -0.094659
+v -0.016104 -0.421210 -0.027553
+vn -0.778748 -0.592062 -0.207400
+v -0.017986 -0.418719 -0.028504
+vn -0.412557 -0.545491 -0.729545
+v 0.003750 -0.416687 0.003167
+vn -0.425154 -0.898789 0.106875
+v -0.009102 -0.426459 -0.020490
+vn -0.169210 -0.484111 -0.858490
+v -0.004181 -0.417227 0.005808
+vn -0.722853 -0.678718 -0.129709
+v -0.016878 -0.420772 -0.024972
+vn -0.862267 -0.487030 -0.138915
+v -0.019996 -0.416068 -0.027061
+vn -0.784717 -0.601201 -0.150920
+v -0.018432 -0.418672 -0.026154
+vn -0.791850 -0.601354 -0.106523
+v -0.018132 -0.418787 -0.027432
+vn -0.700441 -0.708591 -0.085336
+v -0.015475 -0.421979 -0.026201
+vn 0.843633 -0.536456 -0.022308
+v 0.031678 -0.416827 -0.010652
+vn 0.881752 -0.452407 0.133572
+v 0.032699 -0.414918 -0.009544
+vn 0.857474 -0.391892 0.333405
+v 0.032241 -0.413187 -0.005220
+vn 0.866488 -0.408097 0.287497
+v 0.032358 -0.414036 -0.006595
+vn 0.842031 -0.526239 0.118558
+v 0.031219 -0.417338 -0.009112
+vn 0.851477 -0.509668 0.123392
+v 0.031867 -0.416379 -0.009494
+vn 0.815332 -0.509878 0.274333
+v 0.031120 -0.416747 -0.007274
+vn 0.817581 -0.543639 0.189783
+v 0.030822 -0.417773 -0.008512
+vn 0.847444 -0.475576 0.235938
+v 0.031634 -0.416113 -0.007704
+vn 0.843971 -0.442446 0.303241
+v 0.031756 -0.415078 -0.006355
+vn 0.865276 -0.449762 0.221384
+v 0.032204 -0.415242 -0.008069
+vn 0.880576 -0.416194 0.226644
+v 0.032859 -0.414038 -0.008263
+vn 0.801267 -0.597281 0.035013
+v 0.030203 -0.418933 -0.009756
+vn 0.784732 -0.542895 0.299100
+v 0.030612 -0.417293 -0.006883
+vn 0.767150 -0.618212 0.171159
+v 0.030111 -0.418917 -0.009022
+vn 0.759233 -0.593149 0.267845
+v 0.029986 -0.418492 -0.007596
+vn 0.737872 -0.649473 0.183657
+v 0.029569 -0.419465 -0.008809
+vn 0.727558 -0.684268 0.049365
+v 0.029315 -0.420005 -0.009952
+vn 0.739280 -0.659326 -0.136943
+v 0.029175 -0.420108 -0.010983
+vn 0.735790 -0.631154 0.245474
+v 0.029437 -0.419396 -0.008122
+vn 0.772484 -0.580750 0.256900
+v 0.030417 -0.418130 -0.008020
+vn 0.632842 -0.763311 -0.129875
+v 0.027075 -0.422014 -0.011183
+vn 0.560832 -0.784222 0.265451
+v 0.027958 -0.421197 -0.009167
+vn 0.593719 -0.725899 0.347230
+v 0.028417 -0.420439 -0.008203
+vn 0.478898 -0.875235 0.067977
+v 0.025067 -0.423366 -0.011351
+vn 0.700645 -0.712989 0.027254
+v 0.028436 -0.420878 -0.009992
+vn 0.634146 -0.773139 -0.010716
+v 0.027479 -0.421761 -0.010388
+vn 0.664016 -0.740743 0.101896
+v 0.028122 -0.421162 -0.009586
+vn 0.543022 -0.823923 0.162104
+v 0.027270 -0.421864 -0.009954
+vn 0.537241 -0.790462 0.294182
+v 0.027542 -0.421383 -0.008946
+vn 0.551641 -0.834072 0.004008
+v 0.026362 -0.422562 -0.010984
+vn 0.655544 -0.714220 0.245260
+v 0.028500 -0.420640 -0.008815
+vn 0.730039 -0.667952 0.144510
+v 0.028893 -0.420262 -0.008912
+vn 0.671757 -0.672120 0.311445
+v 0.029014 -0.419813 -0.008026
+vn 0.650652 -0.693212 0.310014
+v 0.028328 -0.420065 -0.007268
+vn 0.539657 -0.779214 0.318741
+v 0.027079 -0.421119 -0.007035
+vn 0.427083 -0.902941 0.047944
+v 0.023313 -0.424363 -0.013133
+vn 0.579736 -0.781108 0.231899
+v 0.027156 -0.421519 -0.008532
+vn 0.508992 -0.842502 0.176402
+v 0.026423 -0.422408 -0.010047
+vn 0.474374 -0.859082 0.192215
+v 0.025835 -0.422675 -0.009684
+vn 0.309699 -0.942979 0.121969
+v 0.021937 -0.424713 -0.011719
+vn 0.386469 -0.912077 0.136955
+v 0.023893 -0.423897 -0.011178
+vn 0.471993 -0.854221 0.218013
+v 0.025814 -0.422452 -0.008691
+vn 0.421690 -0.888507 0.180926
+v 0.024347 -0.423380 -0.009388
+vn 0.149442 -0.926101 0.346417
+v 0.021128 -0.424492 -0.009056
+vn 0.315467 -0.939675 0.132254
+v 0.021804 -0.424587 -0.010028
+vn 0.247614 -0.928574 0.276472
+v 0.021888 -0.424404 -0.009259
+vn 0.247039 -0.962393 0.113012
+v 0.020548 -0.425111 -0.011576
+vn 0.201567 -0.969319 0.140683
+v 0.018654 -0.425872 -0.014210
+vn 0.386272 -0.912596 0.134024
+v 0.022859 -0.424141 -0.009859
+vn -0.168743 -0.147185 -0.974609
+v -0.015849 -0.413042 -0.035016
+vn 0.174067 -0.965966 0.191336
+v 0.020334 -0.425025 -0.010542
+vn 0.149851 -0.954313 0.258517
+v 0.019150 -0.425070 -0.010034
+vn 0.206113 -0.973965 0.094389
+v 0.010290 -0.427882 -0.016968
+vn 0.203120 -0.968024 0.147214
+v 0.011979 -0.427221 -0.014462
+vn 0.169018 -0.964610 0.202385
+v 0.015823 -0.425973 -0.011652
+vn 0.195624 -0.971978 0.130348
+v 0.013586 -0.426972 -0.015213
+vn 0.175271 -0.972054 0.156179
+v 0.017608 -0.425864 -0.012761
+vn 0.163338 -0.985178 0.052384
+v 0.007634 -0.428531 -0.018617
+vn 0.067513 -0.997698 0.006448
+v 0.004893 -0.428915 -0.020206
+vn 0.208955 -0.966181 0.151103
+v 0.010246 -0.427632 -0.014623
+vn -0.554111 0.240025 -0.797088
+v -0.018733 -0.402641 -0.034041
+vn -0.392682 0.090171 -0.915243
+v -0.018261 -0.403672 -0.034494
+vn 0.103664 -0.994242 0.027157
+v 0.006295 -0.428794 -0.020407
+vn 0.182793 -0.978440 0.096139
+v 0.007976 -0.428351 -0.016666
+vn 0.208284 -0.974207 0.086826
+v 0.009061 -0.428120 -0.016638
+vn 0.008840 -0.973537 0.228359
+v 0.005291 -0.428257 -0.014749
+vn -0.044655 -0.975722 0.214413
+v 0.003618 -0.428471 -0.015816
+vn -0.641854 -0.658602 -0.392768
+v -0.019576 -0.418031 0.016697
+vn -0.591973 -0.691222 -0.414465
+v -0.018105 -0.418599 0.015434
+vn -0.003479 -0.999255 0.038427
+v 0.003441 -0.428978 -0.020140
+vn -0.044481 -0.999009 -0.001374
+v 0.001314 -0.428967 -0.021980
+vn 0.150515 -0.934160 0.323559
+v 0.011998 -0.426273 -0.010400
+vn 0.093347 -0.907042 0.410562
+v 0.014552 -0.424707 -0.007449
+vn 0.072262 -0.984365 0.160634
+v 0.006955 -0.428502 -0.016820
+vn 0.116746 -0.967681 0.223525
+v 0.007970 -0.427819 -0.013598
+vn 0.083889 -0.908295 0.409833
+v 0.011646 -0.425918 -0.009362
+vn 0.172994 -0.956828 0.233568
+v 0.010772 -0.427194 -0.012883
+vn 0.164672 -0.961401 0.220432
+v 0.009351 -0.427515 -0.013132
+vn 0.117415 -0.976238 0.182133
+v 0.008082 -0.428082 -0.015055
+vn 0.158695 -0.972573 0.170054
+v 0.008761 -0.428037 -0.015265
+vn 0.041300 -0.989543 0.138197
+v 0.005494 -0.428597 -0.016990
+vn 0.075266 -0.981715 0.174847
+v 0.006351 -0.428244 -0.014927
+vn 0.015067 -0.881419 0.472095
+v 0.012674 -0.423963 -0.005634
+vn 0.014662 -0.922167 0.386514
+v 0.007596 -0.427474 -0.012407
+vn -0.340803 -0.759420 -0.554197
+v -0.009071 -0.424491 -0.031753
+vn 0.004144 -0.929896 0.367799
+v 0.006422 -0.427584 -0.012674
+vn -0.010037 -0.946066 0.323817
+v 0.006054 -0.427986 -0.013765
+vn -0.241976 -0.545521 -0.802405
+v -0.009491 -0.423573 -0.032454
+vn -0.057663 -0.877915 0.475332
+v 0.005925 -0.426609 -0.010399
+vn 0.020358 -0.894011 0.447583
+v 0.008568 -0.426441 -0.010167
+vn -0.030820 -0.872668 0.487341
+v 0.011685 -0.423604 -0.004992
+vn 0.012451 -0.883093 0.469033
+v 0.011115 -0.425209 -0.007901
+vn 0.056711 -0.891426 0.449603
+v 0.013035 -0.424584 -0.006849
+vn 0.016191 -0.914983 0.403167
+v 0.006649 -0.427001 -0.011217
+vn 0.337367 -0.458636 0.822093
+v 0.001725 -0.422681 0.029265
+vn 0.125138 -0.941010 0.314389
+v 0.009860 -0.427059 -0.011715
+vn 0.044278 -0.908998 0.414441
+v 0.009571 -0.426721 -0.010801
+vn 0.073571 -0.946906 0.312981
+v 0.008103 -0.427578 -0.012756
+vn 0.043843 -0.959076 0.279732
+v 0.006688 -0.427946 -0.013679
+vn 0.768254 -0.465053 -0.439899
+v 0.023571 -0.423499 -0.015622
+vn -0.216350 0.049960 0.975037
+v -0.014600 -0.409297 0.031508
+vn 0.034302 -0.886772 0.460933
+v 0.013677 -0.423494 -0.004823
+vn -0.317815 -0.882868 -0.345742
+v 0.009207 -0.420783 0.003844
+vn -0.239932 -0.927171 -0.287728
+v 0.010478 -0.421166 0.003888
+vn 0.906343 -0.057171 -0.418658
+v 0.033808 -0.411560 -0.010825
+vn -0.286166 -0.823425 -0.489980
+v 0.007340 -0.420938 0.005485
+vn -0.103418 -0.805258 0.583836
+v 0.007946 -0.424413 -0.006747
+vn -0.005451 -0.867096 0.498112
+v 0.007819 -0.425829 -0.008931
+vn -0.036667 -0.859029 0.510611
+v 0.009796 -0.424548 -0.006755
+vn -0.113363 -0.854816 0.506398
+v 0.010812 -0.422871 -0.003848
+vn -0.186430 -0.925988 0.328313
+v 0.011526 -0.421407 -0.000746
+vn -0.176503 -0.932388 -0.315435
+v 0.010714 -0.421542 0.004869
+vn -0.452229 -0.764411 -0.459527
+v -0.015008 -0.419570 0.013394
+vn -0.224198 -0.837098 0.499001
+v 0.010259 -0.422133 -0.002792
+vn -0.172805 -0.875428 0.451403
+v 0.011025 -0.421875 -0.002006
+vn -0.372172 -0.917345 0.141300
+v 0.010153 -0.420684 0.000525
+vn -0.307435 -0.864441 0.397775
+v 0.010231 -0.421331 -0.001299
+vn 0.221392 -0.975119 -0.011344
+v 0.014802 -0.427391 -0.020571
+vn -0.583851 -0.676530 -0.448805
+v -0.017392 -0.417733 0.013137
+vn -0.101069 -0.908983 -0.404396
+v 0.001702 -0.424702 0.014606
+vn -0.077583 -0.914035 -0.398146
+v 0.005591 -0.424514 0.013321
+vn -0.046639 -0.935861 -0.349270
+v 0.010263 -0.422858 0.008685
+vn -0.177018 0.060177 0.982366
+v -0.015462 -0.411112 0.031474
+vn -0.053898 -0.918721 -0.391211
+v 0.009049 -0.423774 0.011069
+vn -0.220417 -0.969046 0.111199
+v 0.012348 -0.421288 0.000510
+vn -0.105636 -0.912977 -0.394099
+v 0.008057 -0.422716 0.008765
+vn -0.152745 -0.902602 -0.402466
+v 0.008560 -0.422046 0.007063
+vn -0.219485 -0.885697 -0.409106
+v 0.009020 -0.421413 0.005427
+vn -0.155576 -0.861151 -0.483957
+v 0.000984 -0.422847 0.011180
+vn -0.125616 -0.884986 -0.448353
+v 0.000137 -0.424100 0.013771
+vn -0.094745 -0.904637 -0.415519
+v 0.005508 -0.423826 0.011851
+vn -0.145326 -0.882457 -0.447381
+v 0.005264 -0.422613 0.009381
+vn -0.128079 -0.885729 -0.446183
+v 0.003093 -0.423351 0.011487
+vn -0.166826 -0.856653 -0.488174
+v 0.003177 -0.422398 0.009671
+vn 0.123900 -0.968601 -0.215548
+v 0.014910 -0.421743 0.005514
+vn 0.189743 -0.920995 -0.340245
+v 0.017318 -0.422047 0.007680
+vn 0.197432 -0.914732 -0.352541
+v 0.017584 -0.422836 0.009829
+vn -0.057557 -0.943469 -0.326426
+v 0.006113 -0.425303 0.015311
+vn -0.048932 -0.927808 -0.369836
+v 0.007845 -0.424568 0.013084
+vn -0.023018 -0.928893 -0.369632
+v 0.010589 -0.424460 0.012522
+vn 0.133067 -0.931622 -0.338193
+v 0.015671 -0.423922 0.011787
+vn 0.015640 -0.944133 -0.329193
+v 0.012459 -0.422737 0.008256
+vn -0.014597 -0.924227 -0.381565
+v 0.011504 -0.423638 0.010512
+vn 0.063976 -0.939888 -0.335436
+v 0.014236 -0.422535 0.007900
+vn 0.121925 -0.932455 -0.340092
+v 0.015762 -0.422303 0.007673
+vn 0.037965 -0.926000 -0.375608
+v 0.013634 -0.423409 0.009995
+vn 0.118162 -0.924704 -0.361883
+v 0.015584 -0.423126 0.009684
+vn 0.160839 -0.959274 -0.232216
+v 0.016091 -0.421563 0.005527
+vn 0.230957 -0.971217 -0.058278
+v 0.017367 -0.420963 0.003522
+vn -0.258506 0.107347 -0.960027
+v -0.014155 -0.410146 0.007860
+vn -0.096524 -0.931349 0.351102
+v 0.013064 -0.421950 -0.001658
+vn -0.078082 -0.887849 0.453461
+v 0.011958 -0.422283 -0.002542
+vn 0.166310 -0.983661 -0.068930
+v 0.016471 -0.421118 0.003146
+vn 0.197654 -0.979462 -0.039827
+v 0.015584 -0.421284 0.002928
+vn 0.047735 -0.933295 -0.355925
+v 0.013262 -0.424228 0.011971
+vn 0.281240 -0.911083 -0.301382
+v 0.020466 -0.423283 0.013130
+vn 0.235941 -0.930731 -0.279412
+v 0.018539 -0.421137 0.005863
+vn 0.341921 -0.877194 -0.337078
+v 0.021309 -0.421037 0.008090
+vn 0.073155 -0.943458 -0.323319
+v 0.013283 -0.425021 0.014177
+vn 0.154456 -0.938393 -0.309130
+v 0.016194 -0.424570 0.013900
+vn 0.211918 -0.920386 -0.328603
+v 0.018019 -0.423593 0.012101
+vn 0.201852 -0.944870 -0.257831
+v 0.017212 -0.421383 0.005672
+vn 0.239344 -0.961524 -0.134857
+v 0.018190 -0.420821 0.004181
+vn 0.288152 -0.947252 -0.140293
+v 0.019422 -0.420495 0.004096
+vn 0.303997 -0.911608 -0.276688
+v 0.020174 -0.420731 0.006061
+vn 0.351204 -0.919456 -0.176796
+v 0.021022 -0.420022 0.004441
+vn 0.312433 -0.879697 -0.358494
+v 0.020931 -0.421827 0.009658
+vn 0.258498 -0.903545 -0.341740
+v 0.019315 -0.421730 0.008136
+vn 0.389829 -0.848265 -0.358440
+v 0.022568 -0.421365 0.010140
+vn 0.377668 -0.881809 -0.282454
+v 0.021893 -0.420179 0.006356
+vn 0.259452 -0.903100 -0.342191
+v 0.019691 -0.422611 0.010629
+vn 0.432127 -0.837121 -0.335402
+v 0.023135 -0.420367 0.008445
+vn 0.463877 -0.841017 -0.278405
+v 0.023549 -0.419447 0.006542
+vn 0.480400 -0.802853 -0.353049
+v 0.024371 -0.420662 0.010688
+vn 0.327307 -0.881909 -0.339274
+v 0.021739 -0.422283 0.011519
+vn 0.566128 -0.750120 -0.341787
+v 0.026134 -0.419755 0.011322
+vn 0.607835 -0.737674 -0.293895
+v 0.026469 -0.417908 0.007835
+vn 0.533407 -0.780523 -0.325976
+v 0.025099 -0.419335 0.008758
+vn 0.426971 -0.839108 -0.337037
+v 0.023597 -0.421733 0.012167
+vn -0.618627 -0.642238 -0.452584
+v -0.018412 -0.417649 0.014357
+vn 0.488744 -0.853955 -0.178579
+v 0.023791 -0.418788 0.004667
+vn 0.548513 -0.794161 -0.261614
+v 0.025083 -0.418449 0.006385
+vn 0.639258 -0.724897 -0.256659
+v 0.027261 -0.416904 0.006974
+vn 0.687389 -0.648280 -0.327459
+v 0.030433 -0.415746 0.011332
+vn 0.660064 -0.686367 -0.305313
+v 0.028287 -0.416727 0.008896
+vn 0.607625 -0.757483 -0.238768
+v 0.026249 -0.417504 0.006197
+vn 0.633267 -0.752424 -0.181193
+v 0.026982 -0.416673 0.005429
+vn 0.610874 -0.717065 -0.335635
+v 0.027031 -0.418386 0.009991
+vn 0.655938 -0.676391 -0.335024
+v 0.028608 -0.417445 0.011018
+vn 0.628233 -0.704759 -0.329602
+v 0.027857 -0.418679 0.012081
+vn 0.593164 -0.747960 -0.297847
+v 0.026990 -0.419967 0.013320
+vn 0.805887 -0.351899 -0.476144
+v 0.032292 -0.415183 -0.011668
+vn 0.671356 -0.669227 -0.318460
+v 0.029650 -0.417345 0.012872
+vn 0.699941 -0.652898 -0.289493
+v 0.030105 -0.414704 0.008480
+vn 0.728040 -0.644877 -0.232575
+v 0.030418 -0.413686 0.006853
+vn 0.768127 -0.587611 -0.254350
+v 0.032443 -0.411613 0.007631
+vn 0.822319 -0.497099 -0.276920
+v 0.034032 -0.409958 0.008910
+vn 0.799372 -0.509837 -0.317917
+v 0.034389 -0.411666 0.012617
+vn 0.855307 -0.438216 -0.276436
+v 0.035226 -0.409689 0.011871
+vn 0.840092 -0.479476 -0.253670
+v 0.033731 -0.409584 0.007255
+vn 0.874910 -0.401205 -0.271233
+v 0.033938 -0.408901 0.006766
+vn 0.733484 -0.616160 -0.286962
+v 0.031633 -0.412957 0.008433
+vn 0.778898 -0.547608 -0.305685
+v 0.033408 -0.411562 0.010106
+vn 0.728633 -0.602594 -0.325537
+v 0.032371 -0.413463 0.011117
+vn 0.760377 -0.575913 -0.300251
+v 0.033836 -0.413119 0.013736
+vn 0.947013 -0.098853 -0.305604
+v 0.035163 -0.405800 0.007787
+vn 0.913084 -0.273593 -0.302365
+v 0.034556 -0.407690 0.007202
+vn 0.939546 -0.317269 -0.128814
+v 0.036314 -0.408543 0.014259
+vn 0.917486 -0.330647 -0.221117
+v 0.036314 -0.407648 0.012509
+vn 0.883041 -0.368168 -0.291017
+v 0.035495 -0.408146 0.010455
+vn 0.877656 -0.385080 -0.285365
+v 0.034638 -0.408500 0.008380
+vn 0.943882 -0.154789 -0.291766
+v 0.036329 -0.406503 0.011488
+vn 0.974425 -0.125429 -0.186448
+v 0.036752 -0.406345 0.012956
+vn 0.895346 -0.366800 -0.252615
+v 0.033559 -0.408766 0.005310
+vn 0.974615 -0.218798 -0.047474
+v 0.033037 -0.407519 0.000666
+vn 0.953281 -0.219660 -0.207376
+v 0.033428 -0.407361 0.003043
+vn 0.977120 -0.077835 -0.197934
+v 0.033424 -0.406013 0.002008
+vn 0.997100 -0.051871 -0.055687
+v 0.033192 -0.406090 0.000368
+vn 0.932665 -0.339714 -0.121372
+v 0.032755 -0.408874 0.001909
+vn 0.931805 -0.231563 -0.279497
+v 0.034044 -0.407340 0.005302
+vn 0.884465 -0.369258 0.285253
+v 0.032597 -0.410128 -0.002738
+vn 0.948576 -0.279796 0.148045
+v 0.032884 -0.408659 -0.001679
+vn 0.894817 -0.404032 0.189900
+v 0.032202 -0.410378 -0.001720
+vn 0.016257 -0.988432 -0.150791
+v 0.001713 -0.428895 -0.024705
+vn 0.966527 -0.180569 0.182263
+v 0.033853 -0.410637 -0.007249
+vn 0.867463 -0.367282 0.335578
+v 0.032515 -0.411381 -0.003939
+vn 0.882744 -0.351230 0.312092
+v 0.032846 -0.411788 -0.005229
+vn 0.885305 -0.370621 0.280845
+v 0.032960 -0.412808 -0.006761
+vn 0.916191 -0.292608 0.273814
+v 0.033022 -0.409817 -0.003659
+vn 0.921626 -0.270540 0.278234
+v 0.033361 -0.410363 -0.005262
+vn 0.836380 -0.425641 0.345395
+v 0.031633 -0.414407 -0.005166
+vn 0.815924 -0.480515 0.321518
+v 0.031178 -0.415927 -0.006083
+vn 0.817777 -0.431856 0.380448
+v 0.031374 -0.413376 -0.003472
+vn 0.842491 -0.397660 0.363422
+v 0.031909 -0.412911 -0.004128
+vn -0.678302 -0.594737 -0.431502
+v -0.019920 -0.416876 0.015473
+vn 0.837468 -0.461428 0.292800
+v 0.031288 -0.412462 -0.002140
+vn 0.774809 -0.486897 0.403239
+v 0.030510 -0.414742 -0.003306
+vn 0.818369 -0.427012 0.384620
+v 0.031403 -0.414149 -0.004348
+vn 0.882206 -0.459470 0.102952
+v 0.031703 -0.411067 -0.000666
+vn 0.855044 -0.403259 0.326011
+v 0.032032 -0.411601 -0.002977
+vn 0.716557 -0.621475 0.316725
+v 0.029591 -0.418797 -0.007217
+vn 0.758653 -0.568117 0.318886
+v 0.030143 -0.417746 -0.006532
+vn -0.391863 -0.530299 -0.751815
+v -0.012889 -0.421508 -0.032506
+vn 0.808767 -0.462755 0.362978
+v 0.031085 -0.415341 -0.005060
+vn 0.642449 -0.410190 -0.647305
+v 0.025301 -0.420785 -0.014996
+vn -0.790972 -0.609053 0.058461
+v 0.006403 -0.417732 -0.000340
+vn -0.803089 -0.557214 0.211091
+v 0.006559 -0.418178 -0.001267
+vn 0.112580 -0.922562 -0.369061
+v 0.004118 -0.428068 -0.027693
+vn 0.657084 -0.658073 0.367668
+v 0.029176 -0.419031 -0.006829
+vn 0.706001 -0.605756 0.366909
+v 0.029780 -0.418060 -0.006271
+vn 0.697267 -0.611230 0.374456
+v 0.029209 -0.418265 -0.005579
+vn 0.742819 -0.550875 0.380468
+v 0.030224 -0.417031 -0.005551
+vn 0.786726 -0.516757 0.337675
+v 0.030656 -0.416589 -0.005819
+vn 0.528966 -0.496941 -0.687928
+v 0.027489 -0.419535 -0.014067
+vn 0.565218 -0.453567 -0.689062
+v 0.027422 -0.418763 -0.014648
+vn 0.703177 -0.622316 0.343898
+v 0.028613 -0.419081 -0.005771
+vn 0.573865 -0.783440 0.238538
+v 0.027044 -0.421409 -0.007837
+vn 0.667247 -0.699468 0.255979
+v 0.027919 -0.420421 -0.007082
+vn 0.589086 -0.438515 -0.678736
+v 0.028837 -0.418999 -0.013385
+vn 0.470985 -0.820412 0.324189
+v 0.026167 -0.421798 -0.007309
+vn 0.469769 -0.775263 0.422238
+v 0.026299 -0.421050 -0.005922
+vn 0.446222 -0.758396 0.475101
+v 0.025892 -0.420548 -0.004682
+vn 0.331208 -0.829510 0.449683
+v 0.023964 -0.422123 -0.005714
+vn 0.439922 -0.851532 0.285241
+v 0.024774 -0.422771 -0.007864
+vn 0.391554 -0.825774 0.405935
+v 0.024682 -0.422312 -0.006644
+vn 0.278046 -0.951532 0.131443
+v 0.021038 -0.425302 -0.014078
+vn 0.377627 -0.885306 0.271351
+v 0.023238 -0.423727 -0.008570
+vn 0.520670 -0.698695 0.490640
+v 0.026697 -0.418827 -0.002986
+vn 0.421654 -0.767563 0.482757
+v 0.024990 -0.420446 -0.003715
+vn -0.595468 -0.721092 -0.354182
+v -0.018785 -0.419227 0.017585
+vn 0.268512 -0.874732 0.403418
+v 0.022812 -0.423570 -0.007759
+vn 0.233255 -0.830894 0.505180
+v 0.020947 -0.422029 -0.003614
+vn 0.294486 -0.826712 0.479401
+v 0.022134 -0.421938 -0.004049
+vn 0.328551 -0.840416 0.430993
+v 0.022999 -0.422067 -0.004884
+vn 0.374175 -0.783409 0.496249
+v 0.023890 -0.420685 -0.003185
+vn 0.255627 -0.893450 0.369325
+v 0.020834 -0.423358 -0.006041
+vn 0.222278 -0.899428 0.376326
+v 0.021433 -0.423734 -0.007395
+vn 0.150324 -0.920163 0.361529
+v 0.018976 -0.424027 -0.006638
+vn 0.221576 -0.868250 0.443899
+v 0.020466 -0.423030 -0.005066
+vn 0.117468 -0.868899 0.480850
+v 0.019438 -0.423224 -0.005085
+vn 0.173017 -0.941316 0.289808
+v 0.018790 -0.424670 -0.008450
+vn 0.068294 -0.900873 0.428676
+v 0.017830 -0.423983 -0.006307
+vn 0.057067 -0.900952 0.430151
+v 0.015802 -0.424224 -0.006614
+vn 0.127038 -0.933863 0.334306
+v 0.013718 -0.425734 -0.009628
+vn 0.162737 -0.956232 0.243179
+v 0.013486 -0.426352 -0.011516
+vn 0.068210 -0.891521 0.447815
+v 0.017267 -0.423521 -0.005341
+vn 0.126281 -0.944656 0.302783
+v 0.016707 -0.425100 -0.008813
+vn 0.081041 -0.923685 0.374483
+v 0.015752 -0.424919 -0.008093
+vn 0.130970 -0.926672 0.352315
+v 0.017436 -0.421944 -0.002100
+vn 0.004456 -0.273503 0.961861
+v 0.016887 -0.421061 0.026423
+vn 0.143107 -0.899054 0.413790
+v 0.018157 -0.422208 -0.002909
+vn 0.038672 -0.899547 0.435109
+v 0.014292 -0.423559 -0.005011
+vn 0.070340 -0.897181 0.436025
+v 0.017028 -0.422985 -0.004282
+vn 0.104550 -0.905159 0.412015
+v 0.013638 -0.424869 -0.007562
+vn 0.037807 -0.898076 -0.438213
+v 0.002933 -0.427859 -0.028390
+vn 0.115441 -0.909578 0.399175
+v 0.016232 -0.423000 -0.004183
+vn -0.220839 -0.966776 -0.128739
+v 0.011908 -0.421248 0.002800
+vn -0.298766 -0.948133 -0.108545
+v 0.010624 -0.420861 0.002409
+vn 0.266407 -0.947511 0.176777
+v 0.016106 -0.421349 0.000567
+vn -0.131332 -0.930943 -0.340730
+v 0.010547 -0.421958 0.006141
+vn -0.060943 -0.961093 -0.269418
+v 0.011785 -0.422035 0.005950
+vn 0.172815 -0.972074 0.158767
+v 0.017476 -0.421082 0.000803
+vn 0.150104 -0.938210 0.311820
+v 0.016792 -0.421632 -0.001075
+vn 0.167835 -0.979290 0.113234
+v 0.016671 -0.421110 0.001102
+vn 0.152761 -0.870863 0.467185
+v 0.019170 -0.422495 -0.003786
+vn 0.324292 -0.924681 0.199497
+v 0.019973 -0.420440 0.000567
+vn 0.392731 -0.911657 0.121009
+v 0.021821 -0.419573 0.001410
+vn 0.277265 -0.958263 0.069690
+v 0.018708 -0.420621 0.002107
+vn 0.254587 -0.891615 0.374443
+v 0.019641 -0.421233 -0.001473
+vn 0.235522 -0.932411 0.274115
+v 0.018527 -0.421178 -0.000523
+vn 0.343771 -0.939049 0.002921
+v 0.020443 -0.420039 0.002547
+vn 0.413141 -0.885558 -0.212370
+v 0.022446 -0.419540 0.004961
+vn 0.291754 -0.855671 0.427443
+v 0.021224 -0.420908 -0.001833
+vn 0.376043 -0.807845 0.453848
+v 0.023264 -0.420226 -0.002053
+vn 0.344265 -0.889813 0.299524
+v 0.021144 -0.420252 -0.000281
+vn 0.443837 -0.856364 0.263912
+v 0.023292 -0.419191 0.000148
+vn 0.509025 -0.860720 0.007410
+v 0.024124 -0.418346 0.002695
+vn 0.417328 -0.905446 -0.077489
+v 0.022222 -0.419357 0.003329
+vn 0.604940 -0.794067 -0.059205
+v 0.026393 -0.416831 0.003586
+vn 0.575394 -0.801243 -0.164109
+v 0.025453 -0.417720 0.004735
+vn 0.465811 -0.743649 0.479589
+v 0.025389 -0.419116 -0.002104
+vn -0.760119 -0.634383 0.140632
+v 0.007024 -0.418542 -0.000375
+vn -0.784204 -0.620197 -0.019475
+v 0.006649 -0.418004 0.000189
+vn -0.314999 0.801726 0.507948
+v -0.015341 -0.395145 0.025542
+vn 0.721048 -0.643338 -0.257305
+v 0.029132 -0.419806 -0.012049
+vn 0.003925 -0.967049 -0.254558
+v 0.001333 -0.428575 -0.026265
+vn 0.057439 0.994736 0.084862
+v -0.017020 -0.393434 0.018889
+vn 0.093955 0.995197 0.027494
+v -0.016139 -0.393446 0.017721
+vn -0.131561 0.861084 -0.491148
+v -0.011857 -0.394624 0.011599
+vn -0.459135 0.555491 0.693271
+v -0.015629 -0.397463 0.027954
+vn -0.275930 0.949091 0.151950
+v -0.013364 -0.393543 0.022690
+vn -0.306120 0.951206 0.038707
+v -0.012433 -0.393111 0.020929
+vn -0.200136 0.979214 -0.032961
+v -0.013416 -0.393402 0.018645
+vn -0.456935 0.698907 0.550217
+v -0.012607 -0.395611 0.028270
+vn -0.371096 0.717459 0.589525
+v -0.015395 -0.395877 0.026508
+vn 0.891459 -0.450118 -0.051901
+v 0.032042 -0.410348 0.001085
+vn 0.835128 -0.545019 -0.074268
+v 0.031612 -0.411234 0.002255
+vn 0.917924 -0.331767 -0.217593
+v 0.033231 -0.408539 0.003740
+vn 0.877886 -0.450754 -0.161667
+v 0.032585 -0.409856 0.003389
+vn -0.482901 -0.853737 -0.194781
+v -0.015825 -0.422262 0.020832
+vn -0.536972 -0.836396 -0.110016
+v -0.016882 -0.421787 0.021738
+vn 0.846553 -0.531525 0.028787
+v 0.031253 -0.411715 0.000407
+vn 0.929342 -0.367886 0.031369
+v 0.032443 -0.409386 -0.000108
+vn 0.813715 -0.559309 -0.158248
+v 0.032116 -0.410932 0.004382
+vn 0.798763 -0.559247 -0.221857
+v 0.032838 -0.410642 0.006529
+vn 0.845679 -0.486556 -0.219296
+v 0.033102 -0.409834 0.005584
+vn -0.055774 0.914180 0.401452
+v -0.017940 -0.394328 0.023031
+vn 0.904859 0.350807 -0.241176
+v 0.035157 -0.403032 0.008487
+vn 0.957106 0.161208 -0.240749
+v 0.035708 -0.404009 0.009661
+vn 0.889312 0.428407 -0.159975
+v 0.035481 -0.402569 0.010910
+vn 0.926308 -0.206791 -0.314945
+v 0.035544 -0.406896 0.009403
+vn 0.933490 0.331497 -0.136768
+v 0.036187 -0.403659 0.012568
+vn 0.921474 0.387256 -0.030297
+v 0.036111 -0.402867 0.015558
+vn 0.967395 0.158055 -0.197902
+v 0.036661 -0.405250 0.012721
+vn 0.959675 0.053057 -0.276060
+v 0.036125 -0.405244 0.010683
+vn 0.952657 0.058750 -0.298317
+v 0.034988 -0.404234 0.007044
+vn 0.892165 0.323628 -0.315131
+v 0.034638 -0.403261 0.006522
+vn 0.700582 -0.650582 -0.293136
+v 0.031538 -0.415893 0.013950
+vn 0.713299 -0.622823 -0.321398
+v 0.032394 -0.414514 0.013052
+vn 0.818010 -0.550205 -0.167734
+v 0.034620 -0.412983 0.015921
+vn 0.799290 -0.588334 0.122468
+v 0.032754 -0.415934 0.021367
+vn 0.873965 -0.485474 -0.022354
+v 0.034959 -0.412848 0.018163
+vn -0.156403 -0.868189 -0.470942
+v -0.004305 -0.423858 0.014658
+vn -0.198025 -0.854534 -0.480164
+v -0.007580 -0.423239 0.014751
+vn 0.746562 -0.620778 -0.239333
+v 0.033261 -0.414458 0.015100
+vn 0.730359 -0.654853 -0.194277
+v 0.032253 -0.416052 0.016350
+vn 0.827826 -0.559900 0.034859
+v 0.033958 -0.414444 0.019818
+vn 0.789171 -0.603775 -0.112538
+v 0.033790 -0.414511 0.017459
+vn 0.918425 -0.386713 -0.083357
+v 0.035566 -0.411406 0.016415
+vn 0.841755 -0.484275 -0.238593
+v 0.035009 -0.411871 0.014589
+vn 0.761945 -0.643935 -0.069190
+v 0.032373 -0.416484 0.019051
+vn 0.696777 -0.711103 -0.093991
+v 0.030346 -0.418741 0.019429
+vn 0.652511 -0.726573 -0.215223
+v 0.029166 -0.419038 0.015970
+vn 0.699834 -0.694087 -0.168749
+v 0.030787 -0.417881 0.017319
+vn -0.474706 -0.875798 -0.087362
+v -0.015880 -0.422388 0.021786
+vn 0.751383 -0.658769 0.038037
+v 0.031289 -0.417833 0.021017
+vn 0.597564 -0.777494 -0.196010
+v 0.027504 -0.420601 0.016650
+vn 0.500024 -0.848099 -0.175225
+v 0.025056 -0.422652 0.018393
+vn 0.604535 -0.760578 -0.236765
+v 0.027530 -0.420148 0.015063
+vn 0.640989 -0.751582 -0.155746
+v 0.028721 -0.419934 0.017980
+vn 0.574221 -0.801011 -0.169270
+v 0.026878 -0.421344 0.017816
+vn 0.687061 -0.680208 -0.255470
+v 0.030516 -0.417434 0.015050
+vn 0.646756 -0.708395 -0.282635
+v 0.028725 -0.418753 0.014017
+vn 0.622571 -0.779699 -0.066887
+v 0.028233 -0.420648 0.019833
+vn 0.574992 -0.809428 -0.119209
+v 0.026677 -0.421699 0.018942
+vn 0.355411 -0.886442 -0.296484
+v 0.022620 -0.422580 0.013227
+vn 0.297826 -0.928233 -0.222898
+v 0.020719 -0.424654 0.018510
+vn 0.348104 -0.937455 0.001070
+v 0.021300 -0.424816 0.020591
+vn 0.401128 -0.893366 -0.202470
+v 0.022799 -0.423979 0.019010
+vn 0.339199 -0.914383 -0.221015
+v 0.021901 -0.423800 0.016790
+vn 0.451682 -0.868601 -0.203753
+v 0.024308 -0.422720 0.016783
+vn 0.415100 -0.885458 -0.208941
+v 0.023428 -0.423408 0.017859
+vn 0.502876 -0.857528 -0.108456
+v 0.024467 -0.423202 0.019485
+vn 0.480706 -0.876250 0.033296
+v 0.023524 -0.423788 0.020498
+vn 0.410213 -0.905926 -0.104992
+v 0.022535 -0.424270 0.019871
+vn 0.446851 -0.868838 -0.213176
+v 0.024214 -0.422490 0.015600
+vn 0.518823 -0.794268 -0.316168
+v 0.025302 -0.420983 0.012772
+vn 0.538948 -0.805598 -0.246064
+v 0.025844 -0.421236 0.014528
+vn 0.540350 -0.816107 -0.204918
+v 0.025980 -0.421535 0.016009
+vn 0.452487 -0.851867 -0.263777
+v 0.024153 -0.422100 0.013984
+vn 0.520644 -0.832624 -0.188857
+v 0.025633 -0.422046 0.017259
+vn 0.349091 -0.904976 -0.243216
+v 0.022330 -0.423158 0.014822
+vn 0.045988 -0.997217 0.058688
+v 0.012732 -0.426320 0.021230
+vn 0.127790 -0.991670 -0.016146
+v 0.015860 -0.426094 0.020778
+vn 0.087224 -0.993455 -0.073755
+v 0.013976 -0.426243 0.019977
+vn 0.229682 -0.928633 -0.291355
+v 0.018372 -0.424178 0.014066
+vn 0.206173 -0.943626 -0.258964
+v 0.017131 -0.424955 0.015755
+vn 0.241096 -0.947416 -0.210419
+v 0.018174 -0.425137 0.017535
+vn 0.192903 -0.967520 -0.163383
+v 0.017537 -0.425682 0.019465
+vn 0.288842 -0.942039 -0.170682
+v 0.020591 -0.424977 0.019731
+vn 0.280962 -0.928710 -0.241988
+v 0.019796 -0.424186 0.015459
+vn 0.129113 -0.981336 -0.142510
+v 0.014592 -0.426007 0.018491
+vn 0.123911 -0.949734 -0.287491
+v 0.014651 -0.425413 0.015834
+vn 0.045387 -0.963619 -0.263398
+v 0.012167 -0.425789 0.016359
+vn 0.091533 -0.973831 -0.208024
+v 0.013689 -0.425923 0.017274
+vn -0.526883 -0.849906 -0.007351
+v -0.016810 -0.421899 0.022785
+vn 0.017106 -0.999850 0.002682
+v 0.010989 -0.426373 0.020618
+vn -0.615898 -0.720815 -0.317954
+v -0.013833 -0.422676 -0.029582
+vn 0.178373 -0.961805 -0.207638
+v 0.015604 -0.425624 0.017143
+vn 0.030046 -0.995787 -0.086632
+v 0.011482 -0.426318 0.019314
+vn 0.011835 -0.997899 0.063705
+v 0.008181 -0.426353 0.021602
+vn 0.008507 -0.999660 0.024652
+v 0.006618 -0.426377 0.021283
+vn -0.000777 -0.999533 -0.030539
+v 0.008771 -0.426371 0.020193
+vn -0.066661 -0.961571 -0.266341
+v 0.002931 -0.425873 0.017807
+vn 0.001073 -0.953999 -0.299809
+v 0.010521 -0.425624 0.015633
+vn -0.016677 -0.938037 -0.346135
+v 0.010282 -0.425141 0.014283
+vn -0.027630 -0.995539 -0.090220
+v 0.003586 -0.426301 0.020158
+vn -0.037699 -0.983610 -0.176324
+v 0.004676 -0.426132 0.018556
+vn -0.047846 -0.958160 -0.282205
+v 0.007340 -0.425758 0.016434
+vn -0.014047 -0.993296 -0.114740
+v 0.008016 -0.426287 0.019000
+vn 0.026946 -0.987750 -0.153702
+v 0.011579 -0.426159 0.017958
+vn -0.016480 -0.978510 -0.205541
+v 0.009139 -0.426032 0.017258
+vn 0.002846 -0.999833 0.018037
+v 0.003838 -0.426386 0.022240
+vn -0.019960 -0.999409 -0.027992
+v 0.002113 -0.426359 0.021763
+vn -0.010956 -0.998973 -0.043959
+v 0.005729 -0.426365 0.020535
+vn -0.257664 -0.942654 0.212160
+v -0.009319 -0.424869 0.027777
+vn -0.285126 -0.602000 -0.745855
+v 0.002247 -0.418010 0.004829
+vn -0.039492 -0.999160 -0.010962
+v -0.000853 -0.426310 0.022993
+vn -0.044156 -0.997218 -0.060061
+v -0.000051 -0.426286 0.021634
+vn -0.042453 -0.993780 -0.102949
+v 0.000755 -0.426216 0.020288
+vn -0.093654 -0.969813 -0.225146
+v -0.001054 -0.425945 0.019384
+vn -0.058911 -0.980754 -0.186149
+v 0.000980 -0.426105 0.019346
+vn -0.069603 -0.955201 0.287657
+v -0.004294 -0.425535 0.028447
+vn -0.014806 -0.978328 0.206530
+v -0.002082 -0.425943 0.027242
+vn 0.090134 -0.851472 0.516595
+v -0.002731 -0.425413 0.028714
+vn 0.012833 -0.932821 0.360112
+v -0.002964 -0.425645 0.028287
+vn 0.019091 -0.775993 0.630452
+v -0.004610 -0.424800 0.029791
+vn 0.146047 -0.750333 0.644725
+v -0.002587 -0.425012 0.029251
+vn -0.117541 -0.794786 0.595398
+v -0.006742 -0.424511 0.029976
+vn -0.022731 -0.864424 0.502249
+v -0.004596 -0.425196 0.029210
+vn 0.040320 -0.408064 0.912062
+v -0.006412 -0.423394 0.030991
+vn 0.090686 -0.616469 0.782140
+v -0.004500 -0.424284 0.030304
+vn 0.106020 -0.436498 0.893437
+v -0.004561 -0.423558 0.030748
+vn 0.060322 -0.357629 0.931914
+v -0.004121 -0.422448 0.031211
+vn -0.165652 -0.944477 0.283765
+v -0.007503 -0.425069 0.028571
+vn 0.018564 -0.226350 0.973869
+v -0.006652 -0.422453 0.031272
+vn -0.040405 -0.072683 0.996536
+v -0.009043 -0.421751 0.031333
+vn -0.016084 -0.215759 0.976314
+v -0.008314 -0.422559 0.031270
+vn -0.171840 -0.571576 0.802354
+v -0.009402 -0.422969 0.031032
+vn -0.108389 -0.285900 0.952110
+v -0.009550 -0.422538 0.031192
+vn -0.184632 -0.733235 0.654430
+v -0.008733 -0.423567 0.030639
+vn -0.232095 -0.801853 0.550603
+v -0.009151 -0.424070 0.029853
+vn -0.217190 -0.884490 0.412924
+v -0.008678 -0.424584 0.029216
+vn -0.124816 -0.952110 0.279121
+v -0.005927 -0.425321 0.028573
+vn -0.104471 -0.896013 0.431562
+v -0.005905 -0.425067 0.029277
+vn -0.165657 -0.878365 0.448366
+v -0.007446 -0.424750 0.029407
+vn -0.013068 -0.163509 0.986455
+v -0.005922 -0.421721 0.031422
+vn -0.049680 -0.675703 0.735498
+v -0.006495 -0.424011 0.030594
+vn -0.069206 -0.503979 0.860939
+v -0.008122 -0.423203 0.031059
+vn 0.178030 -0.500716 0.847106
+v -0.003009 -0.423653 0.030432
+vn 0.096481 -0.929635 0.355626
+v -0.001050 -0.425850 0.027439
+vn 0.027638 -0.995103 0.094904
+v 0.002073 -0.426333 0.024182
+vn 0.239334 -0.712280 0.659831
+v -0.000780 -0.425021 0.028683
+vn 0.182771 -0.835454 0.518277
+v -0.000700 -0.425529 0.027976
+vn 0.189259 -0.605530 0.772990
+v -0.002661 -0.424418 0.029841
+vn 0.180489 -0.294711 0.938387
+v -0.003688 -0.422539 0.031121
+vn 0.050545 -0.370279 0.927544
+v -0.004592 -0.422707 0.031120
+vn 0.134407 -0.471070 0.871796
+v -0.003409 -0.422971 0.030886
+vn -0.025595 -0.994567 0.100903
+v -0.001034 -0.426193 0.025754
+vn 0.064571 -0.975304 0.211218
+v 0.000480 -0.426155 0.025954
+vn 0.265295 -0.575645 0.773467
+v -0.001291 -0.424337 0.029472
+vn 0.305767 -0.529385 0.791365
+v -0.000046 -0.423860 0.029322
+vn 0.276362 -0.445442 0.851590
+v -0.001434 -0.423379 0.030105
+vn 0.298658 -0.475820 0.827284
+v -0.000662 -0.423484 0.029791
+vn 0.297504 -0.541103 0.786574
+v -0.000502 -0.424096 0.029339
+vn 0.263209 -0.473312 0.840653
+v -0.002185 -0.423494 0.030295
+vn 0.269115 -0.332798 0.903782
+v -0.002677 -0.422803 0.030755
+vn 0.152836 -0.150036 0.976796
+v -0.003978 -0.421652 0.031343
+vn 0.308810 -0.763329 -0.567420
+v 0.009792 -0.427401 -0.026374
+vn 0.072970 -0.113195 0.990890
+v -0.004614 -0.421534 0.031442
+vn 0.246959 -0.872369 -0.421882
+v 0.009623 -0.427829 -0.025763
+vn -0.903322 -0.321980 -0.283441
+v -0.023762 -0.412618 0.017999
+vn 0.253087 -0.354365 0.900207
+v -0.003128 -0.422849 0.030879
+vn 0.229821 -0.217469 0.948625
+v -0.003540 -0.422339 0.031129
+vn 0.135073 -0.186143 0.973194
+v -0.004177 -0.422076 0.031310
+vn 0.317640 -0.410842 0.854584
+v 0.000185 -0.422719 0.029856
+vn 0.205413 -0.282837 0.936915
+v -0.001796 -0.421162 0.031042
+vn 0.209330 -0.239012 0.948185
+v -0.003047 -0.421967 0.031103
+vn 0.047518 -0.075965 0.995978
+v -0.004460 -0.420858 0.031482
+vn -0.006064 -0.058043 0.998296
+v -0.005283 -0.421120 0.031495
+vn 0.289295 -0.392727 0.872968
+v -0.001025 -0.422980 0.030173
+vn 0.255762 -0.327161 0.909699
+v -0.001978 -0.422481 0.030671
+vn 0.005610 -0.188769 0.982005
+v 0.020841 -0.418591 0.027029
+vn -0.020157 -0.166113 0.985901
+v 0.021185 -0.418894 0.026975
+vn 0.028606 -0.110424 0.993473
+v 0.022719 -0.417675 0.027137
+vn 0.045072 -0.410792 0.910614
+v 0.019137 -0.421616 0.026172
+vn -0.005490 -0.230757 0.972996
+v 0.018831 -0.419968 0.026719
+vn 0.002945 -0.273099 0.961981
+v 0.018572 -0.420502 0.026580
+vn -0.016308 -0.238603 0.970980
+v 0.020672 -0.419679 0.026816
+vn 0.008704 -0.244795 0.969536
+v 0.018346 -0.419581 0.026805
+vn 0.046996 -0.294909 0.954369
+v 0.016372 -0.419724 0.026794
+vn -0.658131 -0.747966 -0.086084
+v -0.019112 -0.420153 0.022046
+vn 0.041681 -0.285513 0.957468
+v 0.015700 -0.420734 0.026526
+vn -0.574299 -0.802124 -0.163636
+v -0.017601 -0.421216 0.021282
+vn 0.359021 -0.669155 0.650642
+v 0.002049 -0.424727 0.027668
+vn 0.282786 -0.770987 0.570624
+v 0.001037 -0.425339 0.027509
+vn -0.281779 0.210872 0.936020
+v -0.017692 -0.406812 0.030195
+vn 0.339121 -0.451350 0.825397
+v 0.000671 -0.423196 0.029416
+vn 0.324290 -0.434678 0.840173
+v -0.000264 -0.423266 0.029754
+vn 0.336826 -0.476390 0.812158
+v 0.000346 -0.423593 0.029328
+vn -0.418491 -0.897455 -0.139428
+v -0.014375 -0.423056 0.021096
+vn -0.430774 -0.897449 -0.094968
+v -0.013664 -0.423512 0.022123
+vn -0.466508 -0.884104 -0.027029
+v -0.015346 -0.422731 0.022782
+vn 0.348196 -0.520140 0.779881
+v 0.001109 -0.423905 0.028812
+vn -0.311106 0.037407 -0.949639
+v -0.015060 -0.411978 0.007921
+vn 0.305431 -0.650303 0.695571
+v 0.000461 -0.424749 0.028438
+vn 0.335876 -0.589929 0.734283
+v 0.001029 -0.424303 0.028559
+vn 0.055780 -0.987761 0.145662
+v 0.005154 -0.426299 0.023058
+vn 0.154397 -0.938514 0.308792
+v 0.002869 -0.426101 0.025044
+vn 0.061390 -0.934373 0.350966
+v 0.012456 -0.425996 0.023035
+vn 0.015917 -0.993753 0.110457
+v 0.009690 -0.426316 0.022045
+vn 0.044081 -0.972821 0.227324
+v 0.006925 -0.426178 0.023256
+vn 0.084979 -0.982393 0.166379
+v 0.014270 -0.426167 0.021878
+vn 0.068258 -0.808405 0.584656
+v 0.010949 -0.425507 0.024149
+vn 0.140001 -0.941554 0.306392
+v 0.015862 -0.425833 0.022562
+vn 0.227311 -0.973027 -0.039345
+v 0.018808 -0.425567 0.020629
+vn 0.182170 -0.971489 0.151733
+v 0.017387 -0.425810 0.021596
+vn 0.043463 -0.969355 0.241791
+v 0.011005 -0.426204 0.022540
+vn 0.131825 -0.789146 0.599892
+v 0.015536 -0.425086 0.024126
+vn 0.199577 -0.872278 0.446430
+v 0.017599 -0.425216 0.023287
+vn 0.295716 -0.941216 0.163295
+v 0.020135 -0.425149 0.021453
+vn 0.256432 -0.912476 0.318795
+v 0.019081 -0.425219 0.022386
+vn 0.229675 -0.795241 0.561106
+v 0.019288 -0.424330 0.024046
+vn 0.662237 -0.748499 0.034516
+v 0.029511 -0.419621 0.021034
+vn 0.457952 -0.833913 0.308008
+v 0.023996 -0.423049 0.022762
+vn 0.551940 -0.833880 0.002640
+v 0.025884 -0.422365 0.020254
+vn 0.579465 -0.808674 0.101325
+v 0.027327 -0.421321 0.021247
+vn 0.503230 -0.847582 0.168415
+v 0.024529 -0.423085 0.021529
+vn 0.323184 -0.851865 0.412163
+v 0.021232 -0.424143 0.023228
+vn 0.420606 -0.791057 0.444207
+v 0.023721 -0.422824 0.023624
+vn 0.329151 -0.750425 0.573168
+v 0.022342 -0.423109 0.024210
+vn 0.236194 -0.679586 0.694533
+v 0.020720 -0.423204 0.024830
+vn 0.236255 -0.583731 0.776815
+v 0.021867 -0.421820 0.025638
+vn 0.132661 -0.511632 0.848901
+v 0.020380 -0.421772 0.025988
+vn 0.384880 -0.878533 0.282926
+v 0.021905 -0.424300 0.022133
+vn 0.434330 -0.686831 0.582770
+v 0.024818 -0.421655 0.024346
+vn 0.419120 -0.895139 0.151868
+v 0.022281 -0.424355 0.021260
+vn 0.150579 -0.688418 0.709512
+v 0.017602 -0.424208 0.024672
+vn 0.603785 -0.753376 0.260516
+v 0.028721 -0.420047 0.022248
+vn 0.337113 -0.637194 0.693064
+v 0.023459 -0.421760 0.025074
+vn 0.629136 -0.651342 0.424195
+v 0.029961 -0.418520 0.023124
+vn 0.563798 -0.606606 0.560500
+v 0.029235 -0.418315 0.024237
+vn 0.507385 -0.745694 0.431857
+v 0.025774 -0.421621 0.023485
+vn 0.558806 -0.703643 0.438887
+v 0.028007 -0.420064 0.023337
+vn 0.539208 -0.795329 0.276959
+v 0.026475 -0.421624 0.022448
+vn 0.412498 -0.508561 0.755785
+v 0.026760 -0.418472 0.025923
+vn 0.425753 -0.597592 0.679425
+v 0.025881 -0.420089 0.025202
+vn 0.332685 -0.551399 0.765036
+v 0.024448 -0.420084 0.025956
+vn 0.505355 -0.649917 0.567648
+v 0.027099 -0.420031 0.024337
+vn -0.339848 0.146381 0.929019
+v -0.015403 -0.407109 0.031030
+vn 0.232954 -0.389040 0.891280
+v 0.023601 -0.419218 0.026768
+vn 0.188888 -0.240166 0.952177
+v 0.023742 -0.418576 0.026932
+vn 0.108866 -0.222276 0.968887
+v 0.023038 -0.418715 0.027015
+vn 0.010147 -0.315590 0.948841
+v 0.020732 -0.420218 0.026671
+vn 0.093382 -0.426867 0.899480
+v 0.021388 -0.420484 0.026548
+vn 0.191445 -0.138175 0.971729
+v 0.024885 -0.417183 0.026940
+vn 0.001833 -0.208172 0.978091
+v 0.022188 -0.419019 0.026989
+vn 0.203172 -0.172532 0.963823
+v 0.024793 -0.417690 0.026883
+vn -0.029995 -0.122335 0.992035
+v 0.022257 -0.418573 0.027066
+vn 0.003745 -0.051135 0.998685
+v 0.023042 -0.417842 0.027125
+vn 0.097690 -0.066549 0.992989
+v 0.024042 -0.417077 0.027097
+vn 0.147644 -0.379008 0.913539
+v 0.022779 -0.419474 0.026825
+vn 0.045570 -0.300378 0.952731
+v 0.022151 -0.419511 0.026876
+vn 0.221963 -0.511165 0.830327
+v 0.022708 -0.420340 0.026398
+vn 0.072547 -0.130489 0.988792
+v 0.023173 -0.418140 0.027101
+vn 0.241992 -0.268216 0.932470
+v 0.024656 -0.418322 0.026785
+vn 0.290454 -0.441791 0.848798
+v 0.024454 -0.419116 0.026550
+vn 0.268460 -0.299801 0.915450
+v 0.025847 -0.417832 0.026606
+vn 0.174145 -0.162123 0.971283
+v 0.023928 -0.417925 0.027024
+vn 0.137447 -0.098618 0.985588
+v 0.024040 -0.417423 0.027071
+vn -0.805828 -0.257589 -0.533187
+v -0.022089 -0.410559 0.013210
+vn 0.137328 -0.156031 0.978159
+v 0.026119 -0.415510 0.026977
+vn 0.169737 -0.172606 0.970256
+v 0.026020 -0.416081 0.026894
+vn 0.139120 -0.111933 0.983929
+v 0.025054 -0.416360 0.027017
+vn 0.126581 -0.129907 0.983413
+v 0.027300 -0.413515 0.027142
+vn 0.125494 -0.161277 0.978898
+v 0.027266 -0.414466 0.027003
+vn 0.262122 -0.183674 0.947394
+v 0.029883 -0.413339 0.026794
+vn 0.128643 -0.138678 0.981947
+v 0.028390 -0.413386 0.027037
+vn 0.406437 -0.216019 0.887775
+v 0.031070 -0.412549 0.026548
+vn 0.273961 -0.097354 0.956801
+v 0.030584 -0.412204 0.026759
+vn 0.396243 -0.087911 0.913927
+v 0.031728 -0.411433 0.026428
+vn 0.221296 -0.052553 0.973790
+v 0.030187 -0.411873 0.026880
+vn 0.157924 -0.182029 0.970528
+v 0.028806 -0.414191 0.026842
+vn 0.180218 -0.131349 0.974817
+v 0.029336 -0.413061 0.026936
+vn 0.273088 -0.276973 0.921254
+v 0.029243 -0.414359 0.026727
+vn 0.109310 -0.064980 0.991882
+v 0.028606 -0.412669 0.027086
+vn 0.472513 -0.029213 0.880839
+v 0.032436 -0.410211 0.026155
+vn 0.533532 -0.211039 0.819028
+v 0.032234 -0.411690 0.026128
+vn 0.518062 -0.082594 0.851346
+v 0.032589 -0.410813 0.026047
+vn 0.328538 -0.044930 0.943422
+v 0.031345 -0.411091 0.026592
+vn 0.143870 0.000880 0.989596
+v 0.029553 -0.411817 0.026991
+vn 0.159091 -0.081567 0.983889
+v 0.029253 -0.412501 0.027021
+vn 0.430035 -0.351240 0.831685
+v 0.030283 -0.413812 0.026518
+vn 0.224487 0.009402 0.974432
+v 0.030829 -0.410931 0.026743
+vn 0.141153 -0.175896 0.974236
+v 0.028176 -0.414225 0.026925
+vn 0.157241 -0.174342 0.972049
+v 0.026999 -0.416120 0.026723
+vn 0.352143 -0.381161 0.854817
+v 0.027235 -0.417203 0.026378
+vn 0.240408 -0.256204 0.936249
+v 0.027219 -0.416477 0.026616
+vn 0.200345 -0.187284 0.961658
+v 0.025898 -0.417218 0.026734
+vn 0.144523 -0.195767 0.969942
+v 0.027742 -0.415137 0.026801
+vn 0.149098 -0.190749 0.970250
+v 0.027098 -0.415238 0.026880
+vn 0.158622 -0.178936 0.970990
+v 0.026651 -0.415980 0.026799
+vn 0.378619 -0.369468 0.848611
+v 0.028872 -0.415419 0.026464
+vn 0.202444 -0.245379 0.948054
+v 0.028212 -0.415241 0.026710
+vn 0.184104 -0.156350 0.970392
+v 0.025720 -0.416559 0.026863
+vn 0.174334 -0.127916 0.976343
+v 0.024997 -0.416764 0.026977
+vn 0.175016 -0.144157 0.973955
+v 0.025855 -0.416794 0.026805
+vn 0.537610 -0.428558 0.726164
+v 0.030747 -0.414626 0.025821
+vn 0.455677 -0.453317 0.766069
+v 0.028749 -0.416670 0.025924
+vn 0.657099 -0.560162 0.504420
+v 0.031071 -0.416729 0.023904
+vn 0.742491 -0.603492 0.290697
+v 0.031484 -0.417199 0.022545
+vn -0.567536 -0.809680 0.149403
+v -0.016331 -0.421877 0.026445
+vn -0.530002 -0.845030 0.070866
+v -0.016543 -0.422016 0.024343
+vn 0.663684 -0.473437 0.579121
+v 0.031955 -0.414860 0.024593
+vn 0.558853 -0.508865 0.654782
+v 0.030143 -0.416483 0.025080
+vn 0.838777 -0.496135 0.224285
+v 0.033858 -0.414084 0.021904
+vn 0.691583 -0.691095 0.210001
+v 0.030327 -0.418705 0.022071
+vn 0.344979 -0.446773 0.825459
+v 0.025745 -0.418561 0.026355
+vn 0.490871 -0.557399 0.669591
+v 0.028114 -0.418301 0.025205
+vn -0.043977 -0.002312 0.999030
+v -0.006434 -0.420724 0.031474
+vn -0.058552 0.055283 0.996752
+v -0.007695 -0.420458 0.031393
+vn 0.003712 -0.233213 0.972419
+v -0.004804 -0.421935 0.031385
+vn -0.028470 0.044131 0.998620
+v -0.008512 -0.418804 0.031215
+vn 0.069263 0.003547 0.997592
+v -0.010234 -0.418396 0.031242
+vn 0.036976 0.014869 0.999206
+v -0.010148 -0.419481 0.031252
+vn 0.025772 -0.027828 0.999280
+v -0.010796 -0.419296 0.031280
+vn -0.010238 0.041809 0.999073
+v -0.009658 -0.420062 0.031268
+vn -0.062592 0.077243 0.995046
+v -0.007953 -0.419649 0.031305
+vn -0.023482 0.061333 0.997841
+v -0.008879 -0.419596 0.031259
+vn -0.030573 0.059143 0.997781
+v -0.009222 -0.420091 0.031282
+vn -0.051690 0.078692 0.995558
+v -0.008632 -0.420260 0.031320
+vn -0.369828 0.376930 0.849206
+v -0.018540 -0.399309 0.027331
+vn -0.200606 0.395987 0.896076
+v -0.020547 -0.400905 0.027519
+vn -0.202837 0.339346 0.918532
+v -0.020680 -0.402379 0.028106
+vn -0.252782 0.405103 0.878631
+v -0.019387 -0.399797 0.027272
+vn -0.219658 0.419147 0.880946
+v -0.019688 -0.399266 0.026954
+vn -0.205674 0.394329 0.895658
+v -0.020038 -0.400828 0.027599
+vn -0.238228 0.390348 0.889312
+v -0.019380 -0.400941 0.027800
+vn -0.226930 0.346442 0.910209
+v -0.019816 -0.402135 0.028205
+vn -0.247554 0.226726 0.941972
+v -0.020838 -0.406264 0.029220
+vn -0.240586 0.250289 0.937803
+v -0.019881 -0.405306 0.029218
+vn -0.253499 0.206608 0.945014
+v -0.020215 -0.407272 0.029612
+vn -0.250703 0.263560 0.931496
+v -0.021335 -0.404217 0.028565
+vn 0.044493 -0.443215 0.895310
+v 0.016505 -0.422822 0.025723
+vn 0.112910 -0.327348 0.938134
+v 0.014174 -0.420505 0.026706
+vn -0.583782 -0.688047 -0.431033
+v -0.013738 -0.421746 -0.031472
+vn 0.205728 -0.975765 0.074556
+v 0.017281 -0.426782 -0.018493
+vn 0.543082 -0.810692 -0.218724
+v 0.020062 -0.425801 -0.018363
+vn -0.211399 -0.200104 0.956697
+v -0.013918 -0.419732 0.030963
+vn -0.114172 -0.106066 0.987783
+v -0.011759 -0.420425 0.031189
+vn -0.471957 -0.870304 -0.140807
+v -0.010930 -0.425443 -0.026617
+vn -0.435959 -0.873999 -0.214629
+v -0.010566 -0.425312 -0.028139
+vn -0.116416 -0.130133 0.984638
+v -0.012923 -0.419297 0.031195
+vn -0.029688 -0.072803 0.996904
+v -0.011784 -0.419115 0.031289
+vn -0.058466 -0.068906 0.995908
+v -0.011519 -0.420084 0.031233
+vn -0.188764 -0.224080 0.956115
+v -0.011057 -0.421920 0.031101
+vn -0.037324 -0.061130 0.997432
+v -0.007783 -0.421296 0.031407
+vn -0.254973 -0.955515 -0.148255
+v -0.005356 -0.427904 -0.024695
+vn -0.314475 -0.277943 0.907664
+v -0.015620 -0.419624 0.030516
+vn -0.323261 -0.902249 0.285393
+v -0.011032 -0.424212 0.028221
+vn -0.306966 -0.375861 0.874357
+v -0.013766 -0.421243 0.030572
+vn -0.237152 -0.246354 0.939717
+v -0.013015 -0.421034 0.030877
+vn -0.018992 -0.027466 0.999442
+v -0.010694 -0.420130 0.031259
+vn -0.048988 -0.016122 0.998669
+v -0.010707 -0.420565 0.031252
+vn -0.049923 0.042397 0.997853
+v -0.008845 -0.420738 0.031345
+vn -0.045792 -0.726037 -0.686130
+v -0.004603 -0.425761 -0.031394
+vn 0.064733 -0.913086 0.402597
+v 0.015089 -0.423437 -0.004876
+vn -0.103069 -0.928362 -0.357102
+v -0.002865 -0.427524 -0.028690
+vn -0.098525 -0.868048 -0.486606
+v -0.004035 -0.426743 -0.030156
+vn -0.316373 -0.571868 0.756885
+v -0.012319 -0.422268 0.030544
+vn -0.374010 -0.393780 0.839675
+v -0.015478 -0.420403 0.030289
+vn -0.037572 0.033333 0.998738
+v -0.009873 -0.420592 0.031286
+vn -0.064795 -0.022590 0.997643
+v -0.009916 -0.420992 0.031300
+vn -0.387062 -0.536887 0.749624
+v -0.014234 -0.421441 0.030267
+vn -0.020652 -0.839738 -0.542598
+v -0.002423 -0.426841 -0.030136
+vn -0.000414 0.017562 0.999846
+v -0.010030 -0.420139 0.031267
+vn -0.288450 -0.883258 0.369664
+v -0.009810 -0.424359 0.028967
+vn -0.274463 -0.409488 0.870052
+v -0.011816 -0.422057 0.030858
+vn -0.220793 -0.414513 0.882853
+v -0.010422 -0.422511 0.031050
+vn -0.290390 -0.749275 0.595198
+v -0.011084 -0.423102 0.030275
+vn -0.265554 -0.647146 0.714621
+v -0.010527 -0.422820 0.030808
+vn -0.094391 -0.121983 0.988034
+v -0.009936 -0.421986 0.031261
+vn -0.326869 -0.830411 0.451192
+v -0.011259 -0.423626 0.029407
+vn -0.134148 -0.114727 0.984298
+v -0.011333 -0.421167 0.031168
+vn -0.367226 -0.681965 0.632510
+v -0.013027 -0.422289 0.030198
+vn -0.038568 0.061998 0.997331
+v -0.009350 -0.420415 0.031296
+vn -0.392118 -0.776784 0.492798
+v -0.013066 -0.422652 0.029695
+vn 0.003523 -0.255782 0.966728
+v 0.016873 -0.420748 0.026505
+vn 0.331326 -0.906911 -0.260260
+v 0.012117 -0.427732 -0.023795
+vn -0.213769 -0.965697 -0.147421
+v -0.003755 -0.428120 -0.025516
+vn -0.225214 -0.936233 0.269716
+v -0.008720 -0.424838 0.028506
+vn -0.600813 -0.739169 0.304389
+v -0.016797 -0.420999 0.028286
+vn -0.463600 -0.673177 0.576114
+v -0.014748 -0.421541 0.029858
+vn 0.053161 -0.947072 0.316588
+v 0.008348 -0.426107 0.023346
+vn -0.493772 -0.543932 0.678474
+v -0.016154 -0.420329 0.029951
+vn -0.574953 -0.817078 -0.042570
+v -0.012475 -0.424693 -0.023621
+vn 0.258223 -0.959930 0.108882
+v 0.019533 -0.426067 -0.016660
+vn 0.395950 -0.918139 -0.015606
+v 0.020044 -0.425932 -0.017418
+vn 0.153788 -0.897808 0.412663
+v 0.004692 -0.425866 0.024766
+vn 0.679257 -0.684614 -0.264411
+v 0.022788 -0.424420 -0.015790
+vn -0.433637 -0.897864 0.076155
+v -0.009390 -0.426464 -0.021913
+vn 0.389442 -0.920083 0.042219
+v 0.021506 -0.425279 -0.015316
+vn 0.518624 -0.848900 -0.101969
+v 0.021516 -0.425239 -0.016465
+vn 0.347495 -0.923778 -0.160876
+v 0.018004 -0.426603 -0.019898
+vn 0.576416 -0.698165 -0.424630
+v 0.018302 -0.425891 -0.021127
+vn 0.067736 -0.744299 0.664403
+v 0.012435 -0.425203 0.024394
+vn 0.098974 -0.875899 0.472234
+v 0.014105 -0.425666 0.023513
+vn 0.730290 -0.551652 -0.402935
+v 0.021998 -0.424513 -0.017306
+vn 0.087274 -0.562160 0.822410
+v 0.016500 -0.423740 0.025199
+vn -0.502454 -0.860154 0.087609
+v -0.014309 -0.423219 0.026358
+vn 0.648409 -0.651978 -0.393053
+v 0.020143 -0.425378 -0.019184
+vn 0.629806 -0.723886 -0.281662
+v 0.021312 -0.425170 -0.017467
+vn 0.112487 -0.986711 -0.117250
+v 0.007280 -0.428563 -0.024466
+vn -0.738971 -0.673143 -0.028306
+v -0.020745 -0.418557 0.021896
+vn -0.399485 -0.902323 -0.161939
+v -0.012426 -0.424003 0.021478
+vn -0.361935 -0.849127 -0.384690
+v -0.008852 -0.425242 -0.030614
+vn -0.072066 -0.995814 0.056222
+v 0.000884 -0.428915 -0.020711
+vn -0.171486 -0.938868 -0.298531
+v -0.006066 -0.424780 0.017362
+vn -0.069803 -0.940280 -0.333169
+v 0.003343 -0.425092 0.015205
+vn -0.137367 -0.957287 -0.254425
+v -0.004490 -0.425313 0.018327
+vn -0.437113 -0.832722 -0.339862
+v -0.010534 -0.424765 -0.029837
+vn -0.288609 -0.929471 -0.229758
+v -0.010404 -0.424034 0.018441
+vn -0.378333 -0.732941 0.565386
+v -0.007483 -0.424289 -0.012431
+vn -0.622427 -0.776732 -0.096296
+v -0.018890 -0.420202 0.021068
+vn -0.569887 -0.813175 -0.118215
+v -0.018371 -0.420523 0.020320
+vn -0.496237 -0.838179 -0.226286
+v -0.016593 -0.421488 0.019470
+vn -0.407272 -0.888135 -0.212945
+v -0.013697 -0.422868 0.018891
+vn -0.180161 -0.963330 -0.198839
+v -0.006823 -0.425324 0.019997
+vn -0.234384 -0.939806 -0.248654
+v -0.008567 -0.424737 0.019153
+vn -0.488598 -0.872213 0.022733
+v -0.014655 -0.423108 0.024404
+vn -0.502726 -0.842813 -0.192181
+v -0.016451 -0.421712 0.020153
+vn -0.131308 -0.904587 -0.405563
+v -0.002828 -0.424644 0.015718
+vn -0.103849 -0.940731 -0.322863
+v -0.001421 -0.425161 0.016576
+vn -0.198192 -0.661325 -0.723442
+v -0.008129 -0.424548 -0.032110
+vn -0.351373 -0.912386 -0.209971
+v -0.011986 -0.423899 0.020161
+vn -0.312225 -0.923871 -0.221308
+v -0.010982 -0.424388 0.020623
+vn -0.360319 -0.925472 -0.116927
+v -0.012001 -0.424240 0.021894
+vn -0.535832 -0.092995 0.839188
+v -0.021274 -0.410740 0.029621
+vn -0.225560 0.284103 0.931884
+v -0.020581 -0.404048 0.028687
+vn -0.291173 0.217148 0.931700
+v -0.021324 -0.406096 0.029051
+vn -0.262947 0.165775 0.950462
+v -0.020683 -0.408308 0.029683
+vn -0.430669 0.047971 0.901234
+v -0.021046 -0.409579 0.029759
+vn -0.304689 0.171080 0.936961
+v -0.021127 -0.407939 0.029499
+vn -0.290438 0.097624 0.951901
+v -0.020774 -0.409455 0.029838
+vn -0.700247 -0.004990 0.713883
+v -0.023087 -0.408984 0.028436
+vn -0.518270 0.197207 0.832169
+v -0.022423 -0.405388 0.028455
+vn -0.429104 -0.901561 -0.055293
+v -0.013023 -0.423908 0.023004
+vn -0.517176 -0.802157 -0.298450
+v -0.012094 -0.424137 -0.029192
+vn -0.225364 0.385703 0.894676
+v -0.021125 -0.401565 0.027683
+vn -0.541758 0.070670 0.837559
+v -0.022489 -0.408015 0.028853
+vn -0.697322 0.136501 0.703640
+v -0.023013 -0.406486 0.028248
+vn -0.490157 0.372097 0.788219
+v -0.021922 -0.401536 0.027379
+vn -0.702021 0.352471 0.618814
+v -0.022524 -0.401559 0.026904
+vn -0.346727 0.257693 0.901873
+v -0.021837 -0.404085 0.028383
+vn -0.924531 -0.212241 -0.316538
+v -0.024015 -0.410938 0.017372
+vn -0.400636 0.162872 0.901645
+v -0.021873 -0.406542 0.028946
+vn -0.537221 0.300510 0.788091
+v -0.022253 -0.403185 0.027884
+vn -0.671162 -0.634056 0.384077
+v -0.018314 -0.419456 0.028523
+vn -0.359420 0.425283 0.830633
+v -0.021561 -0.400480 0.027035
+vn -0.341501 0.342140 0.875395
+v -0.021654 -0.402431 0.027886
+vn -0.221216 0.329671 0.917813
+v -0.021235 -0.402631 0.028082
+vn -0.288974 0.404812 0.867537
+v -0.021364 -0.401104 0.027403
+vn -0.650225 -0.720040 0.242383
+v -0.018221 -0.420216 0.027051
+vn -0.268523 0.457147 0.847887
+v -0.020423 -0.398943 0.026594
+vn -0.280692 0.479904 0.831207
+v -0.019737 -0.397960 0.026278
+vn -0.793498 -0.375557 -0.478871
+v -0.021868 -0.412840 0.014323
+vn -0.269739 0.444593 0.854153
+v -0.021065 -0.399860 0.026880
+vn -0.231725 0.432061 0.871566
+v -0.020061 -0.399223 0.026843
+vn -0.233620 0.424315 0.874859
+v -0.020753 -0.400309 0.027193
+vn -0.141469 0.790756 0.595559
+v -0.018159 -0.395363 0.024748
+vn -0.177303 0.707407 0.684207
+v -0.017971 -0.395830 0.025330
+vn -0.312715 0.509730 0.801489
+v -0.020260 -0.397781 0.025987
+vn -0.280260 0.509454 0.813579
+v -0.019169 -0.397243 0.026052
+vn -0.313383 0.569693 0.759763
+v -0.019540 -0.396855 0.025646
+vn -0.325764 0.485255 0.811422
+v -0.020908 -0.398734 0.026312
+vn -0.370621 0.467795 0.802377
+v -0.021402 -0.399590 0.026617
+vn -0.232158 0.600093 0.765501
+v -0.018243 -0.396388 0.025749
+vn -0.305481 0.554212 0.774294
+v -0.018119 -0.396998 0.026228
+vn -0.310879 0.787017 0.532878
+v -0.019148 -0.395208 0.024160
+vn -0.240613 0.757718 0.606605
+v -0.018678 -0.395453 0.024720
+vn -0.309101 0.668024 0.676905
+v -0.019059 -0.395967 0.025109
+vn -0.220541 0.677281 0.701892
+v -0.018488 -0.395907 0.025267
+vn -0.019547 0.939229 -0.342733
+v -0.015226 -0.394144 0.013024
+vn -0.681129 -0.584346 -0.441139
+v -0.018343 -0.416093 0.012088
+vn -0.377404 0.492988 0.783919
+v -0.018142 -0.397772 0.026730
+vn -0.270438 -0.938546 -0.214465
+v -0.006049 -0.427116 -0.027867
+vn -0.589118 -0.805965 -0.057965
+v -0.017937 -0.421123 0.022354
+vn -0.316571 -0.243391 0.916812
+v -0.016611 -0.417976 0.030658
+vn -0.247751 0.118330 -0.961570
+v -0.014781 -0.405617 0.008581
+vn -0.295160 0.169794 0.940240
+v -0.016566 -0.407845 0.030764
+vn 0.052623 -0.433082 0.899817
+v 0.014480 -0.422857 0.025802
+vn 0.104271 -0.471271 0.875803
+v 0.012059 -0.423147 0.025852
+vn 0.067719 -0.656260 0.751490
+v 0.013270 -0.424875 0.024664
+vn 0.034060 -0.355301 0.934131
+v 0.015864 -0.421719 0.026229
+vn -0.458457 -0.277496 0.844283
+v -0.018007 -0.417775 0.030107
+vn -0.249084 0.084647 -0.964776
+v -0.011991 -0.412698 0.006987
+vn 0.149797 -0.595957 0.788921
+v 0.019134 -0.423084 0.025319
+vn 0.327859 -0.623419 0.709829
+v 0.003595 -0.424056 0.027439
+vn 0.285828 -0.782580 0.553056
+v 0.002918 -0.425377 0.026392
+vn -0.275669 -0.898286 -0.342183
+v -0.007013 -0.426221 -0.029874
+vn -0.297975 -0.711966 0.635857
+v 0.008022 -0.422764 -0.004718
+vn -0.355249 -0.898119 -0.259193
+v -0.008584 -0.425996 -0.028868
+vn -0.135884 -0.358945 -0.923414
+v -0.012176 -0.419346 -0.033846
+vn 0.682979 -0.303487 -0.664406
+v 0.030641 -0.415376 -0.013578
+vn -0.078924 -0.842454 0.532955
+v 0.006313 -0.425673 -0.008765
+vn -0.142293 -0.774443 0.616434
+v 0.006205 -0.424878 -0.007639
+vn -0.235515 -0.691911 0.682490
+v 0.006566 -0.423785 -0.006330
+vn -0.043276 0.878279 -0.476186
+v -0.012992 -0.394784 0.011542
+vn -0.414643 0.409887 0.812443
+v -0.018165 -0.398481 0.027112
+vn -0.307422 0.423221 0.852277
+v -0.018983 -0.398620 0.026835
+vn -0.242628 0.450249 0.859307
+v -0.019350 -0.398050 0.026446
+vn -0.172038 -0.810730 0.559571
+v 0.009491 -0.423099 -0.004575
+vn -0.163951 -0.846043 -0.507279
+v -0.005765 -0.426109 -0.030772
+vn -0.161677 -0.807617 0.567111
+v 0.004485 -0.425618 -0.009070
+vn -0.234433 -0.733804 0.637630
+v 0.002846 -0.425355 -0.009275
+vn -0.304748 -0.611812 0.729942
+v 0.003439 -0.424216 -0.007937
+vn -0.334129 -0.581194 0.742005
+v 0.005136 -0.423331 -0.006494
+vn -0.234982 -0.772196 0.590336
+v 0.000288 -0.425813 -0.010797
+vn -0.241128 -0.696175 0.676164
+v 0.004871 -0.424628 -0.007753
+vn 0.370557 -0.857415 -0.357108
+v 0.014010 -0.426993 -0.023701
+vn 0.672433 -0.577101 -0.463452
+v 0.019533 -0.425046 -0.020596
+vn -0.131430 -0.987999 -0.081139
+v -0.002142 -0.428630 -0.023694
+vn 0.054577 -0.503503 0.862268
+v 0.014656 -0.423805 0.025299
+vn 0.267016 -0.963423 0.022761
+v 0.018507 -0.426501 -0.018613
+vn -0.560610 -0.013853 -0.827964
+v -0.019782 -0.408650 0.009783
+vn 0.437745 -0.889984 -0.127705
+v 0.018997 -0.426311 -0.018963
+vn 0.544187 -0.772034 -0.328367
+v 0.018703 -0.426203 -0.019944
+vn 0.023070 -0.358753 0.933147
+v 0.017936 -0.421384 0.026296
+vn 0.741030 -0.481929 -0.467568
+v 0.021170 -0.424408 -0.018814
+vn -0.284585 0.464852 0.838406
+v -0.018890 -0.397869 0.026481
+vn -0.250289 0.511344 0.822121
+v -0.018732 -0.397111 0.026100
+vn -0.284250 0.753088 0.593347
+v -0.016446 -0.395604 0.025586
+vn -0.250718 0.013551 0.967965
+v -0.017099 -0.413348 0.031204
+vn -0.269739 0.586957 0.763363
+v -0.018825 -0.396537 0.025687
+vn 0.687400 -0.482059 -0.543231
+v 0.019077 -0.424774 -0.021529
+vn -0.013655 0.999895 0.004771
+v -0.015030 -0.393553 0.018161
+vn 0.170851 -0.984818 0.030729
+v 0.011997 -0.427950 -0.021498
+vn 0.082727 -0.861009 0.501816
+v 0.008965 -0.425716 0.024070
+vn 0.180231 -0.974609 0.132872
+v 0.015443 -0.426661 -0.015657
+vn 0.160376 -0.974773 0.155232
+v 0.015813 -0.426796 -0.016934
+vn 0.450491 -0.539386 -0.711421
+v 0.012741 -0.426271 -0.025723
+vn -0.259255 -0.038356 -0.965047
+v -0.008226 -0.413536 0.006006
+vn 0.048591 0.114034 -0.992288
+v -0.016473 -0.402011 0.009292
+vn 0.014751 0.124140 -0.992155
+v -0.016651 -0.401059 0.009382
+vn 0.356987 -0.487129 0.797036
+v 0.001581 -0.423444 0.028885
+vn -0.042315 0.138387 -0.989474
+v -0.015992 -0.401484 0.009395
+vn 0.351116 -0.522010 0.777318
+v 0.002952 -0.423069 0.028494
+vn 0.373840 -0.560353 0.739086
+v 0.002202 -0.423969 0.028244
+vn -0.094752 0.994730 0.039161
+v -0.017671 -0.393405 0.018522
+vn -0.052665 0.989292 -0.136120
+v -0.016957 -0.393461 0.015829
+vn 0.055536 0.983104 -0.174422
+v -0.015215 -0.393711 0.014670
+vn 0.251382 -0.607092 0.753821
+v 0.005561 -0.423724 0.026927
+vn 0.194779 -0.698014 -0.689084
+v 0.003659 -0.427083 -0.029333
+vn 0.168709 -0.979677 0.108489
+v 0.014925 -0.427246 -0.018907
+vn 0.587932 -0.169309 -0.790993
+v 0.015355 -0.422347 -0.025474
+vn -0.209152 0.150393 -0.966249
+v -0.015417 -0.400828 0.009440
+vn 0.047444 -0.669091 -0.741665
+v -0.002698 -0.425878 -0.031292
+vn -0.377705 -0.922801 0.076016
+v -0.012574 -0.424102 0.026029
+vn -0.429273 -0.876886 0.216324
+v -0.013564 -0.423468 0.027394
+vn 0.053403 -0.321663 -0.945347
+v -0.006068 -0.423704 -0.032694
+vn 0.018467 -0.536983 -0.843391
+v -0.004768 -0.424896 -0.032111
+vn 0.125572 -0.393265 -0.910810
+v -0.003847 -0.424738 -0.032095
+vn 0.793866 -0.273608 -0.543062
+v 0.021764 -0.423511 -0.018553
+vn 0.806940 -0.233871 -0.542357
+v 0.022883 -0.422868 -0.017166
+vn 0.045273 0.986704 0.156096
+v -0.017210 -0.393609 0.020373
+vn 0.182917 -0.955489 -0.231478
+v 0.009734 -0.428129 -0.024855
+vn 0.794236 -0.184329 -0.578974
+v 0.023372 -0.422117 -0.016707
+vn 0.692637 -0.633317 -0.345200
+v 0.024734 -0.423142 -0.013930
+vn 0.572061 -0.812086 -0.115162
+v 0.023226 -0.424364 -0.014433
+vn 0.283396 -0.453384 0.845062
+v 0.006412 -0.421116 0.028361
+vn 0.588310 0.382167 0.712629
+v 0.033284 -0.403588 0.023387
+vn 0.460904 0.323546 0.826369
+v 0.032856 -0.406757 0.025226
+vn -0.102054 0.941969 0.319810
+v -0.016779 -0.394230 0.022930
+vn -0.075494 0.944350 0.320163
+v -0.018191 -0.394029 0.022273
+vn 0.014824 0.965495 0.259998
+v -0.017496 -0.393895 0.021804
+vn -0.032602 0.984530 0.172154
+v -0.016400 -0.393778 0.021156
+vn -0.243061 0.897391 0.368253
+v -0.015303 -0.394431 0.024207
+vn -0.205034 0.856698 0.473317
+v -0.016594 -0.394864 0.024431
+vn -0.366645 0.807739 0.461658
+v -0.013856 -0.394844 0.026090
+vn 0.487170 -0.740498 -0.462956
+v 0.016558 -0.426220 -0.022684
+vn -0.524383 -0.714458 -0.463220
+v -0.016205 -0.417794 0.011721
+vn -0.343611 -0.937789 -0.049829
+v -0.011855 -0.424389 0.022977
+vn -0.298989 -0.946269 -0.123211
+v -0.011011 -0.424549 0.021574
+vn -0.272898 0.162405 0.948236
+v -0.017633 -0.408648 0.030586
+vn -0.135137 0.988619 0.066114
+v -0.014748 -0.393597 0.020399
+vn -0.170829 0.964310 0.202298
+v -0.015168 -0.393916 0.022528
+vn -0.204157 -0.814273 -0.543396
+v -0.008088 -0.421842 0.012644
+vn -0.378258 0.906189 -0.189057
+v -0.010589 -0.392836 0.015731
+vn 0.095351 0.021910 -0.995203
+v -0.014608 -0.412168 -0.035051
+vn -0.216638 -0.837381 -0.501857
+v -0.008731 -0.422293 0.013604
+vn -0.193145 0.106822 -0.975338
+v -0.017336 -0.403248 -0.034735
+vn -0.173567 0.238193 -0.955583
+v -0.017213 -0.402467 -0.034626
+vn -0.357602 0.215793 -0.908600
+v -0.017989 -0.402630 -0.034441
+vn -0.120303 0.003400 -0.992731
+v -0.007776 -0.406642 -0.034873
+vn -0.028520 0.180526 -0.983157
+v -0.016447 -0.402713 -0.034747
+vn 0.087672 0.216962 0.972235
+v -0.012712 -0.398270 -0.012355
+vn 0.099799 0.120112 0.987731
+v -0.012998 -0.399200 -0.012166
+vn 0.105296 0.053464 0.993003
+v -0.013303 -0.400714 -0.012000
+vn 0.087295 0.011366 0.996118
+v -0.013761 -0.402864 -0.011888
+vn 0.103285 0.152444 0.982900
+v -0.012181 -0.398892 -0.012295
+vn 0.098619 0.149914 0.983768
+v -0.012527 -0.398769 -0.012275
+vn -0.262739 0.113252 0.958197
+v -0.016428 -0.409960 0.031133
+vn 0.214538 0.859726 -0.463513
+v -0.013082 -0.396871 -0.030206
+vn 0.168961 0.803813 -0.570383
+v -0.013430 -0.397321 -0.031059
+vn 0.162124 0.901690 -0.400839
+v -0.013839 -0.396288 -0.029344
+vn 0.136279 0.835511 -0.532306
+v -0.013718 -0.397020 -0.030713
+vn 0.019924 0.867494 -0.497048
+v -0.014603 -0.396518 -0.030008
+vn 0.034194 0.787771 -0.615018
+v -0.014275 -0.397432 -0.031394
+vn 0.049214 0.897643 -0.437967
+v -0.014785 -0.396097 -0.029235
+vn 0.165684 0.931474 -0.323889
+v -0.014163 -0.395970 -0.028688
+vn 0.144271 0.876384 -0.459496
+v -0.013692 -0.396664 -0.030073
+vn -0.007176 0.317874 0.948106
+v 0.011045 -0.374783 0.023986
+vn -0.161549 0.337014 0.927536
+v 0.008604 -0.374434 0.023636
+vn 0.106814 0.508983 0.854123
+v 0.012254 -0.374640 0.023860
+vn 0.205428 0.978618 -0.010297
+v 0.010638 -0.371175 -0.013367
+vn -0.045162 0.948984 -0.312075
+v 0.009808 -0.371510 -0.016893
+vn 0.055489 0.991824 -0.114916
+v 0.009185 -0.371064 -0.015002
+vn 0.047714 0.258167 0.964921
+v 0.014064 -0.376884 0.024500
+vn -0.205143 0.899313 -0.386203
+v 0.007892 -0.371738 -0.016865
+vn -0.240994 0.853978 -0.461133
+v 0.007298 -0.372482 -0.018134
+vn 0.345968 0.561621 0.751591
+v 0.022362 -0.378037 0.023028
+vn 0.616710 0.363968 0.697994
+v 0.026035 -0.380042 0.021806
+vn -0.618403 0.697078 0.362849
+v 0.002932 -0.372876 0.020148
+vn -0.091147 0.941503 0.324444
+v 0.003978 -0.372570 0.020596
+vn 0.715553 0.460544 0.525245
+v 0.028211 -0.380327 0.019772
+vn 0.931483 0.191173 0.309503
+v 0.030086 -0.382768 0.017925
+vn 0.838501 0.386710 0.383889
+v 0.029655 -0.380583 0.017439
+vn -0.083719 0.979724 0.182021
+v 0.003443 -0.371759 0.016859
+vn 0.017378 0.995600 0.092079
+v 0.004191 -0.371164 0.012263
+vn 0.984531 0.038452 0.170939
+v 0.030723 -0.382899 0.015796
+vn -0.565890 0.814295 -0.129199
+v 0.004362 -0.371135 0.004845
+vn 0.096390 0.995054 0.024022
+v 0.005631 -0.370907 0.004901
+vn 0.990523 0.135950 -0.019535
+v 0.029881 -0.378641 0.004634
+vn -0.075708 0.996683 0.029861
+v 0.005998 -0.370842 0.000837
+vn -0.198781 0.979669 0.027103
+v 0.005835 -0.370805 -0.000280
+vn -0.088644 0.994826 0.049625
+v 0.006266 -0.370723 -0.001678
+vn 0.070053 0.992826 0.096896
+v 0.006579 -0.370567 -0.003451
+vn 0.235096 0.970556 0.052444
+v 0.007731 -0.370664 -0.005140
+vn 0.928208 0.345402 -0.138303
+v 0.028845 -0.377877 -0.003319
+vn 0.964172 0.185272 -0.189860
+v 0.029242 -0.379807 -0.003879
+vn 0.102362 0.994222 -0.032314
+v 0.007509 -0.370626 -0.010046
+vn 0.165519 0.986175 -0.007895
+v 0.007595 -0.370542 -0.007754
+vn -0.134634 0.989496 -0.052641
+v 0.005665 -0.370681 -0.010402
+vn -0.284543 0.958577 -0.012889
+v 0.005332 -0.370727 -0.008633
+vn 0.001330 0.995730 -0.092299
+v 0.007315 -0.370794 -0.012675
+vn -0.112290 0.952127 -0.284333
+v 0.008202 -0.371251 -0.015730
+vn -0.146938 0.968789 -0.199645
+v 0.007208 -0.371061 -0.014438
+vn 0.834512 0.264364 -0.483427
+v 0.025874 -0.377455 -0.011815
+vn 0.800303 0.480064 -0.359240
+v 0.025735 -0.375883 -0.010803
+vn 0.800046 0.498493 -0.333813
+v 0.024797 -0.375314 -0.012178
+vn 0.688322 0.404255 -0.602321
+v 0.022927 -0.375226 -0.015522
+vn 0.491635 0.803303 -0.336153
+v 0.023096 -0.374718 -0.014668
+vn 0.617397 0.323322 -0.717136
+v 0.020209 -0.375360 -0.018152
+vn 0.573471 0.377080 -0.727284
+v 0.019108 -0.374438 -0.018619
+vn -0.558689 0.808900 0.183158
+v 0.002548 -0.372097 0.016823
+vn -0.353279 0.932225 0.078427
+v 0.003076 -0.371462 0.013575
+vn 0.308450 0.780522 -0.543732
+v 0.018370 -0.373646 -0.018642
+vn 0.467130 0.308986 -0.828443
+v 0.018325 -0.375030 -0.019435
+vn 0.300117 0.398541 -0.866657
+v 0.016014 -0.376632 -0.020998
+vn 0.319630 0.488428 -0.811958
+v 0.015027 -0.374644 -0.020378
+vn 0.125835 0.411981 -0.902462
+v 0.013841 -0.375191 -0.020929
+vn 0.084760 0.506930 -0.857810
+v 0.013091 -0.373882 -0.020347
+vn 0.038710 0.998949 0.024563
+v 0.006258 -0.370414 -0.004757
+vn -0.175205 0.971356 0.160534
+v 0.005745 -0.370513 -0.003673
+vn 0.047944 0.407113 -0.912118
+v 0.012017 -0.375789 -0.021333
+vn -0.447154 0.893592 0.039333
+v 0.005409 -0.370537 -0.004171
+vn -0.056246 0.596332 -0.800765
+v 0.011374 -0.373472 -0.020156
+vn -0.329045 0.916910 -0.225846
+v 0.005848 -0.371134 -0.013228
+vn -0.151321 0.751802 -0.641791
+v 0.008291 -0.372974 -0.019195
+vn -0.324681 0.880847 -0.344516
+v 0.006812 -0.371520 -0.015592
+vn -0.233106 0.713902 -0.660307
+v 0.007142 -0.373300 -0.019257
+vn -0.384666 0.749963 -0.538134
+v 0.006525 -0.373040 -0.018650
+vn -0.667963 0.686513 -0.287273
+v 0.005443 -0.373031 -0.017265
+vn -0.825632 0.481766 -0.293654
+v 0.004060 -0.372658 -0.013048
+vn -0.698328 0.695628 -0.168640
+v 0.004243 -0.371382 -0.010842
+vn -0.588654 0.805454 0.068773
+v 0.004459 -0.371125 -0.008987
+vn -0.954516 0.295846 0.037072
+v 0.004378 -0.372278 -0.002521
+vn -0.732039 0.670180 0.122383
+v 0.004997 -0.370997 -0.003122
+vn -0.829770 0.555725 0.051505
+v 0.004780 -0.371139 -0.003870
+vn -0.460625 0.884591 -0.072963
+v 0.004986 -0.371046 0.002319
+vn -0.814336 0.560997 -0.148793
+v 0.004127 -0.371534 0.003807
+vn -0.278068 0.959718 -0.040235
+v 0.005016 -0.370922 0.003574
+vn -0.633361 0.744197 -0.212190
+v 0.003744 -0.371053 0.006828
+vn -0.884168 0.462368 0.066809
+v 0.001655 -0.373219 0.016356
+vn -0.763751 0.639544 0.087570
+v 0.002190 -0.372337 0.016198
+vn -0.818548 0.549300 0.168071
+v 0.001966 -0.372929 0.017406
+vn -0.563091 0.791557 0.237415
+v 0.002611 -0.372376 0.017976
+vn -0.829846 0.512945 0.219642
+v 0.001848 -0.373571 0.018657
+vn -0.777254 0.424925 0.464021
+v 0.002033 -0.374317 0.020199
+vn -0.821557 0.396118 0.410041
+v 0.001134 -0.376071 0.020194
+vn -0.687312 0.503910 0.523141
+v 0.002991 -0.373106 0.020587
+vn -0.731975 0.399407 0.551984
+v 0.002607 -0.373993 0.020804
+vn -0.656778 0.460603 0.597066
+v 0.003055 -0.373378 0.020903
+vn -0.421289 0.687416 0.591587
+v 0.003443 -0.372916 0.020864
+vn -0.506089 0.509144 0.696166
+v 0.003760 -0.373734 0.021830
+vn -0.532056 0.363576 0.764676
+v 0.003690 -0.374408 0.022168
+vn -0.229900 0.350630 0.907857
+v 0.005627 -0.374494 0.023032
+vn -0.261196 0.230720 0.937307
+v 0.005136 -0.376169 0.023358
+vn -0.199169 0.232590 0.951963
+v 0.006377 -0.377710 0.024008
+vn -0.105582 0.127922 -0.986148
+v -0.016223 -0.403698 0.009092
+vn -0.297462 -0.866674 -0.400490
+v -0.010125 -0.423620 0.016752
+vn -0.325384 -0.832499 -0.448410
+v -0.011234 -0.422608 0.015605
+vn -0.115815 -0.984297 -0.133215
+v -0.003535 -0.425970 0.020982
+vn -0.154138 -0.980059 -0.125403
+v -0.005955 -0.425695 0.021392
+vn -0.509136 -0.857898 -0.069213
+v -0.010714 -0.425787 -0.024380
+vn -0.669736 -0.654389 0.351040
+v -0.015583 -0.420778 -0.015707
+vn -0.382437 -0.787118 -0.483928
+v -0.013618 -0.419587 0.012113
+vn -0.238473 -0.857376 -0.456110
+v -0.008875 -0.423529 0.015858
+vn -0.173067 -0.889163 -0.423601
+v -0.006422 -0.424213 0.016076
+vn -0.661420 0.051324 -0.748258
+v -0.020421 -0.404804 0.010324
+vn -0.456463 0.034983 -0.889055
+v -0.019380 -0.405003 0.009590
+vn -0.221458 -0.969108 0.108565
+v -0.008199 -0.425397 0.026329
+vn -0.172234 -0.985056 0.000519
+v -0.006848 -0.425756 0.024755
+vn -0.079370 -0.996283 -0.033487
+v -0.002881 -0.426193 0.023148
+vn -0.186508 -0.980884 -0.055505
+v -0.007392 -0.425615 0.023224
+vn -0.127010 -0.990116 -0.059480
+v -0.005004 -0.425964 0.022911
+vn 0.417136 -0.850948 -0.319193
+v 0.017290 -0.426534 -0.021183
+vn 0.365690 -0.878581 -0.307190
+v 0.015622 -0.426797 -0.022449
+vn 0.314326 -0.927090 -0.204215
+v 0.014189 -0.427330 -0.022436
+vn -0.801496 -0.483630 0.351719
+v -0.022514 -0.415466 0.025557
+vn -0.868386 -0.440234 0.228254
+v -0.023332 -0.414658 0.024588
+vn -0.253624 -0.966678 -0.034767
+v -0.009429 -0.425173 0.023620
+vn -0.236262 -0.971285 0.028029
+v -0.008745 -0.425355 0.025034
+vn -0.786325 -0.573391 0.230033
+v -0.021664 -0.417106 0.024844
+vn -0.078591 -0.994223 -0.073104
+v -0.002269 -0.426158 0.021635
+vn -0.082016 -0.958674 -0.272429
+v 0.000113 -0.425576 0.017538
+vn -0.423302 -0.746411 0.513504
+v -0.009258 -0.423909 -0.013236
+vn -0.279360 -0.949860 0.140443
+v -0.010048 -0.424863 0.026730
+vn -0.401541 -0.659604 0.635364
+v -0.008660 -0.423255 -0.012030
+vn -0.371513 -0.608161 0.701512
+v -0.008341 -0.422305 -0.010944
+vn -0.466760 -0.884325 0.010253
+v -0.009852 -0.426297 -0.023188
+vn -0.395755 -0.917253 -0.045009
+v -0.008415 -0.426996 -0.023791
+vn -0.341227 -0.816663 0.465431
+v -0.006977 -0.424825 -0.012923
+vn -0.293183 -0.766437 0.571504
+v -0.005962 -0.424691 -0.012069
+vn -0.336301 -0.670410 0.661401
+v -0.006684 -0.424112 -0.011772
+vn -0.342747 -0.937999 0.051798
+v -0.007426 -0.427354 -0.022602
+vn -0.861472 -0.114413 -0.494748
+v -0.022928 -0.408177 0.013703
+vn -0.869937 -0.210977 -0.445756
+v -0.022959 -0.409759 0.014356
+vn -0.291998 -0.955494 -0.042053
+v -0.006256 -0.427762 -0.023636
+vn -0.130617 -0.991103 0.025588
+v -0.001614 -0.428729 -0.022228
+vn -0.177761 -0.631425 -0.754787
+v -0.000569 -0.418518 0.006058
+vn -0.319776 -0.630655 0.707119
+v -0.006423 -0.423446 -0.011022
+vn -0.562255 -0.824184 -0.067753
+v -0.012064 -0.424846 -0.025623
+vn -0.309503 -0.520522 -0.795779
+v 0.002152 -0.416718 0.003929
+vn -0.205615 -0.683307 -0.700581
+v 0.000615 -0.419302 0.006507
+vn -0.156535 -0.752015 -0.640289
+v -0.001449 -0.419957 0.007729
+vn -0.679263 -0.486869 0.549145
+v 0.006430 -0.419228 -0.002836
+vn -0.201415 -0.979216 -0.023828
+v -0.003882 -0.428381 -0.023385
+vn -0.073074 -0.963702 0.256785
+v 0.002924 -0.428287 -0.015244
+vn -0.039448 -0.954452 0.295744
+v 0.004703 -0.428164 -0.014447
+vn -0.315781 -0.944842 0.086931
+v -0.006792 -0.427455 -0.020972
+vn -0.196086 -0.374737 -0.906158
+v -0.013248 -0.418470 -0.034031
+vn -0.526552 -0.849584 0.030829
+v -0.011473 -0.425368 -0.022603
+vn 0.012955 -0.992003 0.125545
+v 0.005106 -0.428825 -0.018508
+vn 0.096968 -0.990144 0.101055
+v 0.006402 -0.428710 -0.017998
+vn -0.208044 0.048201 0.976931
+v -0.015987 -0.412062 0.031407
+vn 0.172565 -0.984716 0.023585
+v 0.006824 -0.428660 -0.018300
+vn -0.741681 -0.551371 -0.381968
+v -0.021464 -0.416139 0.017073
+vn 0.108941 -0.981779 0.155701
+v 0.007099 -0.428183 -0.014990
+vn -0.613913 -0.668403 -0.419939
+v 0.006145 -0.418105 0.002074
+vn -0.877044 -0.235966 -0.418466
+v -0.023344 -0.410751 0.015637
+vn 0.064750 -0.973217 0.220582
+v 0.006558 -0.428086 -0.014245
+vn 0.025387 -0.983487 0.179190
+v 0.004950 -0.428423 -0.015547
+vn -0.415369 -0.802912 0.427553
+v -0.009591 -0.424409 -0.014398
+vn -0.434628 -0.603937 0.668100
+v -0.011004 -0.421569 -0.011867
+vn -0.389877 -0.912158 -0.126343
+v -0.008684 -0.426734 -0.025035
+vn -0.430537 -0.094213 0.897643
+v -0.018713 -0.413595 0.030629
+vn -0.289757 -0.567933 0.770385
+v -0.006014 -0.422639 -0.010185
+vn -0.647759 -0.526116 0.551008
+v -0.015583 -0.419377 -0.013775
+vn -0.319285 0.054405 0.946096
+v -0.020238 -0.410245 0.030068
+vn -0.764546 -0.471627 -0.439361
+v -0.019113 -0.414502 0.011356
+vn -0.270456 -0.681831 0.679676
+v -0.004882 -0.424352 -0.011220
+vn -0.213377 -0.970338 -0.113644
+v -0.008248 -0.425310 0.021739
+vn -0.149901 -0.985402 0.080699
+v -0.001606 -0.428630 -0.020442
+vn -0.136685 -0.980478 0.141350
+v -0.000807 -0.428509 -0.018282
+vn -0.219762 0.037355 -0.974838
+v -0.018131 -0.404801 0.009155
+vn -0.808854 -0.292473 -0.510113
+v -0.020475 -0.411873 0.011525
+vn -0.273969 -0.958241 -0.081948
+v -0.010252 -0.424862 0.022352
+vn -0.450933 -0.618468 -0.643550
+v 0.004652 -0.418078 0.003620
+vn 0.273584 -0.955396 -0.111219
+v 0.016664 -0.426915 -0.020438
+vn -0.184402 0.074080 -0.980055
+v -0.016969 -0.406071 0.008927
+vn 0.442984 -0.733945 -0.514869
+v 0.014583 -0.426522 -0.024084
+vn -0.262947 -0.602508 0.753554
+v -0.004027 -0.423561 -0.010190
+vn -0.120084 -0.970460 -0.209252
+v -0.003828 -0.425696 0.019619
+vn -0.072363 -0.981450 0.177535
+v -0.003802 -0.425833 0.027350
+vn -0.115821 -0.974831 0.190498
+v -0.005703 -0.425625 0.027469
+vn 0.368443 -0.858651 -0.356326
+v 0.012643 -0.427333 -0.024292
+vn -0.526455 -0.526915 0.667237
+v -0.013411 -0.420195 -0.012418
+vn -0.576694 -0.552259 0.602025
+v -0.014155 -0.420349 -0.013181
+vn -0.078842 -0.987599 -0.135764
+v -0.001341 -0.426117 0.020420
+vn -0.218049 -0.541369 -0.812019
+v -0.007266 -0.417401 0.006668
+vn -0.153749 -0.571280 -0.806226
+v -0.002642 -0.417901 0.005964
+vn -0.221950 -0.469851 -0.854388
+v -0.006256 -0.416858 0.006104
+vn -0.193086 -0.540794 -0.818694
+v -0.005061 -0.417558 0.006223
+vn -0.179355 -0.619951 -0.763867
+v -0.004931 -0.418220 0.006678
+vn -0.150793 -0.693557 -0.704443
+v -0.003291 -0.418832 0.006883
+vn -0.551976 -0.740886 -0.382635
+v -0.017287 -0.419776 0.016386
+vn -0.519143 -0.747238 -0.414880
+v -0.016382 -0.419878 0.015371
+vn -0.013797 -0.989330 0.145037
+v 0.003506 -0.428616 -0.016632
+vn -0.059219 -0.997330 0.042728
+v -0.003005 -0.426186 0.024559
+vn -0.023833 -0.999192 0.032372
+v 0.000023 -0.426323 0.023872
+vn -0.058944 -0.993192 0.100470
+v -0.003234 -0.426069 0.025954
+vn 0.050774 -0.990536 0.127518
+v 0.005041 -0.428522 -0.016208
+vn -0.081207 -0.983102 0.164060
+v 0.001752 -0.428598 -0.017120
+vn -0.100387 -0.988089 0.116627
+v -0.005309 -0.425888 0.026136
+vn -0.174646 -0.966094 0.190162
+v -0.007468 -0.425378 0.027365
+vn -0.154263 -0.983633 0.093112
+v -0.006629 -0.425747 0.025960
+vn -0.259771 -0.652588 0.711792
+v -0.002426 -0.424417 -0.010369
+vn -0.454882 -0.768615 -0.449793
+v -0.015196 -0.420284 0.014757
+vn -0.528115 -0.718457 -0.452673
+v -0.016740 -0.419011 0.014344
+vn -0.109731 -0.993897 0.011275
+v -0.005050 -0.426011 0.024695
+vn -0.267427 -0.536374 0.800491
+v -0.001291 -0.423323 -0.009131
+vn -0.266686 -0.798972 -0.539001
+v -0.010753 -0.420840 0.012313
+vn -0.275940 -0.806888 -0.522292
+v -0.011302 -0.420068 0.011423
+vn -0.489474 -0.745559 0.452281
+v -0.011691 -0.423236 -0.014454
+vn -0.459410 -0.692706 0.555969
+v -0.011398 -0.422523 -0.013150
+vn -0.627156 -0.650973 0.427678
+v -0.014684 -0.420967 -0.014523
+vn -0.488527 -0.611393 0.622527
+v -0.012590 -0.421331 -0.012730
+vn -0.544268 -0.661437 0.516017
+v -0.013430 -0.421399 -0.013585
+vn -0.228130 -0.481640 -0.846156
+v -0.000615 -0.417013 0.005009
+vn -0.615388 -0.537542 0.576495
+v -0.017941 -0.419077 0.029481
+vn -0.814101 -0.571170 0.104904
+v 0.006196 -0.417532 -0.000970
+vn -0.175748 -0.836045 -0.519752
+v -0.003854 -0.422487 0.012157
+vn -0.279418 0.666160 0.691489
+v -0.017395 -0.396213 0.025866
+vn -0.180276 0.782564 0.595898
+v -0.017439 -0.395402 0.024962
+vn -0.619044 -0.380440 0.687059
+v -0.018687 -0.417925 0.029605
+vn -0.683741 -0.303373 0.663674
+v -0.019624 -0.416573 0.029313
+vn -0.733420 -0.487781 0.473462
+v -0.019342 -0.417996 0.028731
+vn -0.232137 -0.749869 -0.619523
+v 0.002476 -0.420592 0.007301
+vn -0.177520 -0.845493 -0.503615
+v -0.006039 -0.422856 0.013505
+vn -0.152169 -0.862561 -0.482527
+v -0.001800 -0.423351 0.012950
+vn -0.245501 -0.261844 -0.933363
+v -0.003897 -0.415376 0.005149
+vn -0.154266 -0.725020 -0.671229
+v -0.002114 -0.419429 0.007275
+vn -0.162653 -0.770427 -0.616430
+v -0.003749 -0.419913 0.008233
+vn -0.306665 -0.394508 -0.866210
+v -0.000686 -0.415350 0.004182
+vn -0.079088 -0.991603 0.102318
+v 0.000896 -0.428783 -0.019060
+vn -0.020192 -0.994661 0.101198
+v 0.003101 -0.428877 -0.019038
+vn -0.304665 -0.687017 -0.659687
+v 0.003392 -0.419325 0.005535
+vn -0.197598 -0.739374 -0.643647
+v 0.000290 -0.420160 0.007516
+vn 0.193046 0.290349 0.937247
+v 0.015193 -0.391548 0.028337
+vn 0.200383 0.238363 0.950279
+v 0.015188 -0.393467 0.028916
+vn 0.155220 0.232746 0.960071
+v 0.011545 -0.393501 0.029549
+vn -0.338118 -0.904036 -0.261522
+v -0.011288 -0.423586 0.017764
+vn 0.184989 0.056974 0.981088
+v 0.014336 -0.399231 0.029811
+vn 0.932144 0.228894 -0.280562
+v 0.030361 -0.393508 0.008862
+vn -0.011982 -0.993817 0.110385
+v 0.003155 -0.428733 -0.017662
+vn 0.011856 -0.999920 0.004395
+v 0.003279 -0.429000 -0.022171
+vn 0.049669 -0.996083 -0.073158
+v 0.003327 -0.428966 -0.023443
+vn 0.815034 -0.572805 0.087263
+v 0.030569 -0.412769 0.000091
+vn 0.767376 -0.615961 -0.178115
+v 0.031687 -0.411783 0.005449
+vn -0.073044 -0.950954 0.300585
+v 0.014355 -0.422248 -0.002109
+vn 0.680577 -0.344654 -0.646551
+v 0.030378 -0.416896 -0.013111
+vn -0.554678 -0.457007 0.695325
+v 0.006080 -0.420502 -0.004082
+vn 0.721907 -0.548255 0.422216
+v 0.029487 -0.415951 -0.002944
+vn -0.326861 -0.538823 0.776422
+v 0.002155 -0.423633 -0.008047
+vn -0.391899 -0.414682 0.821251
+v 0.002645 -0.422353 -0.007084
+vn -0.534769 -0.614506 0.580004
+v 0.007566 -0.420955 -0.003147
+vn -0.073928 -0.026204 -0.996919
+v -0.016677 -0.410438 -0.035116
+vn -0.547446 -0.370802 0.750206
+v 0.004918 -0.420177 -0.004776
+vn -0.534510 -0.287996 0.794580
+v 0.003790 -0.420199 -0.005575
+vn 0.781584 -0.619972 0.069002
+v 0.029911 -0.413566 0.000816
+vn 0.201729 -0.975037 0.092777
+v 0.011245 -0.427752 -0.017818
+vn -0.518938 -0.764250 0.382918
+v 0.008830 -0.420600 -0.001222
+vn 0.803505 -0.595289 -0.003295
+v 0.030727 -0.412442 0.001522
+vn -0.380300 -0.489484 0.784715
+v 0.003823 -0.422823 -0.006767
+vn -0.461797 -0.374536 0.804031
+v 0.003934 -0.421390 -0.005958
+vn -0.528901 -0.754063 -0.389425
+v -0.012464 -0.423236 -0.030553
+vn -0.320243 -0.475539 0.819334
+v 0.001047 -0.423014 -0.008117
+vn -0.384240 -0.349493 0.854526
+v 0.001688 -0.422131 -0.007436
+vn 0.649580 -0.728851 0.216382
+v 0.027617 -0.416144 0.000411
+vn 0.572905 -0.810185 0.124020
+v 0.025823 -0.417307 0.001531
+vn 0.158013 -0.976929 0.143674
+v 0.013774 -0.427364 -0.018464
+vn -0.741721 -0.630056 -0.229954
+v 0.006428 -0.417851 0.000886
+vn 0.187232 -0.970734 0.150397
+v 0.018002 -0.426370 -0.016510
+vn 0.690137 -0.578566 0.434709
+v 0.029083 -0.417539 -0.004282
+vn 0.632640 -0.691734 0.348239
+v 0.027934 -0.420046 -0.006242
+vn 0.166109 -0.975592 0.143625
+v 0.014033 -0.427153 -0.017425
+vn 0.737364 -0.519197 0.432121
+v 0.030336 -0.416123 -0.004571
+vn 0.832317 -0.522986 0.183668
+v 0.030853 -0.412589 -0.000900
+vn 0.782083 -0.531204 0.325836
+v 0.030061 -0.414200 -0.001685
+vn 0.585083 -0.698640 0.411801
+v 0.027142 -0.417396 -0.001430
+vn 0.700611 -0.616582 0.359125
+v 0.028699 -0.415846 -0.001414
+vn 0.742236 -0.635743 -0.211937
+v 0.031121 -0.412709 0.006325
+vn 0.739683 -0.659193 -0.135402
+v 0.030071 -0.413567 0.005060
+vn 0.673710 -0.701242 -0.233185
+v 0.028337 -0.415828 0.006722
+vn 0.708289 -0.679841 -0.190113
+v 0.029320 -0.414609 0.005942
+vn 0.747175 -0.632569 0.203928
+v 0.029227 -0.414629 -0.000161
+vn -0.223823 -0.923344 -0.311992
+v -0.008655 -0.424175 0.017091
+vn 0.725180 -0.685809 0.061480
+v 0.028908 -0.414634 0.001726
+vn -0.518973 -0.202891 0.830363
+v 0.002518 -0.420460 -0.006483
+vn -0.746370 -0.659034 -0.092767
+v 0.006305 -0.417608 0.000426
+vn -0.460942 -0.295791 0.836684
+v 0.002575 -0.421309 -0.006695
+vn -0.320409 0.096318 -0.942370
+v -0.018586 -0.407100 -0.034703
+vn 0.789465 -0.379162 -0.482681
+v 0.022640 -0.423602 -0.017128
+vn 0.136156 -0.512863 -0.847605
+v -0.000807 -0.425799 -0.031114
+vn -0.759710 0.043160 -0.648828
+v -0.020099 -0.406821 -0.033639
+vn -0.372112 -0.068030 -0.925691
+v -0.018127 -0.409035 -0.034913
+vn 0.283986 -0.530658 -0.798595
+v 0.004431 -0.426784 -0.029292
+vn -0.703654 -0.613439 0.358557
+v 0.007290 -0.419338 -0.001544
+vn 0.039791 0.008071 -0.999175
+v -0.015720 -0.411221 -0.035116
+vn -0.097468 0.077231 -0.992238
+v -0.017499 -0.408044 -0.035004
+vn -0.034731 -0.505798 -0.861953
+v -0.006477 -0.424320 -0.032420
+vn -0.403602 -0.808184 -0.428887
+v 0.007600 -0.420040 0.003743
+vn 0.772389 -0.499645 0.392136
+v 0.030647 -0.416014 -0.004990
+vn 0.781579 -0.462101 0.419044
+v 0.030910 -0.414929 -0.004230
+vn 0.535079 -0.727111 0.430116
+v 0.027447 -0.420180 -0.005748
+vn 0.699522 -0.587049 0.407482
+v 0.029781 -0.417518 -0.005448
+vn 0.017302 0.039050 -0.999087
+v -0.016525 -0.409503 -0.035109
+vn -0.443522 -0.880977 -0.164825
+v 0.008962 -0.420179 0.002220
+vn -0.562773 -0.778592 -0.277636
+v 0.007392 -0.419188 0.002130
+vn -0.346715 -0.809178 -0.474363
+v -0.012189 -0.421088 0.013586
+vn -0.703799 -0.697857 -0.132900
+v 0.006994 -0.418486 0.000832
+vn -0.269984 -0.816249 -0.510731
+v -0.010296 -0.421563 0.013161
+vn -0.168005 0.091543 -0.981527
+v -0.017745 -0.406764 -0.034851
+vn -0.496075 0.121078 -0.859797
+v -0.019150 -0.405889 -0.034283
+vn 0.192457 -0.974341 0.116704
+v 0.013238 -0.427244 -0.016990
+vn 0.172311 -0.983133 0.061298
+v 0.009783 -0.428223 -0.019991
+vn 0.176356 -0.977301 0.117389
+v 0.012270 -0.427627 -0.018537
+vn 0.170841 -0.824977 -0.538727
+v 0.004176 -0.427590 -0.028551
+vn 0.245446 -0.753227 -0.610250
+v 0.005669 -0.427318 -0.028341
+vn 0.178695 -0.981728 0.065401
+v 0.008754 -0.428363 -0.019060
+vn 0.758535 -0.639843 -0.123393
+v 0.030753 -0.412684 0.004595
+vn -0.134983 -0.377603 -0.916076
+v -0.009985 -0.422125 -0.033086
+vn -0.369594 -0.762514 0.531010
+v 0.009187 -0.421805 -0.002864
+vn -0.403759 -0.575794 0.710943
+v 0.006522 -0.422246 -0.004983
+vn -0.770210 -0.633613 -0.072876
+v 0.005880 -0.417042 -0.000103
+vn 0.656051 -0.754233 0.027002
+v 0.027667 -0.415772 0.002647
+vn -0.008892 -0.889231 0.457371
+v 0.013042 -0.422878 -0.003611
+vn 0.627838 -0.636567 0.447887
+v 0.028327 -0.418730 -0.004735
+vn 0.537317 -0.689282 0.485985
+v 0.027385 -0.419258 -0.004309
+vn 0.723327 -0.541489 0.428471
+v 0.029803 -0.416619 -0.004287
+vn 0.629372 -0.623495 0.463837
+v 0.028267 -0.417449 -0.003005
+vn -0.340757 0.023253 -0.939864
+v -0.018833 -0.407173 0.009330
+vn 0.148953 -0.988507 0.025822
+v 0.009572 -0.428342 -0.021734
+vn 0.164532 -0.982677 0.085295
+v 0.012035 -0.427840 -0.019946
+vn 0.087471 -0.995999 -0.018285
+v 0.005531 -0.428877 -0.021925
+vn 0.149259 -0.988622 0.018661
+v 0.007548 -0.428638 -0.020625
+vn 0.520791 -0.783354 0.339314
+v 0.025504 -0.418101 -0.000464
+vn 0.802563 -0.566806 0.186075
+v 0.030124 -0.413550 -0.000515
+vn 0.784324 -0.614154 -0.087465
+v 0.031114 -0.412067 0.003441
+vn 0.761736 -0.647128 -0.031343
+v 0.030035 -0.413343 0.002934
+vn 0.714897 -0.696177 -0.065268
+v 0.029001 -0.414543 0.003828
+vn 0.667944 -0.734106 -0.122227
+v 0.027992 -0.415633 0.004686
+vn -0.441679 -0.718787 -0.536903
+v 0.005957 -0.419072 0.003696
+vn -0.614766 -0.252751 -0.747115
+v -0.018775 -0.411231 -0.034100
+vn -0.463983 -0.179881 -0.867388
+v -0.018195 -0.410325 -0.034715
+vn -0.565395 -0.289648 -0.772290
+v -0.018038 -0.412427 -0.034246
+vn 0.078926 -0.499319 0.862816
+v 0.018136 -0.422748 0.025668
+vn -0.024319 0.077534 -0.996693
+v -0.016337 -0.403618 -0.034871
+vn -0.601183 -0.794438 0.086304
+v 0.008142 -0.419560 0.000394
+vn -0.268095 -0.108001 -0.957320
+v -0.017468 -0.410129 -0.035016
+vn 0.017144 -0.209398 -0.977680
+v -0.012294 -0.417181 -0.034455
+vn 0.100031 0.048029 -0.993824
+v -0.015379 -0.410197 -0.035058
+vn -0.102733 -0.461998 -0.880911
+v -0.008194 -0.423766 -0.032606
+vn -0.451555 -0.246359 -0.857558
+v -0.017757 -0.413034 0.009150
+vn -0.346895 -0.028533 -0.937470
+v -0.017066 -0.411270 0.008611
+vn -0.302169 0.063433 -0.951141
+v -0.018127 -0.405157 -0.034637
+vn 0.113436 0.050479 -0.992262
+v -0.014990 -0.408425 -0.034913
+vn -0.035322 -0.214658 -0.976050
+v -0.013388 -0.416475 -0.034612
+vn 0.030990 0.062642 -0.997555
+v -0.016434 -0.408258 -0.035037
+vn -0.246720 -0.436510 -0.865210
+v -0.012172 -0.420867 -0.033180
+vn -0.350394 0.004103 -0.936594
+v -0.018316 -0.408132 -0.034849
+vn -0.757997 0.119395 -0.641238
+v -0.020147 -0.405228 -0.033364
+vn -0.555053 -0.066122 -0.829183
+v -0.019191 -0.407861 -0.034423
+vn -0.271669 -0.289710 -0.917749
+v -0.015018 -0.415781 -0.034565
+vn -0.329595 -0.376773 -0.865685
+v -0.014756 -0.417276 -0.034095
+vn -0.082241 -0.322527 -0.942981
+v -0.010939 -0.420262 -0.033663
+vn -0.130089 0.011820 -0.991432
+v -0.017461 -0.409009 -0.035060
+vn 0.372650 -0.780090 -0.502584
+v 0.011409 -0.427263 -0.025547
+vn 0.413662 -0.738742 -0.532113
+v 0.012682 -0.426912 -0.025075
+vn -0.576067 -0.137306 -0.805787
+v -0.018879 -0.409235 -0.034472
+vn 0.737531 -0.237520 -0.632164
+v 0.019397 -0.424275 -0.021441
+vn -0.071699 0.082508 -0.994008
+v -0.010414 -0.404854 -0.034524
+vn 0.642551 -0.565491 -0.517058
+v 0.026420 -0.421560 -0.013342
+vn 0.839711 -0.164174 -0.517621
+v 0.033460 -0.412382 -0.011264
+vn 0.325103 -0.262610 -0.908484
+v 0.007033 -0.424280 -0.029188
+vn 0.322801 -0.293088 -0.899944
+v 0.007015 -0.422477 -0.029741
+vn 0.010879 0.035996 -0.999293
+v -0.011752 -0.409961 -0.034673
+vn 0.036945 0.034811 -0.998711
+v -0.012568 -0.408338 -0.034624
+vn 0.104495 0.053490 -0.993086
+v -0.013420 -0.409354 -0.034743
+vn 0.655191 -0.330187 -0.679486
+v 0.029297 -0.415364 -0.014902
+vn 0.676539 -0.247170 -0.693687
+v 0.029235 -0.414007 -0.015554
+vn 0.695966 -0.180556 -0.695004
+v 0.027718 -0.412911 -0.017377
+vn 0.649476 -0.341486 -0.679388
+v 0.021675 -0.417033 -0.021537
+vn 0.661897 -0.413171 -0.625445
+v 0.023189 -0.417569 -0.019654
+vn 0.312872 -0.246630 -0.917216
+v 0.005971 -0.425265 -0.029296
+vn 0.332576 -0.328560 -0.883992
+v 0.006398 -0.426164 -0.028882
+vn 0.670024 -0.446650 -0.592935
+v 0.024201 -0.419971 -0.016770
+vn 0.735424 -0.072267 -0.673743
+v 0.031782 -0.411441 -0.013514
+vn 0.774920 -0.117104 -0.621117
+v 0.032882 -0.412095 -0.012153
+vn 0.165339 0.054758 -0.984715
+v -0.014714 -0.404242 -0.034711
+vn 0.562237 -0.437806 -0.701581
+v 0.028393 -0.418925 -0.013794
+vn 0.013963 0.072599 -0.997263
+v -0.012222 -0.405122 -0.034497
+vn 0.282969 -0.230850 -0.930933
+v 0.004099 -0.425406 -0.029869
+vn -0.460315 -0.793810 -0.397462
+v -0.015071 -0.420967 0.015880
+vn 0.599043 -0.425246 -0.678464
+v 0.027554 -0.417910 -0.015073
+vn 0.384037 -0.361710 -0.849518
+v 0.008715 -0.425997 -0.027989
+vn 0.425060 -0.252210 -0.869318
+v 0.011089 -0.422879 -0.027897
+vn 0.357561 -0.273139 -0.893054
+v 0.007821 -0.425390 -0.028571
+vn 0.371307 -0.268655 -0.888794
+v 0.009034 -0.423718 -0.028574
+vn 0.629374 -0.360575 -0.688385
+v 0.029176 -0.416985 -0.014188
+vn 0.550831 -0.524371 -0.649323
+v 0.027164 -0.420315 -0.013734
+vn -0.457852 -0.087993 0.884663
+v -0.019764 -0.412269 0.030235
+vn 0.531580 -0.490996 -0.690178
+v 0.028051 -0.419506 -0.013667
+vn 0.643231 -0.624750 -0.442653
+v 0.028720 -0.419810 -0.012845
+vn 0.623094 -0.443583 -0.644196
+v 0.025850 -0.421095 -0.014283
+vn 0.749097 0.048823 -0.660659
+v 0.031875 -0.409650 -0.013413
+vn 0.285631 -0.246410 -0.926119
+v 0.005322 -0.424406 -0.029728
+vn 0.582644 -0.395992 -0.709730
+v 0.028388 -0.418338 -0.014142
+vn 0.619370 -0.373468 -0.690581
+v 0.029268 -0.418105 -0.013537
+vn 0.439641 -0.356406 -0.824433
+v 0.011724 -0.425420 -0.026768
+vn 0.401431 -0.282939 -0.871091
+v 0.010308 -0.424757 -0.027700
+vn 0.542862 -0.459722 -0.702821
+v 0.027803 -0.419017 -0.014193
+vn 0.552491 -0.497059 -0.669094
+v 0.026854 -0.419392 -0.014657
+vn 0.503063 -0.213939 -0.837351
+v 0.013126 -0.422292 -0.026966
+vn 0.581176 -0.193428 -0.790455
+v 0.015879 -0.424014 -0.024747
+vn 0.544327 -0.544059 -0.638520
+v 0.028244 -0.419792 -0.013309
+vn 0.637201 -0.490679 -0.594314
+v 0.029033 -0.419248 -0.013046
+vn 0.071397 0.027337 -0.997073
+v -0.012235 -0.412562 -0.034823
+vn -0.405544 -0.825877 -0.391740
+v -0.013505 -0.422080 0.016474
+vn -0.383833 -0.804351 -0.453533
+v -0.013491 -0.421276 0.014929
+vn 0.784177 0.001636 -0.620535
+v 0.032887 -0.410562 -0.012273
+vn 0.720306 -0.142733 -0.678813
+v 0.019208 -0.421949 -0.022065
+vn 0.706410 -0.137635 -0.694292
+v 0.030303 -0.412691 -0.014865
+vn 0.693751 -0.248164 -0.676110
+v 0.030450 -0.414166 -0.014293
+vn 0.507794 -0.240082 -0.827349
+v 0.013833 -0.424010 -0.026092
+vn 0.114588 0.030901 -0.992932
+v -0.015020 -0.406070 -0.034826
+vn -0.017980 0.037162 -0.999147
+v -0.016531 -0.406296 -0.034933
+vn 0.222368 0.191031 -0.956065
+v -0.014027 -0.402259 -0.034326
+vn 0.202702 0.122152 -0.971592
+v -0.014201 -0.403031 -0.034492
+vn 0.760744 -0.418202 -0.496363
+v 0.030944 -0.417109 -0.012279
+vn 0.736016 -0.303232 -0.605253
+v 0.031621 -0.415298 -0.012542
+vn 0.798890 -0.149037 -0.582720
+v 0.022193 -0.422702 -0.018213
+vn 0.200235 0.117352 -0.972694
+v -0.014828 -0.403100 -0.034648
+vn 0.647839 -0.436104 -0.624594
+v 0.024615 -0.417240 -0.018369
+vn 0.652654 -0.390000 -0.649571
+v 0.025895 -0.416251 -0.017735
+vn 0.626909 -0.443190 -0.640756
+v 0.026023 -0.417580 -0.016696
+vn 0.660835 -0.436508 -0.610539
+v 0.024058 -0.418557 -0.018000
+vn 0.657599 -0.369178 -0.656713
+v 0.023879 -0.416335 -0.019754
+vn 0.734461 -0.115423 -0.668763
+v 0.019333 -0.423105 -0.021715
+vn 0.664872 -0.136295 -0.734417
+v 0.017463 -0.422806 -0.023666
+vn 0.598398 -0.631767 -0.492738
+v 0.027855 -0.420517 -0.012941
+vn 0.650222 -0.683767 -0.331170
+v 0.027857 -0.421034 -0.012116
+vn 0.713470 -0.331048 -0.617550
+v 0.024901 -0.421605 -0.014953
+vn 0.724203 -0.423893 -0.543915
+v 0.024932 -0.422270 -0.014531
+vn 0.764583 -0.174539 -0.620443
+v 0.020467 -0.421646 -0.020669
+vn 0.786152 -0.153597 -0.598642
+v 0.021207 -0.421807 -0.019675
+vn 0.715661 -0.257536 -0.649233
+v 0.020275 -0.420455 -0.021271
+vn 0.846537 -0.004371 -0.532312
+v 0.033542 -0.411010 -0.011317
+vn -0.312744 -0.002537 -0.949834
+v -0.013087 -0.413807 0.007178
+vn -0.420430 -0.162241 -0.892702
+v -0.014970 -0.413968 0.007949
+vn 0.783821 -0.299279 -0.544111
+v 0.023885 -0.422508 -0.015829
+vn 0.388398 -0.485230 -0.783389
+v 0.009547 -0.426484 -0.027347
+vn 0.307852 -0.250068 -0.917983
+v 0.005230 -0.425826 -0.029391
+vn 0.294280 -0.254445 -0.921226
+v 0.003690 -0.426057 -0.029837
+vn -0.492781 -0.430435 -0.756236
+v -0.015609 -0.415266 0.008788
+vn 0.780938 -0.106149 -0.615523
+v 0.021234 -0.422588 -0.019507
+vn -0.433027 -0.415625 -0.799840
+v -0.014289 -0.415548 0.008122
+vn 0.708835 -0.170596 -0.684434
+v 0.018614 -0.424136 -0.022316
+vn 0.596995 -0.323366 -0.734187
+v 0.016673 -0.424788 -0.023908
+vn 0.450633 -0.260993 -0.853705
+v 0.012172 -0.423989 -0.027035
+vn 0.669331 -0.145963 -0.728485
+v 0.017590 -0.423918 -0.023375
+# 4900 vertices, 0 vertices normals
+
+f 2//2 80//80 1//1
+f 15//15 1//1 6//6
+f 18//18 17//17 5//5
+f 18//18 5//5 2073//2073
+f 2//2 1//1 15//15
+f 4//4 19//19 6//6
+f 15//15 6//6 19//19
+f 6//6 1//1 3//3
+f 1//1 2099//2099 3//3
+f 7//7 6//6 2101//2101
+f 6//6 3//3 2101//2101
+f 80//80 2099//2099 1//1
+f 17//17 4//4 7//7
+f 5//5 17//17 7//7
+f 4//4 6//6 7//7
+f 25//25 8//8 14//14
+f 21//21 25//25 14//14
+f 24//24 10//10 8//8
+f 14//14 8//8 9//9
+f 9//9 10//10 66//66
+f 10//10 9//9 8//8
+f 10//10 24//24 69//69
+f 11//11 12//12 20//20
+f 29//29 11//11 20//20
+f 12//12 13//13 21//21
+f 20//20 12//12 21//21
+f 25//25 21//21 13//13
+f 19//19 4//4 14//14
+f 15//15 9//9 66//66
+f 15//15 19//19 9//9
+f 2//2 15//15 66//66
+f 16//16 18//18 2066//2066
+f 18//18 2073//2073 2066//2066
+f 17//17 18//18 20//20
+f 19//19 14//14 9//9
+f 29//29 20//20 18//18
+f 16//16 29//29 18//18
+f 21//21 17//17 20//20
+f 4//4 17//17 21//21
+f 4//4 21//21 14//14
+f 8//8 25//25 28//28
+f 22//22 2182//2182 24//24
+f 24//24 8//8 22//22
+f 11//11 43//43 12//12
+f 2073//2073 5//5 2075//2075
+f 2182//2182 23//23 24//24
+f 8//8 28//28 22//22
+f 43//43 11//11 42//42
+f 13//13 12//12 30//30
+f 13//13 30//30 26//26
+f 25//25 26//26 28//28
+f 25//25 13//13 26//26
+f 32//32 27//27 28//28
+f 28//28 26//26 32//32
+f 29//29 228//228 42//42
+f 7//7 2101//2101 2077//2077
+f 11//11 29//29 42//42
+f 5//5 7//7 2077//2077
+f 12//12 43//43 30//30
+f 29//29 16//16 228//228
+f 2075//2075 5//5 2077//2077
+f 28//28 27//27 22//22
+f 1075//1075 4322//4322 31//31
+f 27//27 32//32 2184//2184
+f 37//37 41//41 2178//2178
+f 279//279 33//33 36//36
+f 220//220 39//39 34//34
+f 35//35 279//279 36//36
+f 39//39 44//44 30//30
+f 2184//2184 32//32 37//37
+f 44//44 38//38 37//37
+f 279//279 35//35 4432//4432
+f 4811//4811 37//37 38//38
+f 4811//4811 38//38 4432//4432
+f 38//38 44//44 39//39
+f 4432//4432 38//38 40//40
+f 4432//4432 40//40 218//218
+f 39//39 220//220 38//38
+f 38//38 220//220 40//40
+f 2184//2184 2183//2183 27//27
+f 37//37 2178//2178 2184//2184
+f 2180//2180 2178//2178 41//41
+f 4651//4651 4425//4425 4751//4751
+f 279//279 4432//4432 218//218
+f 218//218 259//259 278//278
+f 43//43 42//42 34//34
+f 26//26 30//30 44//44
+f 30//30 43//43 39//39
+f 43//43 34//34 39//39
+f 32//32 26//26 44//44
+f 32//32 44//44 37//37
+f 49//49 51//51 46//46
+f 3403//3403 55//55 45//45
+f 55//55 48//48 51//51
+f 3394//3394 3287//3287 48//48
+f 3287//3287 46//46 48//48
+f 47//47 54//54 74//74
+f 48//48 46//46 51//51
+f 46//46 3286//3286 49//49
+f 50//50 51//51 49//49
+f 49//49 52//52 50//50
+f 54//54 3423//3423 74//74
+f 54//54 77//77 51//51
+f 57//57 3307//3307 75//75
+f 53//53 78//78 3240//3240
+f 78//78 97//97 3240//3240
+f 54//54 51//51 50//50
+f 45//45 55//55 56//56
+f 55//55 51//51 77//77
+f 78//78 53//53 3233//3233
+f 4440//4440 4328//4328 4422//4422
+f 75//75 74//74 57//57
+f 57//57 74//74 3423//3423
+f 86//86 95//95 87//87
+f 82//82 60//60 58//58
+f 82//82 58//58 59//59
+f 60//60 88//88 93//93
+f 58//58 60//60 93//93
+f 114//114 61//61 93//93
+f 88//88 114//114 93//93
+f 59//59 58//58 92//92
+f 90//90 62//62 71//71
+f 62//62 96//96 64//64
+f 4441//4441 97//97 79//79
+f 65//65 59//59 95//95
+f 60//60 63//63 88//88
+f 114//114 88//88 116//116
+f 59//59 92//92 71//71
+f 87//87 64//64 47//47
+f 87//87 95//95 62//62
+f 64//64 87//87 62//62
+f 65//65 82//82 59//59
+f 66//66 10//10 90//90
+f 24//24 23//23 69//69
+f 3304//3304 76//76 75//75
+f 2//2 81//81 67//67
+f 2//2 66//66 81//81
+f 66//66 90//90 81//81
+f 91//91 94//94 72//72
+f 67//67 91//91 72//72
+f 72//72 94//94 68//68
+f 69//69 70//70 96//96
+f 95//95 59//59 71//71
+f 10//10 69//69 90//90
+f 80//80 67//67 72//72
+f 2099//2099 80//80 73//73
+f 79//79 97//97 78//78
+f 78//78 3233//3233 64//64
+f 3233//3233 77//77 47//47
+f 3304//3304 84//84 76//76
+f 64//64 3233//3233 47//47
+f 74//74 75//75 76//76
+f 54//54 47//47 77//77
+f 47//47 74//74 76//76
+f 80//80 2//2 67//67
+f 69//69 23//23 70//70
+f 78//78 64//64 79//79
+f 72//72 68//68 73//73
+f 72//72 73//73 80//80
+f 81//81 92//92 67//67
+f 82//82 65//65 83//83
+f 82//82 83//83 89//89
+f 65//65 86//86 1996//1996
+f 83//83 65//65 1996//1996
+f 84//84 85//85 76//76
+f 86//86 87//87 85//85
+f 76//76 85//85 87//87
+f 84//84 2017//2017 85//85
+f 60//60 89//89 63//63
+f 65//65 95//95 86//86
+f 47//47 76//76 87//87
+f 88//88 117//117 116//116
+f 88//88 63//63 117//117
+f 117//117 63//63 1978//1978
+f 60//60 82//82 89//89
+f 58//58 93//93 91//91
+f 79//79 70//70 4441//4441
+f 62//62 90//90 96//96
+f 90//90 69//69 96//96
+f 71//71 92//92 81//81
+f 90//90 71//71 81//81
+f 67//67 92//92 91//91
+f 79//79 96//96 70//70
+f 92//92 58//58 91//91
+f 93//93 61//61 94//94
+f 91//91 93//93 94//94
+f 61//61 113//113 94//94
+f 71//71 62//62 95//95
+f 96//96 79//79 64//64
+f 97//97 4441//4441 4449//4449
+f 68//68 113//113 99//99
+f 104//104 109//109 108//108
+f 106//106 98//98 101//101
+f 108//108 98//98 106//106
+f 2089//2089 105//105 101//101
+f 106//106 101//101 99//99
+f 101//101 105//105 99//99
+f 1992//1992 102//102 1990//1990
+f 102//102 100//100 1990//1990
+f 102//102 104//104 100//100
+f 98//98 126//126 2090//2090
+f 98//98 2090//2090 101//101
+f 2089//2089 101//101 2090//2090
+f 1992//1992 119//119 103//103
+f 102//102 1992//1992 103//103
+f 102//102 103//103 109//109
+f 104//104 102//102 109//109
+f 109//109 98//98 108//108
+f 116//116 100//100 115//115
+f 100//100 116//116 118//118
+f 73//73 99//99 105//105
+f 68//68 99//99 73//73
+f 73//73 105//105 2099//2099
+f 100//100 104//104 115//115
+f 104//104 108//108 115//115
+f 108//108 106//106 107//107
+f 115//115 108//108 107//107
+f 106//106 99//99 113//113
+f 107//107 106//106 113//113
+f 1990//1990 100//100 118//118
+f 98//98 109//109 126//126
+f 148//148 124//124 135//135
+f 135//135 125//125 134//134
+f 135//135 124//124 125//125
+f 134//134 125//125 110//110
+f 134//134 110//110 111//111
+f 110//110 133//133 111//111
+f 110//110 112//112 133//133
+f 133//133 122//122 1976//1976
+f 133//133 112//112 122//122
+f 94//94 113//113 68//68
+f 61//61 114//114 107//107
+f 61//61 107//107 113//113
+f 114//114 115//115 107//107
+f 114//114 116//116 115//115
+f 116//116 117//117 118//118
+f 117//117 1978//1978 118//118
+f 2090//2090 124//124 148//148
+f 2090//2090 126//126 124//124
+f 119//119 121//121 120//120
+f 103//103 119//119 120//120
+f 103//103 120//120 123//123
+f 109//109 103//103 123//123
+f 126//126 109//109 123//123
+f 122//122 1979//1979 1976//1976
+f 122//122 112//112 120//120
+f 122//122 121//121 1979//1979
+f 121//121 122//122 120//120
+f 112//112 110//110 123//123
+f 120//120 112//112 123//123
+f 110//110 125//125 123//123
+f 124//124 126//126 125//125
+f 125//125 126//126 123//123
+f 127//127 128//128 129//129
+f 129//129 128//128 2081//2081
+f 127//127 130//130 128//128
+f 128//128 2084//2084 2081//2081
+f 128//128 130//130 138//138
+f 2084//2084 128//128 138//138
+f 151//151 139//139 150//150
+f 150//150 139//139 2086//2086
+f 139//139 152//152 129//129
+f 139//139 129//129 131//131
+f 127//127 129//129 152//152
+f 129//129 2081//2081 131//131
+f 132//132 141//141 111//111
+f 111//111 133//133 132//132
+f 134//134 111//111 136//136
+f 135//135 136//136 149//149
+f 135//135 134//134 136//136
+f 148//148 135//135 149//149
+f 137//137 138//138 130//130
+f 168//168 138//138 137//137
+f 2084//2084 138//138 169//169
+f 2084//2084 169//169 2085//2085
+f 133//133 1976//1976 132//132
+f 2086//2086 139//139 131//131
+f 1937//1937 153//153 140//140
+f 140//140 153//153 146//146
+f 111//111 141//141 136//136
+f 141//141 143//143 142//142
+f 136//136 141//141 142//142
+f 141//141 132//132 1964//1964
+f 143//143 1964//1964 144//144
+f 143//143 141//141 1964//1964
+f 143//143 144//144 147//147
+f 145//145 146//146 147//147
+f 151//151 143//143 147//147
+f 130//130 153//153 137//137
+f 149//149 136//136 142//142
+f 148//148 149//149 2086//2086
+f 151//151 150//150 142//142
+f 150//150 149//149 142//142
+f 149//149 150//150 2086//2086
+f 139//139 151//151 152//152
+f 143//143 151//151 142//142
+f 152//152 147//147 146//146
+f 152//152 151//151 147//147
+f 127//127 152//152 146//146
+f 127//127 146//146 153//153
+f 130//130 127//127 153//153
+f 156//156 1935//1935 165//165
+f 1941//1941 1940//1940 1943//1943
+f 1940//1940 154//154 1943//1943
+f 154//154 162//162 155//155
+f 154//154 1940//1940 197//197
+f 153//153 1937//1937 137//137
+f 1935//1935 155//155 162//162
+f 168//168 156//156 165//165
+f 1935//1935 162//162 165//165
+f 154//154 197//197 160//160
+f 137//137 1937//1937 1938//1938
+f 137//137 156//156 168//168
+f 137//137 1938//1938 156//156
+f 155//155 1935//1935 157//157
+f 154//154 155//155 1943//1943
+f 2059//2059 161//161 158//158
+f 158//158 161//161 188//188
+f 161//161 2058//2058 159//159
+f 193//193 159//159 160//160
+f 161//161 2059//2059 2058//2058
+f 188//188 161//161 193//193
+f 159//159 193//193 161//161
+f 2058//2058 2060//2060 159//159
+f 162//162 159//159 163//163
+f 162//162 160//160 159//159
+f 2060//2060 164//164 163//163
+f 165//165 163//163 166//166
+f 159//159 2060//2060 163//163
+f 165//165 162//162 163//163
+f 166//166 164//164 2085//2085
+f 169//169 168//168 166//166
+f 169//169 166//166 2085//2085
+f 168//168 165//165 166//166
+f 2059//2059 192//192 167//167
+f 163//163 164//164 166//166
+f 168//168 169//169 138//138
+f 160//160 197//197 170//170
+f 162//162 154//154 160//160
+f 2059//2059 158//158 192//192
+f 193//193 160//160 170//170
+f 176//176 177//177 930//930
+f 171//171 173//173 187//187
+f 171//171 177//177 173//173
+f 182//182 173//173 172//172
+f 187//187 173//173 182//182
+f 185//185 172//172 186//186
+f 173//173 174//174 172//172
+f 186//186 191//191 221//221
+f 186//186 172//172 191//191
+f 186//186 221//221 175//175
+f 221//221 236//236 175//175
+f 175//175 211//211 178//178
+f 924//924 176//176 932//932
+f 177//177 171//171 904//904
+f 178//178 184//184 179//179
+f 184//184 178//178 207//207
+f 179//179 180//180 181//181
+f 178//178 179//179 181//181
+f 185//185 180//180 183//183
+f 180//180 185//185 181//181
+f 182//182 183//183 194//194
+f 187//187 201//201 171//171
+f 187//187 182//182 194//194
+f 187//187 194//194 196//196
+f 182//182 185//185 183//183
+f 195//195 184//184 207//207
+f 182//182 172//172 185//185
+f 186//186 175//175 181//181
+f 185//185 186//186 181//181
+f 175//175 178//178 181//181
+f 178//178 211//211 207//207
+f 177//177 904//904 930//930
+f 187//187 196//196 201//201
+f 904//904 171//171 201//201
+f 158//158 188//188 179//179
+f 184//184 158//158 179//179
+f 189//189 190//190 1940//1940
+f 1940//1940 190//190 196//196
+f 221//221 955//955 965//965
+f 191//191 955//955 221//221
+f 192//192 184//184 195//195
+f 180//180 193//193 183//183
+f 193//193 170//170 183//183
+f 179//179 188//188 180//180
+f 188//188 193//193 180//180
+f 170//170 197//197 194//194
+f 183//183 170//170 194//194
+f 167//167 192//192 195//195
+f 197//197 1940//1940 196//196
+f 194//194 197//197 196//196
+f 192//192 158//158 184//184
+f 946//946 198//198 200//200
+f 199//199 176//176 200//200
+f 924//924 200//200 176//176
+f 199//199 174//174 173//173
+f 177//177 176//176 199//199
+f 172//172 174//174 191//191
+f 177//177 199//199 173//173
+f 191//191 964//964 958//958
+f 191//191 174//174 964//964
+f 964//964 174//174 198//198
+f 174//174 199//199 198//198
+f 200//200 198//198 199//199
+f 201//201 196//196 190//190
+f 213//213 2055//2055 2054//2054
+f 202//202 203//203 205//205
+f 215//215 213//213 204//204
+f 209//209 225//225 222//222
+f 227//227 205//205 212//212
+f 226//226 2052//2052 222//222
+f 2055//2055 222//222 2052//2052
+f 202//202 205//205 227//227
+f 223//223 230//230 206//206
+f 213//213 209//209 222//222
+f 207//207 211//211 216//216
+f 167//167 195//195 204//204
+f 250//250 223//223 224//224
+f 167//167 204//204 2054//2054
+f 210//210 250//250 224//224
+f 216//216 208//208 214//214
+f 2076//2076 217//217 220//220
+f 34//34 2076//2076 220//220
+f 225//225 203//203 219//219
+f 209//209 205//205 225//225
+f 236//236 211//211 175//175
+f 211//211 236//236 210//210
+f 205//205 203//203 225//225
+f 215//215 204//204 214//214
+f 216//216 211//211 210//210
+f 250//250 210//210 236//236
+f 209//209 215//215 212//212
+f 205//205 209//209 212//212
+f 213//213 2054//2054 204//204
+f 34//34 42//42 2076//2076
+f 208//208 215//215 214//214
+f 212//212 215//215 208//208
+f 213//213 215//215 209//209
+f 216//216 210//210 224//224
+f 219//219 217//217 226//226
+f 226//226 217//217 2051//2051
+f 224//224 206//206 208//208
+f 218//218 40//40 219//219
+f 216//216 224//224 208//208
+f 203//203 218//218 219//219
+f 204//204 195//195 214//214
+f 227//227 212//212 206//206
+f 220//220 217//217 40//40
+f 195//195 207//207 214//214
+f 207//207 216//216 214//214
+f 206//206 212//212 208//208
+f 221//221 965//965 236//236
+f 217//217 219//219 40//40
+f 2055//2055 213//213 222//222
+f 203//203 202//202 259//259
+f 225//225 219//219 226//226
+f 223//223 206//206 224//224
+f 258//258 202//202 229//229
+f 222//222 225//225 226//226
+f 206//206 231//231 227//227
+f 227//227 231//231 229//229
+f 217//217 2076//2076 2051//2051
+f 259//259 202//202 258//258
+f 2076//2076 228//228 2051//2051
+f 259//259 218//218 203//203
+f 202//202 227//227 229//229
+f 2052//2052 226//226 2051//2051
+f 206//206 230//230 231//231
+f 252//252 232//232 247//247
+f 232//232 252//252 251//251
+f 240//240 247//247 253//253
+f 238//238 256//256 245//245
+f 241//241 242//242 233//233
+f 235//235 3260//3260 280//280
+f 256//256 241//241 233//233
+f 235//235 242//242 243//243
+f 233//233 242//242 235//235
+f 258//258 229//229 249//249
+f 243//243 4846//4846 3257//3257
+f 234//234 258//258 249//249
+f 233//233 235//235 280//280
+f 3257//3257 2125//2125 3260//3260
+f 235//235 243//243 3257//3257
+f 3260//3260 235//235 3257//3257
+f 236//236 965//965 237//237
+f 256//256 238//238 241//241
+f 239//239 242//242 241//241
+f 238//238 240//240 241//241
+f 240//240 239//239 241//241
+f 242//242 4815//4815 243//243
+f 242//242 239//239 4815//4815
+f 243//243 4814//4814 4846//4846
+f 244//244 255//255 245//245
+f 249//249 244//244 245//245
+f 238//238 245//245 255//255
+f 233//233 280//280 263//263
+f 231//231 230//230 244//244
+f 249//249 229//229 244//244
+f 246//246 237//237 967//967
+f 229//229 231//231 244//244
+f 247//247 232//232 248//248
+f 232//232 967//967 248//248
+f 249//249 245//245 257//257
+f 232//232 246//246 967//967
+f 232//232 251//251 246//246
+f 250//250 237//237 246//246
+f 250//250 236//236 237//237
+f 251//251 223//223 246//246
+f 223//223 250//250 246//246
+f 230//230 223//223 251//251
+f 251//251 255//255 230//230
+f 252//252 238//238 255//255
+f 253//253 247//247 971//971
+f 253//253 971//971 254//254
+f 253//253 254//254 3255//3255
+f 251//251 252//252 255//255
+f 230//230 255//255 244//244
+f 257//257 256//256 263//263
+f 233//233 263//263 256//256
+f 252//252 240//240 238//238
+f 245//245 256//256 257//257
+f 258//258 265//265 259//259
+f 254//254 4836//4836 3255//3255
+f 240//240 253//253 239//239
+f 247//247 248//248 971//971
+f 252//252 247//247 240//240
+f 260//260 267//267 271//271
+f 275//275 2134//2134 260//260
+f 275//275 261//261 3268//3268
+f 261//261 271//271 274//274
+f 261//261 3265//3265 3268//3268
+f 2134//2134 273//273 260//260
+f 2159//2159 273//273 262//262
+f 269//269 33//33 277//277
+f 268//268 263//263 267//267
+f 263//263 280//280 267//267
+f 257//257 268//268 234//234
+f 257//257 263//263 268//268
+f 36//36 33//33 4816//4816
+f 264//264 277//277 276//276
+f 267//267 260//260 268//268
+f 234//234 268//268 276//276
+f 268//268 272//272 276//276
+f 277//277 265//265 276//276
+f 276//276 265//265 234//234
+f 277//277 33//33 278//278
+f 265//265 277//277 278//278
+f 281//281 274//274 270//270
+f 3260//3260 281//281 266//266
+f 259//259 265//265 278//278
+f 234//234 265//265 258//258
+f 249//249 257//257 234//234
+f 271//271 267//267 270//270
+f 281//281 270//270 266//266
+f 266//266 270//270 267//267
+f 268//268 260//260 272//272
+f 4857//4857 269//269 264//264
+f 270//270 274//274 271//271
+f 2158//2158 264//264 272//272
+f 2159//2159 272//272 273//273
+f 33//33 279//279 278//278
+f 2159//2159 2158//2158 272//272
+f 274//274 3263//3263 261//261
+f 260//260 271//271 275//275
+f 273//273 272//272 260//260
+f 271//271 261//261 275//275
+f 264//264 2158//2158 4857//4857
+f 276//276 272//272 264//264
+f 269//269 277//277 264//264
+f 218//218 278//278 279//279
+f 280//280 3260//3260 266//266
+f 280//280 266//266 267//267
+f 3260//3260 3259//3259 281//281
+f 2875//2875 282//282 986//986
+f 3462//3462 283//283 3459//3459
+f 979//979 3462//3462 981//981
+f 3437//3437 312//312 3426//3426
+f 312//312 285//285 3426//3426
+f 3462//3462 979//979 283//283
+f 287//287 288//288 1994//1994
+f 1994//1994 3336//3336 1995//1995
+f 284//284 3336//3336 286//286
+f 286//286 3336//3336 3335//3335
+f 286//286 3335//3335 3426//3426
+f 285//285 286//286 3426//3426
+f 285//285 809//809 284//284
+f 284//284 1995//1995 3336//3336
+f 286//286 285//285 284//284
+f 288//288 287//287 3303//3303
+f 287//287 2007//2007 3303//3303
+f 309//309 2868//2868 310//310
+f 309//309 2867//2867 2868//2868
+f 1994//1994 288//288 3357//3357
+f 3437//3437 309//309 312//312
+f 2007//2007 287//287 2016//2016
+f 1075//1075 31//31 4304//4304
+f 289//289 1994//1994 3357//3357
+f 297//297 322//322 293//293
+f 301//301 311//311 303//303
+f 300//300 325//325 327//327
+f 995//995 290//290 316//316
+f 317//317 291//291 294//294
+f 313//313 298//298 319//319
+f 316//316 294//294 995//995
+f 295//295 302//302 314//314
+f 302//302 295//295 292//292
+f 302//302 293//293 310//310
+f 314//314 320//320 984//984
+f 297//297 293//293 302//302
+f 314//314 302//302 310//310
+f 995//995 294//294 296//296
+f 322//322 297//297 291//291
+f 294//294 299//299 296//296
+f 984//984 295//295 314//314
+f 295//295 978//978 292//292
+f 296//296 299//299 983//983
+f 299//299 292//292 983//983
+f 297//297 292//292 299//299
+f 993//993 995//995 296//296
+f 294//294 291//291 299//299
+f 298//298 327//327 334//334
+f 291//291 297//297 299//299
+f 300//300 327//327 298//298
+f 301//301 303//303 325//325
+f 300//300 301//301 325//325
+f 297//297 302//302 292//292
+f 303//303 311//311 306//306
+f 809//809 313//313 319//319
+f 809//809 285//285 313//313
+f 2868//2868 304//304 320//320
+f 314//314 2868//2868 320//320
+f 316//316 305//305 315//315
+f 304//304 4436//4436 320//320
+f 4437//4437 320//320 4436//4436
+f 283//283 979//979 4437//4437
+f 306//306 311//311 307//307
+f 307//307 305//305 308//308
+f 315//315 305//305 307//307
+f 309//309 310//310 312//312
+f 313//313 312//312 310//310
+f 311//311 315//315 307//307
+f 285//285 312//312 313//313
+f 2868//2868 314//314 310//310
+f 313//313 293//293 298//298
+f 322//322 300//300 293//293
+f 293//293 313//313 310//310
+f 317//317 316//316 315//315
+f 316//316 290//290 305//305
+f 316//316 317//317 294//294
+f 317//317 315//315 318//318
+f 322//322 291//291 318//318
+f 291//291 317//317 318//318
+f 319//319 298//298 334//334
+f 320//320 4437//4437 984//984
+f 4540//4540 4749//4749 321//321
+f 311//311 301//301 318//318
+f 301//301 322//322 318//318
+f 300//300 298//298 293//293
+f 322//322 301//301 300//300
+f 315//315 311//311 318//318
+f 306//306 323//323 328//328
+f 308//308 342//342 324//324
+f 303//303 328//328 326//326
+f 306//306 307//307 324//324
+f 323//323 306//306 324//324
+f 334//334 327//327 332//332
+f 350//350 353//353 338//338
+f 332//332 335//335 334//334
+f 307//307 308//308 324//324
+f 325//325 326//326 332//332
+f 327//327 325//325 332//332
+f 303//303 306//306 328//328
+f 325//325 303//303 326//326
+f 329//329 1010//1010 1009//1009
+f 330//330 802//802 336//336
+f 330//330 336//336 335//335
+f 332//332 326//326 331//331
+f 335//335 332//332 331//331
+f 326//326 328//328 333//333
+f 331//331 326//326 333//333
+f 328//328 323//323 341//341
+f 333//333 328//328 341//341
+f 334//334 335//335 336//336
+f 353//353 356//356 339//339
+f 338//338 353//353 339//339
+f 356//356 799//799 340//340
+f 339//339 356//356 340//340
+f 809//809 319//319 802//802
+f 343//343 337//337 323//323
+f 338//338 333//333 341//341
+f 343//343 342//342 1010//1010
+f 343//343 1010//1010 344//344
+f 331//331 339//339 340//340
+f 335//335 340//340 330//330
+f 335//335 331//331 340//340
+f 340//340 799//799 330//330
+f 331//331 333//333 339//339
+f 324//324 343//343 323//323
+f 341//341 323//323 337//337
+f 324//324 342//342 343//343
+f 1010//1010 329//329 344//344
+f 337//337 344//344 347//347
+f 337//337 343//343 344//344
+f 338//338 341//341 347//347
+f 341//341 337//337 347//347
+f 333//333 338//338 339//339
+f 352//352 361//361 368//368
+f 352//352 368//368 351//351
+f 319//319 334//334 336//336
+f 361//361 357//357 345//345
+f 344//344 329//329 348//348
+f 361//361 352//352 357//357
+f 356//356 355//355 799//799
+f 345//345 349//349 362//362
+f 350//350 338//338 347//347
+f 346//346 797//797 354//354
+f 347//347 344//344 348//348
+f 355//355 354//354 797//797
+f 797//797 346//346 367//367
+f 350//350 347//347 348//348
+f 319//319 336//336 802//802
+f 351//351 368//368 346//346
+f 354//354 351//351 346//346
+f 357//357 348//348 349//349
+f 348//348 329//329 349//349
+f 352//352 348//348 357//357
+f 350//350 352//352 351//351
+f 352//352 350//350 348//348
+f 353//353 351//351 354//354
+f 353//353 350//350 351//351
+f 355//355 356//356 354//354
+f 356//356 353//353 354//354
+f 345//345 357//357 349//349
+f 362//362 358//358 365//365
+f 362//362 349//349 358//358
+f 365//365 359//359 1046//1046
+f 365//365 358//358 359//359
+f 358//358 1009//1009 359//359
+f 349//349 329//329 358//358
+f 329//329 1009//1009 358//358
+f 403//403 437//437 360//360
+f 361//361 345//345 380//380
+f 361//361 380//380 378//378
+f 345//345 362//362 363//363
+f 380//380 345//345 363//363
+f 362//362 365//365 364//364
+f 363//363 362//362 364//364
+f 364//364 365//365 366//366
+f 365//365 1046//1046 366//366
+f 366//366 1046//1046 1048//1048
+f 797//797 367//367 371//371
+f 346//346 377//377 367//367
+f 346//346 368//368 377//377
+f 368//368 361//361 378//378
+f 368//368 378//378 377//377
+f 786//786 369//369 386//386
+f 379//379 375//375 369//369
+f 386//386 360//360 370//370
+f 370//370 786//786 386//386
+f 786//786 379//379 369//369
+f 786//786 795//795 379//379
+f 371//371 367//367 796//796
+f 367//367 377//377 795//795
+f 373//373 400//400 372//372
+f 373//373 372//372 1047//1047
+f 372//372 384//384 376//376
+f 374//374 384//384 383//383
+f 376//376 384//384 374//374
+f 375//375 374//374 383//383
+f 375//375 383//383 369//369
+f 363//363 364//364 376//376
+f 363//363 376//376 374//374
+f 364//364 366//366 372//372
+f 364//364 372//372 376//376
+f 366//366 1048//1048 1047//1047
+f 372//372 366//366 1047//1047
+f 796//796 367//367 795//795
+f 377//377 378//378 379//379
+f 377//377 379//379 795//795
+f 378//378 375//375 379//379
+f 378//378 380//380 375//375
+f 380//380 363//363 374//374
+f 375//375 380//380 374//374
+f 372//372 400//400 384//384
+f 400//400 373//373 381//381
+f 383//383 382//382 369//369
+f 383//383 384//384 385//385
+f 382//382 383//383 385//385
+f 384//384 400//400 385//385
+f 386//386 382//382 403//403
+f 373//373 1045//1045 387//387
+f 403//403 360//360 386//386
+f 386//386 369//369 382//382
+f 373//373 1047//1047 1045//1045
+f 381//381 373//373 387//387
+f 396//396 389//389 395//395
+f 389//389 396//396 388//388
+f 395//395 389//389 429//429
+f 1070//1070 1071//1071 390//390
+f 398//398 1070//1070 390//390
+f 391//391 1071//1071 404//404
+f 390//390 1071//1071 391//391
+f 392//392 404//404 393//393
+f 391//391 404//404 392//392
+f 388//388 396//396 393//393
+f 396//396 392//392 393//393
+f 395//395 394//394 402//402
+f 430//430 394//394 438//438
+f 394//394 395//395 438//438
+f 396//396 395//395 402//402
+f 392//392 402//402 397//397
+f 392//392 396//396 402//402
+f 391//391 397//397 401//401
+f 391//391 392//392 397//397
+f 390//390 401//401 399//399
+f 390//390 391//391 401//401
+f 399//399 398//398 390//390
+f 399//399 1022//1022 398//398
+f 1055//1055 398//398 1022//1022
+f 395//395 429//429 438//438
+f 399//399 387//387 1022//1022
+f 401//401 381//381 399//399
+f 381//381 387//387 399//399
+f 397//397 400//400 401//401
+f 400//400 381//381 401//401
+f 402//402 385//385 397//397
+f 385//385 400//400 397//397
+f 394//394 382//382 402//402
+f 382//382 385//385 402//402
+f 430//430 403//403 394//394
+f 403//403 430//430 427//427
+f 403//403 382//382 394//394
+f 437//437 403//403 427//427
+f 1066//1066 2612//2612 393//393
+f 393//393 2612//2612 388//388
+f 393//393 404//404 1066//1066
+f 1066//1066 1071//1071 1067//1067
+f 404//404 1071//1071 1066//1066
+f 398//398 1055//1055 1070//1070
+f 4689//4689 3669//3669 405//405
+f 413//413 421//421 408//408
+f 1605//1605 407//407 1607//1607
+f 665//665 417//417 473//473
+f 415//415 423//423 417//417
+f 665//665 473//473 638//638
+f 423//423 424//424 484//484
+f 473//473 423//423 406//406
+f 407//407 413//413 1607//1607
+f 1608//1608 1607//1607 4426//4426
+f 408//408 4426//4426 413//413
+f 1607//1607 413//413 4426//4426
+f 409//409 414//414 442//442
+f 1531//1531 480//480 1605//1605
+f 3847//3847 409//409 435//435
+f 480//480 407//407 1605//1605
+f 413//413 407//407 410//410
+f 412//412 424//424 411//411
+f 423//423 484//484 406//406
+f 411//411 421//421 412//412
+f 412//412 484//484 424//424
+f 421//421 413//413 410//410
+f 421//421 410//410 412//412
+f 3847//3847 435//435 434//434
+f 418//418 408//408 409//409
+f 421//421 411//411 414//414
+f 408//408 414//414 409//409
+f 411//411 440//440 428//428
+f 414//414 411//411 428//428
+f 417//417 423//423 473//473
+f 422//422 415//415 441//441
+f 440//440 422//422 441//441
+f 415//415 426//426 416//416
+f 423//423 415//415 422//422
+f 665//665 664//664 417//417
+f 417//417 664//664 426//426
+f 415//415 417//417 426//426
+f 426//426 664//664 425//425
+f 408//408 418//418 4426//4426
+f 3243//3243 419//419 420//420
+f 421//421 414//414 408//408
+f 440//440 424//424 422//422
+f 424//424 423//423 422//422
+f 411//411 424//424 440//440
+f 441//441 415//415 416//416
+f 426//426 425//425 433//433
+f 433//433 425//425 663//663
+f 416//416 426//426 433//433
+f 663//663 660//660 436//436
+f 427//427 436//436 652//652
+f 427//427 430//430 431//431
+f 442//442 428//428 429//429
+f 438//438 439//439 432//432
+f 430//430 438//438 432//432
+f 431//431 430//430 432//432
+f 436//436 427//427 431//431
+f 439//439 441//441 432//432
+f 416//416 433//433 431//431
+f 416//416 431//431 432//432
+f 663//663 436//436 433//433
+f 434//434 435//435 2651//2651
+f 435//435 388//388 2651//2651
+f 433//433 436//436 431//431
+f 652//652 436//436 660//660
+f 437//437 427//427 652//652
+f 438//438 429//429 439//439
+f 389//389 388//388 435//435
+f 414//414 428//428 442//442
+f 389//389 435//435 442//442
+f 440//440 441//441 439//439
+f 428//428 439//439 429//429
+f 428//428 440//440 439//439
+f 441//441 416//416 432//432
+f 409//409 442//442 435//435
+f 429//429 389//389 442//442
+f 453//453 446//446 443//443
+f 482//482 481//481 464//464
+f 454//454 451//451 517//517
+f 444//444 453//453 445//445
+f 446//446 453//453 462//462
+f 447//447 1550//1550 448//448
+f 446//446 447//447 448//448
+f 1550//1550 449//449 450//450
+f 448//448 1550//1550 450//450
+f 450//450 449//449 459//459
+f 449//449 460//460 459//459
+f 517//517 451//451 1523//1523
+f 452//452 488//488 455//455
+f 453//453 443//443 445//445
+f 517//517 452//452 454//454
+f 460//460 454//454 461//461
+f 454//454 455//455 470//470
+f 455//455 454//454 452//452
+f 445//445 443//443 485//485
+f 443//443 457//457 456//456
+f 485//485 443//443 456//456
+f 456//456 457//457 474//474
+f 457//457 458//458 474//474
+f 458//458 463//463 474//474
+f 458//458 482//482 463//463
+f 443//443 446//446 457//457
+f 446//446 448//448 457//457
+f 457//457 448//448 458//458
+f 458//458 448//448 450//450
+f 482//482 450//450 459//459
+f 482//482 458//458 450//450
+f 481//481 482//482 459//459
+f 459//459 460//460 461//461
+f 481//481 459//459 461//461
+f 451//451 454//454 460//460
+f 446//446 462//462 447//447
+f 463//463 464//464 471//471
+f 464//464 467//467 466//466
+f 468//468 465//465 466//466
+f 644//644 466//466 465//465
+f 471//471 464//464 466//466
+f 644//644 471//471 466//466
+f 467//467 470//470 468//468
+f 465//465 468//468 631//631
+f 466//466 467//467 468//468
+f 470//470 455//455 487//487
+f 487//487 469//469 632//632
+f 631//631 487//487 632//632
+f 468//468 470//470 487//487
+f 471//471 478//478 477//477
+f 478//478 475//475 477//477
+f 486//486 476//476 472//472
+f 406//406 486//486 638//638
+f 472//472 638//638 486//486
+f 473//473 406//406 638//638
+f 474//474 477//477 476//476
+f 477//477 475//475 476//476
+f 474//474 463//463 477//477
+f 471//471 644//644 478//478
+f 463//463 471//471 477//477
+f 469//469 487//487 483//483
+f 410//410 444//444 445//445
+f 410//410 407//407 444//444
+f 461//461 454//454 470//470
+f 480//480 479//479 444//444
+f 407//407 480//480 444//444
+f 480//480 1531//1531 479//479
+f 464//464 481//481 467//467
+f 481//481 461//461 467//467
+f 482//482 464//464 463//463
+f 483//483 489//489 535//535
+f 462//462 453//453 479//479
+f 453//453 444//444 479//479
+f 631//631 468//468 487//487
+f 406//406 485//485 486//486
+f 406//406 484//484 485//485
+f 455//455 488//488 483//483
+f 483//483 488//488 489//489
+f 469//469 483//483 535//535
+f 486//486 456//456 476//476
+f 484//484 445//445 485//485
+f 484//484 412//412 445//445
+f 476//476 475//475 472//472
+f 456//456 474//474 476//476
+f 485//485 456//456 486//486
+f 461//461 470//470 467//467
+f 487//487 455//455 483//483
+f 412//412 410//410 445//445
+f 488//488 493//493 491//491
+f 488//488 452//452 493//493
+f 489//489 492//492 490//490
+f 489//489 491//491 492//492
+f 489//489 488//488 491//491
+f 491//491 493//493 512//512
+f 494//494 495//495 1653//1653
+f 495//495 494//494 499//499
+f 496//496 499//499 497//497
+f 495//495 499//499 496//496
+f 498//498 497//497 500//500
+f 496//496 497//497 498//498
+f 504//504 500//500 503//503
+f 498//498 500//500 504//504
+f 505//505 503//503 501//501
+f 512//512 511//511 502//502
+f 499//499 515//515 514//514
+f 499//499 494//494 515//515
+f 497//497 499//499 514//514
+f 500//500 514//514 521//521
+f 500//500 497//497 514//514
+f 503//503 521//521 524//524
+f 503//503 500//500 521//521
+f 501//501 524//524 520//520
+f 501//501 503//503 524//524
+f 502//502 511//511 520//520
+f 511//511 501//501 520//520
+f 491//491 512//512 502//502
+f 504//504 503//503 505//505
+f 498//498 504//504 1518//1518
+f 498//498 1518//1518 1530//1530
+f 504//504 505//505 1518//1518
+f 1514//1514 505//505 510//510
+f 1518//1518 505//505 1514//1514
+f 506//506 510//510 507//507
+f 1514//1514 510//510 506//506
+f 1524//1524 507//507 508//508
+f 506//506 507//507 1524//1524
+f 1525//1525 508//508 509//509
+f 1524//1524 508//508 1525//1525
+f 1525//1525 509//509 1523//1523
+f 496//496 1530//1530 1520//1520
+f 501//501 511//511 510//510
+f 505//505 501//501 510//510
+f 511//511 512//512 507//507
+f 510//510 511//511 507//507
+f 512//512 493//493 508//508
+f 507//507 512//512 508//508
+f 508//508 493//493 509//509
+f 496//496 1520//1520 495//495
+f 1653//1653 495//495 1441//1441
+f 495//495 513//513 1441//1441
+f 496//496 498//498 1530//1530
+f 1520//1520 513//513 495//495
+f 1650//1650 1649//1649 515//515
+f 530//530 4537//4537 516//516
+f 4537//4537 515//515 516//516
+f 530//530 516//516 4451//4451
+f 1651//1651 4452//4452 4450//4450
+f 1649//1649 516//516 515//515
+f 514//514 515//515 4537//4537
+f 1649//1649 518//518 516//516
+f 493//493 452//452 509//509
+f 452//452 517//517 509//509
+f 509//509 517//517 1523//1523
+f 518//518 1649//1649 4450//4450
+f 4456//4456 1697//1697 1651//1651
+f 4536//4536 525//525 4537//4537
+f 532//532 522//522 523//523
+f 520//520 524//524 522//522
+f 533//533 492//492 519//519
+f 492//492 502//502 519//519
+f 532//532 533//533 519//519
+f 502//502 520//520 519//519
+f 492//492 533//533 527//527
+f 492//492 491//491 502//502
+f 1650//1650 515//515 494//494
+f 520//520 522//522 519//519
+f 4537//4537 525//525 521//521
+f 514//514 4537//4537 521//521
+f 523//523 522//522 525//525
+f 522//522 524//524 525//525
+f 525//525 4536//4536 523//523
+f 524//524 521//521 525//525
+f 522//522 532//532 519//519
+f 731//731 4534//4534 529//529
+f 586//586 527//527 526//526
+f 533//533 526//526 527//527
+f 526//526 534//534 598//598
+f 534//534 536//536 528//528
+f 529//529 4534//4534 536//536
+f 536//536 4534//4534 528//528
+f 529//529 537//537 732//732
+f 526//526 598//598 4530//4530
+f 4530//4530 598//598 4531//4531
+f 4534//4534 4533//4533 528//528
+f 530//530 4451//4451 531//531
+f 532//532 523//523 536//536
+f 533//533 534//534 526//526
+f 533//533 532//532 534//534
+f 492//492 527//527 490//490
+f 490//490 535//535 489//489
+f 534//534 532//532 536//536
+f 529//529 523//523 537//537
+f 523//523 4536//4536 537//537
+f 523//523 529//529 536//536
+f 4452//4452 545//545 543//543
+f 538//538 1689//1689 547//547
+f 1646//1646 539//539 549//549
+f 1806//1806 4463//4463 540//540
+f 1716//1716 1637//1637 565//565
+f 541//541 567//567 1709//1709
+f 4496//4496 4492//4492 542//542
+f 614//614 696//696 615//615
+f 734//734 543//543 544//544
+f 4451//4451 736//736 734//734
+f 544//544 543//543 545//545
+f 546//546 545//545 4452//4452
+f 1697//1697 546//546 4452//4452
+f 547//547 1760//1760 1648//1648
+f 1760//1760 547//547 1700//1700
+f 1694//1694 1689//1689 1690//1690
+f 4459//4459 1690//1690 1689//1689
+f 1646//1646 548//548 1682//1682
+f 1682//1682 4459//4459 1646//1646
+f 1646//1646 549//549 548//548
+f 4460//4460 1673//1673 549//549
+f 548//548 549//549 1673//1673
+f 4460//4460 1825//1825 550//550
+f 540//540 550//550 1825//1825
+f 867//867 550//550 4463//4463
+f 858//858 4463//4463 551//551
+f 858//858 867//867 4463//4463
+f 4465//4465 858//858 551//551
+f 859//859 4465//4465 552//552
+f 552//552 553//553 554//554
+f 552//552 1808//1808 553//553
+f 553//553 556//556 554//554
+f 555//555 556//556 557//557
+f 554//554 556//556 555//555
+f 1768//1768 558//558 834//834
+f 555//555 557//557 1768//1768
+f 558//558 1831//1831 559//559
+f 559//559 862//862 558//558
+f 559//559 1831//1831 561//561
+f 560//560 559//559 561//561
+f 562//562 560//560 561//561
+f 562//562 561//561 563//563
+f 4665//4665 990//990 2819//2819
+f 564//564 562//562 565//565
+f 1637//1637 564//564 565//565
+f 1638//1638 1637//1637 4477//4477
+f 566//566 4477//4477 1709//1709
+f 567//567 566//566 1709//1709
+f 1671//1671 567//567 541//541
+f 1678//1678 567//567 1671//1671
+f 1712//1712 1672//1672 1671//1671
+f 1672//1672 568//568 1681//1681
+f 568//568 4487//4487 1681//1681
+f 569//569 1681//1681 4487//4487
+f 4488//4488 569//569 4487//4487
+f 1696//1696 570//570 1688//1688
+f 1696//1696 1688//1688 4490//4490
+f 1698//1698 1696//1696 4490//4490
+f 4490//4490 4489//4489 571//571
+f 1698//1698 4490//4490 571//571
+f 573//573 571//571 572//572
+f 1698//1698 571//571 573//573
+f 573//573 576//576 574//574
+f 1752//1752 1728//1728 576//576
+f 575//575 1752//1752 576//576
+f 1751//1751 1752//1752 575//575
+f 866//866 1751//1751 4492//4492
+f 1751//1751 866//866 1830//1830
+f 866//866 577//577 1830//1830
+f 4495//4495 866//866 865//865
+f 577//577 866//866 4495//4495
+f 1918//1918 577//577 4495//4495
+f 578//578 1918//1918 4495//4495
+f 578//578 856//856 1918//1918
+f 1920//1920 1918//1918 856//856
+f 851//851 1926//1926 4498//4498
+f 4500//4500 579//579 851//851
+f 851//851 579//579 1926//1926
+f 580//580 4500//4500 581//581
+f 580//580 579//579 4500//4500
+f 582//582 584//584 580//580
+f 580//580 584//584 579//579
+f 583//583 2027//2027 582//582
+f 2027//2027 584//584 582//582
+f 585//585 4524//4524 4522//4522
+f 591//591 714//714 4526//4526
+f 586//586 4527//4527 600//600
+f 2034//2034 583//583 4507//4507
+f 714//714 591//591 4528//4528
+f 4528//4528 600//600 4527//4527
+f 583//583 2034//2034 2027//2027
+f 4458//4458 2034//2034 4507//4507
+f 586//586 587//587 4527//4527
+f 587//587 586//586 4530//4530
+f 585//585 4522//4522 611//611
+f 4522//4522 612//612 611//611
+f 588//588 612//612 4522//4522
+f 589//589 591//591 585//585
+f 611//611 589//589 585//585
+f 589//589 601//601 591//591
+f 601//601 590//590 4528//4528
+f 601//601 4528//4528 591//591
+f 592//592 593//593 4511//4511
+f 4457//4457 592//592 4508//4508
+f 592//592 4457//4457 4458//4458
+f 4526//4526 585//585 591//591
+f 592//592 2018//2018 4508//4508
+f 4524//4524 585//585 4526//4526
+f 600//600 4528//4528 590//590
+f 2018//2018 595//595 4508//4508
+f 594//594 595//595 2018//2018
+f 595//595 594//594 4506//4506
+f 597//597 4506//4506 594//594
+f 597//597 596//596 4506//4506
+f 596//596 597//597 4513//4513
+f 596//596 4513//4513 599//599
+f 599//599 4513//4513 738//738
+f 598//598 534//534 528//528
+f 729//729 4531//4531 598//598
+f 4514//4514 599//599 738//738
+f 527//527 586//586 600//600
+f 490//490 527//527 600//600
+f 526//526 4530//4530 586//586
+f 490//490 600//600 590//590
+f 4514//4514 4482//4482 4481//4481
+f 4482//4482 4514//4514 716//716
+f 535//535 490//490 590//590
+f 601//601 535//535 590//590
+f 602//602 703//703 604//604
+f 603//603 605//605 604//604
+f 603//603 4517//4517 605//605
+f 4517//4517 4504//4504 605//605
+f 4504//4504 4517//4517 4516//4516
+f 4504//4504 4516//4516 4502//4502
+f 4502//4502 4516//4516 725//725
+f 4474//4474 607//607 4473//4473
+f 606//606 4473//4473 607//607
+f 606//606 4472//4472 4473//4473
+f 4472//4472 606//606 4518//4518
+f 4472//4472 4518//4518 608//608
+f 608//608 4518//4518 4520//4520
+f 609//609 678//678 653//653
+f 610//610 609//609 636//636
+f 609//609 610//610 678//678
+f 636//636 640//640 625//625
+f 601//601 589//589 643//643
+f 589//589 611//611 634//634
+f 634//634 611//611 613//613
+f 611//611 612//612 613//613
+f 612//612 614//614 613//613
+f 612//612 700//700 614//614
+f 613//613 614//614 615//615
+f 613//613 615//615 616//616
+f 616//616 615//615 620//620
+f 624//624 620//620 617//617
+f 616//616 620//620 624//624
+f 617//617 618//618 630//630
+f 617//617 630//630 624//624
+f 639//639 619//619 620//620
+f 620//620 615//615 639//639
+f 617//617 637//637 621//621
+f 617//617 620//620 637//637
+f 618//618 621//621 692//692
+f 618//618 617//617 621//621
+f 618//618 692//692 682//682
+f 465//465 631//631 622//622
+f 623//623 624//624 625//625
+f 625//625 624//624 630//630
+f 610//610 625//625 630//630
+f 610//610 630//630 626//626
+f 626//626 633//633 628//628
+f 610//610 626//626 628//628
+f 632//632 469//469 627//627
+f 678//678 610//610 628//628
+f 632//632 627//627 629//629
+f 469//469 535//535 643//643
+f 469//469 643//643 627//627
+f 630//630 618//618 626//626
+f 618//618 682//682 626//626
+f 631//631 632//632 629//629
+f 633//633 626//626 682//682
+f 622//622 631//631 629//629
+f 613//613 616//616 641//641
+f 634//634 613//613 641//641
+f 641//641 616//616 642//642
+f 624//624 623//623 642//642
+f 624//624 642//642 616//616
+f 615//615 696//696 639//639
+f 627//627 634//634 641//641
+f 589//589 634//634 643//643
+f 535//535 601//601 643//643
+f 475//475 478//478 640//640
+f 635//635 692//692 724//724
+f 636//636 475//475 640//640
+f 724//724 621//621 4519//4519
+f 692//692 621//621 724//724
+f 620//620 619//619 637//637
+f 638//638 472//472 667//667
+f 696//696 695//695 639//639
+f 636//636 625//625 610//610
+f 623//623 640//640 644//644
+f 623//623 625//625 640//640
+f 622//622 623//623 644//644
+f 623//623 622//622 642//642
+f 667//667 472//472 609//609
+f 609//609 472//472 636//636
+f 472//472 475//475 636//636
+f 622//622 629//629 642//642
+f 629//629 627//627 641//641
+f 629//629 641//641 642//642
+f 627//627 643//643 634//634
+f 478//478 644//644 640//640
+f 644//644 465//465 622//622
+f 588//588 700//700 612//612
+f 4519//4519 4469//4469 4520//4520
+f 4469//4469 645//645 4470//4470
+f 614//614 699//699 696//696
+f 645//645 4469//4469 619//619
+f 645//645 619//619 4521//4521
+f 655//655 657//657 651//651
+f 647//647 650//650 669//669
+f 651//651 654//654 646//646
+f 647//647 648//648 649//649
+f 651//651 646//646 669//669
+f 646//646 647//647 669//669
+f 650//650 647//647 649//649
+f 651//651 674//674 676//676
+f 651//651 669//669 674//674
+f 437//437 652//652 784//784
+f 655//655 676//676 653//653
+f 655//655 651//651 676//676
+f 653//653 678//678 655//655
+f 655//655 678//678 679//679
+f 654//654 689//689 658//658
+f 646//646 654//654 658//658
+f 654//654 657//657 689//689
+f 656//656 687//687 688//688
+f 657//657 656//656 688//688
+f 687//687 656//656 680//680
+f 689//689 657//657 688//688
+f 651//651 657//657 654//654
+f 655//655 679//679 656//656
+f 657//657 655//655 656//656
+f 656//656 679//679 680//680
+f 648//648 647//647 789//789
+f 633//633 681//681 680//680
+f 789//789 646//646 658//658
+f 647//647 646//646 789//789
+f 660//660 663//663 659//659
+f 660//660 659//659 671//671
+f 660//660 661//661 652//652
+f 660//660 671//671 661//661
+f 784//784 652//652 661//661
+f 784//784 661//661 670//670
+f 675//675 425//425 662//662
+f 425//425 664//664 662//662
+f 659//659 663//663 675//675
+f 663//663 425//425 675//675
+f 664//664 665//665 666//666
+f 662//662 664//664 666//666
+f 665//665 667//667 666//666
+f 638//638 667//667 665//665
+f 672//672 673//673 671//671
+f 667//667 653//653 666//666
+f 653//653 676//676 666//666
+f 653//653 667//667 609//609
+f 670//670 673//673 668//668
+f 650//650 673//673 672//672
+f 673//673 650//650 668//668
+f 650//650 649//649 668//668
+f 674//674 669//669 672//672
+f 669//669 650//650 672//672
+f 670//670 661//661 673//673
+f 671//671 659//659 672//672
+f 671//671 673//673 661//661
+f 674//674 659//659 675//675
+f 659//659 674//674 672//672
+f 676//676 675//675 662//662
+f 676//676 674//674 675//675
+f 676//676 662//662 666//666
+f 690//690 686//686 677//677
+f 678//678 628//628 679//679
+f 679//679 628//628 680//680
+f 681//681 682//682 683//683
+f 686//686 683//683 710//710
+f 691//691 684//684 708//708
+f 633//633 682//682 681//681
+f 691//691 708//708 791//791
+f 684//684 690//690 685//685
+f 628//628 633//633 680//680
+f 690//690 677//677 685//685
+f 681//681 687//687 680//680
+f 686//686 710//710 677//677
+f 687//687 681//681 683//683
+f 690//690 688//688 686//686
+f 688//688 687//687 686//686
+f 689//689 690//690 684//684
+f 689//689 688//688 690//690
+f 691//691 658//658 684//684
+f 658//658 689//689 684//684
+f 789//789 658//658 691//691
+f 790//790 789//789 691//691
+f 686//686 687//687 683//683
+f 692//692 635//635 693//693
+f 710//710 683//683 693//693
+f 684//684 685//685 708//708
+f 682//682 692//692 683//683
+f 683//683 692//692 693//693
+f 645//645 4521//4521 697//697
+f 697//697 4521//4521 695//695
+f 694//694 695//695 696//696
+f 697//697 694//694 698//698
+f 699//699 694//694 696//696
+f 699//699 701//701 694//694
+f 699//699 700//700 701//701
+f 700//700 4494//4494 701//701
+f 702//702 739//739 705//705
+f 703//703 716//716 728//728
+f 706//706 4494//4494 588//588
+f 4515//4515 4517//4517 711//711
+f 703//703 728//728 715//715
+f 739//739 704//704 705//705
+f 705//705 704//704 717//717
+f 588//588 4523//4523 706//706
+f 685//685 727//727 707//707
+f 685//685 677//677 727//727
+f 685//685 707//707 709//709
+f 708//708 709//709 718//718
+f 708//708 685//685 709//709
+f 708//708 718//718 791//791
+f 693//693 723//723 726//726
+f 710//710 726//726 4515//4515
+f 710//710 693//693 726//726
+f 677//677 710//710 4515//4515
+f 4515//4515 711//711 677//677
+f 693//693 635//635 723//723
+f 4525//4525 712//712 2038//2038
+f 2038//2038 712//712 2036//2036
+f 713//713 712//712 714//714
+f 713//713 2036//2036 712//712
+f 604//604 715//715 603//603
+f 716//716 720//720 719//719
+f 728//728 716//716 719//719
+f 720//720 717//717 718//718
+f 719//719 720//720 718//718
+f 704//704 721//721 717//717
+f 718//718 717//717 721//721
+f 4461//4461 713//713 4527//4527
+f 4461//4461 2036//2036 713//713
+f 722//722 4462//4462 4461//4461
+f 4461//4461 587//587 722//722
+f 723//723 635//635 607//607
+f 709//709 719//719 718//718
+f 709//709 707//707 719//719
+f 721//721 743//743 737//737
+f 791//791 718//718 721//721
+f 721//721 737//737 791//791
+f 635//635 724//724 606//606
+f 723//723 725//725 726//726
+f 726//726 4516//4516 4515//4515
+f 727//727 711//711 603//603
+f 677//677 711//711 727//727
+f 715//715 727//727 603//603
+f 728//728 707//707 715//715
+f 707//707 727//727 715//715
+f 707//707 728//728 719//719
+f 587//587 4529//4529 722//722
+f 722//722 4529//4529 4532//4532
+f 4532//4532 730//730 4462//4462
+f 730//730 4532//4532 729//729
+f 729//729 528//528 4533//4533
+f 4533//4533 730//730 729//729
+f 4533//4533 731//731 730//730
+f 733//733 2035//2035 730//730
+f 731//731 732//732 733//733
+f 732//732 4535//4535 733//733
+f 531//531 733//733 4535//4535
+f 530//530 531//531 4535//4535
+f 531//531 734//734 735//735
+f 736//736 543//543 734//734
+f 544//544 735//735 734//734
+f 4451//4451 734//734 531//531
+f 621//621 637//637 4519//4519
+f 724//724 4519//4519 4518//4518
+f 791//791 737//737 745//745
+f 742//742 747//747 743//743
+f 720//720 738//738 717//717
+f 4513//4513 702//702 705//705
+f 739//739 751//751 740//740
+f 739//739 702//702 751//751
+f 738//738 705//705 717//717
+f 704//704 740//740 744//744
+f 704//704 739//739 740//740
+f 721//721 704//704 744//744
+f 831//831 741//741 850//850
+f 742//742 743//743 744//744
+f 743//743 721//721 744//744
+f 743//743 746//746 737//737
+f 737//737 746//746 745//745
+f 4512//4512 753//753 759//759
+f 597//597 4512//4512 702//702
+f 4512//4512 751//751 702//702
+f 743//743 747//747 746//746
+f 744//744 750//750 742//742
+f 742//742 749//749 747//747
+f 742//742 748//748 749//749
+f 744//744 740//740 750//750
+f 742//742 750//750 748//748
+f 740//740 751//751 752//752
+f 750//750 740//740 752//752
+f 751//751 4512//4512 759//759
+f 751//751 759//759 752//752
+f 4465//4465 859//859 858//858
+f 753//753 761//761 759//759
+f 755//755 754//754 2033//2033
+f 754//754 755//755 753//753
+f 554//554 859//859 552//552
+f 554//554 756//756 859//859
+f 555//555 756//756 554//554
+f 555//555 834//834 756//756
+f 753//753 4512//4512 594//594
+f 834//834 555//555 1768//1768
+f 748//748 750//750 757//757
+f 749//749 748//748 758//758
+f 748//748 826//826 758//758
+f 748//748 757//757 826//826
+f 759//759 761//761 760//760
+f 753//753 755//755 761//761
+f 752//752 760//760 762//762
+f 752//752 759//759 760//760
+f 757//757 750//750 762//762
+f 750//750 752//752 762//762
+f 670//670 668//668 780//780
+f 649//649 648//648 779//779
+f 784//784 670//670 763//763
+f 670//670 780//780 763//763
+f 784//784 763//763 785//785
+f 770//770 786//786 764//764
+f 773//773 777//777 804//804
+f 763//763 780//780 765//765
+f 768//768 765//765 766//766
+f 763//763 765//765 768//768
+f 767//767 766//766 771//771
+f 766//766 765//765 775//775
+f 766//766 775//775 783//783
+f 771//771 766//766 783//783
+f 771//771 783//783 777//777
+f 771//771 777//777 773//773
+f 804//804 777//777 810//810
+f 793//793 804//804 805//805
+f 793//793 805//805 787//787
+f 785//785 763//763 768//768
+f 785//785 768//768 769//769
+f 764//764 768//768 767//767
+f 768//768 764//764 769//769
+f 770//770 767//767 772//772
+f 770//770 764//764 767//767
+f 768//768 766//766 767//767
+f 767//767 771//771 772//772
+f 771//771 773//773 772//772
+f 772//772 773//773 774//774
+f 773//773 804//804 793//793
+f 773//773 793//793 774//774
+f 798//798 778//778 749//749
+f 758//758 798//798 749//749
+f 749//749 776//776 747//747
+f 778//778 776//776 749//749
+f 648//648 790//790 782//782
+f 779//779 782//782 781//781
+f 648//648 782//782 779//779
+f 775//775 779//779 781//781
+f 776//776 775//775 781//781
+f 783//783 775//775 776//776
+f 778//778 783//783 776//776
+f 798//798 810//810 778//778
+f 810//810 777//777 778//778
+f 649//649 779//779 780//780
+f 775//775 765//765 779//779
+f 765//765 780//780 779//779
+f 747//747 776//776 781//781
+f 746//746 747//747 781//781
+f 782//782 746//746 781//781
+f 790//790 745//745 782//782
+f 745//745 746//746 782//782
+f 777//777 783//783 778//778
+f 437//437 784//784 785//785
+f 370//370 360//360 769//769
+f 786//786 370//370 764//764
+f 764//764 370//370 769//769
+f 360//360 785//785 769//769
+f 371//371 787//787 788//788
+f 371//371 796//796 792//792
+f 648//648 789//789 790//790
+f 790//790 691//691 745//745
+f 691//691 791//791 745//745
+f 797//797 371//371 788//788
+f 360//360 437//437 785//785
+f 787//787 371//371 792//792
+f 793//793 787//787 792//792
+f 772//772 774//774 794//794
+f 794//794 774//774 792//792
+f 774//774 793//793 792//792
+f 795//795 770//770 794//794
+f 795//795 786//786 770//770
+f 770//770 772//772 794//794
+f 668//668 649//649 780//780
+f 796//796 795//795 794//794
+f 796//796 794//794 792//792
+f 787//787 805//805 788//788
+f 355//355 797//797 788//788
+f 798//798 813//813 811//811
+f 758//758 813//813 798//798
+f 355//355 788//788 800//800
+f 799//799 355//355 800//800
+f 799//799 800//800 801//801
+f 330//330 799//799 801//801
+f 801//801 2006//2006 802//802
+f 330//330 801//801 802//802
+f 829//829 2011//2011 811//811
+f 803//803 812//812 2003//2003
+f 2011//2011 812//812 811//811
+f 806//806 2005//2005 2006//2006
+f 808//808 804//804 807//807
+f 800//800 805//805 808//808
+f 805//805 804//804 808//808
+f 788//788 805//805 800//800
+f 2011//2011 829//829 2008//2008
+f 2003//2003 812//812 2011//2011
+f 807//807 812//812 806//806
+f 811//811 812//812 807//807
+f 801//801 808//808 806//806
+f 808//808 807//807 806//806
+f 801//801 800//800 808//808
+f 809//809 802//802 2006//2006
+f 809//809 2006//2006 1999//1999
+f 810//810 798//798 811//811
+f 2005//2005 806//806 803//803
+f 810//810 811//811 807//807
+f 804//804 810//810 807//807
+f 803//803 2002//2002 2013//2013
+f 803//803 2003//2003 2002//2002
+f 812//812 803//803 806//806
+f 2014//2014 1999//1999 2005//2005
+f 2006//2006 801//801 806//806
+f 829//829 811//811 813//813
+f 820//820 819//819 814//814
+f 827//827 831//831 815//815
+f 815//815 1986//1986 821//821
+f 814//814 827//827 815//815
+f 814//814 815//815 821//821
+f 831//831 832//832 815//815
+f 815//815 832//832 828//828
+f 1986//1986 815//815 828//828
+f 814//814 819//819 823//823
+f 825//825 824//824 817//817
+f 816//816 830//830 817//817
+f 824//824 822//822 816//816
+f 817//817 824//824 816//816
+f 830//830 816//816 818//818
+f 823//823 819//819 822//822
+f 819//819 820//820 818//818
+f 822//822 819//819 816//816
+f 816//816 819//819 818//818
+f 827//827 814//814 823//823
+f 820//820 814//814 821//821
+f 741//741 831//831 2033//2033
+f 755//755 827//827 823//823
+f 755//755 2033//2033 827//827
+f 761//761 823//823 822//822
+f 761//761 755//755 823//823
+f 760//760 761//761 822//822
+f 824//824 762//762 822//822
+f 762//762 760//760 822//822
+f 825//825 757//757 824//824
+f 757//757 762//762 824//824
+f 813//813 826//826 825//825
+f 826//826 757//757 825//825
+f 758//758 826//826 813//813
+f 827//827 2033//2033 831//831
+f 1986//1986 828//828 846//846
+f 829//829 813//813 825//825
+f 829//829 825//825 817//817
+f 2008//2008 829//829 817//817
+f 2009//2009 817//817 830//830
+f 2009//2009 2008//2008 817//817
+f 831//831 850//850 832//832
+f 840//840 839//839 836//836
+f 845//845 836//836 837//837
+f 840//840 835//835 833//833
+f 558//558 862//862 834//834
+f 840//840 836//836 835//835
+f 836//836 845//845 835//835
+f 839//839 848//848 838//838
+f 837//837 838//838 847//847
+f 836//836 839//839 838//838
+f 837//837 836//836 838//838
+f 4509//4509 840//840 844//844
+f 848//848 839//839 4507//4507
+f 840//840 833//833 844//844
+f 839//839 840//840 4509//4509
+f 850//850 741//741 593//593
+f 4458//4458 4509//4509 4510//4510
+f 839//839 4509//4509 4507//4507
+f 849//849 841//841 842//842
+f 841//841 843//843 842//842
+f 843//843 841//841 4503//4503
+f 843//843 4503//4503 1967//1967
+f 833//833 832//832 844//844
+f 828//828 832//832 833//833
+f 835//835 828//828 833//833
+f 845//845 846//846 835//835
+f 846//846 828//828 835//835
+f 843//843 847//847 842//842
+f 838//838 848//848 842//842
+f 847//847 838//838 842//842
+f 848//848 849//849 842//842
+f 832//832 850//850 844//844
+f 849//849 4505//4505 841//841
+f 851//851 4498//4498 4499//4499
+f 1967//1967 854//854 1934//1934
+f 4499//4499 854//854 4503//4503
+f 4497//4497 1920//1920 856//856
+f 4503//4503 841//841 581//581
+f 1933//1933 852//852 864//864
+f 852//852 542//542 864//864
+f 852//852 1933//1933 853//853
+f 4496//4496 542//542 852//852
+f 583//583 849//849 848//848
+f 582//582 4505//4505 849//849
+f 1967//1967 4503//4503 854//854
+f 855//855 1931//1931 857//857
+f 4498//4498 4497//4497 857//857
+f 854//854 1931//1931 1934//1934
+f 4499//4499 4498//4498 857//857
+f 1931//1931 854//854 857//857
+f 854//854 4499//4499 857//857
+f 853//853 4497//4497 852//852
+f 856//856 852//852 4497//4497
+f 853//853 855//855 4497//4497
+f 856//856 4496//4496 852//852
+f 855//855 857//857 4497//4497
+f 4463//4463 550//550 540//540
+f 858//858 868//868 867//867
+f 859//859 868//868 858//858
+f 859//859 860//860 868//868
+f 756//756 860//860 859//859
+f 756//756 861//861 860//860
+f 834//834 861//861 756//756
+f 834//834 862//862 861//861
+f 559//559 863//863 862//862
+f 560//560 863//863 559//559
+f 4491//4491 542//542 575//575
+f 4496//4496 578//578 865//865
+f 562//562 1669//1669 560//560
+f 562//562 564//564 1669//1669
+f 864//864 542//542 4491//4491
+f 895//895 864//864 572//572
+f 4491//4491 572//572 864//864
+f 864//864 889//889 1933//1933
+f 864//864 895//895 889//889
+f 865//865 866//866 4492//4492
+f 841//841 4505//4505 581//581
+f 572//572 887//887 895//895
+f 550//550 1666//1666 4460//4460
+f 867//867 1666//1666 550//550
+f 867//867 1674//1674 1666//1666
+f 868//868 1674//1674 867//867
+f 868//868 1667//1667 1674//1674
+f 860//860 1667//1667 868//868
+f 860//860 1675//1675 1667//1667
+f 861//861 1675//1675 860//860
+f 861//861 869//869 1675//1675
+f 886//886 875//875 877//877
+f 870//870 886//886 877//877
+f 871//871 870//870 877//877
+f 870//870 871//871 872//872
+f 889//889 888//888 873//873
+f 878//878 871//871 877//877
+f 877//877 875//875 874//874
+f 874//874 875//875 876//876
+f 883//883 876//876 875//875
+f 883//883 896//896 876//876
+f 879//879 880//880 900//900
+f 896//896 879//879 900//900
+f 877//877 874//874 878//878
+f 879//879 883//883 882//882
+f 883//883 879//879 896//896
+f 873//873 879//879 882//882
+f 879//879 873//873 880//880
+f 873//873 888//888 880//880
+f 880//880 888//888 881//881
+f 889//889 873//873 890//890
+f 1947//1947 882//882 883//883
+f 882//882 1947//1947 890//890
+f 1947//1947 884//884 1946//1946
+f 883//883 884//884 1947//1947
+f 886//886 885//885 884//884
+f 885//885 1946//1946 884//884
+f 886//886 1942//1942 885//885
+f 890//890 873//873 882//882
+f 883//883 875//875 884//884
+f 884//884 875//875 886//886
+f 870//870 1942//1942 886//886
+f 870//870 872//872 1941//1941
+f 870//870 1941//1941 1942//1942
+f 871//871 925//925 872//872
+f 887//887 881//881 895//895
+f 889//889 895//895 888//888
+f 872//872 189//189 1941//1941
+f 1933//1933 889//889 890//890
+f 892//892 1721//1721 894//894
+f 891//891 1715//1715 892//892
+f 892//892 1715//1715 1721//1721
+f 1715//1715 891//891 4486//4486
+f 4489//4489 893//893 899//899
+f 862//862 869//869 861//861
+f 863//863 869//869 862//862
+f 899//899 903//903 902//902
+f 899//899 893//893 903//903
+f 1720//1720 1702//1702 897//897
+f 1720//1720 897//897 894//894
+f 894//894 1721//1721 1720//1720
+f 887//887 899//899 881//881
+f 895//895 881//881 888//888
+f 900//900 902//902 901//901
+f 892//892 896//896 891//891
+f 896//896 900//900 891//891
+f 876//876 896//896 892//892
+f 876//876 892//892 894//894
+f 874//874 876//876 894//894
+f 874//874 894//894 897//897
+f 874//874 897//897 898//898
+f 878//878 874//874 898//898
+f 899//899 902//902 881//881
+f 900//900 880//880 902//902
+f 880//880 881//881 902//902
+f 4489//4489 899//899 887//887
+f 893//893 1688//1688 903//903
+f 4486//4486 891//891 901//901
+f 891//891 900//900 901//901
+f 902//902 903//903 901//901
+f 1669//1669 863//863 560//560
+f 904//904 201//201 905//905
+f 904//904 905//905 928//928
+f 931//931 911//911 920//920
+f 931//931 920//920 915//915
+f 910//910 913//913 916//916
+f 920//920 910//910 916//916
+f 916//916 913//913 917//917
+f 931//931 906//906 911//911
+f 923//923 907//907 908//908
+f 920//920 911//911 910//910
+f 910//910 927//927 909//909
+f 913//913 909//909 912//912
+f 913//913 910//910 909//909
+f 910//910 911//911 927//927
+f 907//907 912//912 908//908
+f 913//913 912//912 907//907
+f 925//925 923//923 908//908
+f 920//920 916//916 914//914
+f 914//914 1898//1898 915//915
+f 916//916 917//917 1869//1869
+f 914//914 916//916 1869//1869
+f 921//921 919//919 922//922
+f 921//921 922//922 926//926
+f 1869//1869 917//917 1885//1885
+f 1884//1884 917//917 918//918
+f 1885//1885 917//917 1884//1884
+f 919//919 918//918 922//922
+f 1884//1884 918//918 919//919
+f 920//920 914//914 915//915
+f 1882//1882 921//921 926//926
+f 913//913 907//907 918//918
+f 917//917 913//913 918//918
+f 918//918 907//907 922//922
+f 907//907 923//923 922//922
+f 922//922 923//923 926//926
+f 923//923 878//878 926//926
+f 946//946 200//200 906//906
+f 906//906 200//200 924//924
+f 932//932 176//176 930//930
+f 190//190 929//929 905//905
+f 201//201 190//190 905//905
+f 872//872 925//925 929//929
+f 898//898 897//897 1882//1882
+f 878//878 898//898 926//926
+f 898//898 1882//1882 926//926
+f 871//871 878//878 923//923
+f 871//871 923//923 925//925
+f 190//190 189//189 929//929
+f 189//189 872//872 929//929
+f 897//897 1702//1702 1882//1882
+f 927//927 911//911 924//924
+f 928//928 909//909 932//932
+f 909//909 927//927 932//932
+f 912//912 909//909 928//928
+f 928//928 905//905 912//912
+f 912//912 905//905 908//908
+f 929//929 908//908 905//905
+f 925//925 908//908 929//929
+f 930//930 904//904 928//928
+f 906//906 931//931 936//936
+f 927//927 924//924 932//932
+f 930//930 928//928 932//932
+f 946//946 906//906 936//936
+f 911//911 906//906 924//924
+f 934//934 933//933 940//940
+f 933//933 934//934 935//935
+f 4820//4820 943//943 4821//4821
+f 935//935 934//934 944//944
+f 934//934 941//941 4821//4821
+f 933//933 947//947 936//936
+f 936//936 951//951 933//933
+f 933//933 935//935 947//947
+f 935//935 939//939 937//937
+f 938//938 942//942 944//944
+f 935//935 944//944 939//939
+f 934//934 940//940 941//941
+f 947//947 937//937 963//963
+f 933//933 951//951 940//940
+f 947//947 935//935 937//937
+f 4877//4877 942//942 938//938
+f 4821//4821 943//943 938//938
+f 4821//4821 944//944 934//934
+f 4821//4821 938//938 944//944
+f 941//941 4827//4827 4862//4862
+f 4821//4821 941//941 4862//4862
+f 941//941 940//940 945//945
+f 940//940 951//951 1904//1904
+f 945//945 940//940 1899//1899
+f 946//946 936//936 947//947
+f 946//946 947//947 963//963
+f 940//940 1904//1904 1899//1899
+f 948//948 3278//3278 950//950
+f 944//944 942//942 939//939
+f 4826//4826 949//949 4876//4876
+f 949//949 4826//4826 950//950
+f 4886//4886 949//949 3278//3278
+f 3278//3278 949//949 950//950
+f 936//936 931//931 951//951
+f 915//915 1898//1898 1904//1904
+f 951//951 915//915 1904//1904
+f 931//931 915//915 951//951
+f 949//949 953//953 4876//4876
+f 3278//3278 4884//4884 4886//4886
+f 953//953 4886//4886 952//952
+f 949//949 4886//4886 953//953
+f 4822//4822 952//952 962//962
+f 4822//4822 953//953 952//952
+f 942//942 4822//4822 962//962
+f 939//939 962//962 957//957
+f 939//939 942//942 962//962
+f 957//957 954//954 956//956
+f 954//954 960//960 968//968
+f 966//966 955//955 956//956
+f 958//958 969//969 957//957
+f 956//956 968//968 966//966
+f 956//956 958//958 957//957
+f 956//956 954//954 968//968
+f 959//959 970//970 961//961
+f 952//952 959//959 961//961
+f 4393//4393 970//970 959//959
+f 4861//4861 959//959 4886//4886
+f 4886//4886 959//959 952//952
+f 961//961 960//960 954//954
+f 962//962 961//961 954//954
+f 961//961 970//970 960//960
+f 962//962 952//952 961//961
+f 957//957 962//962 954//954
+f 960//960 248//248 968//968
+f 963//963 969//969 964//964
+f 198//198 946//946 963//963
+f 198//198 963//963 964//964
+f 958//958 964//964 969//969
+f 191//191 958//958 955//955
+f 955//955 958//958 956//956
+f 965//965 955//955 966//966
+f 237//237 965//965 966//966
+f 967//967 237//237 966//966
+f 968//968 967//967 966//966
+f 248//248 967//967 968//968
+f 937//937 957//957 969//969
+f 937//937 939//939 957//957
+f 963//963 937//937 969//969
+f 254//254 970//970 4853//4853
+f 971//971 248//248 960//960
+f 254//254 971//971 970//970
+f 971//971 960//960 970//970
+f 2861//2861 973//973 972//972
+f 998//998 2860//2860 999//999
+f 993//993 973//973 994//994
+f 998//998 994//994 2860//2860
+f 973//973 993//993 972//972
+f 994//994 973//973 2860//2860
+f 998//998 290//290 994//994
+f 974//974 2865//2865 982//982
+f 2865//2865 2829//2829 988//988
+f 975//975 976//976 974//974
+f 2845//2845 2865//2865 974//974
+f 974//974 976//976 2845//2845
+f 980//980 977//977 979//979
+f 987//987 978//978 977//977
+f 977//977 978//978 295//295
+f 976//976 975//975 987//987
+f 978//978 987//987 975//975
+f 979//979 981//981 980//980
+f 2875//2875 981//981 2874//2874
+f 980//980 981//981 2875//2875
+f 2865//2865 988//988 982//982
+f 2845//2845 2832//2832 2865//2865
+f 988//988 2829//2829 2864//2864
+f 982//982 988//988 985//985
+f 292//292 975//975 983//983
+f 975//975 974//974 983//983
+f 978//978 975//975 292//292
+f 984//984 977//977 295//295
+f 984//984 4437//4437 979//979
+f 977//977 984//984 979//979
+f 983//983 974//974 982//982
+f 972//972 993//993 985//985
+f 993//993 296//296 985//985
+f 296//296 982//982 985//985
+f 296//296 983//983 982//982
+f 2863//2863 972//972 985//985
+f 986//986 2871//2871 2875//2875
+f 980//980 987//987 977//977
+f 2863//2863 985//985 988//988
+f 3817//3817 2444//2444 989//989
+f 1095//1095 2819//2819 990//990
+f 2886//2886 976//976 987//987
+f 2885//2885 2845//2845 976//976
+f 308//308 305//305 991//991
+f 308//308 991//991 1011//1011
+f 305//305 992//992 991//991
+f 305//305 290//290 992//992
+f 995//995 994//994 290//290
+f 290//290 998//998 992//992
+f 993//993 994//994 995//995
+f 1000//1000 996//996 2848//2848
+f 1016//1016 991//991 997//997
+f 991//991 992//992 997//997
+f 1016//1016 997//997 1001//1001
+f 992//992 996//996 997//997
+f 1011//1011 991//991 1016//1016
+f 996//996 2858//2858 2848//2848
+f 992//992 998//998 996//996
+f 998//998 999//999 996//996
+f 1000//1000 1002//1002 1017//1017
+f 1000//1000 2848//2848 1002//1002
+f 997//997 1000//1000 1001//1001
+f 996//996 1000//1000 997//997
+f 1001//1001 1000//1000 1017//1017
+f 1002//1002 2850//2850 1007//1007
+f 1003//1003 1004//1004 1012//1012
+f 1012//1012 1004//1004 1013//1013
+f 1015//1015 1019//1019 1033//1033
+f 1033//1033 1019//1019 1005//1005
+f 1005//1005 1013//1013 1006//1006
+f 1007//1007 1005//1005 1019//1019
+f 1015//1015 1031//1031 1008//1008
+f 342//342 308//308 1011//1011
+f 1017//1017 1002//1002 1007//1007
+f 1008//1008 1009//1009 1020//1020
+f 1009//1009 1010//1010 1020//1020
+f 1010//1010 1011//1011 1020//1020
+f 1010//1010 342//342 1011//1011
+f 359//359 1009//1009 1008//1008
+f 1031//1031 1046//1046 1008//1008
+f 1046//1046 359//359 1008//1008
+f 1012//1012 1013//1013 1005//1005
+f 1012//1012 1005//1005 1007//1007
+f 1007//1007 2850//2850 1012//1012
+f 1031//1031 1015//1015 1014//1014
+f 1015//1015 1008//1008 1018//1018
+f 1011//1011 1016//1016 1020//1020
+f 1016//1016 1001//1001 1018//1018
+f 1020//1020 1016//1016 1018//1018
+f 1001//1001 1017//1017 1019//1019
+f 1001//1001 1019//1019 1018//1018
+f 1017//1017 1007//1007 1019//1019
+f 1013//1013 1004//1004 1034//1034
+f 1034//1034 1004//1004 1021//1021
+f 1015//1015 1033//1033 1014//1014
+f 1019//1019 1015//1015 1018//1018
+f 1008//1008 1020//1020 1018//1018
+f 1028//1028 1034//1034 1021//1021
+f 1060//1060 1024//1024 1038//1038
+f 1038//1038 1041//1041 1061//1061
+f 1024//1024 1022//1022 1023//1023
+f 1038//1038 1024//1024 1023//1023
+f 1023//1023 1022//1022 1039//1039
+f 1059//1059 1026//1026 1027//1027
+f 1026//1026 1025//1025 1908//1908
+f 1906//1906 4764//4764 1025//1025
+f 1026//1026 1908//1908 1027//1027
+f 1025//1025 4764//4764 1908//1908
+f 1061//1061 1041//1041 1026//1026
+f 1025//1025 1026//1026 1041//1041
+f 1060//1060 1038//1038 1061//1061
+f 1028//1028 1030//1030 1034//1034
+f 4673//4673 1029//1029 4582//4582
+f 1059//1059 1027//1027 1029//1029
+f 1026//1026 1059//1059 1061//1061
+f 1040//1040 1036//1036 1006//1006
+f 1036//1036 1037//1037 1032//1032
+f 1030//1030 1042//1042 1044//1044
+f 1031//1031 1014//1014 1032//1032
+f 1014//1014 1033//1033 1036//1036
+f 1014//1014 1036//1036 1032//1032
+f 1005//1005 1006//1006 1033//1033
+f 1036//1036 1033//1033 1006//1006
+f 1013//1013 1034//1034 1044//1044
+f 1006//1006 1013//1013 1044//1044
+f 1030//1030 1044//1044 1034//1034
+f 1906//1906 2813//2813 1035//1035
+f 1042//1042 1030//1030 2813//2813
+f 1036//1036 1040//1040 1037//1037
+f 1038//1038 1023//1023 1043//1043
+f 1041//1041 1038//1038 1043//1043
+f 1023//1023 1039//1039 1040//1040
+f 1023//1023 1040//1040 1043//1043
+f 1040//1040 1039//1039 1037//1037
+f 1025//1025 1041//1041 1042//1042
+f 1025//1025 1042//1042 1906//1906
+f 2813//2813 1906//1906 1042//1042
+f 1044//1044 1041//1041 1043//1043
+f 1042//1042 1041//1041 1044//1044
+f 1040//1040 1006//1006 1043//1043
+f 1006//1006 1044//1044 1043//1043
+f 1045//1045 1047//1047 1037//1037
+f 1039//1039 1045//1045 1037//1037
+f 1048//1048 1046//1046 1031//1031
+f 387//387 1045//1045 1039//1039
+f 1047//1047 1048//1048 1032//1032
+f 1022//1022 387//387 1039//1039
+f 1048//1048 1031//1031 1032//1032
+f 1037//1037 1047//1047 1032//1032
+f 1055//1055 1054//1054 1049//1049
+f 1050//1050 1051//1051 1054//1054
+f 1057//1057 1058//1058 1052//1052
+f 1050//1050 1052//1052 1053//1053
+f 1057//1057 1052//1052 1050//1050
+f 1060//1060 1050//1050 1054//1054
+f 1055//1055 1049//1049 1070//1070
+f 4584//4584 4672//4672 4671//4671
+f 3506//3506 1064//1064 4670//4670
+f 2635//2635 1062//1062 1056//1056
+f 1057//1057 1061//1061 1058//1058
+f 1059//1059 1058//1058 1061//1061
+f 1057//1057 1050//1050 1060//1060
+f 1060//1060 1061//1061 1057//1057
+f 1024//1024 1060//1060 1054//1054
+f 1022//1022 1024//1024 1055//1055
+f 1024//1024 1054//1054 1055//1055
+f 1053//1053 1063//1063 4380//4380
+f 1052//1052 1058//1058 1063//1063
+f 1053//1053 1052//1052 1063//1063
+f 4670//4670 1064//1064 4380//4380
+f 2635//2635 1064//1064 1062//1062
+f 1064//1064 3506//3506 1062//1062
+f 1063//1063 4670//4670 4380//4380
+f 1063//1063 4673//4673 4670//4670
+f 1054//1054 1051//1051 1049//1049
+f 3006//3006 1049//1049 1051//1051
+f 1050//1050 1053//1053 1051//1051
+f 1064//1064 2635//2635 2402//2402
+f 2402//2402 4334//4334 4380//4380
+f 1064//1064 2402//2402 4380//4380
+f 1053//1053 4380//4380 1065//1065
+f 1051//1051 1053//1053 1065//1065
+f 2402//2402 2635//2635 2634//2634
+f 1066//1066 1067//1067 2644//2644
+f 1066//1066 2644//2644 2612//2612
+f 1069//1069 1067//1067 1068//1068
+f 4327//4327 1069//1069 1068//1068
+f 1071//1071 1070//1070 1068//1068
+f 1049//1049 4327//4327 1068//1068
+f 1070//1070 1049//1049 1068//1068
+f 1067//1067 1071//1071 1068//1068
+f 1072//1072 4035//4035 1073//1073
+f 1074//1074 1957//1957 1072//1072
+f 4109//4109 1075//1075 4304//4304
+f 1076//1076 1078//1078 1077//1077
+f 321//321 3882//3882 4546//4546
+f 1073//1073 1077//1077 1078//1078
+f 1073//1073 4035//4035 2267//2267
+f 1077//1077 1073//1073 2267//2267
+f 1079//1079 1082//1082 1084//1084
+f 1082//1082 1089//1089 1080//1080
+f 1089//1089 1081//1081 1080//1080
+f 1085//1085 1115//1115 1086//1086
+f 1082//1082 1079//1079 1089//1089
+f 1102//1102 1085//1085 1356//1356
+f 1082//1082 1083//1083 4170//4170
+f 1083//1083 4347//4347 4170//4170
+f 1082//1082 1080//1080 1083//1083
+f 1084//1084 1082//1082 4170//4170
+f 1115//1115 1093//1093 1086//1086
+f 1085//1085 1086//1086 1081//1081
+f 1080//1080 1081//1081 1086//1086
+f 1081//1081 1088//1088 1085//1085
+f 1094//1094 1355//1355 1087//1087
+f 1088//1088 1087//1087 1355//1355
+f 1081//1081 1089//1089 1087//1087
+f 1081//1081 1087//1087 1088//1088
+f 1089//1089 1079//1079 1087//1087
+f 1079//1079 1094//1094 1087//1087
+f 1085//1085 1088//1088 1356//1356
+f 3848//3848 1090//1090 1092//1092
+f 3848//3848 1091//1091 1602//1602
+f 1091//1091 1114//1114 1577//1577
+f 1114//1114 1105//1105 1577//1577
+f 1091//1091 1092//1092 1114//1114
+f 1114//1114 1093//1093 1116//1116
+f 1084//1084 1076//1076 1079//1079
+f 1076//1076 1077//1077 1094//1094
+f 1076//1076 1094//1094 1079//1079
+f 1083//1083 1096//1096 1090//1090
+f 1095//1095 3039//3039 1345//1345
+f 1093//1093 1092//1092 1096//1096
+f 1093//1093 1096//1096 1086//1086
+f 1080//1080 1086//1086 1096//1096
+f 1080//1080 1096//1096 1083//1083
+f 1092//1092 1090//1090 1096//1096
+f 3848//3848 1092//1092 1091//1091
+f 1114//1114 1092//1092 1093//1093
+f 1106//1106 1097//1097 1100//1100
+f 1584//1584 1098//1098 1100//1100
+f 1106//1106 1100//1100 1098//1098
+f 1130//1130 1119//1119 1103//1103
+f 1100//1100 1099//1099 1578//1578
+f 1100//1100 1578//1578 1584//1584
+f 1100//1100 1101//1101 1099//1099
+f 1100//1100 1097//1097 1101//1101
+f 1103//1103 1350//1350 1130//1130
+f 1101//1101 1097//1097 1117//1117
+f 1106//1106 1104//1104 1097//1097
+f 1104//1104 1103//1103 1118//1118
+f 1104//1104 1118//1118 1097//1097
+f 1104//1104 1102//1102 1356//1356
+f 1103//1103 1356//1356 1350//1350
+f 1103//1103 1104//1104 1356//1356
+f 1107//1107 1108//1108 1105//1105
+f 1116//1116 1107//1107 1105//1105
+f 1107//1107 1116//1116 1102//1102
+f 1106//1106 1107//1107 1102//1102
+f 1104//1104 1106//1106 1102//1102
+f 1106//1106 1098//1098 1107//1107
+f 1105//1105 1108//1108 1567//1567
+f 1108//1108 1107//1107 1098//1098
+f 1109//1109 1113//1113 1110//1110
+f 1109//1109 1110//1110 1528//1528
+f 1109//1109 1528//1528 1570//1570
+f 1128//1128 1126//1126 1111//1111
+f 1111//1111 1126//1126 1133//1133
+f 1111//1111 1133//1133 1112//1112
+f 1111//1111 1112//1112 1113//1113
+f 1110//1110 1113//1113 1112//1112
+f 1114//1114 1116//1116 1105//1105
+f 1093//1093 1115//1115 1116//1116
+f 1085//1085 1102//1102 1115//1115
+f 1105//1105 1567//1567 1577//1577
+f 1116//1116 1115//1115 1102//1102
+f 1099//1099 1101//1101 1109//1109
+f 1099//1099 1109//1109 1570//1570
+f 1109//1109 1101//1101 1113//1113
+f 1101//1101 1117//1117 1113//1113
+f 1117//1117 1111//1111 1113//1113
+f 1097//1097 1118//1118 1117//1117
+f 1578//1578 1099//1099 1569//1569
+f 1099//1099 1570//1570 1569//1569
+f 1130//1130 1131//1131 1119//1119
+f 1117//1117 1128//1128 1111//1111
+f 1118//1118 1119//1119 1128//1128
+f 1128//1128 1117//1117 1118//1118
+f 1103//1103 1119//1119 1118//1118
+f 1123//1123 1321//1321 1320//1320
+f 1124//1124 1268//1268 1266//1266
+f 1120//1120 1124//1124 1148//1148
+f 1122//1122 1121//1121 1127//1127
+f 1122//1122 1120//1120 1121//1121
+f 1126//1126 1122//1122 1127//1127
+f 1123//1123 1172//1172 1147//1147
+f 1124//1124 1266//1266 1147//1147
+f 1266//1266 1123//1123 1147//1147
+f 1120//1120 1268//1268 1124//1124
+f 1122//1122 1125//1125 1120//1120
+f 1125//1125 1268//1268 1120//1120
+f 1133//1133 1127//1127 1132//1132
+f 1133//1133 1126//1126 1127//1127
+f 1126//1126 1128//1128 1129//1129
+f 1129//1129 1122//1122 1126//1126
+f 1128//1128 1119//1119 1129//1129
+f 1130//1130 1261//1261 1131//1131
+f 1119//1119 1131//1131 1129//1129
+f 1122//1122 1129//1129 1262//1262
+f 1122//1122 1262//1262 1125//1125
+f 1129//1129 1131//1131 1262//1262
+f 1528//1528 1137//1137 1474//1474
+f 1528//1528 1110//1110 1137//1137
+f 1112//1112 1137//1137 1110//1110
+f 1112//1112 1132//1132 1137//1137
+f 1112//1112 1133//1133 1132//1132
+f 1142//1142 1474//1474 1143//1143
+f 1143//1143 1134//1134 1149//1149
+f 1137//1137 1132//1132 1134//1134
+f 1135//1135 1139//1139 1141//1141
+f 1168//1168 1138//1138 1144//1144
+f 1136//1136 1145//1145 1144//1144
+f 1136//1136 1135//1135 1145//1145
+f 1143//1143 1137//1137 1134//1134
+f 1139//1139 1138//1138 1461//1461
+f 1138//1138 1139//1139 1136//1136
+f 1138//1138 1136//1136 1144//1144
+f 1142//1142 1143//1143 1141//1141
+f 1138//1138 1169//1169 1461//1461
+f 1141//1141 1139//1139 1140//1140
+f 1143//1143 1149//1149 1141//1141
+f 1142//1142 1141//1141 1140//1140
+f 1149//1149 1135//1135 1141//1141
+f 1474//1474 1137//1137 1143//1143
+f 1172//1172 1144//1144 1145//1145
+f 1147//1147 1145//1145 1146//1146
+f 1147//1147 1172//1172 1145//1145
+f 1148//1148 1124//1124 1146//1146
+f 1124//1124 1147//1147 1146//1146
+f 1120//1120 1148//1148 1121//1121
+f 1135//1135 1149//1149 1146//1146
+f 1169//1169 1138//1138 1168//1168
+f 1140//1140 1139//1139 1461//1461
+f 1145//1145 1135//1135 1146//1146
+f 1134//1134 1148//1148 1149//1149
+f 1149//1149 1148//1148 1146//1146
+f 1139//1139 1135//1135 1136//1136
+f 1132//1132 1121//1121 1134//1134
+f 1127//1127 1121//1121 1132//1132
+f 1121//1121 1148//1148 1134//1134
+f 1150//1150 1157//1157 1151//1151
+f 1154//1154 1160//1160 1322//1322
+f 1151//1151 1173//1173 1152//1152
+f 1316//1316 1153//1153 1324//1324
+f 1153//1153 1167//1167 1154//1154
+f 1154//1154 1167//1167 1155//1155
+f 1155//1155 1167//1167 1156//1156
+f 1155//1155 1156//1156 1157//1157
+f 1151//1151 1157//1157 1156//1156
+f 1158//1158 1150//1150 1152//1152
+f 1150//1150 1158//1158 1159//1159
+f 1158//1158 1152//1152 1162//1162
+f 1159//1159 1164//1164 1171//1171
+f 1159//1159 1158//1158 1164//1164
+f 1159//1159 1171//1171 1161//1161
+f 1322//1322 1324//1324 1154//1154
+f 1165//1165 1320//1320 1160//1160
+f 1171//1171 1165//1165 1161//1161
+f 1165//1165 1160//1160 1161//1161
+f 1160//1160 1320//1320 1322//1322
+f 1154//1154 1155//1155 1160//1160
+f 1162//1162 1163//1163 1170//1170
+f 1164//1164 1158//1158 1170//1170
+f 1160//1160 1155//1155 1161//1161
+f 1155//1155 1157//1157 1161//1161
+f 1158//1158 1162//1162 1170//1170
+f 1157//1157 1150//1150 1159//1159
+f 1157//1157 1159//1159 1161//1161
+f 1151//1151 1152//1152 1150//1150
+f 1165//1165 1172//1172 1320//1320
+f 1153//1153 1192//1192 1175//1175
+f 1175//1175 1192//1192 1166//1166
+f 1193//1193 1176//1176 1175//1175
+f 1167//1167 1175//1175 1176//1176
+f 1169//1169 1168//1168 1170//1170
+f 1163//1163 1169//1169 1170//1170
+f 1168//1168 1164//1164 1170//1170
+f 1168//1168 1144//1144 1171//1171
+f 1164//1164 1168//1168 1171//1171
+f 1171//1171 1144//1144 1165//1165
+f 1144//1144 1172//1172 1165//1165
+f 1480//1480 1173//1173 1174//1174
+f 1151//1151 1174//1174 1173//1173
+f 1174//1174 1177//1177 1480//1480
+f 1151//1151 1156//1156 1174//1174
+f 1324//1324 1153//1153 1154//1154
+f 1153//1153 1175//1175 1167//1167
+f 1156//1156 1176//1176 1174//1174
+f 1176//1176 1193//1193 1177//1177
+f 1156//1156 1167//1167 1176//1176
+f 1177//1177 1174//1174 1176//1176
+f 1175//1175 1166//1166 1193//1193
+f 1186//1186 1194//1194 1178//1178
+f 1190//1190 1178//1178 1205//1205
+f 1179//1179 1205//1205 1180//1180
+f 1181//1181 1196//1196 1205//1205
+f 1178//1178 1194//1194 1181//1181
+f 1181//1181 1194//1194 1195//1195
+f 1339//1339 1182//1182 1166//1166
+f 1166//1166 1182//1182 1195//1195
+f 1183//1183 1451//1451 1189//1189
+f 1451//1451 1449//1449 1189//1189
+f 1449//1449 1184//1184 1185//1185
+f 1185//1185 1184//1184 1187//1187
+f 1189//1189 1449//1449 1185//1185
+f 1186//1186 1185//1185 1187//1187
+f 1183//1183 1188//1188 1451//1451
+f 1178//1178 1189//1189 1185//1185
+f 1178//1178 1190//1190 1189//1189
+f 1186//1186 1178//1178 1185//1185
+f 1191//1191 1180//1180 1205//1205
+f 1192//1192 1339//1339 1166//1166
+f 1193//1193 1166//1166 1195//1195
+f 1194//1194 1193//1193 1195//1195
+f 1186//1186 1177//1177 1194//1194
+f 1177//1177 1193//1193 1194//1194
+f 1480//1480 1186//1186 1187//1187
+f 1480//1480 1177//1177 1186//1186
+f 1196//1196 1335//1335 1191//1191
+f 1181//1181 1205//1205 1178//1178
+f 1196//1196 1181//1181 1182//1182
+f 1195//1195 1182//1182 1181//1181
+f 1191//1191 1205//1205 1196//1196
+f 1197//1197 1203//1203 1204//1204
+f 4113//4113 1197//1197 4114//4114
+f 1208//1208 1199//1199 1198//1198
+f 1204//1204 1208//1208 1198//1198
+f 1204//1204 1179//1179 1208//1208
+f 1198//1198 1199//1199 4112//4112
+f 1180//1180 1210//1210 1200//1200
+f 4113//4113 1203//1203 1197//1197
+f 1201//1201 1183//1183 1203//1203
+f 1202//1202 1201//1201 1203//1203
+f 4113//4113 1202//1202 1203//1203
+f 1203//1203 1190//1190 1204//1204
+f 1190//1190 1183//1183 1189//1189
+f 1190//1190 1179//1179 1204//1204
+f 1190//1190 1203//1203 1183//1183
+f 1205//1205 1179//1179 1190//1190
+f 1180//1180 1191//1191 1206//1206
+f 1180//1180 1206//1206 1210//1210
+f 1200//1200 1179//1179 1180//1180
+f 1179//1179 1200//1200 1208//1208
+f 1210//1210 1207//1207 1200//1200
+f 1208//1208 1200//1200 2658//2658
+f 1199//1199 1208//1208 2658//2658
+f 1334//1334 1209//1209 1211//1211
+f 1336//1336 2384//2384 1209//1209
+f 1334//1334 1211//1211 1206//1206
+f 2384//2384 4183//4183 1209//1209
+f 3001//3001 2384//2384 1336//1336
+f 1206//1206 1211//1211 4029//4029
+f 1206//1206 4029//4029 1210//1210
+f 1209//1209 4183//4183 1211//1211
+f 1206//1206 1191//1191 1334//1334
+f 4608//4608 1212//1212 4841//4841
+f 1215//1215 1217//1217 1222//1222
+f 1217//1217 1215//1215 1214//1214
+f 1213//1213 1217//1217 1214//1214
+f 1213//1213 1218//1218 1217//1217
+f 1219//1219 1214//1214 1215//1215
+f 1214//1214 1219//1219 2131//2131
+f 1222//1222 2569//2569 1216//1216
+f 1222//1222 1217//1217 2569//2569
+f 1217//1217 1218//1218 2569//2569
+f 4594//4594 2022//2022 1228//1228
+f 1218//1218 4605//4605 990//990
+f 2131//2131 1219//1219 4371//4371
+f 1219//1219 1228//1228 4371//4371
+f 1228//1228 1626//1626 4371//4371
+f 1242//1242 1220//1220 1221//1221
+f 1242//1242 1221//1221 1223//1223
+f 1221//1221 1220//1220 1229//1229
+f 1223//1223 1224//1224 1242//1242
+f 1224//1224 1223//1223 4192//4192
+f 1229//1229 1215//1215 1221//1221
+f 1222//1222 1221//1221 1215//1215
+f 1223//1223 1221//1221 1222//1222
+f 1224//1224 1243//1243 1242//1242
+f 1224//1224 1245//1245 1243//1243
+f 4162//4162 1225//1225 1226//1226
+f 1227//1227 1226//1226 1225//1225
+f 1225//1225 4161//4161 1227//1227
+f 1245//1245 4162//4162 1226//1226
+f 1228//1228 1229//1229 4594//4594
+f 4594//4594 1229//1229 1220//1220
+f 1228//1228 1219//1219 1229//1229
+f 1219//1219 1215//1215 1229//1229
+f 1224//1224 4164//4164 1231//1231
+f 1225//1225 4162//4162 1230//1230
+f 1222//1222 1216//1216 1223//1223
+f 1224//1224 1231//1231 1245//1245
+f 1232//1232 1235//1235 1240//1240
+f 1233//1233 2569//2569 1218//1218
+f 1238//1238 1240//1240 1237//1237
+f 1240//1240 1235//1235 1234//1234
+f 1237//1237 1240//1240 1234//1234
+f 1234//1234 1235//1235 1264//1264
+f 1236//1236 1237//1237 2285//2285
+f 2284//2284 1236//1236 2285//2285
+f 1247//1247 1238//1238 1246//1246
+f 1244//1244 1239//1239 1238//1238
+f 1247//1247 1244//1244 1238//1238
+f 1240//1240 1238//1238 1239//1239
+f 1232//1232 1240//1240 1239//1239
+f 3619//3619 2248//2248 1241//1241
+f 1246//1246 1236//1236 1241//1241
+f 1236//1236 3619//3619 1241//1241
+f 1238//1238 1237//1237 1246//1246
+f 1237//1237 1236//1236 1246//1246
+f 1242//1242 1243//1243 1241//1241
+f 2248//2248 1242//1242 1241//1241
+f 1220//1220 1242//1242 2248//2248
+f 1247//1247 1226//1226 1244//1244
+f 1245//1245 1226//1226 1247//1247
+f 1243//1243 1247//1247 1246//1246
+f 1243//1243 1245//1245 1247//1247
+f 1241//1241 1243//1243 1246//1246
+f 1077//1077 2267//2267 2287//2287
+f 2285//2285 1237//1237 1248//1248
+f 1360//1360 2285//2285 1248//1248
+f 1360//1360 1359//1359 2284//2284
+f 1357//1357 1234//1234 1264//1264
+f 1244//1244 1272//1272 1239//1239
+f 1227//1227 1244//1244 1226//1226
+f 1234//1234 1357//1357 1248//1248
+f 1237//1237 1234//1234 1248//1248
+f 1264//1264 1258//1258 1249//1249
+f 1252//1252 1250//1250 1281//1281
+f 1281//1281 1250//1250 1251//1251
+f 1263//1263 1250//1250 1252//1252
+f 1249//1249 1256//1256 1255//1255
+f 1259//1259 1249//1249 1255//1255
+f 1258//1258 1256//1256 1249//1249
+f 1251//1251 1250//1250 1278//1278
+f 1278//1278 1253//1253 1254//1254
+f 1253//1253 1279//1279 1283//1283
+f 1254//1254 1253//1253 1283//1283
+f 1250//1250 1255//1255 1278//1278
+f 1278//1278 1255//1255 1253//1253
+f 1255//1255 1256//1256 1253//1253
+f 1253//1253 1256//1256 1279//1279
+f 1256//1256 1258//1258 1257//1257
+f 1279//1279 1256//1256 1257//1257
+f 1257//1257 1258//1258 1280//1280
+f 1259//1259 1255//1255 1263//1263
+f 1249//1249 1259//1259 1260//1260
+f 1261//1261 1263//1263 1131//1131
+f 1252//1252 1262//1262 1263//1263
+f 1250//1250 1263//1263 1255//1255
+f 1321//1321 1123//1123 1265//1265
+f 1125//1125 1262//1262 1252//1252
+f 1264//1264 1249//1249 1357//1357
+f 1123//1123 1266//1266 1265//1265
+f 1265//1265 1266//1266 1267//1267
+f 1266//1266 1268//1268 1267//1267
+f 1268//1268 1281//1281 1267//1267
+f 1268//1268 1125//1125 1281//1281
+f 1125//1125 1252//1252 1281//1281
+f 1279//1279 1257//1257 1284//1284
+f 1282//1282 1285//1285 1269//1269
+f 1311//1311 1282//1282 1269//1269
+f 1285//1285 1287//1287 1270//1270
+f 1269//1269 1285//1285 1270//1270
+f 1239//1239 1274//1274 1232//1232
+f 1287//1287 1274//1274 1275//1275
+f 1274//1274 1287//1287 1286//1286
+f 1321//1321 1265//1265 1291//1291
+f 1291//1291 1289//1289 1273//1273
+f 1321//1321 1291//1291 1273//1273
+f 1311//1311 1271//1271 1282//1282
+f 1239//1239 1272//1272 1274//1274
+f 1310//1310 1311//1311 1325//1325
+f 1273//1273 1289//1289 1310//1310
+f 1311//1311 1310//1310 1271//1271
+f 1289//1289 1271//1271 1310//1310
+f 1274//1274 1272//1272 1275//1275
+f 1275//1275 1276//1276 1270//1270
+f 1287//1287 1275//1275 1270//1270
+f 1269//1269 1270//1270 1277//1277
+f 1270//1270 1276//1276 1277//1277
+f 1311//1311 1269//1269 1314//1314
+f 1269//1269 1277//1277 1314//1314
+f 1232//1232 1274//1274 1286//1286
+f 1251//1251 1278//1278 1290//1290
+f 1251//1251 1290//1290 1267//1267
+f 1290//1290 1278//1278 1254//1254
+f 1283//1283 1279//1279 1284//1284
+f 1257//1257 1280//1280 1288//1288
+f 1284//1284 1257//1257 1288//1288
+f 1258//1258 1264//1264 1280//1280
+f 1264//1264 1235//1235 1280//1280
+f 1281//1281 1251//1251 1267//1267
+f 1271//1271 1289//1289 1254//1254
+f 1271//1271 1254//1254 1283//1283
+f 1282//1282 1283//1283 1284//1284
+f 1282//1282 1271//1271 1283//1283
+f 1285//1285 1284//1284 1288//1288
+f 1285//1285 1282//1282 1284//1284
+f 1286//1286 1287//1287 1288//1288
+f 1287//1287 1285//1285 1288//1288
+f 1235//1235 1232//1232 1286//1286
+f 1280//1280 1235//1235 1286//1286
+f 1280//1280 1286//1286 1288//1288
+f 1290//1290 1291//1291 1267//1267
+f 1291//1291 1265//1265 1267//1267
+f 1289//1289 1290//1290 1254//1254
+f 1289//1289 1291//1291 1290//1290
+f 1296//1296 1295//1295 1292//1292
+f 1306//1306 1295//1295 1296//1296
+f 1302//1302 1292//1292 1293//1293
+f 1296//1296 1292//1292 1302//1302
+f 1303//1303 1293//1293 1304//1304
+f 1302//1302 1293//1293 1303//1303
+f 1294//1294 1304//1304 1299//1299
+f 1306//1306 4017//4017 1295//1295
+f 1321//1321 1273//1273 1326//1326
+f 4161//4161 1317//1317 1227//1227
+f 1227//1227 1272//1272 1244//1244
+f 1296//1296 1297//1297 1298//1298
+f 1302//1302 1303//1303 1329//1329
+f 1297//1297 1302//1302 1329//1329
+f 1303//1303 1294//1294 1328//1328
+f 1329//1329 1303//1303 1328//1328
+f 1300//1300 1294//1294 1299//1299
+f 1328//1328 1294//1294 1300//1300
+f 1300//1300 1299//1299 1301//1301
+f 1296//1296 1302//1302 1297//1297
+f 1303//1303 1304//1304 1294//1294
+f 1305//1305 1306//1306 1298//1298
+f 1317//1317 1305//1305 1298//1298
+f 1306//1306 1296//1296 1298//1298
+f 1275//1275 1272//1272 1318//1318
+f 1272//1272 1227//1227 1318//1318
+f 1307//1307 4015//4015 1308//1308
+f 1273//1273 1310//1310 1309//1309
+f 1273//1273 1309//1309 1326//1326
+f 1275//1275 1318//1318 1330//1330
+f 1309//1309 1310//1310 1325//1325
+f 1311//1311 1314//1314 1313//1313
+f 1325//1325 1311//1311 1313//1313
+f 1314//1314 1277//1277 1312//1312
+f 1313//1313 1314//1314 1312//1312
+f 1277//1277 1276//1276 1312//1312
+f 1304//1304 1315//1315 1343//1343
+f 1304//1304 1293//1293 1315//1315
+f 1299//1299 1304//1304 1319//1319
+f 1316//1316 1299//1299 1319//1319
+f 1293//1293 1292//1292 1308//1308
+f 1295//1295 1307//1307 1308//1308
+f 1307//1307 4017//4017 4006//4006
+f 1295//1295 4017//4017 1307//1307
+f 1308//1308 4015//4015 1315//1315
+f 1292//1292 1295//1295 1308//1308
+f 1315//1315 1293//1293 1308//1308
+f 1317//1317 1298//1298 1318//1318
+f 1192//1192 1316//1316 1319//1319
+f 1192//1192 1153//1153 1316//1316
+f 1172//1172 1123//1123 1320//1320
+f 4161//4161 4166//4166 1317//1317
+f 1317//1317 4166//4166 1305//1305
+f 1320//1320 1321//1321 1326//1326
+f 1322//1322 1320//1320 1326//1326
+f 1322//1322 1326//1326 1323//1323
+f 1324//1324 1322//1322 1323//1323
+f 1301//1301 1324//1324 1323//1323
+f 1299//1299 1316//1316 1301//1301
+f 1316//1316 1324//1324 1301//1301
+f 1309//1309 1325//1325 1332//1332
+f 1326//1326 1309//1309 1323//1323
+f 1301//1301 1323//1323 1332//1332
+f 1300//1300 1332//1332 1327//1327
+f 1300//1300 1301//1301 1332//1332
+f 1328//1328 1327//1327 1333//1333
+f 1328//1328 1300//1300 1327//1327
+f 1329//1329 1333//1333 1331//1331
+f 1329//1329 1328//1328 1333//1333
+f 1297//1297 1331//1331 1330//1330
+f 1297//1297 1329//1329 1331//1331
+f 1318//1318 1298//1298 1330//1330
+f 1298//1298 1297//1297 1330//1330
+f 1227//1227 1317//1317 1318//1318
+f 1331//1331 1276//1276 1330//1330
+f 1275//1275 1330//1330 1276//1276
+f 1333//1333 1312//1312 1331//1331
+f 1312//1312 1276//1276 1331//1331
+f 1323//1323 1309//1309 1332//1332
+f 1325//1325 1313//1313 1327//1327
+f 1332//1332 1325//1325 1327//1327
+f 1313//1313 1312//1312 1333//1333
+f 1327//1327 1313//1313 1333//1333
+f 1334//1334 1191//1191 1335//1335
+f 1335//1335 1196//1196 1338//1338
+f 4408//4408 3001//3001 1336//1336
+f 1209//1209 1334//1334 1341//1341
+f 1336//1336 1209//1209 1341//1341
+f 1334//1334 1335//1335 1341//1341
+f 1196//1196 1182//1182 1338//1338
+f 1338//1338 1182//1182 1339//1339
+f 1337//1337 4408//4408 1346//1346
+f 1346//1346 4408//4408 1347//1347
+f 1339//1339 1192//1192 1344//1344
+f 1338//1338 1339//1339 1340//1340
+f 1342//1342 1349//1349 4015//4015
+f 1336//1336 1340//1340 4408//4408
+f 1336//1336 1341//1341 1340//1340
+f 4408//4408 1340//1340 1347//1347
+f 1335//1335 1338//1338 1341//1341
+f 1341//1341 1338//1338 1340//1340
+f 1342//1342 1348//1348 1349//1349
+f 3613//3613 1346//1346 1348//1348
+f 1315//1315 1349//1349 1343//1343
+f 1315//1315 4015//4015 1349//1349
+f 1344//1344 1319//1319 1343//1343
+f 1304//1304 1343//1343 1319//1319
+f 1192//1192 1319//1319 1344//1344
+f 1347//1347 1340//1340 1344//1344
+f 1340//1340 1339//1339 1344//1344
+f 1343//1343 1349//1349 1347//1347
+f 1344//1344 1343//1343 1347//1347
+f 1345//1345 2819//2819 1095//1095
+f 1346//1346 1347//1347 1348//1348
+f 1349//1349 1348//1348 1347//1347
+f 4014//4014 1348//1348 1342//1342
+f 4014//4014 3613//3613 1348//1348
+f 1350//1350 1352//1352 1351//1351
+f 1130//1130 1350//1350 1351//1351
+f 1260//1260 1353//1353 1358//1358
+f 1361//1361 1353//1353 1260//1260
+f 1354//1354 1353//1353 1352//1352
+f 1358//1358 1353//1353 1354//1354
+f 1355//1355 1352//1352 1088//1088
+f 1354//1354 1352//1352 1355//1355
+f 1352//1352 1350//1350 1088//1088
+f 1131//1131 1263//1263 1262//1262
+f 1355//1355 1094//1094 2287//2287
+f 1356//1356 1088//1088 1350//1350
+f 1130//1130 1351//1351 1261//1261
+f 1248//1248 1357//1357 1358//1358
+f 1357//1357 1260//1260 1358//1358
+f 1357//1357 1249//1249 1260//1260
+f 1260//1260 1259//1259 1361//1361
+f 1360//1360 1358//1358 1354//1354
+f 1359//1359 1354//1354 1355//1355
+f 1359//1359 1360//1360 1354//1354
+f 1360//1360 1248//1248 1358//1358
+f 1361//1361 1351//1351 1353//1353
+f 1351//1351 1352//1352 1353//1353
+f 1094//1094 1077//1077 2287//2287
+f 1351//1351 1361//1361 1261//1261
+f 1259//1259 1263//1263 1261//1261
+f 1361//1361 1259//1259 1261//1261
+f 1915//1915 1797//1797 1369//1369
+f 1775//1775 1400//1400 1777//1777
+f 1777//1777 1392//1392 1363//1363
+f 1363//1363 1365//1365 1362//1362
+f 2726//2726 1365//1365 1363//1363
+f 1362//1362 1365//1365 1364//1364
+f 2726//2726 1398//1398 2725//2725
+f 1364//1364 1365//1365 2729//2729
+f 1398//1398 2726//2726 1363//1363
+f 2729//2729 1366//1366 1364//1364
+f 1367//1367 1370//1370 1364//1364
+f 1915//1915 1369//1369 1368//1368
+f 1369//1369 1373//1373 1368//1368
+f 1368//1368 1373//1373 1371//1371
+f 1367//1367 1373//1373 1370//1370
+f 1777//1777 1363//1363 1362//1362
+f 1367//1367 1364//1364 1366//1366
+f 1362//1362 1364//1364 1370//1370
+f 1371//1371 1367//1367 1372//1372
+f 1367//1367 1371//1371 1373//1373
+f 1367//1367 1366//1366 1372//1372
+f 1388//1388 1433//1433 1374//1374
+f 1381//1381 1374//1374 1376//1376
+f 1388//1388 1375//1375 1433//1433
+f 1381//1381 1388//1388 1374//1374
+f 1389//1389 1376//1376 1377//1377
+f 1828//1828 1411//1411 1378//1378
+f 1434//1434 1379//1379 1383//1383
+f 1383//1383 1380//1380 1434//1434
+f 1380//1380 1424//1424 1434//1434
+f 1383//1383 1382//1382 1380//1380
+f 1380//1380 1382//1382 1377//1377
+f 1389//1389 1377//1377 1382//1382
+f 1381//1381 1376//1376 1389//1389
+f 1382//1382 1383//1383 1408//1408
+f 1390//1390 1382//1382 1408//1408
+f 1408//1408 1383//1383 1384//1384
+f 1385//1385 1384//1384 1379//1379
+f 1383//1383 1379//1379 1384//1384
+f 1384//1384 1385//1385 1414//1414
+f 1414//1414 1385//1385 1624//1624
+f 1375//1375 1386//1386 1387//1387
+f 1388//1388 1828//1828 1375//1375
+f 1828//1828 1386//1386 1375//1375
+f 1388//1388 1381//1381 1411//1411
+f 1388//1388 1411//1411 1828//1828
+f 1411//1411 1381//1381 1412//1412
+f 1381//1381 1389//1389 1410//1410
+f 1412//1412 1381//1381 1410//1410
+f 1389//1389 1382//1382 1390//1390
+f 1389//1389 1390//1390 1410//1410
+f 1399//1399 1404//1404 2725//2725
+f 1396//1396 1415//1415 1416//1416
+f 1391//1391 1395//1395 1393//1393
+f 1395//1395 1418//1418 1393//1393
+f 1399//1399 1391//1391 1393//1393
+f 1395//1395 1392//1392 1816//1816
+f 1816//1816 1394//1394 1395//1395
+f 1395//1395 1391//1391 1392//1392
+f 1393//1393 1418//1418 1415//1415
+f 1816//1816 1392//1392 1400//1400
+f 1394//1394 1418//1418 1395//1395
+f 1404//1404 1399//1399 1396//1396
+f 2717//2717 1397//1397 2715//2715
+f 1403//1403 1405//1405 2717//2717
+f 1415//1415 1396//1396 1393//1393
+f 2725//2725 1398//1398 1399//1399
+f 1398//1398 1363//1363 1391//1391
+f 1399//1399 1398//1398 1391//1391
+f 1391//1391 1363//1363 1392//1392
+f 1777//1777 1400//1400 1392//1392
+f 1402//1402 2715//2715 1401//1401
+f 1401//1401 2715//2715 2724//2724
+f 1402//1402 1403//1403 2715//2715
+f 2717//2717 2715//2715 1403//1403
+f 1404//1404 1396//1396 1401//1401
+f 1396//1396 1416//1416 1401//1401
+f 1401//1401 1416//1416 1402//1402
+f 1396//1396 1399//1399 1393//1393
+f 1414//1414 1624//1624 1405//1405
+f 1624//1624 4409//4409 1405//1405
+f 1414//1414 1406//1406 1384//1384
+f 1405//1405 1403//1403 1414//1414
+f 1408//1408 1406//1406 1407//1407
+f 1408//1408 1384//1384 1406//1406
+f 1390//1390 1407//1407 1409//1409
+f 1390//1390 1408//1408 1407//1407
+f 1410//1410 1409//1409 1417//1417
+f 1410//1410 1390//1390 1409//1409
+f 1412//1412 1417//1417 1419//1419
+f 1412//1412 1410//1410 1417//1417
+f 1378//1378 1411//1411 1419//1419
+f 1411//1411 1412//1412 1419//1419
+f 4409//4409 1624//1624 1413//1413
+f 1405//1405 2720//2720 2717//2717
+f 1405//1405 4409//4409 2720//2720
+f 1403//1403 1402//1402 1406//1406
+f 1403//1403 1406//1406 1414//1414
+f 1402//1402 1416//1416 1407//1407
+f 1406//1406 1402//1402 1407//1407
+f 1416//1416 1415//1415 1409//1409
+f 1407//1407 1416//1416 1409//1409
+f 1415//1415 1418//1418 1417//1417
+f 1409//1409 1415//1415 1417//1417
+f 1419//1419 1418//1418 1394//1394
+f 1417//1417 1418//1418 1419//1419
+f 1378//1378 1419//1419 1394//1394
+f 1527//1527 1423//1423 1427//1427
+f 1420//1420 1431//1431 1422//1422
+f 1431//1431 1430//1430 1437//1437
+f 1422//1422 1431//1431 1437//1437
+f 1437//1437 1430//1430 1432//1432
+f 1423//1423 1527//1527 513//513
+f 1427//1427 4678//4678 1421//1421
+f 1527//1527 1427//1427 1421//1421
+f 4678//4678 1422//1422 1435//1435
+f 1436//1436 1655//1655 1435//1435
+f 1428//1428 1427//1427 1423//1423
+f 1458//1458 1456//1456 1429//1429
+f 1458//1458 1429//1429 1442//1442
+f 1425//1425 1424//1424 1429//1429
+f 1456//1456 1425//1425 1429//1429
+f 1440//1440 1426//1426 4680//4680
+f 1427//1427 1428//1428 4679//4679
+f 1431//1431 1429//1429 1424//1424
+f 1431//1431 1420//1420 1429//1429
+f 1428//1428 1423//1423 4680//4680
+f 1424//1424 1380//1380 1430//1430
+f 1430//1430 1431//1431 1424//1424
+f 1380//1380 1377//1377 1430//1430
+f 1377//1377 1376//1376 1432//1432
+f 1430//1430 1377//1377 1432//1432
+f 1376//1376 1374//1374 1432//1432
+f 1432//1432 1374//1374 1438//1438
+f 1374//1374 1433//1433 1438//1438
+f 1433//1433 1387//1387 1659//1659
+f 1438//1438 1433//1433 1659//1659
+f 1433//1433 1375//1375 1387//1387
+f 1425//1425 1434//1434 1424//1424
+f 1654//1654 1435//1435 1655//1655
+f 1421//1421 4678//4678 1435//1435
+f 1654//1654 1421//1421 1435//1435
+f 1422//1422 1437//1437 1436//1436
+f 1435//1435 1422//1422 1436//1436
+f 1655//1655 1436//1436 1663//1663
+f 1436//1436 1437//1437 1656//1656
+f 1438//1438 1659//1659 1656//1656
+f 1432//1432 1438//1438 1437//1437
+f 1437//1437 1438//1438 1656//1656
+f 1439//1439 4682//4682 1440//1440
+f 4682//4682 1426//1426 1440//1440
+f 1441//1441 1421//1421 1654//1654
+f 1441//1441 1527//1527 1421//1421
+f 1427//1427 4679//4679 4678//4678
+f 1442//1442 1426//1426 1458//1458
+f 1188//1188 1443//1443 1444//1444
+f 1445//1445 1627//1627 1443//1443
+f 1443//1443 1188//1188 1445//1445
+f 1627//1627 1446//1446 1443//1443
+f 1443//1443 1446//1446 1448//1448
+f 1184//1184 1459//1459 1460//1460
+f 1632//1632 1455//1455 1446//1446
+f 1632//1632 1622//1622 1455//1455
+f 1456//1456 1622//1622 1618//1618
+f 1447//1447 1459//1459 1454//1454
+f 1482//1482 1459//1459 1483//1483
+f 1447//1447 1483//1483 1459//1459
+f 1454//1454 1450//1450 1457//1457
+f 1448//1448 1450//1450 1444//1444
+f 1443//1443 1448//1448 1444//1444
+f 1632//1632 1446//1446 1633//1633
+f 1187//1187 1460//1460 1480//1480
+f 1184//1184 1460//1460 1187//1187
+f 1184//1184 1449//1449 1454//1454
+f 1459//1459 1184//1184 1454//1454
+f 1449//1449 1451//1451 1450//1450
+f 1454//1454 1449//1449 1450//1450
+f 1451//1451 1188//1188 1444//1444
+f 1450//1450 1451//1451 1444//1444
+f 1448//1448 1455//1455 1452//1452
+f 1448//1448 1446//1446 1455//1455
+f 1434//1434 1425//1425 1617//1617
+f 1457//1457 1450//1450 1452//1452
+f 1450//1450 1448//1448 1452//1452
+f 1457//1457 1453//1453 1447//1447
+f 1447//1447 1453//1453 1483//1483
+f 1447//1447 1454//1454 1457//1457
+f 4682//4682 1439//1439 1457//1457
+f 1425//1425 1618//1618 1617//1617
+f 1458//1458 1455//1455 1456//1456
+f 1425//1425 1456//1456 1618//1618
+f 4682//4682 1457//1457 1452//1452
+f 1439//1439 1453//1453 1457//1457
+f 1455//1455 1622//1622 1456//1456
+f 1455//1455 1458//1458 1452//1452
+f 1458//1458 4682//4682 1452//1452
+f 1459//1459 1482//1482 1460//1460
+f 1140//1140 1461//1461 1462//1462
+f 1461//1461 1169//1169 1464//1464
+f 1477//1477 1463//1463 1485//1485
+f 1477//1477 1468//1468 1470//1470
+f 1485//1485 1468//1468 1477//1477
+f 1140//1140 1462//1462 1473//1473
+f 1470//1470 1468//1468 1466//1466
+f 1472//1472 1466//1466 1465//1465
+f 1464//1464 1169//1169 1163//1163
+f 1516//1516 1465//1465 1488//1488
+f 1526//1526 1516//1516 1488//1488
+f 1488//1488 1465//1465 1467//1467
+f 1462//1462 1461//1461 1464//1464
+f 1466//1466 1479//1479 1467//1467
+f 1466//1466 1467//1467 1465//1465
+f 1466//1466 1468//1468 1479//1479
+f 1470//1470 1466//1466 1472//1472
+f 1478//1478 1469//1469 1521//1521
+f 1521//1521 1469//1469 1471//1471
+f 1469//1469 1473//1473 1475//1475
+f 1521//1521 1471//1471 1476//1476
+f 1477//1477 1470//1470 1163//1163
+f 1470//1470 1464//1464 1163//1163
+f 1462//1462 1470//1470 1472//1472
+f 1462//1462 1464//1464 1470//1470
+f 1471//1471 1469//1469 1475//1475
+f 1475//1475 1473//1473 1472//1472
+f 1473//1473 1462//1462 1472//1472
+f 1474//1474 1142//1142 1469//1469
+f 1465//1465 1516//1516 1475//1475
+f 1465//1465 1475//1475 1472//1472
+f 1142//1142 1473//1473 1469//1469
+f 1471//1471 1516//1516 1517//1517
+f 1516//1516 1471//1471 1475//1475
+f 1476//1476 1471//1471 1517//1517
+f 1152//1152 1173//1173 1463//1463
+f 1162//1162 1152//1152 1463//1463
+f 1163//1163 1162//1162 1477//1477
+f 1477//1477 1162//1162 1463//1463
+f 1142//1142 1140//1140 1473//1473
+f 1474//1474 1478//1478 1529//1529
+f 1478//1478 1474//1474 1469//1469
+f 1485//1485 1484//1484 1479//1479
+f 1480//1480 1460//1460 1173//1173
+f 1479//1479 1484//1484 1481//1481
+f 1468//1468 1485//1485 1479//1479
+f 1460//1460 1482//1482 1487//1487
+f 1486//1486 1467//1467 1481//1481
+f 1485//1485 1463//1463 1487//1487
+f 1482//1482 1484//1484 1487//1487
+f 1482//1482 1483//1483 1484//1484
+f 1484//1484 1483//1483 1481//1481
+f 1528//1528 1474//1474 1529//1529
+f 1453//1453 1486//1486 1481//1481
+f 1463//1463 1173//1173 1487//1487
+f 1484//1484 1485//1485 1487//1487
+f 1467//1467 1479//1479 1481//1481
+f 1526//1526 1488//1488 1522//1522
+f 1488//1488 1467//1467 1486//1486
+f 1173//1173 1460//1460 1487//1487
+f 1486//1486 1522//1522 1488//1488
+f 1453//1453 1439//1439 1486//1486
+f 1483//1483 1453//1453 1481//1481
+f 1486//1486 1439//1439 1522//1522
+f 1489//1489 1498//1498 1490//1490
+f 1498//1498 1489//1489 1499//1499
+f 1499//1499 1512//1512 1515//1515
+f 1504//1504 1490//1490 1507//1507
+f 1489//1489 1490//1490 1504//1504
+f 1546//1546 1529//1529 1491//1491
+f 1492//1492 1573//1573 1491//1491
+f 1492//1492 1478//1478 1501//1501
+f 1529//1529 1478//1478 1492//1492
+f 4680//4680 1423//1423 1493//1493
+f 1423//1423 513//513 1493//1493
+f 1509//1509 1494//1494 1495//1495
+f 1493//1493 1494//1494 1503//1503
+f 1494//1494 1509//1509 1503//1503
+f 1497//1497 1495//1495 1496//1496
+f 1509//1509 1495//1495 1497//1497
+f 1515//1515 1512//1512 1496//1496
+f 1512//1512 1497//1497 1496//1496
+f 1498//1498 1499//1499 1515//1515
+f 1522//1522 1439//1439 1502//1502
+f 1500//1500 1506//1506 1508//1508
+f 1501//1501 1506//1506 1500//1500
+f 1500//1500 1492//1492 1501//1501
+f 1440//1440 1503//1503 1502//1502
+f 1439//1439 1440//1440 1502//1502
+f 1492//1492 1491//1491 1529//1529
+f 1492//1492 1500//1500 1573//1573
+f 4680//4680 1493//1493 1503//1503
+f 1440//1440 4680//4680 1503//1503
+f 1525//1525 1587//1587 1507//1507
+f 1504//1504 1501//1501 1505//1505
+f 1507//1507 1587//1587 1571//1571
+f 1506//1506 1507//1507 1571//1571
+f 1504//1504 1507//1507 1506//1506
+f 1508//1508 1506//1506 1571//1571
+f 1503//1503 1509//1509 1502//1502
+f 1509//1509 1497//1497 1510//1510
+f 1502//1502 1509//1509 1510//1510
+f 1497//1497 1512//1512 1511//1511
+f 1510//1510 1497//1497 1511//1511
+f 1512//1512 1499//1499 1519//1519
+f 1511//1511 1512//1512 1519//1519
+f 1499//1499 1489//1489 1513//1513
+f 1519//1519 1499//1499 1513//1513
+f 1489//1489 1504//1504 1505//1505
+f 1489//1489 1505//1505 1513//1513
+f 1501//1501 1504//1504 1506//1506
+f 1501//1501 1521//1521 1505//1505
+f 1514//1514 1515//1515 1496//1496
+f 1514//1514 506//506 1515//1515
+f 1495//1495 1518//1518 1496//1496
+f 1516//1516 1526//1526 1517//1517
+f 1518//1518 1514//1514 1496//1496
+f 1476//1476 1517//1517 1513//1513
+f 1494//1494 1530//1530 1495//1495
+f 1530//1530 1518//1518 1495//1495
+f 1517//1517 1519//1519 1513//1513
+f 1493//1493 1520//1520 1494//1494
+f 1521//1521 1476//1476 1505//1505
+f 1526//1526 1519//1519 1517//1517
+f 1526//1526 1511//1511 1519//1519
+f 1526//1526 1522//1522 1510//1510
+f 1522//1522 1502//1502 1510//1510
+f 1490//1490 1525//1525 1507//1507
+f 1523//1523 1587//1587 1525//1525
+f 1498//1498 1524//1524 1490//1490
+f 1524//1524 1525//1525 1490//1490
+f 506//506 1524//1524 1498//1498
+f 506//506 1498//1498 1515//1515
+f 1478//1478 1521//1521 1501//1501
+f 1526//1526 1510//1510 1511//1511
+f 1441//1441 513//513 1527//1527
+f 1528//1528 1529//1529 1546//1546
+f 513//513 1520//1520 1493//1493
+f 1520//1520 1530//1530 1494//1494
+f 1505//1505 1476//1476 1513//1513
+f 1538//1538 1574//1574 1542//1542
+f 1531//1531 1613//1613 1549//1549
+f 1543//1543 1533//1533 1541//1541
+f 1533//1533 1532//1532 1539//1539
+f 1541//1541 1533//1533 1539//1539
+f 1562//1562 1532//1532 1534//1534
+f 1532//1532 1562//1562 1539//1539
+f 1535//1535 1542//1542 1536//1536
+f 1532//1532 1533//1533 1590//1590
+f 4560//4560 1599//1599 1619//1619
+f 1534//1534 1532//1532 1591//1591
+f 1591//1591 1585//1585 1534//1534
+f 1585//1585 1537//1537 1540//1540
+f 1534//1534 1585//1585 1540//1540
+f 1535//1535 1536//1536 1537//1537
+f 1537//1537 1536//1536 1540//1540
+f 1538//1538 1542//1542 1535//1535
+f 1540//1540 1536//1536 1588//1588
+f 1541//1541 1558//1558 1611//1611
+f 1541//1541 1539//1539 1558//1558
+f 1540//1540 1588//1588 1586//1586
+f 1536//1536 1542//1542 1579//1579
+f 1588//1588 1536//1536 1579//1579
+f 1541//1541 1611//1611 1606//1606
+f 1542//1542 1574//1574 1544//1544
+f 1543//1543 1541//1541 1606//1606
+f 1579//1579 1542//1542 1544//1544
+f 3852//3852 1543//1543 1606//1606
+f 1544//1544 1574//1574 1575//1575
+f 1558//1558 1539//1539 1560//1560
+f 1534//1534 1540//1540 1586//1586
+f 1562//1562 1534//1534 1586//1586
+f 479//479 1531//1531 1549//1549
+f 1582//1582 1553//1553 1552//1552
+f 1539//1539 1562//1562 1560//1560
+f 1532//1532 1590//1590 1591//1591
+f 449//449 1545//1545 1580//1580
+f 449//449 1550//1550 1545//1545
+f 1528//1528 1546//1546 1570//1570
+f 460//460 1580//1580 1589//1589
+f 1548//1548 1555//1555 1547//1547
+f 1553//1553 1548//1548 1547//1547
+f 1555//1555 1556//1556 1594//1594
+f 1547//1547 1555//1555 1594//1594
+f 1546//1546 1538//1538 1570//1570
+f 1550//1550 1551//1551 1545//1545
+f 447//447 462//462 1566//1566
+f 1551//1551 447//447 1566//1566
+f 479//479 1549//1549 462//462
+f 1550//1550 447//447 1551//1551
+f 1549//1549 1566//1566 462//462
+f 1544//1544 1575//1575 1555//1555
+f 1561//1561 1565//1565 1559//1559
+f 1552//1552 1581//1581 1582//1582
+f 1552//1552 1561//1561 1581//1581
+f 1561//1561 1552//1552 1565//1565
+f 1553//1553 1582//1582 1548//1548
+f 1554//1554 3852//3852 1610//1610
+f 1543//1543 1554//1554 1576//1576
+f 1543//1543 3852//3852 1554//1554
+f 1590//1590 1533//1533 1576//1576
+f 1533//1533 1543//1543 1576//1576
+f 1548//1548 1544//1544 1555//1555
+f 1594//1594 1556//1556 1568//1568
+f 1555//1555 1575//1575 1556//1556
+f 1557//1557 1558//1558 1592//1592
+f 1558//1558 1557//1557 1611//1611
+f 1561//1561 1559//1559 1592//1592
+f 1558//1558 1560//1560 1592//1592
+f 1560//1560 1581//1581 1561//1561
+f 1560//1560 1561//1561 1592//1592
+f 1562//1562 1581//1581 1560//1560
+f 1545//1545 1593//1593 1563//1563
+f 1580//1580 1563//1563 1564//1564
+f 1580//1580 1545//1545 1563//1563
+f 1580//1580 1564//1564 1572//1572
+f 1586//1586 1588//1588 1582//1582
+f 1589//1589 1580//1580 1572//1572
+f 1538//1538 1546//1546 1574//1574
+f 1549//1549 1565//1565 1566//1566
+f 1565//1565 1583//1583 1566//1566
+f 1583//1583 1551//1551 1566//1566
+f 1577//1577 1567//1567 1554//1554
+f 1551//1551 1583//1583 1593//1593
+f 1545//1545 1551//1551 1593//1593
+f 1575//1575 1573//1573 1556//1556
+f 1556//1556 1500//1500 1568//1568
+f 1500//1500 1508//1508 1568//1568
+f 1569//1569 1538//1538 1535//1535
+f 1578//1578 1569//1569 1535//1535
+f 1569//1569 1570//1570 1538//1538
+f 1571//1571 1589//1589 1572//1572
+f 1508//1508 1571//1571 1572//1572
+f 1571//1571 1587//1587 1589//1589
+f 1573//1573 1500//1500 1556//1556
+f 1574//1574 1491//1491 1575//1575
+f 1546//1546 1491//1491 1574//1574
+f 1491//1491 1573//1573 1575//1575
+f 1554//1554 1567//1567 1576//1576
+f 1568//1568 1508//1508 1572//1572
+f 1567//1567 1590//1590 1576//1576
+f 1577//1577 1554//1554 1610//1610
+f 1584//1584 1578//1578 1537//1537
+f 1579//1579 1544//1544 1548//1548
+f 1108//1108 1098//1098 1591//1591
+f 460//460 449//449 1580//1580
+f 1581//1581 1586//1586 1582//1582
+f 1552//1552 1583//1583 1565//1565
+f 1552//1552 1553//1553 1583//1583
+f 1591//1591 1584//1584 1585//1585
+f 1562//1562 1586//1586 1581//1581
+f 1523//1523 451//451 1587//1587
+f 451//451 460//460 1589//1589
+f 1098//1098 1584//1584 1591//1591
+f 1585//1585 1584//1584 1537//1537
+f 1582//1582 1588//1588 1548//1548
+f 1588//1588 1579//1579 1548//1548
+f 1587//1587 451//451 1589//1589
+f 1549//1549 1613//1613 1559//1559
+f 1564//1564 1568//1568 1572//1572
+f 1567//1567 1108//1108 1590//1590
+f 1590//1590 1108//1108 1591//1591
+f 1565//1565 1549//1549 1559//1559
+f 1557//1557 1592//1592 1601//1601
+f 1592//1592 1559//1559 1601//1601
+f 1559//1559 1613//1613 1601//1601
+f 1583//1583 1553//1553 1593//1593
+f 1553//1553 1547//1547 1593//1593
+f 1547//1547 1594//1594 1563//1563
+f 1593//1593 1547//1547 1563//1563
+f 1594//1594 1568//1568 1564//1564
+f 1563//1563 1594//1594 1564//1564
+f 1578//1578 1535//1535 1537//1537
+f 3242//3242 3847//3847 3243//3243
+f 1595//1595 1596//1596 1597//1597
+f 4346//4346 1595//1595 419//419
+f 4346//4346 1598//1598 1595//1595
+f 2624//2624 4396//4396 1599//1599
+f 4396//4396 1619//1619 1599//1599
+f 1600//1600 3850//3850 1612//1612
+f 3851//3851 3850//3850 1600//1600
+f 3848//3848 1602//1602 1603//1603
+f 1601//1601 1613//1613 3850//3850
+f 1601//1601 3850//3850 3849//3849
+f 1613//1613 1612//1612 3850//3850
+f 3853//3853 1602//1602 1609//1609
+f 3852//3852 1609//1609 1610//1610
+f 1603//1603 3241//3241 3848//3848
+f 1602//1602 3853//3853 1603//1603
+f 1604//1604 4417//4417 1606//1606
+f 1605//1605 1607//1607 1600//1600
+f 1606//1606 4417//4417 3852//3852
+f 1606//1606 1611//1611 1604//1604
+f 1607//1607 1608//1608 1600//1600
+f 1602//1602 1091//1091 1610//1610
+f 1610//1610 1609//1609 1602//1602
+f 1091//1091 1577//1577 1610//1610
+f 1611//1611 1557//1557 1604//1604
+f 1601//1601 3849//3849 1557//1557
+f 1531//1531 1605//1605 1612//1612
+f 1605//1605 1600//1600 1612//1612
+f 1613//1613 1531//1531 1612//1612
+f 1616//1616 1614//1614 1615//1615
+f 1614//1614 1616//1616 1625//1625
+f 1616//1616 2662//2662 1625//1625
+f 1614//1614 1625//1625 1627//1627
+f 1614//1614 1445//1445 1201//1201
+f 1201//1201 2661//2661 1615//1615
+f 1627//1627 1445//1445 1614//1614
+f 1631//1631 1618//1618 1623//1623
+f 1379//1379 1434//1434 1617//1617
+f 1385//1385 1379//1379 1617//1617
+f 1617//1617 1618//1618 1631//1631
+f 4420//4420 4616//4616 1619//1619
+f 1623//1623 1629//1629 1413//1413
+f 1631//1631 1623//1623 1413//1413
+f 1413//1413 1621//1621 1628//1628
+f 1621//1621 1413//1413 1629//1629
+f 1620//1620 4410//4410 2662//2662
+f 2662//2662 1621//1621 1630//1630
+f 1625//1625 2662//2662 1630//1630
+f 1183//1183 1201//1201 1445//1445
+f 1621//1621 4410//4410 1628//1628
+f 4410//4410 1621//1621 2662//2662
+f 1621//1621 1629//1629 1630//1630
+f 1622//1622 1629//1629 1623//1623
+f 1624//1624 1631//1631 1413//1413
+f 1625//1625 1633//1633 1627//1627
+f 2021//2021 2117//2117 1626//1626
+f 1627//1627 1633//1633 1446//1446
+f 1188//1188 1183//1183 1445//1445
+f 4409//4409 1413//1413 1628//1628
+f 1622//1622 1632//1632 1629//1629
+f 1385//1385 1617//1617 1631//1631
+f 1629//1629 1632//1632 1630//1630
+f 1633//1633 1625//1625 1630//1630
+f 1618//1618 1622//1622 1623//1623
+f 1624//1624 1385//1385 1631//1631
+f 1632//1632 1633//1633 1630//1630
+f 4456//4456 1651//1651 1650//1650
+f 1634//1634 863//863 1669//1669
+f 4456//4456 1641//1641 1635//1635
+f 564//564 1670//1670 1669//1669
+f 1636//1636 4420//4420 1619//1619
+f 1637//1637 1638//1638 564//564
+f 4477//4477 566//566 1638//1638
+f 1650//1650 1641//1641 4456//4456
+f 1639//1639 1640//1640 538//538
+f 538//538 1640//1640 1642//1642
+f 1635//1635 1661//1661 1639//1639
+f 1639//1639 1661//1661 1640//1640
+f 1635//1635 1641//1641 1662//1662
+f 1661//1661 1635//1635 1662//1662
+f 538//538 1642//1642 1644//1644
+f 1645//1645 1643//1643 1644//1644
+f 1643//1643 1645//1645 539//539
+f 1642//1642 1645//1645 1644//1644
+f 1644//1644 1643//1643 4459//4459
+f 1643//1643 539//539 1646//1646
+f 1825//1825 4460//4460 1817//1817
+f 1650//1650 1652//1652 1641//1641
+f 1662//1662 1641//1641 1665//1665
+f 1641//1641 1652//1652 1665//1665
+f 1645//1645 1642//1642 1647//1647
+f 1806//1806 540//540 1820//1820
+f 1635//1635 1639//1639 1648//1648
+f 549//549 539//539 1817//1817
+f 1639//1639 538//538 547//547
+f 1644//1644 1689//1689 538//538
+f 1648//1648 1697//1697 4456//4456
+f 4450//4450 1649//1649 1651//1651
+f 1649//1649 1650//1650 1651//1651
+f 1650//1650 494//494 1652//1652
+f 494//494 1653//1653 1652//1652
+f 1652//1652 1653//1653 1664//1664
+f 1653//1653 1441//1441 1654//1654
+f 1653//1653 1654//1654 1664//1664
+f 1660//1660 1387//1387 1647//1647
+f 1655//1655 1665//1665 1664//1664
+f 1654//1654 1655//1655 1664//1664
+f 1655//1655 1663//1663 1665//1665
+f 1436//1436 1656//1656 1658//1658
+f 1663//1663 1436//1436 1658//1658
+f 1656//1656 1659//1659 1657//1657
+f 1658//1658 1656//1656 1657//1657
+f 1659//1659 1387//1387 1660//1660
+f 1659//1659 1660//1660 1657//1657
+f 1640//1640 1658//1658 1657//1657
+f 1640//1640 1661//1661 1658//1658
+f 1661//1661 1662//1662 1663//1663
+f 1661//1661 1663//1663 1658//1658
+f 1662//1662 1665//1665 1663//1663
+f 1652//1652 1664//1664 1665//1665
+f 4460//4460 1666//1666 1673//1673
+f 1642//1642 1660//1660 1647//1647
+f 1642//1642 1640//1640 1657//1657
+f 1660//1660 1642//1642 1657//1657
+f 1668//1668 1683//1683 1667//1667
+f 1667//1667 1675//1675 1668//1668
+f 863//863 1634//1634 869//869
+f 1669//1669 1670//1670 1634//1634
+f 1638//1638 1670//1670 564//564
+f 1676//1676 1670//1670 1638//1638
+f 566//566 1676//1676 1638//1638
+f 566//566 1678//1678 1676//1676
+f 567//567 1678//1678 566//566
+f 1671//1671 1672//1672 1678//1678
+f 1673//1673 1680//1680 548//548
+f 1666//1666 1680//1680 1673//1673
+f 1674//1674 1680//1680 1666//1666
+f 1683//1683 1680//1680 1674//1674
+f 1667//1667 1683//1683 1674//1674
+f 1675//1675 1832//1832 1668//1668
+f 869//869 1832//1832 1675//1675
+f 869//869 1714//1714 1832//1832
+f 1634//1634 1714//1714 869//869
+f 1634//1634 1691//1691 1714//1714
+f 1691//1691 1634//1634 1670//1670
+f 1676//1676 1691//1691 1670//1670
+f 1676//1676 1677//1677 1691//1691
+f 1678//1678 1677//1677 1676//1676
+f 1678//1678 1679//1679 1677//1677
+f 1672//1672 1679//1679 1678//1678
+f 1672//1672 1681//1681 1679//1679
+f 568//568 1672//1672 1712//1712
+f 1682//1682 1690//1690 4459//4459
+f 1680//1680 1682//1682 548//548
+f 1680//1680 1683//1683 1682//1682
+f 2441//2441 2489//2489 2508//2508
+f 1679//1679 1686//1686 1677//1677
+f 1681//1681 1686//1686 1679//1679
+f 1681//1681 569//569 1686//1686
+f 901//901 4488//4488 4487//4487
+f 1682//1682 1683//1683 1690//1690
+f 1668//1668 1695//1695 1683//1683
+f 1668//1668 1684//1684 1695//1695
+f 1668//1668 1832//1832 1684//1684
+f 1677//1677 1685//1685 1691//1691
+f 1686//1686 1685//1685 1677//1677
+f 1686//1686 1687//1687 1685//1685
+f 569//569 1687//1687 1686//1686
+f 569//569 570//570 1687//1687
+f 4488//4488 570//570 569//569
+f 1688//1688 570//570 4488//4488
+f 1694//1694 1700//1700 1689//1689
+f 1695//1695 1690//1690 1683//1683
+f 1695//1695 1694//1694 1690//1690
+f 1829//1829 1714//1714 1713//1713
+f 1685//1685 1714//1714 1691//1691
+f 1687//1687 1692//1692 1685//1685
+f 1696//1696 1687//1687 570//570
+f 1722//1722 1687//1687 1696//1696
+f 1695//1695 1693//1693 1694//1694
+f 1695//1695 1684//1684 1693//1693
+f 1829//1829 1832//1832 1714//1714
+f 1685//1685 1692//1692 1714//1714
+f 1722//1722 1692//1692 1687//1687
+f 1698//1698 1722//1722 1696//1696
+f 574//574 1722//1722 1698//1698
+f 1697//1697 1648//1648 1760//1760
+f 1700//1700 547//547 1689//1689
+f 1694//1694 1693//1693 1700//1700
+f 573//573 574//574 1698//1698
+f 1700//1700 1699//1699 1760//1760
+f 1693//1693 1699//1699 1700//1700
+f 1684//1684 1699//1699 1693//1693
+f 2028//2028 1832//1832 1829//1829
+f 1720//1720 1701//1701 1702//1702
+f 1703//1703 1704//1704 1737//1737
+f 1716//1716 1718//1718 1724//1724
+f 1718//1718 1716//1716 1705//1705
+f 4478//4478 1724//1724 1725//1725
+f 1716//1716 1724//1724 4478//4478
+f 1717//1717 1725//1725 1707//1707
+f 4478//4478 1725//1725 1717//1717
+f 1706//1706 1707//1707 1708//1708
+f 1717//1717 1707//1707 1706//1706
+f 1708//1708 1707//1707 1723//1723
+f 1720//1720 1708//1708 1701//1701
+f 1709//1709 1717//1717 1706//1706
+f 541//541 1706//1706 1710//1710
+f 1711//1711 1712//1712 1710//1710
+f 4486//4486 568//568 1719//1719
+f 1721//1721 1719//1719 1711//1711
+f 1712//1712 1711//1711 1719//1719
+f 1713//1713 1714//1714 1692//1692
+f 903//903 4488//4488 901//901
+f 1715//1715 4486//4486 1719//1719
+f 1708//1708 1711//1711 1710//1710
+f 1706//1706 1708//1708 1710//1710
+f 1718//1718 1705//1705 1737//1737
+f 565//565 1705//1705 1716//1716
+f 4477//4477 4478//4478 1717//1717
+f 1718//1718 1704//1704 1726//1726
+f 1704//1704 1881//1881 1726//1726
+f 901//901 4487//4487 4486//4486
+f 1704//1704 1727//1727 1881//1881
+f 1737//1737 1704//1704 1718//1718
+f 1720//1720 1711//1711 1708//1708
+f 1715//1715 1719//1719 1721//1721
+f 1704//1704 1703//1703 1727//1727
+f 1720//1720 1721//1721 1711//1711
+f 1727//1727 1703//1703 1762//1762
+f 1722//1722 1729//1729 1692//1692
+f 574//574 1729//1729 1722//1722
+f 1725//1725 1891//1891 1892//1892
+f 1723//1723 1707//1707 1892//1892
+f 1701//1701 1708//1708 1723//1723
+f 1724//1724 1891//1891 1725//1725
+f 1707//1707 1725//1725 1892//1892
+f 1724//1724 1718//1718 1726//1726
+f 1726//1726 1881//1881 1891//1891
+f 1726//1726 1891//1891 1724//1724
+f 1846//1846 1727//1727 1762//1762
+f 574//574 1728//1728 1729//1729
+f 3055//3055 3039//3039 2530//2530
+f 2530//2530 2508//2508 3055//3055
+f 1705//1705 563//563 1730//1730
+f 1733//1733 1735//1735 1763//1763
+f 1737//1737 1730//1730 1731//1731
+f 1703//1703 1737//1737 1731//1731
+f 1703//1703 1731//1731 1743//1743
+f 1728//1728 574//574 576//576
+f 1735//1735 1733//1733 1732//1732
+f 1733//1733 1771//1771 1849//1849
+f 1732//1732 1733//1733 1849//1849
+f 1732//1732 1755//1755 1734//1734
+f 1732//1732 1734//1734 1747//1747
+f 1732//1732 1849//1849 1755//1755
+f 1735//1735 1732//1732 1747//1747
+f 1762//1762 1703//1703 1743//1743
+f 1733//1733 1764//1764 1771//1771
+f 2489//2489 1736//1736 2508//2508
+f 1737//1737 1705//1705 1730//1730
+f 1738//1738 1753//1753 1743//1743
+f 1743//1743 1731//1731 1738//1738
+f 1744//1744 1746//1746 1739//1739
+f 1830//1830 1728//1728 1752//1752
+f 1740//1740 1746//1746 1745//1745
+f 1739//1739 1746//1746 1740//1740
+f 1754//1754 1745//1745 1741//1741
+f 1740//1740 1745//1745 1754//1754
+f 1753//1753 1742//1742 1743//1743
+f 1744//1744 1763//1763 1735//1735
+f 1763//1763 1744//1744 1791//1791
+f 1735//1735 1747//1747 1746//1746
+f 1744//1744 1735//1735 1746//1746
+f 1745//1745 1747//1747 1748//1748
+f 1746//1746 1747//1747 1745//1745
+f 1741//1741 1748//1748 1756//1756
+f 1741//1741 1745//1745 1748//1748
+f 1742//1742 1753//1753 1756//1756
+f 1753//1753 1741//1741 1756//1756
+f 1741//1741 1753//1753 1749//1749
+f 1754//1754 1749//1749 1767//1767
+f 1750//1750 1749//1749 1769//1769
+f 1767//1767 1749//1749 1750//1750
+f 1751//1751 1830//1830 1752//1752
+f 4471//4471 1730//1730 563//563
+f 1637//1637 1716//1716 4478//4478
+f 1767//1767 1765//1765 1740//1740
+f 1754//1754 1741//1741 1749//1749
+f 1753//1753 1738//1738 1769//1769
+f 1749//1749 1753//1753 1769//1769
+f 1769//1769 1738//1738 4471//4471
+f 1738//1738 1731//1731 1730//1730
+f 4471//4471 1738//1738 1730//1730
+f 1739//1739 1740//1740 1765//1765
+f 1770//1770 1739//1739 1765//1765
+f 1740//1740 1754//1754 1767//1767
+f 1755//1755 1761//1761 1734//1734
+f 1756//1756 1748//1748 1734//1734
+f 1759//1759 1757//1757 1758//1758
+f 1759//1759 1742//1742 1757//1757
+f 1757//1757 1761//1761 1758//1758
+f 1742//1742 1756//1756 1757//1757
+f 1846//1846 1762//1762 1759//1759
+f 1762//1762 1742//1742 1759//1759
+f 1758//1758 1846//1846 1759//1759
+f 1747//1747 1734//1734 1748//1748
+f 1760//1760 546//546 1697//1697
+f 1734//1734 1761//1761 1757//1757
+f 1756//1756 1734//1734 1757//1757
+f 1762//1762 1743//1743 1742//1742
+f 1733//1733 1763//1763 1764//1764
+f 1770//1770 1765//1765 1766//1766
+f 1766//1766 1765//1765 557//557
+f 1699//1699 546//546 1760//1760
+f 1767//1767 1750//1750 1768//1768
+f 1750//1750 1769//1769 1831//1831
+f 565//565 563//563 1705//1705
+f 1770//1770 1766//1766 1799//1799
+f 1771//1771 1764//1764 1778//1778
+f 1847//1847 1771//1771 1774//1774
+f 1778//1778 1764//1764 1772//1772
+f 1739//1739 1770//1770 1782//1782
+f 1798//1798 4683//4683 1370//1370
+f 1764//1764 1763//1763 1772//1772
+f 1773//1773 1788//1788 1779//1779
+f 1773//1773 1774//1774 1788//1788
+f 1773//1773 1841//1841 1774//1774
+f 1775//1775 1777//1777 1776//1776
+f 4683//4683 1780//1780 1779//1779
+f 1772//1772 1763//1763 1791//1791
+f 1841//1841 1847//1847 1774//1774
+f 1362//1362 1370//1370 1790//1790
+f 1791//1791 1744//1744 1782//1782
+f 1774//1774 1778//1778 1788//1788
+f 1744//1744 1739//1739 1782//1782
+f 1776//1776 1777//1777 1790//1790
+f 1777//1777 1362//1362 1790//1790
+f 1774//1774 1771//1771 1778//1778
+f 1780//1780 1842//1842 1779//1779
+f 1784//1784 1780//1780 1856//1856
+f 1841//1841 1773//1773 1842//1842
+f 1773//1773 1779//1779 1842//1842
+f 4683//4683 1779//1779 1789//1789
+f 1811//1811 1775//1775 1776//1776
+f 1781//1781 1811//1811 1776//1776
+f 1770//1770 1801//1801 1783//1783
+f 1770//1770 1783//1783 1782//1782
+f 1800//1800 1793//1793 1801//1801
+f 1783//1783 1801//1801 1793//1793
+f 1800//1800 1795//1795 1793//1793
+f 1781//1781 1776//1776 1796//1796
+f 1842//1842 1780//1780 1784//1784
+f 1772//1772 1791//1791 1792//1792
+f 1792//1792 1794//1794 1785//1785
+f 1772//1772 1792//1792 1785//1785
+f 1794//1794 1786//1786 1787//1787
+f 1788//1788 1778//1778 1785//1785
+f 1788//1788 1785//1785 1787//1787
+f 1778//1778 1772//1772 1785//1785
+f 1790//1790 4683//4683 1789//1789
+f 1786//1786 1789//1789 1787//1787
+f 1785//1785 1794//1794 1787//1787
+f 1779//1779 1788//1788 1787//1787
+f 1789//1789 1779//1779 1787//1787
+f 1795//1795 1781//1781 1796//1796
+f 1776//1776 1790//1790 1796//1796
+f 1791//1791 1782//1782 1783//1783
+f 1783//1783 1793//1793 1792//1792
+f 1791//1791 1783//1783 1792//1792
+f 1793//1793 1795//1795 1794//1794
+f 1792//1792 1793//1793 1794//1794
+f 1795//1795 1796//1796 1786//1786
+f 1794//1794 1795//1795 1786//1786
+f 1796//1796 1789//1789 1786//1786
+f 1796//1796 1790//1790 1789//1789
+f 1781//1781 1810//1810 1811//1811
+f 1369//1369 1797//1797 1844//1844
+f 1781//1781 1800//1800 1810//1810
+f 1856//1856 1798//1798 1844//1844
+f 1798//1798 1373//1373 1844//1844
+f 1736//1736 2313//2313 3040//3040
+f 1801//1801 1770//1770 1799//1799
+f 1373//1373 1369//1369 1844//1844
+f 1370//1370 1373//1373 1798//1798
+f 1801//1801 1799//1799 1804//1804
+f 1800//1800 1801//1801 1804//1804
+f 1800//1800 1781//1781 1795//1795
+f 551//551 1807//1807 4464//4464
+f 1819//1819 1805//1805 553//553
+f 1684//1684 1928//1928 1699//1699
+f 1802//1802 4468//4468 1808//1808
+f 4468//4468 1802//1802 1803//1803
+f 1802//1802 4464//4464 1803//1803
+f 4468//4468 1826//1826 1804//1804
+f 4468//4468 1803//1803 1826//1826
+f 4464//4464 1807//1807 1822//1822
+f 1799//1799 1766//1766 1805//1805
+f 1387//1387 1386//1386 1647//1647
+f 1813//1813 1803//1803 1814//1814
+f 551//551 1806//1806 1807//1807
+f 1803//1803 4464//4464 1814//1814
+f 1803//1803 1813//1813 1826//1826
+f 1825//1825 1815//1815 1821//1821
+f 1815//1815 1817//1817 1818//1818
+f 1813//1813 1810//1810 1826//1826
+f 1810//1810 1800//1800 1826//1826
+f 1766//1766 557//557 556//556
+f 1805//1805 1766//1766 556//556
+f 1819//1819 553//553 1808//1808
+f 1386//1386 1828//1828 1815//1815
+f 1816//1816 1809//1809 1827//1827
+f 1394//1394 1816//1816 1827//1827
+f 1811//1811 1810//1810 1813//1813
+f 1811//1811 1813//1813 1812//1812
+f 1775//1775 1811//1811 1812//1812
+f 1813//1813 1814//1814 1812//1812
+f 1775//1775 1812//1812 1400//1400
+f 1386//1386 1815//1815 1818//1818
+f 1647//1647 1386//1386 1818//1818
+f 1809//1809 1400//1400 1812//1812
+f 1816//1816 1400//1400 1809//1809
+f 4464//4464 1822//1822 1814//1814
+f 4465//4465 4464//4464 1802//1802
+f 1825//1825 1821//1821 1820//1820
+f 1645//1645 1647//1647 1818//1818
+f 1817//1817 539//539 1818//1818
+f 539//539 1645//1645 1818//1818
+f 1828//1828 1378//1378 1821//1821
+f 1821//1821 1378//1378 1824//1824
+f 1819//1819 4468//4468 1804//1804
+f 1824//1824 1827//1827 1823//1823
+f 1814//1814 1822//1822 1809//1809
+f 1814//1814 1809//1809 1812//1812
+f 1820//1820 1821//1821 1824//1824
+f 1827//1827 1822//1822 1823//1823
+f 1822//1822 1807//1807 1823//1823
+f 1819//1819 1799//1799 1805//1805
+f 1394//1394 1827//1827 1824//1824
+f 1378//1378 1394//1394 1824//1824
+f 1804//1804 1799//1799 1819//1819
+f 1807//1807 1806//1806 1823//1823
+f 1815//1815 1825//1825 1817//1817
+f 1806//1806 1820//1820 1823//1823
+f 1800//1800 1804//1804 1826//1826
+f 1820//1820 1824//1824 1823//1823
+f 1809//1809 1822//1822 1827//1827
+f 1815//1815 1828//1828 1821//1821
+f 1922//1922 1829//1829 1713//1713
+f 1729//1729 1713//1713 1692//1692
+f 1728//1728 1924//1924 1729//1729
+f 1830//1830 1924//1924 1728//1728
+f 1830//1830 577//577 1924//1924
+f 1767//1767 557//557 1765//1765
+f 1831//1831 1769//1769 4471//4471
+f 1699//1699 1921//1921 546//546
+f 1699//1699 1928//1928 1921//1921
+f 1832//1832 1928//1928 1684//1684
+f 1922//1922 2023//2023 1829//1829
+f 1844//1844 1797//1797 1855//1855
+f 1850//1850 1854//1854 1833//1833
+f 1850//1850 1834//1834 1835//1835
+f 1850//1850 1833//1833 1845//1845
+f 1727//1727 1846//1846 1889//1889
+f 1845//1845 1833//1833 1836//1836
+f 1837//1837 1836//1836 1851//1851
+f 1845//1845 1836//1836 1837//1837
+f 1837//1837 1851//1851 1890//1890
+f 1837//1837 1890//1890 1846//1846
+f 1846//1846 1890//1890 1889//1889
+f 1847//1847 1840//1840 1834//1834
+f 1833//1833 1854//1854 1838//1838
+f 1833//1833 1838//1838 1839//1839
+f 1836//1836 1833//1833 1839//1839
+f 1843//1843 1852//1852 1840//1840
+f 1834//1834 1840//1840 1835//1835
+f 1850//1850 1835//1835 1854//1854
+f 1841//1841 1842//1842 1843//1843
+f 1840//1840 1841//1841 1843//1843
+f 1842//1842 1784//1784 1843//1843
+f 1843//1843 1784//1784 1857//1857
+f 1784//1784 1856//1856 1857//1857
+f 1844//1844 1855//1855 1856//1856
+f 1847//1847 1834//1834 1849//1849
+f 1834//1834 1850//1850 1755//1755
+f 1850//1850 1845//1845 1761//1761
+f 1761//1761 1845//1845 1758//1758
+f 1845//1845 1837//1837 1758//1758
+f 2313//2313 4559//4559 3040//3040
+f 1837//1837 1846//1846 1758//1758
+f 1847//1847 1841//1841 1840//1840
+f 1839//1839 1910//1910 1878//1878
+f 1910//1910 1839//1839 1838//1838
+f 1910//1910 1838//1838 1911//1911
+f 1911//1911 1838//1838 1858//1858
+f 1858//1858 1912//1912 1911//1911
+f 1858//1858 1848//1848 1912//1912
+f 1848//1848 1914//1914 1912//1912
+f 1771//1771 1847//1847 1849//1849
+f 1849//1849 1834//1834 1755//1755
+f 1755//1755 1850//1850 1761//1761
+f 1848//1848 1857//1857 1914//1914
+f 1839//1839 1878//1878 1879//1879
+f 1836//1836 1839//1839 1879//1879
+f 1851//1851 1836//1836 1879//1879
+f 1843//1843 1857//1857 1852//1852
+f 1835//1835 1852//1852 1853//1853
+f 1840//1840 1852//1852 1835//1835
+f 1854//1854 1835//1835 1853//1853
+f 1857//1857 1855//1855 1914//1914
+f 1856//1856 1855//1855 1857//1857
+f 1857//1857 1848//1848 1852//1852
+f 1848//1848 1858//1858 1853//1853
+f 1852//1852 1848//1848 1853//1853
+f 1838//1838 1854//1854 1858//1858
+f 1858//1858 1854//1854 1853//1853
+f 1859//1859 1875//1875 1863//1863
+f 1859//1859 1863//1863 1865//1865
+f 1867//1867 1863//1863 1876//1876
+f 1876//1876 1863//1863 1875//1875
+f 1867//1867 1896//1896 1860//1860
+f 1887//1887 1877//1877 1861//1861
+f 1877//1877 1865//1865 1862//1862
+f 1861//1861 1877//1877 1862//1862
+f 1863//1863 1864//1864 1865//1865
+f 1862//1862 1865//1865 1886//1886
+f 1863//1863 1867//1867 1864//1864
+f 1865//1865 1864//1864 1886//1886
+f 1867//1867 1860//1860 1883//1883
+f 1883//1883 1860//1860 1866//1866
+f 1867//1867 1883//1883 1885//1885
+f 1881//1881 1889//1889 1871//1871
+f 1865//1865 1877//1877 1868//1868
+f 914//914 1883//1883 1866//1866
+f 914//914 1869//1869 1883//1883
+f 1898//1898 914//914 1866//1866
+f 1881//1881 1871//1871 1870//1870
+f 1871//1871 1872//1872 1873//1873
+f 1870//1870 1871//1871 1873//1873
+f 1868//1868 1872//1872 1888//1888
+f 1872//1872 1868//1868 1873//1873
+f 1859//1859 1888//1888 1880//1880
+f 1859//1859 1868//1868 1888//1888
+f 1875//1875 1880//1880 1874//1874
+f 1875//1875 1874//1874 1876//1876
+f 1875//1875 1859//1859 1880//1880
+f 1876//1876 1896//1896 1867//1867
+f 1887//1887 1870//1870 1873//1873
+f 1868//1868 1877//1877 1873//1873
+f 1877//1877 1887//1887 1873//1873
+f 1859//1859 1865//1865 1868//1868
+f 921//921 1882//1882 1861//1861
+f 921//921 1861//1861 1862//1862
+f 1880//1880 1878//1878 1874//1874
+f 1879//1879 1878//1878 1880//1880
+f 1851//1851 1879//1879 1888//1888
+f 1888//1888 1879//1879 1880//1880
+f 1881//1881 1727//1727 1889//1889
+f 1882//1882 1702//1702 1861//1861
+f 1702//1702 1701//1701 1861//1861
+f 1869//1869 1885//1885 1883//1883
+f 1885//1885 1884//1884 1864//1864
+f 1864//1864 1867//1867 1885//1885
+f 1884//1884 919//919 1886//1886
+f 1884//1884 1886//1886 1864//1864
+f 919//919 921//921 1862//1862
+f 919//919 1862//1862 1886//1886
+f 1887//1887 1892//1892 1870//1870
+f 1723//1723 1892//1892 1887//1887
+f 1701//1701 1887//1887 1861//1861
+f 1701//1701 1723//1723 1887//1887
+f 1872//1872 1851//1851 1888//1888
+f 1890//1890 1851//1851 1872//1872
+f 1889//1889 1890//1890 1871//1871
+f 1891//1891 1881//1881 1870//1870
+f 1892//1892 1891//1891 1870//1870
+f 1871//1871 1890//1890 1872//1872
+f 2799//2799 1897//1897 1901//1901
+f 2797//2797 1893//1893 1894//1894
+f 1895//1895 1900//1900 1916//1916
+f 1900//1900 1895//1895 2799//2799
+f 1893//1893 2797//2797 2794//2794
+f 2797//2797 1894//1894 1897//1897
+f 1896//1896 2799//2799 1901//1901
+f 1901//1901 1897//1897 1894//1894
+f 1898//1898 1894//1894 1899//1899
+f 1898//1898 1866//1866 1894//1894
+f 1897//1897 2801//2801 2797//2797
+f 2801//2801 1897//1897 2798//2798
+f 1897//1897 2799//2799 2798//2798
+f 1900//1900 1874//1874 1916//1916
+f 1876//1876 1874//1874 1900//1900
+f 1896//1896 1876//1876 1900//1900
+f 1866//1866 1860//1860 1901//1901
+f 1900//1900 2799//2799 1896//1896
+f 1874//1874 1878//1878 1916//1916
+f 1866//1866 1901//1901 1894//1894
+f 1860//1860 1896//1896 1901//1901
+f 2971//2971 1902//1902 4252//4252
+f 1893//1893 2794//2794 1903//1903
+f 945//945 4845//4845 941//941
+f 945//945 1899//1899 1893//1893
+f 1893//1893 1903//1903 945//945
+f 1904//1904 1898//1898 1899//1899
+f 1893//1893 1899//1899 1894//1894
+f 1907//1907 1905//1905 1908//1908
+f 1908//1908 1905//1905 2954//2954
+f 1027//1027 1908//1908 2954//2954
+f 1906//1906 1035//1035 4660//4660
+f 4764//4764 1907//1907 1908//1908
+f 3510//3510 1029//1029 1909//1909
+f 1029//1029 1027//1027 1909//1909
+f 1359//1359 4088//4088 2284//2284
+f 1878//1878 1910//1910 1916//1916
+f 2803//2803 1911//1911 1913//1913
+f 1911//1911 1912//1912 1913//1913
+f 1912//1912 1914//1914 1913//1913
+f 1914//1914 1855//1855 1917//1917
+f 1913//1913 1914//1914 1917//1917
+f 1797//1797 2808//2808 1917//1917
+f 2808//2808 1915//1915 2731//2731
+f 1915//1915 1368//1368 2731//2731
+f 1915//1915 2808//2808 1797//1797
+f 1855//1855 1797//1797 1917//1917
+f 1910//1910 1911//1911 2803//2803
+f 1916//1916 1910//1910 2803//2803
+f 2804//2804 2803//2803 1913//1913
+f 1913//1913 1917//1917 2806//2806
+f 2804//2804 1913//1913 2806//2806
+f 1917//1917 2808//2808 2806//2806
+f 1729//1729 1924//1924 1713//1713
+f 577//577 1919//1919 1924//1924
+f 577//577 1918//1918 1919//1919
+f 546//546 1921//1921 545//545
+f 1919//1919 1923//1923 1924//1924
+f 1918//1918 1920//1920 1919//1919
+f 1921//1921 544//544 545//545
+f 1921//1921 1927//1927 544//544
+f 1928//1928 1927//1927 1921//1921
+f 2028//2028 1928//1928 1832//1832
+f 1713//1713 1923//1923 1922//1922
+f 1923//1923 1713//1713 1924//1924
+f 1919//1919 1925//1925 1923//1923
+f 1920//1920 1925//1925 1919//1919
+f 1920//1920 1926//1926 1925//1925
+f 1927//1927 735//735 544//544
+f 1927//1927 1929//1929 735//735
+f 1928//1928 1929//1929 1927//1927
+f 1928//1928 2028//2028 1929//1929
+f 1829//1829 1930//1930 2028//2028
+f 1829//1829 2023//2023 1930//1930
+f 2019//2019 4453//4453 1925//1925
+f 1925//1925 2020//2020 2019//2019
+f 1926//1926 2020//2020 1925//1925
+f 1926//1926 579//579 2020//2020
+f 4498//4498 1926//1926 1920//1920
+f 1931//1931 855//855 1932//1932
+f 1934//1934 1932//1932 1948//1948
+f 855//855 853//853 1939//1939
+f 1932//1932 855//855 1939//1939
+f 1933//1933 1956//1956 853//853
+f 2021//2021 1626//1626 1958//1958
+f 1969//1969 1950//1950 1954//1954
+f 1933//1933 890//890 1956//1956
+f 1953//1953 1937//1937 1955//1955
+f 1967//1967 1934//1934 1950//1950
+f 1970//1970 1967//1967 1950//1950
+f 1931//1931 1932//1932 1934//1934
+f 1950//1950 1934//1934 1948//1948
+f 157//157 1935//1935 1936//1936
+f 1935//1935 156//156 1936//1936
+f 156//156 1938//1938 1936//1936
+f 1936//1936 1938//1938 1951//1951
+f 1938//1938 1937//1937 1953//1953
+f 1951//1951 1938//1938 1953//1953
+f 853//853 1956//1956 1939//1939
+f 1940//1940 1941//1941 189//189
+f 1941//1941 1943//1943 1942//1942
+f 1942//1942 1943//1943 1944//1944
+f 155//155 157//157 1944//1944
+f 1943//1943 155//155 1944//1944
+f 157//157 1945//1945 1946//1946
+f 157//157 1946//1946 1944//1944
+f 157//157 1936//1936 1945//1945
+f 1936//1936 1951//1951 1945//1945
+f 1951//1951 1949//1949 1945//1945
+f 1949//1949 1951//1951 1952//1952
+f 885//885 1942//1942 1944//1944
+f 1946//1946 885//885 1944//1944
+f 1956//1956 1947//1947 1946//1946
+f 890//890 1947//1947 1956//1956
+f 1932//1932 1939//1939 1949//1949
+f 1948//1948 1949//1949 1952//1952
+f 1948//1948 1932//1932 1949//1949
+f 1948//1948 1952//1952 1954//1954
+f 1950//1950 1948//1948 1954//1954
+f 1950//1950 1969//1969 1970//1970
+f 1951//1951 1953//1953 1952//1952
+f 1952//1952 1953//1953 1954//1954
+f 1954//1954 1953//1953 1955//1955
+f 1969//1969 1954//1954 1955//1955
+f 1945//1945 1956//1956 1946//1946
+f 1939//1939 1956//1956 1945//1945
+f 1957//1957 2422//2422 1958//1958
+f 1939//1939 1945//1945 1949//1949
+f 1959//1959 1960//1960 1971//1971
+f 1960//1960 145//145 144//144
+f 1988//1988 1966//1966 1961//1961
+f 1973//1973 1965//1965 1988//1988
+f 1961//1961 1987//1987 1988//1988
+f 1960//1960 1959//1959 145//145
+f 1971//1971 1969//1969 1959//1959
+f 1971//1971 1960//1960 1968//1968
+f 132//132 1962//1962 1964//1964
+f 1960//1960 1963//1963 1972//1972
+f 1968//1968 1960//1960 1972//1972
+f 1961//1961 1966//1966 1963//1963
+f 144//144 1964//1964 1963//1963
+f 140//140 1959//1959 1955//1955
+f 140//140 1955//1955 1937//1937
+f 146//146 145//145 140//140
+f 140//140 145//145 1959//1959
+f 147//147 144//144 145//145
+f 1987//1987 1961//1961 1962//1962
+f 1964//1964 1961//1961 1963//1963
+f 1962//1962 1961//1961 1964//1964
+f 1960//1960 144//144 1963//1963
+f 1965//1965 837//837 1966//1966
+f 837//837 847//847 1966//1966
+f 1973//1973 845//845 1965//1965
+f 845//845 837//837 1965//1965
+f 1967//1967 1970//1970 1968//1968
+f 843//843 1967//1967 1968//1968
+f 846//846 845//845 1973//1973
+f 1969//1969 1955//1955 1959//1959
+f 1970//1970 1969//1969 1971//1971
+f 1970//1970 1971//1971 1968//1968
+f 843//843 1968//1968 1972//1972
+f 1966//1966 1988//1988 1965//1965
+f 847//847 843//843 1972//1972
+f 1963//1963 1966//1966 1972//1972
+f 1966//1966 847//847 1972//1972
+f 1983//1983 818//818 1977//1977
+f 1983//1983 1984//1984 830//830
+f 830//830 818//818 1983//1983
+f 846//846 1973//1973 1974//1974
+f 119//119 1992//1992 1981//1981
+f 121//121 119//119 1975//1975
+f 121//121 1975//1975 1989//1989
+f 132//132 1976//1976 1962//1962
+f 1986//1986 846//846 1974//1974
+f 2009//2009 830//830 1984//1984
+f 1987//1987 1979//1979 1989//1989
+f 1975//1975 1981//1981 1980//1980
+f 818//818 820//820 1977//1977
+f 1978//1978 1993//1993 1991//1991
+f 1976//1976 1987//1987 1962//1962
+f 1979//1979 121//121 1989//1989
+f 1985//1985 1981//1981 1982//1982
+f 1980//1980 1981//1981 1985//1985
+f 1991//1991 1977//1977 1982//1982
+f 1977//1977 1985//1985 1982//1982
+f 1993//1993 1983//1983 1991//1991
+f 1983//1983 1977//1977 1991//1991
+f 1984//1984 1983//1983 1993//1993
+f 820//820 821//821 1985//1985
+f 1977//1977 820//820 1985//1985
+f 821//821 1986//1986 1980//1980
+f 821//821 1980//1980 1985//1985
+f 1974//1974 1975//1975 1980//1980
+f 1986//1986 1974//1974 1980//1980
+f 1976//1976 1979//1979 1987//1987
+f 118//118 1978//1978 1991//1991
+f 1973//1973 1988//1988 1989//1989
+f 1974//1974 1973//1973 1989//1989
+f 1988//1988 1987//1987 1989//1989
+f 1975//1975 1974//1974 1989//1989
+f 1990//1990 1991//1991 1982//1982
+f 119//119 1981//1981 1975//1975
+f 1992//1992 1990//1990 1982//1982
+f 1981//1981 1992//1992 1982//1982
+f 1990//1990 118//118 1991//1991
+f 89//89 83//83 2012//2012
+f 63//63 89//89 2001//2001
+f 1993//1993 1978//1978 2001//2001
+f 1978//1978 63//63 2001//2001
+f 287//287 1994//1994 1995//1995
+f 1996//1996 1997//1997 1998//1998
+f 1996//1996 86//86 1997//1997
+f 2012//2012 83//83 1998//1998
+f 85//85 2000//2000 86//86
+f 83//83 1996//1996 1998//1998
+f 284//284 809//809 1999//1999
+f 1995//1995 284//284 1999//1999
+f 85//85 2017//2017 2000//2000
+f 89//89 2012//2012 2001//2001
+f 2002//2002 2003//2003 2004//2004
+f 2003//2003 2011//2011 2004//2004
+f 2005//2005 2013//2013 2014//2014
+f 803//803 2013//2013 2005//2005
+f 1999//1999 2014//2014 2016//2016
+f 2005//2005 1999//1999 2006//2006
+f 2009//2009 1984//1984 2010//2010
+f 2016//2016 287//287 1995//1995
+f 2007//2007 2016//2016 2015//2015
+f 1999//1999 2016//2016 1995//1995
+f 2007//2007 2015//2015 3308//3308
+f 2008//2008 2009//2009 2010//2010
+f 2004//2004 2008//2008 2010//2010
+f 2011//2011 2008//2008 2004//2004
+f 2012//2012 1998//1998 2004//2004
+f 2012//2012 2004//2004 2010//2010
+f 1998//1998 1997//1997 2002//2002
+f 1998//1998 2002//2002 2004//2004
+f 1984//1984 1993//1993 2001//2001
+f 1997//1997 2000//2000 2013//2013
+f 2002//2002 1997//1997 2013//2013
+f 2014//2014 2000//2000 2015//2015
+f 2013//2013 2000//2000 2014//2014
+f 2014//2014 2015//2015 2016//2016
+f 2000//2000 2017//2017 2015//2015
+f 2015//2015 2017//2017 3308//3308
+f 3308//3308 2017//2017 3302//3302
+f 2017//2017 84//84 3302//3302
+f 86//86 2000//2000 1997//1997
+f 1984//1984 2001//2001 2010//2010
+f 2001//2001 2012//2012 2010//2010
+f 1922//1922 2032//2032 2023//2023
+f 2033//2033 754//754 4511//4511
+f 753//753 2018//2018 754//754
+f 4453//4453 2025//2025 1923//1923
+f 1925//1925 4453//4453 1923//1923
+f 2020//2020 2026//2026 2019//2019
+f 2020//2020 584//584 2026//2026
+f 579//579 584//584 2020//2020
+f 844//844 850//850 4510//4510
+f 2035//2035 733//733 735//735
+f 1929//1929 2035//2035 735//735
+f 2330//2330 2021//2021 1958//1958
+f 4440//4440 4422//4422 2022//2022
+f 2023//2023 2024//2024 1930//1930
+f 2043//2043 2024//2024 2023//2023
+f 4510//4510 850//850 593//593
+f 4509//4509 844//844 4510//4510
+f 2025//2025 2032//2032 1922//1922
+f 2025//2025 4476//4476 2032//2032
+f 1923//1923 2025//2025 1922//1922
+f 2026//2026 4454//4454 2019//2019
+f 2026//2026 2027//2027 4454//4454
+f 584//584 2027//2027 2026//2026
+f 733//733 531//531 735//735
+f 1929//1929 2037//2037 2035//2035
+f 2028//2028 2037//2037 1929//1929
+f 2028//2028 2029//2029 2037//2037
+f 1930//1930 2029//2029 2028//2028
+f 2022//2022 4594//4594 4440//4440
+f 1930//1930 2040//2040 2029//2029
+f 2024//2024 2040//2040 1930//1930
+f 2023//2023 2031//2031 2043//2043
+f 2032//2032 2031//2031 2023//2023
+f 2030//2030 2031//2031 2032//2032
+f 2032//2032 2045//2045 2030//2030
+f 2033//2033 4511//4511 741//741
+f 4476//4476 2045//2045 2032//2032
+f 2019//2019 4455//4455 4453//4453
+f 2019//2019 4454//4454 4455//4455
+f 2027//2027 2034//2034 4454//4454
+f 731//731 733//733 730//730
+f 2035//2035 4462//4462 730//730
+f 2037//2037 4462//4462 2035//2035
+f 2038//2038 2036//2036 2037//2037
+f 2029//2029 2038//2038 2037//2037
+f 2029//2029 4466//4466 2038//2038
+f 2029//2029 2039//2039 4466//4466
+f 2040//2040 2039//2039 2029//2029
+f 2024//2024 2042//2042 2040//2040
+f 2043//2043 2042//2042 2024//2024
+f 2043//2043 2041//2041 2042//2042
+f 4470//4470 2043//2043 2031//2031
+f 4470//4470 2041//2041 2043//2043
+f 2031//2031 608//608 4470//4470
+f 2030//2030 608//608 2031//2031
+f 2030//2030 4472//4472 608//608
+f 2030//2030 2044//2044 4472//4472
+f 2045//2045 2044//2044 2030//2030
+f 2045//2045 4474//4474 2044//2044
+f 2045//2045 4475//4475 4474//4474
+f 4476//4476 4475//4475 2045//2045
+f 2046//2046 4501//4501 4476//4476
+f 164//164 2060//2060 2047//2047
+f 164//164 2047//2047 2048//2048
+f 2085//2085 164//164 2048//2048
+f 167//167 2054//2054 2056//2056
+f 2059//2059 167//167 2056//2056
+f 2047//2047 2050//2050 2048//2048
+f 2247//2247 4328//4328 4440//4440
+f 2047//2047 2049//2049 2071//2071
+f 2050//2050 2047//2047 2071//2071
+f 2051//2051 228//228 2074//2074
+f 2051//2051 2074//2074 2052//2052
+f 2055//2055 2052//2052 2053//2053
+f 2056//2056 2054//2054 2053//2053
+f 2054//2054 2055//2055 2053//2053
+f 2059//2059 2056//2056 2057//2057
+f 2049//2049 2058//2058 2057//2057
+f 2058//2058 2059//2059 2057//2057
+f 2060//2060 2058//2058 2049//2049
+f 2047//2047 2060//2060 2049//2049
+f 2053//2053 2052//2052 2068//2068
+f 2061//2061 2050//2050 2062//2062
+f 2061//2061 2062//2062 2063//2063
+f 2062//2062 2072//2072 2065//2065
+f 2063//2063 2062//2062 2065//2065
+f 2072//2072 2064//2064 2066//2066
+f 2065//2065 2072//2072 2066//2066
+f 2066//2066 2064//2064 2067//2067
+f 2064//2064 2068//2068 2067//2067
+f 2068//2068 2052//2052 2074//2074
+f 2067//2067 2068//2068 2074//2074
+f 2068//2068 2064//2064 2069//2069
+f 2071//2071 2049//2049 2070//2070
+f 2049//2049 2057//2057 2070//2070
+f 2057//2057 2056//2056 2069//2069
+f 2057//2057 2069//2069 2070//2070
+f 2056//2056 2053//2053 2069//2069
+f 2050//2050 2071//2071 2062//2062
+f 2062//2062 2071//2071 2072//2072
+f 2071//2071 2070//2070 2072//2072
+f 2072//2072 2070//2070 2064//2064
+f 2069//2069 2064//2064 2070//2070
+f 2053//2053 2068//2068 2069//2069
+f 2073//2073 2063//2063 2065//2065
+f 2066//2066 2073//2073 2065//2065
+f 16//16 2067//2067 2074//2074
+f 16//16 2066//2066 2067//2067
+f 2073//2073 2075//2075 2063//2063
+f 228//228 16//16 2074//2074
+f 42//42 228//228 2076//2076
+f 131//131 2081//2081 2080//2080
+f 131//131 2091//2091 2086//2086
+f 2422//2422 2330//2330 1958//1958
+f 2091//2091 2088//2088 2089//2089
+f 2080//2080 2088//2088 2091//2091
+f 2075//2075 2077//2077 2078//2078
+f 2063//2063 2075//2075 2078//2078
+f 2084//2084 2083//2083 2079//2079
+f 2081//2081 2084//2084 2079//2079
+f 2080//2080 2081//2081 2079//2079
+f 2079//2079 2082//2082 2087//2087
+f 2079//2079 2083//2083 2082//2082
+f 2082//2082 2083//2083 2098//2098
+f 2048//2048 2050//2050 2098//2098
+f 2098//2098 2050//2050 2061//2061
+f 2083//2083 2048//2048 2098//2098
+f 2061//2061 2063//2063 2078//2078
+f 2080//2080 2079//2079 2087//2087
+f 105//105 2089//2089 2095//2095
+f 2084//2084 2085//2085 2083//2083
+f 2085//2085 2048//2048 2083//2083
+f 148//148 2086//2086 2091//2091
+f 2080//2080 2087//2087 2088//2088
+f 131//131 2080//2080 2091//2091
+f 2089//2089 2090//2090 2091//2091
+f 2090//2090 148//148 2091//2091
+f 2099//2099 105//105 2095//2095
+f 2088//2088 2095//2095 2089//2089
+f 2185//2185 2556//2556 2642//2642
+f 2096//2096 2092//2092 2093//2093
+f 2093//2093 2092//2092 2100//2100
+f 2092//2092 2094//2094 2100//2100
+f 2094//2094 2078//2078 2100//2100
+f 2061//2061 2078//2078 2094//2094
+f 2093//2093 2095//2095 2096//2096
+f 2082//2082 2098//2098 2094//2094
+f 2092//2092 2082//2082 2094//2094
+f 2096//2096 2087//2087 2092//2092
+f 2087//2087 2082//2082 2092//2092
+f 2095//2095 2088//2088 2096//2096
+f 2088//2088 2087//2087 2096//2096
+f 2097//2097 4206//4206 4213//4213
+f 2098//2098 2061//2061 2094//2094
+f 3//3 2095//2095 2093//2093
+f 3//3 2099//2099 2095//2095
+f 2101//2101 2093//2093 2100//2100
+f 2101//2101 3//3 2093//2093
+f 2078//2078 2077//2077 2100//2100
+f 2077//2077 2101//2101 2100//2100
+f 2103//2103 2102//2102 3763//3763
+f 2102//2102 4826//4826 2279//2279
+f 2103//2103 4826//4826 2102//2102
+f 2280//2280 4705//4705 3936//3936
+f 2279//2279 4852//4852 2102//2102
+f 3763//3763 2102//2102 2104//2104
+f 3772//3772 2104//2104 4852//4852
+f 2102//2102 4852//4852 2104//2104
+f 2108//2108 3843//3843 2106//2106
+f 3518//3518 2105//2105 2776//2776
+f 3518//3518 2106//2106 2105//2105
+f 2106//2106 3843//3843 2107//2107
+f 3536//3536 2108//2108 3530//3530
+f 2109//2109 3539//3539 2110//2110
+f 4881//4881 3536//3536 2109//2109
+f 3539//3539 2111//2111 2110//2110
+f 2111//2111 3539//3539 3548//3548
+f 3542//3542 2111//2111 3548//3548
+f 3554//3554 2111//2111 3542//3542
+f 2422//2422 2331//2331 2330//2330
+f 4406//4406 2111//2111 4407//4407
+f 4757//4757 3813//3813 2112//2112
+f 2152//2152 3761//3761 2113//2113
+f 4883//4883 4882//4882 4844//4844
+f 2280//2280 3936//3936 2298//2298
+f 2110//2110 4406//4406 4812//4812
+f 2114//2114 2128//2128 2112//2112
+f 2293//2293 2298//2298 2306//2306
+f 2115//2115 2116//2116 4828//4828
+f 4605//4605 1213//1213 2117//2117
+f 4375//4375 2671//2671 2814//2814
+f 4891//4891 2160//2160 2118//2118
+f 4849//4849 4891//4891 4835//4835
+f 4835//4835 4891//4891 2118//2118
+f 2118//2118 4736//4736 2121//2121
+f 2118//2118 2160//2160 4736//4736
+f 3710//3710 4870//4870 2115//2115
+f 3258//3258 2119//2119 2125//2125
+f 2358//2358 4817//4817 2120//2120
+f 2292//2292 2358//2358 2120//2120
+f 4892//4892 4893//4893 2124//2124
+f 4893//4893 2121//2121 2122//2122
+f 4893//4893 2122//2122 2124//2124
+f 2122//2122 2123//2123 2124//2124
+f 4832//4832 2124//2124 2119//2119
+f 2124//2124 2126//2126 2119//2119
+f 2125//2125 2119//2119 2127//2127
+f 2119//2119 2126//2126 2127//2127
+f 4856//4856 2274//2274 2107//2107
+f 2121//2121 4736//4736 2128//2128
+f 2118//2118 2121//2121 4893//4893
+f 2121//2121 4733//4733 2122//2122
+f 2123//2123 2122//2122 2129//2129
+f 4400//4400 2130//2130 2129//2129
+f 2132//2132 2129//2129 2130//2130
+f 2123//2123 2132//2132 3264//3264
+f 2129//2129 2122//2122 4733//4733
+f 2128//2128 4733//4733 2121//2121
+f 4400//4400 2129//2129 4733//4733
+f 2123//2123 2129//2129 2132//2132
+f 2126//2126 2123//2123 3264//3264
+f 2126//2126 2124//2124 2123//2123
+f 1213//1213 2131//2131 2117//2117
+f 2132//2132 2130//2130 3266//3266
+f 3266//3266 2130//2130 4398//4398
+f 2130//2130 4400//4400 4398//4398
+f 2133//2133 3266//3266 4398//4398
+f 4398//4398 4740//4740 2133//2133
+f 2135//2135 2134//2134 3269//3269
+f 2159//2159 262//262 2144//2144
+f 2134//2134 2135//2135 262//262
+f 262//262 2135//2135 2143//2143
+f 2135//2135 2136//2136 2143//2143
+f 2134//2134 262//262 273//273
+f 2134//2134 275//275 3269//3269
+f 2135//2135 3269//3269 2966//2966
+f 4797//4797 2163//2163 2137//2137
+f 2137//2137 2162//2162 2144//2144
+f 2301//2301 2138//2138 2139//2139
+f 2140//2140 2141//2141 4797//4797
+f 2465//2465 2604//2604 4301//4301
+f 2688//2688 2139//2139 2304//2304
+f 4790//4790 2142//2142 2137//2137
+f 4790//4790 2144//2144 2143//2143
+f 2137//2137 2144//2144 4790//4790
+f 2142//2142 4797//4797 2137//2137
+f 2144//2144 262//262 2143//2143
+f 4869//4869 2107//2107 2274//2274
+f 2145//2145 2136//2136 4341//4341
+f 1626//1626 2117//2117 4371//4371
+f 2146//2146 4804//4804 2150//2150
+f 4804//4804 2147//2147 4590//4590
+f 4757//4757 4758//4758 2148//2148
+f 2149//2149 4803//4803 4804//4804
+f 2156//2156 4803//4803 2149//2149
+f 2152//2152 2150//2150 2151//2151
+f 2146//2146 2149//2149 4804//4804
+f 2150//2150 2152//2152 2286//2286
+f 2156//2156 2149//2149 2155//2155
+f 2152//2152 2113//2113 2286//2286
+f 4758//4758 2669//2669 2148//2148
+f 4594//4594 1220//1220 3641//3641
+f 2167//2167 2153//2153 2164//2164
+f 2167//2167 2166//2166 3568//3568
+f 2154//2154 2167//2167 3568//3568
+f 2153//2153 2167//2167 2154//2154
+f 4738//4738 2153//2153 2154//2154
+f 2155//2155 2168//2168 2156//2156
+f 4803//4803 2156//2156 2166//2166
+f 2156//2156 3568//3568 2166//2166
+f 2157//2157 2161//2161 2162//2162
+f 2161//2161 2158//2158 2162//2162
+f 2147//2147 4803//4803 2141//2141
+f 2140//2140 2147//2147 2141//2141
+f 2159//2159 2144//2144 2162//2162
+f 2141//2141 4803//4803 2166//2166
+f 2160//2160 2669//2669 4758//4758
+f 2163//2163 2162//2162 2137//2137
+f 2153//2153 4427//4427 2157//2157
+f 2153//2153 2157//2157 2164//2164
+f 2158//2158 2159//2159 2162//2162
+f 2158//2158 2161//2161 4857//4857
+f 2157//2157 4427//4427 2161//2161
+f 2165//2165 2164//2164 2163//2163
+f 2157//2157 2162//2162 2163//2163
+f 2157//2157 2163//2163 2164//2164
+f 2163//2163 4797//4797 2165//2165
+f 2167//2167 2164//2164 2165//2165
+f 2166//2166 2167//2167 2165//2165
+f 2141//2141 2166//2166 2165//2165
+f 4797//4797 2141//2141 2165//2165
+f 4427//4427 2169//2169 2161//2161
+f 2169//2169 4427//4427 4791//4791
+f 4696//4696 2154//2154 2168//2168
+f 4857//4857 2161//2161 2169//2169
+f 2490//2490 2481//2481 2491//2491
+f 2170//2170 4857//4857 2169//2169
+f 943//943 2171//2171 4874//4874
+f 4823//4823 4822//4822 4877//4877
+f 2176//2176 2174//2174 2195//2195
+f 2263//2263 2172//2172 2174//2174
+f 2174//2174 2173//2173 2195//2195
+f 2174//2174 4433//4433 2263//2263
+f 2669//2669 4009//4009 2282//2282
+f 2175//2175 2204//2204 2173//2173
+f 2172//2172 2196//2196 2175//2175
+f 2172//2172 2175//2175 2173//2173
+f 2174//2174 2172//2172 2173//2173
+f 4787//4787 4433//4433 2176//2176
+f 2263//2263 4433//4433 4430//4430
+f 4433//4433 2174//2174 2176//2176
+f 2176//2176 2177//2177 4787//4787
+f 2184//2184 2178//2178 2179//2179
+f 4204//4204 2790//2790 4740//4740
+f 2178//2178 2207//2207 2179//2179
+f 2179//2179 2207//2207 2198//2198
+f 2180//2180 4867//4867 2207//2207
+f 2198//2198 2207//2207 2200//2200
+f 23//23 2182//2182 2193//2193
+f 2182//2182 22//22 2190//2190
+f 2193//2193 2182//2182 2181//2181
+f 2182//2182 2190//2190 2181//2181
+f 22//22 27//27 2190//2190
+f 27//27 2183//2183 2190//2190
+f 2183//2183 2184//2184 2187//2187
+f 4686//4686 4685//4685 2783//2783
+f 2185//2185 2783//2783 2186//2186
+f 2201//2201 2183//2183 2187//2187
+f 2184//2184 2179//2179 2187//2187
+f 2193//2193 2189//2189 2188//2188
+f 2193//2193 2181//2181 2189//2189
+f 2189//2189 2202//2202 2188//2188
+f 2202//2202 2191//2191 2188//2188
+f 2181//2181 2190//2190 2201//2201
+f 2189//2189 2181//2181 2201//2201
+f 2190//2190 2183//2183 2201//2201
+f 2202//2202 2233//2233 2191//2191
+f 23//23 2193//2193 2192//2192
+f 2188//2188 2230//2230 2192//2192
+f 2193//2193 2188//2188 2192//2192
+f 2191//2191 2230//2230 2188//2188
+f 4872//4872 2195//2195 2206//2206
+f 2196//2196 2194//2194 2175//2175
+f 2205//2205 2198//2198 2204//2204
+f 2195//2195 2199//2199 2206//2206
+f 2196//2196 2233//2233 2194//2194
+f 2173//2173 2204//2204 2199//2199
+f 2195//2195 2173//2173 2199//2199
+f 2176//2176 2195//2195 4872//4872
+f 2160//2160 2197//2197 4007//4007
+f 2204//2204 2175//2175 2194//2194
+f 2179//2179 2198//2198 2205//2205
+f 2198//2198 2199//2199 2204//2204
+f 2198//2198 2200//2200 2199//2199
+f 2199//2199 2200//2200 2206//2206
+f 4867//4867 2206//2206 2200//2200
+f 2189//2189 2201//2201 2203//2203
+f 2202//2202 2189//2189 2203//2203
+f 2201//2201 2187//2187 2203//2203
+f 2203//2203 2187//2187 2205//2205
+f 2187//2187 2179//2179 2205//2205
+f 2194//2194 2233//2233 2203//2203
+f 2194//2194 2203//2203 2205//2205
+f 2204//2204 2194//2194 2205//2205
+f 2206//2206 4868//4868 4872//4872
+f 2180//2180 2207//2207 2178//2178
+f 2233//2233 2202//2202 2203//2203
+f 2236//2236 2208//2208 2213//2213
+f 2208//2208 2269//2269 2229//2229
+f 2229//2229 2209//2209 2208//2208
+f 2224//2224 2235//2235 2238//2238
+f 2229//2229 2235//2235 2224//2224
+f 2220//2220 2238//2238 2211//2211
+f 2224//2224 2238//2238 2220//2220
+f 2210//2210 2211//2211 2212//2212
+f 2220//2220 2211//2211 2210//2210
+f 2225//2225 2212//2212 2226//2226
+f 2210//2210 2212//2212 2225//2225
+f 2213//2213 2208//2208 2209//2209
+f 2236//2236 2217//2217 2266//2266
+f 2266//2266 2217//2217 2215//2215
+f 2214//2214 2216//2216 2215//2215
+f 2215//2215 2217//2217 2214//2214
+f 2216//2216 2276//2276 2215//2215
+f 2214//2214 2234//2234 2216//2216
+f 2216//2216 2234//2234 2262//2262
+f 2234//2234 2214//2214 2232//2232
+f 2214//2214 2217//2217 2227//2227
+f 2232//2232 2214//2214 2227//2227
+f 2236//2236 2213//2213 2217//2217
+f 2217//2217 2213//2213 2227//2227
+f 2218//2218 2226//2226 3491//3491
+f 2229//2229 2224//2224 2219//2219
+f 2220//2220 3151//3151 2221//2221
+f 2224//2224 2220//2220 2221//2221
+f 3151//3151 2210//2210 2222//2222
+f 2220//2220 2210//2210 3151//3151
+f 2222//2222 2225//2225 3150//3150
+f 2210//2210 2225//2225 2222//2222
+f 3150//3150 2218//2218 2223//2223
+f 2225//2225 2218//2218 3150//3150
+f 2223//2223 3497//3497 3148//3148
+f 2218//2218 3495//3495 2223//2223
+f 2223//2223 3495//3495 3497//3497
+f 2219//2219 2224//2224 2221//2221
+f 2225//2225 2226//2226 2218//2218
+f 2218//2218 3491//3491 3495//3495
+f 2232//2232 3155//3155 2231//2231
+f 3155//3155 2227//2227 2228//2228
+f 2232//2232 2227//2227 3155//3155
+f 2227//2227 2213//2213 2228//2228
+f 4689//4689 989//989 2892//2892
+f 2213//2213 2209//2209 2228//2228
+f 2892//2892 3815//3815 3643//3643
+f 4774//4774 4686//4686 2688//2688
+f 2209//2209 2229//2229 2219//2219
+f 2196//2196 2172//2172 2262//2262
+f 2172//2172 2263//2263 2262//2262
+f 2260//2260 3507//3507 3491//3491
+f 2226//2226 2260//2260 3491//3491
+f 2226//2226 2259//2259 2260//2260
+f 2212//2212 2259//2259 2226//2226
+f 2212//2212 2211//2211 2259//2259
+f 2211//2211 2258//2258 2259//2259
+f 2258//2258 2238//2238 2268//2268
+f 2230//2230 2232//2232 2231//2231
+f 2230//2230 2191//2191 2232//2232
+f 2191//2191 2233//2233 2234//2234
+f 2191//2191 2234//2234 2232//2232
+f 2233//2233 2196//2196 2234//2234
+f 2234//2234 2196//2196 2262//2262
+f 2235//2235 2269//2269 2268//2268
+f 2238//2238 2235//2235 2268//2268
+f 2229//2229 2269//2269 2235//2235
+f 2208//2208 2236//2236 2237//2237
+f 3619//3619 1236//1236 2284//2284
+f 2269//2269 2208//2208 2237//2237
+f 2236//2236 2266//2266 2237//2237
+f 2211//2211 2238//2238 2258//2258
+f 2250//2250 2244//2244 2239//2239
+f 2250//2250 2241//2241 2244//2244
+f 2244//2244 2256//2256 2239//2239
+f 2240//2240 2256//2256 2244//2244
+f 4772//4772 3153//3153 2672//2672
+f 2261//2261 2257//2257 2242//2242
+f 2244//2244 2271//2271 2240//2240
+f 2261//2261 2242//2242 2253//2253
+f 2241//2241 2253//2253 2242//2242
+f 2243//2243 2257//2257 2273//2273
+f 2257//2257 2268//2268 2273//2273
+f 2242//2242 2257//2257 2243//2243
+f 2244//2244 2241//2241 2271//2271
+f 2240//2240 2271//2271 2270//2270
+f 2271//2271 2241//2241 2245//2245
+f 2241//2241 2242//2242 2245//2245
+f 2242//2242 2243//2243 2245//2245
+f 2249//2249 3509//3509 2252//2252
+f 2629//2629 2631//2631 2246//2246
+f 2246//2246 4705//4705 2629//2629
+f 3619//3619 2247//2247 2248//2248
+f 2251//2251 2254//2254 2252//2252
+f 2249//2249 2252//2252 2250//2250
+f 3509//3509 2251//2251 2252//2252
+f 4272//4272 4792//4792 2790//2790
+f 2253//2253 2250//2250 2252//2252
+f 2254//2254 2253//2253 2252//2252
+f 2261//2261 2253//2253 2254//2254
+f 4705//4705 4184//4184 2629//2629
+f 2256//2256 2255//2255 2293//2293
+f 2249//2249 2239//2239 3504//3504
+f 2239//2239 2256//2256 3504//3504
+f 2250//2250 2239//2239 2249//2249
+f 2241//2241 2250//2250 2253//2253
+f 2216//2216 2262//2262 2264//2264
+f 2264//2264 4431//4431 2276//2276
+f 3641//3641 1220//1220 2248//2248
+f 2261//2261 2258//2258 2257//2257
+f 2258//2258 2268//2268 2257//2257
+f 2259//2259 2258//2258 2261//2261
+f 2260//2260 2261//2261 2254//2254
+f 2260//2260 2259//2259 2261//2261
+f 2251//2251 3507//3507 2254//2254
+f 3507//3507 2260//2260 2254//2254
+f 2262//2262 2263//2263 2264//2264
+f 2276//2276 2216//2216 2264//2264
+f 2215//2215 2276//2276 3579//3579
+f 2266//2266 3579//3579 2265//2265
+f 2266//2266 2215//2215 3579//3579
+f 2237//2237 2266//2266 2265//2265
+f 2237//2237 2265//2265 4801//4801
+f 2267//2267 4088//4088 2287//2287
+f 2268//2268 2269//2269 2273//2273
+f 2273//2273 4801//4801 4734//4734
+f 2273//2273 2269//2269 4801//4801
+f 2275//2275 4829//4829 4868//4868
+f 2790//2790 4351//4351 2789//2789
+f 2270//2270 2271//2271 2272//2272
+f 4809//4809 2270//2270 2272//2272
+f 2271//2271 2245//2245 2272//2272
+f 2272//2272 2245//2245 2277//2277
+f 2245//2245 2243//2243 2277//2277
+f 2243//2243 4734//4734 2277//2277
+f 2243//2243 2273//2273 4734//4734
+f 3777//3777 4848//4848 2274//2274
+f 4868//4868 2180//2180 2275//2275
+f 2276//2276 4431//4431 3579//3579
+f 4802//4802 2272//2272 2277//2277
+f 4847//4847 3773//3773 4834//4834
+f 4734//4734 2278//2278 2277//2277
+f 3773//3773 2279//2279 4834//4834
+f 4342//4342 4870//4870 4690//4690
+f 4340//4340 4190//4190 4259//4259
+f 4705//4705 4259//4259 4286//4286
+f 2149//2149 2146//2146 2294//2294
+f 2294//2294 2146//2146 2297//2297
+f 2146//2146 2150//2150 2297//2297
+f 2150//2150 2286//2286 2297//2297
+f 2280//2280 2299//2299 2281//2281
+f 2300//2300 2298//2298 2293//2293
+f 2298//2298 2299//2299 2280//2280
+f 2522//2522 2282//2282 2288//2288
+f 2283//2283 2255//2255 2240//2240
+f 2283//2283 2270//2270 4783//4783
+f 2270//2270 4809//4809 4783//4783
+f 1360//1360 2284//2284 2285//2285
+f 2240//2240 2270//2270 2283//2283
+f 2286//2286 2113//2113 2281//2281
+f 2286//2286 2281//2281 2295//2295
+f 2287//2287 1359//1359 1355//1355
+f 2288//2288 2672//2672 4251//4251
+f 4783//4783 2291//2291 2283//2283
+f 2289//2289 2290//2290 4866//4866
+f 4785//4785 2294//2294 2296//2296
+f 2291//2291 4785//4785 2296//2296
+f 3669//3669 4689//4689 3643//3643
+f 4783//4783 4785//4785 2291//2291
+f 2292//2292 4829//4829 2275//2275
+f 2305//2305 3504//3504 2293//2293
+f 2305//2305 2293//2293 2306//2306
+f 2155//2155 2149//2149 2294//2294
+f 2297//2297 2295//2295 2296//2296
+f 2296//2296 2295//2295 2300//2300
+f 2297//2297 2286//2286 2295//2295
+f 2293//2293 2255//2255 2300//2300
+f 2298//2298 2300//2300 2299//2299
+f 2295//2295 2299//2299 2300//2300
+f 2299//2299 2295//2295 2281//2281
+f 3504//3504 2256//2256 2293//2293
+f 2294//2294 2297//2297 2296//2296
+f 2255//2255 2283//2283 2291//2291
+f 2291//2291 2296//2296 2300//2300
+f 2256//2256 2240//2240 2255//2255
+f 2255//2255 2291//2291 2300//2300
+f 2301//2301 3844//3844 2302//2302
+f 4593//4593 4592//4592 2303//2303
+f 4251//4251 2304//2304 2522//2522
+f 2305//2305 2306//2306 2308//2308
+f 2305//2305 2308//2308 2307//2307
+f 2307//2307 2308//2308 3508//3508
+f 2310//2310 4581//4581 3517//3517
+f 2308//2308 2309//2309 2310//2310
+f 2310//2310 4189//4189 4581//4581
+f 3508//3508 2308//2308 2310//2310
+f 3607//3607 2333//2333 2314//2314
+f 3606//3606 4792//4792 4272//4272
+f 2315//2315 3761//3761 4799//4799
+f 2436//2436 2311//2311 4559//4559
+f 1902//1902 2311//2311 2312//2312
+f 2313//2313 2436//2436 4559//4559
+f 2314//2314 2333//2333 2323//2323
+f 2313//2313 1736//1736 2447//2447
+f 4587//4587 2319//2319 2322//2322
+f 2319//2319 2314//2314 2322//2322
+f 4761//4761 3606//3606 2315//2315
+f 2097//2097 2302//2302 2407//2407
+f 2324//2324 2316//2316 2317//2317
+f 2316//2316 2324//2324 2325//2325
+f 2318//2318 2316//2316 2325//2325
+f 4613//4613 2318//2318 2325//2325
+f 2247//2247 4088//4088 4328//4328
+f 2314//2314 3612//3612 3607//3607
+f 2314//2314 2319//2319 3612//3612
+f 4286//4286 4190//4190 2309//2309
+f 2319//2319 4588//4588 3604//3604
+f 2319//2319 3604//3604 3612//3612
+f 2318//2318 4642//4642 4587//4587
+f 2324//2324 2355//2355 2320//2320
+f 2334//2334 2346//2346 2321//2321
+f 2355//2355 2334//2334 2320//2320
+f 2320//2320 2334//2334 2321//2321
+f 2318//2318 2322//2322 2316//2316
+f 2322//2322 2323//2323 2316//2316
+f 2317//2317 2354//2354 2324//2324
+f 2317//2317 2316//2316 2323//2323
+f 2315//2315 4799//4799 4761//4761
+f 2320//2320 2321//2321 2326//2326
+f 2322//2322 2314//2314 2323//2323
+f 4726//4726 4547//4547 4256//4256
+f 2325//2325 2320//2320 2326//2326
+f 2325//2325 2324//2324 2320//2320
+f 2325//2325 2326//2326 4613//4613
+f 2332//2332 2317//2317 2333//2333
+f 2423//2423 2489//2489 2441//2441
+f 2324//2324 2354//2354 2355//2355
+f 4352//4352 2327//2327 2352//2352
+f 2327//2327 4353//4353 4356//4356
+f 4356//4356 2328//2328 2329//2329
+f 2330//2330 2331//2331 4608//4608
+f 2354//2354 2317//2317 2332//2332
+f 2327//2327 4352//4352 4353//4353
+f 2317//2317 2323//2323 2333//2333
+f 2334//2334 2337//2337 2335//2335
+f 2335//2335 2337//2337 2336//2336
+f 2336//2336 2337//2337 2338//2338
+f 2338//2338 2339//2339 2336//2336
+f 2336//2336 2339//2339 2350//2350
+f 2350//2350 2339//2339 2343//2343
+f 2350//2350 2343//2343 2344//2344
+f 2344//2344 2343//2343 2340//2340
+f 2341//2341 2356//2356 2338//2338
+f 2338//2338 2337//2337 2341//2341
+f 2339//2339 2338//2338 2342//2342
+f 2343//2343 2342//2342 2353//2353
+f 2343//2343 2339//2339 2342//2342
+f 4286//4286 2309//2309 3936//3936
+f 2340//2340 2343//2343 2353//2353
+f 2345//2345 2366//2366 2367//2367
+f 2340//2340 2366//2366 2345//2345
+f 2340//2340 2353//2353 2366//2366
+f 2350//2350 2344//2344 2351//2351
+f 2351//2351 2344//2344 3496//3496
+f 2344//2344 2345//2345 2348//2348
+f 3496//3496 2344//2344 2348//2348
+f 2349//2349 3112//3112 3499//3499
+f 2355//2355 2337//2337 2334//2334
+f 4282//4282 1212//1212 4608//4608
+f 2334//2334 2335//2335 2346//2346
+f 2340//2340 2345//2345 2344//2344
+f 2367//2367 2347//2347 2345//2345
+f 2345//2345 2347//2347 2348//2348
+f 2348//2348 2347//2347 2349//2349
+f 2336//2336 3494//3494 2335//2335
+f 2335//2335 3494//3494 2346//2346
+f 2336//2336 2350//2350 3494//3494
+f 2350//2350 2351//2351 3494//3494
+f 1957//1957 1074//1074 2422//2422
+f 2356//2356 2327//2327 2329//2329
+f 2328//2328 2363//2363 2329//2329
+f 4356//4356 2329//2329 2327//2327
+f 2352//2352 2333//2333 3607//3607
+f 2332//2332 2333//2333 2352//2352
+f 2352//2352 2327//2327 2356//2356
+f 2332//2332 2352//2352 2356//2356
+f 2353//2353 2342//2342 2329//2329
+f 2353//2353 2329//2329 2363//2363
+f 2366//2366 2353//2353 2363//2363
+f 2355//2355 2354//2354 2341//2341
+f 2337//2337 2355//2355 2341//2341
+f 2342//2342 2356//2356 2329//2329
+f 2332//2332 2356//2356 2341//2341
+f 2354//2354 2332//2332 2341//2341
+f 2338//2338 2356//2356 2342//2342
+f 2940//2940 3103//3103 4607//4607
+f 3104//3104 4544//4544 4655//4655
+f 3104//3104 2357//2357 3103//3103
+f 2358//2358 2292//2292 4831//4831
+f 4204//4204 4351//4351 2790//2790
+f 2365//2365 2359//2359 3110//3110
+f 3111//3111 2359//2359 4653//4653
+f 3110//3110 2359//2359 3111//3111
+f 3110//3110 3112//3112 2365//2365
+f 2365//2365 3112//3112 2349//2349
+f 3111//3111 4653//4653 2360//2360
+f 4602//4602 2362//2362 2361//2361
+f 4571//4571 4602//4602 2361//2361
+f 2362//2362 2367//2367 2361//2361
+f 2361//2361 2363//2363 2328//2328
+f 4571//4571 2361//2361 2364//2364
+f 2361//2361 2328//2328 2364//2364
+f 2365//2365 2349//2349 2362//2362
+f 4602//4602 2365//2365 2362//2362
+f 2359//2359 2365//2365 4602//4602
+f 4653//4653 2359//2359 4602//4602
+f 2367//2367 2366//2366 2363//2363
+f 2367//2367 2363//2363 2361//2361
+f 2347//2347 2367//2367 2362//2362
+f 2349//2349 2347//2347 2362//2362
+f 2368//2368 4703//4703 4355//4355
+f 4693//4693 2369//2369 4354//4354
+f 4703//4703 4693//4693 4354//4354
+f 2370//2370 4356//4356 2369//2369
+f 4354//4354 4355//4355 4703//4703
+f 4354//4354 2369//2369 4353//4353
+f 2370//2370 2328//2328 4356//4356
+f 4598//4598 4750//4750 4748//4748
+f 2371//2371 3624//3624 2372//2372
+f 2372//2372 3624//3624 2381//2381
+f 2372//2372 2381//2381 2374//2374
+f 4584//4584 4677//4677 2960//2960
+f 4677//4677 2373//2373 2960//2960
+f 4667//4667 2382//2382 2373//2373
+f 2372//2372 2374//2374 2382//2382
+f 2375//2375 4652//4652 2376//2376
+f 2306//2306 2298//2298 3936//3936
+f 2375//2375 2376//2376 2377//2377
+f 4799//4799 4805//4805 4761//4761
+f 2975//2975 2981//2981 2378//2378
+f 2375//2375 2377//2377 2380//2380
+f 2380//2380 2377//2377 2374//2374
+f 2379//2379 2377//2377 2376//2376
+f 2975//2975 2379//2379 2376//2376
+f 2378//2378 2379//2379 2975//2975
+f 3652//3652 3647//3647 2380//2380
+f 2380//2380 3647//3647 4661//4661
+f 3881//3881 4669//4669 3648//3648
+f 3652//3652 2380//2380 2374//2374
+f 2374//2374 2381//2381 3652//3652
+f 2382//2382 2377//2377 2379//2379
+f 2382//2382 2374//2374 2377//2377
+f 2378//2378 2373//2373 2379//2379
+f 2373//2373 2382//2382 2379//2379
+f 2960//2960 2373//2373 2378//2378
+f 4562//4562 2844//2844 2923//2923
+f 2923//2923 4558//4558 4562//4562
+f 4661//4661 3647//3647 4669//4669
+f 2407//2407 2784//2784 2642//2642
+f 4337//4337 2383//2383 4047//4047
+f 4047//4047 4237//4237 4337//4337
+f 2384//2384 2385//2385 4183//4183
+f 4031//4031 2385//2385 2387//2387
+f 4337//4337 4034//4034 2383//2383
+f 2385//2385 3002//3002 2386//2386
+f 2386//2386 4332//4332 2387//2387
+f 2387//2387 2385//2385 2386//2386
+f 4337//4337 4033//4033 4034//4034
+f 2388//2388 2418//2418 3272//3272
+f 2397//2397 4244//4244 2400//2400
+f 2389//2389 2397//2397 2400//2400
+f 2388//2388 3003//3003 2390//2390
+f 1957//1957 1958//1958 2022//2022
+f 2392//2392 3006//3006 3668//3668
+f 4336//4336 2410//2410 4390//4390
+f 2389//2389 2391//2391 2398//2398
+f 2392//2392 3668//3668 2412//2412
+f 4619//4619 2395//2395 4771//4771
+f 2409//2409 2396//2396 2398//2398
+f 3844//3844 2784//2784 2407//2407
+f 2625//2625 2622//2622 4614//4614
+f 2398//2398 4390//4390 2409//2409
+f 2622//2622 2625//2625 2609//2609
+f 4382//4382 2974//2974 2622//2622
+f 2395//2395 2393//2393 2394//2394
+f 2394//2394 4771//4771 2395//2395
+f 2398//2398 2397//2397 2389//2389
+f 3001//3001 2391//2391 2389//2389
+f 2396//2396 2397//2397 2398//2398
+f 2626//2626 4888//4888 2632//2632
+f 4888//4888 2626//2626 2627//2627
+f 2627//2627 2399//2399 2401//2401
+f 4331//4331 2414//2414 2413//2413
+f 4331//4331 2413//2413 2400//2400
+f 2400//2400 4244//4244 4331//4331
+f 2413//2413 4330//4330 2400//2400
+f 2389//2389 2400//2400 4330//4330
+f 2819//2819 2852//2852 4665//4665
+f 2404//2404 2401//2401 2399//2399
+f 4334//4334 2402//2402 2632//2632
+f 4596//4596 2403//2403 3041//3041
+f 2417//2417 2403//2403 4596//4596
+f 2403//2403 2417//2417 2457//2457
+f 2959//2959 2982//2982 2404//2404
+f 2968//2968 4793//4793 2405//2405
+f 4894//4894 2968//2968 2405//2405
+f 2979//2979 4896//4896 2983//2983
+f 2393//2393 2406//2406 2394//2394
+f 2642//2642 2097//2097 2407//2407
+f 2393//2393 4793//4793 2973//2973
+f 2982//2982 2983//2983 4896//4896
+f 3788//3788 2418//2418 2416//2416
+f 2417//2417 3788//3788 2416//2416
+f 4264//4264 2419//2419 2390//2390
+f 2390//2390 2419//2419 2388//2388
+f 2418//2418 2388//2388 2419//2419
+f 2419//2419 4264//4264 2453//2453
+f 2453//2453 4264//4264 4263//4263
+f 2991//2991 4549//4549 4548//4548
+f 2608//2608 2625//2625 2991//2991
+f 2408//2408 2409//2409 2410//2410
+f 2394//2394 2411//2411 4363//4363
+f 4390//4390 2410//2410 2409//2409
+f 2392//2392 2412//2412 2395//2395
+f 2413//2413 2414//2414 4361//4361
+f 2418//2418 2415//2415 2416//2416
+f 2457//2457 2417//2417 2416//2416
+f 2457//2457 2416//2416 2415//2415
+f 2418//2418 2419//2419 2415//2415
+f 2419//2419 2453//2453 2415//2415
+f 2430//2430 2428//2428 2446//2446
+f 2438//2438 2437//2437 2420//2420
+f 2437//2437 2443//2443 2449//2449
+f 2420//2420 2437//2437 2449//2449
+f 2424//2424 2463//2463 2432//2432
+f 2424//2424 2454//2454 2463//2463
+f 1902//1902 2312//2312 4252//4252
+f 4252//4252 2459//2459 2421//2421
+f 2422//2422 1074//1074 2331//2331
+f 2440//2440 2426//2426 2427//2427
+f 2446//2446 2428//2428 2423//2423
+f 2423//2423 2428//2428 2426//2426
+f 2428//2428 2430//2430 2454//2454
+f 2426//2426 2424//2424 2433//2433
+f 2428//2428 2424//2424 2426//2426
+f 2427//2427 2433//2433 2425//2425
+f 2427//2427 2426//2426 2433//2433
+f 2449//2449 2443//2443 2425//2425
+f 2443//2443 2427//2427 2425//2425
+f 2428//2428 2454//2454 2424//2424
+f 4289//4289 2429//2429 2430//2430
+f 2430//2430 2429//2429 2455//2455
+f 2454//2454 2430//2430 2455//2455
+f 2420//2420 2449//2449 2431//2431
+f 2449//2449 2425//2425 2448//2448
+f 2448//2448 2462//2462 2431//2431
+f 2425//2425 2433//2433 2450//2450
+f 2448//2448 2425//2425 2450//2450
+f 2424//2424 2432//2432 2433//2433
+f 2432//2432 2450//2450 2433//2433
+f 4252//4252 2312//2312 2459//2459
+f 2443//2443 2437//2437 2439//2439
+f 2459//2459 2312//2312 2435//2435
+f 2446//2446 2423//2423 2441//2441
+f 2312//2312 2436//2436 2434//2434
+f 2438//2438 2435//2435 2434//2434
+f 2435//2435 2312//2312 2434//2434
+f 2439//2439 2436//2436 2313//2313
+f 2436//2436 2439//2439 2434//2434
+f 2439//2439 2437//2437 2434//2434
+f 2439//2439 2313//2313 2447//2447
+f 2437//2437 2438//2438 2434//2434
+f 2443//2443 2439//2439 2447//2447
+f 1736//1736 2440//2440 2447//2447
+f 2423//2423 2440//2440 2489//2489
+f 2426//2426 2440//2440 2423//2423
+f 2446//2446 4283//4283 4289//4289
+f 2446//2446 2441//2441 4283//4283
+f 2312//2312 2311//2311 2436//2436
+f 2814//2814 2684//2684 4227//4227
+f 4397//4397 2442//2442 2472//2472
+f 2427//2427 2443//2443 2447//2447
+f 3819//3819 2444//2444 2445//2445
+f 2430//2430 2446//2446 4289//4289
+f 2440//2440 2427//2427 2447//2447
+f 4206//4206 2097//2097 2642//2642
+f 4293//4293 2420//2420 2431//2431
+f 2432//2432 2463//2463 2452//2452
+f 4010//4010 2438//2438 2420//2420
+f 4010//4010 3041//3041 2438//2438
+f 4296//4296 4230//4230 2468//2468
+f 2448//2448 2431//2431 2449//2449
+f 2462//2462 2448//2448 2469//2469
+f 2431//2431 2462//2462 2460//2460
+f 2448//2448 2450//2450 2469//2469
+f 2451//2451 2460//2460 2588//2588
+f 2451//2451 2431//2431 2460//2460
+f 2450//2450 2432//2432 2452//2452
+f 2451//2451 4293//4293 2431//2431
+f 2415//2415 2453//2453 2421//2421
+f 2459//2459 2415//2415 2421//2421
+f 2453//2453 4263//4263 2421//2421
+f 2455//2455 2429//2429 4291//4291
+f 3041//3041 2403//2403 2438//2438
+f 2463//2463 2454//2454 2458//2458
+f 2438//2438 2403//2403 2435//2435
+f 2458//2458 2455//2455 2456//2456
+f 2403//2403 2457//2457 2435//2435
+f 2454//2454 2455//2455 2458//2458
+f 4291//4291 2456//2456 2455//2455
+f 2457//2457 2415//2415 2459//2459
+f 2457//2457 2459//2459 2435//2435
+f 2588//2588 2460//2460 2461//2461
+f 2460//2460 2462//2462 2504//2504
+f 2461//2461 2460//2460 2504//2504
+f 2452//2452 2463//2463 2464//2464
+f 4184//4184 4705//4705 2280//2280
+f 2463//2463 2458//2458 2464//2464
+f 2458//2458 2456//2456 2487//2487
+f 2464//2464 2458//2458 2487//2487
+f 2481//2481 2604//2604 2491//2491
+f 2465//2465 2466//2466 2491//2491
+f 4291//4291 2506//2506 2467//2467
+f 2456//2456 4291//4291 2467//2467
+f 4659//4659 4296//4296 2468//2468
+f 2462//2462 2469//2469 2470//2470
+f 2504//2504 2462//2462 2470//2470
+f 2450//2450 2452//2452 2469//2469
+f 2470//2470 2469//2469 2471//2471
+f 2452//2452 2471//2471 2469//2469
+f 4230//4230 2472//2472 2473//2473
+f 2452//2452 2464//2464 2471//2471
+f 4664//4664 4659//4659 2474//2474
+f 2497//2497 2475//2475 2476//2476
+f 2495//2495 2477//2477 2478//2478
+f 2482//2482 2476//2476 2486//2486
+f 2497//2497 2476//2476 2482//2482
+f 2494//2494 2520//2520 2518//2518
+f 4252//4252 4030//4030 2971//2971
+f 2488//2488 2480//2480 2479//2479
+f 2520//2520 2479//2479 2480//2480
+f 2971//2971 4030//4030 2490//2490
+f 2481//2481 2490//2490 4325//4325
+f 2497//2497 2482//2482 2516//2516
+f 2483//2483 2497//2497 2516//2516
+f 2482//2482 2484//2484 2517//2517
+f 2483//2483 2487//2487 2485//2485
+f 2482//2482 2486//2486 2484//2484
+f 2505//2505 2484//2484 2562//2562
+f 2467//2467 2478//2478 2492//2492
+f 2507//2507 2478//2478 2467//2467
+f 2456//2456 2492//2492 2488//2488
+f 2467//2467 2492//2492 2456//2456
+f 2485//2485 2487//2487 2488//2488
+f 2487//2487 2456//2456 2488//2488
+f 2497//2497 2483//2483 2485//2485
+f 2494//2494 2479//2479 2520//2520
+f 2494//2494 2475//2475 2479//2479
+f 2479//2479 2475//2475 2485//2485
+f 2479//2479 2485//2485 2488//2488
+f 2562//2562 2484//2484 2600//2600
+f 2489//2489 2440//2440 1736//1736
+f 2480//2480 2488//2488 2492//2492
+f 2490//2490 2491//2491 2971//2971
+f 2498//2498 2492//2492 2478//2478
+f 2498//2498 2480//2480 2492//2492
+f 2493//2493 2499//2499 2486//2486
+f 2484//2484 2486//2486 2600//2600
+f 2499//2499 2600//2600 2486//2486
+f 2486//2486 2476//2476 2493//2493
+f 2493//2493 2476//2476 2494//2494
+f 2476//2476 2475//2475 2494//2494
+f 2478//2478 2507//2507 2495//2495
+f 2512//2512 2566//2566 2496//2496
+f 2497//2497 2485//2485 2475//2475
+f 2493//2493 2494//2494 2518//2518
+f 2496//2496 2493//2493 2518//2518
+f 2477//2477 2498//2498 2478//2478
+f 2600//2600 2499//2499 2601//2601
+f 2493//2493 2496//2496 2499//2499
+f 2477//2477 2500//2500 2501//2501
+f 4032//4032 4030//4030 4262//4262
+f 2470//2470 2517//2517 2592//2592
+f 2501//2501 2500//2500 2502//2502
+f 2538//2538 2501//2501 2502//2502
+f 2517//2517 2505//2505 2592//2592
+f 2480//2480 2498//2498 2514//2514
+f 2480//2480 2514//2514 2503//2503
+f 2514//2514 2538//2538 2515//2515
+f 2498//2498 2477//2477 2501//2501
+f 2516//2516 2482//2482 2517//2517
+f 2484//2484 2505//2505 2517//2517
+f 2498//2498 2501//2501 2514//2514
+f 2514//2514 2501//2501 2538//2538
+f 2593//2593 2461//2461 2504//2504
+f 2593//2593 2504//2504 2592//2592
+f 2505//2505 2561//2561 2592//2592
+f 2506//2506 2507//2507 2467//2467
+f 2471//2471 2464//2464 2483//2483
+f 2471//2471 2483//2483 2516//2516
+f 4283//4283 2441//2441 2508//2508
+f 2530//2530 4283//4283 2508//2508
+f 2509//2509 4273//4273 4039//4039
+f 2464//2464 2487//2487 2483//2483
+f 4267//4267 4274//4274 4273//4273
+f 4325//4325 3934//3934 2481//2481
+f 2539//2539 2511//2511 2519//2519
+f 2512//2512 2539//2539 2510//2510
+f 2518//2518 2511//2511 2496//2496
+f 2496//2496 2511//2511 2512//2512
+f 2511//2511 2539//2539 2512//2512
+f 2520//2520 2503//2503 2513//2513
+f 2513//2513 2503//2503 2536//2536
+f 2514//2514 2515//2515 2503//2503
+f 2503//2503 2515//2515 2536//2536
+f 2470//2470 2471//2471 2516//2516
+f 2470//2470 2516//2516 2517//2517
+f 2470//2470 2592//2592 2504//2504
+f 2518//2518 2520//2520 2513//2513
+f 2519//2519 2513//2513 2536//2536
+f 2511//2511 2518//2518 2513//2513
+f 2519//2519 2511//2511 2513//2513
+f 2520//2520 2480//2480 2503//2503
+f 4412//4412 2521//2521 2528//2528
+f 2481//2481 3934//3934 4151//4151
+f 3859//3859 4032//4032 3858//3858
+f 2288//2288 4251//4251 2522//2522
+f 2523//2523 2524//2524 2531//2531
+f 2521//2521 2535//2535 2537//2537
+f 2521//2521 2531//2531 2535//2535
+f 2528//2528 2537//2537 4317//4317
+f 2528//2528 2521//2521 2537//2537
+f 2525//2525 2526//2526 4276//4276
+f 2527//2527 2528//2528 4317//4317
+f 2529//2529 4387//4387 2532//2532
+f 2530//2530 3039//3039 2526//2526
+f 2521//2521 2523//2523 2531//2531
+f 2532//2532 2543//2543 2534//2534
+f 2531//2531 2524//2524 2529//2529
+f 2529//2529 2524//2524 4387//4387
+f 2529//2529 2532//2532 2534//2534
+f 2529//2529 2534//2534 2533//2533
+f 2535//2535 2531//2531 2533//2533
+f 2531//2531 2529//2529 2533//2533
+f 2519//2519 2533//2533 2534//2534
+f 2536//2536 2515//2515 2535//2535
+f 2536//2536 2535//2535 2533//2533
+f 2515//2515 2538//2538 2537//2537
+f 2535//2535 2515//2515 2537//2537
+f 1212//1212 4280//4280 4276//4276
+f 2537//2537 2538//2538 4317//4317
+f 2510//2510 2539//2539 2543//2543
+f 2538//2538 2502//2502 4317//4317
+f 2539//2539 2519//2519 2534//2534
+f 2543//2543 2539//2539 2534//2534
+f 2536//2536 2533//2533 2519//2519
+f 3603//3603 2246//2246 2631//2631
+f 2546//2546 2551//2551 4388//4388
+f 2547//2547 2546//2546 2540//2540
+f 2545//2545 2551//2551 2541//2541
+f 2546//2546 2547//2547 2542//2542
+f 4254//4254 4338//4338 4340//4340
+f 2543//2543 2532//2532 2545//2545
+f 2543//2543 2545//2545 2541//2541
+f 2551//2551 2544//2544 4388//4388
+f 2545//2545 2544//2544 2551//2551
+f 4388//4388 2540//2540 2546//2546
+f 4321//4321 1596//1596 2553//2553
+f 2547//2547 4321//4321 2553//2553
+f 2546//2546 2548//2548 2551//2551
+f 2542//2542 2552//2552 2548//2548
+f 2548//2548 2552//2552 2571//2571
+f 2550//2550 2548//2548 2571//2571
+f 2550//2550 2571//2571 2570//2570
+f 2541//2541 2550//2550 2549//2549
+f 2550//2550 2570//2570 2549//2549
+f 4387//4387 2544//2544 2532//2532
+f 2532//2532 2544//2544 2545//2545
+f 3845//3845 3270//3270 4387//4387
+f 2541//2541 2551//2551 2550//2550
+f 2546//2546 2542//2542 2548//2548
+f 2548//2548 2550//2550 2551//2551
+f 2542//2542 2547//2547 2555//2555
+f 2542//2542 2555//2555 2552//2552
+f 2547//2547 2553//2553 2554//2554
+f 2555//2555 2547//2547 2554//2554
+f 4206//4206 2556//2556 2557//2557
+f 2580//2580 2555//2555 2554//2554
+f 4665//4665 4333//4333 1233//1233
+f 2580//2580 2554//2554 2610//2610
+f 2510//2510 2541//2541 2549//2549
+f 2510//2510 2543//2543 2541//2541
+f 2578//2578 2565//2565 2563//2563
+f 2575//2575 2558//2558 2565//2565
+f 2578//2578 2575//2575 2565//2565
+f 2575//2575 2567//2567 2558//2558
+f 2496//2496 2566//2566 2499//2499
+f 2559//2559 2581//2581 2568//2568
+f 2567//2567 2559//2559 2568//2568
+f 2688//2688 2560//2560 4774//4774
+f 2568//2568 2581//2581 2576//2576
+f 2579//2579 2561//2561 2562//2562
+f 2579//2579 2578//2578 2563//2563
+f 2561//2561 2579//2579 2563//2563
+f 2561//2561 2563//2563 2593//2593
+f 2593//2593 2563//2563 2564//2564
+f 2565//2565 2586//2586 2563//2563
+f 2563//2563 2586//2586 2564//2564
+f 2565//2565 2558//2558 2596//2596
+f 2586//2586 2565//2565 2596//2596
+f 2558//2558 2567//2567 2602//2602
+f 2601//2601 2499//2499 2566//2566
+f 2512//2512 2549//2549 2566//2566
+f 2567//2567 2568//2568 2602//2602
+f 2568//2568 2576//2576 2603//2603
+f 2602//2602 2568//2568 2603//2603
+f 4326//4326 2569//2569 1233//1233
+f 2619//2619 2603//2603 2576//2576
+f 2579//2579 2601//2601 2573//2573
+f 2571//2571 2552//2552 2574//2574
+f 2571//2571 2574//2574 2572//2572
+f 2552//2552 2555//2555 2577//2577
+f 2574//2574 2552//2552 2577//2577
+f 2512//2512 2510//2510 2549//2549
+f 2990//2990 4340//4340 4324//4324
+f 2570//2570 2573//2573 2566//2566
+f 2570//2570 2571//2571 2572//2572
+f 2573//2573 2570//2570 2572//2572
+f 2601//2601 2566//2566 2573//2573
+f 2555//2555 2580//2580 2577//2577
+f 2574//2574 2578//2578 2572//2572
+f 2575//2575 2578//2578 2574//2574
+f 2575//2575 2574//2574 2577//2577
+f 2581//2581 2610//2610 2576//2576
+f 2567//2567 2575//2575 2577//2577
+f 2567//2567 2577//2577 2559//2559
+f 2578//2578 2579//2579 2573//2573
+f 2578//2578 2573//2573 2572//2572
+f 2577//2577 2580//2580 2559//2559
+f 2580//2580 2610//2610 2581//2581
+f 2559//2559 2580//2580 2581//2581
+f 2589//2589 2582//2582 2590//2590
+f 2993//2993 2583//2583 2584//2584
+f 2564//2564 2590//2590 2585//2585
+f 2583//2583 2599//2599 2605//2605
+f 2590//2590 2582//2582 2585//2585
+f 2586//2586 2596//2596 2594//2594
+f 2589//2589 2594//2594 2988//2988
+f 2590//2590 2586//2586 2594//2594
+f 2505//2505 2562//2562 2561//2561
+f 2601//2601 2579//2579 2562//2562
+f 2564//2564 2585//2585 2461//2461
+f 2585//2585 2587//2587 2588//2588
+f 2564//2564 2586//2586 2590//2590
+f 2461//2461 2585//2585 2588//2588
+f 2589//2589 2590//2590 2594//2594
+f 2591//2591 2597//2597 2599//2599
+f 2993//2993 2591//2591 2599//2599
+f 2592//2592 2561//2561 2593//2593
+f 2993//2993 2988//2988 2591//2591
+f 2594//2594 2596//2596 2591//2591
+f 2988//2988 2594//2594 2591//2591
+f 2583//2583 2605//2605 2595//2595
+f 2591//2591 2596//2596 2597//2597
+f 2597//2597 2602//2602 2605//2605
+f 2603//2603 2598//2598 2595//2595
+f 2605//2605 2603//2603 2595//2595
+f 2583//2583 2993//2993 2599//2599
+f 2596//2596 2558//2558 2597//2597
+f 2558//2558 2602//2602 2597//2597
+f 2562//2562 2600//2600 2601//2601
+f 2602//2602 2603//2603 2605//2605
+f 2595//2595 2598//2598 2607//2607
+f 2583//2583 2606//2606 2584//2584
+f 2583//2583 2595//2595 2606//2606
+f 2585//2585 2582//2582 2587//2587
+f 2606//2606 2608//2608 2991//2991
+f 2549//2549 2570//2570 2566//2566
+f 2593//2593 2564//2564 2461//2461
+f 2604//2604 2481//2481 4151//4151
+f 2599//2599 2597//2597 2605//2605
+f 2608//2608 2606//2606 2607//2607
+f 2606//2606 2595//2595 2607//2607
+f 2630//2630 3761//3761 2315//2315
+f 2613//2613 2621//2621 2647//2647
+f 2613//2613 2616//2616 2621//2621
+f 2612//2612 2644//2644 2611//2611
+f 2611//2611 2644//2644 2617//2617
+f 2607//2607 2598//2598 2618//2618
+f 2607//2607 2609//2609 2608//2608
+f 2607//2607 2618//2618 2609//2609
+f 2618//2618 2598//2598 2619//2619
+f 2603//2603 2619//2619 2598//2598
+f 2576//2576 2647//2647 2619//2619
+f 2576//2576 2610//2610 2647//2647
+f 2611//2611 2614//2614 2649//2649
+f 2611//2611 2649//2649 2612//2612
+f 2611//2611 2616//2616 2614//2614
+f 2616//2616 2613//2613 2614//2614
+f 2611//2611 2617//2617 2615//2615
+f 4382//4382 2616//2616 2615//2615
+f 2616//2616 2611//2611 2615//2615
+f 2618//2618 2623//2623 2609//2609
+f 2623//2623 2622//2622 2609//2609
+f 2617//2617 4394//4394 4384//4384
+f 2621//2621 2623//2623 2618//2618
+f 2615//2615 2617//2617 4384//4384
+f 2619//2619 2621//2621 2618//2618
+f 2647//2647 2621//2621 2619//2619
+f 4665//4665 4664//4664 4333//4333
+f 2629//2629 4184//4184 2620//2620
+f 2616//2616 4382//4382 2623//2623
+f 2401//2401 2982//2982 4896//4896
+f 2621//2621 2616//2616 2623//2623
+f 4382//4382 2622//2622 2623//2623
+f 4381//4381 4382//4382 2615//2615
+f 4888//4888 2627//2627 2401//2401
+f 2315//2315 3606//3606 2631//2631
+f 4566//4566 4198//4198 2624//2624
+f 2401//2401 2404//2404 2982//2982
+f 2608//2608 2609//2609 2625//2625
+f 4561//4561 4566//4566 1599//1599
+f 4560//4560 4561//4561 1599//1599
+f 2315//2315 2631//2631 2630//2630
+f 2445//2445 2444//2444 3672//3672
+f 2626//2626 2640//2640 2638//2638
+f 2627//2627 2626//2626 2638//2638
+f 2638//2638 2628//2628 2627//2627
+f 2627//2627 2628//2628 2399//2399
+f 2638//2638 2637//2637 2628//2628
+f 2630//2630 2629//2629 2620//2620
+f 4228//4228 2556//2556 2185//2185
+f 2630//2630 2631//2631 2629//2629
+f 2626//2626 2632//2632 2640//2640
+f 4616//4616 4560//4560 1619//1619
+f 2402//2402 2634//2634 2632//2632
+f 2634//2634 1056//1056 2633//2633
+f 2640//2640 2632//2632 2634//2634
+f 2635//2635 1056//1056 2634//2634
+f 2633//2633 1056//1056 2636//2636
+f 2633//2633 2636//2636 2637//2637
+f 3512//3512 4631//4631 2636//2636
+f 2636//2636 4631//4631 2637//2637
+f 2638//2638 2633//2633 2637//2637
+f 2638//2638 2640//2640 2633//2633
+f 4631//4631 2639//2639 2628//2628
+f 2640//2640 2634//2634 2633//2633
+f 2637//2637 4631//4631 2628//2628
+f 1056//1056 1062//1062 2636//2636
+f 1062//1062 3506//3506 3512//3512
+f 3510//3510 1909//1909 2641//2641
+f 2784//2784 2185//2185 2642//2642
+f 4560//4560 4616//4616 4611//4611
+f 2620//2620 3761//3761 2630//2630
+f 4421//4421 4274//4274 2643//2643
+f 1067//1067 1069//1069 4394//4394
+f 1069//1069 4538//4538 4384//4384
+f 2644//2644 4394//4394 2617//2617
+f 1067//1067 4394//4394 2644//2644
+f 388//388 2612//2612 2649//2649
+f 2652//2652 2648//2648 2646//2646
+f 2650//2650 2648//2648 2652//2652
+f 4616//4616 4421//4421 2643//2643
+f 2646//2646 2648//2648 2645//2645
+f 2654//2654 2645//2645 2653//2653
+f 2646//2646 2645//2645 2654//2654
+f 2654//2654 2653//2653 2553//2553
+f 4338//4338 4324//4324 4340//4340
+f 2647//2647 2653//2653 2645//2645
+f 2647//2647 2645//2645 2613//2613
+f 2648//2648 2613//2613 2645//2645
+f 2650//2650 2614//2614 2648//2648
+f 2614//2614 2613//2613 2648//2648
+f 2649//2649 2614//2614 2650//2650
+f 2651//2651 388//388 2649//2649
+f 2652//2652 2651//2651 2650//2650
+f 2651//2651 2649//2649 2650//2650
+f 2554//2554 2553//2553 2653//2653
+f 2647//2647 2610//2610 2653//2653
+f 2610//2610 2554//2554 2653//2653
+f 2652//2652 2646//2646 2655//2655
+f 2655//2655 2646//2646 1598//1598
+f 2646//2646 2654//2654 1598//1598
+f 2654//2654 2553//2553 1596//1596
+f 1598//1598 2654//2654 1596//1596
+f 4346//4346 2657//2657 2655//2655
+f 3865//3865 2527//2527 2656//2656
+f 3847//3847 434//434 2657//2657
+f 2657//2657 434//434 2655//2655
+f 434//434 2652//2652 2655//2655
+f 434//434 2651//2651 2652//2652
+f 2657//2657 4346//4346 3243//3243
+f 1199//1199 2658//2658 4023//4023
+f 2661//2661 2659//2659 1615//1615
+f 1614//1614 1201//1201 1615//1615
+f 1616//1616 2660//2660 2667//2667
+f 1201//1201 1202//1202 2661//2661
+f 2661//2661 4129//4129 4132//4132
+f 4132//4132 2663//2663 2659//2659
+f 1615//1615 2659//2659 2660//2660
+f 2661//2661 4132//4132 2659//2659
+f 2659//2659 2663//2663 2668//2668
+f 4129//4129 1202//1202 4124//4124
+f 2660//2660 2659//2659 2668//2668
+f 2661//2661 1202//1202 4129//4129
+f 1616//1616 1615//1615 2660//2660
+f 4301//4301 2604//2604 4150//4150
+f 2662//2662 1616//1616 2667//2667
+f 2667//2667 2660//2660 2668//2668
+f 2666//2666 2668//2668 2663//2663
+f 2696//2696 2682//2682 2664//2664
+f 2664//2664 2690//2690 2665//2665
+f 2667//2667 2666//2666 1620//1620
+f 2667//2667 2668//2668 2666//2666
+f 2666//2666 2678//2678 1620//1620
+f 2282//2282 2148//2148 2669//2669
+f 2696//2696 2677//2677 2670//2670
+f 2696//2696 2664//2664 2677//2677
+f 2662//2662 2667//2667 1620//1620
+f 2677//2677 4410//4410 2670//2670
+f 4410//4410 2677//2677 2675//2675
+f 4410//4410 1620//1620 2670//2670
+f 2671//2671 2672//2672 2288//2288
+f 2696//2696 2670//2670 2673//2673
+f 2673//2673 2670//2670 2678//2678
+f 2675//2675 2677//2677 2676//2676
+f 2675//2675 2704//2704 2674//2674
+f 2676//2676 2704//2704 2675//2675
+f 2676//2676 2665//2665 2704//2704
+f 2664//2664 2665//2665 2676//2676
+f 2677//2677 2664//2664 2676//2676
+f 1620//1620 2678//2678 2670//2670
+f 4125//4125 4127//4127 2679//2679
+f 4127//4127 2691//2691 2679//2679
+f 2687//2687 2680//2680 2697//2697
+f 2695//2695 2681//2681 2682//2682
+f 2681//2681 2695//2695 2683//2683
+f 2681//2681 2687//2687 2697//2697
+f 2695//2695 2682//2682 2692//2692
+f 2671//2671 2288//2288 4404//4404
+f 2681//2681 2697//2697 2690//2690
+f 4404//4404 2282//2282 4009//4009
+f 4146//4146 4152//4152 2694//2694
+f 2681//2681 2690//2690 2682//2682
+f 2678//2678 4125//4125 2679//2679
+f 2687//2687 2683//2683 2694//2694
+f 2687//2687 2681//2681 2683//2683
+f 2694//2694 4152//4152 2685//2685
+f 2687//2687 2694//2694 2685//2685
+f 2684//2684 2671//2671 4404//4404
+f 2685//2685 4152//4152 2686//2686
+f 2686//2686 4154//4154 2685//2685
+f 2680//2680 2687//2687 2685//2685
+f 2680//2680 2685//2685 4154//4154
+f 2304//2304 2139//2139 2138//2138
+f 2688//2688 4686//4686 2139//2139
+f 2694//2694 2693//2693 4146//4146
+f 2693//2693 2689//2689 4126//4126
+f 2664//2664 2682//2682 2690//2690
+f 4127//4127 4126//4126 2691//2691
+f 2691//2691 4126//4126 2689//2689
+f 2679//2679 2691//2691 2692//2692
+f 2695//2695 2689//2689 2683//2683
+f 2673//2673 2679//2679 2692//2692
+f 2678//2678 2666//2666 4125//4125
+f 2673//2673 2678//2678 2679//2679
+f 2694//2694 2689//2689 2693//2693
+f 2694//2694 2683//2683 2689//2689
+f 2691//2691 2695//2695 2692//2692
+f 2696//2696 2673//2673 2692//2692
+f 2691//2691 2689//2689 2695//2695
+f 2682//2682 2696//2696 2692//2692
+f 3878//3878 3887//3887 3880//3880
+f 2697//2697 2699//2699 2712//2712
+f 2698//2698 2706//2706 2714//2714
+f 2714//2714 2705//2705 3887//3887
+f 3726//3726 2707//2707 2708//2708
+f 2705//2705 2699//2699 2701//2701
+f 2699//2699 2705//2705 2706//2706
+f 3878//3878 3888//3888 3887//3887
+f 3887//3887 2700//2700 2714//2714
+f 3880//3880 3887//3887 2705//2705
+f 2680//2680 4154//4154 2701//2701
+f 2699//2699 2680//2680 2701//2701
+f 2697//2697 2680//2680 2699//2699
+f 2697//2697 2712//2712 2690//2690
+f 2714//2714 2700//2700 3726//3726
+f 2705//2705 2701//2701 3880//3880
+f 2699//2699 2706//2706 2712//2712
+f 2704//2704 2702//2702 2703//2703
+f 2704//2704 2719//2719 2674//2674
+f 2704//2704 2703//2703 2719//2719
+f 2714//2714 2706//2706 2705//2705
+f 2702//2702 2706//2706 2698//2698
+f 2707//2707 2711//2711 2708//2708
+f 2698//2698 2714//2714 2708//2708
+f 2719//2719 2703//2703 2721//2721
+f 2708//2708 2711//2711 2713//2713
+f 2721//2721 2713//2713 2722//2722
+f 2711//2711 2709//2709 2710//2710
+f 2721//2721 2703//2703 2713//2713
+f 2722//2722 2710//2710 3871//3871
+f 2722//2722 2711//2711 2710//2710
+f 2713//2713 2711//2711 2722//2722
+f 2665//2665 2712//2712 2702//2702
+f 2704//2704 2665//2665 2702//2702
+f 2665//2665 2690//2690 2712//2712
+f 2703//2703 2702//2702 2698//2698
+f 2698//2698 2708//2708 2713//2713
+f 2712//2712 2706//2706 2702//2702
+f 2703//2703 2698//2698 2713//2713
+f 2707//2707 2709//2709 2711//2711
+f 3726//3726 2708//2708 2714//2714
+f 1397//1397 2724//2724 2715//2715
+f 2718//2718 2716//2716 1397//1397
+f 2717//2717 2720//2720 2718//2718
+f 1397//1397 3871//3871 2724//2724
+f 3871//3871 1397//1397 2716//2716
+f 2717//2717 2718//2718 1397//1397
+f 2719//2719 2721//2721 2718//2718
+f 2720//2720 2719//2719 2718//2718
+f 2674//2674 2719//2719 2720//2720
+f 2721//2721 2722//2722 2716//2716
+f 2722//2722 3871//3871 2716//2716
+f 4151//4151 4150//4150 2604//2604
+f 2718//2718 2721//2721 2716//2716
+f 1365//1365 2726//2726 2728//2728
+f 3868//3868 2723//2723 2727//2727
+f 3742//3742 2784//2784 3844//3844
+f 3868//3868 2727//2727 2730//2730
+f 2724//2724 3871//3871 2730//2730
+f 2724//2724 1404//1404 1401//1401
+f 2727//2727 2725//2725 2730//2730
+f 2728//2728 2726//2726 2727//2727
+f 2726//2726 2725//2725 2727//2727
+f 1365//1365 2728//2728 2729//2729
+f 2723//2723 2728//2728 2727//2727
+f 2724//2724 2730//2730 1404//1404
+f 2725//2725 1404//1404 2730//2730
+f 2729//2729 2733//2733 2732//2732
+f 1371//1371 2810//2810 1368//1368
+f 2731//2731 1368//1368 2810//2810
+f 2732//2732 2734//2734 1372//1372
+f 1372//1372 1366//1366 2732//2732
+f 1371//1371 2737//2737 2810//2810
+f 1371//1371 1372//1372 2737//2737
+f 2729//2729 2728//2728 2733//2733
+f 1366//1366 2729//2729 2732//2732
+f 2731//2731 2810//2810 2811//2811
+f 2728//2728 2723//2723 2733//2733
+f 2732//2732 2733//2733 2734//2734
+f 2736//2736 2737//2737 2735//2735
+f 2810//2810 2737//2737 2736//2736
+f 1372//1372 2734//2734 2737//2737
+f 2737//2737 2734//2734 2735//2735
+f 3735//3735 2749//2749 2738//2738
+f 3874//3874 2738//2738 2741//2741
+f 3875//3875 2741//2741 2740//2740
+f 2741//2741 3875//3875 3874//3874
+f 3875//3875 2740//2740 2739//2739
+f 2733//2733 2723//2723 2739//2739
+f 2735//2735 2734//2734 2740//2740
+f 2735//2735 2740//2740 2742//2742
+f 2740//2740 2741//2741 2742//2742
+f 2738//2738 2749//2749 2741//2741
+f 2740//2740 2734//2734 2739//2739
+f 2741//2741 2749//2749 2742//2742
+f 2734//2734 2733//2733 2739//2739
+f 2743//2743 3735//3735 3736//3736
+f 2749//2749 3735//3735 2743//2743
+f 2735//2735 2742//2742 2744//2744
+f 2751//2751 2750//2750 2748//2748
+f 2745//2745 2746//2746 2750//2750
+f 2750//2750 2746//2746 2755//2755
+f 2746//2746 2747//2747 2757//2757
+f 2748//2748 2750//2750 2755//2755
+f 2746//2746 2745//2745 2747//2747
+f 2750//2750 2743//2743 2745//2745
+f 2745//2745 3736//3736 2747//2747
+f 2743//2743 3736//3736 2745//2745
+f 2749//2749 2743//2743 2751//2751
+f 2743//2743 2750//2750 2751//2751
+f 2751//2751 2748//2748 2807//2807
+f 2744//2744 2742//2742 2751//2751
+f 2744//2744 2751//2751 2807//2807
+f 2742//2742 2749//2749 2751//2751
+f 2139//2139 4686//4686 3742//3742
+f 2752//2752 2754//2754 2753//2753
+f 2752//2752 2753//2753 3743//3743
+f 2752//2752 2756//2756 2754//2754
+f 2760//2760 2763//2763 2805//2805
+f 2807//2807 2748//2748 2805//2805
+f 2748//2748 2755//2755 2760//2760
+f 2748//2748 2760//2760 2805//2805
+f 2755//2755 2746//2746 2754//2754
+f 2755//2755 2754//2754 2756//2756
+f 2760//2760 2755//2755 2756//2756
+f 2746//2746 2757//2757 2754//2754
+f 2754//2754 2757//2757 2753//2753
+f 4351//4351 4338//4338 2789//2789
+f 2759//2759 2767//2767 2758//2758
+f 2756//2756 2767//2767 2759//2759
+f 2759//2759 2762//2762 2760//2760
+f 2760//2760 2756//2756 2759//2759
+f 2759//2759 2761//2761 2762//2762
+f 2759//2759 2758//2758 2761//2761
+f 2773//2773 2768//2768 2765//2765
+f 2767//2767 2756//2756 2752//2752
+f 2762//2762 2800//2800 2763//2763
+f 2760//2760 2762//2762 2763//2763
+f 2752//2752 2765//2765 2767//2767
+f 2765//2765 2772//2772 2773//2773
+f 2768//2768 2773//2773 2764//2764
+f 2758//2758 2787//2787 2761//2761
+f 2758//2758 2766//2766 2780//2780
+f 2787//2787 2758//2758 2780//2780
+f 2765//2765 2766//2766 2767//2767
+f 2765//2765 2752//2752 2772//2772
+f 2765//2765 2768//2768 2766//2766
+f 2780//2780 2766//2766 2795//2795
+f 2758//2758 2767//2767 2766//2766
+f 2766//2766 2768//2768 2795//2795
+f 2752//2752 3743//3743 2772//2772
+f 3743//3743 2769//2769 2770//2770
+f 2772//2772 3743//3743 2770//2770
+f 2771//2771 2770//2770 3519//3519
+f 2776//2776 2771//2771 3519//3519
+f 2771//2771 2776//2776 2774//2774
+f 2770//2770 2769//2769 3529//3529
+f 3519//3519 2770//2770 3529//3529
+f 2771//2771 2772//2772 2770//2770
+f 2773//2773 2772//2772 2771//2771
+f 2773//2773 2774//2774 2764//2764
+f 2773//2773 2771//2771 2774//2774
+f 3710//3710 2775//2775 2777//2777
+f 2778//2778 2775//2775 4813//4813
+f 4813//4813 3623//3623 2778//2778
+f 2775//2775 2774//2774 2777//2777
+f 2777//2777 2776//2776 2105//2105
+f 2764//2764 2778//2778 2785//2785
+f 2778//2778 3623//3623 2785//2785
+f 2774//2774 2776//2776 2777//2777
+f 2764//2764 2774//2774 2775//2775
+f 2764//2764 2775//2775 2778//2778
+f 2777//2777 2105//2105 3710//3710
+f 2779//2779 2786//2786 3280//3280
+f 2768//2768 2785//2785 2795//2795
+f 2780//2780 2792//2792 2781//2781
+f 2780//2780 2795//2795 2792//2792
+f 2444//2444 2782//2782 2892//2892
+f 2783//2783 2784//2784 3742//3742
+f 2785//2785 3623//3623 2796//2796
+f 2795//2795 2785//2785 2796//2796
+f 2508//2508 1736//1736 3055//3055
+f 2779//2779 1903//1903 2786//2786
+f 2787//2787 2780//2780 2781//2781
+f 2781//2781 2788//2788 2793//2793
+f 2786//2786 1903//1903 2794//2794
+f 2789//2789 4272//4272 2790//2790
+f 2787//2787 2781//2781 2791//2791
+f 2792//2792 2788//2788 2781//2781
+f 2768//2768 2764//2764 2785//2785
+f 3623//3623 4887//4887 2796//2796
+f 3280//3280 2786//2786 2793//2793
+f 3742//3742 4686//4686 2783//2783
+f 4887//4887 2788//2788 2796//2796
+f 2788//2788 2792//2792 2796//2796
+f 2781//2781 2793//2793 2791//2791
+f 2788//2788 3280//3280 2793//2793
+f 2793//2793 2786//2786 2794//2794
+f 2795//2795 2796//2796 2792//2792
+f 2794//2794 2797//2797 2791//2791
+f 2791//2791 2793//2793 2794//2794
+f 2797//2797 2801//2801 2791//2791
+f 2801//2801 2798//2798 2761//2761
+f 2801//2801 2761//2761 2787//2787
+f 2798//2798 2799//2799 2800//2800
+f 2798//2798 2800//2800 2762//2762
+f 2761//2761 2798//2798 2762//2762
+f 2799//2799 1895//1895 2800//2800
+f 2801//2801 2787//2787 2791//2791
+f 2800//2800 1895//1895 2763//2763
+f 2763//2763 1895//1895 2802//2802
+f 1895//1895 1916//1916 2802//2802
+f 2804//2804 2805//2805 2802//2802
+f 2803//2803 2804//2804 2802//2802
+f 1916//1916 2803//2803 2802//2802
+f 2807//2807 2805//2805 2804//2804
+f 2805//2805 2763//2763 2802//2802
+f 2809//2809 2811//2811 2736//2736
+f 2804//2804 2806//2806 2809//2809
+f 2809//2809 2806//2806 2811//2811
+f 2744//2744 2807//2807 2809//2809
+f 2807//2807 2804//2804 2809//2809
+f 2736//2736 2735//2735 2744//2744
+f 2806//2806 2808//2808 2811//2811
+f 2744//2744 2809//2809 2736//2736
+f 2731//2731 2811//2811 2808//2808
+f 2810//2810 2736//2736 2811//2811
+f 2813//2813 1030//1030 1028//1028
+f 2812//2812 2813//2813 1028//1028
+f 4227//4227 4557//4557 2814//2814
+f 1028//1028 1021//1021 4585//4585
+f 2815//2815 1021//1021 1004//1004
+f 1004//1004 1003//1003 2815//2815
+f 4697//4697 1021//1021 2815//2815
+f 4697//4697 4691//4691 1021//1021
+f 2951//2951 2854//2854 2816//2816
+f 4728//4728 2853//2853 2951//2951
+f 4698//4698 2851//2851 4728//4728
+f 2816//2816 2854//2854 2945//2945
+f 2815//2815 2851//2851 4698//4698
+f 2815//2815 1003//1003 2851//2851
+f 2948//2948 2947//2947 2818//2818
+f 2855//2855 2945//2945 2854//2854
+f 2947//2947 2817//2817 2818//2818
+f 2817//2817 2937//2937 2818//2818
+f 2949//2949 2945//2945 2855//2855
+f 2951//2951 2856//2856 2854//2854
+f 2937//2937 2817//2817 2823//2823
+f 2923//2923 2844//2844 2852//2852
+f 2932//2932 2833//2833 2842//2842
+f 2932//2932 2822//2822 2825//2825
+f 2833//2833 2932//2932 2825//2825
+f 2833//2833 2825//2825 2862//2862
+f 2852//2852 2819//2819 2923//2923
+f 2823//2823 2817//2817 2820//2820
+f 2820//2820 2817//2817 2859//2859
+f 2823//2823 2820//2820 2821//2821
+f 2821//2821 2820//2820 2826//2826
+f 2859//2859 2824//2824 2820//2820
+f 2820//2820 2824//2824 2826//2826
+f 2822//2822 2930//2930 2821//2821
+f 2823//2823 2821//2821 2930//2930
+f 2826//2826 2824//2824 2861//2861
+f 2826//2826 2861//2861 2862//2862
+f 2862//2862 2825//2825 2826//2826
+f 2825//2825 2821//2821 2826//2826
+f 2825//2825 2822//2822 2821//2821
+f 2835//2835 2843//2843 2841//2841
+f 2836//2836 2911//2911 2907//2907
+f 2841//2841 2834//2834 2827//2827
+f 2836//2836 2907//2907 2835//2835
+f 2834//2834 2828//2828 2827//2827
+f 2827//2827 2828//2828 2864//2864
+f 2827//2827 2864//2864 2830//2830
+f 2832//2832 2830//2830 2829//2829
+f 2836//2836 2910//2910 2911//2911
+f 2833//2833 2862//2862 2834//2834
+f 2834//2834 2862//2862 2828//2828
+f 4562//4562 1902//1902 2466//2466
+f 2830//2830 2838//2838 2831//2831
+f 2838//2838 2832//2832 2847//2847
+f 2832//2832 2838//2838 2830//2830
+f 2833//2833 2834//2834 2843//2843
+f 2843//2843 2834//2834 2841//2841
+f 2835//2835 2841//2841 2836//2836
+f 2932//2932 2842//2842 2840//2840
+f 2836//2836 2831//2831 2910//2910
+f 2838//2838 2847//2847 2837//2837
+f 2831//2831 2838//2838 2910//2910
+f 2910//2910 2838//2838 2837//2837
+f 2842//2842 2839//2839 2840//2840
+f 2841//2841 2827//2827 2831//2831
+f 2836//2836 2841//2841 2831//2831
+f 2835//2835 2842//2842 2843//2843
+f 2842//2842 2833//2833 2843//2843
+f 2827//2827 2830//2830 2831//2831
+f 2842//2842 2835//2835 2839//2839
+f 2847//2847 2885//2885 2837//2837
+f 2466//2466 2465//2465 2844//2844
+f 2832//2832 2845//2845 2847//2847
+f 2466//2466 2844//2844 4562//4562
+f 2846//2846 2878//2878 2837//2837
+f 2847//2847 2845//2845 2885//2885
+f 2885//2885 2846//2846 2837//2837
+f 2837//2837 2878//2878 2913//2913
+f 2848//2848 2853//2853 2849//2849
+f 2848//2848 2858//2858 2853//2853
+f 2850//2850 1002//1002 2849//2849
+f 1002//1002 2848//2848 2849//2849
+f 2851//2851 2850//2850 2849//2849
+f 1003//1003 2850//2850 2851//2851
+f 4728//4728 2851//2851 2849//2849
+f 1012//1012 2850//2850 1003//1003
+f 2853//2853 4728//4728 2849//2849
+f 4665//4665 2852//2852 4666//4666
+f 2856//2856 2853//2853 2858//2858
+f 2951//2951 2853//2853 2856//2856
+f 2855//2855 2854//2854 2856//2856
+f 999//999 2859//2859 2857//2857
+f 2856//2856 2858//2858 2857//2857
+f 2855//2855 2856//2856 2857//2857
+f 999//999 2857//2857 2858//2858
+f 2859//2859 2817//2817 2947//2947
+f 996//996 999//999 2858//2858
+f 2947//2947 2949//2949 2857//2857
+f 2949//2949 2855//2855 2857//2857
+f 973//973 2861//2861 2860//2860
+f 2859//2859 2947//2947 2857//2857
+f 2824//2824 2859//2859 999//999
+f 2860//2860 2824//2824 999//999
+f 2861//2861 2824//2824 2860//2860
+f 972//972 2862//2862 2861//2861
+f 2862//2862 972//972 2828//2828
+f 2828//2828 2863//2863 2864//2864
+f 972//972 2863//2863 2828//2828
+f 2863//2863 988//988 2864//2864
+f 2864//2864 2829//2829 2830//2830
+f 2865//2865 2832//2832 2829//2829
+f 2869//2869 2866//2866 2867//2867
+f 2866//2866 4438//4438 2867//2867
+f 2868//2868 2867//2867 4438//4438
+f 4434//4434 2866//2866 2869//2869
+f 981//981 3462//3462 2870//2870
+f 2870//2870 2874//2874 981//981
+f 2875//2875 2874//2874 282//282
+f 2918//2918 2914//2914 2889//2889
+f 2914//2914 2879//2879 2889//2889
+f 2873//2873 2886//2886 2876//2876
+f 2873//2873 2876//2876 2872//2872
+f 2886//2886 2871//2871 2876//2876
+f 986//986 2876//2876 2871//2871
+f 2890//2890 2877//2877 2879//2879
+f 2872//2872 2887//2887 2873//2873
+f 2874//2874 2870//2870 2881//2881
+f 2877//2877 2887//2887 2879//2879
+f 2875//2875 2871//2871 980//980
+f 2876//2876 986//986 2872//2872
+f 2887//2887 2877//2877 2873//2873
+f 2877//2877 2890//2890 2878//2878
+f 2879//2879 2914//2914 2890//2890
+f 4666//4666 2852//2852 3074//3074
+f 2880//2880 3471//3471 2884//2884
+f 3471//3471 2882//2882 2884//2884
+f 2882//2882 3448//3448 3047//3047
+f 3473//3473 2883//2883 2881//2881
+f 2881//2881 2883//2883 2874//2874
+f 2882//2882 3047//3047 2888//2888
+f 2874//2874 2883//2883 282//282
+f 2870//2870 3454//3454 2881//2881
+f 2913//2913 2878//2878 2891//2891
+f 2882//2882 3471//3471 3448//3448
+f 3473//3473 2880//2880 2883//2883
+f 2880//2880 2887//2887 2883//2883
+f 2887//2887 2880//2880 2884//2884
+f 986//986 282//282 2872//2872
+f 282//282 2883//2883 2872//2872
+f 2886//2886 2873//2873 2846//2846
+f 2885//2885 2886//2886 2846//2846
+f 2879//2879 2887//2887 2884//2884
+f 2879//2879 2884//2884 2889//2889
+f 2873//2873 2877//2877 2846//2846
+f 2887//2887 2872//2872 2883//2883
+f 2882//2882 2888//2888 2889//2889
+f 2884//2884 2882//2882 2889//2889
+f 2890//2890 2914//2914 2891//2891
+f 2878//2878 2890//2890 2891//2891
+f 2846//2846 2877//2877 2878//2878
+f 2933//2933 2904//2904 2895//2895
+f 989//989 2444//2444 2892//2892
+f 3074//3074 2852//2852 2844//2844
+f 2893//2893 2899//2899 2917//2917
+f 2914//2914 2918//2918 2899//2899
+f 2904//2904 2933//2933 2934//2934
+f 2894//2894 2893//2893 2896//2896
+f 2905//2905 2897//2897 2895//2895
+f 2844//2844 2465//2465 3074//3074
+f 2905//2905 2894//2894 2897//2897
+f 2896//2896 2893//2893 2917//2917
+f 2897//2897 2894//2894 2896//2896
+f 2933//2933 2895//2895 2936//2936
+f 2898//2898 2894//2894 2911//2911
+f 2900//2900 2894//2894 2905//2905
+f 2903//2903 2900//2900 2905//2905
+f 2893//2893 2894//2894 2898//2898
+f 2893//2893 2902//2902 2899//2899
+f 2893//2893 2898//2898 2902//2902
+f 2900//2900 2911//2911 2894//2894
+f 2906//2906 2901//2901 2934//2934
+f 2898//2898 2913//2913 2902//2902
+f 2904//2904 2901//2901 2903//2903
+f 2901//2901 2904//2904 2934//2934
+f 2904//2904 2905//2905 2895//2895
+f 2904//2904 2903//2903 2905//2905
+f 2932//2932 2840//2840 2931//2931
+f 2900//2900 2903//2903 2907//2907
+f 2903//2903 2901//2901 2839//2839
+f 2907//2907 2903//2903 2839//2839
+f 2901//2901 2906//2906 2840//2840
+f 2839//2839 2901//2901 2840//2840
+f 2840//2840 2906//2906 2931//2931
+f 2907//2907 2911//2911 2900//2900
+f 2888//2888 3047//3047 3051//3051
+f 2922//2922 3051//3051 3048//3048
+f 2922//2922 3048//3048 2921//2921
+f 2918//2918 2921//2921 2908//2908
+f 2908//2908 2921//2921 2909//2909
+f 2916//2916 2908//2908 3035//3035
+f 2908//2908 2909//2909 3035//3035
+f 2888//2888 3051//3051 2922//2922
+f 2918//2918 2917//2917 2899//2899
+f 2910//2910 2837//2837 2913//2913
+f 2898//2898 2910//2910 2913//2913
+f 2835//2835 2907//2907 2839//2839
+f 2911//2911 2910//2910 2898//2898
+f 2920//2920 2912//2912 3102//3102
+f 2889//2889 2888//2888 2922//2922
+f 2913//2913 2891//2891 2902//2902
+f 2891//2891 2899//2899 2902//2902
+f 2891//2891 2914//2914 2899//2899
+f 2936//2936 2924//2924 2915//2915
+f 2895//2895 2897//2897 2924//2924
+f 2936//2936 2895//2895 2924//2924
+f 2924//2924 2897//2897 2919//2919
+f 2897//2897 2896//2896 2919//2919
+f 2919//2919 2896//2896 2916//2916
+f 2916//2916 2896//2896 2917//2917
+f 2917//2917 2918//2918 2908//2908
+f 2912//2912 2919//2919 2916//2916
+f 2912//2912 2920//2920 2919//2919
+f 2908//2908 2916//2916 2917//2917
+f 2924//2924 2920//2920 2915//2915
+f 2921//2921 2918//2918 2922//2922
+f 2889//2889 2922//2922 2918//2918
+f 2923//2923 2819//2819 1345//1345
+f 2920//2920 2924//2924 2919//2919
+f 2940//2940 2915//2915 2920//2920
+f 2925//2925 2938//2938 2926//2926
+f 2939//2939 2925//2925 2927//2927
+f 2928//2928 4603//4603 2929//2929
+f 2929//2929 4568//4568 2927//2927
+f 2925//2925 2929//2929 2927//2927
+f 2938//2938 2930//2930 2926//2926
+f 2938//2938 2823//2823 2930//2930
+f 2937//2937 2823//2823 2938//2938
+f 2930//2930 2822//2822 2931//2931
+f 2822//2822 2932//2932 2931//2931
+f 2929//2929 2925//2925 2935//2935
+f 2928//2928 2929//2929 2935//2935
+f 2935//2935 2925//2925 2926//2926
+f 2933//2933 2928//2928 2935//2935
+f 2934//2934 2933//2933 2935//2935
+f 2906//2906 2934//2934 2935//2935
+f 2906//2906 2935//2935 2926//2926
+f 2931//2931 2906//2906 2926//2926
+f 2933//2933 2936//2936 2928//2928
+f 1345//1345 4558//4558 2923//2923
+f 2936//2936 2915//2915 4626//4626
+f 2936//2936 4626//4626 2928//2928
+f 2930//2930 2931//2931 2926//2926
+f 2937//2937 2938//2938 2946//2946
+f 2938//2938 2939//2939 2946//2946
+f 2939//2939 2938//2938 2925//2925
+f 2939//2939 2927//2927 4606//4606
+f 2940//2940 4607//4607 2915//2915
+f 4617//4617 4782//4782 4676//4676
+f 4650//4650 2941//2941 2942//2942
+f 2943//2943 4589//4589 4574//4574
+f 2942//2942 2941//2941 2948//2948
+f 2942//2942 2948//2948 2946//2946
+f 2941//2941 2945//2945 2949//2949
+f 2941//2941 4650//4650 2944//2944
+f 2945//2945 2941//2941 2944//2944
+f 2945//2945 2944//2944 2816//2816
+f 2816//2816 2944//2944 4707//4707
+f 2818//2818 2937//2937 2946//2946
+f 2948//2948 2818//2818 2946//2946
+f 2947//2947 2948//2948 2949//2949
+f 2948//2948 2941//2941 2949//2949
+f 3074//3074 4296//4296 4666//4666
+f 4650//4650 2950//2950 4706//4706
+f 2944//2944 4650//4650 4706//4706
+f 2939//2939 2942//2942 2946//2946
+f 2951//2951 2816//2816 4730//4730
+f 3002//3002 3001//3001 2389//2389
+f 2952//2952 2641//2641 2953//2953
+f 1027//1027 2954//2954 1909//1909
+f 1909//1909 2954//2954 2641//2641
+f 3764//3764 4660//4660 3765//3765
+f 1035//1035 2812//2812 3765//3765
+f 1035//1035 2813//2813 2812//2812
+f 1905//1905 2953//2953 2641//2641
+f 2954//2954 1905//1905 2641//2641
+f 1905//1905 1907//1907 2955//2955
+f 2953//2953 1905//1905 2955//2955
+f 1907//1907 4764//4764 3764//3764
+f 2955//2955 1907//1907 4729//4729
+f 2953//2953 2955//2955 4598//4598
+f 4598//4598 2955//2955 4711//4711
+f 2956//2956 2992//2992 4227//4227
+f 4629//4629 2639//2639 4632//4632
+f 2639//2639 4631//4631 4632//4632
+f 4629//4629 4632//4632 4633//4633
+f 2962//2962 2957//2957 2964//2964
+f 2958//2958 2982//2982 2959//2959
+f 2957//2957 2958//2958 2959//2959
+f 2399//2399 2628//2628 2639//2639
+f 2961//2961 2960//2960 2378//2378
+f 4672//4672 2961//2961 2986//2986
+f 2962//2962 2984//2984 2958//2958
+f 2961//2961 2963//2963 2986//2986
+f 2962//2962 2964//2964 2986//2986
+f 4634//4634 2986//2986 2964//2964
+f 2957//2957 2962//2962 2958//2958
+f 2639//2639 4629//4629 2965//2965
+f 4629//4629 2957//2957 2959//2959
+f 2965//2965 4629//4629 2959//2959
+f 2404//2404 2965//2965 2959//2959
+f 2136//2136 2966//2966 4341//4341
+f 2399//2399 2639//2639 2965//2965
+f 2404//2404 2399//2399 2965//2965
+f 2968//2968 2980//2980 2967//2967
+f 2968//2968 4894//4894 2980//2980
+f 2967//2967 2980//2980 2977//2977
+f 2969//2969 2967//2967 2970//2970
+f 2967//2967 4419//4419 2970//2970
+f 2969//2969 2970//2970 2997//2997
+f 2984//2984 2985//2985 2983//2983
+f 2979//2979 4894//4894 4896//4896
+f 2985//2985 2979//2979 2983//2983
+f 2466//2466 2971//2971 2491//2491
+f 4894//4894 2979//2979 2980//2980
+f 2393//2393 2973//2973 2406//2406
+f 2973//2973 2972//2972 2406//2406
+f 4793//4793 2969//2969 2973//2973
+f 4793//4793 2968//2968 2969//2969
+f 2968//2968 2967//2967 2969//2969
+f 2969//2969 2997//2997 2998//2998
+f 2973//2973 2969//2969 2998//2998
+f 2972//2972 2973//2973 2998//2998
+f 4614//4614 2622//2622 2974//2974
+f 2376//2376 4652//4652 2978//2978
+f 2975//2975 2978//2978 2976//2976
+f 2975//2975 2376//2376 2978//2978
+f 4652//4652 4545//4545 2977//2977
+f 2378//2378 2981//2981 2961//2961
+f 2975//2975 2976//2976 2981//2981
+f 2961//2961 4672//4672 2960//2960
+f 2976//2976 2985//2985 2981//2981
+f 2985//2985 2963//2963 2981//2981
+f 2980//2980 2976//2976 2978//2978
+f 2979//2979 2985//2985 2976//2976
+f 4652//4652 2977//2977 2978//2978
+f 2977//2977 2980//2980 2978//2978
+f 2979//2979 2976//2976 2980//2980
+f 2967//2967 2977//2977 4419//4419
+f 2981//2981 2963//2963 2961//2961
+f 2982//2982 2958//2958 2983//2983
+f 2625//2625 4614//4614 4549//4549
+f 2958//2958 2984//2984 2983//2983
+f 2984//2984 2962//2962 2963//2963
+f 2984//2984 2963//2963 2985//2985
+f 2986//2986 2963//2963 2962//2962
+f 2996//2996 2988//2988 2987//2987
+f 2988//2988 2993//2993 2987//2987
+f 2589//2589 2996//2996 4575//4575
+f 2989//2989 3007//3007 2994//2994
+f 4548//4548 3008//3008 3007//3007
+f 2989//2989 4548//4548 3007//3007
+f 2587//2587 2582//2582 4576//4576
+f 2582//2582 2589//2589 4575//4575
+f 2589//2589 2988//2988 2996//2996
+f 2992//2992 4807//4807 4625//4625
+f 2995//2995 2994//2994 4615//4615
+f 2990//2990 4190//4190 4340//4340
+f 2584//2584 2991//2991 4548//4548
+f 4227//4227 2992//2992 4625//4625
+f 2684//2684 2956//2956 4227//4227
+f 2987//2987 2993//2993 2989//2989
+f 2993//2993 2584//2584 2989//2989
+f 2987//2987 2994//2994 2995//2995
+f 2987//2987 2989//2989 2994//2994
+f 2989//2989 2584//2584 4548//4548
+f 3881//3881 4270//4270 4547//4547
+f 2606//2606 2991//2991 2584//2584
+f 2996//2996 2987//2987 2995//2995
+f 4732//4732 4402//4402 4890//4890
+f 2380//2380 4661//4661 2375//2375
+f 2997//2997 2970//2970 4609//4609
+f 2970//2970 4322//4322 4609//4609
+f 2998//2998 2997//2997 3000//3000
+f 2972//2972 2998//2998 2999//2999
+f 2998//2998 3000//3000 2999//2999
+f 2997//2997 4609//4609 3000//3000
+f 2406//2406 2972//2972 2411//2411
+f 2406//2406 2411//2411 2394//2394
+f 2384//2384 3001//3001 3002//3002
+f 2451//2451 2588//2588 2587//2587
+f 4858//4858 3003//3003 2388//2388
+f 4858//4858 2388//2388 3272//3272
+f 3002//3002 2385//2385 2384//2384
+f 3004//3004 4367//4367 4359//4359
+f 4609//4609 4615//4615 3000//3000
+f 3000//3000 4615//4615 3009//3009
+f 3272//3272 2418//2418 3788//3788
+f 1075//1075 4615//4615 4609//4609
+f 4359//4359 3005//3005 4373//4373
+f 3008//3008 4363//4363 3010//3010
+f 1051//1051 1065//1065 3006//3006
+f 3009//3009 3007//3007 3010//3010
+f 3007//3007 3008//3008 3010//3010
+f 4615//4615 2994//2994 3009//3009
+f 2994//2994 3007//3007 3009//3009
+f 4576//4576 4109//4109 4304//4304
+f 2972//2972 2999//2999 2411//2411
+f 1049//1049 3006//3006 4327//4327
+f 2411//2411 2999//2999 3010//3010
+f 4363//4363 2411//2411 3010//3010
+f 2999//2999 3000//3000 3009//3009
+f 2999//2999 3009//3009 3010//3010
+f 3011//3011 3016//3016 3030//3030
+f 3012//3012 3030//3030 3031//3031
+f 3011//3011 3030//3030 3012//3012
+f 3044//3044 3031//3031 3024//3024
+f 3016//3016 3046//3046 3068//3068
+f 3046//3046 3013//3013 3072//3072
+f 3068//3068 3046//3046 3072//3072
+f 3013//3013 3020//3020 3014//3014
+f 3072//3072 3013//3013 3014//3014
+f 3020//3020 3018//3018 3029//3029
+f 3014//3014 3020//3020 3029//3029
+f 3018//3018 3465//3465 3015//3015
+f 3012//3012 3031//3031 3044//3044
+f 3046//3046 3016//3016 3017//3017
+f 3013//3013 3050//3050 3049//3049
+f 3013//3013 3046//3046 3050//3050
+f 3020//3020 3049//3049 3052//3052
+f 3020//3020 3013//3013 3049//3049
+f 3018//3018 3052//3052 3019//3019
+f 3018//3018 3019//3019 3465//3465
+f 3018//3018 3020//3020 3052//3052
+f 3021//3021 3022//3022 3114//3114
+f 3021//3021 3023//3023 3022//3022
+f 3044//3044 3024//3024 3023//3023
+f 3023//3023 3024//3024 3022//3022
+f 3465//3465 3463//3463 3025//3025
+f 3072//3072 3014//3014 3026//3026
+f 3014//3014 3029//3029 3027//3027
+f 3026//3026 3014//3014 3027//3027
+f 3029//3029 3015//3015 3070//3070
+f 3027//3027 3029//3029 3070//3070
+f 3015//3015 3469//3469 3070//3070
+f 3028//3028 3078//3078 3099//3099
+f 3098//3098 3028//3028 3099//3099
+f 3029//3029 3018//3018 3015//3015
+f 3022//3022 3024//3024 3098//3098
+f 3024//3024 3031//3031 3028//3028
+f 3098//3098 3024//3024 3028//3028
+f 3031//3031 3030//3030 3073//3073
+f 3028//3028 3031//3031 3073//3073
+f 3073//3073 3030//3030 3032//3032
+f 3030//3030 3016//3016 3032//3032
+f 3016//3016 3068//3068 3032//3032
+f 3033//3033 2909//2909 3036//3036
+f 2909//2909 2921//2921 3038//3038
+f 3034//3034 3035//3035 3042//3042
+f 3036//3036 2909//2909 3038//3038
+f 3033//3033 3043//3043 3042//3042
+f 3035//3035 3033//3033 3042//3042
+f 3033//3033 3036//3036 3037//3037
+f 3038//3038 2921//2921 3048//3048
+f 1345//1345 3039//3039 3055//3055
+f 3055//3055 1736//1736 3040//3040
+f 3049//3049 3047//3047 3053//3053
+f 3003//3003 4858//4858 4540//4540
+f 3102//3102 2940//2940 2920//2920
+f 2912//2912 2916//2916 3034//3034
+f 3041//3041 4010//4010 3144//3144
+f 2916//2916 3035//3035 3034//3034
+f 3035//3035 2909//2909 3033//3033
+f 3043//3043 3033//3033 3037//3037
+f 3021//3021 2912//2912 3034//3034
+f 3023//3023 3034//3034 3042//3042
+f 3023//3023 3021//3021 3034//3034
+f 3044//3044 3042//3042 3043//3043
+f 3044//3044 3023//3023 3042//3042
+f 3012//3012 3043//3043 3037//3037
+f 3012//3012 3044//3044 3043//3043
+f 3011//3011 3037//3037 3045//3045
+f 3011//3011 3012//3012 3037//3037
+f 3017//3017 3016//3016 3045//3045
+f 3016//3016 3011//3011 3045//3045
+f 3046//3046 3017//3017 3050//3050
+f 3036//3036 3038//3038 3045//3045
+f 3037//3037 3036//3036 3045//3045
+f 3047//3047 3049//3049 3051//3051
+f 3017//3017 3038//3038 3048//3048
+f 3038//3038 3017//3017 3045//3045
+f 3050//3050 3048//3048 3051//3051
+f 3050//3050 3017//3017 3048//3048
+f 3049//3049 3050//3050 3051//3051
+f 3052//3052 3049//3049 3053//3053
+f 3052//3052 3053//3053 3019//3019
+f 3066//3066 3054//3054 3026//3026
+f 3194//3194 3065//3065 3061//3061
+f 3194//3194 3066//3066 3065//3065
+f 1345//1345 3055//3055 3040//3040
+f 3064//3064 3066//3066 3194//3194
+f 3056//3056 3054//3054 3076//3076
+f 3054//3054 3058//3058 3076//3076
+f 3054//3054 3056//3056 3026//3026
+f 3060//3060 3057//3057 3201//3201
+f 3058//3058 3054//3054 3064//3064
+f 3076//3076 3058//3058 3077//3077
+f 3201//3201 3157//3157 3100//3100
+f 3040//3040 4559//4559 4558//4558
+f 3061//3061 3067//3067 3059//3059
+f 3201//3201 3100//3100 3060//3060
+f 3067//3067 3062//3062 3059//3059
+f 3061//3061 3065//3065 3067//3067
+f 3059//3059 3062//3062 3063//3063
+f 3054//3054 3066//3066 3064//3064
+f 3065//3065 3066//3066 3069//3069
+f 3065//3065 3069//3069 3071//3071
+f 3067//3067 3071//3071 3467//3467
+f 3067//3067 3065//3065 3071//3071
+f 3062//3062 3067//3067 3467//3467
+f 4558//4558 1345//1345 3040//3040
+f 3068//3068 3056//3056 3076//3076
+f 3056//3056 3072//3072 3026//3026
+f 3026//3026 3027//3027 3066//3066
+f 3027//3027 3070//3070 3069//3069
+f 3066//3066 3027//3027 3069//3069
+f 3070//3070 3469//3469 3071//3071
+f 3069//3069 3070//3070 3071//3071
+f 3467//3467 3469//3469 3474//3474
+f 3071//3071 3469//3469 3467//3467
+f 3068//3068 3072//3072 3056//3056
+f 3073//3073 3075//3075 3078//3078
+f 3028//3028 3073//3073 3078//3078
+f 3073//3073 3032//3032 3075//3075
+f 4301//4301 4296//4296 3074//3074
+f 3075//3075 3032//3032 3076//3076
+f 3032//3032 3068//3068 3076//3076
+f 2465//2465 4301//4301 3074//3074
+f 3060//3060 3078//3078 3075//3075
+f 3078//3078 3060//3060 3100//3100
+f 3077//3077 3075//3075 3076//3076
+f 3060//3060 3075//3075 3077//3077
+f 3077//3077 3058//3058 3057//3057
+f 3099//3099 3078//3078 3100//3100
+f 3057//3057 3060//3060 3077//3077
+f 3083//3083 3087//3087 3152//3152
+f 3500//3500 3499//3499 3080//3080
+f 3079//3079 3082//3082 3487//3487
+f 3500//3500 3080//3080 3081//3081
+f 3080//3080 3096//3096 3105//3105
+f 3102//3102 3113//3113 3108//3108
+f 3082//3082 3085//3085 3084//3084
+f 3086//3086 3087//3087 3083//3083
+f 3082//3082 3084//3084 3487//3487
+f 3084//3084 3085//3085 3488//3488
+f 3086//3086 3085//3085 3089//3089
+f 3085//3085 3086//3086 3488//3488
+f 3087//3087 3086//3086 3089//3089
+f 3087//3087 3089//3089 3088//3088
+f 3093//3093 3087//3087 3088//3088
+f 3089//3089 3085//3085 3107//3107
+f 3089//3089 3107//3107 3109//3109
+f 3088//3088 3089//3089 3109//3109
+f 3088//3088 3109//3109 3090//3090
+f 3097//3097 3088//3088 3114//3114
+f 3088//3088 3090//3090 3114//3114
+f 3088//3088 3097//3097 3093//3093
+f 3096//3096 3115//3115 3106//3106
+f 3105//3105 3096//3096 3106//3106
+f 3091//3091 3092//3092 3093//3093
+f 3081//3081 3105//3105 3079//3079
+f 3152//3152 3093//3093 3092//3092
+f 3152//3152 3087//3087 3093//3093
+f 3081//3081 3080//3080 3105//3105
+f 3106//3106 3115//3115 3094//3094
+f 3093//3093 3097//3097 3091//3091
+f 3091//3091 3097//3097 3095//3095
+f 3101//3101 3091//3091 3095//3095
+f 3091//3091 3101//3101 3092//3092
+f 4544//4544 3104//3104 3115//3115
+f 3096//3096 4544//4544 3115//3115
+f 3111//3111 4544//4544 3096//3096
+f 3114//3114 3022//3022 3097//3097
+f 3080//3080 3110//3110 3096//3096
+f 3115//3115 3104//3104 3116//3116
+f 3098//3098 3099//3099 3095//3095
+f 3097//3097 3098//3098 3095//3095
+f 3095//3095 3100//3100 3101//3101
+f 3099//3099 3100//3100 3095//3095
+f 3022//3022 3098//3098 3097//3097
+f 2940//2940 3102//3102 3116//3116
+f 3103//3103 2940//2940 3116//3116
+f 3104//3104 3103//3103 3116//3116
+f 3082//3082 3105//3105 3106//3106
+f 3082//3082 3079//3079 3105//3105
+f 3082//3082 3106//3106 3107//3107
+f 3107//3107 3106//3106 3094//3094
+f 3085//3085 3082//3082 3107//3107
+f 3109//3109 3107//3107 3094//3094
+f 3109//3109 3094//3094 3108//3108
+f 3113//3113 3090//3090 3108//3108
+f 2912//2912 3021//3021 3113//3113
+f 3090//3090 3109//3109 3108//3108
+f 3102//3102 2912//2912 3113//3113
+f 3113//3113 3114//3114 3090//3090
+f 3110//3110 3111//3111 3096//3096
+f 3499//3499 3112//3112 3080//3080
+f 3112//3112 3110//3110 3080//3080
+f 3148//3148 3086//3086 3083//3083
+f 3086//3086 3148//3148 3488//3488
+f 3113//3113 3021//3021 3114//3114
+f 3100//3100 3157//3157 3101//3101
+f 3115//3115 3116//3116 3094//3094
+f 3094//3094 3116//3116 3108//3108
+f 3102//3102 3108//3108 3116//3116
+f 3129//3129 3157//3157 3117//3117
+f 3119//3119 3117//3117 3118//3118
+f 3121//3121 3119//3119 3118//3118
+f 3127//3127 3121//3121 3120//3120
+f 3121//3121 3118//3118 3120//3120
+f 3127//3127 3120//3120 3122//3122
+f 3124//3124 3127//3127 3122//3122
+f 3123//3123 3124//3124 3197//3197
+f 3124//3124 3122//3122 3197//3197
+f 3129//3129 3117//3117 3119//3119
+f 3126//3126 3123//3123 3125//3125
+f 3125//3125 3123//3123 3139//3139
+f 3124//3124 3126//3126 3134//3134
+f 3124//3124 3123//3123 3126//3126
+f 3127//3127 3134//3134 3128//3128
+f 3127//3127 3124//3124 3134//3134
+f 3121//3121 3128//3128 3136//3136
+f 3121//3121 3127//3127 3128//3128
+f 3119//3119 3136//3136 3137//3137
+f 3119//3119 3121//3121 3136//3136
+f 3129//3129 3119//3119 3137//3137
+f 3140//3140 3130//3130 3131//3131
+f 3130//3130 3140//3140 3143//3143
+f 3125//3125 3131//3131 3156//3156
+f 3125//3125 3140//3140 3131//3131
+f 3126//3126 3156//3156 3132//3132
+f 3126//3126 3125//3125 3156//3156
+f 3134//3134 3132//3132 3149//3149
+f 3134//3134 3126//3126 3132//3132
+f 3128//3128 3149//3149 3133//3133
+f 3128//3128 3134//3134 3149//3149
+f 3136//3136 3133//3133 3135//3135
+f 3136//3136 3128//3128 3133//3133
+f 3137//3137 3136//3136 3135//3135
+f 4445//4445 3138//3138 4447//4447
+f 3139//3139 3200//3200 3140//3140
+f 3125//3125 3139//3139 3140//3140
+f 3205//3205 3203//3203 3143//3143
+f 3143//3143 3140//3140 3205//3205
+f 3147//3147 3146//3146 4447//4447
+f 3138//3138 3143//3143 3147//3147
+f 3138//3138 3147//3147 4447//4447
+f 3138//3138 3142//3142 3154//3154
+f 2231//2231 3141//3141 4446//4446
+f 3142//3142 4445//4445 3141//3141
+f 3138//3138 4445//4445 3142//3142
+f 3130//3130 3143//3143 3154//3154
+f 3143//3143 3138//3138 3154//3154
+f 3152//3152 3092//3092 3129//3129
+f 3152//3152 3129//3129 3137//3137
+f 3092//3092 3101//3101 3129//3129
+f 3129//3129 3101//3101 3157//3157
+f 4293//4293 4599//4599 3144//3144
+f 3144//3144 3145//3145 3041//3041
+f 3146//3146 3147//3147 3203//3203
+f 3143//3143 3203//3203 3147//3147
+f 3148//3148 3083//3083 3135//3135
+f 2223//2223 3148//3148 3135//3135
+f 3133//3133 3150//3150 3135//3135
+f 3150//3150 2223//2223 3135//3135
+f 3149//3149 2222//2222 3133//3133
+f 2222//2222 3150//3150 3133//3133
+f 3132//3132 3151//3151 3149//3149
+f 3083//3083 3152//3152 3137//3137
+f 3083//3083 3137//3137 3135//3135
+f 4772//4772 2560//2560 3153//3153
+f 2231//2231 3155//3155 3142//3142
+f 3130//3130 2209//2209 3131//3131
+f 2209//2209 2219//2219 3131//3131
+f 2228//2228 3130//3130 3154//3154
+f 2228//2228 2209//2209 3130//3130
+f 2231//2231 4446//4446 2230//2230
+f 2230//2230 4446//4446 2192//2192
+f 3123//3123 3197//3197 3139//3139
+f 3142//3142 3155//3155 3154//3154
+f 3155//3155 2228//2228 3154//3154
+f 2231//2231 3142//3142 3141//3141
+f 3131//3131 2219//2219 3156//3156
+f 3156//3156 2221//2221 3132//3132
+f 3151//3151 2222//2222 3149//3149
+f 2221//2221 3151//3151 3132//3132
+f 2219//2219 2221//2221 3156//3156
+f 3140//3140 3200//3200 3205//3205
+f 3188//3188 3165//3165 3163//3163
+f 3165//3165 3188//3188 3189//3189
+f 3189//3189 3178//3178 3166//3166
+f 3165//3165 3189//3189 3166//3166
+f 3178//3178 3160//3160 3158//3158
+f 3157//3157 3201//3201 3206//3206
+f 3166//3166 3178//3178 3158//3158
+f 3160//3160 3181//3181 3159//3159
+f 3158//3158 3160//3160 3159//3159
+f 3161//3161 3163//3163 3235//3235
+f 3063//3063 3374//3374 3059//3059
+f 3161//3161 3235//3235 3231//3231
+f 3188//3188 3163//3163 3161//3161
+f 3181//3181 3167//3167 3191//3191
+f 3191//3191 3162//3162 3223//3223
+f 3164//3164 3163//3163 3165//3165
+f 3211//3211 3164//3164 3165//3165
+f 3165//3165 3166//3166 3211//3211
+f 3166//3166 3158//3158 3210//3210
+f 3211//3211 3166//3166 3210//3210
+f 3158//3158 3159//3159 3380//3380
+f 3210//3210 3158//3158 3380//3380
+f 3159//3159 3181//3181 3191//3191
+f 3162//3162 3167//3167 3184//3184
+f 3191//3191 3167//3167 3162//3162
+f 3184//3184 3168//3168 3221//3221
+f 3162//3162 3184//3184 3221//3221
+f 3168//3168 3193//3193 3222//3222
+f 3221//3221 3168//3168 3222//3222
+f 3193//3193 3168//3168 3185//3185
+f 3175//3175 3177//3177 3187//3187
+f 3169//3169 3207//3207 3190//3190
+f 3177//3177 3169//3169 3190//3190
+f 3170//3170 3180//3180 3207//3207
+f 3190//3190 3207//3207 3179//3179
+f 3170//3170 3219//3219 3180//3180
+f 3207//3207 3180//3180 3179//3179
+f 3219//3219 3171//3171 3182//3182
+f 3180//3180 3219//3219 3182//3182
+f 3171//3171 3215//3215 3183//3183
+f 3182//3182 3171//3171 3183//3183
+f 3185//3185 3215//3215 3202//3202
+f 3172//3172 3231//3231 3229//3229
+f 3229//3229 3238//3238 3195//3195
+f 3174//3174 3176//3176 3173//3173
+f 3172//3172 3174//3174 3173//3173
+f 3175//3175 3187//3187 3176//3176
+f 3176//3176 3187//3187 3173//3173
+f 3177//3177 3175//3175 3169//3169
+f 3215//3215 3185//3185 3183//3183
+f 3178//3178 3189//3189 3177//3177
+f 3160//3160 3190//3190 3179//3179
+f 3160//3160 3178//3178 3190//3190
+f 3181//3181 3179//3179 3180//3180
+f 3181//3181 3160//3160 3179//3179
+f 3167//3167 3180//3180 3182//3182
+f 3167//3167 3181//3181 3180//3180
+f 3184//3184 3182//3182 3183//3183
+f 3184//3184 3167//3167 3182//3182
+f 3185//3185 3168//3168 3183//3183
+f 3168//3168 3184//3184 3183//3183
+f 3186//3186 3193//3193 3185//3185
+f 3186//3186 3185//3185 3202//3202
+f 3161//3161 3172//3172 3173//3173
+f 3161//3161 3231//3231 3172//3172
+f 3187//3187 3188//3188 3173//3173
+f 3188//3188 3161//3161 3173//3173
+f 3177//3177 3189//3189 3187//3187
+f 3189//3189 3188//3188 3187//3187
+f 3178//3178 3177//3177 3190//3190
+f 3191//3191 3223//3223 3159//3159
+f 3061//3061 3059//3059 3192//3192
+f 3061//3061 3192//3192 3222//3222
+f 3194//3194 3061//3061 3222//3222
+f 3193//3193 3194//3194 3222//3222
+f 3064//3064 3194//3194 3193//3193
+f 3186//3186 3064//3064 3193//3193
+f 3058//3058 3064//3064 3186//3186
+f 3057//3057 3186//3186 3202//3202
+f 3057//3057 3058//3058 3186//3186
+f 3207//3207 3208//3208 3170//3170
+f 3169//3169 3175//3175 3198//3198
+f 3208//3208 3169//3169 3198//3198
+f 3175//3175 3176//3176 3199//3199
+f 3198//3198 3175//3175 3199//3199
+f 3176//3176 3174//3174 3200//3200
+f 3199//3199 3176//3176 3200//3200
+f 3229//3229 3196//3196 3172//3172
+f 3174//3174 3172//3172 3196//3196
+f 3195//3195 3196//3196 3229//3229
+f 3238//3238 3204//3204 3195//3195
+f 3205//3205 3200//3200 3174//3174
+f 3170//3170 3120//3120 3218//3218
+f 3120//3120 3118//3118 3218//3218
+f 3205//3205 3174//3174 3196//3196
+f 3122//3122 3120//3120 3208//3208
+f 3120//3120 3170//3170 3208//3208
+f 3122//3122 3208//3208 3198//3198
+f 3197//3197 3198//3198 3199//3199
+f 3197//3197 3122//3122 3198//3198
+f 3139//3139 3197//3197 3199//3199
+f 3139//3139 3199//3199 3200//3200
+f 3206//3206 3201//3201 3202//3202
+f 3201//3201 3057//3057 3202//3202
+f 3146//3146 3203//3203 3195//3195
+f 3146//3146 3195//3195 3204//3204
+f 3203//3203 3205//3205 3196//3196
+f 3157//3157 3206//3206 3216//3216
+f 3117//3117 3216//3216 3217//3217
+f 3203//3203 3196//3196 3195//3195
+f 3117//3117 3157//3157 3216//3216
+f 3218//3218 3118//3118 3217//3217
+f 3118//3118 3117//3117 3217//3217
+f 3207//3207 3169//3169 3208//3208
+f 3159//3159 3223//3223 3209//3209
+f 3223//3223 3375//3375 3209//3209
+f 3159//3159 3209//3209 3380//3380
+f 3210//3210 3380//3380 3212//3212
+f 3211//3211 3210//3210 3213//3213
+f 3210//3210 3212//3212 3213//3213
+f 3223//3223 3220//3220 3375//3375
+f 3214//3214 3192//3192 3374//3374
+f 3059//3059 3374//3374 3192//3192
+f 3214//3214 3374//3374 3376//3376
+f 3220//3220 3214//3214 3376//3376
+f 3375//3375 3220//3220 3376//3376
+f 3215//3215 3206//3206 3202//3202
+f 3215//3215 3171//3171 3216//3216
+f 3206//3206 3215//3215 3216//3216
+f 3171//3171 3219//3219 3217//3217
+f 3216//3216 3171//3171 3217//3217
+f 3219//3219 3170//3170 3218//3218
+f 3219//3219 3218//3218 3217//3217
+f 3222//3222 3192//3192 3214//3214
+f 3220//3220 3221//3221 3214//3214
+f 3221//3221 3222//3222 3214//3214
+f 3223//3223 3162//3162 3220//3220
+f 3162//3162 3221//3221 3220//3220
+f 3228//3228 3224//3224 3236//3236
+f 3228//3228 3225//3225 3226//3226
+f 3226//3226 3225//3225 3233//3233
+f 3232//3232 45//45 3236//3236
+f 3237//3237 3227//3227 3226//3226
+f 3226//3226 3227//3227 3228//3228
+f 3238//3238 3229//3229 3234//3234
+f 3229//3229 3231//3231 3234//3234
+f 3204//3204 3239//3239 3230//3230
+f 3235//3235 3224//3224 3231//3231
+f 3204//3204 3238//3238 3239//3239
+f 3163//3163 3164//3164 3232//3232
+f 3235//3235 3163//3163 3232//3232
+f 3237//3237 53//53 3230//3230
+f 53//53 3237//3237 3226//3226
+f 3233//3233 53//53 3226//3226
+f 3227//3227 3237//3237 3239//3239
+f 3228//3228 3227//3227 3224//3224
+f 77//77 3233//3233 3225//3225
+f 3225//3225 3228//3228 56//56
+f 3235//3235 3236//3236 3224//3224
+f 3231//3231 3224//3224 3234//3234
+f 3234//3234 3224//3224 3227//3227
+f 3234//3234 3227//3227 3239//3239
+f 3235//3235 3232//3232 3236//3236
+f 3239//3239 3237//3237 3230//3230
+f 3204//3204 3230//3230 4448//4448
+f 55//55 77//77 3225//3225
+f 55//55 3225//3225 56//56
+f 3238//3238 3234//3234 3239//3239
+f 3240//3240 4448//4448 3230//3230
+f 53//53 3240//3240 3230//3230
+f 1090//1090 3241//3241 4323//4323
+f 3241//3241 4316//4316 4323//4323
+f 4309//4309 4315//4315 4662//4662
+f 3242//3242 3243//3243 420//420
+f 420//420 419//419 3248//3248
+f 420//420 3248//3248 3247//3247
+f 3247//3247 3244//3244 3245//3245
+f 3247//3247 4389//4389 3252//3252
+f 3247//3247 3252//3252 3246//3246
+f 3247//3247 3248//3248 4389//4389
+f 3249//3249 4897//4897 4878//4878
+f 4865//4865 4829//4829 2292//2292
+f 3251//3251 3846//3846 3252//3252
+f 3851//3851 3246//3246 4374//4374
+f 4895//4895 4871//4871 3249//3249
+f 3245//3245 3242//3242 420//420
+f 4810//4810 3254//3254 4897//4897
+f 4897//4897 3254//3254 4898//4898
+f 3250//3250 4895//4895 4878//4878
+f 418//418 3242//3242 3245//3245
+f 4884//4884 4885//4885 3250//3250
+f 4389//4389 3251//3251 3252//3252
+f 3256//3256 3253//3253 4898//4898
+f 3252//3252 3846//3846 4374//4374
+f 3254//3254 3005//3005 3256//3256
+f 239//239 253//253 3255//3255
+f 3256//3256 4379//4379 3253//3253
+f 2304//2304 3153//3153 2688//2688
+f 4395//4395 2114//2114 4213//4213
+f 3251//3251 3270//3270 3846//3846
+f 243//243 4815//4815 4814//4814
+f 3257//3257 4846//4846 3258//3258
+f 3257//3257 3258//3258 2125//2125
+f 3259//3259 2125//2125 2127//2127
+f 3260//3260 2125//2125 3259//3259
+f 3262//3262 2127//2127 2126//2126
+f 3259//3259 2127//2127 3262//3262
+f 3262//3262 3261//3261 3259//3259
+f 3259//3259 3261//3261 281//281
+f 3263//3263 274//274 3261//3261
+f 3267//3267 3262//3262 3264//3264
+f 3267//3267 3263//3263 3261//3261
+f 3262//3262 3267//3267 3261//3261
+f 281//281 3261//3261 274//274
+f 3264//3264 2132//2132 3267//3267
+f 2126//2126 3264//3264 3262//3262
+f 3263//3263 3266//3266 3265//3265
+f 2132//2132 3266//3266 3267//3267
+f 261//261 3263//3263 3265//3265
+f 3268//3268 3265//3265 4805//4805
+f 3269//3269 3268//3268 2966//2966
+f 2966//2966 3268//3268 4805//4805
+f 2779//2779 4845//4845 1903//1903
+f 3267//3267 3266//3266 3263//3263
+f 275//275 3268//3268 3269//3269
+f 3279//3279 3277//3277 4405//4405
+f 3846//3846 3270//3270 3845//3845
+f 4401//4401 4871//4871 4402//4402
+f 3845//3845 3271//3271 3846//3846
+f 3763//3763 4882//4882 3277//3277
+f 3272//3272 4635//4635 4833//4833
+f 4895//4895 3273//3273 4871//4871
+f 3273//3273 3274//3274 3276//3276
+f 3279//3279 4405//4405 3273//3273
+f 4895//4895 3274//3274 3273//3273
+f 4826//4826 2103//2103 3275//3275
+f 2103//2103 3279//3279 3275//3275
+f 948//948 950//950 3276//3276
+f 3274//3274 4885//4885 948//948
+f 4838//4838 3255//3255 4836//4836
+f 3274//3274 948//948 3276//3276
+f 950//950 4826//4826 3275//3275
+f 3276//3276 950//950 3275//3275
+f 2103//2103 3763//3763 3277//3277
+f 3273//3273 3276//3276 3279//3279
+f 3278//3278 948//948 4885//4885
+f 3279//3279 2103//2103 3277//3277
+f 4405//4405 4871//4871 3273//3273
+f 3276//3276 3275//3275 3279//3279
+f 2779//2779 3280//3280 4860//4860
+f 4845//4845 945//945 1903//1903
+f 3285//3285 52//52 3281//3281
+f 3281//3281 52//52 3286//3286
+f 3282//3282 3281//3281 3286//3286
+f 3282//3282 3402//3402 3283//3283
+f 54//54 50//50 3284//3284
+f 50//50 52//52 3285//3285
+f 3284//3284 50//50 3285//3285
+f 54//54 3284//3284 3423//3423
+f 3286//3286 46//46 3282//3282
+f 3281//3281 3282//3282 3283//3283
+f 52//52 49//49 3286//3286
+f 3282//3282 3287//3287 3402//3402
+f 3287//3287 3282//3282 46//46
+f 3288//3288 3289//3289 3296//3296
+f 3296//3296 3289//3289 3295//3295
+f 3289//3289 3288//3288 3314//3314
+f 3299//3299 3298//3298 3294//3294
+f 3295//3295 3289//3289 3298//3298
+f 3289//3289 3294//3294 3298//3298
+f 3295//3295 3298//3298 3293//3293
+f 3309//3309 3293//3293 3297//3297
+f 3309//3309 3306//3306 3293//3293
+f 3419//3419 3292//3292 3290//3290
+f 3295//3295 3293//3293 3291//3291
+f 3419//3419 3301//3301 3422//3422
+f 3419//3419 3290//3290 3301//3301
+f 3290//3290 3295//3295 3291//3291
+f 3290//3290 3292//3292 3295//3295
+f 3418//3418 3312//3312 3311//3311
+f 3293//3293 3298//3298 3297//3297
+f 3294//3294 3320//3320 3319//3319
+f 3294//3294 3319//3319 3299//3299
+f 3296//3296 3295//3295 3292//3292
+f 3288//3288 3296//3296 3418//3418
+f 3297//3297 3298//3298 3299//3299
+f 3300//3300 3297//3297 3299//3299
+f 3290//3290 3291//3291 3301//3301
+f 3304//3304 75//75 3307//3307
+f 3309//3309 84//84 3306//3306
+f 3302//3302 84//84 3309//3309
+f 3303//3303 2007//2007 3300//3300
+f 3299//3299 3303//3303 3300//3300
+f 2007//2007 3308//3308 3300//3300
+f 3303//3303 3299//3299 3319//3319
+f 84//84 3304//3304 3306//3306
+f 3306//3306 3304//3304 3305//3305
+f 3291//3291 3306//3306 3305//3305
+f 3293//3293 3306//3306 3291//3291
+f 3304//3304 3307//3307 3305//3305
+f 3422//3422 3301//3301 3305//3305
+f 3301//3301 3291//3291 3305//3305
+f 3307//3307 3422//3422 3305//3305
+f 3308//3308 3302//3302 3300//3300
+f 3302//3302 3309//3309 3297//3297
+f 3302//3302 3297//3297 3300//3300
+f 3355//3355 3310//3310 3333//3333
+f 3333//3333 3311//3311 3312//3312
+f 3333//3333 3312//3312 3332//3332
+f 3310//3310 3311//3311 3333//3333
+f 3349//3349 3313//3313 3315//3315
+f 3313//3313 3349//3349 3328//3328
+f 3360//3360 3325//3325 3323//3323
+f 3360//3360 3356//3356 3325//3325
+f 3311//3311 3310//3310 3314//3314
+f 3314//3314 3310//3310 3315//3315
+f 3320//3320 3315//3315 3313//3313
+f 3314//3314 3315//3315 3320//3320
+f 3318//3318 3313//3313 3316//3316
+f 3320//3320 3313//3313 3318//3318
+f 3318//3318 3316//3316 3353//3353
+f 3352//3352 3353//3353 3316//3316
+f 3356//3356 3352//3352 3325//3325
+f 3311//3311 3314//3314 3288//3288
+f 3294//3294 3314//3314 3320//3320
+f 3353//3353 3354//3354 3317//3317
+f 3362//3362 3381//3381 3363//3363
+f 3319//3319 3318//3318 3317//3317
+f 3318//3318 3353//3353 3317//3317
+f 3318//3318 3319//3319 3320//3320
+f 3289//3289 3314//3314 3294//3294
+f 3325//3325 3324//3324 3321//3321
+f 3321//3321 3324//3324 3322//3322
+f 3323//3323 3325//3325 3321//3321
+f 3321//3321 3322//3322 3329//3329
+f 3324//3324 3352//3352 3326//3326
+f 3324//3324 3326//3326 3343//3343
+f 3325//3325 3352//3352 3324//3324
+f 3322//3322 3324//3324 3343//3343
+f 3326//3326 3316//3316 3327//3327
+f 3326//3326 3327//3327 3344//3344
+f 3352//3352 3316//3316 3326//3326
+f 3343//3343 3326//3326 3344//3344
+f 3327//3327 3328//3328 3344//3344
+f 3316//3316 3313//3313 3327//3327
+f 3327//3327 3313//3313 3328//3328
+f 3418//3418 3311//3311 3288//3288
+f 3323//3323 3321//3321 3330//3330
+f 3321//3321 3329//3329 3330//3330
+f 3439//3439 3330//3330 3341//3341
+f 3335//3335 3336//3336 289//289
+f 3335//3335 289//289 3323//3323
+f 4636//4636 4648//4648 4647//4647
+f 3350//3350 3361//3361 3430//3430
+f 3345//3345 3331//3331 3362//3362
+f 3333//3333 3332//3332 3334//3334
+f 3355//3355 3333//3333 3334//3334
+f 3355//3355 3391//3391 3392//3392
+f 3335//3335 3439//3439 3426//3426
+f 3439//3439 3335//3335 3330//3330
+f 3334//3334 3332//3332 3398//3398
+f 3335//3335 3323//3323 3330//3330
+f 3336//3336 1994//1994 289//289
+f 3347//3347 3348//3348 3331//3331
+f 3339//3339 3340//3340 3337//3337
+f 3340//3340 3355//3355 3392//3392
+f 3340//3340 3392//3392 3337//3337
+f 3328//3328 3347//3347 3338//3338
+f 3328//3328 3349//3349 3347//3347
+f 3347//3347 3349//3349 3339//3339
+f 3315//3315 3310//3310 3340//3340
+f 3315//3315 3340//3340 3349//3349
+f 3310//3310 3355//3355 3340//3340
+f 3330//3330 3329//3329 3341//3341
+f 3322//3322 3342//3342 3329//3329
+f 3322//3322 3343//3343 3351//3351
+f 3342//3342 3322//3322 3351//3351
+f 3343//3343 3344//3344 3346//3346
+f 3351//3351 3343//3343 3346//3346
+f 3344//3344 3328//3328 3338//3338
+f 3344//3344 3338//3338 3346//3346
+f 3338//3338 3345//3345 3346//3346
+f 3345//3345 3361//3361 3346//3346
+f 3347//3347 3331//3331 3338//3338
+f 3331//3331 3345//3345 3338//3338
+f 3347//3347 3339//3339 3348//3348
+f 3340//3340 3339//3339 3349//3349
+f 3350//3350 3342//3342 3351//3351
+f 3361//3361 3351//3351 3346//3346
+f 3361//3361 3350//3350 3351//3351
+f 3353//3353 3352//3352 3356//3356
+f 3353//3353 3356//3356 3354//3354
+f 3391//3391 3355//3355 3334//3334
+f 3357//3357 3356//3356 3360//3360
+f 3356//3356 3357//3357 3354//3354
+f 288//288 3303//3303 3317//3317
+f 3348//3348 3358//3358 3393//3393
+f 3358//3358 3359//3359 3393//3393
+f 3357//3357 288//288 3354//3354
+f 3348//3348 3393//3393 3362//3362
+f 3303//3303 3319//3319 3317//3317
+f 3358//3358 3337//3337 3359//3359
+f 3337//3337 3392//3392 3359//3359
+f 288//288 3317//3317 3354//3354
+f 3348//3348 3339//3339 3358//3358
+f 289//289 3360//3360 3323//3323
+f 289//289 3357//3357 3360//3360
+f 3339//3339 3337//3337 3358//3358
+f 3363//3363 3361//3361 3345//3345
+f 3362//3362 3363//3363 3345//3345
+f 3348//3348 3362//3362 3331//3331
+f 3364//3364 3370//3370 3369//3369
+f 3369//3369 3370//3370 3365//3365
+f 3377//3377 3366//3366 3371//3371
+f 3367//3367 3385//3385 3364//3364
+f 3367//3367 3378//3378 3385//3385
+f 3368//3368 3408//3408 3373//3373
+f 3365//3365 3368//3368 3369//3369
+f 3372//3372 3379//3379 3386//3386
+f 3370//3370 3399//3399 3365//3365
+f 3377//3377 3371//3371 3385//3385
+f 3371//3371 3366//3366 3399//3399
+f 3364//3364 3385//3385 3371//3371
+f 3370//3370 3364//3364 3371//3371
+f 3372//3372 3386//3386 3384//3384
+f 3370//3370 3371//3371 3399//3399
+f 3379//3379 3367//3367 3383//3383
+f 3383//3383 3373//3373 3382//3382
+f 3374//3374 3063//3063 3405//3405
+f 3385//3385 3375//3375 3377//3377
+f 3387//3387 3211//3211 3213//3213
+f 3164//3164 3211//3211 3387//3387
+f 3372//3372 3367//3367 3379//3379
+f 3376//3376 3374//3374 3405//3405
+f 3377//3377 3376//3376 3405//3405
+f 3212//3212 3380//3380 3378//3378
+f 3212//3212 3378//3378 3372//3372
+f 3383//3383 3382//3382 3379//3379
+f 3380//3380 3209//3209 3378//3378
+f 3362//3362 3393//3393 3381//3381
+f 3363//3363 3381//3381 3405//3405
+f 3213//3213 3212//3212 3384//3384
+f 3373//3373 3408//3408 3382//3382
+f 3382//3382 3408//3408 3397//3397
+f 3368//3368 3373//3373 3369//3369
+f 3367//3367 3364//3364 3383//3383
+f 3364//3364 3369//3369 3383//3383
+f 3383//3383 3369//3369 3373//3373
+f 3378//3378 3209//3209 3385//3385
+f 3375//3375 3376//3376 3377//3377
+f 3212//3212 3372//3372 3384//3384
+f 3378//3378 3367//3367 3372//3372
+f 3209//3209 3375//3375 3385//3385
+f 3386//3386 3388//3388 3390//3390
+f 3401//3401 3388//3388 3386//3386
+f 3164//3164 3387//3387 3403//3403
+f 3388//3388 3401//3401 3389//3389
+f 3213//3213 3384//3384 3404//3404
+f 3387//3387 3213//3213 3404//3404
+f 3390//3390 3404//3404 3384//3384
+f 3386//3386 3390//3390 3384//3384
+f 3390//3390 3394//3394 3404//3404
+f 3391//3391 3334//3334 3395//3395
+f 3391//3391 3395//3395 3396//3396
+f 3392//3392 3391//3391 3396//3396
+f 3359//3359 3392//3392 3396//3396
+f 3359//3359 3396//3396 3400//3400
+f 3393//3393 3400//3400 3381//3381
+f 3393//3393 3359//3359 3400//3400
+f 3063//3063 3363//3363 3405//3405
+f 3287//3287 3390//3390 3388//3388
+f 3287//3287 3394//3394 3390//3390
+f 3334//3334 3398//3398 3395//3395
+f 3395//3395 3399//3399 3396//3396
+f 3382//3382 3397//3397 3401//3401
+f 3365//3365 3399//3399 3395//3395
+f 3401//3401 3397//3397 3389//3389
+f 3398//3398 3365//3365 3395//3395
+f 3400//3400 3366//3366 3381//3381
+f 3396//3396 3399//3399 3400//3400
+f 3399//3399 3366//3366 3400//3400
+f 3386//3386 3379//3379 3401//3401
+f 3402//3402 3388//3388 3389//3389
+f 3402//3402 3287//3287 3388//3388
+f 3403//3403 3387//3387 3394//3394
+f 3387//3387 3404//3404 3394//3394
+f 3405//3405 3381//3381 3366//3366
+f 3379//3379 3382//3382 3401//3401
+f 3415//3415 3402//3402 3389//3389
+f 3377//3377 3405//3405 3366//3366
+f 3414//3414 3413//3413 3417//3417
+f 3412//3412 3420//3420 3406//3406
+f 3397//3397 3408//3408 3407//3407
+f 3397//3397 3407//3407 3410//3410
+f 3420//3420 3412//3412 3409//3409
+f 3407//3407 3408//3408 3409//3409
+f 3408//3408 3368//3368 3409//3409
+f 3397//3397 3410//3410 3389//3389
+f 3412//3412 3407//3407 3409//3409
+f 3421//3421 3416//3416 3406//3406
+f 3410//3410 3415//3415 3389//3389
+f 3413//3413 3410//3410 3411//3411
+f 3415//3415 3410//3410 3413//3413
+f 3411//3411 3417//3417 3413//3413
+f 3417//3417 3411//3411 3416//3416
+f 3407//3407 3412//3412 3411//3411
+f 3410//3410 3407//3407 3411//3411
+f 3416//3416 3411//3411 3412//3412
+f 3416//3416 3412//3412 3406//3406
+f 3283//3283 3413//3413 3414//3414
+f 3283//3283 3402//3402 3415//3415
+f 3283//3283 3415//3415 3413//3413
+f 3419//3419 3422//3422 3421//3421
+f 3423//3423 3416//3416 3421//3421
+f 3423//3423 3284//3284 3416//3416
+f 3284//3284 3417//3417 3416//3416
+f 3284//3284 3285//3285 3417//3417
+f 3417//3417 3285//3285 3414//3414
+f 3285//3285 3281//3281 3414//3414
+f 3281//3281 3283//3283 3414//3414
+f 3312//3312 3418//3418 3420//3420
+f 3420//3420 3418//3418 3406//3406
+f 3332//3332 3312//3312 3420//3420
+f 3292//3292 3419//3419 3421//3421
+f 3332//3332 3420//3420 3409//3409
+f 3418//3418 3296//3296 3406//3406
+f 3296//3296 3292//3292 3406//3406
+f 3398//3398 3332//3332 3409//3409
+f 3292//3292 3421//3421 3406//3406
+f 3368//3368 3365//3365 3398//3398
+f 3307//3307 57//57 3422//3422
+f 3368//3368 3398//3398 3409//3409
+f 3422//3422 57//57 3421//3421
+f 57//57 3423//3423 3421//3421
+f 3425//3425 4434//4434 2869//2869
+f 3361//3361 3363//3363 3431//3431
+f 3361//3361 3431//3431 3430//3430
+f 3424//3424 3425//3425 3458//3458
+f 3437//3437 3426//3426 3439//3439
+f 3329//3329 3342//3342 3427//3427
+f 3342//3342 3350//3350 3427//3427
+f 2869//2869 3436//3436 3425//3425
+f 3341//3341 3329//3329 3427//3427
+f 3458//3458 3425//3425 3435//3435
+f 3434//3434 3428//3428 3460//3460
+f 3434//3434 3430//3430 3428//3428
+f 3429//3429 3433//3433 3438//3438
+f 3430//3430 3434//3434 3429//3429
+f 3350//3350 3430//3430 3429//3429
+f 3427//3427 3350//3350 3429//3429
+f 3431//3431 3428//3428 3430//3430
+f 3425//3425 3432//3432 3435//3435
+f 3425//3425 3436//3436 3432//3432
+f 3458//3458 3435//3435 3461//3461
+f 3433//3433 3429//3429 3432//3432
+f 3429//3429 3434//3434 3432//3432
+f 3434//3434 3460//3460 3435//3435
+f 3432//3432 3434//3434 3435//3435
+f 3433//3433 3432//3432 3436//3436
+f 3427//3427 3429//3429 3438//3438
+f 2867//2867 309//309 3436//3436
+f 2869//2869 2867//2867 3436//3436
+f 309//309 3433//3433 3436//3436
+f 309//309 3437//3437 3433//3433
+f 3437//3437 3438//3438 3433//3433
+f 3438//3438 3341//3341 3427//3427
+f 3438//3438 3437//3437 3439//3439
+f 3438//3438 3439//3439 3341//3341
+f 3442//3442 3446//3446 3447//3447
+f 3447//3447 3449//3449 3442//3442
+f 3441//3441 3440//3440 3470//3470
+f 3441//3441 3449//3449 3440//3440
+f 3455//3455 3449//3449 3441//3441
+f 3449//3449 3455//3455 3442//3442
+f 3460//3460 3443//3443 3461//3461
+f 3443//3443 3460//3460 3479//3479
+f 3450//3450 3443//3443 3444//3444
+f 3461//3461 3443//3443 3450//3450
+f 3450//3450 3444//3444 3445//3445
+f 3446//3446 3445//3445 3475//3475
+f 3450//3450 3445//3445 3446//3446
+f 3446//3446 3475//3475 3447//3447
+f 3449//3449 3447//3447 3481//3481
+f 3448//3448 3472//3472 3466//3466
+f 3449//3449 3481//3481 3440//3440
+f 3470//3470 3440//3440 3472//3472
+f 3458//3458 3461//3461 3450//3450
+f 3473//3473 3453//3453 3441//3441
+f 3453//3453 3452//3452 3441//3441
+f 3470//3470 3473//3473 3441//3441
+f 3456//3456 3457//3457 3442//3442
+f 3451//3451 3478//3478 3477//3477
+f 3452//3452 3453//3453 3454//3454
+f 3459//3459 3478//3478 3452//3452
+f 3452//3452 3454//3454 3459//3459
+f 3478//3478 3456//3456 3455//3455
+f 3452//3452 3478//3478 3455//3455
+f 3455//3455 3456//3456 3442//3442
+f 3446//3446 3442//3442 3457//3457
+f 3450//3450 3457//3457 3458//3458
+f 3450//3450 3446//3446 3457//3457
+f 3455//3455 3441//3441 3452//3452
+f 3451//3451 4435//4435 3424//3424
+f 3047//3047 3448//3448 3053//3053
+f 3456//3456 3451//3451 3424//3424
+f 4434//4434 3424//3424 4435//4435
+f 283//283 3477//3477 3459//3459
+f 3462//3462 3454//3454 2870//2870
+f 3425//3425 3424//3424 4434//4434
+f 3435//3435 3460//3460 3461//3461
+f 283//283 4437//4437 3477//3477
+f 3454//3454 3462//3462 3459//3459
+f 3465//3465 3025//3025 3015//3015
+f 3463//3463 3465//3465 3464//3464
+f 3465//3465 3019//3019 3464//3464
+f 3019//3019 3053//3053 3466//3466
+f 3464//3464 3019//3019 3466//3466
+f 3063//3063 3431//3431 3363//3363
+f 3431//3431 3063//3063 3482//3482
+f 3063//3063 3062//3062 3482//3482
+f 3062//3062 3467//3467 3468//3468
+f 3482//3482 3062//3062 3468//3468
+f 3468//3468 3467//3467 3474//3474
+f 3474//3474 3469//3469 3476//3476
+f 3469//3469 3025//3025 3476//3476
+f 3469//3469 3015//3015 3025//3025
+f 3471//3471 2880//2880 3470//3470
+f 3471//3471 3470//3470 3472//3472
+f 3448//3448 3471//3471 3472//3472
+f 2881//2881 3454//3454 3453//3453
+f 3473//3473 2881//2881 3453//3453
+f 3470//3470 2880//2880 3473//3473
+f 3474//3474 3444//3444 3480//3480
+f 3474//3474 3476//3476 3444//3444
+f 3468//3468 3474//3474 3480//3480
+f 3482//3482 3468//3468 3479//3479
+f 3458//3458 3457//3457 3424//3424
+f 3457//3457 3456//3456 3424//3424
+f 3025//3025 3463//3463 3475//3475
+f 3476//3476 3025//3025 3475//3475
+f 3477//3477 3478//3478 3459//3459
+f 3456//3456 3478//3478 3451//3451
+f 3472//3472 3440//3440 3464//3464
+f 3466//3466 3472//3472 3464//3464
+f 3440//3440 3481//3481 3464//3464
+f 3443//3443 3479//3479 3480//3480
+f 3480//3480 3444//3444 3443//3443
+f 3468//3468 3480//3480 3479//3479
+f 3053//3053 3448//3448 3466//3466
+f 3481//3481 3447//3447 3463//3463
+f 3464//3464 3481//3481 3463//3463
+f 3463//3463 3447//3447 3475//3475
+f 3476//3476 3445//3445 3444//3444
+f 3476//3476 3475//3475 3445//3445
+f 3428//3428 3482//3482 3479//3479
+f 3428//3428 3431//3431 3482//3482
+f 3460//3460 3428//3428 3479//3479
+f 3514//3514 3493//3493 3495//3495
+f 2346//2346 3492//3492 3511//3511
+f 3494//3494 3492//3492 2346//2346
+f 3493//3493 3514//3514 3515//3515
+f 3515//3515 3513//3513 3483//3483
+f 3493//3493 3515//3515 3483//3483
+f 3513//3513 3485//3485 3486//3486
+f 3483//3483 3513//3513 3486//3486
+f 3485//3485 3484//3484 3489//3489
+f 3486//3486 3485//3485 3489//3489
+f 3484//3484 3502//3502 3503//3503
+f 3489//3489 3484//3484 3503//3503
+f 3081//3081 3079//3079 3489//3489
+f 3489//3489 3079//3079 3486//3486
+f 3486//3486 3487//3487 3501//3501
+f 3079//3079 3487//3487 3486//3486
+f 3487//3487 3084//3084 3501//3501
+f 3501//3501 3488//3488 3498//3498
+f 3084//3084 3488//3488 3501//3501
+f 3500//3500 3081//3081 3503//3503
+f 3081//3081 3489//3489 3503//3503
+f 3500//3500 3490//3490 3496//3496
+f 3500//3500 3503//3503 3490//3490
+f 3491//3491 3507//3507 3514//3514
+f 2349//2349 3499//3499 2348//2348
+f 3494//3494 3490//3490 3492//3492
+f 3497//3497 3493//3493 3498//3498
+f 3494//3494 2351//2351 3490//3490
+f 3497//3497 3495//3495 3493//3493
+f 3491//3491 3514//3514 3495//3495
+f 2351//2351 3496//3496 3490//3490
+f 3488//3488 3148//3148 3498//3498
+f 3148//3148 3497//3497 3498//3498
+f 3499//3499 3500//3500 3496//3496
+f 3496//3496 2348//2348 3499//3499
+f 3483//3483 3501//3501 3498//3498
+f 3483//3483 3486//3486 3501//3501
+f 3493//3493 3483//3483 3498//3498
+f 3502//3502 3492//3492 3503//3503
+f 3511//3511 3492//3492 3502//3502
+f 3503//3503 3492//3492 3490//3490
+f 3504//3504 2305//2305 2307//2307
+f 3508//3508 3516//3516 3509//3509
+f 2310//2310 3517//3517 3508//3508
+f 3516//3516 3508//3508 3517//3517
+f 4581//4581 4543//4543 3505//3505
+f 3517//3517 4581//4581 3505//3505
+f 4630//4630 3512//3512 3506//3506
+f 3514//3514 3507//3507 3516//3516
+f 3507//3507 2251//2251 3516//3516
+f 2249//2249 3504//3504 2307//2307
+f 2307//2307 3508//3508 3509//3509
+f 3509//3509 2249//2249 2307//2307
+f 2251//2251 3509//3509 3516//3516
+f 3502//3502 3484//3484 4591//4591
+f 3511//3511 3502//3502 4239//4239
+f 3505//3505 4543//4543 4234//4234
+f 4582//4582 3510//3510 2952//2952
+f 2321//2321 2346//2346 3511//3511
+f 3484//3484 4234//4234 4591//4591
+f 3485//3485 3513//3513 3505//3505
+f 4630//4630 4632//4632 3512//3512
+f 4589//4589 2321//2321 3511//3511
+f 3484//3484 3485//3485 4234//4234
+f 3485//3485 3505//3505 4234//4234
+f 3513//3513 3515//3515 3517//3517
+f 3513//3513 3517//3517 3505//3505
+f 3515//3515 3514//3514 3516//3516
+f 3515//3515 3516//3516 3517//3517
+f 2776//2776 3519//3519 3518//3518
+f 3530//3530 2108//2108 3522//3522
+f 3518//3518 3523//3523 2108//2108
+f 3519//3519 3529//3529 3523//3523
+f 3523//3523 3518//3518 3519//3519
+f 2108//2108 3523//3523 3522//3522
+f 3746//3746 3521//3521 3528//3528
+f 3746//3746 3528//3528 3529//3529
+f 3528//3528 3521//3521 3526//3526
+f 3520//3520 3521//3521 3745//3745
+f 3746//3746 3745//3745 3521//3521
+f 3522//3522 3526//3526 3525//3525
+f 3522//3522 3523//3523 3528//3528
+f 3522//3522 3528//3528 3526//3526
+f 3527//3527 3524//3524 3526//3526
+f 3527//3527 3750//3750 3524//3524
+f 3524//3524 3525//3525 3526//3526
+f 3526//3526 3521//3521 3527//3527
+f 3521//3521 3520//3520 3527//3527
+f 3527//3527 3520//3520 3749//3749
+f 3528//3528 3523//3523 3529//3529
+f 3543//3543 3534//3534 3550//3550
+f 3532//3532 3535//3535 3530//3530
+f 3538//3538 3534//3534 3532//3532
+f 3533//3533 3538//3538 3531//3531
+f 3532//3532 3534//3534 3535//3535
+f 3538//3538 3533//3533 3534//3534
+f 3534//3534 3543//3543 3535//3535
+f 3531//3531 3538//3538 3524//3524
+f 3532//3532 3525//3525 3538//3538
+f 3525//3525 3532//3532 3530//3530
+f 2109//2109 3536//3536 3535//3535
+f 3535//3535 3536//3536 3530//3530
+f 3522//3522 3525//3525 3530//3530
+f 3550//3550 3534//3534 3537//3537
+f 3534//3534 3533//3533 3537//3537
+f 3543//3543 2109//2109 3535//3535
+f 3544//3544 3543//3543 3545//3545
+f 3525//3525 3524//3524 3538//3538
+f 3539//3539 2109//2109 3543//3543
+f 3545//3545 3550//3550 3549//3549
+f 3545//3545 3549//3549 3540//3540
+f 3540//3540 3549//3549 3541//3541
+f 3540//3540 3541//3541 3547//3547
+f 3544//3544 3546//3546 3548//3548
+f 3548//3548 3546//3546 3556//3556
+f 3548//3548 3556//3556 3542//3542
+f 3543//3543 3544//3544 3539//3539
+f 3544//3544 3545//3545 3546//3546
+f 3545//3545 3540//3540 3546//3546
+f 3540//3540 3547//3547 3546//3546
+f 3546//3546 3547//3547 3556//3556
+f 3548//3548 3539//3539 3544//3544
+f 3537//3537 3533//3533 3759//3759
+f 3543//3543 3550//3550 3545//3545
+f 3549//3549 3550//3550 3551//3551
+f 3551//3551 3550//3550 3537//3537
+f 3541//3541 3767//3767 3552//3552
+f 3547//3547 3541//3541 3552//3552
+f 3549//3549 3551//3551 3541//3541
+f 3557//3557 3775//3775 3560//3560
+f 3555//3555 3775//3775 3557//3557
+f 3560//3560 3775//3775 3553//3553
+f 3778//3778 3560//3560 3553//3553
+f 3552//3552 3776//3776 3555//3555
+f 3554//3554 3542//3542 3559//3559
+f 3556//3556 3555//3555 3557//3557
+f 3547//3547 3555//3555 3556//3556
+f 3547//3547 3552//3552 3555//3555
+f 4324//4324 4228//4228 2990//2990
+f 3555//3555 3776//3776 3775//3775
+f 3542//3542 3556//3556 3557//3557
+f 3542//3542 3557//3557 3559//3559
+f 3561//3561 3567//3567 3558//3558
+f 3561//3561 3558//3558 3559//3559
+f 4334//4334 4888//4888 4043//4043
+f 3558//3558 3784//3784 3559//3559
+f 3557//3557 3560//3560 3559//3559
+f 3560//3560 3561//3561 3559//3559
+f 3560//3560 3778//3778 3561//3561
+f 3561//3561 3778//3778 3782//3782
+f 3567//3567 3782//3782 3785//3785
+f 3567//3567 3561//3561 3782//3782
+f 3565//3565 3563//3563 3569//3569
+f 3569//3569 3564//3564 3562//3562
+f 3563//3563 3564//3564 3569//3569
+f 3565//3565 3567//3567 3563//3563
+f 3567//3567 3785//3785 3563//3563
+f 3563//3563 3785//3785 3564//3564
+f 3784//3784 3558//3558 3566//3566
+f 3558//3558 3567//3567 3565//3565
+f 3566//3566 3558//3558 3565//3565
+f 3575//3575 3574//3574 4377//4377
+f 4377//4377 3574//3574 4754//4754
+f 3566//3566 3565//3565 3575//3575
+f 2156//2156 2168//2168 3568//3568
+f 4377//4377 4754//4754 4715//4715
+f 4378//4378 3566//3566 4377//4377
+f 3566//3566 3575//3575 4377//4377
+f 3569//3569 3570//3570 3575//3575
+f 3569//3569 3575//3575 3565//3565
+f 3569//3569 3562//3562 3570//3570
+f 3571//3571 3574//3574 3572//3572
+f 4791//4791 4798//4798 4796//4796
+f 3572//3572 3574//3574 3573//3573
+f 4798//4798 4752//4752 4866//4866
+f 3574//3574 3575//3575 3573//3573
+f 3570//3570 3573//3573 3575//3575
+f 3576//3576 3577//3577 3581//3581
+f 3572//3572 3578//3578 3583//3583
+f 3571//3571 3583//3583 3576//3576
+f 3571//3571 3572//3572 3583//3583
+f 4759//4759 3576//3576 3581//3581
+f 3581//3581 4685//4685 4774//4774
+f 2237//2237 4801//4801 2269//2269
+f 2265//2265 3579//3579 3580//3580
+f 3581//3581 3577//3577 4685//4685
+f 3572//3572 3803//3803 3578//3578
+f 3583//3583 3578//3578 3582//3582
+f 3583//3583 3582//3582 4595//4595
+f 3583//3583 4595//4595 3576//3576
+f 4601//4601 3584//3584 3585//3585
+f 4588//4588 4587//4587 3585//3585
+f 4601//4601 4600//4600 3584//3584
+f 3584//3584 3605//3605 4588//4588
+f 4593//4593 3582//3582 3592//3592
+f 4595//4595 2303//2303 3577//3577
+f 4635//4635 3586//3586 3587//3587
+f 3576//3576 4595//4595 3577//3577
+f 4685//4685 3589//3589 2186//2186
+f 3577//3577 3588//3588 3589//3589
+f 4595//4595 3582//3582 4593//4593
+f 3812//3812 3590//3590 3591//3591
+f 3594//3594 3590//3590 3812//3812
+f 3592//3592 3582//3582 3598//3598
+f 3592//3592 3598//3598 3597//3597
+f 3145//3145 31//31 3753//3753
+f 3614//3614 3593//3593 3596//3596
+f 3614//3614 3590//3590 3594//3594
+f 3596//3596 3590//3590 3614//3614
+f 3595//3595 3590//3590 3596//3596
+f 3595//3595 3596//3596 3597//3597
+f 3597//3597 3598//3598 3595//3595
+f 3578//3578 3598//3598 3582//3582
+f 4601//4601 4641//4641 3600//3600
+f 3599//3599 3592//3592 3597//3597
+f 3617//3617 4600//4600 3616//3616
+f 3600//3600 3593//3593 4600//4600
+f 3596//3596 3593//3593 4597//4597
+f 3698//3698 4648//4648 3587//3587
+f 3593//3593 3600//3600 4597//4597
+f 3616//3616 3593//3593 3614//3614
+f 3593//3593 3616//3616 4600//4600
+f 3597//3597 4597//4597 3599//3599
+f 3597//3597 3596//3596 4597//4597
+f 3631//3631 4648//4648 3637//3637
+f 4419//4419 4545//4545 3631//3631
+f 3609//3609 3601//3601 4766//4766
+f 3610//3610 3601//3601 3609//3609
+f 3615//3615 3594//3594 3610//3610
+f 3601//3601 3610//3610 3611//3611
+f 3608//3608 3627//3627 3626//3626
+f 3608//3608 3604//3604 3602//3602
+f 3603//3603 2631//2631 3606//3606
+f 3604//3604 3605//3605 3602//3602
+f 3605//3605 3617//3617 3602//3602
+f 3606//3606 4272//4272 3603//3603
+f 3612//3612 3626//3626 3607//3607
+f 3615//3615 3610//3610 3608//3608
+f 3615//3615 3608//3608 3602//3602
+f 3608//3608 3610//3610 3627//3627
+f 3610//3610 3609//3609 3627//3627
+f 3610//3610 3594//3594 3611//3611
+f 2391//2391 4408//4408 1337//1337
+f 3608//3608 3626//3626 3612//3612
+f 4373//4373 3004//3004 4359//4359
+f 4385//4385 1346//1346 3613//3613
+f 3616//3616 3614//3614 3615//3615
+f 3615//3615 3614//3614 3594//3594
+f 3616//3616 3615//3615 3602//3602
+f 3616//3616 3602//3602 3617//3617
+f 3613//3613 4014//4014 4036//4036
+f 3612//3612 3604//3604 3608//3608
+f 4246//4246 4732//4732 3618//3618
+f 3628//3628 3609//3609 3670//3670
+f 3609//3609 4766//4766 3670//3670
+f 2247//2247 3619//3619 4088//4088
+f 3620//3620 3611//3611 3810//3810
+f 3620//3620 3601//3601 3611//3611
+f 4681//4681 3003//3003 4539//4539
+f 3594//3594 3812//3812 3611//3611
+f 3601//3601 3620//3620 4766//4766
+f 2145//2145 4790//4790 2136//2136
+f 3643//3643 3816//3816 3634//3634
+f 3621//3621 3816//3816 3622//3622
+f 3646//3646 3621//3621 3622//3622
+f 4636//4636 4647//4647 4833//4833
+f 3635//3635 4701//4701 4762//4762
+f 2140//2140 2142//2142 2145//2145
+f 3623//3623 4813//4813 4887//4887
+f 3637//3637 4648//4648 3698//3698
+f 2381//2381 3624//3624 3646//3646
+f 1902//1902 2971//2971 2466//2466
+f 3646//3646 3624//3624 3621//3621
+f 3633//3633 3628//3628 3670//3670
+f 3627//3627 4350//4350 3625//3625
+f 3627//3627 3625//3625 3626//3626
+f 3609//3609 3628//3628 3627//3627
+f 3628//3628 4350//4350 3627//3627
+f 3629//3629 3670//3670 3669//3669
+f 4350//4350 3628//3628 3632//3632
+f 3632//3632 3628//3628 3633//3633
+f 4406//4406 4238//4238 3618//3618
+f 4034//4034 3992//3992 2383//2383
+f 3819//3819 2782//2782 2444//2444
+f 3618//3618 4238//4238 4246//4246
+f 3633//3633 3670//3670 3629//3629
+f 4771//4771 4614//4614 4619//4619
+f 3646//3646 3622//3622 3630//3630
+f 3631//3631 4859//4859 4647//4647
+f 4043//4043 3668//3668 4334//4334
+f 3635//3635 3632//3632 3633//3633
+f 3635//3635 3633//3633 3629//3629
+f 3643//3643 3634//3634 3629//3629
+f 3634//3634 3635//3635 3629//3629
+f 3636//3636 4618//4618 4185//4185
+f 3637//3637 3698//3698 31//31
+f 3622//3622 3815//3815 3630//3630
+f 3622//3622 3816//3816 3815//3815
+f 3657//3657 3656//3656 4257//4257
+f 3648//3648 3638//3638 4270//4270
+f 3639//3639 3638//3638 3649//3649
+f 3639//3639 4257//4257 3638//3638
+f 3639//3639 3657//3657 4257//4257
+f 3640//3640 3642//3642 3649//3649
+f 3648//3648 3651//3651 3649//3649
+f 3648//3648 3649//3649 3638//3638
+f 3641//3641 2248//2248 2247//2247
+f 3639//3639 3642//3642 3657//3657
+f 3629//3629 3669//3669 3643//3643
+f 3650//3650 3645//3645 3644//3644
+f 3644//3644 3645//3645 3818//3818
+f 989//989 3821//3821 3817//3817
+f 3818//3818 3819//3819 3644//3644
+f 3819//3819 3640//3640 3644//3644
+f 3630//3630 2782//2782 3818//3818
+f 3640//3640 3819//3819 2445//2445
+f 3646//3646 3630//3630 3645//3645
+f 3645//3645 3630//3630 3818//3818
+f 2381//2381 3646//3646 3645//3645
+f 3650//3650 2381//2381 3645//3645
+f 3647//3647 3652//3652 3650//3650
+f 4669//4669 3647//3647 3651//3651
+f 3647//3647 3650//3650 3651//3651
+f 3648//3648 4669//4669 3651//3651
+f 3639//3639 3649//3649 3642//3642
+f 3651//3651 3650//3650 3644//3644
+f 3649//3649 3651//3651 3644//3644
+f 3649//3649 3644//3644 3640//3640
+f 3652//3652 2381//2381 3650//3650
+f 3662//3662 3653//3653 3666//3666
+f 3662//3662 3660//3660 3653//3653
+f 3666//3666 3680//3680 3663//3663
+f 3666//3666 3663//3663 3662//3662
+f 3663//3663 3680//3680 3654//3654
+f 3663//3663 3654//3654 3665//3665
+f 3665//3665 3654//3654 3655//3655
+f 3658//3658 3656//3656 3657//3657
+f 3944//3944 3658//3658 3673//3673
+f 405//405 4766//4766 3620//3620
+f 3657//3657 3642//3642 3661//3661
+f 3658//3658 3657//3657 3661//3661
+f 4304//4304 4599//4599 4576//4576
+f 3658//3658 3661//3661 3664//3664
+f 3673//3673 3658//3658 3664//3664
+f 3664//3664 3665//3665 3673//3673
+f 3673//3673 3665//3665 3659//3659
+f 3659//3659 3677//3677 3673//3673
+f 3642//3642 3660//3660 3661//3661
+f 3660//3660 3662//3662 3661//3661
+f 3661//3661 3662//3662 3664//3664
+f 3662//3662 3663//3663 3664//3664
+f 3663//3663 3665//3665 3664//3664
+f 3665//3665 3655//3655 3659//3659
+f 3666//3666 3667//3667 3680//3680
+f 3667//3667 3666//3666 3671//3671
+f 3668//3668 3006//3006 1065//1065
+f 4766//4766 3669//3669 3670//3670
+f 1065//1065 4334//4334 3668//3668
+f 3642//3642 3640//3640 3660//3660
+f 3640//3640 2445//2445 3660//3660
+f 3680//3680 3667//3667 3681//3681
+f 3666//3666 3653//3653 3671//3671
+f 3587//3587 3586//3586 3753//3753
+f 3658//3658 3944//3944 3656//3656
+f 3671//3671 3653//3653 3672//3672
+f 3673//3673 3677//3677 3944//3944
+f 3674//3674 3679//3679 3689//3689
+f 3679//3679 3924//3924 3678//3678
+f 3655//3655 3689//3689 3679//3679
+f 3689//3689 3655//3655 3686//3686
+f 3659//3659 3655//3655 3679//3679
+f 3686//3686 3654//3654 3675//3675
+f 3683//3683 3676//3676 3686//3686
+f 3655//3655 3654//3654 3686//3686
+f 3675//3675 3683//3683 3686//3686
+f 3677//3677 3659//3659 3678//3678
+f 3659//3659 3679//3679 3678//3678
+f 3683//3683 3675//3675 3682//3682
+f 3654//3654 3680//3680 3675//3675
+f 3680//3680 3681//3681 3675//3675
+f 3675//3675 3681//3681 3682//3682
+f 3683//3683 3682//3682 3684//3684
+f 3689//3689 3686//3686 3685//3685
+f 3685//3685 3686//3686 3676//3676
+f 3689//3689 3693//3693 3674//3674
+f 3674//3674 3693//3693 3904//3904
+f 3687//3687 3697//3697 3693//3693
+f 3688//3688 3684//3684 3830//3830
+f 3689//3689 3685//3685 3693//3693
+f 3679//3679 3674//3674 3924//3924
+f 3683//3683 3684//3684 3688//3688
+f 3685//3685 3676//3676 3687//3687
+f 3676//3676 3690//3690 3687//3687
+f 3688//3688 3830//3830 3691//3691
+f 3690//3690 3688//3688 3691//3691
+f 3691//3691 3830//3830 3699//3699
+f 3697//3697 3687//3687 3692//3692
+f 3688//3688 3690//3690 3676//3676
+f 3676//3676 3683//3683 3688//3688
+f 3693//3693 3685//3685 3687//3687
+f 3692//3692 3687//3687 3690//3690
+f 3708//3708 3709//3709 3694//3694
+f 3694//3694 3709//3709 3915//3915
+f 3695//3695 3706//3706 3696//3696
+f 3706//3706 3694//3694 3696//3696
+f 3695//3695 3696//3696 3700//3700
+f 3700//3700 3704//3704 3695//3695
+f 3690//3690 3691//3691 3696//3696
+f 3691//3691 3700//3700 3696//3696
+f 3692//3692 3915//3915 3697//3697
+f 3692//3692 3694//3694 3915//3915
+f 3694//3694 3692//3692 3696//3696
+f 3692//3692 3690//3690 3696//3696
+f 3693//3693 3697//3697 3904//3904
+f 3753//3753 31//31 3698//3698
+f 3691//3691 3699//3699 3700//3700
+f 3708//3708 3694//3694 3706//3706
+f 2788//2788 4887//4887 3280//3280
+f 3705//3705 4723//4723 3701//3701
+f 3701//3701 4723//4723 3703//3703
+f 3703//3703 3702//3702 3707//3707
+f 4723//4723 3712//3712 3703//3703
+f 3711//3711 3901//3901 3708//3708
+f 3711//3711 3707//3707 3702//3702
+f 3695//3695 3701//3701 3703//3703
+f 3704//3704 3701//3701 3695//3695
+f 3701//3701 3704//3704 3705//3705
+f 3706//3706 3703//3703 3707//3707
+f 3706//3706 3695//3695 3703//3703
+f 3707//3707 3711//3711 3708//3708
+f 3708//3708 3706//3706 3707//3707
+f 3709//3709 3708//3708 3901//3901
+f 2105//2105 4869//4869 3710//3710
+f 4721//4721 4760//4760 4688//4688
+f 3702//3702 3712//3712 3722//3722
+f 3703//3703 3712//3712 3702//3702
+f 3722//3722 3877//3877 3702//3702
+f 3711//3711 3702//3702 3876//3876
+f 3712//3712 4724//4724 3713//3713
+f 2115//2115 4813//4813 2775//2775
+f 3702//3702 3877//3877 3876//3876
+f 3722//3722 3712//3712 3720//3720
+f 2106//2106 4869//4869 2105//2105
+f 3713//3713 4721//4721 3720//3720
+f 3712//3712 3713//3713 3720//3720
+f 3863//3863 3715//3715 3714//3714
+f 3714//3714 3715//3715 3721//3721
+f 4688//4688 3863//3863 3714//3714
+f 2700//2700 3888//3888 3716//3716
+f 2700//2700 3716//3716 3717//3717
+f 3715//3715 3863//3863 3718//3718
+f 3718//3718 3863//3863 3864//3864
+f 3718//3718 3864//3864 3719//3719
+f 3720//3720 3714//3714 3721//3721
+f 3720//3720 4721//4721 3714//3714
+f 3722//3722 3716//3716 3723//3723
+f 3722//3722 3721//3721 3716//3716
+f 3722//3722 3720//3720 3721//3721
+f 3877//3877 3722//3722 3723//3723
+f 4721//4721 4688//4688 3714//3714
+f 3716//3716 3721//3721 3717//3717
+f 3721//3721 3715//3715 3717//3717
+f 3716//3716 3888//3888 3723//3723
+f 3726//3726 2700//2700 3717//3717
+f 3869//3869 3728//3728 3729//3729
+f 3725//3725 3738//3738 3724//3724
+f 3728//3728 3869//3869 3730//3730
+f 3869//3869 3729//3729 3725//3725
+f 3731//3731 2707//2707 3727//3727
+f 2707//2707 3726//3726 3727//3727
+f 3726//3726 3717//3717 3727//3727
+f 3715//3715 3728//3728 3717//3717
+f 3715//3715 3718//3718 3729//3729
+f 3728//3728 3727//3727 3717//3717
+f 3728//3728 3715//3715 3729//3729
+f 3728//3728 3730//3730 3727//3727
+f 3730//3730 3731//3731 3727//3727
+f 3718//3718 3719//3719 3729//3729
+f 3729//3729 3719//3719 3725//3725
+f 3738//3738 3732//3732 3856//3856
+f 3734//3734 3738//3738 3856//3856
+f 3861//3861 3733//3733 3737//3737
+f 3737//3737 3734//3734 3856//3856
+f 3725//3725 3719//3719 3732//3732
+f 3738//3738 3725//3725 3732//3732
+f 3735//3735 2738//2738 3734//3734
+f 3736//3736 3735//3735 3733//3733
+f 3737//3737 3733//3733 3734//3734
+f 3735//3735 3734//3734 3733//3733
+f 2738//2738 3738//3738 3734//3734
+f 3724//3724 3738//3738 2738//2738
+f 3733//3733 3861//3861 3740//3740
+f 2757//2757 2747//2747 3740//3740
+f 2757//2757 3740//3740 3747//3747
+f 2747//2747 3733//3733 3740//3740
+f 3747//3747 3739//3739 3744//3744
+f 3740//3740 3741//3741 3739//3739
+f 2747//2747 3736//3736 3733//3733
+f 3739//3739 3747//3747 3740//3740
+f 3740//3740 3861//3861 3741//3741
+f 3520//3520 3745//3745 3744//3744
+f 3747//3747 3745//3745 3748//3748
+f 3742//3742 3844//3844 2139//2139
+f 3743//3743 2753//2753 2769//2769
+f 2753//2753 3748//3748 2769//2769
+f 4661//4661 4668//4668 4425//4425
+f 3744//3744 3745//3745 3747//3747
+f 2753//2753 2757//2757 3748//3748
+f 3745//3745 3746//3746 3748//3748
+f 3748//3748 3746//3746 2769//2769
+f 3746//3746 3529//3529 2769//2769
+f 2757//2757 3747//3747 3748//3748
+f 3754//3754 3758//3758 3741//3741
+f 3758//3758 3754//3754 3751//3751
+f 3754//3754 3757//3757 4717//4717
+f 3752//3752 3749//3749 3520//3520
+f 3749//3749 3762//3762 3750//3750
+f 3750//3750 3527//3527 3749//3749
+f 3752//3752 3520//3520 3744//3744
+f 3752//3752 3744//3744 3758//3758
+f 3751//3751 3752//3752 3758//3758
+f 3753//3753 3698//3698 3587//3587
+f 4718//4718 3755//3755 3754//3754
+f 3754//3754 3755//3755 3751//3751
+f 3756//3756 3751//3751 4743//4743
+f 3710//3710 2115//2115 2775//2775
+f 3762//3762 3749//3749 3756//3756
+f 3749//3749 3752//3752 3756//3756
+f 3762//3762 3756//3756 4743//4743
+f 3752//3752 3751//3751 3756//3756
+f 3741//3741 3757//3757 3754//3754
+f 4842//4842 4840//4840 3772//3772
+f 3741//3741 3861//3861 3757//3757
+f 3739//3739 3741//3741 3758//3758
+f 3744//3744 3739//3739 3758//3758
+f 3551//3551 3537//3537 3759//3759
+f 3759//3759 3760//3760 3768//3768
+f 3767//3767 3759//3759 3768//3768
+f 3524//3524 3750//3750 3531//3531
+f 3533//3533 3531//3531 3760//3760
+f 3551//3551 3759//3759 3767//3767
+f 3541//3541 3551//3551 3767//3767
+f 3552//3552 3767//3767 3769//3769
+f 4799//4799 3761//3761 2152//2152
+f 3750//3750 3762//3762 3771//3771
+f 3531//3531 3750//3750 3771//3771
+f 3762//3762 4743//4743 4742//4742
+f 3771//3771 3762//3762 4742//4742
+f 4844//4844 4882//4882 3763//3763
+f 3840//3840 3764//3764 3765//3765
+f 3760//3760 3771//3771 3770//3770
+f 3768//3768 3760//3760 3770//3770
+f 2138//2138 2301//2301 3766//3766
+f 3767//3767 3768//3768 4745//4745
+f 3769//3769 3767//3767 4745//4745
+f 3771//3771 4742//4742 3770//3770
+f 3760//3760 3531//3531 3771//3771
+f 3759//3759 3533//3533 3760//3760
+f 3774//3774 4713//4713 4767//4767
+f 4851//4851 3772//3772 4852//4852
+f 2279//2279 3773//3773 4852//4852
+f 4840//4840 2104//2104 3772//3772
+f 3246//3246 3851//3851 1608//1608
+f 4855//4855 4840//4840 4842//4842
+f 3769//3769 4713//4713 3774//3774
+f 3553//3553 4714//4714 4744//4744
+f 3553//3553 3775//3775 4714//4714
+f 3775//3775 3776//3776 4714//4714
+f 3776//3776 3774//3774 4714//4714
+f 3776//3776 3552//3552 3774//3774
+f 3552//3552 3769//3769 3774//3774
+f 4856//4856 4855//4855 3777//3777
+f 3778//3778 3553//3553 4744//4744
+f 3778//3778 4744//4744 3779//3779
+f 3518//3518 2108//2108 2106//2106
+f 3246//3246 1608//1608 3244//3244
+f 3247//3247 3246//3246 3244//3244
+f 3782//3782 3778//3778 3779//3779
+f 3783//3783 3779//3779 3780//3780
+f 4240//4240 3554//3554 3784//3784
+f 3787//3787 3781//3781 3780//3780
+f 3785//3785 3783//3783 3789//3789
+f 3785//3785 3782//3782 3783//3783
+f 3783//3783 3780//3780 3781//3781
+f 3783//3783 3781//3781 3789//3789
+f 3782//3782 3779//3779 3783//3783
+f 4235//4235 4240//4240 3784//3784
+f 3564//3564 3785//3785 3789//3789
+f 3786//3786 3839//3839 3787//3787
+f 4635//4635 3272//3272 3788//3788
+f 3562//3562 3564//3564 3789//3789
+f 3787//3787 3793//3793 3792//3792
+f 3781//3781 3792//3792 3795//3795
+f 3789//3789 3795//3795 3562//3562
+f 3781//3781 3787//3787 3792//3792
+f 3781//3781 3795//3795 3789//3789
+f 3796//3796 3794//3794 3797//3797
+f 3795//3795 3570//3570 3562//3562
+f 3797//3797 3794//3794 3791//3791
+f 2148//2148 2522//2522 2138//2138
+f 3797//3797 3791//3791 3790//3790
+f 3792//3792 3791//3791 3794//3794
+f 3792//3792 3793//3793 3791//3791
+f 3799//3799 3795//3795 3794//3794
+f 3795//3795 3792//3792 3794//3794
+f 3795//3795 3799//3799 3570//3570
+f 3799//3799 3794//3794 3796//3796
+f 3797//3797 3790//3790 3798//3798
+f 3796//3796 3800//3800 3805//3805
+f 2148//2148 2282//2282 2522//2522
+f 3798//3798 3790//3790 3823//3823
+f 3800//3800 3798//3798 3823//3823
+f 3788//3788 3586//3586 4635//4635
+f 3798//3798 3800//3800 3796//3796
+f 3796//3796 3797//3797 3798//3798
+f 3573//3573 3570//3570 3799//3799
+f 3799//3799 3805//3805 3573//3573
+f 3799//3799 3796//3796 3805//3805
+f 3803//3803 3802//3802 3595//3595
+f 3805//3805 3806//3806 3803//3803
+f 3598//3598 3578//3578 3595//3595
+f 3800//3800 3804//3804 3806//3806
+f 3806//3806 3801//3801 3802//3802
+f 3590//3590 3595//3595 3802//3802
+f 3590//3590 3802//3802 3591//3591
+f 2522//2522 2304//2304 2138//2138
+f 3591//3591 3801//3801 3814//3814
+f 3802//3802 3801//3801 3591//3591
+f 3591//3591 4205//4205 3812//3812
+f 3573//3573 3803//3803 3572//3572
+f 3801//3801 3806//3806 3804//3804
+f 3573//3573 3805//3805 3803//3803
+f 3578//3578 3803//3803 3595//3595
+f 2138//2138 3766//3766 2148//2148
+f 4539//4539 4726//4726 4681//4681
+f 3806//3806 3802//3802 3803//3803
+f 3805//3805 3800//3800 3806//3806
+f 3821//3821 3814//3814 3807//3807
+f 3809//3809 3807//3807 3811//3811
+f 3998//3998 4047//4047 2383//2383
+f 3630//3630 3815//3815 2782//2782
+f 4668//4668 3881//3881 3882//3882
+f 4366//4366 4332//4332 4329//4329
+f 4332//4332 4026//4026 3808//3808
+f 3807//3807 3809//3809 3828//3828
+f 3671//3671 3672//3672 3822//3822
+f 3611//3611 3812//3812 3810//3810
+f 3671//3671 3822//3822 3820//3820
+f 3753//3753 4596//4596 3145//3145
+f 3591//3591 3814//3814 4205//4205
+f 4205//4205 4689//4689 3810//3810
+f 3801//3801 3804//3804 3811//3811
+f 3620//3620 3810//3810 405//405
+f 3804//3804 3809//3809 3811//3811
+f 3814//3814 3801//3801 3811//3811
+f 989//989 4205//4205 3814//3814
+f 3812//3812 4205//4205 3810//3810
+f 3766//3766 2301//2301 3813//3813
+f 3814//3814 3811//3811 3807//3807
+f 3821//3821 989//989 3814//3814
+f 3815//3815 3816//3816 3643//3643
+f 3681//3681 3667//3667 3826//3826
+f 3820//3820 3828//3828 3826//3826
+f 3807//3807 3828//3828 3820//3820
+f 3753//3753 3586//3586 4596//4596
+f 3672//3672 3817//3817 3822//3822
+f 3818//3818 2782//2782 3819//3819
+f 3667//3667 3671//3671 3820//3820
+f 3667//3667 3820//3820 3826//3826
+f 4330//4330 3002//3002 2389//2389
+f 3822//3822 3821//3821 3820//3820
+f 3821//3821 3807//3807 3820//3820
+f 3817//3817 3821//3821 3822//3822
+f 3823//3823 3790//3790 3831//3831
+f 3824//3824 3827//3827 3833//3833
+f 3809//3809 3804//3804 3823//3823
+f 3825//3825 3836//3836 3829//3829
+f 3824//3824 3825//3825 3829//3829
+f 3827//3827 3831//3831 3833//3833
+f 3824//3824 3833//3833 3825//3825
+f 3826//3826 3828//3828 3824//3824
+f 3827//3827 3809//3809 3823//3823
+f 3682//3682 3826//3826 3829//3829
+f 3826//3826 3824//3824 3829//3829
+f 3800//3800 3823//3823 3804//3804
+f 3682//3682 3681//3681 3826//3826
+f 3831//3831 3827//3827 3823//3823
+f 3684//3684 3829//3829 3836//3836
+f 3824//3824 3828//3828 3827//3827
+f 3828//3828 3809//3809 3827//3827
+f 3684//3684 3682//3682 3829//3829
+f 3831//3831 3790//3790 3832//3832
+f 3836//3836 3835//3835 3699//3699
+f 3791//3791 3793//3793 3832//3832
+f 3790//3790 3791//3791 3832//3832
+f 3830//3830 3684//3684 3836//3836
+f 3831//3831 3832//3832 3834//3834
+f 3144//3144 4599//4599 4304//4304
+f 3833//3833 3831//3831 3834//3834
+f 3825//3825 3834//3834 3835//3835
+f 3825//3825 3833//3833 3834//3834
+f 3699//3699 3835//3835 3838//3838
+f 3836//3836 3825//3825 3835//3835
+f 3699//3699 3830//3830 3836//3836
+f 3832//3832 3839//3839 4776//4776
+f 3787//3787 3839//3839 3793//3793
+f 3835//3835 4709//4709 3837//3837
+f 3835//3835 3834//3834 4709//4709
+f 3838//3838 3835//3835 3837//3837
+f 3700//3700 3699//3699 3838//3838
+f 3700//3700 3838//3838 3704//3704
+f 3838//3838 3837//3837 3705//3705
+f 3832//3832 3793//3793 3839//3839
+f 3834//3834 3832//3832 4776//4776
+f 3840//3840 3841//3841 3764//3764
+f 4781//4781 3705//3705 3837//3837
+f 1609//1609 4417//4417 3842//3842
+f 3843//3843 4843//4843 2107//2107
+f 4415//4415 4417//4417 1604//1604
+f 4855//4855 4842//4842 3777//3777
+f 4843//4843 3843//3843 4880//4880
+f 3704//3704 3838//3838 3705//3705
+f 3853//3853 3842//3842 4370//4370
+f 3843//3843 2108//2108 3536//3536
+f 2302//2302 3844//3844 2407//2407
+f 4374//4374 4423//4423 3851//3851
+f 3845//3845 2524//2524 4403//4403
+f 4374//4374 3846//3846 3271//3271
+f 3845//3845 4403//4403 3271//3271
+f 409//409 3847//3847 3242//3242
+f 1090//1090 3848//3848 3241//3241
+f 4726//4726 4256//4256 4260//4260
+f 1083//1083 1090//1090 4347//4347
+f 3849//3849 3850//3850 4423//4423
+f 4423//4423 3850//3850 3851//3851
+f 1600//1600 1608//1608 3851//3851
+f 3852//3852 4417//4417 1609//1609
+f 3842//3842 3853//3853 1609//1609
+f 1557//1557 3849//3849 1604//1604
+f 4370//4370 1603//1603 3853//3853
+f 3857//3857 3855//3855 3854//3854
+f 2524//2524 3845//3845 4387//4387
+f 3855//3855 4702//4702 3860//3860
+f 3854//3854 3855//3855 3860//3860
+f 3855//3855 3862//3862 4778//4778
+f 3856//3856 3732//3732 3857//3857
+f 3603//3603 2789//2789 4254//4254
+f 4039//4039 3893//3893 3858//3858
+f 3737//3737 3857//3857 3854//3854
+f 3893//3893 3859//3859 3858//3858
+f 3757//3757 3854//3854 3860//3860
+f 3737//3737 3856//3856 3857//3857
+f 3757//3757 3861//3861 3854//3854
+f 3861//3861 3737//3737 3854//3854
+f 2109//2109 2110//2110 4881//4881
+f 4717//4717 3757//3757 3860//3860
+f 4406//4406 2110//2110 2111//2111
+f 3862//3862 4688//4688 4778//4778
+f 3863//3863 4688//4688 3862//3862
+f 3864//3864 3863//3863 3862//3862
+f 2528//2528 3865//3865 4412//4412
+f 3857//3857 3864//3864 3862//3862
+f 3732//3732 3864//3864 3857//3857
+f 2521//2521 4412//4412 2523//2523
+f 3719//3719 3864//3864 3732//3732
+f 2528//2528 2527//2527 3865//3865
+f 3855//3855 3857//3857 3862//3862
+f 2709//2709 2707//2707 3731//3731
+f 2709//2709 3731//3731 3872//3872
+f 3867//3867 3724//3724 3874//3874
+f 2723//2723 3868//3868 3866//3866
+f 2739//2739 2723//2723 3866//3866
+f 3868//3868 2730//2730 3870//3870
+f 3867//3867 3868//3868 3870//3870
+f 3866//3866 3868//3868 3867//3867
+f 3869//3869 3724//3724 3873//3873
+f 3872//3872 3730//3730 3873//3873
+f 3730//3730 3869//3869 3873//3873
+f 3870//3870 3872//3872 3873//3873
+f 3871//3871 2710//2710 3870//3870
+f 2730//2730 3871//3871 3870//3870
+f 2710//2710 2709//2709 3872//3872
+f 2710//2710 3872//3872 3870//3870
+f 3724//3724 3867//3867 3873//3873
+f 3867//3867 3870//3870 3873//3873
+f 3874//3874 3875//3875 3866//3866
+f 3866//3866 3867//3867 3874//3874
+f 3725//3725 3724//3724 3869//3869
+f 3875//3875 2739//2739 3866//3866
+f 2738//2738 3874//3874 3724//3724
+f 3731//3731 3730//3730 3872//3872
+f 3880//3880 2701//2701 3885//3885
+f 3876//3876 3877//3877 3883//3883
+f 3883//3883 3878//3878 3886//3886
+f 3877//3877 3723//3723 3883//3883
+f 3889//3889 3885//3885 3879//3879
+f 3880//3880 3885//3885 3886//3886
+f 3881//3881 4547//4547 3882//3882
+f 3885//3885 4154//4154 3879//3879
+f 2701//2701 4154//4154 3885//3885
+f 3876//3876 3883//3883 3884//3884
+f 3886//3886 3885//3885 3889//3889
+f 3886//3886 3884//3884 3883//3883
+f 3886//3886 3889//3889 3884//3884
+f 3887//3887 3888//3888 2700//2700
+f 3723//3723 3878//3878 3883//3883
+f 3888//3888 3878//3878 3723//3723
+f 3889//3889 3879//3879 3894//3894
+f 3878//3878 3880//3880 3886//3886
+f 3889//3889 3890//3890 3892//3892
+f 3711//3711 3876//3876 3900//3900
+f 3890//3890 3902//3902 3898//3898
+f 3890//3890 3894//3894 4062//4062
+f 3898//3898 3891//3891 3892//3892
+f 3891//3891 3900//3900 3892//3892
+f 3892//3892 3890//3890 3898//3898
+f 3884//3884 3889//3889 3892//3892
+f 3900//3900 3884//3884 3892//3892
+f 3934//3934 3893//3893 4041//4041
+f 3889//3889 3894//3894 3890//3890
+f 3902//3902 3890//3890 4062//4062
+f 3876//3876 3884//3884 3900//3900
+f 3891//3891 3895//3895 3897//3897
+f 3891//3891 3898//3898 3895//3895
+f 3891//3891 3897//3897 3901//3901
+f 3895//3895 3899//3899 3919//3919
+f 3895//3895 3917//3917 3897//3897
+f 3902//3902 3903//3903 3898//3898
+f 3919//3919 3899//3899 3896//3896
+f 3895//3895 3919//3919 3917//3917
+f 3898//3898 3903//3903 3899//3899
+f 3897//3897 3917//3917 3916//3916
+f 3898//3898 3899//3899 3895//3895
+f 3900//3900 3891//3891 3901//3901
+f 3901//3901 3711//3711 3900//3900
+f 3709//3709 3897//3897 3916//3916
+f 3709//3709 3901//3901 3897//3897
+f 3911//3911 3903//3903 4064//4064
+f 3911//3911 4064//4064 3912//3912
+f 3903//3903 3902//3902 4064//4064
+f 3899//3899 3903//3903 3896//3896
+f 3903//3903 3911//3911 3896//3896
+f 3904//3904 3697//3697 3918//3918
+f 3906//3906 4054//4054 3928//3928
+f 3905//3905 3907//3907 3928//3928
+f 3906//3906 3928//3928 3913//3913
+f 3928//3928 3907//3907 3913//3913
+f 3905//3905 3908//3908 3910//3910
+f 3907//3907 3905//3905 3910//3910
+f 3910//3910 3908//3908 3909//3909
+f 3908//3908 3920//3920 3914//3914
+f 3909//3909 3908//3908 3914//3914
+f 3896//3896 3907//3907 3910//3910
+f 3911//3911 3913//3913 3907//3907
+f 3911//3911 3912//3912 3913//3913
+f 3912//3912 3906//3906 3913//3913
+f 3920//3920 3918//3918 3914//3914
+f 3915//3915 3709//3709 3916//3916
+f 3697//3697 3915//3915 3918//3918
+f 3915//3915 3916//3916 3918//3918
+f 3904//3904 3918//3918 3920//3920
+f 3916//3916 3917//3917 3914//3914
+f 3918//3918 3916//3916 3914//3914
+f 3917//3917 3919//3919 3909//3909
+f 3917//3917 3909//3909 3914//3914
+f 3919//3919 3896//3896 3910//3910
+f 3919//3919 3910//3910 3909//3909
+f 3896//3896 3911//3911 3907//3907
+f 3904//3904 3920//3920 3674//3674
+f 3920//3920 3929//3929 3674//3674
+f 3920//3920 3908//3908 3929//3929
+f 3908//3908 3905//3905 3926//3926
+f 3922//3922 3921//3921 3923//3923
+f 3923//3923 3930//3930 3927//3927
+f 3922//3922 3923//3923 3927//3927
+f 3924//3924 3674//3674 3929//3929
+f 3924//3924 3929//3929 3925//3925
+f 3926//3926 3930//3930 3937//3937
+f 3926//3926 3937//3937 3925//3925
+f 3930//3930 3926//3926 3927//3927
+f 3905//3905 3928//3928 3927//3927
+f 3929//3929 3908//3908 3926//3926
+f 3926//3926 3905//3905 3927//3927
+f 3928//3928 4054//4054 3927//3927
+f 4054//4054 3922//3922 3927//3927
+f 3929//3929 3926//3926 3925//3925
+f 3949//3949 3933//3933 3930//3930
+f 3930//3930 3938//3938 3949//3949
+f 3932//3932 3931//3931 3933//3933
+f 3949//3949 3932//3932 3933//3933
+f 3678//3678 3931//3931 3677//3677
+f 3934//3934 3859//3859 3893//3893
+f 3939//3939 3935//3935 4049//4049
+f 3935//3935 3941//3941 3938//3938
+f 2309//2309 2306//2306 3936//3936
+f 3678//3678 3924//3924 3925//3925
+f 3925//3925 3931//3931 3678//3678
+f 3925//3925 3937//3937 3931//3931
+f 3937//3937 3930//3930 3933//3933
+f 3937//3937 3933//3933 3931//3931
+f 3930//3930 3923//3923 3938//3938
+f 3921//3921 3935//3935 3923//3923
+f 3923//3923 3935//3935 3938//3938
+f 3932//3932 3677//3677 3931//3931
+f 3939//3939 4046//4046 3940//3940
+f 3940//3940 3941//3941 3939//3939
+f 3942//3942 3946//3946 3960//3960
+f 3677//3677 3932//3932 3943//3943
+f 3944//3944 3677//3677 3943//3943
+f 3944//3944 3943//3943 3947//3947
+f 3656//3656 3944//3944 3947//3947
+f 3942//3942 3656//3656 3947//3947
+f 3952//3952 3951//3951 4639//4639
+f 3952//3952 3957//3957 3945//3945
+f 3945//3945 3957//3957 3958//3958
+f 3960//3960 3946//3946 3958//3958
+f 3946//3946 3945//3945 3958//3958
+f 3946//3946 3942//3942 3947//3947
+f 3940//3940 3951//3951 3953//3953
+f 3946//3946 3947//3947 3950//3950
+f 3948//3948 3946//3946 3950//3950
+f 3950//3950 3947//3947 3943//3943
+f 3945//3945 3948//3948 3953//3953
+f 3932//3932 3949//3949 3950//3950
+f 3932//3932 3950//3950 3943//3943
+f 3949//3949 3938//3938 3948//3948
+f 3949//3949 3948//3948 3950//3950
+f 3941//3941 3953//3953 3948//3948
+f 3941//3941 3948//3948 3938//3938
+f 3940//3940 3953//3953 3941//3941
+f 3952//3952 3945//3945 3953//3953
+f 3951//3951 3952//3952 3953//3953
+f 3945//3945 3946//3946 3948//3948
+f 3656//3656 3942//3942 4564//4564
+f 3954//3954 4644//4644 4229//4229
+f 3955//3955 2952//2952 4617//4617
+f 3952//3952 4639//4639 3956//3956
+f 3952//3952 3956//3956 3957//3957
+f 4628//4628 3960//3960 3958//3958
+f 3959//3959 4564//4564 3942//3942
+f 3960//3960 3959//3959 3942//3942
+f 4656//4656 4657//4657 4654//4654
+f 3958//3958 3957//3957 4563//4563
+f 4583//4583 4671//4671 4579//4579
+f 3961//3961 4623//4623 3962//3962
+f 4592//4592 4675//4675 3588//3588
+f 3968//3968 3961//3961 3964//3964
+f 3961//3961 3962//3962 3964//3964
+f 3965//3965 3968//3968 3963//3963
+f 3968//3968 3964//3964 3963//3963
+f 3965//3965 3963//3963 3966//3966
+f 3986//3986 3970//3970 3969//3969
+f 3965//3965 3966//3966 3970//3970
+f 3968//3968 3965//3965 3967//3967
+f 3961//3961 3983//3983 3982//3982
+f 3961//3961 3968//3968 3983//3983
+f 3961//3961 3982//3982 4623//4623
+f 2302//2302 3813//3813 2301//2301
+f 3973//3973 4229//4229 4644//4644
+f 3969//3969 3970//3970 3971//3971
+f 4160//4160 3985//3985 3995//3995
+f 3971//3971 3995//3995 3974//3974
+f 4160//4160 3995//3995 3972//3972
+f 4624//4624 3973//3973 4644//4644
+f 3968//3968 3967//3967 3983//3983
+f 3893//3893 4039//4039 4041//4041
+f 3974//3974 4195//4195 3975//3975
+f 4195//4195 3974//3974 3985//3985
+f 3974//3974 3976//3976 3969//3969
+f 321//321 4428//4428 3882//3882
+f 3969//3969 3976//3976 3987//3987
+f 3975//3975 4220//4220 3976//3976
+f 3974//3974 3975//3975 3976//3976
+f 3978//3978 4217//4217 3977//3977
+f 3978//3978 3977//3977 3987//3987
+f 3987//3987 3976//3976 3978//3978
+f 3987//3987 3977//3977 3979//3979
+f 3967//3967 3980//3980 3984//3984
+f 3984//3984 3981//3981 3973//3973
+f 3981//3981 3984//3984 3980//3980
+f 4229//4229 3973//3973 3981//3981
+f 3982//3982 3983//3983 3984//3984
+f 4624//4624 3982//3982 3973//3973
+f 3969//3969 3971//3971 3974//3974
+f 3985//3985 3974//3974 3995//3995
+f 3969//3969 3987//3987 3986//3986
+f 3967//3967 3965//3965 3986//3986
+f 3970//3970 3986//3986 3965//3965
+f 3973//3973 3982//3982 3984//3984
+f 3979//3979 3967//3967 3986//3986
+f 3979//3979 3980//3980 3967//3967
+f 3987//3987 3979//3979 3986//3986
+f 3983//3983 3967//3967 3984//3984
+f 4004//4004 3988//3988 3999//3999
+f 3992//3992 3991//3991 3966//3966
+f 2383//2383 3992//3992 3989//3989
+f 3998//3998 3997//3997 3990//3990
+f 3990//3990 4047//4047 3998//3998
+f 2383//2383 3989//3989 3998//3998
+f 4041//4041 4268//4268 4151//4151
+f 3989//3989 3962//3962 3998//3998
+f 3951//3951 4046//4046 3990//3990
+f 4046//4046 3951//3951 3940//3940
+f 4639//4639 3951//3951 3990//3990
+f 3999//3999 4003//4003 4004//4004
+f 4034//4034 4044//4044 3991//3991
+f 4034//4034 3991//3991 3992//3992
+f 3970//3970 3966//3966 3993//3993
+f 3971//3971 3970//3970 3993//3993
+f 3971//3971 3993//3993 3988//3988
+f 3995//3995 3971//3971 3988//3988
+f 3996//3996 3994//3994 3972//3972
+f 3972//3972 3995//3995 3996//3996
+f 4639//4639 3990//3990 3997//3997
+f 3962//3962 3997//3997 3998//3998
+f 3989//3989 3964//3964 3962//3962
+f 3992//3992 3963//3963 3989//3989
+f 3963//3963 3964//3964 3989//3989
+f 3966//3966 3963//3963 3992//3992
+f 3972//3972 3994//3994 4013//4013
+f 3988//3988 3996//3996 3995//3995
+f 3988//3988 4004//4004 3996//3996
+f 3991//3991 3999//3999 3993//3993
+f 3988//3988 3993//3993 3999//3999
+f 3966//3966 3991//3991 3993//3993
+f 2302//2302 2114//2114 2112//2112
+f 4160//4160 3972//3972 4013//4013
+f 4019//4019 4020//4020 4001//4001
+f 4011//4011 4004//4004 4005//4005
+f 4004//4004 4001//4001 4005//4005
+f 4014//4014 4019//4019 4037//4037
+f 4019//4019 4002//4002 4037//4037
+f 4002//4002 4001//4001 4000//4000
+f 4019//4019 4001//4001 4002//4002
+f 4000//4000 4001//4001 4003//4003
+f 4004//4004 4003//4003 4001//4001
+f 4001//4001 4020//4020 4005//4005
+f 4016//4016 4006//4006 4012//4012
+f 4005//4005 4016//4016 4012//4012
+f 2160//2160 4007//4007 2669//2669
+f 4018//4018 4160//4160 4008//4008
+f 4160//4160 4013//4013 4008//4008
+f 3994//3994 3996//3996 4011//4011
+f 3996//3996 4004//4004 4011//4011
+f 4008//4008 4013//4013 4006//4006
+f 4009//4009 2669//2669 4007//4007
+f 2114//2114 2302//2302 2097//2097
+f 4010//4010 4293//4293 3144//3144
+f 4005//4005 4020//4020 4016//4016
+f 4011//4011 4005//4005 4012//4012
+f 4013//4013 3994//3994 4012//4012
+f 3994//3994 4011//4011 4012//4012
+f 4013//4013 4012//4012 4006//4006
+f 4036//4036 4014//4014 4037//4037
+f 4015//4015 1307//1307 4016//4016
+f 1307//1307 4006//4006 4016//4016
+f 4017//4017 4018//4018 4008//4008
+f 4017//4017 4008//4008 4006//4006
+f 1342//1342 4019//4019 4014//4014
+f 4019//4019 1342//1342 4020//4020
+f 1342//1342 4015//4015 4020//4020
+f 4015//4015 4016//4016 4020//4020
+f 3813//3813 2302//2302 2112//2112
+f 4751//4751 4428//4428 321//321
+f 1207//1207 4021//4021 2658//2658
+f 1200//1200 1207//1207 2658//2658
+f 4023//4023 2658//2658 4021//4021
+f 4022//4022 4023//4023 4021//4021
+f 4021//4021 4028//4028 4025//4025
+f 4366//4366 4024//4024 4092//4092
+f 4024//4024 4786//4786 4072//4072
+f 4182//4182 4024//4024 4366//4366
+f 4021//4021 4025//4025 4022//4022
+f 4366//4366 4092//4092 4027//4027
+f 4026//4026 4366//4366 4027//4027
+f 4025//4025 4026//4026 4022//4022
+f 4022//4022 4026//4026 4027//4027
+f 4366//4366 4026//4026 4332//4332
+f 4043//4043 4889//4889 2405//2405
+f 4026//4026 4025//4025 3808//3808
+f 3808//3808 4025//4025 4226//4226
+f 4028//4028 1210//1210 4029//4029
+f 1207//1207 1210//1210 4028//4028
+f 4021//4021 1207//1207 4028//4028
+f 2490//2490 4030//4030 4325//4325
+f 4028//4028 4029//4029 4031//4031
+f 4025//4025 4028//4028 4226//4226
+f 4226//4226 4028//4028 4031//4031
+f 4030//4030 4032//4032 4325//4325
+f 4396//4396 4248//4248 1636//1636
+f 4396//4396 1636//1636 1619//1619
+f 4033//4033 4044//4044 4034//4034
+f 1211//1211 4183//4183 4029//4029
+f 4000//4000 4003//4003 4045//4045
+f 4035//4035 4422//4422 4328//4328
+f 4036//4036 4037//4037 4038//4038
+f 4037//4037 4002//4002 4038//4038
+f 4038//4038 4002//4002 4000//4000
+f 4000//4000 4045//4045 4042//4042
+f 4039//4039 4040//4040 4041//4041
+f 4038//4038 4042//4042 4383//4383
+f 4038//4038 4000//4000 4042//4042
+f 4031//4031 4183//4183 2385//2385
+f 4045//4045 4386//4386 4042//4042
+f 4043//4043 4888//4888 4889//4889
+f 3921//3921 4056//4056 4049//4049
+f 4039//4039 4273//4273 4040//4040
+f 3935//3935 3939//3939 3941//3941
+f 3921//3921 4049//4049 3935//3935
+f 4386//4386 4045//4045 4033//4033
+f 4003//4003 3999//3999 4044//4044
+f 4003//4003 4044//4044 4045//4045
+f 3990//3990 4046//4046 4047//4047
+f 4376//4376 4232//4232 4056//4056
+f 4376//4376 4056//4056 4048//4048
+f 4056//4056 4232//4232 4049//4049
+f 3668//3668 4043//4043 2412//2412
+f 4046//4046 3939//3939 4050//4050
+f 4046//4046 4050//4050 4237//4237
+f 3939//3939 4049//4049 4050//4050
+f 4050//4050 4049//4049 4232//4232
+f 4056//4056 3921//3921 4051//4051
+f 4245//4245 4052//4052 4048//4048
+f 4053//4053 4048//4048 4051//4051
+f 4053//4053 4245//4245 4048//4048
+f 4055//4055 4053//4053 4051//4051
+f 3922//3922 4054//4054 4055//4055
+f 4051//4051 3922//3922 4055//4055
+f 3921//3921 3922//3922 4051//4051
+f 4056//4056 4051//4051 4048//4048
+f 2392//2392 2395//2395 4619//4619
+f 4057//4057 4245//4245 4058//4058
+f 4053//4053 4060//4060 4058//4058
+f 4245//4245 4053//4053 4058//4058
+f 4055//4055 4054//4054 4059//4059
+f 4053//4053 4055//4055 4060//4060
+f 4059//4059 3906//3906 4075//4075
+f 4058//4058 4060//4060 4067//4067
+f 4061//4061 4058//4058 4067//4067
+f 4060//4060 4059//4059 4073//4073
+f 4067//4067 4060//4060 4073//4073
+f 4059//4059 4075//4075 4073//4073
+f 3906//3906 3912//3912 4075//4075
+f 4054//4054 3906//3906 4059//4059
+f 4055//4055 4059//4059 4060//4060
+f 4057//4057 4058//4058 4061//4061
+f 4057//4057 4061//4061 4076//4076
+f 4064//4064 3902//3902 4065//4065
+f 4078//4078 4070//4070 4069//4069
+f 3902//3902 4062//4062 4065//4065
+f 4067//4067 4073//4073 4063//4063
+f 3912//3912 4064//4064 4066//4066
+f 4064//4064 4065//4065 4066//4066
+f 4067//4067 4068//4068 4069//4069
+f 4070//4070 4078//4078 4071//4071
+f 4335//4335 4070//4070 4071//4071
+f 4072//4072 4335//4335 4071//4071
+f 4063//4063 4073//4073 4066//4066
+f 4073//4073 4075//4075 4066//4066
+f 4067//4067 4063//4063 4068//4068
+f 4066//4066 4065//4065 4083//4083
+f 4061//4061 4067//4067 4069//4069
+f 4078//4078 4069//4069 4074//4074
+f 4069//4069 4068//4068 4074//4074
+f 4068//4068 4081//4081 4074//4074
+f 4068//4068 4063//4063 4081//4081
+f 4063//4063 4066//4066 4083//4083
+f 4072//4072 4786//4786 4335//4335
+f 4075//4075 3912//3912 4066//4066
+f 4247//4247 4076//4076 4335//4335
+f 4076//4076 4061//4061 4070//4070
+f 4076//4076 4070//4070 4335//4335
+f 4070//4070 4061//4061 4069//4069
+f 4063//4063 4083//4083 4081//4081
+f 4155//4155 4077//4077 4062//4062
+f 4071//4071 4078//4078 4086//4086
+f 4086//4086 4078//4078 4085//4085
+f 4078//4078 4074//4074 4085//4085
+f 4086//4086 4102//4102 4071//4071
+f 4065//4065 4062//4062 4077//4077
+f 4086//4086 4105//4105 4102//4102
+f 4062//4062 3894//3894 4155//4155
+f 4079//4079 4080//4080 4082//4082
+f 4074//4074 4081//4081 4087//4087
+f 4082//4082 4080//4080 4087//4087
+f 4077//4077 4155//4155 4079//4079
+f 4077//4077 4079//4079 4082//4082
+f 4083//4083 4077//4077 4082//4082
+f 4083//4083 4065//4065 4077//4077
+f 4081//4081 4083//4083 4082//4082
+f 4081//4081 4082//4082 4087//4087
+f 4084//4084 4085//4085 4157//4157
+f 4086//4086 4085//4085 4084//4084
+f 4157//4157 4087//4087 4080//4080
+f 4085//4085 4074//4074 4087//4087
+f 4156//4156 4086//4086 4084//4084
+f 4086//4086 4156//4156 4105//4105
+f 4085//4085 4087//4087 4157//4157
+f 4096//4096 4107//4107 4090//4090
+f 3619//3619 2284//2284 4088//4088
+f 4089//4089 4104//4104 4090//4090
+f 4090//4090 4104//4104 4096//4096
+f 4089//4089 4090//4090 4100//4100
+f 4102//4102 4089//4089 4100//4100
+f 4090//4090 4107//4107 4091//4091
+f 4100//4100 4090//4090 4091//4091
+f 4107//4107 4094//4094 4108//4108
+f 4027//4027 4092//4092 4095//4095
+f 4104//4104 4106//4106 4137//4137
+f 4104//4104 4137//4137 4096//4096
+f 4071//4071 4102//4102 4072//4072
+f 4024//4024 4072//4072 4093//4093
+f 4092//4092 4024//4024 4093//4093
+f 4108//4108 4094//4094 4144//4144
+f 4092//4092 4093//4093 4101//4101
+f 4095//4095 4092//4092 4101//4101
+f 2445//2445 3653//3653 3660//3660
+f 4107//4107 4096//4096 4094//4094
+f 4103//4103 4098//4098 4097//4097
+f 4103//4103 4108//4108 4098//4098
+f 4108//4108 4144//4144 4099//4099
+f 4100//4100 4091//4091 4101//4101
+f 4093//4093 4100//4100 4101//4101
+f 4102//4102 4100//4100 4093//4093
+f 4102//4102 4093//4093 4072//4072
+f 4103//4103 4097//4097 4095//4095
+f 4023//4023 4098//4098 4099//4099
+f 4103//4103 4095//4095 4091//4091
+f 4091//4091 4095//4095 4101//4101
+f 4105//4105 4104//4104 4089//4089
+f 4097//4097 4027//4027 4095//4095
+f 4105//4105 4156//4156 4106//4106
+f 4104//4104 4105//4105 4106//4106
+f 4022//4022 4027//4027 4097//4097
+f 4102//4102 4105//4105 4089//4089
+f 4107//4107 4103//4103 4091//4091
+f 4107//4107 4108//4108 4103//4103
+f 4023//4023 4022//4022 4098//4098
+f 4023//4023 4099//4099 1199//1199
+f 4098//4098 4108//4108 4099//4099
+f 4098//4098 4022//4022 4097//4097
+f 1198//1198 4110//4110 1197//1197
+f 4110//4110 1198//1198 4112//4112
+f 4114//4114 4139//4139 4133//4133
+f 4109//4109 2995//2995 1075//1075
+f 4110//4110 4139//4139 4114//4114
+f 4110//4110 4111//4111 4139//4139
+f 4111//4111 4140//4140 4139//4139
+f 1197//1197 4110//4110 4114//4114
+f 1198//1198 1197//1197 1204//1204
+f 4112//4112 4144//4144 4111//4111
+f 4110//4110 4112//4112 4111//4111
+f 4113//4113 4114//4114 4116//4116
+f 4112//4112 1199//1199 4099//4099
+f 4099//4099 4144//4144 4112//4112
+f 4113//4113 4124//4124 1202//1202
+f 4113//4113 4116//4116 4124//4124
+f 4118//4118 4122//4122 4115//4115
+f 2509//2509 4039//4039 3858//3858
+f 4121//4121 4122//4122 4133//4133
+f 4122//4122 4121//4121 4115//4115
+f 4133//4133 4122//4122 4116//4116
+f 4117//4117 4115//4115 4131//4131
+f 4128//4128 4118//4118 4119//4119
+f 4118//4118 4128//4128 4120//4120
+f 4121//4121 4138//4138 4142//4142
+f 4128//4128 4119//4119 4125//4125
+f 4122//4122 4120//4120 4130//4130
+f 4122//4122 4118//4118 4120//4120
+f 4116//4116 4122//4122 4130//4130
+f 4118//4118 4117//4117 4119//4119
+f 4115//4115 4117//4117 4118//4118
+f 4115//4115 4121//4121 4123//4123
+f 4121//4121 4142//4142 4123//4123
+f 4131//4131 4115//4115 4123//4123
+f 2666//2666 2663//2663 4125//4125
+f 4124//4124 4116//4116 4130//4130
+f 4119//4119 4127//4127 4125//4125
+f 4126//4126 4127//4127 4119//4119
+f 4117//4117 4126//4126 4119//4119
+f 2693//2693 4126//4126 4117//4117
+f 2693//2693 4117//4117 4131//4131
+f 4138//4138 4133//4133 4139//4139
+f 4128//4128 4132//4132 4120//4120
+f 4121//4121 4133//4133 4138//4138
+f 4132//4132 4129//4129 4120//4120
+f 4129//4129 4124//4124 4130//4130
+f 4120//4120 4129//4129 4130//4130
+f 4146//4146 2693//2693 4131//4131
+f 4131//4131 4123//4123 4141//4141
+f 2663//2663 4128//4128 4125//4125
+f 2663//2663 4132//4132 4128//4128
+f 4116//4116 4114//4114 4133//4133
+f 4134//4134 4140//4140 4145//4145
+f 4145//4145 4140//4140 4143//4143
+f 4142//4142 4134//4134 4136//4136
+f 4142//4142 4138//4138 4134//4134
+f 4141//4141 4142//4142 4136//4136
+f 4135//4135 4141//4141 4136//4136
+f 4136//4136 4134//4134 4137//4137
+f 4137//4137 4134//4134 4145//4145
+f 4140//4140 4134//4134 4138//4138
+f 4140//4140 4138//4138 4139//4139
+f 4106//4106 4135//4135 4136//4136
+f 4106//4106 4136//4136 4137//4137
+f 4140//4140 4111//4111 4143//4143
+f 4141//4141 4123//4123 4142//4142
+f 4094//4094 4096//4096 4145//4145
+f 4094//4094 4145//4145 4143//4143
+f 2445//2445 3672//3672 3653//3653
+f 4143//4143 4111//4111 4144//4144
+f 4094//4094 4143//4143 4144//4144
+f 4096//4096 4137//4137 4145//4145
+f 4131//4131 4141//4141 4146//4146
+f 4157//4157 4153//4153 4147//4147
+f 4157//4157 4080//4080 4153//4153
+f 4153//4153 4080//4080 4148//4148
+f 4080//4080 4079//4079 4148//4148
+f 3766//3766 3813//3813 4757//4757
+f 4148//4148 4079//4079 4149//4149
+f 4079//4079 4155//4155 4149//4149
+f 4146//4146 4141//4141 4147//4147
+f 4153//4153 4146//4146 4147//4147
+f 4150//4150 4151//4151 4268//4268
+f 4152//4152 4153//4153 4148//4148
+f 4152//4152 4146//4146 4153//4153
+f 4154//4154 2686//2686 3879//3879
+f 2686//2686 4152//4152 4148//4148
+f 3879//3879 2686//2686 4149//4149
+f 2686//2686 4148//4148 4149//4149
+f 4155//4155 3894//3894 4149//4149
+f 3894//3894 3879//3879 4149//4149
+f 4106//4106 4156//4156 4135//4135
+f 4156//4156 4084//4084 4135//4135
+f 4135//4135 4084//4084 4147//4147
+f 4084//4084 4157//4157 4147//4147
+f 4141//4141 4135//4135 4147//4147
+f 4169//4169 4203//4203 4159//4159
+f 1305//1305 4159//4159 4158//4158
+f 4195//4195 3985//3985 4158//4158
+f 1305//1305 4166//4166 4159//4159
+f 4203//4203 4195//4195 4159//4159
+f 1306//1306 4158//4158 4018//4018
+f 1306//1306 1305//1305 4158//4158
+f 4017//4017 1306//1306 4018//4018
+f 4159//4159 4195//4195 4158//4158
+f 4160//4160 4018//4018 3985//3985
+f 4158//4158 3985//3985 4018//4018
+f 1231//1231 4164//4164 4163//4163
+f 1225//1225 1230//1230 4167//4167
+f 4161//4161 1225//1225 4167//4167
+f 4166//4166 4161//4161 4167//4167
+f 1231//1231 4163//4163 4162//4162
+f 4164//4164 1224//1224 4192//4192
+f 4162//4162 4163//4163 1230//1230
+f 1245//1245 1231//1231 4162//4162
+f 4224//4224 4165//4165 4214//4214
+f 4168//4168 4165//4165 4224//4224
+f 1230//1230 4165//4165 4168//4168
+f 1230//1230 4163//4163 4165//4165
+f 4167//4167 1230//1230 4168//4168
+f 4168//4168 4224//4224 4169//4169
+f 4163//4163 4164//4164 4214//4214
+f 4163//4163 4214//4214 4165//4165
+f 4159//4159 4166//4166 4169//4169
+f 4166//4166 4167//4167 4169//4169
+f 4167//4167 4168//4168 4169//4169
+f 4164//4164 4201//4201 4214//4214
+f 4757//4757 2148//2148 3766//3766
+f 4201//4201 4164//4164 4192//4192
+f 3241//3241 1603//1603 4662//4662
+f 4347//4347 4348//4348 4170//4170
+f 1084//1084 4170//4170 4173//4173
+f 4175//4175 4171//4171 4172//4172
+f 4173//4173 4174//4174 4175//4175
+f 4174//4174 4171//4171 4175//4175
+f 4175//4175 4176//4176 4173//4173
+f 4176//4176 1084//1084 4173//4173
+f 4170//4170 4348//4348 4173//4173
+f 4175//4175 4172//4172 4177//4177
+f 4177//4177 4172//4172 4277//4277
+f 4173//4173 4348//4348 4174//4174
+f 4349//4349 4306//4306 4174//4174
+f 4174//4174 4306//4306 4171//4171
+f 2892//2892 2782//2782 3815//3815
+f 1078//1078 1076//1076 4177//4177
+f 4323//4323 4368//4368 4348//4348
+f 4368//4368 4349//4349 4174//4174
+f 4368//4368 4174//4174 4348//4348
+f 1078//1078 4177//4177 4277//4277
+f 1076//1076 1084//1084 4176//4176
+f 4176//4176 4175//4175 4177//4177
+f 1076//1076 4176//4176 4177//4177
+f 4180//4180 4178//4178 4279//4279
+f 4180//4180 4179//4179 4178//4178
+f 4180//4180 4279//4279 2331//2331
+f 1073//1073 1078//1078 4179//4179
+f 1072//1072 1073//1073 4179//4179
+f 4180//4180 1072//1072 4179//4179
+f 1074//1074 4180//4180 2331//2331
+f 1074//1074 1072//1072 4180//4180
+f 1078//1078 4277//4277 4179//4179
+f 4179//4179 4277//4277 4178//4178
+f 4172//4172 4181//4181 4277//4277
+f 4024//4024 4182//4182 4786//4786
+f 4029//4029 4183//4183 4031//4031
+f 3808//3808 4226//4226 2387//2387
+f 4659//4659 4664//4664 4666//4666
+f 4279//4279 4282//4282 4608//4608
+f 2113//2113 4184//4184 2281//2281
+f 4185//4185 4618//4618 4362//4362
+f 4364//4364 4186//4186 4236//4236
+f 2331//2331 4279//4279 4608//4608
+f 4184//4184 2280//2280 2281//2281
+f 4197//4197 4187//4187 4326//4326
+f 2309//2309 2308//2308 2306//2306
+f 4187//4187 4222//4222 4188//4188
+f 4187//4187 4188//4188 4191//4191
+f 4191//4191 4188//4188 4193//4193
+f 4189//4189 2309//2309 4190//4190
+f 4761//4761 4805//4805 3265//3265
+f 4187//4187 4191//4191 1216//1216
+f 1216//1216 2569//2569 4326//4326
+f 4326//4326 4187//4187 1216//1216
+f 4192//4192 1223//1223 4191//4191
+f 4192//4192 4191//4191 4193//4193
+f 4191//4191 1223//1223 1216//1216
+f 4201//4201 4192//4192 4193//4193
+f 4222//4222 4194//4194 4220//4220
+f 4222//4222 4200//4200 4194//4194
+f 4689//4689 405//405 3810//3810
+f 3975//3975 4195//4195 4211//4211
+f 2990//2990 4228//4228 4196//4196
+f 4199//4199 4197//4197 4209//4209
+f 4197//4197 2474//2474 4209//4209
+f 4360//4360 4228//4228 2186//2186
+f 2990//2990 4189//4189 4190//4190
+f 2442//2442 4198//4198 4221//4221
+f 4194//4194 4200//4200 4216//4216
+f 4216//4216 4200//4200 4199//4199
+f 4199//4199 4200//4200 4197//4197
+f 4193//4193 4202//4202 4201//4201
+f 4201//4201 4202//4202 4210//4210
+f 4193//4193 4188//4188 4202//4202
+f 4222//4222 4187//4187 4200//4200
+f 4169//4169 4224//4224 4203//4203
+f 4204//4204 4395//4395 4213//4213
+f 4195//4195 4203//4203 4211//4211
+f 4187//4187 4197//4197 4200//4200
+f 989//989 4689//4689 4205//4205
+f 4213//4213 4206//4206 4207//4207
+f 3979//3979 3977//3977 4219//4219
+f 4218//4218 3979//3979 4219//4219
+f 4209//4209 4233//4233 4212//4212
+f 4202//4202 4188//4188 4211//4211
+f 4216//4216 4199//4199 4208//4208
+f 4199//4199 4209//4209 4212//4212
+f 4199//4199 4212//4212 4208//4208
+f 4210//4210 4202//4202 4211//4211
+f 4208//4208 4212//4212 4223//4223
+f 4224//4224 4214//4214 4210//4210
+f 4229//4229 3981//3981 4215//4215
+f 4213//4213 2114//2114 2097//2097
+f 3981//3981 3980//3980 4215//4215
+f 4207//4207 4204//4204 4213//4213
+f 4210//4210 4214//4214 4201//4201
+f 3980//3980 4221//4221 4215//4215
+f 3980//3980 3979//3979 4218//4218
+f 4221//4221 3980//3980 4218//4218
+f 4219//4219 4216//4216 4208//4208
+f 4219//4219 4217//4217 4216//4216
+f 4218//4218 4219//4219 4208//4208
+f 4220//4220 4194//4194 3978//3978
+f 4216//4216 4217//4217 4194//4194
+f 4194//4194 4217//4217 3978//3978
+f 4221//4221 4218//4218 4225//4225
+f 3976//3976 4220//4220 3978//3978
+f 4221//4221 4225//4225 2442//2442
+f 4188//4188 4222//4222 4211//4211
+f 4222//4222 4220//4220 3975//3975
+f 4211//4211 4222//4222 3975//3975
+f 4212//4212 4231//4231 4223//4223
+f 3977//3977 4217//4217 4219//4219
+f 4223//4223 4231//4231 4225//4225
+f 4203//4203 4224//4224 4210//4210
+f 4225//4225 4218//4218 4223//4223
+f 4211//4211 4203//4203 4210//4210
+f 3882//3882 4428//4428 4668//4668
+f 4208//4208 4223//4223 4218//4218
+f 4236//4236 4235//4235 4362//4362
+f 4364//4364 4236//4236 4362//4362
+f 2387//2387 4226//4226 4031//4031
+f 4227//4227 4625//4625 4557//4557
+f 4036//4036 4038//4038 4383//4383
+f 4250//4250 4246//4246 4238//4238
+f 4241//4241 4186//4186 4250//4250
+f 4241//4241 4250//4250 4238//4238
+f 4042//4042 4386//4386 4383//4383
+f 2556//2556 4228//4228 4324//4324
+f 3954//3954 4229//4229 4215//4215
+f 3954//3954 4215//4215 4198//4198
+f 4566//4566 3954//3954 4198//4198
+f 4150//4150 2472//2472 4230//4230
+f 2473//2473 2472//2472 2442//2442
+f 2624//2624 4198//4198 2442//2442
+f 4233//4233 4231//4231 4212//4212
+f 2474//2474 4233//4233 4209//4209
+f 4225//4225 2473//2473 2442//2442
+f 4225//4225 4231//4231 2473//2473
+f 4221//4221 4198//4198 4215//4215
+f 4050//4050 4232//4232 2408//2408
+f 4230//4230 2473//2473 2468//2468
+f 3934//3934 4041//4041 4151//4151
+f 4231//4231 4233//4233 2468//2468
+f 2473//2473 4231//4231 2468//2468
+f 4659//4659 4233//4233 2474//2474
+f 4233//4233 4659//4659 2468//2468
+f 4234//4234 4569//4569 4591//4591
+f 4240//4240 4235//4235 4236//4236
+f 4241//4241 4240//4240 4236//4236
+f 4186//4186 4241//4241 4236//4236
+f 3991//3991 4044//4044 3999//3999
+f 4047//4047 4046//4046 4237//4237
+f 4407//4407 4241//4241 4238//4238
+f 4239//4239 4591//4591 4569//4569
+f 2408//2408 4232//4232 4376//4376
+f 4251//4251 2672//2672 3153//3153
+f 4033//4033 4045//4045 4044//4044
+f 4407//4407 3554//3554 4240//4240
+f 4240//4240 4241//4241 4407//4407
+f 4618//4618 4242//4242 4362//4362
+f 4243//4243 4249//4249 4365//4365
+f 4244//4244 4052//4052 4245//4245
+f 4365//4365 4186//4186 4364//4364
+f 4052//4052 4244//4244 2397//2397
+f 4376//4376 4048//4048 4052//4052
+f 4244//4244 4245//4245 4057//4057
+f 4367//4367 4246//4246 4249//4249
+f 2414//2414 4057//4057 4076//4076
+f 4249//4249 4246//4246 4250//4250
+f 4247//4247 2414//2414 4076//4076
+f 4248//4248 4396//4396 4397//4397
+f 4365//4365 4249//4249 4186//4186
+f 4249//4249 4250//4250 4186//4186
+f 4794//4794 2393//2393 2395//2395
+f 4794//4794 2395//2395 2412//2412
+f 2472//2472 4248//4248 4397//4397
+f 4251//4251 3153//3153 2304//2304
+f 2412//2412 4043//4043 4794//4794
+f 2405//2405 4794//4794 4043//4043
+f 4030//4030 4252//4252 4262//4262
+f 4252//4252 2421//2421 4262//4262
+f 4268//4268 4041//4041 4040//4040
+f 4040//4040 4273//4273 4253//4253
+f 2789//2789 4338//4338 4254//4254
+f 4285//4285 4268//4268 4040//4040
+f 4255//4255 4612//4612 4578//4578
+f 2421//2421 4263//4263 4262//4262
+f 3638//3638 4271//4271 4270//4270
+f 2390//2390 4681//4681 4265//4265
+f 4253//4253 4285//4285 4040//4040
+f 4271//4271 3638//3638 4257//4257
+f 4266//4266 4267//4267 4258//4258
+f 4258//4258 4267//4267 4256//4256
+f 4248//4248 2472//2472 4150//4150
+f 3648//3648 4270//4270 3881//3881
+f 3656//3656 4564//4564 4257//4257
+f 4622//4622 4258//4258 4564//4564
+f 4259//4259 2246//2246 4254//4254
+f 2509//2509 4269//4269 4265//4265
+f 2509//2509 4265//4265 4260//4260
+f 4567//4567 4565//4565 4261//4261
+f 4273//4273 2509//2509 4260//4260
+f 4260//4260 4267//4267 4273//4273
+f 4254//4254 2246//2246 3603//3603
+f 4263//4263 4264//4264 4269//4269
+f 4262//4262 4263//4263 4269//4269
+f 4264//4264 2390//2390 4265//4265
+f 4264//4264 4265//4265 4269//4269
+f 4261//4261 4573//4573 4567//4567
+f 4266//4266 2643//2643 4267//4267
+f 4268//4268 4285//4285 1636//1636
+f 4268//4268 1636//1636 4248//4248
+f 4258//4258 4271//4271 4564//4564
+f 4248//4248 4150//4150 4268//4268
+f 4257//4257 4564//4564 4271//4271
+f 4271//4271 4258//4258 4256//4256
+f 3858//3858 4269//4269 2509//2509
+f 4032//4032 4262//4262 4269//4269
+f 4269//4269 3858//3858 4032//4032
+f 3882//3882 4547//4547 4546//4546
+f 4270//4270 4271//4271 4256//4256
+f 3603//3603 4272//4272 2789//2789
+f 4273//4273 4274//4274 4253//4253
+f 4275//4275 4285//4285 4253//4253
+f 4274//4274 4421//4421 4275//4275
+f 4280//4280 2525//2525 4276//4276
+f 4181//4181 4278//4278 4277//4277
+f 4253//4253 4274//4274 4275//4275
+f 4278//4278 4294//4294 2525//2525
+f 4275//4275 4421//4421 4420//4420
+f 4281//4281 4178//4178 4278//4278
+f 4279//4279 4178//4178 4281//4281
+f 4280//4280 4282//4282 4281//4281
+f 4279//4279 4281//4281 4282//4282
+f 4282//4282 4280//4280 1212//1212
+f 2530//2530 2526//2526 4288//4288
+f 4283//4283 2530//2530 4288//4288
+f 4178//4178 4277//4277 4278//4278
+f 2526//2526 2525//2525 4294//4294
+f 4420//4420 1636//1636 4285//4285
+f 2526//2526 4294//4294 4288//4288
+f 4278//4278 2525//2525 4281//4281
+f 4280//4280 4281//4281 2525//2525
+f 4278//4278 4181//4181 4294//4294
+f 4172//4172 4171//4171 4299//4299
+f 4172//4172 4299//4299 4181//4181
+f 4289//4289 4288//4288 4284//4284
+f 4420//4420 4285//4285 4275//4275
+f 4289//4289 4283//4283 4288//4288
+f 4287//4287 4171//4171 4307//4307
+f 4190//4190 4286//4286 4259//4259
+f 4171//4171 4287//4287 4299//4299
+f 4294//4294 4284//4284 4288//4288
+f 3145//3145 4304//4304 31//31
+f 2429//2429 4284//4284 4295//4295
+f 2429//2429 4289//4289 4284//4284
+f 4295//4295 4290//4290 4291//4291
+f 2429//2429 4295//4295 4291//4291
+f 4292//4292 4298//4298 4295//4295
+f 4292//4292 4299//4299 4298//4298
+f 4290//4290 4295//4295 4298//4298
+f 4293//4293 2451//2451 4599//4599
+f 4284//4284 4294//4294 4292//4292
+f 4294//4294 4181//4181 4292//4292
+f 4181//4181 4299//4299 4292//4292
+f 4295//4295 4284//4284 4292//4292
+f 4230//4230 4296//4296 4301//4301
+f 4290//4290 4300//4300 4297//4297
+f 2506//2506 4291//4291 4290//4290
+f 2506//2506 4290//4290 4297//4297
+f 4290//4290 4298//4298 4300//4300
+f 2186//2186 4228//4228 2185//2185
+f 4298//4298 4287//4287 4300//4300
+f 4298//4298 4299//4299 4287//4287
+f 4287//4287 4307//4307 4300//4300
+f 4150//4150 4230//4230 4301//4301
+f 2185//2185 2784//2784 2783//2783
+f 4302//4302 4310//4310 4313//4313
+f 4313//4313 4310//4310 2495//2495
+f 4302//4302 4306//4306 4303//4303
+f 4303//4303 4306//4306 4349//4349
+f 3144//3144 4304//4304 3145//3145
+f 2500//2500 2495//2495 4310//4310
+f 4576//4576 4575//4575 4109//4109
+f 4310//4310 4302//4302 4303//4303
+f 4302//4302 4305//4305 4307//4307
+f 4307//4307 4171//4171 4306//4306
+f 4314//4314 4305//4305 4313//4313
+f 4305//4305 4302//4302 4313//4313
+f 4302//4302 4307//4307 4306//4306
+f 4314//4314 4297//4297 4305//4305
+f 4297//4297 4300//4300 4305//4305
+f 4305//4305 4300//4300 4307//4307
+f 4303//4303 4349//4349 4311//4311
+f 4308//4308 4309//4309 4663//4663
+f 2500//2500 4310//4310 4312//4312
+f 4310//4310 4303//4303 4312//4312
+f 4303//4303 4311//4311 4312//4312
+f 2465//2465 2491//2491 2604//2604
+f 2500//2500 4312//4312 2502//2502
+f 2502//2502 4312//4312 4319//4319
+f 4312//4312 4372//4372 4319//4319
+f 2495//2495 2500//2500 2477//2477
+f 2656//2656 2527//2527 4308//4308
+f 2507//2507 4313//4313 2495//2495
+f 4313//4313 2507//2507 4314//4314
+f 2506//2506 4297//4297 4314//4314
+f 2507//2507 2506//2506 4314//4314
+f 4325//4325 3859//3859 3934//3934
+f 4349//4349 4368//4368 4369//4369
+f 3244//3244 418//418 3245//3245
+f 4315//4315 4316//4316 4662//4662
+f 4317//4317 4319//4319 4318//4318
+f 2527//2527 4317//4317 4318//4318
+f 4317//4317 2502//2502 4319//4319
+f 420//420 3247//3247 3245//3245
+f 4319//4319 4372//4372 4320//4320
+f 4318//4318 4319//4319 4320//4320
+f 4315//4315 4369//4369 4316//4316
+f 4308//4308 4318//4318 4320//4320
+f 4321//4321 2540//2540 4389//4389
+f 2547//2547 2540//2540 4321//4321
+f 31//31 4322//4322 3637//3637
+f 4323//4323 4347//4347 1090//1090
+f 2557//2557 2556//2556 4324//4324
+f 4315//4315 4372//4372 4369//4369
+f 4388//4388 2544//2544 3270//3270
+f 4372//4372 4311//4311 4369//4369
+f 2540//2540 4388//4388 3251//3251
+f 1608//1608 4426//4426 3244//3244
+f 4032//4032 3859//3859 4325//4325
+f 4237//4237 4050//4050 2408//2408
+f 1599//1599 4566//4566 2624//2624
+f 4333//4333 4326//4326 1233//1233
+f 4619//4619 4327//4327 2392//2392
+f 4664//4664 2474//2474 4333//4333
+f 4256//4256 4547//4547 4270//4270
+f 4328//4328 4088//4088 2267//2267
+f 1957//1957 4422//4422 1072//1072
+f 2413//2413 4329//4329 4330//4330
+f 4331//4331 4057//4057 2414//2414
+f 2392//2392 4327//4327 3006//3006
+f 4332//4332 3808//3808 2387//2387
+f 2474//2474 4197//4197 4333//4333
+f 4197//4197 4326//4326 4333//4333
+f 4334//4334 2632//2632 4888//4888
+f 4786//4786 4247//4247 4335//4335
+f 4052//4052 2397//2397 2396//2396
+f 4422//4422 4035//4035 1072//1072
+f 2410//2410 4336//4336 4337//4337
+f 4763//4763 4339//4339 4695//4695
+f 4338//4338 2557//2557 4324//4324
+f 4763//4763 4345//4345 4339//4339
+f 4340//4340 4259//4259 4254//4254
+f 4035//4035 4328//4328 2267//2267
+f 3847//3847 2657//2657 3243//3243
+f 1598//1598 1596//1596 1595//1595
+f 4590//4590 2145//2145 4341//4341
+f 4839//4839 4342//4342 4690//4690
+f 3626//3626 3625//3625 4343//4343
+f 3625//3625 4344//4344 4343//4343
+f 4339//4339 4345//4345 3625//3625
+f 4345//4345 4344//4344 3625//3625
+f 4344//4344 4345//4345 4357//4357
+f 2655//2655 1598//1598 4346//4346
+f 3243//3243 4346//4346 419//419
+f 4347//4347 4323//4323 4348//4348
+f 4311//4311 4349//4349 4369//4369
+f 4311//4311 4372//4372 4312//4312
+f 3625//3625 4350//4350 4339//4339
+f 4350//4350 3632//3632 4762//4762
+f 4339//4339 4350//4350 4762//4762
+f 4351//4351 4204//4204 4207//4207
+f 4763//4763 4691//4691 2368//2368
+f 4352//4352 2352//2352 4343//4343
+f 4352//4352 4343//4343 4344//4344
+f 2352//2352 3607//3607 4343//4343
+f 3607//3607 3626//3626 4343//4343
+f 4368//4368 4323//4323 4316//4316
+f 4354//4354 4353//4353 4357//4357
+f 4345//4345 4355//4355 4357//4357
+f 4355//4355 4354//4354 4357//4357
+f 4763//4763 4355//4355 4345//4345
+f 4353//4353 4352//4352 4357//4357
+f 4356//4356 4353//4353 2369//2369
+f 4352//4352 4344//4344 4357//4357
+f 2557//2557 4338//4338 4351//4351
+f 4360//4360 4196//4196 4228//4228
+f 4808//4808 4620//4620 4358//4358
+f 4620//4620 4556//4556 4358//4358
+f 4336//4336 4386//4386 4033//4033
+f 4359//4359 4243//4243 3005//3005
+f 4578//4578 4586//4586 4360//4360
+f 4336//4336 4033//4033 4337//4337
+f 4359//4359 4249//4249 4243//4243
+f 2186//2186 3589//3589 4360//4360
+f 4605//4605 1218//1218 1213//1213
+f 4207//4207 4206//4206 2557//2557
+f 4329//4329 2413//2413 4361//4361
+f 3589//3589 4578//4578 4360//4360
+f 4185//4185 4362//4362 4235//4235
+f 4207//4207 2557//2557 4351//4351
+f 3577//3577 3589//3589 4685//4685
+f 1213//1213 1214//1214 2131//2131
+f 4362//4362 4242//4242 4364//4364
+f 3580//3580 3579//3579 4431//4431
+f 4802//4802 4809//4809 2272//2272
+f 4771//4771 2394//2394 4363//4363
+f 4364//4364 4242//4242 4365//4365
+f 2386//2386 3002//3002 4330//4330
+f 4365//4365 4555//4555 4243//4243
+f 4182//4182 4366//4366 4329//4329
+f 4359//4359 4367//4367 4249//4249
+f 4329//4329 4332//4332 2386//2386
+f 3554//3554 4407//4407 2111//2111
+f 2386//2386 4330//4330 4329//4329
+f 4368//4368 4316//4316 4369//4369
+f 1603//1603 4370//4370 4662//4662
+f 2197//2197 4891//4891 4379//4379
+f 2956//2956 2684//2684 4404//4404
+f 4308//4308 2527//2527 4318//4318
+f 2117//2117 2131//2131 4371//4371
+f 1626//1626 1228//1228 2022//2022
+f 4372//4372 4315//4315 4320//4320
+f 3254//3254 4810//4810 4373//4373
+f 3241//3241 4662//4662 4316//4316
+f 4423//4423 4374//4374 3271//3271
+f 2409//2409 2408//2408 4376//4376
+f 2396//2396 2409//2409 4376//4376
+f 4897//4897 3249//3249 4810//4810
+f 2524//2524 2523//2523 4403//4403
+f 4375//4375 4392//4392 4773//4773
+f 3254//3254 4373//4373 3005//3005
+f 3005//3005 4418//4418 3256//3256
+f 4376//4376 4052//4052 2396//2396
+f 4377//4377 4715//4715 4378//4378
+f 4379//4379 3256//3256 4620//4620
+f 4315//4315 4309//4309 4320//4320
+f 4320//4320 4309//4309 4308//4308
+f 1597//1597 4321//4321 3248//3248
+f 1065//1065 4380//4380 4334//4334
+f 4712//4712 3566//3566 4378//4378
+f 4392//4392 4712//4712 4378//4378
+f 4375//4375 2814//2814 3636//3636
+f 4327//4327 4538//4538 1069//1069
+f 3571//3571 4700//4700 3574//3574
+f 4367//4367 4401//4401 4732//4732
+f 4375//4375 3636//3636 4392//4392
+f 3249//3249 4871//4871 4401//4401
+f 4710//4710 4392//4392 4378//4378
+f 4381//4381 2974//2974 4382//4382
+f 4383//4383 3613//3613 4036//4036
+f 2615//2615 4384//4384 4381//4381
+f 4385//4385 4386//4386 4336//4336
+f 4869//4869 2106//2106 2107//2107
+f 1596//1596 4321//4321 1597//1597
+f 4387//4387 3270//3270 2544//2544
+f 4388//4388 3270//3270 3251//3251
+f 4389//4389 2540//2540 3251//3251
+f 3248//3248 4321//4321 4389//4389
+f 2180//2180 41//41 2275//2275
+f 4390//4390 1337//1337 4336//4336
+f 4757//4757 2112//2112 4391//4391
+f 3554//3554 3559//3559 3784//3784
+f 4007//4007 2197//2197 4807//4807
+f 2992//2992 4009//4009 4007//4007
+f 2624//2624 2442//2442 4397//4397
+f 2992//2992 4007//4007 4807//4807
+f 3636//3636 4185//4185 4392//4392
+f 4414//4414 4423//4423 3271//3271
+f 4854//4854 4393//4393 4879//4879
+f 4557//4557 4618//4618 3636//3636
+f 4413//4413 4414//4414 4403//4403
+f 4557//4557 3636//3636 2814//2814
+f 1069//1069 4384//4384 4394//4394
+f 3566//3566 4712//4712 3784//3784
+f 4400//4400 4733//4733 4395//4395
+f 4400//4400 4395//4395 4399//4399
+f 4396//4396 2624//2624 4397//4397
+f 4740//4740 4398//4398 4399//4399
+f 2523//2523 4412//4412 4413//4413
+f 4398//4398 4400//4400 4399//4399
+f 4732//4732 4401//4401 4402//4402
+f 4246//4246 4367//4367 4732//4732
+f 4761//4761 3265//3265 2133//2133
+f 2523//2523 4413//4413 4403//4403
+f 2684//2684 2814//2814 2671//2671
+f 4009//4009 2956//2956 4404//4404
+f 3618//3618 4890//4890 4883//4883
+f 4890//4890 4402//4402 4405//4405
+f 4238//4238 4406//4406 4407//4407
+f 4418//4418 3005//3005 4243//4243
+f 4555//4555 4418//4418 4243//4243
+f 3001//3001 4408//4408 2391//2391
+f 1337//1337 4390//4390 2391//2391
+f 4242//4242 4555//4555 4365//4365
+f 4409//4409 2674//2674 2720//2720
+f 4409//4409 1628//1628 2674//2674
+f 2674//2674 1628//1628 2675//2675
+f 1628//1628 4410//4410 2675//2675
+f 4414//4414 3271//3271 4403//4403
+f 3865//3865 4411//4411 4413//4413
+f 3865//3865 4413//4413 4412//4412
+f 4413//4413 4411//4411 4414//4414
+f 4424//4424 4423//4423 4414//4414
+f 4414//4414 4411//4411 4424//4424
+f 4546//4546 4540//4540 321//321
+f 4411//4411 4416//4416 4415//4415
+f 4416//4416 3842//3842 4415//4415
+f 3842//3842 4417//4417 4415//4415
+f 3256//3256 4418//4418 4620//4620
+f 3637//3637 4322//4322 4419//4419
+f 4420//4420 4421//4421 4616//4616
+f 1604//1604 3849//3849 4424//4424
+f 4415//4415 1604//1604 4424//4424
+f 4422//4422 1957//1957 2022//2022
+f 2289//2289 4787//4787 2290//2290
+f 4411//4411 4415//4415 4424//4424
+f 3849//3849 4423//4423 4424//4424
+f 4751//4751 4425//4425 4428//4428
+f 4426//4426 418//418 3244//3244
+f 3242//3242 418//418 409//409
+f 4411//4411 3865//3865 2656//2656
+f 4416//4416 4411//4411 2656//2656
+f 4791//4791 4427//4427 4738//4738
+f 4370//4370 3842//3842 4416//4416
+f 4651//4651 2375//2375 4425//4425
+f 4668//4668 4428//4428 4425//4425
+f 4433//4433 4429//4429 4430//4430
+f 4696//4696 4738//4738 2154//2154
+f 2264//2264 4430//4430 4431//4431
+f 2263//2263 4430//4430 2264//2264
+f 4416//4416 2656//2656 4663//4663
+f 4663//4663 4370//4370 4416//4416
+f 35//35 4811//4811 4432//4432
+f 4433//4433 4787//4787 4429//4429
+f 1958//1958 1626//1626 2022//2022
+f 4434//4434 4435//4435 4439//4439
+f 3451//3451 4436//4436 4435//4435
+f 4436//4436 3451//3451 3477//3477
+f 4436//4436 304//304 4435//4435
+f 4437//4437 4436//4436 3477//3477
+f 4439//4439 4438//4438 2866//2866
+f 4438//4438 4439//4439 304//304
+f 4434//4434 4439//4439 2866//2866
+f 2868//2868 4438//4438 304//304
+f 304//304 4439//4439 4435//4435
+f 3641//3641 2247//2247 4440//4440
+f 987//987 980//980 2871//2871
+f 987//987 2871//2871 2886//2886
+f 2886//2886 2885//2885 976//976
+f 4441//4441 70//70 4442//4442
+f 4444//4444 4441//4441 4442//4442
+f 4442//4442 70//70 23//23
+f 97//97 4449//4449 4443//4443
+f 4449//4449 4441//4441 4444//4444
+f 3141//3141 4449//4449 4444//4444
+f 3141//3141 4445//4445 4449//4449
+f 4446//4446 3141//3141 4444//4444
+f 4446//4446 4444//4444 4442//4442
+f 2192//2192 4442//4442 23//23
+f 2192//2192 4446//4446 4442//4442
+f 3146//3146 3204//3204 4448//4448
+f 3146//3146 4448//4448 4447//4447
+f 4447//4447 4448//4448 4443//4443
+f 4449//4449 4445//4445 4443//4443
+f 4445//4445 4447//4447 4443//4443
+f 97//97 4443//4443 3240//3240
+f 4443//4443 4448//4448 3240//3240
+f 3153//3153 2560//2560 2688//2688
+f 3164//3164 45//45 3232//3232
+f 55//55 3403//3403 48//48
+f 48//48 3403//3403 3394//3394
+f 3236//3236 56//56 3228//3228
+f 56//56 3236//3236 45//45
+f 45//45 3164//3164 3403//3403
+f 3936//3936 4705//4705 4286//4286
+f 518//518 736//736 4451//4451
+f 518//518 4450//4450 736//736
+f 516//516 518//518 4451//4451
+f 4476//4476 4480//4480 2046//2046
+f 736//736 4450//4450 543//543
+f 2025//2025 4480//4480 4476//4476
+f 2025//2025 4479//4479 4480//4480
+f 4453//4453 4479//4479 2025//2025
+f 4450//4450 4452//4452 543//543
+f 4453//4453 4483//4483 4479//4479
+f 1651//1651 1697//1697 4452//4452
+f 4453//4453 4455//4455 4483//4483
+f 4454//4454 4484//4484 4455//4455
+f 4454//4454 4457//4457 4484//4484
+f 4456//4456 1635//1635 1648//1648
+f 2034//2034 4457//4457 4454//4454
+f 1639//1639 547//547 1648//1648
+f 2034//2034 4458//4458 4457//4457
+f 1689//1689 1644//1644 4459//4459
+f 4459//4459 1643//1643 1646//1646
+f 4460//4460 549//549 1817//1817
+f 540//540 1825//1825 1820//1820
+f 722//722 4532//4532 4462//4462
+f 2036//2036 4461//4461 4462//4462
+f 1806//1806 551//551 4463//4463
+f 2036//2036 4462//4462 2037//2037
+f 4465//4465 551//551 4464//4464
+f 4525//4525 2038//2038 4466//4466
+f 552//552 4465//4465 1802//1802
+f 4525//4525 4466//4466 4493//4493
+f 4493//4493 4524//4524 4525//4525
+f 1802//1802 1808//1808 552//552
+f 706//706 4466//4466 2039//2039
+f 2040//2040 4467//4467 2039//2039
+f 1819//1819 1808//1808 4468//4468
+f 2042//2042 4467//4467 2040//2040
+f 553//553 1805//1805 556//556
+f 2042//2042 698//698 4467//4467
+f 2041//2041 698//698 2042//2042
+f 2041//2041 697//697 698//698
+f 2041//2041 645//645 697//697
+f 4470//4470 645//645 2041//2041
+f 557//557 1767//1767 1768//1768
+f 4520//4520 4469//4469 4470//4470
+f 1750//1750 1831//1831 558//558
+f 608//608 4520//4520 4470//4470
+f 1768//1768 1750//1750 558//558
+f 561//561 1831//1831 4471//4471
+f 2044//2044 4473//4473 4472//4472
+f 4474//4474 4473//4473 2044//2044
+f 725//725 607//607 4474//4474
+f 4475//4475 725//725 4474//4474
+f 4471//4471 563//563 561//561
+f 4475//4475 4502//4502 725//725
+f 4475//4475 4501//4501 4502//4502
+f 4501//4501 4475//4475 4476//4476
+f 565//565 562//562 563//563
+f 4480//4480 602//602 2046//2046
+f 4477//4477 1637//1637 4478//4478
+f 4479//4479 602//602 4480//4480
+f 1709//1709 4477//4477 1717//1717
+f 4481//4481 4482//4482 4479//4479
+f 1709//1709 1706//1706 541//541
+f 1671//1671 541//541 1710//1710
+f 4483//4483 4481//4481 4479//4479
+f 1712//1712 1671//1671 1710//1710
+f 4455//4455 4485//4485 4483//4483
+f 568//568 1712//1712 1719//1719
+f 4484//4484 4485//4485 4455//4455
+f 4484//4484 4508//4508 4485//4485
+f 4486//4486 4487//4487 568//568
+f 4457//4457 4508//4508 4484//4484
+f 593//593 592//592 4458//4458
+f 903//903 1688//1688 4488//4488
+f 893//893 4490//4490 1688//1688
+f 571//571 4489//4489 887//887
+f 4489//4489 4490//4490 893//893
+f 572//572 4491//4491 573//573
+f 573//573 4491//4491 576//576
+f 571//571 887//887 572//572
+f 576//576 4491//4491 575//575
+f 542//542 4492//4492 575//575
+f 4492//4492 1751//1751 575//575
+f 4493//4493 4523//4523 4524//4524
+f 4493//4493 706//706 4523//4523
+f 706//706 4493//4493 4466//4466
+f 4496//4496 865//865 4492//4492
+f 2039//2039 4494//4494 706//706
+f 4467//4467 4494//4494 2039//2039
+f 4495//4495 865//865 578//578
+f 4467//4467 701//701 4494//4494
+f 856//856 578//578 4496//4496
+f 698//698 701//701 4467//4467
+f 698//698 694//694 701//701
+f 695//695 694//694 697//697
+f 4497//4497 4498//4498 1920//1920
+f 851//851 4499//4499 4500//4500
+f 4500//4500 4499//4499 4503//4503
+f 581//581 4505//4505 580//580
+f 4501//4501 4504//4504 4502//4502
+f 4500//4500 4503//4503 581//581
+f 4501//4501 605//605 4504//4504
+f 2046//2046 605//605 4501//4501
+f 583//583 582//582 849//849
+f 2046//2046 604//604 605//605
+f 4505//4505 582//582 580//580
+f 602//602 604//604 2046//2046
+f 4482//4482 703//703 602//602
+f 4482//4482 602//602 4479//4479
+f 599//599 4514//4514 4481//4481
+f 4481//4481 596//596 599//599
+f 4483//4483 596//596 4481//4481
+f 4483//4483 4506//4506 596//596
+f 4485//4485 4506//4506 4483//4483
+f 4485//4485 595//595 4506//4506
+f 848//848 4507//4507 583//583
+f 4508//4508 595//595 4485//4485
+f 4507//4507 4509//4509 4458//4458
+f 4458//4458 4510//4510 593//593
+f 593//593 741//741 4511//4511
+f 754//754 2018//2018 592//592
+f 4511//4511 754//754 592//592
+f 753//753 594//594 2018//2018
+f 597//597 594//594 4512//4512
+f 702//702 4513//4513 597//597
+f 738//738 4513//4513 705//705
+f 720//720 4514//4514 738//738
+f 716//716 4514//4514 720//720
+f 703//703 4482//4482 716//716
+f 604//604 703//703 715//715
+f 4517//4517 603//603 711//711
+f 4515//4515 4516//4516 4517//4517
+f 725//725 4516//4516 726//726
+f 725//725 723//723 607//607
+f 607//607 635//635 606//606
+f 606//606 724//724 4518//4518
+f 4518//4518 4519//4519 4520//4520
+f 4519//4519 637//637 4469//4469
+f 4469//4469 637//637 619//619
+f 619//619 639//639 4521//4521
+f 639//639 695//695 4521//4521
+f 699//699 614//614 700//700
+f 588//588 4494//4494 700//700
+f 4522//4522 4523//4523 588//588
+f 4523//4523 4522//4522 4524//4524
+f 4524//4524 4526//4526 4525//4525
+f 712//712 4525//4525 4526//4526
+f 4527//4527 714//714 4528//4528
+f 714//714 4527//4527 713//713
+f 4461//4461 4527//4527 587//587
+f 712//712 4526//4526 714//714
+f 4529//4529 4530//4530 4531//4531
+f 4531//4531 4532//4532 4529//4529
+f 587//587 4530//4530 4529//4529
+f 4531//4531 729//729 4532//4532
+f 528//528 729//729 598//598
+f 731//731 4533//4533 4534//4534
+f 529//529 732//732 731//731
+f 732//732 537//537 4535//4535
+f 4536//4536 4535//4535 537//537
+f 4535//4535 4536//4536 530//530
+f 4536//4536 4537//4537 530//530
+f 4381//4381 4384//4384 4538//4538
+f 4267//4267 2643//2643 4274//4274
+f 2974//2974 4381//4381 4538//4538
+f 4596//4596 3586//3586 2417//2417
+f 4556//4556 4620//4620 4418//4418
+f 4539//4539 4540//4540 4546//4546
+f 4579//4579 4671//4671 4634//4634
+f 4541//4541 4563//4563 4554//4554
+f 4579//4579 4658//4658 3955//3955
+f 4627//4627 4607//4607 2357//2357
+f 4626//4626 2915//2915 4627//4627
+f 4554//4554 4542//4542 4541//4541
+f 3586//3586 3788//3788 2417//2417
+f 4542//4542 4622//4622 4541//4541
+f 4580//4580 4573//4573 4610//4610
+f 4579//4579 4630//4630 4658//4658
+f 4553//4553 4542//4542 4554//4554
+f 4570//4570 4543//4543 4604//4604
+f 4655//4655 4544//4544 2360//2360
+f 4293//4293 4010//4010 2420//2420
+f 2977//2977 4545//4545 4419//4419
+f 4646//4646 4650//4650 4621//4621
+f 4611//4611 4553//4553 4560//4560
+f 4546//4546 4547//4547 4726//4726
+f 4556//4556 4418//4418 4555//4555
+f 3008//3008 4548//4548 4549//4549
+f 2326//2326 4589//4589 2943//2943
+f 3104//3104 4655//4655 2357//2357
+f 3103//3103 2357//2357 4607//4607
+f 4550//4550 4566//4566 4561//4561
+f 4561//4561 4551//4551 4550//4550
+f 4554//4554 4649//4649 4551//4551
+f 4645//4645 4550//4550 4551//4551
+f 4792//4792 4740//4740 2790//2790
+f 4551//4551 4649//4649 4645//4645
+f 4552//4552 4649//4649 4554//4554
+f 4772//4772 4755//4755 2560//2560
+f 4553//4553 4554//4554 4551//4551
+f 4539//4539 3003//3003 4540//4540
+f 4554//4554 4563//4563 4552//4552
+f 4572//4572 4571//4571 2364//2364
+f 4266//4266 4258//4258 4622//4622
+f 4557//4557 4556//4556 4618//4618
+f 4544//4544 3111//3111 2360//2360
+f 4561//4561 4553//4553 4551//4551
+f 4276//4276 2526//2526 3039//3039
+f 4555//4555 4242//4242 4618//4618
+f 4556//4556 4557//4557 4358//4358
+f 2311//2311 1902//1902 4562//4562
+f 4558//4558 4559//4559 2311//2311
+f 4560//4560 4553//4553 4561//4561
+f 1337//1337 1346//1346 4385//4385
+f 4639//4639 3997//3997 4640//4640
+f 4644//4644 4643//4643 4624//4624
+f 4623//4623 3982//3982 4624//4624
+f 4623//4623 3997//3997 3962//3962
+f 2311//2311 4562//4562 4558//4558
+f 2364//2364 2328//2328 2370//2370
+f 3956//3956 4552//4552 3957//3957
+f 3958//3958 4563//4563 4628//4628
+f 4552//4552 4563//4563 3957//3957
+f 4564//4564 3959//3959 4622//4622
+f 4565//4565 4653//4653 4602//4602
+f 4572//4572 2364//2364 4610//4610
+f 4628//4628 3959//3959 3960//3960
+f 4639//4639 4638//4638 3956//3956
+f 4550//4550 3954//3954 4566//4566
+f 3954//3954 4550//4550 4644//4644
+f 4567//4567 4573//4573 4580//4580
+f 4567//4567 4580//4580 4568//4568
+f 4569//4569 4570//4570 4574//4574
+f 4574//4574 4589//4589 4239//4239
+f 4261//4261 4571//4571 4572//4572
+f 4572//4572 4610//4610 4573//4573
+f 4573//4573 4261//4261 4572//4572
+f 4574//4574 4239//4239 4569//4569
+f 2582//2582 4575//4575 4576//4576
+f 4577//4577 4586//4586 2943//2943
+f 4586//4586 4578//4578 2943//2943
+f 4583//4583 4579//4579 3955//3955
+f 4580//4580 4610//4610 4621//4621
+f 4543//4543 4581//4581 4604//4604
+f 4582//4582 2952//2952 3955//3955
+f 3955//3955 4658//4658 4582//4582
+f 4658//4658 4630//4630 3506//3506
+f 4785//4785 4784//4784 2155//2155
+f 2371//2371 2372//2372 4667//4667
+f 4583//4583 4677//4677 4584//4584
+f 4584//4584 4671//4671 4583//4583
+f 4575//4575 2996//2996 4109//4109
+f 4109//4109 2996//2996 2995//2995
+f 4634//4634 4633//4633 4630//4630
+f 4577//4577 2943//2943 4574//4574
+f 2970//2970 4419//4419 4322//4322
+f 4189//4189 2990//2990 4604//4604
+f 4630//4630 4579//4579 4634//4634
+f 2952//2952 2953//2953 4617//4617
+f 4660//4660 1035//1035 3765//3765
+f 2812//2812 1028//1028 4585//4585
+f 4604//4604 2990//2990 4196//4196
+f 4196//4196 4586//4586 4577//4577
+f 4196//4196 4577//4577 4604//4604
+f 2318//2318 4613//4613 4642//4642
+f 2322//2322 2318//2318 4587//4587
+f 4587//4587 4642//4642 3585//3585
+f 4588//4588 3605//3605 3604//3604
+f 2321//2321 4589//4589 2326//2326
+f 4599//4599 2451//2451 2587//2587
+f 4588//4588 3585//3585 3584//3584
+f 2151//2151 4590//4590 4341//4341
+f 4726//4726 4539//4539 4546//4546
+f 4570//4570 4569//4569 4543//4543
+f 4569//4569 4234//4234 4543//4543
+f 2625//2625 4549//4549 2991//2991
+f 4570//4570 4604//4604 4577//4577
+f 4574//4574 4570//4570 4577//4577
+f 4589//4589 3511//3511 4239//4239
+f 4239//4239 3502//3502 4591//4591
+f 3589//3589 3588//3588 4255//4255
+f 4255//4255 4578//4578 3589//3589
+f 4592//4592 4593//4593 3592//3592
+f 4594//4594 3641//3641 4440//4440
+f 4592//4592 3592//3592 3599//3599
+f 2303//2303 4595//4595 4593//4593
+f 3588//3588 3577//3577 2303//2303
+f 3613//3613 4383//4383 4385//4385
+f 4596//4596 3041//3041 3145//3145
+f 4641//4641 4597//4597 3600//3600
+f 4641//4641 3599//3599 4597//4597
+f 4750//4750 4598//4598 4711//4711
+f 4599//4599 2587//2587 4576//4576
+f 4568//4568 4603//4603 4567//4567
+f 1095//1095 990//990 4841//4841
+f 4189//4189 2310//2310 2309//2309
+f 3584//3584 3617//3617 3605//3605
+f 4600//4600 4601//4601 3600//3600
+f 3631//3631 3637//3637 4419//4419
+f 4571//4571 4565//4565 4602//4602
+f 2950//2950 2369//2369 4693//4693
+f 2950//2950 2370//2370 2369//2369
+f 3997//3997 4623//4623 4640//4640
+f 2929//2929 4603//4603 4568//4568
+f 4626//4626 4603//4603 2928//2928
+f 4586//4586 4196//4196 4360//4360
+f 4604//4604 4581//4581 4189//4189
+f 4565//4565 4571//4571 4261//4261
+f 4600//4600 3617//3617 3584//3584
+f 4841//4841 4605//4605 2021//2021
+f 2927//2927 4568//4568 4606//4606
+f 4607//4607 4627//4627 2915//2915
+f 4608//4608 4841//4841 2330//2330
+f 2939//2939 4606//4606 2942//2942
+f 2330//2330 4841//4841 2021//2021
+f 4336//4336 1337//1337 4385//4385
+f 4322//4322 1075//1075 4609//4609
+f 4646//4646 4621//4621 4610//4610
+f 4553//4553 4611//4611 4542//4542
+f 4588//4588 2319//2319 4587//4587
+f 4610//4610 2364//2364 4646//4646
+f 2643//2643 4266//4266 4611//4611
+f 2556//2556 4206//4206 2642//2642
+f 4612//4612 4613//4613 2326//2326
+f 4619//4619 4614//4614 2974//2974
+f 1075//1075 2995//2995 4615//4615
+f 4616//4616 2643//2643 4611//4611
+f 2021//2021 4605//4605 2117//2117
+f 4617//4617 2953//2953 4598//4598
+f 4618//4618 4556//4556 4555//4555
+f 2974//2974 4538//4538 4619//4619
+f 4379//4379 4620//4620 4808//4808
+f 4652//4652 4651//4651 4545//4545
+f 4621//4621 4650//4650 2942//2942
+f 2942//2942 4606//4606 4621//4621
+f 4603//4603 4656//4656 4654//4654
+f 4612//4612 2326//2326 2943//2943
+f 2943//2943 4578//4578 4612//4612
+f 4606//4606 4568//4568 4580//4580
+f 4606//4606 4580//4580 4621//4621
+f 4542//4542 4266//4266 4622//4622
+f 4266//4266 4542//4542 4611//4611
+f 4541//4541 4622//4622 3959//3959
+f 3959//3959 4628//4628 4541//4541
+f 4640//4640 4623//4623 4624//4624
+f 4624//4624 4643//4643 4640//4640
+f 2287//2287 4088//4088 1359//1359
+f 4358//4358 4557//4557 4625//4625
+f 4656//4656 4603//4603 4626//4626
+f 4626//4626 4627//4627 4656//4656
+f 4628//4628 4563//4563 4541//4541
+f 2357//2357 4655//4655 4657//4657
+f 4629//4629 4633//4633 2957//2957
+f 4630//4630 4633//4633 4632//4632
+f 4631//4631 3512//3512 4632//4632
+f 4633//4633 4634//4634 2964//2964
+f 4635//4635 3587//3587 4636//4636
+f 4565//4565 4567//4567 4654//4654
+f 3585//3585 4642//4642 4637//4637
+f 4552//4552 3956//3956 4638//4638
+f 4657//4657 2360//2360 4654//4654
+f 4639//4639 4640//4640 4638//4638
+f 4637//4637 4641//4641 4601//4601
+f 4649//4649 4552//4552 4638//4638
+f 4601//4601 3585//3585 4637//4637
+f 4637//4637 4642//4642 4684//4684
+f 4638//4638 4640//4640 4643//4643
+f 4638//4638 4643//4643 4649//4649
+f 4645//4645 4643//4643 4644//4644
+f 4644//4644 4550//4550 4645//4645
+f 4646//4646 2364//2364 2370//2370
+f 2370//2370 2950//2950 4646//4646
+f 4684//4684 3599//3599 4641//4641
+f 4647//4647 4648//4648 3631//3631
+f 4643//4643 4645//4645 4649//4649
+f 2950//2950 4650//4650 4646//4646
+f 4684//4684 4641//4641 4637//4637
+f 2375//2375 4651//4651 4652//4652
+f 4653//4653 4565//4565 4654//4654
+f 4654//4654 2360//2360 4653//4653
+f 2303//2303 4592//4592 3588//3588
+f 4255//4255 3588//3588 4675//4675
+f 4655//4655 2360//2360 4657//4657
+f 4567//4567 4603//4603 4654//4654
+f 4627//4627 4657//4657 4656//4656
+f 4627//4627 2357//2357 4657//4657
+f 4658//4658 4670//4670 4673//4673
+f 4296//4296 4659//4659 4666//4666
+f 4582//4582 1029//1029 3510//3510
+f 4660//4660 4764//4764 1906//1906
+f 4661//4661 4669//4669 4668//4668
+f 2656//2656 4308//4308 4663//4663
+f 4309//4309 4662//4662 4663//4663
+f 4662//4662 4370//4370 4663//4663
+f 4667//4667 2372//2372 2382//2382
+f 4664//4664 4665//4665 4666//4666
+f 4677//4677 4667//4667 2373//2373
+f 2636//2636 1062//1062 3512//3512
+f 3672//3672 2444//2444 3817//3817
+f 2952//2952 3510//3510 2641//2641
+f 4540//4540 4858//4858 4859//4859
+f 1597//1597 419//419 1595//1595
+f 4673//4673 4582//4582 4658//4658
+f 4668//4668 4669//4669 3881//3881
+f 4374//4374 3246//3246 3252//3252
+f 4670//4670 4658//4658 3506//3506
+f 2390//2390 3003//3003 4681//4681
+f 2964//2964 2957//2957 4633//4633
+f 4671//4671 4672//4672 2986//2986
+f 4584//4584 2960//2960 4672//4672
+f 4671//4671 2986//2986 4634//4634
+f 4673//4673 1059//1059 1029//1029
+f 4673//4673 1058//1058 1059//1059
+f 1058//1058 4673//4673 1063//1063
+f 4674//4674 4613//4613 4612//4612
+f 3248//3248 419//419 1597//1597
+f 4612//4612 4255//4255 4674//4674
+f 4267//4267 4260//4260 4256//4256
+f 4782//4782 4598//4598 4748//4748
+f 4782//4782 4617//4617 4598//4598
+f 1428//1428 1442//1442 4679//4679
+f 4674//4674 4255//4255 4675//4675
+f 1426//1426 1442//1442 1428//1428
+f 2371//2371 4667//4667 4676//4676
+f 4667//4667 4677//4677 4676//4676
+f 4678//4678 4679//4679 1420//1420
+f 1442//1442 1420//1420 4679//4679
+f 4678//4678 1420//1420 1422//1422
+f 4674//4674 4675//4675 4684//4684
+f 1426//1426 1428//1428 4680//4680
+f 1429//1429 1420//1420 1442//1442
+f 4675//4675 4592//4592 3599//3599
+f 4675//4675 3599//3599 4684//4684
+f 4265//4265 4681//4681 4260//4260
+f 1426//1426 4682//4682 1458//1458
+f 1856//1856 1780//1780 1798//1798
+f 1780//1780 4683//4683 1798//1798
+f 1790//1790 1370//1370 4683//4683
+f 4674//4674 4684//4684 4642//4642
+f 4642//4642 4613//4613 4674//4674
+f 4676//4676 3955//3955 4617//4617
+f 4583//4583 3955//3955 4676//4676
+f 4782//4782 2371//2371 4676//4676
+f 4583//4583 4676//4676 4677//4677
+f 2368//2368 4355//4355 4763//4763
+f 4774//4774 4685//4685 4686//4686
+f 4702//4702 4687//4687 3860//3860
+f 4687//4687 4717//4717 3860//3860
+f 4702//4702 3855//3855 4778//4778
+f 4688//4688 4760//4760 4778//4778
+f 4781//4781 3837//3837 4765//4765
+f 3839//3839 3786//3786 4719//4719
+f 4689//4689 2892//2892 3643//3643
+f 4776//4776 3839//3839 4719//4719
+f 4709//4709 3834//3834 4776//4776
+f 3786//3786 3787//3787 3780//3780
+f 3779//3779 4768//4768 3780//3780
+f 4848//4848 4839//4839 4690//4690
+f 4695//4695 4585//4585 4691//4691
+f 4859//4859 4749//4749 4540//4540
+f 4691//4691 4585//4585 1021//1021
+f 3786//3786 4770//4770 4719//4719
+f 4690//4690 4869//4869 2274//2274
+f 4692//4692 4720//4720 4770//4770
+f 4770//4770 4713//4713 4692//4692
+f 4625//4625 4808//4808 4358//4358
+f 4819//4819 4342//4342 4839//4839
+f 4693//4693 4703//4703 4694//4694
+f 4703//4703 4704//4704 4694//4694
+f 4695//4695 4737//4737 4585//4585
+f 2186//2186 2783//2783 4685//4685
+f 3765//3765 2812//2812 4737//4737
+f 35//35 2358//2358 4831//4831
+f 4898//4898 3253//3253 4854//4854
+f 4730//4730 4694//4694 4704//4704
+f 2672//2672 4375//4375 4772//4772
+f 4185//4185 4235//4235 4712//4712
+f 4789//4789 4696//4696 2168//2168
+f 4900//4900 4898//4898 4854//4854
+f 4836//4836 4899//4899 4850//4850
+f 4704//4704 2368//2368 4697//4697
+f 4692//4692 4718//4718 4720//4720
+f 4698//4698 4730//4730 4704//4704
+f 4777//4777 4725//4725 4718//4718
+f 4777//4777 4699//4699 4725//4725
+f 4697//4697 4698//4698 4704//4704
+f 4699//4699 4727//4727 4725//4725
+f 4777//4777 4687//4687 4699//4699
+f 4361//4361 4182//4182 4329//4329
+f 4700//4700 3571//3571 4759//4759
+f 2672//2672 2671//2671 4375//4375
+f 4788//4788 3840//3840 4737//4737
+f 4780//4780 4765//4765 4727//4727
+f 4779//4779 4722//4722 4780//4780
+f 4695//4695 4701//4701 4737//4737
+f 4727//4727 4779//4779 4780//4780
+f 4699//4699 4702//4702 4779//4779
+f 4737//4737 4701//4701 4788//4788
+f 4864//4864 3253//3253 4849//4849
+f 4703//4703 2368//2368 4704//4704
+f 4705//4705 2246//2246 4259//4259
+f 4697//4697 2815//2815 4698//4698
+f 4699//4699 4779//4779 4727//4727
+f 2950//2950 4693//4693 4706//4706
+f 3631//3631 4545//4545 4749//4749
+f 4707//4707 2944//2944 4706//4706
+f 4708//4708 4765//4765 4709//4709
+f 4728//4728 2951//2951 4730//4730
+f 2816//2816 4707//4707 4730//4730
+f 4755//4755 4773//4773 4756//4756
+f 4392//4392 4710//4710 4773//4773
+f 4375//4375 4773//4773 4772//4772
+f 2955//2955 4729//4729 4711//4711
+f 4744//4744 4768//4768 3779//3779
+f 4712//4712 4392//4392 4185//4185
+f 4769//4769 4713//4713 3769//3769
+f 4714//4714 4767//4767 4744//4744
+f 3769//3769 4745//4745 4769//4769
+f 3774//3774 4767//4767 4714//4714
+f 3768//3768 3770//3770 4745//4745
+f 4378//4378 4715//4715 4710//4710
+f 4742//4742 4716//4716 3770//3770
+f 4743//4743 3751//3751 3755//3755
+f 3754//3754 4717//4717 4718//4718
+f 4725//4725 4727//4727 4708//4708
+f 4708//4708 4719//4719 4720//4720
+f 3713//3713 4722//4722 4721//4721
+f 3713//3713 4724//4724 4722//4722
+f 3712//3712 4723//4723 4724//4724
+f 4718//4718 4725//4725 4720//4720
+f 4708//4708 4720//4720 4725//4725
+f 4260//4260 4681//4681 4726//4726
+f 4723//4723 3705//3705 4781//4781
+f 3841//3841 4750//4750 4711//4711
+f 4727//4727 4765//4765 4708//4708
+f 2956//2956 4009//4009 2992//2992
+f 4698//4698 4728//4728 4730//4730
+f 3841//3841 4729//4729 3764//3764
+f 4395//4395 4204//4204 4399//4399
+f 4549//4549 4614//4614 4771//4771
+f 4429//4429 2289//2289 3580//3580
+f 4244//4244 4057//4057 4331//4331
+f 4730//4730 4707//4707 4694//4694
+f 3841//3841 4711//4711 4729//4729
+f 2278//2278 4753//4753 4731//4731
+f 4732//4732 4890//4890 3618//3618
+f 4733//4733 2114//2114 4395//4395
+f 2278//2278 4802//4802 2277//2277
+f 4801//4801 4753//4753 4734//4734
+f 4753//4753 2278//2278 4734//4734
+f 2155//2155 2294//2294 4785//4785
+f 4736//4736 4758//4758 4391//4391
+f 4800//4800 4806//4806 4735//4735
+f 4736//4736 4391//4391 2128//2128
+f 4696//4696 4806//4806 4746//4746
+f 4746//4746 4738//4738 4696//4696
+f 4806//4806 4696//4696 4789//4789
+f 4789//4789 4735//4735 4806//4806
+f 4737//4737 3840//3840 3765//3765
+f 2140//2140 2145//2145 4590//4590
+f 2154//2154 3568//3568 2168//2168
+f 2153//2153 4738//4738 4427//4427
+f 4800//4800 4739//4739 4806//4806
+f 4771//4771 4363//4363 3008//3008
+f 4695//4695 4762//4762 4701//4701
+f 4399//4399 4204//4204 4740//4740
+f 3621//3621 4741//4741 4747//4747
+f 2112//2112 2128//2128 4391//4391
+f 4745//4745 3770//3770 4716//4716
+f 4716//4716 4742//4742 4743//4743
+f 4235//4235 3784//3784 4712//4712
+f 4768//4768 4744//4744 4767//4767
+f 4769//4769 4745//4745 4716//4716
+f 4716//4716 4743//4743 3755//3755
+f 4706//4706 4693//4693 4694//4694
+f 4746//4746 4739//4739 4798//4798
+f 4747//4747 4741//4741 4748//4748
+f 4747//4747 4748//4748 4788//4788
+f 4859//4859 3631//3631 4749//4749
+f 4750//4750 4788//4788 4748//4748
+f 1095//1095 1212//1212 4276//4276
+f 321//321 4749//4749 4751//4751
+f 2265//2265 3580//3580 4795//4795
+f 4753//4753 4795//4795 4752//4752
+f 4753//4753 2265//2265 4795//4795
+f 4749//4749 4545//4545 4651//4651
+f 4585//4585 4737//4737 2812//2812
+f 4749//4749 4651//4651 4751//4751
+f 4756//4756 4773//4773 4710//4710
+f 4710//4710 4715//4715 4756//4756
+f 3840//3840 4750//4750 3841//3841
+f 4756//4756 4715//4715 4754//4754
+f 4756//4756 4754//4754 4700//4700
+f 4755//4755 4756//4756 4700//4700
+f 3621//3621 4747//4747 3816//3816
+f 4757//4757 4391//4391 4758//4758
+f 4759//4759 4775//4775 4755//4755
+f 4700//4700 4759//4759 4755//4755
+f 4760//4760 4721//4721 4722//4722
+f 4701//4701 3635//3635 3634//3634
+f 3786//3786 3780//3780 4768//4768
+f 3816//3816 4747//4747 3634//3634
+f 4792//4792 4761//4761 2133//2133
+f 3624//3624 4741//4741 3621//3621
+f 4695//4695 4339//4339 4762//4762
+f 4699//4699 4687//4687 4702//4702
+f 2375//2375 4661//4661 4425//4425
+f 4763//4763 4695//4695 4691//4691
+f 4724//4724 4723//4723 4781//4781
+f 3635//3635 4762//4762 3632//3632
+f 4748//4748 4741//4741 4782//4782
+f 1907//1907 3764//3764 4729//4729
+f 2197//2197 2160//2160 4891//4891
+f 4660//4660 3764//3764 4764//4764
+f 3837//3837 4709//4709 4765//4765
+f 2160//2160 4758//4758 4736//4736
+f 3840//3840 4788//4788 4750//4750
+f 3669//3669 4766//4766 405//405
+f 4768//4768 4767//4767 4770//4770
+f 3786//3786 4768//4768 4770//4770
+f 3755//3755 4692//4692 4769//4769
+f 4716//4716 3755//3755 4769//4769
+f 4755//4755 4775//4775 2560//2560
+f 4719//4719 4770//4770 4720//4720
+f 3008//3008 4549//4549 4771//4771
+f 4772//4772 4773//4773 4755//4755
+f 3755//3755 4718//4718 4692//4692
+f 4538//4538 4327//4327 4619//4619
+f 4767//4767 4713//4713 4770//4770
+f 4775//4775 4774//4774 2560//2560
+f 4754//4754 3574//3574 4700//4700
+f 4759//4759 3571//3571 3576//3576
+f 4775//4775 4759//4759 3581//3581
+f 4692//4692 4713//4713 4769//4769
+f 4775//4775 3581//3581 4774//4774
+f 4776//4776 4719//4719 4708//4708
+f 4709//4709 4776//4776 4708//4708
+f 4777//4777 4718//4718 4717//4717
+f 4717//4717 4687//4687 4777//4777
+f 1095//1095 4276//4276 3039//3039
+f 4383//4383 4386//4386 4385//4385
+f 4760//4760 4722//4722 4779//4779
+f 4779//4779 4702//4702 4778//4778
+f 3624//3624 2371//2371 4741//4741
+f 4778//4778 4760//4760 4779//4779
+f 4724//4724 4781//4781 4780//4780
+f 4722//4722 4724//4724 4780//4780
+f 4799//4799 4341//4341 4805//4805
+f 4781//4781 4765//4765 4780//4780
+f 4741//4741 2371//2371 4782//4782
+f 4784//4784 4783//4783 4809//4809
+f 4783//4783 4784//4784 4785//4785
+f 4792//4792 2133//2133 4740//4740
+f 4786//4786 4182//4182 4247//4247
+f 4431//4431 4429//4429 3580//3580
+f 4787//4787 2289//2289 4429//4429
+f 4833//4833 4647//4647 4859//4859
+f 4431//4431 4430//4430 4429//4429
+f 2289//2289 4866//4866 4795//4795
+f 4701//4701 3634//3634 4788//4788
+f 3634//3634 4747//4747 4788//4788
+f 2289//2289 4795//4795 3580//3580
+f 4707//4707 4706//4706 4694//4694
+f 4799//4799 2151//2151 4341//4341
+f 2155//2155 4784//4784 4789//4789
+f 2368//2368 4691//4691 4697//4697
+f 2620//2620 4184//4184 2113//2113
+f 4247//4247 4182//4182 4361//4361
+f 4361//4361 2414//2414 4247//4247
+f 2168//2168 2155//2155 4789//4789
+f 2133//2133 3265//3265 3266//3266
+f 2113//2113 3761//3761 2620//2620
+f 2145//2145 2142//2142 4790//4790
+f 4791//4791 4738//4738 4746//4746
+f 4761//4761 4792//4792 3606//3606
+f 4793//4793 2393//2393 4794//4794
+f 2405//2405 4793//4793 4794//4794
+f 4804//4804 4590//4590 2151//2151
+f 4733//4733 2128//2128 2114//2114
+f 2136//2136 4790//4790 2143//2143
+f 4795//4795 4866//4866 4752//4752
+f 4796//4796 4798//4798 4866//4866
+f 2135//2135 2966//2966 2136//2136
+f 2140//2140 4590//4590 2147//2147
+f 2142//2142 2140//2140 4797//4797
+f 1218//1218 990//990 1233//1233
+f 4665//4665 1233//1233 990//990
+f 4739//4739 4731//4731 4752//4752
+f 4753//4753 4752//4752 4731//4731
+f 2301//2301 2139//2139 3844//3844
+f 4752//4752 4798//4798 4739//4739
+f 4625//4625 4807//4807 4808//4808
+f 2410//2410 4337//4337 4237//4237
+f 4237//4237 2408//2408 2410//2410
+f 2151//2151 4799//4799 2152//2152
+f 4731//4731 4739//4739 4800//4800
+f 4753//4753 4801//4801 2265//2265
+f 4800//4800 4802//4802 4731//4731
+f 4804//4804 2151//2151 2150//2150
+f 4731//4731 4802//4802 2278//2278
+f 4803//4803 2147//2147 4804//4804
+f 4841//4841 1212//1212 1095//1095
+f 2197//2197 4379//4379 4808//4808
+f 4791//4791 4746//4746 4798//4798
+f 4341//4341 2966//2966 4805//4805
+f 4806//4806 4739//4739 4746//4746
+f 4807//4807 2197//2197 4808//4808
+f 4809//4809 4802//4802 4800//4800
+f 4735//4735 4784//4784 4809//4809
+f 4809//4809 4800//4800 4735//4735
+f 3004//3004 4810//4810 3249//3249
+f 4789//4789 4784//4784 4735//4735
+f 2288//2288 2282//2282 4404//4404
+f 4811//4811 35//35 4831//4831
+f 4883//4883 4844//4844 4812//4812
+f 4880//4880 4812//4812 4840//4840
+f 4812//4812 4844//4844 4840//4840
+f 4887//4887 4813//4813 4828//4828
+f 4831//4831 2292//2292 2275//2275
+f 4813//4813 2115//2115 4828//4828
+f 4838//4838 4814//4814 4815//4815
+f 4875//4875 2279//2279 4876//4876
+f 4827//4827 2116//2116 4862//4862
+f 4342//4342 4819//4819 4863//4863
+f 4863//4863 2116//2116 4342//4342
+f 2115//2115 4870//4870 2116//2116
+f 4817//4817 4816//4816 4818//4818
+f 2120//2120 4817//4817 4818//4818
+f 2170//2170 2169//2169 4818//4818
+f 2116//2116 4870//4870 4342//4342
+f 938//938 4874//4874 4877//4877
+f 4849//4849 4850//4850 4899//4899
+f 938//938 943//943 4874//4874
+f 4822//4822 942//942 4877//4877
+f 4820//4820 4819//4819 943//943
+f 4820//4820 4821//4821 4862//4862
+f 4822//4822 4823//4823 953//953
+f 4823//4823 4876//4876 953//953
+f 4892//4892 4824//4824 4837//4837
+f 4825//4825 4892//4892 4837//4837
+f 2279//2279 4826//4826 4876//4876
+f 4827//4827 4828//4828 2116//2116
+f 941//941 4845//4845 4827//4827
+f 4847//4847 4830//4830 4851//4851
+f 2290//2290 2177//2177 4829//4829
+f 4835//4835 2118//2118 4825//4825
+f 4834//4834 2279//2279 4875//4875
+f 4830//4830 3772//3772 4851//4851
+f 2104//2104 4844//4844 3763//3763
+f 4847//4847 4848//4848 4830//4830
+f 4875//4875 2171//2171 4834//4834
+f 4831//4831 41//41 4811//4811
+f 2275//2275 41//41 4831//4831
+f 4892//4892 4832//4832 4824//4824
+f 4832//4832 3258//3258 4824//4824
+f 4874//4874 2171//2171 4875//4875
+f 4868//4868 4867//4867 2180//2180
+f 4858//4858 3272//3272 4833//4833
+f 4834//4834 2171//2171 4839//4839
+f 4839//4839 4848//4848 4847//4847
+f 4847//4847 4834//4834 4839//4839
+f 4816//4816 269//269 2170//2170
+f 4837//4837 4835//4835 4825//4825
+f 4850//4850 4835//4835 4837//4837
+f 4838//4838 4836//4836 4850//4850
+f 4837//4837 4838//4838 4850//4850
+f 2171//2171 4819//4819 4839//4839
+f 4863//4863 4819//4819 4820//4820
+f 4379//4379 4849//4849 3253//3253
+f 4880//4880 4840//4840 4855//4855
+f 990//990 4605//4605 4841//4841
+f 4856//4856 3777//3777 2274//2274
+f 4842//4842 3772//3772 4830//4830
+f 2107//2107 4843//4843 4856//4856
+f 4827//4827 4845//4845 4860//4860
+f 4844//4844 2104//2104 4840//4840
+f 4845//4845 2779//2779 4860//4860
+f 4846//4846 4814//4814 4824//4824
+f 4818//4818 4816//4816 2170//2170
+f 3258//3258 4846//4846 4824//4824
+f 4847//4847 4851//4851 3773//3773
+f 3777//3777 4830//4830 4848//4848
+f 4885//4885 3274//3274 3250//3250
+f 4824//4824 4814//4814 4837//4837
+f 4818//4818 4796//4796 2120//2120
+f 4849//4849 4835//4835 4850//4850
+f 4814//4814 4838//4838 4837//4837
+f 4851//4851 4852//4852 3773//3773
+f 3777//3777 4842//4842 4830//4830
+f 4406//4406 4883//4883 4812//4812
+f 2290//2290 4829//4829 4865//4865
+f 2120//2120 4796//4796 4865//4865
+f 4865//4865 2292//2292 2120//2120
+f 4836//4836 4853//4853 4899//4899
+f 2169//2169 4796//4796 4818//4818
+f 4854//4854 3253//3253 4864//4864
+f 4880//4880 3843//3843 4881//4881
+f 4843//4843 4855//4855 4856//4856
+f 37//37 4811//4811 41//41
+f 36//36 4816//4816 4817//4817
+f 36//36 4817//4817 2358//2358
+f 4843//4843 4880//4880 4855//4855
+f 2358//2358 35//35 36//36
+f 33//33 269//269 4816//4816
+f 269//269 4857//4857 2170//2170
+f 4858//4858 4833//4833 4859//4859
+f 4860//4860 3280//3280 4887//4887
+f 3250//3250 4878//4878 4861//4861
+f 4791//4791 4796//4796 2169//2169
+f 3843//3843 3536//3536 4881//4881
+f 4863//4863 4820//4820 4862//4862
+f 4862//4862 2116//2116 4863//4863
+f 4864//4864 4393//4393 4854//4854
+f 4866//4866 2290//2290 4865//4865
+f 4884//4884 3250//3250 4861//4861
+f 4865//4865 4796//4796 4866//4866
+f 2177//2177 2176//2176 4872//4872
+f 4848//4848 4690//4690 2274//2274
+f 4867//4867 2200//2200 2207//2207
+f 4867//4867 4868//4868 2206//2206
+f 4868//4868 4829//4829 4872//4872
+f 3710//3710 4869//4869 4870//4870
+f 4402//4402 4871//4871 4405//4405
+f 2177//2177 4872//4872 4829//4829
+f 4870//4870 4869//4869 4690//4690
+f 4873//4873 4877//4877 4874//4874
+f 3249//3249 4401//4401 3004//3004
+f 4874//4874 4875//4875 4873//4873
+f 2401//2401 4896//4896 4889//4889
+f 4873//4873 4875//4875 4876//4876
+f 4401//4401 4367//4367 3004//3004
+f 4873//4873 4876//4876 4823//4823
+f 4823//4823 4877//4877 4873//4873
+f 3587//3587 4648//4648 4636//4636
+f 4833//4833 4635//4635 4636//4636
+f 4878//4878 4900//4900 4879//4879
+f 4861//4861 4878//4878 4879//4879
+f 4812//4812 4880//4880 4881//4881
+f 4881//4881 2110//2110 4812//4812
+f 4890//4890 4882//4882 4883//4883
+f 4899//4899 4864//4864 4849//4849
+f 943//943 4819//4819 2171//2171
+f 4815//4815 3255//3255 4838//4838
+f 4815//4815 239//239 3255//3255
+f 4861//4861 4879//4879 959//959
+f 4879//4879 4393//4393 959//959
+f 4393//4393 4853//4853 970//970
+f 4884//4884 3278//3278 4885//4885
+f 4861//4861 4886//4886 4884//4884
+f 4860//4860 4887//4887 4828//4828
+f 4836//4836 254//254 4853//4853
+f 4888//4888 2401//2401 4889//4889
+f 3277//3277 4882//4882 4890//4890
+f 4849//4849 4379//4379 4891//4891
+f 4825//4825 2118//2118 4893//4893
+f 2119//2119 3258//3258 4832//4832
+f 4827//4827 4860//4860 4828//4828
+f 2391//2391 4390//4390 2398//2398
+f 4883//4883 4406//4406 3618//3618
+f 4832//4832 4892//4892 2124//2124
+f 4892//4892 4825//4825 4893//4893
+f 3004//3004 4373//4373 4810//4810
+f 4894//4894 2405//2405 4889//4889
+f 4405//4405 3277//3277 4890//4890
+f 4895//4895 3250//3250 3274//3274
+f 3249//3249 4878//4878 4895//4895
+f 4896//4896 4894//4894 4889//4889
+f 4897//4897 4898//4898 4900//4900
+f 4878//4878 4897//4897 4900//4900
+f 3254//3254 3256//3256 4898//4898
+f 4853//4853 4393//4393 4864//4864
+f 4899//4899 4853//4853 4864//4864
+f 2177//2177 2290//2290 4787//4787
+f 4879//4879 4900//4900 4854//4854
+# 9796 faces, 0 coords texture
+
+# End of File
\ No newline at end of file
diff --git a/Simbody/tests/adhoc/ContactBigMeshes_Patella.obj b/Simbody/tests/adhoc/ContactBigMeshes_Patella.obj
new file mode 100644
index 0000000..98a5573
--- /dev/null
+++ b/Simbody/tests/adhoc/ContactBigMeshes_Patella.obj
@@ -0,0 +1,6504 @@
+####
+#
+# OBJ File Generated by Meshlab
+#
+####
+# Object Pat_w_Cart_m_OS.obj
+#
+# Vertices: 1623
+# Faces: 3242
+#
+####
+vn -0.399876 0.321614 -0.858291
+v 0.000338 0.030457 -0.017672
+vn 0.088623 0.158574 -0.983362
+v 0.001728 0.030697 -0.017799
+vn -0.122962 0.220330 -0.967644
+v 0.000682 0.029729 -0.017967
+vn -0.583295 0.342823 -0.736369
+v 0.000720 0.032194 -0.017168
+vn -0.687181 0.408958 -0.600446
+v -0.000103 0.030579 -0.017275
+vn 0.338484 0.079892 -0.937575
+v 0.002790 0.031296 -0.017496
+vn -0.581676 0.472628 -0.662024
+v 0.001491 0.035138 -0.016387
+vn 0.127266 0.180874 -0.975237
+v 0.002339 0.032628 -0.017416
+vn -0.188292 0.248712 -0.950099
+v 0.001348 0.032044 -0.017498
+vn -0.096635 0.257733 -0.961372
+v 0.001958 0.033887 -0.017144
+vn 0.246688 0.249128 -0.936526
+v 0.002766 0.034196 -0.017006
+vn -0.660337 0.500490 -0.559879
+v -0.001756 0.029177 -0.016033
+vn -0.773347 0.377758 -0.509150
+v 0.000127 0.031774 -0.016681
+vn -0.716566 0.340082 -0.608997
+v 0.000734 0.034426 -0.015930
+vn -0.737206 0.394385 -0.548624
+v -0.000600 0.030967 -0.016095
+vn -0.386872 0.374422 -0.842697
+v 0.001642 0.034643 -0.016819
+vn -0.481002 0.297807 -0.824590
+v 0.001315 0.033677 -0.016991
+vn -0.689248 0.356003 -0.631030
+v 0.001198 0.034460 -0.016557
+vn -0.089471 0.408730 -0.908259
+v 0.002150 0.034969 -0.016798
+vn -0.751332 0.316515 -0.579067
+v 0.000117 0.032685 -0.016019
+vn -0.748217 0.325561 -0.578084
+v 0.000752 0.033386 -0.016573
+vn -0.327108 0.713628 -0.619464
+v 0.001679 0.036734 -0.015151
+vn -0.581577 0.479632 -0.657055
+v 0.001259 0.035819 -0.015647
+vn 0.333800 0.528379 -0.780637
+v 0.000866 0.037810 -0.013895
+vn -0.442799 0.246758 -0.861997
+v -0.000037 0.033628 -0.015469
+vn -0.428790 0.362652 -0.827419
+v 0.000462 0.035403 -0.015156
+vn -0.285466 0.536733 -0.793994
+v 0.002068 0.035679 -0.016351
+vn -0.336783 0.642241 -0.688552
+v 0.002004 0.036236 -0.015896
+vn 0.224284 0.726154 -0.649920
+v 0.001733 0.038311 -0.013121
+vn 0.170949 0.813448 -0.555949
+v 0.003036 0.038663 -0.012286
+vn -0.074183 0.840746 -0.536323
+v 0.002492 0.037243 -0.014737
+vn -0.082830 0.773717 -0.628094
+v 0.001564 0.037485 -0.014143
+vn 0.029942 0.854985 -0.517788
+v 0.002630 0.037908 -0.013580
+vn 0.067342 0.870301 -0.487894
+v 0.003646 0.037567 -0.014183
+vn 0.112070 0.797722 -0.592520
+v 0.003615 0.036996 -0.015134
+vn 0.306507 0.843756 -0.440601
+v 0.005543 0.039213 -0.010415
+vn 0.357909 0.772880 -0.523982
+v 0.003510 0.040546 -0.009481
+vn 0.418397 0.720295 -0.553281
+v 0.002028 0.040227 -0.010884
+vn 0.385263 0.681518 -0.622178
+v 0.002003 0.039181 -0.012086
+vn 0.239930 0.800078 -0.549826
+v 0.004741 0.037283 -0.014372
+vn 0.182303 0.862731 -0.471657
+v 0.004864 0.037888 -0.013286
+vn 0.185628 0.869731 -0.457285
+v 0.005035 0.038519 -0.012016
+vn 0.293940 0.772054 -0.563500
+v 0.003391 0.039560 -0.010910
+vn 0.195697 0.844234 -0.498970
+v 0.004425 0.039003 -0.011325
+vn 0.324491 0.839674 -0.435492
+v 0.005836 0.038392 -0.011828
+vn 0.097474 0.874175 -0.475727
+v 0.003958 0.038221 -0.012871
+vn -0.057676 0.766774 -0.639321
+v 0.002639 0.036653 -0.015619
+vn 0.612752 0.757288 -0.225940
+v 0.008252 0.039540 -0.005363
+vn 0.344729 0.799218 -0.492354
+v 0.005785 0.037638 -0.013241
+vn 0.585392 0.625815 -0.515433
+v 0.006462 0.037413 -0.012986
+vn 0.481912 0.770179 -0.417834
+v 0.006629 0.038059 -0.011748
+vn 0.888908 0.194454 -0.414764
+v 0.008338 0.037061 -0.010007
+vn 0.448016 0.806304 -0.386208
+v 0.006566 0.039102 -0.009699
+vn 0.867972 0.095929 -0.487259
+v 0.007470 0.036847 -0.011690
+vn 0.589869 0.765965 -0.255642
+v 0.007380 0.039833 -0.006687
+vn 0.566640 0.756146 -0.327357
+v 0.007374 0.039206 -0.008337
+vn 0.773883 0.503361 -0.384360
+v 0.007966 0.037662 -0.010211
+vn 0.689744 0.659728 -0.298349
+v 0.008168 0.038603 -0.008084
+vn 0.608457 0.703392 -0.367450
+v 0.007418 0.038354 -0.010081
+vn 0.912922 0.264352 -0.310954
+v 0.009388 0.037187 -0.007096
+vn 0.947874 0.058154 -0.313293
+v 0.009773 0.036139 -0.006406
+vn 0.930382 0.084965 -0.356609
+v 0.008997 0.036655 -0.008519
+vn 0.921494 0.287415 -0.261230
+v 0.010030 0.037663 -0.004673
+vn 0.821856 0.513170 -0.247406
+v 0.009791 0.038326 -0.004534
+vn 0.864161 0.400694 -0.304418
+v 0.009609 0.037864 -0.005743
+vn 0.942766 0.173537 -0.284742
+v 0.009989 0.036913 -0.005387
+vn 0.687023 0.681184 -0.252957
+v 0.009273 0.038812 -0.004900
+vn 0.758400 0.580492 -0.296411
+v 0.009003 0.038249 -0.006785
+vn 0.840536 0.429372 -0.330361
+v 0.008727 0.037554 -0.008612
+vn 0.950463 0.201239 -0.236903
+v 0.010480 0.037070 -0.003528
+vn 0.926341 -0.068363 -0.370431
+v 0.009258 0.035567 -0.007794
+vn 0.969041 0.035352 -0.244355
+v 0.011071 0.034992 -0.002125
+vn 0.973314 -0.172416 -0.151437
+v 0.011617 0.033539 0.000695
+vn 0.970674 -0.223564 -0.088380
+v 0.012024 0.033801 0.003584
+vn 0.953627 -0.066889 -0.293464
+v 0.010730 0.033920 -0.003284
+vn 0.949243 -0.033591 -0.312745
+v 0.010249 0.035012 -0.004971
+vn 0.960462 0.085756 -0.264874
+v 0.010518 0.035959 -0.003975
+vn 0.963138 -0.130518 -0.235224
+v 0.011045 0.033097 -0.001882
+vn 0.957045 -0.281258 -0.070423
+v 0.010743 0.030179 0.001259
+vn 0.932678 -0.263760 -0.246055
+v 0.009819 0.029692 -0.003705
+vn 0.926416 -0.345796 -0.148928
+v 0.009919 0.028488 -0.001368
+vn 0.919311 -0.333562 -0.208813
+v 0.009628 0.028493 -0.002882
+vn 0.959020 -0.234574 -0.158920
+v 0.011088 0.032346 -0.000916
+vn 0.956491 -0.267634 -0.116177
+v 0.010894 0.031171 -0.000046
+vn 0.946587 -0.275014 -0.168344
+v 0.010272 0.029733 -0.001658
+vn 0.873967 -0.337127 -0.350040
+v 0.007172 0.026282 -0.008235
+vn 0.892698 -0.350329 -0.283480
+v 0.007951 0.026615 -0.006405
+vn 0.901375 -0.367883 -0.228441
+v 0.008718 0.027066 -0.004296
+vn 0.906694 -0.379393 -0.184303
+v 0.009172 0.027157 -0.002454
+vn 0.919254 -0.227788 -0.321069
+v 0.009074 0.029678 -0.006135
+vn 0.909879 -0.314890 -0.270120
+v 0.009001 0.028333 -0.005106
+vn 0.892796 -0.298056 -0.337754
+v 0.008085 0.027996 -0.007513
+vn 0.949757 -0.193144 -0.246286
+v 0.010619 0.031999 -0.002828
+vn 0.949185 -0.245978 -0.196324
+v 0.010484 0.030990 -0.002316
+vn 0.846920 -0.333845 -0.413852
+v 0.004701 0.023251 -0.011068
+vn 0.497381 -0.576252 -0.648495
+v 0.001403 0.021448 -0.014800
+vn 0.633715 -0.498929 -0.591164
+v 0.002194 0.021711 -0.014322
+vn 0.359444 -0.614079 -0.702643
+v 0.000475 0.021553 -0.015517
+vn 0.607122 -0.484119 -0.630104
+v 0.001653 0.022557 -0.015502
+vn 0.181014 -0.572965 -0.799341
+v -0.000218 0.022338 -0.016384
+vn 0.666249 -0.419648 -0.616448
+v 0.001905 0.023749 -0.016104
+vn 0.788481 -0.390315 -0.475345
+v 0.002916 0.022298 -0.013781
+vn 0.827582 -0.329910 -0.454166
+v 0.003668 0.023743 -0.013460
+vn 0.843307 -0.338162 -0.417706
+v 0.005122 0.025413 -0.011913
+vn 0.249556 -0.439061 -0.863104
+v 0.000195 0.024171 -0.017371
+vn 0.266696 -0.496407 -0.826107
+v 0.000277 0.023226 -0.016837
+vn 0.022411 -0.486552 -0.873364
+v -0.000698 0.023433 -0.017120
+vn 0.443746 -0.526283 -0.725338
+v 0.000888 0.022730 -0.016234
+vn 0.729907 -0.386055 -0.564090
+v 0.002485 0.022665 -0.014646
+vn 0.774005 -0.365710 -0.516888
+v 0.002724 0.023872 -0.015111
+vn 0.863228 -0.341207 -0.372043
+v 0.005425 0.022676 -0.009029
+vn 0.476593 -0.640114 -0.602588
+v 0.001328 0.020524 -0.013971
+vn 0.666995 -0.558616 -0.493017
+v 0.002243 0.021232 -0.013783
+vn 0.581029 -0.640096 -0.502675
+v 0.001599 0.020039 -0.013120
+vn 0.647260 -0.605240 -0.463399
+v 0.002193 0.019951 -0.012213
+vn 0.727425 -0.519038 -0.448835
+v 0.002673 0.021225 -0.013077
+vn 0.748727 -0.494642 -0.441290
+v 0.003129 0.021085 -0.012169
+vn 0.398062 -0.701925 -0.590633
+v 0.000312 0.018945 -0.012933
+vn 0.808199 -0.405441 -0.427120
+v 0.003907 0.021530 -0.011215
+vn 0.820924 -0.373068 -0.432324
+v 0.003582 0.022319 -0.012561
+vn 0.305282 -0.677907 -0.668763
+v 0.000003 0.020005 -0.014254
+vn 0.858133 -0.336215 -0.388031
+v 0.006039 0.024921 -0.009597
+vn -0.027020 -0.442701 -0.896262
+v -0.000938 0.024541 -0.017700
+vn -0.378898 -0.476335 -0.793436
+v -0.003432 0.022474 -0.015516
+vn 0.147328 -0.642463 -0.752021
+v -0.000746 0.021064 -0.015499
+vn -0.061686 -0.538025 -0.840669
+v -0.001285 0.022397 -0.016503
+vn -0.342271 -0.484944 -0.804786
+v -0.002103 0.022275 -0.016227
+vn -0.466490 -0.493300 -0.734195
+v -0.002780 0.021653 -0.015419
+vn -0.303684 -0.422784 -0.853833
+v -0.001809 0.023824 -0.017161
+vn -0.424877 -0.586673 -0.689416
+v -0.002740 0.020022 -0.014190
+vn -0.087982 -0.620813 -0.779006
+v -0.001658 0.020972 -0.015458
+vn -0.360590 -0.571114 -0.737430
+v -0.002230 0.020920 -0.015242
+vn -0.492188 -0.377261 -0.784491
+v -0.002652 0.023299 -0.016444
+vn -0.592743 -0.211740 -0.777060
+v -0.002395 0.024934 -0.017267
+vn -0.180993 -0.056722 -0.981847
+v -0.003444 0.025650 -0.016804
+vn -0.600025 0.076004 -0.796362
+v -0.002450 0.026225 -0.017278
+vn -0.162722 -0.656461 -0.736601
+v -0.004485 0.021671 -0.014596
+vn -0.632280 0.495623 -0.595467
+v -0.002039 0.028245 -0.016663
+vn -0.383033 0.611731 -0.692150
+v -0.003040 0.027973 -0.016058
+vn -0.753265 0.462444 -0.467694
+v -0.000787 0.030025 -0.016701
+vn -0.445275 0.516388 -0.731487
+v -0.002733 0.028333 -0.015759
+vn 0.276374 0.048811 0.959810
+v 0.004368 0.030727 0.021051
+vn 0.010558 0.047799 0.998801
+v 0.004931 0.031748 0.020944
+vn 0.234648 0.045063 0.971035
+v 0.004376 0.032080 0.021000
+vn -0.263872 0.020483 0.964340
+v 0.005318 0.031390 0.021042
+vn -0.220255 0.162815 0.961758
+v 0.005395 0.032929 0.020898
+vn 0.075497 0.101223 0.991995
+v 0.004984 0.032914 0.020849
+vn 0.388602 0.490012 0.780306
+v 0.007280 0.039486 0.016836
+vn 0.067704 0.656712 0.751096
+v 0.006446 0.041312 0.015697
+vn 0.206262 0.673862 0.709483
+v 0.006154 0.040447 0.016494
+vn 0.355429 0.550150 0.755649
+v 0.006558 0.039582 0.017023
+vn 0.230951 0.575386 0.784597
+v 0.006816 0.040109 0.016582
+vn 0.582348 0.436747 0.685656
+v 0.007737 0.039447 0.016555
+vn 0.354684 0.483558 0.800232
+v 0.006959 0.038991 0.017235
+vn 0.486167 0.435754 0.757469
+v 0.007438 0.038406 0.017333
+vn 0.448869 0.557688 0.698212
+v 0.007459 0.037295 0.018011
+vn 0.338534 0.785135 0.518611
+v 0.008100 0.035964 0.019187
+vn 0.034698 0.583178 0.811603
+v 0.007864 0.035018 0.020551
+vn 0.110000 0.759030 0.641696
+v 0.007591 0.035606 0.019951
+vn 0.287357 0.716087 0.636118
+v 0.007338 0.036351 0.019031
+vn 0.551673 0.704464 0.446529
+v 0.008810 0.036298 0.018031
+vn 0.519266 0.765926 0.379105
+v 0.009361 0.035687 0.018537
+vn 0.450563 0.563890 0.692113
+v 0.006822 0.037088 0.018580
+vn 0.393676 0.012501 0.919164
+v 0.008181 0.033620 0.020839
+vn 0.592983 -0.127647 0.795033
+v 0.008459 0.032740 0.020634
+vn 0.454623 0.646086 0.613100
+v 0.008732 0.035046 0.020188
+vn 0.504105 0.186610 0.843241
+v 0.008640 0.034562 0.020528
+vn 0.541021 0.682026 0.492073
+v 0.009226 0.035174 0.019612
+vn 0.627960 -0.023880 0.777879
+v 0.008891 0.033940 0.020411
+vn 0.781593 -0.037952 0.622633
+v 0.009426 0.034116 0.019876
+vn 0.662841 0.268939 0.698795
+v 0.009116 0.034681 0.020139
+vn 0.259486 0.783727 0.564304
+v 0.008221 0.035371 0.020076
+vn 0.397741 0.790358 0.465978
+v 0.008710 0.035541 0.019477
+vn 0.311445 0.575822 0.755930
+v 0.008359 0.034967 0.020494
+vn 0.833178 -0.402360 0.379369
+v 0.009032 0.029781 0.017897
+vn 0.854997 -0.301056 0.422309
+v 0.009777 0.032011 0.018516
+vn 0.862348 -0.053086 0.503526
+v 0.009988 0.034261 0.019075
+vn 0.834557 -0.197557 0.514281
+v 0.009674 0.033190 0.019332
+vn 0.750040 -0.181871 0.635896
+v 0.009077 0.033036 0.020102
+vn 0.794637 -0.304992 0.524911
+v 0.009125 0.031842 0.019540
+vn 0.914717 -0.337333 0.222482
+v 0.010362 0.031448 0.016126
+vn 0.942658 -0.318927 0.098395
+v 0.010443 0.029738 0.011880
+vn 0.894603 -0.205488 0.396813
+v 0.010270 0.033323 0.018269
+vn 0.855843 -0.381339 0.349447
+v 0.009673 0.030954 0.017784
+vn 0.906904 -0.332549 0.258722
+v 0.009647 0.029504 0.015900
+vn 0.956858 -0.129394 0.260152
+v 0.011128 0.034246 0.016338
+vn 0.932818 -0.301002 0.198113
+v 0.010930 0.032633 0.015439
+vn 0.958806 -0.219028 0.180878
+v 0.011458 0.033710 0.014468
+vn 0.929305 -0.243648 0.277538
+v 0.010732 0.033240 0.016967
+vn 0.902050 -0.310898 0.299414
+v 0.010235 0.032000 0.017361
+vn 0.929098 -0.359647 0.086200
+v 0.011110 0.031347 0.011164
+vn 0.930841 -0.336032 0.143591
+v 0.010987 0.031788 0.013520
+vn 0.931231 -0.323336 0.168116
+v 0.010237 0.030127 0.014371
+vn 0.946273 -0.250294 0.204746
+v 0.009415 0.027877 0.014780
+vn 0.944250 -0.326287 0.043911
+v 0.010751 0.030034 0.009048
+vn 0.956805 -0.284496 0.059888
+v 0.010109 0.028129 0.009977
+vn 0.968557 -0.226233 0.103515
+v 0.012064 0.033761 0.010640
+vn 0.945874 -0.293965 0.137506
+v 0.011603 0.032998 0.012390
+vn 0.930980 -0.356999 0.076344
+v 0.011670 0.032548 0.010443
+vn 0.984549 -0.173559 0.023267
+v 0.012311 0.034205 0.008348
+vn 0.863308 0.496495 0.090513
+v 0.011925 0.036406 0.007644
+vn 0.904640 0.404774 0.133360
+v 0.011911 0.035790 0.010397
+vn 0.835622 0.540968 0.095334
+v 0.011368 0.037250 0.007764
+vn 0.991895 0.069303 0.106494
+v 0.012257 0.034823 0.010046
+vn 0.941770 0.335493 0.022670
+v 0.012146 0.036049 0.006936
+vn 0.752555 0.624717 0.208302
+v 0.011048 0.036347 0.013058
+vn 0.987255 0.027031 0.156835
+v 0.011887 0.034580 0.012645
+vn 0.788515 0.588230 0.179525
+v 0.010955 0.037124 0.010974
+vn 0.830890 0.538514 0.140091
+v 0.011386 0.036597 0.010758
+vn 0.885668 0.430902 0.172962
+v 0.011475 0.035618 0.013542
+vn 0.955699 0.285783 0.070481
+v 0.012237 0.035551 0.008569
+vn 0.966340 0.146714 0.211335
+v 0.011409 0.034912 0.015246
+vn 0.844706 0.271193 0.461440
+v 0.010201 0.034896 0.018621
+vn 0.637387 0.672606 0.375951
+v 0.010003 0.035307 0.018388
+vn 0.705833 0.668124 0.235393
+v 0.010859 0.035846 0.015191
+vn 0.751631 0.334972 0.568195
+v 0.009635 0.034823 0.019488
+vn 0.679650 0.684677 0.263238
+v 0.010399 0.036310 0.015051
+vn 0.605267 0.737258 0.300171
+v 0.010164 0.035782 0.017012
+vn 0.635880 0.696120 0.333277
+v 0.009633 0.036277 0.016812
+vn 0.780906 0.558957 0.278843
+v 0.009962 0.036985 0.014596
+vn 0.923996 -0.067847 0.376334
+v 0.010599 0.034380 0.017893
+vn 0.897701 0.291951 0.329996
+v 0.010772 0.035027 0.017283
+vn 0.773321 0.576450 0.263968
+v 0.010847 0.035450 0.016314
+vn 0.771393 0.594361 0.227349
+v 0.010550 0.037060 0.012651
+vn 0.492151 0.637350 0.592935
+v 0.008078 0.036786 0.018065
+vn 0.872435 0.445046 0.201969
+v 0.010110 0.037910 0.012231
+vn 0.912868 0.286061 0.291275
+v 0.009091 0.039736 0.014008
+vn 0.894547 0.365304 0.257561
+v 0.009683 0.037874 0.013982
+vn 0.728624 0.481090 0.487503
+v 0.008713 0.037383 0.016776
+vn 0.876952 0.335149 0.344426
+v 0.009207 0.038165 0.015093
+vn 0.936286 0.269433 0.225333
+v 0.009520 0.039078 0.013162
+vn 0.654804 0.425268 0.624803
+v 0.008083 0.038097 0.017000
+vn 0.777493 0.513136 0.363588
+v 0.009331 0.037040 0.016059
+vn 0.829199 0.546802 0.115915
+v 0.010876 0.037781 0.008889
+vn 0.842893 0.538080 0.001300
+v 0.009884 0.039697 0.006150
+vn 0.901434 0.432602 -0.016509
+v 0.009647 0.040163 0.007765
+vn 0.882331 0.460655 -0.096383
+v 0.009299 0.041066 0.008191
+vn 0.933005 0.357542 -0.040809
+v 0.009446 0.040997 0.009519
+vn 0.866890 0.492474 0.077270
+v 0.010240 0.038952 0.008132
+vn 0.756929 0.651431 -0.051919
+v 0.009737 0.039734 0.002821
+vn 0.793843 0.606529 -0.043990
+v 0.009561 0.040080 0.004599
+vn 0.840532 0.517109 0.161566
+v 0.010485 0.037995 0.010301
+vn 0.946957 0.284884 0.148703
+v 0.009824 0.038889 0.011812
+vn 0.916530 0.386958 0.101171
+v 0.010023 0.038998 0.009957
+vn 0.958446 0.284318 0.023349
+v 0.009729 0.039994 0.009757
+vn 0.820301 0.567130 -0.073961
+v 0.009221 0.040669 0.005667
+vn 0.832245 0.551358 0.058085
+v 0.010595 0.038572 0.006600
+vn 0.708558 0.691425 -0.140984
+v 0.008525 0.041393 0.004808
+vn 0.582690 0.794657 -0.170270
+v 0.007570 0.041937 0.003612
+vn 0.639682 0.752174 -0.158245
+v 0.008162 0.041924 0.005602
+vn 0.569368 0.800335 -0.187841
+v 0.006837 0.042685 0.004628
+vn 0.538679 0.820335 -0.192030
+v 0.004956 0.043851 0.004050
+vn 0.653099 0.716161 -0.246120
+v 0.008043 0.042843 0.008602
+vn 0.676777 0.717997 -0.162644
+v 0.008369 0.042008 0.006928
+vn 0.586968 0.782057 -0.209418
+v 0.007067 0.043092 0.006902
+vn 0.736417 0.669925 -0.094283
+v 0.009012 0.040661 0.003758
+vn 0.811952 0.571902 -0.116884
+v 0.008950 0.041280 0.006606
+vn 0.456625 0.886351 -0.076657
+v 0.006409 0.044158 0.009530
+vn 0.722040 0.672411 -0.162854
+v 0.008438 0.042752 0.009446
+vn 0.557521 0.816719 -0.148799
+v 0.007562 0.043459 0.009391
+vn 0.851105 0.509135 -0.128072
+v 0.008974 0.041987 0.009350
+vn 0.760189 0.620269 -0.193336
+v 0.008740 0.041964 0.008300
+vn 0.663492 0.748043 -0.014475
+v 0.008174 0.043091 0.010076
+vn 0.534596 0.844562 0.030357
+v 0.007355 0.043684 0.010664
+vn 0.803581 0.502096 0.319621
+v 0.008464 0.041743 0.013456
+vn 0.877552 0.454268 0.153437
+v 0.008969 0.041825 0.011364
+vn 0.694362 0.555593 0.457359
+v 0.007959 0.041820 0.014349
+vn 0.796673 0.565501 0.213356
+v 0.008445 0.042402 0.012182
+vn 0.775590 0.440264 0.452358
+v 0.008388 0.040846 0.014715
+vn 0.960378 0.255476 0.111379
+v 0.009631 0.040037 0.011187
+vn 0.927922 0.364971 0.075868
+v 0.009334 0.041250 0.010540
+vn 0.932305 0.305755 0.193186
+v 0.009345 0.040498 0.012147
+vn 0.839904 0.541540 0.036005
+v 0.008821 0.042323 0.010195
+vn 0.735318 0.668286 0.112700
+v 0.008311 0.042879 0.010996
+vn 0.877138 0.362247 0.315287
+v 0.008782 0.040776 0.013942
+vn 0.895244 0.381514 0.230187
+v 0.008937 0.041236 0.012764
+vn 0.860204 0.313284 0.402370
+v 0.008694 0.039949 0.014912
+vn 0.830000 0.329203 0.450250
+v 0.008711 0.038726 0.015717
+vn 0.751876 0.368368 0.546798
+v 0.008280 0.039605 0.015855
+vn 0.026790 0.826423 0.562412
+v 0.006046 0.043341 0.013595
+vn 0.447606 0.875208 0.183465
+v 0.006894 0.043806 0.011988
+vn 0.285847 0.773282 0.565974
+v 0.006930 0.042862 0.014038
+vn -0.435538 0.806711 0.399404
+v 0.003470 0.041899 0.012993
+vn -0.733434 0.402361 0.547886
+v 0.003387 0.042672 0.011973
+vn 0.560920 0.687207 0.461644
+v 0.007507 0.042567 0.013986
+vn 0.701588 0.640969 0.311339
+v 0.007916 0.042641 0.013141
+vn 0.526401 0.781511 0.334877
+v 0.007269 0.043240 0.013055
+vn 0.637410 0.749081 0.180516
+v 0.007716 0.043264 0.011955
+vn -0.286095 0.851827 0.438795
+v 0.004479 0.041391 0.014961
+vn -0.133991 0.706087 0.695332
+v 0.005858 0.042487 0.014582
+vn 0.282396 0.867250 0.410037
+v 0.006646 0.043566 0.013050
+vn 0.178227 0.932919 0.312884
+v 0.006048 0.044039 0.012223
+vn 0.623572 0.497035 0.603418
+v 0.007872 0.040744 0.015525
+vn 0.343452 0.582970 0.736333
+v 0.007223 0.040740 0.015988
+vn 0.440136 0.635464 0.634401
+v 0.007393 0.041857 0.014939
+vn -0.478680 0.590237 0.649989
+v 0.005161 0.042474 0.014258
+vn 0.133559 0.704290 0.697236
+v 0.006671 0.042160 0.014922
+vn -0.316259 0.727174 0.609260
+v 0.005078 0.043389 0.013371
+vn -0.094555 0.904344 0.416198
+v 0.005053 0.044118 0.012204
+vn -0.451513 0.729031 0.514442
+v 0.003978 0.044002 0.011808
+vn -0.665789 0.443177 0.600266
+v 0.004296 0.043109 0.012937
+vn -0.606677 0.579485 0.544188
+v 0.004516 0.042186 0.013823
+vn 0.065275 0.790008 0.609612
+v 0.005462 0.040757 0.016321
+vn -0.136406 0.703855 0.697124
+v 0.005744 0.041576 0.015389
+vn 0.334333 0.939859 -0.069898
+v 0.004240 0.044908 0.007704
+vn 0.332142 0.939399 -0.084913
+v 0.002616 0.045264 0.004846
+vn -0.144376 0.941403 0.304821
+v 0.003744 0.044727 0.010080
+vn -0.798370 0.257450 0.544357
+v 0.000440 0.043246 0.007550
+vn -0.728162 0.374157 0.574271
+v 0.002983 0.043577 0.011059
+vn -0.515439 0.708367 0.482224
+v 0.002408 0.044487 0.009539
+vn -0.764687 0.331280 0.552727
+v 0.001278 0.043967 0.008561
+vn -0.790996 0.240262 0.562671
+v 0.002047 0.043120 0.009919
+vn 0.429903 0.893798 -0.127706
+v 0.005436 0.044484 0.008294
+vn 0.140225 0.984917 0.101368
+v 0.004337 0.044876 0.009358
+vn 0.359835 0.929362 0.082488
+v 0.006230 0.044246 0.010893
+vn 0.316536 0.948558 0.006515
+v 0.005305 0.044623 0.009573
+vn 0.120639 0.971018 0.206324
+v 0.005179 0.044522 0.010926
+vn -0.186788 0.940282 0.284568
+v 0.001932 0.045193 0.007568
+vn -0.283046 0.922332 0.263037
+v 0.000582 0.045460 0.005654
+vn 0.534654 0.820011 -0.204274
+v 0.006596 0.043810 0.008202
+vn 0.126067 0.989013 0.077208
+v 0.003054 0.045200 0.007545
+vn 0.087177 0.994413 0.059514
+v 0.001450 0.045554 0.005340
+vn -0.599120 0.648685 0.469322
+v 0.000709 0.044891 0.007000
+vn 0.484353 0.858172 -0.170128
+v 0.004842 0.044409 0.006207
+vn 0.111604 0.976348 -0.185173
+v -0.000172 0.045294 -0.000781
+vn 0.368898 0.910211 -0.188228
+v 0.001299 0.045038 -0.000075
+vn -0.570767 0.821069 -0.008421
+v -0.002242 0.044933 -0.000205
+vn -0.862786 0.409718 0.296196
+v -0.002928 0.043493 0.001406
+vn -0.867020 0.464021 0.181553
+v -0.003334 0.043587 -0.000129
+vn 0.033762 0.993441 -0.109248
+v -0.000284 0.045572 0.000897
+vn -0.477448 0.867128 -0.141887
+v -0.002470 0.044612 -0.002110
+vn 0.507591 0.836032 -0.208332
+v 0.002972 0.043894 -0.001151
+vn 0.192998 0.955228 -0.224258
+v 0.000006 0.044895 -0.002594
+vn 0.414941 0.881656 -0.224736
+v 0.001463 0.044532 -0.001980
+vn 0.312687 0.900012 -0.303654
+v 0.000365 0.044298 -0.004362
+vn -0.790344 0.587301 0.174454
+v -0.002734 0.044321 0.000656
+vn -0.207139 0.965688 -0.156650
+v -0.001379 0.045240 -0.000824
+vn -0.746875 0.664473 0.025575
+v -0.003115 0.044147 -0.001102
+vn -0.850597 0.519280 0.082657
+v -0.003665 0.043336 -0.001483
+vn -0.216341 0.969209 0.117605
+v -0.000016 0.045614 0.003956
+vn -0.313330 0.949093 -0.032363
+v -0.001315 0.045432 0.000697
+vn -0.235732 0.970845 0.043485
+v -0.000677 0.045589 0.002180
+vn 0.126660 0.991152 -0.039683
+v 0.000714 0.045650 0.003023
+vn -0.611916 0.779018 0.136710
+v -0.001815 0.045110 0.001521
+vn -0.814284 0.498144 0.297984
+v -0.002185 0.044388 0.002365
+vn -0.839181 0.360435 0.407262
+v -0.002224 0.043456 0.003150
+vn -0.603211 0.762810 0.232933
+v -0.001228 0.045197 0.003166
+vn -0.812794 0.403070 0.420596
+v -0.001461 0.044393 0.004037
+vn -0.633805 0.679015 0.370447
+v -0.000499 0.045140 0.004881
+vn -0.810746 0.298280 0.503706
+v -0.001159 0.043397 0.005069
+vn 0.296722 0.945080 -0.137039
+v 0.001161 0.045409 0.001677
+vn -0.794169 0.325926 0.512902
+v -0.000343 0.044254 0.006034
+vn -0.657620 0.596202 0.460521
+v -0.000520 0.042531 0.006624
+vn 0.454232 0.875574 -0.164449
+v 0.002948 0.044839 0.002930
+vn -0.702073 0.705875 -0.093993
+v -0.003341 0.043813 -0.002854
+vn -0.673324 0.713926 -0.192210
+v -0.003454 0.043362 -0.004372
+vn -0.457674 0.858607 -0.230930
+v -0.002622 0.044080 -0.004110
+vn -0.452479 0.838178 -0.304499
+v -0.002608 0.043488 -0.005882
+vn -0.173347 0.959861 -0.220493
+v -0.001403 0.044791 -0.003015
+vn -0.814896 0.579249 -0.020388
+v -0.003842 0.043158 -0.002858
+vn 0.025087 0.955769 -0.293047
+v -0.000779 0.044503 -0.004437
+vn -0.241092 0.919100 -0.311656
+v -0.001716 0.044090 -0.005252
+vn 0.429759 0.829878 -0.355822
+v 0.000085 0.042345 -0.009399
+vn 0.319508 0.885641 -0.336978
+v 0.000802 0.043404 -0.006242
+vn 0.487928 0.757280 -0.434112
+v 0.000616 0.041798 -0.009772
+vn 0.103915 0.916296 -0.386785
+v -0.000688 0.043896 -0.005969
+vn -0.175599 0.906124 -0.384843
+v -0.001655 0.043529 -0.006669
+vn -0.214108 0.943068 -0.254520
+v -0.002176 0.043055 -0.007486
+vn 0.070059 0.927125 -0.368146
+v -0.000799 0.043246 -0.007402
+vn 0.402367 0.853325 -0.331567
+v 0.003326 0.042068 -0.007026
+vn 0.393747 0.795212 -0.461088
+v 0.003454 0.041424 -0.008177
+vn 0.439225 0.828699 -0.346900
+v 0.004713 0.041383 -0.006965
+vn 0.373554 0.764664 -0.525116
+v 0.002276 0.041167 -0.009426
+vn 0.449363 0.855248 -0.258117
+v 0.002050 0.043739 -0.003709
+vn 0.496764 0.848376 -0.182984
+v 0.005435 0.041801 -0.003812
+vn 0.436665 0.865687 -0.244766
+v 0.003170 0.042677 -0.005309
+vn 0.352492 0.868274 -0.349070
+v 0.001728 0.042613 -0.007383
+vn 0.466969 0.855457 -0.223903
+v 0.004934 0.041647 -0.005799
+vn 0.384639 0.773734 -0.503377
+v 0.001392 0.041661 -0.009311
+vn 0.313954 0.814552 -0.487790
+v 0.004716 0.039920 -0.009658
+vn 0.369829 0.807056 -0.460312
+v 0.002157 0.041945 -0.008345
+vn 0.461179 0.747643 -0.477853
+v 0.001188 0.041110 -0.010312
+vn 0.396566 0.803539 -0.443914
+v 0.004709 0.040806 -0.008135
+vn 0.366493 0.821489 -0.436850
+v 0.000984 0.042236 -0.008721
+vn 0.544573 0.816623 -0.191227
+v 0.006705 0.040746 -0.005112
+vn 0.404787 0.818515 -0.407653
+v 0.005753 0.040091 -0.008510
+vn 0.497947 0.799905 -0.334964
+v 0.006598 0.040056 -0.007529
+vn 0.456427 0.813741 -0.359861
+v 0.005648 0.040768 -0.007188
+vn 0.549898 0.817324 -0.172027
+v 0.007347 0.040853 -0.002588
+vn 0.522761 0.835645 -0.168577
+v 0.006081 0.041925 -0.001235
+vn 0.510661 0.819999 -0.258508
+v 0.006100 0.040814 -0.006349
+vn 0.596306 0.789703 -0.144182
+v 0.008406 0.040581 -0.000210
+vn 0.800351 0.584172 -0.134838
+v 0.010211 0.038665 -0.001606
+vn 0.746488 0.660535 -0.080307
+v 0.010017 0.039229 0.000550
+vn 0.799928 0.600091 -0.002436
+v 0.010310 0.039091 0.004358
+vn 0.789327 0.612450 -0.043225
+v 0.010543 0.038732 0.002044
+vn 0.921901 0.339594 -0.186478
+v 0.010738 0.037634 -0.001760
+vn 0.842231 0.528175 -0.108063
+v 0.010734 0.038242 0.000011
+vn 0.939027 0.305278 -0.158220
+v 0.011274 0.037188 0.000242
+vn 0.822081 0.569329 -0.006823
+v 0.011062 0.038101 0.003551
+vn 0.685640 0.721089 -0.099643
+v 0.009141 0.040226 0.001479
+vn 0.725254 0.666024 -0.174410
+v 0.009689 0.038927 -0.003190
+vn 0.632823 0.761604 -0.139622
+v 0.008288 0.041164 0.002564
+vn 0.884847 0.421742 -0.197940
+v 0.010202 0.038095 -0.003364
+vn 0.818012 0.573165 0.048343
+v 0.010980 0.038148 0.005319
+vn 0.601512 0.778700 -0.178350
+v 0.008820 0.039750 -0.002944
+vn 0.674953 0.727451 -0.123504
+v 0.009441 0.039600 -0.001030
+vn 0.966226 0.144446 -0.213405
+v 0.011143 0.036289 -0.001324
+vn 0.955794 -0.277080 -0.098407
+v 0.011546 0.032788 0.001642
+vn 0.943380 -0.327341 -0.053683
+v 0.011781 0.032829 0.004429
+vn 0.992383 0.070058 -0.101329
+v 0.012141 0.035274 0.004134
+vn 0.998442 0.054957 -0.009623
+v 0.012358 0.034991 0.007501
+vn 0.960380 -0.276941 -0.031212
+v 0.012155 0.033640 0.006325
+vn 0.981246 0.170465 -0.089986
+v 0.012042 0.036018 0.003895
+vn 0.943500 0.316424 -0.098405
+v 0.011713 0.036792 0.002501
+vn 0.981798 0.066406 -0.177938
+v 0.011682 0.035193 0.000747
+vn 0.990417 -0.032738 -0.134172
+v 0.011980 0.034458 0.002455
+vn 0.840605 0.539598 0.047089
+v 0.011527 0.037333 0.005449
+vn 0.977444 0.141943 -0.156381
+v 0.011734 0.036140 0.001593
+vn 0.977096 -0.037174 -0.209525
+v 0.011460 0.034167 -0.000506
+vn 0.995164 -0.077142 -0.060818
+v 0.012290 0.034481 0.005574
+vn 0.867924 0.491264 -0.073271
+v 0.011215 0.037750 0.001767
+vn 0.886609 0.461686 -0.027768
+v 0.011634 0.037187 0.003779
+vn 0.986146 0.162452 -0.033556
+v 0.012243 0.035641 0.006290
+vn 0.950009 0.309535 -0.040885
+v 0.012021 0.036351 0.004907
+vn 0.892416 0.450719 0.021121
+v 0.011918 0.036650 0.005756
+vn 0.937350 -0.348367 -0.003852
+v 0.009611 0.026185 0.006006
+vn 0.954643 -0.297122 0.019364
+v 0.010502 0.029007 0.006834
+vn 0.942679 -0.333592 -0.008519
+v 0.011227 0.031199 0.005853
+vn 0.953932 -0.300010 -0.002748
+v 0.010248 0.028098 0.004933
+vn 0.940843 -0.336764 -0.037478
+v 0.009955 0.027309 0.003341
+vn 0.923582 -0.380799 -0.044590
+v 0.009442 0.025901 0.004102
+vn 0.927401 -0.372955 0.028844
+v 0.011417 0.031719 0.008663
+vn 0.933324 -0.358785 -0.013378
+v 0.011785 0.032574 0.006763
+vn 0.951513 -0.302586 -0.055366
+v 0.011262 0.031641 0.002617
+vn 0.955934 -0.292776 -0.021738
+v 0.010809 0.030034 0.003670
+vn 0.935304 -0.344635 -0.080213
+v 0.010001 0.027743 0.001515
+vn 0.943963 -0.328831 0.028344
+v 0.011981 0.033107 0.008624
+vn 0.925331 -0.331819 -0.183464
+v 0.007479 0.022818 -0.002235
+vn 0.917512 -0.369945 -0.145990
+v 0.009515 0.027308 -0.000829
+vn 0.940445 -0.322717 -0.106853
+v 0.010180 0.028544 0.000588
+vn 0.921013 -0.385072 -0.058769
+v 0.008875 0.024714 0.003228
+vn 0.927813 -0.293314 -0.230500
+v 0.006506 0.020684 -0.003463
+vn 0.897318 -0.361173 -0.253719
+v 0.007532 0.025109 -0.005723
+vn 0.902503 -0.374112 -0.213374
+v 0.008234 0.025763 -0.003992
+vn 0.906815 -0.348700 -0.236842
+v 0.007152 0.023738 -0.005090
+vn 0.892974 -0.348114 -0.285332
+v 0.006280 0.022687 -0.006765
+vn 0.946374 -0.302974 -0.112174
+v 0.010348 0.029357 -0.000293
+vn 0.954367 -0.295290 -0.044588
+v 0.010416 0.028857 0.002264
+vn 0.914874 -0.325030 -0.239500
+v 0.006684 0.022036 -0.004498
+vn 0.912463 -0.360606 -0.193327
+v 0.008190 0.024980 -0.002769
+vn 0.898695 -0.301479 -0.318524
+v 0.005641 0.019490 -0.005118
+vn 0.933385 -0.352919 -0.065122
+v 0.008042 0.022850 0.002043
+vn 0.922326 -0.374660 -0.094574
+v 0.008762 0.024876 0.001024
+vn 0.920038 -0.373471 -0.118529
+v 0.009503 0.026791 0.000481
+vn 0.920827 -0.382397 -0.076481
+v 0.009495 0.026357 0.002112
+vn 0.919127 -0.364421 -0.149673
+v 0.008593 0.025033 -0.000755
+vn -0.140401 -0.754299 -0.641343
+v -0.001529 0.014611 -0.007997
+vn 0.390027 -0.538367 -0.747021
+v 0.001339 0.015482 -0.008469
+vn 0.100501 -0.732078 -0.673767
+v -0.000628 0.015339 -0.008868
+vn 0.218159 -0.630917 -0.744547
+v 0.000478 0.015382 -0.008685
+vn 0.855288 -0.350626 -0.381502
+v 0.004814 0.018782 -0.006516
+vn 0.865435 -0.358179 -0.350329
+v 0.005121 0.019801 -0.006840
+vn 0.678931 -0.468888 -0.564975
+v 0.002961 0.017831 -0.008688
+vn 0.808569 -0.389505 -0.441024
+v 0.004346 0.018661 -0.007375
+vn 0.209392 -0.633197 -0.745129
+v 0.000824 0.014279 -0.007716
+vn 0.495830 -0.529027 -0.688682
+v 0.001825 0.014891 -0.007731
+vn 0.602785 -0.447373 -0.660687
+v 0.002255 0.016291 -0.008350
+vn 0.726283 -0.407991 -0.553223
+v 0.003339 0.017397 -0.007907
+vn 0.786384 -0.365942 -0.497681
+v 0.003827 0.017233 -0.007084
+vn 0.765730 -0.473398 -0.435375
+v 0.003886 0.020580 -0.010308
+vn 0.840046 -0.368548 -0.398115
+v 0.004574 0.021487 -0.009868
+vn 0.782800 -0.475632 -0.401247
+v 0.004146 0.020265 -0.009465
+vn 0.705947 -0.549258 -0.447162
+v 0.003077 0.020213 -0.011205
+vn 0.894756 -0.329506 -0.301394
+v 0.005632 0.020321 -0.005994
+vn 0.098909 -0.700197 -0.707065
+v -0.001308 0.019447 -0.014098
+vn 0.880568 -0.346866 -0.322930
+v 0.006596 0.024270 -0.007643
+vn 0.614981 -0.617873 -0.489931
+v 0.001940 0.018731 -0.010916
+vn 0.524273 -0.680480 -0.511943
+v 0.000917 0.018611 -0.011978
+vn 0.540151 -0.617057 -0.572257
+v 0.001374 0.017497 -0.010100
+vn 0.836660 -0.408365 -0.365019
+v 0.004689 0.020682 -0.008762
+vn 0.866614 -0.365290 -0.339916
+v 0.005425 0.021555 -0.007912
+vn 0.693416 -0.556090 -0.458189
+v 0.003274 0.019529 -0.010044
+vn 0.516923 -0.543645 -0.661241
+v 0.001608 0.016748 -0.009175
+vn 0.793215 -0.423893 -0.437178
+v 0.004247 0.019278 -0.008104
+vn 0.739326 -0.502007 -0.448761
+v 0.003725 0.019270 -0.009006
+vn 0.858811 -0.365197 -0.359269
+v 0.005177 0.020410 -0.007312
+vn 0.898373 -0.338256 -0.280195
+v 0.005965 0.021444 -0.006253
+vn 0.328612 -0.750725 -0.573085
+v -0.000406 0.017418 -0.011458
+vn 0.244169 -0.770819 -0.588405
+v -0.000675 0.016285 -0.010053
+vn 0.459072 -0.695160 -0.553177
+v 0.000503 0.017372 -0.010730
+vn 0.650140 -0.552588 -0.521503
+v 0.002513 0.018274 -0.009650
+vn 0.358345 -0.679695 -0.640003
+v 0.000403 0.016406 -0.009661
+vn 0.702283 -0.620594 -0.348801
+v 0.003141 0.012274 -0.003483
+vn 0.764517 -0.369012 -0.528530
+v 0.003540 0.015844 -0.006588
+vn 0.573191 -0.591506 -0.567073
+v 0.002422 0.013263 -0.005819
+vn 0.568056 -0.676522 -0.468647
+v 0.002354 0.012155 -0.004531
+vn 0.158944 -0.680103 -0.715679
+v 0.000524 0.012882 -0.006534
+vn 0.401353 -0.622747 -0.671641
+v 0.001572 0.013542 -0.006753
+vn 0.845947 -0.317411 -0.428513
+v 0.004548 0.017455 -0.006001
+vn 0.890380 -0.295915 -0.345915
+v 0.005186 0.017554 -0.004691
+vn 0.894676 -0.311573 -0.320120
+v 0.004949 0.015821 -0.003755
+vn 0.869311 -0.397354 -0.293954
+v 0.004636 0.014287 -0.002992
+vn 0.860146 -0.478628 -0.176253
+v 0.004636 0.013723 -0.001887
+vn 0.736825 -0.444321 -0.509576
+v 0.003344 0.014449 -0.005844
+vn 0.826107 -0.406579 -0.390181
+v 0.004022 0.014323 -0.004553
+vn 0.841033 -0.338685 -0.421847
+v 0.004277 0.015848 -0.005380
+vn 0.707372 -0.549201 -0.444976
+v 0.003181 0.013183 -0.004785
+vn -0.150436 -0.864426 -0.479726
+v -0.000854 0.010829 -0.004299
+vn -0.058823 -0.753287 -0.655057
+v -0.000605 0.011446 -0.005201
+vn -0.391665 -0.700969 -0.596021
+v -0.001520 0.011539 -0.004999
+vn -0.080620 -0.680586 -0.728220
+v -0.000531 0.012509 -0.006241
+vn 0.201582 -0.744608 -0.636336
+v 0.000442 0.011675 -0.005321
+vn 0.619209 -0.490271 -0.613363
+v 0.002562 0.014551 -0.006847
+vn -0.018619 -0.675756 -0.736891
+v -0.000393 0.014027 -0.007597
+vn 0.679326 -0.414609 -0.605488
+v 0.002812 0.015905 -0.007536
+vn 0.216509 -0.847423 -0.484765
+v 0.000323 0.010871 -0.004242
+vn 0.395093 -0.694846 -0.600908
+v 0.001464 0.012287 -0.005525
+vn -0.523860 -0.719584 -0.455818
+v -0.003885 0.014277 -0.005736
+vn -0.488287 -0.708359 -0.509709
+v -0.002982 0.014036 -0.006325
+vn -0.672179 -0.670426 -0.314171
+v -0.004655 0.013556 -0.003605
+vn -0.710245 -0.671808 -0.210303
+v -0.003582 0.012065 -0.002734
+vn -0.557958 -0.623632 -0.547508
+v -0.003180 0.013539 -0.005533
+vn -0.608730 -0.639271 -0.469874
+v -0.003888 0.013502 -0.004714
+vn -0.647909 -0.647855 -0.400622
+v -0.003248 0.012293 -0.003981
+vn -0.508244 -0.733700 -0.450968
+v -0.002006 0.011315 -0.004237
+vn -0.516195 -0.786906 -0.338114
+v -0.005424 0.015874 -0.007060
+vn -0.365555 -0.742120 -0.561807
+v -0.002322 0.014230 -0.007156
+vn -0.360896 -0.642942 -0.675558
+v -0.001523 0.012369 -0.005789
+vn -0.562358 -0.615998 -0.551634
+v -0.002575 0.012499 -0.005074
+vn -0.277930 -0.670856 -0.687537
+v -0.001469 0.013496 -0.006848
+vn -0.484345 -0.632570 -0.604373
+v -0.002353 0.013353 -0.006125
+vn -0.408335 -0.806893 -0.426832
+v -0.003302 0.014759 -0.007229
+vn -0.340030 -0.817382 -0.465045
+v -0.003230 0.015520 -0.008837
+vn -0.509899 -0.824548 -0.245202
+v -0.008191 0.017345 -0.005944
+vn -0.566135 -0.770106 -0.293985
+v -0.006448 0.015990 -0.005542
+vn -0.584358 -0.718482 -0.377238
+v -0.005118 0.014658 -0.004808
+vn -0.503166 -0.784199 -0.363120
+v -0.004707 0.015127 -0.006321
+vn -0.524142 -0.781494 -0.338440
+v -0.006681 0.016855 -0.007336
+vn -0.641110 -0.736986 0.214080
+v -0.008432 0.017176 0.001748
+vn -0.675583 -0.566144 0.472300
+v -0.004487 0.016086 0.006924
+vn -0.666023 -0.594802 0.450137
+v -0.005195 0.016318 0.006169
+vn -0.601490 -0.686078 0.409276
+v -0.004729 0.016591 0.007261
+vn -0.429322 -0.726551 0.536476
+v 0.000092 0.014411 0.010437
+vn -0.143017 -0.814394 0.562413
+v 0.001092 0.014507 0.011181
+vn -0.647703 -0.606133 0.461610
+v -0.003827 0.016157 0.007941
+vn -0.625845 -0.606440 0.490457
+v -0.002845 0.015781 0.008799
+vn 0.250031 -0.817046 0.519539
+v 0.002291 0.015653 0.012837
+vn 0.606261 -0.701668 0.374314
+v 0.003481 0.016472 0.013004
+vn -0.539151 -0.673049 0.506282
+v -0.000602 0.016272 0.012120
+vn -0.539399 -0.660013 0.522906
+v -0.000755 0.015047 0.010442
+vn -0.606560 -0.612403 0.506998
+v -0.001713 0.015399 0.009748
+vn -0.581109 -0.637630 0.505708
+v -0.001717 0.014206 0.008330
+vn 0.509743 -0.735301 0.446649
+v 0.002931 0.014984 0.011266
+vn 0.176642 -0.830722 0.527918
+v 0.001907 0.014173 0.010670
+vn -0.681712 -0.625850 0.378921
+v -0.003910 0.013635 0.004767
+vn -0.713571 -0.581540 0.390677
+v -0.004883 0.014855 0.004854
+vn -0.720716 -0.633945 0.280504
+v -0.004695 0.013976 0.003680
+vn 0.349099 -0.858049 0.376671
+v 0.001450 0.010396 0.004067
+vn 0.051060 -0.882241 0.468021
+v 0.000850 0.010691 0.004906
+vn -0.202319 -0.860467 0.467614
+v 0.000114 0.010474 0.004351
+vn -0.190788 -0.842205 0.504273
+v 0.000354 0.011719 0.006612
+vn -0.317705 -0.878130 0.357703
+v -0.000567 0.009620 0.002290
+vn -0.524221 -0.756264 0.391480
+v -0.001314 0.010461 0.003217
+vn -0.346486 -0.816056 0.462602
+v -0.000565 0.010394 0.003809
+vn -0.483069 -0.735521 0.475030
+v -0.001295 0.011609 0.005144
+vn -0.587081 -0.699898 0.406790
+v -0.002155 0.011724 0.004318
+vn -0.069735 -0.928222 0.365432
+v 0.000146 0.009637 0.002618
+vn -0.661147 -0.564241 0.494486
+v -0.003772 0.015438 0.007204
+vn 0.171547 -0.847164 0.502877
+v 0.001730 0.012992 0.008815
+vn -0.320413 -0.785269 0.529800
+v 0.000265 0.013124 0.008795
+vn -0.069879 -0.844225 0.531415
+v 0.000981 0.013041 0.008932
+vn 0.078562 -0.868153 0.490039
+v 0.001214 0.011898 0.007020
+vn -0.411277 -0.763222 0.498340
+v -0.000422 0.012017 0.006566
+vn -0.500233 -0.702321 0.506470
+v -0.001111 0.013310 0.007805
+vn -0.613446 -0.650056 0.448454
+v -0.002977 0.013127 0.005402
+vn -0.561203 -0.672592 0.482360
+v -0.002036 0.012941 0.006297
+vn -0.619672 -0.606547 0.498103
+v -0.002722 0.014443 0.007405
+vn 0.360652 -0.833784 0.418011
+v 0.001782 0.011390 0.005812
+vn -0.678510 -0.668275 0.305011
+v -0.003294 0.012520 0.003835
+vn -0.676204 -0.691800 0.253300
+v -0.002389 0.011135 0.002691
+vn -0.692754 -0.589814 0.414983
+v -0.005693 0.016062 0.005087
+vn -0.695012 -0.550159 0.462908
+v -0.004754 0.015580 0.005981
+vn -0.659781 -0.587353 0.468728
+v -0.003877 0.014453 0.005941
+vn -0.715313 -0.615718 0.330484
+v -0.005961 0.015571 0.003844
+vn -0.679623 -0.681364 0.271764
+v -0.007161 0.016425 0.002874
+vn -0.695903 -0.697818 0.169614
+v -0.007472 0.016199 0.001224
+vn -0.682603 -0.728677 -0.055536
+v -0.007913 0.016318 -0.002091
+vn -0.625332 -0.780291 -0.010299
+v -0.008621 0.016956 -0.001673
+vn -0.625321 -0.756079 -0.193179
+v -0.007629 0.016356 -0.003833
+vn -0.650625 -0.716695 -0.251069
+v -0.006162 0.014990 -0.003577
+vn -0.704990 -0.705649 0.071060
+v -0.007742 0.016188 -0.000452
+vn -0.721019 -0.692245 0.030469
+v -0.006829 0.015187 -0.000724
+vn -0.724453 -0.649482 0.230956
+v -0.006020 0.015007 0.002465
+vn -0.726173 -0.686020 0.045267
+v -0.005027 0.013335 -0.000200
+vn -0.725512 -0.674352 0.137410
+v -0.006271 0.014809 0.000820
+vn -0.332969 -0.935931 0.114740
+v -0.000698 0.009395 0.001395
+vn -0.715999 -0.689025 0.112207
+v -0.002754 0.011018 0.000865
+vn -0.713547 -0.688768 -0.128258
+v -0.005014 0.013441 -0.002141
+vn -0.009537 -0.985576 -0.168964
+v -0.000527 0.009510 -0.000530
+vn -0.401532 -0.911552 -0.088568
+v -0.001298 0.009669 -0.000560
+vn -0.142063 -0.952082 -0.270846
+v -0.000961 0.009797 -0.001637
+vn -0.497828 -0.837609 -0.224895
+v -0.001837 0.010197 -0.001944
+vn -0.693910 -0.706395 -0.139623
+v -0.002643 0.010874 -0.002007
+vn -0.697084 -0.710103 -0.099129
+v -0.006618 0.015035 -0.002025
+vn -0.075778 -0.994351 -0.074323
+v -0.000389 0.009387 0.000523
+vn -0.677104 -0.735879 -0.003662
+v -0.002181 0.010297 -0.000640
+vn -0.618178 -0.752809 0.226129
+v -0.001408 0.009943 0.001874
+vn -0.722372 -0.667058 0.182240
+v -0.004266 0.012930 0.002108
+vn -0.576505 -0.814692 0.062594
+v -0.001413 0.009724 0.000676
+vn -0.730344 -0.682427 -0.029843
+v -0.003461 0.011654 -0.001115
+vn -0.594355 -0.736238 -0.323568
+v -0.002434 0.011065 -0.003161
+vn -0.266250 -0.889652 -0.370985
+v -0.001247 0.010385 -0.003124
+vn 0.319031 -0.945878 -0.059446
+v 0.000857 0.009516 0.001006
+vn 0.184632 -0.957480 -0.221680
+v 0.000136 0.009678 -0.000998
+vn 0.366938 -0.906292 -0.209740
+v 0.001071 0.009994 -0.001255
+vn 0.075467 -0.951046 -0.299694
+v -0.000130 0.009954 -0.002191
+vn 0.682173 -0.730334 -0.035381
+v 0.002718 0.010717 0.000761
+vn 0.327531 -0.895711 -0.300708
+v 0.000831 0.010166 -0.002253
+vn 0.538915 -0.840427 -0.057040
+v 0.001798 0.009992 0.000827
+vn 0.256579 -0.955312 -0.146786
+v 0.000525 0.009551 0.000069
+vn 0.450615 -0.879727 -0.151746
+v 0.001452 0.009941 -0.000185
+vn 0.615633 -0.776601 -0.133741
+v 0.002377 0.010564 -0.000360
+vn 0.806577 -0.551028 -0.214013
+v 0.003891 0.012656 -0.002239
+vn 0.786132 -0.602783 -0.136566
+v 0.003741 0.012142 -0.001241
+vn 0.569224 -0.746475 -0.344614
+v 0.002405 0.011423 -0.003166
+vn 0.712645 -0.657787 -0.243829
+v 0.003212 0.011817 -0.002351
+vn 0.774578 -0.631448 -0.036091
+v 0.003573 0.011666 0.000577
+vn 0.432042 -0.833283 -0.344933
+v 0.001405 0.010682 -0.002901
+vn 0.542665 -0.809475 -0.224199
+v 0.001960 0.010518 -0.001493
+vn 0.625085 -0.748042 -0.222938
+v 0.002711 0.011201 -0.001871
+vn 0.835178 -0.547189 -0.055330
+v 0.004285 0.012687 0.000144
+vn 0.722026 -0.680867 -0.122878
+v 0.003193 0.011381 -0.000655
+vn 0.864541 -0.492329 -0.100897
+v 0.004643 0.013405 -0.000703
+vn 0.120671 -0.914696 -0.385708
+v 0.000016 0.010353 -0.003271
+vn 0.421364 -0.775813 -0.469646
+v 0.001430 0.011328 -0.004241
+vn 0.051881 -0.992870 0.107317
+v 0.000080 0.009342 0.001518
+vn 0.800304 -0.509922 -0.315426
+v 0.003875 0.013232 -0.003475
+vn 0.946099 -0.323393 -0.017730
+v 0.005892 0.015872 0.001616
+vn 0.896360 -0.442974 -0.017717
+v 0.005245 0.014312 0.001206
+vn 0.914589 -0.398761 -0.067205
+v 0.005447 0.014895 -0.000099
+vn 0.936793 -0.257565 -0.236808
+v 0.005661 0.016878 -0.002406
+vn 0.960143 -0.251460 -0.122040
+v 0.006376 0.018336 -0.000543
+vn 0.963266 -0.261225 -0.062292
+v 0.006445 0.018340 0.000215
+vn 0.957062 -0.287480 -0.037242
+v 0.006634 0.018837 0.001345
+vn 0.965736 -0.234945 -0.110246
+v 0.006039 0.016917 -0.000514
+vn 0.952779 -0.235116 -0.192178
+v 0.005840 0.016683 -0.001407
+vn 0.948613 -0.286290 -0.134799
+v 0.006748 0.019844 -0.000843
+vn 0.934533 -0.334679 -0.120986
+v 0.007703 0.022521 -0.000255
+vn 0.970035 -0.242963 -0.000555
+v 0.006220 0.017150 0.002167
+vn 0.962597 -0.267798 -0.041128
+v 0.006023 0.016458 0.000802
+vn 0.951578 -0.298362 -0.074028
+v 0.005799 0.015885 -0.000339
+vn 0.921597 -0.275574 -0.273344
+v 0.005879 0.019012 -0.003977
+vn 0.939372 -0.296709 -0.171882
+v 0.006848 0.020740 -0.001900
+vn 0.938958 -0.276112 -0.205233
+v 0.006235 0.019380 -0.002950
+vn 0.927670 -0.277250 -0.250123
+v 0.005679 0.017794 -0.003355
+vn 0.945885 -0.266240 -0.185522
+v 0.006213 0.018718 -0.002101
+vn 0.953052 -0.246394 -0.176020
+v 0.006154 0.018016 -0.001433
+vn 0.903516 -0.397644 -0.159808
+v 0.005256 0.014761 -0.001454
+vn 0.919044 -0.295793 -0.260507
+v 0.005304 0.015403 -0.002234
+vn 0.946369 -0.275117 -0.169399
+v 0.005644 0.015692 -0.001188
+vn 0.969314 -0.239851 -0.053861
+v 0.006199 0.017303 0.000394
+vn 0.931795 -0.362645 0.015716
+v 0.006958 0.019648 0.005600
+vn 0.917170 -0.397744 0.024458
+v 0.007455 0.020919 0.007928
+vn 0.928106 -0.370447 -0.037267
+v 0.008262 0.023048 0.004683
+vn 0.953895 -0.300069 -0.006474
+v 0.006598 0.018591 0.002823
+vn 0.969602 -0.243176 -0.027146
+v 0.006315 0.017654 0.001363
+vn 0.940238 -0.339135 -0.030647
+v 0.007332 0.020744 0.003289
+vn 0.948231 -0.309438 -0.071455
+v 0.007060 0.020316 0.000727
+vn 0.756750 -0.632745 0.164203
+v 0.003708 0.012208 0.003762
+vn 0.690849 -0.672641 0.265107
+v 0.003462 0.012516 0.005551
+vn 0.864010 -0.477470 0.159716
+v 0.004955 0.014208 0.004599
+vn 0.912794 -0.379856 0.150055
+v 0.005440 0.015193 0.004437
+vn 0.938824 -0.320592 0.125821
+v 0.005895 0.016381 0.004231
+vn 0.876244 -0.439357 0.197896
+v 0.005465 0.016905 0.007521
+vn 0.845764 -0.533076 0.022661
+v 0.004538 0.013041 0.001712
+vn 0.929086 -0.365776 0.054831
+v 0.005732 0.015481 0.002857
+vn 0.868171 -0.438897 0.231622
+v 0.004856 0.014833 0.006379
+vn 0.311001 -0.936080 0.164418
+v 0.000959 0.009566 0.001964
+vn 0.572784 -0.814738 0.090111
+v 0.001912 0.010069 0.001878
+vn 0.452874 -0.775334 0.440185
+v 0.002564 0.013621 0.009355
+vn 0.365036 -0.824845 0.431717
+v 0.002067 0.012452 0.007664
+vn 0.583314 -0.731820 0.352400
+v 0.002911 0.012781 0.007267
+vn 0.796606 -0.566738 0.210303
+v 0.004354 0.013541 0.005397
+vn 0.908339 -0.368525 0.197762
+v 0.005474 0.016048 0.005906
+vn 0.702510 -0.706208 0.088031
+v 0.002872 0.010916 0.002045
+vn 0.661420 -0.721255 0.205709
+v 0.002757 0.011110 0.003475
+vn 0.823652 -0.555928 0.111984
+v 0.004500 0.013210 0.003477
+vn 0.785032 -0.616195 0.063471
+v 0.003742 0.011920 0.002023
+vn 0.960631 -0.273966 0.046169
+v 0.006072 0.016555 0.002924
+vn 0.946316 -0.318207 0.056828
+v 0.006308 0.017705 0.004176
+vn 0.914349 -0.379023 0.142501
+v 0.005953 0.017244 0.005852
+vn 0.131807 -0.922407 0.363031
+v 0.000827 0.009872 0.003180
+vn 0.506321 -0.828376 0.239651
+v 0.001727 0.010163 0.002996
+vn 0.886000 -0.459039 0.065469
+v 0.005179 0.014267 0.003027
+vn 0.561815 -0.764135 0.316957
+v 0.002376 0.011261 0.004801
+vn 0.855384 -0.500882 0.132043
+v 0.006320 0.018936 0.009232
+vn 0.792905 -0.576436 0.197542
+v 0.005023 0.017681 0.011471
+vn 0.825091 -0.542718 0.157104
+v 0.006021 0.019235 0.012000
+vn 0.868484 -0.481029 0.119778
+v 0.007061 0.020736 0.011261
+vn 0.325740 -0.813926 0.481060
+v 0.002861 0.017081 0.014849
+vn 0.589669 -0.723471 0.358997
+v 0.003964 0.017908 0.015114
+vn 0.773659 -0.601492 0.199145
+v 0.005340 0.018798 0.013681
+vn 0.721341 -0.643561 0.255923
+v 0.004451 0.017620 0.013320
+vn 0.779993 -0.568872 0.260760
+v 0.004250 0.016558 0.011295
+vn 0.869208 -0.471351 0.149347
+v 0.005961 0.017879 0.007615
+vn 0.904675 -0.416681 0.089106
+v 0.006412 0.018504 0.006706
+vn 0.746650 -0.574230 0.335818
+v 0.003775 0.015511 0.010699
+vn 0.826278 -0.484314 0.287583
+v 0.004271 0.015047 0.008672
+vn 0.836620 -0.501163 0.221141
+v 0.004872 0.016702 0.009445
+vn 0.891414 -0.446705 0.076393
+v 0.006945 0.019898 0.008464
+vn 0.879888 -0.408069 0.243469
+v 0.004891 0.015848 0.007852
+vn 0.845124 -0.461848 0.269188
+v 0.004360 0.015860 0.009616
+vn 0.830554 -0.529322 0.173201
+v 0.005583 0.017830 0.009449
+vn 0.668258 -0.651891 0.358425
+v 0.003488 0.014342 0.009338
+vn 0.752053 -0.593299 0.287076
+v 0.004012 0.013923 0.007361
+vn 0.604870 -0.646125 0.465461
+v 0.005308 0.020964 0.018300
+vn 0.606826 -0.688583 0.397008
+v 0.004565 0.019272 0.016759
+vn 0.389675 -0.781798 0.486770
+v 0.003466 0.018318 0.016482
+vn 0.778869 -0.576684 0.246571
+v 0.006384 0.021280 0.016681
+vn 0.846619 -0.508839 0.155946
+v 0.007664 0.023221 0.016777
+vn 0.759303 -0.605128 0.239330
+v 0.005662 0.019836 0.015491
+vn 0.828817 -0.533149 0.169750
+v 0.006999 0.021950 0.016068
+vn 0.811704 -0.556527 0.177241
+v 0.006188 0.020293 0.014742
+vn 0.700540 -0.629330 0.336433
+v 0.005577 0.020383 0.016900
+vn 0.697105 -0.657945 0.284872
+v 0.004910 0.018868 0.015234
+vn 0.850321 -0.508052 0.137248
+v 0.006910 0.021188 0.013954
+vn -0.142385 -0.761610 0.632200
+v 0.002041 0.018433 0.016860
+vn 0.120567 -0.717344 0.686208
+v 0.003248 0.019361 0.017921
+vn -0.021280 -0.823875 0.566373
+v 0.002212 0.017445 0.015608
+vn 0.099502 -0.787151 0.608681
+v 0.002831 0.018488 0.017010
+vn -0.299322 -0.803789 0.514130
+v 0.000652 0.017679 0.015292
+vn 0.440343 -0.714974 0.543057
+v 0.004026 0.019493 0.017727
+vn -0.095476 -0.693629 0.713977
+v 0.002381 0.019187 0.017717
+vn -0.429803 -0.737325 0.521173
+v 0.000379 0.017019 0.014124
+vn -0.226760 -0.785117 0.576343
+v 0.001536 0.017612 0.015675
+vn 0.049428 -0.627793 0.776809
+v 0.003391 0.020010 0.018488
+vn -0.393676 -0.736181 0.550506
+v 0.000659 0.016373 0.013461
+vn -0.143143 -0.809043 0.570053
+v 0.001535 0.016071 0.013530
+vn 0.137794 -0.588508 0.796662
+v 0.004021 0.020991 0.019134
+vn 0.369916 -0.656468 0.657428
+v 0.004309 0.020482 0.018646
+vn 0.064107 -0.526846 0.847540
+v 0.002958 0.022209 0.019999
+vn 0.046467 -0.569244 0.820854
+v 0.003412 0.021362 0.019443
+vn 0.053006 -0.363033 0.930267
+v 0.004791 0.023658 0.020705
+vn 0.007124 -0.418329 0.908268
+v 0.003806 0.023113 0.020474
+vn 0.072508 -0.512336 0.855719
+v 0.004135 0.022560 0.020181
+vn 0.024538 -0.602796 0.797518
+v 0.002252 0.020831 0.019089
+vn 0.426969 -0.620678 0.657614
+v 0.005235 0.021831 0.019360
+vn 0.208115 -0.570557 0.794451
+v 0.004749 0.022210 0.019857
+vn 0.428462 -0.572410 0.699119
+v 0.005921 0.023101 0.020044
+vn 0.192564 -0.329381 0.924353
+v 0.005723 0.024177 0.020811
+vn 0.146426 -0.428440 0.891627
+v 0.003349 0.023584 0.020695
+vn 0.048866 -0.275311 0.960113
+v 0.004288 0.024118 0.020850
+vn 0.270931 0.111598 0.956108
+v 0.006248 0.025551 0.020805
+vn 0.092475 -0.102019 0.990475
+v 0.005447 0.024779 0.020945
+vn 0.180458 0.107685 0.977670
+v 0.005597 0.025846 0.020881
+vn 0.226096 -0.512229 0.828554
+v 0.005347 0.023332 0.020461
+vn 0.570858 0.019175 0.820825
+v 0.006705 0.025230 0.020632
+vn 0.320409 0.044472 0.946235
+v 0.004978 0.026208 0.020958
+vn 0.336007 -0.160946 0.928006
+v 0.006305 0.024783 0.020792
+vn 0.152564 -0.093337 0.983876
+v 0.004755 0.025184 0.021021
+vn 0.271474 0.096352 0.957611
+v 0.005537 0.026912 0.020704
+vn 0.222393 -0.055304 0.973387
+v 0.005375 0.028188 0.020709
+vn 0.045685 -0.183688 0.981922
+v 0.005932 0.030200 0.021106
+vn 0.014504 -0.184071 0.982806
+v 0.005618 0.029164 0.020846
+vn 0.264613 -0.033553 0.963771
+v 0.005803 0.027787 0.020585
+vn 0.246375 -0.216663 0.944646
+v 0.005955 0.028695 0.020682
+vn 0.404904 -0.000392 0.914359
+v 0.006138 0.027598 0.020449
+vn -0.095371 0.051694 0.994099
+v 0.006074 0.031971 0.021283
+vn 0.105747 -0.019760 0.994197
+v 0.004912 0.030281 0.020949
+vn 0.337403 -0.021833 0.941107
+v 0.004946 0.028764 0.020868
+vn 0.585427 -0.362379 0.725229
+v 0.007122 0.029193 0.020250
+vn 0.613721 -0.211823 0.760577
+v 0.006764 0.028316 0.020129
+vn -0.060876 -0.104649 0.992644
+v 0.005286 0.029723 0.020916
+vn -0.247974 -0.065348 0.966560
+v 0.005611 0.030811 0.021150
+vn 0.439064 -0.152654 0.885392
+v 0.006281 0.028180 0.020440
+vn 0.300357 -0.261552 0.917265
+v 0.006269 0.029596 0.020865
+vn 0.475647 -0.265595 0.838582
+v 0.006517 0.028887 0.020498
+vn 0.328755 -0.230177 0.915936
+v 0.006876 0.030560 0.020948
+vn 0.443780 -0.340164 0.829064
+v 0.006825 0.029401 0.020537
+vn 0.252822 0.244085 0.936218
+v 0.008174 0.034574 0.020740
+vn 0.750927 -0.380444 0.539788
+v 0.008348 0.030433 0.019724
+vn 0.232596 -0.025050 0.972251
+v 0.007291 0.032533 0.021147
+vn 0.098164 0.213856 0.971920
+v 0.007604 0.034109 0.020947
+vn 0.454976 -0.312192 0.833986
+v 0.007331 0.030439 0.020725
+vn 0.171657 -0.097873 0.980283
+v 0.006491 0.031331 0.021237
+vn 0.720354 -0.289344 0.630373
+v 0.008485 0.031678 0.020295
+vn 0.716274 -0.370684 0.591223
+v 0.007598 0.029163 0.019775
+vn 0.576082 -0.257436 0.775794
+v 0.007967 0.031460 0.020713
+vn 0.653151 -0.371637 0.659757
+v 0.007754 0.030336 0.020362
+vn 0.382003 -0.134678 0.914295
+v 0.007717 0.031948 0.020945
+vn -0.000739 0.180619 0.983553
+v 0.006778 0.033351 0.021133
+vn -0.063495 0.492241 0.868140
+v 0.007224 0.034848 0.020622
+vn -0.202266 0.293968 0.934169
+v 0.006216 0.033704 0.020968
+vn -0.126152 0.445618 0.886290
+v 0.006677 0.034638 0.020675
+vn 0.020341 0.672007 0.740265
+v 0.006925 0.035587 0.020007
+vn 0.143078 0.396757 0.906704
+v 0.005846 0.035251 0.020208
+vn -0.197653 0.328855 0.923465
+v 0.005829 0.034104 0.020701
+vn -0.320218 0.152658 0.934963
+v 0.005726 0.032562 0.021110
+vn -0.116163 0.510261 0.852138
+v 0.006328 0.035078 0.020325
+vn 0.276883 0.570961 0.772877
+v 0.006471 0.036202 0.019507
+vn 0.872220 -0.335452 0.355956
+v 0.009029 0.029059 0.017125
+vn 0.919980 -0.031874 0.390666
+v 0.008406 0.027256 0.017722
+vn 0.891522 -0.175756 0.417491
+v 0.008492 0.028097 0.017675
+vn 0.845836 -0.294645 0.444687
+v 0.008339 0.028620 0.018245
+vn 0.922366 -0.221026 0.316842
+v 0.008950 0.028101 0.016588
+vn 0.790932 -0.386311 0.474542
+v 0.008265 0.029369 0.018977
+vn 0.807864 -0.387306 0.444241
+v 0.009067 0.030656 0.018757
+vn 0.878477 -0.381361 0.287823
+v 0.009748 0.030464 0.016892
+vn 0.798691 -0.234015 0.554373
+v 0.007674 0.028245 0.019151
+vn 0.966641 -0.229037 0.114659
+v 0.009085 0.025662 0.014252
+vn 0.972762 -0.148828 0.177718
+v 0.009090 0.026415 0.015040
+vn 0.967385 -0.196372 0.160012
+v 0.008852 0.025578 0.015820
+vn 0.864263 -0.488847 0.118650
+v 0.007656 0.022817 0.015275
+vn 0.894403 -0.440368 0.078221
+v 0.008414 0.023984 0.014150
+vn 0.882883 -0.454269 0.118986
+v 0.008215 0.024014 0.016112
+vn 0.924044 -0.368141 0.103026
+v 0.008751 0.024874 0.015025
+vn 0.791533 0.021853 0.610735
+v 0.007458 0.027256 0.019304
+vn 0.858288 -0.263559 0.440316
+v 0.007671 0.024660 0.019268
+vn 0.972237 -0.061057 0.225891
+v 0.008780 0.026155 0.016534
+vn 0.957637 -0.097907 0.270824
+v 0.008841 0.026990 0.016423
+vn 0.860931 -0.057914 0.505415
+v 0.007976 0.027616 0.018553
+vn 0.641045 -0.002917 0.767498
+v 0.006621 0.027589 0.020158
+vn 0.551532 0.132407 0.823578
+v 0.006493 0.026988 0.020326
+vn 0.928832 -0.329480 0.169453
+v 0.008499 0.024865 0.016829
+vn 0.865818 0.077044 0.494391
+v 0.007935 0.026646 0.018745
+vn 0.955730 -0.113840 0.271330
+v 0.008515 0.025558 0.017463
+vn 0.580519 0.204119 0.788247
+v 0.006676 0.025954 0.020490
+vn 0.912414 -0.274439 0.303618
+v 0.008141 0.024822 0.018269
+vn 0.810464 -0.496047 0.311584
+v 0.007572 0.023854 0.018588
+vn 0.936814 0.017243 0.349402
+v 0.008426 0.026326 0.017757
+vn 0.895778 0.000070 0.444502
+v 0.007990 0.025612 0.018836
+vn 0.726329 0.056446 0.685026
+v 0.007010 0.027222 0.019806
+vn 0.866439 -0.456609 0.201970
+v 0.007990 0.024056 0.017575
+vn 0.338504 0.167382 0.925958
+v 0.006142 0.026620 0.020590
+vn 0.793823 0.128987 0.594312
+v 0.007467 0.026298 0.019506
+vn 0.722638 0.164162 0.671450
+v 0.006981 0.026301 0.020069
+vn 0.708572 -0.205636 0.675011
+v 0.007146 0.028120 0.019741
+vn 0.616399 -0.606146 0.502633
+v 0.006291 0.022735 0.019362
+vn 0.616930 -0.506485 0.602386
+v 0.006736 0.023857 0.020021
+vn 0.437488 -0.456761 0.774580
+v 0.006267 0.024049 0.020521
+vn 0.634523 -0.283625 0.718984
+v 0.006806 0.024625 0.020425
+vn 0.739750 -0.521204 0.425578
+v 0.007127 0.023731 0.019370
+vn 0.723248 -0.595111 0.350364
+v 0.006581 0.022335 0.018291
+vn 0.759850 0.046167 0.648457
+v 0.007033 0.025271 0.020278
+vn 0.800998 -0.548299 0.240353
+v 0.007260 0.022883 0.017575
+vn 0.776491 -0.277334 0.565815
+v 0.007218 0.024554 0.019932
+vn 0.818109 0.037505 0.573839
+v 0.007440 0.025362 0.019751
+vn 0.947404 -0.318829 0.027828
+v 0.009732 0.026629 0.008507
+vn 0.901081 -0.427108 0.075043
+v 0.007524 0.021273 0.009733
+vn 0.909050 -0.410923 0.069067
+v 0.008056 0.022651 0.010879
+vn 0.922643 -0.382744 0.047291
+v 0.008834 0.024518 0.011551
+vn 0.886872 -0.451871 0.096288
+v 0.007809 0.022604 0.013114
+vn 0.921977 -0.386523 0.023633
+v 0.008056 0.022392 0.008787
+vn 0.954047 -0.293478 0.060544
+v 0.009527 0.026376 0.011376
+vn 0.967094 -0.221914 0.124432
+v 0.009367 0.026487 0.013281
+vn 0.937300 -0.342590 0.064042
+v 0.009056 0.025242 0.013136
+vn 0.922677 -0.384750 -0.025188
+v 0.008964 0.024623 0.005813
+vn 0.924300 -0.381480 -0.011910
+v 0.008428 0.023275 0.007135
+vn 0.928950 -0.369987 -0.012723
+v 0.007764 0.021677 0.006349
+vn 0.927942 -0.372193 0.019905
+v 0.008877 0.024360 0.008699
+vn 0.954056 -0.274001 0.121244
+v 0.009828 0.027975 0.012529
+vn -0.472536 -0.758983 -0.447945
+v -0.006484 0.017451 -0.008757
+vn -0.401154 -0.572484 -0.715079
+v -0.003592 0.020837 -0.014307
+vn -0.434837 -0.610076 -0.662362
+v -0.003472 0.019084 -0.012880
+vn -0.347422 -0.788411 -0.507648
+v -0.007150 0.018623 -0.010003
+vn -0.340835 -0.713614 -0.612035
+v -0.005711 0.019211 -0.011616
+vn -0.437082 -0.660799 -0.610168
+v -0.004237 0.017969 -0.011251
+vn -0.210840 -0.835631 -0.507215
+v -0.002424 0.015040 -0.008349
+vn -0.121360 -0.831128 -0.542678
+v -0.002518 0.015847 -0.009714
+vn -0.448771 -0.799851 -0.398552
+v -0.004199 0.015493 -0.007870
+vn 0.211553 -0.741708 -0.636486
+v -0.000966 0.018120 -0.012591
+vn 0.106192 -0.800802 -0.589440
+v -0.001653 0.016598 -0.010806
+vn -0.415898 -0.659220 -0.626465
+v -0.003204 0.017583 -0.011633
+vn -0.390267 -0.732560 -0.557717
+v -0.003287 0.016501 -0.010312
+vn -0.175847 -0.767206 -0.616825
+v -0.002566 0.016882 -0.011173
+vn -0.030561 -0.748173 -0.662799
+v -0.001924 0.017896 -0.012483
+vn -0.333121 -0.661930 -0.671476
+v -0.002622 0.018331 -0.012719
+vn 0.031787 -0.817607 -0.574898
+v -0.001658 0.015519 -0.009211
+vn -0.490767 -0.753397 -0.437653
+v -0.005298 0.016429 -0.008362
+vn -0.435832 -0.711794 -0.550817
+v -0.005452 0.017581 -0.009884
+vn -0.452356 -0.727313 -0.516129
+v -0.004161 0.016373 -0.009413
+vn -0.208886 -0.668098 -0.714151
+v -0.002059 0.019510 -0.014098
+vn 0.540935 0.818764 -0.192391
+v 0.004692 0.043399 0.001466
+vn 0.508152 0.838531 -0.196587
+v 0.004292 0.042887 -0.001880
+vn 0.538718 0.823364 -0.178479
+v 0.006134 0.042433 0.001428
+vn 0.554028 0.816708 -0.161372
+v 0.007293 0.041546 0.000774
+vn 0.498000 0.845250 -0.193777
+v 0.002984 0.044371 0.000892
+vn -0.581767 0.376408 -0.721016
+v -0.000679 0.029056 -0.017667
+vn 0.077584 0.020183 -0.996782
+v 0.000368 0.027226 -0.018344
+vn -0.342771 0.274329 -0.898472
+v -0.000382 0.028565 -0.018027
+vn -0.388045 -0.305447 -0.869553
+v -0.003143 0.024322 -0.016531
+vn -0.100490 -0.013746 -0.994843
+v -0.000669 0.026443 -0.018338
+vn -0.659525 0.366012 -0.656553
+v -0.001739 0.027841 -0.017333
+vn -0.121701 0.161568 -0.979329
+v -0.000074 0.027841 -0.018265
+vn -0.365664 0.189322 -0.911289
+v -0.001090 0.027010 -0.018181
+vn -0.353057 -0.068323 -0.933104
+v -0.001448 0.025996 -0.018155
+vn -0.636200 0.072031 -0.768154
+v -0.001902 0.026181 -0.017848
+vn -0.597654 0.295910 -0.745149
+v -0.001430 0.027484 -0.017830
+vn -0.707879 0.450077 -0.544369
+v -0.001000 0.029183 -0.017217
+vn 0.138506 -0.218975 -0.965850
+v 0.000037 0.026089 -0.018289
+vn -0.395386 -0.316054 -0.862427
+v -0.001756 0.025193 -0.017812
+vn -0.043877 -0.338128 -0.940077
+v -0.000939 0.025472 -0.018138
+vn 0.074557 0.132205 -0.988414
+v 0.001004 0.028781 -0.018148
+vn 0.332758 -0.111702 -0.936373
+v 0.001117 0.027066 -0.018178
+vn 0.329491 0.051865 -0.942733
+v 0.002108 0.029534 -0.017848
+vn 0.268497 0.018613 -0.963101
+v 0.001346 0.028106 -0.018158
+vn 0.731179 -0.159224 -0.663344
+v 0.004450 0.029923 -0.016276
+vn 0.645936 -0.155793 -0.747325
+v 0.003300 0.028559 -0.017095
+vn 0.688661 -0.009616 -0.725019
+v 0.004470 0.031278 -0.016425
+vn 0.810518 -0.064212 -0.582183
+v 0.005195 0.031066 -0.015596
+vn 0.844923 -0.212589 -0.490826
+v 0.005312 0.030188 -0.015132
+vn 0.885055 -0.129975 -0.446972
+v 0.006138 0.030929 -0.013793
+vn 0.526375 -0.188047 -0.829197
+v 0.002062 0.027217 -0.017736
+vn 0.477404 -0.065126 -0.876267
+v 0.002267 0.028388 -0.017779
+vn 0.690696 -0.348465 -0.633649
+v 0.002639 0.026413 -0.016914
+vn 0.786898 -0.344333 -0.512081
+v 0.003566 0.026799 -0.015924
+vn 0.842936 -0.346517 -0.411564
+v 0.005480 0.027201 -0.012710
+vn 0.855662 -0.307992 -0.415914
+v 0.005890 0.028766 -0.013184
+vn 0.836255 -0.290121 -0.465304
+v 0.005183 0.028911 -0.014625
+vn 0.780786 -0.285019 -0.556001
+v 0.004022 0.028329 -0.016196
+vn 0.671502 -0.274615 -0.688238
+v 0.002832 0.027324 -0.017173
+vn 0.864220 -0.321769 -0.386767
+v 0.006738 0.027241 -0.010072
+vn 0.879977 -0.279396 -0.384160
+v 0.006847 0.028977 -0.011243
+vn 0.386629 -0.297773 -0.872840
+v 0.000953 0.026203 -0.018071
+vn 0.526319 -0.356672 -0.771863
+v 0.001675 0.026158 -0.017626
+vn 0.878318 -0.200217 -0.434133
+v 0.006311 0.030112 -0.013167
+vn 0.821919 -0.351162 -0.448480
+v 0.003987 0.025457 -0.014130
+vn 0.781996 -0.368623 -0.502593
+v 0.003026 0.025248 -0.015640
+vn 0.489373 -0.390621 -0.779698
+v 0.001342 0.025071 -0.017314
+vn 0.679100 -0.377768 -0.629377
+v 0.002232 0.025084 -0.016597
+vn 0.485260 -0.432929 -0.759668
+v 0.001154 0.023885 -0.016809
+vn 0.821581 -0.351928 -0.448499
+v 0.004450 0.027057 -0.014594
+vn 0.259975 -0.414195 -0.872270
+v 0.000230 0.025260 -0.017905
+vn 0.912190 -0.083349 -0.401200
+v 0.007537 0.031935 -0.010991
+vn 0.923738 -0.145928 -0.354136
+v 0.009096 0.031550 -0.007049
+vn 0.913922 -0.160082 -0.372989
+v 0.007853 0.030883 -0.009927
+vn 0.901774 -0.235419 -0.362465
+v 0.008010 0.029416 -0.008768
+vn 0.933950 -0.197118 -0.298130
+v 0.009956 0.031049 -0.004399
+vn 0.928082 -0.097278 -0.359445
+v 0.009576 0.033890 -0.006561
+vn 0.938430 -0.125194 -0.321988
+v 0.010267 0.032597 -0.004288
+vn 0.901599 -0.072544 -0.426447
+v 0.006881 0.031637 -0.012392
+vn 0.894859 -0.043368 -0.444237
+v 0.007352 0.033662 -0.011579
+vn 0.900025 -0.190095 -0.392198
+v 0.006943 0.030345 -0.011869
+vn 0.906713 -0.084999 -0.413094
+v 0.008392 0.034908 -0.009610
+vn 0.916806 -0.091325 -0.388750
+v 0.008365 0.032959 -0.009276
+vn 0.900376 -0.053983 -0.431751
+v 0.008306 0.036287 -0.010096
+vn 0.737770 0.505464 -0.447439
+v 0.007158 0.037541 -0.011829
+vn 0.802447 0.196529 -0.563432
+v 0.006549 0.036924 -0.013205
+vn 0.523631 0.567149 -0.635730
+v 0.005730 0.037054 -0.014032
+vn 0.785487 0.076229 -0.614166
+v 0.005135 0.032338 -0.015601
+vn 0.551393 -0.045140 -0.833024
+v 0.003389 0.030090 -0.017205
+vn 0.053350 0.591827 -0.804297
+v 0.002812 0.036079 -0.016195
+vn 0.213709 0.367598 -0.905097
+v 0.002891 0.035321 -0.016602
+vn 0.649144 0.126847 -0.750014
+v 0.004267 0.032802 -0.016470
+vn 0.538876 0.050367 -0.840878
+v 0.003703 0.031692 -0.017041
+vn 0.729398 0.128403 -0.671930
+v 0.004828 0.033902 -0.015686
+vn 0.837315 -0.012233 -0.546584
+v 0.006475 0.036132 -0.013344
+vn 0.709045 0.125530 -0.693900
+v 0.004941 0.035666 -0.015262
+vn 0.885409 -0.001505 -0.464811
+v 0.006436 0.032741 -0.013363
+vn 0.836271 0.066726 -0.544241
+v 0.005683 0.033400 -0.014628
+vn 0.787108 0.077298 -0.611953
+v 0.005316 0.034825 -0.014937
+vn 0.403220 0.335958 -0.851203
+v 0.003702 0.035722 -0.016160
+vn 0.536624 0.314799 -0.782903
+v 0.004372 0.035967 -0.015680
+vn 0.526826 0.202847 -0.825414
+v 0.003788 0.034451 -0.016469
+vn 0.646841 0.152098 -0.747304
+v 0.004514 0.035224 -0.015740
+vn 0.782836 0.036814 -0.621139
+v 0.005588 0.035708 -0.014528
+vn 0.873045 -0.017335 -0.487331
+v 0.005790 0.031778 -0.014624
+vn 0.417260 0.162531 -0.894135
+v 0.003307 0.032944 -0.017084
+vn 0.877636 -0.056011 -0.476045
+v 0.007356 0.035634 -0.011805
+vn 0.855065 -0.001246 -0.518519
+v 0.006302 0.034686 -0.013526
+vn 0.718529 0.178359 -0.672238
+v 0.005685 0.036475 -0.014352
+vn 0.262987 0.613975 -0.744226
+v 0.003798 0.036410 -0.015709
+vn 0.410481 0.585804 -0.698812
+v 0.004753 0.036748 -0.014972
+vn 0.610154 0.298312 -0.733976
+v 0.004864 0.036211 -0.015186
+vn -0.755999 0.037803 -0.653481
+v -0.008740 0.024923 -0.014648
+vn -0.863334 0.082241 -0.497887
+v -0.009582 0.025054 -0.013421
+vn -0.788131 0.206501 -0.579833
+v -0.008929 0.026131 -0.014154
+vn -0.396394 -0.233730 -0.887830
+v -0.006984 0.023491 -0.015800
+vn -0.199762 -0.364700 -0.909444
+v -0.006146 0.023352 -0.016039
+vn -0.490871 -0.156689 -0.857026
+v -0.007030 0.024344 -0.015979
+vn -0.280402 -0.299319 -0.912021
+v -0.006068 0.024225 -0.016389
+vn -0.507219 -0.054041 -0.860121
+v -0.006777 0.025151 -0.016265
+vn -0.583577 0.188900 -0.789782
+v -0.007128 0.026164 -0.015941
+vn -0.274127 -0.147797 -0.950269
+v -0.005733 0.024980 -0.016698
+vn -0.387015 0.290783 -0.875023
+v -0.006397 0.026457 -0.016292
+vn -0.366289 0.106347 -0.924404
+v -0.006159 0.025741 -0.016556
+vn -0.680311 0.276752 -0.678664
+v -0.007916 0.027052 -0.014942
+vn -0.690388 -0.152040 -0.707282
+v -0.008387 0.023837 -0.014913
+vn -0.553818 -0.204659 -0.807094
+v -0.007632 0.023535 -0.015458
+vn -0.664127 0.097346 -0.741255
+v -0.007899 0.025498 -0.015412
+vn -0.603865 -0.071062 -0.793913
+v -0.007868 0.024351 -0.015429
+vn -0.520673 -0.519938 -0.677174
+v -0.008124 0.021912 -0.014306
+vn -0.230569 -0.764971 -0.601380
+v -0.007403 0.021270 -0.014089
+vn -0.184261 -0.838284 -0.513154
+v -0.007966 0.020329 -0.012453
+vn -0.425832 -0.692544 -0.582280
+v -0.008676 0.020867 -0.012896
+vn -0.157320 -0.508797 -0.846390
+v -0.006345 0.022601 -0.015679
+vn -0.028871 -0.765062 -0.643310
+v -0.006248 0.021780 -0.014974
+vn -0.885129 -0.126035 -0.447953
+v -0.010441 0.022783 -0.011703
+vn -0.824113 -0.324700 -0.464120
+v -0.010224 0.021430 -0.011454
+vn -0.699238 -0.403446 -0.590168
+v -0.009220 0.021675 -0.013106
+vn -0.807075 -0.086320 -0.584105
+v -0.009208 0.023939 -0.013998
+vn -0.883366 -0.024296 -0.468055
+v -0.010002 0.023869 -0.012694
+vn -0.702558 -0.277157 -0.655436
+v -0.008738 0.022842 -0.014261
+vn -0.571437 -0.351521 -0.741548
+v -0.007798 0.022880 -0.015108
+vn -0.411285 -0.373305 -0.831557
+v -0.007065 0.022899 -0.015590
+vn -0.338207 -0.592605 -0.731051
+v -0.007041 0.022161 -0.015159
+vn -0.817588 -0.204068 -0.538430
+v -0.009644 0.022732 -0.013071
+vn -0.927667 0.024655 -0.372595
+v -0.010756 0.023794 -0.011047
+vn -0.939648 -0.034690 -0.340380
+v -0.011021 0.022969 -0.010418
+vn -0.931455 0.125693 -0.341456
+v -0.010865 0.025701 -0.010429
+vn -0.913663 0.076604 -0.399189
+v -0.010426 0.024864 -0.011718
+vn -0.909870 -0.195494 -0.365950
+v -0.010959 0.021969 -0.010284
+vn -0.951541 0.055320 -0.302506
+v -0.011161 0.024654 -0.009798
+vn -0.983054 0.074064 -0.167690
+v -0.011621 0.024619 -0.007955
+vn -0.947438 0.225746 -0.226716
+v -0.011056 0.026702 -0.009224
+vn -0.913198 0.278343 -0.297649
+v -0.010355 0.028268 -0.010138
+vn -0.912419 0.294200 -0.284497
+v -0.009864 0.030509 -0.009430
+vn -0.881347 0.297963 -0.366669
+v -0.009469 0.030029 -0.010943
+vn -0.948833 0.257375 -0.182959
+v -0.010805 0.028577 -0.008050
+vn -0.969169 0.226967 -0.095908
+v -0.011314 0.027995 -0.005441
+vn -0.962937 0.261608 -0.065680
+v -0.011015 0.029742 -0.003702
+vn -0.909348 0.217537 -0.354633
+v -0.010521 0.026805 -0.010850
+vn -0.865839 0.267059 -0.423088
+v -0.009780 0.027962 -0.011784
+vn -0.953978 0.273256 -0.123522
+v -0.010615 0.030303 -0.006256
+vn -0.967018 0.132394 -0.217595
+v -0.011388 0.025506 -0.008785
+vn -0.974655 0.188411 -0.120622
+v -0.011532 0.026189 -0.007085
+vn -0.933860 0.291149 -0.207698
+v -0.010121 0.030954 -0.007994
+vn -0.943865 0.294652 -0.149327
+v -0.009257 0.034826 -0.006011
+vn -0.953413 0.292785 -0.072668
+v -0.009463 0.034897 -0.004088
+vn -0.939450 0.298330 -0.168623
+v -0.009743 0.032739 -0.006998
+vn -0.954454 0.285532 -0.086535
+v -0.010178 0.032385 -0.004548
+vn -0.922841 0.302523 -0.238418
+v -0.008797 0.034997 -0.007940
+vn -0.928964 0.311237 -0.200392
+v -0.008414 0.036871 -0.007002
+vn -0.923521 0.357114 -0.139924
+v -0.008080 0.038433 -0.005817
+vn -0.941737 0.314102 -0.120295
+v -0.008796 0.036708 -0.005070
+vn -0.905712 0.300632 -0.298841
+v -0.007833 0.037021 -0.008944
+vn -0.899058 0.324209 -0.294252
+v -0.007206 0.038768 -0.009089
+vn -0.914928 0.341740 -0.214757
+v -0.007643 0.038585 -0.007756
+vn -0.789373 0.282117 -0.545253
+v -0.006545 0.037392 -0.011355
+vn -0.861434 0.297503 -0.411612
+v -0.007011 0.038177 -0.010075
+vn -0.708721 0.243158 -0.662260
+v -0.005971 0.035902 -0.012681
+vn -0.738800 0.256181 -0.623334
+v -0.005483 0.037943 -0.012498
+vn -0.676644 0.238650 -0.696562
+v -0.004283 0.038421 -0.013607
+vn -0.678256 0.199990 -0.707087
+v -0.004844 0.036769 -0.013566
+vn -0.609269 0.166992 -0.775180
+v -0.003689 0.036602 -0.014608
+vn -0.547091 0.218434 -0.808071
+v -0.002976 0.037864 -0.014866
+vn -0.372506 0.375718 -0.848573
+v -0.002486 0.038952 -0.014774
+vn -0.285806 0.225827 -0.931299
+v -0.001940 0.037514 -0.015449
+vn -0.749432 0.320803 -0.579169
+v -0.005157 0.039791 -0.012045
+vn -0.782773 0.327588 -0.529105
+v -0.005848 0.039513 -0.011243
+vn -0.738362 0.671841 -0.058739
+v -0.006376 0.041643 -0.005348
+vn -0.178606 0.674514 -0.716332
+v -0.002365 0.040816 -0.013609
+vn -0.789366 0.476506 -0.387096
+v -0.005975 0.040676 -0.010063
+vn -0.668295 0.702459 -0.244813
+v -0.005567 0.041677 -0.009091
+vn -0.379156 0.866402 -0.324943
+v -0.004462 0.041821 -0.010773
+vn -0.504250 0.805600 -0.311031
+v -0.004823 0.041686 -0.010700
+vn -0.829537 0.338248 -0.444361
+v -0.006359 0.039534 -0.010363
+vn -0.864953 0.367283 -0.341992
+v -0.006713 0.039599 -0.009560
+vn -0.836137 0.473404 -0.277063
+v -0.006366 0.040634 -0.009144
+vn -0.384252 0.547888 -0.743081
+v -0.003125 0.040385 -0.013648
+vn -0.583009 0.325422 -0.744447
+v -0.003570 0.039517 -0.013874
+vn -0.351727 0.734946 -0.579777
+v -0.003321 0.041325 -0.012608
+vn -0.453219 0.749039 -0.483253
+v -0.004498 0.041383 -0.011700
+vn -0.371995 0.751612 -0.544701
+v -0.003977 0.041289 -0.012212
+vn -0.362299 0.851063 -0.380041
+v -0.003908 0.041888 -0.011179
+vn -0.757863 0.478446 -0.443546
+v -0.005158 0.041051 -0.011088
+vn -0.607411 0.746915 -0.270500
+v -0.005158 0.041720 -0.009950
+vn -0.714586 0.508378 -0.480540
+v -0.004766 0.041213 -0.011574
+vn -0.084844 0.397654 -0.913604
+v -0.001553 0.038592 -0.015162
+vn -0.758507 0.498779 -0.419388
+v -0.005607 0.040892 -0.010465
+vn -0.715218 0.328367 -0.616959
+v -0.004522 0.040244 -0.012606
+vn -0.554052 0.506297 -0.660825
+v -0.004114 0.040752 -0.012722
+vn -0.414029 0.893710 -0.172809
+v -0.004840 0.042156 -0.008965
+vn -0.096717 0.967390 -0.234099
+v -0.002268 0.042527 -0.010476
+vn 0.017452 0.984171 -0.176361
+v -0.001501 0.042669 -0.009894
+vn -0.366232 0.904729 -0.217577
+v -0.004183 0.042326 -0.009562
+vn -0.295905 0.951576 -0.083321
+v -0.004266 0.042575 -0.007709
+vn -0.214190 0.971650 -0.100092
+v -0.003285 0.042790 -0.007682
+vn -0.129644 0.976940 -0.169647
+v -0.002964 0.042639 -0.009517
+vn -0.262782 0.926928 -0.267863
+v -0.003553 0.042367 -0.010232
+vn -0.229836 0.963664 -0.136116
+v -0.003655 0.042641 -0.008849
+vn -0.457323 0.874442 -0.161883
+v -0.003410 0.042981 -0.005828
+vn 0.228521 0.946104 -0.229491
+v -0.000659 0.042638 -0.009468
+vn -0.087042 0.984171 -0.154372
+v -0.002191 0.042779 -0.008998
+vn 0.029412 0.973755 -0.225689
+v -0.001168 0.042858 -0.008657
+vn 0.245820 0.905988 -0.344614
+v 0.000140 0.042796 -0.008226
+vn -0.614958 -0.783942 0.085215
+v -0.008597 0.016977 -0.000175
+vn -0.831891 -0.519553 0.194995
+v -0.010386 0.018419 -0.000663
+vn -0.942832 -0.309170 0.124428
+v -0.011188 0.019625 -0.002096
+vn -0.490617 -0.871358 -0.005509
+v -0.009312 0.017382 -0.001761
+vn -0.834673 -0.390307 0.388562
+v -0.009531 0.019087 0.002772
+vn -0.918618 -0.337480 0.205543
+v -0.010920 0.019649 -0.000494
+vn -0.659641 -0.751566 0.004708
+v -0.010495 0.018035 -0.003540
+vn -0.417588 -0.906320 -0.064835
+v -0.009791 0.017666 -0.003368
+vn -0.627549 -0.765950 0.139654
+v -0.009444 0.017533 -0.000148
+vn -0.382376 -0.907504 -0.173851
+v -0.009465 0.017792 -0.005177
+vn -0.639338 -0.764817 0.079381
+v -0.010103 0.017806 -0.001900
+vn -0.426925 -0.873645 -0.233408
+v -0.010094 0.018169 -0.005917
+vn -0.534071 -0.836795 -0.120594
+v -0.008862 0.017254 -0.003515
+vn -0.725435 -0.650797 -0.224068
+v -0.010738 0.018688 -0.006105
+vn -0.871004 -0.489947 -0.036106
+v -0.011014 0.018696 -0.004511
+vn -0.871562 -0.479118 0.104047
+v -0.010854 0.018565 -0.002729
+vn -0.981742 -0.150806 0.115935
+v -0.011969 0.023749 -0.000298
+vn -0.834267 -0.292575 0.467330
+v -0.009703 0.021137 0.004008
+vn -0.919908 -0.156124 0.359715
+v -0.011438 0.024294 0.002420
+vn -0.942422 -0.222943 0.249273
+v -0.011608 0.023381 0.001242
+vn -0.869592 -0.393018 0.298908
+v -0.010327 0.019219 0.000963
+vn -0.922194 -0.296351 0.248463
+v -0.011028 0.020794 0.000512
+vn -0.957814 -0.251966 0.138226
+v -0.011450 0.021039 -0.001174
+vn -0.982088 -0.053118 0.180782
+v -0.011900 0.024651 0.000924
+vn -0.952329 -0.249418 0.175673
+v -0.011602 0.022396 0.000051
+vn -0.978092 -0.193223 0.077470
+v -0.011885 0.022629 -0.001680
+vn -0.879593 -0.272956 0.389630
+v -0.010911 0.022561 0.002584
+vn -0.916311 -0.286249 0.280063
+v -0.011178 0.022042 0.001387
+vn -0.874738 -0.321655 0.362451
+v -0.010465 0.020703 0.002113
+vn -0.993602 -0.112925 -0.001836
+v -0.011983 0.022845 -0.003602
+vn -0.997957 0.036180 -0.052665
+v -0.012032 0.024212 -0.003876
+vn -0.994455 0.099694 -0.033466
+v -0.012057 0.024995 -0.002387
+vn -0.977203 -0.205923 0.051666
+v -0.011695 0.021173 -0.003406
+vn -0.990907 -0.131560 -0.028209
+v -0.011833 0.021828 -0.005476
+vn -0.970060 -0.054012 -0.236782
+v -0.011430 0.022585 -0.009007
+vn -0.991999 0.087529 -0.090980
+v -0.011869 0.024529 -0.005962
+vn -0.978481 -0.172982 -0.112481
+v -0.011656 0.021243 -0.007075
+vn -0.997188 -0.029253 -0.069000
+v -0.011935 0.023121 -0.005571
+vn -0.973012 -0.228361 -0.033139
+v -0.011619 0.020605 -0.005697
+vn -0.988860 0.007159 -0.148674
+v -0.011740 0.023401 -0.007477
+vn -0.989169 -0.069023 -0.129540
+v -0.011730 0.022204 -0.007287
+vn -0.965992 0.027896 -0.257064
+v -0.011359 0.023656 -0.009279
+vn -0.997866 -0.063819 0.013829
+v -0.012087 0.023941 -0.002003
+vn -0.987910 0.131702 0.081784
+v -0.011879 0.025990 0.000471
+vn -0.979002 0.202608 0.022486
+v -0.011617 0.027617 -0.000304
+vn -0.976696 0.209756 -0.045478
+v -0.011551 0.027739 -0.002723
+vn -0.985094 0.171510 -0.013231
+v -0.011903 0.026132 -0.001546
+vn -0.984208 0.164052 -0.066493
+v -0.011825 0.025921 -0.004150
+vn -0.969162 0.245972 -0.014946
+v -0.011276 0.029141 -0.001401
+vn -0.972709 0.188897 0.134739
+v -0.011504 0.027522 0.001664
+vn -0.830823 -0.165637 0.531317
+v -0.009993 0.025195 0.005423
+vn -0.944963 0.131222 0.299709
+v -0.011154 0.027490 0.003315
+vn -0.854162 0.034153 0.518884
+v -0.009816 0.027761 0.006096
+vn -0.861358 -0.066038 0.503688
+v -0.010362 0.026669 0.005165
+vn -0.902569 -0.073067 0.424300
+v -0.011036 0.025785 0.003746
+vn -0.997736 0.047587 0.047526
+v -0.012065 0.024884 -0.000718
+vn -0.965301 0.253212 0.063855
+v -0.011191 0.029310 0.000588
+vn -0.850267 -0.218514 0.478851
+v -0.010561 0.023672 0.003931
+vn -0.966868 0.039640 0.252180
+v -0.011648 0.025822 0.002109
+vn -0.897800 0.091603 0.430771
+v -0.010527 0.027672 0.004812
+vn -0.947899 0.243408 0.205526
+v -0.010875 0.029428 0.002793
+vn -0.742583 0.467306 0.479787
+v -0.008022 0.032170 0.006385
+vn -0.792384 0.450794 0.410989
+v -0.009129 0.031776 0.004968
+vn -0.792591 0.427096 0.435188
+v -0.008384 0.033121 0.004851
+vn -0.839982 0.429486 0.331620
+v -0.009193 0.033056 0.003194
+vn -0.917648 0.340416 0.205034
+v -0.010454 0.030958 0.002646
+vn -0.871335 0.344557 0.349365
+v -0.010089 0.030637 0.004201
+vn -0.819152 0.336038 0.464832
+v -0.009202 0.030421 0.006163
+vn -0.851036 0.440503 0.285824
+v -0.009904 0.031872 0.003025
+vn -0.896327 0.214769 0.387908
+v -0.010322 0.029165 0.004718
+vn -0.845604 0.181651 0.501953
+v -0.009221 0.029040 0.006816
+vn -0.951939 0.295473 0.080674
+v -0.010715 0.030872 0.000990
+vn -0.763009 0.407530 0.501734
+v -0.007094 0.033201 0.006854
+vn -0.805411 0.364601 0.467311
+v -0.007443 0.034575 0.005250
+vn -0.845125 0.373353 0.382585
+v -0.008392 0.034486 0.003465
+vn -0.796355 0.394790 0.458214
+v -0.006351 0.038465 0.004196
+vn -0.831926 0.349474 0.431008
+v -0.007502 0.036219 0.003857
+vn -0.730111 0.504272 0.461138
+v -0.003290 0.040363 0.007389
+vn -0.766086 0.389447 0.511315
+v -0.002083 0.039836 0.009680
+vn -0.565480 0.756060 0.329553
+v -0.003268 0.041120 0.006156
+vn -0.875329 0.335438 0.348253
+v -0.008333 0.035723 0.002425
+vn -0.789533 0.355694 0.500119
+v -0.006252 0.036399 0.005912
+vn -0.783208 0.508730 0.357462
+v -0.006368 0.040538 0.002013
+vn -0.730225 0.517768 0.445745
+v -0.004790 0.040146 0.005224
+vn -0.857370 0.347905 0.379314
+v -0.007730 0.037338 0.002423
+vn -0.473635 0.824468 0.309715
+v -0.002162 0.041882 0.005697
+vn -0.698393 0.664313 0.266338
+v -0.005949 0.041325 0.001581
+vn -0.708999 0.561469 0.426700
+v -0.005617 0.040535 0.003332
+vn -0.399530 0.885076 0.238782
+v -0.001411 0.041616 0.007841
+vn -0.673996 0.618234 0.404371
+v -0.002090 0.042602 0.004203
+vn -0.491498 0.821574 0.288871
+v -0.003327 0.042051 0.003389
+vn -0.448022 0.833520 0.323296
+v -0.004527 0.041675 0.002814
+vn -0.546258 0.742812 0.387083
+v -0.005383 0.041455 0.002305
+vn -0.300599 0.928731 0.217022
+v -0.004703 0.042038 0.001444
+vn -0.540678 0.769359 0.340227
+v -0.004110 0.041294 0.004353
+vn -0.387217 0.919995 0.060606
+v -0.005378 0.042318 -0.002172
+vn -0.726022 0.611329 0.314912
+v -0.003020 0.042702 0.002225
+vn -0.719233 0.685081 0.115618
+v -0.003985 0.042732 -0.001205
+vn -0.806415 0.550748 0.215339
+v -0.003546 0.042910 0.000261
+vn -0.480648 0.851588 0.209225
+v -0.003921 0.042307 0.001261
+vn -0.550719 0.834555 -0.015050
+v -0.004110 0.042781 -0.003272
+vn -0.325211 0.938415 0.116689
+v -0.004404 0.042409 -0.000537
+vn -0.301220 0.945501 0.123669
+v -0.005012 0.042232 -0.000141
+vn -0.271632 0.960746 0.056422
+v -0.004752 0.042492 -0.002309
+vn -0.292946 0.955558 -0.033034
+v -0.004338 0.042662 -0.005235
+vn -0.339592 0.937938 -0.070349
+v -0.004995 0.042445 -0.006265
+vn -0.276646 0.960942 0.007619
+v -0.005074 0.042487 -0.003905
+vn -0.831804 0.540469 0.126472
+v -0.007099 0.040524 -0.000669
+vn -0.743704 0.666661 0.049665
+v -0.006551 0.041445 -0.002240
+vn -0.615495 0.782387 -0.095061
+v -0.005865 0.042001 -0.006380
+vn -0.831836 0.534522 -0.149446
+v -0.006655 0.040966 -0.007200
+vn -0.741323 0.640241 -0.201323
+v -0.006033 0.041447 -0.008363
+vn -0.897332 0.435089 -0.074117
+v -0.007735 0.039654 -0.004342
+vn -0.563520 0.819545 0.103880
+v -0.005727 0.041992 -0.000715
+vn -0.608076 0.793633 0.019729
+v -0.005958 0.042056 -0.003359
+vn -0.476354 0.869165 -0.132816
+v -0.005403 0.042128 -0.007576
+vn -0.447300 0.893895 -0.029561
+v -0.005529 0.042310 -0.004741
+vn -0.481059 0.853536 0.200148
+v -0.005402 0.041853 0.001161
+vn -0.712770 0.686784 0.142431
+v -0.006194 0.041445 0.000281
+vn -0.885548 0.408842 -0.220572
+v -0.006947 0.039920 -0.008374
+vn -0.830392 0.554631 -0.053231
+v -0.007024 0.040868 -0.004701
+vn -0.891709 0.429175 -0.143753
+v -0.007348 0.039904 -0.006489
+vn -0.877274 0.412312 0.245740
+v -0.007461 0.039124 0.001070
+vn -0.940177 0.338790 0.035910
+v -0.008616 0.037624 -0.002394
+vn -0.932795 0.354339 -0.065858
+v -0.008408 0.038129 -0.003893
+vn -0.948721 0.313428 -0.041138
+v -0.008966 0.036594 -0.003522
+vn -0.849339 0.526931 0.031107
+v -0.007410 0.040340 -0.002547
+vn -0.913737 0.406251 0.006723
+v -0.008049 0.039127 -0.002755
+vn -0.830464 0.514461 0.213682
+v -0.006778 0.040528 0.000849
+vn -0.906309 0.329088 0.265150
+v -0.008614 0.036070 0.001246
+vn -0.892655 0.357773 0.274162
+v -0.007977 0.037780 0.001308
+vn -0.822786 0.413359 0.390074
+v -0.007005 0.039139 0.002326
+vn -0.951055 0.308258 0.021732
+v -0.009289 0.035598 -0.002476
+vn -0.899098 0.402571 0.171929
+v -0.007790 0.039002 -0.000120
+vn -0.923488 0.343350 0.171114
+v -0.008588 0.036850 0.000133
+vn -0.905641 0.410517 0.106258
+v -0.007966 0.039093 -0.001431
+vn -0.935157 0.337144 0.108697
+v -0.008703 0.037089 -0.001182
+vn -0.945507 0.322260 0.046540
+v -0.010289 0.032423 -0.000571
+vn -0.920336 0.344577 0.185061
+v -0.009355 0.034535 0.000431
+vn -0.954197 0.298718 -0.016628
+v -0.009992 0.033399 -0.002513
+vn -0.958581 0.284391 -0.015595
+v -0.010734 0.031049 -0.001572
+vn -0.910133 0.385471 0.151887
+v -0.010262 0.031984 0.001277
+vn -0.887055 0.362574 0.285787
+v -0.009027 0.034405 0.001877
+vn -0.940842 0.327755 0.085977
+v -0.009551 0.034572 -0.001032
+vn -0.886952 0.402382 0.226726
+v -0.009713 0.033049 0.001527
+vn -0.921145 0.363911 0.138062
+v -0.009941 0.033157 0.000128
+vn -0.597477 -0.679367 0.426007
+v -0.005458 0.017201 0.007379
+vn -0.758452 -0.360960 0.542640
+v -0.004866 0.018795 0.009799
+vn -0.727393 -0.492247 0.478114
+v -0.002403 0.018444 0.013169
+vn -0.652725 -0.584934 0.481458
+v -0.004290 0.017536 0.009477
+vn -0.790273 -0.272764 0.548697
+v -0.004990 0.020461 0.010485
+vn -0.696441 -0.524655 0.489598
+v -0.003241 0.018001 0.011448
+vn -0.726413 -0.424712 0.540319
+v -0.005971 0.018204 0.007902
+vn -0.585841 -0.682607 0.436851
+v -0.003573 0.016670 0.009075
+vn -0.565612 -0.693761 0.445846
+v -0.002501 0.017134 0.011231
+vn -0.807978 -0.278419 0.519283
+v -0.004744 0.022096 0.011644
+vn -0.807419 -0.335033 0.485620
+v -0.002191 0.020097 0.014748
+vn -0.815393 -0.281326 0.505954
+v -0.003608 0.020523 0.012621
+vn -0.775447 -0.280152 0.565859
+v -0.006562 0.020129 0.008182
+vn -0.786086 -0.339951 0.516238
+v -0.003660 0.019208 0.011812
+vn -0.798888 -0.278536 0.533100
+v -0.005688 0.023480 0.010962
+vn -0.848022 -0.226947 0.478910
+v -0.004047 0.024942 0.014538
+vn -0.864227 -0.108516 0.491260
+v -0.003047 0.025800 0.016663
+vn -0.853338 -0.092756 0.513040
+v -0.004787 0.026548 0.013787
+vn -0.841286 -0.298007 0.451031
+v -0.003059 0.023326 0.015379
+vn -0.842094 -0.005995 0.539298
+v -0.001822 0.026144 0.018772
+vn -0.825000 -0.292304 0.483666
+v -0.004519 0.023612 0.012911
+vn -0.826992 -0.205642 0.523254
+v -0.005975 0.025092 0.011360
+vn -0.797228 0.375603 0.472600
+v -0.000879 0.030917 0.018414
+vn -0.765354 0.426105 0.482356
+v -0.002418 0.031080 0.015739
+vn -0.783922 0.359811 0.505967
+v -0.001950 0.029845 0.017524
+vn -0.811645 0.251768 0.527110
+v -0.003289 0.029183 0.015892
+vn -0.756067 0.414374 0.506613
+v -0.003819 0.030248 0.014368
+vn -0.815903 0.370150 0.444174
+v -0.000782 0.035711 0.014335
+vn -0.764782 0.405582 0.500611
+v -0.002267 0.034587 0.012829
+vn -0.800988 0.402478 0.443205
+v -0.001023 0.034225 0.015241
+vn -0.799095 0.425115 0.425117
+v 0.001284 0.039791 0.015369
+vn -0.842790 0.313819 0.437289
+v -0.000174 0.037465 0.014181
+vn -0.832473 0.325702 0.448227
+v 0.000233 0.039272 0.013705
+vn -0.846323 0.347334 0.403851
+v 0.000102 0.034282 0.017444
+vn -0.858390 0.333700 0.389630
+v 0.000119 0.036151 0.015858
+vn -0.796591 0.398315 0.454739
+v -0.001056 0.032496 0.016751
+vn -0.863214 0.324126 0.387046
+v 0.000784 0.038149 0.015761
+vn -0.109121 0.938076 0.328795
+v 0.003096 0.041243 0.015087
+vn 0.016330 0.851562 0.523999
+v 0.004374 0.040223 0.017296
+vn -0.108518 0.877624 0.466905
+v 0.003410 0.040689 0.016363
+vn -0.833917 0.368383 0.410946
+v 0.001529 0.037977 0.017614
+vn -0.388106 0.718853 0.576735
+v 0.002920 0.039702 0.017689
+vn -0.673352 0.568224 0.472989
+v 0.002158 0.039837 0.016814
+vn -0.614492 0.671634 0.413892
+v 0.001727 0.040839 0.014779
+vn -0.126539 0.825586 0.549905
+v 0.003729 0.040023 0.017572
+vn -0.363924 0.812388 0.455615
+v 0.002683 0.040768 0.015981
+vn -0.313704 0.901850 0.297081
+v 0.002236 0.041377 0.014130
+vn -0.272802 0.937426 0.216362
+v 0.001378 0.041668 0.011947
+vn -0.085558 0.936959 0.338804
+v 0.003723 0.041288 0.014931
+vn -0.064861 0.863893 0.499481
+v 0.004833 0.040779 0.016286
+vn -0.329798 0.749199 0.574399
+v 0.005143 0.041550 0.015165
+vn -0.058221 0.897675 0.436796
+v 0.004143 0.040727 0.016322
+vn -0.578480 -0.568284 0.585161
+v -0.000348 0.019065 0.016580
+vn -0.571506 -0.670602 0.472942
+v -0.001884 0.016403 0.010803
+vn -0.418344 -0.711494 0.564592
+v 0.000009 0.018446 0.016150
+vn -0.701111 -0.490696 0.517359
+v -0.000982 0.018968 0.015719
+vn -0.550328 -0.663647 0.506667
+v -0.000711 0.018212 0.015184
+vn -0.413376 -0.798860 0.436970
+v -0.000559 0.017675 0.014447
+vn -0.325934 -0.680313 0.656461
+v 0.000460 0.018956 0.016993
+vn -0.798400 -0.338394 0.498043
+v -0.002707 0.019379 0.013454
+vn -0.733866 -0.449248 0.509526
+v -0.001721 0.018791 0.014536
+vn -0.570684 -0.693443 0.439838
+v -0.001749 0.017633 0.013008
+vn -0.154674 -0.699582 0.697611
+v 0.001473 0.019239 0.017607
+vn -0.363702 -0.654960 0.662381
+v 0.000550 0.019862 0.017924
+vn -0.187454 -0.772698 0.606464
+v 0.001108 0.018431 0.016610
+vn -0.312276 -0.805507 0.503629
+v 0.000179 0.017971 0.015573
+vn -0.609003 -0.643558 0.463626
+v -0.001402 0.018067 0.014169
+vn -0.480682 -0.746442 0.460183
+v -0.000782 0.017072 0.013102
+vn -0.481652 -0.589790 0.648196
+v 0.000237 0.020884 0.018753
+vn -0.627443 -0.460482 0.627910
+v -0.000171 0.022123 0.019418
+vn -0.828893 -0.301358 0.471296
+v -0.003354 0.021908 0.013863
+vn -0.825223 -0.332129 0.456834
+v -0.002066 0.021567 0.016014
+vn -0.772677 -0.397055 0.495296
+v -0.001066 0.021280 0.017547
+vn -0.645867 -0.505355 0.572251
+v -0.000303 0.021016 0.018348
+vn -0.596415 -0.530965 0.601967
+v -0.000295 0.019867 0.017328
+vn -0.760422 -0.409889 0.503736
+v -0.001122 0.020005 0.016433
+vn -0.771929 -0.371874 0.515592
+v -0.000880 0.022555 0.018838
+vn -0.825015 -0.265839 0.498678
+v -0.001390 0.024030 0.018959
+vn -0.798082 -0.120329 0.590411
+v -0.000988 0.025002 0.019930
+vn -0.729930 -0.314758 0.606737
+v -0.000542 0.023631 0.019988
+vn -0.437454 -0.357363 0.825182
+v 0.000693 0.024179 0.021249
+vn -0.674393 -0.197857 0.711369
+v -0.000115 0.024633 0.020841
+vn -0.522594 -0.429593 0.736441
+v 0.000198 0.023143 0.020407
+vn -0.140114 -0.479204 0.866448
+v 0.001478 0.023952 0.021384
+vn -0.855395 0.010578 0.517868
+v -0.003157 0.027136 0.016561
+vn -0.519328 0.047016 0.853281
+v 0.000894 0.026701 0.021826
+vn -0.853728 -0.138970 0.501833
+v -0.001890 0.025133 0.018570
+vn -0.618772 -0.080143 0.781472
+v 0.000387 0.025640 0.021465
+vn -0.670930 0.110668 0.733216
+v 0.000133 0.027475 0.021191
+vn -0.831842 -0.317250 0.455403
+v -0.001807 0.022932 0.017505
+vn -0.854316 -0.243109 0.459394
+v -0.002527 0.024452 0.017114
+vn -0.507233 0.238267 0.828217
+v 0.001530 0.031363 0.021234
+vn -0.683394 0.265344 0.680121
+v 0.001101 0.031817 0.020754
+vn -0.710302 0.235506 0.663332
+v 0.000050 0.029760 0.020512
+vn -0.521408 0.208887 0.827345
+v 0.001216 0.029655 0.021522
+vn -0.801637 0.321038 0.504294
+v -0.000162 0.030810 0.019728
+vn -0.813019 0.213879 0.541532
+v -0.001704 0.028673 0.018562
+vn -0.839020 0.121335 0.530399
+v -0.002837 0.028192 0.016951
+vn -0.828880 0.074329 0.554466
+v -0.001695 0.027417 0.018877
+vn -0.792482 0.284866 0.539280
+v -0.000758 0.029571 0.019549
+vn -0.440766 0.117303 0.889924
+v 0.001332 0.027741 0.021962
+vn -0.614261 0.199498 0.763468
+v 0.000719 0.029169 0.021276
+vn -0.772220 0.011428 0.635253
+v -0.000611 0.026291 0.020486
+vn -0.777176 0.144021 0.612581
+v -0.000677 0.028107 0.020196
+vn -0.824409 0.308778 0.474348
+v 0.001174 0.034790 0.019375
+vn -0.721018 0.290002 0.629310
+v 0.000664 0.031920 0.020257
+vn -0.239281 0.215098 0.946825
+v 0.002020 0.031157 0.021495
+vn -0.540505 0.243192 0.805427
+v 0.001881 0.033192 0.020931
+vn -0.846252 0.319306 0.426500
+v 0.000616 0.033718 0.019064
+vn -0.306138 0.188356 0.933167
+v 0.001770 0.029079 0.021901
+vn -0.867542 0.317385 0.382932
+v 0.000834 0.036121 0.017619
+vn -0.796946 0.316142 0.514714
+v 0.000297 0.031766 0.019864
+vn -0.376869 0.364051 0.851726
+v 0.002621 0.035738 0.020530
+vn -0.518597 0.280189 0.807806
+v 0.002243 0.034678 0.020715
+vn -0.830087 0.350243 0.433918
+v -0.000016 0.032356 0.018827
+vn -0.258357 0.190746 0.947031
+v 0.002310 0.032955 0.021199
+vn -0.681658 0.309564 0.662957
+v 0.001976 0.035407 0.020204
+vn -0.509136 0.414781 0.754147
+v 0.002437 0.036452 0.020069
+vn -0.650190 0.432886 0.624390
+v 0.002213 0.037165 0.019482
+vn -0.731999 0.274650 0.623494
+v 0.001542 0.033928 0.020370
+vn -0.808110 0.338524 0.482037
+v 0.001766 0.036502 0.019309
+vn -0.080615 0.272058 0.958898
+v 0.003256 0.034934 0.020967
+vn 0.226372 0.055343 0.972467
+v 0.003697 0.032800 0.021171
+vn 0.299312 0.092457 0.949665
+v 0.003627 0.031117 0.021271
+vn 0.102958 0.147820 0.983641
+v 0.002754 0.031498 0.021460
+vn 0.302802 0.200222 0.931784
+v 0.003051 0.030040 0.021667
+vn -0.021104 0.222551 0.974693
+v 0.002370 0.029733 0.021873
+vn 0.136863 0.126322 0.982503
+v 0.003705 0.034322 0.021089
+vn 0.319010 0.080164 0.944355
+v 0.004432 0.034361 0.020899
+vn -0.012219 0.133716 0.990944
+v 0.002923 0.033387 0.021234
+vn -0.267954 0.254706 0.929153
+v 0.002634 0.034523 0.020957
+vn 0.423534 0.047968 0.904609
+v 0.004454 0.029412 0.021084
+vn 0.423036 0.135894 0.895865
+v 0.003809 0.029880 0.021368
+vn 0.427535 0.037056 0.903239
+v 0.005000 0.027370 0.020853
+vn 0.549317 0.038662 0.834719
+v 0.004487 0.028085 0.021148
+vn 0.229143 -0.525418 0.819408
+v 0.002528 0.023316 0.020761
+vn -0.360120 -0.230877 0.903886
+v 0.001217 0.025166 0.021842
+vn 0.001214 -0.362072 0.932149
+v 0.001990 0.024933 0.021904
+vn 0.234426 -0.306520 0.922545
+v 0.003872 0.024740 0.021076
+vn -0.263305 -0.086632 0.960815
+v 0.001672 0.026103 0.022158
+vn 0.342201 -0.433486 0.833660
+v 0.002609 0.024731 0.021635
+vn 0.177649 -0.526601 0.831344
+v 0.002112 0.023952 0.021356
+vn 0.351391 -0.422091 0.835681
+v 0.003170 0.024590 0.021259
+vn 0.414657 -0.288427 0.863058
+v 0.003049 0.025736 0.021873
+vn 0.140253 -0.218333 0.965743
+v 0.002414 0.025861 0.022156
+vn -0.099390 -0.684753 0.721966
+v 0.001514 0.019991 0.018356
+vn -0.238892 -0.536854 0.809147
+v 0.000948 0.022802 0.020548
+vn -0.366163 -0.562237 0.741494
+v 0.000502 0.021690 0.019579
+vn -0.235573 -0.655225 0.717764
+v 0.000874 0.020545 0.018762
+vn -0.027725 -0.626765 0.778715
+v 0.002635 0.020100 0.018516
+vn 0.087458 -0.582910 0.807816
+v 0.001963 0.021753 0.019797
+vn -0.026081 -0.639382 0.768447
+v 0.001505 0.020714 0.019018
+vn 0.061193 -0.580768 0.811766
+v 0.001703 0.022818 0.020622
+vn -0.098024 -0.607725 0.788074
+v 0.001210 0.021543 0.019669
+vn 0.556289 0.033707 0.830305
+v 0.003843 0.027700 0.021654
+vn 0.236611 -0.061633 0.969648
+v 0.002749 0.026894 0.022237
+vn 0.392021 0.069083 0.917359
+v 0.003077 0.027943 0.022109
+vn 0.460809 0.156441 0.873603
+v 0.003402 0.028970 0.021784
+vn 0.535827 -0.107660 0.837436
+v 0.003943 0.026479 0.021532
+vn 0.384794 -0.142967 0.911863
+v 0.004277 0.025752 0.021209
+vn -0.032366 0.132288 0.990683
+v 0.002407 0.028204 0.022208
+vn -0.172344 0.042276 0.984129
+v 0.002042 0.027139 0.022259
+vn 0.487714 -0.102631 0.866950
+v 0.003408 0.026737 0.021920
+vn 0.438431 -0.280925 0.853732
+v 0.003632 0.025636 0.021501
+vn 0.229457 0.206512 0.951158
+v 0.002787 0.028995 0.022033
+vn 0.538705 0.100763 0.836447
+v 0.004017 0.028840 0.021424
+vn 0.529237 -0.008425 0.848432
+v 0.004451 0.026826 0.021208
+vn 0.339823 0.193159 0.920440
+v 0.005198 0.034880 0.020508
+vn 0.302904 0.188452 0.934203
+v 0.004528 0.035380 0.020737
+vn 0.558141 0.444586 0.700587
+v 0.006230 0.037205 0.018982
+vn 0.262353 0.075011 0.962052
+v 0.004665 0.033504 0.020864
+vn 0.062767 0.224562 0.972436
+v 0.005354 0.034062 0.020651
+vn 0.112463 0.455237 0.883239
+v 0.004502 0.036616 0.020309
+vn 0.074246 0.300619 0.950850
+v 0.004048 0.035581 0.020783
+vn 0.288709 0.334676 0.897017
+v 0.004740 0.036240 0.020429
+vn 0.481965 0.295808 0.824747
+v 0.005227 0.036384 0.020142
+vn 0.468036 0.339454 0.815912
+v 0.005728 0.036211 0.019857
+vn 0.461744 0.173193 0.869941
+v 0.005053 0.035483 0.020482
+vn 0.605560 0.490226 0.626877
+v 0.006202 0.038724 0.017939
+vn 0.485635 0.572853 0.660301
+v 0.006161 0.039477 0.017362
+vn 0.467352 0.589388 0.658942
+v 0.005755 0.039052 0.018093
+vn 0.589543 0.470439 0.656602
+v 0.006469 0.037944 0.018237
+vn 0.456648 0.490974 0.741901
+v 0.006956 0.037952 0.017852
+vn 0.600014 0.465698 0.650468
+v 0.006000 0.038170 0.018580
+vn 0.344989 0.466160 0.814664
+v 0.005123 0.037182 0.019838
+vn 0.498987 0.492456 0.713091
+v 0.006560 0.038755 0.017586
+vn 0.557448 0.395958 0.729705
+v 0.005703 0.037351 0.019376
+vn 0.172901 0.538172 0.824910
+v 0.004561 0.037714 0.019657
+vn 0.170920 0.782265 0.599038
+v 0.004936 0.040084 0.017394
+vn 0.385315 0.550470 0.740618
+v 0.005419 0.038235 0.019002
+vn 0.075157 0.745602 0.662140
+v 0.004241 0.039557 0.018233
+vn 0.100014 0.611731 0.784718
+v 0.004066 0.038587 0.019165
+vn 0.319210 0.697724 0.641316
+v 0.005605 0.039902 0.017339
+vn 0.267400 0.649252 0.712017
+v 0.004967 0.039117 0.018465
+vn -0.193129 0.574557 0.795352
+v 0.003399 0.038160 0.019463
+vn -0.304065 0.472338 0.827309
+v 0.002983 0.037059 0.020013
+vn -0.690049 0.504274 0.519172
+v 0.002230 0.038446 0.018424
+vn -0.449796 0.574776 0.683605
+v 0.002790 0.038153 0.019173
+vn -0.158625 0.384223 0.909512
+v 0.003102 0.035981 0.020565
+vn -0.219543 0.703494 0.675942
+v 0.003495 0.039106 0.018665
+vn -0.011275 0.482829 0.875642
+v 0.003663 0.037372 0.019957
+vn -0.005384 0.423182 0.906029
+v 0.003829 0.036346 0.020468
+vn -0.439513 0.854571 0.276653
+v -0.000477 0.041496 0.009909
+vn -0.533003 0.770099 0.350508
+v 0.000728 0.041302 0.012342
+vn -0.677333 0.596246 0.430942
+v -0.001285 0.040915 0.009868
+vn -0.574825 0.744779 0.338940
+v -0.002204 0.041160 0.007966
+vn -0.425839 0.851572 0.305755
+v 0.000047 0.042008 0.008447
+vn -0.291282 0.935862 0.198286
+v 0.000331 0.041787 0.009875
+vn -0.650994 0.591539 0.475698
+v 0.001703 0.042414 0.009841
+vn -0.311583 0.913924 0.260115
+v 0.001836 0.041905 0.011189
+vn -0.164703 0.958877 0.231146
+v 0.002829 0.041621 0.013499
+vn -0.791623 0.329957 0.514257
+v -0.000890 0.039452 0.011690
+vn -0.794452 0.321799 0.515065
+v -0.001093 0.037960 0.012294
+vn -0.753312 0.472474 0.457482
+v 0.000141 0.040507 0.012605
+vn -0.770346 0.377621 0.513779
+v -0.004470 0.038555 0.007095
+vn -0.765137 0.351898 0.539196
+v -0.004654 0.036146 0.008406
+vn -0.789837 0.347080 0.505661
+v -0.001300 0.036313 0.013008
+vn -0.769414 0.340124 0.540665
+v -0.002630 0.038099 0.009996
+vn -0.843057 0.066598 0.533685
+v -0.004852 0.028274 0.013716
+vn -0.800190 0.273642 0.533682
+v -0.005453 0.029700 0.012359
+vn -0.831780 0.111422 0.543808
+v -0.007254 0.028951 0.009920
+vn -0.757770 0.431026 0.489898
+v -0.003177 0.032590 0.013176
+vn -0.734975 0.451383 0.506028
+v -0.006003 0.030751 0.010742
+vn -0.763381 0.354285 0.540122
+v -0.002780 0.036049 0.011050
+vn -0.743443 0.458939 0.486485
+v -0.007919 0.031220 0.007520
+vn -0.795661 0.313296 0.518430
+v -0.007624 0.030145 0.008866
+vn -0.775915 0.356027 0.520770
+v -0.006234 0.034634 0.007123
+vn -0.755540 0.395928 0.521919
+v -0.004792 0.033912 0.009694
+vn -0.737038 0.453023 0.501543
+v -0.006164 0.032001 0.009281
+vn -0.735158 0.462648 0.495479
+v -0.004380 0.031211 0.012616
+vn -0.792711 -0.255232 0.553594
+v -0.006811 0.023753 0.009504
+vn -0.827547 -0.101557 0.552134
+v -0.009232 0.026768 0.006929
+vn -0.835589 0.021708 0.548926
+v -0.008698 0.028131 0.007835
+vn -0.835765 -0.077392 0.543605
+v -0.007128 0.027156 0.010136
+vn -0.783143 -0.264088 0.562978
+v -0.006143 0.022130 0.009650
+vn -0.812550 -0.183580 0.553227
+v -0.008161 0.024916 0.008027
+vn -0.810085 -0.235798 0.536807
+v -0.009216 0.023031 0.005778
+vn -0.788066 -0.247925 0.563459
+v -0.007644 0.022479 0.007772
+vn -0.799579 -0.274267 0.534275
+v -0.008256 0.020653 0.006080
+vn -0.777487 -0.341272 0.528248
+v -0.007355 0.018752 0.006384
+vn -0.591655 -0.724458 0.353703
+v -0.006759 0.017239 0.005487
+vn -0.681993 -0.561815 0.468240
+v -0.006718 0.017722 0.006464
+vn -0.762365 -0.585497 0.275667
+v -0.009493 0.018012 0.001362
+vn -0.596186 -0.702987 0.387779
+v -0.005847 0.016769 0.005903
+vn -0.645323 -0.677478 0.352961
+v -0.006658 0.016670 0.004408
+vn -0.637549 -0.708817 0.301843
+v -0.007648 0.017221 0.003782
+vn -0.811726 -0.361220 0.458934
+v -0.008540 0.019010 0.004635
+vn -0.733721 -0.544237 0.406766
+v -0.007658 0.017834 0.005045
+vn -0.742882 -0.574485 0.343648
+v -0.008592 0.017886 0.003256
+vn -0.393027 -0.787829 -0.474188
+v -0.009254 0.019927 -0.011110
+vn -0.859040 -0.380109 -0.342881
+v -0.010923 0.020707 -0.009416
+vn -0.465530 -0.796538 -0.385758
+v -0.009711 0.019190 -0.009264
+vn -0.755724 -0.585830 -0.292720
+v -0.010737 0.019467 -0.007972
+vn -0.183918 -0.814673 -0.549984
+v -0.006981 0.019928 -0.012003
+vn -0.955863 -0.291665 0.035474
+v -0.011381 0.019639 -0.004098
+vn -0.488366 -0.815082 -0.311675
+v -0.009985 0.018616 -0.007520
+vn -0.394083 -0.910681 -0.123932
+v -0.010144 0.017908 -0.004550
+vn -0.647985 -0.749326 -0.136479
+v -0.010592 0.018210 -0.004863
+vn -0.376597 -0.883451 -0.278727
+v -0.009168 0.018127 -0.007051
+vn -0.432207 -0.824604 -0.365000
+v -0.007844 0.017927 -0.008101
+vn -0.335901 -0.860073 -0.383985
+v -0.008812 0.018626 -0.008799
+vn -0.916146 -0.348884 -0.197376
+v -0.011366 0.020428 -0.007581
+vn -0.906652 -0.402635 -0.125962
+v -0.011270 0.019524 -0.006025
+vn -0.255832 -0.845318 -0.469029
+v -0.008423 0.019379 -0.010588
+vn -0.658822 -0.550664 -0.512565
+v -0.009645 0.020657 -0.011737
+vn -0.707550 -0.579939 -0.403787
+v -0.010229 0.020061 -0.010093
+vn -0.952282 -0.184185 -0.243381
+v -0.011411 0.021465 -0.008613
+vn -0.047136 -0.539010 -0.840979
+v -0.003954 0.023170 -0.015780
+vn -0.038512 -0.839983 -0.541244
+v -0.006596 0.020952 -0.013700
+vn -0.146278 -0.761825 -0.631051
+v -0.005662 0.020685 -0.013330
+vn -0.352249 -0.649519 -0.673829
+v -0.004570 0.019998 -0.013029
+vn 0.080266 -0.777728 -0.623455
+v -0.005456 0.021690 -0.014685
+vn 0.091872 -0.616416 -0.782043
+v -0.005549 0.022485 -0.015598
+vn 0.002421 0.293100 -0.956079
+v -0.003984 0.027855 -0.016122
+vn -0.055700 0.342895 -0.937721
+v -0.004318 0.027278 -0.016329
+vn 0.057385 -0.386445 -0.920526
+v -0.003837 0.024420 -0.016481
+vn -0.116366 0.078224 -0.990121
+v -0.005100 0.025526 -0.016836
+vn -0.019465 0.185858 -0.982384
+v -0.004392 0.025987 -0.016784
+vn -0.267191 0.362098 -0.893025
+v -0.005895 0.027033 -0.016232
+vn -0.175116 0.276294 -0.944985
+v -0.005464 0.026251 -0.016633
+vn 0.115456 -0.394026 -0.911819
+v -0.004437 0.024528 -0.016646
+vn -0.055410 -0.398545 -0.915474
+v -0.005147 0.024297 -0.016574
+vn 0.096969 -0.133701 -0.986266
+v -0.004235 0.025191 -0.016807
+vn -0.089136 0.342456 -0.935296
+v -0.004833 0.026764 -0.016519
+vn -0.290916 0.444683 -0.847127
+v -0.003331 0.027302 -0.016417
+vn -0.130496 0.279446 -0.951252
+v -0.003803 0.026572 -0.016637
+vn 0.021640 -0.478235 -0.877965
+v -0.005294 0.023396 -0.016125
+vn 0.121023 -0.510176 -0.851512
+v -0.004488 0.023714 -0.016209
+vn 0.143469 -0.629046 -0.764014
+v -0.004766 0.022638 -0.015511
+vn -0.038837 -0.211108 -0.976691
+v -0.004890 0.024933 -0.016823
+vn -0.483511 0.303788 -0.820933
+v -0.002795 0.026903 -0.016869
+vn -0.157268 0.134718 -0.978324
+v -0.004406 0.028976 -0.015955
+vn 0.253519 0.435176 -0.863916
+v -0.003347 0.028113 -0.015853
+vn -0.126939 0.060306 -0.990076
+v -0.003876 0.029677 -0.015986
+vn 0.106088 0.141526 -0.984234
+v -0.003797 0.028397 -0.015989
+vn -0.125239 0.237551 -0.963268
+v -0.004783 0.028197 -0.016039
+vn -0.172148 0.326037 -0.929551
+v -0.005264 0.027591 -0.016153
+vn -0.350314 0.234151 -0.906892
+v -0.005142 0.029407 -0.015680
+vn -0.290555 0.281128 -0.914628
+v -0.005501 0.028454 -0.015821
+vn -0.450716 0.308236 -0.837762
+v -0.006053 0.029132 -0.015360
+vn -0.744720 0.318298 -0.586583
+v -0.008187 0.029983 -0.013210
+vn -0.638226 0.335213 -0.693037
+v -0.007487 0.029975 -0.013973
+vn -0.827192 0.297955 -0.476421
+v -0.008959 0.029720 -0.012219
+vn -0.548416 0.338260 -0.764735
+v -0.007020 0.028237 -0.015147
+vn -0.579824 0.339347 -0.740708
+v -0.006556 0.031213 -0.014160
+vn -0.873736 0.186533 -0.449211
+v -0.009940 0.026252 -0.012369
+vn -0.794028 0.274529 -0.542359
+v -0.008898 0.027804 -0.013407
+vn -0.505886 0.338966 -0.793210
+v -0.006869 0.027146 -0.015748
+vn -0.557064 0.330129 -0.762033
+v -0.006805 0.029657 -0.014671
+vn -0.676833 0.314600 -0.665526
+v -0.007827 0.028632 -0.014289
+vn -0.379365 0.340858 -0.860173
+v -0.006162 0.027946 -0.015757
+vn -0.913168 0.307704 -0.267288
+v -0.009281 0.032869 -0.008744
+vn -0.651248 0.351036 -0.672792
+v -0.007401 0.031107 -0.013478
+vn -0.853444 0.311963 -0.417508
+v -0.008741 0.031610 -0.011323
+vn -0.614656 0.336634 -0.713355
+v -0.006423 0.032629 -0.013573
+vn -0.368406 0.194407 -0.909111
+v -0.004436 0.030587 -0.015712
+vn -0.467375 0.248596 -0.848387
+v -0.004099 0.032398 -0.015403
+vn -0.504039 0.292930 -0.812488
+v -0.005489 0.030870 -0.015063
+vn 0.208072 -0.056974 -0.976453
+v -0.001876 0.030545 -0.015767
+vn -0.126442 0.098631 -0.987058
+v -0.002150 0.029273 -0.015722
+vn 0.153935 -0.098337 -0.983175
+v -0.002767 0.028666 -0.015722
+vn 0.062767 -0.030070 -0.997575
+v -0.003209 0.029882 -0.016000
+vn 0.128066 -0.034615 -0.991161
+v -0.002470 0.031073 -0.015972
+vn 0.298701 -0.047226 -0.953178
+v -0.001579 0.032062 -0.015794
+vn 0.211899 -0.113796 -0.970644
+v -0.002608 0.029601 -0.015848
+vn 0.137761 0.028717 -0.990049
+v -0.001202 0.031436 -0.015625
+vn 0.225324 -0.038352 -0.973529
+v -0.003161 0.028322 -0.015774
+vn 0.160014 -0.064715 -0.984991
+v -0.003516 0.028743 -0.015935
+vn 0.228875 -0.140672 -0.963238
+v -0.003100 0.028890 -0.015860
+vn -0.366473 0.251569 -0.895774
+v -0.001305 0.030418 -0.015725
+vn 0.377946 -0.016482 -0.925681
+v -0.001107 0.033834 -0.015699
+vn 0.329827 0.114629 -0.937056
+v -0.000239 0.035023 -0.015222
+vn 0.195784 0.046845 -0.979527
+v -0.000766 0.032873 -0.015512
+vn 0.055475 0.028793 -0.998045
+v -0.002103 0.032821 -0.015959
+vn -0.463966 0.247555 -0.850560
+v -0.000588 0.031879 -0.015640
+vn 0.054140 0.064437 -0.996452
+v -0.001622 0.034533 -0.015867
+vn 0.087263 0.221143 -0.971330
+v -0.001243 0.037580 -0.015505
+vn 0.601662 0.574898 -0.554522
+v 0.001031 0.039783 -0.012325
+vn -0.312243 0.566441 -0.762659
+v 0.000922 0.036720 -0.014682
+vn 0.333411 0.311014 -0.890004
+v 0.000341 0.036796 -0.014603
+vn 0.448260 0.047716 -0.892629
+v -0.000755 0.035690 -0.015543
+vn 0.473764 0.140560 -0.869362
+v -0.000718 0.037300 -0.015370
+vn 0.251059 0.505513 -0.825485
+v -0.000993 0.039296 -0.014742
+vn 0.322304 0.338856 -0.883910
+v -0.000927 0.038342 -0.015198
+vn 0.671345 0.297364 -0.678874
+v 0.000252 0.038414 -0.014149
+vn 0.569437 0.447867 -0.689317
+v -0.000309 0.039559 -0.014179
+vn 0.702101 0.420867 -0.574391
+v 0.000336 0.039625 -0.013390
+vn 0.636713 0.136174 -0.758982
+v -0.000167 0.037052 -0.014961
+vn 0.613225 0.283795 -0.737167
+v -0.000400 0.038478 -0.014819
+vn 0.576383 0.486184 -0.656817
+v 0.000968 0.038858 -0.013146
+vn 0.322060 0.877363 -0.355685
+v -0.000960 0.042220 -0.011309
+vn 0.193546 0.950573 -0.242799
+v -0.001075 0.042538 -0.010434
+vn 0.443357 0.848522 -0.288869
+v -0.000489 0.042353 -0.010302
+vn 0.574278 0.748946 -0.330583
+v -0.000289 0.041884 -0.011161
+vn 0.576514 0.743543 -0.338785
+v 0.000025 0.042006 -0.010248
+vn 0.603270 0.706051 -0.370887
+v 0.000352 0.041435 -0.010838
+vn 0.590729 0.671311 -0.447640
+v 0.000786 0.040648 -0.011542
+vn -0.133626 0.542019 -0.829674
+v -0.001923 0.039776 -0.014495
+vn 0.658188 0.662619 -0.357385
+v 0.000037 0.041209 -0.011940
+vn 0.453557 0.787763 -0.416792
+v -0.000638 0.041658 -0.012206
+vn 0.593579 0.656513 -0.465462
+v -0.000269 0.041047 -0.012799
+vn 0.549283 0.559260 -0.620899
+v -0.000285 0.040444 -0.013477
+vn 0.177923 0.634608 -0.752074
+v -0.001181 0.040408 -0.013984
+vn -0.019814 0.780801 -0.624465
+v -0.001841 0.041359 -0.013108
+vn 0.279171 0.739539 -0.612491
+v -0.000919 0.041201 -0.013129
+vn -0.216635 0.824098 -0.523385
+v -0.002751 0.041623 -0.012501
+vn -0.111484 0.901515 -0.418141
+v -0.002226 0.042052 -0.011914
+vn -0.299459 0.861728 -0.409571
+v -0.003383 0.041923 -0.011584
+vn -0.187884 0.927292 -0.323773
+v -0.002921 0.042245 -0.011089
+vn 0.126889 0.870225 -0.476033
+v -0.001426 0.041890 -0.012278
+vn 0.018094 0.949230 -0.314063
+v -0.001641 0.042395 -0.011136
+vn 0.699799 0.551431 -0.454099
+v 0.000283 0.040457 -0.012666
+vn -0.213628 0.116510 -0.969943
+v -0.003303 0.031525 -0.015930
+vn -0.325749 0.166397 -0.930698
+v -0.002759 0.033490 -0.015786
+vn -0.566007 0.163149 -0.808095
+v -0.003497 0.035204 -0.014982
+vn 0.000413 0.113048 -0.993589
+v -0.001442 0.036222 -0.015720
+vn -0.440425 0.141792 -0.886522
+v -0.002475 0.036307 -0.015461
+vn -0.601101 0.263082 -0.754629
+v -0.004947 0.034081 -0.014165
+vn -0.638949 0.192554 -0.744760
+v -0.004843 0.035381 -0.013887
+vn -0.528844 0.240742 -0.813860
+v -0.003673 0.033952 -0.015163
+vn -0.565044 0.311519 -0.763990
+v -0.005183 0.032722 -0.014538
+vn -0.368796 0.144865 -0.918152
+v -0.002386 0.034954 -0.015676
+vn -0.887229 0.309600 -0.342013
+v -0.008928 0.032428 -0.010206
+vn -0.841986 0.316933 -0.436592
+v -0.008152 0.033700 -0.010932
+vn -0.851140 0.297868 -0.432246
+v -0.007526 0.036050 -0.010495
+vn -0.754133 0.304196 -0.582021
+v -0.007272 0.034445 -0.011852
+vn -0.689320 0.338386 -0.640573
+v -0.007475 0.032250 -0.012779
+vn -0.653960 0.285328 -0.700659
+v -0.006274 0.034076 -0.013047
+vn -0.784753 0.325257 -0.527609
+v -0.008220 0.031884 -0.012057
+vn -0.893244 0.305569 -0.329762
+v -0.008357 0.034755 -0.009586
+# 1623 vertices, 0 vertices normals
+
+f 904//904 2//2 906//906
+f 1//1 4//4 9//9
+f 9//9 4//4 17//17
+f 2//2 904//904 3//3
+f 9//9 2//2 3//3
+f 1//1 9//9 3//3
+f 4//4 1//1 5//5
+f 2//2 8//8 6//6
+f 906//906 2//2 6//6
+f 2//2 9//9 8//8
+f 16//16 19//19 10//10
+f 17//17 16//16 10//10
+f 19//19 16//16 7//7
+f 8//8 9//9 10//10
+f 8//8 10//10 11//11
+f 10//10 19//19 11//11
+f 9//9 17//17 10//10
+f 12//12 1553//1553 1563//1563
+f 15//15 1563//1563 1568//1568
+f 12//12 1563//1563 15//15
+f 140//140 15//15 13//13
+f 20//20 14//14 21//21
+f 14//14 7//7 18//18
+f 14//14 20//20 25//25
+f 21//21 14//14 18//18
+f 13//13 20//20 21//21
+f 13//13 15//15 20//20
+f 20//20 1568//1568 25//25
+f 4//4 5//5 13//13
+f 16//16 17//17 18//18
+f 7//7 16//16 18//18
+f 19//19 7//7 27//27
+f 15//15 1568//1568 20//20
+f 17//17 21//21 18//18
+f 17//17 4//4 21//21
+f 4//4 13//13 21//21
+f 5//5 140//140 13//13
+f 12//12 15//15 140//140
+f 23//23 1572//1572 22//22
+f 22//22 1572//1572 32//32
+f 23//23 14//14 26//26
+f 32//32 24//24 29//29
+f 32//32 1572//1572 24//24
+f 14//14 25//25 26//26
+f 27//27 7//7 23//23
+f 27//27 23//23 28//28
+f 7//7 14//14 23//23
+f 23//23 22//22 28//28
+f 1572//1572 23//23 26//26
+f 29//29 39//39 30//30
+f 22//22 31//31 47//47
+f 22//22 32//32 31//31
+f 31//31 32//32 33//33
+f 32//32 29//29 33//33
+f 34//34 46//46 41//41
+f 34//34 41//41 40//40
+f 40//40 974//974 35//35
+f 34//34 40//40 35//35
+f 35//35 974//974 973//973
+f 31//31 33//33 34//34
+f 31//31 34//34 35//35
+f 47//47 35//35 973//973
+f 47//47 31//31 35//35
+f 47//47 973//973 953//953
+f 37//37 43//43 38//38
+f 43//43 39//39 38//38
+f 30//30 39//39 43//43
+f 33//33 30//30 46//46
+f 29//29 30//30 33//33
+f 34//34 33//33 46//46
+f 36//36 44//44 380//380
+f 44//44 43//43 380//380
+f 43//43 37//37 380//380
+f 36//36 380//380 386//386
+f 38//38 1590//1590 382//382
+f 38//38 39//39 1571//1571
+f 40//40 41//41 49//49
+f 49//49 41//41 45//45
+f 974//974 40//40 950//950
+f 40//40 49//49 950//950
+f 44//44 36//36 42//42
+f 46//46 44//44 42//42
+f 36//36 45//45 42//42
+f 30//30 43//43 44//44
+f 46//46 30//30 44//44
+f 45//45 41//41 42//42
+f 41//41 46//46 42//42
+f 28//28 47//47 953//953
+f 28//28 22//22 47//47
+f 27//27 28//28 953//953
+f 48//48 68//68 58//58
+f 68//68 48//48 67//67
+f 58//58 68//68 69//69
+f 48//48 58//58 55//55
+f 51//51 45//45 53//53
+f 45//45 36//36 53//53
+f 49//49 51//51 50//50
+f 49//49 45//45 51//51
+f 950//950 49//49 50//50
+f 58//58 69//69 57//57
+f 57//57 69//69 52//52
+f 57//57 52//52 54//54
+f 53//53 386//386 387//387
+f 36//36 386//386 53//53
+f 53//53 56//56 59//59
+f 53//53 387//387 56//56
+f 59//59 57//57 948//948
+f 50//50 51//51 948//948
+f 51//51 59//59 948//948
+f 51//51 53//53 59//59
+f 57//57 54//54 948//948
+f 387//387 55//55 56//56
+f 55//55 58//58 56//56
+f 58//58 57//57 59//59
+f 56//56 58//58 59//59
+f 60//60 66//66 61//61
+f 62//62 61//61 71//71
+f 60//60 61//61 62//62
+f 64//64 404//404 63//63
+f 404//404 64//64 402//402
+f 64//64 63//63 65//65
+f 63//63 66//66 65//65
+f 66//66 60//60 65//65
+f 69//69 60//60 62//62
+f 52//52 69//69 62//62
+f 64//64 67//67 402//402
+f 67//67 64//64 65//65
+f 68//68 67//67 65//65
+f 60//60 68//68 65//65
+f 69//69 68//68 60//60
+f 66//66 63//63 70//70
+f 70//70 404//404 397//397
+f 71//71 76//76 940//940
+f 71//71 61//61 76//76
+f 63//63 404//404 70//70
+f 77//77 70//70 408//408
+f 70//70 397//397 408//408
+f 61//61 66//66 77//77
+f 76//76 61//61 77//77
+f 66//66 70//70 77//77
+f 416//416 420//420 72//72
+f 78//78 93//93 941//941
+f 77//77 408//408 72//72
+f 83//83 78//78 73//73
+f 78//78 420//420 73//73
+f 420//420 78//78 75//75
+f 93//93 78//78 83//83
+f 83//83 73//73 409//409
+f 73//73 417//417 74//74
+f 408//408 416//416 72//72
+f 73//73 420//420 417//417
+f 941//941 940//940 75//75
+f 940//940 76//76 75//75
+f 409//409 435//435 84//84
+f 83//83 409//409 84//84
+f 72//72 420//420 75//75
+f 76//76 72//72 75//75
+f 435//435 409//409 410//410
+f 76//76 77//77 72//72
+f 78//78 941//941 75//75
+f 87//87 477//477 86//86
+f 84//84 435//435 79//79
+f 94//94 85//85 80//80
+f 80//80 85//85 82//82
+f 94//94 80//80 939//939
+f 939//939 80//80 90//90
+f 80//80 82//82 91//91
+f 82//82 81//81 89//89
+f 82//82 85//85 81//81
+f 93//93 94//94 939//939
+f 83//83 84//84 94//94
+f 94//94 84//84 85//85
+f 85//85 84//84 448//448
+f 84//84 79//79 448//448
+f 444//444 88//88 445//445
+f 86//86 923//923 92//92
+f 477//477 87//87 444//444
+f 444//444 87//87 88//88
+f 87//87 86//86 92//92
+f 90//90 80//80 91//91
+f 91//91 82//82 88//88
+f 82//82 89//89 88//88
+f 91//91 87//87 92//92
+f 87//87 91//91 88//88
+f 90//90 91//91 92//92
+f 93//93 83//83 94//94
+f 104//104 122//122 95//95
+f 109//109 97//97 96//96
+f 97//97 109//109 102//102
+f 99//99 96//96 98//98
+f 109//109 96//96 99//99
+f 99//99 98//98 108//108
+f 108//108 98//98 100//100
+f 97//97 102//102 113//113
+f 96//96 112//112 121//121
+f 96//96 97//97 112//112
+f 98//98 96//96 121//121
+f 100//100 98//98 125//125
+f 104//104 95//95 103//103
+f 105//105 932//932 106//106
+f 932//932 931//931 101//101
+f 108//108 932//932 101//101
+f 101//101 929//929 110//110
+f 103//103 110//110 928//928
+f 110//110 929//929 928//928
+f 102//102 109//109 103//103
+f 102//102 103//103 120//120
+f 104//104 103//103 928//928
+f 103//103 95//95 120//120
+f 105//105 106//106 107//107
+f 106//106 100//100 107//107
+f 932//932 108//108 106//106
+f 108//108 100//100 106//106
+f 99//99 101//101 110//110
+f 99//99 108//108 101//101
+f 103//103 109//109 110//110
+f 109//109 99//99 110//110
+f 95//95 122//122 111//111
+f 95//95 111//111 472//472
+f 112//112 113//113 114//114
+f 112//112 114//114 118//118
+f 114//114 116//116 115//115
+f 114//114 113//113 116//116
+f 116//116 120//120 117//117
+f 115//115 116//116 117//117
+f 117//117 120//120 119//119
+f 115//115 117//117 474//474
+f 118//118 114//114 479//479
+f 114//114 115//115 479//479
+f 95//95 472//472 119//119
+f 119//119 472//472 471//471
+f 474//474 119//119 471//471
+f 117//117 119//119 474//474
+f 923//923 86//86 122//122
+f 97//97 113//113 112//112
+f 122//122 477//477 111//111
+f 102//102 120//120 116//116
+f 113//113 102//102 116//116
+f 86//86 477//477 122//122
+f 120//120 95//95 119//119
+f 98//98 121//121 125//125
+f 125//125 121//121 476//476
+f 104//104 923//923 122//122
+f 121//121 112//112 118//118
+f 129//129 902//902 123//123
+f 128//128 864//864 124//124
+f 128//128 124//124 133//133
+f 125//125 476//476 131//131
+f 131//131 476//476 883//883
+f 107//107 100//100 126//126
+f 125//125 131//131 126//126
+f 100//100 125//125 126//126
+f 131//131 132//132 127//127
+f 126//126 131//131 127//127
+f 132//132 128//128 127//127
+f 107//107 129//129 123//123
+f 129//129 126//126 127//127
+f 129//129 107//107 126//126
+f 105//105 107//107 123//123
+f 132//132 883//883 130//130
+f 131//131 883//883 132//132
+f 864//864 128//128 130//130
+f 128//128 132//132 130//130
+f 128//128 133//133 127//127
+f 864//864 130//130 865//865
+f 133//133 129//129 127//127
+f 902//902 129//129 134//134
+f 892//892 135//135 136//136
+f 129//129 133//133 134//134
+f 135//135 892//892 1509//1509
+f 134//134 133//133 892//892
+f 124//124 864//864 137//137
+f 892//892 124//124 1501//1501
+f 1509//1509 892//892 1501//1501
+f 133//133 124//124 892//892
+f 124//124 137//137 1501//1501
+f 894//894 1524//1524 138//138
+f 136//136 135//135 1524//1524
+f 140//140 5//5 900//900
+f 894//894 138//138 900//900
+f 894//894 136//136 1524//1524
+f 12//12 138//138 139//139
+f 12//12 139//139 141//141
+f 138//138 1524//1524 1518//1518
+f 139//139 138//138 1518//1518
+f 12//12 140//140 138//138
+f 138//138 140//140 900//900
+f 1553//1553 12//12 141//141
+f 142//142 770//770 143//143
+f 143//143 770//770 145//145
+f 144//144 143//143 147//147
+f 142//142 143//143 144//144
+f 770//770 142//142 1365//1365
+f 147//147 143//143 146//146
+f 143//143 145//145 146//146
+f 146//146 145//145 799//799
+f 146//146 798//798 1405//1405
+f 147//147 146//146 1405//1405
+f 1405//1405 798//798 797//797
+f 149//149 152//152 294//294
+f 1419//1419 151//151 1413//1413
+f 1413//1413 151//151 150//150
+f 152//152 151//151 154//154
+f 152//152 154//154 148//148
+f 294//294 152//152 148//148
+f 294//294 148//148 153//153
+f 294//294 153//153 293//293
+f 152//152 149//149 150//150
+f 151//151 1419//1419 154//154
+f 151//151 152//152 150//150
+f 154//154 1416//1416 155//155
+f 155//155 1416//1416 156//156
+f 156//156 1416//1416 163//163
+f 154//154 1419//1419 1416//1416
+f 153//153 155//155 232//232
+f 153//153 148//148 155//155
+f 148//148 154//154 155//155
+f 155//155 156//156 232//232
+f 161//161 157//157 173//173
+f 173//173 157//157 172//172
+f 158//158 781//781 174//174
+f 172//172 158//158 174//174
+f 158//158 172//172 159//159
+f 160//160 157//157 225//225
+f 157//157 161//161 225//225
+f 172//172 157//157 159//159
+f 159//159 157//157 160//160
+f 161//161 173//173 162//162
+f 232//232 156//156 225//225
+f 156//156 160//160 225//225
+f 156//156 163//163 160//160
+f 164//164 165//165 169//169
+f 164//164 169//169 167//167
+f 172//172 174//174 166//166
+f 174//174 167//167 166//166
+f 162//162 173//173 168//168
+f 169//169 170//170 171//171
+f 168//168 171//171 216//216
+f 171//171 170//170 216//216
+f 170//170 169//169 179//179
+f 166//166 167//167 171//171
+f 167//167 169//169 171//171
+f 169//169 165//165 179//179
+f 173//173 172//172 166//166
+f 168//168 166//166 171//171
+f 173//173 166//166 168//168
+f 179//179 165//165 787//787
+f 781//781 164//164 167//167
+f 174//174 781//781 167//167
+f 809//809 175//175 802//802
+f 808//808 176//176 180//180
+f 192//192 187//187 193//193
+f 176//176 808//808 184//184
+f 175//175 809//809 184//184
+f 186//186 221//221 189//189
+f 190//190 189//189 183//183
+f 183//183 177//177 178//178
+f 189//189 221//221 183//183
+f 183//183 221//221 177//177
+f 808//808 180//180 782//782
+f 808//808 175//175 184//184
+f 176//176 190//190 183//183
+f 170//170 179//179 178//178
+f 180//180 176//176 178//178
+f 184//184 809//809 190//190
+f 809//809 185//185 181//181
+f 192//192 193//193 182//182
+f 190//190 809//809 181//181
+f 176//176 183//183 178//178
+f 176//176 184//184 190//190
+f 193//193 185//185 194//194
+f 179//179 787//787 180//180
+f 185//185 193//193 181//181
+f 179//179 180//180 178//178
+f 187//187 186//186 189//189
+f 187//187 189//189 181//181
+f 186//186 187//187 188//188
+f 187//187 192//192 188//188
+f 180//180 787//787 782//782
+f 177//177 170//170 178//178
+f 193//193 187//187 181//181
+f 189//189 190//190 181//181
+f 191//191 182//182 195//195
+f 192//192 191//191 198//198
+f 200//200 438//438 413//413
+f 862//862 855//855 196//196
+f 182//182 193//193 862//862
+f 182//182 862//862 196//196
+f 196//196 855//855 849//849
+f 196//196 849//849 428//428
+f 193//193 194//194 862//862
+f 195//195 428//428 429//429
+f 195//195 182//182 196//196
+f 855//855 862//862 856//856
+f 195//195 196//196 428//428
+f 438//438 200//200 197//197
+f 438//438 197//197 199//199
+f 197//197 198//198 199//199
+f 198//198 191//191 199//199
+f 198//198 197//197 207//207
+f 188//188 198//198 207//207
+f 191//191 195//195 433//433
+f 192//192 182//182 191//191
+f 438//438 199//199 433//433
+f 199//199 191//191 433//433
+f 188//188 192//192 198//198
+f 412//412 211//211 204//204
+f 200//200 412//412 204//204
+f 201//201 203//203 202//202
+f 202//202 203//203 209//209
+f 211//211 412//412 424//424
+f 211//211 424//424 205//205
+f 197//197 200//200 204//204
+f 201//201 211//211 205//205
+f 201//201 205//205 426//426
+f 224//224 206//206 208//208
+f 207//207 202//202 210//210
+f 188//188 207//207 212//212
+f 206//206 209//209 208//208
+f 211//211 201//201 202//202
+f 208//208 209//209 234//234
+f 234//234 203//203 247//247
+f 209//209 203//203 234//234
+f 202//202 209//209 210//210
+f 207//207 210//210 212//212
+f 209//209 206//206 210//210
+f 207//207 204//204 202//202
+f 204//204 211//211 202//202
+f 207//207 197//197 204//204
+f 223//223 218//218 214//214
+f 212//212 210//210 223//223
+f 214//214 216//216 213//213
+f 216//216 170//170 177//177
+f 216//216 177//177 213//213
+f 218//218 162//162 214//214
+f 223//223 210//210 215//215
+f 218//218 223//223 215//215
+f 162//162 168//168 214//214
+f 168//168 216//216 214//214
+f 186//186 188//188 212//212
+f 186//186 212//212 222//222
+f 219//219 218//218 217//217
+f 218//218 215//215 217//217
+f 219//219 217//217 220//220
+f 177//177 221//221 213//213
+f 221//221 186//186 222//222
+f 213//213 221//221 222//222
+f 215//215 206//206 217//217
+f 210//210 206//206 215//215
+f 214//214 213//213 222//222
+f 162//162 218//218 219//219
+f 223//223 214//214 222//222
+f 212//212 223//223 222//222
+f 217//217 224//224 220//220
+f 206//206 224//224 217//217
+f 225//225 161//161 229//229
+f 220//220 224//224 226//226
+f 220//220 228//228 233//233
+f 232//232 229//229 278//278
+f 278//278 229//229 230//230
+f 227//227 230//230 231//231
+f 231//231 243//243 270//270
+f 227//227 231//231 272//272
+f 231//231 270//270 272//272
+f 161//161 162//162 219//219
+f 220//220 226//226 228//228
+f 229//229 161//161 233//233
+f 233//233 228//228 230//230
+f 229//229 233//233 230//230
+f 230//230 228//228 231//231
+f 228//228 226//226 243//243
+f 231//231 228//228 243//243
+f 232//232 225//225 229//229
+f 161//161 219//219 233//233
+f 219//219 220//220 233//233
+f 208//208 234//234 242//242
+f 242//242 239//239 244//244
+f 244//244 239//239 236//236
+f 236//236 235//235 246//246
+f 236//236 239//239 235//235
+f 244//244 236//236 245//245
+f 247//247 203//203 405//405
+f 245//245 236//236 237//237
+f 245//245 237//237 238//238
+f 242//242 234//234 239//239
+f 247//247 405//405 395//395
+f 395//395 240//240 241//241
+f 235//235 395//395 241//241
+f 235//235 247//247 395//395
+f 241//241 240//240 256//256
+f 226//226 224//224 242//242
+f 243//243 226//226 244//244
+f 226//226 242//242 244//244
+f 270//270 243//243 245//245
+f 243//243 244//244 245//245
+f 270//270 245//245 238//238
+f 246//246 235//235 241//241
+f 224//224 208//208 242//242
+f 234//234 247//247 239//239
+f 246//246 241//241 256//256
+f 239//239 247//247 235//235
+f 403//403 887//887 249//249
+f 248//248 249//249 250//250
+f 251//251 249//249 886//886
+f 250//250 249//249 251//251
+f 251//251 886//886 884//884
+f 248//248 250//250 257//257
+f 403//403 249//249 248//248
+f 255//255 251//251 252//252
+f 255//255 252//252 324//324
+f 237//237 257//257 254//254
+f 237//237 254//254 262//262
+f 254//254 255//255 262//262
+f 262//262 255//255 253//253
+f 253//253 255//255 320//320
+f 250//250 251//251 254//254
+f 257//257 250//250 254//254
+f 251//251 255//255 254//254
+f 238//238 261//261 271//271
+f 271//271 261//261 273//273
+f 273//273 261//261 259//259
+f 273//273 259//259 263//263
+f 270//270 238//238 271//271
+f 256//256 403//403 248//248
+f 246//246 256//256 248//248
+f 246//246 248//248 257//257
+f 236//236 246//246 257//257
+f 237//237 236//236 257//257
+f 320//320 258//258 260//260
+f 259//259 253//253 260//260
+f 253//253 320//320 260//260
+f 261//261 253//253 259//259
+f 262//262 253//253 261//261
+f 238//238 262//262 261//261
+f 238//238 237//237 262//262
+f 263//263 259//259 260//260
+f 263//263 260//260 264//264
+f 269//269 277//277 275//275
+f 269//269 275//275 265//265
+f 265//265 275//275 276//276
+f 265//265 276//276 268//268
+f 277//277 269//269 279//279
+f 268//268 276//276 266//266
+f 276//276 272//272 266//266
+f 277//277 227//227 275//275
+f 268//268 266//266 274//274
+f 269//269 265//265 267//267
+f 274//274 264//264 288//288
+f 268//268 288//288 286//286
+f 267//267 265//265 286//286
+f 268//268 274//274 288//288
+f 265//265 268//268 286//286
+f 293//293 269//269 267//267
+f 269//269 293//293 279//279
+f 227//227 272//272 276//276
+f 272//272 270//270 271//271
+f 272//272 271//271 266//266
+f 271//271 273//273 266//266
+f 266//266 273//273 274//274
+f 273//273 263//263 274//274
+f 263//263 264//264 274//274
+f 275//275 227//227 276//276
+f 278//278 277//277 279//279
+f 278//278 227//227 277//277
+f 232//232 278//278 279//279
+f 278//278 230//230 227//227
+f 153//153 232//232 279//279
+f 293//293 153//153 279//279
+f 282//282 291//291 280//280
+f 267//267 285//285 295//295
+f 267//267 286//286 285//285
+f 282//282 280//280 297//297
+f 281//281 315//315 292//292
+f 292//292 315//315 317//317
+f 295//295 282//282 297//297
+f 294//294 295//295 297//297
+f 302//302 283//283 289//289
+f 283//283 302//302 284//284
+f 285//285 286//286 287//287
+f 286//286 288//288 287//287
+f 288//288 281//281 287//287
+f 288//288 264//264 281//281
+f 302//302 289//289 1284//1284
+f 280//280 292//292 299//299
+f 280//280 291//291 292//292
+f 149//149 290//290 304//304
+f 285//285 282//282 295//295
+f 282//282 285//285 287//287
+f 291//291 282//282 287//287
+f 281//281 291//291 287//287
+f 291//291 281//281 292//292
+f 292//292 317//317 299//299
+f 281//281 264//264 315//315
+f 293//293 267//267 295//295
+f 294//294 293//293 295//295
+f 290//290 298//298 296//296
+f 296//296 298//298 301//301
+f 304//304 290//290 296//296
+f 301//301 300//300 309//309
+f 301//301 298//298 300//300
+f 280//280 299//299 298//298
+f 149//149 297//297 290//290
+f 297//297 280//280 290//290
+f 149//149 294//294 297//297
+f 298//298 299//299 300//300
+f 290//290 280//280 298//298
+f 284//284 301//301 309//309
+f 284//284 302//302 301//301
+f 302//302 296//296 301//301
+f 302//302 1284//1284 296//296
+f 1284//1284 304//304 296//296
+f 304//304 1284//1284 303//303
+f 150//150 304//304 303//303
+f 150//150 149//149 304//304
+f 321//321 305//305 306//306
+f 324//324 305//305 313//313
+f 305//305 324//324 306//306
+f 312//312 311//311 308//308
+f 321//321 318//318 307//307
+f 264//264 258//258 315//315
+f 307//307 318//318 310//310
+f 310//310 318//318 323//323
+f 260//260 258//258 264//264
+f 308//308 353//353 1442//1442
+f 312//312 308//308 1442//1442
+f 312//312 309//309 311//311
+f 311//311 310//310 323//323
+f 306//306 324//324 354//354
+f 258//258 320//320 313//313
+f 311//311 309//309 310//310
+f 313//313 305//305 316//316
+f 284//284 312//312 1442//1442
+f 284//284 309//309 312//312
+f 283//283 284//284 1442//1442
+f 258//258 313//313 316//316
+f 305//305 321//321 314//314
+f 316//316 305//305 314//314
+f 321//321 307//307 314//314
+f 315//315 258//258 316//316
+f 317//317 316//316 314//314
+f 317//317 315//315 316//316
+f 307//307 317//317 314//314
+f 318//318 322//322 319//319
+f 318//318 321//321 322//322
+f 299//299 317//317 307//307
+f 323//323 318//318 319//319
+f 300//300 307//307 310//310
+f 324//324 252//252 354//354
+f 255//255 324//324 320//320
+f 321//321 306//306 322//322
+f 311//311 323//323 352//352
+f 308//308 311//311 352//352
+f 320//320 324//324 313//313
+f 309//309 300//300 310//310
+f 300//300 299//299 307//307
+f 330//330 351//351 325//325
+f 333//333 326//326 334//334
+f 334//334 326//326 888//888
+f 330//330 325//325 337//337
+f 327//327 344//344 341//341
+f 344//344 327//327 336//336
+f 329//329 1186//1186 328//328
+f 328//328 1186//1186 1184//1184
+f 336//336 329//329 328//328
+f 330//330 337//337 341//341
+f 337//337 327//327 341//341
+f 329//329 338//338 339//339
+f 355//355 338//338 331//331
+f 334//334 332//332 374//374
+f 333//333 334//334 335//335
+f 336//336 327//327 338//338
+f 329//329 336//336 338//338
+f 327//327 337//337 331//331
+f 338//338 327//327 331//331
+f 337//337 325//325 359//359
+f 331//331 337//337 359//359
+f 333//333 325//325 326//326
+f 325//325 333//333 359//359
+f 334//334 374//374 335//335
+f 339//339 338//338 355//355
+f 1186//1186 329//329 339//339
+f 1186//1186 339//339 1185//1185
+f 342//342 347//347 340//340
+f 342//342 340//340 343//343
+f 341//341 344//344 342//342
+f 330//330 342//342 343//343
+f 347//347 344//344 345//345
+f 342//342 344//344 347//347
+f 328//328 1184//1184 346//346
+f 345//345 328//328 346//346
+f 346//346 1184//1184 1177//1177
+f 345//345 346//346 348//348
+f 306//306 351//351 343//343
+f 306//306 354//354 351//351
+f 347//347 348//348 349//349
+f 340//340 347//347 349//349
+f 352//352 348//348 350//350
+f 347//347 345//345 348//348
+f 346//346 1177//1177 350//350
+f 1177//1177 353//353 350//350
+f 348//348 352//352 349//349
+f 348//348 346//346 350//350
+f 344//344 336//336 345//345
+f 334//334 888//888 332//332
+f 326//326 354//354 888//888
+f 325//325 351//351 326//326
+f 336//336 328//328 345//345
+f 351//351 354//354 326//326
+f 319//319 340//340 349//349
+f 319//319 322//322 340//340
+f 352//352 323//323 349//349
+f 323//323 319//319 349//349
+f 308//308 352//352 350//350
+f 353//353 308//308 350//350
+f 330//330 341//341 342//342
+f 354//354 252//252 888//888
+f 351//351 330//330 343//343
+f 322//322 306//306 343//343
+f 340//340 322//322 343//343
+f 358//358 356//356 357//357
+f 355//355 356//356 360//360
+f 358//358 367//367 368//368
+f 358//358 357//357 362//362
+f 367//367 358//358 362//362
+f 1185//1185 360//360 1188//1188
+f 331//331 359//359 357//357
+f 355//355 331//331 357//357
+f 356//356 355//355 357//357
+f 339//339 355//355 360//360
+f 1185//1185 339//339 360//360
+f 360//360 356//356 1188//1188
+f 358//358 368//368 1083//1083
+f 356//356 358//358 1083//1083
+f 1188//1188 356//356 1083//1083
+f 333//333 335//335 361//361
+f 359//359 333//333 361//361
+f 357//357 359//359 362//362
+f 379//379 365//365 384//384
+f 384//384 365//365 363//363
+f 384//384 363//363 1087//1087
+f 335//335 364//364 366//366
+f 361//361 335//335 366//366
+f 364//364 1087//1087 369//369
+f 1087//1087 364//364 377//377
+f 379//379 382//382 365//365
+f 335//335 374//374 364//364
+f 384//384 1087//1087 377//377
+f 366//366 364//364 369//369
+f 369//369 1087//1087 1086//1086
+f 367//367 366//366 369//369
+f 367//367 362//362 366//366
+f 362//362 361//361 366//366
+f 368//368 369//369 1086//1086
+f 368//368 367//367 369//369
+f 362//362 359//359 361//361
+f 370//370 372//372 371//371
+f 375//375 385//385 378//378
+f 377//377 370//370 381//381
+f 372//372 370//370 378//378
+f 370//370 371//371 381//381
+f 381//381 371//371 373//373
+f 371//371 372//372 383//383
+f 383//383 372//372 388//388
+f 379//379 381//381 373//373
+f 376//376 374//374 885//885
+f 372//372 378//378 391//391
+f 388//388 372//372 391//391
+f 375//375 378//378 376//376
+f 375//375 376//376 885//885
+f 364//364 374//374 376//376
+f 377//377 364//364 376//376
+f 370//370 377//377 376//376
+f 378//378 370//370 376//376
+f 382//382 379//379 373//373
+f 37//37 371//371 383//383
+f 380//380 37//37 383//383
+f 379//379 384//384 381//381
+f 38//38 382//382 373//373
+f 37//37 38//38 373//373
+f 371//371 37//37 373//373
+f 386//386 380//380 383//383
+f 386//386 383//383 388//388
+f 384//384 377//377 381//381
+f 48//48 55//55 385//385
+f 48//48 385//385 389//389
+f 67//67 48//48 406//406
+f 375//375 885//885 390//390
+f 387//387 386//386 388//388
+f 385//385 375//375 389//389
+f 387//387 388//388 391//391
+f 389//389 375//375 390//390
+f 378//378 385//385 391//391
+f 385//385 55//55 391//391
+f 55//55 387//387 391//391
+f 48//48 389//389 406//406
+f 389//389 887//887 392//392
+f 887//887 389//389 390//390
+f 406//406 389//389 392//392
+f 887//887 403//403 392//392
+f 392//392 401//401 407//407
+f 393//393 394//394 398//398
+f 397//397 393//393 398//398
+f 398//398 394//394 396//396
+f 394//394 240//240 396//396
+f 240//240 395//395 396//396
+f 397//397 398//398 399//399
+f 402//402 407//407 393//393
+f 393//393 407//407 394//394
+f 407//407 401//401 394//394
+f 401//401 240//240 394//394
+f 399//399 422//422 415//415
+f 399//399 415//415 419//419
+f 398//398 396//396 422//422
+f 399//399 398//398 422//422
+f 422//422 396//396 400//400
+f 423//423 400//400 418//418
+f 422//422 400//400 423//423
+f 256//256 240//240 401//401
+f 404//404 402//402 393//393
+f 392//392 403//403 401//401
+f 403//403 256//256 401//401
+f 397//397 404//404 393//393
+f 405//405 203//203 418//418
+f 400//400 405//405 418//418
+f 395//395 405//405 400//400
+f 396//396 395//395 400//400
+f 406//406 392//392 407//407
+f 402//402 406//406 407//407
+f 408//408 397//397 399//399
+f 408//408 399//399 419//419
+f 402//402 67//67 406//406
+f 409//409 74//74 410//410
+f 409//409 73//73 74//74
+f 412//412 200//200 421//421
+f 412//412 421//421 411//411
+f 424//424 412//412 411//411
+f 424//424 411//411 414//414
+f 413//413 74//74 421//421
+f 410//410 74//74 413//413
+f 419//419 415//415 414//414
+f 415//415 423//423 425//425
+f 414//414 415//415 425//425
+f 411//411 419//419 414//414
+f 416//416 419//419 411//411
+f 421//421 417//417 411//411
+f 417//417 416//416 411//411
+f 74//74 417//417 421//421
+f 201//201 426//426 418//418
+f 203//203 201//201 418//418
+f 416//416 408//408 419//419
+f 420//420 416//416 417//417
+f 200//200 413//413 421//421
+f 424//424 414//414 425//425
+f 423//423 426//426 425//425
+f 426//426 423//423 418//418
+f 415//415 422//422 423//423
+f 205//205 424//424 425//425
+f 426//426 205//205 425//425
+f 429//429 428//428 436//436
+f 431//431 427//427 432//432
+f 849//849 427//427 430//430
+f 428//428 849//849 430//430
+f 435//435 429//429 436//436
+f 436//436 428//428 430//430
+f 431//431 432//432 456//456
+f 456//456 432//432 442//442
+f 433//433 429//429 434//434
+f 410//410 413//413 434//434
+f 435//435 410//410 429//429
+f 429//429 410//410 434//434
+f 79//79 436//436 449//449
+f 79//79 435//435 436//436
+f 195//195 429//429 433//433
+f 449//449 431//431 437//437
+f 431//431 456//456 437//437
+f 436//436 430//430 449//449
+f 430//430 431//431 449//449
+f 438//438 433//433 434//434
+f 413//413 438//438 434//434
+f 427//427 431//431 430//430
+f 450//450 451//451 439//439
+f 439//439 451//451 457//457
+f 457//457 451//451 440//440
+f 449//449 437//437 441//441
+f 440//440 441//441 455//455
+f 441//441 437//437 455//455
+f 437//437 456//456 455//455
+f 456//456 442//442 454//454
+f 443//443 653//653 452//452
+f 457//457 440//440 455//455
+f 451//451 89//89 440//440
+f 477//477 444//444 447//447
+f 444//444 445//445 446//446
+f 447//447 444//444 446//446
+f 447//447 446//446 488//488
+f 451//451 450//450 446//446
+f 446//446 450//450 488//488
+f 81//81 85//85 448//448
+f 440//440 448//448 441//441
+f 79//79 449//449 441//441
+f 88//88 89//89 445//445
+f 450//450 452//452 475//475
+f 81//81 448//448 440//440
+f 89//89 81//81 440//440
+f 439//439 649//649 654//654
+f 443//443 439//439 654//654
+f 448//448 79//79 441//441
+f 445//445 451//451 446//446
+f 450//450 443//443 452//452
+f 445//445 89//89 451//451
+f 452//452 653//653 501//501
+f 649//649 454//454 453//453
+f 454//454 442//442 453//453
+f 457//457 455//455 454//454
+f 450//450 439//439 443//443
+f 455//455 456//456 454//454
+f 439//439 457//457 649//649
+f 457//457 454//454 649//649
+f 460//460 515//515 458//458
+f 460//460 458//458 879//879
+f 515//515 461//461 466//466
+f 466//466 461//461 459//459
+f 515//515 460//460 461//461
+f 460//460 879//879 490//490
+f 461//461 460//460 493//493
+f 462//462 470//470 465//465
+f 475//475 462//462 463//463
+f 462//462 465//465 463//463
+f 459//459 461//461 484//484
+f 469//469 464//464 485//485
+f 465//465 470//470 469//469
+f 484//484 464//464 468//468
+f 464//464 469//469 468//468
+f 459//459 484//484 468//468
+f 466//466 459//459 467//467
+f 459//459 468//468 467//467
+f 467//467 468//468 516//516
+f 468//468 469//469 516//516
+f 469//469 470//470 516//516
+f 516//516 470//470 495//495
+f 495//495 470//470 500//500
+f 470//470 462//462 500//500
+f 461//461 493//493 484//484
+f 481//481 111//111 482//482
+f 472//472 481//481 473//473
+f 472//472 111//111 481//481
+f 471//471 473//473 483//483
+f 471//471 472//472 473//473
+f 474//474 483//483 478//478
+f 488//488 450//450 475//475
+f 452//452 501//501 500//500
+f 462//462 452//452 500//500
+f 475//475 452//452 462//462
+f 111//111 477//477 482//482
+f 118//118 489//489 872//872
+f 476//476 121//121 872//872
+f 477//477 447//447 482//482
+f 121//121 118//118 872//872
+f 474//474 471//471 483//483
+f 115//115 474//474 478//478
+f 479//479 115//115 478//478
+f 118//118 479//479 489//489
+f 482//482 447//447 487//487
+f 489//489 479//479 491//491
+f 485//485 482//482 487//487
+f 447//447 488//488 487//487
+f 491//491 478//478 480//480
+f 490//490 489//489 491//491
+f 481//481 485//485 486//486
+f 479//479 478//478 491//491
+f 483//483 473//473 486//486
+f 473//473 481//481 486//486
+f 480//480 478//478 492//492
+f 483//483 486//486 492//492
+f 485//485 481//481 482//482
+f 478//478 483//483 492//492
+f 486//486 464//464 492//492
+f 464//464 484//484 492//492
+f 485//485 464//464 486//486
+f 465//465 485//485 487//487
+f 465//465 469//469 485//485
+f 488//488 463//463 487//487
+f 463//463 465//465 487//487
+f 488//488 475//475 463//463
+f 490//490 879//879 873//873
+f 460//460 490//490 493//493
+f 489//489 490//490 873//873
+f 872//872 489//489 873//873
+f 493//493 491//491 480//480
+f 493//493 490//490 491//491
+f 484//484 480//480 492//492
+f 484//484 493//493 480//480
+f 508//508 494//494 497//497
+f 508//508 497//497 496//496
+f 508//508 506//506 638//638
+f 494//494 508//508 638//638
+f 495//495 505//505 514//514
+f 508//508 496//496 505//505
+f 505//505 496//496 514//514
+f 496//496 518//518 499//499
+f 518//518 497//497 636//636
+f 466//466 467//467 499//499
+f 466//466 499//499 498//498
+f 497//497 494//494 626//626
+f 514//514 496//496 499//499
+f 467//467 514//514 499//499
+f 496//496 497//497 518//518
+f 500//500 501//501 507//507
+f 507//507 501//501 502//502
+f 638//638 506//506 503//503
+f 638//638 503//503 504//504
+f 505//505 495//495 507//507
+f 506//506 505//505 507//507
+f 506//506 508//508 505//505
+f 506//506 507//507 502//502
+f 506//506 502//502 503//503
+f 517//517 509//509 510//510
+f 510//510 509//509 511//511
+f 509//509 517//517 635//635
+f 509//509 613//613 526//526
+f 509//509 635//635 613//613
+f 510//510 511//511 529//529
+f 510//510 512//512 513//513
+f 510//510 529//529 512//512
+f 517//517 510//510 513//513
+f 516//516 495//495 514//514
+f 495//495 500//500 507//507
+f 458//458 515//515 531//531
+f 467//467 516//516 514//514
+f 635//635 517//517 629//629
+f 517//517 636//636 629//629
+f 636//636 517//517 513//513
+f 518//518 636//636 513//513
+f 498//498 518//518 513//513
+f 499//499 518//518 498//498
+f 515//515 466//466 498//498
+f 515//515 512//512 531//531
+f 515//515 498//498 512//512
+f 512//512 529//529 531//531
+f 512//512 498//498 513//513
+f 524//524 537//537 519//519
+f 523//523 520//520 532//532
+f 523//523 532//532 530//530
+f 523//523 519//519 520//520
+f 519//519 537//537 538//538
+f 524//524 525//525 521//521
+f 521//521 525//525 522//522
+f 525//525 526//526 612//612
+f 522//522 525//525 612//612
+f 526//526 613//613 612//612
+f 524//524 519//519 523//523
+f 524//524 523//523 530//530
+f 524//524 530//530 525//525
+f 530//530 526//526 525//525
+f 537//537 524//524 521//521
+f 511//511 526//526 530//530
+f 511//511 509//509 526//526
+f 880//880 871//871 527//527
+f 531//531 532//532 528//528
+f 531//531 529//529 532//532
+f 532//532 529//529 530//530
+f 458//458 531//531 528//528
+f 529//529 511//511 530//530
+f 528//528 520//520 533//533
+f 520//520 519//519 533//533
+f 532//532 520//520 528//528
+f 458//458 528//528 869//869
+f 871//871 538//538 527//527
+f 538//538 871//871 533//533
+f 533//533 871//871 534//534
+f 519//519 538//538 533//533
+f 528//528 533//533 869//869
+f 1493//1493 539//539 535//535
+f 535//535 539//539 536//536
+f 535//535 536//536 590//590
+f 863//863 1493//1493 866//866
+f 539//539 1493//1493 863//863
+f 536//536 537//537 591//591
+f 537//537 521//521 591//591
+f 538//538 537//537 536//536
+f 539//539 527//527 536//536
+f 527//527 538//538 536//536
+f 880//880 539//539 863//863
+f 880//880 527//527 539//539
+f 880//880 863//863 881//881
+f 590//590 536//536 591//591
+f 535//535 590//590 1100//1100
+f 587//587 540//540 1088//1088
+f 583//583 541//541 542//542
+f 542//542 541//541 543//543
+f 542//542 543//543 1477//1477
+f 543//543 541//541 546//546
+f 545//545 555//555 548//548
+f 545//545 739//739 544//544
+f 548//548 555//555 554//554
+f 545//545 548//548 739//739
+f 548//548 554//554 549//549
+f 550//550 1301//1301 1287//1287
+f 550//550 1287//1287 552//552
+f 546//546 569//569 547//547
+f 547//547 569//569 578//578
+f 544//544 738//738 551//551
+f 550//550 552//552 551//551
+f 1301//1301 550//550 735//735
+f 548//548 549//549 701//701
+f 544//544 739//739 738//738
+f 738//738 550//550 551//551
+f 550//550 738//738 735//735
+f 544//544 551//551 575//575
+f 545//545 544//544 571//571
+f 544//544 575//575 574//574
+f 555//555 545//545 572//572
+f 546//546 541//541 569//569
+f 543//543 546//546 1241//1241
+f 546//546 547//547 1241//1241
+f 552//552 1287//1287 547//547
+f 547//547 1287//1287 1241//1241
+f 551//551 552//552 553//553
+f 575//575 551//551 553//553
+f 552//552 547//547 578//578
+f 552//552 578//578 553//553
+f 545//545 571//571 572//572
+f 554//554 555//555 681//681
+f 681//681 555//555 570//570
+f 555//555 572//572 570//570
+f 556//556 557//557 558//558
+f 559//559 579//579 560//560
+f 561//561 560//560 562//562
+f 560//560 561//561 693//693
+f 561//561 574//574 565//565
+f 574//574 561//561 562//562
+f 564//564 566//566 567//567
+f 565//565 574//574 566//566
+f 567//567 577//577 576//576
+f 567//567 566//566 577//577
+f 556//556 558//558 580//580
+f 679//679 568//568 637//637
+f 568//568 565//565 563//563
+f 637//637 568//568 563//563
+f 564//564 565//565 566//566
+f 576//576 556//556 580//580
+f 567//567 576//576 580//580
+f 559//559 560//560 693//693
+f 561//561 565//565 568//568
+f 693//693 561//561 568//568
+f 578//578 569//569 584//584
+f 571//571 544//544 574//574
+f 571//571 574//574 562//562
+f 681//681 570//570 682//682
+f 682//682 570//570 573//573
+f 570//570 572//572 573//573
+f 572//572 571//571 562//562
+f 572//572 562//562 573//573
+f 566//566 575//575 577//577
+f 574//574 575//575 566//566
+f 557//557 556//556 584//584
+f 556//556 576//576 584//584
+f 553//553 578//578 577//577
+f 575//575 553//553 577//577
+f 576//576 578//578 584//584
+f 577//577 578//578 576//576
+f 579//579 682//682 573//573
+f 562//562 560//560 573//573
+f 560//560 579//579 573//573
+f 563//563 564//564 608//608
+f 580//580 558//558 609//609
+f 567//567 580//580 581//581
+f 565//565 564//564 563//563
+f 608//608 564//564 581//581
+f 564//564 567//567 581//581
+f 542//542 1477//1477 582//582
+f 582//582 1477//1477 1478//1478
+f 609//609 558//558 594//594
+f 583//583 542//542 582//582
+f 557//557 582//582 585//585
+f 557//557 583//583 582//582
+f 583//583 557//557 584//584
+f 541//541 583//583 569//569
+f 569//569 583//583 584//584
+f 540//540 587//587 586//586
+f 594//594 558//558 585//585
+f 558//558 557//557 585//585
+f 585//585 1478//1478 586//586
+f 582//582 1478//1478 585//585
+f 587//587 594//594 586//586
+f 594//594 585//585 586//586
+f 595//595 593//593 605//605
+f 605//605 591//591 599//599
+f 1100//1100 588//588 589//589
+f 1100//1100 590//590 588//588
+f 591//591 521//521 599//599
+f 1088//1088 589//589 592//592
+f 587//587 1088//1088 592//592
+f 589//589 588//588 592//592
+f 587//587 592//592 596//596
+f 592//592 593//593 596//596
+f 588//588 593//593 592//592
+f 594//594 587//587 596//596
+f 609//609 594//594 596//596
+f 590//590 591//591 605//605
+f 595//595 609//609 596//596
+f 593//593 595//595 596//596
+f 605//605 593//593 588//588
+f 590//590 605//605 588//588
+f 606//606 637//637 597//597
+f 606//606 610//610 601//601
+f 606//606 597//597 610//610
+f 610//610 598//598 607//607
+f 609//609 595//595 598//598
+f 595//595 599//599 611//611
+f 601//601 607//607 603//603
+f 600//600 601//601 602//602
+f 601//601 603//603 602//602
+f 607//607 611//611 604//604
+f 603//603 607//607 604//604
+f 595//595 605//605 599//599
+f 606//606 601//601 600//600
+f 601//601 610//610 607//607
+f 607//607 598//598 611//611
+f 598//598 595//595 611//611
+f 563//563 608//608 597//597
+f 603//603 613//613 602//602
+f 521//521 522//522 599//599
+f 599//599 522//522 611//611
+f 580//580 609//609 581//581
+f 581//581 609//609 598//598
+f 610//610 608//608 598//598
+f 608//608 581//581 598//598
+f 597//597 608//608 610//610
+f 637//637 563//563 597//597
+f 522//522 612//612 604//604
+f 611//611 522//522 604//604
+f 612//612 613//613 603//603
+f 612//612 603//603 604//604
+f 630//630 623//623 616//616
+f 630//630 616//616 619//619
+f 680//680 679//679 614//614
+f 615//615 616//616 622//622
+f 616//616 615//615 617//617
+f 623//623 628//628 618//618
+f 618//618 628//628 689//689
+f 618//618 689//689 686//686
+f 629//629 630//630 619//619
+f 635//635 629//629 619//619
+f 618//618 686//686 620//620
+f 622//622 618//618 620//620
+f 686//686 680//680 620//620
+f 635//635 619//619 617//617
+f 619//619 616//616 617//617
+f 620//620 614//614 621//621
+f 680//680 614//614 620//620
+f 622//622 620//620 621//621
+f 615//615 622//622 621//621
+f 622//622 623//623 618//618
+f 616//616 623//623 622//622
+f 628//628 623//623 633//633
+f 494//494 624//624 627//627
+f 633//633 625//625 632//632
+f 627//627 624//624 625//625
+f 626//626 494//494 627//627
+f 627//627 625//625 631//631
+f 626//626 627//627 631//631
+f 689//689 628//628 676//676
+f 629//629 626//626 631//631
+f 630//630 629//629 631//631
+f 633//633 630//630 631//631
+f 623//623 630//630 633//633
+f 625//625 633//633 631//631
+f 628//628 632//632 676//676
+f 628//628 633//633 632//632
+f 615//615 602//602 617//617
+f 600//600 602//602 615//615
+f 636//636 497//497 626//626
+f 600//600 615//615 621//621
+f 494//494 638//638 624//624
+f 504//504 503//503 659//659
+f 624//624 504//504 634//634
+f 504//504 659//659 641//641
+f 602//602 613//613 617//617
+f 613//613 635//635 617//617
+f 636//636 626//626 629//629
+f 606//606 600//600 621//621
+f 614//614 606//606 621//621
+f 679//679 637//637 614//614
+f 637//637 606//606 614//614
+f 638//638 504//504 624//624
+f 640//640 641//641 639//639
+f 676//676 632//632 640//640
+f 625//625 624//624 634//634
+f 634//634 504//504 641//641
+f 632//632 634//634 640//640
+f 634//634 641//641 640//640
+f 632//632 625//625 634//634
+f 658//658 647//647 642//642
+f 647//647 658//658 643//643
+f 644//644 643//643 669//669
+f 643//643 658//658 648//648
+f 669//669 643//643 648//648
+f 642//642 502//502 656//656
+f 644//644 669//669 645//645
+f 644//644 645//645 662//662
+f 644//644 662//662 646//646
+f 662//662 645//645 667//667
+f 502//502 642//642 660//660
+f 642//642 647//647 660//660
+f 643//643 644//644 646//646
+f 647//647 643//643 646//646
+f 660//660 647//647 661//661
+f 647//647 646//646 661//661
+f 654//654 649//649 648//648
+f 649//649 669//669 648//648
+f 639//639 641//641 651//651
+f 639//639 651//651 650//650
+f 641//641 659//659 652//652
+f 651//651 641//641 652//652
+f 659//659 661//661 652//652
+f 501//501 653//653 656//656
+f 653//653 443//443 655//655
+f 656//656 653//653 655//655
+f 443//443 654//654 655//655
+f 654//654 648//648 657//657
+f 642//642 656//656 657//657
+f 656//656 655//655 657//657
+f 658//658 642//642 657//657
+f 648//648 658//658 657//657
+f 659//659 503//503 660//660
+f 659//659 660//660 661//661
+f 655//655 654//654 657//657
+f 502//502 501//501 656//656
+f 503//503 502//502 660//660
+f 646//646 662//662 652//652
+f 661//661 646//646 652//652
+f 662//662 667//667 651//651
+f 662//662 651//651 652//652
+f 651//651 667//667 650//650
+f 432//432 427//427 858//858
+f 707//707 692//692 691//691
+f 663//663 860//860 664//664
+f 860//860 668//668 665//665
+f 665//665 858//858 859//859
+f 442//442 432//432 858//858
+f 639//639 650//650 690//690
+f 860//860 665//665 859//859
+f 707//707 663//663 711//711
+f 663//663 664//664 711//711
+f 860//860 663//663 668//668
+f 442//442 858//858 665//665
+f 645//645 669//669 668//668
+f 668//668 453//453 665//665
+f 663//663 691//691 666//666
+f 650//650 667//667 666//666
+f 645//645 668//668 666//666
+f 667//667 645//645 666//666
+f 691//691 650//650 666//666
+f 650//650 691//691 690//690
+f 453//453 442//442 665//665
+f 669//669 453//453 668//668
+f 668//668 663//663 666//666
+f 707//707 691//691 663//663
+f 649//649 453//453 669//669
+f 687//687 670//670 671//671
+f 671//671 683//683 696//696
+f 684//684 688//688 672//672
+f 678//678 672//672 673//673
+f 685//685 673//673 674//674
+f 678//678 673//673 685//685
+f 683//683 671//671 716//716
+f 716//716 678//678 709//709
+f 675//675 692//692 706//706
+f 674//674 673//673 677//677
+f 687//687 671//671 696//696
+f 692//692 685//685 674//674
+f 684//684 678//678 716//716
+f 639//639 690//690 677//677
+f 671//671 684//684 716//716
+f 685//685 675//675 712//712
+f 640//640 677//677 695//695
+f 676//676 640//640 695//695
+f 678//678 685//685 712//712
+f 640//640 639//639 677//677
+f 709//709 678//678 712//712
+f 679//679 680//680 694//694
+f 681//681 682//682 683//683
+f 682//682 579//579 683//683
+f 684//684 672//672 678//678
+f 685//685 692//692 675//675
+f 680//680 686//686 687//687
+f 688//688 676//676 695//695
+f 689//689 676//676 688//688
+f 679//679 693//693 568//568
+f 696//696 559//559 694//694
+f 559//559 693//693 694//694
+f 579//579 559//559 696//696
+f 683//683 579//579 696//696
+f 690//690 674//674 677//677
+f 690//690 691//691 674//674
+f 691//691 692//692 674//674
+f 693//693 679//679 694//694
+f 677//677 673//673 695//695
+f 684//684 670//670 688//688
+f 671//671 670//670 684//684
+f 687//687 686//686 670//670
+f 686//686 689//689 670//670
+f 673//673 672//672 695//695
+f 672//672 688//688 695//695
+f 680//680 687//687 694//694
+f 692//692 707//707 706//706
+f 670//670 689//689 688//688
+f 687//687 696//696 694//694
+f 699//699 697//697 700//700
+f 714//714 699//699 698//698
+f 699//699 700//700 727//727
+f 701//701 549//549 702//702
+f 698//698 703//703 704//704
+f 698//698 699//699 703//703
+f 704//704 549//549 705//705
+f 698//698 704//704 705//705
+f 707//707 711//711 697//697
+f 706//706 697//697 714//714
+f 706//706 707//707 697//697
+f 709//709 713//713 708//708
+f 709//709 708//708 715//715
+f 705//705 713//713 710//710
+f 705//705 549//549 708//708
+f 697//697 711//711 700//700
+f 697//697 699//699 714//714
+f 698//698 705//705 710//710
+f 714//714 698//698 710//710
+f 713//713 705//705 708//708
+f 712//712 675//675 710//710
+f 713//713 712//712 710//710
+f 675//675 706//706 714//714
+f 675//675 714//714 710//710
+f 702//702 549//549 704//704
+f 703//703 699//699 724//724
+f 554//554 681//681 715//715
+f 708//708 554//554 715//715
+f 681//681 683//683 715//715
+f 549//549 554//554 708//708
+f 683//683 716//716 715//715
+f 716//716 709//709 715//715
+f 709//709 712//712 713//713
+f 727//727 700//700 853//853
+f 699//699 727//727 724//724
+f 718//718 717//717 733//733
+f 717//717 718//718 725//725
+f 718//718 733//733 719//719
+f 725//725 720//720 844//844
+f 717//717 725//725 844//844
+f 701//701 702//702 719//719
+f 723//723 721//721 846//846
+f 720//720 723//723 846//846
+f 724//724 720//720 722//722
+f 720//720 725//725 722//722
+f 723//723 727//727 814//814
+f 721//721 723//723 814//814
+f 702//702 718//718 719//719
+f 718//718 702//702 726//726
+f 733//733 717//717 741//741
+f 703//703 724//724 722//722
+f 703//703 722//722 726//726
+f 724//724 727//727 723//723
+f 720//720 724//724 723//723
+f 702//702 704//704 726//726
+f 704//704 703//703 726//726
+f 725//725 718//718 726//726
+f 722//722 725//725 726//726
+f 727//727 853//853 814//814
+f 719//719 731//731 730//730
+f 731//731 728//728 736//736
+f 728//728 731//731 729//729
+f 729//729 737//737 734//734
+f 728//728 729//729 734//734
+f 701//701 719//719 730//730
+f 730//730 731//731 736//736
+f 738//738 730//730 736//736
+f 719//719 733//733 731//731
+f 731//731 733//733 729//729
+f 739//739 701//701 730//730
+f 739//739 548//548 701//701
+f 729//729 741//741 737//737
+f 736//736 728//728 732//732
+f 733//733 741//741 729//729
+f 734//734 1296//1296 1298//1298
+f 728//728 734//734 1298//1298
+f 735//735 736//736 732//732
+f 734//734 737//737 1383//1383
+f 732//732 728//728 1298//1298
+f 1301//1301 735//735 732//732
+f 735//735 738//738 736//736
+f 738//738 739//739 730//730
+f 1296//1296 734//734 1383//1383
+f 740//740 741//741 748//748
+f 742//742 743//743 746//746
+f 746//746 749//749 757//757
+f 746//746 757//757 744//744
+f 746//746 743//743 749//749
+f 746//746 744//744 745//745
+f 742//742 746//746 745//745
+f 743//743 742//742 747//747
+f 757//757 749//749 750//750
+f 743//743 740//740 749//749
+f 1383//1383 743//743 747//747
+f 737//737 741//741 740//740
+f 1383//1383 737//737 740//740
+f 1383//1383 740//740 743//743
+f 740//740 748//748 749//749
+f 749//749 748//748 750//750
+f 742//742 745//745 752//752
+f 1367//1367 759//759 762//762
+f 744//744 757//757 751//751
+f 752//752 745//745 753//753
+f 757//757 750//750 841//841
+f 745//745 744//744 753//753
+f 759//759 1400//1400 1393//1393
+f 760//760 754//754 755//755
+f 754//754 828//828 835//835
+f 759//759 1367//1367 1400//1400
+f 753//753 761//761 1372//1372
+f 761//761 1393//1393 1372//1372
+f 762//762 759//759 756//756
+f 759//759 761//761 756//756
+f 761//761 755//755 756//756
+f 835//835 762//762 756//756
+f 755//755 754//754 756//756
+f 754//754 835//835 756//756
+f 751//751 757//757 841//841
+f 762//762 835//835 766//766
+f 828//828 754//754 758//758
+f 754//754 760//760 758//758
+f 752//752 753//753 1372//1372
+f 759//759 1393//1393 761//761
+f 751//751 760//760 755//755
+f 753//753 755//755 761//761
+f 751//751 841//841 760//760
+f 753//753 744//744 755//755
+f 744//744 751//751 755//755
+f 766//766 835//835 768//768
+f 762//762 766//766 763//763
+f 1367//1367 763//763 771//771
+f 1367//1367 762//762 763//763
+f 764//764 777//777 786//786
+f 773//773 778//778 823//823
+f 764//764 786//786 769//769
+f 771//771 763//763 774//774
+f 763//763 766//766 765//765
+f 774//774 763//763 765//765
+f 766//766 768//768 767//767
+f 765//765 766//766 767//767
+f 768//768 835//835 824//824
+f 768//768 824//824 776//776
+f 764//764 769//769 775//775
+f 767//767 768//768 776//776
+f 769//769 799//799 775//775
+f 778//778 780//780 779//779
+f 770//770 771//771 774//774
+f 780//780 773//773 772//772
+f 778//778 773//773 780//780
+f 774//774 765//765 775//775
+f 764//764 765//765 767//767
+f 765//765 764//764 775//775
+f 777//777 767//767 776//776
+f 777//777 764//764 767//767
+f 824//824 823//823 776//776
+f 770//770 1365//1365 771//771
+f 778//778 777//777 776//776
+f 799//799 145//145 775//775
+f 823//823 778//778 776//776
+f 777//777 778//778 779//779
+f 145//145 774//774 775//775
+f 145//145 770//770 774//774
+f 165//165 164//164 791//791
+f 165//165 791//791 789//789
+f 769//769 786//786 792//792
+f 786//786 777//777 779//779
+f 791//791 164//164 783//783
+f 779//779 780//780 785//785
+f 164//164 781//781 784//784
+f 780//780 772//772 785//785
+f 769//769 792//792 794//794
+f 783//783 164//164 784//784
+f 787//787 789//789 790//790
+f 782//782 787//787 790//790
+f 792//792 783//783 784//784
+f 792//792 786//786 783//783
+f 785//785 772//772 790//790
+f 786//786 779//779 783//783
+f 799//799 769//769 794//794
+f 787//787 165//165 789//789
+f 790//790 772//772 788//788
+f 789//789 791//791 785//785
+f 789//789 785//785 790//790
+f 779//779 791//791 783//783
+f 791//791 779//779 785//785
+f 794//794 792//792 795//795
+f 146//146 799//799 798//798
+f 159//159 796//796 793//793
+f 158//158 159//159 793//793
+f 795//795 796//796 800//800
+f 796//796 795//795 793//793
+f 160//160 163//163 801//801
+f 784//784 158//158 793//793
+f 792//792 784//784 793//793
+f 794//794 795//795 800//800
+f 797//797 800//800 801//801
+f 781//781 158//158 784//784
+f 159//159 160//160 796//796
+f 796//796 160//160 801//801
+f 797//797 798//798 800//800
+f 798//798 794//794 800//800
+f 798//798 799//799 794//794
+f 795//795 792//792 793//793
+f 800//800 796//796 801//801
+f 802//802 805//805 804//804
+f 782//782 790//790 788//788
+f 804//804 822//822 803//803
+f 804//804 805//805 822//822
+f 822//822 805//805 810//810
+f 185//185 802//802 806//806
+f 802//802 804//804 806//806
+f 194//194 185//185 806//806
+f 804//804 803//803 806//806
+f 782//782 788//788 807//807
+f 808//808 782//782 807//807
+f 809//809 802//802 185//185
+f 802//802 175//175 805//805
+f 810//810 805//805 807//807
+f 806//806 803//803 821//821
+f 194//194 806//806 821//821
+f 805//805 175//175 807//807
+f 788//788 810//810 807//807
+f 175//175 808//808 807//807
+f 194//194 821//821 812//812
+f 813//813 827//827 825//825
+f 813//813 817//817 811//811
+f 811//811 817//817 857//857
+f 812//812 813//813 811//811
+f 813//813 812//812 820//820
+f 827//827 813//813 820//820
+f 814//814 815//815 816//816
+f 857//857 817//817 815//815
+f 816//816 825//825 834//834
+f 813//813 825//825 817//817
+f 814//814 853//853 815//815
+f 721//721 814//814 816//816
+f 721//721 816//816 834//834
+f 812//812 821//821 820//820
+f 815//815 817//817 816//816
+f 817//817 825//825 816//816
+f 818//818 810//810 838//838
+f 788//788 772//772 838//838
+f 848//848 832//832 836//836
+f 832//832 848//848 819//819
+f 820//820 821//821 831//831
+f 822//822 810//810 818//818
+f 810//810 788//788 838//838
+f 822//822 818//818 826//826
+f 823//823 824//824 837//837
+f 825//825 827//827 829//829
+f 836//836 818//818 833//833
+f 818//818 836//836 826//826
+f 772//772 773//773 838//838
+f 827//827 832//832 829//829
+f 824//824 828//828 837//837
+f 834//834 829//829 830//830
+f 836//836 832//832 826//826
+f 823//823 837//837 833//833
+f 846//846 834//834 830//830
+f 821//821 803//803 831//831
+f 832//832 827//827 831//831
+f 803//803 822//822 826//826
+f 832//832 831//831 826//826
+f 831//831 803//803 826//826
+f 773//773 823//823 833//833
+f 838//838 773//773 833//833
+f 827//827 820//820 831//831
+f 829//829 819//819 830//830
+f 834//834 825//825 829//829
+f 835//835 828//828 824//824
+f 837//837 836//836 833//833
+f 848//848 836//836 837//837
+f 818//818 838//838 833//833
+f 832//832 819//819 829//829
+f 842//842 840//840 847//847
+f 842//842 841//841 840//840
+f 750//750 748//748 839//839
+f 717//717 844//844 839//839
+f 758//758 760//760 842//842
+f 840//840 750//750 839//839
+f 841//841 750//750 840//840
+f 760//760 841//841 842//842
+f 846//846 721//721 834//834
+f 748//748 717//717 839//839
+f 741//741 717//717 748//748
+f 819//819 848//848 847//847
+f 837//837 828//828 845//845
+f 819//819 847//847 843//843
+f 830//830 819//819 843//843
+f 839//839 844//844 843//843
+f 842//842 847//847 845//845
+f 758//758 842//842 845//845
+f 840//840 839//839 843//843
+f 847//847 840//840 843//843
+f 844//844 830//830 843//843
+f 844//844 720//720 846//846
+f 844//844 846//846 830//830
+f 828//828 758//758 845//845
+f 847//847 848//848 845//845
+f 848//848 837//837 845//845
+f 861//861 849//849 852//852
+f 849//849 855//855 852//852
+f 856//856 811//811 857//857
+f 850//850 854//854 851//851
+f 854//854 861//861 851//851
+f 852//852 853//853 851//851
+f 856//856 812//812 811//811
+f 856//856 194//194 812//812
+f 427//427 849//849 861//861
+f 859//859 861//861 854//854
+f 850//850 664//664 854//854
+f 711//711 664//664 850//850
+f 815//815 853//853 852//852
+f 857//857 815//815 852//852
+f 861//861 852//852 851//851
+f 855//855 857//857 852//852
+f 855//855 856//856 857//857
+f 858//858 427//427 861//861
+f 853//853 700//700 851//851
+f 700//700 850//850 851//851
+f 700//700 711//711 850//850
+f 860//860 859//859 854//854
+f 664//664 860//860 854//854
+f 859//859 858//858 861//861
+f 862//862 194//194 856//856
+f 863//863 866//866 881//881
+f 137//137 864//864 1504//1504
+f 864//864 865//865 1504//1504
+f 881//881 866//866 867//867
+f 867//867 1504//1504 868//868
+f 1504//1504 865//865 868//868
+f 881//881 867//867 868//868
+f 879//879 458//458 869//869
+f 879//879 869//869 870//870
+f 871//871 880//880 882//882
+f 872//872 873//873 877//877
+f 869//869 533//533 534//534
+f 869//869 534//534 870//870
+f 877//877 873//873 876//876
+f 868//868 874//874 875//875
+f 874//874 876//876 875//875
+f 873//873 870//870 876//876
+f 868//868 865//865 874//874
+f 877//877 876//876 878//878
+f 876//876 874//874 878//878
+f 873//873 879//879 870//870
+f 876//876 870//870 875//875
+f 881//881 868//868 882//882
+f 882//882 868//868 875//875
+f 534//534 882//882 875//875
+f 870//870 534//534 875//875
+f 865//865 130//130 878//878
+f 874//874 865//865 878//878
+f 883//883 877//877 878//878
+f 883//883 476//476 877//877
+f 476//476 872//872 877//877
+f 880//880 881//881 882//882
+f 130//130 883//883 878//878
+f 534//534 871//871 882//882
+f 885//885 886//886 390//390
+f 252//252 884//884 888//888
+f 251//251 884//884 252//252
+f 249//249 887//887 886//886
+f 884//884 886//886 885//885
+f 886//886 887//887 390//390
+f 888//888 884//884 332//332
+f 332//332 884//884 885//885
+f 374//374 332//332 885//885
+f 900//900 5//5 889//889
+f 5//5 1//1 889//889
+f 891//891 3//3 895//895
+f 3//3 904//904 895//895
+f 904//904 907//907 890//890
+f 1//1 3//3 891//891
+f 889//889 1//1 891//891
+f 134//134 136//136 898//898
+f 902//902 898//898 897//897
+f 902//902 134//134 898//898
+f 134//134 892//892 136//136
+f 895//895 893//893 896//896
+f 136//136 894//894 899//899
+f 891//891 896//896 899//899
+f 891//891 895//895 896//896
+f 897//897 898//898 896//896
+f 893//893 897//897 896//896
+f 898//898 136//136 899//899
+f 896//896 898//898 899//899
+f 894//894 900//900 889//889
+f 894//894 889//889 899//899
+f 889//889 891//891 899//899
+f 105//105 123//123 934//934
+f 934//934 123//123 903//903
+f 934//934 903//903 901//901
+f 903//903 893//893 901//901
+f 925//925 901//901 905//905
+f 934//934 901//901 925//925
+f 902//902 897//897 903//903
+f 123//123 902//902 903//903
+f 904//904 906//906 907//907
+f 895//895 904//904 890//890
+f 897//897 893//893 903//903
+f 901//901 890//890 905//905
+f 893//893 890//890 901//901
+f 895//895 890//890 893//893
+f 890//890 907//907 905//905
+f 906//906 6//6 952//952
+f 905//905 907//907 915//915
+f 952//952 908//908 909//909
+f 908//908 910//910 911//911
+f 913//913 942//942 927//927
+f 913//913 927//927 920//920
+f 913//913 920//920 912//912
+f 912//912 920//920 921//921
+f 908//908 952//952 910//910
+f 915//915 952//952 909//909
+f 942//942 913//913 960//960
+f 960//960 913//913 968//968
+f 913//913 912//912 968//968
+f 912//912 911//911 968//968
+f 908//908 911//911 912//912
+f 905//905 915//915 914//914
+f 925//925 905//905 914//914
+f 908//908 912//912 921//921
+f 909//909 908//908 921//921
+f 919//919 918//918 933//933
+f 906//906 952//952 915//915
+f 907//907 906//906 915//915
+f 921//921 917//917 916//916
+f 916//916 931//931 930//930
+f 917//917 931//931 916//916
+f 916//916 930//930 926//926
+f 918//918 919//919 924//924
+f 924//924 919//919 944//944
+f 920//920 919//919 933//933
+f 920//920 927//927 919//919
+f 920//920 933//933 917//917
+f 921//921 920//920 917//917
+f 914//914 909//909 922//922
+f 909//909 921//921 922//922
+f 915//915 909//909 914//914
+f 916//916 926//926 922//922
+f 921//921 916//916 922//922
+f 926//926 914//914 922//922
+f 923//923 918//918 924//924
+f 930//930 934//934 926//926
+f 925//925 914//914 926//926
+f 919//919 927//927 944//944
+f 934//934 925//925 926//926
+f 927//927 942//942 944//944
+f 928//928 929//929 933//933
+f 933//933 929//929 917//917
+f 101//101 931//931 929//929
+f 929//929 931//931 917//917
+f 932//932 105//105 930//930
+f 931//931 932//932 930//930
+f 923//923 104//104 918//918
+f 104//104 928//928 918//918
+f 918//918 928//928 933//933
+f 105//105 934//934 930//930
+f 943//943 946//946 935//935
+f 946//946 940//940 936//936
+f 946//946 936//936 937//937
+f 935//935 946//946 937//937
+f 92//92 923//923 938//938
+f 923//923 924//924 938//938
+f 90//90 92//92 938//938
+f 936//936 90//90 938//938
+f 939//939 90//90 936//936
+f 941//941 93//93 939//939
+f 941//941 939//939 936//936
+f 940//940 941//941 936//936
+f 937//937 936//936 938//938
+f 942//942 943//943 935//935
+f 924//924 937//937 938//938
+f 924//924 944//944 937//937
+f 944//944 942//942 935//935
+f 944//944 935//935 937//937
+f 943//943 970//970 945//945
+f 54//54 970//970 958//958
+f 946//946 943//943 945//945
+f 71//71 940//940 945//945
+f 940//940 946//946 945//945
+f 71//71 945//945 947//947
+f 970//970 54//54 947//947
+f 945//945 970//970 947//947
+f 50//50 948//948 949//949
+f 948//948 54//54 949//949
+f 54//54 958//958 949//949
+f 949//949 958//958 972//972
+f 62//62 71//71 947//947
+f 52//52 62//62 947//947
+f 54//54 52//52 947//947
+f 950//950 50//50 949//949
+f 943//943 960//960 971//971
+f 958//958 970//970 971//971
+f 969//969 965//965 955//955
+f 969//969 11//11 965//965
+f 965//965 954//954 963//963
+f 910//910 955//955 951//951
+f 952//952 6//6 956//956
+f 27//27 953//953 954//954
+f 910//910 952//952 956//956
+f 955//955 910//910 956//956
+f 969//969 955//955 956//956
+f 960//960 968//968 961//961
+f 975//975 959//959 964//964
+f 964//964 959//959 966//966
+f 966//966 959//959 962//962
+f 966//966 962//962 957//957
+f 972//972 958//958 967//967
+f 959//959 975//975 967//967
+f 975//975 972//972 967//967
+f 971//971 962//962 967//967
+f 962//962 959//959 967//967
+f 971//971 960//960 961//961
+f 971//971 961//961 962//962
+f 962//962 961//961 957//957
+f 955//955 965//965 957//957
+f 951//951 955//955 957//957
+f 965//965 966//966 957//957
+f 968//968 951//951 961//961
+f 961//961 951//951 957//957
+f 963//963 964//964 966//966
+f 965//965 963//963 966//966
+f 950//950 949//949 972//972
+f 958//958 971//971 967//967
+f 911//911 910//910 951//951
+f 968//968 911//911 951//951
+f 942//942 960//960 943//943
+f 954//954 953//953 963//963
+f 19//19 27//27 954//954
+f 965//965 11//11 954//954
+f 11//11 19//19 954//954
+f 8//8 11//11 969//969
+f 6//6 969//969 956//956
+f 6//6 8//8 969//969
+f 970//970 943//943 971//971
+f 974//974 950//950 972//972
+f 963//963 973//973 964//964
+f 953//953 973//973 963//963
+f 973//973 975//975 964//964
+f 974//974 972//972 975//975
+f 973//973 974//974 975//975
+f 989//989 1002//1002 976//976
+f 976//976 1002//1002 977//977
+f 989//989 1005//1005 1004//1004
+f 977//977 1539//1539 978//978
+f 1006//1006 979//979 997//997
+f 997//997 979//979 980//980
+f 980//980 979//979 981//981
+f 980//980 981//981 982//982
+f 983//983 981//981 991//991
+f 982//982 981//981 983//983
+f 979//979 1006//1006 990//990
+f 979//979 990//990 992//992
+f 981//981 979//979 992//992
+f 991//991 981//981 992//992
+f 982//982 1515//1515 1520//1520
+f 982//982 983//983 985//985
+f 1515//1515 982//982 985//985
+f 987//987 983//983 984//984
+f 985//985 983//983 987//987
+f 986//986 984//984 1541//1541
+f 987//987 984//984 986//986
+f 986//986 1541//1541 1512//1512
+f 983//983 991//991 984//984
+f 984//984 991//991 988//988
+f 1541//1541 984//984 988//988
+f 980//980 1520//1520 1506//1506
+f 980//980 982//982 1520//1520
+f 989//989 976//976 992//992
+f 1005//1005 989//989 990//990
+f 976//976 977//977 978//978
+f 990//990 989//989 992//992
+f 1006//1006 1005//1005 990//990
+f 991//991 978//978 988//988
+f 976//976 991//991 992//992
+f 976//976 978//978 991//991
+f 996//996 1498//1498 1001//1001
+f 1007//1007 994//994 993//993
+f 994//994 996//996 993//993
+f 996//996 994//994 995//995
+f 996//996 995//995 1483//1483
+f 1001//1001 1004//1004 993//993
+f 996//996 1001//1001 993//993
+f 1505//1505 1502//1502 998//998
+f 997//997 1506//1506 998//998
+f 1506//1506 1505//1505 998//998
+f 1007//1007 997//997 998//998
+f 995//995 994//994 1502//1502
+f 1502//1502 994//994 998//998
+f 994//994 1007//1007 998//998
+f 999//999 1000//1000 1013//1013
+f 1001//1001 1498//1498 1000//1000
+f 1000//1000 999//999 1008//1008
+f 1001//1001 1000//1000 1008//1008
+f 1002//1002 1004//1004 1008//1008
+f 1002//1002 989//989 1004//1004
+f 1002//1002 1008//1008 1003//1003
+f 1004//1004 1005//1005 993//993
+f 1005//1005 1006//1006 1007//1007
+f 1005//1005 1007//1007 993//993
+f 1006//1006 997//997 1007//1007
+f 997//997 980//980 1506//1506
+f 1004//1004 1001//1001 1008//1008
+f 1008//1008 999//999 1003//1003
+f 999//999 1010//1010 1009//1009
+f 1009//1009 1010//1010 1129//1129
+f 999//999 1013//1013 1010//1010
+f 1026//1026 1016//1016 1011//1011
+f 1026//1026 1011//1011 1014//1014
+f 1014//1014 1011//1011 1012//1012
+f 1009//1009 1014//1014 1012//1012
+f 1003//1003 999//999 1009//1009
+f 1014//1014 1129//1129 1015//1015
+f 1009//1009 1129//1129 1014//1014
+f 1010//1010 1013//1013 1122//1122
+f 1129//1129 1010//1010 1122//1122
+f 1026//1026 1014//1014 1015//1015
+f 977//977 1003//1003 1012//1012
+f 977//977 1002//1002 1003//1003
+f 1003//1003 1009//1009 1012//1012
+f 1011//1011 1016//1016 1023//1023
+f 1539//1539 1011//1011 1023//1023
+f 1011//1011 1539//1539 1012//1012
+f 1539//1539 977//977 1012//1012
+f 1017//1017 1018//1018 1019//1019
+f 1018//1018 1017//1017 1020//1020
+f 1018//1018 1020//1020 1028//1028
+f 1020//1020 1021//1021 1025//1025
+f 1025//1025 1021//1021 1022//1022
+f 1539//1539 1023//1023 1024//1024
+f 1023//1023 1017//1017 1024//1024
+f 1023//1023 1016//1016 1017//1017
+f 1017//1017 1016//1016 1020//1020
+f 1020//1020 1016//1016 1027//1027
+f 1021//1021 1020//1020 1027//1027
+f 1017//1017 1019//1019 1024//1024
+f 1016//1016 1026//1026 1027//1027
+f 1028//1028 1020//1020 1025//1025
+f 1026//1026 1015//1015 1027//1027
+f 1018//1018 1028//1028 1545//1545
+f 1545//1545 1028//1028 1031//1031
+f 1028//1028 1025//1025 1031//1031
+f 1545//1545 1031//1031 1033//1033
+f 1033//1033 1031//1031 1029//1029
+f 1227//1227 1030//1030 1032//1032
+f 1030//1030 1029//1029 1032//1032
+f 1031//1031 1025//1025 1032//1032
+f 1029//1029 1031//1031 1032//1032
+f 1025//1025 1022//1022 1032//1032
+f 1033//1033 1029//1029 1034//1034
+f 1029//1029 1030//1030 1036//1036
+f 1034//1034 1029//1029 1036//1036
+f 1034//1034 1035//1035 1039//1039
+f 1034//1034 1036//1036 1035//1035
+f 1545//1545 1033//1033 1623//1623
+f 1033//1033 1034//1034 1037//1037
+f 1034//1034 1039//1039 1037//1037
+f 1059//1059 1038//1038 1207//1207
+f 1623//1623 1033//1033 1037//1037
+f 1037//1037 1038//1038 1041//1041
+f 1037//1037 1041//1041 1618//1618
+f 1623//1623 1037//1037 1618//1618
+f 1039//1039 1035//1035 1209//1209
+f 1041//1041 1058//1058 1040//1040
+f 1041//1041 1059//1059 1058//1058
+f 1038//1038 1039//1039 1207//1207
+f 1038//1038 1059//1059 1041//1041
+f 1618//1618 1041//1041 1040//1040
+f 1037//1037 1039//1039 1038//1038
+f 1047//1047 1049//1049 1610//1610
+f 1045//1045 1042//1042 1043//1043
+f 1047//1047 1062//1062 1048//1048
+f 1062//1062 1047//1047 1044//1044
+f 1044//1044 1050//1050 1072//1072
+f 1045//1045 1044//1044 1046//1046
+f 1044//1044 1047//1047 1046//1046
+f 1044//1044 1043//1043 1050//1050
+f 1570//1570 1049//1049 1070//1070
+f 1049//1049 1047//1047 1048//1048
+f 1049//1049 1048//1048 1070//1070
+f 1045//1045 1043//1043 1044//1044
+f 1043//1043 1040//1040 1051//1051
+f 1050//1050 1043//1043 1051//1051
+f 1040//1040 1058//1058 1051//1051
+f 1047//1047 1610//1610 1046//1046
+f 1049//1049 1570//1570 1609//1609
+f 1055//1055 1197//1197 1203//1203
+f 1198//1198 1052//1052 1199//1199
+f 1053//1053 1061//1061 1063//1063
+f 1074//1074 1077//1077 1056//1056
+f 1051//1051 1058//1058 1054//1054
+f 1051//1051 1054//1054 1071//1071
+f 1054//1054 1199//1199 1071//1071
+f 1199//1199 1055//1055 1071//1071
+f 1197//1197 1055//1055 1199//1199
+f 1056//1056 1064//1064 1057//1057
+f 1203//1203 1197//1197 1204//1204
+f 1198//1198 1199//1199 1060//1060
+f 1199//1199 1054//1054 1060//1060
+f 1054//1054 1058//1058 1060//1060
+f 1058//1058 1059//1059 1060//1060
+f 1063//1063 1061//1061 1065//1065
+f 1065//1065 1061//1061 1073//1073
+f 1048//1048 1062//1062 1061//1061
+f 1061//1061 1062//1062 1073//1073
+f 1601//1601 1063//1063 1066//1066
+f 1063//1063 1065//1065 1066//1066
+f 1064//1064 1056//1056 1065//1065
+f 1072//1072 1067//1067 1069//1069
+f 1065//1065 1056//1056 1066//1066
+f 1064//1064 1065//1065 1073//1073
+f 1069//1069 1067//1067 1068//1068
+f 1057//1057 1069//1069 1068//1068
+f 1074//1074 1057//1057 1068//1068
+f 1070//1070 1048//1048 1591//1591
+f 1050//1050 1051//1051 1071//1071
+f 1071//1071 1055//1055 1067//1067
+f 1050//1050 1071//1071 1067//1067
+f 1061//1061 1053//1053 1591//1591
+f 1062//1062 1044//1044 1072//1072
+f 1055//1055 1203//1203 1068//1068
+f 1067//1067 1055//1055 1068//1068
+f 1203//1203 1074//1074 1068//1068
+f 1074//1074 1056//1056 1057//1057
+f 1048//1048 1061//1061 1591//1591
+f 1057//1057 1064//1064 1069//1069
+f 1064//1064 1073//1073 1069//1069
+f 1062//1062 1072//1072 1073//1073
+f 1073//1073 1072//1072 1069//1069
+f 1072//1072 1050//1050 1067//1067
+f 1077//1077 1074//1074 1078//1078
+f 1601//1601 1066//1066 1081//1081
+f 1066//1066 1077//1077 1081//1081
+f 1056//1056 1077//1077 1066//1066
+f 1604//1604 1075//1075 1076//1076
+f 1604//1604 1076//1076 1585//1585
+f 1082//1082 1078//1078 1079//1079
+f 1077//1077 1078//1078 1082//1082
+f 1074//1074 1203//1203 1193//1193
+f 1079//1079 1078//1078 1192//1192
+f 1075//1075 1604//1604 1600//1600
+f 1079//1079 368//368 1085//1085
+f 1078//1078 1193//1193 1192//1192
+f 1082//1082 1079//1079 1080//1080
+f 1081//1081 1077//1077 1082//1082
+f 1076//1076 1075//1075 1085//1085
+f 1085//1085 1075//1075 1080//1080
+f 1080//1080 1075//1075 1602//1602
+f 1081//1081 1080//1080 1602//1602
+f 1601//1601 1081//1081 1602//1602
+f 1079//1079 1085//1085 1080//1080
+f 1081//1081 1082//1082 1080//1080
+f 1075//1075 1600//1600 1602//1602
+f 1083//1083 1079//1079 1192//1192
+f 1083//1083 368//368 1079//1079
+f 1078//1078 1074//1074 1193//1193
+f 1602//1602 1600//1600 1599//1599
+f 1585//1585 1076//1076 1084//1084
+f 368//368 1086//1086 1085//1085
+f 1086//1086 1076//1076 1085//1085
+f 1076//1076 1086//1086 1084//1084
+f 1086//1086 1087//1087 1084//1084
+f 1091//1091 1088//1088 1096//1096
+f 1100//1100 589//589 1091//1091
+f 589//589 1088//1088 1091//1091
+f 1089//1089 1090//1090 1103//1103
+f 1103//1103 1090//1090 1488//1488
+f 1103//1103 1488//1488 1102//1102
+f 1100//1100 1091//1091 1095//1095
+f 1092//1092 1108//1108 1476//1476
+f 1108//1108 1089//1089 1476//1476
+f 1089//1089 1108//1108 1093//1093
+f 1090//1090 1089//1089 1093//1093
+f 1490//1490 1094//1094 1491//1491
+f 1095//1095 1091//1091 1098//1098
+f 1094//1094 1095//1095 1098//1098
+f 1091//1091 1096//1096 1098//1098
+f 535//535 1097//1097 1492//1492
+f 535//535 1100//1100 1097//1097
+f 1491//1491 1101//1101 1099//1099
+f 1490//1490 1095//1095 1094//1094
+f 1096//1096 1089//1089 1098//1098
+f 1089//1089 1096//1096 1476//1476
+f 1492//1492 1097//1097 1099//1099
+f 1097//1097 1490//1490 1099//1099
+f 1100//1100 1095//1095 1097//1097
+f 1095//1095 1490//1490 1097//1097
+f 1491//1491 1102//1102 1101//1101
+f 1103//1103 1102//1102 1094//1094
+f 1094//1094 1102//1102 1491//1491
+f 1103//1103 1094//1094 1098//1098
+f 1089//1089 1103//1103 1098//1098
+f 1104//1104 1107//1107 1111//1111
+f 1111//1111 1107//1107 1106//1106
+f 1145//1145 1106//1106 1114//1114
+f 1145//1145 1114//1114 1105//1105
+f 1145//1145 1105//1105 1470//1470
+f 1114//1114 1107//1107 1115//1115
+f 1106//1106 1107//1107 1114//1114
+f 1108//1108 1116//1116 1109//1109
+f 1108//1108 1092//1092 1116//1116
+f 1093//1093 1109//1109 1110//1110
+f 1093//1093 1108//1108 1109//1109
+f 1090//1090 1093//1093 1110//1110
+f 1090//1090 1110//1110 1120//1120
+f 1488//1488 1090//1090 1120//1120
+f 1104//1104 1143//1143 1130//1130
+f 1104//1104 1111//1111 1143//1143
+f 1110//1110 1109//1109 1112//1112
+f 1110//1110 1112//1112 1113//1113
+f 1120//1120 1110//1110 1113//1113
+f 1107//1107 1104//1104 1112//1112
+f 1115//1115 1107//1107 1112//1112
+f 1104//1104 1130//1130 1113//1113
+f 1112//1112 1104//1104 1113//1113
+f 1114//1114 1115//1115 1116//1116
+f 1116//1116 1115//1115 1109//1109
+f 1109//1109 1115//1115 1112//1112
+f 1117//1117 1130//1130 1118//1118
+f 1117//1117 1118//1118 1125//1125
+f 1125//1125 1118//1118 1123//1123
+f 1123//1123 1015//1015 1127//1127
+f 1118//1118 1130//1130 1119//1119
+f 1120//1120 1113//1113 1117//1117
+f 1120//1120 1117//1117 1121//1121
+f 1122//1122 1013//1013 1500//1500
+f 1027//1027 1123//1123 1135//1135
+f 1118//1118 1119//1119 1135//1135
+f 1123//1123 1118//1118 1135//1135
+f 1127//1127 1122//1122 1128//1128
+f 1121//1121 1128//1128 1124//1124
+f 1121//1121 1124//1124 1126//1126
+f 1128//1128 1500//1500 1124//1124
+f 1122//1122 1500//1500 1128//1128
+f 1121//1121 1125//1125 1128//1128
+f 1125//1125 1123//1123 1127//1127
+f 1117//1117 1125//1125 1121//1121
+f 1120//1120 1121//1121 1126//1126
+f 1125//1125 1127//1127 1128//1128
+f 1130//1130 1143//1143 1119//1119
+f 1488//1488 1120//1120 1126//1126
+f 1015//1015 1129//1129 1127//1127
+f 1129//1129 1122//1122 1127//1127
+f 1113//1113 1130//1130 1117//1117
+f 1022//1022 1021//1021 1133//1133
+f 1133//1133 1134//1134 1132//1132
+f 1132//1132 1134//1134 1131//1131
+f 1132//1132 1144//1144 1136//1136
+f 1021//1021 1027//1027 1135//1135
+f 1133//1133 1135//1135 1134//1134
+f 1021//1021 1135//1135 1133//1133
+f 1133//1133 1132//1132 1136//1136
+f 1022//1022 1133//1133 1136//1136
+f 1134//1134 1143//1143 1131//1131
+f 1119//1119 1143//1143 1134//1134
+f 1027//1027 1015//1015 1123//1123
+f 1135//1135 1119//1119 1134//1134
+f 1131//1131 1146//1146 1137//1137
+f 1158//1158 1466//1466 1454//1454
+f 1466//1466 1158//1158 1140//1140
+f 1140//1140 1158//1158 1147//1147
+f 1138//1138 1465//1465 1141//1141
+f 1140//1140 1147//1147 1141//1141
+f 1147//1147 1139//1139 1142//1142
+f 1141//1141 1147//1147 1142//1142
+f 1465//1465 1140//1140 1141//1141
+f 1139//1139 1146//1146 1142//1142
+f 1137//1137 1146//1146 1139//1139
+f 1138//1138 1141//1141 1142//1142
+f 1106//1106 1145//1145 1142//1142
+f 1111//1111 1106//1106 1146//1146
+f 1131//1131 1111//1111 1146//1146
+f 1143//1143 1111//1111 1131//1131
+f 1132//1132 1131//1131 1137//1137
+f 1144//1144 1132//1132 1137//1137
+f 1144//1144 1137//1137 1148//1148
+f 1145//1145 1470//1470 1138//1138
+f 1145//1145 1138//1138 1142//1142
+f 1146//1146 1106//1106 1142//1142
+f 1147//1147 1158//1158 1157//1157
+f 1139//1139 1147//1147 1157//1157
+f 1137//1137 1139//1139 1148//1148
+f 1148//1148 1139//1139 1157//1157
+f 1458//1458 1462//1462 1149//1149
+f 1150//1150 1149//1149 1151//1151
+f 1458//1458 1149//1149 1150//1150
+f 1150//1150 1151//1151 1152//1152
+f 1149//1149 1462//1462 1160//1160
+f 1151//1151 1149//1149 1160//1160
+f 1232//1232 1152//1152 1230//1230
+f 1156//1156 1232//1232 1229//1229
+f 1156//1156 1229//1229 1153//1153
+f 1156//1156 1153//1153 1154//1154
+f 1150//1150 1156//1156 1154//1154
+f 1150//1150 1154//1154 1155//1155
+f 1150//1150 1152//1152 1156//1156
+f 1152//1152 1232//1232 1156//1156
+f 1458//1458 1155//1155 1459//1459
+f 1153//1153 1229//1229 1159//1159
+f 1458//1458 1150//1150 1155//1155
+f 1144//1144 1148//1148 1159//1159
+f 1230//1230 1162//1162 1168//1168
+f 1154//1154 1157//1157 1155//1155
+f 1157//1157 1158//1158 1155//1155
+f 1158//1158 1454//1454 1459//1459
+f 1155//1155 1158//1158 1459//1459
+f 1153//1153 1148//1148 1154//1154
+f 1148//1148 1157//1157 1154//1154
+f 1148//1148 1153//1153 1159//1159
+f 1151//1151 1160//1160 1161//1161
+f 1162//1162 1151//1151 1161//1161
+f 1152//1152 1151//1151 1162//1162
+f 1230//1230 1152//1152 1162//1162
+f 1169//1169 1161//1161 1460//1460
+f 1161//1161 1160//1160 1460//1460
+f 1219//1219 1172//1172 1163//1163
+f 1163//1163 1169//1169 1448//1448
+f 1448//1448 1169//1169 1449//1449
+f 1163//1163 1448//1448 1171//1171
+f 1169//1169 1163//1163 1164//1164
+f 1163//1163 1172//1172 1164//1164
+f 1169//1169 1460//1460 1449//1449
+f 1438//1438 1439//1439 1165//1165
+f 1438//1438 1165//1165 1166//1166
+f 1165//1165 1448//1448 1166//1166
+f 1171//1171 1165//1165 1167//1167
+f 1171//1171 1448//1448 1165//1165
+f 1168//1168 1162//1162 1164//1164
+f 1162//1162 1161//1161 1164//1164
+f 1161//1161 1169//1169 1164//1164
+f 1182//1182 1179//1179 1175//1175
+f 1219//1219 1175//1175 1170//1170
+f 1170//1170 1175//1175 1180//1180
+f 1170//1170 1180//1180 1174//1174
+f 1219//1219 1163//1163 1175//1175
+f 1171//1171 1167//1167 1182//1182
+f 1171//1171 1182//1182 1175//1175
+f 1163//1163 1171//1171 1175//1175
+f 1172//1172 1168//1168 1164//1164
+f 353//353 1177//1177 1173//1173
+f 353//353 1173//1173 1440//1440
+f 1165//1165 1439//1439 1167//1167
+f 1184//1184 1187//1187 1178//1178
+f 1177//1177 1184//1184 1178//1178
+f 1174//1174 1180//1180 1205//1205
+f 1180//1180 1179//1179 1181//1181
+f 1175//1175 1179//1179 1180//1180
+f 1173//1173 1167//1167 1176//1176
+f 1173//1173 1177//1177 1178//1178
+f 1440//1440 1173//1173 1176//1176
+f 1179//1179 1187//1187 1181//1181
+f 1205//1205 1180//1180 1181//1181
+f 1187//1187 1179//1179 1178//1178
+f 1167//1167 1439//1439 1176//1176
+f 1182//1182 1173//1173 1178//1178
+f 1182//1182 1167//1167 1173//1173
+f 1179//1179 1182//1182 1178//1178
+f 1194//1194 1204//1204 1183//1183
+f 1184//1184 1186//1186 1187//1187
+f 1185//1185 1188//1188 1189//1189
+f 1188//1188 1083//1083 1192//1192
+f 1186//1186 1185//1185 1187//1187
+f 1187//1187 1185//1185 1189//1189
+f 1189//1189 1188//1188 1191//1191
+f 1188//1188 1192//1192 1191//1191
+f 1181//1181 1189//1189 1190//1190
+f 1181//1181 1187//1187 1189//1189
+f 1205//1205 1181//1181 1190//1190
+f 1191//1191 1183//1183 1190//1190
+f 1205//1205 1190//1190 1201//1201
+f 1190//1190 1183//1183 1201//1201
+f 1194//1194 1183//1183 1191//1191
+f 1189//1189 1191//1191 1190//1190
+f 1192//1192 1194//1194 1191//1191
+f 1193//1193 1204//1204 1194//1194
+f 1192//1192 1193//1193 1194//1194
+f 1214//1214 1195//1195 1196//1196
+f 1196//1196 1052//1052 1208//1208
+f 1196//1196 1195//1195 1206//1206
+f 1052//1052 1196//1196 1202//1202
+f 1197//1197 1052//1052 1202//1202
+f 1052//1052 1198//1198 1208//1208
+f 1052//1052 1197//1197 1199//1199
+f 1215//1215 1214//1214 1200//1200
+f 1214//1214 1196//1196 1208//1208
+f 1214//1214 1208//1208 1200//1200
+f 1207//1207 1039//1039 1209//1209
+f 1209//1209 1035//1035 1200//1200
+f 1174//1174 1205//1205 1206//1206
+f 1035//1035 1036//1036 1212//1212
+f 1059//1059 1207//1207 1060//1060
+f 1201//1201 1196//1196 1206//1206
+f 1201//1201 1183//1183 1202//1202
+f 1196//1196 1201//1201 1202//1202
+f 1183//1183 1204//1204 1202//1202
+f 1204//1204 1197//1197 1202//1202
+f 1203//1203 1204//1204 1193//1193
+f 1205//1205 1201//1201 1206//1206
+f 1207//1207 1198//1198 1060//1060
+f 1207//1207 1209//1209 1198//1198
+f 1198//1198 1209//1209 1208//1208
+f 1208//1208 1209//1209 1200//1200
+f 1219//1219 1170//1170 1210//1210
+f 1215//1215 1200//1200 1212//1212
+f 1215//1215 1212//1212 1211//1211
+f 1170//1170 1174//1174 1216//1216
+f 1210//1210 1170//1170 1216//1216
+f 1211//1211 1212//1212 1213//1213
+f 1214//1214 1215//1215 1223//1223
+f 1215//1215 1211//1211 1223//1223
+f 1211//1211 1213//1213 1220//1220
+f 1174//1174 1206//1206 1216//1216
+f 1212//1212 1036//1036 1213//1213
+f 1036//1036 1030//1030 1213//1213
+f 1200//1200 1035//1035 1212//1212
+f 1172//1172 1218//1218 1217//1217
+f 1168//1168 1172//1172 1217//1217
+f 1219//1219 1210//1210 1218//1218
+f 1172//1172 1219//1219 1218//1218
+f 1213//1213 1030//1030 1220//1220
+f 1224//1224 1231//1231 1222//1222
+f 1222//1222 1231//1231 1226//1226
+f 1216//1216 1195//1195 1221//1221
+f 1223//1223 1224//1224 1221//1221
+f 1224//1224 1222//1222 1221//1221
+f 1218//1218 1210//1210 1222//1222
+f 1217//1217 1218//1218 1222//1222
+f 1217//1217 1222//1222 1226//1226
+f 1210//1210 1216//1216 1221//1221
+f 1222//1222 1210//1210 1221//1221
+f 1195//1195 1223//1223 1221//1221
+f 1224//1224 1220//1220 1231//1231
+f 1211//1211 1220//1220 1224//1224
+f 1195//1195 1214//1214 1223//1223
+f 1223//1223 1211//1211 1224//1224
+f 1206//1206 1195//1195 1216//1216
+f 1030//1030 1227//1227 1220//1220
+f 1144//1144 1159//1159 1228//1228
+f 1228//1228 1159//1159 1225//1225
+f 1136//1136 1144//1144 1228//1228
+f 1227//1227 1032//1032 1228//1228
+f 1230//1230 1217//1217 1226//1226
+f 1220//1220 1227//1227 1231//1231
+f 1032//1032 1022//1022 1228//1228
+f 1022//1022 1136//1136 1228//1228
+f 1230//1230 1168//1168 1217//1217
+f 1159//1159 1229//1229 1225//1225
+f 1225//1225 1229//1229 1233//1233
+f 1227//1227 1228//1228 1225//1225
+f 1229//1229 1232//1232 1233//1233
+f 1232//1232 1230//1230 1226//1226
+f 1226//1226 1231//1231 1233//1233
+f 1232//1232 1226//1226 1233//1233
+f 1231//1231 1227//1227 1225//1225
+f 1231//1231 1225//1225 1233//1233
+f 543//543 1241//1241 1234//1234
+f 1241//1241 1287//1287 1242//1242
+f 1235//1235 1238//1238 1246//1246
+f 1477//1477 1234//1234 1474//1474
+f 1477//1477 543//543 1234//1234
+f 1235//1235 1246//1246 1240//1240
+f 1236//1236 1293//1293 1247//1247
+f 1234//1234 1241//1241 1237//1237
+f 1236//1236 1247//1247 1239//1239
+f 1238//1238 1235//1235 1247//1247
+f 1247//1247 1235//1235 1239//1239
+f 1240//1240 1234//1234 1237//1237
+f 1237//1237 1242//1242 1239//1239
+f 1240//1240 1246//1246 1473//1473
+f 1235//1235 1237//1237 1239//1239
+f 1241//1241 1242//1242 1237//1237
+f 1235//1235 1240//1240 1237//1237
+f 1243//1243 1245//1245 1304//1304
+f 1304//1304 1245//1245 1244//1244
+f 1243//1243 1238//1238 1245//1245
+f 1238//1238 1243//1243 1468//1468
+f 1246//1246 1238//1238 1468//1468
+f 1238//1238 1247//1247 1245//1245
+f 1243//1243 1304//1304 1254//1254
+f 1247//1247 1293//1293 1245//1245
+f 1468//1468 1248//1248 1464//1464
+f 1243//1243 1254//1254 1248//1248
+f 1468//1468 1243//1243 1248//1248
+f 1245//1245 1293//1293 1244//1244
+f 1254//1254 1304//1304 1252//1252
+f 1253//1253 1332//1332 1318//1318
+f 1253//1253 1318//1318 1250//1250
+f 1252//1252 1324//1324 1249//1249
+f 1248//1248 1254//1254 1255//1255
+f 1250//1250 1318//1318 1251//1251
+f 1249//1249 1250//1250 1251//1251
+f 1251//1251 1318//1318 1452//1452
+f 1254//1254 1252//1252 1249//1249
+f 1253//1253 1250//1250 1320//1320
+f 1452//1452 1318//1318 1331//1331
+f 1255//1255 1249//1249 1251//1251
+f 1254//1254 1249//1249 1255//1255
+f 1256//1256 1269//1269 1257//1257
+f 1260//1260 1259//1259 1258//1258
+f 1258//1258 1259//1259 1330//1330
+f 1257//1257 1260//1260 1258//1258
+f 1256//1256 1257//1257 1258//1258
+f 1452//1452 1331//1331 1259//1259
+f 1452//1452 1259//1259 1453//1453
+f 1257//1257 1455//1455 1463//1463
+f 1455//1455 1269//1269 1263//1263
+f 1259//1259 1260//1260 1453//1453
+f 1260//1260 1257//1257 1463//1463
+f 1257//1257 1269//1269 1455//1455
+f 1259//1259 1331//1331 1330//1330
+f 1450//1450 1262//1262 1261//1261
+f 1455//1455 1263//1263 1262//1262
+f 1262//1262 1263//1263 1261//1261
+f 1270//1270 1265//1265 1268//1268
+f 1450//1450 1261//1261 1265//1265
+f 1265//1265 1261//1261 1268//1268
+f 1266//1266 1264//1264 1447//1447
+f 1270//1270 1268//1268 1344//1344
+f 1264//1264 1270//1270 1274//1274
+f 1265//1265 1270//1270 1266//1266
+f 1266//1266 1270//1270 1264//1264
+f 1268//1268 1263//1263 1267//1267
+f 1344//1344 1268//1268 1267//1267
+f 1274//1274 1430//1430 1276//1276
+f 1263//1263 1269//1269 1267//1267
+f 1261//1261 1263//1263 1268//1268
+f 1270//1270 1344//1344 1274//1274
+f 1282//1282 1444//1444 1271//1271
+f 1282//1282 1273//1273 1285//1285
+f 1282//1282 1271//1271 1273//1273
+f 1285//1285 1278//1278 1272//1272
+f 1285//1285 1273//1273 1278//1278
+f 1264//1264 1274//1274 1276//1276
+f 1264//1264 1276//1276 1277//1277
+f 1447//1447 1264//1264 1277//1277
+f 1447//1447 1277//1277 1437//1437
+f 1276//1276 1275//1275 1279//1279
+f 1276//1276 1430//1430 1275//1275
+f 1277//1277 1279//1279 1280//1280
+f 1277//1277 1276//1276 1279//1279
+f 1437//1437 1280//1280 1281//1281
+f 1437//1437 1277//1277 1280//1280
+f 1278//1278 1273//1273 1275//1275
+f 1273//1273 1271//1271 1279//1279
+f 1275//1275 1273//1273 1279//1279
+f 1271//1271 1444//1444 1280//1280
+f 1279//1279 1271//1271 1280//1280
+f 1280//1280 1444//1444 1281//1281
+f 289//289 283//283 1282//1282
+f 1282//1282 283//283 1444//1444
+f 1284//1284 289//289 1283//1283
+f 303//303 1284//1284 1283//1283
+f 289//289 1285//1285 1283//1283
+f 289//289 1282//1282 1285//1285
+f 1285//1285 1272//1272 1283//1283
+f 1426//1426 303//303 1422//1422
+f 1283//1283 1272//1272 1422//1422
+f 303//303 1283//1283 1422//1422
+f 150//150 303//303 1426//1426
+f 1236//1236 1239//1239 1295//1295
+f 1236//1236 1295//1295 1300//1300
+f 1292//1292 1308//1308 1286//1286
+f 1287//1287 1301//1301 1242//1242
+f 1290//1290 1288//1288 1289//1289
+f 1292//1292 1286//1286 1288//1288
+f 1288//1288 1286//1286 1289//1289
+f 1308//1308 1292//1292 1297//1297
+f 1298//1298 1292//1292 1288//1288
+f 1300//1300 1295//1295 1291//1291
+f 1290//1290 1300//1300 1291//1291
+f 1290//1290 1291//1291 1299//1299
+f 1298//1298 1288//1288 1299//1299
+f 1288//1288 1290//1290 1299//1299
+f 1298//1298 1296//1296 1292//1292
+f 1289//1289 1286//1286 1309//1309
+f 1286//1286 1308//1308 1309//1309
+f 1244//1244 1289//1289 1309//1309
+f 1289//1289 1244//1244 1294//1294
+f 1293//1293 1236//1236 1294//1294
+f 1239//1239 1242//1242 1295//1295
+f 1244//1244 1293//1293 1294//1294
+f 1295//1295 1301//1301 1291//1291
+f 1301//1301 732//732 1291//1291
+f 1292//1292 1296//1296 1297//1297
+f 732//732 1298//1298 1299//1299
+f 1236//1236 1300//1300 1294//1294
+f 1300//1300 1290//1290 1294//1294
+f 1291//1291 732//732 1299//1299
+f 1242//1242 1301//1301 1295//1295
+f 1290//1290 1289//1289 1294//1294
+f 1316//1316 1381//1381 1380//1380
+f 1244//1244 1309//1309 1305//1305
+f 1381//1381 1303//1303 1302//1302
+f 1302//1302 1303//1303 1307//1307
+f 1309//1309 1308//1308 1306//1306
+f 1304//1304 1244//1244 1305//1305
+f 1252//1252 1304//1304 1305//1305
+f 1381//1381 1302//1302 1382//1382
+f 1381//1381 1316//1316 1303//1303
+f 1323//1323 1306//1306 1310//1310
+f 1305//1305 1306//1306 1323//1323
+f 1252//1252 1305//1305 1323//1323
+f 1308//1308 1307//1307 1306//1306
+f 1308//1308 1297//1297 1307//1307
+f 1305//1305 1309//1309 1306//1306
+f 1306//1306 1307//1307 1310//1310
+f 1307//1307 1303//1303 1310//1310
+f 1302//1302 1297//1297 1382//1382
+f 1297//1297 1302//1302 1307//1307
+f 1303//1303 1316//1316 1313//1313
+f 1320//1320 1311//1311 1312//1312
+f 1311//1311 1313//1313 1312//1312
+f 1313//1313 1316//1316 1315//1315
+f 1312//1312 1313//1313 1315//1315
+f 1316//1316 1380//1380 1314//1314
+f 1315//1315 1316//1316 1314//1314
+f 1314//1314 1380//1380 1317//1317
+f 1310//1310 1313//1313 1311//1311
+f 1310//1310 1303//1303 1313//1313
+f 1324//1324 1250//1250 1249//1249
+f 1320//1320 1324//1324 1311//1311
+f 1253//1253 1320//1320 1312//1312
+f 1318//1318 1332//1332 1331//1331
+f 1314//1314 1370//1370 1321//1321
+f 1321//1321 1370//1370 1319//1319
+f 1319//1319 1370//1370 1373//1373
+f 1321//1321 1322//1322 1336//1336
+f 1250//1250 1324//1324 1320//1320
+f 1321//1321 1319//1319 1322//1322
+f 1312//1312 1315//1315 1336//1336
+f 1253//1253 1312//1312 1336//1336
+f 1324//1324 1323//1323 1311//1311
+f 1332//1332 1253//1253 1336//1336
+f 1315//1315 1314//1314 1321//1321
+f 1323//1323 1310//1310 1311//1311
+f 1315//1315 1321//1321 1336//1336
+f 1324//1324 1252//1252 1323//1323
+f 1314//1314 1317//1317 1370//1370
+f 1360//1360 1340//1340 1343//1343
+f 1343//1343 1325//1325 1328//1328
+f 1335//1335 1328//1328 1326//1326
+f 1328//1328 1325//1325 1326//1326
+f 1335//1335 1326//1326 1339//1339
+f 1339//1339 1345//1345 1327//1327
+f 1343//1343 1334//1334 1395//1395
+f 1333//1333 1327//1327 1329//1329
+f 1256//1256 1333//1333 1329//1329
+f 1343//1343 1328//1328 1334//1334
+f 1328//1328 1335//1335 1334//1334
+f 1256//1256 1329//1329 1348//1348
+f 1327//1327 1345//1345 1329//1329
+f 1329//1329 1345//1345 1348//1348
+f 1335//1335 1339//1339 1327//1327
+f 1258//1258 1330//1330 1333//1333
+f 1256//1256 1258//1258 1333//1333
+f 1269//1269 1256//1256 1348//1348
+f 1331//1331 1332//1332 1330//1330
+f 1330//1330 1332//1332 1337//1337
+f 1322//1322 1335//1335 1327//1327
+f 1322//1322 1319//1319 1335//1335
+f 1327//1327 1333//1333 1337//1337
+f 1322//1322 1327//1327 1337//1337
+f 1333//1333 1330//1330 1337//1337
+f 1334//1334 1373//1373 1395//1395
+f 1319//1319 1373//1373 1334//1334
+f 1335//1335 1319//1319 1334//1334
+f 1336//1336 1322//1322 1337//1337
+f 1332//1332 1336//1336 1337//1337
+f 1342//1342 1339//1339 1338//1338
+f 1339//1339 1326//1326 1338//1338
+f 1326//1326 1325//1325 1353//1353
+f 1338//1338 1326//1326 1353//1353
+f 1325//1325 1340//1340 1341//1341
+f 1348//1348 1345//1345 1342//1342
+f 1344//1344 1267//1267 1342//1342
+f 1267//1267 1348//1348 1342//1342
+f 1343//1343 1340//1340 1325//1325
+f 1344//1344 1342//1342 1338//1338
+f 1353//1353 1325//1325 1341//1341
+f 1341//1341 1340//1340 1349//1349
+f 1345//1345 1339//1339 1342//1342
+f 1364//1364 1346//1346 1347//1347
+f 1353//1353 1341//1341 1350//1350
+f 1267//1267 1269//1269 1348//1348
+f 1364//1364 1355//1355 1432//1432
+f 1350//1350 1351//1351 1352//1352
+f 1353//1353 1350//1350 1354//1354
+f 1350//1350 1352//1352 1354//1354
+f 1341//1341 1349//1349 1347//1347
+f 1349//1349 1364//1364 1347//1347
+f 1347//1347 1346//1346 1351//1351
+f 1341//1341 1347//1347 1350//1350
+f 1350//1350 1347//1347 1351//1351
+f 1274//1274 1344//1344 1338//1338
+f 1352//1352 1430//1430 1354//1354
+f 1274//1274 1338//1338 1354//1354
+f 1430//1430 1274//1274 1354//1354
+f 1338//1338 1353//1353 1354//1354
+f 1356//1356 1363//1363 1358//1358
+f 1355//1355 1363//1363 1361//1361
+f 1356//1356 1358//1358 1357//1357
+f 144//144 1356//1356 1357//1357
+f 1363//1363 1349//1349 1358//1358
+f 1357//1357 1359//1359 1366//1366
+f 1357//1357 1358//1358 1359//1359
+f 1358//1358 1360//1360 1359//1359
+f 142//142 144//144 1357//1357
+f 144//144 147//147 1404//1404
+f 1363//1363 1356//1356 1361//1361
+f 1361//1361 1356//1356 1362//1362
+f 1356//1356 1404//1404 1362//1362
+f 142//142 1357//1357 1366//1366
+f 1364//1364 1349//1349 1363//1363
+f 1340//1340 1360//1360 1358//1358
+f 1349//1349 1340//1340 1358//1358
+f 1355//1355 1364//1364 1363//1363
+f 1356//1356 144//144 1404//1404
+f 1365//1365 142//142 1366//1366
+f 1365//1365 1366//1366 1399//1399
+f 771//771 1365//1365 1368//1368
+f 1365//1365 1399//1399 1368//1368
+f 1367//1367 771//771 1368//1368
+f 1400//1400 1367//1367 1368//1368
+f 1317//1317 1386//1386 1375//1375
+f 742//742 1369//1369 1384//1384
+f 742//742 752//752 1369//1369
+f 1369//1369 1386//1386 1384//1384
+f 1317//1317 1380//1380 1386//1386
+f 1376//1376 1372//1372 1397//1397
+f 1369//1369 752//752 1376//1376
+f 752//752 1372//1372 1376//1376
+f 1373//1373 1370//1370 1371//1371
+f 1376//1376 1397//1397 1377//1377
+f 1370//1370 1317//1317 1371//1371
+f 1372//1372 1393//1393 1397//1397
+f 1373//1373 1371//1371 1378//1378
+f 1378//1378 1371//1371 1374//1374
+f 1369//1369 1376//1376 1374//1374
+f 1375//1375 1369//1369 1374//1374
+f 1371//1371 1375//1375 1374//1374
+f 1317//1317 1375//1375 1371//1371
+f 1375//1375 1386//1386 1369//1369
+f 1376//1376 1377//1377 1374//1374
+f 1377//1377 1378//1378 1374//1374
+f 1385//1385 1382//1382 1379//1379
+f 1386//1386 1380//1380 1387//1387
+f 1382//1382 1385//1385 1387//1387
+f 1381//1381 1382//1382 1387//1387
+f 1296//1296 1383//1383 1379//1379
+f 1297//1297 1296//1296 1379//1379
+f 1382//1382 1297//1297 1379//1379
+f 1383//1383 747//747 1379//1379
+f 747//747 742//742 1384//1384
+f 747//747 1385//1385 1379//1379
+f 1380//1380 1381//1381 1387//1387
+f 1385//1385 1384//1384 1387//1387
+f 1384//1384 1386//1386 1387//1387
+f 747//747 1384//1384 1385//1385
+f 1388//1388 1399//1399 1391//1391
+f 1388//1388 1390//1390 1396//1396
+f 1388//1388 1396//1396 1392//1392
+f 1390//1390 1394//1394 1389//1389
+f 1396//1396 1390//1390 1389//1389
+f 1394//1394 1395//1395 1389//1389
+f 1390//1390 1391//1391 1398//1398
+f 1388//1388 1391//1391 1390//1390
+f 1394//1394 1390//1390 1398//1398
+f 1366//1366 1359//1359 1391//1391
+f 1399//1399 1366//1366 1391//1391
+f 1343//1343 1395//1395 1394//1394
+f 1397//1397 1393//1393 1392//1392
+f 1393//1393 1400//1400 1392//1392
+f 1360//1360 1343//1343 1394//1394
+f 1360//1360 1394//1394 1398//1398
+f 1395//1395 1373//1373 1378//1378
+f 1395//1395 1378//1378 1389//1389
+f 1378//1378 1377//1377 1389//1389
+f 1377//1377 1396//1396 1389//1389
+f 1377//1377 1397//1397 1396//1396
+f 1396//1396 1397//1397 1392//1392
+f 1391//1391 1359//1359 1398//1398
+f 1359//1359 1360//1360 1398//1398
+f 1368//1368 1399//1399 1388//1388
+f 1400//1400 1368//1368 1388//1388
+f 1400//1400 1388//1388 1392//1392
+f 1362//1362 1404//1404 1401//1401
+f 1355//1355 1361//1361 1407//1407
+f 1361//1361 1362//1362 1402//1402
+f 1418//1418 1408//1408 1409//1409
+f 1410//1410 1403//1403 1420//1420
+f 1410//1410 1409//1409 1411//1411
+f 1407//1407 1406//1406 1435//1435
+f 1408//1408 1418//1418 1406//1406
+f 1401//1401 797//797 1410//1410
+f 797//797 801//801 1410//1410
+f 1410//1410 801//801 1403//1403
+f 147//147 1405//1405 1404//1404
+f 1404//1404 1405//1405 1401//1401
+f 1405//1405 797//797 1401//1401
+f 1407//1407 1408//1408 1406//1406
+f 1407//1407 1402//1402 1408//1408
+f 1402//1402 1362//1362 1411//1411
+f 1362//1362 1401//1401 1411//1411
+f 1355//1355 1407//1407 1435//1435
+f 1407//1407 1361//1361 1402//1402
+f 1408//1408 1402//1402 1409//1409
+f 1401//1401 1410//1410 1411//1411
+f 1409//1409 1402//1402 1411//1411
+f 1412//1412 1414//1414 1417//1417
+f 1412//1412 1413//1413 1414//1414
+f 1409//1409 1410//1410 1420//1420
+f 1417//1417 1403//1403 1415//1415
+f 1416//1416 1419//1419 1415//1415
+f 1420//1420 1403//1403 1417//1417
+f 1419//1419 1412//1412 1415//1415
+f 1412//1412 1417//1417 1415//1415
+f 1418//1418 1409//1409 1420//1420
+f 163//163 1416//1416 1415//1415
+f 1403//1403 163//163 1415//1415
+f 801//801 163//163 1403//1403
+f 1419//1419 1413//1413 1412//1412
+f 1420//1420 1417//1417 1423//1423
+f 1413//1413 150//150 1426//1426
+f 1418//1418 1420//1420 1423//1423
+f 1418//1418 1423//1423 1421//1421
+f 1422//1422 1272//1272 1424//1424
+f 1406//1406 1418//1418 1421//1421
+f 1414//1414 1426//1426 1427//1427
+f 1423//1423 1414//1414 1427//1427
+f 1427//1427 1424//1424 1425//1425
+f 1426//1426 1422//1422 1427//1427
+f 1422//1422 1424//1424 1427//1427
+f 1413//1413 1426//1426 1414//1414
+f 1421//1421 1427//1427 1425//1425
+f 1421//1421 1423//1423 1427//1427
+f 1417//1417 1414//1414 1423//1423
+f 1431//1431 1428//1428 1433//1433
+f 1346//1346 1432//1432 1429//1429
+f 1351//1351 1346//1346 1429//1429
+f 1428//1428 1429//1429 1434//1434
+f 1275//1275 1431//1431 1433//1433
+f 1429//1429 1432//1432 1434//1434
+f 1428//1428 1431//1431 1429//1429
+f 1352//1352 1351//1351 1431//1431
+f 1430//1430 1352//1352 1431//1431
+f 1431//1431 1351//1351 1429//1429
+f 1435//1435 1406//1406 1421//1421
+f 1275//1275 1430//1430 1431//1431
+f 1272//1272 1278//1278 1424//1424
+f 1346//1346 1364//1364 1432//1432
+f 1424//1424 1278//1278 1433//1433
+f 1432//1432 1435//1435 1434//1434
+f 1428//1428 1425//1425 1433//1433
+f 1425//1425 1428//1428 1434//1434
+f 1435//1435 1421//1421 1434//1434
+f 1421//1421 1425//1425 1434//1434
+f 1278//1278 1275//1275 1433//1433
+f 1432//1432 1355//1355 1435//1435
+f 1425//1425 1424//1424 1433//1433
+f 283//283 1442//1442 1443//1443
+f 1444//1444 283//283 1443//1443
+f 1442//1442 1440//1440 1443//1443
+f 1439//1439 1438//1438 1436//1436
+f 1438//1438 1447//1447 1437//1437
+f 1438//1438 1437//1437 1436//1436
+f 1176//1176 1439//1439 1436//1436
+f 1440//1440 1176//1176 1441//1441
+f 1436//1436 1281//1281 1441//1441
+f 1442//1442 353//353 1440//1440
+f 1176//1176 1436//1436 1441//1441
+f 1437//1437 1281//1281 1436//1436
+f 1281//1281 1443//1443 1441//1441
+f 1443//1443 1440//1440 1441//1441
+f 1281//1281 1444//1444 1443//1443
+f 1266//1266 1445//1445 1446//1446
+f 1265//1265 1266//1266 1446//1446
+f 1450//1450 1446//1446 1457//1457
+f 1450//1450 1265//1265 1446//1446
+f 1266//1266 1447//1447 1445//1445
+f 1448//1448 1449//1449 1451//1451
+f 1166//1166 1448//1448 1451//1451
+f 1262//1262 1450//1450 1457//1457
+f 1262//1262 1457//1457 1461//1461
+f 1457//1457 1446//1446 1451//1451
+f 1446//1446 1445//1445 1451//1451
+f 1447//1447 1166//1166 1445//1445
+f 1438//1438 1166//1166 1447//1447
+f 1445//1445 1166//1166 1451//1451
+f 1449//1449 1457//1457 1451//1451
+f 1454//1454 1452//1452 1453//1453
+f 1459//1459 1454//1454 1453//1453
+f 1459//1459 1453//1453 1456//1456
+f 1455//1455 1262//1262 1461//1461
+f 1449//1449 1460//1460 1461//1461
+f 1462//1462 1458//1458 1456//1456
+f 1457//1457 1449//1449 1461//1461
+f 1160//1160 1462//1462 1461//1461
+f 1458//1458 1459//1459 1456//1456
+f 1462//1462 1455//1455 1461//1461
+f 1260//1260 1463//1463 1456//1456
+f 1463//1463 1462//1462 1456//1456
+f 1460//1460 1160//1160 1461//1461
+f 1455//1455 1462//1462 1463//1463
+f 1453//1453 1260//1260 1456//1456
+f 1466//1466 1465//1465 1467//1467
+f 1251//1251 1452//1452 1467//1467
+f 1255//1255 1251//1251 1467//1467
+f 1467//1467 1465//1465 1469//1469
+f 1464//1464 1255//1255 1469//1469
+f 1255//1255 1467//1467 1469//1469
+f 1465//1465 1138//1138 1469//1469
+f 1454//1454 1466//1466 1467//1467
+f 1466//1466 1140//1140 1465//1465
+f 1138//1138 1470//1470 1469//1469
+f 1464//1464 1248//1248 1255//1255
+f 1452//1452 1454//1454 1467//1467
+f 1468//1468 1464//1464 1471//1471
+f 1472//1472 1105//1105 1480//1480
+f 1105//1105 1092//1092 1480//1480
+f 1464//1464 1469//1469 1471//1471
+f 1469//1469 1470//1470 1471//1471
+f 1470//1470 1105//1105 1472//1472
+f 1246//1246 1468//1468 1471//1471
+f 1471//1471 1470//1470 1472//1472
+f 1246//1246 1471//1471 1472//1472
+f 1092//1092 1105//1105 1116//1116
+f 1105//1105 1114//1114 1116//1116
+f 1474//1474 1475//1475 1481//1481
+f 1473//1473 1480//1480 1481//1481
+f 1475//1475 1473//1473 1481//1481
+f 1473//1473 1246//1246 1472//1472
+f 1473//1473 1472//1472 1480//1480
+f 1481//1481 1482//1482 1479//1479
+f 1474//1474 1481//1481 1479//1479
+f 1476//1476 540//540 1482//1482
+f 1482//1482 540//540 1479//1479
+f 540//540 586//586 1479//1479
+f 586//586 1478//1478 1479//1479
+f 1088//1088 540//540 1096//1096
+f 1092//1092 1476//1476 1482//1482
+f 1474//1474 1234//1234 1475//1475
+f 1240//1240 1473//1473 1475//1475
+f 1234//1234 1240//1240 1475//1475
+f 1096//1096 540//540 1476//1476
+f 1478//1478 1477//1477 1474//1474
+f 1478//1478 1474//1474 1479//1479
+f 1480//1480 1092//1092 1482//1482
+f 1481//1481 1480//1480 1482//1482
+f 1489//1489 1101//1101 1486//1486
+f 1486//1486 1499//1499 1485//1485
+f 1489//1489 1486//1486 1485//1485
+f 1499//1499 1483//1483 1485//1485
+f 1486//1486 1495//1495 1484//1484
+f 1485//1485 1497//1497 1494//1494
+f 1485//1485 1483//1483 1497//1497
+f 1492//1492 1489//1489 1494//1494
+f 1489//1489 1485//1485 1494//1494
+f 1495//1495 1486//1486 1496//1496
+f 1486//1486 1101//1101 1496//1496
+f 1499//1499 1486//1486 1484//1484
+f 866//866 1497//1497 1487//1487
+f 1102//1102 1488//1488 1496//1496
+f 1488//1488 1126//1126 1496//1496
+f 1099//1099 1101//1101 1489//1489
+f 1101//1101 1102//1102 1496//1496
+f 1490//1490 1491//1491 1099//1099
+f 1493//1493 1492//1492 1494//1494
+f 1493//1493 535//535 1492//1492
+f 1497//1497 866//866 1494//1494
+f 866//866 1493//1493 1494//1494
+f 1126//1126 1124//1124 1495//1495
+f 1126//1126 1495//1495 1496//1496
+f 1124//1124 1500//1500 1495//1495
+f 1495//1495 1500//1500 1484//1484
+f 1497//1497 995//995 1487//1487
+f 995//995 1502//1502 1487//1487
+f 1483//1483 995//995 1497//1497
+f 1498//1498 1483//1483 1499//1499
+f 1498//1498 996//996 1483//1483
+f 1000//1000 1499//1499 1484//1484
+f 1000//1000 1498//1498 1499//1499
+f 1500//1500 1013//1013 1484//1484
+f 1013//1013 1000//1000 1484//1484
+f 1492//1492 1099//1099 1489//1489
+f 867//867 866//866 1487//1487
+f 1501//1501 137//137 1522//1522
+f 1504//1504 867//867 1503//1503
+f 1506//1506 1520//1520 1522//1522
+f 137//137 1505//1505 1522//1522
+f 1505//1505 137//137 1503//1503
+f 1502//1502 1505//1505 1503//1503
+f 1487//1487 1502//1502 1503//1503
+f 867//867 1487//1487 1503//1503
+f 137//137 1504//1504 1503//1503
+f 1505//1505 1506//1506 1522//1522
+f 139//139 1518//1518 1507//1507
+f 1518//1518 1519//1519 1508//1508
+f 1509//1509 1501//1501 1521//1521
+f 1501//1501 1522//1522 1521//1521
+f 135//135 1509//1509 1516//1516
+f 1516//1516 1509//1509 1514//1514
+f 1509//1509 1521//1521 1514//1514
+f 1516//1516 1510//1510 1511//1511
+f 1511//1511 1510//1510 1513//1513
+f 986//986 1512//1512 1513//1513
+f 987//987 986//986 1513//1513
+f 1510//1510 987//987 1513//1513
+f 985//985 1510//1510 1523//1523
+f 985//985 987//987 1510//1510
+f 1514//1514 1515//1515 1523//1523
+f 135//135 1516//1516 1511//1511
+f 1519//1519 1511//1511 1517//1517
+f 1508//1508 1519//1519 1517//1517
+f 1507//1507 1518//1518 1508//1508
+f 1508//1508 1517//1517 1530//1530
+f 1517//1517 1512//1512 1530//1530
+f 1513//1513 1512//1512 1517//1517
+f 1511//1511 1513//1513 1517//1517
+f 1518//1518 1524//1524 1519//1519
+f 1521//1521 1515//1515 1514//1514
+f 1515//1515 985//985 1523//1523
+f 1520//1520 1515//1515 1521//1521
+f 1522//1522 1520//1520 1521//1521
+f 1510//1510 1516//1516 1523//1523
+f 1516//1516 1514//1514 1523//1523
+f 1524//1524 135//135 1519//1519
+f 1519//1519 135//135 1511//1511
+f 1549//1549 1525//1525 1531//1531
+f 1528//1528 1561//1561 1526//1526
+f 1528//1528 1525//1525 1527//1527
+f 141//141 1526//1526 1560//1560
+f 1542//1542 1533//1533 1537//1537
+f 1508//1508 1530//1530 1529//1529
+f 139//139 1507//1507 1528//1528
+f 139//139 1528//1528 1526//1526
+f 1561//1561 1528//1528 1527//1527
+f 1507//1507 1525//1525 1528//1528
+f 1531//1531 1525//1525 1529//1529
+f 1531//1531 1529//1529 1532//1532
+f 1525//1525 1507//1507 1529//1529
+f 1526//1526 1561//1561 1560//1560
+f 1532//1532 1530//1530 1544//1544
+f 1529//1529 1530//1530 1532//1532
+f 1507//1507 1508//1508 1529//1529
+f 1537//1537 1533//1533 1544//1544
+f 1533//1533 1531//1531 1532//1532
+f 1533//1533 1532//1532 1544//1544
+f 141//141 139//139 1526//1526
+f 1530//1530 1512//1512 1544//1544
+f 1534//1534 1535//1535 1543//1543
+f 1534//1534 1543//1543 1540//1540
+f 1540//1540 1024//1024 1536//1536
+f 1543//1543 988//988 1540//1540
+f 1537//1537 988//988 1543//1543
+f 1535//1535 1542//1542 1543//1543
+f 1024//1024 1019//1019 1536//1536
+f 1534//1534 1540//1540 1536//1536
+f 1542//1542 1535//1535 1538//1538
+f 978//978 1539//1539 1540//1540
+f 1539//1539 1024//1024 1540//1540
+f 1512//1512 1541//1541 1544//1544
+f 988//988 978//978 1540//1540
+f 1541//1541 988//988 1537//1537
+f 1542//1542 1537//1537 1543//1543
+f 1541//1541 1537//1537 1544//1544
+f 1619//1619 1621//1621 1620//1620
+f 1619//1619 1620//1620 1622//1622
+f 1018//1018 1545//1545 1616//1616
+f 1019//1019 1018//1018 1616//1616
+f 1620//1620 1548//1548 1546//1546
+f 1019//1019 1616//1616 1547//1547
+f 1535//1535 1534//1534 1546//1546
+f 1538//1538 1535//1535 1546//1546
+f 1534//1534 1620//1620 1546//1546
+f 1534//1534 1536//1536 1622//1622
+f 1548//1548 1538//1538 1546//1546
+f 1622//1622 1536//1536 1547//1547
+f 1536//1536 1019//1019 1547//1547
+f 1620//1620 1534//1534 1622//1622
+f 1525//1525 1549//1549 1527//1527
+f 1551//1551 1614//1614 1550//1550
+f 1551//1551 1538//1538 1614//1614
+f 1549//1549 1551//1551 1550//1550
+f 1542//1542 1538//1538 1551//1551
+f 1538//1538 1548//1548 1614//1614
+f 1549//1549 1550//1550 1606//1606
+f 1527//1527 1549//1549 1606//1606
+f 1531//1531 1533//1533 1551//1551
+f 1549//1549 1531//1531 1551//1551
+f 1533//1533 1542//1542 1551//1551
+f 1559//1559 1552//1552 1557//1557
+f 1552//1552 1553//1553 1558//1558
+f 1558//1558 1553//1553 1554//1554
+f 1553//1553 141//141 1554//1554
+f 1527//1527 1606//1606 1555//1555
+f 1561//1561 1527//1527 1555//1555
+f 141//141 1560//1560 1554//1554
+f 1555//1555 1606//1606 1556//1556
+f 1556//1556 1606//1606 1567//1567
+f 1557//1557 1556//1556 1567//1567
+f 1561//1561 1555//1555 1562//1562
+f 1555//1555 1558//1558 1562//1562
+f 1558//1558 1555//1555 1556//1556
+f 1558//1558 1554//1554 1562//1562
+f 1552//1552 1558//1558 1556//1556
+f 1557//1557 1552//1552 1556//1556
+f 1559//1559 1563//1563 1552//1552
+f 1568//1568 1563//1563 1559//1559
+f 1560//1560 1561//1561 1562//1562
+f 1554//1554 1560//1560 1562//1562
+f 1563//1563 1553//1553 1552//1552
+f 1564//1564 1565//1565 1566//1566
+f 1557//1557 1564//1564 1566//1566
+f 1574//1574 1564//1564 1569//1569
+f 25//25 1568//1568 1566//1566
+f 1557//1557 1567//1567 1564//1564
+f 1564//1564 1567//1567 1569//1569
+f 1568//1568 1559//1559 1566//1566
+f 1559//1559 1557//1557 1566//1566
+f 1565//1565 1564//1564 1574//1574
+f 1609//1609 1574//1574 1569//1569
+f 26//26 25//25 1565//1565
+f 1565//1565 25//25 1566//1566
+f 1570//1570 1070//1070 1577//1577
+f 1571//1571 1583//1583 1580//1580
+f 1565//1565 1581//1581 1573//1573
+f 1572//1572 26//26 1573//1573
+f 26//26 1565//1565 1573//1573
+f 1574//1574 1609//1609 1575//1575
+f 1609//1609 1570//1570 1575//1575
+f 1570//1570 1577//1577 1575//1575
+f 1577//1577 1070//1070 1576//1576
+f 1565//1565 1574//1574 1581//1581
+f 1581//1581 1575//1575 1582//1582
+f 1577//1577 1576//1576 1582//1582
+f 1581//1581 1574//1574 1575//1575
+f 1575//1575 1577//1577 1582//1582
+f 1580//1580 1578//1578 1579//1579
+f 24//24 1573//1573 1578//1578
+f 1580//1580 1583//1583 1578//1578
+f 1583//1583 24//24 1578//1578
+f 1581//1581 1582//1582 1578//1578
+f 1582//1582 1576//1576 1579//1579
+f 1573//1573 1581//1581 1578//1578
+f 1578//1578 1582//1582 1579//1579
+f 24//24 1572//1572 1573//1573
+f 1590//1590 38//38 1571//1571
+f 29//29 24//24 1583//1583
+f 39//39 29//29 1583//1583
+f 1571//1571 39//39 1583//1583
+f 1584//1584 1586//1586 1587//1587
+f 1589//1589 1587//1587 1588//1588
+f 1584//1584 1585//1585 1586//1586
+f 1587//1587 1586//1586 1588//1588
+f 1084//1084 363//363 1586//1586
+f 1087//1087 363//363 1084//1084
+f 1587//1587 1589//1589 1592//1592
+f 365//365 1589//1589 1588//1588
+f 365//365 382//382 1589//1589
+f 1586//1586 363//363 1588//1588
+f 363//363 365//365 1588//1588
+f 1592//1592 1590//1590 1605//1605
+f 1589//1589 1590//1590 1592//1592
+f 1585//1585 1084//1084 1586//1586
+f 1590//1590 1571//1571 1605//1605
+f 382//382 1590//1590 1589//1589
+f 1580//1580 1579//1579 1595//1595
+f 1605//1605 1580//1580 1595//1595
+f 1576//1576 1591//1591 1596//1596
+f 1070//1070 1591//1591 1576//1576
+f 1595//1595 1579//1579 1596//1596
+f 1579//1579 1576//1576 1596//1596
+f 1587//1587 1592//1592 1593//1593
+f 1593//1593 1592//1592 1594//1594
+f 1596//1596 1597//1597 1598//1598
+f 1593//1593 1594//1594 1598//1598
+f 1594//1594 1595//1595 1598//1598
+f 1592//1592 1605//1605 1594//1594
+f 1605//1605 1595//1595 1594//1594
+f 1591//1591 1053//1053 1596//1596
+f 1596//1596 1053//1053 1597//1597
+f 1595//1595 1596//1596 1598//1598
+f 1598//1598 1597//1597 1603//1603
+f 1053//1053 1599//1599 1597//1597
+f 1593//1593 1598//1598 1603//1603
+f 1584//1584 1593//1593 1603//1603
+f 1584//1584 1587//1587 1593//1593
+f 1599//1599 1600//1600 1597//1597
+f 1600//1600 1604//1604 1603//1603
+f 1597//1597 1600//1600 1603//1603
+f 1063//1063 1601//1601 1599//1599
+f 1601//1601 1602//1602 1599//1599
+f 1604//1604 1584//1584 1603//1603
+f 1053//1053 1063//1063 1599//1599
+f 1604//1604 1585//1585 1584//1584
+f 1571//1571 1580//1580 1605//1605
+f 1606//1606 1550//1550 1607//1607
+f 1567//1567 1606//1606 1607//1607
+f 1550//1550 1614//1614 1613//1613
+f 1607//1607 1550//1550 1613//1613
+f 1614//1614 1548//1548 1611//1611
+f 1609//1609 1569//1569 1615//1615
+f 1042//1042 1045//1045 1612//1612
+f 1045//1045 1046//1046 1612//1612
+f 1612//1612 1046//1046 1608//1608
+f 1046//1046 1610//1610 1608//1608
+f 1610//1610 1049//1049 1609//1609
+f 1610//1610 1615//1615 1608//1608
+f 1610//1610 1609//1609 1615//1615
+f 1611//1611 1621//1621 1612//1612
+f 1611//1611 1612//1612 1608//1608
+f 1615//1615 1613//1613 1608//1608
+f 1613//1613 1611//1611 1608//1608
+f 1607//1607 1613//1613 1615//1615
+f 1548//1548 1621//1621 1611//1611
+f 1613//1613 1614//1614 1611//1611
+f 1621//1621 1042//1042 1612//1612
+f 1569//1569 1607//1607 1615//1615
+f 1569//1569 1567//1567 1607//1607
+f 1042//1042 1040//1040 1043//1043
+f 1040//1040 1042//1042 1619//1619
+f 1619//1619 1042//1042 1621//1621
+f 1616//1616 1623//1623 1617//1617
+f 1623//1623 1618//1618 1617//1617
+f 1618//1618 1040//1040 1619//1619
+f 1618//1618 1619//1619 1617//1617
+f 1620//1620 1621//1621 1548//1548
+f 1622//1622 1547//1547 1617//1617
+f 1547//1547 1616//1616 1617//1617
+f 1619//1619 1622//1622 1617//1617
+f 1545//1545 1623//1623 1616//1616
+# 3242 faces, 0 coords texture
+
+# End of File
\ No newline at end of file
diff --git a/Simbody/tests/adhoc/ContactTest.cpp b/Simbody/tests/adhoc/ContactTest.cpp
new file mode 100644
index 0000000..4178acf
--- /dev/null
+++ b/Simbody/tests/adhoc/ContactTest.cpp
@@ -0,0 +1,429 @@
+/* -------------------------------------------------------------------------- *
+ * SimTK 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) 2006-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* Test compliant contact. Attempts to run at 3X faster than real time. */
+
+#include "Simbody.h"
+
+#include <string>
+#include <iostream>
+#include <exception>
+#include <cmath>
+#include <ctime>
+using std::cout;
+using std::endl;
+
+using namespace SimTK;
+
+#define ANIMATE // off to get more accurate CPU time (you can still playback)
+#define USE_ELLIPSOIDS // otherwise use round balls
+
+namespace {
+
+const Real mu_s = .2; // Friction coefficients.
+const Real mu_d = .1;
+const Real mu_v = 0.01;
+const Real transitionVelocity = 1e-2; // slide->stick velocity
+
+// material properties
+// Steel
+const Real steel_density = 8000.; // kg/m^3
+const Real steel_young = 200e9; // pascals (N/m)
+const Real steel_poisson = 0.3; // ratio
+const Real steel_planestrain =
+ ContactMaterial::calcPlaneStrainStiffness(steel_young,steel_poisson);
+const Real steel_dissipation = 0.001;
+
+const ContactMaterial steel(steel_planestrain,steel_dissipation,0,0,0);
+
+// Concrete
+const Real concrete_density = 2300.; // kg/m^3
+const Real concrete_young = 25e9; // pascals (N/m)
+const Real concrete_poisson = 0.15; // ratio
+const Real concrete_planestrain =
+ ContactMaterial::calcPlaneStrainStiffness(concrete_young,concrete_poisson);
+const Real concrete_dissipation = 0.005;
+
+const ContactMaterial concrete(concrete_planestrain,concrete_dissipation,
+ mu_s,mu_d,mu_v);
+
+// Nylon
+const Real nylon_density = 1100.; // kg/m^3
+const Real nylon_young = 2.5e9; // pascals (N/m)
+const Real nylon_poisson = 0.4; // ratio
+const Real nylon_planestrain =
+ ContactMaterial::calcPlaneStrainStiffness(nylon_young,nylon_poisson);
+const Real nylon_dissipation = 0.005;
+
+const ContactMaterial nylon(nylon_planestrain,nylon_dissipation,
+ mu_s,mu_d,mu_v);
+
+// Rubber
+const Real rubber_density = 1100.; // kg/m^3
+const Real rubber_young = 0.01e9; // pascals (N/m)
+const Real rubber_poisson = 0.5; // ratio
+const Real rubber_planestrain =
+ ContactMaterial::calcPlaneStrainStiffness(rubber_young,rubber_poisson);
+const Real rubber_dissipation = 0.005;
+
+const ContactMaterial rubber(rubber_planestrain,rubber_dissipation,
+ mu_s,mu_d,mu_v);
+
+extern "C" void SimTK_version_SimTKlapack(int*,int*,int*);
+extern "C" void SimTK_about_SimTKlapack(const char*, int, char*);
+
+const int NColors =16;
+Vec3 colors[NColors] = {
+ Red,Green,Blue,Yellow,Orange,Magenta,Cyan,Purple,
+ Red/1.5,Green/1.5,Blue/1.5,Yellow/1.5,Orange/1.5,Magenta/1.5,Cyan/1.5,Purple/1.5
+};
+
+const ContactMaterial wallMaterial = nylon;
+const ContactMaterial softMaterial = rubber;
+const ContactMaterial hardMaterial = nylon;
+
+const int NRubberBalls = 12, NHardBalls = 25;
+const Real PendBallRadius = 3, RubberBallRadius = 5, HardBallRadius = 4;//m
+
+const Real PendBallMass = rubber_density*(4./3.)*Pi*cube(PendBallRadius);
+const Real RubberBallMass = rubber_density*(4./3.)*Pi*cube(RubberBallRadius);
+const Real HardBallMass = steel_density*(4./3.)*Pi*cube(HardBallRadius);
+
+
+// Write interesting integrator info to stdout.
+void dumpIntegratorStats(const Integrator& integ);
+}
+
+
+//==============================================================================
+// MAIN
+//==============================================================================
+int main() {
+
+ int major,minor,build;
+ char out[100];
+ const char* keylist[] = { "version", "library", "type", "debug", "authors",
+ "copyright", "svn_revision", 0 };
+
+ SimTK_version_SimTKcommon(&major,&minor,&build);
+ std::printf("==> SimTKcommon library version: %d.%d.%d\n", major, minor, build);
+ std::printf(" SimTK_about_SimTKcommon():\n");
+ for (const char** p = keylist; *p; ++p) {
+ SimTK_about_SimTKcommon(*p, 100, out);
+ std::printf(" about(%s)='%s'\n", *p, out);
+ }
+
+ SimTK_version_simmath(&major,&minor,&build);
+ std::printf("==> SimTKmath library version: %d.%d.%d\n", major, minor, build);
+ std::printf(" SimTK_about_simmath():\n");
+ for (const char** p = keylist; *p; ++p) {
+ SimTK_about_simmath(*p, 100, out);
+ std::printf(" about(%s)='%s'\n", *p, out);
+ }
+
+ SimTK_version_simbody(&major,&minor,&build);
+ std::printf("==> simbody library version: %d.%d.%d\n", major, minor, build);
+ std::printf(" SimTK_about_simbody():\n");
+ for (const char** p = keylist; *p; ++p) {
+ SimTK_about_simbody(*p, 100, out);
+ std::printf(" about(%s)='%s'\n", *p, out);
+ }
+
+
+try
+ { Real g = 9.8; // m/s^2
+ //Real g = 1.6; // m/s^2 (moon)
+
+ MultibodySystem mbs;
+ SimbodyMatterSubsystem matter(mbs);
+
+ GeneralForceSubsystem forces(mbs);
+ Force::Gravity gravityForces(forces, matter, -YAxis, g);
+
+ ContactTrackerSubsystem tracker(mbs);
+ CompliantContactSubsystem contactForces(mbs, tracker);
+ contactForces.setTransitionVelocity(transitionVelocity);
+
+ //Force::Thermostat thermostat(forces, matter, 6000/*boltzmann??*/, 500, 1);
+ //Force::Thermostat thermostat(forces, matter, 6000/*boltzmann??*/, 1000, 1, 0);
+ //thermostat.setDefaultNumChains(3);
+ //thermostat.setDisabledByDefault(true);
+
+ // No, thank you.
+ matter.setShowDefaultGeometry(false);
+
+ MobilizedBody& Ground = matter.Ground(); // nicer name for Ground
+
+ // Add the Ground contact geometry. Contact half spaces have -XAxis normals
+ // (right hand wall) so we'll have to rotate them.
+ const Rotation R_right; // identity
+ const Rotation R_back(Pi/2,YAxis);
+ const Rotation R_left(Pi,YAxis);
+ const Rotation R_front(-Pi/2,YAxis);
+ const Rotation R_ceiling(Pi/2,ZAxis);
+ const Rotation R_floor(-Pi/2,ZAxis);
+
+
+ Ground.updBody().addContactSurface(Transform(R_floor,Vec3(0)),
+ ContactSurface(ContactGeometry::HalfSpace(),wallMaterial));
+ Ground.updBody().addContactSurface(Transform(R_left,Vec3(-20,0,0)),
+ ContactSurface(ContactGeometry::HalfSpace(),wallMaterial));
+ Ground.updBody().addContactSurface(Transform(R_right,Vec3(20,0,0)),
+ ContactSurface(ContactGeometry::HalfSpace(),wallMaterial));
+ Ground.updBody().addContactSurface(Transform(R_front,Vec3(0,0,20)),
+ ContactSurface(ContactGeometry::HalfSpace(),wallMaterial));
+ Ground.updBody().addContactSurface(Transform(R_back,Vec3(0,0,-20)),
+ ContactSurface(ContactGeometry::HalfSpace(),wallMaterial));
+
+ DecorativeBrick smallWall(Vec3(.1,20,20)), tallWall(Vec3(.1,30,20));
+ Ground.addBodyDecoration(Transform(R_floor,Vec3(0)),
+ smallWall.setColor(Green).setOpacity(1));
+ Ground.addBodyDecoration(Transform(R_left,Vec3(-20,20,0)),
+ tallWall.setColor(Yellow).setOpacity(.2));
+ Ground.addBodyDecoration(Transform(R_right,Vec3(20,20,0)),
+ tallWall.setColor(Yellow).setOpacity(.2));
+ Ground.addBodyDecoration(Transform(R_front,Vec3(0,20,20)),
+ smallWall.setColor(Gray).setOpacity(.05));
+ Ground.addBodyDecoration(Transform(R_back, Vec3(0,20,-20)),
+ tallWall.setColor(Cyan).setOpacity(.2));
+
+ // start with a 2body pendulum with big rubber balls
+ Real linkLength = 20.; // m
+ Vec3 pendGroundPt1 = Vec3(-10,50,-10);
+ Vec3 pendGroundPt2 = Vec3(20,20,-10);
+ Inertia pendBallInertia(PendBallMass*UnitInertia::sphere(PendBallRadius));
+ const MassProperties pendMProps(PendBallMass, Vec3(0, -linkLength/2, 0),
+ pendBallInertia.shiftFromMassCenter(Vec3(0, -linkLength/2, 0), PendBallMass));
+
+ Body::Rigid pendBodyInfo(pendMProps);
+ pendBodyInfo.addContactSurface(Vec3(0, -linkLength/2, 0),
+ ContactSurface(ContactGeometry::Sphere(PendBallRadius), softMaterial));
+ pendBodyInfo.addDecoration(Vec3(0,-linkLength/2,0),
+ DecorativeSphere(PendBallRadius).setColor(Gray).setOpacity(1));
+ pendBodyInfo.addDecoration(Vec3(0),
+ DecorativeLine(Vec3(0,linkLength/2,0),Vec3(0,-linkLength/2,0)));
+
+ MobilizedBody::Ball pend1(matter.Ground(), Transform(pendGroundPt1),
+ pendBodyInfo, Transform(Vec3(0, linkLength/2, 0)));
+ MobilizedBody::Ball pend2(pend1, Transform(Vec3(0,-linkLength/2,0)),
+ pendBodyInfo, Transform(Vec3(0, linkLength/2, 0)));
+
+ // Add a "rod" constraint to make the two-link pendulum a loop.
+ Constraint::Rod theConstraint(matter.Ground(), pendGroundPt2,
+ pend2, Vec3(0, -linkLength/2, 0),
+ 20);
+
+
+ Body::Rigid hardBallInfo(MassProperties(HardBallMass, Vec3(0),
+ UnitInertia::sphere(HardBallRadius)));
+ Body::Rigid rubberBallInfo(MassProperties(RubberBallMass, Vec3(0),
+ UnitInertia::sphere(RubberBallRadius)));
+
+#ifdef USE_ELLIPSOIDS
+ hardBallInfo.addContactSurface(Vec3(0),
+ ContactSurface(ContactGeometry::Ellipsoid(Vec3(3,HardBallRadius,5)),hardMaterial));
+ DecorativeEllipsoid hardSphere(Vec3(3,HardBallRadius,5));
+ rubberBallInfo.addContactSurface(Vec3(0),
+ ContactSurface(ContactGeometry::Ellipsoid(Vec3(7,RubberBallRadius,4)),softMaterial));
+ DecorativeEllipsoid rubberSphere(Vec3(7,RubberBallRadius,4));
+#else
+ hardBallInfo.addContactSurface(Vec3(0),
+ ContactSurface(ContactGeometry::Sphere(HardBallRadius),hardMaterial));
+ DecorativeSphere hardSphere(HardBallRadius);
+ rubberBallInfo.addContactSurface(Vec3(0),
+ ContactSurface(ContactGeometry::Sphere(RubberBallRadius),softMaterial));
+ DecorativeSphere rubberSphere(RubberBallRadius);
+#endif
+
+ const Vec3 firstHardBallPos = Vec3(-6,30, 0),
+ firstRubberBallPos = Vec3(13,30,-15);
+
+ for (int i=0; i<NRubberBalls; ++i) {
+ //MobilizedBody::Cartesian
+ MobilizedBody::Free
+ rubberBall(matter.Ground(),
+ Transform(firstRubberBallPos+i*Vec3(0,2*RubberBallRadius+1,0)),
+ rubberBallInfo, Transform());
+ rubberBall.addBodyDecoration(Vec3(0),
+ rubberSphere.setColor((Real(i+1)/NRubberBalls)*Gray));
+ }
+
+ for (int i=0; i < NHardBalls; ++i) {
+ //MobilizedBody::Cartesian
+ MobilizedBody::Free
+ hardBall(matter.Ground(),
+ Transform(firstHardBallPos+i*Vec3(0,2*HardBallRadius+1,0)
+ + (i==NHardBalls-1)*Vec3(1e-14,0,1e-16)),
+ hardBallInfo, Transform());
+ hardBall.addBodyDecoration(Vec3(0),
+ hardSphere.setColor(colors[i % NColors]));
+ }
+
+
+ State s = mbs.realizeTopology();
+
+ const Real TargetElapsedTime = 10;
+ const Real FrameRate = 30;
+ //const Real TimeScale = 1;
+ //const Real FrameRate = 60;
+ const Real TimeScale = 3; // i.e., 3X realtime
+ Visualizer viz(mbs);
+ viz.setShowFrameRate(true);
+ viz.setShowSimTime(true);
+
+ //viz.setMode(Visualizer::Sampling);
+ //viz.setMode(Visualizer::PassThrough);
+ viz.setMode(Visualizer::RealTime);
+ viz.setDesiredFrameRate(FrameRate);
+ viz.setRealTimeScale(TimeScale);
+
+ // Illustrate the rod with a rubber band line.
+ DecorativeLine rbProto; rbProto.setColor(Orange).setLineThickness(1);
+ viz.addRubberBandLine(GroundIndex, pendGroundPt2,
+ pend2, Vec3(0,-linkLength/2,0), rbProto);
+
+ mbs.realizeModel(s);
+ bool suppressProjection = false;
+
+ //RungeKuttaMersonIntegrator ee(mbs); const Real Accuracy = .02;
+
+ //RungeKutta3Integrator ee(mbs); const Real Accuracy = .05;
+
+ //This runs the fastest:
+ SemiExplicitEuler2Integrator ee(mbs); const Real Accuracy = .3;
+ ee.setMaximumStepSize(.02); // 20ms
+
+ ee.setAccuracy(Accuracy);
+ ee.setConstraintTolerance(std::min(.001, Accuracy/10));
+
+
+ viz.report(s);
+
+ std::vector<State> saveEm;
+ saveEm.reserve(10000);
+
+ const Real h = TimeScale/FrameRate; // output every frame
+ const Real tstart = 0.;
+ const Real tmax = TimeScale*TargetElapsedTime;
+
+
+ s.updTime() = tstart;
+ mbs.realize(s, Stage::Acceleration);
+ for (int i=0; i<25; ++i)
+ saveEm.push_back(s); // delay
+ viz.report(s);
+
+ ee.initialize(s);
+ for (int i=0; i<25; ++i)
+ saveEm.push_back(ee.getState()); // delay
+ viz.report(ee.getState());
+
+ cout << "Num mobilities=" << matter.getNumMobilities() << endl;
+ cout << "Using Integrator " << std::string(ee.getMethodName()) << ":\n";
+ cout << "ACCURACY IN USE=" << ee.getAccuracyInUse() << endl;
+ cout << "CTOL IN USE=" << ee.getConstraintToleranceInUse() << endl;
+ cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
+ cout << "U WEIGHTS=" << s.getUWeights() << endl;
+ cout << "Z WEIGHTS=" << s.getZWeights() << endl;
+ cout << "1/QTOLS=" << s.getQErrWeights() << endl;
+ cout << "1/UTOLS=" << s.getUErrWeights() << endl;
+
+ const double startCPU = cpuTime();
+ const double startThreadCPU = threadCpuTime();
+ const double startTime = realTime();
+
+ int step = 0;
+ while (ee.getTime() <= tmax) {
+ const State& ss = ee.getState();
+ if (!(step % 30)) {
+ mbs.realize(ss);
+ cout << ss.getTime()
+ //<< ": T=" << thermostat.getCurrentTemperature(ss)
+ << " E=" << mbs.calcEnergy(ss)
+ << " (pe=" << mbs.calcPotentialEnergy(ss)
+ << ", ke=" << mbs.calcKineticEnergy(ss)
+ << ") qerr=" << matter.getQErr(ss).normRMS()
+ << " uerr=" << matter.getUErr(ss).normRMS()
+ << " hNext=" << ee.getPredictedNextStepSize() << endl;
+ }
+ ++step;
+
+ ee.stepTo(ss.getTime() + h);
+ #ifdef ANIMATE
+ viz.report(ss);
+ #endif
+ saveEm.push_back(ss);
+
+ //if (std::abs(ss.getTime()-30) < .001)
+ // thermostat.setBathTemperature(ee.updAdvancedState(), 10);
+ //if (std::abs(ss.getTime()-80) < .001)
+ // thermostat.setBathTemperature(ee.updAdvancedState(), 200);
+ }
+
+ std::cout << "Simulated " << ee.getTime() << " seconds in " <<
+ realTime()-startTime << " elapsed s\n";
+ std::cout << "Process CPU=" << cpuTime()-startCPU << std::endl;
+ std::cout << "Thread CPU=" << threadCpuTime()-startThreadCPU << std::endl;
+
+
+ //printCPodesStats(ee.getCPodes());
+ dumpIntegratorStats(ee);
+ viz.dumpStats(std::cout);
+
+ while(true) {
+ for (int i=0; i < (int)saveEm.size(); ++i) {
+ viz.report(saveEm[i]);
+ }
+ getchar();
+ }
+
+
+ }
+catch (const std::exception& e)
+ {
+ printf("EXCEPTION THROWN: %s\n", e.what());
+ }
+
+ return 0;
+}
+
+namespace {
+//==============================================================================
+// DUMP INTEGRATOR STATS
+//==============================================================================
+void dumpIntegratorStats(const Integrator& integ) {
+ const int evals = integ.getNumRealizations();
+ std::cout << "\nDone -- simulated " << integ.getTime() << "s with "
+ << integ.getNumStepsTaken() << " steps, avg step="
+ << (1000*integ.getTime())/integ.getNumStepsTaken() << "ms "
+ << (1000*integ.getTime())/evals << "ms/eval\n";
+
+ printf("Used Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(),
+ integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(),
+ integ.getNumProjections());
+}
+}
diff --git a/Simbody/tests/adhoc/ElasticFoundation.cpp b/Simbody/tests/adhoc/ElasticFoundation.cpp
new file mode 100644
index 0000000..4336102
--- /dev/null
+++ b/Simbody/tests/adhoc/ElasticFoundation.cpp
@@ -0,0 +1,247 @@
+#include "SimTKsimbody.h"
+#include "SimTKcommon/Testing.h"
+
+#include <cstdio>
+#include <iostream>
+#include <exception>
+
+using namespace SimTK;
+using std::cout;using std::endl;
+
+
+static const Real smallRad = 5*1.25/100;
+static const Real largeRad = 10.5/100;
+static const MobodIndex smallBodx = MobodIndex(1);
+static const MobodIndex largeBodx = MobodIndex(0);
+static const Vec3 smallPos(-.28,0,0);
+static const Vec3 largePos(-.28,0,0);
+static const Real hSmall = .03; // 6cm=100% strain
+static const Real hLarge = .03;
+
+class ForceReporter : public PeriodicEventReporter {
+public:
+ ForceReporter(const MultibodySystem& system,
+ const CompliantContactSubsystem& complCont,
+ Real reportInterval)
+ : PeriodicEventReporter(reportInterval), m_system(system),
+ m_compliant(complCont)
+ {}
+
+ ~ForceReporter() {}
+
+ void handleEvent(const State& state) const {
+ m_system.realize(state, Stage::Dynamics);
+ //cout << state.getTime() << ": E = " << m_system.calcEnergy(state)
+ // << " Ediss=" << m_compliant.getDissipatedEnergy(state)
+ // << " E+Ediss=" << m_system.calcEnergy(state)
+ // +m_compliant.getDissipatedEnergy(state)
+ // << endl;
+ const int ncont = m_compliant.getNumContactForces(state);
+ //cout << "Num contacts: " << m_compliant.getNumContactForces(state) << endl;
+ if (ncont==0)
+ printf("%g 0.0 0.0 0.0\n", state.getTime());
+
+ const SimbodyMatterSubsystem& matter = m_system.getMatterSubsystem();
+ const MobilizedBody& smallBod=matter.getMobilizedBody(smallBodx);
+ const MobilizedBody& largeBod=matter.getMobilizedBody(largeBodx);
+ const Vec3 smallCtr = smallBod.findStationLocationInGround(state, smallPos);
+ const Vec3 largeCtr = largeBod.findStationLocationInGround(state, largePos);
+ const Real d = (smallRad+largeRad)-(smallCtr-largeCtr).norm();
+
+ for (int i=0; i < ncont; ++i) {
+ const ContactForce& force = m_compliant.getContactForce(state,i);
+ const ContactId id = force.getContactId();
+ cout << state.getTime()
+ << " " << force.getForceOnSurface2()[1][1] // Normal
+ << " " << force.getForceOnSurface2()[1][0] // Tangential
+ << " " << d << "\n"; // penetration distance
+
+ //ContactPatch patch;
+ //const bool found = m_compliant.calcContactPatchDetailsById(state,id,patch);
+ //cout << "patch for id" << id << " found=" << found << endl;
+ //cout << "resultant=" << patch.getContactForce() << endl;
+ //cout << "num details=" << patch.getNumDetails() << endl;
+ //Real patchArea=0, maxDepth=0, maxf=0;
+ //Vec3 ftot(0);
+ //for (int i=0; i < patch.getNumDetails(); ++i) {
+ // const ContactDetail& detail = patch.getContactDetail(i);
+ // patchArea += detail.getPatchArea();
+ // maxDepth = std::max(maxDepth, detail.getDeformation());
+ // ftot += detail.getForceOnSurface2();
+ // maxf = std::max(maxf, detail.getForceOnSurface2().norm());
+ //}
+ //cout << "patchArea=" << patchArea
+ // << " ftot=" << ftot << " maxDepth=" << maxDepth << "\n";
+ //cout << "maxf=" << maxf << "\n";
+ }
+ }
+private:
+ const MultibodySystem& m_system;
+ const CompliantContactSubsystem& m_compliant;
+};
+
+// Nylon
+static const Real nylon_density = 1100.; // kg/m^3
+static const Real nylon_young = .05*2.5e9; // pascals (N/m)
+static const Real nylon_poisson = 0.4; // ratio
+static const Real nylon_planestrain =
+ ContactMaterial::calcPlaneStrainStiffness(nylon_young, nylon_poisson);
+static const Real nylon_confined =
+ ContactMaterial::calcConfinedCompressionStiffness(nylon_young, nylon_poisson);
+static const Real nylon_dissipation = 10*0.005;
+
+int main() {
+ try
+ { // Create the system.
+
+ MultibodySystem system; system.setUseUniformBackground(true);
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::Gravity gravity(forces, matter, -YAxis, 10);
+ //Force::GlobalDamper damper(forces, matter, 1.0);
+
+ ContactTrackerSubsystem tracker(system);
+ CompliantContactSubsystem contactForces(system, tracker);
+ contactForces.setTrackDissipatedEnergy(true);
+ contactForces.setTransitionVelocity(1e-3);
+
+ // g=10, mass==.5 => weight = .5*10=5N.
+ // body origin at the hinge on the right
+ Real mass = 0.5;
+ Vec3 com = Vec3(-.14,0,0);
+ Vec3 halfDims(.14, .01, .01); // a rectangular solid
+ Inertia inertia = mass*
+ UnitInertia::brick(halfDims).shiftFromCentroid(Vec3(.14,0,0));
+
+ Body::Rigid lidBody(MassProperties(mass,com,inertia));
+ lidBody.addDecoration(Vec3(-.14,0,0),
+ DecorativeBrick(halfDims).setColor(Cyan).setOpacity(.1));
+
+ PolygonalMesh smallMesh, largeMesh;
+
+#define USE_VIZ
+//#define SHOW_NORMALS
+Array_<DecorativeLine> smallNormals;
+Array_<DecorativeLine> largeNormals;
+const Real NormalLength = .001;
+
+#define USE_MESH_SMALL
+#define USE_MESH_BIG
+#ifdef USE_MESH_SMALL
+ smallMesh = PolygonalMesh::createSphereMesh(smallRad, 5);
+ std::cerr << "small mesh faces: " << smallMesh.getNumFaces() << "\n";
+ ContactGeometry::TriangleMesh smallContact(smallMesh);
+ DecorativeMesh smallArtwork(smallContact.createPolygonalMesh());
+ smallArtwork.setColor(Red);
+ #ifdef SHOW_NORMALS
+ for (int fx=0; fx < smallContact.getNumFaces(); ++fx) {
+ smallNormals.push_back(
+ DecorativeLine(smallContact.findCentroid(fx),
+ smallContact.findCentroid(fx)
+ + NormalLength*smallContact.getFaceNormal(fx))
+ .setColor(Black).setLineThickness(2));
+ }
+ #endif
+#else
+ DecorativeSphere smallArtwork(smallRad);
+ smallArtwork.setResolution(2).setRepresentation(DecorativeGeometry::DrawWireframe);
+ ContactGeometry::Sphere smallContact(smallRad);
+#endif
+#ifdef USE_MESH_BIG
+ largeMesh = PolygonalMesh::createSphereMesh(largeRad, 6);
+ std::cerr << "large mesh faces: " << largeMesh.getNumFaces() << "\n";
+ ContactGeometry::TriangleMesh largeContact(largeMesh);
+ DecorativeMesh largeArtwork(largeContact.createPolygonalMesh());
+ //DecorativeSphere largeArtwork(largeRad);
+ largeArtwork.setColor(Green);
+ #ifdef SHOW_NORMALS
+
+ for (int fx=0; fx < largeContact.getNumFaces(); ++fx) {
+ largeNormals.push_back(
+ DecorativeLine(largeContact.findCentroid(fx),
+ largeContact.findCentroid(fx)
+ + NormalLength*largeContact.getFaceNormal(fx))
+ .setColor(Black).setLineThickness(2));
+ }
+ #endif
+#else
+ DecorativeSphere largeArtwork(largeRad);
+ largeArtwork.setResolution(4).setRepresentation(DecorativeGeometry::DrawWireframe);
+ ContactGeometry::Sphere largeContact(largeRad);
+#endif
+
+
+ ContactMaterial nylon(
+ //nylon_planestrain,
+ nylon_confined,
+ nylon_dissipation,
+ .1*.9,.1*.8,.1*.6); // static, dynamic, viscous friction
+
+ lidBody.addDecoration(smallPos,
+ smallArtwork.setOpacity(0.5));
+ lidBody.addDecoration(smallPos,
+ smallArtwork.setRepresentation(DecorativeGeometry::DrawWireframe));
+
+ for (unsigned i=0; i < smallNormals.size(); ++i)
+ lidBody.addDecoration(smallPos, smallNormals[i]);
+
+ lidBody.addContactSurface(smallPos,
+ ContactSurface(smallContact, nylon, hSmall));
+
+ matter.Ground().updBody().addDecoration(largePos,
+ largeArtwork.setOpacity(0.5));
+ matter.Ground().updBody().addDecoration(largePos,
+ largeArtwork.setRepresentation(DecorativeGeometry::DrawWireframe));
+
+ for (unsigned i=0; i < largeNormals.size(); ++i)
+ matter.Ground().updBody().addDecoration(largePos,
+ largeNormals[i]);
+ matter.Ground().updBody().addContactSurface(largePos,
+ ContactSurface(largeContact, nylon, hLarge));
+
+ MobilizedBody::Pin lid(matter.Ground(), Vec3(0.05,smallRad+largeRad,0),
+ lidBody, Vec3(0,0,0));
+
+#ifdef USE_VIZ
+ Visualizer viz(system);
+ viz.setCameraClippingPlanes(.01,100);
+#endif
+
+ system.addEventReporter(new Visualizer::Reporter(viz, 1./1000));
+ ForceReporter* frcReporter =
+ new ForceReporter(system, contactForces, 1./100000);
+ system.addEventReporter(frcReporter);
+
+ RungeKuttaMersonIntegrator integ(system);
+ //CPodesIntegrator integ(system);
+ //integ.setMaximumStepSize(.01);
+ integ.setAccuracy(1e-5);
+ TimeStepper ts(system, integ);
+
+ system.realizeTopology();
+ State startState = system.getDefaultState();
+ lid.setOneQ(startState, 0, -Pi/10);
+ //lid.setOneQ(startState, 0, 0.0610351);
+ system.realize(startState, Stage::Position);
+#ifdef USE_VIZ
+ viz.report(startState);
+#endif
+ system.realize(startState);
+ //frcReporter->handleEvent(startState);
+ //printf("type to go ...\n");
+ // getchar();
+ ts.initialize(startState);
+ ts.stepTo(0.5);
+
+ } catch (const std::exception& e) {
+ std::printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+
+ } catch (...) {
+ std::printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+ return 0;
+}
+
diff --git a/Simbody/tests/adhoc/FreeWater.cpp b/Simbody/tests/adhoc/FreeWater.cpp
new file mode 100644
index 0000000..a73f986
--- /dev/null
+++ b/Simbody/tests/adhoc/FreeWater.cpp
@@ -0,0 +1,219 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): Free Water Test *
+ * -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This is an outer block for simulating ??? in various ways with Simbody.
+ * This is about testing Simbody, *not* studying ???!
+ */
+
+#include "SimTKsimbody.h"
+
+#include <string>
+#include <iostream>
+#include <exception>
+
+using std::cout;
+using std::cin;
+using std::endl;
+
+using namespace SimTK;
+
+static const Real Deg2Rad = (Real)SimTK_DEGREE_TO_RADIAN,
+ Rad2Deg = (Real)SimTK_RADIAN_TO_DEGREE;
+
+static Real g = 9.8;
+static Real m = 1;
+
+int main(int argc, char** argv) {
+ static const Transform GroundFrame;
+ static const Rotation ZalongY(UnitVec3(XAxis), XAxis, UnitVec3(YAxis), ZAxis);
+ static const Rotation ZalongX(UnitVec3(XAxis), ZAxis, UnitVec3(YAxis), YAxis);
+ static const Rotation YalongZ(UnitVec3(ZAxis), XAxis, UnitVec3(XAxis), YAxis);
+ static const Vec3 TestLoc(1,0,0);
+
+ try { // If anything goes wrong, an exception will be thrown.
+
+ // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
+ MultibodySystem mbs;
+
+ SimbodyMatterSubsystem matter(mbs);
+ GeneralForceSubsystem forces(mbs);
+ DecorationSubsystem viz(mbs);
+ //Force::UniformGravity gravity(forces, matter, Vec3(0, -g, 0));
+
+ // ADD BODIES AND THEIR MOBILIZERS
+ Body::Rigid oxygen = Body::Rigid(MassProperties(m, Vec3(0), Inertia(0)));
+ oxygen.addDecoration(DecorativeSphere(.1).setColor(Red).setOpacity(.3));
+ Body::Rigid hydrogen = Body::Rigid(MassProperties(m, Vec3(0), Inertia(0)));
+ hydrogen.addDecoration(DecorativeSphere(.05).setColor(Green).setOpacity(.3));
+
+ MobilizedBody::Cartesian
+ masslessFrame(matter.Ground(), Transform(ZalongY, Vec3(0)),
+ MassProperties(0,Vec3(0),Inertia(0)), Transform());
+
+ MobilizedBody::Pin
+ H1(masslessFrame, Transform(), hydrogen, Transform());
+
+ MobilizedBody::Pin
+ O(H1, ZalongY, oxygen, Transform(Vec3(0,1,0)));
+
+ MobilizedBody::Universal
+ H2(O, Transform(YalongZ), hydrogen, Transform(Vec3(0,0,1)));
+
+ Force::MobilityLinearSpring(forces, H2, 1, 2, 60*Deg2Rad); // harmonic bend
+
+
+ //MobilizedBody::SphericalCoords
+ // H2(matter.Ground(), Transform(ZUp, TestLoc),
+ // particle, Transform(),
+ // 0*Deg2Rad, false, // azimuth offset, negated
+ // 0, false, // zenith offset, negated
+ // ZAxis, true); // translation axis, negated
+
+ //anAtom.setDefaultRadius(.1);
+ //anAtom.setDefaultAngles(Vec2(0, 30*Deg2Rad));
+
+ //viz.addRubberBandLine(matter.Ground(), TestLoc,
+ // anAtom, Vec3(0),
+ // DecorativeLine().setColor(Orange).setLineThickness(4));
+
+ //Force::MobilityLinearSpring(forces, anAtom, 1, 2, 45*Deg2Rad); // harmonic bend
+ //Force::MobilityLinearSpring(forces, anAtom, 2, 20, .5); // harmonic stretch
+
+ //Force::MobilityLinearDamper(forces, anAtom, 0, .1); // harmonic bend
+ //Force::MobilityLinearDamper(forces, anAtom, 1, .1); // harmonic bend
+ //Force::MobilityLinearDamper(forces, anAtom, 2, .1); // harmonic stretch
+
+
+ State s = mbs.realizeTopology(); // returns a reference to the the default state
+ mbs.realizeModel(s); // define appropriate states for this System
+ mbs.realize(s, Stage::Instance); // instantiate constraints if any
+
+ Visualizer display(mbs);
+ display.setBackgroundColor(White);
+ display.setBackgroundType(Visualizer::SolidColor);
+
+ mbs.realize(s, Stage::Velocity);
+ display.report(s);
+
+ cout << "q=" << s.getQ() << endl;
+ cout << "u=" << s.getU() << endl;
+
+ char c;
+ //cout << "Default configuration shown. Ready?\n"; cin >> c;
+
+ O.setOneQ(s, 0, 30*Deg2Rad);
+ //H2.setOneQ(s, 0, 30*Deg2Rad);
+ H2.setOneQ(s, 0, 30*Deg2Rad);
+ H2.setOneQ(s, 1, 50*Deg2Rad);
+
+ s.updU() = Vector(Vec7(0,0,0,1,1,1,0));
+
+ //anAtom.setQToFitRotation(s, Rotation(-.9*Pi/2,YAxis));
+
+ //while (true) {
+ // Real x;
+ // cout << "Torsion (deg)? "; cin >> x; if (x==1234) break;
+ // Vec2 a = anAtom.getAngles(s); a[0]=x*Deg2Rad; anAtom.setAngles(s, a);
+ // display.report(s);
+ // cout << "Bend (deg)? "; cin >> x; if (x==1234) break;
+ // a = anAtom.getAngles(s); a[1]=x*Deg2Rad; anAtom.setAngles(s, a);
+ // display.report(s);
+ // cout << "Radius? "; cin >> x; if (x==1234) break;
+ // anAtom.setRadius(s, x);
+ // display.report(s);
+ //}
+ //anAtom.setUToFitAngularVelocity(s, Vec3(.1,.2,.3));
+
+ //anAtom.setAngle(s, 45*Deg2Rad);
+ //anAtom.setTranslation(s, Vec2(.4, .1));
+
+ mbs.realize(s, Stage::Dynamics);
+ mbs.realize(s, Stage::Acceleration);
+
+ cout << "q=" << s.getQ() << endl;
+ cout << "u=" << s.getU() << endl;
+ cout << "qdot=" << s.getQDot() << endl;
+ cout << "udot=" << s.getUDot() << endl;
+ cout << "qdotdot=" << s.getQDotDot() << endl;
+ display.report(s);
+
+ cout << "Initialized configuration shown. Ready? ";
+ cin >> c;
+
+ RungeKuttaMersonIntegrator myStudy(mbs);
+ myStudy.setAccuracy(1e-4);
+
+ const Real dt = .02; // output intervals
+ const Real finalTime = 20;
+
+ myStudy.setFinalTime(finalTime);
+
+ // Peforms assembly if constraints are violated.
+ myStudy.initialize(s);
+
+ cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
+ cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
+ cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
+ cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
+ cout << "U WEIGHTS=" << s.getUWeights() << endl;
+ cout << "Z WEIGHTS=" << s.getZWeights() << endl;
+ cout << "1/QTOLS=" << s.getQErrWeights() << endl;
+ cout << "1/UTOLS=" << s.getUErrWeights() << endl;
+
+ Integrator::SuccessfulStepStatus status;
+ int nextReport = 0;
+ while ((status=myStudy.stepTo(nextReport*dt))
+ != Integrator::EndOfSimulation)
+ {
+ const State& s = myStudy.getState();
+ mbs.realize(s);
+ printf("%5g %10.4g %10.4g %10.4g %10.4g E=%10.8g h%3d=%g %s%s\n", s.getTime(),
+ H1.getAngle(s), O.getAngle(s), H2.getOneQ(s,0), H2.getOneQ(s,1),
+ mbs.calcEnergy(s), myStudy.getNumStepsTaken(),
+ myStudy.getPreviousStepSizeTaken(),
+ Integrator::getSuccessfulStepStatusString(status).c_str(),
+ myStudy.isStateInterpolated()?" (INTERP)":"");
+
+ display.report(s);
+
+ if (status == Integrator::ReachedReportTime)
+ ++nextReport;
+ }
+
+ printf("Using Integrator %s:\n", myStudy.getMethodName());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());
+
+ }
+ catch (const std::exception& e) {
+ printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+ }
+ catch (...) {
+ printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+}
diff --git a/Simbody/tests/adhoc/GeometryPlayground.cpp b/Simbody/tests/adhoc/GeometryPlayground.cpp
new file mode 100644
index 0000000..87a0ac3
--- /dev/null
+++ b/Simbody/tests/adhoc/GeometryPlayground.cpp
@@ -0,0 +1,744 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm): Geometry Playground *
+ * -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "Simbody.h"
+#include "simmath/internal/OBBTree.h"
+
+using namespace SimTK;
+using std::cout; using std::endl;
+
+// Trim r to 2 decimal places.
+static Real trim2(Real r) {
+ return std::floor(r*100+0.5)/100;
+}
+
+template <class P>
+static void drawBoundingSphere( const Array_<Vec<3,P> >& v,
+ const Transform& pose,
+ MobilizedBody& body,
+ const Vec3& color=Orange)
+{
+ Array_<int> which, support;
+ Real tstart = realTime();
+ Geo::Sphere_<P> pms = Geo::Point_<P>::calcBoundingSphere(v, which);
+ printf("Time for %d points: %gs\n", v.size(), realTime()-tstart);
+ cout << "ctr=" << pms.getCenter() << " rad=" << pms.getRadius() << "\n";
+ cout << "volume=" << pms.findVolume() << "\n";
+ for (unsigned i=0; i<v.size(); ++i) {
+ const P d = pms.getRadius()-(v[i]-pms.getCenter()).norm();
+ if (d < 0)
+ printf("*** %d outside by %.17g\n", i, d);
+ }
+ std::cout << "which=" << which << "\n";
+ Decorations dec;
+ //dec.addDecoration(Vec3(pms.getCenter()),
+ // DecorativeSphere(pms.getRadius()).setColor(color).setResolution(5));
+ //for (unsigned i=0; i < which.size(); ++i)
+ // dec.addDecoration(Vec3(v[which[i]]), DecorativePoint().setColor(Green).setScale(3));
+ //body.addBodyDecoration(pose,
+ // dec.setOpacity(.3).setLineThickness(1));
+
+ tstart = realTime();
+ SimTK::Geo::OrientedBox_<P> obb = Geo::Point_<P>::calcOrientedBoundingBox(v, support);
+ printf("OBB time for %d points: %gs\n", v.size(), realTime()-tstart);
+ cout << "ctr=" << obb.getCenter() << " rad=" << obb.getHalfLengths() << "\n";
+ cout << "volume=" << obb.getBox().findVolume() << "\n";
+
+
+ Decorations obbDec;
+ Transform X_FB; X_FB.updP() = Vec3(obb.getTransform().p());
+ UnitVec3 x(Vec3(obb.getTransform().x())), y(Vec3(obb.getTransform().y()));
+ X_FB.updR() = Rotation(x, XAxis, y, YAxis);
+ obbDec.addDecoration(X_FB,
+ DecorativeBrick(Vec3(obb.getHalfLengths())).setColor(Blue)
+ //.setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setLineThickness(2).setOpacity(0.1));
+ for (unsigned i=0; i < support.size(); ++i) {
+ const Vec<3,P>& supP = v[support[i]];
+ const Vec3 sup((Real)supP[0], (Real)supP[1], (Real)supP[2]);
+ obbDec.addDecoration(sup,
+ DecorativePoint().setColor(Blue).setScale(4));
+ }
+
+ Array_<int> badSupport;
+ Geo::OrientedBox_<P> badObb =
+ Geo::Point_<P>::calcOrientedBoundingBox(v, badSupport, false);
+ X_FB.updP() = Vec3(badObb.getTransform().p());
+ x = UnitVec3(Vec3(badObb.getTransform().x()));
+ y = UnitVec3(Vec3(badObb.getTransform().y()));
+ X_FB.updR() = Rotation(x, XAxis, y, YAxis);
+ obbDec.addDecoration(X_FB,
+ DecorativeBrick(Vec3(badObb.getHalfLengths())).setColor(Gray)
+ //.setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setOpacity(0.1));
+
+ body.addBodyDecoration(pose, obbDec);
+}
+
+
+template <class P>
+static void drawApproxBoundingSphere( const Array_<Vec<3,P> >& v,
+ const Transform& pose,
+ MobilizedBody& body,
+ const Vec3& color=Gray)
+{
+ return;
+ Real tstart = realTime();
+ SimTK::Geo::Sphere_<P> pms = Geo::Point_<P>::calcApproxBoundingSphere(v);
+ printf("Time for approx sphere around %d points: %gs\n",
+ v.size(), realTime()-tstart);
+ cout << "ctr=" << pms.getCenter() << " rad=" << pms.getRadius() << "\n";
+ cout << "volume=" << pms.findVolume() << "\n";
+ for (unsigned i=0; i<v.size(); ++i) {
+ const P d = pms.getRadius()-(v[i]-pms.getCenter()).norm();
+ if (d < 0)
+ printf("*** %d outside by %.17g\n", i, d);
+ }
+ Decorations dec;
+ dec.addDecoration(Vec3(pms.getCenter()),
+ DecorativeSphere(pms.getRadius()).setColor(color).setResolution(5));
+ body.addBodyDecoration(pose,
+ dec.setOpacity(.1).setLineThickness(1));
+}
+
+static void draw(const Geo::CubicBezierCurve& curve, const Transform& pose,
+ MobilizedBody& body, const Vec3& color=Red, bool control=true)
+{
+ const int Resolution=10;
+ Decorations dec; dec.setColor(color).setLineThickness(2);
+
+ Vec3 prev=curve.evalP(0);
+ for (int i=1; i<=Resolution; ++i) {
+ const Real u = (Real)i/Resolution;
+ Vec3 p = curve.evalP(u);
+ dec.addDecoration(DecorativeLine(prev,p));
+ prev=p;
+ }
+ if (control) {
+ Vec<4,Vec3> B=curve.getControlPoints();
+ for (int i=0; i<4; ++i) {
+ dec.addDecoration(DecorativePoint(B[i]));
+ dec.addDecoration(DecorativeLine(B[i], B[(i+1)%4])
+ .setColor(Gray).setLineThickness(1));
+ }
+ dec.addDecoration(DecorativeLine(B[0], B[2])
+ .setColor(Green).setLineThickness(1));
+ dec.addDecoration(DecorativeLine(B[1], B[3])
+ .setColor(Green).setLineThickness(1));
+
+ const int NumFrames = 0*100;
+ for (int i=0; i < NumFrames; ++i) {
+ const Real u = (Real)i/(NumFrames-1);
+ Transform X_FP;
+ const Real k = curve.calcCurveFrame(u, X_FP);
+ dec.addDecoration(
+ DecorativeLine(X_FP.p(), X_FP.p()+X_FP.x()/4)
+ .setColor(Blue).setLineThickness(1));
+ dec.addDecoration(
+ DecorativeLine(X_FP.p(), X_FP.p()+X_FP.z()/8)
+ .setColor(Red).setLineThickness(1));
+ }
+ }
+ body.addBodyDecoration(pose,dec);
+}
+
+static void drawLines(const Vec3& a, const Vec3& b, const Vec3& c, const Vec3& d,
+ const Transform& pose,
+ MobilizedBody& body, const Vec3& color=Black)
+{
+ body.addBodyDecoration(pose, DecorativeLine(a,b).setLineThickness(2));
+ body.addBodyDecoration(pose, DecorativeLine(b,c).setLineThickness(2));
+ body.addBodyDecoration(pose, DecorativeLine(c,d).setLineThickness(2));
+ body.addBodyDecoration(pose, DecorativeLine(d,a).setLineThickness(2));
+}
+
+
+static void draw(const Geo::BicubicBezierPatch& patch, const Transform& pose,
+ MobilizedBody& body, const Vec3& color=Red, bool control=true)
+{
+ const int Resolution=10;
+ Mat<4,4,Vec3> B=patch.getControlPoints();
+
+ for (int i=0; i<=Resolution; ++i) {
+ const Real t = Real(i)/Resolution;
+ Geo::CubicBezierCurve iso = Geo::BicubicBezierPatch::calcIsoCurveU(B,t);
+ draw(iso,pose,body,color,false);
+ iso = Geo::BicubicBezierPatch::calcIsoCurveW(B,t);
+ draw(iso,pose,body,color,false);
+ }
+
+ if (control) {
+ for (int i=0; i<4; ++i)
+ for (int j=0; j<4; ++j) {
+ body.addBodyDecoration(pose,
+ DecorativePoint(B[i][j]).setScale(4).setColor(Magenta));
+ }
+ drawLines(B[0][0], B[0][3], B[3][3], B[3][0], pose, body);//base
+ drawLines(B[1][1], B[1][2], B[2][2], B[2][1], pose, body);//cap
+
+ drawLines(B[0][0], B[0][1], B[0][2], B[0][3], pose, body);//bounds
+ drawLines(B[3][0], B[3][1], B[3][2], B[3][3], pose, body);
+ drawLines(B[0][0], B[1][0], B[2][0], B[3][0], pose, body);
+ drawLines(B[0][3], B[1][3], B[2][3], B[3][3], pose, body);
+
+ drawLines(B[0][0], B[0][1], B[1][1], B[1][0], pose, body);//corners
+ drawLines(B[2][0], B[2][1], B[3][1], B[3][0], pose, body);
+ drawLines(B[2][2], B[2][3], B[3][3], B[3][2], pose, body);
+ drawLines(B[0][2], B[0][3], B[1][3], B[1][2], pose, body);
+
+ drawLines(B[0][1], B[0][2], B[1][2], B[1][1], pose, body);//middles
+ drawLines(B[1][0], B[1][1], B[2][1], B[2][0], pose, body);
+ drawLines(B[2][1], B[2][2], B[3][2], B[3][1], pose, body);
+ drawLines(B[1][2], B[1][3], B[2][3], B[2][2], pose, body);
+
+ Geo::OrientedBox bs = patch.calcOrientedBoundingBox();
+ body.addBodyDecoration(pose*bs.getTransform(),
+ DecorativeBrick(bs.getHalfLengths()).setOpacity(.2)
+ .setResolution(10));
+ }
+}
+
+void makeDecorations(int level, const OBBNode& node, int which,
+ Decorations& dec) {
+ static Vec3 colors[6] = {Cyan, Green, Magenta, Blue, Orange, Red};
+
+ Vec3 color = colors[node.height%6];
+
+ if (node.height ==0 || node.depth<2)
+ dec.addDecoration(node.box.getTransform(),
+ DecorativeBrick(node.box.getHalfLengths()).setColor(color)
+ .setOpacity(Real(.9)/(node.height+1)));
+
+ for (unsigned i=0; i<node.children.size(); ++i)
+ makeDecorations(level+1,node.children[i],which,dec);
+}
+
+void draw(const OBBTree& tree, int which,
+ const Transform& pose, MobilizedBody& body) {
+ const OBBNode& root = tree.getRoot();
+ Decorations dec;
+ makeDecorations(0, root, which, dec);
+ body.addBodyDecoration(pose, dec);
+}
+
+void drawSpline(const Vector& x, const Vector& y,
+ MobilizedBody& body) {
+
+ Vec3 colors[3] = {Blue, Magenta, Green};
+
+ for (int m=1; m <= 3; ++m) {
+ int order = 2*m-1;
+
+ Spline_<Real> curve[3] = {
+ SplineFitter<Real>::fitForSmoothingParameter(order, x, y, 0)
+ .getSpline(),
+ SplineFitter<Real>::fitForSmoothingParameter(order, x, y, .1)
+ .getSpline(),
+ SplineFitter<Real>::fitForSmoothingParameter(order, x, y, 10)
+ .getSpline()
+ };
+
+ Vec3 offs[3] = {Vec3(10*m,0,0), Vec3(10*m,-3,0), Vec3(10*m,-6,0)};
+ for (int c=0; c<3; ++c)
+ for (int i=0; i < x.size(); ++i) {
+ body.addBodyDecoration(offs[c],
+ DecorativePoint(Vec3(x[i],y[i],0)).setScale(2)
+ .setColor(colors[c]));
+ }
+
+ const int NSegs = 100;
+ const Real dx = (x[x.size()-1]-x[0])/NSegs;
+ for (int c=0; c<3; ++c)
+ for (int s=0; s<NSegs; ++s) {
+ Real xx = x[0] + s*dx;
+ Vec3 p1(xx, curve[c].calcValue(Vector(1,xx)), 0);
+ Vec3 p2(xx+dx, curve[c].calcValue(Vector(1,xx+dx)), 0);
+ body.addBodyDecoration(offs[c],
+ DecorativeLine(p1,p2).setColor(colors[c])
+ .setLineThickness(3));
+ }
+
+ }
+}
+
+
+int main() {
+
+ // Create the system, with subsystems for the bodies and some forces.
+ MultibodySystem system;
+ system.setUseUniformBackground(true);
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+
+ Real spxdata[] = {0,1,2,3,4,5,6};
+ Real spydata[] = {0,1,1.5,2,1,1,3};
+ Vector spx(7, spxdata), spy(7, spydata);
+ drawSpline(spx, spy, matter.Ground());
+
+ Vec<4,Vec3> B( Vec3(3,0,0), Vec3(5,.5,2), Vec3(4,1,0), Vec3(6,0,0));
+ Geo::CubicBezierCurve curve(B);
+ draw(curve, Vec3(0), matter.Ground(), Orange);
+ Geo::CubicBezierCurve left, right,rl,rr;
+ curve.split(.25, left, right);
+ right.bisect(rl,rr);
+ Vec3 offs(0,0,-2);
+ draw(left, offs, matter.Ground(), Blue);
+ draw(rl, offs, matter.Ground(), Green);
+ draw(rr, offs, matter.Ground(), Magenta);
+ Geo::OrientedBox leftOBB = left.calcOrientedBoundingBox();
+ Geo::OrientedBox rlOBB = rl.calcOrientedBoundingBox();
+ Geo::OrientedBox rrOBB = rr.calcOrientedBoundingBox();
+ matter.Ground().addBodyDecoration(leftOBB.getTransform()+offs,
+ DecorativeBrick(leftOBB.getHalfLengths()).setOpacity(.1).setColor(Blue));
+ matter.Ground().addBodyDecoration(rlOBB.getTransform()+offs,
+ DecorativeBrick(rlOBB.getHalfLengths()).setOpacity(.1).setColor(Green));
+ matter.Ground().addBodyDecoration(rrOBB.getTransform()+offs,
+ DecorativeBrick(rrOBB.getHalfLengths()).setOpacity(.1).setColor(Magenta));
+
+ Geo::Box box(Vec3(3,4,2)); // half lengths
+
+ Geo::OrientedBox obox(Transform(), Vec3(1,2,3));
+ // Non-intersecting box for which no face will serve as separator.
+ // In this case the fast method can't tell they are separated.
+ obox.setTransform(Transform(
+ Rotation(BodyRotationSequence, Pi/4, XAxis, Pi/8, YAxis, -Pi/4, ZAxis),
+ Vec3(1.5, -5, 5.25)));
+ matter.Ground().addBodyDecoration(Vec3(0),
+ DecorativeBrick(box.getHalfLengths()).setOpacity(.1).setColor(Blue)
+ .setIndexOnBody(14).setUserRef(&matter.Ground()));
+ matter.Ground().addBodyDecoration(obox.getTransform(),
+ DecorativeBrick(obox.getHalfLengths()).setOpacity(.1).setColor(Green)
+ .setIndexOnBody(22).setUserRef(&matter.Ground()));
+ cout << "May intersect=" << box.mayIntersectOrientedBox(obox) << "\n";
+ cout << "Intersects=" << box.intersectsOrientedBox(obox) << "\n";
+
+ const Body& groundBody = matter.Ground().getBody();
+ const int nDecGeoms = groundBody.getNumDecorations();
+ printf("%d Ground body decorations (&Ground=0x%llx). Last two:\n", nDecGeoms,
+ (unsigned long long)&matter.Ground());
+ for (int i=nDecGeoms-2; i < nDecGeoms; ++i) {
+ printf("%d: bodyId=%d, index=%d, userRef=0x%llx\n", i,
+ groundBody.getDecoration(i).getBodyId(),
+ groundBody.getDecoration(i).getIndexOnBody(),
+ (unsigned long long)groundBody.getDecoration(i).getUserRef());
+ }
+
+ Geo::Sphere curveSphere = curve.calcBoundingSphere();
+ Geo::AlignedBox curveAABB = curve.calcAxisAlignedBoundingBox();
+ Geo::OrientedBox curveOBB = curve.calcOrientedBoundingBox();
+ cout << "vs=" << curveSphere.findVolume()
+ << " as=" << curveAABB.getBox().findVolume()
+ << " os=" << curveOBB.getBox().findVolume() << "\n";
+ //matter.Ground().addBodyDecoration(curveSphere.getCenter(),
+ // DecorativeSphere(curveSphere.getRadius()).setOpacity(.1));
+ //matter.Ground().addBodyDecoration(curveAABB.getCenter(),
+ // DecorativeBrick(curveAABB.getHalfLengths())
+ // .setRepresentation(DecorativeGeometry::DrawWireframe)
+ // .setColor(Gray));
+ //matter.Ground().addBodyDecoration(curveOBB.getTransform(),
+ // DecorativeBrick(curveOBB.getHalfLengths())
+ // .setOpacity(.1)
+ // .setColor(Blue));
+ const int n=5;
+ const fVec3 shft(0,0,0);
+ fVec3 pd[] = {shft+fVec3(.5f,0,-.5f), shft+fVec3(0,1,.4f), shft+fVec3(-.5f,1e-3f,0)
+ , shft+fVec3(.5f,-.2f,.1f), shft+fVec3(.5f,.5f,.5f)
+ };
+ Array_<int> which;
+ //Array_<fVec3> p(pd, pd+n);
+ Array_<fVec3> p;
+ fVec3 start(0,0,0); float r=.7f; float noise=1e-7f;
+ p.push_back(fVec3(r,0,0));
+ p.push_back(fVec3(0,r,0));
+ p.push_back(fVec3(0,0,r));
+ for (int i=0; i<n-3; ++i) {
+ UnitVec3 uv(Test::randVec3());
+ float nz = noise*(float)Test::randReal();
+ fVec3 fuv((float)uv[0],(float)uv[1],(float)uv[2]);
+ fVec3 fru = start+(r+nz)*fuv;
+ p.push_back(fru);
+ }
+ drawBoundingSphere<float>(p, Transform(), matter.Ground());
+ drawApproxBoundingSphere<float>(p, Transform(), matter.Ground());
+
+ const int Nx = 4, Ny = 5;
+ const Real xData[Nx] = { .1, 1, 2, 4 };
+ const Real yData[Ny] = { -3, -2, 0, 1, 3 };
+ const Real fData[Nx*Ny] = { 1, 2, 3, 3, 2,
+ 1.1, 2.1, 3.1, 3.1, 2.1,
+ 1, 2, 7, 3, 2,
+ 1.2, 2.2, 3.2, 3.2, 2.2 };
+ const Vector x(Nx, xData);
+ const Vector y(Ny, yData);
+ const Matrix f(Nx,Ny, fData);
+ BicubicSurface rough(x, y, f, 0);
+ BicubicSurface smooth(x, y, f, 1);
+
+
+
+ Vector xp(Vec2(.25,3.25)), yp(Vec2(.75,5.75));
+ // Worst possible patch?:
+ Matrix fp(Mat22(1.5, 1.7,
+ 1.3, 1.6));
+ Matrix fxp(2*Mat22(-1.1, -1.2,
+ -1.3, -1.4));
+ Matrix fyp(-2*Mat22(1.2, 1.3,
+ 1.4, 1.1));
+ Matrix fxyp(2*Mat22(1.01, -1.02,
+ -1.04, 1.03));
+ // Nice patch:
+ //Matrix fp(Mat22(1, 1,
+ // 1, 1));
+ //Matrix fxp(.5*Mat22(-1, 0,
+ // 0, -1));
+ //Matrix fyp(.5*Mat22(-1, 0,
+ // 0, -1));
+ //Matrix fxyp(0*Mat22(1, -1,
+ // -1, 1));
+ // One-hump patch:
+ //Matrix fp(Mat22(1, 1,
+ // 1, 1));
+ //Matrix fxp(Mat22(1, 1,
+ // -1, -1));
+ //Matrix fyp(Mat22(1, -1,
+ // 1, -1));
+ //Matrix fxyp(0.5*Mat22(1, 3,
+ // -3, 4));
+
+ BicubicSurface patch(xp, yp, fp, fxp, fyp, fxyp);
+ Rotation xm90(-Pi/2, XAxis);
+ Transform patchPose(xm90, Vec3(4,2,0));
+
+
+ int nx,ny; patch.getNumPatches(nx,ny);
+ printf("surface 'patch' has %dx%d patches\n", nx,ny);
+ Geo::BicubicBezierPatch bpatch = patch.calcBezierPatch(0,0);
+ const Mat<4,4,Vec3>& pts = bpatch.getControlPoints();
+ cout << "Bezier pts:\n" << pts;
+ Geo::BicubicBezierPatch patch00,patch01,patch10,patch11;
+ bpatch.split(.2, .3, patch00,patch01,patch10,patch11);
+
+ draw(patch00, patchPose+Vec3(5,0,0), matter.Ground(), Cyan);
+ draw(patch01, patchPose+Vec3(5,0,0), matter.Ground(), Green);
+ draw(patch10, patchPose+Vec3(5,0,0), matter.Ground(), Purple);
+ draw(patch11, patchPose+Vec3(5,0,0), matter.Ground(), Blue);
+
+
+
+ for (int i=0; i<4; ++i)
+ for (int j=0; j<4; ++j)
+ matter.Ground().addBodyDecoration(patchPose,
+ DecorativePoint(pts[i][j]).setScale(4).setColor(Magenta));
+
+ draw(bpatch.getBoundaryCurveU0(), patchPose,matter.Ground());
+ draw(bpatch.calcIsoCurveU(pts,.5), patchPose, matter.Ground());
+ draw(bpatch.getBoundaryCurveU1(), patchPose,matter.Ground());
+ draw(bpatch.getBoundaryCurveW0(), patchPose,matter.Ground(), Blue);
+ draw(bpatch.calcIsoCurveW(pts,.5), patchPose, matter.Ground(), Blue);
+ draw(bpatch.getBoundaryCurveW1(), patchPose,matter.Ground(), Blue);
+
+ Real resolution = 31;
+ PolygonalMesh patchMesh = patch.createPolygonalMesh(resolution);
+
+ Array_<Vec3> v(patchMesh.getNumVertices());
+ for (int i=0; i < patchMesh.getNumVertices(); ++i)
+ v[i] = patchMesh.getVertexPosition(i);
+ //drawBoundingSphere<Real>(v, patchPose, matter.Ground());
+ //drawApproxBoundingSphere<Real>(v, patchPose, matter.Ground());
+
+ matter.Ground().addBodyDecoration(patchPose,
+ DecorativeMesh(patchMesh).setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setColor(Gray));
+ //matter.Ground().addBodyDecoration(patchPose,
+ // DecorativeMesh(patchMesh)
+ // .setColor(Blue).setOpacity(.4));
+
+ matter.Ground().addBodyDecoration(patchPose,
+ Decorations()
+ .addDecoration(DecorativePoint(Vec3(xp[0],yp[0],fp(0,0))))
+ .addDecoration(DecorativePoint(Vec3(xp[0],yp[1],fp(0,1))).setColor(Blue))
+ .addDecoration(DecorativePoint(Vec3(xp[1],yp[0],fp(1,0))))
+ .addDecoration(DecorativePoint(Vec3(xp[1],yp[1],fp(1,1))))
+ );
+
+ //BicubicSurface surf(Vec2(0,0), Vec2(1,1), f, 0);
+ Array_<int> dxx(2,0), dyy(2,0), dxy(2);
+ dxy[0]=0; dxy[1]=1;
+ Array_<int> dx(1,0), dy(1,1);
+
+ Transform pose1(xm90, Vec3(0,-15,0));
+ Transform pose2(xm90, Vec3(5,-15,0));
+
+
+ ContactGeometry::SmoothHeightMap smoothMap(smooth);
+
+ draw(smoothMap.getOBBTree(), 1, pose2, matter.Ground());
+
+//#ifdef NOTDEF
+ for (int s=0; s<=1; ++s) {
+ BicubicSurface surf = s?smooth:rough;
+ Transform pose =s?pose2:pose1;
+ for (int i=0; i<Nx; ++i)
+ for (int j=0; j<Ny; ++j) {
+ const Vec2 pt(x[i],y[j]);
+
+ Vec2 k; Transform X_SP;
+ surf.calcParaboloid(pt, X_SP, k);
+ const Vec3& P = X_SP.p();
+
+ // The original tangents.
+ const Vec3 dPdx(1,0,surf.calcDerivative(dx,pt));
+ const Vec3 dPdy(0,1,surf.calcDerivative(dy,pt));
+
+ const Real BigLen = 0.5, TextScale=.1;
+ matter.Ground().addBodyDecoration(pose,
+ DecorativeLine(P, P+BigLen*X_SP.z()).setColor(Black));
+ matter.Ground().addBodyDecoration(pose,
+ DecorativeLine(P, P+BigLen*X_SP.x()).setColor(Red).setLineThickness(3));
+ matter.Ground().addBodyDecoration(
+ Transform(pose.p() + pose.R()*(P+1.1*BigLen*X_SP.x())),
+ DecorativeText(String(trim2(k[0]))).setColor(Red).setScale(TextScale)
+ .setFaceCamera(false));
+ matter.Ground().addBodyDecoration(pose,
+ DecorativeLine(P, P+BigLen*X_SP.y()).setColor(Blue).setLineThickness(3));
+ matter.Ground().addBodyDecoration(
+ Transform(pose.p() + pose.R()*(P+1.1*BigLen*X_SP.y())),
+ DecorativeText(String(trim2(k[1]))).setColor(Blue).setScale(TextScale));
+ matter.Ground().addBodyDecoration(pose,
+ DecorativeLine(P, P+.75*BigLen*UnitVec3(dPdx)).setColor(Red));
+ matter.Ground().addBodyDecoration(pose,
+ DecorativeLine(P, P+.75*BigLen*UnitVec3(dPdy)).setColor(Blue));
+ }
+ }
+
+ PolygonalMesh origMesh = rough.createPolygonalMesh(0);
+ PolygonalMesh roughMesh = rough.createPolygonalMesh(resolution);
+ PolygonalMesh smoothMesh = smooth.createPolygonalMesh(resolution);
+ printf("stats: access=%d, same pt=%d, same patch=%d, nearby=%d\n",
+ rough.getNumAccesses(), rough.getNumAccessesSamePoint(),
+ rough.getNumAccessesSamePatch(), rough.getNumAccessesNearbyPatch());
+
+ v.resize(roughMesh.getNumVertices());
+ for (int i=0; i < roughMesh.getNumVertices(); ++i)
+ v[i] = roughMesh.getVertexPosition(i);
+ drawBoundingSphere(v, pose1, matter.Ground());
+ drawApproxBoundingSphere(v, pose1, matter.Ground());
+
+ v.resize(smoothMesh.getNumVertices());
+ for (int i=0; i < smoothMesh.getNumVertices(); ++i)
+ v[i] = smoothMesh.getVertexPosition(i);
+ //drawBoundingSphere(v, pose2, matter.Ground());
+ //drawApproxBoundingSphere(v, pose2, matter.Ground());
+
+ v.resize(smoothMesh.getNumVertices()+roughMesh.getNumVertices());
+ const Transform X_21 = ~pose2*pose1;
+ for (int i=0; i < roughMesh.getNumVertices(); ++i)
+ v[smoothMesh.getNumVertices()+i] = X_21*roughMesh.getVertexPosition(i);
+ //drawBoundingSphere(v, pose2, matter.Ground(), Blue);
+ //drawApproxBoundingSphere(v, pose2, matter.Ground(), Blue);
+
+ //matter.Ground().addBodyDecoration(pose1,
+ // DecorativeMesh(origMesh).setRepresentation(DecorativeGeometry::DrawWireframe)
+ // .setColor(Gray));
+ matter.Ground().addBodyDecoration(pose1,
+ DecorativeMesh(roughMesh)
+ .setColor(Gray).setOpacity(.4));
+
+ //matter.Ground().addBodyDecoration(pose2,
+ // DecorativeMesh(origMesh).setRepresentation(DecorativeGeometry::DrawWireframe)
+ // .setColor(Gray));
+ matter.Ground().addBodyDecoration(pose2,
+ DecorativeMesh(smoothMesh)
+ .setColor(Black).setOpacity(.4)
+ .setRepresentation(DecorativeGeometry::DrawWireframe));
+
+ BicubicSurface::PatchHint hint1, hint2;
+ Array_<int> fx(1,0), fy(1,1);
+ const Real Len = .25;
+ Vec2 xy0 = rough.getMinXY(), range = rough.getMaxXY()-xy0;
+ const int NSamples = 25;
+ const Vec2 incr( range / (NSamples-1) );
+ //for (int i=0; i<NSamples; ++i)
+ // for (int j=0; j<NSamples; ++j) {
+ // Vec2 xy(xy0 + Vec2(i*incr[0], j*incr[1]));
+ // Vec3 start1(xy[0],xy[1], rough.calcValue(xy,hint1));
+ // UnitVec3 nz1 = rough.calcUnitNormal(xy,hint1);
+ // matter.Ground().addBodyDecoration(pose1,
+ // DecorativeLine(start1, start1+Len*nz1)
+ // .setColor(Green));
+
+ // Vec3 start2(xy[0],xy[1], smooth.calcValue(xy,hint2));
+ // UnitVec3 nz2 = smooth.calcUnitNormal(xy,hint2);
+ // matter.Ground().addBodyDecoration(pose2,
+ // DecorativeLine(start2, start2+Len*nz2)
+ // .setColor(Green));
+ // }
+//#endif
+
+ // Add gravity as a force element.
+ Rotation x45(Pi/4, XAxis);
+ Rotation y45(Pi/4, YAxis);
+ Rotation z45(Pi/4, ZAxis);
+ Force::UniformGravity gravity(forces, matter, Vec3(10, -9.8, 3));
+ // Create the body and some artwork for it.
+ Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1).setColor(Red));
+ pendulumBody.addDecoration(Transform(), DecorativePoint(Vec3(0,.5,0)).setColor(Green));
+ // Add an instance of the body to the multibody system by connecting
+ // it to Ground via a pin mobilizer.
+ MobilizedBody::Pin pendulum1(matter.updGround(),
+ Transform(/*x45,*/Vec3(0,-1,0)),
+ pendulumBody,
+ Transform(Vec3(0, 1, 0)));
+ MobilizedBody::Pin pendulum1b(pendulum1,
+ Transform(/*x45,*/Vec3(0,-1,0)),
+ pendulumBody,
+ Transform(Vec3(0, 1, 0)));
+
+
+ MobilizedBody::Free pendulum2(matter.updGround(),
+ Transform(/*x45,*/Vec3(2,-1,0)),
+ pendulumBody,
+ Transform(Vec3(0,1,0)));
+ Constraint::Ball ballcons2(matter.updGround(), Vec3(2,-1,0),
+ pendulum2, Vec3(0,1,0));
+ const Transform& X_GF2 = pendulum2.getDefaultInboardFrame();
+ const Transform& X_P2M = pendulum2.getDefaultOutboardFrame();
+ Constraint::ConstantAngle angx2(matter.Ground(), X_GF2.x(),
+ pendulum2, X_P2M.z());
+ Constraint::ConstantAngle angy2(matter.Ground(), X_GF2.y(),
+ pendulum2, X_P2M.z());
+
+ MobilizedBody::Free pendulum2b(pendulum2,
+ Transform(/*x45,*/Vec3(0,-1,0)),
+ pendulumBody,
+ Transform(Vec3(0,1,0)));
+ Constraint::Ball ballcons2b(pendulum2, Vec3(0,-1,0),
+ pendulum2b, Vec3(0,1,0));
+ const Transform& X_GF2b = pendulum2b.getDefaultInboardFrame();
+ const Transform& X_P2Mb = pendulum2b.getDefaultOutboardFrame();
+ Constraint::ConstantAngle angx2b(pendulum2, X_GF2b.x(),
+ pendulum2b, X_P2Mb.z());
+ Constraint::ConstantAngle angy2b(pendulum2, X_GF2b.y(),
+ pendulum2b, X_P2Mb.z());
+
+
+ // Visualize with default options; ask for a report every 1/30 of a second
+ // to match the Visualizer's default 30 frames per second rate.
+ Visualizer viz(system);
+ system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ pendulum1.setOneQ(state, 0, Pi/4);
+ //pendulum1.setOneU(state, 0, 1.0); // initial velocity 1 rad/sec
+
+ //pendulum1b.setOneU(state, 0, 1.0); // initial velocity 1 rad/sec
+ pendulum1b.setOneQ(state, 0, Pi/4);
+
+ pendulum2.setQToFitRotation(state, Rotation(Pi/4, ZAxis));
+ //pendulum2.setUToFitAngularVelocity(state, Vec3(0,0,1));
+ pendulum2b.setQToFitRotation(state, Rotation(Pi/4, ZAxis));
+ //pendulum2b.setUToFitAngularVelocity(state, Vec3(0,0,1));
+
+ system.realize(state);
+ const Vector lambda = state.getMultipliers();
+ Vector_<SpatialVec> consBodyForcesInG;
+ Vector consMobForces;
+ matter.calcConstraintForcesFromMultipliers(state, -lambda, consBodyForcesInG,
+ consMobForces);
+ const MobodIndex p2x = pendulum2.getMobilizedBodyIndex();
+ const MobodIndex p2bx = pendulum2b.getMobilizedBodyIndex();
+ const Rotation& R_G2 = pendulum2.getBodyTransform(state).R();
+ //consBodyForcesInG[p2x] = shiftForceFromTo(consBodyForcesInG[p2x],
+ // Vec3(0), R_G2*Vec3(0,1,0));
+
+ const int nb = matter.getNumBodies();
+ Vector_<SpatialVec> forcesAtMInG;
+ matter.calcMobilizerReactionForces(state, forcesAtMInG);
+
+ Vector_<SpatialVec> forcesAtFInG(nb); // to hold the result
+ forcesAtFInG[0] = -forcesAtMInG[0]; // Ground is "welded" at origin
+ for (MobilizedBodyIndex i(1); i < matter.getNumBodies(); ++i) {
+ const MobilizedBody& body = matter.getMobilizedBody(i);
+ const MobilizedBody& parent = body.getParentMobilizedBody();
+ // Want to shift negated reaction by p_MF_G, the vector from M
+ // to F across the mobilizer, expressed in Ground. We can get p_FM,
+ // then re-express in Ground for the shift and negate.
+ const Vec3& p_FM = body.getMobilizerTransform(state).p();
+ const Rotation& R_PF = body.getInboardFrame(state).R(); // In parent.
+ const Rotation& R_GP = parent.getBodyTransform(state).R();
+ Rotation R_GF = R_GP*R_PF; // F frame orientation in Ground.
+ Vec3 p_MF_G = -(R_GF*p_FM); // Re-express and negate shift vector.
+ forcesAtFInG[i] = -shiftForceBy(forcesAtMInG[i], p_MF_G);
+ }
+
+ std::cout << "Reactions @M: " << forcesAtMInG << "\n";
+ std::cout << "Reactions @F: " << forcesAtFInG << "\n";
+
+ const MobodIndex p1x = pendulum1.getMobilizedBodyIndex();
+ const MobodIndex p1bx = pendulum1b.getMobilizedBodyIndex();
+ const Rotation& R_G1 = pendulum1.getBodyTransform(state).R();
+ const Rotation& R_G1b = pendulum1b.getBodyTransform(state).R();
+
+ for (MobodIndex i(0); i < nb; ++i) {
+ const Mobod& body = matter.getMobilizedBody(i);
+ const Vec3& p_BM = body.getOutboardFrame(state).p();
+ const Rotation& R_GB = body.getBodyTransform(state).R();
+ forcesAtMInG[i] = shiftForceFromTo(forcesAtMInG[i],
+ R_GB*p_BM, Vec3(0));
+ }
+
+
+ std::cout << "FB_G=" << forcesAtMInG[p1x] << " " << forcesAtMInG[p1bx] << "\n";
+
+ cout << "FC_G=" << -(ballcons2.getConstrainedBodyForcesAsVector(state)
+ + angx2.getConstrainedBodyForcesAsVector(state)
+ + angy2.getConstrainedBodyForcesAsVector(state))[1] << " ";
+ cout << -(ballcons2b.getConstrainedBodyForcesAsVector(state)
+ + angx2b.getConstrainedBodyForcesAsVector(state)
+ + angy2b.getConstrainedBodyForcesAsVector(state))[1] << endl;
+
+ viz.report(state);
+ // Simulate it.
+ getchar();
+
+ RungeKuttaMersonIntegrator integ(system);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(1.0);
+ state = integ.getState();
+ system.realize(state);
+ matter.calcMobilizerReactionForces(state, forcesAtMInG);
+ const Transform& X_GP = pendulum1.getBodyTransform(state);
+ //forcesAtMInG[1][1] = X_GP.R()*forcesAtMInG[1][1];
+ std::cout << "FM_G=" << forcesAtMInG << "\n";
+ ts.stepTo(1.2);
+ state = integ.getState();
+ system.realize(state);
+ matter.calcMobilizerReactionForces(state, forcesAtMInG);
+ std::cout << "FM_G=" << forcesAtMInG << "\n";
+
+}
diff --git a/Simbody/tests/adhoc/JunkMain1.cpp b/Simbody/tests/adhoc/JunkMain1.cpp
new file mode 100644
index 0000000..58a0c0f
--- /dev/null
+++ b/Simbody/tests/adhoc/JunkMain1.cpp
@@ -0,0 +1,217 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Cable Path *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* Simbody ExampleCablePath
+This example shows how to use a CableTrackerSubsystem to follow the motion of
+a cable that connects two bodies and passes around obstacles. We'll then
+create a force element that generates spring forces that result from the
+stretching and stretching rate of the cable. */
+
+#include "Simbody.h"
+
+#include <cassert>
+#include <iostream>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+// This gets called periodically to dump out interesting things about
+// the cables and the system as a whole. It also saves states so that we
+// can play back at the end.
+static Array_<State> saveStates;
+class ShowStuff : public PeriodicEventReporter {
+public:
+ ShowStuff(const MultibodySystem& mbs,
+ const CableSpring& cable1, Real interval)
+ : PeriodicEventReporter(interval),
+ mbs(mbs), cable1(cable1) {}
+
+ static void showHeading(std::ostream& o) {
+ printf("%8s %10s %10s %10s %10s %10s %10s %10s %10s %12s\n",
+ "time", "length", "rate", "integ-rate", "unitpow", "tension", "disswork",
+ "KE", "PE", "KE+PE-W");
+ }
+
+ /** This is the implementation of the EventReporter virtual. **/
+ void handleEvent(const State& state) const OVERRIDE_11 {
+ const CablePath& path1 = cable1.getCablePath();
+ printf("%8g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %12.6g CPU=%g\n",
+ state.getTime(),
+ path1.getCableLength(state),
+ path1.getCableLengthDot(state),
+ path1.getIntegratedCableLengthDot(state),
+ path1.calcCablePower(state, 1), // unit power
+ cable1.getTension(state),
+ cable1.getDissipatedEnergy(state),
+ mbs.calcKineticEnergy(state),
+ mbs.calcPotentialEnergy(state),
+ mbs.calcEnergy(state)
+ + cable1.getDissipatedEnergy(state),
+ cpuTime());
+ saveStates.push_back(state);
+ }
+private:
+ const MultibodySystem& mbs;
+ CableSpring cable1;
+};
+
+int main() {
+ try {
+ // Create the system.
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ CableTrackerSubsystem cables(system);
+ GeneralForceSubsystem forces(system);
+
+ system.setUseUniformBackground(true); // no ground plane in display
+
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ //Force::GlobalDamper(forces, matter, 5);
+
+ Body::Rigid someBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ const Real Rad = .25;
+ someBody.addDecoration(Transform(),
+ DecorativeSphere(Rad).setOpacity(.75).setResolution(4));
+
+ Body::Rigid biggerBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ const Real BiggerRad = .5;
+ biggerBody.addDecoration(Transform(),
+ DecorativeSphere(BiggerRad).setOpacity(.75).setResolution(4));
+
+ const Vec3 radii(.3, .25, .25);
+ Body::Rigid ellipsoidBody(MassProperties(1.0, Vec3(0),
+ UnitInertia::ellipsoid(radii)));
+ ellipsoidBody.addDecoration(Transform(),
+ DecorativeEllipsoid(radii).setOpacity(.75).setResolution(4)
+ .setColor(Orange));
+
+ const Real CylRad = .25, HalfLen = .5;
+ Body::Rigid cylinderBody(MassProperties(1.0, Vec3(0),
+ 1.*UnitInertia::cylinderAlongX(Rad,HalfLen)));
+ cylinderBody.addDecoration(Rotation(-Pi/2,ZAxis),
+ DecorativeCylinder(CylRad,HalfLen).setOpacity(.75)
+ .setResolution(4).setColor(Orange));
+
+ Body::Rigid fancyBody = biggerBody; // NOT USING ELLIPSOID
+
+ MobilizedBody Ground = matter.Ground();
+
+ MobilizedBody::Free body1(Ground, Transform(Vec3(0)),
+ someBody, Transform(Vec3(0, 1, 0)));
+ //MobilizedBody::Ball body2(body1, Transform(Vec3(0)),
+ // someBody, Transform(Vec3(0, 1, 0)));
+ //MobilizedBody::Ball body3(body2, Transform(Vec3(0)),
+ // someBody, Transform(Vec3(0, 1, 0)));
+ //MobilizedBody::Ball body4(body3, Transform(Vec3(0)),
+ // fancyBody, Transform(Vec3(0, 1, 0)));
+ //MobilizedBody::Ball body5(body4, Transform(Vec3(0)),
+ // someBody, Transform(Vec3(0, 1, 0)));
+
+ CablePath path1(cables, Ground, Vec3(0,0,0), // origin
+ body1, Vec3(0,Rad,0)); // termination
+
+ //CableObstacle::Surface obstacle(path1, Ground, Vec3(1,-1,0),
+ // ContactGeometry::Sphere(Rad));
+ //obstacle.setContactPointHints(Rad*UnitVec3(1,1,0),Rad*UnitVec3(1,.5,0));
+ //
+ CableObstacle::Surface obstacle2(path1, Ground, Vec3(0,-1,0),
+ //ContactGeometry::Sphere(CylRad));
+ ContactGeometry::Ellipsoid(radii));
+ //ContactGeometry::Torus(CylRad,.1));
+ //ContactGeometry::Cylinder(CylRad));
+
+ obstacle2.setContactPointHints(1.5*CylRad*UnitVec3(1,1,0),1.5*CylRad*UnitVec3(1,.5,0));
+ obstacle2.setDisabledByDefault(true);
+
+ ////CableObstacle::ViaPoint p4(path1, body4, Rad*UnitVec3(0,1,1));
+ ////CableObstacle::ViaPoint p5(path1, body4, Rad*UnitVec3(1,0,1));
+ //CableObstacle::Surface obs5(path1, body4,
+ // // Transform(), ContactGeometry::Ellipsoid(radii));
+ // //Rotation(Pi/2, YAxis), ContactGeometry::Cylinder(CylRad)); // along y
+ // //Transform(), ContactGeometry::Sphere(Rad));
+ // Transform(), ContactGeometry::Sphere(BiggerRad));
+ ////obs5.setContactPointHints(Rad*UnitVec3(0,-1,-1),Rad*UnitVec3(0.1,-1,-1));
+ //obs5.setContactPointHints(Rad*UnitVec3(.1,.125,-.2),
+ // Rad*UnitVec3(0.1,-.1,-.2));
+
+ CableSpring cable1(forces, path1, .5*100., 2, 0.1);
+
+ //obs1.setPathPreferencePoint(Vec3(2,3,4));
+ //obs1.setDecorativeGeometry(DecorativeSphere(0.25).setOpacity(.5));
+
+ Visualizer viz(system);
+ viz.setShowFrameNumber(true);
+ system.addEventReporter(new Visualizer::Reporter(viz, 0.1*1./30));
+ system.addEventReporter(new ShowStuff(system, cable1, 0.1*0.1));
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ //Random::Gaussian random;
+ //for (int i = 0; i < state.getNQ(); ++i)
+ // state.updQ()[i] = random.getValue();
+ //for (int i = 0; i < state.getNU(); ++i)
+ // state.updU()[i] = 0.1*random.getValue();
+ body1.setQToFitTranslation(state, Vec3(2,-1,.5));
+
+ system.realize(state, Stage::Position);
+ viz.report(state);
+ cout << "path1 init length=" << path1.getCableLength(state) << endl;
+ cout << "Hit ENTER ...";
+ getchar();
+
+ path1.setIntegratedCableLengthDot(state, path1.getCableLength(state));
+
+
+ // Simulate it.
+ saveStates.clear(); saveStates.reserve(2000);
+
+ //RungeKutta3Integrator integ(system);
+ RungeKuttaMersonIntegrator integ(system);
+ //CPodesIntegrator integ(system);
+ //integ.setAllowInterpolation(false);
+ integ.setAccuracy(1e-3);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ShowStuff::showHeading(cout);
+
+ const Real finalTime = 2;
+ const double startTime = realTime(), startCPU = cpuTime();
+ ts.stepTo(finalTime);
+ cout << "DONE with " << finalTime
+ << "s simulated in " << realTime()-startTime
+ << "s elapsed, " << cpuTime()-startCPU << "s CPU.\n";
+
+
+ while (true) {
+ cout << "Hit ENTER FOR REPLAY, Q to quit ...";
+ const char ch = getchar();
+ if (ch=='q' || ch=='Q') break;
+ for (unsigned i=0; i < saveStates.size(); ++i)
+ viz.report(saveStates[i]);
+ }
+
+ } catch (const std::exception& e) {
+ cout << "EXCEPTION: " << e.what() << "\n";
+ }
+}
diff --git a/Simbody/tests/adhoc/JunkMain2.cpp b/Simbody/tests/adhoc/JunkMain2.cpp
new file mode 100644
index 0000000..27f167e
--- /dev/null
+++ b/Simbody/tests/adhoc/JunkMain2.cpp
@@ -0,0 +1,191 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Example: Cable Path *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* Simbody ExampleCablePath
+This example shows how to use a CableTrackerSubsystem to follow the motion of
+a cable that connects two bodies and passes around obstacles. We'll then
+create a force element that generates spring forces that result from the
+stretching and stretching rate of the cable. */
+
+#include "Simbody.h"
+
+#include <cassert>
+#include <iostream>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+
+// This gets called periodically to dump out interesting things about
+// the cables and the system as a whole. It also saves states so that we
+// can play back at the end.
+static Array_<State> saveStates;
+class ShowStuff : public PeriodicEventReporter {
+public:
+ ShowStuff(const MultibodySystem& mbs,
+ const CableSpring& cable1, Real interval)
+ : PeriodicEventReporter(interval),
+ mbs(mbs), cable1(cable1) {}
+
+ static void showHeading(std::ostream& o) {
+ printf("%8s %10s %10s %10s %10s %10s %10s %10s %10s %12s\n",
+ "time", "length", "rate", "integ-rate", "unitpow", "tension", "disswork",
+ "KE", "PE", "KE+PE-W");
+ }
+
+ /** This is the implementation of the EventReporter virtual. **/
+ void handleEvent(const State& state) const OVERRIDE_11 {
+ const CablePath& path1 = cable1.getCablePath();
+ printf("%8g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %12.6g CPU=%g\n",
+ state.getTime(),
+ path1.getCableLength(state),
+ path1.getCableLengthDot(state),
+ path1.getIntegratedCableLengthDot(state),
+ path1.calcCablePower(state, 1), // unit power
+ cable1.getTension(state),
+ cable1.getDissipatedEnergy(state),
+ mbs.calcKineticEnergy(state),
+ mbs.calcPotentialEnergy(state),
+ mbs.calcEnergy(state)
+ + cable1.getDissipatedEnergy(state),
+ cpuTime());
+ saveStates.push_back(state);
+ }
+private:
+ const MultibodySystem& mbs;
+ CableSpring cable1;
+};
+
+int main() {
+ try {
+ // Create the system.
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ CableTrackerSubsystem cables(system);
+ GeneralForceSubsystem forces(system);
+
+ system.setUseUniformBackground(true); // no ground plane in display
+
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+ //Force::GlobalDamper(forces, matter, 5);
+
+ Body::Rigid someBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ const Real Rad = 1.5;
+ someBody.addDecoration(Transform(),
+ DecorativeSphere(Rad).setOpacity(.75).setResolution(4));
+
+ Body::Rigid biggerBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ const Real BiggerRad = .5;
+ biggerBody.addDecoration(Transform(),
+ DecorativeSphere(BiggerRad).setOpacity(.75).setResolution(4));
+
+ const Vec3 radii(2,1.5,1.7);
+ Body::Rigid ellipsoidBody(MassProperties(1.0, Vec3(0),
+ UnitInertia::ellipsoid(radii)));
+ //ellipsoidBody.addDecoration(Transform(),
+ // DecorativeEllipsoid(radii).setOpacity(.9).setResolution(4)
+ // .setColor(Orange));
+
+ const Real CylRad = .25, HalfLen = .5;
+ Body::Rigid cylinderBody(MassProperties(1.0, Vec3(0),
+ 1.*UnitInertia::cylinderAlongX(Rad,HalfLen)));
+ cylinderBody.addDecoration(Rotation(-Pi/2,ZAxis),
+ DecorativeCylinder(CylRad,HalfLen).setOpacity(.75)
+ .setResolution(4).setColor(Orange));
+
+ Body::Rigid fancyBody = biggerBody; // NOT USING ELLIPSOID
+
+ MobilizedBody Ground = matter.Ground();
+
+ MobilizedBody::Free body1(Ground, Transform(Vec3(0)),
+ ellipsoidBody, Transform(Vec3(0)));
+
+ CablePath path1(cables, Ground, Vec3(-5,0,-.5),
+ Ground, Vec3(5,0,-.5));
+ CablePath path2(cables, Ground, Vec3(-5,0,.5),
+ Ground, Vec3(5,0,.5));
+
+ CableSpring cable1(forces, path1, 15000., 10, 0.1);
+ CableSpring cable2(forces, path2, 15000., 10, 0.1);
+
+ CableObstacle::Surface theBall1(path1, body1, Vec3(0),
+ ContactGeometry::Ellipsoid(radii));
+ //ContactGeometry::Sphere(Rad));
+ theBall1.setContactPointHints(Vec3(-.5,-1.5,-.5),Vec3(.5,-1.5,-.5));
+
+ CableObstacle::Surface theBall2(path2, body1, Vec3(0),
+ ContactGeometry::Ellipsoid(radii));
+ //ContactGeometry::Sphere(Rad));
+ theBall2.setContactPointHints(Vec3(-.5,-1.5,.5),Vec3(.5,-1.5,.5));
+
+ Visualizer viz(system);
+ viz.setShowFrameNumber(true);
+ system.addEventReporter(new Visualizer::Reporter(viz, 0.1*1./30));
+ system.addEventReporter(new ShowStuff(system, cable1, 0.1*0.1));
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+
+ body1.setQToFitTranslation(state, Vec3(0,.9,0));
+ body1.setUToFitAngularVelocity(state, 0*.25*Vec3(1,4,1));
+
+ system.realize(state, Stage::Position);
+ viz.report(state);
+ cout << "path1 init length=" << path1.getCableLength(state) << endl;
+ cout << "Hit ENTER ...";
+ getchar();
+
+ path1.setIntegratedCableLengthDot(state, path1.getCableLength(state));
+
+
+ // Simulate it.
+ saveStates.clear(); saveStates.reserve(2000);
+
+ RungeKuttaMersonIntegrator integ(system);
+ //CPodesIntegrator integ(system);
+ integ.setAccuracy(1e-3);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ShowStuff::showHeading(cout);
+
+ const Real finalTime = 5;
+ const double startTime = realTime(), startCPU = cpuTime();
+ ts.stepTo(finalTime);
+ cout << "DONE with " << finalTime
+ << "s simulated in " << realTime()-startTime
+ << "s elapsed, " << cpuTime()-startCPU << "s CPU.\n";
+
+
+ while (true) {
+ cout << "Hit ENTER FOR REPLAY, Q to quit ...";
+ const char ch = getchar();
+ if (ch=='q' || ch=='Q') break;
+ for (unsigned i=0; i < saveStates.size(); ++i)
+ viz.report(saveStates[i]);
+ }
+
+ } catch (const std::exception& e) {
+ cout << "EXCEPTION: " << e.what() << "\n";
+ }
+}
diff --git a/Simbody/tests/adhoc/LockUnlockConstraint.cpp b/Simbody/tests/adhoc/LockUnlockConstraint.cpp
new file mode 100644
index 0000000..65d5ebb
--- /dev/null
+++ b/Simbody/tests/adhoc/LockUnlockConstraint.cpp
@@ -0,0 +1,473 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This is an outer block for simulating ??? in various ways with Simbody.
+ * This is about testing Simbody, *not* studying ???!
+ */
+
+#include "Simbody.h"
+
+#include <string>
+#include <iostream>
+#include <exception>
+
+using std::cout;
+using std::endl;
+
+using namespace SimTK;
+
+const Real ReportInterval=0.033;
+const Real RunTime=20;
+
+class ShowLocking : public DecorationGenerator {
+public:
+ ShowLocking(const MobilizedBody& mobod,
+ const Constraint::ConstantSpeed& lock)
+ : m_mobod(mobod), m_lock(lock)
+ {
+ }
+ void generateDecorations(const State& state,
+ Array_<DecorativeGeometry>& geometry) OVERRIDE_11
+ {
+ if (!m_lock.isDisabled(state)) {
+ const Transform& X_BM = m_mobod.getDefaultOutboardFrame();
+ geometry.push_back(DecorativeSphere(.5)
+ .setTransform(X_BM)
+ .setColor(Red).setOpacity(.25)
+ .setBodyId(m_mobod.getMobilizedBodyIndex()));
+ geometry.push_back(DecorativeText("LOCKED")
+ .setTransform(X_BM+Vec3(.5,0,0))
+ .setBodyId(m_mobod.getMobilizedBodyIndex()));
+ }
+ }
+private:
+ const MobilizedBody& m_mobod;
+ const Constraint::ConstantSpeed& m_lock;
+};
+
+class StateSaver : public PeriodicEventReporter {
+public:
+ StateSaver(const MultibodySystem& system,
+ const Constraint::ConstantSpeed& lock,
+ const Integrator& integ,
+ Real reportInterval)
+ : PeriodicEventReporter(reportInterval),
+ m_system(system), m_lock(lock), m_integ(integ)
+ { m_states.reserve(2000); }
+
+ ~StateSaver() {}
+
+ void clear() {m_states.clear();}
+ int getNumSavedStates() const {return m_states.size();}
+ const State& getState(int n) const {return m_states[n];}
+
+ void handleEvent(const State& s) const {
+ const SimbodyMatterSubsystem& matter=m_system.getMatterSubsystem();
+ const SpatialVec PG = matter.calcSystemMomentumAboutGroundOrigin(s);
+
+ const bool isLocked = !m_lock.isDisabled(s);
+
+ printf("%3d: %5g mom=%g,%g E=%g %s", m_integ.getNumStepsTaken(),
+ s.getTime(),
+ PG[0].norm(), PG[1].norm(), m_system.calcEnergy(s),
+ isLocked?"LOCKED":"FREE");
+
+ if (isLocked) {
+ m_system.realize(s, Stage::Acceleration);
+ printf(" lambda=%g", m_lock.getMultiplier(s));
+ }
+ cout << " Triggers=" << s.getEventTriggers() << endl;
+
+ m_states.push_back(s);
+ }
+private:
+ const MultibodySystem& m_system;
+ const Constraint::ConstantSpeed& m_lock;
+ const Integrator& m_integ;
+ mutable Array_<State,int> m_states;
+};
+
+class LockOn: public TriggeredEventHandler {
+public:
+ LockOn(const MultibodySystem& system,
+ const MobilizedBody& mobod, Real lockangle, // must be 1dof
+ const Constraint::ConstantSpeed& lock,
+ Real low, Real high)
+ : TriggeredEventHandler(Stage::Position),
+ m_mbs(system), m_mobod(mobod), m_lockangle(lockangle),
+ m_lock(lock), m_low(low), m_high(high)
+ {
+ //getTriggerInfo().setTriggerOnRisingSignTransition(false);
+ }
+
+ const Array_<Real>& getOnTimes() const {return m_onTimes;}
+
+ Real getValue(const State& state) const {
+ if (!m_lock.isDisabled(state))
+ return 0; // already locked
+ const Real qdist = m_mobod.getOneQ(state, 0) - m_lockangle;
+ return qdist;
+ }
+
+ void handleEvent
+ (State& s, Real accuracy, bool& shouldTerminate) const
+ {
+ const SimbodyMatterSubsystem& matter = m_mbs.getMatterSubsystem();
+ assert(m_lock.isDisabled(s));
+
+ const Vector uin = s.getU();
+ cout << "BEFORE u=" << uin << endl;
+ cout << "before uerr=" << s.getUErr() << endl;
+
+ // This is a ground-connected system -- system "center of mass" doesn't
+ // really mean anything since ground's mass is infinite.
+ SpatialVec PG = matter.calcSystemMomentumAboutGroundOrigin(s);
+ SpatialVec PC = matter.calcSystemCentralMomentum(s);
+
+ printf("Locking: BEFORE q=%.15g u=%.15g\n",
+ m_mobod.getOneQ(s,0), m_mobod.getOneU(s,0));
+ printf(" %5g G mom=%g,%g C mom=%g,%g E=%g\n", s.getTime(),
+ PG[0].norm(), PG[1].norm(), PC[0].norm(), PC[1].norm(), m_mbs.calcEnergy(s));
+
+ const Real CoefRest = 0.0, MinVel=.05;
+
+ const UIndex ux = m_mobod.getFirstUIndex(s);
+ const Real saveQ = m_mobod.getOneQ(s,0);
+
+ m_lock.enable(s);
+ m_mbs.realize(s, Stage::Dynamics);
+
+ // We're using Poisson's definition of the coefficient of
+ // restitution, relating impulses, rather than Newton's,
+ // relating velocities, since Newton's can produce non-physical
+ // results for a multibody system. For Poisson, calculate the impulse
+ // that would bring the velocity to zero, multiply by the coefficient
+ // of restitution to calculate the rest of the impulse, then apply
+ // both impulses to produce changes in velocity. In most cases this
+ // will produce the same rebound velocity as Newton, but not always.
+
+ Vector desiredDeltaV; // in constraint space
+ Vector myDeltaV(1); // just one scalar for this constraint
+ myDeltaV[0] = -uin[ux]; // dump the current velocity first
+ m_lock.setMyPartInConstraintSpaceVector(s, myDeltaV, desiredDeltaV);
+
+ Vector lambda, f, deltaU;
+
+
+ cout << "USING POISSON COEF " << CoefRest << endl;
+
+ Vector lambda0; // impulse that gets this velocity to zero
+ matter.solveForConstraintImpulses(s, desiredDeltaV, lambda0);
+
+ // This is the total impulse we want, in constraint space.
+ lambda = (1+CoefRest)*lambda0;
+ // Convert constraint impulse to generalized impulse.
+ matter.multiplyByGTranspose(s,lambda,f);
+ // Convert generalized impulse to generalized speed change.
+ matter.multiplyByMInv(s,f,deltaU);
+ // If the new speed is slow enough, we'll declare that it "stuck" and
+ // drive it to exactly zero instead (using lambda0 instead of lambda).
+ Real achievedU = uin[ux]+deltaU[ux];
+ if (CoefRest > 0 && std::abs(achievedU) < MinVel) {
+ cout << " rebound " << achievedU
+ << " too slow; drive to zero instead\n";
+ cout << " deltaU was=" << deltaU << endl;
+ lambda = lambda0; f /= 1+CoefRest; deltaU /= 1+CoefRest;
+ cout << " new deltaU=" << deltaU << endl;
+ }
+
+ // Now update all the generalized speeds.
+ s.updU() = uin + deltaU;
+ cout << "lambda=" << lambda << endl;
+ cout << "f=" << f << endl;
+ cout << "du=" << deltaU << endl;
+
+ cout << "AFTER u=" << s.getU() << endl;
+
+ // If this leaves us rebounding then don't turn on the constraint.
+ if (std::abs(s.getU()[ux]) > SignificantReal) {
+ printf("REBOUND vel=%g -- not locking.\n", s.getU()[1]);
+ m_lock.disable(s);
+ // Move q a small distance in the direction of u so that the
+ // witness function will be non-zero and we'll consider an
+ // immediate reverse as a new event (transitions away from zero
+ // are not events).
+ m_mobod.setOneQ(s,0,m_lockangle+sign(s.getU()[ux])*SignificantReal);
+
+ m_mbs.realize(s, Stage::Acceleration);
+ PG = matter.calcSystemMomentumAboutGroundOrigin(s);
+ PC = matter.calcSystemCentralMomentum(s);
+ printf(" %5g G mom=%g,%g C mom=%g,%g E=%g\n", s.getTime(),
+ PG[0].norm(), PG[1].norm(), PC[0].norm(), PC[1].norm(),
+ m_mbs.calcEnergy(s));
+ cout << " uerr=" << s.getUErr() << endl;
+ return;
+ }
+
+ // Tidy up the q to be exactly zero when we lock to avoid
+ // accidental retriggering fo this event. We'll put this back
+ // if we decide below not to hold the lock.
+ m_mobod.setOneQ(s,0,m_lockangle);
+
+ // Can we really hold the lock?
+ m_mbs.realize(s, Stage::Acceleration);
+ const Real lockForce = m_lock.getMultiplier(s);
+
+ if (lockForce < m_low || lockForce > m_high) {
+ m_lock.disable(s); // oops can't lock
+ s.updU() = uin;
+ m_mobod.setOneQ(s,0, saveQ);
+ printf("CAN'T LOCK: force would have been %g\n", lockForce);
+ return;
+ }
+
+ printf("LOCKED: reaction force is now %g\n", lockForce);
+
+ m_onTimes.push_back(s.getTime());
+
+ printf(" after q=%.15g\n", m_mobod.getOneQ(s,0));
+
+ PG = matter.calcSystemMomentumAboutGroundOrigin(s);
+ PC = matter.calcSystemCentralMomentum(s);
+ printf(" %5g G mom=%g,%g C mom=%g,%g E=%g\n", s.getTime(),
+ PG[0].norm(), PG[1].norm(), PC[0].norm(), PC[1].norm(), m_mbs.calcEnergy(s));
+ cout << " after uerr=" << s.getUErr() << endl;
+ }
+
+private:
+ const MultibodySystem& m_mbs;
+ const MobilizedBody& m_mobod;
+ const Real m_lockangle;
+ const Constraint::ConstantSpeed& m_lock;
+ const Real m_low, m_high;
+
+ mutable Array_<Real> m_onTimes;
+};
+
+class LockOff: public TriggeredEventHandler {
+public:
+ LockOff(const MultibodySystem& system,
+ MobilizedBody& mobod, Real lockangle,
+ Constraint::ConstantSpeed& lock,
+ Real low, Real high)
+ : TriggeredEventHandler(Stage::Acceleration),
+ m_system(system), m_mobod(mobod), m_lockangle(lockangle), m_lock(lock),
+ m_low(low), m_high(high)
+ {
+ getTriggerInfo().setTriggerOnRisingSignTransition(false);
+ }
+
+ const Array_<Real>& getOffTimes() const {return m_offTimes;}
+
+ Real getValue(const State& state) const {
+ if (m_lock.isDisabled(state)) return 0;
+ const Real f = m_lock.getMultiplier(state);
+ const Real mid = (m_high+m_low)/2;
+ return f > mid ? m_high - f : f - m_low;
+ }
+
+ void handleEvent
+ (State& s, Real accuracy, bool& shouldTerminate) const
+ {
+ assert(!m_lock.isDisabled(s));
+
+ m_system.realize(s, Stage::Acceleration);
+ printf("\nUNLOCK: disabling at t=%g q=%g lambda=%g",
+ s.getTime(), s.getQ()[1], m_lock.getMultiplier(s));
+ cout << " Triggers=" << s.getEventTriggers() << "\n\n";
+
+ m_mobod.setOneQ(s, 0, m_lockangle); // avoid retriggering lock
+
+ m_lock.disable(s);
+
+ m_offTimes.push_back(s.getTime());
+ }
+
+private:
+ const MultibodySystem& m_system;
+ const MobilizedBody& m_mobod;
+ const Real m_lockangle;
+ const Constraint::ConstantSpeed& m_lock;
+ const Real m_low;
+ const Real m_high;
+
+ mutable Array_<Real> m_offTimes;
+};
+
+static const Real Deg2Rad = (Real)SimTK_DEGREE_TO_RADIAN,
+ Rad2Deg = (Real)SimTK_RADIAN_TO_DEGREE;
+
+
+
+static Real g = 9.8;
+
+int main(int argc, char** argv) {
+ static const Transform GroundFrame;
+ static const Rotation ZUp(UnitVec3(XAxis), XAxis, UnitVec3(YAxis), ZAxis);
+ static const Vec3 TestLoc(1,0,0);
+
+ try { // If anything goes wrong, an exception will be thrown.
+
+ // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
+ MultibodySystem mbs;
+
+ SimbodyMatterSubsystem matter(mbs);
+ GeneralForceSubsystem forces(mbs);
+ Force::Gravity gravity(forces, matter, Vec3(0, -g, 0));
+
+ // ADD BODIES AND THEIR MOBILIZERS
+ const Vec3 thighHDim(.5,2,.25);
+ const Real thighVol=8*thighHDim[0]*thighHDim[1]*thighHDim[2];
+ const Vec3 calfHDim(.25,2,.125);
+ const Real calfVol=8*calfHDim[0]*calfHDim[1]*calfHDim[2];
+ const Real density = 1000; // water
+ const Real thighMass = density*thighVol, calfMass = density*calfVol;
+ Body::Rigid thighBody =
+ Body::Rigid(MassProperties(10*thighMass, Vec3(0),
+ 10*thighMass*UnitInertia::brick(thighHDim)));
+ thighBody.addDecoration(Transform(), DecorativeBrick(thighHDim)
+ .setColor(Red).setOpacity(.3));
+ Body::Rigid calfBody =
+ Body::Rigid(MassProperties(calfMass, Vec3(0),
+ calfMass*UnitInertia::brick(calfHDim)));
+ calfBody.addDecoration(Transform(), DecorativeBrick(calfHDim)
+ .setColor(Blue).setOpacity(.3));
+ Body::Rigid footBody =
+ Body::Rigid(MassProperties(10*calfMass, Vec3(0),
+ 10*calfMass*UnitInertia::brick(calfHDim)));
+ footBody.addDecoration(Transform(), DecorativeBrick(calfHDim)
+ .setColor(Black).setOpacity(.3));
+ MobilizedBody::Pin thigh(matter.Ground(), Vec3(0),
+ thighBody, Vec3(0,thighHDim[1],0));
+ MobilizedBody::Pin calf(thigh, Vec3(0,-thighHDim[1],0),
+ calfBody, Vec3(0,calfHDim[1],0));
+ MobilizedBody::Pin foot(calf, Vec3(0,-calfHDim[1],0),
+ footBody, Vec3(0,calfHDim[1],0));
+ //Constraint::PrescribedMotion pres(matter,
+ // new Function::Constant(Pi/4,1), foot, MobilizerQIndex(0));
+ Constraint::PrescribedMotion pres(matter,
+ new Function::Sinusoid(Pi/4,2*Pi,-Pi/4), foot, MobilizerQIndex(0));
+
+ Constraint::ConstantSpeed lock(calf,0);
+ lock.setDisabledByDefault(true);
+
+ Visualizer viz(mbs);
+ viz.addDecorationGenerator(new ShowLocking(calf,lock));
+ mbs.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));
+
+ //ExplicitEulerIntegrator integ(mbs);
+ //CPodesIntegrator integ(mbs,CPodes::BDF,CPodes::Newton);
+ //RungeKuttaFeldbergIntegrator integ(mbs);
+ //RungeKuttaMersonIntegrator integ(mbs);
+ RungeKutta3Integrator integ(mbs);
+ //VerletIntegrator integ(mbs);
+ integ.setAccuracy(1e-3);
+ //integ.setAllowInterpolation(false);
+
+ StateSaver* stateSaver = new StateSaver(mbs,lock,integ,ReportInterval);
+ mbs.addEventReporter(stateSaver);
+
+ const Real low=-110000*4, high=110000*4;
+ const Real lockAngle = 0;
+ LockOn* lockOn = new LockOn(mbs,calf,lockAngle,lock,low,high);
+ mbs.addEventHandler(lockOn);
+
+ LockOff* lockOff = new LockOff(mbs,calf,lockAngle,lock,low,high);
+ mbs.addEventHandler(lockOff);
+
+ State s = mbs.realizeTopology(); // returns a reference to the the default state
+ mbs.realizeModel(s); // define appropriate states for this System
+ mbs.realize(s, Stage::Instance); // instantiate constraints if any
+
+ thigh.setAngle(s, 20*Deg2Rad);
+ calf.setAngle(s, 90*Deg2Rad);
+ //calf.setRate(s, -10);
+
+ mbs.realize(s, Stage::Velocity);
+ viz.report(s);
+
+ mbs.realize(s, Stage::Acceleration);
+
+
+ cout << "q=" << s.getQ() << endl;
+ cout << "u=" << s.getU() << endl;
+ cout << "qerr=" << s.getQErr() << endl;
+ cout << "uerr=" << s.getUErr() << endl;
+ cout << "udoterr=" << s.getUDotErr() << endl;
+ cout << "mults=" << s.getMultipliers() << endl;
+ cout << "qdot=" << s.getQDot() << endl;
+ cout << "udot=" << s.getUDot() << endl;
+ cout << "qdotdot=" << s.getQDotDot() << endl;
+ viz.report(s);
+
+ cout << "Initial configuration shown. Next? ";
+ getchar();
+
+ Assembler(mbs).assemble(s);
+ viz.report(s);
+ cout << "Assembled configuration shown. Ready? ";
+ getchar();
+
+ // Simulate it.
+ const double start = realTime();
+
+ // TODO: misses some transitions if interpolating
+ //integ.setAllowInterpolation(false);
+ TimeStepper ts(mbs, integ);
+ ts.initialize(s);
+ ts.stepTo(RunTime);
+
+ const double timeInSec = realTime()-start;
+ const int evals = integ.getNumRealizations();
+ cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
+ timeInSec << "s for " << ts.getTime() << "s sim (avg step="
+ << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
+ << (1000*ts.getTime())/evals << "ms/eval\n";
+ cout << "On times: " << lockOn->getOnTimes() << endl;
+ cout << "Off times: " << lockOff->getOffTimes() << endl;
+
+ printf("Used Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
+
+ while(true) {
+ for (int i=0; i < stateSaver->getNumSavedStates(); ++i) {
+ viz.report(stateSaver->getState(i));
+ }
+ getchar();
+ }
+
+ }
+ catch (const std::exception& e) {
+ printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+ }
+ catch (...) {
+ printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+}
diff --git a/Simbody/tests/adhoc/MiscConstraints.cpp b/Simbody/tests/adhoc/MiscConstraints.cpp
new file mode 100644
index 0000000..4f5a7ee
--- /dev/null
+++ b/Simbody/tests/adhoc/MiscConstraints.cpp
@@ -0,0 +1,436 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Throwaway main program.
+ */
+
+#include "SimTKsimbody.h"
+
+#include <cmath>
+#include <cstdio>
+#include <exception>
+#include <vector>
+
+using namespace std;
+using namespace SimTK;
+
+static const Real Deg2Rad = (Real)SimTK_DEGREE_TO_RADIAN,
+ Rad2Deg = (Real)SimTK_RADIAN_TO_DEGREE;
+
+static const Transform GroundFrame;
+
+static const Real m = 1; // kg
+static const Real g = 9.8; // meters/s^2; apply in �y direction
+static const Real d = 0.5; // meters
+
+static const Vec3 hl(1, 0.5, 0.5); // body half lengths
+
+class MyConstraintImplementation : public Constraint::Custom::Implementation {
+public:
+ MyConstraintImplementation(MobilizedBody& mobilizer, Real speed)
+ : Implementation(mobilizer.updMatterSubsystem(), 0,1,0),
+ theMobilizer(), whichMobility(), prescribedSpeed(NaN)
+ {
+ theMobilizer = addConstrainedMobilizer(mobilizer);
+ whichMobility = MobilizerUIndex(0);
+ prescribedSpeed = speed;
+ }
+ MyConstraintImplementation* clone() const {return new MyConstraintImplementation(*this);}
+
+ // Implementation of virtuals required for nonholonomic constraints.
+
+ // One non-holonomic (well, velocity-level) constraint equation.
+ // verr = u - s
+ // aerr = udot
+ //
+ void calcVelocityErrors
+ (const State& s,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedU,
+ Array_<Real>& verr) const
+ {
+ assert(verr.size() == 1);
+ verr[0] = getOneU(s, constrainedU, theMobilizer, whichMobility)
+ - prescribedSpeed;
+ }
+
+ void calcVelocityDotErrors
+ (const State& s,
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedUIndex>& constrainedUDot,
+ Array_<Real>& vaerr) const
+ {
+ assert(vaerr.size() == 1);
+ vaerr[0] = getOneUDot(s, constrainedUDot,
+ theMobilizer, whichMobility);
+ }
+
+ // apply generalized force lambda to the mobility
+ void addInVelocityConstraintForcesVirtual
+ (const State& s,
+ const Array_<Real>& multipliers,
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real,ConstrainedUIndex>& mobilityForces) const
+ {
+ assert(multipliers.size() == 1);
+ const Real lambda = multipliers[0];
+ addInOneMobilityForce(s, theMobilizer, whichMobility,
+ lambda, mobilityForces);
+ }
+
+private:
+ ConstrainedMobilizerIndex theMobilizer;
+ MobilizerUIndex whichMobility;
+ Real prescribedSpeed;
+};
+
+class MyConstraint : public Constraint::Custom {
+public:
+ explicit MyConstraint(MobilizedBody& mobilizer, Real speed)
+ : Custom(new MyConstraintImplementation(mobilizer, speed))
+ {
+ }
+};
+
+
+int main(int argc, char** argv) {
+ try { // If anything goes wrong, an exception will be thrown.
+
+ // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
+ MultibodySystem mbs;
+
+ SimbodyMatterSubsystem matter(mbs);
+ GeneralForceSubsystem forces(mbs);
+ DecorationSubsystem viz(mbs);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -g, 0));
+
+ // ADD BODIES AND THEIR MOBILIZERS
+ Body::Rigid body = Body::Rigid(MassProperties(m, Vec3(0), m*UnitInertia::brick(hl[0],hl[1],hl[2])));
+ body.addDecoration(DecorativeBrick(hl).setOpacity(.5));
+ body.addDecoration(DecorativeLine(Vec3(0), Vec3(0,1,0)).setColor(Green));
+
+ MobilizedBody::Free mobilizedBody(matter.Ground(), Transform(), body, Transform());
+ MobilizedBody::Free mobilizedBody0(mobilizedBody, Transform(Vec3(1,2,0)), body, Transform(Vec3(0,1,0)));
+ MobilizedBody::Free mobilizedBody2(mobilizedBody0, Vec3(-5,0,0), body, Transform());
+
+ Body::Rigid gear1body = Body::Rigid(MassProperties(m, Vec3(0), m*UnitInertia::cylinderAlongZ(.5, .1)));
+ gear1body.addDecoration(DecorativeCircle(.5).setColor(Green).setOpacity(.7));
+ gear1body.addDecoration(DecorativeLine(Vec3(0), Vec3(.5,0,0)).setColor(Black).setLineThickness(4));
+ Body::Rigid gear2body = Body::Rigid(MassProperties(m, Vec3(0), m*UnitInertia::cylinderAlongZ(1.5, .1)));
+ gear2body.addDecoration(Transform(), DecorativeCircle(1.5).setColor(Blue).setOpacity(.7));
+ gear2body.addDecoration(Transform(), DecorativeLine(Vec3(0), Vec3(1.5,0,0)).setColor(Black).setLineThickness(4));
+
+ MobilizedBody::Pin gear1(mobilizedBody2, Vec3(-1,0,0), gear1body, Transform()); // along z
+ MobilizedBody::Pin gear2(mobilizedBody2, Vec3(1,0,0), gear2body, Transform()); // along z
+ Constraint::NoSlip1D(mobilizedBody2, Vec3(-.5,0,0), UnitVec3(0,1,0), gear1, gear2);
+
+ Constraint::ConstantSpeed(gear1, 100.);
+
+ //Constraint::Ball myc2(matter.Ground(), Vec3(-4,2,0), mobilizedBody2, Vec3(0,1,0));
+ Constraint::Weld myc(matter.Ground(), Vec3(1,2,0), mobilizedBody, Vec3(0,1,0));
+ Constraint::Ball ball1(mobilizedBody, Vec3(2,0,0), mobilizedBody0, Vec3(3,1,1));
+ Constraint::Ball ball2(mobilizedBody0, Vec3(2,0,0), mobilizedBody2, Vec3(3,0,0));
+ //Constraint::PointInPlane pip(mobilizedBody0, UnitVec3(0,-1,0), 3, mobilizedBody2, Vec3(3,0,0));
+
+ //Constraint::ConstantOrientation ori(mobilizedBody, Rotation(), mobilizedBody0, Rotation());
+ //Constraint::ConstantOrientation ori2(mobilizedBody2, Rotation(), mobilizedBody0, Rotation());
+ //Constraint::Weld weld(mobilizedBody, Transform(Rotation(Pi/4, ZAxis), Vec3(1,1,0)),
+ // mobilizedBody2, Transform(Rotation(-Pi/4, ZAxis), Vec3(-1,-1,0)));
+
+ //MyConstraint xyz(gear1, 100.);
+
+ viz.addBodyFixedDecoration(mobilizedBody, Transform(Vec3(1,2,3)), DecorativeText("hello world").setScale(.1));
+
+
+
+/*
+ class MyHandler : public ScheduledEventHandler {
+ public:
+ MyHandler(const Constraint& cons) : c(cons) { }
+ Real getNextEventTime(const State&, bool includeCurrentTime) const {
+ return .314;
+ }
+ void handleEvent(State& s, Real acc, const Vector& ywts, const Vector& cwts, Stage& modified,
+ bool& shouldTerminate) const
+ {
+ cout << "<<<< TRIGGERED AT T=" << s.getTime() << endl;
+ c.enable(s);
+ modified = Stage::Model;
+ }
+ private:
+ const Constraint& c;
+ };
+
+ mbs.addEventHandler(new MyHandler(xyz));
+*/
+
+
+ State s = mbs.realizeTopology(); // returns a reference to the the default state
+
+ //xyz.disable(s);
+
+ //matter.setUseEulerAngles(s, true);
+ mbs.realizeModel(s); // define appropriate states for this System
+
+ //mobilizedBody0.setQ(s, .1);
+ //mobilizedBody.setQ(s, .2);
+
+ Visualizer display(mbs);
+ display.setBackgroundColor(White);
+ display.setBackgroundType(Visualizer::SolidColor);
+
+ mbs.realize(s, Stage::Velocity);
+ display.report(s);
+ cout << "q=" << s.getQ() << endl;
+ cout << "u=" << s.getU() << endl;
+ cout << "qErr=" << s.getQErr() << endl;
+ cout << "uErr=" << s.getUErr() << endl;
+
+ for (ConstraintIndex cid(0); cid < matter.getNumConstraints(); ++cid) {
+ const Constraint& c = matter.getConstraint(cid);
+ int mp,mv,ma;
+ c.getNumConstraintEquationsInUse(s, mp,mv,ma);
+
+ cout << "CONSTRAINT " << cid << (c.isDisabled(s) ? "**DISABLED** " : "")
+ << " constrained bodies=" << c.getNumConstrainedBodies();
+ if (c.getNumConstrainedBodies()) cout << " ancestor=" << c.getAncestorMobilizedBody().getMobilizedBodyIndex();
+ cout << " constrained mobilizers/nq/nu=" << c.getNumConstrainedMobilizers()
+ << "/" << c.getNumConstrainedQ(s) << "/" << c.getNumConstrainedU(s)
+ << " mp,mv,ma=" << mp << "," << mv << "," << ma
+ << endl;
+ for (ConstrainedBodyIndex cid(0); cid < c.getNumConstrainedBodies(); ++cid) {
+ cout << " constrained body: " << c.getMobilizedBodyFromConstrainedBody(cid).getMobilizedBodyIndex();
+ cout << endl;
+ }
+ for (ConstrainedMobilizerIndex cmx(0); cmx < c.getNumConstrainedMobilizers(); ++cmx) {
+ cout << " constrained mobilizer " << c.getMobilizedBodyFromConstrainedMobilizer(cmx).getMobilizedBodyIndex()
+ << ", q(" << c.getNumConstrainedQ(s, cmx) << ")=";
+ for (MobilizerQIndex i(0); i < c.getNumConstrainedQ(s, cmx); ++i)
+ cout << " " << c.getConstrainedQIndex(s, cmx, i);
+ cout << ", u(" << c.getNumConstrainedU(s, cmx) << ")=";
+ for (MobilizerUIndex i(0); i < c.getNumConstrainedU(s, cmx); ++i)
+ cout << " " << c.getConstrainedUIndex(s, cmx, i);
+ cout << endl;
+ }
+ cout << c.getSubtree();
+
+ if (mp) {
+ cout << "perr=" << c.getPositionErrorsAsVector(s) << endl;
+ cout << " d(perrdot)/du=" << c.calcPositionConstraintMatrixP(s);
+ cout << " ~d(Pt lambda)/dlambda=" << ~c.calcPositionConstraintMatrixPt(s);
+ cout << " d(perr)/dq=" << c.calcPositionConstraintMatrixPNInv(s);
+
+ Matrix P = c.calcPositionConstraintMatrixP(s);
+ Matrix PQ(mp,matter.getNQ(s));
+ Vector out(matter.getNQ(s));
+ for (int i=0; i<mp; ++i) {
+ Vector in = ~P[i];
+ matter.multiplyByNInv(s, true, in, out);
+ PQ[i] = ~out;
+ }
+ cout << " calculated d(perr)/dq=" << PQ;
+ }
+
+
+ if (mv) {
+ cout << "verr=" << c.getVelocityErrorsAsVector(s) << endl;
+ //cout << " d(verrdot)/dudot=" << c.calcVelocityConstraintMatrixV(s);
+ cout << " ~d(Vt lambda)/dlambda=" << ~c.calcVelocityConstraintMatrixVt(s);
+ }
+
+ }
+ const Constraint& c = matter.getConstraint(myc.getConstraintIndex());
+
+ cout << "Default configuration shown. Ready? "; getchar();
+
+ mobilizedBody.setQToFitTransform (s, Transform(Rotation(.05,Vec3(1,1,1)),Vec3(.1,.2,.3)));
+ mobilizedBody0.setQToFitTransform (s, Transform(Rotation(.05,Vec3(1,-1,1)),Vec3(.2,.2,.3)));
+ mobilizedBody2.setQToFitTransform (s, Transform(Rotation(.05,Vec3(-1,1,1)),Vec3(.1,.2,.1)));
+ mobilizedBody.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
+ mobilizedBody0.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
+ mobilizedBody2.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
+
+ //gear1.setUToFitAngularVelocity(s, Vec3(0,0,500)); // these should be opposite directions!
+ //gear2.setUToFitAngularVelocity(s, Vec3(0,0,100));
+
+ mbs.realize(s, Stage::Velocity);
+ display.report(s);
+
+ cout << "q=" << s.getQ() << endl;
+ cout << "u=" << s.getU() << endl;
+ cout << "qErr=" << s.getQErr() << endl;
+ cout << "uErr=" << s.getUErr() << endl;
+ cout << "p_MbM=" << mobilizedBody.getMobilizerTransform(s).p() << endl;
+ cout << "v_MbM=" << mobilizedBody.getMobilizerVelocity(s)[1] << endl;
+ cout << "Unassembled configuration shown. Ready to assemble? "; getchar();
+
+ // These are the SimTK Simmath integrators:
+ RungeKuttaMersonIntegrator myStudy(mbs);
+ //CPodesIntegrator myStudy(mbs, CPodes::BDF, CPodes::/*Newton*/Functional);
+ //myStudy.setOrderLimit(2); // cpodes only
+ //VerletIntegrator myStudy(mbs);
+ // ExplicitEulerIntegrator myStudy(mbs, .0005); // fixed step
+ //ExplicitEulerIntegrator myStudy(mbs); // variable step
+
+
+ //myStudy.setMaximumStepSize(0.001);
+ myStudy.setAccuracy(1e-6); myStudy.setAccuracy(1e-1);
+ //myStudy.setProjectEveryStep(true);
+ //myStudy.setProjectInterpolatedStates(false);
+ myStudy.setConstraintTolerance(1e-7); myStudy.setConstraintTolerance(1e-2);
+ //myStudy.setAllowInterpolation(false);
+ //myStudy.setMaximumStepSize(.1);
+
+ const Real dt = .02; // output intervals
+ const Real finalTime = 2;
+
+ myStudy.setFinalTime(finalTime);
+
+ std::vector<State> saveEm;
+ saveEm.reserve(2000);
+
+ for (int i=0; i<50; ++i)
+ saveEm.push_back(s); // delay
+
+
+ // Peforms assembly if constraints are violated.
+ myStudy.initialize(s);
+
+ for (int i=0; i<50; ++i)
+ saveEm.push_back(s); // delay
+
+ cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
+ cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
+ cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
+ cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
+ cout << "U WEIGHTS=" << s.getUWeights() << endl;
+ cout << "Z WEIGHTS=" << s.getZWeights() << endl;
+ cout << "1/QTOLS=" << s.getQErrWeights() << endl;
+ cout << "1/UTOLS=" << s.getUErrWeights() << endl;
+
+ {
+ const State& s = myStudy.getState();
+ display.report(s);
+ cout << "q=" << s.getQ() << endl;
+ cout << "u=" << s.getU() << endl;
+ cout << "qErr=" << s.getQErr() << endl;
+ cout << "uErr=" << s.getUErr() << endl;
+ cout << "p_MbM=" << mobilizedBody.getMobilizerTransform(s).p() << endl;
+ cout << "PE=" << mbs.calcPotentialEnergy(s) << " KE=" << mbs.calcKineticEnergy(s) << " E=" << mbs.calcEnergy(s) << endl;
+ cout << "angle=" << std::acos(~mobilizedBody.expressVectorInGroundFrame(s, Vec3(0,1,0)) * UnitVec3(1,1,1)) << endl;
+ cout << "Assembled configuration shown. Ready to simulate? "; getchar();
+ }
+
+ Integrator::SuccessfulStepStatus status;
+ int nextReport = 0;
+
+ mbs.resetAllCountersToZero();
+
+ int stepNum = 0;
+ while ((status=myStudy.stepTo(nextReport*dt))
+ != Integrator::EndOfSimulation)
+ {
+ const State& s = myStudy.getState();
+ mbs.realize(s, Stage::Acceleration);
+
+ if ((stepNum++%10)==0) {
+ const Real angle = std::acos(~mobilizedBody.expressVectorInGroundFrame(s, Vec3(0,1,0)) * UnitVec3(1,1,1));
+ printf("%5g %10.4g E=%10.8g h%3d=%g %s%s\n", s.getTime(),
+ angle,
+ mbs.calcEnergy(s), myStudy.getNumStepsTaken(),
+ myStudy.getPreviousStepSizeTaken(),
+ Integrator::getSuccessfulStepStatusString(status).c_str(),
+ myStudy.isStateInterpolated()?" (INTERP)":"");
+ printf(" qerr=%10.8g uerr=%10.8g uderr=%10.8g\n",
+ matter.getQErr(s).normRMS(),
+ matter.getUErr(s).normRMS(),
+ s.getSystemStage() >= Stage::Acceleration ? matter.getUDotErr(s).normRMS() : Real(-1));
+#ifdef HASC
+ cout << "CONSTRAINT perr=" << c.getPositionError(s)
+ << " verr=" << c.getVelocityError(s)
+ << " aerr=" << c.getAccelerationError(s)
+ << endl;
+#endif
+ //cout << " d(perrdot)/du=" << c.calcPositionConstraintMatrixP(s);
+ //cout << " ~d(f)/d lambda=" << c.calcPositionConstraintMatrixPT(s);
+ //cout << " d(perr)/dq=" << c.calcPositionConstraintMatrixPQInverse(s);
+ cout << "Q=" << matter.getQ(s) << endl;
+ cout << "U=" << matter.getU(s) << endl;
+ cout << "Multipliers=" << matter.getMultipliers(s) << endl;
+ }
+
+ Vector qdot;
+ matter.calcQDot(s, s.getU(), qdot);
+ // cout << "===> qdot =" << qdot << endl;
+
+ Vector qdot2;
+ matter.multiplyByN(s, false, s.getU(), qdot2);
+ // cout << "===> qdot2=" << qdot2 << endl;
+
+ Vector u1,u2;
+ matter.multiplyByNInv(s, false, qdot, u1);
+ matter.multiplyByNInv(s, false, qdot2, u2);
+ // cout << "===> u =" << s.getU() << endl;
+ // cout << "===> u1=" << u1 << endl;
+ // cout << "===> u2=" << u2 << endl;
+ // cout << " norm=" << (s.getU()-u2).normRMS() << endl;
+
+ display.report(s);
+ saveEm.push_back(s);
+
+ if (status == Integrator::ReachedReportTime)
+ ++nextReport;
+ }
+
+ printf("Using Integrator %s:\n", myStudy.getMethodName());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());
+
+ printf("System stats: realize %dP %dV %dA, projectQ %d, projectU %d\n",
+ mbs.getNumRealizationsOfThisStage(Stage::Position),
+ mbs.getNumRealizationsOfThisStage(Stage::Velocity),
+ mbs.getNumRealizationsOfThisStage(Stage::Acceleration),
+ mbs.getNumProjectQCalls(), mbs.getNumProjectUCalls());
+
+
+ while(true) {
+ for (int i=0; i < (int)saveEm.size(); ++i) {
+ display.report(saveEm[i]);
+ //display.report(saveEm[i]); // half speed
+ }
+ getchar();
+ }
+ }
+ catch (const exception& e) {
+ printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+ }
+ catch (...) {
+ printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+}
+
diff --git a/Simbody/tests/adhoc/MovingMusclePointMomentArm.cpp b/Simbody/tests/adhoc/MovingMusclePointMomentArm.cpp
new file mode 100644
index 0000000..58ce067
--- /dev/null
+++ b/Simbody/tests/adhoc/MovingMusclePointMomentArm.cpp
@@ -0,0 +1,455 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Adhoc test: Moving Muscle Point Moment Arm *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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 "Simbody.h"
+#include <iostream>
+
+using namespace SimTK;
+using std::cout; using std::endl;
+
+// Uncomment this to use an explicit rack-and-pinion mechanism rather than
+// the moving muscle point.
+#define USE_RACK
+
+/*
+In biomechanics muscle forces are modeled as acting on the system via
+frictionless cables anchored at an "origin point" on one bone and following a
+curved path over obstacles to a final anchor at an "insertion point" on another
+bone. The cables have uniform tension and apply forces to the end points and
+the obstacles over which they pass.
+
+Because it can be very expensive to calculate the actual path, obstacle
+surfaces are often replaced by simplified representations involving "via points"
+(frictionless eyelets fixed to bones), or "moving muscle points" (MMPs). An MMP
+is a via point that moves around on its body's surface, with its location P(q)
+given in its body's local frame as a smooth kinematic function of a designated
+generalized coordinate q. The function P(q) is typically a vector-valued spline
+fit through point locations measured at sampled coordinate values taken on a
+cadaver or a more complex computational model.
+
+For information on how we calculate moment arm in OpenSim using Simbody's
+tools, see the paper
+ Sherman MA, Seth A, Delp SL. What is moment arm? Calculating muscle
+ effectiveness in biomechanical models using generalized coordinates. Proc.
+ ASME IDETC/CIE Conference, Paper DETC2013-13633, Aug 2013, Portland, Oregon.
+ http://doi.org/10.1115/DETC2013-13633
+This paper discusses problems with MMPs. The method demonstrated below addresses
+those problems allowing meaningful moment arms to be calculate in models with
+MMPs.
+
+We would like to ensure that the moment arm and dynamics we calculate using the
+reduced model with MMPs is the same as we would have gotten with the more
+complex model, assuming that P(q) is the same in both models. (Reaction forces
+will unavoidably differ, but accelerations should be the same.) One way to do
+that is to calculate moment arm by perturbation r(theta)=dL/dtheta where L is
+the length of the muscle path. We would like to be able to obtain the same value
+for r(theta) with an instantaneous calculation r(theta)=tau_theta/s where s
+is the tension in the muscle path and tau_theta is the generalized torque
+produced about theta by that tension. The two values are equivalent if all
+constraints in the model are workless. For an MMP, there must be a workless
+constraint that accounts for its motion. In the real system, that will be
+produced by the mechanical contacts and tendons that form the joint. In the MMP
+model we don't have that mechanical system present but would like to treat it as
+though motion were caused by an equivalent "gearbox" driven by q.
+
+In this example, we will build a simplified knee-like mechanism in which the
+gearbox is explicitly present, and then attempt to get the same moment arm from
+a simplified model in which only P(q) is known. The mechanism looks like this:
+
+ / femur
+ .I/ I=insertion point fixed on femur
+ M . / M=muscle path of interest (dots)
+ . @ q
+ P---<--===-->--- rack
+ . |
+ . | tibia
+ .O O=origin point fixed on tibia (fixed to Ground)
+ |
+ ||| Ground
+
+We have built a rack mechanism to replace the patella for determining where
+point P is located as a function of q. Specifically, given a pitch we can find
+P=(Px,Py,Pz)=(P0x + pitch*q, P0y, P0z) and dP/dq=(pitch,0,0). The muscle path
+length L=|P-O|+|I-P|.
+*/
+
+/* This muscle uses a via point fixed to the rack body. */
+class MuscleVP : public Force::Custom::Implementation {
+public:
+ MuscleVP(const SimbodyMatterSubsystem& matter,
+ const MobilizedBody& A, const Vec3& origin,
+ const MobilizedBody& V, const Vec3& via,
+ const MobilizedBody& B, const Vec3& insertion,
+ Real stiffness, Real zeroLength)
+ : m_matter(matter), m_A(A), m_ptA(origin), m_V(V), m_ptV(via),
+ m_B(B), m_ptB(insertion), m_k(stiffness), m_zero(zeroLength),
+ m_tension(NaN)
+ {
+ }
+
+ Real calcLength(const State& state) const {
+ // End points and via point in Ground.
+ const Vec3 ptA = m_A.findStationLocationInGround(state, m_ptA);
+ const Vec3 ptB = m_B.findStationLocationInGround(state, m_ptB);
+ const Vec3 ptV = m_V.findStationLocationInGround(state, m_ptV);
+
+ const Vec3 A2V(ptV - ptA), B2V(ptV - ptB);
+ const Real Alen = A2V.norm(), Blen = B2V.norm();
+ const Real len = Alen + Blen;
+ return len;
+ }
+
+ Real calcTension(const State& state) const {
+ const Real len = calcLength(state);
+ if (len <= m_zero) return 0;
+ return m_k*(len-m_zero);
+ }
+
+ // Set to NaN to enable k*x tension instead. Don't forget to invalidate
+ // the state.
+ void setTension(Real tension) {m_tension=tension;}
+ Real getTension() const { return m_tension; }
+
+ // Calculate the muscle forces and accumulate into bodyForces array.
+ // (We aren't going to generate any particle or mobility forces.)
+ void calcForce(const State& state,
+ Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces,
+ Vector& mobilityForces) const OVERRIDE_11
+ {
+ // End points and via point in Ground.
+ const Vec3 ptA = m_A.findStationLocationInGround(state, m_ptA);
+ const Vec3 ptB = m_B.findStationLocationInGround(state, m_ptB);
+ const Vec3 ptV = m_V.findStationLocationInGround(state, m_ptV);
+
+ const Vec3 A2V(ptV - ptA), B2V(ptV - ptB);
+ const Real Alen = A2V.norm(), Blen = B2V.norm();
+ const Real len = Alen + Blen;
+
+ const UnitVec3 uA2V(A2V / Alen, true), uB2V(B2V / Blen, true);
+
+ const Real tension = isNaN(m_tension)
+ ? ((len>m_zero) ? m_k * (len - m_zero) : Real(0))
+ : m_tension;
+
+ const Vec3 fA = tension*uA2V;
+ const Vec3 fB = tension*uB2V;
+
+ m_A.applyForceToBodyPoint(state, m_ptA, fA, bodyForces);
+ m_B.applyForceToBodyPoint(state, m_ptB, fB, bodyForces);
+ m_V.applyForceToBodyPoint(state, m_ptV, -(fA + fB), bodyForces);
+ }
+
+ Real calcPotentialEnergy(const State&) const OVERRIDE_11 { return 0; }
+private:
+ const SimbodyMatterSubsystem& m_matter;
+ const MobilizedBody m_A, m_V, m_B;
+ const Vec3 m_ptA, m_ptV, m_ptB;
+ const Real m_k, m_zero;
+
+ Real m_tension; // NaN to calculate as k*x
+};
+
+/* This muscle uses a differentiable function
+ P(q)_A=P0_A + (pitch, 0, 0)*q
+to determine the location of a moving muscle point on the origin body A;
+there is no rack body. */
+class MuscleMMP : public Force::Custom::Implementation {
+public:
+ MuscleMMP(const SimbodyMatterSubsystem& matter,
+ const MobilizedBody& A, const Vec3& origin,
+ const MobilizedBody& B, const Vec3& insertion,
+ const Vec3& P0_A, Real pitch,
+ Real stiffness, Real zeroLength)
+ : m_matter(matter), m_A(A), m_ptA(origin), m_B(B), m_ptB(insertion),
+ m_P0(P0_A), m_pitch(pitch),
+ m_k(stiffness), m_zero(zeroLength), m_tension(NaN)
+ {
+ }
+
+ // Calculate P(q), in A frame.
+ Vec3 calcP(const State& state) const {
+ const Real q = m_B.getOneQ(state, MobilizerQIndex(0));
+ return m_P0 + Vec3(m_pitch*q, 0, 0);
+ }
+
+ // Calculate dP/dq.
+ Vec3 calcdPdq(const State& state) const {
+ return Vec3(m_pitch, 0, 0);
+ }
+
+ void calcPathPoints(const State& state, Array_<Vec3>& pts_G) const {
+ // End points and moving muscle point in Ground.
+ const Vec3 ptA = m_A.findStationLocationInGround(state, m_ptA);
+ const Vec3 ptP = m_A.findStationLocationInGround(state, calcP(state));
+ const Vec3 ptB = m_B.findStationLocationInGround(state, m_ptB);
+ pts_G.clear();
+ pts_G.push_back(ptA); pts_G.push_back(ptP); pts_G.push_back(ptB);
+ }
+
+
+ Real calcLength(const State& state) const {
+ // End points and via point in Ground.
+ const Vec3 ptA = m_A.findStationLocationInGround(state, m_ptA);
+ const Vec3 ptP = m_A.findStationLocationInGround(state, calcP(state));
+ const Vec3 ptB = m_B.findStationLocationInGround(state, m_ptB);
+
+ const Vec3 A2P(ptP - ptA), B2P(ptP - ptB);
+ const Real Alen = A2P.norm(), Blen = B2P.norm();
+ const Real len = Alen + Blen;
+ return len;
+ }
+
+ Real calcTension(const State& state) const {
+ const Real len = calcLength(state);
+ if (len <= m_zero) return 0;
+ return m_k*(len - m_zero);
+ }
+
+ // Set to NaN to enable k*x tension instead. Don't forget to invalidate
+ // the state.
+ void setTension(Real tension) { m_tension = tension; }
+ Real getTension() const { return m_tension; }
+
+ // Calculate the muscle forces and accumulate into bodyForces Vector, plus
+ // a correction force that is added to the mobilityForces Vector.
+ void calcForce(const State& state,
+ Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces,
+ Vector& mobilityForces) const OVERRIDE_11
+ {
+ // End points and via point in Ground.
+ const Vec3 ptA = m_A.findStationLocationInGround(state, m_ptA);
+ const Vec3 ptB = m_B.findStationLocationInGround(state, m_ptB);
+ const Vec3 ptP_A = calcP(state);
+ const Vec3 ptP = m_A.findStationLocationInGround(state, ptP_A);
+
+ const Vec3 A2P(ptP - ptA), B2P(ptP - ptB);
+ const Real Alen = A2P.norm(), Blen = B2P.norm();
+ const Real len = Alen + Blen;
+
+ const UnitVec3 uA2P(A2P / Alen, true), uB2P(B2P / Blen, true);
+
+ const Real tension = isNaN(m_tension)
+ ? ((len>m_zero) ? m_k * (len - m_zero) : Real(0))
+ : m_tension;
+
+ const Vec3 fA = tension*uA2P;
+ const Vec3 fB = tension*uB2P;
+ const Vec3 fP = -(fA + fB);
+ const Real f = ~calcdPdq(state)*fP;
+
+ m_A.applyForceToBodyPoint(state, m_ptA, fA, bodyForces);
+ m_B.applyForceToBodyPoint(state, m_ptB, fB, bodyForces);
+ m_A.applyForceToBodyPoint(state, ptP_A, fP, bodyForces);
+ m_B.applyOneMobilityForce(state, MobilizerQIndex(0), f, mobilityForces);
+ }
+
+ Real calcPotentialEnergy(const State&) const OVERRIDE_11 { return 0; }
+private:
+ const SimbodyMatterSubsystem& m_matter;
+ const MobilizedBody m_A, m_V, m_B;
+ const Vec3 m_ptA, m_ptB, m_P0;
+ const Real m_pitch, m_k, m_zero;
+
+ Real m_tension;
+};
+
+class DrawPath : public DecorationGenerator {
+public:
+ explicit DrawPath(const MuscleMMP& muscle)
+ : m_muscle(muscle) {}
+
+ void generateDecorations(const State& state,
+ Array_<DecorativeGeometry>& geometry) OVERRIDE_11{
+ Array_<Vec3> path;
+ m_muscle.calcPathPoints(state, path);
+ for (unsigned i = 1; i < path.size(); ++i) {
+ geometry.push_back(DecorativeLine(path[i - 1], path[i])
+ .setColor(Red).setLineThickness(3));
+ if (i + 1 < path.size())
+ geometry.push_back(DecorativePoint(path[i]).setColor(Cyan));
+ }
+ }
+private:
+ const MuscleMMP& m_muscle;
+};
+
+//==============================================================================
+// MAIN
+//==============================================================================
+int main() {
+ try {
+ // Create the system.
+ // ------------------
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+
+ // Add gravity as a force element.
+ Force::Gravity gravity(forces, matter, -YAxis, 9.8);
+
+ // Create body specifications.
+ // ---------------------------
+ Vec3 tibiaSz(.07, 1., .05), femurSz(.1, 1., .05), rackSz(1, .02, .02);
+ Vec3 origin_T(-tibiaSz[0] / 2, -tibiaSz[1] / 6, 0);
+ Vec3 insertion_F(-femurSz[0] / 2, 0.9*femurSz[1] / 2, 0);
+ Vec3 mmp_R(-rackSz[0]/2, 0, 0);
+ // This is where mmp is in the tibia frame when q=0.
+ Real rackHtInTibia = 0.9*tibiaSz[1]/2;
+ Vec3 P0_A(-rackSz[0]/2, rackHtInTibia, 0);
+
+ Body::Rigid tibiaBody(MassProperties(1, Vec3(0),
+ UnitInertia::brick(tibiaSz / 2.)));
+ tibiaBody.addDecoration(DecorativeBrick(tibiaSz / 2.).setColor(Green));
+ tibiaBody.addDecoration(DecorativePoint(origin_T));
+
+ Body::Rigid femurBody(MassProperties(1, Vec3(0),
+ UnitInertia::brick(femurSz / 2.)));
+ femurBody.addDecoration(DecorativeBrick(femurSz / 2.).setColor(Blue));
+ femurBody.addDecoration(DecorativePoint(insertion_F));
+
+ Body::Rigid rackBody(MassProperties(.01, Vec3(0),
+ UnitInertia::brick(rackSz/2.)));
+ rackBody.addDecoration(DecorativeBrick(rackSz / 2.).setColor(Red));
+
+ // Create mobilized bodies.
+ // ------------------------
+ MobilizedBody::Weld tibia(
+ matter.Ground(), Vec3(0),
+ tibiaBody, Vec3(0, -tibiaSz[1] / 2, 0));
+ MobilizedBody::Pin femur(
+ tibia, Vec3(0, tibiaSz[1] / 2, 0),
+ femurBody, Vec3(0, -femurSz[1] / 2, 0));
+
+ // Visualize.
+ // ----------
+ Visualizer viz(system);
+ const Real pitch = .3; // m/radian
+
+#ifdef USE_RACK
+ MobilizedBody::Slider rack(
+ tibia, Vec3(0, rackHtInTibia, 0),
+ rackBody, Vec3(0));
+ // Add rack & pinion constraint.
+ Array_<MobilizedBodyIndex> mobods(2);
+ Array_<MobilizerQIndex> coords(2);
+ mobods[0] = femur; coords[0] = MobilizerQIndex(0);
+ mobods[1] = rack; coords[1] = MobilizerQIndex(0);
+ Constraint::CoordinateCoupler(matter,
+ new Function::Linear(Vector(Vec3(-pitch,1,0))),
+ mobods, coords);
+ MuscleVP* mmp = new MuscleVP(matter, tibia, origin_T, rack, mmp_R,
+ femur, insertion_F, 100., 2.75);
+
+ viz.addRubberBandLine(tibia, origin_T, rack, mmp_R,
+ DecorativeLine().setColor(Red));
+ viz.addRubberBandLine(femur, insertion_F, rack, mmp_R,
+ DecorativeLine().setColor(Red));
+#else
+ MuscleMMP* mmp = new MuscleMMP(matter, tibia, origin_T, femur, insertion_F,
+ P0_A, pitch, 100., 2.75);
+ viz.addDecorationGenerator(new DrawPath(*mmp));
+#endif
+
+ Force::Custom muscle(forces, mmp);
+
+ // Add some damping.
+ // -----------------
+ //Force::MobilityLinearDamper(forces, femur, MobilizerQIndex(0), 1.);
+
+ // Report at the framerate (real-time).
+ system.addEventReporter(new Visualizer::Reporter(viz, 1. / 30));
+
+ // Initialize the system and state.
+ // --------------------------------
+ State state = system.realizeTopology();
+ femur.lockAt(state, -Pi/4);
+ system.projectQ(state, 1e-10);
+
+ // Calculate moment arm by dL/dq.
+ system.realize(state);
+ Real q0 = femur.getAngle(state);
+ Real L0 = mmp->calcLength(state);
+ femur.lockAt(state, q0 + 1e-6);
+ system.projectQ(state, 1e-10);
+ Real q1 = femur.getAngle(state);
+ Real L1 = mmp->calcLength(state);
+ Real r = (L1 - L0) / (q1-q0);
+ printf("q1-q0=%g, L1-L0=%g, r=%g\n",
+ q1 - q0, L1 - L0, r);
+ femur.unlock(state);
+
+ printf("Assembled:\n");
+ viz.report(state);
+ getchar();
+
+ //----------------------------
+ // CALCULATE COUPLING MATRIX C
+ //----------------------------
+ state.updU() = 0;
+ femur.lockAt(state, 1., Motion::Velocity);
+ system.realize(state, Stage::Velocity);
+ cout << "before project u=" << state.getU() << endl;
+ system.projectU(state, 1e-10);
+ cout << "after project u=" << state.getU()
+ << " uerr=" << state.getUErr() << endl;
+ const Vector C(state.getU());
+ femur.unlock(state);
+ state.updU() = 0;
+
+ Vector_<SpatialVec> bodyForces;
+ Vector_<Vec3> particleForces;
+ Vector mobilityForces;
+ mmp->setTension(1); // override k*x spring force with unit tension
+ state.invalidateAllCacheAtOrAbove(Stage::Velocity);
+ system.realize(state, Stage::Velocity);
+ muscle.calcForceContribution(state, bodyForces, particleForces,
+ mobilityForces);
+ mmp->setTension(NaN); // back to k*x spring
+ cout << "bodyForces=" << bodyForces << endl;
+ cout << "mobilityForces=" << mobilityForces << endl;
+
+ Vector equivForces;
+ matter.multiplyBySystemJacobianTranspose(state, bodyForces, equivForces);
+ equivForces += mobilityForces;
+ cout << "tension=1 --> equivForces=" << equivForces << endl;
+
+ Real r2 = ~C*equivForces;
+ printf("gen force r=%g\n", r2);
+ getchar();
+
+ // Simulate.
+ // ---------
+ RungeKuttaMersonIntegrator integ(system);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(5.0);
+
+ } catch (const std::exception& e) {
+ std::cout << "Exception: " << e.what() << std::endl;
+ return 1;
+ }
+ return 0;
+};
\ No newline at end of file
diff --git a/Simbody/tests/adhoc/OpenSimPartyDemoCable.cpp b/Simbody/tests/adhoc/OpenSimPartyDemoCable.cpp
new file mode 100644
index 0000000..eedccf4
--- /dev/null
+++ b/Simbody/tests/adhoc/OpenSimPartyDemoCable.cpp
@@ -0,0 +1,261 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) Adhoc Test: Cable Over Bicubic Surfaces *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Andreas Scholz *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* Simbody OpenSimPartyDemoCable
+THIS DOESN'T WORK YET */
+
+#include "Simbody.h"
+
+#include <cassert>
+#include <iostream>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+// This gets called periodically to dump out interesting things about
+// the cables and the system as a whole. It also saves states so that we
+// can play back at the end.
+static Array_<State> saveStates;
+class ShowStuff : public PeriodicEventReporter {
+public:
+ ShowStuff(const MultibodySystem& mbs,
+ const CableSpring& cable1, Real interval)
+ : PeriodicEventReporter(interval),
+ mbs(mbs), cable1(cable1) {}
+
+ static void showHeading(std::ostream& o) {
+ printf("%8s %10s %10s %10s %10s %10s %10s %10s %10s %12s\n",
+ "time", "length", "rate", "integ-rate", "unitpow", "tension", "disswork",
+ "KE", "PE", "KE+PE-W");
+ }
+
+ /** This is the implementation of the EventReporter virtual. **/
+ void handleEvent(const State& state) const OVERRIDE_11 {
+ const CablePath& path1 = cable1.getCablePath();
+ printf("%8g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %10.4g %12.6g CPU=%g\n",
+ state.getTime(),
+ path1.getCableLength(state),
+ path1.getCableLengthDot(state),
+ path1.getIntegratedCableLengthDot(state),
+ path1.calcCablePower(state, 1), // unit power
+ cable1.getTension(state),
+ cable1.getDissipatedEnergy(state),
+ mbs.calcKineticEnergy(state),
+ mbs.calcPotentialEnergy(state),
+ mbs.calcEnergy(state)
+ + cable1.getDissipatedEnergy(state),
+ cpuTime());
+ saveStates.push_back(state);
+ }
+private:
+ const MultibodySystem& mbs;
+ CableSpring cable1;
+};
+
+int main() {
+ try {
+ // Create the system.
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+
+ matter.setShowDefaultGeometry(false);
+
+ CableTrackerSubsystem cables(system);
+ GeneralForceSubsystem forces(system);
+
+ Force::Gravity gravity(forces, matter, -YAxis, 9.81);
+ // Force::GlobalDamper(forces, matter, 5);
+
+ system.setUseUniformBackground(true); // no ground plane in display
+ MobilizedBody Ground = matter.Ground(); // convenient abbreviation
+
+ // Read in some bones.
+ PolygonalMesh femur;
+ PolygonalMesh tibia;
+
+ femur.loadVtpFile("CableOverBicubicSurfaces-femur.vtp");
+ tibia.loadVtpFile("CableOverBicubicSurfaces-tibia.vtp");
+ femur.scaleMesh(30);
+ tibia.scaleMesh(30);
+
+ // Build a pendulum
+ Body::Rigid pendulumBodyFemur( MassProperties(1.0, Vec3(0, -5, 0),
+ UnitInertia(1).shiftFromCentroid(Vec3(0, 5, 0))));
+
+ pendulumBodyFemur.addDecoration(Transform(), DecorativeMesh(femur).setColor(Vec3(0.8, 0.8, 0.8)));
+
+ Body::Rigid pendulumBodyTibia( MassProperties(1.0, Vec3(0, -5, 0),
+ UnitInertia(1).shiftFromCentroid(Vec3(0, 5, 0))));
+
+ pendulumBodyTibia.addDecoration(Transform(), DecorativeMesh(tibia).setColor(Vec3(0.8, 0.8, 0.8)));
+
+ Rotation z180(Pi, YAxis);
+
+ MobilizedBody::Pin pendulumFemur( matter.updGround(),
+ Transform(Vec3(0, 0, 0)),
+ pendulumBodyFemur,
+ Transform(Vec3(0, 0, 0)) );
+
+ Rotation rotZ45(-Pi/4, ZAxis);
+
+ MobilizedBody::Pin pendulumTibia( pendulumFemur,
+ Transform(rotZ45, Vec3(0, -12, 0)),
+ pendulumBodyTibia,
+ Transform(Vec3(0, 0, 0)) );
+
+ Real initialPendulumOffset = -0.25*Pi;
+
+ Constraint::PrescribedMotion pres(matter,
+ new Function::Sinusoid(0.25*Pi, 0.2*Pi, 0*initialPendulumOffset), pendulumTibia, MobilizerQIndex(0));
+
+ // Build a wrapping cable path
+ CablePath path2(cables, Ground, Vec3(1, 3, 1), // origin
+ pendulumTibia, Vec3(1, -4, 0)); // termination
+
+ // Create a bicubic surface
+ Vec3 patchOffset(0, -5, -1);
+ Rotation rotZ90(0.5*Pi, ZAxis);
+ Rotation rotX90(0.2*Pi, XAxis);
+
+ Rotation patchRotation = rotZ90 * rotX90 * rotZ90;
+ Transform patchTransform(patchRotation, patchOffset);
+
+ Real patchScaleX = 2.0;
+ Real patchScaleY = 2.0;
+ Real patchScaleF = 0.75;
+
+ const int Nx = 4, Ny = 4;
+
+ const Real xData[Nx] = { -2, -1, 1, 2 };
+ const Real yData[Ny] = { -2, -1, 1, 2 };
+
+ const Real fData[Nx*Ny] = { 2, 3, 3, 1,
+ 0, 1.5, 1.5, 0,
+ 0, 1.5, 1.5, 0,
+ 2, 3, 3, 1 };
+
+ const Vector x_(Nx, xData);
+ const Vector y_(Ny, yData);
+ const Matrix f_(Nx, Ny, fData);
+
+ Vector x = patchScaleX*x_;
+ Vector y = patchScaleY*y_;
+ Matrix f = patchScaleF*f_;
+
+ BicubicSurface patch(x, y, f, 0);
+
+ Real highRes = 30;
+ Real lowRes = 1;
+
+ PolygonalMesh highResPatchMesh = patch.createPolygonalMesh(highRes);
+ PolygonalMesh lowResPatchMesh = patch.createPolygonalMesh(lowRes);
+
+
+ pendulumFemur.addBodyDecoration(patchTransform,
+ DecorativeMesh(highResPatchMesh).setColor(Cyan).setOpacity(.75));
+
+ pendulumFemur.addBodyDecoration(patchTransform,
+ DecorativeMesh(lowResPatchMesh).setRepresentation(DecorativeGeometry::DrawWireframe));
+
+ Vec3 patchP(-0.5,-1,2), patchQ(-0.5,1,2);
+
+ pendulumFemur.addBodyDecoration(patchTransform,
+ DecorativePoint(patchP).setColor(Green).setScale(2));
+
+ pendulumFemur.addBodyDecoration(patchTransform,
+ DecorativePoint(patchQ).setColor(Red).setScale(2));
+
+ CableObstacle::Surface patchObstacle(path2, pendulumFemur, patchTransform,
+ ContactGeometry::SmoothHeightMap(patch));
+
+ patchObstacle.setContactPointHints(patchP, patchQ);
+
+ patchObstacle.setDisabledByDefault(true);
+
+ // Sphere
+ Real sphRadius = 1.5;
+
+ Vec3 sphOffset(0, -0.5, 0);
+ Rotation sphRotation(0*Pi, YAxis);
+ Transform sphTransform(sphRotation, sphOffset);
+
+ CableObstacle::Surface tibiaSphere(path2, pendulumTibia, sphTransform,
+ ContactGeometry::Sphere(sphRadius));
+
+ Vec3 sphP(1.5,-0.5,0), sphQ(1.5,0.5,0);
+ tibiaSphere.setContactPointHints(sphP, sphQ);
+
+ pendulumTibia.addBodyDecoration(sphTransform,
+ DecorativeSphere(sphRadius).setColor(Red).setOpacity(0.5));
+
+ // Make cable a spring
+ CableSpring cable2(forces, path2, 50., 18., 0.1);
+
+ Visualizer viz(system);
+ viz.setShowFrameNumber(true);
+ system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
+ system.addEventReporter(new ShowStuff(system, cable2, 0.02));
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+
+ system.realize(state, Stage::Position);
+ viz.report(state);
+ cout << "path2 init length=" << path2.getCableLength(state) << endl;
+ cout << "Hit ENTER ...";
+ getchar();
+
+ // path1.setIntegratedCableLengthDot(state, path1.getCableLength(state));
+
+ // Simulate it.
+ saveStates.clear(); saveStates.reserve(2000);
+
+ // RungeKutta3Integrator integ(system);
+ RungeKuttaMersonIntegrator integ(system);
+ // CPodesIntegrator integ(system);
+ // integ.setAllowInterpolation(false);
+ integ.setAccuracy(1e-5);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ShowStuff::showHeading(cout);
+
+ const Real finalTime = 10;
+ const double startTime = realTime();
+ ts.stepTo(finalTime);
+ cout << "DONE with " << finalTime
+ << "s simulated in " << realTime()-startTime
+ << "s elapsed.\n";
+
+ while (true) {
+ cout << "Hit ENTER FOR REPLAY, Q to quit ...";
+ const char ch = getchar();
+ if (ch=='q' || ch=='Q') break;
+ for (unsigned i=0; i < saveStates.size(); ++i)
+ viz.report(saveStates[i]);
+ }
+
+ } catch (const std::exception& e) {
+ cout << "EXCEPTION: " << e.what() << "\n";
+ }
+}
diff --git a/Simbody/tests/adhoc/PassThrough.cpp b/Simbody/tests/adhoc/PassThrough.cpp
new file mode 100644
index 0000000..c05a58e
--- /dev/null
+++ b/Simbody/tests/adhoc/PassThrough.cpp
@@ -0,0 +1,411 @@
+
+/**
+This is a stripped down version of the collision playground example to illustrate the
+behavior of the collision detection system and a few geometry dependent odities.
+I have removed the ground plane and collision cliques for the purposes of this
+example.
+
+try the following:
+1)all spheres: collisions are detected forces are drawn but not applied. No collision interaction;
+2)all cubes(size 0.4-0.5): Collisions are detected but notice that the moving cube "cuts" into the stationary one
+ before collision is detected. No forces are generated at this stage. Collisions seem to occur
+ on a spherical boundary within the cube. This is similar to the contact playground example.
+
+*/
+#include "SimTKsimbody.h"
+
+#include <cstdio>
+#include <exception>
+#include <algorithm>
+#include <iostream>
+#include <fstream>
+
+#include <string.h>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+Array_<State> saveEm;
+
+static const Real TimeScale = 0.21;
+static const Real FrameRate = 30;
+static const Real ReportInterval = TimeScale/FrameRate;
+static const Real ForceScale = .25;
+static const Real MomentScale = .5;
+
+
+class ForceArrowGenerator : public DecorationGenerator {
+public:
+ ForceArrowGenerator(const MultibodySystem& system,
+ const CompliantContactSubsystem& complCont)
+ : m_system(system), m_compliant(complCont) {}
+
+ virtual void generateDecorations(const State& state, Array_<DecorativeGeometry>& geometry) {
+ const Vec3 frcColors[] = {Red,Orange,Cyan};
+ const Vec3 momColors[] = {Blue,Green,Purple};
+ m_system.realize(state, Stage::Velocity);
+
+ const int ncont = m_compliant.getNumContactForces(state);
+ for (int i=0; i < ncont; ++i) {
+ const ContactForce& force = m_compliant.getContactForce(state,i);
+ const ContactId id = force.getContactId();
+ const Vec3& frc = force.getForceOnSurface2()[1];
+
+ const Vec3& mom = force.getForceOnSurface2()[0];
+ Real frcMag = frc.norm(), momMag=mom.norm();
+ int frcThickness = 1, momThickness = 1;
+ Real frcScale = ForceScale, momScale = ForceScale;
+ while (frcMag > 10)
+ frcThickness++, frcScale /= 10, frcMag /= 10;
+ while (momMag > 10)
+ momThickness++, momScale /= 10, momMag /= 10;
+ DecorativeLine frcLine(force.getContactPoint(),
+ force.getContactPoint() + frcScale*frc);
+ DecorativeLine momLine(force.getContactPoint(),
+ force.getContactPoint() + momScale*mom);
+ frcLine.setColor(frcColors[id%3]);
+ momLine.setColor(momColors[id%3]);
+ frcLine.setLineThickness(2*frcThickness);
+ momLine.setLineThickness(2*momThickness);
+ geometry.push_back(frcLine);
+ geometry.push_back(momLine);
+
+ ContactPatch patch;
+ const bool found = m_compliant.calcContactPatchDetailsById(state,id,patch);
+ //cout << "patch for id" << id << " found=" << found << endl;
+ //cout << "resultant=" << patch.getContactForce() << endl;
+ //cout << "num details=" << patch.getNumDetails() << endl;
+ for (int i=0; i < patch.getNumDetails(); ++i) {
+ const ContactDetail& detail = patch.getContactDetail(i);
+ const Real peakPressure = detail.getPeakPressure();
+ // Make a black line from the element's contact point in the normal
+ // direction, with length proportional to log(peak pressure)
+ // on that element.
+
+ DecorativeLine normal(detail.getContactPoint(),
+ detail.getContactPoint()+ std::log10(peakPressure)
+ * detail.getContactNormal());
+ normal.setColor(Black);
+ geometry.push_back(normal);
+
+ // Make a red line that extends from the contact
+ // point in the direction of the slip velocity, of length 3*slipvel.
+ DecorativeLine slip(detail.getContactPoint(),
+ detail.getContactPoint()+3*detail.getSlipVelocity());
+ slip.setColor(Red);
+ geometry.push_back(slip);
+ }
+ }
+ }
+private:
+ const MultibodySystem& m_system;
+ const CompliantContactSubsystem& m_compliant;
+};
+
+class MyReporter : public PeriodicEventReporter {
+public:
+ MyReporter(const MultibodySystem& system,
+ const CompliantContactSubsystem& complCont,
+ Real reportInterval)
+ : PeriodicEventReporter(reportInterval), m_system(system),
+ m_compliant(complCont)
+ {}
+
+ ~MyReporter() {}
+
+ void handleEvent(const State& state) const {
+ m_system.realize(state, Stage::Dynamics);
+ /* cout << state.getTime() << ": E = " << m_system.calcEnergy(state)
+ << " Ediss=" << m_compliant.getDissipatedEnergy(state)
+ << " E+Ediss=" << m_system.calcEnergy(state)
+ +m_compliant.getDissipatedEnergy(state)
+ << endl;*/
+ const int ncont = m_compliant.getNumContactForces(state);
+ if(ncont>0)
+ {
+ cout << "Num contacts: " << m_compliant.getNumContactForces(state) << endl;
+ for (int i=0; i < ncont; ++i) {
+ const ContactForce& force = m_compliant.getContactForce(state,i);
+ cout << force;
+ }
+ }
+ saveEm.push_back(state);
+ }
+private:
+ const MultibodySystem& m_system;
+ const CompliantContactSubsystem& m_compliant;
+};
+
+// These are the item numbers for the entries on the Run menu.
+static const int RunMenuId = 3, HelpMenuId = 7;
+static const int GoItem = 1, ReplayItem=2, QuitItem=3;
+
+// This is a periodic event handler that interrupts the simulation on a regular
+// basis to poll the InputSilo for user input. If there has been some, process it.
+// This one does nothing but look for the Run->Quit selection.
+class UserInputHandler : public PeriodicEventHandler {
+public:
+ UserInputHandler(Visualizer::InputSilo& silo, Real interval)
+ : PeriodicEventHandler(interval), m_silo(silo) {}
+
+ virtual void handleEvent(State& state, Real accuracy,
+ bool& shouldTerminate) const
+ {
+ int menuId, item;
+ if (m_silo.takeMenuPick(menuId, item) && menuId==RunMenuId && item==QuitItem)
+ shouldTerminate = true;
+ }
+
+private:
+ Visualizer::InputSilo& m_silo;
+};
+
+int main() {
+ try
+ { // Create the system.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+
+ /// uncoment gravity to get some sort of collision interaction
+ /// for cylinder mesh
+ // Force::UniformGravity gravity(forces, matter,Vec3(0,0.001,0), 2);
+
+ ContactTrackerSubsystem tracker(system);
+ //GeneralContactSubsystem contactsys(system);
+ CompliantContactSubsystem contactForces(system, tracker);
+ contactForces.setTrackDissipatedEnergy(true);
+
+ for(SubsystemIndex i(0); i<system.getNumSubsystems(); ++i)
+ {
+ fprintf(stderr,"subsytem name %d %s\n", (int)i,
+ system.getSubsystem((SubsystemIndex)i).getName().c_str());
+ }
+
+ const Real rad = .4;
+ PolygonalMesh pyramidMesh1,pyramidMesh2;
+
+ /// load cylinder forces drawn, but interaction depends on gravity???
+
+
+ const Real fFac =1; // to turn off friction
+ const Real fDis = .5*0.2; // to turn off dissipation
+ const Real fVis = .1*.1; // to turn off viscous friction
+ const Real fK = 100*1e6; // pascals
+
+ Body::Rigid pendulumBody3(MassProperties(100.0, Vec3(0), 100*Inertia(1)));
+ PolygonalMesh body3contact = PolygonalMesh::createSphereMesh(rad, 2);
+ ContactGeometry::TriangleMesh geo3(body3contact);
+
+ const DecorativeMesh mesh3(geo3.createPolygonalMesh());
+ pendulumBody3.addDecoration(Transform(),
+ DecorativeMesh(mesh3).setOpacity(.2));
+ pendulumBody3.addDecoration(Transform(),
+ DecorativeMesh(mesh3).setColor(Gray)
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ .setOpacity(.1));
+ ContactSurface s1(geo3,
+ ContactMaterial(fK*.1,fDis*.9,fFac*.8,fFac*.7,fVis*10));
+ s1.setThickness(1);
+ s1.setShape(geo3);
+ //ContactGeometry::Sphere geo3(rad);
+ pendulumBody3.addContactSurface(Transform(),s1);
+ /*
+ std::ifstream meshFile1,meshFile2;
+ meshFile1.open("cyl3.obj");
+ pyramidMesh1.loadObjFile(meshFile1); meshFile1.close();
+*/
+ pyramidMesh1 = PolygonalMesh::createSphereMesh(rad, 2);
+ ContactGeometry::TriangleMesh pyramid1(pyramidMesh1);
+
+ DecorativeMesh showPyramid1(pyramid1.createPolygonalMesh());
+ const Real ballMass = 200;
+ Body::Rigid ballBody(MassProperties(ballMass, Vec3(0),
+ ballMass*UnitInertia::sphere(1)));
+
+ ballBody.addDecoration(Transform(),
+ showPyramid1.setColor(Cyan).setOpacity(.2));
+ ballBody.addDecoration(Transform(),
+ showPyramid1.setColor(Gray)
+ .setRepresentation(DecorativeGeometry::DrawWireframe));
+
+ ContactSurface s2(pyramid1,
+ ContactMaterial(fK*.1,fDis*.9,
+ .1*fFac*.8,.1*fFac*.7,fVis*1));
+ s2.setThickness(1);
+ s2.setShape(pyramid1);
+ ballBody.addContactSurface(Transform(),/*ContactSurface(ContactGeometry::Sphere(rad),ContactMaterial(fK*.1,fDis*.9,
+ .1*fFac*.8,.1*fFac*.7,fVis*1))*/ s2/*.joinClique(clique1)*/);
+
+ /* Body::Rigid d(MassProperties(1.0, Vec3(0),Inertia(1)));
+
+ MobilizedBody::Pin dud(matter.Ground(),Transform(),d,Transform());
+*/
+ MobilizedBody::Free ball(matter.Ground(), Transform(Vec3(-2,-2,0)),
+ ballBody, Transform(Vec3(0)));
+
+
+
+ MobilizedBody::Free ball1(matter.Ground(), Transform(Vec3(0,0,0)),
+ ballBody, Transform(Vec3(0)));
+ /*
+ MobilizedBody::Free ball2(matter.Ground(), Transform(Vec3(-4,0,0)),
+ ballBody, Transform(Vec3(0)));
+*/
+
+ MobilizedBody::Free ball3(matter.Ground(), Transform(Vec3(-1,-2,0)),
+ ballBody, Transform(Vec3(0)));
+
+
+ MobilizedBody::Pin pendulum3(matter.Ground(), Transform(Vec3(-2,0,0)),
+ pendulumBody3, Transform(Vec3(0, 2, 0)));
+
+ ball.updBody();
+ ball1.updBody();
+
+ Visualizer viz(system);
+ viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
+ viz.setMode(Visualizer::RealTime);
+ viz.setDesiredBufferLengthInSec(1);
+ viz.setDesiredFrameRate(FrameRate);
+ viz.setGroundHeight(-3);
+ viz.setShowShadows(true);
+ viz.setBackgroundType(Visualizer::SolidColor);
+ Visualizer::InputSilo* silo = new Visualizer::InputSilo();
+ viz.addInputListener(silo);
+ Array_<std::pair<String,int> > runMenuItems;
+ runMenuItems.push_back(std::make_pair("Go", GoItem));
+ runMenuItems.push_back(std::make_pair("Replay", ReplayItem));
+ runMenuItems.push_back(std::make_pair("Quit", QuitItem));
+ viz.addMenu("Run", RunMenuId, runMenuItems);
+
+ Array_<std::pair<String,int> > helpMenuItems;
+ helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1));
+ viz.addMenu("Help", HelpMenuId, helpMenuItems);
+
+ system.addEventReporter(new MyReporter(system,contactForces,ReportInterval));
+ system.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));
+
+ // Check for a Run->Quit menu pick every 1/4 second.
+ system.addEventHandler(new UserInputHandler(*silo, .25));
+ // system.addEventHandler(new TriggeredEventHandler(Stage::Model));
+ // Initialize the system and state.
+
+ system.realizeTopology();
+
+ State state = system.getDefaultState();
+ /*
+ ball.setQToFitTransform(state, Transform(Rotation(Pi/2,XAxis),
+ Vec3(0,-1.8,0)));
+*/
+ //pendulum.setOneQ(state, 0, -Pi/12);
+ pendulum3.setOneQ(state, 0, -Pi/2);
+ pendulum3.setOneU(state, 0, Pi/4);
+ // ball.setOneU(state, 1, 0.1);
+ viz.report(state);
+ matter.updAllParticleVelocities(state);
+ printf("Default state\n");
+ /* cout << "t=" << state.getTime()
+ << " q=" << pendulum.getQAsVector(state) << pendulum2.getQAsVector(state)
+ << " u=" << pendulum.getUAsVector(state) << pendulum2.getUAsVector(state)
+ << endl;
+*/
+ cout << "\nChoose 'Go' from Run menu to simulate:\n";
+ int menuId, item;
+ do { silo->waitForMenuPick(menuId, item);
+ if (menuId != RunMenuId || item != GoItem)
+ cout << "\aDude ... follow instructions!\n";
+ } while (menuId != RunMenuId || item != GoItem);
+
+ // Simulate it.
+
+ // The system as parameterized is very stiff (mostly due to friction)
+ // and thus runs best with CPodes which is extremely stable for
+ // stiff problems. To get reasonable performance out of the explicit
+ // integrators (like the RKs) you'll have to run at a very loose
+ // accuracy like 0.1, or reduce the friction coefficients and
+ // maybe the stiffnesses.
+
+ //ExplicitEulerIntegrator integ(system);
+ CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
+ //RungeKuttaFeldbergIntegrator integ(system);
+ //RungeKuttaMersonIntegrator integ(system);
+ //RungeKutta3Integrator integ(system);
+ //VerletIntegrator integ(system);
+ //integ.setMaximumStepSize(1e-1);
+ //integ.setAllowInterpolation(false);
+ integ.setAccuracy(1e-3); // minimum for CPodes
+ //integ.setAccuracy(.1);
+ TimeStepper ts(system, integ);
+
+
+ ts.initialize(state);
+ double cpuStart = cpuTime();
+ double realStart = realTime();
+
+ ts.stepTo(2000.0);
+
+ const double timeInSec = realTime() - realStart;
+ const int evals = integ.getNumRealizations();
+ /* cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
+ timeInSec << "s elapsed for " << ts.getTime() << "s sim (avg step="
+ << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
+ << (1000*ts.getTime())/evals << "ms/eval\n";
+ cout << " CPU time was " << cpuTime() - cpuStart << "s\n";
+
+ printf("Using Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
+*/
+ viz.dumpStats(std::cout);
+
+ // Add as slider to control playback speed.
+ viz.addSlider("Speed", 1, 0, 4, 1);
+ viz.setMode(Visualizer::PassThrough);
+
+ silo->clear(); // forget earlier input
+ double speed = 1; // will change if slider moves
+ while(true) {
+ cout << "Choose Run/Replay to see that again ...\n";
+
+ int menuId, item;
+ silo->waitForMenuPick(menuId, item);
+
+
+ if (menuId != RunMenuId) {
+ cout << "\aUse the Run menu!\n";
+ continue;
+ }
+
+ if (item == QuitItem)
+ break;
+ if (item != ReplayItem) {
+ cout << "\aHuh? Try again.\n";
+ continue;
+ }
+
+ for (double i=0; i < (int)saveEm.size(); i += speed ) {
+ int slider; Real newValue;
+ if (silo->takeSliderMove(slider,newValue)) {
+ speed = newValue;
+ }
+ viz.report(saveEm[(int)i]);
+ }
+ }
+
+ } catch (const std::exception& e) {
+ std::printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+
+ } catch (...) {
+ std::printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+ return 0;
+}
+
+
diff --git a/Simbody/tests/adhoc/PendulumExample.cpp b/Simbody/tests/adhoc/PendulumExample.cpp
new file mode 100644
index 0000000..9b814c4
--- /dev/null
+++ b/Simbody/tests/adhoc/PendulumExample.cpp
@@ -0,0 +1,179 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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"
+
+#include <cmath>
+#include <cstdio>
+#include <exception>
+#include <vector>
+
+using namespace std;
+using namespace SimTK;
+
+static const Real Deg2Rad = (Real)SimTK_DEGREE_TO_RADIAN,
+ Rad2Deg = (Real)SimTK_RADIAN_TO_DEGREE;
+
+static const Transform GroundFrame;
+
+static const Real m = 5; // kg
+static const Real g = 9.8; // meters/s^2; apply in �y direction
+static const Real d = 0.5; // meters
+
+int main(int argc, char** argv) {
+try { // If anything goes wrong, an exception will be thrown.
+
+ MultibodySystem mbs; mbs.setUseUniformBackground(true);
+ GeneralForceSubsystem forces(mbs);
+ SimbodyMatterSubsystem pend(mbs);
+ DecorationSubsystem viz(mbs);
+ Force::UniformGravity gravity(forces, pend, Vec3(0, -g, 0));
+
+ MobilizedBody::Ball connector(pend.Ground(),
+ Transform(1*Vec3(0, 0, 0)),
+ MassProperties(1, Vec3(0,0,0), Inertia(10,20,30)),
+ Transform(1*Vec3(0, .5, 0)));
+
+ connector.setDefaultRadius(0.05); // for the artwork
+
+ //connector.setDefaultRotation( Rotation(Pi/4, Vec3(0,0,1) );
+
+ const Real m1 = 5;
+ const Real m2 = 1;
+ const Real radiusRatio = std::pow(m2/m1, 1./3.);
+ const Vec3 weight1Location(0, 0, -d/2); // in local frame of swinging body
+ const Vec3 weight2Location(0, 0, d/2); // in local frame of swinging body
+ const Vec3 COM = (m1*weight1Location+m2*weight2Location)/(m1+m2);
+
+ const MassProperties swingerMassProps
+ (m1+m2, COM, 1*Inertia(1,1,1) + m1*UnitInertia::pointMassAt(weight1Location)
+ + m2*UnitInertia::pointMassAt(weight2Location));
+ MobilizedBody::Screw swinger(connector,
+ Transform( Rotation( 0*.7, Vec3(9,8,7) ),
+ 1*Vec3(0,-.5,0)),
+ swingerMassProps,
+ Transform( Rotation(0*1.3, Vec3(0,0,1) ),
+ COM+0*Vec3(0,0,3)), // inboard joint location
+ 0.3); // pitch
+
+ // Add a blue sphere around the weight.
+ viz.addBodyFixedDecoration(swinger, weight1Location,
+ DecorativeSphere(d/8).setColor(Blue).setOpacity(.2));
+ viz.addBodyFixedDecoration(swinger, weight2Location,
+ DecorativeSphere(radiusRatio*d/8).setColor(Green).setOpacity(.2));
+ viz.addRubberBandLine(GroundIndex, Vec3(0),
+ swinger, Vec3(0),
+ DecorativeLine().setColor(Blue).setLineThickness(10)
+ .setRepresentation(DecorativeGeometry::DrawPoints));
+
+ //forces.addMobilityConstantForce(swinger, 0, 10);
+ Force::ConstantTorque(forces, swinger, Vec3(0,0,10));
+ //forces.addConstantForce(swinger, Vec3(0), Vec3(0,10,0));
+ //forces.addConstantForce(swinger, Vec3(0,0,0), Vec3(10,10,0)); // z should do nothing
+ //forces.addMobilityConstantForce(swinger, 1, 10);
+ // forces.addMobilityConstantForce(swinger, 2, 60-1.2);
+
+ State s = mbs.realizeTopology(); // define appropriate states for this System
+
+
+ pend.setUseEulerAngles(s, true);
+ mbs.realizeModel(s);
+
+ mbs.realize(s);
+
+ // Create a study using the Runge Kutta Merson integrator
+ RungeKuttaMersonIntegrator myStudy(mbs);
+ myStudy.setAccuracy(1e-6);
+
+ // This will pick up decorative geometry from
+ // each subsystem that generates any, including of course the
+ // DecorationSubsystem, but not limited to it.
+ Visualizer display(mbs);
+
+
+ const Real expectedPeriod = 2*Pi*std::sqrt(d/g);
+ printf("Expected period: %g seconds\n", expectedPeriod);
+
+ const Real dt = 1./30; // output intervals
+ const Real finalTime = 1*expectedPeriod;
+
+ for (Real startAngle = 10; startAngle <= 90; startAngle += 10) {
+ s = mbs.getDefaultState();
+ connector.setQToFitRotation(s, Rotation(Pi/4, Vec3(1,1,1)) );
+
+ printf("time theta energy *************\n");
+
+ swinger.setQToFitTransform(s, Transform( Rotation( BodyRotationSequence, 0*Pi/2, XAxis, 0*Pi/2, YAxis ), Vec3(0,0,0) ));
+ swinger.setUToFitVelocity(s, SpatialVec(0*Vec3(1.1,1.2,1.3),Vec3(0,0,-1)));
+
+ s.updTime() = 0;
+ myStudy.initialize(s);
+
+ cout << "MassProperties in B=" << swinger.expressMassPropertiesInAnotherBodyFrame(myStudy.getState(),swinger);
+ cout << "MassProperties in G=" << swinger.expressMassPropertiesInGroundFrame(myStudy.getState());
+ cout << "Spatial Inertia =" << swinger.calcBodySpatialInertiaMatrixInGround(myStudy.getState());
+
+ int stepNum = 0;
+ for (;;) {
+ // Should check for errors and other interesting status returns.
+ myStudy.stepTo(myStudy.getTime() + dt);
+ const State& s = myStudy.getState(); // s is now the integrator's internal state
+
+ if ((stepNum++%10)==0) {
+ // This is so we can calculate potential energy (although logically
+ // one should be able to do that at Stage::Position).
+ mbs.realize(s, Stage::Dynamics);
+
+ cout << s.getTime() << ": E=" << mbs.calcEnergy(s)
+ << " connector q=" << connector.getQ(s)
+ << ": swinger q=" << swinger.getQ(s) << endl;
+
+ // This is so we can look at the UDots.
+ mbs.realize(s, Stage::Acceleration);
+
+ cout << "q =" << pend.getQ(s) << endl;
+ cout << "u =" << pend.getU(s) << endl;
+ cout << "ud=" << pend.getUDot(s) << endl;
+
+ cout << "Connector V=" << connector.getMobilizerVelocity(s) << endl;
+ cout << "Swinger V=" << swinger.getMobilizerVelocity(s) << endl;
+
+ const Rotation& R_MbM = swinger.getMobilizerTransform(s).R();
+ Vec4 aaMb = R_MbM.convertRotationToAngleAxis();
+ cout << "angle=" << aaMb[0] << endl;
+ cout << "axisMb=" << aaMb.drop1(0) << endl;
+ cout << "axisMb=" << ~R_MbM*aaMb.drop1(0) << endl;
+ }
+
+ display.report(s);
+ if (s.getTime() >= finalTime)
+ break;
+ }
+ }
+}
+catch (const exception& e) {
+ printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+}
+}
+
diff --git a/Simbody/tests/adhoc/PrescribedMotionPlayground.cpp b/Simbody/tests/adhoc/PrescribedMotionPlayground.cpp
new file mode 100644
index 0000000..639930b
--- /dev/null
+++ b/Simbody/tests/adhoc/PrescribedMotionPlayground.cpp
@@ -0,0 +1,387 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Adhoc main program for playing with prescribed motion.
+ */
+
+#include "SimTKsimbody.h"
+#include "SimTKcommon/Testing.h"
+
+#include <cstdio>
+#include <exception>
+#include <iostream>
+using std::cout; using std::endl;
+using std::clog; using std::cerr;
+
+using namespace SimTK;
+
+class MyReporter : public PeriodicEventReporter {
+public:
+ MyReporter(const MultibodySystem& system,
+ const Measure& power,
+ const Measure& work,
+ Real reportInterval)
+ : PeriodicEventReporter(reportInterval),
+ m_system(system), m_power(power), m_work(work)
+ {}
+
+ ~MyReporter() {}
+
+ void handleEvent(const State& state) const {
+ cout << state.getTime() << " " << m_system.calcEnergy(state);
+ cout << " " << m_power.getValue(state) << " " << m_work.getValue(state)
+ << " " <<
+ m_system.calcEnergy(state) - m_work.getValue(state);
+ //for (int i=0; i < state.getNQ(); ++i)
+ // cout << " " << state.getQ()[i];
+ cout << "\n";
+ const SimbodyMatterSubsystem& matter = m_system.getMatterSubsystem();
+ cout << "qperr=" << matter.calcMotionErrors(state,Stage::Position) << "\n";
+ cout << "uperr=" << matter.calcMotionErrors(state,Stage::Velocity) << "\n";
+ cout << "qerr=" << state.getQErr() << "\n";
+ cout << "uerr=" << state.getUErr() << "\n";
+ }
+private:
+ const MultibodySystem& m_system;
+ const Measure m_power, m_work;
+};
+
+
+/* This Measure returns the instantaneous power being generated by the
+constraints and motions. */
+template <class T>
+class PowerMeasure : public Measure_<T> {
+public:
+ SimTK_MEASURE_HANDLE_PREAMBLE(PowerMeasure, Measure_<T>);
+
+ PowerMeasure(Subsystem& sub, const SimbodyMatterSubsystem& matter)
+ : Measure_<T>(sub, new Implementation(matter), AbstractMeasure::SetHandle()) {}
+ SimTK_MEASURE_HANDLE_POSTSCRIPT(PowerMeasure, Measure_<T>);
+};
+
+
+template <class T>
+class PowerMeasure<T>::Implementation : public Measure_<T>::Implementation {
+public:
+ Implementation(const SimbodyMatterSubsystem& matter)
+ : Measure_<T>::Implementation(1), m_matter(matter) {}
+
+ // Default copy constructor, destructor, copy assignment are fine.
+
+ // Implementations of virtual methods.
+ Implementation* cloneVirtual() const {return new Implementation(*this);}
+ int getNumTimeDerivativesVirtual() const {return 0;}
+ Stage getDependsOnStageVirtual(int order) const
+ { return Stage::Acceleration; }
+
+ void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
+ {
+ SimTK_ASSERT1_ALWAYS(derivOrder==0,
+ "PowerMeasure::Implementation::calcCachedValueVirtual():"
+ " derivOrder %d seen but only 0 allowed.", derivOrder);
+
+ value = m_matter.calcMotionPower(s) + m_matter.calcConstraintPower(s);
+ }
+private:
+ const SimbodyMatterSubsystem& m_matter;
+};
+
+const Real RodLength = 1.1;
+int main() {
+ try
+ { // Create the system.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+
+ PowerMeasure<Real> powMeas(matter, matter);
+ Measure::Zero zeroMeas(matter);
+ Measure::Integrate workMeas(matter, powMeas, zeroMeas);
+
+ Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
+ pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1));
+
+
+ // Prescribed system.
+ MobilizedBody::Pin pendulum(matter.Ground(), Transform(Vec3(0)),
+ pendulumBody, Transform(Vec3(0, 1, 0)));
+ Motion::Sinusoid(pendulum, Motion::Position, Pi/8, 2*Pi, Pi/4); // amp, rate, phase
+ MobilizedBody::Pin pendulum2(pendulum, Transform(Vec3(0)),
+ pendulumBody, Transform(Vec3(0, 1, 0)));
+ MobilizedBody::Pin pendulum3(pendulum2, Vec3(0),
+ pendulumBody, Vec3(0,.5,0));
+ Motion::Steady(pendulum3, 4*Pi); // rate
+ Force::MobilityLinearSpring(forces, pendulum2, MobilizerUIndex(0),
+ 100, 0*(Pi/180));
+
+ // Identical constrained system.
+ MobilizedBody::Pin cpendulum(matter.Ground(), Transform(Vec3(2,0,0)),
+ pendulumBody, Transform(Vec3(0, 1, 0)));
+ Constraint::PrescribedMotion(matter,
+ new Function::Sinusoid(Pi/8,2*Pi,Pi/4), //amp,rate,phase
+ cpendulum, MobilizerQIndex(0));
+ MobilizedBody::Pin cpendulum2(cpendulum, Transform(Vec3(0)),
+ pendulumBody, Transform(Vec3(0, 1, 0)));
+ MobilizedBody::Pin cpendulum3(cpendulum2, Vec3(0),
+ pendulumBody, Vec3(0,.5,0));
+ Constraint::ConstantSpeed(cpendulum3, 4*Pi);
+ Force::MobilityLinearSpring(forces, cpendulum2, MobilizerUIndex(0),
+ 100, 0*(Pi/180));
+
+ Constraint::Rod(pendulum3, cpendulum3, RodLength);
+
+ // Identical mixed system 1.
+ MobilizedBody::Pin m1pendulum(matter.Ground(), Transform(Vec3(4,0,0)),
+ pendulumBody, Transform(Vec3(0, 1, 0)));
+ Motion::Sinusoid(m1pendulum, Motion::Position, Pi/8, 2*Pi, Pi/4); // amp, rate, phase
+ MobilizedBody::Pin m1pendulum2(m1pendulum, Transform(Vec3(0)),
+ pendulumBody, Transform(Vec3(0, 1, 0)));
+ MobilizedBody::Pin m1pendulum3(m1pendulum2, Vec3(0),
+ pendulumBody, Vec3(0,.5,0));
+ Constraint::ConstantSpeed(m1pendulum3, 4*Pi);
+ Force::MobilityLinearSpring(forces, m1pendulum2, MobilizerUIndex(0),
+ 100, 0*(Pi/180));
+
+
+ // Identical mixed system 2.
+ MobilizedBody::Pin m2pendulum(matter.Ground(), Transform(Vec3(6,0,0)),
+ pendulumBody, Transform(Vec3(0, 1, 0)));
+ Constraint::PrescribedMotion(matter,
+ new Function::Sinusoid(Pi/8,2*Pi,Pi/4), //amp,rate,phase
+ m2pendulum, MobilizerQIndex(0));
+ MobilizedBody::Pin m2pendulum2(m2pendulum, Transform(Vec3(0)),
+ pendulumBody, Transform(Vec3(0, 1, 0)));
+ MobilizedBody::Pin m2pendulum3(m2pendulum2, Vec3(0),
+ pendulumBody, Vec3(0,.5,0));
+ Motion::Steady(m2pendulum3, 4*Pi); // rat
+ Force::MobilityLinearSpring(forces, m2pendulum2, MobilizerUIndex(0),
+ 100, 0*(Pi/180));
+
+ Constraint::Rod(m1pendulum3, m2pendulum3, RodLength);
+
+
+ Visualizer viz(system);
+ system.addEventReporter(new Visualizer::Reporter(viz, 0.01));
+ system.addEventReporter(new MyReporter(system, powMeas, workMeas, 0.01));
+
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+ system.realize(state, Stage::Instance);
+ const int nq = state.getNQ();
+ const int nu = state.getNU();
+ const int m = state.getNMultipliers();
+
+ viz.report(state);
+ system.realize(state, Stage::Velocity);
+ clog << "Default state -- hit ENTER\n";
+ clog << "t=" << state.getTime()
+ << "\nq=" << state.getQ()
+ << "\nu=" << state.getU()
+ << "\nqerr=" << state.getQErr()
+ << "\nuerr=" << state.getUErr()
+ << endl;
+ char c=getchar();
+
+ state.setTime(0);
+ system.realize(state, Stage::Time);
+
+
+ clog << "After realize(Time) motion qerr="
+ << matter.calcMotionErrors(state, Stage::Position) << "\n";
+ system.prescribeQ(state);
+
+ clog << "After prescribe() motion qerr="
+ << matter.calcMotionErrors(state, Stage::Position) << "\n";
+
+ //if (matter.prescribe(state, Stage::Position)) clog << "Some PresQ\n";
+ //else clog << "NO PresQ\n";
+ //clog << "after prescribe q=" << state.getQ() << "\n";
+ system.realize(state, Stage::Position);
+
+ system.projectQ(state, 1e-10);
+ //Assembler asmb(system);
+ //asmb.setAccuracy(1e-10).assemble(state);
+
+ viz.report(state);
+ clog << "After prescribe(Position) & project -- hit ENTER\n";
+ clog << "t=" << state.getTime()
+ << "\nq=" << state.getQ()
+ << "\nu=" << state.getU()
+ << "\nqerr=" << state.getQErr()
+ << endl;
+
+ const int nfq = matter.getFreeQIndex(state).size();
+ clog << "freeQ: " << matter.getFreeQIndex(state) << "\n";
+
+ Vector q = state.getQ(), packedQ(nfq), unpackedQ(nq);
+ clog << "allQ =" << q << "\n";
+ matter.packFreeQ(state, q, packedQ);
+ clog << "packedFreeQ =" << packedQ << "\n";
+ matter.unpackFreeQ(state, packedQ, unpackedQ);
+ clog << "unpackedFreeQ=" << unpackedQ << "\n";
+
+ c=getchar();
+
+ clog << "After realize(Position) motion uerr="
+ << matter.calcMotionErrors(state, Stage::Velocity) << "\n";
+ system.prescribeU(state);
+ clog << "After prescribe() motion uerr="
+ << matter.calcMotionErrors(state, Stage::Velocity) << "\n";
+
+ system.realize(state, Stage::Velocity);
+ system.projectU(state, 1e-10);
+ viz.report(state);
+ clog << "After prescribe(Velocity) & project -- hit ENTER\n";
+ clog << "t=" << state.getTime()
+ << "\nq=" << state.getQ()
+ << "\nu=" << state.getU()
+ << "\nuerr=" << state.getUErr()
+ << endl;
+
+ const int nfu = matter.getFreeUIndex(state).size();
+ clog << "freeU: " << matter.getFreeUIndex(state) << "\n";
+
+ Vector u = state.getU(), packedU(nfu), unpackedU(nu);
+ clog << "allU =" << u << "\n";
+ matter.packFreeU(state, u, packedU);
+ clog << "packedFreeU =" << packedU << "\n";
+ matter.unpackFreeU(state, packedU, unpackedU);
+ clog << "unpackedFreeU=" << unpackedU << "\n";
+
+
+ c=getchar();
+
+ system.realize(state, Stage::Acceleration);
+ clog << "After realize(Acceleration) motion udoterr="
+ << matter.calcMotionErrors(state, Stage::Acceleration) << "\n";
+
+ clog << "After realize(Acceleration) -- hit ENTER\n";
+ clog << "t=" << state.getTime()
+ << "\nq=" << state.getQ()
+ << "\nu=" << state.getU()
+ << "\nudot=" << state.getUDot()
+ << "\ntau=" << pendulum.getTauAsVector(state) << pendulum2.getTauAsVector(state) << pendulum3.getTauAsVector(state)
+ << endl;
+
+ Vector_<SpatialVec> reactionForces, reactionForcesFreebody;
+ matter.calcMobilizerReactionForces(state, reactionForces);
+ matter.calcMobilizerReactionForcesUsingFreebodyMethod(state, reactionForcesFreebody);
+
+ clog << "reactions PA+z: " << reactionForces << endl;
+ clog << "react freebody: " << reactionForcesFreebody << endl;
+ clog << "diff=" << reactionForces-reactionForcesFreebody << "\n";
+ SimTK_TEST_EQ_SIZE(reactionForces, reactionForcesFreebody, nu);
+
+ clog << "tau=" << matter.getMotionMultipliers(state) << "\n";
+ Vector motFrcs;
+ matter.findMotionForces(state, motFrcs);
+ clog << "motion frc=" << motFrcs << "\n";
+ clog << "gen speeds=" << matter.getU(state) << "\n";
+ clog << "motion pwr=" << matter.calcMotionPower(state) << "\n";
+
+ clog << "lambda=" << matter.getConstraintMultipliers(state) << "\n";
+ Vector_<SpatialVec> consBodyFrc;
+ Vector consMobFrc;
+ matter.findConstraintForces(state, consBodyFrc, consMobFrc);
+ clog << "cons bfrc=" << consBodyFrc << "\n";
+ clog << "cons mfrc=" << consMobFrc << "\n";
+ clog << "cons pwr=" << matter.calcConstraintPower(state) << "\n";
+
+
+ clog << "freeUDot: " << matter.getFreeUDotIndex(state) << "\n";
+ clog << "knownUDot: " << matter.getKnownUDotIndex(state) << "\n";
+
+ Matrix GMInvGt_r;
+ matter.calcProjectedMInv(state, GMInvGt_r);
+
+ Matrix G, M, Minv_r;
+ matter.calcG(state, G);
+ matter.calcM(state, M);
+ matter.calcMInv(state, Minv_r);
+ Matrix Minv = M.invert();
+
+ const Array_<UIndex>& freeUDot = matter.getFreeUDotIndex(state);
+ const int nfudot = freeUDot.size();
+ Matrix Gr(m,nfudot), Mr(nfudot,nfudot);
+
+ for (int i=0; i < nfu; ++i) {
+ Gr(i) = G(freeUDot[i]);
+ matter.packFreeU(state, M(freeUDot[i]), Mr(i));
+ }
+
+ cout << std::fixed;
+ cout << "G=" << G;
+ cout << "Gr=" << Gr;
+ cout << "M=" << M;
+ cout << "Mr=" << Mr;
+
+ cout << "inv(M) =" << Minv;
+ cout << "Minv_r =" << Minv_r;
+ cout << "inv(Mr)=" << Mr.invert();
+
+ clog << "GMInvGt_r =" << GMInvGt_r;
+ clog << "GMInv_rGt =" << G*Minv_r*~G;
+ clog << "Gr inv(Mr) ~Gr=" << Gr*Mr.invert()*~Gr;
+ clog << "GMInvGt =" << G*Minv*~G;
+
+
+ c=getchar();
+
+ //pendulum.setOneU(state, 0, 1.0);
+
+
+ // Simulate it.
+
+
+ //ExplicitEulerIntegrator integ(system);
+ //CPodesIntegrator integ(system);
+ //RungeKutta3Integrator integ(system);
+ //VerletIntegrator integ(system);
+ RungeKuttaMersonIntegrator integ(system);
+ //RungeKuttaFeldbergIntegrator integ(system);
+ //integ.setAllowInterpolation(false);
+ //integ.setProjectInterpolatedStates(false);
+ //integ.setMinimumStepSize(1e-1);
+ integ.setAccuracy(1e-2);
+ //integ.setConstraintTolerance(1e-3);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(10.0);
+
+ } catch (const std::exception& e) {
+ cerr << "EXCEPTION THROWN: " << e.what() << "\n";
+ exit(1);
+
+ } catch (...) {
+ cerr << "UNKNOWN EXCEPTION THROWN\n";
+ exit(1);
+ }
+
+ return 0;
+}
diff --git a/Simbody/tests/adhoc/RadusDrifter.cpp b/Simbody/tests/adhoc/RadusDrifter.cpp
new file mode 100644
index 0000000..f23000a
--- /dev/null
+++ b/Simbody/tests/adhoc/RadusDrifter.cpp
@@ -0,0 +1,249 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This is a simple demo of the need for constraint stabilization.
+ */
+
+#include "SimTKsimbody.h"
+
+#include <cmath>
+#include <cstdio>
+#include <exception>
+#include <vector>
+
+using namespace std;
+using namespace SimTK;
+
+static const Real Deg2Rad = (Real)SimTK_DEGREE_TO_RADIAN,
+ Rad2Deg = (Real)SimTK_RADIAN_TO_DEGREE;
+
+static const Transform GroundFrame;
+
+static const Real m = 1; // kg
+static const Real g = 9.8; // meters/s^2; apply in �y direction
+static const Real d = 0.5; // meters
+
+
+void ff(Vector& v) {
+ v = 23.;
+}
+
+int main(int argc, char** argv) {
+ try { // If anything goes wrong, an exception will be thrown.
+
+ // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
+ MultibodySystem mbs; mbs.setUseUniformBackground(true);
+
+ SimbodyMatterSubsystem crankRocker(mbs);
+ GeneralForceSubsystem forces(mbs);
+ DecorationSubsystem viz(mbs);
+ Force::UniformGravity gravity(forces, crankRocker, Vec3(0, -g, 0));
+
+ // ADD BODIES AND THEIR MOBILIZERS
+ Body::Rigid crankBody = Body::Rigid(MassProperties(.1, Vec3(0), 0.1*UnitInertia::brick(1,3,.5)));
+ crankBody.addDecoration(DecorativeEllipsoid(0.1*Vec3(1,3,.4))
+ .setResolution(10)
+ .setOpacity(.2));
+ Body::Rigid sliderBody = Body::Rigid(MassProperties(.2, Vec3(0), 0.2*UnitInertia::brick(1,5,.5)));
+ sliderBody.addDecoration(DecorativeEllipsoid(0.2*Vec3(1,5,.4))
+ .setColor(Blue)
+ .setResolution(10)
+ .setOpacity(.2));
+ Body::Rigid longBar = Body::Rigid(MassProperties(0.01, Vec3(0), 0.01*UnitInertia::cylinderAlongX(.1, 5)));
+ longBar.addDecoration(Rotation(Pi/2,ZAxis), DecorativeCylinder(.01, 1));
+
+ MobilizedBody::Pin
+ crank(crankRocker.Ground(), Transform(), crankBody, Transform(Vec3(0, .15, 0)));
+ MobilizedBody::Pin
+ slider(crankRocker.Ground(), Transform(Vec3(2.5,0,0)), sliderBody, Transform(Vec3(0, 1, 0)));
+
+ MobilizedBody::Universal
+ rightConn(crank, Transform(Rotation(-Pi/2,YAxis),Vec3(0,-.3,0)),
+ longBar, Transform(Rotation(-Pi/2,YAxis),Vec3(-1,0,0)));
+
+ Constraint::Ball ball(slider, Vec3(0,-1, 0), rightConn, Vec3(1,0,0));
+ ball.setDefaultRadius(0.01);
+
+ //Constraint::Rod rodl(leftConn, Vec3(0), rightConn, Vec3(-2.5,0,0), 2.5);
+ //Constraint::Rod rodr(leftConn, Vec3(2.5,-1,0), rightConn, Vec3(-1.5,0,0), std::sqrt(2.));
+ //Constraint::Rod rodo(leftConn, Vec3(1.5,0,0), rightConn, Vec3(-2.5,1,0), std::sqrt(2.));
+ //Constraint::Rod rodl(leftConn, Vec3(-2.5,0,0), rightConn, Vec3(-2.5,0,0), 5);
+ //Constraint::Rod rodr(leftConn, Vec3(2.5,0,0), rightConn, Vec3(2.5,0,0), 5);
+ //Constraint::Rod rodo(leftConn, Vec3(2.5,-1,0), rightConn, Vec3(-2.5,1,0), 2);
+
+ // Add visualization line (orange=spring, black=constraint)
+ //viz.addRubberBandLine(crank, Vec3(0,-3,0),
+ // slider, Vec3(0,-5,0),
+ // DecorativeLine().setColor(Black).setLineThickness(4));
+
+ //forces.addMobilityConstantForce(leftPendulum, 0, 20);
+ //forces.addCustomForce(ShermsForce(leftPendulum,rightPendulum));
+ //forces.addGlobalEnergyDrain(1);
+
+ Force::MobilityConstantForce(forces, crank, 0, 1);
+ Force::MobilityLinearDamper(forces, crank, 0, 1.0);
+
+
+ //Motion::Linear(crank, Vec3(a,b,c)); // crank(t)=at^2+bt+c
+ //Motion::Linear lmot(rightConn, Vec3(a,b,c)); // both axes follow
+ //lmot.setAxis(1, Vec3(d,e,f));
+ //Motion::Orientation(someBall, orientFuncOfT);
+ //someBall.prescribeOrientation(orientFunc);
+ //Motion::Relax(crank); // acc=vel=0, pos determined by some default relaxation solver
+
+ // Like this, or should this be an Instance-stage mode of every mobilizer?
+ //Motion::Lock(crank); // acc=vel=0; pos is discrete or fast
+ //Motion::Steady(crank, Vec3(1,2,3)); // acc=0; vel=constant; pos integrated
+ //Motion::Steady crankRate(crank, 1); // acc=0; vel=constant, same for each axis; pos integrated
+ // or ...
+ //crank.lock(state);
+ //crank.setMotionType(state, Regular|Discrete|Fast|Prescribed, stage);
+
+ // need a way to register a mobilizer with a particular relaxation solver,
+ // switch between dynamic, continuous relaxed, end-of-step relaxed, discrete.
+ // what about a "local" (explicit) relaxation, like q=(q1+q2)/2 ?
+
+
+ State s = mbs.realizeTopology(); // returns a reference to the the default state
+ mbs.realizeModel(s); // define appropriate states for this System
+
+ //crankRate.setRate(s, 3);
+ crank.setAngle(s, 5); //q
+ crank.setRate(s, 3); //u
+
+ Visualizer display(mbs);
+
+ mbs.realize(s, Stage::Position);
+ display.report(s);
+ cout << "q=" << s.getQ() << endl;
+ cout << "qErr=" << s.getQErr() << endl;
+
+ crank.setAngle(s, -30*Deg2Rad);
+ //crank.setQToFitRotation (s, Rotation::aboutZ(-.9*Pi/2));
+
+
+ //TODO
+ //rightPendulum.setUToFitLinearVelocity(s, Vec3(1.1,0,1.2));
+
+ //crank.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
+
+ mbs.realize(s, Stage::Velocity);
+ display.report(s);
+
+ cout << "q=" << s.getQ() << endl;
+ cout << "qErr=" << s.getQErr() << endl;
+
+
+ // These are the SimTK Simmath integrators:
+ RungeKuttaMersonIntegrator myStudy(mbs);
+ //CPodesIntegrator myStudy(mbs, CPodes::BDF, CPodes::Newton);
+
+
+ //myStudy.setMaximumStepSize(0.001);
+ myStudy.setAccuracy(1e-3);
+ //myStudy.setProjectEveryStep(true);
+ //myStudy.setAllowInterpolation(false);
+ //myStudy.setMaximumStepSize(.1);
+
+ const Real dt = 1./30; // output intervals
+ const Real finalTime = 10;
+
+ myStudy.setFinalTime(finalTime);
+
+ cout << "Hit ENTER in console to continue ...\n";
+ getchar();
+ display.report(s);
+
+ cout << "Hit ENTER in console to continue ...\n";
+ getchar();
+
+ // Peforms assembly if constraints are violated.
+ myStudy.initialize(s);
+ myStudy.setProjectEveryStep(false);
+ myStudy.setConstraintTolerance(.001);
+ myStudy.initialize(myStudy.getState());
+
+ cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
+ cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
+ cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
+ cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
+ cout << "U WEIGHTS=" << s.getUWeights() << endl;
+ cout << "Z WEIGHTS=" << s.getZWeights() << endl;
+ cout << "1/QTOLS=" << s.getQErrWeights() << endl;
+ cout << "1/UTOLS=" << s.getUErrWeights() << endl;
+
+ {
+ const State& s = myStudy.getState();
+ display.report(s);
+ cout << "q=" << s.getQ() << endl;
+ cout << "qErr=" << s.getQErr() << endl;
+ }
+
+
+ Integrator::SuccessfulStepStatus status;
+ int nextReport = 0;
+ while ((status=myStudy.stepTo(nextReport*dt, Infinity))
+ != Integrator::EndOfSimulation)
+ {
+ const State& s = myStudy.getState();
+ mbs.realize(s);
+ const Real crankAngle = crank.getBodyRotation(s).convertRotationToAngleAxis()[0] * Rad2Deg;
+ printf("%5g %10.4g E=%10.8g h%3d=%g %s%s\n", s.getTime(),
+ crankAngle,
+ mbs.calcEnergy(s), myStudy.getNumStepsTaken(),
+ myStudy.getPreviousStepSizeTaken(),
+ Integrator::getSuccessfulStepStatusString(status).c_str(),
+ myStudy.isStateInterpolated()?" (INTERP)":"");
+ printf(" qerr=%10.8g uerr=%10.8g uderr=%10.8g\n",
+ crankRocker.getQErr(s).normRMS(),
+ crankRocker.getUErr(s).normRMS(),
+ crankRocker.getUDotErr(s).normRMS());
+
+ display.report(s);
+
+
+ if (status == Integrator::ReachedReportTime)
+ ++nextReport;
+ }
+
+ for (int i=0; i<100; ++i)
+ display.report(myStudy.getState());
+
+ printf("Using Integrator %s:\n", myStudy.getMethodName());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());
+
+ }
+ catch (const exception& e) {
+ printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+ }
+ catch (...) {
+ printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+}
+
diff --git a/Simbody/tests/adhoc/RattleBack_ReverseEllipsoid.cpp b/Simbody/tests/adhoc/RattleBack_ReverseEllipsoid.cpp
new file mode 100644
index 0000000..56bc323
--- /dev/null
+++ b/Simbody/tests/adhoc/RattleBack_ReverseEllipsoid.cpp
@@ -0,0 +1,183 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) - Rattleback using ellipsoid mobilizer *
+ * -------------------------------------------------------------------------- *
+ * 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) 2009-13 Stanford University and the Authors. *
+ * Authors: Ajay Seth *
+ * Contributors: Michael Sherman *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+
+/////// TODO TODO TODO ////////
+// This test is currently broken because it is using the wrong kind of
+// "no slip" constraint. It should be using an ellipsoid-on-halfplane
+// rolling constraint but we don't have one yet.
+// Thomas Uchida says he'll write one ...
+/////// TODO TODO TODO ////////
+
+#define USE_BAD_CONSTRAINTS // won't conserve energy if these are enabled
+
+/* This program attempts to implement a rattleback (which has an ellipsoid
+shape) using Simbody's Ellipsoid mobilizer (3 rotational dofs) rather than
+contact constraints. A massless base plate is used to provide two sliding
+dofs along the ground plane, for a total of 5 dofs (can't lift off). For
+rolling, there must be 2 dofs removed by "no slip" constraint equations.
+By construction the contact point between the ellipsoid and ground occurs at
+a fixed point in the base plate frame; you can see that in the animation. */
+
+#include "Simbody.h"
+#include <iostream>
+
+using namespace SimTK;
+
+// A periodic reporter to dump out the energy and momentum.
+// The total energy should be conserved throughout the simulation.
+class EnergyReporter : public PeriodicEventReporter {
+public:
+ EnergyReporter(const MultibodySystem& system,
+ const MobilizedBody& body, Real interval)
+ : PeriodicEventReporter(interval), system(system), body(body) {}
+
+ void handleEvent(const State& state) const {
+ system.realize(state, Stage::Dynamics);
+ Real energy = system.calcEnergy(state);
+ SpatialVec momentum = system.getMatterSubsystem()
+ .calcSystemMomentumAboutGroundOrigin(state);
+ std::cout << state.getTime() << " \tEnergy = " << energy
+ << " \tAngMom: " << momentum[0].norm()
+ << " \tLinMom: " << momentum[1].norm() << std::endl;
+ }
+private:
+ const MultibodySystem& system;
+ const MobilizedBody& body;
+};
+
+
+//==============================================================================
+// MAIN
+//==============================================================================
+int main() {
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ matter.setShowDefaultGeometry(false);
+ GeneralForceSubsystem forces(system);
+ Force::Gravity gravity(forces, matter, -YAxis, 9.80665);
+ Visualizer viz(system);
+
+ MobilizedBody& Ground = matter.updGround(); // short name for Ground
+
+ // Calculate the mass properties for a half-ellipsoid. a1,b1,c1 are the
+ // radii (semi-axis lengths) of the full ellipsoid in x,y,z resp.
+ // Body origin is still center of full ellipsoid (0,0,0) but the COM moves
+ // lower in y which affects the xx and zz inertias. Note that Simbody
+ // requires the inertias to be given about the body origin, *not* COM.
+ // Unit inertia about the body origin is the same as for a full ellipsoid,
+ // but will be weighted by half as much mass.
+ // TODO: is this right?
+ Real m1 = 1.0, a1 = 0.25, b1 = 0.083333333333333, c1 = 0.083333333333333;
+ Real comShiftY = (3./8.)*b1; // Because it's a half ellipsoid.
+ Body::Rigid halfEllipsoid(MassProperties(m1, Vec3(0, -comShiftY, 0),
+ UnitInertia::ellipsoid(Vec3(a1,b1,c1))));
+ // Add some artwork -- don't have a half-ellipsoid unfortunately.
+ halfEllipsoid.addDecoration(Transform(),
+ DecorativeEllipsoid(Vec3(a1,b1,c1)).setColor(Red).setResolution(10));
+
+ // Now define a rectangular solid that we'll weld to the rattleback to
+ // give it asymmetrical mass properties.
+ Real m2 = 2.0, a2 = 2.0*a1, b2 = 0.02, c2 = 0.05;
+ const Vec3 barHalfDims = Vec3(a2,b2,c2)/2;
+ Body::Rigid barBody(MassProperties(m2, Vec3(0),
+ UnitInertia::brick(barHalfDims)));
+ barBody.addDecoration(Transform(), DecorativeBrick(barHalfDims)
+ .setColor(Blue).setOpacity(1.));
+
+
+ // Create a massless x-z base to provide the two slipping dofs.
+ MobilizedBody::Slider xdir(Ground, Transform(),
+ Body::Massless(), Transform());
+ const Rotation x2z(-Pi/2, YAxis); // rotate so +x moves to +z
+ MobilizedBody::Slider base(xdir, x2z, Body::Massless(), x2z);
+ base.addBodyDecoration(Transform(), DecorativeBrick(Vec3(0.25, 0.001, 0.25))
+ .setColor(Orange).setOpacity(0.50));
+
+
+ // Use a reverse mobilizer so that the contact point remains in a fixed
+ // location of the base body.
+ MobilizedBody::Ellipsoid rattle(base, Rotation(Pi/2, XAxis),
+ halfEllipsoid, Rotation(Pi/2, XAxis),
+ Vec3(a1,b1,c1), // ellipsoid half-radii
+ MobilizedBody::Reverse);
+
+ // Weld the bar to the ellipsoid at a 45 degree angle to produce lopsided
+ // inertia properties.
+ MobilizedBody::Weld bar(rattle, Transform(Rotation(45*Pi/180, YAxis),
+ Vec3(0,-b2/2.1,0)),
+ barBody, Transform());
+
+ // Finally, the rattle cannot just slide on the surface of the ground, it
+ // must roll.
+ #ifdef USE_BAD_CONSTRAINTS
+ // TODO: (sherm 20130620) these are the wrong constraints because they
+ // ignore the acceleration term caused by the contact point moving on the
+ // ellipsoid's surface. The correct constraint has to cognizant of the
+ // ellipsoid geometry at the contact point. Use of these constraints fails
+ // to conserve energy.
+
+ viz.addDecoration(Ground, Vec3(0),
+ DecorativeText("TODO: BROKEN -- USING INVALID NOSLIP CONSTRAINTS")
+ .setIsScreenText(true));
+ Constraint::NoSlip1D contactPointXdir(base, Vec3(0), UnitVec3(1,0,0),
+ matter.updGround(), rattle);
+ Constraint::NoSlip1D contactPointZdir(base, Vec3(0), UnitVec3(0,0,1),
+ matter.updGround(), rattle);
+ #endif
+
+ // Draw a cute green box to rattle around in.
+ Ground.addBodyDecoration(Vec3(0, 0*1e-5, 0), // floor
+ DecorativeBrick(Vec3(.5, .00001, .5)).setColor(Green).setOpacity(.1));
+ Ground.addBodyDecoration(Vec3(0.5, 0.25, 0), // right wall
+ DecorativeBrick(Vec3(1e-5, .25, .5)).setColor(Green).setOpacity(.25));
+ Ground.addBodyDecoration(Vec3(-.5, .25, 0), // left
+ DecorativeBrick(Vec3(1e-5, .25, .5)).setColor(Green).setOpacity(.25));
+ Ground.addBodyDecoration(Vec3(0, .25, -.5), // back
+ DecorativeBrick(Vec3(.5, .25, 1e-5)).setColor(Green).setOpacity(.25));
+ Ground.addBodyDecoration(Vec3(0, .25, .5), // front
+ DecorativeBrick(Vec3(.5, .25, 1e-5)).setColor(Green).setOpacity(.1));
+
+ // Output a visualization frame every 1/30 of a second, and output
+ // energy information every 1/4 second.
+ system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
+ system.addEventReporter(new EnergyReporter(system, rattle, 1./4));
+
+ // We're done building the system. Create it and obtain a copy of the
+ // default state.
+ State state = system.realizeTopology();
+
+ // Start this off at an angle so it will do something.
+ // Caution -- this joint is reversed.
+ rattle.setQToFitRotation(state, ~Rotation(10*Pi/180., YAxis));
+ //rattle.setUToFitAngularVelocity(state, Vec3(0, -0.5*Pi, 1.0*Pi));
+
+ // Set up simulation.
+ RungeKuttaMersonIntegrator integ(system);
+ integ.setAccuracy(1e-5);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+
+ // Simulate.
+ ts.stepTo(50);
+}
diff --git a/Simbody/tests/adhoc/SBPendulum1.cpp b/Simbody/tests/adhoc/SBPendulum1.cpp
new file mode 100644
index 0000000..844a71d
--- /dev/null
+++ b/Simbody/tests/adhoc/SBPendulum1.cpp
@@ -0,0 +1,402 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2006-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * A one-body pendulum, to test proper frame alignment and basic
+ * functioning of Simbody.
+ */
+
+/* Sketch:
+ *
+ * | \ | g
+ * *-- *-- v
+ * / G / Jb
+ *
+ *
+ * | |
+ * *==---------*==---------W
+ * / J / B weight
+ * <--- L/2 ---|--- L/2 --->
+ *
+ *
+ * The pendulum is a massless rod with origin frame
+ * B, joint attachment frame J, and a point mass W.
+ * The rod length is L, with the joint and mass
+ * located in opposite directions along the B
+ * frame X axis.
+ *
+ * There is a frame Jb on GroundIndex which will connect
+ * to J via a torsion joint around their mutual z axis.
+ * Gravity is in the -y direction of the GroundIndex frame.
+ * Note that Jb may not be aligned with G, and J may
+ * differ from B so the reference configuration may
+ * involve twisting the pendulum around somewhat.
+ */
+
+#include "SimTKsimbody.h"
+
+#include <string>
+#include <iostream>
+#include <exception>
+#include <cmath>
+using std::cout;
+using std::endl;
+
+using namespace SimTK;
+
+static const Real RadiansPerDegree = Pi/180;
+
+void stateTest() {
+ try {
+ State s;
+ s.setNumSubsystems(1);
+ s.advanceSubsystemToStage(SubsystemIndex(0), Stage::Topology);
+ s.advanceSystemToStage(Stage::Topology);
+
+ Vector v3(3), v2(2);
+ int q1 = s.allocateQ(SubsystemIndex(0), v3);
+ int q2 = s.allocateQ(SubsystemIndex(0), v2);
+
+ printf("q1,2=%d,%d\n", q1, q2);
+ cout << s;
+
+ DiscreteVariableIndex dv = s.allocateDiscreteVariable(SubsystemIndex(0), Stage::Dynamics, new Value<int>(5));
+
+ s.advanceSubsystemToStage(SubsystemIndex(0), Stage::Model);
+ //long dv2 = s.allocateDiscreteVariable(SubsystemIndex(0), Stage::Position, new Value<int>(5));
+
+ Value<int>::downcast(s.updDiscreteVariable(SubsystemIndex(0), dv)) = 71;
+ cout << s.getDiscreteVariable(SubsystemIndex(0), dv) << endl;
+
+ s.advanceSystemToStage(Stage::Model);
+
+ cout << s;
+
+ }
+ catch(const std::exception& e) {
+ printf("*** STATE TEST EXCEPTION\n%s\n***\n", e.what());
+ }
+
+}
+
+extern "C" void SimTK_version_SimTKlapack(int*,int*,int*);
+extern "C" void SimTK_about_SimTKlapack(const char*, int, char*);
+
+int main() {
+ stateTest();
+ //exit(0);
+
+ int major,minor,build;
+ char out[100];
+ const char* keylist[] = { "version", "library", "type", "debug", "authors", "copyright", "svn_revision", 0 };
+
+ //SimTK_version_SimTKlapack(&major,&minor,&build);
+ //std::printf("==> SimTKlapack library version: %d.%d.%d\n", major, minor, build);
+ //std::printf(" SimTK_about_SimTKlapack():\n");
+ //for (const char** p = keylist; *p; ++p) {
+ // SimTK_about_SimTKlapack(*p, 100, out);
+ // std::printf(" about(%s)='%s'\n", *p, out);
+ //}
+
+ SimTK_version_SimTKcommon(&major,&minor,&build);
+ std::printf("==> SimTKcommon library version: %d.%d.%d\n", major, minor, build);
+ std::printf(" SimTK_about_SimTKcommon():\n");
+ for (const char** p = keylist; *p; ++p) {
+ SimTK_about_SimTKcommon(*p, 100, out);
+ std::printf(" about(%s)='%s'\n", *p, out);
+ }
+
+ SimTK_version_simbody(&major,&minor,&build);
+ std::printf("==> simbody library version: %d.%d.%d\n", major, minor, build);
+ std::printf(" SimTK_about_simbody():\n");
+ for (const char** p = keylist; *p; ++p) {
+ SimTK_about_simbody(*p, 100, out);
+ std::printf(" about(%s)='%s'\n", *p, out);
+ }
+
+
+try {
+ MultibodySystem mbs;
+ SimbodyMatterSubsystem pend(mbs);
+ GeneralForceSubsystem springs(mbs);
+ HuntCrossleyContact contact(mbs);
+
+ Real L = 1.;
+ Real m = 3.;
+ Real g = 9.8;
+ Transform groundFrame;
+ Transform baseFrame;
+
+ Transform jointFrame(Vec3(-L/2,0,0));
+ MassProperties mprops(m, Vec3(L/2,0,0), Inertia(Vec3(L/2,0,0), m)+Inertia(1e-6,1e-6,1e-6));
+ cout << "mprops about body frame: " << mprops.getMass() << ", "
+ << mprops.getMassCenter() << ", " << mprops.getUnitInertia() << endl;
+
+ Vec3 gravity(0.,-g,0.);
+ Force::UniformGravity gravityForces(springs, pend, gravity);
+ cout << "period should be " << 2*std::acos(-1.)*std::sqrt(L/g) << " seconds." << endl;
+
+ MobilizedBody::Free aPendulum(pend.Ground(), Transform(), // ground, at origin
+ Body::Rigid(mprops), jointFrame);
+
+ const Real ballMass = 10;
+ const Real ballRadius = 2;
+ const MassProperties ballMProps(ballMass, Vec3(0), ballMass*UnitInertia::sphere(ballRadius));
+ const Vec3 ballPos = Vec3(-3,5,0);
+
+ MobilizedBody::Cartesian aBall(pend.Ground(), Transform(ballPos),
+ Body::Rigid(ballMProps), Transform());
+
+ MobilizedBody::Cartesian aBall2(pend.Ground(), Transform(ballPos+Vec3(0.1,10,0)),
+ Body::Rigid(ballMProps), Transform());
+
+ Constraint::Ball ballConstraint(pend.Ground(), Transform().p(),
+ aPendulum, jointFrame.p());
+
+ const Vec3 attachPt(1.5, 1, 0);
+ Force::TwoPointLinearSpring(springs, pend.Ground(), attachPt,
+ aPendulum, Vec3(L/2,0,0),
+ 100, 1);
+
+ const Real k = 1000, c = 0.0;
+ contact.addHalfSpace(GroundIndex, UnitVec3(0,1,0), 0, k, c); // h,k,c
+ contact.addHalfSpace(GroundIndex, UnitVec3(1,0,0), -10, k, c); // h,k,c
+ contact.addHalfSpace(GroundIndex, UnitVec3(-1,0,0), -10, k, c); // h,k,c
+
+ contact.addSphere(aBall, Vec3(0), ballRadius, k, c); // r,k,c
+ contact.addSphere(aBall2, Vec3(0), ballRadius, k, c); // r,k,c
+
+ State s = mbs.realizeTopology();
+ cout << "mbs State as built: " << s;
+
+ Visualizer viz(mbs);
+ viz.setBackgroundType(Visualizer::SolidColor);
+
+ viz.addDecoration(GroundIndex, Transform(), DecorativeBrick(Vec3(20,.1,20)).setColor(1.5*Gray).setOpacity(.3));
+ viz.addDecoration(GroundIndex, Transform(Vec3(-10,0,0)), DecorativeBrick(Vec3(.1,20,20)).setColor(Yellow).setOpacity(1));
+ viz.addDecoration(GroundIndex, Transform(Vec3(10,0,0)), DecorativeBrick(Vec3(.1,20,20)).setColor(Yellow).setOpacity(1));
+
+ DecorativeSphere bouncer(ballRadius);
+ viz.addDecoration(aBall, Transform(), bouncer.setColor(Orange));
+ viz.addDecoration(aBall2, Transform(), bouncer.setColor(Blue));
+
+ DecorativeLine rbProto; rbProto.setColor(Orange).setLineThickness(3);
+ viz.addRubberBandLine(GroundIndex, attachPt, aPendulum, Vec3(L/2,0,0), rbProto);
+
+ DecorativeSphere sphere(0.25);
+ sphere.setRepresentation(DecorativeGeometry::DrawPoints);
+ sphere.setResolution(2);
+ viz.addDecoration(GroundIndex, Transform(Vec3(1,2,3)), sphere);
+ sphere.setScale(0.5); sphere.setResolution(1);
+ viz.addDecoration(aPendulum, Transform(Vec3(0.1,0.2,0.3)), sphere);
+ Quaternion qqq; qqq.setQuaternionFromAngleAxis(Pi/4, UnitVec3(1,0,0));
+ viz.addDecoration(aPendulum, Transform(Rotation(qqq), Vec3(0,1,0)), DecorativeBrick(Vec3(.5,.1,.25)));
+ DecorativeCylinder cyl(0.1); cyl.setOpacity(0.3);
+ viz.addDecoration(aPendulum, Transform(Vec3(-1,0,0)),
+ DecorativeCylinder(0.1).setOpacity(0.3));
+
+ viz.addDecoration(aPendulum, Transform(Vec3(3, 0, 0)), DecorativeSphere().setColor(Black));
+ viz.addDecoration(aPendulum, Transform(Vec3(3, 0.5, 0)), DecorativeSphere().setColor(Gray));
+ viz.addDecoration(aPendulum, Transform(Vec3(3, 1, 0)), DecorativeSphere().setColor(White));
+ viz.addDecoration(aPendulum, Transform(Vec3(3, 1.5, 0)), DecorativeSphere().setColor(Red));
+ viz.addDecoration(aPendulum, Transform(Vec3(3, 2, 0)), DecorativeSphere().setColor(Green));
+ viz.addDecoration(aPendulum, Transform(Vec3(3, 2.5, 0)), DecorativeSphere().setColor(Blue));
+ viz.addDecoration(aPendulum, Transform(Vec3(3, 3, 0)), DecorativeSphere().setColor(Yellow));
+ viz.addDecoration(aPendulum, Transform(Vec3(3, 3.5, 0)), DecorativeSphere().setColor(Orange));
+ viz.addDecoration(aPendulum, Transform(Vec3(3, 4, 0)), DecorativeSphere().setColor(Magenta));
+ viz.addDecoration(aPendulum, Transform(Vec3(3, 4.5, 0)), DecorativeSphere().setColor(Cyan));
+ viz.addDecoration(aPendulum, Transform(Vec3(3, 5, 0)), DecorativeSphere().setColor(Purple));
+
+
+ viz.report(s);
+
+ // set Modeling stuff (s)
+ pend.setUseEulerAngles(s, false); // this is the default
+ //pend.setUseEulerAngles(s, true);
+ mbs.realizeModel(s);
+ cout << "mbs State as modeled: " << s;
+
+ printf("GLOBAL ny=%d q:y(%d,%d) u:y(%d,%d) z:y(%d,%d)\n",
+ (int)s.getNY(), (int)s.getQStart(), (int)s.getNQ(),
+ (int)s.getUStart(), (int)s.getNU(), (int)s.getZStart(), (int)s.getNZ());
+
+ mbs.realize(s, Stage::Instance);
+ printf(" nyerr=%d qerr:yerr(%d,%d) uerr:yerr(%d,%d)\n",
+ (int)s.getNYErr(), (int)s.getQErrStart(), (int)s.getNQErr(),
+ (int)s.getUErrStart(), (int)s.getNUErr());
+ printf(" nudoterr=%d\n", s.getNUDotErr());
+ for (SubsystemIndex i(0); i<s.getNumSubsystems(); ++i) {
+ printf("Subsys %d: q:y(%d,%d) u:y(%d,%d) z:y(%d,%d)\n",
+ (int)i,(int)s.getQStart()+(int)s.getQStart(i),(int)s.getNQ(i),
+ (int)s.getUStart()+(int)s.getUStart(i),(int)s.getNU(i),
+ (int)s.getZStart()+(int)s.getZStart(i),(int)s.getNZ(i));
+ printf(" qerr:yerr(%d,%d) uerr:yerr(%d,%d) uderr(%d,%d)\n",
+ (int)s.getQErrStart()+(int)s.getQErrStart(i),(int)s.getNQErr(i),
+ (int)s.getUErrStart()+(int)s.getUErrStart(i),(int)s.getNUErr(i),
+ (int)s.getUDotErrStart(i),(int)s.getNUDotErr(i));
+ }
+
+
+ //pend.setJointQ(s,1,0,0);
+ // pend.setJointQ(s,1,3,-1.1);
+ // pend.setJointQ(s,1,4,-2.2);
+ // pend.setJointQ(s,1,5,-3.3);
+
+ mbs.realize(s, Stage::Position);
+
+ Transform bodyConfig = aPendulum.getBodyTransform(s);
+ cout << "q=" << s.getQ() << endl;
+ cout << "body frame: " << bodyConfig;
+
+ Vector dummy; ProjectResults results;
+ mbs.projectQ(s, dummy, ProjectOptions().setRequiredAccuracy(1e-3),
+ results);
+
+ cout << "-------> STATE after realize(Position):" << s;
+ cout << "<------- STATE after realize(Position)." << endl;
+
+ cout << "after assembly body frame: " << aPendulum.getBodyTransform(s);
+
+ Vector_<SpatialVec> dEdR(pend.getNumBodies());
+ dEdR[0] = 0;
+ for (int i=1; i < pend.getNumBodies(); ++i)
+ dEdR[i] = SpatialVec(Vec3(0), Vec3(0.,2.,0.));
+ Vector dEdQ;
+ pend.multiplyBySystemJacobianTranspose(s, dEdR, dEdQ);
+ cout << "dEdR=" << dEdR << endl;
+ cout << "dEdQ=" << dEdQ << endl;
+
+ pend.getMobilizedBody(MobilizedBodyIndex(1)).setOneU(s,0,10.);
+
+ Vector_<SpatialVec> bodyForces(pend.getNumBodies());
+ Vector_<Vec3> particleForces(pend.getNumParticles());
+ Vector mobilityForces(pend.getNumMobilities());
+
+ bodyForces.setToZero();
+ particleForces.setToZero();
+ mobilityForces.setToZero();
+
+ pend.addInMobilityForce(s, aPendulum, MobilizerUIndex(0), 147, mobilityForces);
+
+ mbs.realize(s, Stage::Velocity);
+ SpatialVec bodyVel = aPendulum.getBodyVelocity(s);
+ cout << "body vel: " << bodyVel << endl;
+
+ cout << "wXwXr=" << bodyVel[0] % (bodyVel[0] % Vec3(2.5,0,0)) << endl;
+
+
+ cout << "after applying gravity, body forces=" << bodyForces << endl;
+ cout << " joint forces=" << mobilityForces << endl;
+
+ mbs.realize(s, Stage::Dynamics);
+ Vector equivT;
+ pend.calcTreeEquivalentMobilityForces(s, bodyForces, equivT);
+ cout << "body forces -> equiv joint forces=" << equivT << endl;
+
+ mbs.realize(s, Stage::Acceleration);
+
+ SpatialVec bodyAcc = aPendulum.getBodyAcceleration(s);
+ cout << "body acc: " << bodyAcc << endl;
+
+ aPendulum.setOneU(s, 0, 0.);
+
+ const Real angleInDegrees = 45;
+ const Vec4 aa(angleInDegrees*RadiansPerDegree,0, 0, 1);
+ Quaternion q; q.setQuaternionFromAngleAxis(aa);
+ aPendulum.setQToFitTransform(s,Transform(Rotation(q), Vec3(.1,.2,.3)));
+ viz.report(s);
+
+ //pend.updQ(s)[2] = -.1;
+ //pend.setJointQ(s, 1, 2, -0.999*std::acos(-1.)/2);
+
+ const Real h = 1./30;
+ const Real tstart = 0.;
+ const Real tmax = 20;
+
+ RungeKuttaMersonIntegrator ee(mbs);
+ ee.setFinalTime(tmax);
+
+ s.updTime() = tstart;
+ ee.initialize(s); // assemble if needed
+ s = ee.getState();
+ viz.report(s);
+
+ Integrator::SuccessfulStepStatus status;
+ int step = 0;
+ while ((status=ee.stepTo(step*h)) != Integrator::EndOfSimulation) {
+ const State& s = ee.getState();
+
+ if (!(step % 10)) {
+ // This is so we can calculate potential energy (although logically
+ // one should be able to do that at Stage::Position).
+ mbs.realize(s, Stage::Dynamics);
+
+ cout << " E=" << mbs.calcEnergy(s)
+ << " (pe=" << mbs.calcPotentialEnergy(s)
+ << ", ke=" << mbs.calcKineticEnergy(s)
+ << ") hNext=" << ee.getPredictedNextStepSize() << endl;
+
+ const Vector qdot = pend.getQDot(s);
+
+ Transform x = aPendulum.getBodyTransform(s);
+ SpatialVec v = aPendulum.getBodyVelocity(s);
+
+ //Vec3 err = x.p()-Vec3(2.5,0.,0.);
+ //Real d = err.norm();
+ //Real k = m*gravity.norm(); // stiffness, should balance at 1
+ // Real c = 10.; // damping
+ //Vec3 fk = -k*err;
+ //Real fc = -c*pend.getU(s)[2];
+ //pend.applyPointForce(s,aPendulum,Vec3(0,0,0),fk);
+ //pend.applyJointForce(s,aPendulum,2,fc);
+
+ cout << s.getTime() << " "
+ << s.getQ() << " " << s.getU()
+ << " hNext=" << ee.getPredictedNextStepSize() << endl;
+ cout << "body config=" << x;
+ cout << "body velocity=" << v << endl;
+ //cout << "err=" << err << " |err|=" << d << endl;
+ //cout << "spring force=" << fk << endl;
+ //cout << "damping joint forces=" << fc << endl;
+
+ }
+
+ viz.report(s);
+
+ if (status == Integrator::ReachedReportTime)
+ ++step;
+
+
+ if (!(step % 100)) {
+ mbs.realize(s, Stage::Acceleration);
+ const Vector udot = s.getUDot();
+ cout << "udot = " << udot << endl;
+ }
+ }
+
+}
+catch (const std::exception& e) {
+ printf("EXCEPTION THROWN: %s\n", e.what());
+}
+ return 0;
+}
diff --git a/Simbody/tests/adhoc/SphericalCoordsMobilizerTest.cpp b/Simbody/tests/adhoc/SphericalCoordsMobilizerTest.cpp
new file mode 100644
index 0000000..9988874
--- /dev/null
+++ b/Simbody/tests/adhoc/SphericalCoordsMobilizerTest.cpp
@@ -0,0 +1,196 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This is an outer block for simulating ??? in various ways with Simbody.
+ * This is about testing Simbody, *not* studying ???!
+ */
+
+#include "SimTKsimbody.h"
+
+#include <string>
+#include <iostream>
+#include <exception>
+
+using std::cout;
+using std::cin;
+using std::endl;
+
+using namespace SimTK;
+
+static const Real Deg2Rad = (Real)SimTK_DEGREE_TO_RADIAN,
+ Rad2Deg = (Real)SimTK_RADIAN_TO_DEGREE;
+
+
+
+static Real g = 9.8;
+static Real m = 1;
+
+int main(int argc, char** argv) {
+ static const Transform GroundFrame;
+ static const Rotation ZUp(UnitVec3(XAxis), XAxis, UnitVec3(YAxis), ZAxis);
+ static const Vec3 TestLoc(1,0,0);
+
+ try { // If anything goes wrong, an exception will be thrown.
+
+ // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
+ MultibodySystem mbs;
+
+ SimbodyMatterSubsystem matter(mbs);
+ GeneralForceSubsystem forces(mbs);
+ DecorationSubsystem viz(mbs);
+ //Force::UniformGravity gravity(forces, matter, Vec3(0, -g, 0));
+
+ // ADD BODIES AND THEIR MOBILIZERS
+ Body::Rigid particle = Body::Rigid(MassProperties(m, Vec3(0), Inertia(0)));
+ particle.addDecoration(DecorativeSphere(.1).setColor(Red).setOpacity(.3));
+
+ MobilizedBody::SphericalCoords
+ anAtom(matter.Ground(), Transform(ZUp, TestLoc),
+ particle, Transform(),
+ 0*Deg2Rad, false, // azimuth offset, negated
+ 0, false, // zenith offset, negated
+ ZAxis, true); // translation axis, negated
+
+ anAtom.setDefaultRadius(.1);
+ anAtom.setDefaultAngles(Vec2(0, 30*Deg2Rad));
+
+ viz.addRubberBandLine(matter.Ground(), TestLoc,
+ anAtom, Vec3(0),
+ DecorativeLine().setColor(Orange).setLineThickness(4));
+
+ Force::MobilityLinearSpring(forces, anAtom, 0, 2, -30*Deg2Rad); // harmonic bend
+ Force::MobilityLinearSpring(forces, anAtom, 1, 2, 45*Deg2Rad); // harmonic bend
+ Force::MobilityLinearSpring(forces, anAtom, 2, 20, .5); // harmonic stretch
+
+ Force::MobilityLinearDamper(forces, anAtom, 0, .1); // harmonic bend
+ Force::MobilityLinearDamper(forces, anAtom, 1, .1); // harmonic bend
+ Force::MobilityLinearDamper(forces, anAtom, 2, .1); // harmonic stretch
+
+
+ State s = mbs.realizeTopology(); // returns a reference to the the default state
+ mbs.realizeModel(s); // define appropriate states for this System
+ mbs.realize(s, Stage::Instance); // instantiate constraints if any
+
+ Visualizer display(mbs);
+ display.setBackgroundType(Visualizer::SolidColor);
+
+ mbs.realize(s, Stage::Velocity);
+ display.report(s);
+
+ cout << "q=" << s.getQ() << endl;
+ cout << "u=" << s.getU() << endl;
+
+ char c;
+ cout << "Default configuration shown. 1234 to move on.\n";
+
+ //anAtom.setQToFitRotation(s, Rotation(-.9*Pi/2,YAxis));
+
+ while (true) {
+ Real x;
+ cout << "Torsion (deg)? "; cin >> x; if (x==1234) break;
+ Vec2 a = anAtom.getAngles(s); a[0]=x*Deg2Rad; anAtom.setAngles(s, a);
+ display.report(s);
+ cout << "Bend (deg)? "; cin >> x; if (x==1234) break;
+ a = anAtom.getAngles(s); a[1]=x*Deg2Rad; anAtom.setAngles(s, a);
+ display.report(s);
+ cout << "Radius? "; cin >> x; if (x==1234) break;
+ anAtom.setRadius(s, x);
+ display.report(s);
+ }
+ anAtom.setUToFitAngularVelocity(s, Vec3(.1,.2,.3));
+
+ //anAtom.setAngle(s, 45*Deg2Rad);
+ //anAtom.setTranslation(s, Vec2(.4, .1));
+
+ mbs.realize(s, Stage::Dynamics);
+ mbs.realize(s, Stage::Acceleration);
+
+ cout << "q=" << s.getQ() << endl;
+ cout << "u=" << s.getU() << endl;
+ cout << "qdot=" << s.getQDot() << endl;
+ cout << "udot=" << s.getUDot() << endl;
+ cout << "qdotdot=" << s.getQDotDot() << endl;
+ display.report(s);
+
+ cout << "Initialized configuration shown. Ready? ";
+ cin >> c;
+
+ RungeKuttaMersonIntegrator myStudy(mbs);
+ myStudy.setAccuracy(1e-4);
+
+ const Real dt = .02; // output intervals
+ const Real finalTime = 20;
+
+ myStudy.setFinalTime(finalTime);
+
+ // Peforms assembly if constraints are violated.
+ myStudy.initialize(s);
+
+ cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
+ cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
+ cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
+ cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
+ cout << "U WEIGHTS=" << s.getUWeights() << endl;
+ cout << "Z WEIGHTS=" << s.getZWeights() << endl;
+ cout << "1/QTOLS=" << s.getQErrWeights() << endl;
+ cout << "1/UTOLS=" << s.getUErrWeights() << endl;
+
+ Integrator::SuccessfulStepStatus status;
+ int nextReport = 0;
+ while ((status=myStudy.stepTo(nextReport*dt))
+ != Integrator::EndOfSimulation)
+ {
+ const State& s = myStudy.getState();
+ mbs.realize(s);
+ printf("%5g %10.4g %10.4g %10.4g E=%10.8g h%3d=%g %s%s\n", s.getTime(),
+ anAtom.getAngles(s)[0], anAtom.getAngles(s)[1], anAtom.getRadius(s),
+ //anAtom.getAngle(s), anAtom.getTranslation(s)[0], anAtom.getTranslation(s)[1],
+ //anAtom.getQ(s)[0], anAtom.getQ(s)[1], anAtom.getQ(s)[2],
+ mbs.calcEnergy(s), myStudy.getNumStepsTaken(),
+ myStudy.getPreviousStepSizeTaken(),
+ Integrator::getSuccessfulStepStatusString(status).c_str(),
+ myStudy.isStateInterpolated()?" (INTERP)":"");
+
+ display.report(s);
+
+ if (status == Integrator::ReachedReportTime)
+ ++nextReport;
+ }
+
+ printf("Using Integrator %s:\n", myStudy.getMethodName());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());
+
+ }
+ catch (const std::exception& e) {
+ printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+ }
+ catch (...) {
+ printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+}
diff --git a/Simbody/tests/adhoc/TestMultibodyPerformance.cpp b/Simbody/tests/adhoc/TestMultibodyPerformance.cpp
new file mode 100644
index 0000000..142173a
--- /dev/null
+++ b/Simbody/tests/adhoc/TestMultibodyPerformance.cpp
@@ -0,0 +1,592 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2011-12 Stanford University and the Authors. *
+ * Authors: Peter Eastman *
+ * Contributors: Michael Sherman *
+ * *
+ * 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"
+#include <string>
+#include <ctime>
+
+using std::cout;
+using std::endl;
+using std::string;
+
+using namespace SimTK;
+
+/**
+ * This test measures the speed of various multibody calculations. It executes a collection of operations
+ * on various systems. The CPU time required to perform each operation 1000 times is measured and
+ * printed to the console.
+ *
+ * Each test system contains 256 identical bodies (plus ground), but they differ in the type of
+ * bodies and their arrangement into a multibody tree. The arrangements include 1) all bodies attached
+ * directly to ground, 2) the bodies linked in a single chain, and 3) the bodies arranged to form
+ * a binary tree.
+ */
+
+// The following routines define the operations to be profiled.
+
+void doRealizeTime(MultibodySystem& system, State& state) {
+ state.invalidateAllCacheAtOrAbove(Stage::Time);
+ system.realize(state, Stage::Time);
+}
+
+void doRealizePosition(MultibodySystem& system, State& state) {
+ state.invalidateAllCacheAtOrAbove(Stage::Position);
+ system.realize(state, Stage::Position);
+}
+
+void doRealizeVelocity(MultibodySystem& system, State& state) {
+ state.invalidateAllCacheAtOrAbove(Stage::Velocity);
+ system.realize(state, Stage::Velocity);
+}
+
+void doRealizeArticulatedBodyInertias(MultibodySystem& system, State& state) {
+ const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
+ matter.invalidateArticulatedBodyInertias(state);
+ matter.realizeArticulatedBodyInertias(state);
+}
+
+void doRealizeDynamics(MultibodySystem& system, State& state) {
+ state.invalidateAllCacheAtOrAbove(Stage::Dynamics);
+ system.realize(state, Stage::Dynamics);
+}
+
+void doRealizeAcceleration(MultibodySystem& system, State& state) {
+ state.invalidateAllCacheAtOrAbove(Stage::Acceleration);
+ system.realize(state, Stage::Acceleration);
+}
+
+// Cost to re-evaluate accelerations after applying some new forces, but leaving
+// the state variables unchanged.
+void doRealizeDynamics2Acceleration(MultibodySystem& system, State& state) {
+ state.invalidateAllCacheAtOrAbove(Stage::Dynamics);
+ system.realize(state, Stage::Acceleration);
+}
+
+// Cost to re-evaluate accelerations after updating velocities, but leaving
+// the positions unchanged (e.g. semi-implicit Euler iterating velocities).
+void doRealizeVelocity2Acceleration(MultibodySystem& system, State& state) {
+ state.invalidateAllCacheAtOrAbove(Stage::Velocity);
+ system.realize(state, Stage::Acceleration);
+}
+
+// Cost for a complete acceleration calculation at a new time and state.
+// This includes the cost of articulated body inertias.
+void doRealizeTime2Acceleration(MultibodySystem& system, State& state) {
+ state.invalidateAllCacheAtOrAbove(Stage::Time);
+ system.realize(state, Stage::Acceleration);
+}
+
+void doMultiplyByM(MultibodySystem& system, State& state) {
+ const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
+ Vector v(matter.getNumMobilities(), 1.0);
+ Vector mv;
+ matter.multiplyByM(state, v, mv);
+}
+
+void doMultiplyByMInv(MultibodySystem& system, State& state) {
+ const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
+ Vector v(matter.getNumMobilities(), 1.0);
+ Vector minvv;
+ matter.multiplyByMInv(state, v, minvv);
+}
+
+void doCalcResidualForceIgnoringConstraints(MultibodySystem& system, State& state) {
+ const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
+ Vector appliedMobilityForces(matter.getNumMobilities(), 1.0);
+ Vector_<SpatialVec> appliedBodyForces(matter.getNumBodies(), SpatialVec(Vec3(1, 0, 0), Vec3(0, 1, 0)));
+ Vector knownUdot, residualMobilityForces;
+ matter.calcResidualForceIgnoringConstraints(state, appliedMobilityForces, appliedBodyForces, knownUdot, residualMobilityForces);
+}
+
+void doCalcMobilizerReactionForces(MultibodySystem& system, State& state) {
+ Vector_<SpatialVec> forces;
+ system.getMatterSubsystem().calcMobilizerReactionForces(state, forces);
+}
+
+void doMultiplyBySystemJacobianTranspose(MultibodySystem& system, State& state) {
+ const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
+ Vector_<SpatialVec> dEdR(matter.getNumBodies(), SpatialVec(Vec3(1, 0, 0), Vec3(0, 1, 0)));
+ Vector dEdQ;
+ matter.multiplyBySystemJacobianTranspose(state, dEdR, dEdQ);
+}
+
+void doCalcCompositeBodyInertias(MultibodySystem& system, State& state) {
+ Array_<SpatialInertia, MobilizedBodyIndex> r;
+ system.getMatterSubsystem().calcCompositeBodyInertias(state, r);
+}
+
+static double flopTimeInNs;
+
+/**
+ * Time how long it takes to perform an operation 1000 times. The test is repeated 5 times,
+ * and the average is returned. The return value represents CPU time, *not* clock time.
+ */
+void timeComputation(MultibodySystem& system, void function(MultibodySystem& system, State& state),
+ const string& name, int iterations, bool useEulerAngles) {
+ const int repeats = 3;
+ Vector cpuTimes(repeats);
+ State state = system.getDefaultState();
+ if (useEulerAngles) {
+ system.getMatterSubsystem().setUseEulerAngles(state, true);
+ system.realizeModel(state);
+ }
+ system.realize(state, Stage::Acceleration);
+
+ const int ndof = system.getMatterSubsystem().getNumMobilities();
+ const int nmovbod = system.getMatterSubsystem().getNumBodies()-1; // not Ground
+
+ // Repeatedly measure the CPU time for performing the operation 1000 times.
+
+ for (int i = 0; i < repeats; i++) {
+ double startCpu = threadCpuTime();
+ for (int j = 0; j < iterations; j++)
+ function(system, state);
+ double endCpu = threadCpuTime();
+ cpuTimes[i] = Real(endCpu-startCpu);
+ }
+
+ double timePerIterUs = mean(cpuTimes)*1000000/iterations; // us
+ double flopTimeUs = flopTimeInNs / 1000;
+ double flopTimePerIter = timePerIterUs/flopTimeUs;
+ std::printf("%40s:%6.4gus -> %4d flp/dof, %4d flp/bod\n",
+ name.c_str(), timePerIterUs, (int)(flopTimePerIter/ndof),
+ (int)(flopTimePerIter/nmovbod));
+}
+
+/**
+ * Time all the different calculations for one system.
+ */
+void runAllTests(MultibodySystem& system, bool useEulerAngles=false) {
+ std::cout << "# dofs=" << system.getMatterSubsystem().getNumMobilities() << "\n";
+ timeComputation(system, doRealizeTime, "realizeTime", 5000, useEulerAngles);
+ timeComputation(system, doRealizePosition, "realizePosition", 5000, useEulerAngles);
+ timeComputation(system, doRealizeVelocity, "realizeVelocity", 5000, useEulerAngles);
+ timeComputation(system, doRealizeArticulatedBodyInertias, "doRealizeArticulatedBodyInertias", 3000, useEulerAngles);
+ timeComputation(system, doRealizeDynamics, "realizeDynamics", 5000, useEulerAngles);
+ timeComputation(system, doRealizeAcceleration, "realizeAcceleration", 5000, useEulerAngles);
+ timeComputation(system, doRealizeDynamics2Acceleration, "doRealizeDynamics2Acceleration", 5000, useEulerAngles);
+ timeComputation(system, doRealizeVelocity2Acceleration, "realizeVelocity2Acceleration", 3000, useEulerAngles);
+ timeComputation(system, doRealizeTime2Acceleration, "realizeTime2Acceleration", 2000, useEulerAngles);
+ timeComputation(system, doMultiplyByM, "multiplyByM", 5000, useEulerAngles);
+ timeComputation(system, doMultiplyByMInv, "multiplyByMInv", 5000, useEulerAngles);
+ timeComputation(system, doCalcResidualForceIgnoringConstraints, "calcResidualForceIgnoringConstraints", 5000, useEulerAngles);
+ timeComputation(system, doCalcMobilizerReactionForces, "calcMobilizerReactionForces", 1000, useEulerAngles);
+ timeComputation(system, doMultiplyBySystemJacobianTranspose, "multiplyBySystemJacobianTranspose", 5000, useEulerAngles);
+ timeComputation(system, doCalcCompositeBodyInertias, "calcCompositeBodyInertias", 5000, useEulerAngles);
+}
+
+// The following routines create the systems to be profiled.
+
+void createParticles(MultibodySystem& system) {
+ SimbodyMatterSubsystem matter(system);
+ Body::Rigid body;
+ for (int i = 0; i < 256; i++)
+ MobilizedBody::Translation next(matter.updGround(), Vec3(1, 0, 0), body, Vec3(0));
+ system.realizeTopology();
+}
+
+void createFreeBodies(MultibodySystem& system) {
+ SimbodyMatterSubsystem matter(system);
+ Body::Rigid body;
+ for (int i = 0; i < 256; i++)
+ MobilizedBody::Free next(matter.updGround(), Vec3(1, 0, 0), body, Vec3(0));
+ system.realizeTopology();
+}
+
+void createPinChain(MultibodySystem& system) {
+ SimbodyMatterSubsystem matter(system);
+ Body::Rigid body;
+ MobilizedBody last = matter.updGround();
+ for (int i = 0; i < 256; i++) {
+ MobilizedBody::Pin next(last, Vec3(1, 0, 0), body, Vec3(0));
+ last = next;
+ }
+ system.realizeTopology();
+}
+
+void createSliderChain(MultibodySystem& system) {
+ SimbodyMatterSubsystem matter(system);
+ Body::Rigid body;
+ MobilizedBody last = matter.updGround();
+ for (int i = 0; i < 256; i++) {
+ MobilizedBody::Slider next(last, Vec3(1, 0, 0), body, Vec3(0));
+ last = next;
+ }
+ system.realizeTopology();
+}
+
+void createBallChain(MultibodySystem& system) {
+ SimbodyMatterSubsystem matter(system);
+ Body::Rigid body;
+ MobilizedBody last = matter.updGround();
+ for (int i = 0; i < 256; i++) {
+ MobilizedBody::Ball next(last, Vec3(1, 0, 0), body, Vec3(0));
+ last = next;
+ }
+ system.realizeTopology();
+}
+
+
+void createGimbalChain(MultibodySystem& system) {
+ SimbodyMatterSubsystem matter(system);
+ Body::Rigid body;
+ MobilizedBody last = matter.updGround();
+ for (int i = 0; i < 256; i++) {
+ MobilizedBody::Gimbal next(last, Vec3(1, 0, 0), body, Vec3(0));
+ last = next;
+ }
+ system.realizeTopology();
+}
+
+void createPinTree(MultibodySystem& system) {
+ SimbodyMatterSubsystem matter(system);
+ Body::Rigid body;
+ for (int i = 0; i < 256; i++) {
+ MobilizedBody& parent = matter.updMobilizedBody(MobilizedBodyIndex(i/2));
+ MobilizedBody::Pin next(parent, Vec3(1, 0, 0), body, Vec3(0));
+ }
+ system.realizeTopology();
+}
+
+void createBallTree(MultibodySystem& system) {
+ SimbodyMatterSubsystem matter(system);
+ Body::Rigid body;
+ for (int i = 0; i < 256; i++) {
+ MobilizedBody& parent = matter.updMobilizedBody(MobilizedBodyIndex(i/2));
+ MobilizedBody::Ball next(parent, Vec3(1, 0, 0), body, Vec3(0));
+ }
+ system.realizeTopology();
+}
+
+static int tenInts[10];
+static Real tenReals[10];
+// These should multiply out to about 1.
+static Real tenMults[10] =
+ {Real(0.501),Real(0.2501),Real(0.201),Real(0.101),Real(1.000000001),
+ Real(1/1.000000002),Real(1/.101), Real(1/.201), Real(1/.2501), Real(1/.501)};
+void testFunctions(double& flopTime, bool flopTimeOnly=false) {
+ Real addRes=1,subRes=1,mulRes=1,divRes=1,sqrtRes=1,oosqrtRes=1,
+ sinRes=1,cosRes=1,atan2Res=1,logRes=1,expRes=1;
+ int intAddRes=1;
+ Random::Uniform rand; rand.setMin(-5); rand.setMax(5);
+ for (int i=0; i<10; i++) tenInts[i] = rand.getIntValue();
+ for (int i=0; i<10; i++) tenReals[i] = rand.getValue();
+
+ double tprev = threadCpuTime();
+ for (int i = 0; i < 10*100000000; i++) {
+ intAddRes += tenInts[0];
+ intAddRes -= tenInts[1];
+ intAddRes += tenInts[2];
+ intAddRes -= tenInts[3];
+ intAddRes += tenInts[4];
+ intAddRes -= tenInts[5];
+ intAddRes += tenInts[6];
+ intAddRes -= tenInts[7];
+ intAddRes += tenInts[8];
+ intAddRes -= tenInts[9];
+ }
+ double t = threadCpuTime(); double intAddTime = (t-tprev)/10; // time for 1e9 ops
+ printf("intAdd %gs\n", t-tprev);
+ tprev = threadCpuTime();
+ for (int i = 0; i < 5*100000000; i++) {
+ addRes += Real(1.1);
+ addRes += Real(1.2);
+ addRes += Real(1.3);
+ addRes += Real(1.4);
+ addRes += Real(1.501);
+ addRes += Real(1.6);
+ addRes += Real(1.7);
+ addRes += Real(1.8);
+ addRes += Real(1.9);
+ addRes += Real(2.007);
+ }
+ t = threadCpuTime(); double addTime = (t-tprev)/5;
+ printf("add %gs\n", t-tprev);
+ tprev = threadCpuTime();
+ for (int i = 0; i < 5*100000000; i++) {
+ subRes -= Real(1.1);
+ subRes -= Real(1.2);
+ subRes -= Real(1.3);
+ subRes -= Real(1.4);
+ subRes -= Real(1.501);
+ subRes -= Real(1.6);
+ subRes -= Real(1.7);
+ subRes -= Real(1.8);
+ subRes -= Real(1.9);
+ subRes -= Real(2.007);
+ }
+ t = threadCpuTime(); double subTime = (t-tprev)/5;
+ printf("sub %gs\n", t-tprev);
+ tprev = threadCpuTime();
+ for (int i = 0; i < 3*100000000; i++) {
+ mulRes *= Real(0.501);
+ mulRes *= Real(0.2501);
+ mulRes *= Real(0.201);
+ mulRes *= Real(0.101);
+ mulRes *= Real(1.000000001);
+ mulRes *= Real(1/1.000000002); // done at compile time
+ mulRes *= Real(1/.101);
+ mulRes *= Real(1/.201);
+ mulRes *= Real(1/.2501);
+ mulRes *= Real(1/.501);
+ }
+ t = threadCpuTime(); double mulTime=(t-tprev)/3;
+ printf("mul %gs\n", t-tprev);
+ flopTime = (addTime+mulTime)/2;
+ std::cout << "1 flop=avg(add,mul)=" << flopTime << "ns\n";
+ if (flopTimeOnly)
+ return;
+
+ tprev = threadCpuTime();
+ for (int i = 0; i < 100000000; i++) {
+ divRes /= tenMults[7];
+ divRes /= tenMults[9];
+ divRes /= tenMults[8];
+ divRes /= tenMults[6];
+ divRes /= tenMults[2];
+ divRes /= tenMults[3];
+ divRes /= tenMults[1];
+ divRes /= tenMults[0];
+ divRes /= tenMults[4];
+ divRes /= tenMults[5];
+ // prevent clever optimization VC10 did to turn divides
+ // into multiplies.
+ tenMults[i%10] = Real(tenMults[i%10]*1.0000000000001);
+ tenMults[(i+5)%10] = Real(tenMults[(i+5)%10]*0.9999999999999);
+ }
+ t = threadCpuTime(); double divTime=(t-tprev);
+ printf("div %gs\n", t-tprev);
+ tprev = threadCpuTime();
+
+ for (int i = 0; i < 100000000/2; i++) {
+ const Real ir = (Real)i;
+ sqrtRes += std::sqrt(ir+(Real)0.001); // two adds
+ sqrtRes += std::sqrt(ir+(Real)0.1);
+ sqrtRes += std::sqrt(ir+(Real)0.2);
+ sqrtRes += std::sqrt(ir+(Real)0.3);
+ sqrtRes += std::sqrt(ir+(Real)0.4);
+ sqrtRes += std::sqrt(ir+(Real)0.501);
+ sqrtRes += std::sqrt(ir+(Real)0.6);
+ sqrtRes += std::sqrt(ir+(Real)0.7);
+ sqrtRes += std::sqrt(ir+(Real)0.8);
+ sqrtRes += std::sqrt(ir+(Real)0.9);
+ }
+ t = threadCpuTime(); double sqrtTime=2*(t-tprev)-2*addTime;
+ printf("sqrt %gs\n", t-tprev);
+ tprev = threadCpuTime();
+ for (int i = 0; i < 100000000/4; i++) {
+ const Real ir = (Real)i;
+ oosqrtRes += 1/std::sqrt(ir+(Real)0.001); // two adds
+ oosqrtRes += 1/std::sqrt(ir+(Real)0.1);
+ oosqrtRes += 1/std::sqrt(ir+(Real)0.2);
+ oosqrtRes += 1/std::sqrt(ir+(Real)0.3);
+ oosqrtRes += 1/std::sqrt(ir+(Real)0.4);
+ oosqrtRes += 1/std::sqrt(ir+(Real)0.501);
+ oosqrtRes += 1/std::sqrt(ir+(Real)0.6);
+ oosqrtRes += 1/std::sqrt(ir+(Real)0.7);
+ oosqrtRes += 1/std::sqrt(ir+(Real)0.8);
+ oosqrtRes += 1/std::sqrt(ir+(Real)0.9);
+ }
+ t = threadCpuTime(); double oosqrtTime=4*(t-tprev)-2*addTime;
+ printf("1/sqrt %gs\n", t-tprev);
+ tprev = threadCpuTime();
+ for (int i = 0; i < 100000000/5; i++) {
+ const Real ir = (Real)i;
+ logRes += std::log(ir+(Real)0.001); // two adds
+ logRes += std::log(ir+(Real)0.1);
+ logRes += std::log(ir+(Real)0.2);
+ logRes += std::log(ir+(Real)0.3);
+ logRes += std::log(ir+(Real)0.4);
+ logRes += std::log(ir+(Real)0.501);
+ logRes += std::log(ir+(Real)0.6);
+ logRes += std::log(ir+(Real)0.7);
+ logRes += std::log(ir+(Real)0.8);
+ logRes += std::log(ir+(Real)0.9);
+ }
+ t = threadCpuTime(); double logTime=(t-tprev)*5-2*addTime;
+ printf("log %gs\n", t-tprev);
+ tprev = threadCpuTime();
+ for (int i = 0; i < 100000000/5; i++) {
+ const Real ir = (Real).000000001*(Real)i;
+ expRes += std::exp(ir+(Real)0.001); // two adds
+ expRes += std::exp(ir+(Real)0.1);
+ expRes += std::exp(ir+(Real)0.2);
+ expRes += std::exp(ir+(Real)0.3);
+ expRes += std::exp(ir+(Real)0.4);
+ expRes += std::exp(ir+(Real)0.501);
+ expRes += std::exp(ir+(Real)0.6);
+ expRes += std::exp(ir+(Real)0.7);
+ expRes += std::exp(ir+(Real)0.8);
+ expRes += std::exp(ir+(Real)0.9);
+ }
+ t = threadCpuTime(); double expTime=(t-tprev)*5-2*addTime;
+ printf("exp %gs\n", t-tprev);
+ tprev = threadCpuTime();
+ for (int i = 0; i < 100000000/10; i++) {
+ const Real ir = (Real)i;
+ sinRes += std::sin(ir+(Real)0.001); // two adds
+ sinRes += std::sin(ir+(Real)0.1);
+ sinRes += std::sin(ir+(Real)0.2);
+ sinRes += std::sin(ir+(Real)0.3);
+ sinRes += std::sin(ir+(Real)0.4);
+ sinRes += std::sin(ir+(Real)0.501);
+ sinRes += std::sin(ir+(Real)0.6);
+ sinRes += std::sin(ir+(Real)0.7);
+ sinRes += std::sin(ir+(Real)0.8);
+ sinRes += std::sin(ir+(Real)0.9);
+ }
+ t = threadCpuTime(); double sinTime=(t-tprev)*10-2*addTime;
+ printf("sin %gs\n", t-tprev);
+ tprev = threadCpuTime();
+ for (int i = 0; i < 100000000/10; i++) {
+ const Real ir = (Real)i;
+ cosRes += std::cos(ir+(Real)0.001); // two adds
+ cosRes += std::cos(ir+(Real)0.1);
+ cosRes += std::cos(ir+(Real)0.2);
+ cosRes += std::cos(ir+(Real)0.3);
+ cosRes += std::cos(ir+(Real)0.4);
+ cosRes += std::cos(ir+(Real)0.501);
+ cosRes += std::cos(ir+(Real)0.6);
+ cosRes += std::cos(ir+(Real)0.7);
+ cosRes += std::cos(ir+(Real)0.8);
+ cosRes += std::cos(ir+(Real)0.9);
+ }
+ t = threadCpuTime(); double cosTime=(t-tprev)*10-2*addTime;
+ printf("cos %gs\n", t-tprev);
+ tprev = threadCpuTime();
+ for (int i = 0; i < 100000000/10; i++) {
+ const Real ir = (Real)i;
+ atan2Res += std::atan2(ir+(Real)0.001,ir-(Real)0.001); // three adds
+ atan2Res += std::atan2(ir+(Real)0.1,ir-(Real)0.1);
+ atan2Res += std::atan2(ir+(Real)0.2,ir-(Real)0.2);
+ atan2Res += std::atan2(ir+(Real)0.3,ir-(Real)0.3);
+ atan2Res += std::atan2(ir+(Real)0.4,ir-(Real)0.4);
+ atan2Res += std::atan2(ir+(Real)0.501,ir-(Real)0.501);
+ atan2Res += std::atan2(ir+(Real)0.6,ir-(Real)0.6);
+ atan2Res += std::atan2(ir+(Real)0.7,ir-(Real)0.7);
+ atan2Res += std::atan2(ir+(Real)0.8,ir-(Real)0.8);
+ atan2Res += std::atan2(ir+(Real)0.9,ir-(Real)0.9);
+ }
+ t = threadCpuTime(); double atan2Time=(t-tprev)*10-3*addTime;
+ printf("atan2 %gs\n", t-tprev);
+ tprev = threadCpuTime();
+
+ std::cout << std::setprecision(5);
+ std::cout << "1 flop=avg(add,mul)=" << flopTime << "ns\n";
+ printf("op\t t/10^9\t flops\t final result\n");
+ std::cout << "int+:\t" <<intAddTime<<"\t"<<intAddTime /flopTime<<"\t"<<intAddRes<< "\n";
+ std::cout << "+:\t" <<addTime<<"\t"<<addTime /flopTime<<"\t"<<addRes<< "\n";
+ std::cout << "-:\t" <<subTime<<"\t"<<subTime /flopTime<<"\t"<<subRes<< "\n";
+ std::cout << "*:\t" <<mulTime<<"\t"<<mulTime /flopTime<<"\t"<<mulRes<< "\n";
+ std::cout << "/:\t" <<divTime<<"\t"<<divTime /flopTime<<"\t"<<divRes<< "\n";
+ std::cout << "sqrt:\t" <<sqrtTime<<"\t"<<sqrtTime /flopTime<<"\t"<<sqrtRes<< "\n";
+ std::cout << "1/sqrt:\t"<<oosqrtTime<<"\t"<<oosqrtTime/flopTime<<"\t"<<oosqrtRes<< "\n";
+ std::cout << "log:\t" <<logTime<<"\t"<<logTime /flopTime<<"\t"<<logRes<< "\n";
+ std::cout << "exp:\t" <<expTime<<"\t"<<expTime /flopTime<<"\t"<<expRes<< "\n";
+ std::cout << "sin:\t" <<sinTime<<"\t"<<sinTime /flopTime<<"\t"<<sinRes<< "\n";
+ std::cout << "cos:\t" <<cosTime<<"\t"<<cosTime /flopTime<<"\t"<<cosRes<< "\n";
+ std::cout << "atan2:\t" <<atan2Time<<"\t"<<atan2Time /flopTime<<"\t"<<atan2Res<< "\n";
+}
+
+
+int main() {
+ time_t now;
+ time(&now);
+ printf("Starting: %s\n", ctime(&now));
+ { std::cout << "\nCPU performance\n" << std::endl;
+ testFunctions(flopTimeInNs, true /*flop time only*/);
+ }
+ double startClock = realTime();
+ double startCpu = cpuTime();
+ double startThread = threadCpuTime();
+
+ {
+ std::cout << "\nParticles:\n" << std::endl;
+ MultibodySystem system;
+ createParticles(system);
+ runAllTests(system);
+ }
+
+ {
+ std::cout << "\nFree Bodies (Quaternions):\n" << std::endl;
+ MultibodySystem system;
+ createFreeBodies(system);
+ runAllTests(system, false);
+ }
+ {
+ std::cout << "\nFree Bodies (Euler angles):\n" << std::endl;
+ MultibodySystem system;
+ createFreeBodies(system);
+ runAllTests(system, true);
+ }
+
+ {
+ std::cout << "\nPin Chain:\n" << std::endl;
+ MultibodySystem system;
+ createPinChain(system);
+ runAllTests(system);
+ }
+ {
+ std::cout << "\nSlider Chain:\n" << std::endl;
+ MultibodySystem system;
+ createSliderChain(system);
+ runAllTests(system);
+ }
+ {
+ std::cout << "\nBall Chain (Quaternions):\n" << std::endl;
+ MultibodySystem system;
+ createBallChain(system);
+ runAllTests(system, false);
+ }
+ {
+ std::cout << "\nBall Chain (Euler angles):\n" << std::endl;
+ MultibodySystem system;
+ createBallChain(system);
+ runAllTests(system, true);
+ }
+ {
+ std::cout << "\nGimbal Chain:\n" << std::endl;
+ MultibodySystem system;
+ createGimbalChain(system);
+ runAllTests(system);
+ }
+
+ {
+ std::cout << "\nPin Tree:\n" << std::endl;
+ MultibodySystem system;
+ createPinTree(system);
+ runAllTests(system);
+ }
+ {
+ std::cout << "\nBall Tree:\n" << std::endl;
+ MultibodySystem system;
+ createBallTree(system);
+ runAllTests(system);
+ }
+
+
+ std::cout << "Total time:\n";
+ std::cout << " process CPU=" << cpuTime()-startCpu << "s\n";
+ std::cout << " thread CPU =" << threadCpuTime()-startThread << "s\n";
+ std::cout << " real time =" << realTime()-startClock << "s\n";
+}
\ No newline at end of file
diff --git a/Simbody/tests/adhoc/TestPLUS_SingleBrick.cpp b/Simbody/tests/adhoc/TestPLUS_SingleBrick.cpp
new file mode 100644
index 0000000..d326ddd
--- /dev/null
+++ b/Simbody/tests/adhoc/TestPLUS_SingleBrick.cpp
@@ -0,0 +1,2778 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) - Test PLUS impact model with a single brick *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013-2014 Stanford University and the Authors. *
+ * Authors: Thomas Uchida *
+ * Contributors: Michael Sherman *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* Test of Poisson-Lankarani-Uchida-Sherman (PLUS) impact model. A brick falls
+under the force of gravity and spheres attached to its vertices collide with the
+horizontal ground plane. Exhaustive search and successive pruning strategies are
+uesd by the position projection and impact handlers to determine suitable active
+sets. In this simple test, repeated impacts are used in place of an explicit
+contact handler. */
+
+#include "Simbody.h"
+using namespace SimTK;
+using std::cout;
+using std::endl;
+
+// Unique index types to avoid confusing the brick vertex indices (0..7) with
+// indices of proximal points (of which there will be 4 or fewer).
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(BrickVertexIndex);
+SimTK_DEFINE_UNIQUE_INDEX_TYPE(ProximalPointIndex);
+
+
+//==============================================================================
+// PARAMETERS
+//==============================================================================
+const bool SetVizModeToRealtime = false; //Enable when saving movie.
+
+const bool PauseBeforeClosingVizWindow = true;
+const bool PauseBeforeAndAfterProjection = false;
+const bool PauseBeforeAndAfterImpact = false;
+const bool PauseAfterEachImpactInterval = false;
+const bool PauseAfterEachActiveSetCandidate = false;
+
+const bool PrintBasicInfo = true;
+const bool PrintSystemEnergy = false;
+const bool PrintDebugInfoProjection = false;
+const bool PrintDebugInfoImpact = false;
+const bool PrintDebugInfoActiveSetSearch = false;
+const bool PrintDebugInfoStepLength = false;
+
+const bool ExhaustiveSearchProjection = false;
+const bool ExhaustiveSearchImpact = false;
+const bool CompareProjectionStrategies = false;
+const bool CompareImpactStrategies = false;
+
+const bool DebugStepLengthCalculator = false;
+
+const bool RunTestsOnePoint = true;
+const bool RunTestsTwoPoints = true;
+const bool RunTestsFourPoints = true;
+const bool RunTestsLongSimulations = true;
+
+// Notes:
+// 1. SlidingDirStepLength must be sufficiently small such that, if a sliding
+// interval advancing by this amount causes a sliding direction reversal,
+// then |v_t| < MaxStickingTangVel (and the point is actually rolling).
+
+const Real TolProjectQ = 1.0e-6; //Accuracy used by projectQ().
+const Real TolPositionFuzziness = 1.0e-4; //Expected position tolerance.
+const Real TolVelocityFuzziness = 1.0e-5; //Expected velocity tolerance.
+const Real TolReliableDirection = 1.0e-4; //Whether to trust v_t direction.
+const Real TolMaxDifDirIteration = 0.05; //Slip direction within 2.9 degrees.
+const Real MinMeaningfulImpulse = 1.0e-6; //Smallest acceptable impulse.
+const Real MaxStickingTangVel = 1.0e-1; //Cannot stick above this velocity.
+const Real MaxSlidingDirChange = 0.5; //Direction can change 28.6 degrees.
+const Real SlidingDirStepLength = 1.0e-4; //Used to determine slip directions.
+const int MaxIterSlipDirection = 10; //Iteration limit for directions.
+
+const Real IntegAccuracy = 1.0e-8;
+const Real MaxStepSize = 1.0e-3;
+const int DesiredFPS = 30;
+const int DrawEveryN = int(1.0/DesiredFPS/MaxStepSize + 0.5);
+const Vec3 BrickColor = Blue;
+const Vec3 SphereColor = Red;
+
+
+//==============================================================================
+// FREE UNILATERAL BRICK
+//==============================================================================
+// Establish a free brick with a unilaterally-constrained sphere attached to
+// each of its vertices. Each sphere can impact the horizontal ground plane at
+// Z=0. All spheres are assumed to have the same radius and material properties.
+class FreeUnilateralBrick : public MobilizedBody::Free {
+public:
+
+ //--------------------------------------------------------------------------
+ // Constructors
+ //--------------------------------------------------------------------------
+ FreeUnilateralBrick() : Mobod::Free() {}
+ FreeUnilateralBrick(Mobod& parent, const Transform& X_PF,
+ const Body& bodyInfo, const Transform& X_BM,
+ const Vec3& brickHalfLengths, Real sphereRadii,
+ Real muDyn, Real vMinRebound, Real vPlasticDeform,
+ Real minCOR);
+
+ //--------------------------------------------------------------------------
+ // Getters
+ //--------------------------------------------------------------------------
+ Real get_muDyn() const {return m_muDyn;}
+ Real get_vMinRebound() const {return m_vMinRebound;}
+ Real get_vPlasticDeform() const {return m_vPlasticDeform;}
+ Real get_minCOR() const {return m_minCOR;}
+ int get_numVertices() const {return (int)m_vertices.size();}
+
+ //--------------------------------------------------------------------------
+ // Temporary point-in-plane constraints
+ //--------------------------------------------------------------------------
+ Real get_pipConstraintHeightInGround(const State& s,
+ const BrickVertexIndex i) const;
+ void set_pipConstraintLocation(const State& s, const BrickVertexIndex i,
+ const Vec3& positionInGround);
+ void enablePipConstraint(State& s, const BrickVertexIndex i) const;
+ void disableAllPipConstraints(State& s) const;
+
+ //--------------------------------------------------------------------------
+ // Temporary ball constraints
+ //--------------------------------------------------------------------------
+ MultiplierIndex get_ballConstraintFirstIndex(const State& s,
+ const BrickVertexIndex i) const;
+ void set_ballConstraintLocation(const State& s, const BrickVertexIndex i,
+ const Vec3& positionInGround);
+ void enableBallConstraint(State& s, const BrickVertexIndex i) const;
+ void disableAllBallConstraints(State& s) const;
+
+ //--------------------------------------------------------------------------
+ // Position-level information
+ //--------------------------------------------------------------------------
+ // Find the location of the lowest point on the ith sphere, measured from
+ // the ground origin and resolved in the ground frame.
+ Vec3 findLowestPointLocationInGround(const State& s,
+ const BrickVertexIndex i) const;
+
+ // Find the location of the lowest point on the ith sphere, measured from
+ // the body's origin and resolved in the body's frame.
+ Vec3 findLowestPointLocationInBodyFrame(const State& s,
+ const BrickVertexIndex i) const;
+
+ // Assemble an array containing the location of the lowest point on each
+ // sphere, measured from the ground origin and resolved in the ground frame.
+ void findAllLowestPointLocationsInGround(const State& s,
+ Array_<Vec3,BrickVertexIndex>& lowestPointLocationsInG) const;
+
+ // Check for interpenetration of the brick with the ground plane.
+ bool isBrickInterpenetrating(const Array_<Vec3,BrickVertexIndex>&
+ lowestPointLocationsInG) const;
+ bool isBrickInterpenetrating(const State& s) const;
+
+ // Check for proximity of a sphere to the ground plane.
+ bool isPointProximal(const Vec3& lowestPointLocationInG) const;
+
+ // Assemble an array containing the indices of the proximal points.
+ void findProximalPointIndices(
+ const Array_<Vec3,BrickVertexIndex>& lowestPointLocationsInG,
+ Array_<BrickVertexIndex,ProximalPointIndex>& proximalPointIndices)
+ const;
+
+ //--------------------------------------------------------------------------
+ // Velocity-level information
+ //--------------------------------------------------------------------------
+ // Find the velocity of the lowest point on the ith sphere resolved in the
+ // ground frame.
+ Vec3 findLowestPointVelocityInGround(const State& s,
+ const BrickVertexIndex i) const;
+
+ // Find the angle between the global X-axis and the tangential velocity
+ // vector, in [-pi, pi]. Returns NaN if the magnitude of the tangential
+ // velocity is too small to provide a reliable direction.
+ Real findTangentialVelocityAngle(const Vec3& vel) const;
+
+ // Check for impact of the brick with the ground plane.
+ bool isBrickImpacting(const Array_<Vec3,ProximalPointIndex>&
+ proximalPointVelocities) const;
+
+private:
+ Vec3 m_brickHalfLengths;
+ Real m_sphereRadii;
+ Real m_muDyn;
+ Real m_vMinRebound;
+ Real m_vPlasticDeform;
+ Real m_minCOR;
+ Array_<Vec3,BrickVertexIndex> m_vertices;
+
+ // PointInPlane and Ball constraints are created for each sphere. The
+ // locations of the constraints corresponding to the proximal points are
+ // adjusted by the PositionProjecter and Impacter constructors.
+ Array_<Constraint::PointInPlane,BrickVertexIndex> m_pipConstraints;
+ Array_<Constraint::Ball,BrickVertexIndex> m_ballConstraints;
+};
+
+
+//==============================================================================
+// POSITION PROJECTER
+//==============================================================================
+class PositionProjecter {
+public:
+
+ //--------------------------------------------------------------------------
+ // Constructor
+ //--------------------------------------------------------------------------
+ PositionProjecter(MultibodySystem& mbs, FreeUnilateralBrick& brick,
+ const State& s0,
+ const Array_<Vec3,BrickVertexIndex>& positionsInG);
+
+ //--------------------------------------------------------------------------
+ // Resolve position-level violations
+ //--------------------------------------------------------------------------
+ // Try projecting all combinations of proximal points; select the projection
+ // that resolves all violations while requiring the smallest change in Q.
+ // Updates state and returns active set.
+ Array_<ProximalPointIndex> projectPositionsExhaustive(State& s) const;
+
+ // Begin by projecting using the constraints associated with all proximal
+ // points; successively prune the constraint associated with the most
+ // distant proximal point until the projection is successful. Updates state
+ // and returns active set.
+ Array_<ProximalPointIndex> projectPositionsPruning(State& s) const;
+
+private:
+ MultibodySystem& m_mbs;
+ FreeUnilateralBrick& m_brick;
+ Array_<BrickVertexIndex,ProximalPointIndex> m_proximalPointIndices;
+
+ // Assemble all combinations of 1 or more indices in [0, sizeOfSet]. Creates
+ // an array of 2^sizeOfSet-1 index arrays (indices into proximal positions).
+ void createProximalIndexArrays(int sizeOfSet,
+ Array_<Array_<ProximalPointIndex> >& arrayOfIndexArrays) const;
+
+ // Try projecting positions using the provided combination of constraint
+ // indices. Return the 2-norm distance between the original and final Q (or
+ // SimTK::Infinity if projection was unsuccessful).
+ Real evaluateProjection(State& sTemp, const State& sOrig,
+ const Array_<ProximalPointIndex>& indexArray) const;
+
+ // Find and display new proximal points. For debugging only.
+ void displayNewProximalPoints(State& s) const;
+};
+
+
+//==============================================================================
+// IMPACTER
+//==============================================================================
+class Impacter {
+public:
+
+ //--------------------------------------------------------------------------
+ // Constructor
+ //--------------------------------------------------------------------------
+ Impacter(MultibodySystem& mbs, FreeUnilateralBrick& brick, const State& s0,
+ const Array_<Vec3,BrickVertexIndex>& allPositionsInG,
+ const Array_<BrickVertexIndex,ProximalPointIndex>&
+ proximalPointIndices);
+
+ //--------------------------------------------------------------------------
+ // Perform one complete impact
+ //--------------------------------------------------------------------------
+ // Evaluate all active set candidates for each impact interval, selecting
+ // the most fit candidate for each interval. Returns a string describing the
+ // active set used for each interval.
+ std::string performImpactExhaustive(State& s,
+ Array_<Vec3,ProximalPointIndex>& proximalVelsInG,
+ Array_<bool,ProximalPointIndex>& hasRebounded,
+ Real& totalEnergyDissipated) const;
+
+ // Begin by impacting using the constraints associated with all proximal
+ // points; successively prune the constraint associated with the worst
+ // violation until either no violations occur or the active set is empty (in
+ // which case the least objectionable active set candidate is retained).
+ // Returns a string describing the active set used for each interval.
+ std::string performImpactPruning(State& s,
+ Array_<Vec3,ProximalPointIndex>& proximalVelsInG,
+ Array_<bool,ProximalPointIndex>& hasRebounded,
+ Real& totalEnergyDissipated) const;
+
+private:
+ enum ImpactPhase {Compression, Restitution};
+ enum TangentialState {Observing=0, Sliding, Rolling};
+ enum SolutionCategory {
+ SolCat_NoViolations=0, //Ideal
+ SolCat_ActiveConstraintDoesNothing, //Non-minimal active set
+ SolCat_RestitutionImpulsesIgnored, //Simultaneity is lost
+ SolCat_TangentialVelocityTooLargeToStick, //Violates friction rule
+ SolCat_StickingImpulseExceedsStictionLimit, //Violates friction limit
+ SolCat_GroundAppliesAttractiveImpulse, //Violates physical law
+ SolCat_NegativePostCompressionNormalVelocity, //Fatal: solution is wrong
+ SolCat_NoImpulsesApplied, //Fatal: no progress made
+ SolCat_UnableToResolveUnknownSlipDirection, //Fatal: direction unknown
+ SolCat_MinStepCausesSlipDirectionReversal, //Fatal: direction unknown
+ SolCat_NotEvaluated,
+ SolCat_WorstCatWithNoSeriousViolations //These categories can be
+ = SolCat_RestitutionImpulsesIgnored, //accepted without issue.
+ SolCat_WorstCatWithSomeUsableSolution //Use these categories
+ = SolCat_GroundAppliesAttractiveImpulse //only if no other choice.
+ };
+
+ struct ActiveSetCandidate {
+ Array_<int,ProximalPointIndex> tangentialStates;
+ Vector systemVelocityChange;
+ Vector localImpulses;
+ SolutionCategory solutionCategory;
+ Real fitness;
+ ProximalPointIndex worstConstraint;
+ };
+
+ //--------------------------------------------------------------------------
+ // Private methods: display
+ //--------------------------------------------------------------------------
+ // Display active set candidate in human-readable form.
+ void printFormattedActiveSet(std::ostream& stream,
+ const Array_<int,ProximalPointIndex>& tangentialStates,
+ const std::string& prefix = "") const;
+ std::string printFormattedActiveSet(
+ const Array_<int,ProximalPointIndex>& tangentialStates,
+ const std::string& prefix = "") const;
+
+ // Map an enumerated solution category to a descriptive string.
+ const char* getSolutionCategoryDescription(SolutionCategory solCat) const;
+
+ // Display information about the evaluation of an active set candidate.
+ void printActiveSetInfo(const ActiveSetCandidate& asc) const;
+
+ //--------------------------------------------------------------------------
+ // Private methods: helper functions
+ //--------------------------------------------------------------------------
+ // Add addThis to vec in base N; return false on overflow.
+ bool addInBaseN(int N, Array_<int,ProximalPointIndex>& vec, int addThis)
+ const;
+
+ // Create 3^n-1 arrays of size n, where n is the number of proximal points,
+ // and each element corresponds to one of the three TangentialStates.
+ void initializeActiveSetCandidateArray(
+ Array_<ActiveSetCandidate>& activeSetCandidates) const;
+
+ // Initialize active set candidate for pruning search. Begin with maximal
+ // active set, but disallow sticking at points whose tangential velocity
+ // magnitude is too large.
+ void initializeActiveSetCandidate(const State& s, ActiveSetCandidate& asc)
+ const;
+
+ // Clear data associated with active set candidate; called before each
+ // active set candidate is evaluated in pruning algorithm.
+ void resetActiveSetCandidate(ActiveSetCandidate& asc) const;
+
+ // Clear data associated with active set candidates; called before each
+ // impact interval begins in exhaustive search algorithm.
+ void resetActiveSetCandidateArray(
+ Array_<ActiveSetCandidate>& activeSetCandidates) const;
+
+ // Return row index associated with first component of ith constraint.
+ int getIndexOfFirstMultiplier(const State& s,
+ const ProximalPointIndex i) const;
+
+ // Find the absolute difference between two angles in [-pi, pi] radians,
+ // accounting for multiples of 2*pi; returns a value in [0, pi].
+ Real calcAbsDiffBetweenAngles(Real a, Real b) const;
+
+ // Given point P and line segment AB, find the point closest to P that lies
+ // on AB, which we call Q. Returns stepLength, the ratio AQ:AB. In our case,
+ // P is the origin and AB is the line segment connecting the initial and
+ // final tangential velocity vectors.
+ Real calcSlidingStepLengthToOrigin(const Vec2& A, const Vec2& B, Vec2& Q)
+ const;
+ Real calcSlidingStepLengthToOrigin(const Vec3& A, const Vec3& B, Vec3& Q)
+ const;
+
+ // Given vectors A and B, find step length alpha such that the angle between
+ // A and A+alpha*(B-A) is MaxSlidingDirChange. The solutions were generated
+ // in Maple using the law of cosines, then exported as optimized code.
+ // Returns step length greater than 1 if the angle between vectors A and B
+ // exceeds MaxSlidingDirChange.
+ Real calcSlidingStepLengthToMaxChange(const Vec2& A, const Vec2& B) const;
+ Real calcSlidingStepLengthToMaxChange(const Vec3& A, const Vec3& B) const;
+
+ // Determine whether active set candidate is empty.
+ bool isActiveSetCandidateEmpty(const ActiveSetCandidate& asc) const;
+
+ // Determine whether any sliding directions are undefined.
+ bool isAnySlidingDirectionUndefined(const Array_<Real>& slidingDirections)
+ const;
+
+ //--------------------------------------------------------------------------
+ // Private methods: calculators
+ //--------------------------------------------------------------------------
+ // Calculate coefficient of restitution.
+ Real calcCOR(Real vNormal) const;
+
+ // Generate and solve a linear system of equations to determine the system
+ // velocity changes and impulses; assign to ActiveSetCandidate. Resolves
+ // unknown sliding directions.
+ void generateAndSolveLinearSystem(const State& s0,
+ const ImpactPhase impactPhase,
+ const Vector& restitutionImpulses,
+ ActiveSetCandidate& asc) const;
+
+ // Determine category of linear system solution and calculate fitness value
+ // for active set candidate (if it has not already been categorized). Assign
+ // worst applicable disqualification category to ActiveSetCandidate.
+ void evaluateLinearSystemSolution(const State& s,
+ const ImpactPhase impactPhase,
+ const Vector& restitutionImpulses,
+ ActiveSetCandidate& asc) const;
+
+ // Determine the interval step length for the selected active set. Searches
+ // for the maximum step that can be taken while ensuring each sliding point
+ // undergoes a direction change no greater than the maximum permitted in a
+ // single interval.
+ Real calculateIntervalStepLength(const State& s0,
+ const Array_<Vec3>& currVels,
+ const ActiveSetCandidate& asc,
+ int intervalCtr) const;
+
+ //--------------------------------------------------------------------------
+ // Member variables
+ //--------------------------------------------------------------------------
+ MultibodySystem& m_mbs;
+ FreeUnilateralBrick& m_brick;
+ Array_<BrickVertexIndex,ProximalPointIndex> m_proximalPointIndices;
+};
+
+
+//==============================================================================
+// PAUSABLE VISUALIZER
+//==============================================================================
+class PausableVisualizer : public Visualizer {
+public:
+ PausableVisualizer(const MultibodySystem& system) : Visualizer(system) {}
+
+ void reportAndPause(const State& s, const std::string& msg) const
+ {
+ report(s);
+ printf("t=%0.3f %s", s.getTime(), msg.c_str());
+ char trash = getchar();
+ }
+};
+
+
+//==============================================================================
+// BODY WATCHER
+//==============================================================================
+// Prior to rendering each frame, point the camera at the given body's origin.
+// Adapted from TimsBox.cpp.
+class BodyWatcher : public Visualizer::FrameController {
+public:
+ explicit BodyWatcher(const MobilizedBody& body) : m_body(body) {}
+
+ void generateControls(const Visualizer& viz,
+ const State& state,
+ Array_< DecorativeGeometry >& geometry) OVERRIDE_11
+ {
+ const Vec3 Bo = m_body.getBodyOriginLocation(state);
+ const Vec3 p_GC = Bo + Vec3(0,4,2);
+ const Rotation R1(-SimTK::Pi/3, XAxis);
+ const Rotation R2(SimTK::Pi, ZAxis);
+ viz.setCameraTransform(Transform(R1*R2, p_GC));
+ }
+private:
+ const MobilizedBody m_body;
+};
+
+
+//==============================================================================
+// Function: CREATE MULTIBODY SYSTEM
+//==============================================================================
+static void createMultibodySystem(MultibodySystem& mbs,
+ FreeUnilateralBrick& brick,
+ Real muDyn, Real minCOR)
+{
+ // Set up multibody system.
+ SimbodyMatterSubsystem matter(mbs);
+ mbs.setUpDirection(ZAxis);
+
+ // Physical parameters.
+ const Vec3 brickHalfLengths(0.2, 0.3, 0.4);
+ const Real sphereRadii = 0.1;
+ const Real brickMass = 2.0;
+ const Real vMinRebound = 1.0e-6;
+ const Real vPlasticDeform = 0.1;
+
+ // Configure brick.
+ const Inertia brickInertia(brickMass*UnitInertia::brick(brickHalfLengths));
+ Body::Rigid brickInfo(MassProperties(brickMass, Vec3(0), brickInertia));
+ DecorativeBrick brickGeom(brickHalfLengths);
+ brickGeom.setColor(BrickColor);
+ brickInfo.addDecoration(brickGeom);
+
+ // Add brick to multibody system.
+ brick = FreeUnilateralBrick(mbs.updMatterSubsystem().Ground(), Vec3(0),
+ brickInfo, Vec3(0), brickHalfLengths,
+ sphereRadii, muDyn, vMinRebound, vPlasticDeform,
+ minCOR);
+}
+
+
+//==============================================================================
+// Function: PRINT HORIZONTAL RULE
+//==============================================================================
+static void printHorizontalRule(int spacesTop, int spacesBottom,
+ char ruleCharacter, const std::string& msg = "")
+{
+ for (int i=0; i<spacesTop; ++i) printf("\n");
+ for (int i=0; i<60; ++i) cout << ruleCharacter;
+ cout << " " << msg << endl;
+ for (int i=0; i<spacesBottom; ++i) printf("\n");
+}
+
+
+//==============================================================================
+// Function: SIMULATE MULTIBODY SYSTEM
+//==============================================================================
+static void simulateMultibodySystem(const std::string& description,
+ const Vector& initialQ,
+ const Vector& initialU,
+ Real simDuration, Real muDyn, Real minCOR)
+{
+ printHorizontalRule(3,0,'=');
+ cout << description << endl;
+ printHorizontalRule(0,1,'=');
+
+ // Create the multibody system.
+ MultibodySystem mbs;
+ FreeUnilateralBrick brick;
+ createMultibodySystem(mbs, brick, muDyn, minCOR);
+
+ // Add gravity.
+ GeneralForceSubsystem forces(mbs);
+ Force::Gravity gravity(forces, mbs.updMatterSubsystem(), -ZAxis, 9.81);
+
+ // Set up the visualizer.
+ PausableVisualizer viz(mbs);
+ viz.setShowSimTime(true).setShowFrameRate(true);
+ if (SetVizModeToRealtime)
+ viz.setMode(Visualizer::RealTime);
+ viz.addFrameController(new BodyWatcher(brick));
+ mbs.updMatterSubsystem().setShowDefaultGeometry(false);
+
+ // Initialize.
+ State s0 = mbs.realizeTopology();
+ mbs.realize(s0, Stage::Dynamics);
+ s0.setQ(initialQ);
+ s0.setU(initialU);
+ mbs.realize(s0, Stage::Dynamics);
+ const Real initialEnergy = mbs.calcEnergy(s0);
+
+ // Set up integrator.
+ RungeKutta3Integrator integ(mbs);
+ integ.setAccuracy(IntegAccuracy);
+ integ.setAllowInterpolation(false);
+ integ.initialize(s0);
+ const int totalSteps = int(simDuration/MaxStepSize + 0.5);
+
+ // Simulate.
+ printf("Simulating for %0.1f seconds (%d steps of size %0.3f)\n",
+ simDuration, totalSteps, MaxStepSize);
+ viz.reportAndPause(s0, "Press <Enter> to simulate...");
+
+ for (int stepNum=1; stepNum<totalSteps; ++stepNum) {
+
+ //----------------------------- INTEGRATE ------------------------------
+ // Advance time by MaxStepSize. Might require multiple internal steps.
+ const Real tNext = stepNum*MaxStepSize;
+ do {integ.stepBy(MaxStepSize);} while (integ.getTime() < tNext);
+
+ // The state being used by the integrator.
+ State& s = integ.updAdvancedState();
+
+ //----------------- RESOLVE POSITION-LEVEL VIOLATIONS ------------------
+ // Project positions to resolve interpenetration. The PositionProjecter
+ // guarantees that no points are below -TolPositionFuzziness on exit.
+ mbs.realize(s, Stage::Position);
+
+ // Calculate the position of the lowest point on each sphere.
+ Array_<Vec3,BrickVertexIndex> preProjectionPos;
+ brick.findAllLowestPointLocationsInGround(s, preProjectionPos);
+
+ // Can impact only if interpenetration occurred.
+ bool interpenetrationDetected = false;
+
+ if (brick.isBrickInterpenetrating(preProjectionPos)) {
+ interpenetrationDetected = true;
+
+ if (PrintSystemEnergy) {
+ mbs.realize(s, Stage::Dynamics);
+ cout << " [normalized energy before project] "
+ << mbs.calcEnergy(s)/initialEnergy << endl;
+ }
+ if (PauseBeforeAndAfterProjection) {
+ cout << " [pos0] " << s.getQ() << endl;
+ viz.reportAndPause(s, "Ready to project positions");
+ }
+
+ PositionProjecter positionProjecter(mbs,brick,s,preProjectionPos);
+ if (CompareProjectionStrategies) {
+
+ // Run exhaustive search and successive pruning position
+ // projection handlers, and report if the active sets differ.
+ // Proceed using the solution from the successive pruning
+ // algorithm.
+
+ State sExhaustive(s);
+ Array_<ProximalPointIndex> indexArrayExhaustive =
+ positionProjecter.projectPositionsExhaustive(sExhaustive);
+
+ Array_<ProximalPointIndex> indexArrayPruning =
+ positionProjecter.projectPositionsPruning(s);
+
+ if (!(indexArrayExhaustive == indexArrayPruning))
+ cout << " [pos] Exhaustive: " << indexArrayExhaustive
+ << "\n [pos] Pruning: " << indexArrayPruning
+ << endl;
+
+ } else if (ExhaustiveSearchProjection)
+ positionProjecter.projectPositionsExhaustive(s);
+ else
+ positionProjecter.projectPositionsPruning(s);
+
+ if (PrintSystemEnergy) {
+ mbs.realize(s, Stage::Dynamics);
+ cout << " [normalized energy after project] "
+ << mbs.calcEnergy(s)/initialEnergy << endl;
+ }
+ if (PauseBeforeAndAfterProjection) {
+ cout << " [pos1] " << s.getQ() << endl;
+ viz.reportAndPause(s, "Position projection complete");
+ }
+ }
+
+ //----------------- RESOLVE VELOCITY-LEVEL VIOLATIONS ------------------
+ // Perform impacts to resolve negative normal velocities of proximal
+ // points. The Impacter guarantees that no proximal points have normal
+ // velocities less than -TolVelocityFuzziness on exit.
+
+ // The impact handler is triggered only on actual interpenetration
+ // (i.e., pos[ZAxis] < 0), not just on entering the fuzzy proximal zone
+ // (i.e., pos[ZAxis] < TolPositionFuzziness). This way, other nearly-
+ // penetrating points will have the chance to move into the proximal
+ // zone and be included in the impact event, which will help preserve
+ // symmetry and simultaneity.
+ if (interpenetrationDetected) {
+ mbs.realize(s, Stage::Velocity);
+
+ // Calculate all positions after projection.
+ Array_<Vec3,BrickVertexIndex> allPositionsInG;
+ brick.findAllLowestPointLocationsInGround(s, allPositionsInG);
+
+ // Find the indices of the proximal points.
+ Array_<BrickVertexIndex,ProximalPointIndex> proximalPointIndices;
+ brick.findProximalPointIndices(allPositionsInG,
+ proximalPointIndices);
+
+ // Calculate the velocity of each proximal point.
+ Array_<Vec3,ProximalPointIndex>
+ proximalVelsInG(proximalPointIndices.size());
+ for (ProximalPointIndex i(0);
+ i<(int)proximalPointIndices.size(); ++i)
+ proximalVelsInG[i] = brick.findLowestPointVelocityInGround(s,
+ proximalPointIndices[i]);
+
+ // An impact event occurs only if at least one negative normal
+ // velocity exceeds the velocity tolerance (i.e., vel[ZAxis] <
+ // -TolVelocityFuzziness).
+ if (brick.isBrickImpacting(proximalVelsInG)) {
+
+ // Record which points have already undergone a restitution
+ // phase; set coefficient of restitution to zero for follow-up
+ // impacts at these points.
+ Array_<bool,ProximalPointIndex>
+ hasRebounded(proximalPointIndices.size());
+ for (ProximalPointIndex i(0);
+ i<(int)proximalPointIndices.size(); ++i)
+ hasRebounded[i] = false;
+
+ // Process impacts until all proximal points have non-negative
+ // normal velocities.
+ while (brick.isBrickImpacting(proximalVelsInG)) {
+
+ Real normEnergyBefore = SimTK::NaN;
+ if (PrintSystemEnergy) {
+ mbs.realize(s, Stage::Dynamics);
+ normEnergyBefore = mbs.calcEnergy(s)/initialEnergy;
+ cout << " [normalized energy before impact ] "
+ << normEnergyBefore << endl;
+ }
+ if (PauseBeforeAndAfterImpact) {
+ cout << " [vel0] " << s.getU() << endl;
+ viz.reportAndPause(s, "Ready to perform impact");
+ }
+
+ // Perform one complete impact consisting of a compression
+ // phase and (if necessary) a restitution phase.
+ Impacter impacter(mbs, brick, s, allPositionsInG,
+ proximalPointIndices);
+ Real energyDissipated = SimTK::NaN;
+ if (CompareImpactStrategies) {
+
+ // Run exhaustive search and successive pruning impact
+ // handlers, and report if the active sets differ.
+ // Proceed using the solution from the successive
+ // pruning algorithm.
+
+ State sExhaustive(s);
+ Array_<Vec3,ProximalPointIndex>
+ proximalVelsInGExhaustive(proximalVelsInG);
+ Array_<bool,ProximalPointIndex>
+ hasReboundedExhaustive(hasRebounded);
+ std::string trajExhaustive =
+ impacter.performImpactExhaustive(sExhaustive,
+ proximalVelsInGExhaustive, hasReboundedExhaustive,
+ energyDissipated);
+
+ std::string trajPruning =
+ impacter.performImpactPruning(s, proximalVelsInG,
+ hasRebounded, energyDissipated);
+
+ if (!(trajExhaustive == trajPruning))
+ cout << " [vel] Exhaustive: " << trajExhaustive
+ << "\n [vel] Pruning: " << trajPruning
+ << endl;
+
+ } else if (ExhaustiveSearchImpact)
+ impacter.performImpactExhaustive(s, proximalVelsInG,
+ hasRebounded, energyDissipated);
+ else
+ impacter.performImpactPruning(s, proximalVelsInG,
+ hasRebounded, energyDissipated);
+
+ if (PrintSystemEnergy) {
+ mbs.realize(s, Stage::Dynamics);
+ const Real normEnergyAfter = mbs.calcEnergy(s) /
+ initialEnergy;
+ const Real normEnergyDiss = energyDissipated /
+ initialEnergy;
+ cout << " [normalized energy after impact ] "
+ << normEnergyAfter << " (" << normEnergyDiss
+ << " dissipated, error = "
+ << std::abs(normEnergyBefore - normEnergyAfter
+ - normEnergyDiss) << ")" << endl;
+ }
+ if (PauseBeforeAndAfterImpact) {
+ cout << " [vel1] " << s.getU() << endl;
+ viz.reportAndPause(s, "Impact complete");
+ }
+
+ } //end while processing impacts
+ } //end if impacting
+ } //end if able to impact
+
+ // Output a frame to the visualizer, if necessary.
+ if (stepNum%DrawEveryN == 0 || stepNum == 1 || stepNum == totalSteps-1)
+ viz.report(s);
+
+ } //end simulation loop
+
+ printf("Simulation complete.\n");
+ if (PauseBeforeClosingVizWindow) {
+ printf("Press <Enter> to continue...\n");
+ char trash = getchar();
+ }
+ viz.shutdown();
+}
+
+
+//==============================================================================
+// MAIN
+//==============================================================================
+int main() {
+ try {
+
+ // Perform a series of simulations using different initial conditions.
+
+ //---------------------- (a) One point impacting -----------------------
+ if (RunTestsOnePoint) {
+ Vector initQ = Vector(Vec7(1,0,0,0, 0,1,0.8));
+ initQ(0,4) = Vector(Quaternion(Rotation(SimTK::Pi/4, XAxis) *
+ Rotation(SimTK::Pi/6, YAxis))
+ .asVec4());
+ Vector initU = Vector(Vec6(0,0,0, 0,0,6));
+
+ simulateMultibodySystem("Test A1: One point, no v_tangential",
+ initQ, initU, 1.8, 0.6, 0.5);
+ initU[3] = 0.5;
+ simulateMultibodySystem("Test A2: One point, small v_tangential",
+ initQ, initU, 1.8, 0.6, 0.5);
+ initU[3] = -0.5;
+ simulateMultibodySystem("Test A3: One point, small v_tangential",
+ initQ, initU, 1.8, 0.6, 0.5);
+ initQ[6] = 1.5;
+ initU[3] = 5.0;
+ initU[5] = -4.0;
+ simulateMultibodySystem("Test A4: One point, large v_tangential",
+ initQ, initU, 1.0, 0.3, 1.0);
+ initU[3] = -5.0;
+ simulateMultibodySystem("Test A5: One point, large v_tangential",
+ initQ, initU, 1.0, 0.3, 1.0);
+ }
+
+ //---------------------- (b) Two points impacting ----------------------
+ if (RunTestsTwoPoints) {
+ Vector initQ = Vector(Vec7(1,0,0,0, 0,1,0.8));
+ initQ(0,4) = Vector(Quaternion(Rotation(SimTK::Pi/4, XAxis))
+ .asVec4());
+ Vector initU = Vector(Vec6(0,0,0, 0,0,6));
+
+ simulateMultibodySystem("Test B1: Two points, no v_tangential",
+ initQ, initU, 1.8, 0.6, 0.5);
+ initU[4] = -1.0;
+ simulateMultibodySystem("Test B2: Two points, small v_tangential",
+ initQ, initU, 1.8, 0.6, 0.5);
+ }
+
+ //--------------------- (c) Four points impacting ----------------------
+ if (RunTestsFourPoints) {
+ Vector initQ = Vector(Vec7(1,0,0,0, 0,1,0.8));
+ Vector initU = Vector(Vec6(0,0,0, 0,0,6));
+
+ simulateMultibodySystem("Test C1: Four points, no v_tangential",
+ initQ, initU, 1.8, 0.6, 0.5);
+ initU[3] = 0.5;
+ simulateMultibodySystem("Test C2: Four points, small v_tangential",
+ initQ, initU, 1.8, 0.6, 0.5);
+ }
+
+ //------------------------ (d) Long simulation -------------------------
+ if (RunTestsLongSimulations) {
+ Vector initQ = Vector(Vec7(1,0,0,0, 0,1,1.5));
+ initQ(0,4) = Vector(Quaternion(Rotation(SimTK::Pi/4, XAxis) *
+ Rotation(SimTK::Pi/6, YAxis))
+ .asVec4());
+ Vector initU = Vector(Vec6(0,0,0, -5,0,-4));
+
+ simulateMultibodySystem("Test D1: low muDyn, high minCOR",
+ initQ, initU, 5.5, 0.3, 1.0);
+ simulateMultibodySystem("Test D2: low muDyn, low minCOR",
+ initQ, initU, 3.0, 0.3, 0.5);
+ simulateMultibodySystem("Test D3: high muDyn, high minCOR",
+ initQ, initU, 3.8, 0.8, 1.0);
+ simulateMultibodySystem("Test D4: high muDyn, low minCOR",
+ initQ, initU, 3.5, 0.8, 0.5);
+ }
+
+ } catch (const std::exception& e) {
+ cout << "ERROR: " << e.what() << endl;
+ return 1;
+ }
+ return 0;
+}
+
+
+//==============================================================================
+// Implementation: FREE UNILATERAL BRICK
+//==============================================================================
+FreeUnilateralBrick::
+FreeUnilateralBrick(Mobod& parent, const Transform& X_PF, const Body& bodyInfo,
+ const Transform& X_BM, const Vec3& brickHalfLengths,
+ Real sphereRadii, Real muDyn, Real vMinRebound,
+ Real vPlasticDeform, Real minCOR)
+ : Mobod::Free(parent, X_PF, bodyInfo, X_BM)
+{
+ // Ensure parameters are physically reasonable.
+ m_brickHalfLengths = brickHalfLengths;
+ m_sphereRadii = std::max(0.0, sphereRadii);
+ m_muDyn = std::max(0.0, muDyn);
+ m_vPlasticDeform = std::max(0.0, vPlasticDeform);
+ m_vMinRebound = clamp(0.0, vMinRebound, m_vPlasticDeform);
+ m_minCOR = clamp(0.0, minCOR, 1.0);
+
+ // Add spheres to display geometry.
+ DecorativeSphere sphereGeom(m_sphereRadii);
+ sphereGeom.setColor(SphereColor);
+
+ for (int i=-1; i<=1; i+=2)
+ for (int j=-1; j<=1; j+=2)
+ for (int k=-1; k<=1; k+=2) {
+ Vec3 vertex = Vec3(i,j,k).elementwiseMultiply(m_brickHalfLengths);
+ m_vertices.push_back(vertex);
+ addBodyDecoration(Transform(vertex), sphereGeom);
+ }
+
+ // Create one PointInPlane and one Ball constraint for each sphere.
+ Mobod ground = getMatterSubsystem().getGround();
+ for (BrickVertexIndex i(0); i<(int)m_vertices.size(); ++i) {
+ Constraint::PointInPlane pip(ground, ZAxis, 0, *this, Vec3(0));
+ pip.setDisabledByDefault(true);
+ m_pipConstraints.push_back(pip);
+
+ Constraint::Ball ball(ground, Vec3(0), *this, Vec3(0));
+ ball.setDisabledByDefault(true);
+ m_ballConstraints.push_back(ball);
+ }
+}
+
+//--------------------------- Temporary constraints ----------------------------
+Real FreeUnilateralBrick::
+get_pipConstraintHeightInGround(const State& s, const BrickVertexIndex i) const
+{
+ return getMatterSubsystem().getGround().findStationLocationInAnotherBody(s,
+ m_pipConstraints[i].getDefaultFollowerPoint(), *this)[ZAxis];
+}
+
+void FreeUnilateralBrick::
+set_pipConstraintLocation(const State& s, const BrickVertexIndex i,
+ const Vec3& positionInGround)
+{
+ m_pipConstraints[i].setDefaultFollowerPoint(
+ getMatterSubsystem().getGround().
+ findStationLocationInAnotherBody(s, positionInGround, *this));
+}
+
+void FreeUnilateralBrick::
+enablePipConstraint(State& s, const BrickVertexIndex i) const
+{ m_pipConstraints[i].enable(s); }
+
+void FreeUnilateralBrick::disableAllPipConstraints(State& s) const
+{
+ for (BrickVertexIndex i(0); i<m_pipConstraints.size(); ++i)
+ m_pipConstraints[i].disable(s);
+}
+
+MultiplierIndex FreeUnilateralBrick::
+get_ballConstraintFirstIndex(const State& s, const BrickVertexIndex i) const
+{
+ MultiplierIndex px0,vx0,ax0;
+ m_ballConstraints[i].getIndexOfMultipliersInUse(s, px0, vx0, ax0);
+ return px0;
+}
+
+void FreeUnilateralBrick::
+set_ballConstraintLocation(const State& s, const BrickVertexIndex i,
+ const Vec3& positionInGround)
+{
+ m_ballConstraints[i].setDefaultPointOnBody1(positionInGround);
+ m_ballConstraints[i].setDefaultPointOnBody2(
+ getMatterSubsystem().getGround().
+ findStationLocationInAnotherBody(s, positionInGround, *this));
+}
+
+void FreeUnilateralBrick::
+enableBallConstraint(State& s, const BrickVertexIndex i) const
+{ m_ballConstraints[i].enable(s); }
+
+void FreeUnilateralBrick::disableAllBallConstraints(State& s) const
+{
+ for (BrickVertexIndex i(0); i<m_ballConstraints.size(); ++i)
+ m_ballConstraints[i].disable(s);
+}
+
+//------------------------- Position-level information -------------------------
+Vec3 FreeUnilateralBrick::
+findLowestPointLocationInGround(const State& s, const BrickVertexIndex i) const
+{
+ return findStationLocationInGround(s, m_vertices[i])
+ + Vec3(0,0,-m_sphereRadii);
+}
+
+Vec3 FreeUnilateralBrick::
+findLowestPointLocationInBodyFrame(const State& s,
+ const BrickVertexIndex i) const
+{
+ // This is the vector from the origin of the body to the lowest point on the
+ // ith sphere, resolved in the ground frame.
+ Vec3 posG = findLowestPointLocationInGround(s,i) - getBodyOriginLocation(s);
+
+ // Transform to body-fixed frame by applying rotation only.
+ return expressGroundVectorInBodyFrame(s,posG);
+}
+
+void FreeUnilateralBrick::findAllLowestPointLocationsInGround(const State& s,
+ Array_<Vec3,BrickVertexIndex>& lowestPointLocationsInG) const
+{
+ SimTK_ASSERT(lowestPointLocationsInG.empty(), "Input array must be empty.");
+ for (BrickVertexIndex i(0); i<(int)m_vertices.size(); ++i)
+ lowestPointLocationsInG.push_back(findLowestPointLocationInGround(s,i));
+}
+
+bool FreeUnilateralBrick::isBrickInterpenetrating(
+ const Array_<Vec3,BrickVertexIndex>& lowestPointLocationsInG) const
+{
+ for (BrickVertexIndex i(0); i<(int)lowestPointLocationsInG.size(); ++i)
+ if (lowestPointLocationsInG[i][ZAxis] < -TolPositionFuzziness)
+ return true;
+ return false;
+}
+
+bool FreeUnilateralBrick::isBrickInterpenetrating(const State& s) const
+{
+ for (BrickVertexIndex i(0); i<(int)m_vertices.size(); ++i)
+ if (findLowestPointLocationInGround(s,i)[ZAxis] < -TolPositionFuzziness)
+ return true;
+ return false;
+}
+
+bool FreeUnilateralBrick::
+isPointProximal(const Vec3& lowestPointLocationInG) const
+{ return (lowestPointLocationInG[ZAxis] < TolPositionFuzziness); }
+
+void FreeUnilateralBrick::findProximalPointIndices(
+ const Array_<Vec3,BrickVertexIndex>& lowestPointLocationsInG,
+ Array_<BrickVertexIndex,ProximalPointIndex>& proximalPointIndices) const
+{
+ SimTK_ASSERT(proximalPointIndices.empty(), "Input array must be empty.");
+ for (BrickVertexIndex i(0); i<(int)lowestPointLocationsInG.size(); ++i)
+ if (isPointProximal(lowestPointLocationsInG[i]))
+ proximalPointIndices.push_back(i);
+}
+
+//------------------------- Velocity-level information -------------------------
+Vec3 FreeUnilateralBrick::
+findLowestPointVelocityInGround(const State& s, const BrickVertexIndex i) const
+{
+ return findStationVelocityInGround(s,
+ findLowestPointLocationInBodyFrame(s,i));
+}
+
+Real FreeUnilateralBrick::findTangentialVelocityAngle(const Vec3& vel) const
+{
+ if (vel.getSubVec<2>(0).norm() < TolReliableDirection)
+ return SimTK::NaN;
+ return atan2(vel[YAxis], vel[XAxis]);
+}
+
+bool FreeUnilateralBrick::isBrickImpacting(
+ const Array_<Vec3,ProximalPointIndex>& proximalPointVelocities) const
+{
+ for (ProximalPointIndex i(0); i<(int)proximalPointVelocities.size(); ++i)
+ if (proximalPointVelocities[i][ZAxis] < -TolVelocityFuzziness)
+ return true;
+ return false;
+}
+
+
+//==============================================================================
+// Implementation: POSITION PROJECTER
+//==============================================================================
+PositionProjecter::
+PositionProjecter(MultibodySystem& mbs,
+ FreeUnilateralBrick& brick,
+ const State& s0,
+ const Array_<Vec3,BrickVertexIndex>& positionsInG)
+ : m_mbs(mbs), m_brick(brick)
+{
+ // Create an array containing the index of each proximal point.
+ m_brick.findProximalPointIndices(positionsInG, m_proximalPointIndices);
+
+ // Adjust the position of the PointInPlane constraints corresponding to the
+ // proximal points.
+ for (ProximalPointIndex i(0); i<(int)m_proximalPointIndices.size(); ++i)
+ m_brick.set_pipConstraintLocation(s0, m_proximalPointIndices[i],
+ positionsInG[m_proximalPointIndices[i]]);
+
+ if (PrintDebugInfoProjection) {
+ printHorizontalRule(1,0,'*',"projecting positions");
+ cout << " -> " << m_proximalPointIndices.size() << " proximal point(s)"
+ << endl;
+ for (ProximalPointIndex i(0); i<(int)m_proximalPointIndices.size(); ++i)
+ cout << " [" << i << "] p="
+ << positionsInG[m_proximalPointIndices[i]] << endl;
+ }
+}
+
+//--------------------- Resolve position-level violations ----------------------
+Array_<ProximalPointIndex> PositionProjecter::
+projectPositionsExhaustive(State& s) const
+{
+ // Assemble all combinations of 1 or more proximal points.
+ Array_<Array_<ProximalPointIndex> > arrayOfIndexArrays;
+ createProximalIndexArrays((int)m_proximalPointIndices.size(),
+ arrayOfIndexArrays);
+ if (PrintDebugInfoProjection)
+ cout << " -> " << arrayOfIndexArrays.size() << " combination(s)"
+ << endl;
+
+ // Try projecting using every combination of constraint indices; compute
+ // 2-norm distance between original and final Q.
+ Real minDistance = SimTK::Infinity;
+ Vector minQ;
+ int minIdx;
+ for (int comb=0; comb<(int)arrayOfIndexArrays.size(); ++comb) {
+ State sTemp = m_mbs.realizeTopology();
+ sTemp.setQ(s.getQ());
+ Real dist = evaluateProjection(sTemp, s, arrayOfIndexArrays[comb]);
+
+ // Favor combinations with the most enabled constraints; of those,
+ // select the combination with the smallest distance metric.
+ if (SimTK::isInf(minDistance) ||
+ arrayOfIndexArrays[comb].size() > arrayOfIndexArrays[minIdx].size()
+ || (arrayOfIndexArrays[comb].size() ==
+ arrayOfIndexArrays[minIdx].size() && dist < minDistance))
+ {
+ minDistance = dist;
+ minQ = sTemp.getQ();
+ minIdx = comb;
+ }
+
+ if (PrintDebugInfoProjection)
+ cout << " [" << std::setw(2) << comb << "] "
+ << "d=" << std::setprecision(6) << std::setw(10) << dist
+ << " " << arrayOfIndexArrays[comb] << endl;
+ }
+
+ SimTK_ASSERT_ALWAYS(minDistance < SimTK::Infinity,
+ "No valid position projection found by exhaustive search.");
+
+ // Apply the projection that resolves all violations while requiring the
+ // smallest change in Q.
+ s.setQ(minQ);
+
+ if (PrintDebugInfoProjection) {
+ cout << " -> Exhaustive search selected index " << minIdx
+ << ", constraints " << arrayOfIndexArrays[minIdx] << endl;
+ displayNewProximalPoints(s);
+ printHorizontalRule(0,1,'*');
+ }
+
+ return arrayOfIndexArrays[minIdx];
+}
+
+Array_<ProximalPointIndex> PositionProjecter::
+projectPositionsPruning(State& s) const
+{
+ // Begin with indices of all proximal points.
+ Array_<ProximalPointIndex> indexArray(m_proximalPointIndices.size());
+ for (ProximalPointIndex i(0); i<(int)m_proximalPointIndices.size(); ++i)
+ indexArray[i] = i;
+
+ if (PrintDebugInfoProjection)
+ cout << " -> Starting pruning search with " << indexArray.size()
+ << " constraint(s)" << endl;
+
+ // Realize topology only once.
+ State sTempClean = m_mbs.realizeTopology();
+ sTempClean.setQ(s.getQ());
+
+ // Successively prune constraints until the projection is successful.
+ while(true) {
+
+ // Ensure at least one constraint will be enabled.
+ SimTK_ASSERT_ALWAYS(!indexArray.empty(),
+ "No valid position projection found by pruning search.");
+
+ // Try this set of constraints.
+ State sTemp(sTempClean);
+ Real dist = evaluateProjection(sTemp, s, indexArray);
+ if (PrintDebugInfoProjection)
+ cout << " " << indexArray << " d=" << dist << endl;
+
+ // Exit if successful; otherwise, remove the constraint associated with
+ // the most distant proximal point.
+ if (dist < SimTK::Infinity) {
+ s.setQ(sTemp.getQ());
+ break;
+ } else {
+ Real maxDist = 0;
+ int maxIdx;
+
+ for (int i=0; i<(int)indexArray.size(); ++i) {
+ Real currDist = m_brick.get_pipConstraintHeightInGround(s,
+ m_proximalPointIndices[indexArray[i]]);
+ if (currDist > maxDist) {
+ maxDist = currDist;
+ maxIdx = i;
+ }
+ }
+ indexArray.eraseFast(&indexArray[maxIdx]);
+ }
+ }
+
+ if (PrintDebugInfoProjection) {
+ cout << " -> Pruning search selected constraints " << indexArray
+ << endl;
+ displayNewProximalPoints(s);
+ printHorizontalRule(0,1,'*');
+ }
+
+ return indexArray;
+}
+
+//------------------------------ Private methods -------------------------------
+void PositionProjecter::createProximalIndexArrays(int sizeOfSet,
+ Array_<Array_<ProximalPointIndex> >& arrayOfIndexArrays) const
+{
+ SimTK_ASSERT(arrayOfIndexArrays.empty(), "Input array must be empty.");
+ const int numArrays = (1 << sizeOfSet) - 1;
+ for (int ctr=1; ctr<=numArrays; ++ctr) { //Exclude empty set.
+ Array_<ProximalPointIndex> indexArray;
+ for (ProximalPointIndex idx(0); idx<sizeOfSet; ++idx)
+ if (ctr & (1 << idx))
+ indexArray.push_back(idx);
+ arrayOfIndexArrays.push_back(indexArray);
+ }
+}
+
+Real PositionProjecter::evaluateProjection(State& sTemp, const State& sOrig,
+ const Array_<ProximalPointIndex>& indexArray) const
+{
+ Real dist = SimTK::Infinity;
+
+ // Enable constraints.
+ for (ProximalPointIndex i(0); i<(int)indexArray.size(); ++i)
+ m_brick.enablePipConstraint(sTemp,
+ m_proximalPointIndices[indexArray[i]]);
+
+ // Try projecting. Simbody will throw an exception if the projection
+ // accuracy cannot be achieved.
+ try {
+ m_mbs.projectQ(sTemp, TolProjectQ);
+ if (!m_brick.isBrickInterpenetrating(sTemp))
+ dist = (sOrig.getQ() - sTemp.getQ()).norm();
+ } catch(...) {}
+
+ return dist;
+}
+
+void PositionProjecter::displayNewProximalPoints(State& s) const
+{
+ // New positions.
+ m_mbs.realize(s, Stage::Position);
+ Array_<Vec3,BrickVertexIndex> postProjectionPos;
+ m_brick.findAllLowestPointLocationsInGround(s, postProjectionPos);
+
+ // New proximal points.
+ Array_<BrickVertexIndex,ProximalPointIndex> proximalPointIndices;
+ m_brick.findProximalPointIndices(postProjectionPos, proximalPointIndices);
+
+ // Display.
+ for (ProximalPointIndex i(0); i<(int)proximalPointIndices.size(); ++i)
+ cout << " [" << i << "] p="
+ << postProjectionPos[proximalPointIndices[i]] << endl;
+}
+
+
+//==============================================================================
+// Implementation: IMPACTER
+//==============================================================================
+Impacter::
+Impacter(MultibodySystem& mbs, FreeUnilateralBrick& brick, const State& s0,
+ const Array_<Vec3,BrickVertexIndex>& allPositionsInG,
+ const Array_<BrickVertexIndex,ProximalPointIndex>&
+ proximalPointIndices)
+ : m_mbs(mbs), m_brick(brick), m_proximalPointIndices(proximalPointIndices)
+{
+ // Adjust the position of the Ball constraints corresponding to the proximal
+ // points.
+ for (ProximalPointIndex i(0); i<(int)m_proximalPointIndices.size(); ++i)
+ m_brick.set_ballConstraintLocation(s0, m_proximalPointIndices[i],
+ allPositionsInG[m_proximalPointIndices[i]]);
+
+ if (PrintDebugInfoImpact) {
+ printHorizontalRule(1,0,'*',"starting impact");
+ cout << " -> " << m_proximalPointIndices.size() << " proximal point(s)"
+ << endl;
+ for (ProximalPointIndex i(0); i<(int)m_proximalPointIndices.size(); ++i)
+ cout << " [" << i << "] p="
+ << allPositionsInG[m_proximalPointIndices[i]] << endl;
+ }
+}
+
+//------------------------ Perform one complete impact -------------------------
+std::string Impacter::
+performImpactExhaustive(State& s,
+ Array_<Vec3,ProximalPointIndex>& proximalVelsInG,
+ Array_<bool,ProximalPointIndex>& hasRebounded,
+ Real& totalEnergyDissipated) const
+{
+ // Calculate coefficients of restitution.
+ Array_<Real> CORs(m_proximalPointIndices.size());
+ for (ProximalPointIndex i(0); i<(int)m_proximalPointIndices.size(); ++i) {
+ CORs [i] = hasRebounded[i] ? 0.0 : calcCOR(proximalVelsInG[i][ZAxis]);
+ if (PrintDebugInfoImpact)
+ cout << " ** CORs[" << i << "] = " << CORs[i] << endl;
+ }
+
+ // Enumerate all active set candidates.
+ Array_<ActiveSetCandidate> activeSetCandidates;
+ initializeActiveSetCandidateArray(activeSetCandidates);
+ if (PrintDebugInfoImpact)
+ cout << " -> " << activeSetCandidates.size()
+ << " active set candidate(s)" << endl;
+
+ // Interval-stepping loop.
+ ImpactPhase impactPhase = Compression;
+ Vector restitutionImpulses = Vector(m_proximalPointIndices.size(), 0.0);
+ Vector energyDissipated = Vector(m_proximalPointIndices.size(), 0.0);
+ int intervalCtr = 0;
+ std::string trajectory = ""; //Accumulate active set descriptions.
+ while(true) {
+ ++intervalCtr;
+ if (PrintDebugInfoImpact) {
+ std::stringstream sstream;
+ sstream << (impactPhase == Compression ? "compression"
+ : "restitution")
+ << " interval " << intervalCtr;
+ printHorizontalRule(1,0,'#',sstream.str());
+ }
+
+ // Clear data associated with each active set candidate.
+ resetActiveSetCandidateArray(activeSetCandidates);
+
+ // Generate and solve a linear system of equations for each active set
+ // candidate. Categorize each solution and calculate fitness value.
+ for (int i=0; i<(int)activeSetCandidates.size(); ++i) {
+ if (PrintDebugInfoImpact) {
+ cout << " -> evaluating candidate " << i << " ";
+ printFormattedActiveSet(cout,
+ activeSetCandidates[i].tangentialStates,
+ (impactPhase == Compression ? "c" : "r"));
+ cout << endl;
+ }
+
+ generateAndSolveLinearSystem(s, impactPhase, restitutionImpulses,
+ activeSetCandidates[i]);
+ evaluateLinearSystemSolution(s, impactPhase, restitutionImpulses,
+ activeSetCandidates[i]);
+
+ if (PrintDebugInfoImpact)
+ printActiveSetInfo(activeSetCandidates[i]);
+ if (PauseAfterEachActiveSetCandidate)
+ char trash = getchar();
+ }
+
+ if (PrintDebugInfoActiveSetSearch) {
+ printHorizontalRule(2,0,'=',"exhaustive search summary");
+
+ // Count instances of each category.
+ Array_<int> solCatCtr(11,0);
+ for (int i=0; i<(int)activeSetCandidates.size(); ++i)
+ ++solCatCtr[activeSetCandidates[i].solutionCategory];
+
+ for (int i=0; i<=SolCat_NotEvaluated; ++i)
+ cout << " " << std::setw(2) << solCatCtr[i] << " "
+ << getSolutionCategoryDescription(SolutionCategory(i))
+ << endl;
+ printHorizontalRule(0,1,'=');
+ }
+
+ // Select from among the active set candidates in the best (lowest)
+ // category. Favor candidates with the most non-observing points; of
+ // those, select the candidate with the best (lowest) fitness value.
+ Real bestFitness = SimTK::Infinity;
+ int bestCount = 0;
+ int bestIdx = -1;
+ for (int solcat=0;
+ solcat<=SolCat_WorstCatWithSomeUsableSolution; ++solcat)
+ {
+ for (int idx=0; idx<(int)activeSetCandidates.size(); ++idx) {
+ if (!(activeSetCandidates[idx].solutionCategory == solcat))
+ continue;
+
+ // Count the number of sliding or rolling points.
+ int thisCount = 0;
+ for (ProximalPointIndex i(0);
+ i<(int)m_proximalPointIndices.size(); ++i)
+ {
+ if (activeSetCandidates[idx].tangentialStates[i]
+ > Observing)
+ ++thisCount;
+ }
+
+ if (SimTK::isInf(bestFitness) || thisCount > bestCount
+ || (thisCount == bestCount &&
+ activeSetCandidates[idx].fitness < bestFitness))
+ {
+ bestFitness = activeSetCandidates[idx].fitness;
+ bestCount = thisCount;
+ bestIdx = idx;
+ }
+ }
+
+ // Halt if a usable solution was found in this category.
+ if (bestFitness < SimTK::Infinity)
+ break;
+ }
+ SimTK_ASSERT_ALWAYS(bestFitness < SimTK::Infinity,
+ "No suitable active set found by exhaustive search.");
+
+ if (PrintBasicInfo)
+ printFormattedActiveSet(cout,
+ activeSetCandidates[bestIdx].tangentialStates,
+ (impactPhase == Compression ? "c" : "r"));
+ if (PrintDebugInfoImpact) {
+ cout << " ** selected active set candidate " << bestIdx << endl;
+ printActiveSetInfo(activeSetCandidates[bestIdx]);
+ }
+ trajectory += printFormattedActiveSet(
+ activeSetCandidates[bestIdx].tangentialStates,
+ (impactPhase == Compression ? "c" : "r"));
+
+ // Determine step length and apply impulse.
+ const Real steplength = calculateIntervalStepLength(s, proximalVelsInG,
+ activeSetCandidates[bestIdx], intervalCtr);
+ SimTK_ASSERT_ALWAYS(!isNaN(steplength),
+ "No suitable interval step length found.");
+
+ s.updU() += steplength*
+ activeSetCandidates[bestIdx].systemVelocityChange;
+ m_mbs.realize(s, Stage::Velocity);
+
+ if (PrintDebugInfoImpact)
+ cout << " ** steplength = " << steplength
+ << "\n newU = " << s.getU() << endl;
+
+ // Perform part of energy dissipation calculation now (i.e., before
+ // updating velocities). Initialize powerTime0 array to zero since power
+ // will not be computed for observing proximal points.
+ Array_<Real,ProximalPointIndex>
+ powerTime0((int)m_proximalPointIndices.size(), 0.0);
+ int constraintIdx = -1;
+ for (ProximalPointIndex i(0); i<(int)m_proximalPointIndices.size(); ++i)
+ {
+ if (activeSetCandidates[bestIdx].tangentialStates[i] > Observing) {
+ ++constraintIdx;
+ const Vec2 vTang = proximalVelsInG[i].getSubVec<2>(0);
+ const Vec2 piTang = Vec2(activeSetCandidates[bestIdx].
+ localImpulses[constraintIdx*3],
+ activeSetCandidates[bestIdx].
+ localImpulses[constraintIdx*3+1])
+ * steplength;
+
+ // Use dot product here in case tangential velocity and impulse
+ // are not quite collinear.
+ powerTime0[i] = dot(vTang, piTang)
+ + (proximalVelsInG[i][ZAxis]
+ * activeSetCandidates[bestIdx].
+ localImpulses[constraintIdx*3+2]
+ * steplength);
+ }
+ }
+
+ // Calculate the new velocity of each proximal point.
+ for (ProximalPointIndex i(0); i<(int)m_proximalPointIndices.size(); ++i)
+ proximalVelsInG[i] = m_brick.findLowestPointVelocityInGround(s,
+ m_proximalPointIndices[i]);
+ if (PrintDebugInfoImpact)
+ for (ProximalPointIndex i(0);
+ i<(int)m_proximalPointIndices.size(); ++i)
+ cout << " [" << i << "] v=" << proximalVelsInG[i] << endl;
+
+ // Calculate energy dissipated.
+ constraintIdx = -1;
+ for (ProximalPointIndex i(0); i<(int)m_proximalPointIndices.size(); ++i)
+ {
+ if (activeSetCandidates[bestIdx].tangentialStates[i] > Observing) {
+ ++constraintIdx;
+ const Vec2 vTang = proximalVelsInG[i].getSubVec<2>(0);
+ const Vec2 piTang = Vec2(activeSetCandidates[bestIdx].
+ localImpulses[constraintIdx*3],
+ activeSetCandidates[bestIdx].
+ localImpulses[constraintIdx*3+1])
+ * steplength;
+ const Real powerTime1 = dot(vTang, piTang)
+ + (proximalVelsInG[i][ZAxis]
+ * activeSetCandidates[bestIdx].
+ localImpulses[constraintIdx*3+2] * steplength);
+
+ energyDissipated[i] += (powerTime0[i] + powerTime1) * 0.5;
+ }
+ }
+
+ // Update the impact phase and required restitution impulses; exit if
+ // complete.
+ if (impactPhase == Compression) {
+
+ int constraintIdx = -1;
+ for (ProximalPointIndex i(0);
+ i<(int)m_proximalPointIndices.size(); ++i)
+ {
+ if (activeSetCandidates[bestIdx].tangentialStates[i]
+ > Observing)
+ {
+ ++constraintIdx;
+ restitutionImpulses[i] += -activeSetCandidates[bestIdx].
+ localImpulses[constraintIdx*3+2]
+ * CORs[i] * steplength;
+ }
+ }
+
+ if (!m_brick.isBrickImpacting(proximalVelsInG)) {
+ if (PrintDebugInfoImpact)
+ cout << " ** compression phase complete" << endl;
+
+ // Proceed to restitution phase if any restitution impulses must
+ // be applied; exit otherwise.
+ Real maxRestImpulse = 0;
+ for (int i=0; i<(int)restitutionImpulses.size(); ++i)
+ maxRestImpulse = std::max(maxRestImpulse,
+ restitutionImpulses[i]);
+ if (maxRestImpulse < MinMeaningfulImpulse) {
+ if (PrintDebugInfoImpact)
+ cout << " ** no restitution impulses" << endl;
+ break;
+ } else {
+ impactPhase = Restitution;
+ intervalCtr = 0;
+ }
+ }
+
+ } else if (impactPhase == Restitution) {
+
+ int constraintIdx = -1;
+ for (ProximalPointIndex i(0);
+ i<(int)m_proximalPointIndices.size(); ++i)
+ {
+ if (activeSetCandidates[bestIdx].tangentialStates[i]
+ > Observing)
+ {
+ ++constraintIdx;
+ restitutionImpulses[i] -= -activeSetCandidates[bestIdx].
+ localImpulses[constraintIdx*3+2]
+ * steplength;
+
+ if (std::abs(-activeSetCandidates[bestIdx].
+ localImpulses[constraintIdx*3+2])
+ > MinMeaningfulImpulse)
+
+ hasRebounded[i] = true;
+ }
+ }
+
+ Real maxRestImpulse = 0;
+ for (int i=0; i<(int)restitutionImpulses.size(); ++i)
+ maxRestImpulse = std::max(maxRestImpulse,
+ restitutionImpulses[i]);
+ if (maxRestImpulse < MinMeaningfulImpulse) {
+ if (PrintDebugInfoImpact)
+ cout << " ** restitution phase complete" << endl;
+ break;
+ }
+ }
+
+ if (PrintDebugInfoImpact) {
+ cout << " restitutionImpulses = " << restitutionImpulses
+ << "\n hasRebounded = " << hasRebounded << endl;
+ }
+ if (PauseAfterEachImpactInterval)
+ char trash = getchar();
+
+ } //end interval-stepping loop
+ if (PrintBasicInfo)
+ cout << endl;
+
+ // Ensure the total energy dissipated over the entire impact is positive for
+ // each proximal point.
+ for (int i=0; i<(int)energyDissipated.size(); ++i) {
+ if (energyDissipated[i] < -SimTK::SignificantReal) {
+ printHorizontalRule(1,1,'#',"WARNING");
+ cout << "Negative energy dissipated (" << energyDissipated[i]
+ << ")" << endl;
+ printHorizontalRule(1,1,'#',"WARNING");
+ }
+ }
+
+ totalEnergyDissipated = energyDissipated.sum();
+ return trajectory;
+}
+
+std::string Impacter::
+performImpactPruning(State& s,
+ Array_<Vec3,ProximalPointIndex>& proximalVelsInG,
+ Array_<bool,ProximalPointIndex>& hasRebounded,
+ Real& totalEnergyDissipated) const
+{
+ // Calculate coefficients of restitution.
+ Array_<Real,ProximalPointIndex> CORs(m_proximalPointIndices.size());
+ for (ProximalPointIndex i(0); i<(int)m_proximalPointIndices.size(); ++i) {
+ CORs[i] = hasRebounded[i] ? 0.0 : calcCOR(proximalVelsInG[i][ZAxis]);
+ if (PrintDebugInfoImpact)
+ cout << " ** CORs[" << i << "] = " << CORs[i] << endl;
+ }
+
+ // Interval-stepping loop.
+ ImpactPhase impactPhase = Compression;
+ Vector restitutionImpulses = Vector(m_proximalPointIndices.size(), 0.0);
+ Vector energyDissipated = Vector(m_proximalPointIndices.size(), 0.0);
+ int intervalCtr = 0;
+ std::string trajectory = ""; //Accumulate active set descriptions.
+ while(true) {
+ ++intervalCtr;
+ if (PrintDebugInfoImpact) {
+ std::stringstream sstream;
+ sstream << (impactPhase == Compression ? "compression"
+ : "restitution")
+ << " interval " << intervalCtr;
+ printHorizontalRule(1,0,'#',sstream.str());
+ }
+
+ // Begin with maximal active set, but disallow sticking at points whose
+ // tangential velocity magnitude is too large.
+ ActiveSetCandidate asc;
+ initializeActiveSetCandidate(s,asc);
+ if (PrintDebugInfoImpact) {
+ cout << " -> Starting with active set candidate ";
+ printFormattedActiveSet(cout, asc.tangentialStates, "");
+ cout << endl;
+ }
+
+ // If the pruning search does not converge to an active set candidate
+ // producing a solution with no violations, use the least-objectionable
+ // active set candidate that was examined; throw an exception if all
+ // examined active set candidates are intolerable.
+ ActiveSetCandidate bestAsc;
+ bestAsc.solutionCategory = SolCat_NotEvaluated;
+
+ // Prune until active set candidate is empty.
+ while (!isActiveSetCandidateEmpty(asc)) {
+ resetActiveSetCandidate(asc);
+
+ if (PrintDebugInfoImpact) {
+ cout << " -> evaluating candidate ";
+ printFormattedActiveSet(cout, asc.tangentialStates,
+ (impactPhase == Compression ? "c" : "r"));
+ cout << endl;
+ }
+
+ // Generate and solve a linear system of equations for this active
+ // set candidate. Categorize the solution and calculate the fitness
+ // value.
+ generateAndSolveLinearSystem(s,impactPhase,restitutionImpulses,asc);
+ evaluateLinearSystemSolution(s,impactPhase,restitutionImpulses,asc);
+
+ if (PrintDebugInfoImpact)
+ printActiveSetInfo(asc);
+ if (PauseAfterEachActiveSetCandidate)
+ char trash = getchar();
+
+ // Keep track of least-objectionable active set candidate.
+ if (asc.solutionCategory <= bestAsc.solutionCategory ||
+ (asc.solutionCategory == bestAsc.solutionCategory
+ && asc.fitness <= bestAsc.fitness)) {
+ bestAsc = asc;
+ }
+
+ // Exit if acceptable active set candidate has been found.
+ if (bestAsc.solutionCategory
+ <= SolCat_WorstCatWithNoSeriousViolations)
+ {
+ if (PrintDebugInfoActiveSetSearch)
+ cout << " Acceptable active set candidate found"
+ << endl;
+ break;
+ }
+
+ // Exit if current active set candidate applies no impulses; cannot
+ // make progress by pruning.
+ if (asc.solutionCategory == SolCat_NoImpulsesApplied) {
+ if (PrintDebugInfoActiveSetSearch)
+ cout << " No impulses applied; cannot prune further"
+ << endl;
+ break;
+ }
+
+ // Prune worst constraint.
+ SimTK_ASSERT(asc.tangentialStates[asc.worstConstraint] > Observing,
+ "Invalid worstConstraint index.");
+ asc.tangentialStates[asc.worstConstraint] -= 1;
+
+ } //end pruning loop
+
+ // Throw an exception if no tolerable active set candidate was found.
+ SimTK_ASSERT_ALWAYS(bestAsc.solutionCategory <=
+ SolCat_WorstCatWithSomeUsableSolution,
+ "Pruning search failed to find suitable active set candidate.");
+
+ if (PrintBasicInfo)
+ printFormattedActiveSet(cout, bestAsc.tangentialStates,
+ (impactPhase == Compression ? "c" : "r"));
+ if (PrintDebugInfoImpact) {
+ cout << " ** selected active set candidate ";
+ printFormattedActiveSet(cout, bestAsc.tangentialStates);
+ printActiveSetInfo(bestAsc);
+ }
+ trajectory += printFormattedActiveSet(bestAsc.tangentialStates,
+ (impactPhase == Compression ? "c" : "r"));
+
+ // Determine step length and apply impulse.
+ const Real steplength = calculateIntervalStepLength(s, proximalVelsInG,
+ bestAsc, intervalCtr);
+ SimTK_ASSERT_ALWAYS(!isNaN(steplength),
+ "No suitable interval step length found.");
+
+ s.updU() += steplength*bestAsc.systemVelocityChange;
+ m_mbs.realize(s, Stage::Velocity);
+
+ if (PrintDebugInfoImpact)
+ cout << " ** steplength = " << steplength
+ << "\n newU = " << s.getU() << endl;
+
+ // Perform part of energy dissipation calculation now (i.e., before
+ // updating velocities). Initialize powerTime0 array to zero since power
+ // will not be computed for observing proximal points.
+ Array_<Real,ProximalPointIndex>
+ powerTime0((int)m_proximalPointIndices.size(), 0.0);
+ int constraintIdx = -1;
+ for (ProximalPointIndex i(0); i<(int)m_proximalPointIndices.size(); ++i)
+ {
+ if (bestAsc.tangentialStates[i] > Observing) {
+ ++constraintIdx;
+ const Vec2 vTang = proximalVelsInG[i].getSubVec<2>(0);
+ const Vec2 piTang = Vec2(bestAsc.localImpulses[constraintIdx*3],
+ bestAsc.localImpulses[constraintIdx*3+1])
+ * steplength;
+
+ // Use dot product here in case tangential velocity and impulse
+ // are not quite collinear.
+ powerTime0[i] = dot(vTang, piTang)
+ + (proximalVelsInG[i][ZAxis]
+ * bestAsc.localImpulses[constraintIdx*3+2]
+ * steplength);
+ }
+ }
+
+ // Calculate the new velocity of each proximal point.
+ for (ProximalPointIndex i(0); i<(int)m_proximalPointIndices.size(); ++i)
+ proximalVelsInG[i] = m_brick.findLowestPointVelocityInGround(s,
+ m_proximalPointIndices[i]);
+ if (PrintDebugInfoImpact)
+ for (ProximalPointIndex i(0);
+ i<(int)m_proximalPointIndices.size(); ++i)
+ cout << " [" << i << "] v=" << proximalVelsInG[i] << endl;
+
+ // Calculate energy dissipated.
+ constraintIdx = -1;
+ for (ProximalPointIndex i(0); i<(int)m_proximalPointIndices.size(); ++i)
+ {
+ if (bestAsc.tangentialStates[i] > Observing) {
+ ++constraintIdx;
+ const Vec2 vTang = proximalVelsInG[i].getSubVec<2>(0);
+ const Vec2 piTang = Vec2(bestAsc.localImpulses[constraintIdx*3],
+ bestAsc.localImpulses[constraintIdx*3+1])
+ * steplength;
+ const Real powerTime1 = dot(vTang, piTang)
+ + (proximalVelsInG[i][ZAxis]
+ * bestAsc.localImpulses[constraintIdx*3+2] * steplength);
+
+ energyDissipated[i] += (powerTime0[i] + powerTime1) * 0.5;
+ }
+ }
+
+ // Update the impact phase and required restitution impulses; exit if
+ // complete.
+ if (impactPhase == Compression) {
+
+ int constraintIdx = -1;
+ for (ProximalPointIndex i(0);
+ i<(int)m_proximalPointIndices.size(); ++i)
+ {
+ if (bestAsc.tangentialStates[i] > Observing) {
+ ++constraintIdx;
+ restitutionImpulses[i] += -bestAsc.
+ localImpulses[constraintIdx*3+2]
+ * CORs[i] * steplength;
+ }
+ }
+
+ if (!m_brick.isBrickImpacting(proximalVelsInG)) {
+ if (PrintDebugInfoImpact)
+ cout << " ** compression phase complete" << endl;
+
+ // Proceed to restitution phase if any restitution impulses must
+ // be applied; exit otherwise.
+ Real maxRestImpulse = 0;
+ for (int i=0; i<(int)restitutionImpulses.size(); ++i)
+ maxRestImpulse = std::max(maxRestImpulse,
+ restitutionImpulses[i]);
+ if (maxRestImpulse < MinMeaningfulImpulse) {
+ if (PrintDebugInfoImpact)
+ cout << " ** no restitution impulses" << endl;
+ break;
+ } else {
+ impactPhase = Restitution;
+ intervalCtr = 0;
+ }
+ }
+
+ } else if (impactPhase == Restitution) {
+
+ int constraintIdx = -1;
+ for (ProximalPointIndex i(0);
+ i<(int)m_proximalPointIndices.size(); ++i)
+ {
+ if (bestAsc.tangentialStates[i] > Observing) {
+ ++constraintIdx;
+ restitutionImpulses[i] -= -bestAsc.
+ localImpulses[constraintIdx*3+2]
+ * steplength;
+
+ if (std::abs(-bestAsc.localImpulses[constraintIdx*3+2])
+ > MinMeaningfulImpulse)
+
+ hasRebounded[i] = true;
+ }
+ }
+
+ Real maxRestImpulse = 0;
+ for (int i=0; i<(int)restitutionImpulses.size(); ++i)
+ maxRestImpulse = std::max(maxRestImpulse,
+ restitutionImpulses[i]);
+ if (maxRestImpulse < MinMeaningfulImpulse) {
+ if (PrintDebugInfoImpact)
+ cout << " ** restitution phase complete" << endl;
+ break;
+ }
+ }
+
+ if (PrintDebugInfoImpact) {
+ cout << " restitutionImpulses = " << restitutionImpulses
+ << "\n hasRebounded = " << hasRebounded << endl;
+ }
+ if (PauseAfterEachImpactInterval)
+ char trash = getchar();
+
+ } //end interval-stepping loop
+ if (PrintBasicInfo)
+ cout << endl;
+
+ // Ensure the total energy dissipated over the entire impact is positive for
+ // each proximal point.
+ for (int i=0; i<(int)energyDissipated.size(); ++i) {
+ if (energyDissipated[i] < -SimTK::SignificantReal) {
+ printHorizontalRule(1,1,'#',"WARNING");
+ cout << "Negative energy dissipated (" << energyDissipated[i]
+ << ")" << endl;
+ printHorizontalRule(1,1,'#',"WARNING");
+ }
+ }
+
+ totalEnergyDissipated = energyDissipated.sum();
+ return trajectory;
+}
+
+//-------------------------- Private methods: display --------------------------
+void Impacter::
+printFormattedActiveSet(std::ostream& stream,
+ const Array_<int,ProximalPointIndex>& tangentialStates,
+ const std::string& prefix) const
+{ stream << printFormattedActiveSet(tangentialStates, prefix); }
+
+std::string Impacter::
+printFormattedActiveSet(const Array_<int,ProximalPointIndex>& tangentialStates,
+ const std::string& prefix) const
+{
+ std::string msg = "(" + prefix;
+ for (ProximalPointIndex i(0); i<(int)tangentialStates.size(); ++i) {
+ switch (tangentialStates[i]) {
+ case Observing: msg += "O"; break;
+ case Rolling: msg += "R"; break;
+ case Sliding: msg += "S"; break;
+ default: msg += "?"; break;
+ }
+ }
+ msg += ")";
+
+ return msg;
+}
+
+const char* Impacter::
+getSolutionCategoryDescription(SolutionCategory solCat) const
+{
+ switch (solCat) {
+ case SolCat_NoViolations:
+ return "No violations";
+ case SolCat_ActiveConstraintDoesNothing:
+ return "Active constraint is doing nothing";
+ case SolCat_RestitutionImpulsesIgnored:
+ return "Restitution impulses were ignored";
+ case SolCat_TangentialVelocityTooLargeToStick:
+ return "Sticking not possible at this velocity";
+ case SolCat_StickingImpulseExceedsStictionLimit:
+ return "Sticking impulse exceeds stiction limit";
+ case SolCat_GroundAppliesAttractiveImpulse:
+ return "Ground applying attractive impulse";
+ case SolCat_NegativePostCompressionNormalVelocity:
+ return "Post-compression velocity is negative";
+ case SolCat_NoImpulsesApplied:
+ return "No impulses applied; no progress made";
+ case SolCat_UnableToResolveUnknownSlipDirection:
+ return "Unable to calculate unknown slip direction";
+ case SolCat_MinStepCausesSlipDirectionReversal:
+ return "Slip direction reverses with minimum step";
+ case SolCat_NotEvaluated:
+ return "Not yet evaluated";
+ default:
+ return "Unrecognized solution category";
+ }
+}
+
+void Impacter::printActiveSetInfo(const ActiveSetCandidate& asc) const
+{
+ printf("\n");
+ for (int i=0; i<40; ++i) printf("-");
+ printf(" ");
+ printFormattedActiveSet(cout, asc.tangentialStates);
+ printf("\n");
+
+ cout << " deltaU = " << asc.systemVelocityChange << "\n"
+ << " impulse = " << asc.localImpulses << "\n"
+ << " category = "
+ << getSolutionCategoryDescription(asc.solutionCategory) << "\n"
+ << " fitness = " << asc.fitness << "\n"
+ << " worstIdx = " << asc.worstConstraint << endl;
+}
+
+//--------------------- Private methods: helper functions ----------------------
+bool Impacter::
+addInBaseN(int N, Array_<int,ProximalPointIndex>& vec, int addThis) const
+{
+ vec[ProximalPointIndex(0)] += addThis;
+ for (ProximalPointIndex i(0); i<(int)vec.size(); ++i) {
+
+ // Detect overflow.
+ if (vec[i] >= N && i == vec.size()-1)
+ return false;
+
+ // Restore vec to base N.
+ while (vec[i] >= N) {
+ vec[ProximalPointIndex(i+1)] += 1;
+ vec[i] -= N;
+ }
+ }
+ return true;
+}
+
+void Impacter::initializeActiveSetCandidateArray(
+ Array_<ActiveSetCandidate>& activeSetCandidates) const
+{
+ SimTK_ASSERT(activeSetCandidates.empty(), "Input array must be empty.");
+ const int n = (int)m_proximalPointIndices.size();
+ Array_<int,ProximalPointIndex> tangentialStateArray(n,0);
+
+ // Increment until overflow in base 3 to enumerate all possibilities.
+ while (addInBaseN(3, tangentialStateArray, 1)) {
+ ActiveSetCandidate candidate;
+ candidate.tangentialStates = tangentialStateArray;
+ activeSetCandidates.push_back(candidate);
+ }
+}
+
+void Impacter::initializeActiveSetCandidate(const State& s,
+ ActiveSetCandidate& asc) const
+{
+ const int n = (int)m_proximalPointIndices.size();
+ Array_<int,ProximalPointIndex> tangentialStateArray(n,Rolling);
+ for (ProximalPointIndex i(0); i<n; ++i) {
+ const Vec3 vel = m_brick.findLowestPointVelocityInGround(s,
+ m_proximalPointIndices[i]);
+ const Real tangVelMag = vel.getSubVec<2>(0).norm();
+ if (tangVelMag > MaxStickingTangVel)
+ tangentialStateArray[i] = Sliding;
+ }
+ asc.tangentialStates = tangentialStateArray;
+}
+
+void Impacter::resetActiveSetCandidate(ActiveSetCandidate& asc) const
+{
+ asc.systemVelocityChange = Vector(1,0.0);
+ asc.localImpulses = Vector(1,0.0);
+ asc.solutionCategory = SolCat_NotEvaluated;
+ asc.fitness = SimTK::Infinity;
+ asc.worstConstraint = ProximalPointIndex(
+ std::numeric_limits<int>::max());
+}
+
+void Impacter::resetActiveSetCandidateArray(
+ Array_<ActiveSetCandidate>& activeSetCandidates) const
+{
+ for (int i=0; i<(int)activeSetCandidates.size(); ++i)
+ resetActiveSetCandidate(activeSetCandidates[i]);
+}
+
+int Impacter::getIndexOfFirstMultiplier(const State& s,
+ const ProximalPointIndex i) const
+{ return m_brick.get_ballConstraintFirstIndex(s, m_proximalPointIndices[i]); }
+
+Real Impacter::calcAbsDiffBetweenAngles(Real a, Real b) const
+{
+ // Catch unknown angles.
+ if (isNaN(a) || isNaN(b))
+ return SimTK::NaN;
+
+ // Move angles from [-pi, pi] to [0, 2*pi].
+ const Real twopi = 2.0*SimTK::Pi;
+ if (a<0) a += twopi;
+ if (b<0) b += twopi;
+
+ // Subtract, avoiding negative answers; difference is in [0, 2*pi].
+ Real absdif = std::abs(a-b);
+
+ // Difference can be no greater than pi due to periodicity.
+ return (absdif < SimTK::Pi ? absdif : twopi-absdif);
+}
+
+Real Impacter::calcSlidingStepLengthToOrigin(const Vec2& A, const Vec2& B,
+ Vec2& Q) const
+{
+ // Check whether initial tangential velocity is small (impending slip).
+ if (A.norm() < MaxStickingTangVel) {
+ if (PrintDebugInfoStepLength)
+ cout << " --> A.norm() < MaxStickingTangVel; returning 1.0"
+ << endl;
+ Q = B;
+ return 1.0;
+ }
+
+ const Vec2 P = Vec2(0);
+ const Vec2 AtoP = P-A;
+ const Vec2 AtoB = B-A;
+ const Real ABsqr = AtoB.normSqr();
+
+ // Ensure line segment is of meaningful length.
+ if (ABsqr < SimTK::SignificantReal) {
+ if (PrintDebugInfoStepLength)
+ cout << " --> ABsqr < SimTK::SignificantReal; returning 1.0"
+ << endl;
+ Q = B;
+ return 1.0;
+ }
+
+ // Normalized distance from A to Q.
+ const Real stepLength = clamp(0.0, dot(AtoP,AtoB)/ABsqr, 1.0);
+ Q = A + stepLength*AtoB;
+
+ if (PrintDebugInfoStepLength)
+ cout << " --> returning stepLength = " << stepLength
+ << " (distance to closest point is " << Q.norm() << ")" << endl;
+
+ return stepLength;
+}
+
+Real Impacter::calcSlidingStepLengthToOrigin(const Vec3& A, const Vec3& B,
+ Vec3& Q) const
+{
+ // Check whether initial tangential velocity is small (impending slip).
+ if (A.norm() < MaxStickingTangVel) {
+ if (PrintDebugInfoStepLength)
+ cout << " --> A.norm() < MaxStickingTangVel; returning 1.0"
+ << endl;
+ Q = B;
+ return 1.0;
+ }
+
+ const Vec3 P = Vec3(0);
+ const Vec3 AtoP = P-A;
+ const Vec3 AtoB = B-A;
+ const Real ABsqr = AtoB.normSqr();
+
+ // Ensure line segment is of meaningful length.
+ if (ABsqr < SimTK::SignificantReal) {
+ if (PrintDebugInfoStepLength)
+ cout << " --> ABsqr < SimTK::SignificantReal; returning 1.0"
+ << endl;
+ Q = B;
+ return 1.0;
+ }
+
+ // Normalized distance from A to Q.
+ const Real stepLength = clamp(0.0, dot(AtoP,AtoB)/ABsqr, 1.0);
+ Q = A + stepLength*AtoB;
+
+ if (PrintDebugInfoStepLength)
+ cout << " --> returning stepLength = " << stepLength
+ << " (distance to closest point is " << Q.norm() << ")" << endl;
+
+ return stepLength;
+}
+
+Real Impacter::
+calcSlidingStepLengthToMaxChange(const Vec2& A, const Vec2& B) const
+{
+ // Temporary variables created by dsolve/numeric/optimize.
+ Real t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, sol1, sol2;
+ const Vec2 v = B-A;
+
+ // Optimized computation sequence generated in Maple.
+ t1 = std::cos(MaxSlidingDirChange);
+ t1 *= t1;
+ t2 = t1 - 1;
+ t3 = A[0]*v[1] - A[1]*v[0];
+ t3 = sqrt(-t1*t2*t3*t3);
+ t4 = t2*v[0]*A[0];
+ t5 = A[1]*v[1];
+ t2 *= t5;
+ t6 = v[1]*v[1];
+ t7 = v[0]*v[0];
+ t8 = t6 + t7;
+ t9 = A[1]*A[1];
+ t10 = A[0]*A[0];
+ t1 = t1*(t10*t8 + t8*t9) - t10*t7 - t6*t9 - 2*t5*A[0]*v[0];
+ t5 = t10 + t9;
+ t1 = 1.0 / t1;
+
+ sol1 = -t1*t5*(t2 + t4 + t3);
+ sol2 = -t1*t5*(t2 + t4 - t3);
+
+ if (PrintDebugInfoStepLength)
+ cout << " --> found solutions " << sol1 << " and " << sol2 << endl;
+
+ if (sol1 < 0)
+ return sol2;
+ else if (sol2 < 0)
+ return sol1;
+ else
+ return std::min(sol1, sol2);
+}
+
+Real Impacter::
+calcSlidingStepLengthToMaxChange(const Vec3& A, const Vec3& B) const
+{
+ // Temporary variables created by dsolve/numeric/optimize.
+ Real t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, t11, t12, t13, t14, t15;
+ Real sol1, sol2;
+ const Vec3 v = B-A;
+
+ // Optimized computation sequence generated in Maple.
+ t1 = std::cos(MaxSlidingDirChange);
+ t1 *= t1;
+ t2 = t1 - 1;
+ t3 = A[0] * A[0];
+ t4 = v[0] * v[0];
+ t5 = A[2] * A[2];
+ t6 = v[1] * v[1];
+ t7 = A[1] * A[1];
+ t8 = A[1] * v[1];
+ t9 = A[0] * v[0];
+ t10 = sqrt(-(t1 * t2 * (t3 * t6 + t4 * t7 + t5 * (t6 + t4) + (-2 * A[2]
+ * (t9 + t8) + (t7 + t3) * v[2]) * v[2] - 2 * t8 * t9)));
+ t11 = t9 * t2;
+ t12 = t8 * t2;
+ t13 = A[2] * v[2];
+ t2 = t13 * t2;
+ t14 = v[2] * v[2];
+ t15 = t6 + t14 + t4;
+ t1 = t1 * (t15 * t3 + t15 * t5 + t15 * t7) - t14 * t5 - t3 * t4 - t6 * t7
+ + t9 * (-2 * t8 - 2 * t13) - 2 * t13 * t8;
+ t3 = t7 + t3 + t5;
+ t1 = 1.0 / t1;
+
+ sol1 = -(t12 + t2 + t11 + t10) * t1 * t3;
+ sol2 = -(t12 + t2 + t11 - t10) * t1 * t3;
+
+ if (PrintDebugInfoStepLength)
+ cout << " --> found solutions " << sol1 << " and " << sol2 << endl;
+
+ if (sol1 < 0)
+ return sol2;
+ else if (sol2 < 0)
+ return sol1;
+ else
+ return std::min(sol1, sol2);
+}
+
+bool Impacter::isActiveSetCandidateEmpty(const ActiveSetCandidate& asc) const
+{
+ for (ProximalPointIndex i(0); i<(int)asc.tangentialStates.size(); ++i)
+ if (asc.tangentialStates[i] > Observing)
+ return false;
+ return true;
+}
+
+bool Impacter::
+isAnySlidingDirectionUndefined(const Array_<Real>& slidingDirections) const
+{
+ for (int i=0; i<(int)slidingDirections.size(); ++i)
+ if (isNaN(slidingDirections[i]))
+ return true;
+ return false;
+}
+
+//------------------------ Private methods: calculators ------------------------
+Real Impacter::calcCOR(Real vNormal) const
+{
+ if (-vNormal < m_brick.get_vMinRebound())
+ return 0.0;
+ const Real CORline = ((m_brick.get_minCOR() - 1) /
+ m_brick.get_vPlasticDeform()) * (-vNormal) + 1;
+ return std::max(CORline, m_brick.get_minCOR());
+}
+
+void Impacter::generateAndSolveLinearSystem(const State& s0,
+ const ImpactPhase impactPhase,
+ const Vector& restitutionImpulses,
+ ActiveSetCandidate& asc) const
+{
+ // Enable constraints to initialize the Jacobian.
+ State s = m_mbs.realizeTopology();
+ s.setQ(s0.getQ());
+ s.setU(s0.getU());
+ for (ProximalPointIndex i(0); i<(int)m_proximalPointIndices.size(); ++i)
+ if (asc.tangentialStates[i] > Observing)
+ m_brick.enableBallConstraint(s, m_proximalPointIndices[i]);
+ m_mbs.realize(s, Stage::Velocity);
+
+ // Begin generating linear system to solve.
+ Matrix MassMatrix, GMatrix;
+ m_mbs.getMatterSubsystem().calcM(s, MassMatrix);
+ m_mbs.getMatterSubsystem().calcG(s, GMatrix);
+ const int N = MassMatrix.nrow();
+ const int M = GMatrix.nrow();
+
+ // TODO: Should not need to form a matrix larger than MxM here.
+ // 1. Solve only for impulses.
+ // 2. During restitution, remove equations with no degrees of freedom
+ // (i.e., all normal impulses, and tangential impulses associated
+ // with sliding points).
+ // 3. Figure out how to modify Simbody's GM\~G computation to create
+ // G'M\~G, where G' means modifying the equations associated with
+ // sliding (and normal impulses in the restitution phase).
+ Matrix A = Matrix(N+M, N+M);
+ A(0,0,N,N) = MassMatrix;
+ A(0,N,N,M) = GMatrix.transpose();
+ A(N,0,M,N) = GMatrix;
+ A(N,N,M,M) = 0.0;
+ Vector b = Vector(N+M, 0.0);
+
+ // Define equations.
+ Array_<Real> slidingDirections;
+ for (ProximalPointIndex idx(0);
+ idx<(int)m_proximalPointIndices.size(); ++idx)
+ {
+ if (asc.tangentialStates[idx] > Observing) {
+
+ // Current velocity at this proximal point.
+ const Vec3 currVelAtPoint = m_brick.findLowestPointVelocityInGround(
+ s, m_proximalPointIndices[idx]);
+
+ // Row indices into matrix A corresponding to the constraints for
+ // this proximal point.
+ const int row_x = N + getIndexOfFirstMultiplier(s,idx);
+ const int row_y = row_x + 1;
+ const int row_z = row_y + 1;
+
+ // Tangential directions.
+ if (asc.tangentialStates[idx] == Rolling) {
+
+ // Drive both components of tangential velocity to zero.
+ b[row_x] = -currVelAtPoint[XAxis];
+ b[row_y] = -currVelAtPoint[YAxis];
+
+ } else if (asc.tangentialStates[idx] == Sliding) {
+
+ // Apply friction impulse in the direction opposing the sliding
+ // direction. At this point, set muDyn=0 for all points with
+ // unknown sliding directions.
+ A(row_x,0,2,N) = 0.0;
+ A[row_x][row_x] = 1; b[row_x] = 0;
+ A[row_y][row_y] = 1; b[row_y] = 0;
+
+ // Calculate theta, the angle between the global X-axis and the
+ // tangential velocity vector.
+ const Real theta = m_brick.
+ findTangentialVelocityAngle(currVelAtPoint);
+ slidingDirections.push_back(theta);
+
+ if (PrintDebugInfoImpact)
+ cout << " ** angle of tangential velocity vector for "
+ << "proximal point " << idx << " is " << theta << endl;
+
+ if (!isNaN(theta)) {
+ const Real impulseDir = theta + SimTK::Pi;
+ A[row_x][row_z] = -m_brick.get_muDyn() * cos(impulseDir);
+ A[row_y][row_z] = -m_brick.get_muDyn() * sin(impulseDir);
+ }
+ }
+
+ // Normal direction.
+ if (impactPhase == Compression) {
+
+ // Populate with the compression equation, which drives the
+ // normal velocity of the impacting point to zero.
+ b[row_z] = -currVelAtPoint[ZAxis];
+
+ } else if (impactPhase == Restitution) {
+
+ // Populate with the restitution equation, which sets the normal
+ // impulse to the impulse required in the restitution phase.
+ A(row_z,0,1,N) = 0.0;
+ A[row_z][row_z] = 1;
+ b[row_z] = -restitutionImpulses[idx];
+ }
+
+ } //end if not observing
+ } //end for each proximal point
+
+ // Iterate to find sliding directions, if necessary.
+ if (isAnySlidingDirectionUndefined(slidingDirections)) {
+ if (PrintDebugInfoImpact) {
+ cout << " ** finding sliding directions" << endl;
+
+ int slidingPointNum = -1;
+ for (ProximalPointIndex idx(0);
+ idx<(int)m_proximalPointIndices.size(); ++idx)
+ if (asc.tangentialStates[idx] == Sliding) {
+ ++slidingPointNum;
+
+ // Current velocity at this proximal point.
+ const Vec3 vT = m_brick.findLowestPointVelocityInGround(s,
+ m_proximalPointIndices[idx]);
+ cout << " v[" << idx << "] = " << vT << " (angle = "
+ << slidingDirections[slidingPointNum] << " rad)"
+ << endl;
+ }
+ }
+
+ int numIter = 0;
+ ProximalPointIndex worstConstraintIdx;
+ while(true) {
+
+ // Halt if maximum number of iterations is reached.
+ ++numIter;
+ if (numIter > MaxIterSlipDirection) {
+ if (PrintDebugInfoImpact)
+ cout << " ** maximum number of iterations reached" << endl;
+
+ asc.solutionCategory =
+ SolCat_UnableToResolveUnknownSlipDirection;
+ asc.fitness = SimTK::Infinity;
+ asc.worstConstraint = worstConstraintIdx;
+ break;
+ }
+ if (PrintDebugInfoImpact)
+ cout << " ** beginning iteration " << numIter << endl;
+
+ // Solve using current directions. We have set muDyn=0 for all
+ // points with unknown sliding directions.
+ FactorQTZ qtzA(A);
+ Vector sol;
+ qtzA.solve(b, sol);
+
+ // Calculate new system velocities using SlidingDirStepLength, which
+ // we presume is sufficiently small to avoid direction reversals
+ // (except in situations where |v_t| < MaxStickingTangVel).
+ Vector deltaU = sol(0,N);
+ Vector calcImpulse = sol(N,M);
+
+ if (PrintDebugInfoImpact)
+ cout << " calculated deltaU = " << deltaU << endl
+ << " calculated impulse = " << calcImpulse << endl;
+
+ State sTemp(s);
+ sTemp.setU(s.getU() + SlidingDirStepLength*deltaU);
+ m_mbs.realize(sTemp, Stage::Velocity);
+
+ // Update directions of all sliding points (not just those with
+ // unknown sliding directions).
+ Real maxAngleDif = 0;
+ int slidingPointNum = -1;
+ for (ProximalPointIndex idx(0);
+ idx<(int)m_proximalPointIndices.size(); ++idx)
+ {
+ if (asc.tangentialStates[idx] == Sliding) {
+ ++slidingPointNum;
+
+ // Determine new angle from proposed velocity at this point.
+ const Vec3 vTemp = m_brick.findLowestPointVelocityInGround(
+ sTemp, m_proximalPointIndices[idx]);
+ Real newAngle = m_brick.findTangentialVelocityAngle(vTemp);
+
+ if (PrintDebugInfoImpact)
+ cout << " v[" << idx << "] = " << vTemp
+ << " (angle = " << newAngle << " rad)" << endl;
+
+ // Keep track of maximum absolute difference in angle (note
+ // that oldAngle and/or newAngle might be NaN).
+ Real oldAngle = slidingDirections[slidingPointNum];
+ slidingDirections[slidingPointNum] = newAngle;
+
+ if (isNaN(oldAngle) || isNaN(newAngle)) {
+ maxAngleDif = SimTK::Infinity;
+ worstConstraintIdx = idx;
+ } else {
+ Real angleDif = calcAbsDiffBetweenAngles(oldAngle,
+ newAngle);
+ if (angleDif > maxAngleDif) {
+ maxAngleDif = angleDif;
+ worstConstraintIdx = idx;
+ }
+ }
+
+ if (PrintDebugInfoImpact)
+ cout << " old angle = " << oldAngle
+ << ", new angle = " << newAngle << endl;
+
+ // Update linear system.
+ const int row_x = N + getIndexOfFirstMultiplier(s,idx);
+ const int row_y = row_x + 1;
+ const int row_z = row_y + 1;
+
+ if (!isNaN(newAngle)) {
+ const Real impulseDir = newAngle + SimTK::Pi;
+ A[row_x][row_z] = -m_brick.get_muDyn()*cos(impulseDir);
+ A[row_y][row_z] = -m_brick.get_muDyn()*sin(impulseDir);
+ } else {
+ A[row_x][row_z] = 0;
+ A[row_y][row_z] = 0;
+ }
+
+ } //end if this point is sliding
+ } //end for each proximal point
+
+ if (PrintDebugInfoImpact)
+ cout << " maximum angle change of " << maxAngleDif << endl;
+
+ // Exit if converged.
+ if (maxAngleDif < TolMaxDifDirIteration) {
+ if (PrintDebugInfoImpact)
+ cout << " ** sliding directions converged" << endl;
+ break;
+ }
+
+ // Exit if a sliding direction flips. By our assumption about the
+ // smallness of SlidingDirStepLength, obtaining a flipping direction
+ // indicates that this point should actually be sticking.
+ if (std::abs(maxAngleDif-SimTK::Pi) < TolMaxDifDirIteration) {
+ if (PrintDebugInfoImpact)
+ cout << " ** point will stick, not slide" << endl;
+
+ asc.solutionCategory =
+ SolCat_MinStepCausesSlipDirectionReversal;
+ asc.fitness = SimTK::Infinity;
+ asc.worstConstraint = worstConstraintIdx;
+ return; //Giving up.
+ }
+
+ } //end while directions are unknown
+ } //end if points are sliding
+
+ // Either no points are sliding, or have finished iterating and need one
+ // more solve to reconcile friction impulses with newest directions.
+ // TODO: Do not need to solve a second time if initial directions were
+ // acceptable.
+ // TODO: Replace the above fixed-point iteration with a Newton iteration
+ // (provided the NaN directions are resolved first).
+ FactorQTZ qtzA(A);
+ Vector sol;
+ qtzA.solve(b, sol);
+
+ // Extract system velocity changes and local impulses from sol vector.
+ asc.systemVelocityChange = sol(0,N);
+ asc.localImpulses = sol(N,M);
+
+ if (PrintDebugInfoImpact) {
+ cout << " proximal point velocities after full step:" << endl;
+ State sTemp(s);
+ sTemp.updU() += 1.0*asc.systemVelocityChange;
+ m_mbs.realize(sTemp, Stage::Velocity);
+
+ for (ProximalPointIndex i(0); i<(int)m_proximalPointIndices.size(); ++i)
+ cout << " [" << i << "] v="
+ << m_brick.findLowestPointVelocityInGround(sTemp,
+ m_proximalPointIndices[i]) << endl;
+ }
+}
+
+void Impacter::evaluateLinearSystemSolution(const State& s,
+ const ImpactPhase impactPhase,
+ const Vector& restitutionImpulses,
+ ActiveSetCandidate& asc) const
+{
+ // Return if already evaluated; SolCat_UnableToResolveUnknownSlipDirection
+ // and SolCat_MinStepCausesSlipDirectionReversal will have been caught in
+ // generateAndSolveLinearSystem.
+ if (asc.solutionCategory < SolCat_NotEvaluated)
+ return;
+
+ // Gather information about active set candidate.
+ const int numImpulses = (int)asc.localImpulses.size();
+ SimTK_ASSERT(numImpulses%3 == 0, "Invalid number of impulses.");
+ const int numConstraints = numImpulses/3;
+
+ // Calculate proximal point velocities after taking a full step.
+ State sFullStep(s);
+ sFullStep.updU() += 1.0*asc.systemVelocityChange;
+ m_mbs.realize(sFullStep, Stage::Velocity);
+
+ Array_<Vec3,ProximalPointIndex> fullStepVel(m_proximalPointIndices.size());
+ for (ProximalPointIndex i(0); i<(int)m_proximalPointIndices.size(); ++i)
+ fullStepVel[i] = m_brick.findLowestPointVelocityInGround(sFullStep,
+ m_proximalPointIndices[i]);
+
+ // No impulses applied; no progress made -- avoid infinite looping.
+ if (asc.localImpulses.norm() < MinMeaningfulImpulse) {
+ asc.solutionCategory = SolCat_NoImpulsesApplied;
+ asc.fitness = SimTK::Infinity;
+ return;
+ }
+
+ // Post-compression velocity is negative -- the linear system was generated
+ // incorrectly; the solution is nonsense. Not relevant for proximal points
+ // that are just observing; these would be addressed in a follow-up impact.
+ if (impactPhase == Compression) {
+ Real minNormVel = SimTK::Infinity;
+ ProximalPointIndex minNormVelIdx;
+ for (ProximalPointIndex i(0); i<(int)fullStepVel.size(); ++i) {
+ if (asc.tangentialStates[i] > Observing &&
+ fullStepVel[i][ZAxis] < minNormVel) {
+
+ minNormVel = fullStepVel[i][ZAxis];
+ minNormVelIdx = i;
+ }
+ }
+ if (minNormVel < -TolVelocityFuzziness) {
+ asc.solutionCategory = SolCat_NegativePostCompressionNormalVelocity;
+ asc.fitness = -minNormVel;
+ asc.worstConstraint = minNormVelIdx;
+ return;
+ }
+ }
+
+ // Ground applying attractive impulse -- the normal impulse must always be
+ // negative (note the sign convention).
+ Real maxNormImpulse = 0;
+ ProximalPointIndex maxNormImpulseIdx;
+ int constraintIdx = -1;
+ for (ProximalPointIndex i(0); i<(int)m_proximalPointIndices.size(); ++i) {
+ if (asc.tangentialStates[i] > Observing) {
+ ++constraintIdx;
+
+ if (asc.localImpulses[constraintIdx*3+2] > maxNormImpulse) {
+ maxNormImpulse = asc.localImpulses[constraintIdx*3+2];
+ maxNormImpulseIdx = i;
+ }
+ }
+ }
+ if (maxNormImpulse > MinMeaningfulImpulse) {
+ asc.solutionCategory = SolCat_GroundAppliesAttractiveImpulse;
+ asc.fitness = maxNormImpulse;
+ asc.worstConstraint = maxNormImpulseIdx;
+ return;
+ }
+
+ // Sticking impulse exceeds stiction limit -- should be sliding instead.
+ Real maxExcessiveImpulse = 0;
+ ProximalPointIndex maxExcessiveImpulseIdx;
+ constraintIdx = -1;
+ for (ProximalPointIndex i(0); i<(int)m_proximalPointIndices.size(); ++i) {
+ if (asc.tangentialStates[i] > Observing) {
+ ++constraintIdx;
+
+ if (asc.tangentialStates[i] == Rolling) {
+ const int Xidx = constraintIdx*3;
+ const Real impTangMag = Vec2(asc.localImpulses[Xidx],
+ asc.localImpulses[Xidx+1]).norm();
+ const Real impNormMag = -asc.localImpulses[Xidx+2];
+ const Real excessiveImpulse = impTangMag
+ - m_brick.get_muDyn()*impNormMag;
+
+ if (excessiveImpulse > MinMeaningfulImpulse &&
+ excessiveImpulse > maxExcessiveImpulse) {
+ maxExcessiveImpulse = excessiveImpulse;
+ maxExcessiveImpulseIdx = i;
+ }
+ }
+ }
+ }
+ if (maxExcessiveImpulse > MinMeaningfulImpulse) {
+ asc.solutionCategory = SolCat_StickingImpulseExceedsStictionLimit;
+ asc.fitness = maxExcessiveImpulse;
+ asc.worstConstraint = maxExcessiveImpulseIdx;
+ return;
+ }
+
+ // Sticking not possible at this velocity -- the magnitude of the initial
+ // tangential velocity (i.e., at the beginning of the interval) must be
+ // sufficiently small to allow sticking.
+ State sCurr(s);
+ m_mbs.realize(sCurr, Stage::Velocity);
+ Real maxTangVelMag = 0;
+ ProximalPointIndex maxTangVelMagIdx;
+ for (ProximalPointIndex i(0); i<(int)m_proximalPointIndices.size(); ++i) {
+ if (asc.tangentialStates[i] == Rolling) {
+ const Vec3 vel = m_brick.findLowestPointVelocityInGround(sCurr,
+ m_proximalPointIndices[i]);
+ const Real tangVelMag = vel.getSubVec<2>(0).norm();
+
+ if (tangVelMag > maxTangVelMag) {
+ maxTangVelMag = tangVelMag;
+ maxTangVelMagIdx = i;
+ }
+ }
+ }
+ if (maxTangVelMag > MaxStickingTangVel) {
+ asc.solutionCategory = SolCat_TangentialVelocityTooLargeToStick;
+ asc.fitness = maxTangVelMag;
+ asc.worstConstraint = maxTangVelMagIdx;
+ return;
+ }
+
+ // Restitution impulses were ignored -- avoid applying restitution impulses
+ // sequentially (should be applied simultaneously).
+ if (impactPhase == Restitution) {
+ Real ignoredImpulse = 0;
+ for (int i=0; i<(int)restitutionImpulses.size(); ++i)
+ ignoredImpulse += restitutionImpulses[i];
+ for (int i=0; i<numConstraints; ++i)
+ ignoredImpulse -= -asc.localImpulses[i*3+2];
+
+ if (ignoredImpulse > MinMeaningfulImpulse*restitutionImpulses.size()) {
+ asc.solutionCategory = SolCat_RestitutionImpulsesIgnored;
+ asc.fitness = ignoredImpulse;
+ return;
+ }
+ }
+
+ // Active constraint is doing nothing -- prefer to avoid active sets with
+ // constraints that apply no impulses.
+ for (int i=0; i<numConstraints; ++i) {
+ if (Vec3(asc.localImpulses[i*3], asc.localImpulses[i*3+1],
+ asc.localImpulses[i*3+2]).norm() < MinMeaningfulImpulse) {
+ asc.solutionCategory = SolCat_ActiveConstraintDoesNothing;
+ asc.fitness = asc.localImpulses.norm(); //As usual.
+ return;
+ }
+ }
+
+ // No violations -- ideal case.
+ asc.solutionCategory = SolCat_NoViolations;
+ asc.fitness = asc.localImpulses.norm();
+}
+
+Real Impacter::calculateIntervalStepLength(const State& s0,
+ const Array_<Vec3>& currVels,
+ const ActiveSetCandidate& asc,
+ int intervalCtr) const
+{
+ // This is the return value, which will strictly decrease as we iterate
+ // through the sliding proximal points.
+ Real steplength = 1.0;
+
+ // State at the beginning of the current interval.
+ State s(s0);
+ m_mbs.realize(s, Stage::Velocity);
+
+ // State after taking the proposed step length. Updated below whenever the
+ // proposed step length is decreased.
+ State sProp(s);
+ sProp.updU() += steplength*asc.systemVelocityChange;
+ m_mbs.realize(sProp, Stage::Velocity);
+
+ // Loop through each sliding proximal point and reduce the step length if
+ // necessary.
+ for (ProximalPointIndex i(0); i<(int)asc.tangentialStates.size(); ++i) {
+ if (asc.tangentialStates[i] == Sliding) {
+
+ if (PrintDebugInfoStepLength)
+ cout << " ** analyzing proximal point " << i << "..." << endl;
+
+ // Any step length is acceptable for this point if its initial
+ // tangential velocity vector lies within the "capture velocity
+ // circle" defined by MaxStickingTangVel (i.e., the point is in
+ // impending slip).
+ if (currVels[i].getSubVec<2>(0).norm() < MaxStickingTangVel) {
+ if (PrintDebugInfoStepLength)
+ cout << " -- finished with proximal point " << i
+ << " (impending slip); current steplength is "
+ << steplength << endl;
+ continue; //Proceed to next point.
+ }
+
+ // Calculate the velocity of this proximal point after taking the
+ // proposed step length.
+ const Vec3 propVel = m_brick.findLowestPointVelocityInGround(sProp,
+ m_proximalPointIndices[i]);
+
+ // The current proposed step length is acceptable for this point if
+ // its tangential velocity vector lands within the "capture velocity
+ // circle" (i.e., the sliding point will begin to roll).
+ if (propVel.getSubVec<2>(0).norm() < MaxStickingTangVel) {
+ if (PrintDebugInfoStepLength)
+ cout << " -- finished with proximal point " << i
+ << " (rolling imminent); current steplength is "
+ << steplength << endl;
+ continue; //Proceed to next point.
+ }
+
+ // Check whether the direction change is sufficiently small.
+ const Real ang0 = m_brick.findTangentialVelocityAngle(currVels[i]);
+ const Real ang1 = m_brick.findTangentialVelocityAngle(propVel);
+ if (calcAbsDiffBetweenAngles(ang0, ang1) < MaxSlidingDirChange) {
+ if (PrintDebugInfoStepLength)
+ cout << " -- finished with proximal point " << i
+ << " (acceptable change); current steplength is "
+ << steplength << endl;
+ continue; //Proceed to next point.
+ }
+
+ // Search #1: Determine whether there exists a step length where the
+ // tangential velocity vector lies within the "capture
+ // velocity circle" (i.e., the sliding point will begin
+ // to roll). Since the system is linear, we simply find
+ // the point on the line segment connecting currVels[i]
+ // and propVel that is closest to the origin.
+ Vec2 closestPoint;
+ const Real stepToOrigin = calcSlidingStepLengthToOrigin(
+ currVels[i].getSubVec<2>(0),
+ propVel.getSubVec<2>(0),
+ closestPoint);
+ if (stepToOrigin < 1.0 && closestPoint.norm() < MaxStickingTangVel)
+ {
+ // Adopt the new step length and calculate the new proposed
+ // post-interval velocities.
+ steplength *= stepToOrigin;
+ sProp.setU(s.getU() + steplength*asc.systemVelocityChange);
+ m_mbs.realize(sProp, Stage::Velocity);
+
+ if (PrintDebugInfoStepLength)
+ cout << " -- finished with proximal point " << i
+ << " (transition to rolling); current steplength is "
+ << steplength << endl;
+
+ if (DebugStepLengthCalculator) {
+ const Vec3 testVel = m_brick.findLowestPointVelocityInGround
+ (sProp, m_proximalPointIndices[i]);
+ const Real testMag = testVel.getSubVec<2>(0).norm();
+ cout << " testMag = " << testMag
+ << " (steplength = " << steplength << ")" << endl;
+
+ SimTK_ASSERT(testMag < MaxStickingTangVel,
+ "Incorrectly detected transition to rolling.");
+ }
+
+ continue; //Proceed to next point.
+ }
+
+ // Search #2: We have determined that this point will continue to
+ // slide. Search for the step length that results in the
+ // maximum allowable sliding direction change.
+ const Real stepToMaxChange = calcSlidingStepLengthToMaxChange(
+ currVels[i].getSubVec<2>(0),
+ propVel.getSubVec<2>(0));
+ SimTK_ASSERT(stepToMaxChange >= 0 && stepToMaxChange <= 1,
+ "Invalid step length calculated to elicit maximum change.");
+
+ // Adopt the new step length and calculate the new proposed post-
+ // interval velocities.
+ steplength *= stepToMaxChange;
+ sProp.setU(s.getU() + steplength*asc.systemVelocityChange);
+ m_mbs.realize(sProp, Stage::Velocity);
+
+ if (PrintDebugInfoStepLength)
+ cout << " -- finished with proximal point " << i
+ << " (maximum direction change); current steplength is "
+ << steplength << endl;
+
+ if (DebugStepLengthCalculator) {
+ const Vec3 testVel = m_brick.findLowestPointVelocityInGround(
+ sProp, m_proximalPointIndices[i]);
+ const Real testAng = m_brick.findTangentialVelocityAngle(
+ testVel);
+ const Real testDif = calcAbsDiffBetweenAngles(ang0, testAng);
+ cout << " testDif = " << testDif
+ << " (steplength = " << steplength << ")" << endl;
+
+ SimTK_ASSERT(std::abs(testDif-MaxSlidingDirChange) < 1.0e-8,
+ "Incorrect solution to find maximum direction change.");
+ }
+
+ } //end if sliding
+ } //end for each proximal point
+
+ if (PrintDebugInfoStepLength)
+ cout << " ** final steplength is " << steplength << endl;
+
+ return steplength;
+}
diff --git a/Simbody/tests/adhoc/TestRiboseMobilizer.cpp b/Simbody/tests/adhoc/TestRiboseMobilizer.cpp
new file mode 100644
index 0000000..75a4d87
--- /dev/null
+++ b/Simbody/tests/adhoc/TestRiboseMobilizer.cpp
@@ -0,0 +1,528 @@
+#include "SimTKsimbody.h"
+#include <iostream>
+
+using namespace std;
+using namespace SimTK;
+
+// eliding boost::units expressions to make a more portable example --cmb
+// pay no attention to these types and units. They are hollow echo of
+// the use of boost::units in an earlier version of this example
+typedef Real angle_t;
+typedef Real length_t;
+typedef Real mass_t;
+typedef Vec3 location_t;
+Real degrees = 3.14159/180.0;
+Real radians = 1.0;
+Real angstroms = 0.10;
+Real nanometers = 1.0;
+Real daltons = 1.0;
+
+class ZeroFunction : public Function::Constant {
+public:
+ ZeroFunction() : Function::Constant(0, 0) {}
+};
+
+class UnityFunction : public Function::Constant {
+public:
+ UnityFunction() : Function::Constant(1.0, 0) {}
+};
+
+class IdentityFunction : public Function {
+public:
+ IdentityFunction() {}
+
+ Real calcValue(const Vector& x) const{
+ return x[0];
+ }
+
+ Real calcDerivative(const Array_<int>& derivComponents, const Vector& x) const{
+ if (derivComponents.size() == 1) return 1.0;
+ else return 0.0;
+ }
+
+ int getArgumentSize() const {return 1;}
+
+ int getMaxDerivativeOrder() const {return 1000;}
+};
+
+// Difference between two components of a two component vector
+// Used as a target function for equality coupling constraint
+class DifferenceFunction : public Function::Linear {
+public:
+ DifferenceFunction() : Function::Linear( createCoefficients() ) {}
+
+private:
+ static Vector createCoefficients() {
+ Vector answer(3);
+ answer[0] = 1.0; // ax
+ answer[1] = -1.0; // -by
+ answer[2] = 0.0; // +c
+ return answer;
+ }
+};
+
+
+/// Function that when zero, ensures that three variables are equal
+/// f(x,y,z) = (x-y)^2 + (x-z)^2 + (y-z)^2
+/// df/dx = 2(x-y) + 2(x-z)
+/// df/dx^2 = 4
+/// df/dxdy = -2
+class ThreeDifferencesFunction : public Function {
+public:
+ Real calcValue(const Vector& x) const
+ {
+ assert( 3 == x.size() );
+
+ Real dxy(x[0] - x[1]);
+ Real dxz(x[0] - x[2]);
+ Real dyz(x[1] - x[2]);
+
+ return dxy*dxy + dxz*dxz + dyz*dyz;
+ }
+
+ Real calcDerivative(const Array_<int>& derivComponents, const Vector& x) const
+ {
+ Real deriv = 0;
+
+ assert(3 == x.size());
+
+ int derivOrder = derivComponents.size();
+ assert(1 <= derivOrder);
+
+ if (derivOrder == 1)
+ {
+ // too clever
+ // df/dx
+ // = 2(x-y) + 2(x-z)
+ // = 2x - 2y + 2x - 2z
+ // = 4x - 2y - 2z
+ // = 6x - 2x - 2y - 2z
+ deriv = 2 * (3 * x[derivComponents[0]] -x[0] -x[1] -x[2]);
+ }
+ else if (derivOrder == 2)
+ {
+ if (derivComponents[0] == derivComponents[1])
+ deriv = 4.0; // df/dx^2
+ else
+ deriv = -2.0; // df/dxdy
+ }
+ else ; // all derivatives higher than two are zero
+
+ return deriv;
+ }
+
+ int getArgumentSize() const{
+ return 3;
+ }
+
+ int getMaxDerivativeOrder() const{
+ return 1000;
+ }
+};
+
+/// Implements a simple functional relationship, y = amplitude * sin(x - phase)
+class SinusoidFunction : public Function {
+private:
+ angle_t amplitude;
+ angle_t phase;
+public:
+
+ //Default constructor
+ SinusoidFunction()
+ : amplitude(180.0*degrees), phase(0.0*degrees) {}
+
+ //Convenience constructor to specify the slope and Y-intercept of the linear r
+ SinusoidFunction(angle_t amp, angle_t phi)
+ : amplitude(amp), phase(phi) {}
+
+ Real calcValue(const Vector& x) const{
+ assert( 1 == x.size() );
+ return angle_t(amplitude*sin(x[0]*radians - phase));
+ }
+
+ Real calcDerivative(const Array_<int>& derivComponents, const Vector& x) const{
+ Real deriv = 0;
+
+ assert(1 == x.size());
+
+ // Derivatives repeat after 4
+ int derivOrder = derivComponents.size() % 4;
+
+ // Derivatives 1, 5, 9, 13, ... are cos()
+ if ( 1 == derivOrder ) {
+ deriv = angle_t(amplitude*cos(x[0]*radians - phase));
+ }
+ // Derivatives 2, 6, 10, 14, ... are -sin()
+ else if ( 2 == derivOrder ) {
+ deriv = angle_t(-amplitude*sin(x[0]*radians - phase));
+ }
+ // Derivatives 3, 7, 11, 15, ... are -cos()
+ else if ( 3 == derivOrder ) {
+ deriv = angle_t(-amplitude*cos(x[0]*radians - phase));
+ }
+ // Derivatives 0, 4, 8, 12, ... are sin()
+ else if ( 0 == derivOrder ) {
+ deriv = angle_t(amplitude*sin(x[0]*radians - phase));
+ }
+ else assert(false);
+
+ return deriv;
+ }
+
+ int getArgumentSize() const{
+ return 1;
+ }
+
+ int getMaxDerivativeOrder() const{
+ return 1000;
+ }
+};
+
+// The pupose of TestPinMobilizer is to prove that I have understood the
+// basic syntax of Function-based mobilizers
+class TestPinMobilizer : public MobilizedBody::FunctionBased
+{
+public:
+ TestPinMobilizer(
+ MobilizedBody& parent,
+ const Transform& inbFrame,
+ const Body& body,
+ const Transform& outbFrame
+ )
+ : MobilizedBody::FunctionBased(
+ parent,
+ inbFrame,
+ body,
+ outbFrame,
+ 1,
+ createFunctions(),
+ createCoordIndices()
+ )
+ {}
+
+private:
+ static std::vector< const Function* > createFunctions() {
+ std::vector< const Function* > functions;
+ functions.push_back(new ZeroFunction ); // x rotation
+ functions.push_back(new ZeroFunction ); // y rotation
+
+ // Z-rotation is the only degree of freedom for this mobilizer
+ functions.push_back(new IdentityFunction ); // z rotation
+
+ functions.push_back(new ZeroFunction ); // x translation
+ functions.push_back(new ZeroFunction ); // y translation
+ functions.push_back(new ZeroFunction ); // z translation
+
+ return functions;
+ }
+
+ static std::vector< std::vector<int> > createCoordIndices() {
+ std::vector<int> oneCoordinateVec;
+ oneCoordinateVec.push_back(0); // first and only generalized coordinate index
+
+ std::vector< std::vector<int> > coordIndices;
+ // All six functions take the one generalized coordinate
+ coordIndices.push_back(std::vector<int>()); // 1, empty
+ coordIndices.push_back(std::vector<int>()); // 2, empty
+ coordIndices.push_back(oneCoordinateVec); // 3, one coordinate, zero
+ coordIndices.push_back(std::vector<int>()); // 4, empty
+ coordIndices.push_back(std::vector<int>()); // 5, empty
+ coordIndices.push_back(std::vector<int>()); // 6, empty
+
+ return coordIndices;
+ }
+};
+
+class PseudorotationMobilizer : public MobilizedBody::FunctionBased
+{
+public:
+ PseudorotationMobilizer(
+ MobilizedBody& parent,
+ const Transform& inbFrame,
+ const Body& body,
+ const Transform& outbFrame,
+ angle_t amplitude,
+ angle_t phase
+ )
+ : MobilizedBody::FunctionBased(
+ parent,
+ inbFrame,
+ body,
+ outbFrame,
+ 1,
+ createFunctions(amplitude, phase),
+ createCoordIndices() // TODO ask Ajay
+ )
+ {}
+
+private:
+ static std::vector< const Function* > createFunctions(angle_t amplitude, angle_t phase) {
+ std::vector< const Function* > functions;
+ functions.push_back(new ZeroFunction ); // x rotation
+ functions.push_back(new ZeroFunction ); // y rotation
+
+ // Z-rotation is the only degree of freedom for this mobilizer
+ functions.push_back(new SinusoidFunction(amplitude, phase) ); // z rotation
+
+ functions.push_back(new ZeroFunction ); // x translation
+ functions.push_back(new ZeroFunction ); // y translation
+ functions.push_back(new ZeroFunction ); // z translation
+
+ return functions;
+ }
+
+ static std::vector< std::vector<int> > createCoordIndices() {
+ std::vector<int> oneCoordinateVec;
+ oneCoordinateVec.push_back(0); // first and only generalized coordinate index
+
+ std::vector< std::vector<int> > coordIndices;
+ // All six functions take the one generalized coordinate
+ coordIndices.push_back(std::vector<int>()); // 1, empty
+ coordIndices.push_back(std::vector<int>()); // 2, empty
+ coordIndices.push_back(oneCoordinateVec); // 3, one coordinate, zero
+ coordIndices.push_back(std::vector<int>()); // 4, empty
+ coordIndices.push_back(std::vector<int>()); // 5, empty
+ coordIndices.push_back(std::vector<int>()); // 6, empty
+
+ return coordIndices;
+ }
+};
+
+
+
+void testRiboseMobilizer()
+{
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ DecorationSubsystem decorations(system);
+
+ matter.setShowDefaultGeometry(false);
+
+ // Put some hastily chosen mass there (doesn't help)
+ Body::Rigid rigidBody;
+ rigidBody.setDefaultRigidBodyMassProperties(MassProperties(
+ mass_t(20.0*daltons),
+ location_t(Vec3(0,0,0)*nanometers),
+ Inertia(20.0)
+ ));
+
+ // One body anchored at C4 atom,
+ MobilizedBody::Weld c4Body(
+ matter.updGround(),
+ Rotation(-120*degrees, XAxis),
+ rigidBody,
+ Transform());
+ // sphere for C4 atom
+ decorations.addBodyFixedDecoration(
+ c4Body.getMobilizedBodyIndex(),
+ Transform(),
+ DecorativeSphere( length_t(0.5*angstroms) )
+ );
+ // sphere for C5 atom
+ decorations.addBodyFixedDecoration(
+ c4Body.getMobilizedBodyIndex(),
+ location_t(Vec3(-1.0,-1.0,0.5)*angstroms),
+ DecorativeSphere( length_t(0.5*angstroms) )
+ );
+
+ decorations.addRubberBandLine(
+ c4Body.getMobilizedBodyIndex(),
+ Vec3(0),
+ c4Body.getMobilizedBodyIndex(),
+ location_t(Vec3(-1.0,-1.0,0.5)*angstroms),
+ DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
+
+ // One body anchored at C3 atom -- works
+ // Pin version
+ //MobilizedBody::Pin c3Body(
+ // c4Body,
+ // Transform(),
+ // rigidBody,
+ // Transform(location_t(Vec3(0,0,1.5)*angstroms))
+ // );
+
+ // Function based pin version -- works
+ //TestPinMobilizer c3Body(
+ // c4Body,
+ // Transform(),
+ // rigidBody,
+ // Transform(location_t(Vec3(0,0,1.5)*angstroms))
+ // );
+
+ PseudorotationMobilizer c3Body(
+ c4Body,
+ Transform(),
+ rigidBody,
+ Transform(location_t(Vec3(0,0,1.5)*angstroms)),
+ angle_t(36.4*degrees), // amplitude
+ angle_t(-161.8*degrees) // phase
+ );
+ // sphere for C3 atom
+ decorations.addBodyFixedDecoration(
+ c3Body.getMobilizedBodyIndex(),
+ Transform(),
+ DecorativeSphere( length_t(0.5*angstroms) )
+ );
+ // sphere for O3 atom
+ decorations.addBodyFixedDecoration(
+ c3Body.getMobilizedBodyIndex(),
+ location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
+ DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(1,0,0))
+ );
+
+ decorations.addRubberBandLine(
+ c3Body.getMobilizedBodyIndex(),
+ Vec3(0),
+ c3Body.getMobilizedBodyIndex(),
+ location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
+ DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
+ decorations.addRubberBandLine(
+ c4Body.getMobilizedBodyIndex(),
+ Vec3(0),
+ c3Body.getMobilizedBodyIndex(),
+ Vec3(0),
+ DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
+
+ PseudorotationMobilizer c2Body(
+ c3Body,
+ Rotation( angle_t(-80*degrees), YAxis ),
+ rigidBody,
+ Transform(location_t(Vec3(0,0,1.5)*angstroms)),
+ angle_t(35.8*degrees), // amplitude
+ angle_t(-91.3*degrees) // phase
+ );
+ // sphere for C2 atom
+ decorations.addBodyFixedDecoration(
+ c2Body.getMobilizedBodyIndex(),
+ Transform(),
+ DecorativeSphere( length_t(0.5*angstroms) )
+ );
+ // sphere for O2 atom
+ decorations.addBodyFixedDecoration(
+ c2Body.getMobilizedBodyIndex(),
+ location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
+ DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(1,0,0))
+ );
+
+ decorations.addRubberBandLine(
+ c2Body.getMobilizedBodyIndex(),
+ Vec3(0),
+ c2Body.getMobilizedBodyIndex(),
+ location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
+ DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
+ decorations.addRubberBandLine(
+ c3Body.getMobilizedBodyIndex(),
+ Vec3(0),
+ c2Body.getMobilizedBodyIndex(),
+ Vec3(0),
+ DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
+
+ PseudorotationMobilizer c1Body(
+ c2Body,
+ Rotation( angle_t(-80*degrees), YAxis ),
+ rigidBody,
+ Transform(location_t(Vec3(0,0,1.5)*angstroms)),
+ angle_t(37.6*degrees), // amplitude
+ angle_t(52.8*degrees) // phase
+ );
+ // sphere for C1 atom
+ decorations.addBodyFixedDecoration(
+ c1Body.getMobilizedBodyIndex(),
+ Transform(),
+ DecorativeSphere( length_t(0.5*angstroms) )
+ );
+ // sphere for N1 atom
+ decorations.addBodyFixedDecoration(
+ c1Body.getMobilizedBodyIndex(),
+ location_t(Vec3(-1.0,-1.0,-0.5)*angstroms),
+ DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(0,0,1))
+ );
+ // sphere for O4 atom
+ decorations.addBodyFixedDecoration(
+ c1Body.getMobilizedBodyIndex(),
+ location_t(Vec3(1.0,0,-0.5)*angstroms),
+ DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(1,0,0))
+ );
+
+ decorations.addRubberBandLine(
+ c2Body.getMobilizedBodyIndex(),
+ Vec3(0),
+ c1Body.getMobilizedBodyIndex(),
+ Vec3(0),
+ DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
+ decorations.addRubberBandLine(
+ c1Body.getMobilizedBodyIndex(),
+ Vec3(0),
+ c1Body.getMobilizedBodyIndex(),
+ location_t(Vec3(1.0,0,-0.5)*angstroms),
+ DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
+ decorations.addRubberBandLine(
+ c1Body.getMobilizedBodyIndex(),
+ Vec3(0),
+ c1Body.getMobilizedBodyIndex(),
+ location_t(Vec3(-1.0,-1.0,-0.5)*angstroms),
+ DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
+ decorations.addRubberBandLine(
+ c4Body.getMobilizedBodyIndex(),
+ Vec3(0),
+ c1Body.getMobilizedBodyIndex(),
+ location_t(Vec3(1.0,0,-0.5)*angstroms),
+ DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
+
+ // Prescribed motion
+ Constraint::ConstantSpeed(c3Body, 0.5);
+
+ // Two constraint way works; one constraint way does not
+ bool useTwoConstraints = true;
+
+ if (useTwoConstraints) {
+ // Constraints to make three generalized coordinates identical
+ std::vector<MobilizedBodyIndex> c32bodies(2);
+ c32bodies[0] = c3Body.getMobilizedBodyIndex();
+ c32bodies[1] = c2Body.getMobilizedBodyIndex();
+ std::vector<MobilizerQIndex> coordinates(2, MobilizerQIndex(0));
+ Constraint::CoordinateCoupler(matter, new DifferenceFunction, c32bodies, coordinates);
+
+ std::vector<MobilizedBodyIndex> c21bodies(2);
+ c21bodies[0] = c2Body.getMobilizedBodyIndex();
+ c21bodies[1] = c1Body.getMobilizedBodyIndex();
+ Constraint::CoordinateCoupler(matter, new DifferenceFunction, c21bodies, coordinates);
+ }
+ else { // trying to get single constraint way to work
+ // Try one constraint for all three mobilizers
+ std::vector<MobilizedBodyIndex> c123Bodies(3);
+ c123Bodies[0] = c1Body.getMobilizedBodyIndex();
+ c123Bodies[1] = c2Body.getMobilizedBodyIndex();
+ c123Bodies[2] = c3Body.getMobilizedBodyIndex();
+ std::vector<MobilizerQIndex> coords3(3, MobilizerQIndex(0));
+ Constraint::CoordinateCoupler(matter, new ThreeDifferencesFunction, c123Bodies, coords3);
+ }
+
+
+ Visualizer viz(system);
+ viz.setBackgroundType(Visualizer::SolidColor);
+
+ system.addEventReporter(new Visualizer::Reporter(viz, 0.10));
+
+ system.realizeTopology();
+ State& state = system.updDefaultState();
+
+ // Simulate it.
+ VerletIntegrator integ(system);
+ //RungeKuttaMersonIntegrator integ(system);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ ts.stepTo(50.0);
+}
+
+int main()
+{
+ try {
+ testRiboseMobilizer();
+ } catch (const std::exception& e) {
+ std::cout << "EXCEPTION: " << e.what() << std::endl;
+ return 1;
+ }
+
+ return 0;
+}
diff --git a/Simbody/tests/adhoc/TestThermostat.cpp b/Simbody/tests/adhoc/TestThermostat.cpp
new file mode 100644
index 0000000..f4869bb
--- /dev/null
+++ b/Simbody/tests/adhoc/TestThermostat.cpp
@@ -0,0 +1,202 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2009-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Test the Force::Thermostat force element.
+ */
+
+#include "SimTKsimbody.h"
+#include "SimTKcommon/Testing.h"
+
+
+#include <cstdio>
+#include <exception>
+#include <iostream>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+class ThermoReporter : public PeriodicEventReporter {
+public:
+ ThermoReporter(const MultibodySystem& sys,
+ const Force::Thermostat& thermo,
+ const Force::LinearBushing& bushing1,
+ const Force::LinearBushing& bushing2,
+ Real dt)
+ : PeriodicEventReporter(dt), system(sys), thermo(thermo),
+ bushing1(bushing1), bushing2(bushing2) {}
+
+ void handleEvent(const State& state) const {
+ printf("THERMO t=%g, stage %s, KE+PE=%g Ebath=%g CONSERVED=%g\n",
+ state.getTime(),
+ state.getSystemStage().getName().c_str(),
+ system.calcEnergy(state),
+ thermo.calcBathEnergy(state),
+ system.calcEnergy(state)
+ + thermo.calcBathEnergy(state)
+ + bushing1.getDissipatedEnergy(state)
+ + bushing2.getDissipatedEnergy(state));
+ printf("TEMP=%g N=%d Power=%g Work=%g\n",
+ thermo.getCurrentTemperature(state),
+ thermo.getNumThermalDofs(state),
+ thermo.getExternalPower(state),
+ thermo.getExternalWork(state));
+ }
+private:
+ const MultibodySystem& system;
+ const Force::Thermostat& thermo;
+ const Force::LinearBushing& bushing1;
+ const Force::LinearBushing& bushing2;
+};
+
+void testConservationOfEnergy() {
+ // Create the system.
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
+
+ const Real Mass = 1;
+ const Vec3 HalfShape = Vec3(1,.5,.25)/2;
+ const Transform BodyAttach(Rotation(), Vec3(HalfShape[0],0,0));
+ Body::Rigid brickBody(MassProperties(Mass, Vec3(.1,.2,.3),
+ Mass*Inertia(1,1.1,1.2,0.01,0.02,0.03)));
+ //Body::Rigid brickBody(MassProperties(Mass, Vec3(0),
+ // Mass*UnitInertia::ellipsoid(HalfShape)));
+ brickBody.addDecoration(Transform(), DecorativeEllipsoid(HalfShape)
+ .setOpacity(0.25)
+ .setColor(Blue));
+ brickBody.addDecoration(BodyAttach,
+ DecorativeFrame(0.5).setColor(Red));
+
+ const int NBod=50;
+ MobilizedBody::Free brick1(matter.Ground(), Transform(),
+ brickBody, BodyAttach);
+ MobilizedBody::Free brick2(brick1, Transform(),
+ brickBody, BodyAttach);
+ MobilizedBody prev=brick2;
+ MobilizedBody body25;
+ for (int i=0; i<NBod; ++i) {
+ MobilizedBody::Ball next(prev, -1*BodyAttach.p(),
+ brickBody, BodyAttach);
+ if (i==25) body25=next;
+ //Force::TwoPointLinearSpring(forces,
+ // prev, Vec3(0), next, Vec3(0), 1000, 1);
+ prev=next;
+ }
+
+ Constraint::Ball(matter.Ground(), Vec3(0,1,0)-2*NBod/3*BodyAttach.p(),
+ prev, BodyAttach.p());
+ Constraint::Ball(matter.Ground(), Vec3(0,1,0)-1*NBod/3*BodyAttach.p(),
+ body25, BodyAttach.p());
+
+
+ Vec6 k1(1,100,1,100,100,100), c1(0);
+ Force::LinearBushing(forces, matter.Ground(), -2*NBod/3*BodyAttach.p(),
+ prev, BodyAttach.p(), k1, c1);
+ matter.Ground().addBodyDecoration(-2*NBod/3*BodyAttach.p(),
+ DecorativeFrame().setColor(Green));
+
+ Force::Thermostat thermo(forces, matter,
+ SimTK_BOLTZMANN_CONSTANT_MD,
+ 5000,
+ .1);
+
+ Vec6 k(1,100,1,100,100,100), c(0);
+ Force::LinearBushing bushing1(forces, matter.Ground(), -1*NBod/3*BodyAttach.p(),
+ brick1, BodyAttach, k, c);
+ Force::LinearBushing bushing2(forces, brick1, Transform(),
+ brick2, BodyAttach, k, c);
+ matter.Ground().addBodyDecoration(-1*NBod/3*BodyAttach.p(),
+ DecorativeFrame().setColor(Green));
+
+
+ Visualizer viz(system);
+ Visualizer::Reporter* reporter = new Visualizer::Reporter(viz, 1./30);
+ viz.setBackgroundType(Visualizer::SolidColor);
+ system.addEventReporter(reporter);
+
+ ThermoReporter* thermoReport = new ThermoReporter
+ (system, thermo, bushing1, bushing2, 1./10);
+ system.addEventReporter(thermoReport);
+
+ // Initialize the system and state.
+
+ system.realizeTopology();
+ State state = system.getDefaultState();
+
+ viz.report(state);
+ printf("Default state -- hit ENTER\n");
+ cout << "t=" << state.getTime()
+ << " q=" << brick1.getQAsVector(state) << brick2.getQAsVector(state)
+ << " u=" << brick1.getUAsVector(state) << brick2.getUAsVector(state)
+ << "\nnChains=" << thermo.getNumChains(state)
+ << " T=" << thermo.getBathTemperature(state)
+ << "\nt_relax=" << thermo.getRelaxationTime(state)
+ << " kB=" << thermo.getBoltzmannsConstant()
+ << endl;
+ getchar();
+
+ state.setTime(0);
+ system.realize(state, Stage::Acceleration);
+ Vector initU(state.getNU());
+ initU = Test::randVector(state.getNU());
+ state.updU()=initU;
+
+
+ RungeKuttaMersonIntegrator integ(system);
+ //integ.setMinimumStepSize(1e-1);
+ integ.setAccuracy(1e-2);
+ TimeStepper ts(system, integ);
+ ts.initialize(state);
+ const State& istate = integ.getState();
+
+ viz.report(integ.getState());
+ viz.zoomCameraToShowAllGeometry();
+ printf("After initialize -- hit ENTER\n");
+ cout << "t=" << integ.getTime()
+ << "\nE=" << system.calcEnergy(istate)
+ << "\nEbath=" << thermo.calcBathEnergy(istate)
+ << endl;
+ thermoReport->handleEvent(istate);
+ getchar();
+
+ // Simulate it.
+ ts.stepTo(20.0);
+
+ viz.report(integ.getState());
+ viz.zoomCameraToShowAllGeometry();
+ printf("After simulation:\n");
+ cout << "t=" << integ.getTime()
+ << "\nE=" << system.calcEnergy(istate)
+ << "\nEbath=" << thermo.calcBathEnergy(istate)
+ << "\nNsteps=" << integ.getNumStepsTaken()
+ << endl;
+ thermoReport->handleEvent(istate);
+}
+
+int main() {
+ SimTK_START_TEST("TestThermostat");
+ SimTK_SUBTEST(testConservationOfEnergy);
+ SimTK_END_TEST();
+}
diff --git a/Simbody/tests/adhoc/TimsBox.cpp b/Simbody/tests/adhoc/TimsBox.cpp
new file mode 100644
index 0000000..530e1b5
--- /dev/null
+++ b/Simbody/tests/adhoc/TimsBox.cpp
@@ -0,0 +1,3340 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) - Tim's Box *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* Attempt to duplicate Tim's OpenSim Box to diagnose nonrepeatability
+and excessive sticking problems. */
+
+//#define NDEBUG 1
+
+#include "Simbody.h"
+
+#include <string>
+#include <iostream>
+#include <exception>
+
+using std::cout;
+using std::endl;
+
+using namespace SimTK;
+
+//#define USE_TIMS_PARAMS
+//#define USE_COMPLIANT_CONTACT
+//#define USE_ELASTIC_FOUNDATION // if compliant
+#define ANIMATE // off to get more accurate CPU time (you can still playback)
+
+// Define this to run the simulation NTries times, saving the states and
+// comparing them bitwise to see if the simulations are perfectly repeatable
+// as they should be. You should see nothing but exact zeroes print out for
+// second and subsequent runs.
+//#define TEST_REPEATABILITY
+static const int NTries=3;
+
+const Real ReportInterval=1./30;
+
+
+//==============================================================================
+// UNI POINT IN PLANE IMPL
+//==============================================================================
+// This is Simbody's PointInPlane Constraint with sliding friction added.
+// TODO: not using sliding forces here yet -- still using separate force
+// element MySlidingFrictionForce (below).
+class UniPointInPlaneImpl : public Constraint::Custom::Implementation {
+public:
+UniPointInPlaneImpl
+ (MobilizedBody& plane, const UnitVec3& defPlaneNormal, Real defPlaneHeight,
+ MobilizedBody& follower, const Vec3& defFollowerPoint)
+: Implementation(plane.updMatterSubsystem(),1,0,0)
+{
+ planeBody = addConstrainedBody(plane);
+ followerBody = addConstrainedBody(follower);
+ defaultPlaneNormal = defPlaneNormal;
+ defaultPlaneHeight = defPlaneHeight;
+ defaultFollowerPoint = defFollowerPoint;
+}
+
+
+void setPlaneDisplayHalfWidth(Real h) {
+ // h <= 0 means don't display plane
+ invalidateTopologyCache();
+ planeHalfWidth = h > 0 ? h : 0;
+}
+Real getPlaneDisplayHalfWidth() const {return planeHalfWidth;}
+
+void setPointDisplayRadius(Real r) {
+ // r <= 0 means don't display point
+ invalidateTopologyCache();
+ pointRadius= r > 0 ? r : 0;
+}
+Real getPointDisplayRadius() const {return pointRadius;}
+
+// Allocate state variables:
+// - s, the expected sign of the normal multiplier (-1,0,1)
+// - e, the "previous slip direction" (a 2d unit vector or NaN if none)
+//
+// The auto update for s is s=sign(|lambda|>=SignificantReal ? lambda : 0) so
+// that there is a dead zone in which we consider lambda to be zero.
+//
+// The auto update for e is v/|v| if slip speed |v| is large enough to be
+// considered "reliable", and dot(e,v)>0. Otherwise stick with the current e.
+// A stiction->sliding transition causes e to be set to the impending slip
+// direction (opposite the last known stiction force direction).
+//
+// Let d=s for bilateral constraint, d=max(s,0) for unilateral. Then
+// sliding force multiplier sigma=d*mu_d*lambda, which will be >= 0 if d was
+// correct. The sliding friction force will be -sigma*e.
+//
+
+
+void realizeTopology(State& state) const OVERRIDE_11 {
+ isSlidingIx = getMatterSubsystem().allocateDiscreteVariable
+ (state, Stage::Acceleration, new Value<bool>(false));
+ prevSignIx = getMatterSubsystem().allocateAutoUpdateDiscreteVariable
+ (state, Stage::Acceleration, new Value<Real>(1),
+ Stage::Acceleration); // cache update depends on lambda
+ prevSlipDirIx = getMatterSubsystem().allocateAutoUpdateDiscreteVariable
+ (state, Stage::Acceleration, new Value<Vec2>(Vec2(NaN)),
+ Stage::Velocity); // cache update depends only on slip velocity
+}
+
+bool isSliding(const State& state) const {
+ const bool& sliding = Value<bool>::downcast
+ (getMatterSubsystem().getDiscreteVariable(state, isSlidingIx));
+ return sliding;
+}
+Real getPrevSign(const State& state) const {
+ const Real& prevSign = Value<Real>::downcast
+ (getMatterSubsystem().getDiscreteVariable(state, prevSignIx));
+ return prevSign;
+}
+Vec2 getPrevSlipDir(const State& state) const {
+ const Vec2& prevSlipDir = Value<Vec2>::downcast
+ (getMatterSubsystem().getDiscreteVariable(state, prevSlipDirIx));
+ return prevSlipDir;
+}
+/*
+// If we're sliding, set the update value for the previous slip direction
+// if the current slip velocity is usable.
+void realizeVelocity(const State& state) const OVERRIDE_11 {
+ if (!hasSlidingForce(state))
+ return; // nothing to do
+ const Vec2 Vslip = m_contact.getSlipVelocity(state);
+ const Vec2 prevVslipDir = getPrevSlipDir(state);
+
+ if (shouldUpdate(Vslip, prevVslipDir, m_vtol)) {
+ Vec2& prevSlipUpdate = Value<Vec2>::updDowncast
+ (m_forces.updDiscreteVarUpdateValue(state, m_prevSlipDirIx));
+ const Real v = Vslip.norm();
+ const Vec2 slipDir = Vslip / v;
+ prevSlipUpdate = slipDir;
+ m_forces.markDiscreteVarUpdateValueRealized(state, m_prevSlipDirIx);
+
+ #ifndef NDEBUG
+ //printf("UPDATE %d: prevSlipDir=%g %g; now=%g %g; |v|=%g dot=%g vdot=%g\n",
+ // m_friction.getFrictionIndex(),
+ // prevVslipDir[0],prevVslipDir[1],slipDir[0],slipDir[1],
+ // v, ~slipDir*prevVslipDir, ~Vslip*prevVslipDir);
+ #endif
+ } else {
+ #ifndef NDEBUG
+ printf("NO UPDATE %d: prevSlipDir=%g %g; Vnow=%g %g; |v|=%g vdot=%g\n",
+ m_friction.getFrictionIndex(),
+ prevVslipDir[0],prevVslipDir[1],Vslip[0],Vslip[1],
+ Vslip.norm(), ~Vslip*prevVslipDir);
+ #endif
+ }
+}
+
+// Regardless of whether we're sticking or sliding, as long as the master
+// contact is active use its normal force scalar as the update for our
+// saved normal force.
+void realizeAcceleration(const State& state) const OVERRIDE_11 {
+ if (!m_friction.isMasterActive(state))
+ return; // nothing to save
+ const Real N = m_contact.getForce(state); // normal force
+ const Real prevN = getPrevN(state);
+ if (N==prevN) return; // no need for an update
+
+ Real& prevNupdate = Value<Real>::updDowncast
+ (m_forces.updDiscreteVarUpdateValue(state, m_prevNix));
+
+ #ifndef NDEBUG
+ printf("UPDATE %d: N changing from %g -> %g (%.3g)\n",
+ m_friction.getFrictionIndex(),
+ prevN, N, std::abs(N-prevN)/std::max(N,prevN));
+ #endif
+ prevNupdate = N;
+ m_forces.markDiscreteVarUpdateValueRealized(state, m_prevNix);
+}
+*/
+
+// Implementation of virtuals required for holonomic constraints.
+UniPointInPlaneImpl* clone() const OVERRIDE_11
+{ return new UniPointInPlaneImpl(*this); }
+
+void calcDecorativeGeometryAndAppend
+ (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
+ OVERRIDE_11 {}
+
+void calcPositionErrors
+ (const State& s, // Stage::Time
+ const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr) // mp of these
+ const OVERRIDE_11;
+
+void calcPositionDotErrors
+ (const State& s, // Stage::Position
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr) // mp of these
+ const OVERRIDE_11;
+
+void calcPositionDotDotErrors
+ (const State& s, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr) // mp of these
+ const OVERRIDE_11;
+
+// apply f=lambda*n to the follower point S of body F,
+// -f to point C (coincident point) of body B
+void addInPositionConstraintForces
+ (const State& s, // Stage::Position
+ const Array_<Real>& multipliers, // mp of these
+ Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA,
+ Array_<Real, ConstrainedQIndex>& qForces)
+ const OVERRIDE_11
+{
+ assert(multipliers.size()==1 && bodyForcesInA.size()==2
+ && qForces.size()==0);
+ const Real lambda = multipliers[0];
+
+ //TODO: should be able to get p info from State
+ const Vec3& p_FS = defaultFollowerPoint; // measured & expressed in F
+ const Vec3 p_AS = findStationLocationFromState(s, followerBody,
+ defaultFollowerPoint);
+ const Transform& X_AB = getBodyTransformFromState(s, planeBody);
+ const Vec3 p_BC = ~X_AB * p_AS; // measured & expressed in B
+ const Vec3 force_A = X_AB.R()*(lambda*defaultPlaneNormal);
+
+ addInStationForce(s, followerBody, p_FS, force_A, bodyForcesInA);
+ addInStationForce(s, planeBody, p_BC, -force_A, bodyForcesInA);
+}
+
+//------------------------------------------------------------------------------
+ private:
+
+ConstrainedBodyIndex planeBody; // B1
+ConstrainedBodyIndex followerBody; // B2
+
+UnitVec3 defaultPlaneNormal; // on body 1, exp. in B1 frame
+Real defaultPlaneHeight;
+Vec3 defaultFollowerPoint; // on body 2, exp. in B2 frame
+
+// These are just for visualization
+Real planeHalfWidth;
+Real pointRadius;
+
+mutable DiscreteVariableIndex isSlidingIx; // should generate sliding force?
+mutable DiscreteVariableIndex prevSignIx; // last sign of lambda
+mutable DiscreteVariableIndex prevSlipDirIx; // previous slip direction e
+};
+
+
+//==============================================================================
+// MY CONTACT ELEMENT
+//==============================================================================
+// This abstract class hides the details about which kind of contact constraint
+// we're dealing with, while giving us enough to work with for deciding what's
+// on and off and generating impulses.
+//
+// There is always a scalar associated with the constraint for making
+// decisions. There may be a friction element associated with this contact.
+class MyFrictionElement;
+class MyContactElement {
+public:
+ enum ImpulseType {Compression,Expansion,Capture};
+
+ MyContactElement(Constraint uni, Real multSign, Real coefRest)
+ : m_uni(uni), m_multSign(multSign), m_coefRest(coefRest),
+ m_index(-1), m_friction(0),
+ m_velocityDependentCOR(NaN), m_restitutionDone(false)
+ { m_uni.setDisabledByDefault(true); }
+
+ virtual ~MyContactElement() {}
+
+ // (Re)initialize base & concrete class. If overridden, be sure to
+ // invoke base class first.
+ virtual void initialize() {
+ setRestitutionDone(false);
+ m_velocityDependentCOR = NaN;
+ m_Ic = m_Ie = m_I = 0;
+ }
+
+ // Provide a human-readable string identifying the type of contact
+ // constraint.
+ virtual String getContactType() const = 0;
+
+ // These must be constructed so that a negative value means the
+ // unilateral constraint condition is violated.
+ virtual Real getPerr(const State& state) const = 0;
+ virtual Real getVerr(const State& state) const = 0;
+ virtual Real getAerr(const State& state) const = 0;
+
+ // This returns a point in the ground frame at which you might want to
+ // say the constraint is "located", for purposes of display. This should
+ // return something useful even if the constraint is currently off.
+ virtual Vec3 whereToDisplay(const State& state) const = 0;
+
+ // This is used by some constraints to collect position information that
+ // may be used later to set instance variables when enabling the underlying
+ // Simbody constraint. All constraints zero impulses here.
+ virtual void initializeForImpact(const State& state, Real captureVelocity) {
+ if (-captureVelocity <= getVerr(state) && getVerr(state) < 0) {
+ m_velocityDependentCOR = 0;
+ SimTK_DEBUG3("CAPTURING %d because %g <= v=%g < 0\n",
+ m_index, -captureVelocity, getVerr(state));
+ } else {
+ m_velocityDependentCOR = m_coefRest;
+ }
+
+ setRestitutionDone(false);
+ m_Ic = m_Ie = m_I = 0; }
+
+ // Returns zero if the constraint is not currently enabled. Otherwise
+ // return the signed constraint force, with a negative value indicating
+ // that the unilateral force condition is violated.
+ Real getForce(const State& s) const {
+ if (isDisabled(s)) return 0;
+ const Vector mult = m_uni.getMultipliersAsVector(s);
+ assert(mult.size() == 1);
+ if (isNaN(mult[0]))
+ printf("*** getForce(): mult is NaN\n");
+ return m_multSign*mult[0];
+ }
+
+ bool isProximal(const State& state, Real posTol) const
+ { return !isDisabled(state) || getPerr(state) <= posTol; }
+ bool isCandidate(const State& state, Real posTol, Real velTol) const
+ { return isProximal(state, posTol) && getVerr(state) <= velTol; }
+
+
+ void enable(State& state) const {m_uni.enable(state);}
+ void disable(State& state) const {m_uni.disable(state);}
+ bool isDisabled(const State& state) const {return m_uni.isDisabled(state);}
+
+ void setMyDesiredDeltaV(const State& s,
+ Vector& desiredDeltaV) const
+ { Vector myDesiredDV(1); myDesiredDV[0] = m_multSign*getVerr(s);
+ m_uni.setMyPartInConstraintSpaceVector(s, myDesiredDV,
+ desiredDeltaV); }
+
+ void recordImpulse(ImpulseType type, const State& state,
+ const Vector& lambda) {
+ Vector myImpulse(1);
+ m_uni.getMyPartFromConstraintSpaceVector(state, lambda, myImpulse);
+ const Real I = myImpulse[0];
+ if (type==Compression) m_Ic = I;
+ else if (type==Expansion) m_Ie = I;
+ m_I += I;
+ }
+
+ // Impulse is accumulated internally.
+ Real getImpulse() const {return -m_multSign*m_I;}
+ Real getCompressionImpulse() const {return -m_multSign*m_Ic;}
+ Real getExpansionImpulse() const {return -m_multSign*m_Ie;}
+
+ Real getMyValueFromConstraintSpaceVector(const State& state,
+ const Vector& lambda) const
+ { Vector myValue(1);
+ m_uni.getMyPartFromConstraintSpaceVector(state, lambda, myValue);
+ return -m_multSign*myValue[0]; }
+
+ void setMyExpansionImpulse(const State& state,
+ Real coefRest,
+ Vector& lambda) const
+ { const Real I = coefRest * m_Ic;
+ Vector myImp(1); myImp[0] = I;
+ m_uni.setMyPartInConstraintSpaceVector(state, myImp, lambda); }
+
+
+ Real getMaxCoefRest() const {return m_coefRest;}
+ Real getEffectiveCoefRest() const {return m_velocityDependentCOR;}
+ void setRestitutionDone(bool isDone) {m_restitutionDone=isDone;}
+ bool isRestitutionDone() const {return m_restitutionDone;}
+
+ // Record position within the set of unilateral contact constraints.
+ void setContactIndex(int index) {m_index=index;}
+ int getContactIndex() const {return m_index;}
+ // If there is a friction element for which this is the master contact,
+ // record it here.
+ void setFrictionElement(MyFrictionElement& friction)
+ { m_friction = &friction; }
+ // Return true if there is a friction element associated with this contact
+ // element.
+ bool hasFrictionElement() const {return m_friction != 0;}
+ // Get the associated friction element.
+ const MyFrictionElement& getFrictionElement() const
+ { assert(hasFrictionElement()); return *m_friction; }
+ MyFrictionElement& updFrictionElement() const
+ { assert(hasFrictionElement()); return *m_friction; }
+
+protected:
+ Constraint m_uni;
+ const Real m_multSign; // 1 or -1
+ const Real m_coefRest;
+
+ int m_index; // contact index in unilateral constraint set
+ MyFrictionElement* m_friction; // if any (just a reference, not owned)
+
+ // Runtime -- initialized at start of impact handler.
+ Real m_velocityDependentCOR; // Calculated at start of impact
+ bool m_restitutionDone;
+ Real m_Ic, m_Ie, m_I; // impulses
+};
+
+
+
+//==============================================================================
+// MY FRICTION ELEMENT
+//==============================================================================
+// A Coulomb friction element consists of both a sliding force and a stiction
+// constraint, at most one of which is active. There is a boolean state variable
+// associated with each element that says whether it is in sliding or stiction,
+// and that state can only be changed during event handling.
+//
+// Generated forces depend on a scalar normal force N that comes from a
+// separate "normal force master", which might be one of the following:
+// - a unilateral constraint
+// - a bilateral constraint
+// - a mobilizer
+// - a compliant force element
+// If the master is an inactive unilateral constraint, or if N=0, then no
+// friction forces are generated. In this example, we're only going to use
+// a unilateral contact constraint as the "normal force master".
+//
+// For all but the compliant normal force master, the normal force N is
+// acceleration-dependent and thus may be coupled to the force produced by a
+// sliding friction element. This may require iteration to ensure consistency
+// between the sliding friction force and its master contact's normal force.
+//
+// A Coulomb friction element depends on a scalar slip speed defined by the
+// normal force master (this might be the magnitude of a generalized speed or
+// slip velocity vector). When the slip velocity goes to zero, the stiction
+// constraint is enabled if its constraint force magnitude can be kept to
+// mu_s*|N| or less. Otherwise, or if the slip velocity is nonzero, the sliding
+// force is enabled instead and generates a force of constant magnitude mu_d*|N|
+// that opposes the slip direction, or impending slip direction, as defined by
+// the master.
+//
+// There are two witness functions generated: (1) in slip mode, observes slip
+// velocity reversal and triggers stiction, and (2) in stiction mode, observes
+// stiction force increase past mu_s*|N| and triggers switch to sliding.
+class MyFrictionElement {
+public:
+ MyFrictionElement(Real mu_d, Real mu_s, Real mu_v)
+ : mu_d(mu_d), mu_s(mu_s), mu_v(mu_v), m_index(-1) {}
+
+ virtual ~MyFrictionElement() {}
+
+ // (Re)initialize base & concrete class. If overridden, be sure to
+ // invoke base class first.
+ virtual void initialize() {
+ }
+
+ Real getDynamicFrictionCoef() const {return mu_d;}
+ Real getStaticFrictionCoef() const {return mu_s;}
+ Real getViscousFrictionCoef() const {return mu_v;}
+
+ // Return true if the stiction constraint is enabled.
+ virtual bool isSticking(const State&) const = 0;
+
+ virtual void enableStiction(State&) const = 0;
+ virtual void disableStiction(State&) const = 0;
+
+ // When sticking, record -f/|f| as the previous slip direction, and
+ // max(N,0) as the previous normal force. Stiction
+ // must be currently active and constraint multipliers available.
+ virtual void recordImpendingSlipInfo(const State&) = 0;
+ // When sliding, record current slip velocity as the previous slip
+ // direction.
+ virtual void recordSlipDir(const State&) = 0;
+
+ // In an event handler or at initialization only, set the last recorded slip
+ // direction as the previous direction. This invalidates Velocity stage.
+ virtual void updatePreviousSlipDirFromRecorded(State& state) const = 0;
+
+ // This is the dot product of the current sliding velocity and the
+ // saved previous slip direction. This changes sign when a sliding friction
+ // force of mu_d*|N| would cause a reversal, meaning a switch to stiction is
+ // in order. State must be realized to Velocity stage.
+ virtual Real calcSlipSpeedWitness(const State&) const = 0;
+
+ // When in stiction, this calculates mu_s*|N| - |f|, which is negative if
+ // the stiction force exceeds its limit. (Not suitable for impacts where
+ // the dynamic coefficient should be used.) State must be realized to
+ // Acceleration stage.
+ virtual Real calcStictionForceWitness(const State&) const = 0;
+
+ // This is the magnitude of the current slip velocity. State must be
+ // realized to Velocity stage.
+ virtual Real getActualSlipSpeed(const State&) const = 0;
+
+ // This is the magnitude of the current friction force, whether sliding
+ // or sticking. State must be realized to Acceleration stage.
+ virtual Real getActualFrictionForce(const State&) const = 0;
+
+ // Return the scalar normal force N being generated by the contact master
+ // of this friction element. This may be negative if the master is a
+ // unilateral constraint whose "no-stick" condition is violated.
+ virtual Real getMasterNormalForce(const State&) const = 0;
+
+ // If the sliding force element uses an estimated normal forces to generate
+ // an approximate sliding force, return the estimate it is using here.
+ virtual Real getEstimatedNormalForce(const State&) const = 0;
+
+ // Modify the estimated normal force in the state.
+ virtual void setEstimatedNormalForce(State&, Real N) const = 0;
+
+ // Return true if the normal force master *could* be involved in an
+ // impact event (because it is touching).
+ virtual bool isMasterProximal(const State&, Real posTol) const = 0;
+ // Return true if the normal force master *could* be involved in contact
+ // force generation (because it is touching and not separating).
+ virtual bool isMasterCandidate(const State&, Real posTol, Real velTol)
+ const = 0;
+ // Return true if the normal force master is currently generating a
+ // normal force (or impulse) so that this friction element might be
+ // generating a force also.
+ virtual bool isMasterActive(const State&) const = 0;
+
+
+ // This is used by some stiction constraints to collect position information
+ // that may be used later to set instance variables when enabling the
+ // underlying Simbody constraint. Recorded impulses should be zeroed.
+ virtual void initializeForStiction(const State& state) = 0;
+
+ // If this friction element's stiction constraint is enabled, set its
+ // constraint-space velocity entry(s) in desiredDeltaV to the current
+ // slip velocity (which might be a scalar or 2-vector).
+ virtual void setMyDesiredDeltaV(const State& s,
+ Vector& desiredDeltaV) const = 0;
+
+ // We just applied constraint-space impulse lambda to all active
+ // constraints. If this friction element's stiction constraint is enabled,
+ // save its part of the impulse internally for reporting.
+ virtual void recordImpulse(MyContactElement::ImpulseType type,
+ const State& state,
+ const Vector& lambda) = 0;
+
+ // Output the status, friction force, slip velocity, prev slip direction
+ // (scalar or vector) to the given ostream, indented as indicated and
+ // followed by a newline. May generate multiple lines.
+ virtual std::ostream& writeFrictionInfo(const State& state,
+ const String& indent,
+ std::ostream& o) const = 0;
+
+ // Optional: give some kind of visual representation for the friction force.
+ virtual void showFrictionForce(const State& state,
+ Array_<DecorativeGeometry>& geometry) const {}
+
+
+ void setFrictionIndex(int index) {m_index=index;}
+ int getFrictionIndex() const {return m_index;}
+
+private:
+ Real mu_d, mu_s, mu_v;
+ int m_index; // friction index within unilateral constraint set
+};
+
+
+
+//==============================================================================
+// MY UNILATERAL CONSTRAINT SET
+//==============================================================================
+
+// These are indices into the unilateral constraint set arrays.
+struct MyElementSubset {
+ void clear() {m_contact.clear();m_friction.clear();m_sliding.clear();}
+ Array_<int> m_contact;
+ Array_<int> m_friction; // friction elements that might stick
+ Array_<int> m_sliding; // friction elements that can only slide
+};
+
+class MyUnilateralConstraintSet {
+public:
+ // Capture velocity is used two ways: (1) if the normal approach velocity
+ // is smaller, the coefficient of restitution is set to zero for the
+ // upcoming impact, and (2) if a slip velocity is smaller than this the
+ // contact is a candidate for stiction.
+ MyUnilateralConstraintSet(const MultibodySystem& mbs, Real captureVelocity)
+ : m_mbs(mbs), m_captureVelocity(captureVelocity) {}
+
+ // This class takes over ownership of the heap-allocated contact element.
+ int addContactElement(MyContactElement* contact) {
+ const int index = (int)m_contact.size();
+ m_contact.push_back(contact);
+ contact->setContactIndex(index);
+ return index;
+ }
+ // This class takes over ownership of the heap-allocated friction element.
+ int addFrictionElement(MyFrictionElement* friction) {
+ const int index = (int)m_friction.size();
+ m_friction.push_back(friction);
+ friction->setFrictionIndex(index);
+ return index;
+ }
+
+ Real getCaptureVelocity() const {return m_captureVelocity;}
+ void setCaptureVelocity(Real v) {m_captureVelocity=v;}
+
+ int getNumContactElements() const {return (int)m_contact.size();}
+ int getNumFrictionElements() const {return (int)m_friction.size();}
+ const MyContactElement& getContactElement(int ix) const
+ { return *m_contact[ix]; }
+ const MyFrictionElement& getFrictionElement(int ix) const
+ { return *m_friction[ix]; }
+
+ // Allow writable access to elements from const set so we can record
+ // runtime results (e.g. impulses).
+ MyContactElement& updContactElement(int ix) const {return *m_contact[ix];}
+ MyFrictionElement& updFrictionElement(int ix) const {return *m_friction[ix];}
+
+ // Initialize all runtime fields in the contact & friction elements.
+ void initialize()
+ {
+ for (unsigned i=0; i < m_contact.size(); ++i)
+ m_contact[i]->initialize();
+ for (unsigned i=0; i < m_friction.size(); ++i)
+ m_friction[i]->initialize();
+ }
+
+ // Return the contact and friction elements that might be involved in an
+ // impact occurring in this configuration. They are the contact elements
+ // for which perr <= posTol, and friction elements whose normal force
+ // masters can be involved in the impact. State must be realized through
+ // Position stage.
+ void findProximalElements(const State& state,
+ Real posTol,
+ MyElementSubset& proximals) const
+ {
+ proximals.clear();
+ for (unsigned i=0; i < m_contact.size(); ++i)
+ if (m_contact[i]->isProximal(state,posTol))
+ proximals.m_contact.push_back(i);
+ for (unsigned i=0; i < m_friction.size(); ++i)
+ if (m_friction[i]->isMasterProximal(state,posTol))
+ proximals.m_friction.push_back(i);
+ // Any friction elements might stick if they are proximal since
+ // we'll be changing velocities, so no m_sliding entries in proximals.
+ }
+
+ // Return the contact and friction elements that might be involved in
+ // generating contact forces at the current state. Candidate contact
+ // elements are those that are (a) already enabled, or (b) for which
+ // perr <= posTol and verr <= velTol. Candidate friction elements are those
+ // whose normal force master is unconditional or a candidate and (a) which
+ // are already sticking, or (b) for which vslip <= velTol, or (c) for which
+ // vslip opposes the previous slip direction, meaning it has reversed and
+ // must have passed through zero during the last step. These are the elements
+ // that can be activated without making any changes to the configuration or
+ // velocity state variables, except slightly for constraint projection.
+ //
+ // We also record the friction elements that, if their masters are active,
+ // can only slide because they have a significant slip velocity. State must
+ // be realized through Velocity stage.
+ void findCandidateElements(const State& s,
+ Real posTol,
+ Real velTol,
+ MyElementSubset& candidates) const
+ {
+ candidates.clear();
+ for (unsigned i=0; i < m_contact.size(); ++i)
+ if (m_contact[i]->isCandidate(s,posTol,velTol))
+ candidates.m_contact.push_back(i);
+ for (unsigned i=0; i < m_friction.size(); ++i) {
+ MyFrictionElement& fric = updFrictionElement(i);
+ if (!fric.isMasterCandidate(s,posTol,velTol))
+ continue;
+ if (fric.isSticking(s)
+ || fric.getActualSlipSpeed(s) <= velTol
+ || fric.calcSlipSpeedWitness(s) <= 0)
+ {
+ fric.initializeForStiction(s);
+ candidates.m_friction.push_back(i); // could stick or slide
+ } else {
+ fric.recordSlipDir(s);
+ candidates.m_sliding.push_back(i); // could only slide
+ }
+ }
+ }
+
+ // Look through the given constraint subset and enable any constraints
+ // that are currently disabled. Returns true if any change was made.
+ // If includeStiction==false, we'll only enable contact constraints.
+ bool enableConstraintSubset(const MyElementSubset& subset,
+ bool includeStiction,
+ State& state) const
+ {
+ bool changedSomething = false;
+
+ // Enable contact constraints.
+ for (unsigned i=0; i < subset.m_contact.size(); ++i) {
+ const int which = subset.m_contact[i];
+ const MyContactElement& cont = getContactElement(which);
+ if (cont.isDisabled(state)) {
+ cont.enable(state);
+ changedSomething = true;
+ }
+ }
+
+ if (includeStiction) {
+ // Enable all stiction constraints.
+ for (unsigned i=0; i < subset.m_friction.size(); ++i) {
+ const int which = subset.m_friction[i];
+ const MyFrictionElement& fric = getFrictionElement(which);
+ if (!fric.isSticking(state)) {
+ assert(fric.isMasterActive(state));
+ fric.enableStiction(state);
+ changedSomething = true;
+ }
+ }
+ }
+
+ m_mbs.realize(state, Stage::Instance);
+ return changedSomething;
+ }
+
+ // All event handlers call this method before returning. Given a state for
+ // which no (further) impulse is required, here we decide which contact and
+ // stiction constraints are active, and ensure that they satisfy the
+ // required constraint tolerances to the given accuracy. For sliding
+ // contacts, we will have recorded the slip or impending slip direction and
+ // converged the normal forces.
+ // TODO: in future this may return indicating that an impulse is required
+ // after all, as in Painleve's paradox.
+ void selectActiveConstraints(State& state, Real accuracy) const;
+
+ // This is the inner loop of selectActiveConstraints(). Given a set of
+ // candidates to consider, it finds an active subset and enables those
+ // constraints.
+ void findActiveCandidates(State& state,
+ const MyElementSubset& candidates) const;
+
+ // If there are sliding contacts, adjust normal force estimates and then
+ // recalculate actual normal forces until they agree to a tolerance.
+ void convergeNormalForcesForSlidingContacts(State& state, Real rtol) const;
+
+ // In Debug mode, produce a useful summary of the current state of the
+ // contact and friction elements.
+ void showConstraintStatus(const State& state, const String& place) const;
+
+ ~MyUnilateralConstraintSet() {
+ for (unsigned i=0; i < m_contact.size(); ++i)
+ delete m_contact[i];
+ for (unsigned i=0; i < m_friction.size(); ++i)
+ delete m_friction[i];
+ }
+
+ const MultibodySystem& getMultibodySystem() const {return m_mbs;}
+private:
+ const MultibodySystem& m_mbs;
+ Real m_captureVelocity;
+ Array_<MyContactElement*> m_contact;
+ Array_<MyFrictionElement*> m_friction;
+};
+
+
+
+//==============================================================================
+// STATE SAVER
+//==============================================================================
+// This reporter is called now and again to save the current state so we can
+// play back a movie at the end.
+class StateSaver : public PeriodicEventReporter {
+public:
+ StateSaver(const MultibodySystem& mbs,
+ const MyUnilateralConstraintSet& unis,
+ const Integrator& integ,
+ Real reportInterval)
+ : PeriodicEventReporter(reportInterval),
+ m_mbs(mbs), m_unis(unis), m_integ(integ)
+ { m_states.reserve(2000); }
+
+ ~StateSaver() {}
+
+ void clear() {m_states.clear();}
+ int getNumSavedStates() const {return (int)m_states.size();}
+ const State& getState(int n) const {return m_states[n];}
+
+ void handleEvent(const State& s) const {
+ const SimbodyMatterSubsystem& matter=m_mbs.getMatterSubsystem();
+ const SpatialVec PG = matter.calcSystemMomentumAboutGroundOrigin(s);
+ m_mbs.realize(s, Stage::Acceleration);
+
+#ifndef NDEBUG
+ printf("%3d: %5g mom=%g,%g E=%g", m_integ.getNumStepsTaken(),
+ s.getTime(),
+ PG[0].norm(), PG[1].norm(), m_mbs.calcEnergy(s));
+ cout << " Triggers=" << s.getEventTriggers() << endl;
+ m_unis.showConstraintStatus(s, "STATE SAVER");
+#endif
+
+ m_states.push_back(s);
+ }
+private:
+ const MultibodySystem& m_mbs;
+ const MyUnilateralConstraintSet& m_unis;
+ const Integrator& m_integ;
+ mutable Array_<State> m_states;
+};
+
+// A periodic event reporter that does nothing; useful for exploring the
+// effects of interrupting the simulation.
+class Nada : public PeriodicEventReporter {
+public:
+ explicit Nada(Real reportInterval)
+ : PeriodicEventReporter(reportInterval) {}
+
+ void handleEvent(const State& s) const {
+#ifndef NDEBUG
+ printf("%7g NADA\n", s.getTime());
+#endif
+ }
+};
+
+
+//==============================================================================
+// CONTACT ON HANDLER
+//==============================================================================
+// Allocate three of these for each unilateral contact constraint, using
+// a position, velocity, or acceleration witness function. When the associated
+// contact constraint is inactive, the event triggers are:
+// 1. separation distance goes from positive to negative
+// 2. separation rate goes from positive to negative while distance is zero
+// 3. separation acceleration goes from positive to negative while both
+// distance and rate are zero
+// The first two cases may require an impulse, since the velocities may have to
+// change discontinuously to satisfy the constraints. Case 3 requires only
+// recalculation of the active contacts. In any case the particular contact
+// element that triggered the handler is irrelevant; all "proximal" contacts
+// are solved simultaneously.
+class ContactOn: public TriggeredEventHandler {
+public:
+ ContactOn(const MultibodySystem& system,
+ const MyUnilateralConstraintSet& unis,
+ unsigned which,
+ Stage stage)
+ : TriggeredEventHandler(stage),
+ m_mbs(system), m_unis(unis), m_which(which),
+ m_stage(stage)
+ {
+ // Trigger only as height goes from positive to negative.
+ getTriggerInfo().setTriggerOnRisingSignTransition(false);
+ }
+
+ // This is the witness function.
+ Real getValue(const State& state) const {
+ const SimbodyMatterSubsystem& matter = m_mbs.getMatterSubsystem();
+ const MyContactElement& uni = m_unis.getContactElement(m_which);
+ if (!uni.isDisabled(state))
+ return 0; // already locked
+
+ const Real height = uni.getPerr(state);
+ //printf("getValue %d(%.17g) perr=%g\n", m_which, state.getTime(), height);
+
+ if (m_stage == Stage::Position)
+ return height;
+
+ // Velocity and acceleration triggers are not needed if we're
+ // above ground.
+ if (height > 0) return 0;
+
+ const Real dheight = uni.getVerr(state);
+ //printf("... verr=%g\n", dheight);
+
+ if (m_stage == Stage::Velocity)
+ return dheight;
+
+ // Acceleration trigger is not needed if velocity is positive.
+ if (dheight > 0) return 0;
+
+ const Real ddheight = uni.getAerr(state);
+ //printf("... aerr=%g\n", ddheight);
+
+ return ddheight;
+ }
+
+ // We're using Poisson's definition of the coefficient of
+ // restitution, relating impulses, rather than Newton's,
+ // relating velocities, since Newton's can produce non-physical
+ // results for a multibody system. For Poisson, calculate the impulse
+ // that would bring the velocity to zero, multiply by the coefficient
+ // of restitution to calculate the rest of the impulse, then apply
+ // both impulses to produce changes in velocity. In most cases this
+ // will produce the same rebound velocity as Newton, but not always.
+ void handleEvent(State& s, Real accuracy, bool& shouldTerminate) const;
+
+ // Given the set of proximal constraints, prevent penetration by applying
+ // a nonnegative least squares impulse generating a step change in
+ // velocity. On return, the applied impulse and new velocities are recorded
+ // in the proximal elements, and state is updated to the new velocities and
+ // realized through Velocity stage. Constraints that ended up in contact
+ // are enabled, those that rebounded are disabled.
+ void processCompressionPhase(MyElementSubset& proximal,
+ State& state) const;
+
+ // Given a solution to the compression phase, including the compression
+ // impulse, the set of impacters (enabled) and rebounders (disabled and
+ // with positive rebound velocity), apply an expansion impulse based on
+ // the effective coefficients of restitution of the impacters. Wherever
+ // restitution is applied, the effective coefficient is reset to zero so
+ // that further restitution will not be done for that contact. Returns
+ // true if any expansion was done; otherwise nothing has changed.
+ // Expansion may result in some negative velocities, in which case it has
+ // induced further compression so another compression phase is required.
+ bool processExpansionPhase(MyElementSubset& proximal,
+ State& state) const;
+
+ // Given only the subset of proximal constraints that are active, calculate
+ // the impulse that would eliminate all their velocity errors. No change is
+ // made to the set of active constraints. Some of the resulting impulses
+ // may be negative.
+ void calcStoppingImpulse(const MyElementSubset& proximal,
+ const State& state,
+ Vector& lambda0) const;
+
+ // Given the initial generalized speeds u0, and a constraint-space impulse
+ // lambda, calculate the resulting step velocity change du, modify the
+ // generalized speeds in state to u0+du, and realize Velocity stage.
+ void updateVelocities(const Vector& u0,
+ const Vector& lambda,
+ State& state) const;
+
+
+private:
+ const MultibodySystem& m_mbs;
+ const MyUnilateralConstraintSet& m_unis;
+ const unsigned m_which;
+ const Stage m_stage;
+};
+
+
+
+//==============================================================================
+// CONTACT OFF HANDLER
+//==============================================================================
+// Allocate one of these for each unilateral contact constraint. This handler
+// is invoked when an active contact constraint's contact force crosses zero
+// from positive to negative, meaning it has gone from pushing to sticking.
+// This simply invokes recalculation of the active contacts; the particular
+// source of the event trigger doesn't matter.
+class ContactOff: public TriggeredEventHandler {
+public:
+ ContactOff(const MultibodySystem& system,
+ const MyUnilateralConstraintSet& unis,
+ unsigned which)
+ : TriggeredEventHandler(Stage::Acceleration),
+ m_mbs(system), m_unis(unis), m_which(which)
+ {
+ getTriggerInfo().setTriggerOnRisingSignTransition(false);
+ }
+
+ // This is the witness function.
+ Real getValue(const State& state) const {
+ const MyContactElement& uni = m_unis.getContactElement(m_which);
+ if (uni.isDisabled(state)) return 0;
+ const Real f = uni.getForce(state);
+ return f;
+ }
+
+ void handleEvent
+ (State& s, Real accuracy, bool& shouldTerminate) const
+ {
+ SimTK_DEBUG2("\nhandle %d liftoff@%.17g\n", m_which, s.getTime());
+ SimTK_DEBUG("\n----------------------------------------------------\n");
+ SimTK_DEBUG2("LIFTOFF triggered by constraint %d @t=%.15g\n",
+ m_which, s.getTime());
+ m_mbs.realize(s, Stage::Acceleration);
+
+ #ifndef NDEBUG
+ cout << " triggers=" << s.getEventTriggers() << "\n";
+ #endif
+
+ m_unis.selectActiveConstraints(s, accuracy);
+
+ SimTK_DEBUG("LIFTOFF DONE.\n");
+ SimTK_DEBUG("----------------------------------------------------\n");
+ }
+
+private:
+ const MultibodySystem& m_mbs;
+ const MyUnilateralConstraintSet& m_unis;
+ const unsigned m_which; // one of the contact elements
+};
+
+
+
+//==============================================================================
+// MY POINT CONTACT
+//==============================================================================
+// Define a unilateral constraint to represent contact of a point on a moving
+// body with the ground plane. The ground normal is assumed to be +y.
+class MyPointContact : public MyContactElement {
+ typedef MyContactElement Super;
+public:
+ MyPointContact(MobilizedBody& body, const Vec3& point,
+ Real coefRest)
+ : MyContactElement(
+ //Constraint::PointInPlane(updGround(body), UnitVec3(YAxis), Zero,
+ // body, point),
+ Constraint::Custom(new UniPointInPlaneImpl(
+ updGround(body), UnitVec3(YAxis), Zero,
+ body, point)),
+ Real(-1), // multiplier sign
+ coefRest),
+ m_body(body), m_point(point)
+ {
+ }
+
+ Real getPerr(const State& s) const OVERRIDE_11 {
+ const Vec3 p = m_body.findStationLocationInGround(s, m_point);
+ return p[YAxis];
+ }
+ Real getVerr(const State& s) const OVERRIDE_11 {
+ const Vec3 v = m_body.findStationVelocityInGround(s, m_point);
+ return v[YAxis];
+ }
+ Real getAerr(const State& s) const OVERRIDE_11 {
+ const Vec3 a = m_body.findStationAccelerationInGround(s, m_point);
+ return a[YAxis];
+ }
+
+ String getContactType() const OVERRIDE_11 {return "Point";}
+ Vec3 whereToDisplay(const State& state) const OVERRIDE_11 {
+ return m_body.findStationLocationInGround(state,m_point);
+ }
+
+ // Will be zero if the stiction constraints are on.
+ Vec2 getSlipVelocity(const State& s) const {
+ const Vec3 v = m_body.findStationVelocityInGround(s, m_point);
+ return Vec2(v[XAxis],v[ZAxis]);
+ }
+ // Will be zero if the stiction constraints are on.
+ Vec2 getSlipAcceleration(const State& s) const {
+ const Vec3 a = m_body.findStationAccelerationInGround(s, m_point);
+ return Vec2(a[XAxis],a[ZAxis]);
+ }
+
+ Vec3 getContactPointInPlaneBody(const State& s) const
+ { return m_body.findStationLocationInGround(s, m_point); }
+
+ const MobilizedBody& getBody() const {return m_body;}
+ MobilizedBody& updBody() {return m_body;}
+ const Vec3& getBodyStation() const {return m_point;}
+
+ const MobilizedBody& getPlaneBody() const {
+ const SimbodyMatterSubsystem& matter = m_body.getMatterSubsystem();
+ return matter.getGround();
+ }
+ MobilizedBody& updPlaneBody() const {return updGround(m_body);}
+
+private:
+ // For use during construction before m_body is set.
+ MobilizedBody& updGround(MobilizedBody& body) const {
+ SimbodyMatterSubsystem& matter = body.updMatterSubsystem();
+ return matter.updGround();
+ }
+
+ MobilizedBody& m_body;
+ const Vec3 m_point;
+};
+
+
+
+
+//==============================================================================
+// MY SLIDING FRICTION FORCE -- Declaration
+//==============================================================================
+
+// A nice handle for the sliding friction force. The real code is in the Impl
+// class defined at the bottom of this file.
+class MySlidingFrictionForce : public Force::Custom {
+public:
+ // Add a sliding friction force element to the given force subsystem,
+ // and associate it with a particular contact point.
+ MySlidingFrictionForce(GeneralForceSubsystem& forces,
+ const class MyPointContactFriction& ptFriction,
+ Real vtol);
+
+ void setPrevN(State& state, Real N) const;
+ // This should be a unit vector.
+ void setPrevSlipDir(State& state, const Vec2& slipDir) const;
+
+ Real getPrevN(const State& state) const;
+ Vec2 getPrevSlipDir(const State& state) const;
+
+ bool hasPrevSlipDir(const State& state) const;
+
+ Real calcSlidingForceMagnitude(const State& state) const;
+ Vec2 calcSlidingForce(const State& state) const;
+
+private:
+ const class MySlidingFrictionForceImpl& getImpl() const;
+};
+
+
+//==============================================================================
+// MY POINT CONTACT FRICTION
+//==============================================================================
+// This friction element expects its master to be a unilateral point contact
+// constraint. It provides slipping forces or stiction constraint forces acting
+// in the plane, based on the normal force being applied by the point contact
+// constraint.
+class MyPointContactFriction : public MyFrictionElement {
+ typedef MyFrictionElement Super;
+public:
+ // The constructor allocates two NoSlip1D constraints and a sliding
+ // friction force element.
+ MyPointContactFriction(MyPointContact& contact,
+ Real mu_d, Real mu_s, Real mu_v, Real vtol, //TODO: shouldn't go here
+ GeneralForceSubsystem& forces)
+ : MyFrictionElement(mu_d,mu_s,mu_v), m_contact(contact),
+ m_noslipX(contact.updPlaneBody(), Vec3(0), UnitVec3(XAxis),
+ contact.updPlaneBody(), contact.updBody()),
+ m_noslipZ(contact.updPlaneBody(), Vec3(0), UnitVec3(ZAxis),
+ contact.updPlaneBody(), contact.updBody())
+ {
+ assert((0 <= mu_d && mu_d <= mu_s) && (0 <= mu_v));
+ contact.setFrictionElement(*this);
+ m_noslipX.setDisabledByDefault(true);
+ m_noslipZ.setDisabledByDefault(true);
+ m_sliding = new MySlidingFrictionForce(forces, *this, vtol);
+ initializeRuntimeFields();
+ }
+
+ ~MyPointContactFriction() {delete m_sliding;}
+
+ void initialize() OVERRIDE_11 {
+ Super::initialize();
+ initializeRuntimeFields();
+ }
+
+ // The way we constructed the NoSlip1D constraints makes the multipliers be
+ // the force on Ground; we negate here so we'll get the force on the sliding
+ // body instead.
+ Vec2 getStictionForce(const State& s) const {
+ assert(isSticking(s));
+ return Vec2(-m_noslipX.getMultiplier(s), -m_noslipZ.getMultiplier(s));
+ }
+
+ // Implement pure virtuals from MyFrictionElement base class.
+
+ bool isSticking(const State& s) const OVERRIDE_11
+ { return !m_noslipX.isDisabled(s); } // X,Z always on or off together
+
+ // Note that initializeForStiction() must have been called first.
+ void enableStiction(State& s) const OVERRIDE_11
+ { m_noslipX.setContactPoint(s, m_contactPointInPlane);
+ m_noslipZ.setContactPoint(s, m_contactPointInPlane);
+ m_noslipX.enable(s); m_noslipZ.enable(s); }
+
+ void disableStiction(State& s) const OVERRIDE_11
+ { m_sliding->setPrevN(s, std::max(m_prevN, Real(0)));
+ m_sliding->setPrevSlipDir(s, m_prevSlipDir);
+ m_noslipX.disable(s); m_noslipZ.disable(s); }
+
+ // When sticking with stiction force f, record -f/|f| as the previous slip
+ // direction. If the force is zero we'll leave the direction unchanged.
+ // Also record the master's normal force as the previous normal force
+ // unless it is negative; in that case record zero.
+ // State must be realized through Acceleration stage.
+ void recordImpendingSlipInfo(const State& s) OVERRIDE_11 {
+ const Vec2 f = getStictionForce(s);
+ SimTK_DEBUG3("%d: RECORD IMPENDING, f=%g %g\n",
+ getFrictionIndex(), f[0], f[1]);
+ const Real fmag = f.norm();
+ if (fmag > 0) // TODO: could this ever be zero?
+ m_prevSlipDir = -f/fmag;
+ const Real N = getMasterNormalForce(s); // might be negative
+ m_prevN = N;
+ }
+ // When sliding, record current slip velocity (normalized) as the previous
+ // slip direction. If there is no slip velocity we leave the slip direction
+ // unchanged. State must be realized through Velocity stage.
+ void recordSlipDir(const State& s) OVERRIDE_11 {
+ assert(!isSticking(s));
+ Vec2 v = m_contact.getSlipVelocity(s);
+ Real vmag = v.norm();
+ if (vmag > 0)
+ m_prevSlipDir = v/vmag;
+ }
+
+ // Transfer the locally-stored previous slip direction to the state variable.
+ void updatePreviousSlipDirFromRecorded(State& s) const OVERRIDE_11 {
+ m_sliding->setPrevSlipDir(s, m_prevSlipDir);
+ }
+
+ Real calcSlipSpeedWitness(const State& s) const OVERRIDE_11 {
+ if (isSticking(s)) return 0;
+ const Vec2 vNow = m_contact.getSlipVelocity(s);
+ if (!m_sliding->hasPrevSlipDir(s)) return vNow.norm();
+ const Vec2 vPrev = m_sliding->getPrevSlipDir(s);
+ return dot(vNow, vPrev);
+ }
+
+ Real calcStictionForceWitness(const State& s) const OVERRIDE_11 {
+ if (!isSticking(s)) return 0;
+ const Real mu_s = getStaticFrictionCoef();
+ const Real N = getMasterNormalForce(s); // might be negative
+ const Vec2 f = getStictionForce(s);
+ const Real fmag = f.norm();
+ return mu_s*N - fmag;
+ }
+
+ Real getActualSlipSpeed(const State& s) const OVERRIDE_11 {
+ const Vec2 vNow = m_contact.getSlipVelocity(s);
+ return vNow.norm();
+ }
+
+ Real getActualFrictionForce(const State& s) const OVERRIDE_11 {
+ const Real f = isSticking(s) ? getStictionForce(s).norm()
+ : m_sliding->calcSlidingForceMagnitude(s);
+ return f;
+ }
+
+ Real getMasterNormalForce(const State& s) const OVERRIDE_11 {
+ const Real N = m_contact.getForce(s); // might be negative
+ return N;
+ }
+
+ Real getEstimatedNormalForce(const State& s) const OVERRIDE_11 {
+ return m_sliding->getPrevN(s);
+ }
+
+ void setEstimatedNormalForce(State& s, Real N) const OVERRIDE_11 {
+ m_sliding->setPrevN(s, N);
+ }
+
+
+ bool isMasterProximal(const State& s, Real posTol) const OVERRIDE_11
+ { return m_contact.isProximal(s, posTol); }
+ bool isMasterCandidate(const State& s, Real posTol, Real velTol) const
+ OVERRIDE_11
+ { return m_contact.isCandidate(s, posTol, velTol); }
+ bool isMasterActive(const State& s) const OVERRIDE_11
+ { return !m_contact.isDisabled(s); }
+
+
+ // Set the friction application point to be the projection of the contact
+ // point onto the contact plane. This will be used the next time stiction
+ // is enabled. Requires state realized to Position stage.
+ void initializeForStiction(const State& s) OVERRIDE_11 {
+ const Vec3 p = m_contact.getContactPointInPlaneBody(s);
+ m_contactPointInPlane = p;
+ m_tIc = m_tIe = m_tI = Vec2(0);
+ }
+
+ void recordImpulse(MyContactElement::ImpulseType type, const State& state,
+ const Vector& lambda) OVERRIDE_11
+ {
+ if (!isSticking(state)) return;
+
+ // Record translational impulse.
+ Vector myImpulseX(1), myImpulseZ(1);
+ m_noslipX.getMyPartFromConstraintSpaceVector(state, lambda, myImpulseX);
+ m_noslipZ.getMyPartFromConstraintSpaceVector(state, lambda, myImpulseZ);
+ const Vec2 tI(myImpulseX[0], myImpulseZ[0]);
+ if (type==MyContactElement::Compression) m_tIc = tI;
+ else if (type==MyContactElement::Expansion) m_tIe = tI;
+ m_tI += tI;
+ }
+
+ // Fill in deltaV to eliminate slip velocity using the stiction
+ // constraints.
+ void setMyDesiredDeltaV(const State& s,
+ Vector& desiredDeltaV) const OVERRIDE_11
+ {
+ if (!isSticking(s)) return;
+
+ const Vec2 dv = -m_contact.getSlipVelocity(s); // X,Z
+ Vector myDesiredDV(1); // Nuke translational velocity also.
+ myDesiredDV[0] = dv[0];
+ m_noslipX.setMyPartInConstraintSpaceVector(s, myDesiredDV, desiredDeltaV);
+ myDesiredDV[0] = dv[1];
+ m_noslipZ.setMyPartInConstraintSpaceVector(s, myDesiredDV, desiredDeltaV);
+ }
+
+ Real getMyImpulseMagnitudeFromConstraintSpaceVector(const State& state,
+ const Vector& lambda) const
+ { Vector myImpulseX(1), myImpulseZ(1);
+ m_noslipX.getMyPartFromConstraintSpaceVector(state, lambda, myImpulseX);
+ m_noslipZ.getMyPartFromConstraintSpaceVector(state, lambda, myImpulseZ);
+ const Vec2 tI(myImpulseX[0], myImpulseZ[0]);
+ return tI.norm();
+ }
+
+
+ std::ostream& writeFrictionInfo(const State& s, const String& indent,
+ std::ostream& o) const OVERRIDE_11
+ {
+ o << indent;
+ if (!isMasterActive(s)) o << "OFF";
+ else if (isSticking(s)) o << "STICK";
+ else o << "SLIP";
+
+ const Vec2 v = m_contact.getSlipVelocity(s);
+ const Vec2 pd = m_sliding->getPrevSlipDir(s);
+ const Vec2 f = isSticking(s) ? getStictionForce(s)
+ : m_sliding->calcSlidingForce(s);
+ o << " prevDir=" << ~pd << " V=" << ~v << " Vdot=" << ~v*pd
+ << " F=" << ~f << endl;
+ return o;
+ }
+
+
+ void showFrictionForce(const State& s, Array_<DecorativeGeometry>& geometry)
+ const OVERRIDE_11
+ {
+ const Real Scale = 0.1;
+ const Vec2 f = isSticking(s) ? getStictionForce(s)
+ : m_sliding->calcSlidingForce(s);
+ if (f.normSqr() < square(SignificantReal))
+ return;
+ const MobilizedBody& bodyB = m_contact.getBody();
+ const Vec3& stationB = m_contact.getBodyStation();
+ const Vec3 stationG = bodyB.getBodyTransform(s)*stationB;
+ const Vec3 endG = stationG - Scale*Vec3(f[0], 0, f[1]);
+ geometry.push_back(DecorativeLine(endG + Vec3(0,.05,0),
+ stationG + Vec3(0,.05,0))
+ .setColor(isSticking(s)?Green:Orange));
+ }
+
+ const MyPointContact& getMyPointContact() const {return m_contact;}
+ const MySlidingFrictionForce& getMySlidingFrictionForce() const
+ { return *m_sliding; }
+private:
+ void initializeRuntimeFields() {
+ m_contactPointInPlane = Vec3(0);
+ m_tIc = m_tIe = m_tI = Vec2(NaN);
+ m_prevN = 0;
+ m_prevSlipDir = Vec2(NaN);
+ }
+ const MyPointContact& m_contact;
+
+ Constraint::NoSlip1D m_noslipX, m_noslipZ; // stiction
+ MySlidingFrictionForce* m_sliding; // sliding friction force element
+
+ // Runtime
+ Vec3 m_contactPointInPlane; // point on plane body where friction will act
+ Vec2 m_tIc, m_tIe, m_tI; // impulses
+
+ Real m_prevN; // master's recorded normal force (could be negative)
+ Vec2 m_prevSlipDir; // master's last recording slip or impending direction
+};
+
+
+//==============================================================================
+// SHOW CONTACT
+//==============================================================================
+// For each visualization frame, generate ephemeral geometry to show just
+// during this frame, based on the current State.
+class ShowContact : public DecorationGenerator {
+public:
+ ShowContact(const MyUnilateralConstraintSet& unis)
+ : m_unis(unis) {}
+
+ void generateDecorations(const State& state,
+ Array_<DecorativeGeometry>& geometry) OVERRIDE_11
+ {
+ for (int i=0; i < m_unis.getNumContactElements(); ++i) {
+ const MyContactElement& contact = m_unis.getContactElement(i);
+ const Vec3 loc = contact.whereToDisplay(state);
+ if (!contact.isDisabled(state)) {
+ geometry.push_back(DecorativeSphere(.1)
+ .setTransform(loc)
+ .setColor(Red).setOpacity(.25));
+ String text = "LOCKED";
+ if (contact.hasFrictionElement()) {
+ const MyFrictionElement& friction = contact.getFrictionElement();
+ text = friction.isSticking(state) ? "STICKING"
+ : "CONTACT";
+ m_unis.getMultibodySystem().realize(state, Stage::Acceleration);
+ friction.showFrictionForce(state, geometry);
+ }
+ geometry.push_back(DecorativeText(String(i)+"-"+text)
+ .setColor(White).setScale(.1)
+ .setTransform(loc+Vec3(0,.04,0)));
+ } else {
+ geometry.push_back(DecorativeText(String(i))
+ .setColor(White).setScale(.1)
+ .setTransform(loc+Vec3(0,.02,0)));
+ }
+ }
+ }
+private:
+ const MyUnilateralConstraintSet& m_unis;
+};
+
+
+//==============================================================================
+// BODY WATCHER
+//==============================================================================
+// Prior to rendering each frame, point the camera at the given body's
+// origin.
+class BodyWatcher : public Visualizer::FrameController {
+public:
+ explicit BodyWatcher(const MobilizedBody& body) : m_body(body) {}
+
+ void generateControls(const Visualizer& viz,
+ const State& state,
+ Array_< DecorativeGeometry >& geometry) OVERRIDE_11
+ {
+ const Vec3 Bo = m_body.getBodyOriginLocation(state);
+ const Vec3 p_GC = Bo + Vec3(0, 1, 5); // above and back
+ const Rotation R_GC(UnitVec3(0,1,0), YAxis,
+ p_GC-Bo, ZAxis);
+ viz.setCameraTransform(Transform(R_GC,p_GC));
+ //viz.pointCameraAt(Bo, Vec3(0,1,0));
+ }
+private:
+ const MobilizedBody m_body;
+};
+
+//==============================================================================
+// STICTION ON HANDLER
+//==============================================================================
+// Allocate one of these for each contact constraint that has friction. This
+// handler takes care of turning on the stiction constraints when the sliding
+// velocity drops to zero.
+class StictionOn: public TriggeredEventHandler {
+public:
+ StictionOn(const MultibodySystem& system,
+ const MyUnilateralConstraintSet& unis,
+ unsigned which)
+ : TriggeredEventHandler(Stage::Velocity),
+ m_mbs(system), m_unis(unis), m_which(which)
+ {
+ getTriggerInfo().setTriggerOnRisingSignTransition(false);
+ }
+
+ // This is the witness function.
+ Real getValue(const State& state) const {
+ const MyFrictionElement& friction = m_unis.getFrictionElement(m_which);
+ if (!friction.isMasterActive(state)) return 0;
+ const Real signedSlipSpeed = friction.calcSlipSpeedWitness(state);
+ return signedSlipSpeed;
+ }
+
+ void handleEvent
+ (State& s, Real accuracy, bool& shouldTerminate) const
+ {
+ SimTK_DEBUG2("\nhandle %d slide->stick@%.17g\n", m_which, s.getTime());
+ SimTK_DEBUG("\n----------------------------------------------------\n");
+ SimTK_DEBUG2("STICTION ON triggered by friction element %d @t=%.15g\n",
+ m_which, s.getTime());
+ m_mbs.realize(s, Stage::Acceleration);
+
+ #ifndef NDEBUG
+ m_unis.showConstraintStatus(s, "ENTER STICTION ON");
+ cout << " triggers=" << s.getEventTriggers() << "\n";
+ #endif
+
+ m_unis.selectActiveConstraints(s, accuracy);
+
+ SimTK_DEBUG("STICTION ON done.\n");
+ SimTK_DEBUG("----------------------------------------------------\n");
+ }
+
+private:
+ const MultibodySystem& m_mbs;
+ const MyUnilateralConstraintSet& m_unis;
+ const int m_which; // one of the friction elements
+};
+
+
+
+//==============================================================================
+// STICTION OFF HANDLER
+//==============================================================================
+// Allocate one of these for each contact constraint. This handler takes
+// care of turning off stiction constraints when the stiction force magnitude
+// exceeds mu*N.
+class StictionOff: public TriggeredEventHandler {
+public:
+ StictionOff(const MultibodySystem& system,
+ const MyUnilateralConstraintSet& unis,
+ unsigned which)
+ : TriggeredEventHandler(Stage::Acceleration),
+ m_mbs(system), m_unis(unis), m_which(which)
+ {
+ getTriggerInfo().setTriggerOnRisingSignTransition(false);
+ }
+
+ // This is the witness function. It is positive as long as mu_s*N is greater
+ // than the friction force magnitude.
+ Real getValue(const State& state) const {
+ const MyFrictionElement& friction = m_unis.getFrictionElement(m_which);
+ if (!friction.isMasterActive(state)) return 0;
+ const Real forceMargin = friction.calcStictionForceWitness(state);
+ return forceMargin; // how much stiction capacity is left
+ }
+
+ void handleEvent
+ (State& s, Real accuracy, bool& shouldTerminate) const
+ {
+ SimTK_DEBUG2("\nhandle %d stick->slide@%.17g\n", m_which, s.getTime());
+ SimTK_DEBUG("\n----------------------------------------------------\n");
+ SimTK_DEBUG2("STICTION OFF triggered by friction element %d @t=%.15g\n",
+ m_which, s.getTime());
+ m_mbs.realize(s, Stage::Acceleration);
+
+ #ifndef NDEBUG
+ cout << " triggers=" << s.getEventTriggers() << "\n";
+ #endif
+
+ m_unis.selectActiveConstraints(s, accuracy);
+
+ SimTK_DEBUG("STICTION OFF done.\n");
+ SimTK_DEBUG("----------------------------------------------------\n");
+ }
+
+private:
+ const MultibodySystem& m_mbs;
+ const MyUnilateralConstraintSet& m_unis;
+ const int m_which; // one of the friction elements
+};
+
+
+//==============================================================================
+// MY PUSH FORCE
+//==============================================================================
+// This is a force element that generates a constant force on a body for a
+// specified time interval. It is useful to perturb the system to force it
+// to transition from sticking to sliding, for example.
+class MyPushForceImpl : public Force::Custom::Implementation {
+public:
+ MyPushForceImpl(const MobilizedBody& bodyB,
+ const Vec3& stationB,
+ const Vec3& forceG, // force direction in Ground!
+ Real onTime,
+ Real offTime)
+ : m_bodyB(bodyB), m_stationB(stationB), m_forceG(forceG),
+ m_on(onTime), m_off(offTime)
+ { }
+
+ //--------------------------------------------------------------------------
+ // Custom force virtuals
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+ OVERRIDE_11
+ {
+ if (!(m_on <= state.getTime() && state.getTime() <= m_off))
+ return;
+
+ m_bodyB.applyForceToBodyPoint(state, m_stationB, m_forceG, bodyForces);
+
+ //SimTK_DEBUG4("PUSHING @t=%g (%g,%g,%g)\n", state.getTime(),
+ // m_forceG[0], m_forceG[1], m_forceG[2]);
+ }
+
+ // No potential energy.
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11 {return 0;}
+
+ void calcDecorativeGeometryAndAppend
+ (const State& state, Stage stage,
+ Array_<DecorativeGeometry>& geometry) const OVERRIDE_11
+ {
+ const Real ScaleFactor = 0.1;
+ if (stage != Stage::Time) return;
+ if (!(m_on <= state.getTime() && state.getTime() <= m_off))
+ return;
+ const Vec3 stationG = m_bodyB.findStationLocationInGround(state, m_stationB);
+ geometry.push_back(DecorativeLine(stationG-ScaleFactor*m_forceG, stationG)
+ .setColor(Yellow)
+ .setLineThickness(3));
+ }
+private:
+ const MobilizedBody& m_bodyB;
+ const Vec3 m_stationB;
+ const Vec3 m_forceG;
+ Real m_on;
+ Real m_off;
+};
+
+
+
+//==============================================================================
+// MAIN
+//==============================================================================
+int main(int argc, char** argv) {
+ try { // If anything goes wrong, an exception will be thrown.
+
+ // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
+ MultibodySystem mbs;
+ SimbodyMatterSubsystem matter(mbs);
+ GeneralForceSubsystem forces(mbs);
+ Force::Gravity gravity(forces, matter, -YAxis, 9.8066);
+
+ MobilizedBody& Ground = matter.updGround();
+
+
+ // Predefine some handy rotations.
+ const Rotation Z90(Pi/2, ZAxis); // rotate +90 deg about z
+
+ const Vec3 BrickHalfDims(.1, .25, .5);
+ const Real BrickMass = 10;
+ #ifdef USE_TIMS_PARAMS
+ const Real RunTime=16; // Tim's time
+ const Real Stiffness = 2e7;
+ const Real Dissipation = 1;
+ const Real CoefRest = 0;
+ // Painleve problem with these friction coefficients.
+ //const Real mu_d = 1; /* compliant: .7*/
+ //const Real mu_s = 1; /* compliant: .7*/
+ const Real mu_d = .5;
+ const Real mu_s = .8;
+ const Real mu_v = /*0.05*/0;
+ const Real CaptureVelocity = 0.01;
+ const Real TransitionVelocity = 0.01;
+ const Inertia brickInertia(.1,.1,.1);
+ const Real Radius = .02;
+ #else
+ const Real RunTime=20;
+ const Real Stiffness = 1e6;
+ const Real CoefRest = 0.5;
+ const Real TargetVelocity = 3; // speed at which to match coef rest
+// const Real Dissipation = (1-CoefRest)/TargetVelocity;
+ const Real Dissipation = 0.1;
+ const Real mu_d = .5;
+ const Real mu_s = .7;
+ const Real mu_v = 0.05;
+ const Real CaptureVelocity = 0.01;
+ const Real TransitionVelocity = 0.05;
+ const Inertia brickInertia(BrickMass*UnitInertia::brick(BrickHalfDims));
+ const Real Radius = BrickHalfDims[0]/3;
+ #endif
+
+ printf("\n******************** Tim's Box ********************\n");
+ #ifdef USE_COMPLIANT_CONTACT
+ printf("USING COMPLIANT CONTACT (with %s)\n",
+ #ifdef USE_ELASTIC_FOUNDATION
+ "elastic foundation mesh");
+ #else
+ "Hertz spheres at corners");
+ #endif
+ #else
+ printf("USING RIGID CONTACT\n");
+ #endif
+ #ifdef USE_TIMS_PARAMS
+ printf("Using Tim's parameters:\n");
+ #else
+ printf("Using Sherm's parameters:\n");
+ #endif
+ #ifdef USE_COMPLIANT_CONTACT
+ printf(" stiffness=%g dissipation=%g\n", Stiffness, Dissipation);
+ #else
+ printf(" coef restitution=%g\n", CoefRest);
+ #endif
+ printf(" mu_d=%g mu_s=%g mu_v=%g\n", mu_d, mu_s, mu_v);
+ printf(" transition velocity=%g\n", TransitionVelocity);
+ printf(" radius=%g\n", Radius);
+ printf(" brick inertia=%g %g %g\n",
+ brickInertia.getMoments()[0], brickInertia.getMoments()[1],
+ brickInertia.getMoments()[2]);
+ printf("******************** Tim's Box ********************\n\n");
+
+ #ifdef USE_COMPLIANT_CONTACT
+ ContactTrackerSubsystem tracker(mbs);
+ CompliantContactSubsystem contact(mbs, tracker);
+ // Set stiction max slip velocity to make it less stiff.
+ contact.setTransitionVelocity(0.05);
+
+
+ // Define a material to use for contact. This is not very stiff.
+ ContactMaterial material(Stiffness,
+ Dissipation,
+ mu_s, // mu_static
+ mu_d, // mu_dynamic
+ mu_v); // mu_viscous
+
+ // Make up an assumed thickness for the elastic layer in case we use
+ // an ElasticFoundation-based compliant contact.
+ const Real Thickness = min(BrickHalfDims);
+
+ // Add a contact surface to represent the ground.
+ // Half space normal is -x; must rotate about z to make it +y.
+ Ground.updBody().addContactSurface(Rotation(-Pi/2,ZAxis),
+ ContactSurface(ContactGeometry::HalfSpace(), material));
+ #endif
+
+ // ADD MOBILIZED BODIES AND CONTACT CONSTRAINTS
+
+
+ MyUnilateralConstraintSet unis(mbs, CaptureVelocity);
+
+ Body::Rigid brickBody =
+ Body::Rigid(MassProperties(BrickMass, Vec3(0), brickInertia));
+
+ // First body: cube
+ //MobilizedBody::Cartesian loc(Ground, MassProperties(0,Vec3(0),Inertia(0)));
+ //MobilizedBody::Ball brick(loc, Vec3(0),
+ // brickBody, Vec3(0));
+ MobilizedBody::Free brick(Ground, Vec3(0),
+ brickBody, Vec3(0));
+ brick.addBodyDecoration(Transform(), DecorativeBrick(BrickHalfDims)
+ .setColor(Red).setOpacity(.3));
+/*
+1) t= 0.5, dt = 2 sec, pt = (0.05, 0.2, 0.4), fdir = (1,0,0), mag = 50N
+2) t= 4.0, dt = 0.5 sec, pt = (0.03, 0.06, 0.09), fdir = (0.2,0.8,0), mag = 300N
+3) t= 0.9, dt = 2 sec, pt = (0,0,0), fdir = (0,1,0), mag = 49.333N (half the weight of the block)
+4) t= 13.0, dt = 1 sec, pt = (0 0 0), fdir = (-1,0,0), mag = 200N
+*/
+ Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0.05,0.2,0.4),
+ 50 * Vec3(1,0,0),
+ 0.5, 0.5+2));
+ Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0.03, 0.06, 0.09),
+ 300 * UnitVec3(0.2,0.8,0),
+ //300 * Vec3(0.2,0.8,0),
+ 4, 4+0.5));
+ Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0),
+ 49.033 * Vec3(0,1,0),
+ 9., 9.+2));
+ Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0),
+ 200 * Vec3(-1,0,0),
+ 13, 13+1));
+
+ #ifndef USE_TIMS_PARAMS
+ // Extra late force.
+ Force::Custom(forces, new MyPushForceImpl(brick, Vec3(.1, 0, .45),
+ 20 * Vec3(-1,-1,.5),
+ 15, Infinity));
+ #endif
+
+ #ifdef USE_COMPLIANT_CONTACT
+ #ifndef USE_ELASTIC_FOUNDATION
+ // Using spheres at the corners:
+ for (int i=-1; i<=1; i+=2)
+ for (int j=-1; j<=1; j+=2)
+ for (int k=-1; k<=1; k+=2) {
+ const Vec3 pt = Vec3(i,j,k).elementwiseMultiply(BrickHalfDims);
+ const Vec3 shrink =
+ (abs(pt)-Radius).elementwiseDivide(abs(pt)).elementwiseMultiply(pt);
+ brick.updBody().addContactSurface(shrink,
+ ContactSurface(ContactGeometry::Sphere(Radius), material));
+ brick.addBodyDecoration(shrink, DecorativeSphere(Radius));
+ }
+ #else
+ // Using a brick mesh.
+ //const PolygonalMesh mesh =
+ // PolygonalMesh::createCylinderMesh(UnitVec3(1,1,1),BrickHalfDims[0],BrickHalfDims[2],1);
+ //DecorativeMesh cylDec(cylMesh);
+ //matter.updGround().addBodyDecoration(Vec3(-2,3,0),
+ // cylDec
+ // );
+ //matter.updGround().addBodyDecoration(Vec3(-2,3,0),
+ // DecorativeMesh(cylMesh).setScale(1.02)
+ // .setRepresentation(DecorativeGeometry::DrawWireframe).setColor(Red)
+ // );
+
+ const int rez = 2*(int)(max(BrickHalfDims)/min(BrickHalfDims) + 0.5);
+ const PolygonalMesh mesh =
+ PolygonalMesh::createBrickMesh(BrickHalfDims,rez);
+
+ const ContactGeometry::TriangleMesh triMesh(mesh);
+ DecorativeMesh decMesh(triMesh.createPolygonalMesh());
+
+ decMesh.setLineThickness(3)
+ .setRepresentation(DecorativeGeometry::DrawWireframe)
+ ;
+ brick.updBody().addContactSurface(Vec3(0),
+ ContactSurface(triMesh,material,Thickness));
+ brick.addBodyDecoration(Vec3(0), decMesh);
+ #endif
+ #else
+ for (int i=-1; i<=1; i+=2)
+ for (int j=-1; j<=1; j+=2)
+ for (int k=-1; k<=1; k+=2) {
+ const Vec3 pt = Vec3(i,j,k).elementwiseMultiply(BrickHalfDims);
+ MyPointContact* contact = new MyPointContact(brick, pt, CoefRest);
+ unis.addContactElement(contact);
+ unis.addFrictionElement(
+ new MyPointContactFriction(*contact, mu_d, mu_s, mu_v,
+ CaptureVelocity, // TODO: vtol?
+ forces));
+
+ }
+ #endif
+
+ matter.setShowDefaultGeometry(false);
+ Visualizer viz(mbs);
+ //viz.setDesiredFrameRate(1000);
+ viz.setShowSimTime(true);
+ viz.setShowFrameNumber(true);
+ viz.setShowFrameRate(true);
+ viz.addDecorationGenerator(new ShowContact(unis));
+#ifdef ANIMATE
+ mbs.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));
+#else
+ // This does nothing but interrupt the simulation.
+ mbs.addEventReporter(new Nada(ReportInterval));
+#endif
+
+ viz.addFrameController(new BodyWatcher(brick));
+
+ Vec3 cameraPos(0, 1, 2);
+ UnitVec3 cameraZ(0,0,1);
+ viz.setCameraTransform(Transform(Rotation(cameraZ, ZAxis, UnitVec3(YAxis), YAxis), cameraPos));
+ viz.pointCameraAt(Vec3(0,0,0), Vec3(0,1,0));
+
+#ifdef USE_TIMS_PARAMS
+ Real accuracy = 1e-4;
+ RungeKuttaMersonIntegrator integ(mbs);
+#else
+ Real accuracy = 1e-2;
+ RungeKutta3Integrator integ(mbs);
+#endif
+ integ.setAccuracy(accuracy);
+ //integ.setMaximumStepSize(0.25);
+ integ.setMaximumStepSize(0.05);
+ //integ.setMaximumStepSize(0.001);
+
+ StateSaver* stateSaver = new StateSaver(mbs,unis,integ,ReportInterval);
+ mbs.addEventReporter(stateSaver);
+
+ #ifndef USE_COMPLIANT_CONTACT
+ for (int i=0; i < unis.getNumContactElements(); ++i) {
+ mbs.addEventHandler(new ContactOn(mbs, unis,i, Stage::Position));
+ mbs.addEventHandler(new ContactOn(mbs, unis,i, Stage::Velocity));
+ mbs.addEventHandler(new ContactOn(mbs, unis,i, Stage::Acceleration));
+ mbs.addEventHandler(new ContactOff(mbs, unis,i));
+ }
+
+ for (int i=0; i < unis.getNumFrictionElements(); ++i) {
+ mbs.addEventHandler(new StictionOn(mbs, unis, i));
+ mbs.addEventHandler(new StictionOff(mbs, unis, i));
+ }
+ #endif
+
+ State s = mbs.realizeTopology(); // returns a reference to the the default state
+
+ //matter.setUseEulerAngles(s, true);
+
+ mbs.realizeModel(s); // define appropriate states for this System
+ mbs.realize(s, Stage::Instance); // instantiate constraints if any
+
+
+/*
+rX_q = 0.7 rad
+rX_u = 1.0 rad/sec
+
+rY_q = 0.6 rad
+rY_u = 0.0 rad/sec
+
+rZ_q = 0.5 rad
+rZ_u = 0.2 rad/sec
+
+tX_q = 0.0 m
+tX_u = 10 m/s
+
+tY_q = 1.4 m
+tY_u = 0.0 m/s
+
+tZ_q = 0.0 m
+tZ_u = 0.0 m/s
+*/
+
+#ifdef USE_TIMS_PARAMS
+ brick.setQToFitTranslation(s, Vec3(0,10,0));
+ brick.setUToFitLinearVelocity(s, Vec3(0,0,0));
+#else
+ brick.setQToFitTranslation(s, Vec3(0,1.4,0));
+ brick.setUToFitLinearVelocity(s, Vec3(10,0,0));
+ const Rotation R_BC(SimTK::BodyRotationSequence,
+ 0.7, XAxis, 0.6, YAxis, 0.5, ZAxis);
+ brick.setQToFitRotation(s, R_BC);
+ brick.setUToFitAngularVelocity(s, Vec3(1,0,.2));
+#endif
+
+ mbs.realize(s, Stage::Velocity);
+ viz.report(s);
+
+ Array_<int> enableTheseContacts;
+ for (int i=0; i < unis.getNumContactElements(); ++i) {
+ const Real perr = unis.getContactElement(i).getPerr(s);
+ printf("contact constraint %d has perr=%g%s\n", i, perr,
+ perr<=0?" (ENABLING CONTACT)":"");
+ if (perr <= 0)
+ enableTheseContacts.push_back(i);
+ }
+
+ cout << "Initial configuration shown. Next? ";
+ getchar();
+
+ for (unsigned i=0; i < enableTheseContacts.size(); ++i)
+ unis.getContactElement(enableTheseContacts[i]).enable(s);
+
+ Assembler(mbs).setErrorTolerance(1e-6).assemble(s);
+ viz.report(s);
+ cout << "Assembled configuration shown. Ready? ";
+ getchar();
+
+ // Now look for enabled contacts that aren't sliding; turn on stiction
+ // for those.
+ mbs.realize(s, Stage::Velocity);
+ Array_<int> enableTheseStictions;
+ for (int i=0; i < unis.getNumFrictionElements(); ++i) {
+ break; //TODO
+ MyFrictionElement& fric = unis.updFrictionElement(i);
+ const Real vSlip = fric.getActualSlipSpeed(s);
+ fric.initializeForStiction(s); // just in case
+ printf("friction element %d has v_slip=%g%s\n", i, vSlip,
+ vSlip==0?" (ENABLING STICTION)":"");
+ if (vSlip == 0)
+ enableTheseStictions.push_back(i);
+ }
+ for (unsigned i=0; i < enableTheseStictions.size(); ++i)
+ unis.getFrictionElement(enableTheseStictions[i]).enableStiction(s);
+
+ // Make sure Lapack gets initialized.
+ Matrix M(1,1); M(0,0)=1.23;
+ FactorLU Mlu(M);
+
+
+ // Simulate it.
+
+ integ.setReturnEveryInternalStep(true);
+ //integ.setAllowInterpolation(false);
+ TimeStepper ts(mbs, integ);
+ ts.setReportAllSignificantStates(true);
+
+ #ifdef TEST_REPEATABILITY
+ const int tries = NTries;
+ #else
+ const int tries = 1;
+ #endif
+
+ Array_< Array_<State> > states(tries);
+ Array_< Array_<Real> > timeDiff(tries-1);
+ Array_< Array_<Vector> > yDiff(tries-1);
+ cout.precision(18);
+ for (int ntry=0; ntry < tries; ++ntry) {
+ mbs.resetAllCountersToZero();
+ unis.initialize(); // reinitialize
+ ts.updIntegrator().resetAllStatistics();
+ ts.initialize(s);
+ int nStepsWithEvent = 0;
+
+ const double startReal = realTime();
+ const double startCPU = cpuTime();
+
+ Integrator::SuccessfulStepStatus status;
+ do {
+ status=ts.stepTo(RunTime);
+ #ifdef TEST_REPEATABILITY
+ states[ntry].push_back(ts.getState());
+ #endif
+ const int j = states[ntry].size()-1;
+ if (ntry>0) {
+ int prev = ntry-1;
+ //prev=0;
+ Real dt = states[ntry][j].getTime()
+ - states[prev][j].getTime();
+ volatile double vd;
+ const int ny = states[ntry][j].getNY();
+ Vector d(ny);
+ for (int i=0; i<ny; ++i) {
+ vd = states[ntry][j].getY()[i]
+ - states[prev][j].getY()[i];
+ d[i] = vd;
+ }
+ timeDiff[prev].push_back(dt);
+ yDiff[prev].push_back(d);
+ cout << "\n" << states[prev][j].getTime()
+ << "+" << dt << ":" << d << endl;
+ }
+
+ const bool handledEvent = status==Integrator::ReachedEventTrigger;
+ if (handledEvent) {
+ ++nStepsWithEvent;
+ SimTK_DEBUG3("EVENT %3d @%.17g status=%s\n\n",
+ nStepsWithEvent, ts.getTime(),
+ Integrator::getSuccessfulStepStatusString(status).c_str());
+ } else {
+ SimTK_DEBUG3("step %3d @%.17g status=%s\n", integ.getNumStepsTaken(),
+ ts.getTime(),
+ Integrator::getSuccessfulStepStatusString(status).c_str());
+ }
+ #ifndef NDEBUG
+ viz.report(ts.getState());
+ #endif
+ } while (ts.getTime() < RunTime);
+
+
+ const double timeInSec = realTime()-startReal;
+ const double cpuInSec = cpuTime()-startCPU;
+ const int evals = integ.getNumRealizations();
+ cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
+ timeInSec << "s for " << ts.getTime() << "s sim (avg step="
+ << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
+ << (1000*ts.getTime())/evals << "ms/eval\n";
+ cout << "CPUtime " << cpuInSec << endl;
+
+ printf("Used Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
+ printf("# EVENT STEPS/HANDLER CALLS = %d/%d\n",
+ nStepsWithEvent, mbs.getNumHandleEventCalls());
+ }
+
+ for (int i=0; i<tries; ++i)
+ cout << "nstates " << i << " " << states[i].size() << endl;
+
+ // Instant replay.
+ while(true) {
+ printf("Hit ENTER for replay (%d states) ...",
+ stateSaver->getNumSavedStates());
+ getchar();
+ for (int i=0; i < stateSaver->getNumSavedStates(); ++i) {
+ mbs.realize(stateSaver->getState(i), Stage::Velocity);
+ viz.report(stateSaver->getState(i));
+ }
+ }
+
+ }
+ catch (const std::exception& e) {
+ printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+ }
+ catch (...) {
+ printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+}
+
+
+//==============================================================================
+// IMPACT HANDLING (CONTACT ON)
+//==============================================================================
+
+//------------------------------ HANDLE EVENT ----------------------------------
+// There are three different witness functions that cause this handler to get
+// invoked. The position- and velocity-level ones require an impulse. The
+// acceleration-level one just requires recalculating the active set, in the
+// same manner as liftoff or friction transition events.
+
+void ContactOn::
+handleEvent(State& s, Real accuracy, bool& shouldTerminate) const
+{
+ SimTK_DEBUG3("\nhandle %d impact@%.17g (%s)\n", m_which, s.getTime(),
+ m_stage.getName().c_str());
+
+ if (m_stage == Stage::Acceleration) {
+ SimTK_DEBUG("\n---------------CONTACT ON (ACCEL)--------------\n");
+ SimTK_DEBUG2("CONTACT triggered by element %d @t=%.15g\n",
+ m_which, s.getTime());
+ m_mbs.realize(s, Stage::Acceleration);
+
+ #ifndef NDEBUG
+ cout << " triggers=" << s.getEventTriggers() << "\n";
+ #endif
+
+ m_unis.selectActiveConstraints(s, accuracy);
+ SimTK_DEBUG("---------------CONTACT ON (ACCEL) done.--------------\n");
+ return;
+ }
+
+ MyElementSubset proximal;
+ m_unis.findProximalElements(s, accuracy, proximal);
+
+ // Zero out accumulated impulses and perform any other necessary
+ // initialization of contact and friction elements.
+ for (unsigned i=0; i < proximal.m_contact.size(); ++i) {
+ const int which = proximal.m_contact[i];
+ m_unis.updContactElement(which)
+ .initializeForImpact(s, m_unis.getCaptureVelocity());
+ }
+ for (unsigned i=0; i < proximal.m_friction.size(); ++i) {
+ const int which = proximal.m_friction[i];
+ m_unis.updFrictionElement(which).initializeForStiction(s);
+ }
+
+ SimTK_DEBUG("\n---------------------CONTACT ON---------------------\n");
+ SimTK_DEBUG3("\nIMPACT (%s) for contact %d at t=%.16g\n",
+ m_stage.getName().c_str(), m_which, s.getTime());
+ SimTK_DEBUG2(" %d/%d proximal contact/friction elements\n",
+ proximal.m_contact.size(), proximal.m_friction.size());
+
+ m_unis.showConstraintStatus(s, "ENTER IMPACT (CONTACT ON)");
+
+ bool needMoreCompression = true;
+ while (needMoreCompression) {
+ processCompressionPhase(proximal, s);
+ needMoreCompression = false;
+ if (processExpansionPhase(proximal, s)) {
+ for (unsigned i=0; i < proximal.m_contact.size(); ++i) {
+ const int which = proximal.m_contact[i];
+ const MyContactElement& contact =
+ m_unis.getContactElement(which);
+ if (contact.getVerr(s) < 0) {
+ needMoreCompression = true;
+ break;
+ }
+ }
+ }
+ }
+
+ // Record new previous slip velocities for all the sliding friction
+ // since velocities have changed. First loop collects the velocities.
+ m_mbs.realize(s, Stage::Velocity);
+ for (unsigned i=0; i < proximal.m_friction.size(); ++i) {
+ const int which = proximal.m_friction[i];
+ MyFrictionElement& fric = m_unis.updFrictionElement(which);
+ if (!fric.isMasterActive(s) || fric.isSticking(s)) continue;
+ fric.recordSlipDir(s);
+ }
+
+ // Now update all the previous slip direction state variables from the
+ // recorded values.
+ for (unsigned i=0; i < proximal.m_friction.size(); ++i) {
+ const int which = proximal.m_friction[i];
+ const MyFrictionElement& fric = m_unis.getFrictionElement(which);
+ if (!fric.isMasterActive(s) || fric.isSticking(s)) continue;
+ fric.updatePreviousSlipDirFromRecorded(s);
+ }
+
+ m_unis.selectActiveConstraints(s, accuracy);
+
+ SimTK_DEBUG("\n-----------------END CONTACT ON---------------------\n");
+}
+
+
+
+
+//------------------------ PROCESS COMPRESSION PHASE ---------------------------
+// Given a list of proximal unilateral constraints (contact and stiction),
+// determine which ones are active in the least squares solution for the
+// constraint impulses. Candidates are those constraints that meet the
+// kinematic proximity condition -- for contacts, position less than
+// tolerance; for stiction, master contact is proximal. Also, any
+// constraint that is currently active is a candidate, regardless of its
+// kinematics.
+//
+// TODO: NOT ENABLING STICTION AT ALL; assuming stiction on initially leads
+// to a lot of wasted time and solution difficulty. Must start with sliding.
+// TODO: sliding friction impulses
+//
+// Algorithm
+// ---------
+// We're given a set of candidates including contacts and stiction. If any are
+// inactive, activate them.
+// -- at this point all verr==0, some impulses f might be < 0
+//
+// loop
+// - Calculate impulses with the current active set
+// (iterate sliding impulses until f=mu_d*N to tol, where N is normal
+// impulse)
+// - Calculate f for active constraints, verr for inactive
+// - If all f>=0, verr>=0 -> break loop
+// - Check for verr < 0 [tol?]. Shouldn't happen but if it does must turn on the
+// associated constraint for the worst violation, then -> continue loop
+// - Find worst (most negative) offender:
+// contact offense = fc < 0 ? fc : 0
+// stiction offense = mu_d*max(0, fc) - |fs|
+// - Choose constraint to deactivate:
+// worst is a stiction constraint: choose it
+// worst is a contact constraint: if it has stiction, chose that
+// otherwise choose the contact constraint
+// - Inactivate chosen constraint
+// (if stiction, record impending slip direction & N for stick->slide)
+// end loop
+//
+void ContactOn::
+processCompressionPhase(MyElementSubset& proximal,
+ State& s) const
+{
+ const int ReviseNormalNIters = 6;
+
+ SimTK_DEBUG2("Entering processCompressionPhase(): "
+ "%d/%d impact/stick candidates ...\n", proximal.m_contact.size(),
+ proximal.m_friction.size());
+
+ if (proximal.m_contact.empty()) {
+ // Can't be any friction either, if there are no contacts.
+ SimTK_DEBUG("EXIT processCompressionPhase: no proximal candidates.\n");
+ return;
+ }
+
+ Vector lambda;
+ const Vector u0 = s.getU(); // save presenting velocity
+
+ // Assume at first that all proximal contacts will participate. This is
+ // necessary to ensure that we get a least squares solution for the impulse
+ // involving as many constraints as possible sharing the impulse.
+ // TODO: note stiction is unconditionally disabled
+ m_unis.enableConstraintSubset(proximal, false, s);
+
+ int pass=0, nContactsDisabled=0, nStictionDisabled=0, nContactsReenabled=0;
+ while (true) {
+ ++pass;
+ SimTK_DEBUG1("processCompressionPhase(): pass %d\n", pass);
+
+ // Given an active set, evaluate impulse multipliers & forces, and
+ // evaluate resulting constraint velocity errors.
+ calcStoppingImpulse(proximal, s, lambda);
+ // TODO: ignoring sliding impacts; should converge here.
+ updateVelocities(u0, lambda, s);
+
+ // Scan all proximal contacts to find the active one that has the
+ // most negative normal force, and the inactive one that has the
+ // most negative velocity error (hopefully none will).
+
+ int worstActiveContact=-1; Real mostNegativeContactImpulse=0;
+ int worstInactiveContact=-1; Real mostNegativeVerr=0;
+
+ SimTK_DEBUG("analyzing impact constraints ...\n");
+ for (unsigned i=0; i < proximal.m_contact.size(); ++i) {
+ const int which = proximal.m_contact[i];
+ SimTK_DEBUG1(" %d: ", which);
+ const MyContactElement& cont = m_unis.getContactElement(which);
+ if (cont.isDisabled(s)) {
+ const Real verr = cont.getVerr(s);
+ SimTK_DEBUG1("off verr=%g\n", verr);
+ if (verr < mostNegativeVerr) {
+ worstInactiveContact = which;
+ mostNegativeVerr = verr;
+ }
+ } else {
+ const Real f =
+ cont.getMyValueFromConstraintSpaceVector(s, lambda);
+ SimTK_DEBUG1("on impulse=%g\n", f);
+ if (f < mostNegativeContactImpulse) {
+ worstActiveContact = which;
+ mostNegativeContactImpulse = f;
+ }
+ }
+ }
+
+ // This is bad and might cause cycling.
+ if (mostNegativeVerr < 0) {
+ SimTK_DEBUG2(" !!! Inactive contact %d violated, verr=%g\n",
+ worstInactiveContact, mostNegativeVerr);
+ const MyContactElement& cont =
+ m_unis.getContactElement(worstInactiveContact);
+ //TODO -- must use a tolerance or prevent looping
+ //++nContactsReenabled;
+ //cont.enable(s);
+ //continue;
+ }
+
+ SimTK_DEBUG(" All inactive contacts are satisfied.\n");
+
+ #ifndef NDEBUG
+ if (mostNegativeContactImpulse == 0)
+ printf(" All active contacts are satisfied.\n");
+ else
+ printf(" Active contact %d was worst violator with f=%g\n",
+ worstActiveContact, mostNegativeContactImpulse);
+ #endif
+
+ int worstActiveStiction=-1; Real mostNegativeStictionCapacity=0;
+ SimTK_DEBUG("analyzing stiction constraints ...\n");
+ for (unsigned i=0; i < proximal.m_friction.size(); ++i) {
+ const int which = proximal.m_friction[i];
+ SimTK_DEBUG1(" %d: ", which);
+ const MyFrictionElement& fric = m_unis.getFrictionElement(which);
+ if (!fric.isSticking(s)) {
+ SimTK_DEBUG("off\n");
+ continue;
+ }
+ // TODO: Kludge -- must be point contact.
+ const MyPointContactFriction& ptfric =
+ dynamic_cast<const MyPointContactFriction&>(fric);
+ const MyPointContact& cont = ptfric.getMyPointContact();
+ const Real N = cont.getMyValueFromConstraintSpaceVector(s, lambda);
+ const Real fsmag =
+ ptfric.getMyImpulseMagnitudeFromConstraintSpaceVector(s, lambda);
+ const Real mu_d = fric.getDynamicFrictionCoef();
+ const Real capacity = mu_d*std::max(N,Real(0)) - fsmag;
+ SimTK_DEBUG2("on capacity=%g (N=%g)\n", capacity, N);
+
+ if (capacity < mostNegativeStictionCapacity) {
+ worstActiveStiction = which;
+ mostNegativeStictionCapacity = capacity;
+ }
+ }
+
+ #ifndef NDEBUG
+ if (mostNegativeStictionCapacity == 0)
+ printf(" All active stiction constraints are satisfied.\n");
+ else
+ printf(" Active stiction %d was worst violator with capacity=%g\n",
+ worstActiveStiction, mostNegativeStictionCapacity);
+ #endif
+
+ if (mostNegativeContactImpulse==0 && mostNegativeStictionCapacity==0) {
+ SimTK_DEBUG("DONE. Current active set is a winner.\n");
+ break;
+ }
+
+ // Restore original velocity.
+ s.updU() = u0;
+
+ if (mostNegativeStictionCapacity <= mostNegativeContactImpulse) {
+ SimTK_DEBUG1(" Disable stiction %d\n", worstActiveStiction);
+ MyFrictionElement& fric =
+ m_unis.updFrictionElement(worstActiveStiction);
+ // TODO: need the impulse version of this
+ //fric.recordImpendingSlipInfo(s);
+ ++nStictionDisabled;
+ fric.disableStiction(s);
+ continue;
+ }
+
+ // A contact constraint was the worst violator. If that contact
+ // constraint has an active stiction constraint, we have to disable
+ // the stiction constraint first.
+ SimTK_DEBUG1(" Contact %d was the worst violator.\n",
+ worstActiveContact);
+ const MyContactElement& cont =
+ m_unis.getContactElement(worstActiveContact);
+ assert(!cont.isDisabled(s));
+
+ if (cont.hasFrictionElement()) {
+ MyFrictionElement& fric = cont.updFrictionElement();
+ if (fric.isSticking(s)) {
+ SimTK_DEBUG1(" ... but must disable stiction %d first.\n",
+ fric.getFrictionIndex());
+ // TODO: need the impulse version of this
+ //fric.recordImpendingSlipInfo(s);
+ ++nStictionDisabled;
+ fric.disableStiction(s);
+ continue;
+ }
+ }
+
+ SimTK_DEBUG1(" Disable contact %d\n", worstActiveContact);
+ ++nContactsDisabled;
+ cont.disable(s);
+ // Go back for another pass.
+ }
+
+
+ // Now update the entries for each proximal constraint to reflect the
+ // compression impulse and post-compression velocity.
+ SimTK_DEBUG(" Compression results:\n");
+ for (unsigned i=0; i < proximal.m_contact.size(); ++i) {
+ const int which = proximal.m_contact[i];
+ MyContactElement& cont = m_unis.updContactElement(which);
+ if (!cont.isDisabled(s))
+ cont.recordImpulse(MyContactElement::Compression, s, lambda);
+ SimTK_DEBUG4(" %d %3s: Ic=%g, V=%g\n",
+ which, cont.isDisabled(s) ? "off" : "ON",
+ cont.getCompressionImpulse(), cont.getVerr(s));
+ }
+
+ SimTK_DEBUG("... compression phase done.\n");
+}
+
+//------------------------- PROCESS EXPANSION PHASE ----------------------------
+bool ContactOn::
+processExpansionPhase(MyElementSubset& proximal,
+ State& s) const
+{
+ SimTK_DEBUG("Entering processExpansionPhase() ...\n");
+
+ // Generate an expansion impulse if there were any active contacts that
+ // still have some restitution remaining.
+ Vector expansionImpulse;
+
+ bool anyChange = false;
+ for (unsigned i=0; i<proximal.m_contact.size(); ++i) {
+ const int which = proximal.m_contact[i];
+ MyContactElement& uni = m_unis.updContactElement(which);
+ if (uni.isDisabled(s)||uni.isRestitutionDone()
+ ||uni.getEffectiveCoefRest()==0
+ ||uni.getCompressionImpulse()<=0)
+ continue;
+ uni.setMyExpansionImpulse(s, uni.getEffectiveCoefRest(),
+ expansionImpulse);
+ uni.recordImpulse(MyContactElement::Expansion,s,expansionImpulse);
+ uni.setRestitutionDone(true);
+ anyChange = true;
+ }
+
+ if (!anyChange) {
+ SimTK_DEBUG("... no expansion impulse -- done.\n");
+ return false;
+ }
+
+ // We generated an expansion impulse. Apply it and update velocities.
+ updateVelocities(Vector(), expansionImpulse, s);
+
+ // Release any constraint that now has a positive velocity.
+ Array_<int> toDisable;
+ for (unsigned i=0; i < proximal.m_contact.size(); ++i) {
+ const int which = proximal.m_contact[i];
+ const MyContactElement& uni = m_unis.getContactElement(which);
+ if (!uni.isDisabled(s) && uni.getVerr(s) > 0)
+ toDisable.push_back(which);
+ }
+
+ // Now do the actual disabling (can't mix this with checking velocities)
+ // because disabling invalidates Instance stage.
+ for (unsigned i=0; i < toDisable.size(); ++i) {
+ const int which = toDisable[i];
+ const MyContactElement& uni = m_unis.getContactElement(which);
+ uni.disable(s);
+ }
+
+ SimTK_DEBUG(" Expansion results:\n");
+ m_mbs.realize(s, Stage::Velocity);
+ for (unsigned i=0; i < proximal.m_contact.size(); ++i) {
+ const int which = proximal.m_contact[i];
+ const MyContactElement& uni = m_unis.getContactElement(which);
+ SimTK_DEBUG4(" %d %3s: Ie=%g, V=%g\n",
+ which, uni.isDisabled(s) ? "off" : "ON",
+ uni.getExpansionImpulse(), uni.getVerr(s));
+ }
+
+ SimTK_DEBUG("... expansion phase done.\n");
+
+ return true;
+}
+
+
+
+
+//-------------------------- CALC STOPPING IMPULSE -----------------------------
+// Calculate the impulse that eliminates all residual velocity for the
+// current set of enabled constraints.
+// Note: if you have applied impulses also (like sliding friction),
+// convert to generalized impulse f, then to desired delta V in constraint
+// space like this: deltaV = G*M\f; add that to the verrs to get the total
+// velocity change that must be produced by the impulse.
+void ContactOn::
+calcStoppingImpulse(const MyElementSubset& proximal,
+ const State& s,
+ Vector& lambda0) const
+{
+ const SimbodyMatterSubsystem& matter = m_mbs.getMatterSubsystem();
+ m_mbs.realize(s, Stage::Dynamics); // TODO: should only need Position
+ Vector desiredDeltaV; // in constraint space
+ SimTK_DEBUG(" Entering calcStoppingImpulse() ...\n");
+ bool gotOne = false;
+ for (unsigned i=0; i < proximal.m_contact.size(); ++i) {
+ const int which = proximal.m_contact[i];
+ const MyContactElement& uni = m_unis.getContactElement(which);
+ if (uni.isDisabled(s))
+ continue;
+ SimTK_DEBUG2(" uni constraint %d enabled, v=%g\n",
+ which, uni.getVerr(s));
+ uni.setMyDesiredDeltaV(s, desiredDeltaV);
+ gotOne = true;
+ }
+ for (unsigned i=0; i < proximal.m_friction.size(); ++i) {
+ const int which = proximal.m_friction[i];
+ const MyFrictionElement& fric = m_unis.getFrictionElement(which);
+ if (!fric.isSticking(s))
+ continue;
+ SimTK_DEBUG2(" friction constraint %d enabled, |v|=%g\n",
+ which, fric.getActualSlipSpeed(s));
+ fric.setMyDesiredDeltaV(s, desiredDeltaV);
+ gotOne = true;
+ }
+
+ if (gotOne) matter.solveForConstraintImpulses(s, desiredDeltaV, lambda0);
+ else lambda0.clear();
+#ifndef NDEBUG
+ cout << " ... done. Stopping impulse=" << lambda0 << endl;
+#endif
+}
+
+
+
+//---------------------------- UPDATE VELOCITIES -------------------------------
+void ContactOn::
+updateVelocities(const Vector& u0, const Vector& lambda, State& state) const {
+ const SimbodyMatterSubsystem& matter = m_mbs.getMatterSubsystem();
+ Vector f, deltaU;
+ assert(u0.size()==0 || u0.size() == state.getNU());
+
+ m_mbs.realize(state, Stage::Dynamics); // TODO: Position
+ matter.multiplyByGTranspose(state,lambda,f);
+ matter.multiplyByMInv(state,f,deltaU);
+ if (u0.size()) state.updU() = u0 + deltaU;
+ else state.updU() += deltaU;
+ m_mbs.realize(state, Stage::Velocity);
+}
+
+
+
+//==============================================================================
+// MY UNILATERAL CONSTRAINT SET
+//==============================================================================
+
+
+//------------------------ SELECT ACTIVE CONSTRAINTS ---------------------------
+void MyUnilateralConstraintSet::
+selectActiveConstraints(State& state, Real accuracy) const {
+
+ // Find all the contacts and stiction elements that might be active based
+ // on kinematics.
+ MyElementSubset candidates;
+
+ bool needRestart;
+ do {
+ //TODO: this (mis)use of accuracy needs to be revisited.
+ findCandidateElements(state, accuracy, accuracy, candidates);
+
+ // Evaluate accelerations and reaction forces and check if
+ // any of the active contacts are generating negative ("pulling")
+ // forces; if so, inactivate them.
+ findActiveCandidates(state, candidates);
+
+ Array_<Real> slipVels(candidates.m_friction.size());
+ for (unsigned i=0; i < candidates.m_friction.size(); ++i) {
+ const int which = candidates.m_friction[i];
+ const MyFrictionElement& fric = getFrictionElement(which);
+ slipVels[i] = fric.getActualSlipSpeed(state);
+ }
+
+ // Finally, project active constraints to the constraint manifolds.
+ const Real tol = accuracy/1000;
+ SimTK_DEBUG1("projecting active constraints to tol=%g\n", tol);
+ m_mbs.project(state, tol);
+
+ // It is possible that the projection above took some marginally-sliding
+ // friction and slowed it down enough to make it a stiction candidate.
+ needRestart = false;
+ for (unsigned i=0; i < candidates.m_sliding.size(); ++i) {
+ const int which = candidates.m_sliding[i];
+ const MyFrictionElement& fric = getFrictionElement(which);
+ if ( fric.getActualSlipSpeed(state) <= accuracy
+ || fric.calcSlipSpeedWitness(state) <= 0)
+ {
+ SimTK_DEBUG3("***RESTART1** selectActiveConstraints() because "
+ "sliding velocity of friction %d is |v|=%g or witness=%g\n",
+ which, fric.getActualSlipSpeed(state),
+ fric.calcSlipSpeedWitness(state));
+ needRestart = true;
+ break;
+ }
+ }
+ if (needRestart) continue;
+ for (unsigned i=0; i < candidates.m_friction.size(); ++i) {
+ const int which = candidates.m_friction[i];
+ const MyFrictionElement& fric = getFrictionElement(which);
+ if (fric.isSticking(state)) continue;
+ if (slipVels[i] > accuracy
+ && fric.getActualSlipSpeed(state) <= accuracy)
+ {
+ SimTK_DEBUG3("***RESTART2** selectActiveConstraints() because "
+ "sliding velocity of friction %d went from |v|=%g to |v|=%g\n",
+ which, slipVels[i], fric.getActualSlipSpeed(state));
+ needRestart = true;
+ break;
+ }
+ }
+
+ } while (needRestart);
+}
+
+
+
+
+//-------------------------- FIND ACTIVE CANDIDATES ---------------------------
+// Given a list of candidate unilateral constraints (contact and stiction),
+// determine which ones are active in the least squares solution for the
+// constraint multipliers. Candidates are those constraints that meet all
+// kinematic conditions -- for contacts, position and velocity errors less than
+// tolerance; for stiction, sliding velocity less than tolerance. Also, any
+// constraint that is currently active is a candidate, regardless of its
+// kinematics.
+//
+// This method should be called only from within an event handler. For sliding
+// friction it will have reset the "previous slip direction" to the current
+// slip or impending slip direction, and converged the remembered normal force.
+//
+// Algorithm
+// ---------
+// We're given a set of candidates including contacts and stiction. If any are
+// inactive, activate them.
+// -- at this point all aerr==0, some ferr might be < 0
+//
+// loop
+// - Realize(Acceleration) with the current active set
+// (iterate sliding forces until f=mu_d*N to tol)
+// - Calculate ferr for active constraints, aerr for inactive
+// - If all ferr>=0, aerr>=0 -> break loop
+// - Check for aerr < 0 [tol?]. Shouldn't happen but if it does must turn on the
+// associated constraint for the worst violation, then -> continue loop
+// - Find worst (most negative) offender:
+// contact offense = fc < 0 ? fc : 0
+// stiction offense = mu_s*max(0, fc) - |fs|
+// - Choose constraint to deactivate:
+// worst is a stiction constraint: choose it
+// worst is a contact constraint: if it has stiction, chose that
+// otherwise choose the contact constraint
+// - Inactivate chosen constraint
+// (if stiction, record impending slip direction & N for stick->slide)
+// end loop
+//
+void MyUnilateralConstraintSet::
+findActiveCandidates(State& s, const MyElementSubset& candidates) const
+{
+ const int ReviseNormalNIters = 6;
+ showConstraintStatus(s, "ENTER findActiveCandidates()");
+ if (candidates.m_contact.empty()) {
+ // Can't be any friction either, if there are no contacts.
+ SimTK_DEBUG("EXIT findActiveCandidates: no candidates.\n");
+ m_mbs.realize(s, Stage::Acceleration);
+ return;
+ }
+
+ SimTK_DEBUG3(
+ "findActiveCandidates() for %d/%d/%d contact/stick/slip candidates ...\n",
+ candidates.m_contact.size(), candidates.m_friction.size(),
+ candidates.m_sliding.size());
+
+ // Enable all candidate contact and stiction constraints if any are
+ // currently disabled.
+ enableConstraintSubset(candidates, true, s);
+
+ int pass=0, nContactsDisabled=0, nStictionDisabled=0, nContactsReenabled=0;
+ while (true) {
+ ++pass;
+ SimTK_DEBUG1("\nfindActiveCandidates(): pass %d\n", pass);
+
+ // Given an active set, evaluate multipliers and accelerations, and
+ // converge sliding forces.
+ m_mbs.realize(s, Stage::Acceleration);
+ SimTK_DEBUG("findActiveCandidates(): CONVERGE NORMALS\n");
+ convergeNormalForcesForSlidingContacts(s, SqrtEps);
+ /* for (int i=0; i < ReviseNormalNIters; ++i) {
+ s.autoUpdateDiscreteVariables();
+ s.invalidateAllCacheAtOrAbove(Stage::Dynamics);
+ convergeNormalForcesForSlidingContacts(s, SqrtEps);
+ m_mbs.realize(s, Stage::Acceleration);
+ }*/
+ m_mbs.realize(s, Stage::Acceleration);
+
+ // Scan all candidate contacts to find the active one that has the
+ // most negative normal force, and the inactive one that has the
+ // most negative acceleration error (hopefully none will).
+
+ int worstActiveContact=-1; Real mostNegativeContactForce=0;
+ int worstInactiveContact=-1; Real mostNegativeAerr=0;
+
+ SimTK_DEBUG("analyzing contact constraints ...\n");
+ for (unsigned i=0; i < candidates.m_contact.size(); ++i) {
+ const int which = candidates.m_contact[i];
+ SimTK_DEBUG1(" %d: ", which);
+ const MyContactElement& cont = getContactElement(which);
+ if (cont.isDisabled(s)) {
+ const Real aerr = cont.getAerr(s);
+ SimTK_DEBUG1("off aerr=%g\n", aerr);
+ if (aerr < mostNegativeAerr) {
+ worstInactiveContact = which;
+ mostNegativeAerr = aerr;
+ }
+ } else {
+ const Real f = cont.getForce(s);
+ SimTK_DEBUG1("on f=%g\n", f);
+ if (f < mostNegativeContactForce) {
+ worstActiveContact = which;
+ mostNegativeContactForce = f;
+ }
+ }
+ }
+
+ // This is bad and might cause cycling.
+ if (mostNegativeAerr < 0) {
+ SimTK_DEBUG2(" !!! Inactive contact %d violated, aerr=%g\n",
+ worstInactiveContact, mostNegativeAerr);
+ const MyContactElement& cont = getContactElement(worstInactiveContact);
+ //TODO -- must use a tolerance or prevent looping
+ //++nContactsReenabled;
+ //cont.enable(s);
+ //continue;
+ }
+
+ SimTK_DEBUG(" All inactive contacts are satisfied.\n");
+
+ #ifndef NDEBUG
+ if (mostNegativeContactForce == 0)
+ printf(" All active contacts are satisfied.\n");
+ else
+ printf(" Active contact %d was worst violator with f=%g\n",
+ worstActiveContact, mostNegativeContactForce);
+ #endif
+
+ int worstActiveStiction=-1; Real mostNegativeStictionCapacity=0;
+ SimTK_DEBUG("analyzing stiction constraints ...\n");
+ for (unsigned i=0; i < candidates.m_friction.size(); ++i) {
+ const int which = candidates.m_friction[i];
+ SimTK_DEBUG1(" %d: ", which);
+ const MyFrictionElement& fric = getFrictionElement(which);
+ if (!fric.isSticking(s)) {
+ SimTK_DEBUG("off\n");
+ continue;
+ }
+ const Real mu_s = fric.getStaticFrictionCoef();
+ const Real N = fric.getMasterNormalForce(s); // might be negative
+ const Real fsmag = fric.getActualFrictionForce(s);
+ const Real capacity = mu_s*std::max(N,Real(0)) - fsmag;
+ SimTK_DEBUG2("on capacity=%g (N=%g)\n", capacity, N);
+
+ if (capacity < mostNegativeStictionCapacity) {
+ worstActiveStiction = which;
+ mostNegativeStictionCapacity = capacity;
+ }
+ }
+
+ #ifndef NDEBUG
+ if (mostNegativeStictionCapacity == 0)
+ printf(" All active stiction constraints are satisfied.\n");
+ else
+ printf(" Active stiction %d was worst violator with capacity=%g\n",
+ worstActiveStiction, mostNegativeStictionCapacity);
+ #endif
+
+ if (mostNegativeContactForce==0 && mostNegativeStictionCapacity==0) {
+ SimTK_DEBUG("DONE. Current active set is a winner.\n");
+ break;
+ }
+
+ if (mostNegativeStictionCapacity <= mostNegativeContactForce) {
+ SimTK_DEBUG1(" Disable stiction %d\n", worstActiveStiction);
+ MyFrictionElement& fric = updFrictionElement(worstActiveStiction);
+ fric.recordImpendingSlipInfo(s);
+ ++nStictionDisabled;
+ fric.disableStiction(s);
+ continue;
+ }
+
+ // A contact constraint was the worst violator. If that contact
+ // constraint has an active stiction constraint, we have to disable
+ // the stiction constraint first.
+ SimTK_DEBUG1(" Contact %d was the worst violator.\n", worstActiveContact);
+ const MyContactElement& cont = getContactElement(worstActiveContact);
+ assert(!cont.isDisabled(s));
+
+ if (cont.hasFrictionElement()) {
+ MyFrictionElement& fric = cont.updFrictionElement();
+ if (fric.isSticking(s)) {
+ SimTK_DEBUG1(" ... but must disable stiction %d first.\n",
+ fric.getFrictionIndex());
+ fric.recordImpendingSlipInfo(s);
+ ++nStictionDisabled;
+ fric.disableStiction(s);
+ continue;
+ }
+ }
+
+ SimTK_DEBUG1(" Disable contact %d\n", worstActiveContact);
+ ++nContactsDisabled;
+ cont.disable(s);
+ // Go back for another pass.
+ }
+
+ // Reset all the slip directions so that all slip->stick event witnesses
+ // will be positive when integration resumes.
+ for (unsigned i=0; i < candidates.m_sliding.size(); ++i) {
+ const int which = candidates.m_sliding[i];
+ const MyFrictionElement& fric = getFrictionElement(which);
+ if (!fric.isMasterActive(s)) continue;
+ fric.updatePreviousSlipDirFromRecorded(s);
+ }
+
+ // Always leave at acceleration stage.
+ m_mbs.realize(s, Stage::Acceleration);
+
+ SimTK_DEBUG3("... Done; %d contacts, %d stictions broken; %d re-enabled.\n",
+ nContactsDisabled, nStictionDisabled, nContactsReenabled);
+
+ showConstraintStatus(s, "EXIT findActiveCandidates()");
+}
+
+//---------------- CONVERGE NORMAL FORCES FOR SLIDING CONTACTS -----------------
+namespace {
+class NormalErrors : public Differentiator::JacobianFunction {
+public:
+ NormalErrors(const MyUnilateralConstraintSet& unis,
+ const Array_<int>& sliders,
+ State& state)
+ : Differentiator::JacobianFunction(sliders.size(), sliders.size()),
+ m_unis(unis), m_state(state), m_sliders(sliders) { }
+
+ void getEstimates(const State& s, Vector& estN) const {
+ estN.resize(m_sliders.size());
+ for (int i=0; i < (int)m_sliders.size(); ++i) {
+ const MyFrictionElement& friction =
+ m_unis.getFrictionElement(m_sliders[i]);
+ estN[i] = friction.getEstimatedNormalForce(s);
+ }
+ }
+
+ // Alternate signature for convenience (extra copy).
+ Vector getEstimates(const State& s) {
+ Vector estN(m_sliders.size());
+ getEstimates(s,estN);
+ return estN;
+ }
+
+ void setEstimates(State& s, const Vector& estN) const {
+ for (int i=0; i < (int)m_sliders.size(); ++i) {
+ const MyFrictionElement& friction =
+ m_unis.getFrictionElement(m_sliders[i]);
+ friction.setEstimatedNormalForce(m_state, estN[i]);
+ }
+ }
+
+ void calcErrors(const State& s, Vector& errs) const {
+ errs.resize(m_sliders.size());
+ m_unis.getMultibodySystem().realize(s, Stage::Acceleration);
+ for (int i=0; i < (int)m_sliders.size(); ++i) {
+ const MyFrictionElement& friction =
+ m_unis.getFrictionElement(m_sliders[i]);
+ const Real estN = friction.getEstimatedNormalForce(s);
+ const Real N = friction.getMasterNormalForce(s);
+ errs[i] = estN - N;
+ }
+ }
+
+ // Alternate signature for convenience (extra copy).
+ Vector calcErrors(const State& s) const {
+ Vector errs(m_sliders.size()); calcErrors(s, errs);
+ return errs;
+ }
+
+ // Must provide this pure virtual function.
+ int f(const Vector& y, Vector& fy) const OVERRIDE_11 {
+ assert(y.size() == m_sliders.size() && fy.size() == m_sliders.size());
+ Vector prevEst(m_sliders.size());
+ getEstimates(m_state, prevEst);
+ setEstimates(m_state, y);
+ calcErrors(m_state, fy);
+ setEstimates(m_state, prevEst); // restore
+ return 0;
+ }
+private:
+ const MyUnilateralConstraintSet& m_unis;
+ const Array_<int>& m_sliders;
+ State& m_state;
+};
+}
+
+
+void MyUnilateralConstraintSet::
+convergeNormalForcesForSlidingContacts(State& s, Real rtol) const {
+ Array_<int> sliders;
+ for (int i=0; i < getNumFrictionElements(); ++i) {
+ const MyFrictionElement& friction = getFrictionElement(i);
+ if (friction.isMasterActive(s) && !friction.isSticking(s))
+ sliders.push_back(i);
+ }
+ SimTK_DEBUG1("convergeNormalForces: %d sliding contacts\n", sliders.size());
+ if (sliders.empty()) return;
+
+ NormalErrors normErrs(*this,sliders,s);
+ Differentiator dnormErrs(normErrs);
+
+ Vector estN(sliders.size()), err(sliders.size()), destN(sliders.size()),
+ newEstN(sliders.size());
+ normErrs.getEstimates(s, estN);
+ normErrs.calcErrors(s, err);
+
+ Real prevNorm = err.normInf();
+
+ #ifndef NDEBUG
+ cout << "====> Initial estN=" << estN << "-> err=" << err
+ << " norm=" << prevNorm << endl;
+ #endif
+
+ if (prevNorm <= rtol)
+ return;
+
+ #ifndef NDEBUG
+ cout << "\nNeed to converge sliding force and normal:\n";
+ #endif
+
+ Matrix J(sliders.size(), sliders.size());
+ bool improved;
+ const int MaxIters = 7;
+ int iter = 0;
+ do {
+ ++iter;
+ improved = false;
+ dnormErrs.calcJacobian(estN, err, J, Differentiator::CentralDifference);
+ FactorQTZ Jinv(J);
+ #ifndef NDEBUG
+ cout << "====> iter " << iter << " rank " << Jinv.getRank()
+ << " Jacobian\n" << J << endl;
+ #endif
+ Jinv.solve(err, destN);
+ Real fac = 2, norm;
+ Real worseningAllowance = iter == 1 ? 1.2 : 1.01;
+ do {
+ fac /= 2;
+ newEstN = estN - fac*destN;
+ normErrs.setEstimates(s, newEstN);
+ normErrs.calcErrors(s, err);
+ norm = err.normInf();
+ #ifndef NDEBUG
+ printf("fac=%g new norm=%g", fac, norm);
+ cout << " est=" << newEstN << endl;
+ #endif
+ } while (norm > worseningAllowance*prevNorm);
+ if (iter==1 || norm < prevNorm) {
+ prevNorm = norm;
+ estN = newEstN;
+ improved = true;
+ }
+ #ifndef NDEBUG
+ cout << "====> revised estN=" << estN << "-> err=" << err
+ << " norm=" << prevNorm << endl;
+ #endif
+ } while (iter < MaxIters && improved && err.normInf() > rtol);
+}
+
+
+//-------------------------- SHOW CONSTRAINT STATUS ----------------------------
+void MyUnilateralConstraintSet::
+showConstraintStatus(const State& s, const String& place) const
+{
+#ifndef NDEBUG
+ printf("\n%s: uni status @t=%.15g\n", place.c_str(), s.getTime());
+ m_mbs.realize(s, Stage::Acceleration);
+ for (int i=0; i < getNumContactElements(); ++i) {
+ const MyContactElement& contact = getContactElement(i);
+ const bool isActive = !contact.isDisabled(s);
+ printf(" %6s %2d %s, h=%g dh=%g f=%g\n",
+ isActive?"ACTIVE":"off", i, contact.getContactType().c_str(),
+ contact.getPerr(s),contact.getVerr(s),
+ isActive?contact.getForce(s):Zero);
+ }
+ for (int i=0; i < getNumFrictionElements(); ++i) {
+ const MyFrictionElement& friction = getFrictionElement(i);
+ if (!friction.isMasterActive(s))
+ continue;
+ const bool isSticking = friction.isSticking(s);
+ printf(" %8s friction %2d, |v|=%g witness=%g\n",
+ isSticking?"STICKING":"sliding", i,
+ friction.getActualSlipSpeed(s),
+ isSticking?friction.calcStictionForceWitness(s)
+ :friction.calcSlipSpeedWitness(s));
+ friction.writeFrictionInfo(s, " ", std::cout);
+ }
+ printf("\n");
+#endif
+}
+
+//==============================================================================
+// MY SLIDING FRICTION FORCE -- Implementation
+//==============================================================================
+// This is used for friction when slipping or when slip is impending, so it
+// will generate a force even if there is no slip velocity. Whenever the slip
+// speed |v|<=vtol, or if it has reversed direction, we consider it unreliable
+// and leave the applied force direction unchanged until the next transition
+// event. At that point activating the stiction constraint will be attempted.
+// If the stiction condition is violated, a new impending slip direction is
+// recorded opposing the direction of the resulting constraint force.
+//
+// The friction force is a 2-vector F calculated at Dynamics stage, applied
+// equal and opposite to the two contacting bodies at their mutual contact
+// point:
+// F = -mu*N_est*d_eff
+// d_eff is the effective slip direction that is to be opposed by the force.
+//
+// This is composed of several functions:
+// shouldUpdate(v) = ~v*d_prev > 0 && |v|>vtol
+// d_eff(v) = shouldUpdate(v) ? v/|v| : d_prev
+// v_eff(v) = ~v*d_eff
+// mu(v) = mu_d + mu_v * max(v_eff,0)
+// Fmag(v; N) = mu(v)*N
+// F(v; N) = -Fmag(v,N)*d_eff(v)
+// N_est = N_prev
+//
+// mu_d ... the dynamic coefficient of friction (a scalar constant >= 0)
+// mu_v ... viscous coefficient of friction (>= 0)
+// N_prev... previous normal force magnitude (a discrete state variable)
+// d_prev... previous or impending slip direction (a discrete state variable)
+// d_eff ... the effective slip direction, a unit length 2-vector
+// v_eff ... slip speed in d_eff direction, for viscous friction
+//
+// There is a sliding-to-stiction event witness function
+// e1(v)=dot(v, d_prev) velocity reversal
+//
+// TODO: other possible witness functions:
+// e2(v)=|v|-vtol slowdown (would often be missed)
+// e(v) = dot(v, d_prev) - vtol [signed]
+//
+// N_prev is an auto-update variable whose update value is set at Acceleration
+// stage from the actual normal force magnitude N of this friction element's
+// master contact element. N_prev is also set manually whenever sliding is
+// enabled, to the current normal force. In general we have Ni=Ni(F) (where
+// F={Fi} is a vector of all nf active sliding friction forces), and
+// Fi=Fi(Ni_est), so the error in the magnitude of the i'th applied friction
+// force is Fi_err=mu_i(v_i)*(Ni_est-Ni). If this is too large we have to
+// iterate until Ni_est is close enough to Ni for all i (this has to be done
+// simultaneously for the system as a whole).
+//
+// d_prev is an auto-update variable whose update value is set at Velocity
+// stage, if shouldUpdate(v), otherwise it remains unchanged. It is also set
+// manually when transitioning from sticking to sliding, to -F/|F| where F was
+// the last stiction force vector.
+//
+class MySlidingFrictionForceImpl : public Force::Custom::Implementation {
+public:
+ MySlidingFrictionForceImpl(const GeneralForceSubsystem& forces,
+ const MyPointContactFriction& ptFriction,
+ Real vtol)
+ : m_forces(forces), m_friction(ptFriction),
+ m_contact(ptFriction.getMyPointContact()), m_vtol(vtol)
+ {}
+
+ bool hasSlidingForce(const State& s) const
+ { return m_friction.isMasterActive(s) && !m_friction.isSticking(s); }
+
+
+ // Calculate d_eff, the direction to be opposed by the sliding force.
+ Vec2 getEffectiveSlipDir(const State& s) const {
+ const Vec2 Vslip = m_contact.getSlipVelocity(s);
+ const Vec2 prevVslipDir = getPrevSlipDir(s);
+ if (shouldUpdate(Vslip, prevVslipDir, m_vtol)) { // TODO: tol?
+ const Real v = Vslip.norm();
+ return Vslip/v;
+ }
+ return prevVslipDir;
+ }
+
+ // This is useful for reporting.
+ Real calcSlidingForceMagnitude(const State& state) const {
+ if (!hasSlidingForce(state)) return 0;
+ const Real slipV = m_contact.getSlipVelocity(state).norm();
+ return calcSlidingForceMagnitude(state, slipV);
+ }
+
+ // This is the force that will be applied next.
+ Vec2 calcSlidingForce(const State& state) const {
+ if (!hasSlidingForce(state))
+ return Vec2(0);
+
+ Vec2 dEff = getEffectiveSlipDir(state);
+ if (dEff.isNaN()) {
+ SimTK_DEBUG("NO SLIDING DIRECTION AVAILABLE\n");
+ return Vec2(0);
+ }
+
+ const Vec2 Vslip = m_contact.getSlipVelocity(state);
+ const Real vEff = ~Vslip*dEff;
+
+ const Real FMag = calcSlidingForceMagnitude(state, std::max(vEff,Zero));
+ assert(!isNaN(FMag));
+
+ const Vec2 F = -FMag*dEff;
+ return F;
+ }
+
+ // Return the related contact constraint's normal force value and slip
+ // velocity as recorded at the end of the last time step. Will be zero if
+ // the contact was not active then.
+ Real getPrevN(const State& state) const {
+ const Real& prevN = Value<Real>::downcast
+ (m_forces.getDiscreteVariable(state, m_prevNix));
+ return prevN;
+ }
+ void setPrevN(State& state, Real N) const {
+ Real& prevN = Value<Real>::updDowncast
+ (m_forces.updDiscreteVariable(state, m_prevNix));
+ if (isNaN(N))
+ printf("*** setPrevN(): N is NaN\n");
+ //SimTK_DEBUG3("STATE CHG %d: N from %g->%g\n",
+ // m_friction.getFrictionIndex(), prevN, N);
+ prevN = N;
+
+ }
+ Vec2 getPrevSlipDir(const State& state) const {
+ const Vec2& prevSlipDir = Value<Vec2>::downcast
+ (m_forces.getDiscreteVariable(state, m_prevSlipDirIx));
+ return prevSlipDir;
+ }
+ void setPrevSlipDir(State& state, const Vec2& slipDir) const {
+ Vec2& prevSlipDir = Value<Vec2>::updDowncast
+ (m_forces.updDiscreteVariable(state, m_prevSlipDirIx));
+ prevSlipDir = slipDir;
+ SimTK_DEBUG3("STATE CHG %d: prevDir to %g %g\n",
+ m_friction.getFrictionIndex(), slipDir[0], slipDir[1]);
+ }
+ //--------------------------------------------------------------------------
+ // Custom force virtuals
+ // Apply the sliding friction force if this is enabled.
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+ OVERRIDE_11
+ {
+ if (!hasSlidingForce(state))
+ return; // nothing to do
+
+ const MobilizedBody& bodyB = m_contact.getBody();
+ const MobilizedBody& bodyP = m_contact.getPlaneBody();
+ const Vec3& stationB = m_contact.getBodyStation();
+ const Vec3 stationP = bodyB.findStationLocationInAnotherBody
+ (state, stationB, bodyP);
+ const Vec2 fSlip = calcSlidingForce(state);
+ const Vec3 forceG(fSlip[0], 0, fSlip[1]); // only X,Z components
+ bodyB.applyForceToBodyPoint(state, stationB, forceG, bodyForces);
+ bodyP.applyForceToBodyPoint(state, stationP, -forceG, bodyForces);
+ }
+
+ // Sliding friction does not store energy.
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11 {return 0;}
+
+ // Allocate state variables for storing the previous normal force and
+ // sliding direction.
+ void realizeTopology(State& state) const OVERRIDE_11 {
+ // The previous normal force N is used as a first estimate for the
+ // mu*N sliding friction force calculated at Dynamics stage. However,
+ // the update value N cannot be determined until Acceleration stage.
+ m_prevNix = m_forces.allocateAutoUpdateDiscreteVariable
+ (state, Stage::Dynamics, new Value<Real>(0), Stage::Acceleration);
+ // The previous sliding direction is used in an event witness that
+ // is evaluated at Velocity stage.
+ m_prevSlipDirIx = m_forces.allocateAutoUpdateDiscreteVariable
+ (state, Stage::Velocity, new Value<Vec2>(Vec2(NaN)),
+ Stage::Velocity);
+ }
+
+ // If we're sliding, set the update value for the previous slip direction
+ // if the current slip velocity is usable.
+ void realizeVelocity(const State& state) const OVERRIDE_11 {
+ if (!hasSlidingForce(state))
+ return; // nothing to do
+ const Vec2 Vslip = m_contact.getSlipVelocity(state);
+ const Vec2 prevVslipDir = getPrevSlipDir(state);
+
+ if (shouldUpdate(Vslip, prevVslipDir, m_vtol)) {
+ Vec2& prevSlipUpdate = Value<Vec2>::updDowncast
+ (m_forces.updDiscreteVarUpdateValue(state, m_prevSlipDirIx));
+ const Real v = Vslip.norm();
+ const Vec2 slipDir = Vslip / v;
+ prevSlipUpdate = slipDir;
+ m_forces.markDiscreteVarUpdateValueRealized(state, m_prevSlipDirIx);
+
+ #ifndef NDEBUG
+ //printf("UPDATE %d: prevSlipDir=%g %g; now=%g %g; |v|=%g dot=%g vdot=%g\n",
+ // m_friction.getFrictionIndex(),
+ // prevVslipDir[0],prevVslipDir[1],slipDir[0],slipDir[1],
+ // v, ~slipDir*prevVslipDir, ~Vslip*prevVslipDir);
+ #endif
+ } else {
+ #ifndef NDEBUG
+ printf("NO UPDATE %d: prevSlipDir=%g %g; Vnow=%g %g; |v|=%g vdot=%g\n",
+ m_friction.getFrictionIndex(),
+ prevVslipDir[0],prevVslipDir[1],Vslip[0],Vslip[1],
+ Vslip.norm(), ~Vslip*prevVslipDir);
+ #endif
+ }
+ }
+
+ // Regardless of whether we're sticking or sliding, as long as the master
+ // contact is active use its normal force scalar as the update for our
+ // saved normal force.
+ void realizeAcceleration(const State& state) const OVERRIDE_11 {
+ if (!m_friction.isMasterActive(state))
+ return; // nothing to save
+ const Real N = m_contact.getForce(state); // normal force
+ const Real prevN = getPrevN(state);
+ if (N==prevN) return; // no need for an update
+
+ Real& prevNupdate = Value<Real>::updDowncast
+ (m_forces.updDiscreteVarUpdateValue(state, m_prevNix));
+
+ #ifndef NDEBUG
+ //printf("realize(Acc) CACHE UPDATE %d: N changing from %g -> %g (%.3g)\n",
+ // m_friction.getFrictionIndex(),
+ // prevN, N, std::abs(N-prevN)/std::max(N,prevN));
+ #endif
+ prevNupdate = N;
+ m_forces.markDiscreteVarUpdateValueRealized(state, m_prevNix);
+ }
+
+ //--------------------------------------------------------------------------
+
+private:
+ // Given the norm of the slip velocity already calculated, determine the
+ // magnitude of the slip force. If there is no viscous friction you can
+ // pass a zero vEff since it won't otherwise affect the force.
+ // Don't call this unless you know there may be a sliding force.
+ Real calcSlidingForceMagnitude(const State& state, Real vEff) const {
+ assert(vEff >= 0);
+ const Real prevN = getPrevN(state);
+ if (prevN <= 0) return 0; // no normal force -> no friction force
+
+ const Real mu_d = m_friction.getDynamicFrictionCoef();
+ const Real mu_v = m_friction.getViscousFrictionCoef();
+ const Real fMag = (mu_d + mu_v*vEff)*prevN;
+ return fMag;
+ }
+
+ // Determine whether the current slip velocity is reliable enough that
+ // we should use it to replace the previous slip velocity.
+ static bool shouldUpdate(const Vec2& Vslip, const Vec2& prevVslipDir,
+ Real velTol) {
+ const Real v2 = Vslip.normSqr();
+ if (prevVslipDir.isNaN())
+ return v2 > 0; // we'll take anything
+
+ // Check for reversal.
+ bool reversed = (~Vslip*prevVslipDir < 0);
+ return !reversed && (v2 > square(velTol));
+ }
+
+ const GeneralForceSubsystem& m_forces;
+ const MyPointContactFriction& m_friction;
+ const MyPointContact& m_contact;
+ const Real m_vtol;
+
+ mutable DiscreteVariableIndex m_prevNix; // previous normal force
+ mutable DiscreteVariableIndex m_prevSlipDirIx; // previous slip direction
+};
+
+// This is the force handle's constructor; it just creates the force
+// implementation object.
+MySlidingFrictionForce::MySlidingFrictionForce
+ (GeneralForceSubsystem& forces,
+ const MyPointContactFriction& friction,
+ Real vtol)
+: Force::Custom(forces, new MySlidingFrictionForceImpl(forces,friction,vtol))
+{}
+
+Real MySlidingFrictionForce::getPrevN(const State& state) const
+{ return getImpl().getPrevN(state); }
+void MySlidingFrictionForce::setPrevN(State& state, Real N) const
+{ getImpl().setPrevN(state,N); }
+Vec2 MySlidingFrictionForce::getPrevSlipDir(const State& state) const
+{ return getImpl().getPrevSlipDir(state); }
+bool MySlidingFrictionForce::hasPrevSlipDir(const State& state) const
+{ return !getImpl().getPrevSlipDir(state).isNaN(); }
+void MySlidingFrictionForce::
+setPrevSlipDir(State& state, const Vec2& slipDir) const
+{ getImpl().setPrevSlipDir(state, slipDir); }
+Real MySlidingFrictionForce::calcSlidingForceMagnitude(const State& state) const
+{ return getImpl().calcSlidingForceMagnitude(state); }
+Vec2 MySlidingFrictionForce::calcSlidingForce(const State& state) const
+{ return getImpl().calcSlidingForce(state); }
+
+
+const MySlidingFrictionForceImpl& MySlidingFrictionForce::
+getImpl() const {
+ return dynamic_cast<const MySlidingFrictionForceImpl&>(getImplementation());
+}
+
+
+// --------------------------------
+// perr = ~p_BS*n - h
+// --------------------------------
+void UniPointInPlaneImpl::calcPositionErrors
+ (const State& s, // Stage::Time
+ const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQ,
+ Array_<Real>& perr) // mp of these
+ const
+{
+ assert(allX_AB.size()==2 && constrainedQ.size()==0 && perr.size() == 1);
+
+ const Vec3 p_AS = findStationLocation(allX_AB, followerBody,
+ defaultFollowerPoint);
+ const Transform& X_AB = getBodyTransform(allX_AB, planeBody);
+ const Vec3 p_BC = ~X_AB * p_AS; // shift to B origin, reexpress in B;
+ // C is material pt of B coincident with S
+
+ // We'll calculate this scalar using B-frame vectors, but any frame would
+ // have done.
+ perr[0] = dot(p_BC, defaultPlaneNormal) - defaultPlaneHeight;
+}
+
+// --------------------------------
+// verr = ~v_CS_A*n
+// --------------------------------
+void UniPointInPlaneImpl::calcPositionDotErrors
+ (const State& s, // Stage::Position
+ const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDot,
+ Array_<Real>& pverr) // mp of these
+ const
+{
+ assert(V_AB.size()==2 && constrainedQDot.size()==0 && pverr.size() == 1);
+ //TODO: should be able to get p info from State
+
+ const Vec3 p_AS = findStationLocationFromState(s, followerBody,
+ defaultFollowerPoint);
+ const Transform& X_AB = getBodyTransformFromState(s, planeBody);
+ const Vec3 p_BC = ~X_AB * p_AS; // shift to B origin, reexpress in B;
+ // C is material point of B coincident with S
+ const UnitVec3 n_A = X_AB.R() * defaultPlaneNormal;
+
+ const Vec3 v_AS = findStationVelocity(s, V_AB, followerBody,
+ defaultFollowerPoint);
+ const Vec3 v_AC = findStationVelocity(s, V_AB, planeBody, p_BC);
+
+ // Calculate this scalar using A-frame vectors.
+ pverr[0] = dot( v_AS-v_AC, n_A );
+}
+
+// -------------------------------------
+// aerr = ~(a_CS_A - 2 w_AB X v_CS_A) * n
+// -------------------------------------
+void UniPointInPlaneImpl::calcPositionDotDotErrors
+ (const State& s, // Stage::Velocity
+ const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB,
+ const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
+ Array_<Real>& paerr) // mp of these
+ const
+{
+ assert(A_AB.size()==2 && constrainedQDotDot.size()==0 && paerr.size() == 1);
+ //TODO: should be able to get p and v info from State
+ const Vec3 p_AS = findStationLocationFromState(s, followerBody,
+ defaultFollowerPoint);
+ const Transform& X_AB = getBodyTransformFromState(s, planeBody);
+ const Vec3 p_BC = ~X_AB * p_AS; // shift to B origin, reexpress in B;
+ // C is material point of B coincident with S
+ const UnitVec3 n_A = X_AB.R() * defaultPlaneNormal;
+
+ const Vec3& w_AB = getBodyAngularVelocityFromState(s, planeBody);
+ const Vec3 v_AS = findStationVelocityFromState(s, followerBody,
+ defaultFollowerPoint);
+ const Vec3 v_AC = findStationVelocityFromState(s, planeBody, p_BC);
+
+ const Vec3 a_AS = findStationAcceleration(s, A_AB, followerBody,
+ defaultFollowerPoint);;
+ const Vec3 a_AC = findStationAcceleration(s, A_AB, planeBody, p_BC);
+
+ paerr[0] = dot( (a_AS-a_AC) - 2.*w_AB % (v_AS-v_AC), n_A );
+}
+
+
diff --git a/Simbody/tests/adhoc/TimsBoxBristle.cpp b/Simbody/tests/adhoc/TimsBoxBristle.cpp
new file mode 100644
index 0000000..64cf80d
--- /dev/null
+++ b/Simbody/tests/adhoc/TimsBoxBristle.cpp
@@ -0,0 +1,1189 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) - Tim's Box (hybrid contact model) *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* Investigate a compliant contact/bristle friction model, using Tim's
+box as a test case. */
+
+//#define NDEBUG 1
+
+#include "Simbody.h"
+
+#include <string>
+#include <iostream>
+#include <exception>
+
+using std::cout;
+using std::endl;
+
+using namespace SimTK;
+
+//#define USE_TIMS_PARAMS
+#define ANIMATE // off to get more accurate CPU time (you can still playback)
+
+// Control whether we use a dwell-time model. Otherwise we allow instantaneous
+// sliding/stiction transition.
+#define ENABLE_DWELL_TIME
+
+// Define this to run the simulation NTries times, saving the states and
+// comparing them bitwise to see if the simulations are perfectly repeatable
+// as they should be. You should see nothing but exact zeroes print out for
+// second and subsequent runs.
+//#define TEST_REPEATABILITY
+static const int NTries=3;
+
+//==============================================================================
+// MY HYBRID VERTEX CONTACT ELEMENT
+//==============================================================================
+/* Given a contact material (with compliant material properties), this
+represents a compliant point-and-rigid-halfspace contact element. We track
+the height h of a vertex V on body B over a halfspace H on body P. The
+halfspace frame H serves as the contact frame, with Hz in the contact
+normal direction and Hxy spanning the tangent plane. The pose X_PH is
+a given constant.
+
+Normal force
+------------
+If the vertex height h is above the halfspace surface (h>=0), no forces are
+generated. If below the surface (h<0) we generate a normal force of magnitude
+ N=max(0, -k*h(1-d*hdot)), with N >= 0
+applied to both bodies at the contact point C, which is the point at h=0 just
+up the halfspace normal direction from the vertex (because we're considering
+the halfspace to be rigid). Denote by Cb the station (material point) of B
+that is coincident with C, and Cp the station of P that is coincident with C.
+
+Bristle friction force
+----------------------
+Ref: Gonthier, et al. 2004 Multibody Sys. Dyn. 11:209-33.
+
+k_br bristle stiffness (= alpha0 from Gonthier)
+t_br bristle time constant (= alpha1/alpha0 from Gonthier)
+t_dw dwell time constant
+v_trans Stribeck transition velocity
+v_tol numerical velocity noise limit (< v_trans/10)
+
+f tangential friction force vector (2d)
+
+z bristle deformation vector (2d state)
+v tangential slip velocity vector (2d) [v=0 when not in contact]
+r rolling condition [0,1] (0=slipping, 1=rolling) (=s from Gonthier)
+r_dw r delayed by time constants t_dw,t_br (state) [0,1]
+
+f = { sat(f_br,f_max) - N*mu_v*v, if in contact
+ { 0, otherwise
+
+f_br = -k_br*(z + t_br*zdot) [TODO: need area term here?]
+zdot = r * zdot_s + (1-r) * zdot_d
+zdot_s = v
+zdot_d = -(z + f_d/k_br)/t_br
+f_d = -mu_d*N*dir(v, v_tol)
+
+f_max = mu_eff*N
+mu_eff = mu_d + (mu_s-mu_d)*r_dw
+
+r = exp(-|v|^2/v_trans^2)
+
+r_dw_dot ={ (r - r_dw)/t_dw, r >= r_dw
+ { (r - r_dw)/t_br, r < r_dw
+
+sat(f_br, f_max) = { f_br, |f_br| <= f_max
+ { f_max*f_br/|f_br|, |f_br| > f_max
+
+dir(v, v_tol) = { v/|v|, |v| >= v_tol
+ { v/v_tol, |v| < v_tol [Gonthier has *stepUp(|v|/v_tol)]
+
+*/
+
+// Return f, but with magnitude limited by fmax>=0.
+static inline Vec2 sat(const Vec2& f, Real fmax) {
+ assert(fmax >= 0);
+ const Real fsq = f.normSqr();
+ if (fsq <= fmax*fmax)
+ return f;
+
+ return (fmax/std::sqrt(fsq))*f;
+}
+
+// Implement relaxed unit vector in direction of v, where the direction vector
+// has magnitude less than 1 if |v|<vtol, dropping to 0 when |v|=0.
+static inline Vec2 dir(const Vec2& v, Real vnorm, Real oovtol) {
+ assert(oovtol > 0);
+ const Real scale = vnorm*oovtol;
+ if (scale >= 1)
+ return v/vnorm;
+ //const Real smooth = 1;
+ //const Real smooth = stepUp(scale);
+ const Real smooth = 1.5*scale-0.5*cube(scale);// Gonthier
+ return v*(smooth*oovtol);
+}
+
+// This is a velocity-stage cache entry.
+struct MyBristleContactInfo {
+ MyBristleContactInfo()
+ : h(NaN), Cp(NaN), Cb(NaN), v_HCb(NaN), vSlipMag(NaN), f_HCb(NaN)
+ {
+ }
+ // Position info.
+ Real h; // signed distance, h<0 means contact
+ Vec3 Cp; // station on P coincident with the contact point
+ Vec3 Cb; // station on B coincident with the contact point
+ Rotation R_GH;
+
+ // Velocity info. H frame has z along contact normal, x,y in tangent plane.
+ Vec3 v_HCb; // velocity of Cb in the contact frame H
+ Real vSlipMag; // |(v_HCb.x, v_HCb.y)|
+ Vec3 f_HCb; // contact force on Cb in contact frame H
+};
+
+class MyBristleVertexContactElementImpl : public Force::Custom::Implementation {
+public:
+ MyBristleVertexContactElementImpl(const GeneralForceSubsystem& forces,
+ MobilizedBody& hsmobod, const UnitVec3& hsn, Real hsh,
+ MobilizedBody& vmobod, const Vec3& vertex,
+ const ContactMaterial& material, Real vTrans)
+ : m_matter(hsmobod.getMatterSubsystem()), m_forces(forces),
+ m_hsmobodx(hsmobod), m_X_PH(Rotation(hsn, ZAxis), hsh*hsn),
+ m_vmobodx(vmobod), m_vertex_B(vertex),
+ m_material(material),
+ m_kBristle(NaN), m_tBristle(NaN), m_tDwell(NaN),
+ m_vtrans(vTrans), m_vtol(NaN), // assign later
+ m_index(-1)
+ {
+ m_kBristle = m_material.getStiffness()/10;
+ m_tBristle = .01; // 10 ms
+ m_tDwell = .1; // 100 ms
+ m_vtol = m_vtrans/10;
+ }
+
+ void setContactIndex(int index) {m_index=index;}
+
+ void initialize(State& s) {
+ updZ(s) = Vec2(0);
+ updRDwell(s) = 0;
+ setPrevSlipDir(s, Vec2(NaN));
+ }
+
+ void initializeForStiction(State& s) {
+ updZ(s) = Vec2(0);
+ updRDwell(s) = 1; // rolling
+ setPrevSlipDir(s, Vec2(NaN));
+ }
+
+ bool isInContact(const State& s) const
+ { return findH(s) < 0; }
+
+ bool isSticking(const State& s) const
+ {
+ return getActualSlipSpeed(s) <= m_vtrans;
+ }
+
+ // Return a point in Ground coincident with the vertex.
+ Vec3 whereToDisplay(const State& s) const {
+ return getBodyB().findStationLocationInGround(s, m_vertex_B);
+ }
+
+ Real getActualSlipSpeed(const State& s) const {
+ const MyBristleContactInfo& info = getContactInfo(s);
+ return info.vSlipMag;
+ }
+
+ // Return the normal force N >= 0 currently being generated by this
+ // contact. State must be realized to Stage::Velocity.
+ Real getNormalForce(const State& s) const {
+ const MyBristleContactInfo& info = getContactInfo(s);
+ if (info.h >= 0) return 0;
+ const Real N = info.f_HCb[2]; // z component is normal
+ return N;
+ }
+
+ Real getHeight(const State& s) const {
+ const MyBristleContactInfo& info = getContactInfo(s);
+ return info.h;
+ }
+
+ Real getHeightDot(const State& s) const {
+ const MyBristleContactInfo& info = getContactInfo(s);
+ return info.v_HCb[2]; // velocity in plane normal direction
+ }
+
+ // Return the sliding force 2-vector in the halfspace plane that is being
+ // applied to the contact point station on body B. If we are sticking
+ // then this force is zero; call getStickingForce() to get the real value.
+ const Vec2& getSlidingForce(const State& s) const {
+ const MyBristleContactInfo& info = getContactInfo(s);
+ const Vec2& f = info.f_HCb.getSubVec<2>(0);
+ return f;
+ }
+
+ // Return the sliding velocity 2-vector in the halfspace plane of the
+ // contact point station on body B relative to the contact point station
+ // on body P.
+ const Vec2& getSlipVelocity(const State& s) const {
+ const MyBristleContactInfo& info = getContactInfo(s);
+ const Vec2& v = info.v_HCb.getSubVec<2>(0);
+ return v;
+ }
+
+ // Return height of vertex over plane; negative if penetrated. You can
+ // call this at Stage::Position.
+ Real findH(const State& state) const {
+ const Vec3 V_P = findVInP(state); // location of V in P
+ const Real h = dot(V_P-m_X_PH.p(), m_X_PH.z());
+ return h;
+ }
+
+ // Returns height of vertex over plane same as findH(); also returns
+ // contact point (projection of V onto plane along Hz).
+ Real findContactPointInP(const State& state, Vec3& contactPointInP) const {
+ const Vec3 V_P = findVInP(state); // location of V in P
+ const Real h = dot(V_P-m_X_PH.p(), m_X_PH.z());
+ contactPointInP = V_P - h*m_X_PH.z();
+ return h;
+ }
+
+ // Calculate v_eff, the direction to be opposed by the sliding force.
+ Vec2 getEffectiveSlipDir(const State& s, const Vec2& vSlip, Real vSlipMag) const {
+ const Vec2 prevVslipDir = getPrevSlipDir(s);
+ if (shouldUpdate(vSlip, vSlipMag, prevVslipDir, m_vtol))
+ return vSlip/vSlipMag;
+ else return prevVslipDir;
+ }
+
+ const Vec2& getZ(const State& s) const
+ { return Vec2::getAs(&m_forces.getZ(s)[this->m_bristleDeformationIx]); }
+ Vec2& updZ(State& s) const
+ { return Vec2::updAs(&m_forces.updZ(s)[this->m_bristleDeformationIx]); }
+ Vec2& updZDot(const State& s) const
+ { return Vec2::updAs(&m_forces.updZDot(s)[m_bristleDeformationIx]); }
+
+ const Real& getRDwell(const State& s) const
+ { return m_forces.getZ(s)[this->m_dwellIx]; }
+ Real& updRDwell(State& s)
+ { return m_forces.updZ(s)[this->m_dwellIx]; }
+ Real& updRDwellDot(const State& s) const
+ { return m_forces.updZDot(s)[m_dwellIx]; }
+
+
+ // Return the slip velocity as recorded at the end of the last time step.
+ const Vec2& getPrevSlipDir(const State& state) const {
+ const Vec2& prevSlipDir = Value<Vec2>::downcast
+ (m_forces.getDiscreteVariable(state, m_prevSlipDirIx));
+ return prevSlipDir;
+ }
+ // Modify the discrete state variable directly.
+ void setPrevSlipDir(State& state, const Vec2& slipDir) const {
+ Vec2& prevSlipDir = Value<Vec2>::updDowncast
+ (m_forces.updDiscreteVariable(state, m_prevSlipDirIx));
+ prevSlipDir = slipDir;
+ SimTK_DEBUG3("STATE CHG %d: prevDir to %g %g\n",
+ m_index, slipDir[0], slipDir[1]);
+ }
+
+ // Get access to the auto-update cache variable that will update the
+ // previous slip direction state variable if the step is accepted.
+ Vec2& updPrevSlipDirAutoUpdateValue(const State& state) const {
+ Vec2& prevSlipUpdate = Value<Vec2>::updDowncast
+ (m_forces.updDiscreteVarUpdateValue(state, m_prevSlipDirIx));
+ return prevSlipUpdate;
+ }
+ // Mark the auto-update cache entry valid.
+ void markPrevSlipDirAutoUpdateValueValid(const State& state) const {
+ m_forces.markDiscreteVarUpdateValueRealized(state, m_prevSlipDirIx);
+ }
+
+
+ const MyBristleContactInfo& getContactInfo(const State& state) const {
+ const MyBristleContactInfo& info =
+ Value<MyBristleContactInfo>::downcast
+ (m_forces.getCacheEntry(state, m_contactInfoIx));
+ return info;
+ }
+
+ MyBristleContactInfo& updContactInfo(const State& state) const {
+ MyBristleContactInfo& info =
+ Value<MyBristleContactInfo>::updDowncast
+ (m_forces.updCacheEntry(state, m_contactInfoIx));
+ return info;
+ }
+ //--------------------------------------------------------------------------
+ // Custom force virtuals
+ // Apply the normal force, and the sliding friction force if it is enabled.
+ // This is called during realize(Dynamics).
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+ OVERRIDE_11
+ {
+ const MyBristleContactInfo& info = getContactInfo(state);
+ if (info.h >= 0)
+ return; // no contact
+
+ const Vec3 f_GCb = info.R_GH * info.f_HCb; // re-express in Ground
+ getBodyB().applyForceToBodyPoint(state, info.Cb, f_GCb, bodyForces);
+ getBodyP().applyForceToBodyPoint(state, info.Cp, -f_GCb, bodyForces);
+ }
+
+ // The normal force stores energy as 2/5 k h^(5/2) when h<0.
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11 {
+ const MyBristleContactInfo& info = getContactInfo(state);
+ if (info.h >= 0)
+ return 0; // no contact
+ const Real h52 = square(info.h)*std::sqrt(-info.h);
+ const Real k = m_material.getStiffness();
+ return 0.4*k*h52;
+ }
+
+ // Allocate state variable for storing the previous sliding direction.
+ void realizeTopology(State& state) const OVERRIDE_11 {
+ m_bristleDeformationIx = m_forces.allocateZ(state, Vector(2, Real(0)));
+ m_dwellIx = m_forces.allocateZ(state, Vector(1, Real(0)));
+ m_prevSlipDirIx = m_forces.allocateAutoUpdateDiscreteVariable
+ (state, Stage::Velocity, new Value<Vec2>(Vec2(NaN)),
+ Stage::Velocity);
+
+ m_contactInfoIx = m_forces.allocateCacheEntry
+ (state, Stage::Velocity, new Value<MyBristleContactInfo>());
+ }
+
+
+ // Calculate everything here and save in contact info cache entry where
+ // it can be retrieved for generating forces, reporting, etc.
+ void realizeVelocity(const State& state) const OVERRIDE_11 {
+ MyBristleContactInfo& info = updContactInfo(state);
+
+ // Forces generated only if h<0. Cp always be the projection of the
+ // vertex onto the halfspace surface.
+ info.h = findContactPointInP(state, info.Cp);
+
+ const Rotation& R_PH = m_X_PH.R();
+ const Rotation& R_GP = getBodyP().getBodyRotation(state);
+ info.R_GH = R_GP*R_PH;
+
+ // Station of B coincident with the contact point.
+ info.Cb = info.h < 0
+ ? findPointOfBCoincidentWithPointOfP(state, info.Cp)
+ : getVertexOnB();
+
+ // Velocity of B's contact station in P.
+ const Vec3 v_PCb = getBodyB().findStationVelocityInAnotherBody
+ (state, info.Cb, getBodyP());
+ info.v_HCb = ~R_PH*v_PCb; // re-express in H
+ const Real hdot = info.v_HCb[2]; // z component
+ const Vec2 vSlip_HC(info.v_HCb[0], info.v_HCb[1]);
+ info.vSlipMag = vSlip_HC.norm();
+
+ const Real k = m_material.getStiffness(),
+ c = m_material.getDissipation(),
+ mu_d = m_material.getDynamicFriction(),
+ mu_s = m_material.getStaticFriction(),
+ mu_v = m_material.getViscousFriction();
+
+ const Vec2& z = getZ(state);
+ const Real& rdw = getRDwell(state);
+ Vec2& zdot = updZDot(state); // to be computed
+ Real& rdwdot = updRDwellDot(state);
+
+ info.f_HCb = Vec3(0); // Assume no contact force.
+ Real N = 0;
+
+ if (info.h < 0) { // contact
+ const Real h32 = -info.h*sqrt(-info.h); // |h| ^ (3/2)
+ N = std::max(Real(0), k*h32*(1-c*hdot));
+ }
+
+ if (N==0) {
+ zdot = -z / m_tBristle; // decay fast
+ rdwdot = -rdw / m_tBristle;
+ return; // no contact force
+ }
+
+ // N is the Hz component of the force on Cb.
+
+ Vec2 fT_HCb(0); // This will be the (Hx,Hy) components of force on Cb.
+
+ // Make v unitless slip ratio.
+ const Real v = info.vSlipMag / m_vtrans;
+ Real r = std::exp(-square(v)); // 1==rolling, 0==slipping
+
+ const Vec2& dir_prev = getPrevSlipDir(state);
+ Vec2 dir_eff = dir(vSlip_HC, info.vSlipMag, 1/m_vtol);
+ if (~dir_eff*dir_prev < 0) {
+ //cout << "REVERSED from " << dir_prev << " to " << dir_eff << endl;
+ //dir_eff = dir_prev;
+ }
+ updPrevSlipDirAutoUpdateValue(state) = dir_eff;
+ markPrevSlipDirAutoUpdateValueValid(state);
+
+ // This is what the force would be if we were fully in sliding.
+ //const Vec2 f_d = -mu_d*N*dir(vSlip_HC, info.vSlipMag, 1/m_vtol);
+ const Vec2 f_d = -mu_d*N*dir_eff;
+ // This is the bristle deformation that would generate f_d
+ const Vec2 z_d = -f_d/m_kBristle;
+
+ const Vec2 zdot_s = vSlip_HC;
+ const Vec2 zdot_d = (z_d - z)/m_tBristle;
+
+ #ifdef ENABLE_DWELL_TIME
+ const Real r_dwell = clamp(0,rdw,1);
+ // Update dwell state derivative.
+ const Real tconst = r >= rdw ? m_tDwell : m_tBristle;
+ rdwdot = (r - rdw) / tconst;
+ #else
+ const Real r_dwell = r;
+ rdwdot = 0;
+ #endif
+
+ zdot = r*zdot_s + (1-r)*zdot_d; // Gonthier
+ //zdot = r_dwell*zdot_s + (1-r_dwell)*zdot_d; // is this better?
+ const Vec2 f_br = -m_kBristle*(z + m_tBristle*zdot);
+
+ const Real mu_eff = mu_d + (mu_s-mu_d)*r_dwell;
+ const Real f_max = mu_eff*N;
+ fT_HCb = sat(f_br, f_max) - N * mu_v*vSlip_HC;
+
+ info.f_HCb = Vec3(fT_HCb[0], fT_HCb[1], N); // force on Cb, in H
+ }
+
+ std::ostream& writeFrictionInfo(const State& s, const String& indent,
+ std::ostream& o) const
+ {
+ o << indent;
+ if (!isInContact(s)) o << "OFF";
+ else if (isSticking(s)) o << "STICK";
+ else o << "SLIP";
+
+ const Vec2 v = getSlipVelocity(s);
+ const Vec2 f = getSlidingForce(s);
+ o << " V=" << ~v << " |v|=" << v.norm() << endl;
+ o << indent << " F=" << ~f << " |f|=" << f.norm() << endl;
+ o << indent << " z=" << ~getZ(s) << " |z|=" << getZ(s).norm() << endl;
+ o << indent << " rdw=" << getRDwell(s)
+ << " prevDir=" << ~getPrevSlipDir(s) << endl;
+ return o;
+ }
+
+
+ void showContactForces(const State& s, Array_<DecorativeGeometry>& geometry)
+ const
+ {
+ const Real Scale = 0.01;
+ const Real NH = getNormalForce(s);
+
+ const bool isInStiction = getActualSlipSpeed(s) <= m_vtrans;
+ const Vec2 fH = getSlidingForce(s);
+
+ if (fH.normSqr() < square(SignificantReal) && NH < SignificantReal)
+ return;
+
+ const MyBristleContactInfo& info = getContactInfo(s);
+ const Vec3 fG = info.R_GH * Vec3(fH[0],fH[1],0); // friction
+ const Vec3 NG = info.R_GH * Vec3(0, 0, NH); // normal
+
+ const MobilizedBody& bodyB = getBodyB();
+ const Vec3& stationB = getVertexOnB();
+ const Vec3 stationG = bodyB.getBodyTransform(s)*stationB;
+ const Vec3 endfG = stationG - Scale*fG;
+ const Vec3 endNG = stationG + Scale*NG;
+ geometry.push_back(DecorativeLine(endfG + Vec3(0,.05,0),
+ stationG + Vec3(0,.05,0))
+ .setColor(isInStiction?Green:Orange));
+ geometry.push_back(DecorativeLine(endNG + Vec3(0,.05,0),
+ stationG + Vec3(0,.05,0))
+ .setColor(Red));
+ }
+
+ int getIndex() const {return m_index;}
+ const MobilizedBody& getBodyB() const
+ { return m_matter.getMobilizedBody(m_vmobodx); }
+ const Vec3& getVertexOnB() const {return m_vertex_B;}
+ const MobilizedBody& getBodyP() const
+ { return m_matter.getMobilizedBody(m_hsmobodx); }
+ const Transform& getX_PH() const {return m_X_PH;}
+ const ContactMaterial& getMaterial() const {return m_material;}
+
+ //--------------------------------------------------------------------------
+private:
+ // Determine whether the current slip velocity is reliable enough that
+ // we should use it to replace the previous slip velocity.
+ static bool shouldUpdate(const Vec2& vSlip, Real vSlipMag,
+ const Vec2& prevSlipDir, Real velTol) {
+ if (prevSlipDir.isNaN())
+ return vSlipMag > 0; // we'll take anything
+
+ // Check for reversal.
+ bool reversed = (~vSlip*prevSlipDir < 0);
+ return !reversed && (vSlipMag > velTol);
+ }
+
+ // Find the location of the vertex on B, measured and expressed in P.
+ Vec3 findVInP(const State& s) const {
+ return getBodyB().findStationLocationInAnotherBody
+ (s, m_vertex_B, getBodyP());
+ }
+
+ // Find the location and velocity of the vertex on B, measured from and
+ // expressed in P.
+ Vec3 findVDotInP(const State& s) const {
+ return getBodyB().findStationVelocityInAnotherBody
+ (s, m_vertex_B, getBodyP());
+ }
+
+ Vec3 findPointOfBCoincidentWithPointOfP
+ (const State& s, const Vec3& r_P) const
+ {
+ return getBodyP().findStationLocationInAnotherBody(s, r_P, getBodyB());
+ }
+
+
+private:
+ const GeneralForceSubsystem& m_forces;
+ const SimbodyMatterSubsystem& m_matter;
+ const MobilizedBodyIndex m_hsmobodx; // body P with halfspace
+ Transform m_X_PH; // halfspace frame in P
+ const MobilizedBodyIndex m_vmobodx; // body B with vertex
+ Vec3 m_vertex_B; // vertex location in B
+ ContactMaterial m_material; // composite material props
+ Real m_kBristle; // bristle stiffness
+ Real m_tBristle; // bristle time constant
+ Real m_tDwell; // dwell time constant
+
+ Real m_vtrans; // transition velocity for
+ // Stribeck stiction
+ Real m_vtol; // max meaningful slip velocity
+
+ int m_index; // unique id for this contact
+
+ // Set during realizeTopology().
+ mutable ZIndex m_bristleDeformationIx; // zx,zy
+ mutable ZIndex m_dwellIx; // r_dw
+ mutable DiscreteVariableIndex m_prevSlipDirIx; // previous slip direction
+
+ mutable CacheEntryIndex m_contactInfoIx;
+};
+
+
+//==============================================================================
+// MY UNILATERAL CONSTRAINT SET
+//==============================================================================
+
+class MyUnilateralConstraintSet {
+public:
+ // Transition velocity: if a slip velocity is smaller than this the
+ // contact is a candidate for stiction.
+ MyUnilateralConstraintSet(const MultibodySystem& mbs)
+ : m_mbs(mbs) {}
+
+ // Ownership of this force element belongs to the System; we're just keeping
+ // a reference to it here.
+ int addBristleElement(MyBristleVertexContactElementImpl* vertex) {
+ const int index = (int)m_bristle.size();
+ m_bristle.push_back(vertex);
+ vertex->setContactIndex(index);
+ return index;
+ }
+
+ int getNumContactElements() const {return (int)m_bristle.size();}
+ const MyBristleVertexContactElementImpl& getContactElement(int ix) const
+ { return *m_bristle[ix]; }
+ MyBristleVertexContactElementImpl& updContactElement(int ix)
+ { return *m_bristle[ix]; }
+
+ // Initialize all states and runtime fields in the contact elements.
+ void initialize(State& state)
+ {
+ for (unsigned i=0; i < m_bristle.size(); ++i)
+ m_bristle[i]->initialize(state);
+ }
+
+ // In Debug mode, produce a useful summary of the current state of the
+ // contact and friction elements.
+ void showContactStatus(const State& state, const String& place) const;
+
+ ~MyUnilateralConstraintSet() {
+ // Nothing to delete since we are only holding references.
+ }
+
+ const MultibodySystem& getMultibodySystem() const {return m_mbs;}
+private:
+ const MultibodySystem& m_mbs;
+ Array_<MyBristleVertexContactElementImpl*> m_bristle; // unowned ref
+};
+
+
+
+//==============================================================================
+// STATE SAVER
+//==============================================================================
+// This reporter is called now and again to save the current state so we can
+// play back a movie at the end.
+class StateSaver : public PeriodicEventReporter {
+public:
+ StateSaver(const MultibodySystem& mbs,
+ const MyUnilateralConstraintSet& unis,
+ const Integrator& integ,
+ Real reportInterval)
+ : PeriodicEventReporter(reportInterval),
+ m_mbs(mbs), m_unis(unis), m_integ(integ)
+ { m_states.reserve(2000); }
+
+ ~StateSaver() {}
+
+ void clear() {m_states.clear();}
+ int getNumSavedStates() const {return (int)m_states.size();}
+ const State& getState(int n) const {return m_states[n];}
+
+ void handleEvent(const State& s) const {
+ const SimbodyMatterSubsystem& matter=m_mbs.getMatterSubsystem();
+ const SpatialVec PG = matter.calcSystemMomentumAboutGroundOrigin(s);
+ m_mbs.realize(s, Stage::Acceleration);
+
+#ifndef NDEBUG
+ printf("%3d: %5g mom=%g,%g E=%g", m_integ.getNumStepsTaken(),
+ s.getTime(),
+ PG[0].norm(), PG[1].norm(), m_mbs.calcEnergy(s));
+ m_unis.showContactStatus(s, "STATE SAVER");
+#endif
+
+ m_states.push_back(s);
+ }
+private:
+ const MultibodySystem& m_mbs;
+ const MyUnilateralConstraintSet& m_unis;
+ const Integrator& m_integ;
+ mutable Array_<State> m_states;
+};
+
+// A periodic event reporter that does nothing; useful for exploring the
+// effects of interrupting the simulation.
+class Nada : public PeriodicEventReporter {
+public:
+ explicit Nada(Real reportInterval)
+ : PeriodicEventReporter(reportInterval) {}
+
+ void handleEvent(const State& s) const {
+#ifndef NDEBUG
+ printf("%7g NADA\n", s.getTime());
+#endif
+ }
+};
+
+
+//==============================================================================
+// SHOW CONTACT
+//==============================================================================
+// For each visualization frame, generate ephemeral geometry to show just
+// during this frame, based on the current State.
+class ShowContact : public DecorationGenerator {
+public:
+ ShowContact(const MyUnilateralConstraintSet& unis)
+ : m_unis(unis) {}
+
+ void generateDecorations(const State& state,
+ Array_<DecorativeGeometry>& geometry) OVERRIDE_11
+ {
+ for (int i=0; i < m_unis.getNumContactElements(); ++i) {
+ const MyBristleVertexContactElementImpl& contact =
+ m_unis.getContactElement(i);
+ const Vec3 loc = contact.whereToDisplay(state);
+ if (contact.isInContact(state)) {
+ geometry.push_back(DecorativeSphere(.1)
+ .setTransform(loc)
+ .setColor(Red).setOpacity(.25));
+ String text = "LOCKED";
+ text = contact.isSticking(state) ? "STICKING"
+ : "CONTACT";
+ m_unis.getMultibodySystem().realize(state, Stage::Acceleration);
+ contact.showContactForces(state, geometry);
+ geometry.push_back(DecorativeText(String(i)+"-"+text)
+ .setColor(White).setScale(.1)
+ .setTransform(loc+Vec3(0,.04,0)));
+ } else {
+ geometry.push_back(DecorativeText(String(i))
+ .setColor(White).setScale(.1)
+ .setTransform(loc+Vec3(0,.02,0)));
+ }
+ }
+ }
+private:
+ const MyUnilateralConstraintSet& m_unis;
+};
+
+
+//==============================================================================
+// BODY WATCHER
+//==============================================================================
+// Prior to rendering each frame, point the camera at the given body's
+// origin.
+class BodyWatcher : public Visualizer::FrameController {
+public:
+ explicit BodyWatcher(const MobilizedBody& body) : m_body(body) {}
+
+ void generateControls(const Visualizer& viz,
+ const State& state,
+ Array_< DecorativeGeometry >& geometry) OVERRIDE_11
+ {
+ const Vec3 Bo = m_body.getBodyOriginLocation(state);
+ const Vec3 p_GC = Bo + Vec3(0, 1, 5); // above and back
+ const Rotation R_GC(UnitVec3(0,1,0), YAxis,
+ p_GC-Bo, ZAxis);
+ viz.setCameraTransform(Transform(R_GC,p_GC));
+ //viz.pointCameraAt(Bo, Vec3(0,1,0));
+ }
+private:
+ const MobilizedBody m_body;
+};
+
+
+//==============================================================================
+// MY PUSH FORCE
+//==============================================================================
+// This is a force element that generates a constant force on a body for a
+// specified time interval. It is useful to perturb the system to force it
+// to transition from sticking to sliding, for example.
+class MyPushForceImpl : public Force::Custom::Implementation {
+public:
+ MyPushForceImpl(const MobilizedBody& bodyB,
+ const Vec3& stationB,
+ const Vec3& forceG, // force direction in Ground!
+ Real onTime,
+ Real offTime)
+ : m_bodyB(bodyB), m_stationB(stationB), m_forceG(forceG),
+ m_on(onTime), m_off(offTime)
+ { }
+
+ //--------------------------------------------------------------------------
+ // Custom force virtuals
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+ OVERRIDE_11
+ {
+ if (!(m_on <= state.getTime() && state.getTime() <= m_off))
+ return;
+
+ m_bodyB.applyForceToBodyPoint(state, m_stationB, m_forceG, bodyForces);
+
+ //SimTK_DEBUG4("PUSHING @t=%g (%g,%g,%g)\n", state.getTime(),
+ // m_forceG[0], m_forceG[1], m_forceG[2]);
+ }
+
+ // No potential energy.
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11 {return 0;}
+
+ void calcDecorativeGeometryAndAppend
+ (const State& state, Stage stage,
+ Array_<DecorativeGeometry>& geometry) const OVERRIDE_11
+ {
+ const Real ScaleFactor = 0.1;
+ if (stage != Stage::Time) return;
+ if (!(m_on <= state.getTime() && state.getTime() <= m_off))
+ return;
+ const Vec3 stationG = m_bodyB.findStationLocationInGround(state, m_stationB);
+ geometry.push_back(DecorativeLine(stationG-ScaleFactor*m_forceG, stationG)
+ .setColor(Yellow)
+ .setLineThickness(3));
+ }
+private:
+ const MobilizedBody& m_bodyB;
+ const Vec3 m_stationB;
+ const Vec3 m_forceG;
+ Real m_on;
+ Real m_off;
+};
+
+
+
+//==============================================================================
+// MAIN
+//==============================================================================
+int main(int argc, char** argv) {
+ try { // If anything goes wrong, an exception will be thrown.
+ const Real ReportInterval=1./30;
+ const Vec3 BrickHalfDims(.1, .25, .5);
+ const Real BrickMass = 10;
+ #ifdef USE_TIMS_PARAMS
+ const Real RunTime=16; // Tim's time
+ const Real Stiffness = 2e7;
+ const Real Dissipation = 1;
+ const Real CoefRest = 0;
+ const Real mu_d = .5; /* compliant: .7*/
+ const Real mu_s = .8; /* compliant: .7*/
+ const Real mu_v = /*0.05*/0; //TODO: fails with mu_v=1, vtrans=.01
+ const Real TransitionVelocity = 0.01;
+ const Inertia brickInertia(.1,.1,.1);
+ //const Real Radius = .02;
+ const Real Radius = 1;
+ #else
+ const Real RunTime=20;
+ const Real Stiffness = 1e6;
+ const Real CoefRest = 0;
+ const Real TargetVelocity = 3; // speed at which to match coef rest
+// const Real Dissipation = (1-CoefRest)/TargetVelocity;
+ const Real Dissipation = .1;
+ const Real mu_d = .5;
+ const Real mu_s = .8;
+ const Real mu_v = 0*0.05;
+ const Real TransitionVelocity = 0.01;
+ const Inertia brickInertia(BrickMass*UnitInertia::brick(BrickHalfDims));
+ const Real Radius = BrickHalfDims[0]/3;
+ #endif
+
+ printf("\n******************** Tim's Box Bristle ********************\n");
+ printf("USING BRISTLE MODEL: Compliant material/integrated stiction\n");
+ #ifdef USE_TIMS_PARAMS
+ printf("Using Tim's parameters:\n");
+ #else
+ printf("Using Sherm's parameters:\n");
+ #endif
+ printf(" stiffness=%g dissipation=%g\n", Stiffness, Dissipation);
+ printf(" mu_d=%g mu_s=%g mu_v=%g\n", mu_d, mu_s, mu_v);
+ printf(" transition velocity=%g\n", TransitionVelocity);
+ printf(" brick inertia=%g %g %g\n",
+ brickInertia.getMoments()[0], brickInertia.getMoments()[1],
+ brickInertia.getMoments()[2]);
+ printf("******************** Tim's Box Bristle ********************\n\n");
+
+ // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
+ MultibodySystem mbs;
+ SimbodyMatterSubsystem matter(mbs);
+ GeneralForceSubsystem forces(mbs);
+ Force::Gravity gravity(forces, matter, -YAxis, 9.81);
+ //Force::Gravity gravity(forces, matter, -UnitVec3(.3,1,0), 3*9.81);
+ MobilizedBody& Ground = matter.updGround();
+
+ // Define a material to use for contact. This is not very stiff.
+ ContactMaterial material(std::sqrt(Radius)*Stiffness,
+ Dissipation,
+ mu_s, // mu_static
+ mu_d, // mu_dynamic
+ mu_v); // mu_viscous
+
+ // ADD MOBILIZED BODIES AND CONTACT CONSTRAINTS
+
+ MyUnilateralConstraintSet unis(mbs);
+
+ Body::Rigid brickBody =
+ Body::Rigid(MassProperties(BrickMass, Vec3(0), brickInertia));
+
+ MobilizedBody::Free brick(Ground, Vec3(0),
+ brickBody, Vec3(0));
+ brick.addBodyDecoration(Transform(), DecorativeBrick(BrickHalfDims)
+ .setColor(Red).setOpacity(.3));
+/*
+1) t= 0.5, dt = 2 sec, pt = (0.05, 0.2, 0.4), fdir = (1,0,0), mag = 50N
+2) t= 4.0, dt = 0.5 sec, pt = (0.03, 0.06, 0.09), fdir = (0.2,0.8,0), mag = 300N
+3) t= 0.9, dt = 2 sec, pt = (0,0,0), fdir = (0,1,0), mag = 49.333N (half the weight of the block)
+4) t= 13.0, dt = 1 sec, pt = (0 0 0), fdir = (-1,0,0), mag = 200N
+*/
+ Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0.05,0.2,0.4),
+ 50 * Vec3(1,0,0),
+ 0.5, 0.5+2));
+ Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0.03, 0.06, 0.09),
+ 300 * UnitVec3(0.2,0.8,0),
+ //300 * Vec3(0.2,0.8,0),
+ 4, 4+0.5));
+ Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0),
+ 49.033 * Vec3(0,1,0),
+ 9, 9+2));
+ Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0),
+ 200 * Vec3(-1,0,0),
+ 13, 13+1));
+
+ #ifndef USE_TIMS_PARAMS
+ // Extra late force.
+ Force::Custom(forces, new MyPushForceImpl(brick, Vec3(.1, 0, .45),
+ 20 * Vec3(-1,-1,.5),
+ 15, Infinity));
+ #endif
+
+ for (int i=-1; i<=1; i+=2)
+ for (int j=-1; j<=1; j+=2)
+ for (int k=-1; k<=1; k+=2) {
+ const Vec3 pt = Vec3(i,j,k).elementwiseMultiply(BrickHalfDims);
+ MyBristleVertexContactElementImpl* vertex =
+ new MyBristleVertexContactElementImpl(forces,
+ Ground, YAxis, 0, // halfplane
+ brick, pt, material, TransitionVelocity);
+ Force::Custom(forces, vertex); // add force element to system
+ unis.addBristleElement(vertex); // assign index, transition velocity
+ }
+
+ matter.setShowDefaultGeometry(false);
+ Visualizer viz(mbs);
+ viz.setShowSimTime(true);
+ viz.setShowFrameNumber(true);
+ viz.setShowFrameRate(true);
+ viz.addDecorationGenerator(new ShowContact(unis));
+
+ #ifdef ANIMATE
+ mbs.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));
+ #else
+ // This does nothing but interrupt the simulation so that exact step
+ // sequence will be maintained with animation off.
+ mbs.addEventReporter(new Nada(ReportInterval));
+ #endif
+
+ viz.addFrameController(new BodyWatcher(brick));
+
+ Vec3 cameraPos(0, 1, 2);
+ UnitVec3 cameraZ(0,0,1);
+ viz.setCameraTransform(Transform(Rotation(cameraZ, ZAxis,
+ UnitVec3(YAxis), YAxis),
+ cameraPos));
+ viz.pointCameraAt(Vec3(0,0,0), Vec3(0,1,0));
+
+ #ifdef USE_TIMS_PARAMS
+ Real accuracy = 1e-4;
+ RungeKuttaMersonIntegrator integ(mbs);
+ #else
+ //Real accuracy = 1e-4;
+ Real accuracy = 1e-3;
+ //Real accuracy = 1e-5;
+ //ExplicitEulerIntegrator integ(mbs);
+ //RungeKutta2Integrator integ(mbs);
+ //RungeKutta3Integrator integ(mbs);
+ //SemiExplicitEulerIntegrator integ(mbs, .005);
+ SemiExplicitEuler2Integrator integ(mbs);
+ //RungeKuttaFeldbergIntegrator integ(mbs);
+ //RungeKuttaMersonIntegrator integ(mbs);
+ //VerletIntegrator integ(mbs);
+ //CPodesIntegrator integ(mbs);
+ #endif
+
+ integ.setAccuracy(accuracy);
+ //integ.setMaximumStepSize(0.25);
+ integ.setMaximumStepSize(0.05);
+ //integ.setMaximumStepSize(0.002);
+ //integ.setAllowInterpolation(false);
+
+ StateSaver* stateSaver = new StateSaver(mbs,unis,integ,ReportInterval);
+ mbs.addEventReporter(stateSaver);
+
+ State s = mbs.realizeTopology(); // returns a reference to the the default state
+
+ //matter.setUseEulerAngles(s, true);
+
+ mbs.realizeModel(s); // define appropriate states for this System
+ mbs.realize(s, Stage::Instance); // instantiate constraints if any
+
+
+/*
+rX_q = 0.7 rad
+rX_u = 1.0 rad/sec
+
+rY_q = 0.6 rad
+rY_u = 0.0 rad/sec
+
+rZ_q = 0.5 rad
+rZ_u = 0.2 rad/sec
+
+tX_q = 0.0 m
+tX_u = 10 m/s
+
+tY_q = 1.4 m
+tY_u = 0.0 m/s
+
+tZ_q = 0.0 m
+tZ_u = 0.0 m/s
+*/
+
+ #ifdef USE_TIMS_PARAMS
+ brick.setQToFitTranslation(s, Vec3(0,10,0));
+ brick.setUToFitLinearVelocity(s, Vec3(0,0,0));
+ #else
+ brick.setQToFitTranslation(s, Vec3(0,1.4,0));
+ brick.setUToFitLinearVelocity(s, Vec3(10,0,0));
+ const Rotation R_BC(SimTK::BodyRotationSequence,
+ 0.7, XAxis, 0.6, YAxis, 0.5, ZAxis);
+ brick.setQToFitRotation(s, R_BC);
+ brick.setUToFitAngularVelocity(s, Vec3(1,0,.2));
+ #endif
+
+ mbs.realize(s, Stage::Velocity);
+ viz.report(s);
+
+ cout << "Initial configuration shown. Next? ";
+ getchar();
+
+ Assembler(mbs).setErrorTolerance(1e-6).assemble(s);
+ viz.report(s);
+ cout << "Assembled configuration shown. Ready? ";
+ getchar();
+
+ // Now look for enabled contacts that aren't sliding; turn on stiction
+ // for those.
+ mbs.realize(s, Stage::Velocity);
+ for (int i=0; i < unis.getNumContactElements(); ++i) {
+ MyBristleVertexContactElementImpl& fric = unis.updContactElement(i);
+ if (!fric.isInContact(s)) continue;
+ const Real vSlip = fric.getActualSlipSpeed(s);
+ fric.initializeForStiction(s); // just in case
+ printf("friction element %d has v_slip=%g%s\n", i, vSlip,
+ vSlip==0?" (ENABLING STICTION)":"");
+ }
+
+ // Make sure Lapack gets initialized.
+ Matrix M(1,1); M(0,0)=1.23;
+ FactorLU Mlu(M);
+
+
+ // Simulate it.
+
+ integ.setReturnEveryInternalStep(true);
+ TimeStepper ts(mbs, integ);
+ ts.setReportAllSignificantStates(true);
+
+ #ifdef TEST_REPEATABILITY
+ const int tries = NTries;
+ #else
+ const int tries = 1;
+ #endif
+
+ Array_< Array_<State> > states(tries);
+ Array_< Array_<Real> > timeDiff(tries-1);
+ Array_< Array_<Vector> > yDiff(tries-1);
+ cout.precision(18);
+ for (int ntry=0; ntry < tries; ++ntry) {
+ mbs.resetAllCountersToZero();
+ unis.initialize(s); // reinitialize
+ ts.updIntegrator().resetAllStatistics();
+ ts.initialize(s);
+ int nStepsWithEvent = 0;
+
+ const double startReal = realTime();
+ const double startCPU = cpuTime();
+
+ Integrator::SuccessfulStepStatus status;
+ do {
+ status=ts.stepTo(RunTime);
+ #ifdef TEST_REPEATABILITY
+ states[ntry].push_back(ts.getState());
+ #endif
+ const int j = states[ntry].size()-1;
+ if (ntry>0) {
+ int prev = ntry-1;
+ //prev=0;
+ Real dt = states[ntry][j].getTime()
+ - states[prev][j].getTime();
+ volatile double vd;
+ const int ny = states[ntry][j].getNY();
+ Vector d(ny);
+ for (int i=0; i<ny; ++i) {
+ vd = states[ntry][j].getY()[i]
+ - states[prev][j].getY()[i];
+ d[i] = vd;
+ }
+ timeDiff[prev].push_back(dt);
+ yDiff[prev].push_back(d);
+ cout << "\n" << states[prev][j].getTime()
+ << "+" << dt << ":" << d << endl;
+ }
+
+ const bool handledEvent = status==Integrator::ReachedEventTrigger;
+ if (handledEvent) {
+ ++nStepsWithEvent;
+ SimTK_DEBUG3("EVENT %3d @%.17g status=%s\n\n",
+ nStepsWithEvent, ts.getTime(),
+ Integrator::getSuccessfulStepStatusString(status).c_str());
+ } else {
+ SimTK_DEBUG3("step %3d @%.17g status=%s\n",
+ integ.getNumStepsTaken(), ts.getTime(),
+ Integrator::getSuccessfulStepStatusString(status).c_str());
+ }
+ #ifndef NDEBUG
+ viz.report(ts.getState());
+ #endif
+ } while (ts.getTime() < RunTime);
+
+
+ const double timeInSec = realTime()-startReal;
+ const double cpuInSec = cpuTime()-startCPU;
+ const int evals = integ.getNumRealizations();
+ cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
+ timeInSec << "s for " << ts.getTime() << "s sim (avg step="
+ << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
+ << (1000*ts.getTime())/evals << "ms/eval\n";
+ cout << "CPUtime " << cpuInSec << endl;
+
+ printf("Used Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
+ printf("# EVENT STEPS/HANDLER CALLS = %d/%d\n",
+ nStepsWithEvent, mbs.getNumHandleEventCalls());
+ }
+
+ for (int i=0; i<tries; ++i)
+ cout << "nstates " << i << " " << states[i].size() << endl;
+
+ // Instant replay.
+ while(true) {
+ printf("Hit ENTER for replay (%d states) ...",
+ stateSaver->getNumSavedStates());
+ getchar();
+ for (int i=0; i < stateSaver->getNumSavedStates(); ++i) {
+ mbs.realize(stateSaver->getState(i), Stage::Velocity);
+ viz.report(stateSaver->getState(i));
+ }
+ }
+
+ }
+ catch (const std::exception& e) {
+ printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+ }
+ catch (...) {
+ printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+}
+
+//==============================================================================
+// MY UNILATERAL CONSTRAINT SET
+//==============================================================================
+
+
+//--------------------------- SHOW CONTACT STATUS ------------------------------
+void MyUnilateralConstraintSet::
+showContactStatus(const State& s, const String& place) const
+{
+#ifndef NDEBUG
+ printf("\n%s: uni status @t=%.15g\n", place.c_str(), s.getTime());
+ m_mbs.realize(s, Stage::Acceleration);
+ for (int i=0; i < getNumContactElements(); ++i) {
+ const MyBristleVertexContactElementImpl& contact = getContactElement(i);
+ const bool isActive = contact.isInContact(s);
+ printf(" %6s %2d %s, h=%g dh=%g f=%g\n",
+ isActive?"ACTIVE":"off", i, "bristle",
+ contact.getHeight(s),contact.getHeightDot(s),
+ isActive?contact.getNormalForce(s):Zero);
+ if (!isActive) continue;
+
+ const bool isSticking = contact.isSticking(s);
+ printf(" %8s friction %2d, |v|=%g\n",
+ isSticking?"STICKING":"sliding", i,
+ contact.getActualSlipSpeed(s));
+ contact.writeFrictionInfo(s, " ", std::cout);
+ }
+ printf("\n");
+#endif
+}
diff --git a/Simbody/tests/adhoc/TimsBoxHybrid.cpp b/Simbody/tests/adhoc/TimsBoxHybrid.cpp
new file mode 100644
index 0000000..954bc87
--- /dev/null
+++ b/Simbody/tests/adhoc/TimsBoxHybrid.cpp
@@ -0,0 +1,1731 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) - Tim's Box (hybrid contact model) *
+ * -------------------------------------------------------------------------- *
+ * 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) 2013 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* Investigate a hybrid compliant contact/rigid stiction model, using Tim's
+box as a test case. */
+
+//#define NDEBUG 1
+
+#include "Simbody.h"
+
+#include <string>
+#include <iostream>
+#include <exception>
+
+using std::cout;
+using std::endl;
+
+using namespace SimTK;
+
+//#define USE_TIMS_PARAMS
+#define ANIMATE // off to get more accurate CPU time (you can still playback)
+
+// Set to revert to the no-constraint stiction model for performance comparison
+// with the new constraint-based one.
+//#define USE_CONTINUOUS_STICTION
+
+// Define this to run the simulation NTries times, saving the states and
+// comparing them bitwise to see if the simulations are perfectly repeatable
+// as they should be. You should see nothing but exact zeroes print out for
+// second and subsequent runs.
+//#define TEST_REPEATABILITY
+static const int NTries=3;
+
+
+// This is the continuous stiction model used in Simbody's compliant contact
+// system for comparison with the new hybrid model. See end of this file for
+// implementation.
+static Real stribeck(Real us, Real ud, Real uv, Real v);
+
+//==============================================================================
+// MY HYBRID VERTEX CONTACT ELEMENT
+//==============================================================================
+// Given a contact material (with compliant material properties), this
+// represents a compliant point-and-rigid-halfspace contact element. We track
+// the height h of a vertex V on body B over a halfspace H on body P. The
+// halfspace frame H serves as the contact frame, with Hz in the contact
+// normal direction and Hxy spanning the tangent plane. The pose X_PH is
+// a given constant.
+//
+// Normal force
+// ------------
+// If the vertex height h is above the halfspace surface (h>=0), no forces are
+// generated. If below the surface (h<0) we generate a normal force of magnitude
+// N=max(0, -k*h(1-d*hdot)), with N >= 0
+// applied to both bodies at the contact point C, which is the point at h=0 just
+// up the halfspace normal direction from the vertex (because we're considering
+// the halfspace to be rigid). Denote by Cb the station (material point) of B
+// that is coincident with C, and Cp the station of P that is coincident with C.
+//
+// Sliding force
+// -------------
+// Then if we are in sliding mode, we also generate a tangential force of
+// magnitude T=(mu_d+mu_v*|v|)*N, where mu_d, mu_v are the dynamic and viscous
+// coefficients of friction, and v is the velocity of Cb relative to Cp,
+// projected into the tangent plane (a 2d vector). If |v|>tol then the
+// tangential slip direction is s=v/|v|, and we record this as the previous slip
+// direction s_prev. Otherwise the slip direction remains s=s_prev. In any case
+// the tangential force applied is -T*s.
+//
+// Stiction force
+// --------------
+// If we are instead in stiction mode, then no sliding force is generated.
+// Instead a pair of no-slip constraints is active, generating constraint
+// multipliers (tx,ty), and we record s_prev=-[tx,ty]/|[tx,ty]| as the
+// impending slip direction. Note that this will not be available until
+// Acceleration stage, while the normal force and tangential sliding force can
+// be calculated at Velocity stage.
+//
+// Witness function
+// ----------------
+// (trigger on + -> -):
+// sliding_to_stiction = dot(v, s_prev)
+// stiction_to_sliding = mu_s*N - |[tx,ty]|
+
+// This is a velocity-stage cache entry.
+struct MyHybridContactInfo {
+ MyHybridContactInfo()
+ : h(NaN), Cp(NaN), Cb(NaN), v_HCb(NaN), vSlipMag(NaN), f_HCb(NaN)
+ {
+ }
+ // Position info.
+ Real h; // signed distance, h<0 means contact
+ Vec3 Cp; // station on P coincident with the contact point
+ Vec3 Cb; // station on B coincident with the contact point
+ Rotation R_GH;
+
+ // Velocity info. H frame has z along contact normal, x,y in tangent plane.
+ Vec3 v_HCb; // velocity of Cb in the contact frame H
+ Real vSlipMag; // |(v_HCb.x, v_HCb.y)|
+ Vec3 f_HCb; // contact force on Cb in contact frame H
+};
+
+class MyHybridVertexContactElementImpl : public Force::Custom::Implementation {
+public:
+ MyHybridVertexContactElementImpl(const GeneralForceSubsystem& forces,
+ MobilizedBody& hsmobod, const UnitVec3& hsn, Real hsh,
+ MobilizedBody& vmobod, const Vec3& vertex,
+ const ContactMaterial& material)
+ : m_matter(hsmobod.getMatterSubsystem()), m_forces(forces),
+ m_hsmobodx(hsmobod), m_X_PH(Rotation(hsn, ZAxis), hsh*hsn),
+ m_vmobodx(vmobod), m_vertex_B(vertex),
+ m_material(material),
+ m_noslipX(hsmobod, Vec3(NaN), m_X_PH.x(), hsmobod, vmobod),
+ m_noslipY(hsmobod, Vec3(NaN), m_X_PH.y(), hsmobod, vmobod),
+ m_index(-1), m_vtrans(NaN), // assign later
+ m_contactPointInP(NaN), m_recordedSlipDir(NaN)
+ {
+ m_noslipX.setDisabledByDefault(true);
+ m_noslipY.setDisabledByDefault(true);
+ }
+
+ void setContactIndex(int index) {m_index=index;}
+ void setTransitionVelocity(Real vtrans) {m_vtrans=vtrans;}
+
+ void initialize() { // TODO
+ m_contactPointInP = NaN;
+ }
+
+ // Set the friction application point to be the projection of the contact
+ // point onto the contact plane. This will be used the next time stiction
+ // is enabled. Requires state realized to Position stage.
+ void initializeForStiction(const State& s) {
+ const Real h = findContactPointInP(s, m_contactPointInP);
+ }
+
+ bool isInContact(const State& s) const
+ { return findH(s) < 0; }
+
+ bool isSticking(const State& s) const
+ {
+ #ifdef USE_CONTINUOUS_STICTION
+ return getActualSlipSpeed(s) <= m_vtrans;
+ #else
+ return !m_noslipX.isDisabled(s); // X,Y always on or off together
+ #endif
+ }
+
+ // Return a point in Ground coincident with the vertex.
+ Vec3 whereToDisplay(const State& s) const {
+ return getBodyB().findStationLocationInGround(s, m_vertex_B);
+ }
+
+ Real getActualSlipSpeed(const State& s) const {
+ const MyHybridContactInfo& info = getContactInfo(s);
+ return info.vSlipMag;
+ }
+
+ // Return the normal force N >= 0 currently being generated by this
+ // contact. State must be realized to Stage::Velocity.
+ Real getNormalForce(const State& s) const {
+ const MyHybridContactInfo& info = getContactInfo(s);
+ if (info.h >= 0) return 0;
+ const Real N = info.f_HCb[2]; // z component is normal
+ return N;
+ }
+
+ Real getHeight(const State& s) const {
+ const MyHybridContactInfo& info = getContactInfo(s);
+ return info.h;
+ }
+
+ Real getHeightDot(const State& s) const {
+ const MyHybridContactInfo& info = getContactInfo(s);
+ return info.v_HCb[2]; // velocity in plane normal direction
+ }
+
+ // Return the sliding force 2-vector in the halfspace plane that is being
+ // applied to the contact point station on body B. If we are sticking
+ // then this force is zero; call getStickingForce() to get the real value.
+ const Vec2& getSlidingForce(const State& s) const {
+ const MyHybridContactInfo& info = getContactInfo(s);
+ const Vec2& f = info.f_HCb.getSubVec<2>(0);
+ return f;
+ }
+
+ // Return the sliding velocity 2-vector in the halfspace plane of the
+ // contact point station on body B relative to the contact point station
+ // on body P.
+ const Vec2& getSlipVelocity(const State& s) const {
+ const MyHybridContactInfo& info = getContactInfo(s);
+ const Vec2& v = info.v_HCb.getSubVec<2>(0);
+ return v;
+ }
+
+ // The way we constructed the NoSlip1D constraints makes the multipliers be
+ // the force on the half space on body P; we negate here so we'll get the
+ // force on the vertex body B instead.
+ Vec2 getStictionForce(const State& s) const {
+ assert(isSticking(s));
+ #ifndef USE_CONTINUOUS_STICTION
+ return Vec2(-m_noslipX.getMultiplier(s), -m_noslipY.getMultiplier(s));
+ #else
+ return getSlidingForce(s);
+ #endif
+ }
+
+ void recordImpendingSlipDir(const State& s) const {
+ const Vec2 f = getStictionForce(s);
+ const Real fMag = f.norm();
+ const Vec2 dir = fMag==0 ? Vec2(1,0) : Vec2(-f/fMag);
+ m_recordedSlipDir = dir;
+ }
+
+ void recordActualSlipDir(const State& s) const {
+ const Vec2 v = getSlipVelocity(s);
+ const Real vMag = v.norm();
+ const Vec2 dir = vMag==0 ? Vec2(1,0) : Vec2(v/vMag);
+ m_recordedSlipDir = dir;
+ }
+
+ void updatePrevSlipDirFromRecorded(State& s) const {
+ setPrevSlipDir(s, m_recordedSlipDir);
+ }
+
+ Real calcSlipSpeedWitness(const State& s) const {
+ if (getHeight(s) >= 0 || isSticking(s)) return 0;
+ const Vec2& slipDirPrev = getPrevSlipDir(s);
+ const Vec2& vNow = getSlipVelocity(s);
+ if (slipDirPrev.isNaN()) return vNow.norm();
+ return dot(vNow, slipDirPrev);
+ }
+
+ Real calcStictionForceWitness(const State& s) const {
+ if (getHeight(s) >= 0 || !isSticking(s)) return 0;
+ const Real mu_s = m_material.getStaticFriction();
+ const Real N = getNormalForce(s);
+ const Vec2 f = getStictionForce(s);
+ const Real fmag = f.norm();
+ return mu_s*N - fmag;
+ }
+
+
+
+ // Note that initializeForStiction() must have been called first.
+ void enableStiction(State& s) const
+ { m_noslipX.setContactPoint(s, m_contactPointInP);
+ m_noslipY.setContactPoint(s, m_contactPointInP);
+ m_noslipX.enable(s); m_noslipY.enable(s); }
+
+ void disableStiction(State& s) const
+ { m_noslipX.disable(s); m_noslipY.disable(s); }
+
+ // Return height of vertex over plane; negative if penetrated. You can
+ // call this at Stage::Position.
+ Real findH(const State& state) const {
+ const Vec3 V_P = findVInP(state); // location of V in P
+ const Real h = dot(V_P-m_X_PH.p(), m_X_PH.z());
+ return h;
+ }
+
+ // Returns height of vertex over plane same as findH(); also returns
+ // contact point (projection of V onto plane along Hz).
+ Real findContactPointInP(const State& state, Vec3& contactPointInP) const {
+ const Vec3 V_P = findVInP(state); // location of V in P
+ const Real h = dot(V_P-m_X_PH.p(), m_X_PH.z());
+ contactPointInP = V_P - h*m_X_PH.z();
+ return h;
+ }
+
+ // Calculate v_eff, the direction to be opposed by the sliding force.
+ Vec2 getEffectiveSlipDir(const State& s, const Vec2& vSlip, Real vSlipMag) const {
+ const Vec2 prevVslipDir = getPrevSlipDir(s);
+ if (shouldUpdate(vSlip, vSlipMag, prevVslipDir, m_vtrans))
+ return vSlip/vSlipMag;
+ else return prevVslipDir;
+ }
+
+ // Return the slip velocity as recorded at the end of the last time step.
+ const Vec2& getPrevSlipDir(const State& state) const {
+ const Vec2& prevSlipDir = Value<Vec2>::downcast
+ (m_forces.getDiscreteVariable(state, m_prevSlipDirIx));
+ return prevSlipDir;
+ }
+ // Modify the discrete state variable directly.
+ void setPrevSlipDir(State& state, const Vec2& slipDir) const {
+ Vec2& prevSlipDir = Value<Vec2>::updDowncast
+ (m_forces.updDiscreteVariable(state, m_prevSlipDirIx));
+ prevSlipDir = slipDir;
+ SimTK_DEBUG3("STATE CHG %d: prevDir to %g %g\n",
+ m_index, slipDir[0], slipDir[1]);
+ }
+
+ // Get access to the auto-update cache variable that will update the
+ // previous slip direction state variable if the step is accepted.
+ Vec2& updPrevSlipDirAutoUpdateValue(const State& state) const {
+ Vec2& prevSlipUpdate = Value<Vec2>::updDowncast
+ (m_forces.updDiscreteVarUpdateValue(state, m_prevSlipDirIx));
+ return prevSlipUpdate;
+ }
+ // Mark the auto-update cache entry valid.
+ void markPrevSlipDirAutoUpdateValueValid(const State& state) const {
+ m_forces.markDiscreteVarUpdateValueRealized(state, m_prevSlipDirIx);
+ }
+
+ const MyHybridContactInfo& getContactInfo(const State& state) const {
+ const MyHybridContactInfo& info =
+ Value<MyHybridContactInfo>::downcast
+ (m_forces.getCacheEntry(state, m_contactInfoIx));
+ return info;
+ }
+
+ MyHybridContactInfo& updContactInfo(const State& state) const {
+ MyHybridContactInfo& info =
+ Value<MyHybridContactInfo>::updDowncast
+ (m_forces.updCacheEntry(state, m_contactInfoIx));
+ return info;
+ }
+ //--------------------------------------------------------------------------
+ // Custom force virtuals
+ // Apply the normal force, and the sliding friction force if it is enabled.
+ // This is called during realize(Dynamics).
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+ OVERRIDE_11
+ {
+ const MyHybridContactInfo& info = getContactInfo(state);
+ if (info.h >= 0)
+ return; // no contact
+
+ const Vec3 f_GCb = info.R_GH * info.f_HCb; // re-express in Ground
+ getBodyB().applyForceToBodyPoint(state, info.Cb, f_GCb, bodyForces);
+ getBodyP().applyForceToBodyPoint(state, info.Cp, -f_GCb, bodyForces);
+ }
+
+ // The normal force stores energy as 2/5 k h^(5/2) when h<0.
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11 {
+ const MyHybridContactInfo& info = getContactInfo(state);
+ if (info.h >= 0)
+ return 0; // no contact
+ const Real h52 = square(info.h)*std::sqrt(-info.h);
+ const Real k = m_material.getStiffness();
+ return 0.4*k*h52;
+ }
+
+ // Allocate state variable for storing the previous sliding direction.
+ void realizeTopology(State& state) const OVERRIDE_11 {
+ // The previous sliding direction is used in an event witness that
+ // is evaluated at Velocity stage.
+ m_prevSlipDirIx = m_forces.allocateAutoUpdateDiscreteVariable
+ (state, Stage::Velocity, new Value<Vec2>(Vec2(NaN)),
+ Stage::Velocity);
+
+ m_contactInfoIx = m_forces.allocateCacheEntry
+ (state, Stage::Velocity, new Value<MyHybridContactInfo>());
+ }
+
+
+ // Calculate everything here and save in contact info cache entry where
+ // it can be retrieved for generating forces, reporting, etc.
+ void realizeVelocity(const State& state) const OVERRIDE_11 {
+ MyHybridContactInfo& info = updContactInfo(state);
+
+ // Forces generated only if h<0. Cp always be the projection of the
+ // vertex onto the halfspace surface.
+ info.h = findContactPointInP(state, info.Cp);
+
+ const Rotation& R_PH = m_X_PH.R();
+ const Rotation& R_GP = getBodyP().getBodyRotation(state);
+ info.R_GH = R_GP*R_PH;
+
+ // Station of B coincident with the contact point.
+ info.Cb = info.h < 0
+ ? findPointOfBCoincidentWithPointOfP(state, info.Cp)
+ : getVertexOnB();
+
+ // Velocity of B's contact station in P.
+ const Vec3 v_PCb = getBodyB().findStationVelocityInAnotherBody
+ (state, info.Cb, getBodyP());
+ info.v_HCb = ~R_PH*v_PCb; // re-express in H
+ const Real hdot = info.v_HCb[2]; // z component
+ const Vec2 vSlip_HC(info.v_HCb[0], info.v_HCb[1]);
+ info.vSlipMag = vSlip_HC.norm();
+
+ const Real k = m_material.getStiffness(),
+ c = m_material.getDissipation(),
+ mu_d = m_material.getDynamicFriction(),
+ mu_s = m_material.getStaticFriction(),
+ mu_v = m_material.getViscousFriction();
+
+ info.f_HCb = Vec3(0);
+ if (info.h >= 0)
+ return; // no contact
+
+ const Real h32 = -info.h*sqrt(-info.h); // |h| ^ (3/2)
+ const Real N = std::max(Real(0), k*h32*(1-c*hdot));
+ if (N==0)
+ return; // no contact force
+
+ // N is the Hz component of the force on Cb.
+ Vec2 fT_HCb(0); // This will be the (Hx,Hy) components of force on Cb.
+ #ifdef USE_CONTINUOUS_STICTION
+ {
+ // Make v unitless velocity and scale viscous coefficient to match.
+ const Real v = info.vSlipMag / m_vtrans;
+ const Real mu=stribeck(mu_s,mu_d,mu_v*m_vtrans,v);
+ const Real T = mu*N;
+ fT_HCb = (-T/info.vSlipMag)*vSlip_HC;
+ }
+ #else
+ if (!isSticking(state)) {
+ // Apply sliding force
+ const Real T = (mu_d + mu_v*info.vSlipMag)*N;
+ fT_HCb = -T*getEffectiveSlipDir(state, vSlip_HC, // in Hxy
+ info.vSlipMag);
+ }
+ #endif
+
+ info.f_HCb = Vec3(fT_HCb[0], fT_HCb[1], N); // force on Cb, in H
+ }
+
+ // If we're sticking, set the update value for the previous slip direction
+ // to the opposite of the stiction force direction.
+ // If we're sliding, set the update value for the previous slip direction
+ // if the current slip velocity is usable.
+ #ifndef USE_CONTINUOUS_STICTION
+ void realizeAcceleration(const State& state) const OVERRIDE_11 {
+ const MyHybridContactInfo& info = getContactInfo(state);
+ const Vec2& prevSlipDir = getPrevSlipDir(state);
+
+ if (info.h >= 0) {
+ // No contact. Forget previous slip direction.
+ if (!prevSlipDir.isNaN()) {
+ SimTK_DEBUG1("%d LIFTOFF UPDATE, forget prevSlipDir\n", m_index);
+ updPrevSlipDirAutoUpdateValue(state).setToNaN();
+ markPrevSlipDirAutoUpdateValueValid(state);
+ }
+ return;
+ }
+
+ // Sticking.
+ if (isSticking(state)) {
+ const Vec2 f_HCb = getStictionForce(state);
+ const Real fMag = f_HCb.norm();
+ if (fMag > 0) {
+ Vec2& prevSlipUpdate = updPrevSlipDirAutoUpdateValue(state);
+ prevSlipUpdate = -f_HCb / fMag;
+ #ifndef NDEBUG
+ printf("%d STICKING UPDATE: prevSlipDir=%g %g; now=%g %g\n",
+ m_index, getPrevSlipDir(state)[0],getPrevSlipDir(state)[1],
+ prevSlipUpdate[0], prevSlipUpdate[1]);
+ #endif
+ markPrevSlipDirAutoUpdateValueValid(state);
+ }
+ return;
+ }
+
+ // Sliding.
+ const Vec2& vSlip_HCb = info.v_HCb.getSubVec<2>(0); // x,y
+
+ if (shouldUpdate(vSlip_HCb, info.vSlipMag, prevSlipDir, m_vtrans)) {
+ Vec2& prevSlipUpdate = updPrevSlipDirAutoUpdateValue(state);
+ prevSlipUpdate = vSlip_HCb / info.vSlipMag;
+ markPrevSlipDirAutoUpdateValueValid(state);
+
+ #ifndef NDEBUG
+ printf("%d SLIDING UPDATE: prevSlipDir=%g %g; now=%g %g; |v|=%g dot=%g vdot=%g\n",
+ m_index, prevSlipDir[0],prevSlipDir[1],
+ prevSlipUpdate[0],prevSlipUpdate[1], info.vSlipMag,
+ ~prevSlipUpdate*prevSlipDir, ~vSlip_HCb*prevSlipDir);
+ #endif
+ } else {
+ #ifndef NDEBUG
+ printf("%d SLIDING; NO UPDATE: prevSlipDir=%g %g; Vnow=%g %g; |v|=%g vdot=%g\n",
+ m_index,
+ prevSlipDir[0],prevSlipDir[1],vSlip_HCb[0],vSlip_HCb[1],
+ info.vSlipMag, ~vSlip_HCb*prevSlipDir);
+ #endif
+ }
+ }
+ #endif
+
+ std::ostream& writeFrictionInfo(const State& s, const String& indent,
+ std::ostream& o) const
+ {
+ o << indent;
+ if (!isInContact(s)) o << "OFF";
+ else if (isSticking(s)) o << "STICK";
+ else o << "SLIP";
+
+ const Vec2 v = getSlipVelocity(s);
+ const Vec2 pd = getPrevSlipDir(s);
+ const Vec2 f = isSticking(s) ? getStictionForce(s)
+ : getSlidingForce(s);
+ o << " prevDir=" << ~pd << " V=" << ~v << " Vdot=" << ~v*pd
+ << " F=" << ~f << endl;
+ return o;
+ }
+
+
+ void showContactForces(const State& s, Array_<DecorativeGeometry>& geometry)
+ const
+ {
+ const Real Scale = 0.01;
+ const Real NH = getNormalForce(s);
+
+ #ifdef USE_CONTINUOUS_STICTION
+ const bool isInStiction = getActualSlipSpeed(s) <= m_vtrans;
+ const Vec2 fH = getSlidingForce(s);
+ #else
+ const bool isInStiction = isSticking(s);
+ const Vec2 fH = isSticking(s) ? getStictionForce(s) : getSlidingForce(s);
+ #endif
+
+ if (fH.normSqr() < square(SignificantReal) && NH < SignificantReal)
+ return;
+
+ const MyHybridContactInfo& info = getContactInfo(s);
+ const Vec3 fG = info.R_GH * Vec3(fH[0],fH[1],0); // friction
+ const Vec3 NG = info.R_GH * Vec3(0, 0, NH); // normal
+
+ const MobilizedBody& bodyB = getBodyB();
+ const Vec3& stationB = getVertexOnB();
+ const Vec3 stationG = bodyB.getBodyTransform(s)*stationB;
+ const Vec3 endfG = stationG - Scale*fG;
+ const Vec3 endNG = stationG + Scale*NG;
+ geometry.push_back(DecorativeLine(endfG + Vec3(0,.05,0),
+ stationG + Vec3(0,.05,0))
+ .setColor(isInStiction?Green:Orange));
+ geometry.push_back(DecorativeLine(endNG + Vec3(0,.05,0),
+ stationG + Vec3(0,.05,0))
+ .setColor(Red));
+ }
+
+ int getIndex() const {return m_index;}
+ const MobilizedBody& getBodyB() const
+ { return m_matter.getMobilizedBody(m_vmobodx); }
+ const Vec3& getVertexOnB() const {return m_vertex_B;}
+ const MobilizedBody& getBodyP() const
+ { return m_matter.getMobilizedBody(m_hsmobodx); }
+ const Transform& getX_PH() const {return m_X_PH;}
+ const ContactMaterial& getMaterial() const {return m_material;}
+
+ //--------------------------------------------------------------------------
+private:
+ // Determine whether the current slip velocity is reliable enough that
+ // we should use it to replace the previous slip velocity.
+ static bool shouldUpdate(const Vec2& vSlip, Real vSlipMag,
+ const Vec2& prevSlipDir, Real velTol) {
+ if (prevSlipDir.isNaN())
+ return vSlipMag > 0; // we'll take anything
+
+ // Check for reversal.
+ bool reversed = (~vSlip*prevSlipDir < 0);
+ return !reversed && (vSlipMag > velTol);
+ }
+
+ // Find the location of the vertex on B, measured and expressed in P.
+ Vec3 findVInP(const State& s) const {
+ return getBodyB().findStationLocationInAnotherBody
+ (s, m_vertex_B, getBodyP());
+ }
+
+ // Find the location and velocity of the vertex on B, measured from and
+ // expressed in P.
+ Vec3 findVDotInP(const State& s) const {
+ return getBodyB().findStationVelocityInAnotherBody
+ (s, m_vertex_B, getBodyP());
+ }
+
+
+ Vec3 findPointOfBCoincidentWithPointOfP
+ (const State& s, const Vec3& r_P) const
+ {
+ return getBodyP().findStationLocationInAnotherBody(s, r_P, getBodyB());
+ }
+
+
+private:
+ const GeneralForceSubsystem& m_forces;
+ const SimbodyMatterSubsystem& m_matter;
+ const MobilizedBodyIndex m_hsmobodx; // body P with halfspace
+ Transform m_X_PH; // halfspace frame in P
+ const MobilizedBodyIndex m_vmobodx; // body B with vertex
+ Vec3 m_vertex_B; // vertex location in B
+ ContactMaterial m_material; // composite material props
+
+ Real m_vtrans; // transition velocity for
+ // Stribeck stiction
+
+ Constraint::NoSlip1D m_noslipX;
+ Constraint::NoSlip1D m_noslipY;
+
+ int m_index; // unique id for this contact
+
+ // This is recorded at Position stage prior to turning on stiction
+ // constraints; then we use it to set the contact point in noslipX,Y.
+ Vec3 m_contactPointInP;
+
+ // This is recorded during event handling and then used to set the
+ // remembered previous slip direction at the end of the handler to make
+ // sure all witness functions are positive then.
+ mutable Vec2 m_recordedSlipDir;
+
+ // Set during realizeTopology().
+ mutable DiscreteVariableIndex m_prevSlipDirIx; // previous slip direction
+ mutable CacheEntryIndex m_contactInfoIx;
+};
+
+
+//==============================================================================
+// MY UNILATERAL CONSTRAINT SET
+//==============================================================================
+
+// These are indices into the unilateral constraint set arrays.
+struct MyElementSubset {
+ void clear()
+ { m_contact.clear();m_sliding.clear();m_stiction.clear();
+ m_ineligible.clear(); }
+ Array_<int> m_contact;
+ Array_<int> m_sliding; // subset of above that can only be sliding
+ Array_<int> m_stiction; // subset of above that might be in stiction
+ Array_<int> m_ineligible; // stiction on, but not in contact any more
+};
+
+class MyUnilateralConstraintSet {
+public:
+ // Transition velocity: if a slip velocity is smaller than this the
+ // contact is a candidate for stiction.
+ MyUnilateralConstraintSet(const MultibodySystem& mbs,
+ Real transitionVelocity)
+ : m_mbs(mbs), m_transitionVelocity(transitionVelocity) {}
+
+ // Ownership of this force element belongs to the System; we're just keeping
+ // a reference to it here.
+ int addHybridElement(MyHybridVertexContactElementImpl* vertex) {
+ const int index = (int)m_hybrid.size();
+ m_hybrid.push_back(vertex);
+ vertex->setContactIndex(index);
+ vertex->setTransitionVelocity(m_transitionVelocity);
+ return index;
+ }
+
+ Real getTransitionVelocity() const {return m_transitionVelocity;}
+ void setTransitionVelocity(Real v) {m_transitionVelocity=v;}
+
+ int getNumContactElements() const {return (int)m_hybrid.size();}
+ const MyHybridVertexContactElementImpl& getContactElement(int ix) const
+ { return *m_hybrid[ix]; }
+ MyHybridVertexContactElementImpl& updContactElement(int ix)
+ { return *m_hybrid[ix]; }
+
+ // Initialize all runtime fields in the contact & friction elements.
+ void initialize()
+ {
+ for (unsigned i=0; i < m_hybrid.size(); ++i)
+ m_hybrid[i]->initialize();
+ }
+
+ // Return the contact and friction elements that might be involved in
+ // generating contact forces at the current state. Candidate contact
+ // elements are those that are (a) already enabled, or (b) for which
+ // perr <= posTol and verr <= velTol. Candidate friction elements are those
+ // whose normal force master is unconditional or a candidate and (a) which
+ // are already sticking, or (b) for which vslip <= velTol, or (c) for which
+ // vslip opposes the previous slip direction, meaning it has reversed and
+ // must have passed through zero during the last step. These are the elements
+ // that can be activated without making any changes to the configuration or
+ // velocity state variables, except slightly for constraint projection.
+ //
+ // We also record the friction elements that, if their masters are active,
+ // can only slide because they have a significant slip velocity. State must
+ // be realized through Velocity stage.
+ void findCandidateElements(const State& s,
+ Real velTol,
+ MyElementSubset& candidates) const
+ {
+ candidates.clear();
+ for (unsigned i=0; i < m_hybrid.size(); ++i) {
+ if (!m_hybrid[i]->isInContact(s)) {
+ if (m_hybrid[i]->isSticking(s)) {
+ candidates.m_ineligible.push_back(i); // must disable
+ SimTK_DEBUG2("%d NOW INELIGIBLE because h=%g.\n",
+ i, m_hybrid[i]->findH(s));
+ }
+ continue;
+ }
+
+ candidates.m_contact.push_back(i);
+
+ if (m_hybrid[i]->isSticking(s)
+ || m_hybrid[i]->getActualSlipSpeed(s) <= velTol
+ || m_hybrid[i]->calcSlipSpeedWitness(s) <= 0)
+ {
+ m_hybrid[i]->initializeForStiction(s);
+ candidates.m_stiction.push_back(i); // could stick or slide
+ } else
+ candidates.m_sliding.push_back(i); // can only slide
+ }
+ }
+
+ // Look through the given constraint subset and enable any constraints
+ // that are currently disabled, and disable any constraints that are still
+ // on after liftoff. Returns true if any change was made.
+ bool enableConstraintSubset(const MyElementSubset& subset,
+ State& state) const
+ {
+ bool changedSomething = false;
+
+ // Disable all ineligible constraints.
+ for (unsigned i=0; i < subset.m_ineligible.size(); ++i) {
+ const int which = subset.m_ineligible[i];
+ const MyHybridVertexContactElementImpl& fric =
+ getContactElement(which);
+ SimTK_DEBUG1("%d DISABLING INELIGIBLE STICTION.\n", i);
+ fric.disableStiction(state);
+ changedSomething = true;
+ }
+
+ // Enable all stiction constraints.
+ for (unsigned i=0; i < subset.m_stiction.size(); ++i) {
+ const int which = subset.m_stiction[i];
+ const MyHybridVertexContactElementImpl& fric =
+ getContactElement(which);
+ if (!fric.isSticking(state)) {
+ SimTK_DEBUG1("%d ENABLING CANDIDATE STICTION.\n", i);
+ fric.enableStiction(state);
+ changedSomething = true;
+ }
+ }
+
+ m_mbs.realize(state, Stage::Instance);
+ return changedSomething;
+ }
+
+ // All event handlers call this method before returning. Given a state for
+ // which no (further) impulse is required, here we decide which contact and
+ // stiction constraints are active, and ensure that they satisfy the
+ // required constraint tolerances to the given accuracy. For sliding
+ // contacts, we will have recorded the slip or impending slip direction and
+ // converged the normal forces.
+ // TODO: in future this may return indicating that an impulse is required
+ // after all, as in Painleve's paradox.
+ void selectActiveConstraints(State& state, Real accuracy) const;
+
+ // This is the inner loop of selectActiveConstraints(). Given a set of
+ // candidates to consider, it finds an active subset and enables those
+ // constraints.
+ void findActiveCandidates(State& state,
+ const MyElementSubset& candidates) const;
+
+ // In Debug mode, produce a useful summary of the current state of the
+ // contact and friction elements.
+ void showConstraintStatus(const State& state, const String& place) const;
+
+ ~MyUnilateralConstraintSet() {
+ // Nothing to delete since we are only holding references.
+ }
+
+ const MultibodySystem& getMultibodySystem() const {return m_mbs;}
+private:
+ const MultibodySystem& m_mbs;
+ Real m_transitionVelocity;
+ Array_<MyHybridVertexContactElementImpl*> m_hybrid; // unowned ref
+};
+
+
+
+//==============================================================================
+// STATE SAVER
+//==============================================================================
+// This reporter is called now and again to save the current state so we can
+// play back a movie at the end.
+class StateSaver : public PeriodicEventReporter {
+public:
+ StateSaver(const MultibodySystem& mbs,
+ const MyUnilateralConstraintSet& unis,
+ const Integrator& integ,
+ Real reportInterval)
+ : PeriodicEventReporter(reportInterval),
+ m_mbs(mbs), m_unis(unis), m_integ(integ)
+ { m_states.reserve(2000); }
+
+ ~StateSaver() {}
+
+ void clear() {m_states.clear();}
+ int getNumSavedStates() const {return (int)m_states.size();}
+ const State& getState(int n) const {return m_states[n];}
+
+ void handleEvent(const State& s) const {
+ const SimbodyMatterSubsystem& matter=m_mbs.getMatterSubsystem();
+ const SpatialVec PG = matter.calcSystemMomentumAboutGroundOrigin(s);
+ m_mbs.realize(s, Stage::Acceleration);
+
+#ifndef NDEBUG
+ printf("%3d: %5g mom=%g,%g E=%g", m_integ.getNumStepsTaken(),
+ s.getTime(),
+ PG[0].norm(), PG[1].norm(), m_mbs.calcEnergy(s));
+ cout << " Triggers=" << s.getEventTriggers() << endl;
+ m_unis.showConstraintStatus(s, "STATE SAVER");
+#endif
+
+ m_states.push_back(s);
+ }
+private:
+ const MultibodySystem& m_mbs;
+ const MyUnilateralConstraintSet& m_unis;
+ const Integrator& m_integ;
+ mutable Array_<State> m_states;
+};
+
+// A periodic event reporter that does nothing; useful for exploring the
+// effects of interrupting the simulation.
+class Nada : public PeriodicEventReporter {
+public:
+ explicit Nada(Real reportInterval)
+ : PeriodicEventReporter(reportInterval) {}
+
+ void handleEvent(const State& s) const {
+#ifndef NDEBUG
+ printf("%7g NADA\n", s.getTime());
+#endif
+ }
+};
+
+
+//==============================================================================
+// SHOW CONTACT
+//==============================================================================
+// For each visualization frame, generate ephemeral geometry to show just
+// during this frame, based on the current State.
+class ShowContact : public DecorationGenerator {
+public:
+ ShowContact(const MyUnilateralConstraintSet& unis)
+ : m_unis(unis) {}
+
+ void generateDecorations(const State& state,
+ Array_<DecorativeGeometry>& geometry) OVERRIDE_11
+ {
+ for (int i=0; i < m_unis.getNumContactElements(); ++i) {
+ const MyHybridVertexContactElementImpl& contact =
+ m_unis.getContactElement(i);
+ const Vec3 loc = contact.whereToDisplay(state);
+ if (contact.isInContact(state)) {
+ geometry.push_back(DecorativeSphere(.1)
+ .setTransform(loc)
+ .setColor(Red).setOpacity(.25));
+ String text = "LOCKED";
+ text = contact.isSticking(state) ? "STICKING"
+ : "CONTACT";
+ m_unis.getMultibodySystem().realize(state, Stage::Acceleration);
+ contact.showContactForces(state, geometry);
+ geometry.push_back(DecorativeText(String(i)+"-"+text)
+ .setColor(White).setScale(.1)
+ .setTransform(loc+Vec3(0,.04,0)));
+ } else {
+ geometry.push_back(DecorativeText(String(i))
+ .setColor(White).setScale(.1)
+ .setTransform(loc+Vec3(0,.02,0)));
+ }
+ }
+ }
+private:
+ const MyUnilateralConstraintSet& m_unis;
+};
+
+
+//==============================================================================
+// BODY WATCHER
+//==============================================================================
+// Prior to rendering each frame, point the camera at the given body's
+// origin.
+class BodyWatcher : public Visualizer::FrameController {
+public:
+ explicit BodyWatcher(const MobilizedBody& body) : m_body(body) {}
+
+ void generateControls(const Visualizer& viz,
+ const State& state,
+ Array_< DecorativeGeometry >& geometry) OVERRIDE_11
+ {
+ const Vec3 Bo = m_body.getBodyOriginLocation(state);
+ const Vec3 p_GC = Bo + Vec3(0, 1, 5); // above and back
+ const Rotation R_GC(UnitVec3(0,1,0), YAxis,
+ p_GC-Bo, ZAxis);
+ viz.setCameraTransform(Transform(R_GC,p_GC));
+ //viz.pointCameraAt(Bo, Vec3(0,1,0));
+ }
+private:
+ const MobilizedBody m_body;
+};
+
+//==============================================================================
+// STICTION ON HANDLER
+//==============================================================================
+// Allocate one of these for each contact constraint that has friction. This
+// handler takes care of turning on the stiction constraints when the sliding
+// velocity drops to zero.
+class StictionOn: public TriggeredEventHandler {
+public:
+ StictionOn(const MultibodySystem& system,
+ const MyUnilateralConstraintSet& unis,
+ unsigned which)
+ : TriggeredEventHandler(Stage::Velocity),
+ m_mbs(system), m_unis(unis), m_which(which)
+ {
+ getTriggerInfo().setTriggerOnRisingSignTransition(false);
+ }
+
+ // This is the witness function. It is positive as long as we continue
+ // to slide in the same direction; negative means reversal.
+ Real getValue(const State& state) const {
+ const MyHybridVertexContactElementImpl& contact =
+ m_unis.getContactElement(m_which);
+ if (!contact.isInContact(state)) return 0;
+ const Real signedSlipSpeed = contact.calcSlipSpeedWitness(state);
+ return signedSlipSpeed;
+ }
+
+ void handleEvent
+ (State& s, Real accuracy, bool& shouldTerminate) const
+ {
+ ++m_counter;
+ //printf("StictionOn #%d\n", m_counter);
+ SimTK_DEBUG2("\nhandle %d slide->stick@%.17g\n", m_which, s.getTime());
+ SimTK_DEBUG("\n----------------------------------------------------\n");
+ SimTK_DEBUG2("STICTION ON triggered by friction element %d @t=%.15g\n",
+ m_which, s.getTime());
+ m_mbs.realize(s, Stage::Acceleration);
+
+ #ifndef NDEBUG
+ m_unis.showConstraintStatus(s, "ENTER STICTION ON");
+ cout << " entry triggers=" << s.getEventTriggers() << "\n";
+ #endif
+
+ m_unis.selectActiveConstraints(s, accuracy);
+
+ #ifndef NDEBUG
+ m_mbs.realize(s, Stage::Acceleration);
+ cout << " exit triggers=" << s.getEventTriggers() << "\n";
+ #endif
+
+ SimTK_DEBUG("STICTION ON done.\n");
+ SimTK_DEBUG("----------------------------------------------------\n");
+ }
+
+private:
+ const MultibodySystem& m_mbs;
+ const MyUnilateralConstraintSet& m_unis;
+ const int m_which; // one of the contact elements
+ static int m_counter;
+};
+int StictionOn::m_counter = 0;
+
+
+
+//==============================================================================
+// STICTION OFF HANDLER
+//==============================================================================
+// Allocate one of these for each contact constraint. This handler takes
+// care of turning off stiction constraints when the stiction force magnitude
+// exceeds mu*N.
+class StictionOff: public TriggeredEventHandler {
+public:
+ StictionOff(const MultibodySystem& system,
+ const MyUnilateralConstraintSet& unis,
+ unsigned which)
+ : TriggeredEventHandler(Stage::Acceleration),
+ m_mbs(system), m_unis(unis), m_which(which)
+ {
+ getTriggerInfo().setTriggerOnRisingSignTransition(false);
+ }
+
+ // This is the witness function. It is positive as long as mu_s*N is greater
+ // than the friction force magnitude.
+ Real getValue(const State& state) const {
+ const MyHybridVertexContactElementImpl& contact =
+ m_unis.getContactElement(m_which);
+ if (!contact.isInContact(state)) return 0;
+ const Real capacity = contact.calcStictionForceWitness(state);
+ return capacity; // how much stiction capacity is left
+ }
+
+ void handleEvent
+ (State& s, Real accuracy, bool& shouldTerminate) const
+ {
+ ++m_counter;
+ //printf("StictionOff #%d\n", m_counter);
+ SimTK_DEBUG2("\nhandle %d stick->slide@%.17g\n", m_which, s.getTime());
+ SimTK_DEBUG("\n----------------------------------------------------\n");
+ SimTK_DEBUG2("STICTION OFF triggered by friction element %d @t=%.15g\n",
+ m_which, s.getTime());
+ m_mbs.realize(s, Stage::Acceleration);
+
+ #ifndef NDEBUG
+ m_unis.showConstraintStatus(s, "ENTER STICTION OFF");
+ cout << " triggers=" << s.getEventTriggers() << "\n";
+ #endif
+
+ m_unis.selectActiveConstraints(s, accuracy);
+
+ #ifndef NDEBUG
+ m_mbs.realize(s, Stage::Acceleration);
+ cout << " exit triggers=" << s.getEventTriggers() << "\n";
+ #endif
+
+ SimTK_DEBUG("STICTION OFF done.\n");
+ SimTK_DEBUG("----------------------------------------------------\n");
+ }
+
+private:
+ const MultibodySystem& m_mbs;
+ const MyUnilateralConstraintSet& m_unis;
+ const int m_which; // one of the friction elements
+ static int m_counter;
+};
+int StictionOff::m_counter = 0;
+
+
+
+//==============================================================================
+// MY PUSH FORCE
+//==============================================================================
+// This is a force element that generates a constant force on a body for a
+// specified time interval. It is useful to perturb the system to force it
+// to transition from sticking to sliding, for example.
+class MyPushForceImpl : public Force::Custom::Implementation {
+public:
+ MyPushForceImpl(const MobilizedBody& bodyB,
+ const Vec3& stationB,
+ const Vec3& forceG, // force direction in Ground!
+ Real onTime,
+ Real offTime)
+ : m_bodyB(bodyB), m_stationB(stationB), m_forceG(forceG),
+ m_on(onTime), m_off(offTime)
+ { }
+
+ //--------------------------------------------------------------------------
+ // Custom force virtuals
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+ OVERRIDE_11
+ {
+ if (!(m_on <= state.getTime() && state.getTime() <= m_off))
+ return;
+
+ m_bodyB.applyForceToBodyPoint(state, m_stationB, m_forceG, bodyForces);
+
+ //SimTK_DEBUG4("PUSHING @t=%g (%g,%g,%g)\n", state.getTime(),
+ // m_forceG[0], m_forceG[1], m_forceG[2]);
+ }
+
+ // No potential energy.
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11 {return 0;}
+
+ void calcDecorativeGeometryAndAppend
+ (const State& state, Stage stage,
+ Array_<DecorativeGeometry>& geometry) const OVERRIDE_11
+ {
+ const Real ScaleFactor = 0.1;
+ if (stage != Stage::Time) return;
+ if (!(m_on <= state.getTime() && state.getTime() <= m_off))
+ return;
+ const Vec3 stationG = m_bodyB.findStationLocationInGround(state, m_stationB);
+ geometry.push_back(DecorativeLine(stationG-ScaleFactor*m_forceG, stationG)
+ .setColor(Yellow)
+ .setLineThickness(3));
+ }
+private:
+ const MobilizedBody& m_bodyB;
+ const Vec3 m_stationB;
+ const Vec3 m_forceG;
+ Real m_on;
+ Real m_off;
+};
+
+
+
+//==============================================================================
+// MAIN
+//==============================================================================
+int main(int argc, char** argv) {
+ try { // If anything goes wrong, an exception will be thrown.
+ const Real ReportInterval=1./30;
+ const Vec3 BrickHalfDims(.1, .25, .5);
+ const Real BrickMass = 10;
+ #ifdef USE_TIMS_PARAMS
+ const Real RunTime=16; // Tim's time
+ const Real Stiffness = 2e7;
+ const Real Dissipation = 1;
+ const Real CoefRest = 0;
+ const Real mu_d = .5; /* compliant: .7*/
+ const Real mu_s = .8; /* compliant: .7*/
+ const Real mu_v = /*0.05*/0; //TODO: fails with mu_v=1, vtrans=.01
+ const Real TransitionVelocity = 0.01;
+ const Inertia brickInertia(.1,.1,.1);
+ //const Real Radius = .02;
+ const Real Radius = 1;
+ #else
+ const Real RunTime=20;
+ const Real Stiffness = 1e6;
+ const Real CoefRest = 0;
+ const Real TargetVelocity = 3; // speed at which to match coef rest
+// const Real Dissipation = (1-CoefRest)/TargetVelocity;
+ const Real Dissipation = .1;
+ const Real mu_d = .5;
+ const Real mu_s = .8;
+ const Real mu_v = 0*0.05;
+ const Real TransitionVelocity = 0.01;
+ const Inertia brickInertia(BrickMass*UnitInertia::brick(BrickHalfDims));
+ const Real Radius = BrickHalfDims[0]/3;
+ #endif
+
+ printf("\n******************** Tim's Box Hybrid ********************\n");
+ #ifdef USE_CONTINUOUS_STICTION
+ printf("USING OLD MODEL: Continuous Stiction (Stribeck)\n");
+ #else
+ printf("USING NEW MODEL: Hybrid Compliant material/rigid stiction\n");
+ #endif
+ #ifdef USE_TIMS_PARAMS
+ printf("Using Tim's parameters:\n");
+ #else
+ printf("Using Sherm's parameters:\n");
+ #endif
+ printf(" stiffness=%g dissipation=%g\n", Stiffness, Dissipation);
+ printf(" mu_d=%g mu_s=%g mu_v=%g\n", mu_d, mu_s, mu_v);
+ printf(" transition velocity=%g\n", TransitionVelocity);
+ printf(" brick inertia=%g %g %g\n",
+ brickInertia.getMoments()[0], brickInertia.getMoments()[1],
+ brickInertia.getMoments()[2]);
+ printf("******************** Tim's Box Hybrid ********************\n\n");
+
+ // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
+ MultibodySystem mbs;
+ SimbodyMatterSubsystem matter(mbs);
+ GeneralForceSubsystem forces(mbs);
+ Force::Gravity gravity(forces, matter, -YAxis, 9.81);
+ //Force::Gravity gravity(forces, matter, -UnitVec3(.3,1,0), 3*9.81);
+ MobilizedBody& Ground = matter.updGround();
+
+ // Define a material to use for contact. This is not very stiff.
+ ContactMaterial material(std::sqrt(Radius)*Stiffness,
+ Dissipation,
+ mu_s, // mu_static
+ mu_d, // mu_dynamic
+ mu_v); // mu_viscous
+
+ // ADD MOBILIZED BODIES AND CONTACT CONSTRAINTS
+
+ MyUnilateralConstraintSet unis(mbs, TransitionVelocity);
+
+ Body::Rigid brickBody =
+ Body::Rigid(MassProperties(BrickMass, Vec3(0), brickInertia));
+
+ MobilizedBody::Free brick(Ground, Vec3(0),
+ brickBody, Vec3(0));
+ brick.addBodyDecoration(Transform(), DecorativeBrick(BrickHalfDims)
+ .setColor(Red).setOpacity(.3));
+/*
+1) t= 0.5, dt = 2 sec, pt = (0.05, 0.2, 0.4), fdir = (1,0,0), mag = 50N
+2) t= 4.0, dt = 0.5 sec, pt = (0.03, 0.06, 0.09), fdir = (0.2,0.8,0), mag = 300N
+3) t= 0.9, dt = 2 sec, pt = (0,0,0), fdir = (0,1,0), mag = 49.333N (half the weight of the block)
+4) t= 13.0, dt = 1 sec, pt = (0 0 0), fdir = (-1,0,0), mag = 200N
+*/
+ Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0.05,0.2,0.4),
+ 50 * Vec3(1,0,0),
+ 0.5, 0.5+2));
+ Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0.03, 0.06, 0.09),
+ 300 * UnitVec3(0.2,0.8,0),
+ //300 * Vec3(0.2,0.8,0),
+ 4, 4+0.5));
+ Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0),
+ 49.033 * Vec3(0,1,0),
+ 9, 9+2));
+ Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0),
+ 200 * Vec3(-1,0,0),
+ 13, 13+1));
+
+ #ifndef USE_TIMS_PARAMS
+ // Extra late force.
+ Force::Custom(forces, new MyPushForceImpl(brick, Vec3(.1, 0, .45),
+ 20 * Vec3(-1,-1,.5),
+ 15, Infinity));
+ #endif
+
+ for (int i=-1; i<=1; i+=2)
+ for (int j=-1; j<=1; j+=2)
+ for (int k=-1; k<=1; k+=2) {
+ const Vec3 pt = Vec3(i,j,k).elementwiseMultiply(BrickHalfDims);
+ MyHybridVertexContactElementImpl* vertex =
+ new MyHybridVertexContactElementImpl(forces,
+ Ground, YAxis, 0, // halfplane
+ brick, pt, material);
+ Force::Custom(forces, vertex); // add force element to system
+ unis.addHybridElement(vertex); // assign index, transition velocity
+ #ifndef USE_CONTINUOUS_STICTION
+ mbs.addEventHandler(new StictionOn(mbs, unis, vertex->getIndex()));
+ mbs.addEventHandler(new StictionOff(mbs, unis, vertex->getIndex()));
+ #endif
+ }
+
+ matter.setShowDefaultGeometry(false);
+ Visualizer viz(mbs);
+ viz.setShowSimTime(true);
+ viz.setShowFrameNumber(true);
+ viz.setShowFrameRate(true);
+ viz.addDecorationGenerator(new ShowContact(unis));
+
+ #ifdef ANIMATE
+ mbs.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));
+ #else
+ // This does nothing but interrupt the simulation so that exact step
+ // sequence will be maintained with animation off.
+ mbs.addEventReporter(new Nada(ReportInterval));
+ #endif
+
+ viz.addFrameController(new BodyWatcher(brick));
+
+ Vec3 cameraPos(0, 1, 2);
+ UnitVec3 cameraZ(0,0,1);
+ viz.setCameraTransform(Transform(Rotation(cameraZ, ZAxis,
+ UnitVec3(YAxis), YAxis),
+ cameraPos));
+ viz.pointCameraAt(Vec3(0,0,0), Vec3(0,1,0));
+
+ #ifdef USE_TIMS_PARAMS
+ Real accuracy = 1e-4;
+ RungeKuttaMersonIntegrator integ(mbs);
+ #else
+ //Real accuracy = 1e-1;
+ Real accuracy = 1e-3;
+ //Real accuracy = 1e-5;
+ //ExplicitEulerIntegrator integ(mbs);
+ //RungeKutta2Integrator integ(mbs);
+ //RungeKutta3Integrator integ(mbs);
+ //SemiExplicitEulerIntegrator integ(mbs, .005);
+ SemiExplicitEuler2Integrator integ(mbs);
+ //RungeKuttaFeldbergIntegrator integ(mbs);
+ //RungeKuttaMersonIntegrator integ(mbs);
+ //VerletIntegrator integ(mbs);
+ //CPodesIntegrator integ(mbs);
+ #endif
+
+ integ.setAccuracy(accuracy);
+ //integ.setMaximumStepSize(0.25);
+ integ.setMaximumStepSize(0.05);
+ //integ.setMaximumStepSize(0.002);
+ //integ.setAllowInterpolation(false);
+
+ StateSaver* stateSaver = new StateSaver(mbs,unis,integ,ReportInterval);
+ mbs.addEventReporter(stateSaver);
+
+ State s = mbs.realizeTopology(); // returns a reference to the the default state
+
+ //matter.setUseEulerAngles(s, true);
+
+ mbs.realizeModel(s); // define appropriate states for this System
+ mbs.realize(s, Stage::Instance); // instantiate constraints if any
+
+
+/*
+rX_q = 0.7 rad
+rX_u = 1.0 rad/sec
+
+rY_q = 0.6 rad
+rY_u = 0.0 rad/sec
+
+rZ_q = 0.5 rad
+rZ_u = 0.2 rad/sec
+
+tX_q = 0.0 m
+tX_u = 10 m/s
+
+tY_q = 1.4 m
+tY_u = 0.0 m/s
+
+tZ_q = 0.0 m
+tZ_u = 0.0 m/s
+*/
+
+ #ifdef USE_TIMS_PARAMS
+ brick.setQToFitTranslation(s, Vec3(0,10,0));
+ brick.setUToFitLinearVelocity(s, Vec3(0,0,0));
+ #else
+ brick.setQToFitTranslation(s, Vec3(0,1.4,0));
+ brick.setUToFitLinearVelocity(s, Vec3(10,0,0));
+ const Rotation R_BC(SimTK::BodyRotationSequence,
+ 0.7, XAxis, 0.6, YAxis, 0.5, ZAxis);
+ brick.setQToFitRotation(s, R_BC);
+ brick.setUToFitAngularVelocity(s, Vec3(1,0,.2));
+ #endif
+
+ mbs.realize(s, Stage::Velocity);
+ viz.report(s);
+
+ cout << "Initial configuration shown. Next? ";
+ getchar();
+
+ Assembler(mbs).setErrorTolerance(1e-6).assemble(s);
+ viz.report(s);
+ cout << "Assembled configuration shown. Ready? ";
+ getchar();
+
+ // Now look for enabled contacts that aren't sliding; turn on stiction
+ // for those.
+ mbs.realize(s, Stage::Velocity);
+ Array_<int> enableTheseStictions;
+ for (int i=0; i < unis.getNumContactElements(); ++i) {
+ MyHybridVertexContactElementImpl& fric = unis.updContactElement(i);
+ if (!fric.isInContact(s)) continue;
+ const Real vSlip = fric.getActualSlipSpeed(s);
+ fric.initializeForStiction(s); // just in case
+ printf("friction element %d has v_slip=%g%s\n", i, vSlip,
+ vSlip==0?" (ENABLING STICTION)":"");
+ if (vSlip == 0)
+ enableTheseStictions.push_back(i);
+ }
+ for (unsigned i=0; i < enableTheseStictions.size(); ++i)
+ unis.getContactElement(enableTheseStictions[i]).enableStiction(s);
+
+ // Make sure Lapack gets initialized.
+ Matrix M(1,1); M(0,0)=1.23;
+ FactorLU Mlu(M);
+
+
+ // Simulate it.
+
+ integ.setReturnEveryInternalStep(true);
+ TimeStepper ts(mbs, integ);
+ ts.setReportAllSignificantStates(true);
+
+ #ifdef TEST_REPEATABILITY
+ const int tries = NTries;
+ #else
+ const int tries = 1;
+ #endif
+
+ Array_< Array_<State> > states(tries);
+ Array_< Array_<Real> > timeDiff(tries-1);
+ Array_< Array_<Vector> > yDiff(tries-1);
+ cout.precision(18);
+ for (int ntry=0; ntry < tries; ++ntry) {
+ mbs.resetAllCountersToZero();
+ unis.initialize(); // reinitialize
+ ts.updIntegrator().resetAllStatistics();
+ ts.initialize(s);
+ int nStepsWithEvent = 0;
+
+ const double startReal = realTime();
+ const double startCPU = cpuTime();
+
+ Integrator::SuccessfulStepStatus status;
+ do {
+ status=ts.stepTo(RunTime);
+ #ifdef TEST_REPEATABILITY
+ states[ntry].push_back(ts.getState());
+ #endif
+ const int j = states[ntry].size()-1;
+ if (ntry>0) {
+ int prev = ntry-1;
+ //prev=0;
+ Real dt = states[ntry][j].getTime()
+ - states[prev][j].getTime();
+ volatile double vd;
+ const int ny = states[ntry][j].getNY();
+ Vector d(ny);
+ for (int i=0; i<ny; ++i) {
+ vd = states[ntry][j].getY()[i]
+ - states[prev][j].getY()[i];
+ d[i] = vd;
+ }
+ timeDiff[prev].push_back(dt);
+ yDiff[prev].push_back(d);
+ cout << "\n" << states[prev][j].getTime()
+ << "+" << dt << ":" << d << endl;
+ }
+
+ const bool handledEvent = status==Integrator::ReachedEventTrigger;
+ if (handledEvent) {
+ ++nStepsWithEvent;
+ SimTK_DEBUG3("EVENT %3d @%.17g status=%s\n\n",
+ nStepsWithEvent, ts.getTime(),
+ Integrator::getSuccessfulStepStatusString(status).c_str());
+ } else {
+ SimTK_DEBUG3("step %3d @%.17g status=%s\n",
+ integ.getNumStepsTaken(), ts.getTime(),
+ Integrator::getSuccessfulStepStatusString(status).c_str());
+ }
+ #ifndef NDEBUG
+ viz.report(ts.getState());
+ #endif
+ } while (ts.getTime() < RunTime);
+
+
+ const double timeInSec = realTime()-startReal;
+ const double cpuInSec = cpuTime()-startCPU;
+ const int evals = integ.getNumRealizations();
+ cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
+ timeInSec << "s for " << ts.getTime() << "s sim (avg step="
+ << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
+ << (1000*ts.getTime())/evals << "ms/eval\n";
+ cout << "CPUtime " << cpuInSec << endl;
+
+ printf("Used Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
+ printf("# EVENT STEPS/HANDLER CALLS = %d/%d\n",
+ nStepsWithEvent, mbs.getNumHandleEventCalls());
+ }
+
+ for (int i=0; i<tries; ++i)
+ cout << "nstates " << i << " " << states[i].size() << endl;
+
+ // Instant replay.
+ while(true) {
+ printf("Hit ENTER for replay (%d states) ...",
+ stateSaver->getNumSavedStates());
+ getchar();
+ for (int i=0; i < stateSaver->getNumSavedStates(); ++i) {
+ mbs.realize(stateSaver->getState(i), Stage::Velocity);
+ viz.report(stateSaver->getState(i));
+ }
+ }
+
+ }
+ catch (const std::exception& e) {
+ printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+ }
+ catch (...) {
+ printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+}
+
+//==============================================================================
+// MY UNILATERAL CONSTRAINT SET
+//==============================================================================
+
+//------------------------ SELECT ACTIVE CONSTRAINTS ---------------------------
+void MyUnilateralConstraintSet::
+selectActiveConstraints(State& state, Real vtol) const {
+
+ // Find all the contacts and stiction elements that might be active based
+ // on kinematics.
+ MyElementSubset candidates;
+
+ bool needRestart;
+ do {
+ //TODO: this (mis)use of accuracy needs to be revisited.
+ findCandidateElements(state, vtol, candidates);
+
+ // Evaluate accelerations and reaction forces and check if
+ // any of the active contacts are generating negative ("pulling")
+ // forces; if so, inactivate them.
+ findActiveCandidates(state, candidates);
+
+ // Project active constraints to the constraint manifolds.
+ const Real tol = vtol/1000;
+ SimTK_DEBUG1("projecting active constraints to tol=%g\n", tol);
+ m_mbs.project(state, tol);
+
+ // It is possible that the projection above took some marginally-sliding
+ // friction and slowed it down enough to make it a stiction candidate.
+ needRestart = false;
+ for (unsigned i=0; i < candidates.m_sliding.size(); ++i) {
+ const int which = candidates.m_sliding[i];
+ const MyHybridVertexContactElementImpl& contact =
+ getContactElement(which);
+ assert(!contact.isSticking(state));
+ if ( contact.getActualSlipSpeed(state) <= vtol
+ || contact.calcSlipSpeedWitness(state) <= 0)
+ {
+ SimTK_DEBUG3("***RESTART** selectActiveConstraints() because "
+ "sliding velocity %d is now |v|=%g or witness=%g\n",
+ which, contact.getActualSlipSpeed(state),
+ contact.calcSlipSpeedWitness(state));
+ needRestart = true;
+ break;
+ }
+ }
+ } while (needRestart);
+}
+
+
+
+
+//-------------------------- FIND ACTIVE CANDIDATES ---------------------------
+// Given a list of candidate stiction constraints,
+// determine which ones are active in the least squares solution for the
+// constraint multipliers. Candidates are those constraints that meet all
+// kinematic conditions -- for stiction, sliding velocity less than tolerance,
+// or sliding direction reversed in the last step. Also, any
+// constraint that is currently active is a candidate, regardless of its
+// kinematics (might have drifted but that can't disable it).
+//
+// This method should be called only from within an event handler. For sliding
+// friction it will have reset the "previous slip direction" to the current
+// slip or impending slip direction.
+//
+// Algorithm
+// ---------
+// We're given a set of stiction candidates. If any are inactive, activate them.
+// -- at this point all aerr==0, some ferr might be < 0
+//
+// loop
+// - Realize(Acceleration) with the current active set
+// - Calculate ferr for active constraints, aerr for inactive
+// - If all ferr>=0, aerr>=0 -> break loop
+// - Check for aerr < 0 [tol?]. Shouldn't happen but if it does must turn on the
+// associated constraint for the worst violation, then -> continue loop
+// - Find worst (most negative) offender:
+// stiction offense = mu_s*max(0, fc) - |fs|
+// - Inactivate chosen constraint
+// record impending slip direction stick->slide
+// end loop
+//
+void MyUnilateralConstraintSet::
+findActiveCandidates(State& s, const MyElementSubset& candidates) const
+{
+ const int ReviseNormalNIters = 6;
+ showConstraintStatus(s, "ENTER findActiveCandidates()");
+
+ SimTK_DEBUG3(
+ "findActiveCandidates() %d/%d/%d contact/stick/ineligible ...\n",
+ candidates.m_contact.size(), candidates.m_stiction.size(),
+ candidates.m_ineligible.size());
+
+ // Disable any sticking constraints that are now ineligible due to
+ // liftoff, and enable all other candidate stiction constraints if any
+ // are currently disabled.
+ enableConstraintSubset(candidates, s);
+
+ if (candidates.m_contact.empty()) {
+ // Can't be any friction either, if there are no contacts.
+ SimTK_DEBUG("EXIT findActiveCandidates: no candidates.\n");
+ m_mbs.realize(s, Stage::Acceleration);
+ return;
+ }
+
+ int pass=0, nStictionDisabled=0;
+ while (true) {
+ ++pass;
+ SimTK_DEBUG1("\nfindActiveCandidates(): pass %d\n", pass);
+
+ // Given an active set, evaluate multipliers and accelerations.
+ m_mbs.realize(s, Stage::Acceleration);
+ if (pass==1) {
+ // First time through record all the slip directions.
+ for (unsigned i=0; i < candidates.m_contact.size(); ++i) {
+ const int which = candidates.m_contact[i];
+ const MyHybridVertexContactElementImpl& fric =
+ getContactElement(which);
+ if (fric.isSticking(s))
+ fric.recordImpendingSlipDir(s);
+ else fric.recordActualSlipDir(s);
+ }
+ }
+
+ // Scan all candidate stictions to find the active one that has the
+ // most negative capacity.
+
+ int worstActiveStiction=-1; Real mostNegativeStictionCapacity=0;
+ SimTK_DEBUG("analyzing stiction constraints ...\n");
+ for (unsigned i=0; i < candidates.m_stiction.size(); ++i) {
+ const int which = candidates.m_stiction[i];
+ SimTK_DEBUG1(" %d: ", which);
+ const MyHybridVertexContactElementImpl& fric =
+ getContactElement(which);
+
+ if (!fric.isSticking(s)) {
+ SimTK_DEBUG("off\n");
+ continue;
+ }
+
+ const Real capacity = fric.calcStictionForceWitness(s);
+ SimTK_DEBUG2("on capacity=%g (N=%g)\n",
+ capacity, fric.getNormalForce(s));
+
+ if (capacity < mostNegativeStictionCapacity) {
+ worstActiveStiction = which;
+ mostNegativeStictionCapacity = capacity;
+ }
+ }
+
+ #ifndef NDEBUG
+ if (mostNegativeStictionCapacity == 0)
+ printf(" All active stiction constraints are satisfied.\n");
+ else
+ printf(" Active stiction %d was worst violator with capacity=%g\n",
+ worstActiveStiction, mostNegativeStictionCapacity);
+ #endif
+
+ if (mostNegativeStictionCapacity==0) {
+ SimTK_DEBUG("DONE. Current active set is a winner.\n");
+ break;
+ }
+
+ SimTK_DEBUG1(" Disable stiction %d\n", worstActiveStiction);
+ const MyHybridVertexContactElementImpl& fric =
+ getContactElement(worstActiveStiction);
+
+ ++nStictionDisabled;
+ fric.disableStiction(s);
+
+ // Go back for another pass.
+ }
+
+ // Reset all the slip directions so that all slip->stick event witnesses
+ // will be positive when integration resumes.
+ for (unsigned i=0; i < candidates.m_contact.size(); ++i) {
+ const int which = candidates.m_contact[i];
+ const MyHybridVertexContactElementImpl& fric =
+ getContactElement(which);
+ fric.updatePrevSlipDirFromRecorded(s);
+ }
+
+ // Always leave at acceleration stage.
+ m_mbs.realize(s, Stage::Acceleration);
+
+ SimTK_DEBUG1("... Done; %d stictions broken.\n", nStictionDisabled);
+
+ showConstraintStatus(s, "EXIT findActiveCandidates()");
+}
+
+
+
+//-------------------------- SHOW CONSTRAINT STATUS ----------------------------
+void MyUnilateralConstraintSet::
+showConstraintStatus(const State& s, const String& place) const
+{
+#ifndef NDEBUG
+ printf("\n%s: uni status @t=%.15g\n", place.c_str(), s.getTime());
+ m_mbs.realize(s, Stage::Acceleration);
+ for (int i=0; i < getNumContactElements(); ++i) {
+ const MyHybridVertexContactElementImpl& contact = getContactElement(i);
+ const bool isActive = contact.isInContact(s);
+ printf(" %6s %2d %s, h=%g dh=%g f=%g\n",
+ isActive?"ACTIVE":"off", i, "hybrid",
+ contact.getHeight(s),contact.getHeightDot(s),
+ isActive?contact.getNormalForce(s):Zero);
+ if (!isActive) continue;
+
+ const bool isSticking = contact.isSticking(s);
+ printf(" %8s friction %2d, |v|=%g witness=%g\n",
+ isSticking?"STICKING":"sliding", i,
+ contact.getActualSlipSpeed(s),
+ isSticking?contact.calcStictionForceWitness(s)
+ :contact.calcSlipSpeedWitness(s));
+ contact.writeFrictionInfo(s, " ", std::cout);
+ }
+ printf("\n");
+#endif
+}
+
+//------------------------ STRIBECK FRICTION STATICS ---------------------------
+// This is extracted from Simbody's continuous friction model so that we can
+// compare it with the new implementation.
+
+// Input x goes from 0 to 1; output goes 0 to 1 but smoothed with an S-shaped
+// quintic with two zero derivatives at either end. Cost is 7 flops.
+inline static Real step5(Real x) {
+ assert(0 <= x && x <= 1);
+ const Real x3=x*x*x;
+ return x3*(10+x*(6*x-15)); //10x^3-15x^4+6x^5
+}
+
+// This is the sum of two curves:
+// (1) a wet friction term mu_wet which is a linear function of velocity:
+// mu_wet = uv*v
+// (2) a dry friction term mu_dry which is a quintic spline with 4 segments:
+// mu_dry =
+// (a) v=0..1: smooth interpolation from 0 to us
+// (b) v=1..3: smooth interp from us down to ud (Stribeck)
+// (c) v=3..Inf ud
+// CAUTION: uv and v must be dimensionless in multiples of transition velocity.
+// The function mu = mu_wet + mu_dry is zero at v=0 with 1st deriv (slope) uv
+// and 2nd deriv (curvature) 0. At large velocities v>>0 the value is
+// ud+uv*v, again with slope uv and zero curvature. We want mu(v) to be c2
+// continuous, so mu_wet(v) must have zero slope and curvature at v==0 and
+// at v==3 where it takes on a constant value ud.
+//
+// Cost: stiction 12 flops
+// stribeck 14 flops
+// sliding 3 flops
+// Curve looks like this:
+//
+// us+uv ***
+// * * *
+// * * *____| slope = uv at Inf
+// * * *
+// ud+3uv * * *
+// *
+// *
+// *
+// 0 *____| slope = uv at 0
+//
+// | | |
+// v=0 1 3
+//
+// This calculates a composite coefficient of friction that you should use
+// to scale the normal force to produce the friction force.
+static Real stribeck(Real us, Real ud, Real uv, Real v) {
+ const Real mu_wet = uv*v;
+ Real mu_dry;
+ if (v >= 3) mu_dry = ud; // sliding
+ else if (v >= 1) mu_dry = us - (us-ud)*step5((v-1)/2); // Stribeck
+ else mu_dry = us*step5(v); // 0 <= v < 1 (stiction)
+ return mu_dry + mu_wet;
+}
\ No newline at end of file
diff --git a/Simbody/tests/adhoc/TimsBoxPGS.cpp b/Simbody/tests/adhoc/TimsBoxPGS.cpp
new file mode 100644
index 0000000..1cb176e
--- /dev/null
+++ b/Simbody/tests/adhoc/TimsBoxPGS.cpp
@@ -0,0 +1,2172 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) - Tim's Box PGS *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012-14 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* Solve TimsBox contact & impact using the Projected Gauss Seidel iterative
+solver rather than a direct solver. */
+
+//#define NDEBUG 1
+
+#include "Simbody.h"
+
+#include <string>
+#include <iostream>
+#include <exception>
+
+using std::cout;
+using std::endl;
+
+using namespace SimTK;
+
+//#define USE_TIMS_PARAMS
+#define ANIMATE // off to get more accurate CPU time (you can still playback)
+
+//#define HERTZ
+#define POISSON
+#define NEWTON
+
+//==============================================================================
+// MY CONTACT ELEMENT
+//==============================================================================
+// This abstract class hides the details about which kind of contact constraint
+// we're dealing with, while giving us enough to work with for deciding what's
+// on and off and generating impulses.
+//
+// There is always a scalar associated with the constraint for making
+// decisions. There may be a friction element associated with this contact.
+namespace {
+class MyFrictionElement;
+class MyContactElement {
+public:
+ MyContactElement(Constraint uni, Real multSign, Real coefRest)
+ : m_uni(uni), m_multSign(multSign), m_coefRest(coefRest),
+ m_index(-1), m_friction(0),
+ m_velocityDependentCOR(NaN), m_restitutionDone(false)
+ { m_uni.setDisabledByDefault(true); }
+
+ MultiplierIndex getMultIndex(const State& s) const {
+ int mp, mv, ma;
+ MultiplierIndex px0, vx0, ax0;
+ m_uni.getNumConstraintEquationsInUse(s,mp,mv,ma);
+ assert(mp==1 && mv==0 && ma==0); // don't call if not enabled
+ m_uni.getIndexOfMultipliersInUse(s, px0, vx0, ax0);
+ assert(px0.isValid() && !vx0.isValid() && !ax0.isValid());
+ return px0;
+ }
+
+ virtual ~MyContactElement() {}
+
+ // (Re)initialize base & concrete class. If overridden, be sure to
+ // invoke base class first.
+ virtual void initialize() {
+ setRestitutionDone(false);
+ m_velocityDependentCOR = NaN;
+ }
+
+ // Provide a human-readable string identifying the type of contact
+ // constraint.
+ virtual String getContactType() const = 0;
+
+ // These must be constructed so that a negative value means the
+ // unilateral constraint condition is violated.
+ virtual Real getPerr(const State& state) const = 0;
+ virtual Real getVerr(const State& state) const = 0;
+ virtual Real getAerr(const State& state) const = 0;
+
+ // This returns a point in the ground frame at which you might want to
+ // say the constraint is "located", for purposes of display. This should
+ // return something useful even if the constraint is currently off.
+ virtual Vec3 whereToDisplay(const State& state) const = 0;
+
+ // This is used by some constraints to collect position information that
+ // may be used later to set instance variables when enabling the underlying
+ // Simbody constraint. All constraints zero impulses here.
+ virtual void initializeForImpact(const State& state, Real captureVelocity) {
+ if (-captureVelocity <= getVerr(state) && getVerr(state) < 0) {
+ m_velocityDependentCOR = 0;
+ SimTK_DEBUG3("CAPTURING %d because %g <= v=%g < 0\n",
+ m_index, -captureVelocity, getVerr(state));
+ } else {
+ m_velocityDependentCOR = m_coefRest;
+ }
+
+ setRestitutionDone(false);
+ }
+
+ // Returns zero if the constraint is not currently enabled. Otherwise
+ // return the signed constraint force, with a negative value indicating
+ // that the unilateral force condition is violated.
+ Real getForce(const State& s) const {
+ if (isDisabled(s)) return 0;
+ return m_multSign*s.updMultipliers()[getMultIndex(s)];
+ }
+
+ // Append to geometry array.
+ virtual void showContactForce(const State& s,
+ Array_<DecorativeGeometry>& geometry) const {}
+
+ bool isProximal(const State& state, Real posTol) const
+ { return /*!isDisabled(state) || */getPerr(state) <= posTol; }
+ bool isCandidate(const State& state, Real posTol, Real velTol) const
+ { return isProximal(state, posTol) && getVerr(state) <= velTol; }
+
+
+ void enable(State& state) const {m_uni.enable(state);}
+ void disable(State& state) const {m_uni.disable(state);}
+ bool isDisabled(const State& state) const {return m_uni.isDisabled(state);}
+
+ void setMyDesiredDeltaV(const State& s,
+ Vector& desiredDeltaV) const
+ { Vector myDesiredDV(1); myDesiredDV[0] = m_multSign*getVerr(s);
+ m_uni.setMyPartInConstraintSpaceVector(s, myDesiredDV,
+ desiredDeltaV); }
+
+ Real getMyValueFromConstraintSpaceVector(const State& state,
+ const Vector& lambda) const
+ { Vector myValue(1);
+ m_uni.getMyPartFromConstraintSpaceVector(state, lambda, myValue);
+ return -m_multSign*myValue[0]; }
+
+ Real getMaxCoefRest() const {return m_coefRest;}
+ Real getEffectiveCoefRest() const {return m_velocityDependentCOR;}
+ void setRestitutionDone(bool isDone) {m_restitutionDone=isDone;}
+ bool isRestitutionDone() const {return m_restitutionDone;}
+
+ // Record position within the set of unilateral contact constraints.
+ void setContactIndex(int index) {m_index=index;}
+ int getContactIndex() const {return m_index;}
+ // If there is a friction element for which this is the master contact,
+ // record it here.
+ void setFrictionElement(MyFrictionElement& friction)
+ { m_friction = &friction; }
+ // Return true if there is a friction element associated with this contact
+ // element.
+ bool hasFrictionElement() const {return m_friction != 0;}
+ // Get the associated friction element.
+ const MyFrictionElement& getFrictionElement() const
+ { assert(hasFrictionElement()); return *m_friction; }
+ MyFrictionElement& updFrictionElement() const
+ { assert(hasFrictionElement()); return *m_friction; }
+
+protected:
+ Constraint m_uni;
+ const Real m_multSign; // 1 or -1
+ const Real m_coefRest;
+
+ int m_index; // contact index in unilateral constraint set
+ MyFrictionElement* m_friction; // if any (just a reference, not owned)
+
+ // Runtime -- initialized at start of impact handler.
+ Real m_velocityDependentCOR; // Calculated at start of impact
+ bool m_restitutionDone;
+};
+
+
+
+//==============================================================================
+// MY FRICTION ELEMENT
+//==============================================================================
+// Generated forces during sliding, and the force limit during rolling, depend
+// on a scalar normal force N that comes from a
+// separate "normal force master", which might be one of the following:
+// - a unilateral constraint
+// - a bilateral constraint
+// - a mobilizer
+// - a compliant force element
+// If the master is an inactive unilateral constraint, or if N=0, then no
+// friction forces are generated. In this example, we're only going to use
+// a unilateral contact constraint as the "normal force master".
+//
+// For all but the compliant normal force master, the normal force N is
+// acceleration-dependent and thus may be coupled to the force produced by a
+// sliding friction element. This may require iteration to ensure consistency
+// between the sliding friction force and its master contact's normal force.
+//
+// A Coulomb friction element depends on a scalar slip speed defined by the
+// normal force master (this might be the magnitude of a generalized speed or
+// slip velocity vector). When the slip velocity goes to zero, the stiction
+// constraint is enabled if its constraint force magnitude can be kept to
+// mu_s*|N| or less. Otherwise, or if the slip velocity is nonzero, the sliding
+// force is enabled instead and generates a force of constant magnitude mu_d*|N|
+// that opposes the slip direction, or impending slip direction, as defined by
+// the master.
+class MyFrictionElement {
+public:
+ MyFrictionElement(Real mu_d, Real mu_s, Real mu_v)
+ : mu_d(mu_d), mu_s(mu_s), mu_v(mu_v), m_index(-1) {}
+
+ virtual ~MyFrictionElement() {}
+
+ // (Re)initialize base & concrete class. If overridden, be sure to
+ // invoke base class first.
+ virtual void initialize() {
+ }
+
+ Real getDynamicFrictionCoef() const {return mu_d;}
+ Real getStaticFrictionCoef() const {return mu_s;}
+ Real getViscousFrictionCoef() const {return mu_v;}
+
+ // This returns a point in the ground frame at which you might want to
+ // say the friction is "located", for purposes of display.
+ virtual Vec3 whereToDisplay(const State& state) const = 0;
+
+ // Return true if the stiction constraint is enabled.
+ virtual bool isEnabled(const State&) const = 0;
+
+ virtual void setInstanceParameters(State&) const {}
+ virtual void enable(State&) const = 0;
+ virtual void disable(State&) const = 0;
+
+ // Return true if the normal force master *could* be involved in an
+ // impact event (because it is touching).
+ virtual bool isMasterProximal(const State&, Real posTol) const = 0;
+
+ // Return true if the normal force master is currently generating a
+ // normal force (or impulse) so that this friction element might be
+ // generating a force also.
+ virtual bool isMasterActive(const State&) const = 0;
+
+
+ // This is used by some stiction constraints to collect position information
+ // that may be used later to set instance variables when enabling the
+ // underlying Simbody constraint. Recorded impulses should be zeroed.
+ virtual void initializeFriction(const State& state) = 0;
+
+ // If this friction element's stiction constraint is enabled, set its
+ // constraint-space velocity entry(s) in desiredDeltaV to the current
+ // slip velocity (which might be a scalar or 2-vector).
+ virtual void setMyDesiredDeltaV(const State& s,
+ Vector& desiredDeltaV) const = 0;
+
+ // Output the status, friction force, slip velocity, prev slip direction
+ // (scalar or vector) to the given ostream, indented as indicated and
+ // followed by a newline. May generate multiple lines.
+ virtual std::ostream& writeFrictionInfo(const State& state,
+ const String& indent,
+ std::ostream& o) const = 0;
+
+ // Optional: give some kind of visual representation for the friction force.
+ virtual void showFrictionForce(const State& state,
+ Array_<DecorativeGeometry>& geometry, const Vec3& color) const {}
+
+
+ void setFrictionIndex(int index) {m_index=index;}
+ int getFrictionIndex() const {return m_index;}
+
+private:
+ Real mu_d, mu_s, mu_v;
+ int m_index; // friction index within unilateral constraint set
+};
+
+
+
+//==============================================================================
+// MY UNILATERAL CONSTRAINT SET
+//==============================================================================
+
+// These are indices into the unilateral constraint set arrays.
+struct MyElementSubset {
+ void clear() {m_contact.clear();m_friction.clear();}
+ Array_<int> m_contact;
+ Array_<int> m_friction; // friction elements that might stick
+};
+
+class MyUnilateralConstraintSet {
+public:
+ // Capture velocity: if the normal approach velocity
+ // is smaller, the coefficient of restitution is set to zero for the
+ // upcoming impact. Transition velocity: if a slip velocity is smaller than
+ // this use the static coefficient of friction, otherwise use dynamic
+ // plus viscous.
+ MyUnilateralConstraintSet(const MultibodySystem& mbs,
+ Real captureVelocity, Real transitionVelocity)
+ : m_mbs(mbs), m_captureVelocity(captureVelocity),
+ m_transitionVelocity(transitionVelocity) {}
+
+ // This class takes over ownership of the heap-allocated contact element.
+ int addContactElement(MyContactElement* contact) {
+ const int index = (int)m_contact.size();
+ m_contact.push_back(contact);
+ contact->setContactIndex(index);
+ return index;
+ }
+ // This class takes over ownership of the heap-allocated friction element.
+ int addFrictionElement(MyFrictionElement* friction) {
+ const int index = (int)m_friction.size();
+ m_friction.push_back(friction);
+ friction->setFrictionIndex(index);
+ return index;
+ }
+
+ Real getCaptureVelocity() const {return m_captureVelocity;}
+ void setCaptureVelocity(Real v) {m_captureVelocity=v;}
+ Real getTransitionVelocity() const {return m_transitionVelocity;}
+ void setTransitionVelocity(Real v) {m_transitionVelocity=v;}
+
+ int getNumContactElements() const {return (int)m_contact.size();}
+ int getNumFrictionElements() const {return (int)m_friction.size();}
+ const MyContactElement& getContactElement(int ix) const
+ { return *m_contact[ix]; }
+ const MyFrictionElement& getFrictionElement(int ix) const
+ { return *m_friction[ix]; }
+
+ // Allow writable access to elements from const set so we can record
+ // runtime results (e.g. impulses).
+ MyContactElement& updContactElement(int ix) const {return *m_contact[ix];}
+ MyFrictionElement& updFrictionElement(int ix) const {return *m_friction[ix];}
+
+ // Initialize all runtime fields in the contact & friction elements.
+ void initialize()
+ {
+ for (unsigned i=0; i < m_contact.size(); ++i)
+ m_contact[i]->initialize();
+ for (unsigned i=0; i < m_friction.size(); ++i)
+ m_friction[i]->initialize();
+ }
+
+ // Return the contact and friction elements that might be involved in an
+ // impact occurring in this configuration. They are the contact elements
+ // for which perr <= posTol, and friction elements whose normal force
+ // masters can be involved in the impact. State must be realized through
+ // Position stage.
+ void findProximalElements(const State& state,
+ Real posTol,
+ MyElementSubset& proximals,
+ MyElementSubset& distals) const
+ {
+ proximals.clear(); distals.clear();
+ for (unsigned i=0; i < m_contact.size(); ++i)
+ if (m_contact[i]->isProximal(state,posTol))
+ proximals.m_contact.push_back(i);
+ else distals.m_contact.push_back(i);
+ for (unsigned i=0; i < m_friction.size(); ++i)
+ if (m_friction[i]->isMasterProximal(state,posTol))
+ proximals.m_friction.push_back(i);
+ else distals.m_friction.push_back(i);
+ // Any friction elements might stick if they are proximal since
+ // we'll be changing velocities.
+ }
+
+ // In Debug mode, produce a useful summary of the current state of the
+ // contact and friction elements.
+ void showConstraintStatus(const State& state, const String& place) const;
+
+ ~MyUnilateralConstraintSet() {
+ for (unsigned i=0; i < m_contact.size(); ++i)
+ delete m_contact[i];
+ for (unsigned i=0; i < m_friction.size(); ++i)
+ delete m_friction[i];
+ }
+
+ const MultibodySystem& getMultibodySystem() const {return m_mbs;}
+private:
+ const MultibodySystem& m_mbs;
+ Real m_captureVelocity;
+ Real m_transitionVelocity;
+ Array_<MyContactElement*> m_contact;
+ Array_<MyFrictionElement*> m_friction;
+};
+
+
+//==============================================================================
+// MY POINT CONTACT
+//==============================================================================
+// Define a unilateral constraint to represent contact of a follower point on
+// one body with a plane fixed to another body.
+class MyPointContact : public MyContactElement {
+ typedef MyContactElement Super;
+public:
+ MyPointContact(
+ MobilizedBody& planeBodyB, const UnitVec3& normal_B, Real height,
+ MobilizedBody& followerBodyF, const Vec3& point_F,
+ Real coefRest)
+ : MyContactElement(
+ Constraint::PointInPlane(planeBodyB, normal_B, height,
+ followerBodyF, point_F),
+ Real(-1), // multiplier sign
+ coefRest),
+ m_planeBody(planeBodyB), m_frame(normal_B, ZAxis), m_height(height),
+ m_follower(followerBodyF), m_point(point_F)
+ {
+ }
+
+ Real getPerr(const State& s) const OVERRIDE_11 {
+ const Vec3 p = m_follower.findStationLocationInAnotherBody
+ (s, m_point, m_planeBody);
+ return ~p*m_frame.z() - m_height;
+ }
+ Real getVerr(const State& s) const OVERRIDE_11 {
+ const Vec3 v = m_follower.findStationVelocityInAnotherBody
+ (s, m_point, m_planeBody);
+ return ~v*m_frame.z(); // normal is constant in P
+ }
+ Real getAerr(const State& s) const OVERRIDE_11 {
+ const Vec3 a = m_follower.findStationAccelerationInAnotherBody
+ (s, m_point, m_planeBody);
+ return ~a*m_frame.z(); // normal is constant in P
+ }
+
+ String getContactType() const OVERRIDE_11 {return "Point";}
+ Vec3 whereToDisplay(const State& state) const OVERRIDE_11 {
+ return m_follower.findStationLocationInGround(state,m_point);
+ }
+
+ // Will be zero if the stiction constraints are on.
+ Vec2 getSlipVelocity(const State& s) const {
+ const Vec3 v = m_follower.findStationVelocityInAnotherBody
+ (s, m_point, m_planeBody);
+ return Vec2(~v*m_frame.x(), ~v*m_frame.y());
+ }
+ // Will be zero if the stiction constraints are on.
+ Vec2 getSlipAcceleration(const State& s) const {
+ const Vec3 a = m_follower.findStationAccelerationInAnotherBody
+ (s, m_point, m_planeBody);
+ return Vec2(~a*m_frame.x(), ~a*m_frame.y());
+ }
+
+ Vec3 getContactPointInPlaneBody(const State& s) const
+ { return m_follower.findStationLocationInAnotherBody
+ (s, m_point, m_planeBody); }
+
+ void showContactForce(const State& s,
+ Array_<DecorativeGeometry>& geometry)
+ const OVERRIDE_11
+ {
+ const Real Scale = .1;
+ const Real f = getForce(s);
+ if (std::abs(f) < SignificantReal)
+ return;
+ const Vec3 stationG = whereToDisplay(s);
+ const Vec3 endG = stationG + Scale*f*m_frame.z();
+ geometry.push_back(DecorativeLine(endG + Vec3(0,.05,0),
+ stationG + Vec3(0,.05,0))
+ .setColor(Magenta));
+ }
+
+ const MobilizedBody& getBody() const {return m_follower;}
+ MobilizedBody& updBody() {return m_follower;}
+ const Vec3& getBodyStation() const {return m_point;}
+
+ const MobilizedBody& getPlaneBody() const {return m_planeBody;}
+ MobilizedBody& updPlaneBody() const {return m_planeBody;}
+ const Rotation& getPlaneFrame() const {return m_frame;}
+ Real getPlaneHeight() const {return m_height;}
+
+private:
+ MobilizedBody& m_planeBody; // body P
+ const Rotation m_frame; // z is normal; expressed in P
+ const Real m_height;
+
+ MobilizedBody& m_follower; // body F
+ const Vec3 m_point; // measured & expressed in F
+};
+
+
+//==============================================================================
+// MY POINT CONTACT FRICTION
+//==============================================================================
+// This friction element expects its master to be a unilateral point contact
+// constraint. It provides slipping forces or stiction constraint forces acting
+// in the plane, based on the normal force being applied by the point contact
+// constraint.
+class MyPointContactFriction : public MyFrictionElement {
+ typedef MyFrictionElement Super;
+public:
+ // The constructor allocates two NoSlip1D constraints.
+ MyPointContactFriction(MyPointContact& contact,
+ Real mu_d, Real mu_s, Real mu_v, Real vtol, //TODO: shouldn't go here
+ GeneralForceSubsystem& forces)
+ : MyFrictionElement(mu_d,mu_s,mu_v), m_contact(contact),
+ m_noslipX(contact.updPlaneBody(), Vec3(0), contact.getPlaneFrame().x(),
+ contact.updPlaneBody(), contact.updBody()),
+ m_noslipY(contact.updPlaneBody(), Vec3(0), contact.getPlaneFrame().y(),
+ contact.updPlaneBody(), contact.updBody())
+ {
+ assert((0 <= mu_d && mu_d <= mu_s) && (0 <= mu_v));
+ contact.setFrictionElement(*this);
+ m_noslipX.setDisabledByDefault(true);
+ m_noslipY.setDisabledByDefault(true);
+ initializeRuntimeFields();
+ }
+
+ ~MyPointContactFriction() {}
+
+ void initialize() OVERRIDE_11 {
+ Super::initialize();
+ initializeRuntimeFields();
+ }
+
+ Vec3 whereToDisplay(const State& state) const OVERRIDE_11 {
+ return m_contact.whereToDisplay(state);
+ }
+
+
+ MultiplierIndex getMultIndexX(const State& s) const {
+ int mp, mv, ma;
+ MultiplierIndex px0, vx0, ax0;
+ m_noslipX.getNumConstraintEquationsInUse(s,mp,mv,ma);
+ assert(mp==0 && mv==1 && ma==0); // don't call if not enabled
+ m_noslipX.getIndexOfMultipliersInUse(s, px0, vx0, ax0);
+ assert(!px0.isValid() && vx0.isValid() && !ax0.isValid());
+ return vx0;
+ }
+
+ MultiplierIndex getMultIndexY(const State& s) const {
+ int mp, mv, ma;
+ MultiplierIndex px0, vx0, ax0;
+ m_noslipY.getNumConstraintEquationsInUse(s,mp,mv,ma);
+ assert(mp==0 && mv==1 && ma==0); // don't call if not enabled
+ m_noslipY.getIndexOfMultipliersInUse(s, px0, vx0, ax0);
+ assert(!px0.isValid() && vx0.isValid() && !ax0.isValid());
+ return vx0;
+ }
+
+ // The way we constructed the NoSlip1D constraints makes the multipliers be
+ // the force on Ground; we negate here so we'll get the force on the sliding
+ // body instead.
+ Vec2 getFrictionForce(const State& s) const {
+ if (m_noslipX.isDisabled(s)) return Vec2(0);
+ Vec2 fOnG(s.updMultipliers()[getMultIndexX(s)],
+ s.updMultipliers()[getMultIndexY(s)]);
+ return -fOnG;
+ }
+
+ // Implement pure virtuals from MyFrictionElement base class.
+
+ bool isEnabled(const State& s) const OVERRIDE_11
+ { return !m_noslipX.isDisabled(s); } // X,Z always on or off together
+
+ // Note that initializeForStiction() must have been called first.
+ void setInstanceParameters(State& s) const OVERRIDE_11
+ { m_noslipX.setContactPoint(s, m_contactPointInPlane);
+ m_noslipY.setContactPoint(s, m_contactPointInPlane); }
+
+ void enable(State& s) const OVERRIDE_11
+ { m_noslipX.setContactPoint(s, m_contactPointInPlane);
+ m_noslipY.setContactPoint(s, m_contactPointInPlane);
+ m_noslipX.enable(s); m_noslipY.enable(s); }
+
+ void disable(State& s) const OVERRIDE_11
+ { m_noslipX.disable(s); m_noslipY.disable(s); }
+
+ bool isMasterProximal(const State& s, Real posTol) const OVERRIDE_11
+ { return m_contact.isProximal(s, posTol); }
+
+ bool isMasterActive(const State& s) const OVERRIDE_11
+ { return !m_contact.isDisabled(s); }
+
+
+ // Set the friction application point to be the projection of the contact
+ // point onto the contact plane. This will be used the next time friction
+ // is enabled. Requires state realized to Position stage.
+ void initializeFriction(const State& s) OVERRIDE_11 {
+ const Vec3 p = m_contact.getContactPointInPlaneBody(s);
+ m_contactPointInPlane = p;
+ }
+
+ // Fill in deltaV to eliminate slip velocity using the stiction
+ // constraints.
+ void setMyDesiredDeltaV(const State& s,
+ Vector& desiredDeltaV) const OVERRIDE_11
+ {
+ if (!isEnabled(s)) return;
+
+ const Vec2 dv = -m_contact.getSlipVelocity(s); // X,Z
+ Vector myDesiredDV(1); // Nuke translational velocity also.
+ myDesiredDV[0] = dv[0];
+ m_noslipX.setMyPartInConstraintSpaceVector(s, myDesiredDV, desiredDeltaV);
+ myDesiredDV[0] = dv[1];
+ m_noslipY.setMyPartInConstraintSpaceVector(s, myDesiredDV, desiredDeltaV);
+ }
+
+ Real getMyImpulseMagnitudeFromConstraintSpaceVector(const State& state,
+ const Vector& lambda) const
+ { Vector myImpulseX(1), myImpulseY(1);
+ m_noslipX.getMyPartFromConstraintSpaceVector(state, lambda, myImpulseX);
+ m_noslipY.getMyPartFromConstraintSpaceVector(state, lambda, myImpulseY);
+ const Vec2 tI(myImpulseX[0], myImpulseY[0]);
+ return tI.norm();
+ }
+
+
+ std::ostream& writeFrictionInfo(const State& s, const String& indent,
+ std::ostream& o) const OVERRIDE_11
+ {
+ o << indent;
+ if (!isMasterActive(s)) o << "OFF";
+ else if (isEnabled(s)) o << "STICK";
+ else o << "SLIP";
+
+ const Vec2 v = m_contact.getSlipVelocity(s);
+ const Vec2 f = getFrictionForce(s);
+ o << " V=" << ~v << " F=" << ~f << endl;
+ return o;
+ }
+
+
+ void showFrictionForce(const State& s, Array_<DecorativeGeometry>& geometry,
+ const Vec3& color)
+ const OVERRIDE_11
+ {
+ const Real Scale = 0.1;
+ const Vec2 f = getFrictionForce(s);
+ if (f.normSqr() < square(SignificantReal))
+ return;
+ const MobilizedBody& bodyB = m_contact.getBody();
+ const Vec3& stationB = m_contact.getBodyStation();
+ const Vec3 stationG = bodyB.getBodyTransform(s)*stationB;
+ Vec3 F = f[0]*m_contact.getPlaneFrame().x()
+ + f[1]*m_contact.getPlaneFrame().y();
+ const Vec3 endG = stationG - Scale*F;
+ geometry.push_back(DecorativeLine(endG + Vec3(0,.05,0),
+ stationG + Vec3(0,.05,0))
+ .setColor(color));
+ }
+
+ const MyPointContact& getMyPointContact() const {return m_contact;}
+
+private:
+ void initializeRuntimeFields() {
+ m_contactPointInPlane = Vec3(0);
+ }
+ const MyPointContact& m_contact;
+
+ Constraint::NoSlip1D m_noslipX, m_noslipY; // stiction
+
+ // Runtime
+ Vec3 m_contactPointInPlane; // point on plane body where friction will act
+};
+
+
+
+//==============================================================================
+// BODY WATCHER
+//==============================================================================
+// Prior to rendering each frame, point the camera at the given body's
+// origin.
+class BodyWatcher : public Visualizer::FrameController {
+public:
+ explicit BodyWatcher(const MobilizedBody& body) : m_body(body) {}
+
+ void generateControls(const Visualizer& viz,
+ const State& state,
+ Array_< DecorativeGeometry >& geometry) OVERRIDE_11
+ {
+ const Vec3 Bo = m_body.getBodyOriginLocation(state);
+ const Vec3 p_GC = Bo + Vec3(0, 1.5, 7); // above and back
+ const Rotation R_GC(UnitVec3(0,1,0), YAxis,
+ p_GC-Bo, ZAxis);
+ viz.setCameraTransform(Transform(R_GC,p_GC));
+ //viz.pointCameraAt(Bo, Vec3(0,1,0));
+ }
+private:
+ const MobilizedBody m_body;
+};
+
+//==============================================================================
+// MY PUSH FORCE
+//==============================================================================
+// This is a force element that generates a constant force on a body for a
+// specified time interval. It is useful to perturb the system to force it
+// to transition from sticking to sliding, for example.
+class MyPushForceImpl : public Force::Custom::Implementation {
+public:
+ MyPushForceImpl(const MobilizedBody& bodyB,
+ const Vec3& stationB,
+ const Vec3& forceG, // force direction in Ground!
+ Real onTime,
+ Real offTime)
+ : m_bodyB(bodyB), m_stationB(stationB), m_forceG(forceG),
+ m_on(onTime), m_off(offTime)
+ { }
+
+ //--------------------------------------------------------------------------
+ // Custom force virtuals
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+ OVERRIDE_11
+ {
+ if (!(m_on <= state.getTime() && state.getTime() <= m_off))
+ return;
+
+ m_bodyB.applyForceToBodyPoint(state, m_stationB, m_forceG, bodyForces);
+ }
+
+ // No potential energy.
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11 {return 0;}
+
+ void calcDecorativeGeometryAndAppend
+ (const State& state, Stage stage,
+ Array_<DecorativeGeometry>& geometry) const OVERRIDE_11
+ {
+ const Real ScaleFactor = 0.1;
+ if (stage != Stage::Time) return;
+ if (!(m_on <= state.getTime() && state.getTime() <= m_off))
+ return;
+ const Vec3 stationG = m_bodyB.findStationLocationInGround(state, m_stationB);
+ geometry.push_back(DecorativeLine(stationG-ScaleFactor*m_forceG, stationG)
+ .setColor(Yellow)
+ .setLineThickness(3));
+ }
+private:
+ const MobilizedBody& m_bodyB;
+ const Vec3 m_stationB;
+ const Vec3 m_forceG;
+ Real m_on;
+ Real m_off;
+};
+
+
+//==============================================================================
+// PGS AUGMENTED MULTIBODY SYSTEM
+//==============================================================================
+/* This is a Simbody MultibodySystem able to provide some additional information
+about its constraints, in a form suitable for a PGS solver. The extra
+information allows us to emulate conditional constraints using Simbody's
+unconditional constraints plus its constraint enable/disable feature.
+
+We first construct the system with all possible constraints included, but with
+conditional ones initially disabled. Then for any given configuration q, we
+determine which conditional constraints are "proximal", meaning that they may
+participate in applying forces to the system in that configuration. Those
+constraints are enabled and all other conditional constraints are disabled.
+Simbody can then calculate the constraint Jacobian G(q), and the
+constraint-space compliance matrix A(q)=GM\~G.
+
+Extra constraint info:
+Unconditional:
+ - which multipliers / holonomic or not
+Bounded scalar (stop/contact/clutch/motor):
+ - lower and upper bounds
+ - which multipliers / holonomic or not
+ - if holo, restitution coefficients, capture velocity
+Length-limited vector:
+ - function L(t,q,u) giving max length
+ - which multipliers / holonomic or not
+Frictional constraint:
+ - master constraint (unconditional or bounded, holonomic, dim 1,2, or 3)
+ - friction coefficients, transition velocity
+ - which multipliers
+*/
+const Real DefaultCaptureVelocity = .01,
+ DefaultTransitionVelocity = .01;
+class PGSAugmentedMultibodySystem : public MultibodySystem {
+public:
+ PGSAugmentedMultibodySystem() : m_matter(0), m_forces(0), m_unis(0) {
+ m_matter = new SimbodyMatterSubsystem(*this);
+ m_forces = new GeneralForceSubsystem(*this);
+ m_unis = new MyUnilateralConstraintSet(*this,
+ DefaultCaptureVelocity, DefaultTransitionVelocity);
+ m_matter->setShowDefaultGeometry(false);
+ }
+
+ virtual ~PGSAugmentedMultibodySystem()
+ { delete m_unis; delete m_forces; delete m_matter; }
+
+ virtual const MobilizedBody& getBodyToWatch() const
+ { return m_matter->getGround(); }
+
+ virtual void calcInitialState(State& state) const = 0;
+
+ const SimbodyMatterSubsystem& getMatterSubsystem() const {return *m_matter;}
+ SimbodyMatterSubsystem& updMatterSubsystem() {return *m_matter;}
+
+ const GeneralForceSubsystem& getForceSubsystem() const {return *m_forces;}
+ GeneralForceSubsystem& updForceSubsystem() {return *m_forces;}
+
+ const MyUnilateralConstraintSet& getUnis() const {return *m_unis;}
+ MyUnilateralConstraintSet& updUnis() {return *m_unis;}
+
+private:
+ //TODO: this shouldn't require pointers.
+ SimbodyMatterSubsystem* m_matter;
+ GeneralForceSubsystem* m_forces;
+ MyUnilateralConstraintSet* m_unis;
+};
+
+//==============================================================================
+// PGS TIME STEPPER
+//==============================================================================
+
+struct Bounded {
+ Bounded(MultiplierIndex ix, Real lb, Real ub, Real effCOR)
+ : m_ix(ix), m_lb(lb), m_ub(ub), m_effCOR(effCOR), m_hitBound(false)
+ { assert(m_lb<=m_ub); assert(0<=m_effCOR && m_effCOR<=1); }
+ MultiplierIndex m_ix; // which constraint multiplier
+ Real m_lb, m_ub; // lower, upper bounds; lb <= ub
+ Real m_effCOR; // velocity-dependent COR
+ bool m_hitBound;
+};
+
+struct LengthLimited {
+ LengthLimited(const Array_<MultiplierIndex>& components, Real maxLength)
+ : m_components(components), m_maxLength(maxLength), m_hitLimit(false)
+ { assert(m_components.size()<=3); assert(m_maxLength>=0); }
+ Array_<MultiplierIndex> m_components;
+ Real m_maxLength;
+ bool m_hitLimit;
+};
+
+struct Frictional {
+ Frictional(const Array_<MultiplierIndex>& frictionComponents,
+ const Array_<MultiplierIndex>& normalComponents,
+ Real muEff)
+ : m_Fk(frictionComponents), m_Nk(normalComponents),
+ m_effMu(muEff), m_wasLimited(false)
+ { assert(m_Fk.size()<=3 && m_Nk.size()<=3); assert(m_effMu>=0); }
+ Array_<MultiplierIndex> m_Fk, m_Nk;
+ Real m_effMu;
+ bool m_wasLimited;
+};
+
+/**
+**/
+class PGSTimeStepper {
+public:
+ explicit PGSTimeStepper(const PGSAugmentedMultibodySystem& ambs)
+ : m_ambs(ambs),
+ m_PGSConvergenceTol(1e-6), m_PGSMaxIters(100), m_PGSSOR(1),
+ m_accuracy(1e-2), m_consTol(1e-3), m_useNewton(false)
+ { resetPGSStats(); }
+
+ void setUseNewtonRestitution(bool useNewton) {m_useNewton=useNewton;}
+ bool getUseNewtonRestitution() const {return m_useNewton;}
+
+ /** Set integration accuracy; requires variable length steps. **/
+ void setAccuracy(Real accuracy) {m_accuracy=accuracy;}
+ /** Set the tolerance to which constraints must be satisfied. **/
+ void setConstraintTol(Real consTol) {m_consTol=consTol;}
+
+ Real getAccuracy() const {return m_accuracy;}
+ Real getConstraintTol() const {return m_consTol;}
+
+ void resetPGSStats() const {
+ for (int i=0; i<3; ++i) {
+ m_PGSNumCalls[i] = 0; // mutable
+ m_PGSNumIters[i] = 0;
+ m_PGSNumFailures[i] = 0;
+ }
+ }
+ long long getPGSNumCalls(int phase) const {return m_PGSNumCalls[phase];}
+ long long getPGSNumIters(int phase) const {return m_PGSNumIters[phase];}
+ long long getPGSNumFailures(int phase) const
+ { return m_PGSNumFailures[phase]; }
+
+ void setPGSConvergenceTol(Real tol) {m_PGSConvergenceTol=tol;}
+ void setPGSMaxIters(int mx) {m_PGSMaxIters=mx;}
+ void setPGSSOR(Real sor) {assert(0<=sor && sor<=2); m_PGSSOR = sor;}
+
+ Real getPGSConvergenceTol() const {return m_PGSConvergenceTol;}
+ int getPGSMaxIters() const {return m_PGSMaxIters;}
+ Real getPGSSOR() const {return m_PGSSOR;}
+
+ /** Initialize the PGSTimeStepper's internally maintained state to a copy
+ of the given state. **/
+ void initialize(const State& initState);
+ const State& getState() const {return m_state;}
+ Real getTime() const {return m_state.getTime();}
+
+ /** Advance to the indicated time in one or more steps. **/
+ Integrator::SuccessfulStepStatus stepTo(Real time);
+
+ /** Advance to the indicated time in one or more steps. **/
+ Integrator::SuccessfulStepStatus stepTo2(Real time);
+
+private:
+ // Determine which constraints will be involved for this step.
+ void findProximalConstraints(const State&);
+ // Enable all proximal constraints, disable all distal constraints,
+ // reassigning multipliers if needed. Returns true if anything changed.
+ bool enableProximalConstraints(State&);
+ // Calculate velocity-dependent coefficients of restitution and friction
+ // and apply combining rules for dissimilar materials.
+ void calcContactCoefficients(const State&, const Vector& verr);
+
+ // Easy if there are no constraints active.
+ void takeUnconstrainedStep(State& s, Real h);
+
+ // Adjust given verr to reflect Newton restitution.
+ bool applyNewtonRestitutionIfAny(const State&, Vector& verr) const;
+
+ // Adjust given compression impulse to include Poisson restitution impulse.
+ bool applyPoissonRestitutionIfAny(const State&, Vector& impulse) const;
+
+ // This phase uses all the proximal constraints and should use a starting
+ // guess for impulse saved from the last step if possible.
+ bool doCompressionPhase(const State&, const Vector& eps,
+ Vector& compressionImpulse);
+ // This phase uses all the proximal constraints, but we expect the result
+ // to be zero unless expansion causes new violations.
+ bool doExpansionPhase(const State&, const Vector& eps,
+ Vector& reactionImpulse);
+
+ bool anyPositionErrorsViolated(const State&, const Vector& perr) const;
+
+ // This phase uses only holonomic constraints, and zero is a good initial
+ // guess for the (hopefully small) position correction.
+ bool doPositionCorrectionPhase(const State&, const Vector& eps,
+ Vector& positionImpulse);
+
+private:
+ // Returns true if it converges.
+ bool projGaussSeidel(int phase, // for stats
+ const Matrix& A, const Vector& eps, Vector& pi,
+ const Array_<int>& all,
+ const Array_<int>& unconditional,
+ Array_<Bounded>& bounded,
+ Array_<LengthLimited>& lengthLimited,
+ Array_<Frictional>& frictional) const;
+
+ Real m_PGSConvergenceTol;
+ int m_PGSMaxIters;
+ Real m_PGSSOR;
+
+ mutable long long m_PGSNumCalls[3]; // phases 0=comp, 1=exp, 2=position
+ mutable long long m_PGSNumIters[3];
+ mutable long long m_PGSNumFailures[3];
+
+private:
+ const PGSAugmentedMultibodySystem& m_ambs;
+ Real m_accuracy;
+ Real m_consTol;
+ bool m_useNewton;
+
+ // Runtime data.
+ State m_state;
+ MyElementSubset m_proximals, m_distals;
+ Array_<Bounded> m_bounded, m_boundedPos;
+ Array_<LengthLimited> m_lengthLimited;
+ Array_<Frictional> m_frictional;
+ Array_<MultiplierIndex> m_all, m_allPos, m_uncond, m_uncondPos;
+ Matrix m_GMInvGt; // G M\ ~G
+
+friend class ShowContact;
+};
+
+
+
+//==============================================================================
+// SHOW CONTACT
+//==============================================================================
+// For each visualization frame, generate ephemeral geometry to show just
+// during this frame, based on the current State.
+class ShowContact : public DecorationGenerator {
+public:
+ explicit ShowContact(const PGSTimeStepper& ts)
+ : m_ts(ts) {}
+
+ void generateDecorations(const State& state,
+ Array_<DecorativeGeometry>& geometry) OVERRIDE_11
+ {
+ const PGSAugmentedMultibodySystem& ambs = m_ts.m_ambs;
+ const MyUnilateralConstraintSet& unis = ambs.getUnis();
+ ambs.realize(state, Stage::Dynamics);
+ const Real E = ambs.calcEnergy(state);
+ DecorativeText energy; energy.setIsScreenText(true);
+ energy.setText("Energy: " + String(E, "%.3f"));
+ geometry.push_back(energy);
+
+ for (unsigned i=0; i < m_ts.m_proximals.m_contact.size(); ++i) {
+ const int id = m_ts.m_proximals.m_contact[i];
+ const MyContactElement& contact = unis.getContactElement(id);
+ const Vec3 loc = contact.whereToDisplay(state);
+ if (!contact.isDisabled(state)) {
+ geometry.push_back(DecorativeSphere(.1)
+ .setTransform(loc)
+ .setColor(Red).setOpacity(.25));
+ contact.showContactForce(state, geometry);
+ String text;
+ if (!contact.hasFrictionElement())
+ text = "-ENABLED";
+ geometry.push_back(DecorativeText(String(i)+text)
+ .setColor(White).setScale(.1)
+ .setTransform(loc+Vec3(0,.04,0)));
+ } else {
+ geometry.push_back(DecorativeText(String(i))
+ .setColor(White).setScale(.1)
+ .setTransform(loc+Vec3(0,.02,0)));
+ }
+ }
+
+ for (unsigned i=0; i < m_ts.m_proximals.m_friction.size(); ++i) {
+ const int id = m_ts.m_proximals.m_friction[i];
+ const MyFrictionElement& felt = unis.getFrictionElement(id);
+ const Vec3 loc = felt.whereToDisplay(state);
+ const Frictional& fric = m_ts.m_frictional[i];
+ String text = fric.m_wasLimited ? "slip" : "STICK";
+ Vec3 color = fric.m_wasLimited ? Green : Orange;
+ felt.showFrictionForce(state, geometry, color);
+ geometry.push_back(DecorativeText(text)
+ .setColor(color).setScale(.1)
+ .setTransform(loc+Vec3(0.1,.04,0)));
+ }
+ }
+private:
+ const PGSTimeStepper& m_ts;
+};
+
+//==============================================================================
+// TIM'S BOX
+//==============================================================================
+class TimsBox : public PGSAugmentedMultibodySystem {
+public:
+ TimsBox();
+
+ void calcInitialState(State& state) const OVERRIDE_11;
+
+ const MobilizedBody& getBodyToWatch() const OVERRIDE_11
+ { return m_brick; }
+
+private:
+ Force::Gravity m_gravity;
+ Force::GlobalDamper m_damper;
+ MobilizedBody::Free m_brick;
+ MobilizedBody::Ball m_brick2;
+};
+
+
+//==============================================================================
+// BOUNCING BALLS
+//==============================================================================
+class BouncingBalls : public PGSAugmentedMultibodySystem {
+public:
+ BouncingBalls();
+ ~BouncingBalls() {delete m_contactForces; delete m_tracker;}
+
+ void calcInitialState(State& state) const OVERRIDE_11;
+
+ const MobilizedBody& getBodyToWatch() const OVERRIDE_11
+ { static const MobilizedBody nobod; return nobod; }
+
+ const MobilizedBody::Slider& getHBall(int i) const {return m_Hballs[i];}
+ const MobilizedBody::Slider& getPBall(int i) const {return m_Pballs[i];}
+ const MobilizedBody::Slider& getNBall(int i) const {return m_Nballs[i];}
+
+private:
+ // Add subsystems for compliant contact. TODO: shouldn't need pointers
+ ContactTrackerSubsystem* m_tracker;
+ CompliantContactSubsystem* m_contactForces;
+
+ static const int NBalls = 4;
+
+ Force::Gravity m_gravity;
+ Force::GlobalDamper m_damper;
+ MobilizedBody::Slider m_Hballs[NBalls]; // Hertz
+ MobilizedBody::Slider m_Pballs[NBalls]; // Poisson
+ MobilizedBody::Slider m_Nballs[NBalls]; // Newton
+};
+}
+
+//==============================================================================
+// MAIN
+//==============================================================================
+int main(int argc, char** argv) {
+ #ifdef USE_TIMS_PARAMS
+ const Real RunTime=16; // Tim's time
+ const Real Accuracy = 1e-4;
+ #else
+ const Real RunTime=20;
+ const Real Accuracy = 1e-2;
+ #endif
+
+ const bool UseNewton = false; // default is Poisson restitution
+
+ try { // If anything goes wrong, an exception will be thrown.
+
+ // Create the augmented multibody model.
+ TimsBox mbs;
+ //BouncingBalls mbs;
+ PGSTimeStepper pgs(mbs);
+
+ const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
+ const MyUnilateralConstraintSet& unis = mbs.getUnis();
+
+ Visualizer viz(mbs);
+ viz.setShowSimTime(true);
+ viz.setShowFrameNumber(true);
+ viz.setShowFrameRate(true);
+ viz.addDecorationGenerator(new ShowContact(pgs));
+
+ if (!mbs.getBodyToWatch().isEmptyHandle())
+ viz.addFrameController(new BodyWatcher(mbs.getBodyToWatch()));
+
+ // Simulate it.
+ State s;
+ mbs.calcInitialState(s);
+
+ printf("Initial state shown. ENTER to continue.\n");
+ viz.report(s);
+ getchar();
+
+ const Real ConsTol = .001;
+ const Real PGSConvergenceTol = 1e-5;
+ const int PGSMaxIters = 100;
+ const Real PGSSor = 1.05; // successive over relaxation, 0..2, 1 is neutral
+
+ pgs.setUseNewtonRestitution(UseNewton);
+ pgs.setAccuracy(Accuracy); // integration accuracy
+ pgs.setConstraintTol(ConsTol);
+
+ pgs.setPGSConvergenceTol(PGSConvergenceTol);
+ pgs.setPGSMaxIters(PGSMaxIters);
+ pgs.setPGSSOR(PGSSor);
+
+ pgs.initialize(s);
+ mbs.resetAllCountersToZero();
+ mbs.updUnis().initialize(); // reinitialize
+
+ Array_<State> states; states.reserve(10000);
+
+ int nSteps=0, nStepsWithEvent = 0;
+
+ const double startReal = realTime();
+ const double startCPU = cpuTime();
+
+ const Real h = .001;
+ const int SaveEvery = 33; // save every nth step ~= 33ms
+
+ do {
+ const State& pgsState = pgs.getState();
+ if ((nSteps%SaveEvery)==0) {
+ #ifdef ANIMATE
+ viz.report(pgsState);
+ #endif
+ states.push_back(pgsState);
+ }
+
+ pgs.stepTo(pgsState.getTime() + h);
+
+ ++nSteps;
+ } while (pgs.getTime() < RunTime);
+ // TODO: did you lose the last step?
+
+
+ const double timeInSec = realTime()-startReal;
+ const double cpuInSec = cpuTime()-startCPU;
+ cout << "Done -- took " << nSteps << " steps in " <<
+ timeInSec << "s for " << pgs.getTime() << "s sim (avg step="
+ << (1000*pgs.getTime())/nSteps << "ms) ";
+ cout << "CPUtime " << cpuInSec << endl;
+
+ printf("Used PGS solver (%s) at acc=%g consTol=%g"
+ " convergenceTol=%g maxIters=%d SOR=%g.\n",
+ pgs.getUseNewtonRestitution() ? "Newton" : "Poisson",
+ pgs.getAccuracy(), pgs.getConstraintTol(),
+ pgs.getPGSConvergenceTol(), pgs.getPGSMaxIters(),
+ pgs.getPGSSOR());
+ printf("Compression: ncalls=%lld, niters=%lld (%g/call), nfail=%lld\n",
+ pgs.getPGSNumCalls(0), pgs.getPGSNumIters(0),
+ (double)pgs.getPGSNumIters(0)/std::max(pgs.getPGSNumCalls(0),1LL),
+ pgs.getPGSNumFailures(0));
+ printf("Expansion: ncalls=%lld, niters=%lld (%g/call), nfail=%lld\n",
+ pgs.getPGSNumCalls(1), pgs.getPGSNumIters(1),
+ (double)pgs.getPGSNumIters(1)/std::max(pgs.getPGSNumCalls(1),1LL),
+ pgs.getPGSNumFailures(1));
+ printf("Position: ncalls=%lld, niters=%lld (%g/call), nfail=%lld\n",
+ pgs.getPGSNumCalls(2), pgs.getPGSNumIters(2),
+ (double)pgs.getPGSNumIters(2)/std::max(pgs.getPGSNumCalls(2),1LL),
+ pgs.getPGSNumFailures(2));
+
+ cout << "nstates =" << states.size() << endl;
+
+ // Instant replay.
+ while(true) {
+ printf("Hit ENTER for replay (%d states) ...",
+ states.size());
+ getchar();
+ for (unsigned i=0; i < states.size(); ++i) {
+ mbs.realize(states[i], Stage::Velocity);
+ viz.report(states[i]);
+ }
+ }
+
+ }
+ catch (const std::exception& e) {
+ printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+ }
+ catch (...) {
+ printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+}
+
+//==============================================================================
+// PGS TIME STEPPER
+//==============================================================================
+Integrator::SuccessfulStepStatus PGSTimeStepper::
+stepTo(Real time) {
+ // Abbreviations.
+ const PGSAugmentedMultibodySystem& mbs = m_ambs;
+ const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
+ const MyUnilateralConstraintSet& unis = mbs.getUnis();
+ State& s = m_state;
+
+ const Real h = time - m_state.getTime(); // max timestep
+
+ // Kinematics should already be realized so this won't do anything.
+ mbs.realize(s, Stage::Position);
+ // Determine which constraints will be involved for this step.
+ findProximalConstraints(s);
+
+ // Enable all proximal constraints, reassigning multipliers if needed.
+ enableProximalConstraints(s);
+ mbs.realize(s, Stage::Velocity);
+
+ const Vector& verr0 = s.getUErr();
+ if (!verr0.size()) {
+ takeUnconstrainedStep(s, h);
+ return Integrator::ReachedScheduledEvent;
+ }
+
+ // Calculate velocity-dependent coefficients of restitution and friction
+ // and apply combining rules for dissimilar materials.
+ calcContactCoefficients(s, verr0);
+
+ // We're going to accumulate velocity errors here, starting with the
+ // presenting violation.
+ Vector verr = verr0;
+
+ // If we're in Newton mode, or if a contact specifies Newton restitution,
+ // then modify the appropriate verr's here.
+ const bool anyNewton = applyNewtonRestitutionIfAny(s, verr);
+
+ // Evaluate applied forces and get reference to them. These include
+ // gravity but not centrifugal forces.
+ mbs.realize(s, Stage::Dynamics);
+ const Vector& f = mbs.getMobilityForces(s, Stage::Dynamics);
+ const Vector_<SpatialVec>& F = mbs.getRigidBodyForces(s, Stage::Dynamics);
+
+ // Calculate the constraint compliance matrix GM\~G.
+ matter.calcProjectedMInv(s, m_GMInvGt);
+
+ // Calculate udotExt = M\(f + ~J*(F-C)) where C are centrifugal forces.
+ // This is the unconstrained acceleration.
+ Vector udotExt; Vector_<SpatialVec> A_GB;
+ matter.calcAccelerationIgnoringConstraints(s,f,F,udotExt,A_GB);
+
+ // There are some constraints to deal with.
+ // Calculate verrExt = G*deltaU; the end-of-step constraint error due to
+ // external and centrifugal forces.
+ Vector verrExt;
+ matter.multiplyByG(s, h*udotExt, verrExt);
+
+ // Update velocity error to include verrExt.
+ verr += verrExt; // verr0+Newton restitution+external forces
+
+ Vector impulse;
+ //--------------------------------------------------------------------------
+ // Initialize the impulse to a best guess from the previous step,
+ // compute the compression impulse that eliminates verr.
+ doCompressionPhase(s, verr, impulse);
+ //--------------------------------------------------------------------------
+
+ // Modify the compression impulse to add in expansion impulses.
+ const bool anyPoisson = applyPoissonRestitutionIfAny(s, impulse);
+
+ if (anyPoisson) {
+ // Must calculate a reaction impulse. Move now-known impulse to RHS
+ // (convert to verr).
+ Vector genImpulse, deltaU, expVerr, reacImpulse;
+ // constraint impulse -> gen impulse
+ matter.multiplyByGTranspose(s, impulse, genImpulse);
+ // gen impulse to delta-u, then to verr
+ matter.multiplyByMInv(s, genImpulse, deltaU);
+ matter.multiplyByG(s, deltaU, expVerr);
+
+ // Update verr to include compression+expansion impulse.
+ verr -= expVerr; // watch sign -- multipliers are opposite forces
+
+ //----------------------------------------------------------------------
+ doExpansionPhase(s, verr, reacImpulse); // initial guess=zero
+ //----------------------------------------------------------------------
+
+ impulse += reacImpulse;
+
+ //TODO: reaction impulse should cause a further expansion in a
+ //contact that hasn't bounced yet.
+ }
+
+ // We need forces, not impulses for the next calculation.
+ s.updMultipliers() = impulse/h;
+ // Calculate constraint forces ~G*lambda (body frcs Fc, mobility frcs fc).
+ Vector_<SpatialVec> Fc; Vector fc;
+ matter.calcConstraintForcesFromMultipliers
+ (s,s.updMultipliers(), Fc, fc);
+
+ // Now calculate udot = M\(f-fc + ~J*(F-Fc-C)).
+ Vector udot;
+ matter.calcAccelerationIgnoringConstraints(s,f-fc,F-Fc,udot,A_GB);
+
+ // Update auxiliary states z, invalidating Stage::Dynamics.
+ s.updZ() += h*s.getZDot();
+
+ // Update u from deltaU, invalidating Stage::Velocity.
+ s.updU() += h*udot;
+
+ // Done with velocity update. Now calculate qdot, possiblity including
+ // an additional position error correction term.
+ Vector qdot;
+ const Vector& perr0 = s.getQErr();
+ if (!anyPositionErrorsViolated(s, perr0)) {
+ matter.multiplyByN(s,false,s.getU(),qdot);
+ } else {
+ // Don't include quaternions for position correction.
+ Vector posImpulse, genImpulse, posVerr, deltaU;
+ posVerr.resize(s.getNUErr()); posVerr.setToZero();
+ const int nQuat = matter.getNumQuaternionsInUse(s);
+ posVerr(0, perr0.size()-nQuat) = perr0(0, perr0.size()-nQuat)/h;
+
+ //----------------------------------------------------------------------
+ // Calculate impulse and then deltaU=M\~G*impulse such that -h*deltaU
+ // will eliminate position errors, respecting only position constraints.
+ doPositionCorrectionPhase(s, posVerr, posImpulse);
+ //----------------------------------------------------------------------
+ // constraint impulse -> gen impulse
+ matter.multiplyByGTranspose(s, posImpulse, genImpulse);
+ // gen impulse to deltaU (watch sign)
+ matter.multiplyByMInv(s, genImpulse, deltaU);
+
+ // convert corrected u to qdot (note we're not changing u)
+ matter.multiplyByN(s,false,s.getU()-deltaU, qdot);
+ }
+
+ // We have qdot, now update q, fix quaternions, update time.
+ s.updQ() += h*qdot; // invalidates Stage::Position
+ matter.normalizeQuaternions(s);
+ s.updTime() += h; // invalidates Stage::Time
+
+ // Return from step with kinematics realized. Note that we may have
+ // broken the velocity constraints by updating q, but we won't fix that
+ // until the next step. Also position constraints are only imperfectly
+ // satisfied by the correction above.
+ mbs.realize(s, Stage::Velocity);
+
+ return Integrator::ReachedScheduledEvent;
+}
+
+//==============================================================================
+// PROJECTED GAUSS SEIDEL
+//==============================================================================
+/*
+We are given
+ - A, square matrix of dimension mA
+ - b, rhs vector (length mA)
+ - w, solution vector with initial value w=w0 (length mA)
+representing mA scalar constraint equations A[i]*w=b[i].
+
+A smaller square subset may be selected via
+ - I, selection index set, a subset of IA={1,...,mA}
+
+The selected subset I is partitioned into four disjoint index sets
+ - IU: Unconditional
+ - IB: Bounded scalar
+ - IV: Length-limited vector
+ - IF: Friction
+
+Each bounded scalar constraint k provides
+ - a single constraint index iB_k from IB, and
+ - lower and upper bounds lb_k, ub_k.
+
+Each length-limited vector constraint k specifies
+ - a unique index set of 1-3 distinct constraints IV_k from IV,
+ - a nonnegative scalar L_k specifing the maximum length of the vector.
+
+Each friction constraint k specifies
+ - a unique index set of 1-3 distinct friction constraints IF_k from IF,
+ - an index set of 1-3 distinct normal constraints IN_k from IA-IF,
+ - the effective coefficient of friction mu.
+Note that the normal constraints in IN_k do not have to be selected from the
+active subset I; if not they will be fixed at w0[IN_k] on entry.
+
+Given those inputs, we attempt to solve:
+ A[I,I] w[I] = b[I]
+ subject to lb_k <= w[iB_k] <= ub_k for bounded constraints k in IB
+ and ||w[IV_k]|| <= L_k for vector constraints k in IV
+ and ||w[IF_k]|| <= mu*||w[IN_k]|| for friction constraints k in IF
+
+Implicitly, complementarity conditions must hold:
+ w_i in interior of constraint -> A[i]*w == b[i]
+ w_i on boundary of constraint -> A[i]*w != b[i]
+
+*/
+
+namespace { // local functions
+/** Given a scalar s, ensure that lb <= s <= ub by moving s to the nearest
+bound if necessary. Return true if any change is made. **/
+bool boundScalar(Real lb, Real& s, Real ub) {
+ assert(lb <= ub);
+ if (s > ub) {s=ub; return true;}
+ else if (s < lb) {s=lb; return true;}
+ return false;
+}
+
+/** Given an index set IV, ensure that ||w[IV]|| <= maxLen by scaling the
+vector to that length if necessary. Return true if any change is made. **/
+bool boundVector(Real maxLen, const Array_<int>& IV, Vector& w) {
+ assert(maxLen >= 0);
+ const Real maxLen2 = square(maxLen);
+ Real wNorm2 = 0;
+ for (unsigned i=0; i<IV.size(); ++i) wNorm2 += square(w[IV[i]]);
+ if (wNorm2 <= maxLen2)
+ return false;
+ const Real scale = std::sqrt(maxLen2/wNorm2); // 0 <= scale < 1
+ for (unsigned i=0; i<IV.size(); ++i) w[IV[i]] *= scale;
+ return true;
+}
+
+/** Given index set IN identifying the components of the normal force vector,
+and index set IF identifying the components of the friction vector, ensure
+that ||w[IF]|| <= mu*||w[IN]|| by scaling the friction vector if necessary.
+Return true if any change is made. **/
+bool boundFriction(Real mu, const Array_<int>& IN,
+ const Array_<int>& IF, Vector& w) {
+ assert(mu >= 0);
+ Real N2=0, F2=0; // squares of normal and friction force magnitudes
+ for (unsigned i=0; i<IN.size(); ++i) N2 += square(w[IN[i]]);
+ for (unsigned i=0; i<IF.size(); ++i) F2 += square(w[IF[i]]);
+ const Real mu2N2 = mu*mu*N2;
+ if (F2 <= mu2N2)
+ return false;
+ const Real scale = std::sqrt(mu2N2/F2); // 0 <= scale < 1
+ for (unsigned i=0; i<IF.size(); ++i) w[IF[i]] *= scale;
+ return true;
+}
+}
+
+bool PGSTimeStepper::projGaussSeidel
+ (int phase,
+ const Matrix& A, const Vector& b, Vector& w,
+ const Array_<int>& all,
+ const Array_<int>& unconditional,
+ Array_<Bounded>& bounded,
+ Array_<LengthLimited>& lengthLimited,
+ Array_<Frictional>& frictional) const
+{
+ ++m_PGSNumCalls[phase];
+ const int mA=A.nrow(), nA=A.ncol();
+ assert(mA==nA); assert(b.nrow()==mA); assert(w.nrow()==nA);
+
+ const int m = (int)all.size();
+ assert(m<=mA);
+
+ // Partitions of selected subset.
+ const int mUncond = (int)unconditional.size();
+ const int mBounded = (int)bounded.size();
+ const int mLength = (int)lengthLimited.size();
+ const int mFric = (int)frictional.size();
+
+ // If debugging, check for consistent constraint equation count.
+ #ifndef NDEBUG
+ {int mCount = mUncond + mBounded; // 1 each
+ for (int k=0; k<mLength; ++k)
+ mCount += lengthLimited[k].m_components.size();
+ for (int k=0; k<mFric; ++k)
+ mCount += frictional[k].m_Fk.size();
+ assert(mCount == m);}
+ #endif
+
+ if (m == 0) {
+ printf("PGS %d: nothing to do; converged in 0 iters.\n", phase);
+ return true;
+ }
+
+ // Track total error for all included equations, and the error for just
+ // those equations that are being enforced.
+ bool converged = false;
+ Real normRMSall = Infinity, normRMSenf = Infinity;
+ for (int its=0; its < m_PGSMaxIters; ++its) {
+ ++m_PGSNumIters[phase];
+ const Real prevNormRMSall = normRMSall;
+ Real sum2all = 0, sum2enf = 0; // track solution errors
+
+ // UNCONDITIONAL: these are always on.
+ for (int fx=0; fx < mUncond; ++fx) {
+ const int rx = unconditional[fx];
+ Real rowSum = 0;
+ for (int c=0; c < m; ++c) {
+ const int cx = all[c];
+ rowSum += A(rx,cx)*w[cx];
+ }
+ const Real er = b[rx]-rowSum, er2=er*er;
+ w[rx] += m_PGSSOR * er/A(rx,rx); //TODO: check for zero?
+ sum2all += er2; sum2enf += er2;
+ }
+
+ // BOUNDED: conditional scalar constraints with constant bounds
+ // on resulting w.
+ for (int k=0; k < mBounded; ++k) {
+ Bounded& bnd = bounded[k];
+ const int rx = bnd.m_ix;
+ Real rowSum = 0;
+ for (int c=0; c < m; ++c) {
+ const int cx = all[c];
+ rowSum += A(rx,cx)*w[cx];
+ }
+ const Real er = b[rx]-rowSum, er2=er*er;
+ w[rx] += m_PGSSOR * er/A(rx,rx);
+ sum2all += er2;
+ if (!(bnd.m_hitBound=boundScalar(bnd.m_lb, w[rx], bnd.m_ub)))
+ sum2enf += er2;
+ }
+
+ // LENGTH: a set of constraint equations forming a vector whose
+ // maximum length is limited.
+ for (int k=0; k < mLength; ++k) {
+ LengthLimited& len = lengthLimited[k];
+ const Array_<int>& rows = len.m_components;
+ Vec3 rowSums(0);
+ for (int c=0; c < m; ++c) {
+ const int cx = all[c];
+ for (unsigned i=0; i<rows.size(); ++i)
+ rowSums[i] += A(rows[i],cx)*w[cx];
+ }
+ Real localEr2 = 0;
+ for (unsigned i=0; i<rows.size(); ++i) {
+ const int rx = rows[i];
+ const Real er = b[rx]-rowSums[i];
+ w[rx] += m_PGSSOR * er/A(rx,rx);
+ localEr2 += square(er);
+ }
+ sum2all += localEr2;
+ if (!(len.m_hitLimit=boundVector(len.m_maxLength, rows, w)))
+ sum2enf += localEr2;
+ }
+
+ // FRICTIONAL: a set of constraint equations forming a vector whose
+ // maximum length is limited by the norm of other multipliers w.
+ for (int k=0; k < mFric; ++k) {
+ Frictional& fric = frictional[k];
+ const Array_<int>& Fk = fric.m_Fk; // friction components
+ Vec3 rowSums(0);
+ for (int c=0; c < m; ++c) {
+ const int cx = all[c];
+ for (unsigned i=0; i<Fk.size(); ++i)
+ rowSums[i] += A(Fk[i],cx)*w[cx];
+ }
+ Real localEr2 = 0;
+ for (unsigned i=0; i<Fk.size(); ++i) {
+ const int rx = Fk[i];
+ const Real er = b[rx]-rowSums[i];
+ w[rx] += m_PGSSOR * er/A(rx,rx);
+ localEr2 += square(er);
+ }
+ sum2all += localEr2;
+ if (!(fric.m_wasLimited=boundFriction(fric.m_effMu,fric.m_Nk,Fk,w)))
+ sum2enf += localEr2;
+ }
+ normRMSall = std::sqrt(sum2all/m);
+ normRMSenf = std::sqrt(sum2enf/m);
+
+ SimTK_DEBUG4("%d/%d: EST rmsAll=%g rmsEnf=%g\n", phase, its,
+ normRMSall, normRMSenf);
+ #ifdef NDEBUG // i.e., NOT debugging (TODO)
+ if (its > 50)
+ printf("%d/%d: EST rmsAll=%g rmsEnf=%g\n", phase, its,
+ normRMSall, normRMSenf);
+ #endif
+ //if ( normRMSall < m_PGSConvergenceTol
+ // || ( std::abs(normRMSall-prevNormRMSall)
+ // < m_PGSConvergenceTol*prevNormRMSall))
+ if (normRMSenf < m_PGSConvergenceTol) //TODO: add failure-to-improve check
+ {
+ SimTK_DEBUG3("PGS %d converged to %g in %d iters\n",
+ phase, normRMSenf, its);
+ converged = true;
+ break;
+ }
+ }
+
+ if (!converged) {
+ printf("PGS %d CONVERGENCE FAILURE: %d iters -> norm=%g\n",
+ phase, m_PGSMaxIters, normRMSenf);
+ ++m_PGSNumFailures[phase];
+ }
+
+ return converged;
+}
+
+void PGSTimeStepper::initialize(const State& initState) {
+ m_state = initState;
+ m_ambs.realize(m_state, Stage::Acceleration);
+}
+
+// Determine which constraints will be involved for this step.
+void PGSTimeStepper::
+findProximalConstraints(const State& s) { //TODO: redo
+ const MyUnilateralConstraintSet& unis = m_ambs.getUnis();
+ unis.findProximalElements(s, m_consTol, m_proximals, m_distals);
+}
+
+// Enable all proximal constraints, disable all distal constraints,
+// reassigning multipliers if needed. Returns true if any change was made.
+bool PGSTimeStepper::
+enableProximalConstraints(State& s) {
+ const MyUnilateralConstraintSet& unis = m_ambs.getUnis();
+
+ // Record friction application points. This has to be done while Position
+ // stage is still valid.
+ for (unsigned i=0; i < m_proximals.m_friction.size(); ++i) {
+ const int id = m_proximals.m_friction[i];
+ unis.updFrictionElement(id).initializeFriction(s);
+ }
+
+ bool changed = false;
+
+ // Disable non-proximal constraints if they were previously disabled.
+ for (unsigned i=0; i < m_distals.m_friction.size(); ++i) {
+ const int id = m_distals.m_friction[i];
+ const MyFrictionElement& fric = unis.getFrictionElement(id);
+ if (fric.isEnabled(s)) fric.disable(s), changed=true;
+ }
+ for (unsigned i=0; i < m_distals.m_contact.size(); ++i) {
+ const int id = m_distals.m_contact[i];
+ const MyContactElement& cont = unis.getContactElement(id);
+ if (!cont.isDisabled(s)) cont.disable(s), changed=true;
+ }
+
+ for (unsigned i=0; i < m_proximals.m_contact.size(); ++i) {
+ const int id = m_proximals.m_contact[i];
+ const MyContactElement& cont = unis.getContactElement(id);
+ if (cont.isDisabled(s)) cont.enable(s), changed=true;
+ }
+ for (unsigned i=0; i < m_proximals.m_friction.size(); ++i) {
+ const int id = m_proximals.m_friction[i];
+ const MyFrictionElement& fric = unis.getFrictionElement(id);
+ fric.setInstanceParameters(s);
+ if (!fric.isEnabled(s)) fric.enable(s), changed=true;
+ }
+
+ // TODO: Note that we always have to move the friction application points
+ // which is an Instance stage change; shouldn't be.
+ m_ambs.realize(s, Stage::Instance); // assign multipliers
+
+ return changed;
+}
+
+void PGSTimeStepper::takeUnconstrainedStep(State& s, Real h) {
+ const PGSAugmentedMultibodySystem& mbs = m_ambs;
+ const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
+ mbs.realize(s, Stage::Acceleration);
+ const Vector& udot = s.getUDot(); // grab before invalidated
+ s.updZ() += h*s.getZDot(); // invalidates Stage::Dynamics
+ s.updU() += h*udot; // invalidates Stage::Velocity
+ Vector qdot;
+ matter.multiplyByN(s,false,s.getU(),qdot);
+ s.updQ() += h*qdot; // invalidates Stage::Position
+ matter.normalizeQuaternions(s);
+ s.updTime() += h; // invalidates Stage::Time
+ mbs.realize(s, Stage::Velocity);
+}
+
+
+// Calculate velocity-dependent coefficients of restitution and friction
+// and apply combining rules for dissimilar materials.
+void PGSTimeStepper::
+calcContactCoefficients(const State& s, const Vector& verr) {
+ const MyUnilateralConstraintSet& unis = m_ambs.getUnis();
+ m_bounded.clear();
+ for (unsigned i=0; i < m_proximals.m_contact.size(); ++i) {
+ const int id = m_proximals.m_contact[i];
+ const MyContactElement& elt = unis.getContactElement(id);
+ const MultiplierIndex mx = elt.getMultIndex(s);
+ Real cor = elt.getMaxCoefRest();
+ if (verr[mx] >= -unis.getCaptureVelocity()) cor=0;
+ SimTK_DEBUG3("Contact %d speed=%g -> N cor=%g\n",
+ i, verr[mx], cor);
+ m_bounded.push_back(
+ Bounded(mx,-Infinity, Zero, cor)); // TODO: fix sign convention
+ }
+
+ m_frictional.clear();
+ for (unsigned i=0; i < m_proximals.m_friction.size(); ++i) {
+ const int id = m_proximals.m_friction[i];
+ const MyFrictionElement& felt = unis.getFrictionElement(id);
+ const Real v_trans = unis.getTransitionVelocity();
+ const Real mu_d = felt.getDynamicFrictionCoef();
+ const Real mu_s = felt.getStaticFrictionCoef();
+ const Real mu_v = felt.getViscousFrictionCoef();
+ const MyPointContactFriction& pelt = // TODO: generalize
+ dynamic_cast<const MyPointContactFriction&>(felt);
+ const MultiplierIndex mx = pelt.getMultIndexX(s);
+ const MultiplierIndex my = pelt.getMultIndexY(s);
+ const MultiplierIndex mN = pelt.getMyPointContact().getMultIndex(s);
+ const Vec2 vSlip(verr[mx], verr[my]);
+ const Real speed = vSlip.norm();
+ const Real mu = (speed<=v_trans? mu_s : mu_d + speed*mu_v);
+ SimTK_DEBUG3("Fric %d speed=%g -> mu=%g\n", i, speed, mu);
+ Array_<MultiplierIndex> Fk; Fk.push_back(mx); Fk.push_back(my);
+ Array_<MultiplierIndex> Nk; Nk.push_back(mN);
+ m_frictional.push_back(Frictional(Fk,Nk,mu));
+ }
+
+ const int m = s.getNUErr();
+ m_all.clear(); m_uncond.clear();
+ for (int i=0; i<m; ++i)
+ m_all.push_back(MultiplierIndex(i));
+ // TODO: add in unconditionals in m_uncond
+
+ m_allPos.clear(); m_boundedPos.clear(); m_uncondPos.clear();
+ for (unsigned i=0; i<m_bounded.size(); ++i) {
+ m_allPos.push_back(m_bounded[i].m_ix);
+ m_boundedPos.push_back(m_bounded[i]); // should be holonomics only
+ }
+ // TODO: add in unconditionals in m_uncondPos
+}
+
+bool PGSTimeStepper::
+applyNewtonRestitutionIfAny(const State& s, Vector& verr) const {
+ if (!m_useNewton)
+ return false; //TODO: check individual contacts
+ bool anyRestitution = false;
+ const PGSAugmentedMultibodySystem& mbs = m_ambs;
+ const MyUnilateralConstraintSet& unis = mbs.getUnis();
+ for (unsigned i=0; i < m_proximals.m_contact.size(); ++i) {
+ const int id = m_proximals.m_contact[i];
+ const MyContactElement& elt = unis.getContactElement(id);
+ const MultiplierIndex mx = elt.getMultIndex(s);
+ const Bounded& bounded = m_bounded[i];
+ if (bounded.m_effCOR != 0) {
+ verr[mx] *= (1+bounded.m_effCOR);
+ anyRestitution = true;
+ }
+ }
+ return anyRestitution;
+}
+
+bool PGSTimeStepper::
+applyPoissonRestitutionIfAny(const State& s, Vector& impulse) const {
+ if (m_useNewton)
+ return false; //TODO: check individual contacts
+ bool anyRestitution = false;
+ const PGSAugmentedMultibodySystem& mbs = m_ambs;
+ const MyUnilateralConstraintSet& unis = mbs.getUnis();
+ for (unsigned i=0; i < m_proximals.m_contact.size(); ++i) {
+ const int id = m_proximals.m_contact[i];
+ const MyContactElement& elt = unis.getContactElement(id);
+ const MultiplierIndex mx = elt.getMultIndex(s);
+ const Bounded& bounded = m_bounded[i];
+ if (bounded.m_effCOR != 0) {
+ impulse[mx] *= (1+bounded.m_effCOR);
+ anyRestitution = true;
+ }
+ }
+ return anyRestitution;
+}
+
+// This phase uses all the proximal constraints and should use a starting
+// guess for impulse saved from the last step if possible.
+bool PGSTimeStepper::
+doCompressionPhase(const State&, const Vector& eps, Vector& compImpulse) {
+ // TODO: improve initial guess
+ compImpulse.resize(m_GMInvGt.ncol()); compImpulse.setToZero();
+ bool converged = projGaussSeidel(0, m_GMInvGt, eps, compImpulse,
+ m_all, m_uncond, m_bounded,
+ m_lengthLimited, m_frictional);
+ return converged;
+}
+// This phase uses all the proximal constraints, but we expect the result
+// to be zero unless expansion causes new violations.
+bool PGSTimeStepper::
+doExpansionPhase(const State&, const Vector& eps, Vector& reactionImpulse) {
+ // TODO: improve initial guess
+ reactionImpulse.resize(m_GMInvGt.ncol()); reactionImpulse.setToZero();
+ bool converged = projGaussSeidel(1, m_GMInvGt, eps, reactionImpulse,
+ m_all, m_uncond, m_bounded,
+ m_lengthLimited, m_frictional);
+ return converged;
+}
+
+// This phase uses only holonomic constraints, and zero is a good initial
+// guess for the (hopefully small) position correction.
+bool PGSTimeStepper::
+doPositionCorrectionPhase(const State&, const Vector& eps,
+ Vector& positionImpulse) {
+ positionImpulse.resize(m_GMInvGt.ncol()); positionImpulse.setToZero();
+ Array_<Frictional> noFrictionals;
+ bool converged = projGaussSeidel(2, m_GMInvGt, eps, positionImpulse,
+ m_allPos, m_uncondPos, m_boundedPos,
+ m_lengthLimited, noFrictionals);
+ return converged;
+}
+
+bool PGSTimeStepper::
+anyPositionErrorsViolated(const State&, const Vector& perr) const {
+ // TODO: no need to fix if large perrs satisfy inequalities.
+ bool anyViolated = perr.normInf() > m_consTol;
+ SimTK_DEBUG2("maxAbs(perr)=%g -> %s\n", perr.normInf(),
+ anyViolated ? "VIOLATED" : "OK");
+ return anyViolated;
+}
+
+//==============================================================================
+// TIM'S BOX
+//==============================================================================
+TimsBox::TimsBox() {
+ // Abbreviations.
+ SimbodyMatterSubsystem& matter = updMatterSubsystem();
+ GeneralForceSubsystem& forces = updForceSubsystem();
+ MyUnilateralConstraintSet& unis = updUnis();
+ MobilizedBody& Ground = matter.updGround();
+
+ // Build the multibody system.
+ m_gravity = Force::Gravity(forces, matter, -YAxis, 9.8066);
+ m_damper = Force::GlobalDamper(forces, matter, .1);
+
+ // Predefine some handy rotations.
+ const Rotation Z90(Pi/2, ZAxis); // rotate +90 deg about z
+
+ const Vec3 BrickHalfDims(.1, .25, .5);
+ const Real BrickMass = /*10*/5;
+ #ifdef USE_TIMS_PARAMS
+ const Real RunTime=16; // Tim's time
+ const Real Stiffness = 2e7;
+ const Real Dissipation = 1;
+ const Real CoefRest = 0;
+ // Painleve problem with these friction coefficients.
+ //const Real mu_d = 1; /* compliant: .7*/
+ //const Real mu_s = 1; /* compliant: .7*/
+ const Real mu_d = .5;
+ const Real mu_s = .8;
+ const Real mu_v = /*0.05*/0;
+ const Real CaptureVelocity = 0.01;
+ const Real TransitionVelocity = 0.01;
+ const Inertia brickInertia(.1,.1,.1);
+ const Real Radius = .02;
+ #else
+ const Real RunTime=20;
+ const Real Stiffness = 1e6;
+ const Real CoefRest = 0.3;
+ const Real TargetVelocity = 3; // speed at which to match coef rest
+// const Real Dissipation = (1-CoefRest)/TargetVelocity;
+ const Real Dissipation = 0.1;
+ const Real mu_d = .5;
+ const Real mu_s = 1.0;
+ const Real mu_v = 0*0.1;
+ const Real CaptureVelocity = 0.01;
+ const Real TransitionVelocity = 0.05;
+ const Inertia brickInertia(BrickMass*UnitInertia::brick(BrickHalfDims));
+ const Real Radius = BrickHalfDims[0]/3;
+ #endif
+
+ unis.setCaptureVelocity(CaptureVelocity);
+ unis.setTransitionVelocity(TransitionVelocity);
+
+ printf("\n******************** Tim's Box ********************\n");
+ printf("USING RIGID CONTACT\n");
+ #ifdef USE_TIMS_PARAMS
+ printf("Using Tim's parameters:\n");
+ #else
+ printf("Using Sherm's parameters:\n");
+ #endif
+ printf(" coef restitution=%g\n", CoefRest);
+ printf(" mu_d=%g mu_s=%g mu_v=%g\n", mu_d, mu_s, mu_v);
+ printf(" transition velocity=%g\n", TransitionVelocity);
+ printf(" radius=%g\n", Radius);
+ printf(" brick inertia=%g %g %g\n",
+ brickInertia.getMoments()[0], brickInertia.getMoments()[1],
+ brickInertia.getMoments()[2]);
+ printf("******************** Tim's Box ********************\n\n");
+
+ // ADD MOBILIZED BODIES AND CONTACT CONSTRAINTS
+
+ Body::Rigid brickBody =
+ Body::Rigid(MassProperties(BrickMass, Vec3(0), brickInertia));
+ brickBody.addDecoration(Transform(), DecorativeBrick(BrickHalfDims)
+ .setColor(Red).setOpacity(.3));
+ m_brick = MobilizedBody::Free(Ground, Vec3(0),
+ brickBody, Vec3(0));
+ m_brick2 = MobilizedBody::Ball(m_brick, BrickHalfDims,
+ brickBody, Vec3(-BrickHalfDims));
+ //m_brick3 = MobilizedBody::Ball(brick2, BrickHalfDims,
+ // brickBody, Vec3(-BrickHalfDims));
+
+/*
+1) t= 0.5, dt = 2 sec, pt = (0.05, 0.2, 0.4), fdir = (1,0,0), mag = 50N
+2) t= 4.0, dt = 0.5 sec, pt = (0.03, 0.06, 0.09), fdir = (0.2,0.8,0), mag = 300N
+3) t= 0.9, dt = 2 sec, pt = (0,0,0), fdir = (0,1,0), mag = 49.333N (half the weight of the block)
+4) t= 13.0, dt = 1 sec, pt = (0 0 0), fdir = (-1,0,0), mag = 200N
+*/
+ Force::Custom(forces, new MyPushForceImpl(m_brick, Vec3(0.05,0.2,0.4),
+ 50 * Vec3(1,0,0),
+ 0.5, 0.5+2));
+ Force::Custom(forces, new MyPushForceImpl(m_brick, Vec3(0.03, 0.06, 0.09),
+ 300 * UnitVec3(0.2,0.8,0),
+ //300 * Vec3(0.2,0.8,0),
+ 4, 4+0.5));
+ Force::Custom(forces, new MyPushForceImpl(m_brick, Vec3(0),
+ 1.25*49.033 * Vec3(0,1,0),
+ 9., 9.+2));
+ Force::Custom(forces, new MyPushForceImpl(m_brick, Vec3(0),
+ 200 * Vec3(-1,0,0),
+ 13, 13+1));
+
+ #ifndef USE_TIMS_PARAMS
+ // Extra late force.
+ Force::Custom(forces, new MyPushForceImpl(m_brick, Vec3(.1, 0, .45),
+ 20 * Vec3(-1,-1,.5),
+ 15, Infinity));
+ #endif
+
+ for (int i=-1; i<=1; i+=2)
+ for (int j=-1; j<=1; j+=2)
+ for (int k=-1; k<=1; k+=2) {
+ const Vec3 pt = Vec3(i,j,k).elementwiseMultiply(BrickHalfDims);
+ MyPointContact* contact = new MyPointContact
+ (Ground, YAxis, 0., m_brick, pt, CoefRest);
+ unis.addContactElement(contact);
+ unis.addFrictionElement(
+ new MyPointContactFriction(*contact, mu_d, mu_s, mu_v,
+ CaptureVelocity, // TODO: vtol?
+ forces));
+ if (i==-1 && j==-1 && k==-1)
+ continue;
+ MyPointContact* contact2 = new MyPointContact
+ (Ground, YAxis, 0., m_brick2, pt, CoefRest);
+ unis.addContactElement(contact2);
+ unis.addFrictionElement(
+ new MyPointContactFriction(*contact2, mu_d, mu_s, mu_v,
+ CaptureVelocity, // TODO: vtol?
+ forces));
+ //MyPointContact* contact3 = new MyPointContact
+ // (Ground, YAxis, 0., m_brick3, pt, CoefRest);
+ //unis.addContactElement(contact3);
+ //unis.addFrictionElement(
+ // new MyPointContactFriction(*contact3, mu_d, mu_s, mu_v,
+ // CaptureVelocity, // TODO: vtol?
+ // forces));
+ }
+}
+
+//---------------------------- CALC INITIAL STATE ------------------------------
+void TimsBox::calcInitialState(State& s) const {
+ s = realizeTopology(); // returns a reference to the the default state
+
+ //matter.setUseEulerAngles(s, true);
+
+ realizeModel(s); // define appropriate states for this System
+ realize(s, Stage::Instance); // instantiate constraints if any
+
+
+ /*
+ rX_q = 0.7 rad
+ rX_u = 1.0 rad/sec
+
+ rY_q = 0.6 rad
+ rY_u = 0.0 rad/sec
+
+ rZ_q = 0.5 rad
+ rZ_u = 0.2 rad/sec
+
+ tX_q = 0.0 m
+ tX_u = 10 m/s
+
+ tY_q = 1.4 m
+ tY_u = 0.0 m/s
+
+ tZ_q = 0.0 m
+ tZ_u = 0.0 m/s
+ */
+
+ #ifdef USE_TIMS_PARAMS
+ m_brick.setQToFitTranslation(s, Vec3(0,10,0));
+ m_brick.setUToFitLinearVelocity(s, Vec3(0,0,0));
+ #else
+ m_brick.setQToFitTranslation(s, Vec3(0,1.4,0));
+ m_brick.setUToFitLinearVelocity(s, Vec3(10,0,0));
+ const Rotation R_BC(SimTK::BodyRotationSequence,
+ 0.7, XAxis, 0.6, YAxis, 0.5, ZAxis);
+ m_brick.setQToFitRotation(s, R_BC);
+ m_brick.setUToFitAngularVelocity(s, Vec3(1,0,.2));
+ #endif
+
+ realize(s, Stage::Position);
+ Assembler(*this).setErrorTolerance(1e-6).assemble(s);
+}
+
+//==============================================================================
+// BOUNCING BALLS
+//==============================================================================
+
+BouncingBalls::BouncingBalls() {
+ m_tracker = new ContactTrackerSubsystem(*this);
+ m_contactForces = new CompliantContactSubsystem(*this, *m_tracker);
+
+ // Abbreviations.
+ SimbodyMatterSubsystem& matter = updMatterSubsystem();
+ GeneralForceSubsystem& forces = updForceSubsystem();
+ MyUnilateralConstraintSet& unis = updUnis();
+ MobilizedBody& Ground = matter.updGround();
+
+
+ // Build the multibody system.
+ m_gravity = Force::Gravity(forces, matter, -YAxis, 9.8066);
+ //m_damper = Force::GlobalDamper(forces, matter, .1);
+
+ const Real BallMass = 1;
+ const Real BallRadius = .25;
+ const Real CoefRest = 1;
+ const Real CaptureVelocity = .001;
+ const Real TransitionVelocity = .001;
+
+ // Rubber
+ const Real rubber_density = 1100.; // kg/m^3
+ const Real rubber_young = 0.01e9; // pascals (N/m)
+ const Real rubber_poisson = 0.5; // ratio
+ const Real rubber_planestrain =
+ ContactMaterial::calcPlaneStrainStiffness(rubber_young,rubber_poisson);
+ const Real rubber_dissipation = 0.1;
+ const ContactMaterial rubber(rubber_planestrain,rubber_dissipation,0,0,0);
+ // Nylon
+ const Real nylon_density = 1100.; // kg/m^3
+ const Real nylon_young = 2.5e9; // pascals (N/m)
+ const Real nylon_poisson = 0.4; // ratio
+ const Real nylon_planestrain =
+ ContactMaterial::calcPlaneStrainStiffness(nylon_young,nylon_poisson);
+ const Real nylon_dissipation = 0*0.1;
+ const ContactMaterial nylon(nylon_planestrain,nylon_dissipation,0,0,0);
+
+ const Rotation X2Y(Pi/2, ZAxis); // rotate +90 deg about z
+ const Rotation NegX2Y(-Pi/2,ZAxis); // -90
+
+ Ground.updBody().addContactSurface(Transform(NegX2Y,Vec3(0)),
+ ContactSurface(ContactGeometry::HalfSpace(),nylon));
+
+ unis.setCaptureVelocity(CaptureVelocity);
+ unis.setTransitionVelocity(TransitionVelocity);
+
+
+
+ // ADD MOBILIZED BODIES AND CONTACT CONSTRAINTS
+
+ Body::Rigid ballBody(MassProperties(BallMass, Vec3(0),
+ UnitInertia::sphere(BallRadius)));
+ ballBody.addDecoration(Transform(), DecorativeSphere(BallRadius));
+
+ const Vec3 HColor(Gray), PColor(Red), NColor(Orange);
+
+#ifdef HERTZ
+ m_Hballs[0] = MobilizedBody::Slider
+ (Ground, Transform(X2Y,Vec3(-1,BallRadius,0)),
+ ballBody, X2Y);
+ m_Hballs[0].updBody().addContactSurface(Vec3(0),
+ ContactSurface(ContactGeometry::Sphere(BallRadius), nylon));
+ m_Hballs[0].updBody().updDecoration(0).setColor(HColor);
+ for (int i=1; i<NBalls; ++i) {
+ m_Hballs[i] = MobilizedBody::Slider
+ (m_Hballs[i-1],Transform(X2Y,Vec3(0,2*BallRadius,0)),ballBody, X2Y);
+ m_Hballs[i].updBody().updDecoration(0).setColor(HColor);
+ m_Hballs[i].updBody().addContactSurface(Vec3(0),
+ ContactSurface(ContactGeometry::Sphere(BallRadius), nylon));
+ }
+#endif
+#ifdef POISSON
+ m_Pballs[0] = MobilizedBody::Slider
+ (Ground, Transform(X2Y,Vec3(0,BallRadius,0)),
+ ballBody, X2Y);
+ m_Pballs[0].updBody().updDecoration(0).setColor(PColor);
+ unis.addContactElement(new MyPointContact
+ (Ground, YAxis, 0., m_Pballs[0], Vec3(0,-BallRadius,0), CoefRest));
+ for (int i=1; i<NBalls; ++i) {
+ m_Pballs[i] = MobilizedBody::Slider
+ (m_Pballs[i-1],Transform(X2Y,Vec3(0,2*BallRadius,0)),ballBody, X2Y);
+ m_Pballs[i].updBody().updDecoration(0).setColor(PColor);
+ unis.addContactElement(new MyPointContact
+ (m_Pballs[i-1], YAxis, BallRadius,
+ m_Pballs[i], Vec3(0,-BallRadius,0), CoefRest));
+ }
+
+#endif
+#ifdef NEWTON
+ m_Nballs[0] = MobilizedBody::Slider
+ (Ground, Transform(X2Y,Vec3(1,BallRadius,0)),
+ ballBody, X2Y);
+ m_Nballs[0].updBody().updDecoration(0).setColor(NColor);
+ unis.addContactElement(new MyPointContact
+ (Ground, YAxis, 0., m_Nballs[0], Vec3(0,-BallRadius,0), CoefRest));
+ for (int i=1; i<NBalls; ++i) {
+ m_Nballs[i] = MobilizedBody::Slider
+ (m_Nballs[i-1],Transform(X2Y,Vec3(0,2*BallRadius,0)),ballBody, X2Y);
+ m_Nballs[i].updBody().updDecoration(0).setColor(NColor);
+ unis.addContactElement(new MyPointContact
+ (m_Nballs[i-1], YAxis, BallRadius,
+ m_Nballs[i], Vec3(0,-BallRadius,0), CoefRest));
+ }
+#endif
+
+}
+
+static const Real Separation = .1;
+void BouncingBalls::calcInitialState(State& s) const {
+ s = realizeTopology(); // returns a reference to the the default state
+ realizeModel(s); // define appropriate states for this System
+ realize(s, Stage::Instance); // instantiate constraints if any
+ realize(s, Stage::Position);
+ Assembler(*this).setErrorTolerance(1e-6).assemble(s);
+ #ifdef HERTZ
+ getHBall(0).setOneQ(s, MobilizerQIndex(0), 1);
+ getHBall(0).setOneU(s, MobilizerUIndex(0), -2);
+ for (int i=1; i<NBalls; ++i)
+ getHBall(i).setOneQ(s, MobilizerQIndex(0), Separation);
+ #endif
+ #ifdef POISSON
+ getPBall(0).setOneQ(s, MobilizerQIndex(0), 1);
+ getPBall(0).setOneU(s, MobilizerUIndex(0), -2);
+ for (int i=1; i<NBalls; ++i)
+ getPBall(i).setOneQ(s, MobilizerQIndex(0), Separation);
+ #endif
+ #ifdef NEWTON
+ getNBall(0).setOneQ(s, MobilizerQIndex(0), 1);
+ getNBall(0).setOneU(s, MobilizerUIndex(0), -2);
+ for (int i=1; i<NBalls; ++i)
+ getNBall(i).setOneQ(s, MobilizerQIndex(0), Separation);
+ #endif
+}
+
+
+//-------------------------- SHOW CONSTRAINT STATUS ----------------------------
+void MyUnilateralConstraintSet::
+showConstraintStatus(const State& s, const String& place) const
+{
+#ifndef NDEBUG
+ printf("\n%s: uni status @t=%.15g\n", place.c_str(), s.getTime());
+ m_mbs.realize(s, Stage::Dynamics);
+ for (int i=0; i < getNumContactElements(); ++i) {
+ const MyContactElement& contact = getContactElement(i);
+ const bool isActive = !contact.isDisabled(s);
+ printf(" %6s %2d %s, h=%g dh=%g f=%g\n",
+ isActive?"ACTIVE":"off", i, contact.getContactType().c_str(),
+ contact.getPerr(s),contact.getVerr(s),
+ isActive?contact.getForce(s):Zero);
+ }
+ for (int i=0; i < getNumFrictionElements(); ++i) {
+ const MyFrictionElement& friction = getFrictionElement(i);
+ if (!friction.isMasterActive(s))
+ continue;
+ const bool isEnabled = friction.isEnabled(s);
+ printf(" %8s friction %2d\n",
+ isEnabled?"STICKING":"sliding", i);
+ friction.writeFrictionInfo(s, " ", std::cout);
+ }
+ printf("\n");
+#endif
+}
diff --git a/Simbody/tests/adhoc/TwoPendulums.cpp b/Simbody/tests/adhoc/TwoPendulums.cpp
new file mode 100644
index 0000000..9b78409
--- /dev/null
+++ b/Simbody/tests/adhoc/TwoPendulums.cpp
@@ -0,0 +1,405 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2007-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * This is a simple example of using a constraint.
+ * Here we have two independent pendulums hanging from ground pins.
+ * They can be connected by a spring or a distance constraint.
+ */
+
+#include "SimTKsimbody.h"
+
+#include <cmath>
+#include <cstdio>
+#include <exception>
+#include <vector>
+
+using namespace std;
+using namespace SimTK;
+
+static const Real Deg2Rad = (Real)SimTK_DEGREE_TO_RADIAN,
+ Rad2Deg = (Real)SimTK_RADIAN_TO_DEGREE;
+
+static const Transform GroundFrame;
+
+static const Real m = 1; // kg
+static const Real g = 9.8; // meters/s^2; apply in �y direction
+static const Real d = 0.5; // meters
+
+class ShermsForce : public Force::Custom::Implementation {
+public:
+ ShermsForce(const MobilizedBody& b1, const MobilizedBody& b2) : body1(b1), body2(b2) { }
+ ShermsForce* clone() const {return new ShermsForce(*this);}
+
+ void calcForce(const State& state,
+ Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces,
+ Vector& mobilityForces) const
+ {
+ const Vec3& pos1 = body1.getBodyTransform(state).p();
+ const Vec3& pos2 = body2.getBodyTransform(state).p();
+ const Real d = (pos2-pos1).norm();
+ const Real k = 1000, d0 = 1;
+ const Vec3 f = k*(d-d0)*(pos2-pos1)/d;
+ body1.applyBodyForce(state, SpatialVec(Vec3(0), f), bodyForces);
+ body2.applyBodyForce(state, SpatialVec(Vec3(0), -f), bodyForces);
+ }
+ Real calcPotentialEnergy(const State& state) const {
+ return 0;
+ }
+private:
+ const MobilizedBody& body1;
+ const MobilizedBody& body2;
+};
+//template <class E> Vector_<E>
+//operator*(const VectorView_<E>& l, const typename CNT<E>::StdNumber& r)
+// { return Vector_<E>(l)*=r; }
+
+void ff(Vector& v) {
+ v = 23.;
+}
+
+int main(int argc, char** argv) {
+ try { // If anything goes wrong, an exception will be thrown.
+
+ // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
+ MultibodySystem mbs;
+
+ SimbodyMatterSubsystem twoPends(mbs);
+ GeneralForceSubsystem forces(mbs);
+ DecorationSubsystem viz(mbs);
+ Force::UniformGravity gravity(forces, twoPends, Vec3(0, -g, 0));
+ gravity.setDisabledByDefault(true);
+
+ // ADD BODIES AND THEIR MOBILIZERS
+ Body::Rigid pendulumBody = Body::Rigid(MassProperties(m, Vec3(0), Inertia(1)));
+ pendulumBody.addDecoration(Transform(),
+ DecorativeBrick(Vec3(.1,.0667,.05)).setOpacity(.5));
+
+ MobilizedBody:: Ball /*Gimbal*/ /*FreeLine*/ /*LineOrientation*/ /*Free*/
+ leftPendulum(twoPends.Ground(),
+ Transform(Vec3(-1, 0, 0)),
+ pendulumBody,
+ Transform(Vec3(0, d, 0)));
+/*
+ MobilizedBody::Ball
+ leftPendulum2(leftPendulum,
+ Transform(Vec3(0.5, 0, 0)),
+ pendulumBody,
+ Transform(Vec3(0, d, 0)));
+*/
+
+// leftPendulum.setDefaultRadius(0.2); // for Ball artwork
+
+ Vec3 radii(1.5/2.,1/3.,1/4.); radii*=.5; //radii=Vec3(.333,.5,1);
+ MobilizedBody::Ellipsoid rightPendulum(twoPends.Ground(), pendulumBody);
+ rightPendulum.setDefaultRadii(radii)
+ .setDefaultInboardFrame(Transform(Rotation(),Vec3(1,0,0)))
+ .setDefaultOutboardFrame(Transform( Rotation( SpaceRotationSequence, Pi/2, XAxis, -Pi/2, YAxis ), Vec3(0,d-radii[1],0)));
+
+ //rightPendulum.setDefaultAngle(20*Deg2Rad);
+ // rightPendulum.setDefaultRotation( Rotation(60*Deg2Rad, Vec3(0,0,1)) );
+
+ // OPTIONALLY TIE TOGETHER WITH SPRING/DAMPER OR DISTANCE CONSTRAINT
+
+ const Real distance = /*2*/1.5; // nominal length for spring; length for constraint
+ const Real stiffness = 100; // only if spring is used
+ const Real damping = 10; // "
+
+ char c;
+ cout << "Constraint, spring, or nothing? c/s/n"; cin >> c;
+
+ ConstraintIndex cid;
+ const Vec3 leftAttachPt(.1,0.05,0);
+ if (c == 'c') {
+
+ cid =
+ //Constraint::PointInPlane(twoPends.Ground(), UnitVec3(0,1,0), -2*d,
+ // leftPendulum2, Vec3(0))
+ Constraint::Rod(leftPendulum, leftAttachPt,
+ rightPendulum, Vec3(0),
+ distance)
+ // Constraint::Ball(leftPendulum2, Vec3(.5,0,0),
+ // twoPends.Ground(), Vec3(0,-d,0))
+ .getConstraintIndex();
+
+ } else if (c == 's') {
+ Force::TwoPointLinearSpring(forces, leftPendulum, leftAttachPt,
+ rightPendulum, Vec3(0),
+ stiffness, distance);
+ Force::TwoPointLinearDamper(forces, leftPendulum, leftAttachPt,
+ rightPendulum, Vec3(0),
+ damping);
+ }
+
+ // Add visualization line for spring (Rod constraint has one automatically)
+ if (c=='s')
+ viz.addRubberBandLine(leftPendulum, leftAttachPt,
+ rightPendulum, Vec3(0),
+ DecorativeLine().setColor(Orange).setLineThickness(4));
+
+ //forces.addMobilityConstantForce(leftPendulum, 0, 20);
+ //forces.addCustomForce(ShermsForce(leftPendulum,rightPendulum));
+ //forces.addGlobalEnergyDrain(1);
+
+ mbs.setHasTimeAdvancedEvents(false);
+
+ cout << "HAS TIME ADVANCED EVENTS=" << mbs.hasTimeAdvancedEvents() << endl;
+
+ Measure::Constant meas1(twoPends, 20);
+ const Real amp = 3, freq = 100, phase = Pi/2;
+ Measure::Sinusoid sint(twoPends, amp, freq, phase);
+
+ Measure::Integrate twentyPlus10t(twoPends, Measure::Constant(twoPends, 10), meas1);
+
+ State s = mbs.realizeTopology(); // returns a reference to the the default state
+ //twoPends.setUseEulerAngles(s, true);
+ mbs.realizeModel(s); // define appropriate states for this System
+
+ gravity.enable(s);
+ twentyPlus10t.setValue(s, 20);
+
+ mbs.realize(s, Stage::Instance); // instantiate constraints
+
+ cout << "meas1=" << meas1.getValue(s) << endl;
+
+
+ if (cid.isValid()) {
+ int mp, mv, ma;
+ twoPends.getConstraint(cid).getNumConstraintEquationsInUse(s, mp, mv, ma);
+ cout << "CONSTRAINT ID " << cid << " mp,v,a=" << mp << ", " << mv << ", " << ma << endl;
+ cout << "CONSTRAINT -- " << twoPends.getConstraint(cid).getSubtree();
+ }
+
+ for (MobilizedBodyIndex i(0); i < twoPends.getNumBodies(); ++i) {
+ const MobilizedBody& mb = twoPends.getMobilizedBody(i);
+ cout << "Body " << i
+ << " base=" << mb.getBaseMobilizedBody().getMobilizedBodyIndex()
+ << endl;
+ }
+
+
+ SimbodyMatterSubtree sub(twoPends);
+ sub.addTerminalBody(leftPendulum); sub.addTerminalBody(rightPendulum);
+ sub.realizeTopology();
+ cout << "SUB -- " << sub;
+
+ SimbodyMatterSubtreeResults results;
+ sub.initializeSubtreeResults(s, results);
+ cout << "INIT RESULTS=" << results;
+
+ Visualizer display(mbs);
+ display.setBackgroundType(Visualizer::SolidColor);
+
+ // gravity.disable(s);
+ mbs.realize(s, Stage::Position);
+ display.report(s);
+ cout << "q=" << s.getQ() << endl;
+ cout << "qErr=" << s.getQErr() << endl;
+ cout << "p_MbM=" << rightPendulum.getMobilizerTransform(s).p() << endl;
+
+ Vector_<SpatialVec> bodyForces;
+ Vector_<Vec3> particleForces;
+ Vector mobilityForces;
+ gravity.calcForceContribution(s,bodyForces,particleForces,mobilityForces);
+ cout << "Gravity forces: body:" << bodyForces << endl;
+ cout << " particle:" << particleForces << endl;
+ cout << " mobility:" << mobilityForces << endl;
+ cout << " PE=" << gravity.calcPotentialEnergyContribution(s) << endl;
+
+ if (cid.isValid()) {
+ const Constraint& c = twoPends.getConstraint(cid);
+ cout << "CONSTRAINT perr=" << c.getPositionErrorsAsVector(s)
+ << endl;
+ cout << " d(perrdot)/du=" << c.calcPositionConstraintMatrixP(s);
+ cout << " d(perr)/dq=" << c.calcPositionConstraintMatrixPNInv(s);
+ }
+
+ cout << "Default configuration shown. Ready? "; cin >> c;
+
+ sub.copyPositionsFromState(s, results);
+ cout << "POS RESULTS=" << results;
+
+ //leftPendulum.setAngle(s, -60*Deg2Rad);
+ //leftPendulum.setQToFitRotation(s, Rotation(-60*Deg2Rad,ZAxis));
+
+ //rightPendulum.setQToFitTranslation(s, Vec3(0,1,0));
+ leftPendulum.setQToFitRotation (s, Rotation(-.9*Pi/2,ZAxis));
+ rightPendulum.setQToFitRotation(s, Rotation(-.9*Pi/2,YAxis));
+
+
+ //TODO
+ //rightPendulum.setUToFitLinearVelocity(s, Vec3(1.1,0,1.2));
+
+ leftPendulum.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
+ rightPendulum.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
+
+
+ s.setTime(0);
+
+ mbs.realize(s, Stage::Velocity);
+ display.report(s);
+
+ cout << "q=" << s.getQ() << endl;
+ cout << "qErr=" << s.getQErr() << endl;
+ cout << "p_MbM=" << rightPendulum.getMobilizerTransform(s).p() << endl;
+ cout << "v_MbM=" << rightPendulum.getMobilizerVelocity(s)[1] << endl;
+ cout << "Unassembled configuration shown. Ready to assemble? "; cin >> c;
+
+ // These are the SimTK Simmath integrators:
+ RungeKuttaMersonIntegrator myStudy(mbs);
+ //CPodesIntegrator myStudy(mbs, CPodes::BDF, CPodes::Newton);
+ //myStudy.setOrderLimit(2); // cpodes only
+ //VerletIntegrator myStudy(mbs);
+
+
+ //myStudy.setMaximumStepSize(0.001);
+ myStudy.setAccuracy(1e-1);
+ //myStudy.setProjectEveryStep(true);
+ myStudy.setConstraintTolerance(1e-2);
+ //myStudy.setAllowInterpolation(false);
+ //myStudy.setMaximumStepSize(.1);
+
+ const Real dt = 1./60; // output intervals
+ const Real finalTime = 20;
+
+ myStudy.setFinalTime(finalTime);
+
+ // Peforms assembly if constraints are violated.
+ myStudy.initialize(s);
+
+ cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
+ cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
+ cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
+ cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
+ cout << "U WEIGHTS=" << s.getUWeights() << endl;
+ cout << "Z WEIGHTS=" << s.getZWeights() << endl;
+ cout << "1/QTOLS=" << s.getQErrWeights() << endl;
+ cout << "1/UTOLS=" << s.getUErrWeights() << endl;
+
+ {
+ const State& s = myStudy.getState();
+ display.report(s);
+ cout << "q=" << s.getQ() << endl;
+ cout << "qErr=" << s.getQErr() << endl;
+ cout << "p_MbM=" << rightPendulum.getMobilizerTransform(s).p() << endl;
+ cout << "Assembled configuration shown. Ready to simulate? "; cin >> c;
+ }
+
+ Integrator::SuccessfulStepStatus status;
+ int nextReport = 0;
+ int nextScheduledEvent = 0;
+ Real schedule[] = {1.234, 3.1415, 3.14159, 4.5, 9.090909, 100.};
+ while ((status=myStudy.stepTo(nextReport*dt,schedule[nextScheduledEvent]))
+ != Integrator::EndOfSimulation)
+ {
+ const State& s = myStudy.getState();
+ mbs.realize(s);
+ const Real leftPendulumAngle = leftPendulum.getBodyRotation(s).convertRotationToAngleAxis()[0] * Rad2Deg;
+
+ if (status == Integrator::ReachedScheduledEvent
+ || std::abs(std::floor(s.getTime()+0.5)-s.getTime())<1e-4)
+ {
+ printf("%5g %10.4g E=%10.8g h%3d=%g %s%s\n", s.getTime(),
+ leftPendulumAngle,
+ mbs.calcEnergy(s), myStudy.getNumStepsTaken(),
+ myStudy.getPreviousStepSizeTaken(),
+ Integrator::getSuccessfulStepStatusString(status).c_str(),
+ myStudy.isStateInterpolated()?" (INTERP)":"");
+ printf(" qerr=%10.8g uerr=%10.8g uderr=%10.8g\n",
+ twoPends.getQErr(s).normRMS(),
+ twoPends.getUErr(s).normRMS(),
+ twoPends.getUDotErr(s).normRMS());
+
+ cout << "t=" << s.getTime() << "sint=" << sint.getValue(s) << "a*sin(wt+p)="
+ << amp*std::sin(freq*s.getTime() + phase) << endl;
+
+ cout << "20+10t=" << twentyPlus10t.getValue(s) << endl;
+
+ if (cid.isValid()) {
+ const Constraint& c = twoPends.getConstraint(cid);
+ cout << "CONSTRAINT perr=" << c.getPositionErrorsAsVector(s)
+ << " verr=" << c.getVelocityErrorsAsVector(s)
+ << " aerr=" << c.getAccelerationErrorsAsVector(s)
+ << endl;
+ //cout << " d(perrdot)/du=" << c.calcPositionConstraintMatrixP(s);
+ //cout << " ~d(f)/d lambda=" << c.calcPositionConstraintMatrixPT(s);
+ //cout << " d(perr)/dq=" << c.calcPositionConstraintMatrixPQInverse(s);
+ }
+ }
+
+ Vector qdot;
+ twoPends.calcQDot(s, s.getU(), qdot);
+ // cout << "===> qdot =" << qdot << endl;
+
+ Vector qdot2;
+ twoPends.multiplyByN(s, false, s.getU(), qdot2);
+ // cout << "===> qdot2=" << qdot2 << endl;
+
+ Vector u1,u2;
+ twoPends.multiplyByNInv(s, false, qdot, u1);
+ twoPends.multiplyByNInv(s, false, qdot2, u2);
+ // cout << "===> u =" << s.getU() << endl;
+ // cout << "===> u1=" << u1 << endl;
+ // cout << "===> u2=" << u2 << endl;
+ // cout << " norm=" << (s.getU()-u2).normRMS() << endl;
+
+ //sub.copyPositionsFromState(s, results);
+ //sub.copyVelocitiesFromState(s, results);
+ // sub.copyAccelerationsFromState(s, results);
+ //cout << results;
+
+ display.report(s);
+ //if (s.getTime() >= finalTime)
+ // break;
+
+ //status = myStudy.stepTo(s.getTime() + dt);
+
+ //THIS CAN FAIL SOMETIMES
+ //if (s.getTime() >= nextReport*dt)
+ // ++nextReport;
+
+ if (status == Integrator::ReachedReportTime)
+ ++nextReport;
+
+ if (s.getTime() >= schedule[nextScheduledEvent])
+ ++nextScheduledEvent;
+ }
+
+ printf("Using Integrator %s:\n", myStudy.getMethodName());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());
+
+ }
+ catch (const exception& e) {
+ printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+ }
+ catch (...) {
+ printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+}
+
diff --git a/Simbody/tests/adhoc/UnilateralPointContact.cpp b/Simbody/tests/adhoc/UnilateralPointContact.cpp
new file mode 100644
index 0000000..b4bcf21
--- /dev/null
+++ b/Simbody/tests/adhoc/UnilateralPointContact.cpp
@@ -0,0 +1,1391 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) - UnilateralPointContact Example *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/*
+This example shows a manual approach to dealing with unilateral constraints in
+Simbody, which does not currently have built-in support but has sufficiently
+general facilities. In this example we'll implement non-slipping point
+contact, joint limit constraints, and a rope-like one-sided distance
+constraint. We'll use Simbody bilateral constraints turned on and off with
+manual switching conditions that are set by discrete event handlers.
+
+For each designated contact point that is not in contact, we'll track the
+vertical height over the ground plane and its first and second time derivatives
+and use those to construct switching ("witness") functions to trigger an event
+that may enable the constraint. For each enabled contact constraint, we'll
+track the sign of the normal reaction force and use it as a witness to disable
+the constraint.
+
+Note that there are two separate conditions involving these constraints:
+impact (collision) and contact. Impact occurs during an infinitesimal
+interval and involves impulses and velocities, while contact persists over time
+and involves forces and accelerations. Contact between rigid objects is a
+simple, physically justifiable process in which contact constraints generate
+forces if necessary to prevent interpenetration. Impact of rigid objects, on
+the other hand, requires assumptions to be made about the non-modeled details
+of collision behavior that is assumed to occur in an infinitesimal interval.
+Just producing logically-consistent behavior during impact is very difficult;
+justifying it physically even more so.
+
+How we handle contact
+---------------------
+In this example each contact consists of a constraint that prevents penetration
+of a point on a moving body normal to the ground plane, and constraints
+that prevent slipping tangent to the plane. We implement non-penetration with
+Simbody's "PointInPlane" constraint. We enable this
+constraint when a contact begins, defined so
+that its multiplier is the y component of the reaction force, with +y
+being the ground plane normal. We monitor the reaction force y component, and
+declare the contact broken if that component is negative. The no-slip condition
+is enforced with two of Simbody's "NoSlip1D" constraints, one in the x
+direction and one in the z direction.
+
+A rope is implemented similarly using Simbody's "Rod" (distance) constraint,
+and joint stops are implemented (somewhat inadequately) using the existing
+ConstantSpeed constraint, with the speed set to zero.
+
+How we handle impacts
+---------------------
+In this example, an impact is signaled by a contact point that reaches the
+ground plane with a negative vertical speed vy, with similar conditions for
+the other constraints. This requires a step change to
+the system velocities to avoid penetration or constraint violation. We achieve
+this step change by applying a constraint-space
+impulse to the system, representing constraint-space contact
+forces integrated over the assumed-infinitesimal impact interval. The system
+equations of motion are used to ensure that the velocity changes produced by
+the impulses satisfy Newton's laws. This can produce velocity changes anywhere
+in the system and may result in other impacts or breaking of
+existing contacts.
+
+When an impact is signaled, we determine the subset of potential contacts that
+may be involved in this event; those are called "proximal" contacts and are
+just those whose contact points are at zero height, within a small tolerance.
+The rest are ignored during handling of the impact.
+
+We use Poisson's interpretation of coefficient of restitution as a ratio of
+impulses, rather than Newton's more commonly known but inconsistent
+interpretation as a ratio of velocities. To apply Poisson's interpretation, we
+divide the impact into two distinct phases: compression and expansion.
+During compression we determine what impulse is required to prevent any
+penetration at the proximal contacts, by eliminating any negative speeds vy.
+The task for expansion is to determine an expansion impulse, based
+on the compression impulse, the coefficient of restitution e at each contact,
+and a "capture velocity" vc that says when a rebound velocity is so small we
+should consider a new persistent contact to be initiated.
+
+0) Initialize: initialize the effective coefficients of restitution e(i) for
+each of the proximal constraints. These can be constants associated with the
+contact parameters, or can be calculated from the initial velocities. Set
+the total applied impulse I=0, determine current velocity V. Activate all
+proximal constraints.
+
+1) Compression phase: Determine the nonnegative least squares constraint-space
+impulse Ic that brings any impacting proximal contacts ("impacters") to a stop,
+and leaves non-impacting ones ("rebounders") with a positive vertical speed
+(however small). Note that the set of impacters might not end up being the same
+ones as came in with negative vy; some new ones might be added and some of the
+originals might turn out to be rebounders due to the effects of other impacts.
+At the end we have for the i'th proximal constraint a compression impulse
+Ic(i)>=0 and a post-compression velocity Vc(i)>=0, with contact constraint i
+active (impacter) if Ic(i)>0 or Ic(i)==0 && Vc(i)==0 and inactive
+(rebounder) otherwise, with Ic(i)==0 and Vc(i)>0. Although it may take a few
+iterations to figure out what's going on, we consider everything to be
+simultaneous during a compression phase -- there is a single impulse Ic
+generated that modifies the original velocities to produce Vc. Increment the
+total impulse I+=Ic (>=0), set V=Vc (>=0).
+
+2) Expansion phase: Generate an expansion impulse Ie such that
+Ie(i)=e(i)*Ic(i) for each of the impacters i from the compression phase.
+If Ie==0 there is no expansion to do; go to step 4. Otherwise,
+set e(i)=0 for each of the impacters; the material restitution has now been
+consumed. Increment the total impulse I+=Ie. Apply the impulse Ie to produce a
+velocity change dVe and a new velocity Ve=V+dVe, and update V=Ve. If Ve>=0 for
+all proximal contacts, we are successful. In that case go to step 4 with the
+total impulse I>=0, and velocity V=Ve>=0; active constraints are those where
+V(k)=0.
+
+3) Some contacts now have negative
+vertical speeds vy (these may include both impacters and rebounders from the
+compression phase). This requires a new compression phase, beginning with
+these velocities and with the original impacters now having zero coefficients
+of restitution. So return to step 1.
+
+4) We have determined and applied the compression+expansion impulse I>=0 and
+have the resulting velocities V>=0. Check all contacts for which V>0 (the
+rebounders) to see if any is rebounding very slowly (<= vc). Enable those,
+and calculate the impulse dI that just brings those to zero while maintaining
+other contacts. Apply that impulse to get new velocities V. If that causes
+any V(k)<0 or new V(k)<=vc, declare that a contact too and recalculate dI;
+repeat until all inactive (rebounding) V(k)>vc. Then set the final I+=dI.
+
+5) Now calculate accelerations. If any of the active proximal contacts
+generate a zero or negative vertical reaction force they should be disabled;
+otherwise we would miss the next break-free event.
+*/
+
+#include "Simbody.h"
+
+#include <string>
+#include <iostream>
+#include <exception>
+
+using std::cout;
+using std::endl;
+
+using namespace SimTK;
+
+const Real ReportInterval=1./30;
+const Real RunTime=20;
+
+//==============================================================================
+// MY UNILATERAL CONSTRAINT
+//==============================================================================
+// This abstract class hides the details about which kind of constraint
+// we're dealing with, while giving us enough to work with for deciding what's
+// on and off and generating impulses.
+//
+// There is always a scalar associated with the constraint for making
+// decisions, although contact constraints may also have some additional
+// constraint equations for stiction.
+class MyUnilateralConstraint {
+public:
+ enum ImpulseType {Compression,Expansion,Capture};
+
+ MyUnilateralConstraint(Constraint uni, Real multSign, Real coefRest)
+ : m_uni(uni), m_multSign(multSign), m_coefRest(coefRest),
+ m_restitutionDone(false)
+ { m_uni.setDisabledByDefault(true); }
+
+ virtual ~MyUnilateralConstraint() {}
+
+ // These must be constructed so that a negative value means the
+ // unilateral constraint condition is violated.
+ virtual Real getPerr(const State& state) const = 0;
+ virtual Real getVerr(const State& state) const = 0;
+ virtual Real getAerr(const State& state) const = 0;
+
+ // This returns a point in the ground frame at which you might want to
+ // say the constraint is "located", for purposes of display. This should
+ // return something useful even if the constraint is currently off.
+ virtual Vec3 whereToDisplay(const State& state) const = 0;
+
+ // Returns zero if the constraint is not currently enabled.
+ Real getForce(const State& s) const {
+ if (isDisabled(s)) return 0;
+ const Vector mult = m_uni.getMultipliersAsVector(s);
+ assert(mult.size() == 1);
+ return m_multSign*mult[0];
+ }
+
+ // Override these if you have auxiliary constraints but be sure to
+ // invoke superclass method too.
+
+ virtual void enable(State& state) const {m_uni.enable(state);}
+ virtual void disable(State& state) const {m_uni.disable(state);}
+
+ virtual void setMyDesiredDeltaV(const State& s,
+ Vector& desiredDeltaV) const
+ { Vector myDesiredDV(1); myDesiredDV[0] = m_multSign*getVerr(s);
+ m_uni.setMyPartInConstraintSpaceVector(s, myDesiredDV,
+ desiredDeltaV); }
+
+ virtual void recordImpulse(ImpulseType type, const State& state,
+ const Vector& lambda) {
+ Vector myImpulse(1);
+ m_uni.getMyPartFromConstraintSpaceVector(state, lambda, myImpulse);
+ const Real I = myImpulse[0];
+ if (type==Compression) m_Ic = I;
+ else if (type==Expansion) m_Ie = I;
+ m_I += I;
+ }
+
+ // This is used by some constraints to collect position information that
+ // may be used later to set instance variables when enabling the underlying
+ // Simbody constraint. All constraints zero impulses here.
+ virtual void initializeForImpact(const State& state)
+ { setRestitutionDone(false); m_Ic = m_Ie = m_I = 0; }
+
+ // Impulse is accumulated internally.
+ Real getImpulse() const {return -m_multSign*m_I;}
+ Real getCompressionImpulse() const {return -m_multSign*m_Ic;}
+ Real getExpansionImpulse() const {return -m_multSign*m_Ie;}
+
+ Real getMyValueFromConstraintSpaceVector(const State& state,
+ const Vector& lambda) const
+ { Vector myValue(1);
+ m_uni.getMyPartFromConstraintSpaceVector(state, lambda, myValue);
+ return -m_multSign*myValue[0]; }
+
+ void setMyExpansionImpulse(const State& state,
+ Real coefRest,
+ Vector& lambda) const
+ { const Real I = coefRest * m_Ic;
+ Vector myImp(1); myImp[0] = I;
+ m_uni.setMyPartInConstraintSpaceVector(state, myImp, lambda); }
+
+ bool isDisabled(const State& state) const
+ { return m_uni.isDisabled(state); }
+
+ Real getCoefRest() const {return m_coefRest;}
+ void setRestitutionDone(bool isDone) {m_restitutionDone=isDone;}
+ bool isRestitutionDone() const {return m_restitutionDone;}
+
+protected:
+ Constraint m_uni;
+ const Real m_multSign; // 1 or -1
+ const Real m_coefRest;
+
+ // Runtime
+ bool m_restitutionDone;
+ Real m_Ic, m_Ie, m_I; // impulses
+};
+
+//==============================================================================
+// SHOW CONTACT
+//==============================================================================
+// For each visualization frame, generate ephemeral geometry to show just
+// during this frame, based on the current State.
+class ShowContact : public DecorationGenerator {
+public:
+ ShowContact(const Array_<MyUnilateralConstraint*>& unis)
+ : m_unis(unis) {}
+
+ void generateDecorations(const State& state,
+ Array_<DecorativeGeometry>& geometry) OVERRIDE_11
+ {
+ for (unsigned i=0; i < m_unis.size(); ++i) {
+ const MyUnilateralConstraint& uni = *m_unis[i];
+ const Vec3 loc = uni.whereToDisplay(state);
+ if (!uni.isDisabled(state)) {
+ geometry.push_back(DecorativeSphere(.5)
+ .setTransform(loc)
+ .setColor(Red).setOpacity(.25));
+ geometry.push_back(DecorativeText("LOCKED")
+ .setColor(White).setScale(.5)
+ .setTransform(loc+Vec3(0,.2,0)));
+ } else {
+ geometry.push_back(DecorativeText(String(i))
+ .setColor(White).setScale(.5)
+ .setTransform(loc+Vec3(0,.1,0)));
+ }
+ }
+ }
+private:
+ const Array_<MyUnilateralConstraint*>& m_unis;
+};
+
+
+
+//==============================================================================
+// STATE SAVER
+//==============================================================================
+// This reporter is called now and again to save the current state so we can
+// play back a movie at the end.
+class StateSaver : public PeriodicEventReporter {
+public:
+ StateSaver(const MultibodySystem& system,
+ const Array_<MyUnilateralConstraint*>& unis,
+ const Integrator& integ,
+ Real reportInterval)
+ : PeriodicEventReporter(reportInterval),
+ m_system(system), m_unis(unis), m_integ(integ)
+ { m_states.reserve(2000); }
+
+ ~StateSaver() {}
+
+ void clear() {m_states.clear();}
+ int getNumSavedStates() const {return (int)m_states.size();}
+ const State& getState(int n) const {return m_states[n];}
+
+ void handleEvent(const State& s) const {
+ const SimbodyMatterSubsystem& matter=m_system.getMatterSubsystem();
+ const SpatialVec PG = matter.calcSystemMomentumAboutGroundOrigin(s);
+
+#ifndef NDEBUG
+ printf("%3d: %5g mom=%g,%g E=%g", m_integ.getNumStepsTaken(),
+ s.getTime(),
+ PG[0].norm(), PG[1].norm(), m_system.calcEnergy(s));
+ cout << " Triggers=" << s.getEventTriggers() << endl;
+ for (unsigned i=0; i < m_unis.size(); ++i) {
+ const MyUnilateralConstraint& uni = *m_unis[i];
+ const bool isLocked = !uni.isDisabled(s);
+ printf(" Uni constraint %d is %s, h=%g dh=%g\n", i,
+ isLocked?"LOCKED":"unlocked", uni.getPerr(s),uni.getVerr(s));
+ if (isLocked) {
+ m_system.realize(s, Stage::Acceleration);
+ cout << " force=" << uni.getForce(s) << endl;
+ }
+ }
+#endif
+
+ m_states.push_back(s);
+ }
+private:
+ const MultibodySystem& m_system;
+ const Array_<MyUnilateralConstraint*>& m_unis;
+ const Integrator& m_integ;
+ mutable Array_<State> m_states;
+};
+
+
+
+//==============================================================================
+// CONTACT ON HANDLER
+//==============================================================================
+
+class ContactOn: public TriggeredEventHandler {
+public:
+ ContactOn(const MultibodySystem& system,
+ const Array_<MyUnilateralConstraint*>& unis,
+ unsigned which,
+ Stage stage)
+ : TriggeredEventHandler(stage),
+ m_mbs(system), m_unis(unis), m_which(which),
+ m_stage(stage)
+ {
+ // Trigger only as height goes from positive to negative.
+ getTriggerInfo().setTriggerOnRisingSignTransition(false);
+ }
+
+ // This is the witness function.
+ Real getValue(const State& state) const {
+ const SimbodyMatterSubsystem& matter = m_mbs.getMatterSubsystem();
+ const MyUnilateralConstraint& uni = *m_unis[m_which];
+ if (!uni.isDisabled(state))
+ return 0; // already locked
+
+ const Real height = uni.getPerr(state);
+
+ if (m_stage == Stage::Position)
+ return height;
+
+ // Velocity and acceleration triggers are not needed if we're
+ // above ground.
+ if (height > 0) return 0;
+
+ const Real dheight = uni.getVerr(state);
+
+ if (m_stage == Stage::Velocity)
+ return dheight;
+
+ // Acceleration trigger is not needed if velocity is positive.
+ if (dheight > 0) return 0;
+
+ const Real ddheight = uni.getAerr(state);
+
+ return ddheight;
+ }
+
+ // We're using Poisson's definition of the coefficient of
+ // restitution, relating impulses, rather than Newton's,
+ // relating velocities, since Newton's can produce non-physical
+ // results for a multibody system. For Poisson, calculate the impulse
+ // that would bring the velocity to zero, multiply by the coefficient
+ // of restitution to calculate the rest of the impulse, then apply
+ // both impulses to produce changes in velocity. In most cases this
+ // will produce the same rebound velocity as Newton, but not always.
+ void handleEvent(State& s, Real accuracy, bool& shouldTerminate) const;
+
+
+ // Make a list of all the unilateral constraints that could conceivably
+ // receive an impulse. Any constraint that is currently enabled, or any
+ // currently disabled constraint that is within posTol of contact is
+ // included.
+ void findProximalConstraints(const State& state,
+ Real posTol,
+ Array_<int>& proximal) const;
+
+
+
+ // Given the set of proximal constraints, prevent penetration by applying
+ // a nonnegative least squares impulse generating a step change in
+ // velocity. On return, the applied impulse and new velocities are recorded
+ // in proximal, and state is updated to the new velocities and realized
+ // through Velocity stage. Constraints that were stopped are enabled, those
+ // that rebounded are disabled.
+ void processCompressionPhase(Array_<int>& proximal,
+ State& state) const;
+
+ // Given a solution to the compression phase, including the compression
+ // impulse, the set of impacters (enabled) and rebounders (disabled and
+ // with positive rebound velocity), apply an expansion impulse based on
+ // the effective coefficients of restitution of the impacters. Wherever
+ // restitution is applied, the effective coefficient is reset to zero so
+ // that further restitution will not be done for that contact. Returns
+ // true if any expansion was done; otherwise nothing has changed.
+ // Expansion may result in some negative velocities, in which case it has
+ // induced further compression so another compression phase is required.
+ bool processExpansionPhase(Array_<int>& proximal,
+ State& state) const;
+
+ // Examine the rebounders to see if any are rebounding with a speed at or
+ // below the capture velocity. If so, enable those constraints and apply a
+ // (hopefully small) negative impulse to eliminate that rebound velocity.
+ // Repeat if that induces any negative velocities or any further slow
+ // rebounders. This terminates will all rebounders leaving with velocities
+ // greater than vCapture, or else all constraints are enabled.
+ void captureSlowRebounders(Real vCapture,
+ Array_<int>& proximal,
+ State& state) const;
+
+ // This method is used at the start of compression phase to modify any
+ // constraint parameters as necessary, and then enable all the proximal
+ // constraints. Some or all of these will be disabled during the impact
+ // analysis in compression or expansion phases. On return the state has
+ // been updated and realized through Instance stage.
+ void enableAllProximalConstraints(Array_<int>& proximal,
+ State& state) const;
+
+ // Given only the subset of proximal constraints that are active, calculate
+ // the impulse that would eliminate all their velocity errors. No change is
+ // made to the set of active constraints. Some of the resulting impulses
+ // may be negative.
+ void calcStoppingImpulse(const Array_<int>& proximal,
+ const State& state,
+ Vector& lambda0) const;
+
+ // Given the initial generalized speeds u0, and a constraint-space impulse
+ // lambda, calculate the resulting step velocity change du, modify the
+ // generalized speeds in state to u0+du, and realize Velocity stage.
+ void updateVelocities(const Vector& u0,
+ const Vector& lambda,
+ State& state) const;
+
+
+private:
+ const MultibodySystem& m_mbs;
+ const Array_<MyUnilateralConstraint*>& m_unis;
+ const unsigned m_which;
+ const Stage m_stage;
+};
+
+
+
+//==============================================================================
+// CONTACT OFF HANDLER
+//==============================================================================
+// Allocate one of these for each unilateral constraint. This handler takes
+// care of disabling an active constraint when its contact force crosses zero
+// from positive to negative.
+class ContactOff: public TriggeredEventHandler {
+public:
+ ContactOff(const MultibodySystem& system,
+ const Array_<MyUnilateralConstraint*>& unis,
+ unsigned which)
+ : TriggeredEventHandler(Stage::Acceleration),
+ m_mbs(system), m_unis(unis), m_which(which)
+ {
+ getTriggerInfo().setTriggerOnRisingSignTransition(false);
+ }
+
+ // This is the witness function.
+ Real getValue(const State& state) const {
+ const MyUnilateralConstraint& uni = *m_unis[m_which];
+ if (uni.isDisabled(state)) return 0;
+ const Real f = uni.getForce(state);
+ return f;
+ }
+
+ void handleEvent
+ (State& s, Real accuracy, bool& shouldTerminate) const
+ {
+ SimTK_DEBUG("\n----------------------------------------------------\n");
+ SimTK_DEBUG2("LIFTOFF triggered by constraint %d @t=%.15g\n",
+ m_which, s.getTime());
+ m_mbs.realize(s, Stage::Acceleration);
+
+ #ifndef NDEBUG
+ cout << " triggers=" << s.getEventTriggers() << "\n";
+ #endif
+
+ disablePullingContacts(m_mbs,s,m_unis);
+
+ SimTK_DEBUG("LIFTOFF DONE.\n");
+ SimTK_DEBUG("----------------------------------------------------\n");
+ }
+
+ // This is also used by ContactOn at the end.
+ static void disablePullingContacts
+ (const MultibodySystem& mbs, State& s,
+ const Array_<MyUnilateralConstraint*>& unis);
+
+private:
+ const MultibodySystem& m_mbs;
+ const Array_<MyUnilateralConstraint*>& m_unis;
+ const unsigned m_which; // one of the unis
+};
+
+
+
+//==============================================================================
+// MY POINT CONTACT
+//==============================================================================
+// Define a unilateral constraint to represent contact of a point on a moving
+// body with the ground plane. The ground normal is assumed to be +y. This
+// contact constraint has "super friction" that always sticks if it contacts
+// at all. Note: that can generate non-physical effects.
+class MyPointContact : public MyUnilateralConstraint {
+ typedef MyUnilateralConstraint Super;
+public:
+ MyPointContact(MobilizedBody& body, const Vec3& point,
+ Real coefRest)
+ : MyUnilateralConstraint
+ (Constraint::PointInPlane(updGround(body), UnitVec3(YAxis), Zero,
+ body, point),
+ Real(-1), // multiplier sign
+ coefRest),
+ m_body(body), m_point(point), m_groundPoint(0),
+ m_noslipX(updGround(body), Vec3(0), UnitVec3(XAxis),
+ updGround(body), body),
+ m_noslipZ(updGround(body), Vec3(0), UnitVec3(ZAxis),
+ updGround(body), body)
+ {
+ m_noslipX.setDisabledByDefault(true);
+ m_noslipZ.setDisabledByDefault(true);
+ }
+
+
+ Real getPerr(const State& s) const OVERRIDE_11 {
+ const Vec3 p = m_body.findStationLocationInGround(s, m_point);
+ return p[YAxis];
+ }
+ Real getVerr(const State& s) const OVERRIDE_11 {
+ const Vec3 v = m_body.findStationVelocityInGround(s, m_point);
+ return v[YAxis];
+ }
+ Real getAerr(const State& s) const OVERRIDE_11 {
+ const Vec3 a = m_body.findStationAccelerationInGround(s, m_point);
+ return a[YAxis];
+ }
+
+ Vec3 whereToDisplay(const State& state) const OVERRIDE_11 {
+ return m_body.findStationLocationInGround(state,m_point);
+ }
+
+ void recordImpulse(ImpulseType type, const State& state,
+ const Vector& lambda) OVERRIDE_11
+ {
+ Super::recordImpulse(type, state, lambda);
+
+ // Record translational impulse.
+ Vector myImpulseX(1), myImpulseZ(1);
+ m_noslipX.getMyPartFromConstraintSpaceVector(state, lambda, myImpulseX);
+ m_noslipZ.getMyPartFromConstraintSpaceVector(state, lambda, myImpulseZ);
+ const Vec2 tI(myImpulseX[0], myImpulseZ[0]);
+ if (type==Compression) m_tIc = tI;
+ else if (type==Expansion) m_tIe = tI;
+ m_tI += tI;
+ }
+
+ void setMyDesiredDeltaV(const State& s,
+ Vector& desiredDeltaV) const OVERRIDE_11
+ {
+ Super::setMyDesiredDeltaV(s, desiredDeltaV);
+ const Vec3 dv =
+ m_multSign*m_body.findStationVelocityInGround(s, m_point);
+ Vector myDesiredDV(1); // Nuke translational velocity also.
+ myDesiredDV[0] = dv[XAxis];
+ m_noslipX.setMyPartInConstraintSpaceVector(s, myDesiredDV, desiredDeltaV);
+ myDesiredDV[0] = dv[ZAxis];
+ m_noslipZ.setMyPartInConstraintSpaceVector(s, myDesiredDV, desiredDeltaV);
+ }
+
+ // Note that recordStartingLocation() must have been called first.
+ void enable(State& state) const OVERRIDE_11 {
+ Super::enable(state);
+ m_noslipX.setContactPoint(state, m_groundPoint);
+ m_noslipZ.setContactPoint(state, m_groundPoint);
+ m_noslipX.enable(state); m_noslipZ.enable(state);
+ }
+ void disable(State& state) const OVERRIDE_11 {
+ Super::disable(state);
+ m_noslipX.disable(state); m_noslipZ.disable(state);
+ }
+
+ // Set the ground point to be the projection of the follower point
+ // onto the ground plane. This will be used the next time this constraint
+ // is enabled.
+ void initializeForImpact(const State& s) OVERRIDE_11 {
+ Super::initializeForImpact(s);
+ const Vec3 p = m_body.findStationLocationInGround(s, m_point);
+ m_groundPoint = p;
+ m_tI = m_tIe = m_tIc = Vec2(0);
+ }
+private:
+ MobilizedBody& updGround(MobilizedBody& body) const {
+ SimbodyMatterSubsystem& matter = body.updMatterSubsystem();
+ return matter.updGround();
+ }
+
+ const MobilizedBody& m_body;
+ const Vec3 m_point;
+ Constraint::NoSlip1D m_noslipX, m_noslipZ;
+
+ // Runtime
+ Vec3 m_groundPoint;
+ Vec2 m_tIc; // most recent tangential compression impulse
+ Vec2 m_tIe; // most recent tangential expansion impulse
+ Vec2 m_tI; // accumulated tangential impulse
+};
+
+
+//==============================================================================
+// MY STOP
+//==============================================================================
+// Define a unilateral constraint to represent a joint stop that limits
+// the allowable motion of a single generalized coordinate. You can specify
+// a coefficient of restitution and whether the given limit is the upper or
+// lower limit.
+class MyStop : public MyUnilateralConstraint {
+public:
+ enum Side {Lower,Upper};
+ MyStop(Side side, MobilizedBody& body, int whichQ,
+ Real limit, Real coefRest)
+ : MyUnilateralConstraint
+ (Constraint::ConstantSpeed(body, MobilizerUIndex(whichQ), Real(0)),
+ Real(side==Lower?-1:1), coefRest),
+ m_body(body), m_whichq(whichQ), m_whichu(whichQ),
+ m_sign(side==Lower?1.:-1.), m_limit(limit)
+ {}
+
+ Real getPerr(const State& state) const OVERRIDE_11 {
+ const Real q = m_body.getOneQ(state, m_whichq);
+ return m_sign*(q-m_limit);
+ }
+ Real getVerr(const State& state) const OVERRIDE_11 {
+ const Real u = m_body.getOneU(state, m_whichu);
+ return m_sign*u;
+ }
+ Real getAerr(const State& state) const OVERRIDE_11 {
+ const Real udot = m_body.getOneUDot(state, m_whichu);
+ return m_sign*udot;
+ }
+
+ Vec3 whereToDisplay(const State& state) const OVERRIDE_11 {
+ const Vec3& p_B = m_body.getOutboardFrame(state).p();
+ return m_body.findStationLocationInGround(state,p_B);
+ }
+
+private:
+ const MobilizedBody& m_body;
+ const MobilizerQIndex m_whichq;
+ const MobilizerUIndex m_whichu;
+ Real m_sign; // +1: lower, -1: upper
+ Real m_limit;
+};
+
+//==============================================================================
+// MY ROPE
+//==============================================================================
+// Define a unilateral constraint to represent a "rope" that keeps the
+// distance between two points at or smaller than some limit.
+class MyRope : public MyUnilateralConstraint {
+public:
+ MyRope(MobilizedBody& body1, const Vec3& pt1,
+ MobilizedBody& body2, const Vec3& pt2, Real d,
+ Real coefRest)
+ : MyUnilateralConstraint
+ (Constraint::Rod(body1, pt1, body2, pt2, d), Real(1), coefRest),
+ m_body1(body1), m_point1(pt1), m_body2(body2), m_point2(pt2), m_dist(d)
+ {}
+
+
+ Real getPerr(const State& s) const OVERRIDE_11 {
+ const Vec3 p1 = m_body1.findStationLocationInGround(s,m_point1);
+ const Vec3 p2 = m_body2.findStationLocationInGround(s,m_point2);
+ const Vec3 p = p2-p1;
+ return (square(m_dist) - dot(p,p))/2;
+ }
+ Real getVerr(const State& s) const OVERRIDE_11 {
+ Vec3 p1, v1, p2, v2;
+ m_body1.findStationLocationAndVelocityInGround(s,m_point1,p1,v1);
+ m_body2.findStationLocationAndVelocityInGround(s,m_point2,p2,v2);
+ const Vec3 p = p2 - p1, v = v2 - v1;
+ return -dot(v, p);
+ }
+ Real getAerr(const State& s) const OVERRIDE_11 {
+ Vec3 p1, v1, a1, p2, v2, a2;
+ m_body1.findStationLocationVelocityAndAccelerationInGround
+ (s,m_point1,p1,v1,a1);
+ m_body2.findStationLocationVelocityAndAccelerationInGround
+ (s,m_point2,p2,v2,a2);
+ const Vec3 p = p2 - p1, v = v2 - v1, a = a2 - a1;
+ return -(dot(a, p) + dot(v, v));
+ }
+
+ Vec3 whereToDisplay(const State& state) const OVERRIDE_11 {
+ return m_body2.findStationLocationInGround(state,m_point2);
+ }
+
+private:
+ const MobilizedBody& m_body1;
+ const Vec3 m_point1;
+ const MobilizedBody& m_body2;
+ const Vec3 m_point2;
+ const Real m_dist;
+};
+
+
+
+//==============================================================================
+// MAIN
+//==============================================================================
+int main(int argc, char** argv) {
+ try { // If anything goes wrong, an exception will be thrown.
+
+ // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
+ MultibodySystem mbs;
+
+ SimbodyMatterSubsystem matter(mbs);
+ GeneralForceSubsystem forces(mbs);
+ Force::Gravity gravity(forces, matter, -YAxis, 9.81);
+
+ MobilizedBody& Ground = matter.updGround();
+
+ // Predefine some handy rotations.
+ const Rotation Z90(Pi/2, ZAxis); // rotate +90 deg about z
+
+
+ // ADD MOBILIZED BODIES AND CONTACT CONSTRAINTS
+ const Real CoefRest = 0.8;
+ Array_<MyUnilateralConstraint*> unis;
+
+ const Vec3 CubeHalfDims(3,2,1);
+ const Real CubeMass = 1;
+ Body::Rigid cubeBody =
+ Body::Rigid(MassProperties(CubeMass, Vec3(0),
+ UnitInertia::brick(CubeHalfDims)));
+
+ // First body: cube
+ MobilizedBody::Cartesian loc(Ground, MassProperties(0,Vec3(0),Inertia(0)));
+ MobilizedBody::Ball cube(loc, Vec3(0),
+ cubeBody, Vec3(0));
+ cube.addBodyDecoration(Transform(), DecorativeBrick(CubeHalfDims)
+ .setColor(Red).setOpacity(.3));
+ for (int i=-1; i<=1; i+=2)
+ for (int j=-1; j<=1; j+=2)
+ for (int k=-1; k<=1; k+=2) {
+ const Vec3 pt = Vec3(i,j,k).elementwiseMultiply(CubeHalfDims);
+ unis.push_back(new MyPointContact(cube, pt, CoefRest));
+ }
+
+ unis.push_back(new MyRope(Ground, Vec3(-5,10,0),
+ cube, Vec3(-CubeHalfDims[0],-CubeHalfDims[1],0),
+ 5., .5*CoefRest));
+ //unis.push_back(new MyStop(MyStop::Upper,loc,1, 2.5,CoefRest));
+
+ const Vec3 WeightEdge(-CubeHalfDims[0],-CubeHalfDims[1],0);
+//#ifdef NOTDEF
+ // Second body: weight
+ const Vec3 ConnectEdge1(CubeHalfDims[0],0,CubeHalfDims[2]);
+ MobilizedBody::Pin weight(cube,
+ Transform(Rotation(Pi/2,XAxis), ConnectEdge1),
+ cubeBody, Vec3(WeightEdge));
+ weight.addBodyDecoration(Transform(), DecorativeBrick(CubeHalfDims)
+ .setColor(Gray).setOpacity(.6));
+ //Force::MobilityLinearSpring(forces, weight, 0, 100, -Pi/4);
+ for (int i=-1; i<=1; i+=2)
+ for (int j=-1; j<=1; j+=2)
+ for (int k=-1; k<=1; k+=2) {
+ if (i==-1 && j==-1) continue;
+ const Vec3 pt = Vec3(i,j,k).elementwiseMultiply(CubeHalfDims);
+ unis.push_back(new MyPointContact(weight, pt, 0.5*CoefRest));
+ }
+ unis.push_back(new MyStop(MyStop::Upper,weight,0, Pi/9,0.1*CoefRest));
+ unis.push_back(new MyStop(MyStop::Lower,weight,0, -Pi/9,0.1*CoefRest));
+
+//#endif
+//#ifdef NOTDEF
+ // Third body: weight2
+ const Vec3 ConnectEdge2(CubeHalfDims[0],CubeHalfDims[1],0);
+ MobilizedBody::Pin weight2(cube,
+ Transform(Rotation(), ConnectEdge2),
+ cubeBody, Vec3(WeightEdge));
+ weight2.addBodyDecoration(Transform(), DecorativeBrick(CubeHalfDims)
+ .setColor(Cyan).setOpacity(.6));
+ Force::MobilityLinearSpring(forces, weight2, 0, 1000, Pi/4);
+ for (int i=-1; i<=1; i+=2)
+ for (int j=-1; j<=1; j+=2)
+ for (int k=-1; k<=1; k+=2) {
+ if (i==-1 && j==-1) continue;
+ const Vec3 pt = Vec3(i,j,k).elementwiseMultiply(CubeHalfDims);
+ unis.push_back(new MyPointContact(weight2, pt, CoefRest));
+ }
+//#endif
+
+ Visualizer viz(mbs);
+ viz.setShowSimTime(true);
+ viz.addDecorationGenerator(new ShowContact(unis));
+ mbs.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));
+
+ //ExplicitEulerIntegrator integ(mbs);
+ //CPodesIntegrator integ(mbs,CPodes::BDF,CPodes::Newton);
+ //RungeKuttaFeldbergIntegrator integ(mbs);
+ Real accuracy = 1e-2;
+ RungeKuttaMersonIntegrator integ(mbs);
+ //RungeKutta3Integrator integ(mbs);
+ //VerletIntegrator integ(mbs);
+ integ.setAccuracy(accuracy);
+ //integ.setAllowInterpolation(false);
+ integ.setMaximumStepSize(0.1);
+
+ StateSaver* stateSaver = new StateSaver(mbs,unis,integ,ReportInterval);
+ mbs.addEventReporter(stateSaver);
+
+ for (unsigned i=0; i < unis.size(); ++i) {
+ mbs.addEventHandler(new ContactOn(mbs, unis,i, Stage::Position));
+ mbs.addEventHandler(new ContactOn(mbs, unis,i, Stage::Velocity));
+ mbs.addEventHandler(new ContactOn(mbs, unis,i, Stage::Acceleration));
+ }
+
+ for (unsigned i=0; i < unis.size(); ++i)
+ mbs.addEventHandler(new ContactOff(mbs, unis,i));
+
+ State s = mbs.realizeTopology(); // returns a reference to the the default state
+ mbs.realizeModel(s); // define appropriate states for this System
+ mbs.realize(s, Stage::Instance); // instantiate constraints if any
+
+
+ // Set initial conditions so the -,-,- vertex is in the -y direction.
+ const Rotation R_BC(UnitVec3(CubeHalfDims+1e-7*Vec3(1,0,0)), YAxis, Vec3(1,0,0),XAxis);
+ loc.setQToFitTranslation(s, Vec3(0,10,0));
+ cube.setQToFitTransform(s, Transform(~R_BC, Vec3(0)));
+ cube.setUToFitAngularVelocity(s, Vec3(0,1,0));
+ cube.setUToFitLinearVelocity(s, Vec3(1,0,0));
+
+ mbs.realize(s, Stage::Velocity);
+ viz.report(s);
+
+ Array_<int> enableThese;
+ for (unsigned i=0; i < unis.size(); ++i) {
+ const Real perr = unis[i]->getPerr(s);
+ printf("uni constraint %d has perr=%g%s\n", i, perr,
+ perr<=0?" (ENABLING)":"");
+ if (perr <= 0)
+ enableThese.push_back(i);
+ }
+
+ cout << "Initial configuration shown. Next? ";
+ getchar();
+
+ for (unsigned i=0; i < enableThese.size(); ++i)
+ unis[enableThese[i]]->enable(s);
+
+ Assembler(mbs).assemble(s);
+ viz.report(s);
+ cout << "Assembled configuration shown. Ready? ";
+ getchar();
+
+ // Simulate it.
+
+ TimeStepper ts(mbs, integ);
+ ts.initialize(s);
+
+ const double startReal = realTime();
+ const double startCPU = cpuTime();
+
+ ts.stepTo(RunTime);
+
+ const double timeInSec = realTime()-startReal;
+ const double cpuInSec = cpuTime()-startCPU;
+ const int evals = integ.getNumRealizations();
+ cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
+ timeInSec << "s for " << ts.getTime() << "s sim (avg step="
+ << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
+ << (1000*ts.getTime())/evals << "ms/eval\n";
+ cout << "CPUtime " << cpuInSec << endl;
+
+ printf("Used Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
+
+ // Instant replay.
+ while(true) {
+ for (int i=0; i < stateSaver->getNumSavedStates(); ++i) {
+ viz.report(stateSaver->getState(i));
+ }
+ getchar();
+ }
+
+ }
+ catch (const std::exception& e) {
+ printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+ }
+ catch (...) {
+ printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+}
+
+
+//==============================================================================
+// IMPACT HANDLING (CONTACT ON)
+//==============================================================================
+
+//------------------------------ HANDLE EVENT ----------------------------------
+void ContactOn::
+handleEvent(State& s, Real accuracy, bool& shouldTerminate) const
+{
+ const Real VCapture=1e-2;
+
+ Array_<int> proximal;
+ findProximalConstraints(s, accuracy, proximal);
+
+ SimTK_DEBUG4("\nIMPACT (%s) for uni constraint %d at t=%.16g; %d proximal\n",
+ m_stage.getName().c_str(), m_which, s.getTime(), proximal.size());
+
+ bool needMoreCompression = true;
+ while (needMoreCompression) {
+ processCompressionPhase(proximal, s);
+ needMoreCompression = false;
+
+ if (processExpansionPhase(proximal, s)) {
+ for (unsigned i=0; i<proximal.size(); ++i) {
+ const MyUnilateralConstraint& uni = *m_unis[proximal[i]];
+ if (uni.getVerr(s) < 0) {
+ needMoreCompression = true;
+ break;
+ }
+ }
+ }
+ }
+
+ // Some of the rebounders may be moving so slowly that we would like
+ // to be able to say they have stopped. If so, apply additional
+ // (negative) impulses necessary to stop them; enable their contact
+ // constraints.
+ captureSlowRebounders(VCapture, proximal, s);
+
+ // Make sure all enabled position and velocity constraints
+ // are satisfied.
+ m_mbs.project(s, accuracy);
+
+ // Finally, evaluate accelerations and reaction forces and check if
+ // any of the active contacts are generating negative ("pulling")
+ // forces; if so, inactivate them.
+ ContactOff::disablePullingContacts(m_mbs, s, m_unis);
+
+#ifndef NDEBUG
+ printf("END OF IMPACT for %d proximal constraints:\n",proximal.size());
+ for (unsigned i=0; i < proximal.size(); ++i) {
+ const int which = proximal[i];
+ const MyUnilateralConstraint& uni = *m_unis[which];
+ printf(" %d %3s: I=%g H=%g V=%g A=%g F=%g\n",
+ which, uni.isDisabled(s) ? "off" : "ON",
+ uni.getImpulse(), uni.getPerr(s), uni.getVerr(s),
+ uni.getAerr(s), uni.getForce(s));
+ }
+ printf("DONE WITH IMPACT.\n\n");
+#endif
+}
+
+
+
+//------------------------ FIND PROXIMAL CONSTRAINTS ---------------------------
+void ContactOn::
+findProximalConstraints(const State& s,
+ Real posTol,
+ Array_<int>& proximal) const
+{
+ m_mbs.realize(s, Stage::Position);
+
+ proximal.clear();
+ for (unsigned i=0; i<m_unis.size(); ++i) {
+ MyUnilateralConstraint& uni = *m_unis[i];
+ if (!uni.isDisabled(s) || uni.getPerr(s) <= posTol)
+ {
+ uni.initializeForImpact(s);
+ proximal.push_back(i);
+ }
+ }
+}
+
+
+
+//------------------------ PROCESS COMPRESSION PHASE ---------------------------
+//
+// Strategy:
+// (1) assume all normal & tangential constraints are on; calculate stopping
+// impulse
+// (2) look for negative normal impulses; try disabling those
+// (3) recapture any constraints that would be violated after disabling
+//
+void ContactOn::
+processCompressionPhase(Array_<int>& proximal,
+ State& s) const
+{
+ SimTK_DEBUG("Entering processCompressionPhase() ...\n");
+ Vector lambda0, lambdaTry;
+ Array_<int> maybeDisabled, recapturing, turnOffStiction;
+
+ const Vector u0 = s.getU(); // save presenting velocity
+
+ // Assume at first that everyone will participate. This is necessary to
+ // ensure that we get a least squares solution for the impulse involving
+ // as many constraints as possible sharing the impulse.
+ enableAllProximalConstraints(proximal, s);
+
+ // First try drives all constraints to zero velocity; if that took some
+ // negative impulses we'll have to remove some participants and try again.
+ calcStoppingImpulse(proximal, s, lambda0);
+
+ while (true) {
+ // See if negative impulse was required to satisfy an active constraint.
+ // If so, that is a candidate to be inactivated.
+ maybeDisabled.clear();
+ for (unsigned i=0; i < proximal.size(); ++i) {
+ const int which = proximal[i];
+ const MyUnilateralConstraint& uni = *m_unis[which];
+ if (uni.isDisabled(s))
+ continue;
+ Real imp = uni.getMyValueFromConstraintSpaceVector(s, lambda0);
+ if (imp > 0)
+ continue;
+ maybeDisabled.push_back(which);
+ SimTK_DEBUG2(" uni constraint %d has negative compression impulse=%g\n",
+ which, imp);
+ }
+ if (maybeDisabled.empty())
+ break;
+
+ // Disable the candidates, then see if they rebound.
+ for (unsigned d=0; d < maybeDisabled.size(); ++d)
+ m_unis[maybeDisabled[d]]->disable(s);
+ m_mbs.realize(s, Stage::Instance);
+ calcStoppingImpulse(proximal, s, lambdaTry);
+ updateVelocities(u0, lambdaTry, s);
+
+ recapturing.clear();
+ for (unsigned i=0; i<maybeDisabled.size(); ++i) {
+ const int which = maybeDisabled[i];
+ const MyUnilateralConstraint& uni = *m_unis[which];
+ const Real newV = uni.getVerr(s);
+ SimTK_DEBUG2(" candidate uni constraint %d would have v=%g\n",
+ which, newV);
+ if (newV <= 0) {
+ recapturing.push_back(which);
+ SimTK_DEBUG2(" RECAPTURING uni constraint %d with v=%g\n",
+ which, newV);
+ }
+ }
+
+ // Re-enable the recaptured candidates.
+ if (!recapturing.empty()) {
+ for (unsigned c=0; c < recapturing.size(); ++c)
+ m_unis[recapturing[c]]->enable(s);
+ m_mbs.realize(s, Stage::Instance);
+ }
+
+ const int numDisabled = maybeDisabled.size()-recapturing.size();
+ if (numDisabled == 0) {
+ SimTK_DEBUG(" None of the candidates was actually disabled.\n");
+ // lambda0 is still correct
+ break;
+ }
+
+ if (recapturing.empty()) lambda0 = lambdaTry;
+ else calcStoppingImpulse(proximal, s, lambda0);
+
+ SimTK_DEBUG1(" RETRY with %d constraints disabled\n", numDisabled);
+ }
+ updateVelocities(u0, lambda0, s);
+
+ // Now update the entries for each proximal constraint to reflect the
+ // compression impulse and post-compression velocity.
+ SimTK_DEBUG(" Compression results:\n");
+ for (unsigned i=0; i < proximal.size(); ++i) {
+ const int which = proximal[i];
+ MyUnilateralConstraint& uni = *m_unis[which];
+ if (!uni.isDisabled(s))
+ uni.recordImpulse(MyUnilateralConstraint::Compression, s, lambda0);
+ SimTK_DEBUG4(" %d %3s: Ic=%g, V=%g\n",
+ which, uni.isDisabled(s) ? "off" : "ON",
+ uni.getCompressionImpulse(), uni.getVerr(s));
+ }
+
+ SimTK_DEBUG("... compression phase done.\n");
+}
+
+
+
+//------------------------- PROCESS EXPANSION PHASE ----------------------------
+bool ContactOn::
+processExpansionPhase(Array_<int>& proximal,
+ State& s) const
+{
+ SimTK_DEBUG("Entering processExpansionPhase() ...\n");
+
+ // Generate an expansion impulse if there were any active contacts that
+ // still have some restitution remaining.
+ Vector expansionImpulse;
+
+ bool anyChange = false;
+ for (unsigned i=0; i<proximal.size(); ++i) {
+ const int which = proximal[i];
+ MyUnilateralConstraint& uni = *m_unis[which];
+ if (uni.isDisabled(s)||uni.isRestitutionDone()||uni.getCoefRest()==0
+ ||uni.getCompressionImpulse()<=0)
+ continue;
+ uni.setMyExpansionImpulse(s, uni.getCoefRest(), expansionImpulse);
+ uni.recordImpulse(MyUnilateralConstraint::Expansion,s,expansionImpulse);
+ uni.setRestitutionDone(true);
+ anyChange = true;
+ }
+
+ if (!anyChange) {
+ SimTK_DEBUG("... no expansion impulse -- done.\n");
+ return false;
+ }
+
+ // We generated an expansion impulse. Apply it and update velocities.
+ updateVelocities(Vector(), expansionImpulse, s);
+
+ // Release any constraint that now has a positive velocity.
+ Array_<int> toDisable;
+ for (unsigned i=0; i < proximal.size(); ++i) {
+ const int which = proximal[i];
+ const MyUnilateralConstraint& uni = *m_unis[which];
+ if (!uni.isDisabled(s) && uni.getVerr(s) > 0)
+ toDisable.push_back(which);
+ }
+
+ // Now do the actual disabling (can't mix this with checking velocities)
+ // because disabling invalidates Instance stage.
+ for (unsigned i=0; i < toDisable.size(); ++i) {
+ const int which = toDisable[i];
+ const MyUnilateralConstraint& uni = *m_unis[which];
+ uni.disable(s);
+ }
+
+ SimTK_DEBUG(" Expansion results:\n");
+ m_mbs.realize(s, Stage::Velocity);
+ for (unsigned i=0; i < proximal.size(); ++i) {
+ const int which = proximal[i];
+ const MyUnilateralConstraint& uni = *m_unis[which];
+ SimTK_DEBUG4(" %d %3s: Ie=%g, V=%g\n",
+ which, uni.isDisabled(s) ? "off" : "ON",
+ uni.getExpansionImpulse(), uni.getVerr(s));
+ }
+
+ SimTK_DEBUG("... expansion phase done.\n");
+
+ return true;
+}
+
+
+//------------------------- CAPTURE SLOW REBOUNDERS ----------------------------
+void ContactOn::
+captureSlowRebounders(Real vCapture,
+ Array_<int>& proximal,
+ State& s) const
+{
+ // Capture any rebounder whose velocity is too slow.
+ SimTK_DEBUG("Entering captureSlowRebounders() ...\n");
+
+ int nCaptured=0, nPasses=0;
+ while (true) {
+ ++nPasses;
+ SimTK_DEBUG1(" start capture pass %d:\n", nPasses);
+ Array_<int> toCapture;
+ for (unsigned i=0; i < proximal.size(); ++i) {
+ const int which = proximal[i];
+ MyUnilateralConstraint& uni = *m_unis[which];
+ if (uni.isDisabled(s) && uni.getVerr(s) <= vCapture) {
+ toCapture.push_back(which);
+ ++nCaptured;
+ SimTK_DEBUG2(" capturing constraint %d with v=%g\n",
+ which, uni.getVerr(s));
+ }
+ }
+
+ if (toCapture.empty()) {
+ if (nCaptured==0) SimTK_DEBUG("... done -- nothing captured.\n");
+ else SimTK_DEBUG2("... done -- captured %d rebounders in %d passes.\n",
+ nCaptured, nPasses);
+ return;
+ }
+
+ // Now do the actual capturing by enabling constraints.
+ for (unsigned i=0; i < toCapture.size(); ++i) {
+ const int which = toCapture[i];
+ MyUnilateralConstraint& uni = *m_unis[which];
+ uni.enable(s);
+ }
+
+ m_mbs.realize(s, Stage::Velocity);
+ Vector captureImpulse;
+ calcStoppingImpulse(proximal, s, captureImpulse);
+ updateVelocities(Vector(), captureImpulse, s);
+
+ // Now update the entries for each proximal constraint to reflect the
+ // capture impulse and post-capture velocity.
+ for (unsigned i=0; i < proximal.size(); ++i) {
+ const int which = proximal[i];
+ MyUnilateralConstraint& uni = *m_unis[which];
+ if (!uni.isDisabled(s))
+ uni.recordImpulse(MyUnilateralConstraint::Capture,
+ s, captureImpulse);
+ }
+ }
+}
+
+
+
+//---------------------- ENABLE ALL PROXIMAL CONSTRAINTS -----------------------
+void ContactOn::
+enableAllProximalConstraints(Array_<int>& proximal,
+ State& state) const
+{
+ // Set the contact point and enable the constraints.
+ for (unsigned i=0; i < proximal.size(); ++i) {
+ const int which = proximal[i];
+ const MyUnilateralConstraint& uni = *m_unis[which];
+ if (uni.isDisabled(state))
+ uni.enable(state);
+ }
+ m_mbs.realize(state, Stage::Instance);
+}
+
+
+
+//------------------------- CALC COMPRESSION IMPULSE ---------------------------
+// Calculate the impulse that eliminates all residual velocity for the
+// current set of enabled constraints.
+void ContactOn::
+calcStoppingImpulse(const Array_<int>& proximal,
+ const State& s,
+ Vector& lambda0) const
+{
+ const SimbodyMatterSubsystem& matter = m_mbs.getMatterSubsystem();
+ m_mbs.realize(s, Stage::Dynamics); // TODO: should only need Position
+ Vector desiredDeltaV; // in constraint space
+ SimTK_DEBUG(" Entering calcStoppingImpulse() ...\n");
+ bool gotOne = false;
+ for (unsigned i=0; i < proximal.size(); ++i) {
+ const int which = proximal[i];
+ const MyUnilateralConstraint& uni = *m_unis[which];
+ if (uni.isDisabled(s))
+ continue;
+ SimTK_DEBUG2(" uni constraint %d enabled, v=%g\n",
+ which, uni.getVerr(s));
+ uni.setMyDesiredDeltaV(s, desiredDeltaV);
+ gotOne = true;
+ }
+ if (gotOne) matter.solveForConstraintImpulses(s, desiredDeltaV, lambda0);
+ else lambda0.clear();
+#ifndef NDEBUG
+ cout << " ... done. Stopping impulse=" << lambda0 << endl;
+#endif
+}
+
+
+
+//---------------------------- UPDATE VELOCITIES -------------------------------
+void ContactOn::
+updateVelocities(const Vector& u0, const Vector& lambda, State& state) const {
+ const SimbodyMatterSubsystem& matter = m_mbs.getMatterSubsystem();
+ Vector f, deltaU;
+ assert(u0.size()==0 || u0.size() == state.getNU());
+
+ m_mbs.realize(state, Stage::Dynamics); // TODO: Position
+ matter.multiplyByGTranspose(state,lambda,f);
+ matter.multiplyByMInv(state,f,deltaU);
+ if (u0.size()) state.updU() = u0 + deltaU;
+ else state.updU() += deltaU;
+ m_mbs.realize(state, Stage::Velocity);
+}
+
+
+
+//==============================================================================
+// CONTACT OFF
+//==============================================================================
+
+//-------------------------- DISABLE PULLING CONTACTS --------------------------
+// This method checks the active contacts to see if any of them are generating
+// "pulling" forces. In that case the contact is disabled unless that would
+// lead to a negative acceleration.
+// This is invoked by the ContactOff handler, and as the last step of the
+// ContactOn (impact) handler.
+// TODO: need to search for a consistent set of active contraints.
+
+/*static*/ void ContactOff::disablePullingContacts
+ (const MultibodySystem& mbs, State& s,
+ const Array_<MyUnilateralConstraint*>& unis)
+{
+ SimTK_DEBUG("Entering disablePullingContacts() ...\n");
+
+ mbs.realize(s, Stage::Acceleration);
+ // Check first, disable later because we don't want to invalidate
+ // the reaction forces in the state yet.
+ Array_<int> toBeDisabled;
+ for (unsigned i=0; i < unis.size(); ++i) {
+ const MyUnilateralConstraint& uni = *unis[i];
+ if (uni.isDisabled(s)) continue;
+ const Real f = uni.getForce(s);
+ if (f<0) {
+ SimTK_DEBUG2(" consider disabling uni %d because force=%g\n",
+ i, f);
+ toBeDisabled.push_back(i);
+ }
+ }
+
+ // OK, now tentatively disable the pulling contacts.
+ for (unsigned tbd=0; tbd < toBeDisabled.size(); ++tbd) {
+ const int i = toBeDisabled[tbd];
+ const MyUnilateralConstraint& uni = *unis[i];
+ uni.disable(s);
+ }
+
+ // Now see which of the disabled constraints is violated.
+ mbs.realize(s, Stage::Acceleration);
+ Array_<int> violated;
+ for (unsigned p=0; p < toBeDisabled.size(); ++p) {
+ const int which = toBeDisabled[p];
+ const MyUnilateralConstraint& uni = *unis[which];
+ const Real aerr = uni.getAerr(s);
+ if (aerr < 0) {
+ violated.push_back(which);
+ SimTK_DEBUG2(" RE-ENABLE constraint %d cuz aerr=%g\n",
+ which, aerr);
+ }
+ }
+
+ // Re-enable now.
+ for (unsigned v=0; v < violated.size(); ++v) {
+ const int which = violated[v];
+ const MyUnilateralConstraint& uni = *unis[which];
+ uni.enable(s);
+ }
+
+ // Always leave at acceleration stage.
+ mbs.realize(s, Stage::Acceleration);
+
+ SimTK_DEBUG1("... Done; %d contacts broken.\n",
+ toBeDisabled.size()-violated.size());
+}
diff --git a/Simbody/tests/adhoc/UnilateralPointContactWithFriction.cpp b/Simbody/tests/adhoc/UnilateralPointContactWithFriction.cpp
new file mode 100644
index 0000000..02178d5
--- /dev/null
+++ b/Simbody/tests/adhoc/UnilateralPointContactWithFriction.cpp
@@ -0,0 +1,2921 @@
+/* -------------------------------------------------------------------------- *
+ * Simbody(tm) - UnilateralPointContactWithFriction Example *
+ * -------------------------------------------------------------------------- *
+ * 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) 2012 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/* This example extends the method used in the UnilateralPointContact example
+to add sliding friction and transitions from sticking to sliding and vice
+versa. See UnilateralPointContact for basic information.
+
+Here we separate contact elements from friction elements, and introduce the
+idea of associating a force element with each friction element that is used
+to generate forces when the contact is sliding rather than sticking. Sticking
+continues to be implemented using "no slip" constraints. For any active contact,
+the associated friction element is either sticking (with active constraints) or
+sliding (with an active force element).
+
+Sliding introduces several new problems: (1) The magnitude of the applied force
+mu_d*N depends on a normal force that may in turn depend on the applied force,
+(2) the direction of that force normally opposes the slip direction but at very
+small velocities that direction is essentially noise, (3) how to detect a
+transition to sticking. To address these problems
+the force element uses two discrete state variables, one to hold the most-
+recently-calculated normal force prevN, and the other to hold the previous slip
+direction prevSlipDir. When the velocity is high enough to be reliable we use
+its direction for the force, and update prevSlipDir for next time. Otherwise we
+use prevSlipDir as the direction and don't update it. For the force magnitude,
+here we simply use mu_d*prevN, meaning we're one step out of date. In the
+real Simbody implementation we will iterate if necessary until prevN and the
+current N are the same to within a tolerance. However, when first switching to
+sliding the event handler does converge the normal force using functional
+iteration (that is, we replace prevN by N several times).
+
+For the sliding to sticking transition, we generate a witness function that
+watches for velocity reversal. Watching for zero velocity doesn't work because
+the near-constant acceleration generated by the mu_d*N force will typically
+reverse the velocity within a single integration step, so zero is unlikely to
+be seen. We use the current velocity dotted with prevSlipDir instead, and then
+the event isolation code can find where the zero crossing occurred and call the
+event handler then.
+
+Note: the impact handler here ignores sliding friction and only generates
+tangential impulses for stiction constraints that were already enabled on
+input. Those may get disabled if they would otherwise cause negative normal
+impulses, but no new ones will get enabled.
+
+*/
+//#define NDEBUG 1
+
+#include "Simbody.h"
+
+#include <string>
+#include <iostream>
+#include <exception>
+
+using std::cout;
+using std::endl;
+
+using namespace SimTK;
+
+#define ANIMATE // off to get more accurate CPU time (you can still playback)
+
+// Define this to run the simulation NTries times, saving the states and
+// comparing them bitwise to see if the simulations are perfectly repeatable
+// as they should be. You should see nothing but exact zeroes print out for
+// second and subsequent runs.
+//#define TEST_REPEATABILITY
+static const int NTries=3;
+
+const Real ReportInterval=1./30;
+
+//==============================================================================
+// MY CONTACT ELEMENT
+//==============================================================================
+// This abstract class hides the details about which kind of contact constraint
+// we're dealing with, while giving us enough to work with for deciding what's
+// on and off and generating impulses.
+//
+// There is always a scalar associated with the constraint for making
+// decisions. There may be a friction element associated with this contact.
+class MyFrictionElement;
+class MyContactElement {
+public:
+ enum ImpulseType {Compression,Expansion,Capture};
+
+ MyContactElement(Constraint uni, Real multSign, Real coefRest)
+ : m_uni(uni), m_multSign(multSign), m_coefRest(coefRest),
+ m_index(-1), m_friction(0),
+ m_velocityDependentCOR(NaN), m_restitutionDone(false)
+ { m_uni.setDisabledByDefault(true); }
+
+ virtual ~MyContactElement() {}
+
+ // (Re)initialize base & concrete class. If overridden, be sure to
+ // invoke base class first.
+ virtual void initialize() {
+ setRestitutionDone(false);
+ m_velocityDependentCOR = NaN;
+ m_Ic = m_Ie = m_I = 0;
+ }
+
+ // Provide a human-readable string identifying the type of contact
+ // constraint.
+ virtual String getContactType() const = 0;
+
+ // These must be constructed so that a negative value means the
+ // unilateral constraint condition is violated.
+ virtual Real getPerr(const State& state) const = 0;
+ virtual Real getVerr(const State& state) const = 0;
+ virtual Real getAerr(const State& state) const = 0;
+
+ // This returns a point in the ground frame at which you might want to
+ // say the constraint is "located", for purposes of display. This should
+ // return something useful even if the constraint is currently off.
+ virtual Vec3 whereToDisplay(const State& state) const = 0;
+
+ // This is used by some constraints to collect position information that
+ // may be used later to set instance variables when enabling the underlying
+ // Simbody constraint. All constraints zero impulses here.
+ virtual void initializeForImpact(const State& state, Real captureVelocity) {
+ if (-captureVelocity <= getVerr(state) && getVerr(state) < 0) {
+ m_velocityDependentCOR = 0;
+ SimTK_DEBUG3("CAPTURING %d because %g <= v=%g < 0\n",
+ m_index, -captureVelocity, getVerr(state));
+ } else {
+ m_velocityDependentCOR = m_coefRest;
+ }
+
+ setRestitutionDone(false);
+ m_Ic = m_Ie = m_I = 0; }
+
+ // Returns zero if the constraint is not currently enabled. Otherwise
+ // return the signed constraint force, with a negative value indicating
+ // that the unilateral force condition is violated.
+ Real getForce(const State& s) const {
+ if (isDisabled(s)) return 0;
+ const Vector mult = m_uni.getMultipliersAsVector(s);
+ assert(mult.size() == 1);
+ if (isNaN(mult[0]))
+ printf("*** getForce(): mult is NaN\n");
+ return m_multSign*mult[0];
+ }
+
+ bool isProximal(const State& state, Real posTol) const
+ { return !isDisabled(state) || getPerr(state) <= posTol; }
+ bool isCandidate(const State& state, Real posTol, Real velTol) const
+ { return isProximal(state, posTol) && getVerr(state) <= velTol; }
+
+
+ void enable(State& state) const {m_uni.enable(state);}
+ void disable(State& state) const {m_uni.disable(state);}
+ bool isDisabled(const State& state) const {return m_uni.isDisabled(state);}
+
+ void setMyDesiredDeltaV(const State& s,
+ Vector& desiredDeltaV) const
+ { Vector myDesiredDV(1); myDesiredDV[0] = m_multSign*getVerr(s);
+ m_uni.setMyPartInConstraintSpaceVector(s, myDesiredDV,
+ desiredDeltaV); }
+
+ void recordImpulse(ImpulseType type, const State& state,
+ const Vector& lambda) {
+ Vector myImpulse(1);
+ m_uni.getMyPartFromConstraintSpaceVector(state, lambda, myImpulse);
+ const Real I = myImpulse[0];
+ if (type==Compression) m_Ic = I;
+ else if (type==Expansion) m_Ie = I;
+ m_I += I;
+ }
+
+ // Impulse is accumulated internally.
+ Real getImpulse() const {return -m_multSign*m_I;}
+ Real getCompressionImpulse() const {return -m_multSign*m_Ic;}
+ Real getExpansionImpulse() const {return -m_multSign*m_Ie;}
+
+ Real getMyValueFromConstraintSpaceVector(const State& state,
+ const Vector& lambda) const
+ { Vector myValue(1);
+ m_uni.getMyPartFromConstraintSpaceVector(state, lambda, myValue);
+ return -m_multSign*myValue[0]; }
+
+ void setMyExpansionImpulse(const State& state,
+ Real coefRest,
+ Vector& lambda) const
+ { const Real I = coefRest * m_Ic;
+ Vector myImp(1); myImp[0] = I;
+ m_uni.setMyPartInConstraintSpaceVector(state, myImp, lambda); }
+
+
+ Real getMaxCoefRest() const {return m_coefRest;}
+ Real getEffectiveCoefRest() const {return m_velocityDependentCOR;}
+ void setRestitutionDone(bool isDone) {m_restitutionDone=isDone;}
+ bool isRestitutionDone() const {return m_restitutionDone;}
+
+ // Record position within the set of unilateral contact constraints.
+ void setContactIndex(int index) {m_index=index;}
+ int getContactIndex() const {return m_index;}
+ // If there is a friction element for which this is the master contact,
+ // record it here.
+ void setFrictionElement(MyFrictionElement& friction)
+ { m_friction = &friction; }
+ // Return true if there is a friction element associated with this contact
+ // element.
+ bool hasFrictionElement() const {return m_friction != 0;}
+ // Get the associated friction element.
+ const MyFrictionElement& getFrictionElement() const
+ { assert(hasFrictionElement()); return *m_friction; }
+ MyFrictionElement& updFrictionElement() const
+ { assert(hasFrictionElement()); return *m_friction; }
+
+protected:
+ Constraint m_uni;
+ const Real m_multSign; // 1 or -1
+ const Real m_coefRest;
+
+ int m_index; // contact index in unilateral constraint set
+ MyFrictionElement* m_friction; // if any (just a reference, not owned)
+
+ // Runtime -- initialized at start of impact handler.
+ Real m_velocityDependentCOR; // Calculated at start of impact
+ bool m_restitutionDone;
+ Real m_Ic, m_Ie, m_I; // impulses
+};
+
+
+
+//==============================================================================
+// MY FRICTION ELEMENT
+//==============================================================================
+// A Coulomb friction element consists of both a sliding force and a stiction
+// constraint, at most one of which is active. There is a boolean state variable
+// associated with each element that says whether it is in sliding or stiction,
+// and that state can only be changed during event handling.
+//
+// Generated forces depend on a scalar normal force N that comes from a
+// separate "normal force master", which might be one of the following:
+// - a unilateral constraint
+// - a bilateral constraint
+// - a mobilizer
+// - a compliant force element
+// If the master is an inactive unilateral constraint, or if N=0, then no
+// friction forces are generated. In this example, we're only going to use
+// a unilateral contact constraint as the "normal force master".
+//
+// For all but the compliant normal force master, the normal force N is
+// acceleration-dependent and thus may be coupled to the force produced by a
+// sliding friction element. This may require iteration to ensure consistency
+// between the sliding friction force and its master contact's normal force.
+//
+// A Coulomb friction element depends on a scalar slip speed defined by the
+// normal force master (this might be the magnitude of a generalized speed or
+// slip velocity vector). When the slip velocity goes to zero, the stiction
+// constraint is enabled if its constraint force magnitude can be kept to
+// mu_s*|N| or less. Otherwise, or if the slip velocity is nonzero, the sliding
+// force is enabled instead and generates a force of constant magnitude mu_d*|N|
+// that opposes the slip direction, or impending slip direction, as defined by
+// the master.
+//
+// There are two witness functions generated: (1) in slip mode, observes slip
+// velocity reversal and triggers stiction, and (2) in stiction mode, observes
+// stiction force increase past mu_s*|N| and triggers switch to sliding.
+class MyFrictionElement {
+public:
+ MyFrictionElement(Real mu_d, Real mu_s, Real mu_v)
+ : mu_d(mu_d), mu_s(mu_s), mu_v(mu_v), m_index(-1) {}
+
+ virtual ~MyFrictionElement() {}
+
+ // (Re)initialize base & concrete class. If overridden, be sure to
+ // invoke base class first.
+ virtual void initialize() {
+ }
+
+ Real getDynamicFrictionCoef() const {return mu_d;}
+ Real getStaticFrictionCoef() const {return mu_s;}
+ Real getViscousFrictionCoef() const {return mu_v;}
+
+ // Return true if the stiction constraint is enabled.
+ virtual bool isSticking(const State&) const = 0;
+
+ virtual void enableStiction(State&) const = 0;
+ virtual void disableStiction(State&) const = 0;
+
+ // When sticking, record -f/|f| as the previous slip direction, and
+ // max(N,0) as the previous normal force. Stiction
+ // must be currently active and constraint multipliers available.
+ virtual void recordImpendingSlipInfo(const State&) = 0;
+ // When sliding, record current slip velocity as the previous slip
+ // direction.
+ virtual void recordSlipDir(const State&) = 0;
+
+ // In an event handler or at initialization only, set the last recorded slip
+ // direction as the previous direction. This invalidates Velocity stage.
+ virtual void updatePreviousSlipDirFromRecorded(State& state) const = 0;
+
+ // This is the dot product of the current sliding velocity and the
+ // saved previous slip direction. This changes sign when a sliding friction
+ // force of mu_d*|N| would cause a reversal, meaning a switch to stiction is
+ // in order. State must be realized to Velocity stage.
+ virtual Real calcSlipSpeedWitness(const State&) const = 0;
+
+ // When in stiction, this calculates mu_s*|N| - |f|, which is negative if
+ // the stiction force exceeds its limit. (Not suitable for impacts where
+ // the dynamic coefficient should be used.) State must be realized to
+ // Acceleration stage.
+ virtual Real calcStictionForceWitness(const State&) const = 0;
+
+ // This is the magnitude of the current slip velocity. State must be
+ // realized to Velocity stage.
+ virtual Real getActualSlipSpeed(const State&) const = 0;
+
+ // This is the magnitude of the current friction force, whether sliding
+ // or sticking. State must be realized to Acceleration stage.
+ virtual Real getActualFrictionForce(const State&) const = 0;
+
+ // Return the scalar normal force N being generated by the contact master
+ // of this friction element. This may be negative if the master is a
+ // unilateral constraint whose "no-stick" condition is violated.
+ virtual Real getMasterNormalForce(const State&) const = 0;
+
+ // Return true if the normal force master *could* be involved in an
+ // impact event (because it is touching).
+ virtual bool isMasterProximal(const State&, Real posTol) const = 0;
+ // Return true if the normal force master *could* be involved in contact
+ // force generation (because it is touching and not separating).
+ virtual bool isMasterCandidate(const State&, Real posTol, Real velTol)
+ const = 0;
+ // Return true if the normal force master is currently generating a
+ // normal force (or impulse) so that this friction element might be
+ // generating a force also.
+ virtual bool isMasterActive(const State&) const = 0;
+
+
+ // This is used by some stiction constraints to collect position information
+ // that may be used later to set instance variables when enabling the
+ // underlying Simbody constraint. Recorded impulses should be zeroed.
+ virtual void initializeForStiction(const State& state) = 0;
+
+ // If this friction element's stiction constraint is enabled, set its
+ // constraint-space velocity entry(s) in desiredDeltaV to the current
+ // slip velocity (which might be a scalar or 2-vector).
+ virtual void setMyDesiredDeltaV(const State& s,
+ Vector& desiredDeltaV) const = 0;
+
+ // We just applied constraint-space impulse lambda to all active
+ // constraints. If this friction element's stiction constraint is enabled,
+ // save its part of the impulse internally for reporting.
+ virtual void recordImpulse(MyContactElement::ImpulseType type,
+ const State& state,
+ const Vector& lambda) = 0;
+
+ // Output the status, friction force, slip velocity, prev slip direction
+ // (scalar or vector) to the given ostream, indented as indicated and
+ // followed by a newline. May generate multiple lines.
+ virtual std::ostream& writeFrictionInfo(const State& state,
+ const String& indent,
+ std::ostream& o) const = 0;
+
+ // Optional: give some kind of visual representation for the friction force.
+ virtual void showFrictionForce(const State& state,
+ Array_<DecorativeGeometry>& geometry) const {}
+
+
+ void setFrictionIndex(int index) {m_index=index;}
+ int getFrictionIndex() const {return m_index;}
+
+private:
+ Real mu_d, mu_s, mu_v;
+ int m_index; // friction index within unilateral constraint set
+};
+
+
+
+//==============================================================================
+// MY UNILATERAL CONSTRAINT SET
+//==============================================================================
+
+// These are indices into the unilateral constraint set arrays.
+struct MyElementSubset {
+ void clear() {m_contact.clear();m_friction.clear();m_sliding.clear();}
+ Array_<int> m_contact;
+ Array_<int> m_friction; // friction elements that might stick
+ Array_<int> m_sliding; // friction elements that can only slide
+};
+
+class MyUnilateralConstraintSet {
+public:
+ // Capture velocity is used two ways: (1) if the normal approach velocity
+ // is smaller, the coefficient of restitution is set to zero for the
+ // upcoming impact, and (2) if a slip velocity is smaller than this the
+ // contact is a candidate for stiction.
+ MyUnilateralConstraintSet(const MultibodySystem& mbs, Real captureVelocity)
+ : m_mbs(mbs), m_captureVelocity(captureVelocity) {}
+
+ // This class takes over ownership of the heap-allocated contact element.
+ int addContactElement(MyContactElement* contact) {
+ const int index = (int)m_contact.size();
+ m_contact.push_back(contact);
+ contact->setContactIndex(index);
+ return index;
+ }
+ // This class takes over ownership of the heap-allocated friction element.
+ int addFrictionElement(MyFrictionElement* friction) {
+ const int index = (int)m_friction.size();
+ m_friction.push_back(friction);
+ friction->setFrictionIndex(index);
+ return index;
+ }
+
+ Real getCaptureVelocity() const {return m_captureVelocity;}
+ void setCaptureVelocity(Real v) {m_captureVelocity=v;}
+
+ int getNumContactElements() const {return (int)m_contact.size();}
+ int getNumFrictionElements() const {return (int)m_friction.size();}
+ const MyContactElement& getContactElement(int ix) const
+ { return *m_contact[ix]; }
+ const MyFrictionElement& getFrictionElement(int ix) const
+ { return *m_friction[ix]; }
+
+ // Allow writable access to elements from const set so we can record
+ // runtime results (e.g. impulses).
+ MyContactElement& updContactElement(int ix) const {return *m_contact[ix];}
+ MyFrictionElement& updFrictionElement(int ix) const {return *m_friction[ix];}
+
+ // Initialize all runtime fields in the contact & friction elements.
+ void initialize()
+ {
+ for (unsigned i=0; i < m_contact.size(); ++i)
+ m_contact[i]->initialize();
+ for (unsigned i=0; i < m_friction.size(); ++i)
+ m_friction[i]->initialize();
+ }
+
+ // Return the contact and friction elements that might be involved in an
+ // impact occurring in this configuration. They are the contact elements
+ // for which perr <= posTol, and friction elements whose normal force
+ // masters can be involved in the impact and whose slip velocities are
+ // below tolerance. State must be realized through Velocity stage.
+ void findProximalElements(const State& s,
+ Real posTol,
+ Real velTol,
+ MyElementSubset& proximals) const
+ {
+ proximals.clear();
+ for (unsigned i=0; i < m_contact.size(); ++i)
+ if (m_contact[i]->isProximal(s,posTol))
+ proximals.m_contact.push_back(i);
+ for (unsigned i=0; i < m_friction.size(); ++i) {
+ MyFrictionElement& fric = updFrictionElement(i);
+ if (!fric.isMasterProximal(s,posTol))
+ continue;
+ if (fric.isSticking(s) || fric.getActualSlipSpeed(s) <= velTol)
+ proximals.m_friction.push_back(i);
+ else proximals.m_sliding.push_back(i);
+ }
+ }
+
+ // Return the contact and friction elements that might be involved in
+ // generating contact forces at the current state. Candidate contact
+ // elements are those that are (a) already enabled, or (b) for which
+ // perr <= posTol and verr <= velTol. Candidate friction elements are those
+ // whose normal force master is unconditional or a candidate and (a) which
+ // are already sticking, or (b) for which vslip <= velTol, or (c) for which
+ // vslip opposes the previous slip direction, meaning it has reversed and
+ // must have passed through zero during the last step. These are the elements
+ // that can be activated without making any changes to the configuration or
+ // velocity state variables, except slightly for constraint projection.
+ //
+ // We also record the friction elements that, if their masters are active,
+ // can only slide because they have a significant slip velocity. State must
+ // be realized through Velocity stage.
+ void findCandidateElements(const State& s,
+ Real posTol,
+ Real velTol,
+ MyElementSubset& candidates) const
+ {
+ candidates.clear();
+ for (unsigned i=0; i < m_contact.size(); ++i)
+ if (m_contact[i]->isCandidate(s,posTol,velTol))
+ candidates.m_contact.push_back(i);
+ for (unsigned i=0; i < m_friction.size(); ++i) {
+ MyFrictionElement& fric = updFrictionElement(i);
+ if (!fric.isMasterCandidate(s,posTol,velTol))
+ continue;
+ if (fric.isSticking(s)
+ || fric.getActualSlipSpeed(s) <= velTol
+ || fric.calcSlipSpeedWitness(s) <= 0)
+ {
+ fric.initializeForStiction(s);
+ candidates.m_friction.push_back(i); // could stick or slide
+ } else {
+ fric.recordSlipDir(s);
+ candidates.m_sliding.push_back(i); // could only slide
+ }
+ }
+ }
+
+ // Look through the given constraint subset and enable any constraints
+ // that are currently disabled. Returns true if any change was made.
+ // If includeStiction==false, we'll only enable contact constraints.
+ bool enableConstraintSubset(const MyElementSubset& subset,
+ bool includeStiction,
+ State& state) const
+ {
+ bool changedSomething = false;
+
+ // Enable contact constraints.
+ for (unsigned i=0; i < subset.m_contact.size(); ++i) {
+ const int which = subset.m_contact[i];
+ const MyContactElement& cont = getContactElement(which);
+ if (cont.isDisabled(state)) {
+ cont.enable(state);
+ changedSomething = true;
+ }
+ }
+
+ if (includeStiction) {
+ // Enable all stiction constraints.
+ for (unsigned i=0; i < subset.m_friction.size(); ++i) {
+ const int which = subset.m_friction[i];
+ const MyFrictionElement& fric = getFrictionElement(which);
+ if (!fric.isSticking(state)) {
+ assert(fric.isMasterActive(state));
+ fric.enableStiction(state);
+ changedSomething = true;
+ }
+ }
+ }
+
+ m_mbs.realize(state, Stage::Instance);
+ return changedSomething;
+ }
+
+ // All event handlers call this method before returning. Given a state for
+ // which no (further) impulse is required, here we decide which contact and
+ // stiction constraints are active, and ensure that they satisfy the
+ // required constraint tolerances to the given accuracy. For sliding
+ // contacts, we will have recorded the slip or impending slip direction and
+ // converged the normal forces.
+ // TODO: in future this may return indicating that an impulse is required
+ // after all, as in Painleve's paradox.
+ void selectActiveConstraints(State& state, Real accuracy) const;
+
+ // This is the inner loop of selectActiveConstraints(). Given a set of
+ // candidates to consider, it finds an active subset and enables those
+ // constraints.
+ void findActiveCandidates(State& state,
+ const MyElementSubset& candidates) const;
+
+ // In Debug mode, produce a useful summary of the current state of the
+ // contact and friction elements.
+ void showConstraintStatus(const State& state, const String& place) const;
+
+ ~MyUnilateralConstraintSet() {
+ for (unsigned i=0; i < m_contact.size(); ++i)
+ delete m_contact[i];
+ for (unsigned i=0; i < m_friction.size(); ++i)
+ delete m_friction[i];
+ }
+
+ const MultibodySystem& getMultibodySystem() const {return m_mbs;}
+private:
+ const MultibodySystem& m_mbs;
+ Real m_captureVelocity;
+ Array_<MyContactElement*> m_contact;
+ Array_<MyFrictionElement*> m_friction;
+};
+
+
+
+//==============================================================================
+// STATE SAVER
+//==============================================================================
+// This reporter is called now and again to save the current state so we can
+// play back a movie at the end.
+class StateSaver : public PeriodicEventReporter {
+public:
+ StateSaver(const MultibodySystem& mbs,
+ const MyUnilateralConstraintSet& unis,
+ const Integrator& integ,
+ Real reportInterval)
+ : PeriodicEventReporter(reportInterval),
+ m_mbs(mbs), m_unis(unis), m_integ(integ)
+ { m_states.reserve(2000); }
+
+ ~StateSaver() {}
+
+ void clear() {m_states.clear();}
+ int getNumSavedStates() const {return (int)m_states.size();}
+ const State& getState(int n) const {return m_states[n];}
+
+ void handleEvent(const State& s) const {
+ const SimbodyMatterSubsystem& matter=m_mbs.getMatterSubsystem();
+ const SpatialVec PG = matter.calcSystemMomentumAboutGroundOrigin(s);
+ m_mbs.realize(s, Stage::Acceleration);
+
+#ifndef NDEBUG
+ printf("%3d: %5g mom=%g,%g E=%g", m_integ.getNumStepsTaken(),
+ s.getTime(),
+ PG[0].norm(), PG[1].norm(), m_mbs.calcEnergy(s));
+ cout << " Triggers=" << s.getEventTriggers() << endl;
+ m_unis.showConstraintStatus(s, "STATE SAVER");
+#endif
+
+ m_states.push_back(s);
+ }
+private:
+ const MultibodySystem& m_mbs;
+ const MyUnilateralConstraintSet& m_unis;
+ const Integrator& m_integ;
+ mutable Array_<State> m_states;
+};
+
+// A periodic event reporter that does nothing; useful for exploring the
+// effects of interrupting the simulation.
+class Nada : public PeriodicEventReporter {
+public:
+ explicit Nada(Real reportInterval)
+ : PeriodicEventReporter(reportInterval) {}
+
+ void handleEvent(const State& s) const {
+#ifndef NDEBUG
+ printf("%7g NADA\n", s.getTime());
+#endif
+ }
+};
+
+
+//==============================================================================
+// CONTACT ON HANDLER
+//==============================================================================
+// Allocate three of these for each unilateral contact constraint, using
+// a position, velocity, or acceleration witness function. When the associated
+// contact constraint is inactive, the event triggers are:
+// 1. separation distance goes from positive to negative
+// 2. separation rate goes from positive to negative while distance is zero
+// 3. separation acceleration goes from positive to negative while both
+// distance and rate are zero
+// The first two cases may require an impulse, since the velocities may have to
+// change discontinuously to satisfy the constraints. Case 3 requires only
+// recalculation of the active contacts. In any case the particular contact
+// element that triggered the handler is irrelevant; all "proximal" contacts
+// are solved simultaneously.
+class ContactOn: public TriggeredEventHandler {
+public:
+ ContactOn(const MultibodySystem& system,
+ const MyUnilateralConstraintSet& unis,
+ unsigned which,
+ Stage stage)
+ : TriggeredEventHandler(stage),
+ m_mbs(system), m_unis(unis), m_which(which),
+ m_stage(stage)
+ {
+ // Trigger only as height goes from positive to negative.
+ getTriggerInfo().setTriggerOnRisingSignTransition(false);
+ }
+
+ // This is the witness function.
+ Real getValue(const State& state) const {
+ const SimbodyMatterSubsystem& matter = m_mbs.getMatterSubsystem();
+ const MyContactElement& uni = m_unis.getContactElement(m_which);
+ if (!uni.isDisabled(state))
+ return 0; // already locked
+
+ const Real height = uni.getPerr(state);
+ //printf("getValue %d(%.17g) perr=%g\n", m_which, state.getTime(), height);
+
+ if (m_stage == Stage::Position)
+ return height;
+
+ // Velocity and acceleration triggers are not needed if we're
+ // above ground.
+ if (height > 0) return 0;
+
+ const Real dheight = uni.getVerr(state);
+ //printf("... verr=%g\n", dheight);
+
+ if (m_stage == Stage::Velocity)
+ return dheight;
+
+ // Acceleration trigger is not needed if velocity is positive.
+ if (dheight > 0) return 0;
+
+ const Real ddheight = uni.getAerr(state);
+ //printf("... aerr=%g\n", ddheight);
+
+ return ddheight;
+ }
+
+ // We're using Poisson's definition of the coefficient of
+ // restitution, relating impulses, rather than Newton's,
+ // relating velocities, since Newton's can produce non-physical
+ // results for a multibody system. For Poisson, calculate the impulse
+ // that would bring the velocity to zero, multiply by the coefficient
+ // of restitution to calculate the rest of the impulse, then apply
+ // both impulses to produce changes in velocity. In most cases this
+ // will produce the same rebound velocity as Newton, but not always.
+ void handleEvent(State& s, Real accuracy, bool& shouldTerminate) const;
+
+ // Given the set of proximal constraints, prevent penetration by applying
+ // a nonnegative least squares impulse generating a step change in
+ // velocity. On return, the applied impulse and new velocities are recorded
+ // in the proximal elements, and state is updated to the new velocities and
+ // realized through Velocity stage. Constraints that ended up in contact
+ // are enabled, those that rebounded are disabled.
+ void processCompressionPhase(MyElementSubset& proximal,
+ State& state) const;
+
+ // Given a solution to the compression phase, including the compression
+ // impulse, the set of impacters (enabled) and rebounders (disabled and
+ // with positive rebound velocity), apply an expansion impulse based on
+ // the effective coefficients of restitution of the impacters. Wherever
+ // restitution is applied, the effective coefficient is reset to zero so
+ // that further restitution will not be done for that contact. Returns
+ // true if any expansion was done; otherwise nothing has changed.
+ // Expansion may result in some negative velocities, in which case it has
+ // induced further compression so another compression phase is required.
+ bool processExpansionPhase(MyElementSubset& proximal,
+ State& state) const;
+
+ // Given only the subset of proximal constraints that are active, calculate
+ // the impulse that would eliminate all their velocity errors. No change is
+ // made to the set of active constraints. Some of the resulting impulses
+ // may be negative.
+ void calcStoppingImpulse(const MyElementSubset& proximal,
+ const State& state,
+ Vector& lambda0) const;
+
+ // Given the initial generalized speeds u0, and a constraint-space impulse
+ // lambda, calculate the resulting step velocity change du, modify the
+ // generalized speeds in state to u0+du, and realize Velocity stage.
+ void updateVelocities(const Vector& u0,
+ const Vector& lambda,
+ State& state) const;
+
+
+private:
+ const MultibodySystem& m_mbs;
+ const MyUnilateralConstraintSet& m_unis;
+ const unsigned m_which;
+ const Stage m_stage;
+};
+
+
+
+//==============================================================================
+// CONTACT OFF HANDLER
+//==============================================================================
+// Allocate one of these for each unilateral contact constraint. This handler
+// is invoked when an active contact constraint's contact force crosses zero
+// from positive to negative, meaning it has gone from pushing to sticking.
+// This simply invokes recalculation of the active contacts; the particular
+// source of the event trigger doesn't matter.
+class ContactOff: public TriggeredEventHandler {
+public:
+ ContactOff(const MultibodySystem& system,
+ const MyUnilateralConstraintSet& unis,
+ unsigned which)
+ : TriggeredEventHandler(Stage::Acceleration),
+ m_mbs(system), m_unis(unis), m_which(which)
+ {
+ getTriggerInfo().setTriggerOnRisingSignTransition(false);
+ }
+
+ // This is the witness function.
+ Real getValue(const State& state) const {
+ const MyContactElement& uni = m_unis.getContactElement(m_which);
+ if (uni.isDisabled(state)) return 0;
+ const Real f = uni.getForce(state);
+ return f;
+ }
+
+ void handleEvent
+ (State& s, Real accuracy, bool& shouldTerminate) const
+ {
+ SimTK_DEBUG2("\nhandle %d liftoff@%.17g\n", m_which, s.getTime());
+ SimTK_DEBUG("\n----------------------------------------------------\n");
+ SimTK_DEBUG2("LIFTOFF triggered by constraint %d @t=%.15g\n",
+ m_which, s.getTime());
+ m_mbs.realize(s, Stage::Acceleration);
+
+ #ifndef NDEBUG
+ cout << " triggers=" << s.getEventTriggers() << "\n";
+ #endif
+
+ m_unis.selectActiveConstraints(s, accuracy);
+
+ SimTK_DEBUG("LIFTOFF DONE.\n");
+ SimTK_DEBUG("----------------------------------------------------\n");
+ }
+
+private:
+ const MultibodySystem& m_mbs;
+ const MyUnilateralConstraintSet& m_unis;
+ const unsigned m_which; // one of the contact elements
+};
+
+
+
+//==============================================================================
+// MY POINT CONTACT
+//==============================================================================
+// Define a unilateral constraint to represent contact of a point on a moving
+// body with the ground plane. The ground normal is assumed to be +y.
+class MyPointContact : public MyContactElement {
+ typedef MyContactElement Super;
+public:
+ MyPointContact(MobilizedBody& body, const Vec3& point,
+ Real coefRest)
+ : MyContactElement(
+ Constraint::PointInPlane(updGround(body), UnitVec3(YAxis), Zero,
+ body, point),
+ Real(-1), // multiplier sign
+ coefRest),
+ m_body(body), m_point(point)
+ {
+ }
+
+ Real getPerr(const State& s) const OVERRIDE_11 {
+ const Vec3 p = m_body.findStationLocationInGround(s, m_point);
+ return p[YAxis];
+ }
+ Real getVerr(const State& s) const OVERRIDE_11 {
+ const Vec3 v = m_body.findStationVelocityInGround(s, m_point);
+ return v[YAxis];
+ }
+ Real getAerr(const State& s) const OVERRIDE_11 {
+ const Vec3 a = m_body.findStationAccelerationInGround(s, m_point);
+ return a[YAxis];
+ }
+
+ String getContactType() const OVERRIDE_11 {return "Point";}
+ Vec3 whereToDisplay(const State& state) const OVERRIDE_11 {
+ return m_body.findStationLocationInGround(state,m_point);
+ }
+
+ // Will be zero if the stiction constraints are on.
+ Vec2 getSlipVelocity(const State& s) const {
+ const Vec3 v = m_body.findStationVelocityInGround(s, m_point);
+ return Vec2(v[XAxis],v[ZAxis]);
+ }
+ // Will be zero if the stiction constraints are on.
+ Vec2 getSlipAcceleration(const State& s) const {
+ const Vec3 a = m_body.findStationAccelerationInGround(s, m_point);
+ return Vec2(a[XAxis],a[ZAxis]);
+ }
+
+ Vec3 getContactPointInPlaneBody(const State& s) const
+ { return m_body.findStationLocationInGround(s, m_point); }
+
+ const MobilizedBody& getBody() const {return m_body;}
+ MobilizedBody& updBody() {return m_body;}
+ const Vec3& getBodyStation() const {return m_point;}
+
+ const MobilizedBody& getPlaneBody() const {
+ const SimbodyMatterSubsystem& matter = m_body.getMatterSubsystem();
+ return matter.getGround();
+ }
+ MobilizedBody& updPlaneBody() const {return updGround(m_body);}
+
+private:
+ // For use during construction before m_body is set.
+ MobilizedBody& updGround(MobilizedBody& body) const {
+ SimbodyMatterSubsystem& matter = body.updMatterSubsystem();
+ return matter.updGround();
+ }
+
+ MobilizedBody& m_body;
+ const Vec3 m_point;
+};
+
+
+
+
+//==============================================================================
+// MY SLIDING FRICTION FORCE -- Declaration
+//==============================================================================
+
+// A nice handle for the sliding friction force. The real code is in the Impl
+// class defined at the bottom of this file.
+class MySlidingFrictionForce : public Force::Custom {
+public:
+ // Add a sliding friction force element to the given force subsystem,
+ // and associate it with a particular contact point.
+ MySlidingFrictionForce(GeneralForceSubsystem& forces,
+ const class MyPointContactFriction& ptFriction,
+ Real vtol);
+
+ void setPrevN(State& state, Real N) const;
+ // This should be a unit vector.
+ void setPrevSlipDir(State& state, const Vec2& slipDir) const;
+
+ Real getPrevN(const State& state) const;
+ Vec2 getPrevSlipDir(const State& state) const;
+
+ bool hasPrevSlipDir(const State& state) const;
+
+ Real calcSlidingForceMagnitude(const State& state) const;
+ Vec2 calcSlidingForce(const State& state) const;
+
+private:
+ const class MySlidingFrictionForceImpl& getImpl() const;
+};
+
+
+//==============================================================================
+// MY POINT CONTACT FRICTION
+//==============================================================================
+// This friction element expects its master to be a unilateral point contact
+// constraint. It provides slipping forces or stiction constraint forces acting
+// in the plane, based on the normal force being applied by the point contact
+// constraint.
+class MyPointContactFriction : public MyFrictionElement {
+ typedef MyFrictionElement Super;
+public:
+ // The constructor allocates two NoSlip1D constraints and a sliding
+ // friction force element.
+ MyPointContactFriction(MyPointContact& contact,
+ Real mu_d, Real mu_s, Real mu_v, Real vtol, //TODO: shouldn't go here
+ GeneralForceSubsystem& forces)
+ : MyFrictionElement(mu_d,mu_s,mu_v), m_contact(contact),
+ m_noslipX(contact.updPlaneBody(), Vec3(0), UnitVec3(XAxis),
+ contact.updPlaneBody(), contact.updBody()),
+ m_noslipZ(contact.updPlaneBody(), Vec3(0), UnitVec3(ZAxis),
+ contact.updPlaneBody(), contact.updBody())
+ {
+ assert((0 <= mu_d && mu_d <= mu_s) && (0 <= mu_v));
+ contact.setFrictionElement(*this);
+ m_noslipX.setDisabledByDefault(true);
+ m_noslipZ.setDisabledByDefault(true);
+ m_sliding = new MySlidingFrictionForce(forces, *this, vtol);
+ initializeRuntimeFields();
+ }
+
+ ~MyPointContactFriction() {delete m_sliding;}
+
+ void initialize() OVERRIDE_11 {
+ Super::initialize();
+ initializeRuntimeFields();
+ }
+
+ // The way we constructed the NoSlip1D constraints makes the multipliers be
+ // the force on Ground; we negate here so we'll get the force on the sliding
+ // body instead.
+ Vec2 getStictionForce(const State& s) const {
+ assert(isSticking(s));
+ return Vec2(-m_noslipX.getMultiplier(s), -m_noslipZ.getMultiplier(s));
+ }
+
+ // Implement pure virtuals from MyFrictionElement base class.
+
+ bool isSticking(const State& s) const OVERRIDE_11
+ { return !m_noslipX.isDisabled(s); } // X,Z always on or off together
+
+ // Note that initializeForStiction() must have been called first.
+ void enableStiction(State& s) const OVERRIDE_11
+ { m_noslipX.setContactPoint(s, m_contactPointInPlane);
+ m_noslipZ.setContactPoint(s, m_contactPointInPlane);
+ m_noslipX.enable(s); m_noslipZ.enable(s); }
+
+ void disableStiction(State& s) const OVERRIDE_11
+ { m_sliding->setPrevN(s, std::max(m_prevN, Real(0)));
+ m_sliding->setPrevSlipDir(s, m_prevSlipDir);
+ m_noslipX.disable(s); m_noslipZ.disable(s); }
+
+ // When sticking with stiction force f, record -f/|f| as the previous slip
+ // direction. If the force is zero we'll leave the direction unchanged.
+ // Also record the master's normal force as the previous normal force
+ // unless it is negative; in that case record zero.
+ // State must be realized through Acceleration stage.
+ void recordImpendingSlipInfo(const State& s) OVERRIDE_11 {
+ const Vec2 f = getStictionForce(s);
+ SimTK_DEBUG3("%d: RECORD IMPENDING, f=%g %g\n",
+ getFrictionIndex(), f[0], f[1]);
+ const Real fmag = f.norm();
+ if (fmag > 0) // TODO: could this ever be zero?
+ m_prevSlipDir = -f/fmag;
+ const Real N = getMasterNormalForce(s); // might be negative
+ m_prevN = N;
+ }
+ // When sliding, record current slip velocity (normalized) as the previous
+ // slip direction. If there is no slip velocity we leave the slip direction
+ // unchanged. State must be realized through Velocity stage.
+ void recordSlipDir(const State& s) OVERRIDE_11 {
+ assert(!isSticking(s));
+ Vec2 v = m_contact.getSlipVelocity(s);
+ Real vmag = v.norm();
+ if (vmag > 0)
+ m_prevSlipDir = v/vmag;
+ }
+
+ // Transfer the locally-stored previous slip direction to the state variable.
+ void updatePreviousSlipDirFromRecorded(State& s) const OVERRIDE_11 {
+ m_sliding->setPrevSlipDir(s, m_prevSlipDir);
+ }
+
+ Real calcSlipSpeedWitness(const State& s) const OVERRIDE_11 {
+ if (isSticking(s)) return 0;
+ const Vec2 vNow = m_contact.getSlipVelocity(s);
+ if (!m_sliding->hasPrevSlipDir(s)) return vNow.norm();
+ const Vec2 vPrev = m_sliding->getPrevSlipDir(s);
+ return dot(vNow, vPrev);
+ }
+
+ Real calcStictionForceWitness(const State& s) const OVERRIDE_11 {
+ if (!isSticking(s)) return 0;
+ const Real mu_s = getStaticFrictionCoef();
+ const Real N = getMasterNormalForce(s); // might be negative
+ const Vec2 f = getStictionForce(s);
+ const Real fmag = f.norm();
+ return mu_s*N - fmag;
+ }
+
+ Real getActualSlipSpeed(const State& s) const OVERRIDE_11 {
+ const Vec2 vNow = m_contact.getSlipVelocity(s);
+ return vNow.norm();
+ }
+
+ Real getActualFrictionForce(const State& s) const OVERRIDE_11 {
+ const Real f = isSticking(s) ? getStictionForce(s).norm()
+ : m_sliding->calcSlidingForceMagnitude(s);
+ return f;
+ }
+
+ Real getMasterNormalForce(const State& s) const OVERRIDE_11 {
+ const Real N = m_contact.getForce(s); // might be negative
+ return N;
+ }
+
+
+ bool isMasterProximal(const State& s, Real posTol) const OVERRIDE_11
+ { return m_contact.isProximal(s, posTol); }
+ bool isMasterCandidate(const State& s, Real posTol, Real velTol) const
+ OVERRIDE_11
+ { return m_contact.isCandidate(s, posTol, velTol); }
+ bool isMasterActive(const State& s) const OVERRIDE_11
+ { return !m_contact.isDisabled(s); }
+
+
+ // Set the friction application point to be the projection of the contact
+ // point onto the contact plane. This will be used the next time stiction
+ // is enabled. Requires state realized to Position stage.
+ void initializeForStiction(const State& s) OVERRIDE_11 {
+ const Vec3 p = m_contact.getContactPointInPlaneBody(s);
+ m_contactPointInPlane = p;
+ m_tIc = m_tIe = m_tI = Vec2(0);
+ }
+
+ void recordImpulse(MyContactElement::ImpulseType type, const State& state,
+ const Vector& lambda) OVERRIDE_11
+ {
+ if (!isSticking(state)) return;
+
+ // Record translational impulse.
+ Vector myImpulseX(1), myImpulseZ(1);
+ m_noslipX.getMyPartFromConstraintSpaceVector(state, lambda, myImpulseX);
+ m_noslipZ.getMyPartFromConstraintSpaceVector(state, lambda, myImpulseZ);
+ const Vec2 tI(myImpulseX[0], myImpulseZ[0]);
+ if (type==MyContactElement::Compression) m_tIc = tI;
+ else if (type==MyContactElement::Expansion) m_tIe = tI;
+ m_tI += tI;
+ }
+
+ // Fill in deltaV to eliminate slip velocity using the stiction
+ // constraints.
+ void setMyDesiredDeltaV(const State& s,
+ Vector& desiredDeltaV) const OVERRIDE_11
+ {
+ if (!isSticking(s)) return;
+
+ const Vec2 dv = -m_contact.getSlipVelocity(s); // X,Z
+ Vector myDesiredDV(1); // Nuke translational velocity also.
+ myDesiredDV[0] = dv[0];
+ m_noslipX.setMyPartInConstraintSpaceVector(s, myDesiredDV, desiredDeltaV);
+ myDesiredDV[0] = dv[1];
+ m_noslipZ.setMyPartInConstraintSpaceVector(s, myDesiredDV, desiredDeltaV);
+ }
+
+ Real getMyImpulseMagnitudeFromConstraintSpaceVector(const State& state,
+ const Vector& lambda) const
+ { Vector myImpulseX(1), myImpulseZ(1);
+ m_noslipX.getMyPartFromConstraintSpaceVector(state, lambda, myImpulseX);
+ m_noslipZ.getMyPartFromConstraintSpaceVector(state, lambda, myImpulseZ);
+ const Vec2 tI(myImpulseX[0], myImpulseZ[0]);
+ return tI.norm();
+ }
+
+
+ std::ostream& writeFrictionInfo(const State& s, const String& indent,
+ std::ostream& o) const OVERRIDE_11
+ {
+ o << indent;
+ if (!isMasterActive(s)) o << "OFF";
+ else if (isSticking(s)) o << "STICK";
+ else o << "SLIP";
+
+ const Vec2 v = m_contact.getSlipVelocity(s);
+ const Vec2 pd = m_sliding->getPrevSlipDir(s);
+ const Vec2 f = isSticking(s) ? getStictionForce(s)
+ : m_sliding->calcSlidingForce(s);
+ o << " prevDir=" << ~pd << " V=" << ~v << " Vdot=" << ~v*pd
+ << " F=" << ~f << endl;
+ return o;
+ }
+
+
+ void showFrictionForce(const State& s, Array_<DecorativeGeometry>& geometry)
+ const OVERRIDE_11
+ {
+ const Real Scale = 10;
+ const Vec2 f = isSticking(s) ? getStictionForce(s)
+ : m_sliding->calcSlidingForce(s);
+ if (f.normSqr() < square(SignificantReal))
+ return;
+ const MobilizedBody& bodyB = m_contact.getBody();
+ const Vec3& stationB = m_contact.getBodyStation();
+ const Vec3 stationG = bodyB.getBodyTransform(s)*stationB;
+ const Vec3 endG = stationG - Scale*Vec3(f[0], 0, f[1]);
+ geometry.push_back(DecorativeLine(endG + Vec3(0,.05,0),
+ stationG + Vec3(0,.05,0))
+ .setColor(isSticking(s)?Green:Orange));
+ }
+
+ const MyPointContact& getMyPointContact() const {return m_contact;}
+ const MySlidingFrictionForce& getMySlidingFrictionForce() const
+ { return *m_sliding; }
+private:
+ void initializeRuntimeFields() {
+ m_contactPointInPlane = Vec3(0);
+ m_tIc = m_tIe = m_tI = Vec2(NaN);
+ m_prevN = 0;
+ m_prevSlipDir = Vec2(NaN);
+ }
+ const MyPointContact& m_contact;
+
+ Constraint::NoSlip1D m_noslipX, m_noslipZ; // stiction
+ MySlidingFrictionForce* m_sliding; // sliding friction force element
+
+ // Runtime
+ Vec3 m_contactPointInPlane; // point on plane body where friction will act
+ Vec2 m_tIc, m_tIe, m_tI; // impulses
+
+ Real m_prevN; // master's recorded normal force (could be negative)
+ Vec2 m_prevSlipDir; // master's last recording slip or impending direction
+};
+
+
+//==============================================================================
+// SHOW CONTACT
+//==============================================================================
+// For each visualization frame, generate ephemeral geometry to show just
+// during this frame, based on the current State.
+class ShowContact : public DecorationGenerator {
+public:
+ ShowContact(const MyUnilateralConstraintSet& unis)
+ : m_unis(unis) {}
+
+ void generateDecorations(const State& state,
+ Array_<DecorativeGeometry>& geometry) OVERRIDE_11
+ {
+ for (int i=0; i < m_unis.getNumContactElements(); ++i) {
+ const MyContactElement& contact = m_unis.getContactElement(i);
+ const Vec3 loc = contact.whereToDisplay(state);
+ if (!contact.isDisabled(state)) {
+ geometry.push_back(DecorativeSphere(.5)
+ .setTransform(loc)
+ .setColor(Red).setOpacity(.25));
+ String text = "LOCKED";
+ if (contact.hasFrictionElement()) {
+ const MyFrictionElement& friction = contact.getFrictionElement();
+ text = friction.isSticking(state) ? "STICKING"
+ : "CONTACT";
+ m_unis.getMultibodySystem().realize(state, Stage::Acceleration);
+ friction.showFrictionForce(state, geometry);
+ }
+ geometry.push_back(DecorativeText(String(i)+"-"+text)
+ .setColor(White).setScale(.5)
+ .setTransform(loc+Vec3(0,.2,0)));
+ } else {
+ geometry.push_back(DecorativeText(String(i))
+ .setColor(White).setScale(.5)
+ .setTransform(loc+Vec3(0,.1,0)));
+ }
+ }
+ }
+private:
+ const MyUnilateralConstraintSet& m_unis;
+};
+
+
+//==============================================================================
+// BODY WATCHER
+//==============================================================================
+// Prior to rendering each frame, point the camera at the given body's
+// origin.
+class BodyWatcher : public Visualizer::FrameController {
+public:
+ explicit BodyWatcher(const MobilizedBody& body) : m_body(body) {}
+
+ void generateControls(const Visualizer& viz,
+ const State& state,
+ Array_< DecorativeGeometry >& geometry) OVERRIDE_11
+ {
+ const Vec3 Bo = m_body.getBodyOriginLocation(state);
+ const Vec3 p_GC = Bo + Vec3(0, 1, 5); // above and back
+ const Rotation R_GC(UnitVec3(0,1,0), YAxis,
+ p_GC-Bo, ZAxis);
+ viz.setCameraTransform(Transform(R_GC,p_GC));
+ //viz.pointCameraAt(Bo, Vec3(0,1,0));
+ }
+private:
+ const MobilizedBody m_body;
+};
+
+//==============================================================================
+// STICTION ON HANDLER
+//==============================================================================
+// Allocate one of these for each contact constraint that has friction. This
+// handler takes care of turning on the stiction constraints when the sliding
+// velocity drops to zero.
+class StictionOn: public TriggeredEventHandler {
+public:
+ StictionOn(const MultibodySystem& system,
+ const MyUnilateralConstraintSet& unis,
+ unsigned which)
+ : TriggeredEventHandler(Stage::Velocity),
+ m_mbs(system), m_unis(unis), m_which(which)
+ {
+ getTriggerInfo().setTriggerOnRisingSignTransition(false);
+ }
+
+ // This is the witness function.
+ Real getValue(const State& state) const {
+ const MyFrictionElement& friction = m_unis.getFrictionElement(m_which);
+ if (!friction.isMasterActive(state)) return 0;
+ const Real signedSlipSpeed = friction.calcSlipSpeedWitness(state);
+ return signedSlipSpeed;
+ }
+
+ void handleEvent
+ (State& s, Real accuracy, bool& shouldTerminate) const
+ {
+ SimTK_DEBUG2("\nhandle %d slide->stick@%.17g\n", m_which, s.getTime());
+ SimTK_DEBUG("\n----------------------------------------------------\n");
+ SimTK_DEBUG2("STICTION ON triggered by friction element %d @t=%.15g\n",
+ m_which, s.getTime());
+ m_mbs.realize(s, Stage::Acceleration);
+
+ #ifndef NDEBUG
+ m_unis.showConstraintStatus(s, "ENTER STICTION ON");
+ cout << " triggers=" << s.getEventTriggers() << "\n";
+ #endif
+
+ m_unis.selectActiveConstraints(s, accuracy);
+
+ SimTK_DEBUG("STICTION ON done.\n");
+ SimTK_DEBUG("----------------------------------------------------\n");
+ }
+
+private:
+ const MultibodySystem& m_mbs;
+ const MyUnilateralConstraintSet& m_unis;
+ const int m_which; // one of the friction elements
+};
+
+
+
+//==============================================================================
+// STICTION OFF HANDLER
+//==============================================================================
+// Allocate one of these for each contact constraint. This handler takes
+// care of turning off stiction constraints when the stiction force magnitude
+// exceeds mu*N.
+class StictionOff: public TriggeredEventHandler {
+public:
+ StictionOff(const MultibodySystem& system,
+ const MyUnilateralConstraintSet& unis,
+ unsigned which)
+ : TriggeredEventHandler(Stage::Acceleration),
+ m_mbs(system), m_unis(unis), m_which(which)
+ {
+ getTriggerInfo().setTriggerOnRisingSignTransition(false);
+ }
+
+ // This is the witness function. It is positive as long as mu_s*N is greater
+ // than the friction force magnitude.
+ Real getValue(const State& state) const {
+ const MyFrictionElement& friction = m_unis.getFrictionElement(m_which);
+ if (!friction.isMasterActive(state)) return 0;
+ const Real forceMargin = friction.calcStictionForceWitness(state);
+ return forceMargin; // how much stiction capacity is left
+ }
+
+ void handleEvent
+ (State& s, Real accuracy, bool& shouldTerminate) const
+ {
+ SimTK_DEBUG2("\nhandle %d stick->slide@%.17g\n", m_which, s.getTime());
+ SimTK_DEBUG("\n----------------------------------------------------\n");
+ SimTK_DEBUG2("STICTION OFF triggered by friction element %d @t=%.15g\n",
+ m_which, s.getTime());
+ m_mbs.realize(s, Stage::Acceleration);
+
+ #ifndef NDEBUG
+ cout << " triggers=" << s.getEventTriggers() << "\n";
+ #endif
+
+ m_unis.selectActiveConstraints(s, accuracy);
+
+ SimTK_DEBUG("STICTION OFF done.\n");
+ SimTK_DEBUG("----------------------------------------------------\n");
+ }
+
+private:
+ const MultibodySystem& m_mbs;
+ const MyUnilateralConstraintSet& m_unis;
+ const int m_which; // one of the friction elements
+};
+
+//==============================================================================
+// MY STOP
+//==============================================================================
+// Define a unilateral constraint to represent a joint stop that limits
+// the allowable motion of a single generalized coordinate. You can specify
+// a coefficient of restitution and whether the given limit is the upper or
+// lower limit.
+class MyStop : public MyContactElement {
+public:
+ enum Side {Lower,Upper};
+ MyStop(Side side, MobilizedBody& body, int whichQ,
+ Real limit, Real coefRest)
+ : MyContactElement
+ (Constraint::ConstantSpeed(body, MobilizerUIndex(whichQ), Real(0)),
+ Real(side==Lower?-1:1), coefRest),
+ m_body(body), m_whichq(whichQ), m_whichu(whichQ),
+ m_sign(side==Lower?1.:-1.), m_limit(limit)
+ {}
+
+ String getContactType() const OVERRIDE_11 {return "Stop";}
+
+ Real getPerr(const State& state) const OVERRIDE_11 {
+ const Real q = m_body.getOneQ(state, m_whichq);
+ return m_sign*(q-m_limit);
+ }
+ Real getVerr(const State& state) const OVERRIDE_11 {
+ const Real u = m_body.getOneU(state, m_whichu);
+ return m_sign*u;
+ }
+ Real getAerr(const State& state) const OVERRIDE_11 {
+ const Real udot = m_body.getOneUDot(state, m_whichu);
+ return m_sign*udot;
+ }
+
+ Vec3 whereToDisplay(const State& state) const OVERRIDE_11 {
+ const Vec3& p_B = m_body.getOutboardFrame(state).p();
+ return m_body.findStationLocationInGround(state,p_B);
+ }
+
+private:
+ const MobilizedBody& m_body;
+ const MobilizerQIndex m_whichq;
+ const MobilizerUIndex m_whichu;
+ Real m_sign; // +1: lower, -1: upper
+ Real m_limit;
+};
+
+//==============================================================================
+// MY ROPE
+//==============================================================================
+// Define a unilateral constraint to represent a "rope" that keeps the
+// distance between two points at or smaller than some limit.
+class MyRope : public MyContactElement {
+public:
+ MyRope(MobilizedBody& body1, const Vec3& pt1,
+ MobilizedBody& body2, const Vec3& pt2, Real d,
+ Real coefRest)
+ : MyContactElement
+ (Constraint::Rod(body1, pt1, body2, pt2, d), Real(1), coefRest),
+ m_body1(body1), m_point1(pt1), m_body2(body2), m_point2(pt2), m_dist(d)
+ {}
+
+ String getContactType() const OVERRIDE_11 {return "Rope";}
+
+ Real getPerr(const State& s) const OVERRIDE_11 {
+ const Vec3 p1 = m_body1.findStationLocationInGround(s,m_point1);
+ const Vec3 p2 = m_body2.findStationLocationInGround(s,m_point2);
+ const Vec3 p = p2-p1;
+ return (square(m_dist) - dot(p,p))/2;
+ }
+ Real getVerr(const State& s) const OVERRIDE_11 {
+ Vec3 p1, v1, p2, v2;
+ m_body1.findStationLocationAndVelocityInGround(s,m_point1,p1,v1);
+ m_body2.findStationLocationAndVelocityInGround(s,m_point2,p2,v2);
+ const Vec3 p = p2 - p1, v = v2 - v1;
+ return -dot(v, p);
+ }
+ Real getAerr(const State& s) const OVERRIDE_11 {
+ Vec3 p1, v1, a1, p2, v2, a2;
+ m_body1.findStationLocationVelocityAndAccelerationInGround
+ (s,m_point1,p1,v1,a1);
+ m_body2.findStationLocationVelocityAndAccelerationInGround
+ (s,m_point2,p2,v2,a2);
+ const Vec3 p = p2 - p1, v = v2 - v1, a = a2 - a1;
+ return -(dot(a, p) + dot(v, v));
+ }
+
+ Vec3 whereToDisplay(const State& state) const OVERRIDE_11 {
+ return m_body2.findStationLocationInGround(state,m_point2);
+ }
+
+private:
+ const MobilizedBody& m_body1;
+ const Vec3 m_point1;
+ const MobilizedBody& m_body2;
+ const Vec3 m_point2;
+ const Real m_dist;
+};
+
+//==============================================================================
+// MY PUSH FORCE
+//==============================================================================
+// This is a force element that generates a constant force on a body for a
+// specified time interval. It is useful to perturb the system to force it
+// to transition from sticking to sliding, for example.
+class MyPushForceImpl : public Force::Custom::Implementation {
+public:
+ MyPushForceImpl(const MobilizedBody& bodyB,
+ const Vec3& stationB,
+ const Vec3& forceG, // force direction in Ground!
+ Real onTime,
+ Real offTime)
+ : m_bodyB(bodyB), m_stationB(stationB), m_forceG(forceG),
+ m_on(onTime), m_off(offTime)
+ { }
+
+ //--------------------------------------------------------------------------
+ // Custom force virtuals
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+ OVERRIDE_11
+ {
+ if (!(m_on <= state.getTime() && state.getTime() <= m_off))
+ return;
+
+ m_bodyB.applyForceToBodyPoint(state, m_stationB, m_forceG, bodyForces);
+
+ //SimTK_DEBUG4("PUSHING @t=%g (%g,%g,%g)\n", state.getTime(),
+ // m_forceG[0], m_forceG[1], m_forceG[2]);
+ }
+
+ // No potential energy.
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11 {return 0;}
+
+ void calcDecorativeGeometryAndAppend
+ (const State& state, Stage stage,
+ Array_<DecorativeGeometry>& geometry) const OVERRIDE_11
+ {
+ const Real ScaleFactor = 1.;
+ if (stage != Stage::Time) return;
+ if (!(m_on <= state.getTime() && state.getTime() <= m_off))
+ return;
+ const Vec3 stationG = m_bodyB.findStationLocationInGround(state, m_stationB);
+ geometry.push_back(DecorativeLine(stationG-ScaleFactor*m_forceG, stationG)
+ .setColor(Yellow)
+ .setLineThickness(3));
+ }
+private:
+ const MobilizedBody& m_bodyB;
+ const Vec3 m_stationB;
+ const Vec3 m_forceG;
+ Real m_on;
+ Real m_off;
+};
+
+
+//==============================================================================
+// MAIN
+//==============================================================================
+int main(int argc, char** argv) {
+ try { // If anything goes wrong, an exception will be thrown.
+
+ // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
+ MultibodySystem mbs;
+
+ SimbodyMatterSubsystem matter(mbs);
+ GeneralForceSubsystem forces(mbs);
+ Force::Gravity gravity(forces, matter, -YAxis, 9.81);
+
+ MobilizedBody& Ground = matter.updGround();
+
+ // Predefine some handy rotations.
+ const Rotation Z90(Pi/2, ZAxis); // rotate +90 deg about z
+
+ const Real RunTime=16;
+
+ // ADD MOBILIZED BODIES AND CONTACT CONSTRAINTS
+ const Real CoefRest = 0.4; // TODO: per-contact
+ const Real mu_d = .5;
+ const Real mu_s = .7;
+ const Real mu_v = 0.05;
+ const Real CaptureVelocity = 0.01;
+
+ MyUnilateralConstraintSet unis(mbs, CaptureVelocity);
+
+ const Vec3 CubeHalfDims(3,2,1);
+ const Real CubeMass = 1;
+ Body::Rigid cubeBody =
+ Body::Rigid(MassProperties(CubeMass, Vec3(0),
+ UnitInertia::brick(CubeHalfDims)));
+
+ // First body: cube
+ MobilizedBody::Cartesian loc(Ground, MassProperties(0,Vec3(0),Inertia(0)));
+ MobilizedBody::Ball cube(loc, Vec3(0),
+ cubeBody, Vec3(0));
+ cube.addBodyDecoration(Transform(), DecorativeBrick(CubeHalfDims)
+ .setColor(Red).setOpacity(.3));
+ Force::Custom(forces, new MyPushForceImpl(cube,Vec3(1,1,0),2*Vec3(0,0,10),
+ 4., 6.));
+ Force::Custom(forces, new MyPushForceImpl(cube,Vec3(1,1,0),Vec3(7,8,10),
+ 11., 13.));
+ for (int i=-1; i<=1; i+=2)
+ for (int j=-1; j<=1; j+=2)
+ for (int k=-1; k<=1; k+=2) {
+ const Vec3 pt = Vec3(i,j,k).elementwiseMultiply(CubeHalfDims);
+ MyPointContact* contact = new MyPointContact(cube, pt, CoefRest);
+ unis.addContactElement(contact);
+ unis.addFrictionElement(
+ new MyPointContactFriction(*contact, mu_d, mu_s, mu_v,
+ CaptureVelocity, // TODO: vtol?
+ forces));
+ }
+
+
+ const Vec3 WeightEdge(-CubeHalfDims[0],-CubeHalfDims[1],0);
+//#ifdef NOTDEF
+ // Second body: weight
+ const Vec3 ConnectEdge1(CubeHalfDims[0],0,CubeHalfDims[2]);
+ MobilizedBody::Pin weight(cube,
+ Transform(Rotation(Pi/2,XAxis), ConnectEdge1),
+ cubeBody, Vec3(WeightEdge));
+ weight.addBodyDecoration(Transform(), DecorativeBrick(CubeHalfDims)
+ .setColor(Gray).setOpacity(.6));
+ //Force::MobilityLinearSpring(forces, weight, 0, 100, -Pi/4);
+ for (int i=-1; i<=1; i+=2)
+ for (int j=-1; j<=1; j+=2)
+ for (int k=-1; k<=1; k+=2) {
+ if (i==-1 && j==-1) continue;
+ const Vec3 pt = Vec3(i,j,k).elementwiseMultiply(CubeHalfDims);
+ MyPointContact* contact = new MyPointContact(weight, pt, CoefRest);
+ unis.addContactElement(contact);
+ unis.addFrictionElement(
+ new MyPointContactFriction(*contact, mu_d, mu_s, mu_v,
+ CaptureVelocity, // TODO: vtol?
+ forces));
+ }
+
+//#endif
+//#ifdef NOTDEF
+ // Third body: weight2
+ const Vec3 ConnectEdge2(CubeHalfDims[0],CubeHalfDims[1],0);
+ MobilizedBody::Pin weight2(cube,
+ Transform(Rotation(), ConnectEdge2),
+ cubeBody, Vec3(WeightEdge));
+ weight2.addBodyDecoration(Transform(), DecorativeBrick(CubeHalfDims)
+ .setColor(Cyan).setOpacity(.6));
+ Force::MobilityLinearSpring(forces, weight2, 0, 500, Pi/4);
+ for (int i=-1; i<=1; i+=2)
+ for (int j=-1; j<=1; j+=2)
+ for (int k=-1; k<=1; k+=2) {
+ if (i==-1 && j==-1) continue;
+ const Vec3 pt = Vec3(i,j,k).elementwiseMultiply(CubeHalfDims);
+ MyPointContact* contact = new MyPointContact(weight2, pt, CoefRest);
+ unis.addContactElement(contact);
+ unis.addFrictionElement(
+ new MyPointContactFriction(*contact, mu_d, mu_s, mu_v,
+ CaptureVelocity, // TODO: vtol?
+ forces));
+ }
+//#endif
+
+ // Add joint stops.
+ const Real StopAngle = Pi/6;
+ unis.addContactElement(new MyStop(MyStop::Upper,weight,0, StopAngle,CoefRest/2));
+ unis.addContactElement(new MyStop(MyStop::Lower,weight,0, -StopAngle,CoefRest/2));
+
+ // Add a rope.
+ unis.addContactElement(new MyRope(Ground, Vec3(-5,10,0),
+ cube, Vec3(-CubeHalfDims[0],-CubeHalfDims[1],0),
+ 5., .5*CoefRest));
+ //unis.addContactElement(new MyStop(MyStop::Upper,loc,1, 2.5,CoefRest));
+
+ Visualizer viz(mbs);
+ viz.setShowSimTime(true);
+ viz.addDecorationGenerator(new ShowContact(unis));
+
+ #ifdef ANIMATE
+ mbs.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));
+ #else
+ // This does nothing but interrupt the simulation.
+ mbs.addEventReporter(new Nada(ReportInterval));
+ #endif
+
+ //ExplicitEulerIntegrator integ(mbs);
+ //CPodesIntegrator integ(mbs,CPodes::BDF,CPodes::Newton);
+ Real accuracy = 1e-2;
+ //RungeKuttaFeldbergIntegrator integ(mbs);
+ //RungeKuttaMersonIntegrator integ(mbs);
+ RungeKutta3Integrator integ(mbs);
+ //VerletIntegrator integ(mbs);
+ integ.setAccuracy(accuracy);
+ //integ.setConstraintTolerance(1.);
+ //integ.setAllowInterpolation(false);
+ integ.setMaximumStepSize(0.1);
+
+ StateSaver* stateSaver = new StateSaver(mbs,unis,integ,ReportInterval);
+ mbs.addEventReporter(stateSaver);
+
+ for (int i=0; i < unis.getNumContactElements(); ++i) {
+ mbs.addEventHandler(new ContactOn(mbs, unis,i, Stage::Position));
+ mbs.addEventHandler(new ContactOn(mbs, unis,i, Stage::Velocity));
+ mbs.addEventHandler(new ContactOn(mbs, unis,i, Stage::Acceleration));
+ mbs.addEventHandler(new ContactOff(mbs, unis,i));
+ }
+
+ for (int i=0; i < unis.getNumFrictionElements(); ++i) {
+ mbs.addEventHandler(new StictionOn(mbs, unis, i));
+ mbs.addEventHandler(new StictionOff(mbs, unis, i));
+ }
+
+ State s = mbs.realizeTopology(); // returns a reference to the the default state
+ mbs.realizeModel(s); // define appropriate states for this System
+ mbs.realize(s, Stage::Instance); // instantiate constraints if any
+
+
+ // Set initial conditions so the -,-,- vertex is in the -y direction.
+ const Rotation R_BC(UnitVec3(CubeHalfDims+1e-7*Vec3(1,0,0)), YAxis, Vec3(1,0,0),XAxis);
+ loc.setQToFitTranslation(s, Vec3(0,10,0));
+ cube.setQToFitTransform(s, Transform(~R_BC, Vec3(0)));
+ cube.setUToFitAngularVelocity(s, Vec3(0,1,0));
+ cube.setUToFitLinearVelocity(s, Vec3(1,0,0));
+
+ mbs.realize(s, Stage::Velocity);
+ viz.report(s);
+
+ Array_<int> enableTheseContacts;
+ for (int i=0; i < unis.getNumContactElements(); ++i) {
+ const Real perr = unis.getContactElement(i).getPerr(s);
+ printf("contact constraint %d has perr=%g%s\n", i, perr,
+ perr<=0?" (ENABLING CONTACT)":"");
+ if (perr <= 0)
+ enableTheseContacts.push_back(i);
+ }
+
+ cout << "Initial configuration shown. Next? ";
+ getchar();
+
+ for (unsigned i=0; i < enableTheseContacts.size(); ++i)
+ unis.getContactElement(enableTheseContacts[i]).enable(s);
+
+ Assembler(mbs).setErrorTolerance(1e-6).assemble(s);
+ viz.report(s);
+ cout << "Assembled configuration shown. Ready? ";
+ getchar();
+
+ // Now look for enabled contacts that aren't sliding; turn on stiction
+ // for those.
+ mbs.realize(s, Stage::Velocity);
+ Array_<int> enableTheseStictions;
+ for (int i=0; i < unis.getNumFrictionElements(); ++i) {
+ MyFrictionElement& fric = unis.updFrictionElement(i);
+ const Real vSlip = fric.getActualSlipSpeed(s);
+ fric.initializeForStiction(s); // just in case
+ printf("friction element %d has v_slip=%g%s\n", i, vSlip,
+ vSlip==0?" (ENABLING STICTION)":"");
+ if (vSlip == 0)
+ enableTheseStictions.push_back(i);
+ }
+ for (unsigned i=0; i < enableTheseStictions.size(); ++i)
+ unis.getFrictionElement(enableTheseStictions[i]).enableStiction(s);
+
+ //State copyS = s;
+ //unis.getContactElement(0).enable(copyS);
+ //mbs.realize(copyS, Stage::Instance);
+ //mbs.realize(copyS, Stage::Dynamics);
+ //mbs.realize(copyS, Stage::Acceleration);
+
+ Matrix M(1,1); M(0,0)=1.23;
+ FactorLU Mlu(M);
+ //FactorQTZ Mqtz(M);
+
+
+ // Simulate it.
+
+ integ.setReturnEveryInternalStep(true);
+ //integ.setAllowInterpolation(false);
+ TimeStepper ts(mbs, integ);
+ ts.setReportAllSignificantStates(true);
+
+ #ifdef TEST_REPEATABILITY
+ const int tries = NTries;
+ #else
+ const int tries = 1;
+ #endif
+
+ Array_< Array_<State> > states(NTries);
+ Array_< Array_<Real> > timeDiff(NTries-1);
+ Array_< Array_<Vector> > yDiff(NTries-1);
+ cout.precision(18);
+ for (int ntry=0; ntry < tries; ++ntry) {
+ mbs.resetAllCountersToZero();
+ unis.initialize(); // reinitialize
+ ts.updIntegrator().resetAllStatistics();
+ ts.initialize(s);
+ int nStepsWithEvent = 0;
+
+ const double startReal = realTime();
+ const double startCPU = cpuTime();
+
+ Integrator::SuccessfulStepStatus status;
+ do {
+ status=ts.stepTo(RunTime);
+ #ifdef TEST_REPEATABILITY
+ states[ntry].push_back(ts.getState());
+ #endif
+ const int j = states[ntry].size()-1;
+ if (ntry>0 && j < (int)states[ntry-1].size()) {
+ int prev = ntry-1;
+ Real dt = states[ntry][j].getTime()
+ - states[prev][j].getTime();
+ volatile double vd;
+ const int ny = states[ntry][j].getNY();
+ Vector d(ny);
+ for (int i=0; i<ny; ++i) {
+ vd = states[ntry][j].getY()[i]
+ - states[prev][j].getY()[i];
+ d[i] = vd;
+ }
+ timeDiff[prev].push_back(dt);
+ yDiff[prev].push_back(d);
+ cout << "\n" << states[prev][j].getTime()
+ << "+" << dt << ":" << d << endl;
+ }
+
+ const bool handledEvent = status==Integrator::ReachedEventTrigger;
+ if (handledEvent) {
+ ++nStepsWithEvent;
+ SimTK_DEBUG3("EVENT %3d @%.17g status=%s\n\n",
+ nStepsWithEvent, ts.getTime(),
+ Integrator::getSuccessfulStepStatusString(status).c_str());
+ } else {
+ SimTK_DEBUG3("step %3d @%.17g status=%s\n", integ.getNumStepsTaken(),
+ ts.getTime(),
+ Integrator::getSuccessfulStepStatusString(status).c_str());
+ }
+ #ifndef NDEBUG
+ viz.report(ts.getState());
+ #endif
+ //stateSaver->handleEvent(ts.getState());
+ } while (ts.getTime() < RunTime);
+
+
+ const double timeInSec = realTime()-startReal;
+ const double cpuInSec = cpuTime()-startCPU;
+ const int evals = integ.getNumRealizations();
+ cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
+ timeInSec << "s for " << ts.getTime() << "s sim (avg step="
+ << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
+ << (1000*ts.getTime())/evals << "ms/eval\n";
+ cout << "CPUtime " << cpuInSec << endl;
+
+ printf("Used Integrator %s at accuracy %g:\n",
+ integ.getMethodName(), integ.getAccuracyInUse());
+ printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
+ printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
+ printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
+ printf("# EVENT STEPS/HANDLER CALLS = %d/%d\n",
+ nStepsWithEvent, mbs.getNumHandleEventCalls()); }
+
+ for (int i=0; i<tries; ++i)
+ cout << "nstates " << i << " " << states[i].size() << endl;
+
+ // Instant replay.
+ while(true) {
+ printf("Hit ENTER for replay (%d states) ...",
+ stateSaver->getNumSavedStates());
+ getchar();
+ for (int i=0; i < stateSaver->getNumSavedStates(); ++i) {
+ mbs.realize(stateSaver->getState(i), Stage::Velocity);
+ viz.report(stateSaver->getState(i));
+ }
+ }
+
+ }
+ catch (const std::exception& e) {
+ printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+ }
+ catch (...) {
+ printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+}
+
+
+
+//==============================================================================
+// IMPACT HANDLING (CONTACT ON)
+//==============================================================================
+
+//------------------------------ HANDLE EVENT ----------------------------------
+// There are three different witness functions that cause this handler to get
+// invoked. The position- and velocity-level ones require an impulse. The
+// acceleration-level one just requires recalculating the active set, in the
+// same manner as liftoff or friction transition events.
+
+void ContactOn::
+handleEvent(State& s, Real accuracy, bool& shouldTerminate) const
+{
+ SimTK_DEBUG3("\nhandle %d impact@%.17g (%s)\n", m_which, s.getTime(),
+ m_stage.getName().c_str());
+
+ if (m_stage == Stage::Acceleration) {
+ SimTK_DEBUG("\n---------------CONTACT ON (ACCEL)--------------\n");
+ SimTK_DEBUG2("CONTACT triggered by element %d @t=%.15g\n",
+ m_which, s.getTime());
+ m_mbs.realize(s, Stage::Acceleration);
+
+ #ifndef NDEBUG
+ cout << " triggers=" << s.getEventTriggers() << "\n";
+ #endif
+
+ m_unis.selectActiveConstraints(s, accuracy);
+ SimTK_DEBUG("---------------CONTACT ON (ACCEL) done.--------------\n");
+ return;
+ }
+
+ MyElementSubset proximal;
+ m_unis.findProximalElements(s, accuracy, accuracy, proximal);
+
+ // Zero out accumulated impulses and perform any other necessary
+ // initialization of contact and friction elements.
+ for (unsigned i=0; i < proximal.m_contact.size(); ++i) {
+ const int which = proximal.m_contact[i];
+ m_unis.updContactElement(which)
+ .initializeForImpact(s, m_unis.getCaptureVelocity());
+ }
+ for (unsigned i=0; i < proximal.m_friction.size(); ++i) {
+ const int which = proximal.m_friction[i];
+ m_unis.updFrictionElement(which).initializeForStiction(s);
+ }
+
+ SimTK_DEBUG("\n---------------------CONTACT ON---------------------\n");
+ SimTK_DEBUG3("\nIMPACT (%s) for contact %d at t=%.16g\n",
+ m_stage.getName().c_str(), m_which, s.getTime());
+ SimTK_DEBUG2(" %d/%d proximal contact/friction elements\n",
+ proximal.m_contact.size(), proximal.m_friction.size());
+
+ m_unis.showConstraintStatus(s, "ENTER IMPACT (CONTACT ON)");
+
+ bool needMoreCompression = true;
+ while (needMoreCompression) {
+ processCompressionPhase(proximal, s);
+ needMoreCompression = false;
+ if (processExpansionPhase(proximal, s)) {
+ for (unsigned i=0; i < proximal.m_contact.size(); ++i) {
+ const int which = proximal.m_contact[i];
+ const MyContactElement& contact =
+ m_unis.getContactElement(which);
+ if (contact.getVerr(s) < 0) {
+ needMoreCompression = true;
+ break;
+ }
+ }
+ }
+ if (needMoreCompression) {
+ SimTK_DEBUG("EXPANSION CAUSED SECONDARY IMPACT: compressing again\n");
+ } else {
+ SimTK_DEBUG("All constraints satisfied after expansion: done.\n");
+ }
+ }
+
+ // Record new previous slip velocities for all the sliding friction
+ // since velocities have changed. First loop collects the velocities.
+ m_mbs.realize(s, Stage::Velocity);
+ for (unsigned i=0; i < proximal.m_friction.size(); ++i) {
+ const int which = proximal.m_friction[i];
+ MyFrictionElement& fric = m_unis.updFrictionElement(which);
+ if (!fric.isMasterActive(s) || fric.isSticking(s)) continue;
+ fric.recordSlipDir(s);
+ }
+
+ // Now update all the previous slip direction state variables from the
+ // recorded values.
+ for (unsigned i=0; i < proximal.m_friction.size(); ++i) {
+ const int which = proximal.m_friction[i];
+ const MyFrictionElement& fric = m_unis.getFrictionElement(which);
+ if (!fric.isMasterActive(s) || fric.isSticking(s)) continue;
+ fric.updatePreviousSlipDirFromRecorded(s);
+ }
+
+ m_unis.selectActiveConstraints(s, accuracy);
+
+ SimTK_DEBUG("\n-----------------END CONTACT ON---------------------\n");
+}
+
+
+
+//------------------------ PROCESS COMPRESSION PHASE ---------------------------
+// Given a list of proximal unilateral constraints (contact and stiction),
+// determine which ones are active in the least squares solution for the
+// constraint impulses. Candidates are those constraints that meet the
+// kinematic proximity condition -- for contacts, position less than
+// tolerance; for stiction, master contact is proximal. Also, any
+// constraint that is currently active is a candidate, regardless of its
+// kinematics.
+//
+// TODO: stiction only enabled for contacts that aren't sliding; no other
+// stiction will be enabled after the impact starts.
+// TODO: sliding friction impulses
+//
+// Algorithm
+// ---------
+// We're given a set of candidates including contacts and stiction. If any are
+// inactive, activate them.
+// -- at this point all verr==0, some impulses f might be < 0
+//
+// loop
+// - Calculate impulses with the current active set
+// (iterate sliding impulses until f=mu_d*N to tol, where N is normal
+// impulse)
+// - Calculate f for active constraints, verr for inactive
+// - If all f>=0, verr>=0 -> break loop
+// - Check for verr < 0 [tol?]. Shouldn't happen but if it does must turn on the
+// associated constraint for the worst violation, then -> continue loop
+// - Find worst (most negative) offender:
+// contact offense = fc < 0 ? fc : 0
+// stiction offense = mu_d*max(0, fc) - |fs|
+// - Choose constraint to deactivate:
+// worst is a stiction constraint: choose it
+// worst is a contact constraint: if it has stiction, chose that
+// otherwise choose the contact constraint
+// - Inactivate chosen constraint
+// (if stiction, record impending slip direction & N for stick->slide)
+// end loop
+//
+void ContactOn::
+processCompressionPhase(MyElementSubset& proximal,
+ State& s) const
+{
+ const int ReviseNormalNIters = 6;
+
+ SimTK_DEBUG2("Entering processCompressionPhase(): "
+ "%d/%d impact/stick candidates ...\n", proximal.m_contact.size(),
+ proximal.m_friction.size());
+
+ if (!proximal.m_friction.empty()) {
+ SimTK_DEBUG("**** STICTION IMPACT (possibly)\n");
+ }
+
+ if (proximal.m_contact.empty()) {
+ // Can't be any friction either, if there are no contacts.
+ SimTK_DEBUG("EXIT processCompressionPhase: no proximal candidates.\n");
+ return;
+ }
+
+ // If all the proximal contacts have positive normal
+ // velocities already then we won't need to generate any impulses.
+ // TODO: this needs to be reconsidered for handling Painleve situations
+ // in which the impact is caused by a sliding friction inconsistency.
+ SimTK_DEBUG("preliminary analysis of contact constraints ...\n");
+ bool anyUnsatisfiedContactConstraints = false;
+ for (unsigned i=0; i < proximal.m_contact.size(); ++i) {
+ const int which = proximal.m_contact[i];
+ SimTK_DEBUG1(" %d: ", which);
+ const MyContactElement& cont = m_unis.getContactElement(which);
+ const Real verr = cont.getVerr(s);
+ SimTK_DEBUG1("off verr=%g\n", verr);
+ if (verr < 0) {
+ anyUnsatisfiedContactConstraints = true;
+ break;
+ }
+ }
+
+ if (!anyUnsatisfiedContactConstraints) {
+ SimTK_DEBUG("... nothing to do -- all contact constraints satisfied.\n");
+ return;
+ } else {
+ SimTK_DEBUG("... an impulse is needed. Continuing.\n");
+ }
+
+ Vector lambda;
+ const Vector u0 = s.getU(); // save presenting velocity
+
+ // Assume at first that all proximal contacts will participate. This is
+ // necessary to ensure that we get a least squares solution for the impulse
+ // involving as many constraints as possible sharing the impulse.
+ m_unis.enableConstraintSubset(proximal, true, s);
+
+ int pass=0, nContactsDisabled=0, nStictionDisabled=0, nContactsReenabled=0;
+ while (true) {
+ ++pass;
+ SimTK_DEBUG1("processCompressionPhase(): pass %d\n", pass);
+
+ // Given an active set, evaluate impulse multipliers & forces, and
+ // evaluate resulting constraint velocity errors.
+ calcStoppingImpulse(proximal, s, lambda);
+ // TODO: ignoring sliding impacts; should converge them here.
+ updateVelocities(u0, lambda, s);
+
+ // Scan all proximal contacts to find the active one that has the
+ // most negative normal force, and the inactive one that has the
+ // most negative velocity error (hopefully none will).
+
+ int worstActiveContact=-1; Real mostNegativeContactImpulse=0;
+ int worstInactiveContact=-1; Real mostNegativeVerr=0;
+
+ SimTK_DEBUG("analyzing impact constraints ...\n");
+ for (unsigned i=0; i < proximal.m_contact.size(); ++i) {
+ const int which = proximal.m_contact[i];
+ SimTK_DEBUG1(" %d: ", which);
+ const MyContactElement& cont = m_unis.getContactElement(which);
+ if (cont.isDisabled(s)) {
+ const Real verr = cont.getVerr(s);
+ SimTK_DEBUG1("off verr=%g\n", verr);
+ if (verr < mostNegativeVerr) {
+ worstInactiveContact = which;
+ mostNegativeVerr = verr;
+ }
+ } else {
+ const Real f =
+ cont.getMyValueFromConstraintSpaceVector(s, lambda);
+ SimTK_DEBUG1("on impulse=%g\n", f);
+ if (f < mostNegativeContactImpulse) {
+ worstActiveContact = which;
+ mostNegativeContactImpulse = f;
+ }
+ }
+ }
+
+ // This is bad and might cause cycling.
+ if (mostNegativeVerr < 0) {
+ SimTK_DEBUG2(" !!! Inactive contact %d violated, verr=%g\n",
+ worstInactiveContact, mostNegativeVerr);
+ const MyContactElement& cont =
+ m_unis.getContactElement(worstInactiveContact);
+ //TODO -- must use a tolerance or prevent looping
+ //++nContactsReenabled;
+ //cont.enable(s);
+ //continue;
+ }
+
+ SimTK_DEBUG(" All inactive contacts are satisfied.\n");
+
+ #ifndef NDEBUG
+ if (mostNegativeContactImpulse == 0)
+ printf(" All active contacts are satisfied.\n");
+ else
+ printf(" Active contact %d was worst violator with f=%g\n",
+ worstActiveContact, mostNegativeContactImpulse);
+ #endif
+
+ int worstActiveStiction=-1; Real mostNegativeStictionCapacity=0;
+ SimTK_DEBUG("analyzing stiction constraints ...\n");
+ for (unsigned i=0; i < proximal.m_friction.size(); ++i) {
+ const int which = proximal.m_friction[i];
+ SimTK_DEBUG1(" %d: ", which);
+ const MyFrictionElement& fric = m_unis.getFrictionElement(which);
+ if (!fric.isSticking(s)) {
+ SimTK_DEBUG("off\n");
+ continue;
+ }
+ // TODO: Kludge -- must be point contact.
+ const MyPointContactFriction& ptfric =
+ dynamic_cast<const MyPointContactFriction&>(fric);
+ const MyPointContact& cont = ptfric.getMyPointContact();
+ const Real N = cont.getMyValueFromConstraintSpaceVector(s, lambda);
+ const Real fsmag =
+ ptfric.getMyImpulseMagnitudeFromConstraintSpaceVector(s, lambda);
+ const Real mu_d = fric.getDynamicFrictionCoef();
+ const Real capacity = mu_d*std::max(N,Real(0)) - fsmag;
+ SimTK_DEBUG2("on capacity=%g (N=%g)\n", capacity, N);
+
+ if (capacity < mostNegativeStictionCapacity) {
+ worstActiveStiction = which;
+ mostNegativeStictionCapacity = capacity;
+ }
+ }
+
+ #ifndef NDEBUG
+ if (mostNegativeStictionCapacity == 0)
+ printf(" All active stiction constraints are satisfied.\n");
+ else
+ printf(" Active stiction %d was worst violator with capacity=%g\n",
+ worstActiveStiction, mostNegativeStictionCapacity);
+ #endif
+
+ if (mostNegativeContactImpulse==0 && mostNegativeStictionCapacity==0) {
+ SimTK_DEBUG("DONE. Current active set is a winner.\n");
+ break;
+ }
+
+ // Restore original velocity.
+ s.updU() = u0;
+
+ if (mostNegativeStictionCapacity <= mostNegativeContactImpulse) {
+ SimTK_DEBUG1(" Disable stiction %d\n", worstActiveStiction);
+ MyFrictionElement& fric =
+ m_unis.updFrictionElement(worstActiveStiction);
+ // TODO: need the impulse version of this
+ //fric.recordImpendingSlipInfo(s);
+ ++nStictionDisabled;
+ fric.disableStiction(s);
+ continue;
+ }
+
+ // A contact constraint was the worst violator. If that contact
+ // constraint has an active stiction constraint, we have to disable
+ // the stiction constraint first.
+ SimTK_DEBUG1(" Contact %d was the worst violator.\n",
+ worstActiveContact);
+ const MyContactElement& cont =
+ m_unis.getContactElement(worstActiveContact);
+ assert(!cont.isDisabled(s));
+
+ if (cont.hasFrictionElement()) {
+ MyFrictionElement& fric = cont.updFrictionElement();
+ if (fric.isSticking(s)) {
+ SimTK_DEBUG1(" ... but must disable stiction %d first.\n",
+ fric.getFrictionIndex());
+ // TODO: need the impulse version of this
+ //fric.recordImpendingSlipInfo(s);
+ ++nStictionDisabled;
+ fric.disableStiction(s);
+ continue;
+ }
+ }
+
+ SimTK_DEBUG1(" Disable contact %d\n", worstActiveContact);
+ ++nContactsDisabled;
+ cont.disable(s);
+ // Go back for another pass.
+ }
+
+
+ // Now update the entries for each proximal constraint to reflect the
+ // compression impulse and post-compression velocity.
+ SimTK_DEBUG(" Compression results:\n");
+ for (unsigned i=0; i < proximal.m_contact.size(); ++i) {
+ const int which = proximal.m_contact[i];
+ MyContactElement& cont = m_unis.updContactElement(which);
+ if (!cont.isDisabled(s))
+ cont.recordImpulse(MyContactElement::Compression, s, lambda);
+ SimTK_DEBUG4(" %d %3s: Ic=%g, V=%g\n",
+ which, cont.isDisabled(s) ? "off" : "ON",
+ cont.getCompressionImpulse(), cont.getVerr(s));
+ }
+
+ SimTK_DEBUG("... compression phase done.\n");
+}
+
+
+//------------------------- PROCESS EXPANSION PHASE ----------------------------
+bool ContactOn::
+processExpansionPhase(MyElementSubset& proximal,
+ State& s) const
+{
+ SimTK_DEBUG("Entering processExpansionPhase() ...\n");
+
+ // Generate an expansion impulse if there were any active contacts that
+ // still have some restitution remaining.
+ Vector expansionImpulse;
+
+ bool anyChange = false;
+ for (unsigned i=0; i<proximal.m_contact.size(); ++i) {
+ const int which = proximal.m_contact[i];
+ MyContactElement& uni = m_unis.updContactElement(which);
+ if (uni.isDisabled(s)||uni.isRestitutionDone()
+ ||uni.getEffectiveCoefRest()==0
+ ||uni.getCompressionImpulse()<=0)
+ continue;
+ uni.setMyExpansionImpulse(s, uni.getEffectiveCoefRest(),
+ expansionImpulse);
+ uni.recordImpulse(MyContactElement::Expansion,s,expansionImpulse);
+ uni.setRestitutionDone(true);
+ anyChange = true;
+ }
+
+ if (!anyChange) {
+ SimTK_DEBUG("... no expansion impulse -- done.\n");
+ return false;
+ }
+
+ // We generated an expansion impulse. Apply it and update velocities.
+ updateVelocities(Vector(), expansionImpulse, s);
+
+ // Release any constraint that now has a positive velocity.
+ Array_<int> toDisable;
+ for (unsigned i=0; i < proximal.m_contact.size(); ++i) {
+ const int which = proximal.m_contact[i];
+ const MyContactElement& uni = m_unis.getContactElement(which);
+ if (!uni.isDisabled(s) && uni.getVerr(s) > 0)
+ toDisable.push_back(which);
+ }
+
+ // Now do the actual disabling (can't mix this with checking velocities)
+ // because disabling invalidates Instance stage.
+ for (unsigned i=0; i < toDisable.size(); ++i) {
+ const int which = toDisable[i];
+ const MyContactElement& uni = m_unis.getContactElement(which);
+ uni.disable(s);
+ }
+
+ SimTK_DEBUG(" Expansion results:\n");
+ m_mbs.realize(s, Stage::Velocity);
+ for (unsigned i=0; i < proximal.m_contact.size(); ++i) {
+ const int which = proximal.m_contact[i];
+ const MyContactElement& uni = m_unis.getContactElement(which);
+ SimTK_DEBUG4(" %d %3s: Ie=%g, V=%g\n",
+ which, uni.isDisabled(s) ? "off" : "ON",
+ uni.getExpansionImpulse(), uni.getVerr(s));
+ }
+
+ SimTK_DEBUG("... expansion phase done.\n");
+
+ return true;
+}
+
+
+
+
+//-------------------------- CALC STOPPING IMPULSE -----------------------------
+// Calculate the impulse that eliminates all residual velocity for the
+// current set of enabled constraints.
+// Note: if you have applied impulses also (like sliding friction),
+// convert to generalized impulse f, then to desired delta V in constraint
+// space like this: deltaV = G*M\f; add that to the verrs to get the total
+// velocity change that must be produced by the impulse.
+void ContactOn::
+calcStoppingImpulse(const MyElementSubset& proximal,
+ const State& s,
+ Vector& lambda0) const
+{
+ const SimbodyMatterSubsystem& matter = m_mbs.getMatterSubsystem();
+ m_mbs.realize(s, Stage::Dynamics); // TODO: should only need Position
+ Vector desiredDeltaV; // in constraint space
+ SimTK_DEBUG(" Entering calcStoppingImpulse() ...\n");
+ bool gotOne = false;
+ for (unsigned i=0; i < proximal.m_contact.size(); ++i) {
+ const int which = proximal.m_contact[i];
+ const MyContactElement& uni = m_unis.getContactElement(which);
+ if (uni.isDisabled(s))
+ continue;
+ SimTK_DEBUG2(" uni constraint %d enabled, v=%g\n",
+ which, uni.getVerr(s));
+ uni.setMyDesiredDeltaV(s, desiredDeltaV);
+ gotOne = true;
+ }
+ for (unsigned i=0; i < proximal.m_friction.size(); ++i) {
+ const int which = proximal.m_friction[i];
+ const MyFrictionElement& fric = m_unis.getFrictionElement(which);
+ if (!fric.isSticking(s))
+ continue;
+ SimTK_DEBUG2(" friction constraint %d enabled, |v|=%g\n",
+ which, fric.getActualSlipSpeed(s));
+ fric.setMyDesiredDeltaV(s, desiredDeltaV);
+ gotOne = true;
+ }
+
+ if (gotOne) matter.solveForConstraintImpulses(s, desiredDeltaV, lambda0);
+ else lambda0.clear();
+#ifndef NDEBUG
+ cout << " ... done. Stopping impulse=" << lambda0 << endl;
+#endif
+}
+
+
+
+//---------------------------- UPDATE VELOCITIES -------------------------------
+void ContactOn::
+updateVelocities(const Vector& u0, const Vector& lambda, State& state) const {
+ const SimbodyMatterSubsystem& matter = m_mbs.getMatterSubsystem();
+ Vector f, deltaU;
+ assert(u0.size()==0 || u0.size() == state.getNU());
+
+ m_mbs.realize(state, Stage::Dynamics); // TODO: Position
+ matter.multiplyByGTranspose(state,lambda,f);
+ matter.multiplyByMInv(state,f,deltaU);
+ if (u0.size()) state.updU() = u0 + deltaU;
+ else state.updU() += deltaU;
+ m_mbs.realize(state, Stage::Velocity);
+}
+
+
+
+//==============================================================================
+// MY UNILATERAL CONSTRAINT SET
+//==============================================================================
+
+
+//------------------------ SELECT ACTIVE CONSTRAINTS ---------------------------
+void MyUnilateralConstraintSet::
+selectActiveConstraints(State& state, Real accuracy) const {
+
+ // Find all the contacts and stiction elements that might be active based
+ // on kinematics.
+ MyElementSubset candidates;
+
+ bool needRestart;
+ do {
+ //TODO: this (mis)use of accuracy needs to be revisited.
+ findCandidateElements(state, accuracy, accuracy, candidates);
+
+ // Evaluate accelerations and reaction forces and check if
+ // any of the active contacts are generating negative ("pulling")
+ // forces; if so, inactivate them.
+ findActiveCandidates(state, candidates);
+
+ Array_<Real> slipVels(candidates.m_friction.size());
+ for (unsigned i=0; i < candidates.m_friction.size(); ++i) {
+ const int which = candidates.m_friction[i];
+ const MyFrictionElement& fric = getFrictionElement(which);
+ slipVels[i] = fric.getActualSlipSpeed(state);
+ }
+
+ // Finally, project active constraints to the constraint manifolds.
+ const Real tol = accuracy/1000;
+ SimTK_DEBUG1("projecting active constraints to tol=%g\n", tol);
+ m_mbs.project(state, tol);
+
+ // It is possible that the projection above took some marginally-sliding
+ // friction and slowed it down enough to make it a stiction candidate.
+ needRestart = false;
+ for (unsigned i=0; i < candidates.m_sliding.size(); ++i) {
+ const int which = candidates.m_sliding[i];
+ const MyFrictionElement& fric = getFrictionElement(which);
+ if ( fric.getActualSlipSpeed(state) <= accuracy
+ || fric.calcSlipSpeedWitness(state) <= 0)
+ {
+ SimTK_DEBUG3("***RESTART1** selectActiveConstraints() because "
+ "sliding velocity of friction %d is |v|=%g or witness=%g\n",
+ which, fric.getActualSlipSpeed(state),
+ fric.calcSlipSpeedWitness(state));
+ needRestart = true;
+ break;
+ }
+ }
+ if (needRestart) continue;
+ for (unsigned i=0; i < candidates.m_friction.size(); ++i) {
+ const int which = candidates.m_friction[i];
+ const MyFrictionElement& fric = getFrictionElement(which);
+ if (fric.isSticking(state)) continue;
+ if (slipVels[i] > accuracy
+ && fric.getActualSlipSpeed(state) <= accuracy)
+ {
+ SimTK_DEBUG3("***RESTART2** selectActiveConstraints() because "
+ "sliding velocity of friction %d went from |v|=%g to |v|=%g\n",
+ which, slipVels[i], fric.getActualSlipSpeed(state));
+ needRestart = true;
+ break;
+ }
+ }
+
+ } while (needRestart);
+}
+
+
+
+
+//-------------------------- FIND ACTIVE CANDIDATES ---------------------------
+// Given a list of candidate unilateral constraints (contact and stiction),
+// determine which ones are active in the least squares solution for the
+// constraint multipliers. Candidates are those constraints that meet all
+// kinematic conditions -- for contacts, position and velocity errors less than
+// tolerance; for stiction, sliding velocity less than tolerance. Also, any
+// constraint that is currently active is a candidate, regardless of its
+// kinematics.
+//
+// This method should be called only from within an event handler. For sliding
+// friction it will have reset the "previous slip direction" to the current
+// slip or impending slip direction, and converged the remembered normal force.
+//
+// Algorithm
+// ---------
+// We're given a set of candidates including contacts and stiction. If any are
+// inactive, activate them.
+// -- at this point all aerr==0, some ferr might be < 0
+//
+// loop
+// - Realize(Acceleration) with the current active set
+// (iterate sliding forces until f=mu_d*N to tol)
+// - Calculate ferr for active constraints, aerr for inactive
+// - If all ferr>=0, aerr>=0 -> break loop
+// - Check for aerr < 0 [tol?]. Shouldn't happen but if it does must turn on the
+// associated constraint for the worst violation, then -> continue loop
+// - Find worst (most negative) offender:
+// contact offense = fc < 0 ? fc : 0
+// stiction offense = mu_s*max(0, fc) - |fs|
+// - Choose constraint to deactivate:
+// worst is a stiction constraint: choose it
+// worst is a contact constraint: if it has stiction, chose that
+// otherwise choose the contact constraint
+// - Inactivate chosen constraint
+// (if stiction, record impending slip direction & N for stick->slide)
+// end loop
+//
+void MyUnilateralConstraintSet::
+findActiveCandidates(State& s, const MyElementSubset& candidates) const
+{
+ const int ReviseNormalNIters = 6;
+ showConstraintStatus(s, "ENTER findActiveCandidates()");
+ if (candidates.m_contact.empty()) {
+ // Can't be any friction either, if there are no contacts.
+ SimTK_DEBUG("EXIT findActiveCandidates: no candidates.\n");
+ m_mbs.realize(s, Stage::Acceleration);
+ return;
+ }
+
+ SimTK_DEBUG3(
+ "findActiveCandidates() for %d/%d/%d contact/stick/slip candidates ...\n",
+ candidates.m_contact.size(), candidates.m_friction.size(),
+ candidates.m_sliding.size());
+
+ // Enable all candidate contact and stiction constraints if any are
+ // currently disabled.
+ enableConstraintSubset(candidates, true, s);
+
+ int pass=0, nContactsDisabled=0, nStictionDisabled=0, nContactsReenabled=0;
+ while (true) {
+ ++pass;
+ SimTK_DEBUG1("\nfindActiveCandidates(): pass %d\n", pass);
+
+ // Given an active set, evaluate multipliers and accelerations, and
+ // converge sliding forces.
+ m_mbs.realize(s, Stage::Acceleration);
+ SimTK_DEBUG("REVISE NORMAL #1\n");
+ for (int i=0; i < ReviseNormalNIters; ++i) {
+ s.autoUpdateDiscreteVariables();
+ s.invalidateAllCacheAtOrAbove(Stage::Dynamics);
+ m_mbs.realize(s, Stage::Acceleration);
+ }
+
+ // Scan all candidate contacts to find the active one that has the
+ // most negative normal force, and the inactive one that has the
+ // most negative acceleration error (hopefully none will).
+
+ int worstActiveContact=-1; Real mostNegativeContactForce=0;
+ int worstInactiveContact=-1; Real mostNegativeAerr=0;
+
+ SimTK_DEBUG("analyzing contact constraints ...\n");
+ for (unsigned i=0; i < candidates.m_contact.size(); ++i) {
+ const int which = candidates.m_contact[i];
+ SimTK_DEBUG1(" %d: ", which);
+ const MyContactElement& cont = getContactElement(which);
+ if (cont.isDisabled(s)) {
+ const Real aerr = cont.getAerr(s);
+ SimTK_DEBUG1("off aerr=%g\n", aerr);
+ if (aerr < mostNegativeAerr) {
+ worstInactiveContact = which;
+ mostNegativeAerr = aerr;
+ }
+ } else {
+ const Real f = cont.getForce(s);
+ SimTK_DEBUG1("on f=%g\n", f);
+ if (f < mostNegativeContactForce) {
+ worstActiveContact = which;
+ mostNegativeContactForce = f;
+ }
+ }
+ }
+
+ // This is bad and might cause cycling.
+ if (mostNegativeAerr < 0) {
+ SimTK_DEBUG2(" !!! Inactive contact %d violated, aerr=%g\n",
+ worstInactiveContact, mostNegativeAerr);
+ const MyContactElement& cont = getContactElement(worstInactiveContact);
+ //TODO -- must use a tolerance or prevent looping
+ //++nContactsReenabled;
+ //cont.enable(s);
+ //continue;
+ }
+
+ SimTK_DEBUG(" All inactive contacts are satisfied.\n");
+
+ #ifndef NDEBUG
+ if (mostNegativeContactForce == 0)
+ printf(" All active contacts are satisfied.\n");
+ else
+ printf(" Active contact %d was worst violator with f=%g\n",
+ worstActiveContact, mostNegativeContactForce);
+ #endif
+
+ int worstActiveStiction=-1; Real mostNegativeStictionCapacity=0;
+ SimTK_DEBUG("analyzing stiction constraints ...\n");
+ for (unsigned i=0; i < candidates.m_friction.size(); ++i) {
+ const int which = candidates.m_friction[i];
+ SimTK_DEBUG1(" %d: ", which);
+ const MyFrictionElement& fric = getFrictionElement(which);
+ if (!fric.isSticking(s)) {
+ SimTK_DEBUG("off\n");
+ continue;
+ }
+ const Real mu_s = fric.getStaticFrictionCoef();
+ const Real N = fric.getMasterNormalForce(s); // might be negative
+ const Real fsmag = fric.getActualFrictionForce(s);
+ const Real capacity = mu_s*std::max(N,Real(0)) - fsmag;
+ SimTK_DEBUG2("on capacity=%g (N=%g)\n", capacity, N);
+
+ if (capacity < mostNegativeStictionCapacity) {
+ worstActiveStiction = which;
+ mostNegativeStictionCapacity = capacity;
+ }
+ }
+
+ #ifndef NDEBUG
+ if (mostNegativeStictionCapacity == 0)
+ printf(" All active stiction constraints are satisfied.\n");
+ else
+ printf(" Active stiction %d was worst violator with capacity=%g\n",
+ worstActiveStiction, mostNegativeStictionCapacity);
+ #endif
+
+ if (mostNegativeContactForce==0 && mostNegativeStictionCapacity==0) {
+ SimTK_DEBUG("DONE. Current active set is a winner.\n");
+ break;
+ }
+
+ if (mostNegativeStictionCapacity <= mostNegativeContactForce) {
+ SimTK_DEBUG1(" Disable stiction %d\n", worstActiveStiction);
+ MyFrictionElement& fric = updFrictionElement(worstActiveStiction);
+ fric.recordImpendingSlipInfo(s);
+ ++nStictionDisabled;
+ fric.disableStiction(s);
+ continue;
+ }
+
+ // A contact constraint was the worst violator. If that contact
+ // constraint has an active stiction constraint, we have to disable
+ // the stiction constraint first.
+ SimTK_DEBUG1(" Contact %d was the worst violator.\n", worstActiveContact);
+ const MyContactElement& cont = getContactElement(worstActiveContact);
+ assert(!cont.isDisabled(s));
+
+ if (cont.hasFrictionElement()) {
+ MyFrictionElement& fric = cont.updFrictionElement();
+ if (fric.isSticking(s)) {
+ SimTK_DEBUG1(" ... but must disable stiction %d first.\n",
+ fric.getFrictionIndex());
+ fric.recordImpendingSlipInfo(s);
+ ++nStictionDisabled;
+ fric.disableStiction(s);
+ continue;
+ }
+ }
+
+ SimTK_DEBUG1(" Disable contact %d\n", worstActiveContact);
+ ++nContactsDisabled;
+ cont.disable(s);
+ // Go back for another pass.
+ }
+
+ // Reset all the slip directions so that all slip->stick event witnesses
+ // will be positive when integration resumes.
+ for (unsigned i=0; i < candidates.m_sliding.size(); ++i) {
+ const int which = candidates.m_sliding[i];
+ const MyFrictionElement& fric = getFrictionElement(which);
+ if (!fric.isMasterActive(s)) continue;
+ fric.updatePreviousSlipDirFromRecorded(s);
+ }
+
+ // Always leave at acceleration stage.
+ m_mbs.realize(s, Stage::Acceleration);
+
+ SimTK_DEBUG3("... Done; %d contacts, %d stictions broken; %d re-enabled.\n",
+ nContactsDisabled, nStictionDisabled, nContactsReenabled);
+
+ showConstraintStatus(s, "EXIT findActiveCandidates()");
+}
+
+void MyUnilateralConstraintSet::
+showConstraintStatus(const State& s, const String& place) const
+{
+#ifndef NDEBUG
+ printf("\n%s: uni status @t=%.15g\n", place.c_str(), s.getTime());
+ m_mbs.realize(s, Stage::Acceleration);
+ for (int i=0; i < getNumContactElements(); ++i) {
+ const MyContactElement& contact = getContactElement(i);
+ const bool isActive = !contact.isDisabled(s);
+ printf(" %6s %2d %s, h=%g dh=%g f=%g\n",
+ isActive?"ACTIVE":"off", i, contact.getContactType().c_str(),
+ contact.getPerr(s),contact.getVerr(s),
+ isActive?contact.getForce(s):Zero);
+ }
+ for (int i=0; i < getNumFrictionElements(); ++i) {
+ const MyFrictionElement& friction = getFrictionElement(i);
+ if (!friction.isMasterActive(s))
+ continue;
+ const bool isSticking = friction.isSticking(s);
+ printf(" %8s friction %2d, |v|=%g witness=%g\n",
+ isSticking?"STICKING":"sliding", i,
+ friction.getActualSlipSpeed(s),
+ isSticking?friction.calcStictionForceWitness(s)
+ :friction.calcSlipSpeedWitness(s));
+ friction.writeFrictionInfo(s, " ", std::cout);
+ }
+ printf("\n");
+#endif
+}
+
+//==============================================================================
+// MY SLIDING FRICTION FORCE -- Implementation
+//==============================================================================
+// This is used for friction when slipping or when slip is impending, so it
+// will generate a force even if there is no slip velocity. Whenever the slip
+// speed |v|<=vtol, or if it has reversed direction, we consider it unreliable
+// and leave the applied force direction unchanged until the next transition
+// event. At that point activating the stiction constraint will be attempted.
+// If the stiction condition is violated, a new impending slip direction is
+// recorded opposing the direction of the resulting constraint force.
+//
+// The friction force is a 2-vector F calculated at Dynamics stage, applied
+// equal and opposite to the two contacting bodies at their mutual contact
+// point:
+// F = -mu*N_est*d_eff
+// d_eff is the effective slip direction that is to be opposed by the force.
+//
+// This is composed of several functions:
+// shouldUpdate(v) = ~v*d_prev > 0 && |v|>vtol
+// d_eff(v) = shouldUpdate(v) ? v/|v| : d_prev
+// v_eff(v) = ~v*d_eff
+// mu(v) = mu_d + mu_v * max(v_eff,0)
+// Fmag(v; N) = mu(v)*N
+// F(v; N) = -Fmag(v,N)*d_eff(v)
+// N_est = N_prev
+//
+// mu_d ... the dynamic coefficient of friction (a scalar constant >= 0)
+// mu_v ... viscous coefficient of friction (>= 0)
+// N_prev... previous normal force magnitude (a discrete state variable)
+// d_prev... previous or impending slip direction (a discrete state variable)
+// d_eff ... the effective slip direction, a unit length 2-vector
+// v_eff ... slip speed in d_eff direction, for viscous friction
+//
+// There is a sliding-to-stiction event witness function
+// e1(v)=dot(v, d_prev) velocity reversal
+//
+// TODO: other possible witness functions:
+// e2(v)=|v|-vtol slowdown (would often be missed)
+// e(v) = dot(v, d_prev) - vtol [signed]
+//
+// N_prev is an auto-update variable whose update value is set at Acceleration
+// stage from the actual normal force magnitude N of this friction element's
+// master contact element. N_prev is also set manually whenever sliding is
+// enabled, to the current normal force. In general we have Ni=Ni(F) (where
+// F={Fi} is a vector of all nf active sliding friction forces), and
+// Fi=Fi(Ni_est), so the error in the magnitude of the i'th applied friction
+// force is Fi_err=mu_i(v_i)*(Ni_est-Ni). If this is too large we have to
+// iterate until Ni_est is close enough to Ni for all i (this has to be done
+// simultaneously for the system as a whole).
+//
+// d_prev is an auto-update variable whose update value is set at Velocity
+// stage, if shouldUpdate(v), otherwise it remains unchanged. It is also set
+// manually when transitioning from sticking to sliding, to -F/|F| where F was
+// the last stiction force vector.
+//
+class MySlidingFrictionForceImpl : public Force::Custom::Implementation {
+public:
+ MySlidingFrictionForceImpl(const GeneralForceSubsystem& forces,
+ const MyPointContactFriction& ptFriction,
+ Real vtol)
+ : m_forces(forces), m_friction(ptFriction),
+ m_contact(ptFriction.getMyPointContact()), m_vtol(vtol)
+ {}
+
+ bool hasSlidingForce(const State& s) const
+ { return m_friction.isMasterActive(s) && !m_friction.isSticking(s); }
+
+
+ // Calculate d_eff, the direction to be opposed by the sliding force.
+ Vec2 getEffectiveSlipDir(const State& s) const {
+ const Vec2 Vslip = m_contact.getSlipVelocity(s);
+ const Vec2 prevVslipDir = getPrevSlipDir(s);
+ if (shouldUpdate(Vslip, prevVslipDir, m_vtol)) { // TODO: tol?
+ const Real v = Vslip.norm();
+ return Vslip/v;
+ }
+ return prevVslipDir;
+ }
+
+ // This is useful for reporting.
+ Real calcSlidingForceMagnitude(const State& state) const {
+ if (!hasSlidingForce(state)) return 0;
+ const Real slipV = m_contact.getSlipVelocity(state).norm();
+ return calcSlidingForceMagnitude(state, slipV);
+ }
+
+ // This is the force that will be applied next.
+ Vec2 calcSlidingForce(const State& state) const {
+ if (!hasSlidingForce(state))
+ return Vec2(0);
+
+ Vec2 dEff = getEffectiveSlipDir(state);
+ if (dEff.isNaN()) {
+ SimTK_DEBUG("NO SLIDING DIRECTION AVAILABLE\n");
+ return Vec2(0);
+ }
+
+ const Vec2 Vslip = m_contact.getSlipVelocity(state);
+ const Real vEff = ~Vslip*dEff;
+
+ const Real FMag = calcSlidingForceMagnitude(state, std::max(vEff,Zero));
+ assert(!isNaN(FMag));
+
+ const Vec2 F = -FMag*dEff;
+ return F;
+ }
+
+ // Return the related contact constraint's normal force value and slip
+ // velocity as recorded at the end of the last time step. Will be zero if
+ // the contact was not active then.
+ Real getPrevN(const State& state) const {
+ const Real& prevN = Value<Real>::downcast
+ (m_forces.getDiscreteVariable(state, m_prevNix));
+ return prevN;
+ }
+ void setPrevN(State& state, Real N) const {
+ Real& prevN = Value<Real>::updDowncast
+ (m_forces.updDiscreteVariable(state, m_prevNix));
+ if (isNaN(N))
+ printf("*** setPrevN(): N is NaN\n");
+ prevN = N;
+ }
+ Vec2 getPrevSlipDir(const State& state) const {
+ const Vec2& prevSlipDir = Value<Vec2>::downcast
+ (m_forces.getDiscreteVariable(state, m_prevSlipDirIx));
+ return prevSlipDir;
+ }
+ void setPrevSlipDir(State& state, const Vec2& slipDir) const {
+ Vec2& prevSlipDir = Value<Vec2>::updDowncast
+ (m_forces.updDiscreteVariable(state, m_prevSlipDirIx));
+ prevSlipDir = slipDir;
+ SimTK_DEBUG3("STATE CHG %d: prevDir to %g %g\n",
+ m_friction.getFrictionIndex(), slipDir[0], slipDir[1]);
+ }
+ //--------------------------------------------------------------------------
+ // Custom force virtuals
+ // Apply the sliding friction force if this is enabled.
+ void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces, Vector& mobilityForces) const
+ OVERRIDE_11
+ {
+ if (!hasSlidingForce(state))
+ return; // nothing to do
+
+ const MobilizedBody& bodyB = m_contact.getBody();
+ const MobilizedBody& bodyP = m_contact.getPlaneBody();
+ const Vec3& stationB = m_contact.getBodyStation();
+ const Vec3 stationP = bodyB.findStationLocationInAnotherBody
+ (state, stationB, bodyP);
+ const Vec2 fSlip = calcSlidingForce(state);
+ const Vec3 forceG(fSlip[0], 0, fSlip[1]); // only X,Z components
+ bodyB.applyForceToBodyPoint(state, stationB, forceG, bodyForces);
+ bodyP.applyForceToBodyPoint(state, stationP, -forceG, bodyForces);
+ }
+
+ // Sliding friction does not store energy.
+ Real calcPotentialEnergy(const State& state) const OVERRIDE_11 {return 0;}
+
+ // Allocate state variables for storing the previous normal force and
+ // sliding direction.
+ void realizeTopology(State& state) const OVERRIDE_11 {
+ // The previous normal force N is used as a first estimate for the
+ // mu*N sliding friction force calculated at Dynamics stage. However,
+ // the update value N cannot be determined until Acceleration stage.
+ m_prevNix = m_forces.allocateAutoUpdateDiscreteVariable
+ (state, Stage::Dynamics, new Value<Real>(0), Stage::Acceleration);
+ // The previous sliding direction is used in an event witness that
+ // is evaluated at Velocity stage.
+ m_prevSlipDirIx = m_forces.allocateAutoUpdateDiscreteVariable
+ (state, Stage::Velocity, new Value<Vec2>(Vec2(NaN)),
+ Stage::Velocity);
+ }
+
+ // If we're sliding, set the update value for the previous slip direction
+ // if the current slip velocity is usable.
+ void realizeVelocity(const State& state) const OVERRIDE_11 {
+ if (!hasSlidingForce(state))
+ return; // nothing to do
+ const Vec2 Vslip = m_contact.getSlipVelocity(state);
+ const Vec2 prevVslipDir = getPrevSlipDir(state);
+
+ if (shouldUpdate(Vslip, prevVslipDir, m_vtol)) {
+ Vec2& prevSlipUpdate = Value<Vec2>::updDowncast
+ (m_forces.updDiscreteVarUpdateValue(state, m_prevSlipDirIx));
+ const Real v = Vslip.norm();
+ const Vec2 slipDir = Vslip / v;
+ prevSlipUpdate = slipDir;
+ m_forces.markDiscreteVarUpdateValueRealized(state, m_prevSlipDirIx);
+
+ #ifndef NDEBUG
+ //printf("UPDATE %d: prevSlipDir=%g %g; now=%g %g; |v|=%g dot=%g vdot=%g\n",
+ // m_friction.getFrictionIndex(),
+ // prevVslipDir[0],prevVslipDir[1],slipDir[0],slipDir[1],
+ // v, ~slipDir*prevVslipDir, ~Vslip*prevVslipDir);
+ #endif
+ } else {
+ #ifndef NDEBUG
+ printf("NO UPDATE %d: prevSlipDir=%g %g; Vnow=%g %g; |v|=%g vdot=%g\n",
+ m_friction.getFrictionIndex(),
+ prevVslipDir[0],prevVslipDir[1],Vslip[0],Vslip[1],
+ Vslip.norm(), ~Vslip*prevVslipDir);
+ #endif
+ }
+ }
+
+ // Regardless of whether we're sticking or sliding, as long as the master
+ // contact is active use its normal force scalar as the update for our
+ // saved normal force.
+ void realizeAcceleration(const State& state) const OVERRIDE_11 {
+ if (!m_friction.isMasterActive(state))
+ return; // nothing to save
+ const Real N = m_contact.getForce(state); // normal force
+ const Real prevN = getPrevN(state);
+ if (N==prevN) return; // no need for an update
+
+ Real& prevNupdate = Value<Real>::updDowncast
+ (m_forces.updDiscreteVarUpdateValue(state, m_prevNix));
+
+ #ifndef NDEBUG
+ printf("UPDATE %d: N changing from %g -> %g (%.3g)\n",
+ m_friction.getFrictionIndex(),
+ prevN, N, std::abs(N-prevN)/std::max(N,prevN));
+ #endif
+ prevNupdate = N;
+ m_forces.markDiscreteVarUpdateValueRealized(state, m_prevNix);
+ }
+
+ //--------------------------------------------------------------------------
+
+private:
+ // Given the norm of the slip velocity already calculated, determine the
+ // magnitude of the slip force. If there is no viscous friction you can
+ // pass a zero vEff since it won't otherwise affect the force.
+ // Don't call this unless you know there may be a sliding force.
+ Real calcSlidingForceMagnitude(const State& state, Real vEff) const {
+ assert(vEff >= 0);
+ const Real prevN = getPrevN(state);
+ if (prevN <= 0) return 0; // no normal force -> no friction force
+
+ const Real mu_d = m_friction.getDynamicFrictionCoef();
+ const Real mu_v = m_friction.getViscousFrictionCoef();
+ const Real fMag = (mu_d + mu_v*vEff)*prevN;
+ return fMag;
+ }
+
+ // Determine whether the current slip velocity is reliable enough that
+ // we should use it to replace the previous slip velocity.
+ static bool shouldUpdate(const Vec2& Vslip, const Vec2& prevVslipDir,
+ Real velTol) {
+ const Real v2 = Vslip.normSqr();
+ if (prevVslipDir.isNaN())
+ return v2 > 0; // we'll take anything
+
+ // Check for reversal.
+ bool reversed = (~Vslip*prevVslipDir < 0);
+ return !reversed && (v2 > square(velTol));
+ }
+
+ const GeneralForceSubsystem& m_forces;
+ const MyPointContactFriction& m_friction;
+ const MyPointContact& m_contact;
+ const Real m_vtol;
+
+ mutable DiscreteVariableIndex m_prevNix; // previous normal force
+ mutable DiscreteVariableIndex m_prevSlipDirIx; // previous slip direction
+};
+
+// This is the force handle's constructor; it just creates the force
+// implementation object.
+MySlidingFrictionForce::MySlidingFrictionForce
+ (GeneralForceSubsystem& forces,
+ const MyPointContactFriction& friction,
+ Real vtol)
+: Force::Custom(forces, new MySlidingFrictionForceImpl(forces,friction,vtol))
+{}
+
+Real MySlidingFrictionForce::getPrevN(const State& state) const
+{ return getImpl().getPrevN(state); }
+void MySlidingFrictionForce::setPrevN(State& state, Real N) const
+{ getImpl().setPrevN(state,N); }
+Vec2 MySlidingFrictionForce::getPrevSlipDir(const State& state) const
+{ return getImpl().getPrevSlipDir(state); }
+bool MySlidingFrictionForce::hasPrevSlipDir(const State& state) const
+{ return !getImpl().getPrevSlipDir(state).isNaN(); }
+void MySlidingFrictionForce::
+setPrevSlipDir(State& state, const Vec2& slipDir) const
+{ getImpl().setPrevSlipDir(state, slipDir); }
+Real MySlidingFrictionForce::calcSlidingForceMagnitude(const State& state) const
+{ return getImpl().calcSlidingForceMagnitude(state); }
+Vec2 MySlidingFrictionForce::calcSlidingForce(const State& state) const
+{ return getImpl().calcSlidingForce(state); }
+
+
+const MySlidingFrictionForceImpl& MySlidingFrictionForce::
+getImpl() const {
+ return dynamic_cast<const MySlidingFrictionForceImpl&>(getImplementation());
+}
diff --git a/Simbody/tests/adhoc/WristMomentArm.cpp b/Simbody/tests/adhoc/WristMomentArm.cpp
new file mode 100644
index 0000000..3552744
--- /dev/null
+++ b/Simbody/tests/adhoc/WristMomentArm.cpp
@@ -0,0 +1,908 @@
+/* -------------------------------------------------------------------------- *
+ * 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) 2010-12 Stanford University and the Authors. *
+ * Authors: Michael Sherman *
+ * Contributors: *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+/**@file
+ * Adhoc main program for playing with wrist-like moment arm problem.
+ **/
+
+/*
+ * q0 q1 q2 x
+ * |-----------| |-------| ^
+ * | Forearm | * aux1 * aux2 * | Hand | |
+ * | ===== | | | ===== | y <----* z
+ * |-----------|<..........O....>|-------|
+ * muscle & tendon
+ * length l(q)
+ *
+ * aux1,2 are small bones and "*" is a rotational dof "|" is a sliding dof.
+ * The "O" is a circular wrap surface attached to a body that is on a slider
+ * connected to aux2, where the sliding dof is coupled to one of the
+ * rotational dofs and affects the length of the muscle as it passes
+ * frictionlessly over the wrap circle. There are coupler constraints
+ * connecting all the dofs so that setting any q determines all the others
+ * and thus the muscle length; alternatively we can turn off one of the
+ * coupler constraints and use a contact constraint to remove a dof from
+ * the hand, coupling all the q's indirectly.
+ *
+ * We would like a good
+ * instantaneous calculation of the moment arm r(q) defined as r=dl/dtheta
+ * where l(q) is muscle length and theta is the angle between a line fixed in
+ * the hand (=====) and a line fixed in the forearm. Theta is also the sum
+ * of the three rotational joint angles; we're assuming the q's are
+ * scaled angles such that angle a0=q0/s0, a1=q1/s1, and a2=q2/s2 so we
+ * have theta=q0/s0+q1/s1+q2/s2. In OpenSim it is common to scale the
+ * generalized coordinate of the "independent" angle so that its numerical
+ * value is the total angle theta.
+ *
+ * We can calculate r=ldot/thetadot and if all
+ * velocities are zero we can also calculate r=ldotdot/thetadotdot.
+ * Unfortunately, in practice it is difficult to determine ldot and ldotdot (unless
+ * the muscle is a straight line; i.e., no wrap surface), otherwise
+ * this would be an easy solution: apply a muscle tension t along the
+ * muscle path, measure the resulting ldotdot and thetadotdot and calculate
+ * r directly. But we can get the same result with the following method:
+ *
+ * 1. Determine the coupling matrix C such that qdot=C * thetadot.
+ * 2. Calculate f(t) the generalized forces due to an applied muscle tension t
+ * 3. Calculate r=~C*f/t.
+ *
+ * See the document "Moment arm revisited", Sherm, 10/12/2010 for theory.
+ *
+ * Below we will use this method and compare it with a direct calculation from
+ * the definition r=dl/dtheta done by perturbation.
+ */
+
+#include "SimTKsimbody.h"
+
+#include <cstdio>
+#include <exception>
+#include <algorithm>
+#include <iostream>
+#include <ctime>
+using std::cout; using std::endl;
+
+using namespace SimTK;
+
+Array_<State> saveEm;
+
+static const Real ReportInterval = 0.5;
+
+// Utility for analytically calculating the two tangent points of a
+// line from point p to a circle of radius r, center c. Note that this
+// is limited to a coplanar point and circle.
+static bool find_tangent_points(const Vec2& c, Real r, const Vec2& p,
+ Vec2& p0, Vec2& p1);
+// Given chord length h in a circle of radius r, return the corresponding
+// arc length, going from one end of the chord the short way around to
+// the other end.
+static Real calc_arc_length(Real r, Real h);
+
+
+
+//==============================================================================
+// DEFINE THE MUSCLE
+//==============================================================================
+
+// This helper class holds cached path geometry calculations done by the
+// muscle below.
+class PathInfo {
+public:
+ PathInfo()
+ : p1(NaN), p2(NaN), pathLengthA(NaN), arcLength(NaN), pathLengthB(NaN) {}
+ Vec2 p1, p2; // tangent points on body C, in G
+ Real pathLengthA, arcLength, pathLengthB;
+};
+
+std::ostream& operator<<(std::ostream& o, const PathInfo& pi) {
+ o << "PathInfo: p1=" << pi.p1 << " p2=" << pi.p2 << std::endl;
+ return o;
+}
+
+// This is a model of a muscle with origin/insertion points on bodies A and B,
+// using a planar path that passes frictionlessly over one side of a wrapping
+// circle of radius r on body W.
+//
+// The muscle defines a scalar discrete state variable to hold the current
+// tension value t that is used to generate the muscle forces F=T(q)*t.
+// T is a force transmission matrix that we don't know explicitly.
+class PlanarMuscle : public Force::Custom::Implementation {
+public:
+ PlanarMuscle(const GeneralForceSubsystem& forces,
+ const SimbodyMatterSubsystem& matter,
+ const MobilizedBody& A, const Vec3& ptA,
+ const MobilizedBody& B, const Vec3& ptB,
+ const MobilizedBody& W, const Vec3& center,
+ Real radius, const UnitVec3& side)
+ : m_forces(forces), m_matter(matter),
+ m_A(A), m_ptA(ptA), m_B(B), m_ptB(ptB), // origin/insertion points
+ m_W(W), m_center(center), m_radius(radius), // wrapping circle
+ m_side(side) {} // which side of the circle to wrap around (in W frame)
+
+
+ // Specialized interface for PlanarMuscle.
+
+ // Set the muscle tension value.
+ void setTension(State& state, Real tension) const
+ { Value<Real>::updDowncast
+ (m_forces.updDiscreteVariable(state,m_tensionVarIx)) = tension; }
+ // Get the current muscle tension setting.
+ Real getTension(const State& state) const
+ { return Value<Real>::downcast
+ (m_forces.getDiscreteVariable(state,m_tensionVarIx)); }
+
+ // Cacluate where the two insertion points are in the Ground frame.
+ void findInsertionPoints(const State& state, Vec3& pA, Vec3& pB) const {
+ const MobilizedBody& A = m_matter.getMobilizedBody(m_A);
+ const MobilizedBody& B = m_matter.getMobilizedBody(m_B);
+
+ pA = A.findStationLocationInGround(state, m_ptA);
+ pB = B.findStationLocationInGround(state, m_ptB);
+ }
+
+ // These retrieve items from the PathInfo cache entry that is set
+ // in realizePosition() below, so these can't be called until Position
+ // stage has been realized in the supplied State.
+
+ Real getPathLength(const State& state) const {
+ const PathInfo& info = getPathInfo(state);
+ return info.pathLengthA + info.arcLength + info.pathLengthB;
+ }
+ Real getArcLength(const State& state) const {
+ const PathInfo& info = getPathInfo(state);
+ return info.arcLength;
+ }
+ void getTangentPoints(const State& state, Vec3& pAC, Vec3& pBC) const {
+ const PathInfo& info = getPathInfo(state);
+ pAC = info.p1.append1(0);
+ pBC = info.p2.append1(0);
+ }
+
+ // Satisfy the virtual methods of Force::Custom::Implementation.
+
+ // Calculate the muscle forces and accumulate into bodyForces array.
+ // (We aren't going to generate any particle or mobility forces.)
+ virtual void calcForce(const State& state,
+ Vector_<SpatialVec>& bodyForces,
+ Vector_<Vec3>& particleForces,
+ Vector& mobilityForces) const
+ {
+ const MobilizedBody& A = m_matter.getMobilizedBody(m_A);
+ const MobilizedBody& B = m_matter.getMobilizedBody(m_B);
+ const MobilizedBody& W = m_matter.getMobilizedBody(m_W);
+
+ // Insertion points in Ground.
+ const Vec3 iptA = A.findStationLocationInGround(state, m_ptA);
+ const Vec3 iptB = B.findStationLocationInGround(state, m_ptB);
+ // Wrap circle center in Ground.
+ const Vec3 ctr = W.findStationLocationInGround(state, m_center);
+
+ // Tangent points in Ground.
+ Vec3 tptA, tptB;
+ getTangentPoints(state, tptA, tptB);
+ const Vec3 tptA_W = W.findStationAtGroundPoint(state, tptA);
+ const Vec3 tptB_W = W.findStationAtGroundPoint(state, tptB);
+
+ UnitVec3 a2w(tptA - iptA); // from insertion to tangent point
+ UnitVec3 b2w(tptB - iptB); // "
+ UnitVec3 nta( ctr - tptA); // inward normal at A's tangent pt
+ UnitVec3 ntb( ctr - tptB); // " B's "
+
+ const Real tension = getTension(state);
+
+ const Vec3 fA = tension*a2w;
+ const Vec3 fB = tension*b2w;
+
+ A.applyForceToBodyPoint(state, m_ptA, fA, bodyForces);
+ B.applyForceToBodyPoint(state, m_ptB, fB, bodyForces);
+ W.applyForceToBodyPoint(state, m_center, -(fA+fB), bodyForces);
+ }
+
+ // This muscle model doesn't store energy.
+ virtual Real calcPotentialEnergy(const State&) const {return 0;}
+
+ // Allocate variables and cache entries.
+ virtual void realizeTopology(State& state) const {
+ PlanarMuscle* mThis = const_cast<PlanarMuscle*>(this);
+ mThis->m_tensionVarIx = m_forces
+ .allocateDiscreteVariable(state, Stage::Dynamics,
+ new Value<Real>(0));
+
+ mThis->m_pathInfoIx = m_forces
+ .allocateCacheEntry(state, Stage::Position,
+ new Value<PathInfo>());
+ }
+
+ // Calculate path geometry information.
+ virtual void realizePosition(const State& state) const {
+ // Get all the relevant points in Ground.
+ const MobilizedBody& A = m_matter.getMobilizedBody(m_A);
+ const MobilizedBody& B = m_matter.getMobilizedBody(m_B);
+ const MobilizedBody& W = m_matter.getMobilizedBody(m_W);
+
+ // Only works in x-y plane (we're dropping the z coordinate).
+ const Vec2 ptA = A.findStationLocationInGround(state, m_ptA).drop1(2);
+ const Vec2 ptB = B.findStationLocationInGround(state, m_ptB).drop1(2);
+ const Vec2 ctr = W.findStationLocationInGround(state, m_center).drop1(2);
+ const Vec2 side = W.expressVectorInGroundFrame(state, m_side).drop1(2);
+ Vec2 p0,p1,a,b;
+ find_tangent_points(ctr,m_radius,ptA,p0,p1);
+ a = dot(p0-ctr,side) > dot(p1-ctr,side) ? p0 : p1;
+ find_tangent_points(ctr,m_radius,ptB,p0,p1);
+ b = dot(p0-ctr,side) > dot(p1-ctr,side) ? p0 : p1;
+
+ PathInfo& info = updPathInfo(state);
+ info.p1 = a; info.p2 = b;
+ info.pathLengthA = (a-ptA).norm();
+ info.pathLengthB = (b-ptB).norm();
+ info.arcLength = calc_arc_length(m_radius, (a-b).norm());
+ }
+
+private:
+ const PathInfo& getPathInfo(const State& state) const
+ { return Value<PathInfo>::downcast
+ (m_forces.getCacheEntry(state,m_pathInfoIx)); }
+ PathInfo& updPathInfo(const State& state) const
+ { return Value<PathInfo>::updDowncast
+ (m_forces.updCacheEntry(state,m_pathInfoIx)); }
+
+ const GeneralForceSubsystem& m_forces;
+ const SimbodyMatterSubsystem& m_matter;
+ const MobilizedBodyIndex m_A, m_B, m_W;
+ const Vec3 m_ptA, m_ptB, m_center;
+ const Real m_radius;
+ const UnitVec3 m_side;
+ DiscreteVariableIndex m_tensionVarIx;
+ CacheEntryIndex m_pathInfoIx;
+};
+
+
+
+//==============================================================================
+// DEFINE A CUSTOM MOBILIZER
+//==============================================================================
+// This is a pin joint but with a generalized coordinate
+// q0=scale*theta0
+// where theta0 is its actual angle. We'll use a coupler to make the
+// other pin joints have angles q1=theta0, q2=theta0 so that the
+// total angle theta0+theta1+theta2=q0.
+class ScaledPin : public MobilizedBody::FunctionBased {
+public:
+ ScaledPin(MobilizedBody& parent, const Transform& inbFrameF,
+ const Body& body, const Transform& outbFrameM,
+ Real scale)
+ : FunctionBased(parent,inbFrameF,body,outbFrameM,1,
+ makeFunctions(scale), Array_<Array_<int> >(6, Array_<int>(1,0))),
+ m_scale(scale)
+ {}
+
+ Real getScale() const {return m_scale;}
+
+ void setQ(State& s, Real q) const {setOneQ(s,MobilizerQIndex(0),q);}
+ void setU(State& s, Real u) const {setOneU(s,MobilizerUIndex(0),u);}
+ Real getQ(const State& s) const {return getOneQ(s,MobilizerQIndex(0));}
+ Real getU(const State& s) const {return getOneU(s,MobilizerQIndex(0));}
+
+ void setAngle(State& s, Real angle) const
+ { setOneQ(s, MobilizerQIndex(0), m_scale*angle); }
+ Real getAngle(const State& s) const
+ { return getOneQ(s, MobilizerQIndex(0)) / m_scale; }
+
+ void setAngularRate(State& s, Real angularRate) const
+ { setOneU(s, MobilizerQIndex(0), m_scale*angularRate); }
+ Real getAngularRate(const State& s) const
+ { return getOneU(s, MobilizerQIndex(0)) / m_scale; }
+
+private:
+ Array_<const Function*> makeFunctions(Real scale) {
+ Array_<const Function*> funcs;
+ funcs.push_back(new Function::Constant(0)); // x rotation
+ funcs.push_back(new Function::Constant(0)); // y rotation
+ funcs.push_back(new Function::Linear
+ (Vector(Vec2(1/scale,0)))); // z rotation
+ funcs.push_back(new Function::Constant(0)); // x translation
+ funcs.push_back(new Function::Constant(0)); // y translation
+ funcs.push_back(new Function::Constant(0)); // z translation
+ return funcs;
+ }
+
+ const Real m_scale;
+};
+
+//==============================================================================
+// DEFINE REPORTER
+//==============================================================================
+// This reporter both prints to the console and drives the visualizer. We have
+// to add the path lines to each frame here since the tangent end points move.
+class MyReporter : public PeriodicEventReporter {
+public:
+ MyReporter(const MultibodySystem& system,
+ Visualizer& viz,
+ const PlanarMuscle& planarMuscle,
+ Real reportInterval)
+ : PeriodicEventReporter(reportInterval), m_system(system), m_viz(viz),
+ m_planarMuscle(planarMuscle) {}
+
+ ~MyReporter() {}
+
+ // This is used by handleEvent() below but can also be called directly
+ // to generate a frame from saved states for replay.
+ void report(const State& state) const {
+ m_system.realize(state, Stage::Position);
+ m_viz.report(state);
+ cout << "t=" << state.getTime()
+ << " path length=" << m_planarMuscle.getPathLength(state) << endl;
+ }
+
+ // Supply the required virtual method.
+ virtual void handleEvent(const State& state) const {
+ report(state);
+ saveEm.push_back(state);
+ }
+private:
+ const MultibodySystem& m_system;
+ Visualizer& m_viz;
+ const PlanarMuscle& m_planarMuscle;
+};
+
+
+
+//==============================================================================
+// DRAW PATH LINES
+//==============================================================================
+// We have to add the path lines to each frame here since the tangent end points
+// move.
+class DrawPathLines : public DecorationGenerator {
+public:
+ DrawPathLines(const MultibodySystem& system,
+ const PlanarMuscle& muscle)
+ : m_system(system), m_planarMuscle(muscle) {}
+
+ virtual void generateDecorations(const State& state,
+ Array_<DecorativeGeometry>& geometry)
+ {
+ m_system.realize(state, Stage::Position);
+ Vec3 iptA, iptB, tptA, tptB;
+ m_planarMuscle.findInsertionPoints(state, iptA, iptB);
+ m_planarMuscle.getTangentPoints(state, tptA, tptB);
+ geometry.push_back(DecorativeLine(iptA, tptA).setColor(Red));
+ geometry.push_back(DecorativeLine(iptB, tptB).setColor(Red));
+ }
+private:
+ const MultibodySystem& m_system;
+ const PlanarMuscle& m_planarMuscle;
+};
+
+
+
+//==============================================================================
+// MAIN
+//==============================================================================
+int main() {
+
+ try
+ { // Create the system.
+
+ MultibodySystem system;
+ SimbodyMatterSubsystem matter(system);
+ GeneralForceSubsystem forces(system);
+ //Force::UniformGravity gravity(forces, matter, 1*Vec3(2, -9.8, 0));
+
+ const Real forearmMass = 11;
+ const Real handMass = 3;
+ const Real auxMass = .001;
+ const Real wrapMass = .1;
+ const Vec3 forearmHDim(1,3,.25);
+ const Vec3 handHDim(1,1,.25);
+ const Vec3 auxHDim(.1, .1, .05);
+ const Real wrapRadius = .25; // circle in x,y
+ const Real wrapHThick = .1; // z half-depth
+
+ const Vec3 c(2,3,5); // constrained angle ratios q0:q1:q2
+ const Real csum = sum(c);
+ const Vec4 scale(csum/c[0],csum/c[1],csum/c[2],1); // scaling
+ cout << "scaling=" << scale << endl;
+
+ // Stiffnesses
+ const Real k = 1000;
+
+ // Input tension
+ const Real tension = 2;
+
+ // Global damper.
+ const Real damping = 100;
+
+ // Muscle attachment points
+ const Vec3 forearmAttach(forearmHDim[0]/2,-forearmHDim[1],0);
+ const Vec3 handAttach(handHDim[0], handHDim[1], 0);
+
+ const DecorativeGeometry forearmViz = DecorativeBrick(forearmHDim).setColor(Green);
+ const DecorativeGeometry handViz = DecorativeBrick(handHDim).setColor(Orange);
+ const DecorativeGeometry auxViz = DecorativeBrick(auxHDim).setColor(Blue);
+ const DecorativeGeometry wrapViz = DecorativeCylinder(wrapRadius,wrapHThick)
+ .setTransform(Rotation(Pi/2, XAxis)).setColor(Purple).setResolution(10);
+
+ Body::Rigid forearmBody(MassProperties(forearmMass, Vec3(0),
+ forearmMass*UnitInertia::brick(forearmHDim)));
+ forearmBody.addDecoration(Vec3(0), forearmViz);
+
+ Body::Rigid handBody(MassProperties(handMass, Vec3(0),
+ handMass*UnitInertia::brick(handHDim)));
+ handBody.addDecoration(Vec3(0), handViz);
+
+ Body::Rigid auxBody(MassProperties(auxMass, Vec3(0),
+ auxMass*UnitInertia::brick(auxHDim)));
+ auxBody.addDecoration(Vec3(0), auxViz);
+
+ Body::Rigid wrapBody(MassProperties(wrapMass, Vec3(0),
+ wrapMass*UnitInertia::cylinderAlongZ(wrapRadius, wrapHThick)));
+ wrapBody.addDecoration(Vec3(0), wrapViz);
+
+ MobilizedBody::Weld forearm(matter.Ground(), Vec3(forearmHDim[0], forearmHDim[1], 0),
+ forearmBody, Vec3(0));
+
+
+ ScaledPin aux1(forearm, Vec3(-forearmHDim[0], -forearmHDim[1]-.5, 0),
+ auxBody, Vec3(0, auxHDim[1], 0), scale[0]);
+
+ ScaledPin aux2(aux1, Vec3(0, -auxHDim[1]-.1, 0),
+ auxBody, Vec3(0, auxHDim[1]+.3, 0), scale[1]);
+
+ ScaledPin hand(aux2, Vec3(0, -auxHDim[1], 0),
+ handBody, Vec3(-handHDim[0], handHDim[1]+.5, 0), scale[2]);
+
+ MobilizedBody::Slider wrap(aux2, Vec3(1.5,0,0),
+ wrapBody, Vec3(0));
+
+ // Spring
+ Force::MobilityLinearSpring aux1Spring(forces, aux1,
+ MobilizerUIndex(0), k, 0);
+
+ // Straight-line muscle with no wrapping.
+ //Force::TwoPointConstantForce muscle(forces, forearm, forearmAttach,
+ // hand, handAttach, -tension);
+
+ PlanarMuscle& planarMuscle = *new PlanarMuscle(forces, matter,
+ forearm, forearmAttach,
+ hand, handAttach,
+ wrap, Vec3(0), wrapRadius, UnitVec3(-1,0,0));
+ Force::Custom muscle(forces, &planarMuscle);
+
+ // Energy sucker
+ Force::GlobalDamper damper(forces, matter, damping);
+ damper.setDisabledByDefault(true);
+
+ // Angles are ai=q[i]/s[i]. Then constraints are
+ // a0/c0 - a1/c1 = 0 => q[0]/(s0*c0) - q[1]/(s1*c1) = 0
+ // a0/c0 - a2/c2 = 0 => q[0]/(s0*c0) - q[2]/(s2*c2) = 0
+ const Vec2 ratios1(1/(scale[0]*c[0]), -1/(scale[1]*c[1]));
+ const Vec2 ratios2(1/(scale[0]*c[0]), -1/(scale[2]*c[2]));
+ Array_<MobilizedBodyIndex> aux1aux2, aux1Hand, aux1Wrap;
+ aux1aux2.push_back(MobilizedBodyIndex(aux1));
+ aux1aux2.push_back(MobilizedBodyIndex(aux2));
+ aux1Hand.push_back(MobilizedBodyIndex(aux1));
+ aux1Hand.push_back(MobilizedBodyIndex(hand));
+ aux1Wrap.push_back(MobilizedBodyIndex(aux1));
+ aux1Wrap.push_back(MobilizedBodyIndex(wrap));
+ Array_<MobilizerUIndex> whichUs(2, MobilizerUIndex(0));
+
+ Constraint::CoordinateCoupler
+ aux1toAux2(matter,
+ new Function::Linear(Vector(Vec3(ratios1[0],ratios1[1],0))),
+ aux1aux2, whichUs);
+ aux1toAux2.setDisabledByDefault(true);
+
+ Constraint::CoordinateCoupler
+ aux1toHand(matter,
+ new Function::Linear(Vector(Vec3(ratios2[0],ratios2[1],0))),
+ aux1Hand, whichUs);
+ aux1toHand.setDisabledByDefault(true);
+
+ // Coupling ratio to the wrap surface's body is hardcoded to
+ // an arbitrary value here tied to q[0] rather than the angle.
+ Constraint::CoordinateCoupler
+ aux1toWrap(matter,
+ new Function::Linear(Vector(Vec3(1, -.4, 0))),
+ aux1Wrap, whichUs);
+
+ // Enable this to make a point of the hand touch a plane fixed on ground.
+ // We'll optionally use this constraint instead of one of the couplers
+ // to verify that moment arm works either way.
+ Constraint::PointInPlane inPlane(matter.Ground(), UnitVec3(-1,0,0), -2.25,
+ hand, Vec3(1,-1,0));
+ inPlane.setDisabledByDefault(true);
+
+
+ Visualizer viz(system);
+ viz.setBackgroundType(Visualizer::SolidColor);
+
+ // This draws the straight-line muscle (regardless of whether we're
+ // using it).
+ viz.addRubberBandLine(forearm, forearmAttach, hand, handAttach,
+ DecorativeLine().setColor(Red).setLineThickness(2));
+
+ // Add generator for muscle path lines.
+ viz.addDecorationGenerator(new DrawPathLines(system,planarMuscle));
+
+ MyReporter& myRep = *new MyReporter(system,viz,planarMuscle,ReportInterval);
+ system.addEventReporter(&myRep);
+
+ // Initialize the system and state.
+ State state = system.realizeTopology();
+
+ aux1.setAngle(state,0);
+ aux2.setAngle(state,0);
+ hand.setAngle(state,0);
+ planarMuscle.setTension(state, 0);
+
+ cout << "Muscle tension = " << planarMuscle.getTension(state) << endl;
+
+ myRep.report(state);
+ printf("Initial state -- hit ENTER\n");
+ char ch=getchar();
+
+ //ExplicitEulerIntegrator integ(system);
+ CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
+ //RungeKuttaFeldbergIntegrator integ(system);
+ //RungeKuttaMersonIntegrator integ(system);
+ //RungeKutta3Integrator integ(system);
+ //VerletIntegrator integ(system);
+ //integ.setMaximumStepSize(1e-0001);
+ integ.setAccuracy(1e-6);
+ TimeStepper ts(system, integ);
+
+ damper.enable(state);
+
+#define USE_CONTACT_CONSTRAINT
+#ifdef USE_CONTACT_CONSTRAINT
+ inPlane.enable(state);
+#else
+ aux1toAux2.enable(state);
+#endif
+ aux1toHand.enable(state);
+ system.realize(state, Stage::Instance);
+
+ state.updU() = 0;
+ state.setTime(0);
+ ts.initialize(state);
+ ts.stepTo(40.0);
+
+ state = ts.getState();
+ system.realize(state, Stage::Velocity);
+ const Real pathLength0 = planarMuscle.getPathLength(state);
+ const Real length0 = forearm.calcStationToStationDistance(state, forearmAttach, hand, handAttach);
+ const Vector q0 = state.getQ(); // equilibrium q's
+
+ myRep.report(state);
+ printf("Muscle off ke=%g -- hit ENTER\n", system.calcKineticEnergy(state));
+ cout << "pathLength0=" << pathLength0 << " length0=" << length0 << " q0=" << q0 << endl;
+ ch=getchar();
+
+ //----------------------------
+ // CALCULATE COUPLING MATRIX C
+ //----------------------------
+
+ state.updU() = 0;
+ aux1.setU(state, 1);
+ system.realize(state, Stage::Velocity);
+ cout << "before project u=" << state.getU() << endl;
+
+ system.projectU(state, 1e-10);
+ cout << "after project u=" << state.getU() << " uerr=" << state.getUErr() << endl;
+ // To compute thetadot, convert u's to angular rates and add.
+ const Real thetaDot = aux1.getAngularRate(state)
+ + aux2.getAngularRate(state)
+ + hand.getAngularRate(state);
+ cout << "calculated thetadot=" << thetaDot << endl;
+ const Vector calcc(state.getU()/thetaDot);
+
+ const Real calccsum = sum(calcc);
+ cout << "calc c=" << calcc << " csum=" << calccsum << endl;
+ state.updU() = 0;
+
+ // None of the acceleration- and multiplier-dependent stuff here
+ // matters for moment arm; this is just for playing around with
+ // related dynamic quantities. Feel free to ignore.
+
+ system.realize(state, Stage::Acceleration);
+ // Calculate the joint torques f0 equivalent to the equilibrium forces.
+ Vector f0;
+ matter.multiplyBySystemJacobianTranspose(state,
+ system.getRigidBodyForces(state,Stage::Dynamics),
+ f0);
+ f0 += system.getMobilityForces(state,Stage::Dynamics);
+
+ // This is how you find the scaling if you need it; we don't require
+ // this knowledge to calculate moment arm since it is implicitly
+ // embedded in the coupling matrix C.
+ const SpatialVec H_PB_G = aux1.getHCol(state, MobilizerUIndex(0));
+ const SpatialVec H_FM = aux1.getH_FMCol(state, MobilizerUIndex(0));
+ cout << "H_PB_G=" << H_PB_G << " -> angle scale " << H_PB_G[0].norm() << endl;
+ cout << "H_FM=" << H_FM << " -> angle scale " << H_FM[0].norm() << endl;
+
+ const Vector udot0 = state.getUDot();
+ const Vector lambda0 = state.getMultipliers();
+ Matrix Gt;
+ matter.calcGTranspose(state, Gt);
+ cout << "Gt*l0=" << Gt*lambda0 << endl;
+ Vector f0l = f0 - Gt*lambda0;
+
+ cout << "udot0=" << udot0 << endl;
+ cout << "lambda0=" << lambda0 << endl;
+ cout << "f0=" << f0 << endl;
+ cout << "f0l=" << f0l << endl;
+
+ const Real d2l0 = -forearm.calcStationToStationDistance2ndTimeDerivative
+ (state, forearmAttach, hand, handAttach);
+
+ // Fire the muscle.
+ planarMuscle.setTension(state, tension);
+ cout << "Muscle tension = " << planarMuscle.getTension(state) << endl;
+
+ // See note above regarding irrelevance of acceleration-related
+ // computations for moment arm.
+ system.realize(state, Stage::Acceleration);
+
+ const Vector udot1 = state.getUDot();
+ const Vector lambda1 = state.getMultipliers();
+ cout << "udot1=" << udot1 << endl;
+ cout << "lambda1=" << lambda1 << endl;
+ const Vector dudot = udot1 - udot0;
+ cout << "dudot=" << dudot << endl;
+ const Real d2l = -forearm.calcStationToStationDistance2ndTimeDerivative
+ (state, forearmAttach, hand, handAttach);
+ const Real dd2l = d2l - d2l0;
+ cout << "dd2l=" << dd2l << endl;
+ Vector momentArm = dd2l * dudot.elementwiseInvert();
+ cout << "dd2l ./ dudot=" << momentArm << endl;
+ Vector dudotAdj(dudot);
+ dudotAdj.elementwiseDivideInPlace(Vector(scale)); // fix units so we have angles
+ cout << "dudotAdj=" << dudotAdj << endl;
+ cout << "dd2l / sum(dudotAdj) = " << dd2l / sum(dudotAdj) << endl;
+
+ // Calculate the joint torques f equivalent to the muscle forces F.
+ Vector f1;
+ matter.multiplyBySystemJacobianTranspose(state,
+ system.getRigidBodyForces(state,Stage::Dynamics),
+ f1);
+ f1 += system.getMobilityForces(state,Stage::Dynamics);
+
+ matter.calcGTranspose(state, Gt);
+ cout << "Gt*l1=" << Gt*lambda1 << endl;
+ Vector f1l = f1 - Gt*lambda1;
+
+ Vector f = f1-f0, fl=f1l-f0l;
+ cout << "f/tension=" << f/tension << " ~C*f/tension="
+ << (~calcc*f)/tension << endl;
+ cout << "fl/tension=" << fl/tension << " ~C*fl/tension="
+ << (~calcc*fl)/tension << endl;
+
+ Assembler asmb(system);
+ asmb.setAccuracy(1e-10);
+ Real tol = asmb.assemble(state);
+ system.realize(state, Stage::Position);
+ Real startAngle = aux1.getAngle(state)
+ + aux2.getAngle(state)
+ + hand.getAngle(state);
+ const Real startLength = planarMuscle.getPathLength(state);
+ cout << "ASSEMBLED to tol=" << tol
+ << " startAngle=" << startAngle
+ << " startLength=" << startLength << endl;
+
+ aux1.setQ(state, aux1.getQ(state) + 1e-6); // perturb
+ asmb.lockMobilizer(aux1);
+ tol = asmb.assemble(state);
+ system.realize(state, Stage::Position);
+ Real endAngle = aux1.getAngle(state)
+ + aux2.getAngle(state)
+ + hand.getAngle(state);
+ const Real endLength = planarMuscle.getPathLength(state);
+ cout << "ASSEMBLED to tol=" << tol
+ << " endAngle=" << endAngle
+ << " endLength=" << endLength << endl;
+ cout << "r = " << (endLength-startLength)/(endAngle-startAngle) << endl;
+
+ // Here's another way to do it -- starting at equilibrium, apply a
+ // tension to the muscle and then find the new equilibrium (actually
+ // any change in configuration will work). Then calculated dl and dtheta.
+
+
+ // Simulate it.
+ saveEm.clear(); // Forget the saved trajectory from the equilibrium run above
+ integ.setAccuracy(1e-6);
+ muscle.enable(state);
+ state.updQ() = q0; // back to equilibrium
+ state.updU() = 0;
+ state.setTime(0);
+ ts.initialize(state);
+ cout << "Initialized velocity=" << ts.getState().getU() << endl;
+ const Real angle0 = hand.getBodyRotation(ts.getState()).convertOneAxisRotationToOneAngle( ZAxis );
+ ts.stepTo(40.0);
+
+ state = ts.getState();
+ system.realize(state, Stage::Velocity);
+ const Real length1 = forearm.calcStationToStationDistance(state, forearmAttach, hand, handAttach);
+ const Real pathLength1 = planarMuscle.getPathLength(state);
+
+ const Vector q1 = state.getQ();
+
+ myRep.report(state);
+ printf("Tension %g; final equilibrium ke=%g -- hit ENTER\n",
+ tension, system.calcKineticEnergy(state));
+ cout << "length1=" << length1 << " q1=" << q1 << endl;
+ cout << "pathLength1=" << pathLength1 << endl;
+ const Real dl=-(length1-length0);
+ const Real dpl=-(pathLength1-pathLength0);
+ const Vector dtheta=q1-q0;
+ cout << "dl=" << dl << " dpl=" << dpl << " dtheta=" << dtheta << endl;
+ momentArm = dl * dtheta.elementwiseInvert();
+ cout << "dl ./ dtheta=" << momentArm << endl;
+ cout << "dpl ./ dtheta=" << dpl * dtheta.elementwiseInvert() << endl;
+ Vector dthetaAdj(dtheta);
+ dthetaAdj.elementwiseDivideInPlace(Vector(scale)); // convert to angles
+ cout << "sum of 3 joint angles=" << sum(dthetaAdj(0,3)) << " angles=" << dthetaAdj << endl;
+ const Real angle1 = hand.getBodyRotation(state).convertOneAxisRotationToOneAngle( ZAxis );
+ cout << "angle0=" << angle0 << " angle1=" << angle1 << endl;
+ cout << "calculated dangle=" << angle1-angle0 << endl;
+ cout << " dl/dangle=" << dl / (angle1-angle0) << endl;
+ cout << " dpl/dangle=" << dpl / (angle1-angle0) << endl;
+ system.realize(state, Stage::Acceleration);
+ cout << "perr=" << state.getQErr() << endl;
+ cout << "verr=" << state.getUErr() << endl;
+ cout << "aerr=" << state.getUDotErr() << endl;
+ ch=getchar();
+
+ while(true) {
+ for (int i=0; i < (int)saveEm.size(); ++i) {
+ myRep.report(saveEm[i]);
+ }
+ getchar();
+ }
+
+ } catch (const std::exception& e) {
+ std::printf("EXCEPTION THROWN: %s\n", e.what());
+ exit(1);
+
+ } catch (...) {
+ std::printf("UNKNOWN EXCEPTION THROWN\n");
+ exit(1);
+ }
+
+ return 0;
+}
+
+//==============================================================================
+// PATH GEOMETRY UTILITIES
+//==============================================================================
+// circle_circle_intersection()
+// Determine the two points where 2 circles in a common plane intersect. Returns
+// false quickly if they don't intersect at all or if they are exactly coincident.
+//
+// Method due to Paul Bourke, 1997.
+// http://local.wasp.uwa.edu.au/~pbourke/geometry/2circle/
+//
+// Code adapted from Tim Voght's public domain code linked to be the above
+// article, dated 3/26/2005.
+//
+static bool circle_circle_intersection( const Vec2& c0, Real r0,
+ const Vec2& c1, Real r1,
+ Vec2& p0, Vec2& p1)
+{
+ const Vec2 c2c = c1 - c0;
+ const Real d2 = c2c.normSqr();
+
+ // Check for solvability.
+ if (d2 > square(r0 + r1)) {
+ // no solution. circles do not intersect.
+ p0 = p1 = NaN;
+ return false;
+ }
+ if (d2 < square(r0 - r1)) {
+ // no solution. one circle is contained in the other
+ p0 = p1 = NaN;
+ return false;
+ }
+ if (d2 == 0) {
+ // circles must be coincident and intersect at every point;
+ // treat as unsolvable
+ p0 = p1 = NaN;
+ return false;
+ }
+
+ const Real d = std::sqrt(d2); // distance between the centers
+ const Real ood = 1/d; // compute once
+
+ // 'point 2' is the point where the line through the circle
+ // intersection points crosses the line between the circle
+ // centers.
+
+ // Determine the distance from point 0 to point 2.
+ const Real a = (r0*r0 - r1*r1 + d2) * ood / 2;
+
+ // Determine the coordinates of point 2.
+ const Vec2 p2 = c0 + c2c*a*ood;
+
+ // Determine the distance from point 2 to the intersection points.
+ const Real h = std::sqrt(r0*r0 - a*a);
+ // Now determine the offsets of the intersection points from
+ // point 2.
+ const Vec2 offs = Vec2(-c2c[1],c2c[0]) * h * ood;
+
+ // Determine the absolute intersection points.
+ p0 = p2 + offs;
+ p1 = p2 - offs;
+ return true;
+}
+
+// Find tangent points of line from point p to circle at c, radius r.
+// By Thales' theorem via Wikipedia, the two tangent points are the points of
+// intersection of circle c with a circle centered on the midpoint
+// of line cp and having cp as a diameter.
+static bool find_tangent_points(const Vec2& c, Real r, const Vec2& p,
+ Vec2& p0, Vec2& p1)
+{
+ const Vec2 cp = p - c; // vector from c to p
+ const Vec2 mid = (c+p)/2; // midpoint
+ return circle_circle_intersection(c,r, mid, cp.norm()/2, p0, p1);
+}
+
+// Given a circle of radius r and a chord length h(<= d=2r), calculate the arc length
+// s between them (going the shorter direction). We calculate the angle theta
+// subtended by the chord, then the arc length is r*theta.
+// r*sin(theta/2)=h/2 => theta/2 = asin(h/d)
+// s = r*theta = d*asin(h/d)
+static Real calc_arc_length(Real r, Real h)
+{
+ const Real d = 2*r;
+ assert(0 <= h && h <= d);
+ return d==0 ? 0 : d * std::asin(h/d);
+}
+
+#define TEST
+#ifdef TEST
+static void run_test(double x0, double y0, double r0, double x1, double y1, double r1)
+{ Vec2 p0, p1;
+ printf("x0=%g, y0=%g, r0=%g, x1=%g, y1=%g, r1=%g :\n", x0, y0, r0, x1, y1, r1);
+ circle_circle_intersection(Vec2(x0, y0), r0, Vec2(x1, y1), r1, p0, p1);
+ printf(" p0=%g,%g, p1=%g,%g\n", p0[0],p0[1], p1[0],p1[1]);
+}
+static void testmain() {
+ /* Add more! */
+ run_test(0,0,4,4,0,4);
+ run_test(0,0,4,-2,0,2); // 1 point (-4,0 twice)
+ run_test(0,0,4,-2,0,1.9999); // no points
+ run_test(0,0,4,-2,0,2.0001); // two points near -4,0
+ run_test(-1.0, -1.0, 1.5, 1.0, 1.0, 2.0);
+ run_test(1.0, -1.0, 1.5, -1.0, 1.0, 2.0);
+ run_test(-1.0, 1.0, 1.5, 1.0, -1.0, 2.0);
+ run_test(1.0, 1.0, 1.5, -1.0, -1.0, 2.0);
+
+ Vec2 p0,p1;
+ find_tangent_points(Vec2(0,0),4,Vec2(-4,0), p0,p1);
+ cout << "p0=" << p0 << " p1=" << p1 << endl; // 1 point (-4,0 twice)
+ find_tangent_points(Vec2(0,0),4,Vec2(-4*Sqrt2,0), p0,p1);
+ cout << "p0=" << p0 << " p1=" << p1 << endl; // -2 sqrt(2), +/- 2 sqrt(2)
+
+ cout << "s=" << calc_arc_length(4, (p1-p0).norm()) << " (2pi?)\n";
+
+}
+#endif
diff --git a/SimbodyAPI.html b/SimbodyAPI.html
new file mode 100644
index 0000000..d09ad64
--- /dev/null
+++ b/SimbodyAPI.html
@@ -0,0 +1,5 @@
+<!-- Forward browser to the Doxygen-generated starting page for the -->
+<!-- Simbody API reference documentation. -->
+<head><meta http-equiv=refresh
+ content="0; url=api/Simbody/html/index.html">
+</head>
diff --git a/SimbodyMainpage.h b/SimbodyMainpage.h
new file mode 100644
index 0000000..8012c44
--- /dev/null
+++ b/SimbodyMainpage.h
@@ -0,0 +1,190 @@
+#ifndef SimTK_SIMBODY_DOXYGEN_MAINPAGE_H_
+#define SimTK_SIMBODY_DOXYGEN_MAINPAGE_H_
+
+
+/** @file
+This "header" file is actually just the source for Simbody's Doxygen
+Mainpage, the first page that a user sees when entering the Doxygen-
+generated API documentation. This is not actually included as part of the
+Simbody source and it is not installed with Simbody. **/
+
+
+// This is so Doxygen can locate the symbols we mention.
+namespace SimTK {
+
+/** @mainpage Simbody&tm; API Reference Documentation
+
+<h2>Getting started with Simbody</h2>
+You are looking at the main page for the reference documentation of the Simbody
+API. This documentation was generated by Doxygen directly from the Simbody
+source code as annotated by the original programmers so it is both accurate and
+comprehensive. As you will quickly realize if you look around, this is a large
+system providing a great deal of functionality. It is, however, very easy to
+use if you know where to begin -- and this reference documentation is most
+definitely \e not the place to start if you want to learn how to use
+Simbody. Instead, start with the tutorials in the Simbody User's Guide that
+you can find at the Simbody project site https://simtk.org/home/simbody
+"Documents" tab. There is also a collection of working example programs that
+come with the Simbody installation (in the examples directory). Both source
+and ready-to-execute binaries of the examples are installed. If you have
+specific questions, use the Simbody help forum under the "Public Forums" tab.
+You can also file bug reports and make feature requests using the tools
+provided there.
+
+ at image html isaac.png "Our hero, Sir Isaac Newton"
+
+<h2>What is Simbody?</h2>
+Simbody is a high-performance, industrial-grade open source C++ library
+providing sophisticated treatment of articulated multibody systems with special
+attention to the needs of biomedical simulation. It is useful for predictive
+dynamic simulations of diverse biological systems such as neuromuscular
+biomechanical models (https://simtk.org/home/opensim) and coarse-grained
+biomolecular modeling (https://simtk.org/home/rnatoolbox). It is also well
+suited to related simulation domains such as robotic and avatar simulation and
+control, and provides real time capabilities that make it useful for
+interactive scientific simulations as well as virtual worlds and games.
+
+Simbody uses a minimal coordinate recursive formulation of the equations of
+motion, providing computation of system dynamics to machine precision with
+O(N) computational complexity for N interconnected bodies. Arbitrary adjoined
+constraints, contact modeling, and advanced numerical methods provide fast,
+robust simulation for any mechanical or biomechanical system from machines and
+vehicles to human skeletons and macromolecules.
+
+Simbody 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. Related tools include
+- OpenSim (http://opensim.stanford.edu and https://simtk.org/home/opensim),
+a freely available software system for building, exchanging, and analyzing
+musuloskeletal models and dynamic simulations of movement. Includes both a
+graphical user interface and an API; automatically constructs Simbody
+models from high-level description.
+- Molmodel (https://simtk.org/home/molmodel), a C++ API for constructing
+coarse-grained, Simbody-based internal coordinate models of macromolecules
+like proteins and RNA, and
+- OpenMM (https://simtk.org/home/openmm), a GPU-accelerated high-performance
+numerical library for efficient calculation of molecular force fields.
+OpenMM may be used standalone or through Molmodel.
+
+Simbody is hosted on the simtk.org biosimulation community site,
+at https://simtk.org/home/simbody and is a community resource available for
+unrestricted academic, commercial, government, and personal use. The source
+code is maintained on GitHub, at https://github.com/simbody.
+
+Simbody was conceived and initially implemented by Michael Sherman (Sherm),
+with substantial contributions from Peter Eastman, both of the Simbios Center
+at Stanford. Many others have contributed to the software, support, and
+documentation and we invite well-engineered community contributions.
+
+<h2>How to use this documentation</h2>
+If you know the name of the class, method, or other symbol for which you want
+information, you can enter it into the search box above and click directly to
+the documentation for that symbol. You can also enter words or partial symbols
+and get a list of names that include your search key. Unfortunately Doxygen
+does not currently provide a full-text search capability. Another approach is
+to select the Classes tab to get an alphabetical listing of all the class
+names, and there is a tab below that providing a class hierarchy view. Here is
+an abbreviated list of important classes, all in the SimTK:: namespace:
+- MobilizedBody: combination of a Body and the inboard joint
+connecting it to the multibody tree, which we call a "mobilizer". Base class
+for specific mobilizer types of which there are many predefined; extensible.
+- Force: base class for a selection of built-in force types; easily
+extensible to model your own force elements.
+- Constraint: base class for a selection of built-in constraint types;
+extensible.
+- Motion: prescribed motions permitting mixed forward/inverse dynamics;
+more efficient than the equivalent %Constraint. Built-ins plus
+extendability.
+- Measure: general state-dependent computational block.
+- System, Subsystem, State: basic infrastructure objects.
+- MultibodySystem, SimbodyMatterSubsystem, GeneralForceSubsystem,
+ContactTrackerSubsystem, CompliantContactSubsystem: commonly used
+components.
+- Integrator, TimeStepper, Assembler, Optimizer: solvers.
+- Many utility classes (see tutorials, example programs, Doxygen Modules tab),
+including Rotation_, Transform_, Quaternion_,
+Vector_, Vec, @ref SimTK::SpatialVec "SpatialVec", Xml, and many more.
+- Matrix and vector classes and capabilities are discussed in the module
+ at ref MatVecUtilities "Matrix and Vector Utilities".
+
+For an overview of Simbody, read this paper:
+<a href="https://simtk.org/docman/view.php/47/1589/Sherman-2011-SethDelp-Simbody-ProcediaIUTAM-v2-p241.pdf">
+Michael A. Sherman, Ajay Seth, Scott L. Delp, "Simbody: multibody
+dynamics for biomedical research," <i>Procedia IUTAM</i> 2:241-261 (2011)</a>
+
+<h2>License</h2>
+We encourage the broadest possible use of Simbody for any purpose by using
+the extremely permissive Apache 2.0 License. We would appreciate some
+acknowledgement if our work has been helpful in yours, but our license does not
+require that. For more information see our
+\ref simbody_license_page "license page". **/
+
+} // namespace SimTK
+
+/** @page simbody_license_page Simbody&tm; Copyright and License
+<h2>Copyright and license</h2>
+This license, based on the maximally-permissive Apache 2.0 License, defines the
+terms under which we offer Simbody.
+ at verbatim
+ * -------------------------------------------------------------------------- *
+ * 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. *
+ * *
+ * Please cite: *
+ * Michael A. Sherman, Ajay Seth, Scott L. Delp, Simbody: multibody *
+ * dynamics for biomedical research, Procedia IUTAM 2:241-261 (2011) *
+ * http://dx.doi.org/10.1016/j.piutam.2011.04.023 *
+ * *
+ * Portions copyright (c) 2005-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: Jack Middleton, Christopher Bruns, Paul Mitiguy, Matthew *
+ * Millard, Charles Schwieters, Abhinandan Jain, Isaac Newton *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- *
+ at endverbatim
+
+<h2>What's with that "TM" in Simbody&tm;?</h2>
+That symbol is to let you know that we claim a trademark on the name "Simbody"
+so you may not use that name except to refer to our software as we distribute
+it. The license permits you to make any kind of derivative work, or to use
+all or part of Simbody code in any way you like, but you can't call the
+resulting work "Simbody" without our agreement. We make that restriction
+because we don't want any confusion to exist about what is, and what is not,
+the Simbody project that we are supporting. But ... if you want to make
+improvements to Simbody we would love to have your well-engineered, open source
+contributions to put into the main Simbody source tree, in which case they
+\e would be included under the Simbody name.
+
+<h2>How to acknowledge us</h2>
+Our license does not \e require that you acknowledge us, but we and our
+sponsors would be grateful if you did anyway! If our hard work has helped you
+with yours, please throw us a bone and mention on your "About" page and in
+your documentation that you are using Simbody from
+https://simtk.org/home/simbody. Where appropriate, please cite this paper:
+
+\par
+Michael A. Sherman, Ajay Seth, Scott L. Delp, Simbody: multibody
+dynamics for biomedical research, <i>Procedia IUTAM</i> 2:241-261 (2011)
+http://dx.doi.org/10.1016/j.piutam.2011.04.023
+
+We would be particularly grateful if you mention that Simbody is primarily
+funded by NIH Roadmap grant U54 GM072970. We appreciate that support very
+much, and the NIH appreciates knowing that its funds are having an impact,
+particularly on medical research and human health.
+**/
+
+#endif // SimTK_SIMBODY_DOXYGEN_MAINPAGE_H_
diff --git a/cmake/FindSimbody.cmake b/cmake/FindSimbody.cmake
new file mode 100644
index 0000000..a33bbed
--- /dev/null
+++ b/cmake/FindSimbody.cmake
@@ -0,0 +1,287 @@
+# FindSimbody.cmake
+#
+# Simbios National Center for Physics Based Simulation of Biological Structures
+# Stanford University
+# This cmake file created 2012 by Michael Sherman and is in the public
+# domain; Simbody itself is open source under the Apache 2.0 license.
+#
+# This is a CMake "find" module that will try to find the Simbody multibody
+# dynamics (physics) package installed somewhere on your computer. Simbody
+# is part of the SimTK biosimulation toolkit. For more information, see
+# https://simtk.org/home/simbody.
+#
+# To use this in a find_package() command from your own CMakeLists.txt file,
+# make sure this file is in a directory that is in the CMAKE_MODULE_PATH.
+# You can add a directory to that path with a line like this:
+# list(APPEND CMAKE_MODULE_PATH "myModuleDir")
+#
+# Then, to use Simbody include these lines:
+#
+# find_package(Simbody REQUIRED)
+# include_directories(${Simbody_INCLUDE_DIR})
+# link_directories(${Simbody_LIB_DIR})
+# add_executable(myexe ${my_source_files} ${my_header_files})
+# target_link_libraries(myexe ${Simbody_LIBRARIES})
+#
+# If you don't want to make it REQUIRED, you can check Simbody_FOUND after
+# find_package() returns.
+# TODO: no version selection is implemented here yet; if you provide it
+# to find_package() it will be ignored.
+#
+# This includes several libraries:
+# SimTKsimbody
+# SimTKmath
+# SimTKcommon
+# Windows only: liblapack libblas pthreadVC2[_x64]
+# The above will be in Simbody_ROOT_DIR/lib.
+#
+# On Mac and Linux we don't provide our own lapack but expect it to be
+# available.
+# Mac/Linux only: lapack blas
+#
+# Once done this will define:
+#
+# Simbody_FOUND - Whether search for Simbody libraries and headers succeeded.
+# Simbody_ROOT_DIR - the installation directory; all the pieces must be
+# found together
+# Simbody_INCLUDE_DIR - location of Simbody.h
+# Simbody_LIB_DIR - location of libSimTKsimbody.{a,so,dylib} or SimTKsimbody.lib
+# Simbody_BIN_DIR - location of VisualizerGUI and .dll's on Windows
+# Simbody_LIBRARIES - suitable for target_link_libraries(); includes
+# both optimized and debug libraries if both are
+# available
+# Simbody_STATIC_LIBRARIES - suitable for target_link_libraries(); includes
+# both optimized and debug static libraries if
+# both are available
+#
+# The following environment variables are used if set, in order of decreasing
+# preference:
+# SIMBODY_HOME
+# SimTK_INSTALL_DIR
+#
+# Otherwise we look in platform-specific standard places, in this order:
+# <standardPlaces>/Simbody, /simbody
+# <standardPlaces>/SimTK, /simtk
+#
+# This module looks for certain CMake variables on input and behaves
+# accordingly if they are present:
+#
+# SimTK_INSTALL_DIR
+# This is commonly set by other SimTK software and overrides the
+# environment variables if present. Note that this has the same name
+# as one of the environment variables but is distinct.
+# SimTK_SDK
+# If this is set (probably by the nightly build system) then that
+# will be the only place we'll look for Simbody, overriding environment
+# variables if they are set, and overriding the above CMake variable.
+
+cmake_minimum_required(VERSION 2.8)
+
+# Get values of relevant environment variables for convenient testing.
+set(ENV_SIMBODY_HOME_VALUE $ENV{SIMBODY_HOME})
+set(ENV_SimTK_INSTALL_DIR_VALUE $ENV{SimTK_INSTALL_DIR})
+
+if (SimTK_SDK) # a CMake variable, not an environment variable
+ # This is the SimTK.org nightly builds location. If set it overrides
+ # everything else.
+ set(Simbody_SEARCH_PATHS "${SimTK_SDK}")
+elseif (SimTK_INSTALL_DIR) # a CMake variable, not an environment variable
+ set(Simbody_SEARCH_PATHS "${SimTK_INSTALL_DIR}")
+elseif(ENV_SIMBODY_HOME_VALUE)
+ set(Simbody_SEARCH_PATHS "${ENV_SIMBODY_HOME_VALUE}")
+elseif(ENV_SimTK_INSTALL_DIR_VALUE) # != the eponymous CMake variable.
+ set(Simbody_SEARCH_PATHS "${ENV_SimTK_INSTALL_DIR_VALUE}")
+else()
+ # Nothing is set so we'll have to hunt.
+ set(Simbody_SEARCH_PATHS)
+
+ # UNIX includes Mac, Linux, and Cygwin
+ if (UNIX)
+ list(APPEND Simbody_SEARCH_PATHS /usr/local)
+ endif()
+
+ if (APPLE) # Mac only
+ list(APPEND Simbody_SEARCH_PATHS /Developer)
+ endif()
+
+ # WIN32 includes Windows 32 & 64 bit, and Cygwin
+ if (WIN32)
+ if( ${CMAKE_SIZEOF_VOID_P} EQUAL 8 )
+ # 64 bit target on Win64
+ set(PROGFILE_DIR "$ENV{ProgramW6432}")
+ else() # Target is 32 bit
+ # present if 64bit Windows
+ set(PROGFILE_DIR "$ENV{ProgramFiles(x86)}")
+ if (NOT PROGFILE_DIR)
+ set(PROGFILE_DIR "$ENV{ProgramFiles}") # on 32bit Windows
+ endif()
+ endif()
+ list(APPEND Simbody_SEARCH_PATHS ${PROGFILE_DIR})
+ endif()
+endif(SimTK_SDK)
+
+# We'll use the main Simbody header as the key to finding the installation
+# root directory. We're assuming the header is in
+# Simbody_INCLUDE_DIR == ${Simbody_ROOT_DIR}/include
+# So stripping off the "/include" should give the root directory.
+
+# Force this to be recalculated every time.
+set(Simbody_INCLUDE_DIR "Simbody_INCLUDE_DIR-NOTFOUND" CACHE PATH
+ "The Simbody and SimTK include directory." FORCE)
+
+message("${Simbody_SEARCH_PATHS}")
+
+
+foreach(pth IN LISTS Simbody_SEARCH_PATHS)
+ find_path(Simbody_INCLUDE_DIR
+ NAMES "SimTKsimbody.h" "Simbody.h"
+ PATHS "${pth}"
+ PATH_SUFFIXES "include"
+ "Simbody/include" "simbody/include"
+ "SimTK/include" "simtk/include"
+ NO_DEFAULT_PATH
+ DOC "Location of top-level installed Simbody header files"
+ )
+endforeach()
+
+# This will only be executed if the first loop fails. We're getting
+# desperate!
+find_path(Simbody_INCLUDE_DIR
+ NAMES "SimTKsimbody.h" "Simbody.h"
+ PATH_SUFFIXES "include"
+ "Simbody/include" "simbody/include"
+ "SimTK/include" "simtk/include"
+ DOC "Location of top-level installed Simbody header files"
+)
+
+get_filename_component(Simbody_ROOT_DIR_TEMP "${Simbody_INCLUDE_DIR}" PATH)
+set(Simbody_ROOT_DIR "${Simbody_ROOT_DIR_TEMP}" CACHE PATH
+ "Where we found Simbody; use SimTK_INSTALL_DIR to change." FORCE)
+
+set(Simbody_LIB_DIR ${Simbody_ROOT_DIR}/lib CACHE PATH
+ "Location of Simbody and SimTK libraries." FORCE)
+set(Simbody_BIN_DIR ${Simbody_ROOT_DIR}/bin CACHE PATH
+ "Location of Simbody-related executables and (on Windows) dlls." FORCE)
+
+set(Simbody_LIBRARY_LIST SimTKsimbody;SimTKmath;SimTKcommon)
+
+if (WIN32)
+ set(Simbody_LAPACK_LIBRARY_LIST liblapack;libblas)
+else()
+ set(Simbody_LAPACK_LIBRARY_LIST lapack;blas)
+endif()
+
+
+if (WIN32)
+ if( ${CMAKE_SIZEOF_VOID_P} EQUAL 8 )
+ set(Simbody_EXTRA_LIBRARY_LIST pthreadVC2_x64)
+ else()
+ set(Simbody_EXTRA_LIBRARY_LIST pthreadVC2)
+ endif()
+elseif (APPLE)
+ set(Simbody_EXTRA_LIBRARY_LIST pthread;dl)
+else()
+ set(Simbody_EXTRA_LIBRARY_LIST pthread;rt;dl;m)
+endif()
+
+
+# Find out which of the libraries are available.
+find_library(Simbody_LIBRARY NAMES SimTKsimbody
+ PATHS ${Simbody_LIB_DIR}
+ DOC "This is the main Simbody library."
+ NO_DEFAULT_PATH)
+find_library(Simbody_STATIC_LIBRARY NAMES SimTKsimbody_static
+ PATHS ${Simbody_LIB_DIR}
+ DOC "This is the main Simbody static library."
+ NO_DEFAULT_PATH)
+find_library(Simbody_DEBUG_LIBRARY NAMES SimTKsimbody_d
+ PATHS ${Simbody_LIB_DIR}
+ DOC "This is the main Simbody debug library."
+ NO_DEFAULT_PATH)
+find_library(Simbody_STATIC_DEBUG_LIBRARY NAMES SimTKsimbody_static_d
+ PATHS ${Simbody_LIB_DIR}
+ DOC "This is the main Simbody static debug library."
+ NO_DEFAULT_PATH)
+
+
+# Set composite Simbody_LIBRARIES variable
+set(LIBS)
+if(Simbody_LIBRARY AND Simbody_DEBUG_LIBRARY)
+ foreach(libname IN LISTS Simbody_LIBRARY_LIST)
+ set(LIBS ${LIBS} optimized "${libname}" debug "${libname}_d")
+ endforeach()
+elseif(Simbody_LIBRARY)
+ foreach(libname IN LISTS Simbody_LIBRARY_LIST)
+ set(LIBS ${LIBS} "${libname}")
+ endforeach()
+elseif(Simbody_DEBUG_LIBRARY)
+ foreach(libname IN LISTS Simbody_LIBRARY_LIST)
+ set(LIBS ${LIBS} "${libname}_d")
+ endforeach()
+endif()
+
+if (LIBS)
+ foreach(lapack_lib IN LISTS Simbody_LAPACK_LIBRARY_LIST)
+ set(LIBS ${LIBS} "${lapack_lib}")
+ endforeach()
+ foreach(extra_lib IN LISTS Simbody_EXTRA_LIBRARY_LIST)
+ set(LIBS ${LIBS} "${extra_lib}")
+ endforeach()
+ set(Simbody_LIBRARIES ${LIBS} CACHE STRING
+ "Simbody dynamic libraries" FORCE)
+else()
+ set(Simbody_LIBRARIES Simbody_LIBRARIES-NOTFOUND CACHE STRING
+ "Simbody dynamic libraries" FORCE)
+endif()
+
+# Static library
+set(LIBS)
+if(Simbody_STATIC_LIBRARY AND Simbody_STATIC_DEBUG_LIBRARY)
+ foreach(libname IN LISTS Simbody_LIBRARY_LIST)
+ set(LIBS ${LIBS} optimized "${libname}_static"
+ debug "${libname}_static_d")
+ endforeach()
+elseif(Simbody_STATIC_LIBRARY)
+ foreach(libname IN LISTS Simbody_LIBRARY_LIST)
+ set(LIBS ${LIBS} "${libname}_static")
+ endforeach()
+elseif(Simbody_STATIC_DEBUG_LIBRARY)
+ foreach(libname IN LISTS Simbody_LIBRARY_LIST)
+ set(LIBS ${LIBS} "${libname}_static_d")
+ endforeach()
+endif()
+
+if (LIBS)
+ # these aren't available in static
+ foreach(lapack_lib IN LISTS Simbody_LAPACK_LIBRARY_LIST)
+ set(LIBS ${LIBS} "${lapack_lib}")
+ endforeach()
+ foreach(extra_lib IN LISTS Simbody_EXTRA_LIBRARY_LIST)
+ set(LIBS ${LIBS} "${extra_lib}")
+ endforeach()
+ set(Simbody_STATIC_LIBRARIES "${LIBS}" CACHE STRING
+ "Simbody static libraries" FORCE)
+else()
+ set(Simbody_STATIC_LIBRARIES Simbody_STATIC_LIBRARIES-NOTFOUND CACHE STRING
+ "Simbody static libraries" FORCE)
+endif()
+
+# This CMake-supplied script provides standard error handling.
+include(FindPackageHandleStandardArgs OPTIONAL)
+find_package_handle_standard_args(Simbody DEFAULT_MSG
+ Simbody_INCLUDE_DIR)
+
+# Not all the variables we produced need be returned.
+
+unset(Simbody_LIBRARY CACHE)
+unset(Simbody_DEBUG_LIBRARY CACHE)
+unset(Simbody_STATIC_LIBRARY CACHE)
+unset(Simbody_STATIC_DEBUG_LIBRARY CACHE)
+mark_as_advanced(Simbody_LIBRARIES Simbody_STATIC_LIBRARIES)
+
+mark_as_advanced(
+ Simbody_ROOT_DIR
+ Simbody_INCLUDE_DIR
+ Simbody_BIN_DIR
+ Simbody_LIB_DIR
+)
diff --git a/cmake/SampleCMakeLists.txt b/cmake/SampleCMakeLists.txt
new file mode 100644
index 0000000..7280380
--- /dev/null
+++ b/cmake/SampleCMakeLists.txt
@@ -0,0 +1,22 @@
+# Generic CMakeLists.txt for making a Simbody-using executable.
+# This shows how to use the provided SimbodyConfig.cmake to locate a Simbody
+# installation on your machine so you can use it from your own code.
+# You will most likely want to copy some of these lines into your own
+# CMakeLists.txt rather than use this one verbatim.
+cmake_minimum_required(VERSION 2.8)
+project(myexe)
+
+# List your source and header files here.
+set(my_source_files myexe.cpp)
+set(my_header_files myexe.h)
+
+# This depends on SimbodyConfig.cmake being located somewhere predictable
+# on your machine. If you have installed it somewhere that CMake won't be
+# able to guess, you'll need to tell find_package where to look.
+find_package(Simbody REQUIRED)
+
+include_directories(${Simbody_INCLUDE_DIR})
+link_directories(${Simbody_LIB_DIR})
+
+add_executable(myexe ${my_source_files} ${my_header_files})
+target_link_libraries(myexe ${Simbody_LIBRARIES})
diff --git a/cmake/SimbodyConfig.cmake.in b/cmake/SimbodyConfig.cmake.in
new file mode 100644
index 0000000..70080fc
--- /dev/null
+++ b/cmake/SimbodyConfig.cmake.in
@@ -0,0 +1,216 @@
+# SimbodyConfig.cmake
+
+# Adapted from FindSimbody.cmake
+#
+# This should define the following:
+# Simbody_FOUND - Whether search for Simbody libraries and headers succeeded.
+# Simbody_ROOT_DIR - the installation directory; all the pieces must be
+# found together
+# Simbody_INCLUDE_DIR - location of Simbody.h
+# Simbody_LIB_DIR - location of libSimTKsimbody.{a,so,dylib}
+# or SimTKsimbody.lib
+# Simbody_BIN_DIR - location of VisualizerGUI and .dll's on Windows
+# Simbody_LIBRARIES - suitable for target_link_libraries(); includes
+# both optimized and debug libraries if both are
+# available
+# Simbody_STATIC_LIBRARIES - suitable for target_link_libraries(); includes
+# both optimized and debug static libraries if
+# both are available
+if (@PKG_NAME at _CONFIG_INCLUDED)
+ return()
+endif()
+set(@PKG_NAME at _CONFIG_INCLUDED TRUE)
+
+# Watch out for spaces in pathnames -- must quote.
+list(APPEND @PKG_NAME at _ROOT_DIR
+ "@CMAKE_INSTALL_PREFIX@")
+
+list(APPEND @PKG_NAME at _INCLUDE_DIR
+ "@CMAKE_INSTALL_PREFIX@/@SIMBODY_INCLUDE_INSTALL_DIR@")
+
+list(APPEND @PKG_NAME at _LIB_DIR
+ "@CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_LIBDIR@")
+
+list(APPEND @PKG_NAME at _BIN_DIR
+ "@SIMBODY_VISUALIZER_INSTALL_DIR@")
+
+list(APPEND @PKG_NAME at _CFLAGS
+ -I"@CMAKE_INSTALL_PREFIX@/@SIMBODY_INCLUDE_INSTALL_DIR@")
+
+list(APPEND @PKG_NAME at _LDFLAGS
+ -L"@CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_LIBDIR@")
+
+# Set extra libraries to link against
+if (WIN32)
+ set(Simbody_LAPACK_LIBRARY_LIST liblapack;libblas)
+else()
+ set(Simbody_LAPACK_LIBRARY_LIST lapack;blas)
+endif()
+if (WIN32)
+ if( ${CMAKE_SIZEOF_VOID_P} EQUAL 8 )
+ set(Simbody_EXTRA_LIBRARY_LIST pthreadVC2_x64)
+ else()
+ set(Simbody_EXTRA_LIBRARY_LIST pthreadVC2)
+ endif()
+elseif (APPLE)
+ set(Simbody_EXTRA_LIBRARY_LIST pthread;dl)
+else()
+ set(Simbody_EXTRA_LIBRARY_LIST pthread;rt;dl;m)
+endif()
+
+# Find out which of the libraries are available.
+find_library(Simbody_LIBRARY NAMES SimTKsimbody
+ PATHS ${Simbody_LIB_DIR}
+ DOC "This is the main Simbody library."
+ NO_DEFAULT_PATH)
+find_library(Simbody_STATIC_LIBRARY NAMES SimTKsimbody_static
+ PATHS ${Simbody_LIB_DIR}
+ DOC "This is the main Simbody static library."
+ NO_DEFAULT_PATH)
+find_library(Simbody_DEBUG_LIBRARY NAMES SimTKsimbody_d
+ PATHS ${Simbody_LIB_DIR}
+ DOC "This is the main Simbody debug library."
+ NO_DEFAULT_PATH)
+find_library(Simbody_STATIC_DEBUG_LIBRARY NAMES SimTKsimbody_static_d
+ PATHS ${Simbody_LIB_DIR}
+ DOC "This is the main Simbody static debug library."
+ NO_DEFAULT_PATH)
+
+
+# Set composite Simbody_LIBRARIES variable
+set(LIBS)
+if(Simbody_LIBRARY AND Simbody_DEBUG_LIBRARY)
+ foreach(lib @PKG_LIBRARIES@)
+ # must reset each time around the loop or find_library won't work
+ set(onelib "${lib}-NOTFOUND" CACHE INTERNAL "nothing")
+ set(onelibd "${lib}_d-NOTFOUND" CACHE INTERNAL "nothing")
+ find_library(onelib ${lib}
+ PATHS ${Simbody_LIB_DIR}
+ NO_DEFAULT_PATH)
+ find_library(onelibd ${lib}_d
+ PATHS ${Simbody_LIB_DIR}
+ NO_DEFAULT_PATH)
+ if((NOT onelib) OR (NOT onelibd))
+ message(FATAL_ERROR
+ "Library '${lib}' in package Simbody is not installed properly")
+ endif()
+ set(LIBS ${LIBS} optimized "${onelib}" debug "${onelibd}")
+ endforeach()
+elseif(Simbody_LIBRARY)
+ foreach(lib @PKG_LIBRARIES@)
+ set(onelib "${lib}-NOTFOUND" CACHE INTERNAL "nothing")
+ find_library(onelib ${lib}
+ PATHS ${Simbody_LIB_DIR}
+ NO_DEFAULT_PATH)
+ if(NOT onelib)
+ message(FATAL_ERROR
+ "Library '${lib}' in package Simbody is not installed properly")
+ endif()
+ set(LIBS ${LIBS} "${onelib}")
+ endforeach()
+elseif(Simbody_DEBUG_LIBRARY)
+ foreach(lib @PKG_LIBRARIES@)
+ set(lib "${lib}_d")
+ set(onelibd "${lib}-NOTFOUND" CACHE INTERNAL "nothing")
+ find_library(onelibd ${lib}
+ PATHS ${Simbody_LIB_DIR}
+ NO_DEFAULT_PATH)
+ if(NOT onelibd)
+ message(FATAL_ERROR
+ "Library '${lib}' in package Simbody is not installed properly")
+ endif()
+ set(LIBS ${LIBS} "${onelibd}")
+ endforeach()
+endif()
+
+if (LIBS)
+ foreach(lapack_lib IN LISTS Simbody_LAPACK_LIBRARY_LIST)
+ set(LIBS ${LIBS} "${lapack_lib}")
+ endforeach()
+ foreach(extra_lib IN LISTS Simbody_EXTRA_LIBRARY_LIST)
+ set(LIBS ${LIBS} "${extra_lib}")
+ endforeach()
+ set(Simbody_LIBRARIES ${LIBS} CACHE STRING
+ "Simbody dynamic libraries" FORCE)
+else()
+ set(Simbody_LIBRARIES Simbody_LIBRARIES-NOTFOUND CACHE STRING
+ "Simbody dynamic libraries" FORCE)
+endif()
+
+# Static library
+set(LIBS)
+if(Simbody_STATIC_LIBRARY AND Simbody_STATIC_DEBUG_LIBRARY)
+ foreach(lib @PKG_LIBRARIES@)
+ set(lib "${lib}_static")
+ set(onelib "${lib}-NOTFOUND" CACHE INTERNAL "nothing")
+ set(onelibd "${lib}_d-NOTFOUND" CACHE INTERNAL "nothing")
+ find_library(onelib ${lib}
+ PATHS ${Simbody_LIB_DIR}
+ NO_DEFAULT_PATH)
+ find_library(onelibd ${lib}_d
+ PATHS ${Simbody_LIB_DIR}
+ NO_DEFAULT_PATH)
+ if((NOT onelib) OR (NOT onelibd))
+ message(FATAL_ERROR
+ "Library '${lib}' in package Simbody is not installed properly")
+ endif()
+ set(LIBS ${LIBS} optimized "${onelib}" debug "${onelibd}")
+ endforeach()
+elseif(Simbody_STATIC_LIBRARY)
+ foreach(lib @PKG_LIBRARIES@)
+ set(lib "${lib}_static")
+ set(onelib "${lib}-NOTFOUND" CACHE INTERNAL "nothing")
+ find_library(onelib ${lib}
+ PATHS ${Simbody_LIB_DIR}
+ NO_DEFAULT_PATH
+ )
+ if(NOT onelib)
+ message(FATAL_ERROR
+ "Library '${lib}' in package Simbody is not installed properly")
+ endif()
+ set(LIBS ${LIBS} "${onelib}")
+ endforeach()
+elseif(Simbody_STATIC_DEBUG_LIBRARY)
+ foreach(lib @PKG_LIBRARIES@)
+ set(lib "${lib}_static_d")
+ set(onelibd "${lib}-NOTFOUND" CACHE INTERNAL "nothing")
+ find_library(onelibd ${lib}
+ PATHS ${Simbody_LIB_DIR}
+ NO_DEFAULT_PATH
+ )
+ if(NOT onelibd)
+ message(FATAL_ERROR
+ "Library '${lib}' in package Simbody is not installed properly")
+ endif()
+ set(LIBS ${LIBS} "${onelibd}")
+ endforeach()
+endif()
+
+if (LIBS)
+ # these aren't available in static
+ foreach(lapack_lib IN LISTS Simbody_LAPACK_LIBRARY_LIST)
+ set(LIBS ${LIBS} "${lapack_lib}")
+ endforeach()
+ foreach(extra_lib IN LISTS Simbody_EXTRA_LIBRARY_LIST)
+ set(LIBS ${LIBS} "${extra_lib}")
+ endforeach()
+ set(Simbody_STATIC_LIBRARIES "${LIBS}" CACHE STRING
+ "Simbody static libraries" FORCE)
+else()
+ set(Simbody_STATIC_LIBRARIES Simbody_STATIC_LIBRARIES-NOTFOUND CACHE STRING
+ "Simbody static libraries" FORCE)
+endif()
+
+# This CMake-supplied script provides standard error handling.
+include(FindPackageHandleStandardArgs OPTIONAL)
+find_package_handle_standard_args(Simbody DEFAULT_MSG
+ Simbody_INCLUDE_DIR)
+
+# Not all the variables we produced need be returned.
+unset(onelib CACHE)
+unset(onelibd CACHE)
+unset(Simbody_LIBRARY CACHE)
+unset(Simbody_DEBUG_LIBRARY CACHE)
+unset(Simbody_STATIC_LIBRARY CACHE)
+unset(Simbody_STATIC_DEBUG_LIBRARY CACHE)
+mark_as_advanced(Simbody_LIBRARIES Simbody_STATIC_LIBRARIES)
diff --git a/cmake/SimbodyConfigVersion.cmake.in b/cmake/SimbodyConfigVersion.cmake.in
new file mode 100644
index 0000000..e9c3b40
--- /dev/null
+++ b/cmake/SimbodyConfigVersion.cmake.in
@@ -0,0 +1,55 @@
+# See:
+# http://www.cmake.org/Wiki/CMake/Tutorials/Packaging.
+# http://www.cmake.org/Wiki/CMake/Tutorials/How_to_create_a_ProjectConfig.cmake_file
+#
+# A project using Simbody, say MyProject, can specify the version of Simbody it
+# needs. This script must determine if THIS version of Simbody matches the
+# version of Simbody that MyProject requires.
+#
+# If in MyProject/CMakeLists.txt, there's a line like:
+#
+# find_package(Simbody VERSION 3.4)
+#
+# then CMake will run this script and define the following:
+#
+# PACKAGE_FIND_NAME = the <package> name
+# PACKAGE_FIND_VERSION = full requested version string
+# PACKAGE_FIND_VERSION_MAJOR = requested major version, if any
+# PACKAGE_FIND_VERSION_MINOR = requested minor version, if any
+# PACKAGE_FIND_VERSION_PATCH = requested patch version, if any
+#
+# We must then use the above to set the following:
+#
+# PACKAGE_VERSION = full provided version string
+# PACKAGE_VERSION_EXACT = true if version is exact match
+# PACKAGE_VERSION_COMPATIBLE = true if version is compatible
+# PACKAGE_VERSION_UNSUITABLE = true if unsuitable as any version
+# (CMake >= 2.6.3)
+
+set(PACKAGE_VERSION "@SIMBODY_VERSION@")
+
+
+# General behavior: compatibility requires same major and minor versions.
+# This section should depend on:
+# SIMBODY_MAJOR_VERSION
+# SIMBODY_MINOR_VERSION
+# SIMBODY_PATCH_VERSION
+# Informed by from VTK's VTKConfigVersion.cmake.in.
+if("@SIMBODY_MAJOR_VERSION at .@SIMBODY_MINOR_VERSION@" VERSION_EQUAL
+ "${PACKAGE_FIND_VERSION_MAJOR}.${PACKAGE_FIND_VERSION_MINOR}")
+ if(NOT "@SIMBODY_PATCH_VERSION@" VERSION_LESS "${PACKAGE_FIND_VERSION_PATCH}")
+ set(PACKAGE_VERSION_COMPATIBLE 1)
+ if("@SIMBODY_PATCH_VERSION@" VERSION_EQUAL
+ "${PACKAGE_FIND_VERSION_PATCH}")
+ set(PACKAGE_VERSION_EXACT 1)
+ endif()
+ endif()
+endif()
+
+# Exceptions: this section can depend on hard-coded version numbers. All that
+# should be done here is marking versions incompatiable that would generally be
+# compatible (e.g., set(PACKAGE_VERSION_COMPATIBLE FALSE)).
+# Furthermore, it only makes sense for these hard-coded version numbers to be
+# the current or previous versions of Simbody.
+
+# Nothing to put here yet.
diff --git a/cmake/cmake_uninstall.cmake.in b/cmake/cmake_uninstall.cmake.in
new file mode 100644
index 0000000..2037e36
--- /dev/null
+++ b/cmake/cmake_uninstall.cmake.in
@@ -0,0 +1,21 @@
+if(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+ message(FATAL_ERROR "Cannot find install manifest: @CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+endif(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+
+file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files)
+string(REGEX REPLACE "\n" ";" files "${files}")
+foreach(file ${files})
+ message(STATUS "Uninstalling $ENV{DESTDIR}${file}")
+ if(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}")
+ exec_program(
+ "@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\""
+ OUTPUT_VARIABLE rm_out
+ RETURN_VALUE rm_retval
+ )
+ if(NOT "${rm_retval}" STREQUAL 0)
+ message(FATAL_ERROR "Problem when removing $ENV{DESTDIR}${file}")
+ endif(NOT "${rm_retval}" STREQUAL 0)
+ else(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}")
+ message(STATUS "File $ENV{DESTDIR}${file} does not exist.")
+ endif(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}")
+endforeach(file)
diff --git a/cmake/pkgconfig/simbody.pc.in b/cmake/pkgconfig/simbody.pc.in
new file mode 100644
index 0000000..7e36ed8
--- /dev/null
+++ b/cmake/pkgconfig/simbody.pc.in
@@ -0,0 +1,10 @@
+prefix=@CMAKE_INSTALL_PREFIX@
+libdir=${prefix}/@CMAKE_INSTALL_LIBDIR@
+includedir=${prefix}/@CMAKE_INSTALL_INCLUDEDIR@
+
+Name: simbody
+Description: Simbody Libraries
+Version: @SIMBODY_VERSION@
+Requires:
+Libs: -L${libdir} -lSimTKsimbody -lSimTKmath -lSimTKcommon @PKGCONFIG_PLATFORM_LIBS@
+CFlags: -I${includedir}
diff --git a/debian/changelog b/debian/changelog
new file mode 100644
index 0000000..1582686
--- /dev/null
+++ b/debian/changelog
@@ -0,0 +1,12 @@
+simbody (3.4.1-1~precise) precise; urgency=low
+
+ * New 3.4 upstream version
+
+ -- Jose Luis Rivero <jrivero at osrfoundation.org> Fri, 28 Mar 2014 19:40:46 +0100
+
+simbody (3.3-1~quantal) quantal; urgency=high
+
+ * Initial release. (Closes: #728723)
+
+ -- Jose Luis Rivero <jrivero at osrfoundation.org> Thu, 22 Aug 2013 16:23:00 -0700
+
diff --git a/debian/compat b/debian/compat
new file mode 100644
index 0000000..ec63514
--- /dev/null
+++ b/debian/compat
@@ -0,0 +1 @@
+9
diff --git a/debian/control b/debian/control
new file mode 100644
index 0000000..5b45278
--- /dev/null
+++ b/debian/control
@@ -0,0 +1,78 @@
+Source: simbody
+Maintainer: Michael Sherman <msherman at stanford.edu>
+Standards-Version: 3.9.5
+Section: science
+Priority: extra
+Build-Depends: debhelper (>= 9),
+ cmake,
+ doxygen,
+ freeglut3-dev,
+ libxi-dev,
+ libxmu-dev,
+ liblapack-dev,
+ doxygen-latex
+Vcs-Browser: https://github.com/simbody/simbody
+Vcs-Git: git://github.com/simbody/simbody.git
+Homepage: https://simtk.org/home/simbody
+
+Package: libsimbody3.4
+Section: libs
+Architecture: any
+Pre-Depends: ${misc:Pre-Depends}
+Depends: ${shlibs:Depends}, ${misc:Depends}
+Multi-Arch: same
+Description: SimTK multibody dynamics API - shared library
+ Simbody is a SimTK toolset providing general multibody dynamics capability,
+ that is, the ability to solve Newton's 2nd law F=ma in any set of generalized
+ coordinates subject to arbitrary constraints. Simbody is provided as an open
+ source, object-oriented C++ API and delivers high-performance,
+ accuracy-controlled science/engineering-quality results.
+
+Package: libsimbody-dev
+Architecture: any
+Section: libdevel
+Depends: freeglut3-dev,
+ libxi-dev,
+ libxmu-dev,
+ liblapack-dev,
+ libsimbody3.4 (= ${binary:Version}),
+ ${misc:Depends}
+Multi-Arch: same
+Description: SimTK multibody dynamics API - development files
+ Simbody is a SimTK toolset providing general multibody dynamics capability,
+ that is, the ability to solve Newton's 2nd law F=ma in any set of generalized
+ coordinates subject to arbitrary constraints. Simbody is provided as an open
+ source, object-oriented C++ API and delivers high-performance,
+ accuracy-controlled science/engineering-quality results.
+ .
+ This package contains development files (headers, shared library
+ symbolic link and pkg-config file).
+
+Package: libsimbody3.4-dbg
+Architecture: any
+Section: debug
+Depends: libsimbody3.4 (= ${binary:Version}),
+ ${misc:Depends}
+Multi-Arch: foreign
+Description: SimTK multibody dynamics API - Debugging Symbols
+ Simbody is a SimTK toolset providing general multibody dynamics capability,
+ that is, the ability to solve Newton's 2nd law F=ma in any set of generalized
+ coordinates subject to arbitrary constraints. Simbody is provided as an open
+ source, object-oriented C++ API and delivers high-performance,
+ accuracy-controlled science/engineering-quality results.
+ .
+ This package contains the debugging symbols.
+
+Package: libsimbody-doc
+Architecture: all
+Section: doc
+Depends: ${misc:Depends}
+Multi-Arch: foreign
+Description: SimTK multibody dynamics API - Documentation
+ Simbody is a SimTK toolset providing general multibody dynamics capability,
+ that is, the ability to solve Newton's 2nd law F=ma in any set of generalized
+ coordinates subject to arbitrary constraints. Simbody is provided as an open
+ source, object-oriented C++ API and delivers high-performance,
+ accuracy-controlled science/engineering-quality results.
+ .
+ This package contains documentation (user guides, theory manual, API/Doxygen).
diff --git a/debian/copyright b/debian/copyright
new file mode 100644
index 0000000..9e44f6f
--- /dev/null
+++ b/debian/copyright
@@ -0,0 +1,312 @@
+Format: http://www.debian.org/doc/packaging-manuals/copyright-format/1.0/
+Upstream-Name: simbody
+Upstream-Contact: https://simtk.org/sendmessage.php?touser=106
+Source: https://simtk.org
+
+Files: *
+Copyright: 2005-13, Stanford University and the Authors.
+License: Apache
+ See '/usr/share/common-licenses/Apache-2.0'.
+
+Files: debian/*
+Copyright: 2013, Jose Luis Rivero <jrivero at osrfoundation.org>
+License: Apache
+ See '/usr/share/common-licenses/Apache-2.0'.
+
+Files: SimTKmath/Integrators/*
+Copyright: 2002, The Regents of the University of California.
+License: BSD-3
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions
+ are met:
+ .
+ 1. Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the disclaimer below.
+ .
+ 2. Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the disclaimer (as noted below)
+ in the documentation and/or other materials provided with the
+ distribution.
+ .
+ 3. Neither the name of the UC/LLNL nor the names of its contributors
+ may be used to endorse or promote products derived from this software
+ without specific prior written permission.
+ .
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ REGENTS OF THE UNIVERSITY OF CALIFORNIA, THE U.S. DEPARTMENT OF ENERGY
+ OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ .
+ Additional BSD Notice
+ ---------------------
+ 1. This notice is required to be provided under our contract with
+ the U.S. Department of Energy (DOE). This work was produced at the
+ University of California, Lawrence Livermore National Laboratory
+ under Contract No. W-7405-ENG-48 with the DOE.
+ .
+ 2. Neither the United States Government nor the University of
+ California nor any of their employees, makes any warranty, express
+ or implied, or assumes any liability or responsibility for the
+ accuracy, completeness, or usefulness of any information, apparatus,
+ product, or process disclosed, or represents that its use would not
+ infringe privately-owned rights.
+ .
+ 3. Also, reference herein to any specific commercial products,
+ process, or services by trade name, trademark, manufacturer or
+ otherwise does not necessarily constitute or imply its endorsement,
+ recommendation, or favoring by the United States Government or the
+ University of California. The views and opinions of authors expressed
+ herein do not necessarily state or reflect those of the United States
+ Government or the University of California, and shall not be used for
+ advertising or product endorsement purposes.
+
+Files: SimTKcommon/src/tinyxml.*
+Copyright: 2000-2006, Lee Thomason (www.grinninglizard.com)
+License: AS-IS
+ This software is provided 'as-is', without any express or implied
+ warranty. In no event will the authors be held liable for any
+ damages arising from the use of this software.
+ .
+ Permission is granted to anyone to use this software for any
+ purpose, including commercial applications, and to alter it and
+ redistribute it freely, subject to the following restrictions:
+ .
+ 1. The origin of this software must not be misrepresented; you must
+ not claim that you wrote the original software. If you use this
+ software in a product, an acknowledgment in the product documentation
+ would be appreciated but is not required.
+ .
+ 2. Altered source versions must be plainly marked as such, and
+ must not be misrepresented as being the original software.
+ .
+ 3. This notice may not be removed or altered from any source
+ distribution.
+
+Files: SimTKmath/Optimizers/src/IpOpt
+Copyright: 2004, 2006 International Business Machines and others
+ 2005, 2006 Carnegie Mellon University and others
+ 2005, 2006 International Business Machines and others
+ 2006, International Business Machines and others
+License: Eclipse Public License - v 1.0
+ THE ACCOMPANYING PROGRAM IS PROVIDED UNDER THE TERMS OF THIS ECLIPSE PUBLIC
+ LICENSE ("AGREEMENT"). ANY USE, REPRODUCTION OR DISTRIBUTION OF THE PROGRAM
+ CONSTITUTES RECIPIENT'S ACCEPTANCE OF THIS AGREEMENT.
+ .
+ 1. DEFINITIONS
+ .
+ "Contribution" means:
+ .
+ a) in the case of the initial Contributor, the initial code and documentation
+ distributed under this Agreement, and
+ .
+ b) in the case of each subsequent Contributor:
+ .
+ i) changes to the Program, and
+ .
+ ii) additions to the Program;
+ .
+ where such changes and/or additions to the Program originate from and are
+ distributed by that particular Contributor. A Contribution 'originates' from
+ a Contributor if it was added to the Program by such Contributor itself or
+ anyone acting on such Contributor's behalf. Contributions do not include
+ additions to the Program which: (i) are separate modules of software
+ distributed in conjunction with the Program under their own license agreement,
+ and (ii) are not derivative works of the Program.
+ .
+ "Contributor" means any person or entity that distributes the Program.
+ .
+ "Licensed Patents" mean patent claims licensable by a Contributor which are
+ necessarily infringed by the use or sale of its Contribution alone or when
+ combined with the Program.
+ .
+ "Program" means the Contributions distributed in accordance with this
+ Agreement.
+ .
+ "Recipient" means anyone who receives the Program under this Agreement,
+ including all Contributors.
+ .
+ 2. GRANT OF RIGHTS
+ .
+ a) Subject to the terms of this Agreement, each Contributor hereby grants
+ Recipient a non-exclusive, worldwide, royalty-free copyright license to
+ reproduce, prepare derivative works of, publicly display, publicly perform,
+ distribute and sublicense the Contribution of such Contributor, if any, and
+ such derivative works, in source code and object code form.
+ .
+ b) Subject to the terms of this Agreement, each Contributor hereby grants
+ Recipient a non-exclusive, worldwide, royalty-free patent license under
+ Licensed Patents to make, use, sell, offer to sell, import and otherwise
+ transfer the Contribution of such Contributor, if any, in source code and
+ object code form. This patent license shall apply to the combination of the
+ Contribution and the Program if, at the time the Contribution is added by the
+ Contributor, such addition of the Contribution causes such combination to be
+ covered by the Licensed Patents. The patent license shall not apply to any
+ other combinations which include the Contribution. No hardware per se is
+ licensed hereunder.
+ .
+ c) Recipient understands that although each Contributor grants the licenses
+ to its Contributions set forth herein, no assurances are provided by any
+ Contributor that the Program does not infringe the patent or other
+ intellectual property rights of any other entity. Each Contributor
+ disclaims any liability to Recipient for claims brought by any other
+ entity based on infringement of intellectual property rights or otherwise.
+ As a condition to exercising the rights and licenses granted hereunder, each
+ Recipient hereby assumes sole responsibility to secure any other intellectual
+ property rights needed, if any. For example, if a third party patent license
+ is required to allow Recipient to distribute the Program, it is Recipient's
+ responsibility to acquire that license before distributing the Program.
+ .
+ d) Each Contributor represents that to its knowledge it has sufficient
+ copyright rights in its Contribution, if any, to grant the copyright
+ license set forth in this Agreement.
+ .
+ 3. REQUIREMENTS
+ .
+ A Contributor may choose to distribute the Program in object code form
+ under its own license agreement, provided that:
+ .
+ a) it complies with the terms and conditions of this Agreement; and
+ .
+ b) its license agreement:
+ .
+ i) effectively disclaims on behalf of all Contributors all warranties and
+ conditions, express and implied, including warranties or conditions of
+ title and non-infringement, and implied warranties or conditions of
+ merchantability and fitness for a particular purpose;
+ .
+ ii) effectively excludes on behalf of all Contributors all liability for
+ damages, including direct, indirect, special, incidental and consequential
+ damages, such as lost profits;
+ .
+ iii) states that any provisions which differ from this Agreement are
+ offered by that Contributor alone and not by any other party; and
+ .
+ iv) states that source code for the Program is available from such
+ Contributor, and informs licensees how to obtain it in a reasonable
+ manner on or through a medium customarily used for software exchange.
+ .
+ When the Program is made available in source code form:
+ .
+ a) it must be made available under this Agreement; and
+ .
+ b) a copy of this Agreement must be included with each copy of the Program.
+ .
+ Contributors may not remove or alter any copyright notices contained within
+ the Program.
+ .
+ Each Contributor must identify itself as the originator of its Contribution,
+ if any, in a manner that reasonably allows subsequent Recipients to
+ identify the originator of the Contribution.
+ .
+ 4. COMMERCIAL DISTRIBUTION
+ .
+ Commercial distributors of software may accept certain responsibilities
+ with respect to end users, business partners and the like. While this
+ license is intended to facilitate the commercial use of the Program, the
+ Contributor who includes the Program in a commercial product offering
+ should do so in a manner which does not create potential liability for other
+ Contributors. Therefore, if a Contributor includes the Program in a
+ commercial product offering, such Contributor ("Commercial Contributor")
+ hereby agrees to defend and indemnify every other Contributor ("Indemnified
+ Contributor") against any losses, damages and costs (collectively "Losses")
+ arising from claims, lawsuits and other legal actions brought by a third
+ party against the Indemnified Contributor to the extent caused by the acts
+ or omissions of such Commercial Contributor in connection with its
+ distribution of the Program in a commercial product offering. The
+ obligations in this section do not apply to any claims or Losses relating
+ to any actual or alleged intellectual property infringement. In order to
+ qualify, an Indemnified Contributor must: a) promptly notify the Commercial
+ Contributor in writing of such claim, and b) allow the Commercial
+ Contributor to control, and cooperate with the Commercial Contributor in,
+ the defense and any related settlement negotiations. The Indemnified
+ Contributor may participate in any such claim at its own expense.
+ .
+ For example, a Contributor might include the Program in a commercial
+ product offering, Product X. That Contributor is then a Commercial
+ Contributor. If that Commercial Contributor then makes performance claims,
+ or offers warranties related to Product X, those performance claims and
+ warranties are such Commercial Contributor's responsibility alone. Under
+ this section, the Commercial Contributor would have to defend claims
+ against the other Contributors related to those performance claims and
+ warranties, and if a court requires any other Contributor to pay any
+ damages as a result, the Commercial Contributor must pay those damages.
+ .
+ 5. NO WARRANTY
+ .
+ EXCEPT AS EXPRESSLY SET FORTH IN THIS AGREEMENT, THE PROGRAM IS PROVIDED ON
+ AN "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, EITHER
+ EXPRESS OR IMPLIED INCLUDING, WITHOUT LIMITATION, ANY WARRANTIES OR
+ CONDITIONS OF TITLE, NON-INFRINGEMENT, MERCHANTABILITY OR FITNESS FOR A
+ PARTICULAR PURPOSE. Each Recipient is solely responsible for determining the
+ appropriateness of using and distributing the Program and assumes all risks
+ associated with its exercise of rights under this Agreement , including but
+ not limited to the risks and costs of program errors, compliance with
+ applicable laws, damage to or loss of data, programs or equipment, and
+ unavailability or interruption of operations.
+ .
+ 6. DISCLAIMER OF LIABILITY
+ .
+ EXCEPT AS EXPRESSLY SET FORTH IN THIS AGREEMENT, NEITHER RECIPIENT NOR ANY
+ CONTRIBUTORS SHALL HAVE ANY LIABILITY FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING WITHOUT LIMITATION
+ LOST PROFITS), HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ ARISING IN ANY WAY OUT OF THE USE OR DISTRIBUTION OF THE PROGRAM OR THE
+ EXERCISE OF ANY RIGHTS GRANTED HEREUNDER, EVEN IF ADVISED OF THE
+ POSSIBILITY OF SUCH DAMAGES.
+ .
+ 7. GENERAL
+ .
+ If any provision of this Agreement is invalid or unenforceable under
+ applicable law, it shall not affect the validity or enforceability of the
+ remainder of the terms of this Agreement, and without further action by the
+ parties hereto, such provision shall be reformed to the minimum extent
+ necessary to make such provision valid and enforceable.
+ .
+ If Recipient institutes patent litigation against any entity (including a
+ cross-claim or counterclaim in a lawsuit) alleging that the Program itself
+ (excluding combinations of the Program with other software or hardware)
+ infringes such Recipient's patent(s), then such Recipient's rights granted
+ under Section 2(b) shall terminate as of the date such litigation is filed.
+ .
+ All Recipient's rights under this Agreement shall terminate if it fails to
+ comply with any of the material terms or conditions of this Agreement and
+ does not cure such failure in a reasonable period of time after becoming
+ aware of such noncompliance. If all Recipient's rights under this
+ Agreement terminate, Recipient agrees to cease use and distribution of the
+ Program as soon as reasonably practicable. However, Recipient's obligations
+ under this Agreement and any licenses granted by Recipient relating to the
+ Program shall continue and survive.
+ .
+ Everyone is permitted to copy and distribute copies of this Agreement, but
+ in order to avoid inconsistency the Agreement is copyrighted and may only
+ be modified in the following manner. The Agreement Steward reserves the
+ right to publish new versions (including revisions) of this Agreement from
+ time to time. No one other than the Agreement Steward has the right to
+ modify this Agreement. The Eclipse Foundation is the initial Agreement
+ Steward. The Eclipse Foundation may assign the responsibility to serve as
+ the Agreement Steward to a suitable separate entity. Each new version of
+ the Agreement will be given a distinguishing version number. The Program
+ (including Contributions) may always be distributed subject to the version
+ of the Agreement under which it was received. In addition, after a new
+ version of the Agreement is published, Contributor may elect to distribute
+ the Program (including its Contributions) under the new version. Except
+ as expressly stated in Sections 2(a) and 2(b) above, Recipient receives
+ no rights or licenses to the intellectual property of any Contributor
+ under this Agreement, whether expressly, by implication, estoppel or
+ otherwise. All rights in the Program not expressly granted under this
+ Agreement are reserved.
+ .
+ This Agreement is governed by the laws of the State of New York and the
+ intellectual property laws of the United States of America. No party to
+ this Agreement will bring a legal action under this Agreement more than
+ one year after the cause of action arose. Each party waives its rights
+ to a jury trial in any resulting litigation.
diff --git a/debian/libsimbody-dev.install b/debian/libsimbody-dev.install
new file mode 100644
index 0000000..e8ab036
--- /dev/null
+++ b/debian/libsimbody-dev.install
@@ -0,0 +1,4 @@
+usr/include/*
+usr/lib/*/*.so
+usr/lib/*/cmake/*
+usr/lib/*/pkgconfig/*
diff --git a/debian/libsimbody-doc.doc-base b/debian/libsimbody-doc.doc-base
new file mode 100644
index 0000000..d86db8c
--- /dev/null
+++ b/debian/libsimbody-doc.doc-base
@@ -0,0 +1,9 @@
+Document: simbody
+Title: C++ API reference
+Author: Simbody team
+Abstract: Describe the simbody library C++ API.
+Section: Programming/C++
+
+Format: HTML
+Index: /usr/share/doc/simbody/SimbodyAPI.html
+Files: /usr/share/doc/simbody/api/*
diff --git a/debian/libsimbody-doc.docs b/debian/libsimbody-doc.docs
new file mode 100644
index 0000000..d9d4f6f
--- /dev/null
+++ b/debian/libsimbody-doc.docs
@@ -0,0 +1 @@
+usr/share/doc/*
diff --git a/debian/libsimbody-doc.install b/debian/libsimbody-doc.install
new file mode 100644
index 0000000..d9d4f6f
--- /dev/null
+++ b/debian/libsimbody-doc.install
@@ -0,0 +1 @@
+usr/share/doc/*
diff --git a/debian/libsimbody3.4.install b/debian/libsimbody3.4.install
new file mode 100644
index 0000000..3de3b10
--- /dev/null
+++ b/debian/libsimbody3.4.install
@@ -0,0 +1 @@
+usr/lib/*/*.so.*
diff --git a/debian/rules b/debian/rules
new file mode 100755
index 0000000..379289e
--- /dev/null
+++ b/debian/rules
@@ -0,0 +1,41 @@
+#!/usr/bin/make -f
+# -*- makefile -*-
+
+# Multiarch (if supported).
+DEB_HOST_MULTIARCH ?= $(shell dpkg-architecture -qDEB_HOST_MULTIARCH 2>/dev/null)
+
+# Configuration flags.
+CONFIGURE_ARGS=
+
+# If multiarch is supported, override CMAKE_INSTALL_LIB
+ifneq ($(DEB_HOST_MULTIARCH),)
+CONFIGURE_ARGS+=-DCMAKE_INSTALL_LIBDIR:STRING="lib/${DEB_HOST_MULTIARCH}"
+endif
+
+.PHONY: override_dh_auto_configure \
+ override_dh_auto_build-indep \
+ override_dh_auto_install \
+ override_dh_auto_test \
+ override_dh_strip
+
+override_dh_auto_configure:
+ dh_auto_configure -- \
+ -DCMAKE_INSTALL_PREFIX:PATH=/usr \
+ -DBUILD_EXAMPLES:BOOL=False \
+ $(CONFIGURE_ARGS)
+
+override_dh_auto_build-indep:
+ dh_auto_build -- doxygen
+
+override_dh_auto_install:
+ dh_auto_install --buildsystem=cmake
+
+ # Remove extra LICENSE and INSTALL file
+ rm debian/tmp/usr/share/doc/simbody/LICENSE.txt
+ rm debian/tmp/usr/share/doc/simbody/INSTALL.txt
+
+override_dh_strip:
+ dh_strip -a --dbg-package=libsimbody3.4-dbg
+
+%:
+ dh $@ --parallel --buildsystem=cmake
diff --git a/debian/source/format b/debian/source/format
new file mode 100644
index 0000000..163aaf8
--- /dev/null
+++ b/debian/source/format
@@ -0,0 +1 @@
+3.0 (quilt)
diff --git a/doc/HowToBuildSimbodyFromSource_MacLinux.doc b/doc/HowToBuildSimbodyFromSource_MacLinux.doc
new file mode 100644
index 0000000..2eec79f
Binary files /dev/null and b/doc/HowToBuildSimbodyFromSource_MacLinux.doc differ
diff --git a/doc/HowToBuildSimbodyFromSource_MacLinux.pdf b/doc/HowToBuildSimbodyFromSource_MacLinux.pdf
new file mode 100644
index 0000000..d02b384
Binary files /dev/null and b/doc/HowToBuildSimbodyFromSource_MacLinux.pdf differ
diff --git a/doc/HowToBuildSimbodyFromSource_Windows.doc b/doc/HowToBuildSimbodyFromSource_Windows.doc
new file mode 100644
index 0000000..5d0a435
Binary files /dev/null and b/doc/HowToBuildSimbodyFromSource_Windows.doc differ
diff --git a/doc/HowToBuildSimbodyFromSource_Windows.pdf b/doc/HowToBuildSimbodyFromSource_Windows.pdf
new file mode 100644
index 0000000..0a2df71
Binary files /dev/null and b/doc/HowToBuildSimbodyFromSource_Windows.pdf differ
diff --git a/doc/INSTALL.txt b/doc/INSTALL.txt
new file mode 100644
index 0000000..63c29cf
--- /dev/null
+++ b/doc/INSTALL.txt
@@ -0,0 +1,52 @@
+SimTK Simbody(tm) Release 2.2, June 2011
+https://simtk.org/home/simbody
+
+Installation instructions
+-------------------------
+Detailed installation instructions for Linux, Mac, and Windows installation
+of precompiled binaries are provided in the Simbody User's Guide, which can
+be found in the installation's doc directory or posted online at
+https://simtk.org/home/simbody, Documents tab. If you are building from
+source, use the How To Build From Source document for your platform instead;
+you can find that in the same places.
+
+We highly recommend that you read the detailed installation instructions in
+the User's Guide. However, if you are one of those guys who just won't ask
+for directions, here at least is an abbreviated summary of the installation
+procedure for the precompiled binaries:
+
+ 1 Install prerequisites for your platform (see below)
+ 2 Download an appropriate .zip file from Simbody's Download page.
+ 3 Unzip it into the installation directory, or unzip it somewhere and then
+ move the whole directory structure to where you want it to reside. That
+ can be anywhere you want, but we suggest
+ - Linux or Mac: /usr/local/SimTK
+ - 32-bit Windows: C:\Program Files\SimTK
+ - 64-bit Windows: C:\Program Files (x86)\SimTK
+ 4 Set the environment variable SimTK_INSTALL_DIR to the installation
+ directory you chose in step 3. (Not strictly required but helps with
+ examples and locating the Visualizer.)
+ 5 Set the appropriate PATH variable so the libraries can be found:
+ - Linux: export LD_LIBRARY_PATH=$SimTK_INSTALL_DIR/lib[64]
+ - Mac: export DYLD_LIBRARY_PATH=$SimTK_INSTALL_DIR/lib
+ - Windows: set path=%SimTK_INSTALL_DIR%\bin;%path%
+ 6 Test installation using:
+ $SimTK_INSTALL_DIR/examples/bin/SimbodyInstallTest
+
+Prerequisites:
+ Windows: None
+ Mac: Must have installed developer package (XCode), to get Lapack and Blas
+ (they are part of Apple's Accelerate framework).
+ Linux:
+ - Lapack and blas version 3: install package liblapack-dev. (Must end up
+ with liblapack.so.3 and libblas.so.3 in /usr/lib[64].)
+ - These are needed if you want to run the Simbody Visualizer (you do):
+ - Freeglut: install freeglut-dev package
+ - Xi, Xmu: install libxi-dev, libxmu-dev
+
+Getting help
+------------
+ 1 Read the installation instructions in the User's Guide (see above).
+ 2 Post to the help forum at https://simtk.org/home/simbody, Advanced tab,
+ Public forums.
+
diff --git a/doc/LICENSE.txt b/doc/LICENSE.txt
new file mode 100644
index 0000000..8507e23
--- /dev/null
+++ b/doc/LICENSE.txt
@@ -0,0 +1,74 @@
+ Simbody 3.2: Copyright and License
+ ----------------------------------
+
+Simbody uses the extremely permissive open source Apache 2.0 license as
+referenced below, suitable for any use including commercial, government,
+academic, and personal. Also please see notes below the license.
+
+/* -------------------------------------------------------------------------- *
+ * 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. *
+ * *
+ * Please cite: *
+ * Michael A. Sherman, Ajay Seth, Scott L. Delp, Simbody: multibody *
+ * dynamics for biomedical research, Procedia IUTAM 2:241-261 (2011) *
+ * http://dx.doi.org/10.1016/j.piutam.2011.04.023. *
+ * *
+ * Portions copyright (c) 2005-13 Stanford University and the Authors. *
+ * Authors: Michael Sherman, Peter Eastman *
+ * Contributors: Jack Middleton, Christopher Bruns, Paul Mitiguy, Matthew *
+ * Millard, Charles Schwieters, Abhinandan Jain, Isaac Newton *
+ * *
+ * 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. *
+ * -------------------------------------------------------------------------- */
+
+Notes
+-----
+
+1. What's with that "tm" in Simbody(tm)?
+
+ That trademark symbol is to let you know that we restrict how the name
+ "Simbody" can be used. That's because we want to make sure that it is always
+ clear to what that name refers. The license permits anyone to make any kind
+ of derivative work, or to use all or part of Simbody code in any way they
+ like, so it would become very confusing if all these works were called
+ "Simbody". We hope you will say that your code makes use of Simbody (see
+ below), or was derived from Simbody, but don't "repurpose" the name for
+ anything other than the Simbody project as it is distributed from the
+ official hosting site at https://simtk.org/home/simbody. This small
+ restriction is in the best interests of all Simbody users, including you,
+ since it avoids confusion about what is, and what is not, the carefully
+ managed Simbody community project. But ... if you want to make improvements
+ to Simbody we would love to have your well-engineered, open source (same
+ terms as above or looser) contributions to put into the main Simbody source
+ tree, in which case they would be included under the Simbody name.
+
+2. How to acknowledge us
+
+ Our license does not require that you acknowledge us, but we and our
+ sponsors would be grateful if you did anyway! If our hard work has helped
+ you with yours, please throw us a bone and mention on your "About" page
+ and in your documentation that you are using Simbody from
+ https://simtk.org/home/simbody. Where appropriate, please cite this
+ paper:
+
+ Michael A. Sherman, Ajay Seth, Scott L. Delp, Simbody: multibody
+ dynamics for biomedical research, Procedia IUTAM 2:241-261 (2011)
+ http://dx.doi.org/10.1016/j.piutam.2011.04.023
+
+ We would be particularly grateful if you mention that Simbody is primarily
+ funded by NIH Roadmap grant U54 GM072970. We appreciate that support very
+ much, and the NIH appreciates knowing that its funds are having an impact,
+ particularly on medical research and human health.
diff --git a/doc/images/isaac.png b/doc/images/isaac.png
new file mode 100644
index 0000000..f43ecb6
Binary files /dev/null and b/doc/images/isaac.png differ
diff --git a/doc/images/simbios_11000_body_RNA.gif b/doc/images/simbios_11000_body_RNA.gif
new file mode 100644
index 0000000..6ad922f
Binary files /dev/null and b/doc/images/simbios_11000_body_RNA.gif differ
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-science/packages/simbody.git
More information about the debian-science-commits
mailing list